3327 files changed, 207627 insertions, 77223 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index ba2901e76769..505c676fa9c7 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -206,4 +206,6 @@ source "drivers/fsi/Kconfig"
 
 source "drivers/tee/Kconfig"
 
+source "drivers/mux/Kconfig"
+
 endmenu
diff --git a/drivers/Makefile b/drivers/Makefile
index cfabd141dba2..dfdcda00bfe3 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -181,3 +181,4 @@ obj-$(CONFIG_NVMEM)		+= nvmem/
 obj-$(CONFIG_FPGA)		+= fpga/
 obj-$(CONFIG_FSI)		+= fsi/
 obj-$(CONFIG_TEE)		+= tee/
+obj-$(CONFIG_MULTIPLEXER)	+= mux/
diff --git a/drivers/acpi/acpi_configfs.c b/drivers/acpi/acpi_configfs.c
index 146a77fb762d..853bc7fc673f 100644
--- a/drivers/acpi/acpi_configfs.c
+++ b/drivers/acpi/acpi_configfs.c
@@ -15,11 +15,15 @@
 #include <linux/configfs.h>
 #include <linux/acpi.h>
 
+#include "acpica/accommon.h"
+#include "acpica/actables.h"
+
 static struct config_group *acpi_table_group;
 
 struct acpi_table {
 	struct config_item cfg;
 	struct acpi_table_header *header;
+	u32 index;
 };
 
 static ssize_t acpi_table_aml_write(struct config_item *cfg,
@@ -52,7 +56,11 @@ static ssize_t acpi_table_aml_write(struct config_item *cfg,
 	if (!table->header)
 		return -ENOMEM;
 
-	ret = acpi_load_table(table->header);
+	ACPI_INFO(("Host-directed Dynamic ACPI Table Load:"));
+	ret = acpi_tb_install_and_load_table(
+			ACPI_PTR_TO_PHYSADDR(table->header),
+			ACPI_TABLE_ORIGIN_EXTERNAL_VIRTUAL, FALSE,
+			&table->index);
 	if (ret) {
 		kfree(table->header);
 		table->header = NULL;
@@ -215,8 +223,18 @@ static struct config_item *acpi_table_make_item(struct config_group *group,
 	return &table->cfg;
 }
 
+static void acpi_table_drop_item(struct config_group *group,
+				 struct config_item *cfg)
+{
+	struct acpi_table *table = container_of(cfg, struct acpi_table, cfg);
+
+	ACPI_INFO(("Host-directed Dynamic ACPI Table Unload"));
+	acpi_tb_unload_table(table->index);
+}
+
 struct configfs_group_operations acpi_table_group_ops = {
 	.make_item = acpi_table_make_item,
+	.drop_item = acpi_table_drop_item,
 };
 
 static struct config_item_type acpi_tables_type = {
diff --git a/drivers/acpi/acpi_dbg.c b/drivers/acpi/acpi_dbg.c
index dee86925a9a1..3ec05aa1a903 100644
--- a/drivers/acpi/acpi_dbg.c
+++ b/drivers/acpi/acpi_dbg.c
@@ -10,7 +10,7 @@
  */
 
 /* #define DEBUG */
-#define pr_fmt(fmt) "ACPI : AML: " fmt
+#define pr_fmt(fmt) "ACPI: AML: " fmt
 
 #include <linux/kernel.h>
 #include <linux/module.h>
diff --git a/drivers/acpi/acpi_extlog.c b/drivers/acpi/acpi_extlog.c
index 502ea4dc2080..560fdae8cc59 100644
--- a/drivers/acpi/acpi_extlog.c
+++ b/drivers/acpi/acpi_extlog.c
@@ -141,9 +141,9 @@ static int extlog_print(struct notifier_block *nb, unsigned long val,
 	int	cpu = mce->extcpu;
 	struct acpi_hest_generic_status *estatus, *tmp;
 	struct acpi_hest_generic_data *gdata;
-	const uuid_le *fru_id = &NULL_UUID_LE;
+	const guid_t *fru_id = &guid_null;
 	char *fru_text = "";
-	uuid_le *sec_type;
+	guid_t *sec_type;
 	static u32 err_seq;
 
 	estatus = extlog_elog_entry_check(cpu, bank);
@@ -165,11 +165,11 @@ static int extlog_print(struct notifier_block *nb, unsigned long val,
 	err_seq++;
 	gdata = (struct acpi_hest_generic_data *)(tmp + 1);
 	if (gdata->validation_bits & CPER_SEC_VALID_FRU_ID)
-		fru_id = (uuid_le *)gdata->fru_id;
+		fru_id = (guid_t *)gdata->fru_id;
 	if (gdata->validation_bits & CPER_SEC_VALID_FRU_TEXT)
 		fru_text = gdata->fru_text;
-	sec_type = (uuid_le *)gdata->section_type;
-	if (!uuid_le_cmp(*sec_type, CPER_SEC_PLATFORM_MEM)) {
+	sec_type = (guid_t *)gdata->section_type;
+	if (guid_equal(sec_type, &CPER_SEC_PLATFORM_MEM)) {
 		struct cper_sec_mem_err *mem = (void *)(gdata + 1);
 		if (gdata->error_data_length >= sizeof(*mem))
 			trace_extlog_mem_event(mem, err_seq, fru_id, fru_text,
@@ -182,17 +182,17 @@ out:
 
 static bool __init extlog_get_l1addr(void)
 {
-	u8 uuid[16];
+	guid_t guid;
 	acpi_handle handle;
 	union acpi_object *obj;
 
-	acpi_str_to_uuid(extlog_dsm_uuid, uuid);
-
+	if (guid_parse(extlog_dsm_uuid, &guid))
+		return false;
 	if (ACPI_FAILURE(acpi_get_handle(NULL, "\\_SB", &handle)))
 		return false;
-	if (!acpi_check_dsm(handle, uuid, EXTLOG_DSM_REV, 1 << EXTLOG_FN_ADDR))
+	if (!acpi_check_dsm(handle, &guid, EXTLOG_DSM_REV, 1 << EXTLOG_FN_ADDR))
 		return false;
-	obj = acpi_evaluate_dsm_typed(handle, uuid, EXTLOG_DSM_REV,
+	obj = acpi_evaluate_dsm_typed(handle, &guid, EXTLOG_DSM_REV,
 				      EXTLOG_FN_ADDR, NULL, ACPI_TYPE_INTEGER);
 	if (!obj) {
 		return false;
diff --git a/drivers/acpi/acpica/Makefile b/drivers/acpi/acpica/Makefile
index dea65306b687..b125bdd3d58b 100644
--- a/drivers/acpi/acpica/Makefile
+++ b/drivers/acpi/acpica/Makefile
@@ -172,6 +172,7 @@ acpi-y +=		\
 	utosi.o		\
 	utownerid.o	\
 	utpredef.o	\
+	utresdecode.o	\
 	utresrc.o	\
 	utstate.o	\
 	utstring.o	\
diff --git a/drivers/acpi/acpica/acapps.h b/drivers/acpi/acpica/acapps.h
index b65f2731e9e2..bb6a84b0b4b3 100644
--- a/drivers/acpi/acpica/acapps.h
+++ b/drivers/acpi/acpica/acapps.h
@@ -158,8 +158,8 @@ acpi_dm_finish_namespace_load(union acpi_parse_object *parse_tree_root,
 			      acpi_owner_id owner_id);
 
 void
-acpi_dm_convert_resource_indexes(union acpi_parse_object *parse_tree_root,
-				 struct acpi_namespace_node *namespace_root);
+acpi_dm_convert_parse_objects(union acpi_parse_object *parse_tree_root,
+			      struct acpi_namespace_node *namespace_root);
 
 /*
  * adfile
diff --git a/drivers/acpi/acpica/acglobal.h b/drivers/acpi/acpica/acglobal.h
index abe8c316908c..95eed442703f 100644
--- a/drivers/acpi/acpica/acglobal.h
+++ b/drivers/acpi/acpica/acglobal.h
@@ -315,6 +315,7 @@ ACPI_INIT_GLOBAL(u8, acpi_gbl_force_aml_disassembly, FALSE);
 ACPI_INIT_GLOBAL(u8, acpi_gbl_dm_opt_verbose, TRUE);
 ACPI_INIT_GLOBAL(u8, acpi_gbl_dm_emit_external_opcodes, FALSE);
 ACPI_INIT_GLOBAL(u8, acpi_gbl_do_disassembler_optimizations, TRUE);
+ACPI_INIT_GLOBAL(ACPI_PARSE_OBJECT_LIST, *acpi_gbl_temp_list_head, NULL);
 
 ACPI_GLOBAL(u8, acpi_gbl_dm_opt_disasm);
 ACPI_GLOBAL(u8, acpi_gbl_dm_opt_listing);
@@ -368,6 +369,8 @@ ACPI_GLOBAL(const char, *acpi_gbl_pld_vertical_position_list[]);
 ACPI_GLOBAL(const char, *acpi_gbl_pld_horizontal_position_list[]);
 ACPI_GLOBAL(const char, *acpi_gbl_pld_shape_list[]);
 
+ACPI_INIT_GLOBAL(u8, acpi_gbl_disasm_flag, FALSE);
+
 #endif
 
 /*
diff --git a/drivers/acpi/acpica/aclocal.h b/drivers/acpi/acpica/aclocal.h
index f9b3f7fef462..8ddd3b20e0c6 100644
--- a/drivers/acpi/acpica/aclocal.h
+++ b/drivers/acpi/acpica/aclocal.h
@@ -859,7 +859,7 @@ ACPI_PARSE_COMMON};
  * and bytelists.
  */
 struct acpi_parse_obj_named {
-	ACPI_PARSE_COMMON u8 *path;
+	ACPI_PARSE_COMMON char *path;
 	u8 *data;		/* AML body or bytelist data */
 	u32 length;		/* AML length */
 	u32 name;		/* 4-byte name or zero if no name */
@@ -1142,8 +1142,13 @@ struct acpi_port_info {
 #define ACPI_RESOURCE_NAME_ADDRESS64            0x8A
 #define ACPI_RESOURCE_NAME_EXTENDED_ADDRESS64   0x8B
 #define ACPI_RESOURCE_NAME_GPIO                 0x8C
+#define ACPI_RESOURCE_NAME_PIN_FUNCTION         0x8D
 #define ACPI_RESOURCE_NAME_SERIAL_BUS           0x8E
-#define ACPI_RESOURCE_NAME_LARGE_MAX            0x8E
+#define ACPI_RESOURCE_NAME_PIN_CONFIG           0x8F
+#define ACPI_RESOURCE_NAME_PIN_GROUP            0x90
+#define ACPI_RESOURCE_NAME_PIN_GROUP_FUNCTION   0x91
+#define ACPI_RESOURCE_NAME_PIN_GROUP_CONFIG     0x92
+#define ACPI_RESOURCE_NAME_LARGE_MAX            0x92
 
 /*****************************************************************************
  *
@@ -1176,12 +1181,18 @@ struct acpi_external_list {
 #define ACPI_EXT_INTERNAL_PATH_ALLOCATED    0x04	/* Deallocate internal path on completion */
 #define ACPI_EXT_EXTERNAL_EMITTED           0x08	/* External() statement has been emitted */
 #define ACPI_EXT_ORIGIN_FROM_OPCODE         0x10	/* External came from a External() opcode */
+#define ACPI_EXT_CONFLICTING_DECLARATION    0x20	/* External has a conflicting declaration within AML */
 
 struct acpi_external_file {
 	char *path;
 	struct acpi_external_file *next;
 };
 
+struct acpi_parse_object_list {
+	union acpi_parse_object *op;
+	struct acpi_parse_object_list *next;
+};
+
 /*****************************************************************************
  *
  * Debugger
diff --git a/drivers/acpi/acpica/acopcode.h b/drivers/acpi/acpica/acopcode.h
index a5d9af758c52..cbd59a302679 100644
--- a/drivers/acpi/acpica/acopcode.h
+++ b/drivers/acpi/acpica/acopcode.h
@@ -112,7 +112,7 @@
 #define ARGP_DWORD_OP                   ARGP_LIST1 (ARGP_DWORDDATA)
 #define ARGP_ELSE_OP                    ARGP_LIST2 (ARGP_PKGLENGTH,  ARGP_TERMLIST)
 #define ARGP_EVENT_OP                   ARGP_LIST1 (ARGP_NAME)
-#define ARGP_EXTERNAL_OP                ARGP_LIST3 (ARGP_NAMESTRING, ARGP_BYTEDATA,      ARGP_BYTEDATA)
+#define ARGP_EXTERNAL_OP                ARGP_LIST3 (ARGP_NAME,       ARGP_BYTEDATA,      ARGP_BYTEDATA)
 #define ARGP_FATAL_OP                   ARGP_LIST3 (ARGP_BYTEDATA,   ARGP_DWORDDATA,     ARGP_TERMARG)
 #define ARGP_FIELD_OP                   ARGP_LIST4 (ARGP_PKGLENGTH,  ARGP_NAMESTRING,    ARGP_BYTEDATA,  ARGP_FIELDLIST)
 #define ARGP_FIND_SET_LEFT_BIT_OP       ARGP_LIST2 (ARGP_TERMARG,    ARGP_TARGET)
diff --git a/drivers/acpi/acpica/acpredef.h b/drivers/acpi/acpica/acpredef.h
index dcfc05d40e36..cdfcad8eb74c 100644
--- a/drivers/acpi/acpica/acpredef.h
+++ b/drivers/acpi/acpica/acpredef.h
@@ -581,6 +581,9 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 	{{"_HID", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_INTEGER | ACPI_RTYPE_STRING)}},
 
+	{{"_HMA", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
+
 	{{"_HOT", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
 
@@ -626,6 +629,19 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 		     ACPI_RTYPE_INTEGER | ACPI_RTYPE_BUFFER | ACPI_RTYPE_STRING,
 		     10, 0),
 
+	{{"_LSI", METHOD_0ARGS,
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 3, 0, 0, 0),
+
+	{{"_LSR", METHOD_2ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER),
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 1,
+		     ACPI_RTYPE_BUFFER, 1, 0),
+
+	{{"_LSW",
+	  METHOD_3ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER, ACPI_TYPE_BUFFER),
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
 	{{"_MAT", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
 
diff --git a/drivers/acpi/acpica/acresrc.h b/drivers/acpi/acpica/acresrc.h
index b4d22f6e48e2..438f3098a093 100644
--- a/drivers/acpi/acpica/acresrc.h
+++ b/drivers/acpi/acpica/acresrc.h
@@ -148,7 +148,10 @@ typedef enum {
 	ACPI_RSD_UINT16,
 	ACPI_RSD_UINT32,
 	ACPI_RSD_UINT64,
-	ACPI_RSD_WORDLIST
+	ACPI_RSD_WORDLIST,
+	ACPI_RSD_LABEL,
+	ACPI_RSD_SOURCE_LABEL,
+
 } ACPI_RSDUMP_OPCODES;
 
 /* restore default alignment */
@@ -329,6 +332,11 @@ extern struct acpi_rsconvert_info acpi_rs_convert_fixed_dma[];
 extern struct acpi_rsconvert_info acpi_rs_convert_i2c_serial_bus[];
 extern struct acpi_rsconvert_info acpi_rs_convert_spi_serial_bus[];
 extern struct acpi_rsconvert_info acpi_rs_convert_uart_serial_bus[];
+extern struct acpi_rsconvert_info acpi_rs_convert_pin_function[];
+extern struct acpi_rsconvert_info acpi_rs_convert_pin_config[];
+extern struct acpi_rsconvert_info acpi_rs_convert_pin_group[];
+extern struct acpi_rsconvert_info acpi_rs_convert_pin_group_function[];
+extern struct acpi_rsconvert_info acpi_rs_convert_pin_group_config[];
 
 /* These resources require separate get/set tables */
 
@@ -372,12 +380,17 @@ extern struct acpi_rsdump_info acpi_rs_dump_ext_address64[];
 extern struct acpi_rsdump_info acpi_rs_dump_ext_irq[];
 extern struct acpi_rsdump_info acpi_rs_dump_generic_reg[];
 extern struct acpi_rsdump_info acpi_rs_dump_gpio[];
+extern struct acpi_rsdump_info acpi_rs_dump_pin_function[];
 extern struct acpi_rsdump_info acpi_rs_dump_fixed_dma[];
 extern struct acpi_rsdump_info acpi_rs_dump_common_serial_bus[];
 extern struct acpi_rsdump_info acpi_rs_dump_i2c_serial_bus[];
 extern struct acpi_rsdump_info acpi_rs_dump_spi_serial_bus[];
 extern struct acpi_rsdump_info acpi_rs_dump_uart_serial_bus[];
 extern struct acpi_rsdump_info acpi_rs_dump_general_flags[];
+extern struct acpi_rsdump_info acpi_rs_dump_pin_config[];
+extern struct acpi_rsdump_info acpi_rs_dump_pin_group[];
+extern struct acpi_rsdump_info acpi_rs_dump_pin_group_function[];
+extern struct acpi_rsdump_info acpi_rs_dump_pin_group_config[];
 #endif
 
 #endif				/* __ACRESRC_H__ */
diff --git a/drivers/acpi/acpica/acutils.h b/drivers/acpi/acpica/acutils.h
index 6f28cfae2212..2a3cc4296481 100644
--- a/drivers/acpi/acpica/acutils.h
+++ b/drivers/acpi/acpica/acutils.h
@@ -85,6 +85,7 @@ extern const char *acpi_gbl_bpb_decode[];
 extern const char *acpi_gbl_sb_decode[];
 extern const char *acpi_gbl_fc_decode[];
 extern const char *acpi_gbl_pt_decode[];
+extern const char *acpi_gbl_ptyp_decode[];
 #endif
 
 /*
diff --git a/drivers/acpi/acpica/amlcode.h b/drivers/acpi/acpica/amlcode.h
index 176f7e9b4d0e..f54dc5a34bdc 100644
--- a/drivers/acpi/acpica/amlcode.h
+++ b/drivers/acpi/acpica/amlcode.h
@@ -313,6 +313,11 @@
  *      #A is the number of required arguments
  *      #T is the number of target operands
  *      #R indicates whether there is a return value
+ *
+ * These types are used for the top-level dispatch of the AML
+ * opcode. They group similar operators that can share common
+ * front-end code before dispatch to the final code that implements
+ * the operator.
  */
 
 /*
@@ -353,42 +358,42 @@
  * The opcode Type is used in a dispatch table, do not change
  * or add anything new without updating the table.
  */
-#define AML_TYPE_EXEC_0A_0T_1R      0x00
-#define AML_TYPE_EXEC_1A_0T_0R      0x01	/* Monadic1  */
-#define AML_TYPE_EXEC_1A_0T_1R      0x02	/* Monadic2  */
-#define AML_TYPE_EXEC_1A_1T_0R      0x03
-#define AML_TYPE_EXEC_1A_1T_1R      0x04	/* monadic2_r */
-#define AML_TYPE_EXEC_2A_0T_0R      0x05	/* Dyadic1   */
-#define AML_TYPE_EXEC_2A_0T_1R      0x06	/* Dyadic2   */
-#define AML_TYPE_EXEC_2A_1T_1R      0x07	/* dyadic2_r  */
-#define AML_TYPE_EXEC_2A_2T_1R      0x08
-#define AML_TYPE_EXEC_3A_0T_0R      0x09
-#define AML_TYPE_EXEC_3A_1T_1R      0x0A
-#define AML_TYPE_EXEC_6A_0T_1R      0x0B
+#define AML_TYPE_EXEC_0A_0T_1R      0x00	/* 0 Args, 0 Target, 1 ret_val */
+#define AML_TYPE_EXEC_1A_0T_0R      0x01	/* 1 Args, 0 Target, 0 ret_val */
+#define AML_TYPE_EXEC_1A_0T_1R      0x02	/* 1 Args, 0 Target, 1 ret_val */
+#define AML_TYPE_EXEC_1A_1T_0R      0x03	/* 1 Args, 1 Target, 0 ret_val */
+#define AML_TYPE_EXEC_1A_1T_1R      0x04	/* 1 Args, 1 Target, 1 ret_val */
+#define AML_TYPE_EXEC_2A_0T_0R      0x05	/* 2 Args, 0 Target, 0 ret_val */
+#define AML_TYPE_EXEC_2A_0T_1R      0x06	/* 2 Args, 0 Target, 1 ret_val */
+#define AML_TYPE_EXEC_2A_1T_1R      0x07	/* 2 Args, 1 Target, 1 ret_val */
+#define AML_TYPE_EXEC_2A_2T_1R      0x08	/* 2 Args, 2 Target, 1 ret_val */
+#define AML_TYPE_EXEC_3A_0T_0R      0x09	/* 3 Args, 0 Target, 0 ret_val */
+#define AML_TYPE_EXEC_3A_1T_1R      0x0A	/* 3 Args, 1 Target, 1 ret_val */
+#define AML_TYPE_EXEC_6A_0T_1R      0x0B	/* 6 Args, 0 Target, 1 ret_val */
 /* End of types used in dispatch table */
 
-#define AML_TYPE_LITERAL            0x0B
-#define AML_TYPE_CONSTANT           0x0C
-#define AML_TYPE_METHOD_ARGUMENT    0x0D
-#define AML_TYPE_LOCAL_VARIABLE     0x0E
-#define AML_TYPE_DATA_TERM          0x0F
+#define AML_TYPE_LITERAL            0x0C
+#define AML_TYPE_CONSTANT           0x0D
+#define AML_TYPE_METHOD_ARGUMENT    0x0E
+#define AML_TYPE_LOCAL_VARIABLE     0x0F
+#define AML_TYPE_DATA_TERM          0x10
 
 /* Generic for an op that returns a value */
 
-#define AML_TYPE_METHOD_CALL        0x10
+#define AML_TYPE_METHOD_CALL        0x11
 
 /* Miscellaneous types */
 
-#define AML_TYPE_CREATE_FIELD       0x11
-#define AML_TYPE_CREATE_OBJECT      0x12
-#define AML_TYPE_CONTROL            0x13
-#define AML_TYPE_NAMED_NO_OBJ       0x14
-#define AML_TYPE_NAMED_FIELD        0x15
-#define AML_TYPE_NAMED_SIMPLE       0x16
-#define AML_TYPE_NAMED_COMPLEX      0x17
-#define AML_TYPE_RETURN             0x18
-#define AML_TYPE_UNDEFINED          0x19
-#define AML_TYPE_BOGUS              0x1A
+#define AML_TYPE_CREATE_FIELD       0x12
+#define AML_TYPE_CREATE_OBJECT      0x13
+#define AML_TYPE_CONTROL            0x14
+#define AML_TYPE_NAMED_NO_OBJ       0x15
+#define AML_TYPE_NAMED_FIELD        0x16
+#define AML_TYPE_NAMED_SIMPLE       0x17
+#define AML_TYPE_NAMED_COMPLEX      0x18
+#define AML_TYPE_RETURN             0x19
+#define AML_TYPE_UNDEFINED          0x1A
+#define AML_TYPE_BOGUS              0x1B
 
 /* AML Package Length encodings */
 
diff --git a/drivers/acpi/acpica/amlresrc.h b/drivers/acpi/acpica/amlresrc.h
index 653a3d1ef5d5..1236e9a414e4 100644
--- a/drivers/acpi/acpica/amlresrc.h
+++ b/drivers/acpi/acpica/amlresrc.h
@@ -65,6 +65,7 @@
 #define ACPI_RESTAG_DRIVESTRENGTH               "_DRS"
 #define ACPI_RESTAG_ENDIANNESS                  "_END"
 #define ACPI_RESTAG_FLOWCONTROL                 "_FLC"
+#define ACPI_RESTAG_FUNCTION                    "_FUN"
 #define ACPI_RESTAG_GRANULARITY                 "_GRA"
 #define ACPI_RESTAG_INTERRUPT                   "_INT"
 #define ACPI_RESTAG_INTERRUPTLEVEL              "_LL_"	/* active_lo(1), active_hi(0) */
@@ -84,6 +85,8 @@
 #define ACPI_RESTAG_PHASE                       "_PHA"
 #define ACPI_RESTAG_PIN                         "_PIN"
 #define ACPI_RESTAG_PINCONFIG                   "_PPI"
+#define ACPI_RESTAG_PINCONFIG_TYPE              "_TYP"
+#define ACPI_RESTAG_PINCONFIG_VALUE             "_VAL"
 #define ACPI_RESTAG_POLARITY                    "_POL"
 #define ACPI_RESTAG_REGISTERBITOFFSET           "_RBO"
 #define ACPI_RESTAG_REGISTERBITWIDTH            "_RBW"
@@ -404,6 +407,102 @@ struct aml_resource_uart_serialbus {
 #define AML_RESOURCE_UART_TYPE_REVISION         1	/* ACPI 5.0 */
 #define AML_RESOURCE_UART_MIN_DATA_LEN          10
 
+struct aml_resource_pin_function {
+	AML_RESOURCE_LARGE_HEADER_COMMON u8 revision_id;
+	u16 flags;
+	u8 pin_config;
+	u16 function_number;
+	u16 pin_table_offset;
+	u8 res_source_index;
+	u16 res_source_offset;
+	u16 vendor_offset;
+	u16 vendor_length;
+	/*
+	 * Optional fields follow immediately:
+	 * 1) PIN list (Words)
+	 * 2) Resource Source String
+	 * 3) Vendor Data bytes
+	 */
+};
+
+#define AML_RESOURCE_PIN_FUNCTION_REVISION      1	/* ACPI 6.2 */
+
+struct aml_resource_pin_config {
+	AML_RESOURCE_LARGE_HEADER_COMMON u8 revision_id;
+	u16 flags;
+	u8 pin_config_type;
+	u32 pin_config_value;
+	u16 pin_table_offset;
+	u8 res_source_index;
+	u16 res_source_offset;
+	u16 vendor_offset;
+	u16 vendor_length;
+	/*
+	 * Optional fields follow immediately:
+	 * 1) PIN list (Words)
+	 * 2) Resource Source String
+	 * 3) Vendor Data bytes
+	 */
+};
+
+#define AML_RESOURCE_PIN_CONFIG_REVISION      1	/* ACPI 6.2 */
+
+struct aml_resource_pin_group {
+	AML_RESOURCE_LARGE_HEADER_COMMON u8 revision_id;
+	u16 flags;
+	u16 pin_table_offset;
+	u16 label_offset;
+	u16 vendor_offset;
+	u16 vendor_length;
+	/*
+	 * Optional fields follow immediately:
+	 * 1) PIN list (Words)
+	 * 2) Resource Label String
+	 * 3) Vendor Data bytes
+	 */
+};
+
+#define AML_RESOURCE_PIN_GROUP_REVISION      1	/* ACPI 6.2 */
+
+struct aml_resource_pin_group_function {
+	AML_RESOURCE_LARGE_HEADER_COMMON u8 revision_id;
+	u16 flags;
+	u16 function_number;
+	u8 res_source_index;
+	u16 res_source_offset;
+	u16 res_source_label_offset;
+	u16 vendor_offset;
+	u16 vendor_length;
+	/*
+	 * Optional fields follow immediately:
+	 * 1) Resource Source String
+	 * 2) Resource Source Label String
+	 * 3) Vendor Data bytes
+	 */
+};
+
+#define AML_RESOURCE_PIN_GROUP_FUNCTION_REVISION    1	/* ACPI 6.2 */
+
+struct aml_resource_pin_group_config {
+	AML_RESOURCE_LARGE_HEADER_COMMON u8 revision_id;
+	u16 flags;
+	u8 pin_config_type;
+	u32 pin_config_value;
+	u8 res_source_index;
+	u16 res_source_offset;
+	u16 res_source_label_offset;
+	u16 vendor_offset;
+	u16 vendor_length;
+	/*
+	 * Optional fields follow immediately:
+	 * 1) Resource Source String
+	 * 2) Resource Source Label String
+	 * 3) Vendor Data bytes
+	 */
+};
+
+#define AML_RESOURCE_PIN_GROUP_CONFIG_REVISION    1	/* ACPI 6.2 */
+
 /* restore default alignment */
 
 #pragma pack()
@@ -446,6 +545,11 @@ union aml_resource {
 	struct aml_resource_spi_serialbus spi_serial_bus;
 	struct aml_resource_uart_serialbus uart_serial_bus;
 	struct aml_resource_common_serialbus common_serial_bus;
+	struct aml_resource_pin_function pin_function;
+	struct aml_resource_pin_config pin_config;
+	struct aml_resource_pin_group pin_group;
+	struct aml_resource_pin_group_function pin_group_function;
+	struct aml_resource_pin_group_config pin_group_config;
 
 	/* Utility overlays */
 
diff --git a/drivers/acpi/acpica/dbexec.c b/drivers/acpi/acpica/dbexec.c
index b611cd92b5f5..3b30319752f0 100644
--- a/drivers/acpi/acpica/dbexec.c
+++ b/drivers/acpi/acpica/dbexec.c
@@ -181,6 +181,18 @@ acpi_db_execute_method(struct acpi_db_method_info *info,
 	acpi_gbl_method_executing = FALSE;
 
 	if (ACPI_FAILURE(status)) {
+		if ((status == AE_ABORT_METHOD) || acpi_gbl_abort_method) {
+
+			/* Clear the abort and fall back to the debugger prompt */
+
+			ACPI_EXCEPTION((AE_INFO, status,
+					"Aborting top-level method"));
+
+			acpi_gbl_abort_method = FALSE;
+			status = AE_OK;
+			goto cleanup;
+		}
+
 		ACPI_EXCEPTION((AE_INFO, status,
 				"while executing %s from debugger",
 				info->pathname));
diff --git a/drivers/acpi/acpica/dbobject.c b/drivers/acpi/acpica/dbobject.c
index f2252b1ac0b3..e7b415c20aa8 100644
--- a/drivers/acpi/acpica/dbobject.c
+++ b/drivers/acpi/acpica/dbobject.c
@@ -448,7 +448,7 @@ void acpi_db_decode_locals(struct acpi_walk_state *walk_state)
 
 	if (display_locals) {
 		acpi_os_printf
-		    ("\nInitialized Local Variables for method [%4.4s]:\n",
+		    ("\nInitialized Local Variables for Method [%4.4s]:\n",
 		     acpi_ut_get_node_name(node));
 
 		for (i = 0; i < ACPI_METHOD_NUM_LOCALS; i++) {
@@ -461,7 +461,7 @@ void acpi_db_decode_locals(struct acpi_walk_state *walk_state)
 		}
 	} else {
 		acpi_os_printf
-		    ("No Local Variables are initialized for method [%4.4s]\n",
+		    ("No Local Variables are initialized for Method [%4.4s]\n",
 		     acpi_ut_get_node_name(node));
 	}
 }
@@ -515,7 +515,7 @@ void acpi_db_decode_arguments(struct acpi_walk_state *walk_state)
 		acpi_os_printf("Initialized Arguments for Method [%4.4s]:  "
 			       "(%X arguments defined for method invocation)\n",
 			       acpi_ut_get_node_name(node),
-			       obj_desc->method.param_count);
+			       node->object->method.param_count);
 
 		for (i = 0; i < ACPI_METHOD_NUM_ARGS; i++) {
 			obj_desc = walk_state->arguments[i].object;
diff --git a/drivers/acpi/acpica/dbxface.c b/drivers/acpi/acpica/dbxface.c
index 8f665d94b8b5..b6985323e7eb 100644
--- a/drivers/acpi/acpica/dbxface.c
+++ b/drivers/acpi/acpica/dbxface.c
@@ -244,7 +244,7 @@ acpi_db_single_step(struct acpi_walk_state *walk_state,
 		if ((acpi_gbl_db_output_to_file) ||
 		    (acpi_dbg_level & ACPI_LV_PARSE)) {
 			acpi_os_printf
-			    ("\n[AmlDebug] Next AML Opcode to execute:\n");
+			    ("\nAML Debug: Next AML Opcode to execute:\n");
 		}
 
 		/*
diff --git a/drivers/acpi/acpica/dsargs.c b/drivers/acpi/acpica/dsargs.c
index 287b3fd73cfc..2873455c986d 100644
--- a/drivers/acpi/acpica/dsargs.c
+++ b/drivers/acpi/acpica/dsargs.c
@@ -82,7 +82,7 @@ acpi_ds_execute_arguments(struct acpi_namespace_node *node,
 	union acpi_parse_object *op;
 	struct acpi_walk_state *walk_state;
 
-	ACPI_FUNCTION_TRACE(ds_execute_arguments);
+	ACPI_FUNCTION_TRACE_PTR(ds_execute_arguments, aml_start);
 
 	/* Allocate a new parser op to be the root of the parsed tree */
 
@@ -338,7 +338,8 @@ acpi_status acpi_ds_get_package_arguments(union acpi_operand_object *obj_desc)
 		return_ACPI_STATUS(AE_AML_INTERNAL);
 	}
 
-	ACPI_DEBUG_PRINT((ACPI_DB_EXEC, "Package Arg Init\n"));
+	ACPI_DEBUG_PRINT((ACPI_DB_EXEC, "Package Argument Init, AML Ptr: %p\n",
+			  obj_desc->package.aml_start));
 
 	/* Execute the AML code for the term_arg arguments */
 
diff --git a/drivers/acpi/acpica/dsdebug.c b/drivers/acpi/acpica/dsdebug.c
index 4d885eb8eda9..d1f457eda980 100644
--- a/drivers/acpi/acpica/dsdebug.c
+++ b/drivers/acpi/acpica/dsdebug.c
@@ -196,6 +196,7 @@ acpi_ds_dump_method_stack(acpi_status status,
 				op->common.next = NULL;
 
 #ifdef ACPI_DISASSEMBLER
+				acpi_os_printf("Failed at ");
 				acpi_dm_disassemble(next_walk_state, op,
 						    ACPI_UINT32_MAX);
 #endif
diff --git a/drivers/acpi/acpica/dsmethod.c b/drivers/acpi/acpica/dsmethod.c
index 31c9c7aec3d5..d7fc36917c67 100644
--- a/drivers/acpi/acpica/dsmethod.c
+++ b/drivers/acpi/acpica/dsmethod.c
@@ -212,6 +212,7 @@ acpi_status
 acpi_ds_method_error(acpi_status status, struct acpi_walk_state *walk_state)
 {
 	u32 aml_offset;
+	acpi_name name = 0;
 
 	ACPI_FUNCTION_ENTRY();
 
@@ -237,10 +238,13 @@ acpi_ds_method_error(acpi_status status, struct acpi_walk_state *walk_state)
 						walk_state->parser_state.
 						aml_start);
 
-		status = acpi_gbl_exception_handler(status,
-						    walk_state->method_node ?
-						    walk_state->method_node->
-						    name.integer : 0,
+		if (walk_state->method_node) {
+			name = walk_state->method_node->name.integer;
+		} else if (walk_state->deferred_node) {
+			name = walk_state->deferred_node->name.integer;
+		}
+
+		status = acpi_gbl_exception_handler(status, name,
 						    walk_state->opcode,
 						    aml_offset, NULL);
 		acpi_ex_enter_interpreter();
diff --git a/drivers/acpi/acpica/dsopcode.c b/drivers/acpi/acpica/dsopcode.c
index 9a8f8a992b3e..dfc3c25a083d 100644
--- a/drivers/acpi/acpica/dsopcode.c
+++ b/drivers/acpi/acpica/dsopcode.c
@@ -227,13 +227,12 @@ acpi_ds_init_buffer_field(u16 aml_opcode,
 
 	/* Entire field must fit within the current length of the buffer */
 
-	if ((bit_offset + bit_count) > (8 * (u32) buffer_desc->buffer.length)) {
+	if ((bit_offset + bit_count) > (8 * (u32)buffer_desc->buffer.length)) {
 		ACPI_ERROR((AE_INFO,
-			    "Field [%4.4s] at %u exceeds Buffer [%4.4s] size %u (bits)",
-			    acpi_ut_get_node_name(result_desc),
-			    bit_offset + bit_count,
-			    acpi_ut_get_node_name(buffer_desc->buffer.node),
-			    8 * (u32) buffer_desc->buffer.length));
+			    "Field [%4.4s] at bit offset/length %u/%u "
+			    "exceeds size of target Buffer (%u bits)",
+			    acpi_ut_get_node_name(result_desc), bit_offset,
+			    bit_count, 8 * (u32)buffer_desc->buffer.length));
 		status = AE_AML_BUFFER_LIMIT;
 		goto cleanup;
 	}
diff --git a/drivers/acpi/acpica/dsutils.c b/drivers/acpi/acpica/dsutils.c
index 406edec20de7..0dabd9b95684 100644
--- a/drivers/acpi/acpica/dsutils.c
+++ b/drivers/acpi/acpica/dsutils.c
@@ -633,15 +633,6 @@ acpi_ds_create_operand(struct acpi_walk_state *walk_state,
 
 		if ((op_info->flags & AML_HAS_RETVAL) ||
 		    (arg->common.flags & ACPI_PARSEOP_IN_STACK)) {
-			ACPI_DEBUG_PRINT((ACPI_DB_DISPATCH,
-					  "Argument previously created, already stacked\n"));
-
-			acpi_db_display_argument_object(walk_state->
-							operands[walk_state->
-								 num_operands -
-								 1],
-							walk_state);
-
 			/*
 			 * Use value that was already previously returned
 			 * by the evaluation of this argument
diff --git a/drivers/acpi/acpica/dswexec.c b/drivers/acpi/acpica/dswexec.c
index a2ff8ad70d58..20d7744b06ae 100644
--- a/drivers/acpi/acpica/dswexec.c
+++ b/drivers/acpi/acpica/dswexec.c
@@ -576,8 +576,8 @@ acpi_status acpi_ds_exec_end_op(struct acpi_walk_state *walk_state)
 		case AML_TYPE_CREATE_OBJECT:
 
 			ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
-					  "Executing CreateObject (Buffer/Package) Op=%p\n",
-					  op));
+					  "Executing CreateObject (Buffer/Package) Op=%p AMLPtr=%p\n",
+					  op, op->named.data));
 
 			switch (op->common.parent->common.aml_opcode) {
 			case AML_NAME_OP:
diff --git a/drivers/acpi/acpica/dswload.c b/drivers/acpi/acpica/dswload.c
index cafb3ab567ab..eaa859a89702 100644
--- a/drivers/acpi/acpica/dswload.c
+++ b/drivers/acpi/acpica/dswload.c
@@ -397,7 +397,7 @@ acpi_ds_load1_begin_op(struct acpi_walk_state *walk_state,
 	/* Initialize the op */
 
 #if (defined (ACPI_NO_METHOD_EXECUTION) || defined (ACPI_CONSTANT_EVAL_ONLY))
-	op->named.path = ACPI_CAST_PTR(u8, path);
+	op->named.path = path;
 #endif
 
 	if (node) {
@@ -434,6 +434,10 @@ acpi_status acpi_ds_load1_end_op(struct acpi_walk_state *walk_state)
 	acpi_object_type object_type;
 	acpi_status status = AE_OK;
 
+#ifdef ACPI_ASL_COMPILER
+	u8 param_count;
+#endif
+
 	ACPI_FUNCTION_TRACE(ds_load1_end_op);
 
 	op = walk_state->op;
@@ -514,6 +518,38 @@ acpi_status acpi_ds_load1_end_op(struct acpi_walk_state *walk_state)
 			}
 		}
 	}
+#ifdef ACPI_ASL_COMPILER
+	/*
+	 * For external opcode, get the object type from the argument and
+	 * get the parameter count from the argument's next.
+	 */
+	if (acpi_gbl_disasm_flag &&
+	    op->common.node && op->common.aml_opcode == AML_EXTERNAL_OP) {
+		/*
+		 * Note, if this external is not a method
+		 * Op->Common.Value.Arg->Common.Next->Common.Value.Integer == 0
+		 * Therefore, param_count will be 0.
+		 */
+		param_count =
+		    (u8)op->common.value.arg->common.next->common.value.integer;
+		object_type = (u8)op->common.value.arg->common.value.integer;
+		op->common.node->flags |= ANOBJ_IS_EXTERNAL;
+		op->common.node->type = (u8)object_type;
+
+		acpi_dm_create_subobject_for_external((u8)object_type,
+						      &op->common.node,
+						      param_count);
+
+		/*
+		 * Add the external to the external list because we may be
+		 * emitting code based off of the items within the external list.
+		 */
+		acpi_dm_add_op_to_external_list(op, op->named.path,
+						(u8)object_type, param_count,
+						ACPI_EXT_ORIGIN_FROM_OPCODE |
+						ACPI_EXT_RESOLVED_REFERENCE);
+	}
+#endif
 
 	/*
 	 * If we are executing a method, do not create any namespace objects
@@ -563,7 +599,9 @@ acpi_status acpi_ds_load1_end_op(struct acpi_walk_state *walk_state)
 
 	/* Pop the scope stack (only if loading a table) */
 
-	if (!walk_state->method_node && acpi_ns_opens_scope(object_type)) {
+	if (!walk_state->method_node &&
+	    op->common.aml_opcode != AML_EXTERNAL_OP &&
+	    acpi_ns_opens_scope(object_type)) {
 		ACPI_DEBUG_PRINT((ACPI_DB_DISPATCH,
 				  "(%s): Popping scope for Op %p\n",
 				  acpi_ut_get_type_name(object_type), op));
diff --git a/drivers/acpi/acpica/dswload2.c b/drivers/acpi/acpica/dswload2.c
index 8d510c7e20c8..aad83ef5a4ec 100644
--- a/drivers/acpi/acpica/dswload2.c
+++ b/drivers/acpi/acpica/dswload2.c
@@ -310,6 +310,22 @@ acpi_ds_load2_begin_op(struct acpi_walk_state *walk_state,
 				flags |= ACPI_NS_TEMPORARY;
 			}
 		}
+#ifdef ACPI_ASL_COMPILER
+
+		/*
+		 * Do not open a scope for AML_EXTERNAL_OP
+		 * acpi_ns_lookup can open a new scope based on the object type
+		 * of this op. AML_EXTERNAL_OP is a declaration rather than a
+		 * definition. In the case that this external is a method object,
+		 * acpi_ns_lookup will open a new scope. However, an AML_EXTERNAL_OP
+		 * associated with the ACPI_TYPE_METHOD is a declaration, rather than
+		 * a definition. Flags is set to avoid opening a scope for any
+		 * AML_EXTERNAL_OP.
+		 */
+		if (walk_state->opcode == AML_EXTERNAL_OP) {
+			flags |= ACPI_NS_DONT_OPEN_SCOPE;
+		}
+#endif
 
 		/* Add new entry or lookup existing entry */
 
diff --git a/drivers/acpi/acpica/evxfevnt.c b/drivers/acpi/acpica/evxfevnt.c
index 82e8971f23a4..c773ac4892cb 100644
--- a/drivers/acpi/acpica/evxfevnt.c
+++ b/drivers/acpi/acpica/evxfevnt.c
@@ -180,6 +180,12 @@ acpi_status acpi_enable_event(u32 event, u32 flags)
 
 	ACPI_FUNCTION_TRACE(acpi_enable_event);
 
+	/* If Hardware Reduced flag is set, there are no fixed events */
+
+	if (acpi_gbl_reduced_hardware) {
+		return_ACPI_STATUS(AE_OK);
+	}
+
 	/* Decode the Fixed Event */
 
 	if (event > ACPI_EVENT_MAX) {
@@ -237,6 +243,12 @@ acpi_status acpi_disable_event(u32 event, u32 flags)
 
 	ACPI_FUNCTION_TRACE(acpi_disable_event);
 
+	/* If Hardware Reduced flag is set, there are no fixed events */
+
+	if (acpi_gbl_reduced_hardware) {
+		return_ACPI_STATUS(AE_OK);
+	}
+
 	/* Decode the Fixed Event */
 
 	if (event > ACPI_EVENT_MAX) {
@@ -290,6 +302,12 @@ acpi_status acpi_clear_event(u32 event)
 
 	ACPI_FUNCTION_TRACE(acpi_clear_event);
 
+	/* If Hardware Reduced flag is set, there are no fixed events */
+
+	if (acpi_gbl_reduced_hardware) {
+		return_ACPI_STATUS(AE_OK);
+	}
+
 	/* Decode the Fixed Event */
 
 	if (event > ACPI_EVENT_MAX) {
diff --git a/drivers/acpi/acpica/exdebug.c b/drivers/acpi/acpica/exdebug.c
index ec614f5a3bcb..a8191d2ca5e3 100644
--- a/drivers/acpi/acpica/exdebug.c
+++ b/drivers/acpi/acpica/exdebug.c
@@ -117,10 +117,10 @@ acpi_ex_do_debug_object(union acpi_operand_object *source_desc,
 			timer = ((u32)acpi_os_get_timer() / 10);
 			timer &= 0x03FFFFFF;
 
-			acpi_os_printf("[ACPI Debug T=0x%8.8X] %*s", timer,
+			acpi_os_printf("ACPI Debug: T=0x%8.8X %*s", timer,
 				       level, " ");
 		} else {
-			acpi_os_printf("[ACPI Debug] %*s", level, " ");
+			acpi_os_printf("ACPI Debug: %*s", level, " ");
 		}
 	}
 
diff --git a/drivers/acpi/acpica/exdump.c b/drivers/acpi/acpica/exdump.c
index 970dc6c53994..44092f744477 100644
--- a/drivers/acpi/acpica/exdump.c
+++ b/drivers/acpi/acpica/exdump.c
@@ -645,10 +645,12 @@ void acpi_ex_dump_operand(union acpi_operand_object *obj_desc, u32 depth)
 	/* obj_desc is a valid object */
 
 	if (depth > 0) {
-		ACPI_DEBUG_PRINT((ACPI_DB_EXEC, "%*s[%u] %p ",
-				  depth, " ", depth, obj_desc));
+		ACPI_DEBUG_PRINT((ACPI_DB_EXEC, "%*s[%u] %p Refs=%u ",
+				  depth, " ", depth, obj_desc,
+				  obj_desc->common.reference_count));
 	} else {
-		ACPI_DEBUG_PRINT((ACPI_DB_EXEC, "%p ", obj_desc));
+		ACPI_DEBUG_PRINT((ACPI_DB_EXEC, "%p Refs=%u ",
+				  obj_desc, obj_desc->common.reference_count));
 	}
 
 	/* Decode object type */
@@ -690,8 +692,11 @@ void acpi_ex_dump_operand(union acpi_operand_object *obj_desc, u32 depth)
 
 		case ACPI_REFCLASS_NAME:
 
-			acpi_os_printf("- [%4.4s]\n",
-				       obj_desc->reference.node->name.ascii);
+			acpi_ut_repair_name(obj_desc->reference.node->name.
+					    ascii);
+			acpi_os_printf("- [%4.4s] (Node %p)\n",
+				       obj_desc->reference.node->name.ascii,
+				       obj_desc->reference.node);
 			break;
 
 		case ACPI_REFCLASS_ARG:
@@ -999,9 +1004,15 @@ static void acpi_ex_dump_reference_obj(union acpi_operand_object *obj_desc)
 		status = acpi_ns_handle_to_pathname(obj_desc->reference.node,
 						    &ret_buf, TRUE);
 		if (ACPI_FAILURE(status)) {
-			acpi_os_printf(" Could not convert name to pathname\n");
+			acpi_os_printf
+			    (" Could not convert name to pathname: %s\n",
+			     acpi_format_exception(status));
 		} else {
-			acpi_os_printf("%s\n", (char *)ret_buf.pointer);
+			acpi_os_printf("%s: %s\n",
+				       acpi_ut_get_type_name(obj_desc->
+							     reference.node->
+							     type),
+				       (char *)ret_buf.pointer);
 			ACPI_FREE(ret_buf.pointer);
 		}
 	} else if (obj_desc->reference.object) {
@@ -1111,9 +1122,8 @@ acpi_ex_dump_package_obj(union acpi_operand_object *obj_desc,
 
 	case ACPI_TYPE_LOCAL_REFERENCE:
 
-		acpi_os_printf("[Object Reference] Type [%s] %2.2X",
-			       acpi_ut_get_reference_name(obj_desc),
-			       obj_desc->reference.class);
+		acpi_os_printf("[Object Reference] Class [%s]",
+			       acpi_ut_get_reference_name(obj_desc));
 		acpi_ex_dump_reference_obj(obj_desc);
 		break;
 
diff --git a/drivers/acpi/acpica/exoparg1.c b/drivers/acpi/acpica/exoparg1.c
index e327349675cd..f787651348c1 100644
--- a/drivers/acpi/acpica/exoparg1.c
+++ b/drivers/acpi/acpica/exoparg1.c
@@ -921,13 +921,26 @@ acpi_status acpi_ex_opcode_1A_0T_1R(struct acpi_walk_state *walk_state)
 			 * This is a deref_of (object_reference)
 			 * Get the actual object from the Node (This is the dereference).
 			 * This case may only happen when a local_x or arg_x is
-			 * dereferenced above.
+			 * dereferenced above, or for references to device and
+			 * thermal objects.
 			 */
-			return_desc = acpi_ns_get_attached_object((struct
-								   acpi_namespace_node
-								   *)
-								  operand[0]);
-			acpi_ut_add_reference(return_desc);
+			switch (((struct acpi_namespace_node *)operand[0])->
+				type) {
+			case ACPI_TYPE_DEVICE:
+			case ACPI_TYPE_THERMAL:
+
+				/* These types have no node subobject, return the NS node */
+
+				return_desc = operand[0];
+				break;
+
+			default:
+				/* For most types, get the object attached to the node */
+
+				return_desc = acpi_ns_get_attached_object((struct acpi_namespace_node *)operand[0]);
+				acpi_ut_add_reference(return_desc);
+				break;
+			}
 		} else {
 			/*
 			 * This must be a reference object produced by either the
diff --git a/drivers/acpi/acpica/exresolv.c b/drivers/acpi/acpica/exresolv.c
index aa8c6fd74cc3..5e1854ea85f6 100644
--- a/drivers/acpi/acpica/exresolv.c
+++ b/drivers/acpi/acpica/exresolv.c
@@ -368,11 +368,24 @@ acpi_ex_resolve_multiple(struct acpi_walk_state *walk_state,
 								*)obj_desc);
 		}
 
-		if (!obj_desc) {
-			ACPI_ERROR((AE_INFO,
-				    "[%4.4s] Node is unresolved or uninitialized",
-				    acpi_ut_get_node_name(node)));
-			return_ACPI_STATUS(AE_AML_UNINITIALIZED_NODE);
+		switch (type) {
+		case ACPI_TYPE_DEVICE:
+		case ACPI_TYPE_THERMAL:
+
+			/* These types have no attached subobject */
+			break;
+
+		default:
+
+			/* All other types require a subobject */
+
+			if (!obj_desc) {
+				ACPI_ERROR((AE_INFO,
+					    "[%4.4s] Node is unresolved or uninitialized",
+					    acpi_ut_get_node_name(node)));
+				return_ACPI_STATUS(AE_AML_UNINITIALIZED_NODE);
+			}
+			break;
 		}
 		break;
 
diff --git a/drivers/acpi/acpica/hwxfsleep.c b/drivers/acpi/acpica/hwxfsleep.c
index 5733b1167e46..7ef13934968f 100644
--- a/drivers/acpi/acpica/hwxfsleep.c
+++ b/drivers/acpi/acpica/hwxfsleep.c
@@ -70,11 +70,15 @@ static acpi_status acpi_hw_sleep_dispatch(u8 sleep_state, u32 function_id);
 /* Legacy functions are optional, based upon ACPI_REDUCED_HARDWARE */
 
 static struct acpi_sleep_functions acpi_sleep_dispatch[] = {
-	{ACPI_HW_OPTIONAL_FUNCTION(acpi_hw_legacy_sleep),
-	 acpi_hw_extended_sleep},
-	{ACPI_HW_OPTIONAL_FUNCTION(acpi_hw_legacy_wake_prep),
-	 acpi_hw_extended_wake_prep},
-	{ACPI_HW_OPTIONAL_FUNCTION(acpi_hw_legacy_wake), acpi_hw_extended_wake}
+	{ACPI_STRUCT_INIT(legacy_function,
+			  ACPI_HW_OPTIONAL_FUNCTION(acpi_hw_legacy_sleep)),
+	 ACPI_STRUCT_INIT(extended_function, acpi_hw_extended_sleep) },
+	{ACPI_STRUCT_INIT(legacy_function,
+			  ACPI_HW_OPTIONAL_FUNCTION(acpi_hw_legacy_wake_prep)),
+	 ACPI_STRUCT_INIT(extended_function, acpi_hw_extended_wake_prep) },
+	{ACPI_STRUCT_INIT(legacy_function,
+			  ACPI_HW_OPTIONAL_FUNCTION(acpi_hw_legacy_wake)),
+	 ACPI_STRUCT_INIT(extended_function, acpi_hw_extended_wake) }
 };
 
 /*
diff --git a/drivers/acpi/acpica/nsaccess.c b/drivers/acpi/acpica/nsaccess.c
index fb265b5737de..e5f4fa496572 100644
--- a/drivers/acpi/acpica/nsaccess.c
+++ b/drivers/acpi/acpica/nsaccess.c
@@ -47,6 +47,10 @@
 #include "acnamesp.h"
 #include "acdispat.h"
 
+#ifdef ACPI_ASL_COMPILER
+#include "acdisasm.h"
+#endif
+
 #define _COMPONENT          ACPI_NAMESPACE
 ACPI_MODULE_NAME("nsaccess")
 
@@ -580,6 +584,29 @@ acpi_ns_lookup(union acpi_generic_state *scope_info,
 						  (char *)&current_node->name,
 						  current_node));
 			}
+#ifdef ACPI_ASL_COMPILER
+			/*
+			 * If this ACPI name already exists within the namespace as an
+			 * external declaration, then mark the external as a conflicting
+			 * declaration and proceed to process the current node as if it did
+			 * not exist in the namespace. If this node is not processed as
+			 * normal, then it could cause improper namespace resolution
+			 * by failing to open a new scope.
+			 */
+			if (acpi_gbl_disasm_flag &&
+			    (status == AE_ALREADY_EXISTS) &&
+			    ((this_node->flags & ANOBJ_IS_EXTERNAL) ||
+			     (walk_state
+			      && walk_state->opcode == AML_EXTERNAL_OP))) {
+				this_node->flags &= ~ANOBJ_IS_EXTERNAL;
+				this_node->type = (u8)this_search_type;
+				if (walk_state->opcode != AML_EXTERNAL_OP) {
+					acpi_dm_mark_external_conflict
+					    (this_node);
+				}
+				break;
+			}
+#endif
 
 			*return_node = this_node;
 			return_ACPI_STATUS(status);
diff --git a/drivers/acpi/acpica/nsnames.c b/drivers/acpi/acpica/nsnames.c
index 3db9ca25a620..aa16aeaa8937 100644
--- a/drivers/acpi/acpica/nsnames.c
+++ b/drivers/acpi/acpica/nsnames.c
@@ -190,9 +190,6 @@ acpi_ns_handle_to_pathname(acpi_handle target_handle,
 
 	(void)acpi_ns_build_normalized_path(node, buffer->pointer,
 					    required_size, no_trailing);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
-	}
 
 	ACPI_DEBUG_PRINT((ACPI_DB_EXEC, "%s [%X]\n",
 			  (char *)buffer->pointer, (u32) required_size));
diff --git a/drivers/acpi/acpica/nsutils.c b/drivers/acpi/acpica/nsutils.c
index 2fe87d0dd9d5..b43fe5fce64b 100644
--- a/drivers/acpi/acpica/nsutils.c
+++ b/drivers/acpi/acpica/nsutils.c
@@ -89,7 +89,7 @@ acpi_ns_print_node_pathname(struct acpi_namespace_node *node,
 			acpi_os_printf("%s ", message);
 		}
 
-		acpi_os_printf("[%s] (Node %p)", (char *)buffer.pointer, node);
+		acpi_os_printf("%s", (char *)buffer.pointer);
 		ACPI_FREE(buffer.pointer);
 	}
 }
diff --git a/drivers/acpi/acpica/nsxfeval.c b/drivers/acpi/acpica/nsxfeval.c
index c944ff5c9c3d..538c61677c10 100644
--- a/drivers/acpi/acpica/nsxfeval.c
+++ b/drivers/acpi/acpica/nsxfeval.c
@@ -85,6 +85,8 @@ acpi_evaluate_object_typed(acpi_handle handle,
 {
 	acpi_status status;
 	u8 free_buffer_on_error = FALSE;
+	acpi_handle target_handle;
+	char *full_pathname;
 
 	ACPI_FUNCTION_TRACE(acpi_evaluate_object_typed);
 
@@ -98,38 +100,51 @@ acpi_evaluate_object_typed(acpi_handle handle,
 		free_buffer_on_error = TRUE;
 	}
 
+	status = acpi_get_handle(handle, pathname, &target_handle);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	full_pathname = acpi_ns_get_external_pathname(target_handle);
+	if (!full_pathname) {
+		return_ACPI_STATUS(AE_NO_MEMORY);
+	}
+
 	/* Evaluate the object */
 
-	status = acpi_evaluate_object(handle, pathname,
-				      external_params, return_buffer);
+	status = acpi_evaluate_object(target_handle, NULL, external_params,
+				      return_buffer);
 	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
+		goto exit;
 	}
 
-	/* Type ANY means "don't care" */
+	/* Type ANY means "don't care about return value type" */
 
 	if (return_type == ACPI_TYPE_ANY) {
-		return_ACPI_STATUS(AE_OK);
+		goto exit;
 	}
 
 	if (return_buffer->length == 0) {
 
 		/* Error because caller specifically asked for a return value */
 
-		ACPI_ERROR((AE_INFO, "No return value"));
-		return_ACPI_STATUS(AE_NULL_OBJECT);
+		ACPI_ERROR((AE_INFO, "%s did not return any object",
+			    full_pathname));
+		status = AE_NULL_OBJECT;
+		goto exit;
 	}
 
 	/* Examine the object type returned from evaluate_object */
 
 	if (((union acpi_object *)return_buffer->pointer)->type == return_type) {
-		return_ACPI_STATUS(AE_OK);
+		goto exit;
 	}
 
 	/* Return object type does not match requested type */
 
 	ACPI_ERROR((AE_INFO,
-		    "Incorrect return type [%s] requested [%s]",
+		    "Incorrect return type from %s - received [%s], requested [%s]",
+		    full_pathname,
 		    acpi_ut_get_type_name(((union acpi_object *)return_buffer->
 					   pointer)->type),
 		    acpi_ut_get_type_name(return_type)));
@@ -147,7 +162,11 @@ acpi_evaluate_object_typed(acpi_handle handle,
 	}
 
 	return_buffer->length = 0;
-	return_ACPI_STATUS(AE_TYPE);
+	status = AE_TYPE;
+
+exit:
+	ACPI_FREE(full_pathname);
+	return_ACPI_STATUS(status);
 }
 
 ACPI_EXPORT_SYMBOL(acpi_evaluate_object_typed)
diff --git a/drivers/acpi/acpica/psobject.c b/drivers/acpi/acpica/psobject.c
index 5bcb61831706..ef6384e374fc 100644
--- a/drivers/acpi/acpica/psobject.c
+++ b/drivers/acpi/acpica/psobject.c
@@ -122,6 +122,9 @@ static acpi_status acpi_ps_get_aml_opcode(struct acpi_walk_state *walk_state)
 			     (u32)(aml_offset +
 				   sizeof(struct acpi_table_header)));
 
+			ACPI_ERROR((AE_INFO,
+				    "Aborting disassembly, AML byte code is corrupt"));
+
 			/* Dump the context surrounding the invalid opcode */
 
 			acpi_ut_dump_buffer(((u8 *)walk_state->parser_state.
@@ -130,6 +133,14 @@ static acpi_status acpi_ps_get_aml_opcode(struct acpi_walk_state *walk_state)
 					     sizeof(struct acpi_table_header) -
 					     16));
 			acpi_os_printf(" */\n");
+
+			/*
+			 * Just abort the disassembly, cannot continue because the
+			 * parser is essentially lost. The disassembler can then
+			 * randomly fail because an ill-constructed parse tree
+			 * can result.
+			 */
+			return_ACPI_STATUS(AE_AML_BAD_OPCODE);
 #endif
 		}
 
@@ -331,6 +342,9 @@ acpi_ps_create_op(struct acpi_walk_state *walk_state,
 	if (status == AE_CTRL_PARSE_CONTINUE) {
 		return_ACPI_STATUS(AE_CTRL_PARSE_CONTINUE);
 	}
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
 
 	/* Create Op structure and append to parent's argument list */
 
diff --git a/drivers/acpi/acpica/psopcode.c b/drivers/acpi/acpica/psopcode.c
index c343a0d5a3d2..a402ad772a1e 100644
--- a/drivers/acpi/acpica/psopcode.c
+++ b/drivers/acpi/acpica/psopcode.c
@@ -650,9 +650,11 @@ const struct acpi_opcode_info acpi_gbl_aml_op_info[AML_NUM_OPCODES] = {
 
 /* ACPI 6.0 opcodes */
 
-	/* 81 */ ACPI_OP("External", ARGP_EXTERNAL_OP, ARGI_EXTERNAL_OP,
-			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE, /* ? */
-			 AML_TYPE_EXEC_3A_0T_0R, AML_FLAGS_EXEC_3A_0T_0R),
+/* 81 */ ACPI_OP("External", ARGP_EXTERNAL_OP, ARGI_EXTERNAL_OP,
+			 ACPI_TYPE_ANY, AML_CLASS_NAMED_OBJECT,
+			 AML_TYPE_NAMED_SIMPLE,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 AML_NSNODE | AML_NAMED),
 /* 82 */ ACPI_OP("Comment", ARGP_COMMENT_OP, ARGI_COMMENT_OP,
 			 ACPI_TYPE_STRING, AML_CLASS_ARGUMENT,
 			 AML_TYPE_LITERAL, AML_CONSTANT)
diff --git a/drivers/acpi/acpica/psparse.c b/drivers/acpi/acpica/psparse.c
index 8116a670de39..ac88319dc111 100644
--- a/drivers/acpi/acpica/psparse.c
+++ b/drivers/acpi/acpica/psparse.c
@@ -56,6 +56,7 @@
 #include "acdispat.h"
 #include "amlcode.h"
 #include "acinterp.h"
+#include "acnamesp.h"
 
 #define _COMPONENT          ACPI_PARSER
 ACPI_MODULE_NAME("psparse")
@@ -538,9 +539,16 @@ acpi_status acpi_ps_parse_aml(struct acpi_walk_state *walk_state)
 			/* Either the method parse or actual execution failed */
 
 			acpi_ex_exit_interpreter();
-			ACPI_ERROR_METHOD("Method parse/execution failed",
-					  walk_state->method_node, NULL,
-					  status);
+			if (status == AE_ABORT_METHOD) {
+				acpi_ns_print_node_pathname(walk_state->
+							    method_node,
+							    "Method aborted:");
+				acpi_os_printf("\n");
+			} else {
+				ACPI_ERROR_METHOD
+				    ("Method parse/execution failed",
+				     walk_state->method_node, NULL, status);
+			}
 			acpi_ex_enter_interpreter();
 
 			/* Check for possible multi-thread reentrancy problem */
diff --git a/drivers/acpi/acpica/rscalc.c b/drivers/acpi/acpica/rscalc.c
index 74e47f829ccb..659fb718504a 100644
--- a/drivers/acpi/acpica/rscalc.c
+++ b/drivers/acpi/acpica/rscalc.c
@@ -340,6 +340,22 @@ acpi_rs_get_aml_length(struct acpi_resource *resource,
 
 			break;
 
+		case ACPI_RESOURCE_TYPE_PIN_FUNCTION:
+
+			total_size = (acpi_rs_length)(total_size +
+						      (resource->data.
+						       pin_function.
+						       pin_table_length * 2) +
+						      resource->data.
+						      pin_function.
+						      resource_source.
+						      string_length +
+						      resource->data.
+						      pin_function.
+						      vendor_length);
+
+			break;
+
 		case ACPI_RESOURCE_TYPE_SERIAL_BUS:
 
 			total_size =
@@ -359,6 +375,67 @@ acpi_rs_get_aml_length(struct acpi_resource *resource,
 
 			break;
 
+		case ACPI_RESOURCE_TYPE_PIN_CONFIG:
+
+			total_size = (acpi_rs_length)(total_size +
+						      (resource->data.
+						       pin_config.
+						       pin_table_length * 2) +
+						      resource->data.pin_config.
+						      resource_source.
+						      string_length +
+						      resource->data.pin_config.
+						      vendor_length);
+
+			break;
+
+		case ACPI_RESOURCE_TYPE_PIN_GROUP:
+
+			total_size = (acpi_rs_length)(total_size +
+						      (resource->data.pin_group.
+						       pin_table_length * 2) +
+						      resource->data.pin_group.
+						      resource_label.
+						      string_length +
+						      resource->data.pin_group.
+						      vendor_length);
+
+			break;
+
+		case ACPI_RESOURCE_TYPE_PIN_GROUP_FUNCTION:
+
+			total_size = (acpi_rs_length)(total_size +
+						      resource->data.
+						      pin_group_function.
+						      resource_source.
+						      string_length +
+						      resource->data.
+						      pin_group_function.
+						      resource_source_label.
+						      string_length +
+						      resource->data.
+						      pin_group_function.
+						      vendor_length);
+
+			break;
+
+		case ACPI_RESOURCE_TYPE_PIN_GROUP_CONFIG:
+
+			total_size = (acpi_rs_length)(total_size +
+						      resource->data.
+						      pin_group_config.
+						      resource_source.
+						      string_length +
+						      resource->data.
+						      pin_group_config.
+						      resource_source_label.
+						      string_length +
+						      resource->data.
+						      pin_group_config.
+						      vendor_length);
+
+			break;
+
 		default:
 
 			break;
@@ -537,6 +614,24 @@ acpi_rs_get_list_length(u8 *aml_buffer,
 			}
 			break;
 
+		case ACPI_RESOURCE_NAME_PIN_FUNCTION:
+
+			/* Vendor data is optional */
+
+			if (aml_resource->pin_function.vendor_length) {
+				extra_struct_bytes +=
+				    aml_resource->pin_function.vendor_offset -
+				    aml_resource->pin_function.
+				    pin_table_offset +
+				    aml_resource->pin_function.vendor_length;
+			} else {
+				extra_struct_bytes +=
+				    aml_resource->large_header.resource_length +
+				    sizeof(struct aml_resource_large_header) -
+				    aml_resource->pin_function.pin_table_offset;
+			}
+			break;
+
 		case ACPI_RESOURCE_NAME_SERIAL_BUS:
 
 			minimum_aml_resource_length =
@@ -547,6 +642,50 @@ acpi_rs_get_list_length(u8 *aml_buffer,
 			    minimum_aml_resource_length;
 			break;
 
+		case ACPI_RESOURCE_NAME_PIN_CONFIG:
+
+			/* Vendor data is optional */
+
+			if (aml_resource->pin_config.vendor_length) {
+				extra_struct_bytes +=
+				    aml_resource->pin_config.vendor_offset -
+				    aml_resource->pin_config.pin_table_offset +
+				    aml_resource->pin_config.vendor_length;
+			} else {
+				extra_struct_bytes +=
+				    aml_resource->large_header.resource_length +
+				    sizeof(struct aml_resource_large_header) -
+				    aml_resource->pin_config.pin_table_offset;
+			}
+			break;
+
+		case ACPI_RESOURCE_NAME_PIN_GROUP:
+
+			extra_struct_bytes +=
+			    aml_resource->pin_group.vendor_offset -
+			    aml_resource->pin_group.pin_table_offset +
+			    aml_resource->pin_group.vendor_length;
+
+			break;
+
+		case ACPI_RESOURCE_NAME_PIN_GROUP_FUNCTION:
+
+			extra_struct_bytes +=
+			    aml_resource->pin_group_function.vendor_offset -
+			    aml_resource->pin_group_function.res_source_offset +
+			    aml_resource->pin_group_function.vendor_length;
+
+			break;
+
+		case ACPI_RESOURCE_NAME_PIN_GROUP_CONFIG:
+
+			extra_struct_bytes +=
+			    aml_resource->pin_group_config.vendor_offset -
+			    aml_resource->pin_group_config.res_source_offset +
+			    aml_resource->pin_group_config.vendor_length;
+
+			break;
+
 		default:
 
 			break;
diff --git a/drivers/acpi/acpica/rsdump.c b/drivers/acpi/acpica/rsdump.c
index f4cdf8d832dc..55fd1880efbe 100644
--- a/drivers/acpi/acpica/rsdump.c
+++ b/drivers/acpi/acpica/rsdump.c
@@ -75,6 +75,10 @@ static void acpi_rs_dump_short_byte_list(u8 length, u8 *data);
 static void
 acpi_rs_dump_resource_source(struct acpi_resource_source *resource_source);
 
+static void
+acpi_rs_dump_resource_label(char *title,
+			    struct acpi_resource_label *resource_label);
+
 static void acpi_rs_dump_address_common(union acpi_resource_data *resource);
 
 static void
@@ -371,6 +375,26 @@ acpi_rs_dump_descriptor(void *resource, struct acpi_rsdump_info *table)
 								   target));
 			break;
 
+		case ACPI_RSD_LABEL:
+			/*
+			 * resource_label
+			 */
+			acpi_rs_dump_resource_label("Resource Label",
+						    ACPI_CAST_PTR(struct
+								  acpi_resource_label,
+								  target));
+			break;
+
+		case ACPI_RSD_SOURCE_LABEL:
+			/*
+			 * resource_source_label
+			 */
+			acpi_rs_dump_resource_label("Resource Source Label",
+						    ACPI_CAST_PTR(struct
+								  acpi_resource_label,
+								  target));
+			break;
+
 		default:
 
 			acpi_os_printf("**** Invalid table opcode [%X] ****\n",
@@ -414,6 +438,30 @@ acpi_rs_dump_resource_source(struct acpi_resource_source *resource_source)
 
 /*******************************************************************************
  *
+ * FUNCTION:    acpi_rs_dump_resource_label
+ *
+ * PARAMETERS:  title              - Title of the dumped resource field
+ *              resource_label     - Pointer to a Resource Label struct
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Common routine for dumping the resource_label
+ *
+ ******************************************************************************/
+
+static void
+acpi_rs_dump_resource_label(char *title,
+			    struct acpi_resource_label *resource_label)
+{
+	ACPI_FUNCTION_ENTRY();
+
+	acpi_rs_out_string(title,
+			   resource_label->string_ptr ?
+			   resource_label->string_ptr : "[Not Specified]");
+}
+
+/*******************************************************************************
+ *
  * FUNCTION:    acpi_rs_dump_address_common
  *
  * PARAMETERS:  resource        - Pointer to an internal resource descriptor
diff --git a/drivers/acpi/acpica/rsdumpinfo.c b/drivers/acpi/acpica/rsdumpinfo.c
index 8aacd28293fa..da150e17795b 100644
--- a/drivers/acpi/acpica/rsdumpinfo.c
+++ b/drivers/acpi/acpica/rsdumpinfo.c
@@ -314,6 +314,120 @@ struct acpi_rsdump_info acpi_rs_dump_gpio[16] = {
 	 NULL},
 };
 
+struct acpi_rsdump_info acpi_rs_dump_pin_function[10] = {
+	{ACPI_RSD_TITLE, ACPI_RSD_TABLE_SIZE(acpi_rs_dump_pin_function),
+	 "PinFunction", NULL},
+	{ACPI_RSD_UINT8, ACPI_RSD_OFFSET(pin_function.revision_id),
+	 "RevisionId", NULL},
+	{ACPI_RSD_UINT8, ACPI_RSD_OFFSET(pin_function.pin_config), "PinConfig",
+	 acpi_gbl_ppc_decode},
+	{ACPI_RSD_1BITFLAG, ACPI_RSD_OFFSET(pin_function.sharable), "Sharing",
+	 acpi_gbl_shr_decode},
+	{ACPI_RSD_UINT16, ACPI_RSD_OFFSET(pin_function.function_number),
+	 "FunctionNumber", NULL},
+	{ACPI_RSD_SOURCE, ACPI_RSD_OFFSET(pin_function.resource_source),
+	 "ResourceSource", NULL},
+	{ACPI_RSD_UINT16, ACPI_RSD_OFFSET(pin_function.pin_table_length),
+	 "PinTableLength", NULL},
+	{ACPI_RSD_WORDLIST, ACPI_RSD_OFFSET(pin_function.pin_table), "PinTable",
+	 NULL},
+	{ACPI_RSD_UINT16, ACPI_RSD_OFFSET(pin_function.vendor_length),
+	 "VendorLength", NULL},
+	{ACPI_RSD_SHORTLISTX, ACPI_RSD_OFFSET(pin_function.vendor_data),
+	 "VendorData", NULL},
+};
+
+struct acpi_rsdump_info acpi_rs_dump_pin_config[11] = {
+	{ACPI_RSD_TITLE, ACPI_RSD_TABLE_SIZE(acpi_rs_dump_pin_config),
+	 "PinConfig", NULL},
+	{ACPI_RSD_UINT8, ACPI_RSD_OFFSET(pin_config.revision_id), "RevisionId",
+	 NULL},
+	{ACPI_RSD_1BITFLAG, ACPI_RSD_OFFSET(pin_config.producer_consumer),
+	 "ProducerConsumer", acpi_gbl_consume_decode},
+	{ACPI_RSD_1BITFLAG, ACPI_RSD_OFFSET(pin_config.sharable), "Sharing",
+	 acpi_gbl_shr_decode},
+	{ACPI_RSD_UINT8, ACPI_RSD_OFFSET(pin_config.pin_config_type),
+	 "PinConfigType", NULL},
+	{ACPI_RSD_UINT32, ACPI_RSD_OFFSET(pin_config.pin_config_value),
+	 "PinConfigValue", NULL},
+	{ACPI_RSD_SOURCE, ACPI_RSD_OFFSET(pin_config.resource_source),
+	 "ResourceSource", NULL},
+	{ACPI_RSD_UINT16, ACPI_RSD_OFFSET(pin_config.pin_table_length),
+	 "PinTableLength", NULL},
+	{ACPI_RSD_WORDLIST, ACPI_RSD_OFFSET(pin_config.pin_table), "PinTable",
+	 NULL},
+	{ACPI_RSD_UINT16, ACPI_RSD_OFFSET(pin_config.vendor_length),
+	 "VendorLength", NULL},
+	{ACPI_RSD_SHORTLISTX, ACPI_RSD_OFFSET(pin_config.vendor_data),
+	 "VendorData", NULL},
+};
+
+struct acpi_rsdump_info acpi_rs_dump_pin_group[8] = {
+	{ACPI_RSD_TITLE, ACPI_RSD_TABLE_SIZE(acpi_rs_dump_pin_group),
+	 "PinGroup", NULL},
+	{ACPI_RSD_UINT8, ACPI_RSD_OFFSET(pin_group.revision_id), "RevisionId",
+	 NULL},
+	{ACPI_RSD_1BITFLAG, ACPI_RSD_OFFSET(pin_group.producer_consumer),
+	 "ProducerConsumer", acpi_gbl_consume_decode},
+	{ACPI_RSD_UINT16, ACPI_RSD_OFFSET(pin_group.pin_table_length),
+	 "PinTableLength", NULL},
+	{ACPI_RSD_WORDLIST, ACPI_RSD_OFFSET(pin_group.pin_table), "PinTable",
+	 NULL},
+	{ACPI_RSD_LABEL, ACPI_RSD_OFFSET(pin_group.resource_label),
+	 "ResourceLabel", NULL},
+	{ACPI_RSD_UINT16, ACPI_RSD_OFFSET(pin_group.vendor_length),
+	 "VendorLength", NULL},
+	{ACPI_RSD_SHORTLISTX, ACPI_RSD_OFFSET(pin_group.vendor_data),
+	 "VendorData", NULL},
+};
+
+struct acpi_rsdump_info acpi_rs_dump_pin_group_function[9] = {
+	{ACPI_RSD_TITLE, ACPI_RSD_TABLE_SIZE(acpi_rs_dump_pin_group_function),
+	 "PinGroupFunction", NULL},
+	{ACPI_RSD_UINT8, ACPI_RSD_OFFSET(pin_group_function.revision_id),
+	 "RevisionId", NULL},
+	{ACPI_RSD_1BITFLAG,
+	 ACPI_RSD_OFFSET(pin_group_function.producer_consumer),
+	 "ProducerConsumer", acpi_gbl_consume_decode},
+	{ACPI_RSD_1BITFLAG, ACPI_RSD_OFFSET(pin_group_function.sharable),
+	 "Sharing", acpi_gbl_shr_decode},
+	{ACPI_RSD_UINT16, ACPI_RSD_OFFSET(pin_group_function.function_number),
+	 "FunctionNumber", NULL},
+	{ACPI_RSD_SOURCE_LABEL,
+	 ACPI_RSD_OFFSET(pin_group_function.resource_source_label),
+	 "ResourceSourceLabel", NULL},
+	{ACPI_RSD_SOURCE, ACPI_RSD_OFFSET(pin_group_function.resource_source),
+	 "ResourceSource", NULL},
+	{ACPI_RSD_UINT16, ACPI_RSD_OFFSET(pin_group_function.vendor_length),
+	 "VendorLength", NULL},
+	{ACPI_RSD_SHORTLISTX, ACPI_RSD_OFFSET(pin_group_function.vendor_data),
+	 "VendorData", NULL},
+};
+
+struct acpi_rsdump_info acpi_rs_dump_pin_group_config[10] = {
+	{ACPI_RSD_TITLE, ACPI_RSD_TABLE_SIZE(acpi_rs_dump_pin_group_config),
+	 "PinGroupConfig", NULL},
+	{ACPI_RSD_UINT8, ACPI_RSD_OFFSET(pin_group_config.revision_id),
+	 "RevisionId", NULL},
+	{ACPI_RSD_1BITFLAG, ACPI_RSD_OFFSET(pin_group_config.producer_consumer),
+	 "ProducerConsumer", acpi_gbl_consume_decode},
+	{ACPI_RSD_1BITFLAG, ACPI_RSD_OFFSET(pin_group_config.sharable),
+	 "Sharing", acpi_gbl_shr_decode},
+	{ACPI_RSD_UINT8, ACPI_RSD_OFFSET(pin_group_config.pin_config_type),
+	 "PinConfigType", NULL},
+	{ACPI_RSD_UINT32, ACPI_RSD_OFFSET(pin_group_config.pin_config_value),
+	 "PinConfigValue", NULL},
+	{ACPI_RSD_SOURCE_LABEL,
+	 ACPI_RSD_OFFSET(pin_group_config.resource_source_label),
+	 "ResourceSourceLabel", NULL},
+	{ACPI_RSD_SOURCE, ACPI_RSD_OFFSET(pin_group_config.resource_source),
+	 "ResourceSource", NULL},
+	{ACPI_RSD_UINT16, ACPI_RSD_OFFSET(pin_group_config.vendor_length),
+	 "VendorLength", NULL},
+	{ACPI_RSD_SHORTLISTX, ACPI_RSD_OFFSET(pin_group_config.vendor_data),
+	 "VendorData", NULL},
+};
+
 struct acpi_rsdump_info acpi_rs_dump_fixed_dma[4] = {
 	{ACPI_RSD_TITLE, ACPI_RSD_TABLE_SIZE(acpi_rs_dump_fixed_dma),
 	 "FixedDma", NULL},
diff --git a/drivers/acpi/acpica/rsinfo.c b/drivers/acpi/acpica/rsinfo.c
index 475da9d6aed5..b0e50518d766 100644
--- a/drivers/acpi/acpica/rsinfo.c
+++ b/drivers/acpi/acpica/rsinfo.c
@@ -80,6 +80,11 @@ struct acpi_rsconvert_info *acpi_gbl_set_resource_dispatch[] = {
 	acpi_rs_convert_gpio,	/* 0x11, ACPI_RESOURCE_TYPE_GPIO */
 	acpi_rs_convert_fixed_dma,	/* 0x12, ACPI_RESOURCE_TYPE_FIXED_DMA */
 	NULL,			/* 0x13, ACPI_RESOURCE_TYPE_SERIAL_BUS - Use subtype table below */
+	acpi_rs_convert_pin_function,	/* 0x14, ACPI_RESOURCE_TYPE_PIN_FUNCTION */
+	acpi_rs_convert_pin_config,	/* 0x15, ACPI_RESOURCE_TYPE_PIN_CONFIG */
+	acpi_rs_convert_pin_group,	/* 0x16, ACPI_RESOURCE_TYPE_PIN_GROUP */
+	acpi_rs_convert_pin_group_function,	/* 0x17, ACPI_RESOURCE_TYPE_PIN_GROUP_FUNCTION */
+	acpi_rs_convert_pin_group_config,	/* 0x18, ACPI_RESOURCE_TYPE_PIN_GROUP_CONFIG */
 };
 
 /* Dispatch tables for AML-to-resource (Get Resource) conversion functions */
@@ -119,8 +124,12 @@ struct acpi_rsconvert_info *acpi_gbl_get_resource_dispatch[] = {
 	acpi_rs_convert_address64,	/* 0x0A, ACPI_RESOURCE_NAME_ADDRESS64 */
 	acpi_rs_convert_ext_address64,	/* 0x0B, ACPI_RESOURCE_NAME_EXTENDED_ADDRESS64 */
 	acpi_rs_convert_gpio,	/* 0x0C, ACPI_RESOURCE_NAME_GPIO */
-	NULL,			/* 0x0D, Reserved */
+	acpi_rs_convert_pin_function,	/* 0x0D, ACPI_RESOURCE_NAME_PIN_FUNCTION */
 	NULL,			/* 0x0E, ACPI_RESOURCE_NAME_SERIAL_BUS - Use subtype table below */
+	acpi_rs_convert_pin_config,	/* 0x0F, ACPI_RESOURCE_NAME_PIN_CONFIG */
+	acpi_rs_convert_pin_group,	/* 0x10, ACPI_RESOURCE_NAME_PIN_GROUP */
+	acpi_rs_convert_pin_group_function,	/* 0x11, ACPI_RESOURCE_NAME_PIN_GROUP_FUNCTION */
+	acpi_rs_convert_pin_group_config,	/* 0x12, ACPI_RESOURCE_NAME_PIN_GROUP_CONFIG */
 };
 
 /* Subtype table for serial_bus -- I2C, SPI, and UART */
@@ -157,6 +166,11 @@ struct acpi_rsdump_info *acpi_gbl_dump_resource_dispatch[] = {
 	acpi_rs_dump_gpio,	/* ACPI_RESOURCE_TYPE_GPIO */
 	acpi_rs_dump_fixed_dma,	/* ACPI_RESOURCE_TYPE_FIXED_DMA */
 	NULL,			/* ACPI_RESOURCE_TYPE_SERIAL_BUS */
+	acpi_rs_dump_pin_function,	/* ACPI_RESOURCE_TYPE_PIN_FUNCTION */
+	acpi_rs_dump_pin_config,	/* ACPI_RESOURCE_TYPE_PIN_CONFIG */
+	acpi_rs_dump_pin_group,	/* ACPI_RESOURCE_TYPE_PIN_GROUP */
+	acpi_rs_dump_pin_group_function,	/* ACPI_RESOURCE_TYPE_PIN_GROUP_FUNCTION */
+	acpi_rs_dump_pin_group_config,	/* ACPI_RESOURCE_TYPE_PIN_GROUP_CONFIG */
 };
 
 struct acpi_rsdump_info *acpi_gbl_dump_serial_bus_dispatch[] = {
@@ -193,6 +207,11 @@ const u8 acpi_gbl_aml_resource_sizes[] = {
 	sizeof(struct aml_resource_gpio),	/* ACPI_RESOURCE_TYPE_GPIO */
 	sizeof(struct aml_resource_fixed_dma),	/* ACPI_RESOURCE_TYPE_FIXED_DMA */
 	sizeof(struct aml_resource_common_serialbus),	/* ACPI_RESOURCE_TYPE_SERIAL_BUS */
+	sizeof(struct aml_resource_pin_function),	/* ACPI_RESOURCE_TYPE_PIN_FUNCTION */
+	sizeof(struct aml_resource_pin_config),	/* ACPI_RESOURCE_TYPE_PIN_CONFIG */
+	sizeof(struct aml_resource_pin_group),	/* ACPI_RESOURCE_TYPE_PIN_GROUP */
+	sizeof(struct aml_resource_pin_group_function),	/* ACPI_RESOURCE_TYPE_PIN_GROUP_FUNCTION */
+	sizeof(struct aml_resource_pin_group_config),	/* ACPI_RESOURCE_TYPE_PIN_GROUP_CONFIG */
 };
 
 const u8 acpi_gbl_resource_struct_sizes[] = {
@@ -230,7 +249,12 @@ const u8 acpi_gbl_resource_struct_sizes[] = {
 	ACPI_RS_SIZE(struct acpi_resource_address64),
 	ACPI_RS_SIZE(struct acpi_resource_extended_address64),
 	ACPI_RS_SIZE(struct acpi_resource_gpio),
-	ACPI_RS_SIZE(struct acpi_resource_common_serialbus)
+	ACPI_RS_SIZE(struct acpi_resource_pin_function),
+	ACPI_RS_SIZE(struct acpi_resource_common_serialbus),
+	ACPI_RS_SIZE(struct acpi_resource_pin_config),
+	ACPI_RS_SIZE(struct acpi_resource_pin_group),
+	ACPI_RS_SIZE(struct acpi_resource_pin_group_function),
+	ACPI_RS_SIZE(struct acpi_resource_pin_group_config),
 };
 
 const u8 acpi_gbl_aml_resource_serial_bus_sizes[] = {
diff --git a/drivers/acpi/acpica/rsmisc.c b/drivers/acpi/acpica/rsmisc.c
index 2ae79613f6b7..cc4b5486c4bc 100644
--- a/drivers/acpi/acpica/rsmisc.c
+++ b/drivers/acpi/acpica/rsmisc.c
@@ -596,9 +596,7 @@ acpi_rs_convert_resource_to_aml(struct acpi_resource *resource,
 
 			/* Set vendor offset only if there is vendor data */
 
-			if (resource->data.gpio.vendor_length) {
-				ACPI_SET16(target, aml_length);
-			}
+			ACPI_SET16(target, aml_length);
 
 			acpi_rs_set_resource_length(aml_length, aml);
 			break;
diff --git a/drivers/acpi/acpica/rsserial.c b/drivers/acpi/acpica/rsserial.c
index c20e6d07928d..14d12d6eb716 100644
--- a/drivers/acpi/acpica/rsserial.c
+++ b/drivers/acpi/acpica/rsserial.c
@@ -147,6 +147,82 @@ struct acpi_rsconvert_info acpi_rs_convert_gpio[18] = {
 
 /*******************************************************************************
  *
+ * acpi_rs_convert_pinfunction
+ *
+ ******************************************************************************/
+
+struct acpi_rsconvert_info acpi_rs_convert_pin_function[13] = {
+	{ACPI_RSC_INITGET, ACPI_RESOURCE_TYPE_PIN_FUNCTION,
+	 ACPI_RS_SIZE(struct acpi_resource_pin_function),
+	 ACPI_RSC_TABLE_SIZE(acpi_rs_convert_pin_function)},
+
+	{ACPI_RSC_INITSET, ACPI_RESOURCE_NAME_PIN_FUNCTION,
+	 sizeof(struct aml_resource_pin_function),
+	 0},
+
+	{ACPI_RSC_MOVE8, ACPI_RS_OFFSET(data.pin_function.revision_id),
+	 AML_OFFSET(pin_function.revision_id),
+	 1},
+
+	{ACPI_RSC_1BITFLAG, ACPI_RS_OFFSET(data.pin_function.sharable),
+	 AML_OFFSET(pin_function.flags),
+	 0},
+
+	{ACPI_RSC_MOVE8, ACPI_RS_OFFSET(data.pin_function.pin_config),
+	 AML_OFFSET(pin_function.pin_config),
+	 1},
+
+	{ACPI_RSC_MOVE16, ACPI_RS_OFFSET(data.pin_function.function_number),
+	 AML_OFFSET(pin_function.function_number),
+	 2},
+
+	/* Pin Table */
+
+	/*
+	 * It is OK to use GPIO operations here because none of them refer GPIO
+	 * structures directly but instead use offsets given here.
+	 */
+
+	{ACPI_RSC_COUNT_GPIO_PIN,
+	 ACPI_RS_OFFSET(data.pin_function.pin_table_length),
+	 AML_OFFSET(pin_function.pin_table_offset),
+	 AML_OFFSET(pin_function.res_source_offset)},
+
+	{ACPI_RSC_MOVE_GPIO_PIN, ACPI_RS_OFFSET(data.pin_function.pin_table),
+	 AML_OFFSET(pin_function.pin_table_offset),
+	 0},
+
+	/* Resource Source */
+
+	{ACPI_RSC_MOVE8,
+	 ACPI_RS_OFFSET(data.pin_function.resource_source.index),
+	 AML_OFFSET(pin_function.res_source_index),
+	 1},
+
+	{ACPI_RSC_COUNT_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_function.resource_source.string_length),
+	 AML_OFFSET(pin_function.res_source_offset),
+	 AML_OFFSET(pin_function.vendor_offset)},
+
+	{ACPI_RSC_MOVE_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_function.resource_source.string_ptr),
+	 AML_OFFSET(pin_function.res_source_offset),
+	 0},
+
+	/* Vendor Data */
+
+	{ACPI_RSC_COUNT_GPIO_VEN,
+	 ACPI_RS_OFFSET(data.pin_function.vendor_length),
+	 AML_OFFSET(pin_function.vendor_length),
+	 1},
+
+	{ACPI_RSC_MOVE_GPIO_RES, ACPI_RS_OFFSET(data.pin_function.vendor_data),
+	 AML_OFFSET(pin_function.vendor_offset),
+	 0},
+};
+
+/*******************************************************************************
+ *
  * acpi_rs_convert_i2c_serial_bus
  *
  ******************************************************************************/
@@ -458,3 +534,300 @@ struct acpi_rsconvert_info acpi_rs_convert_uart_serial_bus[23] = {
 	 AML_OFFSET(uart_serial_bus.default_baud_rate),
 	 1},
 };
+
+/*******************************************************************************
+ *
+ * acpi_rs_convert_pin_config
+ *
+ ******************************************************************************/
+
+struct acpi_rsconvert_info acpi_rs_convert_pin_config[14] = {
+	{ACPI_RSC_INITGET, ACPI_RESOURCE_TYPE_PIN_CONFIG,
+	 ACPI_RS_SIZE(struct acpi_resource_pin_config),
+	 ACPI_RSC_TABLE_SIZE(acpi_rs_convert_pin_config)},
+
+	{ACPI_RSC_INITSET, ACPI_RESOURCE_NAME_PIN_CONFIG,
+	 sizeof(struct aml_resource_pin_config),
+	 0},
+
+	{ACPI_RSC_MOVE8, ACPI_RS_OFFSET(data.pin_config.revision_id),
+	 AML_OFFSET(pin_config.revision_id),
+	 1},
+
+	{ACPI_RSC_1BITFLAG, ACPI_RS_OFFSET(data.pin_config.sharable),
+	 AML_OFFSET(pin_config.flags),
+	 0},
+
+	{ACPI_RSC_1BITFLAG, ACPI_RS_OFFSET(data.pin_config.producer_consumer),
+	 AML_OFFSET(pin_config.flags),
+	 1},
+
+	{ACPI_RSC_MOVE8, ACPI_RS_OFFSET(data.pin_config.pin_config_type),
+	 AML_OFFSET(pin_config.pin_config_type),
+	 1},
+
+	{ACPI_RSC_MOVE32, ACPI_RS_OFFSET(data.pin_config.pin_config_value),
+	 AML_OFFSET(pin_config.pin_config_value),
+	 1},
+
+	/* Pin Table */
+
+	/*
+	 * It is OK to use GPIO operations here because none of them refer GPIO
+	 * structures directly but instead use offsets given here.
+	 */
+
+	{ACPI_RSC_COUNT_GPIO_PIN,
+	 ACPI_RS_OFFSET(data.pin_config.pin_table_length),
+	 AML_OFFSET(pin_config.pin_table_offset),
+	 AML_OFFSET(pin_config.res_source_offset)},
+
+	{ACPI_RSC_MOVE_GPIO_PIN, ACPI_RS_OFFSET(data.pin_config.pin_table),
+	 AML_OFFSET(pin_config.pin_table_offset),
+	 0},
+
+	/* Resource Source */
+
+	{ACPI_RSC_MOVE8, ACPI_RS_OFFSET(data.pin_config.resource_source.index),
+	 AML_OFFSET(pin_config.res_source_index),
+	 1},
+
+	{ACPI_RSC_COUNT_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_config.resource_source.string_length),
+	 AML_OFFSET(pin_config.res_source_offset),
+	 AML_OFFSET(pin_config.vendor_offset)},
+
+	{ACPI_RSC_MOVE_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_config.resource_source.string_ptr),
+	 AML_OFFSET(pin_config.res_source_offset),
+	 0},
+
+	/* Vendor Data */
+
+	{ACPI_RSC_COUNT_GPIO_VEN, ACPI_RS_OFFSET(data.pin_config.vendor_length),
+	 AML_OFFSET(pin_config.vendor_length),
+	 1},
+
+	{ACPI_RSC_MOVE_GPIO_RES, ACPI_RS_OFFSET(data.pin_config.vendor_data),
+	 AML_OFFSET(pin_config.vendor_offset),
+	 0},
+};
+
+/*******************************************************************************
+ *
+ * acpi_rs_convert_pin_group
+ *
+ ******************************************************************************/
+
+struct acpi_rsconvert_info acpi_rs_convert_pin_group[10] = {
+	{ACPI_RSC_INITGET, ACPI_RESOURCE_TYPE_PIN_GROUP,
+	 ACPI_RS_SIZE(struct acpi_resource_pin_group),
+	 ACPI_RSC_TABLE_SIZE(acpi_rs_convert_pin_group)},
+
+	{ACPI_RSC_INITSET, ACPI_RESOURCE_NAME_PIN_GROUP,
+	 sizeof(struct aml_resource_pin_group),
+	 0},
+
+	{ACPI_RSC_MOVE8, ACPI_RS_OFFSET(data.pin_group.revision_id),
+	 AML_OFFSET(pin_group.revision_id),
+	 1},
+
+	{ACPI_RSC_1BITFLAG, ACPI_RS_OFFSET(data.pin_group.producer_consumer),
+	 AML_OFFSET(pin_group.flags),
+	 0},
+
+	/* Pin Table */
+
+	/*
+	 * It is OK to use GPIO operations here because none of them refer GPIO
+	 * structures directly but instead use offsets given here.
+	 */
+
+	{ACPI_RSC_COUNT_GPIO_PIN,
+	 ACPI_RS_OFFSET(data.pin_group.pin_table_length),
+	 AML_OFFSET(pin_group.pin_table_offset),
+	 AML_OFFSET(pin_group.label_offset)},
+
+	{ACPI_RSC_MOVE_GPIO_PIN, ACPI_RS_OFFSET(data.pin_group.pin_table),
+	 AML_OFFSET(pin_group.pin_table_offset),
+	 0},
+
+	/* Resource Label */
+
+	{ACPI_RSC_COUNT_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_group.resource_label.string_length),
+	 AML_OFFSET(pin_group.label_offset),
+	 AML_OFFSET(pin_group.vendor_offset)},
+
+	{ACPI_RSC_MOVE_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_group.resource_label.string_ptr),
+	 AML_OFFSET(pin_group.label_offset),
+	 0},
+
+	/* Vendor Data */
+
+	{ACPI_RSC_COUNT_GPIO_VEN, ACPI_RS_OFFSET(data.pin_group.vendor_length),
+	 AML_OFFSET(pin_group.vendor_length),
+	 1},
+
+	{ACPI_RSC_MOVE_GPIO_RES, ACPI_RS_OFFSET(data.pin_group.vendor_data),
+	 AML_OFFSET(pin_group.vendor_offset),
+	 0},
+};
+
+/*******************************************************************************
+ *
+ * acpi_rs_convert_pin_group_function
+ *
+ ******************************************************************************/
+
+struct acpi_rsconvert_info acpi_rs_convert_pin_group_function[13] = {
+	{ACPI_RSC_INITGET, ACPI_RESOURCE_TYPE_PIN_GROUP_FUNCTION,
+	 ACPI_RS_SIZE(struct acpi_resource_pin_group_function),
+	 ACPI_RSC_TABLE_SIZE(acpi_rs_convert_pin_group_function)},
+
+	{ACPI_RSC_INITSET, ACPI_RESOURCE_NAME_PIN_GROUP_FUNCTION,
+	 sizeof(struct aml_resource_pin_group_function),
+	 0},
+
+	{ACPI_RSC_MOVE8, ACPI_RS_OFFSET(data.pin_group_function.revision_id),
+	 AML_OFFSET(pin_group_function.revision_id),
+	 1},
+
+	{ACPI_RSC_1BITFLAG, ACPI_RS_OFFSET(data.pin_group_function.sharable),
+	 AML_OFFSET(pin_group_function.flags),
+	 0},
+
+	{ACPI_RSC_1BITFLAG,
+	 ACPI_RS_OFFSET(data.pin_group_function.producer_consumer),
+	 AML_OFFSET(pin_group_function.flags),
+	 1},
+
+	{ACPI_RSC_MOVE16,
+	 ACPI_RS_OFFSET(data.pin_group_function.function_number),
+	 AML_OFFSET(pin_group_function.function_number),
+	 1},
+
+	/* Resource Source */
+
+	{ACPI_RSC_MOVE8,
+	 ACPI_RS_OFFSET(data.pin_group_function.resource_source.index),
+	 AML_OFFSET(pin_group_function.res_source_index),
+	 1},
+
+	{ACPI_RSC_COUNT_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_group_function.resource_source.string_length),
+	 AML_OFFSET(pin_group_function.res_source_offset),
+	 AML_OFFSET(pin_group_function.res_source_label_offset)},
+
+	{ACPI_RSC_MOVE_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_group_function.resource_source.string_ptr),
+	 AML_OFFSET(pin_group_function.res_source_offset),
+	 0},
+
+	/* Resource Source Label */
+
+	{ACPI_RSC_COUNT_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_group_function.resource_source_label.
+			string_length),
+	 AML_OFFSET(pin_group_function.res_source_label_offset),
+	 AML_OFFSET(pin_group_function.vendor_offset)},
+
+	{ACPI_RSC_MOVE_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_group_function.resource_source_label.
+			string_ptr),
+	 AML_OFFSET(pin_group_function.res_source_label_offset),
+	 0},
+
+	/* Vendor Data */
+
+	{ACPI_RSC_COUNT_GPIO_VEN,
+	 ACPI_RS_OFFSET(data.pin_group_function.vendor_length),
+	 AML_OFFSET(pin_group_function.vendor_length),
+	 1},
+
+	{ACPI_RSC_MOVE_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_group_function.vendor_data),
+	 AML_OFFSET(pin_group_function.vendor_offset),
+	 0},
+};
+
+/*******************************************************************************
+ *
+ * acpi_rs_convert_pin_group_config
+ *
+ ******************************************************************************/
+
+struct acpi_rsconvert_info acpi_rs_convert_pin_group_config[14] = {
+	{ACPI_RSC_INITGET, ACPI_RESOURCE_TYPE_PIN_GROUP_CONFIG,
+	 ACPI_RS_SIZE(struct acpi_resource_pin_group_config),
+	 ACPI_RSC_TABLE_SIZE(acpi_rs_convert_pin_group_config)},
+
+	{ACPI_RSC_INITSET, ACPI_RESOURCE_NAME_PIN_GROUP_CONFIG,
+	 sizeof(struct aml_resource_pin_group_config),
+	 0},
+
+	{ACPI_RSC_MOVE8, ACPI_RS_OFFSET(data.pin_group_config.revision_id),
+	 AML_OFFSET(pin_group_config.revision_id),
+	 1},
+
+	{ACPI_RSC_1BITFLAG, ACPI_RS_OFFSET(data.pin_group_config.sharable),
+	 AML_OFFSET(pin_group_config.flags),
+	 0},
+
+	{ACPI_RSC_1BITFLAG,
+	 ACPI_RS_OFFSET(data.pin_group_config.producer_consumer),
+	 AML_OFFSET(pin_group_config.flags),
+	 1},
+
+	{ACPI_RSC_MOVE8, ACPI_RS_OFFSET(data.pin_group_config.pin_config_type),
+	 AML_OFFSET(pin_group_config.pin_config_type),
+	 1},
+
+	{ACPI_RSC_MOVE32,
+	 ACPI_RS_OFFSET(data.pin_group_config.pin_config_value),
+	 AML_OFFSET(pin_group_config.pin_config_value),
+	 1},
+
+	/* Resource Source */
+
+	{ACPI_RSC_MOVE8,
+	 ACPI_RS_OFFSET(data.pin_group_config.resource_source.index),
+	 AML_OFFSET(pin_group_config.res_source_index),
+	 1},
+
+	{ACPI_RSC_COUNT_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_group_config.resource_source.string_length),
+	 AML_OFFSET(pin_group_config.res_source_offset),
+	 AML_OFFSET(pin_group_config.res_source_label_offset)},
+
+	{ACPI_RSC_MOVE_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_group_config.resource_source.string_ptr),
+	 AML_OFFSET(pin_group_config.res_source_offset),
+	 0},
+
+	/* Resource Source Label */
+
+	{ACPI_RSC_COUNT_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_group_config.resource_source_label.
+			string_length),
+	 AML_OFFSET(pin_group_config.res_source_label_offset),
+	 AML_OFFSET(pin_group_config.vendor_offset)},
+
+	{ACPI_RSC_MOVE_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_group_config.resource_source_label.string_ptr),
+	 AML_OFFSET(pin_group_config.res_source_label_offset),
+	 0},
+
+	/* Vendor Data */
+
+	{ACPI_RSC_COUNT_GPIO_VEN,
+	 ACPI_RS_OFFSET(data.pin_group_config.vendor_length),
+	 AML_OFFSET(pin_group_config.vendor_length),
+	 1},
+
+	{ACPI_RSC_MOVE_GPIO_RES,
+	 ACPI_RS_OFFSET(data.pin_group_config.vendor_data),
+	 AML_OFFSET(pin_group_config.vendor_offset),
+	 0},
+};
diff --git a/drivers/acpi/acpica/tbdata.c b/drivers/acpi/acpica/tbdata.c
index 27c5c27d4818..c9d6fa6d7cc6 100644
--- a/drivers/acpi/acpica/tbdata.c
+++ b/drivers/acpi/acpica/tbdata.c
@@ -867,6 +867,8 @@ exit:
 	return_ACPI_STATUS(status);
 }
 
+ACPI_EXPORT_SYMBOL(acpi_tb_install_and_load_table)
+
 /*******************************************************************************
  *
  * FUNCTION:    acpi_tb_unload_table
@@ -914,3 +916,5 @@ acpi_status acpi_tb_unload_table(u32 table_index)
 	acpi_tb_set_table_loaded_flag(table_index, FALSE);
 	return_ACPI_STATUS(status);
 }
+
+ACPI_EXPORT_SYMBOL(acpi_tb_unload_table)
diff --git a/drivers/acpi/acpica/tbfadt.c b/drivers/acpi/acpica/tbfadt.c
index 51860bfc111e..5f051d82188d 100644
--- a/drivers/acpi/acpica/tbfadt.c
+++ b/drivers/acpi/acpica/tbfadt.c
@@ -449,8 +449,8 @@ void acpi_tb_create_local_fadt(struct acpi_table_header *table, u32 length)
  * The 64-bit X fields are optional extensions to the original 32-bit FADT
  * V1.0 fields. Even if they are present in the FADT, they are optional and
  * are unused if the BIOS sets them to zero. Therefore, we must copy/expand
- * 32-bit V1.0 fields to the 64-bit X fields if the the 64-bit X field is
- * originally zero.
+ * 32-bit V1.0 fields to the 64-bit X fields if the 64-bit X field is originally
+ * zero.
  *
  * For ACPI 1.0 FADTs (that contain no 64-bit addresses), all 32-bit address
  * fields are expanded to the corresponding 64-bit X fields in the internal
diff --git a/drivers/acpi/acpica/tbutils.c b/drivers/acpi/acpica/tbutils.c
index 7abe66505739..0c6768d20395 100644
--- a/drivers/acpi/acpica/tbutils.c
+++ b/drivers/acpi/acpica/tbutils.c
@@ -141,9 +141,9 @@ void acpi_tb_check_dsdt_header(void)
  *
  * FUNCTION:    acpi_tb_copy_dsdt
  *
- * PARAMETERS:  table_desc          - Installed table to copy
+ * PARAMETERS:  table_index         - Index of installed table to copy
  *
- * RETURN:      None
+ * RETURN:      The copied DSDT
  *
  * DESCRIPTION: Implements a subsystem option to copy the DSDT to local memory.
  *              Some very bad BIOSs are known to either corrupt the DSDT or
@@ -239,7 +239,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_address        - Pointer to the RSDP
  *
  * RETURN:      Status
  *
@@ -416,9 +416,18 @@ acpi_tb_get_table(struct acpi_table_desc *table_desc,
 		}
 	}
 
-	table_desc->validation_count++;
-	if (table_desc->validation_count == 0) {
-		table_desc->validation_count--;
+	if (table_desc->validation_count < ACPI_MAX_TABLE_VALIDATIONS) {
+		table_desc->validation_count++;
+
+		/*
+		 * Detect validation_count overflows to ensure that the warning
+		 * message will only be printed once.
+		 */
+		if (table_desc->validation_count >= ACPI_MAX_TABLE_VALIDATIONS) {
+			ACPI_WARNING((AE_INFO,
+				      "Table %p, Validation count overflows\n",
+				      table_desc));
+		}
 	}
 
 	*out_table = table_desc->pointer;
@@ -445,13 +454,20 @@ void acpi_tb_put_table(struct acpi_table_desc *table_desc)
 
 	ACPI_FUNCTION_TRACE(acpi_tb_put_table);
 
-	if (table_desc->validation_count == 0) {
-		ACPI_WARNING((AE_INFO,
-			      "Table %p, Validation count is zero before decrement\n",
-			      table_desc));
-		return_VOID;
+	if (table_desc->validation_count < ACPI_MAX_TABLE_VALIDATIONS) {
+		table_desc->validation_count--;
+
+		/*
+		 * Detect validation_count underflows to ensure that the warning
+		 * message will only be printed once.
+		 */
+		if (table_desc->validation_count >= ACPI_MAX_TABLE_VALIDATIONS) {
+			ACPI_WARNING((AE_INFO,
+				      "Table %p, Validation count underflows\n",
+				      table_desc));
+			return_VOID;
+		}
 	}
-	table_desc->validation_count--;
 
 	if (table_desc->validation_count == 0) {
 
diff --git a/drivers/acpi/acpica/utdecode.c b/drivers/acpi/acpica/utdecode.c
index 60868309e326..02cd2c2d961a 100644
--- a/drivers/acpi/acpica/utdecode.c
+++ b/drivers/acpi/acpica/utdecode.c
@@ -460,9 +460,11 @@ static const char *acpi_gbl_generic_notify[ACPI_GENERIC_NOTIFY_MAX + 1] = {
 	/* 09 */ "Device PLD Check",
 	/* 0A */ "Reserved",
 	/* 0B */ "System Locality Update",
-					/* 0C */ "Shutdown Request",
-					/* Reserved in ACPI 6.0 */
-	/* 0D */ "System Resource Affinity Update"
+								/* 0C */ "Reserved (was previously Shutdown Request)",
+								/* Reserved in ACPI 6.0 */
+	/* 0D */ "System Resource Affinity Update",
+								/* 0E */ "Heterogeneous Memory Attributes Update"
+								/* ACPI 6.2 */
 };
 
 static const char *acpi_gbl_device_notify[5] = {
diff --git a/drivers/acpi/acpica/utownerid.c b/drivers/acpi/acpica/utownerid.c
index c82399f9b456..1b3ee74a87eb 100644
--- a/drivers/acpi/acpica/utownerid.c
+++ b/drivers/acpi/acpica/utownerid.c
@@ -104,13 +104,19 @@ acpi_status acpi_ut_allocate_owner_id(acpi_owner_id *owner_id)
 				break;
 			}
 
-			if (!(acpi_gbl_owner_id_mask[j] & (1 << k))) {
+			/*
+			 * Note: the u32 cast ensures that 1 is stored as a unsigned
+			 * integer. Omitting the cast may result in 1 being stored as an
+			 * int. Some compilers or runtime error detection may flag this as
+			 * an error.
+			 */
+			if (!(acpi_gbl_owner_id_mask[j] & ((u32)1 << k))) {
 				/*
 				 * Found a free ID. The actual ID is the bit index plus one,
 				 * making zero an invalid Owner ID. Save this as the last ID
 				 * allocated and update the global ID mask.
 				 */
-				acpi_gbl_owner_id_mask[j] |= (1 << k);
+				acpi_gbl_owner_id_mask[j] |= ((u32)1 << k);
 
 				acpi_gbl_last_owner_id_index = (u8)j;
 				acpi_gbl_next_owner_id_offset = (u8)(k + 1);
@@ -201,7 +207,7 @@ void acpi_ut_release_owner_id(acpi_owner_id *owner_id_ptr)
 	/* Decode ID to index/offset pair */
 
 	index = ACPI_DIV_32(owner_id);
-	bit = 1 << ACPI_MOD_32(owner_id);
+	bit = (u32)1 << ACPI_MOD_32(owner_id);
 
 	/* Free the owner ID only if it is valid */
 
diff --git a/drivers/acpi/acpica/utresdecode.c b/drivers/acpi/acpica/utresdecode.c
new file mode 100644
index 000000000000..e15a2538558b
--- /dev/null
+++ b/drivers/acpi/acpica/utresdecode.c
@@ -0,0 +1,315 @@
+/*******************************************************************************
+ *
+ * Module Name: utresdecode - Resource descriptor keyword strings
+ *
+ ******************************************************************************/
+
+/*
+ * Copyright (C) 2000 - 2017, Intel Corp.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce at minimum a disclaimer
+ *    substantially similar to the "NO WARRANTY" disclaimer below
+ *    ("Disclaimer") and any redistribution must be conditioned upon
+ *    including a substantially similar Disclaimer requirement for further
+ *    binary redistribution.
+ * 3. Neither the names of the above-listed copyright holders nor the names
+ *    of any contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * NO WARRANTY
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGES.
+ */
+
+#include <acpi/acpi.h>
+#include "accommon.h"
+#include "acresrc.h"
+
+#define _COMPONENT          ACPI_UTILITIES
+ACPI_MODULE_NAME("utresdecode")
+
+#if defined (ACPI_DEBUG_OUTPUT) || \
+	defined (ACPI_DISASSEMBLER) || \
+	defined (ACPI_DEBUGGER)
+/*
+ * Strings used to decode resource descriptors.
+ * Used by both the disassembler and the debugger resource dump routines
+ */
+const char *acpi_gbl_bm_decode[] = {
+	"NotBusMaster",
+	"BusMaster"
+};
+
+const char *acpi_gbl_config_decode[] = {
+	"0 - Good Configuration",
+	"1 - Acceptable Configuration",
+	"2 - Suboptimal Configuration",
+	"3 - ***Invalid Configuration***",
+};
+
+const char *acpi_gbl_consume_decode[] = {
+	"ResourceProducer",
+	"ResourceConsumer"
+};
+
+const char *acpi_gbl_dec_decode[] = {
+	"PosDecode",
+	"SubDecode"
+};
+
+const char *acpi_gbl_he_decode[] = {
+	"Level",
+	"Edge"
+};
+
+const char *acpi_gbl_io_decode[] = {
+	"Decode10",
+	"Decode16"
+};
+
+const char *acpi_gbl_ll_decode[] = {
+	"ActiveHigh",
+	"ActiveLow",
+	"ActiveBoth",
+	"Reserved"
+};
+
+const char *acpi_gbl_max_decode[] = {
+	"MaxNotFixed",
+	"MaxFixed"
+};
+
+const char *acpi_gbl_mem_decode[] = {
+	"NonCacheable",
+	"Cacheable",
+	"WriteCombining",
+	"Prefetchable"
+};
+
+const char *acpi_gbl_min_decode[] = {
+	"MinNotFixed",
+	"MinFixed"
+};
+
+const char *acpi_gbl_mtp_decode[] = {
+	"AddressRangeMemory",
+	"AddressRangeReserved",
+	"AddressRangeACPI",
+	"AddressRangeNVS"
+};
+
+const char *acpi_gbl_rng_decode[] = {
+	"InvalidRanges",
+	"NonISAOnlyRanges",
+	"ISAOnlyRanges",
+	"EntireRange"
+};
+
+const char *acpi_gbl_rw_decode[] = {
+	"ReadOnly",
+	"ReadWrite"
+};
+
+const char *acpi_gbl_shr_decode[] = {
+	"Exclusive",
+	"Shared",
+	"ExclusiveAndWake",	/* ACPI 5.0 */
+	"SharedAndWake"		/* ACPI 5.0 */
+};
+
+const char *acpi_gbl_siz_decode[] = {
+	"Transfer8",
+	"Transfer8_16",
+	"Transfer16",
+	"InvalidSize"
+};
+
+const char *acpi_gbl_trs_decode[] = {
+	"DenseTranslation",
+	"SparseTranslation"
+};
+
+const char *acpi_gbl_ttp_decode[] = {
+	"TypeStatic",
+	"TypeTranslation"
+};
+
+const char *acpi_gbl_typ_decode[] = {
+	"Compatibility",
+	"TypeA",
+	"TypeB",
+	"TypeF"
+};
+
+const char *acpi_gbl_ppc_decode[] = {
+	"PullDefault",
+	"PullUp",
+	"PullDown",
+	"PullNone"
+};
+
+const char *acpi_gbl_ior_decode[] = {
+	"IoRestrictionNone",
+	"IoRestrictionInputOnly",
+	"IoRestrictionOutputOnly",
+	"IoRestrictionNoneAndPreserve"
+};
+
+const char *acpi_gbl_dts_decode[] = {
+	"Width8bit",
+	"Width16bit",
+	"Width32bit",
+	"Width64bit",
+	"Width128bit",
+	"Width256bit",
+};
+
+/* GPIO connection type */
+
+const char *acpi_gbl_ct_decode[] = {
+	"Interrupt",
+	"I/O"
+};
+
+/* Serial bus type */
+
+const char *acpi_gbl_sbt_decode[] = {
+	"/* UNKNOWN serial bus type */",
+	"I2C",
+	"SPI",
+	"UART"
+};
+
+/* I2C serial bus access mode */
+
+const char *acpi_gbl_am_decode[] = {
+	"AddressingMode7Bit",
+	"AddressingMode10Bit"
+};
+
+/* I2C serial bus slave mode */
+
+const char *acpi_gbl_sm_decode[] = {
+	"ControllerInitiated",
+	"DeviceInitiated"
+};
+
+/* SPI serial bus wire mode */
+
+const char *acpi_gbl_wm_decode[] = {
+	"FourWireMode",
+	"ThreeWireMode"
+};
+
+/* SPI serial clock phase */
+
+const char *acpi_gbl_cph_decode[] = {
+	"ClockPhaseFirst",
+	"ClockPhaseSecond"
+};
+
+/* SPI serial bus clock polarity */
+
+const char *acpi_gbl_cpo_decode[] = {
+	"ClockPolarityLow",
+	"ClockPolarityHigh"
+};
+
+/* SPI serial bus device polarity */
+
+const char *acpi_gbl_dp_decode[] = {
+	"PolarityLow",
+	"PolarityHigh"
+};
+
+/* UART serial bus endian */
+
+const char *acpi_gbl_ed_decode[] = {
+	"LittleEndian",
+	"BigEndian"
+};
+
+/* UART serial bus bits per byte */
+
+const char *acpi_gbl_bpb_decode[] = {
+	"DataBitsFive",
+	"DataBitsSix",
+	"DataBitsSeven",
+	"DataBitsEight",
+	"DataBitsNine",
+	"/* UNKNOWN Bits per byte */",
+	"/* UNKNOWN Bits per byte */",
+	"/* UNKNOWN Bits per byte */"
+};
+
+/* UART serial bus stop bits */
+
+const char *acpi_gbl_sb_decode[] = {
+	"StopBitsZero",
+	"StopBitsOne",
+	"StopBitsOnePlusHalf",
+	"StopBitsTwo"
+};
+
+/* UART serial bus flow control */
+
+const char *acpi_gbl_fc_decode[] = {
+	"FlowControlNone",
+	"FlowControlHardware",
+	"FlowControlXON",
+	"/* UNKNOWN flow control keyword */"
+};
+
+/* UART serial bus parity type */
+
+const char *acpi_gbl_pt_decode[] = {
+	"ParityTypeNone",
+	"ParityTypeEven",
+	"ParityTypeOdd",
+	"ParityTypeMark",
+	"ParityTypeSpace",
+	"/* UNKNOWN parity keyword */",
+	"/* UNKNOWN parity keyword */",
+	"/* UNKNOWN parity keyword */"
+};
+
+/* pin_config type */
+
+const char *acpi_gbl_ptyp_decode[] = {
+	"Default",
+	"Bias Pull-up",
+	"Bias Pull-down",
+	"Bias Default",
+	"Bias Disable",
+	"Bias High Impedance",
+	"Bias Bus Hold",
+	"Drive Open Drain",
+	"Drive Open Source",
+	"Drive Push Pull",
+	"Drive Strength",
+	"Slew Rate",
+	"Input Debounce",
+	"Input Schmitt Trigger",
+};
+
+#endif
diff --git a/drivers/acpi/acpica/utresrc.c b/drivers/acpi/acpica/utresrc.c
index e0587c85bafd..70f78a4bf13b 100644
--- a/drivers/acpi/acpica/utresrc.c
+++ b/drivers/acpi/acpica/utresrc.c
@@ -48,251 +48,6 @@
 #define _COMPONENT          ACPI_UTILITIES
 ACPI_MODULE_NAME("utresrc")
 
-#if defined(ACPI_DEBUG_OUTPUT) || defined (ACPI_DISASSEMBLER) || defined (ACPI_DEBUGGER)
-/*
- * Strings used to decode resource descriptors.
- * Used by both the disassembler and the debugger resource dump routines
- */
-const char *acpi_gbl_bm_decode[] = {
-	"NotBusMaster",
-	"BusMaster"
-};
-
-const char *acpi_gbl_config_decode[] = {
-	"0 - Good Configuration",
-	"1 - Acceptable Configuration",
-	"2 - Suboptimal Configuration",
-	"3 - ***Invalid Configuration***",
-};
-
-const char *acpi_gbl_consume_decode[] = {
-	"ResourceProducer",
-	"ResourceConsumer"
-};
-
-const char *acpi_gbl_dec_decode[] = {
-	"PosDecode",
-	"SubDecode"
-};
-
-const char *acpi_gbl_he_decode[] = {
-	"Level",
-	"Edge"
-};
-
-const char *acpi_gbl_io_decode[] = {
-	"Decode10",
-	"Decode16"
-};
-
-const char *acpi_gbl_ll_decode[] = {
-	"ActiveHigh",
-	"ActiveLow",
-	"ActiveBoth",
-	"Reserved"
-};
-
-const char *acpi_gbl_max_decode[] = {
-	"MaxNotFixed",
-	"MaxFixed"
-};
-
-const char *acpi_gbl_mem_decode[] = {
-	"NonCacheable",
-	"Cacheable",
-	"WriteCombining",
-	"Prefetchable"
-};
-
-const char *acpi_gbl_min_decode[] = {
-	"MinNotFixed",
-	"MinFixed"
-};
-
-const char *acpi_gbl_mtp_decode[] = {
-	"AddressRangeMemory",
-	"AddressRangeReserved",
-	"AddressRangeACPI",
-	"AddressRangeNVS"
-};
-
-const char *acpi_gbl_rng_decode[] = {
-	"InvalidRanges",
-	"NonISAOnlyRanges",
-	"ISAOnlyRanges",
-	"EntireRange"
-};
-
-const char *acpi_gbl_rw_decode[] = {
-	"ReadOnly",
-	"ReadWrite"
-};
-
-const char *acpi_gbl_shr_decode[] = {
-	"Exclusive",
-	"Shared",
-	"ExclusiveAndWake",	/* ACPI 5.0 */
-	"SharedAndWake"		/* ACPI 5.0 */
-};
-
-const char *acpi_gbl_siz_decode[] = {
-	"Transfer8",
-	"Transfer8_16",
-	"Transfer16",
-	"InvalidSize"
-};
-
-const char *acpi_gbl_trs_decode[] = {
-	"DenseTranslation",
-	"SparseTranslation"
-};
-
-const char *acpi_gbl_ttp_decode[] = {
-	"TypeStatic",
-	"TypeTranslation"
-};
-
-const char *acpi_gbl_typ_decode[] = {
-	"Compatibility",
-	"TypeA",
-	"TypeB",
-	"TypeF"
-};
-
-const char *acpi_gbl_ppc_decode[] = {
-	"PullDefault",
-	"PullUp",
-	"PullDown",
-	"PullNone"
-};
-
-const char *acpi_gbl_ior_decode[] = {
-	"IoRestrictionNone",
-	"IoRestrictionInputOnly",
-	"IoRestrictionOutputOnly",
-	"IoRestrictionNoneAndPreserve"
-};
-
-const char *acpi_gbl_dts_decode[] = {
-	"Width8bit",
-	"Width16bit",
-	"Width32bit",
-	"Width64bit",
-	"Width128bit",
-	"Width256bit",
-};
-
-/* GPIO connection type */
-
-const char *acpi_gbl_ct_decode[] = {
-	"Interrupt",
-	"I/O"
-};
-
-/* Serial bus type */
-
-const char *acpi_gbl_sbt_decode[] = {
-	"/* UNKNOWN serial bus type */",
-	"I2C",
-	"SPI",
-	"UART"
-};
-
-/* I2C serial bus access mode */
-
-const char *acpi_gbl_am_decode[] = {
-	"AddressingMode7Bit",
-	"AddressingMode10Bit"
-};
-
-/* I2C serial bus slave mode */
-
-const char *acpi_gbl_sm_decode[] = {
-	"ControllerInitiated",
-	"DeviceInitiated"
-};
-
-/* SPI serial bus wire mode */
-
-const char *acpi_gbl_wm_decode[] = {
-	"FourWireMode",
-	"ThreeWireMode"
-};
-
-/* SPI serial clock phase */
-
-const char *acpi_gbl_cph_decode[] = {
-	"ClockPhaseFirst",
-	"ClockPhaseSecond"
-};
-
-/* SPI serial bus clock polarity */
-
-const char *acpi_gbl_cpo_decode[] = {
-	"ClockPolarityLow",
-	"ClockPolarityHigh"
-};
-
-/* SPI serial bus device polarity */
-
-const char *acpi_gbl_dp_decode[] = {
-	"PolarityLow",
-	"PolarityHigh"
-};
-
-/* UART serial bus endian */
-
-const char *acpi_gbl_ed_decode[] = {
-	"LittleEndian",
-	"BigEndian"
-};
-
-/* UART serial bus bits per byte */
-
-const char *acpi_gbl_bpb_decode[] = {
-	"DataBitsFive",
-	"DataBitsSix",
-	"DataBitsSeven",
-	"DataBitsEight",
-	"DataBitsNine",
-	"/* UNKNOWN Bits per byte */",
-	"/* UNKNOWN Bits per byte */",
-	"/* UNKNOWN Bits per byte */"
-};
-
-/* UART serial bus stop bits */
-
-const char *acpi_gbl_sb_decode[] = {
-	"StopBitsZero",
-	"StopBitsOne",
-	"StopBitsOnePlusHalf",
-	"StopBitsTwo"
-};
-
-/* UART serial bus flow control */
-
-const char *acpi_gbl_fc_decode[] = {
-	"FlowControlNone",
-	"FlowControlHardware",
-	"FlowControlXON",
-	"/* UNKNOWN flow control keyword */"
-};
-
-/* UART serial bus parity type */
-
-const char *acpi_gbl_pt_decode[] = {
-	"ParityTypeNone",
-	"ParityTypeEven",
-	"ParityTypeOdd",
-	"ParityTypeMark",
-	"ParityTypeSpace",
-	"/* UNKNOWN parity keyword */",
-	"/* UNKNOWN parity keyword */",
-	"/* UNKNOWN parity keyword */"
-};
-
-#endif
-
 /*
  * Base sizes of the raw AML resource descriptors, indexed by resource type.
  * Zero indicates a reserved (and therefore invalid) resource type.
@@ -332,8 +87,12 @@ const u8 acpi_gbl_resource_aml_sizes[] = {
 	ACPI_AML_SIZE_LARGE(struct aml_resource_address64),
 	ACPI_AML_SIZE_LARGE(struct aml_resource_extended_address64),
 	ACPI_AML_SIZE_LARGE(struct aml_resource_gpio),
-	0,
+	ACPI_AML_SIZE_LARGE(struct aml_resource_pin_function),
 	ACPI_AML_SIZE_LARGE(struct aml_resource_common_serialbus),
+	ACPI_AML_SIZE_LARGE(struct aml_resource_pin_config),
+	ACPI_AML_SIZE_LARGE(struct aml_resource_pin_group),
+	ACPI_AML_SIZE_LARGE(struct aml_resource_pin_group_function),
+	ACPI_AML_SIZE_LARGE(struct aml_resource_pin_group_config),
 };
 
 const u8 acpi_gbl_resource_aml_serial_bus_sizes[] = {
@@ -384,8 +143,12 @@ static const u8 acpi_gbl_resource_types[] = {
 	ACPI_VARIABLE_LENGTH,	/* 0A Qword* address */
 	ACPI_FIXED_LENGTH,	/* 0B Extended* address */
 	ACPI_VARIABLE_LENGTH,	/* 0C Gpio* */
-	0,
-	ACPI_VARIABLE_LENGTH	/* 0E *serial_bus */
+	ACPI_VARIABLE_LENGTH,	/* 0D pin_function */
+	ACPI_VARIABLE_LENGTH,	/* 0E *serial_bus */
+	ACPI_VARIABLE_LENGTH,	/* 0F pin_config */
+	ACPI_VARIABLE_LENGTH,	/* 10 pin_group */
+	ACPI_VARIABLE_LENGTH,	/* 11 pin_group_function */
+	ACPI_VARIABLE_LENGTH,	/* 12 pin_group_config */
 };
 
 /*******************************************************************************
@@ -474,15 +237,6 @@ acpi_ut_walk_aml_resources(struct acpi_walk_state *walk_state,
 				return_ACPI_STATUS(AE_AML_NO_RESOURCE_END_TAG);
 			}
 
-			/*
-			 * The end_tag opcode must be followed by a zero byte.
-			 * Although this byte is technically defined to be a checksum,
-			 * in practice, all ASL compilers set this byte to zero.
-			 */
-			if (*(aml + 1) != 0) {
-				return_ACPI_STATUS(AE_AML_NO_RESOURCE_END_TAG);
-			}
-
 			/* Return the pointer to the end_tag if requested */
 
 			if (!user_function) {
diff --git a/drivers/acpi/acpica/utxfmutex.c b/drivers/acpi/acpica/utxfmutex.c
index c016211c35ae..0b85f113f726 100644
--- a/drivers/acpi/acpica/utxfmutex.c
+++ b/drivers/acpi/acpica/utxfmutex.c
@@ -151,6 +151,8 @@ acpi_acquire_mutex(acpi_handle handle, acpi_string pathname, u16 timeout)
 	return (status);
 }
 
+ACPI_EXPORT_SYMBOL(acpi_acquire_mutex)
+
 /*******************************************************************************
  *
  * FUNCTION:    acpi_release_mutex
@@ -167,7 +169,6 @@ acpi_acquire_mutex(acpi_handle handle, acpi_string pathname, u16 timeout)
  *              not both.
  *
  ******************************************************************************/
-
 acpi_status acpi_release_mutex(acpi_handle handle, acpi_string pathname)
 {
 	acpi_status status;
@@ -185,3 +186,5 @@ acpi_status acpi_release_mutex(acpi_handle handle, acpi_string pathname)
 	acpi_os_release_mutex(mutex_obj->mutex.os_mutex);
 	return (AE_OK);
 }
+
+ACPI_EXPORT_SYMBOL(acpi_release_mutex)
diff --git a/drivers/acpi/apei/Kconfig b/drivers/acpi/apei/Kconfig
index b0140c8fc733..de14d49a5c90 100644
--- a/drivers/acpi/apei/Kconfig
+++ b/drivers/acpi/apei/Kconfig
@@ -39,6 +39,21 @@ config ACPI_APEI_PCIEAER
 	  PCIe AER errors may be reported via APEI firmware first mode.
 	  Turn on this option to enable the corresponding support.
 
+config ACPI_APEI_SEA
+	bool "APEI Synchronous External Abort logging/recovering support"
+	depends on ARM64 && ACPI_APEI_GHES
+	default y
+	help
+	  This option should be enabled if the system supports
+	  firmware first handling of SEA (Synchronous External Abort).
+	  SEA happens with certain faults of data abort or instruction
+	  abort synchronous exceptions on ARMv8 systems. If a system
+	  supports firmware first handling of SEA, the platform analyzes
+	  and handles hardware error notifications from SEA, and it may then
+	  form a HW error record for the OS to parse and handle. This
+	  option allows the OS to look for such hardware error record, and
+	  take appropriate action.
+
 config ACPI_APEI_MEMORY_FAILURE
 	bool "APEI memory error recovering support"
 	depends on ACPI_APEI && MEMORY_FAILURE
diff --git a/drivers/acpi/apei/ghes.c b/drivers/acpi/apei/ghes.c
index d0855c09f32f..d661d452b238 100644
--- a/drivers/acpi/apei/ghes.c
+++ b/drivers/acpi/apei/ghes.c
@@ -45,10 +45,14 @@
 #include <linux/aer.h>
 #include <linux/nmi.h>
 #include <linux/sched/clock.h>
+#include <linux/uuid.h>
+#include <linux/ras.h>
 
+#include <acpi/actbl1.h>
 #include <acpi/ghes.h>
 #include <acpi/apei.h>
 #include <asm/tlbflush.h>
+#include <ras/ras_event.h>
 
 #include "apei-internal.h"
 
@@ -80,6 +84,11 @@
 	((struct acpi_hest_generic_status *)				\
 	 ((struct ghes_estatus_node *)(estatus_node) + 1))
 
+static inline bool is_hest_type_generic_v2(struct ghes *ghes)
+{
+	return ghes->generic->header.type == ACPI_HEST_TYPE_GENERIC_ERROR_V2;
+}
+
 /*
  * This driver isn't really modular, however for the time being,
  * continuing to use module_param is the easiest way to remain
@@ -89,14 +98,14 @@ bool ghes_disable;
 module_param_named(disable, ghes_disable, bool, 0);
 
 /*
- * All error sources notified with SCI shares one notifier function,
- * so they need to be linked and checked one by one.  This is applied
- * to NMI too.
+ * All error sources notified with HED (Hardware Error Device) share a
+ * single notifier callback, so they need to be linked and checked one
+ * by one. This holds true for NMI too.
  *
  * RCU is used for these lists, so ghes_list_mutex is only used for
  * list changing, not for traversing.
  */
-static LIST_HEAD(ghes_sci);
+static LIST_HEAD(ghes_hed);
 static DEFINE_MUTEX(ghes_list_mutex);
 
 /*
@@ -110,11 +119,7 @@ static DEFINE_MUTEX(ghes_list_mutex);
  * Two virtual pages are used, one for IRQ/PROCESS context, the other for
  * NMI context (optionally).
  */
-#ifdef CONFIG_HAVE_ACPI_APEI_NMI
 #define GHES_IOREMAP_PAGES           2
-#else
-#define GHES_IOREMAP_PAGES           1
-#endif
 #define GHES_IOREMAP_IRQ_PAGE(base)	(base)
 #define GHES_IOREMAP_NMI_PAGE(base)	((base) + PAGE_SIZE)
 
@@ -133,6 +138,8 @@ static unsigned long ghes_estatus_pool_size_request;
 static struct ghes_estatus_cache *ghes_estatus_caches[GHES_ESTATUS_CACHES_SIZE];
 static atomic_t ghes_estatus_cache_alloced;
 
+static int ghes_panic_timeout __read_mostly = 30;
+
 static int ghes_ioremap_init(void)
 {
 	ghes_ioremap_area = __get_vm_area(PAGE_SIZE * GHES_IOREMAP_PAGES,
@@ -153,10 +160,14 @@ static void ghes_ioremap_exit(void)
 static void __iomem *ghes_ioremap_pfn_nmi(u64 pfn)
 {
 	unsigned long vaddr;
+	phys_addr_t paddr;
+	pgprot_t prot;
 
 	vaddr = (unsigned long)GHES_IOREMAP_NMI_PAGE(ghes_ioremap_area->addr);
-	ioremap_page_range(vaddr, vaddr + PAGE_SIZE,
-			   pfn << PAGE_SHIFT, PAGE_KERNEL);
+
+	paddr = pfn << PAGE_SHIFT;
+	prot = arch_apei_get_mem_attribute(paddr);
+	ioremap_page_range(vaddr, vaddr + PAGE_SIZE, paddr, prot);
 
 	return (void __iomem *)vaddr;
 }
@@ -240,6 +251,16 @@ static int ghes_estatus_pool_expand(unsigned long len)
 	return 0;
 }
 
+static int map_gen_v2(struct ghes *ghes)
+{
+	return apei_map_generic_address(&ghes->generic_v2->read_ack_register);
+}
+
+static void unmap_gen_v2(struct ghes *ghes)
+{
+	apei_unmap_generic_address(&ghes->generic_v2->read_ack_register);
+}
+
 static struct ghes *ghes_new(struct acpi_hest_generic *generic)
 {
 	struct ghes *ghes;
@@ -249,10 +270,17 @@ static struct ghes *ghes_new(struct acpi_hest_generic *generic)
 	ghes = kzalloc(sizeof(*ghes), GFP_KERNEL);
 	if (!ghes)
 		return ERR_PTR(-ENOMEM);
+
 	ghes->generic = generic;
+	if (is_hest_type_generic_v2(ghes)) {
+		rc = map_gen_v2(ghes);
+		if (rc)
+			goto err_free;
+	}
+
 	rc = apei_map_generic_address(&generic->error_status_address);
 	if (rc)
-		goto err_free;
+		goto err_unmap_read_ack_addr;
 	error_block_length = generic->error_block_length;
 	if (error_block_length > GHES_ESTATUS_MAX_SIZE) {
 		pr_warning(FW_WARN GHES_PFX
@@ -264,13 +292,16 @@ static struct ghes *ghes_new(struct acpi_hest_generic *generic)
 	ghes->estatus = kmalloc(error_block_length, GFP_KERNEL);
 	if (!ghes->estatus) {
 		rc = -ENOMEM;
-		goto err_unmap;
+		goto err_unmap_status_addr;
 	}
 
 	return ghes;
 
-err_unmap:
+err_unmap_status_addr:
 	apei_unmap_generic_address(&generic->error_status_address);
+err_unmap_read_ack_addr:
+	if (is_hest_type_generic_v2(ghes))
+		unmap_gen_v2(ghes);
 err_free:
 	kfree(ghes);
 	return ERR_PTR(rc);
@@ -280,6 +311,8 @@ static void ghes_fini(struct ghes *ghes)
 {
 	kfree(ghes->estatus);
 	apei_unmap_generic_address(&ghes->generic->error_status_address);
+	if (is_hest_type_generic_v2(ghes))
+		unmap_gen_v2(ghes);
 }
 
 static inline int ghes_severity(int severity)
@@ -400,8 +433,7 @@ static void ghes_handle_memory_failure(struct acpi_hest_generic_data *gdata, int
 	unsigned long pfn;
 	int flags = -1;
 	int sec_sev = ghes_severity(gdata->error_severity);
-	struct cper_sec_mem_err *mem_err;
-	mem_err = (struct cper_sec_mem_err *)(gdata + 1);
+	struct cper_sec_mem_err *mem_err = acpi_hest_get_payload(gdata);
 
 	if (!(mem_err->validation_bits & CPER_MEM_VALID_PA))
 		return;
@@ -431,24 +463,32 @@ static void ghes_do_proc(struct ghes *ghes,
 {
 	int sev, sec_sev;
 	struct acpi_hest_generic_data *gdata;
+	guid_t *sec_type;
+	guid_t *fru_id = &NULL_UUID_LE;
+	char *fru_text = "";
 
 	sev = ghes_severity(estatus->error_severity);
 	apei_estatus_for_each_section(estatus, gdata) {
+		sec_type = (guid_t *)gdata->section_type;
 		sec_sev = ghes_severity(gdata->error_severity);
-		if (!uuid_le_cmp(*(uuid_le *)gdata->section_type,
-				 CPER_SEC_PLATFORM_MEM)) {
-			struct cper_sec_mem_err *mem_err;
-			mem_err = (struct cper_sec_mem_err *)(gdata+1);
+		if (gdata->validation_bits & CPER_SEC_VALID_FRU_ID)
+			fru_id = (guid_t *)gdata->fru_id;
+
+		if (gdata->validation_bits & CPER_SEC_VALID_FRU_TEXT)
+			fru_text = gdata->fru_text;
+
+		if (guid_equal(sec_type, &CPER_SEC_PLATFORM_MEM)) {
+			struct cper_sec_mem_err *mem_err = acpi_hest_get_payload(gdata);
+
 			ghes_edac_report_mem_error(ghes, sev, mem_err);
 
 			arch_apei_report_mem_error(sev, mem_err);
 			ghes_handle_memory_failure(gdata, sev);
 		}
 #ifdef CONFIG_ACPI_APEI_PCIEAER
-		else if (!uuid_le_cmp(*(uuid_le *)gdata->section_type,
-				      CPER_SEC_PCIE)) {
-			struct cper_sec_pcie *pcie_err;
-			pcie_err = (struct cper_sec_pcie *)(gdata+1);
+		else if (guid_equal(sec_type, &CPER_SEC_PCIE)) {
+			struct cper_sec_pcie *pcie_err = acpi_hest_get_payload(gdata);
+
 			if (sev == GHES_SEV_RECOVERABLE &&
 			    sec_sev == GHES_SEV_RECOVERABLE &&
 			    pcie_err->validation_bits & CPER_PCIE_VALID_DEVICE_ID &&
@@ -477,6 +517,17 @@ static void ghes_do_proc(struct ghes *ghes,
 
 		}
 #endif
+		else if (guid_equal(sec_type, &CPER_SEC_PROC_ARM)) {
+			struct cper_sec_proc_arm *err = acpi_hest_get_payload(gdata);
+
+			log_arm_hw_error(err);
+		} else {
+			void *err = acpi_hest_get_payload(gdata);
+
+			log_non_standard_event(sec_type, fru_id, fru_text,
+					       sec_sev, err,
+					       gdata->error_data_length);
+		}
 	}
 }
 
@@ -649,6 +700,31 @@ static void ghes_estatus_cache_add(
 	rcu_read_unlock();
 }
 
+static int ghes_ack_error(struct acpi_hest_generic_v2 *gv2)
+{
+	int rc;
+	u64 val = 0;
+
+	rc = apei_read(&val, &gv2->read_ack_register);
+	if (rc)
+		return rc;
+
+	val &= gv2->read_ack_preserve << gv2->read_ack_register.bit_offset;
+	val |= gv2->read_ack_write    << gv2->read_ack_register.bit_offset;
+
+	return apei_write(val, &gv2->read_ack_register);
+}
+
+static void __ghes_panic(struct ghes *ghes)
+{
+	__ghes_print_estatus(KERN_EMERG, ghes->generic, ghes->estatus);
+
+	/* reboot to log the error! */
+	if (!panic_timeout)
+		panic_timeout = ghes_panic_timeout;
+	panic("Fatal hardware error!");
+}
+
 static int ghes_proc(struct ghes *ghes)
 {
 	int rc;
@@ -656,11 +732,26 @@ static int ghes_proc(struct ghes *ghes)
 	rc = ghes_read_estatus(ghes, 0);
 	if (rc)
 		goto out;
+
+	if (ghes_severity(ghes->estatus->error_severity) >= GHES_SEV_PANIC) {
+		__ghes_panic(ghes);
+	}
+
 	if (!ghes_estatus_cached(ghes->estatus)) {
 		if (ghes_print_estatus(NULL, ghes->generic, ghes->estatus))
 			ghes_estatus_cache_add(ghes->generic, ghes->estatus);
 	}
 	ghes_do_proc(ghes, ghes->estatus);
+
+	/*
+	 * GHESv2 type HEST entries introduce support for error acknowledgment,
+	 * so only acknowledge the error if this support is present.
+	 */
+	if (is_hest_type_generic_v2(ghes)) {
+		rc = ghes_ack_error(ghes->generic_v2);
+		if (rc)
+			return rc;
+	}
 out:
 	ghes_clear_estatus(ghes);
 	return rc;
@@ -702,14 +793,14 @@ static irqreturn_t ghes_irq_func(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static int ghes_notify_sci(struct notifier_block *this,
-				  unsigned long event, void *data)
+static int ghes_notify_hed(struct notifier_block *this, unsigned long event,
+			   void *data)
 {
 	struct ghes *ghes;
 	int ret = NOTIFY_DONE;
 
 	rcu_read_lock();
-	list_for_each_entry_rcu(ghes, &ghes_sci, list) {
+	list_for_each_entry_rcu(ghes, &ghes_hed, list) {
 		if (!ghes_proc(ghes))
 			ret = NOTIFY_OK;
 	}
@@ -718,10 +809,59 @@ static int ghes_notify_sci(struct notifier_block *this,
 	return ret;
 }
 
-static struct notifier_block ghes_notifier_sci = {
-	.notifier_call = ghes_notify_sci,
+static struct notifier_block ghes_notifier_hed = {
+	.notifier_call = ghes_notify_hed,
 };
 
+#ifdef CONFIG_ACPI_APEI_SEA
+static LIST_HEAD(ghes_sea);
+
+/*
+ * Return 0 only if one of the SEA error sources successfully reported an error
+ * record sent from the firmware.
+ */
+int ghes_notify_sea(void)
+{
+	struct ghes *ghes;
+	int ret = -ENOENT;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(ghes, &ghes_sea, list) {
+		if (!ghes_proc(ghes))
+			ret = 0;
+	}
+	rcu_read_unlock();
+	return ret;
+}
+
+static void ghes_sea_add(struct ghes *ghes)
+{
+	mutex_lock(&ghes_list_mutex);
+	list_add_rcu(&ghes->list, &ghes_sea);
+	mutex_unlock(&ghes_list_mutex);
+}
+
+static void ghes_sea_remove(struct ghes *ghes)
+{
+	mutex_lock(&ghes_list_mutex);
+	list_del_rcu(&ghes->list);
+	mutex_unlock(&ghes_list_mutex);
+	synchronize_rcu();
+}
+#else /* CONFIG_ACPI_APEI_SEA */
+static inline void ghes_sea_add(struct ghes *ghes)
+{
+	pr_err(GHES_PFX "ID: %d, trying to add SEA notification which is not supported\n",
+	       ghes->generic->header.source_id);
+}
+
+static inline void ghes_sea_remove(struct ghes *ghes)
+{
+	pr_err(GHES_PFX "ID: %d, trying to remove SEA notification which is not supported\n",
+	       ghes->generic->header.source_id);
+}
+#endif /* CONFIG_ACPI_APEI_SEA */
+
 #ifdef CONFIG_HAVE_ACPI_APEI_NMI
 /*
  * printk is not safe in NMI context.  So in NMI handler, we allocate
@@ -742,8 +882,6 @@ static atomic_t ghes_in_nmi = ATOMIC_INIT(0);
 
 static LIST_HEAD(ghes_nmi);
 
-static int ghes_panic_timeout	__read_mostly = 30;
-
 static void ghes_proc_in_irq(struct irq_work *irq_work)
 {
 	struct llist_node *llnode, *next;
@@ -829,18 +967,6 @@ static void __process_error(struct ghes *ghes)
 #endif
 }
 
-static void __ghes_panic(struct ghes *ghes)
-{
-	oops_begin();
-	ghes_print_queued_estatus();
-	__ghes_print_estatus(KERN_EMERG, ghes->generic, ghes->estatus);
-
-	/* reboot to log the error! */
-	if (panic_timeout == 0)
-		panic_timeout = ghes_panic_timeout;
-	panic("Fatal hardware error!");
-}
-
 static int ghes_notify_nmi(unsigned int cmd, struct pt_regs *regs)
 {
 	struct ghes *ghes;
@@ -858,8 +984,11 @@ static int ghes_notify_nmi(unsigned int cmd, struct pt_regs *regs)
 		}
 
 		sev = ghes_severity(ghes->estatus->error_severity);
-		if (sev >= GHES_SEV_PANIC)
+		if (sev >= GHES_SEV_PANIC) {
+			oops_begin();
+			ghes_print_queued_estatus();
 			__ghes_panic(ghes);
+		}
 
 		if (!(ghes->flags & GHES_TO_CLEAR))
 			continue;
@@ -966,6 +1095,17 @@ static int ghes_probe(struct platform_device *ghes_dev)
 	case ACPI_HEST_NOTIFY_POLLED:
 	case ACPI_HEST_NOTIFY_EXTERNAL:
 	case ACPI_HEST_NOTIFY_SCI:
+	case ACPI_HEST_NOTIFY_GSIV:
+	case ACPI_HEST_NOTIFY_GPIO:
+		break;
+
+	case ACPI_HEST_NOTIFY_SEA:
+		if (!IS_ENABLED(CONFIG_ACPI_APEI_SEA)) {
+			pr_warn(GHES_PFX "Generic hardware error source: %d notified via SEA is not supported\n",
+				generic->header.source_id);
+			rc = -ENOTSUPP;
+			goto err;
+		}
 		break;
 	case ACPI_HEST_NOTIFY_NMI:
 		if (!IS_ENABLED(CONFIG_HAVE_ACPI_APEI_NMI)) {
@@ -1024,13 +1164,20 @@ static int ghes_probe(struct platform_device *ghes_dev)
 			goto err_edac_unreg;
 		}
 		break;
+
 	case ACPI_HEST_NOTIFY_SCI:
+	case ACPI_HEST_NOTIFY_GSIV:
+	case ACPI_HEST_NOTIFY_GPIO:
 		mutex_lock(&ghes_list_mutex);
-		if (list_empty(&ghes_sci))
-			register_acpi_hed_notifier(&ghes_notifier_sci);
-		list_add_rcu(&ghes->list, &ghes_sci);
+		if (list_empty(&ghes_hed))
+			register_acpi_hed_notifier(&ghes_notifier_hed);
+		list_add_rcu(&ghes->list, &ghes_hed);
 		mutex_unlock(&ghes_list_mutex);
 		break;
+
+	case ACPI_HEST_NOTIFY_SEA:
+		ghes_sea_add(ghes);
+		break;
 	case ACPI_HEST_NOTIFY_NMI:
 		ghes_nmi_add(ghes);
 		break;
@@ -1039,6 +1186,9 @@ static int ghes_probe(struct platform_device *ghes_dev)
 	}
 	platform_set_drvdata(ghes_dev, ghes);
 
+	/* Handle any pending errors right away */
+	ghes_proc(ghes);
+
 	return 0;
 err_edac_unreg:
 	ghes_edac_unregister(ghes);
@@ -1066,14 +1216,21 @@ static int ghes_remove(struct platform_device *ghes_dev)
 	case ACPI_HEST_NOTIFY_EXTERNAL:
 		free_irq(ghes->irq, ghes);
 		break;
+
 	case ACPI_HEST_NOTIFY_SCI:
+	case ACPI_HEST_NOTIFY_GSIV:
+	case ACPI_HEST_NOTIFY_GPIO:
 		mutex_lock(&ghes_list_mutex);
 		list_del_rcu(&ghes->list);
-		if (list_empty(&ghes_sci))
-			unregister_acpi_hed_notifier(&ghes_notifier_sci);
+		if (list_empty(&ghes_hed))
+			unregister_acpi_hed_notifier(&ghes_notifier_hed);
 		mutex_unlock(&ghes_list_mutex);
 		synchronize_rcu();
 		break;
+
+	case ACPI_HEST_NOTIFY_SEA:
+		ghes_sea_remove(ghes);
+		break;
 	case ACPI_HEST_NOTIFY_NMI:
 		ghes_nmi_remove(ghes);
 		break;
diff --git a/drivers/acpi/apei/hest.c b/drivers/acpi/apei/hest.c
index 8f2a98e23bba..456b488eb1df 100644
--- a/drivers/acpi/apei/hest.c
+++ b/drivers/acpi/apei/hest.c
@@ -52,6 +52,7 @@ static const int hest_esrc_len_tab[ACPI_HEST_TYPE_RESERVED] = {
 	[ACPI_HEST_TYPE_AER_ENDPOINT] = sizeof(struct acpi_hest_aer),
 	[ACPI_HEST_TYPE_AER_BRIDGE] = sizeof(struct acpi_hest_aer_bridge),
 	[ACPI_HEST_TYPE_GENERIC_ERROR] = sizeof(struct acpi_hest_generic),
+	[ACPI_HEST_TYPE_GENERIC_ERROR_V2] = sizeof(struct acpi_hest_generic_v2),
 };
 
 static int hest_esrc_len(struct acpi_hest_header *hest_hdr)
@@ -141,7 +142,8 @@ static int __init hest_parse_ghes_count(struct acpi_hest_header *hest_hdr, void
 {
 	int *count = data;
 
-	if (hest_hdr->type == ACPI_HEST_TYPE_GENERIC_ERROR)
+	if (hest_hdr->type == ACPI_HEST_TYPE_GENERIC_ERROR ||
+	    hest_hdr->type == ACPI_HEST_TYPE_GENERIC_ERROR_V2)
 		(*count)++;
 	return 0;
 }
@@ -152,7 +154,8 @@ static int __init hest_parse_ghes(struct acpi_hest_header *hest_hdr, void *data)
 	struct ghes_arr *ghes_arr = data;
 	int rc, i;
 
-	if (hest_hdr->type != ACPI_HEST_TYPE_GENERIC_ERROR)
+	if (hest_hdr->type != ACPI_HEST_TYPE_GENERIC_ERROR &&
+	    hest_hdr->type != ACPI_HEST_TYPE_GENERIC_ERROR_V2)
 		return 0;
 
 	if (!((struct acpi_hest_generic *)hest_hdr)->enabled)
diff --git a/drivers/acpi/arm64/iort.c b/drivers/acpi/arm64/iort.c
index 797b28dc7b34..d048f72c23f8 100644
--- a/drivers/acpi/arm64/iort.c
+++ b/drivers/acpi/arm64/iort.c
@@ -234,21 +234,6 @@ static struct acpi_iort_node *iort_scan_node(enum acpi_iort_node_type type,
 	return NULL;
 }
 
-static acpi_status
-iort_match_type_callback(struct acpi_iort_node *node, void *context)
-{
-	return AE_OK;
-}
-
-bool iort_node_match(u8 type)
-{
-	struct acpi_iort_node *node;
-
-	node = iort_scan_node(type, iort_match_type_callback, NULL);
-
-	return node != NULL;
-}
-
 static acpi_status iort_match_node_callback(struct acpi_iort_node *node,
 					    void *context)
 {
diff --git a/drivers/acpi/battery.c b/drivers/acpi/battery.c
index d42eeef9d928..1cbb88d938e5 100644
--- a/drivers/acpi/battery.c
+++ b/drivers/acpi/battery.c
@@ -782,7 +782,7 @@ static int acpi_battery_update(struct acpi_battery *battery, bool resume)
 	if ((battery->state & ACPI_BATTERY_STATE_CRITICAL) ||
 	    (test_bit(ACPI_BATTERY_ALARM_PRESENT, &battery->flags) &&
             (battery->capacity_now <= battery->alarm)))
-		pm_wakeup_event(&battery->device->dev, 0);
+		acpi_pm_wakeup_event(&battery->device->dev);
 
 	return result;
 }
diff --git a/drivers/acpi/bus.c b/drivers/acpi/bus.c
index 784bda663d16..5a6fbe0fcaf2 100644
--- a/drivers/acpi/bus.c
+++ b/drivers/acpi/bus.c
@@ -196,42 +196,19 @@ static void acpi_print_osc_error(acpi_handle handle,
 	pr_debug("\n");
 }
 
-acpi_status acpi_str_to_uuid(char *str, u8 *uuid)
-{
-	int i;
-	static int opc_map_to_uuid[16] = {6, 4, 2, 0, 11, 9, 16, 14, 19, 21,
-		24, 26, 28, 30, 32, 34};
-
-	if (strlen(str) != 36)
-		return AE_BAD_PARAMETER;
-	for (i = 0; i < 36; i++) {
-		if (i == 8 || i == 13 || i == 18 || i == 23) {
-			if (str[i] != '-')
-				return AE_BAD_PARAMETER;
-		} else if (!isxdigit(str[i]))
-			return AE_BAD_PARAMETER;
-	}
-	for (i = 0; i < 16; i++) {
-		uuid[i] = hex_to_bin(str[opc_map_to_uuid[i]]) << 4;
-		uuid[i] |= hex_to_bin(str[opc_map_to_uuid[i] + 1]);
-	}
-	return AE_OK;
-}
-EXPORT_SYMBOL_GPL(acpi_str_to_uuid);
-
 acpi_status acpi_run_osc(acpi_handle handle, struct acpi_osc_context *context)
 {
 	acpi_status status;
 	struct acpi_object_list input;
 	union acpi_object in_params[4];
 	union acpi_object *out_obj;
-	u8 uuid[16];
+	guid_t guid;
 	u32 errors;
 	struct acpi_buffer output = {ACPI_ALLOCATE_BUFFER, NULL};
 
 	if (!context)
 		return AE_ERROR;
-	if (ACPI_FAILURE(acpi_str_to_uuid(context->uuid_str, uuid)))
+	if (guid_parse(context->uuid_str, &guid))
 		return AE_ERROR;
 	context->ret.length = ACPI_ALLOCATE_BUFFER;
 	context->ret.pointer = NULL;
@@ -241,7 +218,7 @@ acpi_status acpi_run_osc(acpi_handle handle, struct acpi_osc_context *context)
 	input.pointer = in_params;
 	in_params[0].type 		= ACPI_TYPE_BUFFER;
 	in_params[0].buffer.length 	= 16;
-	in_params[0].buffer.pointer	= uuid;
+	in_params[0].buffer.pointer	= (u8 *)&guid;
 	in_params[1].type 		= ACPI_TYPE_INTEGER;
 	in_params[1].integer.value 	= context->rev;
 	in_params[2].type 		= ACPI_TYPE_INTEGER;
diff --git a/drivers/acpi/button.c b/drivers/acpi/button.c
index e19f530f1083..ef1856b15488 100644
--- a/drivers/acpi/button.c
+++ b/drivers/acpi/button.c
@@ -19,7 +19,7 @@
  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  */
 
-#define pr_fmt(fmt) "ACPI : button: " fmt
+#define pr_fmt(fmt) "ACPI: button: " fmt
 
 #include <linux/kernel.h>
 #include <linux/module.h>
@@ -217,7 +217,7 @@ static int acpi_lid_notify_state(struct acpi_device *device, int state)
 	}
 
 	if (state)
-		pm_wakeup_event(&device->dev, 0);
+		acpi_pm_wakeup_event(&device->dev);
 
 	ret = blocking_notifier_call_chain(&acpi_lid_notifier, state, device);
 	if (ret == NOTIFY_DONE)
@@ -402,7 +402,7 @@ static void acpi_button_notify(struct acpi_device *device, u32 event)
 		} else {
 			int keycode;
 
-			pm_wakeup_event(&device->dev, 0);
+			acpi_pm_wakeup_event(&device->dev);
 			if (button->suspended)
 				break;
 
@@ -534,6 +534,7 @@ static int acpi_button_add(struct acpi_device *device)
 		lid_device = device;
 	}
 
+	device_init_wakeup(&device->dev, true);
 	printk(KERN_INFO PREFIX "%s [%s]\n", name, acpi_device_bid(device));
 	return 0;
 
diff --git a/drivers/acpi/device_pm.c b/drivers/acpi/device_pm.c
index 993fd31394c8..28938b5a334e 100644
--- a/drivers/acpi/device_pm.c
+++ b/drivers/acpi/device_pm.c
@@ -24,6 +24,7 @@
 #include <linux/pm_qos.h>
 #include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
+#include <linux/suspend.h>
 
 #include "internal.h"
 
@@ -385,6 +386,12 @@ EXPORT_SYMBOL(acpi_bus_power_manageable);
 #ifdef CONFIG_PM
 static DEFINE_MUTEX(acpi_pm_notifier_lock);
 
+void acpi_pm_wakeup_event(struct device *dev)
+{
+	pm_wakeup_dev_event(dev, 0, acpi_s2idle_wakeup());
+}
+EXPORT_SYMBOL_GPL(acpi_pm_wakeup_event);
+
 static void acpi_pm_notify_handler(acpi_handle handle, u32 val, void *not_used)
 {
 	struct acpi_device *adev;
@@ -399,9 +406,9 @@ static void acpi_pm_notify_handler(acpi_handle handle, u32 val, void *not_used)
 	mutex_lock(&acpi_pm_notifier_lock);
 
 	if (adev->wakeup.flags.notifier_present) {
-		__pm_wakeup_event(adev->wakeup.ws, 0);
-		if (adev->wakeup.context.work.func)
-			queue_pm_work(&adev->wakeup.context.work);
+		pm_wakeup_ws_event(adev->wakeup.ws, 0, acpi_s2idle_wakeup());
+		if (adev->wakeup.context.func)
+			adev->wakeup.context.func(&adev->wakeup.context);
 	}
 
 	mutex_unlock(&acpi_pm_notifier_lock);
@@ -413,7 +420,7 @@ static void acpi_pm_notify_handler(acpi_handle handle, u32 val, void *not_used)
  * acpi_add_pm_notifier - Register PM notify handler for given ACPI device.
  * @adev: ACPI device to add the notify handler for.
  * @dev: Device to generate a wakeup event for while handling the notification.
- * @work_func: Work function to execute when handling the notification.
+ * @func: Work function to execute when handling the notification.
  *
  * NOTE: @adev need not be a run-wake or wakeup device to be a valid source of
  * PM wakeup events.  For example, wakeup events may be generated for bridges
@@ -421,11 +428,11 @@ static void acpi_pm_notify_handler(acpi_handle handle, u32 val, void *not_used)
  * bridge itself doesn't have a wakeup GPE associated with it.
  */
 acpi_status acpi_add_pm_notifier(struct acpi_device *adev, struct device *dev,
-				 void (*work_func)(struct work_struct *work))
+			void (*func)(struct acpi_device_wakeup_context *context))
 {
 	acpi_status status = AE_ALREADY_EXISTS;
 
-	if (!dev && !work_func)
+	if (!dev && !func)
 		return AE_BAD_PARAMETER;
 
 	mutex_lock(&acpi_pm_notifier_lock);
@@ -435,8 +442,7 @@ acpi_status acpi_add_pm_notifier(struct acpi_device *adev, struct device *dev,
 
 	adev->wakeup.ws = wakeup_source_register(dev_name(&adev->dev));
 	adev->wakeup.context.dev = dev;
-	if (work_func)
-		INIT_WORK(&adev->wakeup.context.work, work_func);
+	adev->wakeup.context.func = func;
 
 	status = acpi_install_notify_handler(adev->handle, ACPI_SYSTEM_NOTIFY,
 					     acpi_pm_notify_handler, NULL);
@@ -469,10 +475,7 @@ acpi_status acpi_remove_pm_notifier(struct acpi_device *adev)
 	if (ACPI_FAILURE(status))
 		goto out;
 
-	if (adev->wakeup.context.work.func) {
-		cancel_work_sync(&adev->wakeup.context.work);
-		adev->wakeup.context.work.func = NULL;
-	}
+	adev->wakeup.context.func = NULL;
 	adev->wakeup.context.dev = NULL;
 	wakeup_source_unregister(adev->wakeup.ws);
 
@@ -493,6 +496,13 @@ bool acpi_bus_can_wakeup(acpi_handle handle)
 }
 EXPORT_SYMBOL(acpi_bus_can_wakeup);
 
+bool acpi_pm_device_can_wakeup(struct device *dev)
+{
+	struct acpi_device *adev = ACPI_COMPANION(dev);
+
+	return adev ? acpi_device_can_wakeup(adev) : false;
+}
+
 /**
  * acpi_dev_pm_get_state - Get preferred power state of ACPI device.
  * @dev: Device whose preferred target power state to return.
@@ -658,16 +668,15 @@ EXPORT_SYMBOL(acpi_pm_device_sleep_state);
 
 /**
  * acpi_pm_notify_work_func - ACPI devices wakeup notification work function.
- * @work: Work item to handle.
+ * @context: Device wakeup context.
  */
-static void acpi_pm_notify_work_func(struct work_struct *work)
+static void acpi_pm_notify_work_func(struct acpi_device_wakeup_context *context)
 {
-	struct device *dev;
+	struct device *dev = context->dev;
 
-	dev = container_of(work, struct acpi_device_wakeup_context, work)->dev;
 	if (dev) {
 		pm_wakeup_event(dev, 0);
-		pm_runtime_resume(dev);
+		pm_request_resume(dev);
 	}
 }
 
@@ -693,80 +702,53 @@ static int acpi_device_wakeup(struct acpi_device *adev, u32 target_state,
 		acpi_status res;
 		int error;
 
+		if (adev->wakeup.flags.enabled)
+			return 0;
+
 		error = acpi_enable_wakeup_device_power(adev, target_state);
 		if (error)
 			return error;
 
-		if (adev->wakeup.flags.enabled)
-			return 0;
-
 		res = acpi_enable_gpe(wakeup->gpe_device, wakeup->gpe_number);
-		if (ACPI_SUCCESS(res)) {
-			adev->wakeup.flags.enabled = 1;
-		} else {
+		if (ACPI_FAILURE(res)) {
 			acpi_disable_wakeup_device_power(adev);
 			return -EIO;
 		}
-	} else {
-		if (adev->wakeup.flags.enabled) {
-			acpi_disable_gpe(wakeup->gpe_device, wakeup->gpe_number);
-			adev->wakeup.flags.enabled = 0;
-		}
+		adev->wakeup.flags.enabled = 1;
+	} else if (adev->wakeup.flags.enabled) {
+		acpi_disable_gpe(wakeup->gpe_device, wakeup->gpe_number);
 		acpi_disable_wakeup_device_power(adev);
+		adev->wakeup.flags.enabled = 0;
 	}
 	return 0;
 }
 
 /**
- * acpi_pm_device_run_wake - Enable/disable remote wakeup for given device.
- * @dev: Device to enable/disable the platform to wake up.
+ * acpi_pm_set_device_wakeup - Enable/disable remote wakeup for given device.
+ * @dev: Device to enable/disable to generate wakeup events.
  * @enable: Whether to enable or disable the wakeup functionality.
  */
-int acpi_pm_device_run_wake(struct device *phys_dev, bool enable)
-{
-	struct acpi_device *adev;
-
-	if (!device_run_wake(phys_dev))
-		return -EINVAL;
-
-	adev = ACPI_COMPANION(phys_dev);
-	if (!adev) {
-		dev_dbg(phys_dev, "ACPI companion missing in %s!\n", __func__);
-		return -ENODEV;
-	}
-
-	return acpi_device_wakeup(adev, ACPI_STATE_S0, enable);
-}
-EXPORT_SYMBOL(acpi_pm_device_run_wake);
-
-#ifdef CONFIG_PM_SLEEP
-/**
- * acpi_pm_device_sleep_wake - Enable or disable device to wake up the system.
- * @dev: Device to enable/desible to wake up the system from sleep states.
- * @enable: Whether to enable or disable @dev to wake up the system.
- */
-int acpi_pm_device_sleep_wake(struct device *dev, bool enable)
+int acpi_pm_set_device_wakeup(struct device *dev, bool enable)
 {
 	struct acpi_device *adev;
 	int error;
 
-	if (!device_can_wakeup(dev))
-		return -EINVAL;
-
 	adev = ACPI_COMPANION(dev);
 	if (!adev) {
 		dev_dbg(dev, "ACPI companion missing in %s!\n", __func__);
 		return -ENODEV;
 	}
 
+	if (!acpi_device_can_wakeup(adev))
+		return -EINVAL;
+
 	error = acpi_device_wakeup(adev, acpi_target_system_state(), enable);
 	if (!error)
-		dev_info(dev, "System wakeup %s by ACPI\n",
-				enable ? "enabled" : "disabled");
+		dev_dbg(dev, "Wakeup %s by ACPI\n", enable ? "enabled" : "disabled");
 
 	return error;
 }
-#endif /* CONFIG_PM_SLEEP */
+EXPORT_SYMBOL(acpi_pm_set_device_wakeup);
 
 /**
  * acpi_dev_pm_low_power - Put ACPI device into a low-power state.
diff --git a/drivers/acpi/ec.c b/drivers/acpi/ec.c
index c24235d8fb52..854d428e2a2d 100644
--- a/drivers/acpi/ec.c
+++ b/drivers/acpi/ec.c
@@ -27,7 +27,7 @@
 
 /* Uncomment next line to get verbose printout */
 /* #define DEBUG */
-#define pr_fmt(fmt) "ACPI : EC: " fmt
+#define pr_fmt(fmt) "ACPI: EC: " fmt
 
 #include <linux/kernel.h>
 #include <linux/module.h>
@@ -190,6 +190,7 @@ static struct workqueue_struct *ec_query_wq;
 
 static int EC_FLAGS_QUERY_HANDSHAKE; /* Needs QR_EC issued when SCI_EVT set */
 static int EC_FLAGS_CORRECT_ECDT; /* Needs ECDT port address correction */
+static int EC_FLAGS_IGNORE_DSDT_GPE; /* Needs ECDT GPE as correction setting */
 
 /* --------------------------------------------------------------------------
  *                           Logging/Debugging
@@ -316,7 +317,7 @@ static inline void acpi_ec_write_data(struct acpi_ec *ec, u8 data)
 	ec->timestamp = jiffies;
 }
 
-#ifdef DEBUG
+#if defined(DEBUG) || defined(CONFIG_DYNAMIC_DEBUG)
 static const char *acpi_ec_cmd_string(u8 cmd)
 {
 	switch (cmd) {
@@ -459,8 +460,10 @@ static bool acpi_ec_submit_flushable_request(struct acpi_ec *ec)
 
 static void acpi_ec_submit_query(struct acpi_ec *ec)
 {
-	if (acpi_ec_event_enabled(ec) &&
-	    !test_and_set_bit(EC_FLAGS_QUERY_PENDING, &ec->flags)) {
+	acpi_ec_set_storm(ec, EC_FLAGS_COMMAND_STORM);
+	if (!acpi_ec_event_enabled(ec))
+		return;
+	if (!test_and_set_bit(EC_FLAGS_QUERY_PENDING, &ec->flags)) {
 		ec_dbg_evt("Command(%s) submitted/blocked",
 			   acpi_ec_cmd_string(ACPI_EC_COMMAND_QUERY));
 		ec->nr_pending_queries++;
@@ -470,11 +473,10 @@ static void acpi_ec_submit_query(struct acpi_ec *ec)
 
 static void acpi_ec_complete_query(struct acpi_ec *ec)
 {
-	if (test_bit(EC_FLAGS_QUERY_PENDING, &ec->flags)) {
-		clear_bit(EC_FLAGS_QUERY_PENDING, &ec->flags);
+	if (test_and_clear_bit(EC_FLAGS_QUERY_PENDING, &ec->flags))
 		ec_dbg_evt("Command(%s) unblocked",
 			   acpi_ec_cmd_string(ACPI_EC_COMMAND_QUERY));
-	}
+	acpi_ec_clear_storm(ec, EC_FLAGS_COMMAND_STORM);
 }
 
 static inline void __acpi_ec_enable_event(struct acpi_ec *ec)
@@ -1362,13 +1364,23 @@ ec_parse_device(acpi_handle handle, u32 Level, void *context, void **retval)
 				     ec_parse_io_ports, ec);
 	if (ACPI_FAILURE(status))
 		return status;
+	if (ec->data_addr == 0 || ec->command_addr == 0)
+		return AE_OK;
 
-	/* Get GPE bit assignment (EC events). */
-	/* TODO: Add support for _GPE returning a package */
-	status = acpi_evaluate_integer(handle, "_GPE", NULL, &tmp);
-	if (ACPI_FAILURE(status))
-		return status;
-	ec->gpe = tmp;
+	if (boot_ec && boot_ec_is_ecdt && EC_FLAGS_IGNORE_DSDT_GPE) {
+		/*
+		 * Always inherit the GPE number setting from the ECDT
+		 * EC.
+		 */
+		ec->gpe = boot_ec->gpe;
+	} else {
+		/* Get GPE bit assignment (EC events). */
+		/* TODO: Add support for _GPE returning a package */
+		status = acpi_evaluate_integer(handle, "_GPE", NULL, &tmp);
+		if (ACPI_FAILURE(status))
+			return status;
+		ec->gpe = tmp;
+	}
 	/* Use the global lock for all EC transactions? */
 	tmp = 0;
 	acpi_evaluate_integer(handle, "_GLK", NULL, &tmp);
@@ -1665,12 +1677,26 @@ static const struct acpi_device_id ec_device_ids[] = {
 	{"", 0},
 };
 
+/*
+ * This function is not Windows-compatible as Windows never enumerates the
+ * namespace EC before the main ACPI device enumeration process. It is
+ * retained for historical reason and will be deprecated in the future.
+ */
 int __init acpi_ec_dsdt_probe(void)
 {
 	acpi_status status;
 	struct acpi_ec *ec;
 	int ret;
 
+	/*
+	 * If a platform has ECDT, there is no need to proceed as the
+	 * following probe is not a part of the ACPI device enumeration,
+	 * executing _STA is not safe, and thus this probe may risk of
+	 * picking up an invalid EC device.
+	 */
+	if (boot_ec)
+		return -ENODEV;
+
 	ec = acpi_ec_alloc();
 	if (!ec)
 		return -ENOMEM;
@@ -1753,11 +1779,43 @@ static int ec_correct_ecdt(const struct dmi_system_id *id)
 	return 0;
 }
 
+/*
+ * Some DSDTs contain wrong GPE setting.
+ * Asus FX502VD/VE, GL702VMK, X550VXK, X580VD
+ * https://bugzilla.kernel.org/show_bug.cgi?id=195651
+ */
+static int ec_honor_ecdt_gpe(const struct dmi_system_id *id)
+{
+	pr_debug("Detected system needing ignore DSDT GPE setting.\n");
+	EC_FLAGS_IGNORE_DSDT_GPE = 1;
+	return 0;
+}
+
 static struct dmi_system_id ec_dmi_table[] __initdata = {
 	{
 	ec_correct_ecdt, "MSI MS-171F", {
 	DMI_MATCH(DMI_SYS_VENDOR, "Micro-Star"),
 	DMI_MATCH(DMI_PRODUCT_NAME, "MS-171F"),}, NULL},
+	{
+	ec_honor_ecdt_gpe, "ASUS FX502VD", {
+	DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+	DMI_MATCH(DMI_PRODUCT_NAME, "FX502VD"),}, NULL},
+	{
+	ec_honor_ecdt_gpe, "ASUS FX502VE", {
+	DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+	DMI_MATCH(DMI_PRODUCT_NAME, "FX502VE"),}, NULL},
+	{
+	ec_honor_ecdt_gpe, "ASUS GL702VMK", {
+	DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+	DMI_MATCH(DMI_PRODUCT_NAME, "GL702VMK"),}, NULL},
+	{
+	ec_honor_ecdt_gpe, "ASUS X550VXK", {
+	DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+	DMI_MATCH(DMI_PRODUCT_NAME, "X550VXK"),}, NULL},
+	{
+	ec_honor_ecdt_gpe, "ASUS X580VD", {
+	DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+	DMI_MATCH(DMI_PRODUCT_NAME, "X580VD"),}, NULL},
 	{},
 };
 
@@ -1835,7 +1893,7 @@ static int acpi_ec_suspend(struct device *dev)
 	struct acpi_ec *ec =
 		acpi_driver_data(to_acpi_device(dev));
 
-	if (ec_freeze_events)
+	if (acpi_sleep_no_ec_events() && ec_freeze_events)
 		acpi_ec_disable_event(ec);
 	return 0;
 }
diff --git a/drivers/acpi/internal.h b/drivers/acpi/internal.h
index 66229ffa909b..be79f7db1850 100644
--- a/drivers/acpi/internal.h
+++ b/drivers/acpi/internal.h
@@ -198,8 +198,12 @@ void acpi_ec_remove_query_handler(struct acpi_ec *ec, u8 query_bit);
                                   Suspend/Resume
   -------------------------------------------------------------------------- */
 #ifdef CONFIG_ACPI_SYSTEM_POWER_STATES_SUPPORT
+extern bool acpi_s2idle_wakeup(void);
+extern bool acpi_sleep_no_ec_events(void);
 extern int acpi_sleep_init(void);
 #else
+static inline bool acpi_s2idle_wakeup(void) { return false; }
+static inline bool acpi_sleep_no_ec_events(void) { return true; }
 static inline int acpi_sleep_init(void) { return -ENXIO; }
 #endif
 
diff --git a/drivers/acpi/ioapic.c b/drivers/acpi/ioapic.c
index 7e4fbf9a53a3..3595aa9c7c18 100644
--- a/drivers/acpi/ioapic.c
+++ b/drivers/acpi/ioapic.c
@@ -21,7 +21,7 @@
  * registered when we parsed the ACPI MADT.
  */
 
-#define pr_fmt(fmt) "ACPI : IOAPIC: " fmt
+#define pr_fmt(fmt) "ACPI: IOAPIC: " fmt
 
 #include <linux/slab.h>
 #include <linux/acpi.h>
diff --git a/drivers/acpi/nfit/core.c b/drivers/acpi/nfit/core.c
index 656acb5d7166..097eff0b963d 100644
--- a/drivers/acpi/nfit/core.c
+++ b/drivers/acpi/nfit/core.c
@@ -74,11 +74,11 @@ struct nfit_table_prev {
 	struct list_head flushes;
 };
 
-static u8 nfit_uuid[NFIT_UUID_MAX][16];
+static guid_t nfit_uuid[NFIT_UUID_MAX];
 
-const u8 *to_nfit_uuid(enum nfit_uuids id)
+const guid_t *to_nfit_uuid(enum nfit_uuids id)
 {
-	return nfit_uuid[id];
+	return &nfit_uuid[id];
 }
 EXPORT_SYMBOL(to_nfit_uuid);
 
@@ -222,7 +222,7 @@ int acpi_nfit_ctl(struct nvdimm_bus_descriptor *nd_desc, struct nvdimm *nvdimm,
 	u32 offset, fw_status = 0;
 	acpi_handle handle;
 	unsigned int func;
-	const u8 *uuid;
+	const guid_t *guid;
 	int rc, i;
 
 	func = cmd;
@@ -245,7 +245,7 @@ int acpi_nfit_ctl(struct nvdimm_bus_descriptor *nd_desc, struct nvdimm *nvdimm,
 		cmd_mask = nvdimm_cmd_mask(nvdimm);
 		dsm_mask = nfit_mem->dsm_mask;
 		desc = nd_cmd_dimm_desc(cmd);
-		uuid = to_nfit_uuid(nfit_mem->family);
+		guid = to_nfit_uuid(nfit_mem->family);
 		handle = adev->handle;
 	} else {
 		struct acpi_device *adev = to_acpi_dev(acpi_desc);
@@ -254,7 +254,7 @@ int acpi_nfit_ctl(struct nvdimm_bus_descriptor *nd_desc, struct nvdimm *nvdimm,
 		cmd_mask = nd_desc->cmd_mask;
 		dsm_mask = cmd_mask;
 		desc = nd_cmd_bus_desc(cmd);
-		uuid = to_nfit_uuid(NFIT_DEV_BUS);
+		guid = to_nfit_uuid(NFIT_DEV_BUS);
 		handle = adev->handle;
 		dimm_name = "bus";
 	}
@@ -289,7 +289,7 @@ int acpi_nfit_ctl(struct nvdimm_bus_descriptor *nd_desc, struct nvdimm *nvdimm,
 			in_buf.buffer.pointer,
 			min_t(u32, 256, in_buf.buffer.length), true);
 
-	out_obj = acpi_evaluate_dsm(handle, uuid, 1, func, &in_obj);
+	out_obj = acpi_evaluate_dsm(handle, guid, 1, func, &in_obj);
 	if (!out_obj) {
 		dev_dbg(dev, "%s:%s _DSM failed cmd: %s\n", __func__, dimm_name,
 				cmd_name);
@@ -409,7 +409,7 @@ int nfit_spa_type(struct acpi_nfit_system_address *spa)
 	int i;
 
 	for (i = 0; i < NFIT_UUID_MAX; i++)
-		if (memcmp(to_nfit_uuid(i), spa->range_guid, 16) == 0)
+		if (guid_equal(to_nfit_uuid(i), (guid_t *)&spa->range_guid))
 			return i;
 	return -1;
 }
@@ -1415,7 +1415,7 @@ static int acpi_nfit_add_dimm(struct acpi_nfit_desc *acpi_desc,
 	struct acpi_device *adev, *adev_dimm;
 	struct device *dev = acpi_desc->dev;
 	unsigned long dsm_mask;
-	const u8 *uuid;
+	const guid_t *guid;
 	int i;
 	int family = -1;
 
@@ -1444,7 +1444,7 @@ static int acpi_nfit_add_dimm(struct acpi_nfit_desc *acpi_desc,
 	/*
 	 * Until standardization materializes we need to consider 4
 	 * different command sets.  Note, that checking for function0 (bit0)
-	 * tells us if any commands are reachable through this uuid.
+	 * tells us if any commands are reachable through this GUID.
 	 */
 	for (i = NVDIMM_FAMILY_INTEL; i <= NVDIMM_FAMILY_MSFT; i++)
 		if (acpi_check_dsm(adev_dimm->handle, to_nfit_uuid(i), 1, 1))
@@ -1474,9 +1474,9 @@ static int acpi_nfit_add_dimm(struct acpi_nfit_desc *acpi_desc,
 		return 0;
 	}
 
-	uuid = to_nfit_uuid(nfit_mem->family);
+	guid = to_nfit_uuid(nfit_mem->family);
 	for_each_set_bit(i, &dsm_mask, BITS_PER_LONG)
-		if (acpi_check_dsm(adev_dimm->handle, uuid, 1, 1ULL << i))
+		if (acpi_check_dsm(adev_dimm->handle, guid, 1, 1ULL << i))
 			set_bit(i, &nfit_mem->dsm_mask);
 
 	return 0;
@@ -1611,7 +1611,7 @@ static int acpi_nfit_register_dimms(struct acpi_nfit_desc *acpi_desc)
 static void acpi_nfit_init_dsms(struct acpi_nfit_desc *acpi_desc)
 {
 	struct nvdimm_bus_descriptor *nd_desc = &acpi_desc->nd_desc;
-	const u8 *uuid = to_nfit_uuid(NFIT_DEV_BUS);
+	const guid_t *guid = to_nfit_uuid(NFIT_DEV_BUS);
 	struct acpi_device *adev;
 	int i;
 
@@ -1621,7 +1621,7 @@ static void acpi_nfit_init_dsms(struct acpi_nfit_desc *acpi_desc)
 		return;
 
 	for (i = ND_CMD_ARS_CAP; i <= ND_CMD_CLEAR_ERROR; i++)
-		if (acpi_check_dsm(adev->handle, uuid, 1, 1ULL << i))
+		if (acpi_check_dsm(adev->handle, guid, 1, 1ULL << i))
 			set_bit(i, &nd_desc->cmd_mask);
 }
 
@@ -3051,19 +3051,19 @@ static __init int nfit_init(void)
 	BUILD_BUG_ON(sizeof(struct acpi_nfit_control_region) != 80);
 	BUILD_BUG_ON(sizeof(struct acpi_nfit_data_region) != 40);
 
-	acpi_str_to_uuid(UUID_VOLATILE_MEMORY, nfit_uuid[NFIT_SPA_VOLATILE]);
-	acpi_str_to_uuid(UUID_PERSISTENT_MEMORY, nfit_uuid[NFIT_SPA_PM]);
-	acpi_str_to_uuid(UUID_CONTROL_REGION, nfit_uuid[NFIT_SPA_DCR]);
-	acpi_str_to_uuid(UUID_DATA_REGION, nfit_uuid[NFIT_SPA_BDW]);
-	acpi_str_to_uuid(UUID_VOLATILE_VIRTUAL_DISK, nfit_uuid[NFIT_SPA_VDISK]);
-	acpi_str_to_uuid(UUID_VOLATILE_VIRTUAL_CD, nfit_uuid[NFIT_SPA_VCD]);
-	acpi_str_to_uuid(UUID_PERSISTENT_VIRTUAL_DISK, nfit_uuid[NFIT_SPA_PDISK]);
-	acpi_str_to_uuid(UUID_PERSISTENT_VIRTUAL_CD, nfit_uuid[NFIT_SPA_PCD]);
-	acpi_str_to_uuid(UUID_NFIT_BUS, nfit_uuid[NFIT_DEV_BUS]);
-	acpi_str_to_uuid(UUID_NFIT_DIMM, nfit_uuid[NFIT_DEV_DIMM]);
-	acpi_str_to_uuid(UUID_NFIT_DIMM_N_HPE1, nfit_uuid[NFIT_DEV_DIMM_N_HPE1]);
-	acpi_str_to_uuid(UUID_NFIT_DIMM_N_HPE2, nfit_uuid[NFIT_DEV_DIMM_N_HPE2]);
-	acpi_str_to_uuid(UUID_NFIT_DIMM_N_MSFT, nfit_uuid[NFIT_DEV_DIMM_N_MSFT]);
+	guid_parse(UUID_VOLATILE_MEMORY, &nfit_uuid[NFIT_SPA_VOLATILE]);
+	guid_parse(UUID_PERSISTENT_MEMORY, &nfit_uuid[NFIT_SPA_PM]);
+	guid_parse(UUID_CONTROL_REGION, &nfit_uuid[NFIT_SPA_DCR]);
+	guid_parse(UUID_DATA_REGION, &nfit_uuid[NFIT_SPA_BDW]);
+	guid_parse(UUID_VOLATILE_VIRTUAL_DISK, &nfit_uuid[NFIT_SPA_VDISK]);
+	guid_parse(UUID_VOLATILE_VIRTUAL_CD, &nfit_uuid[NFIT_SPA_VCD]);
+	guid_parse(UUID_PERSISTENT_VIRTUAL_DISK, &nfit_uuid[NFIT_SPA_PDISK]);
+	guid_parse(UUID_PERSISTENT_VIRTUAL_CD, &nfit_uuid[NFIT_SPA_PCD]);
+	guid_parse(UUID_NFIT_BUS, &nfit_uuid[NFIT_DEV_BUS]);
+	guid_parse(UUID_NFIT_DIMM, &nfit_uuid[NFIT_DEV_DIMM]);
+	guid_parse(UUID_NFIT_DIMM_N_HPE1, &nfit_uuid[NFIT_DEV_DIMM_N_HPE1]);
+	guid_parse(UUID_NFIT_DIMM_N_HPE2, &nfit_uuid[NFIT_DEV_DIMM_N_HPE2]);
+	guid_parse(UUID_NFIT_DIMM_N_MSFT, &nfit_uuid[NFIT_DEV_DIMM_N_MSFT]);
 
 	nfit_wq = create_singlethread_workqueue("nfit");
 	if (!nfit_wq)
diff --git a/drivers/acpi/nfit/nfit.h b/drivers/acpi/nfit/nfit.h
index 58fb7d68e04a..29bdd959517f 100644
--- a/drivers/acpi/nfit/nfit.h
+++ b/drivers/acpi/nfit/nfit.h
@@ -18,7 +18,6 @@
 #include <linux/libnvdimm.h>
 #include <linux/ndctl.h>
 #include <linux/types.h>
-#include <linux/uuid.h>
 #include <linux/acpi.h>
 #include <acpi/acuuid.h>
 
@@ -237,7 +236,7 @@ static inline struct acpi_nfit_desc *to_acpi_desc(
 	return container_of(nd_desc, struct acpi_nfit_desc, nd_desc);
 }
 
-const u8 *to_nfit_uuid(enum nfit_uuids id);
+const guid_t *to_nfit_uuid(enum nfit_uuids id);
 int acpi_nfit_init(struct acpi_nfit_desc *acpi_desc, void *nfit, acpi_size sz);
 void acpi_nfit_shutdown(void *data);
 void __acpi_nfit_notify(struct device *dev, acpi_handle handle, u32 event);
diff --git a/drivers/acpi/pci_root.c b/drivers/acpi/pci_root.c
index 919be0aa2578..9eec3095e6c3 100644
--- a/drivers/acpi/pci_root.c
+++ b/drivers/acpi/pci_root.c
@@ -523,7 +523,7 @@ static int acpi_pci_root_add(struct acpi_device *device,
 	struct acpi_pci_root *root;
 	acpi_handle handle = device->handle;
 	int no_aspm = 0;
-	bool hotadd = system_state != SYSTEM_BOOTING;
+	bool hotadd = system_state == SYSTEM_RUNNING;
 
 	root = kzalloc(sizeof(struct acpi_pci_root), GFP_KERNEL);
 	if (!root)
@@ -608,8 +608,7 @@ static int acpi_pci_root_add(struct acpi_device *device,
 		pcie_no_aspm();
 
 	pci_acpi_add_bus_pm_notifier(device);
-	if (device->wakeup.flags.run_wake)
-		device_set_run_wake(root->bus->bridge, true);
+	device_set_wakeup_capable(root->bus->bridge, device->wakeup.flags.valid);
 
 	if (hotadd) {
 		pcibios_resource_survey_bus(root->bus);
@@ -649,7 +648,7 @@ static void acpi_pci_root_remove(struct acpi_device *device)
 	pci_stop_root_bus(root->bus);
 
 	pci_ioapic_remove(root);
-	device_set_run_wake(root->bus->bridge, false);
+	device_set_wakeup_capable(root->bus->bridge, false);
 	pci_acpi_remove_bus_pm_notifier(device);
 
 	pci_remove_root_bus(root->bus);
diff --git a/drivers/acpi/pmic/intel_pmic_xpower.c b/drivers/acpi/pmic/intel_pmic_xpower.c
index 1a76c784cd4c..3b7d5be5b7ed 100644
--- a/drivers/acpi/pmic/intel_pmic_xpower.c
+++ b/drivers/acpi/pmic/intel_pmic_xpower.c
@@ -21,6 +21,11 @@
 #include "intel_pmic.h"
 
 #define XPOWER_GPADC_LOW	0x5b
+#define XPOWER_GPI1_CTRL	0x92
+
+#define GPI1_LDO_MASK		GENMASK(2, 0)
+#define GPI1_LDO_ON		(3 << 0)
+#define GPI1_LDO_OFF		(4 << 0)
 
 static struct pmic_table power_table[] = {
 	{
@@ -118,6 +123,10 @@ static struct pmic_table power_table[] = {
 		.reg = 0x10,
 		.bit = 0x00
 	}, /* BUC6 */
+	{
+		.address = 0x4c,
+		.reg = 0x92,
+	}, /* GPI1 */
 };
 
 /* TMP0 - TMP5 are the same, all from GPADC */
@@ -156,7 +165,12 @@ static int intel_xpower_pmic_get_power(struct regmap *regmap, int reg,
 	if (regmap_read(regmap, reg, &data))
 		return -EIO;
 
-	*value = (data & BIT(bit)) ? 1 : 0;
+	/* GPIO1 LDO regulator needs special handling */
+	if (reg == XPOWER_GPI1_CTRL)
+		*value = ((data & GPI1_LDO_MASK) == GPI1_LDO_ON);
+	else
+		*value = (data & BIT(bit)) ? 1 : 0;
+
 	return 0;
 }
 
@@ -165,6 +179,11 @@ static int intel_xpower_pmic_update_power(struct regmap *regmap, int reg,
 {
 	int data;
 
+	/* GPIO1 LDO regulator needs special handling */
+	if (reg == XPOWER_GPI1_CTRL)
+		return regmap_update_bits(regmap, reg, GPI1_LDO_MASK,
+					  on ? GPI1_LDO_ON : GPI1_LDO_OFF);
+
 	if (regmap_read(regmap, reg, &data))
 		return -EIO;
 
diff --git a/drivers/acpi/proc.c b/drivers/acpi/proc.c
index a34669cc823b..85ac848ac6ab 100644
--- a/drivers/acpi/proc.c
+++ b/drivers/acpi/proc.c
@@ -42,7 +42,7 @@ acpi_system_wakeup_device_seq_show(struct seq_file *seq, void *offset)
 
 		if (!dev->physical_node_count) {
 			seq_printf(seq, "%c%-8s\n",
-				dev->wakeup.flags.run_wake ? '*' : ' ',
+				dev->wakeup.flags.valid ? '*' : ' ',
 				device_may_wakeup(&dev->dev) ?
 					"enabled" : "disabled");
 		} else {
@@ -58,7 +58,7 @@ acpi_system_wakeup_device_seq_show(struct seq_file *seq, void *offset)
 					seq_printf(seq, "\t\t");
 
 				seq_printf(seq, "%c%-8s  %s:%s\n",
-					dev->wakeup.flags.run_wake ? '*' : ' ',
+					dev->wakeup.flags.valid ? '*' : ' ',
 					(device_may_wakeup(&dev->dev) ||
 					device_may_wakeup(ldev)) ?
 					"enabled" : "disabled",
diff --git a/drivers/acpi/processor_driver.c b/drivers/acpi/processor_driver.c
index 8697a82bd465..591d1dd3f04e 100644
--- a/drivers/acpi/processor_driver.c
+++ b/drivers/acpi/processor_driver.c
@@ -268,9 +268,9 @@ static int acpi_processor_start(struct device *dev)
 		return -ENODEV;
 
 	/* Protect against concurrent CPU hotplug operations */
-	get_online_cpus();
+	cpu_hotplug_disable();
 	ret = __acpi_processor_start(device);
-	put_online_cpus();
+	cpu_hotplug_enable();
 	return ret;
 }
 
diff --git a/drivers/acpi/processor_throttling.c b/drivers/acpi/processor_throttling.c
index 3de34633f7f9..7f9aff4b8d62 100644
--- a/drivers/acpi/processor_throttling.c
+++ b/drivers/acpi/processor_throttling.c
@@ -909,6 +909,13 @@ static long __acpi_processor_get_throttling(void *data)
 	return pr->throttling.acpi_processor_get_throttling(pr);
 }
 
+static int call_on_cpu(int cpu, long (*fn)(void *), void *arg, bool direct)
+{
+	if (direct || (is_percpu_thread() && cpu == smp_processor_id()))
+		return fn(arg);
+	return work_on_cpu(cpu, fn, arg);
+}
+
 static int acpi_processor_get_throttling(struct acpi_processor *pr)
 {
 	if (!pr)
@@ -926,7 +933,7 @@ static int acpi_processor_get_throttling(struct acpi_processor *pr)
 	if (!cpu_online(pr->id))
 		return -ENODEV;
 
-	return work_on_cpu(pr->id, __acpi_processor_get_throttling, pr);
+	return call_on_cpu(pr->id, __acpi_processor_get_throttling, pr, false);
 }
 
 static int acpi_processor_get_fadt_info(struct acpi_processor *pr)
@@ -1076,13 +1083,6 @@ static long acpi_processor_throttling_fn(void *data)
 			arg->target_state, arg->force);
 }
 
-static int call_on_cpu(int cpu, long (*fn)(void *), void *arg, bool direct)
-{
-	if (direct)
-		return fn(arg);
-	return work_on_cpu(cpu, fn, arg);
-}
-
 static int __acpi_processor_set_throttling(struct acpi_processor *pr,
 					   int state, bool force, bool direct)
 {
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index 3a10d7573477..09f65f57bebe 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -835,7 +835,7 @@ static int acpi_bus_extract_wakeup_device_power_package(acpi_handle handle,
 	return err;
 }
 
-static void acpi_wakeup_gpe_init(struct acpi_device *device)
+static bool acpi_wakeup_gpe_init(struct acpi_device *device)
 {
 	static const struct acpi_device_id button_device_ids[] = {
 		{"PNP0C0C", 0},
@@ -845,13 +845,11 @@ static void acpi_wakeup_gpe_init(struct acpi_device *device)
 	};
 	struct acpi_device_wakeup *wakeup = &device->wakeup;
 	acpi_status status;
-	acpi_event_status event_status;
 
 	wakeup->flags.notifier_present = 0;
 
 	/* Power button, Lid switch always enable wakeup */
 	if (!acpi_match_device_ids(device, button_device_ids)) {
-		wakeup->flags.run_wake = 1;
 		if (!acpi_match_device_ids(device, &button_device_ids[1])) {
 			/* Do not use Lid/sleep button for S5 wakeup */
 			if (wakeup->sleep_state == ACPI_STATE_S5)
@@ -859,17 +857,12 @@ static void acpi_wakeup_gpe_init(struct acpi_device *device)
 		}
 		acpi_mark_gpe_for_wake(wakeup->gpe_device, wakeup->gpe_number);
 		device_set_wakeup_capable(&device->dev, true);
-		return;
+		return true;
 	}
 
-	acpi_setup_gpe_for_wake(device->handle, wakeup->gpe_device,
-				wakeup->gpe_number);
-	status = acpi_get_gpe_status(wakeup->gpe_device, wakeup->gpe_number,
-				     &event_status);
-	if (ACPI_FAILURE(status))
-		return;
-
-	wakeup->flags.run_wake = !!(event_status & ACPI_EVENT_FLAG_HAS_HANDLER);
+	status = acpi_setup_gpe_for_wake(device->handle, wakeup->gpe_device,
+					 wakeup->gpe_number);
+	return ACPI_SUCCESS(status);
 }
 
 static void acpi_bus_get_wakeup_device_flags(struct acpi_device *device)
@@ -887,10 +880,10 @@ static void acpi_bus_get_wakeup_device_flags(struct acpi_device *device)
 		return;
 	}
 
-	device->wakeup.flags.valid = 1;
+	device->wakeup.flags.valid = acpi_wakeup_gpe_init(device);
 	device->wakeup.prepare_count = 0;
-	acpi_wakeup_gpe_init(device);
-	/* Call _PSW/_DSW object to disable its ability to wake the sleeping
+	/*
+	 * Call _PSW/_DSW object to disable its ability to wake the sleeping
 	 * system for the ACPI device with the _PRW object.
 	 * The _PSW object is depreciated in ACPI 3.0 and is replaced by _DSW.
 	 * So it is necessary to call _DSW object first. Only when it is not
@@ -1428,6 +1421,37 @@ static void acpi_init_coherency(struct acpi_device *adev)
 	adev->flags.coherent_dma = cca;
 }
 
+static int acpi_check_spi_i2c_slave(struct acpi_resource *ares, void *data)
+{
+	bool *is_spi_i2c_slave_p = data;
+
+	if (ares->type != ACPI_RESOURCE_TYPE_SERIAL_BUS)
+		return 1;
+
+	/*
+	 * devices that are connected to UART still need to be enumerated to
+	 * platform bus
+	 */
+	if (ares->data.common_serial_bus.type != ACPI_RESOURCE_SERIAL_TYPE_UART)
+		*is_spi_i2c_slave_p = true;
+
+	 /* no need to do more checking */
+	return -1;
+}
+
+static bool acpi_is_spi_i2c_slave(struct acpi_device *device)
+{
+	struct list_head resource_list;
+	bool is_spi_i2c_slave = false;
+
+	INIT_LIST_HEAD(&resource_list);
+	acpi_dev_get_resources(device, &resource_list, acpi_check_spi_i2c_slave,
+			       &is_spi_i2c_slave);
+	acpi_dev_free_resource_list(&resource_list);
+
+	return is_spi_i2c_slave;
+}
+
 void acpi_init_device_object(struct acpi_device *device, acpi_handle handle,
 			     int type, unsigned long long sta)
 {
@@ -1443,6 +1467,7 @@ void acpi_init_device_object(struct acpi_device *device, acpi_handle handle,
 	acpi_bus_get_flags(device);
 	device->flags.match_driver = false;
 	device->flags.initialized = true;
+	device->flags.spi_i2c_slave = acpi_is_spi_i2c_slave(device);
 	acpi_device_clear_enumerated(device);
 	device_initialize(&device->dev);
 	dev_set_uevent_suppress(&device->dev, true);
@@ -1727,38 +1752,13 @@ static acpi_status acpi_bus_check_add(acpi_handle handle, u32 lvl_not_used,
 	return AE_OK;
 }
 
-static int acpi_check_spi_i2c_slave(struct acpi_resource *ares, void *data)
-{
-	bool *is_spi_i2c_slave_p = data;
-
-	if (ares->type != ACPI_RESOURCE_TYPE_SERIAL_BUS)
-		return 1;
-
-	/*
-	 * devices that are connected to UART still need to be enumerated to
-	 * platform bus
-	 */
-	if (ares->data.common_serial_bus.type != ACPI_RESOURCE_SERIAL_TYPE_UART)
-		*is_spi_i2c_slave_p = true;
-
-	 /* no need to do more checking */
-	return -1;
-}
-
 static void acpi_default_enumeration(struct acpi_device *device)
 {
-	struct list_head resource_list;
-	bool is_spi_i2c_slave = false;
-
 	/*
 	 * Do not enumerate SPI/I2C slaves as they will be enumerated by their
 	 * respective parents.
 	 */
-	INIT_LIST_HEAD(&resource_list);
-	acpi_dev_get_resources(device, &resource_list, acpi_check_spi_i2c_slave,
-			       &is_spi_i2c_slave);
-	acpi_dev_free_resource_list(&resource_list);
-	if (!is_spi_i2c_slave) {
+	if (!device->flags.spi_i2c_slave) {
 		acpi_create_platform_device(device, NULL);
 		acpi_device_set_enumerated(device);
 	} else {
@@ -1854,7 +1854,7 @@ static void acpi_bus_attach(struct acpi_device *device)
 		return;
 
 	device->flags.match_driver = true;
-	if (ret > 0) {
+	if (ret > 0 && !device->flags.spi_i2c_slave) {
 		acpi_device_set_enumerated(device);
 		goto ok;
 	}
@@ -1863,10 +1863,10 @@ static void acpi_bus_attach(struct acpi_device *device)
 	if (ret < 0)
 		return;
 
-	if (device->pnp.type.platform_id)
-		acpi_default_enumeration(device);
-	else
+	if (!device->pnp.type.platform_id && !device->flags.spi_i2c_slave)
 		acpi_device_set_enumerated(device);
+	else
+		acpi_default_enumeration(device);
 
  ok:
 	list_for_each_entry(child, &device->children, node)
diff --git a/drivers/acpi/sleep.c b/drivers/acpi/sleep.c
index 097d630ab886..be17664736b2 100644
--- a/drivers/acpi/sleep.c
+++ b/drivers/acpi/sleep.c
@@ -650,38 +650,165 @@ static const struct platform_suspend_ops acpi_suspend_ops_old = {
 	.recover = acpi_pm_finish,
 };
 
+static bool s2idle_in_progress;
+static bool s2idle_wakeup;
+
+/*
+ * On platforms supporting the Low Power S0 Idle interface there is an ACPI
+ * device object with the PNP0D80 compatible device ID (System Power Management
+ * Controller) and a specific _DSM method under it.  That method, if present,
+ * can be used to indicate to the platform that the OS is transitioning into a
+ * low-power state in which certain types of activity are not desirable or that
+ * it is leaving such a state, which allows the platform to adjust its operation
+ * mode accordingly.
+ */
+static const struct acpi_device_id lps0_device_ids[] = {
+	{"PNP0D80", },
+	{"", },
+};
+
+#define ACPI_LPS0_DSM_UUID	"c4eb40a0-6cd2-11e2-bcfd-0800200c9a66"
+
+#define ACPI_LPS0_SCREEN_OFF	3
+#define ACPI_LPS0_SCREEN_ON	4
+#define ACPI_LPS0_ENTRY		5
+#define ACPI_LPS0_EXIT		6
+
+#define ACPI_S2IDLE_FUNC_MASK	((1 << ACPI_LPS0_ENTRY) | (1 << ACPI_LPS0_EXIT))
+
+static acpi_handle lps0_device_handle;
+static guid_t lps0_dsm_guid;
+static char lps0_dsm_func_mask;
+
+static void acpi_sleep_run_lps0_dsm(unsigned int func)
+{
+	union acpi_object *out_obj;
+
+	if (!(lps0_dsm_func_mask & (1 << func)))
+		return;
+
+	out_obj = acpi_evaluate_dsm(lps0_device_handle, &lps0_dsm_guid, 1, func, NULL);
+	ACPI_FREE(out_obj);
+
+	acpi_handle_debug(lps0_device_handle, "_DSM function %u evaluation %s\n",
+			  func, out_obj ? "successful" : "failed");
+}
+
+static int lps0_device_attach(struct acpi_device *adev,
+			      const struct acpi_device_id *not_used)
+{
+	union acpi_object *out_obj;
+
+	if (lps0_device_handle)
+		return 0;
+
+	if (!(acpi_gbl_FADT.flags & ACPI_FADT_LOW_POWER_S0))
+		return 0;
+
+	guid_parse(ACPI_LPS0_DSM_UUID, &lps0_dsm_guid);
+	/* Check if the _DSM is present and as expected. */
+	out_obj = acpi_evaluate_dsm(adev->handle, &lps0_dsm_guid, 1, 0, NULL);
+	if (out_obj && out_obj->type == ACPI_TYPE_BUFFER) {
+		char bitmask = *(char *)out_obj->buffer.pointer;
+
+		if ((bitmask & ACPI_S2IDLE_FUNC_MASK) == ACPI_S2IDLE_FUNC_MASK) {
+			lps0_dsm_func_mask = bitmask;
+			lps0_device_handle = adev->handle;
+		}
+
+		acpi_handle_debug(adev->handle, "_DSM function mask: 0x%x\n",
+				  bitmask);
+	} else {
+		acpi_handle_debug(adev->handle,
+				  "_DSM function 0 evaluation failed\n");
+	}
+	ACPI_FREE(out_obj);
+	return 0;
+}
+
+static struct acpi_scan_handler lps0_handler = {
+	.ids = lps0_device_ids,
+	.attach = lps0_device_attach,
+};
+
 static int acpi_freeze_begin(void)
 {
 	acpi_scan_lock_acquire();
+	s2idle_in_progress = true;
 	return 0;
 }
 
 static int acpi_freeze_prepare(void)
 {
-	acpi_enable_wakeup_devices(ACPI_STATE_S0);
-	acpi_enable_all_wakeup_gpes();
-	acpi_os_wait_events_complete();
+	if (lps0_device_handle) {
+		acpi_sleep_run_lps0_dsm(ACPI_LPS0_SCREEN_OFF);
+		acpi_sleep_run_lps0_dsm(ACPI_LPS0_ENTRY);
+	} else {
+		/*
+		 * The configuration of GPEs is changed here to avoid spurious
+		 * wakeups, but that should not be necessary if this is a
+		 * "low-power S0" platform and the low-power S0 _DSM is present.
+		 */
+		acpi_enable_all_wakeup_gpes();
+		acpi_os_wait_events_complete();
+	}
 	if (acpi_sci_irq_valid())
 		enable_irq_wake(acpi_sci_irq);
+
 	return 0;
 }
 
+static void acpi_freeze_wake(void)
+{
+	/*
+	 * If IRQD_WAKEUP_ARMED is not set for the SCI at this point, it means
+	 * that the SCI has triggered while suspended, so cancel the wakeup in
+	 * case it has not been a wakeup event (the GPEs will be checked later).
+	 */
+	if (acpi_sci_irq_valid() &&
+	    !irqd_is_wakeup_armed(irq_get_irq_data(acpi_sci_irq))) {
+		pm_system_cancel_wakeup();
+		s2idle_wakeup = true;
+	}
+}
+
+static void acpi_freeze_sync(void)
+{
+	/*
+	 * Process all pending events in case there are any wakeup ones.
+	 *
+	 * The EC driver uses the system workqueue, so that one needs to be
+	 * flushed too.
+	 */
+	acpi_os_wait_events_complete();
+	flush_scheduled_work();
+	s2idle_wakeup = false;
+}
+
 static void acpi_freeze_restore(void)
 {
-	acpi_disable_wakeup_devices(ACPI_STATE_S0);
 	if (acpi_sci_irq_valid())
 		disable_irq_wake(acpi_sci_irq);
-	acpi_enable_all_runtime_gpes();
+
+	if (lps0_device_handle) {
+		acpi_sleep_run_lps0_dsm(ACPI_LPS0_EXIT);
+		acpi_sleep_run_lps0_dsm(ACPI_LPS0_SCREEN_ON);
+	} else {
+		acpi_enable_all_runtime_gpes();
+	}
 }
 
 static void acpi_freeze_end(void)
 {
+	s2idle_in_progress = false;
 	acpi_scan_lock_release();
 }
 
 static const struct platform_freeze_ops acpi_freeze_ops = {
 	.begin = acpi_freeze_begin,
 	.prepare = acpi_freeze_prepare,
+	.wake = acpi_freeze_wake,
+	.sync = acpi_freeze_sync,
 	.restore = acpi_freeze_restore,
 	.end = acpi_freeze_end,
 };
@@ -696,13 +823,28 @@ static void acpi_sleep_suspend_setup(void)
 
 	suspend_set_ops(old_suspend_ordering ?
 		&acpi_suspend_ops_old : &acpi_suspend_ops);
+
+	acpi_scan_add_handler(&lps0_handler);
 	freeze_set_ops(&acpi_freeze_ops);
 }
 
 #else /* !CONFIG_SUSPEND */
+#define s2idle_in_progress	(false)
+#define s2idle_wakeup		(false)
+#define lps0_device_handle	(NULL)
 static inline void acpi_sleep_suspend_setup(void) {}
 #endif /* !CONFIG_SUSPEND */
 
+bool acpi_s2idle_wakeup(void)
+{
+	return s2idle_wakeup;
+}
+
+bool acpi_sleep_no_ec_events(void)
+{
+	return !s2idle_in_progress || !lps0_device_handle;
+}
+
 #ifdef CONFIG_PM_SLEEP
 static u32 saved_bm_rld;
 
diff --git a/drivers/acpi/utils.c b/drivers/acpi/utils.c
index 27d0dcfcf47d..b9d956c916f5 100644
--- a/drivers/acpi/utils.c
+++ b/drivers/acpi/utils.c
@@ -613,19 +613,19 @@ acpi_status acpi_evaluate_lck(acpi_handle handle, int lock)
 /**
  * acpi_evaluate_dsm - evaluate device's _DSM method
  * @handle: ACPI device handle
- * @uuid: UUID of requested functions, should be 16 bytes
+ * @guid: GUID of requested functions, should be 16 bytes
  * @rev: revision number of requested function
  * @func: requested function number
  * @argv4: the function specific parameter
  *
- * Evaluate device's _DSM method with specified UUID, revision id and
+ * Evaluate device's _DSM method with specified GUID, revision id and
  * function number. Caller needs to free the returned object.
  *
  * Though ACPI defines the fourth parameter for _DSM should be a package,
  * some old BIOSes do expect a buffer or an integer etc.
  */
 union acpi_object *
-acpi_evaluate_dsm(acpi_handle handle, const u8 *uuid, u64 rev, u64 func,
+acpi_evaluate_dsm(acpi_handle handle, const guid_t *guid, u64 rev, u64 func,
 		  union acpi_object *argv4)
 {
 	acpi_status ret;
@@ -638,7 +638,7 @@ acpi_evaluate_dsm(acpi_handle handle, const u8 *uuid, u64 rev, u64 func,
 
 	params[0].type = ACPI_TYPE_BUFFER;
 	params[0].buffer.length = 16;
-	params[0].buffer.pointer = (char *)uuid;
+	params[0].buffer.pointer = (u8 *)guid;
 	params[1].type = ACPI_TYPE_INTEGER;
 	params[1].integer.value = rev;
 	params[2].type = ACPI_TYPE_INTEGER;
@@ -666,7 +666,7 @@ EXPORT_SYMBOL(acpi_evaluate_dsm);
 /**
  * acpi_check_dsm - check if _DSM method supports requested functions.
  * @handle: ACPI device handle
- * @uuid: UUID of requested functions, should be 16 bytes at least
+ * @guid: GUID of requested functions, should be 16 bytes at least
  * @rev: revision number of requested functions
  * @funcs: bitmap of requested functions
  *
@@ -674,7 +674,7 @@ EXPORT_SYMBOL(acpi_evaluate_dsm);
  * functions. Currently only support 64 functions at maximum, should be
  * enough for now.
  */
-bool acpi_check_dsm(acpi_handle handle, const u8 *uuid, u64 rev, u64 funcs)
+bool acpi_check_dsm(acpi_handle handle, const guid_t *guid, u64 rev, u64 funcs)
 {
 	int i;
 	u64 mask = 0;
@@ -683,7 +683,7 @@ bool acpi_check_dsm(acpi_handle handle, const u8 *uuid, u64 rev, u64 funcs)
 	if (funcs == 0)
 		return false;
 
-	obj = acpi_evaluate_dsm(handle, uuid, rev, 0, NULL);
+	obj = acpi_evaluate_dsm(handle, guid, rev, 0, NULL);
 	if (!obj)
 		return false;
 
@@ -697,7 +697,7 @@ bool acpi_check_dsm(acpi_handle handle, const u8 *uuid, u64 rev, u64 funcs)
 
 	/*
 	 * Bit 0 indicates whether there's support for any functions other than
-	 * function 0 for the specified UUID and revision.
+	 * function 0 for the specified GUID and revision.
 	 */
 	if ((mask & 0x1) && (mask & funcs) == funcs)
 		return true;
diff --git a/drivers/acpi/video_detect.c b/drivers/acpi/video_detect.c
index 7f48156cbc0c..d179e8d9177d 100644
--- a/drivers/acpi/video_detect.c
+++ b/drivers/acpi/video_detect.c
@@ -305,6 +305,14 @@ static const struct dmi_system_id video_detect_dmi_table[] = {
 		DMI_MATCH(DMI_PRODUCT_NAME, "Dell System XPS L702X"),
 		},
 	},
+	{
+	 .callback = video_detect_force_native,
+	 .ident = "Dell Precision 7510",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+		DMI_MATCH(DMI_PRODUCT_NAME, "Precision 7510"),
+		},
+	},
 	{ },
 };
 
diff --git a/drivers/amba/bus.c b/drivers/amba/bus.c
index a56fa2a1e9aa..e0f74ddc22b7 100644
--- a/drivers/amba/bus.c
+++ b/drivers/amba/bus.c
@@ -105,6 +105,7 @@ static ssize_t driver_override_store(struct device *_dev,
 
 	return count;
 }
+static DEVICE_ATTR_RW(driver_override);
 
 #define amba_attr_func(name,fmt,arg...)					\
 static ssize_t name##_show(struct device *_dev,				\
@@ -112,25 +113,23 @@ static ssize_t name##_show(struct device *_dev,				\
 {									\
 	struct amba_device *dev = to_amba_device(_dev);			\
 	return sprintf(buf, fmt, arg);					\
-}
-
-#define amba_attr(name,fmt,arg...)	\
-amba_attr_func(name,fmt,arg)		\
-static DEVICE_ATTR(name, S_IRUGO, name##_show, NULL)
+}									\
+static DEVICE_ATTR_RO(name)
 
 amba_attr_func(id, "%08x\n", dev->periphid);
-amba_attr(irq0, "%u\n", dev->irq[0]);
-amba_attr(irq1, "%u\n", dev->irq[1]);
+amba_attr_func(irq0, "%u\n", dev->irq[0]);
+amba_attr_func(irq1, "%u\n", dev->irq[1]);
 amba_attr_func(resource, "\t%016llx\t%016llx\t%016lx\n",
 	 (unsigned long long)dev->res.start, (unsigned long long)dev->res.end,
 	 dev->res.flags);
 
-static struct device_attribute amba_dev_attrs[] = {
-	__ATTR_RO(id),
-	__ATTR_RO(resource),
-	__ATTR_RW(driver_override),
-	__ATTR_NULL,
+static struct attribute *amba_dev_attrs[] = {
+	&dev_attr_id.attr,
+	&dev_attr_resource.attr,
+	&dev_attr_driver_override.attr,
+	NULL,
 };
+ATTRIBUTE_GROUPS(amba_dev);
 
 #ifdef CONFIG_PM
 /*
@@ -192,7 +191,7 @@ static const struct dev_pm_ops amba_pm = {
  */
 struct bus_type amba_bustype = {
 	.name		= "amba",
-	.dev_attrs	= amba_dev_attrs,
+	.dev_groups	= amba_dev_groups,
 	.match		= amba_match,
 	.uevent		= amba_uevent,
 	.pm		= &amba_pm,
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index de3eaf051697..948fc86980a1 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -213,6 +213,16 @@ config SATA_FSL
 
 	  If unsure, say N.
 
+config SATA_GEMINI
+	tristate "Gemini SATA bridge support"
+	depends on PATA_FTIDE010
+	default ARCH_GEMINI
+	help
+	  This enabled support for the FTIDE010 to SATA bridge
+	  found in Cortina Systems Gemini platform.
+
+	  If unsure, say N.
+
 config SATA_AHCI_SEATTLE
 	tristate "AMD Seattle 6.0Gbps AHCI SATA host controller support"
 	depends on ARCH_SEATTLE
@@ -599,6 +609,17 @@ config PATA_EP93XX
 
 	  If unsure, say N.
 
+config PATA_FTIDE010
+	tristate "Faraday Technology FTIDE010 PATA support"
+	depends on OF
+	depends on ARM
+	default ARCH_GEMINI
+	help
+	  This option enables support for the Faraday FTIDE010
+	  PATA controller found in the Cortina Gemini SoCs.
+
+	  If unsure, say N.
+
 config PATA_HPT366
 	tristate "HPT 366/368 PATA support"
 	depends on PCI
diff --git a/drivers/ata/Makefile b/drivers/ata/Makefile
index cd931a5eba92..a26ef5a93919 100644
--- a/drivers/ata/Makefile
+++ b/drivers/ata/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_SATA_ACARD_AHCI)	+= acard-ahci.o libahci.o
 obj-$(CONFIG_SATA_AHCI_SEATTLE)	+= ahci_seattle.o libahci.o libahci_platform.o
 obj-$(CONFIG_SATA_AHCI_PLATFORM) += ahci_platform.o libahci.o libahci_platform.o
 obj-$(CONFIG_SATA_FSL)		+= sata_fsl.o
+obj-$(CONFIG_SATA_GEMINI)	+= sata_gemini.o
 obj-$(CONFIG_SATA_INIC162X)	+= sata_inic162x.o
 obj-$(CONFIG_SATA_SIL24)	+= sata_sil24.o
 obj-$(CONFIG_SATA_DWC)		+= sata_dwc_460ex.o
@@ -60,6 +61,7 @@ obj-$(CONFIG_PATA_CS5536)	+= pata_cs5536.o
 obj-$(CONFIG_PATA_CYPRESS)	+= pata_cypress.o
 obj-$(CONFIG_PATA_EFAR)		+= pata_efar.o
 obj-$(CONFIG_PATA_EP93XX)	+= pata_ep93xx.o
+obj-$(CONFIG_PATA_FTIDE010)	+= pata_ftide010.o
 obj-$(CONFIG_PATA_HPT366)	+= pata_hpt366.o
 obj-$(CONFIG_PATA_HPT37X)	+= pata_hpt37x.o
 obj-$(CONFIG_PATA_HPT3X2N)	+= pata_hpt3x2n.o
diff --git a/drivers/ata/acard-ahci.c b/drivers/ata/acard-ahci.c
index ed6a30cd681a..940ddbc59aa7 100644
--- a/drivers/ata/acard-ahci.c
+++ b/drivers/ata/acard-ahci.c
@@ -25,7 +25,7 @@
  *
  *
  * libata documentation is available via 'make {ps|pdf}docs',
- * as Documentation/DocBook/libata.*
+ * as Documentation/driver-api/libata.rst
  *
  * AHCI hardware documentation:
  * http://www.intel.com/technology/serialata/pdf/rev1_0.pdf
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index c69954023c2e..5a5fd0b404eb 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -24,7 +24,7 @@
  *
  *
  * libata documentation is available via 'make {ps|pdf}docs',
- * as Documentation/DocBook/libata.*
+ * as Documentation/driver-api/libata.rst
  *
  * AHCI hardware documentation:
  * http://www.intel.com/technology/serialata/pdf/rev1_0.pdf
@@ -548,6 +548,8 @@ static const struct pci_device_id ahci_pci_tbl[] = {
 	{ PCI_VDEVICE(ASMEDIA, 0x0602), board_ahci },	/* ASM1060 */
 	{ PCI_VDEVICE(ASMEDIA, 0x0611), board_ahci },	/* ASM1061 */
 	{ PCI_VDEVICE(ASMEDIA, 0x0612), board_ahci },	/* ASM1062 */
+	{ PCI_VDEVICE(ASMEDIA, 0x0621), board_ahci },   /* ASM1061R */
+	{ PCI_VDEVICE(ASMEDIA, 0x0622), board_ahci },   /* ASM1062R */
 
 	/*
 	 * Samsung SSDs found on some macbooks.  NCQ times out if MSI is
diff --git a/drivers/ata/ahci.h b/drivers/ata/ahci.h
index 5db6ab261643..8b61123d2c3c 100644
--- a/drivers/ata/ahci.h
+++ b/drivers/ata/ahci.h
@@ -24,7 +24,7 @@
  *
  *
  * libata documentation is available via 'make {ps|pdf}docs',
- * as Documentation/DocBook/libata.*
+ * as Documentation/driver-api/libata.rst
  *
  * AHCI hardware documentation:
  * http://www.intel.com/technology/serialata/pdf/rev1_0.pdf
@@ -248,6 +248,9 @@ enum {
 	AHCI_HFLAG_MULTI_MSI		= 0,
 #endif
 	AHCI_HFLAG_WAKE_BEFORE_STOP	= (1 << 22), /* wake before DMA stop */
+	AHCI_HFLAG_YES_ALPM		= (1 << 23), /* force ALPM cap on */
+	AHCI_HFLAG_NO_WRITE_TO_RO	= (1 << 24), /* don't write to read
+							only registers */
 
 	/* ap->flags bits */
 
diff --git a/drivers/ata/ahci_brcm.c b/drivers/ata/ahci_brcm.c
index 6f8a7341fa08..5936d1679bf3 100644
--- a/drivers/ata/ahci_brcm.c
+++ b/drivers/ata/ahci_brcm.c
@@ -39,7 +39,6 @@
  #define PIODATA_ENDIAN_SHIFT				6
   #define ENDIAN_SWAP_NONE				0
   #define ENDIAN_SWAP_FULL				2
- #define OVERRIDE_HWINIT				BIT(16)
 #define SATA_TOP_CTRL_TP_CTRL				0x8
 #define SATA_TOP_CTRL_PHY_CTRL				0xc
  #define SATA_TOP_CTRL_PHY_CTRL_1			0x0
@@ -126,17 +125,13 @@ static inline void brcm_sata_writereg(u32 val, void __iomem *addr)
 static void brcm_sata_alpm_init(struct ahci_host_priv *hpriv)
 {
 	struct brcm_ahci_priv *priv = hpriv->plat_data;
-	u32 bus_ctrl, port_ctrl, host_caps;
+	u32 port_ctrl, host_caps;
 	int i;
 
 	/* Enable support for ALPM */
-	bus_ctrl = brcm_sata_readreg(priv->top_ctrl +
-				     SATA_TOP_CTRL_BUS_CTRL);
-	brcm_sata_writereg(bus_ctrl | OVERRIDE_HWINIT,
-			   priv->top_ctrl + SATA_TOP_CTRL_BUS_CTRL);
 	host_caps = readl(hpriv->mmio + HOST_CAP);
-	writel(host_caps | HOST_CAP_ALPM, hpriv->mmio);
-	brcm_sata_writereg(bus_ctrl, priv->top_ctrl + SATA_TOP_CTRL_BUS_CTRL);
+	if (!(host_caps & HOST_CAP_ALPM))
+		hpriv->flags |= AHCI_HFLAG_YES_ALPM;
 
 	/*
 	 * Adjust timeout to allow PLL sufficient time to lock while waking
@@ -360,6 +355,7 @@ static int brcm_ahci_probe(struct platform_device *pdev)
 
 	if (priv->quirks & BRCM_AHCI_QUIRK_NO_NCQ)
 		hpriv->flags |= AHCI_HFLAG_NO_NCQ;
+	hpriv->flags |= AHCI_HFLAG_NO_WRITE_TO_RO;
 
 	ret = ahci_platform_init_host(pdev, hpriv, &ahci_brcm_port_info,
 				      &ahci_platform_sht);
diff --git a/drivers/ata/ahci_qoriq.c b/drivers/ata/ahci_qoriq.c
index 4c96f3ac4976..b6b0bf76dfc7 100644
--- a/drivers/ata/ahci_qoriq.c
+++ b/drivers/ata/ahci_qoriq.c
@@ -47,12 +47,14 @@
 
 #define SATA_ECC_DISABLE	0x00020000
 #define ECC_DIS_ARMV8_CH2	0x80000000
+#define ECC_DIS_LS1088A		0x40000000
 
 enum ahci_qoriq_type {
 	AHCI_LS1021A,
 	AHCI_LS1043A,
 	AHCI_LS2080A,
 	AHCI_LS1046A,
+	AHCI_LS1088A,
 	AHCI_LS2088A,
 };
 
@@ -68,6 +70,7 @@ static const struct of_device_id ahci_qoriq_of_match[] = {
 	{ .compatible = "fsl,ls1043a-ahci", .data = (void *)AHCI_LS1043A},
 	{ .compatible = "fsl,ls2080a-ahci", .data = (void *)AHCI_LS2080A},
 	{ .compatible = "fsl,ls1046a-ahci", .data = (void *)AHCI_LS1046A},
+	{ .compatible = "fsl,ls1088a-ahci", .data = (void *)AHCI_LS1088A},
 	{ .compatible = "fsl,ls2088a-ahci", .data = (void *)AHCI_LS2088A},
 	{},
 };
@@ -203,6 +206,17 @@ static int ahci_qoriq_phy_init(struct ahci_host_priv *hpriv)
 			writel(AHCI_PORT_AXICC_CFG, reg_base + PORT_AXICC);
 		break;
 
+	case AHCI_LS1088A:
+		if (!qpriv->ecc_addr)
+			return -EINVAL;
+		writel(readl(qpriv->ecc_addr) | ECC_DIS_LS1088A,
+		       qpriv->ecc_addr);
+		writel(AHCI_PORT_PHY_1_CFG, reg_base + PORT_PHY1);
+		writel(AHCI_PORT_TRANS_CFG, reg_base + PORT_TRANS);
+		if (qpriv->is_dmacoherent)
+			writel(AHCI_PORT_AXICC_CFG, reg_base + PORT_AXICC);
+		break;
+
 	case AHCI_LS2088A:
 		writel(AHCI_PORT_PHY_1_CFG, reg_base + PORT_PHY1);
 		writel(AHCI_PORT_TRANS_CFG, reg_base + PORT_TRANS);
diff --git a/drivers/ata/ata_piix.c b/drivers/ata/ata_piix.c
index ffbe625e6fd2..8401c3b5be92 100644
--- a/drivers/ata/ata_piix.c
+++ b/drivers/ata/ata_piix.c
@@ -33,7 +33,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Hardware documentation available at http://developer.intel.com/
  *
diff --git a/drivers/ata/libahci.c b/drivers/ata/libahci.c
index 3159f9e66d8f..3e286d86ab42 100644
--- a/drivers/ata/libahci.c
+++ b/drivers/ata/libahci.c
@@ -24,7 +24,7 @@
  *
  *
  * libata documentation is available via 'make {ps|pdf}docs',
- * as Documentation/DocBook/libata.*
+ * as Documentation/driver-api/libata.rst
  *
  * AHCI hardware documentation:
  * http://www.intel.com/technology/serialata/pdf/rev1_0.pdf
@@ -504,6 +504,11 @@ void ahci_save_initial_config(struct device *dev, struct ahci_host_priv *hpriv)
 		cap &= ~HOST_CAP_FBS;
 	}
 
+	if (!(cap & HOST_CAP_ALPM) && (hpriv->flags & AHCI_HFLAG_YES_ALPM)) {
+		dev_info(dev, "controller can do ALPM, turning on CAP_ALPM\n");
+		cap |= HOST_CAP_ALPM;
+	}
+
 	if (hpriv->force_port_map && port_map != hpriv->force_port_map) {
 		dev_info(dev, "forcing port_map 0x%x -> 0x%x\n",
 			 port_map, hpriv->force_port_map);
@@ -940,7 +945,8 @@ int ahci_reset_controller(struct ata_host *host)
 		/* Some registers might be cleared on reset.  Restore
 		 * initial values.
 		 */
-		ahci_restore_initial_config(host);
+		if (!(hpriv->flags & AHCI_HFLAG_NO_WRITE_TO_RO))
+			ahci_restore_initial_config(host);
 	} else
 		dev_info(host->dev, "skipping global host reset\n");
 
@@ -1400,7 +1406,7 @@ int ahci_do_softreset(struct ata_link *link, unsigned int *class,
 
 	ata_tf_init(link->device, &tf);
 
-	/* issue the first D2H Register FIS */
+	/* issue the first H2D Register FIS */
 	msecs = 0;
 	now = jiffies;
 	if (time_after(deadline, now))
@@ -1417,7 +1423,7 @@ int ahci_do_softreset(struct ata_link *link, unsigned int *class,
 	/* spec says at least 5us, but be generous and sleep for 1ms */
 	ata_msleep(ap, 1);
 
-	/* issue the second D2H Register FIS */
+	/* issue the second H2D Register FIS */
 	tf.ctl &= ~ATA_SRST;
 	ahci_exec_polled_cmd(ap, pmp, &tf, 0, 0, 0);
 
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index e157a0e44419..8453f9a4682f 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -25,7 +25,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Hardware documentation available from http://www.t13.org/ and
  *  http://www.sata-io.org/
@@ -2047,6 +2047,110 @@ retry:
 	return rc;
 }
 
+/**
+ *	ata_read_log_page - read a specific log page
+ *	@dev: target device
+ *	@log: log to read
+ *	@page: page to read
+ *	@buf: buffer to store read page
+ *	@sectors: number of sectors to read
+ *
+ *	Read log page using READ_LOG_EXT command.
+ *
+ *	LOCKING:
+ *	Kernel thread context (may sleep).
+ *
+ *	RETURNS:
+ *	0 on success, AC_ERR_* mask otherwise.
+ */
+unsigned int ata_read_log_page(struct ata_device *dev, u8 log,
+			       u8 page, void *buf, unsigned int sectors)
+{
+	unsigned long ap_flags = dev->link->ap->flags;
+	struct ata_taskfile tf;
+	unsigned int err_mask;
+	bool dma = false;
+
+	DPRINTK("read log page - log 0x%x, page 0x%x\n", log, page);
+
+	/*
+	 * Return error without actually issuing the command on controllers
+	 * which e.g. lockup on a read log page.
+	 */
+	if (ap_flags & ATA_FLAG_NO_LOG_PAGE)
+		return AC_ERR_DEV;
+
+retry:
+	ata_tf_init(dev, &tf);
+	if (dev->dma_mode && ata_id_has_read_log_dma_ext(dev->id) &&
+	    !(dev->horkage & ATA_HORKAGE_NO_NCQ_LOG)) {
+		tf.command = ATA_CMD_READ_LOG_DMA_EXT;
+		tf.protocol = ATA_PROT_DMA;
+		dma = true;
+	} else {
+		tf.command = ATA_CMD_READ_LOG_EXT;
+		tf.protocol = ATA_PROT_PIO;
+		dma = false;
+	}
+	tf.lbal = log;
+	tf.lbam = page;
+	tf.nsect = sectors;
+	tf.hob_nsect = sectors >> 8;
+	tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_LBA48 | ATA_TFLAG_DEVICE;
+
+	err_mask = ata_exec_internal(dev, &tf, NULL, DMA_FROM_DEVICE,
+				     buf, sectors * ATA_SECT_SIZE, 0);
+
+	if (err_mask && dma) {
+		dev->horkage |= ATA_HORKAGE_NO_NCQ_LOG;
+		ata_dev_warn(dev, "READ LOG DMA EXT failed, trying unqueued\n");
+		goto retry;
+	}
+
+	DPRINTK("EXIT, err_mask=%x\n", err_mask);
+	return err_mask;
+}
+
+static bool ata_log_supported(struct ata_device *dev, u8 log)
+{
+	struct ata_port *ap = dev->link->ap;
+
+	if (ata_read_log_page(dev, ATA_LOG_DIRECTORY, 0, ap->sector_buf, 1))
+		return false;
+	return get_unaligned_le16(&ap->sector_buf[log * 2]) ? true : false;
+}
+
+static bool ata_identify_page_supported(struct ata_device *dev, u8 page)
+{
+	struct ata_port *ap = dev->link->ap;
+	unsigned int err, i;
+
+	if (!ata_log_supported(dev, ATA_LOG_IDENTIFY_DEVICE)) {
+		ata_dev_warn(dev, "ATA Identify Device Log not supported\n");
+		return false;
+	}
+
+	/*
+	 * Read IDENTIFY DEVICE data log, page 0, to figure out if the page is
+	 * supported.
+	 */
+	err = ata_read_log_page(dev, ATA_LOG_IDENTIFY_DEVICE, 0, ap->sector_buf,
+				1);
+	if (err) {
+		ata_dev_info(dev,
+			     "failed to get Device Identify Log Emask 0x%x\n",
+			     err);
+		return false;
+	}
+
+	for (i = 0; i < ap->sector_buf[8]; i++) {
+		if (ap->sector_buf[9 + i] == page)
+			return true;
+	}
+
+	return false;
+}
+
 static int ata_do_link_spd_horkage(struct ata_device *dev)
 {
 	struct ata_link *plink = ata_dev_phys_link(dev);
@@ -2094,21 +2198,9 @@ static void ata_dev_config_ncq_send_recv(struct ata_device *dev)
 {
 	struct ata_port *ap = dev->link->ap;
 	unsigned int err_mask;
-	int log_index = ATA_LOG_NCQ_SEND_RECV * 2;
-	u16 log_pages;
 
-	err_mask = ata_read_log_page(dev, ATA_LOG_DIRECTORY,
-				     0, ap->sector_buf, 1);
-	if (err_mask) {
-		ata_dev_dbg(dev,
-			    "failed to get Log Directory Emask 0x%x\n",
-			    err_mask);
-		return;
-	}
-	log_pages = get_unaligned_le16(&ap->sector_buf[log_index]);
-	if (!log_pages) {
-		ata_dev_warn(dev,
-			     "NCQ Send/Recv Log not supported\n");
+	if (!ata_log_supported(dev, ATA_LOG_NCQ_SEND_RECV)) {
+		ata_dev_warn(dev, "NCQ Send/Recv Log not supported\n");
 		return;
 	}
 	err_mask = ata_read_log_page(dev, ATA_LOG_NCQ_SEND_RECV,
@@ -2135,19 +2227,8 @@ static void ata_dev_config_ncq_non_data(struct ata_device *dev)
 {
 	struct ata_port *ap = dev->link->ap;
 	unsigned int err_mask;
-	int log_index = ATA_LOG_NCQ_NON_DATA * 2;
-	u16 log_pages;
 
-	err_mask = ata_read_log_page(dev, ATA_LOG_DIRECTORY,
-				     0, ap->sector_buf, 1);
-	if (err_mask) {
-		ata_dev_dbg(dev,
-			    "failed to get Log Directory Emask 0x%x\n",
-			    err_mask);
-		return;
-	}
-	log_pages = get_unaligned_le16(&ap->sector_buf[log_index]);
-	if (!log_pages) {
+	if (!ata_log_supported(dev, ATA_LOG_NCQ_NON_DATA)) {
 		ata_dev_warn(dev,
 			     "NCQ Send/Recv Log not supported\n");
 		return;
@@ -2176,7 +2257,7 @@ static void ata_dev_config_ncq_prio(struct ata_device *dev)
 	}
 
 	err_mask = ata_read_log_page(dev,
-				     ATA_LOG_SATA_ID_DEV_DATA,
+				     ATA_LOG_IDENTIFY_DEVICE,
 				     ATA_LOG_SATA_SETTINGS,
 				     ap->sector_buf,
 				     1);
@@ -2275,8 +2356,6 @@ static void ata_dev_config_zac(struct ata_device *dev)
 	struct ata_port *ap = dev->link->ap;
 	unsigned int err_mask;
 	u8 *identify_buf = ap->sector_buf;
-	int log_index = ATA_LOG_SATA_ID_DEV_DATA * 2, i, found = 0;
-	u16 log_pages;
 
 	dev->zac_zones_optimal_open = U32_MAX;
 	dev->zac_zones_optimal_nonseq = U32_MAX;
@@ -2296,44 +2375,7 @@ static void ata_dev_config_zac(struct ata_device *dev)
 	if (!(dev->flags & ATA_DFLAG_ZAC))
 		return;
 
-	/*
-	 * Read Log Directory to figure out if IDENTIFY DEVICE log
-	 * is supported.
-	 */
-	err_mask = ata_read_log_page(dev, ATA_LOG_DIRECTORY,
-				     0, ap->sector_buf, 1);
-	if (err_mask) {
-		ata_dev_info(dev,
-			     "failed to get Log Directory Emask 0x%x\n",
-			     err_mask);
-		return;
-	}
-	log_pages = get_unaligned_le16(&ap->sector_buf[log_index]);
-	if (log_pages == 0) {
-		ata_dev_warn(dev,
-			     "ATA Identify Device Log not supported\n");
-		return;
-	}
-	/*
-	 * Read IDENTIFY DEVICE data log, page 0, to figure out
-	 * if page 9 is supported.
-	 */
-	err_mask = ata_read_log_page(dev, ATA_LOG_SATA_ID_DEV_DATA, 0,
-				     identify_buf, 1);
-	if (err_mask) {
-		ata_dev_info(dev,
-			     "failed to get Device Identify Log Emask 0x%x\n",
-			     err_mask);
-		return;
-	}
-	log_pages = identify_buf[8];
-	for (i = 0; i < log_pages; i++) {
-		if (identify_buf[9 + i] == ATA_LOG_ZONED_INFORMATION) {
-			found++;
-			break;
-		}
-	}
-	if (!found) {
+	if (!ata_identify_page_supported(dev, ATA_LOG_ZONED_INFORMATION)) {
 		ata_dev_warn(dev,
 			     "ATA Zoned Information Log not supported\n");
 		return;
@@ -2342,7 +2384,7 @@ static void ata_dev_config_zac(struct ata_device *dev)
 	/*
 	 * Read IDENTIFY DEVICE data log, page 9 (Zoned-device information)
 	 */
-	err_mask = ata_read_log_page(dev, ATA_LOG_SATA_ID_DEV_DATA,
+	err_mask = ata_read_log_page(dev, ATA_LOG_IDENTIFY_DEVICE,
 				     ATA_LOG_ZONED_INFORMATION,
 				     identify_buf, 1);
 	if (!err_mask) {
@@ -2363,6 +2405,37 @@ static void ata_dev_config_zac(struct ata_device *dev)
 	}
 }
 
+static void ata_dev_config_trusted(struct ata_device *dev)
+{
+	struct ata_port *ap = dev->link->ap;
+	u64 trusted_cap;
+	unsigned int err;
+
+	if (!ata_identify_page_supported(dev, ATA_LOG_SECURITY)) {
+		ata_dev_warn(dev,
+			     "Security Log not supported\n");
+		return;
+	}
+
+	err = ata_read_log_page(dev, ATA_LOG_IDENTIFY_DEVICE, ATA_LOG_SECURITY,
+			ap->sector_buf, 1);
+	if (err) {
+		ata_dev_dbg(dev,
+			    "failed to read Security Log, Emask 0x%x\n", err);
+		return;
+	}
+
+	trusted_cap = get_unaligned_le64(&ap->sector_buf[40]);
+	if (!(trusted_cap & (1ULL << 63))) {
+		ata_dev_dbg(dev,
+			    "Trusted Computing capability qword not valid!\n");
+		return;
+	}
+
+	if (trusted_cap & (1 << 0))
+		dev->flags |= ATA_DFLAG_TRUSTED;
+}
+
 /**
  *	ata_dev_configure - Configure the specified ATA/ATAPI device
  *	@dev: Target device to configure
@@ -2571,7 +2644,7 @@ int ata_dev_configure(struct ata_device *dev)
 
 			dev->flags |= ATA_DFLAG_DEVSLP;
 			err_mask = ata_read_log_page(dev,
-						     ATA_LOG_SATA_ID_DEV_DATA,
+						     ATA_LOG_IDENTIFY_DEVICE,
 						     ATA_LOG_SATA_SETTINGS,
 						     sata_setting,
 						     1);
@@ -2587,7 +2660,8 @@ int ata_dev_configure(struct ata_device *dev)
 		}
 		ata_dev_config_sense_reporting(dev);
 		ata_dev_config_zac(dev);
-		dev->cdb_len = 16;
+		ata_dev_config_trusted(dev);
+		dev->cdb_len = 32;
 	}
 
 	/* ATAPI-specific feature tests */
diff --git a/drivers/ata/libata-eh.c b/drivers/ata/libata-eh.c
index ef68232b5222..b70bcf6d2914 100644
--- a/drivers/ata/libata-eh.c
+++ b/drivers/ata/libata-eh.c
@@ -25,7 +25,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Hardware documentation available from http://www.t13.org/ and
  *  http://www.sata-io.org/
@@ -1488,70 +1488,6 @@ static const char *ata_err_string(unsigned int err_mask)
 }
 
 /**
- *	ata_read_log_page - read a specific log page
- *	@dev: target device
- *	@log: log to read
- *	@page: page to read
- *	@buf: buffer to store read page
- *	@sectors: number of sectors to read
- *
- *	Read log page using READ_LOG_EXT command.
- *
- *	LOCKING:
- *	Kernel thread context (may sleep).
- *
- *	RETURNS:
- *	0 on success, AC_ERR_* mask otherwise.
- */
-unsigned int ata_read_log_page(struct ata_device *dev, u8 log,
-			       u8 page, void *buf, unsigned int sectors)
-{
-	unsigned long ap_flags = dev->link->ap->flags;
-	struct ata_taskfile tf;
-	unsigned int err_mask;
-	bool dma = false;
-
-	DPRINTK("read log page - log 0x%x, page 0x%x\n", log, page);
-
-	/*
-	 * Return error without actually issuing the command on controllers
-	 * which e.g. lockup on a read log page.
-	 */
-	if (ap_flags & ATA_FLAG_NO_LOG_PAGE)
-		return AC_ERR_DEV;
-
-retry:
-	ata_tf_init(dev, &tf);
-	if (dev->dma_mode && ata_id_has_read_log_dma_ext(dev->id) &&
-	    !(dev->horkage & ATA_HORKAGE_NO_NCQ_LOG)) {
-		tf.command = ATA_CMD_READ_LOG_DMA_EXT;
-		tf.protocol = ATA_PROT_DMA;
-		dma = true;
-	} else {
-		tf.command = ATA_CMD_READ_LOG_EXT;
-		tf.protocol = ATA_PROT_PIO;
-		dma = false;
-	}
-	tf.lbal = log;
-	tf.lbam = page;
-	tf.nsect = sectors;
-	tf.hob_nsect = sectors >> 8;
-	tf.flags |= ATA_TFLAG_ISADDR | ATA_TFLAG_LBA48 | ATA_TFLAG_DEVICE;
-
-	err_mask = ata_exec_internal(dev, &tf, NULL, DMA_FROM_DEVICE,
-				     buf, sectors * ATA_SECT_SIZE, 0);
-
-	if (err_mask && dma) {
-		dev->horkage |= ATA_HORKAGE_NO_NCQ_LOG;
-		ata_dev_warn(dev, "READ LOG DMA EXT failed, trying unqueued\n");
-		goto retry;
-	}
-
-	DPRINTK("EXIT, err_mask=%x\n", err_mask);
-	return err_mask;
-}
-
-/**
  *	ata_eh_read_log_10h - Read log page 10h for NCQ error details
  *	@dev: Device to read log page 10h from
  *	@tag: Resulting tag of the failed command
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index 49ba9834c715..d462c5a3a7ef 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -25,7 +25,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Hardware documentation available from
  *  - http://www.t10.org/
@@ -903,32 +903,32 @@ static void ata_dump_status(unsigned id, struct ata_taskfile *tf)
 {
 	u8 stat = tf->command, err = tf->feature;
 
-	printk(KERN_WARNING "ata%u: status=0x%02x { ", id, stat);
+	pr_warn("ata%u: status=0x%02x { ", id, stat);
 	if (stat & ATA_BUSY) {
-		printk("Busy }\n");	/* Data is not valid in this case */
+		pr_cont("Busy }\n");	/* Data is not valid in this case */
 	} else {
-		if (stat & ATA_DRDY)	printk("DriveReady ");
-		if (stat & ATA_DF)	printk("DeviceFault ");
-		if (stat & ATA_DSC)	printk("SeekComplete ");
-		if (stat & ATA_DRQ)	printk("DataRequest ");
-		if (stat & ATA_CORR)	printk("CorrectedError ");
-		if (stat & ATA_SENSE)	printk("Sense ");
-		if (stat & ATA_ERR)	printk("Error ");
-		printk("}\n");
+		if (stat & ATA_DRDY)	pr_cont("DriveReady ");
+		if (stat & ATA_DF)	pr_cont("DeviceFault ");
+		if (stat & ATA_DSC)	pr_cont("SeekComplete ");
+		if (stat & ATA_DRQ)	pr_cont("DataRequest ");
+		if (stat & ATA_CORR)	pr_cont("CorrectedError ");
+		if (stat & ATA_SENSE)	pr_cont("Sense ");
+		if (stat & ATA_ERR)	pr_cont("Error ");
+		pr_cont("}\n");
 
 		if (err) {
-			printk(KERN_WARNING "ata%u: error=0x%02x { ", id, err);
-			if (err & ATA_ABORTED)	printk("DriveStatusError ");
+			pr_warn("ata%u: error=0x%02x { ", id, err);
+			if (err & ATA_ABORTED)	pr_cont("DriveStatusError ");
 			if (err & ATA_ICRC) {
 				if (err & ATA_ABORTED)
-						printk("BadCRC ");
-				else		printk("Sector ");
+						pr_cont("BadCRC ");
+				else		pr_cont("Sector ");
 			}
-			if (err & ATA_UNC)	printk("UncorrectableError ");
-			if (err & ATA_IDNF)	printk("SectorIdNotFound ");
-			if (err & ATA_TRK0NF)	printk("TrackZeroNotFound ");
-			if (err & ATA_AMNF)	printk("AddrMarkNotFound ");
-			printk("}\n");
+			if (err & ATA_UNC)	pr_cont("UncorrectableError ");
+			if (err & ATA_IDNF)	pr_cont("SectorIdNotFound ");
+			if (err & ATA_TRK0NF)	pr_cont("TrackZeroNotFound ");
+			if (err & ATA_AMNF)	pr_cont("AddrMarkNotFound ");
+			pr_cont("}\n");
 		}
 	}
 }
@@ -1059,8 +1059,7 @@ static void ata_to_sense_error(unsigned id, u8 drv_stat, u8 drv_err, u8 *sk,
 
  translate_done:
 	if (verbose)
-		printk(KERN_ERR "ata%u: translated ATA stat/err 0x%02x/%02x "
-		       "to SCSI SK/ASC/ASCQ 0x%x/%02x/%02x\n",
+		pr_err("ata%u: translated ATA stat/err 0x%02x/%02x to SCSI SK/ASC/ASCQ 0x%x/%02x/%02x\n",
 		       id, drv_stat, drv_err, *sk, *asc, *ascq);
 	return;
 }
@@ -1322,6 +1321,9 @@ static int ata_scsi_dev_config(struct scsi_device *sdev,
 
 	blk_queue_flush_queueable(q, false);
 
+	if (dev->flags & ATA_DFLAG_TRUSTED)
+		sdev->security_supported = 1;
+
 	dev->sdev = sdev;
 	return 0;
 }
@@ -3127,7 +3129,7 @@ ata_scsi_map_proto(u8 byte1)
  *	ata_scsi_pass_thru - convert ATA pass-thru CDB to taskfile
  *	@qc: command structure to be initialized
  *
- *	Handles either 12 or 16-byte versions of the CDB.
+ *	Handles either 12, 16, or 32-byte versions of the CDB.
  *
  *	RETURNS:
  *	Zero on success, non-zero on failure.
@@ -3139,13 +3141,19 @@ static unsigned int ata_scsi_pass_thru(struct ata_queued_cmd *qc)
 	struct ata_device *dev = qc->dev;
 	const u8 *cdb = scmd->cmnd;
 	u16 fp;
+	u16 cdb_offset = 0;
+
+	/* 7Fh variable length cmd means a ata pass-thru(32) */
+	if (cdb[0] == VARIABLE_LENGTH_CMD)
+		cdb_offset = 9;
 
-	if ((tf->protocol = ata_scsi_map_proto(cdb[1])) == ATA_PROT_UNKNOWN) {
+	tf->protocol = ata_scsi_map_proto(cdb[1 + cdb_offset]);
+	if (tf->protocol == ATA_PROT_UNKNOWN) {
 		fp = 1;
 		goto invalid_fld;
 	}
 
-	if (ata_is_ncq(tf->protocol) && (cdb[2] & 0x3) == 0)
+	if (ata_is_ncq(tf->protocol) && (cdb[2 + cdb_offset] & 0x3) == 0)
 		tf->protocol = ATA_PROT_NCQ_NODATA;
 
 	/* enable LBA */
@@ -3181,7 +3189,7 @@ static unsigned int ata_scsi_pass_thru(struct ata_queued_cmd *qc)
 		tf->lbah = cdb[12];
 		tf->device = cdb[13];
 		tf->command = cdb[14];
-	} else {
+	} else if (cdb[0] == ATA_12) {
 		/*
 		 * 12-byte CDB - incapable of extended commands.
 		 */
@@ -3194,6 +3202,30 @@ static unsigned int ata_scsi_pass_thru(struct ata_queued_cmd *qc)
 		tf->lbah = cdb[7];
 		tf->device = cdb[8];
 		tf->command = cdb[9];
+	} else {
+		/*
+		 * 32-byte CDB - may contain extended command fields.
+		 *
+		 * If that is the case, copy the upper byte register values.
+		 */
+		if (cdb[10] & 0x01) {
+			tf->hob_feature = cdb[20];
+			tf->hob_nsect = cdb[22];
+			tf->hob_lbal = cdb[16];
+			tf->hob_lbam = cdb[15];
+			tf->hob_lbah = cdb[14];
+			tf->flags |= ATA_TFLAG_LBA48;
+		} else
+			tf->flags &= ~ATA_TFLAG_LBA48;
+
+		tf->feature = cdb[21];
+		tf->nsect = cdb[23];
+		tf->lbal = cdb[19];
+		tf->lbam = cdb[18];
+		tf->lbah = cdb[17];
+		tf->device = cdb[24];
+		tf->command = cdb[25];
+		tf->auxiliary = get_unaligned_be32(&cdb[28]);
 	}
 
 	/* For NCQ commands copy the tag value */
@@ -3398,9 +3430,10 @@ static size_t ata_format_dsm_trim_descr(struct scsi_cmnd *cmd, u32 trmax,
  *
  * Translate a SCSI WRITE SAME command to be either a DSM TRIM command or
  * an SCT Write Same command.
- * Based on WRITE SAME has the UNMAP flag
- *   When set translate to DSM TRIM
- *   When clear translate to SCT Write Same
+ * Based on WRITE SAME has the UNMAP flag:
+ *
+ *   - When set translate to DSM TRIM
+ *   - When clear translate to SCT Write Same
  */
 static unsigned int ata_scsi_write_same_xlat(struct ata_queued_cmd *qc)
 {
@@ -3563,6 +3596,11 @@ static unsigned int ata_scsiop_maint_in(struct ata_scsi_args *args, u8 *rbuf)
 		    dev->class == ATA_DEV_ZAC)
 			supported = 3;
 		break;
+	case SECURITY_PROTOCOL_IN:
+	case SECURITY_PROTOCOL_OUT:
+		if (dev->flags & ATA_DFLAG_TRUSTED)
+			supported = 3;
+		break;
 	default:
 		break;
 	}
@@ -3909,7 +3947,7 @@ static int ata_mselect_control(struct ata_queued_cmd *qc,
 }
 
 /**
- *	ata_scsiop_mode_select - Simulate MODE SELECT 6, 10 commands
+ *	ata_scsi_mode_select_xlat - Simulate MODE SELECT 6, 10 commands
  *	@qc: Storage for translated ATA taskfile
  *
  *	Converts a MODE SELECT command to an ATA SET FEATURES taskfile.
@@ -4067,6 +4105,99 @@ static unsigned int ata_scsi_mode_select_xlat(struct ata_queued_cmd *qc)
 	return 1;
 }
 
+static u8 ata_scsi_trusted_op(u32 len, bool send, bool dma)
+{
+	if (len == 0)
+		return ATA_CMD_TRUSTED_NONDATA;
+	else if (send)
+		return dma ? ATA_CMD_TRUSTED_SND_DMA : ATA_CMD_TRUSTED_SND;
+	else
+		return dma ? ATA_CMD_TRUSTED_RCV_DMA : ATA_CMD_TRUSTED_RCV;
+}
+
+static unsigned int ata_scsi_security_inout_xlat(struct ata_queued_cmd *qc)
+{
+	struct scsi_cmnd *scmd = qc->scsicmd;
+	const u8 *cdb = scmd->cmnd;
+	struct ata_taskfile *tf = &qc->tf;
+	u8 secp = cdb[1];
+	bool send = (cdb[0] == SECURITY_PROTOCOL_OUT);
+	u16 spsp = get_unaligned_be16(&cdb[2]);
+	u32 len = get_unaligned_be32(&cdb[6]);
+	bool dma = !(qc->dev->flags & ATA_DFLAG_PIO);
+
+	/*
+	 * We don't support the ATA "security" protocol.
+	 */
+	if (secp == 0xef) {
+		ata_scsi_set_invalid_field(qc->dev, scmd, 1, 0);
+		return 1;
+	}
+
+	if (cdb[4] & 7) { /* INC_512 */
+		if (len > 0xffff) {
+			ata_scsi_set_invalid_field(qc->dev, scmd, 6, 0);
+			return 1;
+		}
+	} else {
+		if (len > 0x01fffe00) {
+			ata_scsi_set_invalid_field(qc->dev, scmd, 6, 0);
+			return 1;
+		}
+
+		/* convert to the sector-based ATA addressing */
+		len = (len + 511) / 512;
+	}
+
+	tf->protocol = dma ? ATA_PROT_DMA : ATA_PROT_PIO;
+	tf->flags |= ATA_TFLAG_DEVICE | ATA_TFLAG_ISADDR | ATA_TFLAG_LBA;
+	if (send)
+		tf->flags |= ATA_TFLAG_WRITE;
+	tf->command = ata_scsi_trusted_op(len, send, dma);
+	tf->feature = secp;
+	tf->lbam = spsp & 0xff;
+	tf->lbah = spsp >> 8;
+
+	if (len) {
+		tf->nsect = len & 0xff;
+		tf->lbal = len >> 8;
+	} else {
+		if (!send)
+			tf->lbah = (1 << 7);
+	}
+
+	ata_qc_set_pc_nbytes(qc);
+	return 0;
+}
+
+/**
+ *	ata_scsi_var_len_cdb_xlat - SATL variable length CDB to Handler
+ *	@qc: Command to be translated
+ *
+ *	Translate a SCSI variable length CDB to specified commands.
+ *	It checks a service action value in CDB to call corresponding handler.
+ *
+ *	RETURNS:
+ *	Zero on success, non-zero on failure
+ *
+ */
+static unsigned int ata_scsi_var_len_cdb_xlat(struct ata_queued_cmd *qc)
+{
+	struct scsi_cmnd *scmd = qc->scsicmd;
+	const u8 *cdb = scmd->cmnd;
+	const u16 sa = get_unaligned_be16(&cdb[8]);
+
+	/*
+	 * if service action represents a ata pass-thru(32) command,
+	 * then pass it to ata_scsi_pass_thru handler.
+	 */
+	if (sa == ATA_32)
+		return ata_scsi_pass_thru(qc);
+
+	/* unsupported service action */
+	return 1;
+}
+
 /**
  *	ata_get_xlat_func - check if SCSI to ATA translation is possible
  *	@dev: ATA device
@@ -4107,6 +4238,9 @@ static inline ata_xlat_func_t ata_get_xlat_func(struct ata_device *dev, u8 cmd)
 	case ATA_16:
 		return ata_scsi_pass_thru;
 
+	case VARIABLE_LENGTH_CMD:
+		return ata_scsi_var_len_cdb_xlat;
+
 	case MODE_SELECT:
 	case MODE_SELECT_10:
 		return ata_scsi_mode_select_xlat;
@@ -4118,6 +4252,12 @@ static inline ata_xlat_func_t ata_get_xlat_func(struct ata_device *dev, u8 cmd)
 	case ZBC_OUT:
 		return ata_scsi_zbc_out_xlat;
 
+	case SECURITY_PROTOCOL_IN:
+	case SECURITY_PROTOCOL_OUT:
+		if (!(dev->flags & ATA_DFLAG_TRUSTED))
+			break;
+		return ata_scsi_security_inout_xlat;
+
 	case START_STOP:
 		return ata_scsi_start_stop_xlat;
 	}
@@ -4385,7 +4525,7 @@ int ata_scsi_add_hosts(struct ata_host *host, struct scsi_host_template *sht)
 		shost->max_id = 16;
 		shost->max_lun = 1;
 		shost->max_channel = 1;
-		shost->max_cmd_len = 16;
+		shost->max_cmd_len = 32;
 
 		/* Schedule policy is determined by ->qc_defer()
 		 * callback and it needs to see every deferred qc.
diff --git a/drivers/ata/libata-sff.c b/drivers/ata/libata-sff.c
index 274d6d7193d7..cc2f2e35f4c2 100644
--- a/drivers/ata/libata-sff.c
+++ b/drivers/ata/libata-sff.c
@@ -25,7 +25,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Hardware documentation available from http://www.t13.org/ and
  *  http://www.sata-io.org/
@@ -716,24 +716,10 @@ static void ata_pio_sector(struct ata_queued_cmd *qc)
 
 	DPRINTK("data %s\n", qc->tf.flags & ATA_TFLAG_WRITE ? "write" : "read");
 
-	if (PageHighMem(page)) {
-		unsigned long flags;
-
-		/* FIXME: use a bounce buffer */
-		local_irq_save(flags);
-		buf = kmap_atomic(page);
-
-		/* do the actual data transfer */
-		ap->ops->sff_data_xfer(qc, buf + offset, qc->sect_size,
-				       do_write);
-
-		kunmap_atomic(buf);
-		local_irq_restore(flags);
-	} else {
-		buf = page_address(page);
-		ap->ops->sff_data_xfer(qc, buf + offset, qc->sect_size,
-				       do_write);
-	}
+	/* do the actual data transfer */
+	buf = kmap_atomic(page);
+	ap->ops->sff_data_xfer(qc, buf + offset, qc->sect_size, do_write);
+	kunmap_atomic(buf);
 
 	if (!do_write && !PageSlab(page))
 		flush_dcache_page(page);
@@ -861,24 +847,10 @@ next_sg:
 
 	DPRINTK("data %s\n", qc->tf.flags & ATA_TFLAG_WRITE ? "write" : "read");
 
-	if (PageHighMem(page)) {
-		unsigned long flags;
-
-		/* FIXME: use bounce buffer */
-		local_irq_save(flags);
-		buf = kmap_atomic(page);
-
-		/* do the actual data transfer */
-		consumed = ap->ops->sff_data_xfer(qc, buf + offset,
-								count, rw);
-
-		kunmap_atomic(buf);
-		local_irq_restore(flags);
-	} else {
-		buf = page_address(page);
-		consumed = ap->ops->sff_data_xfer(qc, buf + offset,
-								count, rw);
-	}
+	/* do the actual data transfer */
+	buf = kmap_atomic(page);
+	consumed = ap->ops->sff_data_xfer(qc, buf + offset, count, rw);
+	kunmap_atomic(buf);
 
 	bytes -= min(bytes, consumed);
 	qc->curbytes += count;
diff --git a/drivers/ata/libata-zpodd.c b/drivers/ata/libata-zpodd.c
index f3a65a3140d3..8a01d09ac4db 100644
--- a/drivers/ata/libata-zpodd.c
+++ b/drivers/ata/libata-zpodd.c
@@ -174,8 +174,7 @@ void zpodd_enable_run_wake(struct ata_device *dev)
 	sdev_disable_disk_events(dev->sdev);
 
 	zpodd->powered_off = true;
-	device_set_run_wake(&dev->tdev, true);
-	acpi_pm_device_run_wake(&dev->tdev, true);
+	acpi_pm_set_device_wakeup(&dev->tdev, true);
 }
 
 /* Disable runtime wake capability if it is enabled */
@@ -183,10 +182,8 @@ void zpodd_disable_run_wake(struct ata_device *dev)
 {
 	struct zpodd *zpodd = dev->zpodd;
 
-	if (zpodd->powered_off) {
-		acpi_pm_device_run_wake(&dev->tdev, false);
-		device_set_run_wake(&dev->tdev, false);
-	}
+	if (zpodd->powered_off)
+		acpi_pm_set_device_wakeup(&dev->tdev, false);
 }
 
 /*
diff --git a/drivers/ata/libata.h b/drivers/ata/libata.h
index 120fce0befd3..839d487394b7 100644
--- a/drivers/ata/libata.h
+++ b/drivers/ata/libata.h
@@ -21,7 +21,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  */
 
@@ -98,6 +98,8 @@ extern struct ata_port *ata_port_alloc(struct ata_host *host);
 extern const char *sata_spd_string(unsigned int spd);
 extern int ata_port_probe(struct ata_port *ap);
 extern void __ata_port_probe(struct ata_port *ap);
+extern unsigned int ata_read_log_page(struct ata_device *dev, u8 log,
+				      u8 page, void *buf, unsigned int sectors);
 
 #define to_ata_port(d) container_of(d, struct ata_port, tdev)
 
@@ -160,8 +162,6 @@ extern void ata_eh_about_to_do(struct ata_link *link, struct ata_device *dev,
 			       unsigned int action);
 extern void ata_eh_done(struct ata_link *link, struct ata_device *dev,
 			unsigned int action);
-extern unsigned int ata_read_log_page(struct ata_device *dev, u8 log,
-				      u8 page, void *buf, unsigned int sectors);
 extern void ata_eh_autopsy(struct ata_port *ap);
 const char *ata_get_cmd_descript(u8 command);
 extern void ata_eh_report(struct ata_port *ap);
diff --git a/drivers/ata/pata_bf54x.c b/drivers/ata/pata_bf54x.c
index 9c5780a7e1b9..0e55a8da2748 100644
--- a/drivers/ata/pata_bf54x.c
+++ b/drivers/ata/pata_bf54x.c
@@ -1597,8 +1597,6 @@ static int bfin_atapi_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	platform_set_drvdata(pdev, host);
-
 	return 0;
 }
 
diff --git a/drivers/ata/pata_ep93xx.c b/drivers/ata/pata_ep93xx.c
index bf1b910c5d69..0a550190955a 100644
--- a/drivers/ata/pata_ep93xx.c
+++ b/drivers/ata/pata_ep93xx.c
@@ -944,7 +944,6 @@ static int ep93xx_pata_probe(struct platform_device *pdev)
 		goto err_rel_gpio;
 	}
 
-	platform_set_drvdata(pdev, drv_data);
 	drv_data->pdev = pdev;
 	drv_data->ide_base = ide_base;
 	drv_data->udma_in_phys = mem_res->start + IDEUDMADATAIN;
diff --git a/drivers/ata/pata_ftide010.c b/drivers/ata/pata_ftide010.c
new file mode 100644
index 000000000000..5d4b72e21161
--- /dev/null
+++ b/drivers/ata/pata_ftide010.c
@@ -0,0 +1,567 @@
+/*
+ * Faraday Technology FTIDE010 driver
+ * Copyright (C) 2017 Linus Walleij <linus.walleij@linaro.org>
+ *
+ * Includes portions of the SL2312/SL3516/Gemini PATA driver
+ * Copyright (C) 2003 StorLine, Inc <jason@storlink.com.tw>
+ * Copyright (C) 2009 Janos Laube <janos.dev@gmail.com>
+ * Copyright (C) 2010 Frederic Pecourt <opengemini@free.fr>
+ * Copyright (C) 2011 Tobias Waldvogel <tobias.waldvogel@gmail.com>
+ */
+
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/libata.h>
+#include <linux/bitops.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/clk.h>
+#include "sata_gemini.h"
+
+#define DRV_NAME "pata_ftide010"
+
+/**
+ * struct ftide010 - state container for the Faraday FTIDE010
+ * @dev: pointer back to the device representing this controller
+ * @base: remapped I/O space address
+ * @pclk: peripheral clock for the IDE block
+ * @host: pointer to the ATA host for this device
+ * @master_cbl: master cable type
+ * @slave_cbl: slave cable type
+ * @sg: Gemini SATA bridge pointer, if running on the Gemini
+ * @master_to_sata0: Gemini SATA bridge: the ATA master is connected
+ * to the SATA0 bridge
+ * @slave_to_sata0: Gemini SATA bridge: the ATA slave is connected
+ * to the SATA0 bridge
+ * @master_to_sata1: Gemini SATA bridge: the ATA master is connected
+ * to the SATA1 bridge
+ * @slave_to_sata1: Gemini SATA bridge: the ATA slave is connected
+ * to the SATA1 bridge
+ */
+struct ftide010 {
+	struct device *dev;
+	void __iomem *base;
+	struct clk *pclk;
+	struct ata_host *host;
+	unsigned int master_cbl;
+	unsigned int slave_cbl;
+	/* Gemini-specific properties */
+	struct sata_gemini *sg;
+	bool master_to_sata0;
+	bool slave_to_sata0;
+	bool master_to_sata1;
+	bool slave_to_sata1;
+};
+
+#define FTIDE010_DMA_REG	0x00
+#define FTIDE010_DMA_STATUS	0x02
+#define FTIDE010_IDE_BMDTPR	0x04
+#define FTIDE010_IDE_DEVICE_ID	0x08
+#define FTIDE010_PIO_TIMING	0x10
+#define FTIDE010_MWDMA_TIMING	0x11
+#define FTIDE010_UDMA_TIMING0	0x12 /* Master */
+#define FTIDE010_UDMA_TIMING1	0x13 /* Slave */
+#define FTIDE010_CLK_MOD	0x14
+/* These registers are mapped directly to the IDE registers */
+#define FTIDE010_CMD_DATA	0x20
+#define FTIDE010_ERROR_FEATURES	0x21
+#define FTIDE010_NSECT		0x22
+#define FTIDE010_LBAL		0x23
+#define FTIDE010_LBAM		0x24
+#define FTIDE010_LBAH		0x25
+#define FTIDE010_DEVICE		0x26
+#define FTIDE010_STATUS_COMMAND	0x27
+#define FTIDE010_ALTSTAT_CTRL	0x36
+
+/* Set this bit for UDMA mode 5 and 6 */
+#define FTIDE010_UDMA_TIMING_MODE_56	BIT(7)
+
+/* 0 = 50 MHz, 1 = 66 MHz */
+#define FTIDE010_CLK_MOD_DEV0_CLK_SEL	BIT(0)
+#define FTIDE010_CLK_MOD_DEV1_CLK_SEL	BIT(1)
+/* Enable UDMA on a device */
+#define FTIDE010_CLK_MOD_DEV0_UDMA_EN	BIT(4)
+#define FTIDE010_CLK_MOD_DEV1_UDMA_EN	BIT(5)
+
+static struct scsi_host_template pata_ftide010_sht = {
+	ATA_BMDMA_SHT(DRV_NAME),
+};
+
+/*
+ * Bus timings
+ *
+ * The unit of the below required timings is two clock periods of the ATA
+ * reference clock which is 30 nanoseconds per unit at 66MHz and 20
+ * nanoseconds per unit at 50 MHz. The PIO timings assume 33MHz speed for
+ * PIO.
+ *
+ * pio_active_time: array of 5 elements for T2 timing for Mode 0,
+ * 1, 2, 3 and 4. Range 0..15.
+ * pio_recovery_time: array of 5 elements for T2l timing for Mode 0,
+ * 1, 2, 3 and 4. Range 0..15.
+ * mdma_50_active_time: array of 4 elements for Td timing for multi
+ * word DMA, Mode 0, 1, and 2 at 50 MHz. Range 0..15.
+ * mdma_50_recovery_time: array of 4 elements for Tk timing for
+ * multi word DMA, Mode 0, 1 and 2 at 50 MHz. Range 0..15.
+ * mdma_66_active_time: array of 4 elements for Td timing for multi
+ * word DMA, Mode 0, 1 and 2 at 66 MHz. Range 0..15.
+ * mdma_66_recovery_time: array of 4 elements for Tk timing for
+ * multi word DMA, Mode 0, 1 and 2 at 66 MHz. Range 0..15.
+ * udma_50_setup_time: array of 4 elements for Tvds timing for ultra
+ * DMA, Mode 0, 1, 2, 3, 4 and 5 at 50 MHz. Range 0..7.
+ * udma_50_hold_time: array of 4 elements for Tdvh timing for
+ * multi word DMA, Mode 0, 1, 2, 3, 4 and 5 at 50 MHz, Range 0..7.
+ * udma_66_setup_time: array of 4 elements for Tvds timing for multi
+ * word DMA, Mode 0, 1, 2, 3, 4, 5 and 6 at 66 MHz. Range 0..7.
+ * udma_66_hold_time: array of 4 elements for Tdvh timing for
+ * multi word DMA, Mode 0, 1, 2, 3, 4, 5 and 6 at 66 MHz. Range 0..7.
+ */
+static const u8 pio_active_time[5] = {10, 10, 10, 3, 3};
+static const u8 pio_recovery_time[5] = {10, 3, 1, 3, 1};
+static const u8 mwdma_50_active_time[3] = {6, 2, 2};
+static const u8 mwdma_50_recovery_time[3] = {6, 2, 1};
+static const u8 mwdma_66_active_time[3] = {8, 3, 3};
+static const u8 mwdma_66_recovery_time[3] = {8, 2, 1};
+static const u8 udma_50_setup_time[6] = {3, 3, 2, 2, 1, 1};
+static const u8 udma_50_hold_time[6] = {3, 1, 1, 1, 1, 1};
+static const u8 udma_66_setup_time[7] = {4, 4, 3, 2, };
+static const u8 udma_66_hold_time[7] = {};
+
+/*
+ * We set 66 MHz for all MWDMA modes
+ */
+static const bool set_mdma_66_mhz[] = { true, true, true, true };
+
+/*
+ * We set 66 MHz for UDMA modes 3, 4 and 6 and no others
+ */
+static const bool set_udma_66_mhz[] = { false, false, false, true, true, false, true };
+
+static void ftide010_set_dmamode(struct ata_port *ap, struct ata_device *adev)
+{
+	struct ftide010 *ftide = ap->host->private_data;
+	u8 speed = adev->dma_mode;
+	u8 devno = adev->devno & 1;
+	u8 udma_en_mask;
+	u8 f66m_en_mask;
+	u8 clkreg;
+	u8 timreg;
+	u8 i;
+
+	/* Target device 0 (master) or 1 (slave) */
+	if (!devno) {
+		udma_en_mask = FTIDE010_CLK_MOD_DEV0_UDMA_EN;
+		f66m_en_mask = FTIDE010_CLK_MOD_DEV0_CLK_SEL;
+	} else {
+		udma_en_mask = FTIDE010_CLK_MOD_DEV1_UDMA_EN;
+		f66m_en_mask = FTIDE010_CLK_MOD_DEV1_CLK_SEL;
+	}
+
+	clkreg = readb(ftide->base + FTIDE010_CLK_MOD);
+	clkreg &= ~udma_en_mask;
+	clkreg &= ~f66m_en_mask;
+
+	if (speed & XFER_UDMA_0) {
+		i = speed & ~XFER_UDMA_0;
+		dev_dbg(ftide->dev, "set UDMA mode %02x, index %d\n",
+			speed, i);
+
+		clkreg |= udma_en_mask;
+		if (set_udma_66_mhz[i]) {
+			clkreg |= f66m_en_mask;
+			timreg = udma_66_setup_time[i] << 4 |
+				udma_66_hold_time[i];
+		} else {
+			timreg = udma_50_setup_time[i] << 4 |
+				udma_50_hold_time[i];
+		}
+
+		/* A special bit needs to be set for modes 5 and 6 */
+		if (i >= 5)
+			timreg |= FTIDE010_UDMA_TIMING_MODE_56;
+
+		dev_dbg(ftide->dev, "UDMA write clkreg = %02x, timreg = %02x\n",
+			clkreg, timreg);
+
+		writeb(clkreg, ftide->base + FTIDE010_CLK_MOD);
+		writeb(timreg, ftide->base + FTIDE010_UDMA_TIMING0 + devno);
+	} else {
+		i = speed & ~XFER_MW_DMA_0;
+		dev_dbg(ftide->dev, "set MWDMA mode %02x, index %d\n",
+			speed, i);
+
+		if (set_mdma_66_mhz[i]) {
+			clkreg |= f66m_en_mask;
+			timreg = mwdma_66_active_time[i] << 4 |
+				mwdma_66_recovery_time[i];
+		} else {
+			timreg = mwdma_50_active_time[i] << 4 |
+				mwdma_50_recovery_time[i];
+		}
+		dev_dbg(ftide->dev,
+			"MWDMA write clkreg = %02x, timreg = %02x\n",
+			clkreg, timreg);
+		/* This will affect all devices */
+		writeb(clkreg, ftide->base + FTIDE010_CLK_MOD);
+		writeb(timreg, ftide->base + FTIDE010_MWDMA_TIMING);
+	}
+
+	/*
+	 * Store the current device (master or slave) in ap->private_data
+	 * so that .qc_issue() can detect if this changes and reprogram
+	 * the DMA settings.
+	 */
+	ap->private_data = adev;
+
+	return;
+}
+
+static void ftide010_set_piomode(struct ata_port *ap, struct ata_device *adev)
+{
+	struct ftide010 *ftide = ap->host->private_data;
+	u8 pio = adev->pio_mode - XFER_PIO_0;
+
+	dev_dbg(ftide->dev, "set PIO mode %02x, index %d\n",
+		adev->pio_mode, pio);
+	writeb(pio_active_time[pio] << 4 | pio_recovery_time[pio],
+	       ftide->base + FTIDE010_PIO_TIMING);
+}
+
+/*
+ * We implement our own qc_issue() callback since we may need to set up
+ * the timings differently for master and slave transfers: the CLK_MOD_REG
+ * and MWDMA_TIMING_REG is shared between master and slave, so reprogramming
+ * this may be necessary.
+ */
+static unsigned int ftide010_qc_issue(struct ata_queued_cmd *qc)
+{
+	struct ata_port *ap = qc->ap;
+	struct ata_device *adev = qc->dev;
+
+	/*
+	 * If the device changed, i.e. slave->master, master->slave,
+	 * then set up the DMA mode again so we are sure the timings
+	 * are correct.
+	 */
+	if (adev != ap->private_data && ata_dma_enabled(adev))
+		ftide010_set_dmamode(ap, adev);
+
+	return ata_bmdma_qc_issue(qc);
+}
+
+static struct ata_port_operations pata_ftide010_port_ops = {
+	.inherits	= &ata_bmdma_port_ops,
+	.set_dmamode	= ftide010_set_dmamode,
+	.set_piomode	= ftide010_set_piomode,
+	.qc_issue	= ftide010_qc_issue,
+};
+
+static struct ata_port_info ftide010_port_info[] = {
+	{
+		.flags		= ATA_FLAG_SLAVE_POSS,
+		.mwdma_mask	= ATA_MWDMA2,
+		.udma_mask	= ATA_UDMA6,
+		.pio_mask	= ATA_PIO4,
+		.port_ops	= &pata_ftide010_port_ops,
+	},
+};
+
+#if IS_ENABLED(CONFIG_SATA_GEMINI)
+
+static int pata_ftide010_gemini_port_start(struct ata_port *ap)
+{
+	struct ftide010 *ftide = ap->host->private_data;
+	struct device *dev = ftide->dev;
+	struct sata_gemini *sg = ftide->sg;
+	int bridges = 0;
+	int ret;
+
+	ret = ata_bmdma_port_start(ap);
+	if (ret)
+		return ret;
+
+	if (ftide->master_to_sata0) {
+		dev_info(dev, "SATA0 (master) start\n");
+		ret = gemini_sata_start_bridge(sg, 0);
+		if (!ret)
+			bridges++;
+	}
+	if (ftide->master_to_sata1) {
+		dev_info(dev, "SATA1 (master) start\n");
+		ret = gemini_sata_start_bridge(sg, 1);
+		if (!ret)
+			bridges++;
+	}
+	/* Avoid double-starting */
+	if (ftide->slave_to_sata0 && !ftide->master_to_sata0) {
+		dev_info(dev, "SATA0 (slave) start\n");
+		ret = gemini_sata_start_bridge(sg, 0);
+		if (!ret)
+			bridges++;
+	}
+	/* Avoid double-starting */
+	if (ftide->slave_to_sata1 && !ftide->master_to_sata1) {
+		dev_info(dev, "SATA1 (slave) start\n");
+		ret = gemini_sata_start_bridge(sg, 1);
+		if (!ret)
+			bridges++;
+	}
+
+	dev_info(dev, "brought %d bridges online\n", bridges);
+	return (bridges > 0) ? 0 : -EINVAL; // -ENODEV;
+}
+
+static void pata_ftide010_gemini_port_stop(struct ata_port *ap)
+{
+	struct ftide010 *ftide = ap->host->private_data;
+	struct device *dev = ftide->dev;
+	struct sata_gemini *sg = ftide->sg;
+
+	if (ftide->master_to_sata0) {
+		dev_info(dev, "SATA0 (master) stop\n");
+		gemini_sata_stop_bridge(sg, 0);
+	}
+	if (ftide->master_to_sata1) {
+		dev_info(dev, "SATA1 (master) stop\n");
+		gemini_sata_stop_bridge(sg, 1);
+	}
+	/* Avoid double-stopping */
+	if (ftide->slave_to_sata0 && !ftide->master_to_sata0) {
+		dev_info(dev, "SATA0 (slave) stop\n");
+		gemini_sata_stop_bridge(sg, 0);
+	}
+	/* Avoid double-stopping */
+	if (ftide->slave_to_sata1 && !ftide->master_to_sata1) {
+		dev_info(dev, "SATA1 (slave) stop\n");
+		gemini_sata_stop_bridge(sg, 1);
+	}
+}
+
+static int pata_ftide010_gemini_cable_detect(struct ata_port *ap)
+{
+	struct ftide010 *ftide = ap->host->private_data;
+
+	/*
+	 * Return the master cable, I have no clue how to return a different
+	 * cable for the slave than for the master.
+	 */
+	return ftide->master_cbl;
+}
+
+static int pata_ftide010_gemini_init(struct ftide010 *ftide,
+				     bool is_ata1)
+{
+	struct device *dev = ftide->dev;
+	struct sata_gemini *sg;
+	enum gemini_muxmode muxmode;
+
+	/* Look up SATA bridge */
+	sg = gemini_sata_bridge_get();
+	if (IS_ERR(sg))
+		return PTR_ERR(sg);
+	ftide->sg = sg;
+
+	muxmode = gemini_sata_get_muxmode(sg);
+
+	/* Special ops */
+	pata_ftide010_port_ops.port_start =
+		pata_ftide010_gemini_port_start;
+	pata_ftide010_port_ops.port_stop =
+		pata_ftide010_gemini_port_stop;
+	pata_ftide010_port_ops.cable_detect =
+		pata_ftide010_gemini_cable_detect;
+
+	/* Flag port as SATA-capable */
+	if (gemini_sata_bridge_enabled(sg, is_ata1))
+		ftide010_port_info[0].flags |= ATA_FLAG_SATA;
+
+	/*
+	 * We assume that a simple 40-wire cable is used in the PATA mode.
+	 * if you're adding a system using the PATA interface, make sure
+	 * the right cable is set up here, it might be necessary to use
+	 * special hardware detection or encode the cable type in the device
+	 * tree with special properties.
+	 */
+	if (!is_ata1) {
+		switch (muxmode) {
+		case GEMINI_MUXMODE_0:
+			ftide->master_cbl = ATA_CBL_SATA;
+			ftide->slave_cbl = ATA_CBL_PATA40;
+			ftide->master_to_sata0 = true;
+			break;
+		case GEMINI_MUXMODE_1:
+			ftide->master_cbl = ATA_CBL_SATA;
+			ftide->slave_cbl = ATA_CBL_NONE;
+			ftide->master_to_sata0 = true;
+			break;
+		case GEMINI_MUXMODE_2:
+			ftide->master_cbl = ATA_CBL_PATA40;
+			ftide->slave_cbl = ATA_CBL_PATA40;
+			break;
+		case GEMINI_MUXMODE_3:
+			ftide->master_cbl = ATA_CBL_SATA;
+			ftide->slave_cbl = ATA_CBL_SATA;
+			ftide->master_to_sata0 = true;
+			ftide->slave_to_sata1 = true;
+			break;
+		}
+	} else {
+		switch (muxmode) {
+		case GEMINI_MUXMODE_0:
+			ftide->master_cbl = ATA_CBL_SATA;
+			ftide->slave_cbl = ATA_CBL_NONE;
+			ftide->master_to_sata1 = true;
+			break;
+		case GEMINI_MUXMODE_1:
+			ftide->master_cbl = ATA_CBL_SATA;
+			ftide->slave_cbl = ATA_CBL_PATA40;
+			ftide->master_to_sata1 = true;
+			break;
+		case GEMINI_MUXMODE_2:
+			ftide->master_cbl = ATA_CBL_SATA;
+			ftide->slave_cbl = ATA_CBL_SATA;
+			ftide->slave_to_sata0 = true;
+			ftide->master_to_sata1 = true;
+			break;
+		case GEMINI_MUXMODE_3:
+			ftide->master_cbl = ATA_CBL_PATA40;
+			ftide->slave_cbl = ATA_CBL_PATA40;
+			break;
+		}
+	}
+	dev_info(dev, "set up Gemini PATA%d\n", is_ata1);
+
+	return 0;
+}
+#else
+static int pata_ftide010_gemini_init(struct ftide010 *ftide,
+				     bool is_ata1)
+{
+	return -ENOTSUPP;
+}
+#endif
+
+
+static int pata_ftide010_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	const struct ata_port_info pi = ftide010_port_info[0];
+	const struct ata_port_info *ppi[] = { &pi, NULL };
+	struct ftide010 *ftide;
+	struct resource *res;
+	int irq;
+	int ret;
+	int i;
+
+	ftide = devm_kzalloc(dev, sizeof(*ftide), GFP_KERNEL);
+	if (!ftide)
+		return -ENOMEM;
+	ftide->dev = dev;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	ftide->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(ftide->base))
+		return PTR_ERR(ftide->base);
+
+	ftide->pclk = devm_clk_get(dev, "PCLK");
+	if (!IS_ERR(ftide->pclk)) {
+		ret = clk_prepare_enable(ftide->pclk);
+		if (ret) {
+			dev_err(dev, "failed to enable PCLK\n");
+			return ret;
+		}
+	}
+
+	/* Some special Cortina Gemini init, if needed */
+	if (of_device_is_compatible(np, "cortina,gemini-pata")) {
+		/*
+		 * We need to know which instance is probing (the
+		 * Gemini has two instances of FTIDE010) and we do
+		 * this simply by looking at the physical base
+		 * address, which is 0x63400000 for ATA1, else we
+		 * are ATA0. This will also set up the cable types.
+		 */
+		ret = pata_ftide010_gemini_init(ftide,
+				(res->start == 0x63400000));
+		if (ret)
+			goto err_dis_clk;
+	} else {
+		/* Else assume we are connected using PATA40 */
+		ftide->master_cbl = ATA_CBL_PATA40;
+		ftide->slave_cbl = ATA_CBL_PATA40;
+	}
+
+	ftide->host = ata_host_alloc_pinfo(dev, ppi, 1);
+	if (!ftide->host) {
+		ret = -ENOMEM;
+		goto err_dis_clk;
+	}
+	ftide->host->private_data = ftide;
+
+	for (i = 0; i < ftide->host->n_ports; i++) {
+		struct ata_port *ap = ftide->host->ports[i];
+		struct ata_ioports *ioaddr = &ap->ioaddr;
+
+		ioaddr->bmdma_addr = ftide->base + FTIDE010_DMA_REG;
+		ioaddr->cmd_addr = ftide->base + FTIDE010_CMD_DATA;
+		ioaddr->ctl_addr = ftide->base + FTIDE010_ALTSTAT_CTRL;
+		ioaddr->altstatus_addr = ftide->base + FTIDE010_ALTSTAT_CTRL;
+		ata_sff_std_ports(ioaddr);
+	}
+
+	dev_info(dev, "device ID %08x, irq %d, reg %pR\n",
+		 readl(ftide->base + FTIDE010_IDE_DEVICE_ID), irq, res);
+
+	ret = ata_host_activate(ftide->host, irq, ata_bmdma_interrupt,
+				0, &pata_ftide010_sht);
+	if (ret)
+		goto err_dis_clk;
+
+	return 0;
+
+err_dis_clk:
+	if (!IS_ERR(ftide->pclk))
+		clk_disable_unprepare(ftide->pclk);
+	return ret;
+}
+
+static int pata_ftide010_remove(struct platform_device *pdev)
+{
+	struct ata_host *host = platform_get_drvdata(pdev);
+	struct ftide010 *ftide = host->private_data;
+
+	ata_host_detach(ftide->host);
+	if (!IS_ERR(ftide->pclk))
+		clk_disable_unprepare(ftide->pclk);
+
+	return 0;
+}
+
+static const struct of_device_id pata_ftide010_of_match[] = {
+	{
+		.compatible = "faraday,ftide010",
+	},
+	{},
+};
+
+static struct platform_driver pata_ftide010_driver = {
+	.driver = {
+		.name = DRV_NAME,
+		.of_match_table = of_match_ptr(pata_ftide010_of_match),
+	},
+	.probe = pata_ftide010_probe,
+	.remove = pata_ftide010_remove,
+};
+module_platform_driver(pata_ftide010_driver);
+
+MODULE_AUTHOR("Linus Walleij <linus.walleij@linaro.org>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/ata/pata_octeon_cf.c b/drivers/ata/pata_octeon_cf.c
index f524a9099d01..1ba03d6df951 100644
--- a/drivers/ata/pata_octeon_cf.c
+++ b/drivers/ata/pata_octeon_cf.c
@@ -1038,7 +1038,7 @@ static void octeon_cf_shutdown(struct device *dev)
 	}
 }
 
-static struct of_device_id octeon_cf_match[] = {
+static const struct of_device_id octeon_cf_match[] = {
 	{
 		.compatible = "cavium,ebt3000-compact-flash",
 	},
diff --git a/drivers/ata/pata_pdc2027x.c b/drivers/ata/pata_pdc2027x.c
index d9ef9e276225..82bfd51692f3 100644
--- a/drivers/ata/pata_pdc2027x.c
+++ b/drivers/ata/pata_pdc2027x.c
@@ -17,7 +17,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Hardware information only available under NDA.
  *
diff --git a/drivers/ata/pata_rb532_cf.c b/drivers/ata/pata_rb532_cf.c
index c8b6a780a290..653b9a0bf727 100644
--- a/drivers/ata/pata_rb532_cf.c
+++ b/drivers/ata/pata_rb532_cf.c
@@ -148,8 +148,6 @@ static int rb532_pata_driver_probe(struct platform_device *pdev)
 	if (!ah)
 		return -ENOMEM;
 
-	platform_set_drvdata(pdev, ah);
-
 	info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
diff --git a/drivers/ata/pata_rdc.c b/drivers/ata/pata_rdc.c
index 9ce5952216bc..959bb54fd803 100644
--- a/drivers/ata/pata_rdc.c
+++ b/drivers/ata/pata_rdc.c
@@ -292,7 +292,7 @@ static struct ata_port_operations rdc_pata_ops = {
 	.prereset		= rdc_pata_prereset,
 };
 
-static struct ata_port_info rdc_port_info = {
+static const struct ata_port_info rdc_port_info = {
 
 	.flags		= ATA_FLAG_SLAVE_POSS,
 	.pio_mask	= ATA_PIO4,
diff --git a/drivers/ata/pata_samsung_cf.c b/drivers/ata/pata_samsung_cf.c
index 431c7de30ce6..50801c40b029 100644
--- a/drivers/ata/pata_samsung_cf.c
+++ b/drivers/ata/pata_samsung_cf.c
@@ -582,8 +582,6 @@ static int __init pata_s3c_probe(struct platform_device *pdev)
 	/* Set endianness and enable the interface */
 	pata_s3c_hwinit(info, pdata);
 
-	platform_set_drvdata(pdev, host);
-
 	ret = ata_host_activate(host, info->irq,
 				info->irq ? pata_s3c_irq : NULL,
 				0, &pata_s3c_sht);
diff --git a/drivers/ata/pata_sch.c b/drivers/ata/pata_sch.c
index b920c3407f8b..1b80a66caa54 100644
--- a/drivers/ata/pata_sch.c
+++ b/drivers/ata/pata_sch.c
@@ -81,7 +81,7 @@ static struct ata_port_operations sch_pata_ops = {
 	.set_dmamode		= sch_set_dmamode,
 };
 
-static struct ata_port_info sch_port_info = {
+static const struct ata_port_info sch_port_info = {
 	.flags		= ATA_FLAG_SLAVE_POSS,
 	.pio_mask	= ATA_PIO4,
 	.mwdma_mask	= ATA_MWDMA2,
diff --git a/drivers/ata/pdc_adma.c b/drivers/ata/pdc_adma.c
index 64d682c6ee57..f1e873a37465 100644
--- a/drivers/ata/pdc_adma.c
+++ b/drivers/ata/pdc_adma.c
@@ -21,7 +21,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *
  *  Supports ATA disks in single-packet ADMA mode.
diff --git a/drivers/ata/sata_dwc_460ex.c b/drivers/ata/sata_dwc_460ex.c
index e0939bd5ea73..ce128d5a6ded 100644
--- a/drivers/ata/sata_dwc_460ex.c
+++ b/drivers/ata/sata_dwc_460ex.c
@@ -1285,7 +1285,6 @@ static int sata_dwc_probe(struct platform_device *ofdev)
 	if (err)
 		dev_err(&ofdev->dev, "failed to activate host");
 
-	dev_set_drvdata(&ofdev->dev, host);
 	return 0;
 
 error_out:
diff --git a/drivers/ata/sata_fsl.c b/drivers/ata/sata_fsl.c
index 01734d54c69c..95bf3abda6f6 100644
--- a/drivers/ata/sata_fsl.c
+++ b/drivers/ata/sata_fsl.c
@@ -1523,8 +1523,6 @@ static int sata_fsl_probe(struct platform_device *ofdev)
 	ata_host_activate(host, irq, sata_fsl_interrupt, SATA_FSL_IRQ_FLAG,
 			  &sata_fsl_sht);
 
-	platform_set_drvdata(ofdev, host);
-
 	host_priv->intr_coalescing.show = fsl_sata_intr_coalescing_show;
 	host_priv->intr_coalescing.store = fsl_sata_intr_coalescing_store;
 	sysfs_attr_init(&host_priv->intr_coalescing.attr);
diff --git a/drivers/ata/sata_gemini.c b/drivers/ata/sata_gemini.c
new file mode 100644
index 000000000000..8c704523bae7
--- /dev/null
+++ b/drivers/ata/sata_gemini.c
@@ -0,0 +1,438 @@
+/*
+ * Cortina Systems Gemini SATA bridge add-on to Faraday FTIDE010
+ * Copyright (C) 2017 Linus Walleij <linus.walleij@linaro.org>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/bitops.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+#include <linux/delay.h>
+#include <linux/reset.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include "sata_gemini.h"
+
+#define DRV_NAME "gemini_sata_bridge"
+
+/**
+ * struct sata_gemini - a state container for a Gemini SATA bridge
+ * @dev: the containing device
+ * @base: remapped I/O memory base
+ * @muxmode: the current muxing mode
+ * @ide_pins: if the device is using the plain IDE interface pins
+ * @sata_bridge: if the device enables the SATA bridge
+ * @sata0_reset: SATA0 reset handler
+ * @sata1_reset: SATA1 reset handler
+ * @sata0_pclk: SATA0 PCLK handler
+ * @sata1_pclk: SATA1 PCLK handler
+ */
+struct sata_gemini {
+	struct device *dev;
+	void __iomem *base;
+	enum gemini_muxmode muxmode;
+	bool ide_pins;
+	bool sata_bridge;
+	struct reset_control *sata0_reset;
+	struct reset_control *sata1_reset;
+	struct clk *sata0_pclk;
+	struct clk *sata1_pclk;
+};
+
+/* Global IDE PAD Skew Control Register */
+#define GEMINI_GLOBAL_IDE_SKEW_CTRL		0x18
+#define GEMINI_IDE1_HOST_STROBE_DELAY_SHIFT	28
+#define GEMINI_IDE1_DEVICE_STROBE_DELAY_SHIFT	24
+#define GEMINI_IDE1_OUTPUT_IO_SKEW_SHIFT	20
+#define GEMINI_IDE1_INPUT_IO_SKEW_SHIFT		16
+#define GEMINI_IDE0_HOST_STROBE_DELAY_SHIFT	12
+#define GEMINI_IDE0_DEVICE_STROBE_DELAY_SHIFT	8
+#define GEMINI_IDE0_OUTPUT_IO_SKEW_SHIFT	4
+#define GEMINI_IDE0_INPUT_IO_SKEW_SHIFT		0
+
+/* Miscellaneous Control Register */
+#define GEMINI_GLOBAL_MISC_CTRL		0x30
+/*
+ * Values of IDE IOMUX bits in the misc control register
+ *
+ * Bits 26:24 are "IDE IO Select", which decides what SATA
+ * adapters are connected to which of the two IDE/ATA
+ * controllers in the Gemini. We can connect the two IDE blocks
+ * to one SATA adapter each, both acting as master, or one IDE
+ * blocks to two SATA adapters so the IDE block can act in a
+ * master/slave configuration.
+ *
+ * We also bring out different blocks on the actual IDE
+ * pins (not SATA pins) if (and only if) these are muxed in.
+ *
+ * 111-100 - Reserved
+ * Mode 0: 000 - ata0 master <-> sata0
+ *               ata1 master <-> sata1
+ *               ata0 slave interface brought out on IDE pads
+ * Mode 1: 001 - ata0 master <-> sata0
+ *               ata1 master <-> sata1
+ *               ata1 slave interface brought out on IDE pads
+ * Mode 2: 010 - ata1 master <-> sata1
+ *               ata1 slave  <-> sata0
+ *               ata0 master and slave interfaces brought out
+ *                    on IDE pads
+ * Mode 3: 011 - ata0 master <-> sata0
+ *               ata1 slave  <-> sata1
+ *               ata1 master and slave interfaces brought out
+ *                    on IDE pads
+ */
+#define GEMINI_IDE_IOMUX_MASK			(7 << 24)
+#define GEMINI_IDE_IOMUX_MODE0			(0 << 24)
+#define GEMINI_IDE_IOMUX_MODE1			(1 << 24)
+#define GEMINI_IDE_IOMUX_MODE2			(2 << 24)
+#define GEMINI_IDE_IOMUX_MODE3			(3 << 24)
+#define GEMINI_IDE_IOMUX_SHIFT			(24)
+#define GEMINI_IDE_PADS_ENABLE			BIT(4)
+#define GEMINI_PFLASH_PADS_DISABLE		BIT(1)
+
+/*
+ * Registers directly controlling the PATA<->SATA adapters
+ */
+#define GEMINI_SATA_ID				0x00
+#define GEMINI_SATA_PHY_ID			0x04
+#define GEMINI_SATA0_STATUS			0x08
+#define GEMINI_SATA1_STATUS			0x0c
+#define GEMINI_SATA0_CTRL			0x18
+#define GEMINI_SATA1_CTRL			0x1c
+
+#define GEMINI_SATA_STATUS_BIST_DONE		BIT(5)
+#define GEMINI_SATA_STATUS_BIST_OK		BIT(4)
+#define GEMINI_SATA_STATUS_PHY_READY		BIT(0)
+
+#define GEMINI_SATA_CTRL_PHY_BIST_EN		BIT(14)
+#define GEMINI_SATA_CTRL_PHY_FORCE_IDLE		BIT(13)
+#define GEMINI_SATA_CTRL_PHY_FORCE_READY	BIT(12)
+#define GEMINI_SATA_CTRL_PHY_AFE_LOOP_EN	BIT(10)
+#define GEMINI_SATA_CTRL_PHY_DIG_LOOP_EN	BIT(9)
+#define GEMINI_SATA_CTRL_HOTPLUG_DETECT_EN	BIT(4)
+#define GEMINI_SATA_CTRL_ATAPI_EN		BIT(3)
+#define GEMINI_SATA_CTRL_BUS_WITH_20		BIT(2)
+#define GEMINI_SATA_CTRL_SLAVE_EN		BIT(1)
+#define GEMINI_SATA_CTRL_EN			BIT(0)
+
+/*
+ * There is only ever one instance of this bridge on a system,
+ * so create a singleton so that the FTIDE010 instances can grab
+ * a reference to it.
+ */
+static struct sata_gemini *sg_singleton;
+
+struct sata_gemini *gemini_sata_bridge_get(void)
+{
+	if (sg_singleton)
+		return sg_singleton;
+	return ERR_PTR(-EPROBE_DEFER);
+}
+EXPORT_SYMBOL(gemini_sata_bridge_get);
+
+bool gemini_sata_bridge_enabled(struct sata_gemini *sg, bool is_ata1)
+{
+	if (!sg->sata_bridge)
+		return false;
+	/*
+	 * In muxmode 2 and 3 one of the ATA controllers is
+	 * actually not connected to any SATA bridge.
+	 */
+	if ((sg->muxmode == GEMINI_MUXMODE_2) &&
+	    !is_ata1)
+		return false;
+	if ((sg->muxmode == GEMINI_MUXMODE_3) &&
+	    is_ata1)
+		return false;
+
+	return true;
+}
+EXPORT_SYMBOL(gemini_sata_bridge_enabled);
+
+enum gemini_muxmode gemini_sata_get_muxmode(struct sata_gemini *sg)
+{
+	return sg->muxmode;
+}
+EXPORT_SYMBOL(gemini_sata_get_muxmode);
+
+static int gemini_sata_setup_bridge(struct sata_gemini *sg,
+				    unsigned int bridge)
+{
+	unsigned long timeout = jiffies + (HZ * 1);
+	bool bridge_online;
+	u32 val;
+
+	if (bridge == 0) {
+		val = GEMINI_SATA_CTRL_HOTPLUG_DETECT_EN | GEMINI_SATA_CTRL_EN;
+		/* SATA0 slave mode is only used in muxmode 2 */
+		if (sg->muxmode == GEMINI_MUXMODE_2)
+			val |= GEMINI_SATA_CTRL_SLAVE_EN;
+		writel(val, sg->base + GEMINI_SATA0_CTRL);
+	} else {
+		val = GEMINI_SATA_CTRL_HOTPLUG_DETECT_EN | GEMINI_SATA_CTRL_EN;
+		/* SATA1 slave mode is only used in muxmode 3 */
+		if (sg->muxmode == GEMINI_MUXMODE_3)
+			val |= GEMINI_SATA_CTRL_SLAVE_EN;
+		writel(val, sg->base + GEMINI_SATA1_CTRL);
+	}
+
+	/* Vendor code waits 10 ms here */
+	msleep(10);
+
+	/* Wait for PHY to become ready */
+	do {
+		msleep(100);
+
+		if (bridge == 0)
+			val = readl(sg->base + GEMINI_SATA0_STATUS);
+		else
+			val = readl(sg->base + GEMINI_SATA1_STATUS);
+		if (val & GEMINI_SATA_STATUS_PHY_READY)
+			break;
+	} while (time_before(jiffies, timeout));
+
+	bridge_online = !!(val & GEMINI_SATA_STATUS_PHY_READY);
+
+	dev_info(sg->dev, "SATA%d PHY %s\n", bridge,
+		 bridge_online ? "ready" : "not ready");
+
+	return bridge_online ? 0: -ENODEV;
+}
+
+int gemini_sata_start_bridge(struct sata_gemini *sg, unsigned int bridge)
+{
+	struct clk *pclk;
+	int ret;
+
+	if (bridge == 0)
+		pclk = sg->sata0_pclk;
+	else
+		pclk = sg->sata1_pclk;
+	clk_enable(pclk);
+	msleep(10);
+
+	/* Do not keep clocking a bridge that is not online */
+	ret = gemini_sata_setup_bridge(sg, bridge);
+	if (ret)
+		clk_disable(pclk);
+
+	return ret;
+}
+EXPORT_SYMBOL(gemini_sata_start_bridge);
+
+void gemini_sata_stop_bridge(struct sata_gemini *sg, unsigned int bridge)
+{
+	if (bridge == 0)
+		clk_disable(sg->sata0_pclk);
+	else if (bridge == 1)
+		clk_disable(sg->sata1_pclk);
+}
+EXPORT_SYMBOL(gemini_sata_stop_bridge);
+
+int gemini_sata_reset_bridge(struct sata_gemini *sg,
+			     unsigned int bridge)
+{
+	if (bridge == 0)
+		reset_control_reset(sg->sata0_reset);
+	else
+		reset_control_reset(sg->sata1_reset);
+	msleep(10);
+	return gemini_sata_setup_bridge(sg, bridge);
+}
+EXPORT_SYMBOL(gemini_sata_reset_bridge);
+
+static int gemini_sata_bridge_init(struct sata_gemini *sg)
+{
+	struct device *dev = sg->dev;
+	u32 sata_id, sata_phy_id;
+	int ret;
+
+	sg->sata0_pclk = devm_clk_get(dev, "SATA0_PCLK");
+	if (IS_ERR(sg->sata0_pclk)) {
+		dev_err(dev, "no SATA0 PCLK");
+		return -ENODEV;
+	}
+	sg->sata1_pclk = devm_clk_get(dev, "SATA1_PCLK");
+	if (IS_ERR(sg->sata1_pclk)) {
+		dev_err(dev, "no SATA1 PCLK");
+		return -ENODEV;
+	}
+
+	ret = clk_prepare_enable(sg->sata0_pclk);
+	if (ret) {
+		pr_err("failed to enable SATA0 PCLK\n");
+		return ret;
+	}
+	ret = clk_prepare_enable(sg->sata1_pclk);
+	if (ret) {
+		pr_err("failed to enable SATA1 PCLK\n");
+		clk_disable_unprepare(sg->sata0_pclk);
+		return ret;
+	}
+
+	sg->sata0_reset = devm_reset_control_get(dev, "sata0");
+	if (IS_ERR(sg->sata0_reset)) {
+		dev_err(dev, "no SATA0 reset controller\n");
+		clk_disable_unprepare(sg->sata1_pclk);
+		clk_disable_unprepare(sg->sata0_pclk);
+		return PTR_ERR(sg->sata0_reset);
+	}
+	sg->sata1_reset = devm_reset_control_get(dev, "sata1");
+	if (IS_ERR(sg->sata1_reset)) {
+		dev_err(dev, "no SATA1 reset controller\n");
+		clk_disable_unprepare(sg->sata1_pclk);
+		clk_disable_unprepare(sg->sata0_pclk);
+		return PTR_ERR(sg->sata1_reset);
+	}
+
+	sata_id = readl(sg->base + GEMINI_SATA_ID);
+	sata_phy_id = readl(sg->base + GEMINI_SATA_PHY_ID);
+	sg->sata_bridge = true;
+	clk_disable(sg->sata0_pclk);
+	clk_disable(sg->sata1_pclk);
+
+	dev_info(dev, "SATA ID %08x, PHY ID: %08x\n", sata_id, sata_phy_id);
+
+	return 0;
+}
+
+static int gemini_sata_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct sata_gemini *sg;
+	static struct regmap *map;
+	struct resource *res;
+	enum gemini_muxmode muxmode;
+	u32 gmode;
+	u32 gmask;
+	u32 val;
+	int ret;
+
+	sg = devm_kzalloc(dev, sizeof(*sg), GFP_KERNEL);
+	if (!sg)
+		return -ENOMEM;
+	sg->dev = dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	sg->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(sg->base))
+		return PTR_ERR(sg->base);
+
+	map = syscon_regmap_lookup_by_phandle(np, "syscon");
+	if (IS_ERR(map)) {
+		dev_err(dev, "no global syscon\n");
+		return PTR_ERR(map);
+	}
+
+	/* Set up the SATA bridge if need be */
+	if (of_property_read_bool(np, "cortina,gemini-enable-sata-bridge")) {
+		ret = gemini_sata_bridge_init(sg);
+		if (ret)
+			return ret;
+	}
+
+	if (of_property_read_bool(np, "cortina,gemini-enable-ide-pins"))
+		sg->ide_pins = true;
+
+	if (!sg->sata_bridge && !sg->ide_pins) {
+		dev_err(dev, "neither SATA bridge or IDE output enabled\n");
+		ret = -EINVAL;
+		goto out_unprep_clk;
+	}
+
+	ret = of_property_read_u32(np, "cortina,gemini-ata-muxmode", &muxmode);
+	if (ret) {
+		dev_err(dev, "could not parse ATA muxmode\n");
+		goto out_unprep_clk;
+	}
+	if (muxmode > GEMINI_MUXMODE_3) {
+		dev_err(dev, "illegal muxmode %d\n", muxmode);
+		ret = -EINVAL;
+		goto out_unprep_clk;
+	}
+	sg->muxmode = muxmode;
+	gmask = GEMINI_IDE_IOMUX_MASK;
+	gmode = (muxmode << GEMINI_IDE_IOMUX_SHIFT);
+
+	/*
+	 * If we mux out the IDE, parallel flash must be disabled.
+	 * SATA0 and SATA1 have dedicated pins and may coexist with
+	 * parallel flash.
+	 */
+	if (sg->ide_pins)
+		gmode |= GEMINI_IDE_PADS_ENABLE | GEMINI_PFLASH_PADS_DISABLE;
+	else
+		gmask |= GEMINI_IDE_PADS_ENABLE;
+
+	ret = regmap_update_bits(map, GEMINI_GLOBAL_MISC_CTRL, gmask, gmode);
+	if (ret) {
+		dev_err(dev, "unable to set up IDE muxing\n");
+		ret = -ENODEV;
+		goto out_unprep_clk;
+	}
+
+	/* FIXME: add more elaborate IDE skew control handling */
+	if (sg->ide_pins) {
+		ret = regmap_read(map, GEMINI_GLOBAL_IDE_SKEW_CTRL, &val);
+		if (ret) {
+			dev_err(dev, "cannot read IDE skew control register\n");
+			return ret;
+		}
+		dev_info(dev, "IDE skew control: %08x\n", val);
+	}
+
+	dev_info(dev, "set up the Gemini IDE/SATA nexus\n");
+	platform_set_drvdata(pdev, sg);
+	sg_singleton = sg;
+
+	return 0;
+
+out_unprep_clk:
+	if (sg->sata_bridge) {
+		clk_unprepare(sg->sata1_pclk);
+		clk_unprepare(sg->sata0_pclk);
+	}
+	return ret;
+}
+
+static int gemini_sata_remove(struct platform_device *pdev)
+{
+	struct sata_gemini *sg = platform_get_drvdata(pdev);
+
+	if (sg->sata_bridge) {
+		clk_unprepare(sg->sata1_pclk);
+		clk_unprepare(sg->sata0_pclk);
+	}
+	sg_singleton = NULL;
+
+	return 0;
+}
+
+static const struct of_device_id gemini_sata_of_match[] = {
+	{
+		.compatible = "cortina,gemini-sata-bridge",
+	},
+	{},
+};
+
+static struct platform_driver gemini_sata_driver = {
+	.driver = {
+		.name = DRV_NAME,
+		.of_match_table = of_match_ptr(gemini_sata_of_match),
+	},
+	.probe = gemini_sata_probe,
+	.remove = gemini_sata_remove,
+};
+module_platform_driver(gemini_sata_driver);
+
+MODULE_AUTHOR("Linus Walleij <linus.walleij@linaro.org>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/ata/sata_gemini.h b/drivers/ata/sata_gemini.h
new file mode 100644
index 000000000000..ca1837a394c8
--- /dev/null
+++ b/drivers/ata/sata_gemini.h
@@ -0,0 +1,21 @@
+/* Header for the Gemini SATA bridge */
+#ifndef SATA_GEMINI_H
+#define SATA_GEMINI_H
+
+struct sata_gemini;
+
+enum gemini_muxmode {
+	GEMINI_MUXMODE_0 = 0,
+	GEMINI_MUXMODE_1,
+	GEMINI_MUXMODE_2,
+	GEMINI_MUXMODE_3,
+};
+
+struct sata_gemini *gemini_sata_bridge_get(void);
+bool gemini_sata_bridge_enabled(struct sata_gemini *sg, bool is_ata1);
+enum gemini_muxmode gemini_sata_get_muxmode(struct sata_gemini *sg);
+int gemini_sata_start_bridge(struct sata_gemini *sg, unsigned int bridge);
+void gemini_sata_stop_bridge(struct sata_gemini *sg, unsigned int bridge);
+int gemini_sata_reset_bridge(struct sata_gemini *sg, unsigned int bridge);
+
+#endif
diff --git a/drivers/ata/sata_inic162x.c b/drivers/ata/sata_inic162x.c
index e81a8217f1ff..9b6d7930d1c7 100644
--- a/drivers/ata/sata_inic162x.c
+++ b/drivers/ata/sata_inic162x.c
@@ -737,7 +737,7 @@ static struct ata_port_operations inic_port_ops = {
 	.port_start		= inic_port_start,
 };
 
-static struct ata_port_info inic_port_info = {
+static const struct ata_port_info inic_port_info = {
 	.flags			= ATA_FLAG_SATA | ATA_FLAG_PIO_DMA,
 	.pio_mask		= ATA_PIO4,
 	.mwdma_mask		= ATA_MWDMA2,
diff --git a/drivers/ata/sata_nv.c b/drivers/ata/sata_nv.c
index 734f563b8d37..8c683ddd0f58 100644
--- a/drivers/ata/sata_nv.c
+++ b/drivers/ata/sata_nv.c
@@ -21,7 +21,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  No hardware documentation available outside of NVIDIA.
  *  This driver programs the NVIDIA SATA controller in a similar
diff --git a/drivers/ata/sata_promise.c b/drivers/ata/sata_promise.c
index 0fa211e2831c..d032bf657f70 100644
--- a/drivers/ata/sata_promise.c
+++ b/drivers/ata/sata_promise.c
@@ -25,7 +25,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Hardware information only available under NDA.
  *
diff --git a/drivers/ata/sata_promise.h b/drivers/ata/sata_promise.h
index 00d6000e546f..61633ef5ed72 100644
--- a/drivers/ata/sata_promise.h
+++ b/drivers/ata/sata_promise.h
@@ -20,7 +20,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  */
 
diff --git a/drivers/ata/sata_qstor.c b/drivers/ata/sata_qstor.c
index af987a4f33d1..1fe941688e95 100644
--- a/drivers/ata/sata_qstor.c
+++ b/drivers/ata/sata_qstor.c
@@ -23,7 +23,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  */
 
diff --git a/drivers/ata/sata_rcar.c b/drivers/ata/sata_rcar.c
index b7939a2c1fab..ee9844758736 100644
--- a/drivers/ata/sata_rcar.c
+++ b/drivers/ata/sata_rcar.c
@@ -828,7 +828,7 @@ static void sata_rcar_init_controller(struct ata_host *host)
 	iowrite32(ATAPI_INT_ENABLE_SATAINT, base + ATAPI_INT_ENABLE_REG);
 }
 
-static struct of_device_id sata_rcar_match[] = {
+static const struct of_device_id sata_rcar_match[] = {
 	{
 		/* Deprecated by "renesas,sata-r8a7779" */
 		.compatible = "renesas,rcar-sata",
diff --git a/drivers/ata/sata_sil.c b/drivers/ata/sata_sil.c
index 29bcff086bce..ed76f070d21e 100644
--- a/drivers/ata/sata_sil.c
+++ b/drivers/ata/sata_sil.c
@@ -25,7 +25,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Documentation for SiI 3112:
  *  http://gkernel.sourceforge.net/specs/sii/3112A_SiI-DS-0095-B2.pdf.bz2
diff --git a/drivers/ata/sata_sis.c b/drivers/ata/sata_sis.c
index d1637ac40a73..30f4f35f36d4 100644
--- a/drivers/ata/sata_sis.c
+++ b/drivers/ata/sata_sis.c
@@ -24,7 +24,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Hardware documentation available under NDA.
  *
diff --git a/drivers/ata/sata_svw.c b/drivers/ata/sata_svw.c
index ff614be55d0f..0fd6ac7e57ba 100644
--- a/drivers/ata/sata_svw.c
+++ b/drivers/ata/sata_svw.c
@@ -30,7 +30,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Hardware documentation available under NDA.
  *
diff --git a/drivers/ata/sata_sx4.c b/drivers/ata/sata_sx4.c
index 48301cb3a316..405e606a234d 100644
--- a/drivers/ata/sata_sx4.c
+++ b/drivers/ata/sata_sx4.c
@@ -24,7 +24,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Hardware documentation available under NDA.
  *
diff --git a/drivers/ata/sata_uli.c b/drivers/ata/sata_uli.c
index 08f98c3ed5c8..4f6e8d8156de 100644
--- a/drivers/ata/sata_uli.c
+++ b/drivers/ata/sata_uli.c
@@ -18,7 +18,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Hardware documentation available under NDA.
  *
diff --git a/drivers/ata/sata_via.c b/drivers/ata/sata_via.c
index f3f538eec7b3..93b8d783936a 100644
--- a/drivers/ata/sata_via.c
+++ b/drivers/ata/sata_via.c
@@ -25,7 +25,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Hardware documentation available under NDA.
  *
@@ -80,6 +80,10 @@ struct svia_priv {
 	bool			wd_workaround;
 };
 
+static int vt6420_hotplug;
+module_param_named(vt6420_hotplug, vt6420_hotplug, int, 0644);
+MODULE_PARM_DESC(vt6420_hotplug, "Enable hot-plug support for VT6420 (0=Don't support, 1=support)");
+
 static int svia_init_one(struct pci_dev *pdev, const struct pci_device_id *ent);
 #ifdef CONFIG_PM_SLEEP
 static int svia_pci_device_resume(struct pci_dev *pdev);
@@ -166,7 +170,7 @@ static const struct ata_port_info vt6420_port_info = {
 	.port_ops	= &vt6420_sata_ops,
 };
 
-static struct ata_port_info vt6421_sport_info = {
+static const struct ata_port_info vt6421_sport_info = {
 	.flags		= ATA_FLAG_SATA,
 	.pio_mask	= ATA_PIO4,
 	.mwdma_mask	= ATA_MWDMA2,
@@ -174,7 +178,7 @@ static struct ata_port_info vt6421_sport_info = {
 	.port_ops	= &vt6421_sata_ops,
 };
 
-static struct ata_port_info vt6421_pport_info = {
+static const struct ata_port_info vt6421_pport_info = {
 	.flags		= ATA_FLAG_SLAVE_POSS,
 	.pio_mask	= ATA_PIO4,
 	/* No MWDMA */
@@ -182,7 +186,7 @@ static struct ata_port_info vt6421_pport_info = {
 	.port_ops	= &vt6421_pata_ops,
 };
 
-static struct ata_port_info vt8251_port_info = {
+static const struct ata_port_info vt8251_port_info = {
 	.flags		= ATA_FLAG_SATA | ATA_FLAG_SLAVE_POSS,
 	.pio_mask	= ATA_PIO4,
 	.mwdma_mask	= ATA_MWDMA2,
@@ -473,6 +477,11 @@ static int vt6420_prepare_host(struct pci_dev *pdev, struct ata_host **r_host)
 	struct ata_host *host;
 	int rc;
 
+	if (vt6420_hotplug) {
+		ppi[0]->port_ops->scr_read = svia_scr_read;
+		ppi[0]->port_ops->scr_write = svia_scr_write;
+	}
+
 	rc = ata_pci_bmdma_prepare_host(pdev, ppi, &host);
 	if (rc)
 		return rc;
@@ -556,7 +565,7 @@ static void svia_wd_fix(struct pci_dev *pdev)
 	pci_write_config_byte(pdev, 0x52, tmp8 | BIT(2));
 }
 
-static irqreturn_t vt6421_interrupt(int irq, void *dev_instance)
+static irqreturn_t vt642x_interrupt(int irq, void *dev_instance)
 {
 	struct ata_host *host = dev_instance;
 	irqreturn_t rc = ata_bmdma_interrupt(irq, dev_instance);
@@ -644,7 +653,7 @@ static void svia_configure(struct pci_dev *pdev, int board_id,
 		pci_write_config_byte(pdev, SATA_NATIVE_MODE, tmp8);
 	}
 
-	if (board_id == vt6421) {
+	if ((board_id == vt6420 && vt6420_hotplug) || board_id == vt6421) {
 		/* enable IRQ on hotplug */
 		pci_read_config_byte(pdev, SVIA_MISC_3, &tmp8);
 		if ((tmp8 & SATA_HOTPLUG) != SATA_HOTPLUG) {
@@ -744,8 +753,8 @@ static int svia_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	svia_configure(pdev, board_id, hpriv);
 
 	pci_set_master(pdev);
-	if (board_id == vt6421)
-		return ata_host_activate(host, pdev->irq, vt6421_interrupt,
+	if ((board_id == vt6420 && vt6420_hotplug) || board_id == vt6421)
+		return ata_host_activate(host, pdev->irq, vt642x_interrupt,
 					 IRQF_SHARED, &svia_sht);
 	else
 		return ata_host_activate(host, pdev->irq, ata_bmdma_interrupt,
diff --git a/drivers/ata/sata_vsc.c b/drivers/ata/sata_vsc.c
index 183eb52085df..9648127cca70 100644
--- a/drivers/ata/sata_vsc.c
+++ b/drivers/ata/sata_vsc.c
@@ -26,7 +26,7 @@
  *
  *
  *  libata documentation is available via 'make {ps|pdf}docs',
- *  as Documentation/DocBook/libata.*
+ *  as Documentation/driver-api/libata.rst
  *
  *  Vitesse hardware documentation presumably available under NDA.
  *  Intel 31244 (same hardware interface) documentation presumably
diff --git a/drivers/atm/atmtcp.c b/drivers/atm/atmtcp.c
index 3ef6253e1cce..56fa16c85ebf 100644
--- a/drivers/atm/atmtcp.c
+++ b/drivers/atm/atmtcp.c
@@ -60,7 +60,7 @@ static int atmtcp_send_control(struct atm_vcc *vcc,int type,
 		return -EUNATCH;
 	}
 	atm_force_charge(out_vcc,skb->truesize);
-	new_msg = (struct atmtcp_control *) skb_put(skb,sizeof(*new_msg));
+	new_msg = skb_put(skb, sizeof(*new_msg));
 	*new_msg = *msg;
 	new_msg->hdr.length = ATMTCP_HDR_MAGIC;
 	new_msg->type = type;
@@ -217,7 +217,7 @@ static int atmtcp_v_send(struct atm_vcc *vcc,struct sk_buff *skb)
 		atomic_inc(&vcc->stats->tx_err);
 		return -ENOBUFS;
 	}
-	hdr = (void *) skb_put(new_skb,sizeof(struct atmtcp_hdr));
+	hdr = skb_put(new_skb, sizeof(struct atmtcp_hdr));
 	hdr->vpi = htons(vcc->vpi);
 	hdr->vci = htons(vcc->vci);
 	hdr->length = htonl(skb->len);
diff --git a/drivers/atm/fore200e.c b/drivers/atm/fore200e.c
index 637c3e6b0f9e..f0433adcd8fc 100644
--- a/drivers/atm/fore200e.c
+++ b/drivers/atm/fore200e.c
@@ -924,12 +924,7 @@ fore200e_tx_irq(struct fore200e* fore200e)
 		else {
 		    dev_kfree_skb_any(entry->skb);
 		}
-#if 1
-		/* race fixed by the above incarnation mechanism, but... */
-		if (atomic_read(&sk_atm(vcc)->sk_wmem_alloc) < 0) {
-		    atomic_set(&sk_atm(vcc)->sk_wmem_alloc, 0);
-		}
-#endif
+
 		/* check error condition */
 		if (*entry->status & STATUS_ERROR)
 		    atomic_inc(&vcc->stats->tx_err);
@@ -1104,7 +1099,7 @@ fore200e_push_rpd(struct fore200e* fore200e, struct atm_vcc* vcc, struct rpd* rp
 	/* Make device DMA transfer visible to CPU.  */
 	fore200e->bus->dma_sync_for_cpu(fore200e, buffer->data.dma_addr, rpd->rsd[ i ].length, DMA_FROM_DEVICE);
 	
-	memcpy(skb_put(skb, rpd->rsd[ i ].length), buffer->data.align_addr, rpd->rsd[ i ].length);
+	skb_put_data(skb, buffer->data.align_addr, rpd->rsd[i].length);
 
 	/* Now let the device get at it again.  */
 	fore200e->bus->dma_sync_for_device(fore200e, buffer->data.dma_addr, rpd->rsd[ i ].length, DMA_FROM_DEVICE);
@@ -1130,13 +1125,9 @@ fore200e_push_rpd(struct fore200e* fore200e, struct atm_vcc* vcc, struct rpd* rp
 	return -ENOMEM;
     }
 
-    ASSERT(atomic_read(&sk_atm(vcc)->sk_wmem_alloc) >= 0);
-
     vcc->push(vcc, skb);
     atomic_inc(&vcc->stats->rx);
 
-    ASSERT(atomic_read(&sk_atm(vcc)->sk_wmem_alloc) >= 0);
-
     return 0;
 }
 
@@ -1572,7 +1563,6 @@ fore200e_send(struct atm_vcc *vcc, struct sk_buff *skb)
     unsigned long           flags;
 
     ASSERT(vcc);
-    ASSERT(atomic_read(&sk_atm(vcc)->sk_wmem_alloc) >= 0);
     ASSERT(fore200e);
     ASSERT(fore200e_vcc);
 
diff --git a/drivers/atm/he.c b/drivers/atm/he.c
index 3617659b9184..37ee21c5a5ca 100644
--- a/drivers/atm/he.c
+++ b/drivers/atm/he.c
@@ -1735,7 +1735,7 @@ he_service_rbrq(struct he_dev *he_dev, int group)
 		__net_timestamp(skb);
 
 		list_for_each_entry(heb, &he_vcc->buffers, entry)
-			memcpy(skb_put(skb, heb->len), &heb->data, heb->len);
+			skb_put_data(skb, &heb->data, heb->len);
 
 		switch (vcc->qos.aal) {
 			case ATM_AAL0:
@@ -2395,7 +2395,7 @@ he_close(struct atm_vcc *vcc)
 		 * TBRQ, the host issues the close command to the adapter.
 		 */
 
-		while (((tx_inuse = atomic_read(&sk_atm(vcc)->sk_wmem_alloc)) > 1) &&
+		while (((tx_inuse = refcount_read(&sk_atm(vcc)->sk_wmem_alloc)) > 1) &&
 		       (retry < MAX_RETRY)) {
 			msleep(sleep);
 			if (sleep < 250)
diff --git a/drivers/atm/idt77252.c b/drivers/atm/idt77252.c
index 5ec109533bb9..60bacba03d17 100644
--- a/drivers/atm/idt77252.c
+++ b/drivers/atm/idt77252.c
@@ -724,7 +724,7 @@ push_on_scq(struct idt77252_dev *card, struct vc_map *vc, struct sk_buff *skb)
 		struct sock *sk = sk_atm(vcc);
 
 		vc->estimator->cells += (skb->len + 47) / 48;
-		if (atomic_read(&sk->sk_wmem_alloc) >
+		if (refcount_read(&sk->sk_wmem_alloc) >
 		    (sk->sk_sndbuf >> 1)) {
 			u32 cps = vc->estimator->maxcps;
 
@@ -1090,8 +1090,7 @@ dequeue_rx(struct idt77252_dev *card, struct rsq_entry *rsqe)
 
 			*((u32 *) sb->data) = aal0;
 			skb_put(sb, sizeof(u32));
-			memcpy(skb_put(sb, ATM_CELL_PAYLOAD),
-			       cell, ATM_CELL_PAYLOAD);
+			skb_put_data(sb, cell, ATM_CELL_PAYLOAD);
 
 			ATM_SKB(sb)->vcc = vcc;
 			__net_timestamp(sb);
@@ -1159,8 +1158,7 @@ dequeue_rx(struct idt77252_dev *card, struct rsq_entry *rsqe)
 				return;
 			}
 			skb_queue_walk(&rpp->queue, sb)
-				memcpy(skb_put(skb, sb->len),
-				       sb->data, sb->len);
+				skb_put_data(skb, sb->data, sb->len);
 
 			recycle_rx_pool_skb(card, rpp);
 
@@ -1322,8 +1320,7 @@ idt77252_rx_raw(struct idt77252_dev *card)
 
 		*((u32 *) sb->data) = header;
 		skb_put(sb, sizeof(u32));
-		memcpy(skb_put(sb, ATM_CELL_PAYLOAD), &(queue->data[16]),
-		       ATM_CELL_PAYLOAD);
+		skb_put_data(sb, &(queue->data[16]), ATM_CELL_PAYLOAD);
 
 		ATM_SKB(sb)->vcc = vcc;
 		__net_timestamp(sb);
@@ -2012,9 +2009,9 @@ idt77252_send_oam(struct atm_vcc *vcc, void *cell, int flags)
 		atomic_inc(&vcc->stats->tx_err);
 		return -ENOMEM;
 	}
-	atomic_add(skb->truesize, &sk_atm(vcc)->sk_wmem_alloc);
+	refcount_add(skb->truesize, &sk_atm(vcc)->sk_wmem_alloc);
 
-	memcpy(skb_put(skb, 52), cell, 52);
+	skb_put_data(skb, cell, 52);
 
 	return idt77252_send_skb(vcc, skb, 1);
 }
diff --git a/drivers/atm/solos-pci.c b/drivers/atm/solos-pci.c
index 5ad037c07ec7..c8f2ca6d8b29 100644
--- a/drivers/atm/solos-pci.c
+++ b/drivers/atm/solos-pci.c
@@ -205,7 +205,7 @@ static ssize_t solos_param_show(struct device *dev, struct device_attribute *att
 		return -ENOMEM;
 	}
 
-	header = (void *)skb_put(skb, sizeof(*header));
+	header = skb_put(skb, sizeof(*header));
 
 	buflen = snprintf((void *)&header[1], buflen - 1,
 			  "L%05d\n%s\n", current->pid, attr->attr.name);
@@ -261,7 +261,7 @@ static ssize_t solos_param_store(struct device *dev, struct device_attribute *at
 		return -ENOMEM;
 	}
 
-	header = (void *)skb_put(skb, sizeof(*header));
+	header = skb_put(skb, sizeof(*header));
 
 	buflen = snprintf((void *)&header[1], buflen - 1,
 			  "L%05d\n%s\n%s\n", current->pid, attr->attr.name, buf);
@@ -486,14 +486,14 @@ static int send_command(struct solos_card *card, int dev, const char *buf, size_
 		return 0;
 	}
 
-	header = (void *)skb_put(skb, sizeof(*header));
+	header = skb_put(skb, sizeof(*header));
 
 	header->size = cpu_to_le16(size);
 	header->vpi = cpu_to_le16(0);
 	header->vci = cpu_to_le16(0);
 	header->type = cpu_to_le16(PKT_COMMAND);
 
-	memcpy(skb_put(skb, size), buf, size);
+	skb_put_data(skb, buf, size);
 
 	fpga_queue(card, dev, skb, NULL);
 
@@ -945,7 +945,7 @@ static int popen(struct atm_vcc *vcc)
 			dev_warn(&card->dev->dev, "Failed to allocate sk_buff in popen()\n");
 		return -ENOMEM;
 	}
-	header = (void *)skb_put(skb, sizeof(*header));
+	header = skb_put(skb, sizeof(*header));
 
 	header->size = cpu_to_le16(0);
 	header->vpi = cpu_to_le16(vcc->vpi);
@@ -982,7 +982,7 @@ static void pclose(struct atm_vcc *vcc)
 		dev_warn(&card->dev->dev, "Failed to allocate sk_buff in pclose()\n");
 		return;
 	}
-	header = (void *)skb_put(skb, sizeof(*header));
+	header = skb_put(skb, sizeof(*header));
 
 	header->size = cpu_to_le16(0);
 	header->vpi = cpu_to_le16(vcc->vpi);
@@ -1174,7 +1174,7 @@ static int psend(struct atm_vcc *vcc, struct sk_buff *skb)
 		}
 	}
 
-	header = (void *)skb_push(skb, sizeof(*header));
+	header = skb_push(skb, sizeof(*header));
 
 	/* This does _not_ include the size of the header */
 	header->size = cpu_to_le16(pktlen);
@@ -1251,10 +1251,10 @@ static int fpga_probe(struct pci_dev *dev, const struct pci_device_id *id)
 
 	if (reset) {
 		iowrite32(1, card->config_regs + FPGA_MODE);
-		data32 = ioread32(card->config_regs + FPGA_MODE); 
+		ioread32(card->config_regs + FPGA_MODE);
 
 		iowrite32(0, card->config_regs + FPGA_MODE);
-		data32 = ioread32(card->config_regs + FPGA_MODE); 
+		ioread32(card->config_regs + FPGA_MODE);
 	}
 
 	data32 = ioread32(card->config_regs + FPGA_VER);
@@ -1398,7 +1398,7 @@ static int atm_init(struct solos_card *card, struct device *parent)
 			continue;
 		}
 
-		header = (void *)skb_put(skb, sizeof(*header));
+		header = skb_put(skb, sizeof(*header));
 
 		header->size = cpu_to_le16(0);
 		header->vpi = cpu_to_le16(0);
diff --git a/drivers/auxdisplay/panel.c b/drivers/auxdisplay/panel.c
index e0c014c2356f..7a8b8fb2f572 100644
--- a/drivers/auxdisplay/panel.c
+++ b/drivers/auxdisplay/panel.c
@@ -1345,14 +1345,11 @@ static inline void input_state_falling(struct logical_input *input)
 
 static void panel_process_inputs(void)
 {
-	struct list_head *item;
 	struct logical_input *input;
 
 	keypressed = 0;
 	inputs_stable = 1;
-	list_for_each(item, &logical_inputs) {
-		input = list_entry(item, struct logical_input, list);
-
+	list_for_each_entry(input, &logical_inputs, list) {
 		switch (input->state) {
 		case INPUT_ST_LOW:
 			if ((phys_curr & input->mask) != input->value)
diff --git a/drivers/base/Kconfig b/drivers/base/Kconfig
index d718ae4b907a..f046d21de57d 100644
--- a/drivers/base/Kconfig
+++ b/drivers/base/Kconfig
@@ -339,4 +339,12 @@ config CMA_ALIGNMENT
 
 endif
 
+config GENERIC_ARCH_TOPOLOGY
+	bool
+	help
+	  Enable support for architectures common topology code: e.g., parsing
+	  CPU capacity information from DT, usage of such information for
+	  appropriate scaling, sysfs interface for changing capacity values at
+	  runtime.
+
 endmenu
diff --git a/drivers/base/Makefile b/drivers/base/Makefile
index f2816f6ff76a..397e5c344e6a 100644
--- a/drivers/base/Makefile
+++ b/drivers/base/Makefile
@@ -23,6 +23,7 @@ obj-$(CONFIG_SOC_BUS) += soc.o
 obj-$(CONFIG_PINCTRL) += pinctrl.o
 obj-$(CONFIG_DEV_COREDUMP) += devcoredump.o
 obj-$(CONFIG_GENERIC_MSI_IRQ_DOMAIN) += platform-msi.o
+obj-$(CONFIG_GENERIC_ARCH_TOPOLOGY) += arch_topology.o
 
 obj-y			+= test/
 
diff --git a/drivers/base/arch_topology.c b/drivers/base/arch_topology.c
new file mode 100644
index 000000000000..d1c33a85059e
--- /dev/null
+++ b/drivers/base/arch_topology.c
@@ -0,0 +1,243 @@
+/*
+ * Arch specific cpu topology information
+ *
+ * Copyright (C) 2016, ARM Ltd.
+ * Written by: Juri Lelli, ARM Ltd.
+ *
+ * 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.
+ *
+ * Released under the GPLv2 only.
+ * SPDX-License-Identifier: GPL-2.0
+ */
+
+#include <linux/acpi.h>
+#include <linux/arch_topology.h>
+#include <linux/cpu.h>
+#include <linux/cpufreq.h>
+#include <linux/device.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/sched/topology.h>
+
+static DEFINE_MUTEX(cpu_scale_mutex);
+static DEFINE_PER_CPU(unsigned long, cpu_scale) = SCHED_CAPACITY_SCALE;
+
+unsigned long topology_get_cpu_scale(struct sched_domain *sd, int cpu)
+{
+	return per_cpu(cpu_scale, cpu);
+}
+
+void topology_set_cpu_scale(unsigned int cpu, unsigned long capacity)
+{
+	per_cpu(cpu_scale, cpu) = capacity;
+}
+
+static ssize_t cpu_capacity_show(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct cpu *cpu = container_of(dev, struct cpu, dev);
+
+	return sprintf(buf, "%lu\n",
+			topology_get_cpu_scale(NULL, cpu->dev.id));
+}
+
+static ssize_t cpu_capacity_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf,
+				  size_t count)
+{
+	struct cpu *cpu = container_of(dev, struct cpu, dev);
+	int this_cpu = cpu->dev.id;
+	int i;
+	unsigned long new_capacity;
+	ssize_t ret;
+
+	if (!count)
+		return 0;
+
+	ret = kstrtoul(buf, 0, &new_capacity);
+	if (ret)
+		return ret;
+	if (new_capacity > SCHED_CAPACITY_SCALE)
+		return -EINVAL;
+
+	mutex_lock(&cpu_scale_mutex);
+	for_each_cpu(i, &cpu_topology[this_cpu].core_sibling)
+		topology_set_cpu_scale(i, new_capacity);
+	mutex_unlock(&cpu_scale_mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR_RW(cpu_capacity);
+
+static int register_cpu_capacity_sysctl(void)
+{
+	int i;
+	struct device *cpu;
+
+	for_each_possible_cpu(i) {
+		cpu = get_cpu_device(i);
+		if (!cpu) {
+			pr_err("%s: too early to get CPU%d device!\n",
+			       __func__, i);
+			continue;
+		}
+		device_create_file(cpu, &dev_attr_cpu_capacity);
+	}
+
+	return 0;
+}
+subsys_initcall(register_cpu_capacity_sysctl);
+
+static u32 capacity_scale;
+static u32 *raw_capacity;
+static bool cap_parsing_failed;
+
+void topology_normalize_cpu_scale(void)
+{
+	u64 capacity;
+	int cpu;
+
+	if (!raw_capacity || cap_parsing_failed)
+		return;
+
+	pr_debug("cpu_capacity: capacity_scale=%u\n", capacity_scale);
+	mutex_lock(&cpu_scale_mutex);
+	for_each_possible_cpu(cpu) {
+		pr_debug("cpu_capacity: cpu=%d raw_capacity=%u\n",
+			 cpu, raw_capacity[cpu]);
+		capacity = (raw_capacity[cpu] << SCHED_CAPACITY_SHIFT)
+			/ capacity_scale;
+		topology_set_cpu_scale(cpu, capacity);
+		pr_debug("cpu_capacity: CPU%d cpu_capacity=%lu\n",
+			cpu, topology_get_cpu_scale(NULL, cpu));
+	}
+	mutex_unlock(&cpu_scale_mutex);
+}
+
+int __init topology_parse_cpu_capacity(struct device_node *cpu_node, int cpu)
+{
+	int ret = 1;
+	u32 cpu_capacity;
+
+	if (cap_parsing_failed)
+		return !ret;
+
+	ret = of_property_read_u32(cpu_node,
+				   "capacity-dmips-mhz",
+				   &cpu_capacity);
+	if (!ret) {
+		if (!raw_capacity) {
+			raw_capacity = kcalloc(num_possible_cpus(),
+					       sizeof(*raw_capacity),
+					       GFP_KERNEL);
+			if (!raw_capacity) {
+				pr_err("cpu_capacity: failed to allocate memory for raw capacities\n");
+				cap_parsing_failed = true;
+				return 0;
+			}
+		}
+		capacity_scale = max(cpu_capacity, capacity_scale);
+		raw_capacity[cpu] = cpu_capacity;
+		pr_debug("cpu_capacity: %s cpu_capacity=%u (raw)\n",
+			cpu_node->full_name, raw_capacity[cpu]);
+	} else {
+		if (raw_capacity) {
+			pr_err("cpu_capacity: missing %s raw capacity\n",
+				cpu_node->full_name);
+			pr_err("cpu_capacity: partial information: fallback to 1024 for all CPUs\n");
+		}
+		cap_parsing_failed = true;
+		kfree(raw_capacity);
+	}
+
+	return !ret;
+}
+
+#ifdef CONFIG_CPU_FREQ
+static cpumask_var_t cpus_to_visit;
+static bool cap_parsing_done;
+static void parsing_done_workfn(struct work_struct *work);
+static DECLARE_WORK(parsing_done_work, parsing_done_workfn);
+
+static int
+init_cpu_capacity_callback(struct notifier_block *nb,
+			   unsigned long val,
+			   void *data)
+{
+	struct cpufreq_policy *policy = data;
+	int cpu;
+
+	if (cap_parsing_failed || cap_parsing_done)
+		return 0;
+
+	switch (val) {
+	case CPUFREQ_NOTIFY:
+		pr_debug("cpu_capacity: init cpu capacity for CPUs [%*pbl] (to_visit=%*pbl)\n",
+				cpumask_pr_args(policy->related_cpus),
+				cpumask_pr_args(cpus_to_visit));
+		cpumask_andnot(cpus_to_visit,
+			       cpus_to_visit,
+			       policy->related_cpus);
+		for_each_cpu(cpu, policy->related_cpus) {
+			raw_capacity[cpu] = topology_get_cpu_scale(NULL, cpu) *
+					    policy->cpuinfo.max_freq / 1000UL;
+			capacity_scale = max(raw_capacity[cpu], capacity_scale);
+		}
+		if (cpumask_empty(cpus_to_visit)) {
+			topology_normalize_cpu_scale();
+			kfree(raw_capacity);
+			pr_debug("cpu_capacity: parsing done\n");
+			cap_parsing_done = true;
+			schedule_work(&parsing_done_work);
+		}
+	}
+	return 0;
+}
+
+static struct notifier_block init_cpu_capacity_notifier = {
+	.notifier_call = init_cpu_capacity_callback,
+};
+
+static int __init register_cpufreq_notifier(void)
+{
+	/*
+	 * on ACPI-based systems we need to use the default cpu capacity
+	 * until we have the necessary code to parse the cpu capacity, so
+	 * skip registering cpufreq notifier.
+	 */
+	if (!acpi_disabled || !raw_capacity)
+		return -EINVAL;
+
+	if (!alloc_cpumask_var(&cpus_to_visit, GFP_KERNEL)) {
+		pr_err("cpu_capacity: failed to allocate memory for cpus_to_visit\n");
+		return -ENOMEM;
+	}
+
+	cpumask_copy(cpus_to_visit, cpu_possible_mask);
+
+	return cpufreq_register_notifier(&init_cpu_capacity_notifier,
+					 CPUFREQ_POLICY_NOTIFIER);
+}
+core_initcall(register_cpufreq_notifier);
+
+static void parsing_done_workfn(struct work_struct *work)
+{
+	cpufreq_unregister_notifier(&init_cpu_capacity_notifier,
+					 CPUFREQ_POLICY_NOTIFIER);
+}
+
+#else
+static int __init free_raw_capacity(void)
+{
+	kfree(raw_capacity);
+
+	return 0;
+}
+core_initcall(free_raw_capacity);
+#endif
diff --git a/drivers/base/bus.c b/drivers/base/bus.c
index 6470eb8088f4..e162c9a789ba 100644
--- a/drivers/base/bus.c
+++ b/drivers/base/bus.c
@@ -466,35 +466,6 @@ int bus_for_each_drv(struct bus_type *bus, struct device_driver *start,
 }
 EXPORT_SYMBOL_GPL(bus_for_each_drv);
 
-static int device_add_attrs(struct bus_type *bus, struct device *dev)
-{
-	int error = 0;
-	int i;
-
-	if (!bus->dev_attrs)
-		return 0;
-
-	for (i = 0; bus->dev_attrs[i].attr.name; i++) {
-		error = device_create_file(dev, &bus->dev_attrs[i]);
-		if (error) {
-			while (--i >= 0)
-				device_remove_file(dev, &bus->dev_attrs[i]);
-			break;
-		}
-	}
-	return error;
-}
-
-static void device_remove_attrs(struct bus_type *bus, struct device *dev)
-{
-	int i;
-
-	if (bus->dev_attrs) {
-		for (i = 0; bus->dev_attrs[i].attr.name; i++)
-			device_remove_file(dev, &bus->dev_attrs[i]);
-	}
-}
-
 /**
  * bus_add_device - add device to bus
  * @dev: device being added
@@ -510,12 +481,9 @@ int bus_add_device(struct device *dev)
 
 	if (bus) {
 		pr_debug("bus: '%s': add device %s\n", bus->name, dev_name(dev));
-		error = device_add_attrs(bus, dev);
-		if (error)
-			goto out_put;
 		error = device_add_groups(dev, bus->dev_groups);
 		if (error)
-			goto out_id;
+			goto out_put;
 		error = sysfs_create_link(&bus->p->devices_kset->kobj,
 						&dev->kobj, dev_name(dev));
 		if (error)
@@ -532,8 +500,6 @@ out_subsys:
 	sysfs_remove_link(&bus->p->devices_kset->kobj, dev_name(dev));
 out_groups:
 	device_remove_groups(dev, bus->dev_groups);
-out_id:
-	device_remove_attrs(bus, dev);
 out_put:
 	bus_put(dev->bus);
 	return error;
@@ -590,7 +556,6 @@ void bus_remove_device(struct device *dev)
 	sysfs_remove_link(&dev->kobj, "subsystem");
 	sysfs_remove_link(&dev->bus->p->devices_kset->kobj,
 			  dev_name(dev));
-	device_remove_attrs(dev->bus, dev);
 	device_remove_groups(dev, dev->bus->dev_groups);
 	if (klist_node_attached(&dev->p->knode_bus))
 		klist_del(&dev->p->knode_bus);
@@ -648,10 +613,7 @@ static void remove_probe_files(struct bus_type *bus)
 static ssize_t uevent_store(struct device_driver *drv, const char *buf,
 			    size_t count)
 {
-	enum kobject_action action;
-
-	if (kobject_action_type(buf, count, &action) == 0)
-		kobject_uevent(&drv->p->kobj, action);
+	kobject_synth_uevent(&drv->p->kobj, buf, count);
 	return count;
 }
 static DRIVER_ATTR_WO(uevent);
@@ -868,10 +830,7 @@ static void klist_devices_put(struct klist_node *n)
 static ssize_t bus_uevent_store(struct bus_type *bus,
 				const char *buf, size_t count)
 {
-	enum kobject_action action;
-
-	if (kobject_action_type(buf, count, &action) == 0)
-		kobject_uevent(&bus->p->subsys.kobj, action);
+	kobject_synth_uevent(&bus->p->subsys.kobj, buf, count);
 	return count;
 }
 static BUS_ATTR(uevent, S_IWUSR, NULL, bus_uevent_store);
diff --git a/drivers/base/class.c b/drivers/base/class.c
index a2b2896693d6..52eb8e644acd 100644
--- a/drivers/base/class.c
+++ b/drivers/base/class.c
@@ -119,36 +119,6 @@ static void class_put(struct class *cls)
 		kset_put(&cls->p->subsys);
 }
 
-static int add_class_attrs(struct class *cls)
-{
-	int i;
-	int error = 0;
-
-	if (cls->class_attrs) {
-		for (i = 0; cls->class_attrs[i].attr.name; i++) {
-			error = class_create_file(cls, &cls->class_attrs[i]);
-			if (error)
-				goto error;
-		}
-	}
-done:
-	return error;
-error:
-	while (--i >= 0)
-		class_remove_file(cls, &cls->class_attrs[i]);
-	goto done;
-}
-
-static void remove_class_attrs(struct class *cls)
-{
-	int i;
-
-	if (cls->class_attrs) {
-		for (i = 0; cls->class_attrs[i].attr.name; i++)
-			class_remove_file(cls, &cls->class_attrs[i]);
-	}
-}
-
 static void klist_class_dev_get(struct klist_node *n)
 {
 	struct device *dev = container_of(n, struct device, knode_class);
@@ -217,8 +187,6 @@ int __class_register(struct class *cls, struct lock_class_key *key)
 	}
 	error = class_add_groups(class_get(cls), cls->class_groups);
 	class_put(cls);
-	error = add_class_attrs(class_get(cls));
-	class_put(cls);
 	return error;
 }
 EXPORT_SYMBOL_GPL(__class_register);
@@ -226,7 +194,6 @@ EXPORT_SYMBOL_GPL(__class_register);
 void class_unregister(struct class *cls)
 {
 	pr_debug("device class '%s': unregistering\n", cls->name);
-	remove_class_attrs(cls);
 	class_remove_groups(cls, cls->class_groups);
 	kset_unregister(&cls->p->subsys);
 }
diff --git a/drivers/base/core.c b/drivers/base/core.c
index bbecaf9293be..8dde934f8d15 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -981,12 +981,9 @@ out:
 static ssize_t uevent_store(struct device *dev, struct device_attribute *attr,
 			    const char *buf, size_t count)
 {
-	enum kobject_action action;
+	if (kobject_synth_uevent(&dev->kobj, buf, count))
+		dev_err(dev, "uevent: failed to send synthetic uevent\n");
 
-	if (kobject_action_type(buf, count, &action) == 0)
-		kobject_uevent(&dev->kobj, action);
-	else
-		dev_err(dev, "uevent: unknown action-string\n");
 	return count;
 }
 static DEVICE_ATTR_RW(uevent);
@@ -2884,3 +2881,19 @@ void set_secondary_fwnode(struct device *dev, struct fwnode_handle *fwnode)
 	else
 		dev->fwnode = fwnode;
 }
+
+/**
+ * device_set_of_node_from_dev - reuse device-tree node of another device
+ * @dev: device whose device-tree node is being set
+ * @dev2: device whose device-tree node is being reused
+ *
+ * Takes another reference to the new device-tree node after first dropping
+ * any reference held to the old node.
+ */
+void device_set_of_node_from_dev(struct device *dev, const struct device *dev2)
+{
+	of_node_put(dev->of_node);
+	dev->of_node = of_node_get(dev2->of_node);
+	dev->of_node_reused = true;
+}
+EXPORT_SYMBOL_GPL(device_set_of_node_from_dev);
diff --git a/drivers/base/dma-mapping.c b/drivers/base/dma-mapping.c
index f3deb6af42ad..9dbef4d1baa4 100644
--- a/drivers/base/dma-mapping.c
+++ b/drivers/base/dma-mapping.c
@@ -275,6 +275,24 @@ int dma_common_mmap(struct device *dev, struct vm_area_struct *vma,
 EXPORT_SYMBOL(dma_common_mmap);
 
 #ifdef CONFIG_MMU
+static struct vm_struct *__dma_common_pages_remap(struct page **pages,
+			size_t size, unsigned long vm_flags, pgprot_t prot,
+			const void *caller)
+{
+	struct vm_struct *area;
+
+	area = get_vm_area_caller(size, vm_flags, caller);
+	if (!area)
+		return NULL;
+
+	if (map_vm_area(area, prot, pages)) {
+		vunmap(area->addr);
+		return NULL;
+	}
+
+	return area;
+}
+
 /*
  * remaps an array of PAGE_SIZE pages into another vm_area
  * Cannot be used in non-sleeping contexts
@@ -285,17 +303,12 @@ void *dma_common_pages_remap(struct page **pages, size_t size,
 {
 	struct vm_struct *area;
 
-	area = get_vm_area_caller(size, vm_flags, caller);
+	area = __dma_common_pages_remap(pages, size, vm_flags, prot, caller);
 	if (!area)
 		return NULL;
 
 	area->pages = pages;
 
-	if (map_vm_area(area, prot, pages)) {
-		vunmap(area->addr);
-		return NULL;
-	}
-
 	return area->addr;
 }
 
@@ -310,7 +323,7 @@ void *dma_common_contiguous_remap(struct page *page, size_t size,
 {
 	int i;
 	struct page **pages;
-	void *ptr;
+	struct vm_struct *area;
 
 	pages = kmalloc(sizeof(struct page *) << get_order(size), GFP_KERNEL);
 	if (!pages)
@@ -319,11 +332,13 @@ void *dma_common_contiguous_remap(struct page *page, size_t size,
 	for (i = 0; i < (size >> PAGE_SHIFT); i++)
 		pages[i] = nth_page(page, i);
 
-	ptr = dma_common_pages_remap(pages, size, vm_flags, prot, caller);
+	area = __dma_common_pages_remap(pages, size, vm_flags, prot, caller);
 
 	kfree(pages);
 
-	return ptr;
+	if (!area)
+		return NULL;
+	return area->addr;
 }
 
 /*
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index ac350c518e0c..b9f907eedbf7 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -260,6 +260,38 @@ static int fw_cache_piggyback_on_request(const char *name);
  * guarding for corner cases a global lock should be OK */
 static DEFINE_MUTEX(fw_lock);
 
+static bool __enable_firmware = false;
+
+static void enable_firmware(void)
+{
+	mutex_lock(&fw_lock);
+	__enable_firmware = true;
+	mutex_unlock(&fw_lock);
+}
+
+static void disable_firmware(void)
+{
+	mutex_lock(&fw_lock);
+	__enable_firmware = false;
+	mutex_unlock(&fw_lock);
+}
+
+/*
+ * When disabled only the built-in firmware and the firmware cache will be
+ * used to look for firmware.
+ */
+static bool firmware_enabled(void)
+{
+	bool enabled = false;
+
+	mutex_lock(&fw_lock);
+	if (__enable_firmware)
+		enabled = true;
+	mutex_unlock(&fw_lock);
+
+	return enabled;
+}
+
 static struct firmware_cache fw_cache;
 
 static struct firmware_buf *__allocate_fw_buf(const char *fw_name,
@@ -523,6 +555,44 @@ static int fw_add_devm_name(struct device *dev, const char *name)
 }
 #endif
 
+static int assign_firmware_buf(struct firmware *fw, struct device *device,
+			       unsigned int opt_flags)
+{
+	struct firmware_buf *buf = fw->priv;
+
+	mutex_lock(&fw_lock);
+	if (!buf->size || fw_state_is_aborted(&buf->fw_st)) {
+		mutex_unlock(&fw_lock);
+		return -ENOENT;
+	}
+
+	/*
+	 * add firmware name into devres list so that we can auto cache
+	 * and uncache firmware for device.
+	 *
+	 * device may has been deleted already, but the problem
+	 * should be fixed in devres or driver core.
+	 */
+	/* don't cache firmware handled without uevent */
+	if (device && (opt_flags & FW_OPT_UEVENT) &&
+	    !(opt_flags & FW_OPT_NOCACHE))
+		fw_add_devm_name(device, buf->fw_id);
+
+	/*
+	 * After caching firmware image is started, let it piggyback
+	 * on request firmware.
+	 */
+	if (!(opt_flags & FW_OPT_NOCACHE) &&
+	    buf->fwc->state == FW_LOADER_START_CACHE) {
+		if (fw_cache_piggyback_on_request(buf->fw_id))
+			kref_get(&buf->ref);
+	}
+
+	/* pass the pages buffer to driver at the last minute */
+	fw_set_page_data(buf, fw);
+	mutex_unlock(&fw_lock);
+	return 0;
+}
 
 /*
  * user-mode helper code
@@ -562,23 +632,19 @@ static void fw_load_abort(struct firmware_priv *fw_priv)
 
 static LIST_HEAD(pending_fw_head);
 
-/* reboot notifier for avoid deadlock with usermode_lock */
-static int fw_shutdown_notify(struct notifier_block *unused1,
-			      unsigned long unused2, void *unused3)
+static void kill_pending_fw_fallback_reqs(bool only_kill_custom)
 {
+	struct firmware_buf *buf;
+	struct firmware_buf *next;
+
 	mutex_lock(&fw_lock);
-	while (!list_empty(&pending_fw_head))
-		__fw_load_abort(list_first_entry(&pending_fw_head,
-					       struct firmware_buf,
-					       pending_list));
+	list_for_each_entry_safe(buf, next, &pending_fw_head, pending_list) {
+		if (!buf->need_uevent || !only_kill_custom)
+			 __fw_load_abort(buf);
+	}
 	mutex_unlock(&fw_lock);
-	return NOTIFY_DONE;
 }
 
-static struct notifier_block fw_shutdown_nb = {
-	.notifier_call = fw_shutdown_notify,
-};
-
 static ssize_t timeout_show(struct class *class, struct class_attribute *attr,
 			    char *buf)
 {
@@ -1036,46 +1102,56 @@ err_put_dev:
 
 static int fw_load_from_user_helper(struct firmware *firmware,
 				    const char *name, struct device *device,
-				    unsigned int opt_flags, long timeout)
+				    unsigned int opt_flags)
 {
 	struct firmware_priv *fw_priv;
+	long timeout;
+	int ret;
+
+	timeout = firmware_loading_timeout();
+	if (opt_flags & FW_OPT_NOWAIT) {
+		timeout = usermodehelper_read_lock_wait(timeout);
+		if (!timeout) {
+			dev_dbg(device, "firmware: %s loading timed out\n",
+				name);
+			return -EBUSY;
+		}
+	} else {
+		ret = usermodehelper_read_trylock();
+		if (WARN_ON(ret)) {
+			dev_err(device, "firmware: %s will not be loaded\n",
+				name);
+			return ret;
+		}
+	}
 
 	fw_priv = fw_create_instance(firmware, name, device, opt_flags);
-	if (IS_ERR(fw_priv))
-		return PTR_ERR(fw_priv);
+	if (IS_ERR(fw_priv)) {
+		ret = PTR_ERR(fw_priv);
+		goto out_unlock;
+	}
 
 	fw_priv->buf = firmware->priv;
-	return _request_firmware_load(fw_priv, opt_flags, timeout);
-}
+	ret = _request_firmware_load(fw_priv, opt_flags, timeout);
 
-#ifdef CONFIG_PM_SLEEP
-/* kill pending requests without uevent to avoid blocking suspend */
-static void kill_requests_without_uevent(void)
-{
-	struct firmware_buf *buf;
-	struct firmware_buf *next;
+	if (!ret)
+		ret = assign_firmware_buf(firmware, device, opt_flags);
 
-	mutex_lock(&fw_lock);
-	list_for_each_entry_safe(buf, next, &pending_fw_head, pending_list) {
-		if (!buf->need_uevent)
-			 __fw_load_abort(buf);
-	}
-	mutex_unlock(&fw_lock);
+out_unlock:
+	usermodehelper_read_unlock();
+
+	return ret;
 }
-#endif
 
 #else /* CONFIG_FW_LOADER_USER_HELPER */
 static inline int
 fw_load_from_user_helper(struct firmware *firmware, const char *name,
-			 struct device *device, unsigned int opt_flags,
-			 long timeout)
+			 struct device *device, unsigned int opt_flags)
 {
 	return -ENOENT;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static inline void kill_requests_without_uevent(void) { }
-#endif
+static inline void kill_pending_fw_fallback_reqs(bool only_kill_custom) { }
 
 #endif /* CONFIG_FW_LOADER_USER_HELPER */
 
@@ -1124,45 +1200,6 @@ _request_firmware_prepare(struct firmware **firmware_p, const char *name,
 	return 1; /* need to load */
 }
 
-static int assign_firmware_buf(struct firmware *fw, struct device *device,
-			       unsigned int opt_flags)
-{
-	struct firmware_buf *buf = fw->priv;
-
-	mutex_lock(&fw_lock);
-	if (!buf->size || fw_state_is_aborted(&buf->fw_st)) {
-		mutex_unlock(&fw_lock);
-		return -ENOENT;
-	}
-
-	/*
-	 * add firmware name into devres list so that we can auto cache
-	 * and uncache firmware for device.
-	 *
-	 * device may has been deleted already, but the problem
-	 * should be fixed in devres or driver core.
-	 */
-	/* don't cache firmware handled without uevent */
-	if (device && (opt_flags & FW_OPT_UEVENT) &&
-	    !(opt_flags & FW_OPT_NOCACHE))
-		fw_add_devm_name(device, buf->fw_id);
-
-	/*
-	 * After caching firmware image is started, let it piggyback
-	 * on request firmware.
-	 */
-	if (!(opt_flags & FW_OPT_NOCACHE) &&
-	    buf->fwc->state == FW_LOADER_START_CACHE) {
-		if (fw_cache_piggyback_on_request(buf->fw_id))
-			kref_get(&buf->ref);
-	}
-
-	/* pass the pages buffer to driver at the last minute */
-	fw_set_page_data(buf, fw);
-	mutex_unlock(&fw_lock);
-	return 0;
-}
-
 /* called from request_firmware() and request_firmware_work_func() */
 static int
 _request_firmware(const struct firmware **firmware_p, const char *name,
@@ -1170,7 +1207,6 @@ _request_firmware(const struct firmware **firmware_p, const char *name,
 		  unsigned int opt_flags)
 {
 	struct firmware *fw = NULL;
-	long timeout;
 	int ret;
 
 	if (!firmware_p)
@@ -1185,23 +1221,10 @@ _request_firmware(const struct firmware **firmware_p, const char *name,
 	if (ret <= 0) /* error or already assigned */
 		goto out;
 
-	ret = 0;
-	timeout = firmware_loading_timeout();
-	if (opt_flags & FW_OPT_NOWAIT) {
-		timeout = usermodehelper_read_lock_wait(timeout);
-		if (!timeout) {
-			dev_dbg(device, "firmware: %s loading timed out\n",
-				name);
-			ret = -EBUSY;
-			goto out;
-		}
-	} else {
-		ret = usermodehelper_read_trylock();
-		if (WARN_ON(ret)) {
-			dev_err(device, "firmware: %s will not be loaded\n",
-				name);
-			goto out;
-		}
+	if (!firmware_enabled()) {
+		WARN(1, "firmware request while host is not available\n");
+		ret = -EHOSTDOWN;
+		goto out;
 	}
 
 	ret = fw_get_filesystem_firmware(device, fw->priv);
@@ -1213,15 +1236,11 @@ _request_firmware(const struct firmware **firmware_p, const char *name,
 		if (opt_flags & FW_OPT_USERHELPER) {
 			dev_warn(device, "Falling back to user helper\n");
 			ret = fw_load_from_user_helper(fw, name, device,
-						       opt_flags, timeout);
+						       opt_flags);
 		}
-	}
-
-	if (!ret)
+	} else
 		ret = assign_firmware_buf(fw, device, opt_flags);
 
-	usermodehelper_read_unlock();
-
  out:
 	if (ret < 0) {
 		release_firmware(fw);
@@ -1717,6 +1736,62 @@ static void device_uncache_fw_images_delay(unsigned long delay)
 			   msecs_to_jiffies(delay));
 }
 
+/**
+ * fw_pm_notify - notifier for suspend/resume
+ * @notify_block: unused
+ * @mode: mode we are switching to
+ * @unused: unused
+ *
+ * Used to modify the firmware_class state as we move in between states.
+ * The firmware_class implements a firmware cache to enable device driver
+ * to fetch firmware upon resume before the root filesystem is ready. We
+ * disable API calls which do not use the built-in firmware or the firmware
+ * cache when we know these calls will not work.
+ *
+ * The inner logic behind all this is a bit complex so it is worth summarizing
+ * the kernel's own suspend/resume process with context and focus on how this
+ * can impact the firmware API.
+ *
+ * First a review on how we go to suspend::
+ *
+ *	pm_suspend() --> enter_state() -->
+ *	sys_sync()
+ *	suspend_prepare() -->
+ *		__pm_notifier_call_chain(PM_SUSPEND_PREPARE, ...);
+ *		suspend_freeze_processes() -->
+ *			freeze_processes() -->
+ *				__usermodehelper_set_disable_depth(UMH_DISABLED);
+ *				freeze all tasks ...
+ *			freeze_kernel_threads()
+ *	suspend_devices_and_enter() -->
+ *		dpm_suspend_start() -->
+ *				dpm_prepare()
+ *				dpm_suspend()
+ *		suspend_enter()  -->
+ *			platform_suspend_prepare()
+ *			dpm_suspend_late()
+ *			freeze_enter()
+ *			syscore_suspend()
+ *
+ * When we resume we bail out of a loop from suspend_devices_and_enter() and
+ * unwind back out to the caller enter_state() where we were before as follows::
+ *
+ * 	enter_state() -->
+ *	suspend_devices_and_enter() --> (bail from loop)
+ *		dpm_resume_end() -->
+ *			dpm_resume()
+ *			dpm_complete()
+ *	suspend_finish() -->
+ *		suspend_thaw_processes() -->
+ *			thaw_processes() -->
+ *				__usermodehelper_set_disable_depth(UMH_FREEZING);
+ *				thaw_workqueues();
+ *				thaw all processes ...
+ *				usermodehelper_enable();
+ *		pm_notifier_call_chain(PM_POST_SUSPEND);
+ *
+ * fw_pm_notify() works through pm_notifier_call_chain().
+ */
 static int fw_pm_notify(struct notifier_block *notify_block,
 			unsigned long mode, void *unused)
 {
@@ -1724,8 +1799,13 @@ static int fw_pm_notify(struct notifier_block *notify_block,
 	case PM_HIBERNATION_PREPARE:
 	case PM_SUSPEND_PREPARE:
 	case PM_RESTORE_PREPARE:
-		kill_requests_without_uevent();
+		/*
+		 * kill pending fallback requests with a custom fallback
+		 * to avoid stalling suspend.
+		 */
+		kill_pending_fw_fallback_reqs(true);
 		device_cache_fw_images();
+		disable_firmware();
 		break;
 
 	case PM_POST_SUSPEND:
@@ -1738,6 +1818,7 @@ static int fw_pm_notify(struct notifier_block *notify_block,
 		mutex_lock(&fw_lock);
 		fw_cache.state = FW_LOADER_NO_CACHE;
 		mutex_unlock(&fw_lock);
+		enable_firmware();
 
 		device_uncache_fw_images_delay(10 * MSEC_PER_SEC);
 		break;
@@ -1783,11 +1864,29 @@ static void __init fw_cache_init(void)
 #endif
 }
 
+static int fw_shutdown_notify(struct notifier_block *unused1,
+			      unsigned long unused2, void *unused3)
+{
+	disable_firmware();
+	/*
+	 * Kill all pending fallback requests to avoid both stalling shutdown,
+	 * and avoid a deadlock with the usermode_lock.
+	 */
+	kill_pending_fw_fallback_reqs(false);
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block fw_shutdown_nb = {
+	.notifier_call = fw_shutdown_notify,
+};
+
 static int __init firmware_class_init(void)
 {
+	enable_firmware();
 	fw_cache_init();
-#ifdef CONFIG_FW_LOADER_USER_HELPER
 	register_reboot_notifier(&fw_shutdown_nb);
+#ifdef CONFIG_FW_LOADER_USER_HELPER
 	return class_register(&firmware_class);
 #else
 	return 0;
@@ -1796,12 +1895,13 @@ static int __init firmware_class_init(void)
 
 static void __exit firmware_class_exit(void)
 {
+	disable_firmware();
 #ifdef CONFIG_PM_SLEEP
 	unregister_syscore_ops(&fw_syscore_ops);
 	unregister_pm_notifier(&fw_cache.pm_notify);
 #endif
-#ifdef CONFIG_FW_LOADER_USER_HELPER
 	unregister_reboot_notifier(&fw_shutdown_nb);
+#ifdef CONFIG_FW_LOADER_USER_HELPER
 	class_unregister(&firmware_class);
 #endif
 }
diff --git a/drivers/base/node.c b/drivers/base/node.c
index 5548f9686016..0440d95c9b5b 100644
--- a/drivers/base/node.c
+++ b/drivers/base/node.c
@@ -377,7 +377,7 @@ static int __ref get_nid_for_pfn(unsigned long pfn)
 	if (!pfn_valid_within(pfn))
 		return -1;
 #ifdef CONFIG_DEFERRED_STRUCT_PAGE_INIT
-	if (system_state == SYSTEM_BOOTING)
+	if (system_state < SYSTEM_RUNNING)
 		return early_pfn_to_nid(pfn);
 #endif
 	page = pfn_to_page(pfn);
diff --git a/drivers/base/pinctrl.c b/drivers/base/pinctrl.c
index 5917b4b5fb99..eb929dd6ef1e 100644
--- a/drivers/base/pinctrl.c
+++ b/drivers/base/pinctrl.c
@@ -23,6 +23,9 @@ int pinctrl_bind_pins(struct device *dev)
 {
 	int ret;
 
+	if (dev->of_node_reused)
+		return 0;
+
 	dev->pins = devm_kzalloc(dev, sizeof(*(dev->pins)), GFP_KERNEL);
 	if (!dev->pins)
 		return -ENOMEM;
diff --git a/drivers/base/platform-msi.c b/drivers/base/platform-msi.c
index d35e9a20caf7..e5473525e7b2 100644
--- a/drivers/base/platform-msi.c
+++ b/drivers/base/platform-msi.c
@@ -195,7 +195,7 @@ struct irq_domain *platform_msi_create_irq_domain(struct fwnode_handle *fwnode,
 
 	domain = msi_create_irq_domain(fwnode, info, parent);
 	if (domain)
-		domain->bus_token = DOMAIN_BUS_PLATFORM_MSI;
+		irq_domain_update_bus_token(domain, DOMAIN_BUS_PLATFORM_MSI);
 
 	return domain;
 }
diff --git a/drivers/base/platform.c b/drivers/base/platform.c
index a102152301c8..97332d094fe2 100644
--- a/drivers/base/platform.c
+++ b/drivers/base/platform.c
@@ -866,7 +866,7 @@ static ssize_t driver_override_store(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct platform_device *pdev = to_platform_device(dev);
-	char *driver_override, *old = pdev->driver_override, *cp;
+	char *driver_override, *old, *cp;
 
 	if (count > PATH_MAX)
 		return -EINVAL;
@@ -879,12 +879,15 @@ static ssize_t driver_override_store(struct device *dev,
 	if (cp)
 		*cp = '\0';
 
+	device_lock(dev);
+	old = pdev->driver_override;
 	if (strlen(driver_override)) {
 		pdev->driver_override = driver_override;
 	} else {
 		kfree(driver_override);
 		pdev->driver_override = NULL;
 	}
+	device_unlock(dev);
 
 	kfree(old);
 
@@ -895,8 +898,12 @@ static ssize_t driver_override_show(struct device *dev,
 				    struct device_attribute *attr, char *buf)
 {
 	struct platform_device *pdev = to_platform_device(dev);
+	ssize_t len;
 
-	return sprintf(buf, "%s\n", pdev->driver_override);
+	device_lock(dev);
+	len = sprintf(buf, "%s\n", pdev->driver_override);
+	device_unlock(dev);
+	return len;
 }
 static DEVICE_ATTR_RW(driver_override);
 
diff --git a/drivers/base/power/domain.c b/drivers/base/power/domain.c
index da49a8383dc3..9649dce63e19 100644
--- a/drivers/base/power/domain.c
+++ b/drivers/base/power/domain.c
@@ -126,7 +126,7 @@ static const struct genpd_lock_ops genpd_spin_ops = {
 #define genpd_is_always_on(genpd)	(genpd->flags & GENPD_FLAG_ALWAYS_ON)
 
 static inline bool irq_safe_dev_in_no_sleep_domain(struct device *dev,
-		struct generic_pm_domain *genpd)
+		const struct generic_pm_domain *genpd)
 {
 	bool ret;
 
@@ -181,12 +181,14 @@ static struct generic_pm_domain *dev_to_genpd(struct device *dev)
 	return pd_to_genpd(dev->pm_domain);
 }
 
-static int genpd_stop_dev(struct generic_pm_domain *genpd, struct device *dev)
+static int genpd_stop_dev(const struct generic_pm_domain *genpd,
+			  struct device *dev)
 {
 	return GENPD_DEV_CALLBACK(genpd, int, stop, dev);
 }
 
-static int genpd_start_dev(struct generic_pm_domain *genpd, struct device *dev)
+static int genpd_start_dev(const struct generic_pm_domain *genpd,
+			   struct device *dev)
 {
 	return GENPD_DEV_CALLBACK(genpd, int, start, dev);
 }
@@ -443,7 +445,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 && pdd->dev) {
+		if (pdd) {
 			to_gpd_data(pdd)->td.constraint_changed = true;
 			genpd = dev_to_genpd(dev);
 		} else {
@@ -738,7 +740,7 @@ static bool pm_genpd_present(const struct generic_pm_domain *genpd)
 
 #ifdef CONFIG_PM_SLEEP
 
-static bool genpd_dev_active_wakeup(struct generic_pm_domain *genpd,
+static bool genpd_dev_active_wakeup(const struct generic_pm_domain *genpd,
 				    struct device *dev)
 {
 	return GENPD_DEV_CALLBACK(genpd, bool, active_wakeup, dev);
@@ -840,7 +842,8 @@ static void genpd_sync_power_on(struct generic_pm_domain *genpd, bool use_lock,
  * signal remote wakeup from the system's working state as needed by runtime PM.
  * Return 'true' in either of the above cases.
  */
-static bool resume_needed(struct device *dev, struct generic_pm_domain *genpd)
+static bool resume_needed(struct device *dev,
+			  const struct generic_pm_domain *genpd)
 {
 	bool active_wakeup;
 
@@ -899,19 +902,19 @@ static int pm_genpd_prepare(struct device *dev)
 }
 
 /**
- * pm_genpd_suspend_noirq - Completion of suspend of device in an I/O PM domain.
+ * genpd_finish_suspend - Completion of suspend or hibernation of device in an
+ *   I/O pm domain.
  * @dev: Device to suspend.
+ * @poweroff: Specifies if this is a poweroff_noirq or suspend_noirq callback.
  *
  * Stop the device and remove power from the domain if all devices in it have
  * been stopped.
  */
-static int pm_genpd_suspend_noirq(struct device *dev)
+static int genpd_finish_suspend(struct device *dev, bool poweroff)
 {
 	struct generic_pm_domain *genpd;
 	int ret;
 
-	dev_dbg(dev, "%s()\n", __func__);
-
 	genpd = dev_to_genpd(dev);
 	if (IS_ERR(genpd))
 		return -EINVAL;
@@ -919,6 +922,13 @@ static int pm_genpd_suspend_noirq(struct device *dev)
 	if (dev->power.wakeup_path && genpd_dev_active_wakeup(genpd, dev))
 		return 0;
 
+	if (poweroff)
+		ret = pm_generic_poweroff_noirq(dev);
+	else
+		ret = pm_generic_suspend_noirq(dev);
+	if (ret)
+		return ret;
+
 	if (genpd->dev_ops.stop && genpd->dev_ops.start) {
 		ret = pm_runtime_force_suspend(dev);
 		if (ret)
@@ -934,6 +944,20 @@ static int pm_genpd_suspend_noirq(struct device *dev)
 }
 
 /**
+ * pm_genpd_suspend_noirq - Completion of suspend of device in an I/O PM domain.
+ * @dev: Device to suspend.
+ *
+ * Stop the device and remove power from the domain if all devices in it have
+ * been stopped.
+ */
+static int pm_genpd_suspend_noirq(struct device *dev)
+{
+	dev_dbg(dev, "%s()\n", __func__);
+
+	return genpd_finish_suspend(dev, false);
+}
+
+/**
  * pm_genpd_resume_noirq - Start of resume of device in an I/O PM domain.
  * @dev: Device to resume.
  *
@@ -961,6 +985,10 @@ static int pm_genpd_resume_noirq(struct device *dev)
 	if (genpd->dev_ops.stop && genpd->dev_ops.start)
 		ret = pm_runtime_force_resume(dev);
 
+	ret = pm_generic_resume_noirq(dev);
+	if (ret)
+		return ret;
+
 	return ret;
 }
 
@@ -975,7 +1003,7 @@ static int pm_genpd_resume_noirq(struct device *dev)
  */
 static int pm_genpd_freeze_noirq(struct device *dev)
 {
-	struct generic_pm_domain *genpd;
+	const struct generic_pm_domain *genpd;
 	int ret = 0;
 
 	dev_dbg(dev, "%s()\n", __func__);
@@ -984,6 +1012,10 @@ static int pm_genpd_freeze_noirq(struct device *dev)
 	if (IS_ERR(genpd))
 		return -EINVAL;
 
+	ret = pm_generic_freeze_noirq(dev);
+	if (ret)
+		return ret;
+
 	if (genpd->dev_ops.stop && genpd->dev_ops.start)
 		ret = pm_runtime_force_suspend(dev);
 
@@ -999,7 +1031,7 @@ static int pm_genpd_freeze_noirq(struct device *dev)
  */
 static int pm_genpd_thaw_noirq(struct device *dev)
 {
-	struct generic_pm_domain *genpd;
+	const struct generic_pm_domain *genpd;
 	int ret = 0;
 
 	dev_dbg(dev, "%s()\n", __func__);
@@ -1008,10 +1040,28 @@ static int pm_genpd_thaw_noirq(struct device *dev)
 	if (IS_ERR(genpd))
 		return -EINVAL;
 
-	if (genpd->dev_ops.stop && genpd->dev_ops.start)
+	if (genpd->dev_ops.stop && genpd->dev_ops.start) {
 		ret = pm_runtime_force_resume(dev);
+		if (ret)
+			return ret;
+	}
 
-	return ret;
+	return pm_generic_thaw_noirq(dev);
+}
+
+/**
+ * pm_genpd_poweroff_noirq - Completion of hibernation of device in an
+ *   I/O PM domain.
+ * @dev: Device to poweroff.
+ *
+ * Stop the device and remove power from the domain if all devices in it have
+ * been stopped.
+ */
+static int pm_genpd_poweroff_noirq(struct device *dev)
+{
+	dev_dbg(dev, "%s()\n", __func__);
+
+	return genpd_finish_suspend(dev, true);
 }
 
 /**
@@ -1048,10 +1098,13 @@ static int pm_genpd_restore_noirq(struct device *dev)
 	genpd_sync_power_on(genpd, true, 0);
 	genpd_unlock(genpd);
 
-	if (genpd->dev_ops.stop && genpd->dev_ops.start)
+	if (genpd->dev_ops.stop && genpd->dev_ops.start) {
 		ret = pm_runtime_force_resume(dev);
+		if (ret)
+			return ret;
+	}
 
-	return ret;
+	return pm_generic_restore_noirq(dev);
 }
 
 /**
@@ -1095,8 +1148,8 @@ static void genpd_syscore_switch(struct device *dev, bool suspend)
 {
 	struct generic_pm_domain *genpd;
 
-	genpd = dev_to_genpd(dev);
-	if (!pm_genpd_present(genpd))
+	genpd = genpd_lookup_dev(dev);
+	if (!genpd)
 		return;
 
 	if (suspend) {
@@ -1393,7 +1446,7 @@ EXPORT_SYMBOL_GPL(pm_genpd_add_subdomain);
 int pm_genpd_remove_subdomain(struct generic_pm_domain *genpd,
 			      struct generic_pm_domain *subdomain)
 {
-	struct gpd_link *link;
+	struct gpd_link *l, *link;
 	int ret = -EINVAL;
 
 	if (IS_ERR_OR_NULL(genpd) || IS_ERR_OR_NULL(subdomain))
@@ -1409,7 +1462,7 @@ int pm_genpd_remove_subdomain(struct generic_pm_domain *genpd,
 		goto out;
 	}
 
-	list_for_each_entry(link, &genpd->master_links, master_node) {
+	list_for_each_entry_safe(link, l, &genpd->master_links, master_node) {
 		if (link->slave != subdomain)
 			continue;
 
@@ -1493,7 +1546,7 @@ int pm_genpd_init(struct generic_pm_domain *genpd,
 	genpd->domain.ops.resume_noirq = pm_genpd_resume_noirq;
 	genpd->domain.ops.freeze_noirq = pm_genpd_freeze_noirq;
 	genpd->domain.ops.thaw_noirq = pm_genpd_thaw_noirq;
-	genpd->domain.ops.poweroff_noirq = pm_genpd_suspend_noirq;
+	genpd->domain.ops.poweroff_noirq = pm_genpd_poweroff_noirq;
 	genpd->domain.ops.restore_noirq = pm_genpd_restore_noirq;
 	genpd->domain.ops.complete = pm_genpd_complete;
 
@@ -1584,9 +1637,6 @@ EXPORT_SYMBOL_GPL(pm_genpd_remove);
 
 #ifdef CONFIG_PM_GENERIC_DOMAINS_OF
 
-typedef struct generic_pm_domain *(*genpd_xlate_t)(struct of_phandle_args *args,
-						   void *data);
-
 /*
  * Device Tree based PM domain providers.
  *
@@ -1742,6 +1792,9 @@ int of_genpd_add_provider_onecell(struct device_node *np,
 
 	mutex_lock(&gpd_list_lock);
 
+	if (!data->xlate)
+		data->xlate = genpd_xlate_onecell;
+
 	for (i = 0; i < data->num_domains; i++) {
 		if (!data->domains[i])
 			continue;
@@ -1752,7 +1805,7 @@ int of_genpd_add_provider_onecell(struct device_node *np,
 		data->domains[i]->has_provider = true;
 	}
 
-	ret = genpd_add_provider(np, genpd_xlate_onecell, data);
+	ret = genpd_add_provider(np, data->xlate, data);
 	if (ret < 0)
 		goto error;
 
@@ -1780,12 +1833,12 @@ EXPORT_SYMBOL_GPL(of_genpd_add_provider_onecell);
  */
 void of_genpd_del_provider(struct device_node *np)
 {
-	struct of_genpd_provider *cp;
+	struct of_genpd_provider *cp, *tmp;
 	struct generic_pm_domain *gpd;
 
 	mutex_lock(&gpd_list_lock);
 	mutex_lock(&of_genpd_mutex);
-	list_for_each_entry(cp, &of_genpd_providers, link) {
+	list_for_each_entry_safe(cp, tmp, &of_genpd_providers, link) {
 		if (cp->node == np) {
 			/*
 			 * For each PM domain associated with the
@@ -1925,14 +1978,14 @@ EXPORT_SYMBOL_GPL(of_genpd_add_subdomain);
  */
 struct generic_pm_domain *of_genpd_remove_last(struct device_node *np)
 {
-	struct generic_pm_domain *gpd, *genpd = ERR_PTR(-ENOENT);
+	struct generic_pm_domain *gpd, *tmp, *genpd = ERR_PTR(-ENOENT);
 	int ret;
 
 	if (IS_ERR_OR_NULL(np))
 		return ERR_PTR(-EINVAL);
 
 	mutex_lock(&gpd_list_lock);
-	list_for_each_entry(gpd, &gpd_list, gpd_list_node) {
+	list_for_each_entry_safe(gpd, tmp, &gpd_list, gpd_list_node) {
 		if (gpd->provider == &np->fwnode) {
 			ret = genpd_remove(gpd);
 			genpd = ret ? ERR_PTR(ret) : gpd;
diff --git a/drivers/base/power/domain_governor.c b/drivers/base/power/domain_governor.c
index 2e0fce711135..281f949c5ffe 100644
--- a/drivers/base/power/domain_governor.c
+++ b/drivers/base/power/domain_governor.c
@@ -92,12 +92,6 @@ static bool default_suspend_ok(struct device *dev)
 	return td->cached_suspend_ok;
 }
 
-/**
- * default_power_down_ok - Default generic PM domain power off governor routine.
- * @pd: PM domain to check.
- *
- * This routine must be executed under the PM domain's lock.
- */
 static bool __default_power_down_ok(struct dev_pm_domain *pd,
 				     unsigned int state)
 {
@@ -187,6 +181,12 @@ static bool __default_power_down_ok(struct dev_pm_domain *pd,
 	return true;
 }
 
+/**
+ * default_power_down_ok - Default generic PM domain power off governor routine.
+ * @pd: PM domain to check.
+ *
+ * This routine must be executed under the PM domain's lock.
+ */
 static bool default_power_down_ok(struct dev_pm_domain *pd)
 {
 	struct generic_pm_domain *genpd = pd_to_genpd(pd);
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index 9faee1c893e5..c99f8730de82 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -62,7 +62,7 @@ static pm_message_t pm_transition;
 
 static int async_error;
 
-static char *pm_verb(int event)
+static const char *pm_verb(int event)
 {
 	switch (event) {
 	case PM_EVENT_SUSPEND:
@@ -208,7 +208,8 @@ static ktime_t initcall_debug_start(struct device *dev)
 }
 
 static void initcall_debug_report(struct device *dev, ktime_t calltime,
-				  int error, pm_message_t state, char *info)
+				  int error, pm_message_t state,
+				  const char *info)
 {
 	ktime_t rettime;
 	s64 nsecs;
@@ -403,21 +404,23 @@ static pm_callback_t pm_noirq_op(const struct dev_pm_ops *ops, pm_message_t stat
 	return NULL;
 }
 
-static void pm_dev_dbg(struct device *dev, pm_message_t state, char *info)
+static void pm_dev_dbg(struct device *dev, pm_message_t state, const char *info)
 {
 	dev_dbg(dev, "%s%s%s\n", info, pm_verb(state.event),
 		((state.event & PM_EVENT_SLEEP) && device_may_wakeup(dev)) ?
 		", may wakeup" : "");
 }
 
-static void pm_dev_err(struct device *dev, pm_message_t state, char *info,
+static void pm_dev_err(struct device *dev, pm_message_t state, const char *info,
 			int error)
 {
 	printk(KERN_ERR "PM: Device %s failed to %s%s: error %d\n",
 		dev_name(dev), pm_verb(state.event), info, error);
 }
 
-static void dpm_show_time(ktime_t starttime, pm_message_t state, char *info)
+#ifdef CONFIG_PM_DEBUG
+static void dpm_show_time(ktime_t starttime, pm_message_t state,
+			  const char *info)
 {
 	ktime_t calltime;
 	u64 usecs64;
@@ -433,9 +436,13 @@ static void dpm_show_time(ktime_t starttime, pm_message_t state, char *info)
 		info ?: "", info ? " " : "", pm_verb(state.event),
 		usecs / USEC_PER_MSEC, usecs % USEC_PER_MSEC);
 }
+#else
+static inline void dpm_show_time(ktime_t starttime, pm_message_t state,
+				 const char *info) {}
+#endif /* CONFIG_PM_DEBUG */
 
 static int dpm_run_callback(pm_callback_t cb, struct device *dev,
-			    pm_message_t state, char *info)
+			    pm_message_t state, const char *info)
 {
 	ktime_t calltime;
 	int error;
@@ -535,7 +542,7 @@ static void dpm_watchdog_clear(struct dpm_watchdog *wd)
 static int device_resume_noirq(struct device *dev, pm_message_t state, bool async)
 {
 	pm_callback_t callback = NULL;
-	char *info = NULL;
+	const char *info = NULL;
 	int error = 0;
 
 	TRACE_DEVICE(dev);
@@ -665,7 +672,7 @@ void dpm_resume_noirq(pm_message_t state)
 static int device_resume_early(struct device *dev, pm_message_t state, bool async)
 {
 	pm_callback_t callback = NULL;
-	char *info = NULL;
+	const char *info = NULL;
 	int error = 0;
 
 	TRACE_DEVICE(dev);
@@ -793,7 +800,7 @@ EXPORT_SYMBOL_GPL(dpm_resume_start);
 static int device_resume(struct device *dev, pm_message_t state, bool async)
 {
 	pm_callback_t callback = NULL;
-	char *info = NULL;
+	const char *info = NULL;
 	int error = 0;
 	DECLARE_DPM_WATCHDOG_ON_STACK(wd);
 
@@ -955,7 +962,7 @@ void dpm_resume(pm_message_t state)
 static void device_complete(struct device *dev, pm_message_t state)
 {
 	void (*callback)(struct device *) = NULL;
-	char *info = NULL;
+	const char *info = NULL;
 
 	if (dev->power.syscore)
 		return;
@@ -1080,7 +1087,7 @@ static pm_message_t resume_event(pm_message_t sleep_state)
 static int __device_suspend_noirq(struct device *dev, pm_message_t state, bool async)
 {
 	pm_callback_t callback = NULL;
-	char *info = NULL;
+	const char *info = NULL;
 	int error = 0;
 
 	TRACE_DEVICE(dev);
@@ -1091,11 +1098,6 @@ static int __device_suspend_noirq(struct device *dev, pm_message_t state, bool a
 	if (async_error)
 		goto Complete;
 
-	if (pm_wakeup_pending()) {
-		async_error = -EBUSY;
-		goto Complete;
-	}
-
 	if (dev->power.syscore || dev->power.direct_complete)
 		goto Complete;
 
@@ -1225,7 +1227,7 @@ int dpm_suspend_noirq(pm_message_t state)
 static int __device_suspend_late(struct device *dev, pm_message_t state, bool async)
 {
 	pm_callback_t callback = NULL;
-	char *info = NULL;
+	const char *info = NULL;
 	int error = 0;
 
 	TRACE_DEVICE(dev);
@@ -1384,7 +1386,7 @@ EXPORT_SYMBOL_GPL(dpm_suspend_end);
  */
 static int legacy_suspend(struct device *dev, pm_message_t state,
 			  int (*cb)(struct device *dev, pm_message_t state),
-			  char *info)
+			  const char *info)
 {
 	int error;
 	ktime_t calltime;
@@ -1426,7 +1428,7 @@ static void dpm_clear_suppliers_direct_complete(struct device *dev)
 static int __device_suspend(struct device *dev, pm_message_t state, bool async)
 {
 	pm_callback_t callback = NULL;
-	char *info = NULL;
+	const char *info = NULL;
 	int error = 0;
 	DECLARE_DPM_WATCHDOG_ON_STACK(wd);
 
diff --git a/drivers/base/power/opp/core.c b/drivers/base/power/opp/core.c
index dae61720b314..a8cc14fd8ae4 100644
--- a/drivers/base/power/opp/core.c
+++ b/drivers/base/power/opp/core.c
@@ -180,7 +180,7 @@ unsigned long dev_pm_opp_get_max_volt_latency(struct device *dev)
 {
 	struct opp_table *opp_table;
 	struct dev_pm_opp *opp;
-	struct regulator *reg, **regulators;
+	struct regulator *reg;
 	unsigned long latency_ns = 0;
 	int ret, i, count;
 	struct {
@@ -198,15 +198,9 @@ unsigned long dev_pm_opp_get_max_volt_latency(struct device *dev)
 	if (!count)
 		goto put_opp_table;
 
-	regulators = kmalloc_array(count, sizeof(*regulators), GFP_KERNEL);
-	if (!regulators)
-		goto put_opp_table;
-
 	uV = kmalloc_array(count, sizeof(*uV), GFP_KERNEL);
 	if (!uV)
-		goto free_regulators;
-
-	memcpy(regulators, opp_table->regulators, count * sizeof(*regulators));
+		goto put_opp_table;
 
 	mutex_lock(&opp_table->lock);
 
@@ -232,15 +226,13 @@ unsigned long dev_pm_opp_get_max_volt_latency(struct device *dev)
 	 * isn't freed, while we are executing this routine.
 	 */
 	for (i = 0; i < count; i++) {
-		reg = regulators[i];
+		reg = opp_table->regulators[i];
 		ret = regulator_set_voltage_time(reg, uV[i].min, uV[i].max);
 		if (ret > 0)
 			latency_ns += ret * 1000;
 	}
 
 	kfree(uV);
-free_regulators:
-	kfree(regulators);
 put_opp_table:
 	dev_pm_opp_put_opp_table(opp_table);
 
@@ -543,17 +535,18 @@ _generic_set_opp_clk_only(struct device *dev, struct clk *clk,
 	return ret;
 }
 
-static int _generic_set_opp(struct dev_pm_set_opp_data *data)
+static int _generic_set_opp_regulator(const struct opp_table *opp_table,
+				      struct device *dev,
+				      unsigned long old_freq,
+				      unsigned long freq,
+				      struct dev_pm_opp_supply *old_supply,
+				      struct dev_pm_opp_supply *new_supply)
 {
-	struct dev_pm_opp_supply *old_supply = data->old_opp.supplies;
-	struct dev_pm_opp_supply *new_supply = data->new_opp.supplies;
-	unsigned long old_freq = data->old_opp.rate, freq = data->new_opp.rate;
-	struct regulator *reg = data->regulators[0];
-	struct device *dev= data->dev;
+	struct regulator *reg = opp_table->regulators[0];
 	int ret;
 
 	/* This function only supports single regulator per device */
-	if (WARN_ON(data->regulator_count > 1)) {
+	if (WARN_ON(opp_table->regulator_count > 1)) {
 		dev_err(dev, "multiple regulators are not supported\n");
 		return -EINVAL;
 	}
@@ -566,7 +559,7 @@ static int _generic_set_opp(struct dev_pm_set_opp_data *data)
 	}
 
 	/* Change frequency */
-	ret = _generic_set_opp_clk_only(dev, data->clk, old_freq, freq);
+	ret = _generic_set_opp_clk_only(dev, opp_table->clk, old_freq, freq);
 	if (ret)
 		goto restore_voltage;
 
@@ -580,12 +573,12 @@ static int _generic_set_opp(struct dev_pm_set_opp_data *data)
 	return 0;
 
 restore_freq:
-	if (_generic_set_opp_clk_only(dev, data->clk, freq, old_freq))
+	if (_generic_set_opp_clk_only(dev, opp_table->clk, freq, old_freq))
 		dev_err(dev, "%s: failed to restore old-freq (%lu Hz)\n",
 			__func__, old_freq);
 restore_voltage:
 	/* This shouldn't harm even if the voltages weren't updated earlier */
-	if (old_supply->u_volt)
+	if (old_supply)
 		_set_opp_voltage(dev, reg, old_supply);
 
 	return ret;
@@ -603,10 +596,7 @@ int dev_pm_opp_set_rate(struct device *dev, unsigned long target_freq)
 {
 	struct opp_table *opp_table;
 	unsigned long freq, old_freq;
-	int (*set_opp)(struct dev_pm_set_opp_data *data);
 	struct dev_pm_opp *old_opp, *opp;
-	struct regulator **regulators;
-	struct dev_pm_set_opp_data *data;
 	struct clk *clk;
 	int ret, size;
 
@@ -661,38 +651,35 @@ int dev_pm_opp_set_rate(struct device *dev, unsigned long target_freq)
 	dev_dbg(dev, "%s: switching OPP: %lu Hz --> %lu Hz\n", __func__,
 		old_freq, freq);
 
-	regulators = opp_table->regulators;
-
 	/* Only frequency scaling */
-	if (!regulators) {
+	if (!opp_table->regulators) {
 		ret = _generic_set_opp_clk_only(dev, clk, old_freq, freq);
-		goto put_opps;
-	}
+	} else if (!opp_table->set_opp) {
+		ret = _generic_set_opp_regulator(opp_table, dev, old_freq, freq,
+						 IS_ERR(old_opp) ? NULL : old_opp->supplies,
+						 opp->supplies);
+	} else {
+		struct dev_pm_set_opp_data *data;
 
-	if (opp_table->set_opp)
-		set_opp = opp_table->set_opp;
-	else
-		set_opp = _generic_set_opp;
-
-	data = opp_table->set_opp_data;
-	data->regulators = regulators;
-	data->regulator_count = opp_table->regulator_count;
-	data->clk = clk;
-	data->dev = dev;
-
-	data->old_opp.rate = old_freq;
-	size = sizeof(*opp->supplies) * opp_table->regulator_count;
-	if (IS_ERR(old_opp))
-		memset(data->old_opp.supplies, 0, size);
-	else
-		memcpy(data->old_opp.supplies, old_opp->supplies, size);
+		data = opp_table->set_opp_data;
+		data->regulators = opp_table->regulators;
+		data->regulator_count = opp_table->regulator_count;
+		data->clk = clk;
+		data->dev = dev;
 
-	data->new_opp.rate = freq;
-	memcpy(data->new_opp.supplies, opp->supplies, size);
+		data->old_opp.rate = old_freq;
+		size = sizeof(*opp->supplies) * opp_table->regulator_count;
+		if (IS_ERR(old_opp))
+			memset(data->old_opp.supplies, 0, size);
+		else
+			memcpy(data->old_opp.supplies, old_opp->supplies, size);
 
-	ret = set_opp(data);
+		data->new_opp.rate = freq;
+		memcpy(data->new_opp.supplies, opp->supplies, size);
+
+		ret = opp_table->set_opp(data);
+	}
 
-put_opps:
 	dev_pm_opp_put(opp);
 put_old_opp:
 	if (!IS_ERR(old_opp))
@@ -1376,6 +1363,73 @@ void dev_pm_opp_put_regulators(struct opp_table *opp_table)
 EXPORT_SYMBOL_GPL(dev_pm_opp_put_regulators);
 
 /**
+ * dev_pm_opp_set_clkname() - Set clk name for the device
+ * @dev: Device for which clk name is being set.
+ * @name: Clk name.
+ *
+ * In order to support OPP switching, OPP layer needs to get pointer to the
+ * clock for the device. Simple cases work fine without using this routine (i.e.
+ * by passing connection-id as NULL), but for a device with multiple clocks
+ * available, the OPP core needs to know the exact name of the clk to use.
+ *
+ * This must be called before any OPPs are initialized for the device.
+ */
+struct opp_table *dev_pm_opp_set_clkname(struct device *dev, const char *name)
+{
+	struct opp_table *opp_table;
+	int ret;
+
+	opp_table = dev_pm_opp_get_opp_table(dev);
+	if (!opp_table)
+		return ERR_PTR(-ENOMEM);
+
+	/* This should be called before OPPs are initialized */
+	if (WARN_ON(!list_empty(&opp_table->opp_list))) {
+		ret = -EBUSY;
+		goto err;
+	}
+
+	/* Already have default clk set, free it */
+	if (!IS_ERR(opp_table->clk))
+		clk_put(opp_table->clk);
+
+	/* Find clk for the device */
+	opp_table->clk = clk_get(dev, name);
+	if (IS_ERR(opp_table->clk)) {
+		ret = PTR_ERR(opp_table->clk);
+		if (ret != -EPROBE_DEFER) {
+			dev_err(dev, "%s: Couldn't find clock: %d\n", __func__,
+				ret);
+		}
+		goto err;
+	}
+
+	return opp_table;
+
+err:
+	dev_pm_opp_put_opp_table(opp_table);
+
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL_GPL(dev_pm_opp_set_clkname);
+
+/**
+ * dev_pm_opp_put_clkname() - Releases resources blocked for clk.
+ * @opp_table: OPP table returned from dev_pm_opp_set_clkname().
+ */
+void dev_pm_opp_put_clkname(struct opp_table *opp_table)
+{
+	/* Make sure there are no concurrent readers while updating opp_table */
+	WARN_ON(!list_empty(&opp_table->opp_list));
+
+	clk_put(opp_table->clk);
+	opp_table->clk = ERR_PTR(-EINVAL);
+
+	dev_pm_opp_put_opp_table(opp_table);
+}
+EXPORT_SYMBOL_GPL(dev_pm_opp_put_clkname);
+
+/**
  * dev_pm_opp_register_set_opp_helper() - Register custom set OPP helper
  * @dev: Device for which the helper is getting registered.
  * @set_opp: Custom set OPP helper.
diff --git a/drivers/base/power/opp/debugfs.c b/drivers/base/power/opp/debugfs.c
index 95f433db4ac7..81cf120fcf43 100644
--- a/drivers/base/power/opp/debugfs.c
+++ b/drivers/base/power/opp/debugfs.c
@@ -40,11 +40,10 @@ static bool opp_debug_create_supplies(struct dev_pm_opp *opp,
 				      struct dentry *pdentry)
 {
 	struct dentry *d;
-	int i = 0;
+	int i;
 	char *name;
 
-	/* Always create at least supply-0 directory */
-	do {
+	for (i = 0; i < opp_table->regulator_count; i++) {
 		name = kasprintf(GFP_KERNEL, "supply-%d", i);
 
 		/* Create per-opp directory */
@@ -70,7 +69,7 @@ static bool opp_debug_create_supplies(struct dev_pm_opp *opp,
 		if (!debugfs_create_ulong("u_amp", S_IRUGO, d,
 					  &opp->supplies[i].u_amp))
 			return false;
-	} while (++i < opp_table->regulator_count);
+	}
 
 	return true;
 }
diff --git a/drivers/base/power/opp/of.c b/drivers/base/power/opp/of.c
index 779428676f63..57eec1ca0569 100644
--- a/drivers/base/power/opp/of.c
+++ b/drivers/base/power/opp/of.c
@@ -131,8 +131,14 @@ static int opp_parse_supplies(struct dev_pm_opp *opp, struct device *dev,
 		prop = of_find_property(opp->np, name, NULL);
 
 		/* Missing property isn't a problem, but an invalid entry is */
-		if (!prop)
-			return 0;
+		if (!prop) {
+			if (!opp_table->regulator_count)
+				return 0;
+
+			dev_err(dev, "%s: opp-microvolt missing although OPP managing regulators\n",
+				__func__);
+			return -EINVAL;
+		}
 	}
 
 	vcount = of_property_count_u32_elems(opp->np, name);
diff --git a/drivers/base/power/sysfs.c b/drivers/base/power/sysfs.c
index 33b4b902741a..185a52581cfa 100644
--- a/drivers/base/power/sysfs.c
+++ b/drivers/base/power/sysfs.c
@@ -607,7 +607,7 @@ static struct attribute *power_attrs[] = {
 #endif /* CONFIG_PM_ADVANCED_DEBUG */
 	NULL,
 };
-static struct attribute_group pm_attr_group = {
+static const struct attribute_group pm_attr_group = {
 	.name	= power_group_name,
 	.attrs	= power_attrs,
 };
@@ -629,7 +629,7 @@ static struct attribute *wakeup_attrs[] = {
 #endif
 	NULL,
 };
-static struct attribute_group pm_wakeup_attr_group = {
+static const struct attribute_group pm_wakeup_attr_group = {
 	.name	= power_group_name,
 	.attrs	= wakeup_attrs,
 };
@@ -644,7 +644,7 @@ static struct attribute *runtime_attrs[] = {
 	&dev_attr_autosuspend_delay_ms.attr,
 	NULL,
 };
-static struct attribute_group pm_runtime_attr_group = {
+static const struct attribute_group pm_runtime_attr_group = {
 	.name	= power_group_name,
 	.attrs	= runtime_attrs,
 };
@@ -653,7 +653,7 @@ static struct attribute *pm_qos_resume_latency_attrs[] = {
 	&dev_attr_pm_qos_resume_latency_us.attr,
 	NULL,
 };
-static struct attribute_group pm_qos_resume_latency_attr_group = {
+static const struct attribute_group pm_qos_resume_latency_attr_group = {
 	.name	= power_group_name,
 	.attrs	= pm_qos_resume_latency_attrs,
 };
@@ -662,7 +662,7 @@ static struct attribute *pm_qos_latency_tolerance_attrs[] = {
 	&dev_attr_pm_qos_latency_tolerance_us.attr,
 	NULL,
 };
-static struct attribute_group pm_qos_latency_tolerance_attr_group = {
+static const struct attribute_group pm_qos_latency_tolerance_attr_group = {
 	.name	= power_group_name,
 	.attrs	= pm_qos_latency_tolerance_attrs,
 };
@@ -672,7 +672,7 @@ static struct attribute *pm_qos_flags_attrs[] = {
 	&dev_attr_pm_qos_remote_wakeup.attr,
 	NULL,
 };
-static struct attribute_group pm_qos_flags_attr_group = {
+static const struct attribute_group pm_qos_flags_attr_group = {
 	.name	= power_group_name,
 	.attrs	= pm_qos_flags_attrs,
 };
diff --git a/drivers/base/power/wakeup.c b/drivers/base/power/wakeup.c
index c313b600d356..144e6d8fafc8 100644
--- a/drivers/base/power/wakeup.c
+++ b/drivers/base/power/wakeup.c
@@ -28,8 +28,8 @@ bool events_check_enabled __read_mostly;
 /* First wakeup IRQ seen by the kernel in the last cycle. */
 unsigned int pm_wakeup_irq __read_mostly;
 
-/* If set and the system is suspending, terminate the suspend. */
-static bool pm_abort_suspend __read_mostly;
+/* If greater than 0 and the system is suspending, terminate the suspend. */
+static atomic_t pm_abort_suspend __read_mostly;
 
 /*
  * Combined counters of registered wakeup events and wakeup events in progress.
@@ -60,6 +60,8 @@ static LIST_HEAD(wakeup_sources);
 
 static DECLARE_WAIT_QUEUE_HEAD(wakeup_count_wait_queue);
 
+DEFINE_STATIC_SRCU(wakeup_srcu);
+
 static struct wakeup_source deleted_ws = {
 	.name = "deleted",
 	.lock =  __SPIN_LOCK_UNLOCKED(deleted_ws.lock),
@@ -198,7 +200,7 @@ void wakeup_source_remove(struct wakeup_source *ws)
 	spin_lock_irqsave(&events_lock, flags);
 	list_del_rcu(&ws->entry);
 	spin_unlock_irqrestore(&events_lock, flags);
-	synchronize_rcu();
+	synchronize_srcu(&wakeup_srcu);
 }
 EXPORT_SYMBOL_GPL(wakeup_source_remove);
 
@@ -332,12 +334,12 @@ void device_wakeup_detach_irq(struct device *dev)
 void device_wakeup_arm_wake_irqs(void)
 {
 	struct wakeup_source *ws;
+	int srcuidx;
 
-	rcu_read_lock();
+	srcuidx = srcu_read_lock(&wakeup_srcu);
 	list_for_each_entry_rcu(ws, &wakeup_sources, entry)
 		dev_pm_arm_wake_irq(ws->wakeirq);
-
-	rcu_read_unlock();
+	srcu_read_unlock(&wakeup_srcu, srcuidx);
 }
 
 /**
@@ -348,12 +350,12 @@ void device_wakeup_arm_wake_irqs(void)
 void device_wakeup_disarm_wake_irqs(void)
 {
 	struct wakeup_source *ws;
+	int srcuidx;
 
-	rcu_read_lock();
+	srcuidx = srcu_read_lock(&wakeup_srcu);
 	list_for_each_entry_rcu(ws, &wakeup_sources, entry)
 		dev_pm_disarm_wake_irq(ws->wakeirq);
-
-	rcu_read_unlock();
+	srcu_read_unlock(&wakeup_srcu, srcuidx);
 }
 
 /**
@@ -804,10 +806,10 @@ EXPORT_SYMBOL_GPL(pm_wakeup_dev_event);
 void pm_print_active_wakeup_sources(void)
 {
 	struct wakeup_source *ws;
-	int active = 0;
+	int srcuidx, active = 0;
 	struct wakeup_source *last_activity_ws = NULL;
 
-	rcu_read_lock();
+	srcuidx = srcu_read_lock(&wakeup_srcu);
 	list_for_each_entry_rcu(ws, &wakeup_sources, entry) {
 		if (ws->active) {
 			pr_debug("active wakeup source: %s\n", ws->name);
@@ -823,7 +825,7 @@ void pm_print_active_wakeup_sources(void)
 	if (!active && last_activity_ws)
 		pr_debug("last active wakeup source: %s\n",
 			last_activity_ws->name);
-	rcu_read_unlock();
+	srcu_read_unlock(&wakeup_srcu, srcuidx);
 }
 EXPORT_SYMBOL_GPL(pm_print_active_wakeup_sources);
 
@@ -855,20 +857,26 @@ bool pm_wakeup_pending(void)
 		pm_print_active_wakeup_sources();
 	}
 
-	return ret || pm_abort_suspend;
+	return ret || atomic_read(&pm_abort_suspend) > 0;
 }
 
 void pm_system_wakeup(void)
 {
-	pm_abort_suspend = true;
+	atomic_inc(&pm_abort_suspend);
 	freeze_wake();
 }
 EXPORT_SYMBOL_GPL(pm_system_wakeup);
 
-void pm_wakeup_clear(void)
+void pm_system_cancel_wakeup(void)
+{
+	atomic_dec(&pm_abort_suspend);
+}
+
+void pm_wakeup_clear(bool reset)
 {
-	pm_abort_suspend = false;
 	pm_wakeup_irq = 0;
+	if (reset)
+		atomic_set(&pm_abort_suspend, 0);
 }
 
 void pm_system_irq_wakeup(unsigned int irq_number)
@@ -950,8 +958,9 @@ void pm_wakep_autosleep_enabled(bool set)
 {
 	struct wakeup_source *ws;
 	ktime_t now = ktime_get();
+	int srcuidx;
 
-	rcu_read_lock();
+	srcuidx = srcu_read_lock(&wakeup_srcu);
 	list_for_each_entry_rcu(ws, &wakeup_sources, entry) {
 		spin_lock_irq(&ws->lock);
 		if (ws->autosleep_enabled != set) {
@@ -965,7 +974,7 @@ void pm_wakep_autosleep_enabled(bool set)
 		}
 		spin_unlock_irq(&ws->lock);
 	}
-	rcu_read_unlock();
+	srcu_read_unlock(&wakeup_srcu, srcuidx);
 }
 #endif /* CONFIG_PM_AUTOSLEEP */
 
@@ -1026,15 +1035,16 @@ static int print_wakeup_source_stats(struct seq_file *m,
 static int wakeup_sources_stats_show(struct seq_file *m, void *unused)
 {
 	struct wakeup_source *ws;
+	int srcuidx;
 
 	seq_puts(m, "name\t\tactive_count\tevent_count\twakeup_count\t"
 		"expire_count\tactive_since\ttotal_time\tmax_time\t"
 		"last_change\tprevent_suspend_time\n");
 
-	rcu_read_lock();
+	srcuidx = srcu_read_lock(&wakeup_srcu);
 	list_for_each_entry_rcu(ws, &wakeup_sources, entry)
 		print_wakeup_source_stats(m, ws);
-	rcu_read_unlock();
+	srcu_read_unlock(&wakeup_srcu, srcuidx);
 
 	print_wakeup_source_stats(m, &deleted_ws);
 
diff --git a/drivers/base/regmap/Kconfig b/drivers/base/regmap/Kconfig
index db9d00c36a3e..073c0b77e5b3 100644
--- a/drivers/base/regmap/Kconfig
+++ b/drivers/base/regmap/Kconfig
@@ -3,10 +3,13 @@
 # subsystems should select the appropriate symbols.
 
 config REGMAP
-	default y if (REGMAP_I2C || REGMAP_SPI || REGMAP_SPMI || REGMAP_AC97 || REGMAP_MMIO || REGMAP_IRQ)
+	default y if (REGMAP_I2C || REGMAP_SPI || REGMAP_SPMI || REGMAP_W1 || REGMAP_AC97 || REGMAP_MMIO || REGMAP_IRQ)
+	select IRQ_DOMAIN if REGMAP_IRQ
+	bool
+
+config REGCACHE_COMPRESSED
 	select LZO_COMPRESS
 	select LZO_DECOMPRESS
-	select IRQ_DOMAIN if REGMAP_IRQ
 	bool
 
 config REGMAP_AC97
@@ -24,6 +27,10 @@ config REGMAP_SPMI
 	tristate
 	depends on SPMI
 
+config REGMAP_W1
+	tristate
+	depends on W1
+
 config REGMAP_MMIO
 	tristate
 
diff --git a/drivers/base/regmap/Makefile b/drivers/base/regmap/Makefile
index 609e4c84f485..0cf4abc8fbf1 100644
--- a/drivers/base/regmap/Makefile
+++ b/drivers/base/regmap/Makefile
@@ -2,7 +2,8 @@
 CFLAGS_regmap.o := -I$(src)
 
 obj-$(CONFIG_REGMAP) += regmap.o regcache.o
-obj-$(CONFIG_REGMAP) += regcache-rbtree.o regcache-lzo.o regcache-flat.o
+obj-$(CONFIG_REGMAP) += regcache-rbtree.o regcache-flat.o
+obj-$(CONFIG_REGCACHE_COMPRESSED) += regcache-lzo.o
 obj-$(CONFIG_DEBUG_FS) += regmap-debugfs.o
 obj-$(CONFIG_REGMAP_AC97) += regmap-ac97.o
 obj-$(CONFIG_REGMAP_I2C) += regmap-i2c.o
@@ -10,3 +11,4 @@ obj-$(CONFIG_REGMAP_SPI) += regmap-spi.o
 obj-$(CONFIG_REGMAP_SPMI) += regmap-spmi.o
 obj-$(CONFIG_REGMAP_MMIO) += regmap-mmio.o
 obj-$(CONFIG_REGMAP_IRQ) += regmap-irq.o
+obj-$(CONFIG_REGMAP_W1) += regmap-w1.o
diff --git a/drivers/base/regmap/regcache.c b/drivers/base/regmap/regcache.c
index b0a0dcf32fb7..773560348337 100644
--- a/drivers/base/regmap/regcache.c
+++ b/drivers/base/regmap/regcache.c
@@ -21,7 +21,9 @@
 
 static const struct regcache_ops *cache_types[] = {
 	&regcache_rbtree_ops,
+#if IS_ENABLED(CONFIG_REGCACHE_COMPRESSED)
 	&regcache_lzo_ops,
+#endif
 	&regcache_flat_ops,
 };
 
diff --git a/drivers/base/regmap/regmap-irq.c b/drivers/base/regmap/regmap-irq.c
index cd54189f2b1d..429ca8ed7e51 100644
--- a/drivers/base/regmap/regmap-irq.c
+++ b/drivers/base/regmap/regmap-irq.c
@@ -60,6 +60,16 @@ static void regmap_irq_lock(struct irq_data *data)
 	mutex_lock(&d->lock);
 }
 
+static int regmap_irq_update_bits(struct regmap_irq_chip_data *d,
+				  unsigned int reg, unsigned int mask,
+				  unsigned int val)
+{
+	if (d->chip->mask_writeonly)
+		return regmap_write_bits(d->map, reg, mask, val);
+	else
+		return regmap_update_bits(d->map, reg, mask, val);
+}
+
 static void regmap_irq_sync_unlock(struct irq_data *data)
 {
 	struct regmap_irq_chip_data *d = irq_data_get_irq_chip_data(data);
@@ -84,11 +94,11 @@ static void regmap_irq_sync_unlock(struct irq_data *data)
 		reg = d->chip->mask_base +
 			(i * map->reg_stride * d->irq_reg_stride);
 		if (d->chip->mask_invert) {
-			ret = regmap_update_bits(d->map, reg,
+			ret = regmap_irq_update_bits(d, reg,
 					 d->mask_buf_def[i], ~d->mask_buf[i]);
 		} else if (d->chip->unmask_base) {
 			/* set mask with mask_base register */
-			ret = regmap_update_bits(d->map, reg,
+			ret = regmap_irq_update_bits(d, reg,
 					d->mask_buf_def[i], ~d->mask_buf[i]);
 			if (ret < 0)
 				dev_err(d->map->dev,
@@ -97,12 +107,12 @@ static void regmap_irq_sync_unlock(struct irq_data *data)
 			unmask_offset = d->chip->unmask_base -
 							d->chip->mask_base;
 			/* clear mask with unmask_base register */
-			ret = regmap_update_bits(d->map,
+			ret = regmap_irq_update_bits(d,
 					reg + unmask_offset,
 					d->mask_buf_def[i],
 					d->mask_buf[i]);
 		} else {
-			ret = regmap_update_bits(d->map, reg,
+			ret = regmap_irq_update_bits(d, reg,
 					 d->mask_buf_def[i], d->mask_buf[i]);
 		}
 		if (ret != 0)
@@ -113,11 +123,11 @@ static void regmap_irq_sync_unlock(struct irq_data *data)
 			(i * map->reg_stride * d->irq_reg_stride);
 		if (d->wake_buf) {
 			if (d->chip->wake_invert)
-				ret = regmap_update_bits(d->map, reg,
+				ret = regmap_irq_update_bits(d, reg,
 							 d->mask_buf_def[i],
 							 ~d->wake_buf[i]);
 			else
-				ret = regmap_update_bits(d->map, reg,
+				ret = regmap_irq_update_bits(d, reg,
 							 d->mask_buf_def[i],
 							 d->wake_buf[i]);
 			if (ret != 0)
@@ -153,10 +163,10 @@ static void regmap_irq_sync_unlock(struct irq_data *data)
 		reg = d->chip->type_base +
 			(i * map->reg_stride * d->type_reg_stride);
 		if (d->chip->type_invert)
-			ret = regmap_update_bits(d->map, reg,
+			ret = regmap_irq_update_bits(d, reg,
 				d->type_buf_def[i], ~d->type_buf[i]);
 		else
-			ret = regmap_update_bits(d->map, reg,
+			ret = regmap_irq_update_bits(d, reg,
 				d->type_buf_def[i], d->type_buf[i]);
 		if (ret != 0)
 			dev_err(d->map->dev, "Failed to sync type in %x\n",
@@ -394,7 +404,7 @@ static int regmap_irq_map(struct irq_domain *h, unsigned int virq,
 
 static const struct irq_domain_ops regmap_domain_ops = {
 	.map	= regmap_irq_map,
-	.xlate	= irq_domain_xlate_twocell,
+	.xlate	= irq_domain_xlate_onetwocell,
 };
 
 /**
@@ -519,17 +529,17 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 		reg = chip->mask_base +
 			(i * map->reg_stride * d->irq_reg_stride);
 		if (chip->mask_invert)
-			ret = regmap_update_bits(map, reg,
+			ret = regmap_irq_update_bits(d, reg,
 					 d->mask_buf[i], ~d->mask_buf[i]);
 		else if (d->chip->unmask_base) {
 			unmask_offset = d->chip->unmask_base -
 					d->chip->mask_base;
-			ret = regmap_update_bits(d->map,
+			ret = regmap_irq_update_bits(d,
 					reg + unmask_offset,
 					d->mask_buf[i],
 					d->mask_buf[i]);
 		} else
-			ret = regmap_update_bits(map, reg,
+			ret = regmap_irq_update_bits(d, reg,
 					 d->mask_buf[i], d->mask_buf[i]);
 		if (ret != 0) {
 			dev_err(map->dev, "Failed to set masks in 0x%x: %d\n",
@@ -575,11 +585,11 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 				(i * map->reg_stride * d->irq_reg_stride);
 
 			if (chip->wake_invert)
-				ret = regmap_update_bits(map, reg,
+				ret = regmap_irq_update_bits(d, reg,
 							 d->mask_buf_def[i],
 							 0);
 			else
-				ret = regmap_update_bits(map, reg,
+				ret = regmap_irq_update_bits(d, reg,
 							 d->mask_buf_def[i],
 							 d->wake_buf[i]);
 			if (ret != 0) {
@@ -603,10 +613,10 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 			reg = chip->type_base +
 				(i * map->reg_stride * d->type_reg_stride);
 			if (chip->type_invert)
-				ret = regmap_update_bits(map, reg,
+				ret = regmap_irq_update_bits(d, reg,
 					d->type_buf_def[i], 0xFF);
 			else
-				ret = regmap_update_bits(map, reg,
+				ret = regmap_irq_update_bits(d, reg,
 					d->type_buf_def[i], 0x0);
 			if (ret != 0) {
 				dev_err(map->dev,
diff --git a/drivers/base/regmap/regmap-w1.c b/drivers/base/regmap/regmap-w1.c
new file mode 100644
index 000000000000..5f04e7bf063e
--- /dev/null
+++ b/drivers/base/regmap/regmap-w1.c
@@ -0,0 +1,245 @@
+/*
+ * Register map access API - W1 (1-Wire) support
+ *
+ * Copyright (C) 2017 OAO Radioavionica
+ * Author: Alex A. Mihaylov <minimumlaw@rambler.ru>
+ *
+ * This program is free software; you can 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/regmap.h>
+#include <linux/module.h>
+#include "../../w1/w1.h"
+
+#include "internal.h"
+
+#define W1_CMD_READ_DATA	0x69
+#define W1_CMD_WRITE_DATA	0x6C
+
+/*
+ * 1-Wire slaves registers with addess 8 bit and data 8 bit
+ */
+
+static int w1_reg_a8_v8_read(void *context, unsigned int reg, unsigned int *val)
+{
+	struct device *dev = context;
+	struct w1_slave *sl = container_of(dev, struct w1_slave, dev);
+	int ret = 0;
+
+	if (reg > 255)
+		return -EINVAL;
+
+	mutex_lock(&sl->master->bus_mutex);
+	if (!w1_reset_select_slave(sl)) {
+		w1_write_8(sl->master, W1_CMD_READ_DATA);
+		w1_write_8(sl->master, reg);
+		*val = w1_read_8(sl->master);
+	} else {
+		ret = -ENODEV;
+	}
+	mutex_unlock(&sl->master->bus_mutex);
+
+	return ret;
+}
+
+static int w1_reg_a8_v8_write(void *context, unsigned int reg, unsigned int val)
+{
+	struct device *dev = context;
+	struct w1_slave *sl = container_of(dev, struct w1_slave, dev);
+	int ret = 0;
+
+	if (reg > 255)
+		return -EINVAL;
+
+	mutex_lock(&sl->master->bus_mutex);
+	if (!w1_reset_select_slave(sl)) {
+		w1_write_8(sl->master, W1_CMD_WRITE_DATA);
+		w1_write_8(sl->master, reg);
+		w1_write_8(sl->master, val);
+	} else {
+		ret = -ENODEV;
+	}
+	mutex_unlock(&sl->master->bus_mutex);
+
+	return ret;
+}
+
+/*
+ * 1-Wire slaves registers with addess 8 bit and data 16 bit
+ */
+
+static int w1_reg_a8_v16_read(void *context, unsigned int reg,
+				unsigned int *val)
+{
+	struct device *dev = context;
+	struct w1_slave *sl = container_of(dev, struct w1_slave, dev);
+	int ret = 0;
+
+	if (reg > 255)
+		return -EINVAL;
+
+	mutex_lock(&sl->master->bus_mutex);
+	if (!w1_reset_select_slave(sl)) {
+		w1_write_8(sl->master, W1_CMD_READ_DATA);
+		w1_write_8(sl->master, reg);
+		*val = w1_read_8(sl->master);
+		*val |= w1_read_8(sl->master)<<8;
+	} else {
+		ret = -ENODEV;
+	}
+	mutex_unlock(&sl->master->bus_mutex);
+
+	return ret;
+}
+
+static int w1_reg_a8_v16_write(void *context, unsigned int reg,
+				unsigned int val)
+{
+	struct device *dev = context;
+	struct w1_slave *sl = container_of(dev, struct w1_slave, dev);
+	int ret = 0;
+
+	if (reg > 255)
+		return -EINVAL;
+
+	mutex_lock(&sl->master->bus_mutex);
+	if (!w1_reset_select_slave(sl)) {
+		w1_write_8(sl->master, W1_CMD_WRITE_DATA);
+		w1_write_8(sl->master, reg);
+		w1_write_8(sl->master, val & 0x00FF);
+		w1_write_8(sl->master, val>>8 & 0x00FF);
+	} else {
+		ret = -ENODEV;
+	}
+	mutex_unlock(&sl->master->bus_mutex);
+
+	return ret;
+}
+
+/*
+ * 1-Wire slaves registers with addess 16 bit and data 16 bit
+ */
+
+static int w1_reg_a16_v16_read(void *context, unsigned int reg,
+				unsigned int *val)
+{
+	struct device *dev = context;
+	struct w1_slave *sl = container_of(dev, struct w1_slave, dev);
+	int ret = 0;
+
+	if (reg > 65535)
+		return -EINVAL;
+
+	mutex_lock(&sl->master->bus_mutex);
+	if (!w1_reset_select_slave(sl)) {
+		w1_write_8(sl->master, W1_CMD_READ_DATA);
+		w1_write_8(sl->master, reg & 0x00FF);
+		w1_write_8(sl->master, reg>>8 & 0x00FF);
+		*val = w1_read_8(sl->master);
+		*val |= w1_read_8(sl->master)<<8;
+	} else {
+		ret = -ENODEV;
+	}
+	mutex_unlock(&sl->master->bus_mutex);
+
+	return ret;
+}
+
+static int w1_reg_a16_v16_write(void *context, unsigned int reg,
+				unsigned int val)
+{
+	struct device *dev = context;
+	struct w1_slave *sl = container_of(dev, struct w1_slave, dev);
+	int ret = 0;
+
+	if (reg > 65535)
+		return -EINVAL;
+
+	mutex_lock(&sl->master->bus_mutex);
+	if (!w1_reset_select_slave(sl)) {
+		w1_write_8(sl->master, W1_CMD_WRITE_DATA);
+		w1_write_8(sl->master, reg & 0x00FF);
+		w1_write_8(sl->master, reg>>8 & 0x00FF);
+		w1_write_8(sl->master, val & 0x00FF);
+		w1_write_8(sl->master, val>>8 & 0x00FF);
+	} else {
+		ret = -ENODEV;
+	}
+	mutex_unlock(&sl->master->bus_mutex);
+
+	return ret;
+}
+
+/*
+ * Various types of supported bus addressing
+ */
+
+static struct regmap_bus regmap_w1_bus_a8_v8 = {
+	.reg_read = w1_reg_a8_v8_read,
+	.reg_write = w1_reg_a8_v8_write,
+};
+
+static struct regmap_bus regmap_w1_bus_a8_v16 = {
+	.reg_read = w1_reg_a8_v16_read,
+	.reg_write = w1_reg_a8_v16_write,
+};
+
+static struct regmap_bus regmap_w1_bus_a16_v16 = {
+	.reg_read = w1_reg_a16_v16_read,
+	.reg_write = w1_reg_a16_v16_write,
+};
+
+static const struct regmap_bus *regmap_get_w1_bus(struct device *w1_dev,
+					const struct regmap_config *config)
+{
+	if (config->reg_bits == 8 && config->val_bits == 8)
+		return &regmap_w1_bus_a8_v8;
+
+	if (config->reg_bits == 8 && config->val_bits == 16)
+		return &regmap_w1_bus_a8_v16;
+
+	if (config->reg_bits == 16 && config->val_bits == 16)
+		return &regmap_w1_bus_a16_v16;
+
+	return ERR_PTR(-ENOTSUPP);
+}
+
+struct regmap *__regmap_init_w1(struct device *w1_dev,
+				 const struct regmap_config *config,
+				 struct lock_class_key *lock_key,
+				 const char *lock_name)
+{
+
+	const struct regmap_bus *bus = regmap_get_w1_bus(w1_dev, config);
+
+	if (IS_ERR(bus))
+		return ERR_CAST(bus);
+
+	return __regmap_init(w1_dev, bus, w1_dev, config,
+			 lock_key, lock_name);
+
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(__regmap_init_w1);
+
+struct regmap *__devm_regmap_init_w1(struct device *w1_dev,
+				 const struct regmap_config *config,
+				 struct lock_class_key *lock_key,
+				 const char *lock_name)
+{
+
+	const struct regmap_bus *bus = regmap_get_w1_bus(w1_dev, config);
+
+	if (IS_ERR(bus))
+		return ERR_CAST(bus);
+
+	return __devm_regmap_init(w1_dev, bus, w1_dev, config,
+				 lock_key, lock_name);
+
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(__devm_regmap_init_w1);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/block/DAC960.c b/drivers/block/DAC960.c
index 26a51be77227..245a879b036e 100644
--- a/drivers/block/DAC960.c
+++ b/drivers/block/DAC960.c
@@ -3464,7 +3464,7 @@ static inline bool DAC960_ProcessCompletedRequest(DAC960_Command_T *Command,
 						 bool SuccessfulIO)
 {
 	struct request *Request = Command->Request;
-	int Error = SuccessfulIO ? 0 : -EIO;
+	blk_status_t Error = SuccessfulIO ? BLK_STS_OK : BLK_STS_IOERR;
 
 	pci_unmap_sg(Command->Controller->PCIDevice, Command->cmd_sglist,
 		Command->SegmentCount, Command->DmaDirection);
diff --git a/drivers/block/amiflop.c b/drivers/block/amiflop.c
index a328f673adfe..49908c74bfcb 100644
--- a/drivers/block/amiflop.c
+++ b/drivers/block/amiflop.c
@@ -1378,7 +1378,7 @@ static void redo_fd_request(void)
 	struct amiga_floppy_struct *floppy;
 	char *data;
 	unsigned long flags;
-	int err;
+	blk_status_t err;
 
 next_req:
 	rq = set_next_request();
@@ -1392,7 +1392,7 @@ next_req:
 
 next_segment:
 	/* Here someone could investigate to be more efficient */
-	for (cnt = 0, err = 0; cnt < blk_rq_cur_sectors(rq); cnt++) {
+	for (cnt = 0, err = BLK_STS_OK; cnt < blk_rq_cur_sectors(rq); cnt++) {
 #ifdef DEBUG
 		printk("fd: sector %ld + %d requested for %s\n",
 		       blk_rq_pos(rq), cnt,
@@ -1400,7 +1400,7 @@ next_segment:
 #endif
 		block = blk_rq_pos(rq) + cnt;
 		if ((int)block > floppy->blocks) {
-			err = -EIO;
+			err = BLK_STS_IOERR;
 			break;
 		}
 
@@ -1413,7 +1413,7 @@ next_segment:
 #endif
 
 		if (get_track(drive, track) == -1) {
-			err = -EIO;
+			err = BLK_STS_IOERR;
 			break;
 		}
 
@@ -1424,7 +1424,7 @@ next_segment:
 
 			/* keep the drive spinning while writes are scheduled */
 			if (!fd_motor_on(drive)) {
-				err = -EIO;
+				err = BLK_STS_IOERR;
 				break;
 			}
 			/*
diff --git a/drivers/block/aoe/aoeblk.c b/drivers/block/aoe/aoeblk.c
index 027b876370bc..6797e6c23c8a 100644
--- a/drivers/block/aoe/aoeblk.c
+++ b/drivers/block/aoe/aoeblk.c
@@ -388,6 +388,7 @@ aoeblk_gdalloc(void *vp)
 			d->aoemajor, d->aoeminor);
 		goto err_mempool;
 	}
+	blk_queue_bounce_limit(q, BLK_BOUNCE_HIGH);
 
 	spin_lock_irqsave(&d->lock, flags);
 	WARN_ON(!(d->flags & DEVFL_GD_NOW));
diff --git a/drivers/block/aoe/aoecmd.c b/drivers/block/aoe/aoecmd.c
index 3c606c09fd5a..dc43254e05a4 100644
--- a/drivers/block/aoe/aoecmd.c
+++ b/drivers/block/aoe/aoecmd.c
@@ -1070,8 +1070,8 @@ aoe_end_request(struct aoedev *d, struct request *rq, int fastfail)
 		d->ip.rq = NULL;
 	do {
 		bio = rq->bio;
-		bok = !fastfail && !bio->bi_error;
-	} while (__blk_end_request(rq, bok ? 0 : -EIO, bio->bi_iter.bi_size));
+		bok = !fastfail && !bio->bi_status;
+	} while (__blk_end_request(rq, bok ? BLK_STS_OK : BLK_STS_IOERR, bio->bi_iter.bi_size));
 
 	/* cf. http://lkml.org/lkml/2006/10/31/28 */
 	if (!fastfail)
@@ -1131,7 +1131,7 @@ ktiocomplete(struct frame *f)
 			ahout->cmdstat, ahin->cmdstat,
 			d->aoemajor, d->aoeminor);
 noskb:		if (buf)
-			buf->bio->bi_error = -EIO;
+			buf->bio->bi_status = BLK_STS_IOERR;
 		goto out;
 	}
 
@@ -1144,7 +1144,7 @@ noskb:		if (buf)
 				"aoe: runt data size in read from",
 				(long) d->aoemajor, d->aoeminor,
 			       skb->len, n);
-			buf->bio->bi_error = -EIO;
+			buf->bio->bi_status = BLK_STS_IOERR;
 			break;
 		}
 		if (n > f->iter.bi_size) {
@@ -1152,7 +1152,7 @@ noskb:		if (buf)
 				"aoe: too-large data size in read from",
 				(long) d->aoemajor, d->aoeminor,
 				n, f->iter.bi_size);
-			buf->bio->bi_error = -EIO;
+			buf->bio->bi_status = BLK_STS_IOERR;
 			break;
 		}
 		bvcpy(skb, f->buf->bio, f->iter, n);
@@ -1654,7 +1654,7 @@ aoe_failbuf(struct aoedev *d, struct buf *buf)
 	if (buf == NULL)
 		return;
 	buf->iter.bi_size = 0;
-	buf->bio->bi_error = -EIO;
+	buf->bio->bi_status = BLK_STS_IOERR;
 	if (buf->nframesout == 0)
 		aoe_end_buf(d, buf);
 }
diff --git a/drivers/block/aoe/aoedev.c b/drivers/block/aoe/aoedev.c
index ffd1947500c6..b28fefb90391 100644
--- a/drivers/block/aoe/aoedev.c
+++ b/drivers/block/aoe/aoedev.c
@@ -170,7 +170,7 @@ aoe_failip(struct aoedev *d)
 	if (rq == NULL)
 		return;
 	while ((bio = d->ip.nxbio)) {
-		bio->bi_error = -EIO;
+		bio->bi_status = BLK_STS_IOERR;
 		d->ip.nxbio = bio->bi_next;
 		n = (unsigned long) rq->special;
 		rq->special = (void *) --n;
diff --git a/drivers/block/ataflop.c b/drivers/block/ataflop.c
index fa69ecd52cb5..92da886180aa 100644
--- a/drivers/block/ataflop.c
+++ b/drivers/block/ataflop.c
@@ -378,7 +378,7 @@ static DEFINE_TIMER(readtrack_timer, fd_readtrack_check, 0, 0);
 static DEFINE_TIMER(timeout_timer, fd_times_out, 0, 0);
 static DEFINE_TIMER(fd_timer, check_change, 0, 0);
 	
-static void fd_end_request_cur(int err)
+static void fd_end_request_cur(blk_status_t err)
 {
 	if (!__blk_end_request_cur(fd_request, err))
 		fd_request = NULL;
@@ -620,7 +620,7 @@ static void fd_error( void )
 	fd_request->error_count++;
 	if (fd_request->error_count >= MAX_ERRORS) {
 		printk(KERN_ERR "fd%d: too many errors.\n", SelectedDrive );
-		fd_end_request_cur(-EIO);
+		fd_end_request_cur(BLK_STS_IOERR);
 	}
 	else if (fd_request->error_count == RECALIBRATE_ERRORS) {
 		printk(KERN_WARNING "fd%d: recalibrating\n", SelectedDrive );
@@ -739,7 +739,7 @@ static void do_fd_action( int drive )
 		    }
 		    else {
 			/* all sectors finished */
-			fd_end_request_cur(0);
+			fd_end_request_cur(BLK_STS_OK);
 			redo_fd_request();
 			return;
 		    }
@@ -1144,7 +1144,7 @@ static void fd_rwsec_done1(int status)
 	}
 	else {
 		/* all sectors finished */
-		fd_end_request_cur(0);
+		fd_end_request_cur(BLK_STS_OK);
 		redo_fd_request();
 	}
 	return;
@@ -1445,7 +1445,7 @@ repeat:
 	if (!UD.connected) {
 		/* drive not connected */
 		printk(KERN_ERR "Unknown Device: fd%d\n", drive );
-		fd_end_request_cur(-EIO);
+		fd_end_request_cur(BLK_STS_IOERR);
 		goto repeat;
 	}
 		
@@ -1461,12 +1461,12 @@ repeat:
 		/* user supplied disk type */
 		if (--type >= NUM_DISK_MINORS) {
 			printk(KERN_WARNING "fd%d: invalid disk format", drive );
-			fd_end_request_cur(-EIO);
+			fd_end_request_cur(BLK_STS_IOERR);
 			goto repeat;
 		}
 		if (minor2disktype[type].drive_types > DriveType)  {
 			printk(KERN_WARNING "fd%d: unsupported disk format", drive );
-			fd_end_request_cur(-EIO);
+			fd_end_request_cur(BLK_STS_IOERR);
 			goto repeat;
 		}
 		type = minor2disktype[type].index;
@@ -1476,7 +1476,7 @@ repeat:
 	}
 	
 	if (blk_rq_pos(fd_request) + 1 > UDT->blocks) {
-		fd_end_request_cur(-EIO);
+		fd_end_request_cur(BLK_STS_IOERR);
 		goto repeat;
 	}
 
diff --git a/drivers/block/brd.c b/drivers/block/brd.c
index 57b574f2f66a..6112e99bedf7 100644
--- a/drivers/block/brd.c
+++ b/drivers/block/brd.c
@@ -418,7 +418,6 @@ static struct brd_device *brd_alloc(int i)
 
 	blk_queue_make_request(brd->brd_queue, brd_make_request);
 	blk_queue_max_hw_sectors(brd->brd_queue, 1024);
-	blk_queue_bounce_limit(brd->brd_queue, BLK_BOUNCE_ANY);
 
 	/* This is so fdisk will align partitions on 4k, because of
 	 * direct_access API needing 4k alignment, returning a PFN
diff --git a/drivers/block/cciss.c b/drivers/block/cciss.c
index cd375503f7b0..02a611993bb4 100644
--- a/drivers/block/cciss.c
+++ b/drivers/block/cciss.c
@@ -1864,7 +1864,8 @@ static void cciss_softirq_done(struct request *rq)
 	/* set the residual count for pc requests */
 	if (blk_rq_is_passthrough(rq))
 		scsi_req(rq)->resid_len = c->err_info->ResidualCnt;
-	blk_end_request_all(rq, scsi_req(rq)->result ? -EIO : 0);
+	blk_end_request_all(rq, scsi_req(rq)->result ?
+			BLK_STS_IOERR : BLK_STS_OK);
 
 	spin_lock_irqsave(&h->lock, flags);
 	cmd_free(h, c);
@@ -1956,6 +1957,7 @@ static int cciss_add_disk(ctlr_info_t *h, struct gendisk *disk,
 	disk->queue->cmd_size = sizeof(struct scsi_request);
 	disk->queue->request_fn = do_cciss_request;
 	disk->queue->queue_lock = &h->lock;
+	queue_flag_set_unlocked(QUEUE_FLAG_SCSI_PASSTHROUGH, disk->queue);
 	if (blk_init_allocated_queue(disk->queue) < 0)
 		goto cleanup_queue;
 
diff --git a/drivers/block/drbd/drbd_actlog.c b/drivers/block/drbd/drbd_actlog.c
index 8d7bcfa49c12..e02c45cd3c5a 100644
--- a/drivers/block/drbd/drbd_actlog.c
+++ b/drivers/block/drbd/drbd_actlog.c
@@ -178,7 +178,7 @@ static int _drbd_md_sync_page_io(struct drbd_device *device,
 	else
 		submit_bio(bio);
 	wait_until_done_or_force_detached(device, bdev, &device->md_io.done);
-	if (!bio->bi_error)
+	if (!bio->bi_status)
 		err = device->md_io.error;
 
  out:
diff --git a/drivers/block/drbd/drbd_bitmap.c b/drivers/block/drbd/drbd_bitmap.c
index a804a4107fbc..809fd245c3dc 100644
--- a/drivers/block/drbd/drbd_bitmap.c
+++ b/drivers/block/drbd/drbd_bitmap.c
@@ -959,16 +959,16 @@ static void drbd_bm_endio(struct bio *bio)
 	    !bm_test_page_unchanged(b->bm_pages[idx]))
 		drbd_warn(device, "bitmap page idx %u changed during IO!\n", idx);
 
-	if (bio->bi_error) {
+	if (bio->bi_status) {
 		/* ctx error will hold the completed-last non-zero error code,
 		 * in case error codes differ. */
-		ctx->error = bio->bi_error;
+		ctx->error = blk_status_to_errno(bio->bi_status);
 		bm_set_page_io_err(b->bm_pages[idx]);
 		/* Not identical to on disk version of it.
 		 * Is BM_PAGE_IO_ERROR enough? */
 		if (__ratelimit(&drbd_ratelimit_state))
 			drbd_err(device, "IO ERROR %d on bitmap page idx %u\n",
-					bio->bi_error, idx);
+					bio->bi_status, idx);
 	} else {
 		bm_clear_page_io_err(b->bm_pages[idx]);
 		dynamic_drbd_dbg(device, "bitmap page idx %u completed\n", idx);
diff --git a/drivers/block/drbd/drbd_int.h b/drivers/block/drbd/drbd_int.h
index d5da45bb03a6..d17b6e6393c7 100644
--- a/drivers/block/drbd/drbd_int.h
+++ b/drivers/block/drbd/drbd_int.h
@@ -1441,6 +1441,9 @@ extern struct bio_set *drbd_md_io_bio_set;
 /* to allocate from that set */
 extern struct bio *bio_alloc_drbd(gfp_t gfp_mask);
 
+/* And a bio_set for cloning */
+extern struct bio_set *drbd_io_bio_set;
+
 extern struct mutex resources_mutex;
 
 extern int conn_lowest_minor(struct drbd_connection *connection);
@@ -1627,7 +1630,7 @@ static inline void drbd_generic_make_request(struct drbd_device *device,
 	__release(local);
 	if (!bio->bi_bdev) {
 		drbd_err(device, "drbd_generic_make_request: bio->bi_bdev == NULL\n");
-		bio->bi_error = -ENODEV;
+		bio->bi_status = BLK_STS_IOERR;
 		bio_endio(bio);
 		return;
 	}
diff --git a/drivers/block/drbd/drbd_main.c b/drivers/block/drbd/drbd_main.c
index 84455c365f57..e2ed28d45ce1 100644
--- a/drivers/block/drbd/drbd_main.c
+++ b/drivers/block/drbd/drbd_main.c
@@ -128,6 +128,7 @@ mempool_t *drbd_request_mempool;
 mempool_t *drbd_ee_mempool;
 mempool_t *drbd_md_io_page_pool;
 struct bio_set *drbd_md_io_bio_set;
+struct bio_set *drbd_io_bio_set;
 
 /* I do not use a standard mempool, because:
    1) I want to hand out the pre-allocated objects first.
@@ -1550,7 +1551,6 @@ static int _drbd_send_page(struct drbd_peer_device *peer_device, struct page *pa
 		    int offset, size_t size, unsigned msg_flags)
 {
 	struct socket *socket = peer_device->connection->data.socket;
-	mm_segment_t oldfs = get_fs();
 	int len = size;
 	int err = -EIO;
 
@@ -1565,7 +1565,6 @@ static int _drbd_send_page(struct drbd_peer_device *peer_device, struct page *pa
 
 	msg_flags |= MSG_NOSIGNAL;
 	drbd_update_congested(peer_device->connection);
-	set_fs(KERNEL_DS);
 	do {
 		int sent;
 
@@ -1585,7 +1584,6 @@ static int _drbd_send_page(struct drbd_peer_device *peer_device, struct page *pa
 		len    -= sent;
 		offset += sent;
 	} while (len > 0 /* THINK && device->cstate >= C_CONNECTED*/);
-	set_fs(oldfs);
 	clear_bit(NET_CONGESTED, &peer_device->connection->flags);
 
 	if (len == 0) {
@@ -2098,6 +2096,8 @@ static void drbd_destroy_mempools(void)
 
 	/* D_ASSERT(device, atomic_read(&drbd_pp_vacant)==0); */
 
+	if (drbd_io_bio_set)
+		bioset_free(drbd_io_bio_set);
 	if (drbd_md_io_bio_set)
 		bioset_free(drbd_md_io_bio_set);
 	if (drbd_md_io_page_pool)
@@ -2115,6 +2115,7 @@ static void drbd_destroy_mempools(void)
 	if (drbd_al_ext_cache)
 		kmem_cache_destroy(drbd_al_ext_cache);
 
+	drbd_io_bio_set      = NULL;
 	drbd_md_io_bio_set   = NULL;
 	drbd_md_io_page_pool = NULL;
 	drbd_ee_mempool      = NULL;
@@ -2142,6 +2143,7 @@ static int drbd_create_mempools(void)
 	drbd_pp_pool         = NULL;
 	drbd_md_io_page_pool = NULL;
 	drbd_md_io_bio_set   = NULL;
+	drbd_io_bio_set      = NULL;
 
 	/* caches */
 	drbd_request_cache = kmem_cache_create(
@@ -2165,7 +2167,13 @@ static int drbd_create_mempools(void)
 		goto Enomem;
 
 	/* mempools */
-	drbd_md_io_bio_set = bioset_create(DRBD_MIN_POOL_PAGES, 0);
+	drbd_io_bio_set = bioset_create(BIO_POOL_SIZE, 0, BIOSET_NEED_RESCUER);
+	if (drbd_io_bio_set == NULL)
+		goto Enomem;
+
+	drbd_md_io_bio_set = bioset_create(DRBD_MIN_POOL_PAGES, 0,
+					   BIOSET_NEED_BVECS |
+					   BIOSET_NEED_RESCUER);
 	if (drbd_md_io_bio_set == NULL)
 		goto Enomem;
 
@@ -2839,7 +2847,6 @@ enum drbd_ret_code drbd_create_device(struct drbd_config_context *adm_ctx, unsig
 	/* 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);
-	blk_queue_bounce_limit(q, BLK_BOUNCE_ANY);
 	q->queue_lock = &resource->req_lock;
 
 	device->md_io.page = alloc_page(GFP_KERNEL);
diff --git a/drivers/block/drbd/drbd_nl.c b/drivers/block/drbd/drbd_nl.c
index 02255a0d68b9..ad0fcb43e45c 100644
--- a/drivers/block/drbd/drbd_nl.c
+++ b/drivers/block/drbd/drbd_nl.c
@@ -2294,7 +2294,7 @@ _check_net_options(struct drbd_connection *connection, struct net_conf *old_net_
 static enum drbd_ret_code
 check_net_options(struct drbd_connection *connection, struct net_conf *new_net_conf)
 {
-	static enum drbd_ret_code rv;
+	enum drbd_ret_code rv;
 	struct drbd_peer_device *peer_device;
 	int i;
 
diff --git a/drivers/block/drbd/drbd_receiver.c b/drivers/block/drbd/drbd_receiver.c
index 1b0a2be24f39..c7e95e6380fb 100644
--- a/drivers/block/drbd/drbd_receiver.c
+++ b/drivers/block/drbd/drbd_receiver.c
@@ -1229,9 +1229,9 @@ void one_flush_endio(struct bio *bio)
 	struct drbd_device *device = octx->device;
 	struct issue_flush_context *ctx = octx->ctx;
 
-	if (bio->bi_error) {
-		ctx->error = bio->bi_error;
-		drbd_info(device, "local disk FLUSH FAILED with status %d\n", bio->bi_error);
+	if (bio->bi_status) {
+		ctx->error = blk_status_to_errno(bio->bi_status);
+		drbd_info(device, "local disk FLUSH FAILED with status %d\n", bio->bi_status);
 	}
 	kfree(octx);
 	bio_put(bio);
diff --git a/drivers/block/drbd/drbd_req.c b/drivers/block/drbd/drbd_req.c
index 656624314f0d..f6e865b2d543 100644
--- a/drivers/block/drbd/drbd_req.c
+++ b/drivers/block/drbd/drbd_req.c
@@ -203,7 +203,7 @@ void start_new_tl_epoch(struct drbd_connection *connection)
 void complete_master_bio(struct drbd_device *device,
 		struct bio_and_error *m)
 {
-	m->bio->bi_error = m->error;
+	m->bio->bi_status = errno_to_blk_status(m->error);
 	bio_endio(m->bio);
 	dec_ap_bio(device);
 }
@@ -1157,7 +1157,7 @@ static void drbd_process_discard_req(struct drbd_request *req)
 
 	if (blkdev_issue_zeroout(bdev, req->i.sector, req->i.size >> 9,
 			GFP_NOIO, 0))
-		req->private_bio->bi_error = -EIO;
+		req->private_bio->bi_status = BLK_STS_IOERR;
 	bio_endio(req->private_bio);
 }
 
@@ -1225,7 +1225,7 @@ drbd_request_prepare(struct drbd_device *device, struct bio *bio, unsigned long
 		/* only pass the error to the upper layers.
 		 * if user cannot handle io errors, that's not our business. */
 		drbd_err(device, "could not kmalloc() req\n");
-		bio->bi_error = -ENOMEM;
+		bio->bi_status = BLK_STS_RESOURCE;
 		bio_endio(bio);
 		return ERR_PTR(-ENOMEM);
 	}
@@ -1560,7 +1560,7 @@ blk_qc_t drbd_make_request(struct request_queue *q, struct bio *bio)
 	struct drbd_device *device = (struct drbd_device *) q->queuedata;
 	unsigned long start_jif;
 
-	blk_queue_split(q, &bio, q->bio_split);
+	blk_queue_split(q, &bio);
 
 	start_jif = jiffies;
 
diff --git a/drivers/block/drbd/drbd_req.h b/drivers/block/drbd/drbd_req.h
index eb49e7f2da91..9e1866ab238f 100644
--- a/drivers/block/drbd/drbd_req.h
+++ b/drivers/block/drbd/drbd_req.h
@@ -263,7 +263,7 @@ enum drbd_req_state_bits {
 static inline void drbd_req_make_private_bio(struct drbd_request *req, struct bio *bio_src)
 {
 	struct bio *bio;
-	bio = bio_clone(bio_src, GFP_NOIO); /* XXX cannot fail?? */
+	bio = bio_clone_fast(bio_src, GFP_NOIO, drbd_io_bio_set);
 
 	req->private_bio = bio;
 
diff --git a/drivers/block/drbd/drbd_worker.c b/drivers/block/drbd/drbd_worker.c
index 1afcb4e02d8d..1d8726a8df34 100644
--- a/drivers/block/drbd/drbd_worker.c
+++ b/drivers/block/drbd/drbd_worker.c
@@ -63,7 +63,7 @@ void drbd_md_endio(struct bio *bio)
 	struct drbd_device *device;
 
 	device = bio->bi_private;
-	device->md_io.error = bio->bi_error;
+	device->md_io.error = blk_status_to_errno(bio->bi_status);
 
 	/* We grabbed an extra reference in _drbd_md_sync_page_io() to be able
 	 * to timeout on the lower level device, and eventually detach from it.
@@ -177,13 +177,13 @@ void drbd_peer_request_endio(struct bio *bio)
 	bool is_discard = bio_op(bio) == REQ_OP_WRITE_ZEROES ||
 			  bio_op(bio) == REQ_OP_DISCARD;
 
-	if (bio->bi_error && __ratelimit(&drbd_ratelimit_state))
+	if (bio->bi_status && __ratelimit(&drbd_ratelimit_state))
 		drbd_warn(device, "%s: error=%d s=%llus\n",
 				is_write ? (is_discard ? "discard" : "write")
-					: "read", bio->bi_error,
+					: "read", bio->bi_status,
 				(unsigned long long)peer_req->i.sector);
 
-	if (bio->bi_error)
+	if (bio->bi_status)
 		set_bit(__EE_WAS_ERROR, &peer_req->flags);
 
 	bio_put(bio); /* no need for the bio anymore */
@@ -243,16 +243,16 @@ void drbd_request_endio(struct bio *bio)
 		if (__ratelimit(&drbd_ratelimit_state))
 			drbd_emerg(device, "delayed completion of aborted local request; disk-timeout may be too aggressive\n");
 
-		if (!bio->bi_error)
+		if (!bio->bi_status)
 			drbd_panic_after_delayed_completion_of_aborted_request(device);
 	}
 
 	/* to avoid recursion in __req_mod */
-	if (unlikely(bio->bi_error)) {
+	if (unlikely(bio->bi_status)) {
 		switch (bio_op(bio)) {
 		case REQ_OP_WRITE_ZEROES:
 		case REQ_OP_DISCARD:
-			if (bio->bi_error == -EOPNOTSUPP)
+			if (bio->bi_status == BLK_STS_NOTSUPP)
 				what = DISCARD_COMPLETED_NOTSUPP;
 			else
 				what = DISCARD_COMPLETED_WITH_ERROR;
@@ -272,7 +272,7 @@ void drbd_request_endio(struct bio *bio)
 	}
 
 	bio_put(req->private_bio);
-	req->private_bio = ERR_PTR(bio->bi_error);
+	req->private_bio = ERR_PTR(blk_status_to_errno(bio->bi_status));
 
 	/* not req_mod(), we need irqsave here! */
 	spin_lock_irqsave(&device->resource->req_lock, flags);
diff --git a/drivers/block/floppy.c b/drivers/block/floppy.c
index 60d4c7653178..ce823647a9c4 100644
--- a/drivers/block/floppy.c
+++ b/drivers/block/floppy.c
@@ -2202,7 +2202,7 @@ static int do_format(int drive, struct format_descr *tmp_format_req)
  * =============================
  */
 
-static void floppy_end_request(struct request *req, int error)
+static void floppy_end_request(struct request *req, blk_status_t error)
 {
 	unsigned int nr_sectors = current_count_sectors;
 	unsigned int drive = (unsigned long)req->rq_disk->private_data;
@@ -2263,7 +2263,7 @@ static void request_done(int uptodate)
 			DRWE->last_error_generation = DRS->generation;
 		}
 		spin_lock_irqsave(q->queue_lock, flags);
-		floppy_end_request(req, -EIO);
+		floppy_end_request(req, BLK_STS_IOERR);
 		spin_unlock_irqrestore(q->queue_lock, flags);
 	}
 }
@@ -3780,9 +3780,9 @@ static void floppy_rb0_cb(struct bio *bio)
 	struct rb0_cbdata *cbdata = (struct rb0_cbdata *)bio->bi_private;
 	int drive = cbdata->drive;
 
-	if (bio->bi_error) {
+	if (bio->bi_status) {
 		pr_info("floppy: error %d while reading block 0\n",
-			bio->bi_error);
+			bio->bi_status);
 		set_bit(FD_OPEN_SHOULD_FAIL_BIT, &UDRS->flags);
 	}
 	complete(&cbdata->complete);
@@ -4203,6 +4203,7 @@ static int __init do_floppy_init(void)
 			goto out_put_disk;
 		}
 
+		blk_queue_bounce_limit(disks[drive]->queue, BLK_BOUNCE_HIGH);
 		blk_queue_max_hw_sectors(disks[drive]->queue, 64);
 		disks[drive]->major = FLOPPY_MAJOR;
 		disks[drive]->first_minor = TOMINOR(drive);
diff --git a/drivers/block/loop.c b/drivers/block/loop.c
index ebbd0c3fe0ed..ef8334949b42 100644
--- a/drivers/block/loop.c
+++ b/drivers/block/loop.c
@@ -221,7 +221,8 @@ static void __loop_update_dio(struct loop_device *lo, bool dio)
 }
 
 static int
-figure_loop_size(struct loop_device *lo, loff_t offset, loff_t sizelimit)
+figure_loop_size(struct loop_device *lo, loff_t offset, loff_t sizelimit,
+		 loff_t logical_blocksize)
 {
 	loff_t size = get_size(offset, sizelimit, lo->lo_backing_file);
 	sector_t x = (sector_t)size;
@@ -233,6 +234,12 @@ figure_loop_size(struct loop_device *lo, loff_t offset, loff_t sizelimit)
 		lo->lo_offset = offset;
 	if (lo->lo_sizelimit != sizelimit)
 		lo->lo_sizelimit = sizelimit;
+	if (lo->lo_flags & LO_FLAGS_BLOCKSIZE) {
+		lo->lo_logical_blocksize = logical_blocksize;
+		blk_queue_physical_block_size(lo->lo_queue, lo->lo_blocksize);
+		blk_queue_logical_block_size(lo->lo_queue,
+					     lo->lo_logical_blocksize);
+	}
 	set_capacity(lo->lo_disk, x);
 	bd_set_size(bdev, (loff_t)get_capacity(bdev->bd_disk) << 9);
 	/* let user-space know about the new size */
@@ -266,7 +273,7 @@ static int lo_write_bvec(struct file *file, struct bio_vec *bvec, loff_t *ppos)
 	iov_iter_bvec(&i, ITER_BVEC, bvec, 1, bvec->bv_len);
 
 	file_start_write(file);
-	bw = vfs_iter_write(file, &i, ppos);
+	bw = vfs_iter_write(file, &i, ppos, 0);
 	file_end_write(file);
 
 	if (likely(bw ==  bvec->bv_len))
@@ -342,7 +349,7 @@ static int lo_read_simple(struct loop_device *lo, struct request *rq,
 
 	rq_for_each_segment(bvec, rq, iter) {
 		iov_iter_bvec(&i, ITER_BVEC, &bvec, 1, bvec.bv_len);
-		len = vfs_iter_read(lo->lo_backing_file, &i, &pos);
+		len = vfs_iter_read(lo->lo_backing_file, &i, &pos, 0);
 		if (len < 0)
 			return len;
 
@@ -383,7 +390,7 @@ static int lo_read_transfer(struct loop_device *lo, struct request *rq,
 		b.bv_len = bvec.bv_len;
 
 		iov_iter_bvec(&i, ITER_BVEC, &b, 1, b.bv_len);
-		len = vfs_iter_read(lo->lo_backing_file, &i, &pos);
+		len = vfs_iter_read(lo->lo_backing_file, &i, &pos, 0);
 		if (len < 0) {
 			ret = len;
 			goto out_free_page;
@@ -457,7 +464,7 @@ static void lo_complete_rq(struct request *rq)
 		zero_fill_bio(bio);
 	}
 
-	blk_mq_end_request(rq, cmd->ret < 0 ? -EIO : 0);
+	blk_mq_end_request(rq, cmd->ret < 0 ? BLK_STS_IOERR : BLK_STS_OK);
 }
 
 static void lo_rw_aio_complete(struct kiocb *iocb, long ret, long ret2)
@@ -813,6 +820,7 @@ static void loop_config_discard(struct loop_device *lo)
 	struct file *file = lo->lo_backing_file;
 	struct inode *inode = file->f_mapping->host;
 	struct request_queue *q = lo->lo_queue;
+	int lo_bits = 9;
 
 	/*
 	 * We use punch hole to reclaim the free space used by the
@@ -832,8 +840,11 @@ static void loop_config_discard(struct loop_device *lo)
 
 	q->limits.discard_granularity = inode->i_sb->s_blocksize;
 	q->limits.discard_alignment = 0;
-	blk_queue_max_discard_sectors(q, UINT_MAX >> 9);
-	blk_queue_max_write_zeroes_sectors(q, UINT_MAX >> 9);
+	if (lo->lo_flags & LO_FLAGS_BLOCKSIZE)
+		lo_bits = blksize_bits(lo->lo_logical_blocksize);
+
+	blk_queue_max_discard_sectors(q, UINT_MAX >> lo_bits);
+	blk_queue_max_write_zeroes_sectors(q, UINT_MAX >> lo_bits);
 	queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, q);
 }
 
@@ -843,10 +854,16 @@ static void loop_unprepare_queue(struct loop_device *lo)
 	kthread_stop(lo->worker_task);
 }
 
+static int loop_kthread_worker_fn(void *worker_ptr)
+{
+	current->flags |= PF_LESS_THROTTLE;
+	return kthread_worker_fn(worker_ptr);
+}
+
 static int loop_prepare_queue(struct loop_device *lo)
 {
 	kthread_init_worker(&lo->worker);
-	lo->worker_task = kthread_run(kthread_worker_fn,
+	lo->worker_task = kthread_run(loop_kthread_worker_fn,
 			&lo->worker, "loop%d", lo->lo_number);
 	if (IS_ERR(lo->worker_task))
 		return -ENOMEM;
@@ -921,6 +938,7 @@ static int loop_set_fd(struct loop_device *lo, fmode_t mode,
 
 	lo->use_dio = false;
 	lo->lo_blocksize = lo_blocksize;
+	lo->lo_logical_blocksize = 512;
 	lo->lo_device = bdev;
 	lo->lo_flags = lo_flags;
 	lo->lo_backing_file = file;
@@ -1086,6 +1104,7 @@ loop_set_status(struct loop_device *lo, const struct loop_info64 *info)
 	int err;
 	struct loop_func_table *xfer;
 	kuid_t uid = current_uid();
+	int lo_flags = lo->lo_flags;
 
 	if (lo->lo_encrypt_key_size &&
 	    !uid_eq(lo->lo_key_owner, uid) &&
@@ -1118,12 +1137,30 @@ loop_set_status(struct loop_device *lo, const struct loop_info64 *info)
 	if (err)
 		goto exit;
 
+	if (info->lo_flags & LO_FLAGS_BLOCKSIZE) {
+		if (!(lo->lo_flags & LO_FLAGS_BLOCKSIZE))
+			lo->lo_logical_blocksize = 512;
+		lo->lo_flags |= LO_FLAGS_BLOCKSIZE;
+		if (LO_INFO_BLOCKSIZE(info) != 512 &&
+		    LO_INFO_BLOCKSIZE(info) != 1024 &&
+		    LO_INFO_BLOCKSIZE(info) != 2048 &&
+		    LO_INFO_BLOCKSIZE(info) != 4096)
+			return -EINVAL;
+		if (LO_INFO_BLOCKSIZE(info) > lo->lo_blocksize)
+			return -EINVAL;
+	}
+
 	if (lo->lo_offset != info->lo_offset ||
-	    lo->lo_sizelimit != info->lo_sizelimit)
-		if (figure_loop_size(lo, info->lo_offset, info->lo_sizelimit)) {
+	    lo->lo_sizelimit != info->lo_sizelimit ||
+	    lo->lo_flags != lo_flags ||
+	    ((lo->lo_flags & LO_FLAGS_BLOCKSIZE) &&
+	     lo->lo_logical_blocksize != LO_INFO_BLOCKSIZE(info))) {
+		if (figure_loop_size(lo, info->lo_offset, info->lo_sizelimit,
+				     LO_INFO_BLOCKSIZE(info))) {
 			err = -EFBIG;
 			goto exit;
 		}
+	}
 
 	loop_config_discard(lo);
 
@@ -1306,12 +1343,13 @@ loop_get_status64(struct loop_device *lo, struct loop_info64 __user *arg) {
 	return err;
 }
 
-static int loop_set_capacity(struct loop_device *lo, struct block_device *bdev)
+static int loop_set_capacity(struct loop_device *lo)
 {
 	if (unlikely(lo->lo_state != Lo_bound))
 		return -ENXIO;
 
-	return figure_loop_size(lo, lo->lo_offset, lo->lo_sizelimit);
+	return figure_loop_size(lo, lo->lo_offset, lo->lo_sizelimit,
+				lo->lo_logical_blocksize);
 }
 
 static int loop_set_dio(struct loop_device *lo, unsigned long arg)
@@ -1369,7 +1407,7 @@ static int lo_ioctl(struct block_device *bdev, fmode_t mode,
 	case LOOP_SET_CAPACITY:
 		err = -EPERM;
 		if ((mode & FMODE_WRITE) || capable(CAP_SYS_ADMIN))
-			err = loop_set_capacity(lo, bdev);
+			err = loop_set_capacity(lo);
 		break;
 	case LOOP_SET_DIRECT_IO:
 		err = -EPERM;
@@ -1645,7 +1683,7 @@ int loop_unregister_transfer(int number)
 EXPORT_SYMBOL(loop_register_transfer);
 EXPORT_SYMBOL(loop_unregister_transfer);
 
-static int loop_queue_rq(struct blk_mq_hw_ctx *hctx,
+static blk_status_t loop_queue_rq(struct blk_mq_hw_ctx *hctx,
 		const struct blk_mq_queue_data *bd)
 {
 	struct loop_cmd *cmd = blk_mq_rq_to_pdu(bd->rq);
@@ -1654,7 +1692,7 @@ static int loop_queue_rq(struct blk_mq_hw_ctx *hctx,
 	blk_mq_start_request(bd->rq);
 
 	if (lo->lo_state != Lo_bound)
-		return BLK_MQ_RQ_QUEUE_ERROR;
+		return BLK_STS_IOERR;
 
 	switch (req_op(cmd->rq)) {
 	case REQ_OP_FLUSH:
@@ -1669,7 +1707,7 @@ static int loop_queue_rq(struct blk_mq_hw_ctx *hctx,
 
 	kthread_queue_work(&lo->worker, &cmd->work);
 
-	return BLK_MQ_RQ_QUEUE_OK;
+	return BLK_STS_OK;
 }
 
 static void loop_handle_cmd(struct loop_cmd *cmd)
diff --git a/drivers/block/loop.h b/drivers/block/loop.h
index fecd3f97ef8c..2c096b9a17b8 100644
--- a/drivers/block/loop.h
+++ b/drivers/block/loop.h
@@ -49,6 +49,7 @@ struct loop_device {
 	struct file *	lo_backing_file;
 	struct block_device *lo_device;
 	unsigned	lo_blocksize;
+	unsigned	lo_logical_blocksize;
 	void		*key_data; 
 
 	gfp_t		old_gfp_mask;
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index 3a779a4f5653..61b046f256ca 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -532,7 +532,7 @@ static int mtip_read_log_page(struct mtip_port *port, u8 page, u16 *buffer,
 static int mtip_get_smart_attr(struct mtip_port *port, unsigned int id,
 						struct smart_attr *attrib);
 
-static void mtip_complete_command(struct mtip_cmd *cmd, int status)
+static void mtip_complete_command(struct mtip_cmd *cmd, blk_status_t status)
 {
 	struct request *req = blk_mq_rq_from_pdu(cmd);
 
@@ -568,7 +568,7 @@ static void mtip_handle_tfe(struct driver_data *dd)
 	if (test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags)) {
 		cmd = mtip_cmd_from_tag(dd, MTIP_TAG_INTERNAL);
 		dbg_printk(MTIP_DRV_NAME " TFE for the internal command\n");
-		mtip_complete_command(cmd, -EIO);
+		mtip_complete_command(cmd, BLK_STS_IOERR);
 		return;
 	}
 
@@ -667,7 +667,7 @@ static void mtip_handle_tfe(struct driver_data *dd)
 					tag,
 					fail_reason != NULL ?
 						fail_reason : "unknown");
-					mtip_complete_command(cmd, -ENODATA);
+					mtip_complete_command(cmd, BLK_STS_MEDIUM);
 					continue;
 				}
 			}
@@ -690,7 +690,7 @@ static void mtip_handle_tfe(struct driver_data *dd)
 			dev_warn(&port->dd->pdev->dev,
 				"retiring tag %d\n", tag);
 
-			mtip_complete_command(cmd, -EIO);
+			mtip_complete_command(cmd, BLK_STS_IOERR);
 		}
 	}
 	print_tags(dd, "reissued (TFE)", tagaccum, cmd_cnt);
@@ -1063,23 +1063,10 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 	/* insert request and run queue */
 	blk_execute_rq(rq->q, NULL, rq, true);
 
-	rv = int_cmd->status;
-	if (rv < 0) {
-		if (rv == -ERESTARTSYS) { /* interrupted */
-			dev_err(&dd->pdev->dev,
-				"Internal command [%02X] was interrupted after %u ms\n",
-				fis->command,
-				jiffies_to_msecs(jiffies - start));
-			rv = -EINTR;
-			goto exec_ic_exit;
-		} else if (rv == 0) /* timeout */
-			dev_err(&dd->pdev->dev,
-				"Internal command did not complete [%02X] within timeout of  %lu ms\n",
-				fis->command, timeout);
-		else
-			dev_err(&dd->pdev->dev,
-				"Internal command [%02X] wait returned code [%d] after %lu ms - unhandled\n",
-				fis->command, rv, timeout);
+	if (int_cmd->status) {
+		dev_err(&dd->pdev->dev, "Internal command [%02X] failed %d\n",
+				fis->command, int_cmd->status);
+		rv = -EIO;
 
 		if (mtip_check_surprise_removal(dd->pdev) ||
 			test_bit(MTIP_DDF_REMOVE_PENDING_BIT,
@@ -2753,7 +2740,7 @@ static void mtip_abort_cmd(struct request *req, void *data,
 	dbg_printk(MTIP_DRV_NAME " Aborting request, tag = %d\n", req->tag);
 
 	clear_bit(req->tag, dd->port->cmds_to_issue);
-	cmd->status = -EIO;
+	cmd->status = BLK_STS_IOERR;
 	mtip_softirq_done_fn(req);
 }
 
@@ -3597,7 +3584,7 @@ static int mtip_submit_request(struct blk_mq_hw_ctx *hctx, struct request *rq)
 		int err;
 
 		err = mtip_send_trim(dd, blk_rq_pos(rq), blk_rq_sectors(rq));
-		blk_mq_end_request(rq, err);
+		blk_mq_end_request(rq, err ? BLK_STS_IOERR : BLK_STS_OK);
 		return 0;
 	}
 
@@ -3633,8 +3620,8 @@ static bool mtip_check_unal_depth(struct blk_mq_hw_ctx *hctx,
 	return false;
 }
 
-static int mtip_issue_reserved_cmd(struct blk_mq_hw_ctx *hctx,
-				   struct request *rq)
+static blk_status_t mtip_issue_reserved_cmd(struct blk_mq_hw_ctx *hctx,
+		struct request *rq)
 {
 	struct driver_data *dd = hctx->queue->queuedata;
 	struct mtip_int_cmd *icmd = rq->special;
@@ -3642,7 +3629,7 @@ static int mtip_issue_reserved_cmd(struct blk_mq_hw_ctx *hctx,
 	struct mtip_cmd_sg *command_sg;
 
 	if (mtip_commands_active(dd->port))
-		return BLK_MQ_RQ_QUEUE_BUSY;
+		return BLK_STS_RESOURCE;
 
 	/* Populate the SG list */
 	cmd->command_header->opts =
@@ -3666,10 +3653,10 @@ static int mtip_issue_reserved_cmd(struct blk_mq_hw_ctx *hctx,
 
 	blk_mq_start_request(rq);
 	mtip_issue_non_ncq_command(dd->port, rq->tag);
-	return BLK_MQ_RQ_QUEUE_OK;
+	return 0;
 }
 
-static int mtip_queue_rq(struct blk_mq_hw_ctx *hctx,
+static blk_status_t mtip_queue_rq(struct blk_mq_hw_ctx *hctx,
 			 const struct blk_mq_queue_data *bd)
 {
 	struct request *rq = bd->rq;
@@ -3681,15 +3668,14 @@ static int mtip_queue_rq(struct blk_mq_hw_ctx *hctx,
 		return mtip_issue_reserved_cmd(hctx, rq);
 
 	if (unlikely(mtip_check_unal_depth(hctx, rq)))
-		return BLK_MQ_RQ_QUEUE_BUSY;
+		return BLK_STS_RESOURCE;
 
 	blk_mq_start_request(rq);
 
 	ret = mtip_submit_request(hctx, rq);
 	if (likely(!ret))
-		return BLK_MQ_RQ_QUEUE_OK;
-
-	return BLK_MQ_RQ_QUEUE_ERROR;
+		return BLK_STS_OK;
+	return BLK_STS_IOERR;
 }
 
 static void mtip_free_cmd(struct blk_mq_tag_set *set, struct request *rq,
@@ -3730,7 +3716,7 @@ static enum blk_eh_timer_return mtip_cmd_timeout(struct request *req,
 	if (reserved) {
 		struct mtip_cmd *cmd = blk_mq_rq_to_pdu(req);
 
-		cmd->status = -ETIME;
+		cmd->status = BLK_STS_TIMEOUT;
 		return BLK_EH_HANDLED;
 	}
 
@@ -3961,7 +3947,7 @@ static void mtip_no_dev_cleanup(struct request *rq, void *data, bool reserv)
 {
 	struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
 
-	cmd->status = -ENODEV;
+	cmd->status = BLK_STS_IOERR;
 	blk_mq_complete_request(rq);
 }
 
diff --git a/drivers/block/mtip32xx/mtip32xx.h b/drivers/block/mtip32xx/mtip32xx.h
index 37b8e3e0bb78..e8286af50e16 100644
--- a/drivers/block/mtip32xx/mtip32xx.h
+++ b/drivers/block/mtip32xx/mtip32xx.h
@@ -342,7 +342,7 @@ struct mtip_cmd {
 	int retries; /* The number of retries left for this command. */
 
 	int direction; /* Data transfer direction */
-	int status;
+	blk_status_t status;
 };
 
 /* Structure used to describe a port. */
diff --git a/drivers/block/nbd.c b/drivers/block/nbd.c
index f3f191ba8ca4..977ec960dd2f 100644
--- a/drivers/block/nbd.c
+++ b/drivers/block/nbd.c
@@ -116,7 +116,7 @@ struct nbd_cmd {
 	int index;
 	int cookie;
 	struct completion send_complete;
-	int status;
+	blk_status_t status;
 };
 
 #if IS_ENABLED(CONFIG_DEBUG_FS)
@@ -286,7 +286,7 @@ static enum blk_eh_timer_return nbd_xmit_timeout(struct request *req,
 	struct nbd_config *config;
 
 	if (!refcount_inc_not_zero(&nbd->config_refs)) {
-		cmd->status = -EIO;
+		cmd->status = BLK_STS_TIMEOUT;
 		return BLK_EH_HANDLED;
 	}
 
@@ -331,7 +331,7 @@ static enum blk_eh_timer_return nbd_xmit_timeout(struct request *req,
 				    "Connection timed out\n");
 	}
 	set_bit(NBD_TIMEDOUT, &config->runtime_flags);
-	cmd->status = -EIO;
+	cmd->status = BLK_STS_IOERR;
 	sock_shutdown(nbd);
 	nbd_config_put(nbd);
 
@@ -400,6 +400,7 @@ static int nbd_send_cmd(struct nbd_device *nbd, struct nbd_cmd *cmd, int index)
 	unsigned long size = blk_rq_bytes(req);
 	struct bio *bio;
 	u32 type;
+	u32 nbd_cmd_flags = 0;
 	u32 tag = blk_mq_unique_tag(req);
 	int sent = nsock->sent, skip = 0;
 
@@ -429,6 +430,9 @@ static int nbd_send_cmd(struct nbd_device *nbd, struct nbd_cmd *cmd, int index)
 		return -EIO;
 	}
 
+	if (req->cmd_flags & REQ_FUA)
+		nbd_cmd_flags |= NBD_CMD_FLAG_FUA;
+
 	/* We did a partial send previously, and we at least sent the whole
 	 * request struct, so just go and send the rest of the pages in the
 	 * request.
@@ -442,7 +446,7 @@ static int nbd_send_cmd(struct nbd_device *nbd, struct nbd_cmd *cmd, int index)
 	}
 	cmd->index = index;
 	cmd->cookie = nsock->cookie;
-	request.type = htonl(type);
+	request.type = htonl(type | nbd_cmd_flags);
 	if (type != NBD_CMD_FLUSH) {
 		request.from = cpu_to_be64((u64)blk_rq_pos(req) << 9);
 		request.len = htonl(size);
@@ -465,7 +469,7 @@ static int nbd_send_cmd(struct nbd_device *nbd, struct nbd_cmd *cmd, int index)
 				nsock->pending = req;
 				nsock->sent = sent;
 			}
-			return BLK_MQ_RQ_QUEUE_BUSY;
+			return BLK_STS_RESOURCE;
 		}
 		dev_err_ratelimited(disk_to_dev(nbd->disk),
 			"Send control failed (result %d)\n", result);
@@ -506,7 +510,7 @@ send_pages:
 					 */
 					nsock->pending = req;
 					nsock->sent = sent;
-					return BLK_MQ_RQ_QUEUE_BUSY;
+					return BLK_STS_RESOURCE;
 				}
 				dev_err(disk_to_dev(nbd->disk),
 					"Send data failed (result %d)\n",
@@ -574,7 +578,7 @@ static struct nbd_cmd *nbd_read_stat(struct nbd_device *nbd, int index)
 	if (ntohl(reply.error)) {
 		dev_err(disk_to_dev(nbd->disk), "Other side returned error (%d)\n",
 			ntohl(reply.error));
-		cmd->status = -EIO;
+		cmd->status = BLK_STS_IOERR;
 		return cmd;
 	}
 
@@ -599,7 +603,7 @@ static struct nbd_cmd *nbd_read_stat(struct nbd_device *nbd, int index)
 				 */
 				if (nbd_disconnected(config) ||
 				    config->num_connections <= 1) {
-					cmd->status = -EIO;
+					cmd->status = BLK_STS_IOERR;
 					return cmd;
 				}
 				return ERR_PTR(-EIO);
@@ -651,7 +655,7 @@ static void nbd_clear_req(struct request *req, void *data, bool reserved)
 	if (!blk_mq_request_started(req))
 		return;
 	cmd = blk_mq_rq_to_pdu(req);
-	cmd->status = -EIO;
+	cmd->status = BLK_STS_IOERR;
 	blk_mq_complete_request(req);
 }
 
@@ -740,7 +744,7 @@ static int nbd_handle_cmd(struct nbd_cmd *cmd, int index)
 		nbd_config_put(nbd);
 		return -EINVAL;
 	}
-	cmd->status = 0;
+	cmd->status = BLK_STS_OK;
 again:
 	nsock = config->socks[index];
 	mutex_lock(&nsock->tx_lock);
@@ -794,7 +798,7 @@ out:
 	return ret;
 }
 
-static int nbd_queue_rq(struct blk_mq_hw_ctx *hctx,
+static blk_status_t nbd_queue_rq(struct blk_mq_hw_ctx *hctx,
 			const struct blk_mq_queue_data *bd)
 {
 	struct nbd_cmd *cmd = blk_mq_rq_to_pdu(bd->rq);
@@ -818,13 +822,9 @@ static int nbd_queue_rq(struct blk_mq_hw_ctx *hctx,
 	 * appropriate.
 	 */
 	ret = nbd_handle_cmd(cmd, hctx->queue_num);
-	if (ret < 0)
-		ret = BLK_MQ_RQ_QUEUE_ERROR;
-	if (!ret)
-		ret = BLK_MQ_RQ_QUEUE_OK;
 	complete(&cmd->send_complete);
 
-	return ret;
+	return ret < 0 ? BLK_STS_IOERR : BLK_STS_OK;
 }
 
 static int nbd_add_socket(struct nbd_device *nbd, unsigned long arg,
@@ -910,6 +910,7 @@ static int nbd_reconnect_socket(struct nbd_device *nbd, unsigned long arg)
 			continue;
 		}
 		sk_set_memalloc(sock->sk);
+		sock->sk->sk_sndtimeo = nbd->tag_set.timeout;
 		atomic_inc(&config->recv_threads);
 		refcount_inc(&nbd->config_refs);
 		old = nsock->sock;
@@ -957,8 +958,12 @@ static void nbd_parse_flags(struct nbd_device *nbd)
 		set_disk_ro(nbd->disk, false);
 	if (config->flags & NBD_FLAG_SEND_TRIM)
 		queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, nbd->disk->queue);
-	if (config->flags & NBD_FLAG_SEND_FLUSH)
-		blk_queue_write_cache(nbd->disk->queue, true, false);
+	if (config->flags & NBD_FLAG_SEND_FLUSH) {
+		if (config->flags & NBD_FLAG_SEND_FUA)
+			blk_queue_write_cache(nbd->disk->queue, true, true);
+		else
+			blk_queue_write_cache(nbd->disk->queue, true, false);
+	}
 	else
 		blk_queue_write_cache(nbd->disk->queue, false, false);
 }
@@ -1071,6 +1076,7 @@ static int nbd_start_device(struct nbd_device *nbd)
 			return -ENOMEM;
 		}
 		sk_set_memalloc(config->socks[i]->sock->sk);
+		config->socks[i]->sock->sk->sk_sndtimeo = nbd->tag_set.timeout;
 		atomic_inc(&config->recv_threads);
 		refcount_inc(&nbd->config_refs);
 		INIT_WORK(&args->work, recv_work);
@@ -1305,6 +1311,8 @@ static int nbd_dbg_flags_show(struct seq_file *s, void *unused)
 		seq_puts(s, "NBD_FLAG_READ_ONLY\n");
 	if (flags & NBD_FLAG_SEND_FLUSH)
 		seq_puts(s, "NBD_FLAG_SEND_FLUSH\n");
+	if (flags & NBD_FLAG_SEND_FUA)
+		seq_puts(s, "NBD_FLAG_SEND_FUA\n");
 	if (flags & NBD_FLAG_SEND_TRIM)
 		seq_puts(s, "NBD_FLAG_SEND_TRIM\n");
 
diff --git a/drivers/block/null_blk.c b/drivers/block/null_blk.c
index d946e1eeac8e..71f4422eba81 100644
--- a/drivers/block/null_blk.c
+++ b/drivers/block/null_blk.c
@@ -35,7 +35,8 @@ struct nullb {
 	struct request_queue *q;
 	struct gendisk *disk;
 	struct nvm_dev *ndev;
-	struct blk_mq_tag_set tag_set;
+	struct blk_mq_tag_set *tag_set;
+	struct blk_mq_tag_set __tag_set;
 	struct hrtimer timer;
 	unsigned int queue_depth;
 	spinlock_t lock;
@@ -50,6 +51,7 @@ static struct mutex lock;
 static int null_major;
 static int nullb_indexes;
 static struct kmem_cache *ppa_cache;
+static struct blk_mq_tag_set tag_set;
 
 enum {
 	NULL_IRQ_NONE		= 0,
@@ -109,7 +111,7 @@ static int bs = 512;
 module_param(bs, int, S_IRUGO);
 MODULE_PARM_DESC(bs, "Block size (in bytes)");
 
-static int nr_devices = 2;
+static int nr_devices = 1;
 module_param(nr_devices, int, S_IRUGO);
 MODULE_PARM_DESC(nr_devices, "Number of devices to register");
 
@@ -121,6 +123,10 @@ static bool blocking;
 module_param(blocking, bool, S_IRUGO);
 MODULE_PARM_DESC(blocking, "Register as a blocking blk-mq driver device");
 
+static bool shared_tags;
+module_param(shared_tags, bool, S_IRUGO);
+MODULE_PARM_DESC(shared_tags, "Share tag set between devices for blk-mq");
+
 static int irqmode = NULL_IRQ_SOFTIRQ;
 
 static int null_set_irqmode(const char *str, const struct kernel_param *kp)
@@ -229,11 +235,11 @@ static void end_cmd(struct nullb_cmd *cmd)
 
 	switch (queue_mode)  {
 	case NULL_Q_MQ:
-		blk_mq_end_request(cmd->rq, 0);
+		blk_mq_end_request(cmd->rq, BLK_STS_OK);
 		return;
 	case NULL_Q_RQ:
 		INIT_LIST_HEAD(&cmd->rq->queuelist);
-		blk_end_request_all(cmd->rq, 0);
+		blk_end_request_all(cmd->rq, BLK_STS_OK);
 		break;
 	case NULL_Q_BIO:
 		bio_endio(cmd->bio);
@@ -356,7 +362,7 @@ static void null_request_fn(struct request_queue *q)
 	}
 }
 
-static int null_queue_rq(struct blk_mq_hw_ctx *hctx,
+static blk_status_t null_queue_rq(struct blk_mq_hw_ctx *hctx,
 			 const struct blk_mq_queue_data *bd)
 {
 	struct nullb_cmd *cmd = blk_mq_rq_to_pdu(bd->rq);
@@ -373,34 +379,11 @@ static int null_queue_rq(struct blk_mq_hw_ctx *hctx,
 	blk_mq_start_request(bd->rq);
 
 	null_handle_cmd(cmd);
-	return BLK_MQ_RQ_QUEUE_OK;
-}
-
-static void null_init_queue(struct nullb *nullb, struct nullb_queue *nq)
-{
-	BUG_ON(!nullb);
-	BUG_ON(!nq);
-
-	init_waitqueue_head(&nq->wait);
-	nq->queue_depth = nullb->queue_depth;
-}
-
-static int null_init_hctx(struct blk_mq_hw_ctx *hctx, void *data,
-			  unsigned int index)
-{
-	struct nullb *nullb = data;
-	struct nullb_queue *nq = &nullb->queues[index];
-
-	hctx->driver_data = nq;
-	null_init_queue(nullb, nq);
-	nullb->nr_queues++;
-
-	return 0;
+	return BLK_STS_OK;
 }
 
 static const struct blk_mq_ops null_mq_ops = {
 	.queue_rq       = null_queue_rq,
-	.init_hctx	= null_init_hctx,
 	.complete	= null_softirq_done_fn,
 };
 
@@ -422,11 +405,12 @@ static void cleanup_queues(struct nullb *nullb)
 
 #ifdef CONFIG_NVM
 
-static void null_lnvm_end_io(struct request *rq, int error)
+static void null_lnvm_end_io(struct request *rq, blk_status_t status)
 {
 	struct nvm_rq *rqd = rq->end_io_data;
 
-	rqd->error = error;
+	/* XXX: lighnvm core seems to expect NVM_RSP_* values here.. */
+	rqd->error = status ? -EIO : 0;
 	nvm_end_io(rqd);
 
 	blk_put_request(rq);
@@ -591,8 +575,8 @@ static void null_del_dev(struct nullb *nullb)
 	else
 		del_gendisk(nullb->disk);
 	blk_cleanup_queue(nullb->q);
-	if (queue_mode == NULL_Q_MQ)
-		blk_mq_free_tag_set(&nullb->tag_set);
+	if (queue_mode == NULL_Q_MQ && nullb->tag_set == &nullb->__tag_set)
+		blk_mq_free_tag_set(nullb->tag_set);
 	if (!use_lightnvm)
 		put_disk(nullb->disk);
 	cleanup_queues(nullb);
@@ -614,6 +598,32 @@ static const struct block_device_operations null_fops = {
 	.release =	null_release,
 };
 
+static void null_init_queue(struct nullb *nullb, struct nullb_queue *nq)
+{
+	BUG_ON(!nullb);
+	BUG_ON(!nq);
+
+	init_waitqueue_head(&nq->wait);
+	nq->queue_depth = nullb->queue_depth;
+}
+
+static void null_init_queues(struct nullb *nullb)
+{
+	struct request_queue *q = nullb->q;
+	struct blk_mq_hw_ctx *hctx;
+	struct nullb_queue *nq;
+	int i;
+
+	queue_for_each_hw_ctx(q, hctx, i) {
+		if (!hctx->nr_ctx || !hctx->tags)
+			continue;
+		nq = &nullb->queues[i];
+		hctx->driver_data = nq;
+		null_init_queue(nullb, nq);
+		nullb->nr_queues++;
+	}
+}
+
 static int setup_commands(struct nullb_queue *nq)
 {
 	struct nullb_cmd *cmd;
@@ -694,6 +704,22 @@ static int null_gendisk_register(struct nullb *nullb)
 	return 0;
 }
 
+static int null_init_tag_set(struct blk_mq_tag_set *set)
+{
+	set->ops = &null_mq_ops;
+	set->nr_hw_queues = submit_queues;
+	set->queue_depth = hw_queue_depth;
+	set->numa_node = home_node;
+	set->cmd_size	= sizeof(struct nullb_cmd);
+	set->flags = BLK_MQ_F_SHOULD_MERGE;
+	set->driver_data = NULL;
+
+	if (blocking)
+		set->flags |= BLK_MQ_F_BLOCKING;
+
+	return blk_mq_alloc_tag_set(set);
+}
+
 static int null_add_dev(void)
 {
 	struct nullb *nullb;
@@ -715,26 +741,23 @@ static int null_add_dev(void)
 		goto out_free_nullb;
 
 	if (queue_mode == NULL_Q_MQ) {
-		nullb->tag_set.ops = &null_mq_ops;
-		nullb->tag_set.nr_hw_queues = submit_queues;
-		nullb->tag_set.queue_depth = hw_queue_depth;
-		nullb->tag_set.numa_node = home_node;
-		nullb->tag_set.cmd_size	= sizeof(struct nullb_cmd);
-		nullb->tag_set.flags = BLK_MQ_F_SHOULD_MERGE;
-		nullb->tag_set.driver_data = nullb;
-
-		if (blocking)
-			nullb->tag_set.flags |= BLK_MQ_F_BLOCKING;
-
-		rv = blk_mq_alloc_tag_set(&nullb->tag_set);
+		if (shared_tags) {
+			nullb->tag_set = &tag_set;
+			rv = 0;
+		} else {
+			nullb->tag_set = &nullb->__tag_set;
+			rv = null_init_tag_set(nullb->tag_set);
+		}
+
 		if (rv)
 			goto out_cleanup_queues;
 
-		nullb->q = blk_mq_init_queue(&nullb->tag_set);
+		nullb->q = blk_mq_init_queue(nullb->tag_set);
 		if (IS_ERR(nullb->q)) {
 			rv = -ENOMEM;
 			goto out_cleanup_tags;
 		}
+		null_init_queues(nullb);
 	} else if (queue_mode == NULL_Q_BIO) {
 		nullb->q = blk_alloc_queue_node(GFP_KERNEL, home_node);
 		if (!nullb->q) {
@@ -787,8 +810,8 @@ static int null_add_dev(void)
 out_cleanup_blk_queue:
 	blk_cleanup_queue(nullb->q);
 out_cleanup_tags:
-	if (queue_mode == NULL_Q_MQ)
-		blk_mq_free_tag_set(&nullb->tag_set);
+	if (queue_mode == NULL_Q_MQ && nullb->tag_set == &nullb->__tag_set)
+		blk_mq_free_tag_set(nullb->tag_set);
 out_cleanup_queues:
 	cleanup_queues(nullb);
 out_free_nullb:
@@ -821,6 +844,9 @@ static int __init null_init(void)
 		queue_mode = NULL_Q_MQ;
 	}
 
+	if (queue_mode == NULL_Q_MQ && shared_tags)
+		null_init_tag_set(&tag_set);
+
 	if (queue_mode == NULL_Q_MQ && use_per_node_hctx) {
 		if (submit_queues < nr_online_nodes) {
 			pr_warn("null_blk: submit_queues param is set to %u.",
@@ -881,6 +907,9 @@ static void __exit null_exit(void)
 	}
 	mutex_unlock(&lock);
 
+	if (queue_mode == NULL_Q_MQ && shared_tags)
+		blk_mq_free_tag_set(&tag_set);
+
 	kmem_cache_destroy(ppa_cache);
 }
 
diff --git a/drivers/block/paride/pcd.c b/drivers/block/paride/pcd.c
index b1267ef34d5a..7b8c6368beb7 100644
--- a/drivers/block/paride/pcd.c
+++ b/drivers/block/paride/pcd.c
@@ -305,6 +305,7 @@ static void pcd_init_units(void)
 			put_disk(disk);
 			continue;
 		}
+		blk_queue_bounce_limit(disk->queue, BLK_BOUNCE_HIGH);
 		cd->disk = disk;
 		cd->pi = &cd->pia;
 		cd->present = 0;
@@ -783,7 +784,7 @@ static void pcd_request(void)
 			ps_set_intr(do_pcd_read, NULL, 0, nice);
 			return;
 		} else {
-			__blk_end_request_all(pcd_req, -EIO);
+			__blk_end_request_all(pcd_req, BLK_STS_IOERR);
 			pcd_req = NULL;
 		}
 	}
@@ -794,7 +795,7 @@ static void do_pcd_request(struct request_queue *q)
 	pcd_request();
 }
 
-static inline void next_request(int err)
+static inline void next_request(blk_status_t err)
 {
 	unsigned long saved_flags;
 
@@ -837,7 +838,7 @@ static void pcd_start(void)
 
 	if (pcd_command(pcd_current, rd_cmd, 2048, "read block")) {
 		pcd_bufblk = -1;
-		next_request(-EIO);
+		next_request(BLK_STS_IOERR);
 		return;
 	}
 
@@ -871,7 +872,7 @@ static void do_pcd_read_drq(void)
 			return;
 		}
 		pcd_bufblk = -1;
-		next_request(-EIO);
+		next_request(BLK_STS_IOERR);
 		return;
 	}
 
diff --git a/drivers/block/paride/pd.c b/drivers/block/paride/pd.c
index 7d2402f90978..27a44b97393a 100644
--- a/drivers/block/paride/pd.c
+++ b/drivers/block/paride/pd.c
@@ -438,7 +438,7 @@ static void run_fsm(void)
 				phase = NULL;
 				spin_lock_irqsave(&pd_lock, saved_flags);
 				if (!__blk_end_request_cur(pd_req,
-						res == Ok ? 0 : -EIO)) {
+						res == Ok ? 0 : BLK_STS_IOERR)) {
 					if (!set_next_request())
 						stop = 1;
 				}
@@ -863,6 +863,7 @@ static void pd_probe_drive(struct pd_unit *disk)
 		return;
 	}
 	blk_queue_max_hw_sectors(p->queue, cluster);
+	blk_queue_bounce_limit(p->queue, BLK_BOUNCE_HIGH);
 
 	if (disk->drive == -1) {
 		for (disk->drive = 0; disk->drive <= 1; disk->drive++)
diff --git a/drivers/block/paride/pf.c b/drivers/block/paride/pf.c
index f24ca7315ddc..eef7a91f667d 100644
--- a/drivers/block/paride/pf.c
+++ b/drivers/block/paride/pf.c
@@ -293,6 +293,7 @@ static void __init pf_init_units(void)
 			return;
 		}
 		blk_queue_max_segments(disk->queue, cluster);
+		blk_queue_bounce_limit(disk->queue, BLK_BOUNCE_HIGH);
 		pf->disk = disk;
 		pf->pi = &pf->pia;
 		pf->media_status = PF_NM;
@@ -801,7 +802,7 @@ static int set_next_request(void)
 	return pf_req != NULL;
 }
 
-static void pf_end_request(int err)
+static void pf_end_request(blk_status_t err)
 {
 	if (pf_req && !__blk_end_request_cur(pf_req, err))
 		pf_req = NULL;
@@ -821,7 +822,7 @@ repeat:
 	pf_count = blk_rq_cur_sectors(pf_req);
 
 	if (pf_block + pf_count > get_capacity(pf_req->rq_disk)) {
-		pf_end_request(-EIO);
+		pf_end_request(BLK_STS_IOERR);
 		goto repeat;
 	}
 
@@ -836,7 +837,7 @@ repeat:
 		pi_do_claimed(pf_current->pi, do_pf_write);
 	else {
 		pf_busy = 0;
-		pf_end_request(-EIO);
+		pf_end_request(BLK_STS_IOERR);
 		goto repeat;
 	}
 }
@@ -868,7 +869,7 @@ static int pf_next_buf(void)
 	return 0;
 }
 
-static inline void next_request(int err)
+static inline void next_request(blk_status_t err)
 {
 	unsigned long saved_flags;
 
@@ -896,7 +897,7 @@ static void do_pf_read_start(void)
 			pi_do_claimed(pf_current->pi, do_pf_read_start);
 			return;
 		}
-		next_request(-EIO);
+		next_request(BLK_STS_IOERR);
 		return;
 	}
 	pf_mask = STAT_DRQ;
@@ -915,7 +916,7 @@ static void do_pf_read_drq(void)
 				pi_do_claimed(pf_current->pi, do_pf_read_start);
 				return;
 			}
-			next_request(-EIO);
+			next_request(BLK_STS_IOERR);
 			return;
 		}
 		pi_read_block(pf_current->pi, pf_buf, 512);
@@ -942,7 +943,7 @@ static void do_pf_write_start(void)
 			pi_do_claimed(pf_current->pi, do_pf_write_start);
 			return;
 		}
-		next_request(-EIO);
+		next_request(BLK_STS_IOERR);
 		return;
 	}
 
@@ -955,7 +956,7 @@ static void do_pf_write_start(void)
 				pi_do_claimed(pf_current->pi, do_pf_write_start);
 				return;
 			}
-			next_request(-EIO);
+			next_request(BLK_STS_IOERR);
 			return;
 		}
 		pi_write_block(pf_current->pi, pf_buf, 512);
@@ -975,7 +976,7 @@ static void do_pf_write_done(void)
 			pi_do_claimed(pf_current->pi, do_pf_write_start);
 			return;
 		}
-		next_request(-EIO);
+		next_request(BLK_STS_IOERR);
 		return;
 	}
 	pi_disconnect(pf_current->pi);
diff --git a/drivers/block/pktcdvd.c b/drivers/block/pktcdvd.c
index 205b865ebeb9..6b8b097abbb9 100644
--- a/drivers/block/pktcdvd.c
+++ b/drivers/block/pktcdvd.c
@@ -98,6 +98,7 @@ static int write_congestion_on  = PKT_WRITE_CONGESTION_ON;
 static int write_congestion_off = PKT_WRITE_CONGESTION_OFF;
 static struct mutex ctl_mutex;	/* Serialize open/close/setup/teardown */
 static mempool_t *psd_pool;
+static struct bio_set *pkt_bio_set;
 
 static struct class	*class_pktcdvd = NULL;    /* /sys/class/pktcdvd */
 static struct dentry	*pkt_debugfs_root = NULL; /* /sys/kernel/debug/pktcdvd */
@@ -348,9 +349,9 @@ static void class_pktcdvd_release(struct class *cls)
 {
 	kfree(cls);
 }
-static ssize_t class_pktcdvd_show_map(struct class *c,
-					struct class_attribute *attr,
-					char *data)
+
+static ssize_t device_map_show(struct class *c, struct class_attribute *attr,
+			       char *data)
 {
 	int n = 0;
 	int idx;
@@ -368,11 +369,10 @@ static ssize_t class_pktcdvd_show_map(struct class *c,
 	mutex_unlock(&ctl_mutex);
 	return n;
 }
+static CLASS_ATTR_RO(device_map);
 
-static ssize_t class_pktcdvd_store_add(struct class *c,
-					struct class_attribute *attr,
-					const char *buf,
-					size_t count)
+static ssize_t add_store(struct class *c, struct class_attribute *attr,
+			 const char *buf, size_t count)
 {
 	unsigned int major, minor;
 
@@ -390,11 +390,10 @@ static ssize_t class_pktcdvd_store_add(struct class *c,
 
 	return -EINVAL;
 }
+static CLASS_ATTR_WO(add);
 
-static ssize_t class_pktcdvd_store_remove(struct class *c,
-					  struct class_attribute *attr,
-					  const char *buf,
-					size_t count)
+static ssize_t remove_store(struct class *c, struct class_attribute *attr,
+			    const char *buf, size_t count)
 {
 	unsigned int major, minor;
 	if (sscanf(buf, "%u:%u", &major, &minor) == 2) {
@@ -403,14 +402,15 @@ static ssize_t class_pktcdvd_store_remove(struct class *c,
 	}
 	return -EINVAL;
 }
+static CLASS_ATTR_WO(remove);
 
-static struct class_attribute class_pktcdvd_attrs[] = {
- __ATTR(add,            0200, NULL, class_pktcdvd_store_add),
- __ATTR(remove,         0200, NULL, class_pktcdvd_store_remove),
- __ATTR(device_map,     0444, class_pktcdvd_show_map, NULL),
- __ATTR_NULL
+static struct attribute *class_pktcdvd_attrs[] = {
+	&class_attr_add.attr,
+	&class_attr_remove.attr,
+	&class_attr_device_map.attr,
+	NULL,
 };
-
+ATTRIBUTE_GROUPS(class_pktcdvd);
 
 static int pkt_sysfs_init(void)
 {
@@ -426,7 +426,7 @@ static int pkt_sysfs_init(void)
 	class_pktcdvd->name = DRIVER_NAME;
 	class_pktcdvd->owner = THIS_MODULE;
 	class_pktcdvd->class_release = class_pktcdvd_release;
-	class_pktcdvd->class_attrs = class_pktcdvd_attrs;
+	class_pktcdvd->class_groups = class_pktcdvd_groups;
 	ret = class_register(class_pktcdvd);
 	if (ret) {
 		kfree(class_pktcdvd);
@@ -707,7 +707,6 @@ static int pkt_generic_packet(struct pktcdvd_device *pd, struct packet_command *
 			     REQ_OP_SCSI_OUT : REQ_OP_SCSI_IN, __GFP_RECLAIM);
 	if (IS_ERR(rq))
 		return PTR_ERR(rq);
-	scsi_req_init(rq);
 
 	if (cgc->buflen) {
 		ret = blk_rq_map_kern(q, rq, cgc->buffer, cgc->buflen,
@@ -952,9 +951,9 @@ static void pkt_end_io_read(struct bio *bio)
 
 	pkt_dbg(2, pd, "bio=%p sec0=%llx sec=%llx err=%d\n",
 		bio, (unsigned long long)pkt->sector,
-		(unsigned long long)bio->bi_iter.bi_sector, bio->bi_error);
+		(unsigned long long)bio->bi_iter.bi_sector, bio->bi_status);
 
-	if (bio->bi_error)
+	if (bio->bi_status)
 		atomic_inc(&pkt->io_errors);
 	if (atomic_dec_and_test(&pkt->io_wait)) {
 		atomic_inc(&pkt->run_sm);
@@ -969,7 +968,7 @@ static void pkt_end_io_packet_write(struct bio *bio)
 	struct pktcdvd_device *pd = pkt->pd;
 	BUG_ON(!pd);
 
-	pkt_dbg(2, pd, "id=%d, err=%d\n", pkt->id, bio->bi_error);
+	pkt_dbg(2, pd, "id=%d, err=%d\n", pkt->id, bio->bi_status);
 
 	pd->stats.pkt_ended++;
 
@@ -1305,16 +1304,16 @@ static void pkt_start_write(struct pktcdvd_device *pd, struct packet_data *pkt)
 	pkt_queue_bio(pd, pkt->w_bio);
 }
 
-static void pkt_finish_packet(struct packet_data *pkt, int error)
+static void pkt_finish_packet(struct packet_data *pkt, blk_status_t status)
 {
 	struct bio *bio;
 
-	if (error)
+	if (status)
 		pkt->cache_valid = 0;
 
 	/* Finish all bios corresponding to this packet */
 	while ((bio = bio_list_pop(&pkt->orig_bios))) {
-		bio->bi_error = error;
+		bio->bi_status = status;
 		bio_endio(bio);
 	}
 }
@@ -1349,7 +1348,7 @@ static void pkt_run_state_machine(struct pktcdvd_device *pd, struct packet_data
 			if (atomic_read(&pkt->io_wait) > 0)
 				return;
 
-			if (!pkt->w_bio->bi_error) {
+			if (!pkt->w_bio->bi_status) {
 				pkt_set_state(pkt, PACKET_FINISHED_STATE);
 			} else {
 				pkt_set_state(pkt, PACKET_RECOVERY_STATE);
@@ -1366,7 +1365,7 @@ static void pkt_run_state_machine(struct pktcdvd_device *pd, struct packet_data
 			break;
 
 		case PACKET_FINISHED_STATE:
-			pkt_finish_packet(pkt, pkt->w_bio->bi_error);
+			pkt_finish_packet(pkt, pkt->w_bio->bi_status);
 			return;
 
 		default:
@@ -2301,7 +2300,7 @@ static void pkt_end_io_read_cloned(struct bio *bio)
 	struct packet_stacked_data *psd = bio->bi_private;
 	struct pktcdvd_device *pd = psd->pd;
 
-	psd->bio->bi_error = bio->bi_error;
+	psd->bio->bi_status = bio->bi_status;
 	bio_put(bio);
 	bio_endio(psd->bio);
 	mempool_free(psd, psd_pool);
@@ -2310,7 +2309,7 @@ static void pkt_end_io_read_cloned(struct bio *bio)
 
 static void pkt_make_request_read(struct pktcdvd_device *pd, struct bio *bio)
 {
-	struct bio *cloned_bio = bio_clone(bio, GFP_NOIO);
+	struct bio *cloned_bio = bio_clone_fast(bio, GFP_NOIO, pkt_bio_set);
 	struct packet_stacked_data *psd = mempool_alloc(psd_pool, GFP_NOIO);
 
 	psd->pd = pd;
@@ -2412,9 +2411,7 @@ static blk_qc_t pkt_make_request(struct request_queue *q, struct bio *bio)
 	char b[BDEVNAME_SIZE];
 	struct bio *split;
 
-	blk_queue_bounce(q, &bio);
-
-	blk_queue_split(q, &bio, q->bio_split);
+	blk_queue_split(q, &bio);
 
 	pd = q->queuedata;
 	if (!pd) {
@@ -2455,7 +2452,7 @@ static blk_qc_t pkt_make_request(struct request_queue *q, struct bio *bio)
 
 			split = bio_split(bio, last_zone -
 					  bio->bi_iter.bi_sector,
-					  GFP_NOIO, fs_bio_set);
+					  GFP_NOIO, pkt_bio_set);
 			bio_chain(split, bio);
 		} else {
 			split = bio;
@@ -2583,6 +2580,11 @@ static int pkt_new_dev(struct pktcdvd_device *pd, dev_t dev)
 	bdev = bdget(dev);
 	if (!bdev)
 		return -ENOMEM;
+	if (!blk_queue_scsi_passthrough(bdev_get_queue(bdev))) {
+		WARN_ONCE(true, "Attempt to register a non-SCSI queue\n");
+		bdput(bdev);
+		return -EINVAL;
+	}
 	ret = blkdev_get(bdev, FMODE_READ | FMODE_NDELAY, NULL);
 	if (ret)
 		return ret;
@@ -2919,6 +2921,11 @@ static int __init pkt_init(void)
 					sizeof(struct packet_stacked_data));
 	if (!psd_pool)
 		return -ENOMEM;
+	pkt_bio_set = bioset_create(BIO_POOL_SIZE, 0, 0);
+	if (!pkt_bio_set) {
+		mempool_destroy(psd_pool);
+		return -ENOMEM;
+	}
 
 	ret = register_blkdev(pktdev_major, DRIVER_NAME);
 	if (ret < 0) {
@@ -2951,6 +2958,7 @@ out:
 	unregister_blkdev(pktdev_major, DRIVER_NAME);
 out2:
 	mempool_destroy(psd_pool);
+	bioset_free(pkt_bio_set);
 	return ret;
 }
 
@@ -2964,6 +2972,7 @@ static void __exit pkt_exit(void)
 
 	unregister_blkdev(pktdev_major, DRIVER_NAME);
 	mempool_destroy(psd_pool);
+	bioset_free(pkt_bio_set);
 }
 
 MODULE_DESCRIPTION("Packet writing layer for CD/DVD drives");
diff --git a/drivers/block/ps3disk.c b/drivers/block/ps3disk.c
index a809e3e9feb8..075662f2cf46 100644
--- a/drivers/block/ps3disk.c
+++ b/drivers/block/ps3disk.c
@@ -158,7 +158,7 @@ static int ps3disk_submit_request_sg(struct ps3_storage_device *dev,
 	if (res) {
 		dev_err(&dev->sbd.core, "%s:%u: %s failed %d\n", __func__,
 			__LINE__, op, res);
-		__blk_end_request_all(req, -EIO);
+		__blk_end_request_all(req, BLK_STS_IOERR);
 		return 0;
 	}
 
@@ -180,7 +180,7 @@ static int ps3disk_submit_flush_request(struct ps3_storage_device *dev,
 	if (res) {
 		dev_err(&dev->sbd.core, "%s:%u: sync cache failed 0x%llx\n",
 			__func__, __LINE__, res);
-		__blk_end_request_all(req, -EIO);
+		__blk_end_request_all(req, BLK_STS_IOERR);
 		return 0;
 	}
 
@@ -208,7 +208,7 @@ static void ps3disk_do_request(struct ps3_storage_device *dev,
 			break;
 		default:
 			blk_dump_rq_flags(req, DEVICE_NAME " bad request");
-			__blk_end_request_all(req, -EIO);
+			__blk_end_request_all(req, BLK_STS_IOERR);
 		}
 	}
 }
@@ -231,7 +231,8 @@ static irqreturn_t ps3disk_interrupt(int irq, void *data)
 	struct ps3_storage_device *dev = data;
 	struct ps3disk_private *priv;
 	struct request *req;
-	int res, read, error;
+	int res, read;
+	blk_status_t error;
 	u64 tag, status;
 	const char *op;
 
@@ -269,7 +270,7 @@ static irqreturn_t ps3disk_interrupt(int irq, void *data)
 	if (status) {
 		dev_dbg(&dev->sbd.core, "%s:%u: %s failed 0x%llx\n", __func__,
 			__LINE__, op, status);
-		error = -EIO;
+		error = BLK_STS_IOERR;
 	} else {
 		dev_dbg(&dev->sbd.core, "%s:%u: %s completed\n", __func__,
 			__LINE__, op);
diff --git a/drivers/block/ps3vram.c b/drivers/block/ps3vram.c
index 456b4fe21559..e0e81cacd781 100644
--- a/drivers/block/ps3vram.c
+++ b/drivers/block/ps3vram.c
@@ -428,7 +428,7 @@ static void ps3vram_cache_cleanup(struct ps3_system_bus_device *dev)
 	kfree(priv->cache.tags);
 }
 
-static int ps3vram_read(struct ps3_system_bus_device *dev, loff_t from,
+static blk_status_t ps3vram_read(struct ps3_system_bus_device *dev, loff_t from,
 			size_t len, size_t *retlen, u_char *buf)
 {
 	struct ps3vram_priv *priv = ps3_system_bus_get_drvdata(dev);
@@ -438,7 +438,7 @@ static int ps3vram_read(struct ps3_system_bus_device *dev, loff_t from,
 		(unsigned int)from, len);
 
 	if (from >= priv->size)
-		return -EIO;
+		return BLK_STS_IOERR;
 
 	if (len > priv->size - from)
 		len = priv->size - from;
@@ -472,14 +472,14 @@ static int ps3vram_read(struct ps3_system_bus_device *dev, loff_t from,
 	return 0;
 }
 
-static int ps3vram_write(struct ps3_system_bus_device *dev, loff_t to,
+static blk_status_t ps3vram_write(struct ps3_system_bus_device *dev, loff_t to,
 			 size_t len, size_t *retlen, const u_char *buf)
 {
 	struct ps3vram_priv *priv = ps3_system_bus_get_drvdata(dev);
 	unsigned int cached, count;
 
 	if (to >= priv->size)
-		return -EIO;
+		return BLK_STS_IOERR;
 
 	if (len > priv->size - to)
 		len = priv->size - to;
@@ -554,7 +554,7 @@ static struct bio *ps3vram_do_bio(struct ps3_system_bus_device *dev,
 	int write = bio_data_dir(bio) == WRITE;
 	const char *op = write ? "write" : "read";
 	loff_t offset = bio->bi_iter.bi_sector << 9;
-	int error = 0;
+	blk_status_t error = 0;
 	struct bio_vec bvec;
 	struct bvec_iter iter;
 	struct bio *next;
@@ -578,7 +578,7 @@ static struct bio *ps3vram_do_bio(struct ps3_system_bus_device *dev,
 
 		if (retlen != len) {
 			dev_err(&dev->core, "Short %s\n", op);
-			error = -EIO;
+			error = BLK_STS_IOERR;
 			goto out;
 		}
 
@@ -593,7 +593,7 @@ out:
 	next = bio_list_peek(&priv->list);
 	spin_unlock_irq(&priv->lock);
 
-	bio->bi_error = error;
+	bio->bi_status = error;
 	bio_endio(bio);
 	return next;
 }
@@ -606,7 +606,7 @@ static blk_qc_t ps3vram_make_request(struct request_queue *q, struct bio *bio)
 
 	dev_dbg(&dev->core, "%s\n", __func__);
 
-	blk_queue_split(q, &bio, q->bio_split);
+	blk_queue_split(q, &bio);
 
 	spin_lock_irq(&priv->lock);
 	busy = !bio_list_empty(&priv->list);
diff --git a/drivers/block/rbd.c b/drivers/block/rbd.c
index c16f74547804..b008b6a98098 100644
--- a/drivers/block/rbd.c
+++ b/drivers/block/rbd.c
@@ -442,6 +442,8 @@ static DEFINE_SPINLOCK(rbd_client_list_lock);
 static struct kmem_cache	*rbd_img_request_cache;
 static struct kmem_cache	*rbd_obj_request_cache;
 
+static struct bio_set		*rbd_bio_clone;
+
 static int rbd_major;
 static DEFINE_IDA(rbd_dev_id_ida);
 
@@ -1363,7 +1365,7 @@ static struct bio *bio_clone_range(struct bio *bio_src,
 {
 	struct bio *bio;
 
-	bio = bio_clone(bio_src, gfpmask);
+	bio = bio_clone_fast(bio_src, gfpmask, rbd_bio_clone);
 	if (!bio)
 		return NULL;	/* ENOMEM */
 
@@ -2293,11 +2295,13 @@ static bool rbd_img_obj_end_request(struct rbd_obj_request *obj_request)
 		rbd_assert(img_request->obj_request != NULL);
 		more = obj_request->which < img_request->obj_request_count - 1;
 	} else {
+		blk_status_t status = errno_to_blk_status(result);
+
 		rbd_assert(img_request->rq != NULL);
 
-		more = blk_update_request(img_request->rq, result, xferred);
+		more = blk_update_request(img_request->rq, status, xferred);
 		if (!more)
-			__blk_mq_end_request(img_request->rq, result);
+			__blk_mq_end_request(img_request->rq, status);
 	}
 
 	return more;
@@ -4150,17 +4154,17 @@ err_rq:
 			 obj_op_name(op_type), length, offset, result);
 	ceph_put_snap_context(snapc);
 err:
-	blk_mq_end_request(rq, result);
+	blk_mq_end_request(rq, errno_to_blk_status(result));
 }
 
-static int rbd_queue_rq(struct blk_mq_hw_ctx *hctx,
+static blk_status_t rbd_queue_rq(struct blk_mq_hw_ctx *hctx,
 		const struct blk_mq_queue_data *bd)
 {
 	struct request *rq = bd->rq;
 	struct work_struct *work = blk_mq_rq_to_pdu(rq);
 
 	queue_work(rbd_wq, work);
-	return BLK_MQ_RQ_QUEUE_OK;
+	return BLK_STS_OK;
 }
 
 static void rbd_free_disk(struct rbd_device *rbd_dev)
@@ -6414,8 +6418,16 @@ static int rbd_slab_init(void)
 	if (!rbd_obj_request_cache)
 		goto out_err;
 
+	rbd_assert(!rbd_bio_clone);
+	rbd_bio_clone = bioset_create(BIO_POOL_SIZE, 0, 0);
+	if (!rbd_bio_clone)
+		goto out_err_clone;
+
 	return 0;
 
+out_err_clone:
+	kmem_cache_destroy(rbd_obj_request_cache);
+	rbd_obj_request_cache = NULL;
 out_err:
 	kmem_cache_destroy(rbd_img_request_cache);
 	rbd_img_request_cache = NULL;
@@ -6431,6 +6443,10 @@ static void rbd_slab_exit(void)
 	rbd_assert(rbd_img_request_cache);
 	kmem_cache_destroy(rbd_img_request_cache);
 	rbd_img_request_cache = NULL;
+
+	rbd_assert(rbd_bio_clone);
+	bioset_free(rbd_bio_clone);
+	rbd_bio_clone = NULL;
 }
 
 static int __init rbd_init(void)
diff --git a/drivers/block/rsxx/dev.c b/drivers/block/rsxx/dev.c
index 9c566364ac9c..7f4acebf4657 100644
--- a/drivers/block/rsxx/dev.c
+++ b/drivers/block/rsxx/dev.c
@@ -149,9 +149,9 @@ static blk_qc_t rsxx_make_request(struct request_queue *q, struct bio *bio)
 {
 	struct rsxx_cardinfo *card = q->queuedata;
 	struct rsxx_bio_meta *bio_meta;
-	int st = -EINVAL;
+	blk_status_t st = BLK_STS_IOERR;
 
-	blk_queue_split(q, &bio, q->bio_split);
+	blk_queue_split(q, &bio);
 
 	might_sleep();
 
@@ -161,15 +161,11 @@ static blk_qc_t rsxx_make_request(struct request_queue *q, struct bio *bio)
 	if (bio_end_sector(bio) > get_capacity(card->gendisk))
 		goto req_err;
 
-	if (unlikely(card->halt)) {
-		st = -EFAULT;
+	if (unlikely(card->halt))
 		goto req_err;
-	}
 
-	if (unlikely(card->dma_fault)) {
-		st = (-EFAULT);
+	if (unlikely(card->dma_fault))
 		goto req_err;
-	}
 
 	if (bio->bi_iter.bi_size == 0) {
 		dev_err(CARD_TO_DEV(card), "size zero BIO!\n");
@@ -178,7 +174,7 @@ static blk_qc_t rsxx_make_request(struct request_queue *q, struct bio *bio)
 
 	bio_meta = kmem_cache_alloc(bio_meta_pool, GFP_KERNEL);
 	if (!bio_meta) {
-		st = -ENOMEM;
+		st = BLK_STS_RESOURCE;
 		goto req_err;
 	}
 
@@ -205,7 +201,7 @@ queue_err:
 	kmem_cache_free(bio_meta_pool, bio_meta);
 req_err:
 	if (st)
-		bio->bi_error = st;
+		bio->bi_status = st;
 	bio_endio(bio);
 	return BLK_QC_T_NONE;
 }
@@ -288,7 +284,6 @@ int rsxx_setup_dev(struct rsxx_cardinfo *card)
 	}
 
 	blk_queue_make_request(card->queue, rsxx_make_request);
-	blk_queue_bounce_limit(card->queue, BLK_BOUNCE_ANY);
 	blk_queue_max_hw_sectors(card->queue, blkdev_max_hw_sectors);
 	blk_queue_physical_block_size(card->queue, RSXX_HW_BLK_SIZE);
 
diff --git a/drivers/block/rsxx/dma.c b/drivers/block/rsxx/dma.c
index 5a20385f87d0..6a1b2177951c 100644
--- a/drivers/block/rsxx/dma.c
+++ b/drivers/block/rsxx/dma.c
@@ -611,7 +611,7 @@ static void rsxx_schedule_done(struct work_struct *work)
 	mutex_unlock(&ctrl->work_lock);
 }
 
-static int rsxx_queue_discard(struct rsxx_cardinfo *card,
+static blk_status_t rsxx_queue_discard(struct rsxx_cardinfo *card,
 				  struct list_head *q,
 				  unsigned int laddr,
 				  rsxx_dma_cb cb,
@@ -621,7 +621,7 @@ static int rsxx_queue_discard(struct rsxx_cardinfo *card,
 
 	dma = kmem_cache_alloc(rsxx_dma_pool, GFP_KERNEL);
 	if (!dma)
-		return -ENOMEM;
+		return BLK_STS_RESOURCE;
 
 	dma->cmd          = HW_CMD_BLK_DISCARD;
 	dma->laddr        = laddr;
@@ -640,7 +640,7 @@ static int rsxx_queue_discard(struct rsxx_cardinfo *card,
 	return 0;
 }
 
-static int rsxx_queue_dma(struct rsxx_cardinfo *card,
+static blk_status_t rsxx_queue_dma(struct rsxx_cardinfo *card,
 			      struct list_head *q,
 			      int dir,
 			      unsigned int dma_off,
@@ -655,7 +655,7 @@ static int rsxx_queue_dma(struct rsxx_cardinfo *card,
 
 	dma = kmem_cache_alloc(rsxx_dma_pool, GFP_KERNEL);
 	if (!dma)
-		return -ENOMEM;
+		return BLK_STS_RESOURCE;
 
 	dma->cmd          = dir ? HW_CMD_BLK_WRITE : HW_CMD_BLK_READ;
 	dma->laddr        = laddr;
@@ -677,7 +677,7 @@ static int rsxx_queue_dma(struct rsxx_cardinfo *card,
 	return 0;
 }
 
-int rsxx_dma_queue_bio(struct rsxx_cardinfo *card,
+blk_status_t rsxx_dma_queue_bio(struct rsxx_cardinfo *card,
 			   struct bio *bio,
 			   atomic_t *n_dmas,
 			   rsxx_dma_cb cb,
@@ -694,7 +694,7 @@ int rsxx_dma_queue_bio(struct rsxx_cardinfo *card,
 	unsigned int dma_len;
 	int dma_cnt[RSXX_MAX_TARGETS];
 	int tgt;
-	int st;
+	blk_status_t st;
 	int i;
 
 	addr8 = bio->bi_iter.bi_sector << 9; /* sectors are 512 bytes */
@@ -769,7 +769,6 @@ bvec_err:
 	for (i = 0; i < card->n_targets; i++)
 		rsxx_cleanup_dma_queue(&card->ctrl[i], &dma_list[i],
 					FREE_DMA);
-
 	return st;
 }
 
diff --git a/drivers/block/rsxx/rsxx_priv.h b/drivers/block/rsxx/rsxx_priv.h
index 6bbc64d0f690..277f27e673a2 100644
--- a/drivers/block/rsxx/rsxx_priv.h
+++ b/drivers/block/rsxx/rsxx_priv.h
@@ -391,7 +391,7 @@ int rsxx_dma_cancel(struct rsxx_dma_ctrl *ctrl);
 void rsxx_dma_cleanup(void);
 void rsxx_dma_queue_reset(struct rsxx_cardinfo *card);
 int rsxx_dma_configure(struct rsxx_cardinfo *card);
-int rsxx_dma_queue_bio(struct rsxx_cardinfo *card,
+blk_status_t rsxx_dma_queue_bio(struct rsxx_cardinfo *card,
 			   struct bio *bio,
 			   atomic_t *n_dmas,
 			   rsxx_dma_cb cb,
diff --git a/drivers/block/skd_main.c b/drivers/block/skd_main.c
index 27833e4dae2a..d0368682bd43 100644
--- a/drivers/block/skd_main.c
+++ b/drivers/block/skd_main.c
@@ -451,8 +451,8 @@ static void skd_send_special_fitmsg(struct skd_device *skdev,
 				    struct skd_special_context *skspcl);
 static void skd_request_fn(struct request_queue *rq);
 static void skd_end_request(struct skd_device *skdev,
-			    struct skd_request_context *skreq, int error);
-static int skd_preop_sg_list(struct skd_device *skdev,
+		struct skd_request_context *skreq, blk_status_t status);
+static bool skd_preop_sg_list(struct skd_device *skdev,
 			     struct skd_request_context *skreq);
 static void skd_postop_sg_list(struct skd_device *skdev,
 			       struct skd_request_context *skreq);
@@ -491,7 +491,7 @@ static void skd_fail_all_pending(struct skd_device *skdev)
 		if (req == NULL)
 			break;
 		blk_start_request(req);
-		__blk_end_request_all(req, -EIO);
+		__blk_end_request_all(req, BLK_STS_IOERR);
 	}
 }
 
@@ -545,7 +545,6 @@ static void skd_request_fn(struct request_queue *q)
 	struct request *req = NULL;
 	struct skd_scsi_request *scsi_req;
 	unsigned long io_flags;
-	int error;
 	u32 lba;
 	u32 count;
 	int data_dir;
@@ -716,9 +715,7 @@ static void skd_request_fn(struct request_queue *q)
 		if (!req->bio)
 			goto skip_sg;
 
-		error = skd_preop_sg_list(skdev, skreq);
-
-		if (error != 0) {
+		if (!skd_preop_sg_list(skdev, skreq)) {
 			/*
 			 * Complete the native request with error.
 			 * Note that the request context is still at the
@@ -730,7 +727,7 @@ static void skd_request_fn(struct request_queue *q)
 			 */
 			pr_debug("%s:%s:%d error Out\n",
 				 skdev->name, __func__, __LINE__);
-			skd_end_request(skdev, skreq, error);
+			skd_end_request(skdev, skreq, BLK_STS_RESOURCE);
 			continue;
 		}
 
@@ -805,7 +802,7 @@ skip_sg:
 }
 
 static void skd_end_request(struct skd_device *skdev,
-			    struct skd_request_context *skreq, int error)
+		struct skd_request_context *skreq, blk_status_t error)
 {
 	if (unlikely(error)) {
 		struct request *req = skreq->req;
@@ -822,7 +819,7 @@ static void skd_end_request(struct skd_device *skdev,
 	__blk_end_request_all(skreq->req, error);
 }
 
-static int skd_preop_sg_list(struct skd_device *skdev,
+static bool skd_preop_sg_list(struct skd_device *skdev,
 			     struct skd_request_context *skreq)
 {
 	struct request *req = skreq->req;
@@ -839,7 +836,7 @@ static int skd_preop_sg_list(struct skd_device *skdev,
 
 	n_sg = blk_rq_map_sg(skdev->queue, req, sg);
 	if (n_sg <= 0)
-		return -EINVAL;
+		return false;
 
 	/*
 	 * Map scatterlist to PCI bus addresses.
@@ -847,7 +844,7 @@ static int skd_preop_sg_list(struct skd_device *skdev,
 	 */
 	n_sg = pci_map_sg(skdev->pdev, sg, n_sg, pci_dir);
 	if (n_sg <= 0)
-		return -EINVAL;
+		return false;
 
 	SKD_ASSERT(n_sg <= skdev->sgs_per_request);
 
@@ -882,7 +879,7 @@ static int skd_preop_sg_list(struct skd_device *skdev,
 		}
 	}
 
-	return 0;
+	return true;
 }
 
 static void skd_postop_sg_list(struct skd_device *skdev,
@@ -2333,7 +2330,7 @@ static void skd_resolve_req_exception(struct skd_device *skdev,
 	switch (skd_check_status(skdev, cmp_status, &skreq->err_info)) {
 	case SKD_CHECK_STATUS_REPORT_GOOD:
 	case SKD_CHECK_STATUS_REPORT_SMART_ALERT:
-		skd_end_request(skdev, skreq, 0);
+		skd_end_request(skdev, skreq, BLK_STS_OK);
 		break;
 
 	case SKD_CHECK_STATUS_BUSY_IMMINENT:
@@ -2355,7 +2352,7 @@ static void skd_resolve_req_exception(struct skd_device *skdev,
 
 	case SKD_CHECK_STATUS_REPORT_ERROR:
 	default:
-		skd_end_request(skdev, skreq, -EIO);
+		skd_end_request(skdev, skreq, BLK_STS_IOERR);
 		break;
 	}
 }
@@ -2748,7 +2745,7 @@ static int skd_isr_completion_posted(struct skd_device *skdev,
 			 * native request.
 			 */
 			if (likely(cmp_status == SAM_STAT_GOOD))
-				skd_end_request(skdev, skreq, 0);
+				skd_end_request(skdev, skreq, BLK_STS_OK);
 			else
 				skd_resolve_req_exception(skdev, skreq);
 		}
@@ -3190,7 +3187,7 @@ static void skd_recover_requests(struct skd_device *skdev, int requeue)
 			    SKD_MAX_RETRIES)
 				blk_requeue_request(skdev->queue, skreq->req);
 			else
-				skd_end_request(skdev, skreq, -EIO);
+				skd_end_request(skdev, skreq, BLK_STS_IOERR);
 
 			skreq->req = NULL;
 
@@ -4276,6 +4273,7 @@ static int skd_cons_disk(struct skd_device *skdev)
 		rc = -ENOMEM;
 		goto err_out;
 	}
+	blk_queue_bounce_limit(q, BLK_BOUNCE_HIGH);
 
 	skdev->queue = q;
 	disk->queue = q;
diff --git a/drivers/block/sunvdc.c b/drivers/block/sunvdc.c
index 3f3a3ab3d50a..6b16ead1da58 100644
--- a/drivers/block/sunvdc.c
+++ b/drivers/block/sunvdc.c
@@ -316,7 +316,7 @@ static void vdc_end_one(struct vdc_port *port, struct vio_dring_state *dr,
 
 	rqe->req = NULL;
 
-	__blk_end_request(req, (desc->status ? -EIO : 0), desc->size);
+	__blk_end_request(req, (desc->status ? BLK_STS_IOERR : 0), desc->size);
 
 	vdc_blk_queue_start(port);
 }
@@ -1023,7 +1023,7 @@ static void vdc_queue_drain(struct vdc_port *port)
 	struct request *req;
 
 	while ((req = blk_fetch_request(port->disk->queue)) != NULL)
-		__blk_end_request_all(req, -EIO);
+		__blk_end_request_all(req, BLK_STS_IOERR);
 }
 
 static void vdc_ldc_reset_timer(unsigned long _arg)
diff --git a/drivers/block/swim.c b/drivers/block/swim.c
index 3064be6cf375..84434d3ea19b 100644
--- a/drivers/block/swim.c
+++ b/drivers/block/swim.c
@@ -493,7 +493,7 @@ static inline int swim_read_sector(struct floppy_state *fs,
 	return ret;
 }
 
-static int floppy_read_sectors(struct floppy_state *fs,
+static blk_status_t floppy_read_sectors(struct floppy_state *fs,
 			       int req_sector, int sectors_nb,
 			       unsigned char *buffer)
 {
@@ -516,7 +516,7 @@ static int floppy_read_sectors(struct floppy_state *fs,
 			ret = swim_read_sector(fs, side, track, sector,
 						buffer);
 			if (try-- == 0)
-				return -EIO;
+				return BLK_STS_IOERR;
 		} while (ret != 512);
 
 		buffer += ret;
@@ -553,7 +553,7 @@ static void do_fd_request(struct request_queue *q)
 
 	req = swim_next_request(swd);
 	while (req) {
-		int err = -EIO;
+		blk_status_t err = BLK_STS_IOERR;
 
 		fs = req->rq_disk->private_data;
 		if (blk_rq_pos(req) >= fs->total_secs)
@@ -864,6 +864,8 @@ static int swim_floppy_init(struct swim_priv *swd)
 			put_disk(swd->unit[drive].disk);
 			goto exit_put_disks;
 		}
+		blk_queue_bounce_limit(swd->unit[drive].disk->queue,
+				BLK_BOUNCE_HIGH);
 		swd->unit[drive].disk->queue->queuedata = swd;
 		swd->unit[drive].swd = swd;
 	}
diff --git a/drivers/block/swim3.c b/drivers/block/swim3.c
index ba4809c9bdba..9f931f8f6b4c 100644
--- a/drivers/block/swim3.c
+++ b/drivers/block/swim3.c
@@ -257,7 +257,7 @@ static unsigned int floppy_check_events(struct gendisk *disk,
 					unsigned int clearing);
 static int floppy_revalidate(struct gendisk *disk);
 
-static bool swim3_end_request(struct floppy_state *fs, int err, unsigned int nr_bytes)
+static bool swim3_end_request(struct floppy_state *fs, blk_status_t err, unsigned int nr_bytes)
 {
 	struct request *req = fs->cur_req;
 	int rc;
@@ -334,7 +334,7 @@ static void start_request(struct floppy_state *fs)
 		if (fs->mdev->media_bay &&
 		    check_media_bay(fs->mdev->media_bay) != MB_FD) {
 			swim3_dbg("%s", "  media bay absent, dropping req\n");
-			swim3_end_request(fs, -ENODEV, 0);
+			swim3_end_request(fs, BLK_STS_IOERR, 0);
 			continue;
 		}
 
@@ -350,12 +350,12 @@ static void start_request(struct floppy_state *fs)
 		if (blk_rq_pos(req) >= fs->total_secs) {
 			swim3_dbg("  pos out of bounds (%ld, max is %ld)\n",
 				  (long)blk_rq_pos(req), (long)fs->total_secs);
-			swim3_end_request(fs, -EIO, 0);
+			swim3_end_request(fs, BLK_STS_IOERR, 0);
 			continue;
 		}
 		if (fs->ejected) {
 			swim3_dbg("%s", "  disk ejected\n");
-			swim3_end_request(fs, -EIO, 0);
+			swim3_end_request(fs, BLK_STS_IOERR, 0);
 			continue;
 		}
 
@@ -364,7 +364,7 @@ static void start_request(struct floppy_state *fs)
 				fs->write_prot = swim3_readbit(fs, WRITE_PROT);
 			if (fs->write_prot) {
 				swim3_dbg("%s", "  try to write, disk write protected\n");
-				swim3_end_request(fs, -EIO, 0);
+				swim3_end_request(fs, BLK_STS_IOERR, 0);
 				continue;
 			}
 		}
@@ -548,7 +548,7 @@ static void act(struct floppy_state *fs)
 				if (fs->retries > 5) {
 					swim3_err("Wrong cylinder in transfer, want: %d got %d\n",
 						  fs->req_cyl, fs->cur_cyl);
-					swim3_end_request(fs, -EIO, 0);
+					swim3_end_request(fs, BLK_STS_IOERR, 0);
 					fs->state = idle;
 					return;
 				}
@@ -584,7 +584,7 @@ static void scan_timeout(unsigned long data)
 	out_8(&sw->intr_enable, 0);
 	fs->cur_cyl = -1;
 	if (fs->retries > 5) {
-		swim3_end_request(fs, -EIO, 0);
+		swim3_end_request(fs, BLK_STS_IOERR, 0);
 		fs->state = idle;
 		start_request(fs);
 	} else {
@@ -608,7 +608,7 @@ static void seek_timeout(unsigned long data)
 	out_8(&sw->select, RELAX);
 	out_8(&sw->intr_enable, 0);
 	swim3_err("%s", "Seek timeout\n");
-	swim3_end_request(fs, -EIO, 0);
+	swim3_end_request(fs, BLK_STS_IOERR, 0);
 	fs->state = idle;
 	start_request(fs);
 	spin_unlock_irqrestore(&swim3_lock, flags);
@@ -637,7 +637,7 @@ static void settle_timeout(unsigned long data)
 		goto unlock;
 	}
 	swim3_err("%s", "Seek settle timeout\n");
-	swim3_end_request(fs, -EIO, 0);
+	swim3_end_request(fs, BLK_STS_IOERR, 0);
 	fs->state = idle;
 	start_request(fs);
  unlock:
@@ -666,7 +666,7 @@ static void xfer_timeout(unsigned long data)
 	swim3_err("Timeout %sing sector %ld\n",
 	       (rq_data_dir(fs->cur_req)==WRITE? "writ": "read"),
 	       (long)blk_rq_pos(fs->cur_req));
-	swim3_end_request(fs, -EIO, 0);
+	swim3_end_request(fs, BLK_STS_IOERR, 0);
 	fs->state = idle;
 	start_request(fs);
 	spin_unlock_irqrestore(&swim3_lock, flags);
@@ -703,7 +703,7 @@ static irqreturn_t swim3_interrupt(int irq, void *dev_id)
 				swim3_err("%s", "Seen sector but cyl=ff?\n");
 				fs->cur_cyl = -1;
 				if (fs->retries > 5) {
-					swim3_end_request(fs, -EIO, 0);
+					swim3_end_request(fs, BLK_STS_IOERR, 0);
 					fs->state = idle;
 					start_request(fs);
 				} else {
@@ -786,7 +786,7 @@ static irqreturn_t swim3_interrupt(int irq, void *dev_id)
 				swim3_err("Error %sing block %ld (err=%x)\n",
 				       rq_data_dir(req) == WRITE? "writ": "read",
 				       (long)blk_rq_pos(req), err);
-				swim3_end_request(fs, -EIO, 0);
+				swim3_end_request(fs, BLK_STS_IOERR, 0);
 				fs->state = idle;
 			}
 		} else {
@@ -795,7 +795,7 @@ static irqreturn_t swim3_interrupt(int irq, void *dev_id)
 				swim3_err("fd dma error: stat=%x resid=%d\n", stat, resid);
 				swim3_err("  state=%d, dir=%x, intr=%x, err=%x\n",
 					  fs->state, rq_data_dir(req), intr, err);
-				swim3_end_request(fs, -EIO, 0);
+				swim3_end_request(fs, BLK_STS_IOERR, 0);
 				fs->state = idle;
 				start_request(fs);
 				break;
@@ -1223,6 +1223,7 @@ static int swim3_attach(struct macio_dev *mdev,
 		put_disk(disk);
 		return -ENOMEM;
 	}
+	blk_queue_bounce_limit(disk->queue, BLK_BOUNCE_HIGH);
 	disk->queue->queuedata = &floppy_states[index];
 
 	if (index == 0) {
@@ -1245,7 +1246,7 @@ static int swim3_attach(struct macio_dev *mdev,
 	return 0;
 }
 
-static struct of_device_id swim3_match[] =
+static const struct of_device_id swim3_match[] =
 {
 	{
 	.name		= "swim3",
diff --git a/drivers/block/sx8.c b/drivers/block/sx8.c
index c8e072caf56f..08586dc14e85 100644
--- a/drivers/block/sx8.c
+++ b/drivers/block/sx8.c
@@ -745,7 +745,7 @@ static unsigned int carm_fill_get_fw_ver(struct carm_host *host,
 
 static inline void carm_end_request_queued(struct carm_host *host,
 					   struct carm_request *crq,
-					   int error)
+					   blk_status_t error)
 {
 	struct request *req = crq->rq;
 	int rc;
@@ -791,7 +791,7 @@ static inline void carm_round_robin(struct carm_host *host)
 }
 
 static inline void carm_end_rq(struct carm_host *host, struct carm_request *crq,
-			       int error)
+			       blk_status_t error)
 {
 	carm_end_request_queued(host, crq, error);
 	if (max_queue == 1)
@@ -869,14 +869,14 @@ queue_one_request:
 	sg = &crq->sg[0];
 	n_elem = blk_rq_map_sg(q, rq, sg);
 	if (n_elem <= 0) {
-		carm_end_rq(host, crq, -EIO);
+		carm_end_rq(host, crq, BLK_STS_IOERR);
 		return;		/* request with no s/g entries? */
 	}
 
 	/* map scatterlist to PCI bus addresses */
 	n_elem = pci_map_sg(host->pdev, sg, n_elem, pci_dir);
 	if (n_elem <= 0) {
-		carm_end_rq(host, crq, -EIO);
+		carm_end_rq(host, crq, BLK_STS_IOERR);
 		return;		/* request with no s/g entries? */
 	}
 	crq->n_elem = n_elem;
@@ -937,7 +937,7 @@ queue_one_request:
 
 static void carm_handle_array_info(struct carm_host *host,
 				   struct carm_request *crq, u8 *mem,
-				   int error)
+				   blk_status_t error)
 {
 	struct carm_port *port;
 	u8 *msg_data = mem + sizeof(struct carm_array_info);
@@ -997,7 +997,7 @@ out:
 
 static void carm_handle_scan_chan(struct carm_host *host,
 				  struct carm_request *crq, u8 *mem,
-				  int error)
+				  blk_status_t error)
 {
 	u8 *msg_data = mem + IOC_SCAN_CHAN_OFFSET;
 	unsigned int i, dev_count = 0;
@@ -1029,7 +1029,7 @@ out:
 }
 
 static void carm_handle_generic(struct carm_host *host,
-				struct carm_request *crq, int error,
+				struct carm_request *crq, blk_status_t error,
 				int cur_state, int next_state)
 {
 	DPRINTK("ENTER\n");
@@ -1045,7 +1045,7 @@ static void carm_handle_generic(struct carm_host *host,
 }
 
 static inline void carm_handle_rw(struct carm_host *host,
-				  struct carm_request *crq, int error)
+				  struct carm_request *crq, blk_status_t error)
 {
 	int pci_dir;
 
@@ -1067,7 +1067,7 @@ static inline void carm_handle_resp(struct carm_host *host,
 	u32 handle = le32_to_cpu(ret_handle_le);
 	unsigned int msg_idx;
 	struct carm_request *crq;
-	int error = (status == RMSG_OK) ? 0 : -EIO;
+	blk_status_t error = (status == RMSG_OK) ? 0 : BLK_STS_IOERR;
 	u8 *mem;
 
 	VPRINTK("ENTER, handle == 0x%x\n", handle);
@@ -1155,7 +1155,7 @@ static inline void carm_handle_resp(struct carm_host *host,
 err_out:
 	printk(KERN_WARNING DRV_NAME "(%s): BUG: unhandled message type %d/%d\n",
 	       pci_name(host->pdev), crq->msg_type, crq->msg_subtype);
-	carm_end_rq(host, crq, -EIO);
+	carm_end_rq(host, crq, BLK_STS_IOERR);
 }
 
 static inline void carm_handle_responses(struct carm_host *host)
diff --git a/drivers/block/umem.c b/drivers/block/umem.c
index c141cc3be22b..0677d2514665 100644
--- a/drivers/block/umem.c
+++ b/drivers/block/umem.c
@@ -454,7 +454,7 @@ static void process_page(unsigned long data)
 				PCI_DMA_TODEVICE : PCI_DMA_FROMDEVICE);
 		if (control & DMASCR_HARD_ERROR) {
 			/* error */
-			bio->bi_error = -EIO;
+			bio->bi_status = BLK_STS_IOERR;
 			dev_printk(KERN_WARNING, &card->dev->dev,
 				"I/O error on sector %d/%d\n",
 				le32_to_cpu(desc->local_addr)>>9,
@@ -529,7 +529,7 @@ static blk_qc_t mm_make_request(struct request_queue *q, struct bio *bio)
 		 (unsigned long long)bio->bi_iter.bi_sector,
 		 bio->bi_iter.bi_size);
 
-	blk_queue_split(q, &bio, q->bio_split);
+	blk_queue_split(q, &bio);
 
 	spin_lock_irq(&card->lock);
 	*card->biotail = bio;
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
index 553cc4c542b4..0297ad7c1452 100644
--- a/drivers/block/virtio_blk.c
+++ b/drivers/block/virtio_blk.c
@@ -64,15 +64,15 @@ struct virtblk_req {
 	struct scatterlist sg[];
 };
 
-static inline int virtblk_result(struct virtblk_req *vbr)
+static inline blk_status_t virtblk_result(struct virtblk_req *vbr)
 {
 	switch (vbr->status) {
 	case VIRTIO_BLK_S_OK:
-		return 0;
+		return BLK_STS_OK;
 	case VIRTIO_BLK_S_UNSUPP:
-		return -ENOTTY;
+		return BLK_STS_NOTSUPP;
 	default:
-		return -EIO;
+		return BLK_STS_IOERR;
 	}
 }
 
@@ -214,7 +214,7 @@ static void virtblk_done(struct virtqueue *vq)
 	spin_unlock_irqrestore(&vblk->vqs[qid].lock, flags);
 }
 
-static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx,
+static blk_status_t virtio_queue_rq(struct blk_mq_hw_ctx *hctx,
 			   const struct blk_mq_queue_data *bd)
 {
 	struct virtio_blk *vblk = hctx->queue->queuedata;
@@ -246,7 +246,7 @@ static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx,
 		break;
 	default:
 		WARN_ON_ONCE(1);
-		return BLK_MQ_RQ_QUEUE_ERROR;
+		return BLK_STS_IOERR;
 	}
 
 	vbr->out_hdr.type = cpu_to_virtio32(vblk->vdev, type);
@@ -276,8 +276,8 @@ static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx,
 		/* Out of mem doesn't actually happen, since we fall back
 		 * to direct descriptors */
 		if (err == -ENOMEM || err == -ENOSPC)
-			return BLK_MQ_RQ_QUEUE_BUSY;
-		return BLK_MQ_RQ_QUEUE_ERROR;
+			return BLK_STS_RESOURCE;
+		return BLK_STS_IOERR;
 	}
 
 	if (bd->last && virtqueue_kick_prepare(vblk->vqs[qid].vq))
@@ -286,7 +286,7 @@ static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx,
 
 	if (notify)
 		virtqueue_notify(vblk->vqs[qid].vq);
-	return BLK_MQ_RQ_QUEUE_OK;
+	return BLK_STS_OK;
 }
 
 /* return id (s/n) string for *disk to *id_str
@@ -307,7 +307,7 @@ static int virtblk_get_id(struct gendisk *disk, char *id_str)
 		goto out;
 
 	blk_execute_rq(vblk->disk->queue, vblk->disk, req, false);
-	err = virtblk_result(blk_mq_rq_to_pdu(req));
+	err = blk_status_to_errno(virtblk_result(blk_mq_rq_to_pdu(req)));
 out:
 	blk_put_request(req);
 	return err;
@@ -720,9 +720,6 @@ static int virtblk_probe(struct virtio_device *vdev)
 	/* We can handle whatever the host told us to handle. */
 	blk_queue_max_segments(q, vblk->sg_elems-2);
 
-	/* No need to bounce any requests */
-	blk_queue_bounce_limit(q, BLK_BOUNCE_ANY);
-
 	/* No real sector limit. */
 	blk_queue_max_hw_sectors(q, -1U);
 
diff --git a/drivers/block/xen-blkback/blkback.c b/drivers/block/xen-blkback/blkback.c
index 726c32e35db9..fe7cd58c43d0 100644
--- a/drivers/block/xen-blkback/blkback.c
+++ b/drivers/block/xen-blkback/blkback.c
@@ -609,8 +609,6 @@ int xen_blkif_schedule(void *arg)
 	unsigned long timeout;
 	int ret;
 
-	xen_blkif_get(blkif);
-
 	set_freezable();
 	while (!kthread_should_stop()) {
 		if (try_to_freeze())
@@ -665,7 +663,6 @@ purge_gnt_list:
 		print_stats(ring);
 
 	ring->xenblkd = NULL;
-	xen_blkif_put(blkif);
 
 	return 0;
 }
@@ -1069,20 +1066,17 @@ static void xen_blk_drain_io(struct xen_blkif_ring *ring)
 	atomic_set(&blkif->drain, 0);
 }
 
-/*
- * Completion callback on the bio's. Called as bh->b_end_io()
- */
-
-static void __end_block_io_op(struct pending_req *pending_req, int error)
+static void __end_block_io_op(struct pending_req *pending_req,
+		blk_status_t error)
 {
 	/* An error fails the entire request. */
-	if ((pending_req->operation == BLKIF_OP_FLUSH_DISKCACHE) &&
-	    (error == -EOPNOTSUPP)) {
+	if (pending_req->operation == BLKIF_OP_FLUSH_DISKCACHE &&
+	    error == BLK_STS_NOTSUPP) {
 		pr_debug("flush diskcache op failed, not supported\n");
 		xen_blkbk_flush_diskcache(XBT_NIL, pending_req->ring->blkif->be, 0);
 		pending_req->status = BLKIF_RSP_EOPNOTSUPP;
-	} else if ((pending_req->operation == BLKIF_OP_WRITE_BARRIER) &&
-		    (error == -EOPNOTSUPP)) {
+	} else if (pending_req->operation == BLKIF_OP_WRITE_BARRIER &&
+		   error == BLK_STS_NOTSUPP) {
 		pr_debug("write barrier op failed, not supported\n");
 		xen_blkbk_barrier(XBT_NIL, pending_req->ring->blkif->be, 0);
 		pending_req->status = BLKIF_RSP_EOPNOTSUPP;
@@ -1106,7 +1100,7 @@ static void __end_block_io_op(struct pending_req *pending_req, int error)
  */
 static void end_block_io_op(struct bio *bio)
 {
-	__end_block_io_op(bio->bi_private, bio->bi_error);
+	__end_block_io_op(bio->bi_private, bio->bi_status);
 	bio_put(bio);
 }
 
@@ -1423,7 +1417,7 @@ static int dispatch_rw_block_io(struct xen_blkif_ring *ring,
 	for (i = 0; i < nbio; i++)
 		bio_put(biolist[i]);
 	atomic_set(&pending_req->pendcnt, 1);
-	__end_block_io_op(pending_req, -EINVAL);
+	__end_block_io_op(pending_req, BLK_STS_RESOURCE);
 	msleep(1); /* back off a bit */
 	return -EIO;
 }
@@ -1436,34 +1430,35 @@ static int dispatch_rw_block_io(struct xen_blkif_ring *ring,
 static void make_response(struct xen_blkif_ring *ring, u64 id,
 			  unsigned short op, int st)
 {
-	struct blkif_response  resp;
+	struct blkif_response *resp;
 	unsigned long     flags;
 	union blkif_back_rings *blk_rings;
 	int notify;
 
-	resp.id        = id;
-	resp.operation = op;
-	resp.status    = st;
-
 	spin_lock_irqsave(&ring->blk_ring_lock, flags);
 	blk_rings = &ring->blk_rings;
 	/* Place on the response ring for the relevant domain. */
 	switch (ring->blkif->blk_protocol) {
 	case BLKIF_PROTOCOL_NATIVE:
-		memcpy(RING_GET_RESPONSE(&blk_rings->native, blk_rings->native.rsp_prod_pvt),
-		       &resp, sizeof(resp));
+		resp = RING_GET_RESPONSE(&blk_rings->native,
+					 blk_rings->native.rsp_prod_pvt);
 		break;
 	case BLKIF_PROTOCOL_X86_32:
-		memcpy(RING_GET_RESPONSE(&blk_rings->x86_32, blk_rings->x86_32.rsp_prod_pvt),
-		       &resp, sizeof(resp));
+		resp = RING_GET_RESPONSE(&blk_rings->x86_32,
+					 blk_rings->x86_32.rsp_prod_pvt);
 		break;
 	case BLKIF_PROTOCOL_X86_64:
-		memcpy(RING_GET_RESPONSE(&blk_rings->x86_64, blk_rings->x86_64.rsp_prod_pvt),
-		       &resp, sizeof(resp));
+		resp = RING_GET_RESPONSE(&blk_rings->x86_64,
+					 blk_rings->x86_64.rsp_prod_pvt);
 		break;
 	default:
 		BUG();
 	}
+
+	resp->id        = id;
+	resp->operation = op;
+	resp->status    = st;
+
 	blk_rings->common.rsp_prod_pvt++;
 	RING_PUSH_RESPONSES_AND_CHECK_NOTIFY(&blk_rings->common, notify);
 	spin_unlock_irqrestore(&ring->blk_ring_lock, flags);
diff --git a/drivers/block/xen-blkback/common.h b/drivers/block/xen-blkback/common.h
index dea61f6ab8cb..ecb35fe8ca8d 100644
--- a/drivers/block/xen-blkback/common.h
+++ b/drivers/block/xen-blkback/common.h
@@ -75,9 +75,8 @@ extern unsigned int xenblk_max_queues;
 struct blkif_common_request {
 	char dummy;
 };
-struct blkif_common_response {
-	char dummy;
-};
+
+/* i386 protocol version */
 
 struct blkif_x86_32_request_rw {
 	uint8_t        nr_segments;  /* number of segments                   */
@@ -129,14 +128,6 @@ struct blkif_x86_32_request {
 	} u;
 } __attribute__((__packed__));
 
-/* i386 protocol version */
-#pragma pack(push, 4)
-struct blkif_x86_32_response {
-	uint64_t        id;              /* copied from request */
-	uint8_t         operation;       /* copied from request */
-	int16_t         status;          /* BLKIF_RSP_???       */
-};
-#pragma pack(pop)
 /* x86_64 protocol version */
 
 struct blkif_x86_64_request_rw {
@@ -193,18 +184,12 @@ struct blkif_x86_64_request {
 	} u;
 } __attribute__((__packed__));
 
-struct blkif_x86_64_response {
-	uint64_t       __attribute__((__aligned__(8))) id;
-	uint8_t         operation;       /* copied from request */
-	int16_t         status;          /* BLKIF_RSP_???       */
-};
-
 DEFINE_RING_TYPES(blkif_common, struct blkif_common_request,
-		  struct blkif_common_response);
+		  struct blkif_response);
 DEFINE_RING_TYPES(blkif_x86_32, struct blkif_x86_32_request,
-		  struct blkif_x86_32_response);
+		  struct blkif_response __packed);
 DEFINE_RING_TYPES(blkif_x86_64, struct blkif_x86_64_request,
-		  struct blkif_x86_64_response);
+		  struct blkif_response);
 
 union blkif_back_rings {
 	struct blkif_back_ring        native;
@@ -281,6 +266,7 @@ struct xen_blkif_ring {
 
 	wait_queue_head_t	wq;
 	atomic_t		inflight;
+	bool			active;
 	/* One thread per blkif ring. */
 	struct task_struct	*xenblkd;
 	unsigned int		waiting_reqs;
diff --git a/drivers/block/xen-blkback/xenbus.c b/drivers/block/xen-blkback/xenbus.c
index 1f3dfaa54d87..792da683e70d 100644
--- a/drivers/block/xen-blkback/xenbus.c
+++ b/drivers/block/xen-blkback/xenbus.c
@@ -159,7 +159,7 @@ static int xen_blkif_alloc_rings(struct xen_blkif *blkif)
 		init_waitqueue_head(&ring->shutdown_wq);
 		ring->blkif = blkif;
 		ring->st_print = jiffies;
-		xen_blkif_get(blkif);
+		ring->active = true;
 	}
 
 	return 0;
@@ -249,10 +249,12 @@ static int xen_blkif_disconnect(struct xen_blkif *blkif)
 		struct xen_blkif_ring *ring = &blkif->rings[r];
 		unsigned int i = 0;
 
+		if (!ring->active)
+			continue;
+
 		if (ring->xenblkd) {
 			kthread_stop(ring->xenblkd);
 			wake_up(&ring->shutdown_wq);
-			ring->xenblkd = NULL;
 		}
 
 		/* The above kthread_stop() guarantees that at this point we
@@ -296,7 +298,7 @@ static int xen_blkif_disconnect(struct xen_blkif *blkif)
 		BUG_ON(ring->free_pages_num != 0);
 		BUG_ON(ring->persistent_gnt_c != 0);
 		WARN_ON(i != (XEN_BLKIF_REQS_PER_PAGE * blkif->nr_ring_pages));
-		xen_blkif_put(blkif);
+		ring->active = false;
 	}
 	blkif->nr_ring_pages = 0;
 	/*
@@ -312,9 +314,10 @@ static int xen_blkif_disconnect(struct xen_blkif *blkif)
 
 static void xen_blkif_free(struct xen_blkif *blkif)
 {
-
-	xen_blkif_disconnect(blkif);
+	WARN_ON(xen_blkif_disconnect(blkif));
 	xen_vbd_free(&blkif->vbd);
+	kfree(blkif->be->mode);
+	kfree(blkif->be);
 
 	/* Make sure everything is drained before shutting down */
 	kmem_cache_free(xen_blkif_cachep, blkif);
@@ -511,8 +514,6 @@ static int xen_blkbk_remove(struct xenbus_device *dev)
 		xen_blkif_put(be->blkif);
 	}
 
-	kfree(be->mode);
-	kfree(be);
 	return 0;
 }
 
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index 39459631667c..c852ed3c01d5 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -110,11 +110,6 @@ struct blk_shadow {
 	unsigned long associated_id;
 };
 
-struct split_bio {
-	struct bio *bio;
-	atomic_t pending;
-};
-
 struct blkif_req {
 	int	error;
 };
@@ -881,7 +876,7 @@ static inline bool blkif_request_flush_invalid(struct request *req,
 		 !info->feature_fua));
 }
 
-static int blkif_queue_rq(struct blk_mq_hw_ctx *hctx,
+static blk_status_t blkif_queue_rq(struct blk_mq_hw_ctx *hctx,
 			  const struct blk_mq_queue_data *qd)
 {
 	unsigned long flags;
@@ -904,16 +899,16 @@ static int blkif_queue_rq(struct blk_mq_hw_ctx *hctx,
 
 	flush_requests(rinfo);
 	spin_unlock_irqrestore(&rinfo->ring_lock, flags);
-	return BLK_MQ_RQ_QUEUE_OK;
+	return BLK_STS_OK;
 
 out_err:
 	spin_unlock_irqrestore(&rinfo->ring_lock, flags);
-	return BLK_MQ_RQ_QUEUE_ERROR;
+	return BLK_STS_IOERR;
 
 out_busy:
 	spin_unlock_irqrestore(&rinfo->ring_lock, flags);
 	blk_mq_stop_hw_queue(hctx);
-	return BLK_MQ_RQ_QUEUE_BUSY;
+	return BLK_STS_RESOURCE;
 }
 
 static void blkif_complete_rq(struct request *rq)
@@ -958,9 +953,6 @@ static void blkif_set_queue_limits(struct blkfront_info *info)
 
 	/* Make sure buffer addresses are sector-aligned. */
 	blk_queue_dma_alignment(rq, 511);
-
-	/* Make sure we don't use bounce buffers. */
-	blk_queue_bounce_limit(rq, BLK_BOUNCE_ANY);
 }
 
 static int xlvbd_init_blk_queue(struct gendisk *gd, u16 sector_size,
@@ -1601,14 +1593,18 @@ static irqreturn_t blkif_interrupt(int irq, void *dev_id)
 			continue;
 		}
 
-		blkif_req(req)->error = (bret->status == BLKIF_RSP_OKAY) ? 0 : -EIO;
+		if (bret->status == BLKIF_RSP_OKAY)
+			blkif_req(req)->error = BLK_STS_OK;
+		else
+			blkif_req(req)->error = BLK_STS_IOERR;
+
 		switch (bret->operation) {
 		case BLKIF_OP_DISCARD:
 			if (unlikely(bret->status == BLKIF_RSP_EOPNOTSUPP)) {
 				struct request_queue *rq = info->rq;
 				printk(KERN_WARNING "blkfront: %s: %s op failed\n",
 					   info->gd->disk_name, op_name(bret->operation));
-				blkif_req(req)->error = -EOPNOTSUPP;
+				blkif_req(req)->error = BLK_STS_NOTSUPP;
 				info->feature_discard = 0;
 				info->feature_secdiscard = 0;
 				queue_flag_clear(QUEUE_FLAG_DISCARD, rq);
@@ -1626,11 +1622,11 @@ static irqreturn_t blkif_interrupt(int irq, void *dev_id)
 				     rinfo->shadow[id].req.u.rw.nr_segments == 0)) {
 				printk(KERN_WARNING "blkfront: %s: empty %s op failed\n",
 				       info->gd->disk_name, op_name(bret->operation));
-				blkif_req(req)->error = -EOPNOTSUPP;
+				blkif_req(req)->error = BLK_STS_NOTSUPP;
 			}
 			if (unlikely(blkif_req(req)->error)) {
-				if (blkif_req(req)->error == -EOPNOTSUPP)
-					blkif_req(req)->error = 0;
+				if (blkif_req(req)->error == BLK_STS_NOTSUPP)
+					blkif_req(req)->error = BLK_STS_OK;
 				info->feature_fua = 0;
 				info->feature_flush = 0;
 				xlvbd_flush(info);
@@ -1996,28 +1992,13 @@ static int blkfront_probe(struct xenbus_device *dev,
 	return 0;
 }
 
-static void split_bio_end(struct bio *bio)
-{
-	struct split_bio *split_bio = bio->bi_private;
-
-	if (atomic_dec_and_test(&split_bio->pending)) {
-		split_bio->bio->bi_phys_segments = 0;
-		split_bio->bio->bi_error = bio->bi_error;
-		bio_endio(split_bio->bio);
-		kfree(split_bio);
-	}
-	bio_put(bio);
-}
-
 static int blkif_recover(struct blkfront_info *info)
 {
-	unsigned int i, r_index;
+	unsigned int r_index;
 	struct request *req, *n;
 	int rc;
-	struct bio *bio, *cloned_bio;
-	unsigned int segs, offset;
-	int pending, size;
-	struct split_bio *split_bio;
+	struct bio *bio;
+	unsigned int segs;
 
 	blkfront_gather_backend_features(info);
 	/* Reset limits changed by blk_mq_update_nr_hw_queues(). */
@@ -2056,34 +2037,6 @@ static int blkif_recover(struct blkfront_info *info)
 
 	while ((bio = bio_list_pop(&info->bio_list)) != NULL) {
 		/* Traverse the list of pending bios and re-queue them */
-		if (bio_segments(bio) > segs) {
-			/*
-			 * This bio has more segments than what we can
-			 * handle, we have to split it.
-			 */
-			pending = (bio_segments(bio) + segs - 1) / segs;
-			split_bio = kzalloc(sizeof(*split_bio), GFP_NOIO);
-			BUG_ON(split_bio == NULL);
-			atomic_set(&split_bio->pending, pending);
-			split_bio->bio = bio;
-			for (i = 0; i < pending; i++) {
-				offset = (i * segs * XEN_PAGE_SIZE) >> 9;
-				size = min((unsigned int)(segs * XEN_PAGE_SIZE) >> 9,
-					   (unsigned int)bio_sectors(bio) - offset);
-				cloned_bio = bio_clone(bio, GFP_NOIO);
-				BUG_ON(cloned_bio == NULL);
-				bio_trim(cloned_bio, offset, size);
-				cloned_bio->bi_private = split_bio;
-				cloned_bio->bi_end_io = split_bio_end;
-				submit_bio(cloned_bio);
-			}
-			/*
-			 * Now we have to wait for all those smaller bios to
-			 * end, so we can also end the "parent" bio.
-			 */
-			continue;
-		}
-		/* We don't need to split this bio */
 		submit_bio(bio);
 	}
 
@@ -2137,7 +2090,7 @@ static int blkfront_resume(struct xenbus_device *dev)
 			merge_bio.tail = shadow[j].request->biotail;
 			bio_list_merge(&info->bio_list, &merge_bio);
 			shadow[j].request->bio = NULL;
-			blk_mq_end_request(shadow[j].request, 0);
+			blk_mq_end_request(shadow[j].request, BLK_STS_OK);
 		}
 	}
 
diff --git a/drivers/block/xsysace.c b/drivers/block/xsysace.c
index 757dce2147e0..14459d66ef0c 100644
--- a/drivers/block/xsysace.c
+++ b/drivers/block/xsysace.c
@@ -471,7 +471,7 @@ static struct request *ace_get_next_request(struct request_queue *q)
 		if (!blk_rq_is_passthrough(req))
 			break;
 		blk_start_request(req);
-		__blk_end_request_all(req, -EIO);
+		__blk_end_request_all(req, BLK_STS_IOERR);
 	}
 	return req;
 }
@@ -499,11 +499,11 @@ static void ace_fsm_dostate(struct ace_device *ace)
 
 		/* Drop all in-flight and pending requests */
 		if (ace->req) {
-			__blk_end_request_all(ace->req, -EIO);
+			__blk_end_request_all(ace->req, BLK_STS_IOERR);
 			ace->req = NULL;
 		}
 		while ((req = blk_fetch_request(ace->queue)) != NULL)
-			__blk_end_request_all(req, -EIO);
+			__blk_end_request_all(req, BLK_STS_IOERR);
 
 		/* Drop back to IDLE state and notify waiters */
 		ace->fsm_state = ACE_FSM_STATE_IDLE;
@@ -728,7 +728,7 @@ static void ace_fsm_dostate(struct ace_device *ace)
 		}
 
 		/* bio finished; is there another one? */
-		if (__blk_end_request_cur(ace->req, 0)) {
+		if (__blk_end_request_cur(ace->req, BLK_STS_OK)) {
 			/* dev_dbg(ace->dev, "next block; h=%u c=%u\n",
 			 *      blk_rq_sectors(ace->req),
 			 *      blk_rq_cur_sectors(ace->req));
@@ -993,6 +993,7 @@ static int ace_setup(struct ace_device *ace)
 	if (ace->queue == NULL)
 		goto err_blk_initq;
 	blk_queue_logical_block_size(ace->queue, 512);
+	blk_queue_bounce_limit(ace->queue, BLK_BOUNCE_HIGH);
 
 	/*
 	 * Allocate and initialize GD structure
diff --git a/drivers/block/z2ram.c b/drivers/block/z2ram.c
index 968f9e52effa..41c95c9b2ab4 100644
--- a/drivers/block/z2ram.c
+++ b/drivers/block/z2ram.c
@@ -74,14 +74,14 @@ static void do_z2_request(struct request_queue *q)
 	while (req) {
 		unsigned long start = blk_rq_pos(req) << 9;
 		unsigned long len  = blk_rq_cur_bytes(req);
-		int err = 0;
+		blk_status_t err = BLK_STS_OK;
 
 		if (start + len > z2ram_size) {
 			pr_err(DEVICE_NAME ": bad access: block=%llu, "
 			       "count=%u\n",
 			       (unsigned long long)blk_rq_pos(req),
 			       blk_rq_cur_sectors(req));
-			err = -EIO;
+			err = BLK_STS_IOERR;
 			goto done;
 		}
 		while (len) {
diff --git a/drivers/block/zram/zram_drv.c b/drivers/block/zram/zram_drv.c
index debee952dcc1..558e245fab0c 100644
--- a/drivers/block/zram/zram_drv.c
+++ b/drivers/block/zram/zram_drv.c
@@ -1272,6 +1272,13 @@ static int zram_remove(struct zram *zram)
 }
 
 /* zram-control sysfs attributes */
+
+/*
+ * NOTE: hot_add attribute is not the usual read-only sysfs attribute. In a
+ * sense that reading from this file does alter the state of your system -- it
+ * creates a new un-initialized zram device and returns back this device's
+ * device_id (or an error code if it fails to create a new device).
+ */
 static ssize_t hot_add_show(struct class *class,
 			struct class_attribute *attr,
 			char *buf)
@@ -1286,6 +1293,7 @@ static ssize_t hot_add_show(struct class *class,
 		return ret;
 	return scnprintf(buf, PAGE_SIZE, "%d\n", ret);
 }
+static CLASS_ATTR_RO(hot_add);
 
 static ssize_t hot_remove_store(struct class *class,
 			struct class_attribute *attr,
@@ -1316,23 +1324,19 @@ static ssize_t hot_remove_store(struct class *class,
 	mutex_unlock(&zram_index_mutex);
 	return ret ? ret : count;
 }
+static CLASS_ATTR_WO(hot_remove);
 
-/*
- * NOTE: hot_add attribute is not the usual read-only sysfs attribute. In a
- * sense that reading from this file does alter the state of your system -- it
- * creates a new un-initialized zram device and returns back this device's
- * device_id (or an error code if it fails to create a new device).
- */
-static struct class_attribute zram_control_class_attrs[] = {
-	__ATTR(hot_add, 0400, hot_add_show, NULL),
-	__ATTR_WO(hot_remove),
-	__ATTR_NULL,
+static struct attribute *zram_control_class_attrs[] = {
+	&class_attr_hot_add.attr,
+	&class_attr_hot_remove.attr,
+	NULL,
 };
+ATTRIBUTE_GROUPS(zram_control_class);
 
 static struct class zram_control_class = {
 	.name		= "zram-control",
 	.owner		= THIS_MODULE,
-	.class_attrs	= zram_control_class_attrs,
+	.class_groups	= zram_control_class_groups,
 };
 
 static int zram_remove_cb(int id, void *ptr, void *data)
diff --git a/drivers/bluetooth/Kconfig b/drivers/bluetooth/Kconfig
index 737d93ef27c5..35952a94875e 100644
--- a/drivers/bluetooth/Kconfig
+++ b/drivers/bluetooth/Kconfig
@@ -97,6 +97,7 @@ config BT_HCIUART_NOKIA
 	depends on BT_HCIUART
 	depends on BT_HCIUART_SERDEV
 	depends on PM
+	select BT_HCIUART_H4
 	help
 	  Nokia H4+ is serial protocol for communication between Bluetooth
 	  device and host. This protocol is required for Bluetooth devices
@@ -131,7 +132,7 @@ config BT_HCIUART_ATH3K
 
 config BT_HCIUART_LL
 	bool "HCILL protocol support"
-	depends on BT_HCIUART
+	depends on BT_HCIUART_SERDEV
 	help
 	  HCILL (HCI Low Level) is a serial protocol for communication
 	  between Bluetooth device and host. This protocol is required for
diff --git a/drivers/bluetooth/bfusb.c b/drivers/bluetooth/bfusb.c
index 3bf4ec60e073..ab090a313a5f 100644
--- a/drivers/bluetooth/bfusb.c
+++ b/drivers/bluetooth/bfusb.c
@@ -335,7 +335,7 @@ static inline int bfusb_recv_block(struct bfusb_data *data, int hdr, unsigned ch
 	}
 
 	if (len > 0)
-		memcpy(skb_put(data->reassembly, len), buf, len);
+		skb_put_data(data->reassembly, buf, len);
 
 	if (hdr & 0x08) {
 		hci_recv_frame(data->hdev, data->reassembly);
@@ -505,7 +505,7 @@ static int bfusb_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 		buf[1] = 0x00;
 		buf[2] = (size == BFUSB_MAX_BLOCK_SIZE) ? 0 : size;
 
-		memcpy(skb_put(nskb, 3), buf, 3);
+		skb_put_data(nskb, buf, 3);
 		skb_copy_from_linear_data_offset(skb, sent, skb_put(nskb, size), size);
 
 		sent  += size;
@@ -516,7 +516,7 @@ static int bfusb_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	if ((nskb->len % data->bulk_pkt_size) == 0) {
 		buf[0] = 0xdd;
 		buf[1] = 0x00;
-		memcpy(skb_put(nskb, 2), buf, 2);
+		skb_put_data(nskb, buf, 2);
 	}
 
 	read_lock(&data->lock);
diff --git a/drivers/bluetooth/bluecard_cs.c b/drivers/bluetooth/bluecard_cs.c
index 007c0a45f31b..d4b0b655dde6 100644
--- a/drivers/bluetooth/bluecard_cs.c
+++ b/drivers/bluetooth/bluecard_cs.c
@@ -448,7 +448,7 @@ static void bluecard_receive(struct bluecard_info *info,
 
 		} else {
 
-			*skb_put(info->rx_skb, 1) = buf[i];
+			skb_put_u8(info->rx_skb, buf[i]);
 			info->rx_count--;
 
 			if (info->rx_count == 0) {
@@ -597,7 +597,7 @@ static int bluecard_hci_set_baud_rate(struct hci_dev *hdev, int baud)
 		break;
 	}
 
-	memcpy(skb_put(skb, sizeof(cmd)), cmd, sizeof(cmd));
+	skb_put_data(skb, cmd, sizeof(cmd));
 
 	skb_queue_tail(&(info->txq), skb);
 
diff --git a/drivers/bluetooth/bpa10x.c b/drivers/bluetooth/bpa10x.c
index a9932fe57d92..48d10cb5c9a1 100644
--- a/drivers/bluetooth/bpa10x.c
+++ b/drivers/bluetooth/bpa10x.c
@@ -297,7 +297,7 @@ static int bpa10x_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 		return -ENOMEM;
 
 	/* Prepend skb with frame type */
-	*skb_push(skb, 1) = hci_skb_pkt_type(skb);
+	*(u8 *)skb_push(skb, 1) = hci_skb_pkt_type(skb);
 
 	switch (hci_skb_pkt_type(skb)) {
 	case HCI_COMMAND_PKT:
diff --git a/drivers/bluetooth/bt3c_cs.c b/drivers/bluetooth/bt3c_cs.c
index 8165ef2fe877..32dcac017395 100644
--- a/drivers/bluetooth/bt3c_cs.c
+++ b/drivers/bluetooth/bt3c_cs.c
@@ -282,7 +282,7 @@ static void bt3c_receive(struct bt3c_info *info)
 
 			__u8 x = inb(iobase + DATA_L);
 
-			*skb_put(info->rx_skb, 1) = x;
+			skb_put_u8(info->rx_skb, x);
 			inb(iobase + DATA_H);
 			info->rx_count--;
 
diff --git a/drivers/bluetooth/btbcm.c b/drivers/bluetooth/btbcm.c
index ba3dd2eafc09..9ab6cfbb831d 100644
--- a/drivers/bluetooth/btbcm.c
+++ b/drivers/bluetooth/btbcm.c
@@ -246,6 +246,27 @@ static struct sk_buff *btbcm_read_verbose_config(struct hci_dev *hdev)
 	return skb;
 }
 
+static struct sk_buff *btbcm_read_controller_features(struct hci_dev *hdev)
+{
+	struct sk_buff *skb;
+
+	skb = __hci_cmd_sync(hdev, 0xfc6e, 0, NULL, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s: BCM: Read controller features failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		return skb;
+	}
+
+	if (skb->len != 9) {
+		BT_ERR("%s: BCM: Controller features length mismatch",
+		       hdev->name);
+		kfree_skb(skb);
+		return ERR_PTR(-EIO);
+	}
+
+	return skb;
+}
+
 static struct sk_buff *btbcm_read_usb_product(struct hci_dev *hdev)
 {
 	struct sk_buff *skb;
@@ -274,6 +295,7 @@ static const struct {
 	{ 0x410e, "BCM43341B0"	},	/* 002.001.014 */
 	{ 0x4406, "BCM4324B3"	},	/* 002.004.006 */
 	{ 0x610c, "BCM4354"	},	/* 003.001.012 */
+	{ 0x2209, "BCM43430A1"  },	/* 001.002.009 */
 	{ }
 };
 
@@ -417,6 +439,14 @@ int btbcm_setup_patchram(struct hci_dev *hdev)
 	BT_INFO("%s: BCM: chip id %u", hdev->name, skb->data[1]);
 	kfree_skb(skb);
 
+	/* Read Controller Features */
+	skb = btbcm_read_controller_features(hdev);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	BT_INFO("%s: BCM: features 0x%2.2x", hdev->name, skb->data[1]);
+	kfree_skb(skb);
+
 	/* Read Local Name */
 	skb = btbcm_read_local_name(hdev);
 	if (IS_ERR(skb))
@@ -540,6 +570,13 @@ int btbcm_setup_apple(struct hci_dev *hdev)
 		kfree_skb(skb);
 	}
 
+	/* Read Controller Features */
+	skb = btbcm_read_controller_features(hdev);
+	if (!IS_ERR(skb)) {
+		BT_INFO("%s: BCM: features 0x%2.2x", hdev->name, skb->data[1]);
+		kfree_skb(skb);
+	}
+
 	/* Read Local Name */
 	skb = btbcm_read_local_name(hdev);
 	if (!IS_ERR(skb)) {
diff --git a/drivers/bluetooth/btintel.c b/drivers/bluetooth/btintel.c
index fce154855718..d32e109bd5cb 100644
--- a/drivers/bluetooth/btintel.c
+++ b/drivers/bluetooth/btintel.c
@@ -575,3 +575,5 @@ MODULE_VERSION(VERSION);
 MODULE_LICENSE("GPL");
 MODULE_FIRMWARE("intel/ibt-11-5.sfi");
 MODULE_FIRMWARE("intel/ibt-11-5.ddc");
+MODULE_FIRMWARE("intel/ibt-12-16.sfi");
+MODULE_FIRMWARE("intel/ibt-12-16.ddc");
diff --git a/drivers/bluetooth/btmrvl_main.c b/drivers/bluetooth/btmrvl_main.c
index c38cb5b91291..b280d466f05b 100644
--- a/drivers/bluetooth/btmrvl_main.c
+++ b/drivers/bluetooth/btmrvl_main.c
@@ -189,12 +189,12 @@ static int btmrvl_send_sync_cmd(struct btmrvl_private *priv, u16 opcode,
 		return -ENOMEM;
 	}
 
-	hdr = (struct hci_command_hdr *)skb_put(skb, HCI_COMMAND_HDR_SIZE);
+	hdr = skb_put(skb, HCI_COMMAND_HDR_SIZE);
 	hdr->opcode = cpu_to_le16(opcode);
 	hdr->plen = len;
 
 	if (len)
-		memcpy(skb_put(skb, len), param, len);
+		skb_put_data(skb, param, len);
 
 	hci_skb_pkt_type(skb) = MRVL_VENDOR_PKT;
 
@@ -602,7 +602,7 @@ static int btmrvl_service_main_thread(void *data)
 	struct btmrvl_thread *thread = data;
 	struct btmrvl_private *priv = thread->priv;
 	struct btmrvl_adapter *adapter = priv->adapter;
-	wait_queue_t wait;
+	wait_queue_entry_t wait;
 	struct sk_buff *skb;
 	ulong flags;
 
diff --git a/drivers/bluetooth/btqcomsmd.c b/drivers/bluetooth/btqcomsmd.c
index ef730c173d4b..d00c4fdae924 100644
--- a/drivers/bluetooth/btqcomsmd.c
+++ b/drivers/bluetooth/btqcomsmd.c
@@ -43,7 +43,7 @@ static int btqcomsmd_recv(struct hci_dev *hdev, unsigned int type,
 	}
 
 	hci_skb_pkt_type(skb) = type;
-	memcpy(skb_put(skb, count), data, count);
+	skb_put_data(skb, data, count);
 
 	return hci_recv_frame(hdev, skb);
 }
diff --git a/drivers/bluetooth/btuart_cs.c b/drivers/bluetooth/btuart_cs.c
index 9624b29f8349..7df79bb12350 100644
--- a/drivers/bluetooth/btuart_cs.c
+++ b/drivers/bluetooth/btuart_cs.c
@@ -233,7 +233,7 @@ static void btuart_receive(struct btuart_info *info)
 
 		} else {
 
-			*skb_put(info->rx_skb, 1) = inb(iobase + UART_RX);
+			skb_put_u8(info->rx_skb, inb(iobase + UART_RX));
 			info->rx_count--;
 
 			if (info->rx_count == 0) {
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index 7fa373b428f8..fa24d693af24 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -266,6 +266,7 @@ static const struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x0cf3, 0xe301), .driver_info = BTUSB_QCA_ROME },
 	{ USB_DEVICE(0x0cf3, 0xe360), .driver_info = BTUSB_QCA_ROME },
 	{ USB_DEVICE(0x0489, 0xe092), .driver_info = BTUSB_QCA_ROME },
+	{ USB_DEVICE(0x0489, 0xe0a2), .driver_info = BTUSB_QCA_ROME },
 	{ USB_DEVICE(0x04ca, 0x3011), .driver_info = BTUSB_QCA_ROME },
 
 	/* Broadcom BCM2035 */
@@ -336,6 +337,7 @@ static const struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x8087, 0x0a2a), .driver_info = BTUSB_INTEL },
 	{ USB_DEVICE(0x8087, 0x0a2b), .driver_info = BTUSB_INTEL_NEW },
 	{ USB_DEVICE(0x8087, 0x0aa7), .driver_info = BTUSB_INTEL },
+	{ USB_DEVICE(0x8087, 0x0aaa), .driver_info = BTUSB_INTEL_NEW },
 
 	/* Other Intel Bluetooth devices */
 	{ USB_VENDOR_AND_INTERFACE_INFO(0x8087, 0xe0, 0x01, 0x01),
@@ -476,7 +478,7 @@ static int btusb_recv_intr(struct btusb_data *data, void *buffer, int count)
 		}
 
 		len = min_t(uint, hci_skb_expect(skb), count);
-		memcpy(skb_put(skb, len), buffer, len);
+		skb_put_data(skb, buffer, len);
 
 		count -= len;
 		buffer += len;
@@ -531,7 +533,7 @@ static int btusb_recv_bulk(struct btusb_data *data, void *buffer, int count)
 		}
 
 		len = min_t(uint, hci_skb_expect(skb), count);
-		memcpy(skb_put(skb, len), buffer, len);
+		skb_put_data(skb, buffer, len);
 
 		count -= len;
 		buffer += len;
@@ -588,7 +590,7 @@ static int btusb_recv_isoc(struct btusb_data *data, void *buffer, int count)
 		}
 
 		len = min_t(uint, hci_skb_expect(skb), count);
-		memcpy(skb_put(skb, len), buffer, len);
+		skb_put_data(skb, buffer, len);
 
 		count -= len;
 		buffer += len;
@@ -932,8 +934,8 @@ static void btusb_diag_complete(struct urb *urb)
 
 		skb = bt_skb_alloc(urb->actual_length, GFP_ATOMIC);
 		if (skb) {
-			memcpy(skb_put(skb, urb->actual_length),
-			       urb->transfer_buffer, urb->actual_length);
+			skb_put_data(skb, urb->transfer_buffer,
+				     urb->actual_length);
 			hci_recv_diag(hdev, skb);
 		}
 	} else if (urb->status == -ENOENT) {
@@ -1834,15 +1836,15 @@ static int inject_cmd_complete(struct hci_dev *hdev, __u16 opcode)
 	if (!skb)
 		return -ENOMEM;
 
-	hdr = (struct hci_event_hdr *)skb_put(skb, sizeof(*hdr));
+	hdr = skb_put(skb, sizeof(*hdr));
 	hdr->evt = HCI_EV_CMD_COMPLETE;
 	hdr->plen = sizeof(*evt) + 1;
 
-	evt = (struct hci_ev_cmd_complete *)skb_put(skb, sizeof(*evt));
+	evt = skb_put(skb, sizeof(*evt));
 	evt->ncmd = 0x01;
 	evt->opcode = cpu_to_le16(opcode);
 
-	*skb_put(skb, 1) = 0x00;
+	skb_put_u8(skb, 0x00);
 
 	hci_skb_pkt_type(skb) = HCI_EVENT_PKT;
 
@@ -2036,6 +2038,7 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 	switch (ver.hw_variant) {
 	case 0x0b:	/* SfP */
 	case 0x0c:	/* WsP */
+	case 0x11:	/* JfP */
 	case 0x12:	/* ThP */
 		break;
 	default:
@@ -2138,6 +2141,8 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 	 * Currently the supported hardware variants are:
 	 *   11 (0x0b) for iBT3.0 (LnP/SfP)
 	 *   12 (0x0c) for iBT3.5 (WsP)
+	 *   17 (0x11) for iBT3.5 (JfP)
+	 *   18 (0x12) for iBT3.5 (ThP)
 	 */
 	snprintf(fwname, sizeof(fwname), "intel/ibt-%u-%u.sfi",
 		 le16_to_cpu(ver.hw_variant),
@@ -2390,7 +2395,7 @@ static int marvell_config_oob_wake(struct hci_dev *hdev)
 		return -ENOMEM;
 	}
 
-	memcpy(skb_put(skb, sizeof(cmd)), cmd, sizeof(cmd));
+	skb_put_data(skb, cmd, sizeof(cmd));
 	hci_skb_pkt_type(skb) = HCI_COMMAND_PKT;
 
 	ret = btusb_send_frame(hdev, skb);
@@ -2762,8 +2767,8 @@ static struct urb *alloc_diag_urb(struct hci_dev *hdev, bool enable)
 		return ERR_PTR(-ENOMEM);
 	}
 
-	*skb_put(skb, 1) = 0xf0;
-	*skb_put(skb, 1) = enable;
+	skb_put_u8(skb, 0xf0);
+	skb_put_u8(skb, enable);
 
 	pipe = usb_sndbulkpipe(data->udev, data->diag_tx_ep->bEndpointAddress);
 
diff --git a/drivers/bluetooth/btwilink.c b/drivers/bluetooth/btwilink.c
index b6bb58c41df5..85a3978b064f 100644
--- a/drivers/bluetooth/btwilink.c
+++ b/drivers/bluetooth/btwilink.c
@@ -262,7 +262,6 @@ static int ti_st_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	pkt_type = hci_skb_pkt_type(skb);
 	len = hst->st_write(skb);
 	if (len < 0) {
-		kfree_skb(skb);
 		BT_ERR("ST write failed (%ld)", len);
 		/* Try Again, would only fail if UART has gone bad */
 		return -EAGAIN;
diff --git a/drivers/bluetooth/dtl1_cs.c b/drivers/bluetooth/dtl1_cs.c
index 6317c6f323bf..2adfe4fade76 100644
--- a/drivers/bluetooth/dtl1_cs.c
+++ b/drivers/bluetooth/dtl1_cs.c
@@ -226,7 +226,7 @@ static void dtl1_receive(struct dtl1_info *info)
 			}
 		}
 
-		*skb_put(info->rx_skb, 1) = inb(iobase + UART_RX);
+		skb_put_u8(info->rx_skb, inb(iobase + UART_RX));
 		nsh = (struct nsh *)info->rx_skb->data;
 
 		info->rx_count--;
@@ -414,7 +414,7 @@ static int dtl1_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	skb_reserve(s, NSHL);
 	skb_copy_from_linear_data(skb, skb_put(s, skb->len), skb->len);
 	if (skb->len & 0x0001)
-		*skb_put(s, 1) = 0;	/* PAD */
+		skb_put_u8(s, 0);	/* PAD */
 
 	/* Prepend skb with Nokia frame header and queue */
 	memcpy(skb_push(s, NSHL), &nsh, NSHL);
diff --git a/drivers/bluetooth/hci_bcm.c b/drivers/bluetooth/hci_bcm.c
index f87bfdfee4ff..6a662d0161b4 100644
--- a/drivers/bluetooth/hci_bcm.c
+++ b/drivers/bluetooth/hci_bcm.c
@@ -262,9 +262,9 @@ static int bcm_set_diag(struct hci_dev *hdev, bool enable)
 	if (!skb)
 		return -ENOMEM;
 
-	*skb_put(skb, 1) = BCM_LM_DIAG_PKT;
-	*skb_put(skb, 1) = 0xf0;
-	*skb_put(skb, 1) = enable;
+	skb_put_u8(skb, BCM_LM_DIAG_PKT);
+	skb_put_u8(skb, 0xf0);
+	skb_put_u8(skb, enable);
 
 	skb_queue_tail(&bcm->txq, skb);
 	hci_uart_tx_wakeup(hu);
@@ -419,8 +419,7 @@ finalize:
 	if (err)
 		return err;
 
-	err = bcm_request_irq(bcm);
-	if (!err)
+	if (!bcm_request_irq(bcm))
 		err = bcm_setup_sleep(hu);
 
 	return err;
@@ -657,6 +656,15 @@ static const struct dmi_system_id bcm_wrong_irq_dmi_table[] = {
 		},
 		.driver_data = &acpi_active_low,
 	},
+	{
+		.ident = "Asus T100CHI",
+		.matches = {
+			DMI_EXACT_MATCH(DMI_SYS_VENDOR,
+					"ASUSTeK COMPUTER INC."),
+			DMI_EXACT_MATCH(DMI_PRODUCT_NAME, "T100CHI"),
+		},
+		.driver_data = &acpi_active_low,
+	},
 	{	/* Handle ThinkPad 8 tablets with BCM2E55 chipset ACPI ID */
 		.ident = "Lenovo ThinkPad 8",
 		.matches = {
@@ -762,8 +770,7 @@ static int bcm_acpi_probe(struct bcm_device *dev)
 	if (id)
 		gpio_mapping = (const struct acpi_gpio_mapping *) id->driver_data;
 
-	ret = acpi_dev_add_driver_gpios(ACPI_COMPANION(&pdev->dev),
-					gpio_mapping);
+	ret = devm_acpi_dev_add_driver_gpios(&pdev->dev, gpio_mapping);
 	if (ret)
 		return ret;
 
@@ -834,8 +841,6 @@ static int bcm_remove(struct platform_device *pdev)
 	list_del(&dev->list);
 	mutex_unlock(&bcm_device_lock);
 
-	acpi_dev_remove_driver_gpios(ACPI_COMPANION(&pdev->dev));
-
 	dev_info(&pdev->dev, "%s device unregistered.\n", dev->name);
 
 	return 0;
diff --git a/drivers/bluetooth/hci_bcsp.c b/drivers/bluetooth/hci_bcsp.c
index 910ec968f022..d880f4e33c75 100644
--- a/drivers/bluetooth/hci_bcsp.c
+++ b/drivers/bluetooth/hci_bcsp.c
@@ -125,7 +125,7 @@ static void bcsp_slip_msgdelim(struct sk_buff *skb)
 {
 	const char pkt_delim = 0xc0;
 
-	memcpy(skb_put(skb, 1), &pkt_delim, 1);
+	skb_put_data(skb, &pkt_delim, 1);
 }
 
 static void bcsp_slip_one_byte(struct sk_buff *skb, u8 c)
@@ -135,13 +135,13 @@ static void bcsp_slip_one_byte(struct sk_buff *skb, u8 c)
 
 	switch (c) {
 	case 0xc0:
-		memcpy(skb_put(skb, 2), &esc_c0, 2);
+		skb_put_data(skb, &esc_c0, 2);
 		break;
 	case 0xdb:
-		memcpy(skb_put(skb, 2), &esc_db, 2);
+		skb_put_data(skb, &esc_db, 2);
 		break;
 	default:
-		memcpy(skb_put(skb, 1), &c, 1);
+		skb_put_data(skb, &c, 1);
 	}
 }
 
@@ -423,7 +423,7 @@ static void bcsp_handle_le_pkt(struct hci_uart *hu)
 		BT_DBG("Found a LE conf pkt");
 		if (!nskb)
 			return;
-		memcpy(skb_put(nskb, 4), conf_rsp_pkt, 4);
+		skb_put_data(nskb, conf_rsp_pkt, 4);
 		hci_skb_pkt_type(nskb) = BCSP_LE_PKT;
 
 		skb_queue_head(&bcsp->unrel, nskb);
@@ -447,7 +447,7 @@ static inline void bcsp_unslip_one_byte(struct bcsp_struct *bcsp, unsigned char
 			bcsp->rx_esc_state = BCSP_ESCSTATE_ESC;
 			break;
 		default:
-			memcpy(skb_put(bcsp->rx_skb, 1), &byte, 1);
+			skb_put_data(bcsp->rx_skb, &byte, 1);
 			if ((bcsp->rx_skb->data[0] & 0x40) != 0 &&
 			    bcsp->rx_state != BCSP_W4_CRC)
 				bcsp_crc_update(&bcsp->message_crc, byte);
@@ -458,7 +458,7 @@ static inline void bcsp_unslip_one_byte(struct bcsp_struct *bcsp, unsigned char
 	case BCSP_ESCSTATE_ESC:
 		switch (byte) {
 		case 0xdc:
-			memcpy(skb_put(bcsp->rx_skb, 1), &c0, 1);
+			skb_put_data(bcsp->rx_skb, &c0, 1);
 			if ((bcsp->rx_skb->data[0] & 0x40) != 0 &&
 			    bcsp->rx_state != BCSP_W4_CRC)
 				bcsp_crc_update(&bcsp->message_crc, 0xc0);
@@ -467,7 +467,7 @@ static inline void bcsp_unslip_one_byte(struct bcsp_struct *bcsp, unsigned char
 			break;
 
 		case 0xdd:
-			memcpy(skb_put(bcsp->rx_skb, 1), &db, 1);
+			skb_put_data(bcsp->rx_skb, &db, 1);
 			if ((bcsp->rx_skb->data[0] & 0x40) != 0 &&
 			    bcsp->rx_state != BCSP_W4_CRC)
 				bcsp_crc_update(&bcsp->message_crc, 0xdb);
diff --git a/drivers/bluetooth/hci_h4.c b/drivers/bluetooth/hci_h4.c
index 82e5a32b87a4..4e328d7d47bb 100644
--- a/drivers/bluetooth/hci_h4.c
+++ b/drivers/bluetooth/hci_h4.c
@@ -209,7 +209,7 @@ struct sk_buff *h4_recv_buf(struct hci_dev *hdev, struct sk_buff *skb,
 		}
 
 		len = min_t(uint, hci_skb_expect(skb) - skb->len, count);
-		memcpy(skb_put(skb, len), buffer, len);
+		skb_put_data(skb, buffer, len);
 
 		count -= len;
 		buffer += len;
diff --git a/drivers/bluetooth/hci_h5.c b/drivers/bluetooth/hci_h5.c
index 90d0456b6744..c0e4e26dc30d 100644
--- a/drivers/bluetooth/hci_h5.c
+++ b/drivers/bluetooth/hci_h5.c
@@ -109,7 +109,7 @@ static void h5_link_control(struct hci_uart *hu, const void *data, size_t len)
 
 	hci_skb_pkt_type(nskb) = HCI_3WIRE_LINK_PKT;
 
-	memcpy(skb_put(nskb, len), data, len);
+	skb_put_data(nskb, data, len);
 
 	skb_queue_tail(&h5->unrel, nskb);
 }
@@ -487,7 +487,7 @@ static void h5_unslip_one_byte(struct h5 *h5, unsigned char c)
 		}
 	}
 
-	memcpy(skb_put(h5->rx_skb, 1), byte, 1);
+	skb_put_data(h5->rx_skb, byte, 1);
 	h5->rx_pending--;
 
 	BT_DBG("unsliped 0x%02hhx, rx_pending %zu", *byte, h5->rx_pending);
@@ -579,7 +579,7 @@ static void h5_slip_delim(struct sk_buff *skb)
 {
 	const char delim = SLIP_DELIMITER;
 
-	memcpy(skb_put(skb, 1), &delim, 1);
+	skb_put_data(skb, &delim, 1);
 }
 
 static void h5_slip_one_byte(struct sk_buff *skb, u8 c)
@@ -589,13 +589,13 @@ static void h5_slip_one_byte(struct sk_buff *skb, u8 c)
 
 	switch (c) {
 	case SLIP_DELIMITER:
-		memcpy(skb_put(skb, 2), &esc_delim, 2);
+		skb_put_data(skb, &esc_delim, 2);
 		break;
 	case SLIP_ESC:
-		memcpy(skb_put(skb, 2), &esc_esc, 2);
+		skb_put_data(skb, &esc_esc, 2);
 		break;
 	default:
-		memcpy(skb_put(skb, 1), &c, 1);
+		skb_put_data(skb, &c, 1);
 	}
 }
 
diff --git a/drivers/bluetooth/hci_intel.c b/drivers/bluetooth/hci_intel.c
index fa5099986f1b..aad07e40ea4f 100644
--- a/drivers/bluetooth/hci_intel.c
+++ b/drivers/bluetooth/hci_intel.c
@@ -185,7 +185,7 @@ static int intel_lpm_suspend(struct hci_uart *hu)
 		return -ENOMEM;
 	}
 
-	memcpy(skb_put(skb, sizeof(suspend)), suspend, sizeof(suspend));
+	skb_put_data(skb, suspend, sizeof(suspend));
 	hci_skb_pkt_type(skb) = HCI_LPM_PKT;
 
 	set_bit(STATE_LPM_TRANSACTION, &intel->flags);
@@ -270,8 +270,7 @@ static int intel_lpm_host_wake(struct hci_uart *hu)
 		return -ENOMEM;
 	}
 
-	memcpy(skb_put(skb, sizeof(lpm_resume_ack)), lpm_resume_ack,
-	       sizeof(lpm_resume_ack));
+	skb_put_data(skb, lpm_resume_ack, sizeof(lpm_resume_ack));
 	hci_skb_pkt_type(skb) = HCI_LPM_PKT;
 
 	/* LPM flow is a priority, enqueue packet at list head */
@@ -463,15 +462,15 @@ static int inject_cmd_complete(struct hci_dev *hdev, __u16 opcode)
 	if (!skb)
 		return -ENOMEM;
 
-	hdr = (struct hci_event_hdr *)skb_put(skb, sizeof(*hdr));
+	hdr = skb_put(skb, sizeof(*hdr));
 	hdr->evt = HCI_EV_CMD_COMPLETE;
 	hdr->plen = sizeof(*evt) + 1;
 
-	evt = (struct hci_ev_cmd_complete *)skb_put(skb, sizeof(*evt));
+	evt = skb_put(skb, sizeof(*evt));
 	evt->ncmd = 0x01;
 	evt->opcode = cpu_to_le16(opcode);
 
-	*skb_put(skb, 1) = 0x00;
+	skb_put_u8(skb, 0x00);
 
 	hci_skb_pkt_type(skb) = HCI_EVENT_PKT;
 
@@ -522,7 +521,7 @@ static int intel_set_baudrate(struct hci_uart *hu, unsigned int speed)
 		return -ENOMEM;
 	}
 
-	memcpy(skb_put(skb, sizeof(speed_cmd)), speed_cmd, sizeof(speed_cmd));
+	skb_put_data(skb, speed_cmd, sizeof(speed_cmd));
 	hci_skb_pkt_type(skb) = HCI_COMMAND_PKT;
 
 	hci_uart_set_flow_control(hu, true);
@@ -1205,9 +1204,19 @@ static const struct dev_pm_ops intel_pm_ops = {
 	SET_RUNTIME_PM_OPS(intel_suspend_device, intel_resume_device, NULL)
 };
 
+static const struct acpi_gpio_params reset_gpios = { 0, 0, false };
+static const struct acpi_gpio_params host_wake_gpios = { 1, 0, false };
+
+static const struct acpi_gpio_mapping acpi_hci_intel_gpios[] = {
+	{ "reset-gpios", &reset_gpios, 1 },
+	{ "host-wake-gpios", &host_wake_gpios, 1 },
+	{ },
+};
+
 static int intel_probe(struct platform_device *pdev)
 {
 	struct intel_device *idev;
+	int ret;
 
 	idev = devm_kzalloc(&pdev->dev, sizeof(*idev), GFP_KERNEL);
 	if (!idev)
@@ -1217,6 +1226,10 @@ static int intel_probe(struct platform_device *pdev)
 
 	idev->pdev = pdev;
 
+	ret = devm_acpi_dev_add_driver_gpios(&pdev->dev, acpi_hci_intel_gpios);
+	if (ret)
+		dev_dbg(&pdev->dev, "Unable to add GPIO mapping table\n");
+
 	idev->reset = devm_gpiod_get(&pdev->dev, "reset", GPIOD_OUT_LOW);
 	if (IS_ERR(idev->reset)) {
 		dev_err(&pdev->dev, "Unable to retrieve gpio\n");
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index 2edd30556956..8397b716fa65 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -114,8 +114,12 @@ static inline struct sk_buff *hci_uart_dequeue(struct hci_uart *hu)
 	struct sk_buff *skb = hu->tx_skb;
 
 	if (!skb) {
+		read_lock(&hu->proto_lock);
+
 		if (test_bit(HCI_UART_PROTO_READY, &hu->flags))
 			skb = hu->proto->dequeue(hu);
+
+		read_unlock(&hu->proto_lock);
 	} else {
 		hu->tx_skb = NULL;
 	}
@@ -125,18 +129,23 @@ static inline struct sk_buff *hci_uart_dequeue(struct hci_uart *hu)
 
 int hci_uart_tx_wakeup(struct hci_uart *hu)
 {
+	read_lock(&hu->proto_lock);
+
 	if (!test_bit(HCI_UART_PROTO_READY, &hu->flags))
-		return 0;
+		goto no_schedule;
 
 	if (test_and_set_bit(HCI_UART_SENDING, &hu->tx_state)) {
 		set_bit(HCI_UART_TX_WAKEUP, &hu->tx_state);
-		return 0;
+		goto no_schedule;
 	}
 
 	BT_DBG("");
 
 	schedule_work(&hu->write_work);
 
+no_schedule:
+	read_unlock(&hu->proto_lock);
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(hci_uart_tx_wakeup);
@@ -237,9 +246,13 @@ static int hci_uart_flush(struct hci_dev *hdev)
 	tty_ldisc_flush(tty);
 	tty_driver_flush_buffer(tty);
 
+	read_lock(&hu->proto_lock);
+
 	if (test_bit(HCI_UART_PROTO_READY, &hu->flags))
 		hu->proto->flush(hu);
 
+	read_unlock(&hu->proto_lock);
+
 	return 0;
 }
 
@@ -261,10 +274,15 @@ static int hci_uart_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	BT_DBG("%s: type %d len %d", hdev->name, hci_skb_pkt_type(skb),
 	       skb->len);
 
-	if (!test_bit(HCI_UART_PROTO_READY, &hu->flags))
+	read_lock(&hu->proto_lock);
+
+	if (!test_bit(HCI_UART_PROTO_READY, &hu->flags)) {
+		read_unlock(&hu->proto_lock);
 		return -EUNATCH;
+	}
 
 	hu->proto->enqueue(hu, skb);
+	read_unlock(&hu->proto_lock);
 
 	hci_uart_tx_wakeup(hu);
 
@@ -460,6 +478,8 @@ static int hci_uart_tty_open(struct tty_struct *tty)
 	INIT_WORK(&hu->init_ready, hci_uart_init_work);
 	INIT_WORK(&hu->write_work, hci_uart_write_work);
 
+	rwlock_init(&hu->proto_lock);
+
 	/* Flush any pending characters in the driver */
 	tty_driver_flush_buffer(tty);
 
@@ -475,6 +495,7 @@ static void hci_uart_tty_close(struct tty_struct *tty)
 {
 	struct hci_uart *hu = tty->disc_data;
 	struct hci_dev *hdev;
+	unsigned long flags;
 
 	BT_DBG("tty %p", tty);
 
@@ -490,7 +511,11 @@ static void hci_uart_tty_close(struct tty_struct *tty)
 
 	cancel_work_sync(&hu->write_work);
 
-	if (test_and_clear_bit(HCI_UART_PROTO_READY, &hu->flags)) {
+	if (test_bit(HCI_UART_PROTO_READY, &hu->flags)) {
+		write_lock_irqsave(&hu->proto_lock, flags);
+		clear_bit(HCI_UART_PROTO_READY, &hu->flags);
+		write_unlock_irqrestore(&hu->proto_lock, flags);
+
 		if (hdev) {
 			if (test_bit(HCI_UART_REGISTERED, &hu->flags))
 				hci_unregister_dev(hdev);
@@ -549,13 +574,18 @@ static void hci_uart_tty_receive(struct tty_struct *tty, const u8 *data,
 	if (!hu || tty != hu->tty)
 		return;
 
-	if (!test_bit(HCI_UART_PROTO_READY, &hu->flags))
+	read_lock(&hu->proto_lock);
+
+	if (!test_bit(HCI_UART_PROTO_READY, &hu->flags)) {
+		read_unlock(&hu->proto_lock);
 		return;
+	}
 
 	/* It does not need a lock here as it is already protected by a mutex in
 	 * tty caller
 	 */
 	hu->proto->recv(hu, data, count);
+	read_unlock(&hu->proto_lock);
 
 	if (hu->hdev)
 		hu->hdev->stat.byte_rx += count;
diff --git a/drivers/bluetooth/hci_ll.c b/drivers/bluetooth/hci_ll.c
index adc444f309a3..c982943f0747 100644
--- a/drivers/bluetooth/hci_ll.c
+++ b/drivers/bluetooth/hci_ll.c
@@ -48,6 +48,7 @@
 #include <linux/serdev.h>
 #include <linux/skbuff.h>
 #include <linux/ti_wilink_st.h>
+#include <linux/clk.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
@@ -84,6 +85,7 @@ struct ll_device {
 	struct hci_uart hu;
 	struct serdev_device *serdev;
 	struct gpio_desc *enable_gpio;
+	struct clk *ext_clk;
 };
 
 struct ll_struct {
@@ -118,7 +120,7 @@ static int send_hcill_cmd(u8 cmd, struct hci_uart *hu)
 	}
 
 	/* prepare packet */
-	hcill_packet = (struct hcill_cmd *) skb_put(skb, 1);
+	hcill_packet = skb_put(skb, 1);
 	hcill_packet->cmd = cmd;
 
 	/* send packet */
@@ -146,8 +148,12 @@ static int ll_open(struct hci_uart *hu)
 
 	hu->priv = ll;
 
-	if (hu->serdev)
+	if (hu->serdev) {
+		struct ll_device *lldev = serdev_device_get_drvdata(hu->serdev);
 		serdev_device_open(hu->serdev);
+		if (!IS_ERR(lldev->ext_clk))
+			clk_prepare_enable(lldev->ext_clk);
+	}
 
 	return 0;
 }
@@ -181,6 +187,8 @@ static int ll_close(struct hci_uart *hu)
 		struct ll_device *lldev = serdev_device_get_drvdata(hu->serdev);
 		gpiod_set_value_cansleep(lldev->enable_gpio, 0);
 
+		clk_disable_unprepare(lldev->ext_clk);
+
 		serdev_device_close(hu->serdev);
 	}
 
@@ -405,7 +413,7 @@ static int ll_recv(struct hci_uart *hu, const void *data, int count)
 	while (count) {
 		if (ll->rx_count) {
 			len = min_t(unsigned int, ll->rx_count, count);
-			memcpy(skb_put(ll->rx_skb, len), ptr, len);
+			skb_put_data(ll->rx_skb, ptr, len);
 			ll->rx_count -= len; count -= len; ptr += len;
 
 			if (ll->rx_count)
@@ -624,6 +632,7 @@ static int download_firmware(struct ll_device *lldev)
 			skb = __hci_cmd_sync(lldev->hu.hdev, cmd->opcode, cmd->plen, &cmd->speed, HCI_INIT_TIMEOUT);
 			if (IS_ERR(skb)) {
 				bt_dev_err(lldev->hu.hdev, "send command failed\n");
+				err = PTR_ERR(skb);
 				goto out_rel_fw;
 			}
 			kfree_skb(skb);
@@ -720,6 +729,10 @@ static int hci_ti_probe(struct serdev_device *serdev)
 	if (IS_ERR(lldev->enable_gpio))
 		return PTR_ERR(lldev->enable_gpio);
 
+	lldev->ext_clk = devm_clk_get(&serdev->dev, "ext_clock");
+	if (IS_ERR(lldev->ext_clk) && PTR_ERR(lldev->ext_clk) != -ENOENT)
+		return PTR_ERR(lldev->ext_clk);
+
 	of_property_read_u32(serdev->dev.of_node, "max-speed", &max_speed);
 	hci_uart_set_speeds(hu, 115200, max_speed);
 
@@ -740,6 +753,14 @@ static void hci_ti_remove(struct serdev_device *serdev)
 }
 
 static const struct of_device_id hci_ti_of_match[] = {
+	{ .compatible = "ti,wl1271-st" },
+	{ .compatible = "ti,wl1273-st" },
+	{ .compatible = "ti,wl1281-st" },
+	{ .compatible = "ti,wl1283-st" },
+	{ .compatible = "ti,wl1285-st" },
+	{ .compatible = "ti,wl1801-st" },
+	{ .compatible = "ti,wl1805-st" },
+	{ .compatible = "ti,wl1807-st" },
 	{ .compatible = "ti,wl1831-st" },
 	{ .compatible = "ti,wl1835-st" },
 	{ .compatible = "ti,wl1837-st" },
diff --git a/drivers/bluetooth/hci_mrvl.c b/drivers/bluetooth/hci_mrvl.c
index bbc4b39b1dbf..ffb00669346f 100644
--- a/drivers/bluetooth/hci_mrvl.c
+++ b/drivers/bluetooth/hci_mrvl.c
@@ -328,7 +328,7 @@ static int mrvl_load_firmware(struct hci_dev *hdev, const char *name)
 		}
 		bt_cb(skb)->pkt_type = MRVL_RAW_DATA;
 
-		memcpy(skb_put(skb, mrvl->tx_len), fw_ptr, mrvl->tx_len);
+		skb_put_data(skb, fw_ptr, mrvl->tx_len);
 		fw_ptr += mrvl->tx_len;
 
 		set_bit(STATE_FW_REQ_PENDING, &mrvl->flags);
diff --git a/drivers/bluetooth/hci_nokia.c b/drivers/bluetooth/hci_nokia.c
index a7d687d8d456..181a15b549e5 100644
--- a/drivers/bluetooth/hci_nokia.c
+++ b/drivers/bluetooth/hci_nokia.c
@@ -246,9 +246,9 @@ static int nokia_send_alive_packet(struct hci_uart *hu)
 	hci_skb_pkt_type(skb) = HCI_NOKIA_ALIVE_PKT;
 	memset(skb->data, 0x00, len);
 
-	hdr = (struct hci_nokia_alive_hdr *)skb_put(skb, sizeof(*hdr));
+	hdr = skb_put(skb, sizeof(*hdr));
 	hdr->dlen = sizeof(*pkt);
-	pkt = (struct hci_nokia_alive_pkt *)skb_put(skb, sizeof(*pkt));
+	pkt = skb_put(skb, sizeof(*pkt));
 	pkt->mid = NOKIA_ALIVE_REQ;
 
 	nokia_enqueue(hu, skb);
@@ -285,10 +285,10 @@ static int nokia_send_negotiation(struct hci_uart *hu)
 
 	hci_skb_pkt_type(skb) = HCI_NOKIA_NEG_PKT;
 
-	neg_hdr = (struct hci_nokia_neg_hdr *)skb_put(skb, sizeof(*neg_hdr));
+	neg_hdr = skb_put(skb, sizeof(*neg_hdr));
 	neg_hdr->dlen = sizeof(*neg_cmd);
 
-	neg_cmd = (struct hci_nokia_neg_cmd *)skb_put(skb, sizeof(*neg_cmd));
+	neg_cmd = skb_put(skb, sizeof(*neg_cmd));
 	neg_cmd->ack = NOKIA_NEG_REQ;
 	neg_cmd->baud = cpu_to_le16(baud);
 	neg_cmd->unused1 = 0x0000;
@@ -532,7 +532,7 @@ static int nokia_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 		err = skb_pad(skb, 1);
 		if (err)
 			return err;
-		*skb_put(skb, 1) = 0x00;
+		skb_put_u8(skb, 0x00);
 	}
 
 	skb_queue_tail(&btdev->txq, skb);
@@ -557,7 +557,7 @@ static int nokia_recv_negotiation_packet(struct hci_dev *hdev,
 		goto finish_neg;
 	}
 
-	evt = (struct hci_nokia_neg_evt *)skb_pull(skb, sizeof(*hdr));
+	evt = skb_pull(skb, sizeof(*hdr));
 
 	if (evt->ack != NOKIA_NEG_ACK) {
 		dev_err(dev, "Negotiation received: wrong reply");
@@ -595,7 +595,7 @@ static int nokia_recv_alive_packet(struct hci_dev *hdev, struct sk_buff *skb)
 		goto finish_alive;
 	}
 
-	pkt = (struct hci_nokia_alive_pkt *)skb_pull(skb, sizeof(*hdr));
+	pkt = skb_pull(skb, sizeof(*hdr));
 
 	if (pkt->mid != NOKIA_ALIVE_RESP) {
 		dev_err(dev, "Alive received: invalid response: 0x%02x!",
diff --git a/drivers/bluetooth/hci_qca.c b/drivers/bluetooth/hci_qca.c
index f242dfd0c2e2..392f412b4575 100644
--- a/drivers/bluetooth/hci_qca.c
+++ b/drivers/bluetooth/hci_qca.c
@@ -215,7 +215,7 @@ static int send_hci_ibs_cmd(u8 cmd, struct hci_uart *hu)
 	}
 
 	/* Assign HCI_IBS type */
-	*skb_put(skb, 1) = cmd;
+	skb_put_u8(skb, cmd);
 
 	skb_queue_tail(&qca->txq, skb);
 
@@ -869,7 +869,7 @@ static int qca_set_baudrate(struct hci_dev *hdev, uint8_t baudrate)
 	}
 
 	/* Assign commands to change baudrate and packet type. */
-	memcpy(skb_put(skb, sizeof(cmd)), cmd, sizeof(cmd));
+	skb_put_data(skb, cmd, sizeof(cmd));
 	hci_skb_pkt_type(skb) = HCI_COMMAND_PKT;
 
 	skb_queue_tail(&qca->txq, skb);
diff --git a/drivers/bluetooth/hci_serdev.c b/drivers/bluetooth/hci_serdev.c
index 7de0edc0ff8c..aea930101dd2 100644
--- a/drivers/bluetooth/hci_serdev.c
+++ b/drivers/bluetooth/hci_serdev.c
@@ -31,7 +31,7 @@
 
 #include "hci_uart.h"
 
-struct serdev_device_ops hci_serdev_client_ops;
+static struct serdev_device_ops hci_serdev_client_ops;
 
 static inline void hci_uart_tx_complete(struct hci_uart *hu, int pkt_type)
 {
@@ -268,7 +268,7 @@ static int hci_uart_receive_buf(struct serdev_device *serdev, const u8 *data,
 	return count;
 }
 
-struct serdev_device_ops hci_serdev_client_ops = {
+static struct serdev_device_ops hci_serdev_client_ops = {
 	.receive_buf = hci_uart_receive_buf,
 	.write_wakeup = hci_uart_write_wakeup,
 };
diff --git a/drivers/bluetooth/hci_uart.h b/drivers/bluetooth/hci_uart.h
index 2b05e557fad0..c6e9e1cf63f8 100644
--- a/drivers/bluetooth/hci_uart.h
+++ b/drivers/bluetooth/hci_uart.h
@@ -87,6 +87,7 @@ struct hci_uart {
 	struct work_struct	write_work;
 
 	const struct hci_uart_proto *proto;
+	rwlock_t		proto_lock;	/* Stop work for proto close */
 	void			*priv;
 
 	struct sk_buff		*tx_skb;
diff --git a/drivers/bluetooth/hci_vhci.c b/drivers/bluetooth/hci_vhci.c
index 233e850fdac7..e6f6dbc04131 100644
--- a/drivers/bluetooth/hci_vhci.c
+++ b/drivers/bluetooth/hci_vhci.c
@@ -146,8 +146,8 @@ static int __vhci_create_device(struct vhci_data *data, __u8 opcode)
 
 	hci_skb_pkt_type(skb) = HCI_VENDOR_PKT;
 
-	*skb_put(skb, 1) = 0xff;
-	*skb_put(skb, 1) = opcode;
+	skb_put_u8(skb, 0xff);
+	skb_put_u8(skb, opcode);
 	put_unaligned_le16(hdev->id, skb_put(skb, 2));
 	skb_queue_tail(&data->readq, skb);
 
diff --git a/drivers/bus/Kconfig b/drivers/bus/Kconfig
index 0a52da439abf..d2a5f1184022 100644
--- a/drivers/bus/Kconfig
+++ b/drivers/bus/Kconfig
@@ -57,7 +57,7 @@ config ARM_CCN
 
 config BRCMSTB_GISB_ARB
 	bool "Broadcom STB GISB bus arbiter"
-	depends on ARM || MIPS
+	depends on ARM || ARM64 || MIPS
 	default ARCH_BRCMSTB || BMIPS_GENERIC
 	help
 	  Driver for the Broadcom Set Top Box System-on-a-chip internal bus
diff --git a/drivers/bus/arm-ccn.c b/drivers/bus/arm-ccn.c
index 4d6a2b7e4d3f..e8c6946fed9d 100644
--- a/drivers/bus/arm-ccn.c
+++ b/drivers/bus/arm-ccn.c
@@ -1520,10 +1520,10 @@ static int arm_ccn_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
-	ccn->node = devm_kzalloc(ccn->dev, sizeof(*ccn->node) * ccn->num_nodes,
-		GFP_KERNEL);
-	ccn->xp = devm_kzalloc(ccn->dev, sizeof(*ccn->node) * ccn->num_xps,
-		GFP_KERNEL);
+	ccn->node = devm_kcalloc(ccn->dev, ccn->num_nodes, sizeof(*ccn->node),
+				 GFP_KERNEL);
+	ccn->xp = devm_kcalloc(ccn->dev, ccn->num_xps, sizeof(*ccn->node),
+			       GFP_KERNEL);
 	if (!ccn->node || !ccn->xp)
 		return -ENOMEM;
 
@@ -1544,9 +1544,11 @@ static int arm_ccn_remove(struct platform_device *pdev)
 }
 
 static const struct of_device_id arm_ccn_match[] = {
+	{ .compatible = "arm,ccn-502", },
 	{ .compatible = "arm,ccn-504", },
 	{},
 };
+MODULE_DEVICE_TABLE(of, arm_ccn_match);
 
 static struct platform_driver arm_ccn_driver = {
 	.driver = {
diff --git a/drivers/bus/brcmstb_gisb.c b/drivers/bus/brcmstb_gisb.c
index 72fe0a5a8bf3..68ac3e93b600 100644
--- a/drivers/bus/brcmstb_gisb.c
+++ b/drivers/bus/brcmstb_gisb.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2014 Broadcom Corporation
+ * Copyright (C) 2014-2017 Broadcom
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -24,11 +24,9 @@
 #include <linux/of.h>
 #include <linux/bitops.h>
 #include <linux/pm.h>
-
-#ifdef CONFIG_ARM
-#include <asm/bug.h>
-#include <asm/signal.h>
-#endif
+#include <linux/kernel.h>
+#include <linux/kdebug.h>
+#include <linux/notifier.h>
 
 #ifdef CONFIG_MIPS
 #include <asm/traps.h>
@@ -37,8 +35,6 @@
 #define  ARB_ERR_CAP_CLEAR		(1 << 0)
 #define  ARB_ERR_CAP_STATUS_TIMEOUT	(1 << 12)
 #define  ARB_ERR_CAP_STATUS_TEA		(1 << 11)
-#define  ARB_ERR_CAP_STATUS_BS_SHIFT	(1 << 2)
-#define  ARB_ERR_CAP_STATUS_BS_MASK	0x3c
 #define  ARB_ERR_CAP_STATUS_WRITE	(1 << 1)
 #define  ARB_ERR_CAP_STATUS_VALID	(1 << 0)
 
@@ -47,7 +43,6 @@ enum {
 	ARB_ERR_CAP_CLR,
 	ARB_ERR_CAP_HI_ADDR,
 	ARB_ERR_CAP_ADDR,
-	ARB_ERR_CAP_DATA,
 	ARB_ERR_CAP_STATUS,
 	ARB_ERR_CAP_MASTER,
 };
@@ -57,17 +52,24 @@ static const int gisb_offsets_bcm7038[] = {
 	[ARB_ERR_CAP_CLR]	= 0x0c4,
 	[ARB_ERR_CAP_HI_ADDR]	= -1,
 	[ARB_ERR_CAP_ADDR]	= 0x0c8,
-	[ARB_ERR_CAP_DATA]	= 0x0cc,
 	[ARB_ERR_CAP_STATUS]	= 0x0d0,
 	[ARB_ERR_CAP_MASTER]	= -1,
 };
 
+static const int gisb_offsets_bcm7278[] = {
+	[ARB_TIMER]		= 0x008,
+	[ARB_ERR_CAP_CLR]	= 0x7f8,
+	[ARB_ERR_CAP_HI_ADDR]	= -1,
+	[ARB_ERR_CAP_ADDR]	= 0x7e0,
+	[ARB_ERR_CAP_STATUS]	= 0x7f0,
+	[ARB_ERR_CAP_MASTER]	= 0x7f4,
+};
+
 static const int gisb_offsets_bcm7400[] = {
 	[ARB_TIMER]		= 0x00c,
 	[ARB_ERR_CAP_CLR]	= 0x0c8,
 	[ARB_ERR_CAP_HI_ADDR]	= -1,
 	[ARB_ERR_CAP_ADDR]	= 0x0cc,
-	[ARB_ERR_CAP_DATA]	= 0x0d0,
 	[ARB_ERR_CAP_STATUS]	= 0x0d4,
 	[ARB_ERR_CAP_MASTER]	= 0x0d8,
 };
@@ -77,7 +79,6 @@ static const int gisb_offsets_bcm7435[] = {
 	[ARB_ERR_CAP_CLR]	= 0x168,
 	[ARB_ERR_CAP_HI_ADDR]	= -1,
 	[ARB_ERR_CAP_ADDR]	= 0x16c,
-	[ARB_ERR_CAP_DATA]	= 0x170,
 	[ARB_ERR_CAP_STATUS]	= 0x174,
 	[ARB_ERR_CAP_MASTER]	= 0x178,
 };
@@ -87,7 +88,6 @@ static const int gisb_offsets_bcm7445[] = {
 	[ARB_ERR_CAP_CLR]	= 0x7e4,
 	[ARB_ERR_CAP_HI_ADDR]	= 0x7e8,
 	[ARB_ERR_CAP_ADDR]	= 0x7ec,
-	[ARB_ERR_CAP_DATA]	= 0x7f0,
 	[ARB_ERR_CAP_STATUS]	= 0x7f4,
 	[ARB_ERR_CAP_MASTER]	= 0x7f8,
 };
@@ -109,9 +109,13 @@ static u32 gisb_read(struct brcmstb_gisb_arb_device *gdev, int reg)
 {
 	int offset = gdev->gisb_offsets[reg];
 
-	/* return 1 if the hardware doesn't have ARB_ERR_CAP_MASTER */
-	if (offset == -1)
-		return 1;
+	if (offset < 0) {
+		/* return 1 if the hardware doesn't have ARB_ERR_CAP_MASTER */
+		if (reg == ARB_ERR_CAP_MASTER)
+			return 1;
+		else
+			return 0;
+	}
 
 	if (gdev->big_endian)
 		return ioread32be(gdev->base + offset);
@@ -119,6 +123,16 @@ static u32 gisb_read(struct brcmstb_gisb_arb_device *gdev, int reg)
 		return ioread32(gdev->base + offset);
 }
 
+static u64 gisb_read_address(struct brcmstb_gisb_arb_device *gdev)
+{
+	u64 value;
+
+	value = gisb_read(gdev, ARB_ERR_CAP_ADDR);
+	value |= (u64)gisb_read(gdev, ARB_ERR_CAP_HI_ADDR) << 32;
+
+	return value;
+}
+
 static void gisb_write(struct brcmstb_gisb_arb_device *gdev, u32 val, int reg)
 {
 	int offset = gdev->gisb_offsets[reg];
@@ -127,9 +141,9 @@ static void gisb_write(struct brcmstb_gisb_arb_device *gdev, u32 val, int reg)
 		return;
 
 	if (gdev->big_endian)
-		iowrite32be(val, gdev->base + reg);
+		iowrite32be(val, gdev->base + offset);
 	else
-		iowrite32(val, gdev->base + reg);
+		iowrite32(val, gdev->base + offset);
 }
 
 static ssize_t gisb_arb_get_timeout(struct device *dev,
@@ -185,7 +199,7 @@ static int brcmstb_gisb_arb_decode_addr(struct brcmstb_gisb_arb_device *gdev,
 					const char *reason)
 {
 	u32 cap_status;
-	unsigned long arb_addr;
+	u64 arb_addr;
 	u32 master;
 	const char *m_name;
 	char m_fmt[11];
@@ -197,10 +211,7 @@ static int brcmstb_gisb_arb_decode_addr(struct brcmstb_gisb_arb_device *gdev,
 		return 1;
 
 	/* Read the address and master */
-	arb_addr = gisb_read(gdev, ARB_ERR_CAP_ADDR) & 0xffffffff;
-#if (IS_ENABLED(CONFIG_PHYS_ADDR_T_64BIT))
-	arb_addr |= (u64)gisb_read(gdev, ARB_ERR_CAP_HI_ADDR) << 32;
-#endif
+	arb_addr = gisb_read_address(gdev);
 	master = gisb_read(gdev, ARB_ERR_CAP_MASTER);
 
 	m_name = brcmstb_gisb_master_to_str(gdev, master);
@@ -209,7 +220,7 @@ static int brcmstb_gisb_arb_decode_addr(struct brcmstb_gisb_arb_device *gdev,
 		m_name = m_fmt;
 	}
 
-	pr_crit("%s: %s at 0x%lx [%c %s], core: %s\n",
+	pr_crit("%s: %s at 0x%llx [%c %s], core: %s\n",
 		__func__, reason, arb_addr,
 		cap_status & ARB_ERR_CAP_STATUS_WRITE ? 'W' : 'R',
 		cap_status & ARB_ERR_CAP_STATUS_TIMEOUT ? "timeout" : "",
@@ -221,27 +232,6 @@ static int brcmstb_gisb_arb_decode_addr(struct brcmstb_gisb_arb_device *gdev,
 	return 0;
 }
 
-#ifdef CONFIG_ARM
-static int brcmstb_bus_error_handler(unsigned long addr, unsigned int fsr,
-				     struct pt_regs *regs)
-{
-	int ret = 0;
-	struct brcmstb_gisb_arb_device *gdev;
-
-	/* iterate over each GISB arb registered handlers */
-	list_for_each_entry(gdev, &brcmstb_gisb_arb_device_list, next)
-		ret |= brcmstb_gisb_arb_decode_addr(gdev, "bus error");
-	/*
-	 * If it was an imprecise abort, then we need to correct the
-	 * return address to be _after_ the instruction.
-	*/
-	if (fsr & (1 << 10))
-		regs->ARM_pc += 4;
-
-	return ret;
-}
-#endif
-
 #ifdef CONFIG_MIPS
 static int brcmstb_bus_error_handler(struct pt_regs *regs, int is_fixup)
 {
@@ -279,6 +269,36 @@ static irqreturn_t brcmstb_gisb_tea_handler(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+/*
+ * Dump out gisb errors on die or panic.
+ */
+static int dump_gisb_error(struct notifier_block *self, unsigned long v,
+			   void *p);
+
+static struct notifier_block gisb_die_notifier = {
+	.notifier_call = dump_gisb_error,
+};
+
+static struct notifier_block gisb_panic_notifier = {
+	.notifier_call = dump_gisb_error,
+};
+
+static int dump_gisb_error(struct notifier_block *self, unsigned long v,
+			   void *p)
+{
+	struct brcmstb_gisb_arb_device *gdev;
+	const char *reason = "panic";
+
+	if (self == &gisb_die_notifier)
+		reason = "die";
+
+	/* iterate over each GISB arb registered handlers */
+	list_for_each_entry(gdev, &brcmstb_gisb_arb_device_list, next)
+		brcmstb_gisb_arb_decode_addr(gdev, reason);
+
+	return NOTIFY_DONE;
+}
+
 static DEVICE_ATTR(gisb_arb_timeout, S_IWUSR | S_IRUGO,
 		gisb_arb_get_timeout, gisb_arb_set_timeout);
 
@@ -296,6 +316,7 @@ static const struct of_device_id brcmstb_gisb_arb_of_match[] = {
 	{ .compatible = "brcm,bcm7445-gisb-arb", .data = gisb_offsets_bcm7445 },
 	{ .compatible = "brcm,bcm7435-gisb-arb", .data = gisb_offsets_bcm7435 },
 	{ .compatible = "brcm,bcm7400-gisb-arb", .data = gisb_offsets_bcm7400 },
+	{ .compatible = "brcm,bcm7278-gisb-arb", .data = gisb_offsets_bcm7278 },
 	{ .compatible = "brcm,bcm7038-gisb-arb", .data = gisb_offsets_bcm7038 },
 	{ },
 };
@@ -378,14 +399,16 @@ static int __init brcmstb_gisb_arb_probe(struct platform_device *pdev)
 
 	list_add_tail(&gdev->next, &brcmstb_gisb_arb_device_list);
 
-#ifdef CONFIG_ARM
-	hook_fault_code(22, brcmstb_bus_error_handler, SIGBUS, 0,
-			"imprecise external abort");
-#endif
 #ifdef CONFIG_MIPS
 	board_be_handler = brcmstb_bus_error_handler;
 #endif
 
+	if (list_is_singular(&brcmstb_gisb_arb_device_list)) {
+		register_die_notifier(&gisb_die_notifier);
+		atomic_notifier_chain_register(&panic_notifier_list,
+					       &gisb_panic_notifier);
+	}
+
 	dev_info(&pdev->dev, "registered mem: %p, irqs: %d, %d\n",
 			gdev->base, timeout_irq, tea_irq);
 
diff --git a/drivers/cdrom/cdrom.c b/drivers/cdrom/cdrom.c
index 76c952fd9ab9..e36d160c458f 100644
--- a/drivers/cdrom/cdrom.c
+++ b/drivers/cdrom/cdrom.c
@@ -2178,6 +2178,12 @@ static int cdrom_read_cdda_bpc(struct cdrom_device_info *cdi, __u8 __user *ubuf,
 	if (!q)
 		return -ENXIO;
 
+	if (!blk_queue_scsi_passthrough(q)) {
+		WARN_ONCE(true,
+			  "Attempt read CDDA info through a non-SCSI queue\n");
+		return -EINVAL;
+	}
+
 	cdi->last_sense = 0;
 
 	while (nframes) {
@@ -2195,7 +2201,6 @@ static int cdrom_read_cdda_bpc(struct cdrom_device_info *cdi, __u8 __user *ubuf,
 			break;
 		}
 		req = scsi_req(rq);
-		scsi_req_init(rq);
 
 		ret = blk_rq_map_user(q, rq, NULL, ubuf, len, GFP_KERNEL);
 		if (ret) {
diff --git a/drivers/cdrom/gdrom.c b/drivers/cdrom/gdrom.c
index 1372763a948f..6495b03f576c 100644
--- a/drivers/cdrom/gdrom.c
+++ b/drivers/cdrom/gdrom.c
@@ -583,7 +583,8 @@ static int gdrom_set_interrupt_handlers(void)
  */
 static void gdrom_readdisk_dma(struct work_struct *work)
 {
-	int err, block, block_cnt;
+	int block, block_cnt;
+	blk_status_t err;
 	struct packet_command *read_command;
 	struct list_head *elem, *next;
 	struct request *req;
@@ -641,7 +642,7 @@ static void gdrom_readdisk_dma(struct work_struct *work)
 		__raw_writeb(1, GDROM_DMA_STATUS_REG);
 		wait_event_interruptible_timeout(request_queue,
 			gd.transfer == 0, GDROM_DEFAULT_TIMEOUT);
-		err = gd.transfer ? -EIO : 0;
+		err = gd.transfer ? BLK_STS_IOERR : BLK_STS_OK;
 		gd.transfer = 0;
 		gd.pending = 0;
 		/* now seek to take the request spinlock
@@ -670,11 +671,11 @@ static void gdrom_request(struct request_queue *rq)
 			break;
 		case REQ_OP_WRITE:
 			pr_notice("Read only device - write request ignored\n");
-			__blk_end_request_all(req, -EIO);
+			__blk_end_request_all(req, BLK_STS_IOERR);
 			break;
 		default:
 			printk(KERN_DEBUG "gdrom: Non-fs request ignored\n");
-			__blk_end_request_all(req, -EIO);
+			__blk_end_request_all(req, BLK_STS_IOERR);
 			break;
 		}
 	}
@@ -812,6 +813,7 @@ static int probe_gdrom(struct platform_device *devptr)
 		err = -ENOMEM;
 		goto probe_fail_requestq;
 	}
+	blk_queue_bounce_limit(gd.gdrom_rq, BLK_BOUNCE_HIGH);
 
 	err = probe_gdrom_setupqueue();
 	if (err)
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index 31adbebf812e..ccd239ab879f 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -17,6 +17,8 @@ config DEVMEM
 
 config DEVKMEM
 	bool "/dev/kmem virtual device support"
+	# On arm64, VMALLOC_START < PAGE_OFFSET, which confuses kmem read/write
+	depends on !ARM64
 	help
 	  Say Y here if you want to support the /dev/kmem device. The
 	  /dev/kmem device is rarely used, but can be used for certain
@@ -539,15 +541,6 @@ config HANGCHECK_TIMER
 	  out to lunch past a certain margin.  It can reboot the system
 	  or merely print a warning.
 
-config MMTIMER
-	tristate "MMTIMER Memory mapped RTC for SGI Altix"
-	depends on IA64_GENERIC || IA64_SGI_SN2
-	depends on POSIX_TIMERS
-	default y
-	help
-	  The mmtimer device allows direct userspace access to the
-	  Altix system timer.
-
 config UV_MMTIMER
 	tristate "UV_MMTIMER Memory mapped RTC for SGI UV"
 	depends on X86_UV
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index 6e6c244a66a0..53e33720818c 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -10,7 +10,6 @@ obj-$(CONFIG_VIRTIO_CONSOLE)	+= virtio_console.o
 obj-$(CONFIG_RAW_DRIVER)	+= raw.o
 obj-$(CONFIG_SGI_SNSC)		+= snsc.o snsc_event.o
 obj-$(CONFIG_MSPEC)		+= mspec.o
-obj-$(CONFIG_MMTIMER)		+= mmtimer.o
 obj-$(CONFIG_UV_MMTIMER)	+= uv_mmtimer.o
 obj-$(CONFIG_IBM_BSR)		+= bsr.o
 obj-$(CONFIG_SGI_MBCS)		+= mbcs.o
diff --git a/drivers/char/hw_random/mtk-rng.c b/drivers/char/hw_random/mtk-rng.c
index df8eb54fd5a3..8da7bcf54105 100644
--- a/drivers/char/hw_random/mtk-rng.c
+++ b/drivers/char/hw_random/mtk-rng.c
@@ -25,6 +25,10 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+
+/* Runtime PM autosuspend timeout: */
+#define RNG_AUTOSUSPEND_TIMEOUT		100
 
 #define USEC_POLL			2
 #define TIMEOUT_POLL			20
@@ -90,6 +94,8 @@ static int mtk_rng_read(struct hwrng *rng, void *buf, size_t max, bool wait)
 	struct mtk_rng *priv = to_mtk_rng(rng);
 	int retval = 0;
 
+	pm_runtime_get_sync((struct device *)priv->rng.priv);
+
 	while (max >= sizeof(u32)) {
 		if (!mtk_rng_wait_ready(rng, wait))
 			break;
@@ -100,6 +106,9 @@ static int mtk_rng_read(struct hwrng *rng, void *buf, size_t max, bool wait)
 		max -= sizeof(u32);
 	}
 
+	pm_runtime_mark_last_busy((struct device *)priv->rng.priv);
+	pm_runtime_put_sync_autosuspend((struct device *)priv->rng.priv);
+
 	return retval || !wait ? retval : -EIO;
 }
 
@@ -120,9 +129,12 @@ static int mtk_rng_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	priv->rng.name = pdev->name;
+#ifndef CONFIG_PM
 	priv->rng.init = mtk_rng_init;
 	priv->rng.cleanup = mtk_rng_cleanup;
+#endif
 	priv->rng.read = mtk_rng_read;
+	priv->rng.priv = (unsigned long)&pdev->dev;
 
 	priv->clk = devm_clk_get(&pdev->dev, "rng");
 	if (IS_ERR(priv->clk)) {
@@ -142,11 +154,40 @@ static int mtk_rng_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	dev_set_drvdata(&pdev->dev, priv);
+	pm_runtime_set_autosuspend_delay(&pdev->dev, RNG_AUTOSUSPEND_TIMEOUT);
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
 	dev_info(&pdev->dev, "registered RNG driver\n");
 
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int mtk_rng_runtime_suspend(struct device *dev)
+{
+	struct mtk_rng *priv = dev_get_drvdata(dev);
+
+	mtk_rng_cleanup(&priv->rng);
+
+	return 0;
+}
+
+static int mtk_rng_runtime_resume(struct device *dev)
+{
+	struct mtk_rng *priv = dev_get_drvdata(dev);
+
+	return mtk_rng_init(&priv->rng);
+}
+
+static UNIVERSAL_DEV_PM_OPS(mtk_rng_pm_ops, mtk_rng_runtime_suspend,
+			    mtk_rng_runtime_resume, NULL);
+#define MTK_RNG_PM_OPS (&mtk_rng_pm_ops)
+#else	/* CONFIG_PM */
+#define MTK_RNG_PM_OPS NULL
+#endif	/* CONFIG_PM */
+
 static const struct of_device_id mtk_rng_match[] = {
 	{ .compatible = "mediatek,mt7623-rng" },
 	{},
@@ -157,6 +198,7 @@ static struct platform_driver mtk_rng_driver = {
 	.probe          = mtk_rng_probe,
 	.driver = {
 		.name = MTK_RNG_DEV,
+		.pm = MTK_RNG_PM_OPS,
 		.of_match_table = mtk_rng_match,
 	},
 };
diff --git a/drivers/char/hw_random/omap3-rom-rng.c b/drivers/char/hw_random/omap3-rom-rng.c
index 37a58d78aab3..38b719017186 100644
--- a/drivers/char/hw_random/omap3-rom-rng.c
+++ b/drivers/char/hw_random/omap3-rom-rng.c
@@ -53,7 +53,10 @@ static int omap3_rom_rng_get_random(void *buf, unsigned int count)
 
 	cancel_delayed_work_sync(&idle_work);
 	if (rng_idle) {
-		clk_prepare_enable(rng_clk);
+		r = clk_prepare_enable(rng_clk);
+		if (r)
+			return r;
+
 		r = omap3_rom_rng_call(0, 0, RNG_GEN_PRNG_HW_INIT);
 		if (r != 0) {
 			clk_disable_unprepare(rng_clk);
@@ -88,6 +91,8 @@ static struct hwrng omap3_rom_rng_ops = {
 
 static int omap3_rom_rng_probe(struct platform_device *pdev)
 {
+	int ret = 0;
+
 	pr_info("initializing\n");
 
 	omap3_rom_rng_call = pdev->dev.platform_data;
@@ -104,7 +109,9 @@ static int omap3_rom_rng_probe(struct platform_device *pdev)
 	}
 
 	/* Leave the RNG in reset state. */
-	clk_prepare_enable(rng_clk);
+	ret = clk_prepare_enable(rng_clk);
+	if (ret)
+		return ret;
 	omap3_rom_rng_idle(0);
 
 	return hwrng_register(&omap3_rom_rng_ops);
diff --git a/drivers/char/hw_random/timeriomem-rng.c b/drivers/char/hw_random/timeriomem-rng.c
index a0faa5f05deb..03ff5483d865 100644
--- a/drivers/char/hw_random/timeriomem-rng.c
+++ b/drivers/char/hw_random/timeriomem-rng.c
@@ -151,8 +151,15 @@ static int timeriomem_rng_probe(struct platform_device *pdev)
 			dev_err(&pdev->dev, "missing period\n");
 			return -EINVAL;
 		}
+
+		if (!of_property_read_u32(pdev->dev.of_node,
+						"quality", &i))
+			priv->rng_ops.quality = i;
+		else
+			priv->rng_ops.quality = 0;
 	} else {
 		period = pdata->period;
+		priv->rng_ops.quality = pdata->quality;
 	}
 
 	priv->period = ns_to_ktime(period * NSEC_PER_USEC);
diff --git a/drivers/char/ipmi/ipmi_watchdog.c b/drivers/char/ipmi/ipmi_watchdog.c
index d165af8abe36..a5c6cfe71a8e 100644
--- a/drivers/char/ipmi/ipmi_watchdog.c
+++ b/drivers/char/ipmi/ipmi_watchdog.c
@@ -821,7 +821,7 @@ static ssize_t ipmi_read(struct file *file,
 			 loff_t      *ppos)
 {
 	int          rv = 0;
-	wait_queue_t wait;
+	wait_queue_entry_t wait;
 
 	if (count <= 0)
 		return 0;
diff --git a/drivers/char/mmtimer.c b/drivers/char/mmtimer.c
deleted file mode 100644
index 0e7fcb04f01e..000000000000
--- a/drivers/char/mmtimer.c
+++ /dev/null
@@ -1,858 +0,0 @@
-/*
- * Timer device implementation for SGI SN platforms.
- *
- * 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) 2001-2006 Silicon Graphics, Inc.  All rights reserved.
- *
- * This driver exports an API that should be supportable by any HPET or IA-PC
- * multimedia timer.  The code below is currently specific to the SGI Altix
- * SHub RTC, however.
- *
- * 11/01/01 - jbarnes - initial revision
- * 9/10/04 - Christoph Lameter - remove interrupt support for kernel inclusion
- * 10/1/04 - Christoph Lameter - provide posix clock CLOCK_SGI_CYCLE
- * 10/13/04 - Christoph Lameter, Dimitri Sivanich - provide timer interrupt
- *		support via the posix timer interface
- */
-
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <linux/ioctl.h>
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/errno.h>
-#include <linux/mm.h>
-#include <linux/fs.h>
-#include <linux/mmtimer.h>
-#include <linux/miscdevice.h>
-#include <linux/posix-timers.h>
-#include <linux/interrupt.h>
-#include <linux/time.h>
-#include <linux/math64.h>
-#include <linux/mutex.h>
-#include <linux/slab.h>
-
-#include <linux/uaccess.h>
-#include <asm/sn/addrs.h>
-#include <asm/sn/intr.h>
-#include <asm/sn/shub_mmr.h>
-#include <asm/sn/nodepda.h>
-#include <asm/sn/shubio.h>
-
-MODULE_AUTHOR("Jesse Barnes <jbarnes@sgi.com>");
-MODULE_DESCRIPTION("SGI Altix RTC Timer");
-MODULE_LICENSE("GPL");
-
-/* name of the device, usually in /dev */
-#define MMTIMER_NAME "mmtimer"
-#define MMTIMER_DESC "SGI Altix RTC Timer"
-#define MMTIMER_VERSION "2.1"
-
-#define RTC_BITS 55 /* 55 bits for this implementation */
-
-static struct k_clock sgi_clock;
-
-extern unsigned long sn_rtc_cycles_per_second;
-
-#define RTC_COUNTER_ADDR        ((long *)LOCAL_MMR_ADDR(SH_RTC))
-
-#define rtc_time()              (*RTC_COUNTER_ADDR)
-
-static DEFINE_MUTEX(mmtimer_mutex);
-static long mmtimer_ioctl(struct file *file, unsigned int cmd,
-						unsigned long arg);
-static int mmtimer_mmap(struct file *file, struct vm_area_struct *vma);
-
-/*
- * Period in femtoseconds (10^-15 s)
- */
-static unsigned long mmtimer_femtoperiod = 0;
-
-static const struct file_operations mmtimer_fops = {
-	.owner = THIS_MODULE,
-	.mmap =	mmtimer_mmap,
-	.unlocked_ioctl = mmtimer_ioctl,
-	.llseek = noop_llseek,
-};
-
-/*
- * We only have comparison registers RTC1-4 currently available per
- * node.  RTC0 is used by SAL.
- */
-/* Check for an RTC interrupt pending */
-static int mmtimer_int_pending(int comparator)
-{
-	if (HUB_L((unsigned long *)LOCAL_MMR_ADDR(SH_EVENT_OCCURRED)) &
-			SH_EVENT_OCCURRED_RTC1_INT_MASK << comparator)
-		return 1;
-	else
-		return 0;
-}
-
-/* Clear the RTC interrupt pending bit */
-static void mmtimer_clr_int_pending(int comparator)
-{
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_EVENT_OCCURRED_ALIAS),
-		SH_EVENT_OCCURRED_RTC1_INT_MASK << comparator);
-}
-
-/* Setup timer on comparator RTC1 */
-static void mmtimer_setup_int_0(int cpu, u64 expires)
-{
-	u64 val;
-
-	/* Disable interrupt */
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_RTC1_INT_ENABLE), 0UL);
-
-	/* Initialize comparator value */
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_INT_CMPB), -1L);
-
-	/* Clear pending bit */
-	mmtimer_clr_int_pending(0);
-
-	val = ((u64)SGI_MMTIMER_VECTOR << SH_RTC1_INT_CONFIG_IDX_SHFT) |
-		((u64)cpu_physical_id(cpu) <<
-			SH_RTC1_INT_CONFIG_PID_SHFT);
-
-	/* Set configuration */
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_RTC1_INT_CONFIG), val);
-
-	/* Enable RTC interrupts */
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_RTC1_INT_ENABLE), 1UL);
-
-	/* Initialize comparator value */
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_INT_CMPB), expires);
-
-
-}
-
-/* Setup timer on comparator RTC2 */
-static void mmtimer_setup_int_1(int cpu, u64 expires)
-{
-	u64 val;
-
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_RTC2_INT_ENABLE), 0UL);
-
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_INT_CMPC), -1L);
-
-	mmtimer_clr_int_pending(1);
-
-	val = ((u64)SGI_MMTIMER_VECTOR << SH_RTC2_INT_CONFIG_IDX_SHFT) |
-		((u64)cpu_physical_id(cpu) <<
-			SH_RTC2_INT_CONFIG_PID_SHFT);
-
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_RTC2_INT_CONFIG), val);
-
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_RTC2_INT_ENABLE), 1UL);
-
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_INT_CMPC), expires);
-}
-
-/* Setup timer on comparator RTC3 */
-static void mmtimer_setup_int_2(int cpu, u64 expires)
-{
-	u64 val;
-
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_RTC3_INT_ENABLE), 0UL);
-
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_INT_CMPD), -1L);
-
-	mmtimer_clr_int_pending(2);
-
-	val = ((u64)SGI_MMTIMER_VECTOR << SH_RTC3_INT_CONFIG_IDX_SHFT) |
-		((u64)cpu_physical_id(cpu) <<
-			SH_RTC3_INT_CONFIG_PID_SHFT);
-
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_RTC3_INT_CONFIG), val);
-
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_RTC3_INT_ENABLE), 1UL);
-
-	HUB_S((u64 *)LOCAL_MMR_ADDR(SH_INT_CMPD), expires);
-}
-
-/*
- * This function must be called with interrupts disabled and preemption off
- * in order to insure that the setup succeeds in a deterministic time frame.
- * It will check if the interrupt setup succeeded.
- */
-static int mmtimer_setup(int cpu, int comparator, unsigned long expires,
-	u64 *set_completion_time)
-{
-	switch (comparator) {
-	case 0:
-		mmtimer_setup_int_0(cpu, expires);
-		break;
-	case 1:
-		mmtimer_setup_int_1(cpu, expires);
-		break;
-	case 2:
-		mmtimer_setup_int_2(cpu, expires);
-		break;
-	}
-	/* We might've missed our expiration time */
-	*set_completion_time = rtc_time();
-	if (*set_completion_time <= expires)
-		return 1;
-
-	/*
-	 * If an interrupt is already pending then its okay
-	 * if not then we failed
-	 */
-	return mmtimer_int_pending(comparator);
-}
-
-static int mmtimer_disable_int(long nasid, int comparator)
-{
-	switch (comparator) {
-	case 0:
-		nasid == -1 ? HUB_S((u64 *)LOCAL_MMR_ADDR(SH_RTC1_INT_ENABLE),
-			0UL) : REMOTE_HUB_S(nasid, SH_RTC1_INT_ENABLE, 0UL);
-		break;
-	case 1:
-		nasid == -1 ? HUB_S((u64 *)LOCAL_MMR_ADDR(SH_RTC2_INT_ENABLE),
-			0UL) : REMOTE_HUB_S(nasid, SH_RTC2_INT_ENABLE, 0UL);
-		break;
-	case 2:
-		nasid == -1 ? HUB_S((u64 *)LOCAL_MMR_ADDR(SH_RTC3_INT_ENABLE),
-			0UL) : REMOTE_HUB_S(nasid, SH_RTC3_INT_ENABLE, 0UL);
-		break;
-	default:
-		return -EFAULT;
-	}
-	return 0;
-}
-
-#define COMPARATOR	1		/* The comparator to use */
-
-#define TIMER_OFF	0xbadcabLL	/* Timer is not setup */
-#define TIMER_SET	0		/* Comparator is set for this timer */
-
-#define MMTIMER_INTERVAL_RETRY_INCREMENT_DEFAULT 40
-
-/* There is one of these for each timer */
-struct mmtimer {
-	struct rb_node list;
-	struct k_itimer *timer;
-	int cpu;
-};
-
-struct mmtimer_node {
-	spinlock_t lock ____cacheline_aligned;
-	struct rb_root timer_head;
-	struct rb_node *next;
-	struct tasklet_struct tasklet;
-};
-static struct mmtimer_node *timers;
-
-static unsigned mmtimer_interval_retry_increment =
-	MMTIMER_INTERVAL_RETRY_INCREMENT_DEFAULT;
-module_param(mmtimer_interval_retry_increment, uint, 0644);
-MODULE_PARM_DESC(mmtimer_interval_retry_increment,
-	"RTC ticks to add to expiration on interval retry (default 40)");
-
-/*
- * Add a new mmtimer struct to the node's mmtimer list.
- * This function assumes the struct mmtimer_node is locked.
- */
-static void mmtimer_add_list(struct mmtimer *n)
-{
-	int nodeid = n->timer->it.mmtimer.node;
-	unsigned long expires = n->timer->it.mmtimer.expires;
-	struct rb_node **link = &timers[nodeid].timer_head.rb_node;
-	struct rb_node *parent = NULL;
-	struct mmtimer *x;
-
-	/*
-	 * Find the right place in the rbtree:
-	 */
-	while (*link) {
-		parent = *link;
-		x = rb_entry(parent, struct mmtimer, list);
-
-		if (expires < x->timer->it.mmtimer.expires)
-			link = &(*link)->rb_left;
-		else
-			link = &(*link)->rb_right;
-	}
-
-	/*
-	 * Insert the timer to the rbtree and check whether it
-	 * replaces the first pending timer
-	 */
-	rb_link_node(&n->list, parent, link);
-	rb_insert_color(&n->list, &timers[nodeid].timer_head);
-
-	if (!timers[nodeid].next || expires < rb_entry(timers[nodeid].next,
-			struct mmtimer, list)->timer->it.mmtimer.expires)
-		timers[nodeid].next = &n->list;
-}
-
-/*
- * Set the comparator for the next timer.
- * This function assumes the struct mmtimer_node is locked.
- */
-static void mmtimer_set_next_timer(int nodeid)
-{
-	struct mmtimer_node *n = &timers[nodeid];
-	struct mmtimer *x;
-	struct k_itimer *t;
-	u64 expires, exp, set_completion_time;
-	int i;
-
-restart:
-	if (n->next == NULL)
-		return;
-
-	x = rb_entry(n->next, struct mmtimer, list);
-	t = x->timer;
-	if (!t->it.mmtimer.incr) {
-		/* Not an interval timer */
-		if (!mmtimer_setup(x->cpu, COMPARATOR,
-					t->it.mmtimer.expires,
-					&set_completion_time)) {
-			/* Late setup, fire now */
-			tasklet_schedule(&n->tasklet);
-		}
-		return;
-	}
-
-	/* Interval timer */
-	i = 0;
-	expires = exp = t->it.mmtimer.expires;
-	while (!mmtimer_setup(x->cpu, COMPARATOR, expires,
-				&set_completion_time)) {
-		int to;
-
-		i++;
-		expires = set_completion_time +
-				mmtimer_interval_retry_increment + (1 << i);
-		/* Calculate overruns as we go. */
-		to = ((u64)(expires - exp) / t->it.mmtimer.incr);
-		if (to) {
-			t->it_overrun += to;
-			t->it.mmtimer.expires += t->it.mmtimer.incr * to;
-			exp = t->it.mmtimer.expires;
-		}
-		if (i > 20) {
-			printk(KERN_ALERT "mmtimer: cannot reschedule timer\n");
-			t->it.mmtimer.clock = TIMER_OFF;
-			n->next = rb_next(&x->list);
-			rb_erase(&x->list, &n->timer_head);
-			kfree(x);
-			goto restart;
-		}
-	}
-}
-
-/**
- * mmtimer_ioctl - ioctl interface for /dev/mmtimer
- * @file: file structure for the device
- * @cmd: command to execute
- * @arg: optional argument to command
- *
- * Executes the command specified by @cmd.  Returns 0 for success, < 0 for
- * failure.
- *
- * Valid commands:
- *
- * %MMTIMER_GETOFFSET - Should return the offset (relative to the start
- * of the page where the registers are mapped) for the counter in question.
- *
- * %MMTIMER_GETRES - Returns the resolution of the clock in femto (10^-15)
- * seconds
- *
- * %MMTIMER_GETFREQ - Copies the frequency of the clock in Hz to the address
- * specified by @arg
- *
- * %MMTIMER_GETBITS - Returns the number of bits in the clock's counter
- *
- * %MMTIMER_MMAPAVAIL - Returns 1 if the registers can be mmap'd into userspace
- *
- * %MMTIMER_GETCOUNTER - Gets the current value in the counter and places it
- * in the address specified by @arg.
- */
-static long mmtimer_ioctl(struct file *file, unsigned int cmd,
-						unsigned long arg)
-{
-	int ret = 0;
-
-	mutex_lock(&mmtimer_mutex);
-
-	switch (cmd) {
-	case MMTIMER_GETOFFSET:	/* offset of the counter */
-		/*
-		 * SN RTC registers are on their own 64k page
-		 */
-		if(PAGE_SIZE <= (1 << 16))
-			ret = (((long)RTC_COUNTER_ADDR) & (PAGE_SIZE-1)) / 8;
-		else
-			ret = -ENOSYS;
-		break;
-
-	case MMTIMER_GETRES: /* resolution of the clock in 10^-15 s */
-		if(copy_to_user((unsigned long __user *)arg,
-				&mmtimer_femtoperiod, sizeof(unsigned long)))
-			ret = -EFAULT;
-		break;
-
-	case MMTIMER_GETFREQ: /* frequency in Hz */
-		if(copy_to_user((unsigned long __user *)arg,
-				&sn_rtc_cycles_per_second,
-				sizeof(unsigned long)))
-			ret = -EFAULT;
-		break;
-
-	case MMTIMER_GETBITS: /* number of bits in the clock */
-		ret = RTC_BITS;
-		break;
-
-	case MMTIMER_MMAPAVAIL: /* can we mmap the clock into userspace? */
-		ret = (PAGE_SIZE <= (1 << 16)) ? 1 : 0;
-		break;
-
-	case MMTIMER_GETCOUNTER:
-		if(copy_to_user((unsigned long __user *)arg,
-				RTC_COUNTER_ADDR, sizeof(unsigned long)))
-			ret = -EFAULT;
-		break;
-	default:
-		ret = -ENOTTY;
-		break;
-	}
-	mutex_unlock(&mmtimer_mutex);
-	return ret;
-}
-
-/**
- * mmtimer_mmap - maps the clock's registers into userspace
- * @file: file structure for the device
- * @vma: VMA to map the registers into
- *
- * Calls remap_pfn_range() to map the clock's registers into
- * the calling process' address space.
- */
-static int mmtimer_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	unsigned long mmtimer_addr;
-
-	if (vma->vm_end - vma->vm_start != PAGE_SIZE)
-		return -EINVAL;
-
-	if (vma->vm_flags & VM_WRITE)
-		return -EPERM;
-
-	if (PAGE_SIZE > (1 << 16))
-		return -ENOSYS;
-
-	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
-
-	mmtimer_addr = __pa(RTC_COUNTER_ADDR);
-	mmtimer_addr &= ~(PAGE_SIZE - 1);
-	mmtimer_addr &= 0xfffffffffffffffUL;
-
-	if (remap_pfn_range(vma, vma->vm_start, mmtimer_addr >> PAGE_SHIFT,
-					PAGE_SIZE, vma->vm_page_prot)) {
-		printk(KERN_ERR "remap_pfn_range failed in mmtimer.c\n");
-		return -EAGAIN;
-	}
-
-	return 0;
-}
-
-static struct miscdevice mmtimer_miscdev = {
-	.minor = SGI_MMTIMER,
-	.name = MMTIMER_NAME,
-	.fops = &mmtimer_fops
-};
-
-static struct timespec sgi_clock_offset;
-static int sgi_clock_period;
-
-/*
- * Posix Timer Interface
- */
-
-static struct timespec sgi_clock_offset;
-static int sgi_clock_period;
-
-static int sgi_clock_get(clockid_t clockid, struct timespec64 *tp)
-{
-	u64 nsec;
-
-	nsec = rtc_time() * sgi_clock_period
-			+ sgi_clock_offset.tv_nsec;
-	*tp = ns_to_timespec64(nsec);
-	tp->tv_sec += sgi_clock_offset.tv_sec;
-	return 0;
-};
-
-static int sgi_clock_set(const clockid_t clockid, const struct timespec64 *tp)
-{
-
-	u64 nsec;
-	u32 rem;
-
-	nsec = rtc_time() * sgi_clock_period;
-
-	sgi_clock_offset.tv_sec = tp->tv_sec - div_u64_rem(nsec, NSEC_PER_SEC, &rem);
-
-	if (rem <= tp->tv_nsec)
-		sgi_clock_offset.tv_nsec = tp->tv_sec - rem;
-	else {
-		sgi_clock_offset.tv_nsec = tp->tv_sec + NSEC_PER_SEC - rem;
-		sgi_clock_offset.tv_sec--;
-	}
-	return 0;
-}
-
-/**
- * mmtimer_interrupt - timer interrupt handler
- * @irq: irq received
- * @dev_id: device the irq came from
- *
- * Called when one of the comarators matches the counter, This
- * routine will send signals to processes that have requested
- * them.
- *
- * This interrupt is run in an interrupt context
- * by the SHUB. It is therefore safe to locally access SHub
- * registers.
- */
-static irqreturn_t
-mmtimer_interrupt(int irq, void *dev_id)
-{
-	unsigned long expires = 0;
-	int result = IRQ_NONE;
-	unsigned indx = cpu_to_node(smp_processor_id());
-	struct mmtimer *base;
-
-	spin_lock(&timers[indx].lock);
-	base = rb_entry(timers[indx].next, struct mmtimer, list);
-	if (base == NULL) {
-		spin_unlock(&timers[indx].lock);
-		return result;
-	}
-
-	if (base->cpu == smp_processor_id()) {
-		if (base->timer)
-			expires = base->timer->it.mmtimer.expires;
-		/* expires test won't work with shared irqs */
-		if ((mmtimer_int_pending(COMPARATOR) > 0) ||
-			(expires && (expires <= rtc_time()))) {
-			mmtimer_clr_int_pending(COMPARATOR);
-			tasklet_schedule(&timers[indx].tasklet);
-			result = IRQ_HANDLED;
-		}
-	}
-	spin_unlock(&timers[indx].lock);
-	return result;
-}
-
-static void mmtimer_tasklet(unsigned long data)
-{
-	int nodeid = data;
-	struct mmtimer_node *mn = &timers[nodeid];
-	struct mmtimer *x;
-	struct k_itimer *t;
-	unsigned long flags;
-
-	/* Send signal and deal with periodic signals */
-	spin_lock_irqsave(&mn->lock, flags);
-	if (!mn->next)
-		goto out;
-
-	x = rb_entry(mn->next, struct mmtimer, list);
-	t = x->timer;
-
-	if (t->it.mmtimer.clock == TIMER_OFF)
-		goto out;
-
-	t->it_overrun = 0;
-
-	mn->next = rb_next(&x->list);
-	rb_erase(&x->list, &mn->timer_head);
-
-	if (posix_timer_event(t, 0) != 0)
-		t->it_overrun++;
-
-	if(t->it.mmtimer.incr) {
-		t->it.mmtimer.expires += t->it.mmtimer.incr;
-		mmtimer_add_list(x);
-	} else {
-		/* Ensure we don't false trigger in mmtimer_interrupt */
-		t->it.mmtimer.clock = TIMER_OFF;
-		t->it.mmtimer.expires = 0;
-		kfree(x);
-	}
-	/* Set comparator for next timer, if there is one */
-	mmtimer_set_next_timer(nodeid);
-
-	t->it_overrun_last = t->it_overrun;
-out:
-	spin_unlock_irqrestore(&mn->lock, flags);
-}
-
-static int sgi_timer_create(struct k_itimer *timer)
-{
-	/* Insure that a newly created timer is off */
-	timer->it.mmtimer.clock = TIMER_OFF;
-	return 0;
-}
-
-/* This does not really delete a timer. It just insures
- * that the timer is not active
- *
- * Assumption: it_lock is already held with irq's disabled
- */
-static int sgi_timer_del(struct k_itimer *timr)
-{
-	cnodeid_t nodeid = timr->it.mmtimer.node;
-	unsigned long irqflags;
-
-	spin_lock_irqsave(&timers[nodeid].lock, irqflags);
-	if (timr->it.mmtimer.clock != TIMER_OFF) {
-		unsigned long expires = timr->it.mmtimer.expires;
-		struct rb_node *n = timers[nodeid].timer_head.rb_node;
-		struct mmtimer *uninitialized_var(t);
-		int r = 0;
-
-		timr->it.mmtimer.clock = TIMER_OFF;
-		timr->it.mmtimer.expires = 0;
-
-		while (n) {
-			t = rb_entry(n, struct mmtimer, list);
-			if (t->timer == timr)
-				break;
-
-			if (expires < t->timer->it.mmtimer.expires)
-				n = n->rb_left;
-			else
-				n = n->rb_right;
-		}
-
-		if (!n) {
-			spin_unlock_irqrestore(&timers[nodeid].lock, irqflags);
-			return 0;
-		}
-
-		if (timers[nodeid].next == n) {
-			timers[nodeid].next = rb_next(n);
-			r = 1;
-		}
-
-		rb_erase(n, &timers[nodeid].timer_head);
-		kfree(t);
-
-		if (r) {
-			mmtimer_disable_int(cnodeid_to_nasid(nodeid),
-				COMPARATOR);
-			mmtimer_set_next_timer(nodeid);
-		}
-	}
-	spin_unlock_irqrestore(&timers[nodeid].lock, irqflags);
-	return 0;
-}
-
-/* Assumption: it_lock is already held with irq's disabled */
-static void sgi_timer_get(struct k_itimer *timr, struct itimerspec64 *cur_setting)
-{
-
-	if (timr->it.mmtimer.clock == TIMER_OFF) {
-		cur_setting->it_interval.tv_nsec = 0;
-		cur_setting->it_interval.tv_sec = 0;
-		cur_setting->it_value.tv_nsec = 0;
-		cur_setting->it_value.tv_sec =0;
-		return;
-	}
-
-	cur_setting->it_interval = ns_to_timespec64(timr->it.mmtimer.incr * sgi_clock_period);
-	cur_setting->it_value = ns_to_timespec64((timr->it.mmtimer.expires - rtc_time()) * sgi_clock_period);
-}
-
-
-static int sgi_timer_set(struct k_itimer *timr, int flags,
-	struct itimerspec64 *new_setting,
-	struct itimerspec64 *old_setting)
-{
-	unsigned long when, period, irqflags;
-	int err = 0;
-	cnodeid_t nodeid;
-	struct mmtimer *base;
-	struct rb_node *n;
-
-	if (old_setting)
-		sgi_timer_get(timr, old_setting);
-
-	sgi_timer_del(timr);
-	when = timespec64_to_ns(&new_setting->it_value);
-	period = timespec64_to_ns(&new_setting->it_interval);
-
-	if (when == 0)
-		/* Clear timer */
-		return 0;
-
-	base = kmalloc(sizeof(struct mmtimer), GFP_KERNEL);
-	if (base == NULL)
-		return -ENOMEM;
-
-	if (flags & TIMER_ABSTIME) {
-		struct timespec64 n;
-		unsigned long now;
-
-		getnstimeofday64(&n);
-		now = timespec64_to_ns(&n);
-		if (when > now)
-			when -= now;
-		else
-			/* Fire the timer immediately */
-			when = 0;
-	}
-
-	/*
-	 * Convert to sgi clock period. Need to keep rtc_time() as near as possible
-	 * to getnstimeofday() in order to be as faithful as possible to the time
-	 * specified.
-	 */
-	when = (when + sgi_clock_period - 1) / sgi_clock_period + rtc_time();
-	period = (period + sgi_clock_period - 1)  / sgi_clock_period;
-
-	/*
-	 * We are allocating a local SHub comparator. If we would be moved to another
-	 * cpu then another SHub may be local to us. Prohibit that by switching off
-	 * preemption.
-	 */
-	preempt_disable();
-
-	nodeid =  cpu_to_node(smp_processor_id());
-
-	/* Lock the node timer structure */
-	spin_lock_irqsave(&timers[nodeid].lock, irqflags);
-
-	base->timer = timr;
-	base->cpu = smp_processor_id();
-
-	timr->it.mmtimer.clock = TIMER_SET;
-	timr->it.mmtimer.node = nodeid;
-	timr->it.mmtimer.incr = period;
-	timr->it.mmtimer.expires = when;
-
-	n = timers[nodeid].next;
-
-	/* Add the new struct mmtimer to node's timer list */
-	mmtimer_add_list(base);
-
-	if (timers[nodeid].next == n) {
-		/* No need to reprogram comparator for now */
-		spin_unlock_irqrestore(&timers[nodeid].lock, irqflags);
-		preempt_enable();
-		return err;
-	}
-
-	/* We need to reprogram the comparator */
-	if (n)
-		mmtimer_disable_int(cnodeid_to_nasid(nodeid), COMPARATOR);
-
-	mmtimer_set_next_timer(nodeid);
-
-	/* Unlock the node timer structure */
-	spin_unlock_irqrestore(&timers[nodeid].lock, irqflags);
-
-	preempt_enable();
-
-	return err;
-}
-
-static int sgi_clock_getres(const clockid_t which_clock, struct timespec64 *tp)
-{
-	tp->tv_sec = 0;
-	tp->tv_nsec = sgi_clock_period;
-	return 0;
-}
-
-static struct k_clock sgi_clock = {
-	.clock_set	= sgi_clock_set,
-	.clock_get	= sgi_clock_get,
-	.clock_getres	= sgi_clock_getres,
-	.timer_create	= sgi_timer_create,
-	.timer_set	= sgi_timer_set,
-	.timer_del	= sgi_timer_del,
-	.timer_get	= sgi_timer_get
-};
-
-/**
- * mmtimer_init - device initialization routine
- *
- * Does initial setup for the mmtimer device.
- */
-static int __init mmtimer_init(void)
-{
-	cnodeid_t node, maxn = -1;
-
-	if (!ia64_platform_is("sn2"))
-		return 0;
-
-	/*
-	 * Sanity check the cycles/sec variable
-	 */
-	if (sn_rtc_cycles_per_second < 100000) {
-		printk(KERN_ERR "%s: unable to determine clock frequency\n",
-		       MMTIMER_NAME);
-		goto out1;
-	}
-
-	mmtimer_femtoperiod = ((unsigned long)1E15 + sn_rtc_cycles_per_second /
-			       2) / sn_rtc_cycles_per_second;
-
-	if (request_irq(SGI_MMTIMER_VECTOR, mmtimer_interrupt, IRQF_PERCPU, MMTIMER_NAME, NULL)) {
-		printk(KERN_WARNING "%s: unable to allocate interrupt.",
-			MMTIMER_NAME);
-		goto out1;
-	}
-
-	if (misc_register(&mmtimer_miscdev)) {
-		printk(KERN_ERR "%s: failed to register device\n",
-		       MMTIMER_NAME);
-		goto out2;
-	}
-
-	/* Get max numbered node, calculate slots needed */
-	for_each_online_node(node) {
-		maxn = node;
-	}
-	maxn++;
-
-	/* Allocate list of node ptrs to mmtimer_t's */
-	timers = kzalloc(sizeof(struct mmtimer_node)*maxn, GFP_KERNEL);
-	if (!timers) {
-		printk(KERN_ERR "%s: failed to allocate memory for device\n",
-				MMTIMER_NAME);
-		goto out3;
-	}
-
-	/* Initialize struct mmtimer's for each online node */
-	for_each_online_node(node) {
-		spin_lock_init(&timers[node].lock);
-		tasklet_init(&timers[node].tasklet, mmtimer_tasklet,
-			(unsigned long) node);
-	}
-
-	sgi_clock_period = NSEC_PER_SEC / sn_rtc_cycles_per_second;
-	posix_timers_register_clock(CLOCK_SGI_CYCLE, &sgi_clock);
-
-	printk(KERN_INFO "%s: v%s, %ld MHz\n", MMTIMER_DESC, MMTIMER_VERSION,
-	       sn_rtc_cycles_per_second/(unsigned long)1E6);
-
-	return 0;
-
-out3:
-	misc_deregister(&mmtimer_miscdev);
-out2:
-	free_irq(SGI_MMTIMER_VECTOR, NULL);
-out1:
-	return -1;
-}
-
-module_init(mmtimer_init);
diff --git a/drivers/char/pcmcia/synclink_cs.c b/drivers/char/pcmcia/synclink_cs.c
index d136db1a10f0..62be953e5fb0 100644
--- a/drivers/char/pcmcia/synclink_cs.c
+++ b/drivers/char/pcmcia/synclink_cs.c
@@ -4235,7 +4235,7 @@ static void hdlcdev_rx(MGSLPC_INFO *info, char *buf, int size)
 		return;
 	}
 
-	memcpy(skb_put(skb, size), buf, size);
+	skb_put_data(skb, buf, size);
 
 	skb->protocol = hdlc_type_trans(skb, dev);
 
diff --git a/drivers/char/random.c b/drivers/char/random.c
index e870f329db88..01a260f67437 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -803,13 +803,13 @@ static int crng_fast_load(const char *cp, size_t len)
 		p[crng_init_cnt % CHACHA20_KEY_SIZE] ^= *cp;
 		cp++; crng_init_cnt++; len--;
 	}
+	spin_unlock_irqrestore(&primary_crng.lock, flags);
 	if (crng_init_cnt >= CRNG_INIT_CNT_THRESH) {
 		invalidate_batched_entropy();
 		crng_init = 1;
 		wake_up_interruptible(&crng_init_wait);
 		pr_notice("random: fast init done\n");
 	}
-	spin_unlock_irqrestore(&primary_crng.lock, flags);
 	return 1;
 }
 
@@ -841,6 +841,7 @@ static void crng_reseed(struct crng_state *crng, struct entropy_store *r)
 	}
 	memzero_explicit(&buf, sizeof(buf));
 	crng->init_time = jiffies;
+	spin_unlock_irqrestore(&primary_crng.lock, flags);
 	if (crng == &primary_crng && crng_init < 2) {
 		invalidate_batched_entropy();
 		crng_init = 2;
@@ -848,7 +849,6 @@ static void crng_reseed(struct crng_state *crng, struct entropy_store *r)
 		wake_up_interruptible(&crng_init_wait);
 		pr_notice("random: crng init done\n");
 	}
-	spin_unlock_irqrestore(&primary_crng.lock, flags);
 }
 
 static inline void crng_wait_ready(void)
@@ -2041,8 +2041,8 @@ static DEFINE_PER_CPU(struct batched_entropy, batched_entropy_u64);
 u64 get_random_u64(void)
 {
 	u64 ret;
-	bool use_lock = crng_init < 2;
-	unsigned long flags;
+	bool use_lock = READ_ONCE(crng_init) < 2;
+	unsigned long flags = 0;
 	struct batched_entropy *batch;
 
 #if BITS_PER_LONG == 64
@@ -2073,8 +2073,8 @@ static DEFINE_PER_CPU(struct batched_entropy, batched_entropy_u32);
 u32 get_random_u32(void)
 {
 	u32 ret;
-	bool use_lock = crng_init < 2;
-	unsigned long flags;
+	bool use_lock = READ_ONCE(crng_init) < 2;
+	unsigned long flags = 0;
 	struct batched_entropy *batch;
 
 	if (arch_get_random_int(&ret))
diff --git a/drivers/char/tpm/st33zp24/i2c.c b/drivers/char/tpm/st33zp24/i2c.c
index 1b10e38f214e..be5d1abd3e8e 100644
--- a/drivers/char/tpm/st33zp24/i2c.c
+++ b/drivers/char/tpm/st33zp24/i2c.c
@@ -127,7 +127,7 @@ static int st33zp24_i2c_acpi_request_resources(struct i2c_client *client)
 	struct device *dev = &client->dev;
 	int ret;
 
-	ret = acpi_dev_add_driver_gpios(ACPI_COMPANION(dev), acpi_st33zp24_gpios);
+	ret = devm_acpi_dev_add_driver_gpios(dev, acpi_st33zp24_gpios);
 	if (ret)
 		return ret;
 
@@ -285,7 +285,6 @@ static int st33zp24_i2c_remove(struct i2c_client *client)
 	if (ret)
 		return ret;
 
-	acpi_dev_remove_driver_gpios(ACPI_COMPANION(&client->dev));
 	return 0;
 }
 
diff --git a/drivers/char/tpm/st33zp24/spi.c b/drivers/char/tpm/st33zp24/spi.c
index c69d15198f84..0fc4f20b5f83 100644
--- a/drivers/char/tpm/st33zp24/spi.c
+++ b/drivers/char/tpm/st33zp24/spi.c
@@ -246,7 +246,7 @@ static int st33zp24_spi_acpi_request_resources(struct spi_device *spi_dev)
 	struct device *dev = &spi_dev->dev;
 	int ret;
 
-	ret = acpi_dev_add_driver_gpios(ACPI_COMPANION(dev), acpi_st33zp24_gpios);
+	ret = devm_acpi_dev_add_driver_gpios(dev, acpi_st33zp24_gpios);
 	if (ret)
 		return ret;
 
@@ -402,7 +402,6 @@ static int st33zp24_spi_remove(struct spi_device *dev)
 	if (ret)
 		return ret;
 
-	acpi_dev_remove_driver_gpios(ACPI_COMPANION(&dev->dev));
 	return 0;
 }
 
diff --git a/drivers/char/tpm/tpm-interface.c b/drivers/char/tpm/tpm-interface.c
index 158c1db83f05..d2b4df6d9894 100644
--- a/drivers/char/tpm/tpm-interface.c
+++ b/drivers/char/tpm/tpm-interface.c
@@ -416,7 +416,8 @@ ssize_t tpm_transmit(struct tpm_chip *chip, struct tpm_space *space,
 	/* Store the decision as chip->locality will be changed. */
 	need_locality = chip->locality == -1;
 
-	if (need_locality && chip->ops->request_locality)  {
+	if (!(flags & TPM_TRANSMIT_RAW) &&
+	    need_locality && chip->ops->request_locality)  {
 		rc = chip->ops->request_locality(chip, 0);
 		if (rc < 0)
 			goto out_no_locality;
@@ -429,8 +430,9 @@ ssize_t tpm_transmit(struct tpm_chip *chip, struct tpm_space *space,
 
 	rc = chip->ops->send(chip, (u8 *) buf, count);
 	if (rc < 0) {
-		dev_err(&chip->dev,
-			"tpm_transmit: tpm_send: error %d\n", rc);
+		if (rc != -EPIPE)
+			dev_err(&chip->dev,
+				"%s: tpm_send: error %d\n", __func__, rc);
 		goto out;
 	}
 
@@ -536,59 +538,62 @@ ssize_t tpm_transmit_cmd(struct tpm_chip *chip, struct tpm_space *space,
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(tpm_transmit_cmd);
 
 #define TPM_DIGEST_SIZE 20
 #define TPM_RET_CODE_IDX 6
 #define TPM_INTERNAL_RESULT_SIZE 200
-#define TPM_ORD_GET_CAP cpu_to_be32(101)
-#define TPM_ORD_GET_RANDOM cpu_to_be32(70)
+#define TPM_ORD_GET_CAP 101
+#define TPM_ORD_GET_RANDOM 70
 
 static const struct tpm_input_header tpm_getcap_header = {
-	.tag = TPM_TAG_RQU_COMMAND,
+	.tag = cpu_to_be16(TPM_TAG_RQU_COMMAND),
 	.length = cpu_to_be32(22),
-	.ordinal = TPM_ORD_GET_CAP
+	.ordinal = cpu_to_be32(TPM_ORD_GET_CAP)
 };
 
 ssize_t tpm_getcap(struct tpm_chip *chip, u32 subcap_id, cap_t *cap,
 		   const char *desc, size_t min_cap_length)
 {
-	struct tpm_cmd_t tpm_cmd;
+	struct tpm_buf buf;
 	int rc;
 
-	tpm_cmd.header.in = tpm_getcap_header;
+	rc = tpm_buf_init(&buf, TPM_TAG_RQU_COMMAND, TPM_ORD_GET_CAP);
+	if (rc)
+		return rc;
+
 	if (subcap_id == TPM_CAP_VERSION_1_1 ||
 	    subcap_id == TPM_CAP_VERSION_1_2) {
-		tpm_cmd.params.getcap_in.cap = cpu_to_be32(subcap_id);
-		/*subcap field not necessary */
-		tpm_cmd.params.getcap_in.subcap_size = cpu_to_be32(0);
-		tpm_cmd.header.in.length -= cpu_to_be32(sizeof(__be32));
+		tpm_buf_append_u32(&buf, subcap_id);
+		tpm_buf_append_u32(&buf, 0);
 	} else {
 		if (subcap_id == TPM_CAP_FLAG_PERM ||
 		    subcap_id == TPM_CAP_FLAG_VOL)
-			tpm_cmd.params.getcap_in.cap =
-				cpu_to_be32(TPM_CAP_FLAG);
+			tpm_buf_append_u32(&buf, TPM_CAP_FLAG);
 		else
-			tpm_cmd.params.getcap_in.cap =
-				cpu_to_be32(TPM_CAP_PROP);
-		tpm_cmd.params.getcap_in.subcap_size = cpu_to_be32(4);
-		tpm_cmd.params.getcap_in.subcap = cpu_to_be32(subcap_id);
+			tpm_buf_append_u32(&buf, TPM_CAP_PROP);
+
+		tpm_buf_append_u32(&buf, 4);
+		tpm_buf_append_u32(&buf, subcap_id);
 	}
-	rc = tpm_transmit_cmd(chip, NULL, &tpm_cmd, TPM_INTERNAL_RESULT_SIZE,
+	rc = tpm_transmit_cmd(chip, NULL, buf.data, PAGE_SIZE,
 			      min_cap_length, 0, desc);
 	if (!rc)
-		*cap = tpm_cmd.params.getcap_out.cap;
+		*cap = *(cap_t *)&buf.data[TPM_HEADER_SIZE + 4];
+
+	tpm_buf_destroy(&buf);
 	return rc;
 }
 EXPORT_SYMBOL_GPL(tpm_getcap);
 
-#define TPM_ORD_STARTUP cpu_to_be32(153)
+#define TPM_ORD_STARTUP 153
 #define TPM_ST_CLEAR cpu_to_be16(1)
 #define TPM_ST_STATE cpu_to_be16(2)
 #define TPM_ST_DEACTIVATED cpu_to_be16(3)
 static const struct tpm_input_header tpm_startup_header = {
-	.tag = TPM_TAG_RQU_COMMAND,
+	.tag = cpu_to_be16(TPM_TAG_RQU_COMMAND),
 	.length = cpu_to_be32(12),
-	.ordinal = TPM_ORD_STARTUP
+	.ordinal = cpu_to_be32(TPM_ORD_STARTUP)
 };
 
 static int tpm_startup(struct tpm_chip *chip, __be16 startup_type)
@@ -737,7 +742,7 @@ EXPORT_SYMBOL_GPL(tpm_get_timeouts);
 #define CONTINUE_SELFTEST_RESULT_SIZE 10
 
 static const struct tpm_input_header continue_selftest_header = {
-	.tag = TPM_TAG_RQU_COMMAND,
+	.tag = cpu_to_be16(TPM_TAG_RQU_COMMAND),
 	.length = cpu_to_be32(10),
 	.ordinal = cpu_to_be32(TPM_ORD_CONTINUE_SELFTEST),
 };
@@ -760,13 +765,13 @@ static int tpm_continue_selftest(struct tpm_chip *chip)
 	return rc;
 }
 
-#define TPM_ORDINAL_PCRREAD cpu_to_be32(21)
+#define TPM_ORDINAL_PCRREAD 21
 #define READ_PCR_RESULT_SIZE 30
 #define READ_PCR_RESULT_BODY_SIZE 20
 static const struct tpm_input_header pcrread_header = {
-	.tag = TPM_TAG_RQU_COMMAND,
+	.tag = cpu_to_be16(TPM_TAG_RQU_COMMAND),
 	.length = cpu_to_be32(14),
-	.ordinal = TPM_ORDINAL_PCRREAD
+	.ordinal = cpu_to_be32(TPM_ORDINAL_PCRREAD)
 };
 
 int tpm_pcr_read_dev(struct tpm_chip *chip, int pcr_idx, u8 *res_buf)
@@ -838,15 +843,34 @@ int tpm_pcr_read(u32 chip_num, int pcr_idx, u8 *res_buf)
 }
 EXPORT_SYMBOL_GPL(tpm_pcr_read);
 
-#define TPM_ORD_PCR_EXTEND cpu_to_be32(20)
+#define TPM_ORD_PCR_EXTEND 20
 #define EXTEND_PCR_RESULT_SIZE 34
 #define EXTEND_PCR_RESULT_BODY_SIZE 20
 static const struct tpm_input_header pcrextend_header = {
-	.tag = TPM_TAG_RQU_COMMAND,
+	.tag = cpu_to_be16(TPM_TAG_RQU_COMMAND),
 	.length = cpu_to_be32(34),
-	.ordinal = TPM_ORD_PCR_EXTEND
+	.ordinal = cpu_to_be32(TPM_ORD_PCR_EXTEND)
 };
 
+static int tpm1_pcr_extend(struct tpm_chip *chip, int pcr_idx, const u8 *hash,
+			   char *log_msg)
+{
+	struct tpm_buf buf;
+	int rc;
+
+	rc = tpm_buf_init(&buf, TPM_TAG_RQU_COMMAND, TPM_ORD_PCR_EXTEND);
+	if (rc)
+		return rc;
+
+	tpm_buf_append_u32(&buf, pcr_idx);
+	tpm_buf_append(&buf, hash, TPM_DIGEST_SIZE);
+
+	rc = tpm_transmit_cmd(chip, NULL, buf.data, EXTEND_PCR_RESULT_SIZE,
+			      EXTEND_PCR_RESULT_BODY_SIZE, 0, log_msg);
+	tpm_buf_destroy(&buf);
+	return rc;
+}
+
 /**
  * tpm_pcr_extend - extend pcr value with hash
  * @chip_num:	tpm idx # or AN&
@@ -859,7 +883,6 @@ static const struct tpm_input_header pcrextend_header = {
  */
 int tpm_pcr_extend(u32 chip_num, int pcr_idx, const u8 *hash)
 {
-	struct tpm_cmd_t cmd;
 	int rc;
 	struct tpm_chip *chip;
 	struct tpm2_digest digest_list[ARRAY_SIZE(chip->active_banks)];
@@ -885,13 +908,8 @@ int tpm_pcr_extend(u32 chip_num, int pcr_idx, const u8 *hash)
 		return rc;
 	}
 
-	cmd.header.in = pcrextend_header;
-	cmd.params.pcrextend_in.pcr_idx = cpu_to_be32(pcr_idx);
-	memcpy(cmd.params.pcrextend_in.hash, hash, TPM_DIGEST_SIZE);
-	rc = tpm_transmit_cmd(chip, NULL, &cmd, EXTEND_PCR_RESULT_SIZE,
-			      EXTEND_PCR_RESULT_BODY_SIZE, 0,
-			      "attempting extend a PCR value");
-
+	rc = tpm1_pcr_extend(chip, pcr_idx, hash,
+			     "attempting extend a PCR value");
 	tpm_put_ops(chip);
 	return rc;
 }
@@ -1060,13 +1078,13 @@ again:
 }
 EXPORT_SYMBOL_GPL(wait_for_tpm_stat);
 
-#define TPM_ORD_SAVESTATE cpu_to_be32(152)
+#define TPM_ORD_SAVESTATE 152
 #define SAVESTATE_RESULT_SIZE 10
 
 static const struct tpm_input_header savestate_header = {
-	.tag = TPM_TAG_RQU_COMMAND,
+	.tag = cpu_to_be16(TPM_TAG_RQU_COMMAND),
 	.length = cpu_to_be32(10),
-	.ordinal = TPM_ORD_SAVESTATE
+	.ordinal = cpu_to_be32(TPM_ORD_SAVESTATE)
 };
 
 /*
@@ -1090,15 +1108,9 @@ int tpm_pm_suspend(struct device *dev)
 	}
 
 	/* for buggy tpm, flush pcrs with extend to selected dummy */
-	if (tpm_suspend_pcr) {
-		cmd.header.in = pcrextend_header;
-		cmd.params.pcrextend_in.pcr_idx = cpu_to_be32(tpm_suspend_pcr);
-		memcpy(cmd.params.pcrextend_in.hash, dummy_hash,
-		       TPM_DIGEST_SIZE);
-		rc = tpm_transmit_cmd(chip, NULL, &cmd, EXTEND_PCR_RESULT_SIZE,
-				      EXTEND_PCR_RESULT_BODY_SIZE, 0,
-				      "extending dummy pcr before suspend");
-	}
+	if (tpm_suspend_pcr)
+		rc = tpm1_pcr_extend(chip, tpm_suspend_pcr, dummy_hash,
+				     "extending dummy pcr before suspend");
 
 	/* now do the actual savestate */
 	for (try = 0; try < TPM_RETRY; try++) {
@@ -1149,9 +1161,9 @@ EXPORT_SYMBOL_GPL(tpm_pm_resume);
 
 #define TPM_GETRANDOM_RESULT_SIZE	18
 static const struct tpm_input_header tpm_getrandom_header = {
-	.tag = TPM_TAG_RQU_COMMAND,
+	.tag = cpu_to_be16(TPM_TAG_RQU_COMMAND),
 	.length = cpu_to_be32(14),
-	.ordinal = TPM_ORD_GET_RANDOM
+	.ordinal = cpu_to_be32(TPM_ORD_GET_RANDOM)
 };
 
 /**
diff --git a/drivers/char/tpm/tpm-sysfs.c b/drivers/char/tpm/tpm-sysfs.c
index 55405dbe43fa..4bd0997cfa2d 100644
--- a/drivers/char/tpm/tpm-sysfs.c
+++ b/drivers/char/tpm/tpm-sysfs.c
@@ -22,11 +22,11 @@
 
 #define READ_PUBEK_RESULT_SIZE 314
 #define READ_PUBEK_RESULT_MIN_BODY_SIZE (28 + 256)
-#define TPM_ORD_READPUBEK cpu_to_be32(124)
+#define TPM_ORD_READPUBEK 124
 static const struct tpm_input_header tpm_readpubek_header = {
-	.tag = TPM_TAG_RQU_COMMAND,
+	.tag = cpu_to_be16(TPM_TAG_RQU_COMMAND),
 	.length = cpu_to_be32(30),
-	.ordinal = TPM_ORD_READPUBEK
+	.ordinal = cpu_to_be32(TPM_ORD_READPUBEK)
 };
 static ssize_t pubek_show(struct device *dev, struct device_attribute *attr,
 			  char *buf)
diff --git a/drivers/char/tpm/tpm.h b/drivers/char/tpm/tpm.h
index 4b4c8dee3096..1df0521138d3 100644
--- a/drivers/char/tpm/tpm.h
+++ b/drivers/char/tpm/tpm.h
@@ -247,7 +247,7 @@ struct tpm_output_header {
 	__be32	return_code;
 } __packed;
 
-#define TPM_TAG_RQU_COMMAND cpu_to_be16(193)
+#define TPM_TAG_RQU_COMMAND 193
 
 struct	stclear_flags_t {
 	__be16	tag;
@@ -339,17 +339,6 @@ enum tpm_sub_capabilities {
 	TPM_CAP_PROP_TIS_DURATION = 0x120,
 };
 
-struct	tpm_getcap_params_in {
-	__be32	cap;
-	__be32	subcap_size;
-	__be32	subcap;
-} __packed;
-
-struct	tpm_getcap_params_out {
-	__be32	cap_size;
-	cap_t	cap;
-} __packed;
-
 struct	tpm_readpubek_params_out {
 	u8	algorithm[4];
 	u8	encscheme[2];
@@ -374,11 +363,6 @@ struct tpm_pcrread_in {
 	__be32	pcr_idx;
 } __packed;
 
-struct tpm_pcrextend_in {
-	__be32	pcr_idx;
-	u8	hash[TPM_DIGEST_SIZE];
-} __packed;
-
 /* 128 bytes is an arbitrary cap. This could be as large as TPM_BUFSIZE - 18
  * bytes, but 128 is still a relatively large number of random bytes and
  * anything much bigger causes users of struct tpm_cmd_t to start getting
@@ -399,13 +383,10 @@ struct tpm_startup_in {
 } __packed;
 
 typedef union {
-	struct	tpm_getcap_params_out getcap_out;
 	struct	tpm_readpubek_params_out readpubek_out;
 	u8	readpubek_out_buffer[sizeof(struct tpm_readpubek_params_out)];
-	struct	tpm_getcap_params_in getcap_in;
 	struct	tpm_pcrread_in	pcrread_in;
 	struct	tpm_pcrread_out	pcrread_out;
-	struct	tpm_pcrextend_in pcrextend_in;
 	struct	tpm_getrandom_in getrandom_in;
 	struct	tpm_getrandom_out getrandom_out;
 	struct tpm_startup_in startup_in;
@@ -525,6 +506,7 @@ extern struct idr dev_nums_idr;
 
 enum tpm_transmit_flags {
 	TPM_TRANSMIT_UNLOCKED	= BIT(0),
+	TPM_TRANSMIT_RAW	= BIT(1),
 };
 
 ssize_t tpm_transmit(struct tpm_chip *chip, struct tpm_space *space,
diff --git a/drivers/char/tpm/tpm2-cmd.c b/drivers/char/tpm/tpm2-cmd.c
index 3ee6883f26c1..3a9964326279 100644
--- a/drivers/char/tpm/tpm2-cmd.c
+++ b/drivers/char/tpm/tpm2-cmd.c
@@ -840,7 +840,7 @@ void tpm2_shutdown(struct tpm_chip *chip, u16 shutdown_type)
 	/* In places where shutdown command is sent there's no much we can do
 	 * except print the error code on a system failure.
 	 */
-	if (rc < 0)
+	if (rc < 0 && rc != -EPIPE)
 		dev_warn(&chip->dev, "transmit returned %d while stopping the TPM",
 			 rc);
 }
diff --git a/drivers/char/tpm/tpm_atmel.c b/drivers/char/tpm/tpm_atmel.c
index 0d322ab11faa..66a14526aaf4 100644
--- a/drivers/char/tpm/tpm_atmel.c
+++ b/drivers/char/tpm/tpm_atmel.c
@@ -144,13 +144,11 @@ static void atml_plat_remove(void)
 	struct tpm_chip *chip = dev_get_drvdata(&pdev->dev);
 	struct tpm_atmel_priv *priv = dev_get_drvdata(&chip->dev);
 
-	if (chip) {
-		tpm_chip_unregister(chip);
-		if (priv->have_region)
-			atmel_release_region(priv->base, priv->region_size);
-		atmel_put_base_addr(priv->iobase);
-		platform_device_unregister(pdev);
-	}
+	tpm_chip_unregister(chip);
+	if (priv->have_region)
+		atmel_release_region(priv->base, priv->region_size);
+	atmel_put_base_addr(priv->iobase);
+	platform_device_unregister(pdev);
 }
 
 static SIMPLE_DEV_PM_OPS(tpm_atml_pm, tpm_pm_suspend, tpm_pm_resume);
diff --git a/drivers/char/tpm/tpm_crb.c b/drivers/char/tpm/tpm_crb.c
index b917b9d5f710..fe42c4a0d8d1 100644
--- a/drivers/char/tpm/tpm_crb.c
+++ b/drivers/char/tpm/tpm_crb.c
@@ -27,10 +27,9 @@
 
 #define ACPI_SIG_TPM2 "TPM2"
 
-static const u8 CRB_ACPI_START_UUID[] = {
-	/* 0000 */ 0xAB, 0x6C, 0xBF, 0x6B, 0x63, 0x54, 0x14, 0x47,
-	/* 0008 */ 0xB7, 0xCD, 0xF0, 0x20, 0x3C, 0x03, 0x68, 0xD4
-};
+static const guid_t crb_acpi_start_guid =
+	GUID_INIT(0x6BBF6CAB, 0x5463, 0x4714,
+		  0xB7, 0xCD, 0xF0, 0x20, 0x3C, 0x03, 0x68, 0xD4);
 
 enum crb_defaults {
 	CRB_ACPI_START_REVISION_ID = 1,
@@ -266,7 +265,7 @@ static int crb_do_acpi_start(struct tpm_chip *chip)
 	int rc;
 
 	obj = acpi_evaluate_dsm(chip->acpi_dev_handle,
-				CRB_ACPI_START_UUID,
+				&crb_acpi_start_guid,
 				CRB_ACPI_START_REVISION_ID,
 				CRB_ACPI_START_INDEX,
 				NULL);
@@ -564,12 +563,12 @@ static int crb_acpi_add(struct acpi_device *device)
 	    sm == ACPI_TPM2_COMMAND_BUFFER_WITH_START_METHOD)
 		priv->flags |= CRB_FL_ACPI_START;
 
-	if (sm == ACPI_TPM2_COMMAND_BUFFER_WITH_SMC) {
+	if (sm == ACPI_TPM2_COMMAND_BUFFER_WITH_ARM_SMC) {
 		if (buf->header.length < (sizeof(*buf) + sizeof(*crb_smc))) {
 			dev_err(dev,
 				FW_BUG "TPM2 ACPI table has wrong size %u for start method type %d\n",
 				buf->header.length,
-				ACPI_TPM2_COMMAND_BUFFER_WITH_SMC);
+				ACPI_TPM2_COMMAND_BUFFER_WITH_ARM_SMC);
 			return -EINVAL;
 		}
 		crb_smc = ACPI_ADD_PTR(struct tpm2_crb_smc, buf, sizeof(*buf));
diff --git a/drivers/char/tpm/tpm_i2c_infineon.c b/drivers/char/tpm/tpm_i2c_infineon.c
index dc47fa222a26..79d6bbb58e39 100644
--- a/drivers/char/tpm/tpm_i2c_infineon.c
+++ b/drivers/char/tpm/tpm_i2c_infineon.c
@@ -70,6 +70,7 @@ struct tpm_inf_dev {
 	u8 buf[TPM_BUFSIZE + sizeof(u8)]; /* max. buffer size + addr */
 	struct tpm_chip *chip;
 	enum i2c_chip_type chip_type;
+	unsigned int adapterlimit;
 };
 
 static struct tpm_inf_dev tpm_dev;
@@ -111,6 +112,7 @@ static int iic_tpm_read(u8 addr, u8 *buffer, size_t len)
 
 	int rc = 0;
 	int count;
+	unsigned int msglen = len;
 
 	/* Lock the adapter for the duration of the whole sequence. */
 	if (!tpm_dev.client->adapter->algo->master_xfer)
@@ -131,27 +133,61 @@ static int iic_tpm_read(u8 addr, u8 *buffer, size_t len)
 			usleep_range(SLEEP_DURATION_LOW, SLEEP_DURATION_HI);
 		}
 	} else {
-		/* slb9635 protocol should work in all cases */
-		for (count = 0; count < MAX_COUNT; count++) {
-			rc = __i2c_transfer(tpm_dev.client->adapter, &msg1, 1);
-			if (rc > 0)
-				break;	/* break here to skip sleep */
-
-			usleep_range(SLEEP_DURATION_LOW, SLEEP_DURATION_HI);
-		}
-
-		if (rc <= 0)
-			goto out;
-
-		/* After the TPM has successfully received the register address
-		 * it needs some time, thus we're sleeping here again, before
-		 * retrieving the data
+		/* Expect to send one command message and one data message, but
+		 * support looping over each or both if necessary.
 		 */
-		for (count = 0; count < MAX_COUNT; count++) {
-			usleep_range(SLEEP_DURATION_LOW, SLEEP_DURATION_HI);
-			rc = __i2c_transfer(tpm_dev.client->adapter, &msg2, 1);
-			if (rc > 0)
-				break;
+		while (len > 0) {
+			/* slb9635 protocol should work in all cases */
+			for (count = 0; count < MAX_COUNT; count++) {
+				rc = __i2c_transfer(tpm_dev.client->adapter,
+						    &msg1, 1);
+				if (rc > 0)
+					break;	/* break here to skip sleep */
+
+				usleep_range(SLEEP_DURATION_LOW,
+					     SLEEP_DURATION_HI);
+			}
+
+			if (rc <= 0)
+				goto out;
+
+			/* After the TPM has successfully received the register
+			 * address it needs some time, thus we're sleeping here
+			 * again, before retrieving the data
+			 */
+			for (count = 0; count < MAX_COUNT; count++) {
+				if (tpm_dev.adapterlimit) {
+					msglen = min_t(unsigned int,
+						       tpm_dev.adapterlimit,
+						       len);
+					msg2.len = msglen;
+				}
+				usleep_range(SLEEP_DURATION_LOW,
+					     SLEEP_DURATION_HI);
+				rc = __i2c_transfer(tpm_dev.client->adapter,
+						    &msg2, 1);
+				if (rc > 0) {
+					/* Since len is unsigned, make doubly
+					 * sure we do not underflow it.
+					 */
+					if (msglen > len)
+						len = 0;
+					else
+						len -= msglen;
+					msg2.buf += msglen;
+					break;
+				}
+				/* If the I2C adapter rejected the request (e.g
+				 * when the quirk read_max_len < len) fall back
+				 * to a sane minimum value and try again.
+				 */
+				if (rc == -EOPNOTSUPP)
+					tpm_dev.adapterlimit =
+							I2C_SMBUS_BLOCK_MAX;
+			}
+
+			if (rc <= 0)
+				goto out;
 		}
 	}
 
diff --git a/drivers/char/tpm/tpm_infineon.c b/drivers/char/tpm/tpm_infineon.c
index e3cf9f3545c5..3b1b9f9322d5 100644
--- a/drivers/char/tpm/tpm_infineon.c
+++ b/drivers/char/tpm/tpm_infineon.c
@@ -397,7 +397,7 @@ static int tpm_inf_pnp_probe(struct pnp_dev *dev,
 	int vendorid[2];
 	int version[2];
 	int productid[2];
-	char chipname[20];
+	const char *chipname;
 	struct tpm_chip *chip;
 
 	/* read IO-ports through PnP */
@@ -488,13 +488,13 @@ static int tpm_inf_pnp_probe(struct pnp_dev *dev,
 
 	switch ((productid[0] << 8) | productid[1]) {
 	case 6:
-		snprintf(chipname, sizeof(chipname), " (SLD 9630 TT 1.1)");
+		chipname = " (SLD 9630 TT 1.1)";
 		break;
 	case 11:
-		snprintf(chipname, sizeof(chipname), " (SLB 9635 TT 1.2)");
+		chipname = " (SLB 9635 TT 1.2)";
 		break;
 	default:
-		snprintf(chipname, sizeof(chipname), " (unknown chip)");
+		chipname = " (unknown chip)";
 		break;
 	}
 
diff --git a/drivers/char/tpm/tpm_ppi.c b/drivers/char/tpm/tpm_ppi.c
index 692a2c6ae036..86dd8521feef 100644
--- a/drivers/char/tpm/tpm_ppi.c
+++ b/drivers/char/tpm/tpm_ppi.c
@@ -32,20 +32,16 @@
 #define PPI_VS_REQ_START	128
 #define PPI_VS_REQ_END		255
 
-static const u8 tpm_ppi_uuid[] = {
-	0xA6, 0xFA, 0xDD, 0x3D,
-	0x1B, 0x36,
-	0xB4, 0x4E,
-	0xA4, 0x24,
-	0x8D, 0x10, 0x08, 0x9D, 0x16, 0x53
-};
+static const guid_t tpm_ppi_guid =
+	GUID_INIT(0x3DDDFAA6, 0x361B, 0x4EB4,
+		  0xA4, 0x24, 0x8D, 0x10, 0x08, 0x9D, 0x16, 0x53);
 
 static inline union acpi_object *
 tpm_eval_dsm(acpi_handle ppi_handle, int func, acpi_object_type type,
 	     union acpi_object *argv4)
 {
 	BUG_ON(!ppi_handle);
-	return acpi_evaluate_dsm_typed(ppi_handle, tpm_ppi_uuid,
+	return acpi_evaluate_dsm_typed(ppi_handle, &tpm_ppi_guid,
 				       TPM_PPI_REVISION_ID,
 				       func, argv4, type);
 }
@@ -107,7 +103,7 @@ static ssize_t tpm_store_ppi_request(struct device *dev,
 	 * is updated with function index from SUBREQ to SUBREQ2 since PPI
 	 * version 1.1
 	 */
-	if (acpi_check_dsm(chip->acpi_dev_handle, tpm_ppi_uuid,
+	if (acpi_check_dsm(chip->acpi_dev_handle, &tpm_ppi_guid,
 			   TPM_PPI_REVISION_ID, 1 << TPM_PPI_FN_SUBREQ2))
 		func = TPM_PPI_FN_SUBREQ2;
 
@@ -268,7 +264,7 @@ static ssize_t show_ppi_operations(acpi_handle dev_handle, char *buf, u32 start,
 		"User not required",
 	};
 
-	if (!acpi_check_dsm(dev_handle, tpm_ppi_uuid, TPM_PPI_REVISION_ID,
+	if (!acpi_check_dsm(dev_handle, &tpm_ppi_guid, TPM_PPI_REVISION_ID,
 			    1 << TPM_PPI_FN_GETOPR))
 		return -EPERM;
 
@@ -341,12 +337,12 @@ void tpm_add_ppi(struct tpm_chip *chip)
 	if (!chip->acpi_dev_handle)
 		return;
 
-	if (!acpi_check_dsm(chip->acpi_dev_handle, tpm_ppi_uuid,
+	if (!acpi_check_dsm(chip->acpi_dev_handle, &tpm_ppi_guid,
 			    TPM_PPI_REVISION_ID, 1 << TPM_PPI_FN_VERSION))
 		return;
 
 	/* Cache PPI version string. */
-	obj = acpi_evaluate_dsm_typed(chip->acpi_dev_handle, tpm_ppi_uuid,
+	obj = acpi_evaluate_dsm_typed(chip->acpi_dev_handle, &tpm_ppi_guid,
 				      TPM_PPI_REVISION_ID, TPM_PPI_FN_VERSION,
 				      NULL, ACPI_TYPE_STRING);
 	if (obj) {
diff --git a/drivers/char/tpm/tpm_tis.c b/drivers/char/tpm/tpm_tis.c
index c7e1384f1b08..b14d4aa97af8 100644
--- a/drivers/char/tpm/tpm_tis.c
+++ b/drivers/char/tpm/tpm_tis.c
@@ -80,6 +80,8 @@ static int has_hid(struct acpi_device *dev, const char *hid)
 
 static inline int is_itpm(struct acpi_device *dev)
 {
+	if (!dev)
+		return 0;
 	return has_hid(dev, "INTC0102");
 }
 #else
@@ -89,6 +91,47 @@ static inline int is_itpm(struct acpi_device *dev)
 }
 #endif
 
+#if defined(CONFIG_ACPI)
+#define DEVICE_IS_TPM2 1
+
+static const struct acpi_device_id tpm_acpi_tbl[] = {
+	{"MSFT0101", DEVICE_IS_TPM2},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, tpm_acpi_tbl);
+
+static int check_acpi_tpm2(struct device *dev)
+{
+	const struct acpi_device_id *aid = acpi_match_device(tpm_acpi_tbl, dev);
+	struct acpi_table_tpm2 *tbl;
+	acpi_status st;
+
+	if (!aid || aid->driver_data != DEVICE_IS_TPM2)
+		return 0;
+
+	/* If the ACPI TPM2 signature is matched then a global ACPI_SIG_TPM2
+	 * table is mandatory
+	 */
+	st =
+	    acpi_get_table(ACPI_SIG_TPM2, 1, (struct acpi_table_header **)&tbl);
+	if (ACPI_FAILURE(st) || tbl->header.length < sizeof(*tbl)) {
+		dev_err(dev, FW_BUG "failed to get TPM2 ACPI table\n");
+		return -EINVAL;
+	}
+
+	/* The tpm2_crb driver handles this device */
+	if (tbl->start_method != ACPI_TPM2_MEMORY_MAPPED)
+		return -ENODEV;
+
+	return 0;
+}
+#else
+static int check_acpi_tpm2(struct device *dev)
+{
+	return 0;
+}
+#endif
+
 static int tpm_tcg_read_bytes(struct tpm_tis_data *data, u32 addr, u16 len,
 			      u8 *result)
 {
@@ -141,11 +184,15 @@ static const struct tpm_tis_phy_ops tpm_tcg = {
 	.write32 = tpm_tcg_write32,
 };
 
-static int tpm_tis_init(struct device *dev, struct tpm_info *tpm_info,
-			acpi_handle acpi_dev_handle)
+static int tpm_tis_init(struct device *dev, struct tpm_info *tpm_info)
 {
 	struct tpm_tis_tcg_phy *phy;
 	int irq = -1;
+	int rc;
+
+	rc = check_acpi_tpm2(dev);
+	if (rc)
+		return rc;
 
 	phy = devm_kzalloc(dev, sizeof(struct tpm_tis_tcg_phy), GFP_KERNEL);
 	if (phy == NULL)
@@ -158,11 +205,11 @@ static int tpm_tis_init(struct device *dev, struct tpm_info *tpm_info,
 	if (interrupts)
 		irq = tpm_info->irq;
 
-	if (itpm)
+	if (itpm || is_itpm(ACPI_COMPANION(dev)))
 		phy->priv.flags |= TPM_TIS_ITPM_WORKAROUND;
 
 	return tpm_tis_core_init(dev, &phy->priv, irq, &tpm_tcg,
-				 acpi_dev_handle);
+				 ACPI_HANDLE(dev));
 }
 
 static SIMPLE_DEV_PM_OPS(tpm_tis_pm, tpm_pm_suspend, tpm_tis_resume);
@@ -171,7 +218,6 @@ static int tpm_tis_pnp_init(struct pnp_dev *pnp_dev,
 			    const struct pnp_device_id *pnp_id)
 {
 	struct tpm_info tpm_info = {};
-	acpi_handle acpi_dev_handle = NULL;
 	struct resource *res;
 
 	res = pnp_get_resource(pnp_dev, IORESOURCE_MEM, 0);
@@ -184,14 +230,7 @@ static int tpm_tis_pnp_init(struct pnp_dev *pnp_dev,
 	else
 		tpm_info.irq = -1;
 
-	if (pnp_acpi_device(pnp_dev)) {
-		if (is_itpm(pnp_acpi_device(pnp_dev)))
-			itpm = true;
-
-		acpi_dev_handle = ACPI_HANDLE(&pnp_dev->dev);
-	}
-
-	return tpm_tis_init(&pnp_dev->dev, &tpm_info, acpi_dev_handle);
+	return tpm_tis_init(&pnp_dev->dev, &tpm_info);
 }
 
 static struct pnp_device_id tpm_pnp_tbl[] = {
@@ -231,93 +270,6 @@ 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");
 
-#ifdef CONFIG_ACPI
-static int tpm_check_resource(struct acpi_resource *ares, void *data)
-{
-	struct tpm_info *tpm_info = (struct tpm_info *) data;
-	struct resource res;
-
-	if (acpi_dev_resource_interrupt(ares, 0, &res))
-		tpm_info->irq = res.start;
-	else if (acpi_dev_resource_memory(ares, &res)) {
-		tpm_info->res = res;
-		tpm_info->res.name = NULL;
-	}
-
-	return 1;
-}
-
-static int tpm_tis_acpi_init(struct acpi_device *acpi_dev)
-{
-	struct acpi_table_tpm2 *tbl;
-	acpi_status st;
-	struct list_head resources;
-	struct tpm_info tpm_info = {};
-	int ret;
-
-	st = acpi_get_table(ACPI_SIG_TPM2, 1,
-			    (struct acpi_table_header **) &tbl);
-	if (ACPI_FAILURE(st) || tbl->header.length < sizeof(*tbl)) {
-		dev_err(&acpi_dev->dev,
-			FW_BUG "failed to get TPM2 ACPI table\n");
-		return -EINVAL;
-	}
-
-	if (tbl->start_method != ACPI_TPM2_MEMORY_MAPPED)
-		return -ENODEV;
-
-	INIT_LIST_HEAD(&resources);
-	tpm_info.irq = -1;
-	ret = acpi_dev_get_resources(acpi_dev, &resources, tpm_check_resource,
-				     &tpm_info);
-	if (ret < 0)
-		return ret;
-
-	acpi_dev_free_resource_list(&resources);
-
-	if (resource_type(&tpm_info.res) != IORESOURCE_MEM) {
-		dev_err(&acpi_dev->dev,
-			FW_BUG "TPM2 ACPI table does not define a memory resource\n");
-		return -EINVAL;
-	}
-
-	if (is_itpm(acpi_dev))
-		itpm = true;
-
-	return tpm_tis_init(&acpi_dev->dev, &tpm_info, acpi_dev->handle);
-}
-
-static int tpm_tis_acpi_remove(struct acpi_device *dev)
-{
-	struct tpm_chip *chip = dev_get_drvdata(&dev->dev);
-
-	tpm_chip_unregister(chip);
-	tpm_tis_remove(chip);
-
-	return 0;
-}
-
-static struct acpi_device_id tpm_acpi_tbl[] = {
-	{"MSFT0101", 0},	/* TPM 2.0 */
-	/* Add new here */
-	{"", 0},		/* User Specified */
-	{"", 0}			/* Terminator */
-};
-MODULE_DEVICE_TABLE(acpi, tpm_acpi_tbl);
-
-static struct acpi_driver tis_acpi_driver = {
-	.name = "tpm_tis",
-	.ids = tpm_acpi_tbl,
-	.ops = {
-		.add = tpm_tis_acpi_init,
-		.remove = tpm_tis_acpi_remove,
-	},
-	.drv = {
-		.pm = &tpm_tis_pm,
-	},
-};
-#endif
-
 static struct platform_device *force_pdev;
 
 static int tpm_tis_plat_probe(struct platform_device *pdev)
@@ -332,18 +284,16 @@ static int tpm_tis_plat_probe(struct platform_device *pdev)
 	}
 	tpm_info.res = *res;
 
-	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (res) {
-		tpm_info.irq = res->start;
-	} else {
-		if (pdev == force_pdev)
+	tpm_info.irq = platform_get_irq(pdev, 0);
+	if (tpm_info.irq <= 0) {
+		if (pdev != force_pdev)
 			tpm_info.irq = -1;
 		else
 			/* When forcing auto probe the IRQ */
 			tpm_info.irq = 0;
 	}
 
-	return tpm_tis_init(&pdev->dev, &tpm_info, NULL);
+	return tpm_tis_init(&pdev->dev, &tpm_info);
 }
 
 static int tpm_tis_plat_remove(struct platform_device *pdev)
@@ -371,6 +321,7 @@ static struct platform_driver tis_drv = {
 		.name		= "tpm_tis",
 		.pm		= &tpm_tis_pm,
 		.of_match_table = of_match_ptr(tis_of_platform_match),
+		.acpi_match_table = ACPI_PTR(tpm_acpi_tbl),
 	},
 };
 
@@ -413,11 +364,6 @@ static int __init init_tis(void)
 	if (rc)
 		goto err_platform;
 
-#ifdef CONFIG_ACPI
-	rc = acpi_bus_register_driver(&tis_acpi_driver);
-	if (rc)
-		goto err_acpi;
-#endif
 
 	if (IS_ENABLED(CONFIG_PNP)) {
 		rc = pnp_register_driver(&tis_pnp_driver);
@@ -428,10 +374,6 @@ static int __init init_tis(void)
 	return 0;
 
 err_pnp:
-#ifdef CONFIG_ACPI
-	acpi_bus_unregister_driver(&tis_acpi_driver);
-err_acpi:
-#endif
 	platform_driver_unregister(&tis_drv);
 err_platform:
 	if (force_pdev)
@@ -443,9 +385,6 @@ err_force:
 static void __exit cleanup_tis(void)
 {
 	pnp_unregister_driver(&tis_pnp_driver);
-#ifdef CONFIG_ACPI
-	acpi_bus_unregister_driver(&tis_acpi_driver);
-#endif
 	platform_driver_unregister(&tis_drv);
 
 	if (force_pdev)
diff --git a/drivers/char/tpm/tpm_vtpm_proxy.c b/drivers/char/tpm/tpm_vtpm_proxy.c
index 751059d2140a..1d877cc9af97 100644
--- a/drivers/char/tpm/tpm_vtpm_proxy.c
+++ b/drivers/char/tpm/tpm_vtpm_proxy.c
@@ -43,6 +43,7 @@ struct proxy_dev {
 #define STATE_OPENED_FLAG        BIT(0)
 #define STATE_WAIT_RESPONSE_FLAG BIT(1)  /* waiting for emulator response */
 #define STATE_REGISTERED_FLAG	 BIT(2)
+#define STATE_DRIVER_COMMAND     BIT(3)  /* sending a driver specific command */
 
 	size_t req_len;              /* length of queued TPM request */
 	size_t resp_len;             /* length of queued TPM response */
@@ -299,6 +300,28 @@ out:
 	return len;
 }
 
+static int vtpm_proxy_is_driver_command(struct tpm_chip *chip,
+					u8 *buf, size_t count)
+{
+	struct tpm_input_header *hdr = (struct tpm_input_header *)buf;
+
+	if (count < sizeof(struct tpm_input_header))
+		return 0;
+
+	if (chip->flags & TPM_CHIP_FLAG_TPM2) {
+		switch (be32_to_cpu(hdr->ordinal)) {
+		case TPM2_CC_SET_LOCALITY:
+			return 1;
+		}
+	} else {
+		switch (be32_to_cpu(hdr->ordinal)) {
+		case TPM_ORD_SET_LOCALITY:
+			return 1;
+		}
+	}
+	return 0;
+}
+
 /*
  * Called when core TPM driver forwards TPM requests to 'server side'.
  *
@@ -321,6 +344,10 @@ static int vtpm_proxy_tpm_op_send(struct tpm_chip *chip, u8 *buf, size_t count)
 		return -EIO;
 	}
 
+	if (!(proxy_dev->state & STATE_DRIVER_COMMAND) &&
+	    vtpm_proxy_is_driver_command(chip, buf, count))
+		return -EFAULT;
+
 	mutex_lock(&proxy_dev->buf_lock);
 
 	if (!(proxy_dev->state & STATE_OPENED_FLAG)) {
@@ -371,6 +398,47 @@ static bool vtpm_proxy_tpm_req_canceled(struct tpm_chip  *chip, u8 status)
 	return ret;
 }
 
+static int vtpm_proxy_request_locality(struct tpm_chip *chip, int locality)
+{
+	struct tpm_buf buf;
+	int rc;
+	const struct tpm_output_header *header;
+	struct proxy_dev *proxy_dev = dev_get_drvdata(&chip->dev);
+
+	if (chip->flags & TPM_CHIP_FLAG_TPM2)
+		rc = tpm_buf_init(&buf, TPM2_ST_SESSIONS,
+				  TPM2_CC_SET_LOCALITY);
+	else
+		rc = tpm_buf_init(&buf, TPM_TAG_RQU_COMMAND,
+				  TPM_ORD_SET_LOCALITY);
+	if (rc)
+		return rc;
+	tpm_buf_append_u8(&buf, locality);
+
+	proxy_dev->state |= STATE_DRIVER_COMMAND;
+
+	rc = tpm_transmit_cmd(chip, NULL, buf.data, tpm_buf_length(&buf), 0,
+			      TPM_TRANSMIT_UNLOCKED | TPM_TRANSMIT_RAW,
+			      "attempting to set locality");
+
+	proxy_dev->state &= ~STATE_DRIVER_COMMAND;
+
+	if (rc < 0) {
+		locality = rc;
+		goto out;
+	}
+
+	header = (const struct tpm_output_header *)buf.data;
+	rc = be32_to_cpu(header->return_code);
+	if (rc)
+		locality = -1;
+
+out:
+	tpm_buf_destroy(&buf);
+
+	return locality;
+}
+
 static const struct tpm_class_ops vtpm_proxy_tpm_ops = {
 	.flags = TPM_OPS_AUTO_STARTUP,
 	.recv = vtpm_proxy_tpm_op_recv,
@@ -380,6 +448,7 @@ static const struct tpm_class_ops vtpm_proxy_tpm_ops = {
 	.req_complete_mask = VTPM_PROXY_REQ_COMPLETE_FLAG,
 	.req_complete_val = VTPM_PROXY_REQ_COMPLETE_FLAG,
 	.req_canceled = vtpm_proxy_tpm_req_canceled,
+	.request_locality = vtpm_proxy_request_locality,
 };
 
 /*
diff --git a/drivers/char/tpm/tpmrm-dev.c b/drivers/char/tpm/tpmrm-dev.c
index c636e7fdd1f5..1a0e97a5da5a 100644
--- a/drivers/char/tpm/tpmrm-dev.c
+++ b/drivers/char/tpm/tpmrm-dev.c
@@ -45,7 +45,7 @@ static int tpmrm_release(struct inode *inode, struct file *file)
 	return 0;
 }
 
-ssize_t tpmrm_write(struct file *file, const char __user *buf,
+static ssize_t tpmrm_write(struct file *file, const char __user *buf,
 		   size_t size, loff_t *off)
 {
 	struct file_priv *fpriv = file->private_data;
diff --git a/drivers/clk/meson/Kconfig b/drivers/clk/meson/Kconfig
index 19480bcc7046..2f29ee1a4d00 100644
--- a/drivers/clk/meson/Kconfig
+++ b/drivers/clk/meson/Kconfig
@@ -14,6 +14,7 @@ config COMMON_CLK_MESON8B
 config COMMON_CLK_GXBB
 	bool
 	depends on COMMON_CLK_AMLOGIC
+	select RESET_CONTROLLER
 	help
 	  Support for the clock controller on AmLogic S905 devices, aka gxbb.
 	  Say Y if you want peripherals and CPU frequency scaling to work.
diff --git a/drivers/clk/meson/gxbb.h b/drivers/clk/meson/gxbb.h
index 93b8f07ee7af..4d04f4a3cd59 100644
--- a/drivers/clk/meson/gxbb.h
+++ b/drivers/clk/meson/gxbb.h
@@ -171,7 +171,7 @@
  * to be exposed to client nodes in DT: include/dt-bindings/clock/gxbb-clkc.h
  */
 #define CLKID_SYS_PLL		  0
-/* CLKID_CPUCLK */
+#define CLKID_CPUCLK		  1
 /* CLKID_HDMI_PLL */
 #define CLKID_FIXED_PLL		  3
 /* CLKID_FCLK_DIV2 */
@@ -191,12 +191,12 @@
 #define CLKID_ISA		  18
 #define CLKID_PL301		  19
 #define CLKID_PERIPHS		  20
-#define CLKID_SPICC		  21
+/* CLKID_SPICC */
 /* CLKID_I2C */
 /* #define CLKID_SAR_ADC */
 #define CLKID_SMART_CARD	  24
 /* CLKID_RNG0 */
-#define CLKID_UART0		  26
+/* CLKID_UART0 */
 #define CLKID_SDHC		  27
 #define CLKID_STREAM		  28
 #define CLKID_ASYNC_FIFO	  29
@@ -209,7 +209,7 @@
 /* CLKID_ETH */
 #define CLKID_DEMUX		  37
 /* CLKID_AIU_GLUE */
-#define CLKID_IEC958		  39
+/* CLKID_IEC958 */
 /* CLKID_I2S_OUT */
 #define CLKID_AMCLK		  41
 #define CLKID_AIFIFO2		  42
@@ -218,7 +218,7 @@
 #define CLKID_ADC		  45
 #define CLKID_BLKMV		  46
 /* CLKID_AIU */
-#define CLKID_UART1		  48
+/* CLKID_UART1 */
 #define CLKID_G2D		  49
 /* CLKID_USB0 */
 /* CLKID_USB1 */
@@ -238,7 +238,7 @@
 /* CLKID_USB0_DDR_BRIDGE */
 #define CLKID_MMC_PCLK		  66
 #define CLKID_DVIN		  67
-#define CLKID_UART2		  68
+/* CLKID_UART2 */
 /* #define CLKID_SANA */
 #define CLKID_VPU_INTR		  70
 #define CLKID_SEC_AHB_AHB3_BRIDGE 71
@@ -251,7 +251,7 @@
 #define CLKID_GCLK_VENCI_INT	  78
 #define CLKID_DAC_CLK		  79
 /* CLKID_AOCLK_GATE */
-#define CLKID_IEC958_GATE	  81
+/* CLKID_IEC958_GATE */
 #define CLKID_ENC480P		  82
 #define CLKID_RNG1		  83
 #define CLKID_GCLK_VENCI_INT1	  84
@@ -277,13 +277,13 @@
 #define CLKID_MALI_1_DIV	 104
 /* CLKID_MALI_1	*/
 /* CLKID_MALI	*/
-#define CLKID_CTS_AMCLK		  107
+/* CLKID_CTS_AMCLK */
 #define CLKID_CTS_AMCLK_SEL	  108
 #define CLKID_CTS_AMCLK_DIV	  109
-#define CLKID_CTS_MCLK_I958	  110
+/* CLKID_CTS_MCLK_I958 */
 #define CLKID_CTS_MCLK_I958_SEL	  111
 #define CLKID_CTS_MCLK_I958_DIV	  112
-#define CLKID_CTS_I958		  113
+/* CLKID_CTS_I958 */
 
 #define NR_CLKS			  114
 
diff --git a/drivers/clk/meson/meson8b.h b/drivers/clk/meson/meson8b.h
index 3881defc8644..a687e02547dc 100644
--- a/drivers/clk/meson/meson8b.h
+++ b/drivers/clk/meson/meson8b.h
@@ -87,20 +87,20 @@
 #define CLKID_PERIPHS		20
 #define CLKID_SPICC		21
 #define CLKID_I2C		22
-#define CLKID_SAR_ADC		23
+/* #define CLKID_SAR_ADC */
 #define CLKID_SMART_CARD	24
-#define CLKID_RNG0		25
+/* #define CLKID_RNG0 */
 #define CLKID_UART0		26
 #define CLKID_SDHC		27
 #define CLKID_STREAM		28
 #define CLKID_ASYNC_FIFO	29
-#define CLKID_SDIO		30
+/* #define CLKID_SDIO */
 #define CLKID_ABUF		31
 #define CLKID_HIU_IFACE		32
 #define CLKID_ASSIST_MISC	33
 #define CLKID_SPI		34
 #define CLKID_I2S_SPDIF		35
-#define CLKID_ETH		36
+/* #define CLKID_ETH */
 #define CLKID_DEMUX		37
 #define CLKID_AIU_GLUE		38
 #define CLKID_IEC958		39
@@ -114,12 +114,12 @@
 #define CLKID_AIU		47
 #define CLKID_UART1		48
 #define CLKID_G2D		49
-#define CLKID_USB0		50
-#define CLKID_USB1		51
+/* #define CLKID_USB0 */
+/* #define CLKID_USB1 */
 #define CLKID_RESET		52
 #define CLKID_NAND		53
 #define CLKID_DOS_PARSER	54
-#define CLKID_USB		55
+/* #define CLKID_USB */
 #define CLKID_VDIN1		56
 #define CLKID_AHB_ARB0		57
 #define CLKID_EFUSE		58
@@ -128,12 +128,12 @@
 #define CLKID_AHB_CTRL_BUS	61
 #define CLKID_HDMI_INTR_SYNC	62
 #define CLKID_HDMI_PCLK		63
-#define CLKID_USB1_DDR_BRIDGE	64
-#define CLKID_USB0_DDR_BRIDGE	65
+/* CLKID_USB1_DDR_BRIDGE */
+/* CLKID_USB0_DDR_BRIDGE */
 #define CLKID_MMC_PCLK		66
 #define CLKID_DVIN		67
 #define CLKID_UART2		68
-#define CLKID_SANA		69
+/* #define CLKID_SANA */
 #define CLKID_VPU_INTR		70
 #define CLKID_SEC_AHB_AHB3_BRIDGE	71
 #define CLKID_CLK81_A9		72
diff --git a/drivers/clk/sunxi-ng/Kconfig b/drivers/clk/sunxi-ng/Kconfig
index b0d551a8efe4..eb89c7801f00 100644
--- a/drivers/clk/sunxi-ng/Kconfig
+++ b/drivers/clk/sunxi-ng/Kconfig
@@ -156,6 +156,7 @@ config SUN8I_R_CCU
 	bool "Support for Allwinner SoCs' PRCM CCUs"
 	select SUNXI_CCU_DIV
 	select SUNXI_CCU_GATE
+	select SUNXI_CCU_MP
 	default MACH_SUN8I || (ARCH_SUNXI && ARM64)
 
 endif
diff --git a/drivers/clk/sunxi-ng/ccu-sun50i-a64.h b/drivers/clk/sunxi-ng/ccu-sun50i-a64.h
index 9b3cd24b78d2..061b6fbb4f95 100644
--- a/drivers/clk/sunxi-ng/ccu-sun50i-a64.h
+++ b/drivers/clk/sunxi-ng/ccu-sun50i-a64.h
@@ -31,7 +31,9 @@
 #define CLK_PLL_VIDEO0_2X		8
 #define CLK_PLL_VE			9
 #define CLK_PLL_DDR0			10
-#define CLK_PLL_PERIPH0			11
+
+/* PLL_PERIPH0 exported for PRCM */
+
 #define CLK_PLL_PERIPH0_2X		12
 #define CLK_PLL_PERIPH1			13
 #define CLK_PLL_PERIPH1_2X		14
diff --git a/drivers/clk/sunxi-ng/ccu-sun5i.c b/drivers/clk/sunxi-ng/ccu-sun5i.c
index 5c476f966a72..5372bf8be5e6 100644
--- a/drivers/clk/sunxi-ng/ccu-sun5i.c
+++ b/drivers/clk/sunxi-ng/ccu-sun5i.c
@@ -243,7 +243,7 @@ static SUNXI_CCU_GATE(ahb_ss_clk,	"ahb-ss",	"ahb",
 static SUNXI_CCU_GATE(ahb_dma_clk,	"ahb-dma",	"ahb",
 		      0x060, BIT(6), 0);
 static SUNXI_CCU_GATE(ahb_bist_clk,	"ahb-bist",	"ahb",
-		      0x060, BIT(6), 0);
+		      0x060, BIT(7), 0);
 static SUNXI_CCU_GATE(ahb_mmc0_clk,	"ahb-mmc0",	"ahb",
 		      0x060, BIT(8), 0);
 static SUNXI_CCU_GATE(ahb_mmc1_clk,	"ahb-mmc1",	"ahb",
diff --git a/drivers/clk/sunxi-ng/ccu-sun6i-a31.c b/drivers/clk/sunxi-ng/ccu-sun6i-a31.c
index 89e68d29bf45..df97e25aec76 100644
--- a/drivers/clk/sunxi-ng/ccu-sun6i-a31.c
+++ b/drivers/clk/sunxi-ng/ccu-sun6i-a31.c
@@ -556,7 +556,7 @@ static SUNXI_CCU_M_WITH_MUX_GATE(lcd0_ch1_clk, "lcd0-ch1", lcd_ch1_parents,
 				 0x12c, 0, 4, 24, 3, BIT(31),
 				 CLK_SET_RATE_PARENT);
 static SUNXI_CCU_M_WITH_MUX_GATE(lcd1_ch1_clk, "lcd1-ch1", lcd_ch1_parents,
-				 0x12c, 0, 4, 24, 3, BIT(31),
+				 0x130, 0, 4, 24, 3, BIT(31),
 				 CLK_SET_RATE_PARENT);
 
 static const char * const csi_sclk_parents[] = { "pll-video0", "pll-video1",
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-h3.h b/drivers/clk/sunxi-ng/ccu-sun8i-h3.h
index 85973d1e8165..1b4baea37d81 100644
--- a/drivers/clk/sunxi-ng/ccu-sun8i-h3.h
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-h3.h
@@ -29,7 +29,9 @@
 #define CLK_PLL_VIDEO		6
 #define CLK_PLL_VE		7
 #define CLK_PLL_DDR		8
-#define CLK_PLL_PERIPH0		9
+
+/* PLL_PERIPH0 exported for PRCM */
+
 #define CLK_PLL_PERIPH0_2X	10
 #define CLK_PLL_GPU		11
 #define CLK_PLL_PERIPH1		12
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-v3s.c b/drivers/clk/sunxi-ng/ccu-sun8i-v3s.c
index e58706b40ae9..6297add857b5 100644
--- a/drivers/clk/sunxi-ng/ccu-sun8i-v3s.c
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-v3s.c
@@ -537,7 +537,7 @@ static struct ccu_reset_map sun8i_v3s_ccu_resets[] = {
 	[RST_BUS_EMAC]		=  { 0x2c0, BIT(17) },
 	[RST_BUS_HSTIMER]	=  { 0x2c0, BIT(19) },
 	[RST_BUS_SPI0]		=  { 0x2c0, BIT(20) },
-	[RST_BUS_OTG]		=  { 0x2c0, BIT(23) },
+	[RST_BUS_OTG]		=  { 0x2c0, BIT(24) },
 	[RST_BUS_EHCI0]		=  { 0x2c0, BIT(26) },
 	[RST_BUS_OHCI0]		=  { 0x2c0, BIT(29) },
 
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 545d541ae20e..fcae5ca6ac92 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -1,22 +1,16 @@
 menu "Clock Source drivers"
 	depends on !ARCH_USES_GETTIMEOFFSET
 
-config CLKSRC_OF
+config TIMER_OF
 	bool
-	select CLKSRC_PROBE
-
-config CLKEVT_OF
-	bool
-	select CLKEVT_PROBE
-
-config CLKSRC_ACPI
-	bool
-	select CLKSRC_PROBE
+	depends on GENERIC_CLOCKEVENTS
+	select TIMER_PROBE
 
-config CLKSRC_PROBE
+config TIMER_ACPI
 	bool
+	select TIMER_PROBE
 
-config CLKEVT_PROBE
+config TIMER_PROBE
 	bool
 
 config CLKSRC_I8253
@@ -65,14 +59,14 @@ config DW_APB_TIMER
 config DW_APB_TIMER_OF
 	bool
 	select DW_APB_TIMER
-	select CLKSRC_OF
+	select TIMER_OF
 
 config FTTMR010_TIMER
 	bool "Faraday Technology timer driver" if COMPILE_TEST
 	depends on GENERIC_CLOCKEVENTS
 	depends on HAS_IOMEM
 	select CLKSRC_MMIO
-	select CLKSRC_OF
+	select TIMER_OF
 	select MFD_SYSCON
 	help
 	  Enables support for the Faraday Technology timer block
@@ -81,7 +75,7 @@ config FTTMR010_TIMER
 config ROCKCHIP_TIMER
 	bool "Rockchip timer driver" if COMPILE_TEST
 	depends on ARM || ARM64
-	select CLKSRC_OF
+	select TIMER_OF
 	select CLKSRC_MMIO
 	help
 	  Enables the support for the rockchip timer driver.
@@ -89,7 +83,7 @@ config ROCKCHIP_TIMER
 config ARMADA_370_XP_TIMER
 	bool "Armada 370 and XP timer driver" if COMPILE_TEST
 	depends on ARM
-	select CLKSRC_OF
+	select TIMER_OF
 	select CLKSRC_MMIO
 	help
 	  Enables the support for the Armada 370 and XP timer driver.
@@ -104,16 +98,24 @@ config MESON6_TIMER
 config ORION_TIMER
 	bool "Orion timer driver" if COMPILE_TEST
 	depends on ARM
-	select CLKSRC_OF
+	select TIMER_OF
 	select CLKSRC_MMIO
 	help
 	  Enables the support for the Orion timer driver
 
+config OWL_TIMER
+	bool "Owl timer driver" if COMPILE_TEST
+	depends on GENERIC_CLOCKEVENTS
+	select CLKSRC_MMIO
+	help
+	  Enables the support for the Actions Semi Owl timer driver.
+
 config SUN4I_TIMER
 	bool "Sun4i timer driver" if COMPILE_TEST
 	depends on GENERIC_CLOCKEVENTS
 	depends on HAS_IOMEM
 	select CLKSRC_MMIO
+	select TIMER_OF
 	help
 	  Enables support for the Sun4i timer.
 
@@ -148,7 +150,7 @@ config ASM9260_TIMER
 	bool "ASM9260 timer driver" if COMPILE_TEST
 	depends on GENERIC_CLOCKEVENTS
 	select CLKSRC_MMIO
-	select CLKSRC_OF
+	select TIMER_OF
 	help
 	  Enables support for the ASM9260 timer.
 
@@ -188,13 +190,6 @@ config ATLAS7_TIMER
 	help
 	  Enables support for the Atlas7 timer.
 
-config MOXART_TIMER
-	bool "Moxart timer driver" if COMPILE_TEST
-	depends on GENERIC_CLOCKEVENTS
-	select CLKSRC_MMIO
-	help
-	  Enables support for the Moxart timer.
-
 config MXS_TIMER
 	bool "Mxs timer driver" if COMPILE_TEST
 	depends on GENERIC_CLOCKEVENTS
@@ -261,21 +256,21 @@ config CLKSRC_LPC32XX
 	depends on GENERIC_CLOCKEVENTS && HAS_IOMEM
 	depends on ARM
 	select CLKSRC_MMIO
-	select CLKSRC_OF
+	select TIMER_OF
 	help
 	  Support for the LPC32XX clocksource.
 
 config CLKSRC_PISTACHIO
 	bool "Clocksource for Pistachio SoC" if COMPILE_TEST
 	depends on HAS_IOMEM
-	select CLKSRC_OF
+	select TIMER_OF
 	help
 	  Enables the clocksource for the Pistachio SoC.
 
 config CLKSRC_TI_32K
 	bool "Texas Instruments 32.768 Hz Clocksource" if COMPILE_TEST
 	depends on GENERIC_SCHED_CLOCK
-	select CLKSRC_OF if OF
+	select TIMER_OF if OF
 	help
 	  This option enables support for Texas Instruments 32.768 Hz clocksource
 	  available on many OMAP-like platforms.
@@ -284,7 +279,7 @@ config CLKSRC_NPS
 	bool "NPS400 clocksource driver" if COMPILE_TEST
 	depends on !PHYS_ADDR_T_64BIT
 	select CLKSRC_MMIO
-	select CLKSRC_OF if OF
+	select TIMER_OF if OF
 	help
 	  NPS400 clocksource support.
 	  Got 64 bit counter with update rate up to 1000MHz.
@@ -299,12 +294,12 @@ config CLKSRC_MPS2
 	bool "Clocksource for MPS2 SoCs" if COMPILE_TEST
 	depends on GENERIC_SCHED_CLOCK
 	select CLKSRC_MMIO
-	select CLKSRC_OF
+	select TIMER_OF
 
 config ARC_TIMERS
 	bool "Support for 32-bit TIMERn counters in ARC Cores" if COMPILE_TEST
 	depends on GENERIC_CLOCKEVENTS
-	select CLKSRC_OF
+	select TIMER_OF
 	help
 	  These are legacy 32-bit TIMER0 and TIMER1 counters found on all ARC cores
 	  (ARC700 as well as ARC HS38).
@@ -314,7 +309,7 @@ config ARC_TIMERS_64BIT
 	bool "Support for 64-bit counters in ARC HS38 cores" if COMPILE_TEST
 	depends on GENERIC_CLOCKEVENTS
 	depends on ARC_TIMERS
-	select CLKSRC_OF
+	select TIMER_OF
 	help
 	  This enables 2 different 64-bit timers: RTC (for UP) and GFRC (for SMP)
 	  RTC is implemented inside the core, while GFRC sits outside the core in
@@ -323,8 +318,8 @@ config ARC_TIMERS_64BIT
 
 config ARM_ARCH_TIMER
 	bool
-	select CLKSRC_OF if OF
-	select CLKSRC_ACPI if ACPI
+	select TIMER_OF if OF
+	select TIMER_ACPI if ACPI
 
 config ARM_ARCH_TIMER_EVTSTREAM
 	bool "Enable ARM architected timer event stream generation by default"
@@ -381,7 +376,7 @@ config ARM64_ERRATUM_858921
 
 config ARM_GLOBAL_TIMER
 	bool "Support for the ARM global timer" if COMPILE_TEST
-	select CLKSRC_OF if OF
+	select TIMER_OF if OF
 	depends on ARM
 	help
 	  This options enables support for the ARM global timer unit
@@ -390,7 +385,7 @@ config ARM_TIMER_SP804
 	bool "Support for Dual Timer SP804 module"
 	depends on GENERIC_SCHED_CLOCK && CLKDEV_LOOKUP
 	select CLKSRC_MMIO
-	select CLKSRC_OF if OF
+	select TIMER_OF if OF
 
 config CLKSRC_ARM_GLOBAL_TIMER_SCHED_CLOCK
 	bool
@@ -401,19 +396,19 @@ config CLKSRC_ARM_GLOBAL_TIMER_SCHED_CLOCK
 
 config ARMV7M_SYSTICK
 	bool "Support for the ARMv7M system time" if COMPILE_TEST
-	select CLKSRC_OF if OF
+	select TIMER_OF if OF
 	select CLKSRC_MMIO
 	help
 	  This options enables support for the ARMv7M system timer unit
 
 config ATMEL_PIT
-	select CLKSRC_OF if OF
+	select TIMER_OF if OF
 	def_bool SOC_AT91SAM9 || SOC_SAMA5
 
 config ATMEL_ST
 	bool "Atmel ST timer support" if COMPILE_TEST
 	depends on GENERIC_CLOCKEVENTS
-	select CLKSRC_OF
+	select TIMER_OF
 	select MFD_SYSCON
 	help
 	  Support for the Atmel ST timer.
@@ -456,7 +451,7 @@ config VF_PIT_TIMER
 config OXNAS_RPS_TIMER
 	bool "Oxford Semiconductor OXNAS RPS Timers driver" if COMPILE_TEST
 	depends on GENERIC_CLOCKEVENTS
-	select CLKSRC_OF
+	select TIMER_OF
 	select CLKSRC_MMIO
 	help
 	  This enables support for the Oxford Semiconductor OXNAS RPS timers.
@@ -467,7 +462,7 @@ config SYS_SUPPORTS_SH_CMT
 config MTK_TIMER
 	bool "Mediatek timer driver" if COMPILE_TEST
 	depends on GENERIC_CLOCKEVENTS && HAS_IOMEM
-	select CLKSRC_OF
+	select TIMER_OF
 	select CLKSRC_MMIO
 	help
 	  Support for Mediatek timer driver.
@@ -540,7 +535,7 @@ config EM_TIMER_STI
 config CLKSRC_QCOM
 	bool "Qualcomm MSM timer" if COMPILE_TEST
 	depends on ARM
-	select CLKSRC_OF
+	select TIMER_OF
 	help
 	  This enables the clocksource and the per CPU clockevent driver for the
 	  Qualcomm SoCs.
@@ -548,7 +543,7 @@ config CLKSRC_QCOM
 config CLKSRC_VERSATILE
 	bool "ARM Versatile (Express) reference platforms clock source" if COMPILE_TEST
 	depends on GENERIC_SCHED_CLOCK && !ARCH_USES_GETTIMEOFFSET
-	select CLKSRC_OF
+	select TIMER_OF
 	default y if MFD_VEXPRESS_SYSREG
 	help
 	  This option enables clock source based on free running
@@ -559,12 +554,12 @@ config CLKSRC_VERSATILE
 config CLKSRC_MIPS_GIC
 	bool
 	depends on MIPS_GIC
-	select CLKSRC_OF
+	select TIMER_OF
 
 config CLKSRC_TANGO_XTAL
 	bool "Clocksource for Tango SoC" if COMPILE_TEST
 	depends on ARM
-	select CLKSRC_OF
+	select TIMER_OF
 	select CLKSRC_MMIO
 	help
 	  This enables the clocksource for Tango SoC
@@ -605,7 +600,7 @@ config CLKSRC_IMX_GPT
 
 config CLKSRC_ST_LPC
 	bool "Low power clocksource found in the LPC" if COMPILE_TEST
-	select CLKSRC_OF if OF
+	select TIMER_OF if OF
 	depends on HAS_IOMEM
 	select CLKSRC_MMIO
 	help
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index 2b5b56a6f00f..6df949402dfc 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -1,5 +1,5 @@
-obj-$(CONFIG_CLKSRC_PROBE)	+= clksrc-probe.o
-obj-$(CONFIG_CLKEVT_PROBE)	+= clkevt-probe.o
+obj-$(CONFIG_TIMER_OF)		+= timer-of.o
+obj-$(CONFIG_TIMER_PROBE)	+= timer-probe.o
 obj-$(CONFIG_ATMEL_PIT)		+= timer-atmel-pit.o
 obj-$(CONFIG_ATMEL_ST)		+= timer-atmel-st.o
 obj-$(CONFIG_ATMEL_TCB_CLKSRC)	+= tcb_clksrc.o
@@ -26,7 +26,6 @@ obj-$(CONFIG_ORION_TIMER)	+= time-orion.o
 obj-$(CONFIG_BCM2835_TIMER)	+= bcm2835_timer.o
 obj-$(CONFIG_CLPS711X_TIMER)	+= clps711x-timer.o
 obj-$(CONFIG_ATLAS7_TIMER)	+= timer-atlas7.o
-obj-$(CONFIG_MOXART_TIMER)	+= moxart_timer.o
 obj-$(CONFIG_MXS_TIMER)		+= mxs_timer.o
 obj-$(CONFIG_CLKSRC_PXA)	+= pxa_timer.o
 obj-$(CONFIG_PRIMA2_TIMER)	+= timer-prima2.o
@@ -53,6 +52,7 @@ obj-$(CONFIG_CLKSRC_PISTACHIO)	+= time-pistachio.o
 obj-$(CONFIG_CLKSRC_TI_32K)	+= timer-ti-32k.o
 obj-$(CONFIG_CLKSRC_NPS)	+= timer-nps.o
 obj-$(CONFIG_OXNAS_RPS_TIMER)	+= timer-oxnas-rps.o
+obj-$(CONFIG_OWL_TIMER)		+= owl-timer.o
 
 obj-$(CONFIG_ARC_TIMERS)		+= arc_timer.o
 obj-$(CONFIG_ARM_ARCH_TIMER)		+= arm_arch_timer.o
diff --git a/drivers/clocksource/arc_timer.c b/drivers/clocksource/arc_timer.c
index 21649733827d..4927355f9cbe 100644
--- a/drivers/clocksource/arc_timer.c
+++ b/drivers/clocksource/arc_timer.c
@@ -99,7 +99,7 @@ static int __init arc_cs_setup_gfrc(struct device_node *node)
 
 	return clocksource_register_hz(&arc_counter_gfrc, arc_timer_freq);
 }
-CLOCKSOURCE_OF_DECLARE(arc_gfrc, "snps,archs-timer-gfrc", arc_cs_setup_gfrc);
+TIMER_OF_DECLARE(arc_gfrc, "snps,archs-timer-gfrc", arc_cs_setup_gfrc);
 
 #define AUX_RTC_CTRL	0x103
 #define AUX_RTC_LOW	0x104
@@ -158,7 +158,7 @@ static int __init arc_cs_setup_rtc(struct device_node *node)
 
 	return clocksource_register_hz(&arc_counter_rtc, arc_timer_freq);
 }
-CLOCKSOURCE_OF_DECLARE(arc_rtc, "snps,archs-timer-rtc", arc_cs_setup_rtc);
+TIMER_OF_DECLARE(arc_rtc, "snps,archs-timer-rtc", arc_cs_setup_rtc);
 
 #endif
 
@@ -333,4 +333,4 @@ static int __init arc_of_timer_init(struct device_node *np)
 
 	return ret;
 }
-CLOCKSOURCE_OF_DECLARE(arc_clkevt, "snps,arc-timer", arc_of_timer_init);
+TIMER_OF_DECLARE(arc_clkevt, "snps,arc-timer", arc_of_timer_init);
diff --git a/drivers/clocksource/arm_arch_timer.c b/drivers/clocksource/arm_arch_timer.c
index 4bed671e490e..aae87c4c546e 100644
--- a/drivers/clocksource/arm_arch_timer.c
+++ b/drivers/clocksource/arm_arch_timer.c
@@ -1194,8 +1194,8 @@ static int __init arch_timer_of_init(struct device_node *np)
 
 	return arch_timer_common_init();
 }
-CLOCKSOURCE_OF_DECLARE(armv7_arch_timer, "arm,armv7-timer", arch_timer_of_init);
-CLOCKSOURCE_OF_DECLARE(armv8_arch_timer, "arm,armv8-timer", arch_timer_of_init);
+TIMER_OF_DECLARE(armv7_arch_timer, "arm,armv7-timer", arch_timer_of_init);
+TIMER_OF_DECLARE(armv8_arch_timer, "arm,armv8-timer", arch_timer_of_init);
 
 static u32 __init
 arch_timer_mem_frame_get_cntfrq(struct arch_timer_mem_frame *frame)
@@ -1209,9 +1209,9 @@ arch_timer_mem_frame_get_cntfrq(struct arch_timer_mem_frame *frame)
 		return 0;
 	}
 
-	rate = readl_relaxed(frame + CNTFRQ);
+	rate = readl_relaxed(base + CNTFRQ);
 
-	iounmap(frame);
+	iounmap(base);
 
 	return rate;
 }
@@ -1382,7 +1382,7 @@ out:
 	kfree(timer_mem);
 	return ret;
 }
-CLOCKSOURCE_OF_DECLARE(armv7_arch_timer_mem, "arm,armv7-timer-mem",
+TIMER_OF_DECLARE(armv7_arch_timer_mem, "arm,armv7-timer-mem",
 		       arch_timer_mem_of_init);
 
 #ifdef CONFIG_ACPI_GTDT
@@ -1516,5 +1516,5 @@ static int __init arch_timer_acpi_init(struct acpi_table_header *table)
 
 	return arch_timer_common_init();
 }
-CLOCKSOURCE_ACPI_DECLARE(arch_timer, ACPI_SIG_GTDT, arch_timer_acpi_init);
+TIMER_ACPI_DECLARE(arch_timer, ACPI_SIG_GTDT, arch_timer_acpi_init);
 #endif
diff --git a/drivers/clocksource/arm_global_timer.c b/drivers/clocksource/arm_global_timer.c
index 123ed20ac2ff..095bb965f621 100644
--- a/drivers/clocksource/arm_global_timer.c
+++ b/drivers/clocksource/arm_global_timer.c
@@ -339,5 +339,5 @@ out_unmap:
 }
 
 /* Only tested on r2p2 and r3p0  */
-CLOCKSOURCE_OF_DECLARE(arm_gt, "arm,cortex-a9-global-timer",
+TIMER_OF_DECLARE(arm_gt, "arm,cortex-a9-global-timer",
 			global_timer_of_register);
diff --git a/drivers/clocksource/armv7m_systick.c b/drivers/clocksource/armv7m_systick.c
index a315491b7047..ac046d6fb0bf 100644
--- a/drivers/clocksource/armv7m_systick.c
+++ b/drivers/clocksource/armv7m_systick.c
@@ -82,5 +82,5 @@ out_unmap:
 	return ret;
 }
 
-CLOCKSOURCE_OF_DECLARE(arm_systick, "arm,armv7m-systick",
+TIMER_OF_DECLARE(arm_systick, "arm,armv7m-systick",
 			system_timer_of_register);
diff --git a/drivers/clocksource/asm9260_timer.c b/drivers/clocksource/asm9260_timer.c
index c6780830b8ac..38cd2feb87c4 100644
--- a/drivers/clocksource/asm9260_timer.c
+++ b/drivers/clocksource/asm9260_timer.c
@@ -238,5 +238,5 @@ static int __init asm9260_timer_init(struct device_node *np)
 
 	return 0;
 }
-CLOCKSOURCE_OF_DECLARE(asm9260_timer, "alphascale,asm9260-timer",
+TIMER_OF_DECLARE(asm9260_timer, "alphascale,asm9260-timer",
 		asm9260_timer_init);
diff --git a/drivers/clocksource/bcm2835_timer.c b/drivers/clocksource/bcm2835_timer.c
index dce44307469e..82828d3a4739 100644
--- a/drivers/clocksource/bcm2835_timer.c
+++ b/drivers/clocksource/bcm2835_timer.c
@@ -148,5 +148,5 @@ err_iounmap:
 	iounmap(base);
 	return ret;
 }
-CLOCKSOURCE_OF_DECLARE(bcm2835, "brcm,bcm2835-system-timer",
+TIMER_OF_DECLARE(bcm2835, "brcm,bcm2835-system-timer",
 			bcm2835_timer_init);
diff --git a/drivers/clocksource/bcm_kona_timer.c b/drivers/clocksource/bcm_kona_timer.c
index fda5e1476638..5c40be9880f5 100644
--- a/drivers/clocksource/bcm_kona_timer.c
+++ b/drivers/clocksource/bcm_kona_timer.c
@@ -198,9 +198,9 @@ static int __init kona_timer_init(struct device_node *node)
 	return 0;
 }
 
-CLOCKSOURCE_OF_DECLARE(brcm_kona, "brcm,kona-timer", kona_timer_init);
+TIMER_OF_DECLARE(brcm_kona, "brcm,kona-timer", kona_timer_init);
 /*
  * bcm,kona-timer is deprecated by brcm,kona-timer
  * being kept here for driver compatibility
  */
-CLOCKSOURCE_OF_DECLARE(bcm_kona, "bcm,kona-timer", kona_timer_init);
+TIMER_OF_DECLARE(bcm_kona, "bcm,kona-timer", kona_timer_init);
diff --git a/drivers/clocksource/cadence_ttc_timer.c b/drivers/clocksource/cadence_ttc_timer.c
index 44e5e951583b..29d51755e18b 100644
--- a/drivers/clocksource/cadence_ttc_timer.c
+++ b/drivers/clocksource/cadence_ttc_timer.c
@@ -18,6 +18,7 @@
 #include <linux/clk.h>
 #include <linux/interrupt.h>
 #include <linux/clockchips.h>
+#include <linux/clocksource.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
 #include <linux/slab.h>
@@ -539,4 +540,4 @@ static int __init ttc_timer_init(struct device_node *timer)
 	return 0;
 }
 
-CLOCKSOURCE_OF_DECLARE(ttc, "cdns,ttc", ttc_timer_init);
+TIMER_OF_DECLARE(ttc, "cdns,ttc", ttc_timer_init);
diff --git a/drivers/clocksource/clkevt-probe.c b/drivers/clocksource/clkevt-probe.c
deleted file mode 100644
index eb89b502acbd..000000000000
--- a/drivers/clocksource/clkevt-probe.c
+++ /dev/null
@@ -1,56 +0,0 @@
-/*
- * Copyright (c) 2016, Linaro Ltd.  All rights reserved.
- * Daniel Lezcano <daniel.lezcano@linaro.org>
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <linux/init.h>
-#include <linux/of.h>
-#include <linux/clockchips.h>
-
-extern struct of_device_id __clkevt_of_table[];
-
-static const struct of_device_id __clkevt_of_table_sentinel
-	__used __section(__clkevt_of_table_end);
-
-int __init clockevent_probe(void)
-{
-	struct device_node *np;
-	const struct of_device_id *match;
-	of_init_fn_1_ret init_func;
-	int ret, clockevents = 0;
-
-	for_each_matching_node_and_match(np, __clkevt_of_table, &match) {
-		if (!of_device_is_available(np))
-			continue;
-
-		init_func = match->data;
-
-		ret = init_func(np);
-		if (ret) {
-			pr_warn("Failed to initialize '%s' (%d)\n",
-				np->name, ret);
-			continue;
-		}
-
-		clockevents++;
-	}
-
-	if (!clockevents) {
-		pr_crit("%s: no matching clockevent found\n", __func__);
-		return -ENODEV;
-	}
-
-	return 0;
-}
diff --git a/drivers/clocksource/clksrc-dbx500-prcmu.c b/drivers/clocksource/clksrc-dbx500-prcmu.c
index c69e2772658d..c1b96dc5f444 100644
--- a/drivers/clocksource/clksrc-dbx500-prcmu.c
+++ b/drivers/clocksource/clksrc-dbx500-prcmu.c
@@ -86,5 +86,5 @@ static int __init clksrc_dbx500_prcmu_init(struct device_node *node)
 #endif
 	return clocksource_register_hz(&clocksource_dbx500_prcmu, RATE_32K);
 }
-CLOCKSOURCE_OF_DECLARE(dbx500_prcmu, "stericsson,db8500-prcmu-timer-4",
+TIMER_OF_DECLARE(dbx500_prcmu, "stericsson,db8500-prcmu-timer-4",
 		       clksrc_dbx500_prcmu_init);
diff --git a/drivers/clocksource/clksrc_st_lpc.c b/drivers/clocksource/clksrc_st_lpc.c
index 03cc49217bb4..a1d01ebb81f5 100644
--- a/drivers/clocksource/clksrc_st_lpc.c
+++ b/drivers/clocksource/clksrc_st_lpc.c
@@ -132,4 +132,4 @@ static int __init st_clksrc_of_register(struct device_node *np)
 
 	return ret;
 }
-CLOCKSOURCE_OF_DECLARE(ddata, "st,stih407-lpc", st_clksrc_of_register);
+TIMER_OF_DECLARE(ddata, "st,stih407-lpc", st_clksrc_of_register);
diff --git a/drivers/clocksource/clps711x-timer.c b/drivers/clocksource/clps711x-timer.c
index 24db6d605549..a8dd80576c95 100644
--- a/drivers/clocksource/clps711x-timer.c
+++ b/drivers/clocksource/clps711x-timer.c
@@ -103,7 +103,7 @@ void __init clps711x_clksrc_init(void __iomem *tc1_base, void __iomem *tc2_base,
 	BUG_ON(_clps711x_clkevt_init(tc2, tc2_base, irq));
 }
 
-#ifdef CONFIG_CLKSRC_OF
+#ifdef CONFIG_TIMER_OF
 static int __init clps711x_timer_init(struct device_node *np)
 {
 	unsigned int irq = irq_of_parse_and_map(np, 0);
@@ -119,5 +119,5 @@ static int __init clps711x_timer_init(struct device_node *np)
 		return -EINVAL;
 	}
 }
-CLOCKSOURCE_OF_DECLARE(clps711x, "cirrus,ep7209-timer", clps711x_timer_init);
+TIMER_OF_DECLARE(clps711x, "cirrus,ep7209-timer", clps711x_timer_init);
 #endif
diff --git a/drivers/clocksource/dw_apb_timer_of.c b/drivers/clocksource/dw_apb_timer_of.c
index aee6c0d39a7c..69866cd8f4bb 100644
--- a/drivers/clocksource/dw_apb_timer_of.c
+++ b/drivers/clocksource/dw_apb_timer_of.c
@@ -167,7 +167,7 @@ static int __init dw_apb_timer_init(struct device_node *timer)
 
 	return 0;
 }
-CLOCKSOURCE_OF_DECLARE(pc3x2_timer, "picochip,pc3x2-timer", dw_apb_timer_init);
-CLOCKSOURCE_OF_DECLARE(apb_timer_osc, "snps,dw-apb-timer-osc", dw_apb_timer_init);
-CLOCKSOURCE_OF_DECLARE(apb_timer_sp, "snps,dw-apb-timer-sp", dw_apb_timer_init);
-CLOCKSOURCE_OF_DECLARE(apb_timer, "snps,dw-apb-timer", dw_apb_timer_init);
+TIMER_OF_DECLARE(pc3x2_timer, "picochip,pc3x2-timer", dw_apb_timer_init);
+TIMER_OF_DECLARE(apb_timer_osc, "snps,dw-apb-timer-osc", dw_apb_timer_init);
+TIMER_OF_DECLARE(apb_timer_sp, "snps,dw-apb-timer-sp", dw_apb_timer_init);
+TIMER_OF_DECLARE(apb_timer, "snps,dw-apb-timer", dw_apb_timer_init);
diff --git a/drivers/clocksource/exynos_mct.c b/drivers/clocksource/exynos_mct.c
index 670ff0f25b67..7a244b681876 100644
--- a/drivers/clocksource/exynos_mct.c
+++ b/drivers/clocksource/exynos_mct.c
@@ -610,5 +610,5 @@ static int __init mct_init_ppi(struct device_node *np)
 {
 	return mct_init_dt(np, MCT_INT_PPI);
 }
-CLOCKSOURCE_OF_DECLARE(exynos4210, "samsung,exynos4210-mct", mct_init_spi);
-CLOCKSOURCE_OF_DECLARE(exynos4412, "samsung,exynos4412-mct", mct_init_ppi);
+TIMER_OF_DECLARE(exynos4210, "samsung,exynos4210-mct", mct_init_spi);
+TIMER_OF_DECLARE(exynos4412, "samsung,exynos4412-mct", mct_init_ppi);
diff --git a/drivers/clocksource/fsl_ftm_timer.c b/drivers/clocksource/fsl_ftm_timer.c
index 738515b89073..3ee7e6fea621 100644
--- a/drivers/clocksource/fsl_ftm_timer.c
+++ b/drivers/clocksource/fsl_ftm_timer.c
@@ -329,13 +329,13 @@ static int __init ftm_timer_init(struct device_node *np)
 	priv->clkevt_base = of_iomap(np, 0);
 	if (!priv->clkevt_base) {
 		pr_err("ftm: unable to map event timer registers\n");
-		goto err;
+		goto err_clkevt;
 	}
 
 	priv->clksrc_base = of_iomap(np, 1);
 	if (!priv->clksrc_base) {
 		pr_err("ftm: unable to map source timer registers\n");
-		goto err;
+		goto err_clksrc;
 	}
 
 	ret = -EINVAL;
@@ -366,7 +366,11 @@ static int __init ftm_timer_init(struct device_node *np)
 	return 0;
 
 err:
+	iounmap(priv->clksrc_base);
+err_clksrc:
+	iounmap(priv->clkevt_base);
+err_clkevt:
 	kfree(priv);
 	return ret;
 }
-CLOCKSOURCE_OF_DECLARE(flextimer, "fsl,ftm-timer", ftm_timer_init);
+TIMER_OF_DECLARE(flextimer, "fsl,ftm-timer", ftm_timer_init);
diff --git a/drivers/clocksource/h8300_timer16.c b/drivers/clocksource/h8300_timer16.c
index 5b27fb9997c2..dfbd4f8051cb 100644
--- a/drivers/clocksource/h8300_timer16.c
+++ b/drivers/clocksource/h8300_timer16.c
@@ -187,5 +187,5 @@ free_clk:
 	return ret;
 }
 
-CLOCKSOURCE_OF_DECLARE(h8300_16bit, "renesas,16bit-timer",
+TIMER_OF_DECLARE(h8300_16bit, "renesas,16bit-timer",
 			   h8300_16timer_init);
diff --git a/drivers/clocksource/h8300_timer8.c b/drivers/clocksource/h8300_timer8.c
index 804c489531d6..f6ffb0cef091 100644
--- a/drivers/clocksource/h8300_timer8.c
+++ b/drivers/clocksource/h8300_timer8.c
@@ -207,4 +207,4 @@ free_clk:
 	return ret;
 }
 
-CLOCKSOURCE_OF_DECLARE(h8300_8bit, "renesas,8bit-timer", h8300_8timer_init);
+TIMER_OF_DECLARE(h8300_8bit, "renesas,8bit-timer", h8300_8timer_init);
diff --git a/drivers/clocksource/h8300_tpu.c b/drivers/clocksource/h8300_tpu.c
index 72e1cf2b3096..45a8d17dac1e 100644
--- a/drivers/clocksource/h8300_tpu.c
+++ b/drivers/clocksource/h8300_tpu.c
@@ -154,4 +154,4 @@ free_clk:
 	return ret;
 }
 
-CLOCKSOURCE_OF_DECLARE(h8300_tpu, "renesas,tpu", h8300_tpu_init);
+TIMER_OF_DECLARE(h8300_tpu, "renesas,tpu", h8300_tpu_init);
diff --git a/drivers/clocksource/jcore-pit.c b/drivers/clocksource/jcore-pit.c
index 7c61226f4359..5d3d88e0fc8c 100644
--- a/drivers/clocksource/jcore-pit.c
+++ b/drivers/clocksource/jcore-pit.c
@@ -246,4 +246,4 @@ static int __init jcore_pit_init(struct device_node *node)
 	return 0;
 }
 
-CLOCKSOURCE_OF_DECLARE(jcore_pit, "jcore,pit", jcore_pit_init);
+TIMER_OF_DECLARE(jcore_pit, "jcore,pit", jcore_pit_init);
diff --git a/drivers/clocksource/meson6_timer.c b/drivers/clocksource/meson6_timer.c
index 39d21f693a33..92f20991a937 100644
--- a/drivers/clocksource/meson6_timer.c
+++ b/drivers/clocksource/meson6_timer.c
@@ -174,5 +174,5 @@ static int __init meson6_timer_init(struct device_node *node)
 					1, 0xfffe);
 	return 0;
 }
-CLOCKSOURCE_OF_DECLARE(meson6, "amlogic,meson6-timer",
+TIMER_OF_DECLARE(meson6, "amlogic,meson6-timer",
 		       meson6_timer_init);
diff --git a/drivers/clocksource/mips-gic-timer.c b/drivers/clocksource/mips-gic-timer.c
index 3f52ee219923..17b861ea2626 100644
--- a/drivers/clocksource/mips-gic-timer.c
+++ b/drivers/clocksource/mips-gic-timer.c
@@ -167,10 +167,11 @@ static int __init gic_clocksource_of_init(struct device_node *node)
 
 	clk = of_clk_get(node, 0);
 	if (!IS_ERR(clk)) {
-		if (clk_prepare_enable(clk) < 0) {
+		ret = clk_prepare_enable(clk);
+		if (ret < 0) {
 			pr_err("GIC failed to enable clock\n");
 			clk_put(clk);
-			return PTR_ERR(clk);
+			return ret;
 		}
 
 		gic_frequency = clk_get_rate(clk);
@@ -200,5 +201,5 @@ static int __init gic_clocksource_of_init(struct device_node *node)
 
 	return 0;
 }
-CLOCKSOURCE_OF_DECLARE(mips_gic_timer, "mti,gic-timer",
+TIMER_OF_DECLARE(mips_gic_timer, "mti,gic-timer",
 		       gic_clocksource_of_init);
diff --git a/drivers/clocksource/moxart_timer.c b/drivers/clocksource/moxart_timer.c
deleted file mode 100644
index 7f3430654fbd..000000000000
--- a/drivers/clocksource/moxart_timer.c
+++ /dev/null
@@ -1,256 +0,0 @@
-/*
- * MOXA ART SoCs timer handling.
- *
- * Copyright (C) 2013 Jonas Jensen
- *
- * Jonas Jensen <jonas.jensen@gmail.com>
- *
- * This file is licensed under the terms of the GNU General Public
- * License version 2.  This program is licensed "as is" without any
- * warranty of any kind, whether express or implied.
- */
-
-#include <linux/clk.h>
-#include <linux/clockchips.h>
-#include <linux/interrupt.h>
-#include <linux/irq.h>
-#include <linux/irqreturn.h>
-#include <linux/of.h>
-#include <linux/of_address.h>
-#include <linux/of_irq.h>
-#include <linux/io.h>
-#include <linux/clocksource.h>
-#include <linux/bitops.h>
-#include <linux/slab.h>
-
-#define TIMER1_BASE		0x00
-#define TIMER2_BASE		0x10
-#define TIMER3_BASE		0x20
-
-#define REG_COUNT		0x0 /* writable */
-#define REG_LOAD		0x4
-#define REG_MATCH1		0x8
-#define REG_MATCH2		0xC
-
-#define TIMER_CR		0x30
-#define TIMER_INTR_STATE	0x34
-#define TIMER_INTR_MASK		0x38
-
-/*
- * Moxart TIMER_CR flags:
- *
- * MOXART_CR_*_CLOCK	0: PCLK, 1: EXT1CLK
- * MOXART_CR_*_INT	overflow interrupt enable bit
- */
-#define MOXART_CR_1_ENABLE	BIT(0)
-#define MOXART_CR_1_CLOCK	BIT(1)
-#define MOXART_CR_1_INT	BIT(2)
-#define MOXART_CR_2_ENABLE	BIT(3)
-#define MOXART_CR_2_CLOCK	BIT(4)
-#define MOXART_CR_2_INT	BIT(5)
-#define MOXART_CR_3_ENABLE	BIT(6)
-#define MOXART_CR_3_CLOCK	BIT(7)
-#define MOXART_CR_3_INT	BIT(8)
-#define MOXART_CR_COUNT_UP	BIT(9)
-
-#define MOXART_TIMER1_ENABLE	(MOXART_CR_2_ENABLE | MOXART_CR_1_ENABLE)
-#define MOXART_TIMER1_DISABLE	(MOXART_CR_2_ENABLE)
-
-/*
- * The ASpeed variant of the IP block has a different layout
- * for the control register
- */
-#define ASPEED_CR_1_ENABLE	BIT(0)
-#define ASPEED_CR_1_CLOCK	BIT(1)
-#define ASPEED_CR_1_INT		BIT(2)
-#define ASPEED_CR_2_ENABLE	BIT(4)
-#define ASPEED_CR_2_CLOCK	BIT(5)
-#define ASPEED_CR_2_INT		BIT(6)
-#define ASPEED_CR_3_ENABLE	BIT(8)
-#define ASPEED_CR_3_CLOCK	BIT(9)
-#define ASPEED_CR_3_INT		BIT(10)
-
-#define ASPEED_TIMER1_ENABLE   (ASPEED_CR_2_ENABLE | ASPEED_CR_1_ENABLE)
-#define ASPEED_TIMER1_DISABLE  (ASPEED_CR_2_ENABLE)
-
-struct moxart_timer {
-	void __iomem *base;
-	unsigned int t1_disable_val;
-	unsigned int t1_enable_val;
-	unsigned int count_per_tick;
-	struct clock_event_device clkevt;
-};
-
-static inline struct moxart_timer *to_moxart(struct clock_event_device *evt)
-{
-	return container_of(evt, struct moxart_timer, clkevt);
-}
-
-static inline void moxart_disable(struct clock_event_device *evt)
-{
-	struct moxart_timer *timer = to_moxart(evt);
-
-	writel(timer->t1_disable_val, timer->base + TIMER_CR);
-}
-
-static inline void moxart_enable(struct clock_event_device *evt)
-{
-	struct moxart_timer *timer = to_moxart(evt);
-
-	writel(timer->t1_enable_val, timer->base + TIMER_CR);
-}
-
-static int moxart_shutdown(struct clock_event_device *evt)
-{
-	moxart_disable(evt);
-	return 0;
-}
-
-static int moxart_set_oneshot(struct clock_event_device *evt)
-{
-	moxart_disable(evt);
-	writel(~0, to_moxart(evt)->base + TIMER1_BASE + REG_LOAD);
-	return 0;
-}
-
-static int moxart_set_periodic(struct clock_event_device *evt)
-{
-	struct moxart_timer *timer = to_moxart(evt);
-
-	moxart_disable(evt);
-	writel(timer->count_per_tick, timer->base + TIMER1_BASE + REG_LOAD);
-	writel(0, timer->base + TIMER1_BASE + REG_MATCH1);
-	moxart_enable(evt);
-	return 0;
-}
-
-static int moxart_clkevt_next_event(unsigned long cycles,
-				    struct clock_event_device *evt)
-{
-	struct moxart_timer *timer = to_moxart(evt);
-	u32 u;
-
-	moxart_disable(evt);
-
-	u = readl(timer->base + TIMER1_BASE + REG_COUNT) - cycles;
-	writel(u, timer->base + TIMER1_BASE + REG_MATCH1);
-
-	moxart_enable(evt);
-
-	return 0;
-}
-
-static irqreturn_t moxart_timer_interrupt(int irq, void *dev_id)
-{
-	struct clock_event_device *evt = dev_id;
-	evt->event_handler(evt);
-	return IRQ_HANDLED;
-}
-
-static int __init moxart_timer_init(struct device_node *node)
-{
-	int ret, irq;
-	unsigned long pclk;
-	struct clk *clk;
-	struct moxart_timer *timer;
-
-	timer = kzalloc(sizeof(*timer), GFP_KERNEL);
-	if (!timer)
-		return -ENOMEM;
-
-	timer->base = of_iomap(node, 0);
-	if (!timer->base) {
-		pr_err("%s: of_iomap failed\n", node->full_name);
-		ret = -ENXIO;
-		goto out_free;
-	}
-
-	irq = irq_of_parse_and_map(node, 0);
-	if (irq <= 0) {
-		pr_err("%s: irq_of_parse_and_map failed\n", node->full_name);
-		ret = -EINVAL;
-		goto out_unmap;
-	}
-
-	clk = of_clk_get(node, 0);
-	if (IS_ERR(clk))  {
-		pr_err("%s: of_clk_get failed\n", node->full_name);
-		ret = PTR_ERR(clk);
-		goto out_unmap;
-	}
-
-	pclk = clk_get_rate(clk);
-
-	if (of_device_is_compatible(node, "moxa,moxart-timer")) {
-		timer->t1_enable_val = MOXART_TIMER1_ENABLE;
-		timer->t1_disable_val = MOXART_TIMER1_DISABLE;
-	} else if (of_device_is_compatible(node, "aspeed,ast2400-timer")) {
-		timer->t1_enable_val = ASPEED_TIMER1_ENABLE;
-		timer->t1_disable_val = ASPEED_TIMER1_DISABLE;
-	} else {
-		pr_err("%s: unknown platform\n", node->full_name);
-		ret = -EINVAL;
-		goto out_unmap;
-	}
-
-	timer->count_per_tick = DIV_ROUND_CLOSEST(pclk, HZ);
-
-	timer->clkevt.name = node->name;
-	timer->clkevt.rating = 200;
-	timer->clkevt.features = CLOCK_EVT_FEAT_PERIODIC |
-					CLOCK_EVT_FEAT_ONESHOT;
-	timer->clkevt.set_state_shutdown = moxart_shutdown;
-	timer->clkevt.set_state_periodic = moxart_set_periodic;
-	timer->clkevt.set_state_oneshot = moxart_set_oneshot;
-	timer->clkevt.tick_resume = moxart_set_oneshot;
-	timer->clkevt.set_next_event = moxart_clkevt_next_event;
-	timer->clkevt.cpumask = cpumask_of(0);
-	timer->clkevt.irq = irq;
-
-	ret = clocksource_mmio_init(timer->base + TIMER2_BASE + REG_COUNT,
-				    "moxart_timer", pclk, 200, 32,
-				    clocksource_mmio_readl_down);
-	if (ret) {
-		pr_err("%s: clocksource_mmio_init failed\n", node->full_name);
-		goto out_unmap;
-	}
-
-	ret = request_irq(irq, moxart_timer_interrupt, IRQF_TIMER,
-			  node->name, &timer->clkevt);
-	if (ret) {
-		pr_err("%s: setup_irq failed\n", node->full_name);
-		goto out_unmap;
-	}
-
-	/* Clear match registers */
-	writel(0, timer->base + TIMER1_BASE + REG_MATCH1);
-	writel(0, timer->base + TIMER1_BASE + REG_MATCH2);
-	writel(0, timer->base + TIMER2_BASE + REG_MATCH1);
-	writel(0, timer->base + TIMER2_BASE + REG_MATCH2);
-
-	/*
-	 * Start timer 2 rolling as our main wall clock source, keep timer 1
-	 * disabled
-	 */
-	writel(0, timer->base + TIMER_CR);
-	writel(~0, timer->base + TIMER2_BASE + REG_LOAD);
-	writel(timer->t1_disable_val, timer->base + TIMER_CR);
-
-	/*
-	 * documentation is not publicly available:
-	 * min_delta / max_delta obtained by trial-and-error,
-	 * max_delta 0xfffffffe should be ok because count
-	 * register size is u32
-	 */
-	clockevents_config_and_register(&timer->clkevt, pclk, 0x4, 0xfffffffe);
-
-	return 0;
-
-out_unmap:
-	iounmap(timer->base);
-out_free:
-	kfree(timer);
-	return ret;
-}
-CLOCKSOURCE_OF_DECLARE(moxart, "moxa,moxart-timer", moxart_timer_init);
-CLOCKSOURCE_OF_DECLARE(aspeed, "aspeed,ast2400-timer", moxart_timer_init);
diff --git a/drivers/clocksource/mps2-timer.c b/drivers/clocksource/mps2-timer.c
index 3e4431ed9aa9..aa4d63af8706 100644
--- a/drivers/clocksource/mps2-timer.c
+++ b/drivers/clocksource/mps2-timer.c
@@ -274,4 +274,4 @@ static int __init mps2_timer_init(struct device_node *np)
 	return 0;
 }
 
-CLOCKSOURCE_OF_DECLARE(mps2_timer, "arm,mps2-timer", mps2_timer_init);
+TIMER_OF_DECLARE(mps2_timer, "arm,mps2-timer", mps2_timer_init);
diff --git a/drivers/clocksource/mtk_timer.c b/drivers/clocksource/mtk_timer.c
index 90659493c59c..f9b724fd9950 100644
--- a/drivers/clocksource/mtk_timer.c
+++ b/drivers/clocksource/mtk_timer.c
@@ -265,4 +265,4 @@ err_kzalloc:
 
 	return -EINVAL;
 }
-CLOCKSOURCE_OF_DECLARE(mtk_mt6577, "mediatek,mt6577-timer", mtk_timer_init);
+TIMER_OF_DECLARE(mtk_mt6577, "mediatek,mt6577-timer", mtk_timer_init);
diff --git a/drivers/clocksource/mxs_timer.c b/drivers/clocksource/mxs_timer.c
index 99b77aff0839..a03434e9fe8f 100644
--- a/drivers/clocksource/mxs_timer.c
+++ b/drivers/clocksource/mxs_timer.c
@@ -293,4 +293,4 @@ static int __init mxs_timer_init(struct device_node *np)
 
 	return setup_irq(irq, &mxs_timer_irq);
 }
-CLOCKSOURCE_OF_DECLARE(mxs, "fsl,timrot", mxs_timer_init);
+TIMER_OF_DECLARE(mxs, "fsl,timrot", mxs_timer_init);
diff --git a/drivers/clocksource/nomadik-mtu.c b/drivers/clocksource/nomadik-mtu.c
index 7d44de304f37..8e4ddb9420c6 100644
--- a/drivers/clocksource/nomadik-mtu.c
+++ b/drivers/clocksource/nomadik-mtu.c
@@ -284,5 +284,5 @@ static int __init nmdk_timer_of_init(struct device_node *node)
 
 	return nmdk_timer_init(base, irq, pclk, clk);
 }
-CLOCKSOURCE_OF_DECLARE(nomadik_mtu, "st,nomadik-mtu",
+TIMER_OF_DECLARE(nomadik_mtu, "st,nomadik-mtu",
 		       nmdk_timer_of_init);
diff --git a/drivers/clocksource/owl-timer.c b/drivers/clocksource/owl-timer.c
new file mode 100644
index 000000000000..d19c53c11094
--- /dev/null
+++ b/drivers/clocksource/owl-timer.c
@@ -0,0 +1,172 @@
+/*
+ * Actions Semi Owl timer
+ *
+ * Copyright 2012 Actions Semi Inc.
+ * Author: Actions Semi, Inc.
+ *
+ * Copyright (c) 2017 SUSE Linux GmbH
+ * Author: Andreas Färber
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the 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/clockchips.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/irqreturn.h>
+#include <linux/sched_clock.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+
+#define OWL_Tx_CTL		0x0
+#define OWL_Tx_CMP		0x4
+#define OWL_Tx_VAL		0x8
+
+#define OWL_Tx_CTL_PD		BIT(0)
+#define OWL_Tx_CTL_INTEN	BIT(1)
+#define OWL_Tx_CTL_EN		BIT(2)
+
+static void __iomem *owl_timer_base;
+static void __iomem *owl_clksrc_base;
+static void __iomem *owl_clkevt_base;
+
+static inline void owl_timer_reset(void __iomem *base)
+{
+	writel(0, base + OWL_Tx_CTL);
+	writel(0, base + OWL_Tx_VAL);
+	writel(0, base + OWL_Tx_CMP);
+}
+
+static inline void owl_timer_set_enabled(void __iomem *base, bool enabled)
+{
+	u32 ctl = readl(base + OWL_Tx_CTL);
+
+	/* PD bit is cleared when set */
+	ctl &= ~OWL_Tx_CTL_PD;
+
+	if (enabled)
+		ctl |= OWL_Tx_CTL_EN;
+	else
+		ctl &= ~OWL_Tx_CTL_EN;
+
+	writel(ctl, base + OWL_Tx_CTL);
+}
+
+static u64 notrace owl_timer_sched_read(void)
+{
+	return (u64)readl(owl_clksrc_base + OWL_Tx_VAL);
+}
+
+static int owl_timer_set_state_shutdown(struct clock_event_device *evt)
+{
+	owl_timer_set_enabled(owl_clkevt_base, false);
+
+	return 0;
+}
+
+static int owl_timer_set_state_oneshot(struct clock_event_device *evt)
+{
+	owl_timer_reset(owl_clkevt_base);
+
+	return 0;
+}
+
+static int owl_timer_tick_resume(struct clock_event_device *evt)
+{
+	return 0;
+}
+
+static int owl_timer_set_next_event(unsigned long evt,
+				    struct clock_event_device *ev)
+{
+	void __iomem *base = owl_clkevt_base;
+
+	owl_timer_set_enabled(base, false);
+	writel(OWL_Tx_CTL_INTEN, base + OWL_Tx_CTL);
+	writel(0, base + OWL_Tx_VAL);
+	writel(evt, base + OWL_Tx_CMP);
+	owl_timer_set_enabled(base, true);
+
+	return 0;
+}
+
+static struct clock_event_device owl_clockevent = {
+	.name			= "owl_tick",
+	.rating			= 200,
+	.features		= CLOCK_EVT_FEAT_ONESHOT |
+				  CLOCK_EVT_FEAT_DYNIRQ,
+	.set_state_shutdown	= owl_timer_set_state_shutdown,
+	.set_state_oneshot	= owl_timer_set_state_oneshot,
+	.tick_resume		= owl_timer_tick_resume,
+	.set_next_event		= owl_timer_set_next_event,
+};
+
+static irqreturn_t owl_timer1_interrupt(int irq, void *dev_id)
+{
+	struct clock_event_device *evt = (struct clock_event_device *)dev_id;
+
+	writel(OWL_Tx_CTL_PD, owl_clkevt_base + OWL_Tx_CTL);
+
+	evt->event_handler(evt);
+
+	return IRQ_HANDLED;
+}
+
+static int __init owl_timer_init(struct device_node *node)
+{
+	struct clk *clk;
+	unsigned long rate;
+	int timer1_irq, ret;
+
+	owl_timer_base = of_io_request_and_map(node, 0, "owl-timer");
+	if (IS_ERR(owl_timer_base)) {
+		pr_err("Can't map timer registers");
+		return PTR_ERR(owl_timer_base);
+	}
+
+	owl_clksrc_base = owl_timer_base + 0x08;
+	owl_clkevt_base = owl_timer_base + 0x14;
+
+	timer1_irq = of_irq_get_byname(node, "timer1");
+	if (timer1_irq <= 0) {
+		pr_err("Can't parse timer1 IRQ");
+		return -EINVAL;
+	}
+
+	clk = of_clk_get(node, 0);
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	rate = clk_get_rate(clk);
+
+	owl_timer_reset(owl_clksrc_base);
+	owl_timer_set_enabled(owl_clksrc_base, true);
+
+	sched_clock_register(owl_timer_sched_read, 32, rate);
+	clocksource_mmio_init(owl_clksrc_base + OWL_Tx_VAL, node->name,
+			      rate, 200, 32, clocksource_mmio_readl_up);
+
+	owl_timer_reset(owl_clkevt_base);
+
+	ret = request_irq(timer1_irq, owl_timer1_interrupt, IRQF_TIMER,
+			  "owl-timer", &owl_clockevent);
+	if (ret) {
+		pr_err("failed to request irq %d\n", timer1_irq);
+		return ret;
+	}
+
+	owl_clockevent.cpumask = cpumask_of(0);
+	owl_clockevent.irq = timer1_irq;
+
+	clockevents_config_and_register(&owl_clockevent, rate,
+					0xf, 0xffffffff);
+
+	return 0;
+}
+CLOCKSOURCE_OF_DECLARE(owl_s500, "actions,s500-timer", owl_timer_init);
+CLOCKSOURCE_OF_DECLARE(owl_s900, "actions,s900-timer", owl_timer_init);
diff --git a/drivers/clocksource/pxa_timer.c b/drivers/clocksource/pxa_timer.c
index a10fa667325f..08cd6eaf3795 100644
--- a/drivers/clocksource/pxa_timer.c
+++ b/drivers/clocksource/pxa_timer.c
@@ -216,7 +216,7 @@ static int __init pxa_timer_dt_init(struct device_node *np)
 
 	return pxa_timer_common_init(irq, clk_get_rate(clk));
 }
-CLOCKSOURCE_OF_DECLARE(pxa_timer, "marvell,pxa-timer", pxa_timer_dt_init);
+TIMER_OF_DECLARE(pxa_timer, "marvell,pxa-timer", pxa_timer_dt_init);
 
 /*
  * Legacy timer init for non device-tree boards.
diff --git a/drivers/clocksource/qcom-timer.c b/drivers/clocksource/qcom-timer.c
index ee358cdf4a07..89816f89ff3f 100644
--- a/drivers/clocksource/qcom-timer.c
+++ b/drivers/clocksource/qcom-timer.c
@@ -254,5 +254,5 @@ static int __init msm_dt_timer_init(struct device_node *np)
 
 	return msm_timer_init(freq, 32, irq, !!percpu_offset);
 }
-CLOCKSOURCE_OF_DECLARE(kpss_timer, "qcom,kpss-timer", msm_dt_timer_init);
-CLOCKSOURCE_OF_DECLARE(scss_timer, "qcom,scss-timer", msm_dt_timer_init);
+TIMER_OF_DECLARE(kpss_timer, "qcom,kpss-timer", msm_dt_timer_init);
+TIMER_OF_DECLARE(scss_timer, "qcom,scss-timer", msm_dt_timer_init);
diff --git a/drivers/clocksource/renesas-ostm.c b/drivers/clocksource/renesas-ostm.c
index c76f57668fb2..6cffd7c6001a 100644
--- a/drivers/clocksource/renesas-ostm.c
+++ b/drivers/clocksource/renesas-ostm.c
@@ -262,4 +262,4 @@ err:
 	return 0;
 }
 
-CLOCKSOURCE_OF_DECLARE(ostm, "renesas,ostm", ostm_init);
+TIMER_OF_DECLARE(ostm, "renesas,ostm", ostm_init);
diff --git a/drivers/clocksource/rockchip_timer.c b/drivers/clocksource/rockchip_timer.c
index 49c02be50eca..c27f4c850d83 100644
--- a/drivers/clocksource/rockchip_timer.c
+++ b/drivers/clocksource/rockchip_timer.c
@@ -303,5 +303,5 @@ static int __init rk_timer_init(struct device_node *np)
 	return -EINVAL;
 }
 
-CLOCKSOURCE_OF_DECLARE(rk3288_timer, "rockchip,rk3288-timer", rk_timer_init);
-CLOCKSOURCE_OF_DECLARE(rk3399_timer, "rockchip,rk3399-timer", rk_timer_init);
+TIMER_OF_DECLARE(rk3288_timer, "rockchip,rk3288-timer", rk_timer_init);
+TIMER_OF_DECLARE(rk3399_timer, "rockchip,rk3399-timer", rk_timer_init);
diff --git a/drivers/clocksource/samsung_pwm_timer.c b/drivers/clocksource/samsung_pwm_timer.c
index a68e6538c809..6d5d126357c2 100644
--- a/drivers/clocksource/samsung_pwm_timer.c
+++ b/drivers/clocksource/samsung_pwm_timer.c
@@ -418,7 +418,7 @@ void __init samsung_pwm_clocksource_init(void __iomem *base,
 	_samsung_pwm_clocksource_init();
 }
 
-#ifdef CONFIG_CLKSRC_OF
+#ifdef CONFIG_TIMER_OF
 static int __init samsung_pwm_alloc(struct device_node *np,
 				    const struct samsung_pwm_variant *variant)
 {
@@ -466,7 +466,7 @@ static int __init s3c2410_pwm_clocksource_init(struct device_node *np)
 {
 	return samsung_pwm_alloc(np, &s3c24xx_variant);
 }
-CLOCKSOURCE_OF_DECLARE(s3c2410_pwm, "samsung,s3c2410-pwm", s3c2410_pwm_clocksource_init);
+TIMER_OF_DECLARE(s3c2410_pwm, "samsung,s3c2410-pwm", s3c2410_pwm_clocksource_init);
 
 static const struct samsung_pwm_variant s3c64xx_variant = {
 	.bits		= 32,
@@ -479,7 +479,7 @@ static int __init s3c64xx_pwm_clocksource_init(struct device_node *np)
 {
 	return samsung_pwm_alloc(np, &s3c64xx_variant);
 }
-CLOCKSOURCE_OF_DECLARE(s3c6400_pwm, "samsung,s3c6400-pwm", s3c64xx_pwm_clocksource_init);
+TIMER_OF_DECLARE(s3c6400_pwm, "samsung,s3c6400-pwm", s3c64xx_pwm_clocksource_init);
 
 static const struct samsung_pwm_variant s5p64x0_variant = {
 	.bits		= 32,
@@ -492,7 +492,7 @@ static int __init s5p64x0_pwm_clocksource_init(struct device_node *np)
 {
 	return samsung_pwm_alloc(np, &s5p64x0_variant);
 }
-CLOCKSOURCE_OF_DECLARE(s5p6440_pwm, "samsung,s5p6440-pwm", s5p64x0_pwm_clocksource_init);
+TIMER_OF_DECLARE(s5p6440_pwm, "samsung,s5p6440-pwm", s5p64x0_pwm_clocksource_init);
 
 static const struct samsung_pwm_variant s5p_variant = {
 	.bits		= 32,
@@ -505,5 +505,5 @@ static int __init s5p_pwm_clocksource_init(struct device_node *np)
 {
 	return samsung_pwm_alloc(np, &s5p_variant);
 }
-CLOCKSOURCE_OF_DECLARE(s5pc100_pwm, "samsung,s5pc100-pwm", s5p_pwm_clocksource_init);
+TIMER_OF_DECLARE(s5pc100_pwm, "samsung,s5pc100-pwm", s5p_pwm_clocksource_init);
 #endif
diff --git a/drivers/clocksource/sun4i_timer.c b/drivers/clocksource/sun4i_timer.c
index 4452d5c8f304..6e0180aaf784 100644
--- a/drivers/clocksource/sun4i_timer.c
+++ b/drivers/clocksource/sun4i_timer.c
@@ -24,6 +24,8 @@
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
 
+#include "timer-of.h"
+
 #define TIMER_IRQ_EN_REG	0x00
 #define TIMER_IRQ_EN(val)		BIT(val)
 #define TIMER_IRQ_ST_REG	0x04
@@ -39,38 +41,37 @@
 
 #define TIMER_SYNC_TICKS	3
 
-static void __iomem *timer_base;
-static u32 ticks_per_jiffy;
-
 /*
  * When we disable a timer, we need to wait at least for 2 cycles of
  * the timer source clock. We will use for that the clocksource timer
  * that is already setup and runs at the same frequency than the other
  * timers, and we never will be disabled.
  */
-static void sun4i_clkevt_sync(void)
+static void sun4i_clkevt_sync(void __iomem *base)
 {
-	u32 old = readl(timer_base + TIMER_CNTVAL_REG(1));
+	u32 old = readl(base + TIMER_CNTVAL_REG(1));
 
-	while ((old - readl(timer_base + TIMER_CNTVAL_REG(1))) < TIMER_SYNC_TICKS)
+	while ((old - readl(base + TIMER_CNTVAL_REG(1))) < TIMER_SYNC_TICKS)
 		cpu_relax();
 }
 
-static void sun4i_clkevt_time_stop(u8 timer)
+static void sun4i_clkevt_time_stop(void __iomem *base, u8 timer)
 {
-	u32 val = readl(timer_base + TIMER_CTL_REG(timer));
-	writel(val & ~TIMER_CTL_ENABLE, timer_base + TIMER_CTL_REG(timer));
-	sun4i_clkevt_sync();
+	u32 val = readl(base + TIMER_CTL_REG(timer));
+	writel(val & ~TIMER_CTL_ENABLE, base + TIMER_CTL_REG(timer));
+	sun4i_clkevt_sync(base);
 }
 
-static void sun4i_clkevt_time_setup(u8 timer, unsigned long delay)
+static void sun4i_clkevt_time_setup(void __iomem *base, u8 timer,
+				    unsigned long delay)
 {
-	writel(delay, timer_base + TIMER_INTVAL_REG(timer));
+	writel(delay, base + TIMER_INTVAL_REG(timer));
 }
 
-static void sun4i_clkevt_time_start(u8 timer, bool periodic)
+static void sun4i_clkevt_time_start(void __iomem *base, u8 timer,
+				    bool periodic)
 {
-	u32 val = readl(timer_base + TIMER_CTL_REG(timer));
+	u32 val = readl(base + TIMER_CTL_REG(timer));
 
 	if (periodic)
 		val &= ~TIMER_CTL_ONESHOT;
@@ -78,115 +79,106 @@ static void sun4i_clkevt_time_start(u8 timer, bool periodic)
 		val |= TIMER_CTL_ONESHOT;
 
 	writel(val | TIMER_CTL_ENABLE | TIMER_CTL_RELOAD,
-	       timer_base + TIMER_CTL_REG(timer));
+	       base + TIMER_CTL_REG(timer));
 }
 
 static int sun4i_clkevt_shutdown(struct clock_event_device *evt)
 {
-	sun4i_clkevt_time_stop(0);
+	struct timer_of *to = to_timer_of(evt);
+
+	sun4i_clkevt_time_stop(timer_of_base(to), 0);
+
 	return 0;
 }
 
 static int sun4i_clkevt_set_oneshot(struct clock_event_device *evt)
 {
-	sun4i_clkevt_time_stop(0);
-	sun4i_clkevt_time_start(0, false);
+	struct timer_of *to = to_timer_of(evt);
+
+	sun4i_clkevt_time_stop(timer_of_base(to), 0);
+	sun4i_clkevt_time_start(timer_of_base(to), 0, false);
+
 	return 0;
 }
 
 static int sun4i_clkevt_set_periodic(struct clock_event_device *evt)
 {
-	sun4i_clkevt_time_stop(0);
-	sun4i_clkevt_time_setup(0, ticks_per_jiffy);
-	sun4i_clkevt_time_start(0, true);
+	struct timer_of *to = to_timer_of(evt);
+
+	sun4i_clkevt_time_stop(timer_of_base(to), 0);
+	sun4i_clkevt_time_setup(timer_of_base(to), 0, timer_of_period(to));
+	sun4i_clkevt_time_start(timer_of_base(to), 0, true);
+
 	return 0;
 }
 
 static int sun4i_clkevt_next_event(unsigned long evt,
-				   struct clock_event_device *unused)
+				   struct clock_event_device *clkevt)
 {
-	sun4i_clkevt_time_stop(0);
-	sun4i_clkevt_time_setup(0, evt - TIMER_SYNC_TICKS);
-	sun4i_clkevt_time_start(0, false);
+	struct timer_of *to = to_timer_of(clkevt);
+
+	sun4i_clkevt_time_stop(timer_of_base(to), 0);
+	sun4i_clkevt_time_setup(timer_of_base(to), 0, evt - TIMER_SYNC_TICKS);
+	sun4i_clkevt_time_start(timer_of_base(to), 0, false);
 
 	return 0;
 }
 
-static struct clock_event_device sun4i_clockevent = {
-	.name = "sun4i_tick",
-	.rating = 350,
-	.features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
-	.set_state_shutdown = sun4i_clkevt_shutdown,
-	.set_state_periodic = sun4i_clkevt_set_periodic,
-	.set_state_oneshot = sun4i_clkevt_set_oneshot,
-	.tick_resume = sun4i_clkevt_shutdown,
-	.set_next_event = sun4i_clkevt_next_event,
-};
-
-static void sun4i_timer_clear_interrupt(void)
+static void sun4i_timer_clear_interrupt(void __iomem *base)
 {
-	writel(TIMER_IRQ_EN(0), timer_base + TIMER_IRQ_ST_REG);
+	writel(TIMER_IRQ_EN(0), base + TIMER_IRQ_ST_REG);
 }
 
 static irqreturn_t sun4i_timer_interrupt(int irq, void *dev_id)
 {
 	struct clock_event_device *evt = (struct clock_event_device *)dev_id;
+	struct timer_of *to = to_timer_of(evt);
 
-	sun4i_timer_clear_interrupt();
+	sun4i_timer_clear_interrupt(timer_of_base(to));
 	evt->event_handler(evt);
 
 	return IRQ_HANDLED;
 }
 
-static struct irqaction sun4i_timer_irq = {
-	.name = "sun4i_timer0",
-	.flags = IRQF_TIMER | IRQF_IRQPOLL,
-	.handler = sun4i_timer_interrupt,
-	.dev_id = &sun4i_clockevent,
+static struct timer_of to = {
+	.flags = TIMER_OF_IRQ | TIMER_OF_CLOCK | TIMER_OF_BASE,
+
+	.clkevt = {
+		.name = "sun4i_tick",
+		.rating = 350,
+		.features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
+		.set_state_shutdown = sun4i_clkevt_shutdown,
+		.set_state_periodic = sun4i_clkevt_set_periodic,
+		.set_state_oneshot = sun4i_clkevt_set_oneshot,
+		.tick_resume = sun4i_clkevt_shutdown,
+		.set_next_event = sun4i_clkevt_next_event,
+		.cpumask = cpu_possible_mask,
+	},
+
+	.of_irq = {
+		.handler = sun4i_timer_interrupt,
+		.flags = IRQF_TIMER | IRQF_IRQPOLL,
+	},
 };
 
 static u64 notrace sun4i_timer_sched_read(void)
 {
-	return ~readl(timer_base + TIMER_CNTVAL_REG(1));
+	return ~readl(timer_of_base(&to) + TIMER_CNTVAL_REG(1));
 }
 
 static int __init sun4i_timer_init(struct device_node *node)
 {
-	unsigned long rate = 0;
-	struct clk *clk;
-	int ret, irq;
+	int ret;
 	u32 val;
 
-	timer_base = of_iomap(node, 0);
-	if (!timer_base) {
-		pr_crit("Can't map registers\n");
-		return -ENXIO;
-	}
-
-	irq = irq_of_parse_and_map(node, 0);
-	if (irq <= 0) {
-		pr_crit("Can't parse IRQ\n");
-		return -EINVAL;
-	}
-
-	clk = of_clk_get(node, 0);
-	if (IS_ERR(clk)) {
-		pr_crit("Can't get timer clock\n");
-		return PTR_ERR(clk);
-	}
-
-	ret = clk_prepare_enable(clk);
-	if (ret) {
-		pr_err("Failed to prepare clock\n");
+	ret = timer_of_init(node, &to);
+	if (ret)
 		return ret;
-	}
-
-	rate = clk_get_rate(clk);
 
-	writel(~0, timer_base + TIMER_INTVAL_REG(1));
+	writel(~0, timer_of_base(&to) + TIMER_INTVAL_REG(1));
 	writel(TIMER_CTL_ENABLE | TIMER_CTL_RELOAD |
 	       TIMER_CTL_CLK_SRC(TIMER_CTL_CLK_SRC_OSC24M),
-	       timer_base + TIMER_CTL_REG(1));
+	       timer_of_base(&to) + TIMER_CTL_REG(1));
 
 	/*
 	 * sched_clock_register does not have priorities, and on sun6i and
@@ -195,43 +187,34 @@ static int __init sun4i_timer_init(struct device_node *node)
 	if (of_machine_is_compatible("allwinner,sun4i-a10") ||
 	    of_machine_is_compatible("allwinner,sun5i-a13") ||
 	    of_machine_is_compatible("allwinner,sun5i-a10s"))
-		sched_clock_register(sun4i_timer_sched_read, 32, rate);
+		sched_clock_register(sun4i_timer_sched_read, 32,
+				     timer_of_rate(&to));
 
-	ret = clocksource_mmio_init(timer_base + TIMER_CNTVAL_REG(1), node->name,
-				    rate, 350, 32, clocksource_mmio_readl_down);
+	ret = clocksource_mmio_init(timer_of_base(&to) + TIMER_CNTVAL_REG(1),
+				    node->name, timer_of_rate(&to), 350, 32,
+				    clocksource_mmio_readl_down);
 	if (ret) {
 		pr_err("Failed to register clocksource\n");
 		return ret;
 	}
 
-	ticks_per_jiffy = DIV_ROUND_UP(rate, HZ);
-
 	writel(TIMER_CTL_CLK_SRC(TIMER_CTL_CLK_SRC_OSC24M),
-	       timer_base + TIMER_CTL_REG(0));
+	       timer_of_base(&to) + TIMER_CTL_REG(0));
 
 	/* Make sure timer is stopped before playing with interrupts */
-	sun4i_clkevt_time_stop(0);
+	sun4i_clkevt_time_stop(timer_of_base(&to), 0);
 
 	/* clear timer0 interrupt */
-	sun4i_timer_clear_interrupt();
-
-	sun4i_clockevent.cpumask = cpu_possible_mask;
-	sun4i_clockevent.irq = irq;
+	sun4i_timer_clear_interrupt(timer_of_base(&to));
 
-	clockevents_config_and_register(&sun4i_clockevent, rate,
+	clockevents_config_and_register(&to.clkevt, timer_of_rate(&to),
 					TIMER_SYNC_TICKS, 0xffffffff);
 
-	ret = setup_irq(irq, &sun4i_timer_irq);
-	if (ret) {
-		pr_err("failed to setup irq %d\n", irq);
-		return ret;
-	}
-
 	/* Enable timer0 interrupt */
-	val = readl(timer_base + TIMER_IRQ_EN_REG);
-	writel(val | TIMER_IRQ_EN(0), timer_base + TIMER_IRQ_EN_REG);
+	val = readl(timer_of_base(&to) + TIMER_IRQ_EN_REG);
+	writel(val | TIMER_IRQ_EN(0), timer_of_base(&to) + TIMER_IRQ_EN_REG);
 
 	return ret;
 }
-CLOCKSOURCE_OF_DECLARE(sun4i, "allwinner,sun4i-a10-timer",
+TIMER_OF_DECLARE(sun4i, "allwinner,sun4i-a10-timer",
 		       sun4i_timer_init);
diff --git a/drivers/clocksource/tango_xtal.c b/drivers/clocksource/tango_xtal.c
index 12fcef8cf2d3..c4e1c2e6046f 100644
--- a/drivers/clocksource/tango_xtal.c
+++ b/drivers/clocksource/tango_xtal.c
@@ -53,4 +53,4 @@ static int __init tango_clocksource_init(struct device_node *np)
 	return 0;
 }
 
-CLOCKSOURCE_OF_DECLARE(tango, "sigma,tick-counter", tango_clocksource_init);
+TIMER_OF_DECLARE(tango, "sigma,tick-counter", tango_clocksource_init);
diff --git a/drivers/clocksource/tcb_clksrc.c b/drivers/clocksource/tcb_clksrc.c
index d4ca9962a759..59e8aee0ec16 100644
--- a/drivers/clocksource/tcb_clksrc.c
+++ b/drivers/clocksource/tcb_clksrc.c
@@ -9,6 +9,7 @@
 #include <linux/ioport.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
+#include <linux/syscore_ops.h>
 #include <linux/atmel_tc.h>
 
 
@@ -40,6 +41,14 @@
  */
 
 static void __iomem *tcaddr;
+static struct
+{
+	u32 cmr;
+	u32 imr;
+	u32 rc;
+	bool clken;
+} tcb_cache[3];
+static u32 bmr_cache;
 
 static u64 tc_get_cycles(struct clocksource *cs)
 {
@@ -48,9 +57,9 @@ static u64 tc_get_cycles(struct clocksource *cs)
 
 	raw_local_irq_save(flags);
 	do {
-		upper = __raw_readl(tcaddr + ATMEL_TC_REG(1, CV));
-		lower = __raw_readl(tcaddr + ATMEL_TC_REG(0, CV));
-	} while (upper != __raw_readl(tcaddr + ATMEL_TC_REG(1, CV)));
+		upper = readl_relaxed(tcaddr + ATMEL_TC_REG(1, CV));
+		lower = readl_relaxed(tcaddr + ATMEL_TC_REG(0, CV));
+	} while (upper != readl_relaxed(tcaddr + ATMEL_TC_REG(1, CV)));
 
 	raw_local_irq_restore(flags);
 	return (upper << 16) | lower;
@@ -58,7 +67,47 @@ static u64 tc_get_cycles(struct clocksource *cs)
 
 static u64 tc_get_cycles32(struct clocksource *cs)
 {
-	return __raw_readl(tcaddr + ATMEL_TC_REG(0, CV));
+	return readl_relaxed(tcaddr + ATMEL_TC_REG(0, CV));
+}
+
+void tc_clksrc_suspend(struct clocksource *cs)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(tcb_cache); i++) {
+		tcb_cache[i].cmr = readl(tcaddr + ATMEL_TC_REG(i, CMR));
+		tcb_cache[i].imr = readl(tcaddr + ATMEL_TC_REG(i, IMR));
+		tcb_cache[i].rc = readl(tcaddr + ATMEL_TC_REG(i, RC));
+		tcb_cache[i].clken = !!(readl(tcaddr + ATMEL_TC_REG(i, SR)) &
+					ATMEL_TC_CLKSTA);
+	}
+
+	bmr_cache = readl(tcaddr + ATMEL_TC_BMR);
+}
+
+void tc_clksrc_resume(struct clocksource *cs)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(tcb_cache); i++) {
+		/* Restore registers for the channel, RA and RB are not used  */
+		writel(tcb_cache[i].cmr, tcaddr + ATMEL_TC_REG(i, CMR));
+		writel(tcb_cache[i].rc, tcaddr + ATMEL_TC_REG(i, RC));
+		writel(0, tcaddr + ATMEL_TC_REG(i, RA));
+		writel(0, tcaddr + ATMEL_TC_REG(i, RB));
+		/* Disable all the interrupts */
+		writel(0xff, tcaddr + ATMEL_TC_REG(i, IDR));
+		/* Reenable interrupts that were enabled before suspending */
+		writel(tcb_cache[i].imr, tcaddr + ATMEL_TC_REG(i, IER));
+		/* Start the clock if it was used */
+		if (tcb_cache[i].clken)
+			writel(ATMEL_TC_CLKEN, tcaddr + ATMEL_TC_REG(i, CCR));
+	}
+
+	/* Dual channel, chain channels */
+	writel(bmr_cache, tcaddr + ATMEL_TC_BMR);
+	/* Finally, trigger all the channels*/
+	writel(ATMEL_TC_SYNC, tcaddr + ATMEL_TC_BCR);
 }
 
 static struct clocksource clksrc = {
@@ -67,6 +116,8 @@ static struct clocksource clksrc = {
 	.read           = tc_get_cycles,
 	.mask           = CLOCKSOURCE_MASK(32),
 	.flags		= CLOCK_SOURCE_IS_CONTINUOUS,
+	.suspend	= tc_clksrc_suspend,
+	.resume		= tc_clksrc_resume,
 };
 
 #ifdef CONFIG_GENERIC_CLOCKEVENTS
@@ -96,8 +147,8 @@ static int tc_shutdown(struct clock_event_device *d)
 	struct tc_clkevt_device *tcd = to_tc_clkevt(d);
 	void __iomem		*regs = tcd->regs;
 
-	__raw_writel(0xff, regs + ATMEL_TC_REG(2, IDR));
-	__raw_writel(ATMEL_TC_CLKDIS, regs + ATMEL_TC_REG(2, CCR));
+	writel(0xff, regs + ATMEL_TC_REG(2, IDR));
+	writel(ATMEL_TC_CLKDIS, regs + ATMEL_TC_REG(2, CCR));
 	if (!clockevent_state_detached(d))
 		clk_disable(tcd->clk);
 
@@ -115,9 +166,9 @@ static int tc_set_oneshot(struct clock_event_device *d)
 	clk_enable(tcd->clk);
 
 	/* slow clock, count up to RC, then irq and stop */
-	__raw_writel(timer_clock | ATMEL_TC_CPCSTOP | ATMEL_TC_WAVE |
+	writel(timer_clock | ATMEL_TC_CPCSTOP | ATMEL_TC_WAVE |
 		     ATMEL_TC_WAVESEL_UP_AUTO, regs + ATMEL_TC_REG(2, CMR));
-	__raw_writel(ATMEL_TC_CPCS, regs + ATMEL_TC_REG(2, IER));
+	writel(ATMEL_TC_CPCS, regs + ATMEL_TC_REG(2, IER));
 
 	/* set_next_event() configures and starts the timer */
 	return 0;
@@ -137,25 +188,25 @@ static int tc_set_periodic(struct clock_event_device *d)
 	clk_enable(tcd->clk);
 
 	/* slow clock, count up to RC, then irq and restart */
-	__raw_writel(timer_clock | ATMEL_TC_WAVE | ATMEL_TC_WAVESEL_UP_AUTO,
+	writel(timer_clock | ATMEL_TC_WAVE | ATMEL_TC_WAVESEL_UP_AUTO,
 		     regs + ATMEL_TC_REG(2, CMR));
-	__raw_writel((32768 + HZ / 2) / HZ, tcaddr + ATMEL_TC_REG(2, RC));
+	writel((32768 + HZ / 2) / HZ, tcaddr + ATMEL_TC_REG(2, RC));
 
 	/* Enable clock and interrupts on RC compare */
-	__raw_writel(ATMEL_TC_CPCS, regs + ATMEL_TC_REG(2, IER));
+	writel(ATMEL_TC_CPCS, regs + ATMEL_TC_REG(2, IER));
 
 	/* go go gadget! */
-	__raw_writel(ATMEL_TC_CLKEN | ATMEL_TC_SWTRG, regs +
+	writel(ATMEL_TC_CLKEN | ATMEL_TC_SWTRG, regs +
 		     ATMEL_TC_REG(2, CCR));
 	return 0;
 }
 
 static int tc_next_event(unsigned long delta, struct clock_event_device *d)
 {
-	__raw_writel(delta, tcaddr + ATMEL_TC_REG(2, RC));
+	writel_relaxed(delta, tcaddr + ATMEL_TC_REG(2, RC));
 
 	/* go go gadget! */
-	__raw_writel(ATMEL_TC_CLKEN | ATMEL_TC_SWTRG,
+	writel_relaxed(ATMEL_TC_CLKEN | ATMEL_TC_SWTRG,
 			tcaddr + ATMEL_TC_REG(2, CCR));
 	return 0;
 }
@@ -179,7 +230,7 @@ static irqreturn_t ch2_irq(int irq, void *handle)
 	struct tc_clkevt_device	*dev = handle;
 	unsigned int		sr;
 
-	sr = __raw_readl(dev->regs + ATMEL_TC_REG(2, SR));
+	sr = readl_relaxed(dev->regs + ATMEL_TC_REG(2, SR));
 	if (sr & ATMEL_TC_CPCS) {
 		dev->clkevt.event_handler(&dev->clkevt);
 		return IRQ_HANDLED;
@@ -239,43 +290,43 @@ static int __init setup_clkevents(struct atmel_tc *tc, int clk32k_divisor_idx)
 static void __init tcb_setup_dual_chan(struct atmel_tc *tc, int mck_divisor_idx)
 {
 	/* channel 0:  waveform mode, input mclk/8, clock TIOA0 on overflow */
-	__raw_writel(mck_divisor_idx			/* likely divide-by-8 */
+	writel(mck_divisor_idx			/* likely divide-by-8 */
 			| ATMEL_TC_WAVE
 			| ATMEL_TC_WAVESEL_UP		/* free-run */
 			| ATMEL_TC_ACPA_SET		/* TIOA0 rises at 0 */
 			| ATMEL_TC_ACPC_CLEAR,		/* (duty cycle 50%) */
 			tcaddr + ATMEL_TC_REG(0, CMR));
-	__raw_writel(0x0000, tcaddr + ATMEL_TC_REG(0, RA));
-	__raw_writel(0x8000, tcaddr + ATMEL_TC_REG(0, RC));
-	__raw_writel(0xff, tcaddr + ATMEL_TC_REG(0, IDR));	/* no irqs */
-	__raw_writel(ATMEL_TC_CLKEN, tcaddr + ATMEL_TC_REG(0, CCR));
+	writel(0x0000, tcaddr + ATMEL_TC_REG(0, RA));
+	writel(0x8000, tcaddr + ATMEL_TC_REG(0, RC));
+	writel(0xff, tcaddr + ATMEL_TC_REG(0, IDR));	/* no irqs */
+	writel(ATMEL_TC_CLKEN, tcaddr + ATMEL_TC_REG(0, CCR));
 
 	/* channel 1:  waveform mode, input TIOA0 */
-	__raw_writel(ATMEL_TC_XC1			/* input: TIOA0 */
+	writel(ATMEL_TC_XC1			/* input: TIOA0 */
 			| ATMEL_TC_WAVE
 			| ATMEL_TC_WAVESEL_UP,		/* free-run */
 			tcaddr + ATMEL_TC_REG(1, CMR));
-	__raw_writel(0xff, tcaddr + ATMEL_TC_REG(1, IDR));	/* no irqs */
-	__raw_writel(ATMEL_TC_CLKEN, tcaddr + ATMEL_TC_REG(1, CCR));
+	writel(0xff, tcaddr + ATMEL_TC_REG(1, IDR));	/* no irqs */
+	writel(ATMEL_TC_CLKEN, tcaddr + ATMEL_TC_REG(1, CCR));
 
 	/* chain channel 0 to channel 1*/
-	__raw_writel(ATMEL_TC_TC1XC1S_TIOA0, tcaddr + ATMEL_TC_BMR);
+	writel(ATMEL_TC_TC1XC1S_TIOA0, tcaddr + ATMEL_TC_BMR);
 	/* then reset all the timers */
-	__raw_writel(ATMEL_TC_SYNC, tcaddr + ATMEL_TC_BCR);
+	writel(ATMEL_TC_SYNC, tcaddr + ATMEL_TC_BCR);
 }
 
 static void __init tcb_setup_single_chan(struct atmel_tc *tc, int mck_divisor_idx)
 {
 	/* channel 0:  waveform mode, input mclk/8 */
-	__raw_writel(mck_divisor_idx			/* likely divide-by-8 */
+	writel(mck_divisor_idx			/* likely divide-by-8 */
 			| ATMEL_TC_WAVE
 			| ATMEL_TC_WAVESEL_UP,		/* free-run */
 			tcaddr + ATMEL_TC_REG(0, CMR));
-	__raw_writel(0xff, tcaddr + ATMEL_TC_REG(0, IDR));	/* no irqs */
-	__raw_writel(ATMEL_TC_CLKEN, tcaddr + ATMEL_TC_REG(0, CCR));
+	writel(0xff, tcaddr + ATMEL_TC_REG(0, IDR));	/* no irqs */
+	writel(ATMEL_TC_CLKEN, tcaddr + ATMEL_TC_REG(0, CCR));
 
 	/* then reset all the timers */
-	__raw_writel(ATMEL_TC_SYNC, tcaddr + ATMEL_TC_BCR);
+	writel(ATMEL_TC_SYNC, tcaddr + ATMEL_TC_BCR);
 }
 
 static int __init tcb_clksrc_init(void)
diff --git a/drivers/clocksource/tegra20_timer.c b/drivers/clocksource/tegra20_timer.c
index b9990b9c98c5..c337a8100a7b 100644
--- a/drivers/clocksource/tegra20_timer.c
+++ b/drivers/clocksource/tegra20_timer.c
@@ -237,7 +237,7 @@ static int __init tegra20_init_timer(struct device_node *np)
 
 	return 0;
 }
-CLOCKSOURCE_OF_DECLARE(tegra20_timer, "nvidia,tegra20-timer", tegra20_init_timer);
+TIMER_OF_DECLARE(tegra20_timer, "nvidia,tegra20-timer", tegra20_init_timer);
 
 static int __init tegra20_init_rtc(struct device_node *np)
 {
@@ -261,4 +261,4 @@ static int __init tegra20_init_rtc(struct device_node *np)
 
 	return register_persistent_clock(NULL, tegra_read_persistent_clock64);
 }
-CLOCKSOURCE_OF_DECLARE(tegra20_rtc, "nvidia,tegra20-rtc", tegra20_init_rtc);
+TIMER_OF_DECLARE(tegra20_rtc, "nvidia,tegra20-rtc", tegra20_init_rtc);
diff --git a/drivers/clocksource/time-armada-370-xp.c b/drivers/clocksource/time-armada-370-xp.c
index aea4380129ea..edf1a46269f1 100644
--- a/drivers/clocksource/time-armada-370-xp.c
+++ b/drivers/clocksource/time-armada-370-xp.c
@@ -351,7 +351,7 @@ static int __init armada_xp_timer_init(struct device_node *np)
 
 	return armada_370_xp_timer_common_init(np);
 }
-CLOCKSOURCE_OF_DECLARE(armada_xp, "marvell,armada-xp-timer",
+TIMER_OF_DECLARE(armada_xp, "marvell,armada-xp-timer",
 		       armada_xp_timer_init);
 
 static int __init armada_375_timer_init(struct device_node *np)
@@ -389,7 +389,7 @@ static int __init armada_375_timer_init(struct device_node *np)
 
 	return armada_370_xp_timer_common_init(np);
 }
-CLOCKSOURCE_OF_DECLARE(armada_375, "marvell,armada-375-timer",
+TIMER_OF_DECLARE(armada_375, "marvell,armada-375-timer",
 		       armada_375_timer_init);
 
 static int __init armada_370_timer_init(struct device_node *np)
@@ -412,5 +412,5 @@ static int __init armada_370_timer_init(struct device_node *np)
 
 	return armada_370_xp_timer_common_init(np);
 }
-CLOCKSOURCE_OF_DECLARE(armada_370, "marvell,armada-370-timer",
+TIMER_OF_DECLARE(armada_370, "marvell,armada-370-timer",
 		       armada_370_timer_init);
diff --git a/drivers/clocksource/time-efm32.c b/drivers/clocksource/time-efm32.c
index ce0f97b4e5db..257e810ec1ad 100644
--- a/drivers/clocksource/time-efm32.c
+++ b/drivers/clocksource/time-efm32.c
@@ -283,5 +283,5 @@ static int __init efm32_timer_init(struct device_node *np)
 
 	return ret;
 }
-CLOCKSOURCE_OF_DECLARE(efm32compat, "efm32,timer", efm32_timer_init);
-CLOCKSOURCE_OF_DECLARE(efm32, "energymicro,efm32-timer", efm32_timer_init);
+TIMER_OF_DECLARE(efm32compat, "efm32,timer", efm32_timer_init);
+TIMER_OF_DECLARE(efm32, "energymicro,efm32-timer", efm32_timer_init);
diff --git a/drivers/clocksource/time-lpc32xx.c b/drivers/clocksource/time-lpc32xx.c
index 9649cfdb9213..d51a62a79ef7 100644
--- a/drivers/clocksource/time-lpc32xx.c
+++ b/drivers/clocksource/time-lpc32xx.c
@@ -311,4 +311,4 @@ static int __init lpc32xx_timer_init(struct device_node *np)
 
 	return ret;
 }
-CLOCKSOURCE_OF_DECLARE(lpc32xx_timer, "nxp,lpc3220-timer", lpc32xx_timer_init);
+TIMER_OF_DECLARE(lpc32xx_timer, "nxp,lpc3220-timer", lpc32xx_timer_init);
diff --git a/drivers/clocksource/time-orion.c b/drivers/clocksource/time-orion.c
index b9b97f630c4d..12202067fe4b 100644
--- a/drivers/clocksource/time-orion.c
+++ b/drivers/clocksource/time-orion.c
@@ -189,4 +189,4 @@ static int __init orion_timer_init(struct device_node *np)
 
 	return 0;
 }
-CLOCKSOURCE_OF_DECLARE(orion_timer, "marvell,orion-timer", orion_timer_init);
+TIMER_OF_DECLARE(orion_timer, "marvell,orion-timer", orion_timer_init);
diff --git a/drivers/clocksource/time-pistachio.c b/drivers/clocksource/time-pistachio.c
index 3710e4d9dcba..a2dd85d0c1d7 100644
--- a/drivers/clocksource/time-pistachio.c
+++ b/drivers/clocksource/time-pistachio.c
@@ -214,5 +214,5 @@ static int __init pistachio_clksrc_of_init(struct device_node *node)
 	sched_clock_register(pistachio_read_sched_clock, 32, rate);
 	return clocksource_register_hz(&pcs_gpt.cs, rate);
 }
-CLOCKSOURCE_OF_DECLARE(pistachio_gptimer, "img,pistachio-gptimer",
+TIMER_OF_DECLARE(pistachio_gptimer, "img,pistachio-gptimer",
 		       pistachio_clksrc_of_init);
diff --git a/drivers/clocksource/timer-atlas7.c b/drivers/clocksource/timer-atlas7.c
index 50300eec4a39..62c4bbc55a7e 100644
--- a/drivers/clocksource/timer-atlas7.c
+++ b/drivers/clocksource/timer-atlas7.c
@@ -283,4 +283,4 @@ static int __init sirfsoc_of_timer_init(struct device_node *np)
 
 	return sirfsoc_atlas7_timer_init(np);
 }
-CLOCKSOURCE_OF_DECLARE(sirfsoc_atlas7_timer, "sirf,atlas7-tick", sirfsoc_of_timer_init);
+TIMER_OF_DECLARE(sirfsoc_atlas7_timer, "sirf,atlas7-tick", sirfsoc_of_timer_init);
diff --git a/drivers/clocksource/timer-atmel-pit.c b/drivers/clocksource/timer-atmel-pit.c
index cc112351dc70..ec8a4376f74f 100644
--- a/drivers/clocksource/timer-atmel-pit.c
+++ b/drivers/clocksource/timer-atmel-pit.c
@@ -255,5 +255,5 @@ static int __init at91sam926x_pit_dt_init(struct device_node *node)
 
 	return 0;
 }
-CLOCKSOURCE_OF_DECLARE(at91sam926x_pit, "atmel,at91sam9260-pit",
+TIMER_OF_DECLARE(at91sam926x_pit, "atmel,at91sam9260-pit",
 		       at91sam926x_pit_dt_init);
diff --git a/drivers/clocksource/timer-atmel-st.c b/drivers/clocksource/timer-atmel-st.c
index be4ac7604136..d2e660f475af 100644
--- a/drivers/clocksource/timer-atmel-st.c
+++ b/drivers/clocksource/timer-atmel-st.c
@@ -260,5 +260,5 @@ static int __init atmel_st_timer_init(struct device_node *node)
 	/* register clocksource */
 	return clocksource_register_hz(&clk32k, sclk_rate);
 }
-CLOCKSOURCE_OF_DECLARE(atmel_st_timer, "atmel,at91rm9200-st",
+TIMER_OF_DECLARE(atmel_st_timer, "atmel,at91rm9200-st",
 		       atmel_st_timer_init);
diff --git a/drivers/clocksource/timer-digicolor.c b/drivers/clocksource/timer-digicolor.c
index 94a161eb9cce..1e984a4d8ad0 100644
--- a/drivers/clocksource/timer-digicolor.c
+++ b/drivers/clocksource/timer-digicolor.c
@@ -203,5 +203,5 @@ static int __init digicolor_timer_init(struct device_node *node)
 
 	return 0;
 }
-CLOCKSOURCE_OF_DECLARE(conexant_digicolor, "cnxt,cx92755-timer",
+TIMER_OF_DECLARE(conexant_digicolor, "cnxt,cx92755-timer",
 		       digicolor_timer_init);
diff --git a/drivers/clocksource/timer-fttmr010.c b/drivers/clocksource/timer-fttmr010.c
index b4a6f1e4bc54..66dd909960c6 100644
--- a/drivers/clocksource/timer-fttmr010.c
+++ b/drivers/clocksource/timer-fttmr010.c
@@ -11,12 +11,13 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
-#include <linux/mfd/syscon.h>
-#include <linux/regmap.h>
 #include <linux/clockchips.h>
 #include <linux/clocksource.h>
 #include <linux/sched_clock.h>
 #include <linux/clk.h>
+#include <linux/slab.h>
+#include <linux/bitops.h>
+#include <linux/delay.h>
 
 /*
  * Register definitions for the timers
@@ -37,267 +38,368 @@
 #define TIMER_INTR_STATE	(0x34)
 #define TIMER_INTR_MASK		(0x38)
 
-#define TIMER_1_CR_ENABLE	(1 << 0)
-#define TIMER_1_CR_CLOCK	(1 << 1)
-#define TIMER_1_CR_INT		(1 << 2)
-#define TIMER_2_CR_ENABLE	(1 << 3)
-#define TIMER_2_CR_CLOCK	(1 << 4)
-#define TIMER_2_CR_INT		(1 << 5)
-#define TIMER_3_CR_ENABLE	(1 << 6)
-#define TIMER_3_CR_CLOCK	(1 << 7)
-#define TIMER_3_CR_INT		(1 << 8)
-#define TIMER_1_CR_UPDOWN	(1 << 9)
-#define TIMER_2_CR_UPDOWN	(1 << 10)
-#define TIMER_3_CR_UPDOWN	(1 << 11)
-#define TIMER_DEFAULT_FLAGS	(TIMER_1_CR_UPDOWN | \
-				 TIMER_3_CR_ENABLE | \
-				 TIMER_3_CR_UPDOWN)
-
-#define TIMER_1_INT_MATCH1	(1 << 0)
-#define TIMER_1_INT_MATCH2	(1 << 1)
-#define TIMER_1_INT_OVERFLOW	(1 << 2)
-#define TIMER_2_INT_MATCH1	(1 << 3)
-#define TIMER_2_INT_MATCH2	(1 << 4)
-#define TIMER_2_INT_OVERFLOW	(1 << 5)
-#define TIMER_3_INT_MATCH1	(1 << 6)
-#define TIMER_3_INT_MATCH2	(1 << 7)
-#define TIMER_3_INT_OVERFLOW	(1 << 8)
+#define TIMER_1_CR_ENABLE	BIT(0)
+#define TIMER_1_CR_CLOCK	BIT(1)
+#define TIMER_1_CR_INT		BIT(2)
+#define TIMER_2_CR_ENABLE	BIT(3)
+#define TIMER_2_CR_CLOCK	BIT(4)
+#define TIMER_2_CR_INT		BIT(5)
+#define TIMER_3_CR_ENABLE	BIT(6)
+#define TIMER_3_CR_CLOCK	BIT(7)
+#define TIMER_3_CR_INT		BIT(8)
+#define TIMER_1_CR_UPDOWN	BIT(9)
+#define TIMER_2_CR_UPDOWN	BIT(10)
+#define TIMER_3_CR_UPDOWN	BIT(11)
+
+/*
+ * The Aspeed AST2400 moves bits around in the control register
+ * and lacks bits for setting the timer to count upwards.
+ */
+#define TIMER_1_CR_ASPEED_ENABLE	BIT(0)
+#define TIMER_1_CR_ASPEED_CLOCK		BIT(1)
+#define TIMER_1_CR_ASPEED_INT		BIT(2)
+#define TIMER_2_CR_ASPEED_ENABLE	BIT(4)
+#define TIMER_2_CR_ASPEED_CLOCK		BIT(5)
+#define TIMER_2_CR_ASPEED_INT		BIT(6)
+#define TIMER_3_CR_ASPEED_ENABLE	BIT(8)
+#define TIMER_3_CR_ASPEED_CLOCK		BIT(9)
+#define TIMER_3_CR_ASPEED_INT		BIT(10)
+
+#define TIMER_1_INT_MATCH1	BIT(0)
+#define TIMER_1_INT_MATCH2	BIT(1)
+#define TIMER_1_INT_OVERFLOW	BIT(2)
+#define TIMER_2_INT_MATCH1	BIT(3)
+#define TIMER_2_INT_MATCH2	BIT(4)
+#define TIMER_2_INT_OVERFLOW	BIT(5)
+#define TIMER_3_INT_MATCH1	BIT(6)
+#define TIMER_3_INT_MATCH2	BIT(7)
+#define TIMER_3_INT_OVERFLOW	BIT(8)
 #define TIMER_INT_ALL_MASK	0x1ff
 
-static unsigned int tick_rate;
-static void __iomem *base;
+struct fttmr010 {
+	void __iomem *base;
+	unsigned int tick_rate;
+	bool count_down;
+	u32 t1_enable_val;
+	struct clock_event_device clkevt;
+#ifdef CONFIG_ARM
+	struct delay_timer delay_timer;
+#endif
+};
+
+/*
+ * A local singleton used by sched_clock and delay timer reads, which are
+ * fast and stateless
+ */
+static struct fttmr010 *local_fttmr;
+
+static inline struct fttmr010 *to_fttmr010(struct clock_event_device *evt)
+{
+	return container_of(evt, struct fttmr010, clkevt);
+}
+
+static unsigned long fttmr010_read_current_timer_up(void)
+{
+	return readl(local_fttmr->base + TIMER2_COUNT);
+}
+
+static unsigned long fttmr010_read_current_timer_down(void)
+{
+	return ~readl(local_fttmr->base + TIMER2_COUNT);
+}
+
+static u64 notrace fttmr010_read_sched_clock_up(void)
+{
+	return fttmr010_read_current_timer_up();
+}
 
-static u64 notrace fttmr010_read_sched_clock(void)
+static u64 notrace fttmr010_read_sched_clock_down(void)
 {
-	return readl(base + TIMER3_COUNT);
+	return fttmr010_read_current_timer_down();
 }
 
 static int fttmr010_timer_set_next_event(unsigned long cycles,
 				       struct clock_event_device *evt)
 {
+	struct fttmr010 *fttmr010 = to_fttmr010(evt);
 	u32 cr;
 
-	/* Setup the match register */
-	cr = readl(base + TIMER1_COUNT);
-	writel(cr + cycles, base + TIMER1_MATCH1);
-	if (readl(base + TIMER1_COUNT) - cr > cycles)
-		return -ETIME;
+	/* Stop */
+	cr = readl(fttmr010->base + TIMER_CR);
+	cr &= ~fttmr010->t1_enable_val;
+	writel(cr, fttmr010->base + TIMER_CR);
+
+	/* Setup the match register forward/backward in time */
+	cr = readl(fttmr010->base + TIMER1_COUNT);
+	if (fttmr010->count_down)
+		cr -= cycles;
+	else
+		cr += cycles;
+	writel(cr, fttmr010->base + TIMER1_MATCH1);
+
+	/* Start */
+	cr = readl(fttmr010->base + TIMER_CR);
+	cr |= fttmr010->t1_enable_val;
+	writel(cr, fttmr010->base + TIMER_CR);
 
 	return 0;
 }
 
 static int fttmr010_timer_shutdown(struct clock_event_device *evt)
 {
+	struct fttmr010 *fttmr010 = to_fttmr010(evt);
 	u32 cr;
 
-	/*
-	 * Disable also for oneshot: the set_next() call will arm the timer
-	 * instead.
-	 */
-	/* Stop timer and interrupt. */
-	cr = readl(base + TIMER_CR);
-	cr &= ~(TIMER_1_CR_ENABLE | TIMER_1_CR_INT);
-	writel(cr, base + TIMER_CR);
+	/* Stop */
+	cr = readl(fttmr010->base + TIMER_CR);
+	cr &= ~fttmr010->t1_enable_val;
+	writel(cr, fttmr010->base + TIMER_CR);
+
+	return 0;
+}
+
+static int fttmr010_timer_set_oneshot(struct clock_event_device *evt)
+{
+	struct fttmr010 *fttmr010 = to_fttmr010(evt);
+	u32 cr;
+
+	/* Stop */
+	cr = readl(fttmr010->base + TIMER_CR);
+	cr &= ~fttmr010->t1_enable_val;
+	writel(cr, fttmr010->base + TIMER_CR);
 
-	/* Setup counter start from 0 */
-	writel(0, base + TIMER1_COUNT);
-	writel(0, base + TIMER1_LOAD);
+	/* Setup counter start from 0 or ~0 */
+	writel(0, fttmr010->base + TIMER1_COUNT);
+	if (fttmr010->count_down)
+		writel(~0, fttmr010->base + TIMER1_LOAD);
+	else
+		writel(0, fttmr010->base + TIMER1_LOAD);
 
-	/* enable interrupt */
-	cr = readl(base + TIMER_INTR_MASK);
+	/* Enable interrupt */
+	cr = readl(fttmr010->base + TIMER_INTR_MASK);
 	cr &= ~(TIMER_1_INT_OVERFLOW | TIMER_1_INT_MATCH2);
 	cr |= TIMER_1_INT_MATCH1;
-	writel(cr, base + TIMER_INTR_MASK);
-
-	/* start the timer */
-	cr = readl(base + TIMER_CR);
-	cr |= TIMER_1_CR_ENABLE;
-	writel(cr, base + TIMER_CR);
+	writel(cr, fttmr010->base + TIMER_INTR_MASK);
 
 	return 0;
 }
 
 static int fttmr010_timer_set_periodic(struct clock_event_device *evt)
 {
-	u32 period = DIV_ROUND_CLOSEST(tick_rate, HZ);
+	struct fttmr010 *fttmr010 = to_fttmr010(evt);
+	u32 period = DIV_ROUND_CLOSEST(fttmr010->tick_rate, HZ);
 	u32 cr;
 
-	/* Stop timer and interrupt */
-	cr = readl(base + TIMER_CR);
-	cr &= ~(TIMER_1_CR_ENABLE | TIMER_1_CR_INT);
-	writel(cr, base + TIMER_CR);
-
-	/* Setup timer to fire at 1/HT intervals. */
-	cr = 0xffffffff - (period - 1);
-	writel(cr, base + TIMER1_COUNT);
-	writel(cr, base + TIMER1_LOAD);
-
-	/* enable interrupt on overflow */
-	cr = readl(base + TIMER_INTR_MASK);
-	cr &= ~(TIMER_1_INT_MATCH1 | TIMER_1_INT_MATCH2);
-	cr |= TIMER_1_INT_OVERFLOW;
-	writel(cr, base + TIMER_INTR_MASK);
+	/* Stop */
+	cr = readl(fttmr010->base + TIMER_CR);
+	cr &= ~fttmr010->t1_enable_val;
+	writel(cr, fttmr010->base + TIMER_CR);
+
+	/* Setup timer to fire at 1/HZ intervals. */
+	if (fttmr010->count_down) {
+		writel(period, fttmr010->base + TIMER1_LOAD);
+		writel(0, fttmr010->base + TIMER1_MATCH1);
+	} else {
+		cr = 0xffffffff - (period - 1);
+		writel(cr, fttmr010->base + TIMER1_COUNT);
+		writel(cr, fttmr010->base + TIMER1_LOAD);
+
+		/* Enable interrupt on overflow */
+		cr = readl(fttmr010->base + TIMER_INTR_MASK);
+		cr &= ~(TIMER_1_INT_MATCH1 | TIMER_1_INT_MATCH2);
+		cr |= TIMER_1_INT_OVERFLOW;
+		writel(cr, fttmr010->base + TIMER_INTR_MASK);
+	}
 
 	/* Start the timer */
-	cr = readl(base + TIMER_CR);
-	cr |= TIMER_1_CR_ENABLE;
-	cr |= TIMER_1_CR_INT;
-	writel(cr, base + TIMER_CR);
+	cr = readl(fttmr010->base + TIMER_CR);
+	cr |= fttmr010->t1_enable_val;
+	writel(cr, fttmr010->base + TIMER_CR);
 
 	return 0;
 }
 
-/* Use TIMER1 as clock event */
-static struct clock_event_device fttmr010_clockevent = {
-	.name			= "TIMER1",
-	/* Reasonably fast and accurate clock event */
-	.rating			= 300,
-	.shift                  = 32,
-	.features		= CLOCK_EVT_FEAT_PERIODIC |
-				  CLOCK_EVT_FEAT_ONESHOT,
-	.set_next_event		= fttmr010_timer_set_next_event,
-	.set_state_shutdown	= fttmr010_timer_shutdown,
-	.set_state_periodic	= fttmr010_timer_set_periodic,
-	.set_state_oneshot	= fttmr010_timer_shutdown,
-	.tick_resume		= fttmr010_timer_shutdown,
-};
-
 /*
  * IRQ handler for the timer
  */
 static irqreturn_t fttmr010_timer_interrupt(int irq, void *dev_id)
 {
-	struct clock_event_device *evt = &fttmr010_clockevent;
+	struct clock_event_device *evt = dev_id;
 
 	evt->event_handler(evt);
 	return IRQ_HANDLED;
 }
 
-static struct irqaction fttmr010_timer_irq = {
-	.name		= "Faraday FTTMR010 Timer Tick",
-	.flags		= IRQF_TIMER,
-	.handler	= fttmr010_timer_interrupt,
-};
-
-static int __init fttmr010_timer_common_init(struct device_node *np)
+static int __init fttmr010_common_init(struct device_node *np, bool is_aspeed)
 {
+	struct fttmr010 *fttmr010;
 	int irq;
+	struct clk *clk;
+	int ret;
+	u32 val;
+
+	/*
+	 * These implementations require a clock reference.
+	 * FIXME: we currently only support clocking using PCLK
+	 * and using EXTCLK is not supported in the driver.
+	 */
+	clk = of_clk_get_by_name(np, "PCLK");
+	if (IS_ERR(clk)) {
+		pr_err("could not get PCLK\n");
+		return PTR_ERR(clk);
+	}
+	ret = clk_prepare_enable(clk);
+	if (ret) {
+		pr_err("failed to enable PCLK\n");
+		return ret;
+	}
 
-	base = of_iomap(np, 0);
-	if (!base) {
+	fttmr010 = kzalloc(sizeof(*fttmr010), GFP_KERNEL);
+	if (!fttmr010) {
+		ret = -ENOMEM;
+		goto out_disable_clock;
+	}
+	fttmr010->tick_rate = clk_get_rate(clk);
+
+	fttmr010->base = of_iomap(np, 0);
+	if (!fttmr010->base) {
 		pr_err("Can't remap registers");
-		return -ENXIO;
+		ret = -ENXIO;
+		goto out_free;
 	}
 	/* IRQ for timer 1 */
 	irq = irq_of_parse_and_map(np, 0);
 	if (irq <= 0) {
 		pr_err("Can't parse IRQ");
-		return -EINVAL;
+		ret = -EINVAL;
+		goto out_unmap;
+	}
+
+	/*
+	 * The Aspeed AST2400 moves bits around in the control register,
+	 * otherwise it works the same.
+	 */
+	if (is_aspeed) {
+		fttmr010->t1_enable_val = TIMER_1_CR_ASPEED_ENABLE |
+			TIMER_1_CR_ASPEED_INT;
+		/* Downward not available */
+		fttmr010->count_down = true;
+	} else {
+		fttmr010->t1_enable_val = TIMER_1_CR_ENABLE | TIMER_1_CR_INT;
 	}
 
 	/*
 	 * Reset the interrupt mask and status
 	 */
-	writel(TIMER_INT_ALL_MASK, base + TIMER_INTR_MASK);
-	writel(0, base + TIMER_INTR_STATE);
-	writel(TIMER_DEFAULT_FLAGS, base + TIMER_CR);
+	writel(TIMER_INT_ALL_MASK, fttmr010->base + TIMER_INTR_MASK);
+	writel(0, fttmr010->base + TIMER_INTR_STATE);
+
+	/*
+	 * Enable timer 1 count up, timer 2 count up, except on Aspeed,
+	 * where everything just counts down.
+	 */
+	if (is_aspeed)
+		val = TIMER_2_CR_ASPEED_ENABLE;
+	else {
+		val = TIMER_2_CR_ENABLE;
+		if (!fttmr010->count_down)
+			val |= TIMER_1_CR_UPDOWN | TIMER_2_CR_UPDOWN;
+	}
+	writel(val, fttmr010->base + TIMER_CR);
 
 	/*
 	 * Setup free-running clocksource timer (interrupts
 	 * disabled.)
 	 */
-	writel(0, base + TIMER3_COUNT);
-	writel(0, base + TIMER3_LOAD);
-	writel(0, base + TIMER3_MATCH1);
-	writel(0, base + TIMER3_MATCH2);
-	clocksource_mmio_init(base + TIMER3_COUNT,
-			      "fttmr010_clocksource", tick_rate,
-			      300, 32, clocksource_mmio_readl_up);
-	sched_clock_register(fttmr010_read_sched_clock, 32, tick_rate);
+	local_fttmr = fttmr010;
+	writel(0, fttmr010->base + TIMER2_COUNT);
+	writel(0, fttmr010->base + TIMER2_MATCH1);
+	writel(0, fttmr010->base + TIMER2_MATCH2);
+
+	if (fttmr010->count_down) {
+		writel(~0, fttmr010->base + TIMER2_LOAD);
+		clocksource_mmio_init(fttmr010->base + TIMER2_COUNT,
+				      "FTTMR010-TIMER2",
+				      fttmr010->tick_rate,
+				      300, 32, clocksource_mmio_readl_down);
+		sched_clock_register(fttmr010_read_sched_clock_down, 32,
+				     fttmr010->tick_rate);
+	} else {
+		writel(0, fttmr010->base + TIMER2_LOAD);
+		clocksource_mmio_init(fttmr010->base + TIMER2_COUNT,
+				      "FTTMR010-TIMER2",
+				      fttmr010->tick_rate,
+				      300, 32, clocksource_mmio_readl_up);
+		sched_clock_register(fttmr010_read_sched_clock_up, 32,
+				     fttmr010->tick_rate);
+	}
 
 	/*
-	 * Setup clockevent timer (interrupt-driven.)
+	 * Setup clockevent timer (interrupt-driven) on timer 1.
 	 */
-	writel(0, base + TIMER1_COUNT);
-	writel(0, base + TIMER1_LOAD);
-	writel(0, base + TIMER1_MATCH1);
-	writel(0, base + TIMER1_MATCH2);
-	setup_irq(irq, &fttmr010_timer_irq);
-	fttmr010_clockevent.cpumask = cpumask_of(0);
-	clockevents_config_and_register(&fttmr010_clockevent, tick_rate,
+	writel(0, fttmr010->base + TIMER1_COUNT);
+	writel(0, fttmr010->base + TIMER1_LOAD);
+	writel(0, fttmr010->base + TIMER1_MATCH1);
+	writel(0, fttmr010->base + TIMER1_MATCH2);
+	ret = request_irq(irq, fttmr010_timer_interrupt, IRQF_TIMER,
+			  "FTTMR010-TIMER1", &fttmr010->clkevt);
+	if (ret) {
+		pr_err("FTTMR010-TIMER1 no IRQ\n");
+		goto out_unmap;
+	}
+
+	fttmr010->clkevt.name = "FTTMR010-TIMER1";
+	/* Reasonably fast and accurate clock event */
+	fttmr010->clkevt.rating = 300;
+	fttmr010->clkevt.features = CLOCK_EVT_FEAT_PERIODIC |
+		CLOCK_EVT_FEAT_ONESHOT;
+	fttmr010->clkevt.set_next_event = fttmr010_timer_set_next_event;
+	fttmr010->clkevt.set_state_shutdown = fttmr010_timer_shutdown;
+	fttmr010->clkevt.set_state_periodic = fttmr010_timer_set_periodic;
+	fttmr010->clkevt.set_state_oneshot = fttmr010_timer_set_oneshot;
+	fttmr010->clkevt.tick_resume = fttmr010_timer_shutdown;
+	fttmr010->clkevt.cpumask = cpumask_of(0);
+	fttmr010->clkevt.irq = irq;
+	clockevents_config_and_register(&fttmr010->clkevt,
+					fttmr010->tick_rate,
 					1, 0xffffffff);
 
-	return 0;
-}
+#ifdef CONFIG_ARM
+	/* Also use this timer for delays */
+	if (fttmr010->count_down)
+		fttmr010->delay_timer.read_current_timer =
+			fttmr010_read_current_timer_down;
+	else
+		fttmr010->delay_timer.read_current_timer =
+			fttmr010_read_current_timer_up;
+	fttmr010->delay_timer.freq = fttmr010->tick_rate;
+	register_current_timer_delay(&fttmr010->delay_timer);
+#endif
 
-static int __init fttmr010_timer_of_init(struct device_node *np)
-{
-	/*
-	 * These implementations require a clock reference.
-	 * FIXME: we currently only support clocking using PCLK
-	 * and using EXTCLK is not supported in the driver.
-	 */
-	struct clk *clk;
+	return 0;
 
-	clk = of_clk_get_by_name(np, "PCLK");
-	if (IS_ERR(clk)) {
-		pr_err("could not get PCLK");
-		return PTR_ERR(clk);
-	}
-	tick_rate = clk_get_rate(clk);
+out_unmap:
+	iounmap(fttmr010->base);
+out_free:
+	kfree(fttmr010);
+out_disable_clock:
+	clk_disable_unprepare(clk);
 
-	return fttmr010_timer_common_init(np);
+	return ret;
 }
-CLOCKSOURCE_OF_DECLARE(fttmr010, "faraday,fttmr010", fttmr010_timer_of_init);
 
-/*
- * Gemini-specific: relevant registers in the global syscon
- */
-#define GLOBAL_STATUS		0x04
-#define CPU_AHB_RATIO_MASK	(0x3 << 18)
-#define CPU_AHB_1_1		(0x0 << 18)
-#define CPU_AHB_3_2		(0x1 << 18)
-#define CPU_AHB_24_13		(0x2 << 18)
-#define CPU_AHB_2_1		(0x3 << 18)
-#define REG_TO_AHB_SPEED(reg)	((((reg) >> 15) & 0x7) * 10 + 130)
-
-static int __init gemini_timer_of_init(struct device_node *np)
+static __init int aspeed_timer_init(struct device_node *np)
 {
-	static struct regmap *map;
-	int ret;
-	u32 val;
-
-	map = syscon_regmap_lookup_by_phandle(np, "syscon");
-	if (IS_ERR(map)) {
-		pr_err("Can't get regmap for syscon handle\n");
-		return -ENODEV;
-	}
-	ret = regmap_read(map, GLOBAL_STATUS, &val);
-	if (ret) {
-		pr_err("Can't read syscon status register\n");
-		return -ENXIO;
-	}
-
-	tick_rate = REG_TO_AHB_SPEED(val) * 1000000;
-	pr_info("Bus: %dMHz ", tick_rate / 1000000);
-
-	tick_rate /= 6;		/* APB bus run AHB*(1/6) */
-
-	switch (val & CPU_AHB_RATIO_MASK) {
-	case CPU_AHB_1_1:
-		pr_cont("(1/1)\n");
-		break;
-	case CPU_AHB_3_2:
-		pr_cont("(3/2)\n");
-		break;
-	case CPU_AHB_24_13:
-		pr_cont("(24/13)\n");
-		break;
-	case CPU_AHB_2_1:
-		pr_cont("(2/1)\n");
-		break;
-	}
+	return fttmr010_common_init(np, true);
+}
 
-	return fttmr010_timer_common_init(np);
+static __init int fttmr010_timer_init(struct device_node *np)
+{
+	return fttmr010_common_init(np, false);
 }
-CLOCKSOURCE_OF_DECLARE(gemini, "cortina,gemini-timer", gemini_timer_of_init);
+
+TIMER_OF_DECLARE(fttmr010, "faraday,fttmr010", fttmr010_timer_init);
+TIMER_OF_DECLARE(gemini, "cortina,gemini-timer", fttmr010_timer_init);
+TIMER_OF_DECLARE(moxart, "moxa,moxart-timer", fttmr010_timer_init);
+TIMER_OF_DECLARE(ast2400, "aspeed,ast2400-timer", aspeed_timer_init);
+TIMER_OF_DECLARE(ast2500, "aspeed,ast2500-timer", aspeed_timer_init);
diff --git a/drivers/clocksource/timer-imx-gpt.c b/drivers/clocksource/timer-imx-gpt.c
index f595460bfc58..6ec6d79b237c 100644
--- a/drivers/clocksource/timer-imx-gpt.c
+++ b/drivers/clocksource/timer-imx-gpt.c
@@ -545,15 +545,15 @@ static int __init imx6dl_timer_init_dt(struct device_node *np)
 	return mxc_timer_init_dt(np, GPT_TYPE_IMX6DL);
 }
 
-CLOCKSOURCE_OF_DECLARE(imx1_timer, "fsl,imx1-gpt", imx1_timer_init_dt);
-CLOCKSOURCE_OF_DECLARE(imx21_timer, "fsl,imx21-gpt", imx21_timer_init_dt);
-CLOCKSOURCE_OF_DECLARE(imx27_timer, "fsl,imx27-gpt", imx21_timer_init_dt);
-CLOCKSOURCE_OF_DECLARE(imx31_timer, "fsl,imx31-gpt", imx31_timer_init_dt);
-CLOCKSOURCE_OF_DECLARE(imx25_timer, "fsl,imx25-gpt", imx31_timer_init_dt);
-CLOCKSOURCE_OF_DECLARE(imx50_timer, "fsl,imx50-gpt", imx31_timer_init_dt);
-CLOCKSOURCE_OF_DECLARE(imx51_timer, "fsl,imx51-gpt", imx31_timer_init_dt);
-CLOCKSOURCE_OF_DECLARE(imx53_timer, "fsl,imx53-gpt", imx31_timer_init_dt);
-CLOCKSOURCE_OF_DECLARE(imx6q_timer, "fsl,imx6q-gpt", imx31_timer_init_dt);
-CLOCKSOURCE_OF_DECLARE(imx6dl_timer, "fsl,imx6dl-gpt", imx6dl_timer_init_dt);
-CLOCKSOURCE_OF_DECLARE(imx6sl_timer, "fsl,imx6sl-gpt", imx6dl_timer_init_dt);
-CLOCKSOURCE_OF_DECLARE(imx6sx_timer, "fsl,imx6sx-gpt", imx6dl_timer_init_dt);
+TIMER_OF_DECLARE(imx1_timer, "fsl,imx1-gpt", imx1_timer_init_dt);
+TIMER_OF_DECLARE(imx21_timer, "fsl,imx21-gpt", imx21_timer_init_dt);
+TIMER_OF_DECLARE(imx27_timer, "fsl,imx27-gpt", imx21_timer_init_dt);
+TIMER_OF_DECLARE(imx31_timer, "fsl,imx31-gpt", imx31_timer_init_dt);
+TIMER_OF_DECLARE(imx25_timer, "fsl,imx25-gpt", imx31_timer_init_dt);
+TIMER_OF_DECLARE(imx50_timer, "fsl,imx50-gpt", imx31_timer_init_dt);
+TIMER_OF_DECLARE(imx51_timer, "fsl,imx51-gpt", imx31_timer_init_dt);
+TIMER_OF_DECLARE(imx53_timer, "fsl,imx53-gpt", imx31_timer_init_dt);
+TIMER_OF_DECLARE(imx6q_timer, "fsl,imx6q-gpt", imx31_timer_init_dt);
+TIMER_OF_DECLARE(imx6dl_timer, "fsl,imx6dl-gpt", imx6dl_timer_init_dt);
+TIMER_OF_DECLARE(imx6sl_timer, "fsl,imx6sl-gpt", imx6dl_timer_init_dt);
+TIMER_OF_DECLARE(imx6sx_timer, "fsl,imx6sx-gpt", imx6dl_timer_init_dt);
diff --git a/drivers/clocksource/timer-integrator-ap.c b/drivers/clocksource/timer-integrator-ap.c
index 04ad3066e190..2ff64d9d4fb3 100644
--- a/drivers/clocksource/timer-integrator-ap.c
+++ b/drivers/clocksource/timer-integrator-ap.c
@@ -232,5 +232,5 @@ static int __init integrator_ap_timer_init_of(struct device_node *node)
 	return 0;
 }
 
-CLOCKSOURCE_OF_DECLARE(integrator_ap_timer, "arm,integrator-timer",
+TIMER_OF_DECLARE(integrator_ap_timer, "arm,integrator-timer",
 		       integrator_ap_timer_init_of);
diff --git a/drivers/clocksource/timer-keystone.c b/drivers/clocksource/timer-keystone.c
index ab68a47ab3b4..0eee03250cfc 100644
--- a/drivers/clocksource/timer-keystone.c
+++ b/drivers/clocksource/timer-keystone.c
@@ -226,5 +226,5 @@ err:
 	return error;
 }
 
-CLOCKSOURCE_OF_DECLARE(keystone_timer, "ti,keystone-timer",
+TIMER_OF_DECLARE(keystone_timer, "ti,keystone-timer",
 			   keystone_timer_init);
diff --git a/drivers/clocksource/timer-nps.c b/drivers/clocksource/timer-nps.c
index e74ea1722ad3..7b6bb0df96ae 100644
--- a/drivers/clocksource/timer-nps.c
+++ b/drivers/clocksource/timer-nps.c
@@ -110,9 +110,9 @@ static int __init nps_setup_clocksource(struct device_node *node)
 	return ret;
 }
 
-CLOCKSOURCE_OF_DECLARE(ezchip_nps400_clksrc, "ezchip,nps400-timer",
+TIMER_OF_DECLARE(ezchip_nps400_clksrc, "ezchip,nps400-timer",
 		       nps_setup_clocksource);
-CLOCKSOURCE_OF_DECLARE(ezchip_nps400_clk_src, "ezchip,nps400-timer1",
+TIMER_OF_DECLARE(ezchip_nps400_clk_src, "ezchip,nps400-timer1",
 		       nps_setup_clocksource);
 
 #ifdef CONFIG_EZNPS_MTM_EXT
@@ -279,6 +279,6 @@ static int __init nps_setup_clockevent(struct device_node *node)
 	return 0;
 }
 
-CLOCKSOURCE_OF_DECLARE(ezchip_nps400_clk_evt, "ezchip,nps400-timer0",
+TIMER_OF_DECLARE(ezchip_nps400_clk_evt, "ezchip,nps400-timer0",
 		       nps_setup_clockevent);
 #endif /* CONFIG_EZNPS_MTM_EXT */
diff --git a/drivers/clocksource/timer-of.c b/drivers/clocksource/timer-of.c
new file mode 100644
index 000000000000..f6e7491c873c
--- /dev/null
+++ b/drivers/clocksource/timer-of.c
@@ -0,0 +1,171 @@
+/*
+ * Copyright (c) 2017, Linaro Ltd.  All rights reserved.
+ *
+ * Author: Daniel Lezcano <daniel.lezcano@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/slab.h>
+
+#include "timer-of.h"
+
+static __init void timer_irq_exit(struct of_timer_irq *of_irq)
+{
+	struct timer_of *to = container_of(of_irq, struct timer_of, of_irq);
+
+	struct clock_event_device *clkevt = &to->clkevt;
+
+	of_irq->percpu ? free_percpu_irq(of_irq->irq, clkevt) :
+		free_irq(of_irq->irq, clkevt);
+}
+
+static __init int timer_irq_init(struct device_node *np,
+				 struct of_timer_irq *of_irq)
+{
+	int ret;
+	struct timer_of *to = container_of(of_irq, struct timer_of, of_irq);
+	struct clock_event_device *clkevt = &to->clkevt;
+
+	of_irq->irq = of_irq->name ? of_irq_get_byname(np, of_irq->name):
+		irq_of_parse_and_map(np, of_irq->index);
+	if (!of_irq->irq) {
+		pr_err("Failed to map interrupt for %s\n", np->full_name);
+		return -EINVAL;
+	}
+
+	ret = of_irq->percpu ?
+		request_percpu_irq(of_irq->irq, of_irq->handler,
+				   np->full_name, clkevt) :
+		request_irq(of_irq->irq, of_irq->handler,
+			    of_irq->flags ? of_irq->flags : IRQF_TIMER,
+			    np->full_name, clkevt);
+	if (ret) {
+		pr_err("Failed to request irq %d for %s\n", of_irq->irq,
+		       np->full_name);
+		return ret;
+	}
+
+	clkevt->irq = of_irq->irq;
+
+	return 0;
+}
+
+static __init void timer_clk_exit(struct of_timer_clk *of_clk)
+{
+	of_clk->rate = 0;
+	clk_disable_unprepare(of_clk->clk);
+	clk_put(of_clk->clk);
+}
+
+static __init int timer_clk_init(struct device_node *np,
+				 struct of_timer_clk *of_clk)
+{
+	int ret;
+
+	of_clk->clk = of_clk->name ? of_clk_get_by_name(np, of_clk->name) :
+		of_clk_get(np, of_clk->index);
+	if (IS_ERR(of_clk->clk)) {
+		pr_err("Failed to get clock for %s\n", np->full_name);
+		return PTR_ERR(of_clk->clk);
+	}
+
+	ret = clk_prepare_enable(of_clk->clk);
+	if (ret) {
+		pr_err("Failed for enable clock for %s\n", np->full_name);
+		goto out_clk_put;
+	}
+
+	of_clk->rate = clk_get_rate(of_clk->clk);
+	if (!of_clk->rate) {
+		ret = -EINVAL;
+		pr_err("Failed to get clock rate for %s\n", np->full_name);
+		goto out_clk_disable;
+	}
+
+	of_clk->period = DIV_ROUND_UP(of_clk->rate, HZ);
+out:
+	return ret;
+
+out_clk_disable:
+	clk_disable_unprepare(of_clk->clk);
+out_clk_put:
+	clk_put(of_clk->clk);
+
+	goto out;
+}
+
+static __init void timer_base_exit(struct of_timer_base *of_base)
+{
+	iounmap(of_base->base);
+}
+
+static __init int timer_base_init(struct device_node *np,
+				  struct of_timer_base *of_base)
+{
+	const char *name = of_base->name ? of_base->name : np->full_name;
+
+	of_base->base = of_io_request_and_map(np, of_base->index, name);
+	if (!of_base->base) {
+		pr_err("Failed to iomap (%s)\n", name);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+int __init timer_of_init(struct device_node *np, struct timer_of *to)
+{
+	int ret = -EINVAL;
+	int flags = 0;
+
+	if (to->flags & TIMER_OF_BASE) {
+		ret = timer_base_init(np, &to->of_base);
+		if (ret)
+			goto out_fail;
+		flags |= TIMER_OF_BASE;
+	}
+
+	if (to->flags & TIMER_OF_CLOCK) {
+		ret = timer_clk_init(np, &to->of_clk);
+		if (ret)
+			goto out_fail;
+		flags |= TIMER_OF_CLOCK;
+	}
+
+	if (to->flags & TIMER_OF_IRQ) {
+		ret = timer_irq_init(np, &to->of_irq);
+		if (ret)
+			goto out_fail;
+		flags |= TIMER_OF_IRQ;
+	}
+
+	if (!to->clkevt.name)
+		to->clkevt.name = np->name;
+	return ret;
+
+out_fail:
+	if (flags & TIMER_OF_IRQ)
+		timer_irq_exit(&to->of_irq);
+
+	if (flags & TIMER_OF_CLOCK)
+		timer_clk_exit(&to->of_clk);
+
+	if (flags & TIMER_OF_BASE)
+		timer_base_exit(&to->of_base);
+	return ret;
+}
diff --git a/drivers/clocksource/timer-of.h b/drivers/clocksource/timer-of.h
new file mode 100644
index 000000000000..e0d727255f72
--- /dev/null
+++ b/drivers/clocksource/timer-of.h
@@ -0,0 +1,69 @@
+#ifndef __TIMER_OF_H__
+#define __TIMER_OF_H__
+
+#include <linux/clockchips.h>
+
+#define TIMER_OF_BASE	0x1
+#define TIMER_OF_CLOCK	0x2
+#define TIMER_OF_IRQ	0x4
+
+struct of_timer_irq {
+	int irq;
+	int index;
+	int percpu;
+	const char *name;
+	unsigned long flags;
+	irq_handler_t handler;
+};
+
+struct of_timer_base {
+	void __iomem *base;
+	const char *name;
+	int index;
+};
+
+struct of_timer_clk {
+	struct clk *clk;
+	const char *name;
+	int index;
+	unsigned long rate;
+	unsigned long period;
+};
+
+struct timer_of {
+	unsigned int flags;
+	struct clock_event_device clkevt;
+	struct of_timer_base of_base;
+	struct of_timer_irq  of_irq;
+	struct of_timer_clk  of_clk;
+	void *private_data;
+};
+
+static inline struct timer_of *to_timer_of(struct clock_event_device *clkevt)
+{
+	return container_of(clkevt, struct timer_of, clkevt);
+}
+
+static inline void __iomem *timer_of_base(struct timer_of *to)
+{
+	return to->of_base.base;
+}
+
+static inline int timer_of_irq(struct timer_of *to)
+{
+	return to->of_irq.irq;
+}
+
+static inline unsigned long timer_of_rate(struct timer_of *to)
+{
+	return to->of_clk.rate;
+}
+
+static inline unsigned long timer_of_period(struct timer_of *to)
+{
+	return to->of_clk.period;
+}
+
+extern int __init timer_of_init(struct device_node *np,
+				struct timer_of *to);
+#endif
diff --git a/drivers/clocksource/timer-oxnas-rps.c b/drivers/clocksource/timer-oxnas-rps.c
index d630bf417773..eed6feff8b5f 100644
--- a/drivers/clocksource/timer-oxnas-rps.c
+++ b/drivers/clocksource/timer-oxnas-rps.c
@@ -293,7 +293,7 @@ err_alloc:
 	return ret;
 }
 
-CLOCKSOURCE_OF_DECLARE(ox810se_rps,
+TIMER_OF_DECLARE(ox810se_rps,
 		       "oxsemi,ox810se-rps-timer", oxnas_rps_timer_init);
-CLOCKSOURCE_OF_DECLARE(ox820_rps,
+TIMER_OF_DECLARE(ox820_rps,
 		       "oxsemi,ox820se-rps-timer", oxnas_rps_timer_init);
diff --git a/drivers/clocksource/timer-prima2.c b/drivers/clocksource/timer-prima2.c
index b4122ed1accb..20ff33b698df 100644
--- a/drivers/clocksource/timer-prima2.c
+++ b/drivers/clocksource/timer-prima2.c
@@ -245,5 +245,5 @@ static int __init sirfsoc_prima2_timer_init(struct device_node *np)
 
 	return 0;
 }
-CLOCKSOURCE_OF_DECLARE(sirfsoc_prima2_timer,
+TIMER_OF_DECLARE(sirfsoc_prima2_timer,
 	"sirf,prima2-tick", sirfsoc_prima2_timer_init);
diff --git a/drivers/clocksource/clksrc-probe.c b/drivers/clocksource/timer-probe.c
index ac701ffb8d59..da81e5de74fe 100644
--- a/drivers/clocksource/clksrc-probe.c
+++ b/drivers/clocksource/timer-probe.c
@@ -19,20 +19,20 @@
 #include <linux/of.h>
 #include <linux/clocksource.h>
 
-extern struct of_device_id __clksrc_of_table[];
+extern struct of_device_id __timer_of_table[];
 
-static const struct of_device_id __clksrc_of_table_sentinel
-	__used __section(__clksrc_of_table_end);
+static const struct of_device_id __timer_of_table_sentinel
+	__used __section(__timer_of_table_end);
 
-void __init clocksource_probe(void)
+void __init timer_probe(void)
 {
 	struct device_node *np;
 	const struct of_device_id *match;
 	of_init_fn_1_ret init_func_ret;
-	unsigned clocksources = 0;
+	unsigned timers = 0;
 	int ret;
 
-	for_each_matching_node_and_match(np, __clksrc_of_table, &match) {
+	for_each_matching_node_and_match(np, __timer_of_table, &match) {
 		if (!of_device_is_available(np))
 			continue;
 
@@ -45,11 +45,11 @@ void __init clocksource_probe(void)
 			continue;
 		}
 
-		clocksources++;
+		timers++;
 	}
 
-	clocksources += acpi_probe_device_table(clksrc);
+	timers += acpi_probe_device_table(timer);
 
-	if (!clocksources)
-		pr_crit("%s: no matching clocksources found\n", __func__);
+	if (!timers)
+		pr_crit("%s: no matching timers found\n", __func__);
 }
diff --git a/drivers/clocksource/timer-sp804.c b/drivers/clocksource/timer-sp804.c
index 2d575a8c0939..3ac9dec9a038 100644
--- a/drivers/clocksource/timer-sp804.c
+++ b/drivers/clocksource/timer-sp804.c
@@ -287,7 +287,7 @@ err:
 	iounmap(base);
 	return ret;
 }
-CLOCKSOURCE_OF_DECLARE(sp804, "arm,sp804", sp804_of_init);
+TIMER_OF_DECLARE(sp804, "arm,sp804", sp804_of_init);
 
 static int __init integrator_cp_of_init(struct device_node *np)
 {
@@ -335,4 +335,4 @@ err:
 	iounmap(base);
 	return ret;
 }
-CLOCKSOURCE_OF_DECLARE(intcp, "arm,integrator-cp-timer", integrator_cp_of_init);
+TIMER_OF_DECLARE(intcp, "arm,integrator-cp-timer", integrator_cp_of_init);
diff --git a/drivers/clocksource/timer-stm32.c b/drivers/clocksource/timer-stm32.c
index 1b2574c4fb97..174d1243ea93 100644
--- a/drivers/clocksource/timer-stm32.c
+++ b/drivers/clocksource/timer-stm32.c
@@ -187,4 +187,4 @@ err_clk_get:
 	return ret;
 }
 
-CLOCKSOURCE_OF_DECLARE(stm32, "st,stm32-timer", stm32_clockevent_init);
+TIMER_OF_DECLARE(stm32, "st,stm32-timer", stm32_clockevent_init);
diff --git a/drivers/clocksource/timer-sun5i.c b/drivers/clocksource/timer-sun5i.c
index 2e9c830ae1cd..2a3fe83ec337 100644
--- a/drivers/clocksource/timer-sun5i.c
+++ b/drivers/clocksource/timer-sun5i.c
@@ -12,6 +12,7 @@
 
 #include <linux/clk.h>
 #include <linux/clockchips.h>
+#include <linux/clocksource.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
@@ -358,7 +359,7 @@ static int __init sun5i_timer_init(struct device_node *node)
 
 	return sun5i_setup_clockevent(node, timer_base, clk, irq);
 }
-CLOCKSOURCE_OF_DECLARE(sun5i_a13, "allwinner,sun5i-a13-hstimer",
+TIMER_OF_DECLARE(sun5i_a13, "allwinner,sun5i-a13-hstimer",
 			   sun5i_timer_init);
-CLOCKSOURCE_OF_DECLARE(sun7i_a20, "allwinner,sun7i-a20-hstimer",
+TIMER_OF_DECLARE(sun7i_a20, "allwinner,sun7i-a20-hstimer",
 			   sun5i_timer_init);
diff --git a/drivers/clocksource/timer-ti-32k.c b/drivers/clocksource/timer-ti-32k.c
index 624067712ef0..880a861ab3c8 100644
--- a/drivers/clocksource/timer-ti-32k.c
+++ b/drivers/clocksource/timer-ti-32k.c
@@ -124,5 +124,5 @@ static int __init ti_32k_timer_init(struct device_node *np)
 
 	return 0;
 }
-CLOCKSOURCE_OF_DECLARE(ti_32k_timer, "ti,omap-counter32k",
+TIMER_OF_DECLARE(ti_32k_timer, "ti,omap-counter32k",
 		ti_32k_timer_init);
diff --git a/drivers/clocksource/timer-u300.c b/drivers/clocksource/timer-u300.c
index 704e40c6f151..be34b116d4d2 100644
--- a/drivers/clocksource/timer-u300.c
+++ b/drivers/clocksource/timer-u300.c
@@ -458,5 +458,5 @@ static int __init u300_timer_init_of(struct device_node *np)
 	return 0;
 }
 
-CLOCKSOURCE_OF_DECLARE(u300_timer, "stericsson,u300-apptimer",
+TIMER_OF_DECLARE(u300_timer, "stericsson,u300-apptimer",
 		       u300_timer_init_of);
diff --git a/drivers/clocksource/versatile.c b/drivers/clocksource/versatile.c
index 220b490a8142..39725d38aede 100644
--- a/drivers/clocksource/versatile.c
+++ b/drivers/clocksource/versatile.c
@@ -38,7 +38,7 @@ static int __init versatile_sched_clock_init(struct device_node *node)
 
 	return 0;
 }
-CLOCKSOURCE_OF_DECLARE(vexpress, "arm,vexpress-sysreg",
+TIMER_OF_DECLARE(vexpress, "arm,vexpress-sysreg",
 		       versatile_sched_clock_init);
-CLOCKSOURCE_OF_DECLARE(versatile, "arm,versatile-sysreg",
+TIMER_OF_DECLARE(versatile, "arm,versatile-sysreg",
 		       versatile_sched_clock_init);
diff --git a/drivers/clocksource/vf_pit_timer.c b/drivers/clocksource/vf_pit_timer.c
index e0849e20a307..0f92089ec08c 100644
--- a/drivers/clocksource/vf_pit_timer.c
+++ b/drivers/clocksource/vf_pit_timer.c
@@ -201,4 +201,4 @@ static int __init pit_timer_init(struct device_node *np)
 
 	return pit_clockevent_init(clk_rate, irq);
 }
-CLOCKSOURCE_OF_DECLARE(vf610, "fsl,vf610-pit", pit_timer_init);
+TIMER_OF_DECLARE(vf610, "fsl,vf610-pit", pit_timer_init);
diff --git a/drivers/clocksource/vt8500_timer.c b/drivers/clocksource/vt8500_timer.c
index d02b51075ad1..e0f7489cfc8e 100644
--- a/drivers/clocksource/vt8500_timer.c
+++ b/drivers/clocksource/vt8500_timer.c
@@ -165,4 +165,4 @@ static int __init vt8500_timer_init(struct device_node *np)
 	return 0;
 }
 
-CLOCKSOURCE_OF_DECLARE(vt8500, "via,vt8500-timer", vt8500_timer_init);
+TIMER_OF_DECLARE(vt8500, "via,vt8500-timer", vt8500_timer_init);
diff --git a/drivers/clocksource/zevio-timer.c b/drivers/clocksource/zevio-timer.c
index 9a53f5ef6157..a6a0338eea77 100644
--- a/drivers/clocksource/zevio-timer.c
+++ b/drivers/clocksource/zevio-timer.c
@@ -215,4 +215,4 @@ static int __init zevio_timer_init(struct device_node *node)
 	return zevio_timer_add(node);
 }
 
-CLOCKSOURCE_OF_DECLARE(zevio_timer, "lsi,zevio-timer", zevio_timer_init);
+TIMER_OF_DECLARE(zevio_timer, "lsi,zevio-timer", zevio_timer_init);
diff --git a/drivers/cpufreq/cppc_cpufreq.c b/drivers/cpufreq/cppc_cpufreq.c
index e82bb3c30b92..10be285c9055 100644
--- a/drivers/cpufreq/cppc_cpufreq.c
+++ b/drivers/cpufreq/cppc_cpufreq.c
@@ -144,10 +144,23 @@ static int cppc_cpufreq_cpu_init(struct cpufreq_policy *policy)
 
 	cppc_dmi_max_khz = cppc_get_dmi_max_khz();
 
-	policy->min = cpu->perf_caps.lowest_perf * cppc_dmi_max_khz / cpu->perf_caps.highest_perf;
+	/*
+	 * Set min to lowest nonlinear perf to avoid any efficiency penalty (see
+	 * Section 8.4.7.1.1.5 of ACPI 6.1 spec)
+	 */
+	policy->min = cpu->perf_caps.lowest_nonlinear_perf * cppc_dmi_max_khz /
+		cpu->perf_caps.highest_perf;
 	policy->max = cppc_dmi_max_khz;
-	policy->cpuinfo.min_freq = policy->min;
-	policy->cpuinfo.max_freq = policy->max;
+
+	/*
+	 * Set cpuinfo.min_freq to Lowest to make the full range of performance
+	 * available if userspace wants to use any perf between lowest & lowest
+	 * nonlinear perf
+	 */
+	policy->cpuinfo.min_freq = cpu->perf_caps.lowest_perf * cppc_dmi_max_khz /
+		cpu->perf_caps.highest_perf;
+	policy->cpuinfo.max_freq = cppc_dmi_max_khz;
+
 	policy->cpuinfo.transition_latency = cppc_get_transition_latency(cpu_num);
 	policy->shared_type = cpu->shared_type;
 
diff --git a/drivers/cpufreq/cpufreq-dt-platdev.c b/drivers/cpufreq/cpufreq-dt-platdev.c
index 921b4a6c3d16..1c262923fe58 100644
--- a/drivers/cpufreq/cpufreq-dt-platdev.c
+++ b/drivers/cpufreq/cpufreq-dt-platdev.c
@@ -31,6 +31,7 @@ static const struct of_device_id machines[] __initconst = {
 	{ .compatible = "arm,integrator-ap", },
 	{ .compatible = "arm,integrator-cp", },
 
+	{ .compatible = "hisilicon,hi3660", },
 	{ .compatible = "hisilicon,hi6220", },
 
 	{ .compatible = "fsl,imx27", },
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index 26b643d57847..9bf97a366029 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -632,11 +632,21 @@ show_one(cpuinfo_transition_latency, cpuinfo.transition_latency);
 show_one(scaling_min_freq, min);
 show_one(scaling_max_freq, max);
 
+__weak unsigned int arch_freq_get_on_cpu(int cpu)
+{
+	return 0;
+}
+
 static ssize_t show_scaling_cur_freq(struct cpufreq_policy *policy, char *buf)
 {
 	ssize_t ret;
+	unsigned int freq;
 
-	if (cpufreq_driver && cpufreq_driver->setpolicy && cpufreq_driver->get)
+	freq = arch_freq_get_on_cpu(policy->cpu);
+	if (freq)
+		ret = sprintf(buf, "%u\n", freq);
+	else if (cpufreq_driver && cpufreq_driver->setpolicy &&
+			cpufreq_driver->get)
 		ret = sprintf(buf, "%u\n", cpufreq_driver->get(policy->cpu));
 	else
 		ret = sprintf(buf, "%u\n", policy->cur);
@@ -887,7 +897,7 @@ static ssize_t store(struct kobject *kobj, struct attribute *attr,
 	struct freq_attr *fattr = to_attr(attr);
 	ssize_t ret = -EINVAL;
 
-	get_online_cpus();
+	cpus_read_lock();
 
 	if (cpu_online(policy->cpu)) {
 		down_write(&policy->rwsem);
@@ -895,7 +905,7 @@ static ssize_t store(struct kobject *kobj, struct attribute *attr,
 		up_write(&policy->rwsem);
 	}
 
-	put_online_cpus();
+	cpus_read_unlock();
 
 	return ret;
 }
@@ -2441,7 +2451,7 @@ int cpufreq_register_driver(struct cpufreq_driver *driver_data)
 	pr_debug("trying to register driver %s\n", driver_data->name);
 
 	/* Protect against concurrent CPU online/offline. */
-	get_online_cpus();
+	cpus_read_lock();
 
 	write_lock_irqsave(&cpufreq_driver_lock, flags);
 	if (cpufreq_driver) {
@@ -2474,9 +2484,10 @@ int cpufreq_register_driver(struct cpufreq_driver *driver_data)
 		goto err_if_unreg;
 	}
 
-	ret = cpuhp_setup_state_nocalls(CPUHP_AP_ONLINE_DYN, "cpufreq:online",
-					cpuhp_cpufreq_online,
-					cpuhp_cpufreq_offline);
+	ret = cpuhp_setup_state_nocalls_cpuslocked(CPUHP_AP_ONLINE_DYN,
+						   "cpufreq:online",
+						   cpuhp_cpufreq_online,
+						   cpuhp_cpufreq_offline);
 	if (ret < 0)
 		goto err_if_unreg;
 	hp_online = ret;
@@ -2494,7 +2505,7 @@ err_null_driver:
 	cpufreq_driver = NULL;
 	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 out:
-	put_online_cpus();
+	cpus_read_unlock();
 	return ret;
 }
 EXPORT_SYMBOL_GPL(cpufreq_register_driver);
@@ -2517,17 +2528,17 @@ int cpufreq_unregister_driver(struct cpufreq_driver *driver)
 	pr_debug("unregistering driver %s\n", driver->name);
 
 	/* Protect against concurrent cpu hotplug */
-	get_online_cpus();
+	cpus_read_lock();
 	subsys_interface_unregister(&cpufreq_interface);
 	remove_boost_sysfs_file();
-	cpuhp_remove_state_nocalls(hp_online);
+	cpuhp_remove_state_nocalls_cpuslocked(hp_online);
 
 	write_lock_irqsave(&cpufreq_driver_lock, flags);
 
 	cpufreq_driver = NULL;
 
 	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
-	put_online_cpus();
+	cpus_read_unlock();
 
 	return 0;
 }
diff --git a/drivers/cpufreq/cpufreq_conservative.c b/drivers/cpufreq/cpufreq_conservative.c
index 992f7c20760f..88220ff3e1c2 100644
--- a/drivers/cpufreq/cpufreq_conservative.c
+++ b/drivers/cpufreq/cpufreq_conservative.c
@@ -185,8 +185,8 @@ static ssize_t store_down_threshold(struct gov_attr_set *attr_set,
 	int ret;
 	ret = sscanf(buf, "%u", &input);
 
-	/* cannot be lower than 11 otherwise freq will not fall */
-	if (ret != 1 || input < 11 || input > 100 ||
+	/* cannot be lower than 1 otherwise freq will not fall */
+	if (ret != 1 || input < 1 || input > 100 ||
 			input >= dbs_data->up_threshold)
 		return -EINVAL;
 
diff --git a/drivers/cpufreq/exynos5440-cpufreq.c b/drivers/cpufreq/exynos5440-cpufreq.c
index 9180d34cc9fc..b6b369c22272 100644
--- a/drivers/cpufreq/exynos5440-cpufreq.c
+++ b/drivers/cpufreq/exynos5440-cpufreq.c
@@ -173,12 +173,12 @@ static void exynos_enable_dvfs(unsigned int cur_frequency)
 	/* Enable PSTATE Change Event */
 	tmp = __raw_readl(dvfs_info->base + XMU_PMUEVTEN);
 	tmp |= (1 << PSTATE_CHANGED_EVTEN_SHIFT);
-	 __raw_writel(tmp, dvfs_info->base + XMU_PMUEVTEN);
+	__raw_writel(tmp, dvfs_info->base + XMU_PMUEVTEN);
 
 	/* Enable PSTATE Change IRQ */
 	tmp = __raw_readl(dvfs_info->base + XMU_PMUIRQEN);
 	tmp |= (1 << PSTATE_CHANGED_IRQEN_SHIFT);
-	 __raw_writel(tmp, dvfs_info->base + XMU_PMUIRQEN);
+	__raw_writel(tmp, dvfs_info->base + XMU_PMUIRQEN);
 
 	/* Set initial performance index */
 	cpufreq_for_each_entry(pos, freq_table)
@@ -330,7 +330,7 @@ static int exynos_cpufreq_probe(struct platform_device *pdev)
 	struct resource res;
 	unsigned int cur_frequency;
 
-	np =  pdev->dev.of_node;
+	np = pdev->dev.of_node;
 	if (!np)
 		return -ENODEV;
 
diff --git a/drivers/cpufreq/imx6q-cpufreq.c b/drivers/cpufreq/imx6q-cpufreq.c
index 9c13f097fd8c..b6edd3ccaa55 100644
--- a/drivers/cpufreq/imx6q-cpufreq.c
+++ b/drivers/cpufreq/imx6q-cpufreq.c
@@ -101,7 +101,8 @@ static int imx6q_set_target(struct cpufreq_policy *policy, unsigned int index)
 	 *  - Reprogram pll1_sys_clk and reparent pll1_sw_clk back to it
 	 *  - Disable pll2_pfd2_396m_clk
 	 */
-	if (of_machine_is_compatible("fsl,imx6ul")) {
+	if (of_machine_is_compatible("fsl,imx6ul") ||
+	    of_machine_is_compatible("fsl,imx6ull")) {
 		/*
 		 * When changing pll1_sw_clk's parent to pll1_sys_clk,
 		 * CPU may run at higher than 528MHz, this will lead to
@@ -215,7 +216,8 @@ static int imx6q_cpufreq_probe(struct platform_device *pdev)
 		goto put_clk;
 	}
 
-	if (of_machine_is_compatible("fsl,imx6ul")) {
+	if (of_machine_is_compatible("fsl,imx6ul") ||
+	    of_machine_is_compatible("fsl,imx6ull")) {
 		pll2_bus_clk = clk_get(cpu_dev, "pll2_bus");
 		secondary_sel_clk = clk_get(cpu_dev, "secondary_sel");
 		if (IS_ERR(pll2_bus_clk) || IS_ERR(secondary_sel_clk)) {
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index eb1158532de3..48a98f11a84e 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -231,10 +231,8 @@ struct global_params {
  * @prev_cummulative_iowait: IO Wait time difference from last and
  *			current sample
  * @sample:		Storage for storing last Sample data
- * @min_perf:		Minimum capacity limit as a fraction of the maximum
- *			turbo P-state capacity.
- * @max_perf:		Maximum capacity limit as a fraction of the maximum
- *			turbo P-state capacity.
+ * @min_perf_ratio:	Minimum capacity in terms of PERF or HWP ratios
+ * @max_perf_ratio:	Maximum capacity in terms of PERF or HWP ratios
  * @acpi_perf_data:	Stores ACPI perf information read from _PSS
  * @valid_pss_table:	Set to true for valid ACPI _PSS entries found
  * @epp_powersave:	Last saved HWP energy performance preference
@@ -266,8 +264,8 @@ struct cpudata {
 	u64	prev_tsc;
 	u64	prev_cummulative_iowait;
 	struct sample sample;
-	int32_t	min_perf;
-	int32_t	max_perf;
+	int32_t	min_perf_ratio;
+	int32_t	max_perf_ratio;
 #ifdef CONFIG_ACPI
 	struct acpi_processor_performance acpi_perf_data;
 	bool valid_pss_table;
@@ -653,6 +651,12 @@ static const char * const energy_perf_strings[] = {
 	"power",
 	NULL
 };
+static const unsigned int epp_values[] = {
+	HWP_EPP_PERFORMANCE,
+	HWP_EPP_BALANCE_PERFORMANCE,
+	HWP_EPP_BALANCE_POWERSAVE,
+	HWP_EPP_POWERSAVE
+};
 
 static int intel_pstate_get_energy_pref_index(struct cpudata *cpu_data)
 {
@@ -664,17 +668,14 @@ static int intel_pstate_get_energy_pref_index(struct cpudata *cpu_data)
 		return epp;
 
 	if (static_cpu_has(X86_FEATURE_HWP_EPP)) {
-		/*
-		 * Range:
-		 *	0x00-0x3F	:	Performance
-		 *	0x40-0x7F	:	Balance performance
-		 *	0x80-0xBF	:	Balance power
-		 *	0xC0-0xFF	:	Power
-		 * The EPP is a 8 bit value, but our ranges restrict the
-		 * value which can be set. Here only using top two bits
-		 * effectively.
-		 */
-		index = (epp >> 6) + 1;
+		if (epp == HWP_EPP_PERFORMANCE)
+			return 1;
+		if (epp <= HWP_EPP_BALANCE_PERFORMANCE)
+			return 2;
+		if (epp <= HWP_EPP_BALANCE_POWERSAVE)
+			return 3;
+		else
+			return 4;
 	} else if (static_cpu_has(X86_FEATURE_EPB)) {
 		/*
 		 * Range:
@@ -712,15 +713,8 @@ static int intel_pstate_set_energy_pref_index(struct cpudata *cpu_data,
 
 		value &= ~GENMASK_ULL(31, 24);
 
-		/*
-		 * If epp is not default, convert from index into
-		 * energy_perf_strings to epp value, by shifting 6
-		 * bits left to use only top two bits in epp.
-		 * The resultant epp need to shifted by 24 bits to
-		 * epp position in MSR_HWP_REQUEST.
-		 */
 		if (epp == -EINVAL)
-			epp = (pref_index - 1) << 6;
+			epp = epp_values[pref_index - 1];
 
 		value |= (u64)epp << 24;
 		ret = wrmsrl_on_cpu(cpu_data->cpu, MSR_HWP_REQUEST, value);
@@ -794,25 +788,32 @@ static struct freq_attr *hwp_cpufreq_attrs[] = {
 	NULL,
 };
 
-static void intel_pstate_hwp_set(unsigned int cpu)
+static void intel_pstate_get_hwp_max(unsigned int cpu, int *phy_max,
+				     int *current_max)
 {
-	struct cpudata *cpu_data = all_cpu_data[cpu];
-	int min, hw_min, max, hw_max;
-	u64 value, cap;
-	s16 epp;
+	u64 cap;
 
 	rdmsrl_on_cpu(cpu, MSR_HWP_CAPABILITIES, &cap);
-	hw_min = HWP_LOWEST_PERF(cap);
 	if (global.no_turbo)
-		hw_max = HWP_GUARANTEED_PERF(cap);
+		*current_max = HWP_GUARANTEED_PERF(cap);
 	else
-		hw_max = HWP_HIGHEST_PERF(cap);
+		*current_max = HWP_HIGHEST_PERF(cap);
+
+	*phy_max = HWP_HIGHEST_PERF(cap);
+}
+
+static void intel_pstate_hwp_set(unsigned int cpu)
+{
+	struct cpudata *cpu_data = all_cpu_data[cpu];
+	int max, min;
+	u64 value;
+	s16 epp;
+
+	max = cpu_data->max_perf_ratio;
+	min = cpu_data->min_perf_ratio;
 
-	max = fp_ext_toint(hw_max * cpu_data->max_perf);
 	if (cpu_data->policy == CPUFREQ_POLICY_PERFORMANCE)
 		min = max;
-	else
-		min = fp_ext_toint(hw_max * cpu_data->min_perf);
 
 	rdmsrl_on_cpu(cpu, MSR_HWP_REQUEST, &value);
 
@@ -1528,8 +1529,7 @@ static void intel_pstate_max_within_limits(struct cpudata *cpu)
 
 	update_turbo_state();
 	pstate = intel_pstate_get_base_pstate(cpu);
-	pstate = max(cpu->pstate.min_pstate,
-		     fp_ext_toint(pstate * cpu->max_perf));
+	pstate = max(cpu->pstate.min_pstate, cpu->max_perf_ratio);
 	intel_pstate_set_pstate(cpu, pstate);
 }
 
@@ -1616,9 +1616,6 @@ static inline int32_t get_target_pstate_use_cpu_load(struct cpudata *cpu)
 	int32_t busy_frac, boost;
 	int target, avg_pstate;
 
-	if (cpu->policy == CPUFREQ_POLICY_PERFORMANCE)
-		return cpu->pstate.turbo_pstate;
-
 	busy_frac = div_fp(sample->mperf, sample->tsc);
 
 	boost = cpu->iowait_boost;
@@ -1655,9 +1652,6 @@ static inline int32_t get_target_pstate_use_performance(struct cpudata *cpu)
 	int32_t perf_scaled, max_pstate, current_pstate, sample_ratio;
 	u64 duration_ns;
 
-	if (cpu->policy == CPUFREQ_POLICY_PERFORMANCE)
-		return cpu->pstate.turbo_pstate;
-
 	/*
 	 * perf_scaled is the ratio of the average P-state during the last
 	 * sampling period to the P-state requested last time (in percent).
@@ -1695,9 +1689,8 @@ static int intel_pstate_prepare_request(struct cpudata *cpu, int pstate)
 	int max_pstate = intel_pstate_get_base_pstate(cpu);
 	int min_pstate;
 
-	min_pstate = max(cpu->pstate.min_pstate,
-			 fp_ext_toint(max_pstate * cpu->min_perf));
-	max_pstate = max(min_pstate, fp_ext_toint(max_pstate * cpu->max_perf));
+	min_pstate = max(cpu->pstate.min_pstate, cpu->min_perf_ratio);
+	max_pstate = max(min_pstate, cpu->max_perf_ratio);
 	return clamp_t(int, pstate, min_pstate, max_pstate);
 }
 
@@ -1733,16 +1726,6 @@ static void intel_pstate_adjust_pstate(struct cpudata *cpu, int target_pstate)
 		fp_toint(cpu->iowait_boost * 100));
 }
 
-static void intel_pstate_update_util_hwp(struct update_util_data *data,
-					 u64 time, unsigned int flags)
-{
-	struct cpudata *cpu = container_of(data, struct cpudata, update_util);
-	u64 delta_ns = time - cpu->sample.time;
-
-	if ((s64)delta_ns >= INTEL_PSTATE_HWP_SAMPLING_INTERVAL)
-		intel_pstate_sample(cpu, time);
-}
-
 static void intel_pstate_update_util_pid(struct update_util_data *data,
 					 u64 time, unsigned int flags)
 {
@@ -1934,6 +1917,9 @@ static void intel_pstate_set_update_util_hook(unsigned int cpu_num)
 {
 	struct cpudata *cpu = all_cpu_data[cpu_num];
 
+	if (hwp_active)
+		return;
+
 	if (cpu->update_util_set)
 		return;
 
@@ -1967,52 +1953,61 @@ static void intel_pstate_update_perf_limits(struct cpufreq_policy *policy,
 {
 	int max_freq = intel_pstate_get_max_freq(cpu);
 	int32_t max_policy_perf, min_policy_perf;
+	int max_state, turbo_max;
 
-	max_policy_perf = div_ext_fp(policy->max, max_freq);
-	max_policy_perf = clamp_t(int32_t, max_policy_perf, 0, int_ext_tofp(1));
+	/*
+	 * HWP needs some special consideration, because on BDX the
+	 * HWP_REQUEST uses abstract value to represent performance
+	 * rather than pure ratios.
+	 */
+	if (hwp_active) {
+		intel_pstate_get_hwp_max(cpu->cpu, &turbo_max, &max_state);
+	} else {
+		max_state = intel_pstate_get_base_pstate(cpu);
+		turbo_max = cpu->pstate.turbo_pstate;
+	}
+
+	max_policy_perf = max_state * policy->max / max_freq;
 	if (policy->max == policy->min) {
 		min_policy_perf = max_policy_perf;
 	} else {
-		min_policy_perf = div_ext_fp(policy->min, max_freq);
+		min_policy_perf = max_state * policy->min / max_freq;
 		min_policy_perf = clamp_t(int32_t, min_policy_perf,
 					  0, max_policy_perf);
 	}
 
+	pr_debug("cpu:%d max_state %d min_policy_perf:%d max_policy_perf:%d\n",
+		 policy->cpu, max_state,
+		 min_policy_perf, max_policy_perf);
+
 	/* Normalize user input to [min_perf, max_perf] */
 	if (per_cpu_limits) {
-		cpu->min_perf = min_policy_perf;
-		cpu->max_perf = max_policy_perf;
+		cpu->min_perf_ratio = min_policy_perf;
+		cpu->max_perf_ratio = max_policy_perf;
 	} else {
 		int32_t global_min, global_max;
 
 		/* Global limits are in percent of the maximum turbo P-state. */
-		global_max = percent_ext_fp(global.max_perf_pct);
-		global_min = percent_ext_fp(global.min_perf_pct);
-		if (max_freq != cpu->pstate.turbo_freq) {
-			int32_t turbo_factor;
-
-			turbo_factor = div_ext_fp(cpu->pstate.turbo_pstate,
-						  cpu->pstate.max_pstate);
-			global_min = mul_ext_fp(global_min, turbo_factor);
-			global_max = mul_ext_fp(global_max, turbo_factor);
-		}
+		global_max = DIV_ROUND_UP(turbo_max * global.max_perf_pct, 100);
+		global_min = DIV_ROUND_UP(turbo_max * global.min_perf_pct, 100);
 		global_min = clamp_t(int32_t, global_min, 0, global_max);
 
-		cpu->min_perf = max(min_policy_perf, global_min);
-		cpu->min_perf = min(cpu->min_perf, max_policy_perf);
-		cpu->max_perf = min(max_policy_perf, global_max);
-		cpu->max_perf = max(min_policy_perf, cpu->max_perf);
+		pr_debug("cpu:%d global_min:%d global_max:%d\n", policy->cpu,
+			 global_min, global_max);
 
-		/* Make sure min_perf <= max_perf */
-		cpu->min_perf = min(cpu->min_perf, cpu->max_perf);
-	}
+		cpu->min_perf_ratio = max(min_policy_perf, global_min);
+		cpu->min_perf_ratio = min(cpu->min_perf_ratio, max_policy_perf);
+		cpu->max_perf_ratio = min(max_policy_perf, global_max);
+		cpu->max_perf_ratio = max(min_policy_perf, cpu->max_perf_ratio);
 
-	cpu->max_perf = round_up(cpu->max_perf, EXT_FRAC_BITS);
-	cpu->min_perf = round_up(cpu->min_perf, EXT_FRAC_BITS);
+		/* Make sure min_perf <= max_perf */
+		cpu->min_perf_ratio = min(cpu->min_perf_ratio,
+					  cpu->max_perf_ratio);
 
-	pr_debug("cpu:%d max_perf_pct:%d min_perf_pct:%d\n", policy->cpu,
-		 fp_ext_toint(cpu->max_perf * 100),
-		 fp_ext_toint(cpu->min_perf * 100));
+	}
+	pr_debug("cpu:%d max_perf_ratio:%d min_perf_ratio:%d\n", policy->cpu,
+		 cpu->max_perf_ratio,
+		 cpu->min_perf_ratio);
 }
 
 static int intel_pstate_set_policy(struct cpufreq_policy *policy)
@@ -2039,10 +2034,10 @@ static int intel_pstate_set_policy(struct cpufreq_policy *policy)
 		 */
 		intel_pstate_clear_update_util_hook(policy->cpu);
 		intel_pstate_max_within_limits(cpu);
+	} else {
+		intel_pstate_set_update_util_hook(policy->cpu);
 	}
 
-	intel_pstate_set_update_util_hook(policy->cpu);
-
 	if (hwp_active)
 		intel_pstate_hwp_set(policy->cpu);
 
@@ -2115,8 +2110,8 @@ static int __intel_pstate_cpu_init(struct cpufreq_policy *policy)
 
 	cpu = all_cpu_data[policy->cpu];
 
-	cpu->max_perf = int_ext_tofp(1);
-	cpu->min_perf = 0;
+	cpu->max_perf_ratio = 0xFF;
+	cpu->min_perf_ratio = 0;
 
 	policy->min = cpu->pstate.min_pstate * cpu->pstate.scaling;
 	policy->max = cpu->pstate.turbo_pstate * cpu->pstate.scaling;
@@ -2558,7 +2553,6 @@ static int __init intel_pstate_init(void)
 		} else {
 			hwp_active++;
 			intel_pstate.attr = hwp_cpufreq_attrs;
-			pstate_funcs.update_util = intel_pstate_update_util_hwp;
 			goto hwp_cpu_matched;
 		}
 	} else {
diff --git a/drivers/cpufreq/pasemi-cpufreq.c b/drivers/cpufreq/pasemi-cpufreq.c
index 35dd4d7ffee0..b257fc7d5204 100644
--- a/drivers/cpufreq/pasemi-cpufreq.c
+++ b/drivers/cpufreq/pasemi-cpufreq.c
@@ -226,7 +226,7 @@ static int pas_cpufreq_cpu_exit(struct cpufreq_policy *policy)
 	 * We don't support CPU hotplug. Don't unmap after the system
 	 * has already made it to a running state.
 	 */
-	if (system_state != SYSTEM_BOOTING)
+	if (system_state >= SYSTEM_RUNNING)
 		return 0;
 
 	if (sdcasr_mapbase)
diff --git a/drivers/cpufreq/scpi-cpufreq.c b/drivers/cpufreq/scpi-cpufreq.c
index ea7a4e1b68c2..8de2364b5995 100644
--- a/drivers/cpufreq/scpi-cpufreq.c
+++ b/drivers/cpufreq/scpi-cpufreq.c
@@ -30,46 +30,20 @@
 
 static struct scpi_ops *scpi_ops;
 
-static struct scpi_dvfs_info *scpi_get_dvfs_info(struct device *cpu_dev)
-{
-	int domain = topology_physical_package_id(cpu_dev->id);
-
-	if (domain < 0)
-		return ERR_PTR(-EINVAL);
-	return scpi_ops->dvfs_get_info(domain);
-}
-
 static int scpi_get_transition_latency(struct device *cpu_dev)
 {
-	struct scpi_dvfs_info *info = scpi_get_dvfs_info(cpu_dev);
-
-	if (IS_ERR(info))
-		return PTR_ERR(info);
-	return info->latency;
+	return scpi_ops->get_transition_latency(cpu_dev);
 }
 
 static int scpi_init_opp_table(const struct cpumask *cpumask)
 {
-	int idx, ret;
-	struct scpi_opp *opp;
+	int ret;
 	struct device *cpu_dev = get_cpu_device(cpumask_first(cpumask));
-	struct scpi_dvfs_info *info = scpi_get_dvfs_info(cpu_dev);
-
-	if (IS_ERR(info))
-		return PTR_ERR(info);
-
-	if (!info->opps)
-		return -EIO;
 
-	for (opp = info->opps, idx = 0; idx < info->count; idx++, opp++) {
-		ret = dev_pm_opp_add(cpu_dev, opp->freq, opp->m_volt * 1000);
-		if (ret) {
-			dev_warn(cpu_dev, "failed to add opp %uHz %umV\n",
-				 opp->freq, opp->m_volt);
-			while (idx-- > 0)
-				dev_pm_opp_remove(cpu_dev, (--opp)->freq);
-			return ret;
-		}
+	ret = scpi_ops->add_opps_to_device(cpu_dev);
+	if (ret) {
+		dev_warn(cpu_dev, "failed to add opps to the device\n");
+		return ret;
 	}
 
 	ret = dev_pm_opp_set_sharing_cpus(cpu_dev, cpumask);
diff --git a/drivers/cpufreq/sfi-cpufreq.c b/drivers/cpufreq/sfi-cpufreq.c
index 992ce6f9abec..3779742f86e3 100644
--- a/drivers/cpufreq/sfi-cpufreq.c
+++ b/drivers/cpufreq/sfi-cpufreq.c
@@ -24,7 +24,7 @@
 
 #include <asm/msr.h>
 
-struct cpufreq_frequency_table *freq_table;
+static struct cpufreq_frequency_table *freq_table;
 static struct sfi_freq_table_entry *sfi_cpufreq_array;
 static int num_freq_table_entries;
 
diff --git a/drivers/cpuidle/Kconfig.arm b/drivers/cpuidle/Kconfig.arm
index 21340e0be73e..f52144808455 100644
--- a/drivers/cpuidle/Kconfig.arm
+++ b/drivers/cpuidle/Kconfig.arm
@@ -4,6 +4,7 @@
 config ARM_CPUIDLE
         bool "Generic ARM/ARM64 CPU idle Driver"
         select DT_IDLE_STATES
+	select CPU_IDLE_MULTIPLE_DRIVERS
         help
           Select this to enable generic cpuidle driver for ARM.
           It provides a generic idle driver whose idle states are configured
diff --git a/drivers/cpuidle/cpuidle-arm.c b/drivers/cpuidle/cpuidle-arm.c
index f440d385ed34..7080c384ad5d 100644
--- a/drivers/cpuidle/cpuidle-arm.c
+++ b/drivers/cpuidle/cpuidle-arm.c
@@ -18,6 +18,7 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/slab.h>
+#include <linux/topology.h>
 
 #include <asm/cpuidle.h>
 
@@ -44,7 +45,7 @@ static int arm_enter_idle_state(struct cpuidle_device *dev,
 	return CPU_PM_CPU_IDLE_ENTER(arm_cpuidle_suspend, idx);
 }
 
-static struct cpuidle_driver arm_idle_driver = {
+static struct cpuidle_driver arm_idle_driver __initdata = {
 	.name = "arm_idle",
 	.owner = THIS_MODULE,
 	/*
@@ -80,30 +81,42 @@ static const struct of_device_id arm_idle_state_match[] __initconst = {
 static int __init arm_idle_init(void)
 {
 	int cpu, ret;
-	struct cpuidle_driver *drv = &arm_idle_driver;
+	struct cpuidle_driver *drv;
 	struct cpuidle_device *dev;
 
-	/*
-	 * Initialize idle states data, starting at index 1.
-	 * This driver is DT only, if no DT idle states are detected (ret == 0)
-	 * let the driver initialization fail accordingly since there is no
-	 * reason to initialize the idle driver if only wfi is supported.
-	 */
-	ret = dt_init_idle_driver(drv, arm_idle_state_match, 1);
-	if (ret <= 0)
-		return ret ? : -ENODEV;
-
-	ret = cpuidle_register_driver(drv);
-	if (ret) {
-		pr_err("Failed to register cpuidle driver\n");
-		return ret;
-	}
-
-	/*
-	 * Call arch CPU operations in order to initialize
-	 * idle states suspend back-end specific data
-	 */
 	for_each_possible_cpu(cpu) {
+
+		drv = kmemdup(&arm_idle_driver, sizeof(*drv), GFP_KERNEL);
+		if (!drv) {
+			ret = -ENOMEM;
+			goto out_fail;
+		}
+
+		drv->cpumask = (struct cpumask *)cpumask_of(cpu);
+
+		/*
+		 * Initialize idle states data, starting at index 1.  This
+		 * driver is DT only, if no DT idle states are detected (ret
+		 * == 0) let the driver initialization fail accordingly since
+		 * there is no reason to initialize the idle driver if only
+		 * wfi is supported.
+		 */
+		ret = dt_init_idle_driver(drv, arm_idle_state_match, 1);
+		if (ret <= 0) {
+			ret = ret ? : -ENODEV;
+			goto out_fail;
+		}
+
+		ret = cpuidle_register_driver(drv);
+		if (ret) {
+			pr_err("Failed to register cpuidle driver\n");
+			goto out_fail;
+		}
+
+		/*
+		 * Call arch CPU operations in order to initialize
+		 * idle states suspend back-end specific data
+		 */
 		ret = arm_cpuidle_init(cpu);
 
 		/*
@@ -141,10 +154,11 @@ out_fail:
 		dev = per_cpu(cpuidle_devices, cpu);
 		cpuidle_unregister_device(dev);
 		kfree(dev);
+		drv = cpuidle_get_driver();
+		cpuidle_unregister_driver(drv);
+		kfree(drv);
 	}
 
-	cpuidle_unregister_driver(drv);
-
 	return ret;
 }
 device_initcall(arm_idle_init);
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index 2706be7ed334..60bb64f4329d 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -220,6 +220,7 @@ int cpuidle_enter_state(struct cpuidle_device *dev, struct cpuidle_driver *drv,
 	entered_state = target_state->enter(dev, drv, index);
 	start_critical_timings();
 
+	sched_clock_idle_wakeup_event();
 	time_end = ns_to_ktime(local_clock());
 	trace_cpu_idle_rcuidle(PWR_EVENT_EXIT, dev->cpu);
 
diff --git a/drivers/cpuidle/dt_idle_states.c b/drivers/cpuidle/dt_idle_states.c
index ffca4fc0061d..ae8eb0359889 100644
--- a/drivers/cpuidle/dt_idle_states.c
+++ b/drivers/cpuidle/dt_idle_states.c
@@ -180,8 +180,10 @@ int dt_init_idle_driver(struct cpuidle_driver *drv,
 		if (!state_node)
 			break;
 
-		if (!of_device_is_available(state_node))
+		if (!of_device_is_available(state_node)) {
+			of_node_put(state_node);
 			continue;
+		}
 
 		if (!idle_state_valid(state_node, i, cpumask)) {
 			pr_warn("%s idle state not valid, bailing out\n",
diff --git a/drivers/cpuidle/governors/menu.c b/drivers/cpuidle/governors/menu.c
index b2330fd69e34..61b64c2b2cb8 100644
--- a/drivers/cpuidle/governors/menu.c
+++ b/drivers/cpuidle/governors/menu.c
@@ -286,6 +286,8 @@ static int menu_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 	struct device *device = get_cpu_device(dev->cpu);
 	int latency_req = pm_qos_request(PM_QOS_CPU_DMA_LATENCY);
 	int i;
+	int first_idx;
+	int idx;
 	unsigned int interactivity_req;
 	unsigned int expected_interval;
 	unsigned long nr_iowaiters, cpu_load;
@@ -335,11 +337,11 @@ static int menu_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 		if (data->next_timer_us > polling_threshold &&
 		    latency_req > s->exit_latency && !s->disabled &&
 		    !dev->states_usage[CPUIDLE_DRIVER_STATE_START].disable)
-			data->last_state_idx = CPUIDLE_DRIVER_STATE_START;
+			first_idx = CPUIDLE_DRIVER_STATE_START;
 		else
-			data->last_state_idx = CPUIDLE_DRIVER_STATE_START - 1;
+			first_idx = CPUIDLE_DRIVER_STATE_START - 1;
 	} else {
-		data->last_state_idx = CPUIDLE_DRIVER_STATE_START;
+		first_idx = 0;
 	}
 
 	/*
@@ -359,20 +361,28 @@ static int menu_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 	 * Find the idle state with the lowest power while satisfying
 	 * our constraints.
 	 */
-	for (i = data->last_state_idx + 1; i < drv->state_count; i++) {
+	idx = -1;
+	for (i = first_idx; i < drv->state_count; i++) {
 		struct cpuidle_state *s = &drv->states[i];
 		struct cpuidle_state_usage *su = &dev->states_usage[i];
 
 		if (s->disabled || su->disable)
 			continue;
+		if (idx == -1)
+			idx = i; /* first enabled state */
 		if (s->target_residency > data->predicted_us)
 			break;
 		if (s->exit_latency > latency_req)
 			break;
 
-		data->last_state_idx = i;
+		idx = i;
 	}
 
+	if (idx == -1)
+		idx = 0; /* No states enabled. Must use 0. */
+
+	data->last_state_idx = idx;
+
 	return data->last_state_idx;
 }
 
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index fb1e60f5002e..193204dfbf3a 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -89,6 +89,20 @@ config PKEY
 	  requires to have at least one CEX card in coprocessor mode
 	  available at runtime.
 
+config CRYPTO_PAES_S390
+	tristate "PAES cipher algorithms"
+	depends on S390
+	depends on ZCRYPT
+	depends on PKEY
+	select CRYPTO_ALGAPI
+	select CRYPTO_BLKCIPHER
+	help
+	  This is the s390 hardware accelerated implementation of the
+	  AES cipher algorithms for use with protected key.
+
+	  Select this option if you want to use the paes cipher
+	  for example to use protected key encrypted devices.
+
 config CRYPTO_SHA1_S390
 	tristate "SHA1 digest algorithm"
 	depends on S390
@@ -137,7 +151,6 @@ config CRYPTO_AES_S390
 	depends on S390
 	select CRYPTO_ALGAPI
 	select CRYPTO_BLKCIPHER
-	select PKEY
 	help
 	  This is the s390 hardware accelerated implementation of the
 	  AES cipher algorithms (FIPS-197).
@@ -314,6 +327,15 @@ config HW_RANDOM_PPC4XX
 	 This option provides the kernel-side support for the TRNG hardware
 	 found in the security function of some PowerPC 4xx SoCs.
 
+config CRYPTO_DEV_OMAP
+	tristate "Support for OMAP crypto HW accelerators"
+	depends on ARCH_OMAP2PLUS
+	help
+	  OMAP processors have various crypto HW accelerators. Select this if
+          you want to use the OMAP modules for any of the crypto algorithms.
+
+if CRYPTO_DEV_OMAP
+
 config CRYPTO_DEV_OMAP_SHAM
 	tristate "Support for OMAP MD5/SHA1/SHA2 hw accelerator"
 	depends on ARCH_OMAP2PLUS
@@ -335,6 +357,7 @@ config CRYPTO_DEV_OMAP_AES
 	select CRYPTO_CBC
 	select CRYPTO_ECB
 	select CRYPTO_CTR
+	select CRYPTO_AEAD
 	help
 	  OMAP processors have AES module accelerator. Select this if you
 	  want to use the OMAP module for AES algorithms.
@@ -351,6 +374,8 @@ config CRYPTO_DEV_OMAP_DES
 	  the ECB and CBC modes of operation are supported by the driver. Also
 	  accesses made on unaligned boundaries are supported.
 
+endif # CRYPTO_DEV_OMAP
+
 config CRYPTO_DEV_PICOXCELL
 	tristate "Support for picoXcell IPSEC and Layer2 crypto engines"
 	depends on (ARCH_PICOXCELL || COMPILE_TEST) && HAVE_CLK
@@ -529,6 +554,7 @@ config CRYPTO_DEV_MXS_DCP
 
 source "drivers/crypto/qat/Kconfig"
 source "drivers/crypto/cavium/cpt/Kconfig"
+source "drivers/crypto/cavium/nitrox/Kconfig"
 
 config CRYPTO_DEV_CAVIUM_ZIP
 	tristate "Cavium ZIP driver"
@@ -643,4 +669,21 @@ config CRYPTO_DEV_BCM_SPU
 
 source "drivers/crypto/stm32/Kconfig"
 
+config CRYPTO_DEV_SAFEXCEL
+	tristate "Inside Secure's SafeXcel cryptographic engine driver"
+	depends on HAS_DMA && OF
+	depends on (ARM64 && ARCH_MVEBU) || (COMPILE_TEST && 64BIT)
+	select CRYPTO_AES
+	select CRYPTO_BLKCIPHER
+	select CRYPTO_HASH
+	select CRYPTO_HMAC
+	select CRYPTO_SHA1
+	select CRYPTO_SHA256
+	select CRYPTO_SHA512
+	help
+	  This driver interfaces with the SafeXcel EIP-197 cryptographic engine
+	  designed by Inside Secure. Select this if you want to use CBC/ECB
+	  chain mode, AES cipher mode and SHA1/SHA224/SHA256/SHA512 hash
+	  algorithms.
+
 endif # CRYPTO_HW
diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index 463f33592d93..2c555a3393b2 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -6,6 +6,7 @@ obj-$(CONFIG_CRYPTO_DEV_CAVIUM_ZIP) += cavium/
 obj-$(CONFIG_CRYPTO_DEV_CCP) += ccp/
 obj-$(CONFIG_CRYPTO_DEV_CHELSIO) += chelsio/
 obj-$(CONFIG_CRYPTO_DEV_CPT) += cavium/cpt/
+obj-$(CONFIG_CRYPTO_DEV_NITROX) += cavium/nitrox/
 obj-$(CONFIG_CRYPTO_DEV_EXYNOS_RNG) += exynos-rng.o
 obj-$(CONFIG_CRYPTO_DEV_FSL_CAAM) += caam/
 obj-$(CONFIG_CRYPTO_DEV_GEODE) += geode-aes.o
@@ -20,7 +21,9 @@ obj-$(CONFIG_CRYPTO_DEV_MXC_SCC) += mxc-scc.o
 obj-$(CONFIG_CRYPTO_DEV_NIAGARA2) += n2_crypto.o
 n2_crypto-y := n2_core.o n2_asm.o
 obj-$(CONFIG_CRYPTO_DEV_NX) += nx/
-obj-$(CONFIG_CRYPTO_DEV_OMAP_AES) += omap-aes.o
+obj-$(CONFIG_CRYPTO_DEV_OMAP) += omap-crypto.o
+obj-$(CONFIG_CRYPTO_DEV_OMAP_AES) += omap-aes-driver.o
+omap-aes-driver-objs := omap-aes.o omap-aes-gcm.o
 obj-$(CONFIG_CRYPTO_DEV_OMAP_DES) += omap-des.o
 obj-$(CONFIG_CRYPTO_DEV_OMAP_SHAM) += omap-sham.o
 obj-$(CONFIG_CRYPTO_DEV_PADLOCK_AES) += padlock-aes.o
@@ -39,3 +42,4 @@ obj-$(CONFIG_CRYPTO_DEV_UX500) += ux500/
 obj-$(CONFIG_CRYPTO_DEV_VIRTIO) += virtio/
 obj-$(CONFIG_CRYPTO_DEV_VMX) += vmx/
 obj-$(CONFIG_CRYPTO_DEV_BCM_SPU) += bcm/
+obj-$(CONFIG_CRYPTO_DEV_SAFEXCEL) += inside-secure/
diff --git a/drivers/crypto/amcc/crypto4xx_core.c b/drivers/crypto/amcc/crypto4xx_core.c
index fdc83a2281ca..65dc78b91dea 100644
--- a/drivers/crypto/amcc/crypto4xx_core.c
+++ b/drivers/crypto/amcc/crypto4xx_core.c
@@ -1179,6 +1179,7 @@ static int crypto4xx_probe(struct platform_device *ofdev)
 	dev_set_drvdata(dev, core_dev);
 	core_dev->ofdev = ofdev;
 	core_dev->dev = kzalloc(sizeof(struct crypto4xx_device), GFP_KERNEL);
+	rc = -ENOMEM;
 	if (!core_dev->dev)
 		goto err_alloc_dev;
 
diff --git a/drivers/crypto/bcm/cipher.c b/drivers/crypto/bcm/cipher.c
index cc0d5b98006e..9cfd36c1bcb6 100644
--- a/drivers/crypto/bcm/cipher.c
+++ b/drivers/crypto/bcm/cipher.c
@@ -36,6 +36,7 @@
 #include <crypto/internal/aead.h>
 #include <crypto/aes.h>
 #include <crypto/des.h>
+#include <crypto/hmac.h>
 #include <crypto/sha.h>
 #include <crypto/md5.h>
 #include <crypto/authenc.h>
@@ -2510,8 +2511,8 @@ static int ahash_hmac_setkey(struct crypto_ahash *ahash, const u8 *key,
 		memcpy(ctx->opad, ctx->ipad, blocksize);
 
 		for (index = 0; index < blocksize; index++) {
-			ctx->ipad[index] ^= 0x36;
-			ctx->opad[index] ^= 0x5c;
+			ctx->ipad[index] ^= HMAC_IPAD_VALUE;
+			ctx->opad[index] ^= HMAC_OPAD_VALUE;
 		}
 
 		flow_dump("  ipad: ", ctx->ipad, blocksize);
@@ -2638,7 +2639,7 @@ static int aead_need_fallback(struct aead_request *req)
 	    (spu->spu_type == SPU_TYPE_SPUM) &&
 	    (ctx->digestsize != 8) && (ctx->digestsize != 12) &&
 	    (ctx->digestsize != 16)) {
-		flow_log("%s() AES CCM needs fallbck for digest size %d\n",
+		flow_log("%s() AES CCM needs fallback for digest size %d\n",
 			 __func__, ctx->digestsize);
 		return 1;
 	}
diff --git a/drivers/crypto/caam/caamalg.c b/drivers/crypto/caam/caamalg.c
index 398807d1b77e..fde399c88779 100644
--- a/drivers/crypto/caam/caamalg.c
+++ b/drivers/crypto/caam/caamalg.c
@@ -1187,8 +1187,8 @@ static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
 	struct crypto_aead *aead = crypto_aead_reqtfm(req);
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
 	struct device *jrdev = ctx->jrdev;
-	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
-		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		       GFP_KERNEL : GFP_ATOMIC;
 	int src_nents, mapped_src_nents, dst_nents = 0, mapped_dst_nents = 0;
 	struct aead_edesc *edesc;
 	int sec4_sg_index, sec4_sg_len, sec4_sg_bytes;
@@ -1475,8 +1475,7 @@ static struct ablkcipher_edesc *ablkcipher_edesc_alloc(struct ablkcipher_request
 	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
 	struct caam_ctx *ctx = crypto_ablkcipher_ctx(ablkcipher);
 	struct device *jrdev = ctx->jrdev;
-	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
-					  CRYPTO_TFM_REQ_MAY_SLEEP)) ?
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
 		       GFP_KERNEL : GFP_ATOMIC;
 	int src_nents, mapped_src_nents, dst_nents = 0, mapped_dst_nents = 0;
 	struct ablkcipher_edesc *edesc;
@@ -1681,8 +1680,7 @@ static struct ablkcipher_edesc *ablkcipher_giv_edesc_alloc(
 	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
 	struct caam_ctx *ctx = crypto_ablkcipher_ctx(ablkcipher);
 	struct device *jrdev = ctx->jrdev;
-	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
-					  CRYPTO_TFM_REQ_MAY_SLEEP)) ?
+	gfp_t flags = (req->base.flags &  CRYPTO_TFM_REQ_MAY_SLEEP) ?
 		       GFP_KERNEL : GFP_ATOMIC;
 	int src_nents, mapped_src_nents, dst_nents, mapped_dst_nents;
 	struct ablkcipher_edesc *edesc;
diff --git a/drivers/crypto/caam/caamalg_qi.c b/drivers/crypto/caam/caamalg_qi.c
index ea0e5b8b9171..78c4c0485c58 100644
--- a/drivers/crypto/caam/caamalg_qi.c
+++ b/drivers/crypto/caam/caamalg_qi.c
@@ -555,8 +555,8 @@ static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
 	struct caam_aead_alg *alg = container_of(crypto_aead_alg(aead),
 						 typeof(*alg), aead);
 	struct device *qidev = ctx->qidev;
-	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
-		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		       GFP_KERNEL : GFP_ATOMIC;
 	int src_nents, mapped_src_nents, dst_nents = 0, mapped_dst_nents = 0;
 	struct aead_edesc *edesc;
 	dma_addr_t qm_sg_dma, iv_dma = 0;
@@ -808,8 +808,7 @@ static struct ablkcipher_edesc *ablkcipher_edesc_alloc(struct ablkcipher_request
 	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
 	struct caam_ctx *ctx = crypto_ablkcipher_ctx(ablkcipher);
 	struct device *qidev = ctx->qidev;
-	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
-					  CRYPTO_TFM_REQ_MAY_SLEEP)) ?
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
 		       GFP_KERNEL : GFP_ATOMIC;
 	int src_nents, mapped_src_nents, dst_nents = 0, mapped_dst_nents = 0;
 	struct ablkcipher_edesc *edesc;
@@ -953,8 +952,7 @@ static struct ablkcipher_edesc *ablkcipher_giv_edesc_alloc(
 	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
 	struct caam_ctx *ctx = crypto_ablkcipher_ctx(ablkcipher);
 	struct device *qidev = ctx->qidev;
-	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
-					  CRYPTO_TFM_REQ_MAY_SLEEP)) ?
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
 		       GFP_KERNEL : GFP_ATOMIC;
 	int src_nents, mapped_src_nents, dst_nents, mapped_dst_nents;
 	struct ablkcipher_edesc *edesc;
diff --git a/drivers/crypto/caam/caamhash.c b/drivers/crypto/caam/caamhash.c
index da4f94eab3da..7c44c90ad593 100644
--- a/drivers/crypto/caam/caamhash.c
+++ b/drivers/crypto/caam/caamhash.c
@@ -719,8 +719,8 @@ static int ahash_update_ctx(struct ahash_request *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;
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		       GFP_KERNEL : GFP_ATOMIC;
 	u8 *buf = current_buf(state);
 	int *buflen = current_buflen(state);
 	u8 *next_buf = alt_buf(state);
@@ -849,8 +849,8 @@ static int ahash_final_ctx(struct ahash_request *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;
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		       GFP_KERNEL : GFP_ATOMIC;
 	int buflen = *current_buflen(state);
 	u32 *desc;
 	int sec4_sg_bytes, sec4_sg_src_index;
@@ -926,8 +926,8 @@ static int ahash_finup_ctx(struct ahash_request *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;
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		       GFP_KERNEL : GFP_ATOMIC;
 	int buflen = *current_buflen(state);
 	u32 *desc;
 	int sec4_sg_src_index;
@@ -1013,8 +1013,8 @@ static int ahash_digest(struct ahash_request *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;
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		       GFP_KERNEL : GFP_ATOMIC;
 	u32 *desc;
 	int digestsize = crypto_ahash_digestsize(ahash);
 	int src_nents, mapped_nents;
@@ -1093,8 +1093,8 @@ static int ahash_final_no_ctx(struct ahash_request *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;
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		       GFP_KERNEL : GFP_ATOMIC;
 	u8 *buf = current_buf(state);
 	int buflen = *current_buflen(state);
 	u32 *desc;
@@ -1154,8 +1154,8 @@ static int ahash_update_no_ctx(struct ahash_request *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;
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		       GFP_KERNEL : GFP_ATOMIC;
 	u8 *buf = current_buf(state);
 	int *buflen = current_buflen(state);
 	u8 *next_buf = alt_buf(state);
@@ -1280,8 +1280,8 @@ static int ahash_finup_no_ctx(struct ahash_request *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;
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		       GFP_KERNEL : GFP_ATOMIC;
 	int buflen = *current_buflen(state);
 	u32 *desc;
 	int sec4_sg_bytes, sec4_sg_src_index, src_nents, mapped_nents;
@@ -1370,8 +1370,8 @@ static int ahash_update_first(struct ahash_request *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;
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		       GFP_KERNEL : GFP_ATOMIC;
 	u8 *next_buf = alt_buf(state);
 	int *next_buflen = alt_buflen(state);
 	int to_hash;
diff --git a/drivers/crypto/caam/caampkc.c b/drivers/crypto/caam/caampkc.c
index 49cbdcba7883..7a897209f181 100644
--- a/drivers/crypto/caam/caampkc.c
+++ b/drivers/crypto/caam/caampkc.c
@@ -18,6 +18,10 @@
 #define DESC_RSA_PUB_LEN	(2 * CAAM_CMD_SZ + sizeof(struct rsa_pub_pdb))
 #define DESC_RSA_PRIV_F1_LEN	(2 * CAAM_CMD_SZ + \
 				 sizeof(struct rsa_priv_f1_pdb))
+#define DESC_RSA_PRIV_F2_LEN	(2 * CAAM_CMD_SZ + \
+				 sizeof(struct rsa_priv_f2_pdb))
+#define DESC_RSA_PRIV_F3_LEN	(2 * CAAM_CMD_SZ + \
+				 sizeof(struct rsa_priv_f3_pdb))
 
 static void rsa_io_unmap(struct device *dev, struct rsa_edesc *edesc,
 			 struct akcipher_request *req)
@@ -54,6 +58,42 @@ static void rsa_priv_f1_unmap(struct device *dev, struct rsa_edesc *edesc,
 	dma_unmap_single(dev, pdb->d_dma, key->d_sz, DMA_TO_DEVICE);
 }
 
+static void rsa_priv_f2_unmap(struct device *dev, struct rsa_edesc *edesc,
+			      struct akcipher_request *req)
+{
+	struct crypto_akcipher *tfm = crypto_akcipher_reqtfm(req);
+	struct caam_rsa_ctx *ctx = akcipher_tfm_ctx(tfm);
+	struct caam_rsa_key *key = &ctx->key;
+	struct rsa_priv_f2_pdb *pdb = &edesc->pdb.priv_f2;
+	size_t p_sz = key->p_sz;
+	size_t q_sz = key->p_sz;
+
+	dma_unmap_single(dev, pdb->d_dma, key->d_sz, DMA_TO_DEVICE);
+	dma_unmap_single(dev, pdb->p_dma, p_sz, DMA_TO_DEVICE);
+	dma_unmap_single(dev, pdb->q_dma, q_sz, DMA_TO_DEVICE);
+	dma_unmap_single(dev, pdb->tmp1_dma, p_sz, DMA_TO_DEVICE);
+	dma_unmap_single(dev, pdb->tmp2_dma, q_sz, DMA_TO_DEVICE);
+}
+
+static void rsa_priv_f3_unmap(struct device *dev, struct rsa_edesc *edesc,
+			      struct akcipher_request *req)
+{
+	struct crypto_akcipher *tfm = crypto_akcipher_reqtfm(req);
+	struct caam_rsa_ctx *ctx = akcipher_tfm_ctx(tfm);
+	struct caam_rsa_key *key = &ctx->key;
+	struct rsa_priv_f3_pdb *pdb = &edesc->pdb.priv_f3;
+	size_t p_sz = key->p_sz;
+	size_t q_sz = key->p_sz;
+
+	dma_unmap_single(dev, pdb->p_dma, p_sz, DMA_TO_DEVICE);
+	dma_unmap_single(dev, pdb->q_dma, q_sz, DMA_TO_DEVICE);
+	dma_unmap_single(dev, pdb->dp_dma, p_sz, DMA_TO_DEVICE);
+	dma_unmap_single(dev, pdb->dq_dma, q_sz, DMA_TO_DEVICE);
+	dma_unmap_single(dev, pdb->c_dma, p_sz, DMA_TO_DEVICE);
+	dma_unmap_single(dev, pdb->tmp1_dma, p_sz, DMA_TO_DEVICE);
+	dma_unmap_single(dev, pdb->tmp2_dma, q_sz, DMA_TO_DEVICE);
+}
+
 /* RSA Job Completion handler */
 static void rsa_pub_done(struct device *dev, u32 *desc, u32 err, void *context)
 {
@@ -90,6 +130,42 @@ static void rsa_priv_f1_done(struct device *dev, u32 *desc, u32 err,
 	akcipher_request_complete(req, err);
 }
 
+static void rsa_priv_f2_done(struct device *dev, u32 *desc, u32 err,
+			     void *context)
+{
+	struct akcipher_request *req = context;
+	struct rsa_edesc *edesc;
+
+	if (err)
+		caam_jr_strstatus(dev, err);
+
+	edesc = container_of(desc, struct rsa_edesc, hw_desc[0]);
+
+	rsa_priv_f2_unmap(dev, edesc, req);
+	rsa_io_unmap(dev, edesc, req);
+	kfree(edesc);
+
+	akcipher_request_complete(req, err);
+}
+
+static void rsa_priv_f3_done(struct device *dev, u32 *desc, u32 err,
+			     void *context)
+{
+	struct akcipher_request *req = context;
+	struct rsa_edesc *edesc;
+
+	if (err)
+		caam_jr_strstatus(dev, err);
+
+	edesc = container_of(desc, struct rsa_edesc, hw_desc[0]);
+
+	rsa_priv_f3_unmap(dev, edesc, req);
+	rsa_io_unmap(dev, edesc, req);
+	kfree(edesc);
+
+	akcipher_request_complete(req, err);
+}
+
 static struct rsa_edesc *rsa_edesc_alloc(struct akcipher_request *req,
 					 size_t desclen)
 {
@@ -97,8 +173,8 @@ static struct rsa_edesc *rsa_edesc_alloc(struct akcipher_request *req,
 	struct caam_rsa_ctx *ctx = akcipher_tfm_ctx(tfm);
 	struct device *dev = ctx->dev;
 	struct rsa_edesc *edesc;
-	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
-		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		       GFP_KERNEL : GFP_ATOMIC;
 	int sgc;
 	int sec4_sg_index, sec4_sg_len = 0, sec4_sg_bytes;
 	int src_nents, dst_nents;
@@ -258,6 +334,172 @@ static int set_rsa_priv_f1_pdb(struct akcipher_request *req,
 	return 0;
 }
 
+static int set_rsa_priv_f2_pdb(struct akcipher_request *req,
+			       struct rsa_edesc *edesc)
+{
+	struct crypto_akcipher *tfm = crypto_akcipher_reqtfm(req);
+	struct caam_rsa_ctx *ctx = akcipher_tfm_ctx(tfm);
+	struct caam_rsa_key *key = &ctx->key;
+	struct device *dev = ctx->dev;
+	struct rsa_priv_f2_pdb *pdb = &edesc->pdb.priv_f2;
+	int sec4_sg_index = 0;
+	size_t p_sz = key->p_sz;
+	size_t q_sz = key->p_sz;
+
+	pdb->d_dma = dma_map_single(dev, key->d, key->d_sz, DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, pdb->d_dma)) {
+		dev_err(dev, "Unable to map RSA private exponent memory\n");
+		return -ENOMEM;
+	}
+
+	pdb->p_dma = dma_map_single(dev, key->p, p_sz, DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, pdb->p_dma)) {
+		dev_err(dev, "Unable to map RSA prime factor p memory\n");
+		goto unmap_d;
+	}
+
+	pdb->q_dma = dma_map_single(dev, key->q, q_sz, DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, pdb->q_dma)) {
+		dev_err(dev, "Unable to map RSA prime factor q memory\n");
+		goto unmap_p;
+	}
+
+	pdb->tmp1_dma = dma_map_single(dev, key->tmp1, p_sz, DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, pdb->tmp1_dma)) {
+		dev_err(dev, "Unable to map RSA tmp1 memory\n");
+		goto unmap_q;
+	}
+
+	pdb->tmp2_dma = dma_map_single(dev, key->tmp2, q_sz, DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, pdb->tmp2_dma)) {
+		dev_err(dev, "Unable to map RSA tmp2 memory\n");
+		goto unmap_tmp1;
+	}
+
+	if (edesc->src_nents > 1) {
+		pdb->sgf |= RSA_PRIV_PDB_SGF_G;
+		pdb->g_dma = edesc->sec4_sg_dma;
+		sec4_sg_index += edesc->src_nents;
+	} else {
+		pdb->g_dma = sg_dma_address(req->src);
+	}
+
+	if (edesc->dst_nents > 1) {
+		pdb->sgf |= RSA_PRIV_PDB_SGF_F;
+		pdb->f_dma = edesc->sec4_sg_dma +
+			     sec4_sg_index * sizeof(struct sec4_sg_entry);
+	} else {
+		pdb->f_dma = sg_dma_address(req->dst);
+	}
+
+	pdb->sgf |= (key->d_sz << RSA_PDB_D_SHIFT) | key->n_sz;
+	pdb->p_q_len = (q_sz << RSA_PDB_Q_SHIFT) | p_sz;
+
+	return 0;
+
+unmap_tmp1:
+	dma_unmap_single(dev, pdb->tmp1_dma, p_sz, DMA_TO_DEVICE);
+unmap_q:
+	dma_unmap_single(dev, pdb->q_dma, q_sz, DMA_TO_DEVICE);
+unmap_p:
+	dma_unmap_single(dev, pdb->p_dma, p_sz, DMA_TO_DEVICE);
+unmap_d:
+	dma_unmap_single(dev, pdb->d_dma, key->d_sz, DMA_TO_DEVICE);
+
+	return -ENOMEM;
+}
+
+static int set_rsa_priv_f3_pdb(struct akcipher_request *req,
+			       struct rsa_edesc *edesc)
+{
+	struct crypto_akcipher *tfm = crypto_akcipher_reqtfm(req);
+	struct caam_rsa_ctx *ctx = akcipher_tfm_ctx(tfm);
+	struct caam_rsa_key *key = &ctx->key;
+	struct device *dev = ctx->dev;
+	struct rsa_priv_f3_pdb *pdb = &edesc->pdb.priv_f3;
+	int sec4_sg_index = 0;
+	size_t p_sz = key->p_sz;
+	size_t q_sz = key->p_sz;
+
+	pdb->p_dma = dma_map_single(dev, key->p, p_sz, DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, pdb->p_dma)) {
+		dev_err(dev, "Unable to map RSA prime factor p memory\n");
+		return -ENOMEM;
+	}
+
+	pdb->q_dma = dma_map_single(dev, key->q, q_sz, DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, pdb->q_dma)) {
+		dev_err(dev, "Unable to map RSA prime factor q memory\n");
+		goto unmap_p;
+	}
+
+	pdb->dp_dma = dma_map_single(dev, key->dp, p_sz, DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, pdb->dp_dma)) {
+		dev_err(dev, "Unable to map RSA exponent dp memory\n");
+		goto unmap_q;
+	}
+
+	pdb->dq_dma = dma_map_single(dev, key->dq, q_sz, DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, pdb->dq_dma)) {
+		dev_err(dev, "Unable to map RSA exponent dq memory\n");
+		goto unmap_dp;
+	}
+
+	pdb->c_dma = dma_map_single(dev, key->qinv, p_sz, DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, pdb->c_dma)) {
+		dev_err(dev, "Unable to map RSA CRT coefficient qinv memory\n");
+		goto unmap_dq;
+	}
+
+	pdb->tmp1_dma = dma_map_single(dev, key->tmp1, p_sz, DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, pdb->tmp1_dma)) {
+		dev_err(dev, "Unable to map RSA tmp1 memory\n");
+		goto unmap_qinv;
+	}
+
+	pdb->tmp2_dma = dma_map_single(dev, key->tmp2, q_sz, DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, pdb->tmp2_dma)) {
+		dev_err(dev, "Unable to map RSA tmp2 memory\n");
+		goto unmap_tmp1;
+	}
+
+	if (edesc->src_nents > 1) {
+		pdb->sgf |= RSA_PRIV_PDB_SGF_G;
+		pdb->g_dma = edesc->sec4_sg_dma;
+		sec4_sg_index += edesc->src_nents;
+	} else {
+		pdb->g_dma = sg_dma_address(req->src);
+	}
+
+	if (edesc->dst_nents > 1) {
+		pdb->sgf |= RSA_PRIV_PDB_SGF_F;
+		pdb->f_dma = edesc->sec4_sg_dma +
+			     sec4_sg_index * sizeof(struct sec4_sg_entry);
+	} else {
+		pdb->f_dma = sg_dma_address(req->dst);
+	}
+
+	pdb->sgf |= key->n_sz;
+	pdb->p_q_len = (q_sz << RSA_PDB_Q_SHIFT) | p_sz;
+
+	return 0;
+
+unmap_tmp1:
+	dma_unmap_single(dev, pdb->tmp1_dma, p_sz, DMA_TO_DEVICE);
+unmap_qinv:
+	dma_unmap_single(dev, pdb->c_dma, p_sz, DMA_TO_DEVICE);
+unmap_dq:
+	dma_unmap_single(dev, pdb->dq_dma, q_sz, DMA_TO_DEVICE);
+unmap_dp:
+	dma_unmap_single(dev, pdb->dp_dma, p_sz, DMA_TO_DEVICE);
+unmap_q:
+	dma_unmap_single(dev, pdb->q_dma, q_sz, DMA_TO_DEVICE);
+unmap_p:
+	dma_unmap_single(dev, pdb->p_dma, p_sz, DMA_TO_DEVICE);
+
+	return -ENOMEM;
+}
+
 static int caam_rsa_enc(struct akcipher_request *req)
 {
 	struct crypto_akcipher *tfm = crypto_akcipher_reqtfm(req);
@@ -301,24 +543,14 @@ init_fail:
 	return ret;
 }
 
-static int caam_rsa_dec(struct akcipher_request *req)
+static int caam_rsa_dec_priv_f1(struct akcipher_request *req)
 {
 	struct crypto_akcipher *tfm = crypto_akcipher_reqtfm(req);
 	struct caam_rsa_ctx *ctx = akcipher_tfm_ctx(tfm);
-	struct caam_rsa_key *key = &ctx->key;
 	struct device *jrdev = ctx->dev;
 	struct rsa_edesc *edesc;
 	int ret;
 
-	if (unlikely(!key->n || !key->d))
-		return -EINVAL;
-
-	if (req->dst_len < key->n_sz) {
-		req->dst_len = key->n_sz;
-		dev_err(jrdev, "Output buffer length less than parameter n\n");
-		return -EOVERFLOW;
-	}
-
 	/* Allocate extended descriptor */
 	edesc = rsa_edesc_alloc(req, DESC_RSA_PRIV_F1_LEN);
 	if (IS_ERR(edesc))
@@ -344,17 +576,147 @@ init_fail:
 	return ret;
 }
 
+static int caam_rsa_dec_priv_f2(struct akcipher_request *req)
+{
+	struct crypto_akcipher *tfm = crypto_akcipher_reqtfm(req);
+	struct caam_rsa_ctx *ctx = akcipher_tfm_ctx(tfm);
+	struct device *jrdev = ctx->dev;
+	struct rsa_edesc *edesc;
+	int ret;
+
+	/* Allocate extended descriptor */
+	edesc = rsa_edesc_alloc(req, DESC_RSA_PRIV_F2_LEN);
+	if (IS_ERR(edesc))
+		return PTR_ERR(edesc);
+
+	/* Set RSA Decrypt Protocol Data Block - Private Key Form #2 */
+	ret = set_rsa_priv_f2_pdb(req, edesc);
+	if (ret)
+		goto init_fail;
+
+	/* Initialize Job Descriptor */
+	init_rsa_priv_f2_desc(edesc->hw_desc, &edesc->pdb.priv_f2);
+
+	ret = caam_jr_enqueue(jrdev, edesc->hw_desc, rsa_priv_f2_done, req);
+	if (!ret)
+		return -EINPROGRESS;
+
+	rsa_priv_f2_unmap(jrdev, edesc, req);
+
+init_fail:
+	rsa_io_unmap(jrdev, edesc, req);
+	kfree(edesc);
+	return ret;
+}
+
+static int caam_rsa_dec_priv_f3(struct akcipher_request *req)
+{
+	struct crypto_akcipher *tfm = crypto_akcipher_reqtfm(req);
+	struct caam_rsa_ctx *ctx = akcipher_tfm_ctx(tfm);
+	struct device *jrdev = ctx->dev;
+	struct rsa_edesc *edesc;
+	int ret;
+
+	/* Allocate extended descriptor */
+	edesc = rsa_edesc_alloc(req, DESC_RSA_PRIV_F3_LEN);
+	if (IS_ERR(edesc))
+		return PTR_ERR(edesc);
+
+	/* Set RSA Decrypt Protocol Data Block - Private Key Form #3 */
+	ret = set_rsa_priv_f3_pdb(req, edesc);
+	if (ret)
+		goto init_fail;
+
+	/* Initialize Job Descriptor */
+	init_rsa_priv_f3_desc(edesc->hw_desc, &edesc->pdb.priv_f3);
+
+	ret = caam_jr_enqueue(jrdev, edesc->hw_desc, rsa_priv_f3_done, req);
+	if (!ret)
+		return -EINPROGRESS;
+
+	rsa_priv_f3_unmap(jrdev, edesc, req);
+
+init_fail:
+	rsa_io_unmap(jrdev, edesc, req);
+	kfree(edesc);
+	return ret;
+}
+
+static int caam_rsa_dec(struct akcipher_request *req)
+{
+	struct crypto_akcipher *tfm = crypto_akcipher_reqtfm(req);
+	struct caam_rsa_ctx *ctx = akcipher_tfm_ctx(tfm);
+	struct caam_rsa_key *key = &ctx->key;
+	int ret;
+
+	if (unlikely(!key->n || !key->d))
+		return -EINVAL;
+
+	if (req->dst_len < key->n_sz) {
+		req->dst_len = key->n_sz;
+		dev_err(ctx->dev, "Output buffer length less than parameter n\n");
+		return -EOVERFLOW;
+	}
+
+	if (key->priv_form == FORM3)
+		ret = caam_rsa_dec_priv_f3(req);
+	else if (key->priv_form == FORM2)
+		ret = caam_rsa_dec_priv_f2(req);
+	else
+		ret = caam_rsa_dec_priv_f1(req);
+
+	return ret;
+}
+
 static void caam_rsa_free_key(struct caam_rsa_key *key)
 {
 	kzfree(key->d);
+	kzfree(key->p);
+	kzfree(key->q);
+	kzfree(key->dp);
+	kzfree(key->dq);
+	kzfree(key->qinv);
+	kzfree(key->tmp1);
+	kzfree(key->tmp2);
 	kfree(key->e);
 	kfree(key->n);
-	key->d = NULL;
-	key->e = NULL;
-	key->n = NULL;
-	key->d_sz = 0;
-	key->e_sz = 0;
-	key->n_sz = 0;
+	memset(key, 0, sizeof(*key));
+}
+
+static void caam_rsa_drop_leading_zeros(const u8 **ptr, size_t *nbytes)
+{
+	while (!**ptr && *nbytes) {
+		(*ptr)++;
+		(*nbytes)--;
+	}
+}
+
+/**
+ * caam_read_rsa_crt - Used for reading dP, dQ, qInv CRT members.
+ * dP, dQ and qInv could decode to less than corresponding p, q length, as the
+ * BER-encoding requires that the minimum number of bytes be used to encode the
+ * integer. dP, dQ, qInv decoded values have to be zero-padded to appropriate
+ * length.
+ *
+ * @ptr   : pointer to {dP, dQ, qInv} CRT member
+ * @nbytes: length in bytes of {dP, dQ, qInv} CRT member
+ * @dstlen: length in bytes of corresponding p or q prime factor
+ */
+static u8 *caam_read_rsa_crt(const u8 *ptr, size_t nbytes, size_t dstlen)
+{
+	u8 *dst;
+
+	caam_rsa_drop_leading_zeros(&ptr, &nbytes);
+	if (!nbytes)
+		return NULL;
+
+	dst = kzalloc(dstlen, GFP_DMA | GFP_KERNEL);
+	if (!dst)
+		return NULL;
+
+	memcpy(dst + (dstlen - nbytes), ptr, nbytes);
+
+	return dst;
 }
 
 /**
@@ -370,10 +732,9 @@ static inline u8 *caam_read_raw_data(const u8 *buf, size_t *nbytes)
 {
 	u8 *val;
 
-	while (!*buf && *nbytes) {
-		buf++;
-		(*nbytes)--;
-	}
+	caam_rsa_drop_leading_zeros(&buf, nbytes);
+	if (!*nbytes)
+		return NULL;
 
 	val = kzalloc(*nbytes, GFP_DMA | GFP_KERNEL);
 	if (!val)
@@ -437,6 +798,64 @@ err:
 	return -ENOMEM;
 }
 
+static void caam_rsa_set_priv_key_form(struct caam_rsa_ctx *ctx,
+				       struct rsa_key *raw_key)
+{
+	struct caam_rsa_key *rsa_key = &ctx->key;
+	size_t p_sz = raw_key->p_sz;
+	size_t q_sz = raw_key->q_sz;
+
+	rsa_key->p = caam_read_raw_data(raw_key->p, &p_sz);
+	if (!rsa_key->p)
+		return;
+	rsa_key->p_sz = p_sz;
+
+	rsa_key->q = caam_read_raw_data(raw_key->q, &q_sz);
+	if (!rsa_key->q)
+		goto free_p;
+	rsa_key->q_sz = q_sz;
+
+	rsa_key->tmp1 = kzalloc(raw_key->p_sz, GFP_DMA | GFP_KERNEL);
+	if (!rsa_key->tmp1)
+		goto free_q;
+
+	rsa_key->tmp2 = kzalloc(raw_key->q_sz, GFP_DMA | GFP_KERNEL);
+	if (!rsa_key->tmp2)
+		goto free_tmp1;
+
+	rsa_key->priv_form = FORM2;
+
+	rsa_key->dp = caam_read_rsa_crt(raw_key->dp, raw_key->dp_sz, p_sz);
+	if (!rsa_key->dp)
+		goto free_tmp2;
+
+	rsa_key->dq = caam_read_rsa_crt(raw_key->dq, raw_key->dq_sz, q_sz);
+	if (!rsa_key->dq)
+		goto free_dp;
+
+	rsa_key->qinv = caam_read_rsa_crt(raw_key->qinv, raw_key->qinv_sz,
+					  q_sz);
+	if (!rsa_key->qinv)
+		goto free_dq;
+
+	rsa_key->priv_form = FORM3;
+
+	return;
+
+free_dq:
+	kzfree(rsa_key->dq);
+free_dp:
+	kzfree(rsa_key->dp);
+free_tmp2:
+	kzfree(rsa_key->tmp2);
+free_tmp1:
+	kzfree(rsa_key->tmp1);
+free_q:
+	kzfree(rsa_key->q);
+free_p:
+	kzfree(rsa_key->p);
+}
+
 static int caam_rsa_set_priv_key(struct crypto_akcipher *tfm, const void *key,
 				 unsigned int keylen)
 {
@@ -483,6 +902,8 @@ static int caam_rsa_set_priv_key(struct crypto_akcipher *tfm, const void *key,
 	memcpy(rsa_key->d, raw_key.d, raw_key.d_sz);
 	memcpy(rsa_key->e, raw_key.e, raw_key.e_sz);
 
+	caam_rsa_set_priv_key_form(ctx, &raw_key);
+
 	return 0;
 
 err:
@@ -490,12 +911,11 @@ err:
 	return -ENOMEM;
 }
 
-static int caam_rsa_max_size(struct crypto_akcipher *tfm)
+static unsigned int caam_rsa_max_size(struct crypto_akcipher *tfm)
 {
 	struct caam_rsa_ctx *ctx = akcipher_tfm_ctx(tfm);
-	struct caam_rsa_key *key = &ctx->key;
 
-	return (key->n) ? key->n_sz : -EINVAL;
+	return ctx->key.n_sz;
 }
 
 /* Per session pkc's driver context creation function */
diff --git a/drivers/crypto/caam/caampkc.h b/drivers/crypto/caam/caampkc.h
index f595d159b112..87ab75e9df43 100644
--- a/drivers/crypto/caam/caampkc.h
+++ b/drivers/crypto/caam/caampkc.h
@@ -13,21 +13,75 @@
 #include "pdb.h"
 
 /**
+ * caam_priv_key_form - CAAM RSA private key representation
+ * CAAM RSA private key may have either of three forms.
+ *
+ * 1. The first representation consists of the pair (n, d), where the
+ *    components have the following meanings:
+ *        n      the RSA modulus
+ *        d      the RSA private exponent
+ *
+ * 2. The second representation consists of the triplet (p, q, d), where the
+ *    components have the following meanings:
+ *        p      the first prime factor of the RSA modulus n
+ *        q      the second prime factor of the RSA modulus n
+ *        d      the RSA private exponent
+ *
+ * 3. The third representation consists of the quintuple (p, q, dP, dQ, qInv),
+ *    where the components have the following meanings:
+ *        p      the first prime factor of the RSA modulus n
+ *        q      the second prime factor of the RSA modulus n
+ *        dP     the first factors's CRT exponent
+ *        dQ     the second factors's CRT exponent
+ *        qInv   the (first) CRT coefficient
+ *
+ * The benefit of using the third or the second key form is lower computational
+ * cost for the decryption and signature operations.
+ */
+enum caam_priv_key_form {
+	FORM1,
+	FORM2,
+	FORM3
+};
+
+/**
  * caam_rsa_key - CAAM RSA key structure. Keys are allocated in DMA zone.
  * @n           : RSA modulus raw byte stream
  * @e           : RSA public exponent raw byte stream
  * @d           : RSA private exponent raw byte stream
+ * @p           : RSA prime factor p of RSA modulus n
+ * @q           : RSA prime factor q of RSA modulus n
+ * @dp          : RSA CRT exponent of p
+ * @dp          : RSA CRT exponent of q
+ * @qinv        : RSA CRT coefficient
+ * @tmp1        : CAAM uses this temporary buffer as internal state buffer.
+ *                It is assumed to be as long as p.
+ * @tmp2        : CAAM uses this temporary buffer as internal state buffer.
+ *                It is assumed to be as long as q.
  * @n_sz        : length in bytes of RSA modulus n
  * @e_sz        : length in bytes of RSA public exponent
  * @d_sz        : length in bytes of RSA private exponent
+ * @p_sz        : length in bytes of RSA prime factor p of RSA modulus n
+ * @q_sz        : length in bytes of RSA prime factor q of RSA modulus n
+ * @priv_form   : CAAM RSA private key representation
  */
 struct caam_rsa_key {
 	u8 *n;
 	u8 *e;
 	u8 *d;
+	u8 *p;
+	u8 *q;
+	u8 *dp;
+	u8 *dq;
+	u8 *qinv;
+	u8 *tmp1;
+	u8 *tmp2;
 	size_t n_sz;
 	size_t e_sz;
 	size_t d_sz;
+	size_t p_sz;
+	size_t q_sz;
+	enum caam_priv_key_form priv_form;
 };
 
 /**
@@ -59,6 +113,8 @@ struct rsa_edesc {
 	union {
 		struct rsa_pub_pdb pub;
 		struct rsa_priv_f1_pdb priv_f1;
+		struct rsa_priv_f2_pdb priv_f2;
+		struct rsa_priv_f3_pdb priv_f3;
 	} pdb;
 	u32 hw_desc[];
 };
@@ -66,5 +122,7 @@ struct rsa_edesc {
 /* Descriptor construction primitives. */
 void init_rsa_pub_desc(u32 *desc, struct rsa_pub_pdb *pdb);
 void init_rsa_priv_f1_desc(u32 *desc, struct rsa_priv_f1_pdb *pdb);
+void init_rsa_priv_f2_desc(u32 *desc, struct rsa_priv_f2_pdb *pdb);
+void init_rsa_priv_f3_desc(u32 *desc, struct rsa_priv_f3_pdb *pdb);
 
 #endif
diff --git a/drivers/crypto/caam/jr.c b/drivers/crypto/caam/jr.c
index 27631000b9f8..1ccfb317d468 100644
--- a/drivers/crypto/caam/jr.c
+++ b/drivers/crypto/caam/jr.c
@@ -536,7 +536,7 @@ static int caam_jr_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id caam_jr_match[] = {
+static const struct of_device_id caam_jr_match[] = {
 	{
 		.compatible = "fsl,sec-v4.0-job-ring",
 	},
diff --git a/drivers/crypto/caam/pdb.h b/drivers/crypto/caam/pdb.h
index aaa00dd1c601..31e59963f4d2 100644
--- a/drivers/crypto/caam/pdb.h
+++ b/drivers/crypto/caam/pdb.h
@@ -483,6 +483,8 @@ struct dsa_verify_pdb {
 #define RSA_PDB_E_MASK          (0xFFF << RSA_PDB_E_SHIFT)
 #define RSA_PDB_D_SHIFT         12
 #define RSA_PDB_D_MASK          (0xFFF << RSA_PDB_D_SHIFT)
+#define RSA_PDB_Q_SHIFT         12
+#define RSA_PDB_Q_MASK          (0xFFF << RSA_PDB_Q_SHIFT)
 
 #define RSA_PDB_SGF_F           (0x8 << RSA_PDB_SGF_SHIFT)
 #define RSA_PDB_SGF_G           (0x4 << RSA_PDB_SGF_SHIFT)
@@ -490,6 +492,8 @@ struct dsa_verify_pdb {
 #define RSA_PRIV_PDB_SGF_G      (0x8 << RSA_PDB_SGF_SHIFT)
 
 #define RSA_PRIV_KEY_FRM_1      0
+#define RSA_PRIV_KEY_FRM_2      1
+#define RSA_PRIV_KEY_FRM_3      2
 
 /**
  * RSA Encrypt Protocol Data Block
@@ -525,4 +529,62 @@ struct rsa_priv_f1_pdb {
 	dma_addr_t	d_dma;
 } __packed;
 
+/**
+ * RSA Decrypt PDB - Private Key Form #2
+ * @sgf     : scatter-gather field
+ * @g_dma   : dma address of encrypted input data
+ * @f_dma   : dma address of output data
+ * @d_dma   : dma address of RSA private exponent
+ * @p_dma   : dma address of RSA prime factor p of RSA modulus n
+ * @q_dma   : dma address of RSA prime factor q of RSA modulus n
+ * @tmp1_dma: dma address of temporary buffer. CAAM uses this temporary buffer
+ *            as internal state buffer. It is assumed to be as long as p.
+ * @tmp2_dma: dma address of temporary buffer. CAAM uses this temporary buffer
+ *            as internal state buffer. It is assumed to be as long as q.
+ * @p_q_len : length in bytes of first two prime factors of the RSA modulus n
+ */
+struct rsa_priv_f2_pdb {
+	u32		sgf;
+	dma_addr_t	g_dma;
+	dma_addr_t	f_dma;
+	dma_addr_t	d_dma;
+	dma_addr_t	p_dma;
+	dma_addr_t	q_dma;
+	dma_addr_t	tmp1_dma;
+	dma_addr_t	tmp2_dma;
+	u32		p_q_len;
+} __packed;
+
+/**
+ * RSA Decrypt PDB - Private Key Form #3
+ * This is the RSA Chinese Reminder Theorem (CRT) form for two prime factors of
+ * the RSA modulus.
+ * @sgf     : scatter-gather field
+ * @g_dma   : dma address of encrypted input data
+ * @f_dma   : dma address of output data
+ * @c_dma   : dma address of RSA CRT coefficient
+ * @p_dma   : dma address of RSA prime factor p of RSA modulus n
+ * @q_dma   : dma address of RSA prime factor q of RSA modulus n
+ * @dp_dma  : dma address of RSA CRT exponent of RSA prime factor p
+ * @dp_dma  : dma address of RSA CRT exponent of RSA prime factor q
+ * @tmp1_dma: dma address of temporary buffer. CAAM uses this temporary buffer
+ *            as internal state buffer. It is assumed to be as long as p.
+ * @tmp2_dma: dma address of temporary buffer. CAAM uses this temporary buffer
+ *            as internal state buffer. It is assumed to be as long as q.
+ * @p_q_len : length in bytes of first two prime factors of the RSA modulus n
+ */
+struct rsa_priv_f3_pdb {
+	u32		sgf;
+	dma_addr_t	g_dma;
+	dma_addr_t	f_dma;
+	dma_addr_t	c_dma;
+	dma_addr_t	p_dma;
+	dma_addr_t	q_dma;
+	dma_addr_t	dp_dma;
+	dma_addr_t	dq_dma;
+	dma_addr_t	tmp1_dma;
+	dma_addr_t	tmp2_dma;
+	u32		p_q_len;
+} __packed;
+
 #endif
diff --git a/drivers/crypto/caam/pkc_desc.c b/drivers/crypto/caam/pkc_desc.c
index 4e4183e615ea..9e2ce6fe2e43 100644
--- a/drivers/crypto/caam/pkc_desc.c
+++ b/drivers/crypto/caam/pkc_desc.c
@@ -34,3 +34,39 @@ void init_rsa_priv_f1_desc(u32 *desc, struct rsa_priv_f1_pdb *pdb)
 	append_operation(desc, OP_TYPE_UNI_PROTOCOL | OP_PCLID_RSADEC_PRVKEY |
 			 RSA_PRIV_KEY_FRM_1);
 }
+
+/* Descriptor for RSA Private operation - Private Key Form #2 */
+void init_rsa_priv_f2_desc(u32 *desc, struct rsa_priv_f2_pdb *pdb)
+{
+	init_job_desc_pdb(desc, 0, sizeof(*pdb));
+	append_cmd(desc, pdb->sgf);
+	append_ptr(desc, pdb->g_dma);
+	append_ptr(desc, pdb->f_dma);
+	append_ptr(desc, pdb->d_dma);
+	append_ptr(desc, pdb->p_dma);
+	append_ptr(desc, pdb->q_dma);
+	append_ptr(desc, pdb->tmp1_dma);
+	append_ptr(desc, pdb->tmp2_dma);
+	append_cmd(desc, pdb->p_q_len);
+	append_operation(desc, OP_TYPE_UNI_PROTOCOL | OP_PCLID_RSADEC_PRVKEY |
+			 RSA_PRIV_KEY_FRM_2);
+}
+
+/* Descriptor for RSA Private operation - Private Key Form #3 */
+void init_rsa_priv_f3_desc(u32 *desc, struct rsa_priv_f3_pdb *pdb)
+{
+	init_job_desc_pdb(desc, 0, sizeof(*pdb));
+	append_cmd(desc, pdb->sgf);
+	append_ptr(desc, pdb->g_dma);
+	append_ptr(desc, pdb->f_dma);
+	append_ptr(desc, pdb->c_dma);
+	append_ptr(desc, pdb->p_dma);
+	append_ptr(desc, pdb->q_dma);
+	append_ptr(desc, pdb->dp_dma);
+	append_ptr(desc, pdb->dq_dma);
+	append_ptr(desc, pdb->tmp1_dma);
+	append_ptr(desc, pdb->tmp2_dma);
+	append_cmd(desc, pdb->p_q_len);
+	append_operation(desc, OP_TYPE_UNI_PROTOCOL | OP_PCLID_RSADEC_PRVKEY |
+			 RSA_PRIV_KEY_FRM_3);
+}
diff --git a/drivers/crypto/cavium/cpt/cptvf_algs.c b/drivers/crypto/cavium/cpt/cptvf_algs.c
index cc853f913d4b..1b220f3ed017 100644
--- a/drivers/crypto/cavium/cpt/cptvf_algs.c
+++ b/drivers/crypto/cavium/cpt/cptvf_algs.c
@@ -98,7 +98,6 @@ static inline void update_output_data(struct cpt_request_info *req_info,
 }
 
 static inline u32 create_ctx_hdr(struct ablkcipher_request *req, u32 enc,
-				 u32 cipher_type, u32 aes_key_type,
 				 u32 *argcnt)
 {
 	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
@@ -124,11 +123,11 @@ static inline u32 create_ctx_hdr(struct ablkcipher_request *req, u32 enc,
 	req_info->req.param1 = req->nbytes; /* Encryption Data length */
 	req_info->req.param2 = 0; /*Auth data length */
 
-	fctx->enc.enc_ctrl.e.enc_cipher = cipher_type;
-	fctx->enc.enc_ctrl.e.aes_key = aes_key_type;
+	fctx->enc.enc_ctrl.e.enc_cipher = ctx->cipher_type;
+	fctx->enc.enc_ctrl.e.aes_key = ctx->key_type;
 	fctx->enc.enc_ctrl.e.iv_source = FROM_DPTR;
 
-	if (cipher_type == AES_XTS)
+	if (ctx->cipher_type == AES_XTS)
 		memcpy(fctx->enc.encr_key, ctx->enc_key, ctx->key_len * 2);
 	else
 		memcpy(fctx->enc.encr_key, ctx->enc_key, ctx->key_len);
@@ -154,14 +153,13 @@ static inline u32 create_ctx_hdr(struct ablkcipher_request *req, u32 enc,
 }
 
 static inline u32 create_input_list(struct ablkcipher_request  *req, u32 enc,
-				    u32 cipher_type, u32 aes_key_type,
 				    u32 enc_iv_len)
 {
 	struct cvm_req_ctx *rctx = ablkcipher_request_ctx(req);
 	struct cpt_request_info *req_info = &rctx->cpt_req;
 	u32 argcnt =  0;
 
-	create_ctx_hdr(req, enc, cipher_type, aes_key_type, &argcnt);
+	create_ctx_hdr(req, enc, &argcnt);
 	update_input_iv(req_info, req->info, enc_iv_len, &argcnt);
 	update_input_data(req_info, req->src, req->nbytes, &argcnt);
 	req_info->incnt = argcnt;
@@ -177,7 +175,6 @@ static inline void store_cb_info(struct ablkcipher_request *req,
 }
 
 static inline void create_output_list(struct ablkcipher_request *req,
-				      u32 cipher_type,
 				      u32 enc_iv_len)
 {
 	struct cvm_req_ctx *rctx = ablkcipher_request_ctx(req);
@@ -197,12 +194,9 @@ static inline void create_output_list(struct ablkcipher_request *req,
 	req_info->outcnt = argcnt;
 }
 
-static inline int cvm_enc_dec(struct ablkcipher_request *req, u32 enc,
-			      u32 cipher_type)
+static inline int cvm_enc_dec(struct ablkcipher_request *req, u32 enc)
 {
 	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
-	struct cvm_enc_ctx *ctx = crypto_ablkcipher_ctx(tfm);
-	u32 key_type = AES_128_BIT;
 	struct cvm_req_ctx *rctx = ablkcipher_request_ctx(req);
 	u32 enc_iv_len = crypto_ablkcipher_ivsize(tfm);
 	struct fc_context *fctx = &rctx->fctx;
@@ -210,36 +204,10 @@ static inline int cvm_enc_dec(struct ablkcipher_request *req, u32 enc,
 	void *cdev = NULL;
 	int status;
 
-	switch (ctx->key_len) {
-	case 16:
-		key_type = AES_128_BIT;
-		break;
-	case 24:
-		key_type = AES_192_BIT;
-		break;
-	case 32:
-		if (cipher_type == AES_XTS)
-			key_type = AES_128_BIT;
-		else
-			key_type = AES_256_BIT;
-		break;
-	case 64:
-		if (cipher_type == AES_XTS)
-			key_type = AES_256_BIT;
-		else
-			return -EINVAL;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	if (cipher_type == DES3_CBC)
-		key_type = 0;
-
 	memset(req_info, 0, sizeof(struct cpt_request_info));
 	memset(fctx, 0, sizeof(struct fc_context));
-	create_input_list(req, enc, cipher_type, key_type, enc_iv_len);
-	create_output_list(req, cipher_type, enc_iv_len);
+	create_input_list(req, enc, enc_iv_len);
+	create_output_list(req, enc_iv_len);
 	store_cb_info(req, req_info);
 	cdev = dev_handle.cdev[smp_processor_id()];
 	status = cptvf_do_request(cdev, req_info);
@@ -254,34 +222,14 @@ static inline int cvm_enc_dec(struct ablkcipher_request *req, u32 enc,
 		return -EINPROGRESS;
 }
 
-int cvm_des3_encrypt_cbc(struct ablkcipher_request *req)
+int cvm_encrypt(struct ablkcipher_request *req)
 {
-	return cvm_enc_dec(req, true, DES3_CBC);
+	return cvm_enc_dec(req, true);
 }
 
-int cvm_des3_decrypt_cbc(struct ablkcipher_request *req)
+int cvm_decrypt(struct ablkcipher_request *req)
 {
-	return cvm_enc_dec(req, false, DES3_CBC);
-}
-
-int cvm_aes_encrypt_xts(struct ablkcipher_request *req)
-{
-	return cvm_enc_dec(req, true, AES_XTS);
-}
-
-int cvm_aes_decrypt_xts(struct ablkcipher_request *req)
-{
-	return cvm_enc_dec(req, false, AES_XTS);
-}
-
-int cvm_aes_encrypt_cbc(struct ablkcipher_request *req)
-{
-	return cvm_enc_dec(req, true, AES_CBC);
-}
-
-int cvm_aes_decrypt_cbc(struct ablkcipher_request *req)
-{
-	return cvm_enc_dec(req, false, AES_CBC);
+	return cvm_enc_dec(req, false);
 }
 
 int cvm_xts_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
@@ -299,24 +247,93 @@ int cvm_xts_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
 	ctx->key_len = keylen;
 	memcpy(ctx->enc_key, key1, keylen / 2);
 	memcpy(ctx->enc_key + KEY2_OFFSET, key2, keylen / 2);
+	ctx->cipher_type = AES_XTS;
+	switch (ctx->key_len) {
+	case 32:
+		ctx->key_type = AES_128_BIT;
+		break;
+	case 64:
+		ctx->key_type = AES_256_BIT;
+		break;
+	default:
+		return -EINVAL;
+	}
 
 	return 0;
 }
 
-int cvm_enc_dec_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
-		       u32 keylen)
+static int cvm_validate_keylen(struct cvm_enc_ctx *ctx, u32 keylen)
+{
+	if ((keylen == 16) || (keylen == 24) || (keylen == 32)) {
+		ctx->key_len = keylen;
+		switch (ctx->key_len) {
+		case 16:
+			ctx->key_type = AES_128_BIT;
+			break;
+		case 24:
+			ctx->key_type = AES_192_BIT;
+			break;
+		case 32:
+			ctx->key_type = AES_256_BIT;
+			break;
+		default:
+			return -EINVAL;
+		}
+
+		if (ctx->cipher_type == DES3_CBC)
+			ctx->key_type = 0;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int cvm_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
+		      u32 keylen, u8 cipher_type)
 {
 	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
 	struct cvm_enc_ctx *ctx = crypto_tfm_ctx(tfm);
 
-	if ((keylen == 16) || (keylen == 24) || (keylen == 32)) {
-		ctx->key_len = keylen;
+	ctx->cipher_type = cipher_type;
+	if (!cvm_validate_keylen(ctx, keylen)) {
 		memcpy(ctx->enc_key, key, keylen);
 		return 0;
+	} else {
+		crypto_ablkcipher_set_flags(cipher,
+					    CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
 	}
-	crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+}
 
-	return -EINVAL;
+static int cvm_cbc_aes_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
+			      u32 keylen)
+{
+	return cvm_setkey(cipher, key, keylen, AES_CBC);
+}
+
+static int cvm_ecb_aes_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
+			      u32 keylen)
+{
+	return cvm_setkey(cipher, key, keylen, AES_ECB);
+}
+
+static int cvm_cfb_aes_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
+			      u32 keylen)
+{
+	return cvm_setkey(cipher, key, keylen, AES_CFB);
+}
+
+static int cvm_cbc_des3_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
+			       u32 keylen)
+{
+	return cvm_setkey(cipher, key, keylen, DES3_CBC);
+}
+
+static int cvm_ecb_des3_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
+			       u32 keylen)
+{
+	return cvm_setkey(cipher, key, keylen, DES3_ECB);
 }
 
 int cvm_enc_dec_init(struct crypto_tfm *tfm)
@@ -349,8 +366,8 @@ struct crypto_alg algs[] = { {
 			.min_keysize = 2 * AES_MIN_KEY_SIZE,
 			.max_keysize = 2 * AES_MAX_KEY_SIZE,
 			.setkey = cvm_xts_setkey,
-			.encrypt = cvm_aes_encrypt_xts,
-			.decrypt = cvm_aes_decrypt_xts,
+			.encrypt = cvm_encrypt,
+			.decrypt = cvm_decrypt,
 		},
 	},
 	.cra_init = cvm_enc_dec_init,
@@ -369,9 +386,51 @@ struct crypto_alg algs[] = { {
 			.ivsize = AES_BLOCK_SIZE,
 			.min_keysize = AES_MIN_KEY_SIZE,
 			.max_keysize = AES_MAX_KEY_SIZE,
-			.setkey = cvm_enc_dec_setkey,
-			.encrypt = cvm_aes_encrypt_cbc,
-			.decrypt = cvm_aes_decrypt_cbc,
+			.setkey = cvm_cbc_aes_setkey,
+			.encrypt = cvm_encrypt,
+			.decrypt = cvm_decrypt,
+		},
+	},
+	.cra_init = cvm_enc_dec_init,
+	.cra_module = THIS_MODULE,
+}, {
+	.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize = AES_BLOCK_SIZE,
+	.cra_ctxsize = sizeof(struct cvm_enc_ctx),
+	.cra_alignmask = 7,
+	.cra_priority = 4001,
+	.cra_name = "ecb(aes)",
+	.cra_driver_name = "cavium-ecb-aes",
+	.cra_type = &crypto_ablkcipher_type,
+	.cra_u = {
+		.ablkcipher = {
+			.ivsize = AES_BLOCK_SIZE,
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.setkey = cvm_ecb_aes_setkey,
+			.encrypt = cvm_encrypt,
+			.decrypt = cvm_decrypt,
+		},
+	},
+	.cra_init = cvm_enc_dec_init,
+	.cra_module = THIS_MODULE,
+}, {
+	.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize = AES_BLOCK_SIZE,
+	.cra_ctxsize = sizeof(struct cvm_enc_ctx),
+	.cra_alignmask = 7,
+	.cra_priority = 4001,
+	.cra_name = "cfb(aes)",
+	.cra_driver_name = "cavium-cfb-aes",
+	.cra_type = &crypto_ablkcipher_type,
+	.cra_u = {
+		.ablkcipher = {
+			.ivsize = AES_BLOCK_SIZE,
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.setkey = cvm_cfb_aes_setkey,
+			.encrypt = cvm_encrypt,
+			.decrypt = cvm_decrypt,
 		},
 	},
 	.cra_init = cvm_enc_dec_init,
@@ -390,9 +449,30 @@ struct crypto_alg algs[] = { {
 			.min_keysize = DES3_EDE_KEY_SIZE,
 			.max_keysize = DES3_EDE_KEY_SIZE,
 			.ivsize = DES_BLOCK_SIZE,
-			.setkey = cvm_enc_dec_setkey,
-			.encrypt = cvm_des3_encrypt_cbc,
-			.decrypt = cvm_des3_decrypt_cbc,
+			.setkey = cvm_cbc_des3_setkey,
+			.encrypt = cvm_encrypt,
+			.decrypt = cvm_decrypt,
+		},
+	},
+	.cra_init = cvm_enc_dec_init,
+	.cra_module = THIS_MODULE,
+}, {
+	.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+	.cra_ctxsize = sizeof(struct cvm_des3_ctx),
+	.cra_alignmask = 7,
+	.cra_priority = 4001,
+	.cra_name = "ecb(des3_ede)",
+	.cra_driver_name = "cavium-ecb-des3_ede",
+	.cra_type = &crypto_ablkcipher_type,
+	.cra_u = {
+		.ablkcipher = {
+			.min_keysize = DES3_EDE_KEY_SIZE,
+			.max_keysize = DES3_EDE_KEY_SIZE,
+			.ivsize = DES_BLOCK_SIZE,
+			.setkey = cvm_ecb_des3_setkey,
+			.encrypt = cvm_encrypt,
+			.decrypt = cvm_decrypt,
 		},
 	},
 	.cra_init = cvm_enc_dec_init,
diff --git a/drivers/crypto/cavium/cpt/cptvf_algs.h b/drivers/crypto/cavium/cpt/cptvf_algs.h
index a12050d11b0c..902f25751123 100644
--- a/drivers/crypto/cavium/cpt/cptvf_algs.h
+++ b/drivers/crypto/cavium/cpt/cptvf_algs.h
@@ -77,6 +77,11 @@ union encr_ctrl {
 	} e;
 };
 
+struct cvm_cipher {
+	const char *name;
+	u8 value;
+};
+
 struct enc_context {
 	union encr_ctrl enc_ctrl;
 	u8 encr_key[32];
@@ -96,6 +101,8 @@ struct fc_context {
 struct cvm_enc_ctx {
 	u32 key_len;
 	u8 enc_key[MAX_KEY_SIZE];
+	u8 cipher_type:4;
+	u8 key_type:2;
 };
 
 struct cvm_des3_ctx {
diff --git a/drivers/crypto/cavium/cpt/cptvf_main.c b/drivers/crypto/cavium/cpt/cptvf_main.c
index 6ffc740c7431..5c796ed55eba 100644
--- a/drivers/crypto/cavium/cpt/cptvf_main.c
+++ b/drivers/crypto/cavium/cpt/cptvf_main.c
@@ -525,7 +525,7 @@ static irqreturn_t cptvf_misc_intr_handler(int irq, void *cptvf_irq)
 	intr = cptvf_read_vf_misc_intr_status(cptvf);
 	/*Check for MISC interrupt types*/
 	if (likely(intr & CPT_VF_INTR_MBOX_MASK)) {
-		dev_err(&pdev->dev, "Mailbox interrupt 0x%llx on CPT VF %d\n",
+		dev_dbg(&pdev->dev, "Mailbox interrupt 0x%llx on CPT VF %d\n",
 			intr, cptvf->vfid);
 		cptvf_handle_mbox_intr(cptvf);
 		cptvf_clear_mbox_intr(cptvf);
diff --git a/drivers/crypto/cavium/nitrox/Kconfig b/drivers/crypto/cavium/nitrox/Kconfig
new file mode 100644
index 000000000000..181a1dfec932
--- /dev/null
+++ b/drivers/crypto/cavium/nitrox/Kconfig
@@ -0,0 +1,20 @@
+#
+# Cavium NITROX Crypto Device configuration
+#
+config CRYPTO_DEV_NITROX
+	tristate
+	select CRYPTO_BLKCIPHER
+	select CRYPTO_AES
+	select CRYPTO_DES
+	select FW_LOADER
+
+config CRYPTO_DEV_NITROX_CNN55XX
+	tristate "Support for Cavium CNN55XX driver"
+	depends on PCI_MSI && 64BIT
+	select CRYPTO_DEV_NITROX
+	help
+	  Support for Cavium NITROX family CNN55XX driver
+	  for accelerating crypto workloads.
+
+	  To compile this as a module, choose M here: the module
+	  will be called n5pf.
diff --git a/drivers/crypto/cavium/nitrox/Makefile b/drivers/crypto/cavium/nitrox/Makefile
new file mode 100644
index 000000000000..5af2e4368267
--- /dev/null
+++ b/drivers/crypto/cavium/nitrox/Makefile
@@ -0,0 +1,8 @@
+obj-$(CONFIG_CRYPTO_DEV_NITROX_CNN55XX) += n5pf.o
+
+n5pf-objs := nitrox_main.o \
+	nitrox_isr.o \
+	nitrox_lib.o \
+	nitrox_hal.o \
+	nitrox_reqmgr.o \
+	nitrox_algs.o
diff --git a/drivers/crypto/cavium/nitrox/nitrox_algs.c b/drivers/crypto/cavium/nitrox/nitrox_algs.c
new file mode 100644
index 000000000000..ce330278ef8a
--- /dev/null
+++ b/drivers/crypto/cavium/nitrox/nitrox_algs.c
@@ -0,0 +1,457 @@
+#include <linux/crypto.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/printk.h>
+
+#include <crypto/aes.h>
+#include <crypto/skcipher.h>
+#include <crypto/ctr.h>
+#include <crypto/des.h>
+#include <crypto/xts.h>
+
+#include "nitrox_dev.h"
+#include "nitrox_common.h"
+#include "nitrox_req.h"
+
+#define PRIO 4001
+
+struct nitrox_cipher {
+	const char *name;
+	enum flexi_cipher value;
+};
+
+/**
+ * supported cipher list
+ */
+static const struct nitrox_cipher flexi_cipher_table[] = {
+	{ "null",		CIPHER_NULL },
+	{ "cbc(des3_ede)",	CIPHER_3DES_CBC },
+	{ "ecb(des3_ede)",	CIPHER_3DES_ECB },
+	{ "cbc(aes)",		CIPHER_AES_CBC },
+	{ "ecb(aes)",		CIPHER_AES_ECB },
+	{ "cfb(aes)",		CIPHER_AES_CFB },
+	{ "rfc3686(ctr(aes))",	CIPHER_AES_CTR },
+	{ "xts(aes)",		CIPHER_AES_XTS },
+	{ "cts(cbc(aes))",	CIPHER_AES_CBC_CTS },
+	{ NULL,			CIPHER_INVALID }
+};
+
+static enum flexi_cipher flexi_cipher_type(const char *name)
+{
+	const struct nitrox_cipher *cipher = flexi_cipher_table;
+
+	while (cipher->name) {
+		if (!strcmp(cipher->name, name))
+			break;
+		cipher++;
+	}
+	return cipher->value;
+}
+
+static int flexi_aes_keylen(int keylen)
+{
+	int aes_keylen;
+
+	switch (keylen) {
+	case AES_KEYSIZE_128:
+		aes_keylen = 1;
+		break;
+	case AES_KEYSIZE_192:
+		aes_keylen = 2;
+		break;
+	case AES_KEYSIZE_256:
+		aes_keylen = 3;
+		break;
+	default:
+		aes_keylen = -EINVAL;
+		break;
+	}
+	return aes_keylen;
+}
+
+static int nitrox_skcipher_init(struct crypto_skcipher *tfm)
+{
+	struct nitrox_crypto_ctx *nctx = crypto_skcipher_ctx(tfm);
+	void *fctx;
+
+	/* get the first device */
+	nctx->ndev = nitrox_get_first_device();
+	if (!nctx->ndev)
+		return -ENODEV;
+
+	/* allocate nitrox crypto context */
+	fctx = crypto_alloc_context(nctx->ndev);
+	if (!fctx) {
+		nitrox_put_device(nctx->ndev);
+		return -ENOMEM;
+	}
+	nctx->u.ctx_handle = (uintptr_t)fctx;
+	crypto_skcipher_set_reqsize(tfm, crypto_skcipher_reqsize(tfm) +
+				    sizeof(struct nitrox_kcrypt_request));
+	return 0;
+}
+
+static void nitrox_skcipher_exit(struct crypto_skcipher *tfm)
+{
+	struct nitrox_crypto_ctx *nctx = crypto_skcipher_ctx(tfm);
+
+	/* free the nitrox crypto context */
+	if (nctx->u.ctx_handle) {
+		struct flexi_crypto_context *fctx = nctx->u.fctx;
+
+		memset(&fctx->crypto, 0, sizeof(struct crypto_keys));
+		memset(&fctx->auth, 0, sizeof(struct auth_keys));
+		crypto_free_context((void *)fctx);
+	}
+	nitrox_put_device(nctx->ndev);
+
+	nctx->u.ctx_handle = 0;
+	nctx->ndev = NULL;
+}
+
+static inline int nitrox_skcipher_setkey(struct crypto_skcipher *cipher,
+					 int aes_keylen, const u8 *key,
+					 unsigned int keylen)
+{
+	struct crypto_tfm *tfm = crypto_skcipher_tfm(cipher);
+	struct nitrox_crypto_ctx *nctx = crypto_tfm_ctx(tfm);
+	struct flexi_crypto_context *fctx;
+	enum flexi_cipher cipher_type;
+	const char *name;
+
+	name = crypto_tfm_alg_name(tfm);
+	cipher_type = flexi_cipher_type(name);
+	if (unlikely(cipher_type == CIPHER_INVALID)) {
+		pr_err("unsupported cipher: %s\n", name);
+		return -EINVAL;
+	}
+
+	/* fill crypto context */
+	fctx = nctx->u.fctx;
+	fctx->flags = 0;
+	fctx->w0.cipher_type = cipher_type;
+	fctx->w0.aes_keylen = aes_keylen;
+	fctx->w0.iv_source = IV_FROM_DPTR;
+	fctx->flags = cpu_to_be64(*(u64 *)&fctx->w0);
+	/* copy the key to context */
+	memcpy(fctx->crypto.u.key, key, keylen);
+
+	return 0;
+}
+
+static int nitrox_aes_setkey(struct crypto_skcipher *cipher, const u8 *key,
+			     unsigned int keylen)
+{
+	int aes_keylen;
+
+	aes_keylen = flexi_aes_keylen(keylen);
+	if (aes_keylen < 0) {
+		crypto_skcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+	return nitrox_skcipher_setkey(cipher, aes_keylen, key, keylen);
+}
+
+static void nitrox_skcipher_callback(struct skcipher_request *skreq,
+				     int err)
+{
+	if (err) {
+		pr_err_ratelimited("request failed status 0x%0x\n", err);
+		err = -EINVAL;
+	}
+	skcipher_request_complete(skreq, err);
+}
+
+static int nitrox_skcipher_crypt(struct skcipher_request *skreq, bool enc)
+{
+	struct crypto_skcipher *cipher = crypto_skcipher_reqtfm(skreq);
+	struct nitrox_crypto_ctx *nctx = crypto_skcipher_ctx(cipher);
+	struct nitrox_kcrypt_request *nkreq = skcipher_request_ctx(skreq);
+	int ivsize = crypto_skcipher_ivsize(cipher);
+	struct se_crypto_request *creq;
+
+	creq = &nkreq->creq;
+	creq->flags = skreq->base.flags;
+	creq->gfp = (skreq->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		     GFP_KERNEL : GFP_ATOMIC;
+
+	/* fill the request */
+	creq->ctrl.value = 0;
+	creq->opcode = FLEXI_CRYPTO_ENCRYPT_HMAC;
+	creq->ctrl.s.arg = (enc ? ENCRYPT : DECRYPT);
+	/* param0: length of the data to be encrypted */
+	creq->gph.param0 = cpu_to_be16(skreq->cryptlen);
+	creq->gph.param1 = 0;
+	/* param2: encryption data offset */
+	creq->gph.param2 = cpu_to_be16(ivsize);
+	creq->gph.param3 = 0;
+
+	creq->ctx_handle = nctx->u.ctx_handle;
+	creq->ctrl.s.ctxl = sizeof(struct flexi_crypto_context);
+
+	/* copy the iv */
+	memcpy(creq->iv, skreq->iv, ivsize);
+	creq->ivsize = ivsize;
+	creq->src = skreq->src;
+	creq->dst = skreq->dst;
+
+	nkreq->nctx = nctx;
+	nkreq->skreq = skreq;
+
+	/* send the crypto request */
+	return nitrox_process_se_request(nctx->ndev, creq,
+					 nitrox_skcipher_callback, skreq);
+}
+
+static int nitrox_aes_encrypt(struct skcipher_request *skreq)
+{
+	return nitrox_skcipher_crypt(skreq, true);
+}
+
+static int nitrox_aes_decrypt(struct skcipher_request *skreq)
+{
+	return nitrox_skcipher_crypt(skreq, false);
+}
+
+static int nitrox_3des_setkey(struct crypto_skcipher *cipher,
+			      const u8 *key, unsigned int keylen)
+{
+	if (keylen != DES3_EDE_KEY_SIZE) {
+		crypto_skcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
+	return nitrox_skcipher_setkey(cipher, 0, key, keylen);
+}
+
+static int nitrox_3des_encrypt(struct skcipher_request *skreq)
+{
+	return nitrox_skcipher_crypt(skreq, true);
+}
+
+static int nitrox_3des_decrypt(struct skcipher_request *skreq)
+{
+	return nitrox_skcipher_crypt(skreq, false);
+}
+
+static int nitrox_aes_xts_setkey(struct crypto_skcipher *cipher,
+				 const u8 *key, unsigned int keylen)
+{
+	struct crypto_tfm *tfm = crypto_skcipher_tfm(cipher);
+	struct nitrox_crypto_ctx *nctx = crypto_tfm_ctx(tfm);
+	struct flexi_crypto_context *fctx;
+	int aes_keylen, ret;
+
+	ret = xts_check_key(tfm, key, keylen);
+	if (ret)
+		return ret;
+
+	keylen /= 2;
+
+	aes_keylen = flexi_aes_keylen(keylen);
+	if (aes_keylen < 0) {
+		crypto_skcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
+	fctx = nctx->u.fctx;
+	/* copy KEY2 */
+	memcpy(fctx->auth.u.key2, (key + keylen), keylen);
+
+	return nitrox_skcipher_setkey(cipher, aes_keylen, key, keylen);
+}
+
+static int nitrox_aes_ctr_rfc3686_setkey(struct crypto_skcipher *cipher,
+					 const u8 *key, unsigned int keylen)
+{
+	struct crypto_tfm *tfm = crypto_skcipher_tfm(cipher);
+	struct nitrox_crypto_ctx *nctx = crypto_tfm_ctx(tfm);
+	struct flexi_crypto_context *fctx;
+	int aes_keylen;
+
+	if (keylen < CTR_RFC3686_NONCE_SIZE)
+		return -EINVAL;
+
+	fctx = nctx->u.fctx;
+
+	memcpy(fctx->crypto.iv, key + (keylen - CTR_RFC3686_NONCE_SIZE),
+	       CTR_RFC3686_NONCE_SIZE);
+
+	keylen -= CTR_RFC3686_NONCE_SIZE;
+
+	aes_keylen = flexi_aes_keylen(keylen);
+	if (aes_keylen < 0) {
+		crypto_skcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+	return nitrox_skcipher_setkey(cipher, aes_keylen, key, keylen);
+}
+
+static struct skcipher_alg nitrox_skciphers[] = { {
+	.base = {
+		.cra_name = "cbc(aes)",
+		.cra_driver_name = "n5_cbc(aes)",
+		.cra_priority = PRIO,
+		.cra_flags = CRYPTO_ALG_ASYNC,
+		.cra_blocksize = AES_BLOCK_SIZE,
+		.cra_ctxsize = sizeof(struct nitrox_crypto_ctx),
+		.cra_alignmask = 0,
+		.cra_module = THIS_MODULE,
+	},
+	.min_keysize = AES_MIN_KEY_SIZE,
+	.max_keysize = AES_MAX_KEY_SIZE,
+	.ivsize = AES_BLOCK_SIZE,
+	.setkey = nitrox_aes_setkey,
+	.encrypt = nitrox_aes_encrypt,
+	.decrypt = nitrox_aes_decrypt,
+	.init = nitrox_skcipher_init,
+	.exit = nitrox_skcipher_exit,
+}, {
+	.base = {
+		.cra_name = "ecb(aes)",
+		.cra_driver_name = "n5_ecb(aes)",
+		.cra_priority = PRIO,
+		.cra_flags = CRYPTO_ALG_ASYNC,
+		.cra_blocksize = AES_BLOCK_SIZE,
+		.cra_ctxsize = sizeof(struct nitrox_crypto_ctx),
+		.cra_alignmask = 0,
+		.cra_module = THIS_MODULE,
+	},
+	.min_keysize = AES_MIN_KEY_SIZE,
+	.max_keysize = AES_MAX_KEY_SIZE,
+	.ivsize = AES_BLOCK_SIZE,
+	.setkey = nitrox_aes_setkey,
+	.encrypt = nitrox_aes_encrypt,
+	.decrypt = nitrox_aes_decrypt,
+	.init = nitrox_skcipher_init,
+	.exit = nitrox_skcipher_exit,
+}, {
+	.base = {
+		.cra_name = "cfb(aes)",
+		.cra_driver_name = "n5_cfb(aes)",
+		.cra_priority = PRIO,
+		.cra_flags = CRYPTO_ALG_ASYNC,
+		.cra_blocksize = AES_BLOCK_SIZE,
+		.cra_ctxsize = sizeof(struct nitrox_crypto_ctx),
+		.cra_alignmask = 0,
+		.cra_module = THIS_MODULE,
+	},
+	.min_keysize = AES_MIN_KEY_SIZE,
+	.max_keysize = AES_MAX_KEY_SIZE,
+	.ivsize = AES_BLOCK_SIZE,
+	.setkey = nitrox_aes_setkey,
+	.encrypt = nitrox_aes_encrypt,
+	.decrypt = nitrox_aes_decrypt,
+	.init = nitrox_skcipher_init,
+	.exit = nitrox_skcipher_exit,
+}, {
+	.base = {
+		.cra_name = "xts(aes)",
+		.cra_driver_name = "n5_xts(aes)",
+		.cra_priority = PRIO,
+		.cra_flags = CRYPTO_ALG_ASYNC,
+		.cra_blocksize = AES_BLOCK_SIZE,
+		.cra_ctxsize = sizeof(struct nitrox_crypto_ctx),
+		.cra_alignmask = 0,
+		.cra_module = THIS_MODULE,
+	},
+	.min_keysize = 2 * AES_MIN_KEY_SIZE,
+	.max_keysize = 2 * AES_MAX_KEY_SIZE,
+	.ivsize = AES_BLOCK_SIZE,
+	.setkey = nitrox_aes_xts_setkey,
+	.encrypt = nitrox_aes_encrypt,
+	.decrypt = nitrox_aes_decrypt,
+	.init = nitrox_skcipher_init,
+	.exit = nitrox_skcipher_exit,
+}, {
+	.base = {
+		.cra_name = "rfc3686(ctr(aes))",
+		.cra_driver_name = "n5_rfc3686(ctr(aes))",
+		.cra_priority = PRIO,
+		.cra_flags = CRYPTO_ALG_ASYNC,
+		.cra_blocksize = 1,
+		.cra_ctxsize = sizeof(struct nitrox_crypto_ctx),
+		.cra_alignmask = 0,
+		.cra_module = THIS_MODULE,
+	},
+	.min_keysize = AES_MIN_KEY_SIZE + CTR_RFC3686_NONCE_SIZE,
+	.max_keysize = AES_MAX_KEY_SIZE + CTR_RFC3686_NONCE_SIZE,
+	.ivsize = CTR_RFC3686_IV_SIZE,
+	.init = nitrox_skcipher_init,
+	.exit = nitrox_skcipher_exit,
+	.setkey = nitrox_aes_ctr_rfc3686_setkey,
+	.encrypt = nitrox_aes_encrypt,
+	.decrypt = nitrox_aes_decrypt,
+}, {
+	.base = {
+		.cra_name = "cts(cbc(aes))",
+		.cra_driver_name = "n5_cts(cbc(aes))",
+		.cra_priority = PRIO,
+		.cra_flags = CRYPTO_ALG_ASYNC,
+		.cra_blocksize = AES_BLOCK_SIZE,
+		.cra_ctxsize = sizeof(struct nitrox_crypto_ctx),
+		.cra_alignmask = 0,
+		.cra_type = &crypto_ablkcipher_type,
+		.cra_module = THIS_MODULE,
+	},
+	.min_keysize = AES_MIN_KEY_SIZE,
+	.max_keysize = AES_MAX_KEY_SIZE,
+	.ivsize = AES_BLOCK_SIZE,
+	.setkey = nitrox_aes_setkey,
+	.encrypt = nitrox_aes_encrypt,
+	.decrypt = nitrox_aes_decrypt,
+	.init = nitrox_skcipher_init,
+	.exit = nitrox_skcipher_exit,
+}, {
+	.base = {
+		.cra_name = "cbc(des3_ede)",
+		.cra_driver_name = "n5_cbc(des3_ede)",
+		.cra_priority = PRIO,
+		.cra_flags = CRYPTO_ALG_ASYNC,
+		.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+		.cra_ctxsize = sizeof(struct nitrox_crypto_ctx),
+		.cra_alignmask = 0,
+		.cra_module = THIS_MODULE,
+	},
+	.min_keysize = DES3_EDE_KEY_SIZE,
+	.max_keysize = DES3_EDE_KEY_SIZE,
+	.ivsize = DES3_EDE_BLOCK_SIZE,
+	.setkey = nitrox_3des_setkey,
+	.encrypt = nitrox_3des_encrypt,
+	.decrypt = nitrox_3des_decrypt,
+	.init = nitrox_skcipher_init,
+	.exit = nitrox_skcipher_exit,
+}, {
+	.base = {
+		.cra_name = "ecb(des3_ede)",
+		.cra_driver_name = "n5_ecb(des3_ede)",
+		.cra_priority = PRIO,
+		.cra_flags = CRYPTO_ALG_ASYNC,
+		.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+		.cra_ctxsize = sizeof(struct nitrox_crypto_ctx),
+		.cra_alignmask = 0,
+		.cra_module = THIS_MODULE,
+	},
+	.min_keysize = DES3_EDE_KEY_SIZE,
+	.max_keysize = DES3_EDE_KEY_SIZE,
+	.ivsize = DES3_EDE_BLOCK_SIZE,
+	.setkey = nitrox_3des_setkey,
+	.encrypt = nitrox_3des_encrypt,
+	.decrypt = nitrox_3des_decrypt,
+	.init = nitrox_skcipher_init,
+	.exit = nitrox_skcipher_exit,
+}
+
+};
+
+int nitrox_crypto_register(void)
+{
+	return crypto_register_skciphers(nitrox_skciphers,
+					 ARRAY_SIZE(nitrox_skciphers));
+}
+
+void nitrox_crypto_unregister(void)
+{
+	crypto_unregister_skciphers(nitrox_skciphers,
+				    ARRAY_SIZE(nitrox_skciphers));
+}
diff --git a/drivers/crypto/cavium/nitrox/nitrox_common.h b/drivers/crypto/cavium/nitrox/nitrox_common.h
new file mode 100644
index 000000000000..4888c7823a5f
--- /dev/null
+++ b/drivers/crypto/cavium/nitrox/nitrox_common.h
@@ -0,0 +1,42 @@
+#ifndef __NITROX_COMMON_H
+#define __NITROX_COMMON_H
+
+#include "nitrox_dev.h"
+#include "nitrox_req.h"
+
+int nitrox_crypto_register(void);
+void nitrox_crypto_unregister(void);
+void *crypto_alloc_context(struct nitrox_device *ndev);
+void crypto_free_context(void *ctx);
+struct nitrox_device *nitrox_get_first_device(void);
+void nitrox_put_device(struct nitrox_device *ndev);
+
+void nitrox_pf_cleanup_isr(struct nitrox_device *ndev);
+int nitrox_pf_init_isr(struct nitrox_device *ndev);
+
+int nitrox_common_sw_init(struct nitrox_device *ndev);
+void nitrox_common_sw_cleanup(struct nitrox_device *ndev);
+
+void pkt_slc_resp_handler(unsigned long data);
+int nitrox_process_se_request(struct nitrox_device *ndev,
+			      struct se_crypto_request *req,
+			      completion_t cb,
+			      struct skcipher_request *skreq);
+void backlog_qflush_work(struct work_struct *work);
+
+void nitrox_config_emu_unit(struct nitrox_device *ndev);
+void nitrox_config_pkt_input_rings(struct nitrox_device *ndev);
+void nitrox_config_pkt_solicit_ports(struct nitrox_device *ndev);
+void nitrox_config_vfmode(struct nitrox_device *ndev, int mode);
+void nitrox_config_nps_unit(struct nitrox_device *ndev);
+void nitrox_config_pom_unit(struct nitrox_device *ndev);
+void nitrox_config_rand_unit(struct nitrox_device *ndev);
+void nitrox_config_efl_unit(struct nitrox_device *ndev);
+void nitrox_config_bmi_unit(struct nitrox_device *ndev);
+void nitrox_config_bmo_unit(struct nitrox_device *ndev);
+void nitrox_config_lbc_unit(struct nitrox_device *ndev);
+void invalidate_lbc(struct nitrox_device *ndev);
+void enable_pkt_input_ring(struct nitrox_device *ndev, int ring);
+void enable_pkt_solicit_port(struct nitrox_device *ndev, int port);
+
+#endif /* __NITROX_COMMON_H */
diff --git a/drivers/crypto/cavium/nitrox/nitrox_csr.h b/drivers/crypto/cavium/nitrox/nitrox_csr.h
new file mode 100644
index 000000000000..30b04c4c6076
--- /dev/null
+++ b/drivers/crypto/cavium/nitrox/nitrox_csr.h
@@ -0,0 +1,1084 @@
+#ifndef __NITROX_CSR_H
+#define __NITROX_CSR_H
+
+#include <asm/byteorder.h>
+#include <linux/types.h>
+
+/* EMU clusters */
+#define NR_CLUSTERS		4
+#define AE_CORES_PER_CLUSTER	20
+#define SE_CORES_PER_CLUSTER	16
+
+/* BIST registers */
+#define EMU_BIST_STATUSX(_i)	(0x1402700 + ((_i) * 0x40000))
+#define UCD_BIST_STATUS		0x12C0070
+#define NPS_CORE_BIST_REG	0x10000E8
+#define NPS_CORE_NPC_BIST_REG	0x1000128
+#define NPS_PKT_SLC_BIST_REG	0x1040088
+#define NPS_PKT_IN_BIST_REG	0x1040100
+#define POM_BIST_REG		0x11C0100
+#define BMI_BIST_REG		0x1140080
+#define EFL_CORE_BIST_REGX(_i)	(0x1240100 + ((_i) * 0x400))
+#define EFL_TOP_BIST_STAT	0x1241090
+#define BMO_BIST_REG		0x1180080
+#define LBC_BIST_STATUS		0x1200020
+#define PEM_BIST_STATUSX(_i)	(0x1080468 | ((_i) << 18))
+
+/* EMU registers */
+#define EMU_SE_ENABLEX(_i)	(0x1400000 + ((_i) * 0x40000))
+#define EMU_AE_ENABLEX(_i)	(0x1400008 + ((_i) * 0x40000))
+#define EMU_WD_INT_ENA_W1SX(_i)	(0x1402318 + ((_i) * 0x40000))
+#define EMU_GE_INT_ENA_W1SX(_i)	(0x1402518 + ((_i) * 0x40000))
+#define EMU_FUSE_MAPX(_i)	(0x1402708 + ((_i) * 0x40000))
+
+/* UCD registers */
+#define UCD_UCODE_LOAD_BLOCK_NUM	0x12C0010
+#define UCD_UCODE_LOAD_IDX_DATAX(_i)	(0x12C0018 + ((_i) * 0x20))
+#define UCD_SE_EID_UCODE_BLOCK_NUMX(_i)	(0x12C0000 + ((_i) * 0x1000))
+
+/* NPS core registers */
+#define NPS_CORE_GBL_VFCFG	0x1000000
+#define NPS_CORE_CONTROL	0x1000008
+#define NPS_CORE_INT_ACTIVE	0x1000080
+#define NPS_CORE_INT		0x10000A0
+#define NPS_CORE_INT_ENA_W1S	0x10000B8
+#define NPS_STATS_PKT_DMA_RD_CNT	0x1000180
+#define NPS_STATS_PKT_DMA_WR_CNT	0x1000190
+
+/* NPS packet registers */
+#define NPS_PKT_INT				0x1040018
+#define NPS_PKT_IN_RERR_HI		0x1040108
+#define NPS_PKT_IN_RERR_HI_ENA_W1S	0x1040120
+#define NPS_PKT_IN_RERR_LO		0x1040128
+#define NPS_PKT_IN_RERR_LO_ENA_W1S	0x1040140
+#define NPS_PKT_IN_ERR_TYPE		0x1040148
+#define NPS_PKT_IN_ERR_TYPE_ENA_W1S	0x1040160
+#define NPS_PKT_IN_INSTR_CTLX(_i)	(0x10060 + ((_i) * 0x40000))
+#define NPS_PKT_IN_INSTR_BADDRX(_i)	(0x10068 + ((_i) * 0x40000))
+#define NPS_PKT_IN_INSTR_RSIZEX(_i)	(0x10070 + ((_i) * 0x40000))
+#define NPS_PKT_IN_DONE_CNTSX(_i)	(0x10080 + ((_i) * 0x40000))
+#define NPS_PKT_IN_INSTR_BAOFF_DBELLX(_i)	(0x10078 + ((_i) * 0x40000))
+#define NPS_PKT_IN_INT_LEVELSX(_i)		(0x10088 + ((_i) * 0x40000))
+
+#define NPS_PKT_SLC_RERR_HI		0x1040208
+#define NPS_PKT_SLC_RERR_HI_ENA_W1S	0x1040220
+#define NPS_PKT_SLC_RERR_LO		0x1040228
+#define NPS_PKT_SLC_RERR_LO_ENA_W1S	0x1040240
+#define NPS_PKT_SLC_ERR_TYPE		0x1040248
+#define NPS_PKT_SLC_ERR_TYPE_ENA_W1S	0x1040260
+#define NPS_PKT_SLC_CTLX(_i)		(0x10000 + ((_i) * 0x40000))
+#define NPS_PKT_SLC_CNTSX(_i)		(0x10008 + ((_i) * 0x40000))
+#define NPS_PKT_SLC_INT_LEVELSX(_i)	(0x10010 + ((_i) * 0x40000))
+
+/* POM registers */
+#define POM_INT_ENA_W1S		0x11C0018
+#define POM_GRP_EXECMASKX(_i)	(0x11C1100 | ((_i) * 8))
+#define POM_INT		0x11C0000
+#define POM_PERF_CTL	0x11CC400
+
+/* BMI registers */
+#define BMI_INT		0x1140000
+#define BMI_CTL		0x1140020
+#define BMI_INT_ENA_W1S	0x1140018
+#define BMI_NPS_PKT_CNT	0x1140070
+
+/* EFL registers */
+#define EFL_CORE_INT_ENA_W1SX(_i)		(0x1240018 + ((_i) * 0x400))
+#define EFL_CORE_VF_ERR_INT0X(_i)		(0x1240050 + ((_i) * 0x400))
+#define EFL_CORE_VF_ERR_INT0_ENA_W1SX(_i)	(0x1240068 + ((_i) * 0x400))
+#define EFL_CORE_VF_ERR_INT1X(_i)		(0x1240070 + ((_i) * 0x400))
+#define EFL_CORE_VF_ERR_INT1_ENA_W1SX(_i)	(0x1240088 + ((_i) * 0x400))
+#define EFL_CORE_SE_ERR_INTX(_i)		(0x12400A0 + ((_i) * 0x400))
+#define EFL_RNM_CTL_STATUS			0x1241800
+#define EFL_CORE_INTX(_i)			(0x1240000 + ((_i) * 0x400))
+
+/* BMO registers */
+#define BMO_CTL2		0x1180028
+#define BMO_NPS_SLC_PKT_CNT	0x1180078
+
+/* LBC registers */
+#define LBC_INT			0x1200000
+#define LBC_INVAL_CTL		0x1201010
+#define LBC_PLM_VF1_64_INT	0x1202008
+#define LBC_INVAL_STATUS	0x1202010
+#define LBC_INT_ENA_W1S		0x1203000
+#define LBC_PLM_VF1_64_INT_ENA_W1S	0x1205008
+#define LBC_PLM_VF65_128_INT		0x1206008
+#define LBC_ELM_VF1_64_INT		0x1208000
+#define LBC_PLM_VF65_128_INT_ENA_W1S	0x1209008
+#define LBC_ELM_VF1_64_INT_ENA_W1S	0x120B000
+#define LBC_ELM_VF65_128_INT		0x120C000
+#define LBC_ELM_VF65_128_INT_ENA_W1S	0x120F000
+
+/* PEM registers */
+#define PEM0_INT 0x1080428
+
+/**
+ * struct emu_fuse_map - EMU Fuse Map Registers
+ * @ae_fuse: Fuse settings for AE 19..0
+ * @se_fuse: Fuse settings for SE 15..0
+ *
+ * A set bit indicates the unit is fuse disabled.
+ */
+union emu_fuse_map {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 valid : 1;
+		u64 raz_52_62 : 11;
+		u64 ae_fuse : 20;
+		u64 raz_16_31 : 16;
+		u64 se_fuse : 16;
+#else
+		u64 se_fuse : 16;
+		u64 raz_16_31 : 16;
+		u64 ae_fuse : 20;
+		u64 raz_52_62 : 11;
+		u64 valid : 1;
+#endif
+	} s;
+};
+
+/**
+ * struct emu_se_enable - Symmetric Engine Enable Registers
+ * @enable: Individual enables for each of the clusters
+ *   16 symmetric engines.
+ */
+union emu_se_enable {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz	: 48;
+		u64 enable : 16;
+#else
+		u64 enable : 16;
+		u64 raz	: 48;
+#endif
+	} s;
+};
+
+/**
+ * struct emu_ae_enable - EMU Asymmetric engines.
+ * @enable: Individual enables for each of the cluster's
+ *   20 Asymmetric Engines.
+ */
+union emu_ae_enable {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz	: 44;
+		u64 enable : 20;
+#else
+		u64 enable : 20;
+		u64 raz	: 44;
+#endif
+	} s;
+};
+
+/**
+ * struct emu_wd_int_ena_w1s - EMU Interrupt Enable Registers
+ * @ae_wd: Reads or sets enable for EMU(0..3)_WD_INT[AE_WD]
+ * @se_wd: Reads or sets enable for EMU(0..3)_WD_INT[SE_WD]
+ */
+union emu_wd_int_ena_w1s {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz2 : 12;
+		u64 ae_wd : 20;
+		u64 raz1 : 16;
+		u64 se_wd : 16;
+#else
+		u64 se_wd : 16;
+		u64 raz1 : 16;
+		u64 ae_wd : 20;
+		u64 raz2 : 12;
+#endif
+	} s;
+};
+
+/**
+ * struct emu_ge_int_ena_w1s - EMU Interrupt Enable set registers
+ * @ae_ge: Reads or sets enable for EMU(0..3)_GE_INT[AE_GE]
+ * @se_ge: Reads or sets enable for EMU(0..3)_GE_INT[SE_GE]
+ */
+union emu_ge_int_ena_w1s {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz_52_63 : 12;
+		u64 ae_ge : 20;
+		u64 raz_16_31: 16;
+		u64 se_ge : 16;
+#else
+		u64 se_ge : 16;
+		u64 raz_16_31: 16;
+		u64 ae_ge : 20;
+		u64 raz_52_63 : 12;
+#endif
+	} s;
+};
+
+/**
+ * struct nps_pkt_slc_ctl - Solicited Packet Out Control Registers
+ * @rh: Indicates whether to remove or include the response header
+ *   1 = Include, 0 = Remove
+ * @z: If set, 8 trailing 0x00 bytes will be added to the end of the
+ *   outgoing packet.
+ * @enb: Enable for this port.
+ */
+union nps_pkt_slc_ctl {
+	u64 value;
+	struct {
+#if defined(__BIG_ENDIAN_BITFIELD)
+		u64 raz : 61;
+		u64 rh : 1;
+		u64 z : 1;
+		u64 enb : 1;
+#else
+		u64 enb : 1;
+		u64 z : 1;
+		u64 rh : 1;
+		u64 raz : 61;
+#endif
+	} s;
+};
+
+/**
+ * struct nps_pkt_slc_cnts - Solicited Packet Out Count Registers
+ * @slc_int: Returns a 1 when:
+ *   NPS_PKT_SLC(i)_CNTS[CNT] > NPS_PKT_SLC(i)_INT_LEVELS[CNT], or
+ *   NPS_PKT_SLC(i)_CNTS[TIMER] > NPS_PKT_SLC(i)_INT_LEVELS[TIMET].
+ *   To clear the bit, the CNTS register must be written to clear.
+ * @in_int: Returns a 1 when:
+ *   NPS_PKT_IN(i)_DONE_CNTS[CNT] > NPS_PKT_IN(i)_INT_LEVELS[CNT].
+ *   To clear the bit, the DONE_CNTS register must be written to clear.
+ * @mbox_int: Returns a 1 when:
+ *   NPS_PKT_MBOX_PF_VF(i)_INT[INTR] is set. To clear the bit,
+ *   write NPS_PKT_MBOX_PF_VF(i)_INT[INTR] with 1.
+ * @timer: Timer, incremented every 2048 coprocessor clock cycles
+ *   when [CNT] is not zero. The hardware clears both [TIMER] and
+ *   [INT] when [CNT] goes to 0.
+ * @cnt: Packet counter. Hardware adds to [CNT] as it sends packets out.
+ *   On a write to this CSR, hardware subtracts the amount written to the
+ *   [CNT] field from [CNT].
+ */
+union nps_pkt_slc_cnts {
+	u64 value;
+	struct {
+#if defined(__BIG_ENDIAN_BITFIELD)
+		u64 slc_int : 1;
+		u64 uns_int : 1;
+		u64 in_int : 1;
+		u64 mbox_int : 1;
+		u64 resend : 1;
+		u64 raz : 5;
+		u64 timer : 22;
+		u64 cnt : 32;
+#else
+		u64 cnt	: 32;
+		u64 timer : 22;
+		u64 raz	: 5;
+		u64 resend : 1;
+		u64 mbox_int : 1;
+		u64 in_int : 1;
+		u64 uns_int : 1;
+		u64 slc_int : 1;
+#endif
+	} s;
+};
+
+/**
+ * struct nps_pkt_slc_int_levels - Solicited Packet Out Interrupt Levels
+ *   Registers.
+ * @bmode: Determines whether NPS_PKT_SLC_CNTS[CNT] is a byte or
+ *   packet counter.
+ * @timet: Output port counter time interrupt threshold.
+ * @cnt: Output port counter interrupt threshold.
+ */
+union nps_pkt_slc_int_levels {
+	u64 value;
+	struct {
+#if defined(__BIG_ENDIAN_BITFIELD)
+		u64 bmode : 1;
+		u64 raz	: 9;
+		u64 timet : 22;
+		u64 cnt	: 32;
+#else
+		u64 cnt : 32;
+		u64 timet : 22;
+		u64 raz : 9;
+		u64 bmode : 1;
+#endif
+	} s;
+};
+
+/**
+ * struct nps_pkt_inst - NPS Packet Interrupt Register
+ * @in_err: Set when any NPS_PKT_IN_RERR_HI/LO bit and
+ *    corresponding NPS_PKT_IN_RERR_*_ENA_* bit are bot set.
+ * @uns_err: Set when any NSP_PKT_UNS_RERR_HI/LO bit and
+ *    corresponding NPS_PKT_UNS_RERR_*_ENA_* bit are both set.
+ * @slc_er: Set when any NSP_PKT_SLC_RERR_HI/LO bit and
+ *    corresponding NPS_PKT_SLC_RERR_*_ENA_* bit are both set.
+ */
+union nps_pkt_int {
+	u64 value;
+	struct {
+#if defined(__BIG_ENDIAN_BITFIELD)
+		u64 raz	: 54;
+		u64 uns_wto : 1;
+		u64 in_err : 1;
+		u64 uns_err : 1;
+		u64 slc_err : 1;
+		u64 in_dbe : 1;
+		u64 in_sbe : 1;
+		u64 uns_dbe : 1;
+		u64 uns_sbe : 1;
+		u64 slc_dbe : 1;
+		u64 slc_sbe : 1;
+#else
+		u64 slc_sbe : 1;
+		u64 slc_dbe : 1;
+		u64 uns_sbe : 1;
+		u64 uns_dbe : 1;
+		u64 in_sbe : 1;
+		u64 in_dbe : 1;
+		u64 slc_err : 1;
+		u64 uns_err : 1;
+		u64 in_err : 1;
+		u64 uns_wto : 1;
+		u64 raz	: 54;
+#endif
+	} s;
+};
+
+/**
+ * struct nps_pkt_in_done_cnts - Input instruction ring counts registers
+ * @slc_cnt: Returns a 1 when:
+ *    NPS_PKT_SLC(i)_CNTS[CNT] > NPS_PKT_SLC(i)_INT_LEVELS[CNT], or
+ *    NPS_PKT_SLC(i)_CNTS[TIMER] > NPS_PKT_SCL(i)_INT_LEVELS[TIMET]
+ *    To clear the bit, the CNTS register must be
+ *    written to clear the underlying condition
+ * @uns_int: Return a 1 when:
+ *    NPS_PKT_UNS(i)_CNTS[CNT] > NPS_PKT_UNS(i)_INT_LEVELS[CNT], or
+ *    NPS_PKT_UNS(i)_CNTS[TIMER] > NPS_PKT_UNS(i)_INT_LEVELS[TIMET]
+ *    To clear the bit, the CNTS register must be
+ *    written to clear the underlying condition
+ * @in_int: Returns a 1 when:
+ *    NPS_PKT_IN(i)_DONE_CNTS[CNT] > NPS_PKT_IN(i)_INT_LEVELS[CNT]
+ *    To clear the bit, the DONE_CNTS register
+ *    must be written to clear the underlying condition
+ * @mbox_int: Returns a 1 when:
+ *    NPS_PKT_MBOX_PF_VF(i)_INT[INTR] is set.
+ *    To clear the bit, write NPS_PKT_MBOX_PF_VF(i)_INT[INTR]
+ *    with 1.
+ * @resend: A write of 1 will resend an MSI-X interrupt message if any
+ *    of the following conditions are true for this ring "i".
+ *    NPS_PKT_SLC(i)_CNTS[CNT] > NPS_PKT_SLC(i)_INT_LEVELS[CNT]
+ *    NPS_PKT_SLC(i)_CNTS[TIMER] > NPS_PKT_SLC(i)_INT_LEVELS[TIMET]
+ *    NPS_PKT_UNS(i)_CNTS[CNT] > NPS_PKT_UNS(i)_INT_LEVELS[CNT]
+ *    NPS_PKT_UNS(i)_CNTS[TIMER] > NPS_PKT_UNS(i)_INT_LEVELS[TIMET]
+ *    NPS_PKT_IN(i)_DONE_CNTS[CNT] > NPS_PKT_IN(i)_INT_LEVELS[CNT]
+ *    NPS_PKT_MBOX_PF_VF(i)_INT[INTR] is set
+ * @cnt: Packet counter. Hardware adds to [CNT] as it reads
+ *    packets. On a write to this CSR, hardware substracts the
+ *    amount written to the [CNT] field from [CNT], which will
+ *    clear PKT_IN(i)_INT_STATUS[INTR] if [CNT] becomes <=
+ *    NPS_PKT_IN(i)_INT_LEVELS[CNT]. This register should be
+ *    cleared before enabling a ring by reading the current
+ *    value and writing it back.
+ */
+union nps_pkt_in_done_cnts {
+	u64 value;
+	struct {
+#if defined(__BIG_ENDIAN_BITFIELD)
+		u64 slc_int : 1;
+		u64 uns_int : 1;
+		u64 in_int : 1;
+		u64 mbox_int : 1;
+		u64 resend : 1;
+		u64 raz : 27;
+		u64 cnt	: 32;
+#else
+		u64 cnt	: 32;
+		u64 raz	: 27;
+		u64 resend : 1;
+		u64 mbox_int : 1;
+		u64 in_int : 1;
+		u64 uns_int : 1;
+		u64 slc_int : 1;
+#endif
+	} s;
+};
+
+/**
+ * struct nps_pkt_in_instr_ctl - Input Instruction Ring Control Registers.
+ * @is64b: If 1, the ring uses 64-byte instructions. If 0, the
+ *   ring uses 32-byte instructions.
+ * @enb: Enable for the input ring.
+ */
+union nps_pkt_in_instr_ctl {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz	: 62;
+		u64 is64b : 1;
+		u64 enb	: 1;
+#else
+		u64 enb	: 1;
+		u64 is64b : 1;
+		u64 raz : 62;
+#endif
+	} s;
+};
+
+/**
+ * struct nps_pkt_in_instr_rsize - Input instruction ring size registers
+ * @rsize: Ring size (number of instructions)
+ */
+union nps_pkt_in_instr_rsize {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz	: 32;
+		u64 rsize : 32;
+#else
+		u64 rsize : 32;
+		u64 raz	: 32;
+#endif
+	} s;
+};
+
+/**
+ * struct nps_pkt_in_instr_baoff_dbell - Input instruction ring
+ *   base address offset and doorbell registers
+ * @aoff: Address offset. The offset from the NPS_PKT_IN_INSTR_BADDR
+ *   where the next pointer is read.
+ * @dbell: Pointer list doorbell count. Write operations to this field
+ *   increments the present value here. Read operations return the
+ *   present value.
+ */
+union nps_pkt_in_instr_baoff_dbell {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 aoff : 32;
+		u64 dbell : 32;
+#else
+		u64 dbell : 32;
+		u64 aoff : 32;
+#endif
+	} s;
+};
+
+/**
+ * struct nps_core_int_ena_w1s - NPS core interrupt enable set register
+ * @host_nps_wr_err: Reads or sets enable for
+ *   NPS_CORE_INT[HOST_NPS_WR_ERR].
+ * @npco_dma_malform: Reads or sets enable for
+ *   NPS_CORE_INT[NPCO_DMA_MALFORM].
+ * @exec_wr_timeout: Reads or sets enable for
+ *   NPS_CORE_INT[EXEC_WR_TIMEOUT].
+ * @host_wr_timeout: Reads or sets enable for
+ *   NPS_CORE_INT[HOST_WR_TIMEOUT].
+ * @host_wr_err: Reads or sets enable for
+ *   NPS_CORE_INT[HOST_WR_ERR]
+ */
+union nps_core_int_ena_w1s {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz4 : 55;
+		u64 host_nps_wr_err : 1;
+		u64 npco_dma_malform : 1;
+		u64 exec_wr_timeout : 1;
+		u64 host_wr_timeout : 1;
+		u64 host_wr_err : 1;
+		u64 raz3 : 1;
+		u64 raz2 : 1;
+		u64 raz1 : 1;
+		u64 raz0 : 1;
+#else
+		u64 raz0 : 1;
+		u64 raz1 : 1;
+		u64 raz2 : 1;
+		u64 raz3 : 1;
+		u64 host_wr_err	: 1;
+		u64 host_wr_timeout : 1;
+		u64 exec_wr_timeout : 1;
+		u64 npco_dma_malform : 1;
+		u64 host_nps_wr_err : 1;
+		u64 raz4 : 55;
+#endif
+	} s;
+};
+
+/**
+ * struct nps_core_gbl_vfcfg - Global VF Configuration Register.
+ * @ilk_disable: When set, this bit indicates that the ILK interface has
+ *    been disabled.
+ * @obaf: BMO allocation control
+ *    0 = allocate per queue
+ *    1 = allocate per VF
+ * @ibaf: BMI allocation control
+ *    0 = allocate per queue
+ *    1 = allocate per VF
+ * @zaf: ZIP allocation control
+ *    0 = allocate per queue
+ *    1 = allocate per VF
+ * @aeaf: AE allocation control
+ *    0 = allocate per queue
+ *    1 = allocate per VF
+ * @seaf: SE allocation control
+ *    0 = allocation per queue
+ *    1 = allocate per VF
+ * @cfg: VF/PF mode.
+ */
+union nps_core_gbl_vfcfg {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64  raz :55;
+		u64  ilk_disable :1;
+		u64  obaf :1;
+		u64  ibaf :1;
+		u64  zaf :1;
+		u64  aeaf :1;
+		u64  seaf :1;
+		u64  cfg :3;
+#else
+		u64  cfg :3;
+		u64  seaf :1;
+		u64  aeaf :1;
+		u64  zaf :1;
+		u64  ibaf :1;
+		u64  obaf :1;
+		u64  ilk_disable :1;
+		u64  raz :55;
+#endif
+	} s;
+};
+
+/**
+ * struct nps_core_int_active - NPS Core Interrupt Active Register
+ * @resend: Resend MSI-X interrupt if needs to handle interrupts
+ *    Sofware can set this bit and then exit the ISR.
+ * @ocla: Set when any OCLA(0)_INT and corresponding OCLA(0_INT_ENA_W1C
+ *    bit are set
+ * @mbox: Set when any NPS_PKT_MBOX_INT_LO/HI and corresponding
+ *    NPS_PKT_MBOX_INT_LO_ENA_W1C/HI_ENA_W1C bits are set
+ * @emu: bit i is set in [EMU] when any EMU(i)_INT bit is set
+ * @bmo: Set when any BMO_INT bit is set
+ * @bmi: Set when any BMI_INT bit is set or when any non-RO
+ *    BMI_INT and corresponding BMI_INT_ENA_W1C bits are both set
+ * @aqm: Set when any AQM_INT bit is set
+ * @zqm: Set when any ZQM_INT bit is set
+ * @efl: Set when any EFL_INT RO bit is set or when any non-RO EFL_INT
+ *    and corresponding EFL_INT_ENA_W1C bits are both set
+ * @ilk: Set when any ILK_INT bit is set
+ * @lbc: Set when any LBC_INT RO bit is set or when any non-RO LBC_INT
+ *    and corresponding LBC_INT_ENA_W1C bits are bot set
+ * @pem: Set when any PEM(0)_INT RO bit is set or when any non-RO
+ *    PEM(0)_INT and corresponding PEM(0)_INT_ENA_W1C bit are both set
+ * @ucd: Set when any UCD_INT bit is set
+ * @zctl: Set when any ZIP_INT RO bit is set or when any non-RO ZIP_INT
+ *    and corresponding ZIP_INT_ENA_W1C bits are both set
+ * @lbm: Set when any LBM_INT bit is set
+ * @nps_pkt: Set when any NPS_PKT_INT bit is set
+ * @nps_core: Set when any NPS_CORE_INT RO bit is set or when non-RO
+ *    NPS_CORE_INT and corresponding NSP_CORE_INT_ENA_W1C bits are both set
+ */
+union nps_core_int_active {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 resend : 1;
+		u64 raz	: 43;
+		u64 ocla : 1;
+		u64 mbox : 1;
+		u64 emu	: 4;
+		u64 bmo	: 1;
+		u64 bmi	: 1;
+		u64 aqm	: 1;
+		u64 zqm	: 1;
+		u64 efl	: 1;
+		u64 ilk	: 1;
+		u64 lbc	: 1;
+		u64 pem	: 1;
+		u64 pom	: 1;
+		u64 ucd	: 1;
+		u64 zctl : 1;
+		u64 lbm	: 1;
+		u64 nps_pkt : 1;
+		u64 nps_core : 1;
+#else
+		u64 nps_core : 1;
+		u64 nps_pkt : 1;
+		u64 lbm	: 1;
+		u64 zctl: 1;
+		u64 ucd	: 1;
+		u64 pom	: 1;
+		u64 pem	: 1;
+		u64 lbc	: 1;
+		u64 ilk	: 1;
+		u64 efl	: 1;
+		u64 zqm	: 1;
+		u64 aqm	: 1;
+		u64 bmi	: 1;
+		u64 bmo	: 1;
+		u64 emu	: 4;
+		u64 mbox : 1;
+		u64 ocla : 1;
+		u64 raz	: 43;
+		u64 resend : 1;
+#endif
+	} s;
+};
+
+/**
+ * struct efl_core_int - EFL Interrupt Registers
+ * @epci_decode_err: EPCI decoded a transacation that was unknown
+ *    This error should only occurred when there is a micrcode/SE error
+ *    and should be considered fatal
+ * @ae_err: An AE uncorrectable error occurred.
+ *    See EFL_CORE(0..3)_AE_ERR_INT
+ * @se_err: An SE uncorrectable error occurred.
+ *    See EFL_CORE(0..3)_SE_ERR_INT
+ * @dbe: Double-bit error occurred in EFL
+ * @sbe: Single-bit error occurred in EFL
+ * @d_left: Asserted when new POM-Header-BMI-data is
+ *    being sent to an Exec, and that Exec has Not read all BMI
+ *    data associated with the previous POM header
+ * @len_ovr: Asserted when an Exec-Read is issued that is more than
+ *    14 greater in length that the BMI data left to be read
+ */
+union efl_core_int {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz	: 57;
+		u64 epci_decode_err : 1;
+		u64 ae_err : 1;
+		u64 se_err : 1;
+		u64 dbe	: 1;
+		u64 sbe	: 1;
+		u64 d_left : 1;
+		u64 len_ovr : 1;
+#else
+		u64 len_ovr : 1;
+		u64 d_left : 1;
+		u64 sbe	: 1;
+		u64 dbe	: 1;
+		u64 se_err : 1;
+		u64 ae_err : 1;
+		u64 epci_decode_err  : 1;
+		u64 raz	: 57;
+#endif
+	} s;
+};
+
+/**
+ * struct efl_core_int_ena_w1s - EFL core interrupt enable set register
+ * @epci_decode_err: Reads or sets enable for
+ *   EFL_CORE(0..3)_INT[EPCI_DECODE_ERR].
+ * @d_left: Reads or sets enable for
+ *   EFL_CORE(0..3)_INT[D_LEFT].
+ * @len_ovr: Reads or sets enable for
+ *   EFL_CORE(0..3)_INT[LEN_OVR].
+ */
+union efl_core_int_ena_w1s {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz_7_63 : 57;
+		u64 epci_decode_err : 1;
+		u64 raz_2_5 : 4;
+		u64 d_left : 1;
+		u64 len_ovr : 1;
+#else
+		u64 len_ovr : 1;
+		u64 d_left : 1;
+		u64 raz_2_5 : 4;
+		u64 epci_decode_err : 1;
+		u64 raz_7_63 : 57;
+#endif
+	} s;
+};
+
+/**
+ * struct efl_rnm_ctl_status - RNM Control and Status Register
+ * @ent_sel: Select input to RNM FIFO
+ * @exp_ent: Exported entropy enable for random number generator
+ * @rng_rst: Reset to RNG. Setting this bit to 1 cancels the generation
+ *    of the current random number.
+ * @rnm_rst: Reset the RNM. Setting this bit to 1 clears all sorted numbers
+ *    in the random number memory.
+ * @rng_en: Enabled the output of the RNG.
+ * @ent_en: Entropy enable for random number generator.
+ */
+union efl_rnm_ctl_status {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz_9_63 : 55;
+		u64 ent_sel : 4;
+		u64 exp_ent : 1;
+		u64 rng_rst : 1;
+		u64 rnm_rst : 1;
+		u64 rng_en : 1;
+		u64 ent_en : 1;
+#else
+		u64 ent_en : 1;
+		u64 rng_en : 1;
+		u64 rnm_rst : 1;
+		u64 rng_rst : 1;
+		u64 exp_ent : 1;
+		u64 ent_sel : 4;
+		u64 raz_9_63 : 55;
+#endif
+	} s;
+};
+
+/**
+ * struct bmi_ctl - BMI control register
+ * @ilk_hdrq_thrsh: Maximum number of header queue locations
+ *   that ILK packets may consume. When the threshold is
+ *   exceeded ILK_XOFF is sent to the BMI_X2P_ARB.
+ * @nps_hdrq_thrsh: Maximum number of header queue locations
+ *   that NPS packets may consume. When the threshold is
+ *   exceeded NPS_XOFF is sent to the BMI_X2P_ARB.
+ * @totl_hdrq_thrsh: Maximum number of header queue locations
+ *   that the sum of ILK and NPS packets may consume.
+ * @ilk_free_thrsh: Maximum number of buffers that ILK packet
+ *   flows may consume before ILK_XOFF is sent to the BMI_X2P_ARB.
+ * @nps_free_thrsh: Maximum number of buffers that NPS packet
+ *   flows may consume before NPS XOFF is sent to the BMI_X2p_ARB.
+ * @totl_free_thrsh: Maximum number of buffers that bot ILK and NPS
+ *   packet flows may consume before both NPS_XOFF and ILK_XOFF
+ *   are asserted to the BMI_X2P_ARB.
+ * @max_pkt_len: Maximum packet length, integral number of 256B
+ *   buffers.
+ */
+union bmi_ctl {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz_56_63 : 8;
+		u64 ilk_hdrq_thrsh : 8;
+		u64 nps_hdrq_thrsh : 8;
+		u64 totl_hdrq_thrsh : 8;
+		u64 ilk_free_thrsh : 8;
+		u64 nps_free_thrsh : 8;
+		u64 totl_free_thrsh : 8;
+		u64 max_pkt_len : 8;
+#else
+		u64 max_pkt_len : 8;
+		u64 totl_free_thrsh : 8;
+		u64 nps_free_thrsh : 8;
+		u64 ilk_free_thrsh : 8;
+		u64 totl_hdrq_thrsh : 8;
+		u64 nps_hdrq_thrsh : 8;
+		u64 ilk_hdrq_thrsh : 8;
+		u64 raz_56_63 : 8;
+#endif
+	} s;
+};
+
+/**
+ * struct bmi_int_ena_w1s - BMI interrupt enable set register
+ * @ilk_req_oflw: Reads or sets enable for
+ *   BMI_INT[ILK_REQ_OFLW].
+ * @nps_req_oflw: Reads or sets enable for
+ *   BMI_INT[NPS_REQ_OFLW].
+ * @fpf_undrrn: Reads or sets enable for
+ *   BMI_INT[FPF_UNDRRN].
+ * @eop_err_ilk: Reads or sets enable for
+ *   BMI_INT[EOP_ERR_ILK].
+ * @eop_err_nps: Reads or sets enable for
+ *   BMI_INT[EOP_ERR_NPS].
+ * @sop_err_ilk: Reads or sets enable for
+ *   BMI_INT[SOP_ERR_ILK].
+ * @sop_err_nps: Reads or sets enable for
+ *   BMI_INT[SOP_ERR_NPS].
+ * @pkt_rcv_err_ilk: Reads or sets enable for
+ *   BMI_INT[PKT_RCV_ERR_ILK].
+ * @pkt_rcv_err_nps: Reads or sets enable for
+ *   BMI_INT[PKT_RCV_ERR_NPS].
+ * @max_len_err_ilk: Reads or sets enable for
+ *   BMI_INT[MAX_LEN_ERR_ILK].
+ * @max_len_err_nps: Reads or sets enable for
+ *   BMI_INT[MAX_LEN_ERR_NPS].
+ */
+union bmi_int_ena_w1s {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz_13_63	: 51;
+		u64 ilk_req_oflw : 1;
+		u64 nps_req_oflw : 1;
+		u64 raz_10 : 1;
+		u64 raz_9 : 1;
+		u64 fpf_undrrn	: 1;
+		u64 eop_err_ilk	: 1;
+		u64 eop_err_nps	: 1;
+		u64 sop_err_ilk	: 1;
+		u64 sop_err_nps	: 1;
+		u64 pkt_rcv_err_ilk : 1;
+		u64 pkt_rcv_err_nps : 1;
+		u64 max_len_err_ilk : 1;
+		u64 max_len_err_nps : 1;
+#else
+		u64 max_len_err_nps : 1;
+		u64 max_len_err_ilk : 1;
+		u64 pkt_rcv_err_nps : 1;
+		u64 pkt_rcv_err_ilk : 1;
+		u64 sop_err_nps	: 1;
+		u64 sop_err_ilk	: 1;
+		u64 eop_err_nps	: 1;
+		u64 eop_err_ilk	: 1;
+		u64 fpf_undrrn	: 1;
+		u64 raz_9 : 1;
+		u64 raz_10 : 1;
+		u64 nps_req_oflw : 1;
+		u64 ilk_req_oflw : 1;
+		u64 raz_13_63 : 51;
+#endif
+	} s;
+};
+
+/**
+ * struct bmo_ctl2 - BMO Control2 Register
+ * @arb_sel: Determines P2X Arbitration
+ * @ilk_buf_thrsh: Maximum number of buffers that the
+ *    ILK packet flows may consume before ILK XOFF is
+ *    asserted to the POM.
+ * @nps_slc_buf_thrsh: Maximum number of buffers that the
+ *    NPS_SLC packet flow may consume before NPS_SLC XOFF is
+ *    asserted to the POM.
+ * @nps_uns_buf_thrsh: Maximum number of buffers that the
+ *    NPS_UNS packet flow may consume before NPS_UNS XOFF is
+ *    asserted to the POM.
+ * @totl_buf_thrsh: Maximum number of buffers that ILK, NPS_UNS and
+ *    NPS_SLC packet flows may consume before NPS_UNS XOFF, NSP_SLC and
+ *    ILK_XOFF are all asserted POM.
+ */
+union bmo_ctl2 {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 arb_sel : 1;
+		u64 raz_32_62 : 31;
+		u64 ilk_buf_thrsh : 8;
+		u64 nps_slc_buf_thrsh : 8;
+		u64 nps_uns_buf_thrsh : 8;
+		u64 totl_buf_thrsh : 8;
+#else
+		u64 totl_buf_thrsh : 8;
+		u64 nps_uns_buf_thrsh : 8;
+		u64 nps_slc_buf_thrsh : 8;
+		u64 ilk_buf_thrsh : 8;
+		u64 raz_32_62 : 31;
+		u64 arb_sel : 1;
+#endif
+	} s;
+};
+
+/**
+ * struct pom_int_ena_w1s - POM interrupt enable set register
+ * @illegal_intf: Reads or sets enable for POM_INT[ILLEGAL_INTF].
+ * @illegal_dport: Reads or sets enable for POM_INT[ILLEGAL_DPORT].
+ */
+union pom_int_ena_w1s {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz2 : 60;
+		u64 illegal_intf : 1;
+		u64 illegal_dport : 1;
+		u64 raz1 : 1;
+		u64 raz0 : 1;
+#else
+		u64 raz0 : 1;
+		u64 raz1 : 1;
+		u64 illegal_dport : 1;
+		u64 illegal_intf : 1;
+		u64 raz2 : 60;
+#endif
+	} s;
+};
+
+/**
+ * struct lbc_inval_ctl - LBC invalidation control register
+ * @wait_timer: Wait timer for wait state. [WAIT_TIMER] must
+ *   always be written with its reset value.
+ * @cam_inval_start: Software should write [CAM_INVAL_START]=1
+ *   to initiate an LBC cache invalidation. After this, software
+ *   should read LBC_INVAL_STATUS until LBC_INVAL_STATUS[DONE] is set.
+ *   LBC hardware clears [CAVM_INVAL_START] before software can
+ *   observed LBC_INVAL_STATUS[DONE] to be set
+ */
+union lbc_inval_ctl {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz2 : 48;
+		u64 wait_timer : 8;
+		u64 raz1 : 6;
+		u64 cam_inval_start : 1;
+		u64 raz0 : 1;
+#else
+		u64 raz0 : 1;
+		u64 cam_inval_start : 1;
+		u64 raz1 : 6;
+		u64 wait_timer : 8;
+		u64 raz2 : 48;
+#endif
+	} s;
+};
+
+/**
+ * struct lbc_int_ena_w1s - LBC interrupt enable set register
+ * @cam_hard_err: Reads or sets enable for LBC_INT[CAM_HARD_ERR].
+ * @cam_inval_abort: Reads or sets enable for LBC_INT[CAM_INVAL_ABORT].
+ * @over_fetch_err: Reads or sets enable for LBC_INT[OVER_FETCH_ERR].
+ * @cache_line_to_err: Reads or sets enable for
+ *   LBC_INT[CACHE_LINE_TO_ERR].
+ * @cam_soft_err: Reads or sets enable for
+ *   LBC_INT[CAM_SOFT_ERR].
+ * @dma_rd_err: Reads or sets enable for
+ *   LBC_INT[DMA_RD_ERR].
+ */
+union lbc_int_ena_w1s {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz_10_63 : 54;
+		u64 cam_hard_err : 1;
+		u64 cam_inval_abort : 1;
+		u64 over_fetch_err : 1;
+		u64 cache_line_to_err : 1;
+		u64 raz_2_5 : 4;
+		u64 cam_soft_err : 1;
+		u64 dma_rd_err : 1;
+#else
+		u64 dma_rd_err : 1;
+		u64 cam_soft_err : 1;
+		u64 raz_2_5 : 4;
+		u64 cache_line_to_err : 1;
+		u64 over_fetch_err : 1;
+		u64 cam_inval_abort : 1;
+		u64 cam_hard_err : 1;
+		u64 raz_10_63 : 54;
+#endif
+	} s;
+};
+
+/**
+ * struct lbc_int - LBC interrupt summary register
+ * @cam_hard_err: indicates a fatal hardware error.
+ *   It requires system reset.
+ *   When [CAM_HARD_ERR] is set, LBC stops logging any new information in
+ *   LBC_POM_MISS_INFO_LOG,
+ *   LBC_POM_MISS_ADDR_LOG,
+ *   LBC_EFL_MISS_INFO_LOG, and
+ *   LBC_EFL_MISS_ADDR_LOG.
+ *   Software should sample them.
+ * @cam_inval_abort: indicates a fatal hardware error.
+ *   System reset is required.
+ * @over_fetch_err: indicates a fatal hardware error
+ *   System reset is required
+ * @cache_line_to_err: is a debug feature.
+ *   This timeout interrupt bit tells the software that
+ *   a cacheline in LBC has non-zero usage and the context
+ *   has not been used for greater than the
+ *   LBC_TO_CNT[TO_CNT] time interval.
+ * @sbe: Memory SBE error. This is recoverable via ECC.
+ *   See LBC_ECC_INT for more details.
+ * @dbe: Memory DBE error. This is a fatal and requires a
+ *   system reset.
+ * @pref_dat_len_mismatch_err: Summary bit for context length
+ *   mismatch errors.
+ * @rd_dat_len_mismatch_err: Summary bit for SE read data length
+ *   greater than data prefect length errors.
+ * @cam_soft_err: is recoverable. Software must complete a
+ *   LBC_INVAL_CTL[CAM_INVAL_START] invalidation sequence and
+ *   then clear [CAM_SOFT_ERR].
+ * @dma_rd_err: A context prefect read of host memory returned with
+ *   a read error.
+ */
+union lbc_int {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz_10_63 : 54;
+		u64 cam_hard_err : 1;
+		u64 cam_inval_abort : 1;
+		u64 over_fetch_err : 1;
+		u64 cache_line_to_err : 1;
+		u64 sbe : 1;
+		u64 dbe	: 1;
+		u64 pref_dat_len_mismatch_err : 1;
+		u64 rd_dat_len_mismatch_err : 1;
+		u64 cam_soft_err : 1;
+		u64 dma_rd_err : 1;
+#else
+		u64 dma_rd_err : 1;
+		u64 cam_soft_err : 1;
+		u64 rd_dat_len_mismatch_err : 1;
+		u64 pref_dat_len_mismatch_err : 1;
+		u64 dbe	: 1;
+		u64 sbe	: 1;
+		u64 cache_line_to_err : 1;
+		u64 over_fetch_err : 1;
+		u64 cam_inval_abort : 1;
+		u64 cam_hard_err : 1;
+		u64 raz_10_63 : 54;
+#endif
+	} s;
+};
+
+/**
+ * struct lbc_inval_status: LBC Invalidation status register
+ * @cam_clean_entry_complete_cnt: The number of entries that are
+ *   cleaned up successfully.
+ * @cam_clean_entry_cnt: The number of entries that have the CAM
+ *   inval command issued.
+ * @cam_inval_state: cam invalidation FSM state
+ * @cam_inval_abort: cam invalidation abort
+ * @cam_rst_rdy: lbc_cam reset ready
+ * @done: LBC clears [DONE] when
+ *   LBC_INVAL_CTL[CAM_INVAL_START] is written with a one,
+ *   and sets [DONE] when it completes the invalidation
+ *   sequence.
+ */
+union lbc_inval_status {
+	u64 value;
+	struct {
+#if (defined(__BIG_ENDIAN_BITFIELD))
+		u64 raz3 : 23;
+		u64 cam_clean_entry_complete_cnt : 9;
+		u64 raz2 : 7;
+		u64 cam_clean_entry_cnt : 9;
+		u64 raz1 : 5;
+		u64 cam_inval_state : 3;
+		u64 raz0 : 5;
+		u64 cam_inval_abort : 1;
+		u64 cam_rst_rdy	: 1;
+		u64 done : 1;
+#else
+		u64 done : 1;
+		u64 cam_rst_rdy : 1;
+		u64 cam_inval_abort : 1;
+		u64 raz0 : 5;
+		u64 cam_inval_state : 3;
+		u64 raz1 : 5;
+		u64 cam_clean_entry_cnt : 9;
+		u64 raz2 : 7;
+		u64 cam_clean_entry_complete_cnt : 9;
+		u64 raz3 : 23;
+#endif
+	} s;
+};
+
+#endif /* __NITROX_CSR_H */
diff --git a/drivers/crypto/cavium/nitrox/nitrox_dev.h b/drivers/crypto/cavium/nitrox/nitrox_dev.h
new file mode 100644
index 000000000000..57858b04f165
--- /dev/null
+++ b/drivers/crypto/cavium/nitrox/nitrox_dev.h
@@ -0,0 +1,179 @@
+#ifndef __NITROX_DEV_H
+#define __NITROX_DEV_H
+
+#include <linux/dma-mapping.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+
+#define VERSION_LEN 32
+
+struct nitrox_cmdq {
+	/* command queue lock */
+	spinlock_t cmdq_lock;
+	/* response list lock */
+	spinlock_t response_lock;
+	/* backlog list lock */
+	spinlock_t backlog_lock;
+
+	/* request submitted to chip, in progress */
+	struct list_head response_head;
+	/* hw queue full, hold in backlog list */
+	struct list_head backlog_head;
+
+	/* doorbell address */
+	u8 __iomem *dbell_csr_addr;
+	/* base address of the queue */
+	u8 *head;
+
+	struct nitrox_device *ndev;
+	/* flush pending backlog commands */
+	struct work_struct backlog_qflush;
+
+	/* requests posted waiting for completion */
+	atomic_t pending_count;
+	/* requests in backlog queues */
+	atomic_t backlog_count;
+
+	/* command size 32B/64B */
+	u8 instr_size;
+	u8 qno;
+	u32 qsize;
+
+	/* unaligned addresses */
+	u8 *head_unaligned;
+	dma_addr_t dma_unaligned;
+	/* dma address of the base */
+	dma_addr_t dma;
+};
+
+struct nitrox_hw {
+	/* firmware version */
+	char fw_name[VERSION_LEN];
+
+	u16 vendor_id;
+	u16 device_id;
+	u8 revision_id;
+
+	/* CNN55XX cores */
+	u8 se_cores;
+	u8 ae_cores;
+	u8 zip_cores;
+};
+
+#define MAX_MSIX_VECTOR_NAME	20
+/**
+ * vectors for queues (64 AE, 64 SE and 64 ZIP) and
+ * error condition/mailbox.
+ */
+#define MAX_MSIX_VECTORS	192
+
+struct nitrox_msix {
+	struct msix_entry *entries;
+	char **names;
+	DECLARE_BITMAP(irqs, MAX_MSIX_VECTORS);
+	u32 nr_entries;
+};
+
+struct bh_data {
+	/* slc port completion count address */
+	u8 __iomem *completion_cnt_csr_addr;
+
+	struct nitrox_cmdq *cmdq;
+	struct tasklet_struct resp_handler;
+};
+
+struct nitrox_bh {
+	struct bh_data *slc;
+};
+
+/* NITROX-5 driver state */
+#define NITROX_UCODE_LOADED	0
+#define NITROX_READY		1
+
+/* command queue size */
+#define DEFAULT_CMD_QLEN 2048
+/* command timeout in milliseconds */
+#define CMD_TIMEOUT 2000
+
+#define DEV(ndev) ((struct device *)(&(ndev)->pdev->dev))
+#define PF_MODE 0
+
+#define NITROX_CSR_ADDR(ndev, offset) \
+	((ndev)->bar_addr + (offset))
+
+/**
+ * struct nitrox_device - NITROX Device Information.
+ * @list: pointer to linked list of devices
+ * @bar_addr: iomap address
+ * @pdev: PCI device information
+ * @status: NITROX status
+ * @timeout: Request timeout in jiffies
+ * @refcnt: Device usage count
+ * @idx: device index (0..N)
+ * @node: NUMA node id attached
+ * @qlen: Command queue length
+ * @nr_queues: Number of command queues
+ * @ctx_pool: DMA pool for crypto context
+ * @pkt_cmdqs: SE Command queues
+ * @msix: MSI-X information
+ * @bh: post processing work
+ * @hw: hardware information
+ * @debugfs_dir: debugfs directory
+ */
+struct nitrox_device {
+	struct list_head list;
+
+	u8 __iomem *bar_addr;
+	struct pci_dev *pdev;
+
+	unsigned long status;
+	unsigned long timeout;
+	refcount_t refcnt;
+
+	u8 idx;
+	int node;
+	u16 qlen;
+	u16 nr_queues;
+
+	struct dma_pool *ctx_pool;
+	struct nitrox_cmdq *pkt_cmdqs;
+
+	struct nitrox_msix msix;
+	struct nitrox_bh bh;
+
+	struct nitrox_hw hw;
+#if IS_ENABLED(CONFIG_DEBUG_FS)
+	struct dentry *debugfs_dir;
+#endif
+};
+
+/**
+ * nitrox_read_csr - Read from device register
+ * @ndev: NITROX device
+ * @offset: offset of the register to read
+ *
+ * Returns: value read
+ */
+static inline u64 nitrox_read_csr(struct nitrox_device *ndev, u64 offset)
+{
+	return readq(ndev->bar_addr + offset);
+}
+
+/**
+ * nitrox_write_csr - Write to device register
+ * @ndev: NITROX device
+ * @offset: offset of the register to write
+ * @value: value to write
+ */
+static inline void nitrox_write_csr(struct nitrox_device *ndev, u64 offset,
+				    u64 value)
+{
+	writeq(value, (ndev->bar_addr + offset));
+}
+
+static inline int nitrox_ready(struct nitrox_device *ndev)
+{
+	return test_bit(NITROX_READY, &ndev->status);
+}
+
+#endif /* __NITROX_DEV_H */
diff --git a/drivers/crypto/cavium/nitrox/nitrox_hal.c b/drivers/crypto/cavium/nitrox/nitrox_hal.c
new file mode 100644
index 000000000000..f0655f82fa7d
--- /dev/null
+++ b/drivers/crypto/cavium/nitrox/nitrox_hal.c
@@ -0,0 +1,401 @@
+#include <linux/delay.h>
+
+#include "nitrox_dev.h"
+#include "nitrox_csr.h"
+
+/**
+ * emu_enable_cores - Enable EMU cluster cores.
+ * @ndev: N5 device
+ */
+static void emu_enable_cores(struct nitrox_device *ndev)
+{
+	union emu_se_enable emu_se;
+	union emu_ae_enable emu_ae;
+	int i;
+
+	/* AE cores 20 per cluster */
+	emu_ae.value = 0;
+	emu_ae.s.enable = 0xfffff;
+
+	/* SE cores 16 per cluster */
+	emu_se.value = 0;
+	emu_se.s.enable = 0xffff;
+
+	/* enable per cluster cores */
+	for (i = 0; i < NR_CLUSTERS; i++) {
+		nitrox_write_csr(ndev, EMU_AE_ENABLEX(i), emu_ae.value);
+		nitrox_write_csr(ndev, EMU_SE_ENABLEX(i), emu_se.value);
+	}
+}
+
+/**
+ * nitrox_config_emu_unit - configure EMU unit.
+ * @ndev: N5 device
+ */
+void nitrox_config_emu_unit(struct nitrox_device *ndev)
+{
+	union emu_wd_int_ena_w1s emu_wd_int;
+	union emu_ge_int_ena_w1s emu_ge_int;
+	u64 offset;
+	int i;
+
+	/* enable cores */
+	emu_enable_cores(ndev);
+
+	/* enable general error and watch dog interrupts */
+	emu_ge_int.value = 0;
+	emu_ge_int.s.se_ge = 0xffff;
+	emu_ge_int.s.ae_ge = 0xfffff;
+	emu_wd_int.value = 0;
+	emu_wd_int.s.se_wd = 1;
+
+	for (i = 0; i < NR_CLUSTERS; i++) {
+		offset = EMU_WD_INT_ENA_W1SX(i);
+		nitrox_write_csr(ndev, offset, emu_wd_int.value);
+		offset = EMU_GE_INT_ENA_W1SX(i);
+		nitrox_write_csr(ndev, offset, emu_ge_int.value);
+	}
+}
+
+static void reset_pkt_input_ring(struct nitrox_device *ndev, int ring)
+{
+	union nps_pkt_in_instr_ctl pkt_in_ctl;
+	union nps_pkt_in_instr_baoff_dbell pkt_in_dbell;
+	union nps_pkt_in_done_cnts pkt_in_cnts;
+	u64 offset;
+
+	offset = NPS_PKT_IN_INSTR_CTLX(ring);
+	/* disable the ring */
+	pkt_in_ctl.value = nitrox_read_csr(ndev, offset);
+	pkt_in_ctl.s.enb = 0;
+	nitrox_write_csr(ndev, offset, pkt_in_ctl.value);
+	usleep_range(100, 150);
+
+	/* wait to clear [ENB] */
+	do {
+		pkt_in_ctl.value = nitrox_read_csr(ndev, offset);
+	} while (pkt_in_ctl.s.enb);
+
+	/* clear off door bell counts */
+	offset = NPS_PKT_IN_INSTR_BAOFF_DBELLX(ring);
+	pkt_in_dbell.value = 0;
+	pkt_in_dbell.s.dbell = 0xffffffff;
+	nitrox_write_csr(ndev, offset, pkt_in_dbell.value);
+
+	/* clear done counts */
+	offset = NPS_PKT_IN_DONE_CNTSX(ring);
+	pkt_in_cnts.value = nitrox_read_csr(ndev, offset);
+	nitrox_write_csr(ndev, offset, pkt_in_cnts.value);
+	usleep_range(50, 100);
+}
+
+void enable_pkt_input_ring(struct nitrox_device *ndev, int ring)
+{
+	union nps_pkt_in_instr_ctl pkt_in_ctl;
+	u64 offset;
+
+	/* 64-byte instruction size */
+	offset = NPS_PKT_IN_INSTR_CTLX(ring);
+	pkt_in_ctl.value = nitrox_read_csr(ndev, offset);
+	pkt_in_ctl.s.is64b = 1;
+	pkt_in_ctl.s.enb = 1;
+	nitrox_write_csr(ndev, offset, pkt_in_ctl.value);
+
+	/* wait for set [ENB] */
+	do {
+		pkt_in_ctl.value = nitrox_read_csr(ndev, offset);
+	} while (!pkt_in_ctl.s.enb);
+}
+
+/**
+ * nitrox_config_pkt_input_rings - configure Packet Input Rings
+ * @ndev: N5 device
+ */
+void nitrox_config_pkt_input_rings(struct nitrox_device *ndev)
+{
+	int i;
+
+	for (i = 0; i < ndev->nr_queues; i++) {
+		struct nitrox_cmdq *cmdq = &ndev->pkt_cmdqs[i];
+		union nps_pkt_in_instr_rsize pkt_in_rsize;
+		u64 offset;
+
+		reset_pkt_input_ring(ndev, i);
+
+		/* configure ring base address 16-byte aligned,
+		 * size and interrupt threshold.
+		 */
+		offset = NPS_PKT_IN_INSTR_BADDRX(i);
+		nitrox_write_csr(ndev, NPS_PKT_IN_INSTR_BADDRX(i), cmdq->dma);
+
+		/* configure ring size */
+		offset = NPS_PKT_IN_INSTR_RSIZEX(i);
+		pkt_in_rsize.value = 0;
+		pkt_in_rsize.s.rsize = ndev->qlen;
+		nitrox_write_csr(ndev, offset, pkt_in_rsize.value);
+
+		/* set high threshold for pkt input ring interrupts */
+		offset = NPS_PKT_IN_INT_LEVELSX(i);
+		nitrox_write_csr(ndev, offset, 0xffffffff);
+
+		enable_pkt_input_ring(ndev, i);
+	}
+}
+
+static void reset_pkt_solicit_port(struct nitrox_device *ndev, int port)
+{
+	union nps_pkt_slc_ctl pkt_slc_ctl;
+	union nps_pkt_slc_cnts pkt_slc_cnts;
+	u64 offset;
+
+	/* disable slc port */
+	offset = NPS_PKT_SLC_CTLX(port);
+	pkt_slc_ctl.value = nitrox_read_csr(ndev, offset);
+	pkt_slc_ctl.s.enb = 0;
+	nitrox_write_csr(ndev, offset, pkt_slc_ctl.value);
+	usleep_range(100, 150);
+
+	/* wait to clear [ENB] */
+	do {
+		pkt_slc_ctl.value = nitrox_read_csr(ndev, offset);
+	} while (pkt_slc_ctl.s.enb);
+
+	/* clear slc counters */
+	offset = NPS_PKT_SLC_CNTSX(port);
+	pkt_slc_cnts.value = nitrox_read_csr(ndev, offset);
+	nitrox_write_csr(ndev, offset, pkt_slc_cnts.value);
+	usleep_range(50, 100);
+}
+
+void enable_pkt_solicit_port(struct nitrox_device *ndev, int port)
+{
+	union nps_pkt_slc_ctl pkt_slc_ctl;
+	u64 offset;
+
+	offset = NPS_PKT_SLC_CTLX(port);
+	pkt_slc_ctl.value = 0;
+	pkt_slc_ctl.s.enb = 1;
+
+	/*
+	 * 8 trailing 0x00 bytes will be added
+	 * to the end of the outgoing packet.
+	 */
+	pkt_slc_ctl.s.z = 1;
+	/* enable response header */
+	pkt_slc_ctl.s.rh = 1;
+	nitrox_write_csr(ndev, offset, pkt_slc_ctl.value);
+
+	/* wait to set [ENB] */
+	do {
+		pkt_slc_ctl.value = nitrox_read_csr(ndev, offset);
+	} while (!pkt_slc_ctl.s.enb);
+}
+
+static void config_single_pkt_solicit_port(struct nitrox_device *ndev,
+					   int port)
+{
+	union nps_pkt_slc_int_levels pkt_slc_int;
+	u64 offset;
+
+	reset_pkt_solicit_port(ndev, port);
+
+	offset = NPS_PKT_SLC_INT_LEVELSX(port);
+	pkt_slc_int.value = 0;
+	/* time interrupt threshold */
+	pkt_slc_int.s.timet = 0x3fffff;
+	nitrox_write_csr(ndev, offset, pkt_slc_int.value);
+
+	enable_pkt_solicit_port(ndev, port);
+}
+
+void nitrox_config_pkt_solicit_ports(struct nitrox_device *ndev)
+{
+	int i;
+
+	for (i = 0; i < ndev->nr_queues; i++)
+		config_single_pkt_solicit_port(ndev, i);
+}
+
+/**
+ * enable_nps_interrupts - enable NPS interrutps
+ * @ndev: N5 device.
+ *
+ * This includes NPS core, packet in and slc interrupts.
+ */
+static void enable_nps_interrupts(struct nitrox_device *ndev)
+{
+	union nps_core_int_ena_w1s core_int;
+
+	/* NPS core interrutps */
+	core_int.value = 0;
+	core_int.s.host_wr_err = 1;
+	core_int.s.host_wr_timeout = 1;
+	core_int.s.exec_wr_timeout = 1;
+	core_int.s.npco_dma_malform = 1;
+	core_int.s.host_nps_wr_err = 1;
+	nitrox_write_csr(ndev, NPS_CORE_INT_ENA_W1S, core_int.value);
+
+	/* NPS packet in ring interrupts */
+	nitrox_write_csr(ndev, NPS_PKT_IN_RERR_LO_ENA_W1S, (~0ULL));
+	nitrox_write_csr(ndev, NPS_PKT_IN_RERR_HI_ENA_W1S, (~0ULL));
+	nitrox_write_csr(ndev, NPS_PKT_IN_ERR_TYPE_ENA_W1S, (~0ULL));
+	/* NPS packet slc port interrupts */
+	nitrox_write_csr(ndev, NPS_PKT_SLC_RERR_HI_ENA_W1S, (~0ULL));
+	nitrox_write_csr(ndev, NPS_PKT_SLC_RERR_LO_ENA_W1S, (~0ULL));
+	nitrox_write_csr(ndev, NPS_PKT_SLC_ERR_TYPE_ENA_W1S, (~0uLL));
+}
+
+void nitrox_config_nps_unit(struct nitrox_device *ndev)
+{
+	union nps_core_gbl_vfcfg core_gbl_vfcfg;
+
+	/* endian control information */
+	nitrox_write_csr(ndev, NPS_CORE_CONTROL, 1ULL);
+
+	/* disable ILK interface */
+	core_gbl_vfcfg.value = 0;
+	core_gbl_vfcfg.s.ilk_disable = 1;
+	core_gbl_vfcfg.s.cfg = PF_MODE;
+	nitrox_write_csr(ndev, NPS_CORE_GBL_VFCFG, core_gbl_vfcfg.value);
+	/* config input and solicit ports */
+	nitrox_config_pkt_input_rings(ndev);
+	nitrox_config_pkt_solicit_ports(ndev);
+
+	/* enable interrupts */
+	enable_nps_interrupts(ndev);
+}
+
+void nitrox_config_pom_unit(struct nitrox_device *ndev)
+{
+	union pom_int_ena_w1s pom_int;
+	int i;
+
+	/* enable pom interrupts */
+	pom_int.value = 0;
+	pom_int.s.illegal_dport = 1;
+	nitrox_write_csr(ndev, POM_INT_ENA_W1S, pom_int.value);
+
+	/* enable perf counters */
+	for (i = 0; i < ndev->hw.se_cores; i++)
+		nitrox_write_csr(ndev, POM_PERF_CTL, BIT_ULL(i));
+}
+
+/**
+ * nitrox_config_rand_unit - enable N5 random number unit
+ * @ndev: N5 device
+ */
+void nitrox_config_rand_unit(struct nitrox_device *ndev)
+{
+	union efl_rnm_ctl_status efl_rnm_ctl;
+	u64 offset;
+
+	offset = EFL_RNM_CTL_STATUS;
+	efl_rnm_ctl.value = nitrox_read_csr(ndev, offset);
+	efl_rnm_ctl.s.ent_en = 1;
+	efl_rnm_ctl.s.rng_en = 1;
+	nitrox_write_csr(ndev, offset, efl_rnm_ctl.value);
+}
+
+void nitrox_config_efl_unit(struct nitrox_device *ndev)
+{
+	int i;
+
+	for (i = 0; i < NR_CLUSTERS; i++) {
+		union efl_core_int_ena_w1s efl_core_int;
+		u64 offset;
+
+		/* EFL core interrupts */
+		offset = EFL_CORE_INT_ENA_W1SX(i);
+		efl_core_int.value = 0;
+		efl_core_int.s.len_ovr = 1;
+		efl_core_int.s.d_left = 1;
+		efl_core_int.s.epci_decode_err = 1;
+		nitrox_write_csr(ndev, offset, efl_core_int.value);
+
+		offset = EFL_CORE_VF_ERR_INT0_ENA_W1SX(i);
+		nitrox_write_csr(ndev, offset, (~0ULL));
+		offset = EFL_CORE_VF_ERR_INT1_ENA_W1SX(i);
+		nitrox_write_csr(ndev, offset, (~0ULL));
+	}
+}
+
+void nitrox_config_bmi_unit(struct nitrox_device *ndev)
+{
+	union bmi_ctl bmi_ctl;
+	union bmi_int_ena_w1s bmi_int_ena;
+	u64 offset;
+
+	/* no threshold limits for PCIe */
+	offset = BMI_CTL;
+	bmi_ctl.value = nitrox_read_csr(ndev, offset);
+	bmi_ctl.s.max_pkt_len = 0xff;
+	bmi_ctl.s.nps_free_thrsh = 0xff;
+	bmi_ctl.s.nps_hdrq_thrsh = 0x7a;
+	nitrox_write_csr(ndev, offset, bmi_ctl.value);
+
+	/* enable interrupts */
+	offset = BMI_INT_ENA_W1S;
+	bmi_int_ena.value = 0;
+	bmi_int_ena.s.max_len_err_nps = 1;
+	bmi_int_ena.s.pkt_rcv_err_nps = 1;
+	bmi_int_ena.s.fpf_undrrn = 1;
+	nitrox_write_csr(ndev, offset, bmi_int_ena.value);
+}
+
+void nitrox_config_bmo_unit(struct nitrox_device *ndev)
+{
+	union bmo_ctl2 bmo_ctl2;
+	u64 offset;
+
+	/* no threshold limits for PCIe */
+	offset = BMO_CTL2;
+	bmo_ctl2.value = nitrox_read_csr(ndev, offset);
+	bmo_ctl2.s.nps_slc_buf_thrsh = 0xff;
+	nitrox_write_csr(ndev, offset, bmo_ctl2.value);
+}
+
+void invalidate_lbc(struct nitrox_device *ndev)
+{
+	union lbc_inval_ctl lbc_ctl;
+	union lbc_inval_status lbc_stat;
+	u64 offset;
+
+	/* invalidate LBC */
+	offset = LBC_INVAL_CTL;
+	lbc_ctl.value = nitrox_read_csr(ndev, offset);
+	lbc_ctl.s.cam_inval_start = 1;
+	nitrox_write_csr(ndev, offset, lbc_ctl.value);
+
+	offset = LBC_INVAL_STATUS;
+
+	do {
+		lbc_stat.value = nitrox_read_csr(ndev, offset);
+	} while (!lbc_stat.s.done);
+}
+
+void nitrox_config_lbc_unit(struct nitrox_device *ndev)
+{
+	union lbc_int_ena_w1s lbc_int_ena;
+	u64 offset;
+
+	invalidate_lbc(ndev);
+
+	/* enable interrupts */
+	offset = LBC_INT_ENA_W1S;
+	lbc_int_ena.value = 0;
+	lbc_int_ena.s.dma_rd_err = 1;
+	lbc_int_ena.s.over_fetch_err = 1;
+	lbc_int_ena.s.cam_inval_abort = 1;
+	lbc_int_ena.s.cam_hard_err = 1;
+	nitrox_write_csr(ndev, offset, lbc_int_ena.value);
+
+	offset = LBC_PLM_VF1_64_INT_ENA_W1S;
+	nitrox_write_csr(ndev, offset, (~0ULL));
+	offset = LBC_PLM_VF65_128_INT_ENA_W1S;
+	nitrox_write_csr(ndev, offset, (~0ULL));
+
+	offset = LBC_ELM_VF1_64_INT_ENA_W1S;
+	nitrox_write_csr(ndev, offset, (~0ULL));
+	offset = LBC_ELM_VF65_128_INT_ENA_W1S;
+	nitrox_write_csr(ndev, offset, (~0ULL));
+}
diff --git a/drivers/crypto/cavium/nitrox/nitrox_isr.c b/drivers/crypto/cavium/nitrox/nitrox_isr.c
new file mode 100644
index 000000000000..71f934871a89
--- /dev/null
+++ b/drivers/crypto/cavium/nitrox/nitrox_isr.c
@@ -0,0 +1,467 @@
+#include <linux/pci.h>
+#include <linux/printk.h>
+#include <linux/slab.h>
+
+#include "nitrox_dev.h"
+#include "nitrox_csr.h"
+#include "nitrox_common.h"
+
+#define NR_RING_VECTORS 3
+#define NPS_CORE_INT_ACTIVE_ENTRY 192
+
+/**
+ * nps_pkt_slc_isr - IRQ handler for NPS solicit port
+ * @irq: irq number
+ * @data: argument
+ */
+static irqreturn_t nps_pkt_slc_isr(int irq, void *data)
+{
+	struct bh_data *slc = data;
+	union nps_pkt_slc_cnts pkt_slc_cnts;
+
+	pkt_slc_cnts.value = readq(slc->completion_cnt_csr_addr);
+	/* New packet on SLC output port */
+	if (pkt_slc_cnts.s.slc_int)
+		tasklet_hi_schedule(&slc->resp_handler);
+
+	return IRQ_HANDLED;
+}
+
+static void clear_nps_core_err_intr(struct nitrox_device *ndev)
+{
+	u64 value;
+
+	/* Write 1 to clear */
+	value = nitrox_read_csr(ndev, NPS_CORE_INT);
+	nitrox_write_csr(ndev, NPS_CORE_INT, value);
+
+	dev_err_ratelimited(DEV(ndev), "NSP_CORE_INT  0x%016llx\n", value);
+}
+
+static void clear_nps_pkt_err_intr(struct nitrox_device *ndev)
+{
+	union nps_pkt_int pkt_int;
+	unsigned long value, offset;
+	int i;
+
+	pkt_int.value = nitrox_read_csr(ndev, NPS_PKT_INT);
+	dev_err_ratelimited(DEV(ndev), "NPS_PKT_INT  0x%016llx\n",
+			    pkt_int.value);
+
+	if (pkt_int.s.slc_err) {
+		offset = NPS_PKT_SLC_ERR_TYPE;
+		value = nitrox_read_csr(ndev, offset);
+		nitrox_write_csr(ndev, offset, value);
+		dev_err_ratelimited(DEV(ndev),
+				    "NPS_PKT_SLC_ERR_TYPE  0x%016lx\n", value);
+
+		offset = NPS_PKT_SLC_RERR_LO;
+		value = nitrox_read_csr(ndev, offset);
+		nitrox_write_csr(ndev, offset, value);
+		/* enable the solicit ports */
+		for_each_set_bit(i, &value, BITS_PER_LONG)
+			enable_pkt_solicit_port(ndev, i);
+
+		dev_err_ratelimited(DEV(ndev),
+				    "NPS_PKT_SLC_RERR_LO  0x%016lx\n", value);
+
+		offset = NPS_PKT_SLC_RERR_HI;
+		value = nitrox_read_csr(ndev, offset);
+		nitrox_write_csr(ndev, offset, value);
+		dev_err_ratelimited(DEV(ndev),
+				    "NPS_PKT_SLC_RERR_HI  0x%016lx\n", value);
+	}
+
+	if (pkt_int.s.in_err) {
+		offset = NPS_PKT_IN_ERR_TYPE;
+		value = nitrox_read_csr(ndev, offset);
+		nitrox_write_csr(ndev, offset, value);
+		dev_err_ratelimited(DEV(ndev),
+				    "NPS_PKT_IN_ERR_TYPE  0x%016lx\n", value);
+		offset = NPS_PKT_IN_RERR_LO;
+		value = nitrox_read_csr(ndev, offset);
+		nitrox_write_csr(ndev, offset, value);
+		/* enable the input ring */
+		for_each_set_bit(i, &value, BITS_PER_LONG)
+			enable_pkt_input_ring(ndev, i);
+
+		dev_err_ratelimited(DEV(ndev),
+				    "NPS_PKT_IN_RERR_LO  0x%016lx\n", value);
+
+		offset = NPS_PKT_IN_RERR_HI;
+		value = nitrox_read_csr(ndev, offset);
+		nitrox_write_csr(ndev, offset, value);
+		dev_err_ratelimited(DEV(ndev),
+				    "NPS_PKT_IN_RERR_HI  0x%016lx\n", value);
+	}
+}
+
+static void clear_pom_err_intr(struct nitrox_device *ndev)
+{
+	u64 value;
+
+	value = nitrox_read_csr(ndev, POM_INT);
+	nitrox_write_csr(ndev, POM_INT, value);
+	dev_err_ratelimited(DEV(ndev), "POM_INT  0x%016llx\n", value);
+}
+
+static void clear_pem_err_intr(struct nitrox_device *ndev)
+{
+	u64 value;
+
+	value = nitrox_read_csr(ndev, PEM0_INT);
+	nitrox_write_csr(ndev, PEM0_INT, value);
+	dev_err_ratelimited(DEV(ndev), "PEM(0)_INT  0x%016llx\n", value);
+}
+
+static void clear_lbc_err_intr(struct nitrox_device *ndev)
+{
+	union lbc_int lbc_int;
+	u64 value, offset;
+	int i;
+
+	lbc_int.value = nitrox_read_csr(ndev, LBC_INT);
+	dev_err_ratelimited(DEV(ndev), "LBC_INT  0x%016llx\n", lbc_int.value);
+
+	if (lbc_int.s.dma_rd_err) {
+		for (i = 0; i < NR_CLUSTERS; i++) {
+			offset = EFL_CORE_VF_ERR_INT0X(i);
+			value = nitrox_read_csr(ndev, offset);
+			nitrox_write_csr(ndev, offset, value);
+			offset = EFL_CORE_VF_ERR_INT1X(i);
+			value = nitrox_read_csr(ndev, offset);
+			nitrox_write_csr(ndev, offset, value);
+		}
+	}
+
+	if (lbc_int.s.cam_soft_err) {
+		dev_err_ratelimited(DEV(ndev), "CAM_SOFT_ERR, invalidating LBC\n");
+		invalidate_lbc(ndev);
+	}
+
+	if (lbc_int.s.pref_dat_len_mismatch_err) {
+		offset = LBC_PLM_VF1_64_INT;
+		value = nitrox_read_csr(ndev, offset);
+		nitrox_write_csr(ndev, offset, value);
+		offset = LBC_PLM_VF65_128_INT;
+		value = nitrox_read_csr(ndev, offset);
+		nitrox_write_csr(ndev, offset, value);
+	}
+
+	if (lbc_int.s.rd_dat_len_mismatch_err) {
+		offset = LBC_ELM_VF1_64_INT;
+		value = nitrox_read_csr(ndev, offset);
+		nitrox_write_csr(ndev, offset, value);
+		offset = LBC_ELM_VF65_128_INT;
+		value = nitrox_read_csr(ndev, offset);
+		nitrox_write_csr(ndev, offset, value);
+	}
+	nitrox_write_csr(ndev, LBC_INT, lbc_int.value);
+}
+
+static void clear_efl_err_intr(struct nitrox_device *ndev)
+{
+	int i;
+
+	for (i = 0; i < NR_CLUSTERS; i++) {
+		union efl_core_int core_int;
+		u64 value, offset;
+
+		offset = EFL_CORE_INTX(i);
+		core_int.value = nitrox_read_csr(ndev, offset);
+		nitrox_write_csr(ndev, offset, core_int.value);
+		dev_err_ratelimited(DEV(ndev), "ELF_CORE(%d)_INT  0x%016llx\n",
+				    i, core_int.value);
+		if (core_int.s.se_err) {
+			offset = EFL_CORE_SE_ERR_INTX(i);
+			value = nitrox_read_csr(ndev, offset);
+			nitrox_write_csr(ndev, offset, value);
+		}
+	}
+}
+
+static void clear_bmi_err_intr(struct nitrox_device *ndev)
+{
+	u64 value;
+
+	value = nitrox_read_csr(ndev, BMI_INT);
+	nitrox_write_csr(ndev, BMI_INT, value);
+	dev_err_ratelimited(DEV(ndev), "BMI_INT  0x%016llx\n", value);
+}
+
+/**
+ * clear_nps_core_int_active - clear NPS_CORE_INT_ACTIVE interrupts
+ * @ndev: NITROX device
+ */
+static void clear_nps_core_int_active(struct nitrox_device *ndev)
+{
+	union nps_core_int_active core_int_active;
+
+	core_int_active.value = nitrox_read_csr(ndev, NPS_CORE_INT_ACTIVE);
+
+	if (core_int_active.s.nps_core)
+		clear_nps_core_err_intr(ndev);
+
+	if (core_int_active.s.nps_pkt)
+		clear_nps_pkt_err_intr(ndev);
+
+	if (core_int_active.s.pom)
+		clear_pom_err_intr(ndev);
+
+	if (core_int_active.s.pem)
+		clear_pem_err_intr(ndev);
+
+	if (core_int_active.s.lbc)
+		clear_lbc_err_intr(ndev);
+
+	if (core_int_active.s.efl)
+		clear_efl_err_intr(ndev);
+
+	if (core_int_active.s.bmi)
+		clear_bmi_err_intr(ndev);
+
+	/* If more work callback the ISR, set resend */
+	core_int_active.s.resend = 1;
+	nitrox_write_csr(ndev, NPS_CORE_INT_ACTIVE, core_int_active.value);
+}
+
+static irqreturn_t nps_core_int_isr(int irq, void *data)
+{
+	struct nitrox_device *ndev = data;
+
+	clear_nps_core_int_active(ndev);
+
+	return IRQ_HANDLED;
+}
+
+static int nitrox_enable_msix(struct nitrox_device *ndev)
+{
+	struct msix_entry *entries;
+	char **names;
+	int i, nr_entries, ret;
+
+	/*
+	 * PF MSI-X vectors
+	 *
+	 * Entry 0: NPS PKT ring 0
+	 * Entry 1: AQMQ ring 0
+	 * Entry 2: ZQM ring 0
+	 * Entry 3: NPS PKT ring 1
+	 * Entry 4: AQMQ ring 1
+	 * Entry 5: ZQM ring 1
+	 * ....
+	 * Entry 192: NPS_CORE_INT_ACTIVE
+	 */
+	nr_entries = (ndev->nr_queues * NR_RING_VECTORS) + 1;
+	entries = kzalloc_node(nr_entries * sizeof(struct msix_entry),
+			       GFP_KERNEL, ndev->node);
+	if (!entries)
+		return -ENOMEM;
+
+	names = kcalloc(nr_entries, sizeof(char *), GFP_KERNEL);
+	if (!names) {
+		kfree(entries);
+		return -ENOMEM;
+	}
+
+	/* fill entires */
+	for (i = 0; i < (nr_entries - 1); i++)
+		entries[i].entry = i;
+
+	entries[i].entry = NPS_CORE_INT_ACTIVE_ENTRY;
+
+	for (i = 0; i < nr_entries; i++) {
+		*(names + i) = kzalloc(MAX_MSIX_VECTOR_NAME, GFP_KERNEL);
+		if (!(*(names + i))) {
+			ret = -ENOMEM;
+			goto msix_fail;
+		}
+	}
+	ndev->msix.entries = entries;
+	ndev->msix.names = names;
+	ndev->msix.nr_entries = nr_entries;
+
+	ret = pci_enable_msix_exact(ndev->pdev, ndev->msix.entries,
+				    ndev->msix.nr_entries);
+	if (ret) {
+		dev_err(&ndev->pdev->dev, "Failed to enable MSI-X IRQ(s) %d\n",
+			ret);
+		goto msix_fail;
+	}
+	return 0;
+
+msix_fail:
+	for (i = 0; i < nr_entries; i++)
+		kfree(*(names + i));
+
+	kfree(entries);
+	kfree(names);
+	return ret;
+}
+
+static void nitrox_cleanup_pkt_slc_bh(struct nitrox_device *ndev)
+{
+	int i;
+
+	if (!ndev->bh.slc)
+		return;
+
+	for (i = 0; i < ndev->nr_queues; i++) {
+		struct bh_data *bh = &ndev->bh.slc[i];
+
+		tasklet_disable(&bh->resp_handler);
+		tasklet_kill(&bh->resp_handler);
+	}
+	kfree(ndev->bh.slc);
+	ndev->bh.slc = NULL;
+}
+
+static int nitrox_setup_pkt_slc_bh(struct nitrox_device *ndev)
+{
+	u32 size;
+	int i;
+
+	size = ndev->nr_queues * sizeof(struct bh_data);
+	ndev->bh.slc = kzalloc(size, GFP_KERNEL);
+	if (!ndev->bh.slc)
+		return -ENOMEM;
+
+	for (i = 0; i < ndev->nr_queues; i++) {
+		struct bh_data *bh = &ndev->bh.slc[i];
+		u64 offset;
+
+		offset = NPS_PKT_SLC_CNTSX(i);
+		/* pre calculate completion count address */
+		bh->completion_cnt_csr_addr = NITROX_CSR_ADDR(ndev, offset);
+		bh->cmdq = &ndev->pkt_cmdqs[i];
+
+		tasklet_init(&bh->resp_handler, pkt_slc_resp_handler,
+			     (unsigned long)bh);
+	}
+
+	return 0;
+}
+
+static int nitrox_request_irqs(struct nitrox_device *ndev)
+{
+	struct pci_dev *pdev = ndev->pdev;
+	struct msix_entry *msix_ent = ndev->msix.entries;
+	int nr_ring_vectors, i = 0, ring, cpu, ret;
+	char *name;
+
+	/*
+	 * PF MSI-X vectors
+	 *
+	 * Entry 0: NPS PKT ring 0
+	 * Entry 1: AQMQ ring 0
+	 * Entry 2: ZQM ring 0
+	 * Entry 3: NPS PKT ring 1
+	 * ....
+	 * Entry 192: NPS_CORE_INT_ACTIVE
+	 */
+	nr_ring_vectors = ndev->nr_queues * NR_RING_VECTORS;
+
+	/* request irq for pkt ring/ports only */
+	while (i < nr_ring_vectors) {
+		name = *(ndev->msix.names + i);
+		ring = (i / NR_RING_VECTORS);
+		snprintf(name, MAX_MSIX_VECTOR_NAME, "n5(%d)-slc-ring%d",
+			 ndev->idx, ring);
+
+		ret = request_irq(msix_ent[i].vector, nps_pkt_slc_isr, 0,
+				  name, &ndev->bh.slc[ring]);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to get irq %d for %s\n",
+				msix_ent[i].vector, name);
+			return ret;
+		}
+		cpu = ring % num_online_cpus();
+		irq_set_affinity_hint(msix_ent[i].vector, get_cpu_mask(cpu));
+
+		set_bit(i, ndev->msix.irqs);
+		i += NR_RING_VECTORS;
+	}
+
+	/* Request IRQ for NPS_CORE_INT_ACTIVE */
+	name = *(ndev->msix.names + i);
+	snprintf(name, MAX_MSIX_VECTOR_NAME, "n5(%d)-nps-core-int", ndev->idx);
+	ret = request_irq(msix_ent[i].vector, nps_core_int_isr, 0, name, ndev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to get irq %d for %s\n",
+			msix_ent[i].vector, name);
+		return ret;
+	}
+	set_bit(i, ndev->msix.irqs);
+
+	return 0;
+}
+
+static void nitrox_disable_msix(struct nitrox_device *ndev)
+{
+	struct msix_entry *msix_ent = ndev->msix.entries;
+	char **names = ndev->msix.names;
+	int i = 0, ring, nr_ring_vectors;
+
+	nr_ring_vectors = ndev->msix.nr_entries - 1;
+
+	/* clear pkt ring irqs */
+	while (i < nr_ring_vectors) {
+		if (test_and_clear_bit(i, ndev->msix.irqs)) {
+			ring = (i / NR_RING_VECTORS);
+			irq_set_affinity_hint(msix_ent[i].vector, NULL);
+			free_irq(msix_ent[i].vector, &ndev->bh.slc[ring]);
+		}
+		i += NR_RING_VECTORS;
+	}
+	irq_set_affinity_hint(msix_ent[i].vector, NULL);
+	free_irq(msix_ent[i].vector, ndev);
+	clear_bit(i, ndev->msix.irqs);
+
+	kfree(ndev->msix.entries);
+	for (i = 0; i < ndev->msix.nr_entries; i++)
+		kfree(*(names + i));
+
+	kfree(names);
+	pci_disable_msix(ndev->pdev);
+}
+
+/**
+ * nitrox_pf_cleanup_isr: Cleanup PF MSI-X and IRQ
+ * @ndev: NITROX device
+ */
+void nitrox_pf_cleanup_isr(struct nitrox_device *ndev)
+{
+	nitrox_disable_msix(ndev);
+	nitrox_cleanup_pkt_slc_bh(ndev);
+}
+
+/**
+ * nitrox_init_isr - Initialize PF MSI-X vectors and IRQ
+ * @ndev: NITROX device
+ *
+ * Return: 0 on success, a negative value on failure.
+ */
+int nitrox_pf_init_isr(struct nitrox_device *ndev)
+{
+	int err;
+
+	err = nitrox_setup_pkt_slc_bh(ndev);
+	if (err)
+		return err;
+
+	err = nitrox_enable_msix(ndev);
+	if (err)
+		goto msix_fail;
+
+	err = nitrox_request_irqs(ndev);
+	if (err)
+		goto irq_fail;
+
+	return 0;
+
+irq_fail:
+	nitrox_disable_msix(ndev);
+msix_fail:
+	nitrox_cleanup_pkt_slc_bh(ndev);
+	return err;
+}
diff --git a/drivers/crypto/cavium/nitrox/nitrox_lib.c b/drivers/crypto/cavium/nitrox/nitrox_lib.c
new file mode 100644
index 000000000000..b4a391adb9b6
--- /dev/null
+++ b/drivers/crypto/cavium/nitrox/nitrox_lib.c
@@ -0,0 +1,210 @@
+#include <linux/cpumask.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+#include <linux/delay.h>
+#include <linux/gfp.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci_regs.h>
+#include <linux/vmalloc.h>
+#include <linux/pci.h>
+
+#include "nitrox_dev.h"
+#include "nitrox_common.h"
+#include "nitrox_req.h"
+#include "nitrox_csr.h"
+
+#define CRYPTO_CTX_SIZE	256
+
+/* command queue alignments */
+#define PKT_IN_ALIGN	16
+
+static int cmdq_common_init(struct nitrox_cmdq *cmdq)
+{
+	struct nitrox_device *ndev = cmdq->ndev;
+	u32 qsize;
+
+	qsize = (ndev->qlen) * cmdq->instr_size;
+	cmdq->head_unaligned = dma_zalloc_coherent(DEV(ndev),
+						   (qsize + PKT_IN_ALIGN),
+						   &cmdq->dma_unaligned,
+						   GFP_KERNEL);
+	if (!cmdq->head_unaligned)
+		return -ENOMEM;
+
+	cmdq->head = PTR_ALIGN(cmdq->head_unaligned, PKT_IN_ALIGN);
+	cmdq->dma = PTR_ALIGN(cmdq->dma_unaligned, PKT_IN_ALIGN);
+	cmdq->qsize = (qsize + PKT_IN_ALIGN);
+
+	spin_lock_init(&cmdq->response_lock);
+	spin_lock_init(&cmdq->cmdq_lock);
+	spin_lock_init(&cmdq->backlog_lock);
+
+	INIT_LIST_HEAD(&cmdq->response_head);
+	INIT_LIST_HEAD(&cmdq->backlog_head);
+	INIT_WORK(&cmdq->backlog_qflush, backlog_qflush_work);
+
+	atomic_set(&cmdq->pending_count, 0);
+	atomic_set(&cmdq->backlog_count, 0);
+	return 0;
+}
+
+static void cmdq_common_cleanup(struct nitrox_cmdq *cmdq)
+{
+	struct nitrox_device *ndev = cmdq->ndev;
+
+	cancel_work_sync(&cmdq->backlog_qflush);
+
+	dma_free_coherent(DEV(ndev), cmdq->qsize,
+			  cmdq->head_unaligned, cmdq->dma_unaligned);
+
+	atomic_set(&cmdq->pending_count, 0);
+	atomic_set(&cmdq->backlog_count, 0);
+
+	cmdq->dbell_csr_addr = NULL;
+	cmdq->head = NULL;
+	cmdq->dma = 0;
+	cmdq->qsize = 0;
+	cmdq->instr_size = 0;
+}
+
+static void nitrox_cleanup_pkt_cmdqs(struct nitrox_device *ndev)
+{
+	int i;
+
+	for (i = 0; i < ndev->nr_queues; i++) {
+		struct nitrox_cmdq *cmdq = &ndev->pkt_cmdqs[i];
+
+		cmdq_common_cleanup(cmdq);
+	}
+	kfree(ndev->pkt_cmdqs);
+	ndev->pkt_cmdqs = NULL;
+}
+
+static int nitrox_init_pkt_cmdqs(struct nitrox_device *ndev)
+{
+	int i, err, size;
+
+	size = ndev->nr_queues * sizeof(struct nitrox_cmdq);
+	ndev->pkt_cmdqs = kzalloc(size, GFP_KERNEL);
+	if (!ndev->pkt_cmdqs)
+		return -ENOMEM;
+
+	for (i = 0; i < ndev->nr_queues; i++) {
+		struct nitrox_cmdq *cmdq;
+		u64 offset;
+
+		cmdq = &ndev->pkt_cmdqs[i];
+		cmdq->ndev = ndev;
+		cmdq->qno = i;
+		cmdq->instr_size = sizeof(struct nps_pkt_instr);
+
+		offset = NPS_PKT_IN_INSTR_BAOFF_DBELLX(i);
+		/* SE ring doorbell address for this queue */
+		cmdq->dbell_csr_addr = NITROX_CSR_ADDR(ndev, offset);
+
+		err = cmdq_common_init(cmdq);
+		if (err)
+			goto pkt_cmdq_fail;
+	}
+	return 0;
+
+pkt_cmdq_fail:
+	nitrox_cleanup_pkt_cmdqs(ndev);
+	return err;
+}
+
+static int create_crypto_dma_pool(struct nitrox_device *ndev)
+{
+	size_t size;
+
+	/* Crypto context pool, 16 byte aligned */
+	size = CRYPTO_CTX_SIZE + sizeof(struct ctx_hdr);
+	ndev->ctx_pool = dma_pool_create("crypto-context",
+					 DEV(ndev), size, 16, 0);
+	if (!ndev->ctx_pool)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void destroy_crypto_dma_pool(struct nitrox_device *ndev)
+{
+	if (!ndev->ctx_pool)
+		return;
+
+	dma_pool_destroy(ndev->ctx_pool);
+	ndev->ctx_pool = NULL;
+}
+
+/*
+ * crypto_alloc_context - Allocate crypto context from pool
+ * @ndev: NITROX Device
+ */
+void *crypto_alloc_context(struct nitrox_device *ndev)
+{
+	struct ctx_hdr *ctx;
+	void *vaddr;
+	dma_addr_t dma;
+
+	vaddr = dma_pool_alloc(ndev->ctx_pool, (GFP_ATOMIC | __GFP_ZERO), &dma);
+	if (!vaddr)
+		return NULL;
+
+	/* fill meta data */
+	ctx = vaddr;
+	ctx->pool = ndev->ctx_pool;
+	ctx->dma = dma;
+	ctx->ctx_dma = dma + sizeof(struct ctx_hdr);
+
+	return ((u8 *)vaddr + sizeof(struct ctx_hdr));
+}
+
+/**
+ * crypto_free_context - Free crypto context to pool
+ * @ctx: context to free
+ */
+void crypto_free_context(void *ctx)
+{
+	struct ctx_hdr *ctxp;
+
+	if (!ctx)
+		return;
+
+	ctxp = (struct ctx_hdr *)((u8 *)ctx - sizeof(struct ctx_hdr));
+	dma_pool_free(ctxp->pool, ctxp, ctxp->dma);
+}
+
+/**
+ * nitrox_common_sw_init - allocate software resources.
+ * @ndev: NITROX device
+ *
+ * Allocates crypto context pools and command queues etc.
+ *
+ * Return: 0 on success, or a negative error code on error.
+ */
+int nitrox_common_sw_init(struct nitrox_device *ndev)
+{
+	int err = 0;
+
+	/* per device crypto context pool */
+	err = create_crypto_dma_pool(ndev);
+	if (err)
+		return err;
+
+	err = nitrox_init_pkt_cmdqs(ndev);
+	if (err)
+		destroy_crypto_dma_pool(ndev);
+
+	return err;
+}
+
+/**
+ * nitrox_common_sw_cleanup - free software resources.
+ * @ndev: NITROX device
+ */
+void nitrox_common_sw_cleanup(struct nitrox_device *ndev)
+{
+	nitrox_cleanup_pkt_cmdqs(ndev);
+	destroy_crypto_dma_pool(ndev);
+}
diff --git a/drivers/crypto/cavium/nitrox/nitrox_main.c b/drivers/crypto/cavium/nitrox/nitrox_main.c
new file mode 100644
index 000000000000..ae44a464cd2d
--- /dev/null
+++ b/drivers/crypto/cavium/nitrox/nitrox_main.c
@@ -0,0 +1,640 @@
+#include <linux/aer.h>
+#include <linux/delay.h>
+#include <linux/debugfs.h>
+#include <linux/firmware.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/pci.h>
+#include <linux/pci_ids.h>
+
+#include "nitrox_dev.h"
+#include "nitrox_common.h"
+#include "nitrox_csr.h"
+
+#define CNN55XX_DEV_ID	0x12
+#define MAX_PF_QUEUES	64
+#define UCODE_HLEN 48
+#define SE_GROUP 0
+
+#define DRIVER_VERSION "1.0"
+/* SE microcode */
+#define SE_FW	"cnn55xx_se.fw"
+
+static const char nitrox_driver_name[] = "CNN55XX";
+
+static LIST_HEAD(ndevlist);
+static DEFINE_MUTEX(devlist_lock);
+static unsigned int num_devices;
+
+/**
+ * nitrox_pci_tbl - PCI Device ID Table
+ */
+static const struct pci_device_id nitrox_pci_tbl[] = {
+	{PCI_VDEVICE(CAVIUM, CNN55XX_DEV_ID), 0},
+	/* required last entry */
+	{0, }
+};
+MODULE_DEVICE_TABLE(pci, nitrox_pci_tbl);
+
+static unsigned int qlen = DEFAULT_CMD_QLEN;
+module_param(qlen, uint, 0644);
+MODULE_PARM_DESC(qlen, "Command queue length - default 2048");
+
+/**
+ * struct ucode - Firmware Header
+ * @id: microcode ID
+ * @version: firmware version
+ * @code_size: code section size
+ * @raz: alignment
+ * @code: code section
+ */
+struct ucode {
+	u8 id;
+	char version[VERSION_LEN - 1];
+	__be32 code_size;
+	u8 raz[12];
+	u64 code[0];
+};
+
+/**
+ * write_to_ucd_unit - Write Firmware to NITROX UCD unit
+ */
+static void write_to_ucd_unit(struct nitrox_device *ndev,
+			      struct ucode *ucode)
+{
+	u32 code_size = be32_to_cpu(ucode->code_size) * 2;
+	u64 offset, data;
+	int i = 0;
+
+	/*
+	 * UCD structure
+	 *
+	 *  -------------
+	 *  |    BLK 7  |
+	 *  -------------
+	 *  |    BLK 6  |
+	 *  -------------
+	 *  |    ...    |
+	 *  -------------
+	 *  |    BLK 0  |
+	 *  -------------
+	 *  Total of 8 blocks, each size 32KB
+	 */
+
+	/* set the block number */
+	offset = UCD_UCODE_LOAD_BLOCK_NUM;
+	nitrox_write_csr(ndev, offset, 0);
+
+	code_size = roundup(code_size, 8);
+	while (code_size) {
+		data = ucode->code[i];
+		/* write 8 bytes at a time */
+		offset = UCD_UCODE_LOAD_IDX_DATAX(i);
+		nitrox_write_csr(ndev, offset, data);
+		code_size -= 8;
+		i++;
+	}
+
+	/* put all SE cores in group 0 */
+	offset = POM_GRP_EXECMASKX(SE_GROUP);
+	nitrox_write_csr(ndev, offset, (~0ULL));
+
+	for (i = 0; i < ndev->hw.se_cores; i++) {
+		/*
+		 * write block number and firware length
+		 * bit:<2:0> block number
+		 * bit:3 is set SE uses 32KB microcode
+		 * bit:3 is clear SE uses 64KB microcode
+		 */
+		offset = UCD_SE_EID_UCODE_BLOCK_NUMX(i);
+		nitrox_write_csr(ndev, offset, 0x8);
+	}
+	usleep_range(300, 400);
+}
+
+static int nitrox_load_fw(struct nitrox_device *ndev, const char *fw_name)
+{
+	const struct firmware *fw;
+	struct ucode *ucode;
+	int ret;
+
+	dev_info(DEV(ndev), "Loading firmware \"%s\"\n", fw_name);
+
+	ret = request_firmware(&fw, fw_name, DEV(ndev));
+	if (ret < 0) {
+		dev_err(DEV(ndev), "failed to get firmware %s\n", fw_name);
+		return ret;
+	}
+
+	ucode = (struct ucode *)fw->data;
+	/* copy the firmware version */
+	memcpy(ndev->hw.fw_name, ucode->version, (VERSION_LEN - 2));
+	ndev->hw.fw_name[VERSION_LEN - 1] = '\0';
+
+	write_to_ucd_unit(ndev, ucode);
+	release_firmware(fw);
+
+	set_bit(NITROX_UCODE_LOADED, &ndev->status);
+	/* barrier to sync with other cpus */
+	smp_mb__after_atomic();
+	return 0;
+}
+
+/**
+ * nitrox_add_to_devlist - add NITROX device to global device list
+ * @ndev: NITROX device
+ */
+static int nitrox_add_to_devlist(struct nitrox_device *ndev)
+{
+	struct nitrox_device *dev;
+	int ret = 0;
+
+	INIT_LIST_HEAD(&ndev->list);
+	refcount_set(&ndev->refcnt, 1);
+
+	mutex_lock(&devlist_lock);
+	list_for_each_entry(dev, &ndevlist, list) {
+		if (dev == ndev) {
+			ret = -EEXIST;
+			goto unlock;
+		}
+	}
+	ndev->idx = num_devices++;
+	list_add_tail(&ndev->list, &ndevlist);
+unlock:
+	mutex_unlock(&devlist_lock);
+	return ret;
+}
+
+/**
+ * nitrox_remove_from_devlist - remove NITROX device from
+ *   global device list
+ * @ndev: NITROX device
+ */
+static void nitrox_remove_from_devlist(struct nitrox_device *ndev)
+{
+	mutex_lock(&devlist_lock);
+	list_del(&ndev->list);
+	num_devices--;
+	mutex_unlock(&devlist_lock);
+}
+
+struct nitrox_device *nitrox_get_first_device(void)
+{
+	struct nitrox_device *ndev = NULL;
+
+	mutex_lock(&devlist_lock);
+	list_for_each_entry(ndev, &ndevlist, list) {
+		if (nitrox_ready(ndev))
+			break;
+	}
+	mutex_unlock(&devlist_lock);
+	if (!ndev)
+		return NULL;
+
+	refcount_inc(&ndev->refcnt);
+	/* barrier to sync with other cpus */
+	smp_mb__after_atomic();
+	return ndev;
+}
+
+void nitrox_put_device(struct nitrox_device *ndev)
+{
+	if (!ndev)
+		return;
+
+	refcount_dec(&ndev->refcnt);
+	/* barrier to sync with other cpus */
+	smp_mb__after_atomic();
+}
+
+static int nitrox_reset_device(struct pci_dev *pdev)
+{
+	int pos = 0;
+
+	pos = pci_save_state(pdev);
+	if (pos) {
+		dev_err(&pdev->dev, "Failed to save pci state\n");
+		return -ENOMEM;
+	}
+
+	pos = pci_pcie_cap(pdev);
+	if (!pos)
+		return -ENOTTY;
+
+	if (!pci_wait_for_pending_transaction(pdev))
+		dev_err(&pdev->dev, "waiting for pending transaction\n");
+
+	pcie_capability_set_word(pdev, PCI_EXP_DEVCTL, PCI_EXP_DEVCTL_BCR_FLR);
+	msleep(100);
+	pci_restore_state(pdev);
+
+	return 0;
+}
+
+static int nitrox_pf_sw_init(struct nitrox_device *ndev)
+{
+	int err;
+
+	err = nitrox_common_sw_init(ndev);
+	if (err)
+		return err;
+
+	err = nitrox_pf_init_isr(ndev);
+	if (err)
+		nitrox_common_sw_cleanup(ndev);
+
+	return err;
+}
+
+static void nitrox_pf_sw_cleanup(struct nitrox_device *ndev)
+{
+	nitrox_pf_cleanup_isr(ndev);
+	nitrox_common_sw_cleanup(ndev);
+}
+
+/**
+ * nitrox_bist_check - Check NITORX BIST registers status
+ * @ndev: NITROX device
+ */
+static int nitrox_bist_check(struct nitrox_device *ndev)
+{
+	u64 value = 0;
+	int i;
+
+	for (i = 0; i < NR_CLUSTERS; i++) {
+		value += nitrox_read_csr(ndev, EMU_BIST_STATUSX(i));
+		value += nitrox_read_csr(ndev, EFL_CORE_BIST_REGX(i));
+	}
+	value += nitrox_read_csr(ndev, UCD_BIST_STATUS);
+	value += nitrox_read_csr(ndev, NPS_CORE_BIST_REG);
+	value += nitrox_read_csr(ndev, NPS_CORE_NPC_BIST_REG);
+	value += nitrox_read_csr(ndev, NPS_PKT_SLC_BIST_REG);
+	value += nitrox_read_csr(ndev, NPS_PKT_IN_BIST_REG);
+	value += nitrox_read_csr(ndev, POM_BIST_REG);
+	value += nitrox_read_csr(ndev, BMI_BIST_REG);
+	value += nitrox_read_csr(ndev, EFL_TOP_BIST_STAT);
+	value += nitrox_read_csr(ndev, BMO_BIST_REG);
+	value += nitrox_read_csr(ndev, LBC_BIST_STATUS);
+	value += nitrox_read_csr(ndev, PEM_BIST_STATUSX(0));
+	if (value)
+		return -EIO;
+	return 0;
+}
+
+static void nitrox_get_hwinfo(struct nitrox_device *ndev)
+{
+	union emu_fuse_map emu_fuse;
+	u64 offset;
+	int i;
+
+	for (i = 0; i < NR_CLUSTERS; i++) {
+		u8 dead_cores;
+
+		offset = EMU_FUSE_MAPX(i);
+		emu_fuse.value = nitrox_read_csr(ndev, offset);
+		if (emu_fuse.s.valid) {
+			dead_cores = hweight32(emu_fuse.s.ae_fuse);
+			ndev->hw.ae_cores += AE_CORES_PER_CLUSTER - dead_cores;
+			dead_cores = hweight16(emu_fuse.s.se_fuse);
+			ndev->hw.se_cores += SE_CORES_PER_CLUSTER - dead_cores;
+		}
+	}
+}
+
+static int nitrox_pf_hw_init(struct nitrox_device *ndev)
+{
+	int err;
+
+	err = nitrox_bist_check(ndev);
+	if (err) {
+		dev_err(&ndev->pdev->dev, "BIST check failed\n");
+		return err;
+	}
+	/* get cores information */
+	nitrox_get_hwinfo(ndev);
+
+	nitrox_config_nps_unit(ndev);
+	nitrox_config_pom_unit(ndev);
+	nitrox_config_efl_unit(ndev);
+	/* configure IO units */
+	nitrox_config_bmi_unit(ndev);
+	nitrox_config_bmo_unit(ndev);
+	/* configure Local Buffer Cache */
+	nitrox_config_lbc_unit(ndev);
+	nitrox_config_rand_unit(ndev);
+
+	/* load firmware on SE cores */
+	err = nitrox_load_fw(ndev, SE_FW);
+	if (err)
+		return err;
+
+	nitrox_config_emu_unit(ndev);
+
+	return 0;
+}
+
+#if IS_ENABLED(CONFIG_DEBUG_FS)
+static int registers_show(struct seq_file *s, void *v)
+{
+	struct nitrox_device *ndev = s->private;
+	u64 offset;
+
+	/* NPS DMA stats */
+	offset = NPS_STATS_PKT_DMA_RD_CNT;
+	seq_printf(s, "NPS_STATS_PKT_DMA_RD_CNT  0x%016llx\n",
+		   nitrox_read_csr(ndev, offset));
+	offset = NPS_STATS_PKT_DMA_WR_CNT;
+	seq_printf(s, "NPS_STATS_PKT_DMA_WR_CNT  0x%016llx\n",
+		   nitrox_read_csr(ndev, offset));
+
+	/* BMI/BMO stats */
+	offset = BMI_NPS_PKT_CNT;
+	seq_printf(s, "BMI_NPS_PKT_CNT  0x%016llx\n",
+		   nitrox_read_csr(ndev, offset));
+	offset = BMO_NPS_SLC_PKT_CNT;
+	seq_printf(s, "BMO_NPS_PKT_CNT  0x%016llx\n",
+		   nitrox_read_csr(ndev, offset));
+
+	return 0;
+}
+
+static int registers_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, registers_show, inode->i_private);
+}
+
+static const struct file_operations register_fops = {
+	.owner = THIS_MODULE,
+	.open = registers_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static int firmware_show(struct seq_file *s, void *v)
+{
+	struct nitrox_device *ndev = s->private;
+
+	seq_printf(s, "Version: %s\n", ndev->hw.fw_name);
+	return 0;
+}
+
+static int firmware_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, firmware_show, inode->i_private);
+}
+
+static const struct file_operations firmware_fops = {
+	.owner = THIS_MODULE,
+	.open = firmware_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static int nitrox_show(struct seq_file *s, void *v)
+{
+	struct nitrox_device *ndev = s->private;
+
+	seq_printf(s, "NITROX-5 [idx: %d]\n", ndev->idx);
+	seq_printf(s, "  Revision ID: 0x%0x\n", ndev->hw.revision_id);
+	seq_printf(s, "  Cores [AE: %u  SE: %u]\n",
+		   ndev->hw.ae_cores, ndev->hw.se_cores);
+	seq_printf(s, "  Number of Queues: %u\n", ndev->nr_queues);
+	seq_printf(s, "  Queue length: %u\n", ndev->qlen);
+	seq_printf(s, "  Node: %u\n", ndev->node);
+
+	return 0;
+}
+
+static int nitrox_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, nitrox_show, inode->i_private);
+}
+
+static const struct file_operations nitrox_fops = {
+	.owner = THIS_MODULE,
+	.open = nitrox_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static void nitrox_debugfs_exit(struct nitrox_device *ndev)
+{
+	debugfs_remove_recursive(ndev->debugfs_dir);
+	ndev->debugfs_dir = NULL;
+}
+
+static int nitrox_debugfs_init(struct nitrox_device *ndev)
+{
+	struct dentry *dir, *f;
+
+	dir = debugfs_create_dir(KBUILD_MODNAME, NULL);
+	if (!dir)
+		return -ENOMEM;
+
+	ndev->debugfs_dir = dir;
+	f = debugfs_create_file("counters", 0400, dir, ndev, &register_fops);
+	if (!f)
+		goto err;
+	f = debugfs_create_file("firmware", 0400, dir, ndev, &firmware_fops);
+	if (!f)
+		goto err;
+	f = debugfs_create_file("nitrox", 0400, dir, ndev, &nitrox_fops);
+	if (!f)
+		goto err;
+
+	return 0;
+
+err:
+	nitrox_debugfs_exit(ndev);
+	return -ENODEV;
+}
+#else
+static int nitrox_debugfs_init(struct nitrox_device *ndev)
+{
+	return 0;
+}
+
+static void nitrox_debugfs_exit(struct nitrox_device *ndev)
+{
+}
+#endif
+
+/**
+ * nitrox_probe - NITROX Initialization function.
+ * @pdev: PCI device information struct
+ * @id: entry in nitrox_pci_tbl
+ *
+ * Return: 0, if the driver is bound to the device, or
+ *         a negative error if there is failure.
+ */
+static int nitrox_probe(struct pci_dev *pdev,
+			const struct pci_device_id *id)
+{
+	struct nitrox_device *ndev;
+	int err;
+
+	dev_info_once(&pdev->dev, "%s driver version %s\n",
+		      nitrox_driver_name, DRIVER_VERSION);
+
+	err = pci_enable_device_mem(pdev);
+	if (err)
+		return err;
+
+	/* do FLR */
+	err = nitrox_reset_device(pdev);
+	if (err) {
+		dev_err(&pdev->dev, "FLR failed\n");
+		pci_disable_device(pdev);
+		return err;
+	}
+
+	if (!dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(64))) {
+		dev_dbg(&pdev->dev, "DMA to 64-BIT address\n");
+	} else {
+		err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
+		if (err) {
+			dev_err(&pdev->dev, "DMA configuration failed\n");
+			pci_disable_device(pdev);
+			return err;
+		}
+	}
+
+	err = pci_request_mem_regions(pdev, nitrox_driver_name);
+	if (err) {
+		pci_disable_device(pdev);
+		return err;
+	}
+	pci_set_master(pdev);
+
+	ndev = kzalloc(sizeof(*ndev), GFP_KERNEL);
+	if (!ndev)
+		goto ndev_fail;
+
+	pci_set_drvdata(pdev, ndev);
+	ndev->pdev = pdev;
+
+	/* add to device list */
+	nitrox_add_to_devlist(ndev);
+
+	ndev->hw.vendor_id = pdev->vendor;
+	ndev->hw.device_id = pdev->device;
+	ndev->hw.revision_id = pdev->revision;
+	/* command timeout in jiffies */
+	ndev->timeout = msecs_to_jiffies(CMD_TIMEOUT);
+	ndev->node = dev_to_node(&pdev->dev);
+	if (ndev->node == NUMA_NO_NODE)
+		ndev->node = 0;
+
+	ndev->bar_addr = ioremap(pci_resource_start(pdev, 0),
+				 pci_resource_len(pdev, 0));
+	if (!ndev->bar_addr) {
+		err = -EIO;
+		goto ioremap_err;
+	}
+	/* allocate command queus based on cpus, max queues are 64 */
+	ndev->nr_queues = min_t(u32, MAX_PF_QUEUES, num_online_cpus());
+	ndev->qlen = qlen;
+
+	err = nitrox_pf_sw_init(ndev);
+	if (err)
+		goto ioremap_err;
+
+	err = nitrox_pf_hw_init(ndev);
+	if (err)
+		goto pf_hw_fail;
+
+	err = nitrox_debugfs_init(ndev);
+	if (err)
+		goto pf_hw_fail;
+
+	set_bit(NITROX_READY, &ndev->status);
+	/* barrier to sync with other cpus */
+	smp_mb__after_atomic();
+
+	err = nitrox_crypto_register();
+	if (err)
+		goto crypto_fail;
+
+	return 0;
+
+crypto_fail:
+	nitrox_debugfs_exit(ndev);
+	clear_bit(NITROX_READY, &ndev->status);
+	/* barrier to sync with other cpus */
+	smp_mb__after_atomic();
+pf_hw_fail:
+	nitrox_pf_sw_cleanup(ndev);
+ioremap_err:
+	nitrox_remove_from_devlist(ndev);
+	kfree(ndev);
+	pci_set_drvdata(pdev, NULL);
+ndev_fail:
+	pci_release_mem_regions(pdev);
+	pci_disable_device(pdev);
+	return err;
+}
+
+/**
+ * nitrox_remove - Unbind the driver from the device.
+ * @pdev: PCI device information struct
+ */
+static void nitrox_remove(struct pci_dev *pdev)
+{
+	struct nitrox_device *ndev = pci_get_drvdata(pdev);
+
+	if (!ndev)
+		return;
+
+	if (!refcount_dec_and_test(&ndev->refcnt)) {
+		dev_err(DEV(ndev), "Device refcnt not zero (%d)\n",
+			refcount_read(&ndev->refcnt));
+		return;
+	}
+
+	dev_info(DEV(ndev), "Removing Device %x:%x\n",
+		 ndev->hw.vendor_id, ndev->hw.device_id);
+
+	clear_bit(NITROX_READY, &ndev->status);
+	/* barrier to sync with other cpus */
+	smp_mb__after_atomic();
+
+	nitrox_remove_from_devlist(ndev);
+	nitrox_crypto_unregister();
+	nitrox_debugfs_exit(ndev);
+	nitrox_pf_sw_cleanup(ndev);
+
+	iounmap(ndev->bar_addr);
+	kfree(ndev);
+
+	pci_set_drvdata(pdev, NULL);
+	pci_release_mem_regions(pdev);
+	pci_disable_device(pdev);
+}
+
+static void nitrox_shutdown(struct pci_dev *pdev)
+{
+	pci_set_drvdata(pdev, NULL);
+	pci_release_mem_regions(pdev);
+	pci_disable_device(pdev);
+}
+
+static struct pci_driver nitrox_driver = {
+	.name = nitrox_driver_name,
+	.id_table = nitrox_pci_tbl,
+	.probe = nitrox_probe,
+	.remove	= nitrox_remove,
+	.shutdown = nitrox_shutdown,
+};
+
+module_pci_driver(nitrox_driver);
+
+MODULE_AUTHOR("Srikanth Jampala <Jampala.Srikanth@cavium.com>");
+MODULE_DESCRIPTION("Cavium CNN55XX PF Driver" DRIVER_VERSION " ");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_FIRMWARE(SE_FW);
diff --git a/drivers/crypto/cavium/nitrox/nitrox_req.h b/drivers/crypto/cavium/nitrox/nitrox_req.h
new file mode 100644
index 000000000000..74f4c20dc87d
--- /dev/null
+++ b/drivers/crypto/cavium/nitrox/nitrox_req.h
@@ -0,0 +1,445 @@
+#ifndef __NITROX_REQ_H
+#define __NITROX_REQ_H
+
+#include <linux/dma-mapping.h>
+#include <crypto/aes.h>
+
+#include "nitrox_dev.h"
+
+/**
+ * struct gphdr - General purpose Header
+ * @param0: first parameter.
+ * @param1: second parameter.
+ * @param2: third parameter.
+ * @param3: fourth parameter.
+ *
+ * Params tell the iv and enc/dec data offsets.
+ */
+struct gphdr {
+	__be16 param0;
+	__be16 param1;
+	__be16 param2;
+	__be16 param3;
+};
+
+/**
+ * struct se_req_ctrl - SE request information.
+ * @arg: Minor number of the opcode
+ * @ctxc: Context control.
+ * @unca: Uncertainity enabled.
+ * @info: Additional information for SE cores.
+ * @ctxl: Context length in bytes.
+ * @uddl: User defined data length
+ */
+union se_req_ctrl {
+	u64 value;
+	struct {
+		u64 raz	: 22;
+		u64 arg	: 8;
+		u64 ctxc : 2;
+		u64 unca : 1;
+		u64 info : 3;
+		u64 unc : 8;
+		u64 ctxl : 12;
+		u64 uddl : 8;
+	} s;
+};
+
+struct nitrox_sglist {
+	u16 len;
+	u16 raz0;
+	u32 raz1;
+	dma_addr_t dma;
+};
+
+#define MAX_IV_LEN 16
+
+/**
+ * struct se_crypto_request - SE crypto request structure.
+ * @opcode: Request opcode (enc/dec)
+ * @flags: flags from crypto subsystem
+ * @ctx_handle: Crypto context handle.
+ * @gph: GP Header
+ * @ctrl: Request Information.
+ * @in: Input sglist
+ * @out: Output sglist
+ */
+struct se_crypto_request {
+	u8 opcode;
+	gfp_t gfp;
+	u32 flags;
+	u64 ctx_handle;
+
+	struct gphdr gph;
+	union se_req_ctrl ctrl;
+
+	u8 iv[MAX_IV_LEN];
+	u16 ivsize;
+
+	struct scatterlist *src;
+	struct scatterlist *dst;
+};
+
+/* Crypto opcodes */
+#define FLEXI_CRYPTO_ENCRYPT_HMAC	0x33
+#define ENCRYPT	0
+#define DECRYPT 1
+
+/* IV from context */
+#define IV_FROM_CTX	0
+/* IV from Input data */
+#define IV_FROM_DPTR	1
+
+/**
+ * cipher opcodes for firmware
+ */
+enum flexi_cipher {
+	CIPHER_NULL = 0,
+	CIPHER_3DES_CBC,
+	CIPHER_3DES_ECB,
+	CIPHER_AES_CBC,
+	CIPHER_AES_ECB,
+	CIPHER_AES_CFB,
+	CIPHER_AES_CTR,
+	CIPHER_AES_GCM,
+	CIPHER_AES_XTS,
+	CIPHER_AES_CCM,
+	CIPHER_AES_CBC_CTS,
+	CIPHER_AES_ECB_CTS,
+	CIPHER_INVALID
+};
+
+/**
+ * struct crypto_keys - Crypto keys
+ * @key: Encryption key or KEY1 for AES-XTS
+ * @iv: Encryption IV or Tweak for AES-XTS
+ */
+struct crypto_keys {
+	union {
+		u8 key[AES_MAX_KEY_SIZE];
+		u8 key1[AES_MAX_KEY_SIZE];
+	} u;
+	u8 iv[AES_BLOCK_SIZE];
+};
+
+/**
+ * struct auth_keys - Authentication keys
+ * @ipad: IPAD or KEY2 for AES-XTS
+ * @opad: OPAD or AUTH KEY if auth_input_type = 1
+ */
+struct auth_keys {
+	union {
+		u8 ipad[64];
+		u8 key2[64];
+	} u;
+	u8 opad[64];
+};
+
+/**
+ * struct flexi_crypto_context - Crypto context
+ * @cipher_type: Encryption cipher type
+ * @aes_keylen: AES key length
+ * @iv_source: Encryption IV source
+ * @hash_type: Authentication type
+ * @auth_input_type: Authentication input type
+ *   1 - Authentication IV and KEY, microcode calculates OPAD/IPAD
+ *   0 - Authentication OPAD/IPAD
+ * @mac_len: mac length
+ * @crypto: Crypto keys
+ * @auth: Authentication keys
+ */
+struct flexi_crypto_context {
+	union {
+		__be64 flags;
+		struct {
+#if defined(__BIG_ENDIAN_BITFIELD)
+			u64 cipher_type	: 4;
+			u64 reserved_59	: 1;
+			u64 aes_keylen : 2;
+			u64 iv_source : 1;
+			u64 hash_type : 4;
+			u64 reserved_49_51 : 3;
+			u64 auth_input_type: 1;
+			u64 mac_len : 8;
+			u64 reserved_0_39 : 40;
+#else
+			u64 reserved_0_39 : 40;
+			u64 mac_len : 8;
+			u64 auth_input_type: 1;
+			u64 reserved_49_51 : 3;
+			u64 hash_type : 4;
+			u64 iv_source : 1;
+			u64 aes_keylen : 2;
+			u64 reserved_59	: 1;
+			u64 cipher_type	: 4;
+#endif
+		} w0;
+	};
+
+	struct crypto_keys crypto;
+	struct auth_keys auth;
+};
+
+struct nitrox_crypto_ctx {
+	struct nitrox_device *ndev;
+	union {
+		u64 ctx_handle;
+		struct flexi_crypto_context *fctx;
+	} u;
+};
+
+struct nitrox_kcrypt_request {
+	struct se_crypto_request creq;
+	struct nitrox_crypto_ctx *nctx;
+	struct skcipher_request *skreq;
+};
+
+/**
+ * struct pkt_instr_hdr - Packet Instruction Header
+ * @g: Gather used
+ *   When [G] is set and [GSZ] != 0, the instruction is
+ *   indirect gather instruction.
+ *   When [G] is set and [GSZ] = 0, the instruction is
+ *   direct gather instruction.
+ * @gsz: Number of pointers in the indirect gather list
+ * @ihi: When set hardware duplicates the 1st 8 bytes of pkt_instr_hdr
+ *   and adds them to the packet after the pkt_instr_hdr but before any UDD
+ * @ssz: Not used by the input hardware. But can become slc_store_int[SSZ]
+ *   when [IHI] is set.
+ * @fsz: The number of front data bytes directly included in the
+ *   PCIe instruction.
+ * @tlen: The length of the input packet in bytes, include:
+ *   - 16B pkt_hdr
+ *   - Inline context bytes if any,
+ *   - UDD if any,
+ *   - packet payload bytes
+ */
+union pkt_instr_hdr {
+	u64 value;
+	struct {
+#if defined(__BIG_ENDIAN_BITFIELD)
+		u64 raz_48_63 : 16;
+		u64 g : 1;
+		u64 gsz	: 7;
+		u64 ihi	: 1;
+		u64 ssz	: 7;
+		u64 raz_30_31 : 2;
+		u64 fsz	: 6;
+		u64 raz_16_23 : 8;
+		u64 tlen : 16;
+#else
+		u64 tlen : 16;
+		u64 raz_16_23 : 8;
+		u64 fsz	: 6;
+		u64 raz_30_31 : 2;
+		u64 ssz	: 7;
+		u64 ihi	: 1;
+		u64 gsz	: 7;
+		u64 g : 1;
+		u64 raz_48_63 : 16;
+#endif
+	} s;
+};
+
+/**
+ * struct pkt_hdr - Packet Input Header
+ * @opcode: Request opcode (Major)
+ * @arg: Request opcode (Minor)
+ * @ctxc: Context control.
+ * @unca: When set [UNC] is the uncertainty count for an input packet.
+ *        The hardware uses uncertainty counts to predict
+ *        output buffer use and avoid deadlock.
+ * @info: Not used by input hardware. Available for use
+ *        during SE processing.
+ * @destport: The expected destination port/ring/channel for the packet.
+ * @unc: Uncertainty count for an input packet.
+ * @grp: SE group that will process the input packet.
+ * @ctxl: Context Length in 64-bit words.
+ * @uddl: User-defined data (UDD) length in bytes.
+ * @ctxp: Context pointer. CTXP<63,2:0> must be zero in all cases.
+ */
+union pkt_hdr {
+	u64 value[2];
+	struct {
+#if defined(__BIG_ENDIAN_BITFIELD)
+		u64 opcode : 8;
+		u64 arg	: 8;
+		u64 ctxc : 2;
+		u64 unca : 1;
+		u64 raz_44 : 1;
+		u64 info : 3;
+		u64 destport : 9;
+		u64 unc	: 8;
+		u64 raz_19_23 : 5;
+		u64 grp	: 3;
+		u64 raz_15 : 1;
+		u64 ctxl : 7;
+		u64 uddl : 8;
+#else
+		u64 uddl : 8;
+		u64 ctxl : 7;
+		u64 raz_15 : 1;
+		u64 grp	: 3;
+		u64 raz_19_23 : 5;
+		u64 unc	: 8;
+		u64 destport : 9;
+		u64 info : 3;
+		u64 raz_44 : 1;
+		u64 unca : 1;
+		u64 ctxc : 2;
+		u64 arg	: 8;
+		u64 opcode : 8;
+#endif
+		__be64 ctxp;
+	} s;
+};
+
+/**
+ * struct slc_store_info - Solicited Paceket Output Store Information.
+ * @ssz: The number of scatterlist pointers for the solicited output port
+ *       packet.
+ * @rptr: The result pointer for the solicited output port packet.
+ *        If [SSZ]=0, [RPTR] must point directly to a buffer on the remote
+ *        host that is large enough to hold the entire output packet.
+ *        If [SSZ]!=0, [RPTR] must point to an array of ([SSZ]+3)/4
+ *        sglist components at [RPTR] on the remote host.
+ */
+union slc_store_info {
+	u64 value[2];
+	struct {
+#if defined(__BIG_ENDIAN_BITFIELD)
+		u64 raz_39_63 : 25;
+		u64 ssz	: 7;
+		u64 raz_0_31 : 32;
+#else
+		u64 raz_0_31 : 32;
+		u64 ssz	: 7;
+		u64 raz_39_63 : 25;
+#endif
+		__be64 rptr;
+	} s;
+};
+
+/**
+ * struct nps_pkt_instr - NPS Packet Instruction of SE cores.
+ * @dptr0 : Input pointer points to buffer in remote host.
+ * @ih: Packet Instruction Header (8 bytes)
+ * @irh: Packet Input Header (16 bytes)
+ * @slc: Solicited Packet Output Store Information (16 bytes)
+ * @fdata: Front data
+ *
+ * 64-Byte Instruction Format
+ */
+struct nps_pkt_instr {
+	__be64 dptr0;
+	union pkt_instr_hdr ih;
+	union pkt_hdr irh;
+	union slc_store_info slc;
+	u64 fdata[2];
+};
+
+/**
+ * struct ctx_hdr - Book keeping data about the crypto context
+ * @pool: Pool used to allocate crypto context
+ * @dma: Base DMA address of the cypto context
+ * @ctx_dma: Actual usable crypto context for NITROX
+ */
+struct ctx_hdr {
+	struct dma_pool *pool;
+	dma_addr_t dma;
+	dma_addr_t ctx_dma;
+};
+
+/*
+ * struct sglist_component - SG list component format
+ * @len0: The number of bytes at [PTR0] on the remote host.
+ * @len1: The number of bytes at [PTR1] on the remote host.
+ * @len2: The number of bytes at [PTR2] on the remote host.
+ * @len3: The number of bytes at [PTR3] on the remote host.
+ * @dma0: First pointer point to buffer in remote host.
+ * @dma1: Second pointer point to buffer in remote host.
+ * @dma2: Third pointer point to buffer in remote host.
+ * @dma3: Fourth pointer point to buffer in remote host.
+ */
+struct nitrox_sgcomp {
+	__be16 len[4];
+	__be64 dma[4];
+};
+
+/*
+ * strutct nitrox_sgtable - SG list information
+ * @map_cnt: Number of buffers mapped
+ * @nr_comp: Number of sglist components
+ * @total_bytes: Total bytes in sglist.
+ * @len: Total sglist components length.
+ * @dma: DMA address of sglist component.
+ * @dir: DMA direction.
+ * @buf: crypto request buffer.
+ * @sglist: SG list of input/output buffers.
+ * @sgcomp: sglist component for NITROX.
+ */
+struct nitrox_sgtable {
+	u8 map_bufs_cnt;
+	u8 nr_sgcomp;
+	u16 total_bytes;
+	u32 len;
+	dma_addr_t dma;
+	enum dma_data_direction dir;
+
+	struct scatterlist *buf;
+	struct nitrox_sglist *sglist;
+	struct nitrox_sgcomp *sgcomp;
+};
+
+/* Response Header Length */
+#define ORH_HLEN	8
+/* Completion bytes Length */
+#define COMP_HLEN	8
+
+struct resp_hdr {
+	u64 orh;
+	dma_addr_t orh_dma;
+	u64 completion;
+	dma_addr_t completion_dma;
+};
+
+typedef void (*completion_t)(struct skcipher_request *skreq, int err);
+
+/**
+ * struct nitrox_softreq - Represents the NIROX Request.
+ * @response: response list entry
+ * @backlog: Backlog list entry
+ * @ndev: Device used to submit the request
+ * @cmdq: Command queue for submission
+ * @resp: Response headers
+ * @instr: 64B instruction
+ * @in: SG table for input
+ * @out SG table for output
+ * @tstamp: Request submitted time in jiffies
+ * @callback: callback after request completion/timeout
+ * @cb_arg: callback argument
+ */
+struct nitrox_softreq {
+	struct list_head response;
+	struct list_head backlog;
+
+	u32 flags;
+	gfp_t gfp;
+	atomic_t status;
+	bool inplace;
+
+	struct nitrox_device *ndev;
+	struct nitrox_cmdq *cmdq;
+
+	struct nps_pkt_instr instr;
+	struct resp_hdr resp;
+	struct nitrox_sgtable in;
+	struct nitrox_sgtable out;
+
+	unsigned long tstamp;
+
+	completion_t callback;
+	struct skcipher_request *skreq;
+};
+
+#endif /* __NITROX_REQ_H */
diff --git a/drivers/crypto/cavium/nitrox/nitrox_reqmgr.c b/drivers/crypto/cavium/nitrox/nitrox_reqmgr.c
new file mode 100644
index 000000000000..4bb4377c5ac0
--- /dev/null
+++ b/drivers/crypto/cavium/nitrox/nitrox_reqmgr.c
@@ -0,0 +1,735 @@
+#include <linux/gfp.h>
+#include <linux/workqueue.h>
+#include <crypto/internal/skcipher.h>
+
+#include "nitrox_dev.h"
+#include "nitrox_req.h"
+#include "nitrox_csr.h"
+#include "nitrox_req.h"
+
+/* SLC_STORE_INFO */
+#define MIN_UDD_LEN 16
+/* PKT_IN_HDR + SLC_STORE_INFO */
+#define FDATA_SIZE 32
+/* Base destination port for the solicited requests */
+#define SOLICIT_BASE_DPORT 256
+#define PENDING_SIG	0xFFFFFFFFFFFFFFFFUL
+
+#define REQ_NOT_POSTED 1
+#define REQ_BACKLOG    2
+#define REQ_POSTED     3
+
+/**
+ * Response codes from SE microcode
+ * 0x00 - Success
+ *   Completion with no error
+ * 0x43 - ERR_GC_DATA_LEN_INVALID
+ *   Invalid Data length if Encryption Data length is
+ *   less than 16 bytes for AES-XTS and AES-CTS.
+ * 0x45 - ERR_GC_CTX_LEN_INVALID
+ *   Invalid context length: CTXL != 23 words.
+ * 0x4F - ERR_GC_DOCSIS_CIPHER_INVALID
+ *   DOCSIS support is enabled with other than
+ *   AES/DES-CBC mode encryption.
+ * 0x50 - ERR_GC_DOCSIS_OFFSET_INVALID
+ *   Authentication offset is other than 0 with
+ *   Encryption IV source = 0.
+ *   Authentication offset is other than 8 (DES)/16 (AES)
+ *   with Encryption IV source = 1
+ * 0x51 - ERR_GC_CRC32_INVALID_SELECTION
+ *   CRC32 is enabled for other than DOCSIS encryption.
+ * 0x52 - ERR_GC_AES_CCM_FLAG_INVALID
+ *   Invalid flag options in AES-CCM IV.
+ */
+
+/**
+ * dma_free_sglist - unmap and free the sg lists.
+ * @ndev: N5 device
+ * @sgtbl: SG table
+ */
+static void softreq_unmap_sgbufs(struct nitrox_softreq *sr)
+{
+	struct nitrox_device *ndev = sr->ndev;
+	struct device *dev = DEV(ndev);
+	struct nitrox_sglist *sglist;
+
+	/* unmap in sgbuf */
+	sglist = sr->in.sglist;
+	if (!sglist)
+		goto out_unmap;
+
+	/* unmap iv */
+	dma_unmap_single(dev, sglist->dma, sglist->len, DMA_BIDIRECTIONAL);
+	/* unmpa src sglist */
+	dma_unmap_sg(dev, sr->in.buf, (sr->in.map_bufs_cnt - 1), sr->in.dir);
+	/* unamp gather component */
+	dma_unmap_single(dev, sr->in.dma, sr->in.len, DMA_TO_DEVICE);
+	kfree(sr->in.sglist);
+	kfree(sr->in.sgcomp);
+	sr->in.sglist = NULL;
+	sr->in.buf = NULL;
+	sr->in.map_bufs_cnt = 0;
+
+out_unmap:
+	/* unmap out sgbuf */
+	sglist = sr->out.sglist;
+	if (!sglist)
+		return;
+
+	/* unmap orh */
+	dma_unmap_single(dev, sr->resp.orh_dma, ORH_HLEN, sr->out.dir);
+
+	/* unmap dst sglist */
+	if (!sr->inplace) {
+		dma_unmap_sg(dev, sr->out.buf, (sr->out.map_bufs_cnt - 3),
+			     sr->out.dir);
+	}
+	/* unmap completion */
+	dma_unmap_single(dev, sr->resp.completion_dma, COMP_HLEN, sr->out.dir);
+
+	/* unmap scatter component */
+	dma_unmap_single(dev, sr->out.dma, sr->out.len, DMA_TO_DEVICE);
+	kfree(sr->out.sglist);
+	kfree(sr->out.sgcomp);
+	sr->out.sglist = NULL;
+	sr->out.buf = NULL;
+	sr->out.map_bufs_cnt = 0;
+}
+
+static void softreq_destroy(struct nitrox_softreq *sr)
+{
+	softreq_unmap_sgbufs(sr);
+	kfree(sr);
+}
+
+/**
+ * create_sg_component - create SG componets for N5 device.
+ * @sr: Request structure
+ * @sgtbl: SG table
+ * @nr_comp: total number of components required
+ *
+ * Component structure
+ *
+ *   63     48 47     32 31    16 15      0
+ *   --------------------------------------
+ *   |   LEN0  |  LEN1  |  LEN2  |  LEN3  |
+ *   |-------------------------------------
+ *   |               PTR0                 |
+ *   --------------------------------------
+ *   |               PTR1                 |
+ *   --------------------------------------
+ *   |               PTR2                 |
+ *   --------------------------------------
+ *   |               PTR3                 |
+ *   --------------------------------------
+ *
+ *   Returns 0 if success or a negative errno code on error.
+ */
+static int create_sg_component(struct nitrox_softreq *sr,
+			       struct nitrox_sgtable *sgtbl, int map_nents)
+{
+	struct nitrox_device *ndev = sr->ndev;
+	struct nitrox_sgcomp *sgcomp;
+	struct nitrox_sglist *sglist;
+	dma_addr_t dma;
+	size_t sz_comp;
+	int i, j, nr_sgcomp;
+
+	nr_sgcomp = roundup(map_nents, 4) / 4;
+
+	/* each component holds 4 dma pointers */
+	sz_comp = nr_sgcomp * sizeof(*sgcomp);
+	sgcomp = kzalloc(sz_comp, sr->gfp);
+	if (!sgcomp)
+		return -ENOMEM;
+
+	sgtbl->sgcomp = sgcomp;
+	sgtbl->nr_sgcomp = nr_sgcomp;
+
+	sglist = sgtbl->sglist;
+	/* populate device sg component */
+	for (i = 0; i < nr_sgcomp; i++) {
+		for (j = 0; j < 4; j++) {
+			sgcomp->len[j] = cpu_to_be16(sglist->len);
+			sgcomp->dma[j] = cpu_to_be64(sglist->dma);
+			sglist++;
+		}
+		sgcomp++;
+	}
+	/* map the device sg component */
+	dma = dma_map_single(DEV(ndev), sgtbl->sgcomp, sz_comp, DMA_TO_DEVICE);
+	if (dma_mapping_error(DEV(ndev), dma)) {
+		kfree(sgtbl->sgcomp);
+		sgtbl->sgcomp = NULL;
+		return -ENOMEM;
+	}
+
+	sgtbl->dma = dma;
+	sgtbl->len = sz_comp;
+
+	return 0;
+}
+
+/**
+ * dma_map_inbufs - DMA map input sglist and creates sglist component
+ *                  for N5 device.
+ * @sr: Request structure
+ * @req: Crypto request structre
+ *
+ * Returns 0 if successful or a negative errno code on error.
+ */
+static int dma_map_inbufs(struct nitrox_softreq *sr,
+			  struct se_crypto_request *req)
+{
+	struct device *dev = DEV(sr->ndev);
+	struct scatterlist *sg = req->src;
+	struct nitrox_sglist *glist;
+	int i, nents, ret = 0;
+	dma_addr_t dma;
+	size_t sz;
+
+	nents = sg_nents(req->src);
+
+	/* creater gather list IV and src entries */
+	sz = roundup((1 + nents), 4) * sizeof(*glist);
+	glist = kzalloc(sz, sr->gfp);
+	if (!glist)
+		return -ENOMEM;
+
+	sr->in.sglist = glist;
+	/* map IV */
+	dma = dma_map_single(dev, &req->iv, req->ivsize, DMA_BIDIRECTIONAL);
+	if (dma_mapping_error(dev, dma)) {
+		ret = -EINVAL;
+		goto iv_map_err;
+	}
+
+	sr->in.dir = (req->src == req->dst) ? DMA_BIDIRECTIONAL : DMA_TO_DEVICE;
+	/* map src entries */
+	nents = dma_map_sg(dev, req->src, nents, sr->in.dir);
+	if (!nents) {
+		ret = -EINVAL;
+		goto src_map_err;
+	}
+	sr->in.buf = req->src;
+
+	/* store the mappings */
+	glist->len = req->ivsize;
+	glist->dma = dma;
+	glist++;
+	sr->in.total_bytes += req->ivsize;
+
+	for_each_sg(req->src, sg, nents, i) {
+		glist->len = sg_dma_len(sg);
+		glist->dma = sg_dma_address(sg);
+		sr->in.total_bytes += glist->len;
+		glist++;
+	}
+	/* roundup map count to align with entires in sg component */
+	sr->in.map_bufs_cnt = (1 + nents);
+
+	/* create NITROX gather component */
+	ret = create_sg_component(sr, &sr->in, sr->in.map_bufs_cnt);
+	if (ret)
+		goto incomp_err;
+
+	return 0;
+
+incomp_err:
+	dma_unmap_sg(dev, req->src, nents, sr->in.dir);
+	sr->in.map_bufs_cnt = 0;
+src_map_err:
+	dma_unmap_single(dev, dma, req->ivsize, DMA_BIDIRECTIONAL);
+iv_map_err:
+	kfree(sr->in.sglist);
+	sr->in.sglist = NULL;
+	return ret;
+}
+
+static int dma_map_outbufs(struct nitrox_softreq *sr,
+			   struct se_crypto_request *req)
+{
+	struct device *dev = DEV(sr->ndev);
+	struct nitrox_sglist *glist = sr->in.sglist;
+	struct nitrox_sglist *slist;
+	struct scatterlist *sg;
+	int i, nents, map_bufs_cnt, ret = 0;
+	size_t sz;
+
+	nents = sg_nents(req->dst);
+
+	/* create scatter list ORH, IV, dst entries and Completion header */
+	sz = roundup((3 + nents), 4) * sizeof(*slist);
+	slist = kzalloc(sz, sr->gfp);
+	if (!slist)
+		return -ENOMEM;
+
+	sr->out.sglist = slist;
+	sr->out.dir = DMA_BIDIRECTIONAL;
+	/* map ORH */
+	sr->resp.orh_dma = dma_map_single(dev, &sr->resp.orh, ORH_HLEN,
+					  sr->out.dir);
+	if (dma_mapping_error(dev, sr->resp.orh_dma)) {
+		ret = -EINVAL;
+		goto orh_map_err;
+	}
+
+	/* map completion */
+	sr->resp.completion_dma = dma_map_single(dev, &sr->resp.completion,
+						 COMP_HLEN, sr->out.dir);
+	if (dma_mapping_error(dev, sr->resp.completion_dma)) {
+		ret = -EINVAL;
+		goto compl_map_err;
+	}
+
+	sr->inplace = (req->src == req->dst) ? true : false;
+	/* out place */
+	if (!sr->inplace) {
+		nents = dma_map_sg(dev, req->dst, nents, sr->out.dir);
+		if (!nents) {
+			ret = -EINVAL;
+			goto dst_map_err;
+		}
+	}
+	sr->out.buf = req->dst;
+
+	/* store the mappings */
+	/* orh */
+	slist->len = ORH_HLEN;
+	slist->dma = sr->resp.orh_dma;
+	slist++;
+
+	/* copy the glist mappings */
+	if (sr->inplace) {
+		nents = sr->in.map_bufs_cnt - 1;
+		map_bufs_cnt = sr->in.map_bufs_cnt;
+		while (map_bufs_cnt--) {
+			slist->len = glist->len;
+			slist->dma = glist->dma;
+			slist++;
+			glist++;
+		}
+	} else {
+		/* copy iv mapping */
+		slist->len = glist->len;
+		slist->dma = glist->dma;
+		slist++;
+		/* copy remaining maps */
+		for_each_sg(req->dst, sg, nents, i) {
+			slist->len = sg_dma_len(sg);
+			slist->dma = sg_dma_address(sg);
+			slist++;
+		}
+	}
+
+	/* completion */
+	slist->len = COMP_HLEN;
+	slist->dma = sr->resp.completion_dma;
+
+	sr->out.map_bufs_cnt = (3 + nents);
+
+	ret = create_sg_component(sr, &sr->out, sr->out.map_bufs_cnt);
+	if (ret)
+		goto outcomp_map_err;
+
+	return 0;
+
+outcomp_map_err:
+	if (!sr->inplace)
+		dma_unmap_sg(dev, req->dst, nents, sr->out.dir);
+	sr->out.map_bufs_cnt = 0;
+	sr->out.buf = NULL;
+dst_map_err:
+	dma_unmap_single(dev, sr->resp.completion_dma, COMP_HLEN, sr->out.dir);
+	sr->resp.completion_dma = 0;
+compl_map_err:
+	dma_unmap_single(dev, sr->resp.orh_dma, ORH_HLEN, sr->out.dir);
+	sr->resp.orh_dma = 0;
+orh_map_err:
+	kfree(sr->out.sglist);
+	sr->out.sglist = NULL;
+	return ret;
+}
+
+static inline int softreq_map_iobuf(struct nitrox_softreq *sr,
+				    struct se_crypto_request *creq)
+{
+	int ret;
+
+	ret = dma_map_inbufs(sr, creq);
+	if (ret)
+		return ret;
+
+	ret = dma_map_outbufs(sr, creq);
+	if (ret)
+		softreq_unmap_sgbufs(sr);
+
+	return ret;
+}
+
+static inline void backlog_list_add(struct nitrox_softreq *sr,
+				    struct nitrox_cmdq *cmdq)
+{
+	INIT_LIST_HEAD(&sr->backlog);
+
+	spin_lock_bh(&cmdq->backlog_lock);
+	list_add_tail(&sr->backlog, &cmdq->backlog_head);
+	atomic_inc(&cmdq->backlog_count);
+	atomic_set(&sr->status, REQ_BACKLOG);
+	spin_unlock_bh(&cmdq->backlog_lock);
+}
+
+static inline void response_list_add(struct nitrox_softreq *sr,
+				     struct nitrox_cmdq *cmdq)
+{
+	INIT_LIST_HEAD(&sr->response);
+
+	spin_lock_bh(&cmdq->response_lock);
+	list_add_tail(&sr->response, &cmdq->response_head);
+	spin_unlock_bh(&cmdq->response_lock);
+}
+
+static inline void response_list_del(struct nitrox_softreq *sr,
+				     struct nitrox_cmdq *cmdq)
+{
+	spin_lock_bh(&cmdq->response_lock);
+	list_del(&sr->response);
+	spin_unlock_bh(&cmdq->response_lock);
+}
+
+static struct nitrox_softreq *
+get_first_response_entry(struct nitrox_cmdq *cmdq)
+{
+	return list_first_entry_or_null(&cmdq->response_head,
+					struct nitrox_softreq, response);
+}
+
+static inline bool cmdq_full(struct nitrox_cmdq *cmdq, int qlen)
+{
+	if (atomic_inc_return(&cmdq->pending_count) > qlen) {
+		atomic_dec(&cmdq->pending_count);
+		/* sync with other cpus */
+		smp_mb__after_atomic();
+		return true;
+	}
+	return false;
+}
+
+/**
+ * post_se_instr - Post SE instruction to Packet Input ring
+ * @sr: Request structure
+ *
+ * Returns 0 if successful or a negative error code,
+ * if no space in ring.
+ */
+static void post_se_instr(struct nitrox_softreq *sr,
+			  struct nitrox_cmdq *cmdq)
+{
+	struct nitrox_device *ndev = sr->ndev;
+	union nps_pkt_in_instr_baoff_dbell pkt_in_baoff_dbell;
+	u64 offset;
+	u8 *ent;
+
+	spin_lock_bh(&cmdq->cmdq_lock);
+
+	/* get the next write offset */
+	offset = NPS_PKT_IN_INSTR_BAOFF_DBELLX(cmdq->qno);
+	pkt_in_baoff_dbell.value = nitrox_read_csr(ndev, offset);
+	/* copy the instruction */
+	ent = cmdq->head + pkt_in_baoff_dbell.s.aoff;
+	memcpy(ent, &sr->instr, cmdq->instr_size);
+	/* flush the command queue updates */
+	dma_wmb();
+
+	sr->tstamp = jiffies;
+	atomic_set(&sr->status, REQ_POSTED);
+	response_list_add(sr, cmdq);
+
+	/* Ring doorbell with count 1 */
+	writeq(1, cmdq->dbell_csr_addr);
+	/* orders the doorbell rings */
+	mmiowb();
+
+	spin_unlock_bh(&cmdq->cmdq_lock);
+}
+
+static int post_backlog_cmds(struct nitrox_cmdq *cmdq)
+{
+	struct nitrox_device *ndev = cmdq->ndev;
+	struct nitrox_softreq *sr, *tmp;
+	int ret = 0;
+
+	spin_lock_bh(&cmdq->backlog_lock);
+
+	list_for_each_entry_safe(sr, tmp, &cmdq->backlog_head, backlog) {
+		struct skcipher_request *skreq;
+
+		/* submit until space available */
+		if (unlikely(cmdq_full(cmdq, ndev->qlen))) {
+			ret = -EBUSY;
+			break;
+		}
+		/* delete from backlog list */
+		list_del(&sr->backlog);
+		atomic_dec(&cmdq->backlog_count);
+		/* sync with other cpus */
+		smp_mb__after_atomic();
+
+		skreq = sr->skreq;
+		/* post the command */
+		post_se_instr(sr, cmdq);
+
+		/* backlog requests are posted, wakeup with -EINPROGRESS */
+		skcipher_request_complete(skreq, -EINPROGRESS);
+	}
+	spin_unlock_bh(&cmdq->backlog_lock);
+
+	return ret;
+}
+
+static int nitrox_enqueue_request(struct nitrox_softreq *sr)
+{
+	struct nitrox_cmdq *cmdq = sr->cmdq;
+	struct nitrox_device *ndev = sr->ndev;
+	int ret = -EBUSY;
+
+	if (unlikely(cmdq_full(cmdq, ndev->qlen))) {
+		if (!(sr->flags & CRYPTO_TFM_REQ_MAY_BACKLOG))
+			return -EAGAIN;
+
+		backlog_list_add(sr, cmdq);
+	} else {
+		ret = post_backlog_cmds(cmdq);
+		if (ret) {
+			backlog_list_add(sr, cmdq);
+			return ret;
+		}
+		post_se_instr(sr, cmdq);
+		ret = -EINPROGRESS;
+	}
+	return ret;
+}
+
+/**
+ * nitrox_se_request - Send request to SE core
+ * @ndev: NITROX device
+ * @req: Crypto request
+ *
+ * Returns 0 on success, or a negative error code.
+ */
+int nitrox_process_se_request(struct nitrox_device *ndev,
+			      struct se_crypto_request *req,
+			      completion_t callback,
+			      struct skcipher_request *skreq)
+{
+	struct nitrox_softreq *sr;
+	dma_addr_t ctx_handle = 0;
+	int qno, ret = 0;
+
+	if (!nitrox_ready(ndev))
+		return -ENODEV;
+
+	sr = kzalloc(sizeof(*sr), req->gfp);
+	if (!sr)
+		return -ENOMEM;
+
+	sr->ndev = ndev;
+	sr->flags = req->flags;
+	sr->gfp = req->gfp;
+	sr->callback = callback;
+	sr->skreq = skreq;
+
+	atomic_set(&sr->status, REQ_NOT_POSTED);
+
+	WRITE_ONCE(sr->resp.orh, PENDING_SIG);
+	WRITE_ONCE(sr->resp.completion, PENDING_SIG);
+
+	ret = softreq_map_iobuf(sr, req);
+	if (ret) {
+		kfree(sr);
+		return ret;
+	}
+
+	/* get the context handle */
+	if (req->ctx_handle) {
+		struct ctx_hdr *hdr;
+		u8 *ctx_ptr;
+
+		ctx_ptr = (u8 *)(uintptr_t)req->ctx_handle;
+		hdr = (struct ctx_hdr *)(ctx_ptr - sizeof(struct ctx_hdr));
+		ctx_handle = hdr->ctx_dma;
+	}
+
+	/* select the queue */
+	qno = smp_processor_id() % ndev->nr_queues;
+
+	sr->cmdq = &ndev->pkt_cmdqs[qno];
+
+	/*
+	 * 64-Byte Instruction Format
+	 *
+	 *  ----------------------
+	 *  |      DPTR0         | 8 bytes
+	 *  ----------------------
+	 *  |  PKT_IN_INSTR_HDR  | 8 bytes
+	 *  ----------------------
+	 *  |    PKT_IN_HDR      | 16 bytes
+	 *  ----------------------
+	 *  |    SLC_INFO        | 16 bytes
+	 *  ----------------------
+	 *  |   Front data       | 16 bytes
+	 *  ----------------------
+	 */
+
+	/* fill the packet instruction */
+	/* word 0 */
+	sr->instr.dptr0 = cpu_to_be64(sr->in.dma);
+
+	/* word 1 */
+	sr->instr.ih.value = 0;
+	sr->instr.ih.s.g = 1;
+	sr->instr.ih.s.gsz = sr->in.map_bufs_cnt;
+	sr->instr.ih.s.ssz = sr->out.map_bufs_cnt;
+	sr->instr.ih.s.fsz = FDATA_SIZE + sizeof(struct gphdr);
+	sr->instr.ih.s.tlen = sr->instr.ih.s.fsz + sr->in.total_bytes;
+	sr->instr.ih.value = cpu_to_be64(sr->instr.ih.value);
+
+	/* word 2 */
+	sr->instr.irh.value[0] = 0;
+	sr->instr.irh.s.uddl = MIN_UDD_LEN;
+	/* context length in 64-bit words */
+	sr->instr.irh.s.ctxl = (req->ctrl.s.ctxl / 8);
+	/* offset from solicit base port 256 */
+	sr->instr.irh.s.destport = SOLICIT_BASE_DPORT + qno;
+	sr->instr.irh.s.ctxc = req->ctrl.s.ctxc;
+	sr->instr.irh.s.arg = req->ctrl.s.arg;
+	sr->instr.irh.s.opcode = req->opcode;
+	sr->instr.irh.value[0] = cpu_to_be64(sr->instr.irh.value[0]);
+
+	/* word 3 */
+	sr->instr.irh.s.ctxp = cpu_to_be64(ctx_handle);
+
+	/* word 4 */
+	sr->instr.slc.value[0] = 0;
+	sr->instr.slc.s.ssz = sr->out.map_bufs_cnt;
+	sr->instr.slc.value[0] = cpu_to_be64(sr->instr.slc.value[0]);
+
+	/* word 5 */
+	sr->instr.slc.s.rptr = cpu_to_be64(sr->out.dma);
+
+	/*
+	 * No conversion for front data,
+	 * It goes into payload
+	 * put GP Header in front data
+	 */
+	sr->instr.fdata[0] = *((u64 *)&req->gph);
+	sr->instr.fdata[1] = 0;
+	/* flush the soft_req changes before posting the cmd */
+	wmb();
+
+	ret = nitrox_enqueue_request(sr);
+	if (ret == -EAGAIN)
+		goto send_fail;
+
+	return ret;
+
+send_fail:
+	softreq_destroy(sr);
+	return ret;
+}
+
+static inline int cmd_timeout(unsigned long tstamp, unsigned long timeout)
+{
+	return time_after_eq(jiffies, (tstamp + timeout));
+}
+
+void backlog_qflush_work(struct work_struct *work)
+{
+	struct nitrox_cmdq *cmdq;
+
+	cmdq = container_of(work, struct nitrox_cmdq, backlog_qflush);
+	post_backlog_cmds(cmdq);
+}
+
+/**
+ * process_request_list - process completed requests
+ * @ndev: N5 device
+ * @qno: queue to operate
+ *
+ * Returns the number of responses processed.
+ */
+static void process_response_list(struct nitrox_cmdq *cmdq)
+{
+	struct nitrox_device *ndev = cmdq->ndev;
+	struct nitrox_softreq *sr;
+	struct skcipher_request *skreq;
+	completion_t callback;
+	int req_completed = 0, err = 0, budget;
+
+	/* check all pending requests */
+	budget = atomic_read(&cmdq->pending_count);
+
+	while (req_completed < budget) {
+		sr = get_first_response_entry(cmdq);
+		if (!sr)
+			break;
+
+		if (atomic_read(&sr->status) != REQ_POSTED)
+			break;
+
+		/* check orh and completion bytes updates */
+		if (READ_ONCE(sr->resp.orh) == READ_ONCE(sr->resp.completion)) {
+			/* request not completed, check for timeout */
+			if (!cmd_timeout(sr->tstamp, ndev->timeout))
+				break;
+			dev_err_ratelimited(DEV(ndev),
+					    "Request timeout, orh 0x%016llx\n",
+					    READ_ONCE(sr->resp.orh));
+		}
+		atomic_dec(&cmdq->pending_count);
+		/* sync with other cpus */
+		smp_mb__after_atomic();
+		/* remove from response list */
+		response_list_del(sr, cmdq);
+
+		callback = sr->callback;
+		skreq = sr->skreq;
+
+		/* ORH error code */
+		err = READ_ONCE(sr->resp.orh) & 0xff;
+		softreq_destroy(sr);
+
+		if (callback)
+			callback(skreq, err);
+
+		req_completed++;
+	}
+}
+
+/**
+ * pkt_slc_resp_handler - post processing of SE responses
+ */
+void pkt_slc_resp_handler(unsigned long data)
+{
+	struct bh_data *bh = (void *)(uintptr_t)(data);
+	struct nitrox_cmdq *cmdq = bh->cmdq;
+	union nps_pkt_slc_cnts pkt_slc_cnts;
+
+	/* read completion count */
+	pkt_slc_cnts.value = readq(bh->completion_cnt_csr_addr);
+	/* resend the interrupt if more work to do */
+	pkt_slc_cnts.s.resend = 1;
+
+	process_response_list(cmdq);
+
+	/*
+	 * clear the interrupt with resend bit enabled,
+	 * MSI-X interrupt generates if Completion count > Threshold
+	 */
+	writeq(pkt_slc_cnts.value, bh->completion_cnt_csr_addr);
+	/* order the writes */
+	mmiowb();
+
+	if (atomic_read(&cmdq->backlog_count))
+		schedule_work(&cmdq->backlog_qflush);
+}
diff --git a/drivers/crypto/ccp/Makefile b/drivers/crypto/ccp/Makefile
index 60919a3ec53b..59493fd3a751 100644
--- a/drivers/crypto/ccp/Makefile
+++ b/drivers/crypto/ccp/Makefile
@@ -4,7 +4,8 @@ ccp-objs := ccp-dev.o \
 	    ccp-dev-v3.o \
 	    ccp-dev-v5.o \
 	    ccp-platform.o \
-	    ccp-dmaengine.o
+	    ccp-dmaengine.o \
+	    ccp-debugfs.o
 ccp-$(CONFIG_PCI) += ccp-pci.o
 
 obj-$(CONFIG_CRYPTO_DEV_CCP_CRYPTO) += ccp-crypto.o
diff --git a/drivers/crypto/ccp/ccp-crypto-sha.c b/drivers/crypto/ccp/ccp-crypto-sha.c
index 6b46eea94932..ce97b3868f4a 100644
--- a/drivers/crypto/ccp/ccp-crypto-sha.c
+++ b/drivers/crypto/ccp/ccp-crypto-sha.c
@@ -18,6 +18,7 @@
 #include <linux/crypto.h>
 #include <crypto/algapi.h>
 #include <crypto/hash.h>
+#include <crypto/hmac.h>
 #include <crypto/internal/hash.h>
 #include <crypto/sha.h>
 #include <crypto/scatterwalk.h>
@@ -308,8 +309,8 @@ static int ccp_sha_setkey(struct crypto_ahash *tfm, const u8 *key,
 	}
 
 	for (i = 0; i < block_size; i++) {
-		ctx->u.sha.ipad[i] = ctx->u.sha.key[i] ^ 0x36;
-		ctx->u.sha.opad[i] = ctx->u.sha.key[i] ^ 0x5c;
+		ctx->u.sha.ipad[i] = ctx->u.sha.key[i] ^ HMAC_IPAD_VALUE;
+		ctx->u.sha.opad[i] = ctx->u.sha.key[i] ^ HMAC_OPAD_VALUE;
 	}
 
 	sg_init_one(&ctx->u.sha.opad_sg, ctx->u.sha.opad, block_size);
diff --git a/drivers/crypto/ccp/ccp-debugfs.c b/drivers/crypto/ccp/ccp-debugfs.c
new file mode 100644
index 000000000000..3cd6c83754e0
--- /dev/null
+++ b/drivers/crypto/ccp/ccp-debugfs.c
@@ -0,0 +1,344 @@
+/*
+ * AMD Cryptographic Coprocessor (CCP) driver
+ *
+ * Copyright (C) 2017 Advanced Micro Devices, Inc.
+ *
+ * Author: Gary R Hook <gary.hook@amd.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/debugfs.h>
+#include <linux/ccp.h>
+
+#include "ccp-dev.h"
+
+/* DebugFS helpers */
+#define	OBUFP		(obuf + oboff)
+#define	OBUFLEN		512
+#define	OBUFSPC		(OBUFLEN - oboff)
+#define	OSCNPRINTF(fmt, ...) \
+		scnprintf(OBUFP, OBUFSPC, fmt, ## __VA_ARGS__)
+
+#define BUFLEN	63
+
+#define	RI_VERSION_NUM	0x0000003F
+#define	RI_AES_PRESENT	0x00000040
+#define	RI_3DES_PRESENT	0x00000080
+#define	RI_SHA_PRESENT	0x00000100
+#define	RI_RSA_PRESENT	0x00000200
+#define	RI_ECC_PRESENT	0x00000400
+#define	RI_ZDE_PRESENT	0x00000800
+#define	RI_ZCE_PRESENT	0x00001000
+#define	RI_TRNG_PRESENT	0x00002000
+#define	RI_ELFC_PRESENT	0x00004000
+#define	RI_ELFC_SHIFT	14
+#define	RI_NUM_VQM	0x00078000
+#define	RI_NVQM_SHIFT	15
+#define	RI_NVQM(r)	(((r) * RI_NUM_VQM) >> RI_NVQM_SHIFT)
+#define	RI_LSB_ENTRIES	0x0FF80000
+#define	RI_NLSB_SHIFT	19
+#define	RI_NLSB(r)	(((r) * RI_LSB_ENTRIES) >> RI_NLSB_SHIFT)
+
+static ssize_t ccp5_debugfs_info_read(struct file *filp, char __user *ubuf,
+				      size_t count, loff_t *offp)
+{
+	struct ccp_device *ccp = filp->private_data;
+	unsigned int oboff = 0;
+	unsigned int regval;
+	ssize_t ret;
+	char *obuf;
+
+	if (!ccp)
+		return 0;
+
+	obuf = kmalloc(OBUFLEN, GFP_KERNEL);
+	if (!obuf)
+		return -ENOMEM;
+
+	oboff += OSCNPRINTF("Device name: %s\n", ccp->name);
+	oboff += OSCNPRINTF("   RNG name: %s\n", ccp->rngname);
+	oboff += OSCNPRINTF("   # Queues: %d\n", ccp->cmd_q_count);
+	oboff += OSCNPRINTF("     # Cmds: %d\n", ccp->cmd_count);
+
+	regval = ioread32(ccp->io_regs + CMD5_PSP_CCP_VERSION);
+	oboff += OSCNPRINTF("    Version: %d\n", regval & RI_VERSION_NUM);
+	oboff += OSCNPRINTF("    Engines:");
+	if (regval & RI_AES_PRESENT)
+		oboff += OSCNPRINTF(" AES");
+	if (regval & RI_3DES_PRESENT)
+		oboff += OSCNPRINTF(" 3DES");
+	if (regval & RI_SHA_PRESENT)
+		oboff += OSCNPRINTF(" SHA");
+	if (regval & RI_RSA_PRESENT)
+		oboff += OSCNPRINTF(" RSA");
+	if (regval & RI_ECC_PRESENT)
+		oboff += OSCNPRINTF(" ECC");
+	if (regval & RI_ZDE_PRESENT)
+		oboff += OSCNPRINTF(" ZDE");
+	if (regval & RI_ZCE_PRESENT)
+		oboff += OSCNPRINTF(" ZCE");
+	if (regval & RI_TRNG_PRESENT)
+		oboff += OSCNPRINTF(" TRNG");
+	oboff += OSCNPRINTF("\n");
+	oboff += OSCNPRINTF("     Queues: %d\n",
+		   (regval & RI_NUM_VQM) >> RI_NVQM_SHIFT);
+	oboff += OSCNPRINTF("LSB Entries: %d\n",
+		   (regval & RI_LSB_ENTRIES) >> RI_NLSB_SHIFT);
+
+	ret = simple_read_from_buffer(ubuf, count, offp, obuf, oboff);
+	kfree(obuf);
+
+	return ret;
+}
+
+/* Return a formatted buffer containing the current
+ * statistics across all queues for a CCP.
+ */
+static ssize_t ccp5_debugfs_stats_read(struct file *filp, char __user *ubuf,
+				       size_t count, loff_t *offp)
+{
+	struct ccp_device *ccp = filp->private_data;
+	unsigned long total_xts_aes_ops = 0;
+	unsigned long total_3des_ops = 0;
+	unsigned long total_aes_ops = 0;
+	unsigned long total_sha_ops = 0;
+	unsigned long total_rsa_ops = 0;
+	unsigned long total_ecc_ops = 0;
+	unsigned long total_pt_ops = 0;
+	unsigned long total_ops = 0;
+	unsigned int oboff = 0;
+	ssize_t ret = 0;
+	unsigned int i;
+	char *obuf;
+
+	for (i = 0; i < ccp->cmd_q_count; i++) {
+		struct ccp_cmd_queue *cmd_q = &ccp->cmd_q[i];
+
+		total_ops += cmd_q->total_ops;
+		total_aes_ops += cmd_q->total_aes_ops;
+		total_xts_aes_ops += cmd_q->total_xts_aes_ops;
+		total_3des_ops += cmd_q->total_3des_ops;
+		total_sha_ops += cmd_q->total_sha_ops;
+		total_rsa_ops += cmd_q->total_rsa_ops;
+		total_pt_ops += cmd_q->total_pt_ops;
+		total_ecc_ops += cmd_q->total_ecc_ops;
+	}
+
+	obuf = kmalloc(OBUFLEN, GFP_KERNEL);
+	if (!obuf)
+		return -ENOMEM;
+
+	oboff += OSCNPRINTF("Total Interrupts Handled: %ld\n",
+			    ccp->total_interrupts);
+	oboff += OSCNPRINTF("        Total Operations: %ld\n",
+			    total_ops);
+	oboff += OSCNPRINTF("                     AES: %ld\n",
+			    total_aes_ops);
+	oboff += OSCNPRINTF("                 XTS AES: %ld\n",
+			    total_xts_aes_ops);
+	oboff += OSCNPRINTF("                     SHA: %ld\n",
+			    total_3des_ops);
+	oboff += OSCNPRINTF("                     SHA: %ld\n",
+			    total_sha_ops);
+	oboff += OSCNPRINTF("                     RSA: %ld\n",
+			    total_rsa_ops);
+	oboff += OSCNPRINTF("               Pass-Thru: %ld\n",
+			    total_pt_ops);
+	oboff += OSCNPRINTF("                     ECC: %ld\n",
+			    total_ecc_ops);
+
+	ret = simple_read_from_buffer(ubuf, count, offp, obuf, oboff);
+	kfree(obuf);
+
+	return ret;
+}
+
+/* Reset the counters in a queue
+ */
+static void ccp5_debugfs_reset_queue_stats(struct ccp_cmd_queue *cmd_q)
+{
+	cmd_q->total_ops = 0L;
+	cmd_q->total_aes_ops = 0L;
+	cmd_q->total_xts_aes_ops = 0L;
+	cmd_q->total_3des_ops = 0L;
+	cmd_q->total_sha_ops = 0L;
+	cmd_q->total_rsa_ops = 0L;
+	cmd_q->total_pt_ops = 0L;
+	cmd_q->total_ecc_ops = 0L;
+}
+
+/* A value was written to the stats variable, which
+ * should be used to reset the queue counters across
+ * that device.
+ */
+static ssize_t ccp5_debugfs_stats_write(struct file *filp,
+					const char __user *ubuf,
+					size_t count, loff_t *offp)
+{
+	struct ccp_device *ccp = filp->private_data;
+	int i;
+
+	for (i = 0; i < ccp->cmd_q_count; i++)
+		ccp5_debugfs_reset_queue_stats(&ccp->cmd_q[i]);
+	ccp->total_interrupts = 0L;
+
+	return count;
+}
+
+/* Return a formatted buffer containing the current information
+ * for that queue
+ */
+static ssize_t ccp5_debugfs_queue_read(struct file *filp, char __user *ubuf,
+				       size_t count, loff_t *offp)
+{
+	struct ccp_cmd_queue *cmd_q = filp->private_data;
+	unsigned int oboff = 0;
+	unsigned int regval;
+	ssize_t ret;
+	char *obuf;
+
+	if (!cmd_q)
+		return 0;
+
+	obuf = kmalloc(OBUFLEN, GFP_KERNEL);
+	if (!obuf)
+		return -ENOMEM;
+
+	oboff += OSCNPRINTF("  Total Queue Operations: %ld\n",
+			    cmd_q->total_ops);
+	oboff += OSCNPRINTF("                     AES: %ld\n",
+			    cmd_q->total_aes_ops);
+	oboff += OSCNPRINTF("                 XTS AES: %ld\n",
+			    cmd_q->total_xts_aes_ops);
+	oboff += OSCNPRINTF("                     SHA: %ld\n",
+			    cmd_q->total_3des_ops);
+	oboff += OSCNPRINTF("                     SHA: %ld\n",
+			    cmd_q->total_sha_ops);
+	oboff += OSCNPRINTF("                     RSA: %ld\n",
+			    cmd_q->total_rsa_ops);
+	oboff += OSCNPRINTF("               Pass-Thru: %ld\n",
+			    cmd_q->total_pt_ops);
+	oboff += OSCNPRINTF("                     ECC: %ld\n",
+			    cmd_q->total_ecc_ops);
+
+	regval = ioread32(cmd_q->reg_int_enable);
+	oboff += OSCNPRINTF("      Enabled Interrupts:");
+	if (regval & INT_EMPTY_QUEUE)
+		oboff += OSCNPRINTF(" EMPTY");
+	if (regval & INT_QUEUE_STOPPED)
+		oboff += OSCNPRINTF(" STOPPED");
+	if (regval & INT_ERROR)
+		oboff += OSCNPRINTF(" ERROR");
+	if (regval & INT_COMPLETION)
+		oboff += OSCNPRINTF(" COMPLETION");
+	oboff += OSCNPRINTF("\n");
+
+	ret = simple_read_from_buffer(ubuf, count, offp, obuf, oboff);
+	kfree(obuf);
+
+	return ret;
+}
+
+/* A value was written to the stats variable for a
+ * queue. Reset the queue counters to this value.
+ */
+static ssize_t ccp5_debugfs_queue_write(struct file *filp,
+					const char __user *ubuf,
+					size_t count, loff_t *offp)
+{
+	struct ccp_cmd_queue *cmd_q = filp->private_data;
+
+	ccp5_debugfs_reset_queue_stats(cmd_q);
+
+	return count;
+}
+
+static const struct file_operations ccp_debugfs_info_ops = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = ccp5_debugfs_info_read,
+	.write = NULL,
+};
+
+static const struct file_operations ccp_debugfs_queue_ops = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = ccp5_debugfs_queue_read,
+	.write = ccp5_debugfs_queue_write,
+};
+
+static const struct file_operations ccp_debugfs_stats_ops = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = ccp5_debugfs_stats_read,
+	.write = ccp5_debugfs_stats_write,
+};
+
+static struct dentry *ccp_debugfs_dir;
+static DEFINE_RWLOCK(ccp_debugfs_lock);
+
+#define	MAX_NAME_LEN	20
+
+void ccp5_debugfs_setup(struct ccp_device *ccp)
+{
+	struct ccp_cmd_queue *cmd_q;
+	char name[MAX_NAME_LEN + 1];
+	struct dentry *debugfs_info;
+	struct dentry *debugfs_stats;
+	struct dentry *debugfs_q_instance;
+	struct dentry *debugfs_q_stats;
+	unsigned long flags;
+	int i;
+
+	if (!debugfs_initialized())
+		return;
+
+	write_lock_irqsave(&ccp_debugfs_lock, flags);
+	if (!ccp_debugfs_dir)
+		ccp_debugfs_dir = debugfs_create_dir(KBUILD_MODNAME, NULL);
+	write_unlock_irqrestore(&ccp_debugfs_lock, flags);
+	if (!ccp_debugfs_dir)
+		return;
+
+	ccp->debugfs_instance = debugfs_create_dir(ccp->name, ccp_debugfs_dir);
+	if (!ccp->debugfs_instance)
+		return;
+
+	debugfs_info = debugfs_create_file("info", 0400,
+					   ccp->debugfs_instance, ccp,
+					   &ccp_debugfs_info_ops);
+	if (!debugfs_info)
+		return;
+
+	debugfs_stats = debugfs_create_file("stats", 0600,
+					    ccp->debugfs_instance, ccp,
+					    &ccp_debugfs_stats_ops);
+	if (!debugfs_stats)
+		return;
+
+	for (i = 0; i < ccp->cmd_q_count; i++) {
+		cmd_q = &ccp->cmd_q[i];
+
+		snprintf(name, MAX_NAME_LEN - 1, "q%d", cmd_q->id);
+
+		debugfs_q_instance =
+			debugfs_create_dir(name, ccp->debugfs_instance);
+		if (!debugfs_q_instance)
+			return;
+
+		debugfs_q_stats =
+			debugfs_create_file("stats", 0600,
+					    debugfs_q_instance, cmd_q,
+					    &ccp_debugfs_queue_ops);
+		if (!debugfs_q_stats)
+			return;
+	}
+}
+
+void ccp5_debugfs_destroy(void)
+{
+	debugfs_remove_recursive(ccp_debugfs_dir);
+}
diff --git a/drivers/crypto/ccp/ccp-dev-v5.c b/drivers/crypto/ccp/ccp-dev-v5.c
index ccbe32d5dd1c..b10d2d2075cb 100644
--- a/drivers/crypto/ccp/ccp-dev-v5.c
+++ b/drivers/crypto/ccp/ccp-dev-v5.c
@@ -14,6 +14,7 @@
 #include <linux/kernel.h>
 #include <linux/pci.h>
 #include <linux/kthread.h>
+#include <linux/debugfs.h>
 #include <linux/dma-mapping.h>
 #include <linux/interrupt.h>
 #include <linux/compiler.h>
@@ -231,6 +232,8 @@ static int ccp5_do_cmd(struct ccp5_desc *desc,
 	int	i;
 	int ret = 0;
 
+	cmd_q->total_ops++;
+
 	if (CCP5_CMD_SOC(desc)) {
 		CCP5_CMD_IOC(desc) = 1;
 		CCP5_CMD_SOC(desc) = 0;
@@ -282,6 +285,8 @@ static int ccp5_perform_aes(struct ccp_op *op)
 	union ccp_function function;
 	u32 key_addr = op->sb_key * LSB_ITEM_SIZE;
 
+	op->cmd_q->total_aes_ops++;
+
 	/* Zero out all the fields of the command desc */
 	memset(&desc, 0, Q_DESC_SIZE);
 
@@ -325,6 +330,8 @@ static int ccp5_perform_xts_aes(struct ccp_op *op)
 	union ccp_function function;
 	u32 key_addr = op->sb_key * LSB_ITEM_SIZE;
 
+	op->cmd_q->total_xts_aes_ops++;
+
 	/* Zero out all the fields of the command desc */
 	memset(&desc, 0, Q_DESC_SIZE);
 
@@ -364,6 +371,8 @@ static int ccp5_perform_sha(struct ccp_op *op)
 	struct ccp5_desc desc;
 	union ccp_function function;
 
+	op->cmd_q->total_sha_ops++;
+
 	/* Zero out all the fields of the command desc */
 	memset(&desc, 0, Q_DESC_SIZE);
 
@@ -404,6 +413,8 @@ static int ccp5_perform_des3(struct ccp_op *op)
 	union ccp_function function;
 	u32 key_addr = op->sb_key * LSB_ITEM_SIZE;
 
+	op->cmd_q->total_3des_ops++;
+
 	/* Zero out all the fields of the command desc */
 	memset(&desc, 0, sizeof(struct ccp5_desc));
 
@@ -444,6 +455,8 @@ static int ccp5_perform_rsa(struct ccp_op *op)
 	struct ccp5_desc desc;
 	union ccp_function function;
 
+	op->cmd_q->total_rsa_ops++;
+
 	/* Zero out all the fields of the command desc */
 	memset(&desc, 0, Q_DESC_SIZE);
 
@@ -487,6 +500,8 @@ static int ccp5_perform_passthru(struct ccp_op *op)
 	struct ccp_dma_info *daddr = &op->dst.u.dma;
 
 
+	op->cmd_q->total_pt_ops++;
+
 	memset(&desc, 0, Q_DESC_SIZE);
 
 	CCP5_CMD_ENGINE(&desc) = CCP_ENGINE_PASSTHRU;
@@ -543,6 +558,8 @@ static int ccp5_perform_ecc(struct ccp_op *op)
 	struct ccp5_desc desc;
 	union ccp_function function;
 
+	op->cmd_q->total_ecc_ops++;
+
 	/* Zero out all the fields of the command desc */
 	memset(&desc, 0, Q_DESC_SIZE);
 
@@ -592,7 +609,6 @@ static int ccp_find_lsb_regions(struct ccp_cmd_queue *cmd_q, u64 status)
 	return queues ? 0 : -EINVAL;
 }
 
-
 static int ccp_find_and_assign_lsb_to_q(struct ccp_device *ccp,
 					int lsb_cnt, int n_lsbs,
 					unsigned long *lsb_pub)
@@ -757,6 +773,7 @@ static irqreturn_t ccp5_irq_handler(int irq, void *data)
 	struct ccp_device *ccp = dev_get_drvdata(dev);
 
 	ccp5_disable_queue_interrupts(ccp);
+	ccp->total_interrupts++;
 	if (ccp->use_tasklet)
 		tasklet_schedule(&ccp->irq_tasklet);
 	else
@@ -956,6 +973,9 @@ static int ccp5_init(struct ccp_device *ccp)
 	if (ret)
 		goto e_hwrng;
 
+	/* Set up debugfs entries */
+	ccp5_debugfs_setup(ccp);
+
 	return 0;
 
 e_hwrng:
@@ -992,6 +1012,12 @@ static void ccp5_destroy(struct ccp_device *ccp)
 	/* Remove this device from the list of available units first */
 	ccp_del_device(ccp);
 
+	/* We're in the process of tearing down the entire driver;
+	 * when all the devices are gone clean up debugfs
+	 */
+	if (ccp_present())
+		ccp5_debugfs_destroy();
+
 	/* Disable and clear interrupts */
 	ccp5_disable_queue_interrupts(ccp);
 	for (i = 0; i < ccp->cmd_q_count; i++) {
diff --git a/drivers/crypto/ccp/ccp-dev.c b/drivers/crypto/ccp/ccp-dev.c
index 92d1c6959f08..2506b5025700 100644
--- a/drivers/crypto/ccp/ccp-dev.c
+++ b/drivers/crypto/ccp/ccp-dev.c
@@ -31,8 +31,9 @@
 #include "ccp-dev.h"
 
 MODULE_AUTHOR("Tom Lendacky <thomas.lendacky@amd.com>");
+MODULE_AUTHOR("Gary R Hook <gary.hook@amd.com>");
 MODULE_LICENSE("GPL");
-MODULE_VERSION("1.0.0");
+MODULE_VERSION("1.1.0");
 MODULE_DESCRIPTION("AMD Cryptographic Coprocessor driver");
 
 struct ccp_tasklet_data {
diff --git a/drivers/crypto/ccp/ccp-dev.h b/drivers/crypto/ccp/ccp-dev.h
index 0cb09d0feeaf..a70154ac7405 100644
--- a/drivers/crypto/ccp/ccp-dev.h
+++ b/drivers/crypto/ccp/ccp-dev.h
@@ -70,6 +70,7 @@
 #define LSB_PUBLIC_MASK_HI_OFFSET	0x1C
 #define LSB_PRIVATE_MASK_LO_OFFSET	0x20
 #define LSB_PRIVATE_MASK_HI_OFFSET	0x24
+#define CMD5_PSP_CCP_VERSION		0x100
 
 #define CMD5_Q_CONTROL_BASE		0x0000
 #define CMD5_Q_TAIL_LO_BASE		0x0004
@@ -322,6 +323,16 @@ struct ccp_cmd_queue {
 	/* Interrupt wait queue */
 	wait_queue_head_t int_queue;
 	unsigned int int_rcvd;
+
+	/* Per-queue Statistics */
+	unsigned long total_ops;
+	unsigned long total_aes_ops;
+	unsigned long total_xts_aes_ops;
+	unsigned long total_3des_ops;
+	unsigned long total_sha_ops;
+	unsigned long total_rsa_ops;
+	unsigned long total_pt_ops;
+	unsigned long total_ecc_ops;
 } ____cacheline_aligned;
 
 struct ccp_device {
@@ -419,6 +430,12 @@ struct ccp_device {
 
 	/* DMA caching attribute support */
 	unsigned int axcache;
+
+	/* Device Statistics */
+	unsigned long total_interrupts;
+
+	/* DebugFS info */
+	struct dentry *debugfs_instance;
 };
 
 enum ccp_memtype {
@@ -632,6 +649,9 @@ void ccp_unregister_rng(struct ccp_device *ccp);
 int ccp_dmaengine_register(struct ccp_device *ccp);
 void ccp_dmaengine_unregister(struct ccp_device *ccp);
 
+void ccp5_debugfs_setup(struct ccp_device *ccp);
+void ccp5_debugfs_destroy(void);
+
 /* Structure for computation functions that are device-specific */
 struct ccp_actions {
 	int (*aes)(struct ccp_op *);
diff --git a/drivers/crypto/ccp/ccp-platform.c b/drivers/crypto/ccp/ccp-platform.c
index 351f28d8c336..e26969e601ad 100644
--- a/drivers/crypto/ccp/ccp-platform.c
+++ b/drivers/crypto/ccp/ccp-platform.c
@@ -44,7 +44,7 @@ static struct ccp_vdata *ccp_get_of_version(struct platform_device *pdev)
 	if (match && match->data)
 		return (struct ccp_vdata *)match->data;
 #endif
-	return 0;
+	return NULL;
 }
 
 static struct ccp_vdata *ccp_get_acpi_version(struct platform_device *pdev)
@@ -56,7 +56,7 @@ static struct ccp_vdata *ccp_get_acpi_version(struct platform_device *pdev)
 	if (match && match->driver_data)
 		return (struct ccp_vdata *)match->driver_data;
 #endif
-	return 0;
+	return NULL;
 }
 
 static int ccp_get_irq(struct ccp_device *ccp)
diff --git a/drivers/crypto/chelsio/chcr_algo.c b/drivers/crypto/chelsio/chcr_algo.c
index f00e0d8bd039..cfc723a10610 100644
--- a/drivers/crypto/chelsio/chcr_algo.c
+++ b/drivers/crypto/chelsio/chcr_algo.c
@@ -55,6 +55,8 @@
 #include <crypto/hash.h>
 #include <crypto/sha.h>
 #include <crypto/authenc.h>
+#include <crypto/ctr.h>
+#include <crypto/gf128mul.h>
 #include <crypto/internal/aead.h>
 #include <crypto/null.h>
 #include <crypto/internal/skcipher.h>
@@ -126,13 +128,13 @@ static void chcr_verify_tag(struct aead_request *req, u8 *input, int *err)
 	fw6_pld = (struct cpl_fw6_pld *)input;
 	if ((get_aead_subtype(tfm) == CRYPTO_ALG_SUB_TYPE_AEAD_RFC4106) ||
 	    (get_aead_subtype(tfm) == CRYPTO_ALG_SUB_TYPE_AEAD_GCM)) {
-		cmp = memcmp(&fw6_pld->data[2], (fw6_pld + 1), authsize);
+		cmp = crypto_memneq(&fw6_pld->data[2], (fw6_pld + 1), authsize);
 	} else {
 
 		sg_pcopy_to_buffer(req->src, sg_nents(req->src), temp,
 				authsize, req->assoclen +
 				req->cryptlen - authsize);
-		cmp = memcmp(temp, (fw6_pld + 1), authsize);
+		cmp = crypto_memneq(temp, (fw6_pld + 1), authsize);
 	}
 	if (cmp)
 		*err = -EBADMSG;
@@ -151,12 +153,12 @@ int chcr_handle_resp(struct crypto_async_request *req, unsigned char *input,
 	struct chcr_context *ctx = crypto_tfm_ctx(tfm);
 	struct uld_ctx *u_ctx = ULD_CTX(ctx);
 	struct chcr_req_ctx ctx_req;
-	struct cpl_fw6_pld *fw6_pld;
 	unsigned int digestsize, updated_digestsize;
+	struct adapter *adap = padap(ctx->dev);
 
 	switch (tfm->__crt_alg->cra_flags & CRYPTO_ALG_TYPE_MASK) {
 	case CRYPTO_ALG_TYPE_AEAD:
-		ctx_req.req.aead_req = (struct aead_request *)req;
+		ctx_req.req.aead_req = aead_request_cast(req);
 		ctx_req.ctx.reqctx = aead_request_ctx(ctx_req.req.aead_req);
 		dma_unmap_sg(&u_ctx->lldi.pdev->dev, ctx_req.ctx.reqctx->dst,
 			     ctx_req.ctx.reqctx->dst_nents, DMA_FROM_DEVICE);
@@ -164,32 +166,23 @@ int chcr_handle_resp(struct crypto_async_request *req, unsigned char *input,
 			kfree_skb(ctx_req.ctx.reqctx->skb);
 			ctx_req.ctx.reqctx->skb = NULL;
 		}
+		free_new_sg(ctx_req.ctx.reqctx->newdstsg);
+		ctx_req.ctx.reqctx->newdstsg = NULL;
 		if (ctx_req.ctx.reqctx->verify == VERIFY_SW) {
 			chcr_verify_tag(ctx_req.req.aead_req, input,
 					&err);
 			ctx_req.ctx.reqctx->verify = VERIFY_HW;
 		}
+		ctx_req.req.aead_req->base.complete(req, err);
 		break;
 
 	case CRYPTO_ALG_TYPE_ABLKCIPHER:
-		ctx_req.req.ablk_req = (struct ablkcipher_request *)req;
-		ctx_req.ctx.ablk_ctx =
-			ablkcipher_request_ctx(ctx_req.req.ablk_req);
-		if (!err) {
-			fw6_pld = (struct cpl_fw6_pld *)input;
-			memcpy(ctx_req.req.ablk_req->info, &fw6_pld->data[2],
-			       AES_BLOCK_SIZE);
-		}
-		dma_unmap_sg(&u_ctx->lldi.pdev->dev, ctx_req.req.ablk_req->dst,
-			     ctx_req.ctx.ablk_ctx->dst_nents, DMA_FROM_DEVICE);
-		if (ctx_req.ctx.ablk_ctx->skb) {
-			kfree_skb(ctx_req.ctx.ablk_ctx->skb);
-			ctx_req.ctx.ablk_ctx->skb = NULL;
-		}
+		 err = chcr_handle_cipher_resp(ablkcipher_request_cast(req),
+					       input, err);
 		break;
 
 	case CRYPTO_ALG_TYPE_AHASH:
-		ctx_req.req.ahash_req = (struct ahash_request *)req;
+		ctx_req.req.ahash_req = ahash_request_cast(req);
 		ctx_req.ctx.ahash_ctx =
 			ahash_request_ctx(ctx_req.req.ahash_req);
 		digestsize =
@@ -214,8 +207,10 @@ int chcr_handle_resp(struct crypto_async_request *req, unsigned char *input,
 			       sizeof(struct cpl_fw6_pld),
 			       updated_digestsize);
 		}
+		ctx_req.req.ahash_req->base.complete(req, err);
 		break;
 	}
+	atomic_inc(&adap->chcr_stats.complete);
 	return err;
 }
 
@@ -392,7 +387,7 @@ static void write_phys_cpl(struct cpl_rx_phys_dsgl *phys_cpl,
 			   struct phys_sge_parm *sg_param)
 {
 	struct phys_sge_pairs *to;
-	int out_buf_size = sg_param->obsize;
+	unsigned int len = 0, left_size = sg_param->obsize;
 	unsigned int nents = sg_param->nents, i, j = 0;
 
 	phys_cpl->op_to_tid = htonl(CPL_RX_PHYS_DSGL_OPCODE_V(CPL_RX_PHYS_DSGL)
@@ -409,20 +404,15 @@ static void write_phys_cpl(struct cpl_rx_phys_dsgl *phys_cpl,
 	phys_cpl->rss_hdr_int.hash_val = 0;
 	to = (struct phys_sge_pairs *)((unsigned char *)phys_cpl +
 				       sizeof(struct cpl_rx_phys_dsgl));
-
-	for (i = 0; nents; to++) {
-		for (j = 0; j < 8 && nents; j++, nents--) {
-			out_buf_size -= sg_dma_len(sg);
-			to->len[j] = htons(sg_dma_len(sg));
+	for (i = 0; nents && left_size; to++) {
+		for (j = 0; j < 8 && nents && left_size; j++, nents--) {
+			len = min(left_size, sg_dma_len(sg));
+			to->len[j] = htons(len);
 			to->addr[j] = cpu_to_be64(sg_dma_address(sg));
+			left_size -= len;
 			sg = sg_next(sg);
 		}
 	}
-	if (out_buf_size) {
-		j--;
-		to--;
-		to->len[j] = htons(ntohs(to->len[j]) + (out_buf_size));
-	}
 }
 
 static inline int map_writesg_phys_cpl(struct device *dev,
@@ -431,7 +421,7 @@ static inline int map_writesg_phys_cpl(struct device *dev,
 					struct phys_sge_parm *sg_param)
 {
 	if (!sg || !sg_param->nents)
-		return 0;
+		return -EINVAL;
 
 	sg_param->nents = dma_map_sg(dev, sg, sg_param->nents, DMA_FROM_DEVICE);
 	if (sg_param->nents == 0) {
@@ -498,6 +488,24 @@ write_sg_to_skb(struct sk_buff *skb, unsigned int *frags,
 	}
 }
 
+static int cxgb4_is_crypto_q_full(struct net_device *dev, unsigned int idx)
+{
+	struct adapter *adap = netdev2adap(dev);
+	struct sge_uld_txq_info *txq_info =
+		adap->sge.uld_txq_info[CXGB4_TX_CRYPTO];
+	struct sge_uld_txq *txq;
+	int ret = 0;
+
+	local_bh_disable();
+	txq = &txq_info->uldtxq[idx];
+	spin_lock(&txq->sendq.lock);
+	if (txq->full)
+		ret = -1;
+	spin_unlock(&txq->sendq.lock);
+	local_bh_enable();
+	return ret;
+}
+
 static int generate_copy_rrkey(struct ablk_ctx *ablkctx,
 			       struct _key_ctx *key_ctx)
 {
@@ -512,13 +520,67 @@ static int generate_copy_rrkey(struct ablk_ctx *ablkctx,
 	}
 	return 0;
 }
+static int chcr_sg_ent_in_wr(struct scatterlist *src,
+			     struct scatterlist *dst,
+			     unsigned int minsg,
+			     unsigned int space,
+			     short int *sent,
+			     short int *dent)
+{
+	int srclen = 0, dstlen = 0;
+	int srcsg = minsg, dstsg = 0;
+
+	*sent = 0;
+	*dent = 0;
+	while (src && dst && ((srcsg + 1) <= MAX_SKB_FRAGS) &&
+	       space > (sgl_ent_len[srcsg + 1] + dsgl_ent_len[dstsg])) {
+		srclen += src->length;
+		srcsg++;
+		while (dst && ((dstsg + 1) <= MAX_DSGL_ENT) &&
+		       space > (sgl_ent_len[srcsg] + dsgl_ent_len[dstsg + 1])) {
+			if (srclen <= dstlen)
+				break;
+			dstlen += dst->length;
+			dst = sg_next(dst);
+			dstsg++;
+		}
+		src = sg_next(src);
+	}
+	*sent = srcsg - minsg;
+	*dent = dstsg;
+	return min(srclen, dstlen);
+}
+
+static int chcr_cipher_fallback(struct crypto_skcipher *cipher,
+				u32 flags,
+				struct scatterlist *src,
+				struct scatterlist *dst,
+				unsigned int nbytes,
+				u8 *iv,
+				unsigned short op_type)
+{
+	int err;
 
+	SKCIPHER_REQUEST_ON_STACK(subreq, cipher);
+	skcipher_request_set_tfm(subreq, cipher);
+	skcipher_request_set_callback(subreq, flags, NULL, NULL);
+	skcipher_request_set_crypt(subreq, src, dst,
+				   nbytes, iv);
+
+	err = op_type ? crypto_skcipher_decrypt(subreq) :
+		crypto_skcipher_encrypt(subreq);
+	skcipher_request_zero(subreq);
+
+	return err;
+
+}
 static inline void create_wreq(struct chcr_context *ctx,
 			       struct chcr_wr *chcr_req,
 			       void *req, struct sk_buff *skb,
 			       int kctx_len, int hash_sz,
 			       int is_iv,
-			       unsigned int sc_len)
+			       unsigned int sc_len,
+			       unsigned int lcb)
 {
 	struct uld_ctx *u_ctx = ULD_CTX(ctx);
 	int iv_loc = IV_DSGL;
@@ -543,7 +605,8 @@ static inline void create_wreq(struct chcr_context *ctx,
 	chcr_req->wreq.cookie = cpu_to_be64((uintptr_t)req);
 	chcr_req->wreq.rx_chid_to_rx_q_id =
 		FILL_WR_RX_Q_ID(ctx->dev->rx_channel_id, qid,
-				is_iv ? iv_loc : IV_NOP, ctx->tx_qidx);
+				is_iv ? iv_loc : IV_NOP, !!lcb,
+				ctx->tx_qidx);
 
 	chcr_req->ulptx.cmd_dest = FILL_ULPTX_CMD_DEST(ctx->dev->tx_channel_id,
 						       qid);
@@ -563,69 +626,61 @@ static inline void create_wreq(struct chcr_context *ctx,
  *	@qid: ingress qid where response of this WR should be received.
  *	@op_type:	encryption or decryption
  */
-static struct sk_buff
-*create_cipher_wr(struct ablkcipher_request *req,
-		  unsigned short qid,
-		  unsigned short op_type)
+static struct sk_buff *create_cipher_wr(struct cipher_wr_param *wrparam)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(wrparam->req);
 	struct chcr_context *ctx = crypto_ablkcipher_ctx(tfm);
 	struct uld_ctx *u_ctx = ULD_CTX(ctx);
 	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
 	struct sk_buff *skb = NULL;
 	struct chcr_wr *chcr_req;
 	struct cpl_rx_phys_dsgl *phys_cpl;
-	struct chcr_blkcipher_req_ctx *reqctx = ablkcipher_request_ctx(req);
+	struct chcr_blkcipher_req_ctx *reqctx =
+		ablkcipher_request_ctx(wrparam->req);
 	struct phys_sge_parm sg_param;
 	unsigned int frags = 0, transhdr_len, phys_dsgl;
-	unsigned int ivsize = crypto_ablkcipher_ivsize(tfm), kctx_len;
-	gfp_t flags = req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP ? GFP_KERNEL :
-			GFP_ATOMIC;
-
-	if (!req->info)
-		return ERR_PTR(-EINVAL);
-	reqctx->dst_nents = sg_nents_for_len(req->dst, req->nbytes);
-	if (reqctx->dst_nents <= 0) {
-		pr_err("AES:Invalid Destination sg lists\n");
-		return ERR_PTR(-EINVAL);
-	}
-	if ((ablkctx->enckey_len == 0) || (ivsize > AES_BLOCK_SIZE) ||
-	    (req->nbytes <= 0) || (req->nbytes % AES_BLOCK_SIZE)) {
-		pr_err("AES: Invalid value of Key Len %d nbytes %d IV Len %d\n",
-		       ablkctx->enckey_len, req->nbytes, ivsize);
-		return ERR_PTR(-EINVAL);
-	}
+	int error;
+	unsigned int ivsize = AES_BLOCK_SIZE, kctx_len;
+	gfp_t flags = wrparam->req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP ?
+			GFP_KERNEL : GFP_ATOMIC;
+	struct adapter *adap = padap(ctx->dev);
 
 	phys_dsgl = get_space_for_phys_dsgl(reqctx->dst_nents);
 
 	kctx_len = (DIV_ROUND_UP(ablkctx->enckey_len, 16) * 16);
 	transhdr_len = CIPHER_TRANSHDR_SIZE(kctx_len, phys_dsgl);
 	skb = alloc_skb((transhdr_len + sizeof(struct sge_opaque_hdr)), flags);
-	if (!skb)
-		return ERR_PTR(-ENOMEM);
+	if (!skb) {
+		error = -ENOMEM;
+		goto err;
+	}
 	skb_reserve(skb, sizeof(struct sge_opaque_hdr));
-	chcr_req = (struct chcr_wr *)__skb_put(skb, transhdr_len);
-	memset(chcr_req, 0, transhdr_len);
+	chcr_req = __skb_put_zero(skb, transhdr_len);
 	chcr_req->sec_cpl.op_ivinsrtofst =
 		FILL_SEC_CPL_OP_IVINSR(ctx->dev->rx_channel_id, 2, 1);
 
-	chcr_req->sec_cpl.pldlen = htonl(ivsize + req->nbytes);
+	chcr_req->sec_cpl.pldlen = htonl(ivsize + wrparam->bytes);
 	chcr_req->sec_cpl.aadstart_cipherstop_hi =
 			FILL_SEC_CPL_CIPHERSTOP_HI(0, 0, ivsize + 1, 0);
 
 	chcr_req->sec_cpl.cipherstop_lo_authinsert =
 			FILL_SEC_CPL_AUTHINSERT(0, 0, 0, 0);
-	chcr_req->sec_cpl.seqno_numivs = FILL_SEC_CPL_SCMD0_SEQNO(op_type, 0,
+	chcr_req->sec_cpl.seqno_numivs = FILL_SEC_CPL_SCMD0_SEQNO(reqctx->op, 0,
 							 ablkctx->ciph_mode,
 							 0, 0, ivsize >> 1);
 	chcr_req->sec_cpl.ivgen_hdrlen = FILL_SEC_CPL_IVGEN_HDRLEN(0, 0, 0,
 							  0, 1, phys_dsgl);
 
 	chcr_req->key_ctx.ctx_hdr = ablkctx->key_ctx_hdr;
-	if (op_type == CHCR_DECRYPT_OP) {
+	if ((reqctx->op == CHCR_DECRYPT_OP) &&
+	    (!(get_cryptoalg_subtype(crypto_ablkcipher_tfm(tfm)) ==
+	       CRYPTO_ALG_SUB_TYPE_CTR)) &&
+	    (!(get_cryptoalg_subtype(crypto_ablkcipher_tfm(tfm)) ==
+	       CRYPTO_ALG_SUB_TYPE_CTR_RFC3686))) {
 		generate_copy_rrkey(ablkctx, &chcr_req->key_ctx);
 	} else {
-		if (ablkctx->ciph_mode == CHCR_SCMD_CIPHER_MODE_AES_CBC) {
+		if ((ablkctx->ciph_mode == CHCR_SCMD_CIPHER_MODE_AES_CBC) ||
+		    (ablkctx->ciph_mode == CHCR_SCMD_CIPHER_MODE_AES_CTR)) {
 			memcpy(chcr_req->key_ctx.key, ablkctx->key,
 			       ablkctx->enckey_len);
 		} else {
@@ -640,45 +695,80 @@ static struct sk_buff
 	}
 	phys_cpl = (struct cpl_rx_phys_dsgl *)((u8 *)(chcr_req + 1) + kctx_len);
 	sg_param.nents = reqctx->dst_nents;
-	sg_param.obsize = req->nbytes;
-	sg_param.qid = qid;
-	sg_param.align = 1;
-	if (map_writesg_phys_cpl(&u_ctx->lldi.pdev->dev, phys_cpl, req->dst,
-				 &sg_param))
+	sg_param.obsize =  wrparam->bytes;
+	sg_param.qid = wrparam->qid;
+	error = map_writesg_phys_cpl(&u_ctx->lldi.pdev->dev, phys_cpl,
+				       reqctx->dst, &sg_param);
+	if (error)
 		goto map_fail1;
 
 	skb_set_transport_header(skb, transhdr_len);
-	memcpy(reqctx->iv, req->info, ivsize);
 	write_buffer_to_skb(skb, &frags, reqctx->iv, ivsize);
-	write_sg_to_skb(skb, &frags, req->src, req->nbytes);
-	create_wreq(ctx, chcr_req, req, skb, kctx_len, 0, 1,
-			sizeof(struct cpl_rx_phys_dsgl) + phys_dsgl);
+	write_sg_to_skb(skb, &frags, wrparam->srcsg, wrparam->bytes);
+	atomic_inc(&adap->chcr_stats.cipher_rqst);
+	create_wreq(ctx, chcr_req, &(wrparam->req->base), skb, kctx_len, 0, 1,
+			sizeof(struct cpl_rx_phys_dsgl) + phys_dsgl,
+			ablkctx->ciph_mode == CHCR_SCMD_CIPHER_MODE_AES_CBC);
 	reqctx->skb = skb;
 	skb_get(skb);
 	return skb;
 map_fail1:
 	kfree_skb(skb);
-	return ERR_PTR(-ENOMEM);
+err:
+	return ERR_PTR(error);
 }
 
-static int chcr_aes_cbc_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+static inline int chcr_keyctx_ck_size(unsigned int keylen)
+{
+	int ck_size = 0;
+
+	if (keylen == AES_KEYSIZE_128)
+		ck_size = CHCR_KEYCTX_CIPHER_KEY_SIZE_128;
+	else if (keylen == AES_KEYSIZE_192)
+		ck_size = CHCR_KEYCTX_CIPHER_KEY_SIZE_192;
+	else if (keylen == AES_KEYSIZE_256)
+		ck_size = CHCR_KEYCTX_CIPHER_KEY_SIZE_256;
+	else
+		ck_size = 0;
+
+	return ck_size;
+}
+static int chcr_cipher_fallback_setkey(struct crypto_ablkcipher *cipher,
+				       const u8 *key,
+				       unsigned int keylen)
+{
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
+	struct chcr_context *ctx = crypto_ablkcipher_ctx(cipher);
+	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
+	int err = 0;
+
+	crypto_skcipher_clear_flags(ablkctx->sw_cipher, CRYPTO_TFM_REQ_MASK);
+	crypto_skcipher_set_flags(ablkctx->sw_cipher, cipher->base.crt_flags &
+				  CRYPTO_TFM_REQ_MASK);
+	err = crypto_skcipher_setkey(ablkctx->sw_cipher, key, keylen);
+	tfm->crt_flags &= ~CRYPTO_TFM_RES_MASK;
+	tfm->crt_flags |=
+		crypto_skcipher_get_flags(ablkctx->sw_cipher) &
+		CRYPTO_TFM_RES_MASK;
+	return err;
+}
+
+static int chcr_aes_cbc_setkey(struct crypto_ablkcipher *cipher,
+			       const u8 *key,
 			       unsigned int keylen)
 {
-	struct chcr_context *ctx = crypto_ablkcipher_ctx(tfm);
+	struct chcr_context *ctx = crypto_ablkcipher_ctx(cipher);
 	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
 	unsigned int ck_size, context_size;
 	u16 alignment = 0;
+	int err;
 
-	if (keylen == AES_KEYSIZE_128) {
-		ck_size = CHCR_KEYCTX_CIPHER_KEY_SIZE_128;
-	} else if (keylen == AES_KEYSIZE_192) {
-		alignment = 8;
-		ck_size = CHCR_KEYCTX_CIPHER_KEY_SIZE_192;
-	} else if (keylen == AES_KEYSIZE_256) {
-		ck_size = CHCR_KEYCTX_CIPHER_KEY_SIZE_256;
-	} else {
+	err = chcr_cipher_fallback_setkey(cipher, key, keylen);
+	if (err)
 		goto badkey_err;
-	}
+
+	ck_size = chcr_keyctx_ck_size(keylen);
+	alignment = ck_size == CHCR_KEYCTX_CIPHER_KEY_SIZE_192 ? 8 : 0;
 	memcpy(ablkctx->key, key, keylen);
 	ablkctx->enckey_len = keylen;
 	get_aes_decrypt_key(ablkctx->rrkey, ablkctx->key, keylen << 3);
@@ -690,35 +780,398 @@ static int chcr_aes_cbc_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
 	ablkctx->ciph_mode = CHCR_SCMD_CIPHER_MODE_AES_CBC;
 	return 0;
 badkey_err:
-	crypto_ablkcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+	crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
 	ablkctx->enckey_len = 0;
-	return -EINVAL;
+
+	return err;
 }
 
-static int cxgb4_is_crypto_q_full(struct net_device *dev, unsigned int idx)
+static int chcr_aes_ctr_setkey(struct crypto_ablkcipher *cipher,
+				   const u8 *key,
+				   unsigned int keylen)
 {
-	struct adapter *adap = netdev2adap(dev);
-	struct sge_uld_txq_info *txq_info =
-		adap->sge.uld_txq_info[CXGB4_TX_CRYPTO];
-	struct sge_uld_txq *txq;
+	struct chcr_context *ctx = crypto_ablkcipher_ctx(cipher);
+	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
+	unsigned int ck_size, context_size;
+	u16 alignment = 0;
+	int err;
+
+	err = chcr_cipher_fallback_setkey(cipher, key, keylen);
+	if (err)
+		goto badkey_err;
+	ck_size = chcr_keyctx_ck_size(keylen);
+	alignment = (ck_size == CHCR_KEYCTX_CIPHER_KEY_SIZE_192) ? 8 : 0;
+	memcpy(ablkctx->key, key, keylen);
+	ablkctx->enckey_len = keylen;
+	context_size = (KEY_CONTEXT_HDR_SALT_AND_PAD +
+			keylen + alignment) >> 4;
+
+	ablkctx->key_ctx_hdr = FILL_KEY_CTX_HDR(ck_size, CHCR_KEYCTX_NO_KEY,
+						0, 0, context_size);
+	ablkctx->ciph_mode = CHCR_SCMD_CIPHER_MODE_AES_CTR;
+
+	return 0;
+badkey_err:
+	crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+	ablkctx->enckey_len = 0;
+
+	return err;
+}
+
+static int chcr_aes_rfc3686_setkey(struct crypto_ablkcipher *cipher,
+				   const u8 *key,
+				   unsigned int keylen)
+{
+	struct chcr_context *ctx = crypto_ablkcipher_ctx(cipher);
+	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
+	unsigned int ck_size, context_size;
+	u16 alignment = 0;
+	int err;
+
+	if (keylen < CTR_RFC3686_NONCE_SIZE)
+		return -EINVAL;
+	memcpy(ablkctx->nonce, key + (keylen - CTR_RFC3686_NONCE_SIZE),
+	       CTR_RFC3686_NONCE_SIZE);
+
+	keylen -= CTR_RFC3686_NONCE_SIZE;
+	err = chcr_cipher_fallback_setkey(cipher, key, keylen);
+	if (err)
+		goto badkey_err;
+
+	ck_size = chcr_keyctx_ck_size(keylen);
+	alignment = (ck_size == CHCR_KEYCTX_CIPHER_KEY_SIZE_192) ? 8 : 0;
+	memcpy(ablkctx->key, key, keylen);
+	ablkctx->enckey_len = keylen;
+	context_size = (KEY_CONTEXT_HDR_SALT_AND_PAD +
+			keylen + alignment) >> 4;
+
+	ablkctx->key_ctx_hdr = FILL_KEY_CTX_HDR(ck_size, CHCR_KEYCTX_NO_KEY,
+						0, 0, context_size);
+	ablkctx->ciph_mode = CHCR_SCMD_CIPHER_MODE_AES_CTR;
+
+	return 0;
+badkey_err:
+	crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+	ablkctx->enckey_len = 0;
+
+	return err;
+}
+static void ctr_add_iv(u8 *dstiv, u8 *srciv, u32 add)
+{
+	unsigned int size = AES_BLOCK_SIZE;
+	__be32 *b = (__be32 *)(dstiv + size);
+	u32 c, prev;
+
+	memcpy(dstiv, srciv, AES_BLOCK_SIZE);
+	for (; size >= 4; size -= 4) {
+		prev = be32_to_cpu(*--b);
+		c = prev + add;
+		*b = cpu_to_be32(c);
+		if (prev < c)
+			break;
+		add = 1;
+	}
+
+}
+
+static unsigned int adjust_ctr_overflow(u8 *iv, u32 bytes)
+{
+	__be32 *b = (__be32 *)(iv + AES_BLOCK_SIZE);
+	u64 c;
+	u32 temp = be32_to_cpu(*--b);
+
+	temp = ~temp;
+	c = (u64)temp +  1; // No of block can processed withou overflow
+	if ((bytes / AES_BLOCK_SIZE) > c)
+		bytes = c * AES_BLOCK_SIZE;
+	return bytes;
+}
+
+static int chcr_update_tweak(struct ablkcipher_request *req, u8 *iv)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct chcr_context *ctx = crypto_ablkcipher_ctx(tfm);
+	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
+	struct chcr_blkcipher_req_ctx *reqctx = ablkcipher_request_ctx(req);
+	struct crypto_cipher *cipher;
+	int ret, i;
+	u8 *key;
+	unsigned int keylen;
+
+	cipher = crypto_alloc_cipher("aes-generic", 0, 0);
+	memcpy(iv, req->info, AES_BLOCK_SIZE);
+
+	if (IS_ERR(cipher)) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	keylen = ablkctx->enckey_len / 2;
+	key = ablkctx->key + keylen;
+	ret = crypto_cipher_setkey(cipher, key, keylen);
+	if (ret)
+		goto out1;
+
+	crypto_cipher_encrypt_one(cipher, iv, iv);
+	for (i = 0; i < (reqctx->processed / AES_BLOCK_SIZE); i++)
+		gf128mul_x_ble((le128 *)iv, (le128 *)iv);
+
+	crypto_cipher_decrypt_one(cipher, iv, iv);
+out1:
+	crypto_free_cipher(cipher);
+out:
+	return ret;
+}
+
+static int chcr_update_cipher_iv(struct ablkcipher_request *req,
+				   struct cpl_fw6_pld *fw6_pld, u8 *iv)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct chcr_blkcipher_req_ctx *reqctx = ablkcipher_request_ctx(req);
+	int subtype = get_cryptoalg_subtype(crypto_ablkcipher_tfm(tfm));
 	int ret = 0;
 
-	local_bh_disable();
-	txq = &txq_info->uldtxq[idx];
-	spin_lock(&txq->sendq.lock);
-	if (txq->full)
-		ret = -1;
-	spin_unlock(&txq->sendq.lock);
-	local_bh_enable();
+	if (subtype == CRYPTO_ALG_SUB_TYPE_CTR)
+		ctr_add_iv(iv, req->info, (reqctx->processed /
+			   AES_BLOCK_SIZE));
+	else if (subtype == CRYPTO_ALG_SUB_TYPE_CTR_RFC3686)
+		*(__be32 *)(reqctx->iv + CTR_RFC3686_NONCE_SIZE +
+			CTR_RFC3686_IV_SIZE) = cpu_to_be32((reqctx->processed /
+						AES_BLOCK_SIZE) + 1);
+	else if (subtype == CRYPTO_ALG_SUB_TYPE_XTS)
+		ret = chcr_update_tweak(req, iv);
+	else if (subtype == CRYPTO_ALG_SUB_TYPE_CBC) {
+		if (reqctx->op)
+			sg_pcopy_to_buffer(req->src, sg_nents(req->src), iv,
+					   16,
+					   reqctx->processed - AES_BLOCK_SIZE);
+		else
+			memcpy(iv, &fw6_pld->data[2], AES_BLOCK_SIZE);
+	}
+
 	return ret;
+
 }
 
-static int chcr_aes_encrypt(struct ablkcipher_request *req)
+/* We need separate function for final iv because in rfc3686  Initial counter
+ * starts from 1 and buffer size of iv is 8 byte only which remains constant
+ * for subsequent update requests
+ */
+
+static int chcr_final_cipher_iv(struct ablkcipher_request *req,
+				   struct cpl_fw6_pld *fw6_pld, u8 *iv)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct chcr_blkcipher_req_ctx *reqctx = ablkcipher_request_ctx(req);
+	int subtype = get_cryptoalg_subtype(crypto_ablkcipher_tfm(tfm));
+	int ret = 0;
+
+	if (subtype == CRYPTO_ALG_SUB_TYPE_CTR)
+		ctr_add_iv(iv, req->info, (reqctx->processed /
+			   AES_BLOCK_SIZE));
+	else if (subtype == CRYPTO_ALG_SUB_TYPE_XTS)
+		ret = chcr_update_tweak(req, iv);
+	else if (subtype == CRYPTO_ALG_SUB_TYPE_CBC) {
+		if (reqctx->op)
+			sg_pcopy_to_buffer(req->src, sg_nents(req->src), iv,
+					   16,
+					   reqctx->processed - AES_BLOCK_SIZE);
+		else
+			memcpy(iv, &fw6_pld->data[2], AES_BLOCK_SIZE);
+
+	}
+	return ret;
+
+}
+
+
+static int chcr_handle_cipher_resp(struct ablkcipher_request *req,
+				   unsigned char *input, int err)
 {
 	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
 	struct chcr_context *ctx = crypto_ablkcipher_ctx(tfm);
 	struct uld_ctx *u_ctx = ULD_CTX(ctx);
+	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
 	struct sk_buff *skb;
+	struct cpl_fw6_pld *fw6_pld = (struct cpl_fw6_pld *)input;
+	struct chcr_blkcipher_req_ctx *reqctx = ablkcipher_request_ctx(req);
+	struct  cipher_wr_param wrparam;
+	int bytes;
+
+	dma_unmap_sg(&u_ctx->lldi.pdev->dev, reqctx->dst, reqctx->dst_nents,
+		     DMA_FROM_DEVICE);
+
+	if (reqctx->skb) {
+		kfree_skb(reqctx->skb);
+		reqctx->skb = NULL;
+	}
+	if (err)
+		goto complete;
+
+	if (req->nbytes == reqctx->processed) {
+		err = chcr_final_cipher_iv(req, fw6_pld, req->info);
+		goto complete;
+	}
+
+	if (unlikely(cxgb4_is_crypto_q_full(u_ctx->lldi.ports[0],
+					    ctx->tx_qidx))) {
+		if (!(req->base.flags & CRYPTO_TFM_REQ_MAY_BACKLOG)) {
+			err = -EBUSY;
+			goto complete;
+		}
+
+	}
+	wrparam.srcsg = scatterwalk_ffwd(reqctx->srcffwd, req->src,
+				       reqctx->processed);
+	reqctx->dst = scatterwalk_ffwd(reqctx->dstffwd, reqctx->dstsg,
+					 reqctx->processed);
+	if (!wrparam.srcsg || !reqctx->dst) {
+		pr_err("Input sg list length less that nbytes\n");
+		err = -EINVAL;
+		goto complete;
+	}
+	bytes = chcr_sg_ent_in_wr(wrparam.srcsg, reqctx->dst, 1,
+				 SPACE_LEFT(ablkctx->enckey_len),
+				 &wrparam.snent, &reqctx->dst_nents);
+	if ((bytes + reqctx->processed) >= req->nbytes)
+		bytes  = req->nbytes - reqctx->processed;
+	else
+		bytes = ROUND_16(bytes);
+	err = chcr_update_cipher_iv(req, fw6_pld, reqctx->iv);
+	if (err)
+		goto complete;
+
+	if (unlikely(bytes == 0)) {
+		err = chcr_cipher_fallback(ablkctx->sw_cipher,
+				     req->base.flags,
+				     wrparam.srcsg,
+				     reqctx->dst,
+				     req->nbytes - reqctx->processed,
+				     reqctx->iv,
+				     reqctx->op);
+		goto complete;
+	}
+
+	if (get_cryptoalg_subtype(crypto_ablkcipher_tfm(tfm)) ==
+	    CRYPTO_ALG_SUB_TYPE_CTR)
+		bytes = adjust_ctr_overflow(reqctx->iv, bytes);
+	reqctx->processed += bytes;
+	wrparam.qid = u_ctx->lldi.rxq_ids[ctx->rx_qidx];
+	wrparam.req = req;
+	wrparam.bytes = bytes;
+	skb = create_cipher_wr(&wrparam);
+	if (IS_ERR(skb)) {
+		pr_err("chcr : %s : Failed to form WR. No memory\n", __func__);
+		err = PTR_ERR(skb);
+		goto complete;
+	}
+	skb->dev = u_ctx->lldi.ports[0];
+	set_wr_txq(skb, CPL_PRIORITY_DATA, ctx->tx_qidx);
+	chcr_send_wr(skb);
+	return 0;
+complete:
+	free_new_sg(reqctx->newdstsg);
+	reqctx->newdstsg = NULL;
+	req->base.complete(&req->base, err);
+	return err;
+}
+
+static int process_cipher(struct ablkcipher_request *req,
+				  unsigned short qid,
+				  struct sk_buff **skb,
+				  unsigned short op_type)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	unsigned int ivsize = crypto_ablkcipher_ivsize(tfm);
+	struct chcr_blkcipher_req_ctx *reqctx = ablkcipher_request_ctx(req);
+	struct chcr_context *ctx = crypto_ablkcipher_ctx(tfm);
+	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
+	struct	cipher_wr_param wrparam;
+	int bytes, nents, err = -EINVAL;
+
+	reqctx->newdstsg = NULL;
+	reqctx->processed = 0;
+	if (!req->info)
+		goto error;
+	if ((ablkctx->enckey_len == 0) || (ivsize > AES_BLOCK_SIZE) ||
+	    (req->nbytes == 0) ||
+	    (req->nbytes % crypto_ablkcipher_blocksize(tfm))) {
+		pr_err("AES: Invalid value of Key Len %d nbytes %d IV Len %d\n",
+		       ablkctx->enckey_len, req->nbytes, ivsize);
+		goto error;
+	}
+	wrparam.srcsg = req->src;
+	if (is_newsg(req->dst, &nents)) {
+		reqctx->newdstsg = alloc_new_sg(req->dst, nents);
+		if (IS_ERR(reqctx->newdstsg))
+			return PTR_ERR(reqctx->newdstsg);
+		reqctx->dstsg = reqctx->newdstsg;
+	} else {
+		reqctx->dstsg = req->dst;
+	}
+	bytes = chcr_sg_ent_in_wr(wrparam.srcsg, reqctx->dstsg, MIN_CIPHER_SG,
+				 SPACE_LEFT(ablkctx->enckey_len),
+				 &wrparam.snent,
+				 &reqctx->dst_nents);
+	if ((bytes + reqctx->processed) >= req->nbytes)
+		bytes  = req->nbytes - reqctx->processed;
+	else
+		bytes = ROUND_16(bytes);
+	if (unlikely(bytes > req->nbytes))
+		bytes = req->nbytes;
+	if (get_cryptoalg_subtype(crypto_ablkcipher_tfm(tfm)) ==
+				  CRYPTO_ALG_SUB_TYPE_CTR) {
+		bytes = adjust_ctr_overflow(req->info, bytes);
+	}
+	if (get_cryptoalg_subtype(crypto_ablkcipher_tfm(tfm)) ==
+	    CRYPTO_ALG_SUB_TYPE_CTR_RFC3686) {
+		memcpy(reqctx->iv, ablkctx->nonce, CTR_RFC3686_NONCE_SIZE);
+		memcpy(reqctx->iv + CTR_RFC3686_NONCE_SIZE, req->info,
+				CTR_RFC3686_IV_SIZE);
+
+		/* initialize counter portion of counter block */
+		*(__be32 *)(reqctx->iv + CTR_RFC3686_NONCE_SIZE +
+			CTR_RFC3686_IV_SIZE) = cpu_to_be32(1);
+
+	} else {
+
+		memcpy(reqctx->iv, req->info, ivsize);
+	}
+	if (unlikely(bytes == 0)) {
+		err = chcr_cipher_fallback(ablkctx->sw_cipher,
+					   req->base.flags,
+					   req->src,
+					   req->dst,
+					   req->nbytes,
+					   req->info,
+					   op_type);
+		goto error;
+	}
+	reqctx->processed = bytes;
+	reqctx->dst = reqctx->dstsg;
+	reqctx->op = op_type;
+	wrparam.qid = qid;
+	wrparam.req = req;
+	wrparam.bytes = bytes;
+	*skb = create_cipher_wr(&wrparam);
+	if (IS_ERR(*skb)) {
+		err = PTR_ERR(*skb);
+		goto error;
+	}
+
+	return 0;
+error:
+	free_new_sg(reqctx->newdstsg);
+	reqctx->newdstsg = NULL;
+	return err;
+}
+
+static int chcr_aes_encrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct chcr_context *ctx = crypto_ablkcipher_ctx(tfm);
+	struct sk_buff *skb = NULL;
+	int err;
+	struct uld_ctx *u_ctx = ULD_CTX(ctx);
 
 	if (unlikely(cxgb4_is_crypto_q_full(u_ctx->lldi.ports[0],
 					    ctx->tx_qidx))) {
@@ -726,12 +1179,10 @@ static int chcr_aes_encrypt(struct ablkcipher_request *req)
 			return -EBUSY;
 	}
 
-	skb = create_cipher_wr(req, u_ctx->lldi.rxq_ids[ctx->rx_qidx],
+	err = process_cipher(req, u_ctx->lldi.rxq_ids[ctx->rx_qidx], &skb,
 			       CHCR_ENCRYPT_OP);
-	if (IS_ERR(skb)) {
-		pr_err("chcr : %s : Failed to form WR. No memory\n", __func__);
-		return  PTR_ERR(skb);
-	}
+	if (err || !skb)
+		return  err;
 	skb->dev = u_ctx->lldi.ports[0];
 	set_wr_txq(skb, CPL_PRIORITY_DATA, ctx->tx_qidx);
 	chcr_send_wr(skb);
@@ -743,7 +1194,8 @@ static int chcr_aes_decrypt(struct ablkcipher_request *req)
 	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
 	struct chcr_context *ctx = crypto_ablkcipher_ctx(tfm);
 	struct uld_ctx *u_ctx = ULD_CTX(ctx);
-	struct sk_buff *skb;
+	struct sk_buff *skb = NULL;
+	int err;
 
 	if (unlikely(cxgb4_is_crypto_q_full(u_ctx->lldi.ports[0],
 					    ctx->tx_qidx))) {
@@ -751,12 +1203,10 @@ static int chcr_aes_decrypt(struct ablkcipher_request *req)
 			return -EBUSY;
 	}
 
-	skb = create_cipher_wr(req, u_ctx->lldi.rxq_ids[ctx->rx_qidx],
+	 err = process_cipher(req, u_ctx->lldi.rxq_ids[ctx->rx_qidx], &skb,
 			       CHCR_DECRYPT_OP);
-	if (IS_ERR(skb)) {
-		pr_err("chcr : %s : Failed to form WR. No memory\n", __func__);
-		return PTR_ERR(skb);
-	}
+	if (err || !skb)
+		return err;
 	skb->dev = u_ctx->lldi.ports[0];
 	set_wr_txq(skb, CPL_PRIORITY_DATA, ctx->tx_qidx);
 	chcr_send_wr(skb);
@@ -765,7 +1215,7 @@ static int chcr_aes_decrypt(struct ablkcipher_request *req)
 
 static int chcr_device_init(struct chcr_context *ctx)
 {
-	struct uld_ctx *u_ctx;
+	struct uld_ctx *u_ctx = NULL;
 	struct adapter *adap;
 	unsigned int id;
 	int txq_perchan, txq_idx, ntxq;
@@ -773,12 +1223,12 @@ static int chcr_device_init(struct chcr_context *ctx)
 
 	id = smp_processor_id();
 	if (!ctx->dev) {
-		err = assign_chcr_device(&ctx->dev);
-		if (err) {
+		u_ctx = assign_chcr_device();
+		if (!u_ctx) {
 			pr_err("chcr device assignment fails\n");
 			goto out;
 		}
-		u_ctx = ULD_CTX(ctx);
+		ctx->dev = u_ctx->dev;
 		adap = padap(ctx->dev);
 		ntxq = min_not_zero((unsigned int)u_ctx->lldi.nrxq,
 				    adap->vres.ncrypto_fc);
@@ -801,10 +1251,48 @@ out:
 
 static int chcr_cra_init(struct crypto_tfm *tfm)
 {
+	struct crypto_alg *alg = tfm->__crt_alg;
+	struct chcr_context *ctx = crypto_tfm_ctx(tfm);
+	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
+
+	ablkctx->sw_cipher = crypto_alloc_skcipher(alg->cra_name, 0,
+				CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK);
+	if (IS_ERR(ablkctx->sw_cipher)) {
+		pr_err("failed to allocate fallback for %s\n", alg->cra_name);
+		return PTR_ERR(ablkctx->sw_cipher);
+	}
+	tfm->crt_ablkcipher.reqsize =  sizeof(struct chcr_blkcipher_req_ctx);
+	return chcr_device_init(crypto_tfm_ctx(tfm));
+}
+
+static int chcr_rfc3686_init(struct crypto_tfm *tfm)
+{
+	struct crypto_alg *alg = tfm->__crt_alg;
+	struct chcr_context *ctx = crypto_tfm_ctx(tfm);
+	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
+
+	/*RFC3686 initialises IV counter value to 1, rfc3686(ctr(aes))
+	 * cannot be used as fallback in chcr_handle_cipher_response
+	 */
+	ablkctx->sw_cipher = crypto_alloc_skcipher("ctr(aes)", 0,
+				CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK);
+	if (IS_ERR(ablkctx->sw_cipher)) {
+		pr_err("failed to allocate fallback for %s\n", alg->cra_name);
+		return PTR_ERR(ablkctx->sw_cipher);
+	}
 	tfm->crt_ablkcipher.reqsize =  sizeof(struct chcr_blkcipher_req_ctx);
 	return chcr_device_init(crypto_tfm_ctx(tfm));
 }
 
+
+static void chcr_cra_exit(struct crypto_tfm *tfm)
+{
+	struct chcr_context *ctx = crypto_tfm_ctx(tfm);
+	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
+
+	crypto_free_skcipher(ablkctx->sw_cipher);
+}
+
 static int get_alg_config(struct algo_param *params,
 			  unsigned int auth_size)
 {
@@ -865,6 +1353,7 @@ static struct sk_buff *create_hash_wr(struct ahash_request *req,
 	u8 hash_size_in_response = 0;
 	gfp_t flags = req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP ? GFP_KERNEL :
 		GFP_ATOMIC;
+	struct adapter *adap = padap(ctx->dev);
 
 	iopad_alignment = KEYCTX_ALIGN_PAD(digestsize);
 	kctx_len = param->alg_prm.result_size + iopad_alignment;
@@ -881,8 +1370,7 @@ static struct sk_buff *create_hash_wr(struct ahash_request *req,
 		return skb;
 
 	skb_reserve(skb, sizeof(struct sge_opaque_hdr));
-	chcr_req = (struct chcr_wr *)__skb_put(skb, transhdr_len);
-	memset(chcr_req, 0, transhdr_len);
+	chcr_req = __skb_put_zero(skb, transhdr_len);
 
 	chcr_req->sec_cpl.op_ivinsrtofst =
 		FILL_SEC_CPL_OP_IVINSR(ctx->dev->rx_channel_id, 2, 0);
@@ -921,9 +1409,9 @@ static struct sk_buff *create_hash_wr(struct ahash_request *req,
 				    param->bfr_len);
 	if (param->sg_len != 0)
 		write_sg_to_skb(skb, &frags, req->src, param->sg_len);
-
-	create_wreq(ctx, chcr_req, req, skb, kctx_len, hash_size_in_response, 0,
-			DUMMY_BYTES);
+	atomic_inc(&adap->chcr_stats.digest_rqst);
+	create_wreq(ctx, chcr_req, &req->base, skb, kctx_len,
+		    hash_size_in_response, 0, DUMMY_BYTES, 0);
 	req_ctx->skb = skb;
 	skb_get(skb);
 	return skb;
@@ -1226,21 +1714,17 @@ out:
 	return err;
 }
 
-static int chcr_aes_xts_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+static int chcr_aes_xts_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
 			       unsigned int key_len)
 {
-	struct chcr_context *ctx = crypto_ablkcipher_ctx(tfm);
+	struct chcr_context *ctx = crypto_ablkcipher_ctx(cipher);
 	struct ablk_ctx *ablkctx = ABLK_CTX(ctx);
 	unsigned short context_size = 0;
+	int err;
 
-	if ((key_len != (AES_KEYSIZE_128 << 1)) &&
-	    (key_len != (AES_KEYSIZE_256 << 1))) {
-		crypto_tfm_set_flags((struct crypto_tfm *)tfm,
-				     CRYPTO_TFM_RES_BAD_KEY_LEN);
-		ablkctx->enckey_len = 0;
-		return -EINVAL;
-
-	}
+	err = chcr_cipher_fallback_setkey(cipher, key, key_len);
+	if (err)
+		goto badkey_err;
 
 	memcpy(ablkctx->key, key, key_len);
 	ablkctx->enckey_len = key_len;
@@ -1254,6 +1738,11 @@ static int chcr_aes_xts_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
 				 0, context_size);
 	ablkctx->ciph_mode = CHCR_SCMD_CIPHER_MODE_AES_XTS;
 	return 0;
+badkey_err:
+	crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+	ablkctx->enckey_len = 0;
+
+	return err;
 }
 
 static int chcr_sha_init(struct ahash_request *areq)
@@ -1330,6 +1819,63 @@ static void chcr_hmac_cra_exit(struct crypto_tfm *tfm)
 	}
 }
 
+static int is_newsg(struct scatterlist *sgl, unsigned int *newents)
+{
+	int nents = 0;
+	int ret = 0;
+
+	while (sgl) {
+		if (sgl->length > CHCR_SG_SIZE)
+			ret = 1;
+		nents += DIV_ROUND_UP(sgl->length, CHCR_SG_SIZE);
+		sgl = sg_next(sgl);
+	}
+	*newents = nents;
+	return ret;
+}
+
+static inline void free_new_sg(struct scatterlist *sgl)
+{
+	kfree(sgl);
+}
+
+static struct scatterlist *alloc_new_sg(struct scatterlist *sgl,
+				       unsigned int nents)
+{
+	struct scatterlist *newsg, *sg;
+	int i, len, processed = 0;
+	struct page *spage;
+	int offset;
+
+	newsg = kmalloc_array(nents, sizeof(struct scatterlist), GFP_KERNEL);
+	if (!newsg)
+		return ERR_PTR(-ENOMEM);
+	sg = newsg;
+	sg_init_table(sg, nents);
+	offset = sgl->offset;
+	spage = sg_page(sgl);
+	for (i = 0; i < nents; i++) {
+		len = min_t(u32, sgl->length - processed, CHCR_SG_SIZE);
+		sg_set_page(sg, spage, len, offset);
+		processed += len;
+		offset += len;
+		if (offset >= PAGE_SIZE) {
+			offset = offset % PAGE_SIZE;
+			spage++;
+		}
+		if (processed == sgl->length) {
+			processed = 0;
+			sgl = sg_next(sgl);
+			if (!sgl)
+				break;
+			spage = sg_page(sgl);
+			offset = sgl->offset;
+		}
+		sg = sg_next(sg);
+	}
+	return newsg;
+}
+
 static int chcr_copy_assoc(struct aead_request *req,
 				struct chcr_aead_ctx *ctx)
 {
@@ -1392,16 +1938,20 @@ static struct sk_buff *create_authenc_wr(struct aead_request *req,
 	struct scatterlist *src;
 	unsigned int frags = 0, transhdr_len;
 	unsigned int ivsize = crypto_aead_ivsize(tfm), dst_size = 0;
-	unsigned int   kctx_len = 0;
+	unsigned int   kctx_len = 0, nents;
 	unsigned short stop_offset = 0;
 	unsigned int  assoclen = req->assoclen;
 	unsigned int  authsize = crypto_aead_authsize(tfm);
-	int err = -EINVAL, src_nent;
+	int error = -EINVAL, src_nent;
 	int null = 0;
 	gfp_t flags = req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP ? GFP_KERNEL :
 		GFP_ATOMIC;
+	struct adapter *adap = padap(ctx->dev);
 
-	if (aeadctx->enckey_len == 0 || (req->cryptlen == 0))
+	reqctx->newdstsg = NULL;
+	dst_size = req->assoclen + req->cryptlen + (op_type ? -authsize :
+						   authsize);
+	if (aeadctx->enckey_len == 0 || (req->cryptlen <= 0))
 		goto err;
 
 	if (op_type && req->cryptlen < crypto_aead_authsize(tfm))
@@ -1410,14 +1960,24 @@ static struct sk_buff *create_authenc_wr(struct aead_request *req,
 	if (src_nent < 0)
 		goto err;
 	src = scatterwalk_ffwd(reqctx->srcffwd, req->src, req->assoclen);
-	reqctx->dst = src;
 
 	if (req->src != req->dst) {
-		err = chcr_copy_assoc(req, aeadctx);
-		if (err)
-			return ERR_PTR(err);
-		reqctx->dst = scatterwalk_ffwd(reqctx->dstffwd, req->dst,
-					       req->assoclen);
+		error = chcr_copy_assoc(req, aeadctx);
+		if (error)
+			return ERR_PTR(error);
+	}
+	if (dst_size && is_newsg(req->dst, &nents)) {
+		reqctx->newdstsg = alloc_new_sg(req->dst, nents);
+		if (IS_ERR(reqctx->newdstsg))
+			return ERR_CAST(reqctx->newdstsg);
+		reqctx->dst = scatterwalk_ffwd(reqctx->dstffwd,
+					       reqctx->newdstsg, req->assoclen);
+	} else {
+		if (req->src == req->dst)
+			reqctx->dst = src;
+		else
+			reqctx->dst = scatterwalk_ffwd(reqctx->dstffwd,
+						       req->dst, req->assoclen);
 	}
 	if (get_aead_subtype(tfm) == CRYPTO_ALG_SUB_TYPE_AEAD_NULL) {
 		null = 1;
@@ -1427,6 +1987,7 @@ static struct sk_buff *create_authenc_wr(struct aead_request *req,
 					     (op_type ? -authsize : authsize));
 	if (reqctx->dst_nents < 0) {
 		pr_err("AUTHENC:Invalid Destination sg entries\n");
+		error = -EINVAL;
 		goto err;
 	}
 	dst_size = get_space_for_phys_dsgl(reqctx->dst_nents);
@@ -1437,18 +1998,22 @@ static struct sk_buff *create_authenc_wr(struct aead_request *req,
 			T6_MAX_AAD_SIZE,
 			transhdr_len + (sgl_len(src_nent + MIN_AUTH_SG) * 8),
 				op_type)) {
+		atomic_inc(&adap->chcr_stats.fallback);
+		free_new_sg(reqctx->newdstsg);
+		reqctx->newdstsg = NULL;
 		return ERR_PTR(chcr_aead_fallback(req, op_type));
 	}
 	skb = alloc_skb((transhdr_len + sizeof(struct sge_opaque_hdr)), flags);
-	if (!skb)
+	if (!skb) {
+		error = -ENOMEM;
 		goto err;
+	}
 
 	/* LLD is going to write the sge hdr. */
 	skb_reserve(skb, sizeof(struct sge_opaque_hdr));
 
 	/* Write WR */
-	chcr_req = (struct chcr_wr *) __skb_put(skb, transhdr_len);
-	memset(chcr_req, 0, transhdr_len);
+	chcr_req = __skb_put_zero(skb, transhdr_len);
 
 	stop_offset = (op_type == CHCR_ENCRYPT_OP) ? 0 : authsize;
 
@@ -1493,9 +2058,9 @@ static struct sk_buff *create_authenc_wr(struct aead_request *req,
 	sg_param.nents = reqctx->dst_nents;
 	sg_param.obsize = req->cryptlen + (op_type ? -authsize : authsize);
 	sg_param.qid = qid;
-	sg_param.align = 0;
-	if (map_writesg_phys_cpl(&u_ctx->lldi.pdev->dev, phys_cpl, reqctx->dst,
-				  &sg_param))
+	error = map_writesg_phys_cpl(&u_ctx->lldi.pdev->dev, phys_cpl,
+					reqctx->dst, &sg_param);
+	if (error)
 		goto dstmap_fail;
 
 	skb_set_transport_header(skb, transhdr_len);
@@ -1507,8 +2072,9 @@ static struct sk_buff *create_authenc_wr(struct aead_request *req,
 	}
 	write_buffer_to_skb(skb, &frags, req->iv, ivsize);
 	write_sg_to_skb(skb, &frags, src, req->cryptlen);
-	create_wreq(ctx, chcr_req, req, skb, kctx_len, size, 1,
-		   sizeof(struct cpl_rx_phys_dsgl) + dst_size);
+	atomic_inc(&adap->chcr_stats.cipher_rqst);
+	create_wreq(ctx, chcr_req, &req->base, skb, kctx_len, size, 1,
+		   sizeof(struct cpl_rx_phys_dsgl) + dst_size, 0);
 	reqctx->skb = skb;
 	skb_get(skb);
 
@@ -1517,7 +2083,9 @@ dstmap_fail:
 	/* ivmap_fail: */
 	kfree_skb(skb);
 err:
-	return ERR_PTR(-EINVAL);
+	free_new_sg(reqctx->newdstsg);
+	reqctx->newdstsg = NULL;
+	return ERR_PTR(error);
 }
 
 static int set_msg_len(u8 *block, unsigned int msglen, int csize)
@@ -1724,14 +2292,17 @@ static struct sk_buff *create_aead_ccm_wr(struct aead_request *req,
 	struct phys_sge_parm sg_param;
 	struct scatterlist *src;
 	unsigned int frags = 0, transhdr_len, ivsize = AES_BLOCK_SIZE;
-	unsigned int dst_size = 0, kctx_len;
+	unsigned int dst_size = 0, kctx_len, nents;
 	unsigned int sub_type;
 	unsigned int authsize = crypto_aead_authsize(tfm);
-	int err = -EINVAL, src_nent;
+	int error = -EINVAL, src_nent;
 	gfp_t flags = req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP ? GFP_KERNEL :
 		GFP_ATOMIC;
+	struct adapter *adap = padap(ctx->dev);
 
-
+	dst_size = req->assoclen + req->cryptlen + (op_type ? -authsize :
+						   authsize);
+	reqctx->newdstsg = NULL;
 	if (op_type && req->cryptlen < crypto_aead_authsize(tfm))
 		goto err;
 	src_nent = sg_nents_for_len(req->src, req->assoclen + req->cryptlen);
@@ -1740,26 +2311,35 @@ static struct sk_buff *create_aead_ccm_wr(struct aead_request *req,
 
 	sub_type = get_aead_subtype(tfm);
 	src = scatterwalk_ffwd(reqctx->srcffwd, req->src, req->assoclen);
-	reqctx->dst = src;
-
 	if (req->src != req->dst) {
-		err = chcr_copy_assoc(req, aeadctx);
-		if (err) {
+		error = chcr_copy_assoc(req, aeadctx);
+		if (error) {
 			pr_err("AAD copy to destination buffer fails\n");
-			return ERR_PTR(err);
+			return ERR_PTR(error);
 		}
-		reqctx->dst = scatterwalk_ffwd(reqctx->dstffwd, req->dst,
-					       req->assoclen);
+	}
+	if (dst_size && is_newsg(req->dst, &nents)) {
+		reqctx->newdstsg = alloc_new_sg(req->dst, nents);
+		if (IS_ERR(reqctx->newdstsg))
+			return ERR_CAST(reqctx->newdstsg);
+		reqctx->dst = scatterwalk_ffwd(reqctx->dstffwd,
+					       reqctx->newdstsg, req->assoclen);
+	} else {
+		if (req->src == req->dst)
+			reqctx->dst = src;
+		else
+			reqctx->dst = scatterwalk_ffwd(reqctx->dstffwd,
+						       req->dst, req->assoclen);
 	}
 	reqctx->dst_nents = sg_nents_for_len(reqctx->dst, req->cryptlen +
 					     (op_type ? -authsize : authsize));
 	if (reqctx->dst_nents < 0) {
 		pr_err("CCM:Invalid Destination sg entries\n");
+		error = -EINVAL;
 		goto err;
 	}
-
-
-	if (aead_ccm_validate_input(op_type, req, aeadctx, sub_type))
+	error = aead_ccm_validate_input(op_type, req, aeadctx, sub_type);
+	if (error)
 		goto err;
 
 	dst_size = get_space_for_phys_dsgl(reqctx->dst_nents);
@@ -1769,18 +2349,22 @@ static struct sk_buff *create_aead_ccm_wr(struct aead_request *req,
 			    T6_MAX_AAD_SIZE - 18,
 			    transhdr_len + (sgl_len(src_nent + MIN_CCM_SG) * 8),
 			    op_type)) {
+		atomic_inc(&adap->chcr_stats.fallback);
+		free_new_sg(reqctx->newdstsg);
+		reqctx->newdstsg = NULL;
 		return ERR_PTR(chcr_aead_fallback(req, op_type));
 	}
 
 	skb = alloc_skb((transhdr_len + sizeof(struct sge_opaque_hdr)),  flags);
 
-	if (!skb)
+	if (!skb) {
+		error = -ENOMEM;
 		goto err;
+	}
 
 	skb_reserve(skb, sizeof(struct sge_opaque_hdr));
 
-	chcr_req = (struct chcr_wr *) __skb_put(skb, transhdr_len);
-	memset(chcr_req, 0, transhdr_len);
+	chcr_req = __skb_put_zero(skb, transhdr_len);
 
 	fill_sec_cpl_for_aead(&chcr_req->sec_cpl, dst_size, req, op_type, ctx);
 
@@ -1790,29 +2374,32 @@ static struct sk_buff *create_aead_ccm_wr(struct aead_request *req,
 					16), aeadctx->key, aeadctx->enckey_len);
 
 	phys_cpl = (struct cpl_rx_phys_dsgl *)((u8 *)(chcr_req + 1) + kctx_len);
-	if (ccm_format_packet(req, aeadctx, sub_type, op_type))
+	error = ccm_format_packet(req, aeadctx, sub_type, op_type);
+	if (error)
 		goto dstmap_fail;
 
 	sg_param.nents = reqctx->dst_nents;
 	sg_param.obsize = req->cryptlen + (op_type ? -authsize : authsize);
 	sg_param.qid = qid;
-	sg_param.align = 0;
-	if (map_writesg_phys_cpl(&u_ctx->lldi.pdev->dev, phys_cpl, reqctx->dst,
-				  &sg_param))
+	error = map_writesg_phys_cpl(&u_ctx->lldi.pdev->dev, phys_cpl,
+				 reqctx->dst, &sg_param);
+	if (error)
 		goto dstmap_fail;
 
 	skb_set_transport_header(skb, transhdr_len);
 	frags = fill_aead_req_fields(skb, req, src, ivsize, aeadctx);
-	create_wreq(ctx, chcr_req, req, skb, kctx_len, 0, 1,
-		    sizeof(struct cpl_rx_phys_dsgl) + dst_size);
+	atomic_inc(&adap->chcr_stats.aead_rqst);
+	create_wreq(ctx, chcr_req, &req->base, skb, kctx_len, 0, 1,
+		    sizeof(struct cpl_rx_phys_dsgl) + dst_size, 0);
 	reqctx->skb = skb;
 	skb_get(skb);
 	return skb;
 dstmap_fail:
 	kfree_skb(skb);
-	skb = NULL;
 err:
-	return ERR_PTR(-EINVAL);
+	free_new_sg(reqctx->newdstsg);
+	reqctx->newdstsg = NULL;
+	return ERR_PTR(error);
 }
 
 static struct sk_buff *create_gcm_wr(struct aead_request *req,
@@ -1832,45 +2419,53 @@ static struct sk_buff *create_gcm_wr(struct aead_request *req,
 	struct scatterlist *src;
 	unsigned int frags = 0, transhdr_len;
 	unsigned int ivsize = AES_BLOCK_SIZE;
-	unsigned int dst_size = 0, kctx_len;
+	unsigned int dst_size = 0, kctx_len, nents, assoclen = req->assoclen;
 	unsigned char tag_offset = 0;
-	unsigned int crypt_len = 0;
 	unsigned int authsize = crypto_aead_authsize(tfm);
-	int err = -EINVAL, src_nent;
+	int error = -EINVAL, src_nent;
 	gfp_t flags = req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP ? GFP_KERNEL :
 		GFP_ATOMIC;
+	struct adapter *adap = padap(ctx->dev);
 
+	reqctx->newdstsg = NULL;
+	dst_size = assoclen + req->cryptlen + (op_type ? -authsize :
+						    authsize);
 	/* validate key size */
 	if (aeadctx->enckey_len == 0)
 		goto err;
 
 	if (op_type && req->cryptlen < crypto_aead_authsize(tfm))
 		goto err;
-	src_nent = sg_nents_for_len(req->src, req->assoclen + req->cryptlen);
+	src_nent = sg_nents_for_len(req->src, assoclen + req->cryptlen);
 	if (src_nent < 0)
 		goto err;
 
-	src = scatterwalk_ffwd(reqctx->srcffwd, req->src, req->assoclen);
-	reqctx->dst = src;
+	src = scatterwalk_ffwd(reqctx->srcffwd, req->src, assoclen);
 	if (req->src != req->dst) {
-		err = chcr_copy_assoc(req, aeadctx);
-		if (err)
-			return	ERR_PTR(err);
-		reqctx->dst = scatterwalk_ffwd(reqctx->dstffwd, req->dst,
-					       req->assoclen);
+		error = chcr_copy_assoc(req, aeadctx);
+		if (error)
+			return	ERR_PTR(error);
+	}
+
+	if (dst_size && is_newsg(req->dst, &nents)) {
+		reqctx->newdstsg = alloc_new_sg(req->dst, nents);
+		if (IS_ERR(reqctx->newdstsg))
+			return ERR_CAST(reqctx->newdstsg);
+		reqctx->dst = scatterwalk_ffwd(reqctx->dstffwd,
+					       reqctx->newdstsg, assoclen);
+	} else {
+		if (req->src == req->dst)
+			reqctx->dst = src;
+		else
+			reqctx->dst = scatterwalk_ffwd(reqctx->dstffwd,
+						       req->dst, assoclen);
 	}
 
-	if (!req->cryptlen)
-		/* null-payload is not supported in the hardware.
-		 * software is sending block size
-		 */
-		crypt_len = AES_BLOCK_SIZE;
-	else
-		crypt_len = req->cryptlen;
 	reqctx->dst_nents = sg_nents_for_len(reqctx->dst, req->cryptlen +
 					     (op_type ? -authsize : authsize));
 	if (reqctx->dst_nents < 0) {
 		pr_err("GCM:Invalid Destination sg entries\n");
+		error = -EINVAL;
 		goto err;
 	}
 
@@ -1883,32 +2478,36 @@ static struct sk_buff *create_gcm_wr(struct aead_request *req,
 			    T6_MAX_AAD_SIZE,
 			    transhdr_len + (sgl_len(src_nent + MIN_GCM_SG) * 8),
 			    op_type)) {
+		atomic_inc(&adap->chcr_stats.fallback);
+		free_new_sg(reqctx->newdstsg);
+		reqctx->newdstsg = NULL;
 		return ERR_PTR(chcr_aead_fallback(req, op_type));
 	}
 	skb = alloc_skb((transhdr_len + sizeof(struct sge_opaque_hdr)), flags);
-	if (!skb)
+	if (!skb) {
+		error = -ENOMEM;
 		goto err;
+	}
 
 	/* NIC driver is going to write the sge hdr. */
 	skb_reserve(skb, sizeof(struct sge_opaque_hdr));
 
-	chcr_req = (struct chcr_wr *)__skb_put(skb, transhdr_len);
-	memset(chcr_req, 0, transhdr_len);
+	chcr_req = __skb_put_zero(skb, transhdr_len);
 
 	if (get_aead_subtype(tfm) == CRYPTO_ALG_SUB_TYPE_AEAD_RFC4106)
-		req->assoclen -= 8;
+		assoclen = req->assoclen - 8;
 
 	tag_offset = (op_type == CHCR_ENCRYPT_OP) ? 0 : authsize;
 	chcr_req->sec_cpl.op_ivinsrtofst = FILL_SEC_CPL_OP_IVINSR(
 					ctx->dev->rx_channel_id, 2, (ivsize ?
-					(req->assoclen + 1) : 0));
+					(assoclen + 1) : 0));
 	chcr_req->sec_cpl.pldlen =
-		htonl(req->assoclen + ivsize + req->cryptlen);
+		htonl(assoclen + ivsize + req->cryptlen);
 	chcr_req->sec_cpl.aadstart_cipherstop_hi = FILL_SEC_CPL_CIPHERSTOP_HI(
-					req->assoclen ? 1 : 0, req->assoclen,
-					req->assoclen + ivsize + 1, 0);
+					assoclen ? 1 : 0, assoclen,
+					assoclen + ivsize + 1, 0);
 		chcr_req->sec_cpl.cipherstop_lo_authinsert =
-			FILL_SEC_CPL_AUTHINSERT(0, req->assoclen + ivsize + 1,
+			FILL_SEC_CPL_AUTHINSERT(0, assoclen + ivsize + 1,
 						tag_offset, tag_offset);
 		chcr_req->sec_cpl.seqno_numivs =
 			FILL_SEC_CPL_SCMD0_SEQNO(op_type, (op_type ==
@@ -1938,19 +2537,19 @@ static struct sk_buff *create_gcm_wr(struct aead_request *req,
 	sg_param.nents = reqctx->dst_nents;
 	sg_param.obsize = req->cryptlen + (op_type ? -authsize : authsize);
 	sg_param.qid = qid;
-	sg_param.align = 0;
-	if (map_writesg_phys_cpl(&u_ctx->lldi.pdev->dev, phys_cpl, reqctx->dst,
-				  &sg_param))
+	error = map_writesg_phys_cpl(&u_ctx->lldi.pdev->dev, phys_cpl,
+					  reqctx->dst, &sg_param);
+	if (error)
 		goto dstmap_fail;
 
 	skb_set_transport_header(skb, transhdr_len);
-
-	write_sg_to_skb(skb, &frags, req->src, req->assoclen);
-
+	write_sg_to_skb(skb, &frags, req->src, assoclen);
 	write_buffer_to_skb(skb, &frags, reqctx->iv, ivsize);
 	write_sg_to_skb(skb, &frags, src, req->cryptlen);
-	create_wreq(ctx, chcr_req, req, skb, kctx_len, size, 1,
-			sizeof(struct cpl_rx_phys_dsgl) + dst_size);
+	atomic_inc(&adap->chcr_stats.aead_rqst);
+	create_wreq(ctx, chcr_req, &req->base, skb, kctx_len, size, 1,
+			sizeof(struct cpl_rx_phys_dsgl) + dst_size,
+			reqctx->verify);
 	reqctx->skb = skb;
 	skb_get(skb);
 	return skb;
@@ -1958,9 +2557,10 @@ static struct sk_buff *create_gcm_wr(struct aead_request *req,
 dstmap_fail:
 	/* ivmap_fail: */
 	kfree_skb(skb);
-	skb = NULL;
 err:
-	return skb;
+	free_new_sg(reqctx->newdstsg);
+	reqctx->newdstsg = NULL;
+	return ERR_PTR(error);
 }
 
 
@@ -1972,7 +2572,8 @@ static int chcr_aead_cra_init(struct crypto_aead *tfm)
 	struct aead_alg *alg = crypto_aead_alg(tfm);
 
 	aeadctx->sw_cipher = crypto_alloc_aead(alg->base.cra_name, 0,
-					       CRYPTO_ALG_NEED_FALLBACK);
+					       CRYPTO_ALG_NEED_FALLBACK |
+					       CRYPTO_ALG_ASYNC);
 	if  (IS_ERR(aeadctx->sw_cipher))
 		return PTR_ERR(aeadctx->sw_cipher);
 	crypto_aead_set_reqsize(tfm, max(sizeof(struct chcr_aead_reqctx),
@@ -2206,7 +2807,8 @@ static int chcr_aead_rfc4309_setkey(struct crypto_aead *aead, const u8 *key,
 				    unsigned int keylen)
 {
 	struct chcr_context *ctx = crypto_aead_ctx(aead);
-	 struct chcr_aead_ctx *aeadctx = AEAD_CTX(ctx);
+	struct chcr_aead_ctx *aeadctx = AEAD_CTX(ctx);
+	int error;
 
 	if (keylen < 3) {
 		crypto_tfm_set_flags((struct crypto_tfm *)aead,
@@ -2214,6 +2816,15 @@ static int chcr_aead_rfc4309_setkey(struct crypto_aead *aead, const u8 *key,
 		aeadctx->enckey_len = 0;
 		return	-EINVAL;
 	}
+	crypto_aead_clear_flags(aeadctx->sw_cipher, CRYPTO_TFM_REQ_MASK);
+	crypto_aead_set_flags(aeadctx->sw_cipher, crypto_aead_get_flags(aead) &
+			      CRYPTO_TFM_REQ_MASK);
+	error = crypto_aead_setkey(aeadctx->sw_cipher, key, keylen);
+	crypto_aead_clear_flags(aead, CRYPTO_TFM_RES_MASK);
+	crypto_aead_set_flags(aead, crypto_aead_get_flags(aeadctx->sw_cipher) &
+			      CRYPTO_TFM_RES_MASK);
+	if (error)
+		return error;
 	keylen -= 3;
 	memcpy(aeadctx->salt, key + keylen, 3);
 	return chcr_ccm_common_setkey(aead, key, keylen);
@@ -2552,22 +3163,14 @@ static int chcr_aead_op(struct aead_request *req,
 static struct chcr_alg_template driver_algs[] = {
 	/* AES-CBC */
 	{
-		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_SUB_TYPE_CBC,
 		.is_registered = 0,
 		.alg.crypto = {
 			.cra_name		= "cbc(aes)",
 			.cra_driver_name	= "cbc-aes-chcr",
-			.cra_priority		= CHCR_CRA_PRIORITY,
-			.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER |
-				CRYPTO_ALG_ASYNC,
 			.cra_blocksize		= AES_BLOCK_SIZE,
-			.cra_ctxsize		= sizeof(struct chcr_context)
-				+ sizeof(struct ablk_ctx),
-			.cra_alignmask		= 0,
-			.cra_type		= &crypto_ablkcipher_type,
-			.cra_module		= THIS_MODULE,
 			.cra_init		= chcr_cra_init,
-			.cra_exit		= NULL,
+			.cra_exit		= chcr_cra_exit,
 			.cra_u.ablkcipher	= {
 				.min_keysize	= AES_MIN_KEY_SIZE,
 				.max_keysize	= AES_MAX_KEY_SIZE,
@@ -2579,24 +3182,15 @@ static struct chcr_alg_template driver_algs[] = {
 		}
 	},
 	{
-		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_SUB_TYPE_XTS,
 		.is_registered = 0,
 		.alg.crypto =   {
 			.cra_name		= "xts(aes)",
 			.cra_driver_name	= "xts-aes-chcr",
-			.cra_priority		= CHCR_CRA_PRIORITY,
-			.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER |
-				CRYPTO_ALG_ASYNC,
 			.cra_blocksize		= AES_BLOCK_SIZE,
-			.cra_ctxsize		= sizeof(struct chcr_context) +
-				sizeof(struct ablk_ctx),
-			.cra_alignmask		= 0,
-			.cra_type		= &crypto_ablkcipher_type,
-			.cra_module		= THIS_MODULE,
 			.cra_init		= chcr_cra_init,
 			.cra_exit		= NULL,
-			.cra_u = {
-				.ablkcipher = {
+			.cra_u .ablkcipher = {
 					.min_keysize	= 2 * AES_MIN_KEY_SIZE,
 					.max_keysize	= 2 * AES_MAX_KEY_SIZE,
 					.ivsize		= AES_BLOCK_SIZE,
@@ -2605,6 +3199,47 @@ static struct chcr_alg_template driver_algs[] = {
 					.decrypt	= chcr_aes_decrypt,
 				}
 			}
+	},
+	{
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_SUB_TYPE_CTR,
+		.is_registered = 0,
+		.alg.crypto = {
+			.cra_name		= "ctr(aes)",
+			.cra_driver_name	= "ctr-aes-chcr",
+			.cra_blocksize		= 1,
+			.cra_init		= chcr_cra_init,
+			.cra_exit		= chcr_cra_exit,
+			.cra_u.ablkcipher	= {
+				.min_keysize	= AES_MIN_KEY_SIZE,
+				.max_keysize	= AES_MAX_KEY_SIZE,
+				.ivsize		= AES_BLOCK_SIZE,
+				.setkey		= chcr_aes_ctr_setkey,
+				.encrypt	= chcr_aes_encrypt,
+				.decrypt	= chcr_aes_decrypt,
+			}
+		}
+	},
+	{
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER |
+			CRYPTO_ALG_SUB_TYPE_CTR_RFC3686,
+		.is_registered = 0,
+		.alg.crypto = {
+			.cra_name		= "rfc3686(ctr(aes))",
+			.cra_driver_name	= "rfc3686-ctr-aes-chcr",
+			.cra_blocksize		= 1,
+			.cra_init		= chcr_rfc3686_init,
+			.cra_exit		= chcr_cra_exit,
+			.cra_u.ablkcipher	= {
+				.min_keysize	= AES_MIN_KEY_SIZE +
+					CTR_RFC3686_NONCE_SIZE,
+				.max_keysize	= AES_MAX_KEY_SIZE +
+					CTR_RFC3686_NONCE_SIZE,
+				.ivsize		= CTR_RFC3686_IV_SIZE,
+				.setkey		= chcr_aes_rfc3686_setkey,
+				.encrypt	= chcr_aes_encrypt,
+				.decrypt	= chcr_aes_decrypt,
+				.geniv          = "seqiv",
+			}
 		}
 	},
 	/* SHA */
@@ -2986,6 +3621,18 @@ static int chcr_register_alg(void)
 			continue;
 		switch (driver_algs[i].type & CRYPTO_ALG_TYPE_MASK) {
 		case CRYPTO_ALG_TYPE_ABLKCIPHER:
+			driver_algs[i].alg.crypto.cra_priority =
+				CHCR_CRA_PRIORITY;
+			driver_algs[i].alg.crypto.cra_module = THIS_MODULE;
+			driver_algs[i].alg.crypto.cra_flags =
+				CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC |
+				CRYPTO_ALG_NEED_FALLBACK;
+			driver_algs[i].alg.crypto.cra_ctxsize =
+				sizeof(struct chcr_context) +
+				sizeof(struct ablk_ctx);
+			driver_algs[i].alg.crypto.cra_alignmask = 0;
+			driver_algs[i].alg.crypto.cra_type =
+				&crypto_ablkcipher_type;
 			err = crypto_register_alg(&driver_algs[i].alg.crypto);
 			name = driver_algs[i].alg.crypto.cra_driver_name;
 			break;
diff --git a/drivers/crypto/chelsio/chcr_algo.h b/drivers/crypto/chelsio/chcr_algo.h
index 751d06a58101..583008de51a3 100644
--- a/drivers/crypto/chelsio/chcr_algo.h
+++ b/drivers/crypto/chelsio/chcr_algo.h
@@ -185,11 +185,11 @@
 			FW_CRYPTO_LOOKASIDE_WR_CCTX_LOC_V(1) | \
 			FW_CRYPTO_LOOKASIDE_WR_CCTX_SIZE_V((ctx_len)))
 
-#define FILL_WR_RX_Q_ID(cid, qid, wr_iv, fid) \
+#define FILL_WR_RX_Q_ID(cid, qid, wr_iv, lcb, fid) \
 		htonl( \
 			FW_CRYPTO_LOOKASIDE_WR_RX_CHID_V((cid)) | \
 			FW_CRYPTO_LOOKASIDE_WR_RX_Q_ID_V((qid)) | \
-			FW_CRYPTO_LOOKASIDE_WR_LCB_V(0) | \
+			FW_CRYPTO_LOOKASIDE_WR_LCB_V((lcb)) | \
 			FW_CRYPTO_LOOKASIDE_WR_IV_V((wr_iv)) | \
 			FW_CRYPTO_LOOKASIDE_WR_FQIDX_V(fid))
 
@@ -219,9 +219,26 @@
 #define MAX_NK 8
 #define CRYPTO_MAX_IMM_TX_PKT_LEN 256
 #define MAX_WR_SIZE			512
+#define ROUND_16(bytes)		((bytes) & 0xFFFFFFF0)
+#define MAX_DSGL_ENT			32
+#define MAX_DIGEST_SKB_SGE	(MAX_SKB_FRAGS - 2)
+#define MIN_CIPHER_SG			1 /* IV */
 #define MIN_AUTH_SG			2 /*IV + AAD*/
 #define MIN_GCM_SG			2 /* IV + AAD*/
+#define MIN_DIGEST_SG			1 /*Partial Buffer*/
 #define MIN_CCM_SG			3 /*IV+AAD+B0*/
+#define SPACE_LEFT(len) \
+	((MAX_WR_SIZE - WR_MIN_LEN - (len)))
+
+unsigned int sgl_ent_len[] = {0, 0, 16, 24, 40,
+				48, 64, 72, 88,
+				96, 112, 120, 136,
+				144, 160, 168, 184,
+				192};
+unsigned int dsgl_ent_len[] = {0, 32, 32, 48, 48, 64, 64, 80, 80,
+				112, 112, 128, 128, 144, 144, 160, 160,
+				192, 192, 208, 208, 224, 224, 240, 240,
+				272, 272, 288, 288, 304, 304, 320, 320};
 
 struct algo_param {
 	unsigned int auth_mode;
@@ -239,6 +256,14 @@ struct hash_wr_param {
 	u64 scmd1;
 };
 
+struct cipher_wr_param {
+	struct ablkcipher_request *req;
+	struct scatterlist *srcsg;
+	char *iv;
+	int bytes;
+	short int snent;
+	unsigned short qid;
+};
 enum {
 	AES_KEYLENGTH_128BIT = 128,
 	AES_KEYLENGTH_192BIT = 192,
@@ -293,7 +318,6 @@ struct phys_sge_parm {
 	unsigned int nents;
 	unsigned int obsize;
 	unsigned short qid;
-	unsigned char align;
 };
 
 struct crypto_result {
diff --git a/drivers/crypto/chelsio/chcr_core.c b/drivers/crypto/chelsio/chcr_core.c
index c28e018e0773..b6dd9cbe815f 100644
--- a/drivers/crypto/chelsio/chcr_core.c
+++ b/drivers/crypto/chelsio/chcr_core.c
@@ -29,6 +29,7 @@
 static LIST_HEAD(uld_ctx_list);
 static DEFINE_MUTEX(dev_mutex);
 static atomic_t dev_count;
+static struct uld_ctx *ctx_rr;
 
 typedef int (*chcr_handler_func)(struct chcr_dev *dev, unsigned char *input);
 static int cpl_fw6_pld_handler(struct chcr_dev *dev, unsigned char *input);
@@ -49,25 +50,28 @@ static struct cxgb4_uld_info chcr_uld_info = {
 	.rx_handler = chcr_uld_rx_handler,
 };
 
-int assign_chcr_device(struct chcr_dev **dev)
+struct uld_ctx *assign_chcr_device(void)
 {
-	struct uld_ctx *u_ctx;
-	int ret = -ENXIO;
+	struct uld_ctx *u_ctx = NULL;
 
 	/*
-	 * Which device to use if multiple devices are available TODO
-	 * May be select the device based on round robin. One session
-	 * must go to the same device to maintain the ordering.
+	 * When multiple devices are present in system select
+	 * device in round-robin fashion for crypto operations
+	 * Although One session must use the same device to
+	 * maintain request-response ordering.
 	 */
-	mutex_lock(&dev_mutex); /* TODO ? */
-	list_for_each_entry(u_ctx, &uld_ctx_list, entry)
-		if (u_ctx->dev) {
-			*dev = u_ctx->dev;
-			ret = 0;
-			break;
+	mutex_lock(&dev_mutex);
+	if (!list_empty(&uld_ctx_list)) {
+		u_ctx = ctx_rr;
+		if (list_is_last(&ctx_rr->entry, &uld_ctx_list))
+			ctx_rr = list_first_entry(&uld_ctx_list,
+						  struct uld_ctx,
+						  entry);
+		else
+			ctx_rr = list_next_entry(ctx_rr, entry);
 	}
 	mutex_unlock(&dev_mutex);
-	return ret;
+	return u_ctx;
 }
 
 static int chcr_dev_add(struct uld_ctx *u_ctx)
@@ -82,11 +86,27 @@ static int chcr_dev_add(struct uld_ctx *u_ctx)
 	u_ctx->dev = dev;
 	dev->u_ctx = u_ctx;
 	atomic_inc(&dev_count);
+	mutex_lock(&dev_mutex);
+	list_add_tail(&u_ctx->entry, &uld_ctx_list);
+	if (!ctx_rr)
+		ctx_rr = u_ctx;
+	mutex_unlock(&dev_mutex);
 	return 0;
 }
 
 static int chcr_dev_remove(struct uld_ctx *u_ctx)
 {
+	if (ctx_rr == u_ctx) {
+		if (list_is_last(&ctx_rr->entry, &uld_ctx_list))
+			ctx_rr = list_first_entry(&uld_ctx_list,
+						  struct uld_ctx,
+						  entry);
+		else
+			ctx_rr = list_next_entry(ctx_rr, entry);
+	}
+	list_del(&u_ctx->entry);
+	if (list_empty(&uld_ctx_list))
+		ctx_rr = NULL;
 	kfree(u_ctx->dev);
 	u_ctx->dev = NULL;
 	atomic_dec(&dev_count);
@@ -100,6 +120,7 @@ static int cpl_fw6_pld_handler(struct chcr_dev *dev,
 	struct cpl_fw6_pld *fw6_pld;
 	u32 ack_err_status = 0;
 	int error_status = 0;
+	struct adapter *adap = padap(dev);
 
 	fw6_pld = (struct cpl_fw6_pld *)input;
 	req = (struct crypto_async_request *)(uintptr_t)be64_to_cpu(
@@ -111,11 +132,11 @@ static int cpl_fw6_pld_handler(struct chcr_dev *dev,
 		if (CHK_MAC_ERR_BIT(ack_err_status) ||
 		    CHK_PAD_ERR_BIT(ack_err_status))
 			error_status = -EBADMSG;
+		atomic_inc(&adap->chcr_stats.error);
 	}
 	/* call completion callback with failure status */
 	if (req) {
 		error_status = chcr_handle_resp(req, input, error_status);
-		req->complete(req, error_status);
 	} else {
 		pr_err("Incorrect request address from the firmware\n");
 		return -EFAULT;
@@ -138,10 +159,11 @@ static void *chcr_uld_add(const struct cxgb4_lld_info *lld)
 		u_ctx = ERR_PTR(-ENOMEM);
 		goto out;
 	}
+	if (!(lld->ulp_crypto & ULP_CRYPTO_LOOKASIDE)) {
+		u_ctx = ERR_PTR(-ENOMEM);
+		goto out;
+	}
 	u_ctx->lldi = *lld;
-	mutex_lock(&dev_mutex);
-	list_add_tail(&u_ctx->entry, &uld_ctx_list);
-	mutex_unlock(&dev_mutex);
 out:
 	return u_ctx;
 }
diff --git a/drivers/crypto/chelsio/chcr_core.h b/drivers/crypto/chelsio/chcr_core.h
index cd0c35a18d92..c9a19b2a1e9f 100644
--- a/drivers/crypto/chelsio/chcr_core.h
+++ b/drivers/crypto/chelsio/chcr_core.h
@@ -53,6 +53,9 @@
 #define MAC_ERROR_BIT		0
 #define CHK_MAC_ERR_BIT(x)	(((x) >> MAC_ERROR_BIT) & 1)
 #define MAX_SALT                4
+#define WR_MIN_LEN (sizeof(struct chcr_wr) + \
+		    sizeof(struct cpl_rx_phys_dsgl) + \
+		    sizeof(struct ulptx_sgl))
 
 #define padap(dev) pci_get_drvdata(dev->u_ctx->lldi.pdev)
 
@@ -86,7 +89,7 @@ struct uld_ctx {
 	struct chcr_dev *dev;
 };
 
-int assign_chcr_device(struct chcr_dev **dev);
+struct uld_ctx * assign_chcr_device(void);
 int chcr_send_wr(struct sk_buff *skb);
 int start_crypto(void);
 int stop_crypto(void);
diff --git a/drivers/crypto/chelsio/chcr_crypto.h b/drivers/crypto/chelsio/chcr_crypto.h
index 5b2fabb14229..a4f95b014b46 100644
--- a/drivers/crypto/chelsio/chcr_crypto.h
+++ b/drivers/crypto/chelsio/chcr_crypto.h
@@ -139,6 +139,9 @@
 #define CRYPTO_ALG_SUB_TYPE_AEAD_RFC4309    0x06000000
 #define CRYPTO_ALG_SUB_TYPE_AEAD_NULL       0x07000000
 #define CRYPTO_ALG_SUB_TYPE_CTR             0x08000000
+#define CRYPTO_ALG_SUB_TYPE_CTR_RFC3686     0x09000000
+#define CRYPTO_ALG_SUB_TYPE_XTS		    0x0a000000
+#define CRYPTO_ALG_SUB_TYPE_CBC		    0x0b000000
 #define CRYPTO_ALG_TYPE_HMAC (CRYPTO_ALG_TYPE_AHASH |\
 			      CRYPTO_ALG_SUB_TYPE_HASH_HMAC)
 
@@ -146,19 +149,23 @@
 
 #define CHCR_HASH_MAX_BLOCK_SIZE_64  64
 #define CHCR_HASH_MAX_BLOCK_SIZE_128 128
+#define CHCR_SG_SIZE 2048
 
 /* Aligned to 128 bit boundary */
 
 struct ablk_ctx {
+	struct crypto_skcipher *sw_cipher;
 	__be32 key_ctx_hdr;
 	unsigned int enckey_len;
-	u8 key[CHCR_AES_MAX_KEY_LEN];
 	unsigned char ciph_mode;
+	u8 key[CHCR_AES_MAX_KEY_LEN];
+	u8 nonce[4];
 	u8 rrkey[AES_MAX_KEY_SIZE];
 };
 struct chcr_aead_reqctx {
 	struct	sk_buff	*skb;
 	struct scatterlist *dst;
+	struct scatterlist *newdstsg;
 	struct scatterlist srcffwd[2];
 	struct scatterlist dstffwd[2];
 	short int dst_nents;
@@ -233,7 +240,14 @@ struct chcr_ahash_req_ctx {
 
 struct chcr_blkcipher_req_ctx {
 	struct sk_buff *skb;
-	unsigned int dst_nents;
+	struct scatterlist srcffwd[2];
+	struct scatterlist dstffwd[2];
+	struct scatterlist *dstsg;
+	struct scatterlist *dst;
+	struct scatterlist *newdstsg;
+	unsigned int processed;
+	unsigned int op;
+	short int dst_nents;
 	u8 iv[CHCR_MAX_CRYPTO_IV_LEN];
 };
 
@@ -275,5 +289,10 @@ static int chcr_aead_op(struct aead_request *req_base,
 			  int size,
 			  create_wr_t create_wr_fn);
 static inline int get_aead_subtype(struct crypto_aead *aead);
-
+static int is_newsg(struct scatterlist *sgl, unsigned int *newents);
+static struct scatterlist *alloc_new_sg(struct scatterlist *sgl,
+					unsigned int nents);
+static inline void free_new_sg(struct scatterlist *sgl);
+static int chcr_handle_cipher_resp(struct ablkcipher_request *req,
+				   unsigned char *input, int err);
 #endif /* __CHCR_CRYPTO_H__ */
diff --git a/drivers/crypto/img-hash.c b/drivers/crypto/img-hash.c
index 9b07f3d88feb..0c6a917a9ab8 100644
--- a/drivers/crypto/img-hash.c
+++ b/drivers/crypto/img-hash.c
@@ -1088,9 +1088,17 @@ static int img_hash_suspend(struct device *dev)
 static int img_hash_resume(struct device *dev)
 {
 	struct img_hash_dev *hdev = dev_get_drvdata(dev);
+	int ret;
 
-	clk_prepare_enable(hdev->hash_clk);
-	clk_prepare_enable(hdev->sys_clk);
+	ret = clk_prepare_enable(hdev->hash_clk);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(hdev->sys_clk);
+	if (ret) {
+		clk_disable_unprepare(hdev->hash_clk);
+		return ret;
+	}
 
 	return 0;
 }
diff --git a/drivers/crypto/inside-secure/Makefile b/drivers/crypto/inside-secure/Makefile
new file mode 100644
index 000000000000..302f07dde98c
--- /dev/null
+++ b/drivers/crypto/inside-secure/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_CRYPTO_DEV_SAFEXCEL) += crypto_safexcel.o
+crypto_safexcel-objs := safexcel.o safexcel_ring.o safexcel_cipher.o safexcel_hash.o
diff --git a/drivers/crypto/inside-secure/safexcel.c b/drivers/crypto/inside-secure/safexcel.c
new file mode 100644
index 000000000000..e7f87ac12685
--- /dev/null
+++ b/drivers/crypto/inside-secure/safexcel.c
@@ -0,0 +1,926 @@
+/*
+ * Copyright (C) 2017 Marvell
+ *
+ * Antoine Tenart <antoine.tenart@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+#include <linux/firmware.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/of_irq.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+
+#include <crypto/internal/hash.h>
+#include <crypto/internal/skcipher.h>
+
+#include "safexcel.h"
+
+static u32 max_rings = EIP197_MAX_RINGS;
+module_param(max_rings, uint, 0644);
+MODULE_PARM_DESC(max_rings, "Maximum number of rings to use.");
+
+static void eip197_trc_cache_init(struct safexcel_crypto_priv *priv)
+{
+	u32 val, htable_offset;
+	int i;
+
+	/* Enable the record cache memory access */
+	val = readl(priv->base + EIP197_CS_RAM_CTRL);
+	val &= ~EIP197_TRC_ENABLE_MASK;
+	val |= EIP197_TRC_ENABLE_0;
+	writel(val, priv->base + EIP197_CS_RAM_CTRL);
+
+	/* Clear all ECC errors */
+	writel(0, priv->base + EIP197_TRC_ECCCTRL);
+
+	/*
+	 * Make sure the cache memory is accessible by taking record cache into
+	 * reset.
+	 */
+	val = readl(priv->base + EIP197_TRC_PARAMS);
+	val |= EIP197_TRC_PARAMS_SW_RESET;
+	val &= ~EIP197_TRC_PARAMS_DATA_ACCESS;
+	writel(val, priv->base + EIP197_TRC_PARAMS);
+
+	/* Clear all records */
+	for (i = 0; i < EIP197_CS_RC_MAX; i++) {
+		u32 val, offset = EIP197_CLASSIFICATION_RAMS + i * EIP197_CS_RC_SIZE;
+
+		writel(EIP197_CS_RC_NEXT(EIP197_RC_NULL) |
+		       EIP197_CS_RC_PREV(EIP197_RC_NULL),
+		       priv->base + offset);
+
+		val = EIP197_CS_RC_NEXT(i+1) | EIP197_CS_RC_PREV(i-1);
+		if (i == 0)
+			val |= EIP197_CS_RC_PREV(EIP197_RC_NULL);
+		else if (i == EIP197_CS_RC_MAX - 1)
+			val |= EIP197_CS_RC_NEXT(EIP197_RC_NULL);
+		writel(val, priv->base + offset + sizeof(u32));
+	}
+
+	/* Clear the hash table entries */
+	htable_offset = EIP197_CS_RC_MAX * EIP197_CS_RC_SIZE;
+	for (i = 0; i < 64; i++)
+		writel(GENMASK(29, 0),
+		       priv->base + EIP197_CLASSIFICATION_RAMS + htable_offset + i * sizeof(u32));
+
+	/* Disable the record cache memory access */
+	val = readl(priv->base + EIP197_CS_RAM_CTRL);
+	val &= ~EIP197_TRC_ENABLE_MASK;
+	writel(val, priv->base + EIP197_CS_RAM_CTRL);
+
+	/* Write head and tail pointers of the record free chain */
+	val = EIP197_TRC_FREECHAIN_HEAD_PTR(0) |
+	      EIP197_TRC_FREECHAIN_TAIL_PTR(EIP197_CS_RC_MAX - 1);
+	writel(val, priv->base + EIP197_TRC_FREECHAIN);
+
+	/* Configure the record cache #1 */
+	val = EIP197_TRC_PARAMS2_RC_SZ_SMALL(EIP197_CS_TRC_REC_WC) |
+	      EIP197_TRC_PARAMS2_HTABLE_PTR(EIP197_CS_RC_MAX);
+	writel(val, priv->base + EIP197_TRC_PARAMS2);
+
+	/* Configure the record cache #2 */
+	val = EIP197_TRC_PARAMS_RC_SZ_LARGE(EIP197_CS_TRC_LG_REC_WC) |
+	      EIP197_TRC_PARAMS_BLK_TIMER_SPEED(1) |
+	      EIP197_TRC_PARAMS_HTABLE_SZ(2);
+	writel(val, priv->base + EIP197_TRC_PARAMS);
+}
+
+static void eip197_write_firmware(struct safexcel_crypto_priv *priv,
+				  const struct firmware *fw, u32 ctrl,
+				  u32 prog_en)
+{
+	const u32 *data = (const u32 *)fw->data;
+	u32 val;
+	int i;
+
+	/* Reset the engine to make its program memory accessible */
+	writel(EIP197_PE_ICE_x_CTRL_SW_RESET |
+	       EIP197_PE_ICE_x_CTRL_CLR_ECC_CORR |
+	       EIP197_PE_ICE_x_CTRL_CLR_ECC_NON_CORR,
+	       priv->base + ctrl);
+
+	/* Enable access to the program memory */
+	writel(prog_en, priv->base + EIP197_PE_ICE_RAM_CTRL);
+
+	/* Write the firmware */
+	for (i = 0; i < fw->size / sizeof(u32); i++)
+		writel(be32_to_cpu(data[i]),
+		       priv->base + EIP197_CLASSIFICATION_RAMS + i * sizeof(u32));
+
+	/* Disable access to the program memory */
+	writel(0, priv->base + EIP197_PE_ICE_RAM_CTRL);
+
+	/* Release engine from reset */
+	val = readl(priv->base + ctrl);
+	val &= ~EIP197_PE_ICE_x_CTRL_SW_RESET;
+	writel(val, priv->base + ctrl);
+}
+
+static int eip197_load_firmwares(struct safexcel_crypto_priv *priv)
+{
+	const char *fw_name[] = {"ifpp.bin", "ipue.bin"};
+	const struct firmware *fw[FW_NB];
+	int i, j, ret = 0;
+	u32 val;
+
+	for (i = 0; i < FW_NB; i++) {
+		ret = request_firmware(&fw[i], fw_name[i], priv->dev);
+		if (ret) {
+			dev_err(priv->dev,
+				"Failed to request firmware %s (%d)\n",
+				fw_name[i], ret);
+			goto release_fw;
+		}
+	 }
+
+	/* Clear the scratchpad memory */
+	val = readl(priv->base + EIP197_PE_ICE_SCRATCH_CTRL);
+	val |= EIP197_PE_ICE_SCRATCH_CTRL_CHANGE_TIMER |
+	       EIP197_PE_ICE_SCRATCH_CTRL_TIMER_EN |
+	       EIP197_PE_ICE_SCRATCH_CTRL_SCRATCH_ACCESS |
+	       EIP197_PE_ICE_SCRATCH_CTRL_CHANGE_ACCESS;
+	writel(val, priv->base + EIP197_PE_ICE_SCRATCH_CTRL);
+
+	memset(priv->base + EIP197_PE_ICE_SCRATCH_RAM, 0,
+	       EIP197_NUM_OF_SCRATCH_BLOCKS * sizeof(u32));
+
+	eip197_write_firmware(priv, fw[FW_IFPP], EIP197_PE_ICE_FPP_CTRL,
+			      EIP197_PE_ICE_RAM_CTRL_FPP_PROG_EN);
+
+	eip197_write_firmware(priv, fw[FW_IPUE], EIP197_PE_ICE_PUE_CTRL,
+			      EIP197_PE_ICE_RAM_CTRL_PUE_PROG_EN);
+
+release_fw:
+	for (j = 0; j < i; j++)
+		release_firmware(fw[j]);
+
+	return ret;
+}
+
+static int safexcel_hw_setup_cdesc_rings(struct safexcel_crypto_priv *priv)
+{
+	u32 hdw, cd_size_rnd, val;
+	int i;
+
+	hdw = readl(priv->base + EIP197_HIA_OPTIONS);
+	hdw &= GENMASK(27, 25);
+	hdw >>= 25;
+
+	cd_size_rnd = (priv->config.cd_size + (BIT(hdw) - 1)) >> hdw;
+
+	for (i = 0; i < priv->config.rings; i++) {
+		/* ring base address */
+		writel(lower_32_bits(priv->ring[i].cdr.base_dma),
+		       priv->base + EIP197_HIA_CDR(i) + EIP197_HIA_xDR_RING_BASE_ADDR_LO);
+		writel(upper_32_bits(priv->ring[i].cdr.base_dma),
+		       priv->base + EIP197_HIA_CDR(i) + EIP197_HIA_xDR_RING_BASE_ADDR_HI);
+
+		writel(EIP197_xDR_DESC_MODE_64BIT | (priv->config.cd_offset << 16) |
+		       priv->config.cd_size,
+		       priv->base + EIP197_HIA_CDR(i) + EIP197_HIA_xDR_DESC_SIZE);
+		writel(((EIP197_FETCH_COUNT * (cd_size_rnd << hdw)) << 16) |
+		       (EIP197_FETCH_COUNT * priv->config.cd_offset),
+		       priv->base + EIP197_HIA_CDR(i) + EIP197_HIA_xDR_CFG);
+
+		/* Configure DMA tx control */
+		val = EIP197_HIA_xDR_CFG_WR_CACHE(WR_CACHE_3BITS);
+		val |= EIP197_HIA_xDR_CFG_RD_CACHE(RD_CACHE_3BITS);
+		writel(val,
+		       priv->base + EIP197_HIA_CDR(i) + EIP197_HIA_xDR_DMA_CFG);
+
+		/* clear any pending interrupt */
+		writel(GENMASK(5, 0),
+		       priv->base + EIP197_HIA_CDR(i) + EIP197_HIA_xDR_STAT);
+	}
+
+	return 0;
+}
+
+static int safexcel_hw_setup_rdesc_rings(struct safexcel_crypto_priv *priv)
+{
+	u32 hdw, rd_size_rnd, val;
+	int i;
+
+	hdw = readl(priv->base + EIP197_HIA_OPTIONS);
+	hdw &= GENMASK(27, 25);
+	hdw >>= 25;
+
+	rd_size_rnd = (priv->config.rd_size + (BIT(hdw) - 1)) >> hdw;
+
+	for (i = 0; i < priv->config.rings; i++) {
+		/* ring base address */
+		writel(lower_32_bits(priv->ring[i].rdr.base_dma),
+		       priv->base + EIP197_HIA_RDR(i) + EIP197_HIA_xDR_RING_BASE_ADDR_LO);
+		writel(upper_32_bits(priv->ring[i].rdr.base_dma),
+		       priv->base + EIP197_HIA_RDR(i) + EIP197_HIA_xDR_RING_BASE_ADDR_HI);
+
+		writel(EIP197_xDR_DESC_MODE_64BIT | (priv->config.rd_offset << 16) |
+		       priv->config.rd_size,
+		       priv->base + EIP197_HIA_RDR(i) + EIP197_HIA_xDR_DESC_SIZE);
+
+		writel(((EIP197_FETCH_COUNT * (rd_size_rnd << hdw)) << 16) |
+		       (EIP197_FETCH_COUNT * priv->config.rd_offset),
+		       priv->base + EIP197_HIA_RDR(i) + EIP197_HIA_xDR_CFG);
+
+		/* Configure DMA tx control */
+		val = EIP197_HIA_xDR_CFG_WR_CACHE(WR_CACHE_3BITS);
+		val |= EIP197_HIA_xDR_CFG_RD_CACHE(RD_CACHE_3BITS);
+		val |= EIP197_HIA_xDR_WR_RES_BUF | EIP197_HIA_xDR_WR_CTRL_BUG;
+		writel(val,
+		       priv->base + EIP197_HIA_RDR(i) + EIP197_HIA_xDR_DMA_CFG);
+
+		/* clear any pending interrupt */
+		writel(GENMASK(7, 0),
+		       priv->base + EIP197_HIA_RDR(i) + EIP197_HIA_xDR_STAT);
+
+		/* enable ring interrupt */
+		val = readl(priv->base + EIP197_HIA_AIC_R_ENABLE_CTRL(i));
+		val |= EIP197_RDR_IRQ(i);
+		writel(val, priv->base + EIP197_HIA_AIC_R_ENABLE_CTRL(i));
+	}
+
+	return 0;
+}
+
+static int safexcel_hw_init(struct safexcel_crypto_priv *priv)
+{
+	u32 version, val;
+	int i, ret;
+
+	/* Determine endianess and configure byte swap */
+	version = readl(priv->base + EIP197_HIA_VERSION);
+	val = readl(priv->base + EIP197_HIA_MST_CTRL);
+
+	if ((version & 0xffff) == EIP197_HIA_VERSION_BE)
+		val |= EIP197_MST_CTRL_BYTE_SWAP;
+	else if (((version >> 16) & 0xffff) == EIP197_HIA_VERSION_LE)
+		val |= (EIP197_MST_CTRL_NO_BYTE_SWAP >> 24);
+
+	writel(val, priv->base + EIP197_HIA_MST_CTRL);
+
+
+	/* Configure wr/rd cache values */
+	writel(EIP197_MST_CTRL_RD_CACHE(RD_CACHE_4BITS) |
+	       EIP197_MST_CTRL_WD_CACHE(WR_CACHE_4BITS),
+	       priv->base + EIP197_MST_CTRL);
+
+	/* Interrupts reset */
+
+	/* Disable all global interrupts */
+	writel(0, priv->base + EIP197_HIA_AIC_G_ENABLE_CTRL);
+
+	/* Clear any pending interrupt */
+	writel(GENMASK(31, 0), priv->base + EIP197_HIA_AIC_G_ACK);
+
+	/* Data Fetch Engine configuration */
+
+	/* Reset all DFE threads */
+	writel(EIP197_DxE_THR_CTRL_RESET_PE,
+	       priv->base + EIP197_HIA_DFE_THR_CTRL);
+
+	/* Reset HIA input interface arbiter */
+	writel(EIP197_HIA_RA_PE_CTRL_RESET,
+	       priv->base + EIP197_HIA_RA_PE_CTRL);
+
+	/* DMA transfer size to use */
+	val = EIP197_HIA_DFE_CFG_DIS_DEBUG;
+	val |= EIP197_HIA_DxE_CFG_MIN_DATA_SIZE(5) | EIP197_HIA_DxE_CFG_MAX_DATA_SIZE(9);
+	val |= EIP197_HIA_DxE_CFG_MIN_CTRL_SIZE(5) | EIP197_HIA_DxE_CFG_MAX_CTRL_SIZE(7);
+	val |= EIP197_HIA_DxE_CFG_DATA_CACHE_CTRL(RD_CACHE_3BITS);
+	val |= EIP197_HIA_DxE_CFG_CTRL_CACHE_CTRL(RD_CACHE_3BITS);
+	writel(val, priv->base + EIP197_HIA_DFE_CFG);
+
+	/* Leave the DFE threads reset state */
+	writel(0, priv->base + EIP197_HIA_DFE_THR_CTRL);
+
+	/* Configure the procesing engine thresholds */
+	writel(EIP197_PE_IN_xBUF_THRES_MIN(5) | EIP197_PE_IN_xBUF_THRES_MAX(9),
+	      priv->base + EIP197_PE_IN_DBUF_THRES);
+	writel(EIP197_PE_IN_xBUF_THRES_MIN(5) | EIP197_PE_IN_xBUF_THRES_MAX(7),
+	      priv->base + EIP197_PE_IN_TBUF_THRES);
+
+	/* enable HIA input interface arbiter and rings */
+	writel(EIP197_HIA_RA_PE_CTRL_EN | GENMASK(priv->config.rings - 1, 0),
+	       priv->base + EIP197_HIA_RA_PE_CTRL);
+
+	/* Data Store Engine configuration */
+
+	/* Reset all DSE threads */
+	writel(EIP197_DxE_THR_CTRL_RESET_PE,
+	       priv->base + EIP197_HIA_DSE_THR_CTRL);
+
+	/* Wait for all DSE threads to complete */
+	while ((readl(priv->base + EIP197_HIA_DSE_THR_STAT) &
+		GENMASK(15, 12)) != GENMASK(15, 12))
+		;
+
+	/* DMA transfer size to use */
+	val = EIP197_HIA_DSE_CFG_DIS_DEBUG;
+	val |= EIP197_HIA_DxE_CFG_MIN_DATA_SIZE(7) | EIP197_HIA_DxE_CFG_MAX_DATA_SIZE(8);
+	val |= EIP197_HIA_DxE_CFG_DATA_CACHE_CTRL(WR_CACHE_3BITS);
+	val |= EIP197_HIA_DSE_CFG_ALLWAYS_BUFFERABLE;
+	val |= EIP197_HIA_DSE_CFG_EN_SINGLE_WR;
+	writel(val, priv->base + EIP197_HIA_DSE_CFG);
+
+	/* Leave the DSE threads reset state */
+	writel(0, priv->base + EIP197_HIA_DSE_THR_CTRL);
+
+	/* Configure the procesing engine thresholds */
+	writel(EIP197_PE_OUT_DBUF_THRES_MIN(7) | EIP197_PE_OUT_DBUF_THRES_MAX(8),
+	       priv->base + EIP197_PE_OUT_DBUF_THRES);
+
+	/* Processing Engine configuration */
+
+	/* H/W capabilities selection */
+	val = EIP197_FUNCTION_RSVD;
+	val |= EIP197_PROTOCOL_ENCRYPT_ONLY | EIP197_PROTOCOL_HASH_ONLY;
+	val |= EIP197_ALG_AES_ECB | EIP197_ALG_AES_CBC;
+	val |= EIP197_ALG_SHA1 | EIP197_ALG_HMAC_SHA1;
+	val |= EIP197_ALG_SHA2;
+	writel(val, priv->base + EIP197_PE_EIP96_FUNCTION_EN);
+
+	/* Command Descriptor Rings prepare */
+	for (i = 0; i < priv->config.rings; i++) {
+		/* Clear interrupts for this ring */
+		writel(GENMASK(31, 0),
+		       priv->base + EIP197_HIA_AIC_R_ENABLE_CLR(i));
+
+		/* Disable external triggering */
+		writel(0, priv->base + EIP197_HIA_CDR(i) + EIP197_HIA_xDR_CFG);
+
+		/* Clear the pending prepared counter */
+		writel(EIP197_xDR_PREP_CLR_COUNT,
+		       priv->base + EIP197_HIA_CDR(i) + EIP197_HIA_xDR_PREP_COUNT);
+
+		/* Clear the pending processed counter */
+		writel(EIP197_xDR_PROC_CLR_COUNT,
+		       priv->base + EIP197_HIA_CDR(i) + EIP197_HIA_xDR_PROC_COUNT);
+
+		writel(0,
+		       priv->base + EIP197_HIA_CDR(i) + EIP197_HIA_xDR_PREP_PNTR);
+		writel(0,
+		       priv->base + EIP197_HIA_CDR(i) + EIP197_HIA_xDR_PROC_PNTR);
+
+		writel((EIP197_DEFAULT_RING_SIZE * priv->config.cd_offset) << 2,
+		       priv->base + EIP197_HIA_CDR(i) + EIP197_HIA_xDR_RING_SIZE);
+	}
+
+	/* Result Descriptor Ring prepare */
+	for (i = 0; i < priv->config.rings; i++) {
+		/* Disable external triggering*/
+		writel(0, priv->base + EIP197_HIA_RDR(i) + EIP197_HIA_xDR_CFG);
+
+		/* Clear the pending prepared counter */
+		writel(EIP197_xDR_PREP_CLR_COUNT,
+		       priv->base + EIP197_HIA_RDR(i) + EIP197_HIA_xDR_PREP_COUNT);
+
+		/* Clear the pending processed counter */
+		writel(EIP197_xDR_PROC_CLR_COUNT,
+		       priv->base + EIP197_HIA_RDR(i) + EIP197_HIA_xDR_PROC_COUNT);
+
+		writel(0,
+		       priv->base + EIP197_HIA_RDR(i) + EIP197_HIA_xDR_PREP_PNTR);
+		writel(0,
+		       priv->base + EIP197_HIA_RDR(i) + EIP197_HIA_xDR_PROC_PNTR);
+
+		/* Ring size */
+		writel((EIP197_DEFAULT_RING_SIZE * priv->config.rd_offset) << 2,
+		       priv->base + EIP197_HIA_RDR(i) + EIP197_HIA_xDR_RING_SIZE);
+	}
+
+	/* Enable command descriptor rings */
+	writel(EIP197_DxE_THR_CTRL_EN | GENMASK(priv->config.rings - 1, 0),
+	       priv->base + EIP197_HIA_DFE_THR_CTRL);
+
+	/* Enable result descriptor rings */
+	writel(EIP197_DxE_THR_CTRL_EN | GENMASK(priv->config.rings - 1, 0),
+	       priv->base + EIP197_HIA_DSE_THR_CTRL);
+
+	/* Clear any HIA interrupt */
+	writel(GENMASK(30, 20), priv->base + EIP197_HIA_AIC_G_ACK);
+
+	eip197_trc_cache_init(priv);
+
+	ret = eip197_load_firmwares(priv);
+	if (ret)
+		return ret;
+
+	safexcel_hw_setup_cdesc_rings(priv);
+	safexcel_hw_setup_rdesc_rings(priv);
+
+	return 0;
+}
+
+void safexcel_dequeue(struct safexcel_crypto_priv *priv, int ring)
+{
+	struct crypto_async_request *req, *backlog;
+	struct safexcel_context *ctx;
+	struct safexcel_request *request;
+	int ret, nreq = 0, cdesc = 0, rdesc = 0, commands, results;
+
+	priv->ring[ring].need_dequeue = false;
+
+	do {
+		spin_lock_bh(&priv->ring[ring].queue_lock);
+		backlog = crypto_get_backlog(&priv->ring[ring].queue);
+		req = crypto_dequeue_request(&priv->ring[ring].queue);
+		spin_unlock_bh(&priv->ring[ring].queue_lock);
+
+		if (!req)
+			goto finalize;
+
+		request = kzalloc(sizeof(*request), EIP197_GFP_FLAGS(*req));
+		if (!request) {
+			spin_lock_bh(&priv->ring[ring].queue_lock);
+			crypto_enqueue_request(&priv->ring[ring].queue, req);
+			spin_unlock_bh(&priv->ring[ring].queue_lock);
+
+			priv->ring[ring].need_dequeue = true;
+			goto finalize;
+		}
+
+		ctx = crypto_tfm_ctx(req->tfm);
+		ret = ctx->send(req, ring, request, &commands, &results);
+		if (ret) {
+			kfree(request);
+			req->complete(req, ret);
+			priv->ring[ring].need_dequeue = true;
+			goto finalize;
+		}
+
+		if (backlog)
+			backlog->complete(backlog, -EINPROGRESS);
+
+		spin_lock_bh(&priv->ring[ring].egress_lock);
+		list_add_tail(&request->list, &priv->ring[ring].list);
+		spin_unlock_bh(&priv->ring[ring].egress_lock);
+
+		cdesc += commands;
+		rdesc += results;
+	} while (nreq++ < EIP197_MAX_BATCH_SZ);
+
+finalize:
+	if (nreq == EIP197_MAX_BATCH_SZ)
+		priv->ring[ring].need_dequeue = true;
+	else if (!nreq)
+		return;
+
+	spin_lock_bh(&priv->ring[ring].lock);
+
+	/* Configure when we want an interrupt */
+	writel(EIP197_HIA_RDR_THRESH_PKT_MODE |
+	       EIP197_HIA_RDR_THRESH_PROC_PKT(nreq),
+	       priv->base + EIP197_HIA_RDR(ring) + EIP197_HIA_xDR_THRESH);
+
+	/* let the RDR know we have pending descriptors */
+	writel((rdesc * priv->config.rd_offset) << 2,
+	       priv->base + EIP197_HIA_RDR(ring) + EIP197_HIA_xDR_PREP_COUNT);
+
+	/* let the CDR know we have pending descriptors */
+	writel((cdesc * priv->config.cd_offset) << 2,
+	       priv->base + EIP197_HIA_CDR(ring) + EIP197_HIA_xDR_PREP_COUNT);
+
+	spin_unlock_bh(&priv->ring[ring].lock);
+}
+
+void safexcel_free_context(struct safexcel_crypto_priv *priv,
+			   struct crypto_async_request *req,
+			   int result_sz)
+{
+	struct safexcel_context *ctx = crypto_tfm_ctx(req->tfm);
+
+	if (ctx->result_dma)
+		dma_unmap_single(priv->dev, ctx->result_dma, result_sz,
+				 DMA_FROM_DEVICE);
+
+	if (ctx->cache) {
+		dma_unmap_single(priv->dev, ctx->cache_dma, ctx->cache_sz,
+				 DMA_TO_DEVICE);
+		kfree(ctx->cache);
+		ctx->cache = NULL;
+		ctx->cache_sz = 0;
+	}
+}
+
+void safexcel_complete(struct safexcel_crypto_priv *priv, int ring)
+{
+	struct safexcel_command_desc *cdesc;
+
+	/* Acknowledge the command descriptors */
+	do {
+		cdesc = safexcel_ring_next_rptr(priv, &priv->ring[ring].cdr);
+		if (IS_ERR(cdesc)) {
+			dev_err(priv->dev,
+				"Could not retrieve the command descriptor\n");
+			return;
+		}
+	} while (!cdesc->last_seg);
+}
+
+void safexcel_inv_complete(struct crypto_async_request *req, int error)
+{
+	struct safexcel_inv_result *result = req->data;
+
+	if (error == -EINPROGRESS)
+		return;
+
+	result->error = error;
+	complete(&result->completion);
+}
+
+int safexcel_invalidate_cache(struct crypto_async_request *async,
+			      struct safexcel_context *ctx,
+			      struct safexcel_crypto_priv *priv,
+			      dma_addr_t ctxr_dma, int ring,
+			      struct safexcel_request *request)
+{
+	struct safexcel_command_desc *cdesc;
+	struct safexcel_result_desc *rdesc;
+	int ret = 0;
+
+	spin_lock_bh(&priv->ring[ring].egress_lock);
+
+	/* Prepare command descriptor */
+	cdesc = safexcel_add_cdesc(priv, ring, true, true, 0, 0, 0, ctxr_dma);
+	if (IS_ERR(cdesc)) {
+		ret = PTR_ERR(cdesc);
+		goto unlock;
+	}
+
+	cdesc->control_data.type = EIP197_TYPE_EXTENDED;
+	cdesc->control_data.options = 0;
+	cdesc->control_data.refresh = 0;
+	cdesc->control_data.control0 = CONTEXT_CONTROL_INV_TR;
+
+	/* Prepare result descriptor */
+	rdesc = safexcel_add_rdesc(priv, ring, true, true, 0, 0);
+
+	if (IS_ERR(rdesc)) {
+		ret = PTR_ERR(rdesc);
+		goto cdesc_rollback;
+	}
+
+	request->req = async;
+	goto unlock;
+
+cdesc_rollback:
+	safexcel_ring_rollback_wptr(priv, &priv->ring[ring].cdr);
+
+unlock:
+	spin_unlock_bh(&priv->ring[ring].egress_lock);
+	return ret;
+}
+
+static inline void safexcel_handle_result_descriptor(struct safexcel_crypto_priv *priv,
+						     int ring)
+{
+	struct safexcel_request *sreq;
+	struct safexcel_context *ctx;
+	int ret, i, nreq, ndesc = 0;
+	bool should_complete;
+
+	nreq = readl(priv->base + EIP197_HIA_RDR(ring) + EIP197_HIA_xDR_PROC_COUNT);
+	nreq >>= 24;
+	nreq &= GENMASK(6, 0);
+	if (!nreq)
+		return;
+
+	for (i = 0; i < nreq; i++) {
+		spin_lock_bh(&priv->ring[ring].egress_lock);
+		sreq = list_first_entry(&priv->ring[ring].list,
+					struct safexcel_request, list);
+		list_del(&sreq->list);
+		spin_unlock_bh(&priv->ring[ring].egress_lock);
+
+		ctx = crypto_tfm_ctx(sreq->req->tfm);
+		ndesc = ctx->handle_result(priv, ring, sreq->req,
+					   &should_complete, &ret);
+		if (ndesc < 0) {
+			dev_err(priv->dev, "failed to handle result (%d)", ndesc);
+			return;
+		}
+
+		writel(EIP197_xDR_PROC_xD_PKT(1) |
+		       EIP197_xDR_PROC_xD_COUNT(ndesc * priv->config.rd_offset),
+		       priv->base + EIP197_HIA_RDR(ring) + EIP197_HIA_xDR_PROC_COUNT);
+
+		if (should_complete) {
+			local_bh_disable();
+			sreq->req->complete(sreq->req, ret);
+			local_bh_enable();
+		}
+
+		kfree(sreq);
+	}
+}
+
+static void safexcel_handle_result_work(struct work_struct *work)
+{
+	struct safexcel_work_data *data =
+			container_of(work, struct safexcel_work_data, work);
+	struct safexcel_crypto_priv *priv = data->priv;
+
+	safexcel_handle_result_descriptor(priv, data->ring);
+
+	if (priv->ring[data->ring].need_dequeue)
+		safexcel_dequeue(data->priv, data->ring);
+}
+
+struct safexcel_ring_irq_data {
+	struct safexcel_crypto_priv *priv;
+	int ring;
+};
+
+static irqreturn_t safexcel_irq_ring(int irq, void *data)
+{
+	struct safexcel_ring_irq_data *irq_data = data;
+	struct safexcel_crypto_priv *priv = irq_data->priv;
+	int ring = irq_data->ring;
+	u32 status, stat;
+
+	status = readl(priv->base + EIP197_HIA_AIC_R_ENABLED_STAT(ring));
+	if (!status)
+		return IRQ_NONE;
+
+	/* RDR interrupts */
+	if (status & EIP197_RDR_IRQ(ring)) {
+		stat = readl(priv->base + EIP197_HIA_RDR(ring) + EIP197_HIA_xDR_STAT);
+
+		if (unlikely(stat & EIP197_xDR_ERR)) {
+			/*
+			 * Fatal error, the RDR is unusable and must be
+			 * reinitialized. This should not happen under
+			 * normal circumstances.
+			 */
+			dev_err(priv->dev, "RDR: fatal error.");
+		} else if (likely(stat & EIP197_xDR_THRESH)) {
+			queue_work(priv->ring[ring].workqueue, &priv->ring[ring].work_data.work);
+		}
+
+		/* ACK the interrupts */
+		writel(stat & 0xff,
+		       priv->base + EIP197_HIA_RDR(ring) + EIP197_HIA_xDR_STAT);
+	}
+
+	/* ACK the interrupts */
+	writel(status, priv->base + EIP197_HIA_AIC_R_ACK(ring));
+
+	return IRQ_HANDLED;
+}
+
+static int safexcel_request_ring_irq(struct platform_device *pdev, const char *name,
+				     irq_handler_t handler,
+				     struct safexcel_ring_irq_data *ring_irq_priv)
+{
+	int ret, irq = platform_get_irq_byname(pdev, name);
+
+	if (irq < 0) {
+		dev_err(&pdev->dev, "unable to get IRQ '%s'\n", name);
+		return irq;
+	}
+
+	ret = devm_request_irq(&pdev->dev, irq, handler, 0,
+			       dev_name(&pdev->dev), ring_irq_priv);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to request IRQ %d\n", irq);
+		return ret;
+	}
+
+	return irq;
+}
+
+static struct safexcel_alg_template *safexcel_algs[] = {
+	&safexcel_alg_ecb_aes,
+	&safexcel_alg_cbc_aes,
+	&safexcel_alg_sha1,
+	&safexcel_alg_sha224,
+	&safexcel_alg_sha256,
+	&safexcel_alg_hmac_sha1,
+};
+
+static int safexcel_register_algorithms(struct safexcel_crypto_priv *priv)
+{
+	int i, j, ret = 0;
+
+	for (i = 0; i < ARRAY_SIZE(safexcel_algs); i++) {
+		safexcel_algs[i]->priv = priv;
+
+		if (safexcel_algs[i]->type == SAFEXCEL_ALG_TYPE_SKCIPHER)
+			ret = crypto_register_skcipher(&safexcel_algs[i]->alg.skcipher);
+		else
+			ret = crypto_register_ahash(&safexcel_algs[i]->alg.ahash);
+
+		if (ret)
+			goto fail;
+	}
+
+	return 0;
+
+fail:
+	for (j = 0; j < i; j++) {
+		if (safexcel_algs[j]->type == SAFEXCEL_ALG_TYPE_SKCIPHER)
+			crypto_unregister_skcipher(&safexcel_algs[j]->alg.skcipher);
+		else
+			crypto_unregister_ahash(&safexcel_algs[j]->alg.ahash);
+	}
+
+	return ret;
+}
+
+static void safexcel_unregister_algorithms(struct safexcel_crypto_priv *priv)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(safexcel_algs); i++) {
+		if (safexcel_algs[i]->type == SAFEXCEL_ALG_TYPE_SKCIPHER)
+			crypto_unregister_skcipher(&safexcel_algs[i]->alg.skcipher);
+		else
+			crypto_unregister_ahash(&safexcel_algs[i]->alg.ahash);
+	}
+}
+
+static void safexcel_configure(struct safexcel_crypto_priv *priv)
+{
+	u32 val, mask;
+
+	val = readl(priv->base + EIP197_HIA_OPTIONS);
+	val = (val & GENMASK(27, 25)) >> 25;
+	mask = BIT(val) - 1;
+
+	val = readl(priv->base + EIP197_HIA_OPTIONS);
+	priv->config.rings = min_t(u32, val & GENMASK(3, 0), max_rings);
+
+	priv->config.cd_size = (sizeof(struct safexcel_command_desc) / sizeof(u32));
+	priv->config.cd_offset = (priv->config.cd_size + mask) & ~mask;
+
+	priv->config.rd_size = (sizeof(struct safexcel_result_desc) / sizeof(u32));
+	priv->config.rd_offset = (priv->config.rd_size + mask) & ~mask;
+}
+
+static int safexcel_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	struct safexcel_crypto_priv *priv;
+	u64 dma_mask;
+	int i, ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->dev = dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	priv->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(priv->base)) {
+		dev_err(dev, "failed to get resource\n");
+		return PTR_ERR(priv->base);
+	}
+
+	priv->clk = of_clk_get(dev->of_node, 0);
+	if (!IS_ERR(priv->clk)) {
+		ret = clk_prepare_enable(priv->clk);
+		if (ret) {
+			dev_err(dev, "unable to enable clk (%d)\n", ret);
+			return ret;
+		}
+	} else {
+		/* The clock isn't mandatory */
+		if (PTR_ERR(priv->clk) == -EPROBE_DEFER)
+			return -EPROBE_DEFER;
+	}
+
+	if (of_property_read_u64(dev->of_node, "dma-mask", &dma_mask))
+		dma_mask = DMA_BIT_MASK(64);
+	ret = dma_set_mask_and_coherent(dev, dma_mask);
+	if (ret)
+		goto err_clk;
+
+	priv->context_pool = dmam_pool_create("safexcel-context", dev,
+					      sizeof(struct safexcel_context_record),
+					      1, 0);
+	if (!priv->context_pool) {
+		ret = -ENOMEM;
+		goto err_clk;
+	}
+
+	safexcel_configure(priv);
+
+	for (i = 0; i < priv->config.rings; i++) {
+		char irq_name[6] = {0}; /* "ringX\0" */
+		char wq_name[9] = {0}; /* "wq_ringX\0" */
+		int irq;
+		struct safexcel_ring_irq_data *ring_irq;
+
+		ret = safexcel_init_ring_descriptors(priv,
+						     &priv->ring[i].cdr,
+						     &priv->ring[i].rdr);
+		if (ret)
+			goto err_clk;
+
+		ring_irq = devm_kzalloc(dev, sizeof(*ring_irq), GFP_KERNEL);
+		if (!ring_irq) {
+			ret = -ENOMEM;
+			goto err_clk;
+		}
+
+		ring_irq->priv = priv;
+		ring_irq->ring = i;
+
+		snprintf(irq_name, 6, "ring%d", i);
+		irq = safexcel_request_ring_irq(pdev, irq_name, safexcel_irq_ring,
+						ring_irq);
+
+		if (irq < 0)
+			goto err_clk;
+
+		priv->ring[i].work_data.priv = priv;
+		priv->ring[i].work_data.ring = i;
+		INIT_WORK(&priv->ring[i].work_data.work, safexcel_handle_result_work);
+
+		snprintf(wq_name, 9, "wq_ring%d", i);
+		priv->ring[i].workqueue = create_singlethread_workqueue(wq_name);
+		if (!priv->ring[i].workqueue) {
+			ret = -ENOMEM;
+			goto err_clk;
+		}
+
+		crypto_init_queue(&priv->ring[i].queue,
+				  EIP197_DEFAULT_RING_SIZE);
+
+		INIT_LIST_HEAD(&priv->ring[i].list);
+		spin_lock_init(&priv->ring[i].lock);
+		spin_lock_init(&priv->ring[i].egress_lock);
+		spin_lock_init(&priv->ring[i].queue_lock);
+	}
+
+	platform_set_drvdata(pdev, priv);
+	atomic_set(&priv->ring_used, 0);
+
+	ret = safexcel_hw_init(priv);
+	if (ret) {
+		dev_err(dev, "EIP h/w init failed (%d)\n", ret);
+		goto err_clk;
+	}
+
+	ret = safexcel_register_algorithms(priv);
+	if (ret) {
+		dev_err(dev, "Failed to register algorithms (%d)\n", ret);
+		goto err_clk;
+	}
+
+	return 0;
+
+err_clk:
+	clk_disable_unprepare(priv->clk);
+	return ret;
+}
+
+
+static int safexcel_remove(struct platform_device *pdev)
+{
+	struct safexcel_crypto_priv *priv = platform_get_drvdata(pdev);
+	int i;
+
+	safexcel_unregister_algorithms(priv);
+	clk_disable_unprepare(priv->clk);
+
+	for (i = 0; i < priv->config.rings; i++)
+		destroy_workqueue(priv->ring[i].workqueue);
+
+	return 0;
+}
+
+static const struct of_device_id safexcel_of_match_table[] = {
+	{ .compatible = "inside-secure,safexcel-eip197" },
+	{},
+};
+
+
+static struct platform_driver  crypto_safexcel = {
+	.probe		= safexcel_probe,
+	.remove		= safexcel_remove,
+	.driver		= {
+		.name	= "crypto-safexcel",
+		.of_match_table = safexcel_of_match_table,
+	},
+};
+module_platform_driver(crypto_safexcel);
+
+MODULE_AUTHOR("Antoine Tenart <antoine.tenart@free-electrons.com>");
+MODULE_AUTHOR("Ofer Heifetz <oferh@marvell.com>");
+MODULE_AUTHOR("Igal Liberman <igall@marvell.com>");
+MODULE_DESCRIPTION("Support for SafeXcel cryptographic engine EIP197");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/crypto/inside-secure/safexcel.h b/drivers/crypto/inside-secure/safexcel.h
new file mode 100644
index 000000000000..304c5838c11a
--- /dev/null
+++ b/drivers/crypto/inside-secure/safexcel.h
@@ -0,0 +1,574 @@
+/*
+ * Copyright (C) 2017 Marvell
+ *
+ * Antoine Tenart <antoine.tenart@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#ifndef __SAFEXCEL_H__
+#define __SAFEXCEL_H__
+
+#include <crypto/algapi.h>
+#include <crypto/internal/hash.h>
+#include <crypto/skcipher.h>
+
+#define EIP197_HIA_VERSION_LE			0xca35
+#define EIP197_HIA_VERSION_BE			0x35ca
+
+/* Static configuration */
+#define EIP197_DEFAULT_RING_SIZE		64
+#define EIP197_MAX_TOKENS			5
+#define EIP197_MAX_RINGS			4
+#define EIP197_FETCH_COUNT			1
+#define EIP197_MAX_BATCH_SZ			EIP197_DEFAULT_RING_SIZE
+
+#define EIP197_GFP_FLAGS(base)	((base).flags & CRYPTO_TFM_REQ_MAY_SLEEP ? \
+				 GFP_KERNEL : GFP_ATOMIC)
+
+/* CDR/RDR register offsets */
+#define EIP197_HIA_xDR_OFF(r)			(0x80000 + (r) * 0x1000)
+#define EIP197_HIA_CDR(r)			(EIP197_HIA_xDR_OFF(r))
+#define EIP197_HIA_RDR(r)			(EIP197_HIA_xDR_OFF(r) + 0x800)
+#define EIP197_HIA_xDR_RING_BASE_ADDR_LO	0x0
+#define EIP197_HIA_xDR_RING_BASE_ADDR_HI	0x4
+#define EIP197_HIA_xDR_RING_SIZE		0x18
+#define EIP197_HIA_xDR_DESC_SIZE		0x1c
+#define EIP197_HIA_xDR_CFG			0x20
+#define EIP197_HIA_xDR_DMA_CFG			0x24
+#define EIP197_HIA_xDR_THRESH			0x28
+#define EIP197_HIA_xDR_PREP_COUNT		0x2c
+#define EIP197_HIA_xDR_PROC_COUNT		0x30
+#define EIP197_HIA_xDR_PREP_PNTR		0x34
+#define EIP197_HIA_xDR_PROC_PNTR		0x38
+#define EIP197_HIA_xDR_STAT			0x3c
+
+/* register offsets */
+#define EIP197_HIA_DFE_CFG			0x8c000
+#define EIP197_HIA_DFE_THR_CTRL			0x8c040
+#define EIP197_HIA_DFE_THR_STAT			0x8c044
+#define EIP197_HIA_DSE_CFG			0x8d000
+#define EIP197_HIA_DSE_THR_CTRL			0x8d040
+#define EIP197_HIA_DSE_THR_STAT			0x8d044
+#define EIP197_HIA_RA_PE_CTRL			0x90010
+#define EIP197_HIA_RA_PE_STAT			0x90014
+#define EIP197_HIA_AIC_R_OFF(r)			((r) * 0x1000)
+#define EIP197_HIA_AIC_R_ENABLE_CTRL(r)		(0x9e808 - EIP197_HIA_AIC_R_OFF(r))
+#define EIP197_HIA_AIC_R_ENABLED_STAT(r)	(0x9e810 - EIP197_HIA_AIC_R_OFF(r))
+#define EIP197_HIA_AIC_R_ACK(r)			(0x9e810 - EIP197_HIA_AIC_R_OFF(r))
+#define EIP197_HIA_AIC_R_ENABLE_CLR(r)		(0x9e814 - EIP197_HIA_AIC_R_OFF(r))
+#define EIP197_HIA_AIC_G_ENABLE_CTRL		0x9f808
+#define EIP197_HIA_AIC_G_ENABLED_STAT		0x9f810
+#define EIP197_HIA_AIC_G_ACK			0x9f810
+#define EIP197_HIA_MST_CTRL			0x9fff4
+#define EIP197_HIA_OPTIONS			0x9fff8
+#define EIP197_HIA_VERSION			0x9fffc
+#define EIP197_PE_IN_DBUF_THRES			0xa0000
+#define EIP197_PE_IN_TBUF_THRES			0xa0100
+#define EIP197_PE_ICE_SCRATCH_RAM		0xa0800
+#define EIP197_PE_ICE_PUE_CTRL			0xa0c80
+#define EIP197_PE_ICE_SCRATCH_CTRL		0xa0d04
+#define EIP197_PE_ICE_FPP_CTRL			0xa0d80
+#define EIP197_PE_ICE_RAM_CTRL			0xa0ff0
+#define EIP197_PE_EIP96_FUNCTION_EN		0xa1004
+#define EIP197_PE_EIP96_CONTEXT_CTRL		0xa1008
+#define EIP197_PE_EIP96_CONTEXT_STAT		0xa100c
+#define EIP197_PE_OUT_DBUF_THRES		0xa1c00
+#define EIP197_PE_OUT_TBUF_THRES		0xa1d00
+#define EIP197_CLASSIFICATION_RAMS		0xe0000
+#define EIP197_TRC_CTRL				0xf0800
+#define EIP197_TRC_LASTRES			0xf0804
+#define EIP197_TRC_REGINDEX			0xf0808
+#define EIP197_TRC_PARAMS			0xf0820
+#define EIP197_TRC_FREECHAIN			0xf0824
+#define EIP197_TRC_PARAMS2			0xf0828
+#define EIP197_TRC_ECCCTRL			0xf0830
+#define EIP197_TRC_ECCSTAT			0xf0834
+#define EIP197_TRC_ECCADMINSTAT			0xf0838
+#define EIP197_TRC_ECCDATASTAT			0xf083c
+#define EIP197_TRC_ECCDATA			0xf0840
+#define EIP197_CS_RAM_CTRL			0xf7ff0
+#define EIP197_MST_CTRL				0xffff4
+
+/* EIP197_HIA_xDR_DESC_SIZE */
+#define EIP197_xDR_DESC_MODE_64BIT		BIT(31)
+
+/* EIP197_HIA_xDR_DMA_CFG */
+#define EIP197_HIA_xDR_WR_RES_BUF		BIT(22)
+#define EIP197_HIA_xDR_WR_CTRL_BUG		BIT(23)
+#define EIP197_HIA_xDR_WR_OWN_BUF		BIT(24)
+#define EIP197_HIA_xDR_CFG_WR_CACHE(n)		(((n) & 0x7) << 25)
+#define EIP197_HIA_xDR_CFG_RD_CACHE(n)		(((n) & 0x7) << 29)
+
+/* EIP197_HIA_CDR_THRESH */
+#define EIP197_HIA_CDR_THRESH_PROC_PKT(n)	(n)
+#define EIP197_HIA_CDR_THRESH_PROC_MODE		BIT(22)
+#define EIP197_HIA_CDR_THRESH_PKT_MODE		BIT(23)
+#define EIP197_HIA_CDR_THRESH_TIMEOUT(n)	((n) << 24) /* x256 clk cycles */
+
+/* EIP197_HIA_RDR_THRESH */
+#define EIP197_HIA_RDR_THRESH_PROC_PKT(n)	(n)
+#define EIP197_HIA_RDR_THRESH_PKT_MODE		BIT(23)
+#define EIP197_HIA_RDR_THRESH_TIMEOUT(n)	((n) << 24) /* x256 clk cycles */
+
+/* EIP197_HIA_xDR_PREP_COUNT */
+#define EIP197_xDR_PREP_CLR_COUNT		BIT(31)
+
+/* EIP197_HIA_xDR_PROC_COUNT */
+#define EIP197_xDR_PROC_xD_COUNT(n)		((n) << 2)
+#define EIP197_xDR_PROC_xD_PKT(n)		((n) << 24)
+#define EIP197_xDR_PROC_CLR_COUNT		BIT(31)
+
+/* EIP197_HIA_xDR_STAT */
+#define EIP197_xDR_DMA_ERR			BIT(0)
+#define EIP197_xDR_PREP_CMD_THRES		BIT(1)
+#define EIP197_xDR_ERR				BIT(2)
+#define EIP197_xDR_THRESH			BIT(4)
+#define EIP197_xDR_TIMEOUT			BIT(5)
+
+#define EIP197_HIA_RA_PE_CTRL_RESET		BIT(31)
+#define EIP197_HIA_RA_PE_CTRL_EN		BIT(30)
+
+/* EIP197_HIA_AIC_R_ENABLE_CTRL */
+#define EIP197_CDR_IRQ(n)			BIT((n) * 2)
+#define EIP197_RDR_IRQ(n)			BIT((n) * 2 + 1)
+
+/* EIP197_HIA_DFE/DSE_CFG */
+#define EIP197_HIA_DxE_CFG_MIN_DATA_SIZE(n)	((n) << 0)
+#define EIP197_HIA_DxE_CFG_DATA_CACHE_CTRL(n)	(((n) & 0x7) << 4)
+#define EIP197_HIA_DxE_CFG_MAX_DATA_SIZE(n)	((n) << 8)
+#define EIP197_HIA_DSE_CFG_ALLWAYS_BUFFERABLE	GENMASK(15, 14)
+#define EIP197_HIA_DxE_CFG_MIN_CTRL_SIZE(n)	((n) << 16)
+#define EIP197_HIA_DxE_CFG_CTRL_CACHE_CTRL(n)	(((n) & 0x7) << 20)
+#define EIP197_HIA_DxE_CFG_MAX_CTRL_SIZE(n)	((n) << 24)
+#define EIP197_HIA_DFE_CFG_DIS_DEBUG		(BIT(31) | BIT(29))
+#define EIP197_HIA_DSE_CFG_EN_SINGLE_WR		BIT(29)
+#define EIP197_HIA_DSE_CFG_DIS_DEBUG		BIT(31)
+
+/* EIP197_HIA_DFE/DSE_THR_CTRL */
+#define EIP197_DxE_THR_CTRL_EN			BIT(30)
+#define EIP197_DxE_THR_CTRL_RESET_PE		BIT(31)
+
+/* EIP197_HIA_AIC_G_ENABLED_STAT */
+#define EIP197_G_IRQ_DFE(n)			BIT((n) << 1)
+#define EIP197_G_IRQ_DSE(n)			BIT(((n) << 1) + 1)
+#define EIP197_G_IRQ_RING			BIT(16)
+#define EIP197_G_IRQ_PE(n)			BIT((n) + 20)
+
+/* EIP197_HIA_MST_CTRL */
+#define RD_CACHE_3BITS				0x5
+#define WR_CACHE_3BITS				0x3
+#define RD_CACHE_4BITS				(RD_CACHE_3BITS << 1 | BIT(0))
+#define WR_CACHE_4BITS				(WR_CACHE_3BITS << 1 | BIT(0))
+#define EIP197_MST_CTRL_RD_CACHE(n)		(((n) & 0xf) << 0)
+#define EIP197_MST_CTRL_WD_CACHE(n)		(((n) & 0xf) << 4)
+#define EIP197_MST_CTRL_BYTE_SWAP		BIT(24)
+#define EIP197_MST_CTRL_NO_BYTE_SWAP		BIT(25)
+
+/* EIP197_PE_IN_DBUF/TBUF_THRES */
+#define EIP197_PE_IN_xBUF_THRES_MIN(n)		((n) << 8)
+#define EIP197_PE_IN_xBUF_THRES_MAX(n)		((n) << 12)
+
+/* EIP197_PE_OUT_DBUF_THRES */
+#define EIP197_PE_OUT_DBUF_THRES_MIN(n)		((n) << 0)
+#define EIP197_PE_OUT_DBUF_THRES_MAX(n)		((n) << 4)
+
+/* EIP197_PE_ICE_SCRATCH_CTRL */
+#define EIP197_PE_ICE_SCRATCH_CTRL_CHANGE_TIMER		BIT(2)
+#define EIP197_PE_ICE_SCRATCH_CTRL_TIMER_EN		BIT(3)
+#define EIP197_PE_ICE_SCRATCH_CTRL_CHANGE_ACCESS	BIT(24)
+#define EIP197_PE_ICE_SCRATCH_CTRL_SCRATCH_ACCESS	BIT(25)
+
+/* EIP197_PE_ICE_SCRATCH_RAM */
+#define EIP197_NUM_OF_SCRATCH_BLOCKS		32
+
+/* EIP197_PE_ICE_PUE/FPP_CTRL */
+#define EIP197_PE_ICE_x_CTRL_SW_RESET			BIT(0)
+#define EIP197_PE_ICE_x_CTRL_CLR_ECC_NON_CORR		BIT(14)
+#define EIP197_PE_ICE_x_CTRL_CLR_ECC_CORR		BIT(15)
+
+/* EIP197_PE_ICE_RAM_CTRL */
+#define EIP197_PE_ICE_RAM_CTRL_PUE_PROG_EN	BIT(0)
+#define EIP197_PE_ICE_RAM_CTRL_FPP_PROG_EN	BIT(1)
+
+/* EIP197_PE_EIP96_FUNCTION_EN */
+#define EIP197_FUNCTION_RSVD			(BIT(6) | BIT(15) | BIT(20) | BIT(23))
+#define EIP197_PROTOCOL_HASH_ONLY		BIT(0)
+#define EIP197_PROTOCOL_ENCRYPT_ONLY		BIT(1)
+#define EIP197_PROTOCOL_HASH_ENCRYPT		BIT(2)
+#define EIP197_PROTOCOL_HASH_DECRYPT		BIT(3)
+#define EIP197_PROTOCOL_ENCRYPT_HASH		BIT(4)
+#define EIP197_PROTOCOL_DECRYPT_HASH		BIT(5)
+#define EIP197_ALG_ARC4				BIT(7)
+#define EIP197_ALG_AES_ECB			BIT(8)
+#define EIP197_ALG_AES_CBC			BIT(9)
+#define EIP197_ALG_AES_CTR_ICM			BIT(10)
+#define EIP197_ALG_AES_OFB			BIT(11)
+#define EIP197_ALG_AES_CFB			BIT(12)
+#define EIP197_ALG_DES_ECB			BIT(13)
+#define EIP197_ALG_DES_CBC			BIT(14)
+#define EIP197_ALG_DES_OFB			BIT(16)
+#define EIP197_ALG_DES_CFB			BIT(17)
+#define EIP197_ALG_3DES_ECB			BIT(18)
+#define EIP197_ALG_3DES_CBC			BIT(19)
+#define EIP197_ALG_3DES_OFB			BIT(21)
+#define EIP197_ALG_3DES_CFB			BIT(22)
+#define EIP197_ALG_MD5				BIT(24)
+#define EIP197_ALG_HMAC_MD5			BIT(25)
+#define EIP197_ALG_SHA1				BIT(26)
+#define EIP197_ALG_HMAC_SHA1			BIT(27)
+#define EIP197_ALG_SHA2				BIT(28)
+#define EIP197_ALG_HMAC_SHA2			BIT(29)
+#define EIP197_ALG_AES_XCBC_MAC			BIT(30)
+#define EIP197_ALG_GCM_HASH			BIT(31)
+
+/* EIP197_PE_EIP96_CONTEXT_CTRL */
+#define EIP197_CONTEXT_SIZE(n)			(n)
+#define EIP197_ADDRESS_MODE			BIT(8)
+#define EIP197_CONTROL_MODE			BIT(9)
+
+/* Context Control */
+struct safexcel_context_record {
+	u32 control0;
+	u32 control1;
+
+	__le32 data[12];
+} __packed;
+
+/* control0 */
+#define CONTEXT_CONTROL_TYPE_NULL_OUT		0x0
+#define CONTEXT_CONTROL_TYPE_NULL_IN		0x1
+#define CONTEXT_CONTROL_TYPE_HASH_OUT		0x2
+#define CONTEXT_CONTROL_TYPE_HASH_IN		0x3
+#define CONTEXT_CONTROL_TYPE_CRYPTO_OUT		0x4
+#define CONTEXT_CONTROL_TYPE_CRYPTO_IN		0x5
+#define CONTEXT_CONTROL_TYPE_ENCRYPT_HASH_OUT	0x6
+#define CONTEXT_CONTROL_TYPE_DECRYPT_HASH_IN	0x7
+#define CONTEXT_CONTROL_TYPE_HASH_ENCRYPT_OUT	0x14
+#define CONTEXT_CONTROL_TYPE_HASH_DECRYPT_OUT	0x15
+#define CONTEXT_CONTROL_RESTART_HASH		BIT(4)
+#define CONTEXT_CONTROL_NO_FINISH_HASH		BIT(5)
+#define CONTEXT_CONTROL_SIZE(n)			((n) << 8)
+#define CONTEXT_CONTROL_KEY_EN			BIT(16)
+#define CONTEXT_CONTROL_CRYPTO_ALG_AES128	(0x5 << 17)
+#define CONTEXT_CONTROL_CRYPTO_ALG_AES192	(0x6 << 17)
+#define CONTEXT_CONTROL_CRYPTO_ALG_AES256	(0x7 << 17)
+#define CONTEXT_CONTROL_DIGEST_PRECOMPUTED	(0x1 << 21)
+#define CONTEXT_CONTROL_DIGEST_HMAC		(0x3 << 21)
+#define CONTEXT_CONTROL_CRYPTO_ALG_SHA1		(0x2 << 23)
+#define CONTEXT_CONTROL_CRYPTO_ALG_SHA224	(0x4 << 23)
+#define CONTEXT_CONTROL_CRYPTO_ALG_SHA256	(0x3 << 23)
+#define CONTEXT_CONTROL_INV_FR			(0x5 << 24)
+#define CONTEXT_CONTROL_INV_TR			(0x6 << 24)
+
+/* control1 */
+#define CONTEXT_CONTROL_CRYPTO_MODE_ECB		(0 << 0)
+#define CONTEXT_CONTROL_CRYPTO_MODE_CBC		(1 << 0)
+#define CONTEXT_CONTROL_IV0			BIT(5)
+#define CONTEXT_CONTROL_IV1			BIT(6)
+#define CONTEXT_CONTROL_IV2			BIT(7)
+#define CONTEXT_CONTROL_IV3			BIT(8)
+#define CONTEXT_CONTROL_DIGEST_CNT		BIT(9)
+#define CONTEXT_CONTROL_COUNTER_MODE		BIT(10)
+#define CONTEXT_CONTROL_HASH_STORE		BIT(19)
+
+/* EIP197_CS_RAM_CTRL */
+#define EIP197_TRC_ENABLE_0			BIT(4)
+#define EIP197_TRC_ENABLE_1			BIT(5)
+#define EIP197_TRC_ENABLE_2			BIT(6)
+#define EIP197_TRC_ENABLE_MASK			GENMASK(6, 4)
+
+/* EIP197_TRC_PARAMS */
+#define EIP197_TRC_PARAMS_SW_RESET		BIT(0)
+#define EIP197_TRC_PARAMS_DATA_ACCESS		BIT(2)
+#define EIP197_TRC_PARAMS_HTABLE_SZ(x)		((x) << 4)
+#define EIP197_TRC_PARAMS_BLK_TIMER_SPEED(x)	((x) << 10)
+#define EIP197_TRC_PARAMS_RC_SZ_LARGE(n)	((n) << 18)
+
+/* EIP197_TRC_FREECHAIN */
+#define EIP197_TRC_FREECHAIN_HEAD_PTR(p)	(p)
+#define EIP197_TRC_FREECHAIN_TAIL_PTR(p)	((p) << 16)
+
+/* EIP197_TRC_PARAMS2 */
+#define EIP197_TRC_PARAMS2_HTABLE_PTR(p)	(p)
+#define EIP197_TRC_PARAMS2_RC_SZ_SMALL(n)	((n) << 18)
+
+/* Cache helpers */
+#define EIP197_CS_RC_MAX			52
+#define EIP197_CS_RC_SIZE			(4 * sizeof(u32))
+#define EIP197_CS_RC_NEXT(x)			(x)
+#define EIP197_CS_RC_PREV(x)			((x) << 10)
+#define EIP197_RC_NULL				0x3ff
+#define EIP197_CS_TRC_REC_WC			59
+#define EIP197_CS_TRC_LG_REC_WC			73
+
+/* Result data */
+struct result_data_desc {
+	u32 packet_length:17;
+	u32 error_code:15;
+
+	u8 bypass_length:4;
+	u8 e15:1;
+	u16 rsvd0;
+	u8 hash_bytes:1;
+	u8 hash_length:6;
+	u8 generic_bytes:1;
+	u8 checksum:1;
+	u8 next_header:1;
+	u8 length:1;
+
+	u16 application_id;
+	u16 rsvd1;
+
+	u32 rsvd2;
+} __packed;
+
+
+/* Basic Result Descriptor format */
+struct safexcel_result_desc {
+	u32 particle_size:17;
+	u8 rsvd0:3;
+	u8 descriptor_overflow:1;
+	u8 buffer_overflow:1;
+	u8 last_seg:1;
+	u8 first_seg:1;
+	u16 result_size:8;
+
+	u32 rsvd1;
+
+	u32 data_lo;
+	u32 data_hi;
+
+	struct result_data_desc result_data;
+} __packed;
+
+struct safexcel_token {
+	u32 packet_length:17;
+	u8 stat:2;
+	u16 instructions:9;
+	u8 opcode:4;
+} __packed;
+
+#define EIP197_TOKEN_STAT_LAST_HASH		BIT(0)
+#define EIP197_TOKEN_STAT_LAST_PACKET		BIT(1)
+#define EIP197_TOKEN_OPCODE_DIRECTION		0x0
+#define EIP197_TOKEN_OPCODE_INSERT		0x2
+#define EIP197_TOKEN_OPCODE_NOOP		EIP197_TOKEN_OPCODE_INSERT
+#define EIP197_TOKEN_OPCODE_BYPASS		GENMASK(3, 0)
+
+static inline void eip197_noop_token(struct safexcel_token *token)
+{
+	token->opcode = EIP197_TOKEN_OPCODE_NOOP;
+	token->packet_length = BIT(2);
+}
+
+/* Instructions */
+#define EIP197_TOKEN_INS_INSERT_HASH_DIGEST	0x1c
+#define EIP197_TOKEN_INS_TYPE_OUTPUT		BIT(5)
+#define EIP197_TOKEN_INS_TYPE_HASH		BIT(6)
+#define EIP197_TOKEN_INS_TYPE_CRYTO		BIT(7)
+#define EIP197_TOKEN_INS_LAST			BIT(8)
+
+/* Processing Engine Control Data  */
+struct safexcel_control_data_desc {
+	u32 packet_length:17;
+	u16 options:13;
+	u8 type:2;
+
+	u16 application_id;
+	u16 rsvd;
+
+	u8 refresh:2;
+	u32 context_lo:30;
+	u32 context_hi;
+
+	u32 control0;
+	u32 control1;
+
+	u32 token[EIP197_MAX_TOKENS];
+} __packed;
+
+#define EIP197_OPTION_MAGIC_VALUE	BIT(0)
+#define EIP197_OPTION_64BIT_CTX		BIT(1)
+#define EIP197_OPTION_CTX_CTRL_IN_CMD	BIT(8)
+#define EIP197_OPTION_4_TOKEN_IV_CMD	GENMASK(11, 9)
+
+#define EIP197_TYPE_EXTENDED		0x3
+
+/* Basic Command Descriptor format */
+struct safexcel_command_desc {
+	u32 particle_size:17;
+	u8 rsvd0:5;
+	u8 last_seg:1;
+	u8 first_seg:1;
+	u16 additional_cdata_size:8;
+
+	u32 rsvd1;
+
+	u32 data_lo;
+	u32 data_hi;
+
+	struct safexcel_control_data_desc control_data;
+} __packed;
+
+/*
+ * Internal structures & functions
+ */
+
+enum eip197_fw {
+	FW_IFPP = 0,
+	FW_IPUE,
+	FW_NB
+};
+
+struct safexcel_ring {
+	void *base;
+	void *base_end;
+	dma_addr_t base_dma;
+
+	/* write and read pointers */
+	void *write;
+	void *read;
+
+	/* number of elements used in the ring */
+	unsigned nr;
+	unsigned offset;
+};
+
+enum safexcel_alg_type {
+	SAFEXCEL_ALG_TYPE_SKCIPHER,
+	SAFEXCEL_ALG_TYPE_AHASH,
+};
+
+struct safexcel_request {
+	struct list_head list;
+	struct crypto_async_request *req;
+};
+
+struct safexcel_config {
+	u32 rings;
+
+	u32 cd_size;
+	u32 cd_offset;
+
+	u32 rd_size;
+	u32 rd_offset;
+};
+
+struct safexcel_work_data {
+	struct work_struct work;
+	struct safexcel_crypto_priv *priv;
+	int ring;
+};
+
+struct safexcel_crypto_priv {
+	void __iomem *base;
+	struct device *dev;
+	struct clk *clk;
+	struct safexcel_config config;
+
+	/* context DMA pool */
+	struct dma_pool *context_pool;
+
+	atomic_t ring_used;
+
+	struct {
+		spinlock_t lock;
+		spinlock_t egress_lock;
+
+		struct list_head list;
+		struct workqueue_struct *workqueue;
+		struct safexcel_work_data work_data;
+
+		/* command/result rings */
+		struct safexcel_ring cdr;
+		struct safexcel_ring rdr;
+
+		/* queue */
+		struct crypto_queue queue;
+		spinlock_t queue_lock;
+		bool need_dequeue;
+	} ring[EIP197_MAX_RINGS];
+};
+
+struct safexcel_context {
+	int (*send)(struct crypto_async_request *req, int ring,
+		    struct safexcel_request *request, int *commands,
+		    int *results);
+	int (*handle_result)(struct safexcel_crypto_priv *priv, int ring,
+			     struct crypto_async_request *req, bool *complete,
+			     int *ret);
+	struct safexcel_context_record *ctxr;
+	dma_addr_t ctxr_dma;
+
+	int ring;
+	bool needs_inv;
+	bool exit_inv;
+
+	/* Used for ahash requests */
+	dma_addr_t result_dma;
+	void *cache;
+	dma_addr_t cache_dma;
+	unsigned int cache_sz;
+};
+
+/*
+ * Template structure to describe the algorithms in order to register them.
+ * It also has the purpose to contain our private structure and is actually
+ * the only way I know in this framework to avoid having global pointers...
+ */
+struct safexcel_alg_template {
+	struct safexcel_crypto_priv *priv;
+	enum safexcel_alg_type type;
+	union {
+		struct skcipher_alg skcipher;
+		struct ahash_alg ahash;
+	} alg;
+};
+
+struct safexcel_inv_result {
+	struct completion completion;
+	int error;
+};
+
+void safexcel_dequeue(struct safexcel_crypto_priv *priv, int ring);
+void safexcel_complete(struct safexcel_crypto_priv *priv, int ring);
+void safexcel_free_context(struct safexcel_crypto_priv *priv,
+				  struct crypto_async_request *req,
+				  int result_sz);
+int safexcel_invalidate_cache(struct crypto_async_request *async,
+			      struct safexcel_context *ctx,
+			      struct safexcel_crypto_priv *priv,
+			      dma_addr_t ctxr_dma, int ring,
+			      struct safexcel_request *request);
+int safexcel_init_ring_descriptors(struct safexcel_crypto_priv *priv,
+				   struct safexcel_ring *cdr,
+				   struct safexcel_ring *rdr);
+int safexcel_select_ring(struct safexcel_crypto_priv *priv);
+void *safexcel_ring_next_rptr(struct safexcel_crypto_priv *priv,
+			      struct safexcel_ring *ring);
+void safexcel_ring_rollback_wptr(struct safexcel_crypto_priv *priv,
+				 struct safexcel_ring *ring);
+struct safexcel_command_desc *safexcel_add_cdesc(struct safexcel_crypto_priv *priv,
+						 int ring_id,
+						 bool first, bool last,
+						 dma_addr_t data, u32 len,
+						 u32 full_data_len,
+						 dma_addr_t context);
+struct safexcel_result_desc *safexcel_add_rdesc(struct safexcel_crypto_priv *priv,
+						 int ring_id,
+						bool first, bool last,
+						dma_addr_t data, u32 len);
+void safexcel_inv_complete(struct crypto_async_request *req, int error);
+
+/* available algorithms */
+extern struct safexcel_alg_template safexcel_alg_ecb_aes;
+extern struct safexcel_alg_template safexcel_alg_cbc_aes;
+extern struct safexcel_alg_template safexcel_alg_sha1;
+extern struct safexcel_alg_template safexcel_alg_sha224;
+extern struct safexcel_alg_template safexcel_alg_sha256;
+extern struct safexcel_alg_template safexcel_alg_hmac_sha1;
+
+#endif
diff --git a/drivers/crypto/inside-secure/safexcel_cipher.c b/drivers/crypto/inside-secure/safexcel_cipher.c
new file mode 100644
index 000000000000..d2207ac5ba19
--- /dev/null
+++ b/drivers/crypto/inside-secure/safexcel_cipher.c
@@ -0,0 +1,561 @@
+/*
+ * Copyright (C) 2017 Marvell
+ *
+ * Antoine Tenart <antoine.tenart@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+
+#include <crypto/aes.h>
+#include <crypto/skcipher.h>
+
+#include "safexcel.h"
+
+enum safexcel_cipher_direction {
+	SAFEXCEL_ENCRYPT,
+	SAFEXCEL_DECRYPT,
+};
+
+struct safexcel_cipher_ctx {
+	struct safexcel_context base;
+	struct safexcel_crypto_priv *priv;
+
+	enum safexcel_cipher_direction direction;
+	u32 mode;
+
+	__le32 key[8];
+	unsigned int key_len;
+};
+
+static void safexcel_cipher_token(struct safexcel_cipher_ctx *ctx,
+				  struct crypto_async_request *async,
+				  struct safexcel_command_desc *cdesc,
+				  u32 length)
+{
+	struct skcipher_request *req = skcipher_request_cast(async);
+	struct safexcel_token *token;
+	unsigned offset = 0;
+
+	if (ctx->mode == CONTEXT_CONTROL_CRYPTO_MODE_CBC) {
+		offset = AES_BLOCK_SIZE / sizeof(u32);
+		memcpy(cdesc->control_data.token, req->iv, AES_BLOCK_SIZE);
+
+		cdesc->control_data.options |= EIP197_OPTION_4_TOKEN_IV_CMD;
+	}
+
+	token = (struct safexcel_token *)(cdesc->control_data.token + offset);
+
+	token[0].opcode = EIP197_TOKEN_OPCODE_DIRECTION;
+	token[0].packet_length = length;
+	token[0].stat = EIP197_TOKEN_STAT_LAST_PACKET;
+	token[0].instructions = EIP197_TOKEN_INS_LAST |
+				EIP197_TOKEN_INS_TYPE_CRYTO |
+				EIP197_TOKEN_INS_TYPE_OUTPUT;
+}
+
+static int safexcel_aes_setkey(struct crypto_skcipher *ctfm, const u8 *key,
+			       unsigned int len)
+{
+	struct crypto_tfm *tfm = crypto_skcipher_tfm(ctfm);
+	struct safexcel_cipher_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct crypto_aes_ctx aes;
+	int ret, i;
+
+	ret = crypto_aes_expand_key(&aes, key, len);
+	if (ret) {
+		crypto_skcipher_set_flags(ctfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return ret;
+	}
+
+	for (i = 0; i < len / sizeof(u32); i++) {
+		if (ctx->key[i] != cpu_to_le32(aes.key_enc[i])) {
+			ctx->base.needs_inv = true;
+			break;
+		}
+	}
+
+	for (i = 0; i < len / sizeof(u32); i++)
+		ctx->key[i] = cpu_to_le32(aes.key_enc[i]);
+
+	ctx->key_len = len;
+
+	memzero_explicit(&aes, sizeof(aes));
+	return 0;
+}
+
+static int safexcel_context_control(struct safexcel_cipher_ctx *ctx,
+				    struct safexcel_command_desc *cdesc)
+{
+	struct safexcel_crypto_priv *priv = ctx->priv;
+	int ctrl_size;
+
+	if (ctx->direction == SAFEXCEL_ENCRYPT)
+		cdesc->control_data.control0 |= CONTEXT_CONTROL_TYPE_CRYPTO_OUT;
+	else
+		cdesc->control_data.control0 |= CONTEXT_CONTROL_TYPE_CRYPTO_IN;
+
+	cdesc->control_data.control0 |= CONTEXT_CONTROL_KEY_EN;
+	cdesc->control_data.control1 |= ctx->mode;
+
+	switch (ctx->key_len) {
+	case AES_KEYSIZE_128:
+		cdesc->control_data.control0 |= CONTEXT_CONTROL_CRYPTO_ALG_AES128;
+		ctrl_size = 4;
+		break;
+	case AES_KEYSIZE_192:
+		cdesc->control_data.control0 |= CONTEXT_CONTROL_CRYPTO_ALG_AES192;
+		ctrl_size = 6;
+		break;
+	case AES_KEYSIZE_256:
+		cdesc->control_data.control0 |= CONTEXT_CONTROL_CRYPTO_ALG_AES256;
+		ctrl_size = 8;
+		break;
+	default:
+		dev_err(priv->dev, "aes keysize not supported: %u\n",
+			ctx->key_len);
+		return -EINVAL;
+	}
+	cdesc->control_data.control0 |= CONTEXT_CONTROL_SIZE(ctrl_size);
+
+	return 0;
+}
+
+static int safexcel_handle_result(struct safexcel_crypto_priv *priv, int ring,
+				  struct crypto_async_request *async,
+				  bool *should_complete, int *ret)
+{
+	struct skcipher_request *req = skcipher_request_cast(async);
+	struct safexcel_result_desc *rdesc;
+	int ndesc = 0;
+
+	*ret = 0;
+
+	spin_lock_bh(&priv->ring[ring].egress_lock);
+	do {
+		rdesc = safexcel_ring_next_rptr(priv, &priv->ring[ring].rdr);
+		if (IS_ERR(rdesc)) {
+			dev_err(priv->dev,
+				"cipher: result: could not retrieve the result descriptor\n");
+			*ret = PTR_ERR(rdesc);
+			break;
+		}
+
+		if (rdesc->result_data.error_code) {
+			dev_err(priv->dev,
+				"cipher: result: result descriptor error (%d)\n",
+				rdesc->result_data.error_code);
+			*ret = -EIO;
+		}
+
+		ndesc++;
+	} while (!rdesc->last_seg);
+
+	safexcel_complete(priv, ring);
+	spin_unlock_bh(&priv->ring[ring].egress_lock);
+
+	if (req->src == req->dst) {
+		dma_unmap_sg(priv->dev, req->src,
+			     sg_nents_for_len(req->src, req->cryptlen),
+			     DMA_BIDIRECTIONAL);
+	} else {
+		dma_unmap_sg(priv->dev, req->src,
+			     sg_nents_for_len(req->src, req->cryptlen),
+			     DMA_TO_DEVICE);
+		dma_unmap_sg(priv->dev, req->dst,
+			     sg_nents_for_len(req->dst, req->cryptlen),
+			     DMA_FROM_DEVICE);
+	}
+
+	*should_complete = true;
+
+	return ndesc;
+}
+
+static int safexcel_aes_send(struct crypto_async_request *async,
+			     int ring, struct safexcel_request *request,
+			     int *commands, int *results)
+{
+	struct skcipher_request *req = skcipher_request_cast(async);
+	struct safexcel_cipher_ctx *ctx = crypto_tfm_ctx(req->base.tfm);
+	struct safexcel_crypto_priv *priv = ctx->priv;
+	struct safexcel_command_desc *cdesc;
+	struct safexcel_result_desc *rdesc;
+	struct scatterlist *sg;
+	int nr_src, nr_dst, n_cdesc = 0, n_rdesc = 0, queued = req->cryptlen;
+	int i, ret = 0;
+
+	if (req->src == req->dst) {
+		nr_src = dma_map_sg(priv->dev, req->src,
+				    sg_nents_for_len(req->src, req->cryptlen),
+				    DMA_BIDIRECTIONAL);
+		nr_dst = nr_src;
+		if (!nr_src)
+			return -EINVAL;
+	} else {
+		nr_src = dma_map_sg(priv->dev, req->src,
+				    sg_nents_for_len(req->src, req->cryptlen),
+				    DMA_TO_DEVICE);
+		if (!nr_src)
+			return -EINVAL;
+
+		nr_dst = dma_map_sg(priv->dev, req->dst,
+				    sg_nents_for_len(req->dst, req->cryptlen),
+				    DMA_FROM_DEVICE);
+		if (!nr_dst) {
+			dma_unmap_sg(priv->dev, req->src,
+				     sg_nents_for_len(req->src, req->cryptlen),
+				     DMA_TO_DEVICE);
+			return -EINVAL;
+		}
+	}
+
+	memcpy(ctx->base.ctxr->data, ctx->key, ctx->key_len);
+
+	spin_lock_bh(&priv->ring[ring].egress_lock);
+
+	/* command descriptors */
+	for_each_sg(req->src, sg, nr_src, i) {
+		int len = sg_dma_len(sg);
+
+		/* Do not overflow the request */
+		if (queued - len < 0)
+			len = queued;
+
+		cdesc = safexcel_add_cdesc(priv, ring, !n_cdesc, !(queued - len),
+					   sg_dma_address(sg), len, req->cryptlen,
+					   ctx->base.ctxr_dma);
+		if (IS_ERR(cdesc)) {
+			/* No space left in the command descriptor ring */
+			ret = PTR_ERR(cdesc);
+			goto cdesc_rollback;
+		}
+		n_cdesc++;
+
+		if (n_cdesc == 1) {
+			safexcel_context_control(ctx, cdesc);
+			safexcel_cipher_token(ctx, async, cdesc, req->cryptlen);
+		}
+
+		queued -= len;
+		if (!queued)
+			break;
+	}
+
+	/* result descriptors */
+	for_each_sg(req->dst, sg, nr_dst, i) {
+		bool first = !i, last = (i == nr_dst - 1);
+		u32 len = sg_dma_len(sg);
+
+		rdesc = safexcel_add_rdesc(priv, ring, first, last,
+					   sg_dma_address(sg), len);
+		if (IS_ERR(rdesc)) {
+			/* No space left in the result descriptor ring */
+			ret = PTR_ERR(rdesc);
+			goto rdesc_rollback;
+		}
+		n_rdesc++;
+	}
+
+	spin_unlock_bh(&priv->ring[ring].egress_lock);
+
+	request->req = &req->base;
+	ctx->base.handle_result = safexcel_handle_result;
+
+	*commands = n_cdesc;
+	*results = n_rdesc;
+	return 0;
+
+rdesc_rollback:
+	for (i = 0; i < n_rdesc; i++)
+		safexcel_ring_rollback_wptr(priv, &priv->ring[ring].rdr);
+cdesc_rollback:
+	for (i = 0; i < n_cdesc; i++)
+		safexcel_ring_rollback_wptr(priv, &priv->ring[ring].cdr);
+
+	spin_unlock_bh(&priv->ring[ring].egress_lock);
+
+	if (req->src == req->dst) {
+		dma_unmap_sg(priv->dev, req->src,
+			     sg_nents_for_len(req->src, req->cryptlen),
+			     DMA_BIDIRECTIONAL);
+	} else {
+		dma_unmap_sg(priv->dev, req->src,
+			     sg_nents_for_len(req->src, req->cryptlen),
+			     DMA_TO_DEVICE);
+		dma_unmap_sg(priv->dev, req->dst,
+			     sg_nents_for_len(req->dst, req->cryptlen),
+			     DMA_FROM_DEVICE);
+	}
+
+	return ret;
+}
+
+static int safexcel_handle_inv_result(struct safexcel_crypto_priv *priv,
+				      int ring,
+				      struct crypto_async_request *async,
+				      bool *should_complete, int *ret)
+{
+	struct skcipher_request *req = skcipher_request_cast(async);
+	struct safexcel_cipher_ctx *ctx = crypto_tfm_ctx(req->base.tfm);
+	struct safexcel_result_desc *rdesc;
+	int ndesc = 0, enq_ret;
+
+	*ret = 0;
+
+	spin_lock_bh(&priv->ring[ring].egress_lock);
+	do {
+		rdesc = safexcel_ring_next_rptr(priv, &priv->ring[ring].rdr);
+		if (IS_ERR(rdesc)) {
+			dev_err(priv->dev,
+				"cipher: invalidate: could not retrieve the result descriptor\n");
+			*ret = PTR_ERR(rdesc);
+			break;
+		}
+
+		if (rdesc->result_data.error_code) {
+			dev_err(priv->dev, "cipher: invalidate: result descriptor error (%d)\n",
+				rdesc->result_data.error_code);
+			*ret = -EIO;
+		}
+
+		ndesc++;
+	} while (!rdesc->last_seg);
+
+	safexcel_complete(priv, ring);
+	spin_unlock_bh(&priv->ring[ring].egress_lock);
+
+	if (ctx->base.exit_inv) {
+		dma_pool_free(priv->context_pool, ctx->base.ctxr,
+			      ctx->base.ctxr_dma);
+
+		*should_complete = true;
+
+		return ndesc;
+	}
+
+	ring = safexcel_select_ring(priv);
+	ctx->base.ring = ring;
+	ctx->base.needs_inv = false;
+	ctx->base.send = safexcel_aes_send;
+
+	spin_lock_bh(&priv->ring[ring].queue_lock);
+	enq_ret = crypto_enqueue_request(&priv->ring[ring].queue, async);
+	spin_unlock_bh(&priv->ring[ring].queue_lock);
+
+	if (enq_ret != -EINPROGRESS)
+		*ret = enq_ret;
+
+	if (!priv->ring[ring].need_dequeue)
+		safexcel_dequeue(priv, ring);
+
+	*should_complete = false;
+
+	return ndesc;
+}
+
+static int safexcel_cipher_send_inv(struct crypto_async_request *async,
+				    int ring, struct safexcel_request *request,
+				    int *commands, int *results)
+{
+	struct skcipher_request *req = skcipher_request_cast(async);
+	struct safexcel_cipher_ctx *ctx = crypto_tfm_ctx(req->base.tfm);
+	struct safexcel_crypto_priv *priv = ctx->priv;
+	int ret;
+
+	ctx->base.handle_result = safexcel_handle_inv_result;
+
+	ret = safexcel_invalidate_cache(async, &ctx->base, priv,
+					ctx->base.ctxr_dma, ring, request);
+	if (unlikely(ret))
+		return ret;
+
+	*commands = 1;
+	*results = 1;
+
+	return 0;
+}
+
+static int safexcel_cipher_exit_inv(struct crypto_tfm *tfm)
+{
+	struct safexcel_cipher_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct safexcel_crypto_priv *priv = ctx->priv;
+	struct skcipher_request req;
+	struct safexcel_inv_result result = { 0 };
+	int ring = ctx->base.ring;
+
+	memset(&req, 0, sizeof(struct skcipher_request));
+
+	/* create invalidation request */
+	init_completion(&result.completion);
+	skcipher_request_set_callback(&req, CRYPTO_TFM_REQ_MAY_BACKLOG,
+					safexcel_inv_complete, &result);
+
+	skcipher_request_set_tfm(&req, __crypto_skcipher_cast(tfm));
+	ctx = crypto_tfm_ctx(req.base.tfm);
+	ctx->base.exit_inv = true;
+	ctx->base.send = safexcel_cipher_send_inv;
+
+	spin_lock_bh(&priv->ring[ring].queue_lock);
+	crypto_enqueue_request(&priv->ring[ring].queue, &req.base);
+	spin_unlock_bh(&priv->ring[ring].queue_lock);
+
+	if (!priv->ring[ring].need_dequeue)
+		safexcel_dequeue(priv, ring);
+
+	wait_for_completion_interruptible(&result.completion);
+
+	if (result.error) {
+		dev_warn(priv->dev,
+			"cipher: sync: invalidate: completion error %d\n",
+			 result.error);
+		return result.error;
+	}
+
+	return 0;
+}
+
+static int safexcel_aes(struct skcipher_request *req,
+			enum safexcel_cipher_direction dir, u32 mode)
+{
+	struct safexcel_cipher_ctx *ctx = crypto_tfm_ctx(req->base.tfm);
+	struct safexcel_crypto_priv *priv = ctx->priv;
+	int ret, ring;
+
+	ctx->direction = dir;
+	ctx->mode = mode;
+
+	if (ctx->base.ctxr) {
+		if (ctx->base.needs_inv)
+			ctx->base.send = safexcel_cipher_send_inv;
+	} else {
+		ctx->base.ring = safexcel_select_ring(priv);
+		ctx->base.send = safexcel_aes_send;
+
+		ctx->base.ctxr = dma_pool_zalloc(priv->context_pool,
+						 EIP197_GFP_FLAGS(req->base),
+						 &ctx->base.ctxr_dma);
+		if (!ctx->base.ctxr)
+			return -ENOMEM;
+	}
+
+	ring = ctx->base.ring;
+
+	spin_lock_bh(&priv->ring[ring].queue_lock);
+	ret = crypto_enqueue_request(&priv->ring[ring].queue, &req->base);
+	spin_unlock_bh(&priv->ring[ring].queue_lock);
+
+	if (!priv->ring[ring].need_dequeue)
+		safexcel_dequeue(priv, ring);
+
+	return ret;
+}
+
+static int safexcel_ecb_aes_encrypt(struct skcipher_request *req)
+{
+	return safexcel_aes(req, SAFEXCEL_ENCRYPT,
+			    CONTEXT_CONTROL_CRYPTO_MODE_ECB);
+}
+
+static int safexcel_ecb_aes_decrypt(struct skcipher_request *req)
+{
+	return safexcel_aes(req, SAFEXCEL_DECRYPT,
+			    CONTEXT_CONTROL_CRYPTO_MODE_ECB);
+}
+
+static int safexcel_skcipher_cra_init(struct crypto_tfm *tfm)
+{
+	struct safexcel_cipher_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct safexcel_alg_template *tmpl =
+		container_of(tfm->__crt_alg, struct safexcel_alg_template,
+			     alg.skcipher.base);
+
+	ctx->priv = tmpl->priv;
+
+	return 0;
+}
+
+static void safexcel_skcipher_cra_exit(struct crypto_tfm *tfm)
+{
+	struct safexcel_cipher_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct safexcel_crypto_priv *priv = ctx->priv;
+	int ret;
+
+	memzero_explicit(ctx->key, 8 * sizeof(u32));
+
+	/* context not allocated, skip invalidation */
+	if (!ctx->base.ctxr)
+		return;
+
+	memzero_explicit(ctx->base.ctxr->data, 8 * sizeof(u32));
+
+	ret = safexcel_cipher_exit_inv(tfm);
+	if (ret)
+		dev_warn(priv->dev, "cipher: invalidation error %d\n", ret);
+}
+
+struct safexcel_alg_template safexcel_alg_ecb_aes = {
+	.type = SAFEXCEL_ALG_TYPE_SKCIPHER,
+	.alg.skcipher = {
+		.setkey = safexcel_aes_setkey,
+		.encrypt = safexcel_ecb_aes_encrypt,
+		.decrypt = safexcel_ecb_aes_decrypt,
+		.min_keysize = AES_MIN_KEY_SIZE,
+		.max_keysize = AES_MAX_KEY_SIZE,
+		.base = {
+			.cra_name = "ecb(aes)",
+			.cra_driver_name = "safexcel-ecb-aes",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_TYPE_SKCIPHER | CRYPTO_ALG_ASYNC |
+				     CRYPTO_ALG_KERN_DRIVER_ONLY,
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct safexcel_cipher_ctx),
+			.cra_alignmask = 0,
+			.cra_init = safexcel_skcipher_cra_init,
+			.cra_exit = safexcel_skcipher_cra_exit,
+			.cra_module = THIS_MODULE,
+		},
+	},
+};
+
+static int safexcel_cbc_aes_encrypt(struct skcipher_request *req)
+{
+	return safexcel_aes(req, SAFEXCEL_ENCRYPT,
+			    CONTEXT_CONTROL_CRYPTO_MODE_CBC);
+}
+
+static int safexcel_cbc_aes_decrypt(struct skcipher_request *req)
+{
+	return safexcel_aes(req, SAFEXCEL_DECRYPT,
+			    CONTEXT_CONTROL_CRYPTO_MODE_CBC);
+}
+
+struct safexcel_alg_template safexcel_alg_cbc_aes = {
+	.type = SAFEXCEL_ALG_TYPE_SKCIPHER,
+	.alg.skcipher = {
+		.setkey = safexcel_aes_setkey,
+		.encrypt = safexcel_cbc_aes_encrypt,
+		.decrypt = safexcel_cbc_aes_decrypt,
+		.min_keysize = AES_MIN_KEY_SIZE,
+		.max_keysize = AES_MAX_KEY_SIZE,
+		.ivsize = AES_BLOCK_SIZE,
+		.base = {
+			.cra_name = "cbc(aes)",
+			.cra_driver_name = "safexcel-cbc-aes",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_TYPE_SKCIPHER | CRYPTO_ALG_ASYNC |
+				     CRYPTO_ALG_KERN_DRIVER_ONLY,
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct safexcel_cipher_ctx),
+			.cra_alignmask = 0,
+			.cra_init = safexcel_skcipher_cra_init,
+			.cra_exit = safexcel_skcipher_cra_exit,
+			.cra_module = THIS_MODULE,
+		},
+	},
+};
diff --git a/drivers/crypto/inside-secure/safexcel_hash.c b/drivers/crypto/inside-secure/safexcel_hash.c
new file mode 100644
index 000000000000..8527a5899a2f
--- /dev/null
+++ b/drivers/crypto/inside-secure/safexcel_hash.c
@@ -0,0 +1,1052 @@
+/*
+ * Copyright (C) 2017 Marvell
+ *
+ * Antoine Tenart <antoine.tenart@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <crypto/hmac.h>
+#include <crypto/sha.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+
+
+#include "safexcel.h"
+
+struct safexcel_ahash_ctx {
+	struct safexcel_context base;
+	struct safexcel_crypto_priv *priv;
+
+	u32 alg;
+	u32 digest;
+
+	u32 ipad[SHA1_DIGEST_SIZE / sizeof(u32)];
+	u32 opad[SHA1_DIGEST_SIZE / sizeof(u32)];
+};
+
+struct safexcel_ahash_req {
+	bool last_req;
+	bool finish;
+	bool hmac;
+
+	u8 state_sz;    /* expected sate size, only set once */
+	u32 state[SHA256_DIGEST_SIZE / sizeof(u32)];
+
+	u64 len;
+	u64 processed;
+
+	u8 cache[SHA256_BLOCK_SIZE] __aligned(sizeof(u32));
+	u8 cache_next[SHA256_BLOCK_SIZE] __aligned(sizeof(u32));
+};
+
+struct safexcel_ahash_export_state {
+	u64 len;
+	u64 processed;
+
+	u32 state[SHA256_DIGEST_SIZE / sizeof(u32)];
+	u8 cache[SHA256_BLOCK_SIZE];
+};
+
+static void safexcel_hash_token(struct safexcel_command_desc *cdesc,
+				u32 input_length, u32 result_length)
+{
+	struct safexcel_token *token =
+		(struct safexcel_token *)cdesc->control_data.token;
+
+	token[0].opcode = EIP197_TOKEN_OPCODE_DIRECTION;
+	token[0].packet_length = input_length;
+	token[0].stat = EIP197_TOKEN_STAT_LAST_HASH;
+	token[0].instructions = EIP197_TOKEN_INS_TYPE_HASH;
+
+	token[1].opcode = EIP197_TOKEN_OPCODE_INSERT;
+	token[1].packet_length = result_length;
+	token[1].stat = EIP197_TOKEN_STAT_LAST_HASH |
+			EIP197_TOKEN_STAT_LAST_PACKET;
+	token[1].instructions = EIP197_TOKEN_INS_TYPE_OUTPUT |
+				EIP197_TOKEN_INS_INSERT_HASH_DIGEST;
+}
+
+static void safexcel_context_control(struct safexcel_ahash_ctx *ctx,
+				     struct safexcel_ahash_req *req,
+				     struct safexcel_command_desc *cdesc,
+				     unsigned int digestsize,
+				     unsigned int blocksize)
+{
+	int i;
+
+	cdesc->control_data.control0 |= CONTEXT_CONTROL_TYPE_HASH_OUT;
+	cdesc->control_data.control0 |= ctx->alg;
+	cdesc->control_data.control0 |= ctx->digest;
+
+	if (ctx->digest == CONTEXT_CONTROL_DIGEST_PRECOMPUTED) {
+		if (req->processed) {
+			if (ctx->alg == CONTEXT_CONTROL_CRYPTO_ALG_SHA1)
+				cdesc->control_data.control0 |= CONTEXT_CONTROL_SIZE(6);
+			else if (ctx->alg == CONTEXT_CONTROL_CRYPTO_ALG_SHA224 ||
+				 ctx->alg == CONTEXT_CONTROL_CRYPTO_ALG_SHA256)
+				cdesc->control_data.control0 |= CONTEXT_CONTROL_SIZE(9);
+
+			cdesc->control_data.control1 |= CONTEXT_CONTROL_DIGEST_CNT;
+		} else {
+			cdesc->control_data.control0 |= CONTEXT_CONTROL_RESTART_HASH;
+		}
+
+		if (!req->finish)
+			cdesc->control_data.control0 |= CONTEXT_CONTROL_NO_FINISH_HASH;
+
+		/*
+		 * Copy the input digest if needed, and setup the context
+		 * fields. Do this now as we need it to setup the first command
+		 * descriptor.
+		 */
+		if (req->processed) {
+			for (i = 0; i < digestsize / sizeof(u32); i++)
+				ctx->base.ctxr->data[i] = cpu_to_le32(req->state[i]);
+
+			if (req->finish)
+				ctx->base.ctxr->data[i] = cpu_to_le32(req->processed / blocksize);
+		}
+	} else if (ctx->digest == CONTEXT_CONTROL_DIGEST_HMAC) {
+		cdesc->control_data.control0 |= CONTEXT_CONTROL_SIZE(10);
+
+		memcpy(ctx->base.ctxr->data, ctx->ipad, digestsize);
+		memcpy(ctx->base.ctxr->data + digestsize / sizeof(u32),
+		       ctx->opad, digestsize);
+	}
+}
+
+static int safexcel_handle_result(struct safexcel_crypto_priv *priv, int ring,
+				  struct crypto_async_request *async,
+				  bool *should_complete, int *ret)
+{
+	struct safexcel_result_desc *rdesc;
+	struct ahash_request *areq = ahash_request_cast(async);
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(areq);
+	struct safexcel_ahash_req *sreq = ahash_request_ctx(areq);
+	int cache_len, result_sz = sreq->state_sz;
+
+	*ret = 0;
+
+	spin_lock_bh(&priv->ring[ring].egress_lock);
+	rdesc = safexcel_ring_next_rptr(priv, &priv->ring[ring].rdr);
+	if (IS_ERR(rdesc)) {
+		dev_err(priv->dev,
+			"hash: result: could not retrieve the result descriptor\n");
+		*ret = PTR_ERR(rdesc);
+	} else if (rdesc->result_data.error_code) {
+		dev_err(priv->dev,
+			"hash: result: result descriptor error (%d)\n",
+			rdesc->result_data.error_code);
+		*ret = -EINVAL;
+	}
+
+	safexcel_complete(priv, ring);
+	spin_unlock_bh(&priv->ring[ring].egress_lock);
+
+	if (sreq->finish)
+		result_sz = crypto_ahash_digestsize(ahash);
+	memcpy(sreq->state, areq->result, result_sz);
+
+	dma_unmap_sg(priv->dev, areq->src,
+		     sg_nents_for_len(areq->src, areq->nbytes), DMA_TO_DEVICE);
+
+	safexcel_free_context(priv, async, sreq->state_sz);
+
+	cache_len = sreq->len - sreq->processed;
+	if (cache_len)
+		memcpy(sreq->cache, sreq->cache_next, cache_len);
+
+	*should_complete = true;
+
+	return 1;
+}
+
+static int safexcel_ahash_send(struct crypto_async_request *async, int ring,
+			       struct safexcel_request *request, int *commands,
+			       int *results)
+{
+	struct ahash_request *areq = ahash_request_cast(async);
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(areq);
+	struct safexcel_ahash_req *req = ahash_request_ctx(areq);
+	struct safexcel_ahash_ctx *ctx = crypto_ahash_ctx(crypto_ahash_reqtfm(areq));
+	struct safexcel_crypto_priv *priv = ctx->priv;
+	struct safexcel_command_desc *cdesc, *first_cdesc = NULL;
+	struct safexcel_result_desc *rdesc;
+	struct scatterlist *sg;
+	int i, nents, queued, len, cache_len, extra, n_cdesc = 0, ret = 0;
+
+	queued = len = req->len - req->processed;
+	if (queued < crypto_ahash_blocksize(ahash))
+		cache_len = queued;
+	else
+		cache_len = queued - areq->nbytes;
+
+	/*
+	 * If this is not the last request and the queued data does not fit
+	 * into full blocks, cache it for the next send() call.
+	 */
+	extra = queued & (crypto_ahash_blocksize(ahash) - 1);
+	if (!req->last_req && extra) {
+		sg_pcopy_to_buffer(areq->src, sg_nents(areq->src),
+				   req->cache_next, extra, areq->nbytes - extra);
+
+		queued -= extra;
+		len -= extra;
+	}
+
+	spin_lock_bh(&priv->ring[ring].egress_lock);
+
+	/* Add a command descriptor for the cached data, if any */
+	if (cache_len) {
+		ctx->base.cache = kzalloc(cache_len, EIP197_GFP_FLAGS(*async));
+		if (!ctx->base.cache) {
+			ret = -ENOMEM;
+			goto unlock;
+		}
+		memcpy(ctx->base.cache, req->cache, cache_len);
+		ctx->base.cache_dma = dma_map_single(priv->dev, ctx->base.cache,
+						     cache_len, DMA_TO_DEVICE);
+		if (dma_mapping_error(priv->dev, ctx->base.cache_dma)) {
+			ret = -EINVAL;
+			goto free_cache;
+		}
+
+		ctx->base.cache_sz = cache_len;
+		first_cdesc = safexcel_add_cdesc(priv, ring, 1,
+						 (cache_len == len),
+						 ctx->base.cache_dma,
+						 cache_len, len,
+						 ctx->base.ctxr_dma);
+		if (IS_ERR(first_cdesc)) {
+			ret = PTR_ERR(first_cdesc);
+			goto unmap_cache;
+		}
+		n_cdesc++;
+
+		queued -= cache_len;
+		if (!queued)
+			goto send_command;
+	}
+
+	/* Now handle the current ahash request buffer(s) */
+	nents = dma_map_sg(priv->dev, areq->src,
+		       sg_nents_for_len(areq->src, areq->nbytes),
+		       DMA_TO_DEVICE);
+	if (!nents) {
+		ret = -ENOMEM;
+		goto cdesc_rollback;
+	}
+
+	for_each_sg(areq->src, sg, nents, i) {
+		int sglen = sg_dma_len(sg);
+
+		/* Do not overflow the request */
+		if (queued - sglen < 0)
+			sglen = queued;
+
+		cdesc = safexcel_add_cdesc(priv, ring, !n_cdesc,
+					   !(queued - sglen), sg_dma_address(sg),
+					   sglen, len, ctx->base.ctxr_dma);
+		if (IS_ERR(cdesc)) {
+			ret = PTR_ERR(cdesc);
+			goto cdesc_rollback;
+		}
+		n_cdesc++;
+
+		if (n_cdesc == 1)
+			first_cdesc = cdesc;
+
+		queued -= sglen;
+		if (!queued)
+			break;
+	}
+
+send_command:
+	/* Setup the context options */
+	safexcel_context_control(ctx, req, first_cdesc, req->state_sz,
+				 crypto_ahash_blocksize(ahash));
+
+	/* Add the token */
+	safexcel_hash_token(first_cdesc, len, req->state_sz);
+
+	ctx->base.result_dma = dma_map_single(priv->dev, areq->result,
+					      req->state_sz, DMA_FROM_DEVICE);
+	if (dma_mapping_error(priv->dev, ctx->base.result_dma)) {
+		ret = -EINVAL;
+		goto cdesc_rollback;
+	}
+
+	/* Add a result descriptor */
+	rdesc = safexcel_add_rdesc(priv, ring, 1, 1, ctx->base.result_dma,
+				   req->state_sz);
+	if (IS_ERR(rdesc)) {
+		ret = PTR_ERR(rdesc);
+		goto cdesc_rollback;
+	}
+
+	spin_unlock_bh(&priv->ring[ring].egress_lock);
+
+	req->processed += len;
+	request->req = &areq->base;
+	ctx->base.handle_result = safexcel_handle_result;
+
+	*commands = n_cdesc;
+	*results = 1;
+	return 0;
+
+cdesc_rollback:
+	for (i = 0; i < n_cdesc; i++)
+		safexcel_ring_rollback_wptr(priv, &priv->ring[ring].cdr);
+unmap_cache:
+	if (ctx->base.cache_dma) {
+		dma_unmap_single(priv->dev, ctx->base.cache_dma,
+				 ctx->base.cache_sz, DMA_TO_DEVICE);
+		ctx->base.cache_sz = 0;
+	}
+free_cache:
+	if (ctx->base.cache) {
+		kfree(ctx->base.cache);
+		ctx->base.cache = NULL;
+	}
+
+unlock:
+	spin_unlock_bh(&priv->ring[ring].egress_lock);
+	return ret;
+}
+
+static inline bool safexcel_ahash_needs_inv_get(struct ahash_request *areq)
+{
+	struct safexcel_ahash_ctx *ctx = crypto_ahash_ctx(crypto_ahash_reqtfm(areq));
+	struct safexcel_ahash_req *req = ahash_request_ctx(areq);
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(areq);
+	unsigned int state_w_sz = req->state_sz / sizeof(u32);
+	int i;
+
+	for (i = 0; i < state_w_sz; i++)
+		if (ctx->base.ctxr->data[i] != cpu_to_le32(req->state[i]))
+			return true;
+
+	if (ctx->base.ctxr->data[state_w_sz] !=
+	    cpu_to_le32(req->processed / crypto_ahash_blocksize(ahash)))
+		return true;
+
+	return false;
+}
+
+static int safexcel_handle_inv_result(struct safexcel_crypto_priv *priv,
+				      int ring,
+				      struct crypto_async_request *async,
+				      bool *should_complete, int *ret)
+{
+	struct safexcel_result_desc *rdesc;
+	struct ahash_request *areq = ahash_request_cast(async);
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(areq);
+	struct safexcel_ahash_ctx *ctx = crypto_ahash_ctx(ahash);
+	int enq_ret;
+
+	*ret = 0;
+
+	spin_lock_bh(&priv->ring[ring].egress_lock);
+	rdesc = safexcel_ring_next_rptr(priv, &priv->ring[ring].rdr);
+	if (IS_ERR(rdesc)) {
+		dev_err(priv->dev,
+			"hash: invalidate: could not retrieve the result descriptor\n");
+		*ret = PTR_ERR(rdesc);
+	} else if (rdesc->result_data.error_code) {
+		dev_err(priv->dev,
+			"hash: invalidate: result descriptor error (%d)\n",
+			rdesc->result_data.error_code);
+		*ret = -EINVAL;
+	}
+
+	safexcel_complete(priv, ring);
+	spin_unlock_bh(&priv->ring[ring].egress_lock);
+
+	if (ctx->base.exit_inv) {
+		dma_pool_free(priv->context_pool, ctx->base.ctxr,
+			      ctx->base.ctxr_dma);
+
+		*should_complete = true;
+		return 1;
+	}
+
+	ring = safexcel_select_ring(priv);
+	ctx->base.ring = ring;
+	ctx->base.needs_inv = false;
+	ctx->base.send = safexcel_ahash_send;
+
+	spin_lock_bh(&priv->ring[ring].queue_lock);
+	enq_ret = crypto_enqueue_request(&priv->ring[ring].queue, async);
+	spin_unlock_bh(&priv->ring[ring].queue_lock);
+
+	if (enq_ret != -EINPROGRESS)
+		*ret = enq_ret;
+
+	if (!priv->ring[ring].need_dequeue)
+		safexcel_dequeue(priv, ring);
+
+	*should_complete = false;
+
+	return 1;
+}
+
+static int safexcel_ahash_send_inv(struct crypto_async_request *async,
+				   int ring, struct safexcel_request *request,
+				   int *commands, int *results)
+{
+	struct ahash_request *areq = ahash_request_cast(async);
+	struct safexcel_ahash_ctx *ctx = crypto_ahash_ctx(crypto_ahash_reqtfm(areq));
+	int ret;
+
+	ctx->base.handle_result = safexcel_handle_inv_result;
+	ret = safexcel_invalidate_cache(async, &ctx->base, ctx->priv,
+					ctx->base.ctxr_dma, ring, request);
+	if (unlikely(ret))
+		return ret;
+
+	*commands = 1;
+	*results = 1;
+
+	return 0;
+}
+
+static int safexcel_ahash_exit_inv(struct crypto_tfm *tfm)
+{
+	struct safexcel_ahash_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct safexcel_crypto_priv *priv = ctx->priv;
+	struct ahash_request req;
+	struct safexcel_inv_result result = { 0 };
+	int ring = ctx->base.ring;
+
+	memset(&req, 0, sizeof(struct ahash_request));
+
+	/* create invalidation request */
+	init_completion(&result.completion);
+	ahash_request_set_callback(&req, CRYPTO_TFM_REQ_MAY_BACKLOG,
+				   safexcel_inv_complete, &result);
+
+	ahash_request_set_tfm(&req, __crypto_ahash_cast(tfm));
+	ctx = crypto_tfm_ctx(req.base.tfm);
+	ctx->base.exit_inv = true;
+	ctx->base.send = safexcel_ahash_send_inv;
+
+	spin_lock_bh(&priv->ring[ring].queue_lock);
+	crypto_enqueue_request(&priv->ring[ring].queue, &req.base);
+	spin_unlock_bh(&priv->ring[ring].queue_lock);
+
+	if (!priv->ring[ring].need_dequeue)
+		safexcel_dequeue(priv, ring);
+
+	wait_for_completion_interruptible(&result.completion);
+
+	if (result.error) {
+		dev_warn(priv->dev, "hash: completion error (%d)\n",
+			 result.error);
+		return result.error;
+	}
+
+	return 0;
+}
+
+static int safexcel_ahash_cache(struct ahash_request *areq)
+{
+	struct safexcel_ahash_req *req = ahash_request_ctx(areq);
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(areq);
+	int queued, cache_len;
+
+	cache_len = req->len - areq->nbytes - req->processed;
+	queued = req->len - req->processed;
+
+	/*
+	 * In case there isn't enough bytes to proceed (less than a
+	 * block size), cache the data until we have enough.
+	 */
+	if (cache_len + areq->nbytes <= crypto_ahash_blocksize(ahash)) {
+		sg_pcopy_to_buffer(areq->src, sg_nents(areq->src),
+				   req->cache + cache_len,
+				   areq->nbytes, 0);
+		return areq->nbytes;
+	}
+
+	/* We could'nt cache all the data */
+	return -E2BIG;
+}
+
+static int safexcel_ahash_enqueue(struct ahash_request *areq)
+{
+	struct safexcel_ahash_ctx *ctx = crypto_ahash_ctx(crypto_ahash_reqtfm(areq));
+	struct safexcel_ahash_req *req = ahash_request_ctx(areq);
+	struct safexcel_crypto_priv *priv = ctx->priv;
+	int ret, ring;
+
+	ctx->base.send = safexcel_ahash_send;
+
+	if (req->processed && ctx->digest == CONTEXT_CONTROL_DIGEST_PRECOMPUTED)
+		ctx->base.needs_inv = safexcel_ahash_needs_inv_get(areq);
+
+	if (ctx->base.ctxr) {
+		if (ctx->base.needs_inv)
+			ctx->base.send = safexcel_ahash_send_inv;
+	} else {
+		ctx->base.ring = safexcel_select_ring(priv);
+		ctx->base.ctxr = dma_pool_zalloc(priv->context_pool,
+						 EIP197_GFP_FLAGS(areq->base),
+						 &ctx->base.ctxr_dma);
+		if (!ctx->base.ctxr)
+			return -ENOMEM;
+	}
+
+	ring = ctx->base.ring;
+
+	spin_lock_bh(&priv->ring[ring].queue_lock);
+	ret = crypto_enqueue_request(&priv->ring[ring].queue, &areq->base);
+	spin_unlock_bh(&priv->ring[ring].queue_lock);
+
+	if (!priv->ring[ring].need_dequeue)
+		safexcel_dequeue(priv, ring);
+
+	return ret;
+}
+
+static int safexcel_ahash_update(struct ahash_request *areq)
+{
+	struct safexcel_ahash_ctx *ctx = crypto_ahash_ctx(crypto_ahash_reqtfm(areq));
+	struct safexcel_ahash_req *req = ahash_request_ctx(areq);
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(areq);
+
+	/* If the request is 0 length, do nothing */
+	if (!areq->nbytes)
+		return 0;
+
+	req->len += areq->nbytes;
+
+	safexcel_ahash_cache(areq);
+
+	/*
+	 * We're not doing partial updates when performing an hmac request.
+	 * Everything will be handled by the final() call.
+	 */
+	if (ctx->digest == CONTEXT_CONTROL_DIGEST_HMAC)
+		return 0;
+
+	if (req->hmac)
+		return safexcel_ahash_enqueue(areq);
+
+	if (!req->last_req &&
+	    req->len - req->processed > crypto_ahash_blocksize(ahash))
+		return safexcel_ahash_enqueue(areq);
+
+	return 0;
+}
+
+static int safexcel_ahash_final(struct ahash_request *areq)
+{
+	struct safexcel_ahash_req *req = ahash_request_ctx(areq);
+	struct safexcel_ahash_ctx *ctx = crypto_ahash_ctx(crypto_ahash_reqtfm(areq));
+
+	req->last_req = true;
+	req->finish = true;
+
+	/* If we have an overall 0 length request */
+	if (!(req->len + areq->nbytes)) {
+		if (ctx->alg == CONTEXT_CONTROL_CRYPTO_ALG_SHA1)
+			memcpy(areq->result, sha1_zero_message_hash,
+			       SHA1_DIGEST_SIZE);
+		else if (ctx->alg == CONTEXT_CONTROL_CRYPTO_ALG_SHA224)
+			memcpy(areq->result, sha224_zero_message_hash,
+			       SHA224_DIGEST_SIZE);
+		else if (ctx->alg == CONTEXT_CONTROL_CRYPTO_ALG_SHA256)
+			memcpy(areq->result, sha256_zero_message_hash,
+			       SHA256_DIGEST_SIZE);
+
+		return 0;
+	}
+
+	return safexcel_ahash_enqueue(areq);
+}
+
+static int safexcel_ahash_finup(struct ahash_request *areq)
+{
+	struct safexcel_ahash_req *req = ahash_request_ctx(areq);
+
+	req->last_req = true;
+	req->finish = true;
+
+	safexcel_ahash_update(areq);
+	return safexcel_ahash_final(areq);
+}
+
+static int safexcel_ahash_export(struct ahash_request *areq, void *out)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(areq);
+	struct safexcel_ahash_req *req = ahash_request_ctx(areq);
+	struct safexcel_ahash_export_state *export = out;
+
+	export->len = req->len;
+	export->processed = req->processed;
+
+	memcpy(export->state, req->state, req->state_sz);
+	memset(export->cache, 0, crypto_ahash_blocksize(ahash));
+	memcpy(export->cache, req->cache, crypto_ahash_blocksize(ahash));
+
+	return 0;
+}
+
+static int safexcel_ahash_import(struct ahash_request *areq, const void *in)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(areq);
+	struct safexcel_ahash_req *req = ahash_request_ctx(areq);
+	const struct safexcel_ahash_export_state *export = in;
+	int ret;
+
+	ret = crypto_ahash_init(areq);
+	if (ret)
+		return ret;
+
+	req->len = export->len;
+	req->processed = export->processed;
+
+	memcpy(req->cache, export->cache, crypto_ahash_blocksize(ahash));
+	memcpy(req->state, export->state, req->state_sz);
+
+	return 0;
+}
+
+static int safexcel_ahash_cra_init(struct crypto_tfm *tfm)
+{
+	struct safexcel_ahash_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct safexcel_alg_template *tmpl =
+		container_of(__crypto_ahash_alg(tfm->__crt_alg),
+			     struct safexcel_alg_template, alg.ahash);
+
+	ctx->priv = tmpl->priv;
+
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+				 sizeof(struct safexcel_ahash_req));
+	return 0;
+}
+
+static int safexcel_sha1_init(struct ahash_request *areq)
+{
+	struct safexcel_ahash_ctx *ctx = crypto_ahash_ctx(crypto_ahash_reqtfm(areq));
+	struct safexcel_ahash_req *req = ahash_request_ctx(areq);
+
+	memset(req, 0, sizeof(*req));
+
+	req->state[0] = SHA1_H0;
+	req->state[1] = SHA1_H1;
+	req->state[2] = SHA1_H2;
+	req->state[3] = SHA1_H3;
+	req->state[4] = SHA1_H4;
+
+	ctx->alg = CONTEXT_CONTROL_CRYPTO_ALG_SHA1;
+	ctx->digest = CONTEXT_CONTROL_DIGEST_PRECOMPUTED;
+	req->state_sz = SHA1_DIGEST_SIZE;
+
+	return 0;
+}
+
+static int safexcel_sha1_digest(struct ahash_request *areq)
+{
+	int ret = safexcel_sha1_init(areq);
+
+	if (ret)
+		return ret;
+
+	return safexcel_ahash_finup(areq);
+}
+
+static void safexcel_ahash_cra_exit(struct crypto_tfm *tfm)
+{
+	struct safexcel_ahash_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct safexcel_crypto_priv *priv = ctx->priv;
+	int ret;
+
+	/* context not allocated, skip invalidation */
+	if (!ctx->base.ctxr)
+		return;
+
+	ret = safexcel_ahash_exit_inv(tfm);
+	if (ret)
+		dev_warn(priv->dev, "hash: invalidation error %d\n", ret);
+}
+
+struct safexcel_alg_template safexcel_alg_sha1 = {
+	.type = SAFEXCEL_ALG_TYPE_AHASH,
+	.alg.ahash = {
+		.init = safexcel_sha1_init,
+		.update = safexcel_ahash_update,
+		.final = safexcel_ahash_final,
+		.finup = safexcel_ahash_finup,
+		.digest = safexcel_sha1_digest,
+		.export = safexcel_ahash_export,
+		.import = safexcel_ahash_import,
+		.halg = {
+			.digestsize = SHA1_DIGEST_SIZE,
+			.statesize = sizeof(struct safexcel_ahash_export_state),
+			.base = {
+				.cra_name = "sha1",
+				.cra_driver_name = "safexcel-sha1",
+				.cra_priority = 300,
+				.cra_flags = CRYPTO_ALG_ASYNC |
+					     CRYPTO_ALG_KERN_DRIVER_ONLY,
+				.cra_blocksize = SHA1_BLOCK_SIZE,
+				.cra_ctxsize = sizeof(struct safexcel_ahash_ctx),
+				.cra_init = safexcel_ahash_cra_init,
+				.cra_exit = safexcel_ahash_cra_exit,
+				.cra_module = THIS_MODULE,
+			},
+		},
+	},
+};
+
+static int safexcel_hmac_sha1_init(struct ahash_request *areq)
+{
+	struct safexcel_ahash_ctx *ctx = crypto_ahash_ctx(crypto_ahash_reqtfm(areq));
+
+	safexcel_sha1_init(areq);
+	ctx->digest = CONTEXT_CONTROL_DIGEST_HMAC;
+	return 0;
+}
+
+static int safexcel_hmac_sha1_digest(struct ahash_request *areq)
+{
+	int ret = safexcel_hmac_sha1_init(areq);
+
+	if (ret)
+		return ret;
+
+	return safexcel_ahash_finup(areq);
+}
+
+struct safexcel_ahash_result {
+	struct completion completion;
+	int error;
+};
+
+static void safexcel_ahash_complete(struct crypto_async_request *req, int error)
+{
+	struct safexcel_ahash_result *result = req->data;
+
+	if (error == -EINPROGRESS)
+		return;
+
+	result->error = error;
+	complete(&result->completion);
+}
+
+static int safexcel_hmac_init_pad(struct ahash_request *areq,
+				  unsigned int blocksize, const u8 *key,
+				  unsigned int keylen, u8 *ipad, u8 *opad)
+{
+	struct safexcel_ahash_result result;
+	struct scatterlist sg;
+	int ret, i;
+	u8 *keydup;
+
+	if (keylen <= blocksize) {
+		memcpy(ipad, key, keylen);
+	} else {
+		keydup = kmemdup(key, keylen, GFP_KERNEL);
+		if (!keydup)
+			return -ENOMEM;
+
+		ahash_request_set_callback(areq, CRYPTO_TFM_REQ_MAY_BACKLOG,
+					   safexcel_ahash_complete, &result);
+		sg_init_one(&sg, keydup, keylen);
+		ahash_request_set_crypt(areq, &sg, ipad, keylen);
+		init_completion(&result.completion);
+
+		ret = crypto_ahash_digest(areq);
+		if (ret == -EINPROGRESS) {
+			wait_for_completion_interruptible(&result.completion);
+			ret = result.error;
+		}
+
+		/* Avoid leaking */
+		memzero_explicit(keydup, keylen);
+		kfree(keydup);
+
+		if (ret)
+			return ret;
+
+		keylen = crypto_ahash_digestsize(crypto_ahash_reqtfm(areq));
+	}
+
+	memset(ipad + keylen, 0, blocksize - keylen);
+	memcpy(opad, ipad, blocksize);
+
+	for (i = 0; i < blocksize; i++) {
+		ipad[i] ^= HMAC_IPAD_VALUE;
+		opad[i] ^= HMAC_OPAD_VALUE;
+	}
+
+	return 0;
+}
+
+static int safexcel_hmac_init_iv(struct ahash_request *areq,
+				 unsigned int blocksize, u8 *pad, void *state)
+{
+	struct safexcel_ahash_result result;
+	struct safexcel_ahash_req *req;
+	struct scatterlist sg;
+	int ret;
+
+	ahash_request_set_callback(areq, CRYPTO_TFM_REQ_MAY_BACKLOG,
+				   safexcel_ahash_complete, &result);
+	sg_init_one(&sg, pad, blocksize);
+	ahash_request_set_crypt(areq, &sg, pad, blocksize);
+	init_completion(&result.completion);
+
+	ret = crypto_ahash_init(areq);
+	if (ret)
+		return ret;
+
+	req = ahash_request_ctx(areq);
+	req->hmac = true;
+	req->last_req = true;
+
+	ret = crypto_ahash_update(areq);
+	if (ret && ret != -EINPROGRESS)
+		return ret;
+
+	wait_for_completion_interruptible(&result.completion);
+	if (result.error)
+		return result.error;
+
+	return crypto_ahash_export(areq, state);
+}
+
+static int safexcel_hmac_setkey(const char *alg, const u8 *key,
+				unsigned int keylen, void *istate, void *ostate)
+{
+	struct ahash_request *areq;
+	struct crypto_ahash *tfm;
+	unsigned int blocksize;
+	u8 *ipad, *opad;
+	int ret;
+
+	tfm = crypto_alloc_ahash(alg, CRYPTO_ALG_TYPE_AHASH,
+				 CRYPTO_ALG_TYPE_AHASH_MASK);
+	if (IS_ERR(tfm))
+		return PTR_ERR(tfm);
+
+	areq = ahash_request_alloc(tfm, GFP_KERNEL);
+	if (!areq) {
+		ret = -ENOMEM;
+		goto free_ahash;
+	}
+
+	crypto_ahash_clear_flags(tfm, ~0);
+	blocksize = crypto_tfm_alg_blocksize(crypto_ahash_tfm(tfm));
+
+	ipad = kzalloc(2 * blocksize, GFP_KERNEL);
+	if (!ipad) {
+		ret = -ENOMEM;
+		goto free_request;
+	}
+
+	opad = ipad + blocksize;
+
+	ret = safexcel_hmac_init_pad(areq, blocksize, key, keylen, ipad, opad);
+	if (ret)
+		goto free_ipad;
+
+	ret = safexcel_hmac_init_iv(areq, blocksize, ipad, istate);
+	if (ret)
+		goto free_ipad;
+
+	ret = safexcel_hmac_init_iv(areq, blocksize, opad, ostate);
+
+free_ipad:
+	kfree(ipad);
+free_request:
+	ahash_request_free(areq);
+free_ahash:
+	crypto_free_ahash(tfm);
+
+	return ret;
+}
+
+static int safexcel_hmac_sha1_setkey(struct crypto_ahash *tfm, const u8 *key,
+				     unsigned int keylen)
+{
+	struct safexcel_ahash_ctx *ctx = crypto_tfm_ctx(crypto_ahash_tfm(tfm));
+	struct safexcel_ahash_export_state istate, ostate;
+	int ret, i;
+
+	ret = safexcel_hmac_setkey("safexcel-sha1", key, keylen, &istate, &ostate);
+	if (ret)
+		return ret;
+
+	memcpy(ctx->ipad, &istate.state, SHA1_DIGEST_SIZE);
+	memcpy(ctx->opad, &ostate.state, SHA1_DIGEST_SIZE);
+
+	for (i = 0; i < ARRAY_SIZE(istate.state); i++) {
+		if (ctx->ipad[i] != le32_to_cpu(istate.state[i]) ||
+		    ctx->opad[i] != le32_to_cpu(ostate.state[i])) {
+			ctx->base.needs_inv = true;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+struct safexcel_alg_template safexcel_alg_hmac_sha1 = {
+	.type = SAFEXCEL_ALG_TYPE_AHASH,
+	.alg.ahash = {
+		.init = safexcel_hmac_sha1_init,
+		.update = safexcel_ahash_update,
+		.final = safexcel_ahash_final,
+		.finup = safexcel_ahash_finup,
+		.digest = safexcel_hmac_sha1_digest,
+		.setkey = safexcel_hmac_sha1_setkey,
+		.export = safexcel_ahash_export,
+		.import = safexcel_ahash_import,
+		.halg = {
+			.digestsize = SHA1_DIGEST_SIZE,
+			.statesize = sizeof(struct safexcel_ahash_export_state),
+			.base = {
+				.cra_name = "hmac(sha1)",
+				.cra_driver_name = "safexcel-hmac-sha1",
+				.cra_priority = 300,
+				.cra_flags = CRYPTO_ALG_ASYNC |
+					     CRYPTO_ALG_KERN_DRIVER_ONLY,
+				.cra_blocksize = SHA1_BLOCK_SIZE,
+				.cra_ctxsize = sizeof(struct safexcel_ahash_ctx),
+				.cra_init = safexcel_ahash_cra_init,
+				.cra_exit = safexcel_ahash_cra_exit,
+				.cra_module = THIS_MODULE,
+			},
+		},
+	},
+};
+
+static int safexcel_sha256_init(struct ahash_request *areq)
+{
+	struct safexcel_ahash_ctx *ctx = crypto_ahash_ctx(crypto_ahash_reqtfm(areq));
+	struct safexcel_ahash_req *req = ahash_request_ctx(areq);
+
+	memset(req, 0, sizeof(*req));
+
+	req->state[0] = SHA256_H0;
+	req->state[1] = SHA256_H1;
+	req->state[2] = SHA256_H2;
+	req->state[3] = SHA256_H3;
+	req->state[4] = SHA256_H4;
+	req->state[5] = SHA256_H5;
+	req->state[6] = SHA256_H6;
+	req->state[7] = SHA256_H7;
+
+	ctx->alg = CONTEXT_CONTROL_CRYPTO_ALG_SHA256;
+	ctx->digest = CONTEXT_CONTROL_DIGEST_PRECOMPUTED;
+	req->state_sz = SHA256_DIGEST_SIZE;
+
+	return 0;
+}
+
+static int safexcel_sha256_digest(struct ahash_request *areq)
+{
+	int ret = safexcel_sha256_init(areq);
+
+	if (ret)
+		return ret;
+
+	return safexcel_ahash_finup(areq);
+}
+
+struct safexcel_alg_template safexcel_alg_sha256 = {
+	.type = SAFEXCEL_ALG_TYPE_AHASH,
+	.alg.ahash = {
+		.init = safexcel_sha256_init,
+		.update = safexcel_ahash_update,
+		.final = safexcel_ahash_final,
+		.finup = safexcel_ahash_finup,
+		.digest = safexcel_sha256_digest,
+		.export = safexcel_ahash_export,
+		.import = safexcel_ahash_import,
+		.halg = {
+			.digestsize = SHA256_DIGEST_SIZE,
+			.statesize = sizeof(struct safexcel_ahash_export_state),
+			.base = {
+				.cra_name = "sha256",
+				.cra_driver_name = "safexcel-sha256",
+				.cra_priority = 300,
+				.cra_flags = CRYPTO_ALG_ASYNC |
+					     CRYPTO_ALG_KERN_DRIVER_ONLY,
+				.cra_blocksize = SHA256_BLOCK_SIZE,
+				.cra_ctxsize = sizeof(struct safexcel_ahash_ctx),
+				.cra_init = safexcel_ahash_cra_init,
+				.cra_exit = safexcel_ahash_cra_exit,
+				.cra_module = THIS_MODULE,
+			},
+		},
+	},
+};
+
+static int safexcel_sha224_init(struct ahash_request *areq)
+{
+	struct safexcel_ahash_ctx *ctx = crypto_ahash_ctx(crypto_ahash_reqtfm(areq));
+	struct safexcel_ahash_req *req = ahash_request_ctx(areq);
+
+	memset(req, 0, sizeof(*req));
+
+	req->state[0] = SHA224_H0;
+	req->state[1] = SHA224_H1;
+	req->state[2] = SHA224_H2;
+	req->state[3] = SHA224_H3;
+	req->state[4] = SHA224_H4;
+	req->state[5] = SHA224_H5;
+	req->state[6] = SHA224_H6;
+	req->state[7] = SHA224_H7;
+
+	ctx->alg = CONTEXT_CONTROL_CRYPTO_ALG_SHA224;
+	ctx->digest = CONTEXT_CONTROL_DIGEST_PRECOMPUTED;
+	req->state_sz = SHA256_DIGEST_SIZE;
+
+	return 0;
+}
+
+static int safexcel_sha224_digest(struct ahash_request *areq)
+{
+	int ret = safexcel_sha224_init(areq);
+
+	if (ret)
+		return ret;
+
+	return safexcel_ahash_finup(areq);
+}
+
+struct safexcel_alg_template safexcel_alg_sha224 = {
+	.type = SAFEXCEL_ALG_TYPE_AHASH,
+	.alg.ahash = {
+		.init = safexcel_sha224_init,
+		.update = safexcel_ahash_update,
+		.final = safexcel_ahash_final,
+		.finup = safexcel_ahash_finup,
+		.digest = safexcel_sha224_digest,
+		.export = safexcel_ahash_export,
+		.import = safexcel_ahash_import,
+		.halg = {
+			.digestsize = SHA224_DIGEST_SIZE,
+			.statesize = sizeof(struct safexcel_ahash_export_state),
+			.base = {
+				.cra_name = "sha224",
+				.cra_driver_name = "safexcel-sha224",
+				.cra_priority = 300,
+				.cra_flags = CRYPTO_ALG_ASYNC |
+					     CRYPTO_ALG_KERN_DRIVER_ONLY,
+				.cra_blocksize = SHA224_BLOCK_SIZE,
+				.cra_ctxsize = sizeof(struct safexcel_ahash_ctx),
+				.cra_init = safexcel_ahash_cra_init,
+				.cra_exit = safexcel_ahash_cra_exit,
+				.cra_module = THIS_MODULE,
+			},
+		},
+	},
+};
diff --git a/drivers/crypto/inside-secure/safexcel_ring.c b/drivers/crypto/inside-secure/safexcel_ring.c
new file mode 100644
index 000000000000..c9d2a8716b5b
--- /dev/null
+++ b/drivers/crypto/inside-secure/safexcel_ring.c
@@ -0,0 +1,157 @@
+/*
+ * Copyright (C) 2017 Marvell
+ *
+ * Antoine Tenart <antoine.tenart@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/spinlock.h>
+
+#include "safexcel.h"
+
+int safexcel_init_ring_descriptors(struct safexcel_crypto_priv *priv,
+				   struct safexcel_ring *cdr,
+				   struct safexcel_ring *rdr)
+{
+	cdr->offset = sizeof(u32) * priv->config.cd_offset;
+	cdr->base = dmam_alloc_coherent(priv->dev,
+					cdr->offset * EIP197_DEFAULT_RING_SIZE,
+					&cdr->base_dma, GFP_KERNEL);
+	if (!cdr->base)
+		return -ENOMEM;
+	cdr->write = cdr->base;
+	cdr->base_end = cdr->base + cdr->offset * EIP197_DEFAULT_RING_SIZE;
+	cdr->read = cdr->base;
+
+	rdr->offset = sizeof(u32) * priv->config.rd_offset;
+	rdr->base = dmam_alloc_coherent(priv->dev,
+					rdr->offset * EIP197_DEFAULT_RING_SIZE,
+					&rdr->base_dma, GFP_KERNEL);
+	if (!rdr->base)
+		return -ENOMEM;
+	rdr->write = rdr->base;
+	rdr->base_end = rdr->base + rdr->offset * EIP197_DEFAULT_RING_SIZE;
+	rdr->read = rdr->base;
+
+	return 0;
+}
+
+inline int safexcel_select_ring(struct safexcel_crypto_priv *priv)
+{
+	return (atomic_inc_return(&priv->ring_used) % priv->config.rings);
+}
+
+static void *safexcel_ring_next_wptr(struct safexcel_crypto_priv *priv,
+				     struct safexcel_ring *ring)
+{
+	void *ptr = ring->write;
+
+	if (ring->nr == EIP197_DEFAULT_RING_SIZE - 1)
+		return ERR_PTR(-ENOMEM);
+
+	ring->write += ring->offset;
+	if (ring->write == ring->base_end)
+		ring->write = ring->base;
+
+	ring->nr++;
+	return ptr;
+}
+
+void *safexcel_ring_next_rptr(struct safexcel_crypto_priv *priv,
+			      struct safexcel_ring *ring)
+{
+	void *ptr = ring->read;
+
+	if (!ring->nr)
+		return ERR_PTR(-ENOENT);
+
+	ring->read += ring->offset;
+	if (ring->read == ring->base_end)
+		ring->read = ring->base;
+
+	ring->nr--;
+	return ptr;
+}
+
+void safexcel_ring_rollback_wptr(struct safexcel_crypto_priv *priv,
+				 struct safexcel_ring *ring)
+{
+	if (!ring->nr)
+		return;
+
+	if (ring->write == ring->base)
+		ring->write = ring->base_end - ring->offset;
+	else
+		ring->write -= ring->offset;
+
+	ring->nr--;
+}
+
+struct safexcel_command_desc *safexcel_add_cdesc(struct safexcel_crypto_priv *priv,
+						 int ring_id,
+						 bool first, bool last,
+						 dma_addr_t data, u32 data_len,
+						 u32 full_data_len,
+						 dma_addr_t context) {
+	struct safexcel_command_desc *cdesc;
+	int i;
+
+	cdesc = safexcel_ring_next_wptr(priv, &priv->ring[ring_id].cdr);
+	if (IS_ERR(cdesc))
+		return cdesc;
+
+	memset(cdesc, 0, sizeof(struct safexcel_command_desc));
+
+	cdesc->first_seg = first;
+	cdesc->last_seg = last;
+	cdesc->particle_size = data_len;
+	cdesc->data_lo = lower_32_bits(data);
+	cdesc->data_hi = upper_32_bits(data);
+
+	if (first && context) {
+		struct safexcel_token *token =
+			(struct safexcel_token *)cdesc->control_data.token;
+
+		cdesc->control_data.packet_length = full_data_len;
+		cdesc->control_data.options = EIP197_OPTION_MAGIC_VALUE |
+					      EIP197_OPTION_64BIT_CTX |
+					      EIP197_OPTION_CTX_CTRL_IN_CMD;
+		cdesc->control_data.context_lo =
+			(lower_32_bits(context) & GENMASK(31, 2)) >> 2;
+		cdesc->control_data.context_hi = upper_32_bits(context);
+
+		/* TODO: large xform HMAC with SHA-384/512 uses refresh = 3 */
+		cdesc->control_data.refresh = 2;
+
+		for (i = 0; i < EIP197_MAX_TOKENS; i++)
+			eip197_noop_token(&token[i]);
+	}
+
+	return cdesc;
+}
+
+struct safexcel_result_desc *safexcel_add_rdesc(struct safexcel_crypto_priv *priv,
+						int ring_id,
+						bool first, bool last,
+						dma_addr_t data, u32 len)
+{
+	struct safexcel_result_desc *rdesc;
+
+	rdesc = safexcel_ring_next_wptr(priv, &priv->ring[ring_id].rdr);
+	if (IS_ERR(rdesc))
+		return rdesc;
+
+	memset(rdesc, 0, sizeof(struct safexcel_result_desc));
+
+	rdesc->first_seg = first;
+	rdesc->last_seg = last;
+	rdesc->particle_size = len;
+	rdesc->data_lo = lower_32_bits(data);
+	rdesc->data_hi = upper_32_bits(data);
+
+	return rdesc;
+}
diff --git a/drivers/crypto/ixp4xx_crypto.c b/drivers/crypto/ixp4xx_crypto.c
index 771dd26c7076..427cbe012729 100644
--- a/drivers/crypto/ixp4xx_crypto.c
+++ b/drivers/crypto/ixp4xx_crypto.c
@@ -23,6 +23,7 @@
 #include <crypto/ctr.h>
 #include <crypto/des.h>
 #include <crypto/aes.h>
+#include <crypto/hmac.h>
 #include <crypto/sha.h>
 #include <crypto/algapi.h>
 #include <crypto/internal/aead.h>
@@ -90,8 +91,6 @@
 #define CTL_FLAG_PERFORM_AEAD	0x0008
 #define CTL_FLAG_MASK		0x000f
 
-#define HMAC_IPAD_VALUE   0x36
-#define HMAC_OPAD_VALUE   0x5C
 #define HMAC_PAD_BLOCKLEN SHA1_BLOCK_SIZE
 
 #define MD5_DIGEST_SIZE   16
diff --git a/drivers/crypto/marvell/hash.c b/drivers/crypto/marvell/hash.c
index 77c0fb936f47..e61b08566093 100644
--- a/drivers/crypto/marvell/hash.c
+++ b/drivers/crypto/marvell/hash.c
@@ -12,6 +12,7 @@
  * by the Free Software Foundation.
  */
 
+#include <crypto/hmac.h>
 #include <crypto/md5.h>
 #include <crypto/sha.h>
 
@@ -1164,8 +1165,8 @@ static int mv_cesa_ahmac_pad_init(struct ahash_request *req,
 	memcpy(opad, ipad, blocksize);
 
 	for (i = 0; i < blocksize; i++) {
-		ipad[i] ^= 0x36;
-		opad[i] ^= 0x5c;
+		ipad[i] ^= HMAC_IPAD_VALUE;
+		opad[i] ^= HMAC_OPAD_VALUE;
 	}
 
 	return 0;
diff --git a/drivers/crypto/mediatek/mtk-platform.c b/drivers/crypto/mediatek/mtk-platform.c
index b6ecc288b540..000b6500a22d 100644
--- a/drivers/crypto/mediatek/mtk-platform.c
+++ b/drivers/crypto/mediatek/mtk-platform.c
@@ -504,19 +504,14 @@ static int mtk_crypto_probe(struct platform_device *pdev)
 		}
 	}
 
-	cryp->clk_ethif = devm_clk_get(&pdev->dev, "ethif");
 	cryp->clk_cryp = devm_clk_get(&pdev->dev, "cryp");
-	if (IS_ERR(cryp->clk_ethif) || IS_ERR(cryp->clk_cryp))
+	if (IS_ERR(cryp->clk_cryp))
 		return -EPROBE_DEFER;
 
 	cryp->dev = &pdev->dev;
 	pm_runtime_enable(cryp->dev);
 	pm_runtime_get_sync(cryp->dev);
 
-	err = clk_prepare_enable(cryp->clk_ethif);
-	if (err)
-		goto err_clk_ethif;
-
 	err = clk_prepare_enable(cryp->clk_cryp);
 	if (err)
 		goto err_clk_cryp;
@@ -559,8 +554,6 @@ err_engine:
 err_resource:
 	clk_disable_unprepare(cryp->clk_cryp);
 err_clk_cryp:
-	clk_disable_unprepare(cryp->clk_ethif);
-err_clk_ethif:
 	pm_runtime_put_sync(cryp->dev);
 	pm_runtime_disable(cryp->dev);
 
@@ -576,7 +569,6 @@ static int mtk_crypto_remove(struct platform_device *pdev)
 	mtk_desc_dma_free(cryp);
 
 	clk_disable_unprepare(cryp->clk_cryp);
-	clk_disable_unprepare(cryp->clk_ethif);
 
 	pm_runtime_put_sync(cryp->dev);
 	pm_runtime_disable(cryp->dev);
@@ -596,7 +588,6 @@ static struct platform_driver mtk_crypto_driver = {
 	.remove = mtk_crypto_remove,
 	.driver = {
 		   .name = "mtk-crypto",
-		   .owner = THIS_MODULE,
 		   .of_match_table = of_crypto_id,
 	},
 };
diff --git a/drivers/crypto/mediatek/mtk-platform.h b/drivers/crypto/mediatek/mtk-platform.h
index 303c152dc931..f0831f1742ab 100644
--- a/drivers/crypto/mediatek/mtk-platform.h
+++ b/drivers/crypto/mediatek/mtk-platform.h
@@ -200,7 +200,6 @@ struct mtk_sha_rec {
  * struct mtk_cryp - Cryptographic device
  * @base:	pointer to mapped register I/O base
  * @dev:	pointer to device
- * @clk_ethif:	pointer to ethif clock
  * @clk_cryp:	pointer to crypto clock
  * @irq:	global system and rings IRQ
  * @ring:	pointer to descriptor rings
@@ -215,7 +214,6 @@ struct mtk_sha_rec {
 struct mtk_cryp {
 	void __iomem *base;
 	struct device *dev;
-	struct clk *clk_ethif;
 	struct clk *clk_cryp;
 	int irq[MTK_IRQ_NUM];
 
diff --git a/drivers/crypto/mediatek/mtk-sha.c b/drivers/crypto/mediatek/mtk-sha.c
index 2226f12d1c7a..5f4f845adbb8 100644
--- a/drivers/crypto/mediatek/mtk-sha.c
+++ b/drivers/crypto/mediatek/mtk-sha.c
@@ -12,6 +12,7 @@
  * Some ideas are from atmel-sha.c and omap-sham.c drivers.
  */
 
+#include <crypto/hmac.h>
 #include <crypto/sha.h>
 #include "mtk-platform.h"
 
@@ -825,8 +826,8 @@ static int mtk_sha_setkey(struct crypto_ahash *tfm, const u8 *key,
 	memcpy(bctx->opad, bctx->ipad, bs);
 
 	for (i = 0; i < bs; i++) {
-		bctx->ipad[i] ^= 0x36;
-		bctx->opad[i] ^= 0x5c;
+		bctx->ipad[i] ^= HMAC_IPAD_VALUE;
+		bctx->opad[i] ^= HMAC_OPAD_VALUE;
 	}
 
 	return 0;
diff --git a/drivers/crypto/mv_cesa.c b/drivers/crypto/mv_cesa.c
index 451fa18c1c7b..bf25f415eea6 100644
--- a/drivers/crypto/mv_cesa.c
+++ b/drivers/crypto/mv_cesa.c
@@ -18,6 +18,7 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/clk.h>
+#include <crypto/hmac.h>
 #include <crypto/internal/hash.h>
 #include <crypto/sha.h>
 #include <linux/of.h>
@@ -822,8 +823,8 @@ static int mv_hash_setkey(struct crypto_ahash *tfm, const u8 * key,
 		memcpy(opad, ipad, bs);
 
 		for (i = 0; i < bs; i++) {
-			ipad[i] ^= 0x36;
-			opad[i] ^= 0x5c;
+			ipad[i] ^= HMAC_IPAD_VALUE;
+			opad[i] ^= HMAC_OPAD_VALUE;
 		}
 
 		rc = crypto_shash_init(shash) ? :
diff --git a/drivers/crypto/n2_core.c b/drivers/crypto/n2_core.c
index 4ecb77aa60e1..269451375b63 100644
--- a/drivers/crypto/n2_core.c
+++ b/drivers/crypto/n2_core.c
@@ -2169,7 +2169,7 @@ static int n2_mau_remove(struct platform_device *dev)
 	return 0;
 }
 
-static struct of_device_id n2_crypto_match[] = {
+static const struct of_device_id n2_crypto_match[] = {
 	{
 		.name = "n2cp",
 		.compatible = "SUNW,n2-cwq",
@@ -2196,7 +2196,7 @@ static struct platform_driver n2_crypto_driver = {
 	.remove		=	n2_crypto_remove,
 };
 
-static struct of_device_id n2_mau_match[] = {
+static const struct of_device_id n2_mau_match[] = {
 	{
 		.name = "ncp",
 		.compatible = "SUNW,n2-mau",
diff --git a/drivers/crypto/omap-aes-gcm.c b/drivers/crypto/omap-aes-gcm.c
new file mode 100644
index 000000000000..7d4f8a4be6d8
--- /dev/null
+++ b/drivers/crypto/omap-aes-gcm.c
@@ -0,0 +1,408 @@
+/*
+ * Cryptographic API.
+ *
+ * Support for OMAP AES GCM HW acceleration.
+ *
+ * Copyright (c) 2016 Texas Instruments Incorporated
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ *
+ */
+
+#include <linux/errno.h>
+#include <linux/scatterlist.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/omap-dma.h>
+#include <linux/interrupt.h>
+#include <crypto/aes.h>
+#include <crypto/scatterwalk.h>
+#include <crypto/skcipher.h>
+#include <crypto/internal/aead.h>
+
+#include "omap-crypto.h"
+#include "omap-aes.h"
+
+static int omap_aes_gcm_handle_queue(struct omap_aes_dev *dd,
+				     struct aead_request *req);
+
+static void omap_aes_gcm_finish_req(struct omap_aes_dev *dd, int ret)
+{
+	struct aead_request *req = dd->aead_req;
+
+	dd->flags &= ~FLAGS_BUSY;
+	dd->in_sg = NULL;
+	dd->out_sg = NULL;
+
+	req->base.complete(&req->base, ret);
+}
+
+static void omap_aes_gcm_done_task(struct omap_aes_dev *dd)
+{
+	u8 *tag;
+	int alen, clen, i, ret = 0, nsg;
+	struct omap_aes_reqctx *rctx;
+
+	alen = ALIGN(dd->assoc_len, AES_BLOCK_SIZE);
+	clen = ALIGN(dd->total, AES_BLOCK_SIZE);
+	rctx = aead_request_ctx(dd->aead_req);
+
+	nsg = !!(dd->assoc_len && dd->total);
+
+	dma_sync_sg_for_device(dd->dev, dd->out_sg, dd->out_sg_len,
+			       DMA_FROM_DEVICE);
+	dma_unmap_sg(dd->dev, dd->in_sg, dd->in_sg_len, DMA_TO_DEVICE);
+	dma_unmap_sg(dd->dev, dd->out_sg, dd->out_sg_len, DMA_FROM_DEVICE);
+	omap_aes_crypt_dma_stop(dd);
+
+	omap_crypto_cleanup(dd->out_sg, dd->orig_out,
+			    dd->aead_req->assoclen, dd->total,
+			    FLAGS_OUT_DATA_ST_SHIFT, dd->flags);
+
+	if (dd->flags & FLAGS_ENCRYPT)
+		scatterwalk_map_and_copy(rctx->auth_tag,
+					 dd->aead_req->dst,
+					 dd->total + dd->aead_req->assoclen,
+					 dd->authsize, 1);
+
+	omap_crypto_cleanup(&dd->in_sgl[0], NULL, 0, alen,
+			    FLAGS_ASSOC_DATA_ST_SHIFT, dd->flags);
+
+	omap_crypto_cleanup(&dd->in_sgl[nsg], NULL, 0, clen,
+			    FLAGS_IN_DATA_ST_SHIFT, dd->flags);
+
+	if (!(dd->flags & FLAGS_ENCRYPT)) {
+		tag = (u8 *)rctx->auth_tag;
+		for (i = 0; i < dd->authsize; i++) {
+			if (tag[i]) {
+				dev_err(dd->dev, "GCM decryption: Tag Message is wrong\n");
+				ret = -EBADMSG;
+			}
+		}
+	}
+
+	omap_aes_gcm_finish_req(dd, ret);
+	omap_aes_gcm_handle_queue(dd, NULL);
+}
+
+static int omap_aes_gcm_copy_buffers(struct omap_aes_dev *dd,
+				     struct aead_request *req)
+{
+	int alen, clen, cryptlen, assoclen, ret;
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
+	unsigned int authlen = crypto_aead_authsize(aead);
+	struct scatterlist *tmp, sg_arr[2];
+	int nsg;
+	u16 flags;
+
+	assoclen = req->assoclen;
+	cryptlen = req->cryptlen;
+
+	if (dd->flags & FLAGS_RFC4106_GCM)
+		assoclen -= 8;
+
+	if (!(dd->flags & FLAGS_ENCRYPT))
+		cryptlen -= authlen;
+
+	alen = ALIGN(assoclen, AES_BLOCK_SIZE);
+	clen = ALIGN(cryptlen, AES_BLOCK_SIZE);
+
+	nsg = !!(assoclen && cryptlen);
+
+	omap_aes_clear_copy_flags(dd);
+
+	sg_init_table(dd->in_sgl, nsg + 1);
+	if (assoclen) {
+		tmp = req->src;
+		ret = omap_crypto_align_sg(&tmp, assoclen,
+					   AES_BLOCK_SIZE, dd->in_sgl,
+					   OMAP_CRYPTO_COPY_DATA |
+					   OMAP_CRYPTO_ZERO_BUF |
+					   OMAP_CRYPTO_FORCE_SINGLE_ENTRY,
+					   FLAGS_ASSOC_DATA_ST_SHIFT,
+					   &dd->flags);
+	}
+
+	if (cryptlen) {
+		tmp = scatterwalk_ffwd(sg_arr, req->src, req->assoclen);
+
+		ret = omap_crypto_align_sg(&tmp, cryptlen,
+					   AES_BLOCK_SIZE, &dd->in_sgl[nsg],
+					   OMAP_CRYPTO_COPY_DATA |
+					   OMAP_CRYPTO_ZERO_BUF |
+					   OMAP_CRYPTO_FORCE_SINGLE_ENTRY,
+					   FLAGS_IN_DATA_ST_SHIFT,
+					   &dd->flags);
+	}
+
+	dd->in_sg = dd->in_sgl;
+	dd->total = cryptlen;
+	dd->assoc_len = assoclen;
+	dd->authsize = authlen;
+
+	dd->out_sg = req->dst;
+	dd->orig_out = req->dst;
+
+	dd->out_sg = scatterwalk_ffwd(sg_arr, req->dst, assoclen);
+
+	flags = 0;
+	if (req->src == req->dst || dd->out_sg == sg_arr)
+		flags |= OMAP_CRYPTO_FORCE_COPY;
+
+	ret = omap_crypto_align_sg(&dd->out_sg, cryptlen,
+				   AES_BLOCK_SIZE, &dd->out_sgl,
+				   flags,
+				   FLAGS_OUT_DATA_ST_SHIFT, &dd->flags);
+	if (ret)
+		return ret;
+
+	dd->in_sg_len = sg_nents_for_len(dd->in_sg, alen + clen);
+	dd->out_sg_len = sg_nents_for_len(dd->out_sg, clen);
+
+	return 0;
+}
+
+static void omap_aes_gcm_complete(struct crypto_async_request *req, int err)
+{
+	struct omap_aes_gcm_result *res = req->data;
+
+	if (err == -EINPROGRESS)
+		return;
+
+	res->err = err;
+	complete(&res->completion);
+}
+
+static int do_encrypt_iv(struct aead_request *req, u32 *tag, u32 *iv)
+{
+	struct scatterlist iv_sg, tag_sg;
+	struct skcipher_request *sk_req;
+	struct omap_aes_gcm_result result;
+	struct omap_aes_ctx *ctx = crypto_aead_ctx(crypto_aead_reqtfm(req));
+	int ret = 0;
+
+	sk_req = skcipher_request_alloc(ctx->ctr, GFP_KERNEL);
+	if (!sk_req) {
+		pr_err("skcipher: Failed to allocate request\n");
+		return -1;
+	}
+
+	init_completion(&result.completion);
+
+	sg_init_one(&iv_sg, iv, AES_BLOCK_SIZE);
+	sg_init_one(&tag_sg, tag, AES_BLOCK_SIZE);
+	skcipher_request_set_callback(sk_req, CRYPTO_TFM_REQ_MAY_BACKLOG,
+				      omap_aes_gcm_complete, &result);
+	ret = crypto_skcipher_setkey(ctx->ctr, (u8 *)ctx->key, ctx->keylen);
+	skcipher_request_set_crypt(sk_req, &iv_sg, &tag_sg, AES_BLOCK_SIZE,
+				   NULL);
+	ret = crypto_skcipher_encrypt(sk_req);
+	switch (ret) {
+	case 0:
+		break;
+	case -EINPROGRESS:
+	case -EBUSY:
+		ret = wait_for_completion_interruptible(&result.completion);
+		if (!ret) {
+			ret = result.err;
+			if (!ret) {
+				reinit_completion(&result.completion);
+				break;
+			}
+		}
+		/* fall through */
+	default:
+		pr_err("Encryption of IV failed for GCM mode");
+		break;
+	}
+
+	skcipher_request_free(sk_req);
+	return ret;
+}
+
+void omap_aes_gcm_dma_out_callback(void *data)
+{
+	struct omap_aes_dev *dd = data;
+	struct omap_aes_reqctx *rctx;
+	int i, val;
+	u32 *auth_tag, tag[4];
+
+	if (!(dd->flags & FLAGS_ENCRYPT))
+		scatterwalk_map_and_copy(tag, dd->aead_req->src,
+					 dd->total + dd->aead_req->assoclen,
+					 dd->authsize, 0);
+
+	rctx = aead_request_ctx(dd->aead_req);
+	auth_tag = (u32 *)rctx->auth_tag;
+	for (i = 0; i < 4; i++) {
+		val = omap_aes_read(dd, AES_REG_TAG_N(dd, i));
+		auth_tag[i] = val ^ auth_tag[i];
+		if (!(dd->flags & FLAGS_ENCRYPT))
+			auth_tag[i] = auth_tag[i] ^ tag[i];
+	}
+
+	omap_aes_gcm_done_task(dd);
+}
+
+static int omap_aes_gcm_handle_queue(struct omap_aes_dev *dd,
+				     struct aead_request *req)
+{
+	struct omap_aes_ctx *ctx;
+	struct aead_request *backlog;
+	struct omap_aes_reqctx *rctx;
+	unsigned long flags;
+	int err, ret = 0;
+
+	spin_lock_irqsave(&dd->lock, flags);
+	if (req)
+		ret = aead_enqueue_request(&dd->aead_queue, req);
+	if (dd->flags & FLAGS_BUSY) {
+		spin_unlock_irqrestore(&dd->lock, flags);
+		return ret;
+	}
+
+	backlog = aead_get_backlog(&dd->aead_queue);
+	req = aead_dequeue_request(&dd->aead_queue);
+	if (req)
+		dd->flags |= FLAGS_BUSY;
+	spin_unlock_irqrestore(&dd->lock, flags);
+
+	if (!req)
+		return ret;
+
+	if (backlog)
+		backlog->base.complete(&backlog->base, -EINPROGRESS);
+
+	ctx = crypto_aead_ctx(crypto_aead_reqtfm(req));
+	rctx = aead_request_ctx(req);
+
+	dd->ctx = ctx;
+	rctx->dd = dd;
+	dd->aead_req = req;
+
+	rctx->mode &= FLAGS_MODE_MASK;
+	dd->flags = (dd->flags & ~FLAGS_MODE_MASK) | rctx->mode;
+
+	err = omap_aes_gcm_copy_buffers(dd, req);
+	if (err)
+		return err;
+
+	err = omap_aes_write_ctrl(dd);
+	if (!err)
+		err = omap_aes_crypt_dma_start(dd);
+
+	if (err) {
+		omap_aes_gcm_finish_req(dd, err);
+		omap_aes_gcm_handle_queue(dd, NULL);
+	}
+
+	return ret;
+}
+
+static int omap_aes_gcm_crypt(struct aead_request *req, unsigned long mode)
+{
+	struct omap_aes_reqctx *rctx = aead_request_ctx(req);
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
+	unsigned int authlen = crypto_aead_authsize(aead);
+	struct omap_aes_dev *dd;
+	__be32 counter = cpu_to_be32(1);
+	int err, assoclen;
+
+	memset(rctx->auth_tag, 0, sizeof(rctx->auth_tag));
+	memcpy(rctx->iv + 12, &counter, 4);
+
+	err = do_encrypt_iv(req, (u32 *)rctx->auth_tag, (u32 *)rctx->iv);
+	if (err)
+		return err;
+
+	if (mode & FLAGS_RFC4106_GCM)
+		assoclen = req->assoclen - 8;
+	else
+		assoclen = req->assoclen;
+	if (assoclen + req->cryptlen == 0) {
+		scatterwalk_map_and_copy(rctx->auth_tag, req->dst, 0, authlen,
+					 1);
+		return 0;
+	}
+
+	dd = omap_aes_find_dev(rctx);
+	if (!dd)
+		return -ENODEV;
+	rctx->mode = mode;
+
+	return omap_aes_gcm_handle_queue(dd, req);
+}
+
+int omap_aes_gcm_encrypt(struct aead_request *req)
+{
+	struct omap_aes_reqctx *rctx = aead_request_ctx(req);
+
+	memcpy(rctx->iv, req->iv, 12);
+	return omap_aes_gcm_crypt(req, FLAGS_ENCRYPT | FLAGS_GCM);
+}
+
+int omap_aes_gcm_decrypt(struct aead_request *req)
+{
+	struct omap_aes_reqctx *rctx = aead_request_ctx(req);
+
+	memcpy(rctx->iv, req->iv, 12);
+	return omap_aes_gcm_crypt(req, FLAGS_GCM);
+}
+
+int omap_aes_4106gcm_encrypt(struct aead_request *req)
+{
+	struct omap_aes_ctx *ctx = crypto_aead_ctx(crypto_aead_reqtfm(req));
+	struct omap_aes_reqctx *rctx = aead_request_ctx(req);
+
+	memcpy(rctx->iv, ctx->nonce, 4);
+	memcpy(rctx->iv + 4, req->iv, 8);
+	return omap_aes_gcm_crypt(req, FLAGS_ENCRYPT | FLAGS_GCM |
+				  FLAGS_RFC4106_GCM);
+}
+
+int omap_aes_4106gcm_decrypt(struct aead_request *req)
+{
+	struct omap_aes_ctx *ctx = crypto_aead_ctx(crypto_aead_reqtfm(req));
+	struct omap_aes_reqctx *rctx = aead_request_ctx(req);
+
+	memcpy(rctx->iv, ctx->nonce, 4);
+	memcpy(rctx->iv + 4, req->iv, 8);
+	return omap_aes_gcm_crypt(req, FLAGS_GCM | FLAGS_RFC4106_GCM);
+}
+
+int omap_aes_gcm_setkey(struct crypto_aead *tfm, const u8 *key,
+			unsigned int keylen)
+{
+	struct omap_aes_ctx *ctx = crypto_aead_ctx(tfm);
+
+	if (keylen != AES_KEYSIZE_128 && keylen != AES_KEYSIZE_192 &&
+	    keylen != AES_KEYSIZE_256)
+		return -EINVAL;
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	return 0;
+}
+
+int omap_aes_4106gcm_setkey(struct crypto_aead *tfm, const u8 *key,
+			    unsigned int keylen)
+{
+	struct omap_aes_ctx *ctx = crypto_aead_ctx(tfm);
+
+	if (keylen < 4)
+		return -EINVAL;
+
+	keylen -= 4;
+	if (keylen != AES_KEYSIZE_128 && keylen != AES_KEYSIZE_192 &&
+	    keylen != AES_KEYSIZE_256)
+		return -EINVAL;
+
+	memcpy(ctx->key, key, keylen);
+	memcpy(ctx->nonce, key + keylen, 4);
+	ctx->keylen = keylen;
+
+	return 0;
+}
diff --git a/drivers/crypto/omap-aes.c b/drivers/crypto/omap-aes.c
index fe32dd95ae4f..5120a17731d0 100644
--- a/drivers/crypto/omap-aes.c
+++ b/drivers/crypto/omap-aes.c
@@ -37,155 +37,10 @@
 #include <crypto/aes.h>
 #include <crypto/engine.h>
 #include <crypto/internal/skcipher.h>
+#include <crypto/internal/aead.h>
 
-#define DST_MAXBURST			4
-#define DMA_MIN				(DST_MAXBURST * sizeof(u32))
-
-#define _calc_walked(inout) (dd->inout##_walk.offset - dd->inout##_sg->offset)
-
-/* OMAP TRM gives bitfields as start:end, where start is the higher bit
-   number. For example 7:0 */
-#define FLD_MASK(start, end)	(((1 << ((start) - (end) + 1)) - 1) << (end))
-#define FLD_VAL(val, start, end) (((val) << (end)) & FLD_MASK(start, end))
-
-#define AES_REG_KEY(dd, x)		((dd)->pdata->key_ofs - \
-						((x ^ 0x01) * 0x04))
-#define AES_REG_IV(dd, x)		((dd)->pdata->iv_ofs + ((x) * 0x04))
-
-#define AES_REG_CTRL(dd)		((dd)->pdata->ctrl_ofs)
-#define AES_REG_CTRL_CTR_WIDTH_MASK	GENMASK(8, 7)
-#define AES_REG_CTRL_CTR_WIDTH_32	0
-#define AES_REG_CTRL_CTR_WIDTH_64	BIT(7)
-#define AES_REG_CTRL_CTR_WIDTH_96	BIT(8)
-#define AES_REG_CTRL_CTR_WIDTH_128	GENMASK(8, 7)
-#define AES_REG_CTRL_CTR		BIT(6)
-#define AES_REG_CTRL_CBC		BIT(5)
-#define AES_REG_CTRL_KEY_SIZE		GENMASK(4, 3)
-#define AES_REG_CTRL_DIRECTION		BIT(2)
-#define AES_REG_CTRL_INPUT_READY	BIT(1)
-#define AES_REG_CTRL_OUTPUT_READY	BIT(0)
-#define AES_REG_CTRL_MASK		GENMASK(24, 2)
-
-#define AES_REG_DATA_N(dd, x)		((dd)->pdata->data_ofs + ((x) * 0x04))
-
-#define AES_REG_REV(dd)			((dd)->pdata->rev_ofs)
-
-#define AES_REG_MASK(dd)		((dd)->pdata->mask_ofs)
-#define AES_REG_MASK_SIDLE		BIT(6)
-#define AES_REG_MASK_START		BIT(5)
-#define AES_REG_MASK_DMA_OUT_EN		BIT(3)
-#define AES_REG_MASK_DMA_IN_EN		BIT(2)
-#define AES_REG_MASK_SOFTRESET		BIT(1)
-#define AES_REG_AUTOIDLE		BIT(0)
-
-#define AES_REG_LENGTH_N(x)		(0x54 + ((x) * 0x04))
-
-#define AES_REG_IRQ_STATUS(dd)         ((dd)->pdata->irq_status_ofs)
-#define AES_REG_IRQ_ENABLE(dd)         ((dd)->pdata->irq_enable_ofs)
-#define AES_REG_IRQ_DATA_IN            BIT(1)
-#define AES_REG_IRQ_DATA_OUT           BIT(2)
-#define DEFAULT_TIMEOUT		(5*HZ)
-
-#define DEFAULT_AUTOSUSPEND_DELAY	1000
-
-#define FLAGS_MODE_MASK		0x000f
-#define FLAGS_ENCRYPT		BIT(0)
-#define FLAGS_CBC		BIT(1)
-#define FLAGS_GIV		BIT(2)
-#define FLAGS_CTR		BIT(3)
-
-#define FLAGS_INIT		BIT(4)
-#define FLAGS_FAST		BIT(5)
-#define FLAGS_BUSY		BIT(6)
-
-#define AES_BLOCK_WORDS		(AES_BLOCK_SIZE >> 2)
-
-struct omap_aes_ctx {
-	struct omap_aes_dev *dd;
-
-	int		keylen;
-	u32		key[AES_KEYSIZE_256 / sizeof(u32)];
-	unsigned long	flags;
-	struct crypto_skcipher	*fallback;
-};
-
-struct omap_aes_reqctx {
-	unsigned long mode;
-};
-
-#define OMAP_AES_QUEUE_LENGTH	1
-#define OMAP_AES_CACHE_SIZE	0
-
-struct omap_aes_algs_info {
-	struct crypto_alg	*algs_list;
-	unsigned int		size;
-	unsigned int		registered;
-};
-
-struct omap_aes_pdata {
-	struct omap_aes_algs_info	*algs_info;
-	unsigned int	algs_info_size;
-
-	void		(*trigger)(struct omap_aes_dev *dd, int length);
-
-	u32		key_ofs;
-	u32		iv_ofs;
-	u32		ctrl_ofs;
-	u32		data_ofs;
-	u32		rev_ofs;
-	u32		mask_ofs;
-	u32             irq_enable_ofs;
-	u32             irq_status_ofs;
-
-	u32		dma_enable_in;
-	u32		dma_enable_out;
-	u32		dma_start;
-
-	u32		major_mask;
-	u32		major_shift;
-	u32		minor_mask;
-	u32		minor_shift;
-};
-
-struct omap_aes_dev {
-	struct list_head	list;
-	unsigned long		phys_base;
-	void __iomem		*io_base;
-	struct omap_aes_ctx	*ctx;
-	struct device		*dev;
-	unsigned long		flags;
-	int			err;
-
-	struct tasklet_struct	done_task;
-
-	struct ablkcipher_request	*req;
-	struct crypto_engine		*engine;
-
-	/*
-	 * total is used by PIO mode for book keeping so introduce
-	 * variable total_save as need it to calc page_order
-	 */
-	size_t				total;
-	size_t				total_save;
-
-	struct scatterlist		*in_sg;
-	struct scatterlist		*out_sg;
-
-	/* Buffers for copying for unaligned cases */
-	struct scatterlist		in_sgl;
-	struct scatterlist		out_sgl;
-	struct scatterlist		*orig_out;
-	int				sgs_copied;
-
-	struct scatter_walk		in_walk;
-	struct scatter_walk		out_walk;
-	struct dma_chan		*dma_lch_in;
-	struct dma_chan		*dma_lch_out;
-	int			in_sg_len;
-	int			out_sg_len;
-	int			pio_only;
-	const struct omap_aes_pdata	*pdata;
-};
+#include "omap-crypto.h"
+#include "omap-aes.h"
 
 /* keep registered devices data here */
 static LIST_HEAD(dev_list);
@@ -201,7 +56,7 @@ static DEFINE_SPINLOCK(list_lock);
 	_read_ret;						\
 })
 #else
-static inline u32 omap_aes_read(struct omap_aes_dev *dd, u32 offset)
+inline u32 omap_aes_read(struct omap_aes_dev *dd, u32 offset)
 {
 	return __raw_readl(dd->io_base + offset);
 }
@@ -215,7 +70,7 @@ static inline u32 omap_aes_read(struct omap_aes_dev *dd, u32 offset)
 		__raw_writel(value, dd->io_base + offset);		\
 	} while (0)
 #else
-static inline void omap_aes_write(struct omap_aes_dev *dd, u32 offset,
+inline void omap_aes_write(struct omap_aes_dev *dd, u32 offset,
 				  u32 value)
 {
 	__raw_writel(value, dd->io_base + offset);
@@ -258,8 +113,16 @@ static int omap_aes_hw_init(struct omap_aes_dev *dd)
 	return 0;
 }
 
-static int omap_aes_write_ctrl(struct omap_aes_dev *dd)
+void omap_aes_clear_copy_flags(struct omap_aes_dev *dd)
+{
+	dd->flags &= ~(OMAP_CRYPTO_COPY_MASK << FLAGS_IN_DATA_ST_SHIFT);
+	dd->flags &= ~(OMAP_CRYPTO_COPY_MASK << FLAGS_OUT_DATA_ST_SHIFT);
+	dd->flags &= ~(OMAP_CRYPTO_COPY_MASK << FLAGS_ASSOC_DATA_ST_SHIFT);
+}
+
+int omap_aes_write_ctrl(struct omap_aes_dev *dd)
 {
+	struct omap_aes_reqctx *rctx;
 	unsigned int key32;
 	int i, err;
 	u32 val;
@@ -270,7 +133,11 @@ static int omap_aes_write_ctrl(struct omap_aes_dev *dd)
 
 	key32 = dd->ctx->keylen / sizeof(u32);
 
-	/* it seems a key should always be set even if it has not changed */
+	/* RESET the key as previous HASH keys should not get affected*/
+	if (dd->flags & FLAGS_GCM)
+		for (i = 0; i < 0x40; i = i + 4)
+			omap_aes_write(dd, i, 0x0);
+
 	for (i = 0; i < key32; i++) {
 		omap_aes_write(dd, AES_REG_KEY(dd, i),
 			__le32_to_cpu(dd->ctx->key[i]));
@@ -279,12 +146,21 @@ static int omap_aes_write_ctrl(struct omap_aes_dev *dd)
 	if ((dd->flags & (FLAGS_CBC | FLAGS_CTR)) && dd->req->info)
 		omap_aes_write_n(dd, AES_REG_IV(dd, 0), dd->req->info, 4);
 
+	if ((dd->flags & (FLAGS_GCM)) && dd->aead_req->iv) {
+		rctx = aead_request_ctx(dd->aead_req);
+		omap_aes_write_n(dd, AES_REG_IV(dd, 0), (u32 *)rctx->iv, 4);
+	}
+
 	val = FLD_VAL(((dd->ctx->keylen >> 3) - 1), 4, 3);
 	if (dd->flags & FLAGS_CBC)
 		val |= AES_REG_CTRL_CBC;
-	if (dd->flags & FLAGS_CTR)
+
+	if (dd->flags & (FLAGS_CTR | FLAGS_GCM))
 		val |= AES_REG_CTRL_CTR | AES_REG_CTRL_CTR_WIDTH_128;
 
+	if (dd->flags & FLAGS_GCM)
+		val |= AES_REG_CTRL_GCM;
+
 	if (dd->flags & FLAGS_ENCRYPT)
 		val |= AES_REG_CTRL_DIRECTION;
 
@@ -315,6 +191,8 @@ static void omap_aes_dma_trigger_omap4(struct omap_aes_dev *dd, int length)
 {
 	omap_aes_write(dd, AES_REG_LENGTH_N(0), length);
 	omap_aes_write(dd, AES_REG_LENGTH_N(1), 0);
+	if (dd->flags & FLAGS_GCM)
+		omap_aes_write(dd, AES_REG_A_LEN, dd->assoc_len);
 
 	omap_aes_dma_trigger_omap2(dd, length);
 }
@@ -329,14 +207,14 @@ static void omap_aes_dma_stop(struct omap_aes_dev *dd)
 	omap_aes_write_mask(dd, AES_REG_MASK(dd), 0, mask);
 }
 
-static struct omap_aes_dev *omap_aes_find_dev(struct omap_aes_ctx *ctx)
+struct omap_aes_dev *omap_aes_find_dev(struct omap_aes_reqctx *rctx)
 {
 	struct omap_aes_dev *dd;
 
 	spin_lock_bh(&list_lock);
 	dd = list_first_entry(&dev_list, struct omap_aes_dev, list);
 	list_move_tail(&dd->list, &dev_list);
-	ctx->dd = dd;
+	rctx->dd = dd;
 	spin_unlock_bh(&list_lock);
 
 	return dd;
@@ -387,26 +265,11 @@ static void omap_aes_dma_cleanup(struct omap_aes_dev *dd)
 	dma_release_channel(dd->dma_lch_in);
 }
 
-static void sg_copy_buf(void *buf, struct scatterlist *sg,
-			      unsigned int start, unsigned int nbytes, int out)
+static int omap_aes_crypt_dma(struct omap_aes_dev *dd,
+			      struct scatterlist *in_sg,
+			      struct scatterlist *out_sg,
+			      int in_sg_len, int out_sg_len)
 {
-	struct scatter_walk walk;
-
-	if (!nbytes)
-		return;
-
-	scatterwalk_start(&walk, sg);
-	scatterwalk_advance(&walk, start);
-	scatterwalk_copychunks(buf, &walk, nbytes, out);
-	scatterwalk_done(&walk, out, 0);
-}
-
-static int omap_aes_crypt_dma(struct crypto_tfm *tfm,
-		struct scatterlist *in_sg, struct scatterlist *out_sg,
-		int in_sg_len, int out_sg_len)
-{
-	struct omap_aes_ctx *ctx = crypto_tfm_ctx(tfm);
-	struct omap_aes_dev *dd = ctx->dd;
 	struct dma_async_tx_descriptor *tx_in, *tx_out;
 	struct dma_slave_config cfg;
 	int ret;
@@ -467,7 +330,10 @@ static int omap_aes_crypt_dma(struct crypto_tfm *tfm,
 		return -EINVAL;
 	}
 
-	tx_out->callback = omap_aes_dma_out_callback;
+	if (dd->flags & FLAGS_GCM)
+		tx_out->callback = omap_aes_gcm_dma_out_callback;
+	else
+		tx_out->callback = omap_aes_dma_out_callback;
 	tx_out->callback_param = dd;
 
 	dmaengine_submit(tx_in);
@@ -482,10 +348,8 @@ static int omap_aes_crypt_dma(struct crypto_tfm *tfm,
 	return 0;
 }
 
-static int omap_aes_crypt_dma_start(struct omap_aes_dev *dd)
+int omap_aes_crypt_dma_start(struct omap_aes_dev *dd)
 {
-	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(
-					crypto_ablkcipher_reqtfm(dd->req));
 	int err;
 
 	pr_debug("total: %d\n", dd->total);
@@ -506,7 +370,7 @@ static int omap_aes_crypt_dma_start(struct omap_aes_dev *dd)
 		}
 	}
 
-	err = omap_aes_crypt_dma(tfm, dd->in_sg, dd->out_sg, dd->in_sg_len,
+	err = omap_aes_crypt_dma(dd, dd->in_sg, dd->out_sg, dd->in_sg_len,
 				 dd->out_sg_len);
 	if (err && !dd->pio_only) {
 		dma_unmap_sg(dd->dev, dd->in_sg, dd->in_sg_len, DMA_TO_DEVICE);
@@ -529,7 +393,7 @@ static void omap_aes_finish_req(struct omap_aes_dev *dd, int err)
 	pm_runtime_put_autosuspend(dd->dev);
 }
 
-static int omap_aes_crypt_dma_stop(struct omap_aes_dev *dd)
+int omap_aes_crypt_dma_stop(struct omap_aes_dev *dd)
 {
 	pr_debug("total: %d\n", dd->total);
 
@@ -539,62 +403,6 @@ static int omap_aes_crypt_dma_stop(struct omap_aes_dev *dd)
 	return 0;
 }
 
-static int omap_aes_check_aligned(struct scatterlist *sg, int total)
-{
-	int len = 0;
-
-	if (!IS_ALIGNED(total, AES_BLOCK_SIZE))
-		return -EINVAL;
-
-	while (sg) {
-		if (!IS_ALIGNED(sg->offset, 4))
-			return -1;
-		if (!IS_ALIGNED(sg->length, AES_BLOCK_SIZE))
-			return -1;
-
-		len += sg->length;
-		sg = sg_next(sg);
-	}
-
-	if (len != total)
-		return -1;
-
-	return 0;
-}
-
-static int omap_aes_copy_sgs(struct omap_aes_dev *dd)
-{
-	void *buf_in, *buf_out;
-	int pages, total;
-
-	total = ALIGN(dd->total, AES_BLOCK_SIZE);
-	pages = get_order(total);
-
-	buf_in = (void *)__get_free_pages(GFP_ATOMIC, pages);
-	buf_out = (void *)__get_free_pages(GFP_ATOMIC, pages);
-
-	if (!buf_in || !buf_out) {
-		pr_err("Couldn't allocated pages for unaligned cases.\n");
-		return -1;
-	}
-
-	dd->orig_out = dd->out_sg;
-
-	sg_copy_buf(buf_in, dd->in_sg, 0, dd->total, 0);
-
-	sg_init_table(&dd->in_sgl, 1);
-	sg_set_buf(&dd->in_sgl, buf_in, total);
-	dd->in_sg = &dd->in_sgl;
-	dd->in_sg_len = 1;
-
-	sg_init_table(&dd->out_sgl, 1);
-	sg_set_buf(&dd->out_sgl, buf_out, total);
-	dd->out_sg = &dd->out_sgl;
-	dd->out_sg_len = 1;
-
-	return 0;
-}
-
 static int omap_aes_handle_queue(struct omap_aes_dev *dd,
 				 struct ablkcipher_request *req)
 {
@@ -609,8 +417,10 @@ static int omap_aes_prepare_req(struct crypto_engine *engine,
 {
 	struct omap_aes_ctx *ctx = crypto_ablkcipher_ctx(
 			crypto_ablkcipher_reqtfm(req));
-	struct omap_aes_dev *dd = ctx->dd;
-	struct omap_aes_reqctx *rctx;
+	struct omap_aes_reqctx *rctx = ablkcipher_request_ctx(req);
+	struct omap_aes_dev *dd = rctx->dd;
+	int ret;
+	u16 flags;
 
 	if (!dd)
 		return -ENODEV;
@@ -621,6 +431,23 @@ static int omap_aes_prepare_req(struct crypto_engine *engine,
 	dd->total_save = req->nbytes;
 	dd->in_sg = req->src;
 	dd->out_sg = req->dst;
+	dd->orig_out = req->dst;
+
+	flags = OMAP_CRYPTO_COPY_DATA;
+	if (req->src == req->dst)
+		flags |= OMAP_CRYPTO_FORCE_COPY;
+
+	ret = omap_crypto_align_sg(&dd->in_sg, dd->total, AES_BLOCK_SIZE,
+				   dd->in_sgl, flags,
+				   FLAGS_IN_DATA_ST_SHIFT, &dd->flags);
+	if (ret)
+		return ret;
+
+	ret = omap_crypto_align_sg(&dd->out_sg, dd->total, AES_BLOCK_SIZE,
+				   &dd->out_sgl, 0,
+				   FLAGS_OUT_DATA_ST_SHIFT, &dd->flags);
+	if (ret)
+		return ret;
 
 	dd->in_sg_len = sg_nents_for_len(dd->in_sg, dd->total);
 	if (dd->in_sg_len < 0)
@@ -630,22 +457,11 @@ static int omap_aes_prepare_req(struct crypto_engine *engine,
 	if (dd->out_sg_len < 0)
 		return dd->out_sg_len;
 
-	if (omap_aes_check_aligned(dd->in_sg, dd->total) ||
-	    omap_aes_check_aligned(dd->out_sg, dd->total)) {
-		if (omap_aes_copy_sgs(dd))
-			pr_err("Failed to copy SGs for unaligned cases\n");
-		dd->sgs_copied = 1;
-	} else {
-		dd->sgs_copied = 0;
-	}
-
-	rctx = ablkcipher_request_ctx(req);
-	ctx = crypto_ablkcipher_ctx(crypto_ablkcipher_reqtfm(req));
 	rctx->mode &= FLAGS_MODE_MASK;
 	dd->flags = (dd->flags & ~FLAGS_MODE_MASK) | rctx->mode;
 
 	dd->ctx = ctx;
-	ctx->dd = dd;
+	rctx->dd = dd;
 
 	return omap_aes_write_ctrl(dd);
 }
@@ -653,9 +469,8 @@ static int omap_aes_prepare_req(struct crypto_engine *engine,
 static int omap_aes_crypt_req(struct crypto_engine *engine,
 			      struct ablkcipher_request *req)
 {
-	struct omap_aes_ctx *ctx = crypto_ablkcipher_ctx(
-			crypto_ablkcipher_reqtfm(req));
-	struct omap_aes_dev *dd = ctx->dd;
+	struct omap_aes_reqctx *rctx = ablkcipher_request_ctx(req);
+	struct omap_aes_dev *dd = rctx->dd;
 
 	if (!dd)
 		return -ENODEV;
@@ -666,8 +481,6 @@ static int omap_aes_crypt_req(struct crypto_engine *engine,
 static void omap_aes_done_task(unsigned long data)
 {
 	struct omap_aes_dev *dd = (struct omap_aes_dev *)data;
-	void *buf_in, *buf_out;
-	int pages, len;
 
 	pr_debug("enter done_task\n");
 
@@ -680,17 +493,11 @@ static void omap_aes_done_task(unsigned long data)
 		omap_aes_crypt_dma_stop(dd);
 	}
 
-	if (dd->sgs_copied) {
-		buf_in = sg_virt(&dd->in_sgl);
-		buf_out = sg_virt(&dd->out_sgl);
+	omap_crypto_cleanup(dd->in_sgl, NULL, 0, dd->total_save,
+			    FLAGS_IN_DATA_ST_SHIFT, dd->flags);
 
-		sg_copy_buf(buf_out, dd->orig_out, 0, dd->total_save, 1);
-
-		len = ALIGN(dd->total_save, AES_BLOCK_SIZE);
-		pages = get_order(len);
-		free_pages((unsigned long)buf_in, pages);
-		free_pages((unsigned long)buf_out, pages);
-	}
+	omap_crypto_cleanup(&dd->out_sgl, dd->orig_out, 0, dd->total_save,
+			    FLAGS_OUT_DATA_ST_SHIFT, dd->flags);
 
 	omap_aes_finish_req(dd, 0);
 
@@ -726,7 +533,7 @@ static int omap_aes_crypt(struct ablkcipher_request *req, unsigned long mode)
 		skcipher_request_zero(subreq);
 		return ret;
 	}
-	dd = omap_aes_find_dev(ctx);
+	dd = omap_aes_find_dev(rctx);
 	if (!dd)
 		return -ENODEV;
 
@@ -811,6 +618,36 @@ static int omap_aes_cra_init(struct crypto_tfm *tfm)
 	return 0;
 }
 
+static int omap_aes_gcm_cra_init(struct crypto_aead *tfm)
+{
+	struct omap_aes_dev *dd = NULL;
+	struct omap_aes_ctx *ctx = crypto_aead_ctx(tfm);
+	int err;
+
+	/* Find AES device, currently picks the first device */
+	spin_lock_bh(&list_lock);
+	list_for_each_entry(dd, &dev_list, list) {
+		break;
+	}
+	spin_unlock_bh(&list_lock);
+
+	err = pm_runtime_get_sync(dd->dev);
+	if (err < 0) {
+		dev_err(dd->dev, "%s: failed to get_sync(%d)\n",
+			__func__, err);
+		return err;
+	}
+
+	tfm->reqsize = sizeof(struct omap_aes_reqctx);
+	ctx->ctr = crypto_alloc_skcipher("ecb(aes)", 0, 0);
+	if (IS_ERR(ctx->ctr)) {
+		pr_warn("could not load aes driver for encrypting IV\n");
+		return PTR_ERR(ctx->ctr);
+	}
+
+	return 0;
+}
+
 static void omap_aes_cra_exit(struct crypto_tfm *tfm)
 {
 	struct omap_aes_ctx *ctx = crypto_tfm_ctx(tfm);
@@ -821,6 +658,16 @@ static void omap_aes_cra_exit(struct crypto_tfm *tfm)
 	ctx->fallback = NULL;
 }
 
+static void omap_aes_gcm_cra_exit(struct crypto_aead *tfm)
+{
+	struct omap_aes_ctx *ctx = crypto_aead_ctx(tfm);
+
+	omap_aes_cra_exit(crypto_aead_tfm(tfm));
+
+	if (ctx->ctr)
+		crypto_free_skcipher(ctx->ctr);
+}
+
 /* ********************** ALGS ************************************ */
 
 static struct crypto_alg algs_ecb_cbc[] = {
@@ -905,6 +752,54 @@ static struct omap_aes_algs_info omap_aes_algs_info_ecb_cbc[] = {
 	},
 };
 
+static struct aead_alg algs_aead_gcm[] = {
+{
+	.base = {
+		.cra_name		= "gcm(aes)",
+		.cra_driver_name	= "gcm-aes-omap",
+		.cra_priority		= 300,
+		.cra_flags		= CRYPTO_ALG_ASYNC |
+					  CRYPTO_ALG_KERN_DRIVER_ONLY,
+		.cra_blocksize		= 1,
+		.cra_ctxsize		= sizeof(struct omap_aes_ctx),
+		.cra_alignmask		= 0xf,
+		.cra_module		= THIS_MODULE,
+	},
+	.init		= omap_aes_gcm_cra_init,
+	.exit		= omap_aes_gcm_cra_exit,
+	.ivsize		= 12,
+	.maxauthsize	= AES_BLOCK_SIZE,
+	.setkey		= omap_aes_gcm_setkey,
+	.encrypt	= omap_aes_gcm_encrypt,
+	.decrypt	= omap_aes_gcm_decrypt,
+},
+{
+	.base = {
+		.cra_name		= "rfc4106(gcm(aes))",
+		.cra_driver_name	= "rfc4106-gcm-aes-omap",
+		.cra_priority		= 300,
+		.cra_flags		= CRYPTO_ALG_ASYNC |
+					  CRYPTO_ALG_KERN_DRIVER_ONLY,
+		.cra_blocksize		= 1,
+		.cra_ctxsize		= sizeof(struct omap_aes_ctx),
+		.cra_alignmask		= 0xf,
+		.cra_module		= THIS_MODULE,
+	},
+	.init		= omap_aes_gcm_cra_init,
+	.exit		= omap_aes_gcm_cra_exit,
+	.maxauthsize	= AES_BLOCK_SIZE,
+	.ivsize		= 8,
+	.setkey		= omap_aes_4106gcm_setkey,
+	.encrypt	= omap_aes_4106gcm_encrypt,
+	.decrypt	= omap_aes_4106gcm_decrypt,
+},
+};
+
+static struct omap_aes_aead_algs omap_aes_aead_info = {
+	.algs_list	=	algs_aead_gcm,
+	.size		=	ARRAY_SIZE(algs_aead_gcm),
+};
+
 static const struct omap_aes_pdata omap_aes_pdata_omap2 = {
 	.algs_info	= omap_aes_algs_info_ecb_cbc,
 	.algs_info_size	= ARRAY_SIZE(omap_aes_algs_info_ecb_cbc),
@@ -958,6 +853,7 @@ static const struct omap_aes_pdata omap_aes_pdata_omap3 = {
 static const struct omap_aes_pdata omap_aes_pdata_omap4 = {
 	.algs_info	= omap_aes_algs_info_ecb_cbc_ctr,
 	.algs_info_size	= ARRAY_SIZE(omap_aes_algs_info_ecb_cbc_ctr),
+	.aead_algs_info	= &omap_aes_aead_info,
 	.trigger	= omap_aes_dma_trigger_omap4,
 	.key_ofs	= 0x3c,
 	.iv_ofs		= 0x40,
@@ -1140,6 +1036,7 @@ static int omap_aes_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	struct omap_aes_dev *dd;
 	struct crypto_alg *algp;
+	struct aead_alg *aalg;
 	struct resource res;
 	int err = -ENOMEM, i, j, irq = -1;
 	u32 reg;
@@ -1152,6 +1049,8 @@ static int omap_aes_probe(struct platform_device *pdev)
 	dd->dev = dev;
 	platform_set_drvdata(pdev, dd);
 
+	aead_init_queue(&dd->aead_queue, OMAP_AES_QUEUE_LENGTH);
+
 	err = (dev->of_node) ? omap_aes_get_res_of(dd, dev, &res) :
 			       omap_aes_get_res_pdev(dd, pdev, &res);
 	if (err)
@@ -1207,6 +1106,7 @@ static int omap_aes_probe(struct platform_device *pdev)
 		}
 	}
 
+	spin_lock_init(&dd->lock);
 
 	INIT_LIST_HEAD(&dd->list);
 	spin_lock(&list_lock);
@@ -1243,7 +1143,29 @@ static int omap_aes_probe(struct platform_device *pdev)
 		}
 	}
 
+	if (dd->pdata->aead_algs_info &&
+	    !dd->pdata->aead_algs_info->registered) {
+		for (i = 0; i < dd->pdata->aead_algs_info->size; i++) {
+			aalg = &dd->pdata->aead_algs_info->algs_list[i];
+			algp = &aalg->base;
+
+			pr_debug("reg alg: %s\n", algp->cra_name);
+			INIT_LIST_HEAD(&algp->cra_list);
+
+			err = crypto_register_aead(aalg);
+			if (err)
+				goto err_aead_algs;
+
+			dd->pdata->aead_algs_info->registered++;
+		}
+	}
+
 	return 0;
+err_aead_algs:
+	for (i = dd->pdata->aead_algs_info->registered - 1; i >= 0; i--) {
+		aalg = &dd->pdata->aead_algs_info->algs_list[i];
+		crypto_unregister_aead(aalg);
+	}
 err_algs:
 	for (i = dd->pdata->algs_info_size - 1; i >= 0; i--)
 		for (j = dd->pdata->algs_info[i].registered - 1; j >= 0; j--)
@@ -1268,6 +1190,7 @@ err_data:
 static int omap_aes_remove(struct platform_device *pdev)
 {
 	struct omap_aes_dev *dd = platform_get_drvdata(pdev);
+	struct aead_alg *aalg;
 	int i, j;
 
 	if (!dd)
@@ -1282,7 +1205,13 @@ static int omap_aes_remove(struct platform_device *pdev)
 			crypto_unregister_alg(
 					&dd->pdata->algs_info[i].algs_list[j]);
 
+	for (i = dd->pdata->aead_algs_info->size - 1; i >= 0; i--) {
+		aalg = &dd->pdata->aead_algs_info->algs_list[i];
+		crypto_unregister_aead(aalg);
+	}
+
 	crypto_engine_exit(dd->engine);
+
 	tasklet_kill(&dd->done_task);
 	omap_aes_dma_cleanup(dd);
 	pm_runtime_disable(dd->dev);
diff --git a/drivers/crypto/omap-aes.h b/drivers/crypto/omap-aes.h
new file mode 100644
index 000000000000..8906342e2b9a
--- /dev/null
+++ b/drivers/crypto/omap-aes.h
@@ -0,0 +1,214 @@
+/*
+ * Cryptographic API.
+ *
+ * Support for OMAP AES HW ACCELERATOR defines
+ *
+ * Copyright (c) 2015 Texas Instruments Incorporated
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ *
+ */
+#ifndef __OMAP_AES_H__
+#define __OMAP_AES_H__
+
+#define DST_MAXBURST			4
+#define DMA_MIN				(DST_MAXBURST * sizeof(u32))
+
+#define _calc_walked(inout) (dd->inout##_walk.offset - dd->inout##_sg->offset)
+
+/*
+ * OMAP TRM gives bitfields as start:end, where start is the higher bit
+ * number. For example 7:0
+ */
+#define FLD_MASK(start, end)	(((1 << ((start) - (end) + 1)) - 1) << (end))
+#define FLD_VAL(val, start, end) (((val) << (end)) & FLD_MASK(start, end))
+
+#define AES_REG_KEY(dd, x)		((dd)->pdata->key_ofs - \
+						(((x) ^ 0x01) * 0x04))
+#define AES_REG_IV(dd, x)		((dd)->pdata->iv_ofs + ((x) * 0x04))
+
+#define AES_REG_CTRL(dd)		((dd)->pdata->ctrl_ofs)
+#define AES_REG_CTRL_CONTEXT_READY	BIT(31)
+#define AES_REG_CTRL_CTR_WIDTH_MASK	GENMASK(8, 7)
+#define AES_REG_CTRL_CTR_WIDTH_32	0
+#define AES_REG_CTRL_CTR_WIDTH_64	BIT(7)
+#define AES_REG_CTRL_CTR_WIDTH_96	BIT(8)
+#define AES_REG_CTRL_CTR_WIDTH_128	GENMASK(8, 7)
+#define AES_REG_CTRL_GCM		GENMASK(17, 16)
+#define AES_REG_CTRL_CTR		BIT(6)
+#define AES_REG_CTRL_CBC		BIT(5)
+#define AES_REG_CTRL_KEY_SIZE		GENMASK(4, 3)
+#define AES_REG_CTRL_DIRECTION		BIT(2)
+#define AES_REG_CTRL_INPUT_READY	BIT(1)
+#define AES_REG_CTRL_OUTPUT_READY	BIT(0)
+#define AES_REG_CTRL_MASK		GENMASK(24, 2)
+
+#define AES_REG_C_LEN_0			0x54
+#define AES_REG_C_LEN_1			0x58
+#define AES_REG_A_LEN			0x5C
+
+#define AES_REG_DATA_N(dd, x)		((dd)->pdata->data_ofs + ((x) * 0x04))
+#define AES_REG_TAG_N(dd, x)		(0x70 + ((x) * 0x04))
+
+#define AES_REG_REV(dd)			((dd)->pdata->rev_ofs)
+
+#define AES_REG_MASK(dd)		((dd)->pdata->mask_ofs)
+#define AES_REG_MASK_SIDLE		BIT(6)
+#define AES_REG_MASK_START		BIT(5)
+#define AES_REG_MASK_DMA_OUT_EN		BIT(3)
+#define AES_REG_MASK_DMA_IN_EN		BIT(2)
+#define AES_REG_MASK_SOFTRESET		BIT(1)
+#define AES_REG_AUTOIDLE		BIT(0)
+
+#define AES_REG_LENGTH_N(x)		(0x54 + ((x) * 0x04))
+
+#define AES_REG_IRQ_STATUS(dd)         ((dd)->pdata->irq_status_ofs)
+#define AES_REG_IRQ_ENABLE(dd)         ((dd)->pdata->irq_enable_ofs)
+#define AES_REG_IRQ_DATA_IN            BIT(1)
+#define AES_REG_IRQ_DATA_OUT           BIT(2)
+#define DEFAULT_TIMEOUT		(5 * HZ)
+
+#define DEFAULT_AUTOSUSPEND_DELAY	1000
+
+#define FLAGS_MODE_MASK		0x001f
+#define FLAGS_ENCRYPT		BIT(0)
+#define FLAGS_CBC		BIT(1)
+#define FLAGS_CTR		BIT(2)
+#define FLAGS_GCM		BIT(3)
+#define FLAGS_RFC4106_GCM	BIT(4)
+
+#define FLAGS_INIT		BIT(5)
+#define FLAGS_FAST		BIT(6)
+#define FLAGS_BUSY		BIT(7)
+
+#define FLAGS_IN_DATA_ST_SHIFT	8
+#define FLAGS_OUT_DATA_ST_SHIFT	10
+#define FLAGS_ASSOC_DATA_ST_SHIFT	12
+
+#define AES_BLOCK_WORDS		(AES_BLOCK_SIZE >> 2)
+
+struct omap_aes_gcm_result {
+	struct completion completion;
+	int err;
+};
+
+struct omap_aes_ctx {
+	int		keylen;
+	u32		key[AES_KEYSIZE_256 / sizeof(u32)];
+	u8		nonce[4];
+	struct crypto_skcipher	*fallback;
+	struct crypto_skcipher	*ctr;
+};
+
+struct omap_aes_reqctx {
+	struct omap_aes_dev *dd;
+	unsigned long mode;
+	u8 iv[AES_BLOCK_SIZE];
+	u32 auth_tag[AES_BLOCK_SIZE / sizeof(u32)];
+};
+
+#define OMAP_AES_QUEUE_LENGTH	1
+#define OMAP_AES_CACHE_SIZE	0
+
+struct omap_aes_algs_info {
+	struct crypto_alg	*algs_list;
+	unsigned int		size;
+	unsigned int		registered;
+};
+
+struct omap_aes_aead_algs {
+	struct aead_alg	*algs_list;
+	unsigned int	size;
+	unsigned int	registered;
+};
+
+struct omap_aes_pdata {
+	struct omap_aes_algs_info	*algs_info;
+	unsigned int	algs_info_size;
+	struct omap_aes_aead_algs	*aead_algs_info;
+
+	void		(*trigger)(struct omap_aes_dev *dd, int length);
+
+	u32		key_ofs;
+	u32		iv_ofs;
+	u32		ctrl_ofs;
+	u32		data_ofs;
+	u32		rev_ofs;
+	u32		mask_ofs;
+	u32             irq_enable_ofs;
+	u32             irq_status_ofs;
+
+	u32		dma_enable_in;
+	u32		dma_enable_out;
+	u32		dma_start;
+
+	u32		major_mask;
+	u32		major_shift;
+	u32		minor_mask;
+	u32		minor_shift;
+};
+
+struct omap_aes_dev {
+	struct list_head	list;
+	unsigned long		phys_base;
+	void __iomem		*io_base;
+	struct omap_aes_ctx	*ctx;
+	struct device		*dev;
+	unsigned long		flags;
+	int			err;
+
+	struct tasklet_struct	done_task;
+	struct aead_queue	aead_queue;
+	spinlock_t		lock;
+
+	struct ablkcipher_request	*req;
+	struct aead_request		*aead_req;
+	struct crypto_engine		*engine;
+
+	/*
+	 * total is used by PIO mode for book keeping so introduce
+	 * variable total_save as need it to calc page_order
+	 */
+	size_t				total;
+	size_t				total_save;
+	size_t				assoc_len;
+	size_t				authsize;
+
+	struct scatterlist		*in_sg;
+	struct scatterlist		*out_sg;
+
+	/* Buffers for copying for unaligned cases */
+	struct scatterlist		in_sgl[2];
+	struct scatterlist		out_sgl;
+	struct scatterlist		*orig_out;
+
+	struct scatter_walk		in_walk;
+	struct scatter_walk		out_walk;
+	struct dma_chan		*dma_lch_in;
+	struct dma_chan		*dma_lch_out;
+	int			in_sg_len;
+	int			out_sg_len;
+	int			pio_only;
+	const struct omap_aes_pdata	*pdata;
+};
+
+u32 omap_aes_read(struct omap_aes_dev *dd, u32 offset);
+void omap_aes_write(struct omap_aes_dev *dd, u32 offset, u32 value);
+struct omap_aes_dev *omap_aes_find_dev(struct omap_aes_reqctx *rctx);
+int omap_aes_gcm_setkey(struct crypto_aead *tfm, const u8 *key,
+			unsigned int keylen);
+int omap_aes_4106gcm_setkey(struct crypto_aead *tfm, const u8 *key,
+			    unsigned int keylen);
+int omap_aes_gcm_encrypt(struct aead_request *req);
+int omap_aes_gcm_decrypt(struct aead_request *req);
+int omap_aes_4106gcm_encrypt(struct aead_request *req);
+int omap_aes_4106gcm_decrypt(struct aead_request *req);
+int omap_aes_write_ctrl(struct omap_aes_dev *dd);
+int omap_aes_crypt_dma_start(struct omap_aes_dev *dd);
+int omap_aes_crypt_dma_stop(struct omap_aes_dev *dd);
+void omap_aes_gcm_dma_out_callback(void *data);
+void omap_aes_clear_copy_flags(struct omap_aes_dev *dd);
+
+#endif
diff --git a/drivers/crypto/omap-crypto.c b/drivers/crypto/omap-crypto.c
new file mode 100644
index 000000000000..23e37779317e
--- /dev/null
+++ b/drivers/crypto/omap-crypto.c
@@ -0,0 +1,184 @@
+/*
+ * OMAP Crypto driver common support routines.
+ *
+ * Copyright (c) 2017 Texas Instruments Incorporated
+ *   Tero Kristo <t-kristo@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/kernel.h>
+#include <linux/scatterlist.h>
+#include <crypto/scatterwalk.h>
+
+#include "omap-crypto.h"
+
+static int omap_crypto_copy_sg_lists(int total, int bs,
+				     struct scatterlist **sg,
+				     struct scatterlist *new_sg, u16 flags)
+{
+	int n = sg_nents(*sg);
+	struct scatterlist *tmp;
+
+	if (!(flags & OMAP_CRYPTO_FORCE_SINGLE_ENTRY)) {
+		new_sg = kmalloc_array(n, sizeof(*sg), GFP_KERNEL);
+		if (!new_sg)
+			return -ENOMEM;
+
+		sg_init_table(new_sg, n);
+	}
+
+	tmp = new_sg;
+
+	while (*sg && total) {
+		int len = (*sg)->length;
+
+		if (total < len)
+			len = total;
+
+		if (len > 0) {
+			total -= len;
+			sg_set_page(tmp, sg_page(*sg), len, (*sg)->offset);
+			if (total <= 0)
+				sg_mark_end(tmp);
+			tmp = sg_next(tmp);
+		}
+
+		*sg = sg_next(*sg);
+	}
+
+	*sg = new_sg;
+
+	return 0;
+}
+
+static int omap_crypto_copy_sgs(int total, int bs, struct scatterlist **sg,
+				struct scatterlist *new_sg, u16 flags)
+{
+	void *buf;
+	int pages;
+	int new_len;
+
+	new_len = ALIGN(total, bs);
+	pages = get_order(new_len);
+
+	buf = (void *)__get_free_pages(GFP_ATOMIC, pages);
+	if (!buf) {
+		pr_err("%s: Couldn't allocate pages for unaligned cases.\n",
+		       __func__);
+		return -ENOMEM;
+	}
+
+	if (flags & OMAP_CRYPTO_COPY_DATA) {
+		scatterwalk_map_and_copy(buf, *sg, 0, total, 0);
+		if (flags & OMAP_CRYPTO_ZERO_BUF)
+			memset(buf + total, 0, new_len - total);
+	}
+
+	if (!(flags & OMAP_CRYPTO_FORCE_SINGLE_ENTRY))
+		sg_init_table(new_sg, 1);
+
+	sg_set_buf(new_sg, buf, new_len);
+
+	*sg = new_sg;
+
+	return 0;
+}
+
+static int omap_crypto_check_sg(struct scatterlist *sg, int total, int bs,
+				u16 flags)
+{
+	int len = 0;
+	int num_sg = 0;
+
+	if (!IS_ALIGNED(total, bs))
+		return OMAP_CRYPTO_NOT_ALIGNED;
+
+	while (sg) {
+		num_sg++;
+
+		if (!IS_ALIGNED(sg->offset, 4))
+			return OMAP_CRYPTO_NOT_ALIGNED;
+		if (!IS_ALIGNED(sg->length, bs))
+			return OMAP_CRYPTO_NOT_ALIGNED;
+
+		len += sg->length;
+		sg = sg_next(sg);
+
+		if (len >= total)
+			break;
+	}
+
+	if ((flags & OMAP_CRYPTO_FORCE_SINGLE_ENTRY) && num_sg > 1)
+		return OMAP_CRYPTO_NOT_ALIGNED;
+
+	if (len != total)
+		return OMAP_CRYPTO_BAD_DATA_LENGTH;
+
+	return 0;
+}
+
+int omap_crypto_align_sg(struct scatterlist **sg, int total, int bs,
+			 struct scatterlist *new_sg, u16 flags,
+			 u8 flags_shift, unsigned long *dd_flags)
+{
+	int ret;
+
+	*dd_flags &= ~(OMAP_CRYPTO_COPY_MASK << flags_shift);
+
+	if (flags & OMAP_CRYPTO_FORCE_COPY)
+		ret = OMAP_CRYPTO_NOT_ALIGNED;
+	else
+		ret = omap_crypto_check_sg(*sg, total, bs, flags);
+
+	if (ret == OMAP_CRYPTO_NOT_ALIGNED) {
+		ret = omap_crypto_copy_sgs(total, bs, sg, new_sg, flags);
+		if (ret)
+			return ret;
+		*dd_flags |= OMAP_CRYPTO_DATA_COPIED << flags_shift;
+	} else if (ret == OMAP_CRYPTO_BAD_DATA_LENGTH) {
+		ret = omap_crypto_copy_sg_lists(total, bs, sg, new_sg, flags);
+		if (ret)
+			return ret;
+		if (!(flags & OMAP_CRYPTO_FORCE_SINGLE_ENTRY))
+			*dd_flags |= OMAP_CRYPTO_SG_COPIED << flags_shift;
+	} else if (flags & OMAP_CRYPTO_FORCE_SINGLE_ENTRY) {
+		sg_set_buf(new_sg, sg_virt(*sg), (*sg)->length);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(omap_crypto_align_sg);
+
+void omap_crypto_cleanup(struct scatterlist *sg, struct scatterlist *orig,
+			 int offset, int len, u8 flags_shift,
+			 unsigned long flags)
+{
+	void *buf;
+	int pages;
+
+	flags >>= flags_shift;
+	flags &= OMAP_CRYPTO_COPY_MASK;
+
+	if (!flags)
+		return;
+
+	buf = sg_virt(sg);
+	pages = get_order(len);
+
+	if (orig && (flags & OMAP_CRYPTO_COPY_MASK))
+		scatterwalk_map_and_copy(buf, orig, offset, len, 1);
+
+	if (flags & OMAP_CRYPTO_DATA_COPIED)
+		free_pages((unsigned long)buf, pages);
+	else if (flags & OMAP_CRYPTO_SG_COPIED)
+		kfree(sg);
+}
+EXPORT_SYMBOL_GPL(omap_crypto_cleanup);
+
+MODULE_DESCRIPTION("OMAP crypto support library.");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Tero Kristo <t-kristo@ti.com>");
diff --git a/drivers/crypto/omap-crypto.h b/drivers/crypto/omap-crypto.h
new file mode 100644
index 000000000000..36a230eb87af
--- /dev/null
+++ b/drivers/crypto/omap-crypto.h
@@ -0,0 +1,37 @@
+/*
+ * OMAP Crypto driver common support routines.
+ *
+ * Copyright (c) 2017 Texas Instruments Incorporated
+ *   Tero Kristo <t-kristo@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.
+ */
+
+#ifndef __CRYPTO_OMAP_CRYPTO_H
+#define __CRYPTO_OMAP_CRYPTO_H
+
+enum {
+	OMAP_CRYPTO_NOT_ALIGNED = 1,
+	OMAP_CRYPTO_BAD_DATA_LENGTH,
+};
+
+#define OMAP_CRYPTO_DATA_COPIED		BIT(0)
+#define OMAP_CRYPTO_SG_COPIED		BIT(1)
+
+#define OMAP_CRYPTO_COPY_MASK		0x3
+
+#define OMAP_CRYPTO_COPY_DATA		BIT(0)
+#define OMAP_CRYPTO_FORCE_COPY		BIT(1)
+#define OMAP_CRYPTO_ZERO_BUF		BIT(2)
+#define OMAP_CRYPTO_FORCE_SINGLE_ENTRY	BIT(3)
+
+int omap_crypto_align_sg(struct scatterlist **sg, int total, int bs,
+			 struct scatterlist *new_sg, u16 flags,
+			 u8 flags_shift, unsigned long *dd_flags);
+void omap_crypto_cleanup(struct scatterlist *sg, struct scatterlist *orig,
+			 int offset, int len, u8 flags_shift,
+			 unsigned long flags);
+
+#endif
diff --git a/drivers/crypto/omap-des.c b/drivers/crypto/omap-des.c
index a6f65532fd16..0bcab00e0ff5 100644
--- a/drivers/crypto/omap-des.c
+++ b/drivers/crypto/omap-des.c
@@ -41,6 +41,8 @@
 #include <crypto/algapi.h>
 #include <crypto/engine.h>
 
+#include "omap-crypto.h"
+
 #define DST_MAXBURST			2
 
 #define DES_BLOCK_WORDS		(DES_BLOCK_SIZE >> 2)
@@ -78,6 +80,11 @@
 #define FLAGS_INIT		BIT(4)
 #define FLAGS_BUSY		BIT(6)
 
+#define DEFAULT_AUTOSUSPEND_DELAY	1000
+
+#define FLAGS_IN_DATA_ST_SHIFT	8
+#define FLAGS_OUT_DATA_ST_SHIFT	10
+
 struct omap_des_ctx {
 	struct omap_des_dev *dd;
 
@@ -151,7 +158,6 @@ struct omap_des_dev {
 	struct scatterlist		in_sgl;
 	struct scatterlist		out_sgl;
 	struct scatterlist		*orig_out;
-	int				sgs_copied;
 
 	struct scatter_walk		in_walk;
 	struct scatter_walk		out_walk;
@@ -370,20 +376,6 @@ static void omap_des_dma_cleanup(struct omap_des_dev *dd)
 	dma_release_channel(dd->dma_lch_in);
 }
 
-static void sg_copy_buf(void *buf, struct scatterlist *sg,
-			      unsigned int start, unsigned int nbytes, int out)
-{
-	struct scatter_walk walk;
-
-	if (!nbytes)
-		return;
-
-	scatterwalk_start(&walk, sg);
-	scatterwalk_advance(&walk, start);
-	scatterwalk_copychunks(buf, &walk, nbytes, out);
-	scatterwalk_done(&walk, out, 0);
-}
-
 static int omap_des_crypt_dma(struct crypto_tfm *tfm,
 		struct scatterlist *in_sg, struct scatterlist *out_sg,
 		int in_sg_len, int out_sg_len)
@@ -506,8 +498,10 @@ static void omap_des_finish_req(struct omap_des_dev *dd, int err)
 
 	pr_debug("err: %d\n", err);
 
-	pm_runtime_put(dd->dev);
 	crypto_finalize_cipher_request(dd->engine, req, err);
+
+	pm_runtime_mark_last_busy(dd->dev);
+	pm_runtime_put_autosuspend(dd->dev);
 }
 
 static int omap_des_crypt_dma_stop(struct omap_des_dev *dd)
@@ -522,55 +516,6 @@ static int omap_des_crypt_dma_stop(struct omap_des_dev *dd)
 	return 0;
 }
 
-static int omap_des_copy_needed(struct scatterlist *sg)
-{
-	while (sg) {
-		if (!IS_ALIGNED(sg->offset, 4))
-			return -1;
-		if (!IS_ALIGNED(sg->length, DES_BLOCK_SIZE))
-			return -1;
-		sg = sg_next(sg);
-	}
-	return 0;
-}
-
-static int omap_des_copy_sgs(struct omap_des_dev *dd)
-{
-	void *buf_in, *buf_out;
-	int pages;
-
-	pages = dd->total >> PAGE_SHIFT;
-
-	if (dd->total & (PAGE_SIZE-1))
-		pages++;
-
-	BUG_ON(!pages);
-
-	buf_in = (void *)__get_free_pages(GFP_ATOMIC, pages);
-	buf_out = (void *)__get_free_pages(GFP_ATOMIC, pages);
-
-	if (!buf_in || !buf_out) {
-		pr_err("Couldn't allocated pages for unaligned cases.\n");
-		return -1;
-	}
-
-	dd->orig_out = dd->out_sg;
-
-	sg_copy_buf(buf_in, dd->in_sg, 0, dd->total, 0);
-
-	sg_init_table(&dd->in_sgl, 1);
-	sg_set_buf(&dd->in_sgl, buf_in, dd->total);
-	dd->in_sg = &dd->in_sgl;
-	dd->in_sg_len = 1;
-
-	sg_init_table(&dd->out_sgl, 1);
-	sg_set_buf(&dd->out_sgl, buf_out, dd->total);
-	dd->out_sg = &dd->out_sgl;
-	dd->out_sg_len = 1;
-
-	return 0;
-}
-
 static int omap_des_handle_queue(struct omap_des_dev *dd,
 				 struct ablkcipher_request *req)
 {
@@ -587,6 +532,8 @@ static int omap_des_prepare_req(struct crypto_engine *engine,
 			crypto_ablkcipher_reqtfm(req));
 	struct omap_des_dev *dd = omap_des_find_dev(ctx);
 	struct omap_des_reqctx *rctx;
+	int ret;
+	u16 flags;
 
 	if (!dd)
 		return -ENODEV;
@@ -597,6 +544,23 @@ static int omap_des_prepare_req(struct crypto_engine *engine,
 	dd->total_save = req->nbytes;
 	dd->in_sg = req->src;
 	dd->out_sg = req->dst;
+	dd->orig_out = req->dst;
+
+	flags = OMAP_CRYPTO_COPY_DATA;
+	if (req->src == req->dst)
+		flags |= OMAP_CRYPTO_FORCE_COPY;
+
+	ret = omap_crypto_align_sg(&dd->in_sg, dd->total, DES_BLOCK_SIZE,
+				   &dd->in_sgl, flags,
+				   FLAGS_IN_DATA_ST_SHIFT, &dd->flags);
+	if (ret)
+		return ret;
+
+	ret = omap_crypto_align_sg(&dd->out_sg, dd->total, DES_BLOCK_SIZE,
+				   &dd->out_sgl, 0,
+				   FLAGS_OUT_DATA_ST_SHIFT, &dd->flags);
+	if (ret)
+		return ret;
 
 	dd->in_sg_len = sg_nents_for_len(dd->in_sg, dd->total);
 	if (dd->in_sg_len < 0)
@@ -606,15 +570,6 @@ static int omap_des_prepare_req(struct crypto_engine *engine,
 	if (dd->out_sg_len < 0)
 		return dd->out_sg_len;
 
-	if (omap_des_copy_needed(dd->in_sg) ||
-	    omap_des_copy_needed(dd->out_sg)) {
-		if (omap_des_copy_sgs(dd))
-			pr_err("Failed to copy SGs for unaligned cases\n");
-		dd->sgs_copied = 1;
-	} else {
-		dd->sgs_copied = 0;
-	}
-
 	rctx = ablkcipher_request_ctx(req);
 	ctx = crypto_ablkcipher_ctx(crypto_ablkcipher_reqtfm(req));
 	rctx->mode &= FLAGS_MODE_MASK;
@@ -642,8 +597,6 @@ static int omap_des_crypt_req(struct crypto_engine *engine,
 static void omap_des_done_task(unsigned long data)
 {
 	struct omap_des_dev *dd = (struct omap_des_dev *)data;
-	void *buf_in, *buf_out;
-	int pages;
 
 	pr_debug("enter done_task\n");
 
@@ -656,16 +609,11 @@ static void omap_des_done_task(unsigned long data)
 		omap_des_crypt_dma_stop(dd);
 	}
 
-	if (dd->sgs_copied) {
-		buf_in = sg_virt(&dd->in_sgl);
-		buf_out = sg_virt(&dd->out_sgl);
+	omap_crypto_cleanup(&dd->in_sgl, NULL, 0, dd->total_save,
+			    FLAGS_IN_DATA_ST_SHIFT, dd->flags);
 
-		sg_copy_buf(buf_out, dd->orig_out, 0, dd->total_save, 1);
-
-		pages = get_order(dd->total_save);
-		free_pages((unsigned long)buf_in, pages);
-		free_pages((unsigned long)buf_out, pages);
-	}
+	omap_crypto_cleanup(&dd->out_sgl, dd->orig_out, 0, dd->total_save,
+			    FLAGS_OUT_DATA_ST_SHIFT, dd->flags);
 
 	omap_des_finish_req(dd, 0);
 
@@ -699,16 +647,28 @@ static int omap_des_crypt(struct ablkcipher_request *req, unsigned long mode)
 
 /* ********************** ALG API ************************************ */
 
-static int omap_des_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+static int omap_des_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
 			   unsigned int keylen)
 {
-	struct omap_des_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct omap_des_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
 
 	if (keylen != DES_KEY_SIZE && keylen != (3*DES_KEY_SIZE))
 		return -EINVAL;
 
 	pr_debug("enter, keylen: %d\n", keylen);
 
+	/* Do we need to test against weak key? */
+	if (tfm->crt_flags & CRYPTO_TFM_REQ_WEAK_KEY) {
+		u32 tmp[DES_EXPKEY_WORDS];
+		int ret = des_ekey(tmp, key);
+
+		if (!ret) {
+			tfm->crt_flags |= CRYPTO_TFM_RES_WEAK_KEY;
+			return -EINVAL;
+		}
+	}
+
 	memcpy(ctx->key, key, keylen);
 	ctx->keylen = keylen;
 
@@ -1032,8 +992,10 @@ static int omap_des_probe(struct platform_device *pdev)
 	}
 	dd->phys_base = res->start;
 
+	pm_runtime_use_autosuspend(dev);
+	pm_runtime_set_autosuspend_delay(dev, DEFAULT_AUTOSUSPEND_DELAY);
+
 	pm_runtime_enable(dev);
-	pm_runtime_irq_safe(dev);
 	err = pm_runtime_get_sync(dev);
 	if (err < 0) {
 		pm_runtime_put_noidle(dev);
diff --git a/drivers/crypto/omap-sham.c b/drivers/crypto/omap-sham.c
index d0b16e5e4ee5..9ad9d399daf1 100644
--- a/drivers/crypto/omap-sham.c
+++ b/drivers/crypto/omap-sham.c
@@ -41,6 +41,7 @@
 #include <crypto/algapi.h>
 #include <crypto/sha.h>
 #include <crypto/hash.h>
+#include <crypto/hmac.h>
 #include <crypto/internal/hash.h>
 
 #define MD5_DIGEST_SIZE			16
@@ -225,7 +226,7 @@ struct omap_sham_dev {
 	struct dma_chan		*dma_lch;
 	struct tasklet_struct	done_task;
 	u8			polling_mode;
-	u8			xmit_buf[BUFLEN];
+	u8			xmit_buf[BUFLEN] OMAP_ALIGNED;
 
 	unsigned long		flags;
 	struct crypto_queue	queue;
@@ -750,7 +751,10 @@ static int omap_sham_align_sgs(struct scatterlist *sg,
 	if (final)
 		new_len = DIV_ROUND_UP(new_len, bs) * bs;
 	else
-		new_len = new_len / bs * bs;
+		new_len = (new_len - 1) / bs * bs;
+
+	if (nbytes != new_len)
+		list_ok = false;
 
 	while (nbytes > 0 && sg_tmp) {
 		n++;
@@ -846,6 +850,8 @@ static int omap_sham_prepare_request(struct ahash_request *req, bool update)
 			xmit_len = DIV_ROUND_UP(xmit_len, bs) * bs;
 		else
 			xmit_len = xmit_len / bs * bs;
+	} else if (!final) {
+		xmit_len -= bs;
 	}
 
 	hash_later = rctx->total - xmit_len;
@@ -873,14 +879,21 @@ static int omap_sham_prepare_request(struct ahash_request *req, bool update)
 	}
 
 	if (hash_later) {
-		if (req->nbytes) {
-			scatterwalk_map_and_copy(rctx->buffer, req->src,
-						 req->nbytes - hash_later,
-						 hash_later, 0);
-		} else {
+		int offset = 0;
+
+		if (hash_later > req->nbytes) {
 			memcpy(rctx->buffer, rctx->buffer + xmit_len,
-			       hash_later);
+			       hash_later - req->nbytes);
+			offset = hash_later - req->nbytes;
 		}
+
+		if (req->nbytes) {
+			scatterwalk_map_and_copy(rctx->buffer + offset,
+						 req->src,
+						 offset + req->nbytes -
+						 hash_later, hash_later, 0);
+		}
+
 		rctx->bufcnt = hash_later;
 	} else {
 		rctx->bufcnt = 0;
@@ -1130,7 +1143,7 @@ retry:
 	ctx = ahash_request_ctx(req);
 
 	err = omap_sham_prepare_request(req, ctx->op == OP_UPDATE);
-	if (err)
+	if (err || !ctx->total)
 		goto err1;
 
 	dev_dbg(dd->dev, "handling new req, op: %lu, nbytes: %d\n",
@@ -1189,11 +1202,10 @@ static int omap_sham_update(struct ahash_request *req)
 	if (!req->nbytes)
 		return 0;
 
-	if (ctx->total + req->nbytes < ctx->buflen) {
+	if (ctx->bufcnt + req->nbytes <= ctx->buflen) {
 		scatterwalk_map_and_copy(ctx->buffer + ctx->bufcnt, req->src,
 					 0, req->nbytes, 0);
 		ctx->bufcnt += req->nbytes;
-		ctx->total += req->nbytes;
 		return 0;
 	}
 
@@ -1326,8 +1338,8 @@ static int omap_sham_setkey(struct crypto_ahash *tfm, const u8 *key,
 		memcpy(bctx->opad, bctx->ipad, bs);
 
 		for (i = 0; i < bs; i++) {
-			bctx->ipad[i] ^= 0x36;
-			bctx->opad[i] ^= 0x5c;
+			bctx->ipad[i] ^= HMAC_IPAD_VALUE;
+			bctx->opad[i] ^= HMAC_OPAD_VALUE;
 		}
 	}
 
diff --git a/drivers/crypto/qat/qat_common/adf_aer.c b/drivers/crypto/qat/qat_common/adf_aer.c
index 2839fccdd84b..d3e25c37dc33 100644
--- a/drivers/crypto/qat/qat_common/adf_aer.c
+++ b/drivers/crypto/qat/qat_common/adf_aer.c
@@ -109,20 +109,7 @@ EXPORT_SYMBOL_GPL(adf_reset_sbr);
 
 void adf_reset_flr(struct adf_accel_dev *accel_dev)
 {
-	struct pci_dev *pdev = accel_to_pci_dev(accel_dev);
-	u16 control = 0;
-	int pos = 0;
-
-	dev_info(&GET_DEV(accel_dev), "Function level reset\n");
-	pos = pci_pcie_cap(pdev);
-	if (!pos) {
-		dev_err(&GET_DEV(accel_dev), "Restart device failed\n");
-		return;
-	}
-	pci_read_config_word(pdev, pos + PCI_EXP_DEVCTL, &control);
-	control |= PCI_EXP_DEVCTL_BCR_FLR;
-	pci_write_config_word(pdev, pos + PCI_EXP_DEVCTL, control);
-	msleep(100);
+	pcie_flr(accel_to_pci_dev(accel_dev));
 }
 EXPORT_SYMBOL_GPL(adf_reset_flr);
 
diff --git a/drivers/crypto/qat/qat_common/qat_algs.c b/drivers/crypto/qat/qat_common/qat_algs.c
index 20f35df8a01f..5b5efcc52cb5 100644
--- a/drivers/crypto/qat/qat_common/qat_algs.c
+++ b/drivers/crypto/qat/qat_common/qat_algs.c
@@ -51,6 +51,7 @@
 #include <crypto/aes.h>
 #include <crypto/sha.h>
 #include <crypto/hash.h>
+#include <crypto/hmac.h>
 #include <crypto/algapi.h>
 #include <crypto/authenc.h>
 #include <linux/dma-mapping.h>
@@ -178,8 +179,8 @@ static int qat_alg_do_precomputes(struct icp_qat_hw_auth_algo_blk *hash,
 	for (i = 0; i < block_size; i++) {
 		char *ipad_ptr = ipad + i;
 		char *opad_ptr = opad + i;
-		*ipad_ptr ^= 0x36;
-		*opad_ptr ^= 0x5C;
+		*ipad_ptr ^= HMAC_IPAD_VALUE;
+		*opad_ptr ^= HMAC_OPAD_VALUE;
 	}
 
 	if (crypto_shash_init(shash))
diff --git a/drivers/crypto/qat/qat_common/qat_asym_algs.c b/drivers/crypto/qat/qat_common/qat_asym_algs.c
index 2aab80bc241f..6f5dd68449c6 100644
--- a/drivers/crypto/qat/qat_common/qat_asym_algs.c
+++ b/drivers/crypto/qat/qat_common/qat_asym_algs.c
@@ -521,11 +521,11 @@ static int qat_dh_set_secret(struct crypto_kpp *tfm, const void *buf,
 	return 0;
 }
 
-static int qat_dh_max_size(struct crypto_kpp *tfm)
+static unsigned int qat_dh_max_size(struct crypto_kpp *tfm)
 {
 	struct qat_dh_ctx *ctx = kpp_tfm_ctx(tfm);
 
-	return ctx->p ? ctx->p_size : -EINVAL;
+	return ctx->p_size;
 }
 
 static int qat_dh_init_tfm(struct crypto_kpp *tfm)
@@ -1256,11 +1256,11 @@ static int qat_rsa_setprivkey(struct crypto_akcipher *tfm, const void *key,
 	return qat_rsa_setkey(tfm, key, keylen, true);
 }
 
-static int qat_rsa_max_size(struct crypto_akcipher *tfm)
+static unsigned int qat_rsa_max_size(struct crypto_akcipher *tfm)
 {
 	struct qat_rsa_ctx *ctx = akcipher_tfm_ctx(tfm);
 
-	return (ctx->n) ? ctx->key_sz : -EINVAL;
+	return ctx->key_sz;
 }
 
 static int qat_rsa_init_tfm(struct crypto_akcipher *tfm)
diff --git a/drivers/crypto/sunxi-ss/sun4i-ss-cipher.c b/drivers/crypto/sunxi-ss/sun4i-ss-cipher.c
index 90efd10d57a1..5cf64746731a 100644
--- a/drivers/crypto/sunxi-ss/sun4i-ss-cipher.c
+++ b/drivers/crypto/sunxi-ss/sun4i-ss-cipher.c
@@ -16,13 +16,13 @@
  */
 #include "sun4i-ss.h"
 
-static int sun4i_ss_opti_poll(struct ablkcipher_request *areq)
+static int sun4i_ss_opti_poll(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
 	struct sun4i_ss_ctx *ss = op->ss;
-	unsigned int ivsize = crypto_ablkcipher_ivsize(tfm);
-	struct sun4i_cipher_req_ctx *ctx = ablkcipher_request_ctx(areq);
+	unsigned int ivsize = crypto_skcipher_ivsize(tfm);
+	struct sun4i_cipher_req_ctx *ctx = skcipher_request_ctx(areq);
 	u32 mode = ctx->mode;
 	/* when activating SS, the default FIFO space is SS_RX_DEFAULT(32) */
 	u32 rx_cnt = SS_RX_DEFAULT;
@@ -31,17 +31,17 @@ static int sun4i_ss_opti_poll(struct ablkcipher_request *areq)
 	u32 v;
 	int err = 0;
 	unsigned int i;
-	unsigned int ileft = areq->nbytes;
-	unsigned int oleft = areq->nbytes;
+	unsigned int ileft = areq->cryptlen;
+	unsigned int oleft = areq->cryptlen;
 	unsigned int todo;
 	struct sg_mapping_iter mi, mo;
 	unsigned int oi, oo; /* offset for in and out */
 	unsigned long flags;
 
-	if (areq->nbytes == 0)
+	if (!areq->cryptlen)
 		return 0;
 
-	if (!areq->info) {
+	if (!areq->iv) {
 		dev_err_ratelimited(ss->dev, "ERROR: Empty IV\n");
 		return -EINVAL;
 	}
@@ -56,9 +56,9 @@ static int sun4i_ss_opti_poll(struct ablkcipher_request *areq)
 	for (i = 0; i < op->keylen; i += 4)
 		writel(*(op->key + i / 4), ss->base + SS_KEY0 + i);
 
-	if (areq->info) {
+	if (areq->iv) {
 		for (i = 0; i < 4 && i < ivsize / 4; i++) {
-			v = *(u32 *)(areq->info + i * 4);
+			v = *(u32 *)(areq->iv + i * 4);
 			writel(v, ss->base + SS_IV0 + i * 4);
 		}
 	}
@@ -76,13 +76,13 @@ static int sun4i_ss_opti_poll(struct ablkcipher_request *areq)
 		goto release_ss;
 	}
 
-	ileft = areq->nbytes / 4;
-	oleft = areq->nbytes / 4;
+	ileft = areq->cryptlen / 4;
+	oleft = areq->cryptlen / 4;
 	oi = 0;
 	oo = 0;
 	do {
 		todo = min3(rx_cnt, ileft, (mi.length - oi) / 4);
-		if (todo > 0) {
+		if (todo) {
 			ileft -= todo;
 			writesl(ss->base + SS_RXFIFO, mi.addr + oi, todo);
 			oi += todo * 4;
@@ -97,7 +97,7 @@ static int sun4i_ss_opti_poll(struct ablkcipher_request *areq)
 		tx_cnt = SS_TXFIFO_SPACES(spaces);
 
 		todo = min3(tx_cnt, oleft, (mo.length - oo) / 4);
-		if (todo > 0) {
+		if (todo) {
 			oleft -= todo;
 			readsl(ss->base + SS_TXFIFO, mo.addr + oo, todo);
 			oo += todo * 4;
@@ -106,12 +106,12 @@ static int sun4i_ss_opti_poll(struct ablkcipher_request *areq)
 			sg_miter_next(&mo);
 			oo = 0;
 		}
-	} while (oleft > 0);
+	} while (oleft);
 
-	if (areq->info) {
+	if (areq->iv) {
 		for (i = 0; i < 4 && i < ivsize / 4; i++) {
 			v = readl(ss->base + SS_IV0 + i * 4);
-			*(u32 *)(areq->info + i * 4) = v;
+			*(u32 *)(areq->iv + i * 4) = v;
 		}
 	}
 
@@ -124,16 +124,16 @@ release_ss:
 }
 
 /* Generic function that support SG with size not multiple of 4 */
-static int sun4i_ss_cipher_poll(struct ablkcipher_request *areq)
+static int sun4i_ss_cipher_poll(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
 	struct sun4i_ss_ctx *ss = op->ss;
 	int no_chunk = 1;
 	struct scatterlist *in_sg = areq->src;
 	struct scatterlist *out_sg = areq->dst;
-	unsigned int ivsize = crypto_ablkcipher_ivsize(tfm);
-	struct sun4i_cipher_req_ctx *ctx = ablkcipher_request_ctx(areq);
+	unsigned int ivsize = crypto_skcipher_ivsize(tfm);
+	struct sun4i_cipher_req_ctx *ctx = skcipher_request_ctx(areq);
 	u32 mode = ctx->mode;
 	/* when activating SS, the default FIFO space is SS_RX_DEFAULT(32) */
 	u32 rx_cnt = SS_RX_DEFAULT;
@@ -142,8 +142,8 @@ static int sun4i_ss_cipher_poll(struct ablkcipher_request *areq)
 	u32 spaces;
 	int err = 0;
 	unsigned int i;
-	unsigned int ileft = areq->nbytes;
-	unsigned int oleft = areq->nbytes;
+	unsigned int ileft = areq->cryptlen;
+	unsigned int oleft = areq->cryptlen;
 	unsigned int todo;
 	struct sg_mapping_iter mi, mo;
 	unsigned int oi, oo;	/* offset for in and out */
@@ -154,10 +154,10 @@ static int sun4i_ss_cipher_poll(struct ablkcipher_request *areq)
 	unsigned int obl = 0;	/* length of data in bufo */
 	unsigned long flags;
 
-	if (areq->nbytes == 0)
+	if (!areq->cryptlen)
 		return 0;
 
-	if (!areq->info) {
+	if (!areq->iv) {
 		dev_err_ratelimited(ss->dev, "ERROR: Empty IV\n");
 		return -EINVAL;
 	}
@@ -172,12 +172,12 @@ static int sun4i_ss_cipher_poll(struct ablkcipher_request *areq)
 	 * we can use the SS optimized function
 	 */
 	while (in_sg && no_chunk == 1) {
-		if ((in_sg->length % 4) != 0)
+		if (in_sg->length % 4)
 			no_chunk = 0;
 		in_sg = sg_next(in_sg);
 	}
 	while (out_sg && no_chunk == 1) {
-		if ((out_sg->length % 4) != 0)
+		if (out_sg->length % 4)
 			no_chunk = 0;
 		out_sg = sg_next(out_sg);
 	}
@@ -190,9 +190,9 @@ static int sun4i_ss_cipher_poll(struct ablkcipher_request *areq)
 	for (i = 0; i < op->keylen; i += 4)
 		writel(*(op->key + i / 4), ss->base + SS_KEY0 + i);
 
-	if (areq->info) {
+	if (areq->iv) {
 		for (i = 0; i < 4 && i < ivsize / 4; i++) {
-			v = *(u32 *)(areq->info + i * 4);
+			v = *(u32 *)(areq->iv + i * 4);
 			writel(v, ss->base + SS_IV0 + i * 4);
 		}
 	}
@@ -209,19 +209,19 @@ static int sun4i_ss_cipher_poll(struct ablkcipher_request *areq)
 		err = -EINVAL;
 		goto release_ss;
 	}
-	ileft = areq->nbytes;
-	oleft = areq->nbytes;
+	ileft = areq->cryptlen;
+	oleft = areq->cryptlen;
 	oi = 0;
 	oo = 0;
 
-	while (oleft > 0) {
-		if (ileft > 0) {
+	while (oleft) {
+		if (ileft) {
 			/*
 			 * todo is the number of consecutive 4byte word that we
 			 * can read from current SG
 			 */
 			todo = min3(rx_cnt, ileft / 4, (mi.length - oi) / 4);
-			if (todo > 0 && ob == 0) {
+			if (todo && !ob) {
 				writesl(ss->base + SS_RXFIFO, mi.addr + oi,
 					todo);
 				ileft -= todo * 4;
@@ -240,7 +240,7 @@ static int sun4i_ss_cipher_poll(struct ablkcipher_request *areq)
 				ileft -= todo;
 				oi += todo;
 				ob += todo;
-				if (ob % 4 == 0) {
+				if (!(ob % 4)) {
 					writesl(ss->base + SS_RXFIFO, buf,
 						ob / 4);
 					ob = 0;
@@ -257,14 +257,14 @@ static int sun4i_ss_cipher_poll(struct ablkcipher_request *areq)
 		tx_cnt = SS_TXFIFO_SPACES(spaces);
 		dev_dbg(ss->dev, "%x %u/%u %u/%u cnt=%u %u/%u %u/%u cnt=%u %u\n",
 			mode,
-			oi, mi.length, ileft, areq->nbytes, rx_cnt,
-			oo, mo.length, oleft, areq->nbytes, tx_cnt, ob);
+			oi, mi.length, ileft, areq->cryptlen, rx_cnt,
+			oo, mo.length, oleft, areq->cryptlen, tx_cnt, ob);
 
-		if (tx_cnt == 0)
+		if (!tx_cnt)
 			continue;
 		/* todo in 4bytes word */
 		todo = min3(tx_cnt, oleft / 4, (mo.length - oo) / 4);
-		if (todo > 0) {
+		if (todo) {
 			readsl(ss->base + SS_TXFIFO, mo.addr + oo, todo);
 			oleft -= todo * 4;
 			oo += todo * 4;
@@ -300,10 +300,10 @@ static int sun4i_ss_cipher_poll(struct ablkcipher_request *areq)
 			/* bufo must be fully used here */
 		}
 	}
-	if (areq->info) {
+	if (areq->iv) {
 		for (i = 0; i < 4 && i < ivsize / 4; i++) {
 			v = readl(ss->base + SS_IV0 + i * 4);
-			*(u32 *)(areq->info + i * 4) = v;
+			*(u32 *)(areq->iv + i * 4) = v;
 		}
 	}
 
@@ -317,22 +317,22 @@ release_ss:
 }
 
 /* CBC AES */
-int sun4i_ss_cbc_aes_encrypt(struct ablkcipher_request *areq)
+int sun4i_ss_cbc_aes_encrypt(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
-	struct sun4i_cipher_req_ctx *rctx = ablkcipher_request_ctx(areq);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
+	struct sun4i_cipher_req_ctx *rctx = skcipher_request_ctx(areq);
 
 	rctx->mode = SS_OP_AES | SS_CBC | SS_ENABLED | SS_ENCRYPTION |
 		op->keymode;
 	return sun4i_ss_cipher_poll(areq);
 }
 
-int sun4i_ss_cbc_aes_decrypt(struct ablkcipher_request *areq)
+int sun4i_ss_cbc_aes_decrypt(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
-	struct sun4i_cipher_req_ctx *rctx = ablkcipher_request_ctx(areq);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
+	struct sun4i_cipher_req_ctx *rctx = skcipher_request_ctx(areq);
 
 	rctx->mode = SS_OP_AES | SS_CBC | SS_ENABLED | SS_DECRYPTION |
 		op->keymode;
@@ -340,22 +340,22 @@ int sun4i_ss_cbc_aes_decrypt(struct ablkcipher_request *areq)
 }
 
 /* ECB AES */
-int sun4i_ss_ecb_aes_encrypt(struct ablkcipher_request *areq)
+int sun4i_ss_ecb_aes_encrypt(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
-	struct sun4i_cipher_req_ctx *rctx = ablkcipher_request_ctx(areq);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
+	struct sun4i_cipher_req_ctx *rctx = skcipher_request_ctx(areq);
 
 	rctx->mode = SS_OP_AES | SS_ECB | SS_ENABLED | SS_ENCRYPTION |
 		op->keymode;
 	return sun4i_ss_cipher_poll(areq);
 }
 
-int sun4i_ss_ecb_aes_decrypt(struct ablkcipher_request *areq)
+int sun4i_ss_ecb_aes_decrypt(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
-	struct sun4i_cipher_req_ctx *rctx = ablkcipher_request_ctx(areq);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
+	struct sun4i_cipher_req_ctx *rctx = skcipher_request_ctx(areq);
 
 	rctx->mode = SS_OP_AES | SS_ECB | SS_ENABLED | SS_DECRYPTION |
 		op->keymode;
@@ -363,22 +363,22 @@ int sun4i_ss_ecb_aes_decrypt(struct ablkcipher_request *areq)
 }
 
 /* CBC DES */
-int sun4i_ss_cbc_des_encrypt(struct ablkcipher_request *areq)
+int sun4i_ss_cbc_des_encrypt(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
-	struct sun4i_cipher_req_ctx *rctx = ablkcipher_request_ctx(areq);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
+	struct sun4i_cipher_req_ctx *rctx = skcipher_request_ctx(areq);
 
 	rctx->mode = SS_OP_DES | SS_CBC | SS_ENABLED | SS_ENCRYPTION |
 		op->keymode;
 	return sun4i_ss_cipher_poll(areq);
 }
 
-int sun4i_ss_cbc_des_decrypt(struct ablkcipher_request *areq)
+int sun4i_ss_cbc_des_decrypt(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
-	struct sun4i_cipher_req_ctx *rctx = ablkcipher_request_ctx(areq);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
+	struct sun4i_cipher_req_ctx *rctx = skcipher_request_ctx(areq);
 
 	rctx->mode = SS_OP_DES | SS_CBC | SS_ENABLED | SS_DECRYPTION |
 		op->keymode;
@@ -386,22 +386,22 @@ int sun4i_ss_cbc_des_decrypt(struct ablkcipher_request *areq)
 }
 
 /* ECB DES */
-int sun4i_ss_ecb_des_encrypt(struct ablkcipher_request *areq)
+int sun4i_ss_ecb_des_encrypt(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
-	struct sun4i_cipher_req_ctx *rctx = ablkcipher_request_ctx(areq);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
+	struct sun4i_cipher_req_ctx *rctx = skcipher_request_ctx(areq);
 
 	rctx->mode = SS_OP_DES | SS_ECB | SS_ENABLED | SS_ENCRYPTION |
 		op->keymode;
 	return sun4i_ss_cipher_poll(areq);
 }
 
-int sun4i_ss_ecb_des_decrypt(struct ablkcipher_request *areq)
+int sun4i_ss_ecb_des_decrypt(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
-	struct sun4i_cipher_req_ctx *rctx = ablkcipher_request_ctx(areq);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
+	struct sun4i_cipher_req_ctx *rctx = skcipher_request_ctx(areq);
 
 	rctx->mode = SS_OP_DES | SS_ECB | SS_ENABLED | SS_DECRYPTION |
 		op->keymode;
@@ -409,22 +409,22 @@ int sun4i_ss_ecb_des_decrypt(struct ablkcipher_request *areq)
 }
 
 /* CBC 3DES */
-int sun4i_ss_cbc_des3_encrypt(struct ablkcipher_request *areq)
+int sun4i_ss_cbc_des3_encrypt(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
-	struct sun4i_cipher_req_ctx *rctx = ablkcipher_request_ctx(areq);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
+	struct sun4i_cipher_req_ctx *rctx = skcipher_request_ctx(areq);
 
 	rctx->mode = SS_OP_3DES | SS_CBC | SS_ENABLED | SS_ENCRYPTION |
 		op->keymode;
 	return sun4i_ss_cipher_poll(areq);
 }
 
-int sun4i_ss_cbc_des3_decrypt(struct ablkcipher_request *areq)
+int sun4i_ss_cbc_des3_decrypt(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
-	struct sun4i_cipher_req_ctx *rctx = ablkcipher_request_ctx(areq);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
+	struct sun4i_cipher_req_ctx *rctx = skcipher_request_ctx(areq);
 
 	rctx->mode = SS_OP_3DES | SS_CBC | SS_ENABLED | SS_DECRYPTION |
 		op->keymode;
@@ -432,22 +432,22 @@ int sun4i_ss_cbc_des3_decrypt(struct ablkcipher_request *areq)
 }
 
 /* ECB 3DES */
-int sun4i_ss_ecb_des3_encrypt(struct ablkcipher_request *areq)
+int sun4i_ss_ecb_des3_encrypt(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
-	struct sun4i_cipher_req_ctx *rctx = ablkcipher_request_ctx(areq);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
+	struct sun4i_cipher_req_ctx *rctx = skcipher_request_ctx(areq);
 
 	rctx->mode = SS_OP_3DES | SS_ECB | SS_ENABLED | SS_ENCRYPTION |
 		op->keymode;
 	return sun4i_ss_cipher_poll(areq);
 }
 
-int sun4i_ss_ecb_des3_decrypt(struct ablkcipher_request *areq)
+int sun4i_ss_ecb_des3_decrypt(struct skcipher_request *areq)
 {
-	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
-	struct sun4i_cipher_req_ctx *rctx = ablkcipher_request_ctx(areq);
+	struct crypto_skcipher *tfm = crypto_skcipher_reqtfm(areq);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
+	struct sun4i_cipher_req_ctx *rctx = skcipher_request_ctx(areq);
 
 	rctx->mode = SS_OP_3DES | SS_ECB | SS_ENABLED | SS_DECRYPTION |
 		op->keymode;
@@ -457,24 +457,25 @@ int sun4i_ss_ecb_des3_decrypt(struct ablkcipher_request *areq)
 int sun4i_ss_cipher_init(struct crypto_tfm *tfm)
 {
 	struct sun4i_tfm_ctx *op = crypto_tfm_ctx(tfm);
-	struct crypto_alg *alg = tfm->__crt_alg;
 	struct sun4i_ss_alg_template *algt;
 
 	memset(op, 0, sizeof(struct sun4i_tfm_ctx));
 
-	algt = container_of(alg, struct sun4i_ss_alg_template, alg.crypto);
+	algt = container_of(tfm->__crt_alg, struct sun4i_ss_alg_template,
+			    alg.crypto.base);
 	op->ss = algt->ss;
 
-	tfm->crt_ablkcipher.reqsize = sizeof(struct sun4i_cipher_req_ctx);
+	crypto_skcipher_set_reqsize(__crypto_skcipher_cast(tfm),
+				    sizeof(struct sun4i_cipher_req_ctx));
 
 	return 0;
 }
 
 /* check and set the AES key, prepare the mode to be used */
-int sun4i_ss_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+int sun4i_ss_aes_setkey(struct crypto_skcipher *tfm, const u8 *key,
 			unsigned int keylen)
 {
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
 	struct sun4i_ss_ctx *ss = op->ss;
 
 	switch (keylen) {
@@ -489,7 +490,7 @@ int sun4i_ss_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
 		break;
 	default:
 		dev_err(ss->dev, "ERROR: Invalid keylen %u\n", keylen);
-		crypto_ablkcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		crypto_skcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
 		return -EINVAL;
 	}
 	op->keylen = keylen;
@@ -498,10 +499,10 @@ int sun4i_ss_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
 }
 
 /* check and set the DES key, prepare the mode to be used */
-int sun4i_ss_des_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+int sun4i_ss_des_setkey(struct crypto_skcipher *tfm, const u8 *key,
 			unsigned int keylen)
 {
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
 	struct sun4i_ss_ctx *ss = op->ss;
 	u32 flags;
 	u32 tmp[DES_EXPKEY_WORDS];
@@ -509,15 +510,15 @@ int sun4i_ss_des_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
 
 	if (unlikely(keylen != DES_KEY_SIZE)) {
 		dev_err(ss->dev, "Invalid keylen %u\n", keylen);
-		crypto_ablkcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		crypto_skcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
 		return -EINVAL;
 	}
 
-	flags = crypto_ablkcipher_get_flags(tfm);
+	flags = crypto_skcipher_get_flags(tfm);
 
 	ret = des_ekey(tmp, key);
-	if (unlikely(ret == 0) && (flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
-		crypto_ablkcipher_set_flags(tfm, CRYPTO_TFM_RES_WEAK_KEY);
+	if (unlikely(!ret) && (flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		crypto_skcipher_set_flags(tfm, CRYPTO_TFM_RES_WEAK_KEY);
 		dev_dbg(ss->dev, "Weak key %u\n", keylen);
 		return -EINVAL;
 	}
@@ -528,15 +529,15 @@ int sun4i_ss_des_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
 }
 
 /* check and set the 3DES key, prepare the mode to be used */
-int sun4i_ss_des3_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+int sun4i_ss_des3_setkey(struct crypto_skcipher *tfm, const u8 *key,
 			 unsigned int keylen)
 {
-	struct sun4i_tfm_ctx *op = crypto_ablkcipher_ctx(tfm);
+	struct sun4i_tfm_ctx *op = crypto_skcipher_ctx(tfm);
 	struct sun4i_ss_ctx *ss = op->ss;
 
 	if (unlikely(keylen != 3 * DES_KEY_SIZE)) {
 		dev_err(ss->dev, "Invalid keylen %u\n", keylen);
-		crypto_ablkcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		crypto_skcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
 		return -EINVAL;
 	}
 	op->keylen = keylen;
diff --git a/drivers/crypto/sunxi-ss/sun4i-ss-core.c b/drivers/crypto/sunxi-ss/sun4i-ss-core.c
index 3ac6c6c4ad18..02ad8256e900 100644
--- a/drivers/crypto/sunxi-ss/sun4i-ss-core.c
+++ b/drivers/crypto/sunxi-ss/sun4i-ss-core.c
@@ -83,134 +83,133 @@ static struct sun4i_ss_alg_template ss_algs[] = {
 		}
 	}
 },
-{       .type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+{       .type = CRYPTO_ALG_TYPE_SKCIPHER,
 	.alg.crypto = {
-		.cra_name = "cbc(aes)",
-		.cra_driver_name = "cbc-aes-sun4i-ss",
-		.cra_priority = 300,
-		.cra_blocksize = AES_BLOCK_SIZE,
-		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER,
-		.cra_ctxsize = sizeof(struct sun4i_tfm_ctx),
-		.cra_module = THIS_MODULE,
-		.cra_alignmask = 3,
-		.cra_type = &crypto_ablkcipher_type,
-		.cra_init = sun4i_ss_cipher_init,
-		.cra_ablkcipher = {
-			.min_keysize	= AES_MIN_KEY_SIZE,
-			.max_keysize	= AES_MAX_KEY_SIZE,
-			.ivsize		= AES_BLOCK_SIZE,
-			.setkey         = sun4i_ss_aes_setkey,
-			.encrypt        = sun4i_ss_cbc_aes_encrypt,
-			.decrypt        = sun4i_ss_cbc_aes_decrypt,
+		.setkey         = sun4i_ss_aes_setkey,
+		.encrypt        = sun4i_ss_cbc_aes_encrypt,
+		.decrypt        = sun4i_ss_cbc_aes_decrypt,
+		.min_keysize	= AES_MIN_KEY_SIZE,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.ivsize		= AES_BLOCK_SIZE,
+		.base = {
+			.cra_name = "cbc(aes)",
+			.cra_driver_name = "cbc-aes-sun4i-ss",
+			.cra_priority = 300,
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_SKCIPHER |
+				     CRYPTO_ALG_KERN_DRIVER_ONLY,
+			.cra_ctxsize = sizeof(struct sun4i_tfm_ctx),
+			.cra_module = THIS_MODULE,
+			.cra_alignmask = 3,
+			.cra_init = sun4i_ss_cipher_init,
 		}
 	}
 },
-{       .type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+{       .type = CRYPTO_ALG_TYPE_SKCIPHER,
 	.alg.crypto = {
-		.cra_name = "ecb(aes)",
-		.cra_driver_name = "ecb-aes-sun4i-ss",
-		.cra_priority = 300,
-		.cra_blocksize = AES_BLOCK_SIZE,
-		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER,
-		.cra_ctxsize = sizeof(struct sun4i_tfm_ctx),
-		.cra_module = THIS_MODULE,
-		.cra_alignmask = 3,
-		.cra_type = &crypto_ablkcipher_type,
-		.cra_init = sun4i_ss_cipher_init,
-		.cra_ablkcipher = {
-			.min_keysize	= AES_MIN_KEY_SIZE,
-			.max_keysize	= AES_MAX_KEY_SIZE,
-			.ivsize		= AES_BLOCK_SIZE,
-			.setkey         = sun4i_ss_aes_setkey,
-			.encrypt        = sun4i_ss_ecb_aes_encrypt,
-			.decrypt        = sun4i_ss_ecb_aes_decrypt,
+		.setkey         = sun4i_ss_aes_setkey,
+		.encrypt        = sun4i_ss_ecb_aes_encrypt,
+		.decrypt        = sun4i_ss_ecb_aes_decrypt,
+		.min_keysize	= AES_MIN_KEY_SIZE,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.ivsize		= AES_BLOCK_SIZE,
+		.base = {
+			.cra_name = "ecb(aes)",
+			.cra_driver_name = "ecb-aes-sun4i-ss",
+			.cra_priority = 300,
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_SKCIPHER |
+				     CRYPTO_ALG_KERN_DRIVER_ONLY,
+			.cra_ctxsize = sizeof(struct sun4i_tfm_ctx),
+			.cra_module = THIS_MODULE,
+			.cra_alignmask = 3,
+			.cra_init = sun4i_ss_cipher_init,
 		}
 	}
 },
-{       .type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+{       .type = CRYPTO_ALG_TYPE_SKCIPHER,
 	.alg.crypto = {
-		.cra_name = "cbc(des)",
-		.cra_driver_name = "cbc-des-sun4i-ss",
-		.cra_priority = 300,
-		.cra_blocksize = DES_BLOCK_SIZE,
-		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER,
-		.cra_ctxsize = sizeof(struct sun4i_req_ctx),
-		.cra_module = THIS_MODULE,
-		.cra_alignmask = 3,
-		.cra_type = &crypto_ablkcipher_type,
-		.cra_init = sun4i_ss_cipher_init,
-		.cra_u.ablkcipher = {
-			.min_keysize    = DES_KEY_SIZE,
-			.max_keysize    = DES_KEY_SIZE,
-			.ivsize         = DES_BLOCK_SIZE,
-			.setkey         = sun4i_ss_des_setkey,
-			.encrypt        = sun4i_ss_cbc_des_encrypt,
-			.decrypt        = sun4i_ss_cbc_des_decrypt,
+		.setkey         = sun4i_ss_des_setkey,
+		.encrypt        = sun4i_ss_cbc_des_encrypt,
+		.decrypt        = sun4i_ss_cbc_des_decrypt,
+		.min_keysize    = DES_KEY_SIZE,
+		.max_keysize    = DES_KEY_SIZE,
+		.ivsize         = DES_BLOCK_SIZE,
+		.base = {
+			.cra_name = "cbc(des)",
+			.cra_driver_name = "cbc-des-sun4i-ss",
+			.cra_priority = 300,
+			.cra_blocksize = DES_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_SKCIPHER |
+				     CRYPTO_ALG_KERN_DRIVER_ONLY,
+			.cra_ctxsize = sizeof(struct sun4i_req_ctx),
+			.cra_module = THIS_MODULE,
+			.cra_alignmask = 3,
+			.cra_init = sun4i_ss_cipher_init,
 		}
 	}
 },
-{       .type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+{       .type = CRYPTO_ALG_TYPE_SKCIPHER,
 	.alg.crypto = {
-		.cra_name = "ecb(des)",
-		.cra_driver_name = "ecb-des-sun4i-ss",
-		.cra_priority = 300,
-		.cra_blocksize = DES_BLOCK_SIZE,
-		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER,
-		.cra_ctxsize = sizeof(struct sun4i_req_ctx),
-		.cra_module = THIS_MODULE,
-		.cra_alignmask = 3,
-		.cra_type = &crypto_ablkcipher_type,
-		.cra_init = sun4i_ss_cipher_init,
-		.cra_u.ablkcipher = {
-			.min_keysize    = DES_KEY_SIZE,
-			.max_keysize    = DES_KEY_SIZE,
-			.setkey         = sun4i_ss_des_setkey,
-			.encrypt        = sun4i_ss_ecb_des_encrypt,
-			.decrypt        = sun4i_ss_ecb_des_decrypt,
+		.setkey         = sun4i_ss_des_setkey,
+		.encrypt        = sun4i_ss_ecb_des_encrypt,
+		.decrypt        = sun4i_ss_ecb_des_decrypt,
+		.min_keysize    = DES_KEY_SIZE,
+		.max_keysize    = DES_KEY_SIZE,
+		.base = {
+			.cra_name = "ecb(des)",
+			.cra_driver_name = "ecb-des-sun4i-ss",
+			.cra_priority = 300,
+			.cra_blocksize = DES_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_SKCIPHER |
+				     CRYPTO_ALG_KERN_DRIVER_ONLY,
+			.cra_ctxsize = sizeof(struct sun4i_req_ctx),
+			.cra_module = THIS_MODULE,
+			.cra_alignmask = 3,
+			.cra_init = sun4i_ss_cipher_init,
 		}
 	}
 },
-{       .type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+{       .type = CRYPTO_ALG_TYPE_SKCIPHER,
 	.alg.crypto = {
-		.cra_name = "cbc(des3_ede)",
-		.cra_driver_name = "cbc-des3-sun4i-ss",
-		.cra_priority = 300,
-		.cra_blocksize = DES3_EDE_BLOCK_SIZE,
-		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER,
-		.cra_ctxsize = sizeof(struct sun4i_req_ctx),
-		.cra_module = THIS_MODULE,
-		.cra_alignmask = 3,
-		.cra_type = &crypto_ablkcipher_type,
-		.cra_init = sun4i_ss_cipher_init,
-		.cra_u.ablkcipher = {
-			.min_keysize    = DES3_EDE_KEY_SIZE,
-			.max_keysize    = DES3_EDE_KEY_SIZE,
-			.ivsize         = DES3_EDE_BLOCK_SIZE,
-			.setkey         = sun4i_ss_des3_setkey,
-			.encrypt        = sun4i_ss_cbc_des3_encrypt,
-			.decrypt        = sun4i_ss_cbc_des3_decrypt,
+		.setkey         = sun4i_ss_des3_setkey,
+		.encrypt        = sun4i_ss_cbc_des3_encrypt,
+		.decrypt        = sun4i_ss_cbc_des3_decrypt,
+		.min_keysize    = DES3_EDE_KEY_SIZE,
+		.max_keysize    = DES3_EDE_KEY_SIZE,
+		.ivsize         = DES3_EDE_BLOCK_SIZE,
+		.base = {
+			.cra_name = "cbc(des3_ede)",
+			.cra_driver_name = "cbc-des3-sun4i-ss",
+			.cra_priority = 300,
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_SKCIPHER |
+				     CRYPTO_ALG_KERN_DRIVER_ONLY,
+			.cra_ctxsize = sizeof(struct sun4i_req_ctx),
+			.cra_module = THIS_MODULE,
+			.cra_alignmask = 3,
+			.cra_init = sun4i_ss_cipher_init,
 		}
 	}
 },
-{       .type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+{       .type = CRYPTO_ALG_TYPE_SKCIPHER,
 	.alg.crypto = {
-		.cra_name = "ecb(des3_ede)",
-		.cra_driver_name = "ecb-des3-sun4i-ss",
-		.cra_priority = 300,
-		.cra_blocksize = DES3_EDE_BLOCK_SIZE,
-		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER,
-		.cra_ctxsize = sizeof(struct sun4i_req_ctx),
-		.cra_module = THIS_MODULE,
-		.cra_alignmask = 3,
-		.cra_type = &crypto_ablkcipher_type,
-		.cra_init = sun4i_ss_cipher_init,
-		.cra_u.ablkcipher = {
-			.min_keysize    = DES3_EDE_KEY_SIZE,
-			.max_keysize    = DES3_EDE_KEY_SIZE,
-			.ivsize         = DES3_EDE_BLOCK_SIZE,
-			.setkey         = sun4i_ss_des3_setkey,
-			.encrypt        = sun4i_ss_ecb_des3_encrypt,
-			.decrypt        = sun4i_ss_ecb_des3_decrypt,
+		.setkey         = sun4i_ss_des3_setkey,
+		.encrypt        = sun4i_ss_ecb_des3_encrypt,
+		.decrypt        = sun4i_ss_ecb_des3_decrypt,
+		.min_keysize    = DES3_EDE_KEY_SIZE,
+		.max_keysize    = DES3_EDE_KEY_SIZE,
+		.ivsize         = DES3_EDE_BLOCK_SIZE,
+		.base = {
+			.cra_name = "ecb(des3_ede)",
+			.cra_driver_name = "ecb-des3-sun4i-ss",
+			.cra_priority = 300,
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_SKCIPHER,
+			.cra_ctxsize = sizeof(struct sun4i_req_ctx),
+			.cra_module = THIS_MODULE,
+			.cra_alignmask = 3,
+			.cra_init = sun4i_ss_cipher_init,
 		}
 	}
 },
@@ -266,12 +265,12 @@ static int sun4i_ss_probe(struct platform_device *pdev)
 
 	/* Enable both clocks */
 	err = clk_prepare_enable(ss->busclk);
-	if (err != 0) {
+	if (err) {
 		dev_err(&pdev->dev, "Cannot prepare_enable busclk\n");
 		return err;
 	}
 	err = clk_prepare_enable(ss->ssclk);
-	if (err != 0) {
+	if (err) {
 		dev_err(&pdev->dev, "Cannot prepare_enable ssclk\n");
 		goto error_ssclk;
 	}
@@ -281,7 +280,7 @@ static int sun4i_ss_probe(struct platform_device *pdev)
 	 * Try to set the clock to the maximum allowed
 	 */
 	err = clk_set_rate(ss->ssclk, cr_mod);
-	if (err != 0) {
+	if (err) {
 		dev_err(&pdev->dev, "Cannot set clock rate to ssclk\n");
 		goto error_clk;
 	}
@@ -340,17 +339,17 @@ static int sun4i_ss_probe(struct platform_device *pdev)
 	for (i = 0; i < ARRAY_SIZE(ss_algs); i++) {
 		ss_algs[i].ss = ss;
 		switch (ss_algs[i].type) {
-		case CRYPTO_ALG_TYPE_ABLKCIPHER:
-			err = crypto_register_alg(&ss_algs[i].alg.crypto);
-			if (err != 0) {
+		case CRYPTO_ALG_TYPE_SKCIPHER:
+			err = crypto_register_skcipher(&ss_algs[i].alg.crypto);
+			if (err) {
 				dev_err(ss->dev, "Fail to register %s\n",
-					ss_algs[i].alg.crypto.cra_name);
+					ss_algs[i].alg.crypto.base.cra_name);
 				goto error_alg;
 			}
 			break;
 		case CRYPTO_ALG_TYPE_AHASH:
 			err = crypto_register_ahash(&ss_algs[i].alg.hash);
-			if (err != 0) {
+			if (err) {
 				dev_err(ss->dev, "Fail to register %s\n",
 					ss_algs[i].alg.hash.halg.base.cra_name);
 				goto error_alg;
@@ -364,8 +363,8 @@ error_alg:
 	i--;
 	for (; i >= 0; i--) {
 		switch (ss_algs[i].type) {
-		case CRYPTO_ALG_TYPE_ABLKCIPHER:
-			crypto_unregister_alg(&ss_algs[i].alg.crypto);
+		case CRYPTO_ALG_TYPE_SKCIPHER:
+			crypto_unregister_skcipher(&ss_algs[i].alg.crypto);
 			break;
 		case CRYPTO_ALG_TYPE_AHASH:
 			crypto_unregister_ahash(&ss_algs[i].alg.hash);
@@ -388,8 +387,8 @@ static int sun4i_ss_remove(struct platform_device *pdev)
 
 	for (i = 0; i < ARRAY_SIZE(ss_algs); i++) {
 		switch (ss_algs[i].type) {
-		case CRYPTO_ALG_TYPE_ABLKCIPHER:
-			crypto_unregister_alg(&ss_algs[i].alg.crypto);
+		case CRYPTO_ALG_TYPE_SKCIPHER:
+			crypto_unregister_skcipher(&ss_algs[i].alg.crypto);
 			break;
 		case CRYPTO_ALG_TYPE_AHASH:
 			crypto_unregister_ahash(&ss_algs[i].alg.hash);
diff --git a/drivers/crypto/sunxi-ss/sun4i-ss-hash.c b/drivers/crypto/sunxi-ss/sun4i-ss-hash.c
index 0de2f62d51ff..a4b5ff2b72f8 100644
--- a/drivers/crypto/sunxi-ss/sun4i-ss-hash.c
+++ b/drivers/crypto/sunxi-ss/sun4i-ss-hash.c
@@ -60,7 +60,7 @@ int sun4i_hash_export_md5(struct ahash_request *areq, void *out)
 
 	memcpy(octx->block, op->buf, op->len);
 
-	if (op->byte_count > 0) {
+	if (op->byte_count) {
 		for (i = 0; i < 4; i++)
 			octx->hash[i] = op->hash[i];
 	} else {
@@ -102,7 +102,7 @@ int sun4i_hash_export_sha1(struct ahash_request *areq, void *out)
 
 	memcpy(octx->buffer, op->buf, op->len);
 
-	if (op->byte_count > 0) {
+	if (op->byte_count) {
 		for (i = 0; i < 5; i++)
 			octx->state[i] = op->hash[i];
 	} else {
@@ -167,44 +167,34 @@ int sun4i_hash_import_sha1(struct ahash_request *areq, const void *in)
  */
 static int sun4i_hash(struct ahash_request *areq)
 {
-	u32 v, ivmode = 0;
-	unsigned int i = 0;
 	/*
 	 * i is the total bytes read from SGs, to be compared to areq->nbytes
 	 * i is important because we cannot rely on SG length since the sum of
 	 * SG->length could be greater than areq->nbytes
+	 *
+	 * end is the position when we need to stop writing to the device,
+	 * to be compared to i
+	 *
+	 * in_i: advancement in the current SG
 	 */
-
+	unsigned int i = 0, end, fill, min_fill, nwait, nbw = 0, j = 0, todo;
+	unsigned int in_i = 0;
+	u32 spaces, rx_cnt = SS_RX_DEFAULT, bf[32] = {0}, wb = 0, v, ivmode = 0;
 	struct sun4i_req_ctx *op = ahash_request_ctx(areq);
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(areq);
 	struct sun4i_tfm_ctx *tfmctx = crypto_ahash_ctx(tfm);
 	struct sun4i_ss_ctx *ss = tfmctx->ss;
-	unsigned int in_i = 0; /* advancement in the current SG */
-	unsigned int end;
-	/*
-	 * end is the position when we need to stop writing to the device,
-	 * to be compared to i
-	 */
+	struct scatterlist *in_sg = areq->src;
+	struct sg_mapping_iter mi;
 	int in_r, err = 0;
-	unsigned int todo;
-	u32 spaces, rx_cnt = SS_RX_DEFAULT;
 	size_t copied = 0;
-	struct sg_mapping_iter mi;
-	unsigned int j = 0;
-	int zeros;
-	unsigned int index, padlen;
-	__be64 bits;
-	u32 bf[32];
-	u32 wb = 0;
-	unsigned int nwait, nbw = 0;
-	struct scatterlist *in_sg = areq->src;
 
 	dev_dbg(ss->dev, "%s %s bc=%llu len=%u mode=%x wl=%u h0=%0x",
 		__func__, crypto_tfm_alg_name(areq->base.tfm),
 		op->byte_count, areq->nbytes, op->mode,
 		op->len, op->hash[0]);
 
-	if (unlikely(areq->nbytes == 0) && (op->flags & SS_HASH_FINAL) == 0)
+	if (unlikely(!areq->nbytes) && !(op->flags & SS_HASH_FINAL))
 		return 0;
 
 	/* protect against overflow */
@@ -213,7 +203,7 @@ static int sun4i_hash(struct ahash_request *areq)
 		return -EINVAL;
 	}
 
-	if (op->len + areq->nbytes < 64 && (op->flags & SS_HASH_FINAL) == 0) {
+	if (op->len + areq->nbytes < 64 && !(op->flags & SS_HASH_FINAL)) {
 		/* linearize data to op->buf */
 		copied = sg_pcopy_to_buffer(areq->src, sg_nents(areq->src),
 					    op->buf + op->len, areq->nbytes, 0);
@@ -227,7 +217,7 @@ static int sun4i_hash(struct ahash_request *areq)
 	 * if some data have been processed before,
 	 * we need to restore the partial hash state
 	 */
-	if (op->byte_count > 0) {
+	if (op->byte_count) {
 		ivmode = SS_IV_ARBITRARY;
 		for (i = 0; i < 5; i++)
 			writel(op->hash[i], ss->base + SS_IV0 + i * 4);
@@ -235,11 +225,11 @@ static int sun4i_hash(struct ahash_request *areq)
 	/* Enable the device */
 	writel(op->mode | SS_ENABLED | ivmode, ss->base + SS_CTL);
 
-	if ((op->flags & SS_HASH_UPDATE) == 0)
+	if (!(op->flags & SS_HASH_UPDATE))
 		goto hash_final;
 
 	/* start of handling data */
-	if ((op->flags & SS_HASH_FINAL) == 0) {
+	if (!(op->flags & SS_HASH_FINAL)) {
 		end = ((areq->nbytes + op->len) / 64) * 64 - op->len;
 
 		if (end > areq->nbytes || areq->nbytes - end > 63) {
@@ -253,14 +243,14 @@ static int sun4i_hash(struct ahash_request *areq)
 		end = ((areq->nbytes + op->len) / 4) * 4 - op->len;
 	}
 
-	/* TODO if SGlen % 4 and op->len == 0 then DMA */
+	/* TODO if SGlen % 4 and !op->len then DMA */
 	i = 1;
 	while (in_sg && i == 1) {
-		if ((in_sg->length % 4) != 0)
+		if (in_sg->length % 4)
 			i = 0;
 		in_sg = sg_next(in_sg);
 	}
-	if (i == 1 && op->len == 0)
+	if (i == 1 && !op->len && areq->nbytes)
 		dev_dbg(ss->dev, "We can DMA\n");
 
 	i = 0;
@@ -275,7 +265,7 @@ static int sun4i_hash(struct ahash_request *areq)
 		 * - the buffer is already used
 		 * - the SG does not have enough byte remaining ( < 4)
 		 */
-		if (op->len > 0 || (mi.length - in_i) < 4) {
+		if (op->len || (mi.length - in_i) < 4) {
 			/*
 			 * if we have entered here we have two reason to stop
 			 * - the buffer is full
@@ -294,7 +284,7 @@ static int sun4i_hash(struct ahash_request *areq)
 					in_i = 0;
 				}
 			}
-			if (op->len > 3 && (op->len % 4) == 0) {
+			if (op->len > 3 && !(op->len % 4)) {
 				/* write buf to the device */
 				writesl(ss->base + SS_RXFIFO, op->buf,
 					op->len / 4);
@@ -313,7 +303,7 @@ static int sun4i_hash(struct ahash_request *areq)
 			i += todo * 4;
 			in_i += todo * 4;
 			rx_cnt -= todo;
-			if (rx_cnt == 0) {
+			if (!rx_cnt) {
 				spaces = readl(ss->base + SS_FCSR);
 				rx_cnt = SS_RXFIFO_SPACES(spaces);
 			}
@@ -351,7 +341,7 @@ static int sun4i_hash(struct ahash_request *areq)
 	 * Now if we have the flag final go to finalize part
 	 * If not, store the partial hash
 	 */
-	if ((op->flags & SS_HASH_FINAL) > 0)
+	if (op->flags & SS_HASH_FINAL)
 		goto hash_final;
 
 	writel(op->mode | SS_ENABLED | SS_DATA_END, ss->base + SS_CTL);
@@ -359,7 +349,7 @@ static int sun4i_hash(struct ahash_request *areq)
 	do {
 		v = readl(ss->base + SS_CTL);
 		i++;
-	} while (i < SS_TIMEOUT && (v & SS_DATA_END) > 0);
+	} while (i < SS_TIMEOUT && (v & SS_DATA_END));
 	if (unlikely(i >= SS_TIMEOUT)) {
 		dev_err_ratelimited(ss->dev,
 				    "ERROR: hash end timeout %d>%d ctl=%x len=%u\n",
@@ -368,6 +358,15 @@ static int sun4i_hash(struct ahash_request *areq)
 		goto release_ss;
 	}
 
+	/*
+	 * The datasheet isn't very clear about when to retrieve the digest. The
+	 * bit SS_DATA_END is cleared when the engine has processed the data and
+	 * when the digest is computed *but* it doesn't mean the digest is
+	 * available in the digest registers. Hence the delay to be sure we can
+	 * read it.
+	 */
+	ndelay(1);
+
 	for (i = 0; i < crypto_ahash_digestsize(tfm) / 4; i++)
 		op->hash[i] = readl(ss->base + SS_MD0 + i * 4);
 
@@ -388,56 +387,50 @@ static int sun4i_hash(struct ahash_request *areq)
 hash_final:
 
 	/* write the remaining words of the wait buffer */
-	if (op->len > 0) {
+	if (op->len) {
 		nwait = op->len / 4;
-		if (nwait > 0) {
+		if (nwait) {
 			writesl(ss->base + SS_RXFIFO, op->buf, nwait);
 			op->byte_count += 4 * nwait;
 		}
+
 		nbw = op->len - 4 * nwait;
-		wb = *(u32 *)(op->buf + nwait * 4);
-		wb &= (0xFFFFFFFF >> (4 - nbw) * 8);
+		if (nbw) {
+			wb = *(u32 *)(op->buf + nwait * 4);
+			wb &= GENMASK((nbw * 8) - 1, 0);
+
+			op->byte_count += nbw;
+		}
 	}
 
 	/* write the remaining bytes of the nbw buffer */
-	if (nbw > 0) {
-		wb |= ((1 << 7) << (nbw * 8));
-		bf[j++] = wb;
-	} else {
-		bf[j++] = 1 << 7;
-	}
+	wb |= ((1 << 7) << (nbw * 8));
+	bf[j++] = wb;
 
 	/*
 	 * number of space to pad to obtain 64o minus 8(size) minus 4 (final 1)
 	 * I take the operations from other MD5/SHA1 implementations
 	 */
 
-	/* we have already send 4 more byte of which nbw data */
-	if (op->mode == SS_OP_MD5) {
-		index = (op->byte_count + 4) & 0x3f;
-		op->byte_count += nbw;
-		if (index > 56)
-			zeros = (120 - index) / 4;
-		else
-			zeros = (56 - index) / 4;
-	} else {
-		op->byte_count += nbw;
-		index = op->byte_count & 0x3f;
-		padlen = (index < 56) ? (56 - index) : ((64 + 56) - index);
-		zeros = (padlen - 1) / 4;
-	}
+	/* last block size */
+	fill = 64 - (op->byte_count % 64);
+	min_fill = 2 * sizeof(u32) + (nbw ? 0 : sizeof(u32));
+
+	/* if we can't fill all data, jump to the next 64 block */
+	if (fill < min_fill)
+		fill += 64;
 
-	memset(bf + j, 0, 4 * zeros);
-	j += zeros;
+	j += (fill - min_fill) / sizeof(u32);
 
 	/* write the length of data */
 	if (op->mode == SS_OP_SHA1) {
-		bits = cpu_to_be64(op->byte_count << 3);
-		bf[j++] = bits & 0xffffffff;
-		bf[j++] = (bits >> 32) & 0xffffffff;
+		__be64 bits = cpu_to_be64(op->byte_count << 3);
+		bf[j++] = lower_32_bits(bits);
+		bf[j++] = upper_32_bits(bits);
 	} else {
-		bf[j++] = (op->byte_count << 3) & 0xffffffff;
-		bf[j++] = (op->byte_count >> 29) & 0xffffffff;
+		__le64 bits = op->byte_count << 3;
+		bf[j++] = lower_32_bits(bits);
+		bf[j++] = upper_32_bits(bits);
 	}
 	writesl(ss->base + SS_RXFIFO, bf, j);
 
@@ -453,7 +446,7 @@ hash_final:
 	do {
 		v = readl(ss->base + SS_CTL);
 		i++;
-	} while (i < SS_TIMEOUT && (v & SS_DATA_END) > 0);
+	} while (i < SS_TIMEOUT && (v & SS_DATA_END));
 	if (unlikely(i >= SS_TIMEOUT)) {
 		dev_err_ratelimited(ss->dev,
 				    "ERROR: hash end timeout %d>%d ctl=%x len=%u\n",
@@ -462,6 +455,15 @@ hash_final:
 		goto release_ss;
 	}
 
+	/*
+	 * The datasheet isn't very clear about when to retrieve the digest. The
+	 * bit SS_DATA_END is cleared when the engine has processed the data and
+	 * when the digest is computed *but* it doesn't mean the digest is
+	 * available in the digest registers. Hence the delay to be sure we can
+	 * read it.
+	 */
+	ndelay(1);
+
 	/* Get the hash from the device */
 	if (op->mode == SS_OP_SHA1) {
 		for (i = 0; i < 5; i++) {
@@ -513,7 +515,7 @@ int sun4i_hash_digest(struct ahash_request *areq)
 	struct sun4i_req_ctx *op = ahash_request_ctx(areq);
 
 	err = sun4i_hash_init(areq);
-	if (err != 0)
+	if (err)
 		return err;
 
 	op->flags = SS_HASH_UPDATE | SS_HASH_FINAL;
diff --git a/drivers/crypto/sunxi-ss/sun4i-ss.h b/drivers/crypto/sunxi-ss/sun4i-ss.h
index f04c0f8cf026..a0e1efc1cb2a 100644
--- a/drivers/crypto/sunxi-ss/sun4i-ss.h
+++ b/drivers/crypto/sunxi-ss/sun4i-ss.h
@@ -24,9 +24,11 @@
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <crypto/md5.h>
+#include <crypto/skcipher.h>
 #include <crypto/sha.h>
 #include <crypto/hash.h>
 #include <crypto/internal/hash.h>
+#include <crypto/internal/skcipher.h>
 #include <crypto/aes.h>
 #include <crypto/des.h>
 #include <crypto/internal/rng.h>
@@ -140,7 +142,7 @@ struct sun4i_ss_alg_template {
 	u32 type;
 	u32 mode;
 	union {
-		struct crypto_alg crypto;
+		struct skcipher_alg crypto;
 		struct ahash_alg hash;
 	} alg;
 	struct sun4i_ss_ctx *ss;
@@ -177,25 +179,25 @@ int sun4i_hash_import_md5(struct ahash_request *areq, const void *in);
 int sun4i_hash_export_sha1(struct ahash_request *areq, void *out);
 int sun4i_hash_import_sha1(struct ahash_request *areq, const void *in);
 
-int sun4i_ss_cbc_aes_encrypt(struct ablkcipher_request *areq);
-int sun4i_ss_cbc_aes_decrypt(struct ablkcipher_request *areq);
-int sun4i_ss_ecb_aes_encrypt(struct ablkcipher_request *areq);
-int sun4i_ss_ecb_aes_decrypt(struct ablkcipher_request *areq);
+int sun4i_ss_cbc_aes_encrypt(struct skcipher_request *areq);
+int sun4i_ss_cbc_aes_decrypt(struct skcipher_request *areq);
+int sun4i_ss_ecb_aes_encrypt(struct skcipher_request *areq);
+int sun4i_ss_ecb_aes_decrypt(struct skcipher_request *areq);
 
-int sun4i_ss_cbc_des_encrypt(struct ablkcipher_request *areq);
-int sun4i_ss_cbc_des_decrypt(struct ablkcipher_request *areq);
-int sun4i_ss_ecb_des_encrypt(struct ablkcipher_request *areq);
-int sun4i_ss_ecb_des_decrypt(struct ablkcipher_request *areq);
+int sun4i_ss_cbc_des_encrypt(struct skcipher_request *areq);
+int sun4i_ss_cbc_des_decrypt(struct skcipher_request *areq);
+int sun4i_ss_ecb_des_encrypt(struct skcipher_request *areq);
+int sun4i_ss_ecb_des_decrypt(struct skcipher_request *areq);
 
-int sun4i_ss_cbc_des3_encrypt(struct ablkcipher_request *areq);
-int sun4i_ss_cbc_des3_decrypt(struct ablkcipher_request *areq);
-int sun4i_ss_ecb_des3_encrypt(struct ablkcipher_request *areq);
-int sun4i_ss_ecb_des3_decrypt(struct ablkcipher_request *areq);
+int sun4i_ss_cbc_des3_encrypt(struct skcipher_request *areq);
+int sun4i_ss_cbc_des3_decrypt(struct skcipher_request *areq);
+int sun4i_ss_ecb_des3_encrypt(struct skcipher_request *areq);
+int sun4i_ss_ecb_des3_decrypt(struct skcipher_request *areq);
 
 int sun4i_ss_cipher_init(struct crypto_tfm *tfm);
-int sun4i_ss_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+int sun4i_ss_aes_setkey(struct crypto_skcipher *tfm, const u8 *key,
 			unsigned int keylen);
-int sun4i_ss_des_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+int sun4i_ss_des_setkey(struct crypto_skcipher *tfm, const u8 *key,
 			unsigned int keylen);
-int sun4i_ss_des3_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+int sun4i_ss_des3_setkey(struct crypto_skcipher *tfm, const u8 *key,
 			 unsigned int keylen);
diff --git a/drivers/crypto/talitos.c b/drivers/crypto/talitos.c
index 0bba6a19d36a..79791c690858 100644
--- a/drivers/crypto/talitos.c
+++ b/drivers/crypto/talitos.c
@@ -816,7 +816,7 @@ static void talitos_unregister_rng(struct device *dev)
  * HMAC_SNOOP_NO_AFEA (HSNA) instead of type IPSEC_ESP
  */
 #define TALITOS_CRA_PRIORITY_AEAD_HSNA	(TALITOS_CRA_PRIORITY - 1)
-#define TALITOS_MAX_KEY_SIZE		96
+#define TALITOS_MAX_KEY_SIZE		(AES_MAX_KEY_SIZE + SHA512_BLOCK_SIZE)
 #define TALITOS_MAX_IV_LENGTH		16 /* max of AES_BLOCK_SIZE, DES3_EDE_BLOCK_SIZE */
 
 struct talitos_ctx {
@@ -1495,6 +1495,11 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *cipher,
 {
 	struct talitos_ctx *ctx = crypto_ablkcipher_ctx(cipher);
 
+	if (keylen > TALITOS_MAX_KEY_SIZE) {
+		crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
 	memcpy(&ctx->key, key, keylen);
 	ctx->keylen = keylen;
 
diff --git a/drivers/crypto/vmx/aes.c b/drivers/crypto/vmx/aes.c
index 022c7ab7351a..96072b9b55c4 100644
--- a/drivers/crypto/vmx/aes.c
+++ b/drivers/crypto/vmx/aes.c
@@ -37,15 +37,10 @@ struct p8_aes_ctx {
 
 static int p8_aes_init(struct crypto_tfm *tfm)
 {
-	const char *alg;
+	const char *alg = crypto_tfm_alg_name(tfm);
 	struct crypto_cipher *fallback;
 	struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
 
-	if (!(alg = crypto_tfm_alg_name(tfm))) {
-		printk(KERN_ERR "Failed to get algorithm name.\n");
-		return -ENOENT;
-	}
-
 	fallback = crypto_alloc_cipher(alg, 0, CRYPTO_ALG_NEED_FALLBACK);
 	if (IS_ERR(fallback)) {
 		printk(KERN_ERR
diff --git a/drivers/crypto/vmx/aes_cbc.c b/drivers/crypto/vmx/aes_cbc.c
index 72a26eb4e954..7394d35d5936 100644
--- a/drivers/crypto/vmx/aes_cbc.c
+++ b/drivers/crypto/vmx/aes_cbc.c
@@ -39,15 +39,10 @@ struct p8_aes_cbc_ctx {
 
 static int p8_aes_cbc_init(struct crypto_tfm *tfm)
 {
-	const char *alg;
+	const char *alg = crypto_tfm_alg_name(tfm);
 	struct crypto_skcipher *fallback;
 	struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(tfm);
 
-	if (!(alg = crypto_tfm_alg_name(tfm))) {
-		printk(KERN_ERR "Failed to get algorithm name.\n");
-		return -ENOENT;
-	}
-
 	fallback = crypto_alloc_skcipher(alg, 0,
 			CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK);
 
diff --git a/drivers/crypto/vmx/aes_ctr.c b/drivers/crypto/vmx/aes_ctr.c
index 7cf6d31c1123..9c26d9e8dbea 100644
--- a/drivers/crypto/vmx/aes_ctr.c
+++ b/drivers/crypto/vmx/aes_ctr.c
@@ -36,15 +36,10 @@ struct p8_aes_ctr_ctx {
 
 static int p8_aes_ctr_init(struct crypto_tfm *tfm)
 {
-	const char *alg;
+	const char *alg = crypto_tfm_alg_name(tfm);
 	struct crypto_blkcipher *fallback;
 	struct p8_aes_ctr_ctx *ctx = crypto_tfm_ctx(tfm);
 
-	if (!(alg = crypto_tfm_alg_name(tfm))) {
-		printk(KERN_ERR "Failed to get algorithm name.\n");
-		return -ENOENT;
-	}
-
 	fallback =
 	    crypto_alloc_blkcipher(alg, 0, CRYPTO_ALG_NEED_FALLBACK);
 	if (IS_ERR(fallback)) {
diff --git a/drivers/crypto/vmx/aes_xts.c b/drivers/crypto/vmx/aes_xts.c
index 6adc9290557a..8cd6e62e4c90 100644
--- a/drivers/crypto/vmx/aes_xts.c
+++ b/drivers/crypto/vmx/aes_xts.c
@@ -41,15 +41,10 @@ struct p8_aes_xts_ctx {
 
 static int p8_aes_xts_init(struct crypto_tfm *tfm)
 {
-	const char *alg;
+	const char *alg = crypto_tfm_alg_name(tfm);
 	struct crypto_skcipher *fallback;
 	struct p8_aes_xts_ctx *ctx = crypto_tfm_ctx(tfm);
 
-	if (!(alg = crypto_tfm_alg_name(tfm))) {
-		printk(KERN_ERR "Failed to get algorithm name.\n");
-		return -ENOENT;
-	}
-
 	fallback = crypto_alloc_skcipher(alg, 0,
 			CRYPTO_ALG_ASYNC | CRYPTO_ALG_NEED_FALLBACK);
 	if (IS_ERR(fallback)) {
diff --git a/drivers/devfreq/event/exynos-nocp.c b/drivers/devfreq/event/exynos-nocp.c
index 5c3e7b11e8a6..f6e7956fc91a 100644
--- a/drivers/devfreq/event/exynos-nocp.c
+++ b/drivers/devfreq/event/exynos-nocp.c
@@ -267,7 +267,11 @@ static int exynos_nocp_probe(struct platform_device *pdev)
 	}
 	platform_set_drvdata(pdev, nocp);
 
-	clk_prepare_enable(nocp->clk);
+	ret = clk_prepare_enable(nocp->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to prepare ppmu clock\n");
+		return ret;
+	}
 
 	pr_info("exynos-nocp: new NoC Probe device registered: %s\n",
 			dev_name(dev));
diff --git a/drivers/devfreq/event/exynos-ppmu.c b/drivers/devfreq/event/exynos-ppmu.c
index 9b7350935b73..d96e3dc71cf8 100644
--- a/drivers/devfreq/event/exynos-ppmu.c
+++ b/drivers/devfreq/event/exynos-ppmu.c
@@ -44,7 +44,7 @@ struct exynos_ppmu {
 	{ "ppmu-event2-"#name, PPMU_PMNCNT2 },	\
 	{ "ppmu-event3-"#name, PPMU_PMNCNT3 }
 
-struct __exynos_ppmu_events {
+static struct __exynos_ppmu_events {
 	char *name;
 	int id;
 } ppmu_events[] = {
@@ -648,7 +648,11 @@ static int exynos_ppmu_probe(struct platform_device *pdev)
 			dev_name(&pdev->dev), desc[i].name);
 	}
 
-	clk_prepare_enable(info->ppmu.clk);
+	ret = clk_prepare_enable(info->ppmu.clk);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to prepare ppmu clock\n");
+		return ret;
+	}
 
 	return 0;
 }
diff --git a/drivers/dma/dw/Kconfig b/drivers/dma/dw/Kconfig
index 5a37b9fcf40d..04b9728c1d26 100644
--- a/drivers/dma/dw/Kconfig
+++ b/drivers/dma/dw/Kconfig
@@ -6,17 +6,12 @@ config DW_DMAC_CORE
 	tristate
 	select DMA_ENGINE
 
-config DW_DMAC_BIG_ENDIAN_IO
-	bool
-
 config DW_DMAC
 	tristate "Synopsys DesignWare AHB DMA platform driver"
 	select DW_DMAC_CORE
-	select DW_DMAC_BIG_ENDIAN_IO if AVR32
-	default y if CPU_AT32AP7000
 	help
 	  Support the Synopsys DesignWare AHB DMA controller. This
-	  can be integrated in chips such as the Atmel AT32ap7000.
+	  can be integrated in chips such as the Intel Cherrytrail.
 
 config DW_DMAC_PCI
 	tristate "Synopsys DesignWare AHB DMA PCI driver"
diff --git a/drivers/dma/dw/core.c b/drivers/dma/dw/core.c
index e500950dad82..f43e6dafe446 100644
--- a/drivers/dma/dw/core.c
+++ b/drivers/dma/dw/core.c
@@ -561,92 +561,14 @@ static void dwc_handle_error(struct dw_dma *dw, struct dw_dma_chan *dwc)
 	dwc_descriptor_complete(dwc, bad_desc, true);
 }
 
-/* --------------------- Cyclic DMA API extensions -------------------- */
-
-dma_addr_t dw_dma_get_src_addr(struct dma_chan *chan)
-{
-	struct dw_dma_chan *dwc = to_dw_dma_chan(chan);
-	return channel_readl(dwc, SAR);
-}
-EXPORT_SYMBOL(dw_dma_get_src_addr);
-
-dma_addr_t dw_dma_get_dst_addr(struct dma_chan *chan)
-{
-	struct dw_dma_chan *dwc = to_dw_dma_chan(chan);
-	return channel_readl(dwc, DAR);
-}
-EXPORT_SYMBOL(dw_dma_get_dst_addr);
-
-/* Called with dwc->lock held and all DMAC interrupts disabled */
-static void dwc_handle_cyclic(struct dw_dma *dw, struct dw_dma_chan *dwc,
-		u32 status_block, u32 status_err, u32 status_xfer)
-{
-	unsigned long flags;
-
-	if (status_block & dwc->mask) {
-		void (*callback)(void *param);
-		void *callback_param;
-
-		dev_vdbg(chan2dev(&dwc->chan), "new cyclic period llp 0x%08x\n",
-				channel_readl(dwc, LLP));
-		dma_writel(dw, CLEAR.BLOCK, dwc->mask);
-
-		callback = dwc->cdesc->period_callback;
-		callback_param = dwc->cdesc->period_callback_param;
-
-		if (callback)
-			callback(callback_param);
-	}
-
-	/*
-	 * Error and transfer complete are highly unlikely, and will most
-	 * likely be due to a configuration error by the user.
-	 */
-	if (unlikely(status_err & dwc->mask) ||
-			unlikely(status_xfer & dwc->mask)) {
-		unsigned int i;
-
-		dev_err(chan2dev(&dwc->chan),
-			"cyclic DMA unexpected %s interrupt, stopping DMA transfer\n",
-			status_xfer ? "xfer" : "error");
-
-		spin_lock_irqsave(&dwc->lock, flags);
-
-		dwc_dump_chan_regs(dwc);
-
-		dwc_chan_disable(dw, dwc);
-
-		/* Make sure DMA does not restart by loading a new list */
-		channel_writel(dwc, LLP, 0);
-		channel_writel(dwc, CTL_LO, 0);
-		channel_writel(dwc, CTL_HI, 0);
-
-		dma_writel(dw, CLEAR.BLOCK, dwc->mask);
-		dma_writel(dw, CLEAR.ERROR, dwc->mask);
-		dma_writel(dw, CLEAR.XFER, dwc->mask);
-
-		for (i = 0; i < dwc->cdesc->periods; i++)
-			dwc_dump_lli(dwc, dwc->cdesc->desc[i]);
-
-		spin_unlock_irqrestore(&dwc->lock, flags);
-	}
-
-	/* Re-enable interrupts */
-	channel_set_bit(dw, MASK.BLOCK, dwc->mask);
-}
-
-/* ------------------------------------------------------------------------- */
-
 static void dw_dma_tasklet(unsigned long data)
 {
 	struct dw_dma *dw = (struct dw_dma *)data;
 	struct dw_dma_chan *dwc;
-	u32 status_block;
 	u32 status_xfer;
 	u32 status_err;
 	unsigned int i;
 
-	status_block = dma_readl(dw, RAW.BLOCK);
 	status_xfer = dma_readl(dw, RAW.XFER);
 	status_err = dma_readl(dw, RAW.ERROR);
 
@@ -655,8 +577,7 @@ static void dw_dma_tasklet(unsigned long data)
 	for (i = 0; i < dw->dma.chancnt; i++) {
 		dwc = &dw->chan[i];
 		if (test_bit(DW_DMA_IS_CYCLIC, &dwc->flags))
-			dwc_handle_cyclic(dw, dwc, status_block, status_err,
-					status_xfer);
+			dev_vdbg(dw->dma.dev, "Cyclic xfer is not implemented\n");
 		else if (status_err & (1 << i))
 			dwc_handle_error(dw, dwc);
 		else if (status_xfer & (1 << i))
@@ -1264,255 +1185,6 @@ static void dwc_free_chan_resources(struct dma_chan *chan)
 	dev_vdbg(chan2dev(chan), "%s: done\n", __func__);
 }
 
-/* --------------------- Cyclic DMA API extensions -------------------- */
-
-/**
- * dw_dma_cyclic_start - start the cyclic DMA transfer
- * @chan: the DMA channel to start
- *
- * Must be called with soft interrupts disabled. Returns zero on success or
- * -errno on failure.
- */
-int dw_dma_cyclic_start(struct dma_chan *chan)
-{
-	struct dw_dma_chan	*dwc = to_dw_dma_chan(chan);
-	struct dw_dma		*dw = to_dw_dma(chan->device);
-	unsigned long		flags;
-
-	if (!test_bit(DW_DMA_IS_CYCLIC, &dwc->flags)) {
-		dev_err(chan2dev(&dwc->chan), "missing prep for cyclic DMA\n");
-		return -ENODEV;
-	}
-
-	spin_lock_irqsave(&dwc->lock, flags);
-
-	/* Enable interrupts to perform cyclic transfer */
-	channel_set_bit(dw, MASK.BLOCK, dwc->mask);
-
-	dwc_dostart(dwc, dwc->cdesc->desc[0]);
-
-	spin_unlock_irqrestore(&dwc->lock, flags);
-
-	return 0;
-}
-EXPORT_SYMBOL(dw_dma_cyclic_start);
-
-/**
- * dw_dma_cyclic_stop - stop the cyclic DMA transfer
- * @chan: the DMA channel to stop
- *
- * Must be called with soft interrupts disabled.
- */
-void dw_dma_cyclic_stop(struct dma_chan *chan)
-{
-	struct dw_dma_chan	*dwc = to_dw_dma_chan(chan);
-	struct dw_dma		*dw = to_dw_dma(dwc->chan.device);
-	unsigned long		flags;
-
-	spin_lock_irqsave(&dwc->lock, flags);
-
-	dwc_chan_disable(dw, dwc);
-
-	spin_unlock_irqrestore(&dwc->lock, flags);
-}
-EXPORT_SYMBOL(dw_dma_cyclic_stop);
-
-/**
- * dw_dma_cyclic_prep - prepare the cyclic DMA transfer
- * @chan: the DMA channel to prepare
- * @buf_addr: physical DMA address where the buffer starts
- * @buf_len: total number of bytes for the entire buffer
- * @period_len: number of bytes for each period
- * @direction: transfer direction, to or from device
- *
- * Must be called before trying to start the transfer. Returns a valid struct
- * dw_cyclic_desc if successful or an ERR_PTR(-errno) if not successful.
- */
-struct dw_cyclic_desc *dw_dma_cyclic_prep(struct dma_chan *chan,
-		dma_addr_t buf_addr, size_t buf_len, size_t period_len,
-		enum dma_transfer_direction direction)
-{
-	struct dw_dma_chan		*dwc = to_dw_dma_chan(chan);
-	struct dma_slave_config		*sconfig = &dwc->dma_sconfig;
-	struct dw_cyclic_desc		*cdesc;
-	struct dw_cyclic_desc		*retval = NULL;
-	struct dw_desc			*desc;
-	struct dw_desc			*last = NULL;
-	u8				lms = DWC_LLP_LMS(dwc->dws.m_master);
-	unsigned long			was_cyclic;
-	unsigned int			reg_width;
-	unsigned int			periods;
-	unsigned int			i;
-	unsigned long			flags;
-
-	spin_lock_irqsave(&dwc->lock, flags);
-	if (dwc->nollp) {
-		spin_unlock_irqrestore(&dwc->lock, flags);
-		dev_dbg(chan2dev(&dwc->chan),
-				"channel doesn't support LLP transfers\n");
-		return ERR_PTR(-EINVAL);
-	}
-
-	if (!list_empty(&dwc->queue) || !list_empty(&dwc->active_list)) {
-		spin_unlock_irqrestore(&dwc->lock, flags);
-		dev_dbg(chan2dev(&dwc->chan),
-				"queue and/or active list are not empty\n");
-		return ERR_PTR(-EBUSY);
-	}
-
-	was_cyclic = test_and_set_bit(DW_DMA_IS_CYCLIC, &dwc->flags);
-	spin_unlock_irqrestore(&dwc->lock, flags);
-	if (was_cyclic) {
-		dev_dbg(chan2dev(&dwc->chan),
-				"channel already prepared for cyclic DMA\n");
-		return ERR_PTR(-EBUSY);
-	}
-
-	retval = ERR_PTR(-EINVAL);
-
-	if (unlikely(!is_slave_direction(direction)))
-		goto out_err;
-
-	dwc->direction = direction;
-
-	if (direction == DMA_MEM_TO_DEV)
-		reg_width = __ffs(sconfig->dst_addr_width);
-	else
-		reg_width = __ffs(sconfig->src_addr_width);
-
-	periods = buf_len / period_len;
-
-	/* Check for too big/unaligned periods and unaligned DMA buffer. */
-	if (period_len > (dwc->block_size << reg_width))
-		goto out_err;
-	if (unlikely(period_len & ((1 << reg_width) - 1)))
-		goto out_err;
-	if (unlikely(buf_addr & ((1 << reg_width) - 1)))
-		goto out_err;
-
-	retval = ERR_PTR(-ENOMEM);
-
-	cdesc = kzalloc(sizeof(struct dw_cyclic_desc), GFP_KERNEL);
-	if (!cdesc)
-		goto out_err;
-
-	cdesc->desc = kzalloc(sizeof(struct dw_desc *) * periods, GFP_KERNEL);
-	if (!cdesc->desc)
-		goto out_err_alloc;
-
-	for (i = 0; i < periods; i++) {
-		desc = dwc_desc_get(dwc);
-		if (!desc)
-			goto out_err_desc_get;
-
-		switch (direction) {
-		case DMA_MEM_TO_DEV:
-			lli_write(desc, dar, sconfig->dst_addr);
-			lli_write(desc, sar, buf_addr + period_len * i);
-			lli_write(desc, ctllo, (DWC_DEFAULT_CTLLO(chan)
-				| DWC_CTLL_DST_WIDTH(reg_width)
-				| DWC_CTLL_SRC_WIDTH(reg_width)
-				| DWC_CTLL_DST_FIX
-				| DWC_CTLL_SRC_INC
-				| DWC_CTLL_INT_EN));
-
-			lli_set(desc, ctllo, sconfig->device_fc ?
-					DWC_CTLL_FC(DW_DMA_FC_P_M2P) :
-					DWC_CTLL_FC(DW_DMA_FC_D_M2P));
-
-			break;
-		case DMA_DEV_TO_MEM:
-			lli_write(desc, dar, buf_addr + period_len * i);
-			lli_write(desc, sar, sconfig->src_addr);
-			lli_write(desc, ctllo, (DWC_DEFAULT_CTLLO(chan)
-				| DWC_CTLL_SRC_WIDTH(reg_width)
-				| DWC_CTLL_DST_WIDTH(reg_width)
-				| DWC_CTLL_DST_INC
-				| DWC_CTLL_SRC_FIX
-				| DWC_CTLL_INT_EN));
-
-			lli_set(desc, ctllo, sconfig->device_fc ?
-					DWC_CTLL_FC(DW_DMA_FC_P_P2M) :
-					DWC_CTLL_FC(DW_DMA_FC_D_P2M));
-
-			break;
-		default:
-			break;
-		}
-
-		lli_write(desc, ctlhi, period_len >> reg_width);
-		cdesc->desc[i] = desc;
-
-		if (last)
-			lli_write(last, llp, desc->txd.phys | lms);
-
-		last = desc;
-	}
-
-	/* Let's make a cyclic list */
-	lli_write(last, llp, cdesc->desc[0]->txd.phys | lms);
-
-	dev_dbg(chan2dev(&dwc->chan),
-			"cyclic prepared buf %pad len %zu period %zu periods %d\n",
-			&buf_addr, buf_len, period_len, periods);
-
-	cdesc->periods = periods;
-	dwc->cdesc = cdesc;
-
-	return cdesc;
-
-out_err_desc_get:
-	while (i--)
-		dwc_desc_put(dwc, cdesc->desc[i]);
-out_err_alloc:
-	kfree(cdesc);
-out_err:
-	clear_bit(DW_DMA_IS_CYCLIC, &dwc->flags);
-	return (struct dw_cyclic_desc *)retval;
-}
-EXPORT_SYMBOL(dw_dma_cyclic_prep);
-
-/**
- * dw_dma_cyclic_free - free a prepared cyclic DMA transfer
- * @chan: the DMA channel to free
- */
-void dw_dma_cyclic_free(struct dma_chan *chan)
-{
-	struct dw_dma_chan	*dwc = to_dw_dma_chan(chan);
-	struct dw_dma		*dw = to_dw_dma(dwc->chan.device);
-	struct dw_cyclic_desc	*cdesc = dwc->cdesc;
-	unsigned int		i;
-	unsigned long		flags;
-
-	dev_dbg(chan2dev(&dwc->chan), "%s\n", __func__);
-
-	if (!cdesc)
-		return;
-
-	spin_lock_irqsave(&dwc->lock, flags);
-
-	dwc_chan_disable(dw, dwc);
-
-	dma_writel(dw, CLEAR.BLOCK, dwc->mask);
-	dma_writel(dw, CLEAR.ERROR, dwc->mask);
-	dma_writel(dw, CLEAR.XFER, dwc->mask);
-
-	spin_unlock_irqrestore(&dwc->lock, flags);
-
-	for (i = 0; i < cdesc->periods; i++)
-		dwc_desc_put(dwc, cdesc->desc[i]);
-
-	kfree(cdesc->desc);
-	kfree(cdesc);
-
-	dwc->cdesc = NULL;
-
-	clear_bit(DW_DMA_IS_CYCLIC, &dwc->flags);
-}
-EXPORT_SYMBOL(dw_dma_cyclic_free);
-
-/*----------------------------------------------------------------------*/
-
 int dw_dma_probe(struct dw_dma_chip *chip)
 {
 	struct dw_dma_platform_data *pdata;
@@ -1642,7 +1314,7 @@ int dw_dma_probe(struct dw_dma_chip *chip)
 		if (autocfg) {
 			unsigned int r = DW_DMA_MAX_NR_CHANNELS - i - 1;
 			void __iomem *addr = &__dw_regs(dw)->DWC_PARAMS[r];
-			unsigned int dwc_params = dma_readl_native(addr);
+			unsigned int dwc_params = readl(addr);
 
 			dev_dbg(chip->dev, "DWC_PARAMS[%d]: 0x%08x\n", i,
 					   dwc_params);
diff --git a/drivers/dma/dw/regs.h b/drivers/dma/dw/regs.h
index 32a328721c88..09e7dfdbb790 100644
--- a/drivers/dma/dw/regs.h
+++ b/drivers/dma/dw/regs.h
@@ -116,20 +116,6 @@ struct dw_dma_regs {
 	DW_REG(GLOBAL_CFG);
 };
 
-/*
- * Big endian I/O access when reading and writing to the DMA controller
- * registers.  This is needed on some platforms, like the Atmel AVR32
- * architecture.
- */
-
-#ifdef CONFIG_DW_DMAC_BIG_ENDIAN_IO
-#define dma_readl_native ioread32be
-#define dma_writel_native iowrite32be
-#else
-#define dma_readl_native readl
-#define dma_writel_native writel
-#endif
-
 /* Bitfields in DW_PARAMS */
 #define DW_PARAMS_NR_CHAN	8		/* number of channels */
 #define DW_PARAMS_NR_MASTER	11		/* number of AHB masters */
@@ -280,7 +266,6 @@ struct dw_dma_chan {
 	unsigned long		flags;
 	struct list_head	active_list;
 	struct list_head	queue;
-	struct dw_cyclic_desc	*cdesc;
 
 	unsigned int		descs_allocated;
 
@@ -302,9 +287,9 @@ __dwc_regs(struct dw_dma_chan *dwc)
 }
 
 #define channel_readl(dwc, name) \
-	dma_readl_native(&(__dwc_regs(dwc)->name))
+	readl(&(__dwc_regs(dwc)->name))
 #define channel_writel(dwc, name, val) \
-	dma_writel_native((val), &(__dwc_regs(dwc)->name))
+	writel((val), &(__dwc_regs(dwc)->name))
 
 static inline struct dw_dma_chan *to_dw_dma_chan(struct dma_chan *chan)
 {
@@ -333,9 +318,9 @@ static inline struct dw_dma_regs __iomem *__dw_regs(struct dw_dma *dw)
 }
 
 #define dma_readl(dw, name) \
-	dma_readl_native(&(__dw_regs(dw)->name))
+	readl(&(__dw_regs(dw)->name))
 #define dma_writel(dw, name, val) \
-	dma_writel_native((val), &(__dw_regs(dw)->name))
+	writel((val), &(__dw_regs(dw)->name))
 
 #define idma32_readq(dw, name)				\
 	hi_lo_readq(&(__dw_regs(dw)->name))
@@ -352,43 +337,30 @@ static inline struct dw_dma *to_dw_dma(struct dma_device *ddev)
 	return container_of(ddev, struct dw_dma, dma);
 }
 
-#ifdef CONFIG_DW_DMAC_BIG_ENDIAN_IO
-typedef __be32 __dw32;
-#else
-typedef __le32 __dw32;
-#endif
-
 /* LLI == Linked List Item; a.k.a. DMA block descriptor */
 struct dw_lli {
 	/* values that are not changed by hardware */
-	__dw32		sar;
-	__dw32		dar;
-	__dw32		llp;		/* chain to next lli */
-	__dw32		ctllo;
+	__le32		sar;
+	__le32		dar;
+	__le32		llp;		/* chain to next lli */
+	__le32		ctllo;
 	/* values that may get written back: */
-	__dw32		ctlhi;
+	__le32		ctlhi;
 	/* sstat and dstat can snapshot peripheral register state.
 	 * silicon config may discard either or both...
 	 */
-	__dw32		sstat;
-	__dw32		dstat;
+	__le32		sstat;
+	__le32		dstat;
 };
 
 struct dw_desc {
 	/* FIRST values the hardware uses */
 	struct dw_lli			lli;
 
-#ifdef CONFIG_DW_DMAC_BIG_ENDIAN_IO
-#define lli_set(d, reg, v)		((d)->lli.reg |= cpu_to_be32(v))
-#define lli_clear(d, reg, v)		((d)->lli.reg &= ~cpu_to_be32(v))
-#define lli_read(d, reg)		be32_to_cpu((d)->lli.reg)
-#define lli_write(d, reg, v)		((d)->lli.reg = cpu_to_be32(v))
-#else
 #define lli_set(d, reg, v)		((d)->lli.reg |= cpu_to_le32(v))
 #define lli_clear(d, reg, v)		((d)->lli.reg &= ~cpu_to_le32(v))
 #define lli_read(d, reg)		le32_to_cpu((d)->lli.reg)
 #define lli_write(d, reg, v)		((d)->lli.reg = cpu_to_le32(v))
-#endif
 
 	/* THEN values for driver housekeeping */
 	struct list_head		desc_node;
diff --git a/drivers/dma/omap-dma.c b/drivers/dma/omap-dma.c
index daf479cce691..8c1665c8fe33 100644
--- a/drivers/dma/omap-dma.c
+++ b/drivers/dma/omap-dma.c
@@ -916,12 +916,6 @@ static struct dma_async_tx_descriptor *omap_dma_prep_slave_sg(
 		return NULL;
 	}
 
-	/* When the port_window is used, one frame must cover the window */
-	if (port_window) {
-		burst = port_window;
-		port_window_bytes = port_window * es_bytes[es];
-	}
-
 	/* Now allocate and setup the descriptor. */
 	d = kzalloc(sizeof(*d) + sglen * sizeof(d->sg[0]), GFP_ATOMIC);
 	if (!d)
@@ -931,6 +925,21 @@ static struct dma_async_tx_descriptor *omap_dma_prep_slave_sg(
 	d->dev_addr = dev_addr;
 	d->es = es;
 
+	/* When the port_window is used, one frame must cover the window */
+	if (port_window) {
+		burst = port_window;
+		port_window_bytes = port_window * es_bytes[es];
+
+		d->ei = 1;
+		/*
+		 * One frame covers the port_window and by  configure
+		 * the source frame index to be -1 * (port_window - 1)
+		 * we instruct the sDMA that after a frame is processed
+		 * it should move back to the start of the window.
+		 */
+		d->fi = -(port_window_bytes - 1);
+	}
+
 	d->ccr = c->ccr | CCR_SYNC_FRAME;
 	if (dir == DMA_DEV_TO_MEM) {
 		d->csdp = CSDP_DST_BURST_64 | CSDP_DST_PACKED;
@@ -955,14 +964,6 @@ static struct dma_async_tx_descriptor *omap_dma_prep_slave_sg(
 		d->ccr |= CCR_SRC_AMODE_POSTINC;
 		if (port_window) {
 			d->ccr |= CCR_DST_AMODE_DBLIDX;
-			d->ei = 1;
-			/*
-			 * One frame covers the port_window and by  configure
-			 * the source frame index to be -1 * (port_window - 1)
-			 * we instruct the sDMA that after a frame is processed
-			 * it should move back to the start of the window.
-			 */
-			d->fi = -(port_window_bytes - 1);
 
 			if (port_window_bytes >= 64)
 				d->csdp |= CSDP_DST_BURST_64;
@@ -1018,16 +1019,6 @@ static struct dma_async_tx_descriptor *omap_dma_prep_slave_sg(
 		osg->addr = sg_dma_address(sgent);
 		osg->en = en;
 		osg->fn = sg_dma_len(sgent) / frame_bytes;
-		if (port_window && dir == DMA_DEV_TO_MEM) {
-			osg->ei = 1;
-			/*
-			 * One frame covers the port_window and by  configure
-			 * the source frame index to be -1 * (port_window - 1)
-			 * we instruct the sDMA that after a frame is processed
-			 * it should move back to the start of the window.
-			 */
-			osg->fi = -(port_window_bytes - 1);
-		}
 
 		if (d->using_ll) {
 			osg->t2_desc = dma_pool_alloc(od->desc_pool, GFP_ATOMIC,
diff --git a/drivers/edac/altera_edac.c b/drivers/edac/altera_edac.c
index 7717b094fabb..db75d4b614f7 100644
--- a/drivers/edac/altera_edac.c
+++ b/drivers/edac/altera_edac.c
@@ -214,24 +214,16 @@ static void altr_sdr_mc_create_debugfs_nodes(struct mem_ctl_info *mci)
 static unsigned long get_total_mem(void)
 {
 	struct device_node *np = NULL;
-	const unsigned int *reg, *reg_end;
-	int len, sw, aw;
-	unsigned long start, size, total_mem = 0;
+	struct resource res;
+	int ret;
+	unsigned long total_mem = 0;
 
 	for_each_node_by_type(np, "memory") {
-		aw = of_n_addr_cells(np);
-		sw = of_n_size_cells(np);
-		reg = (const unsigned int *)of_get_property(np, "reg", &len);
-		reg_end = reg + (len / sizeof(u32));
-
-		total_mem = 0;
-		do {
-			start = of_read_number(reg, aw);
-			reg += aw;
-			size = of_read_number(reg, sw);
-			reg += sw;
-			total_mem += size;
-		} while (reg < reg_end);
+		ret = of_address_to_resource(np, 0, &res);
+		if (ret)
+			continue;
+
+		total_mem += resource_size(&res);
 	}
 	edac_dbg(0, "total_mem 0x%lx\n", total_mem);
 	return total_mem;
@@ -1839,7 +1831,7 @@ static int a10_eccmgr_irqdomain_map(struct irq_domain *d, unsigned int irq,
 	return 0;
 }
 
-static struct irq_domain_ops a10_eccmgr_ic_ops = {
+static const struct irq_domain_ops a10_eccmgr_ic_ops = {
 	.map = a10_eccmgr_irqdomain_map,
 	.xlate = irq_domain_xlate_twocell,
 };
diff --git a/drivers/edac/i5000_edac.c b/drivers/edac/i5000_edac.c
index f683919981b0..8f5a56e25bd2 100644
--- a/drivers/edac/i5000_edac.c
+++ b/drivers/edac/i5000_edac.c
@@ -227,7 +227,7 @@
 #define			NREC_RDWR(x)		(((x)>>11) & 1)
 #define			NREC_RANK(x)		(((x)>>8) & 0x7)
 #define		NRECMEMB		0xC0
-#define			NREC_CAS(x)		(((x)>>16) & 0xFFFFFF)
+#define			NREC_CAS(x)		(((x)>>16) & 0xFFF)
 #define			NREC_RAS(x)		((x) & 0x7FFF)
 #define		NRECFGLOG		0xC4
 #define		NREEECFBDA		0xC8
@@ -371,7 +371,7 @@ struct i5000_error_info {
 	/* These registers are input ONLY if there was a
 	 * Non-Recoverable Error */
 	u16 nrecmema;		/* Non-Recoverable Mem log A */
-	u16 nrecmemb;		/* Non-Recoverable Mem log B */
+	u32 nrecmemb;		/* Non-Recoverable Mem log B */
 
 };
 
@@ -407,7 +407,7 @@ static void i5000_get_error_info(struct mem_ctl_info *mci,
 				NERR_FAT_FBD, &info->nerr_fat_fbd);
 		pci_read_config_word(pvt->branchmap_werrors,
 				NRECMEMA, &info->nrecmema);
-		pci_read_config_word(pvt->branchmap_werrors,
+		pci_read_config_dword(pvt->branchmap_werrors,
 				NRECMEMB, &info->nrecmemb);
 
 		/* Clear the error bits, by writing them back */
diff --git a/drivers/edac/i5400_edac.c b/drivers/edac/i5400_edac.c
index 37a9ba71da44..cd889edc8516 100644
--- a/drivers/edac/i5400_edac.c
+++ b/drivers/edac/i5400_edac.c
@@ -368,7 +368,7 @@ struct i5400_error_info {
 
 	/* These registers are input ONLY if there was a Non-Rec Error */
 	u16 nrecmema;		/* Non-Recoverable Mem log A */
-	u16 nrecmemb;		/* Non-Recoverable Mem log B */
+	u32 nrecmemb;		/* Non-Recoverable Mem log B */
 
 };
 
@@ -458,7 +458,7 @@ static void i5400_get_error_info(struct mem_ctl_info *mci,
 				NERR_FAT_FBD, &info->nerr_fat_fbd);
 		pci_read_config_word(pvt->branchmap_werrors,
 				NRECMEMA, &info->nrecmema);
-		pci_read_config_word(pvt->branchmap_werrors,
+		pci_read_config_dword(pvt->branchmap_werrors,
 				NRECMEMB, &info->nrecmemb);
 
 		/* Clear the error bits, by writing them back */
diff --git a/drivers/edac/ie31200_edac.c b/drivers/edac/ie31200_edac.c
index 2733fb5938a4..4260579e6901 100644
--- a/drivers/edac/ie31200_edac.c
+++ b/drivers/edac/ie31200_edac.c
@@ -18,10 +18,12 @@
  * 0c04: Xeon E3-1200 v3/4th Gen Core Processor DRAM Controller
  * 0c08: Xeon E3-1200 v3 Processor DRAM Controller
  * 1918: Xeon E3-1200 v5 Skylake Host Bridge/DRAM Registers
+ * 5918: Xeon E3-1200 Xeon E3-1200 v6/7th Gen Core Processor Host Bridge/DRAM Registers
  *
  * Based on Intel specification:
  * http://www.intel.com/content/dam/www/public/us/en/documents/datasheets/xeon-e3-1200v3-vol-2-datasheet.pdf
  * http://www.intel.com/content/www/us/en/processors/xeon/xeon-e3-1200-family-vol-2-datasheet.html
+ * http://www.intel.com/content/www/us/en/processors/core/7th-gen-core-family-mobile-h-processor-lines-datasheet-vol-2.html
  *
  * According to the above datasheet (p.16):
  * "
@@ -57,6 +59,7 @@
 #define PCI_DEVICE_ID_INTEL_IE31200_HB_6 0x0c04
 #define PCI_DEVICE_ID_INTEL_IE31200_HB_7 0x0c08
 #define PCI_DEVICE_ID_INTEL_IE31200_HB_8 0x1918
+#define PCI_DEVICE_ID_INTEL_IE31200_HB_9 0x5918
 
 #define IE31200_DIMMS			4
 #define IE31200_RANKS			8
@@ -376,7 +379,12 @@ static int ie31200_probe1(struct pci_dev *pdev, int dev_idx)
 	void __iomem *window;
 	struct ie31200_priv *priv;
 	u32 addr_decode, mad_offset;
-	bool skl = (pdev->device == PCI_DEVICE_ID_INTEL_IE31200_HB_8);
+
+	/*
+	 * Kaby Lake seems to work like Skylake. Please re-visit this logic
+	 * when adding new CPU support.
+	 */
+	bool skl = (pdev->device >= PCI_DEVICE_ID_INTEL_IE31200_HB_8);
 
 	edac_dbg(0, "MC:\n");
 
@@ -560,6 +568,9 @@ static const struct pci_device_id ie31200_pci_tbl[] = {
 		PCI_VEND_DEV(INTEL, IE31200_HB_8), PCI_ANY_ID, PCI_ANY_ID, 0, 0,
 		IE31200},
 	{
+		PCI_VEND_DEV(INTEL, IE31200_HB_9), PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+		IE31200},
+	{
 		0,
 	}            /* 0 terminated list. */
 };
diff --git a/drivers/edac/mce_amd.c b/drivers/edac/mce_amd.c
index ba35b7ea3686..9a2658a256a9 100644
--- a/drivers/edac/mce_amd.c
+++ b/drivers/edac/mce_amd.c
@@ -161,7 +161,7 @@ static const char * const smca_ls_mce_desc[] = {
 	"Sys Read data error thread 0",
 	"Sys read data error thread 1",
 	"DC tag error type 2",
-	"DC data error type 1 (poison comsumption)",
+	"DC data error type 1 (poison consumption)",
 	"DC data error type 2",
 	"DC data error type 3",
 	"DC tag error type 4",
diff --git a/drivers/edac/mv64x60_edac.c b/drivers/edac/mv64x60_edac.c
index 14b7e7b71eaa..d3650df94fe8 100644
--- a/drivers/edac/mv64x60_edac.c
+++ b/drivers/edac/mv64x60_edac.c
@@ -32,21 +32,21 @@ static void mv64x60_pci_check(struct edac_pci_ctl_info *pci)
 	struct mv64x60_pci_pdata *pdata = pci->pvt_info;
 	u32 cause;
 
-	cause = in_le32(pdata->pci_vbase + MV64X60_PCI_ERROR_CAUSE);
+	cause = readl(pdata->pci_vbase + MV64X60_PCI_ERROR_CAUSE);
 	if (!cause)
 		return;
 
 	printk(KERN_ERR "Error in PCI %d Interface\n", pdata->pci_hose);
 	printk(KERN_ERR "Cause register: 0x%08x\n", cause);
 	printk(KERN_ERR "Address Low: 0x%08x\n",
-	       in_le32(pdata->pci_vbase + MV64X60_PCI_ERROR_ADDR_LO));
+	       readl(pdata->pci_vbase + MV64X60_PCI_ERROR_ADDR_LO));
 	printk(KERN_ERR "Address High: 0x%08x\n",
-	       in_le32(pdata->pci_vbase + MV64X60_PCI_ERROR_ADDR_HI));
+	       readl(pdata->pci_vbase + MV64X60_PCI_ERROR_ADDR_HI));
 	printk(KERN_ERR "Attribute: 0x%08x\n",
-	       in_le32(pdata->pci_vbase + MV64X60_PCI_ERROR_ATTR));
+	       readl(pdata->pci_vbase + MV64X60_PCI_ERROR_ATTR));
 	printk(KERN_ERR "Command: 0x%08x\n",
-	       in_le32(pdata->pci_vbase + MV64X60_PCI_ERROR_CMD));
-	out_le32(pdata->pci_vbase + MV64X60_PCI_ERROR_CAUSE, ~cause);
+	       readl(pdata->pci_vbase + MV64X60_PCI_ERROR_CMD));
+	writel(~cause, pdata->pci_vbase + MV64X60_PCI_ERROR_CAUSE);
 
 	if (cause & MV64X60_PCI_PE_MASK)
 		edac_pci_handle_pe(pci, pci->ctl_name);
@@ -61,7 +61,7 @@ static irqreturn_t mv64x60_pci_isr(int irq, void *dev_id)
 	struct mv64x60_pci_pdata *pdata = pci->pvt_info;
 	u32 val;
 
-	val = in_le32(pdata->pci_vbase + MV64X60_PCI_ERROR_CAUSE);
+	val = readl(pdata->pci_vbase + MV64X60_PCI_ERROR_CAUSE);
 	if (!val)
 		return IRQ_NONE;
 
@@ -93,7 +93,7 @@ static int __init mv64x60_pci_fixup(struct platform_device *pdev)
 	if (!pci_serr)
 		return -ENOMEM;
 
-	out_le32(pci_serr, in_le32(pci_serr) & ~0x1);
+	writel(readl(pci_serr) & ~0x1, pci_serr);
 	iounmap(pci_serr);
 
 	return 0;
@@ -116,7 +116,7 @@ static int mv64x60_pci_err_probe(struct platform_device *pdev)
 	pdata = pci->pvt_info;
 
 	pdata->pci_hose = pdev->id;
-	pdata->name = "mpc85xx_pci_err";
+	pdata->name = "mv64x60_pci_err";
 	platform_set_drvdata(pdev, pci);
 	pci->dev = &pdev->dev;
 	pci->dev_name = dev_name(&pdev->dev);
@@ -161,10 +161,10 @@ static int mv64x60_pci_err_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	out_le32(pdata->pci_vbase + MV64X60_PCI_ERROR_CAUSE, 0);
-	out_le32(pdata->pci_vbase + MV64X60_PCI_ERROR_MASK, 0);
-	out_le32(pdata->pci_vbase + MV64X60_PCI_ERROR_MASK,
-		 MV64X60_PCIx_ERR_MASK_VAL);
+	writel(0, pdata->pci_vbase + MV64X60_PCI_ERROR_CAUSE);
+	writel(0, pdata->pci_vbase + MV64X60_PCI_ERROR_MASK);
+	writel(MV64X60_PCIx_ERR_MASK_VAL,
+		  pdata->pci_vbase + MV64X60_PCI_ERROR_MASK);
 
 	if (edac_pci_add_device(pci, pdata->edac_idx) > 0) {
 		edac_dbg(3, "failed edac_pci_add_device()\n");
@@ -233,23 +233,23 @@ static void mv64x60_sram_check(struct edac_device_ctl_info *edac_dev)
 	struct mv64x60_sram_pdata *pdata = edac_dev->pvt_info;
 	u32 cause;
 
-	cause = in_le32(pdata->sram_vbase + MV64X60_SRAM_ERR_CAUSE);
+	cause = readl(pdata->sram_vbase + MV64X60_SRAM_ERR_CAUSE);
 	if (!cause)
 		return;
 
 	printk(KERN_ERR "Error in internal SRAM\n");
 	printk(KERN_ERR "Cause register: 0x%08x\n", cause);
 	printk(KERN_ERR "Address Low: 0x%08x\n",
-	       in_le32(pdata->sram_vbase + MV64X60_SRAM_ERR_ADDR_LO));
+	       readl(pdata->sram_vbase + MV64X60_SRAM_ERR_ADDR_LO));
 	printk(KERN_ERR "Address High: 0x%08x\n",
-	       in_le32(pdata->sram_vbase + MV64X60_SRAM_ERR_ADDR_HI));
+	       readl(pdata->sram_vbase + MV64X60_SRAM_ERR_ADDR_HI));
 	printk(KERN_ERR "Data Low: 0x%08x\n",
-	       in_le32(pdata->sram_vbase + MV64X60_SRAM_ERR_DATA_LO));
+	       readl(pdata->sram_vbase + MV64X60_SRAM_ERR_DATA_LO));
 	printk(KERN_ERR "Data High: 0x%08x\n",
-	       in_le32(pdata->sram_vbase + MV64X60_SRAM_ERR_DATA_HI));
+	       readl(pdata->sram_vbase + MV64X60_SRAM_ERR_DATA_HI));
 	printk(KERN_ERR "Parity: 0x%08x\n",
-	       in_le32(pdata->sram_vbase + MV64X60_SRAM_ERR_PARITY));
-	out_le32(pdata->sram_vbase + MV64X60_SRAM_ERR_CAUSE, 0);
+	       readl(pdata->sram_vbase + MV64X60_SRAM_ERR_PARITY));
+	writel(0, pdata->sram_vbase + MV64X60_SRAM_ERR_CAUSE);
 
 	edac_device_handle_ue(edac_dev, 0, 0, edac_dev->ctl_name);
 }
@@ -260,7 +260,7 @@ static irqreturn_t mv64x60_sram_isr(int irq, void *dev_id)
 	struct mv64x60_sram_pdata *pdata = edac_dev->pvt_info;
 	u32 cause;
 
-	cause = in_le32(pdata->sram_vbase + MV64X60_SRAM_ERR_CAUSE);
+	cause = readl(pdata->sram_vbase + MV64X60_SRAM_ERR_CAUSE);
 	if (!cause)
 		return IRQ_NONE;
 
@@ -322,7 +322,7 @@ static int mv64x60_sram_err_probe(struct platform_device *pdev)
 	}
 
 	/* setup SRAM err registers */
-	out_le32(pdata->sram_vbase + MV64X60_SRAM_ERR_CAUSE, 0);
+	writel(0, pdata->sram_vbase + MV64X60_SRAM_ERR_CAUSE);
 
 	edac_dev->mod_name = EDAC_MOD_STR;
 	edac_dev->ctl_name = pdata->name;
@@ -398,7 +398,7 @@ static void mv64x60_cpu_check(struct edac_device_ctl_info *edac_dev)
 	struct mv64x60_cpu_pdata *pdata = edac_dev->pvt_info;
 	u32 cause;
 
-	cause = in_le32(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_CAUSE) &
+	cause = readl(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_CAUSE) &
 	    MV64x60_CPU_CAUSE_MASK;
 	if (!cause)
 		return;
@@ -406,16 +406,16 @@ static void mv64x60_cpu_check(struct edac_device_ctl_info *edac_dev)
 	printk(KERN_ERR "Error on CPU interface\n");
 	printk(KERN_ERR "Cause register: 0x%08x\n", cause);
 	printk(KERN_ERR "Address Low: 0x%08x\n",
-	       in_le32(pdata->cpu_vbase[0] + MV64x60_CPU_ERR_ADDR_LO));
+	       readl(pdata->cpu_vbase[0] + MV64x60_CPU_ERR_ADDR_LO));
 	printk(KERN_ERR "Address High: 0x%08x\n",
-	       in_le32(pdata->cpu_vbase[0] + MV64x60_CPU_ERR_ADDR_HI));
+	       readl(pdata->cpu_vbase[0] + MV64x60_CPU_ERR_ADDR_HI));
 	printk(KERN_ERR "Data Low: 0x%08x\n",
-	       in_le32(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_DATA_LO));
+	       readl(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_DATA_LO));
 	printk(KERN_ERR "Data High: 0x%08x\n",
-	       in_le32(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_DATA_HI));
+	       readl(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_DATA_HI));
 	printk(KERN_ERR "Parity: 0x%08x\n",
-	       in_le32(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_PARITY));
-	out_le32(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_CAUSE, 0);
+	       readl(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_PARITY));
+	writel(0, pdata->cpu_vbase[1] + MV64x60_CPU_ERR_CAUSE);
 
 	edac_device_handle_ue(edac_dev, 0, 0, edac_dev->ctl_name);
 }
@@ -426,7 +426,7 @@ static irqreturn_t mv64x60_cpu_isr(int irq, void *dev_id)
 	struct mv64x60_cpu_pdata *pdata = edac_dev->pvt_info;
 	u32 cause;
 
-	cause = in_le32(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_CAUSE) &
+	cause = readl(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_CAUSE) &
 	    MV64x60_CPU_CAUSE_MASK;
 	if (!cause)
 		return IRQ_NONE;
@@ -515,9 +515,9 @@ static int mv64x60_cpu_err_probe(struct platform_device *pdev)
 	}
 
 	/* setup CPU err registers */
-	out_le32(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_CAUSE, 0);
-	out_le32(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_MASK, 0);
-	out_le32(pdata->cpu_vbase[1] + MV64x60_CPU_ERR_MASK, 0x000000ff);
+	writel(0, pdata->cpu_vbase[1] + MV64x60_CPU_ERR_CAUSE);
+	writel(0, pdata->cpu_vbase[1] + MV64x60_CPU_ERR_MASK);
+	writel(0x000000ff, pdata->cpu_vbase[1] + MV64x60_CPU_ERR_MASK);
 
 	edac_dev->mod_name = EDAC_MOD_STR;
 	edac_dev->ctl_name = pdata->name;
@@ -596,13 +596,13 @@ static void mv64x60_mc_check(struct mem_ctl_info *mci)
 	u32 comp_ecc;
 	u32 syndrome;
 
-	reg = in_le32(pdata->mc_vbase + MV64X60_SDRAM_ERR_ADDR);
+	reg = readl(pdata->mc_vbase + MV64X60_SDRAM_ERR_ADDR);
 	if (!reg)
 		return;
 
 	err_addr = reg & ~0x3;
-	sdram_ecc = in_le32(pdata->mc_vbase + MV64X60_SDRAM_ERR_ECC_RCVD);
-	comp_ecc = in_le32(pdata->mc_vbase + MV64X60_SDRAM_ERR_ECC_CALC);
+	sdram_ecc = readl(pdata->mc_vbase + MV64X60_SDRAM_ERR_ECC_RCVD);
+	comp_ecc = readl(pdata->mc_vbase + MV64X60_SDRAM_ERR_ECC_CALC);
 	syndrome = sdram_ecc ^ comp_ecc;
 
 	/* first bit clear in ECC Err Reg, 1 bit error, correctable by HW */
@@ -620,7 +620,7 @@ static void mv64x60_mc_check(struct mem_ctl_info *mci)
 				     mci->ctl_name, "");
 
 	/* clear the error */
-	out_le32(pdata->mc_vbase + MV64X60_SDRAM_ERR_ADDR, 0);
+	writel(0, pdata->mc_vbase + MV64X60_SDRAM_ERR_ADDR);
 }
 
 static irqreturn_t mv64x60_mc_isr(int irq, void *dev_id)
@@ -629,7 +629,7 @@ static irqreturn_t mv64x60_mc_isr(int irq, void *dev_id)
 	struct mv64x60_mc_pdata *pdata = mci->pvt_info;
 	u32 reg;
 
-	reg = in_le32(pdata->mc_vbase + MV64X60_SDRAM_ERR_ADDR);
+	reg = readl(pdata->mc_vbase + MV64X60_SDRAM_ERR_ADDR);
 	if (!reg)
 		return IRQ_NONE;
 
@@ -664,7 +664,7 @@ static void mv64x60_init_csrows(struct mem_ctl_info *mci,
 
 	get_total_mem(pdata);
 
-	ctl = in_le32(pdata->mc_vbase + MV64X60_SDRAM_CONFIG);
+	ctl = readl(pdata->mc_vbase + MV64X60_SDRAM_CONFIG);
 
 	csrow = mci->csrows[0];
 	dimm = csrow->channels[0]->dimm;
@@ -753,7 +753,7 @@ static int mv64x60_mc_err_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	ctl = in_le32(pdata->mc_vbase + MV64X60_SDRAM_CONFIG);
+	ctl = readl(pdata->mc_vbase + MV64X60_SDRAM_CONFIG);
 	if (!(ctl & MV64X60_SDRAM_ECC)) {
 		/* Non-ECC RAM? */
 		printk(KERN_WARNING "%s: No ECC DIMMs discovered\n", __func__);
@@ -779,10 +779,10 @@ static int mv64x60_mc_err_probe(struct platform_device *pdev)
 	mv64x60_init_csrows(mci, pdata);
 
 	/* setup MC registers */
-	out_le32(pdata->mc_vbase + MV64X60_SDRAM_ERR_ADDR, 0);
-	ctl = in_le32(pdata->mc_vbase + MV64X60_SDRAM_ERR_ECC_CNTL);
+	writel(0, pdata->mc_vbase + MV64X60_SDRAM_ERR_ADDR);
+	ctl = readl(pdata->mc_vbase + MV64X60_SDRAM_ERR_ECC_CNTL);
 	ctl = (ctl & 0xff00ffff) | 0x10000;
-	out_le32(pdata->mc_vbase + MV64X60_SDRAM_ERR_ECC_CNTL, ctl);
+	writel(ctl, pdata->mc_vbase + MV64X60_SDRAM_ERR_ECC_CNTL);
 
 	res = edac_mc_add_mc(mci);
 	if (res) {
@@ -853,10 +853,10 @@ static struct platform_driver * const drivers[] = {
 
 static int __init mv64x60_edac_init(void)
 {
-	int ret = 0;
 
 	printk(KERN_INFO "Marvell MV64x60 EDAC driver " MV64x60_REVISION "\n");
 	printk(KERN_INFO "\t(C) 2006-2007 MontaVista Software\n");
+
 	/* make sure error reporting method is sane */
 	switch (edac_op_state) {
 	case EDAC_OPSTATE_POLL:
diff --git a/drivers/edac/pnd2_edac.c b/drivers/edac/pnd2_edac.c
index 1cad5a9af8d0..8e599490f6de 100644
--- a/drivers/edac/pnd2_edac.c
+++ b/drivers/edac/pnd2_edac.c
@@ -131,7 +131,7 @@ static struct mem_ctl_info *pnd2_mci;
 
 #ifdef CONFIG_X86_INTEL_SBI_APL
 #include "linux/platform_data/sbi_apl.h"
-int sbi_send(int port, int off, int op, u32 *data)
+static int sbi_send(int port, int off, int op, u32 *data)
 {
 	struct sbi_apl_message sbi_arg;
 	int ret, read = 0;
@@ -160,7 +160,7 @@ int sbi_send(int port, int off, int op, u32 *data)
 	return ret;
 }
 #else
-int sbi_send(int port, int off, int op, u32 *data)
+static int sbi_send(int port, int off, int op, u32 *data)
 {
 	return -EUNATCH;
 }
@@ -168,14 +168,15 @@ int sbi_send(int port, int off, int op, u32 *data)
 
 static int apl_rd_reg(int port, int off, int op, void *data, size_t sz, char *name)
 {
-	int	ret = 0;
+	int ret = 0;
 
 	edac_dbg(2, "Read %s port=%x off=%x op=%x\n", name, port, off, op);
 	switch (sz) {
 	case 8:
 		ret = sbi_send(port, off + 4, op, (u32 *)(data + 4));
+		/* fall through */
 	case 4:
-		ret = sbi_send(port, off, op, (u32 *)data);
+		ret |= sbi_send(port, off, op, (u32 *)data);
 		pnd2_printk(KERN_DEBUG, "%s=%x%08x ret=%d\n", name,
 					sz == 8 ? *((u32 *)(data + 4)) : 0, *((u32 *)data), ret);
 		break;
@@ -423,16 +424,21 @@ static void dnv_mk_region(char *name, struct region *rp, void *asym)
 
 static int apl_get_registers(void)
 {
+	int ret = -ENODEV;
 	int i;
 
 	if (RD_REG(&asym_2way, b_cr_asym_2way_mem_region_mchbar))
 		return -ENODEV;
 
+	/*
+	 * RD_REGP() will fail for unpopulated or non-existent
+	 * DIMM slots. Return success if we find at least one DIMM.
+	 */
 	for (i = 0; i < APL_NUM_CHANNELS; i++)
-		if (RD_REGP(&drp0[i], d_cr_drp0, apl_dports[i]))
-			return -ENODEV;
+		if (!RD_REGP(&drp0[i], d_cr_drp0, apl_dports[i]))
+			ret = 0;
 
-	return 0;
+	return ret;
 }
 
 static int dnv_get_registers(void)
diff --git a/drivers/edac/sb_edac.c b/drivers/edac/sb_edac.c
index ea21cb651b3c..80d860cb0746 100644
--- a/drivers/edac/sb_edac.c
+++ b/drivers/edac/sb_edac.c
@@ -35,7 +35,7 @@ static LIST_HEAD(sbridge_edac_list);
 /*
  * Alter this version for the module when modifications are made
  */
-#define SBRIDGE_REVISION    " Ver: 1.1.1 "
+#define SBRIDGE_REVISION    " Ver: 1.1.2 "
 #define EDAC_MOD_STR      "sbridge_edac"
 
 /*
@@ -279,7 +279,7 @@ static const u32 correrrthrsld[] = {
  * sbridge structs
  */
 
-#define NUM_CHANNELS		8	/* 2MC per socket, four chan per MC */
+#define NUM_CHANNELS		4	/* Max channels per MC */
 #define MAX_DIMMS		3	/* Max DIMMS per channel */
 #define KNL_MAX_CHAS		38	/* KNL max num. of Cache Home Agents */
 #define KNL_MAX_CHANNELS	6	/* KNL max num. of PCI channels */
@@ -294,6 +294,12 @@ enum type {
 	KNIGHTS_LANDING,
 };
 
+enum domain {
+	IMC0 = 0,
+	IMC1,
+	SOCK,
+};
+
 struct sbridge_pvt;
 struct sbridge_info {
 	enum type	type;
@@ -324,11 +330,14 @@ struct sbridge_channel {
 struct pci_id_descr {
 	int			dev_id;
 	int			optional;
+	enum domain		dom;
 };
 
 struct pci_id_table {
 	const struct pci_id_descr	*descr;
-	int				n_devs;
+	int				n_devs_per_imc;
+	int				n_devs_per_sock;
+	int				n_imcs_per_sock;
 	enum type			type;
 };
 
@@ -337,7 +346,9 @@ struct sbridge_dev {
 	u8			bus, mc;
 	u8			node_id, source_id;
 	struct pci_dev		**pdev;
+	enum domain		dom;
 	int			n_devs;
+	int			i_devs;
 	struct mem_ctl_info	*mci;
 };
 
@@ -352,11 +363,12 @@ struct knl_pvt {
 };
 
 struct sbridge_pvt {
-	struct pci_dev		*pci_ta, *pci_ddrio, *pci_ras;
+	/* Devices per socket */
+	struct pci_dev		*pci_ddrio;
 	struct pci_dev		*pci_sad0, *pci_sad1;
-	struct pci_dev		*pci_ha0, *pci_ha1;
 	struct pci_dev		*pci_br0, *pci_br1;
-	struct pci_dev		*pci_ha1_ta;
+	/* Devices per memory controller */
+	struct pci_dev		*pci_ha, *pci_ta, *pci_ras;
 	struct pci_dev		*pci_tad[NUM_CHANNELS];
 
 	struct sbridge_dev	*sbridge_dev;
@@ -373,39 +385,42 @@ struct sbridge_pvt {
 	struct knl_pvt knl;
 };
 
-#define PCI_DESCR(device_id, opt)	\
+#define PCI_DESCR(device_id, opt, domain)	\
 	.dev_id = (device_id),		\
-	.optional = opt
+	.optional = opt,	\
+	.dom = domain
 
 static const struct pci_id_descr pci_dev_descr_sbridge[] = {
 		/* Processor Home Agent */
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_HA0, 0)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_HA0,   0, IMC0) },
 
 		/* Memory controller */
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TA, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_RAS, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD0, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD1, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD2, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD3, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_DDRIO, 1)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TA,    0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_RAS,   0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD0,  0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD1,  0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD2,  0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD3,  0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_DDRIO, 1, SOCK) },
 
 		/* System Address Decoder */
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_SAD0, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_SAD1, 0)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_SAD0,      0, SOCK) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_SAD1,      0, SOCK) },
 
 		/* Broadcast Registers */
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_BR, 0)		},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_SBRIDGE_BR,        0, SOCK) },
 };
 
-#define PCI_ID_TABLE_ENTRY(A, T) {	\
+#define PCI_ID_TABLE_ENTRY(A, N, M, T) {	\
 	.descr = A,			\
-	.n_devs = ARRAY_SIZE(A),	\
+	.n_devs_per_imc = N,	\
+	.n_devs_per_sock = ARRAY_SIZE(A),	\
+	.n_imcs_per_sock = M,	\
 	.type = T			\
 }
 
 static const struct pci_id_table pci_dev_descr_sbridge_table[] = {
-	PCI_ID_TABLE_ENTRY(pci_dev_descr_sbridge, SANDY_BRIDGE),
+	PCI_ID_TABLE_ENTRY(pci_dev_descr_sbridge, ARRAY_SIZE(pci_dev_descr_sbridge), 1, SANDY_BRIDGE),
 	{0,}			/* 0 terminated list. */
 };
 
@@ -439,40 +454,39 @@ static const struct pci_id_table pci_dev_descr_sbridge_table[] = {
 
 static const struct pci_id_descr pci_dev_descr_ibridge[] = {
 		/* Processor Home Agent */
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0, 0)		},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0,        0, IMC0) },
 
 		/* Memory controller */
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TA, 0)		},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_RAS, 0)		},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD0, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD1, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD2, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD3, 0)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TA,     0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_RAS,    0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD0,   0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD1,   0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD2,   0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD3,   0, IMC0) },
+
+		/* Optional, mode 2HA */
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1,        1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TA,     1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_RAS,    1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD0,   1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD1,   1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD2,   1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD3,   1, IMC1) },
+
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_1HA_DDRIO0, 1, SOCK) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_2HA_DDRIO0, 1, SOCK) },
 
 		/* System Address Decoder */
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_SAD, 0)			},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_SAD,            0, SOCK) },
 
 		/* Broadcast Registers */
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_BR0, 1)			},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_BR1, 0)			},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_BR0,            1, SOCK) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_BR1,            0, SOCK) },
 
-		/* Optional, mode 2HA */
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1, 1)		},
-#if 0
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TA, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_RAS, 1)	},
-#endif
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD0, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD1, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD2, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD3, 1)	},
-
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_1HA_DDRIO0, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_2HA_DDRIO0, 1)	},
 };
 
 static const struct pci_id_table pci_dev_descr_ibridge_table[] = {
-	PCI_ID_TABLE_ENTRY(pci_dev_descr_ibridge, IVY_BRIDGE),
+	PCI_ID_TABLE_ENTRY(pci_dev_descr_ibridge, 12, 2, IVY_BRIDGE),
 	{0,}			/* 0 terminated list. */
 };
 
@@ -498,9 +512,9 @@ static const struct pci_id_table pci_dev_descr_ibridge_table[] = {
 #define PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0	0x2fa0
 #define PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1	0x2f60
 #define PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TA	0x2fa8
-#define PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_THERMAL 0x2f71
+#define PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TM	0x2f71
 #define PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TA	0x2f68
-#define PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_THERMAL 0x2f79
+#define PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TM	0x2f79
 #define PCI_DEVICE_ID_INTEL_HASWELL_IMC_CBO_SAD0 0x2ffc
 #define PCI_DEVICE_ID_INTEL_HASWELL_IMC_CBO_SAD1 0x2ffd
 #define PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD0 0x2faa
@@ -517,35 +531,33 @@ static const struct pci_id_table pci_dev_descr_ibridge_table[] = {
 #define PCI_DEVICE_ID_INTEL_HASWELL_IMC_DDRIO3 0x2fbb
 static const struct pci_id_descr pci_dev_descr_haswell[] = {
 	/* first item must be the HA */
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0, 0)		},
-
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_CBO_SAD0, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_CBO_SAD1, 0)	},
-
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1, 1)		},
-
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TA, 0)		},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_THERMAL, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD0, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD1, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD2, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD3, 1)	},
-
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_DDRIO0, 1)		},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_DDRIO1, 1)		},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_DDRIO2, 1)		},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_DDRIO3, 1)		},
-
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TA, 1)		},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_THERMAL, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD0, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD1, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD2, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD3, 1)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0,      0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1,      1, IMC1) },
+
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TA,   0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TM,   0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD0, 0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD1, 0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD2, 1, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD3, 1, IMC0) },
+
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TA,   1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TM,   1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD0, 1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD1, 1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD2, 1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD3, 1, IMC1) },
+
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_CBO_SAD0, 0, SOCK) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_CBO_SAD1, 0, SOCK) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_DDRIO0,   1, SOCK) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_DDRIO1,   1, SOCK) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_DDRIO2,   1, SOCK) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_HASWELL_IMC_DDRIO3,   1, SOCK) },
 };
 
 static const struct pci_id_table pci_dev_descr_haswell_table[] = {
-	PCI_ID_TABLE_ENTRY(pci_dev_descr_haswell, HASWELL),
+	PCI_ID_TABLE_ENTRY(pci_dev_descr_haswell, 13, 2, HASWELL),
 	{0,}			/* 0 terminated list. */
 };
 
@@ -559,7 +571,7 @@ static const struct pci_id_table pci_dev_descr_haswell_table[] = {
 /* Memory controller, TAD tables, error injection - 2-8-0, 2-9-0 (2 of these) */
 #define PCI_DEVICE_ID_INTEL_KNL_IMC_MC       0x7840
 /* DRAM channel stuff; bank addrs, dimmmtr, etc.. 2-8-2 - 2-9-4 (6 of these) */
-#define PCI_DEVICE_ID_INTEL_KNL_IMC_CHANNEL  0x7843
+#define PCI_DEVICE_ID_INTEL_KNL_IMC_CHAN     0x7843
 /* kdrwdbu TAD limits/offsets, MCMTR - 2-10-1, 2-11-1 (2 of these) */
 #define PCI_DEVICE_ID_INTEL_KNL_IMC_TA       0x7844
 /* CHA broadcast registers, dram rules - 1-29-0 (1 of these) */
@@ -579,17 +591,17 @@ static const struct pci_id_table pci_dev_descr_haswell_table[] = {
  */
 
 static const struct pci_id_descr pci_dev_descr_knl[] = {
-	[0]         = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_SAD0, 0) },
-	[1]         = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_SAD1, 0) },
-	[2 ... 3]   = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_MC, 0)},
-	[4 ... 41]  = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_CHA, 0) },
-	[42 ... 47] = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_CHANNEL, 0) },
-	[48]        = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_TA, 0) },
-	[49]        = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_TOLHM, 0) },
+	[0 ... 1]   = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_MC,    0, IMC0)},
+	[2 ... 7]   = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_CHAN,  0, IMC0) },
+	[8]	    = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_TA,    0, IMC0) },
+	[9]	    = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_TOLHM, 0, IMC0) },
+	[10]	    = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_SAD0,  0, SOCK) },
+	[11]	    = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_SAD1,  0, SOCK) },
+	[12 ... 49] = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_CHA,   0, SOCK) },
 };
 
 static const struct pci_id_table pci_dev_descr_knl_table[] = {
-	PCI_ID_TABLE_ENTRY(pci_dev_descr_knl, KNIGHTS_LANDING),
+	PCI_ID_TABLE_ENTRY(pci_dev_descr_knl, ARRAY_SIZE(pci_dev_descr_knl), 1, KNIGHTS_LANDING),
 	{0,}
 };
 
@@ -615,9 +627,9 @@ static const struct pci_id_table pci_dev_descr_knl_table[] = {
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0	0x6fa0
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1	0x6f60
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TA	0x6fa8
-#define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_THERMAL 0x6f71
+#define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TM	0x6f71
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TA	0x6f68
-#define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_THERMAL 0x6f79
+#define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TM	0x6f79
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_CBO_SAD0 0x6ffc
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_CBO_SAD1 0x6ffd
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD0 0x6faa
@@ -632,32 +644,30 @@ static const struct pci_id_table pci_dev_descr_knl_table[] = {
 
 static const struct pci_id_descr pci_dev_descr_broadwell[] = {
 	/* first item must be the HA */
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0, 0)		},
-
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_CBO_SAD0, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_CBO_SAD1, 0)	},
-
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1, 1)		},
-
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TA, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_THERMAL, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD0, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD1, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD2, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD3, 1)	},
-
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_DDRIO0, 1)	},
-
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TA, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_THERMAL, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD0, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD1, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD2, 1)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD3, 1)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0,      0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1,      1, IMC1) },
+
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TA,   0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TM,   0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD0, 0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD1, 0, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD2, 1, IMC0) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD3, 1, IMC0) },
+
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TA,   1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TM,   1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD0, 1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD1, 1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD2, 1, IMC1) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD3, 1, IMC1) },
+
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_CBO_SAD0, 0, SOCK) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_CBO_SAD1, 0, SOCK) },
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_DDRIO0,   1, SOCK) },
 };
 
 static const struct pci_id_table pci_dev_descr_broadwell_table[] = {
-	PCI_ID_TABLE_ENTRY(pci_dev_descr_broadwell, BROADWELL),
+	PCI_ID_TABLE_ENTRY(pci_dev_descr_broadwell, 10, 2, BROADWELL),
 	{0,}			/* 0 terminated list. */
 };
 
@@ -709,7 +719,8 @@ static inline int numcol(u32 mtr)
 	return 1 << cols;
 }
 
-static struct sbridge_dev *get_sbridge_dev(u8 bus, int multi_bus)
+static struct sbridge_dev *get_sbridge_dev(u8 bus, enum domain dom, int multi_bus,
+					   struct sbridge_dev *prev)
 {
 	struct sbridge_dev *sbridge_dev;
 
@@ -722,16 +733,19 @@ static struct sbridge_dev *get_sbridge_dev(u8 bus, int multi_bus)
 				struct sbridge_dev, list);
 	}
 
-	list_for_each_entry(sbridge_dev, &sbridge_edac_list, list) {
-		if (sbridge_dev->bus == bus)
+	sbridge_dev = list_entry(prev ? prev->list.next
+				      : sbridge_edac_list.next, struct sbridge_dev, list);
+
+	list_for_each_entry_from(sbridge_dev, &sbridge_edac_list, list) {
+		if (sbridge_dev->bus == bus && (dom == SOCK || dom == sbridge_dev->dom))
 			return sbridge_dev;
 	}
 
 	return NULL;
 }
 
-static struct sbridge_dev *alloc_sbridge_dev(u8 bus,
-					   const struct pci_id_table *table)
+static struct sbridge_dev *alloc_sbridge_dev(u8 bus, enum domain dom,
+					     const struct pci_id_table *table)
 {
 	struct sbridge_dev *sbridge_dev;
 
@@ -739,15 +753,17 @@ static struct sbridge_dev *alloc_sbridge_dev(u8 bus,
 	if (!sbridge_dev)
 		return NULL;
 
-	sbridge_dev->pdev = kzalloc(sizeof(*sbridge_dev->pdev) * table->n_devs,
-				   GFP_KERNEL);
+	sbridge_dev->pdev = kcalloc(table->n_devs_per_imc,
+				    sizeof(*sbridge_dev->pdev),
+				    GFP_KERNEL);
 	if (!sbridge_dev->pdev) {
 		kfree(sbridge_dev);
 		return NULL;
 	}
 
 	sbridge_dev->bus = bus;
-	sbridge_dev->n_devs = table->n_devs;
+	sbridge_dev->dom = dom;
+	sbridge_dev->n_devs = table->n_devs_per_imc;
 	list_add_tail(&sbridge_dev->list, &sbridge_edac_list);
 
 	return sbridge_dev;
@@ -1044,79 +1060,6 @@ static int haswell_chan_hash(int idx, u64 addr)
 	return idx;
 }
 
-/****************************************************************************
-			Memory check routines
- ****************************************************************************/
-static struct pci_dev *get_pdev_same_bus(u8 bus, u32 id)
-{
-	struct pci_dev *pdev = NULL;
-
-	do {
-		pdev = pci_get_device(PCI_VENDOR_ID_INTEL, id, pdev);
-		if (pdev && pdev->bus->number == bus)
-			break;
-	} while (pdev);
-
-	return pdev;
-}
-
-/**
- * check_if_ecc_is_active() - Checks if ECC is active
- * @bus:	Device bus
- * @type:	Memory controller type
- * returns: 0 in case ECC is active, -ENODEV if it can't be determined or
- *	    disabled
- */
-static int check_if_ecc_is_active(const u8 bus, enum type type)
-{
-	struct pci_dev *pdev = NULL;
-	u32 mcmtr, id;
-
-	switch (type) {
-	case IVY_BRIDGE:
-		id = PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TA;
-		break;
-	case HASWELL:
-		id = PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TA;
-		break;
-	case SANDY_BRIDGE:
-		id = PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TA;
-		break;
-	case BROADWELL:
-		id = PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TA;
-		break;
-	case KNIGHTS_LANDING:
-		/*
-		 * KNL doesn't group things by bus the same way
-		 * SB/IB/Haswell does.
-		 */
-		id = PCI_DEVICE_ID_INTEL_KNL_IMC_TA;
-		break;
-	default:
-		return -ENODEV;
-	}
-
-	if (type != KNIGHTS_LANDING)
-		pdev = get_pdev_same_bus(bus, id);
-	else
-		pdev = pci_get_device(PCI_VENDOR_ID_INTEL, id, 0);
-
-	if (!pdev) {
-		sbridge_printk(KERN_ERR, "Couldn't find PCI device "
-					"%04x:%04x! on bus %02d\n",
-					PCI_VENDOR_ID_INTEL, id, bus);
-		return -ENODEV;
-	}
-
-	pci_read_config_dword(pdev,
-			type == KNIGHTS_LANDING ? KNL_MCMTR : MCMTR, &mcmtr);
-	if (!IS_ECC_ENABLED(mcmtr)) {
-		sbridge_printk(KERN_ERR, "ECC is disabled. Aborting\n");
-		return -ENODEV;
-	}
-	return 0;
-}
-
 /* Low bits of TAD limit, and some metadata. */
 static const u32 knl_tad_dram_limit_lo[] = {
 	0x400, 0x500, 0x600, 0x700,
@@ -1587,25 +1530,13 @@ static int knl_get_dimm_capacity(struct sbridge_pvt *pvt, u64 *mc_sizes)
 	return 0;
 }
 
-static int get_dimm_config(struct mem_ctl_info *mci)
+static void get_source_id(struct mem_ctl_info *mci)
 {
 	struct sbridge_pvt *pvt = mci->pvt_info;
-	struct dimm_info *dimm;
-	unsigned i, j, banks, ranks, rows, cols, npages;
-	u64 size;
 	u32 reg;
-	enum edac_type mode;
-	enum mem_type mtype;
-	int channels = pvt->info.type == KNIGHTS_LANDING ?
-		KNL_MAX_CHANNELS : NUM_CHANNELS;
-	u64 knl_mc_sizes[KNL_MAX_CHANNELS];
 
-	if (pvt->info.type == HASWELL || pvt->info.type == BROADWELL) {
-		pci_read_config_dword(pvt->pci_ha0, HASWELL_HASYSDEFEATURE2, &reg);
-		pvt->is_chan_hash = GET_BITFIELD(reg, 21, 21);
-	}
 	if (pvt->info.type == HASWELL || pvt->info.type == BROADWELL ||
-			pvt->info.type == KNIGHTS_LANDING)
+	    pvt->info.type == KNIGHTS_LANDING)
 		pci_read_config_dword(pvt->pci_sad1, SAD_TARGET, &reg);
 	else
 		pci_read_config_dword(pvt->pci_br0, SAD_TARGET, &reg);
@@ -1614,50 +1545,19 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 		pvt->sbridge_dev->source_id = SOURCE_ID_KNL(reg);
 	else
 		pvt->sbridge_dev->source_id = SOURCE_ID(reg);
+}
 
-	pvt->sbridge_dev->node_id = pvt->info.get_node_id(pvt);
-	edac_dbg(0, "mc#%d: Node ID: %d, source ID: %d\n",
-		 pvt->sbridge_dev->mc,
-		 pvt->sbridge_dev->node_id,
-		 pvt->sbridge_dev->source_id);
-
-	/* KNL doesn't support mirroring or lockstep,
-	 * and is always closed page
-	 */
-	if (pvt->info.type == KNIGHTS_LANDING) {
-		mode = EDAC_S4ECD4ED;
-		pvt->is_mirrored = false;
-
-		if (knl_get_dimm_capacity(pvt, knl_mc_sizes) != 0)
-			return -1;
-	} else {
-		pci_read_config_dword(pvt->pci_ras, RASENABLES, &reg);
-		if (IS_MIRROR_ENABLED(reg)) {
-			edac_dbg(0, "Memory mirror is enabled\n");
-			pvt->is_mirrored = true;
-		} else {
-			edac_dbg(0, "Memory mirror is disabled\n");
-			pvt->is_mirrored = false;
-		}
-
-		pci_read_config_dword(pvt->pci_ta, MCMTR, &pvt->info.mcmtr);
-		if (IS_LOCKSTEP_ENABLED(pvt->info.mcmtr)) {
-			edac_dbg(0, "Lockstep is enabled\n");
-			mode = EDAC_S8ECD8ED;
-			pvt->is_lockstep = true;
-		} else {
-			edac_dbg(0, "Lockstep is disabled\n");
-			mode = EDAC_S4ECD4ED;
-			pvt->is_lockstep = false;
-		}
-		if (IS_CLOSE_PG(pvt->info.mcmtr)) {
-			edac_dbg(0, "address map is on closed page mode\n");
-			pvt->is_close_pg = true;
-		} else {
-			edac_dbg(0, "address map is on open page mode\n");
-			pvt->is_close_pg = false;
-		}
-	}
+static int __populate_dimms(struct mem_ctl_info *mci,
+			    u64 knl_mc_sizes[KNL_MAX_CHANNELS],
+			    enum edac_type mode)
+{
+	struct sbridge_pvt *pvt = mci->pvt_info;
+	int channels = pvt->info.type == KNIGHTS_LANDING ? KNL_MAX_CHANNELS
+							 : NUM_CHANNELS;
+	unsigned int i, j, banks, ranks, rows, cols, npages;
+	struct dimm_info *dimm;
+	enum mem_type mtype;
+	u64 size;
 
 	mtype = pvt->info.get_memory_type(pvt);
 	if (mtype == MEM_RDDR3 || mtype == MEM_RDDR4)
@@ -1688,8 +1588,7 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 		}
 
 		for (j = 0; j < max_dimms_per_channel; j++) {
-			dimm = EDAC_DIMM_PTR(mci->layers, mci->dimms, mci->n_layers,
-				       i, j, 0);
+			dimm = EDAC_DIMM_PTR(mci->layers, mci->dimms, mci->n_layers, i, j, 0);
 			if (pvt->info.type == KNIGHTS_LANDING) {
 				pci_read_config_dword(pvt->knl.pci_channel[i],
 					knl_mtr_reg, &mtr);
@@ -1699,6 +1598,12 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 			}
 			edac_dbg(4, "Channel #%d  MTR%d = %x\n", i, j, mtr);
 			if (IS_DIMM_PRESENT(mtr)) {
+				if (!IS_ECC_ENABLED(pvt->info.mcmtr)) {
+					sbridge_printk(KERN_ERR, "CPU SrcID #%d, Ha #%d, Channel #%d has DIMMs, but ECC is disabled\n",
+						       pvt->sbridge_dev->source_id,
+						       pvt->sbridge_dev->dom, i);
+					return -ENODEV;
+				}
 				pvt->channel[i].dimms++;
 
 				ranks = numrank(pvt->info.type, mtr);
@@ -1717,7 +1622,7 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 				npages = MiB_TO_PAGES(size);
 
 				edac_dbg(0, "mc#%d: ha %d channel %d, dimm %d, %lld Mb (%d pages) bank: %d, rank: %d, row: %#x, col: %#x\n",
-					 pvt->sbridge_dev->mc, i/4, i%4, j,
+					 pvt->sbridge_dev->mc, pvt->sbridge_dev->dom, i, j,
 					 size, npages,
 					 banks, ranks, rows, cols);
 
@@ -1727,8 +1632,8 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 				dimm->mtype = mtype;
 				dimm->edac_mode = mode;
 				snprintf(dimm->label, sizeof(dimm->label),
-					 "CPU_SrcID#%u_Ha#%u_Chan#%u_DIMM#%u",
-					 pvt->sbridge_dev->source_id, i/4, i%4, j);
+						 "CPU_SrcID#%u_Ha#%u_Chan#%u_DIMM#%u",
+						 pvt->sbridge_dev->source_id, pvt->sbridge_dev->dom, i, j);
 			}
 		}
 	}
@@ -1736,6 +1641,65 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 	return 0;
 }
 
+static int get_dimm_config(struct mem_ctl_info *mci)
+{
+	struct sbridge_pvt *pvt = mci->pvt_info;
+	u64 knl_mc_sizes[KNL_MAX_CHANNELS];
+	enum edac_type mode;
+	u32 reg;
+
+	if (pvt->info.type == HASWELL || pvt->info.type == BROADWELL) {
+		pci_read_config_dword(pvt->pci_ha, HASWELL_HASYSDEFEATURE2, &reg);
+		pvt->is_chan_hash = GET_BITFIELD(reg, 21, 21);
+	}
+	pvt->sbridge_dev->node_id = pvt->info.get_node_id(pvt);
+	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);
+
+	/* KNL doesn't support mirroring or lockstep,
+	 * and is always closed page
+	 */
+	if (pvt->info.type == KNIGHTS_LANDING) {
+		mode = EDAC_S4ECD4ED;
+		pvt->is_mirrored = false;
+
+		if (knl_get_dimm_capacity(pvt, knl_mc_sizes) != 0)
+			return -1;
+		pci_read_config_dword(pvt->pci_ta, KNL_MCMTR, &pvt->info.mcmtr);
+	} else {
+		pci_read_config_dword(pvt->pci_ras, RASENABLES, &reg);
+		if (IS_MIRROR_ENABLED(reg)) {
+			edac_dbg(0, "Memory mirror is enabled\n");
+			pvt->is_mirrored = true;
+		} else {
+			edac_dbg(0, "Memory mirror is disabled\n");
+			pvt->is_mirrored = false;
+		}
+
+		pci_read_config_dword(pvt->pci_ta, MCMTR, &pvt->info.mcmtr);
+		if (IS_LOCKSTEP_ENABLED(pvt->info.mcmtr)) {
+			edac_dbg(0, "Lockstep is enabled\n");
+			mode = EDAC_S8ECD8ED;
+			pvt->is_lockstep = true;
+		} else {
+			edac_dbg(0, "Lockstep is disabled\n");
+			mode = EDAC_S4ECD4ED;
+			pvt->is_lockstep = false;
+		}
+		if (IS_CLOSE_PG(pvt->info.mcmtr)) {
+			edac_dbg(0, "address map is on closed page mode\n");
+			pvt->is_close_pg = true;
+		} else {
+			edac_dbg(0, "address map is on open page mode\n");
+			pvt->is_close_pg = false;
+		}
+	}
+
+	return __populate_dimms(mci, knl_mc_sizes, mode);
+}
+
 static void get_memory_layout(const struct mem_ctl_info *mci)
 {
 	struct sbridge_pvt *pvt = mci->pvt_info;
@@ -1816,8 +1780,7 @@ static void get_memory_layout(const struct mem_ctl_info *mci)
 	 */
 	prv = 0;
 	for (n_tads = 0; n_tads < MAX_TAD; n_tads++) {
-		pci_read_config_dword(pvt->pci_ha0, tad_dram_rule[n_tads],
-				      &reg);
+		pci_read_config_dword(pvt->pci_ha, tad_dram_rule[n_tads], &reg);
 		limit = TAD_LIMIT(reg);
 		if (limit <= prv)
 			break;
@@ -1899,12 +1862,12 @@ static void get_memory_layout(const struct mem_ctl_info *mci)
 	}
 }
 
-static struct mem_ctl_info *get_mci_for_node_id(u8 node_id)
+static struct mem_ctl_info *get_mci_for_node_id(u8 node_id, u8 ha)
 {
 	struct sbridge_dev *sbridge_dev;
 
 	list_for_each_entry(sbridge_dev, &sbridge_edac_list, list) {
-		if (sbridge_dev->node_id == node_id)
+		if (sbridge_dev->node_id == node_id && sbridge_dev->dom == ha)
 			return sbridge_dev->mci;
 	}
 	return NULL;
@@ -1925,7 +1888,7 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 	int			interleave_mode, shiftup = 0;
 	unsigned		sad_interleave[pvt->info.max_interleave];
 	u32			reg, dram_rule;
-	u8			ch_way, sck_way, pkg, sad_ha = 0, ch_add = 0;
+	u8			ch_way, sck_way, pkg, sad_ha = 0;
 	u32			tad_offset;
 	u32			rir_way;
 	u32			mb, gb;
@@ -2038,13 +2001,10 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		pkg = sad_pkg(pvt->info.interleave_pkg, reg, idx);
 		*socket = sad_pkg_socket(pkg);
 		sad_ha = sad_pkg_ha(pkg);
-		if (sad_ha)
-			ch_add = 4;
 
 		if (a7mode) {
 			/* MCChanShiftUpEnable */
-			pci_read_config_dword(pvt->pci_ha0,
-					      HASWELL_HASYSDEFEATURE2, &reg);
+			pci_read_config_dword(pvt->pci_ha, HASWELL_HASYSDEFEATURE2, &reg);
 			shiftup = GET_BITFIELD(reg, 22, 22);
 		}
 
@@ -2056,8 +2016,6 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		pkg = sad_pkg(pvt->info.interleave_pkg, reg, idx);
 		*socket = sad_pkg_socket(pkg);
 		sad_ha = sad_pkg_ha(pkg);
-		if (sad_ha)
-			ch_add = 4;
 		edac_dbg(0, "SAD interleave package: %d = CPU socket %d, HA %d\n",
 			 idx, *socket, sad_ha);
 	}
@@ -2068,7 +2026,7 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 	 * Move to the proper node structure, in order to access the
 	 * right PCI registers
 	 */
-	new_mci = get_mci_for_node_id(*socket);
+	new_mci = get_mci_for_node_id(*socket, sad_ha);
 	if (!new_mci) {
 		sprintf(msg, "Struct for socket #%u wasn't initialized",
 			*socket);
@@ -2081,14 +2039,7 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 	 * Step 2) Get memory channel
 	 */
 	prv = 0;
-	if (pvt->info.type == SANDY_BRIDGE)
-		pci_ha = pvt->pci_ha0;
-	else {
-		if (sad_ha)
-			pci_ha = pvt->pci_ha1;
-		else
-			pci_ha = pvt->pci_ha0;
-	}
+	pci_ha = pvt->pci_ha;
 	for (n_tads = 0; n_tads < MAX_TAD; n_tads++) {
 		pci_read_config_dword(pci_ha, tad_dram_rule[n_tads], &reg);
 		limit = TAD_LIMIT(reg);
@@ -2139,9 +2090,7 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 	}
 	*channel_mask = 1 << base_ch;
 
-	pci_read_config_dword(pvt->pci_tad[ch_add + base_ch],
-				tad_ch_nilv_offset[n_tads],
-				&tad_offset);
+	pci_read_config_dword(pvt->pci_tad[base_ch], tad_ch_nilv_offset[n_tads], &tad_offset);
 
 	if (pvt->is_mirrored) {
 		*channel_mask |= 1 << ((base_ch + 2) % 4);
@@ -2192,9 +2141,7 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 	 * Step 3) Decode rank
 	 */
 	for (n_rir = 0; n_rir < MAX_RIR_RANGES; n_rir++) {
-		pci_read_config_dword(pvt->pci_tad[ch_add + base_ch],
-				      rir_way_limit[n_rir],
-				      &reg);
+		pci_read_config_dword(pvt->pci_tad[base_ch], rir_way_limit[n_rir], &reg);
 
 		if (!IS_RIR_VALID(reg))
 			continue;
@@ -2222,9 +2169,7 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		idx = (ch_addr >> 13);	/* FIXME: Datasheet says to shift by 15 */
 	idx %= 1 << rir_way;
 
-	pci_read_config_dword(pvt->pci_tad[ch_add + base_ch],
-			      rir_offset[n_rir][idx],
-			      &reg);
+	pci_read_config_dword(pvt->pci_tad[base_ch], rir_offset[n_rir][idx], &reg);
 	*rank = RIR_RNK_TGT(pvt->info.type, reg);
 
 	edac_dbg(0, "RIR#%d: channel address 0x%08Lx < 0x%08Lx, RIR interleave %d, index %d\n",
@@ -2277,10 +2222,11 @@ static int sbridge_get_onedevice(struct pci_dev **prev,
 				 const unsigned devno,
 				 const int multi_bus)
 {
-	struct sbridge_dev *sbridge_dev;
+	struct sbridge_dev *sbridge_dev = NULL;
 	const struct pci_id_descr *dev_descr = &table->descr[devno];
 	struct pci_dev *pdev = NULL;
 	u8 bus = 0;
+	int i = 0;
 
 	sbridge_printk(KERN_DEBUG,
 		"Seeking for: PCI ID %04x:%04x\n",
@@ -2311,9 +2257,14 @@ static int sbridge_get_onedevice(struct pci_dev **prev,
 	}
 	bus = pdev->bus->number;
 
-	sbridge_dev = get_sbridge_dev(bus, multi_bus);
+next_imc:
+	sbridge_dev = get_sbridge_dev(bus, dev_descr->dom, multi_bus, sbridge_dev);
 	if (!sbridge_dev) {
-		sbridge_dev = alloc_sbridge_dev(bus, table);
+
+		if (dev_descr->dom == SOCK)
+			goto out_imc;
+
+		sbridge_dev = alloc_sbridge_dev(bus, dev_descr->dom, table);
 		if (!sbridge_dev) {
 			pci_dev_put(pdev);
 			return -ENOMEM;
@@ -2321,7 +2272,7 @@ static int sbridge_get_onedevice(struct pci_dev **prev,
 		(*num_mc)++;
 	}
 
-	if (sbridge_dev->pdev[devno]) {
+	if (sbridge_dev->pdev[sbridge_dev->i_devs]) {
 		sbridge_printk(KERN_ERR,
 			"Duplicated device for %04x:%04x\n",
 			PCI_VENDOR_ID_INTEL, dev_descr->dev_id);
@@ -2329,8 +2280,16 @@ static int sbridge_get_onedevice(struct pci_dev **prev,
 		return -ENODEV;
 	}
 
-	sbridge_dev->pdev[devno] = pdev;
+	sbridge_dev->pdev[sbridge_dev->i_devs++] = pdev;
+
+	/* pdev belongs to more than one IMC, do extra gets */
+	if (++i > 1)
+		pci_dev_get(pdev);
 
+	if (dev_descr->dom == SOCK && i < table->n_imcs_per_sock)
+		goto next_imc;
+
+out_imc:
 	/* Be sure that the device is enabled */
 	if (unlikely(pci_enable_device(pdev) < 0)) {
 		sbridge_printk(KERN_ERR,
@@ -2374,7 +2333,7 @@ static int sbridge_get_all_devices(u8 *num_mc,
 	if (table->type == KNIGHTS_LANDING)
 		allow_dups = multi_bus = 1;
 	while (table && table->descr) {
-		for (i = 0; i < table->n_devs; i++) {
+		for (i = 0; i < table->n_devs_per_sock; i++) {
 			if (!allow_dups || i == 0 ||
 					table->descr[i].dev_id !=
 						table->descr[i-1].dev_id) {
@@ -2385,7 +2344,7 @@ static int sbridge_get_all_devices(u8 *num_mc,
 							   table, i, multi_bus);
 				if (rc < 0) {
 					if (i == 0) {
-						i = table->n_devs;
+						i = table->n_devs_per_sock;
 						break;
 					}
 					sbridge_put_all_devices();
@@ -2399,6 +2358,13 @@ static int sbridge_get_all_devices(u8 *num_mc,
 	return 0;
 }
 
+/*
+ * Device IDs for {SBRIDGE,IBRIDGE,HASWELL,BROADWELL}_IMC_HA0_TAD0 are in
+ * the format: XXXa. So we can convert from a device to the corresponding
+ * channel like this
+ */
+#define TAD_DEV_TO_CHAN(dev) (((dev) & 0xf) - 0xa)
+
 static int sbridge_mci_bind_devs(struct mem_ctl_info *mci,
 				 struct sbridge_dev *sbridge_dev)
 {
@@ -2423,7 +2389,7 @@ static int sbridge_mci_bind_devs(struct mem_ctl_info *mci,
 			pvt->pci_br0 = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_HA0:
-			pvt->pci_ha0 = pdev;
+			pvt->pci_ha = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TA:
 			pvt->pci_ta = pdev;
@@ -2436,7 +2402,7 @@ static int sbridge_mci_bind_devs(struct mem_ctl_info *mci,
 		case PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD2:
 		case PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD3:
 		{
-			int id = pdev->device - PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD0;
+			int id = TAD_DEV_TO_CHAN(pdev->device);
 			pvt->pci_tad[id] = pdev;
 			saw_chan_mask |= 1 << id;
 		}
@@ -2455,7 +2421,7 @@ static int sbridge_mci_bind_devs(struct mem_ctl_info *mci,
 	}
 
 	/* Check if everything were registered */
-	if (!pvt->pci_sad0 || !pvt->pci_sad1 || !pvt->pci_ha0 ||
+	if (!pvt->pci_sad0 || !pvt->pci_sad1 || !pvt->pci_ha ||
 	    !pvt->pci_ras || !pvt->pci_ta)
 		goto enodev;
 
@@ -2488,19 +2454,26 @@ static int ibridge_mci_bind_devs(struct mem_ctl_info *mci,
 
 		switch (pdev->device) {
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0:
-			pvt->pci_ha0 = pdev;
+		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1:
+			pvt->pci_ha = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TA:
+		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TA:
 			pvt->pci_ta = pdev;
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_RAS:
+		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_RAS:
 			pvt->pci_ras = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD0:
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD1:
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD2:
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD3:
+		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD0:
+		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD1:
+		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD2:
+		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD3:
 		{
-			int id = pdev->device - PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD0;
+			int id = TAD_DEV_TO_CHAN(pdev->device);
 			pvt->pci_tad[id] = pdev;
 			saw_chan_mask |= 1 << id;
 		}
@@ -2520,19 +2493,6 @@ static int ibridge_mci_bind_devs(struct mem_ctl_info *mci,
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_BR1:
 			pvt->pci_br1 = pdev;
 			break;
-		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1:
-			pvt->pci_ha1 = pdev;
-			break;
-		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD0:
-		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD1:
-		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD2:
-		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD3:
-		{
-			int id = pdev->device - PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD0 + 4;
-			pvt->pci_tad[id] = pdev;
-			saw_chan_mask |= 1 << id;
-		}
-			break;
 		default:
 			goto error;
 		}
@@ -2544,13 +2504,12 @@ static int ibridge_mci_bind_devs(struct mem_ctl_info *mci,
 	}
 
 	/* Check if everything were registered */
-	if (!pvt->pci_sad0 || !pvt->pci_ha0 || !pvt->pci_br0 ||
+	if (!pvt->pci_sad0 || !pvt->pci_ha || !pvt->pci_br0 ||
 	    !pvt->pci_br1 || !pvt->pci_ras || !pvt->pci_ta)
 		goto enodev;
 
-	if (saw_chan_mask != 0x0f && /* -EN */
-	    saw_chan_mask != 0x33 && /* -EP */
-	    saw_chan_mask != 0xff)   /* -EX */
+	if (saw_chan_mask != 0x0f && /* -EN/-EX */
+	    saw_chan_mask != 0x03)   /* -EP */
 		goto enodev;
 	return 0;
 
@@ -2593,32 +2552,27 @@ static int haswell_mci_bind_devs(struct mem_ctl_info *mci,
 			pvt->pci_sad1 = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0:
-			pvt->pci_ha0 = pdev;
+		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1:
+			pvt->pci_ha = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TA:
+		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TA:
 			pvt->pci_ta = pdev;
 			break;
-		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_THERMAL:
+		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TM:
+		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TM:
 			pvt->pci_ras = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD0:
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD1:
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD2:
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD3:
-		{
-			int id = pdev->device - PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD0;
-
-			pvt->pci_tad[id] = pdev;
-			saw_chan_mask |= 1 << id;
-		}
-			break;
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD0:
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD1:
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD2:
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD3:
 		{
-			int id = pdev->device - PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD0 + 4;
-
+			int id = TAD_DEV_TO_CHAN(pdev->device);
 			pvt->pci_tad[id] = pdev;
 			saw_chan_mask |= 1 << id;
 		}
@@ -2630,12 +2584,6 @@ static int haswell_mci_bind_devs(struct mem_ctl_info *mci,
 			if (!pvt->pci_ddrio)
 				pvt->pci_ddrio = pdev;
 			break;
-		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1:
-			pvt->pci_ha1 = pdev;
-			break;
-		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TA:
-			pvt->pci_ha1_ta = pdev;
-			break;
 		default:
 			break;
 		}
@@ -2647,13 +2595,12 @@ static int haswell_mci_bind_devs(struct mem_ctl_info *mci,
 	}
 
 	/* Check if everything were registered */
-	if (!pvt->pci_sad0 || !pvt->pci_ha0 || !pvt->pci_sad1 ||
+	if (!pvt->pci_sad0 || !pvt->pci_ha || !pvt->pci_sad1 ||
 	    !pvt->pci_ras  || !pvt->pci_ta || !pvt->info.pci_vtd)
 		goto enodev;
 
-	if (saw_chan_mask != 0x0f && /* -EN */
-	    saw_chan_mask != 0x33 && /* -EP */
-	    saw_chan_mask != 0xff)   /* -EX */
+	if (saw_chan_mask != 0x0f && /* -EN/-EX */
+	    saw_chan_mask != 0x03)   /* -EP */
 		goto enodev;
 	return 0;
 
@@ -2690,30 +2637,27 @@ static int broadwell_mci_bind_devs(struct mem_ctl_info *mci,
 			pvt->pci_sad1 = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0:
-			pvt->pci_ha0 = pdev;
+		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1:
+			pvt->pci_ha = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TA:
+		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TA:
 			pvt->pci_ta = pdev;
 			break;
-		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_THERMAL:
+		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TM:
+		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TM:
 			pvt->pci_ras = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD0:
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD1:
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD2:
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD3:
-		{
-			int id = pdev->device - PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD0;
-			pvt->pci_tad[id] = pdev;
-			saw_chan_mask |= 1 << id;
-		}
-			break;
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD0:
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD1:
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD2:
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD3:
 		{
-			int id = pdev->device - PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD0 + 4;
+			int id = TAD_DEV_TO_CHAN(pdev->device);
 			pvt->pci_tad[id] = pdev;
 			saw_chan_mask |= 1 << id;
 		}
@@ -2721,12 +2665,6 @@ static int broadwell_mci_bind_devs(struct mem_ctl_info *mci,
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_DDRIO0:
 			pvt->pci_ddrio = pdev;
 			break;
-		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1:
-			pvt->pci_ha1 = pdev;
-			break;
-		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TA:
-			pvt->pci_ha1_ta = pdev;
-			break;
 		default:
 			break;
 		}
@@ -2738,13 +2676,12 @@ static int broadwell_mci_bind_devs(struct mem_ctl_info *mci,
 	}
 
 	/* Check if everything were registered */
-	if (!pvt->pci_sad0 || !pvt->pci_ha0 || !pvt->pci_sad1 ||
+	if (!pvt->pci_sad0 || !pvt->pci_ha || !pvt->pci_sad1 ||
 	    !pvt->pci_ras  || !pvt->pci_ta || !pvt->info.pci_vtd)
 		goto enodev;
 
-	if (saw_chan_mask != 0x0f && /* -EN */
-	    saw_chan_mask != 0x33 && /* -EP */
-	    saw_chan_mask != 0xff)   /* -EX */
+	if (saw_chan_mask != 0x0f && /* -EN/-EX */
+	    saw_chan_mask != 0x03)   /* -EP */
 		goto enodev;
 	return 0;
 
@@ -2812,7 +2749,7 @@ static int knl_mci_bind_devs(struct mem_ctl_info *mci,
 			pvt->knl.pci_cha[devidx] = pdev;
 			break;
 
-		case PCI_DEVICE_ID_INTEL_KNL_IMC_CHANNEL:
+		case PCI_DEVICE_ID_INTEL_KNL_IMC_CHAN:
 			devidx = -1;
 
 			/*
@@ -3006,7 +2943,7 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 
 	if (rc < 0)
 		goto err_parsing;
-	new_mci = get_mci_for_node_id(socket);
+	new_mci = get_mci_for_node_id(socket, ha);
 	if (!new_mci) {
 		strcpy(msg, "Error: socket got corrupted!");
 		goto err_parsing;
@@ -3053,7 +2990,7 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 	/* Call the helper to output message */
 	edac_mc_handle_error(tp_event, mci, core_err_cnt,
 			     m->addr >> PAGE_SHIFT, m->addr & ~PAGE_MASK, 0,
-			     4*ha+channel, dimm, -1,
+			     channel, dimm, -1,
 			     optype, msg);
 	return;
 err_parsing:
@@ -3078,7 +3015,7 @@ static int sbridge_mce_check_error(struct notifier_block *nb, unsigned long val,
 	if (edac_get_report_status() == EDAC_REPORTING_DISABLED)
 		return NOTIFY_DONE;
 
-	mci = get_mci_for_node_id(mce->socketid);
+	mci = get_mci_for_node_id(mce->socketid, IMC0);
 	if (!mci)
 		return NOTIFY_DONE;
 	pvt = mci->pvt_info;
@@ -3159,11 +3096,6 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 	struct pci_dev *pdev = sbridge_dev->pdev[0];
 	int rc;
 
-	/* Check the number of active and not disabled channels */
-	rc = check_if_ecc_is_active(sbridge_dev->bus, type);
-	if (unlikely(rc < 0))
-		return rc;
-
 	/* allocate a new MC control structure */
 	layers[0].type = EDAC_MC_LAYER_CHANNEL;
 	layers[0].size = type == KNIGHTS_LANDING ?
@@ -3192,7 +3124,7 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 		MEM_FLAG_DDR4 : MEM_FLAG_DDR3;
 	mci->edac_ctl_cap = EDAC_FLAG_NONE;
 	mci->edac_cap = EDAC_FLAG_NONE;
-	mci->mod_name = "sbridge_edac.c";
+	mci->mod_name = "sb_edac.c";
 	mci->mod_ver = SBRIDGE_REVISION;
 	mci->dev_name = pci_name(pdev);
 	mci->ctl_page_to_phys = NULL;
@@ -3215,12 +3147,14 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 		pvt->info.max_interleave = ARRAY_SIZE(ibridge_interleave_list);
 		pvt->info.interleave_pkg = ibridge_interleave_pkg;
 		pvt->info.get_width = ibridge_get_width;
-		mci->ctl_name = kasprintf(GFP_KERNEL, "Ivy Bridge Socket#%d", mci->mc_idx);
 
 		/* Store pci devices at mci for faster access */
 		rc = ibridge_mci_bind_devs(mci, sbridge_dev);
 		if (unlikely(rc < 0))
 			goto fail0;
+		get_source_id(mci);
+		mci->ctl_name = kasprintf(GFP_KERNEL, "Ivy Bridge SrcID#%d_Ha#%d",
+			pvt->sbridge_dev->source_id, pvt->sbridge_dev->dom);
 		break;
 	case SANDY_BRIDGE:
 		pvt->info.rankcfgr = SB_RANK_CFG_A;
@@ -3238,12 +3172,14 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 		pvt->info.max_interleave = ARRAY_SIZE(sbridge_interleave_list);
 		pvt->info.interleave_pkg = sbridge_interleave_pkg;
 		pvt->info.get_width = sbridge_get_width;
-		mci->ctl_name = kasprintf(GFP_KERNEL, "Sandy Bridge Socket#%d", mci->mc_idx);
 
 		/* Store pci devices at mci for faster access */
 		rc = sbridge_mci_bind_devs(mci, sbridge_dev);
 		if (unlikely(rc < 0))
 			goto fail0;
+		get_source_id(mci);
+		mci->ctl_name = kasprintf(GFP_KERNEL, "Sandy Bridge SrcID#%d_Ha#%d",
+			pvt->sbridge_dev->source_id, pvt->sbridge_dev->dom);
 		break;
 	case HASWELL:
 		/* rankcfgr isn't used */
@@ -3261,12 +3197,14 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 		pvt->info.max_interleave = ARRAY_SIZE(ibridge_interleave_list);
 		pvt->info.interleave_pkg = ibridge_interleave_pkg;
 		pvt->info.get_width = ibridge_get_width;
-		mci->ctl_name = kasprintf(GFP_KERNEL, "Haswell Socket#%d", mci->mc_idx);
 
 		/* Store pci devices at mci for faster access */
 		rc = haswell_mci_bind_devs(mci, sbridge_dev);
 		if (unlikely(rc < 0))
 			goto fail0;
+		get_source_id(mci);
+		mci->ctl_name = kasprintf(GFP_KERNEL, "Haswell SrcID#%d_Ha#%d",
+			pvt->sbridge_dev->source_id, pvt->sbridge_dev->dom);
 		break;
 	case BROADWELL:
 		/* rankcfgr isn't used */
@@ -3284,12 +3222,14 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 		pvt->info.max_interleave = ARRAY_SIZE(ibridge_interleave_list);
 		pvt->info.interleave_pkg = ibridge_interleave_pkg;
 		pvt->info.get_width = broadwell_get_width;
-		mci->ctl_name = kasprintf(GFP_KERNEL, "Broadwell Socket#%d", mci->mc_idx);
 
 		/* Store pci devices at mci for faster access */
 		rc = broadwell_mci_bind_devs(mci, sbridge_dev);
 		if (unlikely(rc < 0))
 			goto fail0;
+		get_source_id(mci);
+		mci->ctl_name = kasprintf(GFP_KERNEL, "Broadwell SrcID#%d_Ha#%d",
+			pvt->sbridge_dev->source_id, pvt->sbridge_dev->dom);
 		break;
 	case KNIGHTS_LANDING:
 		/* pvt->info.rankcfgr == ??? */
@@ -3307,17 +3247,22 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 		pvt->info.max_interleave = ARRAY_SIZE(knl_interleave_list);
 		pvt->info.interleave_pkg = ibridge_interleave_pkg;
 		pvt->info.get_width = knl_get_width;
-		mci->ctl_name = kasprintf(GFP_KERNEL,
-			"Knights Landing Socket#%d", mci->mc_idx);
 
 		rc = knl_mci_bind_devs(mci, sbridge_dev);
 		if (unlikely(rc < 0))
 			goto fail0;
+		get_source_id(mci);
+		mci->ctl_name = kasprintf(GFP_KERNEL, "Knights Landing SrcID#%d_Ha#%d",
+			pvt->sbridge_dev->source_id, pvt->sbridge_dev->dom);
 		break;
 	}
 
 	/* Get dimm basic config and the memory layout */
-	get_dimm_config(mci);
+	rc = get_dimm_config(mci);
+	if (rc < 0) {
+		edac_dbg(0, "MC: failed to get_dimm_config()\n");
+		goto fail;
+	}
 	get_memory_layout(mci);
 
 	/* record ptr to the generic device */
@@ -3327,13 +3272,14 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 	if (unlikely(edac_mc_add_mc(mci))) {
 		edac_dbg(0, "MC: failed edac_mc_add_mc()\n");
 		rc = -EINVAL;
-		goto fail0;
+		goto fail;
 	}
 
 	return 0;
 
-fail0:
+fail:
 	kfree(mci->ctl_name);
+fail0:
 	edac_mc_free(mci);
 	sbridge_dev->mci = NULL;
 	return rc;
diff --git a/drivers/edac/thunderx_edac.c b/drivers/edac/thunderx_edac.c
index 86d585cb6d32..2d352b40ae1c 100644
--- a/drivers/edac/thunderx_edac.c
+++ b/drivers/edac/thunderx_edac.c
@@ -2080,7 +2080,7 @@ static int thunderx_l2c_probe(struct pci_dev *pdev,
 	if (IS_ENABLED(CONFIG_EDAC_DEBUG)) {
 		l2c->debugfs = edac_debugfs_create_dir(pdev->dev.kobj.name);
 
-		thunderx_create_debugfs_nodes(l2c->debugfs, l2c_devattr,
+		ret = thunderx_create_debugfs_nodes(l2c->debugfs, l2c_devattr,
 					      l2c, dfs_entries);
 
 		if (ret != dfs_entries) {
diff --git a/drivers/extcon/Kconfig b/drivers/extcon/Kconfig
index 32f2dc8e4702..6d50071f07d5 100644
--- a/drivers/extcon/Kconfig
+++ b/drivers/extcon/Kconfig
@@ -115,6 +115,7 @@ config EXTCON_PALMAS
 
 config EXTCON_QCOM_SPMI_MISC
 	tristate "Qualcomm USB extcon support"
+	depends on ARCH_QCOM || COMPILE_TEST
 	help
 	  Say Y here to enable SPMI PMIC based USB cable detection
 	  support on Qualcomm PMICs such as PM8941.
diff --git a/drivers/extcon/extcon-arizona.c b/drivers/extcon/extcon-arizona.c
index e2d78cd7030d..f84da4a17724 100644
--- a/drivers/extcon/extcon-arizona.c
+++ b/drivers/extcon/extcon-arizona.c
@@ -1271,9 +1271,7 @@ static int arizona_extcon_get_micd_configs(struct device *dev,
 		goto out;
 
 	nconfs /= entries_per_config;
-
-	micd_configs = devm_kzalloc(dev,
-				    nconfs * sizeof(struct arizona_micd_range),
+	micd_configs = devm_kcalloc(dev, nconfs, sizeof(*micd_configs),
 				    GFP_KERNEL);
 	if (!micd_configs) {
 		ret = -ENOMEM;
diff --git a/drivers/extcon/extcon-intel-int3496.c b/drivers/extcon/extcon-intel-int3496.c
index 9d17984bbbd4..d9f9afe45961 100644
--- a/drivers/extcon/extcon-intel-int3496.c
+++ b/drivers/extcon/extcon-intel-int3496.c
@@ -94,8 +94,7 @@ static int int3496_probe(struct platform_device *pdev)
 	struct int3496_data *data;
 	int ret;
 
-	ret = acpi_dev_add_driver_gpios(ACPI_COMPANION(dev),
-					acpi_int3496_default_gpios);
+	ret = devm_acpi_dev_add_driver_gpios(dev, acpi_int3496_default_gpios);
 	if (ret) {
 		dev_err(dev, "can't add GPIO ACPI mapping\n");
 		return ret;
@@ -169,8 +168,6 @@ static int int3496_remove(struct platform_device *pdev)
 	devm_free_irq(&pdev->dev, data->usb_id_irq, data);
 	cancel_delayed_work_sync(&data->work);
 
-	acpi_dev_remove_driver_gpios(ACPI_COMPANION(&pdev->dev));
-
 	return 0;
 }
 
diff --git a/drivers/extcon/extcon.c b/drivers/extcon/extcon.c
index f422a78ba342..8eccf7b14937 100644
--- a/drivers/extcon/extcon.c
+++ b/drivers/extcon/extcon.c
@@ -964,12 +964,12 @@ EXPORT_SYMBOL_GPL(extcon_unregister_notifier);
 
 /**
  * extcon_register_notifier_all() - Register a notifier block for all connectors
- * @edev:	the extcon device that has the external connecotr.
+ * @edev:	the extcon device that has the external connector.
  * @nb:		a notifier block to be registered.
  *
- * This fucntion registers a notifier block in order to receive the state
+ * This function registers a notifier block in order to receive the state
  * change of all supported external connectors from extcon device.
- * And The second parameter given to the callback of nb (val) is
+ * And the second parameter given to the callback of nb (val) is
  * the current state and third parameter is the edev pointer.
  *
  * Returns 0 if success or error number if fail
@@ -1252,9 +1252,8 @@ int extcon_dev_register(struct extcon_dev *edev)
 	}
 
 	spin_lock_init(&edev->lock);
-
-	edev->nh = devm_kzalloc(&edev->dev,
-			sizeof(*edev->nh) * edev->max_supported, GFP_KERNEL);
+	edev->nh = devm_kcalloc(&edev->dev, edev->max_supported,
+				sizeof(*edev->nh), GFP_KERNEL);
 	if (!edev->nh) {
 		ret = -ENOMEM;
 		goto err_dev;
diff --git a/drivers/firewire/net.c b/drivers/firewire/net.c
index 5d3640264f2d..242359c2d1f1 100644
--- a/drivers/firewire/net.c
+++ b/drivers/firewire/net.c
@@ -219,7 +219,7 @@ static int fwnet_header_create(struct sk_buff *skb, struct net_device *net,
 {
 	struct fwnet_header *h;
 
-	h = (struct fwnet_header *)skb_push(skb, sizeof(*h));
+	h = skb_push(skb, sizeof(*h));
 	put_unaligned_be16(type, &h->h_proto);
 
 	if (net->flags & (IFF_LOOPBACK | IFF_NOARP)) {
@@ -600,7 +600,7 @@ static int fwnet_incoming_packet(struct fwnet_device *dev, __be32 *buf, int len,
 			return -ENOMEM;
 		}
 		skb_reserve(skb, LL_RESERVED_SPACE(net));
-		memcpy(skb_put(skb, len), buf, len);
+		skb_put_data(skb, buf, len);
 
 		return fwnet_finish_incoming_packet(net, skb, source_node_id,
 						    is_broadcast, ether_type);
@@ -961,16 +961,14 @@ static int fwnet_send_packet(struct fwnet_packet_task *ptask)
 	tx_len = ptask->max_payload;
 	switch (fwnet_get_hdr_lf(&ptask->hdr)) {
 	case RFC2374_HDR_UNFRAG:
-		bufhdr = (struct rfc2734_header *)
-				skb_push(ptask->skb, RFC2374_UNFRAG_HDR_SIZE);
+		bufhdr = skb_push(ptask->skb, RFC2374_UNFRAG_HDR_SIZE);
 		put_unaligned_be32(ptask->hdr.w0, &bufhdr->w0);
 		break;
 
 	case RFC2374_HDR_FIRSTFRAG:
 	case RFC2374_HDR_INTFRAG:
 	case RFC2374_HDR_LASTFRAG:
-		bufhdr = (struct rfc2734_header *)
-				skb_push(ptask->skb, RFC2374_FRAG_HDR_SIZE);
+		bufhdr = skb_push(ptask->skb, RFC2374_FRAG_HDR_SIZE);
 		put_unaligned_be32(ptask->hdr.w0, &bufhdr->w0);
 		put_unaligned_be32(ptask->hdr.w1, &bufhdr->w1);
 		break;
diff --git a/drivers/firmware/arm_scpi.c b/drivers/firmware/arm_scpi.c
index f6cfc31d34c7..8043e51de897 100644
--- a/drivers/firmware/arm_scpi.c
+++ b/drivers/firmware/arm_scpi.c
@@ -39,6 +39,7 @@
 #include <linux/of_address.h>
 #include <linux/of_platform.h>
 #include <linux/printk.h>
+#include <linux/pm_opp.h>
 #include <linux/scpi_protocol.h>
 #include <linux/slab.h>
 #include <linux/sort.h>
@@ -684,6 +685,65 @@ static struct scpi_dvfs_info *scpi_dvfs_get_info(u8 domain)
 	return info;
 }
 
+static int scpi_dev_domain_id(struct device *dev)
+{
+	struct of_phandle_args clkspec;
+
+	if (of_parse_phandle_with_args(dev->of_node, "clocks", "#clock-cells",
+				       0, &clkspec))
+		return -EINVAL;
+
+	return clkspec.args[0];
+}
+
+static struct scpi_dvfs_info *scpi_dvfs_info(struct device *dev)
+{
+	int domain = scpi_dev_domain_id(dev);
+
+	if (domain < 0)
+		return ERR_PTR(domain);
+
+	return scpi_dvfs_get_info(domain);
+}
+
+static int scpi_dvfs_get_transition_latency(struct device *dev)
+{
+	struct scpi_dvfs_info *info = scpi_dvfs_info(dev);
+
+	if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	if (!info->latency)
+		return 0;
+
+	return info->latency;
+}
+
+static int scpi_dvfs_add_opps_to_device(struct device *dev)
+{
+	int idx, ret;
+	struct scpi_opp *opp;
+	struct scpi_dvfs_info *info = scpi_dvfs_info(dev);
+
+	if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	if (!info->opps)
+		return -EIO;
+
+	for (opp = info->opps, idx = 0; idx < info->count; idx++, opp++) {
+		ret = dev_pm_opp_add(dev, opp->freq, opp->m_volt * 1000);
+		if (ret) {
+			dev_warn(dev, "failed to add opp %uHz %umV\n",
+				 opp->freq, opp->m_volt);
+			while (idx-- > 0)
+				dev_pm_opp_remove(dev, (--opp)->freq);
+			return ret;
+		}
+	}
+	return 0;
+}
+
 static int scpi_sensor_get_capability(u16 *sensors)
 {
 	struct sensor_capabilities cap_buf;
@@ -765,6 +825,9 @@ static struct scpi_ops scpi_ops = {
 	.dvfs_get_idx = scpi_dvfs_get_idx,
 	.dvfs_set_idx = scpi_dvfs_set_idx,
 	.dvfs_get_info = scpi_dvfs_get_info,
+	.device_domain_id = scpi_dev_domain_id,
+	.get_transition_latency = scpi_dvfs_get_transition_latency,
+	.add_opps_to_device = scpi_dvfs_add_opps_to_device,
 	.sensor_get_capability = scpi_sensor_get_capability,
 	.sensor_get_info = scpi_sensor_get_info,
 	.sensor_get_value = scpi_sensor_get_value,
diff --git a/drivers/firmware/dmi-id.c b/drivers/firmware/dmi-id.c
index dc269cb288c2..951b6c79f166 100644
--- a/drivers/firmware/dmi-id.c
+++ b/drivers/firmware/dmi-id.c
@@ -47,7 +47,7 @@ DEFINE_DMI_ATTR_WITH_SHOW(product_name,		0444, DMI_PRODUCT_NAME);
 DEFINE_DMI_ATTR_WITH_SHOW(product_version,	0444, DMI_PRODUCT_VERSION);
 DEFINE_DMI_ATTR_WITH_SHOW(product_serial,	0400, DMI_PRODUCT_SERIAL);
 DEFINE_DMI_ATTR_WITH_SHOW(product_uuid,		0400, DMI_PRODUCT_UUID);
-DEFINE_DMI_ATTR_WITH_SHOW(product_family,	0400, DMI_PRODUCT_FAMILY);
+DEFINE_DMI_ATTR_WITH_SHOW(product_family,	0444, DMI_PRODUCT_FAMILY);
 DEFINE_DMI_ATTR_WITH_SHOW(board_vendor,		0444, DMI_BOARD_VENDOR);
 DEFINE_DMI_ATTR_WITH_SHOW(board_name,		0444, DMI_BOARD_NAME);
 DEFINE_DMI_ATTR_WITH_SHOW(board_version,	0444, DMI_BOARD_VERSION);
@@ -192,7 +192,7 @@ static void __init dmi_id_init_attr_table(void)
 	ADD_DMI_ATTR(product_version,   DMI_PRODUCT_VERSION);
 	ADD_DMI_ATTR(product_serial,    DMI_PRODUCT_SERIAL);
 	ADD_DMI_ATTR(product_uuid,      DMI_PRODUCT_UUID);
-	ADD_DMI_ATTR(product_family,      DMI_PRODUCT_FAMILY);
+	ADD_DMI_ATTR(product_family,    DMI_PRODUCT_FAMILY);
 	ADD_DMI_ATTR(board_vendor,      DMI_BOARD_VENDOR);
 	ADD_DMI_ATTR(board_name,        DMI_BOARD_NAME);
 	ADD_DMI_ATTR(board_version,     DMI_BOARD_VERSION);
diff --git a/drivers/firmware/dmi_scan.c b/drivers/firmware/dmi_scan.c
index 93f7acdaac7a..783041964439 100644
--- a/drivers/firmware/dmi_scan.c
+++ b/drivers/firmware/dmi_scan.c
@@ -144,7 +144,7 @@ static int __init dmi_walk_early(void (*decode)(const struct dmi_header *,
 
 	buf = dmi_early_remap(dmi_base, orig_dmi_len);
 	if (buf == NULL)
-		return -1;
+		return -ENOMEM;
 
 	dmi_decode_table(buf, decode, NULL);
 
@@ -178,7 +178,7 @@ static void __init dmi_save_ident(const struct dmi_header *dm, int slot,
 	const char *d = (const char *) dm;
 	const char *p;
 
-	if (dmi_ident[slot])
+	if (dmi_ident[slot] || dm->length <= string)
 		return;
 
 	p = dmi_string(dm, d[string]);
@@ -191,13 +191,14 @@ static void __init dmi_save_ident(const struct dmi_header *dm, int slot,
 static void __init dmi_save_uuid(const struct dmi_header *dm, int slot,
 		int index)
 {
-	const u8 *d = (u8 *) dm + index;
+	const u8 *d;
 	char *s;
 	int is_ff = 1, is_00 = 1, i;
 
-	if (dmi_ident[slot])
+	if (dmi_ident[slot] || dm->length <= index + 16)
 		return;
 
+	d = (u8 *) dm + index;
 	for (i = 0; i < 16 && (is_ff || is_00); i++) {
 		if (d[i] != 0x00)
 			is_00 = 0;
@@ -228,16 +229,17 @@ static void __init dmi_save_uuid(const struct dmi_header *dm, int slot,
 static void __init dmi_save_type(const struct dmi_header *dm, int slot,
 		int index)
 {
-	const u8 *d = (u8 *) dm + index;
+	const u8 *d;
 	char *s;
 
-	if (dmi_ident[slot])
+	if (dmi_ident[slot] || dm->length <= index)
 		return;
 
 	s = dmi_alloc(4);
 	if (!s)
 		return;
 
+	d = (u8 *) dm + index;
 	sprintf(s, "%u", *d & 0x7F);
 	dmi_ident[slot] = s;
 }
@@ -278,9 +280,13 @@ static void __init dmi_save_devices(const struct dmi_header *dm)
 
 static void __init dmi_save_oem_strings_devices(const struct dmi_header *dm)
 {
-	int i, count = *(u8 *)(dm + 1);
+	int i, count;
 	struct dmi_device *dev;
 
+	if (dm->length < 0x05)
+		return;
+
+	count = *(u8 *)(dm + 1);
 	for (i = 1; i <= count; i++) {
 		const char *devname = dmi_string(dm, i);
 
@@ -353,6 +359,9 @@ static void __init dmi_save_extended_devices(const struct dmi_header *dm)
 	const char *name;
 	const u8 *d = (u8 *)dm;
 
+	if (dm->length < 0x0B)
+		return;
+
 	/* Skip disabled device */
 	if ((d[0x5] & 0x80) == 0)
 		return;
@@ -387,7 +396,7 @@ static void __init save_mem_devices(const struct dmi_header *dm, void *v)
 	const char *d = (const char *)dm;
 	static int nr;
 
-	if (dm->type != DMI_ENTRY_MEM_DEVICE)
+	if (dm->type != DMI_ENTRY_MEM_DEVICE || dm->length < 0x12)
 		return;
 	if (nr >= dmi_memdev_nr) {
 		pr_warn(FW_BUG "Too many DIMM entries in SMBIOS table\n");
@@ -650,6 +659,21 @@ void __init dmi_scan_machine(void)
 			goto error;
 
 		/*
+		 * Same logic as above, look for a 64-bit entry point
+		 * first, and if not found, fall back to 32-bit entry point.
+		 */
+		memcpy_fromio(buf, p, 16);
+		for (q = p + 16; q < p + 0x10000; q += 16) {
+			memcpy_fromio(buf + 16, q, 16);
+			if (!dmi_smbios3_present(buf)) {
+				dmi_available = 1;
+				dmi_early_unmap(p, 0x10000);
+				goto out;
+			}
+			memcpy(buf, buf + 16, 16);
+		}
+
+		/*
 		 * Iterate over all possible DMI header addresses q.
 		 * Maintain the 32 bytes around q in buf.  On the
 		 * first iteration, substitute zero for the
@@ -659,7 +683,7 @@ void __init dmi_scan_machine(void)
 		memset(buf, 0, 16);
 		for (q = p; q < p + 0x10000; q += 16) {
 			memcpy_fromio(buf + 16, q, 16);
-			if (!dmi_smbios3_present(buf) || !dmi_present(buf)) {
+			if (!dmi_present(buf)) {
 				dmi_available = 1;
 				dmi_early_unmap(p, 0x10000);
 				goto out;
@@ -993,7 +1017,8 @@ EXPORT_SYMBOL(dmi_get_date);
  *	@decode: Callback function
  *	@private_data: Private data to be passed to the callback function
  *
- *	Returns -1 when the DMI table can't be reached, 0 on success.
+ *	Returns 0 on success, -ENXIO if DMI is not selected or not present,
+ *	or a different negative error code if DMI walking fails.
  */
 int dmi_walk(void (*decode)(const struct dmi_header *, void *),
 	     void *private_data)
@@ -1001,11 +1026,11 @@ int dmi_walk(void (*decode)(const struct dmi_header *, void *),
 	u8 *buf;
 
 	if (!dmi_available)
-		return -1;
+		return -ENXIO;
 
 	buf = dmi_remap(dmi_base, dmi_len);
 	if (buf == NULL)
-		return -1;
+		return -ENOMEM;
 
 	dmi_decode_table(buf, decode, private_data);
 
diff --git a/drivers/firmware/efi/Kconfig b/drivers/firmware/efi/Kconfig
index 2e78b0b96d74..394db40ed374 100644
--- a/drivers/firmware/efi/Kconfig
+++ b/drivers/firmware/efi/Kconfig
@@ -112,6 +112,15 @@ config EFI_CAPSULE_LOADER
 
 	  Most users should say N.
 
+config EFI_CAPSULE_QUIRK_QUARK_CSH
+	boolean "Add support for Quark capsules with non-standard headers"
+	depends on X86 && !64BIT
+	select EFI_CAPSULE_LOADER
+	default y
+	help
+	  Add support for processing Quark X1000 EFI capsules, whose header
+	  layout deviates from the layout mandated by the UEFI specification.
+
 config EFI_TEST
 	tristate "EFI Runtime Service Tests Support"
 	depends on EFI
diff --git a/drivers/firmware/efi/arm-runtime.c b/drivers/firmware/efi/arm-runtime.c
index 974c5a31a005..1cc41c3d6315 100644
--- a/drivers/firmware/efi/arm-runtime.c
+++ b/drivers/firmware/efi/arm-runtime.c
@@ -11,6 +11,7 @@
  *
  */
 
+#include <linux/dmi.h>
 #include <linux/efi.h>
 #include <linux/io.h>
 #include <linux/memblock.h>
@@ -166,3 +167,18 @@ void efi_virtmap_unload(void)
 	efi_set_pgd(current->active_mm);
 	preempt_enable();
 }
+
+
+static int __init arm_dmi_init(void)
+{
+	/*
+	 * On arm64/ARM, DMI depends on UEFI, and dmi_scan_machine() needs to
+	 * be called early because dmi_id_init(), which is an arch_initcall
+	 * itself, depends on dmi_scan_machine() having been called already.
+	 */
+	dmi_scan_machine();
+	if (dmi_available)
+		dmi_set_dump_stack_arch_desc();
+	return 0;
+}
+core_initcall(arm_dmi_init);
diff --git a/drivers/firmware/efi/capsule-loader.c b/drivers/firmware/efi/capsule-loader.c
index 9ae6c116c474..ec8ac5c4dd84 100644
--- a/drivers/firmware/efi/capsule-loader.c
+++ b/drivers/firmware/efi/capsule-loader.c
@@ -20,15 +20,9 @@
 
 #define NO_FURTHER_WRITE_ACTION -1
 
-struct capsule_info {
-	bool		header_obtained;
-	int		reset_type;
-	long		index;
-	size_t		count;
-	size_t		total_size;
-	struct page	**pages;
-	size_t		page_bytes_remain;
-};
+#ifndef phys_to_page
+#define phys_to_page(x)		pfn_to_page((x) >> PAGE_SHIFT)
+#endif
 
 /**
  * efi_free_all_buff_pages - free all previous allocated buffer pages
@@ -41,65 +35,70 @@ struct capsule_info {
 static void efi_free_all_buff_pages(struct capsule_info *cap_info)
 {
 	while (cap_info->index > 0)
-		__free_page(cap_info->pages[--cap_info->index]);
+		__free_page(phys_to_page(cap_info->pages[--cap_info->index]));
 
 	cap_info->index = NO_FURTHER_WRITE_ACTION;
 }
 
-/**
- * efi_capsule_setup_info - obtain the efi capsule header in the binary and
- *			    setup capsule_info structure
- * @cap_info: pointer to current instance of capsule_info structure
- * @kbuff: a mapped first page buffer pointer
- * @hdr_bytes: the total received number of bytes for efi header
- **/
-static ssize_t efi_capsule_setup_info(struct capsule_info *cap_info,
-				      void *kbuff, size_t hdr_bytes)
+int __efi_capsule_setup_info(struct capsule_info *cap_info)
 {
-	efi_capsule_header_t *cap_hdr;
 	size_t pages_needed;
 	int ret;
 	void *temp_page;
 
-	/* Only process data block that is larger than efi header size */
-	if (hdr_bytes < sizeof(efi_capsule_header_t))
-		return 0;
-
-	/* Reset back to the correct offset of header */
-	cap_hdr = kbuff - cap_info->count;
-	pages_needed = ALIGN(cap_hdr->imagesize, PAGE_SIZE) >> PAGE_SHIFT;
+	pages_needed = ALIGN(cap_info->total_size, PAGE_SIZE) / PAGE_SIZE;
 
 	if (pages_needed == 0) {
-		pr_err("%s: pages count invalid\n", __func__);
+		pr_err("invalid capsule size");
 		return -EINVAL;
 	}
 
 	/* Check if the capsule binary supported */
-	ret = efi_capsule_supported(cap_hdr->guid, cap_hdr->flags,
-				    cap_hdr->imagesize,
+	ret = efi_capsule_supported(cap_info->header.guid,
+				    cap_info->header.flags,
+				    cap_info->header.imagesize,
 				    &cap_info->reset_type);
 	if (ret) {
-		pr_err("%s: efi_capsule_supported() failed\n",
-		       __func__);
+		pr_err("capsule not supported\n");
 		return ret;
 	}
 
-	cap_info->total_size = cap_hdr->imagesize;
 	temp_page = krealloc(cap_info->pages,
 			     pages_needed * sizeof(void *),
 			     GFP_KERNEL | __GFP_ZERO);
-	if (!temp_page) {
-		pr_debug("%s: krealloc() failed\n", __func__);
+	if (!temp_page)
 		return -ENOMEM;
-	}
 
 	cap_info->pages = temp_page;
-	cap_info->header_obtained = true;
 
 	return 0;
 }
 
 /**
+ * efi_capsule_setup_info - obtain the efi capsule header in the binary and
+ *			    setup capsule_info structure
+ * @cap_info: pointer to current instance of capsule_info structure
+ * @kbuff: a mapped first page buffer pointer
+ * @hdr_bytes: the total received number of bytes for efi header
+ *
+ * Platforms with non-standard capsule update mechanisms can override
+ * this __weak function so they can perform any required capsule
+ * image munging. See quark_quirk_function() for an example.
+ **/
+int __weak efi_capsule_setup_info(struct capsule_info *cap_info, void *kbuff,
+				  size_t hdr_bytes)
+{
+	/* Only process data block that is larger than efi header size */
+	if (hdr_bytes < sizeof(efi_capsule_header_t))
+		return 0;
+
+	memcpy(&cap_info->header, kbuff, sizeof(cap_info->header));
+	cap_info->total_size = cap_info->header.imagesize;
+
+	return __efi_capsule_setup_info(cap_info);
+}
+
+/**
  * efi_capsule_submit_update - invoke the efi_capsule_update API once binary
  *			       upload done
  * @cap_info: pointer to current instance of capsule_info structure
@@ -107,26 +106,17 @@ static ssize_t efi_capsule_setup_info(struct capsule_info *cap_info,
 static ssize_t efi_capsule_submit_update(struct capsule_info *cap_info)
 {
 	int ret;
-	void *cap_hdr_temp;
-
-	cap_hdr_temp = vmap(cap_info->pages, cap_info->index,
-			VM_MAP, PAGE_KERNEL);
-	if (!cap_hdr_temp) {
-		pr_debug("%s: vmap() failed\n", __func__);
-		return -EFAULT;
-	}
 
-	ret = efi_capsule_update(cap_hdr_temp, cap_info->pages);
-	vunmap(cap_hdr_temp);
+	ret = efi_capsule_update(&cap_info->header, cap_info->pages);
 	if (ret) {
-		pr_err("%s: efi_capsule_update() failed\n", __func__);
+		pr_err("capsule update failed\n");
 		return ret;
 	}
 
 	/* Indicate capsule binary uploading is done */
 	cap_info->index = NO_FURTHER_WRITE_ACTION;
-	pr_info("%s: Successfully upload capsule file with reboot type '%s'\n",
-		__func__, !cap_info->reset_type ? "RESET_COLD" :
+	pr_info("Successfully upload capsule file with reboot type '%s'\n",
+		!cap_info->reset_type ? "RESET_COLD" :
 		cap_info->reset_type == 1 ? "RESET_WARM" :
 		"RESET_SHUTDOWN");
 	return 0;
@@ -171,37 +161,30 @@ static ssize_t efi_capsule_write(struct file *file, const char __user *buff,
 	if (!cap_info->page_bytes_remain) {
 		page = alloc_page(GFP_KERNEL);
 		if (!page) {
-			pr_debug("%s: alloc_page() failed\n", __func__);
 			ret = -ENOMEM;
 			goto failed;
 		}
 
-		cap_info->pages[cap_info->index++] = page;
+		cap_info->pages[cap_info->index++] = page_to_phys(page);
 		cap_info->page_bytes_remain = PAGE_SIZE;
+	} else {
+		page = phys_to_page(cap_info->pages[cap_info->index - 1]);
 	}
 
-	page = cap_info->pages[cap_info->index - 1];
-
 	kbuff = kmap(page);
-	if (!kbuff) {
-		pr_debug("%s: kmap() failed\n", __func__);
-		ret = -EFAULT;
-		goto failed;
-	}
 	kbuff += PAGE_SIZE - cap_info->page_bytes_remain;
 
 	/* Copy capsule binary data from user space to kernel space buffer */
 	write_byte = min_t(size_t, count, cap_info->page_bytes_remain);
 	if (copy_from_user(kbuff, buff, write_byte)) {
-		pr_debug("%s: copy_from_user() failed\n", __func__);
 		ret = -EFAULT;
 		goto fail_unmap;
 	}
 	cap_info->page_bytes_remain -= write_byte;
 
 	/* Setup capsule binary info structure */
-	if (!cap_info->header_obtained) {
-		ret = efi_capsule_setup_info(cap_info, kbuff,
+	if (cap_info->header.headersize == 0) {
+		ret = efi_capsule_setup_info(cap_info, kbuff - cap_info->count,
 					     cap_info->count + write_byte);
 		if (ret)
 			goto fail_unmap;
@@ -211,11 +194,10 @@ static ssize_t efi_capsule_write(struct file *file, const char __user *buff,
 	kunmap(page);
 
 	/* Submit the full binary to efi_capsule_update() API */
-	if (cap_info->header_obtained &&
+	if (cap_info->header.headersize > 0 &&
 	    cap_info->count >= cap_info->total_size) {
 		if (cap_info->count > cap_info->total_size) {
-			pr_err("%s: upload size exceeded header defined size\n",
-			       __func__);
+			pr_err("capsule upload size exceeded header defined size\n");
 			ret = -EINVAL;
 			goto failed;
 		}
@@ -249,7 +231,7 @@ static int efi_capsule_flush(struct file *file, fl_owner_t id)
 	struct capsule_info *cap_info = file->private_data;
 
 	if (cap_info->index > 0) {
-		pr_err("%s: capsule upload not complete\n", __func__);
+		pr_err("capsule upload not complete\n");
 		efi_free_all_buff_pages(cap_info);
 		ret = -ECANCELED;
 	}
@@ -328,8 +310,7 @@ static int __init efi_capsule_loader_init(void)
 
 	ret = misc_register(&efi_capsule_misc);
 	if (ret)
-		pr_err("%s: Failed to register misc char file note\n",
-		       __func__);
+		pr_err("Unable to register capsule loader device\n");
 
 	return ret;
 }
diff --git a/drivers/firmware/efi/capsule.c b/drivers/firmware/efi/capsule.c
index 6eedff45e6d7..901b9306bf94 100644
--- a/drivers/firmware/efi/capsule.c
+++ b/drivers/firmware/efi/capsule.c
@@ -214,7 +214,7 @@ efi_capsule_update_locked(efi_capsule_header_t *capsule,
  *
  * Return 0 on success, a converted EFI status code on failure.
  */
-int efi_capsule_update(efi_capsule_header_t *capsule, struct page **pages)
+int efi_capsule_update(efi_capsule_header_t *capsule, phys_addr_t *pages)
 {
 	u32 imagesize = capsule->imagesize;
 	efi_guid_t guid = capsule->guid;
@@ -247,16 +247,13 @@ int efi_capsule_update(efi_capsule_header_t *capsule, struct page **pages)
 		efi_capsule_block_desc_t *sglist;
 
 		sglist = kmap(sg_pages[i]);
-		if (!sglist) {
-			rv = -ENOMEM;
-			goto out;
-		}
 
 		for (j = 0; j < SGLIST_PER_PAGE && count > 0; j++) {
-			u64 sz = min_t(u64, imagesize, PAGE_SIZE);
+			u64 sz = min_t(u64, imagesize,
+				       PAGE_SIZE - (u64)*pages % PAGE_SIZE);
 
 			sglist[j].length = sz;
-			sglist[j].data = page_to_phys(*pages++);
+			sglist[j].data = *pages++;
 
 			imagesize -= sz;
 			count--;
diff --git a/drivers/firmware/efi/cper.c b/drivers/firmware/efi/cper.c
index d42537425438..48a8f69da42a 100644
--- a/drivers/firmware/efi/cper.c
+++ b/drivers/firmware/efi/cper.c
@@ -32,6 +32,10 @@
 #include <linux/acpi.h>
 #include <linux/pci.h>
 #include <linux/aer.h>
+#include <linux/printk.h>
+#include <linux/bcd.h>
+#include <acpi/ghes.h>
+#include <ras/ras_event.h>
 
 #define INDENT_SP	" "
 
@@ -107,12 +111,15 @@ void cper_print_bits(const char *pfx, unsigned int bits,
 static const char * const proc_type_strs[] = {
 	"IA32/X64",
 	"IA64",
+	"ARM",
 };
 
 static const char * const proc_isa_strs[] = {
 	"IA32",
 	"IA64",
 	"X64",
+	"ARM A32/T32",
+	"ARM A64",
 };
 
 static const char * const proc_error_type_strs[] = {
@@ -181,6 +188,122 @@ static void cper_print_proc_generic(const char *pfx,
 		printk("%s""IP: 0x%016llx\n", pfx, proc->ip);
 }
 
+#if defined(CONFIG_ARM64) || defined(CONFIG_ARM)
+static const char * const arm_reg_ctx_strs[] = {
+	"AArch32 general purpose registers",
+	"AArch32 EL1 context registers",
+	"AArch32 EL2 context registers",
+	"AArch32 secure context registers",
+	"AArch64 general purpose registers",
+	"AArch64 EL1 context registers",
+	"AArch64 EL2 context registers",
+	"AArch64 EL3 context registers",
+	"Misc. system register structure",
+};
+
+static void cper_print_proc_arm(const char *pfx,
+				const struct cper_sec_proc_arm *proc)
+{
+	int i, len, max_ctx_type;
+	struct cper_arm_err_info *err_info;
+	struct cper_arm_ctx_info *ctx_info;
+	char newpfx[64];
+
+	printk("%sMIDR: 0x%016llx\n", pfx, proc->midr);
+
+	len = proc->section_length - (sizeof(*proc) +
+		proc->err_info_num * (sizeof(*err_info)));
+	if (len < 0) {
+		printk("%ssection length: %d\n", pfx, proc->section_length);
+		printk("%ssection length is too small\n", pfx);
+		printk("%sfirmware-generated error record is incorrect\n", pfx);
+		printk("%sERR_INFO_NUM is %d\n", pfx, proc->err_info_num);
+		return;
+	}
+
+	if (proc->validation_bits & CPER_ARM_VALID_MPIDR)
+		printk("%sMultiprocessor Affinity Register (MPIDR): 0x%016llx\n",
+			pfx, proc->mpidr);
+
+	if (proc->validation_bits & CPER_ARM_VALID_AFFINITY_LEVEL)
+		printk("%serror affinity level: %d\n", pfx,
+			proc->affinity_level);
+
+	if (proc->validation_bits & CPER_ARM_VALID_RUNNING_STATE) {
+		printk("%srunning state: 0x%x\n", pfx, proc->running_state);
+		printk("%sPower State Coordination Interface state: %d\n",
+			pfx, proc->psci_state);
+	}
+
+	snprintf(newpfx, sizeof(newpfx), "%s%s", pfx, INDENT_SP);
+
+	err_info = (struct cper_arm_err_info *)(proc + 1);
+	for (i = 0; i < proc->err_info_num; i++) {
+		printk("%sError info structure %d:\n", pfx, i);
+
+		printk("%snum errors: %d\n", pfx, err_info->multiple_error + 1);
+
+		if (err_info->validation_bits & CPER_ARM_INFO_VALID_FLAGS) {
+			if (err_info->flags & CPER_ARM_INFO_FLAGS_FIRST)
+				printk("%sfirst error captured\n", newpfx);
+			if (err_info->flags & CPER_ARM_INFO_FLAGS_LAST)
+				printk("%slast error captured\n", newpfx);
+			if (err_info->flags & CPER_ARM_INFO_FLAGS_PROPAGATED)
+				printk("%spropagated error captured\n",
+				       newpfx);
+			if (err_info->flags & CPER_ARM_INFO_FLAGS_OVERFLOW)
+				printk("%soverflow occurred, error info is incomplete\n",
+				       newpfx);
+		}
+
+		printk("%serror_type: %d, %s\n", newpfx, err_info->type,
+			err_info->type < ARRAY_SIZE(proc_error_type_strs) ?
+			proc_error_type_strs[err_info->type] : "unknown");
+		if (err_info->validation_bits & CPER_ARM_INFO_VALID_ERR_INFO)
+			printk("%serror_info: 0x%016llx\n", newpfx,
+			       err_info->error_info);
+		if (err_info->validation_bits & CPER_ARM_INFO_VALID_VIRT_ADDR)
+			printk("%svirtual fault address: 0x%016llx\n",
+				newpfx, err_info->virt_fault_addr);
+		if (err_info->validation_bits & CPER_ARM_INFO_VALID_PHYSICAL_ADDR)
+			printk("%sphysical fault address: 0x%016llx\n",
+				newpfx, err_info->physical_fault_addr);
+		err_info += 1;
+	}
+
+	ctx_info = (struct cper_arm_ctx_info *)err_info;
+	max_ctx_type = ARRAY_SIZE(arm_reg_ctx_strs) - 1;
+	for (i = 0; i < proc->context_info_num; i++) {
+		int size = sizeof(*ctx_info) + ctx_info->size;
+
+		printk("%sContext info structure %d:\n", pfx, i);
+		if (len < size) {
+			printk("%ssection length is too small\n", newpfx);
+			printk("%sfirmware-generated error record is incorrect\n", pfx);
+			return;
+		}
+		if (ctx_info->type > max_ctx_type) {
+			printk("%sInvalid context type: %d (max: %d)\n",
+				newpfx, ctx_info->type, max_ctx_type);
+			return;
+		}
+		printk("%sregister context type: %s\n", newpfx,
+			arm_reg_ctx_strs[ctx_info->type]);
+		print_hex_dump(newpfx, "", DUMP_PREFIX_OFFSET, 16, 4,
+				(ctx_info + 1), ctx_info->size, 0);
+		len -= size;
+		ctx_info = (struct cper_arm_ctx_info *)((long)ctx_info + size);
+	}
+
+	if (len > 0) {
+		printk("%sVendor specific error info has %u bytes:\n", pfx,
+		       len);
+		print_hex_dump(newpfx, "", DUMP_PREFIX_OFFSET, 16, 4, ctx_info,
+				len, true);
+	}
+}
+#endif
+
 static const char * const mem_err_type_strs[] = {
 	"unknown",
 	"no error",
@@ -386,13 +509,38 @@ static void cper_print_pcie(const char *pfx, const struct cper_sec_pcie *pcie,
 	pfx, pcie->bridge.secondary_status, pcie->bridge.control);
 }
 
-static void cper_estatus_print_section(
-	const char *pfx, const struct acpi_hest_generic_data *gdata, int sec_no)
+static void cper_print_tstamp(const char *pfx,
+				   struct acpi_hest_generic_data_v300 *gdata)
+{
+	__u8 hour, min, sec, day, mon, year, century, *timestamp;
+
+	if (gdata->validation_bits & ACPI_HEST_GEN_VALID_TIMESTAMP) {
+		timestamp = (__u8 *)&(gdata->time_stamp);
+		sec       = bcd2bin(timestamp[0]);
+		min       = bcd2bin(timestamp[1]);
+		hour      = bcd2bin(timestamp[2]);
+		day       = bcd2bin(timestamp[4]);
+		mon       = bcd2bin(timestamp[5]);
+		year      = bcd2bin(timestamp[6]);
+		century   = bcd2bin(timestamp[7]);
+
+		printk("%s%ststamp: %02d%02d-%02d-%02d %02d:%02d:%02d\n", pfx,
+		       (timestamp[3] & 0x1 ? "precise " : "imprecise "),
+		       century, year, mon, day, hour, min, sec);
+	}
+}
+
+static void
+cper_estatus_print_section(const char *pfx, struct acpi_hest_generic_data *gdata,
+			   int sec_no)
 {
 	uuid_le *sec_type = (uuid_le *)gdata->section_type;
 	__u16 severity;
 	char newpfx[64];
 
+	if (acpi_hest_get_version(gdata) >= 3)
+		cper_print_tstamp(pfx, (struct acpi_hest_generic_data_v300 *)gdata);
+
 	severity = gdata->error_severity;
 	printk("%s""Error %d, type: %s\n", pfx, sec_no,
 	       cper_severity_str(severity));
@@ -403,14 +551,16 @@ static void cper_estatus_print_section(
 
 	snprintf(newpfx, sizeof(newpfx), "%s%s", pfx, INDENT_SP);
 	if (!uuid_le_cmp(*sec_type, CPER_SEC_PROC_GENERIC)) {
-		struct cper_sec_proc_generic *proc_err = (void *)(gdata + 1);
+		struct cper_sec_proc_generic *proc_err = acpi_hest_get_payload(gdata);
+
 		printk("%s""section_type: general processor error\n", newpfx);
 		if (gdata->error_data_length >= sizeof(*proc_err))
 			cper_print_proc_generic(newpfx, proc_err);
 		else
 			goto err_section_too_small;
 	} else if (!uuid_le_cmp(*sec_type, CPER_SEC_PLATFORM_MEM)) {
-		struct cper_sec_mem_err *mem_err = (void *)(gdata + 1);
+		struct cper_sec_mem_err *mem_err = acpi_hest_get_payload(gdata);
+
 		printk("%s""section_type: memory error\n", newpfx);
 		if (gdata->error_data_length >=
 		    sizeof(struct cper_sec_mem_err_old))
@@ -419,14 +569,32 @@ static void cper_estatus_print_section(
 		else
 			goto err_section_too_small;
 	} else if (!uuid_le_cmp(*sec_type, CPER_SEC_PCIE)) {
-		struct cper_sec_pcie *pcie = (void *)(gdata + 1);
+		struct cper_sec_pcie *pcie = acpi_hest_get_payload(gdata);
+
 		printk("%s""section_type: PCIe error\n", newpfx);
 		if (gdata->error_data_length >= sizeof(*pcie))
 			cper_print_pcie(newpfx, pcie, gdata);
 		else
 			goto err_section_too_small;
-	} else
-		printk("%s""section type: unknown, %pUl\n", newpfx, sec_type);
+#if defined(CONFIG_ARM64) || defined(CONFIG_ARM)
+	} else if (!uuid_le_cmp(*sec_type, CPER_SEC_PROC_ARM)) {
+		struct cper_sec_proc_arm *arm_err = acpi_hest_get_payload(gdata);
+
+		printk("%ssection_type: ARM processor error\n", newpfx);
+		if (gdata->error_data_length >= sizeof(*arm_err))
+			cper_print_proc_arm(newpfx, arm_err);
+		else
+			goto err_section_too_small;
+#endif
+	} else {
+		const void *err = acpi_hest_get_payload(gdata);
+
+		printk("%ssection type: unknown, %pUl\n", newpfx, sec_type);
+		printk("%ssection length: %#x\n", newpfx,
+		       gdata->error_data_length);
+		print_hex_dump(newpfx, "", DUMP_PREFIX_OFFSET, 16, 4, err,
+			       gdata->error_data_length, true);
+	}
 
 	return;
 
@@ -438,7 +606,7 @@ void cper_estatus_print(const char *pfx,
 			const struct acpi_hest_generic_status *estatus)
 {
 	struct acpi_hest_generic_data *gdata;
-	unsigned int data_len, gedata_len;
+	unsigned int data_len;
 	int sec_no = 0;
 	char newpfx[64];
 	__u16 severity;
@@ -452,11 +620,11 @@ void cper_estatus_print(const char *pfx,
 	data_len = estatus->data_length;
 	gdata = (struct acpi_hest_generic_data *)(estatus + 1);
 	snprintf(newpfx, sizeof(newpfx), "%s%s", pfx, INDENT_SP);
-	while (data_len >= sizeof(*gdata)) {
-		gedata_len = gdata->error_data_length;
+
+	while (data_len >= acpi_hest_get_size(gdata)) {
 		cper_estatus_print_section(newpfx, gdata, sec_no);
-		data_len -= gedata_len + sizeof(*gdata);
-		gdata = (void *)(gdata + 1) + gedata_len;
+		data_len -= acpi_hest_get_record_size(gdata);
+		gdata = acpi_hest_get_next(gdata);
 		sec_no++;
 	}
 }
@@ -486,12 +654,14 @@ int cper_estatus_check(const struct acpi_hest_generic_status *estatus)
 		return rc;
 	data_len = estatus->data_length;
 	gdata = (struct acpi_hest_generic_data *)(estatus + 1);
-	while (data_len >= sizeof(*gdata)) {
-		gedata_len = gdata->error_data_length;
-		if (gedata_len > data_len - sizeof(*gdata))
+
+	while (data_len >= acpi_hest_get_size(gdata)) {
+		gedata_len = acpi_hest_get_error_length(gdata);
+		if (gedata_len > data_len - acpi_hest_get_size(gdata))
 			return -EINVAL;
-		data_len -= gedata_len + sizeof(*gdata);
-		gdata = (void *)(gdata + 1) + gedata_len;
+
+		data_len -= acpi_hest_get_record_size(gdata);
+		gdata = acpi_hest_get_next(gdata);
 	}
 	if (data_len)
 		return -EINVAL;
diff --git a/drivers/firmware/efi/efi-pstore.c b/drivers/firmware/efi/efi-pstore.c
index ef1fafdad400..5a0fa939d70f 100644
--- a/drivers/firmware/efi/efi-pstore.c
+++ b/drivers/firmware/efi/efi-pstore.c
@@ -4,7 +4,7 @@
 #include <linux/slab.h>
 #include <linux/ucs2_string.h>
 
-#define DUMP_NAME_LEN 52
+#define DUMP_NAME_LEN 66
 
 static bool efivars_pstore_disable =
 	IS_ENABLED(CONFIG_EFI_VARS_PSTORE_DEFAULT_DISABLE);
@@ -244,12 +244,12 @@ static int efi_pstore_write(struct pstore_record *record)
 	efi_guid_t vendor = LINUX_EFI_CRASH_GUID;
 	int i, ret = 0;
 
-	record->time.tv_sec = get_seconds();
-	record->time.tv_nsec = 0;
-
 	record->id = generic_id(record->time.tv_sec, record->part,
 				record->count);
 
+	/* Since we copy the entire length of name, make sure it is wiped. */
+	memset(name, 0, sizeof(name));
+
 	snprintf(name, sizeof(name), "dump-type%u-%u-%d-%lu-%c",
 		 record->type, record->part, record->count,
 		 record->time.tv_sec, record->compressed ? 'C' : 'D');
@@ -267,44 +267,20 @@ static int efi_pstore_write(struct pstore_record *record)
 	return ret;
 };
 
-struct pstore_erase_data {
-	struct pstore_record *record;
-	efi_char16_t *name;
-};
-
 /*
  * Clean up an entry with the same name
  */
 static int efi_pstore_erase_func(struct efivar_entry *entry, void *data)
 {
-	struct pstore_erase_data *ed = data;
+	efi_char16_t *efi_name = data;
 	efi_guid_t vendor = LINUX_EFI_CRASH_GUID;
-	efi_char16_t efi_name_old[DUMP_NAME_LEN];
-	efi_char16_t *efi_name = ed->name;
-	unsigned long ucs2_len = ucs2_strlen(ed->name);
-	char name_old[DUMP_NAME_LEN];
-	int i;
+	unsigned long ucs2_len = ucs2_strlen(efi_name);
 
 	if (efi_guidcmp(entry->var.VendorGuid, vendor))
 		return 0;
 
-	if (ucs2_strncmp(entry->var.VariableName,
-			  efi_name, (size_t)ucs2_len)) {
-		/*
-		 * Check if an old format, which doesn't support
-		 * holding multiple logs, remains.
-		 */
-		snprintf(name_old, sizeof(name_old), "dump-type%u-%u-%lu",
-			ed->record->type, ed->record->part,
-			ed->record->time.tv_sec);
-
-		for (i = 0; i < DUMP_NAME_LEN; i++)
-			efi_name_old[i] = name_old[i];
-
-		if (ucs2_strncmp(entry->var.VariableName, efi_name_old,
-				  ucs2_strlen(efi_name_old)))
-			return 0;
-	}
+	if (ucs2_strncmp(entry->var.VariableName, efi_name, (size_t)ucs2_len))
+		return 0;
 
 	if (entry->scanning) {
 		/*
@@ -321,35 +297,48 @@ static int efi_pstore_erase_func(struct efivar_entry *entry, void *data)
 	return 1;
 }
 
-static int efi_pstore_erase(struct pstore_record *record)
+static int efi_pstore_erase_name(const char *name)
 {
-	struct pstore_erase_data edata;
 	struct efivar_entry *entry = NULL;
-	char name[DUMP_NAME_LEN];
 	efi_char16_t efi_name[DUMP_NAME_LEN];
 	int found, i;
 
-	snprintf(name, sizeof(name), "dump-type%u-%u-%d-%lu",
-		 record->type, record->part, record->count,
-		 record->time.tv_sec);
-
-	for (i = 0; i < DUMP_NAME_LEN; i++)
+	for (i = 0; i < DUMP_NAME_LEN; i++) {
 		efi_name[i] = name[i];
-
-	edata.record = record;
-	edata.name = efi_name;
+		if (name[i] == '\0')
+			break;
+	}
 
 	if (efivar_entry_iter_begin())
 		return -EINTR;
-	found = __efivar_entry_iter(efi_pstore_erase_func, &efivar_sysfs_list, &edata, &entry);
 
-	if (found && !entry->scanning) {
-		efivar_entry_iter_end();
+	found = __efivar_entry_iter(efi_pstore_erase_func, &efivar_sysfs_list,
+				    efi_name, &entry);
+	efivar_entry_iter_end();
+
+	if (found && !entry->scanning)
 		efivar_unregister(entry);
-	} else
-		efivar_entry_iter_end();
 
-	return 0;
+	return found ? 0 : -ENOENT;
+}
+
+static int efi_pstore_erase(struct pstore_record *record)
+{
+	char name[DUMP_NAME_LEN];
+	int ret;
+
+	snprintf(name, sizeof(name), "dump-type%u-%u-%d-%lu",
+		 record->type, record->part, record->count,
+		 record->time.tv_sec);
+	ret = efi_pstore_erase_name(name);
+	if (ret != -ENOENT)
+		return ret;
+
+	snprintf(name, sizeof(name), "dump-type%u-%u-%lu",
+		record->type, record->part, record->time.tv_sec);
+	ret = efi_pstore_erase_name(name);
+
+	return ret;
 }
 
 static struct pstore_info efi_pstore_info = {
diff --git a/drivers/firmware/efi/efi.c b/drivers/firmware/efi/efi.c
index b372aad3b449..045d6d311bde 100644
--- a/drivers/firmware/efi/efi.c
+++ b/drivers/firmware/efi/efi.c
@@ -528,7 +528,8 @@ int __init efi_config_parse_tables(void *config_tables, int count, int sz,
 		}
 	}
 
-	efi_memattr_init();
+	if (efi_enabled(EFI_MEMMAP))
+		efi_memattr_init();
 
 	/* Parse the EFI Properties table if it exists */
 	if (efi.properties_table != EFI_INVALID_TABLE_ADDR) {
diff --git a/drivers/firmware/efi/test/efi_test.c b/drivers/firmware/efi/test/efi_test.c
index 8cd578f62059..08129b7b80ab 100644
--- a/drivers/firmware/efi/test/efi_test.c
+++ b/drivers/firmware/efi/test/efi_test.c
@@ -71,18 +71,13 @@ copy_ucs2_from_user_len(efi_char16_t **dst, efi_char16_t __user *src,
 	if (!access_ok(VERIFY_READ, src, 1))
 		return -EFAULT;
 
-	buf = kmalloc(len, GFP_KERNEL);
-	if (!buf) {
+	buf = memdup_user(src, len);
+	if (IS_ERR(buf)) {
 		*dst = NULL;
-		return -ENOMEM;
+		return PTR_ERR(buf);
 	}
 	*dst = buf;
 
-	if (copy_from_user(*dst, src, len)) {
-		kfree(buf);
-		return -EFAULT;
-	}
-
 	return 0;
 }
 
diff --git a/drivers/firmware/google/memconsole-coreboot.c b/drivers/firmware/google/memconsole-coreboot.c
index 02711114dece..52738887735c 100644
--- a/drivers/firmware/google/memconsole-coreboot.c
+++ b/drivers/firmware/google/memconsole-coreboot.c
@@ -26,12 +26,52 @@
 
 /* CBMEM firmware console log descriptor. */
 struct cbmem_cons {
-	u32 buffer_size;
-	u32 buffer_cursor;
-	u8  buffer_body[0];
+	u32 size_dont_access_after_boot;
+	u32 cursor;
+	u8  body[0];
 } __packed;
 
+#define CURSOR_MASK ((1 << 28) - 1)
+#define OVERFLOW (1 << 31)
+
 static struct cbmem_cons __iomem *cbmem_console;
+static u32 cbmem_console_size;
+
+/*
+ * The cbmem_console structure is read again on every access because it may
+ * change at any time if runtime firmware logs new messages. This may rarely
+ * lead to race conditions where the firmware overwrites the beginning of the
+ * ring buffer with more lines after we have already read |cursor|. It should be
+ * rare and harmless enough that we don't spend extra effort working around it.
+ */
+static ssize_t memconsole_coreboot_read(char *buf, loff_t pos, size_t count)
+{
+	u32 cursor = cbmem_console->cursor & CURSOR_MASK;
+	u32 flags = cbmem_console->cursor & ~CURSOR_MASK;
+	u32 size = cbmem_console_size;
+	struct seg {	/* describes ring buffer segments in logical order */
+		u32 phys;	/* physical offset from start of mem buffer */
+		u32 len;	/* length of segment */
+	} seg[2] = { {0}, {0} };
+	size_t done = 0;
+	int i;
+
+	if (flags & OVERFLOW) {
+		if (cursor > size)	/* Shouldn't really happen, but... */
+			cursor = 0;
+		seg[0] = (struct seg){.phys = cursor, .len = size - cursor};
+		seg[1] = (struct seg){.phys = 0, .len = cursor};
+	} else {
+		seg[0] = (struct seg){.phys = 0, .len = min(cursor, size)};
+	}
+
+	for (i = 0; i < ARRAY_SIZE(seg) && count > done; i++) {
+		done += memory_read_from_buffer(buf + done, count - done, &pos,
+			cbmem_console->body + seg[i].phys, seg[i].len);
+		pos -= seg[i].len;
+	}
+	return done;
+}
 
 static int memconsole_coreboot_init(phys_addr_t physaddr)
 {
@@ -42,17 +82,17 @@ static int memconsole_coreboot_init(phys_addr_t physaddr)
 	if (!tmp_cbmc)
 		return -ENOMEM;
 
+	/* Read size only once to prevent overrun attack through /dev/mem. */
+	cbmem_console_size = tmp_cbmc->size_dont_access_after_boot;
 	cbmem_console = memremap(physaddr,
-				 tmp_cbmc->buffer_size + sizeof(*cbmem_console),
+				 cbmem_console_size + sizeof(*cbmem_console),
 				 MEMREMAP_WB);
 	memunmap(tmp_cbmc);
 
 	if (!cbmem_console)
 		return -ENOMEM;
 
-	memconsole_setup(cbmem_console->buffer_body,
-		min(cbmem_console->buffer_cursor, cbmem_console->buffer_size));
-
+	memconsole_setup(memconsole_coreboot_read);
 	return 0;
 }
 
diff --git a/drivers/firmware/google/memconsole-x86-legacy.c b/drivers/firmware/google/memconsole-x86-legacy.c
index 1f279ee883b9..8c1bf6dbdaa6 100644
--- a/drivers/firmware/google/memconsole-x86-legacy.c
+++ b/drivers/firmware/google/memconsole-x86-legacy.c
@@ -48,6 +48,15 @@ struct biosmemcon_ebda {
 	};
 } __packed;
 
+static char *memconsole_baseaddr;
+static size_t memconsole_length;
+
+static ssize_t memconsole_read(char *buf, loff_t pos, size_t count)
+{
+	return memory_read_from_buffer(buf, count, &pos, memconsole_baseaddr,
+				       memconsole_length);
+}
+
 static void found_v1_header(struct biosmemcon_ebda *hdr)
 {
 	pr_info("memconsole: BIOS console v1 EBDA structure found at %p\n",
@@ -56,7 +65,9 @@ static void found_v1_header(struct biosmemcon_ebda *hdr)
 		hdr->v1.buffer_addr, hdr->v1.start,
 		hdr->v1.end, hdr->v1.num_chars);
 
-	memconsole_setup(phys_to_virt(hdr->v1.buffer_addr), hdr->v1.num_chars);
+	memconsole_baseaddr = phys_to_virt(hdr->v1.buffer_addr);
+	memconsole_length = hdr->v1.num_chars;
+	memconsole_setup(memconsole_read);
 }
 
 static void found_v2_header(struct biosmemcon_ebda *hdr)
@@ -67,8 +78,9 @@ static void found_v2_header(struct biosmemcon_ebda *hdr)
 		hdr->v2.buffer_addr, hdr->v2.start,
 		hdr->v2.end, hdr->v2.num_bytes);
 
-	memconsole_setup(phys_to_virt(hdr->v2.buffer_addr + hdr->v2.start),
-			 hdr->v2.end - hdr->v2.start);
+	memconsole_baseaddr = phys_to_virt(hdr->v2.buffer_addr + hdr->v2.start);
+	memconsole_length = hdr->v2.end - hdr->v2.start;
+	memconsole_setup(memconsole_read);
 }
 
 /*
diff --git a/drivers/firmware/google/memconsole.c b/drivers/firmware/google/memconsole.c
index 94e200ddb4fa..166f07c68c02 100644
--- a/drivers/firmware/google/memconsole.c
+++ b/drivers/firmware/google/memconsole.c
@@ -22,15 +22,15 @@
 
 #include "memconsole.h"
 
-static char *memconsole_baseaddr;
-static size_t memconsole_length;
+static ssize_t (*memconsole_read_func)(char *, loff_t, size_t);
 
 static ssize_t memconsole_read(struct file *filp, struct kobject *kobp,
 			       struct bin_attribute *bin_attr, char *buf,
 			       loff_t pos, size_t count)
 {
-	return memory_read_from_buffer(buf, count, &pos, memconsole_baseaddr,
-				       memconsole_length);
+	if (WARN_ON_ONCE(!memconsole_read_func))
+		return -EIO;
+	return memconsole_read_func(buf, pos, count);
 }
 
 static struct bin_attribute memconsole_bin_attr = {
@@ -38,16 +38,14 @@ static struct bin_attribute memconsole_bin_attr = {
 	.read = memconsole_read,
 };
 
-void memconsole_setup(void *baseaddr, size_t length)
+void memconsole_setup(ssize_t (*read_func)(char *, loff_t, size_t))
 {
-	memconsole_baseaddr = baseaddr;
-	memconsole_length = length;
+	memconsole_read_func = read_func;
 }
 EXPORT_SYMBOL(memconsole_setup);
 
 int memconsole_sysfs_init(void)
 {
-	memconsole_bin_attr.size = memconsole_length;
 	return sysfs_create_bin_file(firmware_kobj, &memconsole_bin_attr);
 }
 EXPORT_SYMBOL(memconsole_sysfs_init);
diff --git a/drivers/firmware/google/memconsole.h b/drivers/firmware/google/memconsole.h
index 190fc03a51ae..ff1592dc7d1a 100644
--- a/drivers/firmware/google/memconsole.h
+++ b/drivers/firmware/google/memconsole.h
@@ -18,13 +18,14 @@
 #ifndef __FIRMWARE_GOOGLE_MEMCONSOLE_H
 #define __FIRMWARE_GOOGLE_MEMCONSOLE_H
 
+#include <linux/types.h>
+
 /*
  * memconsole_setup
  *
- * Initialize the memory console from raw (virtual) base
- * address and length.
+ * Initialize the memory console, passing the function to handle read accesses.
  */
-void memconsole_setup(void *baseaddr, size_t length);
+void memconsole_setup(ssize_t (*read_func)(char *, loff_t, size_t));
 
 /*
  * memconsole_sysfs_init
diff --git a/drivers/firmware/google/vpd.c b/drivers/firmware/google/vpd.c
index 31058d400bda..78945729388e 100644
--- a/drivers/firmware/google/vpd.c
+++ b/drivers/firmware/google/vpd.c
@@ -118,14 +118,13 @@ static int vpd_section_attrib_add(const u8 *key, s32 key_len,
 	info = kzalloc(sizeof(*info), GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
-	info->key = kzalloc(key_len + 1, GFP_KERNEL);
+
+	info->key = kstrndup(key, key_len, GFP_KERNEL);
 	if (!info->key) {
 		ret = -ENOMEM;
 		goto free_info;
 	}
 
-	memcpy(info->key, key, key_len);
-
 	sysfs_bin_attr_init(&info->bin_attr);
 	info->bin_attr.attr.name = info->key;
 	info->bin_attr.attr.mode = 0444;
@@ -191,8 +190,7 @@ static int vpd_section_create_attribs(struct vpd_section *sec)
 static int vpd_section_init(const char *name, struct vpd_section *sec,
 			    phys_addr_t physaddr, size_t size)
 {
-	int ret;
-	int raw_len;
+	int err;
 
 	sec->baseaddr = memremap(physaddr, size, MEMREMAP_WB);
 	if (!sec->baseaddr)
@@ -201,10 +199,11 @@ static int vpd_section_init(const char *name, struct vpd_section *sec,
 	sec->name = name;
 
 	/* We want to export the raw partion with name ${name}_raw */
-	raw_len = strlen(name) + 5;
-	sec->raw_name = kzalloc(raw_len, GFP_KERNEL);
-	strncpy(sec->raw_name, name, raw_len);
-	strncat(sec->raw_name, "_raw", raw_len);
+	sec->raw_name = kasprintf(GFP_KERNEL, "%s_raw", name);
+	if (!sec->raw_name) {
+		err = -ENOMEM;
+		goto err_iounmap;
+	}
 
 	sysfs_bin_attr_init(&sec->bin_attr);
 	sec->bin_attr.attr.name = sec->raw_name;
@@ -213,14 +212,14 @@ static int vpd_section_init(const char *name, struct vpd_section *sec,
 	sec->bin_attr.read = vpd_section_read;
 	sec->bin_attr.private = sec;
 
-	ret = sysfs_create_bin_file(vpd_kobj, &sec->bin_attr);
-	if (ret)
-		goto free_sec;
+	err = sysfs_create_bin_file(vpd_kobj, &sec->bin_attr);
+	if (err)
+		goto err_free_raw_name;
 
 	sec->kobj = kobject_create_and_add(name, vpd_kobj);
 	if (!sec->kobj) {
-		ret = -EINVAL;
-		goto sysfs_remove;
+		err = -EINVAL;
+		goto err_sysfs_remove;
 	}
 
 	INIT_LIST_HEAD(&sec->attribs);
@@ -230,14 +229,13 @@ static int vpd_section_init(const char *name, struct vpd_section *sec,
 
 	return 0;
 
-sysfs_remove:
+err_sysfs_remove:
 	sysfs_remove_bin_file(vpd_kobj, &sec->bin_attr);
-
-free_sec:
+err_free_raw_name:
 	kfree(sec->raw_name);
+err_iounmap:
 	iounmap(sec->baseaddr);
-
-	return ret;
+	return err;
 }
 
 static int vpd_section_destroy(struct vpd_section *sec)
@@ -319,9 +317,6 @@ static int __init vpd_platform_init(void)
 	if (!vpd_kobj)
 		return -ENOMEM;
 
-	memset(&ro_vpd, 0, sizeof(ro_vpd));
-	memset(&rw_vpd, 0, sizeof(rw_vpd));
-
 	platform_driver_register(&vpd_driver);
 
 	return 0;
diff --git a/drivers/firmware/tegra/bpmp.c b/drivers/firmware/tegra/bpmp.c
index 84e4c9a58a0c..b25179517cc5 100644
--- a/drivers/firmware/tegra/bpmp.c
+++ b/drivers/firmware/tegra/bpmp.c
@@ -211,14 +211,17 @@ static ssize_t tegra_bpmp_channel_read(struct tegra_bpmp_channel *channel,
 	int index;
 
 	index = tegra_bpmp_channel_get_thread_index(channel);
-	if (index < 0)
-		return index;
+	if (index < 0) {
+		err = index;
+		goto unlock;
+	}
 
 	spin_lock_irqsave(&bpmp->lock, flags);
 	err = __tegra_bpmp_channel_read(channel, data, size);
 	clear_bit(index, bpmp->threaded.allocated);
 	spin_unlock_irqrestore(&bpmp->lock, flags);
 
+unlock:
 	up(&bpmp->threaded.lock);
 
 	return err;
@@ -256,18 +259,18 @@ tegra_bpmp_write_threaded(struct tegra_bpmp *bpmp, unsigned int mrq,
 
 	index = find_first_zero_bit(bpmp->threaded.allocated, count);
 	if (index == count) {
-		channel = ERR_PTR(-EBUSY);
+		err = -EBUSY;
 		goto unlock;
 	}
 
 	channel = tegra_bpmp_channel_get_thread(bpmp, index);
 	if (!channel) {
-		channel = ERR_PTR(-EINVAL);
+		err = -EINVAL;
 		goto unlock;
 	}
 
 	if (!tegra_bpmp_master_free(channel)) {
-		channel = ERR_PTR(-EBUSY);
+		err = -EBUSY;
 		goto unlock;
 	}
 
@@ -275,16 +278,21 @@ tegra_bpmp_write_threaded(struct tegra_bpmp *bpmp, unsigned int mrq,
 
 	err = __tegra_bpmp_channel_write(channel, mrq, MSG_ACK | MSG_RING,
 					 data, size);
-	if (err < 0) {
-		clear_bit(index, bpmp->threaded.allocated);
-		goto unlock;
-	}
+	if (err < 0)
+		goto clear_allocated;
 
 	set_bit(index, bpmp->threaded.busy);
 
-unlock:
 	spin_unlock_irqrestore(&bpmp->lock, flags);
 	return channel;
+
+clear_allocated:
+	clear_bit(index, bpmp->threaded.allocated);
+unlock:
+	spin_unlock_irqrestore(&bpmp->lock, flags);
+	up(&bpmp->threaded.lock);
+
+	return ERR_PTR(err);
 }
 
 static ssize_t tegra_bpmp_channel_write(struct tegra_bpmp_channel *channel,
@@ -810,6 +818,10 @@ static int tegra_bpmp_probe(struct platform_device *pdev)
 	if (err < 0)
 		goto free_mrq;
 
+	err = tegra_bpmp_init_powergates(bpmp);
+	if (err < 0)
+		goto free_mrq;
+
 	platform_set_drvdata(pdev, bpmp);
 
 	return 0;
diff --git a/drivers/fsi/Kconfig b/drivers/fsi/Kconfig
index 04c1a0efa7a7..6821ed0cd5e8 100644
--- a/drivers/fsi/Kconfig
+++ b/drivers/fsi/Kconfig
@@ -6,7 +6,33 @@ menu "FSI support"
 
 config FSI
 	tristate "FSI support"
+	select CRC4
 	---help---
 	  FSI - the FRU Support Interface - is a simple bus for low-level
 	  access to POWER-based hardware.
+
+if FSI
+
+config FSI_MASTER_GPIO
+	tristate "GPIO-based FSI master"
+	depends on GPIOLIB
+	select CRC4
+	---help---
+	This option enables a FSI master driver using GPIO lines.
+
+config FSI_MASTER_HUB
+	tristate "FSI hub master"
+	---help---
+	This option enables a FSI hub master driver.  Hub is a type of FSI
+	master that is connected to the upstream master via a slave.  Hubs
+	allow chaining of FSI links to an arbitrary depth.  This allows for
+	a high target device fanout.
+
+config FSI_SCOM
+	tristate "SCOM FSI client device driver"
+	---help---
+	This option enables an FSI based SCOM device driver.
+
+endif
+
 endmenu
diff --git a/drivers/fsi/Makefile b/drivers/fsi/Makefile
index db0e5e7c1655..65eb99dfafdb 100644
--- a/drivers/fsi/Makefile
+++ b/drivers/fsi/Makefile
@@ -1,2 +1,5 @@
 
 obj-$(CONFIG_FSI) += fsi-core.o
+obj-$(CONFIG_FSI_MASTER_HUB) += fsi-master-hub.o
+obj-$(CONFIG_FSI_MASTER_GPIO) += fsi-master-gpio.o
+obj-$(CONFIG_FSI_SCOM) += fsi-scom.o
diff --git a/drivers/fsi/fsi-core.c b/drivers/fsi/fsi-core.c
index 3d55bd547178..a485864cb512 100644
--- a/drivers/fsi/fsi-core.c
+++ b/drivers/fsi/fsi-core.c
@@ -13,9 +13,830 @@
  * GNU General Public License for more details.
  */
 
+#include <linux/crc4.h>
 #include <linux/device.h>
 #include <linux/fsi.h>
+#include <linux/idr.h>
 #include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/bitops.h>
+
+#include "fsi-master.h"
+
+#define CREATE_TRACE_POINTS
+#include <trace/events/fsi.h>
+
+#define FSI_SLAVE_CONF_NEXT_MASK	GENMASK(31, 31)
+#define FSI_SLAVE_CONF_SLOTS_MASK	GENMASK(23, 16)
+#define FSI_SLAVE_CONF_SLOTS_SHIFT	16
+#define FSI_SLAVE_CONF_VERSION_MASK	GENMASK(15, 12)
+#define FSI_SLAVE_CONF_VERSION_SHIFT	12
+#define FSI_SLAVE_CONF_TYPE_MASK	GENMASK(11, 4)
+#define FSI_SLAVE_CONF_TYPE_SHIFT	4
+#define FSI_SLAVE_CONF_CRC_SHIFT	4
+#define FSI_SLAVE_CONF_CRC_MASK		GENMASK(3, 0)
+#define FSI_SLAVE_CONF_DATA_BITS	28
+
+#define FSI_PEEK_BASE			0x410
+
+static const int engine_page_size = 0x400;
+
+#define FSI_SLAVE_BASE			0x800
+
+/*
+ * FSI slave engine control register offsets
+ */
+#define FSI_SMODE		0x0	/* R/W: Mode register */
+#define FSI_SISC		0x8	/* R/W: Interrupt condition */
+#define FSI_SSTAT		0x14	/* R  : Slave status */
+#define FSI_LLMODE		0x100	/* R/W: Link layer mode register */
+
+/*
+ * SMODE fields
+ */
+#define FSI_SMODE_WSC		0x80000000	/* Warm start done */
+#define FSI_SMODE_ECRC		0x20000000	/* Hw CRC check */
+#define FSI_SMODE_SID_SHIFT	24		/* ID shift */
+#define FSI_SMODE_SID_MASK	3		/* ID Mask */
+#define FSI_SMODE_ED_SHIFT	20		/* Echo delay shift */
+#define FSI_SMODE_ED_MASK	0xf		/* Echo delay mask */
+#define FSI_SMODE_SD_SHIFT	16		/* Send delay shift */
+#define FSI_SMODE_SD_MASK	0xf		/* Send delay mask */
+#define FSI_SMODE_LBCRR_SHIFT	8		/* Clk ratio shift */
+#define FSI_SMODE_LBCRR_MASK	0xf		/* Clk ratio mask */
+
+/*
+ * LLMODE fields
+ */
+#define FSI_LLMODE_ASYNC	0x1
+
+#define FSI_SLAVE_SIZE_23b		0x800000
+
+static DEFINE_IDA(master_ida);
+
+struct fsi_slave {
+	struct device		dev;
+	struct fsi_master	*master;
+	int			id;
+	int			link;
+	uint32_t		size;	/* size of slave address space */
+};
+
+#define to_fsi_master(d) container_of(d, struct fsi_master, dev)
+#define to_fsi_slave(d) container_of(d, struct fsi_slave, dev)
+
+static const int slave_retries = 2;
+static int discard_errors;
+
+static int fsi_master_read(struct fsi_master *master, int link,
+		uint8_t slave_id, uint32_t addr, void *val, size_t size);
+static int fsi_master_write(struct fsi_master *master, int link,
+		uint8_t slave_id, uint32_t addr, const void *val, size_t size);
+static int fsi_master_break(struct fsi_master *master, int link);
+
+/*
+ * fsi_device_read() / fsi_device_write() / fsi_device_peek()
+ *
+ * FSI endpoint-device support
+ *
+ * Read / write / peek accessors for a client
+ *
+ * Parameters:
+ * dev:  Structure passed to FSI client device drivers on probe().
+ * addr: FSI address of given device.  Client should pass in its base address
+ *       plus desired offset to access its register space.
+ * val:  For read/peek this is the value read at the specified address. For
+ *       write this is value to write to the specified address.
+ *       The data in val must be FSI bus endian (big endian).
+ * size: Size in bytes of the operation.  Sizes supported are 1, 2 and 4 bytes.
+ *       Addresses must be aligned on size boundaries or an error will result.
+ */
+int fsi_device_read(struct fsi_device *dev, uint32_t addr, void *val,
+		size_t size)
+{
+	if (addr > dev->size || size > dev->size || addr > dev->size - size)
+		return -EINVAL;
+
+	return fsi_slave_read(dev->slave, dev->addr + addr, val, size);
+}
+EXPORT_SYMBOL_GPL(fsi_device_read);
+
+int fsi_device_write(struct fsi_device *dev, uint32_t addr, const void *val,
+		size_t size)
+{
+	if (addr > dev->size || size > dev->size || addr > dev->size - size)
+		return -EINVAL;
+
+	return fsi_slave_write(dev->slave, dev->addr + addr, val, size);
+}
+EXPORT_SYMBOL_GPL(fsi_device_write);
+
+int fsi_device_peek(struct fsi_device *dev, void *val)
+{
+	uint32_t addr = FSI_PEEK_BASE + ((dev->unit - 2) * sizeof(uint32_t));
+
+	return fsi_slave_read(dev->slave, addr, val, sizeof(uint32_t));
+}
+
+static void fsi_device_release(struct device *_device)
+{
+	struct fsi_device *device = to_fsi_dev(_device);
+
+	kfree(device);
+}
+
+static struct fsi_device *fsi_create_device(struct fsi_slave *slave)
+{
+	struct fsi_device *dev;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return NULL;
+
+	dev->dev.parent = &slave->dev;
+	dev->dev.bus = &fsi_bus_type;
+	dev->dev.release = fsi_device_release;
+
+	return dev;
+}
+
+/* FSI slave support */
+static int fsi_slave_calc_addr(struct fsi_slave *slave, uint32_t *addrp,
+		uint8_t *idp)
+{
+	uint32_t addr = *addrp;
+	uint8_t id = *idp;
+
+	if (addr > slave->size)
+		return -EINVAL;
+
+	/* For 23 bit addressing, we encode the extra two bits in the slave
+	 * id (and the slave's actual ID needs to be 0).
+	 */
+	if (addr > 0x1fffff) {
+		if (slave->id != 0)
+			return -EINVAL;
+		id = (addr >> 21) & 0x3;
+		addr &= 0x1fffff;
+	}
+
+	*addrp = addr;
+	*idp = id;
+	return 0;
+}
+
+int fsi_slave_report_and_clear_errors(struct fsi_slave *slave)
+{
+	struct fsi_master *master = slave->master;
+	uint32_t irq, stat;
+	int rc, link;
+	uint8_t id;
+
+	link = slave->link;
+	id = slave->id;
+
+	rc = fsi_master_read(master, link, id, FSI_SLAVE_BASE + FSI_SISC,
+			&irq, sizeof(irq));
+	if (rc)
+		return rc;
+
+	rc =  fsi_master_read(master, link, id, FSI_SLAVE_BASE + FSI_SSTAT,
+			&stat, sizeof(stat));
+	if (rc)
+		return rc;
+
+	dev_info(&slave->dev, "status: 0x%08x, sisc: 0x%08x\n",
+			be32_to_cpu(stat), be32_to_cpu(irq));
+
+	/* clear interrupts */
+	return fsi_master_write(master, link, id, FSI_SLAVE_BASE + FSI_SISC,
+			&irq, sizeof(irq));
+}
+
+static int fsi_slave_set_smode(struct fsi_master *master, int link, int id);
+
+int fsi_slave_handle_error(struct fsi_slave *slave, bool write, uint32_t addr,
+		size_t size)
+{
+	struct fsi_master *master = slave->master;
+	int rc, link;
+	uint32_t reg;
+	uint8_t id;
+
+	if (discard_errors)
+		return -1;
+
+	link = slave->link;
+	id = slave->id;
+
+	dev_dbg(&slave->dev, "handling error on %s to 0x%08x[%zd]",
+			write ? "write" : "read", addr, size);
+
+	/* try a simple clear of error conditions, which may fail if we've lost
+	 * communication with the slave
+	 */
+	rc = fsi_slave_report_and_clear_errors(slave);
+	if (!rc)
+		return 0;
+
+	/* send a TERM and retry */
+	if (master->term) {
+		rc = master->term(master, link, id);
+		if (!rc) {
+			rc = fsi_master_read(master, link, id, 0,
+					&reg, sizeof(reg));
+			if (!rc)
+				rc = fsi_slave_report_and_clear_errors(slave);
+			if (!rc)
+				return 0;
+		}
+	}
+
+	/* getting serious, reset the slave via BREAK */
+	rc = fsi_master_break(master, link);
+	if (rc)
+		return rc;
+
+	rc = fsi_slave_set_smode(master, link, id);
+	if (rc)
+		return rc;
+
+	return fsi_slave_report_and_clear_errors(slave);
+}
+
+int fsi_slave_read(struct fsi_slave *slave, uint32_t addr,
+			void *val, size_t size)
+{
+	uint8_t id = slave->id;
+	int rc, err_rc, i;
+
+	rc = fsi_slave_calc_addr(slave, &addr, &id);
+	if (rc)
+		return rc;
+
+	for (i = 0; i < slave_retries; i++) {
+		rc = fsi_master_read(slave->master, slave->link,
+				id, addr, val, size);
+		if (!rc)
+			break;
+
+		err_rc = fsi_slave_handle_error(slave, false, addr, size);
+		if (err_rc)
+			break;
+	}
+
+	return rc;
+}
+EXPORT_SYMBOL_GPL(fsi_slave_read);
+
+int fsi_slave_write(struct fsi_slave *slave, uint32_t addr,
+			const void *val, size_t size)
+{
+	uint8_t id = slave->id;
+	int rc, err_rc, i;
+
+	rc = fsi_slave_calc_addr(slave, &addr, &id);
+	if (rc)
+		return rc;
+
+	for (i = 0; i < slave_retries; i++) {
+		rc = fsi_master_write(slave->master, slave->link,
+				id, addr, val, size);
+		if (!rc)
+			break;
+
+		err_rc = fsi_slave_handle_error(slave, true, addr, size);
+		if (err_rc)
+			break;
+	}
+
+	return rc;
+}
+EXPORT_SYMBOL_GPL(fsi_slave_write);
+
+extern int fsi_slave_claim_range(struct fsi_slave *slave,
+		uint32_t addr, uint32_t size)
+{
+	if (addr + size < addr)
+		return -EINVAL;
+
+	if (addr + size > slave->size)
+		return -EINVAL;
+
+	/* todo: check for overlapping claims */
+	return 0;
+}
+EXPORT_SYMBOL_GPL(fsi_slave_claim_range);
+
+extern void fsi_slave_release_range(struct fsi_slave *slave,
+		uint32_t addr, uint32_t size)
+{
+}
+EXPORT_SYMBOL_GPL(fsi_slave_release_range);
+
+static int fsi_slave_scan(struct fsi_slave *slave)
+{
+	uint32_t engine_addr;
+	uint32_t conf;
+	int rc, i;
+
+	/*
+	 * scan engines
+	 *
+	 * We keep the peek mode and slave engines for the core; so start
+	 * at the third slot in the configuration table. We also need to
+	 * skip the chip ID entry at the start of the address space.
+	 */
+	engine_addr = engine_page_size * 3;
+	for (i = 2; i < engine_page_size / sizeof(uint32_t); i++) {
+		uint8_t slots, version, type, crc;
+		struct fsi_device *dev;
+
+		rc = fsi_slave_read(slave, (i + 1) * sizeof(conf),
+				&conf, sizeof(conf));
+		if (rc) {
+			dev_warn(&slave->dev,
+				"error reading slave registers\n");
+			return -1;
+		}
+		conf = be32_to_cpu(conf);
+
+		crc = crc4(0, conf, 32);
+		if (crc) {
+			dev_warn(&slave->dev,
+				"crc error in slave register at 0x%04x\n",
+				i);
+			return -1;
+		}
+
+		slots = (conf & FSI_SLAVE_CONF_SLOTS_MASK)
+			>> FSI_SLAVE_CONF_SLOTS_SHIFT;
+		version = (conf & FSI_SLAVE_CONF_VERSION_MASK)
+			>> FSI_SLAVE_CONF_VERSION_SHIFT;
+		type = (conf & FSI_SLAVE_CONF_TYPE_MASK)
+			>> FSI_SLAVE_CONF_TYPE_SHIFT;
+
+		/*
+		 * Unused address areas are marked by a zero type value; this
+		 * skips the defined address areas
+		 */
+		if (type != 0 && slots != 0) {
+
+			/* create device */
+			dev = fsi_create_device(slave);
+			if (!dev)
+				return -ENOMEM;
+
+			dev->slave = slave;
+			dev->engine_type = type;
+			dev->version = version;
+			dev->unit = i;
+			dev->addr = engine_addr;
+			dev->size = slots * engine_page_size;
+
+			dev_dbg(&slave->dev,
+			"engine[%i]: type %x, version %x, addr %x size %x\n",
+					dev->unit, dev->engine_type, version,
+					dev->addr, dev->size);
+
+			dev_set_name(&dev->dev, "%02x:%02x:%02x:%02x",
+					slave->master->idx, slave->link,
+					slave->id, i - 2);
+
+			rc = device_register(&dev->dev);
+			if (rc) {
+				dev_warn(&slave->dev, "add failed: %d\n", rc);
+				put_device(&dev->dev);
+			}
+		}
+
+		engine_addr += slots * engine_page_size;
+
+		if (!(conf & FSI_SLAVE_CONF_NEXT_MASK))
+			break;
+	}
+
+	return 0;
+}
+
+static ssize_t fsi_slave_sysfs_raw_read(struct file *file,
+		struct kobject *kobj, struct bin_attribute *attr, char *buf,
+		loff_t off, size_t count)
+{
+	struct fsi_slave *slave = to_fsi_slave(kobj_to_dev(kobj));
+	size_t total_len, read_len;
+	int rc;
+
+	if (off < 0)
+		return -EINVAL;
+
+	if (off > 0xffffffff || count > 0xffffffff || off + count > 0xffffffff)
+		return -EINVAL;
+
+	for (total_len = 0; total_len < count; total_len += read_len) {
+		read_len = min_t(size_t, count, 4);
+		read_len -= off & 0x3;
+
+		rc = fsi_slave_read(slave, off, buf + total_len, read_len);
+		if (rc)
+			return rc;
+
+		off += read_len;
+	}
+
+	return count;
+}
+
+static ssize_t fsi_slave_sysfs_raw_write(struct file *file,
+		struct kobject *kobj, struct bin_attribute *attr,
+		char *buf, loff_t off, size_t count)
+{
+	struct fsi_slave *slave = to_fsi_slave(kobj_to_dev(kobj));
+	size_t total_len, write_len;
+	int rc;
+
+	if (off < 0)
+		return -EINVAL;
+
+	if (off > 0xffffffff || count > 0xffffffff || off + count > 0xffffffff)
+		return -EINVAL;
+
+	for (total_len = 0; total_len < count; total_len += write_len) {
+		write_len = min_t(size_t, count, 4);
+		write_len -= off & 0x3;
+
+		rc = fsi_slave_write(slave, off, buf + total_len, write_len);
+		if (rc)
+			return rc;
+
+		off += write_len;
+	}
+
+	return count;
+}
+
+static struct bin_attribute fsi_slave_raw_attr = {
+	.attr = {
+		.name = "raw",
+		.mode = 0600,
+	},
+	.size = 0,
+	.read = fsi_slave_sysfs_raw_read,
+	.write = fsi_slave_sysfs_raw_write,
+};
+
+static ssize_t fsi_slave_sysfs_term_write(struct file *file,
+		struct kobject *kobj, struct bin_attribute *attr,
+		char *buf, loff_t off, size_t count)
+{
+	struct fsi_slave *slave = to_fsi_slave(kobj_to_dev(kobj));
+	struct fsi_master *master = slave->master;
+
+	if (!master->term)
+		return -ENODEV;
+
+	master->term(master, slave->link, slave->id);
+	return count;
+}
+
+static struct bin_attribute fsi_slave_term_attr = {
+	.attr = {
+		.name = "term",
+		.mode = 0200,
+	},
+	.size = 0,
+	.write = fsi_slave_sysfs_term_write,
+};
+
+/* Encode slave local bus echo delay */
+static inline uint32_t fsi_smode_echodly(int x)
+{
+	return (x & FSI_SMODE_ED_MASK) << FSI_SMODE_ED_SHIFT;
+}
+
+/* Encode slave local bus send delay */
+static inline uint32_t fsi_smode_senddly(int x)
+{
+	return (x & FSI_SMODE_SD_MASK) << FSI_SMODE_SD_SHIFT;
+}
+
+/* Encode slave local bus clock rate ratio */
+static inline uint32_t fsi_smode_lbcrr(int x)
+{
+	return (x & FSI_SMODE_LBCRR_MASK) << FSI_SMODE_LBCRR_SHIFT;
+}
+
+/* Encode slave ID */
+static inline uint32_t fsi_smode_sid(int x)
+{
+	return (x & FSI_SMODE_SID_MASK) << FSI_SMODE_SID_SHIFT;
+}
+
+static const uint32_t fsi_slave_smode(int id)
+{
+	return FSI_SMODE_WSC | FSI_SMODE_ECRC
+		| fsi_smode_sid(id)
+		| fsi_smode_echodly(0xf) | fsi_smode_senddly(0xf)
+		| fsi_smode_lbcrr(0x8);
+}
+
+static int fsi_slave_set_smode(struct fsi_master *master, int link, int id)
+{
+	uint32_t smode;
+
+	/* set our smode register with the slave ID field to 0; this enables
+	 * extended slave addressing
+	 */
+	smode = fsi_slave_smode(id);
+	smode = cpu_to_be32(smode);
+
+	return fsi_master_write(master, link, id, FSI_SLAVE_BASE + FSI_SMODE,
+			&smode, sizeof(smode));
+}
+
+static void fsi_slave_release(struct device *dev)
+{
+	struct fsi_slave *slave = to_fsi_slave(dev);
+
+	kfree(slave);
+}
+
+static int fsi_slave_init(struct fsi_master *master, int link, uint8_t id)
+{
+	uint32_t chip_id, llmode;
+	struct fsi_slave *slave;
+	uint8_t crc;
+	int rc;
+
+	/* Currently, we only support single slaves on a link, and use the
+	 * full 23-bit address range
+	 */
+	if (id != 0)
+		return -EINVAL;
+
+	rc = fsi_master_read(master, link, id, 0, &chip_id, sizeof(chip_id));
+	if (rc) {
+		dev_dbg(&master->dev, "can't read slave %02x:%02x %d\n",
+				link, id, rc);
+		return -ENODEV;
+	}
+	chip_id = be32_to_cpu(chip_id);
+
+	crc = crc4(0, chip_id, 32);
+	if (crc) {
+		dev_warn(&master->dev, "slave %02x:%02x invalid chip id CRC!\n",
+				link, id);
+		return -EIO;
+	}
+
+	dev_info(&master->dev, "fsi: found chip %08x at %02x:%02x:%02x\n",
+			chip_id, master->idx, link, id);
+
+	rc = fsi_slave_set_smode(master, link, id);
+	if (rc) {
+		dev_warn(&master->dev,
+				"can't set smode on slave:%02x:%02x %d\n",
+				link, id, rc);
+		return -ENODEV;
+	}
+
+	/* If we're behind a master that doesn't provide a self-running bus
+	 * clock, put the slave into async mode
+	 */
+	if (master->flags & FSI_MASTER_FLAG_SWCLOCK) {
+		llmode = cpu_to_be32(FSI_LLMODE_ASYNC);
+		rc = fsi_master_write(master, link, id,
+				FSI_SLAVE_BASE + FSI_LLMODE,
+				&llmode, sizeof(llmode));
+		if (rc)
+			dev_warn(&master->dev,
+				"can't set llmode on slave:%02x:%02x %d\n",
+				link, id, rc);
+	}
+
+	/* We can communicate with a slave; create the slave device and
+	 * register.
+	 */
+	slave = kzalloc(sizeof(*slave), GFP_KERNEL);
+	if (!slave)
+		return -ENOMEM;
+
+	slave->master = master;
+	slave->dev.parent = &master->dev;
+	slave->dev.release = fsi_slave_release;
+	slave->link = link;
+	slave->id = id;
+	slave->size = FSI_SLAVE_SIZE_23b;
+
+	dev_set_name(&slave->dev, "slave@%02x:%02x", link, id);
+	rc = device_register(&slave->dev);
+	if (rc < 0) {
+		dev_warn(&master->dev, "failed to create slave device: %d\n",
+				rc);
+		put_device(&slave->dev);
+		return rc;
+	}
+
+	rc = device_create_bin_file(&slave->dev, &fsi_slave_raw_attr);
+	if (rc)
+		dev_warn(&slave->dev, "failed to create raw attr: %d\n", rc);
+
+	rc = device_create_bin_file(&slave->dev, &fsi_slave_term_attr);
+	if (rc)
+		dev_warn(&slave->dev, "failed to create term attr: %d\n", rc);
+
+	rc = fsi_slave_scan(slave);
+	if (rc)
+		dev_dbg(&master->dev, "failed during slave scan with: %d\n",
+				rc);
+
+	return rc;
+}
+
+/* FSI master support */
+static int fsi_check_access(uint32_t addr, size_t size)
+{
+	if (size != 1 && size != 2 && size != 4)
+		return -EINVAL;
+
+	if ((addr & 0x3) != (size & 0x3))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int fsi_master_read(struct fsi_master *master, int link,
+		uint8_t slave_id, uint32_t addr, void *val, size_t size)
+{
+	int rc;
+
+	trace_fsi_master_read(master, link, slave_id, addr, size);
+
+	rc = fsi_check_access(addr, size);
+	if (!rc)
+		rc = master->read(master, link, slave_id, addr, val, size);
+
+	trace_fsi_master_rw_result(master, link, slave_id, addr, size,
+			false, val, rc);
+
+	return rc;
+}
+
+static int fsi_master_write(struct fsi_master *master, int link,
+		uint8_t slave_id, uint32_t addr, const void *val, size_t size)
+{
+	int rc;
+
+	trace_fsi_master_write(master, link, slave_id, addr, size, val);
+
+	rc = fsi_check_access(addr, size);
+	if (!rc)
+		rc = master->write(master, link, slave_id, addr, val, size);
+
+	trace_fsi_master_rw_result(master, link, slave_id, addr, size,
+			true, val, rc);
+
+	return rc;
+}
+
+static int fsi_master_link_enable(struct fsi_master *master, int link)
+{
+	if (master->link_enable)
+		return master->link_enable(master, link);
+
+	return 0;
+}
+
+/*
+ * Issue a break command on this link
+ */
+static int fsi_master_break(struct fsi_master *master, int link)
+{
+	trace_fsi_master_break(master, link);
+
+	if (master->send_break)
+		return master->send_break(master, link);
+
+	return 0;
+}
+
+static int fsi_master_scan(struct fsi_master *master)
+{
+	int link, rc;
+
+	for (link = 0; link < master->n_links; link++) {
+		rc = fsi_master_link_enable(master, link);
+		if (rc) {
+			dev_dbg(&master->dev,
+				"enable link %d failed: %d\n", link, rc);
+			continue;
+		}
+		rc = fsi_master_break(master, link);
+		if (rc) {
+			dev_dbg(&master->dev,
+				"break to link %d failed: %d\n", link, rc);
+			continue;
+		}
+
+		fsi_slave_init(master, link, 0);
+	}
+
+	return 0;
+}
+
+static int fsi_slave_remove_device(struct device *dev, void *arg)
+{
+	device_unregister(dev);
+	return 0;
+}
+
+static int fsi_master_remove_slave(struct device *dev, void *arg)
+{
+	device_for_each_child(dev, NULL, fsi_slave_remove_device);
+	device_unregister(dev);
+	return 0;
+}
+
+static void fsi_master_unscan(struct fsi_master *master)
+{
+	device_for_each_child(&master->dev, NULL, fsi_master_remove_slave);
+}
+
+static ssize_t master_rescan_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct fsi_master *master = to_fsi_master(dev);
+	int rc;
+
+	fsi_master_unscan(master);
+	rc = fsi_master_scan(master);
+	if (rc < 0)
+		return rc;
+
+	return count;
+}
+
+static DEVICE_ATTR(rescan, 0200, NULL, master_rescan_store);
+
+static ssize_t master_break_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct fsi_master *master = to_fsi_master(dev);
+
+	fsi_master_break(master, 0);
+
+	return count;
+}
+
+static DEVICE_ATTR(break, 0200, NULL, master_break_store);
+
+int fsi_master_register(struct fsi_master *master)
+{
+	int rc;
+
+	if (!master)
+		return -EINVAL;
+
+	master->idx = ida_simple_get(&master_ida, 0, INT_MAX, GFP_KERNEL);
+	dev_set_name(&master->dev, "fsi%d", master->idx);
+
+	rc = device_register(&master->dev);
+	if (rc) {
+		ida_simple_remove(&master_ida, master->idx);
+		return rc;
+	}
+
+	rc = device_create_file(&master->dev, &dev_attr_rescan);
+	if (rc) {
+		device_unregister(&master->dev);
+		ida_simple_remove(&master_ida, master->idx);
+		return rc;
+	}
+
+	rc = device_create_file(&master->dev, &dev_attr_break);
+	if (rc) {
+		device_unregister(&master->dev);
+		ida_simple_remove(&master_ida, master->idx);
+		return rc;
+	}
+
+	fsi_master_scan(master);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(fsi_master_register);
+
+void fsi_master_unregister(struct fsi_master *master)
+{
+	if (master->idx >= 0) {
+		ida_simple_remove(&master_ida, master->idx);
+		master->idx = -1;
+	}
+
+	fsi_master_unscan(master);
+	device_unregister(&master->dev);
+}
+EXPORT_SYMBOL_GPL(fsi_master_unregister);
 
 /* FSI core & Linux bus type definitions */
 
@@ -39,6 +860,23 @@ static int fsi_bus_match(struct device *dev, struct device_driver *drv)
 	return 0;
 }
 
+int fsi_driver_register(struct fsi_driver *fsi_drv)
+{
+	if (!fsi_drv)
+		return -EINVAL;
+	if (!fsi_drv->id_table)
+		return -EINVAL;
+
+	return driver_register(&fsi_drv->drv);
+}
+EXPORT_SYMBOL_GPL(fsi_driver_register);
+
+void fsi_driver_unregister(struct fsi_driver *fsi_drv)
+{
+	driver_unregister(&fsi_drv->drv);
+}
+EXPORT_SYMBOL_GPL(fsi_driver_unregister);
+
 struct bus_type fsi_bus_type = {
 	.name		= "fsi",
 	.match		= fsi_bus_match,
@@ -57,3 +895,6 @@ static void fsi_exit(void)
 
 module_init(fsi_init);
 module_exit(fsi_exit);
+module_param(discard_errors, int, 0664);
+MODULE_LICENSE("GPL");
+MODULE_PARM_DESC(discard_errors, "Don't invoke error handling on bus accesses");
diff --git a/drivers/fsi/fsi-master-gpio.c b/drivers/fsi/fsi-master-gpio.c
new file mode 100644
index 000000000000..ae2618768508
--- /dev/null
+++ b/drivers/fsi/fsi-master-gpio.c
@@ -0,0 +1,604 @@
+/*
+ * A FSI master controller, using a simple GPIO bit-banging interface
+ */
+
+#include <linux/crc4.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/fsi.h>
+#include <linux/gpio/consumer.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include "fsi-master.h"
+
+#define	FSI_GPIO_STD_DLY	1	/* Standard pin delay in nS */
+#define	FSI_ECHO_DELAY_CLOCKS	16	/* Number clocks for echo delay */
+#define	FSI_PRE_BREAK_CLOCKS	50	/* Number clocks to prep for break */
+#define	FSI_BREAK_CLOCKS	256	/* Number of clocks to issue break */
+#define	FSI_POST_BREAK_CLOCKS	16000	/* Number clocks to set up cfam */
+#define	FSI_INIT_CLOCKS		5000	/* Clock out any old data */
+#define	FSI_GPIO_STD_DELAY	10	/* Standard GPIO delay in nS */
+					/* todo: adjust down as low as */
+					/* possible or eliminate */
+#define	FSI_GPIO_CMD_DPOLL      0x2
+#define	FSI_GPIO_CMD_TERM	0x3f
+#define FSI_GPIO_CMD_ABS_AR	0x4
+
+#define	FSI_GPIO_DPOLL_CLOCKS	100      /* < 21 will cause slave to hang */
+
+/* Bus errors */
+#define	FSI_GPIO_ERR_BUSY	1	/* Slave stuck in busy state */
+#define	FSI_GPIO_RESP_ERRA	2	/* Any (misc) Error */
+#define	FSI_GPIO_RESP_ERRC	3	/* Slave reports master CRC error */
+#define	FSI_GPIO_MTOE		4	/* Master time out error */
+#define	FSI_GPIO_CRC_INVAL	5	/* Master reports slave CRC error */
+
+/* Normal slave responses */
+#define	FSI_GPIO_RESP_BUSY	1
+#define	FSI_GPIO_RESP_ACK	0
+#define	FSI_GPIO_RESP_ACKD	4
+
+#define	FSI_GPIO_MAX_BUSY	100
+#define	FSI_GPIO_MTOE_COUNT	1000
+#define	FSI_GPIO_DRAIN_BITS	20
+#define	FSI_GPIO_CRC_SIZE	4
+#define	FSI_GPIO_MSG_ID_SIZE		2
+#define	FSI_GPIO_MSG_RESPID_SIZE	2
+#define	FSI_GPIO_PRIME_SLAVE_CLOCKS	100
+
+struct fsi_master_gpio {
+	struct fsi_master	master;
+	struct device		*dev;
+	spinlock_t		cmd_lock;	/* Lock for commands */
+	struct gpio_desc	*gpio_clk;
+	struct gpio_desc	*gpio_data;
+	struct gpio_desc	*gpio_trans;	/* Voltage translator */
+	struct gpio_desc	*gpio_enable;	/* FSI enable */
+	struct gpio_desc	*gpio_mux;	/* Mux control */
+};
+
+#define CREATE_TRACE_POINTS
+#include <trace/events/fsi_master_gpio.h>
+
+#define to_fsi_master_gpio(m) container_of(m, struct fsi_master_gpio, master)
+
+struct fsi_gpio_msg {
+	uint64_t	msg;
+	uint8_t		bits;
+};
+
+static void clock_toggle(struct fsi_master_gpio *master, int count)
+{
+	int i;
+
+	for (i = 0; i < count; i++) {
+		ndelay(FSI_GPIO_STD_DLY);
+		gpiod_set_value(master->gpio_clk, 0);
+		ndelay(FSI_GPIO_STD_DLY);
+		gpiod_set_value(master->gpio_clk, 1);
+	}
+}
+
+static int sda_in(struct fsi_master_gpio *master)
+{
+	int in;
+
+	ndelay(FSI_GPIO_STD_DLY);
+	in = gpiod_get_value(master->gpio_data);
+	return in ? 1 : 0;
+}
+
+static void sda_out(struct fsi_master_gpio *master, int value)
+{
+	gpiod_set_value(master->gpio_data, value);
+}
+
+static void set_sda_input(struct fsi_master_gpio *master)
+{
+	gpiod_direction_input(master->gpio_data);
+	gpiod_set_value(master->gpio_trans, 0);
+}
+
+static void set_sda_output(struct fsi_master_gpio *master, int value)
+{
+	gpiod_set_value(master->gpio_trans, 1);
+	gpiod_direction_output(master->gpio_data, value);
+}
+
+static void clock_zeros(struct fsi_master_gpio *master, int count)
+{
+	set_sda_output(master, 1);
+	clock_toggle(master, count);
+}
+
+static void serial_in(struct fsi_master_gpio *master, struct fsi_gpio_msg *msg,
+			uint8_t num_bits)
+{
+	uint8_t bit, in_bit;
+
+	set_sda_input(master);
+
+	for (bit = 0; bit < num_bits; bit++) {
+		clock_toggle(master, 1);
+		in_bit = sda_in(master);
+		msg->msg <<= 1;
+		msg->msg |= ~in_bit & 0x1;	/* Data is active low */
+	}
+	msg->bits += num_bits;
+
+	trace_fsi_master_gpio_in(master, num_bits, msg->msg);
+}
+
+static void serial_out(struct fsi_master_gpio *master,
+			const struct fsi_gpio_msg *cmd)
+{
+	uint8_t bit;
+	uint64_t msg = ~cmd->msg;	/* Data is active low */
+	uint64_t sda_mask = 0x1ULL << (cmd->bits - 1);
+	uint64_t last_bit = ~0;
+	int next_bit;
+
+	trace_fsi_master_gpio_out(master, cmd->bits, cmd->msg);
+
+	if (!cmd->bits) {
+		dev_warn(master->dev, "trying to output 0 bits\n");
+		return;
+	}
+	set_sda_output(master, 0);
+
+	/* Send the start bit */
+	sda_out(master, 0);
+	clock_toggle(master, 1);
+
+	/* Send the message */
+	for (bit = 0; bit < cmd->bits; bit++) {
+		next_bit = (msg & sda_mask) >> (cmd->bits - 1);
+		if (last_bit ^ next_bit) {
+			sda_out(master, next_bit);
+			last_bit = next_bit;
+		}
+		clock_toggle(master, 1);
+		msg <<= 1;
+	}
+}
+
+static void msg_push_bits(struct fsi_gpio_msg *msg, uint64_t data, int bits)
+{
+	msg->msg <<= bits;
+	msg->msg |= data & ((1ull << bits) - 1);
+	msg->bits += bits;
+}
+
+static void msg_push_crc(struct fsi_gpio_msg *msg)
+{
+	uint8_t crc;
+	int top;
+
+	top = msg->bits & 0x3;
+
+	/* start bit, and any non-aligned top bits */
+	crc = crc4(0, 1 << top | msg->msg >> (msg->bits - top), top + 1);
+
+	/* aligned bits */
+	crc = crc4(crc, msg->msg, msg->bits - top);
+
+	msg_push_bits(msg, crc, 4);
+}
+
+/*
+ * Encode an Absolute Address command
+ */
+static void build_abs_ar_command(struct fsi_gpio_msg *cmd,
+		uint8_t id, uint32_t addr, size_t size, const void *data)
+{
+	bool write = !!data;
+	uint8_t ds;
+	int i;
+
+	cmd->bits = 0;
+	cmd->msg = 0;
+
+	msg_push_bits(cmd, id, 2);
+	msg_push_bits(cmd, FSI_GPIO_CMD_ABS_AR, 3);
+	msg_push_bits(cmd, write ? 0 : 1, 1);
+
+	/*
+	 * The read/write size is encoded in the lower bits of the address
+	 * (as it must be naturally-aligned), and the following ds bit.
+	 *
+	 *	size	addr:1	addr:0	ds
+	 *	1	x	x	0
+	 *	2	x	0	1
+	 *	4	0	1	1
+	 *
+	 */
+	ds = size > 1 ? 1 : 0;
+	addr &= ~(size - 1);
+	if (size == 4)
+		addr |= 1;
+
+	msg_push_bits(cmd, addr & ((1 << 21) - 1), 21);
+	msg_push_bits(cmd, ds, 1);
+	for (i = 0; write && i < size; i++)
+		msg_push_bits(cmd, ((uint8_t *)data)[i], 8);
+
+	msg_push_crc(cmd);
+}
+
+static void build_dpoll_command(struct fsi_gpio_msg *cmd, uint8_t slave_id)
+{
+	cmd->bits = 0;
+	cmd->msg = 0;
+
+	msg_push_bits(cmd, slave_id, 2);
+	msg_push_bits(cmd, FSI_GPIO_CMD_DPOLL, 3);
+	msg_push_crc(cmd);
+}
+
+static void echo_delay(struct fsi_master_gpio *master)
+{
+	set_sda_output(master, 1);
+	clock_toggle(master, FSI_ECHO_DELAY_CLOCKS);
+}
+
+static void build_term_command(struct fsi_gpio_msg *cmd, uint8_t slave_id)
+{
+	cmd->bits = 0;
+	cmd->msg = 0;
+
+	msg_push_bits(cmd, slave_id, 2);
+	msg_push_bits(cmd, FSI_GPIO_CMD_TERM, 6);
+	msg_push_crc(cmd);
+}
+
+/*
+ * Store information on master errors so handler can detect and clean
+ * up the bus
+ */
+static void fsi_master_gpio_error(struct fsi_master_gpio *master, int error)
+{
+
+}
+
+static int read_one_response(struct fsi_master_gpio *master,
+		uint8_t data_size, struct fsi_gpio_msg *msgp, uint8_t *tagp)
+{
+	struct fsi_gpio_msg msg;
+	uint8_t id, tag;
+	uint32_t crc;
+	int i;
+
+	/* wait for the start bit */
+	for (i = 0; i < FSI_GPIO_MTOE_COUNT; i++) {
+		msg.bits = 0;
+		msg.msg = 0;
+		serial_in(master, &msg, 1);
+		if (msg.msg)
+			break;
+	}
+	if (i == FSI_GPIO_MTOE_COUNT) {
+		dev_dbg(master->dev,
+			"Master time out waiting for response\n");
+		fsi_master_gpio_error(master, FSI_GPIO_MTOE);
+		return -EIO;
+	}
+
+	msg.bits = 0;
+	msg.msg = 0;
+
+	/* Read slave ID & response tag */
+	serial_in(master, &msg, 4);
+
+	id = (msg.msg >> FSI_GPIO_MSG_RESPID_SIZE) & 0x3;
+	tag = msg.msg & 0x3;
+
+	/* If we have an ACK and we're expecting data, clock the data in too */
+	if (tag == FSI_GPIO_RESP_ACK && data_size)
+		serial_in(master, &msg, data_size * 8);
+
+	/* read CRC */
+	serial_in(master, &msg, FSI_GPIO_CRC_SIZE);
+
+	/* we have a whole message now; check CRC */
+	crc = crc4(0, 1, 1);
+	crc = crc4(crc, msg.msg, msg.bits);
+	if (crc) {
+		dev_dbg(master->dev, "ERR response CRC\n");
+		fsi_master_gpio_error(master, FSI_GPIO_CRC_INVAL);
+		return -EIO;
+	}
+
+	if (msgp)
+		*msgp = msg;
+	if (tagp)
+		*tagp = tag;
+
+	return 0;
+}
+
+static int issue_term(struct fsi_master_gpio *master, uint8_t slave)
+{
+	struct fsi_gpio_msg cmd;
+	uint8_t tag;
+	int rc;
+
+	build_term_command(&cmd, slave);
+	serial_out(master, &cmd);
+	echo_delay(master);
+
+	rc = read_one_response(master, 0, NULL, &tag);
+	if (rc < 0) {
+		dev_err(master->dev,
+				"TERM failed; lost communication with slave\n");
+		return -EIO;
+	} else if (tag != FSI_GPIO_RESP_ACK) {
+		dev_err(master->dev, "TERM failed; response %d\n", tag);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int poll_for_response(struct fsi_master_gpio *master,
+		uint8_t slave, uint8_t size, void *data)
+{
+	struct fsi_gpio_msg response, cmd;
+	int busy_count = 0, rc, i;
+	uint8_t tag;
+	uint8_t *data_byte = data;
+
+retry:
+	rc = read_one_response(master, size, &response, &tag);
+	if (rc)
+		return rc;
+
+	switch (tag) {
+	case FSI_GPIO_RESP_ACK:
+		if (size && data) {
+			uint64_t val = response.msg;
+			/* clear crc & mask */
+			val >>= 4;
+			val &= (1ull << (size * 8)) - 1;
+
+			for (i = 0; i < size; i++) {
+				data_byte[size-i-1] = val;
+				val >>= 8;
+			}
+		}
+		break;
+	case FSI_GPIO_RESP_BUSY:
+		/*
+		 * Its necessary to clock slave before issuing
+		 * d-poll, not indicated in the hardware protocol
+		 * spec. < 20 clocks causes slave to hang, 21 ok.
+		 */
+		clock_zeros(master, FSI_GPIO_DPOLL_CLOCKS);
+		if (busy_count++ < FSI_GPIO_MAX_BUSY) {
+			build_dpoll_command(&cmd, slave);
+			serial_out(master, &cmd);
+			echo_delay(master);
+			goto retry;
+		}
+		dev_warn(master->dev,
+			"ERR slave is stuck in busy state, issuing TERM\n");
+		issue_term(master, slave);
+		rc = -EIO;
+		break;
+
+	case FSI_GPIO_RESP_ERRA:
+	case FSI_GPIO_RESP_ERRC:
+		dev_dbg(master->dev, "ERR%c received: 0x%x\n",
+			tag == FSI_GPIO_RESP_ERRA ? 'A' : 'C',
+			(int)response.msg);
+		fsi_master_gpio_error(master, response.msg);
+		rc = -EIO;
+		break;
+	}
+
+	/* Clock the slave enough to be ready for next operation */
+	clock_zeros(master, FSI_GPIO_PRIME_SLAVE_CLOCKS);
+	return rc;
+}
+
+static int fsi_master_gpio_xfer(struct fsi_master_gpio *master, uint8_t slave,
+		struct fsi_gpio_msg *cmd, size_t resp_len, void *resp)
+{
+	unsigned long flags;
+	int rc;
+
+	spin_lock_irqsave(&master->cmd_lock, flags);
+	serial_out(master, cmd);
+	echo_delay(master);
+	rc = poll_for_response(master, slave, resp_len, resp);
+	spin_unlock_irqrestore(&master->cmd_lock, flags);
+
+	return rc;
+}
+
+static int fsi_master_gpio_read(struct fsi_master *_master, int link,
+		uint8_t id, uint32_t addr, void *val, size_t size)
+{
+	struct fsi_master_gpio *master = to_fsi_master_gpio(_master);
+	struct fsi_gpio_msg cmd;
+
+	if (link != 0)
+		return -ENODEV;
+
+	build_abs_ar_command(&cmd, id, addr, size, NULL);
+	return fsi_master_gpio_xfer(master, id, &cmd, size, val);
+}
+
+static int fsi_master_gpio_write(struct fsi_master *_master, int link,
+		uint8_t id, uint32_t addr, const void *val, size_t size)
+{
+	struct fsi_master_gpio *master = to_fsi_master_gpio(_master);
+	struct fsi_gpio_msg cmd;
+
+	if (link != 0)
+		return -ENODEV;
+
+	build_abs_ar_command(&cmd, id, addr, size, val);
+	return fsi_master_gpio_xfer(master, id, &cmd, 0, NULL);
+}
+
+static int fsi_master_gpio_term(struct fsi_master *_master,
+		int link, uint8_t id)
+{
+	struct fsi_master_gpio *master = to_fsi_master_gpio(_master);
+	struct fsi_gpio_msg cmd;
+
+	if (link != 0)
+		return -ENODEV;
+
+	build_term_command(&cmd, id);
+	return fsi_master_gpio_xfer(master, id, &cmd, 0, NULL);
+}
+
+static int fsi_master_gpio_break(struct fsi_master *_master, int link)
+{
+	struct fsi_master_gpio *master = to_fsi_master_gpio(_master);
+
+	if (link != 0)
+		return -ENODEV;
+
+	trace_fsi_master_gpio_break(master);
+
+	set_sda_output(master, 1);
+	sda_out(master, 1);
+	clock_toggle(master, FSI_PRE_BREAK_CLOCKS);
+	sda_out(master, 0);
+	clock_toggle(master, FSI_BREAK_CLOCKS);
+	echo_delay(master);
+	sda_out(master, 1);
+	clock_toggle(master, FSI_POST_BREAK_CLOCKS);
+
+	/* Wait for logic reset to take effect */
+	udelay(200);
+
+	return 0;
+}
+
+static void fsi_master_gpio_init(struct fsi_master_gpio *master)
+{
+	gpiod_direction_output(master->gpio_mux, 1);
+	gpiod_direction_output(master->gpio_trans, 1);
+	gpiod_direction_output(master->gpio_enable, 1);
+	gpiod_direction_output(master->gpio_clk, 1);
+	gpiod_direction_output(master->gpio_data, 1);
+
+	/* todo: evaluate if clocks can be reduced */
+	clock_zeros(master, FSI_INIT_CLOCKS);
+}
+
+static int fsi_master_gpio_link_enable(struct fsi_master *_master, int link)
+{
+	struct fsi_master_gpio *master = to_fsi_master_gpio(_master);
+
+	if (link != 0)
+		return -ENODEV;
+	gpiod_set_value(master->gpio_enable, 1);
+
+	return 0;
+}
+
+static int fsi_master_gpio_probe(struct platform_device *pdev)
+{
+	struct fsi_master_gpio *master;
+	struct gpio_desc *gpio;
+
+	master = devm_kzalloc(&pdev->dev, sizeof(*master), GFP_KERNEL);
+	if (!master)
+		return -ENOMEM;
+
+	master->dev = &pdev->dev;
+	master->master.dev.parent = master->dev;
+
+	gpio = devm_gpiod_get(&pdev->dev, "clock", 0);
+	if (IS_ERR(gpio)) {
+		dev_err(&pdev->dev, "failed to get clock gpio\n");
+		return PTR_ERR(gpio);
+	}
+	master->gpio_clk = gpio;
+
+	gpio = devm_gpiod_get(&pdev->dev, "data", 0);
+	if (IS_ERR(gpio)) {
+		dev_err(&pdev->dev, "failed to get data gpio\n");
+		return PTR_ERR(gpio);
+	}
+	master->gpio_data = gpio;
+
+	/* Optional GPIOs */
+	gpio = devm_gpiod_get_optional(&pdev->dev, "trans", 0);
+	if (IS_ERR(gpio)) {
+		dev_err(&pdev->dev, "failed to get trans gpio\n");
+		return PTR_ERR(gpio);
+	}
+	master->gpio_trans = gpio;
+
+	gpio = devm_gpiod_get_optional(&pdev->dev, "enable", 0);
+	if (IS_ERR(gpio)) {
+		dev_err(&pdev->dev, "failed to get enable gpio\n");
+		return PTR_ERR(gpio);
+	}
+	master->gpio_enable = gpio;
+
+	gpio = devm_gpiod_get_optional(&pdev->dev, "mux", 0);
+	if (IS_ERR(gpio)) {
+		dev_err(&pdev->dev, "failed to get mux gpio\n");
+		return PTR_ERR(gpio);
+	}
+	master->gpio_mux = gpio;
+
+	master->master.n_links = 1;
+	master->master.flags = FSI_MASTER_FLAG_SWCLOCK;
+	master->master.read = fsi_master_gpio_read;
+	master->master.write = fsi_master_gpio_write;
+	master->master.term = fsi_master_gpio_term;
+	master->master.send_break = fsi_master_gpio_break;
+	master->master.link_enable = fsi_master_gpio_link_enable;
+	platform_set_drvdata(pdev, master);
+	spin_lock_init(&master->cmd_lock);
+
+	fsi_master_gpio_init(master);
+
+	return fsi_master_register(&master->master);
+}
+
+
+static int fsi_master_gpio_remove(struct platform_device *pdev)
+{
+	struct fsi_master_gpio *master = platform_get_drvdata(pdev);
+
+	devm_gpiod_put(&pdev->dev, master->gpio_clk);
+	devm_gpiod_put(&pdev->dev, master->gpio_data);
+	if (master->gpio_trans)
+		devm_gpiod_put(&pdev->dev, master->gpio_trans);
+	if (master->gpio_enable)
+		devm_gpiod_put(&pdev->dev, master->gpio_enable);
+	if (master->gpio_mux)
+		devm_gpiod_put(&pdev->dev, master->gpio_mux);
+	fsi_master_unregister(&master->master);
+
+	return 0;
+}
+
+static const struct of_device_id fsi_master_gpio_match[] = {
+	{ .compatible = "fsi-master-gpio" },
+	{ },
+};
+
+static struct platform_driver fsi_master_gpio_driver = {
+	.driver = {
+		.name		= "fsi-master-gpio",
+		.of_match_table	= fsi_master_gpio_match,
+	},
+	.probe	= fsi_master_gpio_probe,
+	.remove = fsi_master_gpio_remove,
+};
+
+module_platform_driver(fsi_master_gpio_driver);
+MODULE_LICENSE("GPL");
diff --git a/drivers/fsi/fsi-master-hub.c b/drivers/fsi/fsi-master-hub.c
new file mode 100644
index 000000000000..133b9bff1d65
--- /dev/null
+++ b/drivers/fsi/fsi-master-hub.c
@@ -0,0 +1,327 @@
+/*
+ * FSI hub master driver
+ *
+ * Copyright (C) IBM Corporation 2016
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/delay.h>
+#include <linux/fsi.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#include "fsi-master.h"
+
+/* Control Registers */
+#define FSI_MMODE		0x0		/* R/W: mode */
+#define FSI_MDLYR		0x4		/* R/W: delay */
+#define FSI_MCRSP		0x8		/* R/W: clock rate */
+#define FSI_MENP0		0x10		/* R/W: enable */
+#define FSI_MLEVP0		0x18		/* R: plug detect */
+#define FSI_MSENP0		0x18		/* S: Set enable */
+#define FSI_MCENP0		0x20		/* C: Clear enable */
+#define FSI_MAEB		0x70		/* R: Error address */
+#define FSI_MVER		0x74		/* R: master version/type */
+#define FSI_MRESP0		0xd0		/* W: Port reset */
+#define FSI_MESRB0		0x1d0		/* R: Master error status */
+#define FSI_MRESB0		0x1d0		/* W: Reset bridge */
+#define FSI_MECTRL		0x2e0		/* W: Error control */
+
+/* MMODE: Mode control */
+#define FSI_MMODE_EIP		0x80000000	/* Enable interrupt polling */
+#define FSI_MMODE_ECRC		0x40000000	/* Enable error recovery */
+#define FSI_MMODE_EPC		0x10000000	/* Enable parity checking */
+#define FSI_MMODE_P8_TO_LSB	0x00000010	/* Timeout value LSB */
+						/*   MSB=1, LSB=0 is 0.8 ms */
+						/*   MSB=0, LSB=1 is 0.9 ms */
+#define FSI_MMODE_CRS0SHFT	18		/* Clk rate selection 0 shift */
+#define FSI_MMODE_CRS0MASK	0x3ff		/* Clk rate selection 0 mask */
+#define FSI_MMODE_CRS1SHFT	8		/* Clk rate selection 1 shift */
+#define FSI_MMODE_CRS1MASK	0x3ff		/* Clk rate selection 1 mask */
+
+/* MRESB: Reset brindge */
+#define FSI_MRESB_RST_GEN	0x80000000	/* General reset */
+#define FSI_MRESB_RST_ERR	0x40000000	/* Error Reset */
+
+/* MRESB: Reset port */
+#define FSI_MRESP_RST_ALL_MASTER 0x20000000	/* Reset all FSI masters */
+#define FSI_MRESP_RST_ALL_LINK	0x10000000	/* Reset all FSI port contr. */
+#define FSI_MRESP_RST_MCR	0x08000000	/* Reset FSI master reg. */
+#define FSI_MRESP_RST_PYE	0x04000000	/* Reset FSI parity error */
+#define FSI_MRESP_RST_ALL	0xfc000000	/* Reset any error */
+
+/* MECTRL: Error control */
+#define FSI_MECTRL_EOAE		0x8000		/* Enable machine check when */
+						/* master 0 in error */
+#define FSI_MECTRL_P8_AUTO_TERM	0x4000		/* Auto terminate */
+
+#define FSI_ENGID_HUB_MASTER		0x1c
+#define FSI_HUB_LINK_OFFSET		0x80000
+#define FSI_HUB_LINK_SIZE		0x80000
+#define FSI_HUB_MASTER_MAX_LINKS	8
+
+#define FSI_LINK_ENABLE_SETUP_TIME	10	/* in mS */
+
+/*
+ * FSI hub master support
+ *
+ * A hub master increases the number of potential target devices that the
+ * primary FSI master can access. For each link a primary master supports,
+ * each of those links can in turn be chained to a hub master with multiple
+ * links of its own.
+ *
+ * The hub is controlled by a set of control registers exposed as a regular fsi
+ * device (the hub->upstream device), and provides access to the downstream FSI
+ * bus as through an address range on the slave itself (->addr and ->size).
+ *
+ * [This differs from "cascaded" masters, which expose the entire downstream
+ * bus entirely through the fsi device address range, and so have a smaller
+ * accessible address space.]
+ */
+struct fsi_master_hub {
+	struct fsi_master	master;
+	struct fsi_device	*upstream;
+	uint32_t		addr, size;	/* slave-relative addr of */
+						/* master address space */
+};
+
+#define to_fsi_master_hub(m) container_of(m, struct fsi_master_hub, master)
+
+static int hub_master_read(struct fsi_master *master, int link,
+			uint8_t id, uint32_t addr, void *val, size_t size)
+{
+	struct fsi_master_hub *hub = to_fsi_master_hub(master);
+
+	if (id != 0)
+		return -EINVAL;
+
+	addr += hub->addr + (link * FSI_HUB_LINK_SIZE);
+	return fsi_slave_read(hub->upstream->slave, addr, val, size);
+}
+
+static int hub_master_write(struct fsi_master *master, int link,
+			uint8_t id, uint32_t addr, const void *val, size_t size)
+{
+	struct fsi_master_hub *hub = to_fsi_master_hub(master);
+
+	if (id != 0)
+		return -EINVAL;
+
+	addr += hub->addr + (link * FSI_HUB_LINK_SIZE);
+	return fsi_slave_write(hub->upstream->slave, addr, val, size);
+}
+
+static int hub_master_break(struct fsi_master *master, int link)
+{
+	uint32_t addr, cmd;
+
+	addr = 0x4;
+	cmd = cpu_to_be32(0xc0de0000);
+
+	return hub_master_write(master, link, 0, addr, &cmd, sizeof(cmd));
+}
+
+static int hub_master_link_enable(struct fsi_master *master, int link)
+{
+	struct fsi_master_hub *hub = to_fsi_master_hub(master);
+	int idx, bit;
+	__be32 reg;
+	int rc;
+
+	idx = link / 32;
+	bit = link % 32;
+
+	reg = cpu_to_be32(0x80000000 >> bit);
+
+	rc = fsi_device_write(hub->upstream, FSI_MSENP0 + (4 * idx), &reg, 4);
+
+	mdelay(FSI_LINK_ENABLE_SETUP_TIME);
+
+	fsi_device_read(hub->upstream, FSI_MENP0 + (4 * idx), &reg, 4);
+
+	return rc;
+}
+
+static void hub_master_release(struct device *dev)
+{
+	struct fsi_master_hub *hub = to_fsi_master_hub(dev_to_fsi_master(dev));
+
+	kfree(hub);
+}
+
+/* mmode encoders */
+static inline u32 fsi_mmode_crs0(u32 x)
+{
+	return (x & FSI_MMODE_CRS0MASK) << FSI_MMODE_CRS0SHFT;
+}
+
+static inline u32 fsi_mmode_crs1(u32 x)
+{
+	return (x & FSI_MMODE_CRS1MASK) << FSI_MMODE_CRS1SHFT;
+}
+
+static int hub_master_init(struct fsi_master_hub *hub)
+{
+	struct fsi_device *dev = hub->upstream;
+	__be32 reg;
+	int rc;
+
+	reg = cpu_to_be32(FSI_MRESP_RST_ALL_MASTER | FSI_MRESP_RST_ALL_LINK
+			| FSI_MRESP_RST_MCR | FSI_MRESP_RST_PYE);
+	rc = fsi_device_write(dev, FSI_MRESP0, &reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	/* Initialize the MFSI (hub master) engine */
+	reg = cpu_to_be32(FSI_MRESP_RST_ALL_MASTER | FSI_MRESP_RST_ALL_LINK
+			| FSI_MRESP_RST_MCR | FSI_MRESP_RST_PYE);
+	rc = fsi_device_write(dev, FSI_MRESP0, &reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	reg = cpu_to_be32(FSI_MECTRL_EOAE | FSI_MECTRL_P8_AUTO_TERM);
+	rc = fsi_device_write(dev, FSI_MECTRL, &reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	reg = cpu_to_be32(FSI_MMODE_EIP | FSI_MMODE_ECRC | FSI_MMODE_EPC
+			| fsi_mmode_crs0(1) | fsi_mmode_crs1(1)
+			| FSI_MMODE_P8_TO_LSB);
+	rc = fsi_device_write(dev, FSI_MMODE, &reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	reg = cpu_to_be32(0xffff0000);
+	rc = fsi_device_write(dev, FSI_MDLYR, &reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	reg = ~0;
+	rc = fsi_device_write(dev, FSI_MSENP0, &reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	/* Leave enabled long enough for master logic to set up */
+	mdelay(FSI_LINK_ENABLE_SETUP_TIME);
+
+	rc = fsi_device_write(dev, FSI_MCENP0, &reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	rc = fsi_device_read(dev, FSI_MAEB, &reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	reg = cpu_to_be32(FSI_MRESP_RST_ALL_MASTER | FSI_MRESP_RST_ALL_LINK);
+	rc = fsi_device_write(dev, FSI_MRESP0, &reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	rc = fsi_device_read(dev, FSI_MLEVP0, &reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	/* Reset the master bridge */
+	reg = cpu_to_be32(FSI_MRESB_RST_GEN);
+	rc = fsi_device_write(dev, FSI_MRESB0, &reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	reg = cpu_to_be32(FSI_MRESB_RST_ERR);
+	return fsi_device_write(dev, FSI_MRESB0, &reg, sizeof(reg));
+}
+
+static int hub_master_probe(struct device *dev)
+{
+	struct fsi_device *fsi_dev = to_fsi_dev(dev);
+	struct fsi_master_hub *hub;
+	uint32_t reg, links;
+	__be32 __reg;
+	int rc;
+
+	rc = fsi_device_read(fsi_dev, FSI_MVER, &__reg, sizeof(__reg));
+	if (rc)
+		return rc;
+
+	reg = be32_to_cpu(__reg);
+	links = (reg >> 8) & 0xff;
+	dev_info(dev, "hub version %08x (%d links)\n", reg, links);
+
+	rc = fsi_slave_claim_range(fsi_dev->slave, FSI_HUB_LINK_OFFSET,
+			FSI_HUB_LINK_SIZE * links);
+	if (rc) {
+		dev_err(dev, "can't claim slave address range for links");
+		return rc;
+	}
+
+	hub = kzalloc(sizeof(*hub), GFP_KERNEL);
+	if (!hub) {
+		rc = -ENOMEM;
+		goto err_release;
+	}
+
+	hub->addr = FSI_HUB_LINK_OFFSET;
+	hub->size = FSI_HUB_LINK_SIZE * links;
+	hub->upstream = fsi_dev;
+
+	hub->master.dev.parent = dev;
+	hub->master.dev.release = hub_master_release;
+
+	hub->master.n_links = links;
+	hub->master.read = hub_master_read;
+	hub->master.write = hub_master_write;
+	hub->master.send_break = hub_master_break;
+	hub->master.link_enable = hub_master_link_enable;
+
+	dev_set_drvdata(dev, hub);
+
+	hub_master_init(hub);
+
+	rc = fsi_master_register(&hub->master);
+	if (!rc)
+		return 0;
+
+	kfree(hub);
+err_release:
+	fsi_slave_release_range(fsi_dev->slave, FSI_HUB_LINK_OFFSET,
+			FSI_HUB_LINK_SIZE * links);
+	return rc;
+}
+
+static int hub_master_remove(struct device *dev)
+{
+	struct fsi_master_hub *hub = dev_get_drvdata(dev);
+
+	fsi_master_unregister(&hub->master);
+	fsi_slave_release_range(hub->upstream->slave, hub->addr, hub->size);
+	return 0;
+}
+
+static struct fsi_device_id hub_master_ids[] = {
+	{
+		.engine_type = FSI_ENGID_HUB_MASTER,
+		.version = FSI_VERSION_ANY,
+	},
+	{ 0 }
+};
+
+static struct fsi_driver hub_master_driver = {
+	.id_table = hub_master_ids,
+	.drv = {
+		.name = "fsi-master-hub",
+		.bus = &fsi_bus_type,
+		.probe = hub_master_probe,
+		.remove = hub_master_remove,
+	}
+};
+
+module_fsi_driver(hub_master_driver);
+MODULE_LICENSE("GPL");
diff --git a/drivers/fsi/fsi-master.h b/drivers/fsi/fsi-master.h
new file mode 100644
index 000000000000..12f7b119567d
--- /dev/null
+++ b/drivers/fsi/fsi-master.h
@@ -0,0 +1,43 @@
+/*
+ * FSI master definitions. These comprise the core <--> master interface,
+ * to allow the core to interact with the (hardware-specific) masters.
+ *
+ * Copyright (C) IBM Corporation 2016
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef DRIVERS_FSI_MASTER_H
+#define DRIVERS_FSI_MASTER_H
+
+#include <linux/device.h>
+
+#define FSI_MASTER_FLAG_SWCLOCK		0x1
+
+struct fsi_master {
+	struct device	dev;
+	int		idx;
+	int		n_links;
+	int		flags;
+	int		(*read)(struct fsi_master *, int link, uint8_t id,
+				uint32_t addr, void *val, size_t size);
+	int		(*write)(struct fsi_master *, int link, uint8_t id,
+				uint32_t addr, const void *val, size_t size);
+	int		(*term)(struct fsi_master *, int link, uint8_t id);
+	int		(*send_break)(struct fsi_master *, int link);
+	int		(*link_enable)(struct fsi_master *, int link);
+};
+
+#define dev_to_fsi_master(d) container_of(d, struct fsi_master, dev)
+
+extern int fsi_master_register(struct fsi_master *master);
+extern void fsi_master_unregister(struct fsi_master *master);
+
+#endif /* DRIVERS_FSI_MASTER_H */
diff --git a/drivers/fsi/fsi-scom.c b/drivers/fsi/fsi-scom.c
new file mode 100644
index 000000000000..98d062fd353e
--- /dev/null
+++ b/drivers/fsi/fsi-scom.c
@@ -0,0 +1,263 @@
+/*
+ * SCOM FSI Client device driver
+ *
+ * Copyright (C) IBM Corporation 2016
+ *
+ * This program is free software; you can 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
+ * MERGCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/fsi.h>
+#include <linux/module.h>
+#include <linux/cdev.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/slab.h>
+#include <linux/miscdevice.h>
+#include <linux/list.h>
+#include <linux/idr.h>
+
+#define FSI_ENGID_SCOM		0x5
+
+#define SCOM_FSI2PIB_DELAY	50
+
+/* SCOM engine register set */
+#define SCOM_DATA0_REG		0x00
+#define SCOM_DATA1_REG		0x04
+#define SCOM_CMD_REG		0x08
+#define SCOM_RESET_REG		0x1C
+
+#define SCOM_RESET_CMD		0x80000000
+#define SCOM_WRITE_CMD		0x80000000
+
+struct scom_device {
+	struct list_head link;
+	struct fsi_device *fsi_dev;
+	struct miscdevice mdev;
+	char	name[32];
+	int idx;
+};
+
+#define to_scom_dev(x)		container_of((x), struct scom_device, mdev)
+
+static struct list_head scom_devices;
+
+static DEFINE_IDA(scom_ida);
+
+static int put_scom(struct scom_device *scom_dev, uint64_t value,
+			uint32_t addr)
+{
+	int rc;
+	uint32_t data;
+
+	data = cpu_to_be32(SCOM_RESET_CMD);
+	rc = fsi_device_write(scom_dev->fsi_dev, SCOM_RESET_REG, &data,
+				sizeof(uint32_t));
+	if (rc)
+		return rc;
+
+	data = cpu_to_be32((value >> 32) & 0xffffffff);
+	rc = fsi_device_write(scom_dev->fsi_dev, SCOM_DATA0_REG, &data,
+				sizeof(uint32_t));
+	if (rc)
+		return rc;
+
+	data = cpu_to_be32(value & 0xffffffff);
+	rc = fsi_device_write(scom_dev->fsi_dev, SCOM_DATA1_REG, &data,
+				sizeof(uint32_t));
+	if (rc)
+		return rc;
+
+	data = cpu_to_be32(SCOM_WRITE_CMD | addr);
+	return fsi_device_write(scom_dev->fsi_dev, SCOM_CMD_REG, &data,
+				sizeof(uint32_t));
+}
+
+static int get_scom(struct scom_device *scom_dev, uint64_t *value,
+			uint32_t addr)
+{
+	uint32_t result, data;
+	int rc;
+
+	*value = 0ULL;
+	data = cpu_to_be32(addr);
+	rc = fsi_device_write(scom_dev->fsi_dev, SCOM_CMD_REG, &data,
+				sizeof(uint32_t));
+	if (rc)
+		return rc;
+
+	rc = fsi_device_read(scom_dev->fsi_dev, SCOM_DATA0_REG, &result,
+				sizeof(uint32_t));
+	if (rc)
+		return rc;
+
+	*value |= (uint64_t)cpu_to_be32(result) << 32;
+	rc = fsi_device_read(scom_dev->fsi_dev, SCOM_DATA1_REG, &result,
+				sizeof(uint32_t));
+	if (rc)
+		return rc;
+
+	*value |= cpu_to_be32(result);
+
+	return 0;
+}
+
+static ssize_t scom_read(struct file *filep, char __user *buf, size_t len,
+			loff_t *offset)
+{
+	int rc;
+	struct miscdevice *mdev =
+				(struct miscdevice *)filep->private_data;
+	struct scom_device *scom = to_scom_dev(mdev);
+	struct device *dev = &scom->fsi_dev->dev;
+	uint64_t val;
+
+	if (len != sizeof(uint64_t))
+		return -EINVAL;
+
+	rc = get_scom(scom, &val, *offset);
+	if (rc) {
+		dev_dbg(dev, "get_scom fail:%d\n", rc);
+		return rc;
+	}
+
+	rc = copy_to_user(buf, &val, len);
+	if (rc)
+		dev_dbg(dev, "copy to user failed:%d\n", rc);
+
+	return rc ? rc : len;
+}
+
+static ssize_t scom_write(struct file *filep, const char __user *buf,
+			size_t len, loff_t *offset)
+{
+	int rc;
+	struct miscdevice *mdev = filep->private_data;
+	struct scom_device *scom = to_scom_dev(mdev);
+	struct device *dev = &scom->fsi_dev->dev;
+	uint64_t val;
+
+	if (len != sizeof(uint64_t))
+		return -EINVAL;
+
+	rc = copy_from_user(&val, buf, len);
+	if (rc) {
+		dev_dbg(dev, "copy from user failed:%d\n", rc);
+		return -EINVAL;
+	}
+
+	rc = put_scom(scom, val, *offset);
+	if (rc) {
+		dev_dbg(dev, "put_scom failed with:%d\n", rc);
+		return rc;
+	}
+
+	return len;
+}
+
+static loff_t scom_llseek(struct file *file, loff_t offset, int whence)
+{
+	switch (whence) {
+	case SEEK_CUR:
+		break;
+	case SEEK_SET:
+		file->f_pos = offset;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return offset;
+}
+
+static const struct file_operations scom_fops = {
+	.owner	= THIS_MODULE,
+	.llseek	= scom_llseek,
+	.read	= scom_read,
+	.write	= scom_write,
+};
+
+static int scom_probe(struct device *dev)
+{
+	struct fsi_device *fsi_dev = to_fsi_dev(dev);
+	struct scom_device *scom;
+
+	scom = devm_kzalloc(dev, sizeof(*scom), GFP_KERNEL);
+	if (!scom)
+		return -ENOMEM;
+
+	scom->idx = ida_simple_get(&scom_ida, 1, INT_MAX, GFP_KERNEL);
+	snprintf(scom->name, sizeof(scom->name), "scom%d", scom->idx);
+	scom->fsi_dev = fsi_dev;
+	scom->mdev.minor = MISC_DYNAMIC_MINOR;
+	scom->mdev.fops = &scom_fops;
+	scom->mdev.name = scom->name;
+	scom->mdev.parent = dev;
+	list_add(&scom->link, &scom_devices);
+
+	return misc_register(&scom->mdev);
+}
+
+static int scom_remove(struct device *dev)
+{
+	struct scom_device *scom, *scom_tmp;
+	struct fsi_device *fsi_dev = to_fsi_dev(dev);
+
+	list_for_each_entry_safe(scom, scom_tmp, &scom_devices, link) {
+		if (scom->fsi_dev == fsi_dev) {
+			list_del(&scom->link);
+			ida_simple_remove(&scom_ida, scom->idx);
+			misc_deregister(&scom->mdev);
+		}
+	}
+
+	return 0;
+}
+
+static struct fsi_device_id scom_ids[] = {
+	{
+		.engine_type = FSI_ENGID_SCOM,
+		.version = FSI_VERSION_ANY,
+	},
+	{ 0 }
+};
+
+static struct fsi_driver scom_drv = {
+	.id_table = scom_ids,
+	.drv = {
+		.name = "scom",
+		.bus = &fsi_bus_type,
+		.probe = scom_probe,
+		.remove = scom_remove,
+	}
+};
+
+static int scom_init(void)
+{
+	INIT_LIST_HEAD(&scom_devices);
+	return fsi_driver_register(&scom_drv);
+}
+
+static void scom_exit(void)
+{
+	struct list_head *pos;
+	struct scom_device *scom;
+
+	list_for_each(pos, &scom_devices) {
+		scom = list_entry(pos, struct scom_device, link);
+		misc_deregister(&scom->mdev);
+		devm_kfree(&scom->fsi_dev->dev, scom);
+	}
+	fsi_driver_unregister(&scom_drv);
+}
+
+module_init(scom_init);
+module_exit(scom_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 23ca51ee6b28..395c85df48fd 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -242,6 +242,17 @@ config GPIO_ICH
 
 	  If unsure, say N.
 
+config GPIO_INGENIC
+	tristate "Ingenic JZ47xx SoCs GPIO support"
+	depends on OF
+	depends on MACH_INGENIC || COMPILE_TEST
+	select GPIOLIB_IRQCHIP
+	help
+	  Say yes here to support the GPIO functionality present on the
+	  JZ4740 and JZ4780 SoCs from Ingenic.
+
+	  If unsure, say N.
+
 config GPIO_IOP
 	tristate "Intel IOP GPIO"
 	depends on ARCH_IOP32X || ARCH_IOP33X || COMPILE_TEST
@@ -1227,23 +1238,6 @@ config GPIO_PISOSR
 
 endmenu
 
-menu "SPI or I2C GPIO expanders"
-	depends on (SPI_MASTER && !I2C) || I2C
-
-config GPIO_MCP23S08
-	tristate "Microchip MCP23xxx I/O expander"
-	depends on OF_GPIO
-	select GPIOLIB_IRQCHIP
-	select REGMAP_I2C if I2C
-	select REGMAP if SPI_MASTER
-	help
-	  SPI/I2C driver for Microchip MCP23S08/MCP23S17/MCP23008/MCP23017
-	  I/O expanders.
-	  This provides a GPIO interface supporting inputs and outputs.
-	  The I2C versions of the chips can be used as interrupt-controller.
-
-endmenu
-
 menu "USB GPIO expanders"
 	depends on USB
 
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 68b96277d9fa..845f990fc987 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -55,6 +55,7 @@ obj-$(CONFIG_GPIO_GPIO_MM)	+= gpio-gpio-mm.o
 obj-$(CONFIG_GPIO_GRGPIO)	+= gpio-grgpio.o
 obj-$(CONFIG_HTC_EGPIO)		+= gpio-htc-egpio.o
 obj-$(CONFIG_GPIO_ICH)		+= gpio-ich.o
+obj-$(CONFIG_GPIO_INGENIC)	+= gpio-ingenic.o
 obj-$(CONFIG_GPIO_IOP)		+= gpio-iop.o
 obj-$(CONFIG_GPIO_IT87)		+= gpio-it87.o
 obj-$(CONFIG_GPIO_JANZ_TTL)	+= gpio-janz-ttl.o
@@ -77,7 +78,6 @@ obj-$(CONFIG_GPIO_MENZ127)	+= gpio-menz127.o
 obj-$(CONFIG_GPIO_MERRIFIELD)	+= gpio-merrifield.o
 obj-$(CONFIG_GPIO_MC33880)	+= gpio-mc33880.o
 obj-$(CONFIG_GPIO_MC9S08DZ60)	+= gpio-mc9s08dz60.o
-obj-$(CONFIG_GPIO_MCP23S08)	+= gpio-mcp23s08.o
 obj-$(CONFIG_GPIO_ML_IOH)	+= gpio-ml-ioh.o
 obj-$(CONFIG_GPIO_MM_LANTIQ)	+= gpio-mm-lantiq.o
 obj-$(CONFIG_GPIO_MOCKUP)      += gpio-mockup.o
diff --git a/drivers/gpio/gpio-ingenic.c b/drivers/gpio/gpio-ingenic.c
new file mode 100644
index 000000000000..254780730b95
--- /dev/null
+++ b/drivers/gpio/gpio-ingenic.c
@@ -0,0 +1,394 @@
+/*
+ * Ingenic JZ47xx GPIO driver
+ *
+ * Copyright (c) 2017 Paul Cercueil <paul@crapouillou.net>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/gpio/driver.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/of_irq.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/regmap.h>
+
+#define GPIO_PIN	0x00
+#define GPIO_MSK	0x20
+
+#define JZ4740_GPIO_DATA	0x10
+#define JZ4740_GPIO_SELECT	0x50
+#define JZ4740_GPIO_DIR		0x60
+#define JZ4740_GPIO_TRIG	0x70
+#define JZ4740_GPIO_FLAG	0x80
+
+#define JZ4770_GPIO_INT		0x10
+#define JZ4770_GPIO_PAT1	0x30
+#define JZ4770_GPIO_PAT0	0x40
+#define JZ4770_GPIO_FLAG	0x50
+
+#define REG_SET(x) ((x) + 0x4)
+#define REG_CLEAR(x) ((x) + 0x8)
+
+enum jz_version {
+	ID_JZ4740,
+	ID_JZ4770,
+	ID_JZ4780,
+};
+
+struct ingenic_gpio_chip {
+	struct regmap *map;
+	struct gpio_chip gc;
+	struct irq_chip irq_chip;
+	unsigned int irq, reg_base;
+	enum jz_version version;
+};
+
+static u32 gpio_ingenic_read_reg(struct ingenic_gpio_chip *jzgc, u8 reg)
+{
+	unsigned int val;
+
+	regmap_read(jzgc->map, jzgc->reg_base + reg, &val);
+
+	return (u32) val;
+}
+
+static void gpio_ingenic_set_bit(struct ingenic_gpio_chip *jzgc,
+		u8 reg, u8 offset, bool set)
+{
+	if (set)
+		reg = REG_SET(reg);
+	else
+		reg = REG_CLEAR(reg);
+
+	regmap_write(jzgc->map, jzgc->reg_base + reg, BIT(offset));
+}
+
+static inline bool gpio_get_value(struct ingenic_gpio_chip *jzgc, u8 offset)
+{
+	unsigned int val = gpio_ingenic_read_reg(jzgc, GPIO_PIN);
+
+	return !!(val & BIT(offset));
+}
+
+static void gpio_set_value(struct ingenic_gpio_chip *jzgc, u8 offset, int value)
+{
+	if (jzgc->version >= ID_JZ4770)
+		gpio_ingenic_set_bit(jzgc, JZ4770_GPIO_PAT0, offset, !!value);
+	else
+		gpio_ingenic_set_bit(jzgc, JZ4740_GPIO_DATA, offset, !!value);
+}
+
+static void irq_set_type(struct ingenic_gpio_chip *jzgc,
+		u8 offset, unsigned int type)
+{
+	u8 reg1, reg2;
+
+	if (jzgc->version >= ID_JZ4770) {
+		reg1 = JZ4770_GPIO_PAT1;
+		reg2 = JZ4770_GPIO_PAT0;
+	} else {
+		reg1 = JZ4740_GPIO_TRIG;
+		reg2 = JZ4740_GPIO_DIR;
+	}
+
+	switch (type) {
+	case IRQ_TYPE_EDGE_RISING:
+		gpio_ingenic_set_bit(jzgc, reg2, offset, true);
+		gpio_ingenic_set_bit(jzgc, reg1, offset, true);
+		break;
+	case IRQ_TYPE_EDGE_FALLING:
+		gpio_ingenic_set_bit(jzgc, reg2, offset, false);
+		gpio_ingenic_set_bit(jzgc, reg1, offset, true);
+		break;
+	case IRQ_TYPE_LEVEL_HIGH:
+		gpio_ingenic_set_bit(jzgc, reg2, offset, true);
+		gpio_ingenic_set_bit(jzgc, reg1, offset, false);
+		break;
+	case IRQ_TYPE_LEVEL_LOW:
+	default:
+		gpio_ingenic_set_bit(jzgc, reg2, offset, false);
+		gpio_ingenic_set_bit(jzgc, reg1, offset, false);
+		break;
+	}
+}
+
+static void ingenic_gpio_irq_mask(struct irq_data *irqd)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(irqd);
+	struct ingenic_gpio_chip *jzgc = gpiochip_get_data(gc);
+
+	gpio_ingenic_set_bit(jzgc, GPIO_MSK, irqd->hwirq, true);
+}
+
+static void ingenic_gpio_irq_unmask(struct irq_data *irqd)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(irqd);
+	struct ingenic_gpio_chip *jzgc = gpiochip_get_data(gc);
+
+	gpio_ingenic_set_bit(jzgc, GPIO_MSK, irqd->hwirq, false);
+}
+
+static void ingenic_gpio_irq_enable(struct irq_data *irqd)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(irqd);
+	struct ingenic_gpio_chip *jzgc = gpiochip_get_data(gc);
+	int irq = irqd->hwirq;
+
+	if (jzgc->version >= ID_JZ4770)
+		gpio_ingenic_set_bit(jzgc, JZ4770_GPIO_INT, irq, true);
+	else
+		gpio_ingenic_set_bit(jzgc, JZ4740_GPIO_SELECT, irq, true);
+
+	ingenic_gpio_irq_unmask(irqd);
+}
+
+static void ingenic_gpio_irq_disable(struct irq_data *irqd)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(irqd);
+	struct ingenic_gpio_chip *jzgc = gpiochip_get_data(gc);
+	int irq = irqd->hwirq;
+
+	ingenic_gpio_irq_mask(irqd);
+
+	if (jzgc->version >= ID_JZ4770)
+		gpio_ingenic_set_bit(jzgc, JZ4770_GPIO_INT, irq, false);
+	else
+		gpio_ingenic_set_bit(jzgc, JZ4740_GPIO_SELECT, irq, false);
+}
+
+static void ingenic_gpio_irq_ack(struct irq_data *irqd)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(irqd);
+	struct ingenic_gpio_chip *jzgc = gpiochip_get_data(gc);
+	int irq = irqd->hwirq;
+	bool high;
+
+	if (irqd_get_trigger_type(irqd) == IRQ_TYPE_EDGE_BOTH) {
+		/*
+		 * Switch to an interrupt for the opposite edge to the one that
+		 * triggered the interrupt being ACKed.
+		 */
+		high = gpio_get_value(jzgc, irq);
+		if (high)
+			irq_set_type(jzgc, irq, IRQ_TYPE_EDGE_FALLING);
+		else
+			irq_set_type(jzgc, irq, IRQ_TYPE_EDGE_RISING);
+	}
+
+	if (jzgc->version >= ID_JZ4770)
+		gpio_ingenic_set_bit(jzgc, JZ4770_GPIO_FLAG, irq, false);
+	else
+		gpio_ingenic_set_bit(jzgc, JZ4740_GPIO_DATA, irq, true);
+}
+
+static int ingenic_gpio_irq_set_type(struct irq_data *irqd, unsigned int type)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(irqd);
+	struct ingenic_gpio_chip *jzgc = gpiochip_get_data(gc);
+
+	switch (type) {
+	case IRQ_TYPE_EDGE_BOTH:
+	case IRQ_TYPE_EDGE_RISING:
+	case IRQ_TYPE_EDGE_FALLING:
+		irq_set_handler_locked(irqd, handle_edge_irq);
+		break;
+	case IRQ_TYPE_LEVEL_HIGH:
+	case IRQ_TYPE_LEVEL_LOW:
+		irq_set_handler_locked(irqd, handle_level_irq);
+		break;
+	default:
+		irq_set_handler_locked(irqd, handle_bad_irq);
+	}
+
+	if (type == IRQ_TYPE_EDGE_BOTH) {
+		/*
+		 * The hardware does not support interrupts on both edges. The
+		 * best we can do is to set up a single-edge interrupt and then
+		 * switch to the opposing edge when ACKing the interrupt.
+		 */
+		bool high = gpio_get_value(jzgc, irqd->hwirq);
+
+		type = high ? IRQ_TYPE_EDGE_FALLING : IRQ_TYPE_EDGE_RISING;
+	}
+
+	irq_set_type(jzgc, irqd->hwirq, type);
+	return 0;
+}
+
+static int ingenic_gpio_irq_set_wake(struct irq_data *irqd, unsigned int on)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(irqd);
+	struct ingenic_gpio_chip *jzgc = gpiochip_get_data(gc);
+
+	return irq_set_irq_wake(jzgc->irq, on);
+}
+
+static void ingenic_gpio_irq_handler(struct irq_desc *desc)
+{
+	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
+	struct ingenic_gpio_chip *jzgc = gpiochip_get_data(gc);
+	struct irq_chip *irq_chip = irq_data_get_irq_chip(&desc->irq_data);
+	unsigned long flag, i;
+
+	chained_irq_enter(irq_chip, desc);
+
+	if (jzgc->version >= ID_JZ4770)
+		flag = gpio_ingenic_read_reg(jzgc, JZ4770_GPIO_FLAG);
+	else
+		flag = gpio_ingenic_read_reg(jzgc, JZ4740_GPIO_FLAG);
+
+	for_each_set_bit(i, &flag, 32)
+		generic_handle_irq(irq_linear_revmap(gc->irqdomain, i));
+	chained_irq_exit(irq_chip, desc);
+}
+
+static void ingenic_gpio_set(struct gpio_chip *gc,
+		unsigned int offset, int value)
+{
+	struct ingenic_gpio_chip *jzgc = gpiochip_get_data(gc);
+
+	gpio_set_value(jzgc, offset, value);
+}
+
+static int ingenic_gpio_get(struct gpio_chip *gc, unsigned int offset)
+{
+	struct ingenic_gpio_chip *jzgc = gpiochip_get_data(gc);
+
+	return (int) gpio_get_value(jzgc, offset);
+}
+
+static int ingenic_gpio_direction_input(struct gpio_chip *gc,
+		unsigned int offset)
+{
+	return pinctrl_gpio_direction_input(gc->base + offset);
+}
+
+static int ingenic_gpio_direction_output(struct gpio_chip *gc,
+		unsigned int offset, int value)
+{
+	ingenic_gpio_set(gc, offset, value);
+	return pinctrl_gpio_direction_output(gc->base + offset);
+}
+
+static const struct of_device_id ingenic_gpio_of_match[] = {
+	{ .compatible = "ingenic,jz4740-gpio", .data = (void *)ID_JZ4740 },
+	{ .compatible = "ingenic,jz4770-gpio", .data = (void *)ID_JZ4770 },
+	{ .compatible = "ingenic,jz4780-gpio", .data = (void *)ID_JZ4780 },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ingenic_gpio_of_match);
+
+static int ingenic_gpio_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	const struct of_device_id *of_id = of_match_device(
+			ingenic_gpio_of_match, dev);
+	struct ingenic_gpio_chip *jzgc;
+	u32 bank;
+	int err;
+
+	jzgc = devm_kzalloc(dev, sizeof(*jzgc), GFP_KERNEL);
+	if (!jzgc)
+		return -ENOMEM;
+
+	jzgc->map = dev_get_drvdata(dev->parent);
+	if (!jzgc->map) {
+		dev_err(dev, "Cannot get parent regmap\n");
+		return -ENXIO;
+	}
+
+	err = of_property_read_u32(dev->of_node, "reg", &bank);
+	if (err) {
+		dev_err(dev, "Cannot read \"reg\" property: %i\n", err);
+		return err;
+	}
+
+	jzgc->reg_base = bank * 0x100;
+
+	jzgc->gc.label = devm_kasprintf(dev, GFP_KERNEL, "GPIO%c", 'A' + bank);
+	if (!jzgc->gc.label)
+		return -ENOMEM;
+
+	/* DO NOT EXPAND THIS: FOR BACKWARD GPIO NUMBERSPACE COMPATIBIBILITY
+	 * ONLY: WORK TO TRANSITION CONSUMERS TO USE THE GPIO DESCRIPTOR API IN
+	 * <linux/gpio/consumer.h> INSTEAD.
+	 */
+	jzgc->gc.base = bank * 32;
+
+	jzgc->gc.ngpio = 32;
+	jzgc->gc.parent = dev;
+	jzgc->gc.of_node = dev->of_node;
+	jzgc->gc.owner = THIS_MODULE;
+	jzgc->version = (enum jz_version)of_id->data;
+
+	jzgc->gc.set = ingenic_gpio_set;
+	jzgc->gc.get = ingenic_gpio_get;
+	jzgc->gc.direction_input = ingenic_gpio_direction_input;
+	jzgc->gc.direction_output = ingenic_gpio_direction_output;
+
+	if (of_property_read_bool(dev->of_node, "gpio-ranges")) {
+		jzgc->gc.request = gpiochip_generic_request;
+		jzgc->gc.free = gpiochip_generic_free;
+	}
+
+	err = devm_gpiochip_add_data(dev, &jzgc->gc, jzgc);
+	if (err)
+		return err;
+
+	jzgc->irq = irq_of_parse_and_map(dev->of_node, 0);
+	if (!jzgc->irq)
+		return -EINVAL;
+
+	jzgc->irq_chip.name = jzgc->gc.label;
+	jzgc->irq_chip.irq_enable = ingenic_gpio_irq_enable;
+	jzgc->irq_chip.irq_disable = ingenic_gpio_irq_disable;
+	jzgc->irq_chip.irq_unmask = ingenic_gpio_irq_unmask;
+	jzgc->irq_chip.irq_mask = ingenic_gpio_irq_mask;
+	jzgc->irq_chip.irq_ack = ingenic_gpio_irq_ack;
+	jzgc->irq_chip.irq_set_type = ingenic_gpio_irq_set_type;
+	jzgc->irq_chip.irq_set_wake = ingenic_gpio_irq_set_wake;
+	jzgc->irq_chip.flags = IRQCHIP_MASK_ON_SUSPEND;
+
+	err = gpiochip_irqchip_add(&jzgc->gc, &jzgc->irq_chip, 0,
+			handle_level_irq, IRQ_TYPE_NONE);
+	if (err)
+		return err;
+
+	gpiochip_set_chained_irqchip(&jzgc->gc, &jzgc->irq_chip,
+			jzgc->irq, ingenic_gpio_irq_handler);
+	return 0;
+}
+
+static int ingenic_gpio_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver ingenic_gpio_driver = {
+	.driver = {
+		.name = "gpio-ingenic",
+		.of_match_table = of_match_ptr(ingenic_gpio_of_match),
+	},
+	.probe = ingenic_gpio_probe,
+	.remove = ingenic_gpio_remove,
+};
+
+static int __init ingenic_gpio_drv_register(void)
+{
+	return platform_driver_register(&ingenic_gpio_driver);
+}
+subsys_initcall(ingenic_gpio_drv_register);
+
+static void __exit ingenic_gpio_drv_unregister(void)
+{
+	platform_driver_unregister(&ingenic_gpio_driver);
+}
+module_exit(ingenic_gpio_drv_unregister);
+
+MODULE_AUTHOR("Paul Cercueil <paul@crapouillou.net>");
+MODULE_DESCRIPTION("Ingenic JZ47xx GPIO driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-mvebu.c b/drivers/gpio/gpio-mvebu.c
index 5104b6398139..c83ea68be792 100644
--- a/drivers/gpio/gpio-mvebu.c
+++ b/drivers/gpio/gpio-mvebu.c
@@ -721,7 +721,7 @@ static int mvebu_pwm_probe(struct platform_device *pdev,
 	u32 set;
 
 	if (!of_device_is_compatible(mvchip->chip.of_node,
-				     "marvell,armada-370-xp-gpio"))
+				     "marvell,armada-370-gpio"))
 		return 0;
 
 	if (IS_ERR(mvchip->clk))
@@ -852,7 +852,7 @@ static const struct of_device_id mvebu_gpio_of_match[] = {
 		.data	    = (void *) MVEBU_GPIO_SOC_VARIANT_ARMADAXP,
 	},
 	{
-		.compatible = "marvell,armada-370-xp-gpio",
+		.compatible = "marvell,armada-370-gpio",
 		.data	    = (void *) MVEBU_GPIO_SOC_VARIANT_ORION,
 	},
 	{
@@ -1128,7 +1128,7 @@ static int mvebu_gpio_probe(struct platform_device *pdev)
 						 mvchip);
 	}
 
-	/* Armada 370/XP has simple PWM support for GPIO lines */
+	/* Some MVEBU SoCs have simple PWM support for GPIO lines */
 	if (IS_ENABLED(CONFIG_PWM))
 		return mvebu_pwm_probe(pdev, mvchip, id);
 
diff --git a/drivers/gpio/gpiolib-acpi.c b/drivers/gpio/gpiolib-acpi.c
index 2185232da823..8fa5fcd00e9a 100644
--- a/drivers/gpio/gpiolib-acpi.c
+++ b/drivers/gpio/gpiolib-acpi.c
@@ -201,7 +201,7 @@ static acpi_status acpi_gpiochip_request_interrupt(struct acpi_resource *ares,
 			handler = acpi_gpio_irq_handler_evt;
 	}
 	if (!handler)
-		return AE_BAD_PARAMETER;
+		return AE_OK;
 
 	pin = acpi_gpiochip_pin_to_gpio_offset(chip->gpiodev, pin);
 	if (pin < 0)
diff --git a/drivers/gpio/gpiolib-sysfs.c b/drivers/gpio/gpiolib-sysfs.c
index 4b44dd97c07f..16fe9742597b 100644
--- a/drivers/gpio/gpiolib-sysfs.c
+++ b/drivers/gpio/gpiolib-sysfs.c
@@ -479,6 +479,7 @@ done:
 		pr_debug("%s: status %d\n", __func__, status);
 	return status ? : len;
 }
+static CLASS_ATTR_WO(export);
 
 static ssize_t unexport_store(struct class *class,
 				struct class_attribute *attr,
@@ -514,18 +515,20 @@ done:
 		pr_debug("%s: status %d\n", __func__, status);
 	return status ? : len;
 }
+static CLASS_ATTR_WO(unexport);
 
-static struct class_attribute gpio_class_attrs[] = {
-	__ATTR(export, 0200, NULL, export_store),
-	__ATTR(unexport, 0200, NULL, unexport_store),
-	__ATTR_NULL,
+static struct attribute *gpio_class_attrs[] = {
+	&class_attr_export.attr,
+	&class_attr_unexport.attr,
+	NULL,
 };
+ATTRIBUTE_GROUPS(gpio_class);
 
 static struct class gpio_class = {
 	.name =		"gpio",
 	.owner =	THIS_MODULE,
 
-	.class_attrs =	gpio_class_attrs,
+	.class_groups = gpio_class_groups,
 };
 
 
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index 5db44139cef8..a42a1eea5714 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -708,7 +708,8 @@ static irqreturn_t lineevent_irq_thread(int irq, void *p)
 
 	ge.timestamp = ktime_get_real_ns();
 
-	if (le->eflags & GPIOEVENT_REQUEST_BOTH_EDGES) {
+	if (le->eflags & GPIOEVENT_REQUEST_RISING_EDGE
+	    && le->eflags & GPIOEVENT_REQUEST_FALLING_EDGE) {
 		int level = gpiod_get_value_cansleep(le->desc);
 
 		if (level)
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_atombios.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_atombios.c
index 1cf78f4dd339..1e8e1123ddf4 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_atombios.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_atombios.c
@@ -693,6 +693,10 @@ int amdgpu_atombios_get_clock_info(struct amdgpu_device *adev)
 			DRM_INFO("Changing default dispclk from %dMhz to 600Mhz\n",
 				 adev->clock.default_dispclk / 100);
 			adev->clock.default_dispclk = 60000;
+		} else if (adev->clock.default_dispclk <= 60000) {
+			DRM_INFO("Changing default dispclk from %dMhz to 625Mhz\n",
+				 adev->clock.default_dispclk / 100);
+			adev->clock.default_dispclk = 62500;
 		}
 		adev->clock.dp_extclk =
 			le16_to_cpu(firmware_info->info_21.usUniphyDPModeExtClkFreq);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_drv.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_drv.c
index f2d705e6a75a..ab6b0d0febab 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_drv.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_drv.c
@@ -449,6 +449,7 @@ static const struct pci_device_id pciidlist[] = {
 	{0x1002, 0x6986, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_POLARIS12},
 	{0x1002, 0x6987, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_POLARIS12},
 	{0x1002, 0x6995, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_POLARIS12},
+	{0x1002, 0x6997, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_POLARIS12},
 	{0x1002, 0x699F, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_POLARIS12},
 	/* Vega 10 */
 	{0x1002, 0x6860, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_VEGA10|AMD_EXP_HW_SUPPORT},
diff --git a/drivers/gpu/drm/amd/amdgpu/atombios_crtc.c b/drivers/gpu/drm/amd/amdgpu/atombios_crtc.c
index 8c9bc75a9c2d..8a0818b23ea4 100644
--- a/drivers/gpu/drm/amd/amdgpu/atombios_crtc.c
+++ b/drivers/gpu/drm/amd/amdgpu/atombios_crtc.c
@@ -165,7 +165,7 @@ void amdgpu_atombios_crtc_powergate(struct drm_crtc *crtc, int state)
 	struct drm_device *dev = crtc->dev;
 	struct amdgpu_device *adev = dev->dev_private;
 	int index = GetIndexIntoMasterTable(COMMAND, EnableDispPowerGating);
-	ENABLE_DISP_POWER_GATING_PARAMETERS_V2_1 args;
+	ENABLE_DISP_POWER_GATING_PS_ALLOCATION args;
 
 	memset(&args, 0, sizeof(args));
 
@@ -178,7 +178,7 @@ void amdgpu_atombios_crtc_powergate(struct drm_crtc *crtc, int state)
 void amdgpu_atombios_crtc_powergate_init(struct amdgpu_device *adev)
 {
 	int index = GetIndexIntoMasterTable(COMMAND, EnableDispPowerGating);
-	ENABLE_DISP_POWER_GATING_PARAMETERS_V2_1 args;
+	ENABLE_DISP_POWER_GATING_PS_ALLOCATION args;
 
 	memset(&args, 0, sizeof(args));
 
diff --git a/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c b/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c
index 0cdeb6a2e4a0..5dffa27afa45 100644
--- a/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c
@@ -1207,8 +1207,11 @@ static void dce_v10_0_program_watermarks(struct amdgpu_device *adev,
 	u32 tmp, wm_mask, lb_vblank_lead_lines = 0;
 
 	if (amdgpu_crtc->base.enabled && num_heads && mode) {
-		active_time = 1000000UL * (u32)mode->crtc_hdisplay / (u32)mode->clock;
-		line_time = min((u32) (1000000UL * (u32)mode->crtc_htotal / (u32)mode->clock), (u32)65535);
+		active_time = (u32) div_u64((u64)mode->crtc_hdisplay * 1000000,
+					    (u32)mode->clock);
+		line_time = (u32) div_u64((u64)mode->crtc_htotal * 1000000,
+					  (u32)mode->clock);
+		line_time = min(line_time, (u32)65535);
 
 		/* watermark for high clocks */
 		if (adev->pm.dpm_enabled) {
diff --git a/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c b/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c
index 773654a19749..47bbc87f96d2 100644
--- a/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c
@@ -1176,8 +1176,11 @@ static void dce_v11_0_program_watermarks(struct amdgpu_device *adev,
 	u32 tmp, wm_mask, lb_vblank_lead_lines = 0;
 
 	if (amdgpu_crtc->base.enabled && num_heads && mode) {
-		active_time = 1000000UL * (u32)mode->crtc_hdisplay / (u32)mode->clock;
-		line_time = min((u32) (1000000UL * (u32)mode->crtc_htotal / (u32)mode->clock), (u32)65535);
+		active_time = (u32) div_u64((u64)mode->crtc_hdisplay * 1000000,
+					    (u32)mode->clock);
+		line_time = (u32) div_u64((u64)mode->crtc_htotal * 1000000,
+					  (u32)mode->clock);
+		line_time = min(line_time, (u32)65535);
 
 		/* watermark for high clocks */
 		if (adev->pm.dpm_enabled) {
diff --git a/drivers/gpu/drm/amd/amdgpu/dce_v6_0.c b/drivers/gpu/drm/amd/amdgpu/dce_v6_0.c
index 1f3552967ba3..d8c9a959493e 100644
--- a/drivers/gpu/drm/amd/amdgpu/dce_v6_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/dce_v6_0.c
@@ -983,8 +983,11 @@ static void dce_v6_0_program_watermarks(struct amdgpu_device *adev,
 	fixed20_12 a, b, c;
 
 	if (amdgpu_crtc->base.enabled && num_heads && mode) {
-		active_time = 1000000UL * (u32)mode->crtc_hdisplay / (u32)mode->clock;
-		line_time = min((u32) (1000000UL * (u32)mode->crtc_htotal / (u32)mode->clock), (u32)65535);
+		active_time = (u32) div_u64((u64)mode->crtc_hdisplay * 1000000,
+					    (u32)mode->clock);
+		line_time = (u32) div_u64((u64)mode->crtc_htotal * 1000000,
+					  (u32)mode->clock);
+		line_time = min(line_time, (u32)65535);
 		priority_a_cnt = 0;
 		priority_b_cnt = 0;
 
diff --git a/drivers/gpu/drm/amd/amdgpu/dce_v8_0.c b/drivers/gpu/drm/amd/amdgpu/dce_v8_0.c
index 3c558c170e5e..db30c6ba563a 100644
--- a/drivers/gpu/drm/amd/amdgpu/dce_v8_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/dce_v8_0.c
@@ -1091,8 +1091,11 @@ static void dce_v8_0_program_watermarks(struct amdgpu_device *adev,
 	u32 tmp, wm_mask, lb_vblank_lead_lines = 0;
 
 	if (amdgpu_crtc->base.enabled && num_heads && mode) {
-		active_time = 1000000UL * (u32)mode->crtc_hdisplay / (u32)mode->clock;
-		line_time = min((u32) (1000000UL * (u32)mode->crtc_htotal / (u32)mode->clock), (u32)65535);
+		active_time = (u32) div_u64((u64)mode->crtc_hdisplay * 1000000,
+					    (u32)mode->clock);
+		line_time = (u32) div_u64((u64)mode->crtc_htotal * 1000000,
+					  (u32)mode->clock);
+		line_time = min(line_time, (u32)65535);
 
 		/* watermark for high clocks */
 		if (adev->pm.dpm_enabled) {
diff --git a/drivers/gpu/drm/bridge/adv7511/adv7511_audio.c b/drivers/gpu/drm/bridge/adv7511/adv7511_audio.c
index cf92ebfe6ab7..67469c26bae8 100644
--- a/drivers/gpu/drm/bridge/adv7511/adv7511_audio.c
+++ b/drivers/gpu/drm/bridge/adv7511/adv7511_audio.c
@@ -11,6 +11,7 @@
 #include <sound/hdmi-codec.h>
 #include <sound/pcm.h>
 #include <sound/soc.h>
+#include <linux/of_graph.h>
 
 #include "adv7511.h"
 
@@ -182,10 +183,31 @@ static void audio_shutdown(struct device *dev, void *data)
 {
 }
 
+static int adv7511_hdmi_i2s_get_dai_id(struct snd_soc_component *component,
+					struct device_node *endpoint)
+{
+	struct of_endpoint of_ep;
+	int ret;
+
+	ret = of_graph_parse_endpoint(endpoint, &of_ep);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * HDMI sound should be located as reg = <2>
+	 * Then, it is sound port 0
+	 */
+	if (of_ep.port == 2)
+		return 0;
+
+	return -EINVAL;
+}
+
 static const struct hdmi_codec_ops adv7511_codec_ops = {
 	.hw_params	= adv7511_hdmi_hw_params,
 	.audio_shutdown = audio_shutdown,
 	.audio_startup	= audio_startup,
+	.get_dai_id	= adv7511_hdmi_i2s_get_dai_id,
 };
 
 static struct hdmi_codec_pdata codec_data = {
diff --git a/drivers/gpu/drm/bridge/synopsys/Kconfig b/drivers/gpu/drm/bridge/synopsys/Kconfig
index 40d2827a6d19..53e78d092d18 100644
--- a/drivers/gpu/drm/bridge/synopsys/Kconfig
+++ b/drivers/gpu/drm/bridge/synopsys/Kconfig
@@ -1,6 +1,7 @@
 config DRM_DW_HDMI
 	tristate
 	select DRM_KMS_HELPER
+	select REGMAP_MMIO
 
 config DRM_DW_HDMI_AHB_AUDIO
 	tristate "Synopsys Designware AHB Audio interface"
diff --git a/drivers/gpu/drm/bridge/synopsys/dw-hdmi-i2s-audio.c b/drivers/gpu/drm/bridge/synopsys/dw-hdmi-i2s-audio.c
index aaf287d2e91d..b2cf59f54c88 100644
--- a/drivers/gpu/drm/bridge/synopsys/dw-hdmi-i2s-audio.c
+++ b/drivers/gpu/drm/bridge/synopsys/dw-hdmi-i2s-audio.c
@@ -82,9 +82,30 @@ static void dw_hdmi_i2s_audio_shutdown(struct device *dev, void *data)
 	hdmi_write(audio, HDMI_AUD_CONF0_SW_RESET, HDMI_AUD_CONF0);
 }
 
+static int dw_hdmi_i2s_get_dai_id(struct snd_soc_component *component,
+				  struct device_node *endpoint)
+{
+	struct of_endpoint of_ep;
+	int ret;
+
+	ret = of_graph_parse_endpoint(endpoint, &of_ep);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * HDMI sound should be located as reg = <2>
+	 * Then, it is sound port 0
+	 */
+	if (of_ep.port == 2)
+		return 0;
+
+	return -EINVAL;
+}
+
 static struct hdmi_codec_ops dw_hdmi_i2s_ops = {
 	.hw_params	= dw_hdmi_i2s_hw_params,
 	.audio_shutdown	= dw_hdmi_i2s_audio_shutdown,
+	.get_dai_id	= dw_hdmi_i2s_get_dai_id,
 };
 
 static int snd_dw_hdmi_probe(struct platform_device *pdev)
diff --git a/drivers/gpu/drm/drm_connector.c b/drivers/gpu/drm/drm_connector.c
index 9f847615ac74..48ca2457df8c 100644
--- a/drivers/gpu/drm/drm_connector.c
+++ b/drivers/gpu/drm/drm_connector.c
@@ -1229,21 +1229,6 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 	if (!connector)
 		return -ENOENT;
 
-	drm_modeset_lock(&dev->mode_config.connection_mutex, NULL);
-	encoder = drm_connector_get_encoder(connector);
-	if (encoder)
-		out_resp->encoder_id = encoder->base.id;
-	else
-		out_resp->encoder_id = 0;
-
-	ret = drm_mode_object_get_properties(&connector->base, file_priv->atomic,
-			(uint32_t __user *)(unsigned long)(out_resp->props_ptr),
-			(uint64_t __user *)(unsigned long)(out_resp->prop_values_ptr),
-			&out_resp->count_props);
-	drm_modeset_unlock(&dev->mode_config.connection_mutex);
-	if (ret)
-		goto out_unref;
-
 	for (i = 0; i < DRM_CONNECTOR_MAX_ENCODER; i++)
 		if (connector->encoder_ids[i] != 0)
 			encoders_count++;
@@ -1256,7 +1241,7 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 				if (put_user(connector->encoder_ids[i],
 					     encoder_ptr + copied)) {
 					ret = -EFAULT;
-					goto out_unref;
+					goto out;
 				}
 				copied++;
 			}
@@ -1300,15 +1285,32 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 			if (copy_to_user(mode_ptr + copied,
 					 &u_mode, sizeof(u_mode))) {
 				ret = -EFAULT;
+				mutex_unlock(&dev->mode_config.mutex);
+
 				goto out;
 			}
 			copied++;
 		}
 	}
 	out_resp->count_modes = mode_count;
-out:
 	mutex_unlock(&dev->mode_config.mutex);
-out_unref:
+
+	drm_modeset_lock(&dev->mode_config.connection_mutex, NULL);
+	encoder = drm_connector_get_encoder(connector);
+	if (encoder)
+		out_resp->encoder_id = encoder->base.id;
+	else
+		out_resp->encoder_id = 0;
+
+	/* Only grab properties after probing, to make sure EDID and other
+	 * properties reflect the latest status. */
+	ret = drm_mode_object_get_properties(&connector->base, file_priv->atomic,
+			(uint32_t __user *)(unsigned long)(out_resp->props_ptr),
+			(uint64_t __user *)(unsigned long)(out_resp->prop_values_ptr),
+			&out_resp->count_props);
+	drm_modeset_unlock(&dev->mode_config.connection_mutex);
+
+out:
 	drm_connector_put(connector);
 
 	return ret;
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_gem.h b/drivers/gpu/drm/etnaviv/etnaviv_gem.h
index c4a091e87426..e437fba1209d 100644
--- a/drivers/gpu/drm/etnaviv/etnaviv_gem.h
+++ b/drivers/gpu/drm/etnaviv/etnaviv_gem.h
@@ -106,9 +106,10 @@ struct etnaviv_gem_submit {
 	struct etnaviv_gpu *gpu;
 	struct ww_acquire_ctx ticket;
 	struct dma_fence *fence;
+	u32 flags;
 	unsigned int nr_bos;
 	struct etnaviv_gem_submit_bo bos[0];
-	u32 flags;
+	/* No new members here, the previous one is variable-length! */
 };
 
 int etnaviv_gem_wait_bo(struct etnaviv_gpu *gpu, struct drm_gem_object *obj,
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_gem_submit.c b/drivers/gpu/drm/etnaviv/etnaviv_gem_submit.c
index de80ee1b71df..1013765274da 100644
--- a/drivers/gpu/drm/etnaviv/etnaviv_gem_submit.c
+++ b/drivers/gpu/drm/etnaviv/etnaviv_gem_submit.c
@@ -172,7 +172,7 @@ static int submit_fence_sync(const struct etnaviv_gem_submit *submit)
 	for (i = 0; i < submit->nr_bos; i++) {
 		struct etnaviv_gem_object *etnaviv_obj = submit->bos[i].obj;
 		bool write = submit->bos[i].flags & ETNA_SUBMIT_BO_WRITE;
-		bool explicit = !(submit->flags & ETNA_SUBMIT_NO_IMPLICIT);
+		bool explicit = !!(submit->flags & ETNA_SUBMIT_NO_IMPLICIT);
 
 		ret = etnaviv_gpu_fence_sync_obj(etnaviv_obj, context, write,
 						 explicit);
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index d689e511744e..4bd1467c17b1 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -292,6 +292,8 @@ static int per_file_stats(int id, void *ptr, void *data)
 	struct file_stats *stats = data;
 	struct i915_vma *vma;
 
+	lockdep_assert_held(&obj->base.dev->struct_mutex);
+
 	stats->count++;
 	stats->total += obj->base.size;
 	if (!obj->bind_count)
@@ -476,6 +478,8 @@ static int i915_gem_object_info(struct seq_file *m, void *data)
 		struct drm_i915_gem_request *request;
 		struct task_struct *task;
 
+		mutex_lock(&dev->struct_mutex);
+
 		memset(&stats, 0, sizeof(stats));
 		stats.file_priv = file->driver_priv;
 		spin_lock(&file->table_lock);
@@ -487,7 +491,6 @@ static int i915_gem_object_info(struct seq_file *m, void *data)
 		 * still alive (e.g. get_pid(current) => fork() => exit()).
 		 * Therefore, we need to protect this ->comm access using RCU.
 		 */
-		mutex_lock(&dev->struct_mutex);
 		request = list_first_entry_or_null(&file_priv->mm.request_list,
 						   struct drm_i915_gem_request,
 						   client_link);
@@ -497,6 +500,7 @@ static int i915_gem_object_info(struct seq_file *m, void *data)
 				PIDTYPE_PID);
 		print_file_stats(m, task ? task->comm : "<unknown>", stats);
 		rcu_read_unlock();
+
 		mutex_unlock(&dev->struct_mutex);
 	}
 	mutex_unlock(&dev->filelist_mutex);
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 462031cbd77f..615f0a855222 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -2285,8 +2285,8 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 	struct page *page;
 	unsigned long last_pfn = 0;	/* suppress gcc warning */
 	unsigned int max_segment;
+	gfp_t noreclaim;
 	int ret;
-	gfp_t gfp;
 
 	/* Assert that the object is not currently in any GPU domain. As it
 	 * wasn't in the GTT, there shouldn't be any way it could have been in
@@ -2315,22 +2315,31 @@ rebuild_st:
 	 * Fail silently without starting the shrinker
 	 */
 	mapping = obj->base.filp->f_mapping;
-	gfp = mapping_gfp_constraint(mapping, ~(__GFP_IO | __GFP_RECLAIM));
-	gfp |= __GFP_NORETRY | __GFP_NOWARN;
+	noreclaim = mapping_gfp_constraint(mapping,
+					   ~(__GFP_IO | __GFP_RECLAIM));
+	noreclaim |= __GFP_NORETRY | __GFP_NOWARN;
+
 	sg = st->sgl;
 	st->nents = 0;
 	for (i = 0; i < page_count; i++) {
-		page = shmem_read_mapping_page_gfp(mapping, i, gfp);
-		if (unlikely(IS_ERR(page))) {
-			i915_gem_shrink(dev_priv,
-					page_count,
-					I915_SHRINK_BOUND |
-					I915_SHRINK_UNBOUND |
-					I915_SHRINK_PURGEABLE);
+		const unsigned int shrink[] = {
+			I915_SHRINK_BOUND | I915_SHRINK_UNBOUND | I915_SHRINK_PURGEABLE,
+			0,
+		}, *s = shrink;
+		gfp_t gfp = noreclaim;
+
+		do {
 			page = shmem_read_mapping_page_gfp(mapping, i, gfp);
-		}
-		if (unlikely(IS_ERR(page))) {
-			gfp_t reclaim;
+			if (likely(!IS_ERR(page)))
+				break;
+
+			if (!*s) {
+				ret = PTR_ERR(page);
+				goto err_sg;
+			}
+
+			i915_gem_shrink(dev_priv, 2 * page_count, *s++);
+			cond_resched();
 
 			/* We've tried hard to allocate the memory by reaping
 			 * our own buffer, now let the real VM do its job and
@@ -2340,15 +2349,26 @@ rebuild_st:
 			 * defer the oom here by reporting the ENOMEM back
 			 * to userspace.
 			 */
-			reclaim = mapping_gfp_mask(mapping);
-			reclaim |= __GFP_NORETRY; /* reclaim, but no oom */
-
-			page = shmem_read_mapping_page_gfp(mapping, i, reclaim);
-			if (IS_ERR(page)) {
-				ret = PTR_ERR(page);
-				goto err_sg;
+			if (!*s) {
+				/* reclaim and warn, but no oom */
+				gfp = mapping_gfp_mask(mapping);
+
+				/* Our bo are always dirty and so we require
+				 * kswapd to reclaim our pages (direct reclaim
+				 * does not effectively begin pageout of our
+				 * buffers on its own). However, direct reclaim
+				 * only waits for kswapd when under allocation
+				 * congestion. So as a result __GFP_RECLAIM is
+				 * unreliable and fails to actually reclaim our
+				 * dirty pages -- unless you try over and over
+				 * again with !__GFP_NORETRY. However, we still
+				 * want to fail this allocation rather than
+				 * trigger the out-of-memory killer and for
+				 * this we want the future __GFP_MAYFAIL.
+				 */
 			}
-		}
+		} while (1);
+
 		if (!i ||
 		    sg->length >= max_segment ||
 		    page_to_pfn(page) != last_pfn + 1) {
@@ -4222,6 +4242,7 @@ i915_gem_object_create(struct drm_i915_private *dev_priv, u64 size)
 
 	mapping = obj->base.filp->f_mapping;
 	mapping_set_gfp_mask(mapping, mask);
+	GEM_BUG_ON(!(mapping_gfp_mask(mapping) & __GFP_RECLAIM));
 
 	i915_gem_object_init(obj, &i915_gem_object_ops);
 
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index a3e59c8ef27b..9ad13eeed904 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -546,11 +546,12 @@ repeat:
 }
 
 static int
-i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
+i915_gem_execbuffer_relocate_entry(struct i915_vma *vma,
 				   struct eb_vmas *eb,
 				   struct drm_i915_gem_relocation_entry *reloc,
 				   struct reloc_cache *cache)
 {
+	struct drm_i915_gem_object *obj = vma->obj;
 	struct drm_i915_private *dev_priv = to_i915(obj->base.dev);
 	struct drm_gem_object *target_obj;
 	struct drm_i915_gem_object *target_i915_obj;
@@ -628,6 +629,16 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 		return -EINVAL;
 	}
 
+	/*
+	 * If we write into the object, we need to force the synchronisation
+	 * barrier, either with an asynchronous clflush or if we executed the
+	 * patching using the GPU (though that should be serialised by the
+	 * timeline). To be completely sure, and since we are required to
+	 * do relocations we are already stalling, disable the user's opt
+	 * of our synchronisation.
+	 */
+	vma->exec_entry->flags &= ~EXEC_OBJECT_ASYNC;
+
 	ret = relocate_entry(obj, reloc, cache, target_offset);
 	if (ret)
 		return ret;
@@ -678,7 +689,7 @@ i915_gem_execbuffer_relocate_vma(struct i915_vma *vma,
 		do {
 			u64 offset = r->presumed_offset;
 
-			ret = i915_gem_execbuffer_relocate_entry(vma->obj, eb, r, &cache);
+			ret = i915_gem_execbuffer_relocate_entry(vma, eb, r, &cache);
 			if (ret)
 				goto out;
 
@@ -726,7 +737,7 @@ i915_gem_execbuffer_relocate_vma_slow(struct i915_vma *vma,
 
 	reloc_cache_init(&cache, eb->i915);
 	for (i = 0; i < entry->relocation_count; i++) {
-		ret = i915_gem_execbuffer_relocate_entry(vma->obj, eb, &relocs[i], &cache);
+		ret = i915_gem_execbuffer_relocate_entry(vma, eb, &relocs[i], &cache);
 		if (ret)
 			break;
 	}
diff --git a/drivers/gpu/drm/i915/i915_gem_request.c b/drivers/gpu/drm/i915/i915_gem_request.c
index 5ddbc9499775..a74d0ac737cb 100644
--- a/drivers/gpu/drm/i915/i915_gem_request.c
+++ b/drivers/gpu/drm/i915/i915_gem_request.c
@@ -623,7 +623,7 @@ i915_gem_request_alloc(struct intel_engine_cs *engine,
 	 * GPU processing the request, we never over-estimate the
 	 * position of the head.
 	 */
-	req->head = req->ring->tail;
+	req->head = req->ring->emit;
 
 	/* Check that we didn't interrupt ourselves with a new request */
 	GEM_BUG_ON(req->timeline->seqno != req->fence.seqno);
diff --git a/drivers/gpu/drm/i915/i915_gem_request.h b/drivers/gpu/drm/i915/i915_gem_request.h
index 129c58bb4805..a4a920c4c454 100644
--- a/drivers/gpu/drm/i915/i915_gem_request.h
+++ b/drivers/gpu/drm/i915/i915_gem_request.h
@@ -123,7 +123,7 @@ struct drm_i915_gem_request {
 	 * It is used by the driver to then queue the request for execution.
 	 */
 	struct i915_sw_fence submit;
-	wait_queue_t submitq;
+	wait_queue_entry_t submitq;
 	wait_queue_head_t execute;
 
 	/* A list of everyone we wait upon, and everyone who waits upon us.
diff --git a/drivers/gpu/drm/i915/i915_guc_submission.c b/drivers/gpu/drm/i915/i915_guc_submission.c
index 1642fff9cf13..ab5140ba108d 100644
--- a/drivers/gpu/drm/i915/i915_guc_submission.c
+++ b/drivers/gpu/drm/i915/i915_guc_submission.c
@@ -480,9 +480,7 @@ static void guc_wq_item_append(struct i915_guc_client *client,
 	GEM_BUG_ON(freespace < wqi_size);
 
 	/* The GuC firmware wants the tail index in QWords, not bytes */
-	tail = rq->tail;
-	assert_ring_tail_valid(rq->ring, rq->tail);
-	tail >>= 3;
+	tail = intel_ring_set_tail(rq->ring, rq->tail) >> 3;
 	GEM_BUG_ON(tail > WQ_RING_TAIL_MAX);
 
 	/* For now workqueue item is 4 DWs; workqueue buffer is 2 pages. So we
diff --git a/drivers/gpu/drm/i915/i915_pvinfo.h b/drivers/gpu/drm/i915/i915_pvinfo.h
index c0cb2974caac..2cfe96d3e5d1 100644
--- a/drivers/gpu/drm/i915/i915_pvinfo.h
+++ b/drivers/gpu/drm/i915/i915_pvinfo.h
@@ -36,10 +36,6 @@
 #define VGT_VERSION_MAJOR 1
 #define VGT_VERSION_MINOR 0
 
-#define INTEL_VGT_IF_VERSION_ENCODE(major, minor) ((major) << 16 | (minor))
-#define INTEL_VGT_IF_VERSION \
-	INTEL_VGT_IF_VERSION_ENCODE(VGT_VERSION_MAJOR, VGT_VERSION_MINOR)
-
 /*
  * notifications from guest to vgpu device model
  */
@@ -55,8 +51,8 @@ enum vgt_g2v_type {
 
 struct vgt_if {
 	u64 magic;		/* VGT_MAGIC */
-	uint16_t version_major;
-	uint16_t version_minor;
+	u16 version_major;
+	u16 version_minor;
 	u32 vgt_id;		/* ID of vGT instance */
 	u32 rsv1[12];		/* pad to offset 0x40 */
 	/*
diff --git a/drivers/gpu/drm/i915/i915_sw_fence.c b/drivers/gpu/drm/i915/i915_sw_fence.c
index a277f8eb7beb..380de4360b8a 100644
--- a/drivers/gpu/drm/i915/i915_sw_fence.c
+++ b/drivers/gpu/drm/i915/i915_sw_fence.c
@@ -152,7 +152,7 @@ static void __i915_sw_fence_wake_up_all(struct i915_sw_fence *fence,
 					struct list_head *continuation)
 {
 	wait_queue_head_t *x = &fence->wait;
-	wait_queue_t *pos, *next;
+	wait_queue_entry_t *pos, *next;
 	unsigned long flags;
 
 	debug_fence_deactivate(fence);
@@ -160,31 +160,30 @@ static void __i915_sw_fence_wake_up_all(struct i915_sw_fence *fence,
 
 	/*
 	 * To prevent unbounded recursion as we traverse the graph of
-	 * i915_sw_fences, we move the task_list from this, the next ready
-	 * fence, to the tail of the original fence's task_list
+	 * i915_sw_fences, we move the entry list from this, the next ready
+	 * fence, to the tail of the original fence's entry list
 	 * (and so added to the list to be woken).
 	 */
 
 	spin_lock_irqsave_nested(&x->lock, flags, 1 + !!continuation);
 	if (continuation) {
-		list_for_each_entry_safe(pos, next, &x->task_list, task_list) {
+		list_for_each_entry_safe(pos, next, &x->head, entry) {
 			if (pos->func == autoremove_wake_function)
 				pos->func(pos, TASK_NORMAL, 0, continuation);
 			else
-				list_move_tail(&pos->task_list, continuation);
+				list_move_tail(&pos->entry, continuation);
 		}
 	} else {
 		LIST_HEAD(extra);
 
 		do {
-			list_for_each_entry_safe(pos, next,
-						 &x->task_list, task_list)
+			list_for_each_entry_safe(pos, next, &x->head, entry)
 				pos->func(pos, TASK_NORMAL, 0, &extra);
 
 			if (list_empty(&extra))
 				break;
 
-			list_splice_tail_init(&extra, &x->task_list);
+			list_splice_tail_init(&extra, &x->head);
 		} while (1);
 	}
 	spin_unlock_irqrestore(&x->lock, flags);
@@ -254,9 +253,9 @@ void i915_sw_fence_commit(struct i915_sw_fence *fence)
 	__i915_sw_fence_commit(fence);
 }
 
-static int i915_sw_fence_wake(wait_queue_t *wq, unsigned mode, int flags, void *key)
+static int i915_sw_fence_wake(wait_queue_entry_t *wq, unsigned mode, int flags, void *key)
 {
-	list_del(&wq->task_list);
+	list_del(&wq->entry);
 	__i915_sw_fence_complete(wq->private, key);
 	i915_sw_fence_put(wq->private);
 	if (wq->flags & I915_SW_FENCE_FLAG_ALLOC)
@@ -267,7 +266,7 @@ static int i915_sw_fence_wake(wait_queue_t *wq, unsigned mode, int flags, void *
 static bool __i915_sw_fence_check_if_after(struct i915_sw_fence *fence,
 				    const struct i915_sw_fence * const signaler)
 {
-	wait_queue_t *wq;
+	wait_queue_entry_t *wq;
 
 	if (__test_and_set_bit(I915_SW_FENCE_CHECKED_BIT, &fence->flags))
 		return false;
@@ -275,7 +274,7 @@ static bool __i915_sw_fence_check_if_after(struct i915_sw_fence *fence,
 	if (fence == signaler)
 		return true;
 
-	list_for_each_entry(wq, &fence->wait.task_list, task_list) {
+	list_for_each_entry(wq, &fence->wait.head, entry) {
 		if (wq->func != i915_sw_fence_wake)
 			continue;
 
@@ -288,12 +287,12 @@ static bool __i915_sw_fence_check_if_after(struct i915_sw_fence *fence,
 
 static void __i915_sw_fence_clear_checked_bit(struct i915_sw_fence *fence)
 {
-	wait_queue_t *wq;
+	wait_queue_entry_t *wq;
 
 	if (!__test_and_clear_bit(I915_SW_FENCE_CHECKED_BIT, &fence->flags))
 		return;
 
-	list_for_each_entry(wq, &fence->wait.task_list, task_list) {
+	list_for_each_entry(wq, &fence->wait.head, entry) {
 		if (wq->func != i915_sw_fence_wake)
 			continue;
 
@@ -320,7 +319,7 @@ static bool i915_sw_fence_check_if_after(struct i915_sw_fence *fence,
 
 static int __i915_sw_fence_await_sw_fence(struct i915_sw_fence *fence,
 					  struct i915_sw_fence *signaler,
-					  wait_queue_t *wq, gfp_t gfp)
+					  wait_queue_entry_t *wq, gfp_t gfp)
 {
 	unsigned long flags;
 	int pending;
@@ -350,7 +349,7 @@ static int __i915_sw_fence_await_sw_fence(struct i915_sw_fence *fence,
 		pending |= I915_SW_FENCE_FLAG_ALLOC;
 	}
 
-	INIT_LIST_HEAD(&wq->task_list);
+	INIT_LIST_HEAD(&wq->entry);
 	wq->flags = pending;
 	wq->func = i915_sw_fence_wake;
 	wq->private = i915_sw_fence_get(fence);
@@ -359,7 +358,7 @@ static int __i915_sw_fence_await_sw_fence(struct i915_sw_fence *fence,
 
 	spin_lock_irqsave(&signaler->wait.lock, flags);
 	if (likely(!i915_sw_fence_done(signaler))) {
-		__add_wait_queue_tail(&signaler->wait, wq);
+		__add_wait_queue_entry_tail(&signaler->wait, wq);
 		pending = 1;
 	} else {
 		i915_sw_fence_wake(wq, 0, 0, NULL);
@@ -372,7 +371,7 @@ static int __i915_sw_fence_await_sw_fence(struct i915_sw_fence *fence,
 
 int i915_sw_fence_await_sw_fence(struct i915_sw_fence *fence,
 				 struct i915_sw_fence *signaler,
-				 wait_queue_t *wq)
+				 wait_queue_entry_t *wq)
 {
 	return __i915_sw_fence_await_sw_fence(fence, signaler, wq, 0);
 }
diff --git a/drivers/gpu/drm/i915/i915_sw_fence.h b/drivers/gpu/drm/i915/i915_sw_fence.h
index d31cefbbcc04..fd3c3bf6c8b7 100644
--- a/drivers/gpu/drm/i915/i915_sw_fence.h
+++ b/drivers/gpu/drm/i915/i915_sw_fence.h
@@ -66,7 +66,7 @@ void i915_sw_fence_commit(struct i915_sw_fence *fence);
 
 int i915_sw_fence_await_sw_fence(struct i915_sw_fence *fence,
 				 struct i915_sw_fence *after,
-				 wait_queue_t *wq);
+				 wait_queue_entry_t *wq);
 int i915_sw_fence_await_sw_fence_gfp(struct i915_sw_fence *fence,
 				     struct i915_sw_fence *after,
 				     gfp_t gfp);
diff --git a/drivers/gpu/drm/i915/i915_vgpu.c b/drivers/gpu/drm/i915/i915_vgpu.c
index 4ab8a973b61f..2e739018fb4c 100644
--- a/drivers/gpu/drm/i915/i915_vgpu.c
+++ b/drivers/gpu/drm/i915/i915_vgpu.c
@@ -60,8 +60,8 @@
  */
 void i915_check_vgpu(struct drm_i915_private *dev_priv)
 {
-	uint64_t magic;
-	uint32_t version;
+	u64 magic;
+	u16 version_major;
 
 	BUILD_BUG_ON(sizeof(struct vgt_if) != VGT_PVINFO_SIZE);
 
@@ -69,10 +69,8 @@ void i915_check_vgpu(struct drm_i915_private *dev_priv)
 	if (magic != VGT_MAGIC)
 		return;
 
-	version = INTEL_VGT_IF_VERSION_ENCODE(
-		__raw_i915_read16(dev_priv, vgtif_reg(version_major)),
-		__raw_i915_read16(dev_priv, vgtif_reg(version_minor)));
-	if (version != INTEL_VGT_IF_VERSION) {
+	version_major = __raw_i915_read16(dev_priv, vgtif_reg(version_major));
+	if (version_major < VGT_VERSION_MAJOR) {
 		DRM_INFO("VGT interface version mismatch!\n");
 		return;
 	}
diff --git a/drivers/gpu/drm/i915/i915_vma.c b/drivers/gpu/drm/i915/i915_vma.c
index 1aba47024656..f066e2d785f5 100644
--- a/drivers/gpu/drm/i915/i915_vma.c
+++ b/drivers/gpu/drm/i915/i915_vma.c
@@ -650,6 +650,11 @@ int i915_vma_unbind(struct i915_vma *vma)
 				break;
 		}
 
+		if (!ret) {
+			ret = i915_gem_active_retire(&vma->last_fence,
+						     &vma->vm->i915->drm.struct_mutex);
+		}
+
 		__i915_vma_unpin(vma);
 		if (ret)
 			return ret;
diff --git a/drivers/gpu/drm/i915/intel_acpi.c b/drivers/gpu/drm/i915/intel_acpi.c
index eb638a1e69d2..42fb436f6cdc 100644
--- a/drivers/gpu/drm/i915/intel_acpi.c
+++ b/drivers/gpu/drm/i915/intel_acpi.c
@@ -15,13 +15,9 @@ static struct intel_dsm_priv {
 	acpi_handle dhandle;
 } intel_dsm_priv;
 
-static const u8 intel_dsm_guid[] = {
-	0xd3, 0x73, 0xd8, 0x7e,
-	0xd0, 0xc2,
-	0x4f, 0x4e,
-	0xa8, 0x54,
-	0x0f, 0x13, 0x17, 0xb0, 0x1c, 0x2c
-};
+static const guid_t intel_dsm_guid =
+	GUID_INIT(0x7ed873d3, 0xc2d0, 0x4e4f,
+		  0xa8, 0x54, 0x0f, 0x13, 0x17, 0xb0, 0x1c, 0x2c);
 
 static char *intel_dsm_port_name(u8 id)
 {
@@ -80,7 +76,7 @@ static void intel_dsm_platform_mux_info(void)
 	int i;
 	union acpi_object *pkg, *connector_count;
 
-	pkg = acpi_evaluate_dsm_typed(intel_dsm_priv.dhandle, intel_dsm_guid,
+	pkg = acpi_evaluate_dsm_typed(intel_dsm_priv.dhandle, &intel_dsm_guid,
 			INTEL_DSM_REVISION_ID, INTEL_DSM_FN_PLATFORM_MUX_INFO,
 			NULL, ACPI_TYPE_PACKAGE);
 	if (!pkg) {
@@ -118,7 +114,7 @@ static bool intel_dsm_pci_probe(struct pci_dev *pdev)
 	if (!dhandle)
 		return false;
 
-	if (!acpi_check_dsm(dhandle, intel_dsm_guid, INTEL_DSM_REVISION_ID,
+	if (!acpi_check_dsm(dhandle, &intel_dsm_guid, INTEL_DSM_REVISION_ID,
 			    1 << INTEL_DSM_FN_PLATFORM_MUX_INFO)) {
 		DRM_DEBUG_KMS("no _DSM method for intel device\n");
 		return false;
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 569717a12723..9106ea32b048 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -120,7 +120,8 @@ static void intel_crtc_init_scalers(struct intel_crtc *crtc,
 static void skylake_pfit_enable(struct intel_crtc *crtc);
 static void ironlake_pfit_disable(struct intel_crtc *crtc, bool force);
 static void ironlake_pfit_enable(struct intel_crtc *crtc);
-static void intel_modeset_setup_hw_state(struct drm_device *dev);
+static void intel_modeset_setup_hw_state(struct drm_device *dev,
+					 struct drm_modeset_acquire_ctx *ctx);
 static void intel_pre_disable_primary_noatomic(struct drm_crtc *crtc);
 
 struct intel_limit {
@@ -3449,7 +3450,7 @@ __intel_display_resume(struct drm_device *dev,
 	struct drm_crtc *crtc;
 	int i, ret;
 
-	intel_modeset_setup_hw_state(dev);
+	intel_modeset_setup_hw_state(dev, ctx);
 	i915_redisable_vga(to_i915(dev));
 
 	if (!state)
@@ -4598,7 +4599,7 @@ static void cpt_verify_modeset(struct drm_device *dev, int pipe)
 
 static int
 skl_update_scaler(struct intel_crtc_state *crtc_state, bool force_detach,
-		  unsigned scaler_user, int *scaler_id, unsigned int rotation,
+		  unsigned int scaler_user, int *scaler_id,
 		  int src_w, int src_h, int dst_w, int dst_h)
 {
 	struct intel_crtc_scaler_state *scaler_state =
@@ -4607,9 +4608,12 @@ skl_update_scaler(struct intel_crtc_state *crtc_state, bool force_detach,
 		to_intel_crtc(crtc_state->base.crtc);
 	int need_scaling;
 
-	need_scaling = drm_rotation_90_or_270(rotation) ?
-		(src_h != dst_w || src_w != dst_h):
-		(src_w != dst_w || src_h != dst_h);
+	/*
+	 * Src coordinates are already rotated by 270 degrees for
+	 * the 90/270 degree plane rotation cases (to match the
+	 * GTT mapping), hence no need to account for rotation here.
+	 */
+	need_scaling = src_w != dst_w || src_h != dst_h;
 
 	/*
 	 * if plane is being disabled or scaler is no more required or force detach
@@ -4671,7 +4675,7 @@ int skl_update_scaler_crtc(struct intel_crtc_state *state)
 	const struct drm_display_mode *adjusted_mode = &state->base.adjusted_mode;
 
 	return skl_update_scaler(state, !state->base.active, SKL_CRTC_INDEX,
-		&state->scaler_state.scaler_id, DRM_ROTATE_0,
+		&state->scaler_state.scaler_id,
 		state->pipe_src_w, state->pipe_src_h,
 		adjusted_mode->crtc_hdisplay, adjusted_mode->crtc_vdisplay);
 }
@@ -4700,7 +4704,6 @@ static int skl_update_scaler_plane(struct intel_crtc_state *crtc_state,
 	ret = skl_update_scaler(crtc_state, force_detach,
 				drm_plane_index(&intel_plane->base),
 				&plane_state->scaler_id,
-				plane_state->base.rotation,
 				drm_rect_width(&plane_state->base.src) >> 16,
 				drm_rect_height(&plane_state->base.src) >> 16,
 				drm_rect_width(&plane_state->base.dst),
@@ -5823,7 +5826,8 @@ static void i9xx_crtc_disable(struct intel_crtc_state *old_crtc_state,
 		intel_update_watermarks(intel_crtc);
 }
 
-static void intel_crtc_disable_noatomic(struct drm_crtc *crtc)
+static void intel_crtc_disable_noatomic(struct drm_crtc *crtc,
+					struct drm_modeset_acquire_ctx *ctx)
 {
 	struct intel_encoder *encoder;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
@@ -5853,7 +5857,7 @@ static void intel_crtc_disable_noatomic(struct drm_crtc *crtc)
 		return;
 	}
 
-	state->acquire_ctx = crtc->dev->mode_config.acquire_ctx;
+	state->acquire_ctx = ctx;
 
 	/* Everything's already locked, -EDEADLK can't happen. */
 	crtc_state = intel_atomic_get_crtc_state(state, intel_crtc);
@@ -15028,7 +15032,7 @@ int intel_modeset_init(struct drm_device *dev)
 	intel_setup_outputs(dev_priv);
 
 	drm_modeset_lock_all(dev);
-	intel_modeset_setup_hw_state(dev);
+	intel_modeset_setup_hw_state(dev, dev->mode_config.acquire_ctx);
 	drm_modeset_unlock_all(dev);
 
 	for_each_intel_crtc(dev, crtc) {
@@ -15065,13 +15069,13 @@ int intel_modeset_init(struct drm_device *dev)
 	return 0;
 }
 
-static void intel_enable_pipe_a(struct drm_device *dev)
+static void intel_enable_pipe_a(struct drm_device *dev,
+				struct drm_modeset_acquire_ctx *ctx)
 {
 	struct intel_connector *connector;
 	struct drm_connector_list_iter conn_iter;
 	struct drm_connector *crt = NULL;
 	struct intel_load_detect_pipe load_detect_temp;
-	struct drm_modeset_acquire_ctx *ctx = dev->mode_config.acquire_ctx;
 	int ret;
 
 	/* We can't just switch on the pipe A, we need to set things up with a
@@ -15143,7 +15147,8 @@ static bool has_pch_trancoder(struct drm_i915_private *dev_priv,
 		(HAS_PCH_LPT_H(dev_priv) && pch_transcoder == TRANSCODER_A);
 }
 
-static void intel_sanitize_crtc(struct intel_crtc *crtc)
+static void intel_sanitize_crtc(struct intel_crtc *crtc,
+				struct drm_modeset_acquire_ctx *ctx)
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
@@ -15189,7 +15194,7 @@ static void intel_sanitize_crtc(struct intel_crtc *crtc)
 		plane = crtc->plane;
 		crtc->base.primary->state->visible = true;
 		crtc->plane = !plane;
-		intel_crtc_disable_noatomic(&crtc->base);
+		intel_crtc_disable_noatomic(&crtc->base, ctx);
 		crtc->plane = plane;
 	}
 
@@ -15199,13 +15204,13 @@ static void intel_sanitize_crtc(struct intel_crtc *crtc)
 		 * resume. Force-enable the pipe to fix this, the update_dpms
 		 * call below we restore the pipe to the right state, but leave
 		 * the required bits on. */
-		intel_enable_pipe_a(dev);
+		intel_enable_pipe_a(dev, ctx);
 	}
 
 	/* Adjust the state of the output pipe according to whether we
 	 * have active connectors/encoders. */
 	if (crtc->active && !intel_crtc_has_encoders(crtc))
-		intel_crtc_disable_noatomic(&crtc->base);
+		intel_crtc_disable_noatomic(&crtc->base, ctx);
 
 	if (crtc->active || HAS_GMCH_DISPLAY(dev_priv)) {
 		/*
@@ -15503,7 +15508,8 @@ get_encoder_power_domains(struct drm_i915_private *dev_priv)
  * and sanitizes it to the current state
  */
 static void
-intel_modeset_setup_hw_state(struct drm_device *dev)
+intel_modeset_setup_hw_state(struct drm_device *dev,
+			     struct drm_modeset_acquire_ctx *ctx)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	enum pipe pipe;
@@ -15523,7 +15529,7 @@ intel_modeset_setup_hw_state(struct drm_device *dev)
 	for_each_pipe(dev_priv, pipe) {
 		crtc = intel_get_crtc_for_pipe(dev_priv, pipe);
 
-		intel_sanitize_crtc(crtc);
+		intel_sanitize_crtc(crtc, ctx);
 		intel_dump_pipe_config(crtc, crtc->config,
 				       "[setup_hw_state]");
 	}
diff --git a/drivers/gpu/drm/i915/intel_dp_aux_backlight.c b/drivers/gpu/drm/i915/intel_dp_aux_backlight.c
index 6532e226db29..40ba3134545e 100644
--- a/drivers/gpu/drm/i915/intel_dp_aux_backlight.c
+++ b/drivers/gpu/drm/i915/intel_dp_aux_backlight.c
@@ -119,8 +119,6 @@ static int intel_dp_aux_setup_backlight(struct intel_connector *connector,
 	struct intel_dp *intel_dp = enc_to_intel_dp(&connector->encoder->base);
 	struct intel_panel *panel = &connector->panel;
 
-	intel_dp_aux_enable_backlight(connector);
-
 	if (intel_dp->edp_dpcd[2] & DP_EDP_BACKLIGHT_BRIGHTNESS_BYTE_COUNT)
 		panel->backlight.max = 0xFFFF;
 	else
diff --git a/drivers/gpu/drm/i915/intel_lrc.c b/drivers/gpu/drm/i915/intel_lrc.c
index dac4e003c1f3..62f44d3e7c43 100644
--- a/drivers/gpu/drm/i915/intel_lrc.c
+++ b/drivers/gpu/drm/i915/intel_lrc.c
@@ -326,8 +326,7 @@ static u64 execlists_update_context(struct drm_i915_gem_request *rq)
 		rq->ctx->ppgtt ?: rq->i915->mm.aliasing_ppgtt;
 	u32 *reg_state = ce->lrc_reg_state;
 
-	assert_ring_tail_valid(rq->ring, rq->tail);
-	reg_state[CTX_RING_TAIL+1] = rq->tail;
+	reg_state[CTX_RING_TAIL+1] = intel_ring_set_tail(rq->ring, rq->tail);
 
 	/* True 32b PPGTT with dynamic page allocation: update PDP
 	 * registers and point the unallocated PDPs to scratch page.
@@ -2036,8 +2035,7 @@ void intel_lr_context_resume(struct drm_i915_private *dev_priv)
 			ce->state->obj->mm.dirty = true;
 			i915_gem_object_unpin_map(ce->state->obj);
 
-			ce->ring->head = ce->ring->tail = 0;
-			intel_ring_update_space(ce->ring);
+			intel_ring_reset(ce->ring, 0);
 		}
 	}
 }
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
index 2ca481b5aa69..078fd1bfa5ea 100644
--- a/drivers/gpu/drm/i915/intel_pm.c
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -3373,20 +3373,26 @@ skl_plane_downscale_amount(const struct intel_crtc_state *cstate,
 
 	/* n.b., src is 16.16 fixed point, dst is whole integer */
 	if (plane->id == PLANE_CURSOR) {
+		/*
+		 * Cursors only support 0/180 degree rotation,
+		 * hence no need to account for rotation here.
+		 */
 		src_w = pstate->base.src_w;
 		src_h = pstate->base.src_h;
 		dst_w = pstate->base.crtc_w;
 		dst_h = pstate->base.crtc_h;
 	} else {
+		/*
+		 * Src coordinates are already rotated by 270 degrees for
+		 * the 90/270 degree plane rotation cases (to match the
+		 * GTT mapping), hence no need to account for rotation here.
+		 */
 		src_w = drm_rect_width(&pstate->base.src);
 		src_h = drm_rect_height(&pstate->base.src);
 		dst_w = drm_rect_width(&pstate->base.dst);
 		dst_h = drm_rect_height(&pstate->base.dst);
 	}
 
-	if (drm_rotation_90_or_270(pstate->base.rotation))
-		swap(dst_w, dst_h);
-
 	downscale_h = max(src_h / dst_h, (uint32_t)DRM_PLANE_HELPER_NO_SCALING);
 	downscale_w = max(src_w / dst_w, (uint32_t)DRM_PLANE_HELPER_NO_SCALING);
 
@@ -3417,12 +3423,14 @@ skl_plane_relative_data_rate(const struct intel_crtc_state *cstate,
 	if (y && format != DRM_FORMAT_NV12)
 		return 0;
 
+	/*
+	 * Src coordinates are already rotated by 270 degrees for
+	 * the 90/270 degree plane rotation cases (to match the
+	 * GTT mapping), hence no need to account for rotation here.
+	 */
 	width = drm_rect_width(&intel_pstate->base.src) >> 16;
 	height = drm_rect_height(&intel_pstate->base.src) >> 16;
 
-	if (drm_rotation_90_or_270(pstate->rotation))
-		swap(width, height);
-
 	/* for planar format */
 	if (format == DRM_FORMAT_NV12) {
 		if (y)  /* y-plane data rate */
@@ -3505,12 +3513,14 @@ skl_ddb_min_alloc(const struct drm_plane_state *pstate,
 	    fb->modifier != I915_FORMAT_MOD_Yf_TILED)
 		return 8;
 
+	/*
+	 * Src coordinates are already rotated by 270 degrees for
+	 * the 90/270 degree plane rotation cases (to match the
+	 * GTT mapping), hence no need to account for rotation here.
+	 */
 	src_w = drm_rect_width(&intel_pstate->base.src) >> 16;
 	src_h = drm_rect_height(&intel_pstate->base.src) >> 16;
 
-	if (drm_rotation_90_or_270(pstate->rotation))
-		swap(src_w, src_h);
-
 	/* Halve UV plane width and height for NV12 */
 	if (fb->format->format == DRM_FORMAT_NV12 && !y) {
 		src_w /= 2;
@@ -3794,13 +3804,15 @@ static int skl_compute_plane_wm(const struct drm_i915_private *dev_priv,
 		width = intel_pstate->base.crtc_w;
 		height = intel_pstate->base.crtc_h;
 	} else {
+		/*
+		 * Src coordinates are already rotated by 270 degrees for
+		 * the 90/270 degree plane rotation cases (to match the
+		 * GTT mapping), hence no need to account for rotation here.
+		 */
 		width = drm_rect_width(&intel_pstate->base.src) >> 16;
 		height = drm_rect_height(&intel_pstate->base.src) >> 16;
 	}
 
-	if (drm_rotation_90_or_270(pstate->rotation))
-		swap(width, height);
-
 	cpp = fb->format->cpp[0];
 	plane_pixel_rate = skl_adjusted_plane_pixel_rate(cstate, intel_pstate);
 
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.c b/drivers/gpu/drm/i915/intel_ringbuffer.c
index 66a2b8b83972..513a0f4b469b 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.c
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.c
@@ -49,7 +49,7 @@ static int __intel_ring_space(int head, int tail, int size)
 
 void intel_ring_update_space(struct intel_ring *ring)
 {
-	ring->space = __intel_ring_space(ring->head, ring->tail, ring->size);
+	ring->space = __intel_ring_space(ring->head, ring->emit, ring->size);
 }
 
 static int
@@ -774,8 +774,8 @@ static void i9xx_submit_request(struct drm_i915_gem_request *request)
 
 	i915_gem_request_submit(request);
 
-	assert_ring_tail_valid(request->ring, request->tail);
-	I915_WRITE_TAIL(request->engine, request->tail);
+	I915_WRITE_TAIL(request->engine,
+			intel_ring_set_tail(request->ring, request->tail));
 }
 
 static void i9xx_emit_breadcrumb(struct drm_i915_gem_request *req, u32 *cs)
@@ -1316,11 +1316,23 @@ err:
 	return PTR_ERR(addr);
 }
 
+void intel_ring_reset(struct intel_ring *ring, u32 tail)
+{
+	GEM_BUG_ON(!list_empty(&ring->request_list));
+	ring->tail = tail;
+	ring->head = tail;
+	ring->emit = tail;
+	intel_ring_update_space(ring);
+}
+
 void intel_ring_unpin(struct intel_ring *ring)
 {
 	GEM_BUG_ON(!ring->vma);
 	GEM_BUG_ON(!ring->vaddr);
 
+	/* Discard any unused bytes beyond that submitted to hw. */
+	intel_ring_reset(ring, ring->tail);
+
 	if (i915_vma_is_map_and_fenceable(ring->vma))
 		i915_vma_unpin_iomap(ring->vma);
 	else
@@ -1562,8 +1574,9 @@ void intel_legacy_submission_resume(struct drm_i915_private *dev_priv)
 	struct intel_engine_cs *engine;
 	enum intel_engine_id id;
 
+	/* Restart from the beginning of the rings for convenience */
 	for_each_engine(engine, dev_priv, id)
-		engine->buffer->head = engine->buffer->tail;
+		intel_ring_reset(engine->buffer, 0);
 }
 
 static int ring_request_alloc(struct drm_i915_gem_request *request)
@@ -1616,7 +1629,7 @@ static int wait_for_space(struct drm_i915_gem_request *req, int bytes)
 		unsigned space;
 
 		/* Would completion of this request free enough space? */
-		space = __intel_ring_space(target->postfix, ring->tail,
+		space = __intel_ring_space(target->postfix, ring->emit,
 					   ring->size);
 		if (space >= bytes)
 			break;
@@ -1641,8 +1654,8 @@ static int wait_for_space(struct drm_i915_gem_request *req, int bytes)
 u32 *intel_ring_begin(struct drm_i915_gem_request *req, int num_dwords)
 {
 	struct intel_ring *ring = req->ring;
-	int remain_actual = ring->size - ring->tail;
-	int remain_usable = ring->effective_size - ring->tail;
+	int remain_actual = ring->size - ring->emit;
+	int remain_usable = ring->effective_size - ring->emit;
 	int bytes = num_dwords * sizeof(u32);
 	int total_bytes, wait_bytes;
 	bool need_wrap = false;
@@ -1678,17 +1691,17 @@ u32 *intel_ring_begin(struct drm_i915_gem_request *req, int num_dwords)
 
 	if (unlikely(need_wrap)) {
 		GEM_BUG_ON(remain_actual > ring->space);
-		GEM_BUG_ON(ring->tail + remain_actual > ring->size);
+		GEM_BUG_ON(ring->emit + remain_actual > ring->size);
 
 		/* Fill the tail with MI_NOOP */
-		memset(ring->vaddr + ring->tail, 0, remain_actual);
-		ring->tail = 0;
+		memset(ring->vaddr + ring->emit, 0, remain_actual);
+		ring->emit = 0;
 		ring->space -= remain_actual;
 	}
 
-	GEM_BUG_ON(ring->tail > ring->size - bytes);
-	cs = ring->vaddr + ring->tail;
-	ring->tail += bytes;
+	GEM_BUG_ON(ring->emit > ring->size - bytes);
+	cs = ring->vaddr + ring->emit;
+	ring->emit += bytes;
 	ring->space -= bytes;
 	GEM_BUG_ON(ring->space < 0);
 
@@ -1699,7 +1712,7 @@ u32 *intel_ring_begin(struct drm_i915_gem_request *req, int num_dwords)
 int intel_ring_cacheline_align(struct drm_i915_gem_request *req)
 {
 	int num_dwords =
-		(req->ring->tail & (CACHELINE_BYTES - 1)) / sizeof(uint32_t);
+		(req->ring->emit & (CACHELINE_BYTES - 1)) / sizeof(uint32_t);
 	u32 *cs;
 
 	if (num_dwords == 0)
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.h b/drivers/gpu/drm/i915/intel_ringbuffer.h
index a82a0807f64d..f7144fe09613 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.h
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.h
@@ -145,6 +145,7 @@ struct intel_ring {
 
 	u32 head;
 	u32 tail;
+	u32 emit;
 
 	int space;
 	int size;
@@ -488,6 +489,8 @@ intel_write_status_page(struct intel_engine_cs *engine, int reg, u32 value)
 struct intel_ring *
 intel_engine_create_ring(struct intel_engine_cs *engine, int size);
 int intel_ring_pin(struct intel_ring *ring, unsigned int offset_bias);
+void intel_ring_reset(struct intel_ring *ring, u32 tail);
+void intel_ring_update_space(struct intel_ring *ring);
 void intel_ring_unpin(struct intel_ring *ring);
 void intel_ring_free(struct intel_ring *ring);
 
@@ -511,7 +514,7 @@ intel_ring_advance(struct drm_i915_gem_request *req, u32 *cs)
 	 * reserved for the command packet (i.e. the value passed to
 	 * intel_ring_begin()).
 	 */
-	GEM_BUG_ON((req->ring->vaddr + req->ring->tail) != cs);
+	GEM_BUG_ON((req->ring->vaddr + req->ring->emit) != cs);
 }
 
 static inline u32
@@ -540,7 +543,19 @@ assert_ring_tail_valid(const struct intel_ring *ring, unsigned int tail)
 	GEM_BUG_ON(tail >= ring->size);
 }
 
-void intel_ring_update_space(struct intel_ring *ring);
+static inline unsigned int
+intel_ring_set_tail(struct intel_ring *ring, unsigned int tail)
+{
+	/* Whilst writes to the tail are strictly order, there is no
+	 * serialisation between readers and the writers. The tail may be
+	 * read by i915_gem_request_retire() just as it is being updated
+	 * by execlists, as although the breadcrumb is complete, the context
+	 * switch hasn't been seen.
+	 */
+	assert_ring_tail_valid(ring, tail);
+	ring->tail = tail;
+	return tail;
+}
 
 void intel_engine_init_global_seqno(struct intel_engine_cs *engine, u32 seqno);
 
diff --git a/drivers/gpu/drm/mgag200/mgag200_mode.c b/drivers/gpu/drm/mgag200/mgag200_mode.c
index adb411a078e8..f4b53588e071 100644
--- a/drivers/gpu/drm/mgag200/mgag200_mode.c
+++ b/drivers/gpu/drm/mgag200/mgag200_mode.c
@@ -1173,7 +1173,10 @@ static int mga_crtc_mode_set(struct drm_crtc *crtc,
 
 
 	if (IS_G200_SE(mdev)) {
-		if (mdev->unique_rev_id >= 0x02) {
+		if  (mdev->unique_rev_id >= 0x04) {
+			WREG8(MGAREG_CRTCEXT_INDEX, 0x06);
+			WREG8(MGAREG_CRTCEXT_DATA, 0);
+		} else if (mdev->unique_rev_id >= 0x02) {
 			u8 hi_pri_lvl;
 			u32 bpp;
 			u32 mb;
@@ -1639,6 +1642,10 @@ static int mga_vga_mode_valid(struct drm_connector *connector,
 			if (mga_vga_calculate_mode_bandwidth(mode, bpp)
 				> (30100 * 1024))
 				return MODE_BANDWIDTH;
+		} else {
+			if (mga_vga_calculate_mode_bandwidth(mode, bpp)
+				> (55000 * 1024))
+				return MODE_BANDWIDTH;
 		}
 	} else if (mdev->type == G200_WB) {
 		if (mode->hdisplay > 1280)
diff --git a/drivers/gpu/drm/mxsfb/mxsfb_crtc.c b/drivers/gpu/drm/mxsfb/mxsfb_crtc.c
index 1144e0c9e894..0abe77675b76 100644
--- a/drivers/gpu/drm/mxsfb/mxsfb_crtc.c
+++ b/drivers/gpu/drm/mxsfb/mxsfb_crtc.c
@@ -35,6 +35,13 @@
 #include "mxsfb_drv.h"
 #include "mxsfb_regs.h"
 
+#define MXS_SET_ADDR		0x4
+#define MXS_CLR_ADDR		0x8
+#define MODULE_CLKGATE		BIT(30)
+#define MODULE_SFTRST		BIT(31)
+/* 1 second delay should be plenty of time for block reset */
+#define RESET_TIMEOUT		1000000
+
 static u32 set_hsync_pulse_width(struct mxsfb_drm_private *mxsfb, u32 val)
 {
 	return (val & mxsfb->devdata->hs_wdth_mask) <<
@@ -159,6 +166,36 @@ static void mxsfb_disable_controller(struct mxsfb_drm_private *mxsfb)
 		clk_disable_unprepare(mxsfb->clk_disp_axi);
 }
 
+/*
+ * Clear the bit and poll it cleared.  This is usually called with
+ * a reset address and mask being either SFTRST(bit 31) or CLKGATE
+ * (bit 30).
+ */
+static int clear_poll_bit(void __iomem *addr, u32 mask)
+{
+	u32 reg;
+
+	writel(mask, addr + MXS_CLR_ADDR);
+	return readl_poll_timeout(addr, reg, !(reg & mask), 0, RESET_TIMEOUT);
+}
+
+static int mxsfb_reset_block(void __iomem *reset_addr)
+{
+	int ret;
+
+	ret = clear_poll_bit(reset_addr, MODULE_SFTRST);
+	if (ret)
+		return ret;
+
+	writel(MODULE_CLKGATE, reset_addr + MXS_CLR_ADDR);
+
+	ret = clear_poll_bit(reset_addr, MODULE_SFTRST);
+	if (ret)
+		return ret;
+
+	return clear_poll_bit(reset_addr, MODULE_CLKGATE);
+}
+
 static void mxsfb_crtc_mode_set_nofb(struct mxsfb_drm_private *mxsfb)
 {
 	struct drm_display_mode *m = &mxsfb->pipe.crtc.state->adjusted_mode;
@@ -173,6 +210,11 @@ static void mxsfb_crtc_mode_set_nofb(struct mxsfb_drm_private *mxsfb)
 	 */
 	mxsfb_enable_axi_clk(mxsfb);
 
+	/* Mandatory eLCDIF reset as per the Reference Manual */
+	err = mxsfb_reset_block(mxsfb->base);
+	if (err)
+		return;
+
 	/* Clear the FIFOs */
 	writel(CTRL1_FIFO_CLEAR, mxsfb->base + LCDC_CTRL1 + REG_SET);
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_acpi.c b/drivers/gpu/drm/nouveau/nouveau_acpi.c
index 39468c218027..7459ef9943ec 100644
--- a/drivers/gpu/drm/nouveau/nouveau_acpi.c
+++ b/drivers/gpu/drm/nouveau/nouveau_acpi.c
@@ -60,15 +60,13 @@ bool nouveau_is_v1_dsm(void) {
 }
 
 #ifdef CONFIG_VGA_SWITCHEROO
-static const char nouveau_dsm_muid[] = {
-	0xA0, 0xA0, 0x95, 0x9D, 0x60, 0x00, 0x48, 0x4D,
-	0xB3, 0x4D, 0x7E, 0x5F, 0xEA, 0x12, 0x9F, 0xD4,
-};
+static const guid_t nouveau_dsm_muid =
+	GUID_INIT(0x9D95A0A0, 0x0060, 0x4D48,
+		  0xB3, 0x4D, 0x7E, 0x5F, 0xEA, 0x12, 0x9F, 0xD4);
 
-static const char nouveau_op_dsm_muid[] = {
-	0xF8, 0xD8, 0x86, 0xA4, 0xDA, 0x0B, 0x1B, 0x47,
-	0xA7, 0x2B, 0x60, 0x42, 0xA6, 0xB5, 0xBE, 0xE0,
-};
+static const guid_t nouveau_op_dsm_muid =
+	GUID_INIT(0xA486D8F8, 0x0BDA, 0x471B,
+		  0xA7, 0x2B, 0x60, 0x42, 0xA6, 0xB5, 0xBE, 0xE0);
 
 static int nouveau_optimus_dsm(acpi_handle handle, int func, int arg, uint32_t *result)
 {
@@ -86,7 +84,7 @@ static int nouveau_optimus_dsm(acpi_handle handle, int func, int arg, uint32_t *
 		args_buff[i] = (arg >> i * 8) & 0xFF;
 
 	*result = 0;
-	obj = acpi_evaluate_dsm_typed(handle, nouveau_op_dsm_muid, 0x00000100,
+	obj = acpi_evaluate_dsm_typed(handle, &nouveau_op_dsm_muid, 0x00000100,
 				      func, &argv4, ACPI_TYPE_BUFFER);
 	if (!obj) {
 		acpi_handle_info(handle, "failed to evaluate _DSM\n");
@@ -138,7 +136,7 @@ static int nouveau_dsm(acpi_handle handle, int func, int arg)
 		.integer.value = arg,
 	};
 
-	obj = acpi_evaluate_dsm_typed(handle, nouveau_dsm_muid, 0x00000102,
+	obj = acpi_evaluate_dsm_typed(handle, &nouveau_dsm_muid, 0x00000102,
 				      func, &argv4, ACPI_TYPE_INTEGER);
 	if (!obj) {
 		acpi_handle_info(handle, "failed to evaluate _DSM\n");
@@ -259,7 +257,7 @@ static void nouveau_dsm_pci_probe(struct pci_dev *pdev, acpi_handle *dhandle_out
 	if (!acpi_has_method(dhandle, "_DSM"))
 		return;
 
-	supports_mux = acpi_check_dsm(dhandle, nouveau_dsm_muid, 0x00000102,
+	supports_mux = acpi_check_dsm(dhandle, &nouveau_dsm_muid, 0x00000102,
 				      1 << NOUVEAU_DSM_POWER);
 	optimus_funcs = nouveau_dsm_get_optimus_functions(dhandle);
 
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/mxm/base.c b/drivers/gpu/drm/nouveau/nvkm/subdev/mxm/base.c
index e3e2f5e83815..f44682d62f75 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/mxm/base.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/mxm/base.c
@@ -81,10 +81,9 @@ mxm_shadow_dsm(struct nvkm_mxm *mxm, u8 version)
 {
 	struct nvkm_subdev *subdev = &mxm->subdev;
 	struct nvkm_device *device = subdev->device;
-	static char muid[] = {
-		0x00, 0xA4, 0x04, 0x40, 0x7D, 0x91, 0xF2, 0x4C,
-		0xB8, 0x9C, 0x79, 0xB6, 0x2F, 0xD5, 0x56, 0x65
-	};
+	static guid_t muid =
+		GUID_INIT(0x4004A400, 0x917D, 0x4CF2,
+			  0xB8, 0x9C, 0x79, 0xB6, 0x2F, 0xD5, 0x56, 0x65);
 	u32 mxms_args[] = { 0x00000000 };
 	union acpi_object argv4 = {
 		.buffer.type = ACPI_TYPE_BUFFER,
@@ -105,7 +104,7 @@ mxm_shadow_dsm(struct nvkm_mxm *mxm, u8 version)
 	 * unless you pass in exactly the version it supports..
 	 */
 	rev = (version & 0xf0) << 4 | (version & 0x0f);
-	obj = acpi_evaluate_dsm(handle, muid, rev, 0x00000010, &argv4);
+	obj = acpi_evaluate_dsm(handle, &muid, rev, 0x00000010, &argv4);
 	if (!obj) {
 		nvkm_debug(subdev, "DSM MXMS failed\n");
 		return false;
diff --git a/drivers/gpu/drm/radeon/cik.c b/drivers/gpu/drm/radeon/cik.c
index 008c145b7f29..ca44233ceacc 100644
--- a/drivers/gpu/drm/radeon/cik.c
+++ b/drivers/gpu/drm/radeon/cik.c
@@ -9267,8 +9267,11 @@ static void dce8_program_watermarks(struct radeon_device *rdev,
 	u32 tmp, wm_mask;
 
 	if (radeon_crtc->base.enabled && num_heads && mode) {
-		active_time = 1000000UL * (u32)mode->crtc_hdisplay / (u32)mode->clock;
-		line_time = min((u32) (1000000UL * (u32)mode->crtc_htotal / (u32)mode->clock), (u32)65535);
+		active_time = (u32) div_u64((u64)mode->crtc_hdisplay * 1000000,
+					    (u32)mode->clock);
+		line_time = (u32) div_u64((u64)mode->crtc_htotal * 1000000,
+					  (u32)mode->clock);
+		line_time = min(line_time, (u32)65535);
 
 		/* watermark for high clocks */
 		if ((rdev->pm.pm_method == PM_METHOD_DPM) &&
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index 0bf103536404..534637203e70 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -2266,8 +2266,11 @@ static void evergreen_program_watermarks(struct radeon_device *rdev,
 	fixed20_12 a, b, c;
 
 	if (radeon_crtc->base.enabled && num_heads && mode) {
-		active_time = 1000000UL * (u32)mode->crtc_hdisplay / (u32)mode->clock;
-		line_time = min((u32) (1000000UL * (u32)mode->crtc_htotal / (u32)mode->clock), (u32)65535);
+		active_time = (u32) div_u64((u64)mode->crtc_hdisplay * 1000000,
+					    (u32)mode->clock);
+		line_time = (u32) div_u64((u64)mode->crtc_htotal * 1000000,
+					  (u32)mode->clock);
+		line_time = min(line_time, (u32)65535);
 		priority_a_cnt = 0;
 		priority_b_cnt = 0;
 		dram_channels = evergreen_get_number_of_dram_channels(rdev);
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index c1c8e2208a21..e562a78510ff 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -375,7 +375,7 @@ struct radeon_fence {
 	unsigned		ring;
 	bool			is_vm_update;
 
-	wait_queue_t		fence_wake;
+	wait_queue_entry_t		fence_wake;
 };
 
 int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring);
diff --git a/drivers/gpu/drm/radeon/radeon_combios.c b/drivers/gpu/drm/radeon/radeon_combios.c
index 432480ff9d22..3178ba0c537c 100644
--- a/drivers/gpu/drm/radeon/radeon_combios.c
+++ b/drivers/gpu/drm/radeon/radeon_combios.c
@@ -3393,6 +3393,13 @@ void radeon_combios_asic_init(struct drm_device *dev)
 	    rdev->pdev->subsystem_vendor == 0x103c &&
 	    rdev->pdev->subsystem_device == 0x280a)
 		return;
+	/* quirk for rs4xx Toshiba Sattellite L20-183 latop to make it resume
+	 * - it hangs on resume inside the dynclk 1 table.
+	 */
+	if (rdev->family == CHIP_RS400 &&
+	    rdev->pdev->subsystem_vendor == 0x1179 &&
+	    rdev->pdev->subsystem_device == 0xff31)
+	        return;
 
 	/* DYN CLK 1 */
 	table = combios_get_table_offset(dev, COMBIOS_DYN_CLK_1_TABLE);
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index 6ecf42783d4b..0a6444d72000 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -136,6 +136,10 @@ static struct radeon_px_quirk radeon_px_quirk_list[] = {
 	 * https://bugzilla.kernel.org/show_bug.cgi?id=51381
 	 */
 	{ PCI_VENDOR_ID_ATI, 0x6840, 0x1043, 0x2122, RADEON_PX_QUIRK_DISABLE_PX },
+	/* Asus K53TK laptop with AMD A6-3420M APU and Radeon 7670m GPU
+	 * https://bugs.freedesktop.org/show_bug.cgi?id=101491
+	 */
+	{ PCI_VENDOR_ID_ATI, 0x6741, 0x1043, 0x2122, RADEON_PX_QUIRK_DISABLE_PX },
 	/* macbook pro 8.2 */
 	{ PCI_VENDOR_ID_ATI, 0x6741, PCI_VENDOR_ID_APPLE, 0x00e2, RADEON_PX_QUIRK_LONG_WAKEUP },
 	{ 0, 0, 0, 0, 0 },
diff --git a/drivers/gpu/drm/radeon/radeon_fence.c b/drivers/gpu/drm/radeon/radeon_fence.c
index ef09f0a63754..e86f2bd38410 100644
--- a/drivers/gpu/drm/radeon/radeon_fence.c
+++ b/drivers/gpu/drm/radeon/radeon_fence.c
@@ -158,7 +158,7 @@ int radeon_fence_emit(struct radeon_device *rdev,
  * for the fence locking itself, so unlocked variants are used for
  * fence_signal, and remove_wait_queue.
  */
-static int radeon_fence_check_signaled(wait_queue_t *wait, unsigned mode, int flags, void *key)
+static int radeon_fence_check_signaled(wait_queue_entry_t *wait, unsigned mode, int flags, void *key)
 {
 	struct radeon_fence *fence;
 	u64 seq;
diff --git a/drivers/gpu/drm/radeon/radeon_uvd.c b/drivers/gpu/drm/radeon/radeon_uvd.c
index 7431eb4a11b7..d34d1cf33895 100644
--- a/drivers/gpu/drm/radeon/radeon_uvd.c
+++ b/drivers/gpu/drm/radeon/radeon_uvd.c
@@ -621,7 +621,7 @@ static int radeon_uvd_cs_reloc(struct radeon_cs_parser *p,
 	}
 
 	/* TODO: is this still necessary on NI+ ? */
-	if ((cmd == 0 || cmd == 1 || cmd == 0x3) &&
+	if ((cmd == 0 || cmd == 0x3) &&
 	    (start >> 28) != (p->rdev->uvd.gpu_addr >> 28)) {
 		DRM_ERROR("msg/fb buffer %LX-%LX out of 256MB segment!\n",
 			  start, end);
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index 76d1888528e6..5303f25d5280 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -2284,8 +2284,11 @@ static void dce6_program_watermarks(struct radeon_device *rdev,
 	fixed20_12 a, b, c;
 
 	if (radeon_crtc->base.enabled && num_heads && mode) {
-		active_time = 1000000UL * (u32)mode->crtc_hdisplay / (u32)mode->clock;
-		line_time = min((u32) (1000000UL * (u32)mode->crtc_htotal / (u32)mode->clock), (u32)65535);
+		active_time = (u32) div_u64((u64)mode->crtc_hdisplay * 1000000,
+					    (u32)mode->clock);
+		line_time = (u32) div_u64((u64)mode->crtc_htotal * 1000000,
+					  (u32)mode->clock);
+		line_time = min(line_time, (u32)65535);
 		priority_a_cnt = 0;
 		priority_b_cnt = 0;
 
diff --git a/drivers/gpu/drm/tegra/drm.c b/drivers/gpu/drm/tegra/drm.c
index 9a1e34e48f64..81f86a67c10d 100644
--- a/drivers/gpu/drm/tegra/drm.c
+++ b/drivers/gpu/drm/tegra/drm.c
@@ -451,18 +451,6 @@ fail:
 
 
 #ifdef CONFIG_DRM_TEGRA_STAGING
-static struct tegra_drm_context *
-tegra_drm_file_get_context(struct tegra_drm_file *file, u32 id)
-{
-	struct tegra_drm_context *context;
-
-	mutex_lock(&file->lock);
-	context = idr_find(&file->contexts, id);
-	mutex_unlock(&file->lock);
-
-	return context;
-}
-
 static int tegra_gem_create(struct drm_device *drm, void *data,
 			    struct drm_file *file)
 {
@@ -551,7 +539,7 @@ static int tegra_client_open(struct tegra_drm_file *fpriv,
 	if (err < 0)
 		return err;
 
-	err = idr_alloc(&fpriv->contexts, context, 0, 0, GFP_KERNEL);
+	err = idr_alloc(&fpriv->contexts, context, 1, 0, GFP_KERNEL);
 	if (err < 0) {
 		client->ops->close_channel(context);
 		return err;
@@ -606,7 +594,7 @@ static int tegra_close_channel(struct drm_device *drm, void *data,
 
 	mutex_lock(&fpriv->lock);
 
-	context = tegra_drm_file_get_context(fpriv, args->context);
+	context = idr_find(&fpriv->contexts, args->context);
 	if (!context) {
 		err = -EINVAL;
 		goto unlock;
@@ -631,7 +619,7 @@ static int tegra_get_syncpt(struct drm_device *drm, void *data,
 
 	mutex_lock(&fpriv->lock);
 
-	context = tegra_drm_file_get_context(fpriv, args->context);
+	context = idr_find(&fpriv->contexts, args->context);
 	if (!context) {
 		err = -ENODEV;
 		goto unlock;
@@ -660,7 +648,7 @@ static int tegra_submit(struct drm_device *drm, void *data,
 
 	mutex_lock(&fpriv->lock);
 
-	context = tegra_drm_file_get_context(fpriv, args->context);
+	context = idr_find(&fpriv->contexts, args->context);
 	if (!context) {
 		err = -ENODEV;
 		goto unlock;
@@ -685,7 +673,7 @@ static int tegra_get_syncpt_base(struct drm_device *drm, void *data,
 
 	mutex_lock(&fpriv->lock);
 
-	context = tegra_drm_file_get_context(fpriv, args->context);
+	context = idr_find(&fpriv->contexts, args->context);
 	if (!context) {
 		err = -ENODEV;
 		goto unlock;
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_cmdbuf_res.c b/drivers/gpu/drm/vmwgfx/vmwgfx_cmdbuf_res.c
index 13db8a2851ed..1f013d45c9e9 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_cmdbuf_res.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_cmdbuf_res.c
@@ -321,6 +321,7 @@ void vmw_cmdbuf_res_man_destroy(struct vmw_cmdbuf_res_manager *man)
 	list_for_each_entry_safe(entry, next, &man->list, head)
 		vmw_cmdbuf_res_free(man, entry);
 
+	drm_ht_remove(&man->resources);
 	kfree(man);
 }
 
diff --git a/drivers/gpu/host1x/dev.c b/drivers/gpu/host1x/dev.c
index f05ebb14fa63..ac65f52850a6 100644
--- a/drivers/gpu/host1x/dev.c
+++ b/drivers/gpu/host1x/dev.c
@@ -172,7 +172,7 @@ static int host1x_probe(struct platform_device *pdev)
 
 	host->rst = devm_reset_control_get(&pdev->dev, "host1x");
 	if (IS_ERR(host->rst)) {
-		err = PTR_ERR(host->clk);
+		err = PTR_ERR(host->rst);
 		dev_err(&pdev->dev, "failed to get reset: %d\n", err);
 		return err;
 	}
diff --git a/drivers/gpu/vga/vgaarb.c b/drivers/gpu/vga/vgaarb.c
index 92f1452dad57..76875f6299b8 100644
--- a/drivers/gpu/vga/vgaarb.c
+++ b/drivers/gpu/vga/vgaarb.c
@@ -417,7 +417,7 @@ int vga_get(struct pci_dev *pdev, unsigned int rsrc, int interruptible)
 {
 	struct vga_device *vgadev, *conflict;
 	unsigned long flags;
-	wait_queue_t wait;
+	wait_queue_entry_t wait;
 	int rc = 0;
 
 	vga_check_first_use();
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 04cee65531d7..2241e7913c0d 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -826,11 +826,35 @@ static int hid_scan_report(struct hid_device *hid)
 				 * hid-rmi should take care of them,
 				 * not hid-generic
 				 */
-				if (IS_ENABLED(CONFIG_HID_RMI))
-					hid->group = HID_GROUP_RMI;
+				hid->group = HID_GROUP_RMI;
 		break;
 	}
 
+	/* fall back to generic driver in case specific driver doesn't exist */
+	switch (hid->group) {
+	case HID_GROUP_MULTITOUCH_WIN_8:
+		/* fall-through */
+	case HID_GROUP_MULTITOUCH:
+		if (!IS_ENABLED(CONFIG_HID_MULTITOUCH))
+			hid->group = HID_GROUP_GENERIC;
+		break;
+	case HID_GROUP_SENSOR_HUB:
+		if (!IS_ENABLED(CONFIG_HID_SENSOR_HUB))
+			hid->group = HID_GROUP_GENERIC;
+		break;
+	case HID_GROUP_RMI:
+		if (!IS_ENABLED(CONFIG_HID_RMI))
+			hid->group = HID_GROUP_GENERIC;
+		break;
+	case HID_GROUP_WACOM:
+		if (!IS_ENABLED(CONFIG_HID_WACOM))
+			hid->group = HID_GROUP_GENERIC;
+		break;
+	case HID_GROUP_LOGITECH_DJ_DEVICE:
+		if (!IS_ENABLED(CONFIG_HID_LOGITECH_DJ))
+			hid->group = HID_GROUP_GENERIC;
+		break;
+	}
 	vfree(parser);
 	return 0;
 }
@@ -1763,15 +1787,23 @@ EXPORT_SYMBOL_GPL(hid_disconnect);
  * used as a driver. See hid_scan_report().
  */
 static const struct hid_device_id hid_have_special_driver[] = {
+#if IS_ENABLED(CONFIG_HID_A4TECH)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_A4TECH, USB_DEVICE_ID_A4TECH_WCP32PU) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_A4TECH, USB_DEVICE_ID_A4TECH_X5_005D) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_A4TECH, USB_DEVICE_ID_A4TECH_RP_649) },
+#endif
+#if IS_ENABLED(CONFIG_HID_ACCUTOUCH)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ELO, USB_DEVICE_ID_ELO_ACCUTOUCH_2216) },
+#endif
+#if IS_ENABLED(CONFIG_HID_ACRUX)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ACRUX, 0x0802) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ACRUX, 0xf705) },
+#endif
+#if IS_ENABLED(CONFIG_HID_ALPS)
 	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_ANY, USB_VENDOR_ID_ALPS_JP, HID_DEVICE_ID_ALPS_U1_DUAL) },
+#endif
+#if IS_ENABLED(CONFIG_HID_APPLE)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_MIGHTYMOUSE) },
-	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_MAGICMOUSE) },
-	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_MAGICTRACKPAD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_FOUNTAIN_ANSI) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_FOUNTAIN_ISO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_GEYSER_ANSI) },
@@ -1792,11 +1824,6 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_GEYSER4_HF_ANSI) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_GEYSER4_HF_ISO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_GEYSER4_HF_JIS) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL2) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL3) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL4) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL5) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_ANSI) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_ISO) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_JIS) },
@@ -1851,62 +1878,100 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_MAGIC_KEYBOARD_ANSI) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_FOUNTAIN_TP_ONLY) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_GEYSER1_TP_ONLY) },
+#endif
+#if IS_ENABLED(CONFIG_HID_APPLEIR)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL2) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL3) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL4) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_IRCONTROL5) },
+#endif
+#if IS_ENABLED(CONFIG_HID_ASUS)
 	{ HID_I2C_DEVICE(USB_VENDOR_ID_ASUSTEK, USB_DEVICE_ID_ASUSTEK_I2C_KEYBOARD) },
 	{ HID_I2C_DEVICE(USB_VENDOR_ID_ASUSTEK, USB_DEVICE_ID_ASUSTEK_I2C_TOUCHPAD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ASUSTEK, USB_DEVICE_ID_ASUSTEK_ROG_KEYBOARD1) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ASUSTEK, USB_DEVICE_ID_ASUSTEK_ROG_KEYBOARD2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ASUSTEK, USB_DEVICE_ID_ASUSTEK_T100_KEYBOARD) },
+#endif
+#if IS_ENABLED(CONFIG_HID_AUREAL)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_AUREAL, USB_DEVICE_ID_AUREAL_W01RN) },
+#endif
+#if IS_ENABLED(CONFIG_HID_BELKIN)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_BELKIN, USB_DEVICE_ID_FLIP_KVM) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LABTEC, USB_DEVICE_ID_LABTEC_WIRELESS_KEYBOARD) },
+#endif
+#if IS_ENABLED(CONFIG_HID_BETOP_FF)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_BETOP_2185BFM, 0x2208) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_BETOP_2185PC, 0x5506) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_BETOP_2185V2PC, 0x1850) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_BETOP_2185V2BFM, 0x5500) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_BTC, USB_DEVICE_ID_BTC_EMPREX_REMOTE) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_BTC, USB_DEVICE_ID_BTC_EMPREX_REMOTE_2) },
+#endif
+#if IS_ENABLED(CONFIG_HID_CHERRY)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CHERRY, USB_DEVICE_ID_CHERRY_CYMOTION) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CHERRY, USB_DEVICE_ID_CHERRY_CYMOTION_SOLAR) },
+#endif
+#if IS_ENABLED(CONFIG_HID_CHICONY)
 	{ 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_CHICONY, USB_DEVICE_ID_CHICONY_ACER_SWITCH12) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_JESS, USB_DEVICE_ID_JESS_ZEN_AIO_KBD) },
+#endif
+#if IS_ENABLED(CONFIG_HID_CMEDIA)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_CMEDIA, USB_DEVICE_ID_CM6533) },
+#endif
+#if IS_ENABLED(CONFIG_HID_CORSAIR)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CORSAIR, USB_DEVICE_ID_CORSAIR_K90) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CORSAIR, USB_DEVICE_ID_CORSAIR_SCIMITAR_PRO_RGB) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_CREATIVELABS, USB_DEVICE_ID_PRODIKEYS_PCMIDI) },
+#endif
+#if IS_ENABLED(CONFIG_HID_CP2112)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYGNAL, USB_DEVICE_ID_CYGNAL_CP2112) },
+#endif
+#if IS_ENABLED(CONFIG_HID_CYPRESS)
 	{ 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_DELCOM, USB_DEVICE_ID_DELCOM_VISUAL_IND) },
+#endif
+#if IS_ENABLED(CONFIG_HID_DRAGONRISE)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, 0x0006) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, 0x0011) },
-#if IS_ENABLED(CONFIG_HID_MAYFLASH)
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, USB_DEVICE_ID_DRAGONRISE_PS3) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, USB_DEVICE_ID_DRAGONRISE_DOLPHINBAR) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, USB_DEVICE_ID_DRAGONRISE_GAMECUBE1) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, USB_DEVICE_ID_DRAGONRISE_GAMECUBE2) },
 #endif
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DREAM_CHEEKY, USB_DEVICE_ID_DREAM_CHEEKY_WN) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DREAM_CHEEKY, USB_DEVICE_ID_DREAM_CHEEKY_FA) },
+#if IS_ENABLED(CONFIG_HID_ELECOM)
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_ELECOM, USB_DEVICE_ID_ELECOM_BM084) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ELECOM, USB_DEVICE_ID_ELECOM_DEFT_WIRED) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ELECOM, USB_DEVICE_ID_ELECOM_DEFT_WIRELESS) },
+#endif
+#if IS_ENABLED(CONFIG_HID_ELO)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ELO, 0x0009) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ELO, 0x0030) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_ELO, USB_DEVICE_ID_ELO_ACCUTOUCH_2216) },
+#endif
+#if IS_ENABLED(CONFIG_HID_EMS_FF)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_EMS, USB_DEVICE_ID_EMS_TRIO_LINKER_PLUS_II) },
+#endif
+#if IS_ENABLED(CONFIG_HID_EZKEY)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_EZKEY, USB_DEVICE_ID_BTC_8193) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GAMERON, USB_DEVICE_ID_GAMERON_DUAL_PSX_ADAPTOR) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GAMERON, USB_DEVICE_ID_GAMERON_DUAL_PCS_ADAPTOR) },
+#endif
+#if IS_ENABLED(CONFIG_HID_GEMBIRD)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GEMBIRD, USB_DEVICE_ID_GEMBIRD_JPD_DUALFORCE2) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GREENASIA, 0x0003) },
+#endif
+#if IS_ENABLED(CONFIG_HID_GFRM)
+        { HID_BLUETOOTH_DEVICE(0x58, 0x2000) },
+        { HID_BLUETOOTH_DEVICE(0x471, 0x2210) },
+#endif
+#if IS_ENABLED(CONFIG_HID_GREENASIA)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GREENASIA, 0x0012) },
+#endif
+#if IS_ENABLED(CONFIG_HID_GT683R)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MSI, USB_DEVICE_ID_MSI_GT683R_LED_PANEL) },
+#endif
+#if IS_ENABLED(CONFIG_HID_GYRATION)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE) },
 	{ 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) },
+#endif
+#if IS_ENABLED(CONFIG_HID_HOLTEK)
 	{ 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_HOLTEK_ALT, USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A04A) },
@@ -1915,12 +1980,17 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK_ALT, USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A072) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK_ALT, USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A081) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK_ALT, USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A0C2) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_HUION, USB_DEVICE_ID_HUION_TABLET) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_JESS, USB_DEVICE_ID_JESS_ZEN_AIO_KBD) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_JESS2, USB_DEVICE_ID_JESS2_COLOR_RUMBLE_PAD) },
+#endif
+#if IS_ENABLED(CONFIG_HID_ICADE)
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_ION, USB_DEVICE_ID_ICADE) },
+#endif
+#if IS_ENABLED(CONFIG_HID_KENSINGTON)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KENSINGTON, USB_DEVICE_ID_KS_SLIMBLADE) },
+#endif
+#if IS_ENABLED(CONFIG_HID_KEYTOUCH)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KEYTOUCH, USB_DEVICE_ID_KEYTOUCH_IEC) },
+#endif
+#if IS_ENABLED(CONFIG_HID_KYE)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_GENIUS_GILA_GAMING_MOUSE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_GENIUS_MANTICORE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_GENIUS_GX_IMPERATOR) },
@@ -1930,21 +2000,29 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_MOUSEPEN_I608X_V2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_EASYPEN_M610X) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_PENSKETCH_M912) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_LABTEC, USB_DEVICE_ID_LABTEC_WIRELESS_KEYBOARD) },
+#endif
+#if IS_ENABLED(CONFIG_HID_LCPOWER)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LCPOWER, USB_DEVICE_ID_LCPOWER_LC1000 ) },
+#endif
+#if IS_ENABLED(CONFIG_HID_LED)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DELCOM, USB_DEVICE_ID_DELCOM_VISUAL_IND) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DREAM_CHEEKY, USB_DEVICE_ID_DREAM_CHEEKY_WN) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DREAM_CHEEKY, USB_DEVICE_ID_DREAM_CHEEKY_FA) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_LUXAFOR) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_RISO_KAGAKU, USB_DEVICE_ID_RI_KA_WEBMAIL) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_THINGM, USB_DEVICE_ID_BLINK1) },
+#endif
 #if IS_ENABLED(CONFIG_HID_LENOVO)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_TPKBD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_CUSBKBD) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_CBTKBD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_TPPRODOCK) },
 #endif
-	{ HID_USB_DEVICE(USB_VENDOR_ID_LG, USB_DEVICE_ID_LG_MELFAS_MT) },
+#if IS_ENABLED(CONFIG_HID_LOGITECH)
 	{ 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) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_RECEIVER) },
-	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_HARMONY_PS3) },
-	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_T651) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_DINOVO_DESKTOP) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_DINOVO_EDGE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_DINOVO_MINI) },
@@ -1957,7 +2035,6 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_RUMBLEPAD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_RUMBLEPAD2_2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_G29_WHEEL) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_G920_WHEEL) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_WINGMAN_F3D) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_WINGMAN_FFG ) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_FORCE3D_PRO) },
@@ -1969,17 +2046,30 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_DFGT_WHEEL) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_G25_WHEEL) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_G27_WHEEL) },
-#if IS_ENABLED(CONFIG_HID_LOGITECH_DJ)
-	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_UNIFYING_RECEIVER) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_UNIFYING_RECEIVER_2) },
-#endif
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_WII_WHEEL) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_RUMBLEPAD2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_SPACETRAVELLER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_SPACENAVIGATOR) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICOLCD) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICOLCD_BOOTLOADER) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_LUXAFOR) },
+#endif
+#if IS_ENABLED(CONFIG_HID_LOGITECH_HIDPP)
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_T651) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_G920_WHEEL) },
+#endif
+#if IS_ENABLED(CONFIG_HID_LOGITECH_DJ)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_UNIFYING_RECEIVER) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_UNIFYING_RECEIVER_2) },
+#endif
+#if IS_ENABLED(CONFIG_HID_MAGICMOUSE)
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_MAGICMOUSE) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_MAGICTRACKPAD) },
+#endif
+#if IS_ENABLED(CONFIG_HID_MAYFLASH)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, USB_DEVICE_ID_DRAGONRISE_PS3) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, USB_DEVICE_ID_DRAGONRISE_DOLPHINBAR) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, USB_DEVICE_ID_DRAGONRISE_GAMECUBE1) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, USB_DEVICE_ID_DRAGONRISE_GAMECUBE2) },
+#endif
+#if IS_ENABLED(CONFIG_HID_MICROSOFT)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_COMFORT_MOUSE_4500) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_COMFORT_KEYBOARD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_SIDEWINDER_GV) },
@@ -1995,9 +2085,22 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_DIGITAL_MEDIA_600) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_DIGITAL_MEDIA_3KV1) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_POWER_COVER) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_PRESENTER_8K_BT) },
+#endif
+#if IS_ENABLED(CONFIG_HID_MONTEREY)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MONTEREY, USB_DEVICE_ID_GENIUS_KB29E) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_MSI, USB_DEVICE_ID_MSI_GT683R_LED_PANEL) },
+#endif
+#if IS_ENABLED(CONFIG_HID_MULTITOUCH)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LG, USB_DEVICE_ID_LG_MELFAS_MT) },
+#endif
+#if IS_ENABLED(CONFIG_HID_WIIMOTE)
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_NINTENDO, USB_DEVICE_ID_NINTENDO_WIIMOTE) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_NINTENDO, USB_DEVICE_ID_NINTENDO_WIIMOTE2) },
+#endif
+#if IS_ENABLED(CONFIG_HID_NTI)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_NTI, USB_DEVICE_ID_USB_SUN) },
+#endif
+#if IS_ENABLED(CONFIG_HID_NTRIG)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_NTRIG, USB_DEVICE_ID_NTRIG_TOUCH_SCREEN) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_NTRIG, USB_DEVICE_ID_NTRIG_TOUCH_SCREEN_1) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_NTRIG, USB_DEVICE_ID_NTRIG_TOUCH_SCREEN_2) },
@@ -2017,13 +2120,41 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_NTRIG, USB_DEVICE_ID_NTRIG_TOUCH_SCREEN_16) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_NTRIG, USB_DEVICE_ID_NTRIG_TOUCH_SCREEN_17) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_NTRIG, USB_DEVICE_ID_NTRIG_TOUCH_SCREEN_18) },
+#endif
+#if IS_ENABLED(CONFIG_HID_ORTEK)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ORTEK, USB_DEVICE_ID_ORTEK_PKB1700) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ORTEK, USB_DEVICE_ID_ORTEK_WKB2000) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_SKYCABLE, USB_DEVICE_ID_SKYCABLE_WIRELESS_PRESENTER) },
+#endif
+#if IS_ENABLED(CONFIG_HID_PANTHERLORD)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_GAMERON, USB_DEVICE_ID_GAMERON_DUAL_PSX_ADAPTOR) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_GAMERON, USB_DEVICE_ID_GAMERON_DUAL_PCS_ADAPTOR) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_GREENASIA, 0x0003) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_JESS2, USB_DEVICE_ID_JESS2_COLOR_RUMBLE_PAD) },
+#endif
+#if IS_ENABLED(CONFIG_HID_PENMOUNT)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PENMOUNT, USB_DEVICE_ID_PENMOUNT_6000) },
+#endif
+#if IS_ENABLED(CONFIG_HID_PETALYNX)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PETALYNX, USB_DEVICE_ID_PETALYNX_MAXTER_REMOTE) },
+#endif
+#if IS_ENABLED(CONFIG_HID_PICOLCD)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICOLCD) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICOLCD_BOOTLOADER) },
+#endif
+#if IS_ENABLED(CONFIG_HID_PLANTRONICS)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PLANTRONICS, HID_ANY_ID) },
+#endif
+#if IS_ENABLED(CONFIG_HID_PRIMAX)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PRIMAX, USB_DEVICE_ID_PRIMAX_KEYBOARD) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_RISO_KAGAKU, USB_DEVICE_ID_RI_KA_WEBMAIL) },
+#endif
+#if IS_ENABLED(CONFIG_HID_PRODIKEYS)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_CREATIVELABS, USB_DEVICE_ID_PRODIKEYS_PCMIDI) },
+#endif
+#if IS_ENABLED(CONFIG_HID_RMI)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_X1_COVER) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_RAZER, USB_DEVICE_ID_RAZER_BLADE_14) },
+#endif
 #if IS_ENABLED(CONFIG_HID_ROCCAT)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ARVO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ISKU) },
@@ -2051,9 +2182,21 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MADCATZ, USB_DEVICE_ID_MADCATZ_RAT5) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MADCATZ, USB_DEVICE_ID_MADCATZ_RAT9) },
 #endif
+#if IS_ENABLED(CONFIG_HID_SAMSUNG)
 	{ 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) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_SKYCABLE, USB_DEVICE_ID_SKYCABLE_WIRELESS_PRESENTER) },
+#endif
+#if IS_ENABLED(CONFIG_HID_SMARTJOYPLUS)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_PLAYDOTCOM, USB_DEVICE_ID_PLAYDOTCOM_EMS_USBII) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_SMARTJOY_PLUS) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_SUPER_JOY_BOX_3) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_DUAL_USB_JOYPAD) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP_LTD, USB_DEVICE_ID_SUPER_JOY_BOX_3_PRO) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP_LTD, USB_DEVICE_ID_SUPER_DUAL_BOX_PRO) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP_LTD, USB_DEVICE_ID_SUPER_JOY_BOX_5_PRO) },
+#endif
+#if IS_ENABLED(CONFIG_HID_SONY)
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_HARMONY_PS3) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SMK, USB_DEVICE_ID_SMK_PS3_BDREMOTE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_BUZZ_CONTROLLER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_WIRELESS_BUZZ_CONTROLLER) },
@@ -2072,9 +2215,17 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_VAIO_VGX_MOUSE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_VAIO_VGP_MOUSE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SINO_LITE, USB_DEVICE_ID_SINO_LITE_CONTROLLER) },
+#endif
+#if IS_ENABLED(CONFIG_HID_SPEEDLINK)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_X_TENSIONS, USB_DEVICE_ID_SPEEDLINK_VAD_CEZANNE) },
+#endif
+#if IS_ENABLED(CONFIG_HID_STEELSERIES)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_STEELSERIES, USB_DEVICE_ID_STEELSERIES_SRWS1) },
+#endif
+#if IS_ENABLED(CONFIG_HID_SUNPLUS)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SUNPLUS, USB_DEVICE_ID_SUNPLUS_WDESKTOP) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_THINGM, USB_DEVICE_ID_BLINK1) },
+#endif
+#if IS_ENABLED(CONFIG_HID_THRUSTMASTER)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_THRUSTMASTER, 0xb300) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_THRUSTMASTER, 0xb304) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_THRUSTMASTER, 0xb323) },
@@ -2083,12 +2234,25 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_THRUSTMASTER, 0xb653) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_THRUSTMASTER, 0xb654) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_THRUSTMASTER, 0xb65a) },
+#endif
+#if IS_ENABLED(CONFIG_HID_TIVO)
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_TIVO, USB_DEVICE_ID_TIVO_SLIDE_BT) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TIVO, USB_DEVICE_ID_TIVO_SLIDE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TIVO, USB_DEVICE_ID_TIVO_SLIDE_PRO) },
+#endif
+#if IS_ENABLED(CONFIG_HID_TOPSEED)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_BTC, USB_DEVICE_ID_BTC_EMPREX_REMOTE) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_BTC, USB_DEVICE_ID_BTC_EMPREX_REMOTE_2) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_CHICONY, USB_DEVICE_ID_CHICONY_WIRELESS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TOPSEED, USB_DEVICE_ID_TOPSEED_CYBERLINK) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TOPSEED2, USB_DEVICE_ID_TOPSEED2_RF_COMBO) },
+#endif
+#if IS_ENABLED(CONFIG_HID_TWINHAN)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_TWINHAN, USB_DEVICE_ID_TWINHAN_IR_REMOTE) },
+#endif
+#if IS_ENABLED(CONFIG_HID_UCLOGIC)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_HUION, USB_DEVICE_ID_HUION_TABLET) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_HUION_TABLET) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_PF1209) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP4030U) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP5540U) },
@@ -2096,20 +2260,17 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_WP1062) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_WIRELESS_TABLET_TWHL850) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_TABLET_TWHA60) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_THQ, USB_DEVICE_ID_THQ_PS3_UDRAW) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_YIYNOVA_TABLET) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UGEE_TABLET_81) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UGEE_TABLET_45) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UCLOGIC, USB_DEVICE_ID_UCLOGIC_DRAWIMAGE_G3) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_UGTIZER, USB_DEVICE_ID_UGTIZER_TABLET_GP0610) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_UGEE, USB_DEVICE_ID_UGEE_TABLET_EX07S) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_SMARTJOY_PLUS) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_SUPER_JOY_BOX_3) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_DUAL_USB_JOYPAD) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP_LTD, USB_DEVICE_ID_SUPER_JOY_BOX_3_PRO) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP_LTD, USB_DEVICE_ID_SUPER_DUAL_BOX_PRO) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP_LTD, USB_DEVICE_ID_SUPER_JOY_BOX_5_PRO) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_PLAYDOTCOM, USB_DEVICE_ID_PLAYDOTCOM_EMS_USBII) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_UGTIZER, USB_DEVICE_ID_UGTIZER_TABLET_GP0610) },
+#endif
+#if IS_ENABLED(CONFIG_HID_UDRAW_PS3)
+	{ HID_USB_DEVICE(USB_VENDOR_ID_THQ, USB_DEVICE_ID_THQ_PS3_UDRAW) },
+#endif
+#if IS_ENABLED(CONFIG_HID_WALTOP)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_SLIM_TABLET_5_8_INCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_SLIM_TABLET_12_1_INCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_Q_PAD) },
@@ -2117,19 +2278,18 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_MEDIA_TABLET_10_6_INCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_MEDIA_TABLET_14_1_INCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_SIRIUS_BATTERY_FREE_TABLET) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_X_TENSIONS, USB_DEVICE_ID_SPEEDLINK_VAD_CEZANNE) },
+#endif
+#if IS_ENABLED(CONFIG_HID_XINMO)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_XIN_MO, USB_DEVICE_ID_XIN_MO_DUAL_ARCADE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_XIN_MO, USB_DEVICE_ID_THT_2P_ARCADE) },
+#endif
+#if IS_ENABLED(CONFIG_HID_ZEROPLUS)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ZEROPLUS, 0x0005) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ZEROPLUS, 0x0030) },
+#endif
+#if IS_ENABLED(CONFIG_HID_ZYDACRON)
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ZYDACRON, USB_DEVICE_ID_ZYDACRON_REMOTE_CONTROL) },
-
-	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_PRESENTER_8K_BT) },
-	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_NINTENDO, USB_DEVICE_ID_NINTENDO_WIIMOTE) },
-	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_NINTENDO, USB_DEVICE_ID_NINTENDO_WIIMOTE2) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_RAZER, USB_DEVICE_ID_RAZER_BLADE_14) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_CMEDIA, USB_DEVICE_ID_CM6533) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_X1_COVER) },
+#endif
 	{ }
 };
 
@@ -2147,7 +2307,7 @@ struct hid_dynid {
  * Adds a new dynamic hid device ID to this driver,
  * and causes the driver to probe for all devices again.
  */
-static ssize_t store_new_id(struct device_driver *drv, const char *buf,
+static ssize_t new_id_store(struct device_driver *drv, const char *buf,
 		size_t count)
 {
 	struct hid_driver *hdrv = to_hid_driver(drv);
@@ -2179,7 +2339,13 @@ static ssize_t store_new_id(struct device_driver *drv, const char *buf,
 
 	return ret ? : count;
 }
-static DRIVER_ATTR(new_id, S_IWUSR, NULL, store_new_id);
+static DRIVER_ATTR_WO(new_id);
+
+static struct attribute *hid_drv_attrs[] = {
+	&driver_attr_new_id.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(hid_drv);
 
 static void hid_free_dynids(struct hid_driver *hdrv)
 {
@@ -2343,6 +2509,7 @@ static int hid_uevent(struct device *dev, struct kobj_uevent_env *env)
 static struct bus_type hid_bus_type = {
 	.name		= "hid",
 	.dev_groups	= hid_dev_groups,
+	.drv_groups	= hid_drv_groups,
 	.match		= hid_bus_match,
 	.probe		= hid_device_probe,
 	.remove		= hid_device_remove,
@@ -2782,8 +2949,6 @@ EXPORT_SYMBOL_GPL(hid_destroy_device);
 int __hid_register_driver(struct hid_driver *hdrv, struct module *owner,
 		const char *mod_name)
 {
-	int ret;
-
 	hdrv->driver.name = hdrv->name;
 	hdrv->driver.bus = &hid_bus_type;
 	hdrv->driver.owner = owner;
@@ -2792,21 +2957,12 @@ int __hid_register_driver(struct hid_driver *hdrv, struct module *owner,
 	INIT_LIST_HEAD(&hdrv->dyn_list);
 	spin_lock_init(&hdrv->dyn_lock);
 
-	ret = driver_register(&hdrv->driver);
-	if (ret)
-		return ret;
-
-	ret = driver_create_file(&hdrv->driver, &driver_attr_new_id);
-	if (ret)
-		driver_unregister(&hdrv->driver);
-
-	return ret;
+	return driver_register(&hdrv->driver);
 }
 EXPORT_SYMBOL_GPL(__hid_register_driver);
 
 void hid_unregister_driver(struct hid_driver *hdrv)
 {
-	driver_remove_file(&hdrv->driver, &driver_attr_new_id);
 	driver_unregister(&hdrv->driver);
 	hid_free_dynids(hdrv);
 }
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 8ca1e8ce0af2..4f9a3938189a 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -319,6 +319,9 @@
 #define USB_VENDOR_ID_DELCOM		0x0fc5
 #define USB_DEVICE_ID_DELCOM_VISUAL_IND	0xb080
 
+#define USB_VENDOR_ID_DELL				0x413c
+#define USB_DEVICE_ID_DELL_PIXART_USB_OPTICAL_MOUSE	0x301a
+
 #define USB_VENDOR_ID_DELORME		0x1163
 #define USB_DEVICE_ID_DELORME_EARTHMATE	0x0100
 #define USB_DEVICE_ID_DELORME_EM_LT20	0x0200
diff --git a/drivers/hid/hid-magicmouse.c b/drivers/hid/hid-magicmouse.c
index 1d6c997b3001..20b40ad26325 100644
--- a/drivers/hid/hid-magicmouse.c
+++ b/drivers/hid/hid-magicmouse.c
@@ -349,7 +349,6 @@ static int magicmouse_raw_event(struct hid_device *hdev,
 
 	if (input->id.product == USB_DEVICE_ID_APPLE_MAGICMOUSE) {
 		magicmouse_emit_buttons(msc, clicks & 3);
-		input_mt_report_pointer_emulation(input, true);
 		input_report_rel(input, REL_X, x);
 		input_report_rel(input, REL_Y, y);
 	} else { /* USB_DEVICE_ID_APPLE_MAGICTRACKPAD */
@@ -389,16 +388,16 @@ static int magicmouse_setup_input(struct input_dev *input, struct hid_device *hd
 		__clear_bit(BTN_RIGHT, input->keybit);
 		__clear_bit(BTN_MIDDLE, input->keybit);
 		__set_bit(BTN_MOUSE, input->keybit);
+		__set_bit(BTN_TOOL_FINGER, input->keybit);
+		__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);
 	}
 
-	__set_bit(BTN_TOOL_FINGER, input->keybit);
-	__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(EV_ABS, input->evbit);
 
diff --git a/drivers/hid/i2c-hid/i2c-hid.c b/drivers/hid/i2c-hid/i2c-hid.c
index fb55fb4c39fc..04015032a35a 100644
--- a/drivers/hid/i2c-hid/i2c-hid.c
+++ b/drivers/hid/i2c-hid/i2c-hid.c
@@ -872,10 +872,9 @@ static int i2c_hid_fetch_hid_descriptor(struct i2c_hid *ihid)
 static int i2c_hid_acpi_pdata(struct i2c_client *client,
 		struct i2c_hid_platform_data *pdata)
 {
-	static u8 i2c_hid_guid[] = {
-		0xF7, 0xF6, 0xDF, 0x3C, 0x67, 0x42, 0x55, 0x45,
-		0xAD, 0x05, 0xB3, 0x0A, 0x3D, 0x89, 0x38, 0xDE,
-	};
+	static guid_t i2c_hid_guid =
+		GUID_INIT(0x3CDFF6F7, 0x4267, 0x4555,
+			  0xAD, 0x05, 0xB3, 0x0A, 0x3D, 0x89, 0x38, 0xDE);
 	union acpi_object *obj;
 	struct acpi_device *adev;
 	acpi_handle handle;
@@ -884,7 +883,7 @@ static int i2c_hid_acpi_pdata(struct i2c_client *client,
 	if (!handle || acpi_bus_get_device(handle, &adev))
 		return -ENODEV;
 
-	obj = acpi_evaluate_dsm_typed(handle, i2c_hid_guid, 1, 1, NULL,
+	obj = acpi_evaluate_dsm_typed(handle, &i2c_hid_guid, 1, 1, NULL,
 				      ACPI_TYPE_INTEGER);
 	if (!obj) {
 		dev_err(&client->dev, "device _DSM execution failed\n");
diff --git a/drivers/hid/intel-ish-hid/ishtp/bus.c b/drivers/hid/intel-ish-hid/ishtp/bus.c
index 5f382fedc2ab..f272cdd9bd55 100644
--- a/drivers/hid/intel-ish-hid/ishtp/bus.c
+++ b/drivers/hid/intel-ish-hid/ishtp/bus.c
@@ -321,11 +321,13 @@ static ssize_t modalias_show(struct device *dev, struct device_attribute *a,
 	len = snprintf(buf, PAGE_SIZE, "ishtp:%s\n", dev_name(dev));
 	return (len >= PAGE_SIZE) ? (PAGE_SIZE - 1) : len;
 }
+static DEVICE_ATTR_RO(modalias);
 
-static struct device_attribute ishtp_cl_dev_attrs[] = {
-	__ATTR_RO(modalias),
-	__ATTR_NULL,
+static struct attribute *ishtp_cl_dev_attrs[] = {
+	&dev_attr_modalias.attr,
+	NULL,
 };
+ATTRIBUTE_GROUPS(ishtp_cl_dev);
 
 static int ishtp_cl_uevent(struct device *dev, struct kobj_uevent_env *env)
 {
@@ -346,7 +348,7 @@ static const struct dev_pm_ops ishtp_cl_bus_dev_pm_ops = {
 
 static struct bus_type ishtp_cl_bus_type = {
 	.name		= "ishtp",
-	.dev_attrs	= ishtp_cl_dev_attrs,
+	.dev_groups	= ishtp_cl_dev_groups,
 	.probe		= ishtp_cl_device_probe,
 	.remove		= ishtp_cl_device_remove,
 	.pm		= &ishtp_cl_bus_dev_pm_ops,
diff --git a/drivers/hid/usbhid/hid-quirks.c b/drivers/hid/usbhid/hid-quirks.c
index 6316498b7812..a88e7c7bea0a 100644
--- a/drivers/hid/usbhid/hid-quirks.c
+++ b/drivers/hid/usbhid/hid-quirks.c
@@ -85,6 +85,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_CORSAIR, USB_DEVICE_ID_CORSAIR_K65RGB_RAPIDFIRE, HID_QUIRK_NO_INIT_REPORTS | HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_CORSAIR, USB_DEVICE_ID_CORSAIR_SCIMITAR_PRO_RGB, HID_QUIRK_NO_INIT_REPORTS | HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_CREATIVELABS, USB_DEVICE_ID_CREATIVE_SB_OMNI_SURROUND_51, HID_QUIRK_NOGET },
+	{ USB_VENDOR_ID_DELL, USB_DEVICE_ID_DELL_PIXART_USB_OPTICAL_MOUSE, HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_DMI, USB_DEVICE_ID_DMI_ENC, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_DRAGONRISE, USB_DEVICE_ID_DRAGONRISE_WIIU, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_DRAGONRISE, USB_DEVICE_ID_DRAGONRISE_PS3, HID_QUIRK_MULTI_INPUT },
diff --git a/drivers/hsi/clients/nokia-modem.c b/drivers/hsi/clients/nokia-modem.c
index c000780d931f..f78cca98266f 100644
--- a/drivers/hsi/clients/nokia-modem.c
+++ b/drivers/hsi/clients/nokia-modem.c
@@ -102,12 +102,10 @@ static int nokia_modem_gpio_probe(struct device *dev)
 		return -EINVAL;
 	}
 
-	modem->gpios = devm_kzalloc(dev, gpio_count *
-				sizeof(struct nokia_modem_gpio), GFP_KERNEL);
-	if (!modem->gpios) {
-		dev_err(dev, "Could not allocate memory for gpios\n");
+	modem->gpios = devm_kcalloc(dev, gpio_count, sizeof(*modem->gpios),
+				    GFP_KERNEL);
+	if (!modem->gpios)
 		return -ENOMEM;
-	}
 
 	modem->gpio_amount = gpio_count;
 
@@ -156,10 +154,9 @@ static int nokia_modem_probe(struct device *dev)
 	}
 
 	modem = devm_kzalloc(dev, sizeof(*modem), GFP_KERNEL);
-	if (!modem) {
-		dev_err(dev, "Could not allocate memory for nokia_modem_device\n");
+	if (!modem)
 		return -ENOMEM;
-	}
+
 	dev_set_drvdata(dev, modem);
 	modem->device = dev;
 
@@ -182,7 +179,7 @@ static int nokia_modem_probe(struct device *dev)
 	}
 	enable_irq_wake(irq);
 
-	if(pm) {
+	if (pm) {
 		err = nokia_modem_gpio_probe(dev);
 		if (err < 0) {
 			dev_err(dev, "Could not probe GPIOs\n");
diff --git a/drivers/hsi/clients/ssi_protocol.c b/drivers/hsi/clients/ssi_protocol.c
index 26b05106f0d3..93d28c0ec8bf 100644
--- a/drivers/hsi/clients/ssi_protocol.c
+++ b/drivers/hsi/clients/ssi_protocol.c
@@ -1066,7 +1066,7 @@ static void ssip_pn_setup(struct net_device *dev)
 	dev->addr_len		= 1;
 	dev->tx_queue_len	= SSIP_TXQUEUE_LEN;
 
-	dev->destructor		= free_netdev;
+	dev->needs_free_netdev	= true;
 	dev->header_ops		= &phonet_header_ops;
 }
 
diff --git a/drivers/hsi/controllers/omap_ssi_core.c b/drivers/hsi/controllers/omap_ssi_core.c
index 9a29b34ed2c8..88e48b346916 100644
--- a/drivers/hsi/controllers/omap_ssi_core.c
+++ b/drivers/hsi/controllers/omap_ssi_core.c
@@ -384,10 +384,8 @@ static int ssi_add_controller(struct hsi_controller *ssi,
 	int err;
 
 	omap_ssi = devm_kzalloc(&ssi->device, sizeof(*omap_ssi), GFP_KERNEL);
-	if (!omap_ssi) {
-		dev_err(&pd->dev, "not enough memory for omap ssi\n");
+	if (!omap_ssi)
 		return -ENOMEM;
-	}
 
 	err = ida_simple_get(&platform_omap_ssi_ida, 0, 0, GFP_KERNEL);
 	if (err < 0)
@@ -421,8 +419,8 @@ static int ssi_add_controller(struct hsi_controller *ssi,
 		goto out_err;
 	}
 
-	omap_ssi->port = devm_kzalloc(&ssi->device,
-		sizeof(struct omap_ssi_port *) * ssi->num_ports, GFP_KERNEL);
+	omap_ssi->port = devm_kcalloc(&ssi->device, ssi->num_ports,
+				      sizeof(*omap_ssi->port), GFP_KERNEL);
 	if (!omap_ssi->port) {
 		err = -ENOMEM;
 		goto out_err;
@@ -467,7 +465,7 @@ static int ssi_hw_init(struct hsi_controller *ssi)
 		dev_err(&ssi->device, "runtime PM failed %d\n", err);
 		return err;
 	}
-	/* Reseting GDD */
+	/* Resetting GDD */
 	writel_relaxed(SSI_SWRESET, omap_ssi->gdd + SSI_GDD_GRST_REG);
 	/* Get FCK rate in KHz */
 	omap_ssi->fck_rate = DIV_ROUND_CLOSEST(ssi_get_clk_rate(ssi), 1000);
diff --git a/drivers/hsi/hsi_boardinfo.c b/drivers/hsi/hsi_boardinfo.c
index e56bc6da5f98..e381cb4c962e 100644
--- a/drivers/hsi/hsi_boardinfo.c
+++ b/drivers/hsi/hsi_boardinfo.c
@@ -49,7 +49,7 @@ int __init hsi_register_board_info(struct hsi_board_info const *info,
 {
 	struct hsi_cl_info *cl_info;
 
-	cl_info = kzalloc(sizeof(*cl_info) * len, GFP_KERNEL);
+	cl_info = kcalloc(len, sizeof(*cl_info), GFP_KERNEL);
 	if (!cl_info)
 		return -ENOMEM;
 
diff --git a/drivers/hsi/hsi_core.c b/drivers/hsi/hsi_core.c
index c2a2a9795b0b..9065efd21851 100644
--- a/drivers/hsi/hsi_core.c
+++ b/drivers/hsi/hsi_core.c
@@ -267,14 +267,13 @@ static void hsi_add_client_from_dt(struct hsi_port *port,
 
 	cl->rx_cfg.num_channels = cells;
 	cl->tx_cfg.num_channels = cells;
-
-	cl->rx_cfg.channels = kzalloc(cells * sizeof(channel), GFP_KERNEL);
+	cl->rx_cfg.channels = kcalloc(cells, sizeof(channel), GFP_KERNEL);
 	if (!cl->rx_cfg.channels) {
 		err = -ENOMEM;
 		goto err;
 	}
 
-	cl->tx_cfg.channels = kzalloc(cells * sizeof(channel), GFP_KERNEL);
+	cl->tx_cfg.channels = kcalloc(cells, sizeof(channel), GFP_KERNEL);
 	if (!cl->tx_cfg.channels) {
 		err = -ENOMEM;
 		goto err2;
@@ -485,7 +484,7 @@ struct hsi_controller *hsi_alloc_controller(unsigned int n_ports, gfp_t flags)
 	hsi = kzalloc(sizeof(*hsi), flags);
 	if (!hsi)
 		return NULL;
-	port = kzalloc(sizeof(*port)*n_ports, flags);
+	port = kcalloc(n_ports, sizeof(*port), flags);
 	if (!port) {
 		kfree(hsi);
 		return NULL;
diff --git a/drivers/hv/channel.c b/drivers/hv/channel.c
index 736ac76d2a6a..e9bf0bb87ac4 100644
--- a/drivers/hv/channel.c
+++ b/drivers/hv/channel.c
@@ -630,9 +630,13 @@ void vmbus_close(struct vmbus_channel *channel)
 	 */
 	list_for_each_safe(cur, tmp, &channel->sc_list) {
 		cur_channel = list_entry(cur, struct vmbus_channel, sc_list);
-		if (cur_channel->state != CHANNEL_OPENED_STATE)
-			continue;
 		vmbus_close_internal(cur_channel);
+		if (cur_channel->rescind) {
+			mutex_lock(&vmbus_connection.channel_mutex);
+			hv_process_channel_removal(cur_channel,
+					   cur_channel->offermsg.child_relid);
+			mutex_unlock(&vmbus_connection.channel_mutex);
+		}
 	}
 	/*
 	 * Now close the primary.
diff --git a/drivers/hv/channel_mgmt.c b/drivers/hv/channel_mgmt.c
index 685572bae1f0..4bbb8dea4727 100644
--- a/drivers/hv/channel_mgmt.c
+++ b/drivers/hv/channel_mgmt.c
@@ -427,7 +427,6 @@ void vmbus_free_channels(void)
 {
 	struct vmbus_channel *channel, *tmp;
 
-	mutex_lock(&vmbus_connection.channel_mutex);
 	list_for_each_entry_safe(channel, tmp, &vmbus_connection.chn_list,
 		listentry) {
 		/* hv_process_channel_removal() needs this */
@@ -435,7 +434,6 @@ void vmbus_free_channels(void)
 
 		vmbus_device_unregister(channel->device_obj);
 	}
-	mutex_unlock(&vmbus_connection.channel_mutex);
 }
 
 /*
@@ -482,8 +480,10 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 			list_add_tail(&newchannel->sc_list, &channel->sc_list);
 			channel->num_sc++;
 			spin_unlock_irqrestore(&channel->lock, flags);
-		} else
+		} else {
+			atomic_dec(&vmbus_connection.offer_in_progress);
 			goto err_free_chan;
+		}
 	}
 
 	dev_type = hv_get_dev_type(newchannel);
@@ -510,6 +510,7 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 	if (!fnew) {
 		if (channel->sc_creation_callback != NULL)
 			channel->sc_creation_callback(newchannel);
+		atomic_dec(&vmbus_connection.offer_in_progress);
 		return;
 	}
 
@@ -531,9 +532,7 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 	 * binding which eventually invokes the device driver's AddDevice()
 	 * method.
 	 */
-	mutex_lock(&vmbus_connection.channel_mutex);
 	ret = vmbus_device_register(newchannel->device_obj);
-	mutex_unlock(&vmbus_connection.channel_mutex);
 
 	if (ret != 0) {
 		pr_err("unable to add child device object (relid %d)\n",
@@ -541,6 +540,8 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 		kfree(newchannel->device_obj);
 		goto err_deq_chan;
 	}
+
+	atomic_dec(&vmbus_connection.offer_in_progress);
 	return;
 
 err_deq_chan:
@@ -796,6 +797,7 @@ static void vmbus_onoffer(struct vmbus_channel_message_header *hdr)
 	newchannel = alloc_channel();
 	if (!newchannel) {
 		vmbus_release_relid(offer->child_relid);
+		atomic_dec(&vmbus_connection.offer_in_progress);
 		pr_err("Unable to allocate channel object\n");
 		return;
 	}
@@ -842,16 +844,38 @@ static void vmbus_onoffer_rescind(struct vmbus_channel_message_header *hdr)
 
 	rescind = (struct vmbus_channel_rescind_offer *)hdr;
 
+	/*
+	 * The offer msg and the corresponding rescind msg
+	 * from the host are guranteed to be ordered -
+	 * offer comes in first and then the rescind.
+	 * Since we process these events in work elements,
+	 * and with preemption, we may end up processing
+	 * the events out of order. Given that we handle these
+	 * work elements on the same CPU, this is possible only
+	 * in the case of preemption. In any case wait here
+	 * until the offer processing has moved beyond the
+	 * point where the channel is discoverable.
+	 */
+
+	while (atomic_read(&vmbus_connection.offer_in_progress) != 0) {
+		/*
+		 * We wait here until any channel offer is currently
+		 * being processed.
+		 */
+		msleep(1);
+	}
+
 	mutex_lock(&vmbus_connection.channel_mutex);
 	channel = relid2channel(rescind->child_relid);
+	mutex_unlock(&vmbus_connection.channel_mutex);
 
 	if (channel == NULL) {
 		/*
-		 * This is very impossible, because in
-		 * vmbus_process_offer(), we have already invoked
-		 * vmbus_release_relid() on error.
+		 * We failed in processing the offer message;
+		 * we would have cleaned up the relid in that
+		 * failure path.
 		 */
-		goto out;
+		return;
 	}
 
 	spin_lock_irqsave(&channel->lock, flags);
@@ -863,7 +887,7 @@ static void vmbus_onoffer_rescind(struct vmbus_channel_message_header *hdr)
 	if (channel->device_obj) {
 		if (channel->chn_rescind_callback) {
 			channel->chn_rescind_callback(channel);
-			goto out;
+			return;
 		}
 		/*
 		 * We will have to unregister this device from the
@@ -874,13 +898,26 @@ static void vmbus_onoffer_rescind(struct vmbus_channel_message_header *hdr)
 			vmbus_device_unregister(channel->device_obj);
 			put_device(dev);
 		}
-	} else {
-		hv_process_channel_removal(channel,
-			channel->offermsg.child_relid);
 	}
-
-out:
-	mutex_unlock(&vmbus_connection.channel_mutex);
+	if (channel->primary_channel != NULL) {
+		/*
+		 * Sub-channel is being rescinded. Following is the channel
+		 * close sequence when initiated from the driveri (refer to
+		 * vmbus_close() for details):
+		 * 1. Close all sub-channels first
+		 * 2. Then close the primary channel.
+		 */
+		if (channel->state == CHANNEL_OPEN_STATE) {
+			/*
+			 * The channel is currently not open;
+			 * it is safe for us to cleanup the channel.
+			 */
+			mutex_lock(&vmbus_connection.channel_mutex);
+			hv_process_channel_removal(channel,
+						channel->offermsg.child_relid);
+			mutex_unlock(&vmbus_connection.channel_mutex);
+		}
+	}
 }
 
 void vmbus_hvsock_device_unregister(struct vmbus_channel *channel)
diff --git a/drivers/hv/connection.c b/drivers/hv/connection.c
index fce27fb141cc..59c11ff90d12 100644
--- a/drivers/hv/connection.c
+++ b/drivers/hv/connection.c
@@ -93,10 +93,13 @@ static int vmbus_negotiate_version(struct vmbus_channel_msginfo *msginfo,
 	 * all the CPUs. This is needed for kexec to work correctly where
 	 * the CPU attempting to connect may not be CPU 0.
 	 */
-	if (version >= VERSION_WIN8_1)
+	if (version >= VERSION_WIN8_1) {
 		msg->target_vcpu = hv_context.vp_index[smp_processor_id()];
-	else
+		vmbus_connection.connect_cpu = smp_processor_id();
+	} else {
 		msg->target_vcpu = 0;
+		vmbus_connection.connect_cpu = 0;
+	}
 
 	/*
 	 * Add to list before we send the request since we may
@@ -370,7 +373,7 @@ int vmbus_post_msg(void *buffer, size_t buflen, bool can_sleep)
 			break;
 		case HV_STATUS_INSUFFICIENT_MEMORY:
 		case HV_STATUS_INSUFFICIENT_BUFFERS:
-			ret = -ENOMEM;
+			ret = -ENOBUFS;
 			break;
 		case HV_STATUS_SUCCESS:
 			return ret;
@@ -387,7 +390,7 @@ int vmbus_post_msg(void *buffer, size_t buflen, bool can_sleep)
 		else
 			mdelay(usec / 1000);
 
-		if (usec < 256000)
+		if (retries < 22)
 			usec *= 2;
 	}
 	return ret;
diff --git a/drivers/hv/hv.c b/drivers/hv/hv.c
index 12e7baecb84e..2ea12207caa0 100644
--- a/drivers/hv/hv.c
+++ b/drivers/hv/hv.c
@@ -82,10 +82,15 @@ int hv_post_message(union hv_connection_id connection_id,
 	aligned_msg->message_type = message_type;
 	aligned_msg->payload_size = payload_size;
 	memcpy((void *)aligned_msg->payload, payload, payload_size);
-	put_cpu_ptr(hv_cpu);
 
 	status = hv_do_hypercall(HVCALL_POST_MESSAGE, aligned_msg, NULL);
 
+	/* Preemption must remain disabled until after the hypercall
+	 * so some other thread can't get scheduled onto this cpu and
+	 * corrupt the per-cpu post_msg_page
+	 */
+	put_cpu_ptr(hv_cpu);
+
 	return status & 0xFFFF;
 }
 
@@ -96,7 +101,7 @@ static int hv_ce_set_next_event(unsigned long delta,
 
 	WARN_ON(!clockevent_state_oneshot(evt));
 
-	hv_get_current_tick(current_tick);
+	current_tick = hyperv_cs->read(NULL);
 	current_tick += delta;
 	hv_init_timer(HV_X64_MSR_STIMER0_COUNT, current_tick);
 	return 0;
diff --git a/drivers/hv/hv_kvp.c b/drivers/hv/hv_kvp.c
index e99ff2ddad40..9a90b915b5be 100644
--- a/drivers/hv/hv_kvp.c
+++ b/drivers/hv/hv_kvp.c
@@ -112,7 +112,7 @@ static void kvp_poll_wrapper(void *channel)
 {
 	/* Transaction is finished, reset the state here to avoid races. */
 	kvp_transaction.state = HVUTIL_READY;
-	hv_kvp_onchannelcallback(channel);
+	tasklet_schedule(&((struct vmbus_channel *)channel)->callback_event);
 }
 
 static void kvp_register_done(void)
@@ -159,7 +159,7 @@ static void kvp_timeout_func(struct work_struct *dummy)
 
 static void kvp_host_handshake_func(struct work_struct *dummy)
 {
-	hv_poll_channel(kvp_transaction.recv_channel, hv_kvp_onchannelcallback);
+	tasklet_schedule(&kvp_transaction.recv_channel->callback_event);
 }
 
 static int kvp_handle_handshake(struct hv_kvp_msg *msg)
@@ -625,16 +625,17 @@ void hv_kvp_onchannelcallback(void *context)
 		     NEGO_IN_PROGRESS,
 		     NEGO_FINISHED} host_negotiatied = NEGO_NOT_STARTED;
 
-	if (host_negotiatied == NEGO_NOT_STARTED &&
-	    kvp_transaction.state < HVUTIL_READY) {
+	if (kvp_transaction.state < HVUTIL_READY) {
 		/*
 		 * If userspace daemon is not connected and host is asking
 		 * us to negotiate we need to delay to not lose messages.
 		 * This is important for Failover IP setting.
 		 */
-		host_negotiatied = NEGO_IN_PROGRESS;
-		schedule_delayed_work(&kvp_host_handshake_work,
+		if (host_negotiatied == NEGO_NOT_STARTED) {
+			host_negotiatied = NEGO_IN_PROGRESS;
+			schedule_delayed_work(&kvp_host_handshake_work,
 				      HV_UTIL_NEGO_TIMEOUT * HZ);
+		}
 		return;
 	}
 	if (kvp_transaction.state > HVUTIL_READY)
@@ -702,6 +703,7 @@ void hv_kvp_onchannelcallback(void *context)
 				       VM_PKT_DATA_INBAND, 0);
 
 		host_negotiatied = NEGO_FINISHED;
+		hv_poll_channel(kvp_transaction.recv_channel, kvp_poll_wrapper);
 	}
 
 }
diff --git a/drivers/hv/hv_util.c b/drivers/hv/hv_util.c
index 186b10083c55..14dce25c104f 100644
--- a/drivers/hv/hv_util.c
+++ b/drivers/hv/hv_util.c
@@ -202,27 +202,39 @@ static void shutdown_onchannelcallback(void *context)
 /*
  * Set the host time in a process context.
  */
+static struct work_struct adj_time_work;
 
-struct adj_time_work {
-	struct work_struct work;
-	u64	host_time;
-	u64	ref_time;
-	u8	flags;
-};
+/*
+ * The last time sample, received from the host. PTP device responds to
+ * requests by using this data and the current partition-wide time reference
+ * count.
+ */
+static struct {
+	u64				host_time;
+	u64				ref_time;
+	spinlock_t			lock;
+} host_ts;
 
-static void hv_set_host_time(struct work_struct *work)
+static struct timespec64 hv_get_adj_host_time(void)
 {
-	struct adj_time_work *wrk;
-	struct timespec64 host_ts;
-	u64 reftime, newtime;
-
-	wrk = container_of(work, struct adj_time_work, work);
+	struct timespec64 ts;
+	u64 newtime, reftime;
+	unsigned long flags;
 
+	spin_lock_irqsave(&host_ts.lock, flags);
 	reftime = hyperv_cs->read(hyperv_cs);
-	newtime = wrk->host_time + (reftime - wrk->ref_time);
-	host_ts = ns_to_timespec64((newtime - WLTIMEDELTA) * 100);
+	newtime = host_ts.host_time + (reftime - host_ts.ref_time);
+	ts = ns_to_timespec64((newtime - WLTIMEDELTA) * 100);
+	spin_unlock_irqrestore(&host_ts.lock, flags);
 
-	do_settimeofday64(&host_ts);
+	return ts;
+}
+
+static void hv_set_host_time(struct work_struct *work)
+{
+	struct timespec64 ts = hv_get_adj_host_time();
+
+	do_settimeofday64(&ts);
 }
 
 /*
@@ -238,62 +250,35 @@ static void hv_set_host_time(struct work_struct *work)
  * typically used as a hint to the guest. The guest is under no obligation
  * to discipline the clock.
  */
-static struct adj_time_work  wrk;
-
-/*
- * The last time sample, received from the host. PTP device responds to
- * requests by using this data and the current partition-wide time reference
- * count.
- */
-static struct {
-	u64				host_time;
-	u64				ref_time;
-	struct system_time_snapshot	snap;
-	spinlock_t			lock;
-} host_ts;
-
 static inline void adj_guesttime(u64 hosttime, u64 reftime, u8 adj_flags)
 {
 	unsigned long flags;
 	u64 cur_reftime;
 
 	/*
-	 * This check is safe since we are executing in the
-	 * interrupt context and time synch messages are always
-	 * delivered on the same CPU.
+	 * Save the adjusted time sample from the host and the snapshot
+	 * of the current system time.
 	 */
-	if (adj_flags & ICTIMESYNCFLAG_SYNC) {
-		/* Queue a job to do do_settimeofday64() */
-		if (work_pending(&wrk.work))
-			return;
-
-		wrk.host_time = hosttime;
-		wrk.ref_time = reftime;
-		wrk.flags = adj_flags;
-		schedule_work(&wrk.work);
-	} else {
-		/*
-		 * Save the adjusted time sample from the host and the snapshot
-		 * of the current system time for PTP device.
-		 */
-		spin_lock_irqsave(&host_ts.lock, flags);
-
-		cur_reftime = hyperv_cs->read(hyperv_cs);
-		host_ts.host_time = hosttime;
-		host_ts.ref_time = cur_reftime;
-		ktime_get_snapshot(&host_ts.snap);
-
-		/*
-		 * TimeSync v4 messages contain reference time (guest's Hyper-V
-		 * clocksource read when the time sample was generated), we can
-		 * improve the precision by adding the delta between now and the
-		 * time of generation.
-		 */
-		if (ts_srv_version > TS_VERSION_3)
-			host_ts.host_time += (cur_reftime - reftime);
-
-		spin_unlock_irqrestore(&host_ts.lock, flags);
-	}
+	spin_lock_irqsave(&host_ts.lock, flags);
+
+	cur_reftime = hyperv_cs->read(hyperv_cs);
+	host_ts.host_time = hosttime;
+	host_ts.ref_time = cur_reftime;
+
+	/*
+	 * TimeSync v4 messages contain reference time (guest's Hyper-V
+	 * clocksource read when the time sample was generated), we can
+	 * improve the precision by adding the delta between now and the
+	 * time of generation. For older protocols we set
+	 * reftime == cur_reftime on call.
+	 */
+	host_ts.host_time += (cur_reftime - reftime);
+
+	spin_unlock_irqrestore(&host_ts.lock, flags);
+
+	/* Schedule work to do do_settimeofday64() */
+	if (adj_flags & ICTIMESYNCFLAG_SYNC)
+		schedule_work(&adj_time_work);
 }
 
 /*
@@ -341,8 +326,8 @@ static void timesync_onchannelcallback(void *context)
 					sizeof(struct vmbuspipe_hdr) +
 					sizeof(struct icmsg_hdr)];
 				adj_guesttime(timedatap->parenttime,
-						0,
-						timedatap->flags);
+					      hyperv_cs->read(hyperv_cs),
+					      timedatap->flags);
 			}
 		}
 
@@ -526,58 +511,17 @@ static int hv_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 
 static int hv_ptp_gettime(struct ptp_clock_info *info, struct timespec64 *ts)
 {
-	unsigned long flags;
-	u64 newtime, reftime;
-
-	spin_lock_irqsave(&host_ts.lock, flags);
-	reftime = hyperv_cs->read(hyperv_cs);
-	newtime = host_ts.host_time + (reftime - host_ts.ref_time);
-	*ts = ns_to_timespec64((newtime - WLTIMEDELTA) * 100);
-	spin_unlock_irqrestore(&host_ts.lock, flags);
+	*ts = hv_get_adj_host_time();
 
 	return 0;
 }
 
-static int hv_ptp_get_syncdevicetime(ktime_t *device,
-				     struct system_counterval_t *system,
-				     void *ctx)
-{
-	system->cs = hyperv_cs;
-	system->cycles = host_ts.ref_time;
-	*device = ns_to_ktime((host_ts.host_time - WLTIMEDELTA) * 100);
-
-	return 0;
-}
-
-static int hv_ptp_getcrosststamp(struct ptp_clock_info *ptp,
-				 struct system_device_crosststamp *xtstamp)
-{
-	unsigned long flags;
-	int ret;
-
-	spin_lock_irqsave(&host_ts.lock, flags);
-
-	/*
-	 * host_ts contains the last time sample from the host and the snapshot
-	 * of system time. We don't need to calculate the time delta between
-	 * the reception and now as get_device_system_crosststamp() does the
-	 * required interpolation.
-	 */
-	ret = get_device_system_crosststamp(hv_ptp_get_syncdevicetime,
-					    NULL, &host_ts.snap, xtstamp);
-
-	spin_unlock_irqrestore(&host_ts.lock, flags);
-
-	return ret;
-}
-
 static struct ptp_clock_info ptp_hyperv_info = {
 	.name		= "hyperv",
 	.enable         = hv_ptp_enable,
 	.adjtime        = hv_ptp_adjtime,
 	.adjfreq        = hv_ptp_adjfreq,
 	.gettime64      = hv_ptp_gettime,
-	.getcrosststamp = hv_ptp_getcrosststamp,
 	.settime64      = hv_ptp_settime,
 	.owner		= THIS_MODULE,
 };
@@ -592,7 +536,7 @@ static int hv_timesync_init(struct hv_util_service *srv)
 
 	spin_lock_init(&host_ts.lock);
 
-	INIT_WORK(&wrk.work, hv_set_host_time);
+	INIT_WORK(&adj_time_work, hv_set_host_time);
 
 	/*
 	 * ptp_clock_register() returns NULL when CONFIG_PTP_1588_CLOCK is
@@ -613,7 +557,7 @@ static void hv_timesync_deinit(void)
 {
 	if (hv_ptp_clock)
 		ptp_clock_unregister(hv_ptp_clock);
-	cancel_work_sync(&wrk.work);
+	cancel_work_sync(&adj_time_work);
 }
 
 static int __init init_hyperv_utils(void)
diff --git a/drivers/hv/hyperv_vmbus.h b/drivers/hv/hyperv_vmbus.h
index 6113e915c50e..1b6a5e0dfa75 100644
--- a/drivers/hv/hyperv_vmbus.h
+++ b/drivers/hv/hyperv_vmbus.h
@@ -303,6 +303,13 @@ enum vmbus_connect_state {
 #define MAX_SIZE_CHANNEL_MESSAGE	HV_MESSAGE_PAYLOAD_BYTE_COUNT
 
 struct vmbus_connection {
+	/*
+	 * CPU on which the initial host contact was made.
+	 */
+	int connect_cpu;
+
+	atomic_t offer_in_progress;
+
 	enum vmbus_connect_state conn_state;
 
 	atomic_t next_gpadl_handle;
@@ -411,6 +418,10 @@ static inline void hv_poll_channel(struct vmbus_channel *channel,
 	if (!channel)
 		return;
 
+	if (in_interrupt() && (channel->target_cpu == smp_processor_id())) {
+		cb(channel);
+		return;
+	}
 	smp_call_function_single(channel->target_cpu, cb, channel, true);
 }
 
diff --git a/drivers/hv/vmbus_drv.c b/drivers/hv/vmbus_drv.c
index 0087b49095eb..ed84e96715a0 100644
--- a/drivers/hv/vmbus_drv.c
+++ b/drivers/hv/vmbus_drv.c
@@ -608,40 +608,6 @@ static void vmbus_free_dynids(struct hv_driver *drv)
 	spin_unlock(&drv->dynids.lock);
 }
 
-/* Parse string of form: 1b4e28ba-2fa1-11d2-883f-b9a761bde3f */
-static int get_uuid_le(const char *str, uuid_le *uu)
-{
-	unsigned int b[16];
-	int i;
-
-	if (strlen(str) < 37)
-		return -1;
-
-	for (i = 0; i < 36; i++) {
-		switch (i) {
-		case 8: case 13: case 18: case 23:
-			if (str[i] != '-')
-				return -1;
-			break;
-		default:
-			if (!isxdigit(str[i]))
-				return -1;
-		}
-	}
-
-	/* unparse little endian output byte order */
-	if (sscanf(str,
-		   "%2x%2x%2x%2x-%2x%2x-%2x%2x-%2x%2x-%2x%2x%2x%2x%2x%2x",
-		   &b[3], &b[2], &b[1], &b[0],
-		   &b[5], &b[4], &b[7], &b[6], &b[8], &b[9],
-		   &b[10], &b[11], &b[12], &b[13], &b[14], &b[15]) != 16)
-		return -1;
-
-	for (i = 0; i < 16; i++)
-		uu->b[i] = b[i];
-	return 0;
-}
-
 /*
  * store_new_id - sysfs frontend to vmbus_add_dynid()
  *
@@ -651,11 +617,12 @@ static ssize_t new_id_store(struct device_driver *driver, const char *buf,
 			    size_t count)
 {
 	struct hv_driver *drv = drv_to_hv_drv(driver);
-	uuid_le guid = NULL_UUID_LE;
+	uuid_le guid;
 	ssize_t retval;
 
-	if (get_uuid_le(buf, &guid) != 0)
-		return -EINVAL;
+	retval = uuid_le_to_bin(buf, &guid);
+	if (retval)
+		return retval;
 
 	if (hv_vmbus_get_id(drv, &guid))
 		return -EEXIST;
@@ -677,12 +644,14 @@ static ssize_t remove_id_store(struct device_driver *driver, const char *buf,
 {
 	struct hv_driver *drv = drv_to_hv_drv(driver);
 	struct vmbus_dynid *dynid, *n;
-	uuid_le guid = NULL_UUID_LE;
-	size_t retval = -ENODEV;
+	uuid_le guid;
+	ssize_t retval;
 
-	if (get_uuid_le(buf, &guid))
-		return -EINVAL;
+	retval = uuid_le_to_bin(buf, &guid);
+	if (retval)
+		return retval;
 
+	retval = -ENODEV;
 	spin_lock(&drv->dynids.lock);
 	list_for_each_entry_safe(dynid, n, &drv->dynids.list, node) {
 		struct hv_vmbus_device_id *id = &dynid->id;
@@ -798,8 +767,10 @@ static void vmbus_device_release(struct device *device)
 	struct hv_device *hv_dev = device_to_hv_device(device);
 	struct vmbus_channel *channel = hv_dev->channel;
 
+	mutex_lock(&vmbus_connection.channel_mutex);
 	hv_process_channel_removal(channel,
 				   channel->offermsg.child_relid);
+	mutex_unlock(&vmbus_connection.channel_mutex);
 	kfree(hv_dev);
 
 }
@@ -877,7 +848,32 @@ void vmbus_on_msg_dpc(unsigned long data)
 		INIT_WORK(&ctx->work, vmbus_onmessage_work);
 		memcpy(&ctx->msg, msg, sizeof(*msg));
 
-		queue_work(vmbus_connection.work_queue, &ctx->work);
+		/*
+		 * The host can generate a rescind message while we
+		 * may still be handling the original offer. We deal with
+		 * this condition by ensuring the processing is done on the
+		 * same CPU.
+		 */
+		switch (hdr->msgtype) {
+		case CHANNELMSG_RESCIND_CHANNELOFFER:
+			/*
+			 * If we are handling the rescind message;
+			 * schedule the work on the global work queue.
+			 */
+			schedule_work_on(vmbus_connection.connect_cpu,
+					 &ctx->work);
+			break;
+
+		case CHANNELMSG_OFFERCHANNEL:
+			atomic_inc(&vmbus_connection.offer_in_progress);
+			queue_work_on(vmbus_connection.connect_cpu,
+				      vmbus_connection.work_queue,
+				      &ctx->work);
+			break;
+
+		default:
+			queue_work(vmbus_connection.work_queue, &ctx->work);
+		}
 	} else
 		entry->message_handler(hdr);
 
diff --git a/drivers/hwmon/ads1015.c b/drivers/hwmon/ads1015.c
index 5140c27d16dd..357b42607164 100644
--- a/drivers/hwmon/ads1015.c
+++ b/drivers/hwmon/ads1015.c
@@ -34,7 +34,7 @@
 #include <linux/of_device.h>
 #include <linux/of.h>
 
-#include <linux/i2c/ads1015.h>
+#include <linux/platform_data/ads1015.h>
 
 /* ADS1015 registers */
 enum {
diff --git a/drivers/hwmon/adt7475.c b/drivers/hwmon/adt7475.c
index c803e3c5fcd4..1baa213a60bd 100644
--- a/drivers/hwmon/adt7475.c
+++ b/drivers/hwmon/adt7475.c
@@ -22,6 +22,7 @@
 #include <linux/hwmon-vid.h>
 #include <linux/err.h>
 #include <linux/jiffies.h>
+#include <linux/util_macros.h>
 
 /* Indexes for the sysfs hooks */
 
@@ -78,6 +79,9 @@
 
 #define REG_TEMP_TRANGE_BASE	0x5F
 
+#define REG_ENHANCE_ACOUSTICS1	0x62
+#define REG_ENHANCE_ACOUSTICS2	0x63
+
 #define REG_PWM_MIN_BASE	0x64
 
 #define REG_TEMP_TMIN_BASE	0x67
@@ -208,6 +212,7 @@ struct adt7475_data {
 	u8 range[3];
 	u8 pwmctl[3];
 	u8 pwmchan[3];
+	u8 enh_acoustics[2];
 
 	u8 vid;
 	u8 vrm;
@@ -314,35 +319,6 @@ static void adt7475_write_word(struct i2c_client *client, int reg, u16 val)
 	i2c_smbus_write_byte_data(client, reg, val & 0xFF);
 }
 
-/*
- * Find the nearest value in a table - used for pwm frequency and
- * auto temp range
- */
-static int find_nearest(long val, const int *array, int size)
-{
-	int i;
-
-	if (val < array[0])
-		return 0;
-
-	if (val > array[size - 1])
-		return size - 1;
-
-	for (i = 0; i < size - 1; i++) {
-		int a, b;
-
-		if (val > array[i + 1])
-			continue;
-
-		a = val - array[i];
-		b = array[i + 1] - val;
-
-		return (a <= b) ? i : i + 1;
-	}
-
-	return 0;
-}
-
 static ssize_t show_voltage(struct device *dev, struct device_attribute *attr,
 			    char *buf)
 {
@@ -550,6 +526,88 @@ static ssize_t set_temp(struct device *dev, struct device_attribute *attr,
 	return count;
 }
 
+/* Assuming CONFIG6[SLOW] is 0 */
+static const int ad7475_st_map[] = {
+	37500, 18800, 12500, 7500, 4700, 3100, 1600, 800,
+};
+
+static ssize_t show_temp_st(struct device *dev, struct device_attribute *attr,
+				  char *buf)
+{
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7475_data *data = i2c_get_clientdata(client);
+	long val;
+
+	switch (sattr->index) {
+	case 0:
+		val = data->enh_acoustics[0] & 0xf;
+		break;
+	case 1:
+		val = (data->enh_acoustics[1] >> 4) & 0xf;
+		break;
+	case 2:
+	default:
+		val = data->enh_acoustics[1] & 0xf;
+		break;
+	}
+
+	if (val & 0x8)
+		return sprintf(buf, "%d\n", ad7475_st_map[val & 0x7]);
+	else
+		return sprintf(buf, "0\n");
+}
+
+static ssize_t set_temp_st(struct device *dev, struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7475_data *data = i2c_get_clientdata(client);
+	unsigned char reg;
+	int shift, idx;
+	ulong val;
+
+	if (kstrtoul(buf, 10, &val))
+		return -EINVAL;
+
+	switch (sattr->index) {
+	case 0:
+		reg = REG_ENHANCE_ACOUSTICS1;
+		shift = 0;
+		idx = 0;
+		break;
+	case 1:
+		reg = REG_ENHANCE_ACOUSTICS2;
+		shift = 0;
+		idx = 1;
+		break;
+	case 2:
+	default:
+		reg = REG_ENHANCE_ACOUSTICS2;
+		shift = 4;
+		idx = 1;
+		break;
+	}
+
+	if (val > 0) {
+		val = find_closest_descending(val, ad7475_st_map,
+					      ARRAY_SIZE(ad7475_st_map));
+		val |= 0x8;
+	}
+
+	mutex_lock(&data->lock);
+
+	data->enh_acoustics[idx] &= ~(0xf << shift);
+	data->enh_acoustics[idx] |= (val << shift);
+
+	i2c_smbus_write_byte_data(client, reg, data->enh_acoustics[idx]);
+
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
 /*
  * Table of autorange values - the user will write the value in millidegrees,
  * and we'll convert it
@@ -606,7 +664,7 @@ static ssize_t set_point2(struct device *dev, struct device_attribute *attr,
 	val -= temp;
 
 	/* Find the nearest table entry to what the user wrote */
-	val = find_nearest(val, autorange_table, ARRAY_SIZE(autorange_table));
+	val = find_closest(val, autorange_table, ARRAY_SIZE(autorange_table));
 
 	data->range[sattr->index] &= ~0xF0;
 	data->range[sattr->index] |= val << 4;
@@ -728,6 +786,43 @@ static ssize_t set_pwm(struct device *dev, struct device_attribute *attr,
 	data->pwm[sattr->nr][sattr->index] = clamp_val(val, 0, 0xFF);
 	i2c_smbus_write_byte_data(client, reg,
 				  data->pwm[sattr->nr][sattr->index]);
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t show_stall_disable(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7475_data *data = i2c_get_clientdata(client);
+	u8 mask = BIT(5 + sattr->index);
+
+	return sprintf(buf, "%d\n", !!(data->enh_acoustics[0] & mask));
+}
+
+static ssize_t set_stall_disable(struct device *dev,
+				 struct device_attribute *attr, const char *buf,
+				 size_t count)
+{
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7475_data *data = i2c_get_clientdata(client);
+	long val;
+	u8 mask = BIT(5 + sattr->index);
+
+	if (kstrtol(buf, 10, &val))
+		return -EINVAL;
+
+	mutex_lock(&data->lock);
+
+	data->enh_acoustics[0] &= ~mask;
+	if (val)
+		data->enh_acoustics[0] |= mask;
+
+	i2c_smbus_write_byte_data(client, REG_ENHANCE_ACOUSTICS1,
+				  data->enh_acoustics[0]);
 
 	mutex_unlock(&data->lock);
 
@@ -839,7 +934,7 @@ static ssize_t set_pwmctrl(struct device *dev, struct device_attribute *attr,
 
 /* List of frequencies for the PWM */
 static const int pwmfreq_table[] = {
-	11, 14, 22, 29, 35, 44, 58, 88
+	11, 14, 22, 29, 35, 44, 58, 88, 22500
 };
 
 static ssize_t show_pwmfreq(struct device *dev, struct device_attribute *attr,
@@ -847,9 +942,10 @@ static ssize_t show_pwmfreq(struct device *dev, struct device_attribute *attr,
 {
 	struct adt7475_data *data = adt7475_update_device(dev);
 	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int i = clamp_val(data->range[sattr->index] & 0xf, 0,
+			  ARRAY_SIZE(pwmfreq_table) - 1);
 
-	return sprintf(buf, "%d\n",
-		       pwmfreq_table[data->range[sattr->index] & 7]);
+	return sprintf(buf, "%d\n", pwmfreq_table[i]);
 }
 
 static ssize_t set_pwmfreq(struct device *dev, struct device_attribute *attr,
@@ -864,13 +960,13 @@ static ssize_t set_pwmfreq(struct device *dev, struct device_attribute *attr,
 	if (kstrtol(buf, 10, &val))
 		return -EINVAL;
 
-	out = find_nearest(val, pwmfreq_table, ARRAY_SIZE(pwmfreq_table));
+	out = find_closest(val, pwmfreq_table, ARRAY_SIZE(pwmfreq_table));
 
 	mutex_lock(&data->lock);
 
 	data->range[sattr->index] =
 		adt7475_read(TEMP_TRANGE_REG(sattr->index));
-	data->range[sattr->index] &= ~7;
+	data->range[sattr->index] &= ~0xf;
 	data->range[sattr->index] |= out;
 
 	i2c_smbus_write_byte_data(client, TEMP_TRANGE_REG(sattr->index),
@@ -995,6 +1091,8 @@ static SENSOR_DEVICE_ATTR_2(temp1_crit, S_IRUGO | S_IWUSR, show_temp, set_temp,
 			    THERM, 0);
 static SENSOR_DEVICE_ATTR_2(temp1_crit_hyst, S_IRUGO | S_IWUSR, show_temp,
 			    set_temp, HYSTERSIS, 0);
+static SENSOR_DEVICE_ATTR_2(temp1_smoothing, S_IRUGO | S_IWUSR, show_temp_st,
+			    set_temp_st, 0, 0);
 static SENSOR_DEVICE_ATTR_2(temp2_input, S_IRUGO, show_temp, NULL, INPUT, 1);
 static SENSOR_DEVICE_ATTR_2(temp2_alarm, S_IRUGO, show_temp, NULL, ALARM, 1);
 static SENSOR_DEVICE_ATTR_2(temp2_max, S_IRUGO | S_IWUSR, show_temp, set_temp,
@@ -1011,6 +1109,8 @@ static SENSOR_DEVICE_ATTR_2(temp2_crit, S_IRUGO | S_IWUSR, show_temp, set_temp,
 			    THERM, 1);
 static SENSOR_DEVICE_ATTR_2(temp2_crit_hyst, S_IRUGO | S_IWUSR, show_temp,
 			    set_temp, HYSTERSIS, 1);
+static SENSOR_DEVICE_ATTR_2(temp2_smoothing, S_IRUGO | S_IWUSR, show_temp_st,
+			    set_temp_st, 0, 1);
 static SENSOR_DEVICE_ATTR_2(temp3_input, S_IRUGO, show_temp, NULL, INPUT, 2);
 static SENSOR_DEVICE_ATTR_2(temp3_alarm, S_IRUGO, show_temp, NULL, ALARM, 2);
 static SENSOR_DEVICE_ATTR_2(temp3_fault, S_IRUGO, show_temp, NULL, FAULT, 2);
@@ -1028,6 +1128,8 @@ static SENSOR_DEVICE_ATTR_2(temp3_crit, S_IRUGO | S_IWUSR, show_temp, set_temp,
 			    THERM, 2);
 static SENSOR_DEVICE_ATTR_2(temp3_crit_hyst, S_IRUGO | S_IWUSR, show_temp,
 			    set_temp, HYSTERSIS, 2);
+static SENSOR_DEVICE_ATTR_2(temp3_smoothing, S_IRUGO | S_IWUSR, show_temp_st,
+			    set_temp_st, 0, 2);
 static SENSOR_DEVICE_ATTR_2(fan1_input, S_IRUGO, show_tach, NULL, INPUT, 0);
 static SENSOR_DEVICE_ATTR_2(fan1_min, S_IRUGO | S_IWUSR, show_tach, set_tach,
 			    MIN, 0);
@@ -1056,6 +1158,8 @@ static SENSOR_DEVICE_ATTR_2(pwm1_auto_point1_pwm, S_IRUGO | S_IWUSR, show_pwm,
 			    set_pwm, MIN, 0);
 static SENSOR_DEVICE_ATTR_2(pwm1_auto_point2_pwm, S_IRUGO | S_IWUSR, show_pwm,
 			    set_pwm, MAX, 0);
+static SENSOR_DEVICE_ATTR_2(pwm1_stall_disable, S_IRUGO | S_IWUSR,
+			    show_stall_disable, set_stall_disable, 0, 0);
 static SENSOR_DEVICE_ATTR_2(pwm2, S_IRUGO | S_IWUSR, show_pwm, set_pwm, INPUT,
 			    1);
 static SENSOR_DEVICE_ATTR_2(pwm2_freq, S_IRUGO | S_IWUSR, show_pwmfreq,
@@ -1068,6 +1172,8 @@ static SENSOR_DEVICE_ATTR_2(pwm2_auto_point1_pwm, S_IRUGO | S_IWUSR, show_pwm,
 			    set_pwm, MIN, 1);
 static SENSOR_DEVICE_ATTR_2(pwm2_auto_point2_pwm, S_IRUGO | S_IWUSR, show_pwm,
 			    set_pwm, MAX, 1);
+static SENSOR_DEVICE_ATTR_2(pwm2_stall_disable, S_IRUGO | S_IWUSR,
+			    show_stall_disable, set_stall_disable, 0, 1);
 static SENSOR_DEVICE_ATTR_2(pwm3, S_IRUGO | S_IWUSR, show_pwm, set_pwm, INPUT,
 			    2);
 static SENSOR_DEVICE_ATTR_2(pwm3_freq, S_IRUGO | S_IWUSR, show_pwmfreq,
@@ -1080,6 +1186,8 @@ static SENSOR_DEVICE_ATTR_2(pwm3_auto_point1_pwm, S_IRUGO | S_IWUSR, show_pwm,
 			    set_pwm, MIN, 2);
 static SENSOR_DEVICE_ATTR_2(pwm3_auto_point2_pwm, S_IRUGO | S_IWUSR, show_pwm,
 			    set_pwm, MAX, 2);
+static SENSOR_DEVICE_ATTR_2(pwm3_stall_disable, S_IRUGO | S_IWUSR,
+			    show_stall_disable, set_stall_disable, 0, 2);
 
 /* Non-standard name, might need revisiting */
 static DEVICE_ATTR_RW(pwm_use_point2_pwm_at_crit);
@@ -1106,6 +1214,7 @@ static struct attribute *adt7475_attrs[] = {
 	&sensor_dev_attr_temp1_auto_point2_temp.dev_attr.attr,
 	&sensor_dev_attr_temp1_crit.dev_attr.attr,
 	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_smoothing.dev_attr.attr,
 	&sensor_dev_attr_temp2_input.dev_attr.attr,
 	&sensor_dev_attr_temp2_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp2_max.dev_attr.attr,
@@ -1115,6 +1224,7 @@ static struct attribute *adt7475_attrs[] = {
 	&sensor_dev_attr_temp2_auto_point2_temp.dev_attr.attr,
 	&sensor_dev_attr_temp2_crit.dev_attr.attr,
 	&sensor_dev_attr_temp2_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp2_smoothing.dev_attr.attr,
 	&sensor_dev_attr_temp3_input.dev_attr.attr,
 	&sensor_dev_attr_temp3_fault.dev_attr.attr,
 	&sensor_dev_attr_temp3_alarm.dev_attr.attr,
@@ -1125,6 +1235,7 @@ static struct attribute *adt7475_attrs[] = {
 	&sensor_dev_attr_temp3_auto_point2_temp.dev_attr.attr,
 	&sensor_dev_attr_temp3_crit.dev_attr.attr,
 	&sensor_dev_attr_temp3_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp3_smoothing.dev_attr.attr,
 	&sensor_dev_attr_fan1_input.dev_attr.attr,
 	&sensor_dev_attr_fan1_min.dev_attr.attr,
 	&sensor_dev_attr_fan1_alarm.dev_attr.attr,
@@ -1140,12 +1251,14 @@ static struct attribute *adt7475_attrs[] = {
 	&sensor_dev_attr_pwm1_auto_channels_temp.dev_attr.attr,
 	&sensor_dev_attr_pwm1_auto_point1_pwm.dev_attr.attr,
 	&sensor_dev_attr_pwm1_auto_point2_pwm.dev_attr.attr,
+	&sensor_dev_attr_pwm1_stall_disable.dev_attr.attr,
 	&sensor_dev_attr_pwm3.dev_attr.attr,
 	&sensor_dev_attr_pwm3_freq.dev_attr.attr,
 	&sensor_dev_attr_pwm3_enable.dev_attr.attr,
 	&sensor_dev_attr_pwm3_auto_channels_temp.dev_attr.attr,
 	&sensor_dev_attr_pwm3_auto_point1_pwm.dev_attr.attr,
 	&sensor_dev_attr_pwm3_auto_point2_pwm.dev_attr.attr,
+	&sensor_dev_attr_pwm3_stall_disable.dev_attr.attr,
 	&dev_attr_pwm_use_point2_pwm_at_crit.attr,
 	NULL,
 };
@@ -1164,6 +1277,7 @@ static struct attribute *pwm2_attrs[] = {
 	&sensor_dev_attr_pwm2_auto_channels_temp.dev_attr.attr,
 	&sensor_dev_attr_pwm2_auto_point1_pwm.dev_attr.attr,
 	&sensor_dev_attr_pwm2_auto_point2_pwm.dev_attr.attr,
+	&sensor_dev_attr_pwm2_stall_disable.dev_attr.attr,
 	NULL
 };
 
diff --git a/drivers/hwmon/aspeed-pwm-tacho.c b/drivers/hwmon/aspeed-pwm-tacho.c
index 9de13d626c68..ddfe66bdff86 100644
--- a/drivers/hwmon/aspeed-pwm-tacho.c
+++ b/drivers/hwmon/aspeed-pwm-tacho.c
@@ -146,14 +146,26 @@
 
 #define PWM_MAX 255
 
+#define BOTH_EDGES 0x02 /* 10b */
+
 #define M_PWM_DIV_H 0x00
 #define M_PWM_DIV_L 0x05
 #define M_PWM_PERIOD 0x5F
 #define M_TACH_CLK_DIV 0x00
-#define M_TACH_MODE 0x00
-#define M_TACH_UNIT 0x1000
+/*
+ * 5:4 Type N fan tach mode selection bit:
+ * 00: falling
+ * 01: rising
+ * 10: both
+ * 11: reserved.
+ */
+#define M_TACH_MODE 0x02 /* 10b */
+#define M_TACH_UNIT 0x00c0
 #define INIT_FAN_CTRL 0xFF
 
+/* How long we sleep in us while waiting for an RPM result. */
+#define ASPEED_RPM_STATUS_SLEEP_USEC	500
+
 struct aspeed_pwm_tacho_data {
 	struct regmap *regmap;
 	unsigned long clk_freq;
@@ -163,6 +175,7 @@ struct aspeed_pwm_tacho_data {
 	u8 type_pwm_clock_division_h[3];
 	u8 type_pwm_clock_division_l[3];
 	u8 type_fan_tach_clock_division[3];
+	u8 type_fan_tach_mode[3];
 	u16 type_fan_tach_unit[3];
 	u8 pwm_port_type[8];
 	u8 pwm_port_fan_ctrl[8];
@@ -498,8 +511,9 @@ static u32 aspeed_get_fan_tach_ch_measure_period(struct aspeed_pwm_tacho_data
 static int aspeed_get_fan_tach_ch_rpm(struct aspeed_pwm_tacho_data *priv,
 				      u8 fan_tach_ch)
 {
-	u32 raw_data, tach_div, clk_source, sec, val;
-	u8 fan_tach_ch_source, type;
+	u32 raw_data, tach_div, clk_source, msec, usec, val;
+	u8 fan_tach_ch_source, type, mode, both;
+	int ret;
 
 	regmap_write(priv->regmap, ASPEED_PTCR_TRIGGER, 0);
 	regmap_write(priv->regmap, ASPEED_PTCR_TRIGGER, 0x1 << fan_tach_ch);
@@ -507,16 +521,31 @@ static int aspeed_get_fan_tach_ch_rpm(struct aspeed_pwm_tacho_data *priv,
 	fan_tach_ch_source = priv->fan_tach_ch_source[fan_tach_ch];
 	type = priv->pwm_port_type[fan_tach_ch_source];
 
-	sec = (1000 / aspeed_get_fan_tach_ch_measure_period(priv, type));
-	msleep(sec);
+	msec = (1000 / aspeed_get_fan_tach_ch_measure_period(priv, type));
+	usec = msec * 1000;
+
+	ret = regmap_read_poll_timeout(
+		priv->regmap,
+		ASPEED_PTCR_RESULT,
+		val,
+		(val & RESULT_STATUS_MASK),
+		ASPEED_RPM_STATUS_SLEEP_USEC,
+		usec);
 
-	regmap_read(priv->regmap, ASPEED_PTCR_RESULT, &val);
-	if (!(val & RESULT_STATUS_MASK))
-		return -ETIMEDOUT;
+	/* return -ETIMEDOUT if we didn't get an answer. */
+	if (ret)
+		return ret;
 
 	raw_data = val & RESULT_VALUE_MASK;
 	tach_div = priv->type_fan_tach_clock_division[type];
-	tach_div = 0x4 << (tach_div * 2);
+	/*
+	 * We need the mode to determine if the raw_data is double (from
+	 * counting both edges).
+	 */
+	mode = priv->type_fan_tach_mode[type];
+	both = (mode & BOTH_EDGES) ? 1 : 0;
+
+	tach_div = (0x4 << both) << (tach_div * 2);
 	clk_source = priv->clk_freq;
 
 	if (raw_data == 0)
@@ -702,6 +731,7 @@ static void aspeed_create_type(struct aspeed_pwm_tacho_data *priv)
 	aspeed_set_tacho_type_enable(priv->regmap, TYPEM, true);
 	priv->type_fan_tach_clock_division[TYPEM] = M_TACH_CLK_DIV;
 	priv->type_fan_tach_unit[TYPEM] = M_TACH_UNIT;
+	priv->type_fan_tach_mode[TYPEM] = M_TACH_MODE;
 	aspeed_set_tacho_type_values(priv->regmap, TYPEM, M_TACH_MODE,
 				     M_TACH_UNIT, M_TACH_CLK_DIV);
 }
diff --git a/drivers/hwmon/ds620.c b/drivers/hwmon/ds620.c
index 0043a4c02b85..57d6958c74b8 100644
--- a/drivers/hwmon/ds620.c
+++ b/drivers/hwmon/ds620.c
@@ -30,7 +30,7 @@
 #include <linux/err.h>
 #include <linux/mutex.h>
 #include <linux/sysfs.h>
-#include <linux/i2c/ds620.h>
+#include <linux/platform_data/ds620.h>
 
 /*
  * Many DS620 constants specified below
diff --git a/drivers/hwmon/ibmpowernv.c b/drivers/hwmon/ibmpowernv.c
index 6d2e6605751c..5ccdd0b52650 100644
--- a/drivers/hwmon/ibmpowernv.c
+++ b/drivers/hwmon/ibmpowernv.c
@@ -50,22 +50,34 @@ enum sensors {
 	TEMP,
 	POWER_SUPPLY,
 	POWER_INPUT,
+	CURRENT,
 	MAX_SENSOR_TYPE,
 };
 
 #define INVALID_INDEX (-1U)
 
+/*
+ * 'compatible' string properties for sensor types as defined in old
+ * PowerNV firmware (skiboot). These are ordered as 'enum sensors'.
+ */
+static const char * const legacy_compatibles[] = {
+	"ibm,opal-sensor-cooling-fan",
+	"ibm,opal-sensor-amb-temp",
+	"ibm,opal-sensor-power-supply",
+	"ibm,opal-sensor-power"
+};
+
 static struct sensor_group {
-	const char *name;
-	const char *compatible;
+	const char *name; /* matches property 'sensor-type' */
 	struct attribute_group group;
 	u32 attr_count;
 	u32 hwmon_index;
 } sensor_groups[] = {
-	{"fan", "ibm,opal-sensor-cooling-fan"},
-	{"temp", "ibm,opal-sensor-amb-temp"},
-	{"in", "ibm,opal-sensor-power-supply"},
-	{"power", "ibm,opal-sensor-power"}
+	{ "fan"   },
+	{ "temp"  },
+	{ "in"    },
+	{ "power" },
+	{ "curr"  },
 };
 
 struct sensor_data {
@@ -239,8 +251,8 @@ static int get_sensor_type(struct device_node *np)
 	enum sensors type;
 	const char *str;
 
-	for (type = 0; type < MAX_SENSOR_TYPE; type++) {
-		if (of_device_is_compatible(np, sensor_groups[type].compatible))
+	for (type = 0; type < ARRAY_SIZE(legacy_compatibles); type++) {
+		if (of_device_is_compatible(np, legacy_compatibles[type]))
 			return type;
 	}
 
@@ -298,10 +310,14 @@ static int populate_attr_groups(struct platform_device *pdev)
 		sensor_groups[type].attr_count++;
 
 		/*
-		 * add a new attribute for labels
+		 * add attributes for labels, min and max
 		 */
 		if (!of_property_read_string(np, "label", &label))
 			sensor_groups[type].attr_count++;
+		if (of_find_property(np, "sensor-data-min", NULL))
+			sensor_groups[type].attr_count++;
+		if (of_find_property(np, "sensor-data-max", NULL))
+			sensor_groups[type].attr_count++;
 	}
 
 	of_node_put(opal);
@@ -337,6 +353,41 @@ static void create_hwmon_attr(struct sensor_data *sdata, const char *attr_name,
 	sdata->dev_attr.show = show;
 }
 
+static void populate_sensor(struct sensor_data *sdata, int od, int hd, int sid,
+			    const char *attr_name, enum sensors type,
+			    const struct attribute_group *pgroup,
+			    ssize_t (*show)(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf))
+{
+	sdata->id = sid;
+	sdata->type = type;
+	sdata->opal_index = od;
+	sdata->hwmon_index = hd;
+	create_hwmon_attr(sdata, attr_name, show);
+	pgroup->attrs[sensor_groups[type].attr_count++] = &sdata->dev_attr.attr;
+}
+
+static char *get_max_attr(enum sensors type)
+{
+	switch (type) {
+	case POWER_INPUT:
+		return "input_highest";
+	default:
+		return "highest";
+	}
+}
+
+static char *get_min_attr(enum sensors type)
+{
+	switch (type) {
+	case POWER_INPUT:
+		return "input_lowest";
+	default:
+		return "lowest";
+	}
+}
+
 /*
  * Iterate through the device tree for each child of 'sensors' node, create
  * a sysfs attribute file, the file is named by translating the DT node name
@@ -417,16 +468,31 @@ static int create_device_attrs(struct platform_device *pdev)
 			 * attribute. They are related to the same
 			 * sensor.
 			 */
-			sdata[count].type = type;
-			sdata[count].opal_index = sdata[count - 1].opal_index;
-			sdata[count].hwmon_index = sdata[count - 1].hwmon_index;
 
 			make_sensor_label(np, &sdata[count], label);
+			populate_sensor(&sdata[count], opal_index,
+					sdata[count - 1].hwmon_index,
+					sensor_id, "label", type, pgroups[type],
+					show_label);
+			count++;
+		}
 
-			create_hwmon_attr(&sdata[count], "label", show_label);
+		if (!of_property_read_u32(np, "sensor-data-max", &sensor_id)) {
+			attr_name = get_max_attr(type);
+			populate_sensor(&sdata[count], opal_index,
+					sdata[count - 1].hwmon_index,
+					sensor_id, attr_name, type,
+					pgroups[type], show_sensor);
+			count++;
+		}
 
-			pgroups[type]->attrs[sensor_groups[type].attr_count++] =
-				&sdata[count++].dev_attr.attr;
+		if (!of_property_read_u32(np, "sensor-data-min", &sensor_id)) {
+			attr_name = get_min_attr(type);
+			populate_sensor(&sdata[count], opal_index,
+					sdata[count - 1].hwmon_index,
+					sensor_id, attr_name, type,
+					pgroups[type], show_sensor);
+			count++;
 		}
 	}
 
diff --git a/drivers/hwmon/ltc4245.c b/drivers/hwmon/ltc4245.c
index 4680d89556ce..082f0a0bd8a0 100644
--- a/drivers/hwmon/ltc4245.c
+++ b/drivers/hwmon/ltc4245.c
@@ -23,7 +23,7 @@
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
 #include <linux/jiffies.h>
-#include <linux/i2c/ltc4245.h>
+#include <linux/platform_data/ltc4245.h>
 
 /* Here are names of the chip's registers (a.k.a. commands) */
 enum ltc4245_cmd {
diff --git a/drivers/hwmon/max6639.c b/drivers/hwmon/max6639.c
index dac6d85f2fd9..f98a83c79ff1 100644
--- a/drivers/hwmon/max6639.c
+++ b/drivers/hwmon/max6639.c
@@ -32,7 +32,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
-#include <linux/i2c/max6639.h>
+#include <linux/platform_data/max6639.h>
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = { 0x2c, 0x2e, 0x2f, I2C_CLIENT_END };
diff --git a/drivers/hwmon/nct6775.c b/drivers/hwmon/nct6775.c
index 2458b406f6aa..c219e43b8f02 100644
--- a/drivers/hwmon/nct6775.c
+++ b/drivers/hwmon/nct6775.c
@@ -40,6 +40,8 @@
  * nct6791d    15      6       6       2+6    0xc800 0xc1    0x5ca3
  * nct6792d    15      6       6       2+6    0xc910 0xc1    0x5ca3
  * nct6793d    15      6       6       2+6    0xd120 0xc1    0x5ca3
+ * nct6795d    14      6       6       2+6    0xd350 0xc1    0x5ca3
+ *
  *
  * #temp lists the number of monitored temperature sources (first value) plus
  * the number of directly connectable temperature sensors (second value).
@@ -58,13 +60,15 @@
 #include <linux/err.h>
 #include <linux/mutex.h>
 #include <linux/acpi.h>
+#include <linux/bitops.h>
 #include <linux/dmi.h>
 #include <linux/io.h>
 #include "lm75.h"
 
 #define USE_ALTERNATE
 
-enum kinds { nct6106, nct6775, nct6776, nct6779, nct6791, nct6792, nct6793 };
+enum kinds { nct6106, nct6775, nct6776, nct6779, nct6791, nct6792, nct6793,
+	     nct6795 };
 
 /* used to set data->name = nct6775_device_names[data->sio_kind] */
 static const char * const nct6775_device_names[] = {
@@ -75,6 +79,7 @@ static const char * const nct6775_device_names[] = {
 	"nct6791",
 	"nct6792",
 	"nct6793",
+	"nct6795",
 };
 
 static const char * const nct6775_sio_names[] __initconst = {
@@ -85,6 +90,7 @@ static const char * const nct6775_sio_names[] __initconst = {
 	"NCT6791D",
 	"NCT6792D",
 	"NCT6793D",
+	"NCT6795D",
 };
 
 static unsigned short force_id;
@@ -104,6 +110,7 @@ MODULE_PARM_DESC(fan_debounce, "Enable debouncing for fan RPM signal");
 #define NCT6775_LD_ACPI		0x0a
 #define NCT6775_LD_HWM		0x0b
 #define NCT6775_LD_VID		0x0d
+#define NCT6775_LD_12		0x12
 
 #define SIO_REG_LDSEL		0x07	/* Logical device select */
 #define SIO_REG_DEVID		0x20	/* Device ID (2 bytes) */
@@ -117,6 +124,7 @@ MODULE_PARM_DESC(fan_debounce, "Enable debouncing for fan RPM signal");
 #define SIO_NCT6791_ID		0xc800
 #define SIO_NCT6792_ID		0xc910
 #define SIO_NCT6793_ID		0xd120
+#define SIO_NCT6795_ID		0xd350
 #define SIO_ID_MASK		0xFFF0
 
 enum pwm_enable { off, manual, thermal_cruise, speed_cruise, sf3, sf4 };
@@ -360,12 +368,24 @@ static const char *const nct6775_temp_label[] = {
 	"PCH_DIM3_TEMP"
 };
 
-static const u16 NCT6775_REG_TEMP_ALTERNATE[ARRAY_SIZE(nct6775_temp_label) - 1]
-	= { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x661, 0x662, 0x664 };
+#define NCT6775_TEMP_MASK	0x001ffffe
+
+static const u16 NCT6775_REG_TEMP_ALTERNATE[32] = {
+	[13] = 0x661,
+	[14] = 0x662,
+	[15] = 0x664,
+};
 
-static const u16 NCT6775_REG_TEMP_CRIT[ARRAY_SIZE(nct6775_temp_label) - 1]
-	= { 0, 0, 0, 0, 0xa00, 0xa01, 0xa02, 0xa03, 0xa04, 0xa05, 0xa06,
-	    0xa07 };
+static const u16 NCT6775_REG_TEMP_CRIT[32] = {
+	[4] = 0xa00,
+	[5] = 0xa01,
+	[6] = 0xa02,
+	[7] = 0xa03,
+	[8] = 0xa04,
+	[9] = 0xa05,
+	[10] = 0xa06,
+	[11] = 0xa07
+};
 
 /* NCT6776 specific data */
 
@@ -434,11 +454,18 @@ static const char *const nct6776_temp_label[] = {
 	"BYTE_TEMP"
 };
 
-static const u16 NCT6776_REG_TEMP_ALTERNATE[ARRAY_SIZE(nct6776_temp_label) - 1]
-	= { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x401, 0x402, 0x404 };
+#define NCT6776_TEMP_MASK	0x007ffffe
 
-static const u16 NCT6776_REG_TEMP_CRIT[ARRAY_SIZE(nct6776_temp_label) - 1]
-	= { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x709, 0x70a };
+static const u16 NCT6776_REG_TEMP_ALTERNATE[32] = {
+	[14] = 0x401,
+	[15] = 0x402,
+	[16] = 0x404,
+};
+
+static const u16 NCT6776_REG_TEMP_CRIT[32] = {
+	[11] = 0x709,
+	[12] = 0x70a,
+};
 
 /* NCT6779 specific data */
 
@@ -525,17 +552,19 @@ static const char *const nct6779_temp_label[] = {
 	"Virtual_TEMP"
 };
 
-#define NCT6779_NUM_LABELS	(ARRAY_SIZE(nct6779_temp_label) - 5)
-#define NCT6791_NUM_LABELS	ARRAY_SIZE(nct6779_temp_label)
+#define NCT6779_TEMP_MASK	0x07ffff7e
+#define NCT6791_TEMP_MASK	0x87ffff7e
 
-static const u16 NCT6779_REG_TEMP_ALTERNATE[NCT6791_NUM_LABELS - 1]
+static const u16 NCT6779_REG_TEMP_ALTERNATE[32]
 	= { 0x490, 0x491, 0x492, 0x493, 0x494, 0x495, 0, 0,
 	    0, 0, 0, 0, 0, 0, 0, 0,
 	    0, 0x400, 0x401, 0x402, 0x404, 0x405, 0x406, 0x407,
 	    0x408, 0 };
 
-static const u16 NCT6779_REG_TEMP_CRIT[NCT6791_NUM_LABELS - 1]
-	= { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x709, 0x70a };
+static const u16 NCT6779_REG_TEMP_CRIT[32] = {
+	[15] = 0x709,
+	[16] = 0x70a,
+};
 
 /* NCT6791 specific data */
 
@@ -602,6 +631,8 @@ static const char *const nct6792_temp_label[] = {
 	"Virtual_TEMP"
 };
 
+#define NCT6792_TEMP_MASK	0x9fffff7e
+
 static const char *const nct6793_temp_label[] = {
 	"",
 	"SYSTIN",
@@ -637,6 +668,45 @@ static const char *const nct6793_temp_label[] = {
 	"Virtual_TEMP"
 };
 
+#define NCT6793_TEMP_MASK	0xbfff037e
+
+static const char *const nct6795_temp_label[] = {
+	"",
+	"SYSTIN",
+	"CPUTIN",
+	"AUXTIN0",
+	"AUXTIN1",
+	"AUXTIN2",
+	"AUXTIN3",
+	"",
+	"SMBUSMASTER 0",
+	"SMBUSMASTER 1",
+	"SMBUSMASTER 2",
+	"SMBUSMASTER 3",
+	"SMBUSMASTER 4",
+	"SMBUSMASTER 5",
+	"SMBUSMASTER 6",
+	"SMBUSMASTER 7",
+	"PECI Agent 0",
+	"PECI Agent 1",
+	"PCH_CHIP_CPU_MAX_TEMP",
+	"PCH_CHIP_TEMP",
+	"PCH_CPU_TEMP",
+	"PCH_MCH_TEMP",
+	"PCH_DIM0_TEMP",
+	"PCH_DIM1_TEMP",
+	"PCH_DIM2_TEMP",
+	"PCH_DIM3_TEMP",
+	"BYTE_TEMP0",
+	"BYTE_TEMP1",
+	"PECI Agent 0 Calibration",
+	"PECI Agent 1 Calibration",
+	"",
+	"Virtual_TEMP"
+};
+
+#define NCT6795_TEMP_MASK	0xbfffff7e
+
 /* NCT6102D/NCT6106D specific data */
 
 #define NCT6106_REG_VBAT	0x318
@@ -731,11 +801,16 @@ static const s8 NCT6106_BEEP_BITS[] = {
 	34, -1				/* intrusion0, intrusion1 */
 };
 
-static const u16 NCT6106_REG_TEMP_ALTERNATE[ARRAY_SIZE(nct6776_temp_label) - 1]
-	= { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x51, 0x52, 0x54 };
+static const u16 NCT6106_REG_TEMP_ALTERNATE[32] = {
+	[14] = 0x51,
+	[15] = 0x52,
+	[16] = 0x54,
+};
 
-static const u16 NCT6106_REG_TEMP_CRIT[ARRAY_SIZE(nct6776_temp_label) - 1]
-	= { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x204, 0x205 };
+static const u16 NCT6106_REG_TEMP_CRIT[32] = {
+	[11] = 0x204,
+	[12] = 0x205,
+};
 
 static enum pwm_enable reg_to_pwm_enable(int pwm, int mode)
 {
@@ -810,7 +885,7 @@ static u16 fan_to_reg(u32 fan, unsigned int divreg)
 static inline unsigned int
 div_from_reg(u8 reg)
 {
-	return 1 << reg;
+	return BIT(reg);
 }
 
 /*
@@ -850,7 +925,7 @@ struct nct6775_data {
 	u8 temp_src[NUM_TEMP];
 	u16 reg_temp_config[NUM_TEMP];
 	const char * const *temp_label;
-	int temp_label_num;
+	u32 temp_mask;
 
 	u16 REG_CONFIG;
 	u16 REG_VBAT;
@@ -1155,6 +1230,7 @@ static bool is_word_sized(struct nct6775_data *data, u16 reg)
 	case nct6791:
 	case nct6792:
 	case nct6793:
+	case nct6795:
 		return reg == 0x150 || reg == 0x153 || reg == 0x155 ||
 		  ((reg & 0xfff0) == 0x4b0 && (reg & 0x000f) < 0x0b) ||
 		  reg == 0x402 ||
@@ -1276,7 +1352,7 @@ static void nct6775_update_fan_div(struct nct6775_data *data)
 	data->fan_div[1] = (i & 0x70) >> 4;
 	i = nct6775_read_value(data, NCT6775_REG_FANDIV2);
 	data->fan_div[2] = i & 0x7;
-	if (data->has_fan & (1 << 3))
+	if (data->has_fan & BIT(3))
 		data->fan_div[3] = (i & 0x70) >> 4;
 }
 
@@ -1298,7 +1374,7 @@ static void nct6775_init_fan_div(struct nct6775_data *data)
 	 * We'll compute a better divider later on.
 	 */
 	for (i = 0; i < ARRAY_SIZE(data->fan_div); i++) {
-		if (!(data->has_fan & (1 << i)))
+		if (!(data->has_fan & BIT(i)))
 			continue;
 		if (data->fan_div[i] == 0) {
 			data->fan_div[i] = 7;
@@ -1321,7 +1397,7 @@ static void nct6775_init_fan_common(struct device *dev,
 	 * prevents the unnecessary warning when fanX_min is reported as 0.
 	 */
 	for (i = 0; i < ARRAY_SIZE(data->fan_min); i++) {
-		if (data->has_fan_min & (1 << i)) {
+		if (data->has_fan_min & BIT(i)) {
 			reg = nct6775_read_value(data, data->REG_FAN_MIN[i]);
 			if (!reg)
 				nct6775_write_value(data, data->REG_FAN_MIN[i],
@@ -1356,7 +1432,7 @@ static void nct6775_select_fan_div(struct device *dev,
 			div_from_reg(fan_div));
 
 		/* Preserve min limit if possible */
-		if (data->has_fan_min & (1 << nr)) {
+		if (data->has_fan_min & BIT(nr)) {
 			fan_min = data->fan_min[nr];
 			if (fan_div > data->fan_div[nr]) {
 				if (fan_min != 255 && fan_min > 1)
@@ -1387,7 +1463,7 @@ static void nct6775_update_pwm(struct device *dev)
 	bool duty_is_dc;
 
 	for (i = 0; i < data->pwm_num; i++) {
-		if (!(data->has_pwm & (1 << i)))
+		if (!(data->has_pwm & BIT(i)))
 			continue;
 
 		duty_is_dc = data->REG_PWM_MODE[i] &&
@@ -1457,7 +1533,7 @@ static void nct6775_update_pwm_limits(struct device *dev)
 	u16 reg_t;
 
 	for (i = 0; i < data->pwm_num; i++) {
-		if (!(data->has_pwm & (1 << i)))
+		if (!(data->has_pwm & BIT(i)))
 			continue;
 
 		for (j = 0; j < ARRAY_SIZE(data->fan_time); j++) {
@@ -1507,6 +1583,7 @@ static void nct6775_update_pwm_limits(struct device *dev)
 		case nct6791:
 		case nct6792:
 		case nct6793:
+		case nct6795:
 			reg = nct6775_read_value(data,
 					data->REG_CRITICAL_PWM_ENABLE[i]);
 			if (reg & data->CRITICAL_PWM_ENABLE_MASK)
@@ -1534,7 +1611,7 @@ static struct nct6775_data *nct6775_update_device(struct device *dev)
 
 		/* Measured voltages and limits */
 		for (i = 0; i < data->in_num; i++) {
-			if (!(data->have_in & (1 << i)))
+			if (!(data->have_in & BIT(i)))
 				continue;
 
 			data->in[i][0] = nct6775_read_value(data,
@@ -1549,14 +1626,14 @@ static struct nct6775_data *nct6775_update_device(struct device *dev)
 		for (i = 0; i < ARRAY_SIZE(data->rpm); i++) {
 			u16 reg;
 
-			if (!(data->has_fan & (1 << i)))
+			if (!(data->has_fan & BIT(i)))
 				continue;
 
 			reg = nct6775_read_value(data, data->REG_FAN[i]);
 			data->rpm[i] = data->fan_from_reg(reg,
 							  data->fan_div[i]);
 
-			if (data->has_fan_min & (1 << i))
+			if (data->has_fan_min & BIT(i))
 				data->fan_min[i] = nct6775_read_value(data,
 					   data->REG_FAN_MIN[i]);
 			data->fan_pulses[i] =
@@ -1571,7 +1648,7 @@ static struct nct6775_data *nct6775_update_device(struct device *dev)
 
 		/* Measured temperatures and limits */
 		for (i = 0; i < NUM_TEMP; i++) {
-			if (!(data->have_temp & (1 << i)))
+			if (!(data->have_temp & BIT(i)))
 				continue;
 			for (j = 0; j < ARRAY_SIZE(data->reg_temp); j++) {
 				if (data->reg_temp[j][i])
@@ -1580,7 +1657,7 @@ static struct nct6775_data *nct6775_update_device(struct device *dev)
 						data->reg_temp[j][i]);
 			}
 			if (i >= NUM_TEMP_FIXED ||
-			    !(data->have_temp_fixed & (1 << i)))
+			    !(data->have_temp_fixed & BIT(i)))
 				continue;
 			data->temp_offset[i]
 			  = nct6775_read_value(data, data->REG_TEMP_OFFSET[i]);
@@ -1801,7 +1878,7 @@ static umode_t nct6775_in_is_visible(struct kobject *kobj,
 	struct nct6775_data *data = dev_get_drvdata(dev);
 	int in = index / 5;	/* voltage index */
 
-	if (!(data->have_in & (1 << in)))
+	if (!(data->have_in & BIT(in)))
 		return 0;
 
 	return attr->mode;
@@ -1911,7 +1988,7 @@ store_fan_min(struct device *dev, struct device_attribute *attr,
 		 * even with the highest divider (128)
 		 */
 		data->fan_min[nr] = 254;
-		new_div = 7; /* 128 == (1 << 7) */
+		new_div = 7; /* 128 == BIT(7) */
 		dev_warn(dev,
 			 "fan%u low limit %lu below minimum %u, set to minimum\n",
 			 nr + 1, val, data->fan_from_reg_min(254, 7));
@@ -1921,7 +1998,7 @@ store_fan_min(struct device *dev, struct device_attribute *attr,
 		 * even with the lowest divider (1)
 		 */
 		data->fan_min[nr] = 1;
-		new_div = 0; /* 1 == (1 << 0) */
+		new_div = 0; /* 1 == BIT(0) */
 		dev_warn(dev,
 			 "fan%u low limit %lu above maximum %u, set to maximum\n",
 			 nr + 1, val, data->fan_from_reg_min(1, 0));
@@ -2008,14 +2085,14 @@ static umode_t nct6775_fan_is_visible(struct kobject *kobj,
 	int fan = index / 6;	/* fan index */
 	int nr = index % 6;	/* attribute index */
 
-	if (!(data->has_fan & (1 << fan)))
+	if (!(data->has_fan & BIT(fan)))
 		return 0;
 
 	if (nr == 1 && data->ALARM_BITS[FAN_ALARM_BASE + fan] == -1)
 		return 0;
 	if (nr == 2 && data->BEEP_BITS[FAN_ALARM_BASE + fan] == -1)
 		return 0;
-	if (nr == 4 && !(data->has_fan_min & (1 << fan)))
+	if (nr == 4 && !(data->has_fan_min & BIT(fan)))
 		return 0;
 	if (nr == 5 && data->kind != nct6775)
 		return 0;
@@ -2193,7 +2270,10 @@ static umode_t nct6775_temp_is_visible(struct kobject *kobj,
 	int temp = index / 10;	/* temp index */
 	int nr = index % 10;	/* attribute index */
 
-	if (!(data->have_temp & (1 << temp)))
+	if (!(data->have_temp & BIT(temp)))
+		return 0;
+
+	if (nr == 1 && !data->temp_label)
 		return 0;
 
 	if (nr == 2 && find_temp_source(data, temp, data->num_temp_alarms) < 0)
@@ -2215,7 +2295,7 @@ static umode_t nct6775_temp_is_visible(struct kobject *kobj,
 		return 0;
 
 	/* offset and type only apply to fixed sensors */
-	if (nr > 7 && !(data->have_temp_fixed & (1 << temp)))
+	if (nr > 7 && !(data->have_temp_fixed & BIT(temp)))
 		return 0;
 
 	return attr->mode;
@@ -2484,7 +2564,7 @@ show_pwm_temp_sel_common(struct nct6775_data *data, char *buf, int src)
 	int i, sel = 0;
 
 	for (i = 0; i < NUM_TEMP; i++) {
-		if (!(data->have_temp & (1 << i)))
+		if (!(data->have_temp & BIT(i)))
 			continue;
 		if (src == data->temp_src[i]) {
 			sel = i + 1;
@@ -2520,7 +2600,7 @@ store_pwm_temp_sel(struct device *dev, struct device_attribute *attr,
 		return err;
 	if (val == 0 || val > NUM_TEMP)
 		return -EINVAL;
-	if (!(data->have_temp & (1 << (val - 1))) || !data->temp_src[val - 1])
+	if (!(data->have_temp & BIT(val - 1)) || !data->temp_src[val - 1])
 		return -EINVAL;
 
 	mutex_lock(&data->update_lock);
@@ -2562,7 +2642,7 @@ store_pwm_weight_temp_sel(struct device *dev, struct device_attribute *attr,
 		return err;
 	if (val > NUM_TEMP)
 		return -EINVAL;
-	if (val && (!(data->have_temp & (1 << (val - 1))) ||
+	if (val && (!(data->have_temp & BIT(val - 1)) ||
 		    !data->temp_src[val - 1]))
 		return -EINVAL;
 
@@ -2923,6 +3003,7 @@ store_auto_pwm(struct device *dev, struct device_attribute *attr,
 		case nct6791:
 		case nct6792:
 		case nct6793:
+		case nct6795:
 			nct6775_write_value(data, data->REG_CRITICAL_PWM[nr],
 					    val);
 			reg = nct6775_read_value(data,
@@ -2995,7 +3076,7 @@ static umode_t nct6775_pwm_is_visible(struct kobject *kobj,
 	int pwm = index / 36;	/* pwm index */
 	int nr = index % 36;	/* attribute index */
 
-	if (!(data->has_pwm & (1 << pwm)))
+	if (!(data->has_pwm & BIT(pwm)))
 		return 0;
 
 	if ((nr >= 14 && nr <= 18) || nr == 21)   /* weight */
@@ -3246,7 +3327,7 @@ static inline void nct6775_init_device(struct nct6775_data *data)
 
 	/* Enable temperature sensors if needed */
 	for (i = 0; i < NUM_TEMP; i++) {
-		if (!(data->have_temp & (1 << i)))
+		if (!(data->have_temp & BIT(i)))
 			continue;
 		if (!data->reg_temp_config[i])
 			continue;
@@ -3264,7 +3345,7 @@ static inline void nct6775_init_device(struct nct6775_data *data)
 	diode = nct6775_read_value(data, data->REG_DIODE);
 
 	for (i = 0; i < data->temp_fixed_num; i++) {
-		if (!(data->have_temp_fixed & (1 << i)))
+		if (!(data->have_temp_fixed & BIT(i)))
 			continue;
 		if ((tmp & (data->DIODE_MASK << i)))	/* diode */
 			data->temp_type[i]
@@ -3290,8 +3371,8 @@ nct6775_check_fan_inputs(struct nct6775_data *data)
 	if (data->kind == nct6775) {
 		regval = superio_inb(sioreg, 0x2c);
 
-		fan3pin = regval & (1 << 6);
-		pwm3pin = regval & (1 << 7);
+		fan3pin = regval & BIT(6);
+		pwm3pin = regval & BIT(7);
 
 		/* On NCT6775, fan4 shares pins with the fdc interface */
 		fan4pin = !(superio_inb(sioreg, 0x2A) & 0x80);
@@ -3357,28 +3438,57 @@ nct6775_check_fan_inputs(struct nct6775_data *data)
 		pwm4pin = false;
 		pwm5pin = false;
 		pwm6pin = false;
-	} else {	/* NCT6779D, NCT6791D, NCT6792D, or NCT6793D */
-		regval = superio_inb(sioreg, 0x1c);
+	} else { /* NCT6779D, NCT6791D, NCT6792D, NCT6793D, or NCT6795D */
+		int regval_1b, regval_2a, regval_eb;
 
-		fan3pin = !(regval & (1 << 5));
-		fan4pin = !(regval & (1 << 6));
-		fan5pin = !(regval & (1 << 7));
+		regval = superio_inb(sioreg, 0x1c);
 
-		pwm3pin = !(regval & (1 << 0));
-		pwm4pin = !(regval & (1 << 1));
-		pwm5pin = !(regval & (1 << 2));
+		fan3pin = !(regval & BIT(5));
+		fan4pin = !(regval & BIT(6));
+		fan5pin = !(regval & BIT(7));
 
-		fan4min = fan4pin;
+		pwm3pin = !(regval & BIT(0));
+		pwm4pin = !(regval & BIT(1));
+		pwm5pin = !(regval & BIT(2));
 
-		if (data->kind == nct6791 || data->kind == nct6792 ||
-		    data->kind == nct6793) {
-			regval = superio_inb(sioreg, 0x2d);
-			fan6pin = (regval & (1 << 1));
-			pwm6pin = (regval & (1 << 0));
-		} else {	/* NCT6779D */
+		regval = superio_inb(sioreg, 0x2d);
+		switch (data->kind) {
+		case nct6791:
+		case nct6792:
+			fan6pin = regval & BIT(1);
+			pwm6pin = regval & BIT(0);
+			break;
+		case nct6793:
+		case nct6795:
+			regval_1b = superio_inb(sioreg, 0x1b);
+			regval_2a = superio_inb(sioreg, 0x2a);
+
+			if (!pwm5pin)
+				pwm5pin = regval & BIT(7);
+			fan6pin = regval & BIT(1);
+			pwm6pin = regval & BIT(0);
+			if (!fan5pin)
+				fan5pin = regval_1b & BIT(5);
+
+			superio_select(sioreg, NCT6775_LD_12);
+			regval_eb = superio_inb(sioreg, 0xeb);
+			if (!fan5pin)
+				fan5pin = regval_eb & BIT(5);
+			if (!pwm5pin)
+				pwm5pin = (regval_eb & BIT(4)) &&
+					   !(regval_2a & BIT(0));
+			if (!fan6pin)
+				fan6pin = regval_eb & BIT(3);
+			if (!pwm6pin)
+				pwm6pin = regval_eb & BIT(2);
+			break;
+		default:	/* NCT6779D */
 			fan6pin = false;
 			pwm6pin = false;
+			break;
 		}
+
+		fan4min = fan4pin;
 	}
 
 	/* fan 1 and 2 (0x03) are always present */
@@ -3403,16 +3513,15 @@ static void add_temp_sensors(struct nct6775_data *data, const u16 *regp,
 			continue;
 		src = nct6775_read_value(data, regp[i]);
 		src &= 0x1f;
-		if (!src || (*mask & (1 << src)))
+		if (!src || (*mask & BIT(src)))
 			continue;
-		if (src >= data->temp_label_num ||
-		    !strlen(data->temp_label[src]))
+		if (!(data->temp_mask & BIT(src)))
 			continue;
 
 		index = __ffs(*available);
 		nct6775_write_value(data, data->REG_TEMP_SOURCE[index], src);
-		*available &= ~(1 << index);
-		*mask |= 1 << src;
+		*available &= ~BIT(index);
+		*mask |= BIT(src);
 	}
 }
 
@@ -3464,7 +3573,7 @@ static int nct6775_probe(struct platform_device *pdev)
 		data->fan_from_reg_min = fan_from_reg13;
 
 		data->temp_label = nct6776_temp_label;
-		data->temp_label_num = ARRAY_SIZE(nct6776_temp_label);
+		data->temp_mask = NCT6776_TEMP_MASK;
 
 		data->REG_VBAT = NCT6106_REG_VBAT;
 		data->REG_DIODE = NCT6106_REG_DIODE;
@@ -3542,7 +3651,7 @@ static int nct6775_probe(struct platform_device *pdev)
 		data->speed_tolerance_limit = 15;
 
 		data->temp_label = nct6775_temp_label;
-		data->temp_label_num = ARRAY_SIZE(nct6775_temp_label);
+		data->temp_mask = NCT6775_TEMP_MASK;
 
 		data->REG_CONFIG = NCT6775_REG_CONFIG;
 		data->REG_VBAT = NCT6775_REG_VBAT;
@@ -3614,7 +3723,7 @@ static int nct6775_probe(struct platform_device *pdev)
 		data->speed_tolerance_limit = 63;
 
 		data->temp_label = nct6776_temp_label;
-		data->temp_label_num = ARRAY_SIZE(nct6776_temp_label);
+		data->temp_mask = NCT6776_TEMP_MASK;
 
 		data->REG_CONFIG = NCT6775_REG_CONFIG;
 		data->REG_VBAT = NCT6775_REG_VBAT;
@@ -3686,7 +3795,7 @@ static int nct6775_probe(struct platform_device *pdev)
 		data->speed_tolerance_limit = 63;
 
 		data->temp_label = nct6779_temp_label;
-		data->temp_label_num = NCT6779_NUM_LABELS;
+		data->temp_mask = NCT6779_TEMP_MASK;
 
 		data->REG_CONFIG = NCT6775_REG_CONFIG;
 		data->REG_VBAT = NCT6775_REG_VBAT;
@@ -3746,6 +3855,7 @@ static int nct6775_probe(struct platform_device *pdev)
 	case nct6791:
 	case nct6792:
 	case nct6793:
+	case nct6795:
 		data->in_num = 15;
 		data->pwm_num = 6;
 		data->auto_pwm_num = 4;
@@ -3767,15 +3877,21 @@ static int nct6775_probe(struct platform_device *pdev)
 		default:
 		case nct6791:
 			data->temp_label = nct6779_temp_label;
+			data->temp_mask = NCT6791_TEMP_MASK;
 			break;
 		case nct6792:
 			data->temp_label = nct6792_temp_label;
+			data->temp_mask = NCT6792_TEMP_MASK;
 			break;
 		case nct6793:
 			data->temp_label = nct6793_temp_label;
+			data->temp_mask = NCT6793_TEMP_MASK;
+			break;
+		case nct6795:
+			data->temp_label = nct6795_temp_label;
+			data->temp_mask = NCT6795_TEMP_MASK;
 			break;
 		}
-		data->temp_label_num = NCT6791_NUM_LABELS;
 
 		data->REG_CONFIG = NCT6775_REG_CONFIG;
 		data->REG_VBAT = NCT6775_REG_VBAT;
@@ -3843,7 +3959,7 @@ static int nct6775_probe(struct platform_device *pdev)
 	default:
 		return -ENODEV;
 	}
-	data->have_in = (1 << data->in_num) - 1;
+	data->have_in = BIT(data->in_num) - 1;
 	data->have_temp = 0;
 
 	/*
@@ -3861,10 +3977,10 @@ static int nct6775_probe(struct platform_device *pdev)
 			continue;
 
 		src = nct6775_read_value(data, data->REG_TEMP_SOURCE[i]) & 0x1f;
-		if (!src || (mask & (1 << src)))
-			available |= 1 << i;
+		if (!src || (mask & BIT(src)))
+			available |= BIT(i);
 
-		mask |= 1 << src;
+		mask |= BIT(src);
 	}
 
 	/*
@@ -3881,23 +3997,22 @@ static int nct6775_probe(struct platform_device *pdev)
 			continue;
 
 		src = nct6775_read_value(data, data->REG_TEMP_SOURCE[i]) & 0x1f;
-		if (!src || (mask & (1 << src)))
+		if (!src || (mask & BIT(src)))
 			continue;
 
-		if (src >= data->temp_label_num ||
-		    !strlen(data->temp_label[src])) {
+		if (!(data->temp_mask & BIT(src))) {
 			dev_info(dev,
 				 "Invalid temperature source %d at index %d, source register 0x%x, temp register 0x%x\n",
 				 src, i, data->REG_TEMP_SOURCE[i], reg_temp[i]);
 			continue;
 		}
 
-		mask |= 1 << src;
+		mask |= BIT(src);
 
 		/* Use fixed index for SYSTIN(1), CPUTIN(2), AUXTIN(3) */
 		if (src <= data->temp_fixed_num) {
-			data->have_temp |= 1 << (src - 1);
-			data->have_temp_fixed |= 1 << (src - 1);
+			data->have_temp |= BIT(src - 1);
+			data->have_temp_fixed |= BIT(src - 1);
 			data->reg_temp[0][src - 1] = reg_temp[i];
 			data->reg_temp[1][src - 1] = reg_temp_over[i];
 			data->reg_temp[2][src - 1] = reg_temp_hyst[i];
@@ -3917,7 +4032,7 @@ static int nct6775_probe(struct platform_device *pdev)
 			continue;
 
 		/* Use dynamic index for other sources */
-		data->have_temp |= 1 << s;
+		data->have_temp |= BIT(s);
 		data->reg_temp[0][s] = reg_temp[i];
 		data->reg_temp[1][s] = reg_temp_over[i];
 		data->reg_temp[2][s] = reg_temp_hyst[i];
@@ -3945,8 +4060,7 @@ static int nct6775_probe(struct platform_device *pdev)
 		if (!src)
 			continue;
 
-		if (src >= data->temp_label_num ||
-		    !strlen(data->temp_label[src])) {
+		if (!(data->temp_mask & BIT(src))) {
 			dev_info(dev,
 				 "Invalid temperature source %d at index %d, source register 0x%x, temp register 0x%x\n",
 				 src, i, data->REG_TEMP_SEL[i],
@@ -3960,17 +4074,17 @@ static int nct6775_probe(struct platform_device *pdev)
 		 * are no duplicates.
 		 */
 		if (src != TEMP_SOURCE_VIRTUAL) {
-			if (mask & (1 << src))
+			if (mask & BIT(src))
 				continue;
-			mask |= 1 << src;
+			mask |= BIT(src);
 		}
 
 		/* Use fixed index for SYSTIN(1), CPUTIN(2), AUXTIN(3) */
 		if (src <= data->temp_fixed_num) {
-			if (data->have_temp & (1 << (src - 1)))
+			if (data->have_temp & BIT(src - 1))
 				continue;
-			data->have_temp |= 1 << (src - 1);
-			data->have_temp_fixed |= 1 << (src - 1);
+			data->have_temp |= BIT(src - 1);
+			data->have_temp_fixed |= BIT(src - 1);
 			data->reg_temp[0][src - 1] = reg_temp_mon[i];
 			data->temp_src[src - 1] = src;
 			continue;
@@ -3980,7 +4094,7 @@ static int nct6775_probe(struct platform_device *pdev)
 			continue;
 
 		/* Use dynamic index for other sources */
-		data->have_temp |= 1 << s;
+		data->have_temp |= BIT(s);
 		data->reg_temp[0][s] = reg_temp_mon[i];
 		data->temp_src[s] = src;
 		s++;
@@ -3993,16 +4107,18 @@ static int nct6775_probe(struct platform_device *pdev)
 	 * The temperature is already monitored if the respective bit in <mask>
 	 * is set.
 	 */
-	for (i = 0; i < data->temp_label_num - 1; i++) {
+	for (i = 0; i < 32; i++) {
+		if (!(data->temp_mask & BIT(i + 1)))
+			continue;
 		if (!reg_temp_alternate[i])
 			continue;
-		if (mask & (1 << (i + 1)))
+		if (mask & BIT(i + 1))
 			continue;
 		if (i < data->temp_fixed_num) {
-			if (data->have_temp & (1 << i))
+			if (data->have_temp & BIT(i))
 				continue;
-			data->have_temp |= 1 << i;
-			data->have_temp_fixed |= 1 << i;
+			data->have_temp |= BIT(i);
+			data->have_temp_fixed |= BIT(i);
 			data->reg_temp[0][i] = reg_temp_alternate[i];
 			if (i < num_reg_temp) {
 				data->reg_temp[1][i] = reg_temp_over[i];
@@ -4015,7 +4131,7 @@ static int nct6775_probe(struct platform_device *pdev)
 		if (s >= NUM_TEMP)	/* Abort if no more space */
 			break;
 
-		data->have_temp |= 1 << s;
+		data->have_temp |= BIT(s);
 		data->reg_temp[0][s] = reg_temp_alternate[i];
 		data->temp_src[s] = i + 1;
 		s++;
@@ -4042,6 +4158,7 @@ static int nct6775_probe(struct platform_device *pdev)
 	case nct6791:
 	case nct6792:
 	case nct6793:
+	case nct6795:
 		break;
 	}
 
@@ -4075,6 +4192,7 @@ static int nct6775_probe(struct platform_device *pdev)
 		case nct6791:
 		case nct6792:
 		case nct6793:
+		case nct6795:
 			tmp |= 0x7e;
 			break;
 		}
@@ -4173,14 +4291,14 @@ static int __maybe_unused nct6775_resume(struct device *dev)
 		superio_outb(sioreg, SIO_REG_ENABLE, data->sio_reg_enable);
 
 	if (data->kind == nct6791 || data->kind == nct6792 ||
-	    data->kind == nct6793)
+	    data->kind == nct6793 || data->kind == nct6795)
 		nct6791_enable_io_mapping(sioreg);
 
 	superio_exit(sioreg);
 
 	/* Restore limits */
 	for (i = 0; i < data->in_num; i++) {
-		if (!(data->have_in & (1 << i)))
+		if (!(data->have_in & BIT(i)))
 			continue;
 
 		nct6775_write_value(data, data->REG_IN_MINMAX[0][i],
@@ -4190,7 +4308,7 @@ static int __maybe_unused nct6775_resume(struct device *dev)
 	}
 
 	for (i = 0; i < ARRAY_SIZE(data->fan_min); i++) {
-		if (!(data->has_fan_min & (1 << i)))
+		if (!(data->has_fan_min & BIT(i)))
 			continue;
 
 		nct6775_write_value(data, data->REG_FAN_MIN[i],
@@ -4198,7 +4316,7 @@ static int __maybe_unused nct6775_resume(struct device *dev)
 	}
 
 	for (i = 0; i < NUM_TEMP; i++) {
-		if (!(data->have_temp & (1 << i)))
+		if (!(data->have_temp & BIT(i)))
 			continue;
 
 		for (j = 1; j < ARRAY_SIZE(data->reg_temp); j++)
@@ -4270,6 +4388,9 @@ static int __init nct6775_find(int sioaddr, struct nct6775_sio_data *sio_data)
 	case SIO_NCT6793_ID:
 		sio_data->kind = nct6793;
 		break;
+	case SIO_NCT6795_ID:
+		sio_data->kind = nct6795;
+		break;
 	default:
 		if (val != 0xffff)
 			pr_debug("unsupported chip ID: 0x%04x\n", val);
@@ -4296,7 +4417,7 @@ static int __init nct6775_find(int sioaddr, struct nct6775_sio_data *sio_data)
 	}
 
 	if (sio_data->kind == nct6791 || sio_data->kind == nct6792 ||
-	    sio_data->kind == nct6793)
+	    sio_data->kind == nct6793 || sio_data->kind == nct6795)
 		nct6791_enable_io_mapping(sioaddr);
 
 	superio_exit(sioaddr);
diff --git a/drivers/hwmon/pmbus/Kconfig b/drivers/hwmon/pmbus/Kconfig
index cad1229b7e17..68d717a3fd59 100644
--- a/drivers/hwmon/pmbus/Kconfig
+++ b/drivers/hwmon/pmbus/Kconfig
@@ -37,6 +37,16 @@ config SENSORS_ADM1275
 	  This driver can also be built as a module. If so, the module will
 	  be called adm1275.
 
+config SENSORS_IR35221
+	tristate "Infineon IR35221"
+	default n
+	help
+	  If you say yes here you get hardware monitoring support for the
+	  Infineon IR35221 controller.
+
+	  This driver can also be built as a module. If so, the module will
+	  be called ir35521.
+
 config SENSORS_LM25066
 	tristate "National Semiconductor LM25066 and compatibles"
 	default n
diff --git a/drivers/hwmon/pmbus/Makefile b/drivers/hwmon/pmbus/Makefile
index 562132054aaf..75bb7ca619d9 100644
--- a/drivers/hwmon/pmbus/Makefile
+++ b/drivers/hwmon/pmbus/Makefile
@@ -5,6 +5,7 @@
 obj-$(CONFIG_PMBUS)		+= pmbus_core.o
 obj-$(CONFIG_SENSORS_PMBUS)	+= pmbus.o
 obj-$(CONFIG_SENSORS_ADM1275)	+= adm1275.o
+obj-$(CONFIG_SENSORS_IR35221)	+= ir35221.o
 obj-$(CONFIG_SENSORS_LM25066)	+= lm25066.o
 obj-$(CONFIG_SENSORS_LTC2978)	+= ltc2978.o
 obj-$(CONFIG_SENSORS_LTC3815)	+= ltc3815.o
diff --git a/drivers/hwmon/pmbus/ir35221.c b/drivers/hwmon/pmbus/ir35221.c
new file mode 100644
index 000000000000..8b906b44484b
--- /dev/null
+++ b/drivers/hwmon/pmbus/ir35221.c
@@ -0,0 +1,337 @@
+/*
+ * Hardware monitoring driver for IR35221
+ *
+ * Copyright (C) IBM Corporation 2017.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the 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/err.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include "pmbus.h"
+
+#define IR35221_MFR_VIN_PEAK		0xc5
+#define IR35221_MFR_VOUT_PEAK		0xc6
+#define IR35221_MFR_IOUT_PEAK		0xc7
+#define IR35221_MFR_TEMP_PEAK		0xc8
+#define IR35221_MFR_VIN_VALLEY		0xc9
+#define IR35221_MFR_VOUT_VALLEY		0xca
+#define IR35221_MFR_IOUT_VALLEY		0xcb
+#define IR35221_MFR_TEMP_VALLEY		0xcc
+
+static long ir35221_reg2data(int data, enum pmbus_sensor_classes class)
+{
+	s16 exponent;
+	s32 mantissa;
+	long val;
+
+	/* We only modify LINEAR11 formats */
+	exponent = ((s16)data) >> 11;
+	mantissa = ((s16)((data & 0x7ff) << 5)) >> 5;
+
+	val = mantissa * 1000L;
+
+	/* scale result to micro-units for power sensors */
+	if (class == PSC_POWER)
+		val = val * 1000L;
+
+	if (exponent >= 0)
+		val <<= exponent;
+	else
+		val >>= -exponent;
+
+	return val;
+}
+
+#define MAX_MANTISSA	(1023 * 1000)
+#define MIN_MANTISSA	(511 * 1000)
+
+static u16 ir35221_data2reg(long val, enum pmbus_sensor_classes class)
+{
+	s16 exponent = 0, mantissa;
+	bool negative = false;
+
+	if (val == 0)
+		return 0;
+
+	if (val < 0) {
+		negative = true;
+		val = -val;
+	}
+
+	/* Power is in uW. Convert to mW before converting. */
+	if (class == PSC_POWER)
+		val = DIV_ROUND_CLOSEST(val, 1000L);
+
+	/* Reduce large mantissa until it fits into 10 bit */
+	while (val >= MAX_MANTISSA && exponent < 15) {
+		exponent++;
+		val >>= 1;
+	}
+	/* Increase small mantissa to improve precision */
+	while (val < MIN_MANTISSA && exponent > -15) {
+		exponent--;
+		val <<= 1;
+	}
+
+	/* Convert mantissa from milli-units to units */
+	mantissa = DIV_ROUND_CLOSEST(val, 1000);
+
+	/* Ensure that resulting number is within range */
+	if (mantissa > 0x3ff)
+		mantissa = 0x3ff;
+
+	/* restore sign */
+	if (negative)
+		mantissa = -mantissa;
+
+	/* Convert to 5 bit exponent, 11 bit mantissa */
+	return (mantissa & 0x7ff) | ((exponent << 11) & 0xf800);
+}
+
+static u16 ir35221_scale_result(s16 data, int shift,
+				enum pmbus_sensor_classes class)
+{
+	long val;
+
+	val = ir35221_reg2data(data, class);
+
+	if (shift < 0)
+		val >>= -shift;
+	else
+		val <<= shift;
+
+	return ir35221_data2reg(val, class);
+}
+
+static int ir35221_read_word_data(struct i2c_client *client, int page, int reg)
+{
+	int ret;
+
+	switch (reg) {
+	case PMBUS_IOUT_OC_FAULT_LIMIT:
+	case PMBUS_IOUT_OC_WARN_LIMIT:
+		ret = pmbus_read_word_data(client, page, reg);
+		if (ret < 0)
+			break;
+		ret = ir35221_scale_result(ret, 1, PSC_CURRENT_OUT);
+		break;
+	case PMBUS_VIN_OV_FAULT_LIMIT:
+	case PMBUS_VIN_OV_WARN_LIMIT:
+	case PMBUS_VIN_UV_WARN_LIMIT:
+		ret = pmbus_read_word_data(client, page, reg);
+		ret = ir35221_scale_result(ret, -4, PSC_VOLTAGE_IN);
+		break;
+	case PMBUS_IIN_OC_WARN_LIMIT:
+		ret = pmbus_read_word_data(client, page, reg);
+		if (ret < 0)
+			break;
+		ret = ir35221_scale_result(ret, -1, PSC_CURRENT_IN);
+		break;
+	case PMBUS_READ_VIN:
+		ret = pmbus_read_word_data(client, page, PMBUS_READ_VIN);
+		if (ret < 0)
+			break;
+		ret = ir35221_scale_result(ret, -5, PSC_VOLTAGE_IN);
+		break;
+	case PMBUS_READ_IIN:
+		ret = pmbus_read_word_data(client, page, PMBUS_READ_IIN);
+		if (ret < 0)
+			break;
+		if (page == 0)
+			ret = ir35221_scale_result(ret, -4, PSC_CURRENT_IN);
+		else
+			ret = ir35221_scale_result(ret, -5, PSC_CURRENT_IN);
+		break;
+	case PMBUS_READ_POUT:
+		ret = pmbus_read_word_data(client, page, PMBUS_READ_POUT);
+		if (ret < 0)
+			break;
+		ret = ir35221_scale_result(ret, -1, PSC_POWER);
+		break;
+	case PMBUS_READ_PIN:
+		ret = pmbus_read_word_data(client, page, PMBUS_READ_PIN);
+		if (ret < 0)
+			break;
+		ret = ir35221_scale_result(ret, -1, PSC_POWER);
+		break;
+	case PMBUS_READ_IOUT:
+		ret = pmbus_read_word_data(client, page, PMBUS_READ_IOUT);
+		if (ret < 0)
+			break;
+		if (page == 0)
+			ret = ir35221_scale_result(ret, -1, PSC_CURRENT_OUT);
+		else
+			ret = ir35221_scale_result(ret, -2, PSC_CURRENT_OUT);
+		break;
+	case PMBUS_VIRT_READ_VIN_MAX:
+		ret = pmbus_read_word_data(client, page, IR35221_MFR_VIN_PEAK);
+		if (ret < 0)
+			break;
+		ret = ir35221_scale_result(ret, -5, PSC_VOLTAGE_IN);
+		break;
+	case PMBUS_VIRT_READ_VOUT_MAX:
+		ret = pmbus_read_word_data(client, page, IR35221_MFR_VOUT_PEAK);
+		break;
+	case PMBUS_VIRT_READ_IOUT_MAX:
+		ret = pmbus_read_word_data(client, page, IR35221_MFR_IOUT_PEAK);
+		if (ret < 0)
+			break;
+		if (page == 0)
+			ret = ir35221_scale_result(ret, -1, PSC_CURRENT_IN);
+		else
+			ret = ir35221_scale_result(ret, -2, PSC_CURRENT_IN);
+		break;
+	case PMBUS_VIRT_READ_TEMP_MAX:
+		ret = pmbus_read_word_data(client, page, IR35221_MFR_TEMP_PEAK);
+		break;
+	case PMBUS_VIRT_READ_VIN_MIN:
+		ret = pmbus_read_word_data(client, page,
+					   IR35221_MFR_VIN_VALLEY);
+		if (ret < 0)
+			break;
+		ret = ir35221_scale_result(ret, -5, PSC_VOLTAGE_IN);
+		break;
+	case PMBUS_VIRT_READ_VOUT_MIN:
+		ret = pmbus_read_word_data(client, page,
+					   IR35221_MFR_VOUT_VALLEY);
+		break;
+	case PMBUS_VIRT_READ_IOUT_MIN:
+		ret = pmbus_read_word_data(client, page,
+					   IR35221_MFR_IOUT_VALLEY);
+		if (ret < 0)
+			break;
+		if (page == 0)
+			ret = ir35221_scale_result(ret, -1, PSC_CURRENT_IN);
+		else
+			ret = ir35221_scale_result(ret, -2, PSC_CURRENT_IN);
+		break;
+	case PMBUS_VIRT_READ_TEMP_MIN:
+		ret = pmbus_read_word_data(client, page,
+					   IR35221_MFR_TEMP_VALLEY);
+		break;
+	default:
+		ret = -ENODATA;
+		break;
+	}
+
+	return ret;
+}
+
+static int ir35221_write_word_data(struct i2c_client *client, int page, int reg,
+				   u16 word)
+{
+	int ret;
+	u16 val;
+
+	switch (reg) {
+	case PMBUS_IOUT_OC_FAULT_LIMIT:
+	case PMBUS_IOUT_OC_WARN_LIMIT:
+		val = ir35221_scale_result(word, -1, PSC_CURRENT_OUT);
+		ret = pmbus_write_word_data(client, page, reg, val);
+		break;
+	case PMBUS_VIN_OV_FAULT_LIMIT:
+	case PMBUS_VIN_OV_WARN_LIMIT:
+	case PMBUS_VIN_UV_WARN_LIMIT:
+		val = ir35221_scale_result(word, 4, PSC_VOLTAGE_IN);
+		ret = pmbus_write_word_data(client, page, reg, val);
+		break;
+	case PMBUS_IIN_OC_WARN_LIMIT:
+		val = ir35221_scale_result(word, 1, PSC_CURRENT_IN);
+		ret = pmbus_write_word_data(client, page, reg, val);
+		break;
+	default:
+		ret = -ENODATA;
+		break;
+	}
+
+	return ret;
+}
+
+static int ir35221_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct pmbus_driver_info *info;
+	u8 buf[I2C_SMBUS_BLOCK_MAX];
+	int ret;
+
+	if (!i2c_check_functionality(client->adapter,
+				     I2C_FUNC_SMBUS_READ_BYTE_DATA
+				| I2C_FUNC_SMBUS_READ_WORD_DATA
+				| I2C_FUNC_SMBUS_READ_BLOCK_DATA))
+		return -ENODEV;
+
+	ret = i2c_smbus_read_block_data(client, PMBUS_MFR_ID, buf);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed to read PMBUS_MFR_ID\n");
+		return ret;
+	}
+	if (ret != 2 || strncmp(buf, "RI", strlen("RI"))) {
+		dev_err(&client->dev, "MFR_ID unrecognised\n");
+		return -ENODEV;
+	}
+
+	ret = i2c_smbus_read_block_data(client, PMBUS_MFR_MODEL, buf);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed to read PMBUS_MFR_MODEL\n");
+		return ret;
+	}
+	if (ret != 2 || !(buf[0] == 0x6c && buf[1] == 0x00)) {
+		dev_err(&client->dev, "MFR_MODEL unrecognised\n");
+		return -ENODEV;
+	}
+
+	info = devm_kzalloc(&client->dev, sizeof(struct pmbus_driver_info),
+			    GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	info->write_word_data = ir35221_write_word_data;
+	info->read_word_data = ir35221_read_word_data;
+
+	info->pages = 2;
+	info->format[PSC_VOLTAGE_IN] = linear;
+	info->format[PSC_VOLTAGE_OUT] = linear;
+	info->format[PSC_CURRENT_IN] = linear;
+	info->format[PSC_CURRENT_OUT] = linear;
+	info->format[PSC_POWER] = linear;
+	info->format[PSC_TEMPERATURE] = linear;
+
+	info->func[0] = PMBUS_HAVE_VIN
+		| PMBUS_HAVE_VOUT | PMBUS_HAVE_IIN
+		| PMBUS_HAVE_IOUT | PMBUS_HAVE_PIN
+		| PMBUS_HAVE_POUT | PMBUS_HAVE_TEMP
+		| PMBUS_HAVE_STATUS_VOUT | PMBUS_HAVE_STATUS_IOUT
+		| PMBUS_HAVE_STATUS_INPUT | PMBUS_HAVE_STATUS_TEMP;
+	info->func[1] = info->func[0];
+
+	return pmbus_do_probe(client, id, info);
+}
+
+static const struct i2c_device_id ir35221_id[] = {
+	{"ir35221", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, ir35221_id);
+
+static struct i2c_driver ir35221_driver = {
+	.driver = {
+		.name	= "ir35221",
+	},
+	.probe		= ir35221_probe,
+	.remove		= pmbus_do_remove,
+	.id_table	= ir35221_id,
+};
+
+module_i2c_driver(ir35221_driver);
+
+MODULE_AUTHOR("Samuel Mendoza-Jonas <sam@mendozajonas.com");
+MODULE_DESCRIPTION("PMBus driver for IR35221");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/pmbus/pmbus.c b/drivers/hwmon/pmbus/pmbus.c
index 44ca8a94873d..7718e58dbda5 100644
--- a/drivers/hwmon/pmbus/pmbus.c
+++ b/drivers/hwmon/pmbus/pmbus.c
@@ -25,7 +25,7 @@
 #include <linux/slab.h>
 #include <linux/mutex.h>
 #include <linux/i2c.h>
-#include <linux/i2c/pmbus.h>
+#include <linux/pmbus.h>
 #include "pmbus.h"
 
 /*
diff --git a/drivers/hwmon/pmbus/pmbus_core.c b/drivers/hwmon/pmbus/pmbus_core.c
index ba59eaef2e07..f1eff6b6c798 100644
--- a/drivers/hwmon/pmbus/pmbus_core.c
+++ b/drivers/hwmon/pmbus/pmbus_core.c
@@ -28,7 +28,7 @@
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
 #include <linux/jiffies.h>
-#include <linux/i2c/pmbus.h>
+#include <linux/pmbus.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include "pmbus.h"
diff --git a/drivers/hwmon/pmbus/ucd9000.c b/drivers/hwmon/pmbus/ucd9000.c
index 3518f0c08934..b74dbeca2e8d 100644
--- a/drivers/hwmon/pmbus/ucd9000.c
+++ b/drivers/hwmon/pmbus/ucd9000.c
@@ -26,7 +26,7 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
-#include <linux/i2c/pmbus.h>
+#include <linux/pmbus.h>
 #include "pmbus.h"
 
 enum chips { ucd9000, ucd90120, ucd90124, ucd90160, ucd9090, ucd90910 };
diff --git a/drivers/hwmon/pmbus/ucd9200.c b/drivers/hwmon/pmbus/ucd9200.c
index a8712c5ded4e..3ed94585837a 100644
--- a/drivers/hwmon/pmbus/ucd9200.c
+++ b/drivers/hwmon/pmbus/ucd9200.c
@@ -25,7 +25,7 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
-#include <linux/i2c/pmbus.h>
+#include <linux/pmbus.h>
 #include "pmbus.h"
 
 #define UCD9200_PHASE_INFO	0xd2
diff --git a/drivers/hwmon/pwm-fan.c b/drivers/hwmon/pwm-fan.c
index f9af3935b427..70cc0d134f3c 100644
--- a/drivers/hwmon/pwm-fan.c
+++ b/drivers/hwmon/pwm-fan.c
@@ -40,31 +40,22 @@ struct pwm_fan_ctx {
 
 static int  __set_pwm(struct pwm_fan_ctx *ctx, unsigned long pwm)
 {
-	struct pwm_args pargs;
-	unsigned long duty;
+	unsigned long period;
 	int ret = 0;
-
-	pwm_get_args(ctx->pwm, &pargs);
+	struct pwm_state state = { };
 
 	mutex_lock(&ctx->lock);
 	if (ctx->pwm_value == pwm)
 		goto exit_set_pwm_err;
 
-	duty = DIV_ROUND_UP(pwm * (pargs.period - 1), MAX_PWM);
-	ret = pwm_config(ctx->pwm, duty, pargs.period);
-	if (ret)
-		goto exit_set_pwm_err;
-
-	if (pwm == 0)
-		pwm_disable(ctx->pwm);
-
-	if (ctx->pwm_value == 0) {
-		ret = pwm_enable(ctx->pwm);
-		if (ret)
-			goto exit_set_pwm_err;
-	}
+	pwm_init_state(ctx->pwm, &state);
+	period = ctx->pwm->args.period;
+	state.duty_cycle = DIV_ROUND_UP(pwm * (period - 1), MAX_PWM);
+	state.enabled = pwm ? true : false;
 
-	ctx->pwm_value = pwm;
+	ret = pwm_apply_state(ctx->pwm, &state);
+	if (!ret)
+		ctx->pwm_value = pwm;
 exit_set_pwm_err:
 	mutex_unlock(&ctx->lock);
 	return ret;
@@ -218,10 +209,9 @@ static int pwm_fan_probe(struct platform_device *pdev)
 {
 	struct thermal_cooling_device *cdev;
 	struct pwm_fan_ctx *ctx;
-	struct pwm_args pargs;
 	struct device *hwmon;
-	int duty_cycle;
 	int ret;
+	struct pwm_state state = { };
 
 	ctx = devm_kzalloc(&pdev->dev, sizeof(*ctx), GFP_KERNEL);
 	if (!ctx)
@@ -237,28 +227,16 @@ static int pwm_fan_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, ctx);
 
-	/*
-	 * FIXME: pwm_apply_args() should be removed when switching to the
-	 * atomic PWM API.
-	 */
-	pwm_apply_args(ctx->pwm);
-
-	/* Set duty cycle to maximum allowed */
-	pwm_get_args(ctx->pwm, &pargs);
-
-	duty_cycle = pargs.period - 1;
 	ctx->pwm_value = MAX_PWM;
 
-	ret = pwm_config(ctx->pwm, duty_cycle, pargs.period);
-	if (ret) {
-		dev_err(&pdev->dev, "Failed to configure PWM\n");
-		return ret;
-	}
+	/* Set duty cycle to maximum allowed and enable PWM output */
+	pwm_init_state(ctx->pwm, &state);
+	state.duty_cycle = ctx->pwm->args.period - 1;
+	state.enabled = true;
 
-	/* Enbale PWM output */
-	ret = pwm_enable(ctx->pwm);
+	ret = pwm_apply_state(ctx->pwm, &state);
 	if (ret) {
-		dev_err(&pdev->dev, "Failed to enable PWM\n");
+		dev_err(&pdev->dev, "Failed to configure PWM\n");
 		return ret;
 	}
 
@@ -266,8 +244,8 @@ static int pwm_fan_probe(struct platform_device *pdev)
 						       ctx, pwm_fan_groups);
 	if (IS_ERR(hwmon)) {
 		dev_err(&pdev->dev, "Failed to register hwmon device\n");
-		pwm_disable(ctx->pwm);
-		return PTR_ERR(hwmon);
+		ret = PTR_ERR(hwmon);
+		goto err_pwm_disable;
 	}
 
 	ret = pwm_fan_of_get_cooling_data(&pdev->dev, ctx);
@@ -282,14 +260,20 @@ static int pwm_fan_probe(struct platform_device *pdev)
 		if (IS_ERR(cdev)) {
 			dev_err(&pdev->dev,
 				"Failed to register pwm-fan as cooling device");
-			pwm_disable(ctx->pwm);
-			return PTR_ERR(cdev);
+			ret = PTR_ERR(cdev);
+			goto err_pwm_disable;
 		}
 		ctx->cdev = cdev;
 		thermal_cdev_update(cdev);
 	}
 
 	return 0;
+
+err_pwm_disable:
+	state.enabled = false;
+	pwm_apply_state(ctx->pwm, &state);
+
+	return ret;
 }
 
 static int pwm_fan_remove(struct platform_device *pdev)
diff --git a/drivers/hwmon/scpi-hwmon.c b/drivers/hwmon/scpi-hwmon.c
index 094f948f99ff..a586480a7ca9 100644
--- a/drivers/hwmon/scpi-hwmon.c
+++ b/drivers/hwmon/scpi-hwmon.c
@@ -16,6 +16,7 @@
 
 #include <linux/hwmon.h>
 #include <linux/module.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/scpi_protocol.h>
 #include <linux/slab.h>
@@ -23,6 +24,7 @@
 #include <linux/thermal.h>
 
 struct sensor_data {
+	unsigned int scale;
 	struct scpi_sensor_info info;
 	struct device_attribute dev_attr_input;
 	struct device_attribute dev_attr_label;
@@ -44,6 +46,30 @@ struct scpi_sensors {
 	const struct attribute_group *groups[2];
 };
 
+static const u32 gxbb_scpi_scale[] = {
+	[TEMPERATURE]	= 1,		/* (celsius)		*/
+	[VOLTAGE]	= 1000,		/* (millivolts)		*/
+	[CURRENT]	= 1000,		/* (milliamperes)	*/
+	[POWER]		= 1000000,	/* (microwatts)		*/
+	[ENERGY]	= 1000000,	/* (microjoules)	*/
+};
+
+static const u32 scpi_scale[] = {
+	[TEMPERATURE]	= 1000,		/* (millicelsius)	*/
+	[VOLTAGE]	= 1000,		/* (millivolts)		*/
+	[CURRENT]	= 1000,		/* (milliamperes)	*/
+	[POWER]		= 1000000,	/* (microwatts)		*/
+	[ENERGY]	= 1000000,	/* (microjoules)	*/
+};
+
+static void scpi_scale_reading(u64 *value, struct sensor_data *sensor)
+{
+	if (scpi_scale[sensor->info.class] != sensor->scale) {
+		*value *= scpi_scale[sensor->info.class];
+		do_div(*value, sensor->scale);
+	}
+}
+
 static int scpi_read_temp(void *dev, int *temp)
 {
 	struct scpi_thermal_zone *zone = dev;
@@ -57,6 +83,8 @@ static int scpi_read_temp(void *dev, int *temp)
 	if (ret)
 		return ret;
 
+	scpi_scale_reading(&value, sensor);
+
 	*temp = value;
 	return 0;
 }
@@ -77,6 +105,8 @@ scpi_show_sensor(struct device *dev, struct device_attribute *attr, char *buf)
 	if (ret)
 		return ret;
 
+	scpi_scale_reading(&value, sensor);
+
 	return sprintf(buf, "%llu\n", value);
 }
 
@@ -94,14 +124,23 @@ static struct thermal_zone_of_device_ops scpi_sensor_ops = {
 	.get_temp = scpi_read_temp,
 };
 
+static const struct of_device_id scpi_of_match[] = {
+	{.compatible = "arm,scpi-sensors", .data = &scpi_scale},
+	{.compatible = "amlogic,meson-gxbb-scpi-sensors", .data = &gxbb_scpi_scale},
+	{},
+};
+MODULE_DEVICE_TABLE(of, scpi_of_match);
+
 static int scpi_hwmon_probe(struct platform_device *pdev)
 {
 	u16 nr_sensors, i;
+	const u32 *scale;
 	int num_temp = 0, num_volt = 0, num_current = 0, num_power = 0;
 	int num_energy = 0;
 	struct scpi_ops *scpi_ops;
 	struct device *hwdev, *dev = &pdev->dev;
 	struct scpi_sensors *scpi_sensors;
+	const struct of_device_id *of_id;
 	int idx, ret;
 
 	scpi_ops = get_scpi_ops();
@@ -131,6 +170,13 @@ static int scpi_hwmon_probe(struct platform_device *pdev)
 
 	scpi_sensors->scpi_ops = scpi_ops;
 
+	of_id = of_match_device(scpi_of_match, &pdev->dev);
+	if (!of_id) {
+		dev_err(&pdev->dev, "Unable to initialize scpi-hwmon data\n");
+		return -ENODEV;
+	}
+	scale = of_id->data;
+
 	for (i = 0, idx = 0; i < nr_sensors; i++) {
 		struct sensor_data *sensor = &scpi_sensors->data[idx];
 
@@ -178,6 +224,8 @@ static int scpi_hwmon_probe(struct platform_device *pdev)
 			continue;
 		}
 
+		sensor->scale = scale[sensor->info.class];
+
 		sensor->dev_attr_input.attr.mode = S_IRUGO;
 		sensor->dev_attr_input.show = scpi_show_sensor;
 		sensor->dev_attr_input.attr.name = sensor->input;
@@ -247,12 +295,6 @@ static int scpi_hwmon_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct of_device_id scpi_of_match[] = {
-	{.compatible = "arm,scpi-sensors"},
-	{},
-};
-MODULE_DEVICE_TABLE(of, scpi_of_match);
-
 static struct platform_driver scpi_hwmon_platdrv = {
 	.driver = {
 		.name	= "scpi-hwmon",
diff --git a/drivers/hwtracing/coresight/Kconfig b/drivers/hwtracing/coresight/Kconfig
index 130cb2114059..8d55d6d79015 100644
--- a/drivers/hwtracing/coresight/Kconfig
+++ b/drivers/hwtracing/coresight/Kconfig
@@ -89,4 +89,18 @@ config CORESIGHT_STM
 	  logging useful software events or data coming from various entities
 	  in the system, possibly running different OSs
 
+config CORESIGHT_CPU_DEBUG
+	tristate "CoreSight CPU Debug driver"
+	depends on ARM || ARM64
+	depends on DEBUG_FS
+	help
+	  This driver provides support for coresight debugging module. This
+	  is primarily used to dump sample-based profiling registers when
+	  system triggers panic, the driver will parse context registers so
+	  can quickly get to know program counter (PC), secure state,
+	  exception level, etc. Before use debugging functionality, platform
+	  needs to ensure the clock domain and power domain are enabled
+	  properly, please refer Documentation/trace/coresight-cpu-debug.txt
+	  for detailed description and the example for usage.
+
 endif
diff --git a/drivers/hwtracing/coresight/Makefile b/drivers/hwtracing/coresight/Makefile
index af480d9c1441..433d59025eb6 100644
--- a/drivers/hwtracing/coresight/Makefile
+++ b/drivers/hwtracing/coresight/Makefile
@@ -16,3 +16,4 @@ obj-$(CONFIG_CORESIGHT_SOURCE_ETM4X) += coresight-etm4x.o \
 					coresight-etm4x-sysfs.o
 obj-$(CONFIG_CORESIGHT_QCOM_REPLICATOR) += coresight-replicator-qcom.o
 obj-$(CONFIG_CORESIGHT_STM) += coresight-stm.o
+obj-$(CONFIG_CORESIGHT_CPU_DEBUG) += coresight-cpu-debug.o
diff --git a/drivers/hwtracing/coresight/coresight-cpu-debug.c b/drivers/hwtracing/coresight/coresight-cpu-debug.c
new file mode 100644
index 000000000000..64a77e00eaa6
--- /dev/null
+++ b/drivers/hwtracing/coresight/coresight-cpu-debug.c
@@ -0,0 +1,700 @@
+/*
+ * Copyright (c) 2017 Linaro Limited. All rights reserved.
+ *
+ * Author: Leo Yan <leo.yan@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 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/amba/bus.h>
+#include <linux/coresight.h>
+#include <linux/cpu.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/iopoll.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/pm_qos.h>
+#include <linux/slab.h>
+#include <linux/smp.h>
+#include <linux/types.h>
+#include <linux/uaccess.h>
+
+#include "coresight-priv.h"
+
+#define EDPCSR				0x0A0
+#define EDCIDSR				0x0A4
+#define EDVIDSR				0x0A8
+#define EDPCSR_HI			0x0AC
+#define EDOSLAR				0x300
+#define EDPRCR				0x310
+#define EDPRSR				0x314
+#define EDDEVID1			0xFC4
+#define EDDEVID				0xFC8
+
+#define EDPCSR_PROHIBITED		0xFFFFFFFF
+
+/* bits definition for EDPCSR */
+#define EDPCSR_THUMB			BIT(0)
+#define EDPCSR_ARM_INST_MASK		GENMASK(31, 2)
+#define EDPCSR_THUMB_INST_MASK		GENMASK(31, 1)
+
+/* bits definition for EDPRCR */
+#define EDPRCR_COREPURQ			BIT(3)
+#define EDPRCR_CORENPDRQ		BIT(0)
+
+/* bits definition for EDPRSR */
+#define EDPRSR_DLK			BIT(6)
+#define EDPRSR_PU			BIT(0)
+
+/* bits definition for EDVIDSR */
+#define EDVIDSR_NS			BIT(31)
+#define EDVIDSR_E2			BIT(30)
+#define EDVIDSR_E3			BIT(29)
+#define EDVIDSR_HV			BIT(28)
+#define EDVIDSR_VMID			GENMASK(7, 0)
+
+/*
+ * bits definition for EDDEVID1:PSCROffset
+ *
+ * NOTE: armv8 and armv7 have different definition for the register,
+ * so consolidate the bits definition as below:
+ *
+ * 0b0000 - Sample offset applies based on the instruction state, we
+ *          rely on EDDEVID to check if EDPCSR is implemented or not
+ * 0b0001 - No offset applies.
+ * 0b0010 - No offset applies, but do not use in AArch32 mode
+ *
+ */
+#define EDDEVID1_PCSR_OFFSET_MASK	GENMASK(3, 0)
+#define EDDEVID1_PCSR_OFFSET_INS_SET	(0x0)
+#define EDDEVID1_PCSR_NO_OFFSET_DIS_AARCH32	(0x2)
+
+/* bits definition for EDDEVID */
+#define EDDEVID_PCSAMPLE_MODE		GENMASK(3, 0)
+#define EDDEVID_IMPL_EDPCSR		(0x1)
+#define EDDEVID_IMPL_EDPCSR_EDCIDSR	(0x2)
+#define EDDEVID_IMPL_FULL		(0x3)
+
+#define DEBUG_WAIT_SLEEP		1000
+#define DEBUG_WAIT_TIMEOUT		32000
+
+struct debug_drvdata {
+	void __iomem	*base;
+	struct device	*dev;
+	int		cpu;
+
+	bool		edpcsr_present;
+	bool		edcidsr_present;
+	bool		edvidsr_present;
+	bool		pc_has_offset;
+
+	u32		edpcsr;
+	u32		edpcsr_hi;
+	u32		edprsr;
+	u32		edvidsr;
+	u32		edcidsr;
+};
+
+static DEFINE_MUTEX(debug_lock);
+static DEFINE_PER_CPU(struct debug_drvdata *, debug_drvdata);
+static int debug_count;
+static struct dentry *debug_debugfs_dir;
+
+static bool debug_enable;
+module_param_named(enable, debug_enable, bool, 0600);
+MODULE_PARM_DESC(enable, "Control to enable coresight CPU debug functionality");
+
+static void debug_os_unlock(struct debug_drvdata *drvdata)
+{
+	/* Unlocks the debug registers */
+	writel_relaxed(0x0, drvdata->base + EDOSLAR);
+
+	/* Make sure the registers are unlocked before accessing */
+	wmb();
+}
+
+/*
+ * According to ARM DDI 0487A.k, before access external debug
+ * registers should firstly check the access permission; if any
+ * below condition has been met then cannot access debug
+ * registers to avoid lockup issue:
+ *
+ * - CPU power domain is powered off;
+ * - The OS Double Lock is locked;
+ *
+ * By checking EDPRSR can get to know if meet these conditions.
+ */
+static bool debug_access_permitted(struct debug_drvdata *drvdata)
+{
+	/* CPU is powered off */
+	if (!(drvdata->edprsr & EDPRSR_PU))
+		return false;
+
+	/* The OS Double Lock is locked */
+	if (drvdata->edprsr & EDPRSR_DLK)
+		return false;
+
+	return true;
+}
+
+static void debug_force_cpu_powered_up(struct debug_drvdata *drvdata)
+{
+	u32 edprcr;
+
+try_again:
+
+	/*
+	 * Send request to power management controller and assert
+	 * DBGPWRUPREQ signal; if power management controller has
+	 * sane implementation, it should enable CPU power domain
+	 * in case CPU is in low power state.
+	 */
+	edprcr = readl_relaxed(drvdata->base + EDPRCR);
+	edprcr |= EDPRCR_COREPURQ;
+	writel_relaxed(edprcr, drvdata->base + EDPRCR);
+
+	/* Wait for CPU to be powered up (timeout~=32ms) */
+	if (readx_poll_timeout_atomic(readl_relaxed, drvdata->base + EDPRSR,
+			drvdata->edprsr, (drvdata->edprsr & EDPRSR_PU),
+			DEBUG_WAIT_SLEEP, DEBUG_WAIT_TIMEOUT)) {
+		/*
+		 * Unfortunately the CPU cannot be powered up, so return
+		 * back and later has no permission to access other
+		 * registers. For this case, should disable CPU low power
+		 * states to ensure CPU power domain is enabled!
+		 */
+		dev_err(drvdata->dev, "%s: power up request for CPU%d failed\n",
+			__func__, drvdata->cpu);
+		return;
+	}
+
+	/*
+	 * At this point the CPU is powered up, so set the no powerdown
+	 * request bit so we don't lose power and emulate power down.
+	 */
+	edprcr = readl_relaxed(drvdata->base + EDPRCR);
+	edprcr |= EDPRCR_COREPURQ | EDPRCR_CORENPDRQ;
+	writel_relaxed(edprcr, drvdata->base + EDPRCR);
+
+	drvdata->edprsr = readl_relaxed(drvdata->base + EDPRSR);
+
+	/* The core power domain got switched off on use, try again */
+	if (unlikely(!(drvdata->edprsr & EDPRSR_PU)))
+		goto try_again;
+}
+
+static void debug_read_regs(struct debug_drvdata *drvdata)
+{
+	u32 save_edprcr;
+
+	CS_UNLOCK(drvdata->base);
+
+	/* Unlock os lock */
+	debug_os_unlock(drvdata);
+
+	/* Save EDPRCR register */
+	save_edprcr = readl_relaxed(drvdata->base + EDPRCR);
+
+	/*
+	 * Ensure CPU power domain is enabled to let registers
+	 * are accessiable.
+	 */
+	debug_force_cpu_powered_up(drvdata);
+
+	if (!debug_access_permitted(drvdata))
+		goto out;
+
+	drvdata->edpcsr = readl_relaxed(drvdata->base + EDPCSR);
+
+	/*
+	 * As described in ARM DDI 0487A.k, if the processing
+	 * element (PE) is in debug state, or sample-based
+	 * profiling is prohibited, EDPCSR reads as 0xFFFFFFFF;
+	 * EDCIDSR, EDVIDSR and EDPCSR_HI registers also become
+	 * UNKNOWN state. So directly bail out for this case.
+	 */
+	if (drvdata->edpcsr == EDPCSR_PROHIBITED)
+		goto out;
+
+	/*
+	 * A read of the EDPCSR normally has the side-effect of
+	 * indirectly writing to EDCIDSR, EDVIDSR and EDPCSR_HI;
+	 * at this point it's safe to read value from them.
+	 */
+	if (IS_ENABLED(CONFIG_64BIT))
+		drvdata->edpcsr_hi = readl_relaxed(drvdata->base + EDPCSR_HI);
+
+	if (drvdata->edcidsr_present)
+		drvdata->edcidsr = readl_relaxed(drvdata->base + EDCIDSR);
+
+	if (drvdata->edvidsr_present)
+		drvdata->edvidsr = readl_relaxed(drvdata->base + EDVIDSR);
+
+out:
+	/* Restore EDPRCR register */
+	writel_relaxed(save_edprcr, drvdata->base + EDPRCR);
+
+	CS_LOCK(drvdata->base);
+}
+
+#ifdef CONFIG_64BIT
+static unsigned long debug_adjust_pc(struct debug_drvdata *drvdata)
+{
+	return (unsigned long)drvdata->edpcsr_hi << 32 |
+	       (unsigned long)drvdata->edpcsr;
+}
+#else
+static unsigned long debug_adjust_pc(struct debug_drvdata *drvdata)
+{
+	unsigned long arm_inst_offset = 0, thumb_inst_offset = 0;
+	unsigned long pc;
+
+	pc = (unsigned long)drvdata->edpcsr;
+
+	if (drvdata->pc_has_offset) {
+		arm_inst_offset = 8;
+		thumb_inst_offset = 4;
+	}
+
+	/* Handle thumb instruction */
+	if (pc & EDPCSR_THUMB) {
+		pc = (pc & EDPCSR_THUMB_INST_MASK) - thumb_inst_offset;
+		return pc;
+	}
+
+	/*
+	 * Handle arm instruction offset, if the arm instruction
+	 * is not 4 byte alignment then it's possible the case
+	 * for implementation defined; keep original value for this
+	 * case and print info for notice.
+	 */
+	if (pc & BIT(1))
+		dev_emerg(drvdata->dev,
+			  "Instruction offset is implementation defined\n");
+	else
+		pc = (pc & EDPCSR_ARM_INST_MASK) - arm_inst_offset;
+
+	return pc;
+}
+#endif
+
+static void debug_dump_regs(struct debug_drvdata *drvdata)
+{
+	struct device *dev = drvdata->dev;
+	unsigned long pc;
+
+	dev_emerg(dev, " EDPRSR:  %08x (Power:%s DLK:%s)\n",
+		  drvdata->edprsr,
+		  drvdata->edprsr & EDPRSR_PU ? "On" : "Off",
+		  drvdata->edprsr & EDPRSR_DLK ? "Lock" : "Unlock");
+
+	if (!debug_access_permitted(drvdata)) {
+		dev_emerg(dev, "No permission to access debug registers!\n");
+		return;
+	}
+
+	if (drvdata->edpcsr == EDPCSR_PROHIBITED) {
+		dev_emerg(dev, "CPU is in Debug state or profiling is prohibited!\n");
+		return;
+	}
+
+	pc = debug_adjust_pc(drvdata);
+	dev_emerg(dev, " EDPCSR:  [<%p>] %pS\n", (void *)pc, (void *)pc);
+
+	if (drvdata->edcidsr_present)
+		dev_emerg(dev, " EDCIDSR: %08x\n", drvdata->edcidsr);
+
+	if (drvdata->edvidsr_present)
+		dev_emerg(dev, " EDVIDSR: %08x (State:%s Mode:%s Width:%dbits VMID:%x)\n",
+			  drvdata->edvidsr,
+			  drvdata->edvidsr & EDVIDSR_NS ?
+			  "Non-secure" : "Secure",
+			  drvdata->edvidsr & EDVIDSR_E3 ? "EL3" :
+				(drvdata->edvidsr & EDVIDSR_E2 ?
+				 "EL2" : "EL1/0"),
+			  drvdata->edvidsr & EDVIDSR_HV ? 64 : 32,
+			  drvdata->edvidsr & (u32)EDVIDSR_VMID);
+}
+
+static void debug_init_arch_data(void *info)
+{
+	struct debug_drvdata *drvdata = info;
+	u32 mode, pcsr_offset;
+	u32 eddevid, eddevid1;
+
+	CS_UNLOCK(drvdata->base);
+
+	/* Read device info */
+	eddevid  = readl_relaxed(drvdata->base + EDDEVID);
+	eddevid1 = readl_relaxed(drvdata->base + EDDEVID1);
+
+	CS_LOCK(drvdata->base);
+
+	/* Parse implementation feature */
+	mode = eddevid & EDDEVID_PCSAMPLE_MODE;
+	pcsr_offset = eddevid1 & EDDEVID1_PCSR_OFFSET_MASK;
+
+	drvdata->edpcsr_present  = false;
+	drvdata->edcidsr_present = false;
+	drvdata->edvidsr_present = false;
+	drvdata->pc_has_offset   = false;
+
+	switch (mode) {
+	case EDDEVID_IMPL_FULL:
+		drvdata->edvidsr_present = true;
+		/* Fall through */
+	case EDDEVID_IMPL_EDPCSR_EDCIDSR:
+		drvdata->edcidsr_present = true;
+		/* Fall through */
+	case EDDEVID_IMPL_EDPCSR:
+		/*
+		 * In ARM DDI 0487A.k, the EDDEVID1.PCSROffset is used to
+		 * define if has the offset for PC sampling value; if read
+		 * back EDDEVID1.PCSROffset == 0x2, then this means the debug
+		 * module does not sample the instruction set state when
+		 * armv8 CPU in AArch32 state.
+		 */
+		drvdata->edpcsr_present =
+			((IS_ENABLED(CONFIG_64BIT) && pcsr_offset != 0) ||
+			 (pcsr_offset != EDDEVID1_PCSR_NO_OFFSET_DIS_AARCH32));
+
+		drvdata->pc_has_offset =
+			(pcsr_offset == EDDEVID1_PCSR_OFFSET_INS_SET);
+		break;
+	default:
+		break;
+	}
+}
+
+/*
+ * Dump out information on panic.
+ */
+static int debug_notifier_call(struct notifier_block *self,
+			       unsigned long v, void *p)
+{
+	int cpu;
+	struct debug_drvdata *drvdata;
+
+	mutex_lock(&debug_lock);
+
+	/* Bail out if the functionality is disabled */
+	if (!debug_enable)
+		goto skip_dump;
+
+	pr_emerg("ARM external debug module:\n");
+
+	for_each_possible_cpu(cpu) {
+		drvdata = per_cpu(debug_drvdata, cpu);
+		if (!drvdata)
+			continue;
+
+		dev_emerg(drvdata->dev, "CPU[%d]:\n", drvdata->cpu);
+
+		debug_read_regs(drvdata);
+		debug_dump_regs(drvdata);
+	}
+
+skip_dump:
+	mutex_unlock(&debug_lock);
+	return 0;
+}
+
+static struct notifier_block debug_notifier = {
+	.notifier_call = debug_notifier_call,
+};
+
+static int debug_enable_func(void)
+{
+	struct debug_drvdata *drvdata;
+	int cpu, ret = 0;
+	cpumask_t mask;
+
+	/*
+	 * Use cpumask to track which debug power domains have
+	 * been powered on and use it to handle failure case.
+	 */
+	cpumask_clear(&mask);
+
+	for_each_possible_cpu(cpu) {
+		drvdata = per_cpu(debug_drvdata, cpu);
+		if (!drvdata)
+			continue;
+
+		ret = pm_runtime_get_sync(drvdata->dev);
+		if (ret < 0)
+			goto err;
+		else
+			cpumask_set_cpu(cpu, &mask);
+	}
+
+	return 0;
+
+err:
+	/*
+	 * If pm_runtime_get_sync() has failed, need rollback on
+	 * all the other CPUs that have been enabled before that.
+	 */
+	for_each_cpu(cpu, &mask) {
+		drvdata = per_cpu(debug_drvdata, cpu);
+		pm_runtime_put_noidle(drvdata->dev);
+	}
+
+	return ret;
+}
+
+static int debug_disable_func(void)
+{
+	struct debug_drvdata *drvdata;
+	int cpu, ret, err = 0;
+
+	/*
+	 * Disable debug power domains, records the error and keep
+	 * circling through all other CPUs when an error has been
+	 * encountered.
+	 */
+	for_each_possible_cpu(cpu) {
+		drvdata = per_cpu(debug_drvdata, cpu);
+		if (!drvdata)
+			continue;
+
+		ret = pm_runtime_put(drvdata->dev);
+		if (ret < 0)
+			err = ret;
+	}
+
+	return err;
+}
+
+static ssize_t debug_func_knob_write(struct file *f,
+		const char __user *buf, size_t count, loff_t *ppos)
+{
+	u8 val;
+	int ret;
+
+	ret = kstrtou8_from_user(buf, count, 2, &val);
+	if (ret)
+		return ret;
+
+	mutex_lock(&debug_lock);
+
+	if (val == debug_enable)
+		goto out;
+
+	if (val)
+		ret = debug_enable_func();
+	else
+		ret = debug_disable_func();
+
+	if (ret) {
+		pr_err("%s: unable to %s debug function: %d\n",
+		       __func__, val ? "enable" : "disable", ret);
+		goto err;
+	}
+
+	debug_enable = val;
+out:
+	ret = count;
+err:
+	mutex_unlock(&debug_lock);
+	return ret;
+}
+
+static ssize_t debug_func_knob_read(struct file *f,
+		char __user *ubuf, size_t count, loff_t *ppos)
+{
+	ssize_t ret;
+	char buf[3];
+
+	mutex_lock(&debug_lock);
+	snprintf(buf, sizeof(buf), "%d\n", debug_enable);
+	mutex_unlock(&debug_lock);
+
+	ret = simple_read_from_buffer(ubuf, count, ppos, buf, sizeof(buf));
+	return ret;
+}
+
+static const struct file_operations debug_func_knob_fops = {
+	.open	= simple_open,
+	.read	= debug_func_knob_read,
+	.write	= debug_func_knob_write,
+};
+
+static int debug_func_init(void)
+{
+	struct dentry *file;
+	int ret;
+
+	/* Create debugfs node */
+	debug_debugfs_dir = debugfs_create_dir("coresight_cpu_debug", NULL);
+	if (!debug_debugfs_dir) {
+		pr_err("%s: unable to create debugfs directory\n", __func__);
+		return -ENOMEM;
+	}
+
+	file = debugfs_create_file("enable", 0644, debug_debugfs_dir, NULL,
+				   &debug_func_knob_fops);
+	if (!file) {
+		pr_err("%s: unable to create enable knob file\n", __func__);
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	/* Register function to be called for panic */
+	ret = atomic_notifier_chain_register(&panic_notifier_list,
+					     &debug_notifier);
+	if (ret) {
+		pr_err("%s: unable to register notifier: %d\n",
+		       __func__, ret);
+		goto err;
+	}
+
+	return 0;
+
+err:
+	debugfs_remove_recursive(debug_debugfs_dir);
+	return ret;
+}
+
+static void debug_func_exit(void)
+{
+	atomic_notifier_chain_unregister(&panic_notifier_list,
+					 &debug_notifier);
+	debugfs_remove_recursive(debug_debugfs_dir);
+}
+
+static int debug_probe(struct amba_device *adev, const struct amba_id *id)
+{
+	void __iomem *base;
+	struct device *dev = &adev->dev;
+	struct debug_drvdata *drvdata;
+	struct resource *res = &adev->res;
+	struct device_node *np = adev->dev.of_node;
+	int ret;
+
+	drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
+	if (!drvdata)
+		return -ENOMEM;
+
+	drvdata->cpu = np ? of_coresight_get_cpu(np) : 0;
+	if (per_cpu(debug_drvdata, drvdata->cpu)) {
+		dev_err(dev, "CPU%d drvdata has already been initialized\n",
+			drvdata->cpu);
+		return -EBUSY;
+	}
+
+	drvdata->dev = &adev->dev;
+	amba_set_drvdata(adev, drvdata);
+
+	/* Validity for the resource is already checked by the AMBA core */
+	base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	drvdata->base = base;
+
+	get_online_cpus();
+	per_cpu(debug_drvdata, drvdata->cpu) = drvdata;
+	ret = smp_call_function_single(drvdata->cpu, debug_init_arch_data,
+				       drvdata, 1);
+	put_online_cpus();
+
+	if (ret) {
+		dev_err(dev, "CPU%d debug arch init failed\n", drvdata->cpu);
+		goto err;
+	}
+
+	if (!drvdata->edpcsr_present) {
+		dev_err(dev, "CPU%d sample-based profiling isn't implemented\n",
+			drvdata->cpu);
+		ret = -ENXIO;
+		goto err;
+	}
+
+	if (!debug_count++) {
+		ret = debug_func_init();
+		if (ret)
+			goto err_func_init;
+	}
+
+	mutex_lock(&debug_lock);
+	/* Turn off debug power domain if debugging is disabled */
+	if (!debug_enable)
+		pm_runtime_put(dev);
+	mutex_unlock(&debug_lock);
+
+	dev_info(dev, "Coresight debug-CPU%d initialized\n", drvdata->cpu);
+	return 0;
+
+err_func_init:
+	debug_count--;
+err:
+	per_cpu(debug_drvdata, drvdata->cpu) = NULL;
+	return ret;
+}
+
+static int debug_remove(struct amba_device *adev)
+{
+	struct device *dev = &adev->dev;
+	struct debug_drvdata *drvdata = amba_get_drvdata(adev);
+
+	per_cpu(debug_drvdata, drvdata->cpu) = NULL;
+
+	mutex_lock(&debug_lock);
+	/* Turn off debug power domain before rmmod the module */
+	if (debug_enable)
+		pm_runtime_put(dev);
+	mutex_unlock(&debug_lock);
+
+	if (!--debug_count)
+		debug_func_exit();
+
+	return 0;
+}
+
+static struct amba_id debug_ids[] = {
+	{       /* Debug for Cortex-A53 */
+		.id	= 0x000bbd03,
+		.mask	= 0x000fffff,
+	},
+	{       /* Debug for Cortex-A57 */
+		.id	= 0x000bbd07,
+		.mask	= 0x000fffff,
+	},
+	{       /* Debug for Cortex-A72 */
+		.id	= 0x000bbd08,
+		.mask	= 0x000fffff,
+	},
+	{ 0, 0 },
+};
+
+static struct amba_driver debug_driver = {
+	.drv = {
+		.name   = "coresight-cpu-debug",
+		.suppress_bind_attrs = true,
+	},
+	.probe		= debug_probe,
+	.remove		= debug_remove,
+	.id_table	= debug_ids,
+};
+
+module_amba_driver(debug_driver);
+
+MODULE_AUTHOR("Leo Yan <leo.yan@linaro.org>");
+MODULE_DESCRIPTION("ARM Coresight CPU Debug Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwtracing/coresight/coresight-etb10.c b/drivers/hwtracing/coresight/coresight-etb10.c
index 979ea6ec7902..d5b96423e1a5 100644
--- a/drivers/hwtracing/coresight/coresight-etb10.c
+++ b/drivers/hwtracing/coresight/coresight-etb10.c
@@ -375,7 +375,7 @@ static void etb_update_buffer(struct coresight_device *csdev,
 
 	/*
 	 * Entries should be aligned to the frame size.  If they are not
-	 * go back to the last alignement point to give decoding tools a
+	 * go back to the last alignment point to give decoding tools a
 	 * chance to fix things.
 	 */
 	if (write_ptr % ETB_FRAME_SIZE_WORDS) {
@@ -675,11 +675,8 @@ static int etb_probe(struct amba_device *adev, const struct amba_id *id)
 
 	drvdata->buf = devm_kzalloc(dev,
 				    drvdata->buffer_depth * 4, GFP_KERNEL);
-	if (!drvdata->buf) {
-		dev_err(dev, "Failed to allocate %u bytes for buffer data\n",
-			drvdata->buffer_depth * 4);
+	if (!drvdata->buf)
 		return -ENOMEM;
-	}
 
 	desc.type = CORESIGHT_DEV_TYPE_SINK;
 	desc.subtype.sink_subtype = CORESIGHT_DEV_SUBTYPE_SINK_BUFFER;
diff --git a/drivers/hwtracing/coresight/coresight-etm-perf.c b/drivers/hwtracing/coresight/coresight-etm-perf.c
index 288a423c1b27..8f546f59a3fd 100644
--- a/drivers/hwtracing/coresight/coresight-etm-perf.c
+++ b/drivers/hwtracing/coresight/coresight-etm-perf.c
@@ -201,6 +201,7 @@ static void *etm_setup_aux(int event_cpu, void **pages,
 	event_data = alloc_event_data(event_cpu);
 	if (!event_data)
 		return NULL;
+	INIT_WORK(&event_data->work, free_event_data);
 
 	/*
 	 * In theory nothing prevent tracers in a trace session from being
@@ -217,8 +218,6 @@ static void *etm_setup_aux(int event_cpu, void **pages,
 	if (!sink)
 		goto err;
 
-	INIT_WORK(&event_data->work, free_event_data);
-
 	mask = &event_data->mask;
 
 	/* Setup the path for each CPU in a trace session */
diff --git a/drivers/hwtracing/coresight/coresight-etm3x.c b/drivers/hwtracing/coresight/coresight-etm3x.c
index a51b6b64ecdf..93ee8fc539be 100644
--- a/drivers/hwtracing/coresight/coresight-etm3x.c
+++ b/drivers/hwtracing/coresight/coresight-etm3x.c
@@ -587,7 +587,7 @@ static void etm_disable_sysfs(struct coresight_device *csdev)
 	 * after cpu online mask indicates the cpu is offline but before the
 	 * DYING hotplug callback is serviced by the ETM driver.
 	 */
-	get_online_cpus();
+	cpus_read_lock();
 	spin_lock(&drvdata->spinlock);
 
 	/*
@@ -597,7 +597,7 @@ static void etm_disable_sysfs(struct coresight_device *csdev)
 	smp_call_function_single(drvdata->cpu, etm_disable_hw, drvdata, 1);
 
 	spin_unlock(&drvdata->spinlock);
-	put_online_cpus();
+	cpus_read_unlock();
 
 	dev_info(drvdata->dev, "ETM tracing disabled\n");
 }
@@ -795,7 +795,7 @@ static int etm_probe(struct amba_device *adev, const struct amba_id *id)
 
 	drvdata->cpu = pdata ? pdata->cpu : 0;
 
-	get_online_cpus();
+	cpus_read_lock();
 	etmdrvdata[drvdata->cpu] = drvdata;
 
 	if (smp_call_function_single(drvdata->cpu,
@@ -803,17 +803,17 @@ static int etm_probe(struct amba_device *adev, const struct amba_id *id)
 		dev_err(dev, "ETM arch init failed\n");
 
 	if (!etm_count++) {
-		cpuhp_setup_state_nocalls(CPUHP_AP_ARM_CORESIGHT_STARTING,
-					  "arm/coresight:starting",
-					  etm_starting_cpu, etm_dying_cpu);
-		ret = cpuhp_setup_state_nocalls(CPUHP_AP_ONLINE_DYN,
-						"arm/coresight:online",
-						etm_online_cpu, NULL);
+		cpuhp_setup_state_nocalls_cpuslocked(CPUHP_AP_ARM_CORESIGHT_STARTING,
+						     "arm/coresight:starting",
+						     etm_starting_cpu, etm_dying_cpu);
+		ret = cpuhp_setup_state_nocalls_cpuslocked(CPUHP_AP_ONLINE_DYN,
+							   "arm/coresight:online",
+							   etm_online_cpu, NULL);
 		if (ret < 0)
 			goto err_arch_supported;
 		hp_online = ret;
 	}
-	put_online_cpus();
+	cpus_read_unlock();
 
 	if (etm_arch_supported(drvdata->arch) == false) {
 		ret = -EINVAL;
diff --git a/drivers/hwtracing/coresight/coresight-etm4x.c b/drivers/hwtracing/coresight/coresight-etm4x.c
index d1340fb4e457..532adc9dd32a 100644
--- a/drivers/hwtracing/coresight/coresight-etm4x.c
+++ b/drivers/hwtracing/coresight/coresight-etm4x.c
@@ -371,7 +371,7 @@ static void etm4_disable_sysfs(struct coresight_device *csdev)
 	 * after cpu online mask indicates the cpu is offline but before the
 	 * DYING hotplug callback is serviced by the ETM driver.
 	 */
-	get_online_cpus();
+	cpus_read_lock();
 	spin_lock(&drvdata->spinlock);
 
 	/*
@@ -381,7 +381,7 @@ static void etm4_disable_sysfs(struct coresight_device *csdev)
 	smp_call_function_single(drvdata->cpu, etm4_disable_hw, drvdata, 1);
 
 	spin_unlock(&drvdata->spinlock);
-	put_online_cpus();
+	cpus_read_unlock();
 
 	dev_info(drvdata->dev, "ETM tracing disabled\n");
 }
@@ -982,7 +982,7 @@ static int etm4_probe(struct amba_device *adev, const struct amba_id *id)
 
 	drvdata->cpu = pdata ? pdata->cpu : 0;
 
-	get_online_cpus();
+	cpus_read_lock();
 	etmdrvdata[drvdata->cpu] = drvdata;
 
 	if (smp_call_function_single(drvdata->cpu,
@@ -990,18 +990,18 @@ static int etm4_probe(struct amba_device *adev, const struct amba_id *id)
 		dev_err(dev, "ETM arch init failed\n");
 
 	if (!etm4_count++) {
-		cpuhp_setup_state_nocalls(CPUHP_AP_ARM_CORESIGHT_STARTING,
-					  "arm/coresight4:starting",
-					  etm4_starting_cpu, etm4_dying_cpu);
-		ret = cpuhp_setup_state_nocalls(CPUHP_AP_ONLINE_DYN,
-						"arm/coresight4:online",
-						etm4_online_cpu, NULL);
+		cpuhp_setup_state_nocalls_cpuslocked(CPUHP_AP_ARM_CORESIGHT_STARTING,
+						     "arm/coresight4:starting",
+						     etm4_starting_cpu, etm4_dying_cpu);
+		ret = cpuhp_setup_state_nocalls_cpuslocked(CPUHP_AP_ONLINE_DYN,
+							   "arm/coresight4:online",
+							   etm4_online_cpu, NULL);
 		if (ret < 0)
 			goto err_arch_supported;
 		hp_online = ret;
 	}
 
-	put_online_cpus();
+	cpus_read_unlock();
 
 	if (etm4_arch_supported(drvdata->arch) == false) {
 		ret = -EINVAL;
diff --git a/drivers/hwtracing/coresight/coresight-tmc-etf.c b/drivers/hwtracing/coresight/coresight-tmc-etf.c
index aec61a6d5c63..e3b9fb82eb8d 100644
--- a/drivers/hwtracing/coresight/coresight-tmc-etf.c
+++ b/drivers/hwtracing/coresight/coresight-tmc-etf.c
@@ -166,9 +166,6 @@ out:
 	if (!used)
 		kfree(buf);
 
-	if (!ret)
-		dev_info(drvdata->dev, "TMC-ETB/ETF enabled\n");
-
 	return ret;
 }
 
@@ -204,15 +201,27 @@ out:
 
 static int tmc_enable_etf_sink(struct coresight_device *csdev, u32 mode)
 {
+	int ret;
+	struct tmc_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+
 	switch (mode) {
 	case CS_MODE_SYSFS:
-		return tmc_enable_etf_sink_sysfs(csdev);
+		ret = tmc_enable_etf_sink_sysfs(csdev);
+		break;
 	case CS_MODE_PERF:
-		return tmc_enable_etf_sink_perf(csdev);
+		ret = tmc_enable_etf_sink_perf(csdev);
+		break;
+	/* We shouldn't be here */
+	default:
+		ret = -EINVAL;
+		break;
 	}
 
-	/* We shouldn't be here */
-	return -EINVAL;
+	if (ret)
+		return ret;
+
+	dev_info(drvdata->dev, "TMC-ETB/ETF enabled\n");
+	return 0;
 }
 
 static void tmc_disable_etf_sink(struct coresight_device *csdev)
@@ -273,7 +282,7 @@ static void tmc_disable_etf_link(struct coresight_device *csdev,
 	drvdata->mode = CS_MODE_DISABLED;
 	spin_unlock_irqrestore(&drvdata->spinlock, flags);
 
-	dev_info(drvdata->dev, "TMC disabled\n");
+	dev_info(drvdata->dev, "TMC-ETF disabled\n");
 }
 
 static void *tmc_alloc_etf_buffer(struct coresight_device *csdev, int cpu,
diff --git a/drivers/hwtracing/coresight/coresight-tmc.c b/drivers/hwtracing/coresight/coresight-tmc.c
index d8517d2a968c..864488793f09 100644
--- a/drivers/hwtracing/coresight/coresight-tmc.c
+++ b/drivers/hwtracing/coresight/coresight-tmc.c
@@ -362,6 +362,13 @@ static int tmc_probe(struct amba_device *adev, const struct amba_id *id)
 		desc.type = CORESIGHT_DEV_TYPE_SINK;
 		desc.subtype.sink_subtype = CORESIGHT_DEV_SUBTYPE_SINK_BUFFER;
 		desc.ops = &tmc_etr_cs_ops;
+		/*
+		 * ETR configuration uses a 40-bit AXI master in place of
+		 * the embedded SRAM of ETB/ETF.
+		 */
+		ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(40));
+		if (ret)
+			goto out;
 	} else {
 		desc.type = CORESIGHT_DEV_TYPE_LINKSINK;
 		desc.subtype.link_subtype = CORESIGHT_DEV_SUBTYPE_LINK_FIFO;
diff --git a/drivers/hwtracing/coresight/coresight.c b/drivers/hwtracing/coresight/coresight.c
index 0c37356e417c..6a0202b7384f 100644
--- a/drivers/hwtracing/coresight/coresight.c
+++ b/drivers/hwtracing/coresight/coresight.c
@@ -253,14 +253,22 @@ static int coresight_enable_source(struct coresight_device *csdev, u32 mode)
 	return 0;
 }
 
-static void coresight_disable_source(struct coresight_device *csdev)
+/**
+ *  coresight_disable_source - Drop the reference count by 1 and disable
+ *  the device if there are no users left.
+ *
+ *  @csdev - The coresight device to disable
+ *
+ *  Returns true if the device has been disabled.
+ */
+static bool coresight_disable_source(struct coresight_device *csdev)
 {
 	if (atomic_dec_return(csdev->refcnt) == 0) {
-		if (source_ops(csdev)->disable) {
+		if (source_ops(csdev)->disable)
 			source_ops(csdev)->disable(csdev, NULL);
-			csdev->enable = false;
-		}
+		csdev->enable = false;
 	}
+	return !csdev->enable;
 }
 
 void coresight_disable_path(struct list_head *path)
@@ -550,6 +558,9 @@ int coresight_enable(struct coresight_device *csdev)
 	int cpu, ret = 0;
 	struct coresight_device *sink;
 	struct list_head *path;
+	enum coresight_dev_subtype_source subtype;
+
+	subtype = csdev->subtype.source_subtype;
 
 	mutex_lock(&coresight_mutex);
 
@@ -557,8 +568,16 @@ int coresight_enable(struct coresight_device *csdev)
 	if (ret)
 		goto out;
 
-	if (csdev->enable)
+	if (csdev->enable) {
+		/*
+		 * There could be multiple applications driving the software
+		 * source. So keep the refcount for each such user when the
+		 * source is already enabled.
+		 */
+		if (subtype == CORESIGHT_DEV_SUBTYPE_SOURCE_SOFTWARE)
+			atomic_inc(csdev->refcnt);
 		goto out;
+	}
 
 	/*
 	 * Search for a valid sink for this session but don't reset the
@@ -585,7 +604,7 @@ int coresight_enable(struct coresight_device *csdev)
 	if (ret)
 		goto err_source;
 
-	switch (csdev->subtype.source_subtype) {
+	switch (subtype) {
 	case CORESIGHT_DEV_SUBTYPE_SOURCE_PROC:
 		/*
 		 * When working from sysFS it is important to keep track
@@ -629,7 +648,7 @@ void coresight_disable(struct coresight_device *csdev)
 	if (ret)
 		goto out;
 
-	if (!csdev->enable)
+	if (!csdev->enable || !coresight_disable_source(csdev))
 		goto out;
 
 	switch (csdev->subtype.source_subtype) {
@@ -647,7 +666,6 @@ void coresight_disable(struct coresight_device *csdev)
 		break;
 	}
 
-	coresight_disable_source(csdev);
 	coresight_disable_path(path);
 	coresight_release_path(path);
 
diff --git a/drivers/hwtracing/coresight/of_coresight.c b/drivers/hwtracing/coresight/of_coresight.c
index 09142e99e915..a18794128bf8 100644
--- a/drivers/hwtracing/coresight/of_coresight.c
+++ b/drivers/hwtracing/coresight/of_coresight.c
@@ -52,7 +52,7 @@ of_coresight_get_endpoint_device(struct device_node *endpoint)
 			       endpoint, of_dev_node_match);
 }
 
-static void of_coresight_get_ports(struct device_node *node,
+static void of_coresight_get_ports(const struct device_node *node,
 				   int *nr_inport, int *nr_outport)
 {
 	struct device_node *ep = NULL;
@@ -101,14 +101,40 @@ static int of_coresight_alloc_memory(struct device *dev,
 	return 0;
 }
 
-struct coresight_platform_data *of_get_coresight_platform_data(
-				struct device *dev, struct device_node *node)
+int of_coresight_get_cpu(const struct device_node *node)
 {
-	int i = 0, ret = 0, cpu;
+	int cpu;
+	bool found;
+	struct device_node *dn, *np;
+
+	dn = of_parse_phandle(node, "cpu", 0);
+
+	/* Affinity defaults to CPU0 */
+	if (!dn)
+		return 0;
+
+	for_each_possible_cpu(cpu) {
+		np = of_cpu_device_node_get(cpu);
+		found = (dn == np);
+		of_node_put(np);
+		if (found)
+			break;
+	}
+	of_node_put(dn);
+
+	/* Affinity to CPU0 if no cpu nodes are found */
+	return found ? cpu : 0;
+}
+EXPORT_SYMBOL_GPL(of_coresight_get_cpu);
+
+struct coresight_platform_data *
+of_get_coresight_platform_data(struct device *dev,
+			       const struct device_node *node)
+{
+	int i = 0, ret = 0;
 	struct coresight_platform_data *pdata;
 	struct of_endpoint endpoint, rendpoint;
 	struct device *rdev;
-	struct device_node *dn;
 	struct device_node *ep = NULL;
 	struct device_node *rparent = NULL;
 	struct device_node *rport = NULL;
@@ -175,16 +201,7 @@ struct coresight_platform_data *of_get_coresight_platform_data(
 		} while (ep);
 	}
 
-	/* Affinity defaults to CPU0 */
-	pdata->cpu = 0;
-	dn = of_parse_phandle(node, "cpu", 0);
-	for (cpu = 0; dn && cpu < nr_cpu_ids; cpu++) {
-		if (dn == of_get_cpu_node(cpu, NULL)) {
-			pdata->cpu = cpu;
-			break;
-		}
-	}
-	of_node_put(dn);
+	pdata->cpu = of_coresight_get_cpu(node);
 
 	return pdata;
 }
diff --git a/drivers/hwtracing/intel_th/core.c b/drivers/hwtracing/intel_th/core.c
index 7563eceeaaea..8da567abc0ce 100644
--- a/drivers/hwtracing/intel_th/core.c
+++ b/drivers/hwtracing/intel_th/core.c
@@ -139,7 +139,6 @@ static int intel_th_remove(struct device *dev)
 
 static struct bus_type intel_th_bus = {
 	.name		= "intel_th",
-	.dev_attrs	= NULL,
 	.match		= intel_th_match,
 	.probe		= intel_th_probe,
 	.remove		= intel_th_remove,
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index 95ed17183e73..54a47b40546f 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -734,9 +734,9 @@ static int i2c_imx_dma_read(struct imx_i2c_struct *i2c_imx,
 		 * the first read operation, otherwise the first read cost
 		 * one extra clock cycle.
 		 */
-		temp = readb(i2c_imx->base + IMX_I2C_I2CR);
+		temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
 		temp |= I2CR_MTX;
-		writeb(temp, i2c_imx->base + IMX_I2C_I2CR);
+		imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
 	}
 	msgs->buf[msgs->len-1] = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2DR);
 
@@ -857,9 +857,9 @@ static int i2c_imx_read(struct imx_i2c_struct *i2c_imx, struct i2c_msg *msgs, bo
 				 * the first read operation, otherwise the first read cost
 				 * one extra clock cycle.
 				 */
-				temp = readb(i2c_imx->base + IMX_I2C_I2CR);
+				temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
 				temp |= I2CR_MTX;
-				writeb(temp, i2c_imx->base + IMX_I2C_I2CR);
+				imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
 			}
 		} else if (i == (msgs->len - 2)) {
 			dev_dbg(&i2c_imx->adapter.dev,
diff --git a/drivers/i2c/busses/i2c-ismt.c b/drivers/i2c/busses/i2c-ismt.c
index f573448d2132..e98e44e584a4 100644
--- a/drivers/i2c/busses/i2c-ismt.c
+++ b/drivers/i2c/busses/i2c-ismt.c
@@ -584,7 +584,7 @@ static int ismt_access(struct i2c_adapter *adap, u16 addr,
 
 	/* unmap the data buffer */
 	if (dma_size != 0)
-		dma_unmap_single(&adap->dev, dma_addr, dma_size, dma_direction);
+		dma_unmap_single(dev, dma_addr, dma_size, dma_direction);
 
 	if (unlikely(!time_left)) {
 		dev_err(dev, "completion wait timed out\n");
diff --git a/drivers/i2c/busses/i2c-rcar.c b/drivers/i2c/busses/i2c-rcar.c
index 214bf2835d1f..8be3e6cb8fe6 100644
--- a/drivers/i2c/busses/i2c-rcar.c
+++ b/drivers/i2c/busses/i2c-rcar.c
@@ -319,7 +319,7 @@ static void rcar_i2c_dma_unmap(struct rcar_i2c_priv *priv)
 	rcar_i2c_write(priv, ICFBSCR, TCYC06);
 
 	dma_unmap_single(chan->device->dev, sg_dma_address(&priv->sg),
-			 priv->msg->len, priv->dma_direction);
+			 sg_dma_len(&priv->sg), priv->dma_direction);
 
 	priv->dma_direction = DMA_NONE;
 }
diff --git a/drivers/i2c/muxes/Kconfig b/drivers/i2c/muxes/Kconfig
index 1e160fc37ecc..2c64d0e0740f 100644
--- a/drivers/i2c/muxes/Kconfig
+++ b/drivers/i2c/muxes/Kconfig
@@ -30,6 +30,19 @@ config I2C_MUX_GPIO
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-mux-gpio.
 
+config I2C_MUX_GPMUX
+	tristate "General Purpose I2C multiplexer"
+	select MULTIPLEXER
+	depends on OF || COMPILE_TEST
+	help
+	  If you say yes to this option, support will be included for a
+	  general purpose I2C multiplexer. This driver provides access to
+	  I2C busses connected through a MUX, which in turn is controlled
+	  by a MUX-controller from the MUX subsystem.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called i2c-mux-gpmux.
+
 config I2C_MUX_LTC4306
 	tristate "LTC LTC4306/5 I2C multiplexer"
 	select GPIOLIB
diff --git a/drivers/i2c/muxes/Makefile b/drivers/i2c/muxes/Makefile
index ff7618cd5312..4a67d3199877 100644
--- a/drivers/i2c/muxes/Makefile
+++ b/drivers/i2c/muxes/Makefile
@@ -6,6 +6,7 @@ obj-$(CONFIG_I2C_ARB_GPIO_CHALLENGE)	+= i2c-arb-gpio-challenge.o
 obj-$(CONFIG_I2C_DEMUX_PINCTRL)		+= i2c-demux-pinctrl.o
 
 obj-$(CONFIG_I2C_MUX_GPIO)	+= i2c-mux-gpio.o
+obj-$(CONFIG_I2C_MUX_GPMUX)	+= i2c-mux-gpmux.o
 obj-$(CONFIG_I2C_MUX_LTC4306)	+= i2c-mux-ltc4306.o
 obj-$(CONFIG_I2C_MUX_MLXCPLD)	+= i2c-mux-mlxcpld.o
 obj-$(CONFIG_I2C_MUX_PCA9541)	+= i2c-mux-pca9541.o
diff --git a/drivers/i2c/muxes/i2c-mux-gpmux.c b/drivers/i2c/muxes/i2c-mux-gpmux.c
new file mode 100644
index 000000000000..92cf5f48afe6
--- /dev/null
+++ b/drivers/i2c/muxes/i2c-mux-gpmux.c
@@ -0,0 +1,173 @@
+/*
+ * General Purpose I2C multiplexer
+ *
+ * Copyright (C) 2017 Axentia Technologies AB
+ *
+ * Author: Peter Rosin <peda@axentia.se>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/i2c.h>
+#include <linux/i2c-mux.h>
+#include <linux/module.h>
+#include <linux/mux/consumer.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+
+struct mux {
+	struct mux_control *control;
+
+	bool do_not_deselect;
+};
+
+static int i2c_mux_select(struct i2c_mux_core *muxc, u32 chan)
+{
+	struct mux *mux = i2c_mux_priv(muxc);
+	int ret;
+
+	ret = mux_control_select(mux->control, chan);
+	mux->do_not_deselect = ret < 0;
+
+	return ret;
+}
+
+static int i2c_mux_deselect(struct i2c_mux_core *muxc, u32 chan)
+{
+	struct mux *mux = i2c_mux_priv(muxc);
+
+	if (mux->do_not_deselect)
+		return 0;
+
+	return mux_control_deselect(mux->control);
+}
+
+static struct i2c_adapter *mux_parent_adapter(struct device *dev)
+{
+	struct device_node *np = dev->of_node;
+	struct device_node *parent_np;
+	struct i2c_adapter *parent;
+
+	parent_np = of_parse_phandle(np, "i2c-parent", 0);
+	if (!parent_np) {
+		dev_err(dev, "Cannot parse i2c-parent\n");
+		return ERR_PTR(-ENODEV);
+	}
+	parent = of_find_i2c_adapter_by_node(parent_np);
+	of_node_put(parent_np);
+	if (!parent)
+		return ERR_PTR(-EPROBE_DEFER);
+
+	return parent;
+}
+
+static const struct of_device_id i2c_mux_of_match[] = {
+	{ .compatible = "i2c-mux", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, i2c_mux_of_match);
+
+static int i2c_mux_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct device_node *child;
+	struct i2c_mux_core *muxc;
+	struct mux *mux;
+	struct i2c_adapter *parent;
+	int children;
+	int ret;
+
+	if (!np)
+		return -ENODEV;
+
+	mux = devm_kzalloc(dev, sizeof(*mux), GFP_KERNEL);
+	if (!mux)
+		return -ENOMEM;
+
+	mux->control = devm_mux_control_get(dev, NULL);
+	if (IS_ERR(mux->control)) {
+		if (PTR_ERR(mux->control) != -EPROBE_DEFER)
+			dev_err(dev, "failed to get control-mux\n");
+		return PTR_ERR(mux->control);
+	}
+
+	parent = mux_parent_adapter(dev);
+	if (IS_ERR(parent)) {
+		if (PTR_ERR(parent) != -EPROBE_DEFER)
+			dev_err(dev, "failed to get i2c-parent adapter\n");
+		return PTR_ERR(parent);
+	}
+
+	children = of_get_child_count(np);
+
+	muxc = i2c_mux_alloc(parent, dev, children, 0, 0,
+			     i2c_mux_select, i2c_mux_deselect);
+	if (!muxc) {
+		ret = -ENOMEM;
+		goto err_parent;
+	}
+	muxc->priv = mux;
+
+	platform_set_drvdata(pdev, muxc);
+
+	muxc->mux_locked = of_property_read_bool(np, "mux-locked");
+
+	for_each_child_of_node(np, child) {
+		u32 chan;
+
+		ret = of_property_read_u32(child, "reg", &chan);
+		if (ret < 0) {
+			dev_err(dev, "no reg property for node '%s'\n",
+				child->name);
+			goto err_children;
+		}
+
+		if (chan >= mux_control_states(mux->control)) {
+			dev_err(dev, "invalid reg %u\n", chan);
+			ret = -EINVAL;
+			goto err_children;
+		}
+
+		ret = i2c_mux_add_adapter(muxc, 0, chan, 0);
+		if (ret)
+			goto err_children;
+	}
+
+	dev_info(dev, "%d-port mux on %s adapter\n", children, parent->name);
+
+	return 0;
+
+err_children:
+	i2c_mux_del_adapters(muxc);
+err_parent:
+	i2c_put_adapter(parent);
+
+	return ret;
+}
+
+static int i2c_mux_remove(struct platform_device *pdev)
+{
+	struct i2c_mux_core *muxc = platform_get_drvdata(pdev);
+
+	i2c_mux_del_adapters(muxc);
+	i2c_put_adapter(muxc->parent);
+
+	return 0;
+}
+
+static struct platform_driver i2c_mux_driver = {
+	.probe	= i2c_mux_probe,
+	.remove	= i2c_mux_remove,
+	.driver	= {
+		.name	= "i2c-mux-gpmux",
+		.of_match_table = i2c_mux_of_match,
+	},
+};
+module_platform_driver(i2c_mux_driver);
+
+MODULE_DESCRIPTION("General Purpose I2C multiplexer driver");
+MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/ide/ide-atapi.c b/drivers/ide/ide-atapi.c
index 5901937284e7..14d1e7d9a1d6 100644
--- a/drivers/ide/ide-atapi.c
+++ b/drivers/ide/ide-atapi.c
@@ -93,7 +93,6 @@ int ide_queue_pc_tail(ide_drive_t *drive, struct gendisk *disk,
 	int error;
 
 	rq = blk_get_request(drive->queue, REQ_OP_DRV_IN, __GFP_RECLAIM);
-	scsi_req_init(rq);
 	ide_req(rq)->type = ATA_PRIV_MISC;
 	rq->special = (char *)pc;
 
@@ -200,7 +199,7 @@ void ide_prep_sense(ide_drive_t *drive, struct request *rq)
 	memset(sense, 0, sizeof(*sense));
 
 	blk_rq_init(rq->q, sense_rq);
-	scsi_req_init(sense_rq);
+	scsi_req_init(req);
 
 	err = blk_rq_map_kern(drive->queue, sense_rq, sense, sense_len,
 			      GFP_NOIO);
@@ -273,7 +272,7 @@ void ide_retry_pc(ide_drive_t *drive)
 	ide_requeue_and_plug(drive, failed_rq);
 	if (ide_queue_sense_rq(drive, pc)) {
 		blk_start_request(failed_rq);
-		ide_complete_rq(drive, -EIO, blk_rq_bytes(failed_rq));
+		ide_complete_rq(drive, BLK_STS_IOERR, blk_rq_bytes(failed_rq));
 	}
 }
 EXPORT_SYMBOL_GPL(ide_retry_pc);
@@ -437,7 +436,8 @@ static ide_startstop_t ide_pc_intr(ide_drive_t *drive)
 
 	/* No more interrupts */
 	if ((stat & ATA_DRQ) == 0) {
-		int uptodate, error;
+		int uptodate;
+		blk_status_t error;
 
 		debug_log("Packet command completed, %d bytes transferred\n",
 			  blk_rq_bytes(rq));
@@ -490,7 +490,7 @@ static ide_startstop_t ide_pc_intr(ide_drive_t *drive)
 
 		if (ata_misc_request(rq)) {
 			scsi_req(rq)->result = 0;
-			error = 0;
+			error = BLK_STS_OK;
 		} else {
 
 			if (blk_rq_is_passthrough(rq) && uptodate <= 0) {
@@ -498,7 +498,7 @@ static ide_startstop_t ide_pc_intr(ide_drive_t *drive)
 					scsi_req(rq)->result = -EIO;
 			}
 
-			error = uptodate ? 0 : -EIO;
+			error = uptodate ? BLK_STS_OK : BLK_STS_IOERR;
 		}
 
 		ide_complete_rq(drive, error, blk_rq_bytes(rq));
diff --git a/drivers/ide/ide-cd.c b/drivers/ide/ide-cd.c
index 07e5ff3a64c3..81e18f9628d0 100644
--- a/drivers/ide/ide-cd.c
+++ b/drivers/ide/ide-cd.c
@@ -228,7 +228,7 @@ static void ide_cd_complete_failed_rq(ide_drive_t *drive, struct request *rq)
 		scsi_req(failed)->sense_len = scsi_req(rq)->sense_len;
 		cdrom_analyze_sense_data(drive, failed);
 
-		if (ide_end_rq(drive, failed, -EIO, blk_rq_bytes(failed)))
+		if (ide_end_rq(drive, failed, BLK_STS_IOERR, blk_rq_bytes(failed)))
 			BUG();
 	} else
 		cdrom_analyze_sense_data(drive, NULL);
@@ -438,7 +438,6 @@ int ide_cd_queue_pc(ide_drive_t *drive, const unsigned char *cmd,
 
 		rq = blk_get_request(drive->queue,
 			write ? REQ_OP_DRV_OUT : REQ_OP_DRV_IN,  __GFP_RECLAIM);
-		scsi_req_init(rq);
 		memcpy(scsi_req(rq)->cmd, cmd, BLK_MAX_CDB);
 		ide_req(rq)->type = ATA_PRIV_PC;
 		rq->rq_flags |= rq_flags;
@@ -508,7 +507,7 @@ static bool ide_cd_error_cmd(ide_drive_t *drive, struct ide_cmd *cmd)
 		nr_bytes -= cmd->last_xfer_len;
 
 	if (nr_bytes > 0) {
-		ide_complete_rq(drive, 0, nr_bytes);
+		ide_complete_rq(drive, BLK_STS_OK, nr_bytes);
 		return true;
 	}
 
@@ -674,7 +673,7 @@ static ide_startstop_t cdrom_newpc_intr(ide_drive_t *drive)
 out_end:
 	if (blk_rq_is_scsi(rq) && rc == 0) {
 		scsi_req(rq)->resid_len = 0;
-		blk_end_request_all(rq, 0);
+		blk_end_request_all(rq, BLK_STS_OK);
 		hwif->rq = NULL;
 	} else {
 		if (sense && uptodate)
@@ -699,7 +698,7 @@ out_end:
 				scsi_req(rq)->resid_len += cmd->last_xfer_len;
 		}
 
-		ide_complete_rq(drive, uptodate ? 0 : -EIO, blk_rq_bytes(rq));
+		ide_complete_rq(drive, uptodate ? BLK_STS_OK : BLK_STS_IOERR, blk_rq_bytes(rq));
 
 		if (sense && rc == 2)
 			ide_error(drive, "request sense failure", stat);
@@ -844,7 +843,7 @@ out_end:
 	if (nsectors == 0)
 		nsectors = 1;
 
-	ide_complete_rq(drive, uptodate ? 0 : -EIO, nsectors << 9);
+	ide_complete_rq(drive, uptodate ? BLK_STS_OK : BLK_STS_IOERR, nsectors << 9);
 
 	return ide_stopped;
 }
diff --git a/drivers/ide/ide-cd_ioctl.c b/drivers/ide/ide-cd_ioctl.c
index 55cd736c39c6..9d26c9737e21 100644
--- a/drivers/ide/ide-cd_ioctl.c
+++ b/drivers/ide/ide-cd_ioctl.c
@@ -304,7 +304,6 @@ int ide_cdrom_reset(struct cdrom_device_info *cdi)
 	int ret;
 
 	rq = blk_get_request(drive->queue, REQ_OP_DRV_IN, __GFP_RECLAIM);
-	scsi_req_init(rq);
 	ide_req(rq)->type = ATA_PRIV_MISC;
 	rq->rq_flags = RQF_QUIET;
 	blk_execute_rq(drive->queue, cd->disk, rq, 0);
diff --git a/drivers/ide/ide-devsets.c b/drivers/ide/ide-devsets.c
index 9b69c32ee560..ef7c8c43a380 100644
--- a/drivers/ide/ide-devsets.c
+++ b/drivers/ide/ide-devsets.c
@@ -166,7 +166,6 @@ int ide_devset_execute(ide_drive_t *drive, const struct ide_devset *setting,
 		return setting->set(drive, arg);
 
 	rq = blk_get_request(q, REQ_OP_DRV_IN, __GFP_RECLAIM);
-	scsi_req_init(rq);
 	ide_req(rq)->type = ATA_PRIV_MISC;
 	scsi_req(rq)->cmd_len = 5;
 	scsi_req(rq)->cmd[0] = REQ_DEVSET_EXEC;
diff --git a/drivers/ide/ide-disk.c b/drivers/ide/ide-disk.c
index 7c06237f3479..241983da5fc4 100644
--- a/drivers/ide/ide-disk.c
+++ b/drivers/ide/ide-disk.c
@@ -478,7 +478,6 @@ static int set_multcount(ide_drive_t *drive, int arg)
 		return -EBUSY;
 
 	rq = blk_get_request(drive->queue, REQ_OP_DRV_IN, __GFP_RECLAIM);
-	scsi_req_init(rq);
 	ide_req(rq)->type = ATA_PRIV_TASKFILE;
 
 	drive->mult_req = arg;
diff --git a/drivers/ide/ide-dma.c b/drivers/ide/ide-dma.c
index 51c81223e56d..54d4d78ca46a 100644
--- a/drivers/ide/ide-dma.c
+++ b/drivers/ide/ide-dma.c
@@ -104,7 +104,7 @@ ide_startstop_t ide_dma_intr(ide_drive_t *drive)
 			if ((cmd->tf_flags & IDE_TFLAG_FS) == 0)
 				ide_finish_cmd(drive, cmd, stat);
 			else
-				ide_complete_rq(drive, 0,
+				ide_complete_rq(drive, BLK_STS_OK,
 						blk_rq_sectors(cmd->rq) << 9);
 			return ide_stopped;
 		}
diff --git a/drivers/ide/ide-eh.c b/drivers/ide/ide-eh.c
index 4b7ffd7d158d..47d5f3379748 100644
--- a/drivers/ide/ide-eh.c
+++ b/drivers/ide/ide-eh.c
@@ -135,7 +135,7 @@ ide_startstop_t ide_error(ide_drive_t *drive, const char *msg, u8 stat)
 			return ide_stopped;
 		}
 		scsi_req(rq)->result = err;
-		ide_complete_rq(drive, err ? -EIO : 0, blk_rq_bytes(rq));
+		ide_complete_rq(drive, err ? BLK_STS_IOERR : BLK_STS_OK, blk_rq_bytes(rq));
 		return ide_stopped;
 	}
 
@@ -143,7 +143,7 @@ ide_startstop_t ide_error(ide_drive_t *drive, const char *msg, u8 stat)
 }
 EXPORT_SYMBOL_GPL(ide_error);
 
-static inline void ide_complete_drive_reset(ide_drive_t *drive, int err)
+static inline void ide_complete_drive_reset(ide_drive_t *drive, blk_status_t err)
 {
 	struct request *rq = drive->hwif->rq;
 
@@ -151,7 +151,7 @@ static inline void ide_complete_drive_reset(ide_drive_t *drive, int err)
 	    scsi_req(rq)->cmd[0] == REQ_DRIVE_RESET) {
 		if (err <= 0 && scsi_req(rq)->result == 0)
 			scsi_req(rq)->result = -EIO;
-		ide_complete_rq(drive, err ? err : 0, blk_rq_bytes(rq));
+		ide_complete_rq(drive, err, blk_rq_bytes(rq));
 	}
 }
 
@@ -191,7 +191,7 @@ static ide_startstop_t atapi_reset_pollfunc(ide_drive_t *drive)
 	}
 	/* done polling */
 	hwif->polling = 0;
-	ide_complete_drive_reset(drive, 0);
+	ide_complete_drive_reset(drive, BLK_STS_OK);
 	return ide_stopped;
 }
 
@@ -225,7 +225,7 @@ static ide_startstop_t reset_pollfunc(ide_drive_t *drive)
 	ide_hwif_t *hwif = drive->hwif;
 	const struct ide_port_ops *port_ops = hwif->port_ops;
 	u8 tmp;
-	int err = 0;
+	blk_status_t err = BLK_STS_OK;
 
 	if (port_ops && port_ops->reset_poll) {
 		err = port_ops->reset_poll(drive);
@@ -247,7 +247,7 @@ static ide_startstop_t reset_pollfunc(ide_drive_t *drive)
 		printk(KERN_ERR "%s: reset timed-out, status=0x%02x\n",
 			hwif->name, tmp);
 		drive->failures++;
-		err = -EIO;
+		err = BLK_STS_IOERR;
 	} else  {
 		tmp = ide_read_error(drive);
 
@@ -257,7 +257,7 @@ static ide_startstop_t reset_pollfunc(ide_drive_t *drive)
 		} else {
 			ide_reset_report_error(hwif, tmp);
 			drive->failures++;
-			err = -EIO;
+			err = BLK_STS_IOERR;
 		}
 	}
 out:
@@ -392,7 +392,7 @@ static ide_startstop_t do_reset1(ide_drive_t *drive, int do_not_try_atapi)
 
 	if (io_ports->ctl_addr == 0) {
 		spin_unlock_irqrestore(&hwif->lock, flags);
-		ide_complete_drive_reset(drive, -ENXIO);
+		ide_complete_drive_reset(drive, BLK_STS_IOERR);
 		return ide_stopped;
 	}
 
diff --git a/drivers/ide/ide-floppy.c b/drivers/ide/ide-floppy.c
index 8ac6048cd2df..627b1f62a749 100644
--- a/drivers/ide/ide-floppy.c
+++ b/drivers/ide/ide-floppy.c
@@ -143,7 +143,7 @@ static ide_startstop_t ide_floppy_issue_pc(ide_drive_t *drive,
 
 		drive->failed_pc = NULL;
 		drive->pc_callback(drive, 0);
-		ide_complete_rq(drive, -EIO, done);
+		ide_complete_rq(drive, BLK_STS_IOERR, done);
 		return ide_stopped;
 	}
 
@@ -248,7 +248,7 @@ static ide_startstop_t ide_floppy_do_request(ide_drive_t *drive,
 
 		if (ata_misc_request(rq)) {
 			scsi_req(rq)->result = 0;
-			ide_complete_rq(drive, 0, blk_rq_bytes(rq));
+			ide_complete_rq(drive, BLK_STS_OK, blk_rq_bytes(rq));
 			return ide_stopped;
 		} else
 			goto out_end;
@@ -303,7 +303,7 @@ out_end:
 	drive->failed_pc = NULL;
 	if (blk_rq_is_passthrough(rq) && scsi_req(rq)->result == 0)
 		scsi_req(rq)->result = -EIO;
-	ide_complete_rq(drive, -EIO, blk_rq_bytes(rq));
+	ide_complete_rq(drive, BLK_STS_IOERR, blk_rq_bytes(rq));
 	return ide_stopped;
 }
 
diff --git a/drivers/ide/ide-io.c b/drivers/ide/ide-io.c
index 323af721f8cb..3a234701d92c 100644
--- a/drivers/ide/ide-io.c
+++ b/drivers/ide/ide-io.c
@@ -54,7 +54,7 @@
 #include <linux/uaccess.h>
 #include <asm/io.h>
 
-int ide_end_rq(ide_drive_t *drive, struct request *rq, int error,
+int ide_end_rq(ide_drive_t *drive, struct request *rq, blk_status_t error,
 	       unsigned int nr_bytes)
 {
 	/*
@@ -112,7 +112,7 @@ void ide_complete_cmd(ide_drive_t *drive, struct ide_cmd *cmd, u8 stat, u8 err)
 	}
 }
 
-int ide_complete_rq(ide_drive_t *drive, int error, unsigned int nr_bytes)
+int ide_complete_rq(ide_drive_t *drive, blk_status_t error, unsigned int nr_bytes)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	struct request *rq = hwif->rq;
@@ -122,7 +122,7 @@ int ide_complete_rq(ide_drive_t *drive, int error, unsigned int nr_bytes)
 	 * if failfast is set on a request, override number of sectors
 	 * and complete the whole request right now
 	 */
-	if (blk_noretry_request(rq) && error <= 0)
+	if (blk_noretry_request(rq) && error)
 		nr_bytes = blk_rq_sectors(rq) << 9;
 
 	rc = ide_end_rq(drive, rq, error, nr_bytes);
@@ -149,7 +149,7 @@ void ide_kill_rq(ide_drive_t *drive, struct request *rq)
 			scsi_req(rq)->result = -EIO;
 	}
 
-	ide_complete_rq(drive, -EIO, blk_rq_bytes(rq));
+	ide_complete_rq(drive, BLK_STS_IOERR, blk_rq_bytes(rq));
 }
 
 static void ide_tf_set_specify_cmd(ide_drive_t *drive, struct ide_taskfile *tf)
@@ -272,7 +272,7 @@ static ide_startstop_t execute_drive_cmd (ide_drive_t *drive,
  	printk("%s: DRIVE_CMD (null)\n", drive->name);
 #endif
 	scsi_req(rq)->result = 0;
-	ide_complete_rq(drive, 0, blk_rq_bytes(rq));
+	ide_complete_rq(drive, BLK_STS_OK, blk_rq_bytes(rq));
 
  	return ide_stopped;
 }
diff --git a/drivers/ide/ide-ioctls.c b/drivers/ide/ide-ioctls.c
index 8c0d17297a7a..3661abb16a5f 100644
--- a/drivers/ide/ide-ioctls.c
+++ b/drivers/ide/ide-ioctls.c
@@ -126,7 +126,6 @@ static int ide_cmd_ioctl(ide_drive_t *drive, unsigned long arg)
 		struct request *rq;
 
 		rq = blk_get_request(drive->queue, REQ_OP_DRV_IN, __GFP_RECLAIM);
-		scsi_req_init(rq);
 		ide_req(rq)->type = ATA_PRIV_TASKFILE;
 		blk_execute_rq(drive->queue, NULL, rq, 0);
 		err = scsi_req(rq)->result ? -EIO : 0;
@@ -224,7 +223,6 @@ static int generic_drive_reset(ide_drive_t *drive)
 	int ret = 0;
 
 	rq = blk_get_request(drive->queue, REQ_OP_DRV_IN, __GFP_RECLAIM);
-	scsi_req_init(rq);
 	ide_req(rq)->type = ATA_PRIV_MISC;
 	scsi_req(rq)->cmd_len = 1;
 	scsi_req(rq)->cmd[0] = REQ_DRIVE_RESET;
diff --git a/drivers/ide/ide-park.c b/drivers/ide/ide-park.c
index 94e3107f59b9..1f264d5d3f3f 100644
--- a/drivers/ide/ide-park.c
+++ b/drivers/ide/ide-park.c
@@ -32,7 +32,6 @@ static void issue_park_cmd(ide_drive_t *drive, unsigned long timeout)
 	spin_unlock_irq(&hwif->lock);
 
 	rq = blk_get_request(q, REQ_OP_DRV_IN, __GFP_RECLAIM);
-	scsi_req_init(rq);
 	scsi_req(rq)->cmd[0] = REQ_PARK_HEADS;
 	scsi_req(rq)->cmd_len = 1;
 	ide_req(rq)->type = ATA_PRIV_MISC;
@@ -48,7 +47,6 @@ static void issue_park_cmd(ide_drive_t *drive, unsigned long timeout)
 	 * timeout has expired, so power management will be reenabled.
 	 */
 	rq = blk_get_request(q, REQ_OP_DRV_IN, GFP_NOWAIT);
-	scsi_req_init(rq);
 	if (IS_ERR(rq))
 		goto out;
 
diff --git a/drivers/ide/ide-pm.c b/drivers/ide/ide-pm.c
index 0977fc1f40ce..544f02d673ca 100644
--- a/drivers/ide/ide-pm.c
+++ b/drivers/ide/ide-pm.c
@@ -19,7 +19,6 @@ int generic_ide_suspend(struct device *dev, pm_message_t mesg)
 
 	memset(&rqpm, 0, sizeof(rqpm));
 	rq = blk_get_request(drive->queue, REQ_OP_DRV_IN, __GFP_RECLAIM);
-	scsi_req_init(rq);
 	ide_req(rq)->type = ATA_PRIV_PM_SUSPEND;
 	rq->special = &rqpm;
 	rqpm.pm_step = IDE_PM_START_SUSPEND;
@@ -40,7 +39,7 @@ int generic_ide_suspend(struct device *dev, pm_message_t mesg)
 	return ret;
 }
 
-static void ide_end_sync_rq(struct request *rq, int error)
+static void ide_end_sync_rq(struct request *rq, blk_status_t error)
 {
 	complete(rq->end_io_data);
 }
@@ -57,7 +56,7 @@ static int ide_pm_execute_rq(struct request *rq)
 	if (unlikely(blk_queue_dying(q))) {
 		rq->rq_flags |= RQF_QUIET;
 		scsi_req(rq)->result = -ENXIO;
-		__blk_end_request_all(rq, 0);
+		__blk_end_request_all(rq, BLK_STS_OK);
 		spin_unlock_irq(q->queue_lock);
 		return -ENXIO;
 	}
@@ -91,7 +90,6 @@ int generic_ide_resume(struct device *dev)
 
 	memset(&rqpm, 0, sizeof(rqpm));
 	rq = blk_get_request(drive->queue, REQ_OP_DRV_IN, __GFP_RECLAIM);
-	scsi_req_init(rq);
 	ide_req(rq)->type = ATA_PRIV_PM_RESUME;
 	rq->rq_flags |= RQF_PREEMPT;
 	rq->special = &rqpm;
@@ -235,7 +233,7 @@ void ide_complete_pm_rq(ide_drive_t *drive, struct request *rq)
 
 	drive->hwif->rq = NULL;
 
-	if (blk_end_request(rq, 0, 0))
+	if (blk_end_request(rq, BLK_STS_OK, 0))
 		BUG();
 }
 
diff --git a/drivers/ide/ide-probe.c b/drivers/ide/ide-probe.c
index 023562565d11..01b2adfd8226 100644
--- a/drivers/ide/ide-probe.c
+++ b/drivers/ide/ide-probe.c
@@ -741,12 +741,12 @@ static void ide_port_tune_devices(ide_hwif_t *hwif)
 	}
 }
 
-static int ide_init_rq(struct request_queue *q, struct request *rq, gfp_t gfp)
+static void ide_initialize_rq(struct request *rq)
 {
 	struct ide_request *req = blk_mq_rq_to_pdu(rq);
 
+	scsi_req_init(&req->sreq);
 	req->sreq.sense = req->sense;
-	return 0;
 }
 
 /*
@@ -771,8 +771,9 @@ static int ide_init_queue(ide_drive_t *drive)
 		return 1;
 
 	q->request_fn = do_ide_request;
-	q->init_rq_fn = ide_init_rq;
+	q->initialize_rq_fn = ide_initialize_rq;
 	q->cmd_size = sizeof(struct ide_request);
+	queue_flag_set_unlocked(QUEUE_FLAG_SCSI_PASSTHROUGH, q);
 	if (blk_init_allocated_queue(q) < 0) {
 		blk_cleanup_queue(q);
 		return 1;
diff --git a/drivers/ide/ide-tape.c b/drivers/ide/ide-tape.c
index a0651f948b76..fd57e8ccc47a 100644
--- a/drivers/ide/ide-tape.c
+++ b/drivers/ide/ide-tape.c
@@ -474,7 +474,7 @@ static ide_startstop_t ide_tape_issue_pc(ide_drive_t *drive,
 
 		drive->failed_pc = NULL;
 		drive->pc_callback(drive, 0);
-		ide_complete_rq(drive, -EIO, blk_rq_bytes(rq));
+		ide_complete_rq(drive, BLK_STS_IOERR, blk_rq_bytes(rq));
 		return ide_stopped;
 	}
 	ide_debug_log(IDE_DBG_SENSE, "retry #%d, cmd: 0x%02x", pc->retries,
@@ -855,7 +855,6 @@ static int idetape_queue_rw_tail(ide_drive_t *drive, int cmd, int size)
 	BUG_ON(size < 0 || size % tape->blk_size);
 
 	rq = blk_get_request(drive->queue, REQ_OP_DRV_IN, __GFP_RECLAIM);
-	scsi_req_init(rq);
 	ide_req(rq)->type = ATA_PRIV_MISC;
 	scsi_req(rq)->cmd[13] = cmd;
 	rq->rq_disk = tape->disk;
diff --git a/drivers/ide/ide-taskfile.c b/drivers/ide/ide-taskfile.c
index d71199d23c9e..4efe4c6e956c 100644
--- a/drivers/ide/ide-taskfile.c
+++ b/drivers/ide/ide-taskfile.c
@@ -318,7 +318,7 @@ static void ide_error_cmd(ide_drive_t *drive, struct ide_cmd *cmd)
 		}
 
 		if (nr_bytes > 0)
-			ide_complete_rq(drive, 0, nr_bytes);
+			ide_complete_rq(drive, BLK_STS_OK, nr_bytes);
 	}
 }
 
@@ -336,7 +336,7 @@ void ide_finish_cmd(ide_drive_t *drive, struct ide_cmd *cmd, u8 stat)
 		ide_driveid_update(drive);
 	}
 
-	ide_complete_rq(drive, err ? -EIO : 0, blk_rq_bytes(rq));
+	ide_complete_rq(drive, err ? BLK_STS_IOERR : BLK_STS_OK, blk_rq_bytes(rq));
 }
 
 /*
@@ -394,7 +394,7 @@ out_end:
 	if ((cmd->tf_flags & IDE_TFLAG_FS) == 0)
 		ide_finish_cmd(drive, cmd, stat);
 	else
-		ide_complete_rq(drive, 0, blk_rq_sectors(cmd->rq) << 9);
+		ide_complete_rq(drive, BLK_STS_OK, blk_rq_sectors(cmd->rq) << 9);
 	return ide_stopped;
 out_err:
 	ide_error_cmd(drive, cmd);
@@ -433,7 +433,6 @@ int ide_raw_taskfile(ide_drive_t *drive, struct ide_cmd *cmd, u8 *buf,
 	rq = blk_get_request(drive->queue,
 		(cmd->tf_flags & IDE_TFLAG_WRITE) ?
 			REQ_OP_DRV_OUT : REQ_OP_DRV_IN, __GFP_RECLAIM);
-	scsi_req_init(rq);
 	ide_req(rq)->type = ATA_PRIV_TASKFILE;
 
 	/*
diff --git a/drivers/ide/siimage.c b/drivers/ide/siimage.c
index 6a1849bb476c..57eea5a9047f 100644
--- a/drivers/ide/siimage.c
+++ b/drivers/ide/siimage.c
@@ -406,7 +406,7 @@ static int siimage_dma_test_irq(ide_drive_t *drive)
  *	yet.
  */
 
-static int sil_sata_reset_poll(ide_drive_t *drive)
+static blk_status_t sil_sata_reset_poll(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	void __iomem *sata_status_addr
@@ -419,11 +419,11 @@ static int sil_sata_reset_poll(ide_drive_t *drive)
 		if ((sata_stat & 0x03) != 0x03) {
 			printk(KERN_WARNING "%s: reset phy dead, status=0x%08x\n",
 					    hwif->name, sata_stat);
-			return -ENXIO;
+			return BLK_STS_IOERR;
 		}
 	}
 
-	return 0;
+	return BLK_STS_OK;
 }
 
 /**
diff --git a/drivers/idle/intel_idle.c b/drivers/idle/intel_idle.c
index 216d7ec88c0c..c2ae819a871c 100644
--- a/drivers/idle/intel_idle.c
+++ b/drivers/idle/intel_idle.c
@@ -51,6 +51,8 @@
 /* un-comment DEBUG to enable pr_debug() statements */
 #define DEBUG
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/cpuidle.h>
 #include <linux/tick.h>
@@ -65,7 +67,6 @@
 #include <asm/msr.h>
 
 #define INTEL_IDLE_VERSION "0.4.1"
-#define PREFIX "intel_idle: "
 
 static struct cpuidle_driver intel_idle_driver = {
 	.name = "intel_idle",
@@ -1111,7 +1112,7 @@ static int __init intel_idle_probe(void)
 	const struct x86_cpu_id *id;
 
 	if (max_cstate == 0) {
-		pr_debug(PREFIX "disabled\n");
+		pr_debug("disabled\n");
 		return -EPERM;
 	}
 
@@ -1119,8 +1120,8 @@ static int __init intel_idle_probe(void)
 	if (!id) {
 		if (boot_cpu_data.x86_vendor == X86_VENDOR_INTEL &&
 		    boot_cpu_data.x86 == 6)
-			pr_debug(PREFIX "does not run on family %d model %d\n",
-				boot_cpu_data.x86, boot_cpu_data.x86_model);
+			pr_debug("does not run on family %d model %d\n",
+				 boot_cpu_data.x86, boot_cpu_data.x86_model);
 		return -ENODEV;
 	}
 
@@ -1134,13 +1135,13 @@ static int __init intel_idle_probe(void)
 	    !mwait_substates)
 			return -ENODEV;
 
-	pr_debug(PREFIX "MWAIT substates: 0x%x\n", mwait_substates);
+	pr_debug("MWAIT substates: 0x%x\n", mwait_substates);
 
 	icpu = (const struct idle_cpu *)id->driver_data;
 	cpuidle_state_table = icpu->state_table;
 
-	pr_debug(PREFIX "v" INTEL_IDLE_VERSION
-		" model 0x%X\n", boot_cpu_data.x86_model);
+	pr_debug("v" INTEL_IDLE_VERSION " model 0x%X\n",
+		 boot_cpu_data.x86_model);
 
 	return 0;
 }
@@ -1340,8 +1341,7 @@ static void __init intel_idle_cpuidle_driver_init(void)
 			break;
 
 		if (cstate + 1 > max_cstate) {
-			printk(PREFIX "max_cstate %d reached\n",
-				max_cstate);
+			pr_info("max_cstate %d reached\n", max_cstate);
 			break;
 		}
 
@@ -1358,8 +1358,8 @@ static void __init intel_idle_cpuidle_driver_init(void)
 
 		/* if state marked as disabled, skip it */
 		if (cpuidle_state_table[cstate].disabled != 0) {
-			pr_debug(PREFIX "state %s is disabled",
-				cpuidle_state_table[cstate].name);
+			pr_debug("state %s is disabled\n",
+				 cpuidle_state_table[cstate].name);
 			continue;
 		}
 
@@ -1395,7 +1395,7 @@ static int intel_idle_cpu_init(unsigned int cpu)
 	dev->cpu = cpu;
 
 	if (cpuidle_register_device(dev)) {
-		pr_debug(PREFIX "cpuidle_register_device %d failed!\n", cpu);
+		pr_debug("cpuidle_register_device %d failed!\n", cpu);
 		return -EIO;
 	}
 
@@ -1447,8 +1447,8 @@ static int __init intel_idle_init(void)
 	retval = cpuidle_register_driver(&intel_idle_driver);
 	if (retval) {
 		struct cpuidle_driver *drv = cpuidle_get_driver();
-		printk(KERN_DEBUG PREFIX "intel_idle yielding to %s",
-			drv ? drv->name : "none");
+		printk(KERN_DEBUG pr_fmt("intel_idle yielding to %s\n"),
+		       drv ? drv->name : "none");
 		goto init_driver_fail;
 	}
 
@@ -1460,8 +1460,8 @@ static int __init intel_idle_init(void)
 	if (retval < 0)
 		goto hp_setup_fail;
 
-	pr_debug(PREFIX "lapic_timer_reliable_states 0x%x\n",
-		lapic_timer_reliable_states);
+	pr_debug("lapic_timer_reliable_states 0x%x\n",
+		 lapic_timer_reliable_states);
 
 	return 0;
 
diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig
index a918270d6f54..b3c8c6ef0dff 100644
--- a/drivers/iio/Kconfig
+++ b/drivers/iio/Kconfig
@@ -83,6 +83,7 @@ source "drivers/iio/humidity/Kconfig"
 source "drivers/iio/imu/Kconfig"
 source "drivers/iio/light/Kconfig"
 source "drivers/iio/magnetometer/Kconfig"
+source "drivers/iio/multiplexer/Kconfig"
 source "drivers/iio/orientation/Kconfig"
 if IIO_TRIGGER
    source "drivers/iio/trigger/Kconfig"
diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile
index 33fa4026f92c..93c769cd99bf 100644
--- a/drivers/iio/Makefile
+++ b/drivers/iio/Makefile
@@ -28,6 +28,7 @@ obj-y += humidity/
 obj-y += imu/
 obj-y += light/
 obj-y += magnetometer/
+obj-y += multiplexer/
 obj-y += orientation/
 obj-y += potentiometer/
 obj-y += potentiostat/
diff --git a/drivers/iio/accel/hid-sensor-accel-3d.c b/drivers/iio/accel/hid-sensor-accel-3d.c
index 43a6cb078193..2238a26aba63 100644
--- a/drivers/iio/accel/hid-sensor-accel-3d.c
+++ b/drivers/iio/accel/hid-sensor-accel-3d.c
@@ -347,7 +347,7 @@ static int accel_3d_parse_report(struct platform_device *pdev,
 static int hid_accel_3d_probe(struct platform_device *pdev)
 {
 	int ret = 0;
-	static const char *name;
+	const char *name;
 	struct iio_dev *indio_dev;
 	struct accel_3d_state *accel_state;
 	const struct iio_chan_spec *channel_spec;
diff --git a/drivers/iio/accel/mma9551.c b/drivers/iio/accel/mma9551.c
index bf2704435629..1f53f08476f5 100644
--- a/drivers/iio/accel/mma9551.c
+++ b/drivers/iio/accel/mma9551.c
@@ -27,7 +27,6 @@
 
 #define MMA9551_DRV_NAME		"mma9551"
 #define MMA9551_IRQ_NAME		"mma9551_event"
-#define MMA9551_GPIO_NAME		"mma9551_int"
 #define MMA9551_GPIO_COUNT		4
 
 /* Tilt application (inclination in IIO terms). */
@@ -418,8 +417,7 @@ static int mma9551_gpio_probe(struct iio_dev *indio_dev)
 	struct device *dev = &data->client->dev;
 
 	for (i = 0; i < MMA9551_GPIO_COUNT; i++) {
-		gpio = devm_gpiod_get_index(dev, MMA9551_GPIO_NAME, i,
-					    GPIOD_IN);
+		gpio = devm_gpiod_get_index(dev, NULL, i, GPIOD_IN);
 		if (IS_ERR(gpio)) {
 			dev_err(dev, "acpi gpio get index failed\n");
 			return PTR_ERR(gpio);
diff --git a/drivers/iio/accel/st_accel_core.c b/drivers/iio/accel/st_accel_core.c
index 784670e2736b..07d1489cd457 100644
--- a/drivers/iio/accel/st_accel_core.c
+++ b/drivers/iio/accel/st_accel_core.c
@@ -710,6 +710,8 @@ static const struct iio_trigger_ops st_accel_trigger_ops = {
 int st_accel_common_probe(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *adata = iio_priv(indio_dev);
+	struct st_sensors_platform_data *pdata =
+		(struct st_sensors_platform_data *)adata->dev->platform_data;
 	int irq = adata->get_irq_data_ready(indio_dev);
 	int err;
 
@@ -736,9 +738,8 @@ int st_accel_common_probe(struct iio_dev *indio_dev)
 					&adata->sensor_settings->fs.fs_avl[0];
 	adata->odr = adata->sensor_settings->odr.odr_avl[0].hz;
 
-	if (!adata->dev->platform_data)
-		adata->dev->platform_data =
-			(struct st_sensors_platform_data *)&default_accel_pdata;
+	if (!pdata)
+		pdata = (struct st_sensors_platform_data *)&default_accel_pdata;
 
 	err = st_sensors_init_sensor(indio_dev, adata->dev->platform_data);
 	if (err < 0)
diff --git a/drivers/iio/accel/st_accel_spi.c b/drivers/iio/accel/st_accel_spi.c
index 29a15f27a51b..1a867f5563a4 100644
--- a/drivers/iio/accel/st_accel_spi.c
+++ b/drivers/iio/accel/st_accel_spi.c
@@ -47,15 +47,11 @@ static int st_accel_spi_remove(struct spi_device *spi)
 }
 
 static const struct spi_device_id st_accel_id_table[] = {
-	{ LSM303DLH_ACCEL_DEV_NAME },
-	{ LSM303DLHC_ACCEL_DEV_NAME },
 	{ LIS3DH_ACCEL_DEV_NAME },
 	{ LSM330D_ACCEL_DEV_NAME },
 	{ LSM330DL_ACCEL_DEV_NAME },
 	{ LSM330DLC_ACCEL_DEV_NAME },
 	{ LIS331DLH_ACCEL_DEV_NAME },
-	{ LSM303DL_ACCEL_DEV_NAME },
-	{ LSM303DLM_ACCEL_DEV_NAME },
 	{ LSM330_ACCEL_DEV_NAME },
 	{ LSM303AGR_ACCEL_DEV_NAME },
 	{ LIS2DH12_ACCEL_DEV_NAME },
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index 401f47b51d83..614fa41559b1 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -679,6 +679,18 @@ config TI_ADC0832
 	  This driver can also be built as a module. If so, the module will be
 	  called ti-adc0832.
 
+config TI_ADC084S021
+	tristate "Texas Instruments ADC084S021"
+	depends on SPI
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  If you say yes here you get support for Texas Instruments ADC084S021
+	  chips.
+
+	  This driver can also be built as a module. If so, the module will be
+	  called ti-adc084s021.
+
 config TI_ADC12138
 	tristate "Texas Instruments ADC12130/ADC12132/ADC12138"
 	depends on SPI
@@ -691,6 +703,18 @@ config TI_ADC12138
 	  This driver can also be built as a module. If so, the module will be
 	  called ti-adc12138.
 
+config TI_ADC108S102
+	tristate "Texas Instruments ADC108S102 and ADC128S102 driver"
+	depends on SPI
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for Texas Instruments ADC108S102 and
+	  ADC128S102 ADC.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called ti-adc108s102.
+
 config TI_ADC128S052
 	tristate "Texas Instruments ADC128S052/ADC122S021/ADC124S021"
 	depends on SPI
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index 9339bec4babe..b546736a5541 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -62,7 +62,9 @@ obj-$(CONFIG_STM32_ADC_CORE) += stm32-adc-core.o
 obj-$(CONFIG_STM32_ADC) += stm32-adc.o
 obj-$(CONFIG_TI_ADC081C) += ti-adc081c.o
 obj-$(CONFIG_TI_ADC0832) += ti-adc0832.o
+obj-$(CONFIG_TI_ADC084S021) += ti-adc084s021.o
 obj-$(CONFIG_TI_ADC12138) += ti-adc12138.o
+obj-$(CONFIG_TI_ADC108S102) += ti-adc108s102.o
 obj-$(CONFIG_TI_ADC128S052) += ti-adc128s052.o
 obj-$(CONFIG_TI_ADC161S626) += ti-adc161s626.o
 obj-$(CONFIG_TI_ADS1015) += ti-ads1015.o
diff --git a/drivers/iio/adc/ad7791.c b/drivers/iio/adc/ad7791.c
index 1817ebf5ad84..34e353c43ac8 100644
--- a/drivers/iio/adc/ad7791.c
+++ b/drivers/iio/adc/ad7791.c
@@ -272,11 +272,9 @@ static ssize_t ad7791_write_frequency(struct device *dev,
 	struct ad7791_state *st = iio_priv(indio_dev);
 	int i, ret;
 
-	for (i = 0; i < ARRAY_SIZE(ad7791_sample_freq_avail); i++)
-		if (sysfs_streq(ad7791_sample_freq_avail[i], buf))
-			break;
-	if (i == ARRAY_SIZE(ad7791_sample_freq_avail))
-		return -EINVAL;
+	i = sysfs_match_string(ad7791_sample_freq_avail, buf);
+	if (i < 0)
+		return i;
 
 	ret = iio_device_claim_direct_mode(indio_dev);
 	if (ret)
diff --git a/drivers/iio/adc/aspeed_adc.c b/drivers/iio/adc/aspeed_adc.c
index 62670cbfa2bb..e0ea411a0b2d 100644
--- a/drivers/iio/adc/aspeed_adc.c
+++ b/drivers/iio/adc/aspeed_adc.c
@@ -212,7 +212,10 @@ static int aspeed_adc_probe(struct platform_device *pdev)
 	}
 
 	/* Start all channels in normal mode. */
-	clk_prepare_enable(data->clk_scaler->clk);
+	ret = clk_prepare_enable(data->clk_scaler->clk);
+	if (ret)
+		goto clk_enable_error;
+
 	adc_engine_control_reg_val = GENMASK(31, 16) |
 		ASPEED_OPERATION_MODE_NORMAL | ASPEED_ENGINE_ENABLE;
 	writel(adc_engine_control_reg_val,
@@ -236,6 +239,7 @@ iio_register_error:
 	writel(ASPEED_OPERATION_MODE_POWER_DOWN,
 		data->base + ASPEED_REG_ENGINE_CONTROL);
 	clk_disable_unprepare(data->clk_scaler->clk);
+clk_enable_error:
 	clk_hw_unregister_divider(data->clk_scaler);
 
 scaler_error:
diff --git a/drivers/iio/adc/cpcap-adc.c b/drivers/iio/adc/cpcap-adc.c
index 62d37f8725b8..6e419d5a7c14 100644
--- a/drivers/iio/adc/cpcap-adc.c
+++ b/drivers/iio/adc/cpcap-adc.c
@@ -52,6 +52,10 @@
 #define CPCAP_BIT_RAND0			BIT(1)	/* Set with CAL_MODE */
 #define CPCAP_BIT_ADEN			BIT(0)	/* Currently unused */
 
+#define CPCAP_REG_ADCC1_DEFAULTS	(CPCAP_BIT_ADEN_AUTO_CLR | \
+					 CPCAP_BIT_ADC_CLK_SEL0 |  \
+					 CPCAP_BIT_RAND1)
+
 /* Register CPCAP_REG_ADCC2 bits */
 #define CPCAP_BIT_CAL_FACTOR_ENABLE	BIT(15)	/* Currently unused */
 #define CPCAP_BIT_BATDETB_EN		BIT(14)	/* Currently unused */
@@ -62,7 +66,7 @@
 #define CPCAP_BIT_ADC_PS_FACTOR0	BIT(9)
 #define CPCAP_BIT_AD4_SELECT		BIT(8)	/* Currently unused */
 #define CPCAP_BIT_ADC_BUSY		BIT(7)	/* Currently unused */
-#define CPCAP_BIT_THERMBIAS_EN		BIT(6)	/* Currently unused */
+#define CPCAP_BIT_THERMBIAS_EN		BIT(6)	/* Bias for AD0_BATTDETB */
 #define CPCAP_BIT_ADTRIG_DIS		BIT(5)	/* Disable interrupt */
 #define CPCAP_BIT_LIADC			BIT(4)	/* Currently unused */
 #define CPCAP_BIT_TS_REFEN		BIT(3)	/* Currently unused */
@@ -70,6 +74,12 @@
 #define CPCAP_BIT_TS_M1			BIT(1)	/* Currently unused */
 #define CPCAP_BIT_TS_M0			BIT(0)	/* Currently unused */
 
+#define CPCAP_REG_ADCC2_DEFAULTS	(CPCAP_BIT_AD4_SELECT | \
+					 CPCAP_BIT_ADTRIG_DIS | \
+					 CPCAP_BIT_LIADC | \
+					 CPCAP_BIT_TS_M2 | \
+					 CPCAP_BIT_TS_M1)
+
 #define CPCAP_MAX_TEMP_LVL		27
 #define CPCAP_FOUR_POINT_TWO_ADC	801
 #define ST_ADC_CAL_CHRGI_HIGH_THRESHOLD	530
@@ -78,7 +88,7 @@
 #define ST_ADC_CAL_BATTI_LOW_THRESHOLD	494
 #define ST_ADC_CALIBRATE_DIFF_THRESHOLD	3
 
-#define CPCAP_ADC_MAX_RETRIES		5	/* Calibration and quirk */
+#define CPCAP_ADC_MAX_RETRIES		5	/* Calibration */
 
 /**
  * struct cpcap_adc_ato - timing settings for cpcap adc
@@ -124,10 +134,10 @@ struct cpcap_adc {
  */
 enum cpcap_adc_channel {
 	/* Bank0 channels */
-	CPCAP_ADC_AD0_BATTDETB,	/* Battery detection */
+	CPCAP_ADC_AD0,		/* Battery temperature */
 	CPCAP_ADC_BATTP,	/* Battery voltage */
 	CPCAP_ADC_VBUS,		/* USB VBUS voltage */
-	CPCAP_ADC_AD3,		/* Battery temperature when charging */
+	CPCAP_ADC_AD3,		/* Die temperature when charging */
 	CPCAP_ADC_BPLUS_AD4,	/* Another battery or system voltage */
 	CPCAP_ADC_CHG_ISENSE,	/* Calibrated charge current */
 	CPCAP_ADC_BATTI,	/* Calibrated system current */
@@ -217,7 +227,7 @@ struct cpcap_adc_request {
 /* Phasing table for channels. Note that channels 16 & 17 use BATTP and BATTI */
 static const struct cpcap_adc_phasing_tbl bank_phasing[] = {
 	/* Bank0 */
-	[CPCAP_ADC_AD0_BATTDETB] = {0, 0x80, 0x80,    0, 1023},
+	[CPCAP_ADC_AD0] =          {0, 0x80, 0x80,    0, 1023},
 	[CPCAP_ADC_BATTP] =        {0, 0x80, 0x80,    0, 1023},
 	[CPCAP_ADC_VBUS] =         {0, 0x80, 0x80,    0, 1023},
 	[CPCAP_ADC_AD3] =          {0, 0x80, 0x80,    0, 1023},
@@ -243,7 +253,7 @@ static const struct cpcap_adc_phasing_tbl bank_phasing[] = {
  */
 static struct cpcap_adc_conversion_tbl bank_conversion[] = {
 	/* Bank0 */
-	[CPCAP_ADC_AD0_BATTDETB] = {
+	[CPCAP_ADC_AD0] = {
 		IIO_CHAN_INFO_PROCESSED,    0,    0, 0,     1,    1,
 	},
 	[CPCAP_ADC_BATTP] = {
@@ -541,6 +551,15 @@ static void cpcap_adc_setup_bank(struct cpcap_adc *ddata,
 		return;
 
 	switch (req->channel) {
+	case CPCAP_ADC_AD0:
+		value2 |= CPCAP_BIT_THERMBIAS_EN;
+		error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
+					   CPCAP_BIT_THERMBIAS_EN,
+					   value2);
+		if (error)
+			return;
+		usleep_range(800, 1000);
+		break;
 	case CPCAP_ADC_AD8 ... CPCAP_ADC_TSY2_AD15:
 		value1 |= CPCAP_BIT_AD_SEL1;
 		break;
@@ -583,7 +602,8 @@ static void cpcap_adc_setup_bank(struct cpcap_adc *ddata,
 	error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
 				   CPCAP_BIT_ATOX_PS_FACTOR |
 				   CPCAP_BIT_ADC_PS_FACTOR1 |
-				   CPCAP_BIT_ADC_PS_FACTOR0,
+				   CPCAP_BIT_ADC_PS_FACTOR0 |
+				   CPCAP_BIT_THERMBIAS_EN,
 				   value2);
 	if (error)
 		return;
@@ -614,27 +634,6 @@ static void cpcap_adc_setup_bank(struct cpcap_adc *ddata,
 	}
 }
 
-/*
- * Occasionally the ADC does not seem to start and there will be no
- * interrupt. Let's re-init interrupt to prevent the ADC from hanging
- * for the next request. It is unclear why this happens, but the next
- * request will usually work after doing this.
- */
-static void cpcap_adc_quirk_reset_lost_irq(struct cpcap_adc *ddata)
-{
-	int error;
-
-	dev_info(ddata->dev, "lost ADC irq, attempting to reinit\n");
-	disable_irq(ddata->irq);
-	error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
-				   CPCAP_BIT_ADTRIG_DIS,
-				   CPCAP_BIT_ADTRIG_DIS);
-	if (error)
-		dev_warn(ddata->dev, "%s reset failed: %i\n",
-			 __func__, error);
-	enable_irq(ddata->irq);
-}
-
 static int cpcap_adc_start_bank(struct cpcap_adc *ddata,
 				struct cpcap_adc_request *req)
 {
@@ -652,7 +651,6 @@ static int cpcap_adc_start_bank(struct cpcap_adc *ddata,
 			return 0;
 
 		if (error == 0) {
-			cpcap_adc_quirk_reset_lost_irq(ddata);
 			error = -ETIMEDOUT;
 			continue;
 		}
@@ -664,6 +662,21 @@ static int cpcap_adc_start_bank(struct cpcap_adc *ddata,
 	return error;
 }
 
+static int cpcap_adc_stop_bank(struct cpcap_adc *ddata)
+{
+	int error;
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC1,
+				   0xffff,
+				   CPCAP_REG_ADCC1_DEFAULTS);
+	if (error)
+		return error;
+
+	return regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
+				  0xffff,
+				  CPCAP_REG_ADCC2_DEFAULTS);
+}
+
 static void cpcap_adc_phase(struct cpcap_adc_request *req)
 {
 	const struct cpcap_adc_conversion_tbl *conv_tbl = req->conv_tbl;
@@ -758,7 +771,7 @@ static void cpcap_adc_convert(struct cpcap_adc_request *req)
 		return;
 
 	/* Temperatures use a lookup table instead of conversion table */
-	if ((req->channel == CPCAP_ADC_AD0_BATTDETB) ||
+	if ((req->channel == CPCAP_ADC_AD0) ||
 	    (req->channel == CPCAP_ADC_AD3)) {
 		req->result =
 			cpcap_adc_table_to_millicelcius(req->result);
@@ -820,7 +833,7 @@ static int cpcap_adc_init_request(struct cpcap_adc_request *req,
 	req->conv_tbl = bank_conversion;
 
 	switch (channel) {
-	case CPCAP_ADC_AD0_BATTDETB ... CPCAP_ADC_USB_ID:
+	case CPCAP_ADC_AD0 ... CPCAP_ADC_USB_ID:
 		req->bank_index = channel;
 		break;
 	case CPCAP_ADC_AD8 ... CPCAP_ADC_TSY2_AD15:
@@ -839,6 +852,22 @@ static int cpcap_adc_init_request(struct cpcap_adc_request *req,
 	return 0;
 }
 
+static int cpcap_adc_read_st_die_temp(struct cpcap_adc *ddata,
+				      int addr, int *val)
+{
+	int error;
+
+	error = regmap_read(ddata->reg, addr, val);
+	if (error)
+		return error;
+
+	*val -= 282;
+	*val *= 114;
+	*val += 25000;
+
+	return 0;
+}
+
 static int cpcap_adc_read(struct iio_dev *indio_dev,
 			  struct iio_chan_spec const *chan,
 			  int *val, int *val2, long mask)
@@ -860,6 +889,9 @@ static int cpcap_adc_read(struct iio_dev *indio_dev,
 		error = regmap_read(ddata->reg, chan->address, val);
 		if (error)
 			goto err_unlock;
+		error = cpcap_adc_stop_bank(ddata);
+		if (error)
+			goto err_unlock;
 		mutex_unlock(&ddata->lock);
 		break;
 	case IIO_CHAN_INFO_PROCESSED:
@@ -867,7 +899,19 @@ static int cpcap_adc_read(struct iio_dev *indio_dev,
 		error = cpcap_adc_start_bank(ddata, &req);
 		if (error)
 			goto err_unlock;
-		error = cpcap_adc_read_bank_scaled(ddata, &req);
+		if ((ddata->vendor == CPCAP_VENDOR_ST) &&
+		    (chan->channel == CPCAP_ADC_AD3)) {
+			error = cpcap_adc_read_st_die_temp(ddata,
+							   chan->address,
+							   &req.result);
+			if (error)
+				goto err_unlock;
+		} else {
+			error = cpcap_adc_read_bank_scaled(ddata, &req);
+			if (error)
+				goto err_unlock;
+		}
+		error = cpcap_adc_stop_bank(ddata);
 		if (error)
 			goto err_unlock;
 		mutex_unlock(&ddata->lock);
diff --git a/drivers/iio/adc/hi8435.c b/drivers/iio/adc/hi8435.c
index 678e8c7ea763..adf7dc712937 100644
--- a/drivers/iio/adc/hi8435.c
+++ b/drivers/iio/adc/hi8435.c
@@ -105,6 +105,26 @@ static int hi8435_writew(struct hi8435_priv *priv, u8 reg, u16 val)
 	return spi_write(priv->spi, priv->reg_buffer, 3);
 }
 
+static int hi8435_read_raw(struct iio_dev *idev,
+			   const struct iio_chan_spec *chan,
+			   int *val, int *val2, long mask)
+{
+	struct hi8435_priv *priv = iio_priv(idev);
+	u32 tmp;
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = hi8435_readl(priv, HI8435_SO31_0_REG, &tmp);
+		if (ret < 0)
+			return ret;
+		*val = !!(tmp & BIT(chan->channel));
+		return IIO_VAL_INT;
+	default:
+		return -EINVAL;
+	}
+}
+
 static int hi8435_read_event_config(struct iio_dev *idev,
 				    const struct iio_chan_spec *chan,
 				    enum iio_event_type type,
@@ -121,10 +141,21 @@ static int hi8435_write_event_config(struct iio_dev *idev,
 				     enum iio_event_direction dir, int state)
 {
 	struct hi8435_priv *priv = iio_priv(idev);
+	int ret;
+	u32 tmp;
+
+	if (state) {
+		ret = hi8435_readl(priv, HI8435_SO31_0_REG, &tmp);
+		if (ret < 0)
+			return ret;
+		if (tmp & BIT(chan->channel))
+			priv->event_prev_val |= BIT(chan->channel);
+		else
+			priv->event_prev_val &= ~BIT(chan->channel);
 
-	priv->event_scan_mask &= ~BIT(chan->channel);
-	if (state)
 		priv->event_scan_mask |= BIT(chan->channel);
+	} else
+		priv->event_scan_mask &= ~BIT(chan->channel);
 
 	return 0;
 }
@@ -325,6 +356,7 @@ static const struct iio_enum hi8435_sensing_mode = {
 
 static const struct iio_chan_spec_ext_info hi8435_ext_info[] = {
 	IIO_ENUM("sensing_mode", IIO_SEPARATE, &hi8435_sensing_mode),
+	IIO_ENUM_AVAILABLE("sensing_mode", &hi8435_sensing_mode),
 	{},
 };
 
@@ -333,6 +365,7 @@ static const struct iio_chan_spec_ext_info hi8435_ext_info[] = {
 	.type = IIO_VOLTAGE,				\
 	.indexed = 1,					\
 	.channel = num,					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
 	.event_spec = hi8435_events,			\
 	.num_event_specs = ARRAY_SIZE(hi8435_events),	\
 	.ext_info = hi8435_ext_info,			\
@@ -376,11 +409,12 @@ static const struct iio_chan_spec hi8435_channels[] = {
 
 static const struct iio_info hi8435_info = {
 	.driver_module = THIS_MODULE,
-	.read_event_config = &hi8435_read_event_config,
+	.read_raw = hi8435_read_raw,
+	.read_event_config = hi8435_read_event_config,
 	.write_event_config = hi8435_write_event_config,
-	.read_event_value = &hi8435_read_event_value,
-	.write_event_value = &hi8435_write_event_value,
-	.debugfs_reg_access = &hi8435_debugfs_reg_access,
+	.read_event_value = hi8435_read_event_value,
+	.write_event_value = hi8435_write_event_value,
+	.debugfs_reg_access = hi8435_debugfs_reg_access,
 };
 
 static void hi8435_iio_push_event(struct iio_dev *idev, unsigned int val)
diff --git a/drivers/iio/adc/ina2xx-adc.c b/drivers/iio/adc/ina2xx-adc.c
index db9838230257..232c0b80d658 100644
--- a/drivers/iio/adc/ina2xx-adc.c
+++ b/drivers/iio/adc/ina2xx-adc.c
@@ -42,13 +42,14 @@
 #define INA2XX_CURRENT                  0x04	/* readonly */
 #define INA2XX_CALIBRATION              0x05
 
-#define INA226_ALERT_MASK		GENMASK(2, 1)
-#define INA266_CVRF			BIT(3)
+#define INA226_MASK_ENABLE		0x06
+#define INA226_CVRF			BIT(3)
 
 #define INA2XX_MAX_REGISTERS            8
 
 /* settings - depend on use case */
 #define INA219_CONFIG_DEFAULT           0x399F	/* PGA=8 */
+#define INA219_DEFAULT_IT		532
 #define INA226_CONFIG_DEFAULT           0x4327
 #define INA226_DEFAULT_AVG              4
 #define INA226_DEFAULT_IT		1110
@@ -56,19 +57,24 @@
 #define INA2XX_RSHUNT_DEFAULT           10000
 
 /*
- * bit mask for reading the averaging setting in the configuration register
+ * bit masks for reading the settings in the configuration register
  * FIXME: use regmap_fields.
  */
 #define INA2XX_MODE_MASK	GENMASK(3, 0)
 
+/* Averaging for VBus/VShunt/Power */
 #define INA226_AVG_MASK		GENMASK(11, 9)
 #define INA226_SHIFT_AVG(val)	((val) << 9)
 
 /* Integration time for VBus */
+#define INA219_ITB_MASK		GENMASK(10, 7)
+#define INA219_SHIFT_ITB(val)	((val) << 7)
 #define INA226_ITB_MASK		GENMASK(8, 6)
 #define INA226_SHIFT_ITB(val)	((val) << 6)
 
 /* Integration time for VShunt */
+#define INA219_ITS_MASK		GENMASK(6, 3)
+#define INA219_SHIFT_ITS(val)	((val) << 3)
 #define INA226_ITS_MASK		GENMASK(5, 3)
 #define INA226_SHIFT_ITS(val)	((val) << 3)
 
@@ -108,6 +114,7 @@ struct ina2xx_config {
 	int bus_voltage_shift;
 	int bus_voltage_lsb;	/* uV */
 	int power_lsb;		/* uW */
+	enum ina2xx_ids chip_id;
 };
 
 struct ina2xx_chip_info {
@@ -130,6 +137,7 @@ static const struct ina2xx_config ina2xx_config[] = {
 		.bus_voltage_shift = 3,
 		.bus_voltage_lsb = 4000,
 		.power_lsb = 20000,
+		.chip_id = ina219,
 	},
 	[ina226] = {
 		.config_default = INA226_CONFIG_DEFAULT,
@@ -138,6 +146,7 @@ static const struct ina2xx_config ina2xx_config[] = {
 		.bus_voltage_shift = 0,
 		.bus_voltage_lsb = 1250,
 		.power_lsb = 25000,
+		.chip_id = ina226,
 	},
 };
 
@@ -283,6 +292,66 @@ static int ina226_set_int_time_vshunt(struct ina2xx_chip_info *chip,
 	return 0;
 }
 
+/* Conversion times in uS. */
+static const int ina219_conv_time_tab_subsample[] = { 84, 148, 276, 532 };
+static const int ina219_conv_time_tab_average[] = { 532, 1060, 2130, 4260,
+						    8510, 17020, 34050, 68100};
+
+static int ina219_lookup_int_time(unsigned int *val_us, int *bits)
+{
+	if (*val_us > 68100 || *val_us < 84)
+		return -EINVAL;
+
+	if (*val_us <= 532) {
+		*bits = find_closest(*val_us, ina219_conv_time_tab_subsample,
+				    ARRAY_SIZE(ina219_conv_time_tab_subsample));
+		*val_us = ina219_conv_time_tab_subsample[*bits];
+	} else {
+		*bits = find_closest(*val_us, ina219_conv_time_tab_average,
+				    ARRAY_SIZE(ina219_conv_time_tab_average));
+		*val_us = ina219_conv_time_tab_average[*bits];
+		*bits |= 0x8;
+	}
+
+	return 0;
+}
+
+static int ina219_set_int_time_vbus(struct ina2xx_chip_info *chip,
+				    unsigned int val_us, unsigned int *config)
+{
+	int bits, ret;
+	unsigned int val_us_best = val_us;
+
+	ret = ina219_lookup_int_time(&val_us_best, &bits);
+	if (ret)
+		return ret;
+
+	chip->int_time_vbus = val_us_best;
+
+	*config &= ~INA219_ITB_MASK;
+	*config |= INA219_SHIFT_ITB(bits) & INA219_ITB_MASK;
+
+	return 0;
+}
+
+static int ina219_set_int_time_vshunt(struct ina2xx_chip_info *chip,
+				      unsigned int val_us, unsigned int *config)
+{
+	int bits, ret;
+	unsigned int val_us_best = val_us;
+
+	ret = ina219_lookup_int_time(&val_us_best, &bits);
+	if (ret)
+		return ret;
+
+	chip->int_time_vshunt = val_us_best;
+
+	*config &= ~INA219_ITS_MASK;
+	*config |= INA219_SHIFT_ITS(bits) & INA219_ITS_MASK;
+
+	return 0;
+}
+
 static int ina2xx_write_raw(struct iio_dev *indio_dev,
 			    struct iio_chan_spec const *chan,
 			    int val, int val2, long mask)
@@ -308,10 +377,21 @@ static int ina2xx_write_raw(struct iio_dev *indio_dev,
 		break;
 
 	case IIO_CHAN_INFO_INT_TIME:
-		if (chan->address == INA2XX_SHUNT_VOLTAGE)
-			ret = ina226_set_int_time_vshunt(chip, val2, &tmp);
-		else
-			ret = ina226_set_int_time_vbus(chip, val2, &tmp);
+		if (chip->config->chip_id == ina226) {
+			if (chan->address == INA2XX_SHUNT_VOLTAGE)
+				ret = ina226_set_int_time_vshunt(chip, val2,
+								 &tmp);
+			else
+				ret = ina226_set_int_time_vbus(chip, val2,
+							       &tmp);
+		} else {
+			if (chan->address == INA2XX_SHUNT_VOLTAGE)
+				ret = ina219_set_int_time_vshunt(chip, val2,
+								 &tmp);
+			else
+				ret = ina219_set_int_time_vbus(chip, val2,
+							       &tmp);
+		}
 		break;
 
 	default:
@@ -412,13 +492,30 @@ static ssize_t ina2xx_shunt_resistor_store(struct device *dev,
 	return len;
 }
 
-#define INA2XX_CHAN(_type, _index, _address) { \
+#define INA219_CHAN(_type, _index, _address) { \
+	.type = (_type), \
+	.address = (_address), \
+	.indexed = 1, \
+	.channel = (_index), \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			      BIT(IIO_CHAN_INFO_SCALE), \
+	.info_mask_shared_by_dir = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+	.scan_index = (_index), \
+	.scan_type = { \
+		.sign = 'u', \
+		.realbits = 16, \
+		.storagebits = 16, \
+		.endianness = IIO_CPU, \
+	} \
+}
+
+#define INA226_CHAN(_type, _index, _address) { \
 	.type = (_type), \
 	.address = (_address), \
 	.indexed = 1, \
 	.channel = (_index), \
-	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) \
-	| BIT(IIO_CHAN_INFO_SCALE), \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			      BIT(IIO_CHAN_INFO_SCALE), \
 	.info_mask_shared_by_dir = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \
 				   BIT(IIO_CHAN_INFO_OVERSAMPLING_RATIO), \
 	.scan_index = (_index), \
@@ -434,7 +531,25 @@ static ssize_t ina2xx_shunt_resistor_store(struct device *dev,
  * Sampling Freq is a consequence of the integration times of
  * the Voltage channels.
  */
-#define INA2XX_CHAN_VOLTAGE(_index, _address) { \
+#define INA219_CHAN_VOLTAGE(_index, _address) { \
+	.type = IIO_VOLTAGE, \
+	.address = (_address), \
+	.indexed = 1, \
+	.channel = (_index), \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			      BIT(IIO_CHAN_INFO_SCALE) | \
+			      BIT(IIO_CHAN_INFO_INT_TIME), \
+	.info_mask_shared_by_dir = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+	.scan_index = (_index), \
+	.scan_type = { \
+		.sign = 'u', \
+		.realbits = 16, \
+		.storagebits = 16, \
+		.endianness = IIO_LE, \
+	} \
+}
+
+#define INA226_CHAN_VOLTAGE(_index, _address) { \
 	.type = IIO_VOLTAGE, \
 	.address = (_address), \
 	.indexed = 1, \
@@ -442,6 +557,8 @@ static ssize_t ina2xx_shunt_resistor_store(struct device *dev,
 	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
 			      BIT(IIO_CHAN_INFO_SCALE) | \
 			      BIT(IIO_CHAN_INFO_INT_TIME), \
+	.info_mask_shared_by_dir = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \
+				   BIT(IIO_CHAN_INFO_OVERSAMPLING_RATIO), \
 	.scan_index = (_index), \
 	.scan_type = { \
 		.sign = 'u', \
@@ -451,11 +568,20 @@ static ssize_t ina2xx_shunt_resistor_store(struct device *dev,
 	} \
 }
 
-static const struct iio_chan_spec ina2xx_channels[] = {
-	INA2XX_CHAN_VOLTAGE(0, INA2XX_SHUNT_VOLTAGE),
-	INA2XX_CHAN_VOLTAGE(1, INA2XX_BUS_VOLTAGE),
-	INA2XX_CHAN(IIO_POWER, 2, INA2XX_POWER),
-	INA2XX_CHAN(IIO_CURRENT, 3, INA2XX_CURRENT),
+
+static const struct iio_chan_spec ina226_channels[] = {
+	INA226_CHAN_VOLTAGE(0, INA2XX_SHUNT_VOLTAGE),
+	INA226_CHAN_VOLTAGE(1, INA2XX_BUS_VOLTAGE),
+	INA226_CHAN(IIO_POWER, 2, INA2XX_POWER),
+	INA226_CHAN(IIO_CURRENT, 3, INA2XX_CURRENT),
+	IIO_CHAN_SOFT_TIMESTAMP(4),
+};
+
+static const struct iio_chan_spec ina219_channels[] = {
+	INA219_CHAN_VOLTAGE(0, INA2XX_SHUNT_VOLTAGE),
+	INA219_CHAN_VOLTAGE(1, INA2XX_BUS_VOLTAGE),
+	INA219_CHAN(IIO_POWER, 2, INA2XX_POWER),
+	INA219_CHAN(IIO_CURRENT, 3, INA2XX_CURRENT),
 	IIO_CHAN_SOFT_TIMESTAMP(4),
 };
 
@@ -481,12 +607,12 @@ static int ina2xx_work_buffer(struct iio_dev *indio_dev)
 	 */
 	if (!chip->allow_async_readout)
 		do {
-			ret = regmap_read(chip->regmap, INA226_ALERT_MASK,
+			ret = regmap_read(chip->regmap, INA226_MASK_ENABLE,
 					  &alert);
 			if (ret < 0)
 				return ret;
 
-			alert &= INA266_CVRF;
+			alert &= INA226_CVRF;
 		} while (!alert);
 
 	/*
@@ -590,7 +716,14 @@ static int ina2xx_debug_reg(struct iio_dev *indio_dev,
 }
 
 /* Possible integration times for vshunt and vbus */
-static IIO_CONST_ATTR_INT_TIME_AVAIL("0.000140 0.000204 0.000332 0.000588 0.001100 0.002116 0.004156 0.008244");
+static IIO_CONST_ATTR_NAMED(ina219_integration_time_available,
+			    integration_time_available,
+			    "0.000084 0.000148 0.000276 0.000532 0.001060 0.002130 0.004260 0.008510 0.017020 0.034050 0.068100");
+
+static IIO_CONST_ATTR_NAMED(ina226_integration_time_available,
+			    integration_time_available,
+			    "0.000140 0.000204 0.000332 0.000588 0.001100 0.002116 0.004156 0.008244");
+
 
 static IIO_DEVICE_ATTR(in_allow_async_readout, S_IRUGO | S_IWUSR,
 		       ina2xx_allow_async_readout_show,
@@ -600,20 +733,39 @@ static IIO_DEVICE_ATTR(in_shunt_resistor, S_IRUGO | S_IWUSR,
 		       ina2xx_shunt_resistor_show,
 		       ina2xx_shunt_resistor_store, 0);
 
-static struct attribute *ina2xx_attributes[] = {
+static struct attribute *ina219_attributes[] = {
+	&iio_dev_attr_in_allow_async_readout.dev_attr.attr,
+	&iio_const_attr_ina219_integration_time_available.dev_attr.attr,
+	&iio_dev_attr_in_shunt_resistor.dev_attr.attr,
+	NULL,
+};
+
+static struct attribute *ina226_attributes[] = {
 	&iio_dev_attr_in_allow_async_readout.dev_attr.attr,
-	&iio_const_attr_integration_time_available.dev_attr.attr,
+	&iio_const_attr_ina226_integration_time_available.dev_attr.attr,
 	&iio_dev_attr_in_shunt_resistor.dev_attr.attr,
 	NULL,
 };
 
-static const struct attribute_group ina2xx_attribute_group = {
-	.attrs = ina2xx_attributes,
+static const struct attribute_group ina219_attribute_group = {
+	.attrs = ina219_attributes,
+};
+
+static const struct attribute_group ina226_attribute_group = {
+	.attrs = ina226_attributes,
 };
 
-static const struct iio_info ina2xx_info = {
+static const struct iio_info ina219_info = {
 	.driver_module = THIS_MODULE,
-	.attrs = &ina2xx_attribute_group,
+	.attrs = &ina219_attribute_group,
+	.read_raw = ina2xx_read_raw,
+	.write_raw = ina2xx_write_raw,
+	.debugfs_reg_access = ina2xx_debug_reg,
+};
+
+static const struct iio_info ina226_info = {
+	.driver_module = THIS_MODULE,
+	.attrs = &ina226_attribute_group,
 	.read_raw = ina2xx_read_raw,
 	.write_raw = ina2xx_write_raw,
 	.debugfs_reg_access = ina2xx_debug_reg,
@@ -684,6 +836,10 @@ static int ina2xx_probe(struct i2c_client *client,
 		ina226_set_average(chip, INA226_DEFAULT_AVG, &val);
 		ina226_set_int_time_vbus(chip, INA226_DEFAULT_IT, &val);
 		ina226_set_int_time_vshunt(chip, INA226_DEFAULT_IT, &val);
+	} else {
+		chip->avg = 1;
+		ina219_set_int_time_vbus(chip, INA219_DEFAULT_IT, &val);
+		ina219_set_int_time_vshunt(chip, INA219_DEFAULT_IT, &val);
 	}
 
 	ret = ina2xx_init(chip, val);
@@ -695,10 +851,16 @@ static int ina2xx_probe(struct i2c_client *client,
 	indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
 	indio_dev->dev.parent = &client->dev;
 	indio_dev->dev.of_node = client->dev.of_node;
-	indio_dev->channels = ina2xx_channels;
-	indio_dev->num_channels = ARRAY_SIZE(ina2xx_channels);
+	if (id->driver_data == ina226) {
+		indio_dev->channels = ina226_channels;
+		indio_dev->num_channels = ARRAY_SIZE(ina226_channels);
+		indio_dev->info = &ina226_info;
+	} else {
+		indio_dev->channels = ina219_channels;
+		indio_dev->num_channels = ARRAY_SIZE(ina219_channels);
+		indio_dev->info = &ina219_info;
+	}
 	indio_dev->name = id->name;
-	indio_dev->info = &ina2xx_info;
 	indio_dev->setup_ops = &ina2xx_setup_ops;
 
 	buffer = devm_iio_kfifo_allocate(&indio_dev->dev);
diff --git a/drivers/iio/adc/lpc32xx_adc.c b/drivers/iio/adc/lpc32xx_adc.c
index 0de709b4288b..6a5b9a9bc662 100644
--- a/drivers/iio/adc/lpc32xx_adc.c
+++ b/drivers/iio/adc/lpc32xx_adc.c
@@ -76,10 +76,14 @@ static int lpc32xx_read_raw(struct iio_dev *indio_dev,
 			    long mask)
 {
 	struct lpc32xx_adc_state *st = iio_priv(indio_dev);
-
+	int ret;
 	if (mask == IIO_CHAN_INFO_RAW) {
 		mutex_lock(&indio_dev->mlock);
-		clk_prepare_enable(st->clk);
+		ret = clk_prepare_enable(st->clk);
+		if (ret) {
+			mutex_unlock(&indio_dev->mlock);
+			return ret;
+		}
 		/* Measurement setup */
 		__raw_writel(LPC32XXAD_INTERNAL | (chan->address) |
 			     LPC32XXAD_REFp | LPC32XXAD_REFm,
diff --git a/drivers/iio/adc/meson_saradc.c b/drivers/iio/adc/meson_saradc.c
index dd4190b50df6..83da50ed73ab 100644
--- a/drivers/iio/adc/meson_saradc.c
+++ b/drivers/iio/adc/meson_saradc.c
@@ -220,6 +220,7 @@ enum meson_sar_adc_chan7_mux_sel {
 };
 
 struct meson_sar_adc_data {
+	bool					has_bl30_integration;
 	unsigned int				resolution;
 	const char				*name;
 };
@@ -437,19 +438,24 @@ static int meson_sar_adc_lock(struct iio_dev *indio_dev)
 
 	mutex_lock(&indio_dev->mlock);
 
-	/* prevent BL30 from using the SAR ADC while we are using it */
-	regmap_update_bits(priv->regmap, MESON_SAR_ADC_DELAY,
-			   MESON_SAR_ADC_DELAY_KERNEL_BUSY,
-			   MESON_SAR_ADC_DELAY_KERNEL_BUSY);
-
-	/* wait until BL30 releases it's lock (so we can use the SAR ADC) */
-	do {
-		udelay(1);
-		regmap_read(priv->regmap, MESON_SAR_ADC_DELAY, &val);
-	} while (val & MESON_SAR_ADC_DELAY_BL30_BUSY && timeout--);
-
-	if (timeout < 0)
-		return -ETIMEDOUT;
+	if (priv->data->has_bl30_integration) {
+		/* prevent BL30 from using the SAR ADC while we are using it */
+		regmap_update_bits(priv->regmap, MESON_SAR_ADC_DELAY,
+				MESON_SAR_ADC_DELAY_KERNEL_BUSY,
+				MESON_SAR_ADC_DELAY_KERNEL_BUSY);
+
+		/*
+		 * wait until BL30 releases it's lock (so we can use the SAR
+		 * ADC)
+		 */
+		do {
+			udelay(1);
+			regmap_read(priv->regmap, MESON_SAR_ADC_DELAY, &val);
+		} while (val & MESON_SAR_ADC_DELAY_BL30_BUSY && timeout--);
+
+		if (timeout < 0)
+			return -ETIMEDOUT;
+	}
 
 	return 0;
 }
@@ -458,9 +464,10 @@ static void meson_sar_adc_unlock(struct iio_dev *indio_dev)
 {
 	struct meson_sar_adc_priv *priv = iio_priv(indio_dev);
 
-	/* allow BL30 to use the SAR ADC again */
-	regmap_update_bits(priv->regmap, MESON_SAR_ADC_DELAY,
-			   MESON_SAR_ADC_DELAY_KERNEL_BUSY, 0);
+	if (priv->data->has_bl30_integration)
+		/* allow BL30 to use the SAR ADC again */
+		regmap_update_bits(priv->regmap, MESON_SAR_ADC_DELAY,
+				MESON_SAR_ADC_DELAY_KERNEL_BUSY, 0);
 
 	mutex_unlock(&indio_dev->mlock);
 }
@@ -468,13 +475,13 @@ static void meson_sar_adc_unlock(struct iio_dev *indio_dev)
 static void meson_sar_adc_clear_fifo(struct iio_dev *indio_dev)
 {
 	struct meson_sar_adc_priv *priv = iio_priv(indio_dev);
-	int count;
+	unsigned int count, tmp;
 
 	for (count = 0; count < MESON_SAR_ADC_MAX_FIFO_SIZE; count++) {
 		if (!meson_sar_adc_get_fifo_count(indio_dev))
 			break;
 
-		regmap_read(priv->regmap, MESON_SAR_ADC_FIFO_RD, 0);
+		regmap_read(priv->regmap, MESON_SAR_ADC_FIFO_RD, &tmp);
 	}
 }
 
@@ -614,14 +621,16 @@ static int meson_sar_adc_init(struct iio_dev *indio_dev)
 	 */
 	meson_sar_adc_set_chan7_mux(indio_dev, CHAN7_MUX_CH7_INPUT);
 
-	/*
-	 * leave sampling delay and the input clocks as configured by BL30 to
-	 * make sure BL30 gets the values it expects when reading the
-	 * temperature sensor.
-	 */
-	regmap_read(priv->regmap, MESON_SAR_ADC_REG3, &regval);
-	if (regval & MESON_SAR_ADC_REG3_BL30_INITIALIZED)
-		return 0;
+	if (priv->data->has_bl30_integration) {
+		/*
+		 * leave sampling delay and the input clocks as configured by
+		 * BL30 to make sure BL30 gets the values it expects when
+		 * reading the temperature sensor.
+		 */
+		regmap_read(priv->regmap, MESON_SAR_ADC_REG3, &regval);
+		if (regval & MESON_SAR_ADC_REG3_BL30_INITIALIZED)
+			return 0;
+	}
 
 	meson_sar_adc_stop_sample_engine(indio_dev);
 
@@ -834,23 +843,46 @@ static const struct iio_info meson_sar_adc_iio_info = {
 	.driver_module = THIS_MODULE,
 };
 
-struct meson_sar_adc_data meson_sar_adc_gxbb_data = {
+static const struct meson_sar_adc_data meson_sar_adc_meson8_data = {
+	.has_bl30_integration = false,
+	.resolution = 10,
+	.name = "meson-meson8-saradc",
+};
+
+static const struct meson_sar_adc_data meson_sar_adc_meson8b_data = {
+	.has_bl30_integration = false,
+	.resolution = 10,
+	.name = "meson-meson8b-saradc",
+};
+
+static const struct meson_sar_adc_data meson_sar_adc_gxbb_data = {
+	.has_bl30_integration = true,
 	.resolution = 10,
 	.name = "meson-gxbb-saradc",
 };
 
-struct meson_sar_adc_data meson_sar_adc_gxl_data = {
+static const struct meson_sar_adc_data meson_sar_adc_gxl_data = {
+	.has_bl30_integration = true,
 	.resolution = 12,
 	.name = "meson-gxl-saradc",
 };
 
-struct meson_sar_adc_data meson_sar_adc_gxm_data = {
+static const struct meson_sar_adc_data meson_sar_adc_gxm_data = {
+	.has_bl30_integration = true,
 	.resolution = 12,
 	.name = "meson-gxm-saradc",
 };
 
 static const struct of_device_id meson_sar_adc_of_match[] = {
 	{
+		.compatible = "amlogic,meson8-saradc",
+		.data = &meson_sar_adc_meson8_data,
+	},
+	{
+		.compatible = "amlogic,meson8b-saradc",
+		.data = &meson_sar_adc_meson8b_data,
+	},
+	{
 		.compatible = "amlogic,meson-gxbb-saradc",
 		.data = &meson_sar_adc_gxbb_data,
 	}, {
diff --git a/drivers/iio/adc/mxs-lradc-adc.c b/drivers/iio/adc/mxs-lradc-adc.c
index b0c7d8ee5cb8..d32b34638c2f 100644
--- a/drivers/iio/adc/mxs-lradc-adc.c
+++ b/drivers/iio/adc/mxs-lradc-adc.c
@@ -48,7 +48,7 @@
 
 #define VREF_MV_BASE 1850
 
-const char *mx23_lradc_adc_irq_names[] = {
+static const char *mx23_lradc_adc_irq_names[] = {
 	"mxs-lradc-channel0",
 	"mxs-lradc-channel1",
 	"mxs-lradc-channel2",
@@ -57,7 +57,7 @@ const char *mx23_lradc_adc_irq_names[] = {
 	"mxs-lradc-channel5",
 };
 
-const char *mx28_lradc_adc_irq_names[] = {
+static const char *mx28_lradc_adc_irq_names[] = {
 	"mxs-lradc-thresh0",
 	"mxs-lradc-thresh1",
 	"mxs-lradc-channel0",
@@ -344,20 +344,20 @@ static ssize_t mxs_lradc_adc_show_scale_avail(struct device *dev,
 	IIO_DEVICE_ATTR(in_voltage##ch##_scale_available, 0444,\
 			mxs_lradc_adc_show_scale_avail, NULL, ch)
 
-SHOW_SCALE_AVAILABLE_ATTR(0);
-SHOW_SCALE_AVAILABLE_ATTR(1);
-SHOW_SCALE_AVAILABLE_ATTR(2);
-SHOW_SCALE_AVAILABLE_ATTR(3);
-SHOW_SCALE_AVAILABLE_ATTR(4);
-SHOW_SCALE_AVAILABLE_ATTR(5);
-SHOW_SCALE_AVAILABLE_ATTR(6);
-SHOW_SCALE_AVAILABLE_ATTR(7);
-SHOW_SCALE_AVAILABLE_ATTR(10);
-SHOW_SCALE_AVAILABLE_ATTR(11);
-SHOW_SCALE_AVAILABLE_ATTR(12);
-SHOW_SCALE_AVAILABLE_ATTR(13);
-SHOW_SCALE_AVAILABLE_ATTR(14);
-SHOW_SCALE_AVAILABLE_ATTR(15);
+static SHOW_SCALE_AVAILABLE_ATTR(0);
+static SHOW_SCALE_AVAILABLE_ATTR(1);
+static SHOW_SCALE_AVAILABLE_ATTR(2);
+static SHOW_SCALE_AVAILABLE_ATTR(3);
+static SHOW_SCALE_AVAILABLE_ATTR(4);
+static SHOW_SCALE_AVAILABLE_ATTR(5);
+static SHOW_SCALE_AVAILABLE_ATTR(6);
+static SHOW_SCALE_AVAILABLE_ATTR(7);
+static SHOW_SCALE_AVAILABLE_ATTR(10);
+static SHOW_SCALE_AVAILABLE_ATTR(11);
+static SHOW_SCALE_AVAILABLE_ATTR(12);
+static SHOW_SCALE_AVAILABLE_ATTR(13);
+static SHOW_SCALE_AVAILABLE_ATTR(14);
+static SHOW_SCALE_AVAILABLE_ATTR(15);
 
 static struct attribute *mxs_lradc_adc_attributes[] = {
 	&iio_dev_attr_in_voltage0_scale_available.dev_attr.attr,
@@ -718,9 +718,12 @@ static int mxs_lradc_adc_probe(struct platform_device *pdev)
 	adc->dev = dev;
 
 	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!iores)
+		return -EINVAL;
+
 	adc->base = devm_ioremap(dev, iores->start, resource_size(iores));
-	if (IS_ERR(adc->base))
-		return PTR_ERR(adc->base);
+	if (!adc->base)
+		return -ENOMEM;
 
 	init_completion(&adc->completion);
 	spin_lock_init(&adc->lock);
diff --git a/drivers/iio/adc/rcar-gyroadc.c b/drivers/iio/adc/rcar-gyroadc.c
index 018ed360e717..27a318164619 100644
--- a/drivers/iio/adc/rcar-gyroadc.c
+++ b/drivers/iio/adc/rcar-gyroadc.c
@@ -73,7 +73,7 @@ enum rcar_gyroadc_model {
 struct rcar_gyroadc {
 	struct device			*dev;
 	void __iomem			*regs;
-	struct clk			*iclk;
+	struct clk			*clk;
 	struct regulator		*vref[8];
 	unsigned int			num_channels;
 	enum rcar_gyroadc_model		model;
@@ -83,7 +83,7 @@ struct rcar_gyroadc {
 
 static void rcar_gyroadc_hw_init(struct rcar_gyroadc *priv)
 {
-	const unsigned long clk_mhz = clk_get_rate(priv->iclk) / 1000000;
+	const unsigned long clk_mhz = clk_get_rate(priv->clk) / 1000000;
 	const unsigned long clk_mul =
 		(priv->mode == RCAR_GYROADC_MODE_SELECT_1_MB88101A) ? 10 : 5;
 	unsigned long clk_len = clk_mhz * clk_mul;
@@ -510,9 +510,9 @@ static int rcar_gyroadc_probe(struct platform_device *pdev)
 	if (IS_ERR(priv->regs))
 		return PTR_ERR(priv->regs);
 
-	priv->iclk = devm_clk_get(dev, "if");
-	if (IS_ERR(priv->iclk)) {
-		ret = PTR_ERR(priv->iclk);
+	priv->clk = devm_clk_get(dev, "fck");
+	if (IS_ERR(priv->clk)) {
+		ret = PTR_ERR(priv->clk);
 		if (ret != -EPROBE_DEFER)
 			dev_err(dev, "Failed to get IF clock (ret=%i)\n", ret);
 		return ret;
@@ -536,7 +536,7 @@ static int rcar_gyroadc_probe(struct platform_device *pdev)
 	indio_dev->info = &rcar_gyroadc_iio_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	ret = clk_prepare_enable(priv->iclk);
+	ret = clk_prepare_enable(priv->clk);
 	if (ret) {
 		dev_err(dev, "Could not prepare or enable the IF clock.\n");
 		goto err_clk_if_enable;
@@ -565,7 +565,7 @@ err_iio_device_register:
 	pm_runtime_put_sync(dev);
 	pm_runtime_disable(dev);
 	pm_runtime_set_suspended(dev);
-	clk_disable_unprepare(priv->iclk);
+	clk_disable_unprepare(priv->clk);
 err_clk_if_enable:
 	rcar_gyroadc_deinit_supplies(indio_dev);
 
@@ -584,7 +584,7 @@ static int rcar_gyroadc_remove(struct platform_device *pdev)
 	pm_runtime_put_sync(dev);
 	pm_runtime_disable(dev);
 	pm_runtime_set_suspended(dev);
-	clk_disable_unprepare(priv->iclk);
+	clk_disable_unprepare(priv->clk);
 	rcar_gyroadc_deinit_supplies(indio_dev);
 
 	return 0;
diff --git a/drivers/iio/adc/stm32-adc-core.c b/drivers/iio/adc/stm32-adc-core.c
index 22b7c9321e78..e09233b03c05 100644
--- a/drivers/iio/adc/stm32-adc-core.c
+++ b/drivers/iio/adc/stm32-adc-core.c
@@ -49,19 +49,66 @@
 /* STM32 F4 maximum analog clock rate (from datasheet) */
 #define STM32F4_ADC_MAX_CLK_RATE	36000000
 
+/* STM32H7 - common registers for all ADC instances */
+#define STM32H7_ADC_CSR			(STM32_ADCX_COMN_OFFSET + 0x00)
+#define STM32H7_ADC_CCR			(STM32_ADCX_COMN_OFFSET + 0x08)
+
+/* STM32H7_ADC_CSR - bit fields */
+#define STM32H7_EOC_SLV			BIT(18)
+#define STM32H7_EOC_MST			BIT(2)
+
+/* STM32H7_ADC_CCR - bit fields */
+#define STM32H7_PRESC_SHIFT		18
+#define STM32H7_PRESC_MASK		GENMASK(21, 18)
+#define STM32H7_CKMODE_SHIFT		16
+#define STM32H7_CKMODE_MASK		GENMASK(17, 16)
+
+/* STM32 H7 maximum analog clock rate (from datasheet) */
+#define STM32H7_ADC_MAX_CLK_RATE	72000000
+
+/**
+ * stm32_adc_common_regs - stm32 common registers, compatible dependent data
+ * @csr:	common status register offset
+ * @eoc1:	adc1 end of conversion flag in @csr
+ * @eoc2:	adc2 end of conversion flag in @csr
+ * @eoc3:	adc3 end of conversion flag in @csr
+ */
+struct stm32_adc_common_regs {
+	u32 csr;
+	u32 eoc1_msk;
+	u32 eoc2_msk;
+	u32 eoc3_msk;
+};
+
+struct stm32_adc_priv;
+
+/**
+ * stm32_adc_priv_cfg - stm32 core compatible configuration data
+ * @regs:	common registers for all instances
+ * @clk_sel:	clock selection routine
+ */
+struct stm32_adc_priv_cfg {
+	const struct stm32_adc_common_regs *regs;
+	int (*clk_sel)(struct platform_device *, struct stm32_adc_priv *);
+};
+
 /**
  * struct stm32_adc_priv - stm32 ADC core private data
  * @irq:		irq for ADC block
  * @domain:		irq domain reference
  * @aclk:		clock reference for the analog circuitry
+ * @bclk:		bus clock common for all ADCs, depends on part used
  * @vref:		regulator reference
+ * @cfg:		compatible configuration data
  * @common:		common data for all ADC instances
  */
 struct stm32_adc_priv {
 	int				irq;
 	struct irq_domain		*domain;
 	struct clk			*aclk;
+	struct clk			*bclk;
 	struct regulator		*vref;
+	const struct stm32_adc_priv_cfg	*cfg;
 	struct stm32_adc_common		common;
 };
 
@@ -85,14 +132,23 @@ static int stm32f4_adc_clk_sel(struct platform_device *pdev,
 	u32 val;
 	int i;
 
+	/* stm32f4 has one clk input for analog (mandatory), enforce it here */
+	if (!priv->aclk) {
+		dev_err(&pdev->dev, "No 'adc' clock found\n");
+		return -ENOENT;
+	}
+
 	rate = clk_get_rate(priv->aclk);
 	for (i = 0; i < ARRAY_SIZE(stm32f4_pclk_div); i++) {
 		if ((rate / stm32f4_pclk_div[i]) <= STM32F4_ADC_MAX_CLK_RATE)
 			break;
 	}
-	if (i >= ARRAY_SIZE(stm32f4_pclk_div))
+	if (i >= ARRAY_SIZE(stm32f4_pclk_div)) {
+		dev_err(&pdev->dev, "adc clk selection failed\n");
 		return -EINVAL;
+	}
 
+	priv->common.rate = rate;
 	val = readl_relaxed(priv->common.base + STM32F4_ADC_CCR);
 	val &= ~STM32F4_ADC_ADCPRE_MASK;
 	val |= i << STM32F4_ADC_ADCPRE_SHIFT;
@@ -104,6 +160,126 @@ static int stm32f4_adc_clk_sel(struct platform_device *pdev,
 	return 0;
 }
 
+/**
+ * struct stm32h7_adc_ck_spec - specification for stm32h7 adc clock
+ * @ckmode: ADC clock mode, Async or sync with prescaler.
+ * @presc: prescaler bitfield for async clock mode
+ * @div: prescaler division ratio
+ */
+struct stm32h7_adc_ck_spec {
+	u32 ckmode;
+	u32 presc;
+	int div;
+};
+
+const struct stm32h7_adc_ck_spec stm32h7_adc_ckmodes_spec[] = {
+	/* 00: CK_ADC[1..3]: Asynchronous clock modes */
+	{ 0, 0, 1 },
+	{ 0, 1, 2 },
+	{ 0, 2, 4 },
+	{ 0, 3, 6 },
+	{ 0, 4, 8 },
+	{ 0, 5, 10 },
+	{ 0, 6, 12 },
+	{ 0, 7, 16 },
+	{ 0, 8, 32 },
+	{ 0, 9, 64 },
+	{ 0, 10, 128 },
+	{ 0, 11, 256 },
+	/* HCLK used: Synchronous clock modes (1, 2 or 4 prescaler) */
+	{ 1, 0, 1 },
+	{ 2, 0, 2 },
+	{ 3, 0, 4 },
+};
+
+static int stm32h7_adc_clk_sel(struct platform_device *pdev,
+			       struct stm32_adc_priv *priv)
+{
+	u32 ckmode, presc, val;
+	unsigned long rate;
+	int i, div;
+
+	/* stm32h7 bus clock is common for all ADC instances (mandatory) */
+	if (!priv->bclk) {
+		dev_err(&pdev->dev, "No 'bus' clock found\n");
+		return -ENOENT;
+	}
+
+	/*
+	 * stm32h7 can use either 'bus' or 'adc' clock for analog circuitry.
+	 * So, choice is to have bus clock mandatory and adc clock optional.
+	 * If optional 'adc' clock has been found, then try to use it first.
+	 */
+	if (priv->aclk) {
+		/*
+		 * Asynchronous clock modes (e.g. ckmode == 0)
+		 * From spec: PLL output musn't exceed max rate
+		 */
+		rate = clk_get_rate(priv->aclk);
+
+		for (i = 0; i < ARRAY_SIZE(stm32h7_adc_ckmodes_spec); i++) {
+			ckmode = stm32h7_adc_ckmodes_spec[i].ckmode;
+			presc = stm32h7_adc_ckmodes_spec[i].presc;
+			div = stm32h7_adc_ckmodes_spec[i].div;
+
+			if (ckmode)
+				continue;
+
+			if ((rate / div) <= STM32H7_ADC_MAX_CLK_RATE)
+				goto out;
+		}
+	}
+
+	/* Synchronous clock modes (e.g. ckmode is 1, 2 or 3) */
+	rate = clk_get_rate(priv->bclk);
+
+	for (i = 0; i < ARRAY_SIZE(stm32h7_adc_ckmodes_spec); i++) {
+		ckmode = stm32h7_adc_ckmodes_spec[i].ckmode;
+		presc = stm32h7_adc_ckmodes_spec[i].presc;
+		div = stm32h7_adc_ckmodes_spec[i].div;
+
+		if (!ckmode)
+			continue;
+
+		if ((rate / div) <= STM32H7_ADC_MAX_CLK_RATE)
+			goto out;
+	}
+
+	dev_err(&pdev->dev, "adc clk selection failed\n");
+	return -EINVAL;
+
+out:
+	/* rate used later by each ADC instance to control BOOST mode */
+	priv->common.rate = rate;
+
+	/* Set common clock mode and prescaler */
+	val = readl_relaxed(priv->common.base + STM32H7_ADC_CCR);
+	val &= ~(STM32H7_CKMODE_MASK | STM32H7_PRESC_MASK);
+	val |= ckmode << STM32H7_CKMODE_SHIFT;
+	val |= presc << STM32H7_PRESC_SHIFT;
+	writel_relaxed(val, priv->common.base + STM32H7_ADC_CCR);
+
+	dev_dbg(&pdev->dev, "Using %s clock/%d source at %ld kHz\n",
+		ckmode ? "bus" : "adc", div, rate / (div * 1000));
+
+	return 0;
+}
+
+/* STM32F4 common registers definitions */
+static const struct stm32_adc_common_regs stm32f4_adc_common_regs = {
+	.csr = STM32F4_ADC_CSR,
+	.eoc1_msk = STM32F4_EOC1,
+	.eoc2_msk = STM32F4_EOC2,
+	.eoc3_msk = STM32F4_EOC3,
+};
+
+/* STM32H7 common registers definitions */
+static const struct stm32_adc_common_regs stm32h7_adc_common_regs = {
+	.csr = STM32H7_ADC_CSR,
+	.eoc1_msk = STM32H7_EOC_MST,
+	.eoc2_msk = STM32H7_EOC_SLV,
+};
+
 /* ADC common interrupt for all instances */
 static void stm32_adc_irq_handler(struct irq_desc *desc)
 {
@@ -112,15 +288,15 @@ static void stm32_adc_irq_handler(struct irq_desc *desc)
 	u32 status;
 
 	chained_irq_enter(chip, desc);
-	status = readl_relaxed(priv->common.base + STM32F4_ADC_CSR);
+	status = readl_relaxed(priv->common.base + priv->cfg->regs->csr);
 
-	if (status & STM32F4_EOC1)
+	if (status & priv->cfg->regs->eoc1_msk)
 		generic_handle_irq(irq_find_mapping(priv->domain, 0));
 
-	if (status & STM32F4_EOC2)
+	if (status & priv->cfg->regs->eoc2_msk)
 		generic_handle_irq(irq_find_mapping(priv->domain, 1));
 
-	if (status & STM32F4_EOC3)
+	if (status & priv->cfg->regs->eoc3_msk)
 		generic_handle_irq(irq_find_mapping(priv->domain, 2));
 
 	chained_irq_exit(chip, desc);
@@ -186,6 +362,7 @@ static void stm32_adc_irq_remove(struct platform_device *pdev,
 static int stm32_adc_probe(struct platform_device *pdev)
 {
 	struct stm32_adc_priv *priv;
+	struct device *dev = &pdev->dev;
 	struct device_node *np = pdev->dev.of_node;
 	struct resource *res;
 	int ret;
@@ -197,6 +374,9 @@ static int stm32_adc_probe(struct platform_device *pdev)
 	if (!priv)
 		return -ENOMEM;
 
+	priv->cfg = (const struct stm32_adc_priv_cfg *)
+		of_match_device(dev->driver->of_match_table, dev)->data;
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	priv->common.base = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(priv->common.base))
@@ -227,25 +407,48 @@ static int stm32_adc_probe(struct platform_device *pdev)
 	priv->aclk = devm_clk_get(&pdev->dev, "adc");
 	if (IS_ERR(priv->aclk)) {
 		ret = PTR_ERR(priv->aclk);
-		dev_err(&pdev->dev, "Can't get 'adc' clock\n");
-		goto err_regulator_disable;
+		if (ret == -ENOENT) {
+			priv->aclk = NULL;
+		} else {
+			dev_err(&pdev->dev, "Can't get 'adc' clock\n");
+			goto err_regulator_disable;
+		}
 	}
 
-	ret = clk_prepare_enable(priv->aclk);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "adc clk enable failed\n");
-		goto err_regulator_disable;
+	if (priv->aclk) {
+		ret = clk_prepare_enable(priv->aclk);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "adc clk enable failed\n");
+			goto err_regulator_disable;
+		}
 	}
 
-	ret = stm32f4_adc_clk_sel(pdev, priv);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "adc clk selection failed\n");
-		goto err_clk_disable;
+	priv->bclk = devm_clk_get(&pdev->dev, "bus");
+	if (IS_ERR(priv->bclk)) {
+		ret = PTR_ERR(priv->bclk);
+		if (ret == -ENOENT) {
+			priv->bclk = NULL;
+		} else {
+			dev_err(&pdev->dev, "Can't get 'bus' clock\n");
+			goto err_aclk_disable;
+		}
+	}
+
+	if (priv->bclk) {
+		ret = clk_prepare_enable(priv->bclk);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "adc clk enable failed\n");
+			goto err_aclk_disable;
+		}
 	}
 
+	ret = priv->cfg->clk_sel(pdev, priv);
+	if (ret < 0)
+		goto err_bclk_disable;
+
 	ret = stm32_adc_irq_probe(pdev, priv);
 	if (ret < 0)
-		goto err_clk_disable;
+		goto err_bclk_disable;
 
 	platform_set_drvdata(pdev, &priv->common);
 
@@ -260,8 +463,13 @@ static int stm32_adc_probe(struct platform_device *pdev)
 err_irq_remove:
 	stm32_adc_irq_remove(pdev, priv);
 
-err_clk_disable:
-	clk_disable_unprepare(priv->aclk);
+err_bclk_disable:
+	if (priv->bclk)
+		clk_disable_unprepare(priv->bclk);
+
+err_aclk_disable:
+	if (priv->aclk)
+		clk_disable_unprepare(priv->aclk);
 
 err_regulator_disable:
 	regulator_disable(priv->vref);
@@ -276,15 +484,34 @@ static int stm32_adc_remove(struct platform_device *pdev)
 
 	of_platform_depopulate(&pdev->dev);
 	stm32_adc_irq_remove(pdev, priv);
-	clk_disable_unprepare(priv->aclk);
+	if (priv->bclk)
+		clk_disable_unprepare(priv->bclk);
+	if (priv->aclk)
+		clk_disable_unprepare(priv->aclk);
 	regulator_disable(priv->vref);
 
 	return 0;
 }
 
+static const struct stm32_adc_priv_cfg stm32f4_adc_priv_cfg = {
+	.regs = &stm32f4_adc_common_regs,
+	.clk_sel = stm32f4_adc_clk_sel,
+};
+
+static const struct stm32_adc_priv_cfg stm32h7_adc_priv_cfg = {
+	.regs = &stm32h7_adc_common_regs,
+	.clk_sel = stm32h7_adc_clk_sel,
+};
+
 static const struct of_device_id stm32_adc_of_match[] = {
-	{ .compatible = "st,stm32f4-adc-core" },
-	{},
+	{
+		.compatible = "st,stm32f4-adc-core",
+		.data = (void *)&stm32f4_adc_priv_cfg
+	}, {
+		.compatible = "st,stm32h7-adc-core",
+		.data = (void *)&stm32h7_adc_priv_cfg
+	}, {
+	},
 };
 MODULE_DEVICE_TABLE(of, stm32_adc_of_match);
 
diff --git a/drivers/iio/adc/stm32-adc-core.h b/drivers/iio/adc/stm32-adc-core.h
index 2ec7abbfbcaa..250ee958a669 100644
--- a/drivers/iio/adc/stm32-adc-core.h
+++ b/drivers/iio/adc/stm32-adc-core.h
@@ -43,11 +43,13 @@
  * struct stm32_adc_common - stm32 ADC driver common data (for all instances)
  * @base:		control registers base cpu addr
  * @phys_base:		control registers base physical addr
+ * @rate:		clock rate used for analog circuitry
  * @vref_mv:		vref voltage (mv)
  */
 struct stm32_adc_common {
 	void __iomem			*base;
 	phys_addr_t			phys_base;
+	unsigned long			rate;
 	int				vref_mv;
 };
 
diff --git a/drivers/iio/adc/stm32-adc.c b/drivers/iio/adc/stm32-adc.c
index c28e7ff80e11..5bfcc1f13105 100644
--- a/drivers/iio/adc/stm32-adc.c
+++ b/drivers/iio/adc/stm32-adc.c
@@ -31,9 +31,11 @@
 #include <linux/iio/triggered_buffer.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
+#include <linux/iopoll.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 
 #include "stm32-adc-core.h"
 
@@ -76,6 +78,78 @@
 #define STM32F4_DMA			BIT(8)
 #define STM32F4_ADON			BIT(0)
 
+/* STM32H7 - Registers for each ADC instance */
+#define STM32H7_ADC_ISR			0x00
+#define STM32H7_ADC_IER			0x04
+#define STM32H7_ADC_CR			0x08
+#define STM32H7_ADC_CFGR		0x0C
+#define STM32H7_ADC_PCSEL		0x1C
+#define STM32H7_ADC_SQR1		0x30
+#define STM32H7_ADC_SQR2		0x34
+#define STM32H7_ADC_SQR3		0x38
+#define STM32H7_ADC_SQR4		0x3C
+#define STM32H7_ADC_DR			0x40
+#define STM32H7_ADC_CALFACT		0xC4
+#define STM32H7_ADC_CALFACT2		0xC8
+
+/* STM32H7_ADC_ISR - bit fields */
+#define STM32H7_EOC			BIT(2)
+#define STM32H7_ADRDY			BIT(0)
+
+/* STM32H7_ADC_IER - bit fields */
+#define STM32H7_EOCIE			STM32H7_EOC
+
+/* STM32H7_ADC_CR - bit fields */
+#define STM32H7_ADCAL			BIT(31)
+#define STM32H7_ADCALDIF		BIT(30)
+#define STM32H7_DEEPPWD			BIT(29)
+#define STM32H7_ADVREGEN		BIT(28)
+#define STM32H7_LINCALRDYW6		BIT(27)
+#define STM32H7_LINCALRDYW5		BIT(26)
+#define STM32H7_LINCALRDYW4		BIT(25)
+#define STM32H7_LINCALRDYW3		BIT(24)
+#define STM32H7_LINCALRDYW2		BIT(23)
+#define STM32H7_LINCALRDYW1		BIT(22)
+#define STM32H7_ADCALLIN		BIT(16)
+#define STM32H7_BOOST			BIT(8)
+#define STM32H7_ADSTP			BIT(4)
+#define STM32H7_ADSTART			BIT(2)
+#define STM32H7_ADDIS			BIT(1)
+#define STM32H7_ADEN			BIT(0)
+
+/* STM32H7_ADC_CFGR bit fields */
+#define STM32H7_EXTEN_SHIFT		10
+#define STM32H7_EXTEN_MASK		GENMASK(11, 10)
+#define STM32H7_EXTSEL_SHIFT		5
+#define STM32H7_EXTSEL_MASK		GENMASK(9, 5)
+#define STM32H7_RES_SHIFT		2
+#define STM32H7_RES_MASK		GENMASK(4, 2)
+#define STM32H7_DMNGT_SHIFT		0
+#define STM32H7_DMNGT_MASK		GENMASK(1, 0)
+
+enum stm32h7_adc_dmngt {
+	STM32H7_DMNGT_DR_ONLY,		/* Regular data in DR only */
+	STM32H7_DMNGT_DMA_ONESHOT,	/* DMA one shot mode */
+	STM32H7_DMNGT_DFSDM,		/* DFSDM mode */
+	STM32H7_DMNGT_DMA_CIRC,		/* DMA circular mode */
+};
+
+/* STM32H7_ADC_CALFACT - bit fields */
+#define STM32H7_CALFACT_D_SHIFT		16
+#define STM32H7_CALFACT_D_MASK		GENMASK(26, 16)
+#define STM32H7_CALFACT_S_SHIFT		0
+#define STM32H7_CALFACT_S_MASK		GENMASK(10, 0)
+
+/* STM32H7_ADC_CALFACT2 - bit fields */
+#define STM32H7_LINCALFACT_SHIFT	0
+#define STM32H7_LINCALFACT_MASK		GENMASK(29, 0)
+
+/* Number of linear calibration shadow registers / LINCALRDYW control bits */
+#define STM32H7_LINCALFACT_NUM		6
+
+/* BOOST bit must be set on STM32H7 when ADC clock is above 20MHz */
+#define STM32H7_BOOST_CLKRATE		20000000UL
+
 #define STM32_ADC_MAX_SQ		16	/* SQ1..SQ16 */
 #define STM32_ADC_TIMEOUT_US		100000
 #define STM32_ADC_TIMEOUT	(msecs_to_jiffies(STM32_ADC_TIMEOUT_US / 1000))
@@ -121,6 +195,18 @@ struct stm32_adc_trig_info {
 };
 
 /**
+ * struct stm32_adc_calib - optional adc calibration data
+ * @calfact_s: Calibration offset for single ended channels
+ * @calfact_d: Calibration offset in differential
+ * @lincalfact: Linearity calibration factor
+ */
+struct stm32_adc_calib {
+	u32			calfact_s;
+	u32			calfact_d;
+	u32			lincalfact[STM32H7_LINCALFACT_NUM];
+};
+
+/**
  * stm32_adc_regs - stm32 ADC misc registers & bitfield desc
  * @reg:		register offset
  * @mask:		bitfield mask
@@ -133,9 +219,56 @@ struct stm32_adc_regs {
 };
 
 /**
+ * stm32_adc_regspec - stm32 registers definition, compatible dependent data
+ * @dr:			data register offset
+ * @ier_eoc:		interrupt enable register & eocie bitfield
+ * @isr_eoc:		interrupt status register & eoc bitfield
+ * @sqr:		reference to sequence registers array
+ * @exten:		trigger control register & bitfield
+ * @extsel:		trigger selection register & bitfield
+ * @res:		resolution selection register & bitfield
+ */
+struct stm32_adc_regspec {
+	const u32 dr;
+	const struct stm32_adc_regs ier_eoc;
+	const struct stm32_adc_regs isr_eoc;
+	const struct stm32_adc_regs *sqr;
+	const struct stm32_adc_regs exten;
+	const struct stm32_adc_regs extsel;
+	const struct stm32_adc_regs res;
+};
+
+struct stm32_adc;
+
+/**
+ * stm32_adc_cfg - stm32 compatible configuration data
+ * @regs:		registers descriptions
+ * @adc_info:		per instance input channels definitions
+ * @trigs:		external trigger sources
+ * @clk_required:	clock is required
+ * @selfcalib:		optional routine for self-calibration
+ * @prepare:		optional prepare routine (power-up, enable)
+ * @start_conv:		routine to start conversions
+ * @stop_conv:		routine to stop conversions
+ * @unprepare:		optional unprepare routine (disable, power-down)
+ */
+struct stm32_adc_cfg {
+	const struct stm32_adc_regspec	*regs;
+	const struct stm32_adc_info	*adc_info;
+	struct stm32_adc_trig_info	*trigs;
+	bool clk_required;
+	int (*selfcalib)(struct stm32_adc *);
+	int (*prepare)(struct stm32_adc *);
+	void (*start_conv)(struct stm32_adc *, bool dma);
+	void (*stop_conv)(struct stm32_adc *);
+	void (*unprepare)(struct stm32_adc *);
+};
+
+/**
  * struct stm32_adc - private data of each ADC IIO instance
  * @common:		reference to ADC block common data
  * @offset:		ADC instance register offset in ADC block
+ * @cfg:		compatible configuration data
  * @completion:		end of single conversion completion
  * @buffer:		data buffer
  * @clk:		clock for this adc instance
@@ -149,10 +282,13 @@ struct stm32_adc_regs {
  * @rx_buf:		dma rx buffer cpu address
  * @rx_dma_buf:		dma rx buffer bus address
  * @rx_buf_sz:		dma rx buffer size
+ * @pcsel		bitmask to preselect channels on some devices
+ * @cal:		optional calibration data on some devices
  */
 struct stm32_adc {
 	struct stm32_adc_common	*common;
 	u32			offset;
+	const struct stm32_adc_cfg	*cfg;
 	struct completion	completion;
 	u16			buffer[STM32_ADC_MAX_SQ];
 	struct clk		*clk;
@@ -166,6 +302,8 @@ struct stm32_adc {
 	u8			*rx_buf;
 	dma_addr_t		rx_dma_buf;
 	unsigned int		rx_buf_sz;
+	u32			pcsel;
+	struct stm32_adc_calib	cal;
 };
 
 /**
@@ -180,8 +318,26 @@ struct stm32_adc_chan_spec {
 	const char		*name;
 };
 
-/* Input definitions common for all STM32F4 instances */
-static const struct stm32_adc_chan_spec stm32f4_adc123_channels[] = {
+/**
+ * struct stm32_adc_info - stm32 ADC, per instance config data
+ * @channels:		Reference to stm32 channels spec
+ * @max_channels:	Number of channels
+ * @resolutions:	available resolutions
+ * @num_res:		number of available resolutions
+ */
+struct stm32_adc_info {
+	const struct stm32_adc_chan_spec *channels;
+	int max_channels;
+	const unsigned int *resolutions;
+	const unsigned int num_res;
+};
+
+/*
+ * Input definitions common for all instances:
+ * stm32f4 can have up to 16 channels
+ * stm32h7 can have up to 20 channels
+ */
+static const struct stm32_adc_chan_spec stm32_adc_channels[] = {
 	{ IIO_VOLTAGE, 0, "in0" },
 	{ IIO_VOLTAGE, 1, "in1" },
 	{ IIO_VOLTAGE, 2, "in2" },
@@ -198,6 +354,10 @@ static const struct stm32_adc_chan_spec stm32f4_adc123_channels[] = {
 	{ IIO_VOLTAGE, 13, "in13" },
 	{ IIO_VOLTAGE, 14, "in14" },
 	{ IIO_VOLTAGE, 15, "in15" },
+	{ IIO_VOLTAGE, 16, "in16" },
+	{ IIO_VOLTAGE, 17, "in17" },
+	{ IIO_VOLTAGE, 18, "in18" },
+	{ IIO_VOLTAGE, 19, "in19" },
 };
 
 static const unsigned int stm32f4_adc_resolutions[] = {
@@ -205,6 +365,25 @@ static const unsigned int stm32f4_adc_resolutions[] = {
 	12, 10, 8, 6,
 };
 
+static const struct stm32_adc_info stm32f4_adc_info = {
+	.channels = stm32_adc_channels,
+	.max_channels = 16,
+	.resolutions = stm32f4_adc_resolutions,
+	.num_res = ARRAY_SIZE(stm32f4_adc_resolutions),
+};
+
+static const unsigned int stm32h7_adc_resolutions[] = {
+	/* sorted values so the index matches RES[2:0] in STM32H7_ADC_CFGR */
+	16, 14, 12, 10, 8,
+};
+
+static const struct stm32_adc_info stm32h7_adc_info = {
+	.channels = stm32_adc_channels,
+	.max_channels = 20,
+	.resolutions = stm32h7_adc_resolutions,
+	.num_res = ARRAY_SIZE(stm32h7_adc_resolutions),
+};
+
 /**
  * stm32f4_sq - describe regular sequence registers
  * - L: sequence len (register & bit field)
@@ -252,6 +431,69 @@ static struct stm32_adc_trig_info stm32f4_adc_trigs[] = {
 	{}, /* sentinel */
 };
 
+static const struct stm32_adc_regspec stm32f4_adc_regspec = {
+	.dr = STM32F4_ADC_DR,
+	.ier_eoc = { STM32F4_ADC_CR1, STM32F4_EOCIE },
+	.isr_eoc = { STM32F4_ADC_SR, STM32F4_EOC },
+	.sqr = stm32f4_sq,
+	.exten = { STM32F4_ADC_CR2, STM32F4_EXTEN_MASK, STM32F4_EXTEN_SHIFT },
+	.extsel = { STM32F4_ADC_CR2, STM32F4_EXTSEL_MASK,
+		    STM32F4_EXTSEL_SHIFT },
+	.res = { STM32F4_ADC_CR1, STM32F4_RES_MASK, STM32F4_RES_SHIFT },
+};
+
+static const struct stm32_adc_regs stm32h7_sq[STM32_ADC_MAX_SQ + 1] = {
+	/* L: len bit field description to be kept as first element */
+	{ STM32H7_ADC_SQR1, GENMASK(3, 0), 0 },
+	/* SQ1..SQ16 registers & bit fields (reg, mask, shift) */
+	{ STM32H7_ADC_SQR1, GENMASK(10, 6), 6 },
+	{ STM32H7_ADC_SQR1, GENMASK(16, 12), 12 },
+	{ STM32H7_ADC_SQR1, GENMASK(22, 18), 18 },
+	{ STM32H7_ADC_SQR1, GENMASK(28, 24), 24 },
+	{ STM32H7_ADC_SQR2, GENMASK(4, 0), 0 },
+	{ STM32H7_ADC_SQR2, GENMASK(10, 6), 6 },
+	{ STM32H7_ADC_SQR2, GENMASK(16, 12), 12 },
+	{ STM32H7_ADC_SQR2, GENMASK(22, 18), 18 },
+	{ STM32H7_ADC_SQR2, GENMASK(28, 24), 24 },
+	{ STM32H7_ADC_SQR3, GENMASK(4, 0), 0 },
+	{ STM32H7_ADC_SQR3, GENMASK(10, 6), 6 },
+	{ STM32H7_ADC_SQR3, GENMASK(16, 12), 12 },
+	{ STM32H7_ADC_SQR3, GENMASK(22, 18), 18 },
+	{ STM32H7_ADC_SQR3, GENMASK(28, 24), 24 },
+	{ STM32H7_ADC_SQR4, GENMASK(4, 0), 0 },
+	{ STM32H7_ADC_SQR4, GENMASK(10, 6), 6 },
+};
+
+/* STM32H7 external trigger sources for all instances */
+static struct stm32_adc_trig_info stm32h7_adc_trigs[] = {
+	{ TIM1_CH1, STM32_EXT0 },
+	{ TIM1_CH2, STM32_EXT1 },
+	{ TIM1_CH3, STM32_EXT2 },
+	{ TIM2_CH2, STM32_EXT3 },
+	{ TIM3_TRGO, STM32_EXT4 },
+	{ TIM4_CH4, STM32_EXT5 },
+	{ TIM8_TRGO, STM32_EXT7 },
+	{ TIM8_TRGO2, STM32_EXT8 },
+	{ TIM1_TRGO, STM32_EXT9 },
+	{ TIM1_TRGO2, STM32_EXT10 },
+	{ TIM2_TRGO, STM32_EXT11 },
+	{ TIM4_TRGO, STM32_EXT12 },
+	{ TIM6_TRGO, STM32_EXT13 },
+	{ TIM3_CH4, STM32_EXT15 },
+	{},
+};
+
+static const struct stm32_adc_regspec stm32h7_adc_regspec = {
+	.dr = STM32H7_ADC_DR,
+	.ier_eoc = { STM32H7_ADC_IER, STM32H7_EOCIE },
+	.isr_eoc = { STM32H7_ADC_ISR, STM32H7_EOC },
+	.sqr = stm32h7_sq,
+	.exten = { STM32H7_ADC_CFGR, STM32H7_EXTEN_MASK, STM32H7_EXTEN_SHIFT },
+	.extsel = { STM32H7_ADC_CFGR, STM32H7_EXTSEL_MASK,
+		    STM32H7_EXTSEL_SHIFT },
+	.res = { STM32H7_ADC_CFGR, STM32H7_RES_MASK, STM32H7_RES_SHIFT },
+};
+
 /**
  * STM32 ADC registers access routines
  * @adc: stm32 adc instance
@@ -265,6 +507,12 @@ static u32 stm32_adc_readl(struct stm32_adc *adc, u32 reg)
 	return readl_relaxed(adc->common->base + adc->offset + reg);
 }
 
+#define stm32_adc_readl_addr(addr)	stm32_adc_readl(adc, addr)
+
+#define stm32_adc_readl_poll_timeout(reg, val, cond, sleep_us, timeout_us) \
+	readx_poll_timeout(stm32_adc_readl_addr, reg, val, \
+			   cond, sleep_us, timeout_us)
+
 static u16 stm32_adc_readw(struct stm32_adc *adc, u32 reg)
 {
 	return readw_relaxed(adc->common->base + adc->offset + reg);
@@ -299,7 +547,8 @@ static void stm32_adc_clr_bits(struct stm32_adc *adc, u32 reg, u32 bits)
  */
 static void stm32_adc_conv_irq_enable(struct stm32_adc *adc)
 {
-	stm32_adc_set_bits(adc, STM32F4_ADC_CR1, STM32F4_EOCIE);
+	stm32_adc_set_bits(adc, adc->cfg->regs->ier_eoc.reg,
+			   adc->cfg->regs->ier_eoc.mask);
 };
 
 /**
@@ -308,19 +557,22 @@ static void stm32_adc_conv_irq_enable(struct stm32_adc *adc)
  */
 static void stm32_adc_conv_irq_disable(struct stm32_adc *adc)
 {
-	stm32_adc_clr_bits(adc, STM32F4_ADC_CR1, STM32F4_EOCIE);
+	stm32_adc_clr_bits(adc, adc->cfg->regs->ier_eoc.reg,
+			   adc->cfg->regs->ier_eoc.mask);
 }
 
 static void stm32_adc_set_res(struct stm32_adc *adc)
 {
-	u32 val = stm32_adc_readl(adc, STM32F4_ADC_CR1);
+	const struct stm32_adc_regs *res = &adc->cfg->regs->res;
+	u32 val;
 
-	val = (val & ~STM32F4_RES_MASK) | (adc->res << STM32F4_RES_SHIFT);
-	stm32_adc_writel(adc, STM32F4_ADC_CR1, val);
+	val = stm32_adc_readl(adc, res->reg);
+	val = (val & ~res->mask) | (adc->res << res->shift);
+	stm32_adc_writel(adc, res->reg, val);
 }
 
 /**
- * stm32_adc_start_conv() - Start conversions for regular channels.
+ * stm32f4_adc_start_conv() - Start conversions for regular channels.
  * @adc: stm32 adc instance
  * @dma: use dma to transfer conversion result
  *
@@ -329,7 +581,7 @@ static void stm32_adc_set_res(struct stm32_adc *adc)
  * conversions, in IIO buffer modes. Otherwise, use ADC interrupt with direct
  * DR read instead (e.g. read_raw, or triggered buffer mode without DMA).
  */
-static void stm32_adc_start_conv(struct stm32_adc *adc, bool dma)
+static void stm32f4_adc_start_conv(struct stm32_adc *adc, bool dma)
 {
 	stm32_adc_set_bits(adc, STM32F4_ADC_CR1, STM32F4_SCAN);
 
@@ -347,7 +599,7 @@ static void stm32_adc_start_conv(struct stm32_adc *adc, bool dma)
 		stm32_adc_set_bits(adc, STM32F4_ADC_CR2, STM32F4_SWSTART);
 }
 
-static void stm32_adc_stop_conv(struct stm32_adc *adc)
+static void stm32f4_adc_stop_conv(struct stm32_adc *adc)
 {
 	stm32_adc_clr_bits(adc, STM32F4_ADC_CR2, STM32F4_EXTEN_MASK);
 	stm32_adc_clr_bits(adc, STM32F4_ADC_SR, STM32F4_STRT);
@@ -357,6 +609,324 @@ static void stm32_adc_stop_conv(struct stm32_adc *adc)
 			   STM32F4_ADON | STM32F4_DMA | STM32F4_DDS);
 }
 
+static void stm32h7_adc_start_conv(struct stm32_adc *adc, bool dma)
+{
+	enum stm32h7_adc_dmngt dmngt;
+	unsigned long flags;
+	u32 val;
+
+	if (dma)
+		dmngt = STM32H7_DMNGT_DMA_CIRC;
+	else
+		dmngt = STM32H7_DMNGT_DR_ONLY;
+
+	spin_lock_irqsave(&adc->lock, flags);
+	val = stm32_adc_readl(adc, STM32H7_ADC_CFGR);
+	val = (val & ~STM32H7_DMNGT_MASK) | (dmngt << STM32H7_DMNGT_SHIFT);
+	stm32_adc_writel(adc, STM32H7_ADC_CFGR, val);
+	spin_unlock_irqrestore(&adc->lock, flags);
+
+	stm32_adc_set_bits(adc, STM32H7_ADC_CR, STM32H7_ADSTART);
+}
+
+static void stm32h7_adc_stop_conv(struct stm32_adc *adc)
+{
+	struct iio_dev *indio_dev = iio_priv_to_dev(adc);
+	int ret;
+	u32 val;
+
+	stm32_adc_set_bits(adc, STM32H7_ADC_CR, STM32H7_ADSTP);
+
+	ret = stm32_adc_readl_poll_timeout(STM32H7_ADC_CR, val,
+					   !(val & (STM32H7_ADSTART)),
+					   100, STM32_ADC_TIMEOUT_US);
+	if (ret)
+		dev_warn(&indio_dev->dev, "stop failed\n");
+
+	stm32_adc_clr_bits(adc, STM32H7_ADC_CFGR, STM32H7_DMNGT_MASK);
+}
+
+static void stm32h7_adc_exit_pwr_down(struct stm32_adc *adc)
+{
+	/* Exit deep power down, then enable ADC voltage regulator */
+	stm32_adc_clr_bits(adc, STM32H7_ADC_CR, STM32H7_DEEPPWD);
+	stm32_adc_set_bits(adc, STM32H7_ADC_CR, STM32H7_ADVREGEN);
+
+	if (adc->common->rate > STM32H7_BOOST_CLKRATE)
+		stm32_adc_set_bits(adc, STM32H7_ADC_CR, STM32H7_BOOST);
+
+	/* Wait for startup time */
+	usleep_range(10, 20);
+}
+
+static void stm32h7_adc_enter_pwr_down(struct stm32_adc *adc)
+{
+	stm32_adc_clr_bits(adc, STM32H7_ADC_CR, STM32H7_BOOST);
+
+	/* Setting DEEPPWD disables ADC vreg and clears ADVREGEN */
+	stm32_adc_set_bits(adc, STM32H7_ADC_CR, STM32H7_DEEPPWD);
+}
+
+static int stm32h7_adc_enable(struct stm32_adc *adc)
+{
+	struct iio_dev *indio_dev = iio_priv_to_dev(adc);
+	int ret;
+	u32 val;
+
+	/* Clear ADRDY by writing one, then enable ADC */
+	stm32_adc_set_bits(adc, STM32H7_ADC_ISR, STM32H7_ADRDY);
+	stm32_adc_set_bits(adc, STM32H7_ADC_CR, STM32H7_ADEN);
+
+	/* Poll for ADRDY to be set (after adc startup time) */
+	ret = stm32_adc_readl_poll_timeout(STM32H7_ADC_ISR, val,
+					   val & STM32H7_ADRDY,
+					   100, STM32_ADC_TIMEOUT_US);
+	if (ret) {
+		stm32_adc_clr_bits(adc, STM32H7_ADC_CR, STM32H7_ADEN);
+		dev_err(&indio_dev->dev, "Failed to enable ADC\n");
+	}
+
+	return ret;
+}
+
+static void stm32h7_adc_disable(struct stm32_adc *adc)
+{
+	struct iio_dev *indio_dev = iio_priv_to_dev(adc);
+	int ret;
+	u32 val;
+
+	/* Disable ADC and wait until it's effectively disabled */
+	stm32_adc_set_bits(adc, STM32H7_ADC_CR, STM32H7_ADDIS);
+	ret = stm32_adc_readl_poll_timeout(STM32H7_ADC_CR, val,
+					   !(val & STM32H7_ADEN), 100,
+					   STM32_ADC_TIMEOUT_US);
+	if (ret)
+		dev_warn(&indio_dev->dev, "Failed to disable\n");
+}
+
+/**
+ * stm32h7_adc_read_selfcalib() - read calibration shadow regs, save result
+ * @adc: stm32 adc instance
+ */
+static int stm32h7_adc_read_selfcalib(struct stm32_adc *adc)
+{
+	struct iio_dev *indio_dev = iio_priv_to_dev(adc);
+	int i, ret;
+	u32 lincalrdyw_mask, val;
+
+	/* Enable adc so LINCALRDYW1..6 bits are writable */
+	ret = stm32h7_adc_enable(adc);
+	if (ret)
+		return ret;
+
+	/* Read linearity calibration */
+	lincalrdyw_mask = STM32H7_LINCALRDYW6;
+	for (i = STM32H7_LINCALFACT_NUM - 1; i >= 0; i--) {
+		/* Clear STM32H7_LINCALRDYW[6..1]: transfer calib to CALFACT2 */
+		stm32_adc_clr_bits(adc, STM32H7_ADC_CR, lincalrdyw_mask);
+
+		/* Poll: wait calib data to be ready in CALFACT2 register */
+		ret = stm32_adc_readl_poll_timeout(STM32H7_ADC_CR, val,
+						   !(val & lincalrdyw_mask),
+						   100, STM32_ADC_TIMEOUT_US);
+		if (ret) {
+			dev_err(&indio_dev->dev, "Failed to read calfact\n");
+			goto disable;
+		}
+
+		val = stm32_adc_readl(adc, STM32H7_ADC_CALFACT2);
+		adc->cal.lincalfact[i] = (val & STM32H7_LINCALFACT_MASK);
+		adc->cal.lincalfact[i] >>= STM32H7_LINCALFACT_SHIFT;
+
+		lincalrdyw_mask >>= 1;
+	}
+
+	/* Read offset calibration */
+	val = stm32_adc_readl(adc, STM32H7_ADC_CALFACT);
+	adc->cal.calfact_s = (val & STM32H7_CALFACT_S_MASK);
+	adc->cal.calfact_s >>= STM32H7_CALFACT_S_SHIFT;
+	adc->cal.calfact_d = (val & STM32H7_CALFACT_D_MASK);
+	adc->cal.calfact_d >>= STM32H7_CALFACT_D_SHIFT;
+
+disable:
+	stm32h7_adc_disable(adc);
+
+	return ret;
+}
+
+/**
+ * stm32h7_adc_restore_selfcalib() - Restore saved self-calibration result
+ * @adc: stm32 adc instance
+ * Note: ADC must be enabled, with no on-going conversions.
+ */
+static int stm32h7_adc_restore_selfcalib(struct stm32_adc *adc)
+{
+	struct iio_dev *indio_dev = iio_priv_to_dev(adc);
+	int i, ret;
+	u32 lincalrdyw_mask, val;
+
+	val = (adc->cal.calfact_s << STM32H7_CALFACT_S_SHIFT) |
+		(adc->cal.calfact_d << STM32H7_CALFACT_D_SHIFT);
+	stm32_adc_writel(adc, STM32H7_ADC_CALFACT, val);
+
+	lincalrdyw_mask = STM32H7_LINCALRDYW6;
+	for (i = STM32H7_LINCALFACT_NUM - 1; i >= 0; i--) {
+		/*
+		 * Write saved calibration data to shadow registers:
+		 * Write CALFACT2, and set LINCALRDYW[6..1] bit to trigger
+		 * data write. Then poll to wait for complete transfer.
+		 */
+		val = adc->cal.lincalfact[i] << STM32H7_LINCALFACT_SHIFT;
+		stm32_adc_writel(adc, STM32H7_ADC_CALFACT2, val);
+		stm32_adc_set_bits(adc, STM32H7_ADC_CR, lincalrdyw_mask);
+		ret = stm32_adc_readl_poll_timeout(STM32H7_ADC_CR, val,
+						   val & lincalrdyw_mask,
+						   100, STM32_ADC_TIMEOUT_US);
+		if (ret) {
+			dev_err(&indio_dev->dev, "Failed to write calfact\n");
+			return ret;
+		}
+
+		/*
+		 * Read back calibration data, has two effects:
+		 * - It ensures bits LINCALRDYW[6..1] are kept cleared
+		 *   for next time calibration needs to be restored.
+		 * - BTW, bit clear triggers a read, then check data has been
+		 *   correctly written.
+		 */
+		stm32_adc_clr_bits(adc, STM32H7_ADC_CR, lincalrdyw_mask);
+		ret = stm32_adc_readl_poll_timeout(STM32H7_ADC_CR, val,
+						   !(val & lincalrdyw_mask),
+						   100, STM32_ADC_TIMEOUT_US);
+		if (ret) {
+			dev_err(&indio_dev->dev, "Failed to read calfact\n");
+			return ret;
+		}
+		val = stm32_adc_readl(adc, STM32H7_ADC_CALFACT2);
+		if (val != adc->cal.lincalfact[i] << STM32H7_LINCALFACT_SHIFT) {
+			dev_err(&indio_dev->dev, "calfact not consistent\n");
+			return -EIO;
+		}
+
+		lincalrdyw_mask >>= 1;
+	}
+
+	return 0;
+}
+
+/**
+ * Fixed timeout value for ADC calibration.
+ * worst cases:
+ * - low clock frequency
+ * - maximum prescalers
+ * Calibration requires:
+ * - 131,072 ADC clock cycle for the linear calibration
+ * - 20 ADC clock cycle for the offset calibration
+ *
+ * Set to 100ms for now
+ */
+#define STM32H7_ADC_CALIB_TIMEOUT_US		100000
+
+/**
+ * stm32h7_adc_selfcalib() - Procedure to calibrate ADC (from power down)
+ * @adc: stm32 adc instance
+ * Exit from power down, calibrate ADC, then return to power down.
+ */
+static int stm32h7_adc_selfcalib(struct stm32_adc *adc)
+{
+	struct iio_dev *indio_dev = iio_priv_to_dev(adc);
+	int ret;
+	u32 val;
+
+	stm32h7_adc_exit_pwr_down(adc);
+
+	/*
+	 * Select calibration mode:
+	 * - Offset calibration for single ended inputs
+	 * - No linearity calibration (do it later, before reading it)
+	 */
+	stm32_adc_clr_bits(adc, STM32H7_ADC_CR, STM32H7_ADCALDIF);
+	stm32_adc_clr_bits(adc, STM32H7_ADC_CR, STM32H7_ADCALLIN);
+
+	/* Start calibration, then wait for completion */
+	stm32_adc_set_bits(adc, STM32H7_ADC_CR, STM32H7_ADCAL);
+	ret = stm32_adc_readl_poll_timeout(STM32H7_ADC_CR, val,
+					   !(val & STM32H7_ADCAL), 100,
+					   STM32H7_ADC_CALIB_TIMEOUT_US);
+	if (ret) {
+		dev_err(&indio_dev->dev, "calibration failed\n");
+		goto pwr_dwn;
+	}
+
+	/*
+	 * Select calibration mode, then start calibration:
+	 * - Offset calibration for differential input
+	 * - Linearity calibration (needs to be done only once for single/diff)
+	 *   will run simultaneously with offset calibration.
+	 */
+	stm32_adc_set_bits(adc, STM32H7_ADC_CR,
+			   STM32H7_ADCALDIF | STM32H7_ADCALLIN);
+	stm32_adc_set_bits(adc, STM32H7_ADC_CR, STM32H7_ADCAL);
+	ret = stm32_adc_readl_poll_timeout(STM32H7_ADC_CR, val,
+					   !(val & STM32H7_ADCAL), 100,
+					   STM32H7_ADC_CALIB_TIMEOUT_US);
+	if (ret) {
+		dev_err(&indio_dev->dev, "calibration failed\n");
+		goto pwr_dwn;
+	}
+
+	stm32_adc_clr_bits(adc, STM32H7_ADC_CR,
+			   STM32H7_ADCALDIF | STM32H7_ADCALLIN);
+
+	/* Read calibration result for future reference */
+	ret = stm32h7_adc_read_selfcalib(adc);
+
+pwr_dwn:
+	stm32h7_adc_enter_pwr_down(adc);
+
+	return ret;
+}
+
+/**
+ * stm32h7_adc_prepare() - Leave power down mode to enable ADC.
+ * @adc: stm32 adc instance
+ * Leave power down mode.
+ * Enable ADC.
+ * Restore calibration data.
+ * Pre-select channels that may be used in PCSEL (required by input MUX / IO).
+ */
+static int stm32h7_adc_prepare(struct stm32_adc *adc)
+{
+	int ret;
+
+	stm32h7_adc_exit_pwr_down(adc);
+
+	ret = stm32h7_adc_enable(adc);
+	if (ret)
+		goto pwr_dwn;
+
+	ret = stm32h7_adc_restore_selfcalib(adc);
+	if (ret)
+		goto disable;
+
+	stm32_adc_writel(adc, STM32H7_ADC_PCSEL, adc->pcsel);
+
+	return 0;
+
+disable:
+	stm32h7_adc_disable(adc);
+pwr_dwn:
+	stm32h7_adc_enter_pwr_down(adc);
+
+	return ret;
+}
+
+static void stm32h7_adc_unprepare(struct stm32_adc *adc)
+{
+	stm32h7_adc_disable(adc);
+	stm32h7_adc_enter_pwr_down(adc);
+}
+
 /**
  * stm32_adc_conf_scan_seq() - Build regular channels scan sequence
  * @indio_dev: IIO device
@@ -371,6 +941,7 @@ static int stm32_adc_conf_scan_seq(struct iio_dev *indio_dev,
 				   const unsigned long *scan_mask)
 {
 	struct stm32_adc *adc = iio_priv(indio_dev);
+	const struct stm32_adc_regs *sqr = adc->cfg->regs->sqr;
 	const struct iio_chan_spec *chan;
 	u32 val, bit;
 	int i = 0;
@@ -388,20 +959,20 @@ static int stm32_adc_conf_scan_seq(struct iio_dev *indio_dev,
 		dev_dbg(&indio_dev->dev, "%s chan %d to SQ%d\n",
 			__func__, chan->channel, i);
 
-		val = stm32_adc_readl(adc, stm32f4_sq[i].reg);
-		val &= ~stm32f4_sq[i].mask;
-		val |= chan->channel << stm32f4_sq[i].shift;
-		stm32_adc_writel(adc, stm32f4_sq[i].reg, val);
+		val = stm32_adc_readl(adc, sqr[i].reg);
+		val &= ~sqr[i].mask;
+		val |= chan->channel << sqr[i].shift;
+		stm32_adc_writel(adc, sqr[i].reg, val);
 	}
 
 	if (!i)
 		return -EINVAL;
 
 	/* Sequence len */
-	val = stm32_adc_readl(adc, stm32f4_sq[0].reg);
-	val &= ~stm32f4_sq[0].mask;
-	val |= ((i - 1) << stm32f4_sq[0].shift);
-	stm32_adc_writel(adc, stm32f4_sq[0].reg, val);
+	val = stm32_adc_readl(adc, sqr[0].reg);
+	val &= ~sqr[0].mask;
+	val |= ((i - 1) << sqr[0].shift);
+	stm32_adc_writel(adc, sqr[0].reg, val);
 
 	return 0;
 }
@@ -412,19 +983,21 @@ static int stm32_adc_conf_scan_seq(struct iio_dev *indio_dev,
  *
  * Returns trigger extsel value, if trig matches, -EINVAL otherwise.
  */
-static int stm32_adc_get_trig_extsel(struct iio_trigger *trig)
+static int stm32_adc_get_trig_extsel(struct iio_dev *indio_dev,
+				     struct iio_trigger *trig)
 {
+	struct stm32_adc *adc = iio_priv(indio_dev);
 	int i;
 
 	/* lookup triggers registered by stm32 timer trigger driver */
-	for (i = 0; stm32f4_adc_trigs[i].name; i++) {
+	for (i = 0; adc->cfg->trigs[i].name; i++) {
 		/**
 		 * Checking both stm32 timer trigger type and trig name
 		 * should be safe against arbitrary trigger names.
 		 */
 		if (is_stm32_timer_trigger(trig) &&
-		    !strcmp(stm32f4_adc_trigs[i].name, trig->name)) {
-			return stm32f4_adc_trigs[i].extsel;
+		    !strcmp(adc->cfg->trigs[i].name, trig->name)) {
+			return adc->cfg->trigs[i].extsel;
 		}
 	}
 
@@ -449,7 +1022,7 @@ static int stm32_adc_set_trig(struct iio_dev *indio_dev,
 	int ret;
 
 	if (trig) {
-		ret = stm32_adc_get_trig_extsel(trig);
+		ret = stm32_adc_get_trig_extsel(indio_dev, trig);
 		if (ret < 0)
 			return ret;
 
@@ -459,11 +1032,11 @@ static int stm32_adc_set_trig(struct iio_dev *indio_dev,
 	}
 
 	spin_lock_irqsave(&adc->lock, flags);
-	val = stm32_adc_readl(adc, STM32F4_ADC_CR2);
-	val &= ~(STM32F4_EXTEN_MASK | STM32F4_EXTSEL_MASK);
-	val |= exten << STM32F4_EXTEN_SHIFT;
-	val |= extsel << STM32F4_EXTSEL_SHIFT;
-	stm32_adc_writel(adc, STM32F4_ADC_CR2, val);
+	val = stm32_adc_readl(adc, adc->cfg->regs->exten.reg);
+	val &= ~(adc->cfg->regs->exten.mask | adc->cfg->regs->extsel.mask);
+	val |= exten << adc->cfg->regs->exten.shift;
+	val |= extsel << adc->cfg->regs->extsel.shift;
+	stm32_adc_writel(adc,  adc->cfg->regs->exten.reg, val);
 	spin_unlock_irqrestore(&adc->lock, flags);
 
 	return 0;
@@ -515,6 +1088,7 @@ static int stm32_adc_single_conv(struct iio_dev *indio_dev,
 				 int *res)
 {
 	struct stm32_adc *adc = iio_priv(indio_dev);
+	const struct stm32_adc_regspec *regs = adc->cfg->regs;
 	long timeout;
 	u32 val;
 	int ret;
@@ -523,21 +1097,27 @@ static int stm32_adc_single_conv(struct iio_dev *indio_dev,
 
 	adc->bufi = 0;
 
+	if (adc->cfg->prepare) {
+		ret = adc->cfg->prepare(adc);
+		if (ret)
+			return ret;
+	}
+
 	/* Program chan number in regular sequence (SQ1) */
-	val = stm32_adc_readl(adc, stm32f4_sq[1].reg);
-	val &= ~stm32f4_sq[1].mask;
-	val |= chan->channel << stm32f4_sq[1].shift;
-	stm32_adc_writel(adc, stm32f4_sq[1].reg, val);
+	val = stm32_adc_readl(adc, regs->sqr[1].reg);
+	val &= ~regs->sqr[1].mask;
+	val |= chan->channel << regs->sqr[1].shift;
+	stm32_adc_writel(adc, regs->sqr[1].reg, val);
 
 	/* Set regular sequence len (0 for 1 conversion) */
-	stm32_adc_clr_bits(adc, stm32f4_sq[0].reg, stm32f4_sq[0].mask);
+	stm32_adc_clr_bits(adc, regs->sqr[0].reg, regs->sqr[0].mask);
 
 	/* Trigger detection disabled (conversion can be launched in SW) */
-	stm32_adc_clr_bits(adc, STM32F4_ADC_CR2, STM32F4_EXTEN_MASK);
+	stm32_adc_clr_bits(adc, regs->exten.reg, regs->exten.mask);
 
 	stm32_adc_conv_irq_enable(adc);
 
-	stm32_adc_start_conv(adc, false);
+	adc->cfg->start_conv(adc, false);
 
 	timeout = wait_for_completion_interruptible_timeout(
 					&adc->completion, STM32_ADC_TIMEOUT);
@@ -550,10 +1130,13 @@ static int stm32_adc_single_conv(struct iio_dev *indio_dev,
 		ret = IIO_VAL_INT;
 	}
 
-	stm32_adc_stop_conv(adc);
+	adc->cfg->stop_conv(adc);
 
 	stm32_adc_conv_irq_disable(adc);
 
+	if (adc->cfg->unprepare)
+		adc->cfg->unprepare(adc);
+
 	return ret;
 }
 
@@ -590,11 +1173,12 @@ static irqreturn_t stm32_adc_isr(int irq, void *data)
 {
 	struct stm32_adc *adc = data;
 	struct iio_dev *indio_dev = iio_priv_to_dev(adc);
-	u32 status = stm32_adc_readl(adc, STM32F4_ADC_SR);
+	const struct stm32_adc_regspec *regs = adc->cfg->regs;
+	u32 status = stm32_adc_readl(adc, regs->isr_eoc.reg);
 
-	if (status & STM32F4_EOC) {
+	if (status & regs->isr_eoc.mask) {
 		/* Reading DR also clears EOC status flag */
-		adc->buffer[adc->bufi] = stm32_adc_readw(adc, STM32F4_ADC_DR);
+		adc->buffer[adc->bufi] = stm32_adc_readw(adc, regs->dr);
 		if (iio_buffer_enabled(indio_dev)) {
 			adc->bufi++;
 			if (adc->bufi >= adc->num_conv) {
@@ -621,7 +1205,7 @@ static irqreturn_t stm32_adc_isr(int irq, void *data)
 static int stm32_adc_validate_trigger(struct iio_dev *indio_dev,
 				      struct iio_trigger *trig)
 {
-	return stm32_adc_get_trig_extsel(trig) < 0 ? -EINVAL : 0;
+	return stm32_adc_get_trig_extsel(indio_dev, trig) < 0 ? -EINVAL : 0;
 }
 
 static int stm32_adc_set_watermark(struct iio_dev *indio_dev, unsigned int val)
@@ -777,10 +1361,16 @@ static int stm32_adc_buffer_postenable(struct iio_dev *indio_dev)
 	struct stm32_adc *adc = iio_priv(indio_dev);
 	int ret;
 
+	if (adc->cfg->prepare) {
+		ret = adc->cfg->prepare(adc);
+		if (ret)
+			return ret;
+	}
+
 	ret = stm32_adc_set_trig(indio_dev, indio_dev->trig);
 	if (ret) {
 		dev_err(&indio_dev->dev, "Can't set trigger\n");
-		return ret;
+		goto err_unprepare;
 	}
 
 	ret = stm32_adc_dma_start(indio_dev);
@@ -799,7 +1389,7 @@ static int stm32_adc_buffer_postenable(struct iio_dev *indio_dev)
 	if (!adc->dma_chan)
 		stm32_adc_conv_irq_enable(adc);
 
-	stm32_adc_start_conv(adc, !!adc->dma_chan);
+	adc->cfg->start_conv(adc, !!adc->dma_chan);
 
 	return 0;
 
@@ -808,6 +1398,9 @@ err_stop_dma:
 		dmaengine_terminate_all(adc->dma_chan);
 err_clr_trig:
 	stm32_adc_set_trig(indio_dev, NULL);
+err_unprepare:
+	if (adc->cfg->unprepare)
+		adc->cfg->unprepare(adc);
 
 	return ret;
 }
@@ -817,7 +1410,7 @@ static int stm32_adc_buffer_predisable(struct iio_dev *indio_dev)
 	struct stm32_adc *adc = iio_priv(indio_dev);
 	int ret;
 
-	stm32_adc_stop_conv(adc);
+	adc->cfg->stop_conv(adc);
 	if (!adc->dma_chan)
 		stm32_adc_conv_irq_disable(adc);
 
@@ -831,6 +1424,9 @@ static int stm32_adc_buffer_predisable(struct iio_dev *indio_dev)
 	if (stm32_adc_set_trig(indio_dev, NULL))
 		dev_err(&indio_dev->dev, "Can't clear trigger\n");
 
+	if (adc->cfg->unprepare)
+		adc->cfg->unprepare(adc);
+
 	return ret;
 }
 
@@ -895,12 +1491,12 @@ static int stm32_adc_of_get_resolution(struct iio_dev *indio_dev)
 	u32 res;
 
 	if (of_property_read_u32(node, "assigned-resolution-bits", &res))
-		res = stm32f4_adc_resolutions[0];
+		res = adc->cfg->adc_info->resolutions[0];
 
-	for (i = 0; i < ARRAY_SIZE(stm32f4_adc_resolutions); i++)
-		if (res == stm32f4_adc_resolutions[i])
+	for (i = 0; i < adc->cfg->adc_info->num_res; i++)
+		if (res == adc->cfg->adc_info->resolutions[i])
 			break;
-	if (i >= ARRAY_SIZE(stm32f4_adc_resolutions)) {
+	if (i >= adc->cfg->adc_info->num_res) {
 		dev_err(&indio_dev->dev, "Bad resolution: %u bits\n", res);
 		return -EINVAL;
 	}
@@ -926,14 +1522,19 @@ static void stm32_adc_chan_init_one(struct iio_dev *indio_dev,
 	chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW);
 	chan->info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE);
 	chan->scan_type.sign = 'u';
-	chan->scan_type.realbits = stm32f4_adc_resolutions[adc->res];
+	chan->scan_type.realbits = adc->cfg->adc_info->resolutions[adc->res];
 	chan->scan_type.storagebits = 16;
 	chan->ext_info = stm32_adc_ext_info;
+
+	/* pre-build selected channels mask */
+	adc->pcsel |= BIT(chan->channel);
 }
 
 static int stm32_adc_chan_of_init(struct iio_dev *indio_dev)
 {
 	struct device_node *node = indio_dev->dev.of_node;
+	struct stm32_adc *adc = iio_priv(indio_dev);
+	const struct stm32_adc_info *adc_info = adc->cfg->adc_info;
 	struct property *prop;
 	const __be32 *cur;
 	struct iio_chan_spec *channels;
@@ -942,7 +1543,7 @@ static int stm32_adc_chan_of_init(struct iio_dev *indio_dev)
 
 	num_channels = of_property_count_u32_elems(node, "st,adc-channels");
 	if (num_channels < 0 ||
-	    num_channels >= ARRAY_SIZE(stm32f4_adc123_channels)) {
+	    num_channels >= adc_info->max_channels) {
 		dev_err(&indio_dev->dev, "Bad st,adc-channels?\n");
 		return num_channels < 0 ? num_channels : -EINVAL;
 	}
@@ -953,12 +1554,12 @@ static int stm32_adc_chan_of_init(struct iio_dev *indio_dev)
 		return -ENOMEM;
 
 	of_property_for_each_u32(node, "st,adc-channels", prop, cur, val) {
-		if (val >= ARRAY_SIZE(stm32f4_adc123_channels)) {
+		if (val >= adc_info->max_channels) {
 			dev_err(&indio_dev->dev, "Invalid channel %d\n", val);
 			return -EINVAL;
 		}
 		stm32_adc_chan_init_one(indio_dev, &channels[scan_index],
-					&stm32f4_adc123_channels[val],
+					&adc_info->channels[val],
 					scan_index);
 		scan_index++;
 	}
@@ -990,7 +1591,7 @@ static int stm32_adc_dma_request(struct iio_dev *indio_dev)
 	/* Configure DMA channel to read data register */
 	memset(&config, 0, sizeof(config));
 	config.src_addr = (dma_addr_t)adc->common->phys_base;
-	config.src_addr += adc->offset + STM32F4_ADC_DR;
+	config.src_addr += adc->offset + adc->cfg->regs->dr;
 	config.src_addr_width = DMA_SLAVE_BUSWIDTH_2_BYTES;
 
 	ret = dmaengine_slave_config(adc->dma_chan, &config);
@@ -1011,6 +1612,7 @@ err_release:
 static int stm32_adc_probe(struct platform_device *pdev)
 {
 	struct iio_dev *indio_dev;
+	struct device *dev = &pdev->dev;
 	struct stm32_adc *adc;
 	int ret;
 
@@ -1025,6 +1627,8 @@ static int stm32_adc_probe(struct platform_device *pdev)
 	adc->common = dev_get_drvdata(pdev->dev.parent);
 	spin_lock_init(&adc->lock);
 	init_completion(&adc->completion);
+	adc->cfg = (const struct stm32_adc_cfg *)
+		of_match_device(dev->driver->of_match_table, dev)->data;
 
 	indio_dev->name = dev_name(&pdev->dev);
 	indio_dev->dev.parent = &pdev->dev;
@@ -1055,14 +1659,21 @@ static int stm32_adc_probe(struct platform_device *pdev)
 
 	adc->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(adc->clk)) {
-		dev_err(&pdev->dev, "Can't get clock\n");
-		return PTR_ERR(adc->clk);
+		ret = PTR_ERR(adc->clk);
+		if (ret == -ENOENT && !adc->cfg->clk_required) {
+			adc->clk = NULL;
+		} else {
+			dev_err(&pdev->dev, "Can't get clock\n");
+			return ret;
+		}
 	}
 
-	ret = clk_prepare_enable(adc->clk);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "clk enable failed\n");
-		return ret;
+	if (adc->clk) {
+		ret = clk_prepare_enable(adc->clk);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "clk enable failed\n");
+			return ret;
+		}
 	}
 
 	ret = stm32_adc_of_get_resolution(indio_dev);
@@ -1070,6 +1681,12 @@ static int stm32_adc_probe(struct platform_device *pdev)
 		goto err_clk_disable;
 	stm32_adc_set_res(adc);
 
+	if (adc->cfg->selfcalib) {
+		ret = adc->cfg->selfcalib(adc);
+		if (ret)
+			goto err_clk_disable;
+	}
+
 	ret = stm32_adc_chan_of_init(indio_dev);
 	if (ret < 0)
 		goto err_clk_disable;
@@ -1106,7 +1723,8 @@ err_dma_disable:
 		dma_release_channel(adc->dma_chan);
 	}
 err_clk_disable:
-	clk_disable_unprepare(adc->clk);
+	if (adc->clk)
+		clk_disable_unprepare(adc->clk);
 
 	return ret;
 }
@@ -1124,13 +1742,35 @@ static int stm32_adc_remove(struct platform_device *pdev)
 				  adc->rx_buf, adc->rx_dma_buf);
 		dma_release_channel(adc->dma_chan);
 	}
-	clk_disable_unprepare(adc->clk);
+	if (adc->clk)
+		clk_disable_unprepare(adc->clk);
 
 	return 0;
 }
 
+static const struct stm32_adc_cfg stm32f4_adc_cfg = {
+	.regs = &stm32f4_adc_regspec,
+	.adc_info = &stm32f4_adc_info,
+	.trigs = stm32f4_adc_trigs,
+	.clk_required = true,
+	.start_conv = stm32f4_adc_start_conv,
+	.stop_conv = stm32f4_adc_stop_conv,
+};
+
+static const struct stm32_adc_cfg stm32h7_adc_cfg = {
+	.regs = &stm32h7_adc_regspec,
+	.adc_info = &stm32h7_adc_info,
+	.trigs = stm32h7_adc_trigs,
+	.selfcalib = stm32h7_adc_selfcalib,
+	.start_conv = stm32h7_adc_start_conv,
+	.stop_conv = stm32h7_adc_stop_conv,
+	.prepare = stm32h7_adc_prepare,
+	.unprepare = stm32h7_adc_unprepare,
+};
+
 static const struct of_device_id stm32_adc_of_match[] = {
-	{ .compatible = "st,stm32f4-adc" },
+	{ .compatible = "st,stm32f4-adc", .data = (void *)&stm32f4_adc_cfg },
+	{ .compatible = "st,stm32h7-adc", .data = (void *)&stm32h7_adc_cfg },
 	{},
 };
 MODULE_DEVICE_TABLE(of, stm32_adc_of_match);
diff --git a/drivers/iio/adc/ti-adc084s021.c b/drivers/iio/adc/ti-adc084s021.c
new file mode 100644
index 000000000000..a355121c11a4
--- /dev/null
+++ b/drivers/iio/adc/ti-adc084s021.c
@@ -0,0 +1,275 @@
+/**
+ * Copyright (C) 2017 Axis Communications AB
+ *
+ * Driver for Texas Instruments' ADC084S021 ADC chip.
+ * Datasheets can be found here:
+ * http://www.ti.com/lit/ds/symlink/adc084s021.pdf
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/spi/spi.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/regulator/consumer.h>
+
+#define ADC084S021_DRIVER_NAME "adc084s021"
+
+struct adc084s021 {
+	struct spi_device *spi;
+	struct spi_message message;
+	struct spi_transfer spi_trans;
+	struct regulator *reg;
+	struct mutex lock;
+	/*
+	 * DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache line.
+	 */
+	u16 tx_buf[4] ____cacheline_aligned;
+	__be16 rx_buf[5]; /* First 16-bits are trash */
+};
+
+#define ADC084S021_VOLTAGE_CHANNEL(num)                  \
+	{                                                      \
+		.type = IIO_VOLTAGE,                                 \
+		.channel = (num),                                    \
+		.indexed = 1,                                        \
+		.scan_index = (num),                                 \
+		.scan_type = {                                       \
+			.sign = 'u',                                       \
+			.realbits = 8,                                     \
+			.storagebits = 16,                                 \
+			.shift = 4,                                        \
+			.endianness = IIO_BE,                              \
+		},                                                   \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),        \
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),\
+	}
+
+static const struct iio_chan_spec adc084s021_channels[] = {
+	ADC084S021_VOLTAGE_CHANNEL(0),
+	ADC084S021_VOLTAGE_CHANNEL(1),
+	ADC084S021_VOLTAGE_CHANNEL(2),
+	ADC084S021_VOLTAGE_CHANNEL(3),
+	IIO_CHAN_SOFT_TIMESTAMP(4),
+};
+
+/**
+ * Read an ADC channel and return its value.
+ *
+ * @adc: The ADC SPI data.
+ * @data: Buffer for converted data.
+ */
+static int adc084s021_adc_conversion(struct adc084s021 *adc, void *data)
+{
+	int n_words = (adc->spi_trans.len >> 1) - 1; /* Discard first word */
+	int ret, i = 0;
+	u16 *p = data;
+
+	/* Do the transfer */
+	ret = spi_sync(adc->spi, &adc->message);
+	if (ret < 0)
+		return ret;
+
+	for (; i < n_words; i++)
+		*(p + i) = adc->rx_buf[i + 1];
+
+	return ret;
+}
+
+static int adc084s021_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *channel, int *val,
+			   int *val2, long mask)
+{
+	struct adc084s021 *adc = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = iio_device_claim_direct_mode(indio_dev);
+		if (ret < 0)
+			return ret;
+
+		ret = regulator_enable(adc->reg);
+		if (ret) {
+			iio_device_release_direct_mode(indio_dev);
+			return ret;
+		}
+
+		adc->tx_buf[0] = channel->channel << 3;
+		ret = adc084s021_adc_conversion(adc, val);
+		iio_device_release_direct_mode(indio_dev);
+		regulator_disable(adc->reg);
+		if (ret < 0)
+			return ret;
+
+		*val = be16_to_cpu(*val);
+		*val = (*val >> channel->scan_type.shift) & 0xff;
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		ret = regulator_enable(adc->reg);
+		if (ret)
+			return ret;
+
+		ret = regulator_get_voltage(adc->reg);
+		regulator_disable(adc->reg);
+		if (ret < 0)
+			return ret;
+
+		*val = ret / 1000;
+
+		return IIO_VAL_INT;
+	default:
+		return -EINVAL;
+	}
+}
+
+/**
+ * Read enabled ADC channels and push data to the buffer.
+ *
+ * @irq: The interrupt number (not used).
+ * @pollfunc: Pointer to the poll func.
+ */
+static irqreturn_t adc084s021_buffer_trigger_handler(int irq, void *pollfunc)
+{
+	struct iio_poll_func *pf = pollfunc;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct adc084s021 *adc = iio_priv(indio_dev);
+	__be16 data[8] = {0}; /* 4 * 16-bit words of data + 8 bytes timestamp */
+
+	mutex_lock(&adc->lock);
+
+	if (adc084s021_adc_conversion(adc, &data) < 0)
+		dev_err(&adc->spi->dev, "Failed to read data\n");
+
+	iio_push_to_buffers_with_timestamp(indio_dev, data,
+					   iio_get_time_ns(indio_dev));
+	mutex_unlock(&adc->lock);
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static int adc084s021_buffer_preenable(struct iio_dev *indio_dev)
+{
+	struct adc084s021 *adc = iio_priv(indio_dev);
+	int scan_index;
+	int i = 0;
+
+	for_each_set_bit(scan_index, indio_dev->active_scan_mask,
+			 indio_dev->masklength) {
+		const struct iio_chan_spec *channel =
+			&indio_dev->channels[scan_index];
+		adc->tx_buf[i++] = channel->channel << 3;
+	}
+	adc->spi_trans.len = 2 + (i * sizeof(__be16)); /* Trash + channels */
+
+	return regulator_enable(adc->reg);
+}
+
+static int adc084s021_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct adc084s021 *adc = iio_priv(indio_dev);
+
+	adc->spi_trans.len = 4; /* Trash + single channel */
+
+	return regulator_disable(adc->reg);
+}
+
+static const struct iio_info adc084s021_info = {
+	.read_raw = adc084s021_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static const struct iio_buffer_setup_ops adc084s021_buffer_setup_ops = {
+	.preenable = adc084s021_buffer_preenable,
+	.postenable = iio_triggered_buffer_postenable,
+	.predisable = iio_triggered_buffer_predisable,
+	.postdisable = adc084s021_buffer_postdisable,
+};
+
+static int adc084s021_probe(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev;
+	struct adc084s021 *adc;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*adc));
+	if (!indio_dev) {
+		dev_err(&spi->dev, "Failed to allocate IIO device\n");
+		return -ENOMEM;
+	}
+
+	adc = iio_priv(indio_dev);
+	adc->spi = spi;
+
+	/* Connect the SPI device and the iio dev */
+	spi_set_drvdata(spi, indio_dev);
+
+	/* Initiate the Industrial I/O device */
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->dev.of_node = spi->dev.of_node;
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &adc084s021_info;
+	indio_dev->channels = adc084s021_channels;
+	indio_dev->num_channels = ARRAY_SIZE(adc084s021_channels);
+
+	/* Create SPI transfer for channel reads */
+	adc->spi_trans.tx_buf = adc->tx_buf;
+	adc->spi_trans.rx_buf = adc->rx_buf;
+	adc->spi_trans.len = 4; /* Trash + single channel */
+	spi_message_init_with_transfers(&adc->message, &adc->spi_trans, 1);
+
+	adc->reg = devm_regulator_get(&spi->dev, "vref");
+	if (IS_ERR(adc->reg))
+		return PTR_ERR(adc->reg);
+
+	mutex_init(&adc->lock);
+
+	/* Setup triggered buffer with pollfunction */
+	ret = devm_iio_triggered_buffer_setup(&spi->dev, indio_dev, NULL,
+					    adc084s021_buffer_trigger_handler,
+					    &adc084s021_buffer_setup_ops);
+	if (ret) {
+		dev_err(&spi->dev, "Failed to setup triggered buffer\n");
+		return ret;
+	}
+
+	return devm_iio_device_register(&spi->dev, indio_dev);
+}
+
+static const struct of_device_id adc084s021_of_match[] = {
+	{ .compatible = "ti,adc084s021", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, adc084s021_of_match);
+
+static const struct spi_device_id adc084s021_id[] = {
+	{ ADC084S021_DRIVER_NAME, 0},
+	{}
+};
+MODULE_DEVICE_TABLE(spi, adc084s021_id);
+
+static struct spi_driver adc084s021_driver = {
+	.driver = {
+		.name = ADC084S021_DRIVER_NAME,
+		.of_match_table = of_match_ptr(adc084s021_of_match),
+	},
+	.probe = adc084s021_probe,
+	.id_table = adc084s021_id,
+};
+module_spi_driver(adc084s021_driver);
+
+MODULE_AUTHOR("Mårten Lindahl <martenli@axis.com>");
+MODULE_DESCRIPTION("Texas Instruments ADC084S021");
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION("1.0");
diff --git a/drivers/iio/adc/ti-adc108s102.c b/drivers/iio/adc/ti-adc108s102.c
new file mode 100644
index 000000000000..de4e5ac98c6e
--- /dev/null
+++ b/drivers/iio/adc/ti-adc108s102.c
@@ -0,0 +1,348 @@
+/*
+ * TI ADC108S102 SPI ADC driver
+ *
+ * Copyright (c) 2013-2015 Intel Corporation.
+ * Copyright (c) 2017 Siemens AG
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * This IIO device driver is designed to work with the following
+ * analog to digital converters from Texas Instruments:
+ *  ADC108S102
+ *  ADC128S102
+ * The communication with ADC chip is via the SPI bus (mode 3).
+ */
+
+#include <linux/acpi.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/types.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/property.h>
+#include <linux/regulator/consumer.h>
+#include <linux/spi/spi.h>
+
+/*
+ * In case of ACPI, we use the hard-wired 5000 mV of the Galileo and IOT2000
+ * boards as default for the reference pin VA. Device tree users encode that
+ * via the vref-supply regulator.
+ */
+#define ADC108S102_VA_MV_ACPI_DEFAULT	5000
+
+/*
+ * Defining the ADC resolution being 12 bits, we can use the same driver for
+ * both ADC108S102 (10 bits resolution) and ADC128S102 (12 bits resolution)
+ * chips. The ADC108S102 effectively returns a 12-bit result with the 2
+ * least-significant bits unset.
+ */
+#define ADC108S102_BITS		12
+#define ADC108S102_MAX_CHANNELS	8
+
+/*
+ * 16-bit SPI command format:
+ *   [15:14] Ignored
+ *   [13:11] 3-bit channel address
+ *   [10:0]  Ignored
+ */
+#define ADC108S102_CMD(ch)		((u16)(ch) << 11)
+
+/*
+ * 16-bit SPI response format:
+ *   [15:12] Zeros
+ *   [11:0]  12-bit ADC sample (for ADC108S102, [1:0] will always be 0).
+ */
+#define ADC108S102_RES_DATA(res)	((u16)res & GENMASK(11, 0))
+
+struct adc108s102_state {
+	struct spi_device		*spi;
+	struct regulator		*reg;
+	u32				va_millivolt;
+	/* SPI transfer used by triggered buffer handler*/
+	struct spi_transfer		ring_xfer;
+	/* SPI transfer used by direct scan */
+	struct spi_transfer		scan_single_xfer;
+	/* SPI message used by ring_xfer SPI transfer */
+	struct spi_message		ring_msg;
+	/* SPI message used by scan_single_xfer SPI transfer */
+	struct spi_message		scan_single_msg;
+
+	/*
+	 * SPI message buffers:
+	 *  tx_buf: |C0|C1|C2|C3|C4|C5|C6|C7|XX|
+	 *  rx_buf: |XX|R0|R1|R2|R3|R4|R5|R6|R7|tt|tt|tt|tt|
+	 *
+	 *  tx_buf: 8 channel read commands, plus 1 dummy command
+	 *  rx_buf: 1 dummy response, 8 channel responses, plus 64-bit timestamp
+	 */
+	__be16				rx_buf[13] ____cacheline_aligned;
+	__be16				tx_buf[9] ____cacheline_aligned;
+};
+
+#define ADC108S102_V_CHAN(index)					\
+	{								\
+		.type = IIO_VOLTAGE,					\
+		.indexed = 1,						\
+		.channel = index,					\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |		\
+			BIT(IIO_CHAN_INFO_SCALE),			\
+		.address = index,					\
+		.scan_index = index,					\
+		.scan_type = {						\
+			.sign = 'u',					\
+			.realbits = ADC108S102_BITS,			\
+			.storagebits = 16,				\
+			.endianness = IIO_BE,				\
+		},							\
+	}
+
+static const struct iio_chan_spec adc108s102_channels[] = {
+	ADC108S102_V_CHAN(0),
+	ADC108S102_V_CHAN(1),
+	ADC108S102_V_CHAN(2),
+	ADC108S102_V_CHAN(3),
+	ADC108S102_V_CHAN(4),
+	ADC108S102_V_CHAN(5),
+	ADC108S102_V_CHAN(6),
+	ADC108S102_V_CHAN(7),
+	IIO_CHAN_SOFT_TIMESTAMP(8),
+};
+
+static int adc108s102_update_scan_mode(struct iio_dev *indio_dev,
+		unsigned long const *active_scan_mask)
+{
+	struct adc108s102_state *st = iio_priv(indio_dev);
+	unsigned int bit, cmds;
+
+	/*
+	 * Fill in the first x shorts of tx_buf with the number of channels
+	 * enabled for sampling by the triggered buffer.
+	 */
+	cmds = 0;
+	for_each_set_bit(bit, active_scan_mask, ADC108S102_MAX_CHANNELS)
+		st->tx_buf[cmds++] = cpu_to_be16(ADC108S102_CMD(bit));
+
+	/* One dummy command added, to clock in the last response */
+	st->tx_buf[cmds++] = 0x00;
+
+	/* build SPI ring message */
+	st->ring_xfer.tx_buf = &st->tx_buf[0];
+	st->ring_xfer.rx_buf = &st->rx_buf[0];
+	st->ring_xfer.len = cmds * sizeof(st->tx_buf[0]);
+
+	spi_message_init_with_transfers(&st->ring_msg, &st->ring_xfer, 1);
+
+	return 0;
+}
+
+static irqreturn_t adc108s102_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct adc108s102_state *st = iio_priv(indio_dev);
+	int ret;
+
+	ret = spi_sync(st->spi, &st->ring_msg);
+	if (ret < 0)
+		goto out_notify;
+
+	/* Skip the dummy response in the first slot */
+	iio_push_to_buffers_with_timestamp(indio_dev,
+					   (u8 *)&st->rx_buf[1],
+					   iio_get_time_ns(indio_dev));
+
+out_notify:
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static int adc108s102_scan_direct(struct adc108s102_state *st, unsigned int ch)
+{
+	int ret;
+
+	st->tx_buf[0] = cpu_to_be16(ADC108S102_CMD(ch));
+	ret = spi_sync(st->spi, &st->scan_single_msg);
+	if (ret)
+		return ret;
+
+	/* Skip the dummy response in the first slot */
+	return be16_to_cpu(st->rx_buf[1]);
+}
+
+static int adc108s102_read_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int *val, int *val2, long m)
+{
+	struct adc108s102_state *st = iio_priv(indio_dev);
+	int ret;
+
+	switch (m) {
+	case IIO_CHAN_INFO_RAW:
+		ret = iio_device_claim_direct_mode(indio_dev);
+		if (ret)
+			return ret;
+
+		ret = adc108s102_scan_direct(st, chan->address);
+
+		iio_device_release_direct_mode(indio_dev);
+
+		if (ret < 0)
+			return ret;
+
+		*val = ADC108S102_RES_DATA(ret);
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		if (chan->type != IIO_VOLTAGE)
+			break;
+
+		*val = st->va_millivolt;
+		*val2 = chan->scan_type.realbits;
+
+		return IIO_VAL_FRACTIONAL_LOG2;
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info adc108s102_info = {
+	.read_raw		= &adc108s102_read_raw,
+	.update_scan_mode	= &adc108s102_update_scan_mode,
+	.driver_module		= THIS_MODULE,
+};
+
+static int adc108s102_probe(struct spi_device *spi)
+{
+	struct adc108s102_state *st;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+
+	if (ACPI_COMPANION(&spi->dev)) {
+		st->va_millivolt = ADC108S102_VA_MV_ACPI_DEFAULT;
+	} else {
+		st->reg = devm_regulator_get(&spi->dev, "vref");
+		if (IS_ERR(st->reg))
+			return PTR_ERR(st->reg);
+
+		ret = regulator_enable(st->reg);
+		if (ret < 0) {
+			dev_err(&spi->dev, "Cannot enable vref regulator\n");
+			return ret;
+		}
+
+		ret = regulator_get_voltage(st->reg);
+		if (ret < 0) {
+			dev_err(&spi->dev, "vref get voltage failed\n");
+			return ret;
+		}
+
+		st->va_millivolt = ret / 1000;
+	}
+
+	spi_set_drvdata(spi, indio_dev);
+	st->spi = spi;
+
+	indio_dev->name = spi->modalias;
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = adc108s102_channels;
+	indio_dev->num_channels = ARRAY_SIZE(adc108s102_channels);
+	indio_dev->info = &adc108s102_info;
+
+	/* Setup default message */
+	st->scan_single_xfer.tx_buf = st->tx_buf;
+	st->scan_single_xfer.rx_buf = st->rx_buf;
+	st->scan_single_xfer.len = 2 * sizeof(st->tx_buf[0]);
+
+	spi_message_init_with_transfers(&st->scan_single_msg,
+					&st->scan_single_xfer, 1);
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+					 &adc108s102_trigger_handler, NULL);
+	if (ret)
+		goto error_disable_reg;
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&spi->dev, "Failed to register IIO device\n");
+		goto error_cleanup_triggered_buffer;
+	}
+	return 0;
+
+error_cleanup_triggered_buffer:
+	iio_triggered_buffer_cleanup(indio_dev);
+
+error_disable_reg:
+	regulator_disable(st->reg);
+
+	return ret;
+}
+
+static int adc108s102_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct adc108s102_state *st = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+
+	regulator_disable(st->reg);
+
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id adc108s102_of_match[] = {
+	{ .compatible = "ti,adc108s102" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, adc108s102_of_match);
+#endif
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id adc108s102_acpi_ids[] = {
+	{ "INT3495", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, adc108s102_acpi_ids);
+#endif
+
+static const struct spi_device_id adc108s102_id[] = {
+	{ "adc108s102", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, adc108s102_id);
+
+static struct spi_driver adc108s102_driver = {
+	.driver = {
+		.name   = "adc108s102",
+		.of_match_table = of_match_ptr(adc108s102_of_match),
+		.acpi_match_table = ACPI_PTR(adc108s102_acpi_ids),
+	},
+	.probe		= adc108s102_probe,
+	.remove		= adc108s102_remove,
+	.id_table	= adc108s102_id,
+};
+module_spi_driver(adc108s102_driver);
+
+MODULE_AUTHOR("Bogdan Pricop <bogdan.pricop@emutex.com>");
+MODULE_DESCRIPTION("Texas Instruments ADC108S102 and ADC128S102 driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ti-ads1015.c b/drivers/iio/adc/ti-ads1015.c
index f76d979fb7e8..884b8e461b17 100644
--- a/drivers/iio/adc/ti-ads1015.c
+++ b/drivers/iio/adc/ti-ads1015.c
@@ -23,7 +23,7 @@
 #include <linux/mutex.h>
 #include <linux/delay.h>
 
-#include <linux/i2c/ads1015.h>
+#include <linux/platform_data/ads1015.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/types.h>
diff --git a/drivers/iio/adc/twl4030-madc.c b/drivers/iio/adc/twl4030-madc.c
index 0c74869a540a..bd3d37fc2144 100644
--- a/drivers/iio/adc/twl4030-madc.c
+++ b/drivers/iio/adc/twl4030-madc.c
@@ -36,7 +36,6 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/i2c/twl.h>
-#include <linux/i2c/twl4030-madc.h>
 #include <linux/module.h>
 #include <linux/stddef.h>
 #include <linux/mutex.h>
@@ -49,9 +48,121 @@
 
 #include <linux/iio/iio.h>
 
+#define TWL4030_MADC_MAX_CHANNELS 16
+
+#define TWL4030_MADC_CTRL1		0x00
+#define TWL4030_MADC_CTRL2		0x01
+
+#define TWL4030_MADC_RTSELECT_LSB	0x02
+#define TWL4030_MADC_SW1SELECT_LSB	0x06
+#define TWL4030_MADC_SW2SELECT_LSB	0x0A
+
+#define TWL4030_MADC_RTAVERAGE_LSB	0x04
+#define TWL4030_MADC_SW1AVERAGE_LSB	0x08
+#define TWL4030_MADC_SW2AVERAGE_LSB	0x0C
+
+#define TWL4030_MADC_CTRL_SW1		0x12
+#define TWL4030_MADC_CTRL_SW2		0x13
+
+#define TWL4030_MADC_RTCH0_LSB		0x17
+#define TWL4030_MADC_GPCH0_LSB		0x37
+
+#define TWL4030_MADC_MADCON	(1 << 0)	/* MADC power on */
+#define TWL4030_MADC_BUSY	(1 << 0)	/* MADC busy */
+/* MADC conversion completion */
+#define TWL4030_MADC_EOC_SW	(1 << 1)
+/* MADC SWx start conversion */
+#define TWL4030_MADC_SW_START	(1 << 5)
+#define TWL4030_MADC_ADCIN0	(1 << 0)
+#define TWL4030_MADC_ADCIN1	(1 << 1)
+#define TWL4030_MADC_ADCIN2	(1 << 2)
+#define TWL4030_MADC_ADCIN3	(1 << 3)
+#define TWL4030_MADC_ADCIN4	(1 << 4)
+#define TWL4030_MADC_ADCIN5	(1 << 5)
+#define TWL4030_MADC_ADCIN6	(1 << 6)
+#define TWL4030_MADC_ADCIN7	(1 << 7)
+#define TWL4030_MADC_ADCIN8	(1 << 8)
+#define TWL4030_MADC_ADCIN9	(1 << 9)
+#define TWL4030_MADC_ADCIN10	(1 << 10)
+#define TWL4030_MADC_ADCIN11	(1 << 11)
+#define TWL4030_MADC_ADCIN12	(1 << 12)
+#define TWL4030_MADC_ADCIN13	(1 << 13)
+#define TWL4030_MADC_ADCIN14	(1 << 14)
+#define TWL4030_MADC_ADCIN15	(1 << 15)
+
+/* Fixed channels */
+#define TWL4030_MADC_BTEMP	TWL4030_MADC_ADCIN1
+#define TWL4030_MADC_VBUS	TWL4030_MADC_ADCIN8
+#define TWL4030_MADC_VBKB	TWL4030_MADC_ADCIN9
+#define TWL4030_MADC_ICHG	TWL4030_MADC_ADCIN10
+#define TWL4030_MADC_VCHG	TWL4030_MADC_ADCIN11
+#define TWL4030_MADC_VBAT	TWL4030_MADC_ADCIN12
+
+/* Step size and prescaler ratio */
+#define TEMP_STEP_SIZE          147
+#define TEMP_PSR_R              100
+#define CURR_STEP_SIZE		147
+#define CURR_PSR_R1		44
+#define CURR_PSR_R2		88
+
+#define TWL4030_BCI_BCICTL1	0x23
+#define TWL4030_BCI_CGAIN	0x020
+#define TWL4030_BCI_MESBAT	(1 << 1)
+#define TWL4030_BCI_TYPEN	(1 << 4)
+#define TWL4030_BCI_ITHEN	(1 << 3)
+
+#define REG_BCICTL2             0x024
+#define TWL4030_BCI_ITHSENS	0x007
+
+/* Register and bits for GPBR1 register */
+#define TWL4030_REG_GPBR1		0x0c
+#define TWL4030_GPBR1_MADC_HFCLK_EN	(1 << 7)
+
 #define TWL4030_USB_SEL_MADC_MCPC	(1<<3)
 #define TWL4030_USB_CARKIT_ANA_CTRL	0xBB
 
+struct twl4030_madc_conversion_method {
+	u8 sel;
+	u8 avg;
+	u8 rbase;
+	u8 ctrl;
+};
+
+/**
+ * struct twl4030_madc_request - madc request packet for channel conversion
+ * @channels:	16 bit bitmap for individual channels
+ * @do_avg:	sample the input channel for 4 consecutive cycles
+ * @method:	RT, SW1, SW2
+ * @type:	Polling or interrupt based method
+ * @active:	Flag if request is active
+ * @result_pending: Flag from irq handler, that result is ready
+ * @raw:	Return raw value, do not convert it
+ * @rbuf:	Result buffer
+ */
+struct twl4030_madc_request {
+	unsigned long channels;
+	bool do_avg;
+	u16 method;
+	u16 type;
+	bool active;
+	bool result_pending;
+	bool raw;
+	int rbuf[TWL4030_MADC_MAX_CHANNELS];
+};
+
+enum conversion_methods {
+	TWL4030_MADC_RT,
+	TWL4030_MADC_SW1,
+	TWL4030_MADC_SW2,
+	TWL4030_MADC_NUM_METHODS
+};
+
+enum sample_type {
+	TWL4030_MADC_WAIT,
+	TWL4030_MADC_IRQ_ONESHOT,
+	TWL4030_MADC_IRQ_REARM
+};
+
 /**
  * struct twl4030_madc_data - a container for madc info
  * @dev:		Pointer to device structure for madc
@@ -72,6 +183,8 @@ struct twl4030_madc_data {
 	u8 isr;
 };
 
+static int twl4030_madc_conversion(struct twl4030_madc_request *req);
+
 static int twl4030_madc_read(struct iio_dev *iio_dev,
 			     const struct iio_chan_spec *chan,
 			     int *val, int *val2, long mask)
@@ -84,7 +197,6 @@ static int twl4030_madc_read(struct iio_dev *iio_dev,
 
 	req.channels = BIT(chan->channel);
 	req.active = false;
-	req.func_cb = NULL;
 	req.type = TWL4030_MADC_WAIT;
 	req.raw = !(mask == IIO_CHAN_INFO_PROCESSED);
 	req.do_avg = (mask == IIO_CHAN_INFO_AVERAGE_RAW);
@@ -341,37 +453,6 @@ static int twl4030_madc_read_channels(struct twl4030_madc_data *madc,
 }
 
 /*
- * Enables irq.
- * @madc - pointer to twl4030_madc_data struct
- * @id - irq number to be enabled
- * can take one of TWL4030_MADC_RT, TWL4030_MADC_SW1, TWL4030_MADC_SW2
- * corresponding to RT, SW1, SW2 conversion requests.
- * If the i2c read fails it returns an error else returns 0.
- */
-static int twl4030_madc_enable_irq(struct twl4030_madc_data *madc, u8 id)
-{
-	u8 val;
-	int ret;
-
-	ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &val, madc->imr);
-	if (ret) {
-		dev_err(madc->dev, "unable to read imr register 0x%X\n",
-			madc->imr);
-		return ret;
-	}
-
-	val &= ~(1 << id);
-	ret = twl_i2c_write_u8(TWL4030_MODULE_MADC, val, madc->imr);
-	if (ret) {
-		dev_err(madc->dev,
-			"unable to write imr register 0x%X\n", madc->imr);
-		return ret;
-	}
-
-	return 0;
-}
-
-/*
  * Disables irq.
  * @madc - pointer to twl4030_madc_data struct
  * @id - irq number to be disabled
@@ -440,11 +521,6 @@ static irqreturn_t twl4030_madc_threaded_irq_handler(int irq, void *_madc)
 		/* Read results */
 		len = twl4030_madc_read_channels(madc, method->rbase,
 						 r->channels, r->rbuf, r->raw);
-		/* Return results to caller */
-		if (r->func_cb != NULL) {
-			r->func_cb(len, r->channels, r->rbuf);
-			r->func_cb = NULL;
-		}
 		/* Free request */
 		r->result_pending = 0;
 		r->active = 0;
@@ -466,11 +542,6 @@ err_i2c:
 		/* Read results */
 		len = twl4030_madc_read_channels(madc, method->rbase,
 						 r->channels, r->rbuf, r->raw);
-		/* Return results to caller */
-		if (r->func_cb != NULL) {
-			r->func_cb(len, r->channels, r->rbuf);
-			r->func_cb = NULL;
-		}
 		/* Free request */
 		r->result_pending = 0;
 		r->active = 0;
@@ -480,23 +551,6 @@ err_i2c:
 	return IRQ_HANDLED;
 }
 
-static int twl4030_madc_set_irq(struct twl4030_madc_data *madc,
-				struct twl4030_madc_request *req)
-{
-	struct twl4030_madc_request *p;
-	int ret;
-
-	p = &madc->requests[req->method];
-	memcpy(p, req, sizeof(*req));
-	ret = twl4030_madc_enable_irq(madc, req->method);
-	if (ret < 0) {
-		dev_err(madc->dev, "enable irq failed!!\n");
-		return ret;
-	}
-
-	return 0;
-}
-
 /*
  * Function which enables the madc conversion
  * by writing to the control register.
@@ -568,7 +622,7 @@ static int twl4030_madc_wait_conversion_ready(struct twl4030_madc_data *madc,
  * be a negative error value in the corresponding array element.
  * returns 0 if succeeds else error value
  */
-int twl4030_madc_conversion(struct twl4030_madc_request *req)
+static int twl4030_madc_conversion(struct twl4030_madc_request *req)
 {
 	const struct twl4030_madc_conversion_method *method;
 	int ret;
@@ -605,17 +659,6 @@ int twl4030_madc_conversion(struct twl4030_madc_request *req)
 			goto out;
 		}
 	}
-	if (req->type == TWL4030_MADC_IRQ_ONESHOT && req->func_cb != NULL) {
-		ret = twl4030_madc_set_irq(twl4030_madc, req);
-		if (ret < 0)
-			goto out;
-		ret = twl4030_madc_start_conversion(twl4030_madc, req->method);
-		if (ret < 0)
-			goto out;
-		twl4030_madc->requests[req->method].active = 1;
-		ret = 0;
-		goto out;
-	}
 	/* With RT method we should not be here anymore */
 	if (req->method == TWL4030_MADC_RT) {
 		ret = -EINVAL;
@@ -640,28 +683,6 @@ out:
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(twl4030_madc_conversion);
-
-int twl4030_get_madc_conversion(int channel_no)
-{
-	struct twl4030_madc_request req;
-	int temp = 0;
-	int ret;
-
-	req.channels = (1 << channel_no);
-	req.method = TWL4030_MADC_SW2;
-	req.active = 0;
-	req.raw = 0;
-	req.func_cb = NULL;
-	ret = twl4030_madc_conversion(&req);
-	if (ret < 0)
-		return ret;
-	if (req.rbuf[channel_no] > 0)
-		temp = req.rbuf[channel_no];
-
-	return temp;
-}
-EXPORT_SYMBOL_GPL(twl4030_get_madc_conversion);
 
 /**
  * twl4030_madc_set_current_generator() - setup bias current
diff --git a/drivers/iio/adc/xilinx-xadc-core.c b/drivers/iio/adc/xilinx-xadc-core.c
index 56cf5907a5f0..4a60497a1f19 100644
--- a/drivers/iio/adc/xilinx-xadc-core.c
+++ b/drivers/iio/adc/xilinx-xadc-core.c
@@ -1204,7 +1204,10 @@ static int xadc_probe(struct platform_device *pdev)
 		ret = PTR_ERR(xadc->clk);
 		goto err_free_samplerate_trigger;
 	}
-	clk_prepare_enable(xadc->clk);
+
+	ret = clk_prepare_enable(xadc->clk);
+	if (ret)
+		goto err_free_samplerate_trigger;
 
 	ret = xadc->ops->setup(pdev, indio_dev, irq);
 	if (ret)
diff --git a/drivers/iio/buffer/industrialio-buffer-dma.c b/drivers/iio/buffer/industrialio-buffer-dma.c
index dd99d273bae9..ff03324dee13 100644
--- a/drivers/iio/buffer/industrialio-buffer-dma.c
+++ b/drivers/iio/buffer/industrialio-buffer-dma.c
@@ -14,6 +14,7 @@
 #include <linux/sched.h>
 #include <linux/poll.h>
 #include <linux/iio/buffer.h>
+#include <linux/iio/buffer_impl.h>
 #include <linux/iio/buffer-dma.h>
 #include <linux/dma-mapping.h>
 #include <linux/sizes.h>
diff --git a/drivers/iio/buffer/industrialio-buffer-dmaengine.c b/drivers/iio/buffer/industrialio-buffer-dmaengine.c
index 9fabed47053d..2b5a320f42c5 100644
--- a/drivers/iio/buffer/industrialio-buffer-dmaengine.c
+++ b/drivers/iio/buffer/industrialio-buffer-dmaengine.c
@@ -14,6 +14,7 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/buffer.h>
+#include <linux/iio/buffer_impl.h>
 #include <linux/iio/buffer-dma.h>
 #include <linux/iio/buffer-dmaengine.h>
 
diff --git a/drivers/iio/common/hid-sensors/Kconfig b/drivers/iio/common/hid-sensors/Kconfig
index 39188b72cd3b..80105378b0ba 100644
--- a/drivers/iio/common/hid-sensors/Kconfig
+++ b/drivers/iio/common/hid-sensors/Kconfig
@@ -16,7 +16,7 @@ config HID_SENSOR_IIO_COMMON
 
 config HID_SENSOR_IIO_TRIGGER
 	tristate "Common module (trigger) for all HID Sensor IIO drivers"
-	depends on HID_SENSOR_HUB && HID_SENSOR_IIO_COMMON
+	depends on HID_SENSOR_HUB && HID_SENSOR_IIO_COMMON && IIO_BUFFER
 	select IIO_TRIGGER
 	help
 	  Say yes here to build trigger support for HID sensors.
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
index 1c0874cdf665..f5d4d786e193 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
@@ -69,6 +69,12 @@ static struct {
 	{HID_USAGE_SENSOR_TIME_TIMESTAMP, HID_USAGE_SENSOR_UNITS_MILLISECOND,
 		1000000, 0},
 
+	{HID_USAGE_SENSOR_DEVICE_ORIENTATION, 0, 1, 0},
+
+	{HID_USAGE_SENSOR_RELATIVE_ORIENTATION, 0, 1, 0},
+
+	{HID_USAGE_SENSOR_GEOMAGNETIC_ORIENTATION, 0, 1, 0},
+
 	{HID_USAGE_SENSOR_TEMPERATURE, 0, 1000, 0},
 	{HID_USAGE_SENSOR_TEMPERATURE, HID_USAGE_SENSOR_UNITS_DEGREES, 1000, 0},
 
@@ -230,7 +236,7 @@ int hid_sensor_write_samp_freq_value(struct hid_sensor_common *st,
 	ret = sensor_hub_set_feature(st->hsdev, st->poll.report_id,
 				     st->poll.index, sizeof(value), &value);
 	if (ret < 0 || value < 0)
-		ret = -EINVAL;
+		return -EINVAL;
 
 	ret = sensor_hub_get_feature(st->hsdev,
 				     st->poll.report_id,
@@ -283,7 +289,7 @@ int hid_sensor_write_raw_hyst_value(struct hid_sensor_common *st,
 				     st->sensitivity.index, sizeof(value),
 				     &value);
 	if (ret < 0 || value < 0)
-		ret = -EINVAL;
+		return -EINVAL;
 
 	ret = sensor_hub_get_feature(st->hsdev,
 				     st->sensitivity.report_id,
@@ -404,6 +410,48 @@ int hid_sensor_get_reporting_interval(struct hid_sensor_hub_device *hsdev,
 
 }
 
+static void hid_sensor_get_report_latency_info(struct hid_sensor_hub_device *hsdev,
+					       u32 usage_id,
+					       struct hid_sensor_common *st)
+{
+	sensor_hub_input_get_attribute_info(hsdev, HID_FEATURE_REPORT,
+					    usage_id,
+					    HID_USAGE_SENSOR_PROP_REPORT_LATENCY,
+					    &st->report_latency);
+
+	hid_dbg(hsdev->hdev, "Report latency attributes: %x:%x\n",
+		st->report_latency.index, st->report_latency.report_id);
+}
+
+int hid_sensor_get_report_latency(struct hid_sensor_common *st)
+{
+	int ret;
+	int value;
+
+	ret = sensor_hub_get_feature(st->hsdev, st->report_latency.report_id,
+				     st->report_latency.index, sizeof(value),
+				     &value);
+	if (ret < 0)
+		return ret;
+
+	return value;
+}
+EXPORT_SYMBOL(hid_sensor_get_report_latency);
+
+int hid_sensor_set_report_latency(struct hid_sensor_common *st, int latency_ms)
+{
+	return sensor_hub_set_feature(st->hsdev, st->report_latency.report_id,
+				      st->report_latency.index,
+				      sizeof(latency_ms), &latency_ms);
+}
+EXPORT_SYMBOL(hid_sensor_set_report_latency);
+
+bool hid_sensor_batch_mode_supported(struct hid_sensor_common *st)
+{
+	return st->report_latency.index > 0 && st->report_latency.report_id > 0;
+}
+EXPORT_SYMBOL(hid_sensor_batch_mode_supported);
+
 int hid_sensor_parse_common_attributes(struct hid_sensor_hub_device *hsdev,
 					u32 usage_id,
 					struct hid_sensor_common *st)
@@ -445,6 +493,8 @@ int hid_sensor_parse_common_attributes(struct hid_sensor_hub_device *hsdev,
 	} else
 		st->timestamp_ns_scale = 1000000000;
 
+	hid_sensor_get_report_latency_info(hsdev, usage_id, st);
+
 	hid_dbg(hsdev->hdev, "common attributes: %x:%x, %x:%x, %x:%x %x:%x %x:%x\n",
 		st->poll.index, st->poll.report_id,
 		st->report_state.index, st->report_state.report_id,
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
index 0b5dea050239..16ade0a0327b 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
@@ -26,9 +26,84 @@
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/trigger.h>
+#include <linux/iio/buffer.h>
 #include <linux/iio/sysfs.h>
 #include "hid-sensor-trigger.h"
 
+static ssize_t _hid_sensor_set_report_latency(struct device *dev,
+					      struct device_attribute *attr,
+					      const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct hid_sensor_common *attrb = iio_device_get_drvdata(indio_dev);
+	int integer, fract, ret;
+	int latency;
+
+	ret = iio_str_to_fixpoint(buf, 100000, &integer, &fract);
+	if (ret)
+		return ret;
+
+	latency = integer * 1000 + fract / 1000;
+	ret = hid_sensor_set_report_latency(attrb, latency);
+	if (ret < 0)
+		return len;
+
+	attrb->latency_ms = hid_sensor_get_report_latency(attrb);
+
+	return len;
+}
+
+static ssize_t _hid_sensor_get_report_latency(struct device *dev,
+					      struct device_attribute *attr,
+					      char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct hid_sensor_common *attrb = iio_device_get_drvdata(indio_dev);
+	int latency;
+
+	latency = hid_sensor_get_report_latency(attrb);
+	if (latency < 0)
+		return latency;
+
+	return sprintf(buf, "%d.%06u\n", latency / 1000, (latency % 1000) * 1000);
+}
+
+static ssize_t _hid_sensor_get_fifo_state(struct device *dev,
+					  struct device_attribute *attr,
+					  char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct hid_sensor_common *attrb = iio_device_get_drvdata(indio_dev);
+	int latency;
+
+	latency = hid_sensor_get_report_latency(attrb);
+	if (latency < 0)
+		return latency;
+
+	return sprintf(buf, "%d\n", !!latency);
+}
+
+static IIO_DEVICE_ATTR(hwfifo_timeout, 0644,
+		       _hid_sensor_get_report_latency,
+		       _hid_sensor_set_report_latency, 0);
+static IIO_DEVICE_ATTR(hwfifo_enabled, 0444,
+		       _hid_sensor_get_fifo_state, NULL, 0);
+
+static const struct attribute *hid_sensor_fifo_attributes[] = {
+	&iio_dev_attr_hwfifo_timeout.dev_attr.attr,
+	&iio_dev_attr_hwfifo_enabled.dev_attr.attr,
+	NULL,
+};
+
+static void hid_sensor_setup_batch_mode(struct iio_dev *indio_dev,
+					struct hid_sensor_common *st)
+{
+	if (!hid_sensor_batch_mode_supported(st))
+		return;
+
+	iio_buffer_set_attrs(indio_dev->buffer, hid_sensor_fifo_attributes);
+}
+
 static int _hid_sensor_power_state(struct hid_sensor_common *st, bool state)
 {
 	int state_val;
@@ -141,6 +216,9 @@ static void hid_sensor_set_power_work(struct work_struct *work)
 				       sizeof(attrb->raw_hystersis),
 				       &attrb->raw_hystersis);
 
+	if (attrb->latency_ms > 0)
+		hid_sensor_set_report_latency(attrb, attrb->latency_ms);
+
 	_hid_sensor_power_state(attrb, true);
 }
 
@@ -192,6 +270,8 @@ int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
 	attrb->trigger = trig;
 	indio_dev->trig = iio_trigger_get(trig);
 
+	hid_sensor_setup_batch_mode(indio_dev, attrb);
+
 	ret = pm_runtime_set_active(&indio_dev->dev);
 	if (ret)
 		goto error_unreg_trigger;
diff --git a/drivers/iio/dac/Kconfig b/drivers/iio/dac/Kconfig
index df5abc46cd3f..25bed2d7d2b9 100644
--- a/drivers/iio/dac/Kconfig
+++ b/drivers/iio/dac/Kconfig
@@ -13,7 +13,8 @@ config AD5064
 	  AD5045, AD5064, AD5064-1, AD5065, AD5625, AD5625R, AD5627, AD5627R,
 	  AD5628, AD5629R, AD5645R, AD5647R, AD5648, AD5665, AD5665R, AD5666,
 	  AD5667, AD5667R, AD5668, AD5669R, LTC2606, LTC2607, LTC2609, LTC2616,
-	  LTC2617, LTC2619, LTC2626, LTC2627, LTC2629 Digital to Analog Converter.
+	  LTC2617, LTC2619, LTC2626, LTC2627, LTC2629, LTC2631, LTC2633, LTC2635
+	  Digital to Analog Converter.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad5064.
diff --git a/drivers/iio/dac/ad5064.c b/drivers/iio/dac/ad5064.c
index 6803e4a137cd..3f9399c27869 100644
--- a/drivers/iio/dac/ad5064.c
+++ b/drivers/iio/dac/ad5064.c
@@ -2,8 +2,8 @@
  * AD5024, AD5025, AD5044, AD5045, AD5064, AD5064-1, AD5065, AD5625, AD5625R,
  * AD5627, AD5627R, AD5628, AD5629R, AD5645R, AD5647R, AD5648, AD5665, AD5665R,
  * AD5666, AD5667, AD5667R, AD5668, AD5669R, LTC2606, LTC2607, LTC2609, LTC2616,
- * LTC2617, LTC2619, LTC2626, LTC2627, LTC2629 Digital to analog converters
- * driver
+ * LTC2617, LTC2619, LTC2626, LTC2627, LTC2629, LTC2631, LTC2633, LTC2635
+ * Digital to analog converters driver
  *
  * Copyright 2011 Analog Devices Inc.
  *
@@ -168,6 +168,24 @@ enum ad5064_type {
 	ID_LTC2626,
 	ID_LTC2627,
 	ID_LTC2629,
+	ID_LTC2631_L12,
+	ID_LTC2631_H12,
+	ID_LTC2631_L10,
+	ID_LTC2631_H10,
+	ID_LTC2631_L8,
+	ID_LTC2631_H8,
+	ID_LTC2633_L12,
+	ID_LTC2633_H12,
+	ID_LTC2633_L10,
+	ID_LTC2633_H10,
+	ID_LTC2633_L8,
+	ID_LTC2633_H8,
+	ID_LTC2635_L12,
+	ID_LTC2635_H12,
+	ID_LTC2635_L10,
+	ID_LTC2635_H10,
+	ID_LTC2635_L8,
+	ID_LTC2635_H8,
 };
 
 static int ad5064_write(struct ad5064_state *st, unsigned int cmd,
@@ -425,6 +443,19 @@ static DECLARE_AD5064_CHANNELS(ad5669_channels, 16, 0, ad5064_ext_info);
 static DECLARE_AD5064_CHANNELS(ltc2607_channels, 16, 0, ltc2617_ext_info);
 static DECLARE_AD5064_CHANNELS(ltc2617_channels, 14, 2, ltc2617_ext_info);
 static DECLARE_AD5064_CHANNELS(ltc2627_channels, 12, 4, ltc2617_ext_info);
+#define ltc2631_12_channels ltc2627_channels
+static DECLARE_AD5064_CHANNELS(ltc2631_10_channels, 10, 6, ltc2617_ext_info);
+static DECLARE_AD5064_CHANNELS(ltc2631_8_channels, 8, 8, ltc2617_ext_info);
+
+#define LTC2631_INFO(vref, pchannels, nchannels)	\
+	{						\
+		.shared_vref = true,			\
+		.internal_vref = vref,			\
+		.channels = pchannels,			\
+		.num_channels = nchannels,		\
+		.regmap_type = AD5064_REGMAP_LTC,	\
+	}
+
 
 static const struct ad5064_chip_info ad5064_chip_info_tbl[] = {
 	[ID_AD5024] = {
@@ -724,6 +755,24 @@ static const struct ad5064_chip_info ad5064_chip_info_tbl[] = {
 		.num_channels = 4,
 		.regmap_type = AD5064_REGMAP_LTC,
 	},
+	[ID_LTC2631_L12] = LTC2631_INFO(2500000, ltc2631_12_channels, 1),
+	[ID_LTC2631_H12] = LTC2631_INFO(4096000, ltc2631_12_channels, 1),
+	[ID_LTC2631_L10] = LTC2631_INFO(2500000, ltc2631_10_channels, 1),
+	[ID_LTC2631_H10] = LTC2631_INFO(4096000, ltc2631_10_channels, 1),
+	[ID_LTC2631_L8] = LTC2631_INFO(2500000, ltc2631_8_channels, 1),
+	[ID_LTC2631_H8] = LTC2631_INFO(4096000, ltc2631_8_channels, 1),
+	[ID_LTC2633_L12] = LTC2631_INFO(2500000, ltc2631_12_channels, 2),
+	[ID_LTC2633_H12] = LTC2631_INFO(4096000, ltc2631_12_channels, 2),
+	[ID_LTC2633_L10] = LTC2631_INFO(2500000, ltc2631_10_channels, 2),
+	[ID_LTC2633_H10] = LTC2631_INFO(4096000, ltc2631_10_channels, 2),
+	[ID_LTC2633_L8] = LTC2631_INFO(2500000, ltc2631_8_channels, 2),
+	[ID_LTC2633_H8] = LTC2631_INFO(4096000, ltc2631_8_channels, 2),
+	[ID_LTC2635_L12] = LTC2631_INFO(2500000, ltc2631_12_channels, 4),
+	[ID_LTC2635_H12] = LTC2631_INFO(4096000, ltc2631_12_channels, 4),
+	[ID_LTC2635_L10] = LTC2631_INFO(2500000, ltc2631_10_channels, 4),
+	[ID_LTC2635_H10] = LTC2631_INFO(4096000, ltc2631_10_channels, 4),
+	[ID_LTC2635_L8] = LTC2631_INFO(2500000, ltc2631_8_channels, 4),
+	[ID_LTC2635_H8] = LTC2631_INFO(4096000, ltc2631_8_channels, 4),
 };
 
 static inline unsigned int ad5064_num_vref(struct ad5064_state *st)
@@ -982,6 +1031,24 @@ static const struct i2c_device_id ad5064_i2c_ids[] = {
 	{"ltc2626", ID_LTC2626},
 	{"ltc2627", ID_LTC2627},
 	{"ltc2629", ID_LTC2629},
+	{"ltc2631-l12", ID_LTC2631_L12},
+	{"ltc2631-h12", ID_LTC2631_H12},
+	{"ltc2631-l10", ID_LTC2631_L10},
+	{"ltc2631-h10", ID_LTC2631_H10},
+	{"ltc2631-l8", ID_LTC2631_L8},
+	{"ltc2631-h8", ID_LTC2631_H8},
+	{"ltc2633-l12", ID_LTC2633_L12},
+	{"ltc2633-h12", ID_LTC2633_H12},
+	{"ltc2633-l10", ID_LTC2633_L10},
+	{"ltc2633-h10", ID_LTC2633_H10},
+	{"ltc2633-l8", ID_LTC2633_L8},
+	{"ltc2633-h8", ID_LTC2633_H8},
+	{"ltc2635-l12", ID_LTC2635_L12},
+	{"ltc2635-h12", ID_LTC2635_H12},
+	{"ltc2635-l10", ID_LTC2635_L10},
+	{"ltc2635-h10", ID_LTC2635_H10},
+	{"ltc2635-l8", ID_LTC2635_L8},
+	{"ltc2635-h8", ID_LTC2635_H8},
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, ad5064_i2c_ids);
diff --git a/drivers/iio/humidity/hts221.h b/drivers/iio/humidity/hts221.h
index c7154665512e..94510266e0a5 100644
--- a/drivers/iio/humidity/hts221.h
+++ b/drivers/iio/humidity/hts221.h
@@ -57,12 +57,15 @@ struct hts221_hw {
 
 	struct hts221_sensor sensors[HTS221_SENSOR_MAX];
 
+	bool enabled;
 	u8 odr;
 
 	const struct hts221_transfer_function *tf;
 	struct hts221_transfer_buffer tb;
 };
 
+extern const struct dev_pm_ops hts221_pm_ops;
+
 int hts221_config_drdy(struct hts221_hw *hw, bool enable);
 int hts221_probe(struct iio_dev *iio_dev);
 int hts221_power_on(struct hts221_hw *hw);
diff --git a/drivers/iio/humidity/hts221_core.c b/drivers/iio/humidity/hts221_core.c
index 3f3ef4a1a474..a56da3999e00 100644
--- a/drivers/iio/humidity/hts221_core.c
+++ b/drivers/iio/humidity/hts221_core.c
@@ -13,6 +13,7 @@
 #include <linux/device.h>
 #include <linux/iio/sysfs.h>
 #include <linux/delay.h>
+#include <linux/pm.h>
 #include <asm/unaligned.h>
 
 #include "hts221.h"
@@ -307,15 +308,30 @@ hts221_sysfs_temp_oversampling_avail(struct device *dev,
 
 int hts221_power_on(struct hts221_hw *hw)
 {
-	return hts221_update_odr(hw, hw->odr);
+	int err;
+
+	err = hts221_update_odr(hw, hw->odr);
+	if (err < 0)
+		return err;
+
+	hw->enabled = true;
+
+	return 0;
 }
 
 int hts221_power_off(struct hts221_hw *hw)
 {
-	u8 data[] = {0x00, 0x00};
+	__le16 data = 0;
+	int err;
 
-	return hw->tf->write(hw->dev, HTS221_REG_CNTRL1_ADDR, sizeof(data),
-			     data);
+	err = hw->tf->write(hw->dev, HTS221_REG_CNTRL1_ADDR, sizeof(data),
+			    (u8 *)&data);
+	if (err < 0)
+		return err;
+
+	hw->enabled = false;
+
+	return 0;
 }
 
 static int hts221_parse_temp_caldata(struct hts221_hw *hw)
@@ -682,6 +698,36 @@ int hts221_probe(struct iio_dev *iio_dev)
 }
 EXPORT_SYMBOL(hts221_probe);
 
+static int __maybe_unused hts221_suspend(struct device *dev)
+{
+	struct iio_dev *iio_dev = dev_get_drvdata(dev);
+	struct hts221_hw *hw = iio_priv(iio_dev);
+	__le16 data = 0;
+	int err;
+
+	err = hw->tf->write(hw->dev, HTS221_REG_CNTRL1_ADDR, sizeof(data),
+			    (u8 *)&data);
+
+	return err < 0 ? err : 0;
+}
+
+static int __maybe_unused hts221_resume(struct device *dev)
+{
+	struct iio_dev *iio_dev = dev_get_drvdata(dev);
+	struct hts221_hw *hw = iio_priv(iio_dev);
+	int err = 0;
+
+	if (hw->enabled)
+		err = hts221_update_odr(hw, hw->odr);
+
+	return err;
+}
+
+const struct dev_pm_ops hts221_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(hts221_suspend, hts221_resume)
+};
+EXPORT_SYMBOL(hts221_pm_ops);
+
 MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
 MODULE_DESCRIPTION("STMicroelectronics hts221 sensor driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/humidity/hts221_i2c.c b/drivers/iio/humidity/hts221_i2c.c
index 8333c0296c0e..f38e4b7e0160 100644
--- a/drivers/iio/humidity/hts221_i2c.c
+++ b/drivers/iio/humidity/hts221_i2c.c
@@ -105,6 +105,7 @@ MODULE_DEVICE_TABLE(i2c, hts221_i2c_id_table);
 static struct i2c_driver hts221_driver = {
 	.driver = {
 		.name = "hts221_i2c",
+		.pm = &hts221_pm_ops,
 		.of_match_table = of_match_ptr(hts221_i2c_of_match),
 		.acpi_match_table = ACPI_PTR(hts221_acpi_match),
 	},
diff --git a/drivers/iio/humidity/hts221_spi.c b/drivers/iio/humidity/hts221_spi.c
index 70df5e7150c1..57cbc256771b 100644
--- a/drivers/iio/humidity/hts221_spi.c
+++ b/drivers/iio/humidity/hts221_spi.c
@@ -113,6 +113,7 @@ MODULE_DEVICE_TABLE(spi, hts221_spi_id_table);
 static struct spi_driver hts221_driver = {
 	.driver = {
 		.name = "hts221_spi",
+		.pm = &hts221_pm_ops,
 		.of_match_table = of_match_ptr(hts221_spi_of_match),
 	},
 	.probe = hts221_spi_probe,
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 96dabbd2f004..44830bce13df 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -41,6 +41,7 @@ static const int accel_scale[] = {598, 1196, 2392, 4785};
 static const struct inv_mpu6050_reg_map reg_set_6500 = {
 	.sample_rate_div	= INV_MPU6050_REG_SAMPLE_RATE_DIV,
 	.lpf                    = INV_MPU6050_REG_CONFIG,
+	.accel_lpf              = INV_MPU6500_REG_ACCEL_CONFIG_2,
 	.user_ctrl              = INV_MPU6050_REG_USER_CTRL,
 	.fifo_en                = INV_MPU6050_REG_FIFO_EN,
 	.gyro_config            = INV_MPU6050_REG_GYRO_CONFIG,
@@ -187,7 +188,6 @@ int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on)
 	int result = 0;
 
 	if (power_on) {
-		/* Already under indio-dev->mlock mutex */
 		if (!st->powerup_count)
 			result = regmap_write(st->map, st->reg->pwr_mgmt_1, 0);
 		if (!result)
@@ -211,6 +211,37 @@ int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on)
 EXPORT_SYMBOL_GPL(inv_mpu6050_set_power_itg);
 
 /**
+ *  inv_mpu6050_set_lpf_regs() - set low pass filter registers, chip dependent
+ *
+ *  MPU60xx/MPU9150 use only 1 register for accelerometer + gyroscope
+ *  MPU6500 and above have a dedicated register for accelerometer
+ */
+static int inv_mpu6050_set_lpf_regs(struct inv_mpu6050_state *st,
+				    enum inv_mpu6050_filter_e val)
+{
+	int result;
+
+	result = regmap_write(st->map, st->reg->lpf, val);
+	if (result)
+		return result;
+
+	switch (st->chip_type) {
+	case INV_MPU6050:
+	case INV_MPU6000:
+	case INV_MPU9150:
+		/* old chips, nothing to do */
+		result = 0;
+		break;
+	default:
+		/* set accel lpf */
+		result = regmap_write(st->map, st->reg->accel_lpf, val);
+		break;
+	}
+
+	return result;
+}
+
+/**
  *  inv_mpu6050_init_config() - Initialize hardware, disable FIFO.
  *
  *  Initial configuration:
@@ -233,8 +264,7 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev)
 	if (result)
 		return result;
 
-	d = INV_MPU6050_FILTER_20HZ;
-	result = regmap_write(st->map, st->reg->lpf, d);
+	result = inv_mpu6050_set_lpf_regs(st, INV_MPU6050_FILTER_20HZ);
 	if (result)
 		return result;
 
@@ -298,50 +328,37 @@ inv_mpu6050_read_raw(struct iio_dev *indio_dev,
 		int result;
 
 		ret = IIO_VAL_INT;
-		result = 0;
-		mutex_lock(&indio_dev->mlock);
-		if (!st->chip_config.enable) {
-			result = inv_mpu6050_set_power_itg(st, true);
-			if (result)
-				goto error_read_raw;
-		}
-		/* when enable is on, power is already on */
+		mutex_lock(&st->lock);
+		result = iio_device_claim_direct_mode(indio_dev);
+		if (result)
+			goto error_read_raw_unlock;
+		result = inv_mpu6050_set_power_itg(st, true);
+		if (result)
+			goto error_read_raw_release;
 		switch (chan->type) {
 		case IIO_ANGL_VEL:
-			if (!st->chip_config.gyro_fifo_enable ||
-			    !st->chip_config.enable) {
-				result = inv_mpu6050_switch_engine(st, true,
-						INV_MPU6050_BIT_PWR_GYRO_STBY);
-				if (result)
-					goto error_read_raw;
-			}
+			result = inv_mpu6050_switch_engine(st, true,
+					INV_MPU6050_BIT_PWR_GYRO_STBY);
+			if (result)
+				goto error_read_raw_power_off;
 			ret = inv_mpu6050_sensor_show(st, st->reg->raw_gyro,
 						      chan->channel2, val);
-			if (!st->chip_config.gyro_fifo_enable ||
-			    !st->chip_config.enable) {
-				result = inv_mpu6050_switch_engine(st, false,
-						INV_MPU6050_BIT_PWR_GYRO_STBY);
-				if (result)
-					goto error_read_raw;
-			}
+			result = inv_mpu6050_switch_engine(st, false,
+					INV_MPU6050_BIT_PWR_GYRO_STBY);
+			if (result)
+				goto error_read_raw_power_off;
 			break;
 		case IIO_ACCEL:
-			if (!st->chip_config.accl_fifo_enable ||
-			    !st->chip_config.enable) {
-				result = inv_mpu6050_switch_engine(st, true,
-						INV_MPU6050_BIT_PWR_ACCL_STBY);
-				if (result)
-					goto error_read_raw;
-			}
+			result = inv_mpu6050_switch_engine(st, true,
+					INV_MPU6050_BIT_PWR_ACCL_STBY);
+			if (result)
+				goto error_read_raw_power_off;
 			ret = inv_mpu6050_sensor_show(st, st->reg->raw_accl,
 						      chan->channel2, val);
-			if (!st->chip_config.accl_fifo_enable ||
-			    !st->chip_config.enable) {
-				result = inv_mpu6050_switch_engine(st, false,
-						INV_MPU6050_BIT_PWR_ACCL_STBY);
-				if (result)
-					goto error_read_raw;
-			}
+			result = inv_mpu6050_switch_engine(st, false,
+					INV_MPU6050_BIT_PWR_ACCL_STBY);
+			if (result)
+				goto error_read_raw_power_off;
 			break;
 		case IIO_TEMP:
 			/* wait for stablization */
@@ -353,10 +370,12 @@ inv_mpu6050_read_raw(struct iio_dev *indio_dev,
 			ret = -EINVAL;
 			break;
 		}
-error_read_raw:
-		if (!st->chip_config.enable)
-			result |= inv_mpu6050_set_power_itg(st, false);
-		mutex_unlock(&indio_dev->mlock);
+error_read_raw_power_off:
+		result |= inv_mpu6050_set_power_itg(st, false);
+error_read_raw_release:
+		iio_device_release_direct_mode(indio_dev);
+error_read_raw_unlock:
+		mutex_unlock(&st->lock);
 		if (result)
 			return result;
 
@@ -365,13 +384,17 @@ error_read_raw:
 	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_ANGL_VEL:
+			mutex_lock(&st->lock);
 			*val  = 0;
 			*val2 = gyro_scale_6050[st->chip_config.fsr];
+			mutex_unlock(&st->lock);
 
 			return IIO_VAL_INT_PLUS_NANO;
 		case IIO_ACCEL:
+			mutex_lock(&st->lock);
 			*val = 0;
 			*val2 = accel_scale[st->chip_config.accl_fs];
+			mutex_unlock(&st->lock);
 
 			return IIO_VAL_INT_PLUS_MICRO;
 		case IIO_TEMP:
@@ -394,12 +417,16 @@ error_read_raw:
 	case IIO_CHAN_INFO_CALIBBIAS:
 		switch (chan->type) {
 		case IIO_ANGL_VEL:
+			mutex_lock(&st->lock);
 			ret = inv_mpu6050_sensor_show(st, st->reg->gyro_offset,
 						chan->channel2, val);
+			mutex_unlock(&st->lock);
 			return IIO_VAL_INT;
 		case IIO_ACCEL:
+			mutex_lock(&st->lock);
 			ret = inv_mpu6050_sensor_show(st, st->reg->accl_offset,
 						chan->channel2, val);
+			mutex_unlock(&st->lock);
 			return IIO_VAL_INT;
 
 		default:
@@ -475,18 +502,17 @@ static int inv_mpu6050_write_raw(struct iio_dev *indio_dev,
 	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
 	int result;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	/*
 	 * we should only update scale when the chip is disabled, i.e.
 	 * not running
 	 */
-	if (st->chip_config.enable) {
-		result = -EBUSY;
-		goto error_write_raw;
-	}
+	result = iio_device_claim_direct_mode(indio_dev);
+	if (result)
+		goto error_write_raw_unlock;
 	result = inv_mpu6050_set_power_itg(st, true);
 	if (result)
-		goto error_write_raw;
+		goto error_write_raw_release;
 
 	switch (mask) {
 	case IIO_CHAN_INFO_SCALE:
@@ -522,9 +548,11 @@ static int inv_mpu6050_write_raw(struct iio_dev *indio_dev,
 		break;
 	}
 
-error_write_raw:
 	result |= inv_mpu6050_set_power_itg(st, false);
-	mutex_unlock(&indio_dev->mlock);
+error_write_raw_release:
+	iio_device_release_direct_mode(indio_dev);
+error_write_raw_unlock:
+	mutex_unlock(&st->lock);
 
 	return result;
 }
@@ -537,6 +565,8 @@ error_write_raw:
  *                  would be alising. This function basically search for the
  *                  correct low pass parameters based on the fifo rate, e.g,
  *                  sampling frequency.
+ *
+ *  lpf is set automatically when setting sampling rate to avoid any aliases.
  */
 static int inv_mpu6050_set_lpf(struct inv_mpu6050_state *st, int rate)
 {
@@ -552,7 +582,7 @@ static int inv_mpu6050_set_lpf(struct inv_mpu6050_state *st, int rate)
 	while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1))
 		i++;
 	data = d[i];
-	result = regmap_write(st->map, st->reg->lpf, data);
+	result = inv_mpu6050_set_lpf_regs(st, data);
 	if (result)
 		return result;
 	st->chip_config.lpf = data;
@@ -578,31 +608,35 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
 	if (fifo_rate < INV_MPU6050_MIN_FIFO_RATE ||
 	    fifo_rate > INV_MPU6050_MAX_FIFO_RATE)
 		return -EINVAL;
-	if (fifo_rate == st->chip_config.fifo_rate)
-		return count;
 
-	mutex_lock(&indio_dev->mlock);
-	if (st->chip_config.enable) {
-		result = -EBUSY;
-		goto fifo_rate_fail;
+	mutex_lock(&st->lock);
+	if (fifo_rate == st->chip_config.fifo_rate) {
+		result = 0;
+		goto fifo_rate_fail_unlock;
 	}
+	result = iio_device_claim_direct_mode(indio_dev);
+	if (result)
+		goto fifo_rate_fail_unlock;
 	result = inv_mpu6050_set_power_itg(st, true);
 	if (result)
-		goto fifo_rate_fail;
+		goto fifo_rate_fail_release;
 
 	d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1;
 	result = regmap_write(st->map, st->reg->sample_rate_div, d);
 	if (result)
-		goto fifo_rate_fail;
+		goto fifo_rate_fail_power_off;
 	st->chip_config.fifo_rate = fifo_rate;
 
 	result = inv_mpu6050_set_lpf(st, fifo_rate);
 	if (result)
-		goto fifo_rate_fail;
+		goto fifo_rate_fail_power_off;
 
-fifo_rate_fail:
+fifo_rate_fail_power_off:
 	result |= inv_mpu6050_set_power_itg(st, false);
-	mutex_unlock(&indio_dev->mlock);
+fifo_rate_fail_release:
+	iio_device_release_direct_mode(indio_dev);
+fifo_rate_fail_unlock:
+	mutex_unlock(&st->lock);
 	if (result)
 		return result;
 
@@ -617,8 +651,13 @@ inv_fifo_rate_show(struct device *dev, struct device_attribute *attr,
 		   char *buf)
 {
 	struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev));
+	unsigned fifo_rate;
 
-	return sprintf(buf, "%d\n", st->chip_config.fifo_rate);
+	mutex_lock(&st->lock);
+	fifo_rate = st->chip_config.fifo_rate;
+	mutex_unlock(&st->lock);
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", fifo_rate);
 }
 
 /**
@@ -645,7 +684,8 @@ static ssize_t inv_attr_show(struct device *dev, struct device_attribute *attr,
 	case ATTR_ACCL_MATRIX:
 		m = st->plat_data.orientation;
 
-		return sprintf(buf, "%d, %d, %d; %d, %d, %d; %d, %d, %d\n",
+		return scnprintf(buf, PAGE_SIZE,
+			"%d, %d, %d; %d, %d, %d; %d, %d, %d\n",
 			m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
 	default:
 		return -EINVAL;
@@ -770,27 +810,42 @@ static int inv_check_and_setup_chip(struct inv_mpu6050_state *st)
 {
 	int result;
 	unsigned int regval;
+	int i;
 
 	st->hw  = &hw_info[st->chip_type];
 	st->reg = hw_info[st->chip_type].reg;
 
-	/* reset to make sure previous state are not there */
-	result = regmap_write(st->map, st->reg->pwr_mgmt_1,
-			      INV_MPU6050_BIT_H_RESET);
-	if (result)
-		return result;
-	msleep(INV_MPU6050_POWER_UP_TIME);
-
 	/* check chip self-identification */
 	result = regmap_read(st->map, INV_MPU6050_REG_WHOAMI, &regval);
 	if (result)
 		return result;
 	if (regval != st->hw->whoami) {
-		dev_warn(regmap_get_device(st->map),
-				"whoami mismatch got %#02x expected %#02hhx for %s\n",
+		/* check whoami against all possible values */
+		for (i = 0; i < INV_NUM_PARTS; ++i) {
+			if (regval == hw_info[i].whoami) {
+				dev_warn(regmap_get_device(st->map),
+					"whoami mismatch got %#02x (%s)"
+					"expected %#02hhx (%s)\n",
+					regval, hw_info[i].name,
+					st->hw->whoami, st->hw->name);
+				break;
+			}
+		}
+		if (i >= INV_NUM_PARTS) {
+			dev_err(regmap_get_device(st->map),
+				"invalid whoami %#02x expected %#02hhx (%s)\n",
 				regval, st->hw->whoami, st->hw->name);
+			return -ENODEV;
+		}
 	}
 
+	/* reset to make sure previous state are not there */
+	result = regmap_write(st->map, st->reg->pwr_mgmt_1,
+			      INV_MPU6050_BIT_H_RESET);
+	if (result)
+		return result;
+	msleep(INV_MPU6050_POWER_UP_TIME);
+
 	/*
 	 * toggle power state. After reset, the sleep bit could be on
 	 * or off depending on the OTP settings. Toggling power would
@@ -836,6 +891,7 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 		return -ENODEV;
 	}
 	st = iio_priv(indio_dev);
+	mutex_init(&st->lock);
 	st->chip_type = chip_type;
 	st->powerup_count = 0;
 	st->irq = irq;
@@ -929,12 +985,26 @@ EXPORT_SYMBOL_GPL(inv_mpu_core_remove);
 
 static int inv_mpu_resume(struct device *dev)
 {
-	return inv_mpu6050_set_power_itg(iio_priv(dev_get_drvdata(dev)), true);
+	struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev));
+	int result;
+
+	mutex_lock(&st->lock);
+	result = inv_mpu6050_set_power_itg(st, true);
+	mutex_unlock(&st->lock);
+
+	return result;
 }
 
 static int inv_mpu_suspend(struct device *dev)
 {
-	return inv_mpu6050_set_power_itg(iio_priv(dev_get_drvdata(dev)), false);
+	struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev));
+	int result;
+
+	mutex_lock(&st->lock);
+	result = inv_mpu6050_set_power_itg(st, false);
+	mutex_unlock(&st->lock);
+
+	return result;
 }
 #endif /* CONFIG_PM_SLEEP */
 
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
index 64b5f5b92200..fcd7a92b6cf8 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
@@ -32,7 +32,7 @@ static int inv_mpu6050_select_bypass(struct i2c_mux_core *muxc, u32 chan_id)
 	int ret = 0;
 
 	/* Use the same mutex which was used everywhere to protect power-op */
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	if (!st->powerup_count) {
 		ret = regmap_write(st->map, st->reg->pwr_mgmt_1, 0);
 		if (ret)
@@ -48,7 +48,7 @@ static int inv_mpu6050_select_bypass(struct i2c_mux_core *muxc, u32 chan_id)
 				   INV_MPU6050_BIT_BYPASS_EN);
 	}
 write_error:
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -58,14 +58,14 @@ static int inv_mpu6050_deselect_bypass(struct i2c_mux_core *muxc, u32 chan_id)
 	struct iio_dev *indio_dev = i2c_mux_priv(muxc);
 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	/* It doesn't really mattter, if any of the calls fails */
 	regmap_write(st->map, st->reg->int_pin_cfg, INV_MPU6050_INT_PIN_CFG);
 	st->powerup_count--;
 	if (!st->powerup_count)
 		regmap_write(st->map, st->reg->pwr_mgmt_1,
 			     INV_MPU6050_BIT_SLEEP);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return 0;
 }
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index ef13de7a2c20..065794162d65 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -14,6 +14,7 @@
 #include <linux/i2c-mux.h>
 #include <linux/kfifo.h>
 #include <linux/spinlock.h>
+#include <linux/mutex.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/buffer.h>
 #include <linux/regmap.h>
@@ -28,6 +29,7 @@
  *  struct inv_mpu6050_reg_map - Notable registers.
  *  @sample_rate_div:	Divider applied to gyro output rate.
  *  @lpf:		Configures internal low pass filter.
+ *  @accel_lpf:		Configures accelerometer low pass filter.
  *  @user_ctrl:		Enables/resets the FIFO.
  *  @fifo_en:		Determines which data will appear in FIFO.
  *  @gyro_config:	gyro config register.
@@ -47,6 +49,7 @@
 struct inv_mpu6050_reg_map {
 	u8 sample_rate_div;
 	u8 lpf;
+	u8 accel_lpf;
 	u8 user_ctrl;
 	u8 fifo_en;
 	u8 gyro_config;
@@ -80,7 +83,6 @@ enum inv_devices {
  *  @fsr:		Full scale range.
  *  @lpf:		Digital low pass filter frequency.
  *  @accl_fs:		accel full scale range.
- *  @enable:		master enable state.
  *  @accl_fifo_enable:	enable accel data output
  *  @gyro_fifo_enable:	enable gyro data output
  *  @fifo_rate:		FIFO update rate.
@@ -89,7 +91,6 @@ struct inv_mpu6050_chip_config {
 	unsigned int fsr:2;
 	unsigned int lpf:3;
 	unsigned int accl_fs:2;
-	unsigned int enable:1;
 	unsigned int accl_fifo_enable:1;
 	unsigned int gyro_fifo_enable:1;
 	u16 fifo_rate;
@@ -112,6 +113,7 @@ struct inv_mpu6050_hw {
 /*
  *  struct inv_mpu6050_state - Driver state variables.
  *  @TIMESTAMP_FIFO_SIZE: fifo size for timestamp.
+ *  @lock:              Chip access lock.
  *  @trig:              IIO trigger.
  *  @chip_config:	Cached attribute information.
  *  @reg:		Map of important registers.
@@ -126,6 +128,7 @@ struct inv_mpu6050_hw {
  */
 struct inv_mpu6050_state {
 #define TIMESTAMP_FIFO_SIZE 16
+	struct mutex lock;
 	struct iio_trigger  *trig;
 	struct inv_mpu6050_chip_config chip_config;
 	const struct inv_mpu6050_reg_map *reg;
@@ -188,6 +191,7 @@ struct inv_mpu6050_state {
 #define INV_MPU6050_FIFO_THRESHOLD           500
 
 /* mpu6500 registers */
+#define INV_MPU6500_REG_ACCEL_CONFIG_2      0x1D
 #define INV_MPU6500_REG_ACCEL_OFFSET        0x77
 
 /* delay time in milliseconds */
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 3a9f3eac91ab..ff81c6aa009d 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -128,7 +128,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	u16 fifo_count;
 	s64 timestamp;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	if (!(st->chip_config.accl_fifo_enable |
 		st->chip_config.gyro_fifo_enable))
 		goto end_session;
@@ -178,7 +178,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	}
 
 end_session:
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 	iio_trigger_notify_done(indio_dev->trig);
 
 	return IRQ_HANDLED;
@@ -186,7 +186,7 @@ end_session:
 flush_fifo:
 	/* Flush HW and SW FIFOs. */
 	inv_reset_fifo(indio_dev);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 	iio_trigger_notify_done(indio_dev->trig);
 
 	return IRQ_HANDLED;
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index e8818d4dd4b8..540070f0a230 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -90,7 +90,6 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 		if (result)
 			return result;
 	}
-	st->chip_config.enable = enable;
 
 	return 0;
 }
@@ -103,7 +102,15 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig,
 					      bool state)
 {
-	return inv_mpu6050_set_enable(iio_trigger_get_drvdata(trig), state);
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	int result;
+
+	mutex_lock(&st->lock);
+	result = inv_mpu6050_set_enable(indio_dev, state);
+	mutex_unlock(&st->lock);
+
+	return result;
 }
 
 static const struct iio_trigger_ops inv_mpu_trigger_ops = {
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index 4839db7b9690..46352c7bff43 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -135,6 +135,8 @@ struct st_lsm6dsx_hw {
 #endif /* CONFIG_SPI_MASTER */
 };
 
+extern const struct dev_pm_ops st_lsm6dsx_pm_ops;
+
 int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
 		     const struct st_lsm6dsx_transfer_function *tf_ops);
 int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor);
@@ -144,5 +146,8 @@ int st_lsm6dsx_write_with_mask(struct st_lsm6dsx_hw *hw, u8 addr, u8 mask,
 			       u8 val);
 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor,
 				u16 watermark);
+int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw);
+int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
+			     enum st_lsm6dsx_fifo_mode fifo_mode);
 
 #endif /* ST_LSM6DSX_H */
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index c8e5cfd0ef0b..2a72acc6e049 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -37,6 +37,8 @@
 #define ST_LSM6DSX_REG_FIFO_THH_ADDR		0x07
 #define ST_LSM6DSX_FIFO_TH_MASK			GENMASK(11, 0)
 #define ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR	0x08
+#define ST_LSM6DSX_REG_HLACTIVE_ADDR		0x12
+#define ST_LSM6DSX_REG_HLACTIVE_MASK		BIT(5)
 #define ST_LSM6DSX_REG_FIFO_MODE_ADDR		0x0a
 #define ST_LSM6DSX_FIFO_MODE_MASK		GENMASK(2, 0)
 #define ST_LSM6DSX_FIFO_ODR_MASK		GENMASK(6, 3)
@@ -130,8 +132,8 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
 	return 0;
 }
 
-static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
-				    enum st_lsm6dsx_fifo_mode fifo_mode)
+int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
+			     enum st_lsm6dsx_fifo_mode fifo_mode)
 {
 	u8 data;
 	int err;
@@ -303,7 +305,7 @@ static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 	return read_len;
 }
 
-static int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
+int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
 {
 	int err;
 
@@ -417,6 +419,7 @@ int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
 {
 	struct iio_buffer *buffer;
 	unsigned long irq_type;
+	bool irq_active_low;
 	int i, err;
 
 	irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
@@ -424,12 +427,23 @@ int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
 	switch (irq_type) {
 	case IRQF_TRIGGER_HIGH:
 	case IRQF_TRIGGER_RISING:
+		irq_active_low = false;
+		break;
+	case IRQF_TRIGGER_LOW:
+	case IRQF_TRIGGER_FALLING:
+		irq_active_low = true;
 		break;
 	default:
 		dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
 		return -EINVAL;
 	}
 
+	err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_HLACTIVE_ADDR,
+					 ST_LSM6DSX_REG_HLACTIVE_MASK,
+					 irq_active_low);
+	if (err < 0)
+		return err;
+
 	err = devm_request_threaded_irq(hw->dev, hw->irq,
 					st_lsm6dsx_handler_irq,
 					st_lsm6dsx_handler_thread,
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 462a27b70453..b485540da89e 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -36,6 +36,7 @@
 #include <linux/delay.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
+#include <linux/pm.h>
 
 #include <linux/platform_data/st_sensors_pdata.h>
 
@@ -731,6 +732,57 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
 }
 EXPORT_SYMBOL(st_lsm6dsx_probe);
 
+static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
+{
+	struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev);
+	struct st_lsm6dsx_sensor *sensor;
+	int i, err = 0;
+
+	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+		sensor = iio_priv(hw->iio_devs[i]);
+		if (!(hw->enable_mask & BIT(sensor->id)))
+			continue;
+
+		err = st_lsm6dsx_write_with_mask(hw,
+				st_lsm6dsx_odr_table[sensor->id].reg.addr,
+				st_lsm6dsx_odr_table[sensor->id].reg.mask, 0);
+		if (err < 0)
+			return err;
+	}
+
+	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS)
+		err = st_lsm6dsx_flush_fifo(hw);
+
+	return err;
+}
+
+static int __maybe_unused st_lsm6dsx_resume(struct device *dev)
+{
+	struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev);
+	struct st_lsm6dsx_sensor *sensor;
+	int i, err = 0;
+
+	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+		sensor = iio_priv(hw->iio_devs[i]);
+		if (!(hw->enable_mask & BIT(sensor->id)))
+			continue;
+
+		err = st_lsm6dsx_set_odr(sensor, sensor->odr);
+		if (err < 0)
+			return err;
+	}
+
+	if (hw->enable_mask)
+		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
+
+	return err;
+}
+
+const struct dev_pm_ops st_lsm6dsx_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsx_suspend, st_lsm6dsx_resume)
+};
+EXPORT_SYMBOL(st_lsm6dsx_pm_ops);
+
 MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
 MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx driver");
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
index 09a51cfb9b5e..305fec712ab0 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
@@ -98,6 +98,7 @@ MODULE_DEVICE_TABLE(i2c, st_lsm6dsx_i2c_id_table);
 static struct i2c_driver st_lsm6dsx_driver = {
 	.driver = {
 		.name = "st_lsm6dsx_i2c",
+		.pm = &st_lsm6dsx_pm_ops,
 		.of_match_table = of_match_ptr(st_lsm6dsx_i2c_of_match),
 	},
 	.probe = st_lsm6dsx_i2c_probe,
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
index f765a5058488..95472f153ad2 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
@@ -115,6 +115,7 @@ MODULE_DEVICE_TABLE(spi, st_lsm6dsx_spi_id_table);
 static struct spi_driver st_lsm6dsx_driver = {
 	.driver = {
 		.name = "st_lsm6dsx_spi",
+		.pm = &st_lsm6dsx_pm_ops,
 		.of_match_table = of_match_ptr(st_lsm6dsx_spi_of_match),
 	},
 	.probe = st_lsm6dsx_spi_probe,
diff --git a/drivers/iio/industrialio-core.c b/drivers/iio/industrialio-core.c
index 57c14da5708f..17ec4cee51dc 100644
--- a/drivers/iio/industrialio-core.c
+++ b/drivers/iio/industrialio-core.c
@@ -478,21 +478,16 @@ ssize_t iio_enum_write(struct iio_dev *indio_dev,
 	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 = __sysfs_match_string(e->items, e->num_items, buf);
+	if (ret < 0)
+		return ret;
 
-	ret = e->set(indio_dev, chan, i);
+	ret = e->set(indio_dev, chan, ret);
 	return ret ? ret : len;
 }
 EXPORT_SYMBOL_GPL(iio_enum_write);
@@ -1089,7 +1084,7 @@ static int iio_device_add_info_mask_type(struct iio_dev *indio_dev,
 {
 	int i, ret, attrcount = 0;
 
-	for_each_set_bit(i, infomask, sizeof(infomask)*8) {
+	for_each_set_bit(i, infomask, sizeof(*infomask)*8) {
 		if (i >= ARRAY_SIZE(iio_chan_info_postfix))
 			return -EINVAL;
 		ret = __iio_add_chan_devattr(iio_chan_info_postfix[i],
@@ -1118,7 +1113,7 @@ static int iio_device_add_info_mask_type_avail(struct iio_dev *indio_dev,
 	int i, ret, attrcount = 0;
 	char *avail_postfix;
 
-	for_each_set_bit(i, infomask, sizeof(infomask) * 8) {
+	for_each_set_bit(i, infomask, sizeof(*infomask) * 8) {
 		avail_postfix = kasprintf(GFP_KERNEL,
 					  "%s_available",
 					  iio_chan_info_postfix[i]);
@@ -1428,7 +1423,7 @@ static void iio_device_unregister_sysfs(struct iio_dev *indio_dev)
 static void iio_dev_release(struct device *device)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(device);
-	if (indio_dev->modes & (INDIO_BUFFER_TRIGGERED | INDIO_EVENT_TRIGGERED))
+	if (indio_dev->modes & INDIO_ALL_TRIGGERED_MODES)
 		iio_device_unregister_trigger_consumer(indio_dev);
 	iio_device_unregister_eventset(indio_dev);
 	iio_device_unregister_sysfs(indio_dev);
@@ -1710,7 +1705,7 @@ int iio_device_register(struct iio_dev *indio_dev)
 			"Failed to register event set\n");
 		goto error_free_sysfs;
 	}
-	if (indio_dev->modes & (INDIO_BUFFER_TRIGGERED | INDIO_EVENT_TRIGGERED))
+	if (indio_dev->modes & INDIO_ALL_TRIGGERED_MODES)
 		iio_device_register_trigger_consumer(indio_dev);
 
 	if ((indio_dev->modes & INDIO_ALL_BUFFER_MODES) &&
diff --git a/drivers/iio/inkern.c b/drivers/iio/inkern.c
index 7a13535dc3e9..da3d06b073bb 100644
--- a/drivers/iio/inkern.c
+++ b/drivers/iio/inkern.c
@@ -750,11 +750,9 @@ int iio_read_avail_channel_raw(struct iio_channel *chan,
 err_unlock:
 	mutex_unlock(&chan->indio_dev->info_exist_lock);
 
-	if (ret >= 0 && type != IIO_VAL_INT) {
+	if (ret >= 0 && type != IIO_VAL_INT)
 		/* raw values are assumed to be IIO_VAL_INT */
 		ret = -EINVAL;
-		goto err_unlock;
-	}
 
 	return ret;
 }
@@ -869,3 +867,63 @@ err_unlock:
 	return ret;
 }
 EXPORT_SYMBOL_GPL(iio_write_channel_raw);
+
+unsigned int iio_get_channel_ext_info_count(struct iio_channel *chan)
+{
+	const struct iio_chan_spec_ext_info *ext_info;
+	unsigned int i = 0;
+
+	if (!chan->channel->ext_info)
+		return i;
+
+	for (ext_info = chan->channel->ext_info; ext_info->name; ext_info++)
+		++i;
+
+	return i;
+}
+EXPORT_SYMBOL_GPL(iio_get_channel_ext_info_count);
+
+static const struct iio_chan_spec_ext_info *iio_lookup_ext_info(
+						const struct iio_channel *chan,
+						const char *attr)
+{
+	const struct iio_chan_spec_ext_info *ext_info;
+
+	if (!chan->channel->ext_info)
+		return NULL;
+
+	for (ext_info = chan->channel->ext_info; ext_info->name; ++ext_info) {
+		if (!strcmp(attr, ext_info->name))
+			return ext_info;
+	}
+
+	return NULL;
+}
+
+ssize_t iio_read_channel_ext_info(struct iio_channel *chan,
+				  const char *attr, char *buf)
+{
+	const struct iio_chan_spec_ext_info *ext_info;
+
+	ext_info = iio_lookup_ext_info(chan, attr);
+	if (!ext_info)
+		return -EINVAL;
+
+	return ext_info->read(chan->indio_dev, ext_info->private,
+			      chan->channel, buf);
+}
+EXPORT_SYMBOL_GPL(iio_read_channel_ext_info);
+
+ssize_t iio_write_channel_ext_info(struct iio_channel *chan, const char *attr,
+				   const char *buf, size_t len)
+{
+	const struct iio_chan_spec_ext_info *ext_info;
+
+	ext_info = iio_lookup_ext_info(chan, attr);
+	if (!ext_info)
+		return -EINVAL;
+
+	return ext_info->write(chan->indio_dev, ext_info->private,
+			       chan->channel, buf, len);
+}
+EXPORT_SYMBOL_GPL(iio_write_channel_ext_info);
diff --git a/drivers/iio/light/Kconfig b/drivers/iio/light/Kconfig
index 33e755d8d825..2356ed9285df 100644
--- a/drivers/iio/light/Kconfig
+++ b/drivers/iio/light/Kconfig
@@ -172,6 +172,16 @@ config SENSORS_ISL29018
 	 in lux, proximity infrared sensing and normal infrared sensing.
 	 Data from sensor is accessible via sysfs.
 
+config SENSORS_ISL29028
+	tristate "Intersil ISL29028 Concurrent Light and Proximity Sensor"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	 Provides driver for the Intersil's ISL29028 device.
+	 This driver supports the sysfs interface to get the ALS, IR intensity,
+	 Proximity value via iio. The ISL29028 provides the concurrent sensing
+	 of ambient light and proximity.
+
 config ISL29125
 	tristate "Intersil ISL29125 digital color light sensor"
 	depends on I2C
diff --git a/drivers/iio/light/Makefile b/drivers/iio/light/Makefile
index 681363c2b298..fa32fa459e2e 100644
--- a/drivers/iio/light/Makefile
+++ b/drivers/iio/light/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_GP2AP020A00F)	+= gp2ap020a00f.o
 obj-$(CONFIG_HID_SENSOR_ALS)	+= hid-sensor-als.o
 obj-$(CONFIG_HID_SENSOR_PROX)	+= hid-sensor-prox.o
 obj-$(CONFIG_SENSORS_ISL29018)	+= isl29018.o
+obj-$(CONFIG_SENSORS_ISL29028)	+= isl29028.o
 obj-$(CONFIG_ISL29125)		+= isl29125.o
 obj-$(CONFIG_JSA1212)		+= jsa1212.o
 obj-$(CONFIG_SENSORS_LM3533)	+= lm3533-als.o
diff --git a/drivers/iio/light/isl29018.c b/drivers/iio/light/isl29018.c
index 917dd8b43e72..61f5924b472d 100644
--- a/drivers/iio/light/isl29018.c
+++ b/drivers/iio/light/isl29018.c
@@ -807,6 +807,7 @@ static SIMPLE_DEV_PM_OPS(isl29018_pm_ops, isl29018_suspend, isl29018_resume);
 #define ISL29018_PM_OPS NULL
 #endif
 
+#ifdef CONFIG_ACPI
 static const struct acpi_device_id isl29018_acpi_match[] = {
 	{"ISL29018", isl29018},
 	{"ISL29023", isl29023},
@@ -814,6 +815,7 @@ static const struct acpi_device_id isl29018_acpi_match[] = {
 	{},
 };
 MODULE_DEVICE_TABLE(acpi, isl29018_acpi_match);
+#endif
 
 static const struct i2c_device_id isl29018_id[] = {
 	{"isl29018", isl29018},
diff --git a/drivers/staging/iio/light/isl29028.c b/drivers/iio/light/isl29028.c
index 5375e7a81205..3d09c1fc4dad 100644
--- a/drivers/staging/iio/light/isl29028.c
+++ b/drivers/iio/light/isl29028.c
@@ -16,6 +16,10 @@
  *
  * You should have received a copy of the GNU General Public License
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Datasheets:
+ *  - http://www.intersil.com/content/dam/Intersil/documents/isl2/isl29028.pdf
+ *  - http://www.intersil.com/content/dam/Intersil/documents/isl2/isl29030.pdf
  */
 
 #include <linux/module.h>
@@ -64,8 +68,25 @@
 
 #define ISL29028_POWER_OFF_DELAY_MS		2000
 
-static const unsigned int isl29028_prox_sleep_time[] = {800, 400, 200, 100, 75,
-							50, 12, 0};
+struct isl29028_prox_data {
+	int sampling_int;
+	int sampling_fract;
+	int sleep_time;
+};
+
+static const struct isl29028_prox_data isl29028_prox_data[] = {
+	{   1, 250000, 800 },
+	{   2, 500000, 400 },
+	{   5,      0, 200 },
+	{  10,      0, 100 },
+	{  13, 300000,  75 },
+	{  20,      0,  50 },
+	{  80,      0,  13 }, /*
+			       * Note: Data sheet lists 12.5 ms sleep time.
+			       * Round up a half millisecond for msleep().
+			       */
+	{ 100,  0,   0 }
+};
 
 enum isl29028_als_ir_mode {
 	ISL29028_MODE_NONE = 0,
@@ -76,32 +97,37 @@ enum isl29028_als_ir_mode {
 struct isl29028_chip {
 	struct mutex			lock;
 	struct regmap			*regmap;
-	unsigned int			prox_sampling;
+	int				prox_sampling_int;
+	int				prox_sampling_frac;
 	bool				enable_prox;
 	int				lux_scale;
 	enum isl29028_als_ir_mode	als_ir_mode;
 };
 
-static int isl29028_find_prox_sleep_time_index(int sampling)
+static int isl29028_find_prox_sleep_index(int sampling_int, int sampling_fract)
 {
-	unsigned int period = DIV_ROUND_UP(1000, sampling);
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(isl29028_prox_sleep_time); ++i) {
-		if (period >= isl29028_prox_sleep_time[i])
-			break;
+	for (i = 0; i < ARRAY_SIZE(isl29028_prox_data); ++i) {
+		if (isl29028_prox_data[i].sampling_int == sampling_int &&
+		    isl29028_prox_data[i].sampling_fract == sampling_fract)
+			return i;
 	}
 
-	return i;
+	return -EINVAL;
 }
 
 static int isl29028_set_proxim_sampling(struct isl29028_chip *chip,
-					unsigned int sampling)
+					int sampling_int, int sampling_fract)
 {
 	struct device *dev = regmap_get_device(chip->regmap);
 	int sleep_index, ret;
 
-	sleep_index = isl29028_find_prox_sleep_time_index(sampling);
+	sleep_index = isl29028_find_prox_sleep_index(sampling_int,
+						     sampling_fract);
+	if (sleep_index < 0)
+		return sleep_index;
+
 	ret = regmap_update_bits(chip->regmap, ISL29028_REG_CONFIGURE,
 				 ISL29028_CONF_PROX_SLP_MASK,
 				 sleep_index << ISL29028_CONF_PROX_SLP_SH);
@@ -112,16 +138,18 @@ static int isl29028_set_proxim_sampling(struct isl29028_chip *chip,
 		return ret;
 	}
 
-	chip->prox_sampling = sampling;
+	chip->prox_sampling_int = sampling_int;
+	chip->prox_sampling_frac = sampling_fract;
 
 	return ret;
 }
 
 static int isl29028_enable_proximity(struct isl29028_chip *chip)
 {
-	int sleep_index, ret;
+	int prox_index, ret;
 
-	ret = isl29028_set_proxim_sampling(chip, chip->prox_sampling);
+	ret = isl29028_set_proxim_sampling(chip, chip->prox_sampling_int,
+					   chip->prox_sampling_frac);
 	if (ret < 0)
 		return ret;
 
@@ -132,8 +160,12 @@ static int isl29028_enable_proximity(struct isl29028_chip *chip)
 		return ret;
 
 	/* Wait for conversion to be complete for first sample */
-	sleep_index = isl29028_find_prox_sleep_time_index(chip->prox_sampling);
-	msleep(isl29028_prox_sleep_time[sleep_index]);
+	prox_index = isl29028_find_prox_sleep_index(chip->prox_sampling_int,
+						    chip->prox_sampling_frac);
+	if (prox_index < 0)
+		return prox_index;
+
+	msleep(isl29028_prox_data[prox_index].sleep_time);
 
 	return 0;
 }
@@ -361,7 +393,7 @@ static int isl29028_write_raw(struct iio_dev *indio_dev,
 			break;
 		}
 
-		ret = isl29028_set_proxim_sampling(chip, val);
+		ret = isl29028_set_proxim_sampling(chip, val, val2);
 		break;
 	case IIO_LIGHT:
 		if (mask != IIO_CHAN_INFO_SCALE) {
@@ -439,7 +471,8 @@ static int isl29028_read_raw(struct iio_dev *indio_dev,
 		if (chan->type != IIO_PROXIMITY)
 			break;
 
-		*val = chip->prox_sampling;
+		*val = chip->prox_sampling_int;
+		*val2 = chip->prox_sampling_frac;
 		ret = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
@@ -472,7 +505,7 @@ static int isl29028_read_raw(struct iio_dev *indio_dev,
 }
 
 static IIO_CONST_ATTR(in_proximity_sampling_frequency_available,
-				"1 3 5 10 13 20 83 100");
+				"1.25 2.5 5 10 13.3 20 80 100");
 static IIO_CONST_ATTR(in_illuminance_scale_available, "125 2000");
 
 #define ISL29028_CONST_ATTR(name) (&iio_const_attr_##name.dev_attr.attr)
@@ -571,7 +604,8 @@ static int isl29028_probe(struct i2c_client *client,
 	}
 
 	chip->enable_prox  = false;
-	chip->prox_sampling = 20;
+	chip->prox_sampling_int = 20;
+	chip->prox_sampling_frac = 0;
 	chip->lux_scale = 2000;
 
 	ret = regmap_write(chip->regmap, ISL29028_REG_TEST1_MODE, 0x0);
@@ -664,6 +698,7 @@ static const struct dev_pm_ops isl29028_pm_ops = {
 
 static const struct i2c_device_id isl29028_id[] = {
 	{"isl29028", 0},
+	{"isl29030", 0},
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, isl29028_id);
@@ -671,6 +706,7 @@ MODULE_DEVICE_TABLE(i2c, isl29028_id);
 static const struct of_device_id isl29028_of_match[] = {
 	{ .compatible = "isl,isl29028", }, /* for backward compat., don't use */
 	{ .compatible = "isil,isl29028", },
+	{ .compatible = "isil,isl29030", },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, isl29028_of_match);
diff --git a/drivers/iio/light/rpr0521.c b/drivers/iio/light/rpr0521.c
index 7de0f397194b..9d0c2e859bb2 100644
--- a/drivers/iio/light/rpr0521.c
+++ b/drivers/iio/light/rpr0521.c
@@ -9,7 +9,7 @@
  *
  * IIO driver for RPR-0521RS (7-bit I2C slave address 0x38).
  *
- * TODO: illuminance channel, PM support, buffer
+ * TODO: illuminance channel, buffer
  */
 
 #include <linux/module.h>
@@ -30,6 +30,7 @@
 #define RPR0521_REG_PXS_DATA		0x44 /* 16-bit, little endian */
 #define RPR0521_REG_ALS_DATA0		0x46 /* 16-bit, little endian */
 #define RPR0521_REG_ALS_DATA1		0x48 /* 16-bit, little endian */
+#define RPR0521_REG_PS_OFFSET_LSB	0x53
 #define RPR0521_REG_ID			0x92
 
 #define RPR0521_MODE_ALS_MASK		BIT(7)
@@ -77,9 +78,9 @@ static const struct rpr0521_gain rpr0521_pxs_gain[3] = {
 };
 
 enum rpr0521_channel {
+	RPR0521_CHAN_PXS,
 	RPR0521_CHAN_ALS_DATA0,
 	RPR0521_CHAN_ALS_DATA1,
-	RPR0521_CHAN_PXS,
 };
 
 struct rpr0521_reg_desc {
@@ -88,6 +89,10 @@ struct rpr0521_reg_desc {
 };
 
 static const struct rpr0521_reg_desc rpr0521_data_reg[] = {
+	[RPR0521_CHAN_PXS]	= {
+		.address	= RPR0521_REG_PXS_DATA,
+		.device_mask	= RPR0521_MODE_PXS_MASK,
+	},
 	[RPR0521_CHAN_ALS_DATA0] = {
 		.address	= RPR0521_REG_ALS_DATA0,
 		.device_mask	= RPR0521_MODE_ALS_MASK,
@@ -96,10 +101,6 @@ static const struct rpr0521_reg_desc rpr0521_data_reg[] = {
 		.address	= RPR0521_REG_ALS_DATA1,
 		.device_mask	= RPR0521_MODE_ALS_MASK,
 	},
-	[RPR0521_CHAN_PXS]	= {
-		.address	= RPR0521_REG_PXS_DATA,
-		.device_mask	= RPR0521_MODE_PXS_MASK,
-	},
 };
 
 static const struct rpr0521_gain_info {
@@ -109,6 +110,13 @@ static const struct rpr0521_gain_info {
 	const struct rpr0521_gain *gain;
 	int size;
 } rpr0521_gain[] = {
+	[RPR0521_CHAN_PXS] = {
+		.reg	= RPR0521_REG_PXS_CTRL,
+		.mask	= RPR0521_PXS_GAIN_MASK,
+		.shift	= RPR0521_PXS_GAIN_SHIFT,
+		.gain	= rpr0521_pxs_gain,
+		.size	= ARRAY_SIZE(rpr0521_pxs_gain),
+	},
 	[RPR0521_CHAN_ALS_DATA0] = {
 		.reg	= RPR0521_REG_ALS_CTRL,
 		.mask	= RPR0521_ALS_DATA0_GAIN_MASK,
@@ -123,13 +131,30 @@ static const struct rpr0521_gain_info {
 		.gain	= rpr0521_als_gain,
 		.size	= ARRAY_SIZE(rpr0521_als_gain),
 	},
-	[RPR0521_CHAN_PXS] = {
-		.reg	= RPR0521_REG_PXS_CTRL,
-		.mask	= RPR0521_PXS_GAIN_MASK,
-		.shift	= RPR0521_PXS_GAIN_SHIFT,
-		.gain	= rpr0521_pxs_gain,
-		.size	= ARRAY_SIZE(rpr0521_pxs_gain),
-	},
+};
+
+struct rpr0521_samp_freq {
+	int	als_hz;
+	int	als_uhz;
+	int	pxs_hz;
+	int	pxs_uhz;
+};
+
+static const struct rpr0521_samp_freq rpr0521_samp_freq_i[13] = {
+/*	{ALS, PXS},		   W==currently writable option */
+	{0, 0, 0, 0},		/* W0000, 0=standby */
+	{0, 0, 100, 0},		/*  0001 */
+	{0, 0, 25, 0},		/*  0010 */
+	{0, 0, 10, 0},		/*  0011 */
+	{0, 0, 2, 500000},	/*  0100 */
+	{10, 0, 20, 0},		/*  0101 */
+	{10, 0, 10, 0},		/* W0110 */
+	{10, 0, 2, 500000},	/*  0111 */
+	{2, 500000, 20, 0},	/*  1000, measurement 100ms, sleep 300ms */
+	{2, 500000, 10, 0},	/*  1001, measurement 100ms, sleep 300ms */
+	{2, 500000, 0, 0},	/*  1010, high sensitivity mode */
+	{2, 500000, 2, 500000},	/* W1011, high sensitivity mode */
+	{20, 0, 20, 0}	/* 1100, ALS_data x 0.5, see specification P.18 */
 };
 
 struct rpr0521_data {
@@ -142,9 +167,11 @@ struct rpr0521_data {
 	bool als_dev_en;
 	bool pxs_dev_en;
 
-	/* optimize runtime pm ops - enable device only if needed */
+	/* optimize runtime pm ops - enable/disable device only if needed */
 	bool als_ps_need_en;
 	bool pxs_ps_need_en;
+	bool als_need_dis;
+	bool pxs_need_dis;
 
 	struct regmap *regmap;
 };
@@ -152,9 +179,16 @@ struct rpr0521_data {
 static IIO_CONST_ATTR(in_intensity_scale_available, RPR0521_ALS_SCALE_AVAIL);
 static IIO_CONST_ATTR(in_proximity_scale_available, RPR0521_PXS_SCALE_AVAIL);
 
+/*
+ * Start with easy freq first, whole table of freq combinations is more
+ * complicated.
+ */
+static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("2.5 10");
+
 static struct attribute *rpr0521_attributes[] = {
 	&iio_const_attr_in_intensity_scale_available.dev_attr.attr,
 	&iio_const_attr_in_proximity_scale_available.dev_attr.attr,
+	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
 	NULL,
 };
 
@@ -164,12 +198,21 @@ static const struct attribute_group rpr0521_attribute_group = {
 
 static const struct iio_chan_spec rpr0521_channels[] = {
 	{
+		.type = IIO_PROXIMITY,
+		.address = RPR0521_CHAN_PXS,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_OFFSET) |
+			BIT(IIO_CHAN_INFO_SCALE),
+		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
+	},
+	{
 		.type = IIO_INTENSITY,
 		.modified = 1,
 		.address = RPR0521_CHAN_ALS_DATA0,
 		.channel2 = IIO_MOD_LIGHT_BOTH,
 		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
 			BIT(IIO_CHAN_INFO_SCALE),
+		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
 	},
 	{
 		.type = IIO_INTENSITY,
@@ -178,13 +221,8 @@ static const struct iio_chan_spec rpr0521_channels[] = {
 		.channel2 = IIO_MOD_LIGHT_IR,
 		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
 			BIT(IIO_CHAN_INFO_SCALE),
+		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
 	},
-	{
-		.type = IIO_PROXIMITY,
-		.address = RPR0521_CHAN_PXS,
-		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
-			BIT(IIO_CHAN_INFO_SCALE),
-	}
 };
 
 static int rpr0521_als_enable(struct rpr0521_data *data, u8 status)
@@ -197,7 +235,10 @@ static int rpr0521_als_enable(struct rpr0521_data *data, u8 status)
 	if (ret < 0)
 		return ret;
 
-	data->als_dev_en = true;
+	if (status & RPR0521_MODE_ALS_MASK)
+		data->als_dev_en = true;
+	else
+		data->als_dev_en = false;
 
 	return 0;
 }
@@ -212,7 +253,10 @@ static int rpr0521_pxs_enable(struct rpr0521_data *data, u8 status)
 	if (ret < 0)
 		return ret;
 
-	data->pxs_dev_en = true;
+	if (status & RPR0521_MODE_PXS_MASK)
+		data->pxs_dev_en = true;
+	else
+		data->pxs_dev_en = false;
 
 	return 0;
 }
@@ -224,40 +268,32 @@ static int rpr0521_pxs_enable(struct rpr0521_data *data, u8 status)
  * @on: state to be set for devices in @device_mask
  * @device_mask: bitmask specifying for which device we need to update @on state
  *
- * We rely on rpr0521_runtime_resume to enable our @device_mask devices, but
- * if (for example) PXS was enabled (pxs_dev_en = true) by a previous call to
- * rpr0521_runtime_resume and we want to enable ALS we MUST set ALS enable
- * bit of RPR0521_REG_MODE_CTRL here because rpr0521_runtime_resume will not
- * be called twice.
+ * Calls for this function must be balanced so that each ON should have matching
+ * OFF. Otherwise pm usage_count gets out of sync.
  */
 static int rpr0521_set_power_state(struct rpr0521_data *data, bool on,
 				   u8 device_mask)
 {
 #ifdef CONFIG_PM
 	int ret;
-	u8 update_mask = 0;
 
 	if (device_mask & RPR0521_MODE_ALS_MASK) {
-		if (on && !data->als_ps_need_en && data->pxs_dev_en)
-			update_mask |= RPR0521_MODE_ALS_MASK;
-		else
-			data->als_ps_need_en = on;
+		data->als_ps_need_en = on;
+		data->als_need_dis = !on;
 	}
 
 	if (device_mask & RPR0521_MODE_PXS_MASK) {
-		if (on && !data->pxs_ps_need_en && data->als_dev_en)
-			update_mask |= RPR0521_MODE_PXS_MASK;
-		else
-			data->pxs_ps_need_en = on;
-	}
-
-	if (update_mask) {
-		ret = regmap_update_bits(data->regmap, RPR0521_REG_MODE_CTRL,
-					 update_mask, update_mask);
-		if (ret < 0)
-			return ret;
+		data->pxs_ps_need_en = on;
+		data->pxs_need_dis = !on;
 	}
 
+	/*
+	 * On: _resume() is called only when we are suspended
+	 * Off: _suspend() is called after delay if _resume() is not
+	 * called before that.
+	 * Note: If either measurement is re-enabled before _suspend(),
+	 * both stay enabled until _suspend().
+	 */
 	if (on) {
 		ret = pm_runtime_get_sync(&data->client->dev);
 	} else {
@@ -273,6 +309,23 @@ static int rpr0521_set_power_state(struct rpr0521_data *data, bool on,
 
 		return ret;
 	}
+
+	if (on) {
+		/* If _resume() was not called, enable measurement now. */
+		if (data->als_ps_need_en) {
+			ret = rpr0521_als_enable(data, RPR0521_MODE_ALS_ENABLE);
+			if (ret)
+				return ret;
+			data->als_ps_need_en = false;
+		}
+
+		if (data->pxs_ps_need_en) {
+			ret = rpr0521_pxs_enable(data, RPR0521_MODE_PXS_ENABLE);
+			if (ret)
+				return ret;
+			data->pxs_ps_need_en = false;
+		}
+	}
 #endif
 	return 0;
 }
@@ -314,6 +367,106 @@ static int rpr0521_set_gain(struct rpr0521_data *data, int chan,
 				  idx << rpr0521_gain[chan].shift);
 }
 
+static int rpr0521_read_samp_freq(struct rpr0521_data *data,
+				enum iio_chan_type chan_type,
+			    int *val, int *val2)
+{
+	int reg, ret;
+
+	ret = regmap_read(data->regmap, RPR0521_REG_MODE_CTRL, &reg);
+	if (ret < 0)
+		return ret;
+
+	reg &= RPR0521_MODE_MEAS_TIME_MASK;
+	if (reg >= ARRAY_SIZE(rpr0521_samp_freq_i))
+		return -EINVAL;
+
+	switch (chan_type) {
+	case IIO_INTENSITY:
+		*val = rpr0521_samp_freq_i[reg].als_hz;
+		*val2 = rpr0521_samp_freq_i[reg].als_uhz;
+		return 0;
+
+	case IIO_PROXIMITY:
+		*val = rpr0521_samp_freq_i[reg].pxs_hz;
+		*val2 = rpr0521_samp_freq_i[reg].pxs_uhz;
+		return 0;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int rpr0521_write_samp_freq_common(struct rpr0521_data *data,
+				enum iio_chan_type chan_type,
+				int val, int val2)
+{
+	int i;
+
+	/*
+	 * Ignore channel
+	 * both pxs and als are setup only to same freq because of simplicity
+	 */
+	switch (val) {
+	case 0:
+		i = 0;
+		break;
+
+	case 2:
+		if (val2 != 500000)
+			return -EINVAL;
+
+		i = 11;
+		break;
+
+	case 10:
+		i = 6;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return regmap_update_bits(data->regmap,
+		RPR0521_REG_MODE_CTRL,
+		RPR0521_MODE_MEAS_TIME_MASK,
+		i);
+}
+
+static int rpr0521_read_ps_offset(struct rpr0521_data *data, int *offset)
+{
+	int ret;
+	__le16 buffer;
+
+	ret = regmap_bulk_read(data->regmap,
+		RPR0521_REG_PS_OFFSET_LSB, &buffer, sizeof(buffer));
+
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Failed to read PS OFFSET register\n");
+		return ret;
+	}
+	*offset = le16_to_cpu(buffer);
+
+	return ret;
+}
+
+static int rpr0521_write_ps_offset(struct rpr0521_data *data, int offset)
+{
+	int ret;
+	__le16 buffer;
+
+	buffer = cpu_to_le16(offset & 0x3ff);
+	ret = regmap_raw_write(data->regmap,
+		RPR0521_REG_PS_OFFSET_LSB, &buffer, sizeof(buffer));
+
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Failed to write PS OFFSET register\n");
+		return ret;
+	}
+
+	return ret;
+}
+
 static int rpr0521_read_raw(struct iio_dev *indio_dev,
 			    struct iio_chan_spec const *chan, int *val,
 			    int *val2, long mask)
@@ -339,7 +492,7 @@ static int rpr0521_read_raw(struct iio_dev *indio_dev,
 
 		ret = regmap_bulk_read(data->regmap,
 				       rpr0521_data_reg[chan->address].address,
-				       &raw_data, 2);
+				       &raw_data, sizeof(raw_data));
 		if (ret < 0) {
 			rpr0521_set_power_state(data, false, device_mask);
 			mutex_unlock(&data->lock);
@@ -354,6 +507,7 @@ static int rpr0521_read_raw(struct iio_dev *indio_dev,
 		*val = le16_to_cpu(raw_data);
 
 		return IIO_VAL_INT;
+
 	case IIO_CHAN_INFO_SCALE:
 		mutex_lock(&data->lock);
 		ret = rpr0521_get_gain(data, chan->address, val, val2);
@@ -362,6 +516,25 @@ static int rpr0521_read_raw(struct iio_dev *indio_dev,
 			return ret;
 
 		return IIO_VAL_INT_PLUS_MICRO;
+
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		mutex_lock(&data->lock);
+		ret = rpr0521_read_samp_freq(data, chan->type, val, val2);
+		mutex_unlock(&data->lock);
+		if (ret < 0)
+			return ret;
+
+		return IIO_VAL_INT_PLUS_MICRO;
+
+	case IIO_CHAN_INFO_OFFSET:
+		mutex_lock(&data->lock);
+		ret = rpr0521_read_ps_offset(data, val);
+		mutex_unlock(&data->lock);
+		if (ret < 0)
+			return ret;
+
+		return IIO_VAL_INT;
+
 	default:
 		return -EINVAL;
 	}
@@ -381,6 +554,22 @@ static int rpr0521_write_raw(struct iio_dev *indio_dev,
 		mutex_unlock(&data->lock);
 
 		return ret;
+
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		mutex_lock(&data->lock);
+		ret = rpr0521_write_samp_freq_common(data, chan->type,
+						     val, val2);
+		mutex_unlock(&data->lock);
+
+		return ret;
+
+	case IIO_CHAN_INFO_OFFSET:
+		mutex_lock(&data->lock);
+		ret = rpr0521_write_ps_offset(data, val);
+		mutex_unlock(&data->lock);
+
+		return ret;
+
 	default:
 		return -EINVAL;
 	}
@@ -419,12 +608,14 @@ static int rpr0521_init(struct rpr0521_data *data)
 		return ret;
 	}
 
+#ifndef CONFIG_PM
 	ret = rpr0521_als_enable(data, RPR0521_MODE_ALS_ENABLE);
 	if (ret < 0)
 		return ret;
 	ret = rpr0521_pxs_enable(data, RPR0521_MODE_PXS_ENABLE);
 	if (ret < 0)
 		return ret;
+#endif
 
 	return 0;
 }
@@ -510,13 +701,26 @@ static int rpr0521_probe(struct i2c_client *client,
 
 	ret = pm_runtime_set_active(&client->dev);
 	if (ret < 0)
-		return ret;
+		goto err_poweroff;
 
 	pm_runtime_enable(&client->dev);
 	pm_runtime_set_autosuspend_delay(&client->dev, RPR0521_SLEEP_DELAY_MS);
 	pm_runtime_use_autosuspend(&client->dev);
 
-	return iio_device_register(indio_dev);
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto err_pm_disable;
+
+	return 0;
+
+err_pm_disable:
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+err_poweroff:
+	rpr0521_poweroff(data);
+
+	return ret;
 }
 
 static int rpr0521_remove(struct i2c_client *client)
@@ -541,9 +745,16 @@ static int rpr0521_runtime_suspend(struct device *dev)
 	struct rpr0521_data *data = iio_priv(indio_dev);
 	int ret;
 
-	/* disable channels and sets {als,pxs}_dev_en to false */
 	mutex_lock(&data->lock);
+	/* If measurements are enabled, enable them on resume */
+	if (!data->als_need_dis)
+		data->als_ps_need_en = data->als_dev_en;
+	if (!data->pxs_need_dis)
+		data->pxs_ps_need_en = data->pxs_dev_en;
+
+	/* disable channels and sets {als,pxs}_dev_en to false */
 	ret = rpr0521_poweroff(data);
+	regcache_mark_dirty(data->regmap);
 	mutex_unlock(&data->lock);
 
 	return ret;
@@ -555,6 +766,7 @@ static int rpr0521_runtime_resume(struct device *dev)
 	struct rpr0521_data *data = iio_priv(indio_dev);
 	int ret;
 
+	regcache_sync(data->regmap);
 	if (data->als_ps_need_en) {
 		ret = rpr0521_als_enable(data, RPR0521_MODE_ALS_ENABLE);
 		if (ret < 0)
@@ -568,6 +780,7 @@ static int rpr0521_runtime_resume(struct device *dev)
 			return ret;
 		data->pxs_ps_need_en = false;
 	}
+	msleep(100);	//wait for first measurement result
 
 	return 0;
 }
diff --git a/drivers/iio/light/tsl2583.c b/drivers/iio/light/tsl2583.c
index a78b6025c465..1679181d2bdd 100644
--- a/drivers/iio/light/tsl2583.c
+++ b/drivers/iio/light/tsl2583.c
@@ -3,7 +3,7 @@
  * within the TAOS tsl258x family of devices (tsl2580, tsl2581, tsl2583).
  *
  * Copyright (c) 2011, TAOS Corporation.
- * Copyright (c) 2016 Brian Masney <masneyb@onstation.org>
+ * Copyright (c) 2016-2017 Brian Masney <masneyb@onstation.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
@@ -27,6 +27,7 @@
 #include <linux/module.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
+#include <linux/pm_runtime.h>
 
 /* Device Registers and Masks */
 #define TSL2583_CNTRL			0x00
@@ -64,6 +65,8 @@
 #define TSL2583_CHIP_ID			0x90
 #define TSL2583_CHIP_ID_MASK		0xf0
 
+#define TSL2583_POWER_OFF_DELAY_MS	2000
+
 /* Per-device data */
 struct tsl2583_als_info {
 	u16 als_ch0;
@@ -108,7 +111,6 @@ struct tsl2583_chip {
 	struct tsl2583_settings als_settings;
 	int als_time_scale;
 	int als_saturation;
-	bool suspended;
 };
 
 struct gainadj {
@@ -460,8 +462,6 @@ static int tsl2583_chip_init_and_power_on(struct iio_dev *indio_dev)
 	if (ret < 0)
 		return ret;
 
-	chip->suspended = false;
-
 	return ret;
 }
 
@@ -513,11 +513,6 @@ static ssize_t in_illuminance_calibrate_store(struct device *dev,
 
 	mutex_lock(&chip->als_mutex);
 
-	if (chip->suspended) {
-		ret = -EBUSY;
-		goto done;
-	}
-
 	ret = tsl2583_als_calibrate(indio_dev);
 	if (ret < 0)
 		goto done;
@@ -645,20 +640,36 @@ static const struct iio_chan_spec tsl2583_channels[] = {
 	},
 };
 
+static int tsl2583_set_pm_runtime_busy(struct tsl2583_chip *chip, bool on)
+{
+	int ret;
+
+	if (on) {
+		ret = pm_runtime_get_sync(&chip->client->dev);
+		if (ret < 0)
+			pm_runtime_put_noidle(&chip->client->dev);
+	} else {
+		pm_runtime_mark_last_busy(&chip->client->dev);
+		ret = pm_runtime_put_autosuspend(&chip->client->dev);
+	}
+
+	return ret;
+}
+
 static int tsl2583_read_raw(struct iio_dev *indio_dev,
 			    struct iio_chan_spec const *chan,
 			    int *val, int *val2, long mask)
 {
 	struct tsl2583_chip *chip = iio_priv(indio_dev);
-	int ret = -EINVAL;
+	int ret, pm_ret;
 
-	mutex_lock(&chip->als_mutex);
+	ret = tsl2583_set_pm_runtime_busy(chip, true);
+	if (ret < 0)
+		return ret;
 
-	if (chip->suspended) {
-		ret = -EBUSY;
-		goto read_done;
-	}
+	mutex_lock(&chip->als_mutex);
 
+	ret = -EINVAL;
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
 		if (chan->type == IIO_LIGHT) {
@@ -719,6 +730,18 @@ static int tsl2583_read_raw(struct iio_dev *indio_dev,
 read_done:
 	mutex_unlock(&chip->als_mutex);
 
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * Preserve the ret variable if the call to
+	 * tsl2583_set_pm_runtime_busy() is successful so the reading
+	 * (if applicable) is returned to user space.
+	 */
+	pm_ret = tsl2583_set_pm_runtime_busy(chip, false);
+	if (pm_ret < 0)
+		return pm_ret;
+
 	return ret;
 }
 
@@ -727,15 +750,15 @@ static int tsl2583_write_raw(struct iio_dev *indio_dev,
 			     int val, int val2, long mask)
 {
 	struct tsl2583_chip *chip = iio_priv(indio_dev);
-	int ret = -EINVAL;
+	int ret;
 
-	mutex_lock(&chip->als_mutex);
+	ret = tsl2583_set_pm_runtime_busy(chip, true);
+	if (ret < 0)
+		return ret;
 
-	if (chip->suspended) {
-		ret = -EBUSY;
-		goto write_done;
-	}
+	mutex_lock(&chip->als_mutex);
 
+	ret = -EINVAL;
 	switch (mask) {
 	case IIO_CHAN_INFO_CALIBBIAS:
 		if (chan->type == IIO_LIGHT) {
@@ -767,9 +790,15 @@ static int tsl2583_write_raw(struct iio_dev *indio_dev,
 		break;
 	}
 
-write_done:
 	mutex_unlock(&chip->als_mutex);
 
+	if (ret < 0)
+		return ret;
+
+	ret = tsl2583_set_pm_runtime_busy(chip, false);
+	if (ret < 0)
+		return ret;
+
 	return ret;
 }
 
@@ -803,7 +832,6 @@ static int tsl2583_probe(struct i2c_client *clientp,
 	i2c_set_clientdata(clientp, indio_dev);
 
 	mutex_init(&chip->als_mutex);
-	chip->suspended = true;
 
 	ret = i2c_smbus_read_byte_data(clientp,
 				       TSL2583_CMD_REG | TSL2583_CHIPID);
@@ -826,6 +854,11 @@ static int tsl2583_probe(struct i2c_client *clientp,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->name = chip->client->name;
 
+	pm_runtime_enable(&clientp->dev);
+	pm_runtime_set_autosuspend_delay(&clientp->dev,
+					 TSL2583_POWER_OFF_DELAY_MS);
+	pm_runtime_use_autosuspend(&clientp->dev);
+
 	ret = devm_iio_device_register(indio_dev->dev.parent, indio_dev);
 	if (ret) {
 		dev_err(&clientp->dev, "%s: iio registration failed\n",
@@ -836,16 +869,25 @@ static int tsl2583_probe(struct i2c_client *clientp,
 	/* Load up the V2 defaults (these are hard coded defaults for now) */
 	tsl2583_defaults(chip);
 
-	/* Make sure the chip is on */
-	ret = tsl2583_chip_init_and_power_on(indio_dev);
-	if (ret < 0)
-		return ret;
-
 	dev_info(&clientp->dev, "Light sensor found.\n");
 
 	return 0;
 }
 
+static int tsl2583_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct tsl2583_chip *chip = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+
+	return tsl2583_set_power_state(chip, TSL2583_CNTL_PWR_OFF);
+}
+
 static int __maybe_unused tsl2583_suspend(struct device *dev)
 {
 	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
@@ -855,7 +897,6 @@ static int __maybe_unused tsl2583_suspend(struct device *dev)
 	mutex_lock(&chip->als_mutex);
 
 	ret = tsl2583_set_power_state(chip, TSL2583_CNTL_PWR_OFF);
-	chip->suspended = true;
 
 	mutex_unlock(&chip->als_mutex);
 
@@ -877,7 +918,11 @@ static int __maybe_unused tsl2583_resume(struct device *dev)
 	return ret;
 }
 
-static SIMPLE_DEV_PM_OPS(tsl2583_pm_ops, tsl2583_suspend, tsl2583_resume);
+static const struct dev_pm_ops tsl2583_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(tsl2583_suspend, tsl2583_resume, NULL)
+};
 
 static struct i2c_device_id tsl2583_idtable[] = {
 	{ "tsl2580", 0 },
@@ -904,6 +949,7 @@ static struct i2c_driver tsl2583_driver = {
 	},
 	.id_table = tsl2583_idtable,
 	.probe = tsl2583_probe,
+	.remove = tsl2583_remove,
 };
 module_i2c_driver(tsl2583_driver);
 
diff --git a/drivers/iio/magnetometer/st_magn_spi.c b/drivers/iio/magnetometer/st_magn_spi.c
index 6325e7dc8e03..f3cb4dc05391 100644
--- a/drivers/iio/magnetometer/st_magn_spi.c
+++ b/drivers/iio/magnetometer/st_magn_spi.c
@@ -48,8 +48,6 @@ static int st_magn_spi_remove(struct spi_device *spi)
 }
 
 static const struct spi_device_id st_magn_id_table[] = {
-	{ LSM303DLHC_MAGN_DEV_NAME },
-	{ LSM303DLM_MAGN_DEV_NAME },
 	{ LIS3MDL_MAGN_DEV_NAME },
 	{ LSM303AGR_MAGN_DEV_NAME },
 	{},
diff --git a/drivers/iio/multiplexer/Kconfig b/drivers/iio/multiplexer/Kconfig
new file mode 100644
index 000000000000..735a7b0e6fd8
--- /dev/null
+++ b/drivers/iio/multiplexer/Kconfig
@@ -0,0 +1,18 @@
+#
+# Multiplexer drivers
+#
+# When adding new entries keep the list in alphabetical order
+
+menu "Multiplexers"
+
+config IIO_MUX
+	tristate "IIO multiplexer driver"
+	select MULTIPLEXER
+	depends on OF || COMPILE_TEST
+	help
+	  Say yes here to build support for the IIO multiplexer.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called iio-mux.
+
+endmenu
diff --git a/drivers/iio/multiplexer/Makefile b/drivers/iio/multiplexer/Makefile
new file mode 100644
index 000000000000..68be3c4abd07
--- /dev/null
+++ b/drivers/iio/multiplexer/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for industrial I/O multiplexer drivers
+#
+
+# When adding new entries keep the list in alphabetical order
+obj-$(CONFIG_IIO_MUX) += iio-mux.o
diff --git a/drivers/iio/multiplexer/iio-mux.c b/drivers/iio/multiplexer/iio-mux.c
new file mode 100644
index 000000000000..37ba007f8dca
--- /dev/null
+++ b/drivers/iio/multiplexer/iio-mux.c
@@ -0,0 +1,459 @@
+/*
+ * IIO multiplexer driver
+ *
+ * Copyright (C) 2017 Axentia Technologies AB
+ *
+ * Author: Peter Rosin <peda@axentia.se>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/iio/consumer.h>
+#include <linux/iio/iio.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/mux/consumer.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+
+struct mux_ext_info_cache {
+	char *data;
+	ssize_t size;
+};
+
+struct mux_child {
+	struct mux_ext_info_cache *ext_info_cache;
+};
+
+struct mux {
+	int cached_state;
+	struct mux_control *control;
+	struct iio_channel *parent;
+	struct iio_dev *indio_dev;
+	struct iio_chan_spec *chan;
+	struct iio_chan_spec_ext_info *ext_info;
+	struct mux_child *child;
+};
+
+static int iio_mux_select(struct mux *mux, int idx)
+{
+	struct mux_child *child = &mux->child[idx];
+	struct iio_chan_spec const *chan = &mux->chan[idx];
+	int ret;
+	int i;
+
+	ret = mux_control_select(mux->control, chan->channel);
+	if (ret < 0) {
+		mux->cached_state = -1;
+		return ret;
+	}
+
+	if (mux->cached_state == chan->channel)
+		return 0;
+
+	if (chan->ext_info) {
+		for (i = 0; chan->ext_info[i].name; ++i) {
+			const char *attr = chan->ext_info[i].name;
+			struct mux_ext_info_cache *cache;
+
+			cache = &child->ext_info_cache[i];
+
+			if (cache->size < 0)
+				continue;
+
+			ret = iio_write_channel_ext_info(mux->parent, attr,
+							 cache->data,
+							 cache->size);
+
+			if (ret < 0) {
+				mux_control_deselect(mux->control);
+				mux->cached_state = -1;
+				return ret;
+			}
+		}
+	}
+	mux->cached_state = chan->channel;
+
+	return 0;
+}
+
+static void iio_mux_deselect(struct mux *mux)
+{
+	mux_control_deselect(mux->control);
+}
+
+static int mux_read_raw(struct iio_dev *indio_dev,
+			struct iio_chan_spec const *chan,
+			int *val, int *val2, long mask)
+{
+	struct mux *mux = iio_priv(indio_dev);
+	int idx = chan - mux->chan;
+	int ret;
+
+	ret = iio_mux_select(mux, idx);
+	if (ret < 0)
+		return ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = iio_read_channel_raw(mux->parent, val);
+		break;
+
+	case IIO_CHAN_INFO_SCALE:
+		ret = iio_read_channel_scale(mux->parent, val, val2);
+		break;
+
+	default:
+		ret = -EINVAL;
+	}
+
+	iio_mux_deselect(mux);
+
+	return ret;
+}
+
+static int mux_read_avail(struct iio_dev *indio_dev,
+			  struct iio_chan_spec const *chan,
+			  const int **vals, int *type, int *length,
+			  long mask)
+{
+	struct mux *mux = iio_priv(indio_dev);
+	int idx = chan - mux->chan;
+	int ret;
+
+	ret = iio_mux_select(mux, idx);
+	if (ret < 0)
+		return ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		*type = IIO_VAL_INT;
+		ret = iio_read_avail_channel_raw(mux->parent, vals, length);
+		break;
+
+	default:
+		ret = -EINVAL;
+	}
+
+	iio_mux_deselect(mux);
+
+	return ret;
+}
+
+static int mux_write_raw(struct iio_dev *indio_dev,
+			 struct iio_chan_spec const *chan,
+			 int val, int val2, long mask)
+{
+	struct mux *mux = iio_priv(indio_dev);
+	int idx = chan - mux->chan;
+	int ret;
+
+	ret = iio_mux_select(mux, idx);
+	if (ret < 0)
+		return ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = iio_write_channel_raw(mux->parent, val);
+		break;
+
+	default:
+		ret = -EINVAL;
+	}
+
+	iio_mux_deselect(mux);
+
+	return ret;
+}
+
+static const struct iio_info mux_info = {
+	.read_raw = mux_read_raw,
+	.read_avail = mux_read_avail,
+	.write_raw = mux_write_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static ssize_t mux_read_ext_info(struct iio_dev *indio_dev, uintptr_t private,
+				 struct iio_chan_spec const *chan, char *buf)
+{
+	struct mux *mux = iio_priv(indio_dev);
+	int idx = chan - mux->chan;
+	ssize_t ret;
+
+	ret = iio_mux_select(mux, idx);
+	if (ret < 0)
+		return ret;
+
+	ret = iio_read_channel_ext_info(mux->parent,
+					mux->ext_info[private].name,
+					buf);
+
+	iio_mux_deselect(mux);
+
+	return ret;
+}
+
+static ssize_t mux_write_ext_info(struct iio_dev *indio_dev, uintptr_t private,
+				  struct iio_chan_spec const *chan,
+				  const char *buf, size_t len)
+{
+	struct device *dev = indio_dev->dev.parent;
+	struct mux *mux = iio_priv(indio_dev);
+	int idx = chan - mux->chan;
+	char *new;
+	ssize_t ret;
+
+	if (len >= PAGE_SIZE)
+		return -EINVAL;
+
+	ret = iio_mux_select(mux, idx);
+	if (ret < 0)
+		return ret;
+
+	new = devm_kmemdup(dev, buf, len + 1, GFP_KERNEL);
+	if (!new) {
+		iio_mux_deselect(mux);
+		return -ENOMEM;
+	}
+
+	new[len] = 0;
+
+	ret = iio_write_channel_ext_info(mux->parent,
+					 mux->ext_info[private].name,
+					 buf, len);
+	if (ret < 0) {
+		iio_mux_deselect(mux);
+		devm_kfree(dev, new);
+		return ret;
+	}
+
+	devm_kfree(dev, mux->child[idx].ext_info_cache[private].data);
+	mux->child[idx].ext_info_cache[private].data = new;
+	mux->child[idx].ext_info_cache[private].size = len;
+
+	iio_mux_deselect(mux);
+
+	return ret;
+}
+
+static int mux_configure_channel(struct device *dev, struct mux *mux,
+				 u32 state, const char *label, int idx)
+{
+	struct mux_child *child = &mux->child[idx];
+	struct iio_chan_spec *chan = &mux->chan[idx];
+	struct iio_chan_spec const *pchan = mux->parent->channel;
+	char *page = NULL;
+	int num_ext_info;
+	int i;
+	int ret;
+
+	chan->indexed = 1;
+	chan->output = pchan->output;
+	chan->datasheet_name = label;
+	chan->ext_info = mux->ext_info;
+
+	ret = iio_get_channel_type(mux->parent, &chan->type);
+	if (ret < 0) {
+		dev_err(dev, "failed to get parent channel type\n");
+		return ret;
+	}
+
+	if (iio_channel_has_info(pchan, IIO_CHAN_INFO_RAW))
+		chan->info_mask_separate |= BIT(IIO_CHAN_INFO_RAW);
+	if (iio_channel_has_info(pchan, IIO_CHAN_INFO_SCALE))
+		chan->info_mask_separate |= BIT(IIO_CHAN_INFO_SCALE);
+
+	if (iio_channel_has_available(pchan, IIO_CHAN_INFO_RAW))
+		chan->info_mask_separate_available |= BIT(IIO_CHAN_INFO_RAW);
+
+	if (state >= mux_control_states(mux->control)) {
+		dev_err(dev, "too many channels\n");
+		return -EINVAL;
+	}
+
+	chan->channel = state;
+
+	num_ext_info = iio_get_channel_ext_info_count(mux->parent);
+	if (num_ext_info) {
+		page = devm_kzalloc(dev, PAGE_SIZE, GFP_KERNEL);
+		if (!page)
+			return -ENOMEM;
+	}
+	child->ext_info_cache = devm_kzalloc(dev,
+					     sizeof(*child->ext_info_cache) *
+					     num_ext_info, GFP_KERNEL);
+	for (i = 0; i < num_ext_info; ++i) {
+		child->ext_info_cache[i].size = -1;
+
+		if (!pchan->ext_info[i].write)
+			continue;
+		if (!pchan->ext_info[i].read)
+			continue;
+
+		ret = iio_read_channel_ext_info(mux->parent,
+						mux->ext_info[i].name,
+						page);
+		if (ret < 0) {
+			dev_err(dev, "failed to get ext_info '%s'\n",
+				pchan->ext_info[i].name);
+			return ret;
+		}
+		if (ret >= PAGE_SIZE) {
+			dev_err(dev, "too large ext_info '%s'\n",
+				pchan->ext_info[i].name);
+			return -EINVAL;
+		}
+
+		child->ext_info_cache[i].data = devm_kmemdup(dev, page, ret + 1,
+							     GFP_KERNEL);
+		child->ext_info_cache[i].data[ret] = 0;
+		child->ext_info_cache[i].size = ret;
+	}
+
+	if (page)
+		devm_kfree(dev, page);
+
+	return 0;
+}
+
+/*
+ * Same as of_property_for_each_string(), but also keeps track of the
+ * index of each string.
+ */
+#define of_property_for_each_string_index(np, propname, prop, s, i)	\
+	for (prop = of_find_property(np, propname, NULL),		\
+	     s = of_prop_next_string(prop, NULL),			\
+	     i = 0;							\
+	     s;								\
+	     s = of_prop_next_string(prop, s),				\
+	     i++)
+
+static int mux_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = pdev->dev.of_node;
+	struct iio_dev *indio_dev;
+	struct iio_channel *parent;
+	struct mux *mux;
+	struct property *prop;
+	const char *label;
+	u32 state;
+	int sizeof_ext_info;
+	int children;
+	int sizeof_priv;
+	int i;
+	int ret;
+
+	if (!np)
+		return -ENODEV;
+
+	parent = devm_iio_channel_get(dev, "parent");
+	if (IS_ERR(parent)) {
+		if (PTR_ERR(parent) != -EPROBE_DEFER)
+			dev_err(dev, "failed to get parent channel\n");
+		return PTR_ERR(parent);
+	}
+
+	sizeof_ext_info = iio_get_channel_ext_info_count(parent);
+	if (sizeof_ext_info) {
+		sizeof_ext_info += 1; /* one extra entry for the sentinel */
+		sizeof_ext_info *= sizeof(*mux->ext_info);
+	}
+
+	children = 0;
+	of_property_for_each_string(np, "channels", prop, label) {
+		if (*label)
+			children++;
+	}
+	if (children <= 0) {
+		dev_err(dev, "not even a single child\n");
+		return -EINVAL;
+	}
+
+	sizeof_priv = sizeof(*mux);
+	sizeof_priv += sizeof(*mux->child) * children;
+	sizeof_priv += sizeof(*mux->chan) * children;
+	sizeof_priv += sizeof_ext_info;
+
+	indio_dev = devm_iio_device_alloc(dev, sizeof_priv);
+	if (!indio_dev)
+		return -ENOMEM;
+
+	mux = iio_priv(indio_dev);
+	mux->child = (struct mux_child *)(mux + 1);
+	mux->chan = (struct iio_chan_spec *)(mux->child + children);
+
+	platform_set_drvdata(pdev, indio_dev);
+
+	mux->parent = parent;
+	mux->cached_state = -1;
+
+	indio_dev->name = dev_name(dev);
+	indio_dev->dev.parent = dev;
+	indio_dev->info = &mux_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = mux->chan;
+	indio_dev->num_channels = children;
+	if (sizeof_ext_info) {
+		mux->ext_info = devm_kmemdup(dev,
+					     parent->channel->ext_info,
+					     sizeof_ext_info, GFP_KERNEL);
+		if (!mux->ext_info)
+			return -ENOMEM;
+
+		for (i = 0; mux->ext_info[i].name; ++i) {
+			if (parent->channel->ext_info[i].read)
+				mux->ext_info[i].read = mux_read_ext_info;
+			if (parent->channel->ext_info[i].write)
+				mux->ext_info[i].write = mux_write_ext_info;
+			mux->ext_info[i].private = i;
+		}
+	}
+
+	mux->control = devm_mux_control_get(dev, NULL);
+	if (IS_ERR(mux->control)) {
+		if (PTR_ERR(mux->control) != -EPROBE_DEFER)
+			dev_err(dev, "failed to get control-mux\n");
+		return PTR_ERR(mux->control);
+	}
+
+	i = 0;
+	of_property_for_each_string_index(np, "channels", prop, label, state) {
+		if (!*label)
+			continue;
+
+		ret = mux_configure_channel(dev, mux, state, label, i++);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = devm_iio_device_register(dev, indio_dev);
+	if (ret) {
+		dev_err(dev, "failed to register iio device\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id mux_match[] = {
+	{ .compatible = "io-channel-mux" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mux_match);
+
+static struct platform_driver mux_driver = {
+	.probe = mux_probe,
+	.driver = {
+		.name = "iio-mux",
+		.of_match_table = mux_match,
+	},
+};
+module_platform_driver(mux_driver);
+
+MODULE_DESCRIPTION("IIO multiplexer driver");
+MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/orientation/hid-sensor-rotation.c b/drivers/iio/orientation/hid-sensor-rotation.c
index a97e802ca523..e9fa86c87db5 100644
--- a/drivers/iio/orientation/hid-sensor-rotation.c
+++ b/drivers/iio/orientation/hid-sensor-rotation.c
@@ -31,6 +31,10 @@ struct dev_rot_state {
 	struct hid_sensor_common common_attributes;
 	struct hid_sensor_hub_attribute_info quaternion;
 	u32 sampled_vals[4];
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
 };
 
 /* Channel definitions */
@@ -41,6 +45,8 @@ static const struct iio_chan_spec dev_rot_channels[] = {
 		.channel2 = IIO_MOD_QUATERNION,
 		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+					BIT(IIO_CHAN_INFO_OFFSET) |
+					BIT(IIO_CHAN_INFO_SCALE) |
 					BIT(IIO_CHAN_INFO_HYSTERESIS)
 	}
 };
@@ -80,6 +86,15 @@ static int dev_rot_read_raw(struct iio_dev *indio_dev,
 		} else
 			ret_type = -EINVAL;
 		break;
+	case IIO_CHAN_INFO_SCALE:
+		vals[0] = rot_state->scale_pre_decml;
+		vals[1] = rot_state->scale_post_decml;
+		return rot_state->scale_precision;
+
+	case IIO_CHAN_INFO_OFFSET:
+		*vals = rot_state->value_offset;
+		return IIO_VAL_INT;
+
 	case IIO_CHAN_INFO_SAMP_FREQ:
 		ret_type = hid_sensor_read_samp_freq_value(
 			&rot_state->common_attributes, &vals[0], &vals[1]);
@@ -199,6 +214,11 @@ static int dev_rot_parse_report(struct platform_device *pdev,
 	dev_dbg(&pdev->dev, "dev_rot: attrib size %d\n",
 				st->quaternion.size);
 
+	st->scale_precision = hid_sensor_format_scale(
+				hsdev->usage,
+				&st->quaternion,
+				&st->scale_pre_decml, &st->scale_post_decml);
+
 	/* Set Sensitivity field ids, when there is no individual modifier */
 	if (st->common_attributes.sensitivity.index < 0) {
 		sensor_hub_input_get_attribute_info(hsdev,
@@ -218,7 +238,7 @@ static int dev_rot_parse_report(struct platform_device *pdev,
 static int hid_dev_rot_probe(struct platform_device *pdev)
 {
 	int ret;
-	static char *name = "dev_rotation";
+	static char *name;
 	struct iio_dev *indio_dev;
 	struct dev_rot_state *rot_state;
 	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
@@ -234,8 +254,21 @@ static int hid_dev_rot_probe(struct platform_device *pdev)
 	rot_state->common_attributes.hsdev = hsdev;
 	rot_state->common_attributes.pdev = pdev;
 
-	ret = hid_sensor_parse_common_attributes(hsdev,
-				HID_USAGE_SENSOR_DEVICE_ORIENTATION,
+	switch (hsdev->usage) {
+	case HID_USAGE_SENSOR_DEVICE_ORIENTATION:
+		name = "dev_rotation";
+		break;
+	case HID_USAGE_SENSOR_RELATIVE_ORIENTATION:
+		name = "relative_orientation";
+		break;
+	case HID_USAGE_SENSOR_GEOMAGNETIC_ORIENTATION:
+		name = "geomagnetic_orientation";
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = hid_sensor_parse_common_attributes(hsdev, hsdev->usage,
 				&rot_state->common_attributes);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to setup common attributes\n");
@@ -252,8 +285,7 @@ static int hid_dev_rot_probe(struct platform_device *pdev)
 
 	ret = dev_rot_parse_report(pdev, hsdev,
 				   (struct iio_chan_spec *)indio_dev->channels,
-				   HID_USAGE_SENSOR_DEVICE_ORIENTATION,
-				   rot_state);
+					hsdev->usage, rot_state);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to setup attributes\n");
 		return ret;
@@ -288,8 +320,7 @@ static int hid_dev_rot_probe(struct platform_device *pdev)
 	rot_state->callbacks.send_event = dev_rot_proc_event;
 	rot_state->callbacks.capture_sample = dev_rot_capture_sample;
 	rot_state->callbacks.pdev = pdev;
-	ret = sensor_hub_register_callback(hsdev,
-					HID_USAGE_SENSOR_DEVICE_ORIENTATION,
+	ret = sensor_hub_register_callback(hsdev, hsdev->usage,
 					&rot_state->callbacks);
 	if (ret) {
 		dev_err(&pdev->dev, "callback reg failed\n");
@@ -314,7 +345,7 @@ static int hid_dev_rot_remove(struct platform_device *pdev)
 	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
 	struct dev_rot_state *rot_state = iio_priv(indio_dev);
 
-	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_DEVICE_ORIENTATION);
+	sensor_hub_remove_callback(hsdev, hsdev->usage);
 	iio_device_unregister(indio_dev);
 	hid_sensor_remove_trigger(&rot_state->common_attributes);
 	iio_triggered_buffer_cleanup(indio_dev);
@@ -327,6 +358,14 @@ static const struct platform_device_id hid_dev_rot_ids[] = {
 		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
 		.name = "HID-SENSOR-20008a",
 	},
+	{
+		/* Relative orientation(AG) sensor */
+		.name = "HID-SENSOR-20008e",
+	},
+	{
+		/* Geomagnetic orientation(AM) sensor */
+		.name = "HID-SENSOR-2000c1",
+	},
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(platform, hid_dev_rot_ids);
diff --git a/drivers/iio/pressure/Kconfig b/drivers/iio/pressure/Kconfig
index 5d16b252ab6b..eaa7cfcb4c2a 100644
--- a/drivers/iio/pressure/Kconfig
+++ b/drivers/iio/pressure/Kconfig
@@ -23,7 +23,7 @@ config BMP280
 	select BMP280_SPI if (SPI_MASTER)
 	help
 	  Say yes here to build support for Bosch Sensortec BMP180 and BMP280
-	  pressure and temperature sensors. Also supports the BE280 with
+	  pressure and temperature sensors. Also supports the BME280 with
 	  an additional humidity sensor channel.
 
 	  To compile this driver as a module, choose M here: the core module
diff --git a/drivers/iio/pressure/st_pressure_core.c b/drivers/iio/pressure/st_pressure_core.c
index fd0edca0e656..aa61ec15c139 100644
--- a/drivers/iio/pressure/st_pressure_core.c
+++ b/drivers/iio/pressure/st_pressure_core.c
@@ -568,6 +568,8 @@ static const struct iio_trigger_ops st_press_trigger_ops = {
 int st_press_common_probe(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *press_data = iio_priv(indio_dev);
+	struct st_sensors_platform_data *pdata =
+		(struct st_sensors_platform_data *)press_data->dev->platform_data;
 	int irq = press_data->get_irq_data_ready(indio_dev);
 	int err;
 
@@ -603,10 +605,8 @@ int st_press_common_probe(struct iio_dev *indio_dev)
 	press_data->odr = press_data->sensor_settings->odr.odr_avl[0].hz;
 
 	/* Some devices don't support a data ready pin. */
-	if (!press_data->dev->platform_data &&
-				press_data->sensor_settings->drdy_irq.addr)
-		press_data->dev->platform_data =
-			(struct st_sensors_platform_data *)&default_press_pdata;
+	if (!pdata && press_data->sensor_settings->drdy_irq.addr)
+		pdata =	(struct st_sensors_platform_data *)&default_press_pdata;
 
 	err = st_sensors_init_sensor(indio_dev, press_data->dev->platform_data);
 	if (err < 0)
diff --git a/drivers/iio/pressure/zpa2326.c b/drivers/iio/pressure/zpa2326.c
index e58a0ad07477..c92a95f9f52c 100644
--- a/drivers/iio/pressure/zpa2326.c
+++ b/drivers/iio/pressure/zpa2326.c
@@ -867,12 +867,13 @@ static int zpa2326_wait_oneshot_completion(const struct iio_dev   *indio_dev,
 {
 	int          ret;
 	unsigned int val;
+	long     timeout;
 
 	zpa2326_dbg(indio_dev, "waiting for one shot completion interrupt");
 
-	ret = wait_for_completion_interruptible_timeout(
+	timeout = wait_for_completion_interruptible_timeout(
 		&private->data_ready, ZPA2326_CONVERSION_JIFFIES);
-	if (ret > 0)
+	if (timeout > 0)
 		/*
 		 * Interrupt handler completed before timeout: return operation
 		 * status.
@@ -882,13 +883,16 @@ static int zpa2326_wait_oneshot_completion(const struct iio_dev   *indio_dev,
 	/* Clear all interrupts just to be sure. */
 	regmap_read(private->regmap, ZPA2326_INT_SOURCE_REG, &val);
 
-	if (!ret)
+	if (!timeout) {
 		/* Timed out. */
+		zpa2326_warn(indio_dev, "no one shot interrupt occurred (%ld)",
+			     timeout);
 		ret = -ETIME;
-
-	if (ret != -ERESTARTSYS)
-		zpa2326_warn(indio_dev, "no one shot interrupt occurred (%d)",
-			     ret);
+	} else if (timeout < 0) {
+		zpa2326_warn(indio_dev,
+			     "wait for one shot interrupt cancelled");
+		ret = -ERESTARTSYS;
+	}
 
 	return ret;
 }
diff --git a/drivers/iio/proximity/as3935.c b/drivers/iio/proximity/as3935.c
index aa4df0dcc8c9..0eeff29b61be 100644
--- a/drivers/iio/proximity/as3935.c
+++ b/drivers/iio/proximity/as3935.c
@@ -176,13 +176,13 @@ static int as3935_read_raw(struct iio_dev *indio_dev,
 		if (ret)
 			return ret;
 
-		if (m == IIO_CHAN_INFO_RAW)
-			return IIO_VAL_INT;
-
 		/* storm out of range */
 		if (*val == AS3935_DATA_MASK)
 			return -EINVAL;
 
+		if (m == IIO_CHAN_INFO_RAW)
+			return IIO_VAL_INT;
+
 		if (m == IIO_CHAN_INFO_PROCESSED)
 			*val *= 1000;
 		break;
diff --git a/drivers/iio/proximity/sx9500.c b/drivers/iio/proximity/sx9500.c
index 9ea147f1a50d..f42b3a1c75ff 100644
--- a/drivers/iio/proximity/sx9500.c
+++ b/drivers/iio/proximity/sx9500.c
@@ -878,8 +878,7 @@ static void sx9500_gpio_probe(struct i2c_client *client,
 
 	dev = &client->dev;
 
-	data->gpiod_rst = devm_gpiod_get_index(dev, SX9500_GPIO_RESET,
-					       0, GPIOD_OUT_HIGH);
+	data->gpiod_rst = devm_gpiod_get(dev, SX9500_GPIO_RESET, GPIOD_OUT_HIGH);
 	if (IS_ERR(data->gpiod_rst)) {
 		dev_warn(dev, "gpio get reset pin failed\n");
 		data->gpiod_rst = NULL;
diff --git a/drivers/iio/temperature/maxim_thermocouple.c b/drivers/iio/temperature/maxim_thermocouple.c
index 557214202eff..d70e2e53d6a7 100644
--- a/drivers/iio/temperature/maxim_thermocouple.c
+++ b/drivers/iio/temperature/maxim_thermocouple.c
@@ -267,6 +267,7 @@ static int maxim_thermocouple_remove(struct spi_device *spi)
 static const struct spi_device_id maxim_thermocouple_id[] = {
 	{"max6675", MAX6675},
 	{"max31855", MAX31855},
+	{"max31856", MAX31855},
 	{},
 };
 MODULE_DEVICE_TABLE(spi, maxim_thermocouple_id);
diff --git a/drivers/iio/trigger/stm32-timer-trigger.c b/drivers/iio/trigger/stm32-timer-trigger.c
index 25248d644e7c..d22bc56dd9fc 100644
--- a/drivers/iio/trigger/stm32-timer-trigger.c
+++ b/drivers/iio/trigger/stm32-timer-trigger.c
@@ -14,19 +14,19 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 
-#define MAX_TRIGGERS 6
+#define MAX_TRIGGERS 7
 #define MAX_VALIDS 5
 
 /* List the triggers created by each timer */
 static const void *triggers_table[][MAX_TRIGGERS] = {
-	{ TIM1_TRGO, TIM1_CH1, TIM1_CH2, TIM1_CH3, TIM1_CH4,},
+	{ TIM1_TRGO, TIM1_TRGO2, TIM1_CH1, TIM1_CH2, TIM1_CH3, TIM1_CH4,},
 	{ TIM2_TRGO, TIM2_CH1, TIM2_CH2, TIM2_CH3, TIM2_CH4,},
 	{ TIM3_TRGO, TIM3_CH1, TIM3_CH2, TIM3_CH3, TIM3_CH4,},
 	{ TIM4_TRGO, TIM4_CH1, TIM4_CH2, TIM4_CH3, TIM4_CH4,},
 	{ TIM5_TRGO, TIM5_CH1, TIM5_CH2, TIM5_CH3, TIM5_CH4,},
 	{ TIM6_TRGO,},
 	{ TIM7_TRGO,},
-	{ TIM8_TRGO, TIM8_CH1, TIM8_CH2, TIM8_CH3, TIM8_CH4,},
+	{ TIM8_TRGO, TIM8_TRGO2, TIM8_CH1, TIM8_CH2, TIM8_CH3, TIM8_CH4,},
 	{ TIM9_TRGO, TIM9_CH1, TIM9_CH2,},
 	{ }, /* timer 10 */
 	{ }, /* timer 11 */
@@ -56,9 +56,16 @@ struct stm32_timer_trigger {
 	u32 max_arr;
 	const void *triggers;
 	const void *valids;
+	bool has_trgo2;
 };
 
+static bool stm32_timer_is_trgo2_name(const char *name)
+{
+	return !!strstr(name, "trgo2");
+}
+
 static int stm32_timer_start(struct stm32_timer_trigger *priv,
+			     struct iio_trigger *trig,
 			     unsigned int frequency)
 {
 	unsigned long long prd, div;
@@ -102,7 +109,12 @@ static int stm32_timer_start(struct stm32_timer_trigger *priv,
 	regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_ARPE, TIM_CR1_ARPE);
 
 	/* Force master mode to update mode */
-	regmap_update_bits(priv->regmap, TIM_CR2, TIM_CR2_MMS, 0x20);
+	if (stm32_timer_is_trgo2_name(trig->name))
+		regmap_update_bits(priv->regmap, TIM_CR2, TIM_CR2_MMS2,
+				   0x2 << TIM_CR2_MMS2_SHIFT);
+	else
+		regmap_update_bits(priv->regmap, TIM_CR2, TIM_CR2_MMS,
+				   0x2 << TIM_CR2_MMS_SHIFT);
 
 	/* Make sure that registers are updated */
 	regmap_update_bits(priv->regmap, TIM_EGR, TIM_EGR_UG, TIM_EGR_UG);
@@ -150,7 +162,7 @@ static ssize_t stm32_tt_store_frequency(struct device *dev,
 	if (freq == 0) {
 		stm32_timer_stop(priv);
 	} else {
-		ret = stm32_timer_start(priv, freq);
+		ret = stm32_timer_start(priv, trig, freq);
 		if (ret)
 			return ret;
 	}
@@ -183,6 +195,9 @@ static IIO_DEV_ATTR_SAMP_FREQ(0660,
 			      stm32_tt_read_frequency,
 			      stm32_tt_store_frequency);
 
+#define MASTER_MODE_MAX		7
+#define MASTER_MODE2_MAX	15
+
 static char *master_mode_table[] = {
 	"reset",
 	"enable",
@@ -191,7 +206,16 @@ static char *master_mode_table[] = {
 	"OC1REF",
 	"OC2REF",
 	"OC3REF",
-	"OC4REF"
+	"OC4REF",
+	/* Master mode selection 2 only */
+	"OC5REF",
+	"OC6REF",
+	"compare_pulse_OC4REF",
+	"compare_pulse_OC6REF",
+	"compare_pulse_OC4REF_r_or_OC6REF_r",
+	"compare_pulse_OC4REF_r_or_OC6REF_f",
+	"compare_pulse_OC5REF_r_or_OC6REF_r",
+	"compare_pulse_OC5REF_r_or_OC6REF_f",
 };
 
 static ssize_t stm32_tt_show_master_mode(struct device *dev,
@@ -199,10 +223,15 @@ static ssize_t stm32_tt_show_master_mode(struct device *dev,
 					 char *buf)
 {
 	struct stm32_timer_trigger *priv = dev_get_drvdata(dev);
+	struct iio_trigger *trig = to_iio_trigger(dev);
 	u32 cr2;
 
 	regmap_read(priv->regmap, TIM_CR2, &cr2);
-	cr2 = (cr2 & TIM_CR2_MMS) >> TIM_CR2_MMS_SHIFT;
+
+	if (stm32_timer_is_trgo2_name(trig->name))
+		cr2 = (cr2 & TIM_CR2_MMS2) >> TIM_CR2_MMS2_SHIFT;
+	else
+		cr2 = (cr2 & TIM_CR2_MMS) >> TIM_CR2_MMS_SHIFT;
 
 	return snprintf(buf, PAGE_SIZE, "%s\n", master_mode_table[cr2]);
 }
@@ -212,13 +241,25 @@ static ssize_t stm32_tt_store_master_mode(struct device *dev,
 					  const char *buf, size_t len)
 {
 	struct stm32_timer_trigger *priv = dev_get_drvdata(dev);
+	struct iio_trigger *trig = to_iio_trigger(dev);
+	u32 mask, shift, master_mode_max;
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(master_mode_table); i++) {
+	if (stm32_timer_is_trgo2_name(trig->name)) {
+		mask = TIM_CR2_MMS2;
+		shift = TIM_CR2_MMS2_SHIFT;
+		master_mode_max = MASTER_MODE2_MAX;
+	} else {
+		mask = TIM_CR2_MMS;
+		shift = TIM_CR2_MMS_SHIFT;
+		master_mode_max = MASTER_MODE_MAX;
+	}
+
+	for (i = 0; i <= master_mode_max; i++) {
 		if (!strncmp(master_mode_table[i], buf,
 			     strlen(master_mode_table[i]))) {
-			regmap_update_bits(priv->regmap, TIM_CR2,
-					   TIM_CR2_MMS, i << TIM_CR2_MMS_SHIFT);
+			regmap_update_bits(priv->regmap, TIM_CR2, mask,
+					   i << shift);
 			/* Make sure that registers are updated */
 			regmap_update_bits(priv->regmap, TIM_EGR,
 					   TIM_EGR_UG, TIM_EGR_UG);
@@ -229,8 +270,31 @@ static ssize_t stm32_tt_store_master_mode(struct device *dev,
 	return -EINVAL;
 }
 
-static IIO_CONST_ATTR(master_mode_available,
-	"reset enable update compare_pulse OC1REF OC2REF OC3REF OC4REF");
+static ssize_t stm32_tt_show_master_mode_avail(struct device *dev,
+					       struct device_attribute *attr,
+					       char *buf)
+{
+	struct iio_trigger *trig = to_iio_trigger(dev);
+	unsigned int i, master_mode_max;
+	size_t len = 0;
+
+	if (stm32_timer_is_trgo2_name(trig->name))
+		master_mode_max = MASTER_MODE2_MAX;
+	else
+		master_mode_max = MASTER_MODE_MAX;
+
+	for (i = 0; i <= master_mode_max; i++)
+		len += scnprintf(buf + len, PAGE_SIZE - len,
+			"%s ", master_mode_table[i]);
+
+	/* replace trailing space by newline */
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR(master_mode_available, 0444,
+		       stm32_tt_show_master_mode_avail, NULL, 0);
 
 static IIO_DEVICE_ATTR(master_mode, 0660,
 		       stm32_tt_show_master_mode,
@@ -240,7 +304,7 @@ static IIO_DEVICE_ATTR(master_mode, 0660,
 static struct attribute *stm32_trigger_attrs[] = {
 	&iio_dev_attr_sampling_frequency.dev_attr.attr,
 	&iio_dev_attr_master_mode.dev_attr.attr,
-	&iio_const_attr_master_mode_available.dev_attr.attr,
+	&iio_dev_attr_master_mode_available.dev_attr.attr,
 	NULL,
 };
 
@@ -264,6 +328,12 @@ static int stm32_setup_iio_triggers(struct stm32_timer_trigger *priv)
 
 	while (cur && *cur) {
 		struct iio_trigger *trig;
+		bool cur_is_trgo2 = stm32_timer_is_trgo2_name(*cur);
+
+		if (cur_is_trgo2 && !priv->has_trgo2) {
+			cur++;
+			continue;
+		}
 
 		trig = devm_iio_trigger_alloc(priv->dev, "%s", *cur);
 		if  (!trig)
@@ -277,7 +347,7 @@ static int stm32_setup_iio_triggers(struct stm32_timer_trigger *priv)
 		 * should only be available on trgo trigger which
 		 * is always the first in the list.
 		 */
-		if (cur == priv->triggers)
+		if (cur == priv->triggers || cur_is_trgo2)
 			trig->dev.groups = stm32_trigger_attr_groups;
 
 		iio_trigger_set_drvdata(trig, priv);
@@ -347,12 +417,70 @@ static int stm32_counter_write_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
+static int stm32_counter_validate_trigger(struct iio_dev *indio_dev,
+					  struct iio_trigger *trig)
+{
+	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+	const char * const *cur = priv->valids;
+	unsigned int i = 0;
+
+	if (!is_stm32_timer_trigger(trig))
+		return -EINVAL;
+
+	while (cur && *cur) {
+		if (!strncmp(trig->name, *cur, strlen(trig->name))) {
+			regmap_update_bits(priv->regmap,
+					   TIM_SMCR, TIM_SMCR_TS,
+					   i << TIM_SMCR_TS_SHIFT);
+			return 0;
+		}
+		cur++;
+		i++;
+	}
+
+	return -EINVAL;
+}
+
 static const struct iio_info stm32_trigger_info = {
 	.driver_module = THIS_MODULE,
+	.validate_trigger = stm32_counter_validate_trigger,
 	.read_raw = stm32_counter_read_raw,
 	.write_raw = stm32_counter_write_raw
 };
 
+static const char *const stm32_trigger_modes[] = {
+	"trigger",
+};
+
+static int stm32_set_trigger_mode(struct iio_dev *indio_dev,
+				  const struct iio_chan_spec *chan,
+				  unsigned int mode)
+{
+	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+
+	regmap_update_bits(priv->regmap, TIM_SMCR, TIM_SMCR_SMS, TIM_SMCR_SMS);
+
+	return 0;
+}
+
+static int stm32_get_trigger_mode(struct iio_dev *indio_dev,
+				  const struct iio_chan_spec *chan)
+{
+	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+	u32 smcr;
+
+	regmap_read(priv->regmap, TIM_SMCR, &smcr);
+
+	return smcr == TIM_SMCR_SMS ? 0 : -EINVAL;
+}
+
+static const struct iio_enum stm32_trigger_mode_enum = {
+	.items = stm32_trigger_modes,
+	.num_items = ARRAY_SIZE(stm32_trigger_modes),
+	.set = stm32_set_trigger_mode,
+	.get = stm32_get_trigger_mode
+};
+
 static const char *const stm32_enable_modes[] = {
 	"always",
 	"gated",
@@ -536,6 +664,8 @@ static const struct iio_chan_spec_ext_info stm32_trigger_count_info[] = {
 	IIO_ENUM_AVAILABLE("quadrature_mode", &stm32_quadrature_mode_enum),
 	IIO_ENUM("enable_mode", IIO_SEPARATE, &stm32_enable_mode_enum),
 	IIO_ENUM_AVAILABLE("enable_mode", &stm32_enable_mode_enum),
+	IIO_ENUM("trigger_mode", IIO_SEPARATE, &stm32_trigger_mode_enum),
+	IIO_ENUM_AVAILABLE("trigger_mode", &stm32_trigger_mode_enum),
 	{}
 };
 
@@ -560,6 +690,7 @@ static struct stm32_timer_trigger *stm32_setup_counter_device(struct device *dev
 	indio_dev->name = dev_name(dev);
 	indio_dev->dev.parent = dev;
 	indio_dev->info = &stm32_trigger_info;
+	indio_dev->modes = INDIO_HARDWARE_TRIGGERED;
 	indio_dev->num_channels = 1;
 	indio_dev->channels = &stm32_trigger_channel;
 	indio_dev->dev.of_node = dev->of_node;
@@ -584,6 +715,20 @@ bool is_stm32_timer_trigger(struct iio_trigger *trig)
 }
 EXPORT_SYMBOL(is_stm32_timer_trigger);
 
+static void stm32_timer_detect_trgo2(struct stm32_timer_trigger *priv)
+{
+	u32 val;
+
+	/*
+	 * Master mode selection 2 bits can only be written and read back when
+	 * timer supports it.
+	 */
+	regmap_update_bits(priv->regmap, TIM_CR2, TIM_CR2_MMS2, TIM_CR2_MMS2);
+	regmap_read(priv->regmap, TIM_CR2, &val);
+	regmap_update_bits(priv->regmap, TIM_CR2, TIM_CR2_MMS2, 0);
+	priv->has_trgo2 = !!val;
+}
+
 static int stm32_timer_trigger_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -614,6 +759,7 @@ static int stm32_timer_trigger_probe(struct platform_device *pdev)
 	priv->max_arr = ddata->max_arr;
 	priv->triggers = triggers_table[index];
 	priv->valids = valids_table[index];
+	stm32_timer_detect_trgo2(priv);
 
 	ret = stm32_setup_iio_triggers(priv);
 	if (ret)
diff --git a/drivers/infiniband/core/Makefile b/drivers/infiniband/core/Makefile
index 6ebd9ad95010..e3cdafff8ece 100644
--- a/drivers/infiniband/core/Makefile
+++ b/drivers/infiniband/core/Makefile
@@ -10,7 +10,8 @@ obj-$(CONFIG_INFINIBAND_USER_ACCESS) +=	ib_uverbs.o ib_ucm.o \
 ib_core-y :=			packer.o ud_header.o verbs.o cq.o rw.o sysfs.o \
 				device.o fmr_pool.o cache.o netlink.o \
 				roce_gid_mgmt.o mr_pool.o addr.o sa_query.o \
-				multicast.o mad.o smi.o agent.o mad_rmpp.o
+				multicast.o mad.o smi.o agent.o mad_rmpp.o \
+				security.o
 ib_core-$(CONFIG_INFINIBAND_USER_MEM) += umem.o
 ib_core-$(CONFIG_INFINIBAND_ON_DEMAND_PAGING) += umem_odp.o umem_rbtree.o
 ib_core-$(CONFIG_CGROUP_RDMA) += cgroup.o
diff --git a/drivers/infiniband/core/addr.c b/drivers/infiniband/core/addr.c
index 02971e239a18..a6cb379a4ebc 100644
--- a/drivers/infiniband/core/addr.c
+++ b/drivers/infiniband/core/addr.c
@@ -179,8 +179,7 @@ static int ib_nl_ip_send_msg(struct rdma_dev_addr *dev_addr,
 	}
 
 	/* Construct the family header first */
-	header = (struct rdma_ls_ip_resolve_header *)
-		skb_put(skb, NLMSG_ALIGN(sizeof(*header)));
+	header = skb_put(skb, NLMSG_ALIGN(sizeof(*header)));
 	header->ifindex = dev_addr->bound_dev_if;
 	nla_put(skb, attrtype, size, daddr);
 
@@ -449,12 +448,7 @@ static int addr6_resolve(struct sockaddr_in6 *src_in,
 		return ret;
 
 	rt = (struct rt6_info *)dst;
-	if (ipv6_addr_any(&fl6.saddr)) {
-		ret = ipv6_dev_get_saddr(addr->net, ip6_dst_idev(dst)->dev,
-					 &fl6.daddr, 0, &fl6.saddr);
-		if (ret)
-			goto put;
-
+	if (ipv6_addr_any(&src_in->sin6_addr)) {
 		src_in->sin6_family = AF_INET6;
 		src_in->sin6_addr = fl6.saddr;
 	}
@@ -471,9 +465,6 @@ static int addr6_resolve(struct sockaddr_in6 *src_in,
 
 	*pdst = dst;
 	return 0;
-put:
-	dst_release(dst);
-	return ret;
 }
 #else
 static int addr6_resolve(struct sockaddr_in6 *src_in,
diff --git a/drivers/infiniband/core/cache.c b/drivers/infiniband/core/cache.c
index b1371eb9f46c..efc94304dee3 100644
--- a/drivers/infiniband/core/cache.c
+++ b/drivers/infiniband/core/cache.c
@@ -53,6 +53,7 @@ struct ib_update_work {
 	struct work_struct work;
 	struct ib_device  *device;
 	u8                 port_num;
+	bool		   enforce_security;
 };
 
 union ib_gid zgid;
@@ -911,6 +912,26 @@ int ib_get_cached_pkey(struct ib_device *device,
 }
 EXPORT_SYMBOL(ib_get_cached_pkey);
 
+int ib_get_cached_subnet_prefix(struct ib_device *device,
+				u8                port_num,
+				u64              *sn_pfx)
+{
+	unsigned long flags;
+	int p;
+
+	if (port_num < rdma_start_port(device) ||
+	    port_num > rdma_end_port(device))
+		return -EINVAL;
+
+	p = port_num - rdma_start_port(device);
+	read_lock_irqsave(&device->cache.lock, flags);
+	*sn_pfx = device->cache.ports[p].subnet_prefix;
+	read_unlock_irqrestore(&device->cache.lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(ib_get_cached_subnet_prefix);
+
 int ib_find_cached_pkey(struct ib_device *device,
 			u8                port_num,
 			u16               pkey,
@@ -1022,7 +1043,8 @@ int ib_get_cached_port_state(struct ib_device   *device,
 EXPORT_SYMBOL(ib_get_cached_port_state);
 
 static void ib_cache_update(struct ib_device *device,
-			    u8                port)
+			    u8                port,
+			    bool	      enforce_security)
 {
 	struct ib_port_attr       *tprops = NULL;
 	struct ib_pkey_cache      *pkey_cache = NULL, *old_pkey_cache;
@@ -1108,8 +1130,15 @@ static void ib_cache_update(struct ib_device *device,
 	device->cache.ports[port - rdma_start_port(device)].port_state =
 		tprops->state;
 
+	device->cache.ports[port - rdma_start_port(device)].subnet_prefix =
+							tprops->subnet_prefix;
 	write_unlock_irq(&device->cache.lock);
 
+	if (enforce_security)
+		ib_security_cache_change(device,
+					 port,
+					 tprops->subnet_prefix);
+
 	kfree(gid_cache);
 	kfree(old_pkey_cache);
 	kfree(tprops);
@@ -1126,7 +1155,9 @@ static void ib_cache_task(struct work_struct *_work)
 	struct ib_update_work *work =
 		container_of(_work, struct ib_update_work, work);
 
-	ib_cache_update(work->device, work->port_num);
+	ib_cache_update(work->device,
+			work->port_num,
+			work->enforce_security);
 	kfree(work);
 }
 
@@ -1147,6 +1178,12 @@ static void ib_cache_event(struct ib_event_handler *handler,
 			INIT_WORK(&work->work, ib_cache_task);
 			work->device   = event->device;
 			work->port_num = event->element.port_num;
+			if (event->event == IB_EVENT_PKEY_CHANGE ||
+			    event->event == IB_EVENT_GID_CHANGE)
+				work->enforce_security = true;
+			else
+				work->enforce_security = false;
+
 			queue_work(ib_wq, &work->work);
 		}
 	}
@@ -1172,7 +1209,7 @@ int ib_cache_setup_one(struct ib_device *device)
 		goto out;
 
 	for (p = 0; p <= rdma_end_port(device) - rdma_start_port(device); ++p)
-		ib_cache_update(device, p + rdma_start_port(device));
+		ib_cache_update(device, p + rdma_start_port(device), true);
 
 	INIT_IB_EVENT_HANDLER(&device->cache.event_handler,
 			      device, ib_cache_event);
diff --git a/drivers/infiniband/core/core_priv.h b/drivers/infiniband/core/core_priv.h
index d92ab4eaa8f3..11ae67514e13 100644
--- a/drivers/infiniband/core/core_priv.h
+++ b/drivers/infiniband/core/core_priv.h
@@ -38,6 +38,16 @@
 #include <linux/cgroup_rdma.h>
 
 #include <rdma/ib_verbs.h>
+#include <rdma/ib_mad.h>
+#include "mad_priv.h"
+
+struct pkey_index_qp_list {
+	struct list_head    pkey_index_list;
+	u16                 pkey_index;
+	/* Lock to hold while iterating the qp_list. */
+	spinlock_t          qp_list_lock;
+	struct list_head    qp_list;
+};
 
 #if IS_ENABLED(CONFIG_INFINIBAND_ADDR_TRANS_CONFIGFS)
 int cma_configfs_init(void);
@@ -186,4 +196,109 @@ int ib_nl_handle_set_timeout(struct sk_buff *skb,
 int ib_nl_handle_ip_res_resp(struct sk_buff *skb,
 			     struct netlink_callback *cb);
 
+int ib_get_cached_subnet_prefix(struct ib_device *device,
+				u8                port_num,
+				u64              *sn_pfx);
+
+#ifdef CONFIG_SECURITY_INFINIBAND
+int ib_security_pkey_access(struct ib_device *dev,
+			    u8 port_num,
+			    u16 pkey_index,
+			    void *sec);
+
+void ib_security_destroy_port_pkey_list(struct ib_device *device);
+
+void ib_security_cache_change(struct ib_device *device,
+			      u8 port_num,
+			      u64 subnet_prefix);
+
+int ib_security_modify_qp(struct ib_qp *qp,
+			  struct ib_qp_attr *qp_attr,
+			  int qp_attr_mask,
+			  struct ib_udata *udata);
+
+int ib_create_qp_security(struct ib_qp *qp, struct ib_device *dev);
+void ib_destroy_qp_security_begin(struct ib_qp_security *sec);
+void ib_destroy_qp_security_abort(struct ib_qp_security *sec);
+void ib_destroy_qp_security_end(struct ib_qp_security *sec);
+int ib_open_shared_qp_security(struct ib_qp *qp, struct ib_device *dev);
+void ib_close_shared_qp_security(struct ib_qp_security *sec);
+int ib_mad_agent_security_setup(struct ib_mad_agent *agent,
+				enum ib_qp_type qp_type);
+void ib_mad_agent_security_cleanup(struct ib_mad_agent *agent);
+int ib_mad_enforce_security(struct ib_mad_agent_private *map, u16 pkey_index);
+#else
+static inline int ib_security_pkey_access(struct ib_device *dev,
+					  u8 port_num,
+					  u16 pkey_index,
+					  void *sec)
+{
+	return 0;
+}
+
+static inline void ib_security_destroy_port_pkey_list(struct ib_device *device)
+{
+}
+
+static inline void ib_security_cache_change(struct ib_device *device,
+					    u8 port_num,
+					    u64 subnet_prefix)
+{
+}
+
+static inline int ib_security_modify_qp(struct ib_qp *qp,
+					struct ib_qp_attr *qp_attr,
+					int qp_attr_mask,
+					struct ib_udata *udata)
+{
+	return qp->device->modify_qp(qp->real_qp,
+				     qp_attr,
+				     qp_attr_mask,
+				     udata);
+}
+
+static inline int ib_create_qp_security(struct ib_qp *qp,
+					struct ib_device *dev)
+{
+	return 0;
+}
+
+static inline void ib_destroy_qp_security_begin(struct ib_qp_security *sec)
+{
+}
+
+static inline void ib_destroy_qp_security_abort(struct ib_qp_security *sec)
+{
+}
+
+static inline void ib_destroy_qp_security_end(struct ib_qp_security *sec)
+{
+}
+
+static inline int ib_open_shared_qp_security(struct ib_qp *qp,
+					     struct ib_device *dev)
+{
+	return 0;
+}
+
+static inline void ib_close_shared_qp_security(struct ib_qp_security *sec)
+{
+}
+
+static inline int ib_mad_agent_security_setup(struct ib_mad_agent *agent,
+					      enum ib_qp_type qp_type)
+{
+	return 0;
+}
+
+static inline void ib_mad_agent_security_cleanup(struct ib_mad_agent *agent)
+{
+}
+
+static inline int ib_mad_enforce_security(struct ib_mad_agent_private *map,
+					  u16 pkey_index)
+{
+	return 0;
+}
+#endif
 #endif /* _CORE_PRIV_H */
diff --git a/drivers/infiniband/core/device.c b/drivers/infiniband/core/device.c
index 81d447da0048..631eaa9daf65 100644
--- a/drivers/infiniband/core/device.c
+++ b/drivers/infiniband/core/device.c
@@ -39,6 +39,8 @@
 #include <linux/init.h>
 #include <linux/mutex.h>
 #include <linux/netdevice.h>
+#include <linux/security.h>
+#include <linux/notifier.h>
 #include <rdma/rdma_netlink.h>
 #include <rdma/ib_addr.h>
 #include <rdma/ib_cache.h>
@@ -82,6 +84,14 @@ static LIST_HEAD(client_list);
 static DEFINE_MUTEX(device_mutex);
 static DECLARE_RWSEM(lists_rwsem);
 
+static int ib_security_change(struct notifier_block *nb, unsigned long event,
+			      void *lsm_data);
+static void ib_policy_change_task(struct work_struct *work);
+static DECLARE_WORK(ib_policy_change_work, ib_policy_change_task);
+
+static struct notifier_block ibdev_lsm_nb = {
+	.notifier_call = ib_security_change,
+};
 
 static int ib_device_check_mandatory(struct ib_device *device)
 {
@@ -325,6 +335,64 @@ void ib_get_device_fw_str(struct ib_device *dev, char *str, size_t str_len)
 }
 EXPORT_SYMBOL(ib_get_device_fw_str);
 
+static int setup_port_pkey_list(struct ib_device *device)
+{
+	int i;
+
+	/**
+	 * device->port_pkey_list is indexed directly by the port number,
+	 * Therefore it is declared as a 1 based array with potential empty
+	 * slots at the beginning.
+	 */
+	device->port_pkey_list = kcalloc(rdma_end_port(device) + 1,
+					 sizeof(*device->port_pkey_list),
+					 GFP_KERNEL);
+
+	if (!device->port_pkey_list)
+		return -ENOMEM;
+
+	for (i = 0; i < (rdma_end_port(device) + 1); i++) {
+		spin_lock_init(&device->port_pkey_list[i].list_lock);
+		INIT_LIST_HEAD(&device->port_pkey_list[i].pkey_list);
+	}
+
+	return 0;
+}
+
+static void ib_policy_change_task(struct work_struct *work)
+{
+	struct ib_device *dev;
+
+	down_read(&lists_rwsem);
+	list_for_each_entry(dev, &device_list, core_list) {
+		int i;
+
+		for (i = rdma_start_port(dev); i <= rdma_end_port(dev); i++) {
+			u64 sp;
+			int ret = ib_get_cached_subnet_prefix(dev,
+							      i,
+							      &sp);
+
+			WARN_ONCE(ret,
+				  "ib_get_cached_subnet_prefix err: %d, this should never happen here\n",
+				  ret);
+			ib_security_cache_change(dev, i, sp);
+		}
+	}
+	up_read(&lists_rwsem);
+}
+
+static int ib_security_change(struct notifier_block *nb, unsigned long event,
+			      void *lsm_data)
+{
+	if (event != LSM_POLICY_CHANGE)
+		return NOTIFY_DONE;
+
+	schedule_work(&ib_policy_change_work);
+
+	return NOTIFY_OK;
+}
+
 /**
  * ib_register_device - Register an IB device with IB core
  * @device:Device to register
@@ -385,6 +453,12 @@ int ib_register_device(struct ib_device *device,
 		goto out;
 	}
 
+	ret = setup_port_pkey_list(device);
+	if (ret) {
+		pr_warn("Couldn't create per port_pkey_list\n");
+		goto out;
+	}
+
 	ret = ib_cache_setup_one(device);
 	if (ret) {
 		pr_warn("Couldn't set up InfiniBand P_Key/GID cache\n");
@@ -468,6 +542,9 @@ void ib_unregister_device(struct ib_device *device)
 	ib_device_unregister_sysfs(device);
 	ib_cache_cleanup_one(device);
 
+	ib_security_destroy_port_pkey_list(device);
+	kfree(device->port_pkey_list);
+
 	down_write(&lists_rwsem);
 	spin_lock_irqsave(&device->client_data_lock, flags);
 	list_for_each_entry_safe(context, tmp, &device->client_data_list, list)
@@ -1082,10 +1159,18 @@ static int __init ib_core_init(void)
 		goto err_sa;
 	}
 
+	ret = register_lsm_notifier(&ibdev_lsm_nb);
+	if (ret) {
+		pr_warn("Couldn't register LSM notifier. ret %d\n", ret);
+		goto err_ibnl_clients;
+	}
+
 	ib_cache_setup();
 
 	return 0;
 
+err_ibnl_clients:
+	ib_remove_ibnl_clients();
 err_sa:
 	ib_sa_cleanup();
 err_mad:
@@ -1105,6 +1190,7 @@ err:
 
 static void __exit ib_core_cleanup(void)
 {
+	unregister_lsm_notifier(&ibdev_lsm_nb);
 	ib_cache_cleanup();
 	ib_remove_ibnl_clients();
 	ib_sa_cleanup();
diff --git a/drivers/infiniband/core/mad.c b/drivers/infiniband/core/mad.c
index 192ee3dafb80..f8f53bb90837 100644
--- a/drivers/infiniband/core/mad.c
+++ b/drivers/infiniband/core/mad.c
@@ -40,9 +40,11 @@
 #include <linux/dma-mapping.h>
 #include <linux/slab.h>
 #include <linux/module.h>
+#include <linux/security.h>
 #include <rdma/ib_cache.h>
 
 #include "mad_priv.h"
+#include "core_priv.h"
 #include "mad_rmpp.h"
 #include "smi.h"
 #include "opa_smi.h"
@@ -369,6 +371,12 @@ struct ib_mad_agent *ib_register_mad_agent(struct ib_device *device,
 	atomic_set(&mad_agent_priv->refcount, 1);
 	init_completion(&mad_agent_priv->comp);
 
+	ret2 = ib_mad_agent_security_setup(&mad_agent_priv->agent, qp_type);
+	if (ret2) {
+		ret = ERR_PTR(ret2);
+		goto error4;
+	}
+
 	spin_lock_irqsave(&port_priv->reg_lock, flags);
 	mad_agent_priv->agent.hi_tid = ++ib_mad_client_id;
 
@@ -386,7 +394,7 @@ struct ib_mad_agent *ib_register_mad_agent(struct ib_device *device,
 				if (method) {
 					if (method_in_use(&method,
 							   mad_reg_req))
-						goto error4;
+						goto error5;
 				}
 			}
 			ret2 = add_nonoui_reg_req(mad_reg_req, mad_agent_priv,
@@ -402,14 +410,14 @@ struct ib_mad_agent *ib_register_mad_agent(struct ib_device *device,
 					if (is_vendor_method_in_use(
 							vendor_class,
 							mad_reg_req))
-						goto error4;
+						goto error5;
 				}
 			}
 			ret2 = add_oui_reg_req(mad_reg_req, mad_agent_priv);
 		}
 		if (ret2) {
 			ret = ERR_PTR(ret2);
-			goto error4;
+			goto error5;
 		}
 	}
 
@@ -418,9 +426,10 @@ struct ib_mad_agent *ib_register_mad_agent(struct ib_device *device,
 	spin_unlock_irqrestore(&port_priv->reg_lock, flags);
 
 	return &mad_agent_priv->agent;
-
-error4:
+error5:
 	spin_unlock_irqrestore(&port_priv->reg_lock, flags);
+	ib_mad_agent_security_cleanup(&mad_agent_priv->agent);
+error4:
 	kfree(reg_req);
 error3:
 	kfree(mad_agent_priv);
@@ -491,6 +500,7 @@ struct ib_mad_agent *ib_register_mad_snoop(struct ib_device *device,
 	struct ib_mad_agent *ret;
 	struct ib_mad_snoop_private *mad_snoop_priv;
 	int qpn;
+	int err;
 
 	/* Validate parameters */
 	if ((is_snooping_sends(mad_snoop_flags) && !snoop_handler) ||
@@ -525,17 +535,25 @@ struct ib_mad_agent *ib_register_mad_snoop(struct ib_device *device,
 	mad_snoop_priv->agent.port_num = port_num;
 	mad_snoop_priv->mad_snoop_flags = mad_snoop_flags;
 	init_completion(&mad_snoop_priv->comp);
+
+	err = ib_mad_agent_security_setup(&mad_snoop_priv->agent, qp_type);
+	if (err) {
+		ret = ERR_PTR(err);
+		goto error2;
+	}
+
 	mad_snoop_priv->snoop_index = register_snoop_agent(
 						&port_priv->qp_info[qpn],
 						mad_snoop_priv);
 	if (mad_snoop_priv->snoop_index < 0) {
 		ret = ERR_PTR(mad_snoop_priv->snoop_index);
-		goto error2;
+		goto error3;
 	}
 
 	atomic_set(&mad_snoop_priv->refcount, 1);
 	return &mad_snoop_priv->agent;
-
+error3:
+	ib_mad_agent_security_cleanup(&mad_snoop_priv->agent);
 error2:
 	kfree(mad_snoop_priv);
 error1:
@@ -581,6 +599,8 @@ static void unregister_mad_agent(struct ib_mad_agent_private *mad_agent_priv)
 	deref_mad_agent(mad_agent_priv);
 	wait_for_completion(&mad_agent_priv->comp);
 
+	ib_mad_agent_security_cleanup(&mad_agent_priv->agent);
+
 	kfree(mad_agent_priv->reg_req);
 	kfree(mad_agent_priv);
 }
@@ -599,6 +619,8 @@ static void unregister_mad_snoop(struct ib_mad_snoop_private *mad_snoop_priv)
 	deref_snoop_agent(mad_snoop_priv);
 	wait_for_completion(&mad_snoop_priv->comp);
 
+	ib_mad_agent_security_cleanup(&mad_snoop_priv->agent);
+
 	kfree(mad_snoop_priv);
 }
 
@@ -1215,12 +1237,16 @@ int ib_post_send_mad(struct ib_mad_send_buf *send_buf,
 
 	/* Walk list of send WRs and post each on send list */
 	for (; send_buf; send_buf = next_send_buf) {
-
 		mad_send_wr = container_of(send_buf,
 					   struct ib_mad_send_wr_private,
 					   send_buf);
 		mad_agent_priv = mad_send_wr->mad_agent_priv;
 
+		ret = ib_mad_enforce_security(mad_agent_priv,
+					      mad_send_wr->send_wr.pkey_index);
+		if (ret)
+			goto error;
+
 		if (!send_buf->mad_agent->send_handler ||
 		    (send_buf->timeout_ms &&
 		     !send_buf->mad_agent->recv_handler)) {
@@ -1946,6 +1972,14 @@ static void ib_mad_complete_recv(struct ib_mad_agent_private *mad_agent_priv,
 	struct ib_mad_send_wr_private *mad_send_wr;
 	struct ib_mad_send_wc mad_send_wc;
 	unsigned long flags;
+	int ret;
+
+	ret = ib_mad_enforce_security(mad_agent_priv,
+				      mad_recv_wc->wc->pkey_index);
+	if (ret) {
+		ib_free_recv_mad(mad_recv_wc);
+		deref_mad_agent(mad_agent_priv);
+	}
 
 	INIT_LIST_HEAD(&mad_recv_wc->rmpp_list);
 	list_add(&mad_recv_wc->recv_buf.list, &mad_recv_wc->rmpp_list);
@@ -2003,6 +2037,8 @@ static void ib_mad_complete_recv(struct ib_mad_agent_private *mad_agent_priv,
 						   mad_recv_wc);
 		deref_mad_agent(mad_agent_priv);
 	}
+
+	return;
 }
 
 static enum smi_action handle_ib_smi(const struct ib_mad_port_private *port_priv,
diff --git a/drivers/infiniband/core/sa_query.c b/drivers/infiniband/core/sa_query.c
index fb7aec4047c8..70fa4cabe48e 100644
--- a/drivers/infiniband/core/sa_query.c
+++ b/drivers/infiniband/core/sa_query.c
@@ -759,8 +759,7 @@ static void ib_nl_set_path_rec_attrs(struct sk_buff *skb,
 	query->mad_buf->context[1] = NULL;
 
 	/* Construct the family header first */
-	header = (struct rdma_ls_resolve_header *)
-		skb_put(skb, NLMSG_ALIGN(sizeof(*header)));
+	header = skb_put(skb, NLMSG_ALIGN(sizeof(*header)));
 	memcpy(header->device_name, query->port->agent->device->name,
 	       LS_DEVICE_NAME_MAX);
 	header->port_num = query->port->port_num;
diff --git a/drivers/infiniband/core/security.c b/drivers/infiniband/core/security.c
new file mode 100644
index 000000000000..3e8c38953912
--- /dev/null
+++ b/drivers/infiniband/core/security.c
@@ -0,0 +1,705 @@
+/*
+ * Copyright (c) 2016 Mellanox Technologies Ltd.  All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifdef CONFIG_SECURITY_INFINIBAND
+
+#include <linux/security.h>
+#include <linux/completion.h>
+#include <linux/list.h>
+
+#include <rdma/ib_verbs.h>
+#include <rdma/ib_cache.h>
+#include "core_priv.h"
+#include "mad_priv.h"
+
+static struct pkey_index_qp_list *get_pkey_idx_qp_list(struct ib_port_pkey *pp)
+{
+	struct pkey_index_qp_list *pkey = NULL;
+	struct pkey_index_qp_list *tmp_pkey;
+	struct ib_device *dev = pp->sec->dev;
+
+	spin_lock(&dev->port_pkey_list[pp->port_num].list_lock);
+	list_for_each_entry(tmp_pkey,
+			    &dev->port_pkey_list[pp->port_num].pkey_list,
+			    pkey_index_list) {
+		if (tmp_pkey->pkey_index == pp->pkey_index) {
+			pkey = tmp_pkey;
+			break;
+		}
+	}
+	spin_unlock(&dev->port_pkey_list[pp->port_num].list_lock);
+	return pkey;
+}
+
+static int get_pkey_and_subnet_prefix(struct ib_port_pkey *pp,
+				      u16 *pkey,
+				      u64 *subnet_prefix)
+{
+	struct ib_device *dev = pp->sec->dev;
+	int ret;
+
+	ret = ib_get_cached_pkey(dev, pp->port_num, pp->pkey_index, pkey);
+	if (ret)
+		return ret;
+
+	ret = ib_get_cached_subnet_prefix(dev, pp->port_num, subnet_prefix);
+
+	return ret;
+}
+
+static int enforce_qp_pkey_security(u16 pkey,
+				    u64 subnet_prefix,
+				    struct ib_qp_security *qp_sec)
+{
+	struct ib_qp_security *shared_qp_sec;
+	int ret;
+
+	ret = security_ib_pkey_access(qp_sec->security, subnet_prefix, pkey);
+	if (ret)
+		return ret;
+
+	if (qp_sec->qp == qp_sec->qp->real_qp) {
+		list_for_each_entry(shared_qp_sec,
+				    &qp_sec->shared_qp_list,
+				    shared_qp_list) {
+			ret = security_ib_pkey_access(shared_qp_sec->security,
+						      subnet_prefix,
+						      pkey);
+			if (ret)
+				return ret;
+		}
+	}
+	return 0;
+}
+
+/* The caller of this function must hold the QP security
+ * mutex of the QP of the security structure in *pps.
+ *
+ * It takes separate ports_pkeys and security structure
+ * because in some cases the pps will be for a new settings
+ * or the pps will be for the real QP and security structure
+ * will be for a shared QP.
+ */
+static int check_qp_port_pkey_settings(struct ib_ports_pkeys *pps,
+				       struct ib_qp_security *sec)
+{
+	u64 subnet_prefix;
+	u16 pkey;
+	int ret = 0;
+
+	if (!pps)
+		return 0;
+
+	if (pps->main.state != IB_PORT_PKEY_NOT_VALID) {
+		get_pkey_and_subnet_prefix(&pps->main,
+					   &pkey,
+					   &subnet_prefix);
+
+		ret = enforce_qp_pkey_security(pkey,
+					       subnet_prefix,
+					       sec);
+	}
+	if (ret)
+		return ret;
+
+	if (pps->alt.state != IB_PORT_PKEY_NOT_VALID) {
+		get_pkey_and_subnet_prefix(&pps->alt,
+					   &pkey,
+					   &subnet_prefix);
+
+		ret = enforce_qp_pkey_security(pkey,
+					       subnet_prefix,
+					       sec);
+	}
+
+	return ret;
+}
+
+/* The caller of this function must hold the QP security
+ * mutex.
+ */
+static void qp_to_error(struct ib_qp_security *sec)
+{
+	struct ib_qp_security *shared_qp_sec;
+	struct ib_qp_attr attr = {
+		.qp_state = IB_QPS_ERR
+	};
+	struct ib_event event = {
+		.event = IB_EVENT_QP_FATAL
+	};
+
+	/* If the QP is in the process of being destroyed
+	 * the qp pointer in the security structure is
+	 * undefined.  It cannot be modified now.
+	 */
+	if (sec->destroying)
+		return;
+
+	ib_modify_qp(sec->qp,
+		     &attr,
+		     IB_QP_STATE);
+
+	if (sec->qp->event_handler && sec->qp->qp_context) {
+		event.element.qp = sec->qp;
+		sec->qp->event_handler(&event,
+				       sec->qp->qp_context);
+	}
+
+	list_for_each_entry(shared_qp_sec,
+			    &sec->shared_qp_list,
+			    shared_qp_list) {
+		struct ib_qp *qp = shared_qp_sec->qp;
+
+		if (qp->event_handler && qp->qp_context) {
+			event.element.qp = qp;
+			event.device = qp->device;
+			qp->event_handler(&event,
+					  qp->qp_context);
+		}
+	}
+}
+
+static inline void check_pkey_qps(struct pkey_index_qp_list *pkey,
+				  struct ib_device *device,
+				  u8 port_num,
+				  u64 subnet_prefix)
+{
+	struct ib_port_pkey *pp, *tmp_pp;
+	bool comp;
+	LIST_HEAD(to_error_list);
+	u16 pkey_val;
+
+	if (!ib_get_cached_pkey(device,
+				port_num,
+				pkey->pkey_index,
+				&pkey_val)) {
+		spin_lock(&pkey->qp_list_lock);
+		list_for_each_entry(pp, &pkey->qp_list, qp_list) {
+			if (atomic_read(&pp->sec->error_list_count))
+				continue;
+
+			if (enforce_qp_pkey_security(pkey_val,
+						     subnet_prefix,
+						     pp->sec)) {
+				atomic_inc(&pp->sec->error_list_count);
+				list_add(&pp->to_error_list,
+					 &to_error_list);
+			}
+		}
+		spin_unlock(&pkey->qp_list_lock);
+	}
+
+	list_for_each_entry_safe(pp,
+				 tmp_pp,
+				 &to_error_list,
+				 to_error_list) {
+		mutex_lock(&pp->sec->mutex);
+		qp_to_error(pp->sec);
+		list_del(&pp->to_error_list);
+		atomic_dec(&pp->sec->error_list_count);
+		comp = pp->sec->destroying;
+		mutex_unlock(&pp->sec->mutex);
+
+		if (comp)
+			complete(&pp->sec->error_complete);
+	}
+}
+
+/* The caller of this function must hold the QP security
+ * mutex.
+ */
+static int port_pkey_list_insert(struct ib_port_pkey *pp)
+{
+	struct pkey_index_qp_list *tmp_pkey;
+	struct pkey_index_qp_list *pkey;
+	struct ib_device *dev;
+	u8 port_num = pp->port_num;
+	int ret = 0;
+
+	if (pp->state != IB_PORT_PKEY_VALID)
+		return 0;
+
+	dev = pp->sec->dev;
+
+	pkey = get_pkey_idx_qp_list(pp);
+
+	if (!pkey) {
+		bool found = false;
+
+		pkey = kzalloc(sizeof(*pkey), GFP_KERNEL);
+		if (!pkey)
+			return -ENOMEM;
+
+		spin_lock(&dev->port_pkey_list[port_num].list_lock);
+		/* Check for the PKey again.  A racing process may
+		 * have created it.
+		 */
+		list_for_each_entry(tmp_pkey,
+				    &dev->port_pkey_list[port_num].pkey_list,
+				    pkey_index_list) {
+			if (tmp_pkey->pkey_index == pp->pkey_index) {
+				kfree(pkey);
+				pkey = tmp_pkey;
+				found = true;
+				break;
+			}
+		}
+
+		if (!found) {
+			pkey->pkey_index = pp->pkey_index;
+			spin_lock_init(&pkey->qp_list_lock);
+			INIT_LIST_HEAD(&pkey->qp_list);
+			list_add(&pkey->pkey_index_list,
+				 &dev->port_pkey_list[port_num].pkey_list);
+		}
+		spin_unlock(&dev->port_pkey_list[port_num].list_lock);
+	}
+
+	spin_lock(&pkey->qp_list_lock);
+	list_add(&pp->qp_list, &pkey->qp_list);
+	spin_unlock(&pkey->qp_list_lock);
+
+	pp->state = IB_PORT_PKEY_LISTED;
+
+	return ret;
+}
+
+/* The caller of this function must hold the QP security
+ * mutex.
+ */
+static void port_pkey_list_remove(struct ib_port_pkey *pp)
+{
+	struct pkey_index_qp_list *pkey;
+
+	if (pp->state != IB_PORT_PKEY_LISTED)
+		return;
+
+	pkey = get_pkey_idx_qp_list(pp);
+
+	spin_lock(&pkey->qp_list_lock);
+	list_del(&pp->qp_list);
+	spin_unlock(&pkey->qp_list_lock);
+
+	/* The setting may still be valid, i.e. after
+	 * a destroy has failed for example.
+	 */
+	pp->state = IB_PORT_PKEY_VALID;
+}
+
+static void destroy_qp_security(struct ib_qp_security *sec)
+{
+	security_ib_free_security(sec->security);
+	kfree(sec->ports_pkeys);
+	kfree(sec);
+}
+
+/* The caller of this function must hold the QP security
+ * mutex.
+ */
+static struct ib_ports_pkeys *get_new_pps(const struct ib_qp *qp,
+					  const struct ib_qp_attr *qp_attr,
+					  int qp_attr_mask)
+{
+	struct ib_ports_pkeys *new_pps;
+	struct ib_ports_pkeys *qp_pps = qp->qp_sec->ports_pkeys;
+
+	new_pps = kzalloc(sizeof(*new_pps), GFP_KERNEL);
+	if (!new_pps)
+		return NULL;
+
+	if (qp_attr_mask & (IB_QP_PKEY_INDEX | IB_QP_PORT)) {
+		if (!qp_pps) {
+			new_pps->main.port_num = qp_attr->port_num;
+			new_pps->main.pkey_index = qp_attr->pkey_index;
+		} else {
+			new_pps->main.port_num = (qp_attr_mask & IB_QP_PORT) ?
+						  qp_attr->port_num :
+						  qp_pps->main.port_num;
+
+			new_pps->main.pkey_index =
+					(qp_attr_mask & IB_QP_PKEY_INDEX) ?
+					 qp_attr->pkey_index :
+					 qp_pps->main.pkey_index;
+		}
+		new_pps->main.state = IB_PORT_PKEY_VALID;
+	} else if (qp_pps) {
+		new_pps->main.port_num = qp_pps->main.port_num;
+		new_pps->main.pkey_index = qp_pps->main.pkey_index;
+		if (qp_pps->main.state != IB_PORT_PKEY_NOT_VALID)
+			new_pps->main.state = IB_PORT_PKEY_VALID;
+	}
+
+	if (qp_attr_mask & IB_QP_ALT_PATH) {
+		new_pps->alt.port_num = qp_attr->alt_port_num;
+		new_pps->alt.pkey_index = qp_attr->alt_pkey_index;
+		new_pps->alt.state = IB_PORT_PKEY_VALID;
+	} else if (qp_pps) {
+		new_pps->alt.port_num = qp_pps->alt.port_num;
+		new_pps->alt.pkey_index = qp_pps->alt.pkey_index;
+		if (qp_pps->alt.state != IB_PORT_PKEY_NOT_VALID)
+			new_pps->alt.state = IB_PORT_PKEY_VALID;
+	}
+
+	new_pps->main.sec = qp->qp_sec;
+	new_pps->alt.sec = qp->qp_sec;
+	return new_pps;
+}
+
+int ib_open_shared_qp_security(struct ib_qp *qp, struct ib_device *dev)
+{
+	struct ib_qp *real_qp = qp->real_qp;
+	int ret;
+
+	ret = ib_create_qp_security(qp, dev);
+
+	if (ret)
+		return ret;
+
+	mutex_lock(&real_qp->qp_sec->mutex);
+	ret = check_qp_port_pkey_settings(real_qp->qp_sec->ports_pkeys,
+					  qp->qp_sec);
+
+	if (ret)
+		goto ret;
+
+	if (qp != real_qp)
+		list_add(&qp->qp_sec->shared_qp_list,
+			 &real_qp->qp_sec->shared_qp_list);
+ret:
+	mutex_unlock(&real_qp->qp_sec->mutex);
+	if (ret)
+		destroy_qp_security(qp->qp_sec);
+
+	return ret;
+}
+
+void ib_close_shared_qp_security(struct ib_qp_security *sec)
+{
+	struct ib_qp *real_qp = sec->qp->real_qp;
+
+	mutex_lock(&real_qp->qp_sec->mutex);
+	list_del(&sec->shared_qp_list);
+	mutex_unlock(&real_qp->qp_sec->mutex);
+
+	destroy_qp_security(sec);
+}
+
+int ib_create_qp_security(struct ib_qp *qp, struct ib_device *dev)
+{
+	int ret;
+
+	qp->qp_sec = kzalloc(sizeof(*qp->qp_sec), GFP_KERNEL);
+	if (!qp->qp_sec)
+		return -ENOMEM;
+
+	qp->qp_sec->qp = qp;
+	qp->qp_sec->dev = dev;
+	mutex_init(&qp->qp_sec->mutex);
+	INIT_LIST_HEAD(&qp->qp_sec->shared_qp_list);
+	atomic_set(&qp->qp_sec->error_list_count, 0);
+	init_completion(&qp->qp_sec->error_complete);
+	ret = security_ib_alloc_security(&qp->qp_sec->security);
+	if (ret)
+		kfree(qp->qp_sec);
+
+	return ret;
+}
+EXPORT_SYMBOL(ib_create_qp_security);
+
+void ib_destroy_qp_security_begin(struct ib_qp_security *sec)
+{
+	mutex_lock(&sec->mutex);
+
+	/* Remove the QP from the lists so it won't get added to
+	 * a to_error_list during the destroy process.
+	 */
+	if (sec->ports_pkeys) {
+		port_pkey_list_remove(&sec->ports_pkeys->main);
+		port_pkey_list_remove(&sec->ports_pkeys->alt);
+	}
+
+	/* If the QP is already in one or more of those lists
+	 * the destroying flag will ensure the to error flow
+	 * doesn't operate on an undefined QP.
+	 */
+	sec->destroying = true;
+
+	/* Record the error list count to know how many completions
+	 * to wait for.
+	 */
+	sec->error_comps_pending = atomic_read(&sec->error_list_count);
+
+	mutex_unlock(&sec->mutex);
+}
+
+void ib_destroy_qp_security_abort(struct ib_qp_security *sec)
+{
+	int ret;
+	int i;
+
+	/* If a concurrent cache update is in progress this
+	 * QP security could be marked for an error state
+	 * transition.  Wait for this to complete.
+	 */
+	for (i = 0; i < sec->error_comps_pending; i++)
+		wait_for_completion(&sec->error_complete);
+
+	mutex_lock(&sec->mutex);
+	sec->destroying = false;
+
+	/* Restore the position in the lists and verify
+	 * access is still allowed in case a cache update
+	 * occurred while attempting to destroy.
+	 *
+	 * Because these setting were listed already
+	 * and removed during ib_destroy_qp_security_begin
+	 * we know the pkey_index_qp_list for the PKey
+	 * already exists so port_pkey_list_insert won't fail.
+	 */
+	if (sec->ports_pkeys) {
+		port_pkey_list_insert(&sec->ports_pkeys->main);
+		port_pkey_list_insert(&sec->ports_pkeys->alt);
+	}
+
+	ret = check_qp_port_pkey_settings(sec->ports_pkeys, sec);
+	if (ret)
+		qp_to_error(sec);
+
+	mutex_unlock(&sec->mutex);
+}
+
+void ib_destroy_qp_security_end(struct ib_qp_security *sec)
+{
+	int i;
+
+	/* If a concurrent cache update is occurring we must
+	 * wait until this QP security structure is processed
+	 * in the QP to error flow before destroying it because
+	 * the to_error_list is in use.
+	 */
+	for (i = 0; i < sec->error_comps_pending; i++)
+		wait_for_completion(&sec->error_complete);
+
+	destroy_qp_security(sec);
+}
+
+void ib_security_cache_change(struct ib_device *device,
+			      u8 port_num,
+			      u64 subnet_prefix)
+{
+	struct pkey_index_qp_list *pkey;
+
+	list_for_each_entry(pkey,
+			    &device->port_pkey_list[port_num].pkey_list,
+			    pkey_index_list) {
+		check_pkey_qps(pkey,
+			       device,
+			       port_num,
+			       subnet_prefix);
+	}
+}
+
+void ib_security_destroy_port_pkey_list(struct ib_device *device)
+{
+	struct pkey_index_qp_list *pkey, *tmp_pkey;
+	int i;
+
+	for (i = rdma_start_port(device); i <= rdma_end_port(device); i++) {
+		spin_lock(&device->port_pkey_list[i].list_lock);
+		list_for_each_entry_safe(pkey,
+					 tmp_pkey,
+					 &device->port_pkey_list[i].pkey_list,
+					 pkey_index_list) {
+			list_del(&pkey->pkey_index_list);
+			kfree(pkey);
+		}
+		spin_unlock(&device->port_pkey_list[i].list_lock);
+	}
+}
+
+int ib_security_modify_qp(struct ib_qp *qp,
+			  struct ib_qp_attr *qp_attr,
+			  int qp_attr_mask,
+			  struct ib_udata *udata)
+{
+	int ret = 0;
+	struct ib_ports_pkeys *tmp_pps;
+	struct ib_ports_pkeys *new_pps;
+	bool special_qp = (qp->qp_type == IB_QPT_SMI ||
+			   qp->qp_type == IB_QPT_GSI ||
+			   qp->qp_type >= IB_QPT_RESERVED1);
+	bool pps_change = ((qp_attr_mask & (IB_QP_PKEY_INDEX | IB_QP_PORT)) ||
+			   (qp_attr_mask & IB_QP_ALT_PATH));
+
+	if (pps_change && !special_qp) {
+		mutex_lock(&qp->qp_sec->mutex);
+		new_pps = get_new_pps(qp,
+				      qp_attr,
+				      qp_attr_mask);
+
+		/* Add this QP to the lists for the new port
+		 * and pkey settings before checking for permission
+		 * in case there is a concurrent cache update
+		 * occurring.  Walking the list for a cache change
+		 * doesn't acquire the security mutex unless it's
+		 * sending the QP to error.
+		 */
+		ret = port_pkey_list_insert(&new_pps->main);
+
+		if (!ret)
+			ret = port_pkey_list_insert(&new_pps->alt);
+
+		if (!ret)
+			ret = check_qp_port_pkey_settings(new_pps,
+							  qp->qp_sec);
+	}
+
+	if (!ret)
+		ret = qp->device->modify_qp(qp->real_qp,
+					    qp_attr,
+					    qp_attr_mask,
+					    udata);
+
+	if (pps_change && !special_qp) {
+		/* Clean up the lists and free the appropriate
+		 * ports_pkeys structure.
+		 */
+		if (ret) {
+			tmp_pps = new_pps;
+		} else {
+			tmp_pps = qp->qp_sec->ports_pkeys;
+			qp->qp_sec->ports_pkeys = new_pps;
+		}
+
+		if (tmp_pps) {
+			port_pkey_list_remove(&tmp_pps->main);
+			port_pkey_list_remove(&tmp_pps->alt);
+		}
+		kfree(tmp_pps);
+		mutex_unlock(&qp->qp_sec->mutex);
+	}
+	return ret;
+}
+EXPORT_SYMBOL(ib_security_modify_qp);
+
+int ib_security_pkey_access(struct ib_device *dev,
+			    u8 port_num,
+			    u16 pkey_index,
+			    void *sec)
+{
+	u64 subnet_prefix;
+	u16 pkey;
+	int ret;
+
+	ret = ib_get_cached_pkey(dev, port_num, pkey_index, &pkey);
+	if (ret)
+		return ret;
+
+	ret = ib_get_cached_subnet_prefix(dev, port_num, &subnet_prefix);
+
+	if (ret)
+		return ret;
+
+	return security_ib_pkey_access(sec, subnet_prefix, pkey);
+}
+EXPORT_SYMBOL(ib_security_pkey_access);
+
+static int ib_mad_agent_security_change(struct notifier_block *nb,
+					unsigned long event,
+					void *data)
+{
+	struct ib_mad_agent *ag = container_of(nb, struct ib_mad_agent, lsm_nb);
+
+	if (event != LSM_POLICY_CHANGE)
+		return NOTIFY_DONE;
+
+	ag->smp_allowed = !security_ib_endport_manage_subnet(ag->security,
+							     ag->device->name,
+							     ag->port_num);
+
+	return NOTIFY_OK;
+}
+
+int ib_mad_agent_security_setup(struct ib_mad_agent *agent,
+				enum ib_qp_type qp_type)
+{
+	int ret;
+
+	ret = security_ib_alloc_security(&agent->security);
+	if (ret)
+		return ret;
+
+	if (qp_type != IB_QPT_SMI)
+		return 0;
+
+	ret = security_ib_endport_manage_subnet(agent->security,
+						agent->device->name,
+						agent->port_num);
+	if (ret)
+		return ret;
+
+	agent->lsm_nb.notifier_call = ib_mad_agent_security_change;
+	ret = register_lsm_notifier(&agent->lsm_nb);
+	if (ret)
+		return ret;
+
+	agent->smp_allowed = true;
+	agent->lsm_nb_reg = true;
+	return 0;
+}
+
+void ib_mad_agent_security_cleanup(struct ib_mad_agent *agent)
+{
+	security_ib_free_security(agent->security);
+	if (agent->lsm_nb_reg)
+		unregister_lsm_notifier(&agent->lsm_nb);
+}
+
+int ib_mad_enforce_security(struct ib_mad_agent_private *map, u16 pkey_index)
+{
+	int ret;
+
+	if (map->agent.qp->qp_type == IB_QPT_SMI && !map->agent.smp_allowed)
+		return -EACCES;
+
+	ret = ib_security_pkey_access(map->agent.device,
+				      map->agent.port_num,
+				      pkey_index,
+				      map->agent.security);
+
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+#endif /* CONFIG_SECURITY_INFINIBAND */
diff --git a/drivers/infiniband/core/uverbs_cmd.c b/drivers/infiniband/core/uverbs_cmd.c
index 70b7fb156414..8ba9bfb073d1 100644
--- a/drivers/infiniband/core/uverbs_cmd.c
+++ b/drivers/infiniband/core/uverbs_cmd.c
@@ -1508,6 +1508,10 @@ static int create_qp(struct ib_uverbs_file *file,
 	}
 
 	if (cmd->qp_type != IB_QPT_XRC_TGT) {
+		ret = ib_create_qp_security(qp, device);
+		if (ret)
+			goto err_cb;
+
 		qp->real_qp	  = qp;
 		qp->device	  = device;
 		qp->pd		  = pd;
@@ -1931,6 +1935,11 @@ static int modify_qp(struct ib_uverbs_file *file,
 		goto out;
 	}
 
+	if (!rdma_is_port_valid(qp->device, cmd->base.port_num)) {
+		ret = -EINVAL;
+		goto release_qp;
+	}
+
 	attr->qp_state		  = cmd->base.qp_state;
 	attr->cur_qp_state	  = cmd->base.cur_qp_state;
 	attr->path_mtu		  = cmd->base.path_mtu;
@@ -2002,14 +2011,17 @@ static int modify_qp(struct ib_uverbs_file *file,
 			if (ret)
 				goto release_qp;
 		}
-		ret = qp->device->modify_qp(qp, attr,
+		ret = ib_security_modify_qp(qp,
+					    attr,
 					    modify_qp_mask(qp->qp_type,
 							   cmd->base.attr_mask),
 					    udata);
 	} else {
-		ret = ib_modify_qp(qp, attr,
-				   modify_qp_mask(qp->qp_type,
-						  cmd->base.attr_mask));
+		ret = ib_security_modify_qp(qp,
+					    attr,
+					    modify_qp_mask(qp->qp_type,
+							   cmd->base.attr_mask),
+					    NULL);
 	}
 
 release_qp:
@@ -2541,6 +2553,9 @@ ssize_t ib_uverbs_create_ah(struct ib_uverbs_file *file,
 	if (copy_from_user(&cmd, buf, sizeof cmd))
 		return -EFAULT;
 
+	if (!rdma_is_port_valid(ib_dev, cmd.attr.port_num))
+		return -EINVAL;
+
 	INIT_UDATA(&udata, buf + sizeof(cmd),
 		   (unsigned long)cmd.response + sizeof(resp),
 		   in_len - sizeof(cmd), out_len - sizeof(resp));
diff --git a/drivers/infiniband/core/verbs.c b/drivers/infiniband/core/verbs.c
index 4792f5209ac2..c973a83c898b 100644
--- a/drivers/infiniband/core/verbs.c
+++ b/drivers/infiniband/core/verbs.c
@@ -44,6 +44,7 @@
 #include <linux/in.h>
 #include <linux/in6.h>
 #include <net/addrconf.h>
+#include <linux/security.h>
 
 #include <rdma/ib_verbs.h>
 #include <rdma/ib_cache.h>
@@ -713,12 +714,20 @@ static struct ib_qp *__ib_open_qp(struct ib_qp *real_qp,
 {
 	struct ib_qp *qp;
 	unsigned long flags;
+	int err;
 
 	qp = kzalloc(sizeof *qp, GFP_KERNEL);
 	if (!qp)
 		return ERR_PTR(-ENOMEM);
 
 	qp->real_qp = real_qp;
+	err = ib_open_shared_qp_security(qp, real_qp->device);
+	if (err) {
+		kfree(qp);
+		return ERR_PTR(err);
+	}
+
+	qp->real_qp = real_qp;
 	atomic_inc(&real_qp->usecnt);
 	qp->device = real_qp->device;
 	qp->event_handler = event_handler;
@@ -804,6 +813,12 @@ struct ib_qp *ib_create_qp(struct ib_pd *pd,
 	if (IS_ERR(qp))
 		return qp;
 
+	ret = ib_create_qp_security(qp, device);
+	if (ret) {
+		ib_destroy_qp(qp);
+		return ERR_PTR(ret);
+	}
+
 	qp->device     = device;
 	qp->real_qp    = qp;
 	qp->uobject    = NULL;
@@ -1266,7 +1281,7 @@ int ib_modify_qp(struct ib_qp *qp,
 			return ret;
 	}
 
-	return qp->device->modify_qp(qp->real_qp, qp_attr, qp_attr_mask, NULL);
+	return ib_security_modify_qp(qp->real_qp, qp_attr, qp_attr_mask, NULL);
 }
 EXPORT_SYMBOL(ib_modify_qp);
 
@@ -1295,6 +1310,7 @@ int ib_close_qp(struct ib_qp *qp)
 	spin_unlock_irqrestore(&real_qp->device->event_handler_lock, flags);
 
 	atomic_dec(&real_qp->usecnt);
+	ib_close_shared_qp_security(qp->qp_sec);
 	kfree(qp);
 
 	return 0;
@@ -1335,6 +1351,7 @@ int ib_destroy_qp(struct ib_qp *qp)
 	struct ib_cq *scq, *rcq;
 	struct ib_srq *srq;
 	struct ib_rwq_ind_table *ind_tbl;
+	struct ib_qp_security *sec;
 	int ret;
 
 	WARN_ON_ONCE(qp->mrs_used > 0);
@@ -1350,6 +1367,9 @@ int ib_destroy_qp(struct ib_qp *qp)
 	rcq  = qp->recv_cq;
 	srq  = qp->srq;
 	ind_tbl = qp->rwq_ind_tbl;
+	sec  = qp->qp_sec;
+	if (sec)
+		ib_destroy_qp_security_begin(sec);
 
 	if (!qp->uobject)
 		rdma_rw_cleanup_mrs(qp);
@@ -1366,6 +1386,11 @@ int ib_destroy_qp(struct ib_qp *qp)
 			atomic_dec(&srq->usecnt);
 		if (ind_tbl)
 			atomic_dec(&ind_tbl->usecnt);
+		if (sec)
+			ib_destroy_qp_security_end(sec);
+	} else {
+		if (sec)
+			ib_destroy_qp_security_abort(sec);
 	}
 
 	return ret;
diff --git a/drivers/infiniband/hw/bnxt_re/bnxt_re.h b/drivers/infiniband/hw/bnxt_re/bnxt_re.h
index ebf7be8d4139..08772836fded 100644
--- a/drivers/infiniband/hw/bnxt_re/bnxt_re.h
+++ b/drivers/infiniband/hw/bnxt_re/bnxt_re.h
@@ -56,6 +56,10 @@
 #define BNXT_RE_MAX_SRQC_COUNT		(64 * 1024)
 #define BNXT_RE_MAX_CQ_COUNT		(64 * 1024)
 
+#define BNXT_RE_UD_QP_HW_STALL		0x400000
+
+#define BNXT_RE_RQ_WQE_THRESHOLD	32
+
 struct bnxt_re_work {
 	struct work_struct	work;
 	unsigned long		event;
diff --git a/drivers/infiniband/hw/bnxt_re/ib_verbs.c b/drivers/infiniband/hw/bnxt_re/ib_verbs.c
index 7ba9e699d7ab..c7bd68311d0c 100644
--- a/drivers/infiniband/hw/bnxt_re/ib_verbs.c
+++ b/drivers/infiniband/hw/bnxt_re/ib_verbs.c
@@ -61,6 +61,48 @@
 #include "ib_verbs.h"
 #include <rdma/bnxt_re-abi.h>
 
+static int __from_ib_access_flags(int iflags)
+{
+	int qflags = 0;
+
+	if (iflags & IB_ACCESS_LOCAL_WRITE)
+		qflags |= BNXT_QPLIB_ACCESS_LOCAL_WRITE;
+	if (iflags & IB_ACCESS_REMOTE_READ)
+		qflags |= BNXT_QPLIB_ACCESS_REMOTE_READ;
+	if (iflags & IB_ACCESS_REMOTE_WRITE)
+		qflags |= BNXT_QPLIB_ACCESS_REMOTE_WRITE;
+	if (iflags & IB_ACCESS_REMOTE_ATOMIC)
+		qflags |= BNXT_QPLIB_ACCESS_REMOTE_ATOMIC;
+	if (iflags & IB_ACCESS_MW_BIND)
+		qflags |= BNXT_QPLIB_ACCESS_MW_BIND;
+	if (iflags & IB_ZERO_BASED)
+		qflags |= BNXT_QPLIB_ACCESS_ZERO_BASED;
+	if (iflags & IB_ACCESS_ON_DEMAND)
+		qflags |= BNXT_QPLIB_ACCESS_ON_DEMAND;
+	return qflags;
+};
+
+static enum ib_access_flags __to_ib_access_flags(int qflags)
+{
+	enum ib_access_flags iflags = 0;
+
+	if (qflags & BNXT_QPLIB_ACCESS_LOCAL_WRITE)
+		iflags |= IB_ACCESS_LOCAL_WRITE;
+	if (qflags & BNXT_QPLIB_ACCESS_REMOTE_WRITE)
+		iflags |= IB_ACCESS_REMOTE_WRITE;
+	if (qflags & BNXT_QPLIB_ACCESS_REMOTE_READ)
+		iflags |= IB_ACCESS_REMOTE_READ;
+	if (qflags & BNXT_QPLIB_ACCESS_REMOTE_ATOMIC)
+		iflags |= IB_ACCESS_REMOTE_ATOMIC;
+	if (qflags & BNXT_QPLIB_ACCESS_MW_BIND)
+		iflags |= IB_ACCESS_MW_BIND;
+	if (qflags & BNXT_QPLIB_ACCESS_ZERO_BASED)
+		iflags |= IB_ZERO_BASED;
+	if (qflags & BNXT_QPLIB_ACCESS_ON_DEMAND)
+		iflags |= IB_ACCESS_ON_DEMAND;
+	return iflags;
+};
+
 static int bnxt_re_build_sgl(struct ib_sge *ib_sg_list,
 			     struct bnxt_qplib_sge *sg_list, int num)
 {
@@ -149,8 +191,8 @@ int bnxt_re_query_device(struct ib_device *ibdev,
 	ib_attr->max_total_mcast_qp_attach = 0;
 	ib_attr->max_ah = dev_attr->max_ah;
 
-	ib_attr->max_fmr = dev_attr->max_fmr;
-	ib_attr->max_map_per_fmr = 1;	/* ? */
+	ib_attr->max_fmr = 0;
+	ib_attr->max_map_per_fmr = 0;
 
 	ib_attr->max_srq = dev_attr->max_srq;
 	ib_attr->max_srq_wr = dev_attr->max_srq_wqes;
@@ -410,6 +452,158 @@ enum rdma_link_layer bnxt_re_get_link_layer(struct ib_device *ibdev,
 	return IB_LINK_LAYER_ETHERNET;
 }
 
+#define	BNXT_RE_FENCE_PBL_SIZE	DIV_ROUND_UP(BNXT_RE_FENCE_BYTES, PAGE_SIZE)
+
+static void bnxt_re_create_fence_wqe(struct bnxt_re_pd *pd)
+{
+	struct bnxt_re_fence_data *fence = &pd->fence;
+	struct ib_mr *ib_mr = &fence->mr->ib_mr;
+	struct bnxt_qplib_swqe *wqe = &fence->bind_wqe;
+
+	memset(wqe, 0, sizeof(*wqe));
+	wqe->type = BNXT_QPLIB_SWQE_TYPE_BIND_MW;
+	wqe->wr_id = BNXT_QPLIB_FENCE_WRID;
+	wqe->flags |= BNXT_QPLIB_SWQE_FLAGS_SIGNAL_COMP;
+	wqe->flags |= BNXT_QPLIB_SWQE_FLAGS_UC_FENCE;
+	wqe->bind.zero_based = false;
+	wqe->bind.parent_l_key = ib_mr->lkey;
+	wqe->bind.va = (u64)(unsigned long)fence->va;
+	wqe->bind.length = fence->size;
+	wqe->bind.access_cntl = __from_ib_access_flags(IB_ACCESS_REMOTE_READ);
+	wqe->bind.mw_type = SQ_BIND_MW_TYPE_TYPE1;
+
+	/* Save the initial rkey in fence structure for now;
+	 * wqe->bind.r_key will be set at (re)bind time.
+	 */
+	fence->bind_rkey = ib_inc_rkey(fence->mw->rkey);
+}
+
+static int bnxt_re_bind_fence_mw(struct bnxt_qplib_qp *qplib_qp)
+{
+	struct bnxt_re_qp *qp = container_of(qplib_qp, struct bnxt_re_qp,
+					     qplib_qp);
+	struct ib_pd *ib_pd = qp->ib_qp.pd;
+	struct bnxt_re_pd *pd = container_of(ib_pd, struct bnxt_re_pd, ib_pd);
+	struct bnxt_re_fence_data *fence = &pd->fence;
+	struct bnxt_qplib_swqe *fence_wqe = &fence->bind_wqe;
+	struct bnxt_qplib_swqe wqe;
+	int rc;
+
+	memcpy(&wqe, fence_wqe, sizeof(wqe));
+	wqe.bind.r_key = fence->bind_rkey;
+	fence->bind_rkey = ib_inc_rkey(fence->bind_rkey);
+
+	dev_dbg(rdev_to_dev(qp->rdev),
+		"Posting bind fence-WQE: rkey: %#x QP: %d PD: %p\n",
+		wqe.bind.r_key, qp->qplib_qp.id, pd);
+	rc = bnxt_qplib_post_send(&qp->qplib_qp, &wqe);
+	if (rc) {
+		dev_err(rdev_to_dev(qp->rdev), "Failed to bind fence-WQE\n");
+		return rc;
+	}
+	bnxt_qplib_post_send_db(&qp->qplib_qp);
+
+	return rc;
+}
+
+static void bnxt_re_destroy_fence_mr(struct bnxt_re_pd *pd)
+{
+	struct bnxt_re_fence_data *fence = &pd->fence;
+	struct bnxt_re_dev *rdev = pd->rdev;
+	struct device *dev = &rdev->en_dev->pdev->dev;
+	struct bnxt_re_mr *mr = fence->mr;
+
+	if (fence->mw) {
+		bnxt_re_dealloc_mw(fence->mw);
+		fence->mw = NULL;
+	}
+	if (mr) {
+		if (mr->ib_mr.rkey)
+			bnxt_qplib_dereg_mrw(&rdev->qplib_res, &mr->qplib_mr,
+					     true);
+		if (mr->ib_mr.lkey)
+			bnxt_qplib_free_mrw(&rdev->qplib_res, &mr->qplib_mr);
+		kfree(mr);
+		fence->mr = NULL;
+	}
+	if (fence->dma_addr) {
+		dma_unmap_single(dev, fence->dma_addr, BNXT_RE_FENCE_BYTES,
+				 DMA_BIDIRECTIONAL);
+		fence->dma_addr = 0;
+	}
+}
+
+static int bnxt_re_create_fence_mr(struct bnxt_re_pd *pd)
+{
+	int mr_access_flags = IB_ACCESS_LOCAL_WRITE | IB_ACCESS_MW_BIND;
+	struct bnxt_re_fence_data *fence = &pd->fence;
+	struct bnxt_re_dev *rdev = pd->rdev;
+	struct device *dev = &rdev->en_dev->pdev->dev;
+	struct bnxt_re_mr *mr = NULL;
+	dma_addr_t dma_addr = 0;
+	struct ib_mw *mw;
+	u64 pbl_tbl;
+	int rc;
+
+	dma_addr = dma_map_single(dev, fence->va, BNXT_RE_FENCE_BYTES,
+				  DMA_BIDIRECTIONAL);
+	rc = dma_mapping_error(dev, dma_addr);
+	if (rc) {
+		dev_err(rdev_to_dev(rdev), "Failed to dma-map fence-MR-mem\n");
+		rc = -EIO;
+		fence->dma_addr = 0;
+		goto fail;
+	}
+	fence->dma_addr = dma_addr;
+
+	/* Allocate a MR */
+	mr = kzalloc(sizeof(*mr), GFP_KERNEL);
+	if (!mr) {
+		rc = -ENOMEM;
+		goto fail;
+	}
+	fence->mr = mr;
+	mr->rdev = rdev;
+	mr->qplib_mr.pd = &pd->qplib_pd;
+	mr->qplib_mr.type = CMDQ_ALLOCATE_MRW_MRW_FLAGS_PMR;
+	mr->qplib_mr.flags = __from_ib_access_flags(mr_access_flags);
+	rc = bnxt_qplib_alloc_mrw(&rdev->qplib_res, &mr->qplib_mr);
+	if (rc) {
+		dev_err(rdev_to_dev(rdev), "Failed to alloc fence-HW-MR\n");
+		goto fail;
+	}
+
+	/* Register MR */
+	mr->ib_mr.lkey = mr->qplib_mr.lkey;
+	mr->qplib_mr.va = (u64)(unsigned long)fence->va;
+	mr->qplib_mr.total_size = BNXT_RE_FENCE_BYTES;
+	pbl_tbl = dma_addr;
+	rc = bnxt_qplib_reg_mr(&rdev->qplib_res, &mr->qplib_mr, &pbl_tbl,
+			       BNXT_RE_FENCE_PBL_SIZE, false);
+	if (rc) {
+		dev_err(rdev_to_dev(rdev), "Failed to register fence-MR\n");
+		goto fail;
+	}
+	mr->ib_mr.rkey = mr->qplib_mr.rkey;
+
+	/* Create a fence MW only for kernel consumers */
+	mw = bnxt_re_alloc_mw(&pd->ib_pd, IB_MW_TYPE_1, NULL);
+	if (!mw) {
+		dev_err(rdev_to_dev(rdev),
+			"Failed to create fence-MW for PD: %p\n", pd);
+		rc = -EINVAL;
+		goto fail;
+	}
+	fence->mw = mw;
+
+	bnxt_re_create_fence_wqe(pd);
+	return 0;
+
+fail:
+	bnxt_re_destroy_fence_mr(pd);
+	return rc;
+}
+
 /* Protection Domains */
 int bnxt_re_dealloc_pd(struct ib_pd *ib_pd)
 {
@@ -417,6 +611,7 @@ int bnxt_re_dealloc_pd(struct ib_pd *ib_pd)
 	struct bnxt_re_dev *rdev = pd->rdev;
 	int rc;
 
+	bnxt_re_destroy_fence_mr(pd);
 	if (ib_pd->uobject && pd->dpi.dbr) {
 		struct ib_ucontext *ib_uctx = ib_pd->uobject->context;
 		struct bnxt_re_ucontext *ucntx;
@@ -498,6 +693,10 @@ struct ib_pd *bnxt_re_alloc_pd(struct ib_device *ibdev,
 		}
 	}
 
+	if (!udata)
+		if (bnxt_re_create_fence_mr(pd))
+			dev_warn(rdev_to_dev(rdev),
+				 "Failed to create Fence-MR\n");
 	return &pd->ib_pd;
 dbfail:
 	(void)bnxt_qplib_dealloc_pd(&rdev->qplib_res, &rdev->qplib_res.pd_tbl,
@@ -849,12 +1048,16 @@ static struct bnxt_re_qp *bnxt_re_create_shadow_qp
 	/* Shadow QP SQ depth should be same as QP1 RQ depth */
 	qp->qplib_qp.sq.max_wqe = qp1_qp->rq.max_wqe;
 	qp->qplib_qp.sq.max_sge = 2;
+	/* Q full delta can be 1 since it is internal QP */
+	qp->qplib_qp.sq.q_full_delta = 1;
 
 	qp->qplib_qp.scq = qp1_qp->scq;
 	qp->qplib_qp.rcq = qp1_qp->rcq;
 
 	qp->qplib_qp.rq.max_wqe = qp1_qp->rq.max_wqe;
 	qp->qplib_qp.rq.max_sge = qp1_qp->rq.max_sge;
+	/* Q full delta can be 1 since it is internal QP */
+	qp->qplib_qp.rq.q_full_delta = 1;
 
 	qp->qplib_qp.mtu = qp1_qp->mtu;
 
@@ -917,10 +1120,6 @@ struct ib_qp *bnxt_re_create_qp(struct ib_pd *ib_pd,
 	qp->qplib_qp.sig_type = ((qp_init_attr->sq_sig_type ==
 				  IB_SIGNAL_ALL_WR) ? true : false);
 
-	entries = roundup_pow_of_two(qp_init_attr->cap.max_send_wr + 1);
-	qp->qplib_qp.sq.max_wqe = min_t(u32, entries,
-					dev_attr->max_qp_wqes + 1);
-
 	qp->qplib_qp.sq.max_sge = qp_init_attr->cap.max_send_sge;
 	if (qp->qplib_qp.sq.max_sge > dev_attr->max_qp_sges)
 		qp->qplib_qp.sq.max_sge = dev_attr->max_qp_sges;
@@ -959,6 +1158,9 @@ struct ib_qp *bnxt_re_create_qp(struct ib_pd *ib_pd,
 		qp->qplib_qp.rq.max_wqe = min_t(u32, entries,
 						dev_attr->max_qp_wqes + 1);
 
+		qp->qplib_qp.rq.q_full_delta = qp->qplib_qp.rq.max_wqe -
+						qp_init_attr->cap.max_recv_wr;
+
 		qp->qplib_qp.rq.max_sge = qp_init_attr->cap.max_recv_sge;
 		if (qp->qplib_qp.rq.max_sge > dev_attr->max_qp_sges)
 			qp->qplib_qp.rq.max_sge = dev_attr->max_qp_sges;
@@ -967,6 +1169,12 @@ struct ib_qp *bnxt_re_create_qp(struct ib_pd *ib_pd,
 	qp->qplib_qp.mtu = ib_mtu_enum_to_int(iboe_get_mtu(rdev->netdev->mtu));
 
 	if (qp_init_attr->qp_type == IB_QPT_GSI) {
+		/* Allocate 1 more than what's provided */
+		entries = roundup_pow_of_two(qp_init_attr->cap.max_send_wr + 1);
+		qp->qplib_qp.sq.max_wqe = min_t(u32, entries,
+						dev_attr->max_qp_wqes + 1);
+		qp->qplib_qp.sq.q_full_delta = qp->qplib_qp.sq.max_wqe -
+						qp_init_attr->cap.max_send_wr;
 		qp->qplib_qp.rq.max_sge = dev_attr->max_qp_sges;
 		if (qp->qplib_qp.rq.max_sge > dev_attr->max_qp_sges)
 			qp->qplib_qp.rq.max_sge = dev_attr->max_qp_sges;
@@ -1006,6 +1214,22 @@ struct ib_qp *bnxt_re_create_qp(struct ib_pd *ib_pd,
 		}
 
 	} else {
+		/* Allocate 128 + 1 more than what's provided */
+		entries = roundup_pow_of_two(qp_init_attr->cap.max_send_wr +
+					     BNXT_QPLIB_RESERVED_QP_WRS + 1);
+		qp->qplib_qp.sq.max_wqe = min_t(u32, entries,
+						dev_attr->max_qp_wqes +
+						BNXT_QPLIB_RESERVED_QP_WRS + 1);
+		qp->qplib_qp.sq.q_full_delta = BNXT_QPLIB_RESERVED_QP_WRS + 1;
+
+		/*
+		 * Reserving one slot for Phantom WQE. Application can
+		 * post one extra entry in this case. But allowing this to avoid
+		 * unexpected Queue full condition
+		 */
+
+		qp->qplib_qp.sq.q_full_delta -= 1;
+
 		qp->qplib_qp.max_rd_atomic = dev_attr->max_qp_rd_atom;
 		qp->qplib_qp.max_dest_rd_atomic = dev_attr->max_qp_init_rd_atom;
 		if (udata) {
@@ -1025,6 +1249,7 @@ struct ib_qp *bnxt_re_create_qp(struct ib_pd *ib_pd,
 
 	qp->ib_qp.qp_num = qp->qplib_qp.id;
 	spin_lock_init(&qp->sq_lock);
+	spin_lock_init(&qp->rq_lock);
 
 	if (udata) {
 		struct bnxt_re_qp_resp resp;
@@ -1129,48 +1354,6 @@ static enum ib_mtu __to_ib_mtu(u32 mtu)
 	}
 }
 
-static int __from_ib_access_flags(int iflags)
-{
-	int qflags = 0;
-
-	if (iflags & IB_ACCESS_LOCAL_WRITE)
-		qflags |= BNXT_QPLIB_ACCESS_LOCAL_WRITE;
-	if (iflags & IB_ACCESS_REMOTE_READ)
-		qflags |= BNXT_QPLIB_ACCESS_REMOTE_READ;
-	if (iflags & IB_ACCESS_REMOTE_WRITE)
-		qflags |= BNXT_QPLIB_ACCESS_REMOTE_WRITE;
-	if (iflags & IB_ACCESS_REMOTE_ATOMIC)
-		qflags |= BNXT_QPLIB_ACCESS_REMOTE_ATOMIC;
-	if (iflags & IB_ACCESS_MW_BIND)
-		qflags |= BNXT_QPLIB_ACCESS_MW_BIND;
-	if (iflags & IB_ZERO_BASED)
-		qflags |= BNXT_QPLIB_ACCESS_ZERO_BASED;
-	if (iflags & IB_ACCESS_ON_DEMAND)
-		qflags |= BNXT_QPLIB_ACCESS_ON_DEMAND;
-	return qflags;
-};
-
-static enum ib_access_flags __to_ib_access_flags(int qflags)
-{
-	enum ib_access_flags iflags = 0;
-
-	if (qflags & BNXT_QPLIB_ACCESS_LOCAL_WRITE)
-		iflags |= IB_ACCESS_LOCAL_WRITE;
-	if (qflags & BNXT_QPLIB_ACCESS_REMOTE_WRITE)
-		iflags |= IB_ACCESS_REMOTE_WRITE;
-	if (qflags & BNXT_QPLIB_ACCESS_REMOTE_READ)
-		iflags |= IB_ACCESS_REMOTE_READ;
-	if (qflags & BNXT_QPLIB_ACCESS_REMOTE_ATOMIC)
-		iflags |= IB_ACCESS_REMOTE_ATOMIC;
-	if (qflags & BNXT_QPLIB_ACCESS_MW_BIND)
-		iflags |= IB_ACCESS_MW_BIND;
-	if (qflags & BNXT_QPLIB_ACCESS_ZERO_BASED)
-		iflags |= IB_ZERO_BASED;
-	if (qflags & BNXT_QPLIB_ACCESS_ON_DEMAND)
-		iflags |= IB_ACCESS_ON_DEMAND;
-	return iflags;
-};
-
 static int bnxt_re_modify_shadow_qp(struct bnxt_re_dev *rdev,
 				    struct bnxt_re_qp *qp1_qp,
 				    int qp_attr_mask)
@@ -1378,11 +1561,21 @@ int bnxt_re_modify_qp(struct ib_qp *ib_qp, struct ib_qp_attr *qp_attr,
 		entries = roundup_pow_of_two(qp_attr->cap.max_send_wr);
 		qp->qplib_qp.sq.max_wqe = min_t(u32, entries,
 						dev_attr->max_qp_wqes + 1);
+		qp->qplib_qp.sq.q_full_delta = qp->qplib_qp.sq.max_wqe -
+						qp_attr->cap.max_send_wr;
+		/*
+		 * Reserving one slot for Phantom WQE. Some application can
+		 * post one extra entry in this case. Allowing this to avoid
+		 * unexpected Queue full condition
+		 */
+		qp->qplib_qp.sq.q_full_delta -= 1;
 		qp->qplib_qp.sq.max_sge = qp_attr->cap.max_send_sge;
 		if (qp->qplib_qp.rq.max_wqe) {
 			entries = roundup_pow_of_two(qp_attr->cap.max_recv_wr);
 			qp->qplib_qp.rq.max_wqe =
 				min_t(u32, entries, dev_attr->max_qp_wqes + 1);
+			qp->qplib_qp.rq.q_full_delta = qp->qplib_qp.rq.max_wqe -
+						       qp_attr->cap.max_recv_wr;
 			qp->qplib_qp.rq.max_sge = qp_attr->cap.max_recv_sge;
 		} else {
 			/* SRQ was used prior, just ignore the RQ caps */
@@ -1883,6 +2076,22 @@ static int bnxt_re_copy_wr_payload(struct bnxt_re_dev *rdev,
 	return payload_sz;
 }
 
+static void bnxt_ud_qp_hw_stall_workaround(struct bnxt_re_qp *qp)
+{
+	if ((qp->ib_qp.qp_type == IB_QPT_UD ||
+	     qp->ib_qp.qp_type == IB_QPT_GSI ||
+	     qp->ib_qp.qp_type == IB_QPT_RAW_ETHERTYPE) &&
+	     qp->qplib_qp.wqe_cnt == BNXT_RE_UD_QP_HW_STALL) {
+		int qp_attr_mask;
+		struct ib_qp_attr qp_attr;
+
+		qp_attr_mask = IB_QP_STATE;
+		qp_attr.qp_state = IB_QPS_RTS;
+		bnxt_re_modify_qp(&qp->ib_qp, &qp_attr, qp_attr_mask, NULL);
+		qp->qplib_qp.wqe_cnt = 0;
+	}
+}
+
 static int bnxt_re_post_send_shadow_qp(struct bnxt_re_dev *rdev,
 				       struct bnxt_re_qp *qp,
 				struct ib_send_wr *wr)
@@ -1928,6 +2137,7 @@ bad:
 		wr = wr->next;
 	}
 	bnxt_qplib_post_send_db(&qp->qplib_qp);
+	bnxt_ud_qp_hw_stall_workaround(qp);
 	spin_unlock_irqrestore(&qp->sq_lock, flags);
 	return rc;
 }
@@ -2024,6 +2234,7 @@ bad:
 		wr = wr->next;
 	}
 	bnxt_qplib_post_send_db(&qp->qplib_qp);
+	bnxt_ud_qp_hw_stall_workaround(qp);
 	spin_unlock_irqrestore(&qp->sq_lock, flags);
 
 	return rc;
@@ -2071,7 +2282,10 @@ int bnxt_re_post_recv(struct ib_qp *ib_qp, struct ib_recv_wr *wr,
 	struct bnxt_re_qp *qp = container_of(ib_qp, struct bnxt_re_qp, ib_qp);
 	struct bnxt_qplib_swqe wqe;
 	int rc = 0, payload_sz = 0;
+	unsigned long flags;
+	u32 count = 0;
 
+	spin_lock_irqsave(&qp->rq_lock, flags);
 	while (wr) {
 		/* House keeping */
 		memset(&wqe, 0, sizeof(wqe));
@@ -2100,9 +2314,21 @@ int bnxt_re_post_recv(struct ib_qp *ib_qp, struct ib_recv_wr *wr,
 			*bad_wr = wr;
 			break;
 		}
+
+		/* Ring DB if the RQEs posted reaches a threshold value */
+		if (++count >= BNXT_RE_RQ_WQE_THRESHOLD) {
+			bnxt_qplib_post_recv_db(&qp->qplib_qp);
+			count = 0;
+		}
+
 		wr = wr->next;
 	}
-	bnxt_qplib_post_recv_db(&qp->qplib_qp);
+
+	if (count)
+		bnxt_qplib_post_recv_db(&qp->qplib_qp);
+
+	spin_unlock_irqrestore(&qp->rq_lock, flags);
+
 	return rc;
 }
 
@@ -2643,12 +2869,36 @@ static void bnxt_re_process_res_ud_wc(struct ib_wc *wc,
 		wc->opcode = IB_WC_RECV_RDMA_WITH_IMM;
 }
 
+static int send_phantom_wqe(struct bnxt_re_qp *qp)
+{
+	struct bnxt_qplib_qp *lib_qp = &qp->qplib_qp;
+	unsigned long flags;
+	int rc = 0;
+
+	spin_lock_irqsave(&qp->sq_lock, flags);
+
+	rc = bnxt_re_bind_fence_mw(lib_qp);
+	if (!rc) {
+		lib_qp->sq.phantom_wqe_cnt++;
+		dev_dbg(&lib_qp->sq.hwq.pdev->dev,
+			"qp %#x sq->prod %#x sw_prod %#x phantom_wqe_cnt %d\n",
+			lib_qp->id, lib_qp->sq.hwq.prod,
+			HWQ_CMP(lib_qp->sq.hwq.prod, &lib_qp->sq.hwq),
+			lib_qp->sq.phantom_wqe_cnt);
+	}
+
+	spin_unlock_irqrestore(&qp->sq_lock, flags);
+	return rc;
+}
+
 int bnxt_re_poll_cq(struct ib_cq *ib_cq, int num_entries, struct ib_wc *wc)
 {
 	struct bnxt_re_cq *cq = container_of(ib_cq, struct bnxt_re_cq, ib_cq);
 	struct bnxt_re_qp *qp;
 	struct bnxt_qplib_cqe *cqe;
 	int i, ncqe, budget;
+	struct bnxt_qplib_q *sq;
+	struct bnxt_qplib_qp *lib_qp;
 	u32 tbl_idx;
 	struct bnxt_re_sqp_entries *sqp_entry = NULL;
 	unsigned long flags;
@@ -2661,7 +2911,21 @@ int bnxt_re_poll_cq(struct ib_cq *ib_cq, int num_entries, struct ib_wc *wc)
 	}
 	cqe = &cq->cql[0];
 	while (budget) {
-		ncqe = bnxt_qplib_poll_cq(&cq->qplib_cq, cqe, budget);
+		lib_qp = NULL;
+		ncqe = bnxt_qplib_poll_cq(&cq->qplib_cq, cqe, budget, &lib_qp);
+		if (lib_qp) {
+			sq = &lib_qp->sq;
+			if (sq->send_phantom) {
+				qp = container_of(lib_qp,
+						  struct bnxt_re_qp, qplib_qp);
+				if (send_phantom_wqe(qp) == -ENOMEM)
+					dev_err(rdev_to_dev(cq->rdev),
+						"Phantom failed! Scheduled to send again\n");
+				else
+					sq->send_phantom = false;
+			}
+		}
+
 		if (!ncqe)
 			break;
 
@@ -2822,6 +3086,12 @@ int bnxt_re_dereg_mr(struct ib_mr *ib_mr)
 	struct bnxt_re_dev *rdev = mr->rdev;
 	int rc;
 
+	rc = bnxt_qplib_free_mrw(&rdev->qplib_res, &mr->qplib_mr);
+	if (rc) {
+		dev_err(rdev_to_dev(rdev), "Dereg MR failed: %#x\n", rc);
+		return rc;
+	}
+
 	if (mr->npages && mr->pages) {
 		rc = bnxt_qplib_free_fast_reg_page_list(&rdev->qplib_res,
 							&mr->qplib_frpl);
@@ -2829,8 +3099,6 @@ int bnxt_re_dereg_mr(struct ib_mr *ib_mr)
 		mr->npages = 0;
 		mr->pages = NULL;
 	}
-	rc = bnxt_qplib_free_mrw(&rdev->qplib_res, &mr->qplib_mr);
-
 	if (!IS_ERR_OR_NULL(mr->ib_umem))
 		ib_umem_release(mr->ib_umem);
 
@@ -2914,97 +3182,52 @@ fail:
 	return ERR_PTR(rc);
 }
 
-/* Fast Memory Regions */
-struct ib_fmr *bnxt_re_alloc_fmr(struct ib_pd *ib_pd, int mr_access_flags,
-				 struct ib_fmr_attr *fmr_attr)
+struct ib_mw *bnxt_re_alloc_mw(struct ib_pd *ib_pd, enum ib_mw_type type,
+			       struct ib_udata *udata)
 {
 	struct bnxt_re_pd *pd = container_of(ib_pd, struct bnxt_re_pd, ib_pd);
 	struct bnxt_re_dev *rdev = pd->rdev;
-	struct bnxt_re_fmr *fmr;
+	struct bnxt_re_mw *mw;
 	int rc;
 
-	if (fmr_attr->max_pages > MAX_PBL_LVL_2_PGS ||
-	    fmr_attr->max_maps > rdev->dev_attr.max_map_per_fmr) {
-		dev_err(rdev_to_dev(rdev), "Allocate FMR exceeded Max limit");
+	mw = kzalloc(sizeof(*mw), GFP_KERNEL);
+	if (!mw)
 		return ERR_PTR(-ENOMEM);
-	}
-	fmr = kzalloc(sizeof(*fmr), GFP_KERNEL);
-	if (!fmr)
-		return ERR_PTR(-ENOMEM);
-
-	fmr->rdev = rdev;
-	fmr->qplib_fmr.pd = &pd->qplib_pd;
-	fmr->qplib_fmr.type = CMDQ_ALLOCATE_MRW_MRW_FLAGS_PMR;
+	mw->rdev = rdev;
+	mw->qplib_mw.pd = &pd->qplib_pd;
 
-	rc = bnxt_qplib_alloc_mrw(&rdev->qplib_res, &fmr->qplib_fmr);
-	if (rc)
+	mw->qplib_mw.type = (type == IB_MW_TYPE_1 ?
+			       CMDQ_ALLOCATE_MRW_MRW_FLAGS_MW_TYPE1 :
+			       CMDQ_ALLOCATE_MRW_MRW_FLAGS_MW_TYPE2B);
+	rc = bnxt_qplib_alloc_mrw(&rdev->qplib_res, &mw->qplib_mw);
+	if (rc) {
+		dev_err(rdev_to_dev(rdev), "Allocate MW failed!");
 		goto fail;
+	}
+	mw->ib_mw.rkey = mw->qplib_mw.rkey;
 
-	fmr->qplib_fmr.flags = __from_ib_access_flags(mr_access_flags);
-	fmr->ib_fmr.lkey = fmr->qplib_fmr.lkey;
-	fmr->ib_fmr.rkey = fmr->ib_fmr.lkey;
+	atomic_inc(&rdev->mw_count);
+	return &mw->ib_mw;
 
-	atomic_inc(&rdev->mr_count);
-	return &fmr->ib_fmr;
 fail:
-	kfree(fmr);
+	kfree(mw);
 	return ERR_PTR(rc);
 }
 
-int bnxt_re_map_phys_fmr(struct ib_fmr *ib_fmr, u64 *page_list, int list_len,
-			 u64 iova)
+int bnxt_re_dealloc_mw(struct ib_mw *ib_mw)
 {
-	struct bnxt_re_fmr *fmr = container_of(ib_fmr, struct bnxt_re_fmr,
-					     ib_fmr);
-	struct bnxt_re_dev *rdev = fmr->rdev;
+	struct bnxt_re_mw *mw = container_of(ib_mw, struct bnxt_re_mw, ib_mw);
+	struct bnxt_re_dev *rdev = mw->rdev;
 	int rc;
 
-	fmr->qplib_fmr.va = iova;
-	fmr->qplib_fmr.total_size = list_len * PAGE_SIZE;
-
-	rc = bnxt_qplib_reg_mr(&rdev->qplib_res, &fmr->qplib_fmr, page_list,
-			       list_len, true);
-	if (rc)
-		dev_err(rdev_to_dev(rdev), "Failed to map FMR for lkey = 0x%x!",
-			fmr->ib_fmr.lkey);
-	return rc;
-}
-
-int bnxt_re_unmap_fmr(struct list_head *fmr_list)
-{
-	struct bnxt_re_dev *rdev;
-	struct bnxt_re_fmr *fmr;
-	struct ib_fmr *ib_fmr;
-	int rc = 0;
-
-	/* Validate each FMRs inside the fmr_list */
-	list_for_each_entry(ib_fmr, fmr_list, list) {
-		fmr = container_of(ib_fmr, struct bnxt_re_fmr, ib_fmr);
-		rdev = fmr->rdev;
-
-		if (rdev) {
-			rc = bnxt_qplib_dereg_mrw(&rdev->qplib_res,
-						  &fmr->qplib_fmr, true);
-			if (rc)
-				break;
-		}
+	rc = bnxt_qplib_free_mrw(&rdev->qplib_res, &mw->qplib_mw);
+	if (rc) {
+		dev_err(rdev_to_dev(rdev), "Free MW failed: %#x\n", rc);
+		return rc;
 	}
-	return rc;
-}
-
-int bnxt_re_dealloc_fmr(struct ib_fmr *ib_fmr)
-{
-	struct bnxt_re_fmr *fmr = container_of(ib_fmr, struct bnxt_re_fmr,
-					       ib_fmr);
-	struct bnxt_re_dev *rdev = fmr->rdev;
-	int rc;
 
-	rc = bnxt_qplib_free_mrw(&rdev->qplib_res, &fmr->qplib_fmr);
-	if (rc)
-		dev_err(rdev_to_dev(rdev), "Failed to free FMR");
-
-	kfree(fmr);
-	atomic_dec(&rdev->mr_count);
+	kfree(mw);
+	atomic_dec(&rdev->mw_count);
 	return rc;
 }
 
diff --git a/drivers/infiniband/hw/bnxt_re/ib_verbs.h b/drivers/infiniband/hw/bnxt_re/ib_verbs.h
index 5c3d71765454..6c160f6a5398 100644
--- a/drivers/infiniband/hw/bnxt_re/ib_verbs.h
+++ b/drivers/infiniband/hw/bnxt_re/ib_verbs.h
@@ -44,11 +44,23 @@ struct bnxt_re_gid_ctx {
 	u32			refcnt;
 };
 
+#define BNXT_RE_FENCE_BYTES	64
+struct bnxt_re_fence_data {
+	u32 size;
+	u8 va[BNXT_RE_FENCE_BYTES];
+	dma_addr_t dma_addr;
+	struct bnxt_re_mr *mr;
+	struct ib_mw *mw;
+	struct bnxt_qplib_swqe bind_wqe;
+	u32 bind_rkey;
+};
+
 struct bnxt_re_pd {
 	struct bnxt_re_dev	*rdev;
 	struct ib_pd		ib_pd;
 	struct bnxt_qplib_pd	qplib_pd;
 	struct bnxt_qplib_dpi	dpi;
+	struct bnxt_re_fence_data fence;
 };
 
 struct bnxt_re_ah {
@@ -62,6 +74,7 @@ struct bnxt_re_qp {
 	struct bnxt_re_dev	*rdev;
 	struct ib_qp		ib_qp;
 	spinlock_t		sq_lock;	/* protect sq */
+	spinlock_t		rq_lock;	/* protect rq */
 	struct bnxt_qplib_qp	qplib_qp;
 	struct ib_umem		*sumem;
 	struct ib_umem		*rumem;
@@ -181,12 +194,9 @@ int bnxt_re_map_mr_sg(struct ib_mr *ib_mr, struct scatterlist *sg, int sg_nents,
 struct ib_mr *bnxt_re_alloc_mr(struct ib_pd *ib_pd, enum ib_mr_type mr_type,
 			       u32 max_num_sg);
 int bnxt_re_dereg_mr(struct ib_mr *mr);
-struct ib_fmr *bnxt_re_alloc_fmr(struct ib_pd *pd, int mr_access_flags,
-				 struct ib_fmr_attr *fmr_attr);
-int bnxt_re_map_phys_fmr(struct ib_fmr *fmr, u64 *page_list, int list_len,
-			 u64 iova);
-int bnxt_re_unmap_fmr(struct list_head *fmr_list);
-int bnxt_re_dealloc_fmr(struct ib_fmr *fmr);
+struct ib_mw *bnxt_re_alloc_mw(struct ib_pd *ib_pd, enum ib_mw_type type,
+			       struct ib_udata *udata);
+int bnxt_re_dealloc_mw(struct ib_mw *mw);
 struct ib_mr *bnxt_re_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 				  u64 virt_addr, int mr_access_flags,
 				  struct ib_udata *udata);
diff --git a/drivers/infiniband/hw/bnxt_re/main.c b/drivers/infiniband/hw/bnxt_re/main.c
index 5d355401179b..1fce5e73216b 100644
--- a/drivers/infiniband/hw/bnxt_re/main.c
+++ b/drivers/infiniband/hw/bnxt_re/main.c
@@ -507,10 +507,6 @@ static int bnxt_re_register_ib(struct bnxt_re_dev *rdev)
 	ibdev->dereg_mr			= bnxt_re_dereg_mr;
 	ibdev->alloc_mr			= bnxt_re_alloc_mr;
 	ibdev->map_mr_sg		= bnxt_re_map_mr_sg;
-	ibdev->alloc_fmr		= bnxt_re_alloc_fmr;
-	ibdev->map_phys_fmr		= bnxt_re_map_phys_fmr;
-	ibdev->unmap_fmr		= bnxt_re_unmap_fmr;
-	ibdev->dealloc_fmr		= bnxt_re_dealloc_fmr;
 
 	ibdev->reg_user_mr		= bnxt_re_reg_user_mr;
 	ibdev->alloc_ucontext		= bnxt_re_alloc_ucontext;
diff --git a/drivers/infiniband/hw/bnxt_re/qplib_fp.c b/drivers/infiniband/hw/bnxt_re/qplib_fp.c
index 43d08b5e9085..f05500bcdcf1 100644
--- a/drivers/infiniband/hw/bnxt_re/qplib_fp.c
+++ b/drivers/infiniband/hw/bnxt_re/qplib_fp.c
@@ -284,7 +284,7 @@ int bnxt_qplib_create_qp1(struct bnxt_qplib_res *res, struct bnxt_qplib_qp *qp)
 {
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct cmdq_create_qp1 req;
-	struct creq_create_qp1_resp *resp;
+	struct creq_create_qp1_resp resp;
 	struct bnxt_qplib_pbl *pbl;
 	struct bnxt_qplib_q *sq = &qp->sq;
 	struct bnxt_qplib_q *rq = &qp->rq;
@@ -394,31 +394,12 @@ int bnxt_qplib_create_qp1(struct bnxt_qplib_res *res, struct bnxt_qplib_qp *qp)
 
 	req.pd_id = cpu_to_le32(qp->pd->id);
 
-	resp = (struct creq_create_qp1_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, 0);
-	if (!resp) {
-		dev_err(&res->pdev->dev, "QPLIB: FP: CREATE_QP1 send failed");
-		rc = -EINVAL;
-		goto fail;
-	}
-	if (!bnxt_qplib_rcfw_wait_for_resp(rcfw, le16_to_cpu(req.cookie))) {
-		/* Cmd timed out */
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: CREATE_QP1 timed out");
-		rc = -ETIMEDOUT;
-		goto fail;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: CREATE_QP1 failed ");
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		rc = -EINVAL;
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
+					  (void *)&resp, NULL, 0);
+	if (rc)
 		goto fail;
-	}
-	qp->id = le32_to_cpu(resp->xid);
+
+	qp->id = le32_to_cpu(resp.xid);
 	qp->cur_qp_state = CMDQ_MODIFY_QP_NEW_STATE_RESET;
 	sq->flush_in_progress = false;
 	rq->flush_in_progress = false;
@@ -442,7 +423,7 @@ int bnxt_qplib_create_qp(struct bnxt_qplib_res *res, struct bnxt_qplib_qp *qp)
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct sq_send *hw_sq_send_hdr, **hw_sq_send_ptr;
 	struct cmdq_create_qp req;
-	struct creq_create_qp_resp *resp;
+	struct creq_create_qp_resp resp;
 	struct bnxt_qplib_pbl *pbl;
 	struct sq_psn_search **psn_search_ptr;
 	unsigned long int psn_search, poff = 0;
@@ -627,31 +608,12 @@ int bnxt_qplib_create_qp(struct bnxt_qplib_res *res, struct bnxt_qplib_qp *qp)
 	}
 	req.pd_id = cpu_to_le32(qp->pd->id);
 
-	resp = (struct creq_create_qp_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, 0);
-	if (!resp) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: CREATE_QP send failed");
-		rc = -EINVAL;
-		goto fail;
-	}
-	if (!bnxt_qplib_rcfw_wait_for_resp(rcfw, le16_to_cpu(req.cookie))) {
-		/* Cmd timed out */
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: CREATE_QP timed out");
-		rc = -ETIMEDOUT;
-		goto fail;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: CREATE_QP failed ");
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		rc = -EINVAL;
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
+					  (void *)&resp, NULL, 0);
+	if (rc)
 		goto fail;
-	}
-	qp->id = le32_to_cpu(resp->xid);
+
+	qp->id = le32_to_cpu(resp.xid);
 	qp->cur_qp_state = CMDQ_MODIFY_QP_NEW_STATE_RESET;
 	sq->flush_in_progress = false;
 	rq->flush_in_progress = false;
@@ -769,10 +731,11 @@ int bnxt_qplib_modify_qp(struct bnxt_qplib_res *res, struct bnxt_qplib_qp *qp)
 {
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct cmdq_modify_qp req;
-	struct creq_modify_qp_resp *resp;
+	struct creq_modify_qp_resp resp;
 	u16 cmd_flags = 0, pkey;
 	u32 temp32[4];
 	u32 bmask;
+	int rc;
 
 	RCFW_CMD_PREP(req, MODIFY_QP, cmd_flags);
 
@@ -862,27 +825,10 @@ int bnxt_qplib_modify_qp(struct bnxt_qplib_res *res, struct bnxt_qplib_qp *qp)
 
 	req.vlan_pcp_vlan_dei_vlan_id = cpu_to_le16(qp->vlan_id);
 
-	resp = (struct creq_modify_qp_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, 0);
-	if (!resp) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: MODIFY_QP send failed");
-		return -EINVAL;
-	}
-	if (!bnxt_qplib_rcfw_wait_for_resp(rcfw, le16_to_cpu(req.cookie))) {
-		/* Cmd timed out */
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: MODIFY_QP timed out");
-		return -ETIMEDOUT;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: MODIFY_QP failed ");
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		return -EINVAL;
-	}
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
+					  (void *)&resp, NULL, 0);
+	if (rc)
+		return rc;
 	qp->cur_qp_state = qp->state;
 	return 0;
 }
@@ -891,37 +837,26 @@ int bnxt_qplib_query_qp(struct bnxt_qplib_res *res, struct bnxt_qplib_qp *qp)
 {
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct cmdq_query_qp req;
-	struct creq_query_qp_resp *resp;
+	struct creq_query_qp_resp resp;
+	struct bnxt_qplib_rcfw_sbuf *sbuf;
 	struct creq_query_qp_resp_sb *sb;
 	u16 cmd_flags = 0;
 	u32 temp32[4];
-	int i;
+	int i, rc = 0;
 
 	RCFW_CMD_PREP(req, QUERY_QP, cmd_flags);
 
+	sbuf = bnxt_qplib_rcfw_alloc_sbuf(rcfw, sizeof(*sb));
+	if (!sbuf)
+		return -ENOMEM;
+	sb = sbuf->sb;
+
 	req.qp_cid = cpu_to_le32(qp->id);
 	req.resp_size = sizeof(*sb) / BNXT_QPLIB_CMDQE_UNITS;
-	resp = (struct creq_query_qp_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     (void **)&sb, 0);
-	if (!resp) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: QUERY_QP send failed");
-		return -EINVAL;
-	}
-	if (!bnxt_qplib_rcfw_wait_for_resp(rcfw, le16_to_cpu(req.cookie))) {
-		/* Cmd timed out */
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: QUERY_QP timed out");
-		return -ETIMEDOUT;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: QUERY_QP failed ");
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		return -EINVAL;
-	}
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req, (void *)&resp,
+					  (void *)sbuf, 0);
+	if (rc)
+		goto bail;
 	/* Extract the context from the side buffer */
 	qp->state = sb->en_sqd_async_notify_state &
 			CREQ_QUERY_QP_RESP_SB_STATE_MASK;
@@ -976,7 +911,9 @@ int bnxt_qplib_query_qp(struct bnxt_qplib_res *res, struct bnxt_qplib_qp *qp)
 	qp->dest_qpn = le32_to_cpu(sb->dest_qp_id);
 	memcpy(qp->smac, sb->src_mac, 6);
 	qp->vlan_id = le16_to_cpu(sb->vlan_pcp_vlan_dei_vlan_id);
-	return 0;
+bail:
+	bnxt_qplib_rcfw_free_sbuf(rcfw, sbuf);
+	return rc;
 }
 
 static void __clean_cq(struct bnxt_qplib_cq *cq, u64 qp)
@@ -1021,34 +958,18 @@ int bnxt_qplib_destroy_qp(struct bnxt_qplib_res *res,
 {
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct cmdq_destroy_qp req;
-	struct creq_destroy_qp_resp *resp;
+	struct creq_destroy_qp_resp resp;
 	unsigned long flags;
 	u16 cmd_flags = 0;
+	int rc;
 
 	RCFW_CMD_PREP(req, DESTROY_QP, cmd_flags);
 
 	req.qp_cid = cpu_to_le32(qp->id);
-	resp = (struct creq_destroy_qp_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, 0);
-	if (!resp) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: DESTROY_QP send failed");
-		return -EINVAL;
-	}
-	if (!bnxt_qplib_rcfw_wait_for_resp(rcfw, le16_to_cpu(req.cookie))) {
-		/* Cmd timed out */
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: DESTROY_QP timed out");
-		return -ETIMEDOUT;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: DESTROY_QP failed ");
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		return -EINVAL;
-	}
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
+					  (void *)&resp, NULL, 0);
+	if (rc)
+		return rc;
 
 	/* Must walk the associated CQs to nullified the QP ptr */
 	spin_lock_irqsave(&qp->scq->hwq.lock, flags);
@@ -1162,8 +1083,12 @@ int bnxt_qplib_post_send(struct bnxt_qplib_qp *qp,
 		rc = -EINVAL;
 		goto done;
 	}
-	if (HWQ_CMP((sq->hwq.prod + 1), &sq->hwq) ==
-	    HWQ_CMP(sq->hwq.cons, &sq->hwq)) {
+
+	if (bnxt_qplib_queue_full(sq)) {
+		dev_err(&sq->hwq.pdev->dev,
+			"QPLIB: prod = %#x cons = %#x qdepth = %#x delta = %#x",
+			sq->hwq.prod, sq->hwq.cons, sq->hwq.max_elements,
+			sq->q_full_delta);
 		rc = -ENOMEM;
 		goto done;
 	}
@@ -1373,6 +1298,9 @@ int bnxt_qplib_post_send(struct bnxt_qplib_qp *qp,
 	}
 
 	sq->hwq.prod++;
+
+	qp->wqe_cnt++;
+
 done:
 	return rc;
 }
@@ -1411,8 +1339,7 @@ int bnxt_qplib_post_recv(struct bnxt_qplib_qp *qp,
 		rc = -EINVAL;
 		goto done;
 	}
-	if (HWQ_CMP((rq->hwq.prod + 1), &rq->hwq) ==
-	    HWQ_CMP(rq->hwq.cons, &rq->hwq)) {
+	if (bnxt_qplib_queue_full(rq)) {
 		dev_err(&rq->hwq.pdev->dev,
 			"QPLIB: FP: QP (0x%x) RQ is full!", qp->id);
 		rc = -EINVAL;
@@ -1483,7 +1410,7 @@ int bnxt_qplib_create_cq(struct bnxt_qplib_res *res, struct bnxt_qplib_cq *cq)
 {
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct cmdq_create_cq req;
-	struct creq_create_cq_resp *resp;
+	struct creq_create_cq_resp resp;
 	struct bnxt_qplib_pbl *pbl;
 	u16 cmd_flags = 0;
 	int rc;
@@ -1525,30 +1452,12 @@ int bnxt_qplib_create_cq(struct bnxt_qplib_res *res, struct bnxt_qplib_cq *cq)
 			(cq->cnq_hw_ring_id & CMDQ_CREATE_CQ_CNQ_ID_MASK) <<
 			 CMDQ_CREATE_CQ_CNQ_ID_SFT);
 
-	resp = (struct creq_create_cq_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, 0);
-	if (!resp) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: CREATE_CQ send failed");
-		return -EINVAL;
-	}
-	if (!bnxt_qplib_rcfw_wait_for_resp(rcfw, le16_to_cpu(req.cookie))) {
-		/* Cmd timed out */
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: CREATE_CQ timed out");
-		rc = -ETIMEDOUT;
-		goto fail;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: CREATE_CQ failed ");
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		rc = -EINVAL;
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
+					  (void *)&resp, NULL, 0);
+	if (rc)
 		goto fail;
-	}
-	cq->id = le32_to_cpu(resp->xid);
+
+	cq->id = le32_to_cpu(resp.xid);
 	cq->dbr_base = res->dpi_tbl.dbr_bar_reg_iomem;
 	cq->period = BNXT_QPLIB_QUEUE_START_PERIOD;
 	init_waitqueue_head(&cq->waitq);
@@ -1566,33 +1475,17 @@ int bnxt_qplib_destroy_cq(struct bnxt_qplib_res *res, struct bnxt_qplib_cq *cq)
 {
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct cmdq_destroy_cq req;
-	struct creq_destroy_cq_resp *resp;
+	struct creq_destroy_cq_resp resp;
 	u16 cmd_flags = 0;
+	int rc;
 
 	RCFW_CMD_PREP(req, DESTROY_CQ, cmd_flags);
 
 	req.cq_cid = cpu_to_le32(cq->id);
-	resp = (struct creq_destroy_cq_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, 0);
-	if (!resp) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: DESTROY_CQ send failed");
-		return -EINVAL;
-	}
-	if (!bnxt_qplib_rcfw_wait_for_resp(rcfw, le16_to_cpu(req.cookie))) {
-		/* Cmd timed out */
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: DESTROY_CQ timed out");
-		return -ETIMEDOUT;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: FP: DESTROY_CQ failed ");
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		return -EINVAL;
-	}
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
+					  (void *)&resp, NULL, 0);
+	if (rc)
+		return rc;
 	bnxt_qplib_free_hwq(res->pdev, &cq->hwq);
 	return 0;
 }
@@ -1664,14 +1557,113 @@ static int __flush_rq(struct bnxt_qplib_q *rq, struct bnxt_qplib_qp *qp,
 	return rc;
 }
 
+/* Note: SQE is valid from sw_sq_cons up to cqe_sq_cons (exclusive)
+ *       CQE is track from sw_cq_cons to max_element but valid only if VALID=1
+ */
+static int do_wa9060(struct bnxt_qplib_qp *qp, struct bnxt_qplib_cq *cq,
+		     u32 cq_cons, u32 sw_sq_cons, u32 cqe_sq_cons)
+{
+	struct bnxt_qplib_q *sq = &qp->sq;
+	struct bnxt_qplib_swq *swq;
+	u32 peek_sw_cq_cons, peek_raw_cq_cons, peek_sq_cons_idx;
+	struct cq_base *peek_hwcqe, **peek_hw_cqe_ptr;
+	struct cq_req *peek_req_hwcqe;
+	struct bnxt_qplib_qp *peek_qp;
+	struct bnxt_qplib_q *peek_sq;
+	int i, rc = 0;
+
+	/* Normal mode */
+	/* Check for the psn_search marking before completing */
+	swq = &sq->swq[sw_sq_cons];
+	if (swq->psn_search &&
+	    le32_to_cpu(swq->psn_search->flags_next_psn) & 0x80000000) {
+		/* Unmark */
+		swq->psn_search->flags_next_psn = cpu_to_le32
+			(le32_to_cpu(swq->psn_search->flags_next_psn)
+				     & ~0x80000000);
+		dev_dbg(&cq->hwq.pdev->dev,
+			"FP: Process Req cq_cons=0x%x qp=0x%x sq cons sw=0x%x cqe=0x%x marked!\n",
+			cq_cons, qp->id, sw_sq_cons, cqe_sq_cons);
+		sq->condition = true;
+		sq->send_phantom = true;
+
+		/* TODO: Only ARM if the previous SQE is ARMALL */
+		bnxt_qplib_arm_cq(cq, DBR_DBR_TYPE_CQ_ARMALL);
+
+		rc = -EAGAIN;
+		goto out;
+	}
+	if (sq->condition) {
+		/* Peek at the completions */
+		peek_raw_cq_cons = cq->hwq.cons;
+		peek_sw_cq_cons = cq_cons;
+		i = cq->hwq.max_elements;
+		while (i--) {
+			peek_sw_cq_cons = HWQ_CMP((peek_sw_cq_cons), &cq->hwq);
+			peek_hw_cqe_ptr = (struct cq_base **)cq->hwq.pbl_ptr;
+			peek_hwcqe = &peek_hw_cqe_ptr[CQE_PG(peek_sw_cq_cons)]
+						     [CQE_IDX(peek_sw_cq_cons)];
+			/* If the next hwcqe is VALID */
+			if (CQE_CMP_VALID(peek_hwcqe, peek_raw_cq_cons,
+					  cq->hwq.max_elements)) {
+				/* If the next hwcqe is a REQ */
+				if ((peek_hwcqe->cqe_type_toggle &
+				    CQ_BASE_CQE_TYPE_MASK) ==
+				    CQ_BASE_CQE_TYPE_REQ) {
+					peek_req_hwcqe = (struct cq_req *)
+							 peek_hwcqe;
+					peek_qp = (struct bnxt_qplib_qp *)
+						((unsigned long)
+						 le64_to_cpu
+						 (peek_req_hwcqe->qp_handle));
+					peek_sq = &peek_qp->sq;
+					peek_sq_cons_idx = HWQ_CMP(le16_to_cpu(
+						peek_req_hwcqe->sq_cons_idx) - 1
+						, &sq->hwq);
+					/* If the hwcqe's sq's wr_id matches */
+					if (peek_sq == sq &&
+					    sq->swq[peek_sq_cons_idx].wr_id ==
+					    BNXT_QPLIB_FENCE_WRID) {
+						/*
+						 *  Unbreak only if the phantom
+						 *  comes back
+						 */
+						dev_dbg(&cq->hwq.pdev->dev,
+							"FP:Got Phantom CQE");
+						sq->condition = false;
+						sq->single = true;
+						rc = 0;
+						goto out;
+					}
+				}
+				/* Valid but not the phantom, so keep looping */
+			} else {
+				/* Not valid yet, just exit and wait */
+				rc = -EINVAL;
+				goto out;
+			}
+			peek_sw_cq_cons++;
+			peek_raw_cq_cons++;
+		}
+		dev_err(&cq->hwq.pdev->dev,
+			"Should not have come here! cq_cons=0x%x qp=0x%x sq cons sw=0x%x hw=0x%x",
+			cq_cons, qp->id, sw_sq_cons, cqe_sq_cons);
+		rc = -EINVAL;
+	}
+out:
+	return rc;
+}
+
 static int bnxt_qplib_cq_process_req(struct bnxt_qplib_cq *cq,
 				     struct cq_req *hwcqe,
-				     struct bnxt_qplib_cqe **pcqe, int *budget)
+				     struct bnxt_qplib_cqe **pcqe, int *budget,
+				     u32 cq_cons, struct bnxt_qplib_qp **lib_qp)
 {
 	struct bnxt_qplib_qp *qp;
 	struct bnxt_qplib_q *sq;
 	struct bnxt_qplib_cqe *cqe;
-	u32 sw_cons, cqe_cons;
+	u32 sw_sq_cons, cqe_sq_cons;
+	struct bnxt_qplib_swq *swq;
 	int rc = 0;
 
 	qp = (struct bnxt_qplib_qp *)((unsigned long)
@@ -1683,13 +1675,13 @@ static int bnxt_qplib_cq_process_req(struct bnxt_qplib_cq *cq,
 	}
 	sq = &qp->sq;
 
-	cqe_cons = HWQ_CMP(le16_to_cpu(hwcqe->sq_cons_idx), &sq->hwq);
-	if (cqe_cons > sq->hwq.max_elements) {
+	cqe_sq_cons = HWQ_CMP(le16_to_cpu(hwcqe->sq_cons_idx), &sq->hwq);
+	if (cqe_sq_cons > sq->hwq.max_elements) {
 		dev_err(&cq->hwq.pdev->dev,
 			"QPLIB: FP: CQ Process req reported ");
 		dev_err(&cq->hwq.pdev->dev,
 			"QPLIB: sq_cons_idx 0x%x which exceeded max 0x%x",
-			cqe_cons, sq->hwq.max_elements);
+			cqe_sq_cons, sq->hwq.max_elements);
 		return -EINVAL;
 	}
 	/* If we were in the middle of flushing the SQ, continue */
@@ -1698,53 +1690,74 @@ static int bnxt_qplib_cq_process_req(struct bnxt_qplib_cq *cq,
 
 	/* Require to walk the sq's swq to fabricate CQEs for all previously
 	 * signaled SWQEs due to CQE aggregation from the current sq cons
-	 * to the cqe_cons
+	 * to the cqe_sq_cons
 	 */
 	cqe = *pcqe;
 	while (*budget) {
-		sw_cons = HWQ_CMP(sq->hwq.cons, &sq->hwq);
-		if (sw_cons == cqe_cons)
+		sw_sq_cons = HWQ_CMP(sq->hwq.cons, &sq->hwq);
+		if (sw_sq_cons == cqe_sq_cons)
+			/* Done */
 			break;
+
+		swq = &sq->swq[sw_sq_cons];
 		memset(cqe, 0, sizeof(*cqe));
 		cqe->opcode = CQ_BASE_CQE_TYPE_REQ;
 		cqe->qp_handle = (u64)(unsigned long)qp;
 		cqe->src_qp = qp->id;
-		cqe->wr_id = sq->swq[sw_cons].wr_id;
-		cqe->type = sq->swq[sw_cons].type;
+		cqe->wr_id = swq->wr_id;
+		if (cqe->wr_id == BNXT_QPLIB_FENCE_WRID)
+			goto skip;
+		cqe->type = swq->type;
 
 		/* For the last CQE, check for status.  For errors, regardless
 		 * of the request being signaled or not, it must complete with
 		 * the hwcqe error status
 		 */
-		if (HWQ_CMP((sw_cons + 1), &sq->hwq) == cqe_cons &&
+		if (HWQ_CMP((sw_sq_cons + 1), &sq->hwq) == cqe_sq_cons &&
 		    hwcqe->status != CQ_REQ_STATUS_OK) {
 			cqe->status = hwcqe->status;
 			dev_err(&cq->hwq.pdev->dev,
 				"QPLIB: FP: CQ Processed Req ");
 			dev_err(&cq->hwq.pdev->dev,
 				"QPLIB: wr_id[%d] = 0x%llx with status 0x%x",
-				sw_cons, cqe->wr_id, cqe->status);
+				sw_sq_cons, cqe->wr_id, cqe->status);
 			cqe++;
 			(*budget)--;
 			sq->flush_in_progress = true;
 			/* Must block new posting of SQ and RQ */
 			qp->state = CMDQ_MODIFY_QP_NEW_STATE_ERR;
+			sq->condition = false;
+			sq->single = false;
 		} else {
-			if (sq->swq[sw_cons].flags &
-			    SQ_SEND_FLAGS_SIGNAL_COMP) {
+			if (swq->flags & SQ_SEND_FLAGS_SIGNAL_COMP) {
+				/* Before we complete, do WA 9060 */
+				if (do_wa9060(qp, cq, cq_cons, sw_sq_cons,
+					      cqe_sq_cons)) {
+					*lib_qp = qp;
+					goto out;
+				}
 				cqe->status = CQ_REQ_STATUS_OK;
 				cqe++;
 				(*budget)--;
 			}
 		}
+skip:
 		sq->hwq.cons++;
+		if (sq->single)
+			break;
 	}
+out:
 	*pcqe = cqe;
-	if (!*budget && HWQ_CMP(sq->hwq.cons, &sq->hwq) != cqe_cons) {
+	if (HWQ_CMP(sq->hwq.cons, &sq->hwq) != cqe_sq_cons) {
 		/* Out of budget */
 		rc = -EAGAIN;
 		goto done;
 	}
+	/*
+	 * Back to normal completion mode only after it has completed all of
+	 * the WC for this CQE
+	 */
+	sq->single = false;
 	if (!sq->flush_in_progress)
 		goto done;
 flush:
@@ -2074,7 +2087,7 @@ static int bnxt_qplib_cq_process_cutoff(struct bnxt_qplib_cq *cq,
 }
 
 int bnxt_qplib_poll_cq(struct bnxt_qplib_cq *cq, struct bnxt_qplib_cqe *cqe,
-		       int num_cqes)
+		       int num_cqes, struct bnxt_qplib_qp **lib_qp)
 {
 	struct cq_base *hw_cqe, **hw_cqe_ptr;
 	unsigned long flags;
@@ -2099,7 +2112,8 @@ int bnxt_qplib_poll_cq(struct bnxt_qplib_cq *cq, struct bnxt_qplib_cqe *cqe,
 		case CQ_BASE_CQE_TYPE_REQ:
 			rc = bnxt_qplib_cq_process_req(cq,
 						       (struct cq_req *)hw_cqe,
-						       &cqe, &budget);
+						       &cqe, &budget,
+						       sw_cons, lib_qp);
 			break;
 		case CQ_BASE_CQE_TYPE_RES_RC:
 			rc = bnxt_qplib_cq_process_res_rc(cq,
diff --git a/drivers/infiniband/hw/bnxt_re/qplib_fp.h b/drivers/infiniband/hw/bnxt_re/qplib_fp.h
index f0150f8da1e3..36b7b7db0e3f 100644
--- a/drivers/infiniband/hw/bnxt_re/qplib_fp.h
+++ b/drivers/infiniband/hw/bnxt_re/qplib_fp.h
@@ -88,6 +88,7 @@ struct bnxt_qplib_swq {
 
 struct bnxt_qplib_swqe {
 	/* General */
+#define	BNXT_QPLIB_FENCE_WRID	0x46454E43	/* "FENC" */
 	u64				wr_id;
 	u8				reqs_type;
 	u8				type;
@@ -216,9 +217,16 @@ struct bnxt_qplib_q {
 	struct scatterlist		*sglist;
 	u32				nmap;
 	u32				max_wqe;
+	u16				q_full_delta;
 	u16				max_sge;
 	u32				psn;
 	bool				flush_in_progress;
+	bool				condition;
+	bool				single;
+	bool				send_phantom;
+	u32				phantom_wqe_cnt;
+	u32				phantom_cqe_cnt;
+	u32				next_cq_cons;
 };
 
 struct bnxt_qplib_qp {
@@ -242,6 +250,7 @@ struct bnxt_qplib_qp {
 	u8				timeout;
 	u8				retry_cnt;
 	u8				rnr_retry;
+	u64				wqe_cnt;
 	u32				min_rnr_timer;
 	u32				max_rd_atomic;
 	u32				max_dest_rd_atomic;
@@ -301,6 +310,13 @@ struct bnxt_qplib_qp {
 	(!!((hdr)->cqe_type_toggle & CQ_BASE_TOGGLE) ==		\
 	   !((raw_cons) & (cp_bit)))
 
+static inline bool bnxt_qplib_queue_full(struct bnxt_qplib_q *qplib_q)
+{
+	return HWQ_CMP((qplib_q->hwq.prod + qplib_q->q_full_delta),
+		       &qplib_q->hwq) == HWQ_CMP(qplib_q->hwq.cons,
+						 &qplib_q->hwq);
+}
+
 struct bnxt_qplib_cqe {
 	u8				status;
 	u8				type;
@@ -432,7 +448,7 @@ int bnxt_qplib_post_recv(struct bnxt_qplib_qp *qp,
 int bnxt_qplib_create_cq(struct bnxt_qplib_res *res, struct bnxt_qplib_cq *cq);
 int bnxt_qplib_destroy_cq(struct bnxt_qplib_res *res, struct bnxt_qplib_cq *cq);
 int bnxt_qplib_poll_cq(struct bnxt_qplib_cq *cq, struct bnxt_qplib_cqe *cqe,
-		       int num);
+		       int num, struct bnxt_qplib_qp **qp);
 void bnxt_qplib_req_notify_cq(struct bnxt_qplib_cq *cq, u32 arm_type);
 void bnxt_qplib_free_nq(struct bnxt_qplib_nq *nq);
 int bnxt_qplib_alloc_nq(struct pci_dev *pdev, struct bnxt_qplib_nq *nq);
diff --git a/drivers/infiniband/hw/bnxt_re/qplib_rcfw.c b/drivers/infiniband/hw/bnxt_re/qplib_rcfw.c
index 23fb7260662b..16e42754dbec 100644
--- a/drivers/infiniband/hw/bnxt_re/qplib_rcfw.c
+++ b/drivers/infiniband/hw/bnxt_re/qplib_rcfw.c
@@ -39,72 +39,55 @@
 #include <linux/spinlock.h>
 #include <linux/pci.h>
 #include <linux/prefetch.h>
+#include <linux/delay.h>
+
 #include "roce_hsi.h"
 #include "qplib_res.h"
 #include "qplib_rcfw.h"
 static void bnxt_qplib_service_creq(unsigned long data);
 
 /* Hardware communication channel */
-int bnxt_qplib_rcfw_wait_for_resp(struct bnxt_qplib_rcfw *rcfw, u16 cookie)
+static int __wait_for_resp(struct bnxt_qplib_rcfw *rcfw, u16 cookie)
 {
 	u16 cbit;
 	int rc;
 
-	cookie &= RCFW_MAX_COOKIE_VALUE;
 	cbit = cookie % RCFW_MAX_OUTSTANDING_CMD;
-	if (!test_bit(cbit, rcfw->cmdq_bitmap))
-		dev_warn(&rcfw->pdev->dev,
-			 "QPLIB: CMD bit %d for cookie 0x%x is not set?",
-			 cbit, cookie);
-
 	rc = wait_event_timeout(rcfw->waitq,
 				!test_bit(cbit, rcfw->cmdq_bitmap),
 				msecs_to_jiffies(RCFW_CMD_WAIT_TIME_MS));
-	if (!rc) {
-		dev_warn(&rcfw->pdev->dev,
-			 "QPLIB: Bono Error: timeout %d msec, msg {0x%x}\n",
-			 RCFW_CMD_WAIT_TIME_MS, cookie);
-	}
-
-	return rc;
+	return rc ? 0 : -ETIMEDOUT;
 };
 
-int bnxt_qplib_rcfw_block_for_resp(struct bnxt_qplib_rcfw *rcfw, u16 cookie)
+static int __block_for_resp(struct bnxt_qplib_rcfw *rcfw, u16 cookie)
 {
-	u32 count = -1;
+	u32 count = RCFW_BLOCKED_CMD_WAIT_COUNT;
 	u16 cbit;
 
-	cookie &= RCFW_MAX_COOKIE_VALUE;
 	cbit = cookie % RCFW_MAX_OUTSTANDING_CMD;
 	if (!test_bit(cbit, rcfw->cmdq_bitmap))
 		goto done;
 	do {
+		mdelay(1); /* 1m sec */
 		bnxt_qplib_service_creq((unsigned long)rcfw);
 	} while (test_bit(cbit, rcfw->cmdq_bitmap) && --count);
 done:
-	return count;
+	return count ? 0 : -ETIMEDOUT;
 };
 
-void *bnxt_qplib_rcfw_send_message(struct bnxt_qplib_rcfw *rcfw,
-				   struct cmdq_base *req, void **crsbe,
-				   u8 is_block)
+static int __send_message(struct bnxt_qplib_rcfw *rcfw, struct cmdq_base *req,
+			  struct creq_base *resp, void *sb, u8 is_block)
 {
-	struct bnxt_qplib_crsq *crsq = &rcfw->crsq;
 	struct bnxt_qplib_cmdqe *cmdqe, **cmdq_ptr;
 	struct bnxt_qplib_hwq *cmdq = &rcfw->cmdq;
-	struct bnxt_qplib_hwq *crsb = &rcfw->crsb;
-	struct bnxt_qplib_crsqe *crsqe = NULL;
-	struct bnxt_qplib_crsbe **crsb_ptr;
+	struct bnxt_qplib_crsq *crsqe;
 	u32 sw_prod, cmdq_prod;
-	u8 retry_cnt = 0xFF;
-	dma_addr_t dma_addr;
 	unsigned long flags;
 	u32 size, opcode;
 	u16 cookie, cbit;
 	int pg, idx;
 	u8 *preq;
 
-retry:
 	opcode = req->opcode;
 	if (!test_bit(FIRMWARE_INITIALIZED_FLAG, &rcfw->flags) &&
 	    (opcode != CMDQ_BASE_OPCODE_QUERY_FUNC &&
@@ -112,63 +95,50 @@ retry:
 		dev_err(&rcfw->pdev->dev,
 			"QPLIB: RCFW not initialized, reject opcode 0x%x",
 			opcode);
-		return NULL;
+		return -EINVAL;
 	}
 
 	if (test_bit(FIRMWARE_INITIALIZED_FLAG, &rcfw->flags) &&
 	    opcode == CMDQ_BASE_OPCODE_INITIALIZE_FW) {
 		dev_err(&rcfw->pdev->dev, "QPLIB: RCFW already initialized!");
-		return NULL;
+		return -EINVAL;
 	}
 
 	/* Cmdq are in 16-byte units, each request can consume 1 or more
 	 * cmdqe
 	 */
 	spin_lock_irqsave(&cmdq->lock, flags);
-	if (req->cmd_size > cmdq->max_elements -
-	    ((HWQ_CMP(cmdq->prod, cmdq) - HWQ_CMP(cmdq->cons, cmdq)) &
-	     (cmdq->max_elements - 1))) {
+	if (req->cmd_size >= HWQ_FREE_SLOTS(cmdq)) {
 		dev_err(&rcfw->pdev->dev, "QPLIB: RCFW: CMDQ is full!");
 		spin_unlock_irqrestore(&cmdq->lock, flags);
-
-		if (!retry_cnt--)
-			return NULL;
-		goto retry;
+		return -EAGAIN;
 	}
 
-	retry_cnt = 0xFF;
 
-	cookie = atomic_inc_return(&rcfw->seq_num) & RCFW_MAX_COOKIE_VALUE;
+	cookie = rcfw->seq_num & RCFW_MAX_COOKIE_VALUE;
 	cbit = cookie % RCFW_MAX_OUTSTANDING_CMD;
 	if (is_block)
 		cookie |= RCFW_CMD_IS_BLOCKING;
+
+	set_bit(cbit, rcfw->cmdq_bitmap);
 	req->cookie = cpu_to_le16(cookie);
-	if (test_and_set_bit(cbit, rcfw->cmdq_bitmap)) {
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: RCFW MAX outstanding cmd reached!");
-		atomic_dec(&rcfw->seq_num);
+	crsqe = &rcfw->crsqe_tbl[cbit];
+	if (crsqe->resp) {
 		spin_unlock_irqrestore(&cmdq->lock, flags);
-
-		if (!retry_cnt--)
-			return NULL;
-		goto retry;
+		return -EBUSY;
 	}
-	/* Reserve a resp buffer slot if requested */
-	if (req->resp_size && crsbe) {
-		spin_lock(&crsb->lock);
-		sw_prod = HWQ_CMP(crsb->prod, crsb);
-		crsb_ptr = (struct bnxt_qplib_crsbe **)crsb->pbl_ptr;
-		*crsbe = (void *)&crsb_ptr[get_crsb_pg(sw_prod)]
-					  [get_crsb_idx(sw_prod)];
-		bnxt_qplib_crsb_dma_next(crsb->pbl_dma_ptr, sw_prod, &dma_addr);
-		req->resp_addr = cpu_to_le64(dma_addr);
-		crsb->prod++;
-		spin_unlock(&crsb->lock);
-
-		req->resp_size = (sizeof(struct bnxt_qplib_crsbe) +
-				  BNXT_QPLIB_CMDQE_UNITS - 1) /
-				 BNXT_QPLIB_CMDQE_UNITS;
+	memset(resp, 0, sizeof(*resp));
+	crsqe->resp = (struct creq_qp_event *)resp;
+	crsqe->resp->cookie = req->cookie;
+	crsqe->req_size = req->cmd_size;
+	if (req->resp_size && sb) {
+		struct bnxt_qplib_rcfw_sbuf *sbuf = sb;
+
+		req->resp_addr = cpu_to_le64(sbuf->dma_addr);
+		req->resp_size = (sbuf->size + BNXT_QPLIB_CMDQE_UNITS - 1) /
+				  BNXT_QPLIB_CMDQE_UNITS;
 	}
+
 	cmdq_ptr = (struct bnxt_qplib_cmdqe **)cmdq->pbl_ptr;
 	preq = (u8 *)req;
 	size = req->cmd_size * BNXT_QPLIB_CMDQE_UNITS;
@@ -190,23 +160,24 @@ retry:
 		preq += min_t(u32, size, sizeof(*cmdqe));
 		size -= min_t(u32, size, sizeof(*cmdqe));
 		cmdq->prod++;
+		rcfw->seq_num++;
 	} while (size > 0);
 
+	rcfw->seq_num++;
+
 	cmdq_prod = cmdq->prod;
 	if (rcfw->flags & FIRMWARE_FIRST_FLAG) {
-		/* The very first doorbell write is required to set this flag
-		 * which prompts the FW to reset its internal pointers
+		/* The very first doorbell write
+		 * is required to set this flag
+		 * which prompts the FW to reset
+		 * its internal pointers
 		 */
 		cmdq_prod |= FIRMWARE_FIRST_FLAG;
 		rcfw->flags &= ~FIRMWARE_FIRST_FLAG;
 	}
-	sw_prod = HWQ_CMP(crsq->prod, crsq);
-	crsqe = &crsq->crsq[sw_prod];
-	memset(crsqe, 0, sizeof(*crsqe));
-	crsq->prod++;
-	crsqe->req_size = req->cmd_size;
 
 	/* ring CMDQ DB */
+	wmb();
 	writel(cmdq_prod, rcfw->cmdq_bar_reg_iomem +
 	       rcfw->cmdq_bar_reg_prod_off);
 	writel(RCFW_CMDQ_TRIG_VAL, rcfw->cmdq_bar_reg_iomem +
@@ -214,9 +185,56 @@ retry:
 done:
 	spin_unlock_irqrestore(&cmdq->lock, flags);
 	/* Return the CREQ response pointer */
-	return crsqe ? &crsqe->qp_event : NULL;
+	return 0;
 }
 
+int bnxt_qplib_rcfw_send_message(struct bnxt_qplib_rcfw *rcfw,
+				 struct cmdq_base *req,
+				 struct creq_base *resp,
+				 void *sb, u8 is_block)
+{
+	struct creq_qp_event *evnt = (struct creq_qp_event *)resp;
+	u16 cookie;
+	u8 opcode, retry_cnt = 0xFF;
+	int rc = 0;
+
+	do {
+		opcode = req->opcode;
+		rc = __send_message(rcfw, req, resp, sb, is_block);
+		cookie = le16_to_cpu(req->cookie) & RCFW_MAX_COOKIE_VALUE;
+		if (!rc)
+			break;
+
+		if (!retry_cnt || (rc != -EAGAIN && rc != -EBUSY)) {
+			/* send failed */
+			dev_err(&rcfw->pdev->dev, "QPLIB: cmdq[%#x]=%#x send failed",
+				cookie, opcode);
+			return rc;
+		}
+		is_block ? mdelay(1) : usleep_range(500, 1000);
+
+	} while (retry_cnt--);
+
+	if (is_block)
+		rc = __block_for_resp(rcfw, cookie);
+	else
+		rc = __wait_for_resp(rcfw, cookie);
+	if (rc) {
+		/* timed out */
+		dev_err(&rcfw->pdev->dev, "QPLIB: cmdq[%#x]=%#x timedout (%d)msec",
+			cookie, opcode, RCFW_CMD_WAIT_TIME_MS);
+		return rc;
+	}
+
+	if (evnt->status) {
+		/* failed with status */
+		dev_err(&rcfw->pdev->dev, "QPLIB: cmdq[%#x]=%#x status %#x",
+			cookie, opcode, evnt->status);
+		rc = -EFAULT;
+	}
+
+	return rc;
+}
 /* Completions */
 static int bnxt_qplib_process_func_event(struct bnxt_qplib_rcfw *rcfw,
 					 struct creq_func_event *func_event)
@@ -260,12 +278,12 @@ static int bnxt_qplib_process_func_event(struct bnxt_qplib_rcfw *rcfw,
 static int bnxt_qplib_process_qp_event(struct bnxt_qplib_rcfw *rcfw,
 				       struct creq_qp_event *qp_event)
 {
-	struct bnxt_qplib_crsq *crsq = &rcfw->crsq;
 	struct bnxt_qplib_hwq *cmdq = &rcfw->cmdq;
-	struct bnxt_qplib_crsqe *crsqe;
-	u16 cbit, cookie, blocked = 0;
+	struct bnxt_qplib_crsq *crsqe;
 	unsigned long flags;
-	u32 sw_cons;
+	u16 cbit, blocked = 0;
+	u16 cookie;
+	__le16  mcookie;
 
 	switch (qp_event->event) {
 	case CREQ_QP_EVENT_EVENT_QP_ERROR_NOTIFICATION:
@@ -275,24 +293,31 @@ static int bnxt_qplib_process_qp_event(struct bnxt_qplib_rcfw *rcfw,
 	default:
 		/* Command Response */
 		spin_lock_irqsave(&cmdq->lock, flags);
-		sw_cons = HWQ_CMP(crsq->cons, crsq);
-		crsqe = &crsq->crsq[sw_cons];
-		crsq->cons++;
-		memcpy(&crsqe->qp_event, qp_event, sizeof(crsqe->qp_event));
-
-		cookie = le16_to_cpu(crsqe->qp_event.cookie);
+		cookie = le16_to_cpu(qp_event->cookie);
+		mcookie = qp_event->cookie;
 		blocked = cookie & RCFW_CMD_IS_BLOCKING;
 		cookie &= RCFW_MAX_COOKIE_VALUE;
 		cbit = cookie % RCFW_MAX_OUTSTANDING_CMD;
+		crsqe = &rcfw->crsqe_tbl[cbit];
+		if (crsqe->resp &&
+		    crsqe->resp->cookie  == mcookie) {
+			memcpy(crsqe->resp, qp_event, sizeof(*qp_event));
+			crsqe->resp = NULL;
+		} else {
+			dev_err(&rcfw->pdev->dev,
+				"QPLIB: CMD %s resp->cookie = %#x, evnt->cookie = %#x",
+				crsqe->resp ? "mismatch" : "collision",
+				crsqe->resp ? crsqe->resp->cookie : 0, mcookie);
+		}
 		if (!test_and_clear_bit(cbit, rcfw->cmdq_bitmap))
 			dev_warn(&rcfw->pdev->dev,
 				 "QPLIB: CMD bit %d was not requested", cbit);
-
 		cmdq->cons += crsqe->req_size;
-		spin_unlock_irqrestore(&cmdq->lock, flags);
+		crsqe->req_size = 0;
+
 		if (!blocked)
 			wake_up(&rcfw->waitq);
-		break;
+		spin_unlock_irqrestore(&cmdq->lock, flags);
 	}
 	return 0;
 }
@@ -305,12 +330,12 @@ static void bnxt_qplib_service_creq(unsigned long data)
 	struct creq_base *creqe, **creq_ptr;
 	u32 sw_cons, raw_cons;
 	unsigned long flags;
-	u32 type;
+	u32 type, budget = CREQ_ENTRY_POLL_BUDGET;
 
-	/* Service the CREQ until empty */
+	/* Service the CREQ until budget is over */
 	spin_lock_irqsave(&creq->lock, flags);
 	raw_cons = creq->cons;
-	while (1) {
+	while (budget > 0) {
 		sw_cons = HWQ_CMP(raw_cons, creq);
 		creq_ptr = (struct creq_base **)creq->pbl_ptr;
 		creqe = &creq_ptr[get_creq_pg(sw_cons)][get_creq_idx(sw_cons)];
@@ -320,15 +345,9 @@ static void bnxt_qplib_service_creq(unsigned long data)
 		type = creqe->type & CREQ_BASE_TYPE_MASK;
 		switch (type) {
 		case CREQ_BASE_TYPE_QP_EVENT:
-			if (!bnxt_qplib_process_qp_event
-			    (rcfw, (struct creq_qp_event *)creqe))
-				rcfw->creq_qp_event_processed++;
-			else {
-				dev_warn(&rcfw->pdev->dev, "QPLIB: crsqe with");
-				dev_warn(&rcfw->pdev->dev,
-					 "QPLIB: type = 0x%x not handled",
-					 type);
-			}
+			bnxt_qplib_process_qp_event
+				(rcfw, (struct creq_qp_event *)creqe);
+			rcfw->creq_qp_event_processed++;
 			break;
 		case CREQ_BASE_TYPE_FUNC_EVENT:
 			if (!bnxt_qplib_process_func_event
@@ -346,7 +365,9 @@ static void bnxt_qplib_service_creq(unsigned long data)
 			break;
 		}
 		raw_cons++;
+		budget--;
 	}
+
 	if (creq->cons != raw_cons) {
 		creq->cons = raw_cons;
 		CREQ_DB_REARM(rcfw->creq_bar_reg_iomem, raw_cons,
@@ -375,23 +396,16 @@ static irqreturn_t bnxt_qplib_creq_irq(int irq, void *dev_instance)
 /* RCFW */
 int bnxt_qplib_deinit_rcfw(struct bnxt_qplib_rcfw *rcfw)
 {
-	struct creq_deinitialize_fw_resp *resp;
 	struct cmdq_deinitialize_fw req;
+	struct creq_deinitialize_fw_resp resp;
 	u16 cmd_flags = 0;
+	int rc;
 
 	RCFW_CMD_PREP(req, DEINITIALIZE_FW, cmd_flags);
-	resp = (struct creq_deinitialize_fw_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, 0);
-	if (!resp)
-		return -EINVAL;
-
-	if (!bnxt_qplib_rcfw_wait_for_resp(rcfw, le16_to_cpu(req.cookie)))
-		return -ETIMEDOUT;
-
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie))
-		return -EFAULT;
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req, (void *)&resp,
+					  NULL, 0);
+	if (rc)
+		return rc;
 
 	clear_bit(FIRMWARE_INITIALIZED_FLAG, &rcfw->flags);
 	return 0;
@@ -417,9 +431,10 @@ static int __get_pbl_pg_idx(struct bnxt_qplib_pbl *pbl)
 int bnxt_qplib_init_rcfw(struct bnxt_qplib_rcfw *rcfw,
 			 struct bnxt_qplib_ctx *ctx, int is_virtfn)
 {
-	struct creq_initialize_fw_resp *resp;
 	struct cmdq_initialize_fw req;
+	struct creq_initialize_fw_resp resp;
 	u16 cmd_flags = 0, level;
+	int rc;
 
 	RCFW_CMD_PREP(req, INITIALIZE_FW, cmd_flags);
 
@@ -482,37 +497,19 @@ int bnxt_qplib_init_rcfw(struct bnxt_qplib_rcfw *rcfw,
 
 skip_ctx_setup:
 	req.stat_ctx_id = cpu_to_le32(ctx->stats.fw_id);
-	resp = (struct creq_initialize_fw_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, 0);
-	if (!resp) {
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: RCFW: INITIALIZE_FW send failed");
-		return -EINVAL;
-	}
-	if (!bnxt_qplib_rcfw_wait_for_resp(rcfw, le16_to_cpu(req.cookie))) {
-		/* Cmd timed out */
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: RCFW: INITIALIZE_FW timed out");
-		return -ETIMEDOUT;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: RCFW: INITIALIZE_FW failed");
-		return -EINVAL;
-	}
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req, (void *)&resp,
+					  NULL, 0);
+	if (rc)
+		return rc;
 	set_bit(FIRMWARE_INITIALIZED_FLAG, &rcfw->flags);
 	return 0;
 }
 
 void bnxt_qplib_free_rcfw_channel(struct bnxt_qplib_rcfw *rcfw)
 {
-	bnxt_qplib_free_hwq(rcfw->pdev, &rcfw->crsb);
-	kfree(rcfw->crsq.crsq);
+	kfree(rcfw->crsqe_tbl);
 	bnxt_qplib_free_hwq(rcfw->pdev, &rcfw->cmdq);
 	bnxt_qplib_free_hwq(rcfw->pdev, &rcfw->creq);
-
 	rcfw->pdev = NULL;
 }
 
@@ -539,21 +536,11 @@ int bnxt_qplib_alloc_rcfw_channel(struct pci_dev *pdev,
 		goto fail;
 	}
 
-	rcfw->crsq.max_elements = rcfw->cmdq.max_elements;
-	rcfw->crsq.crsq = kcalloc(rcfw->crsq.max_elements,
-				  sizeof(*rcfw->crsq.crsq), GFP_KERNEL);
-	if (!rcfw->crsq.crsq)
+	rcfw->crsqe_tbl = kcalloc(rcfw->cmdq.max_elements,
+				  sizeof(*rcfw->crsqe_tbl), GFP_KERNEL);
+	if (!rcfw->crsqe_tbl)
 		goto fail;
 
-	rcfw->crsb.max_elements = BNXT_QPLIB_CRSBE_MAX_CNT;
-	if (bnxt_qplib_alloc_init_hwq(rcfw->pdev, &rcfw->crsb, NULL, 0,
-				      &rcfw->crsb.max_elements,
-				      BNXT_QPLIB_CRSBE_UNITS, 0, PAGE_SIZE,
-				      HWQ_TYPE_CTX)) {
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: HW channel CRSB allocation failed");
-		goto fail;
-	}
 	return 0;
 
 fail:
@@ -606,7 +593,7 @@ int bnxt_qplib_enable_rcfw_channel(struct pci_dev *pdev,
 	int rc;
 
 	/* General */
-	atomic_set(&rcfw->seq_num, 0);
+	rcfw->seq_num = 0;
 	rcfw->flags = FIRMWARE_FIRST_FLAG;
 	bmap_size = BITS_TO_LONGS(RCFW_MAX_OUTSTANDING_CMD *
 				  sizeof(unsigned long));
@@ -636,10 +623,6 @@ int bnxt_qplib_enable_rcfw_channel(struct pci_dev *pdev,
 
 	rcfw->cmdq_bar_reg_trig_off = RCFW_COMM_TRIG_OFFSET;
 
-	/* CRSQ */
-	rcfw->crsq.prod = 0;
-	rcfw->crsq.cons = 0;
-
 	/* CREQ */
 	rcfw->creq_bar_reg = RCFW_COMM_CONS_PCI_BAR_REGION;
 	res_base = pci_resource_start(pdev, rcfw->creq_bar_reg);
@@ -692,3 +675,34 @@ int bnxt_qplib_enable_rcfw_channel(struct pci_dev *pdev,
 	__iowrite32_copy(rcfw->cmdq_bar_reg_iomem, &init, sizeof(init) / 4);
 	return 0;
 }
+
+struct bnxt_qplib_rcfw_sbuf *bnxt_qplib_rcfw_alloc_sbuf(
+		struct bnxt_qplib_rcfw *rcfw,
+		u32 size)
+{
+	struct bnxt_qplib_rcfw_sbuf *sbuf;
+
+	sbuf = kzalloc(sizeof(*sbuf), GFP_ATOMIC);
+	if (!sbuf)
+		return NULL;
+
+	sbuf->size = size;
+	sbuf->sb = dma_zalloc_coherent(&rcfw->pdev->dev, sbuf->size,
+				       &sbuf->dma_addr, GFP_ATOMIC);
+	if (!sbuf->sb)
+		goto bail;
+
+	return sbuf;
+bail:
+	kfree(sbuf);
+	return NULL;
+}
+
+void bnxt_qplib_rcfw_free_sbuf(struct bnxt_qplib_rcfw *rcfw,
+			       struct bnxt_qplib_rcfw_sbuf *sbuf)
+{
+	if (sbuf->sb)
+		dma_free_coherent(&rcfw->pdev->dev, sbuf->size,
+				  sbuf->sb, sbuf->dma_addr);
+	kfree(sbuf);
+}
diff --git a/drivers/infiniband/hw/bnxt_re/qplib_rcfw.h b/drivers/infiniband/hw/bnxt_re/qplib_rcfw.h
index d3567d75bf58..09ce121770cd 100644
--- a/drivers/infiniband/hw/bnxt_re/qplib_rcfw.h
+++ b/drivers/infiniband/hw/bnxt_re/qplib_rcfw.h
@@ -73,6 +73,7 @@
 #define RCFW_MAX_OUTSTANDING_CMD	BNXT_QPLIB_CMDQE_MAX_CNT
 #define RCFW_MAX_COOKIE_VALUE		0x7FFF
 #define RCFW_CMD_IS_BLOCKING		0x8000
+#define RCFW_BLOCKED_CMD_WAIT_COUNT	0x4E20
 
 /* Cmdq contains a fix number of a 16-Byte slots */
 struct bnxt_qplib_cmdqe {
@@ -94,32 +95,6 @@ struct bnxt_qplib_crsbe {
 	u8			data[1024];
 };
 
-/* CRSQ SB */
-#define BNXT_QPLIB_CRSBE_MAX_CNT	4
-#define BNXT_QPLIB_CRSBE_UNITS		sizeof(struct bnxt_qplib_crsbe)
-#define BNXT_QPLIB_CRSBE_CNT_PER_PG	(PAGE_SIZE / BNXT_QPLIB_CRSBE_UNITS)
-
-#define MAX_CRSB_IDX			(BNXT_QPLIB_CRSBE_MAX_CNT - 1)
-#define MAX_CRSB_IDX_PER_PG		(BNXT_QPLIB_CRSBE_CNT_PER_PG - 1)
-
-static inline u32 get_crsb_pg(u32 val)
-{
-	return (val & ~MAX_CRSB_IDX_PER_PG) / BNXT_QPLIB_CRSBE_CNT_PER_PG;
-}
-
-static inline u32 get_crsb_idx(u32 val)
-{
-	return val & MAX_CRSB_IDX_PER_PG;
-}
-
-static inline void bnxt_qplib_crsb_dma_next(dma_addr_t *pg_map_arr,
-					    u32 prod, dma_addr_t *dma_addr)
-{
-		*dma_addr = pg_map_arr[(prod) / BNXT_QPLIB_CRSBE_CNT_PER_PG];
-		*dma_addr += ((prod) % BNXT_QPLIB_CRSBE_CNT_PER_PG) *
-			      BNXT_QPLIB_CRSBE_UNITS;
-}
-
 /* CREQ */
 /* Allocate 1 per QP for async error notification for now */
 #define BNXT_QPLIB_CREQE_MAX_CNT	(64 * 1024)
@@ -158,17 +133,19 @@ static inline u32 get_creq_idx(u32 val)
 #define CREQ_DB(db, raw_cons, cp_bit)				\
 	writel(CREQ_DB_CP_FLAGS | ((raw_cons) & ((cp_bit) - 1)), db)
 
+#define CREQ_ENTRY_POLL_BUDGET		0x100
+
 /* HWQ */
-struct bnxt_qplib_crsqe {
-	struct creq_qp_event	qp_event;
+
+struct bnxt_qplib_crsq {
+	struct creq_qp_event	*resp;
 	u32			req_size;
 };
 
-struct bnxt_qplib_crsq {
-	struct bnxt_qplib_crsqe	*crsq;
-	u32			prod;
-	u32			cons;
-	u32			max_elements;
+struct bnxt_qplib_rcfw_sbuf {
+	void *sb;
+	dma_addr_t dma_addr;
+	u32 size;
 };
 
 /* RCFW Communication Channels */
@@ -185,7 +162,7 @@ struct bnxt_qplib_rcfw {
 	wait_queue_head_t	waitq;
 	int			(*aeq_handler)(struct bnxt_qplib_rcfw *,
 					       struct creq_func_event *);
-	atomic_t		seq_num;
+	u32			seq_num;
 
 	/* Bar region info */
 	void __iomem		*cmdq_bar_reg_iomem;
@@ -203,8 +180,7 @@ struct bnxt_qplib_rcfw {
 
 	/* Actual Cmd and Resp Queues */
 	struct bnxt_qplib_hwq	cmdq;
-	struct bnxt_qplib_crsq	crsq;
-	struct bnxt_qplib_hwq	crsb;
+	struct bnxt_qplib_crsq	*crsqe_tbl;
 };
 
 void bnxt_qplib_free_rcfw_channel(struct bnxt_qplib_rcfw *rcfw);
@@ -219,11 +195,14 @@ int bnxt_qplib_enable_rcfw_channel(struct pci_dev *pdev,
 					(struct bnxt_qplib_rcfw *,
 					 struct creq_func_event *));
 
-int bnxt_qplib_rcfw_block_for_resp(struct bnxt_qplib_rcfw *rcfw, u16 cookie);
-int bnxt_qplib_rcfw_wait_for_resp(struct bnxt_qplib_rcfw *rcfw, u16 cookie);
-void *bnxt_qplib_rcfw_send_message(struct bnxt_qplib_rcfw *rcfw,
-				   struct cmdq_base *req, void **crsbe,
-				   u8 is_block);
+struct bnxt_qplib_rcfw_sbuf *bnxt_qplib_rcfw_alloc_sbuf(
+				struct bnxt_qplib_rcfw *rcfw,
+				u32 size);
+void bnxt_qplib_rcfw_free_sbuf(struct bnxt_qplib_rcfw *rcfw,
+			       struct bnxt_qplib_rcfw_sbuf *sbuf);
+int bnxt_qplib_rcfw_send_message(struct bnxt_qplib_rcfw *rcfw,
+				 struct cmdq_base *req, struct creq_base *resp,
+				 void *sbuf, u8 is_block);
 
 int bnxt_qplib_deinit_rcfw(struct bnxt_qplib_rcfw *rcfw);
 int bnxt_qplib_init_rcfw(struct bnxt_qplib_rcfw *rcfw,
diff --git a/drivers/infiniband/hw/bnxt_re/qplib_res.h b/drivers/infiniband/hw/bnxt_re/qplib_res.h
index 6277d802ca4b..2e4855509719 100644
--- a/drivers/infiniband/hw/bnxt_re/qplib_res.h
+++ b/drivers/infiniband/hw/bnxt_re/qplib_res.h
@@ -48,6 +48,10 @@ extern const struct bnxt_qplib_gid bnxt_qplib_gid_zero;
 
 #define HWQ_CMP(idx, hwq)	((idx) & ((hwq)->max_elements - 1))
 
+#define HWQ_FREE_SLOTS(hwq)	(hwq->max_elements - \
+				((HWQ_CMP(hwq->prod, hwq)\
+				- HWQ_CMP(hwq->cons, hwq))\
+				& (hwq->max_elements - 1)))
 enum bnxt_qplib_hwq_type {
 	HWQ_TYPE_CTX,
 	HWQ_TYPE_QUEUE,
diff --git a/drivers/infiniband/hw/bnxt_re/qplib_sp.c b/drivers/infiniband/hw/bnxt_re/qplib_sp.c
index 7b31eccedf11..fde18cf0e406 100644
--- a/drivers/infiniband/hw/bnxt_re/qplib_sp.c
+++ b/drivers/infiniband/hw/bnxt_re/qplib_sp.c
@@ -55,37 +55,30 @@ int bnxt_qplib_get_dev_attr(struct bnxt_qplib_rcfw *rcfw,
 			    struct bnxt_qplib_dev_attr *attr)
 {
 	struct cmdq_query_func req;
-	struct creq_query_func_resp *resp;
+	struct creq_query_func_resp resp;
+	struct bnxt_qplib_rcfw_sbuf *sbuf;
 	struct creq_query_func_resp_sb *sb;
 	u16 cmd_flags = 0;
 	u32 temp;
 	u8 *tqm_alloc;
-	int i;
+	int i, rc = 0;
 
 	RCFW_CMD_PREP(req, QUERY_FUNC, cmd_flags);
 
-	req.resp_size = sizeof(*sb) / BNXT_QPLIB_CMDQE_UNITS;
-	resp = (struct creq_query_func_resp *)
-		bnxt_qplib_rcfw_send_message(rcfw, (void *)&req, (void **)&sb,
-					     0);
-	if (!resp) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: QUERY_FUNC send failed");
-		return -EINVAL;
-	}
-	if (!bnxt_qplib_rcfw_wait_for_resp(rcfw, le16_to_cpu(req.cookie))) {
-		/* Cmd timed out */
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: QUERY_FUNC timed out");
-		return -ETIMEDOUT;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: QUERY_FUNC failed ");
+	sbuf = bnxt_qplib_rcfw_alloc_sbuf(rcfw, sizeof(*sb));
+	if (!sbuf) {
 		dev_err(&rcfw->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		return -EINVAL;
+			"QPLIB: SP: QUERY_FUNC alloc side buffer failed");
+		return -ENOMEM;
 	}
+
+	sb = sbuf->sb;
+	req.resp_size = sizeof(*sb) / BNXT_QPLIB_CMDQE_UNITS;
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req, (void *)&resp,
+					  (void *)sbuf, 0);
+	if (rc)
+		goto bail;
+
 	/* Extract the context from the side buffer */
 	attr->max_qp = le32_to_cpu(sb->max_qp);
 	attr->max_qp_rd_atom =
@@ -95,6 +88,11 @@ int bnxt_qplib_get_dev_attr(struct bnxt_qplib_rcfw *rcfw,
 		sb->max_qp_init_rd_atom > BNXT_QPLIB_MAX_OUT_RD_ATOM ?
 		BNXT_QPLIB_MAX_OUT_RD_ATOM : sb->max_qp_init_rd_atom;
 	attr->max_qp_wqes = le16_to_cpu(sb->max_qp_wr);
+	/*
+	 * 128 WQEs needs to be reserved for the HW (8916). Prevent
+	 * reporting the max number
+	 */
+	attr->max_qp_wqes -= BNXT_QPLIB_RESERVED_QP_WRS;
 	attr->max_qp_sges = sb->max_sge;
 	attr->max_cq = le32_to_cpu(sb->max_cq);
 	attr->max_cq_wqes = le32_to_cpu(sb->max_cqe);
@@ -130,7 +128,10 @@ int bnxt_qplib_get_dev_attr(struct bnxt_qplib_rcfw *rcfw,
 		attr->tqm_alloc_reqs[i * 4 + 2] = *(++tqm_alloc);
 		attr->tqm_alloc_reqs[i * 4 + 3] = *(++tqm_alloc);
 	}
-	return 0;
+
+bail:
+	bnxt_qplib_rcfw_free_sbuf(rcfw, sbuf);
+	return rc;
 }
 
 /* SGID */
@@ -178,8 +179,9 @@ int bnxt_qplib_del_sgid(struct bnxt_qplib_sgid_tbl *sgid_tbl,
 	/* Remove GID from the SGID table */
 	if (update) {
 		struct cmdq_delete_gid req;
-		struct creq_delete_gid_resp *resp;
+		struct creq_delete_gid_resp resp;
 		u16 cmd_flags = 0;
+		int rc;
 
 		RCFW_CMD_PREP(req, DELETE_GID, cmd_flags);
 		if (sgid_tbl->hw_id[index] == 0xFFFF) {
@@ -188,31 +190,10 @@ int bnxt_qplib_del_sgid(struct bnxt_qplib_sgid_tbl *sgid_tbl,
 			return -EINVAL;
 		}
 		req.gid_index = cpu_to_le16(sgid_tbl->hw_id[index]);
-		resp = (struct creq_delete_gid_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req, NULL,
-						     0);
-		if (!resp) {
-			dev_err(&res->pdev->dev,
-				"QPLIB: SP: DELETE_GID send failed");
-			return -EINVAL;
-		}
-		if (!bnxt_qplib_rcfw_wait_for_resp(rcfw,
-						   le16_to_cpu(req.cookie))) {
-			/* Cmd timed out */
-			dev_err(&res->pdev->dev,
-				"QPLIB: SP: DELETE_GID timed out");
-			return -ETIMEDOUT;
-		}
-		if (resp->status ||
-		    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-			dev_err(&res->pdev->dev,
-				"QPLIB: SP: DELETE_GID failed ");
-			dev_err(&res->pdev->dev,
-				"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-				resp->status, le16_to_cpu(req.cookie),
-				le16_to_cpu(resp->cookie));
-			return -EINVAL;
-		}
+		rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
+						  (void *)&resp, NULL, 0);
+		if (rc)
+			return rc;
 	}
 	memcpy(&sgid_tbl->tbl[index], &bnxt_qplib_gid_zero,
 	       sizeof(bnxt_qplib_gid_zero));
@@ -234,7 +215,7 @@ int bnxt_qplib_add_sgid(struct bnxt_qplib_sgid_tbl *sgid_tbl,
 						   struct bnxt_qplib_res,
 						   sgid_tbl);
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
-	int i, free_idx, rc = 0;
+	int i, free_idx;
 
 	if (!sgid_tbl) {
 		dev_err(&res->pdev->dev, "QPLIB: SGID table not allocated");
@@ -266,10 +247,11 @@ int bnxt_qplib_add_sgid(struct bnxt_qplib_sgid_tbl *sgid_tbl,
 	}
 	if (update) {
 		struct cmdq_add_gid req;
-		struct creq_add_gid_resp *resp;
+		struct creq_add_gid_resp resp;
 		u16 cmd_flags = 0;
 		u32 temp32[4];
 		u16 temp16[3];
+		int rc;
 
 		RCFW_CMD_PREP(req, ADD_GID, cmd_flags);
 
@@ -290,31 +272,11 @@ int bnxt_qplib_add_sgid(struct bnxt_qplib_sgid_tbl *sgid_tbl,
 		req.src_mac[1] = cpu_to_be16(temp16[1]);
 		req.src_mac[2] = cpu_to_be16(temp16[2]);
 
-		resp = (struct creq_add_gid_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, 0);
-		if (!resp) {
-			dev_err(&res->pdev->dev,
-				"QPLIB: SP: ADD_GID send failed");
-			return -EINVAL;
-		}
-		if (!bnxt_qplib_rcfw_wait_for_resp(rcfw,
-						   le16_to_cpu(req.cookie))) {
-			/* Cmd timed out */
-			dev_err(&res->pdev->dev,
-				"QPIB: SP: ADD_GID timed out");
-			return -ETIMEDOUT;
-		}
-		if (resp->status ||
-		    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-			dev_err(&res->pdev->dev, "QPLIB: SP: ADD_GID failed ");
-			dev_err(&res->pdev->dev,
-				"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-				resp->status, le16_to_cpu(req.cookie),
-				le16_to_cpu(resp->cookie));
-			return -EINVAL;
-		}
-		sgid_tbl->hw_id[free_idx] = le32_to_cpu(resp->xid);
+		rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
+						  (void *)&resp, NULL, 0);
+		if (rc)
+			return rc;
+		sgid_tbl->hw_id[free_idx] = le32_to_cpu(resp.xid);
 	}
 	/* Add GID to the sgid_tbl */
 	memcpy(&sgid_tbl->tbl[free_idx], gid, sizeof(*gid));
@@ -325,7 +287,7 @@ int bnxt_qplib_add_sgid(struct bnxt_qplib_sgid_tbl *sgid_tbl,
 
 	*index = free_idx;
 	/* unlock */
-	return rc;
+	return 0;
 }
 
 /* pkeys */
@@ -422,10 +384,11 @@ int bnxt_qplib_create_ah(struct bnxt_qplib_res *res, struct bnxt_qplib_ah *ah)
 {
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct cmdq_create_ah req;
-	struct creq_create_ah_resp *resp;
+	struct creq_create_ah_resp resp;
 	u16 cmd_flags = 0;
 	u32 temp32[4];
 	u16 temp16[3];
+	int rc;
 
 	RCFW_CMD_PREP(req, CREATE_AH, cmd_flags);
 
@@ -450,28 +413,12 @@ int bnxt_qplib_create_ah(struct bnxt_qplib_res *res, struct bnxt_qplib_ah *ah)
 	req.dest_mac[1] = cpu_to_le16(temp16[1]);
 	req.dest_mac[2] = cpu_to_le16(temp16[2]);
 
-	resp = (struct creq_create_ah_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, 1);
-	if (!resp) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: CREATE_AH send failed");
-		return -EINVAL;
-	}
-	if (!bnxt_qplib_rcfw_block_for_resp(rcfw, le16_to_cpu(req.cookie))) {
-		/* Cmd timed out */
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: CREATE_AH timed out");
-		return -ETIMEDOUT;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: CREATE_AH failed ");
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		return -EINVAL;
-	}
-	ah->id = le32_to_cpu(resp->xid);
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req, (void *)&resp,
+					  NULL, 1);
+	if (rc)
+		return rc;
+
+	ah->id = le32_to_cpu(resp.xid);
 	return 0;
 }
 
@@ -479,35 +426,19 @@ int bnxt_qplib_destroy_ah(struct bnxt_qplib_res *res, struct bnxt_qplib_ah *ah)
 {
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct cmdq_destroy_ah req;
-	struct creq_destroy_ah_resp *resp;
+	struct creq_destroy_ah_resp resp;
 	u16 cmd_flags = 0;
+	int rc;
 
 	/* Clean up the AH table in the device */
 	RCFW_CMD_PREP(req, DESTROY_AH, cmd_flags);
 
 	req.ah_cid = cpu_to_le32(ah->id);
 
-	resp = (struct creq_destroy_ah_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, 1);
-	if (!resp) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: DESTROY_AH send failed");
-		return -EINVAL;
-	}
-	if (!bnxt_qplib_rcfw_block_for_resp(rcfw, le16_to_cpu(req.cookie))) {
-		/* Cmd timed out */
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: DESTROY_AH timed out");
-		return -ETIMEDOUT;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: DESTROY_AH failed ");
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		return -EINVAL;
-	}
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req, (void *)&resp,
+					  NULL, 1);
+	if (rc)
+		return rc;
 	return 0;
 }
 
@@ -516,8 +447,9 @@ int bnxt_qplib_free_mrw(struct bnxt_qplib_res *res, struct bnxt_qplib_mrw *mrw)
 {
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct cmdq_deallocate_key req;
-	struct creq_deallocate_key_resp *resp;
+	struct creq_deallocate_key_resp resp;
 	u16 cmd_flags = 0;
+	int rc;
 
 	if (mrw->lkey == 0xFFFFFFFF) {
 		dev_info(&res->pdev->dev,
@@ -536,27 +468,11 @@ int bnxt_qplib_free_mrw(struct bnxt_qplib_res *res, struct bnxt_qplib_mrw *mrw)
 	else
 		req.key = cpu_to_le32(mrw->lkey);
 
-	resp = (struct creq_deallocate_key_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, 0);
-	if (!resp) {
-		dev_err(&res->pdev->dev, "QPLIB: SP: FREE_MR send failed");
-		return -EINVAL;
-	}
-	if (!bnxt_qplib_rcfw_wait_for_resp(rcfw, le16_to_cpu(req.cookie))) {
-		/* Cmd timed out */
-		dev_err(&res->pdev->dev, "QPLIB: SP: FREE_MR timed out");
-		return -ETIMEDOUT;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&res->pdev->dev, "QPLIB: SP: FREE_MR failed ");
-		dev_err(&res->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		return -EINVAL;
-	}
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req, (void *)&resp,
+					  NULL, 0);
+	if (rc)
+		return rc;
+
 	/* Free the qplib's MRW memory */
 	if (mrw->hwq.max_elements)
 		bnxt_qplib_free_hwq(res->pdev, &mrw->hwq);
@@ -568,9 +484,10 @@ int bnxt_qplib_alloc_mrw(struct bnxt_qplib_res *res, struct bnxt_qplib_mrw *mrw)
 {
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct cmdq_allocate_mrw req;
-	struct creq_allocate_mrw_resp *resp;
+	struct creq_allocate_mrw_resp resp;
 	u16 cmd_flags = 0;
 	unsigned long tmp;
+	int rc;
 
 	RCFW_CMD_PREP(req, ALLOCATE_MRW, cmd_flags);
 
@@ -584,33 +501,17 @@ int bnxt_qplib_alloc_mrw(struct bnxt_qplib_res *res, struct bnxt_qplib_mrw *mrw)
 	tmp = (unsigned long)mrw;
 	req.mrw_handle = cpu_to_le64(tmp);
 
-	resp = (struct creq_allocate_mrw_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, 0);
-	if (!resp) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: ALLOC_MRW send failed");
-		return -EINVAL;
-	}
-	if (!bnxt_qplib_rcfw_wait_for_resp(rcfw, le16_to_cpu(req.cookie))) {
-		/* Cmd timed out */
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: ALLOC_MRW timed out");
-		return -ETIMEDOUT;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: ALLOC_MRW failed ");
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		return -EINVAL;
-	}
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
+					  (void *)&resp, NULL, 0);
+	if (rc)
+		return rc;
+
 	if ((mrw->type == CMDQ_ALLOCATE_MRW_MRW_FLAGS_MW_TYPE1)  ||
 	    (mrw->type == CMDQ_ALLOCATE_MRW_MRW_FLAGS_MW_TYPE2A) ||
 	    (mrw->type == CMDQ_ALLOCATE_MRW_MRW_FLAGS_MW_TYPE2B))
-		mrw->rkey = le32_to_cpu(resp->xid);
+		mrw->rkey = le32_to_cpu(resp.xid);
 	else
-		mrw->lkey = le32_to_cpu(resp->xid);
+		mrw->lkey = le32_to_cpu(resp.xid);
 	return 0;
 }
 
@@ -619,40 +520,17 @@ int bnxt_qplib_dereg_mrw(struct bnxt_qplib_res *res, struct bnxt_qplib_mrw *mrw,
 {
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct cmdq_deregister_mr req;
-	struct creq_deregister_mr_resp *resp;
+	struct creq_deregister_mr_resp resp;
 	u16 cmd_flags = 0;
 	int rc;
 
 	RCFW_CMD_PREP(req, DEREGISTER_MR, cmd_flags);
 
 	req.lkey = cpu_to_le32(mrw->lkey);
-	resp = (struct creq_deregister_mr_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, block);
-	if (!resp) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: DEREG_MR send failed");
-		return -EINVAL;
-	}
-	if (block)
-		rc = bnxt_qplib_rcfw_block_for_resp(rcfw,
-						    le16_to_cpu(req.cookie));
-	else
-		rc = bnxt_qplib_rcfw_wait_for_resp(rcfw,
-						   le16_to_cpu(req.cookie));
-	if (!rc) {
-		/* Cmd timed out */
-		dev_err(&res->pdev->dev, "QPLIB: SP: DEREG_MR timed out");
-		return -ETIMEDOUT;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&rcfw->pdev->dev, "QPLIB: SP: DEREG_MR failed ");
-		dev_err(&rcfw->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		return -EINVAL;
-	}
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
+					  (void *)&resp, NULL, block);
+	if (rc)
+		return rc;
 
 	/* Free the qplib's MR memory */
 	if (mrw->hwq.max_elements) {
@@ -669,7 +547,7 @@ int bnxt_qplib_reg_mr(struct bnxt_qplib_res *res, struct bnxt_qplib_mrw *mr,
 {
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct cmdq_register_mr req;
-	struct creq_register_mr_resp *resp;
+	struct creq_register_mr_resp resp;
 	u16 cmd_flags = 0, level;
 	int pg_ptrs, pages, i, rc;
 	dma_addr_t **pbl_ptr;
@@ -730,36 +608,11 @@ int bnxt_qplib_reg_mr(struct bnxt_qplib_res *res, struct bnxt_qplib_mrw *mr,
 	req.key = cpu_to_le32(mr->lkey);
 	req.mr_size = cpu_to_le64(mr->total_size);
 
-	resp = (struct creq_register_mr_resp *)
-			bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
-						     NULL, block);
-	if (!resp) {
-		dev_err(&res->pdev->dev, "SP: REG_MR send failed");
-		rc = -EINVAL;
-		goto fail;
-	}
-	if (block)
-		rc = bnxt_qplib_rcfw_block_for_resp(rcfw,
-						    le16_to_cpu(req.cookie));
-	else
-		rc = bnxt_qplib_rcfw_wait_for_resp(rcfw,
-						   le16_to_cpu(req.cookie));
-	if (!rc) {
-		/* Cmd timed out */
-		dev_err(&res->pdev->dev, "SP: REG_MR timed out");
-		rc = -ETIMEDOUT;
-		goto fail;
-	}
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&res->pdev->dev, "QPLIB: SP: REG_MR failed ");
-		dev_err(&res->pdev->dev,
-			"QPLIB: SP: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		rc = -EINVAL;
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
+					  (void *)&resp, NULL, block);
+	if (rc)
 		goto fail;
-	}
+
 	return 0;
 
 fail:
@@ -804,35 +657,15 @@ int bnxt_qplib_map_tc2cos(struct bnxt_qplib_res *res, u16 *cids)
 {
 	struct bnxt_qplib_rcfw *rcfw = res->rcfw;
 	struct cmdq_map_tc_to_cos req;
-	struct creq_map_tc_to_cos_resp *resp;
+	struct creq_map_tc_to_cos_resp resp;
 	u16 cmd_flags = 0;
-	int tleft;
+	int rc = 0;
 
 	RCFW_CMD_PREP(req, MAP_TC_TO_COS, cmd_flags);
 	req.cos0 = cpu_to_le16(cids[0]);
 	req.cos1 = cpu_to_le16(cids[1]);
 
-	resp = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req, NULL, 0);
-	if (!resp) {
-		dev_err(&res->pdev->dev, "QPLIB: SP: MAP_TC2COS send failed");
-		return -EINVAL;
-	}
-
-	tleft = bnxt_qplib_rcfw_block_for_resp(rcfw, le16_to_cpu(req.cookie));
-	if (!tleft) {
-		dev_err(&res->pdev->dev, "QPLIB: SP: MAP_TC2COS timed out");
-		return -ETIMEDOUT;
-	}
-
-	if (resp->status ||
-	    le16_to_cpu(resp->cookie) != le16_to_cpu(req.cookie)) {
-		dev_err(&res->pdev->dev, "QPLIB: SP: MAP_TC2COS failed ");
-		dev_err(&res->pdev->dev,
-			"QPLIB: with status 0x%x cmdq 0x%x resp 0x%x",
-			resp->status, le16_to_cpu(req.cookie),
-			le16_to_cpu(resp->cookie));
-		return -EINVAL;
-	}
-
+	rc = bnxt_qplib_rcfw_send_message(rcfw, (void *)&req,
+					  (void *)&resp, NULL, 0);
 	return 0;
 }
diff --git a/drivers/infiniband/hw/bnxt_re/qplib_sp.h b/drivers/infiniband/hw/bnxt_re/qplib_sp.h
index 1442a617e968..a543f959098b 100644
--- a/drivers/infiniband/hw/bnxt_re/qplib_sp.h
+++ b/drivers/infiniband/hw/bnxt_re/qplib_sp.h
@@ -40,6 +40,8 @@
 #ifndef __BNXT_QPLIB_SP_H__
 #define __BNXT_QPLIB_SP_H__
 
+#define BNXT_QPLIB_RESERVED_QP_WRS	128
+
 struct bnxt_qplib_dev_attr {
 	char				fw_ver[32];
 	u16				max_sgid;
diff --git a/drivers/infiniband/hw/cxgb3/cxio_hal.c b/drivers/infiniband/hw/cxgb3/cxio_hal.c
index 558d6a03375d..3eff6541bd6f 100644
--- a/drivers/infiniband/hw/cxgb3/cxio_hal.c
+++ b/drivers/infiniband/hw/cxgb3/cxio_hal.c
@@ -142,8 +142,7 @@ static int cxio_hal_clear_qp_ctx(struct cxio_rdev *rdev_p, u32 qpid)
 		pr_debug("%s alloc_skb failed\n", __func__);
 		return -ENOMEM;
 	}
-	wqe = (struct t3_modify_qp_wr *) skb_put(skb, sizeof(*wqe));
-	memset(wqe, 0, sizeof(*wqe));
+	wqe = skb_put_zero(skb, sizeof(*wqe));
 	build_fw_riwrh((struct fw_riwrh *) wqe, T3_WR_QP_MOD,
 		       T3_COMPLETION_FLAG | T3_NOTIFY_FLAG, 0, qpid, 7,
 		       T3_SOPEOP);
@@ -561,8 +560,7 @@ static int cxio_hal_init_ctrl_qp(struct cxio_rdev *rdev_p)
 	ctx1 |= ((u64) (V_EC_BASE_HI((u32) base_addr & 0xf) | V_EC_RESPQ(0) |
 			V_EC_TYPE(0) | V_EC_GEN(1) |
 			V_EC_UP_TOKEN(T3_CTL_QP_TID) | F_EC_VALID)) << 32;
-	wqe = (struct t3_modify_qp_wr *) skb_put(skb, sizeof(*wqe));
-	memset(wqe, 0, sizeof(*wqe));
+	wqe = skb_put_zero(skb, sizeof(*wqe));
 	build_fw_riwrh((struct fw_riwrh *) wqe, T3_WR_QP_MOD, 0, 0,
 		       T3_CTL_QP_TID, 7, T3_SOPEOP);
 	wqe->flags = cpu_to_be32(MODQP_WRITE_EC);
@@ -837,7 +835,7 @@ int cxio_rdma_init(struct cxio_rdev *rdev_p, struct t3_rdma_init_attr *attr)
 	if (!skb)
 		return -ENOMEM;
 	pr_debug("%s rdev_p %p\n", __func__, rdev_p);
-	wqe = (struct t3_rdma_init_wr *) __skb_put(skb, sizeof(*wqe));
+	wqe = __skb_put(skb, sizeof(*wqe));
 	wqe->wrh.op_seop_flags = cpu_to_be32(V_FW_RIWR_OP(T3_WR_INIT));
 	wqe->wrh.gen_tid_len = cpu_to_be32(V_FW_RIWR_TID(attr->tid) |
 					   V_FW_RIWR_LEN(sizeof(*wqe) >> 3));
diff --git a/drivers/infiniband/hw/cxgb3/iwch_cm.c b/drivers/infiniband/hw/cxgb3/iwch_cm.c
index b61630eba912..86975370a4c0 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_cm.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_cm.c
@@ -175,7 +175,7 @@ static void release_tid(struct t3cdev *tdev, u32 hwtid, struct sk_buff *skb)
 	skb = get_skb(skb, sizeof *req, GFP_KERNEL);
 	if (!skb)
 		return;
-	req = (struct cpl_tid_release *) skb_put(skb, sizeof(*req));
+	req = skb_put(skb, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_TID_RELEASE, hwtid));
 	skb->priority = CPL_PRIORITY_SETUP;
@@ -190,7 +190,7 @@ int iwch_quiesce_tid(struct iwch_ep *ep)
 
 	if (!skb)
 		return -ENOMEM;
-	req = (struct cpl_set_tcb_field *) skb_put(skb, sizeof(*req));
+	req = skb_put(skb, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 	req->wr.wr_lo = htonl(V_WR_TID(ep->hwtid));
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_SET_TCB_FIELD, ep->hwtid));
@@ -211,7 +211,7 @@ int iwch_resume_tid(struct iwch_ep *ep)
 
 	if (!skb)
 		return -ENOMEM;
-	req = (struct cpl_set_tcb_field *) skb_put(skb, sizeof(*req));
+	req = skb_put(skb, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 	req->wr.wr_lo = htonl(V_WR_TID(ep->hwtid));
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_SET_TCB_FIELD, ep->hwtid));
@@ -398,7 +398,7 @@ static int send_halfclose(struct iwch_ep *ep, gfp_t gfp)
 	}
 	skb->priority = CPL_PRIORITY_DATA;
 	set_arp_failure_handler(skb, arp_failure_discard);
-	req = (struct cpl_close_con_req *) skb_put(skb, sizeof(*req));
+	req = skb_put(skb, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_OFLD_CLOSE_CON));
 	req->wr.wr_lo = htonl(V_WR_TID(ep->hwtid));
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_CLOSE_CON_REQ, ep->hwtid));
@@ -417,8 +417,7 @@ static int send_abort(struct iwch_ep *ep, struct sk_buff *skb, gfp_t gfp)
 	}
 	skb->priority = CPL_PRIORITY_DATA;
 	set_arp_failure_handler(skb, abort_arp_failure);
-	req = (struct cpl_abort_req *) skb_put(skb, sizeof(*req));
-	memset(req, 0, sizeof(*req));
+	req = skb_put_zero(skb, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_OFLD_HOST_ABORT_CON_REQ));
 	req->wr.wr_lo = htonl(V_WR_TID(ep->hwtid));
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_ABORT_REQ, ep->hwtid));
@@ -456,7 +455,7 @@ static int send_connect(struct iwch_ep *ep)
 	skb->priority = CPL_PRIORITY_SETUP;
 	set_arp_failure_handler(skb, act_open_req_arp_failure);
 
-	req = (struct cpl_act_open_req *) skb_put(skb, sizeof(*req));
+	req = skb_put(skb, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_ACT_OPEN_REQ, ep->atid));
 	req->local_port = ep->com.local_addr.sin_port;
@@ -514,7 +513,7 @@ static void send_mpa_req(struct iwch_ep *ep, struct sk_buff *skb)
 	set_arp_failure_handler(skb, arp_failure_discard);
 	skb_reset_transport_header(skb);
 	len = skb->len;
-	req = (struct tx_data_wr *) skb_push(skb, sizeof(*req));
+	req = skb_push(skb, sizeof(*req));
 	req->wr_hi = htonl(V_WR_OP(FW_WROPCODE_OFLD_TX_DATA)|F_WR_COMPL);
 	req->wr_lo = htonl(V_WR_TID(ep->hwtid));
 	req->len = htonl(len);
@@ -547,7 +546,7 @@ static int send_mpa_reject(struct iwch_ep *ep, const void *pdata, u8 plen)
 		return -ENOMEM;
 	}
 	skb_reserve(skb, sizeof(*req));
-	mpa = (struct mpa_message *) skb_put(skb, mpalen);
+	mpa = skb_put(skb, mpalen);
 	memset(mpa, 0, sizeof(*mpa));
 	memcpy(mpa->key, MPA_KEY_REP, sizeof(mpa->key));
 	mpa->flags = MPA_REJECT;
@@ -565,7 +564,7 @@ static int send_mpa_reject(struct iwch_ep *ep, const void *pdata, u8 plen)
 	skb->priority = CPL_PRIORITY_DATA;
 	set_arp_failure_handler(skb, arp_failure_discard);
 	skb_reset_transport_header(skb);
-	req = (struct tx_data_wr *) skb_push(skb, sizeof(*req));
+	req = skb_push(skb, sizeof(*req));
 	req->wr_hi = htonl(V_WR_OP(FW_WROPCODE_OFLD_TX_DATA)|F_WR_COMPL);
 	req->wr_lo = htonl(V_WR_TID(ep->hwtid));
 	req->len = htonl(mpalen);
@@ -597,7 +596,7 @@ static int send_mpa_reply(struct iwch_ep *ep, const void *pdata, u8 plen)
 	}
 	skb->priority = CPL_PRIORITY_DATA;
 	skb_reserve(skb, sizeof(*req));
-	mpa = (struct mpa_message *) skb_put(skb, mpalen);
+	mpa = skb_put(skb, mpalen);
 	memset(mpa, 0, sizeof(*mpa));
 	memcpy(mpa->key, MPA_KEY_REP, sizeof(mpa->key));
 	mpa->flags = (ep->mpa_attr.crc_enabled ? MPA_CRC : 0) |
@@ -616,7 +615,7 @@ static int send_mpa_reply(struct iwch_ep *ep, const void *pdata, u8 plen)
 	set_arp_failure_handler(skb, arp_failure_discard);
 	skb_reset_transport_header(skb);
 	len = skb->len;
-	req = (struct tx_data_wr *) skb_push(skb, sizeof(*req));
+	req = skb_push(skb, sizeof(*req));
 	req->wr_hi = htonl(V_WR_OP(FW_WROPCODE_OFLD_TX_DATA)|F_WR_COMPL);
 	req->wr_lo = htonl(V_WR_TID(ep->hwtid));
 	req->len = htonl(len);
@@ -801,7 +800,7 @@ static int update_rx_credits(struct iwch_ep *ep, u32 credits)
 		return 0;
 	}
 
-	req = (struct cpl_rx_data_ack *) skb_put(skb, sizeof(*req));
+	req = skb_put(skb, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_RX_DATA_ACK, ep->hwtid));
 	req->credit_dack = htonl(V_RX_CREDITS(credits) | V_RX_FORCE_ACK(1));
@@ -1206,7 +1205,7 @@ static int listen_start(struct iwch_listen_ep *ep)
 		return -ENOMEM;
 	}
 
-	req = (struct cpl_pass_open_req *) skb_put(skb, sizeof(*req));
+	req = skb_put(skb, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_PASS_OPEN_REQ, ep->stid));
 	req->local_port = ep->com.local_addr.sin_port;
@@ -1247,7 +1246,7 @@ static int listen_stop(struct iwch_listen_ep *ep)
 		pr_err("%s - failed to alloc skb\n", __func__);
 		return -ENOMEM;
 	}
-	req = (struct cpl_close_listserv_req *) skb_put(skb, sizeof(*req));
+	req = skb_put(skb, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 	req->cpu_idx = 0;
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_CLOSE_LISTSRV_REQ, ep->stid));
@@ -1615,7 +1614,7 @@ static int peer_abort(struct t3cdev *tdev, struct sk_buff *skb, void *ctx)
 		goto out;
 	}
 	rpl_skb->priority = CPL_PRIORITY_DATA;
-	rpl = (struct cpl_abort_rpl *) skb_put(rpl_skb, sizeof(*rpl));
+	rpl = skb_put(rpl_skb, sizeof(*rpl));
 	rpl->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_OFLD_HOST_ABORT_CON_RPL));
 	rpl->wr.wr_lo = htonl(V_WR_TID(ep->hwtid));
 	OPCODE_TID(rpl) = htonl(MK_OPCODE_TID(CPL_ABORT_RPL, ep->hwtid));
diff --git a/drivers/infiniband/hw/cxgb3/iwch_qp.c b/drivers/infiniband/hw/cxgb3/iwch_qp.c
index ba6d5d281b03..7f633da0185d 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_qp.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_qp.c
@@ -670,8 +670,7 @@ int iwch_post_zb_read(struct iwch_ep *ep)
 		pr_err("%s cannot send zb_read!!\n", __func__);
 		return -ENOMEM;
 	}
-	wqe = (union t3_wr *)skb_put(skb, sizeof(struct t3_rdma_read_wr));
-	memset(wqe, 0, sizeof(struct t3_rdma_read_wr));
+	wqe = skb_put_zero(skb, sizeof(struct t3_rdma_read_wr));
 	wqe->read.rdmaop = T3_READ_REQ;
 	wqe->read.reserved[0] = 0;
 	wqe->read.reserved[1] = 0;
@@ -702,8 +701,7 @@ int iwch_post_terminate(struct iwch_qp *qhp, struct respQ_msg_t *rsp_msg)
 		pr_err("%s cannot send TERMINATE!\n", __func__);
 		return -ENOMEM;
 	}
-	wqe = (union t3_wr *)skb_put(skb, 40);
-	memset(wqe, 0, 40);
+	wqe = skb_put_zero(skb, 40);
 	wqe->send.rdmaop = T3_TERMINATE;
 
 	/* immediate data length */
diff --git a/drivers/infiniband/hw/cxgb4/cm.c b/drivers/infiniband/hw/cxgb4/cm.c
index 0910faf3587b..e49b34c3b136 100644
--- a/drivers/infiniband/hw/cxgb4/cm.c
+++ b/drivers/infiniband/hw/cxgb4/cm.c
@@ -398,7 +398,8 @@ void _c4iw_free_ep(struct kref *kref)
 					(const u32 *)&sin6->sin6_addr.s6_addr,
 					1);
 		}
-		cxgb4_remove_tid(ep->com.dev->rdev.lldi.tids, 0, ep->hwtid);
+		cxgb4_remove_tid(ep->com.dev->rdev.lldi.tids, 0, ep->hwtid,
+				 ep->com.local_addr.ss_family);
 		dst_release(ep->dst);
 		cxgb4_l2t_release(ep->l2t);
 		if (ep->mpa_skb)
@@ -596,7 +597,7 @@ static int send_flowc(struct c4iw_ep *ep)
 	else
 		nparams = 9;
 
-	flowc = (struct fw_flowc_wr *)__skb_put(skb, FLOWC_LEN);
+	flowc = __skb_put(skb, FLOWC_LEN);
 
 	flowc->op_to_nparams = cpu_to_be32(FW_WR_OP_V(FW_FLOWC_WR) |
 					   FW_FLOWC_WR_NPARAMS_V(nparams));
@@ -786,18 +787,16 @@ static int send_connect(struct c4iw_ep *ep)
 	if (ep->com.remote_addr.ss_family == AF_INET) {
 		switch (CHELSIO_CHIP_VERSION(adapter_type)) {
 		case CHELSIO_T4:
-			req = (struct cpl_act_open_req *)skb_put(skb, wrlen);
+			req = skb_put(skb, wrlen);
 			INIT_TP_WR(req, 0);
 			break;
 		case CHELSIO_T5:
-			t5req = (struct cpl_t5_act_open_req *)skb_put(skb,
-					wrlen);
+			t5req = skb_put(skb, wrlen);
 			INIT_TP_WR(t5req, 0);
 			req = (struct cpl_act_open_req *)t5req;
 			break;
 		case CHELSIO_T6:
-			t6req = (struct cpl_t6_act_open_req *)skb_put(skb,
-					wrlen);
+			t6req = skb_put(skb, wrlen);
 			INIT_TP_WR(t6req, 0);
 			req = (struct cpl_act_open_req *)t6req;
 			t5req = (struct cpl_t5_act_open_req *)t6req;
@@ -838,18 +837,16 @@ static int send_connect(struct c4iw_ep *ep)
 	} else {
 		switch (CHELSIO_CHIP_VERSION(adapter_type)) {
 		case CHELSIO_T4:
-			req6 = (struct cpl_act_open_req6 *)skb_put(skb, wrlen);
+			req6 = skb_put(skb, wrlen);
 			INIT_TP_WR(req6, 0);
 			break;
 		case CHELSIO_T5:
-			t5req6 = (struct cpl_t5_act_open_req6 *)skb_put(skb,
-					wrlen);
+			t5req6 = skb_put(skb, wrlen);
 			INIT_TP_WR(t5req6, 0);
 			req6 = (struct cpl_act_open_req6 *)t5req6;
 			break;
 		case CHELSIO_T6:
-			t6req6 = (struct cpl_t6_act_open_req6 *)skb_put(skb,
-					wrlen);
+			t6req6 = skb_put(skb, wrlen);
 			INIT_TP_WR(t6req6, 0);
 			req6 = (struct cpl_act_open_req6 *)t6req6;
 			t5req6 = (struct cpl_t5_act_open_req6 *)t6req6;
@@ -926,8 +923,7 @@ static int send_mpa_req(struct c4iw_ep *ep, struct sk_buff *skb,
 	}
 	set_wr_txq(skb, CPL_PRIORITY_DATA, ep->txq_idx);
 
-	req = (struct fw_ofld_tx_data_wr *)skb_put(skb, wrlen);
-	memset(req, 0, wrlen);
+	req = skb_put_zero(skb, wrlen);
 	req->op_to_immdlen = cpu_to_be32(
 		FW_WR_OP_V(FW_OFLD_TX_DATA_WR) |
 		FW_WR_COMPL_F |
@@ -1033,8 +1029,7 @@ static int send_mpa_reject(struct c4iw_ep *ep, const void *pdata, u8 plen)
 	}
 	set_wr_txq(skb, CPL_PRIORITY_DATA, ep->txq_idx);
 
-	req = (struct fw_ofld_tx_data_wr *)skb_put(skb, wrlen);
-	memset(req, 0, wrlen);
+	req = skb_put_zero(skb, wrlen);
 	req->op_to_immdlen = cpu_to_be32(
 		FW_WR_OP_V(FW_OFLD_TX_DATA_WR) |
 		FW_WR_COMPL_F |
@@ -1114,8 +1109,7 @@ static int send_mpa_reply(struct c4iw_ep *ep, const void *pdata, u8 plen)
 	}
 	set_wr_txq(skb, CPL_PRIORITY_DATA, ep->txq_idx);
 
-	req = (struct fw_ofld_tx_data_wr *) skb_put(skb, wrlen);
-	memset(req, 0, wrlen);
+	req = skb_put_zero(skb, wrlen);
 	req->op_to_immdlen = cpu_to_be32(
 		FW_WR_OP_V(FW_OFLD_TX_DATA_WR) |
 		FW_WR_COMPL_F |
@@ -1199,7 +1193,7 @@ static int act_establish(struct c4iw_dev *dev, struct sk_buff *skb)
 
 	/* setup the hwtid for this connection */
 	ep->hwtid = tid;
-	cxgb4_insert_tid(t, ep, tid);
+	cxgb4_insert_tid(t, ep, tid, ep->com.local_addr.ss_family);
 	insert_ep_tid(ep);
 
 	ep->snd_seq = be32_to_cpu(req->snd_isn);
@@ -1906,8 +1900,7 @@ static int send_fw_act_open_req(struct c4iw_ep *ep, unsigned int atid)
 	int win;
 
 	skb = get_skb(NULL, sizeof(*req), GFP_KERNEL);
-	req = (struct fw_ofld_connection_wr *)__skb_put(skb, sizeof(*req));
-	memset(req, 0, sizeof(*req));
+	req = __skb_put_zero(skb, sizeof(*req));
 	req->op_compl = htonl(WR_OP_V(FW_OFLD_CONNECTION_WR));
 	req->len16_pkd = htonl(FW_WR_LEN16_V(DIV_ROUND_UP(sizeof(*req), 16)));
 	req->le.filter = cpu_to_be32(cxgb4_select_ntuple(
@@ -2304,7 +2297,8 @@ fail:
 				   (const u32 *)&sin6->sin6_addr.s6_addr, 1);
 	}
 	if (status && act_open_has_tid(status))
-		cxgb4_remove_tid(ep->com.dev->rdev.lldi.tids, 0, GET_TID(rpl));
+		cxgb4_remove_tid(ep->com.dev->rdev.lldi.tids, 0, GET_TID(rpl),
+				 ep->com.local_addr.ss_family);
 
 	remove_handle(ep->com.dev, &ep->com.dev->atid_idr, atid);
 	cxgb4_free_atid(t, atid);
@@ -2581,7 +2575,8 @@ static int pass_accept_req(struct c4iw_dev *dev, struct sk_buff *skb)
 		 child_ep->tx_chan, child_ep->smac_idx, child_ep->rss_qid);
 
 	init_timer(&child_ep->timer);
-	cxgb4_insert_tid(t, child_ep, hwtid);
+	cxgb4_insert_tid(t, child_ep, hwtid,
+			 child_ep->com.local_addr.ss_family);
 	insert_ep_tid(child_ep);
 	if (accept_cr(child_ep, skb, req)) {
 		c4iw_put_ep(&parent_ep->com);
@@ -2849,7 +2844,8 @@ out:
 					1);
 		}
 		remove_handle(ep->com.dev, &ep->com.dev->hwtid_idr, ep->hwtid);
-		cxgb4_remove_tid(ep->com.dev->rdev.lldi.tids, 0, ep->hwtid);
+		cxgb4_remove_tid(ep->com.dev->rdev.lldi.tids, 0, ep->hwtid,
+				 ep->com.local_addr.ss_family);
 		dst_release(ep->dst);
 		cxgb4_l2t_release(ep->l2t);
 		c4iw_reconnect(ep);
@@ -3752,9 +3748,9 @@ static void build_cpl_pass_accept_req(struct sk_buff *skb, int stid , u8 tos)
 	 */
 	memset(&tmp_opt, 0, sizeof(tmp_opt));
 	tcp_clear_options(&tmp_opt);
-	tcp_parse_options(skb, &tmp_opt, 0, NULL);
+	tcp_parse_options(&init_net, skb, &tmp_opt, 0, NULL);
 
-	req = (struct cpl_pass_accept_req *)__skb_push(skb, sizeof(*req));
+	req = __skb_push(skb, sizeof(*req));
 	memset(req, 0, sizeof(*req));
 	req->l2info = cpu_to_be16(SYN_INTF_V(intf) |
 			 SYN_MAC_IDX_V(RX_MACIDX_G(
@@ -3806,8 +3802,7 @@ static void send_fw_pass_open_req(struct c4iw_dev *dev, struct sk_buff *skb,
 	req_skb = alloc_skb(sizeof(struct fw_ofld_connection_wr), GFP_KERNEL);
 	if (!req_skb)
 		return;
-	req = (struct fw_ofld_connection_wr *)__skb_put(req_skb, sizeof(*req));
-	memset(req, 0, sizeof(*req));
+	req = __skb_put_zero(req_skb, sizeof(*req));
 	req->op_compl = htonl(WR_OP_V(FW_OFLD_CONNECTION_WR) | FW_WR_COMPL_F);
 	req->len16_pkd = htonl(FW_WR_LEN16_V(DIV_ROUND_UP(sizeof(*req), 16)));
 	req->le.version_cpl = htonl(FW_OFLD_CONNECTION_WR_CPL_F);
diff --git a/drivers/infiniband/hw/cxgb4/cq.c b/drivers/infiniband/hw/cxgb4/cq.c
index 14de5bde1b63..e16fcaf6b5a3 100644
--- a/drivers/infiniband/hw/cxgb4/cq.c
+++ b/drivers/infiniband/hw/cxgb4/cq.c
@@ -44,8 +44,7 @@ static int destroy_cq(struct c4iw_rdev *rdev, struct t4_cq *cq,
 	wr_len = sizeof *res_wr + sizeof *res;
 	set_wr_txq(skb, CPL_PRIORITY_CONTROL, 0);
 
-	res_wr = (struct fw_ri_res_wr *)__skb_put(skb, wr_len);
-	memset(res_wr, 0, wr_len);
+	res_wr = __skb_put_zero(skb, wr_len);
 	res_wr->op_nres = cpu_to_be32(
 			FW_WR_OP_V(FW_RI_RES_WR) |
 			FW_RI_RES_WR_NRES_V(1) |
@@ -114,8 +113,7 @@ static int create_cq(struct c4iw_rdev *rdev, struct t4_cq *cq,
 	}
 	set_wr_txq(skb, CPL_PRIORITY_CONTROL, 0);
 
-	res_wr = (struct fw_ri_res_wr *)__skb_put(skb, wr_len);
-	memset(res_wr, 0, wr_len);
+	res_wr = __skb_put_zero(skb, wr_len);
 	res_wr->op_nres = cpu_to_be32(
 			FW_WR_OP_V(FW_RI_RES_WR) |
 			FW_RI_RES_WR_NRES_V(1) |
diff --git a/drivers/infiniband/hw/cxgb4/device.c b/drivers/infiniband/hw/cxgb4/device.c
index f96a96dbcf1f..ae0b79aeea2e 100644
--- a/drivers/infiniband/hw/cxgb4/device.c
+++ b/drivers/infiniband/hw/cxgb4/device.c
@@ -767,7 +767,7 @@ void c4iw_release_dev_ucontext(struct c4iw_rdev *rdev,
 		kfree(entry);
 	}
 
-	list_for_each_safe(pos, nxt, &uctx->qpids) {
+	list_for_each_safe(pos, nxt, &uctx->cqids) {
 		entry = list_entry(pos, struct c4iw_qid_list, entry);
 		list_del_init(&entry->entry);
 		kfree(entry);
@@ -880,13 +880,15 @@ static int c4iw_rdev_open(struct c4iw_rdev *rdev)
 	rdev->free_workq = create_singlethread_workqueue("iw_cxgb4_free");
 	if (!rdev->free_workq) {
 		err = -ENOMEM;
-		goto err_free_status_page;
+		goto err_free_status_page_and_wr_log;
 	}
 
 	rdev->status_page->db_off = 0;
 
 	return 0;
-err_free_status_page:
+err_free_status_page_and_wr_log:
+	if (c4iw_wr_log && rdev->wr_log)
+		kfree(rdev->wr_log);
 	free_page((unsigned long)rdev->status_page);
 destroy_ocqp_pool:
 	c4iw_ocqp_pool_destroy(rdev);
@@ -903,9 +905,11 @@ static void c4iw_rdev_close(struct c4iw_rdev *rdev)
 {
 	destroy_workqueue(rdev->free_workq);
 	kfree(rdev->wr_log);
+	c4iw_release_dev_ucontext(rdev, &rdev->uctx);
 	free_page((unsigned long)rdev->status_page);
 	c4iw_pblpool_destroy(rdev);
 	c4iw_rqtpool_destroy(rdev);
+	c4iw_ocqp_pool_destroy(rdev);
 	c4iw_destroy_resource(&rdev->resource);
 }
 
diff --git a/drivers/infiniband/hw/cxgb4/mem.c b/drivers/infiniband/hw/cxgb4/mem.c
index 3ee7f43e419a..5332f06b99ba 100644
--- a/drivers/infiniband/hw/cxgb4/mem.c
+++ b/drivers/infiniband/hw/cxgb4/mem.c
@@ -81,8 +81,7 @@ static int _c4iw_write_mem_dma_aligned(struct c4iw_rdev *rdev, u32 addr,
 	}
 	set_wr_txq(skb, CPL_PRIORITY_CONTROL, 0);
 
-	req = (struct ulp_mem_io *)__skb_put(skb, wr_len);
-	memset(req, 0, wr_len);
+	req = __skb_put_zero(skb, wr_len);
 	INIT_ULPTX_WR(req, wr_len, 0, 0);
 	req->wr.wr_hi = cpu_to_be32(FW_WR_OP_V(FW_ULPTX_WR) |
 			(wait ? FW_WR_COMPL_F : 0));
@@ -142,8 +141,7 @@ static int _c4iw_write_mem_inline(struct c4iw_rdev *rdev, u32 addr, u32 len,
 		}
 		set_wr_txq(skb, CPL_PRIORITY_CONTROL, 0);
 
-		req = (struct ulp_mem_io *)__skb_put(skb, wr_len);
-		memset(req, 0, wr_len);
+		req = __skb_put_zero(skb, wr_len);
 		INIT_ULPTX_WR(req, wr_len, 0, 0);
 
 		if (i == (num_wqe-1)) {
diff --git a/drivers/infiniband/hw/cxgb4/qp.c b/drivers/infiniband/hw/cxgb4/qp.c
index 8e4154b4253e..bfc77596acbe 100644
--- a/drivers/infiniband/hw/cxgb4/qp.c
+++ b/drivers/infiniband/hw/cxgb4/qp.c
@@ -293,8 +293,7 @@ static int create_qp(struct c4iw_rdev *rdev, struct t4_wq *wq,
 	}
 	set_wr_txq(skb, CPL_PRIORITY_CONTROL, 0);
 
-	res_wr = (struct fw_ri_res_wr *)__skb_put(skb, wr_len);
-	memset(res_wr, 0, wr_len);
+	res_wr = __skb_put_zero(skb, wr_len);
 	res_wr->op_nres = cpu_to_be32(
 			FW_WR_OP_V(FW_RI_RES_WR) |
 			FW_RI_RES_WR_NRES_V(2) |
@@ -1228,7 +1227,7 @@ static void post_terminate(struct c4iw_qp *qhp, struct t4_cqe *err_cqe,
 
 	set_wr_txq(skb, CPL_PRIORITY_DATA, qhp->ep->txq_idx);
 
-	wqe = (struct fw_ri_wr *)__skb_put(skb, sizeof(*wqe));
+	wqe = __skb_put(skb, sizeof(*wqe));
 	memset(wqe, 0, sizeof *wqe);
 	wqe->op_compl = cpu_to_be32(FW_WR_OP_V(FW_RI_INIT_WR));
 	wqe->flowid_len16 = cpu_to_be32(
@@ -1350,7 +1349,7 @@ static int rdma_fini(struct c4iw_dev *rhp, struct c4iw_qp *qhp,
 
 	set_wr_txq(skb, CPL_PRIORITY_DATA, ep->txq_idx);
 
-	wqe = (struct fw_ri_wr *)__skb_put(skb, sizeof(*wqe));
+	wqe = __skb_put(skb, sizeof(*wqe));
 	memset(wqe, 0, sizeof *wqe);
 	wqe->op_compl = cpu_to_be32(
 		FW_WR_OP_V(FW_RI_INIT_WR) |
@@ -1419,7 +1418,7 @@ static int rdma_init(struct c4iw_dev *rhp, struct c4iw_qp *qhp)
 	}
 	set_wr_txq(skb, CPL_PRIORITY_DATA, qhp->ep->txq_idx);
 
-	wqe = (struct fw_ri_wr *)__skb_put(skb, sizeof(*wqe));
+	wqe = __skb_put(skb, sizeof(*wqe));
 	memset(wqe, 0, sizeof *wqe);
 	wqe->op_compl = cpu_to_be32(
 		FW_WR_OP_V(FW_RI_INIT_WR) |
diff --git a/drivers/infiniband/hw/hfi1/verbs.c b/drivers/infiniband/hw/hfi1/verbs.c
index 90e7b77d68e8..2d19f9bb434d 100644
--- a/drivers/infiniband/hw/hfi1/verbs.c
+++ b/drivers/infiniband/hw/hfi1/verbs.c
@@ -1779,7 +1779,6 @@ int hfi1_register_ib_device(struct hfi1_devdata *dd)
 	ibdev->alloc_hw_stats = alloc_hw_stats;
 	ibdev->get_hw_stats = get_hw_stats;
 	ibdev->alloc_rdma_netdev = hfi1_vnic_alloc_rn;
-	ibdev->free_rdma_netdev = hfi1_vnic_free_rn;
 
 	/* keep process mad in the driver */
 	ibdev->process_mad = hfi1_process_mad;
diff --git a/drivers/infiniband/hw/hfi1/vnic.h b/drivers/infiniband/hw/hfi1/vnic.h
index e2c455299b53..4a621cde4abb 100644
--- a/drivers/infiniband/hw/hfi1/vnic.h
+++ b/drivers/infiniband/hw/hfi1/vnic.h
@@ -176,7 +176,6 @@ struct net_device *hfi1_vnic_alloc_rn(struct ib_device *device,
 				      const char *name,
 				      unsigned char name_assign_type,
 				      void (*setup)(struct net_device *));
-void hfi1_vnic_free_rn(struct net_device *netdev);
 int hfi1_vnic_send_dma(struct hfi1_devdata *dd, u8 q_idx,
 		       struct hfi1_vnic_vport_info *vinfo,
 		       struct sk_buff *skb, u64 pbc, u8 plen);
diff --git a/drivers/infiniband/hw/hfi1/vnic_main.c b/drivers/infiniband/hw/hfi1/vnic_main.c
index b601c2929f8f..339f0cdd56d6 100644
--- a/drivers/infiniband/hw/hfi1/vnic_main.c
+++ b/drivers/infiniband/hw/hfi1/vnic_main.c
@@ -833,6 +833,15 @@ static const struct net_device_ops hfi1_netdev_ops = {
 	.ndo_get_stats64 = hfi1_vnic_get_stats64,
 };
 
+static void hfi1_vnic_free_rn(struct net_device *netdev)
+{
+	struct hfi1_vnic_vport_info *vinfo = opa_vnic_dev_priv(netdev);
+
+	hfi1_vnic_deinit(vinfo);
+	mutex_destroy(&vinfo->lock);
+	free_netdev(netdev);
+}
+
 struct net_device *hfi1_vnic_alloc_rn(struct ib_device *device,
 				      u8 port_num,
 				      enum rdma_netdev_t type,
@@ -864,6 +873,7 @@ struct net_device *hfi1_vnic_alloc_rn(struct ib_device *device,
 	vinfo->num_tx_q = dd->chip_sdma_engines;
 	vinfo->num_rx_q = HFI1_NUM_VNIC_CTXT;
 	vinfo->netdev = netdev;
+	rn->free_rdma_netdev = hfi1_vnic_free_rn;
 	rn->set_id = hfi1_vnic_set_vesw_id;
 
 	netdev->features = NETIF_F_HIGHDMA | NETIF_F_SG;
@@ -892,12 +902,3 @@ init_fail:
 	free_netdev(netdev);
 	return ERR_PTR(rc);
 }
-
-void hfi1_vnic_free_rn(struct net_device *netdev)
-{
-	struct hfi1_vnic_vport_info *vinfo = opa_vnic_dev_priv(netdev);
-
-	hfi1_vnic_deinit(vinfo);
-	mutex_destroy(&vinfo->lock);
-	free_netdev(netdev);
-}
diff --git a/drivers/infiniband/hw/i40iw/i40iw_main.c b/drivers/infiniband/hw/i40iw/i40iw_main.c
index a3f18a22f5ed..e0f47cc2effc 100644
--- a/drivers/infiniband/hw/i40iw/i40iw_main.c
+++ b/drivers/infiniband/hw/i40iw/i40iw_main.c
@@ -1939,7 +1939,7 @@ static int i40iw_virtchnl_receive(struct i40e_info *ldev,
 bool i40iw_vf_clear_to_send(struct i40iw_sc_dev *dev)
 {
 	struct i40iw_device *iwdev;
-	wait_queue_t wait;
+	wait_queue_entry_t wait;
 
 	iwdev = dev->back_dev;
 
diff --git a/drivers/infiniband/hw/mlx4/main.c b/drivers/infiniband/hw/mlx4/main.c
index 521d0def2d9e..75b2f7d4cd95 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -61,8 +61,7 @@
 #include <rdma/mlx4-abi.h>
 
 #define DRV_NAME	MLX4_IB_DRV_NAME
-#define DRV_VERSION	"2.2-1"
-#define DRV_RELDATE	"Feb 2014"
+#define DRV_VERSION	"4.0-0"
 
 #define MLX4_IB_FLOW_MAX_PRIO 0xFFF
 #define MLX4_IB_FLOW_QPN_MASK 0xFFFFFF
@@ -79,7 +78,7 @@ MODULE_PARM_DESC(sm_guid_assign, "Enable SM alias_GUID assignment if sm_guid_ass
 
 static const char mlx4_ib_version[] =
 	DRV_NAME ": Mellanox ConnectX InfiniBand driver v"
-	DRV_VERSION " (" DRV_RELDATE ")\n";
+	DRV_VERSION "\n";
 
 static void do_slave_init(struct mlx4_ib_dev *ibdev, int slave, int do_init);
 
diff --git a/drivers/infiniband/hw/mlx5/cq.c b/drivers/infiniband/hw/mlx5/cq.c
index 94c049b62c2f..a384d72ea3cd 100644
--- a/drivers/infiniband/hw/mlx5/cq.c
+++ b/drivers/infiniband/hw/mlx5/cq.c
@@ -788,7 +788,7 @@ static int create_cq_user(struct mlx5_ib_dev *dev, struct ib_udata *udata,
 
 	*inlen = MLX5_ST_SZ_BYTES(create_cq_in) +
 		 MLX5_FLD_SZ_BYTES(create_cq_in, pas[0]) * ncont;
-	*cqb = mlx5_vzalloc(*inlen);
+	*cqb = kvzalloc(*inlen, GFP_KERNEL);
 	if (!*cqb) {
 		err = -ENOMEM;
 		goto err_db;
@@ -884,7 +884,7 @@ static int create_cq_kernel(struct mlx5_ib_dev *dev, struct mlx5_ib_cq *cq,
 
 	*inlen = MLX5_ST_SZ_BYTES(create_cq_in) +
 		 MLX5_FLD_SZ_BYTES(create_cq_in, pas[0]) * cq->buf.buf.npages;
-	*cqb = mlx5_vzalloc(*inlen);
+	*cqb = kvzalloc(*inlen, GFP_KERNEL);
 	if (!*cqb) {
 		err = -ENOMEM;
 		goto err_buf;
@@ -1314,7 +1314,7 @@ int mlx5_ib_resize_cq(struct ib_cq *ibcq, int entries, struct ib_udata *udata)
 	inlen = MLX5_ST_SZ_BYTES(modify_cq_in) +
 		MLX5_FLD_SZ_BYTES(modify_cq_in, pas[0]) * npas;
 
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in) {
 		err = -ENOMEM;
 		goto ex_resize;
diff --git a/drivers/infiniband/hw/mlx5/mad.c b/drivers/infiniband/hw/mlx5/mad.c
index f1b56de64871..95db929bdc34 100644
--- a/drivers/infiniband/hw/mlx5/mad.c
+++ b/drivers/infiniband/hw/mlx5/mad.c
@@ -218,7 +218,7 @@ static int process_pma_cmd(struct ib_device *ibdev, u8 port_num,
 			(struct ib_pma_portcounters_ext *)(out_mad->data + 40);
 		int sz = MLX5_ST_SZ_BYTES(query_vport_counter_out);
 
-		out_cnt = mlx5_vzalloc(sz);
+		out_cnt = kvzalloc(sz, GFP_KERNEL);
 		if (!out_cnt)
 			return IB_MAD_RESULT_FAILURE;
 
@@ -231,7 +231,7 @@ static int process_pma_cmd(struct ib_device *ibdev, u8 port_num,
 			(struct ib_pma_portcounters *)(out_mad->data + 40);
 		int sz = MLX5_ST_SZ_BYTES(ppcnt_reg);
 
-		out_cnt = mlx5_vzalloc(sz);
+		out_cnt = kvzalloc(sz, GFP_KERNEL);
 		if (!out_cnt)
 			return IB_MAD_RESULT_FAILURE;
 
diff --git a/drivers/infiniband/hw/mlx5/main.c b/drivers/infiniband/hw/mlx5/main.c
index 0c79983c8b1a..a7f2e60085c4 100644
--- a/drivers/infiniband/hw/mlx5/main.c
+++ b/drivers/infiniband/hw/mlx5/main.c
@@ -60,8 +60,7 @@
 #include "cmd.h"
 
 #define DRIVER_NAME "mlx5_ib"
-#define DRIVER_VERSION "2.2-1"
-#define DRIVER_RELDATE	"Feb 2014"
+#define DRIVER_VERSION "5.0-0"
 
 MODULE_AUTHOR("Eli Cohen <eli@mellanox.com>");
 MODULE_DESCRIPTION("Mellanox Connect-IB HCA IB driver");
@@ -70,7 +69,7 @@ MODULE_VERSION(DRIVER_VERSION);
 
 static char mlx5_version[] =
 	DRIVER_NAME ": Mellanox Connect-IB Infiniband driver v"
-	DRIVER_VERSION " (" DRIVER_RELDATE ")\n";
+	DRIVER_VERSION "\n";
 
 enum {
 	MLX5_ATOMIC_SIZE_QP_8BYTES = 1 << 3,
@@ -224,8 +223,8 @@ static int translate_eth_proto_oper(u32 eth_proto_oper, u8 *active_speed,
 	return 0;
 }
 
-static void mlx5_query_port_roce(struct ib_device *device, u8 port_num,
-				 struct ib_port_attr *props)
+static int mlx5_query_port_roce(struct ib_device *device, u8 port_num,
+				struct ib_port_attr *props)
 {
 	struct mlx5_ib_dev *dev = to_mdev(device);
 	struct mlx5_core_dev *mdev = dev->mdev;
@@ -233,12 +232,14 @@ static void mlx5_query_port_roce(struct ib_device *device, u8 port_num,
 	enum ib_mtu ndev_ib_mtu;
 	u16 qkey_viol_cntr;
 	u32 eth_prot_oper;
+	int err;
 
 	/* Possible bad flows are checked before filling out props so in case
 	 * of an error it will still be zeroed out.
 	 */
-	if (mlx5_query_port_eth_proto_oper(mdev, &eth_prot_oper, port_num))
-		return;
+	err = mlx5_query_port_eth_proto_oper(mdev, &eth_prot_oper, port_num);
+	if (err)
+		return err;
 
 	translate_eth_proto_oper(eth_prot_oper, &props->active_speed,
 				 &props->active_width);
@@ -259,7 +260,7 @@ static void mlx5_query_port_roce(struct ib_device *device, u8 port_num,
 
 	ndev = mlx5_ib_get_netdev(device, port_num);
 	if (!ndev)
-		return;
+		return 0;
 
 	if (mlx5_lag_is_active(dev->mdev)) {
 		rcu_read_lock();
@@ -282,75 +283,49 @@ static void mlx5_query_port_roce(struct ib_device *device, u8 port_num,
 	dev_put(ndev);
 
 	props->active_mtu	= min(props->max_mtu, ndev_ib_mtu);
+	return 0;
 }
 
-static void ib_gid_to_mlx5_roce_addr(const union ib_gid *gid,
-				     const struct ib_gid_attr *attr,
-				     void *mlx5_addr)
+static int set_roce_addr(struct mlx5_ib_dev *dev, u8 port_num,
+			 unsigned int index, const union ib_gid *gid,
+			 const struct ib_gid_attr *attr)
 {
-#define MLX5_SET_RA(p, f, v) MLX5_SET(roce_addr_layout, p, f, v)
-	char *mlx5_addr_l3_addr	= MLX5_ADDR_OF(roce_addr_layout, mlx5_addr,
-					       source_l3_address);
-	void *mlx5_addr_mac	= MLX5_ADDR_OF(roce_addr_layout, mlx5_addr,
-					       source_mac_47_32);
-
-	if (!gid)
-		return;
+	enum ib_gid_type gid_type = IB_GID_TYPE_IB;
+	u8 roce_version = 0;
+	u8 roce_l3_type = 0;
+	bool vlan = false;
+	u8 mac[ETH_ALEN];
+	u16 vlan_id = 0;
 
-	ether_addr_copy(mlx5_addr_mac, attr->ndev->dev_addr);
+	if (gid) {
+		gid_type = attr->gid_type;
+		ether_addr_copy(mac, attr->ndev->dev_addr);
 
-	if (is_vlan_dev(attr->ndev)) {
-		MLX5_SET_RA(mlx5_addr, vlan_valid, 1);
-		MLX5_SET_RA(mlx5_addr, vlan_id, vlan_dev_vlan_id(attr->ndev));
+		if (is_vlan_dev(attr->ndev)) {
+			vlan = true;
+			vlan_id = vlan_dev_vlan_id(attr->ndev);
+		}
 	}
 
-	switch (attr->gid_type) {
+	switch (gid_type) {
 	case IB_GID_TYPE_IB:
-		MLX5_SET_RA(mlx5_addr, roce_version, MLX5_ROCE_VERSION_1);
+		roce_version = MLX5_ROCE_VERSION_1;
 		break;
 	case IB_GID_TYPE_ROCE_UDP_ENCAP:
-		MLX5_SET_RA(mlx5_addr, roce_version, MLX5_ROCE_VERSION_2);
+		roce_version = MLX5_ROCE_VERSION_2;
+		if (ipv6_addr_v4mapped((void *)gid))
+			roce_l3_type = MLX5_ROCE_L3_TYPE_IPV4;
+		else
+			roce_l3_type = MLX5_ROCE_L3_TYPE_IPV6;
 		break;
 
 	default:
-		WARN_ON(true);
+		mlx5_ib_warn(dev, "Unexpected GID type %u\n", gid_type);
 	}
 
-	if (attr->gid_type != IB_GID_TYPE_IB) {
-		if (ipv6_addr_v4mapped((void *)gid))
-			MLX5_SET_RA(mlx5_addr, roce_l3_type,
-				    MLX5_ROCE_L3_TYPE_IPV4);
-		else
-			MLX5_SET_RA(mlx5_addr, roce_l3_type,
-				    MLX5_ROCE_L3_TYPE_IPV6);
-	}
-
-	if ((attr->gid_type == IB_GID_TYPE_IB) ||
-	    !ipv6_addr_v4mapped((void *)gid))
-		memcpy(mlx5_addr_l3_addr, gid, sizeof(*gid));
-	else
-		memcpy(&mlx5_addr_l3_addr[12], &gid->raw[12], 4);
-}
-
-static int set_roce_addr(struct ib_device *device, u8 port_num,
-			 unsigned int index,
-			 const union ib_gid *gid,
-			 const struct ib_gid_attr *attr)
-{
-	struct mlx5_ib_dev *dev = to_mdev(device);
-	u32  in[MLX5_ST_SZ_DW(set_roce_address_in)]  = {0};
-	u32 out[MLX5_ST_SZ_DW(set_roce_address_out)] = {0};
-	void *in_addr = MLX5_ADDR_OF(set_roce_address_in, in, roce_address);
-	enum rdma_link_layer ll = mlx5_ib_port_link_layer(device, port_num);
-
-	if (ll != IB_LINK_LAYER_ETHERNET)
-		return -EINVAL;
-
-	ib_gid_to_mlx5_roce_addr(gid, attr, in_addr);
-
-	MLX5_SET(set_roce_address_in, in, roce_address_index, index);
-	MLX5_SET(set_roce_address_in, in, opcode, MLX5_CMD_OP_SET_ROCE_ADDRESS);
-	return mlx5_cmd_exec(dev->mdev, in, sizeof(in), out, sizeof(out));
+	return mlx5_core_roce_gid_set(dev->mdev, index, roce_version,
+				      roce_l3_type, gid->raw, mac, vlan,
+				      vlan_id);
 }
 
 static int mlx5_ib_add_gid(struct ib_device *device, u8 port_num,
@@ -358,13 +333,13 @@ static int mlx5_ib_add_gid(struct ib_device *device, u8 port_num,
 			   const struct ib_gid_attr *attr,
 			   __always_unused void **context)
 {
-	return set_roce_addr(device, port_num, index, gid, attr);
+	return set_roce_addr(to_mdev(device), port_num, index, gid, attr);
 }
 
 static int mlx5_ib_del_gid(struct ib_device *device, u8 port_num,
 			   unsigned int index, __always_unused void **context)
 {
-	return set_roce_addr(device, port_num, index, NULL, NULL);
+	return set_roce_addr(to_mdev(device), port_num, index, NULL, NULL);
 }
 
 __be16 mlx5_get_roce_udp_sport(struct mlx5_ib_dev *dev, u8 port_num,
@@ -440,7 +415,7 @@ static void get_atomic_caps(struct mlx5_ib_dev *dev,
 	u8 atomic_operations = MLX5_CAP_ATOMIC(dev->mdev, atomic_operations);
 	u8 atomic_size_qp = MLX5_CAP_ATOMIC(dev->mdev, atomic_size_qp);
 	u8 atomic_req_8B_endianness_mode =
-		MLX5_CAP_ATOMIC(dev->mdev, atomic_req_8B_endianess_mode);
+		MLX5_CAP_ATOMIC(dev->mdev, atomic_req_8B_endianness_mode);
 
 	/* Check if HW supports 8 bytes standard atomic operations and capable
 	 * of host endianness respond
@@ -979,20 +954,31 @@ out:
 int mlx5_ib_query_port(struct ib_device *ibdev, u8 port,
 		       struct ib_port_attr *props)
 {
+	unsigned int count;
+	int ret;
+
 	switch (mlx5_get_vport_access_method(ibdev)) {
 	case MLX5_VPORT_ACCESS_METHOD_MAD:
-		return mlx5_query_mad_ifc_port(ibdev, port, props);
+		ret = mlx5_query_mad_ifc_port(ibdev, port, props);
+		break;
 
 	case MLX5_VPORT_ACCESS_METHOD_HCA:
-		return mlx5_query_hca_port(ibdev, port, props);
+		ret = mlx5_query_hca_port(ibdev, port, props);
+		break;
 
 	case MLX5_VPORT_ACCESS_METHOD_NIC:
-		mlx5_query_port_roce(ibdev, port, props);
-		return 0;
+		ret = mlx5_query_port_roce(ibdev, port, props);
+		break;
 
 	default:
-		return -EINVAL;
+		ret = -EINVAL;
+	}
+
+	if (!ret && props) {
+		count = mlx5_core_reserved_gids_count(to_mdev(ibdev)->mdev);
+		props->gid_tbl_len -= count;
 	}
+	return ret;
 }
 
 static int mlx5_ib_query_gid(struct ib_device *ibdev, u8 port, int index,
@@ -2263,7 +2249,7 @@ static struct mlx5_ib_flow_handler *create_flow_rule(struct mlx5_ib_dev *dev,
 	if (!is_valid_attr(dev->mdev, flow_attr))
 		return ERR_PTR(-EINVAL);
 
-	spec = mlx5_vzalloc(sizeof(*spec));
+	spec = kvzalloc(sizeof(*spec), GFP_KERNEL);
 	handler = kzalloc(sizeof(*handler), GFP_KERNEL);
 	if (!handler || !spec) {
 		err = -ENOMEM;
@@ -3468,7 +3454,7 @@ static int mlx5_ib_query_q_counters(struct mlx5_ib_dev *dev,
 	__be32 val;
 	int ret, i;
 
-	out = mlx5_vzalloc(outlen);
+	out = kvzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
 
@@ -3497,7 +3483,7 @@ static int mlx5_ib_query_cong_counters(struct mlx5_ib_dev *dev,
 	int ret, i;
 	int offset = port->cnts.num_q_counters;
 
-	out = mlx5_vzalloc(outlen);
+	out = kvzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
 
@@ -3542,6 +3528,11 @@ static int mlx5_ib_get_hw_stats(struct ib_device *ibdev,
 	return num_counters;
 }
 
+static void mlx5_ib_free_rdma_netdev(struct net_device *netdev)
+{
+	return mlx5_rdma_netdev_free(netdev);
+}
+
 static struct net_device*
 mlx5_ib_alloc_rdma_netdev(struct ib_device *hca,
 			  u8 port_num,
@@ -3550,16 +3541,19 @@ mlx5_ib_alloc_rdma_netdev(struct ib_device *hca,
 			  unsigned char name_assign_type,
 			  void (*setup)(struct net_device *))
 {
+	struct net_device *netdev;
+	struct rdma_netdev *rn;
+
 	if (type != RDMA_NETDEV_IPOIB)
 		return ERR_PTR(-EOPNOTSUPP);
 
-	return mlx5_rdma_netdev_alloc(to_mdev(hca)->mdev, hca,
-				      name, setup);
-}
-
-static void mlx5_ib_free_rdma_netdev(struct net_device *netdev)
-{
-	return mlx5_rdma_netdev_free(netdev);
+	netdev = mlx5_rdma_netdev_alloc(to_mdev(hca)->mdev, hca,
+					name, setup);
+	if (likely(!IS_ERR_OR_NULL(netdev))) {
+		rn = netdev_priv(netdev);
+		rn->free_rdma_netdev = mlx5_ib_free_rdma_netdev;
+	}
+	return netdev;
 }
 
 static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
@@ -3692,8 +3686,9 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 	dev->ib_dev.check_mr_status	= mlx5_ib_check_mr_status;
 	dev->ib_dev.get_port_immutable  = mlx5_port_immutable;
 	dev->ib_dev.get_dev_fw_str      = get_dev_fw_str;
-	dev->ib_dev.alloc_rdma_netdev	= mlx5_ib_alloc_rdma_netdev;
-	dev->ib_dev.free_rdma_netdev	= mlx5_ib_free_rdma_netdev;
+	if (MLX5_CAP_GEN(mdev, ipoib_enhanced_offloads))
+		dev->ib_dev.alloc_rdma_netdev	= mlx5_ib_alloc_rdma_netdev;
+
 	if (mlx5_core_is_pf(mdev)) {
 		dev->ib_dev.get_vf_config	= mlx5_ib_get_vf_config;
 		dev->ib_dev.set_vf_link_state	= mlx5_ib_set_vf_link_state;
diff --git a/drivers/infiniband/hw/mlx5/mr.c b/drivers/infiniband/hw/mlx5/mr.c
index 366433f71b58..763bb5b36144 100644
--- a/drivers/infiniband/hw/mlx5/mr.c
+++ b/drivers/infiniband/hw/mlx5/mr.c
@@ -1110,7 +1110,7 @@ static struct mlx5_ib_mr *reg_create(struct ib_mr *ibmr, struct ib_pd *pd,
 
 	inlen = MLX5_ST_SZ_BYTES(create_mkey_in) +
 		sizeof(*pas) * ((npages + 1) / 2) * 2;
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in) {
 		err = -ENOMEM;
 		goto err_1;
diff --git a/drivers/infiniband/hw/mlx5/qp.c b/drivers/infiniband/hw/mlx5/qp.c
index ebb6768684de..0889ff367c86 100644
--- a/drivers/infiniband/hw/mlx5/qp.c
+++ b/drivers/infiniband/hw/mlx5/qp.c
@@ -823,7 +823,7 @@ static int create_user_qp(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 
 	*inlen = MLX5_ST_SZ_BYTES(create_qp_in) +
 		 MLX5_FLD_SZ_BYTES(create_qp_in, pas[0]) * ncont;
-	*in = mlx5_vzalloc(*inlen);
+	*in = kvzalloc(*inlen, GFP_KERNEL);
 	if (!*in) {
 		err = -ENOMEM;
 		goto err_umem;
@@ -931,7 +931,7 @@ static int create_kernel_qp(struct mlx5_ib_dev *dev,
 	qp->sq.qend = mlx5_get_send_wqe(qp, qp->sq.wqe_cnt);
 	*inlen = MLX5_ST_SZ_BYTES(create_qp_in) +
 		 MLX5_FLD_SZ_BYTES(create_qp_in, pas[0]) * qp->buf.npages;
-	*in = mlx5_vzalloc(*inlen);
+	*in = kvzalloc(*inlen, GFP_KERNEL);
 	if (!*in) {
 		err = -ENOMEM;
 		goto err_buf;
@@ -1060,7 +1060,7 @@ static int create_raw_packet_qp_sq(struct mlx5_ib_dev *dev,
 		return err;
 
 	inlen = MLX5_ST_SZ_BYTES(create_sq_in) + sizeof(u64) * ncont;
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in) {
 		err = -ENOMEM;
 		goto err_umem;
@@ -1140,7 +1140,7 @@ static int create_raw_packet_qp_rq(struct mlx5_ib_dev *dev,
 	u32 rq_pas_size = get_rq_pas_size(qpc);
 
 	inlen = MLX5_ST_SZ_BYTES(create_rq_in) + rq_pas_size;
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -1193,7 +1193,7 @@ static int create_raw_packet_qp_tir(struct mlx5_ib_dev *dev,
 	int err;
 
 	inlen = MLX5_ST_SZ_BYTES(create_tir_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -1372,7 +1372,7 @@ static int create_rss_raw_qp_tir(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp,
 	}
 
 	inlen = MLX5_ST_SZ_BYTES(create_tir_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -1633,7 +1633,7 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 		if (err)
 			return err;
 	} else {
-		in = mlx5_vzalloc(inlen);
+		in = kvzalloc(inlen, GFP_KERNEL);
 		if (!in)
 			return -ENOMEM;
 
@@ -2164,7 +2164,7 @@ static int modify_raw_packet_eth_prio(struct mlx5_core_dev *dev,
 	int err;
 
 	inlen = MLX5_ST_SZ_BYTES(modify_tis_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -2189,7 +2189,7 @@ static int modify_raw_packet_tx_affinity(struct mlx5_core_dev *dev,
 	int err;
 
 	inlen = MLX5_ST_SZ_BYTES(modify_tis_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -2434,7 +2434,7 @@ static int modify_raw_packet_qp_rq(struct mlx5_ib_dev *dev,
 	int err;
 
 	inlen = MLX5_ST_SZ_BYTES(modify_rq_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -2479,7 +2479,7 @@ static int modify_raw_packet_qp_sq(struct mlx5_core_dev *dev,
 	int err;
 
 	inlen = MLX5_ST_SZ_BYTES(modify_sq_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -4281,7 +4281,7 @@ static int query_raw_packet_qp_sq_state(struct mlx5_ib_dev *dev,
 	int err;
 
 	inlen = MLX5_ST_SZ_BYTES(query_sq_out);
-	out = mlx5_vzalloc(inlen);
+	out = kvzalloc(inlen, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
 
@@ -4308,7 +4308,7 @@ static int query_raw_packet_qp_rq_state(struct mlx5_ib_dev *dev,
 	int err;
 
 	inlen = MLX5_ST_SZ_BYTES(query_rq_out);
-	out = mlx5_vzalloc(inlen);
+	out = kvzalloc(inlen, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
 
@@ -4612,7 +4612,7 @@ static int  create_rq(struct mlx5_ib_rwq *rwq, struct ib_pd *pd,
 	dev = to_mdev(pd->device);
 
 	inlen = MLX5_ST_SZ_BYTES(create_rq_in) + sizeof(u64) * rwq->rq_num_pas;
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -4842,7 +4842,7 @@ struct ib_rwq_ind_table *mlx5_ib_create_rwq_ind_table(struct ib_device *device,
 		return ERR_PTR(-ENOMEM);
 
 	inlen = MLX5_ST_SZ_BYTES(create_rqt_in) + sizeof(u32) * sz;
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in) {
 		err = -ENOMEM;
 		goto err;
@@ -4921,7 +4921,7 @@ int mlx5_ib_modify_wq(struct ib_wq *wq, struct ib_wq_attr *wq_attr,
 		return -EOPNOTSUPP;
 
 	inlen = MLX5_ST_SZ_BYTES(modify_rq_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
diff --git a/drivers/infiniband/hw/mlx5/srq.c b/drivers/infiniband/hw/mlx5/srq.c
index 7cb145f9a6db..43707b101f47 100644
--- a/drivers/infiniband/hw/mlx5/srq.c
+++ b/drivers/infiniband/hw/mlx5/srq.c
@@ -127,7 +127,7 @@ static int create_srq_user(struct ib_pd *pd, struct mlx5_ib_srq *srq,
 		goto err_umem;
 	}
 
-	in->pas = mlx5_vzalloc(sizeof(*in->pas) * ncont);
+	in->pas = kvzalloc(sizeof(*in->pas) * ncont, GFP_KERNEL);
 	if (!in->pas) {
 		err = -ENOMEM;
 		goto err_umem;
@@ -189,7 +189,7 @@ static int create_srq_kernel(struct mlx5_ib_dev *dev, struct mlx5_ib_srq *srq,
 	}
 
 	mlx5_ib_dbg(dev, "srq->buf.page_shift = %d\n", srq->buf.page_shift);
-	in->pas = mlx5_vzalloc(sizeof(*in->pas) * srq->buf.npages);
+	in->pas = kvzalloc(sizeof(*in->pas) * srq->buf.npages, GFP_KERNEL);
 	if (!in->pas) {
 		err = -ENOMEM;
 		goto err_buf;
diff --git a/drivers/infiniband/hw/nes/nes.c b/drivers/infiniband/hw/nes/nes.c
index 5b9601014f0c..a30aa6527f7e 100644
--- a/drivers/infiniband/hw/nes/nes.c
+++ b/drivers/infiniband/hw/nes/nes.c
@@ -815,7 +815,7 @@ static struct pci_driver nes_pci_driver = {
 	.remove = nes_remove,
 };
 
-static ssize_t nes_show_adapter(struct device_driver *ddp, char *buf)
+static ssize_t adapter_show(struct device_driver *ddp, char *buf)
 {
 	unsigned int  devfn = 0xffffffff;
 	unsigned char bus_number = 0xff;
@@ -834,7 +834,7 @@ static ssize_t nes_show_adapter(struct device_driver *ddp, char *buf)
 	return snprintf(buf, PAGE_SIZE, "%x:%x\n", bus_number, devfn);
 }
 
-static ssize_t nes_store_adapter(struct device_driver *ddp,
+static ssize_t adapter_store(struct device_driver *ddp,
 	const char *buf, size_t count)
 {
 	char *p = (char *)buf;
@@ -843,7 +843,7 @@ static ssize_t nes_store_adapter(struct device_driver *ddp,
 	return strnlen(buf, count);
 }
 
-static ssize_t nes_show_ee_cmd(struct device_driver *ddp, char *buf)
+static ssize_t eeprom_cmd_show(struct device_driver *ddp, char *buf)
 {
 	u32 eeprom_cmd = 0xdead;
 	u32 i = 0;
@@ -859,7 +859,7 @@ static ssize_t nes_show_ee_cmd(struct device_driver *ddp, char *buf)
 	return snprintf(buf, PAGE_SIZE, "0x%x\n", eeprom_cmd);
 }
 
-static ssize_t nes_store_ee_cmd(struct device_driver *ddp,
+static ssize_t eeprom_cmd_store(struct device_driver *ddp,
 	const char *buf, size_t count)
 {
 	char *p = (char *)buf;
@@ -880,7 +880,7 @@ static ssize_t nes_store_ee_cmd(struct device_driver *ddp,
 	return strnlen(buf, count);
 }
 
-static ssize_t nes_show_ee_data(struct device_driver *ddp, char *buf)
+static ssize_t eeprom_data_show(struct device_driver *ddp, char *buf)
 {
 	u32 eeprom_data = 0xdead;
 	u32 i = 0;
@@ -897,7 +897,7 @@ static ssize_t nes_show_ee_data(struct device_driver *ddp, char *buf)
 	return  snprintf(buf, PAGE_SIZE, "0x%x\n", eeprom_data);
 }
 
-static ssize_t nes_store_ee_data(struct device_driver *ddp,
+static ssize_t eeprom_data_store(struct device_driver *ddp,
 	const char *buf, size_t count)
 {
 	char *p = (char *)buf;
@@ -918,7 +918,7 @@ static ssize_t nes_store_ee_data(struct device_driver *ddp,
 	return strnlen(buf, count);
 }
 
-static ssize_t nes_show_flash_cmd(struct device_driver *ddp, char *buf)
+static ssize_t flash_cmd_show(struct device_driver *ddp, char *buf)
 {
 	u32 flash_cmd = 0xdead;
 	u32 i = 0;
@@ -935,7 +935,7 @@ static ssize_t nes_show_flash_cmd(struct device_driver *ddp, char *buf)
 	return  snprintf(buf, PAGE_SIZE, "0x%x\n", flash_cmd);
 }
 
-static ssize_t nes_store_flash_cmd(struct device_driver *ddp,
+static ssize_t flash_cmd_store(struct device_driver *ddp,
 	const char *buf, size_t count)
 {
 	char *p = (char *)buf;
@@ -956,7 +956,7 @@ static ssize_t nes_store_flash_cmd(struct device_driver *ddp,
 	return strnlen(buf, count);
 }
 
-static ssize_t nes_show_flash_data(struct device_driver *ddp, char *buf)
+static ssize_t flash_data_show(struct device_driver *ddp, char *buf)
 {
 	u32 flash_data = 0xdead;
 	u32 i = 0;
@@ -973,7 +973,7 @@ static ssize_t nes_show_flash_data(struct device_driver *ddp, char *buf)
 	return  snprintf(buf, PAGE_SIZE, "0x%x\n", flash_data);
 }
 
-static ssize_t nes_store_flash_data(struct device_driver *ddp,
+static ssize_t flash_data_store(struct device_driver *ddp,
 	const char *buf, size_t count)
 {
 	char *p = (char *)buf;
@@ -994,12 +994,12 @@ static ssize_t nes_store_flash_data(struct device_driver *ddp,
 	return strnlen(buf, count);
 }
 
-static ssize_t nes_show_nonidx_addr(struct device_driver *ddp, char *buf)
+static ssize_t nonidx_addr_show(struct device_driver *ddp, char *buf)
 {
 	return  snprintf(buf, PAGE_SIZE, "0x%x\n", sysfs_nonidx_addr);
 }
 
-static ssize_t nes_store_nonidx_addr(struct device_driver *ddp,
+static ssize_t nonidx_addr_store(struct device_driver *ddp,
 	const char *buf, size_t count)
 {
 	char *p = (char *)buf;
@@ -1010,7 +1010,7 @@ static ssize_t nes_store_nonidx_addr(struct device_driver *ddp,
 	return strnlen(buf, count);
 }
 
-static ssize_t nes_show_nonidx_data(struct device_driver *ddp, char *buf)
+static ssize_t nonidx_data_show(struct device_driver *ddp, char *buf)
 {
 	u32 nonidx_data = 0xdead;
 	u32 i = 0;
@@ -1027,7 +1027,7 @@ static ssize_t nes_show_nonidx_data(struct device_driver *ddp, char *buf)
 	return  snprintf(buf, PAGE_SIZE, "0x%x\n", nonidx_data);
 }
 
-static ssize_t nes_store_nonidx_data(struct device_driver *ddp,
+static ssize_t nonidx_data_store(struct device_driver *ddp,
 	const char *buf, size_t count)
 {
 	char *p = (char *)buf;
@@ -1048,12 +1048,12 @@ static ssize_t nes_store_nonidx_data(struct device_driver *ddp,
 	return strnlen(buf, count);
 }
 
-static ssize_t nes_show_idx_addr(struct device_driver *ddp, char *buf)
+static ssize_t idx_addr_show(struct device_driver *ddp, char *buf)
 {
 	return  snprintf(buf, PAGE_SIZE, "0x%x\n", sysfs_idx_addr);
 }
 
-static ssize_t nes_store_idx_addr(struct device_driver *ddp,
+static ssize_t idx_addr_store(struct device_driver *ddp,
 	const char *buf, size_t count)
 {
 	char *p = (char *)buf;
@@ -1064,7 +1064,7 @@ static ssize_t nes_store_idx_addr(struct device_driver *ddp,
 	return strnlen(buf, count);
 }
 
-static ssize_t nes_show_idx_data(struct device_driver *ddp, char *buf)
+static ssize_t idx_data_show(struct device_driver *ddp, char *buf)
 {
 	u32 idx_data = 0xdead;
 	u32 i = 0;
@@ -1081,7 +1081,7 @@ static ssize_t nes_show_idx_data(struct device_driver *ddp, char *buf)
 	return  snprintf(buf, PAGE_SIZE, "0x%x\n", idx_data);
 }
 
-static ssize_t nes_store_idx_data(struct device_driver *ddp,
+static ssize_t idx_data_store(struct device_driver *ddp,
 	const char *buf, size_t count)
 {
 	char *p = (char *)buf;
@@ -1102,11 +1102,7 @@ static ssize_t nes_store_idx_data(struct device_driver *ddp,
 	return strnlen(buf, count);
 }
 
-
-/**
- * nes_show_wqm_quanta
- */
-static ssize_t nes_show_wqm_quanta(struct device_driver *ddp, char *buf)
+static ssize_t wqm_quanta_show(struct device_driver *ddp, char *buf)
 {
 	u32 wqm_quanta_value = 0xdead;
 	u32 i = 0;
@@ -1123,12 +1119,8 @@ static ssize_t nes_show_wqm_quanta(struct device_driver *ddp, char *buf)
 	return  snprintf(buf, PAGE_SIZE, "0x%X\n", wqm_quanta_value);
 }
 
-
-/**
- * nes_store_wqm_quanta
- */
-static ssize_t nes_store_wqm_quanta(struct device_driver *ddp,
-					const char *buf, size_t count)
+static ssize_t wqm_quanta_store(struct device_driver *ddp, const char *buf,
+				size_t count)
 {
 	unsigned long wqm_quanta_value;
 	u32 wqm_config1;
@@ -1153,26 +1145,16 @@ static ssize_t nes_store_wqm_quanta(struct device_driver *ddp,
 	return strnlen(buf, count);
 }
 
-static DRIVER_ATTR(adapter, S_IRUSR | S_IWUSR,
-		   nes_show_adapter, nes_store_adapter);
-static DRIVER_ATTR(eeprom_cmd, S_IRUSR | S_IWUSR,
-		   nes_show_ee_cmd, nes_store_ee_cmd);
-static DRIVER_ATTR(eeprom_data, S_IRUSR | S_IWUSR,
-		   nes_show_ee_data, nes_store_ee_data);
-static DRIVER_ATTR(flash_cmd, S_IRUSR | S_IWUSR,
-		   nes_show_flash_cmd, nes_store_flash_cmd);
-static DRIVER_ATTR(flash_data, S_IRUSR | S_IWUSR,
-		   nes_show_flash_data, nes_store_flash_data);
-static DRIVER_ATTR(nonidx_addr, S_IRUSR | S_IWUSR,
-		   nes_show_nonidx_addr, nes_store_nonidx_addr);
-static DRIVER_ATTR(nonidx_data, S_IRUSR | S_IWUSR,
-		   nes_show_nonidx_data, nes_store_nonidx_data);
-static DRIVER_ATTR(idx_addr, S_IRUSR | S_IWUSR,
-		   nes_show_idx_addr, nes_store_idx_addr);
-static DRIVER_ATTR(idx_data, S_IRUSR | S_IWUSR,
-		   nes_show_idx_data, nes_store_idx_data);
-static DRIVER_ATTR(wqm_quanta, S_IRUSR | S_IWUSR,
-		   nes_show_wqm_quanta, nes_store_wqm_quanta);
+static DRIVER_ATTR_RW(adapter);
+static DRIVER_ATTR_RW(eeprom_cmd);
+static DRIVER_ATTR_RW(eeprom_data);
+static DRIVER_ATTR_RW(flash_cmd);
+static DRIVER_ATTR_RW(flash_data);
+static DRIVER_ATTR_RW(nonidx_addr);
+static DRIVER_ATTR_RW(nonidx_data);
+static DRIVER_ATTR_RW(idx_addr);
+static DRIVER_ATTR_RW(idx_data);
+static DRIVER_ATTR_RW(wqm_quanta);
 
 static int nes_create_driver_sysfs(struct pci_driver *drv)
 {
diff --git a/drivers/infiniband/hw/nes/nes_cm.c b/drivers/infiniband/hw/nes/nes_cm.c
index 30b256a2c54e..de4025deaa4a 100644
--- a/drivers/infiniband/hw/nes/nes_cm.c
+++ b/drivers/infiniband/hw/nes/nes_cm.c
@@ -742,7 +742,7 @@ int schedule_nes_timer(struct nes_cm_node *cm_node, struct sk_buff *skb,
 
 	if (type == NES_TIMER_TYPE_SEND) {
 		new_send->seq_num = ntohl(tcp_hdr(skb)->seq);
-		atomic_inc(&new_send->skb->users);
+		refcount_inc(&new_send->skb->users);
 		spin_lock_irqsave(&cm_node->retrans_list_lock, flags);
 		cm_node->send_entry = new_send;
 		add_ref_cm_node(cm_node);
@@ -924,7 +924,7 @@ static void nes_cm_timer_tick(unsigned long pass)
 						  flags);
 				break;
 			}
-			atomic_inc(&send_entry->skb->users);
+			refcount_inc(&send_entry->skb->users);
 			cm_packets_retrans++;
 			nes_debug(NES_DBG_CM, "Retransmitting send_entry %p "
 				  "for node %p, jiffies = %lu, time to send = "
diff --git a/drivers/infiniband/hw/qedr/main.c b/drivers/infiniband/hw/qedr/main.c
index 6a72095d6c7a..b5851fd67d4f 100644
--- a/drivers/infiniband/hw/qedr/main.c
+++ b/drivers/infiniband/hw/qedr/main.c
@@ -37,7 +37,7 @@
 #include <linux/iommu.h>
 #include <linux/pci.h>
 #include <net/addrconf.h>
-#include <linux/qed/qede_roce.h>
+
 #include <linux/qed/qed_chain.h>
 #include <linux/qed/qed_if.h>
 #include "qedr.h"
@@ -276,7 +276,7 @@ static int qedr_alloc_resources(struct qedr_dev *dev)
 						   QED_CHAIN_CNT_TYPE_U16,
 						   n_entries,
 						   sizeof(struct regpair *),
-						   &cnq->pbl);
+						   &cnq->pbl, NULL);
 		if (rc)
 			goto err4;
 
@@ -886,9 +886,9 @@ static void qedr_mac_address_change(struct qedr_dev *dev)
 	memcpy(&sgid->raw[8], guid, sizeof(guid));
 
 	/* Update LL2 */
-	rc = dev->ops->roce_ll2_set_mac_filter(dev->cdev,
-					       dev->gsi_ll2_mac_address,
-					       dev->ndev->dev_addr);
+	rc = dev->ops->ll2_set_mac_filter(dev->cdev,
+					  dev->gsi_ll2_mac_address,
+					  dev->ndev->dev_addr);
 
 	ether_addr_copy(dev->gsi_ll2_mac_address, dev->ndev->dev_addr);
 
@@ -902,7 +902,7 @@ static void qedr_mac_address_change(struct qedr_dev *dev)
  * initialization done before RoCE driver notifies
  * event to stack.
  */
-static void qedr_notify(struct qedr_dev *dev, enum qede_roce_event event)
+static void qedr_notify(struct qedr_dev *dev, enum qede_rdma_event event)
 {
 	switch (event) {
 	case QEDE_UP:
@@ -931,12 +931,12 @@ static struct qedr_driver qedr_drv = {
 
 static int __init qedr_init_module(void)
 {
-	return qede_roce_register_driver(&qedr_drv);
+	return qede_rdma_register_driver(&qedr_drv);
 }
 
 static void __exit qedr_exit_module(void)
 {
-	qede_roce_unregister_driver(&qedr_drv);
+	qede_rdma_unregister_driver(&qedr_drv);
 }
 
 module_init(qedr_init_module);
diff --git a/drivers/infiniband/hw/qedr/qedr.h b/drivers/infiniband/hw/qedr/qedr.h
index aa08c76a4245..ab7784bfdac6 100644
--- a/drivers/infiniband/hw/qedr/qedr.h
+++ b/drivers/infiniband/hw/qedr/qedr.h
@@ -36,8 +36,8 @@
 #include <rdma/ib_addr.h>
 #include <linux/qed/qed_if.h>
 #include <linux/qed/qed_chain.h>
-#include <linux/qed/qed_roce_if.h>
-#include <linux/qed/qede_roce.h>
+#include <linux/qed/qed_rdma_if.h>
+#include <linux/qed/qede_rdma.h>
 #include <linux/qed/roce_common.h>
 #include "qedr_hsi_rdma.h"
 
@@ -58,7 +58,10 @@
 #define QEDR_MSG_QP   "  QP"
 #define QEDR_MSG_GSI  " GSI"
 
-#define QEDR_CQ_MAGIC_NUMBER   (0x11223344)
+#define QEDR_CQ_MAGIC_NUMBER	(0x11223344)
+
+#define FW_PAGE_SIZE		(RDMA_RING_PAGE_SIZE)
+#define FW_PAGE_SHIFT		(12)
 
 struct qedr_dev;
 
@@ -150,6 +153,8 @@ struct qedr_dev {
 	u32			dp_module;
 	u8			dp_level;
 	u8			num_hwfns;
+	u8			gsi_ll2_handle;
+
 	uint			wq_multiplier;
 	u8			gsi_ll2_mac_address[ETH_ALEN];
 	int			gsi_qp_created;
diff --git a/drivers/infiniband/hw/qedr/qedr_cm.c b/drivers/infiniband/hw/qedr/qedr_cm.c
index d86dbe814d98..4689e802b332 100644
--- a/drivers/infiniband/hw/qedr/qedr_cm.c
+++ b/drivers/infiniband/hw/qedr/qedr_cm.c
@@ -44,7 +44,7 @@
 #include <rdma/ib_cache.h>
 
 #include <linux/qed/qed_if.h>
-#include <linux/qed/qed_roce_if.h>
+#include <linux/qed/qed_rdma_if.h>
 #include "qedr.h"
 #include "verbs.h"
 #include <rdma/qedr-abi.h>
@@ -64,9 +64,14 @@ void qedr_store_gsi_qp_cq(struct qedr_dev *dev, struct qedr_qp *qp,
 	dev->gsi_qp = qp;
 }
 
-void qedr_ll2_tx_cb(void *_qdev, struct qed_roce_ll2_packet *pkt)
+void qedr_ll2_complete_tx_packet(void *cxt,
+				 u8 connection_handle,
+				 void *cookie,
+				 dma_addr_t first_frag_addr,
+				 bool b_last_fragment, bool b_last_packet)
 {
-	struct qedr_dev *dev = (struct qedr_dev *)_qdev;
+	struct qedr_dev *dev = (struct qedr_dev *)cxt;
+	struct qed_roce_ll2_packet *pkt = cookie;
 	struct qedr_cq *cq = dev->gsi_sqcq;
 	struct qedr_qp *qp = dev->gsi_qp;
 	unsigned long flags;
@@ -88,20 +93,26 @@ void qedr_ll2_tx_cb(void *_qdev, struct qed_roce_ll2_packet *pkt)
 		(*cq->ibcq.comp_handler) (&cq->ibcq, cq->ibcq.cq_context);
 }
 
-void qedr_ll2_rx_cb(void *_dev, struct qed_roce_ll2_packet *pkt,
-		    struct qed_roce_ll2_rx_params *params)
+void qedr_ll2_complete_rx_packet(void *cxt,
+				 struct qed_ll2_comp_rx_data *data)
 {
-	struct qedr_dev *dev = (struct qedr_dev *)_dev;
+	struct qedr_dev *dev = (struct qedr_dev *)cxt;
 	struct qedr_cq *cq = dev->gsi_rqcq;
 	struct qedr_qp *qp = dev->gsi_qp;
 	unsigned long flags;
 
 	spin_lock_irqsave(&qp->q_lock, flags);
 
-	qp->rqe_wr_id[qp->rq.gsi_cons].rc = params->rc;
-	qp->rqe_wr_id[qp->rq.gsi_cons].vlan_id = params->vlan_id;
-	qp->rqe_wr_id[qp->rq.gsi_cons].sg_list[0].length = pkt->payload[0].len;
-	ether_addr_copy(qp->rqe_wr_id[qp->rq.gsi_cons].smac, params->smac);
+	qp->rqe_wr_id[qp->rq.gsi_cons].rc = data->u.data_length_error ?
+		-EINVAL : 0;
+	qp->rqe_wr_id[qp->rq.gsi_cons].vlan_id = data->vlan;
+	/* note: length stands for data length i.e. GRH is excluded */
+	qp->rqe_wr_id[qp->rq.gsi_cons].sg_list[0].length =
+		data->length.data_length;
+	*((u32 *)&qp->rqe_wr_id[qp->rq.gsi_cons].smac[0]) =
+		ntohl(data->opaque_data_0);
+	*((u16 *)&qp->rqe_wr_id[qp->rq.gsi_cons].smac[4]) =
+		ntohs((u16)data->opaque_data_1);
 
 	qedr_inc_sw_gsi_cons(&qp->rq);
 
@@ -111,6 +122,14 @@ void qedr_ll2_rx_cb(void *_dev, struct qed_roce_ll2_packet *pkt,
 		(*cq->ibcq.comp_handler) (&cq->ibcq, cq->ibcq.cq_context);
 }
 
+void qedr_ll2_release_rx_packet(void *cxt,
+				u8 connection_handle,
+				void *cookie,
+				dma_addr_t rx_buf_addr, bool b_last_packet)
+{
+	/* Do nothing... */
+}
+
 static void qedr_destroy_gsi_cq(struct qedr_dev *dev,
 				struct ib_qp_init_attr *attrs)
 {
@@ -159,27 +178,159 @@ static inline int qedr_check_gsi_qp_attrs(struct qedr_dev *dev,
 	return 0;
 }
 
+static int qedr_ll2_post_tx(struct qedr_dev *dev,
+			    struct qed_roce_ll2_packet *pkt)
+{
+	enum qed_ll2_roce_flavor_type roce_flavor;
+	struct qed_ll2_tx_pkt_info ll2_tx_pkt;
+	int rc;
+	int i;
+
+	memset(&ll2_tx_pkt, 0, sizeof(ll2_tx_pkt));
+
+	roce_flavor = (pkt->roce_mode == ROCE_V1) ?
+	    QED_LL2_ROCE : QED_LL2_RROCE;
+
+	if (pkt->roce_mode == ROCE_V2_IPV4)
+		ll2_tx_pkt.enable_ip_cksum = 1;
+
+	ll2_tx_pkt.num_of_bds = 1 /* hdr */  + pkt->n_seg;
+	ll2_tx_pkt.vlan = 0;
+	ll2_tx_pkt.tx_dest = pkt->tx_dest;
+	ll2_tx_pkt.qed_roce_flavor = roce_flavor;
+	ll2_tx_pkt.first_frag = pkt->header.baddr;
+	ll2_tx_pkt.first_frag_len = pkt->header.len;
+	ll2_tx_pkt.cookie = pkt;
+
+	/* tx header */
+	rc = dev->ops->ll2_prepare_tx_packet(dev->rdma_ctx,
+					     dev->gsi_ll2_handle,
+					     &ll2_tx_pkt, 1);
+	if (rc) {
+		/* TX failed while posting header - release resources */
+		dma_free_coherent(&dev->pdev->dev, pkt->header.len,
+				  pkt->header.vaddr, pkt->header.baddr);
+		kfree(pkt);
+
+		DP_ERR(dev, "roce ll2 tx: header failed (rc=%d)\n", rc);
+		return rc;
+	}
+
+	/* tx payload */
+	for (i = 0; i < pkt->n_seg; i++) {
+		rc = dev->ops->ll2_set_fragment_of_tx_packet(
+			dev->rdma_ctx,
+			dev->gsi_ll2_handle,
+			pkt->payload[i].baddr,
+			pkt->payload[i].len);
+
+		if (rc) {
+			/* if failed not much to do here, partial packet has
+			 * been posted we can't free memory, will need to wait
+			 * for completion
+			 */
+			DP_ERR(dev, "ll2 tx: payload failed (rc=%d)\n", rc);
+			return rc;
+		}
+	}
+
+	return 0;
+}
+
+int qedr_ll2_stop(struct qedr_dev *dev)
+{
+	int rc;
+
+	if (dev->gsi_ll2_handle == QED_LL2_UNUSED_HANDLE)
+		return 0;
+
+	/* remove LL2 MAC address filter */
+	rc = dev->ops->ll2_set_mac_filter(dev->cdev,
+					  dev->gsi_ll2_mac_address, NULL);
+
+	rc = dev->ops->ll2_terminate_connection(dev->rdma_ctx,
+						dev->gsi_ll2_handle);
+	if (rc)
+		DP_ERR(dev, "Failed to terminate LL2 connection (rc=%d)\n", rc);
+
+	dev->ops->ll2_release_connection(dev->rdma_ctx, dev->gsi_ll2_handle);
+
+	dev->gsi_ll2_handle = QED_LL2_UNUSED_HANDLE;
+
+	return rc;
+}
+
+int qedr_ll2_start(struct qedr_dev *dev,
+		   struct ib_qp_init_attr *attrs, struct qedr_qp *qp)
+{
+	struct qed_ll2_acquire_data data;
+	struct qed_ll2_cbs cbs;
+	int rc;
+
+	/* configure and start LL2 */
+	cbs.rx_comp_cb = qedr_ll2_complete_rx_packet;
+	cbs.tx_comp_cb = qedr_ll2_complete_tx_packet;
+	cbs.rx_release_cb = qedr_ll2_release_rx_packet;
+	cbs.tx_release_cb = qedr_ll2_complete_tx_packet;
+	cbs.cookie = dev;
+
+	memset(&data, 0, sizeof(data));
+	data.input.conn_type = QED_LL2_TYPE_ROCE;
+	data.input.mtu = dev->ndev->mtu;
+	data.input.rx_num_desc = attrs->cap.max_recv_wr;
+	data.input.rx_drop_ttl0_flg = true;
+	data.input.rx_vlan_removal_en = false;
+	data.input.tx_num_desc = attrs->cap.max_send_wr;
+	data.input.tx_tc = 0;
+	data.input.tx_dest = QED_LL2_TX_DEST_NW;
+	data.input.ai_err_packet_too_big = QED_LL2_DROP_PACKET;
+	data.input.ai_err_no_buf = QED_LL2_DROP_PACKET;
+	data.input.gsi_enable = 1;
+	data.p_connection_handle = &dev->gsi_ll2_handle;
+	data.cbs = &cbs;
+
+	rc = dev->ops->ll2_acquire_connection(dev->rdma_ctx, &data);
+	if (rc) {
+		DP_ERR(dev,
+		       "ll2 start: failed to acquire LL2 connection (rc=%d)\n",
+		       rc);
+		return rc;
+	}
+
+	rc = dev->ops->ll2_establish_connection(dev->rdma_ctx,
+						dev->gsi_ll2_handle);
+	if (rc) {
+		DP_ERR(dev,
+		       "ll2 start: failed to establish LL2 connection (rc=%d)\n",
+		       rc);
+		goto err1;
+	}
+
+	rc = dev->ops->ll2_set_mac_filter(dev->cdev, NULL, dev->ndev->dev_addr);
+	if (rc)
+		goto err2;
+
+	return 0;
+
+err2:
+	dev->ops->ll2_terminate_connection(dev->rdma_ctx, dev->gsi_ll2_handle);
+err1:
+	dev->ops->ll2_release_connection(dev->rdma_ctx, dev->gsi_ll2_handle);
+
+	return rc;
+}
+
 struct ib_qp *qedr_create_gsi_qp(struct qedr_dev *dev,
 				 struct ib_qp_init_attr *attrs,
 				 struct qedr_qp *qp)
 {
-	struct qed_roce_ll2_params ll2_params;
 	int rc;
 
 	rc = qedr_check_gsi_qp_attrs(dev, attrs);
 	if (rc)
 		return ERR_PTR(rc);
 
-	/* configure and start LL2 */
-	memset(&ll2_params, 0, sizeof(ll2_params));
-	ll2_params.max_tx_buffers = attrs->cap.max_send_wr;
-	ll2_params.max_rx_buffers = attrs->cap.max_recv_wr;
-	ll2_params.cbs.tx_cb = qedr_ll2_tx_cb;
-	ll2_params.cbs.rx_cb = qedr_ll2_rx_cb;
-	ll2_params.cb_cookie = (void *)dev;
-	ll2_params.mtu = dev->ndev->mtu;
-	ether_addr_copy(ll2_params.mac_address, dev->ndev->dev_addr);
-	rc = dev->ops->roce_ll2_start(dev->cdev, &ll2_params);
+	rc = qedr_ll2_start(dev, attrs, qp);
 	if (rc) {
 		DP_ERR(dev, "create gsi qp: failed on ll2 start. rc=%d\n", rc);
 		return ERR_PTR(rc);
@@ -214,7 +365,7 @@ struct ib_qp *qedr_create_gsi_qp(struct qedr_dev *dev,
 err:
 	kfree(qp->rqe_wr_id);
 
-	rc = dev->ops->roce_ll2_stop(dev->cdev);
+	rc = qedr_ll2_stop(dev);
 	if (rc)
 		DP_ERR(dev, "create gsi qp: failed destroy on create\n");
 
@@ -223,15 +374,7 @@ err:
 
 int qedr_destroy_gsi_qp(struct qedr_dev *dev)
 {
-	int rc;
-
-	rc = dev->ops->roce_ll2_stop(dev->cdev);
-	if (rc)
-		DP_ERR(dev, "destroy gsi qp: failed (rc=%d)\n", rc);
-	else
-		DP_DEBUG(dev, QEDR_MSG_GSI, "destroy gsi qp: success\n");
-
-	return rc;
+	return qedr_ll2_stop(dev);
 }
 
 #define QEDR_MAX_UD_HEADER_SIZE	(100)
@@ -421,7 +564,6 @@ int qedr_gsi_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 {
 	struct qed_roce_ll2_packet *pkt = NULL;
 	struct qedr_qp *qp = get_qedr_qp(ibqp);
-	struct qed_roce_ll2_tx_params params;
 	struct qedr_dev *dev = qp->dev;
 	unsigned long flags;
 	int rc;
@@ -449,8 +591,6 @@ int qedr_gsi_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 		goto err;
 	}
 
-	memset(&params, 0, sizeof(params));
-
 	spin_lock_irqsave(&qp->q_lock, flags);
 
 	rc = qedr_gsi_build_packet(dev, qp, wr, &pkt);
@@ -459,7 +599,8 @@ int qedr_gsi_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 		goto err;
 	}
 
-	rc = dev->ops->roce_ll2_tx(dev->cdev, pkt, &params);
+	rc = qedr_ll2_post_tx(dev, pkt);
+
 	if (!rc) {
 		qp->wqe_wr_id[qp->sq.prod].wr_id = wr->wr_id;
 		qedr_inc_sw_prod(&qp->sq);
@@ -467,17 +608,6 @@ int qedr_gsi_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 			 "gsi post send: opcode=%d, in_irq=%ld, irqs_disabled=%d, wr_id=%llx\n",
 			 wr->opcode, in_irq(), irqs_disabled(), wr->wr_id);
 	} else {
-		if (rc == QED_ROCE_TX_HEAD_FAILURE) {
-			/* TX failed while posting header - release resources */
-			dma_free_coherent(&dev->pdev->dev, pkt->header.len,
-					  pkt->header.vaddr, pkt->header.baddr);
-			kfree(pkt);
-		} else if (rc == QED_ROCE_TX_FRAG_FAILURE) {
-			/* NTD since TX failed while posting a fragment. We will
-			 * release the resources on TX callback
-			 */
-		}
-
 		DP_ERR(dev, "gsi post send: failed to transmit (rc=%d)\n", rc);
 		rc = -EAGAIN;
 		*bad_wr = wr;
@@ -504,10 +634,8 @@ int qedr_gsi_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *wr,
 {
 	struct qedr_dev *dev = get_qedr_dev(ibqp->device);
 	struct qedr_qp *qp = get_qedr_qp(ibqp);
-	struct qed_roce_ll2_buffer buf;
 	unsigned long flags;
-	int status = 0;
-	int rc;
+	int rc = 0;
 
 	if ((qp->state != QED_ROCE_QP_STATE_RTR) &&
 	    (qp->state != QED_ROCE_QP_STATE_RTS)) {
@@ -518,8 +646,6 @@ int qedr_gsi_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *wr,
 		return -EINVAL;
 	}
 
-	memset(&buf, 0, sizeof(buf));
-
 	spin_lock_irqsave(&qp->q_lock, flags);
 
 	while (wr) {
@@ -530,10 +656,12 @@ int qedr_gsi_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *wr,
 			goto err;
 		}
 
-		buf.baddr = wr->sg_list[0].addr;
-		buf.len = wr->sg_list[0].length;
-
-		rc = dev->ops->roce_ll2_post_rx_buffer(dev->cdev, &buf, 0, 1);
+		rc = dev->ops->ll2_post_rx_buffer(dev->rdma_ctx,
+						  dev->gsi_ll2_handle,
+						  wr->sg_list[0].addr,
+						  wr->sg_list[0].length,
+						  0 /* cookie */,
+						  1 /* notify_fw */);
 		if (rc) {
 			DP_ERR(dev,
 			       "gsi post recv: failed to post rx buffer (rc=%d)\n",
@@ -553,7 +681,7 @@ int qedr_gsi_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *wr,
 
 	spin_unlock_irqrestore(&qp->q_lock, flags);
 
-	return status;
+	return rc;
 err:
 	spin_unlock_irqrestore(&qp->q_lock, flags);
 	*bad_wr = wr;
diff --git a/drivers/infiniband/hw/qedr/verbs.c b/drivers/infiniband/hw/qedr/verbs.c
index 17685cfea6a2..548e4d1e998f 100644
--- a/drivers/infiniband/hw/qedr/verbs.c
+++ b/drivers/infiniband/hw/qedr/verbs.c
@@ -653,14 +653,15 @@ static int qedr_prepare_pbl_tbl(struct qedr_dev *dev,
 
 static void qedr_populate_pbls(struct qedr_dev *dev, struct ib_umem *umem,
 			       struct qedr_pbl *pbl,
-			       struct qedr_pbl_info *pbl_info)
+			       struct qedr_pbl_info *pbl_info, u32 pg_shift)
 {
 	int shift, pg_cnt, pages, pbe_cnt, total_num_pbes = 0;
+	u32 fw_pg_cnt, fw_pg_per_umem_pg;
 	struct qedr_pbl *pbl_tbl;
 	struct scatterlist *sg;
 	struct regpair *pbe;
+	u64 pg_addr;
 	int entry;
-	u32 addr;
 
 	if (!pbl_info->num_pbes)
 		return;
@@ -683,29 +684,35 @@ static void qedr_populate_pbls(struct qedr_dev *dev, struct ib_umem *umem,
 
 	shift = umem->page_shift;
 
+	fw_pg_per_umem_pg = BIT(umem->page_shift - pg_shift);
+
 	for_each_sg(umem->sg_head.sgl, sg, umem->nmap, entry) {
 		pages = sg_dma_len(sg) >> shift;
+		pg_addr = sg_dma_address(sg);
 		for (pg_cnt = 0; pg_cnt < pages; pg_cnt++) {
-			/* store the page address in pbe */
-			pbe->lo = cpu_to_le32(sg_dma_address(sg) +
-					      (pg_cnt << shift));
-			addr = upper_32_bits(sg_dma_address(sg) +
-					     (pg_cnt << shift));
-			pbe->hi = cpu_to_le32(addr);
-			pbe_cnt++;
-			total_num_pbes++;
-			pbe++;
-
-			if (total_num_pbes == pbl_info->num_pbes)
-				return;
-
-			/* If the given pbl is full storing the pbes,
-			 * move to next pbl.
-			 */
-			if (pbe_cnt == (pbl_info->pbl_size / sizeof(u64))) {
-				pbl_tbl++;
-				pbe = (struct regpair *)pbl_tbl->va;
-				pbe_cnt = 0;
+			for (fw_pg_cnt = 0; fw_pg_cnt < fw_pg_per_umem_pg;) {
+				pbe->lo = cpu_to_le32(pg_addr);
+				pbe->hi = cpu_to_le32(upper_32_bits(pg_addr));
+
+				pg_addr += BIT(pg_shift);
+				pbe_cnt++;
+				total_num_pbes++;
+				pbe++;
+
+				if (total_num_pbes == pbl_info->num_pbes)
+					return;
+
+				/* If the given pbl is full storing the pbes,
+				 * move to next pbl.
+				 */
+				if (pbe_cnt ==
+				    (pbl_info->pbl_size / sizeof(u64))) {
+					pbl_tbl++;
+					pbe = (struct regpair *)pbl_tbl->va;
+					pbe_cnt = 0;
+				}
+
+				fw_pg_cnt++;
 			}
 		}
 	}
@@ -754,7 +761,7 @@ static inline int qedr_init_user_queue(struct ib_ucontext *ib_ctx,
 				       u64 buf_addr, size_t buf_len,
 				       int access, int dmasync)
 {
-	int page_cnt;
+	u32 fw_pages;
 	int rc;
 
 	q->buf_addr = buf_addr;
@@ -766,8 +773,10 @@ static inline int qedr_init_user_queue(struct ib_ucontext *ib_ctx,
 		return PTR_ERR(q->umem);
 	}
 
-	page_cnt = ib_umem_page_count(q->umem);
-	rc = qedr_prepare_pbl_tbl(dev, &q->pbl_info, page_cnt, 0);
+	fw_pages = ib_umem_page_count(q->umem) <<
+	    (q->umem->page_shift - FW_PAGE_SHIFT);
+
+	rc = qedr_prepare_pbl_tbl(dev, &q->pbl_info, fw_pages, 0);
 	if (rc)
 		goto err0;
 
@@ -777,7 +786,8 @@ static inline int qedr_init_user_queue(struct ib_ucontext *ib_ctx,
 		goto err0;
 	}
 
-	qedr_populate_pbls(dev, q->umem, q->pbl_tbl, &q->pbl_info);
+		qedr_populate_pbls(dev, q->umem, q->pbl_tbl, &q->pbl_info,
+				   FW_PAGE_SHIFT);
 
 	return 0;
 
@@ -925,7 +935,7 @@ struct ib_cq *qedr_create_cq(struct ib_device *ibdev,
 						   QED_CHAIN_CNT_TYPE_U32,
 						   chain_entries,
 						   sizeof(union rdma_cqe),
-						   &cq->pbl);
+						   &cq->pbl, NULL);
 		if (rc)
 			goto err1;
 
@@ -1413,7 +1423,7 @@ qedr_roce_create_kernel_qp(struct qedr_dev *dev,
 					   QED_CHAIN_CNT_TYPE_U32,
 					   n_sq_elems,
 					   QEDR_SQE_ELEMENT_SIZE,
-					   &qp->sq.pbl);
+					   &qp->sq.pbl, NULL);
 
 	if (rc)
 		return rc;
@@ -1427,7 +1437,7 @@ qedr_roce_create_kernel_qp(struct qedr_dev *dev,
 					   QED_CHAIN_CNT_TYPE_U32,
 					   n_rq_elems,
 					   QEDR_RQE_ELEMENT_SIZE,
-					   &qp->rq.pbl);
+					   &qp->rq.pbl, NULL);
 	if (rc)
 		return rc;
 
@@ -2226,7 +2236,7 @@ struct ib_mr *qedr_reg_user_mr(struct ib_pd *ibpd, u64 start, u64 len,
 		goto err1;
 
 	qedr_populate_pbls(dev, mr->umem, mr->info.pbl_table,
-			   &mr->info.pbl_info);
+			   &mr->info.pbl_info, mr->umem->page_shift);
 
 	rc = dev->ops->rdma_alloc_tid(dev->rdma_ctx, &mr->hw_mr.itid);
 	if (rc) {
@@ -3209,6 +3219,10 @@ static int process_req(struct qedr_dev *dev, struct qedr_qp *qp,
 		case IB_WC_REG_MR:
 			qp->wqe_wr_id[qp->sq.cons].mr->info.completed++;
 			break;
+		case IB_WC_RDMA_READ:
+		case IB_WC_SEND:
+			wc->byte_len = qp->wqe_wr_id[qp->sq.cons].bytes_len;
+			break;
 		default:
 			break;
 		}
diff --git a/drivers/infiniband/sw/rxe/rxe.h b/drivers/infiniband/sw/rxe/rxe.h
index ecdba2fce083..1ac5b8551a4d 100644
--- a/drivers/infiniband/sw/rxe/rxe.h
+++ b/drivers/infiniband/sw/rxe/rxe.h
@@ -68,6 +68,7 @@
 static inline u32 rxe_crc32(struct rxe_dev *rxe,
 			    u32 crc, void *next, size_t len)
 {
+	u32 retval;
 	int err;
 
 	SHASH_DESC_ON_STACK(shash, rxe->tfm);
@@ -81,7 +82,9 @@ static inline u32 rxe_crc32(struct rxe_dev *rxe,
 		return crc32_le(crc, next, len);
 	}
 
-	return *(u32 *)shash_desc_ctx(shash);
+	retval = *(u32 *)shash_desc_ctx(shash);
+	barrier_data(shash_desc_ctx(shash));
+	return retval;
 }
 
 int rxe_set_mtu(struct rxe_dev *rxe, unsigned int dev_mtu);
diff --git a/drivers/infiniband/sw/rxe/rxe_verbs.c b/drivers/infiniband/sw/rxe/rxe_verbs.c
index 83d709e74dfb..073e66783f1d 100644
--- a/drivers/infiniband/sw/rxe/rxe_verbs.c
+++ b/drivers/infiniband/sw/rxe/rxe_verbs.c
@@ -740,13 +740,8 @@ static int init_send_wqe(struct rxe_qp *qp, struct ib_send_wr *ibwr,
 
 		sge = ibwr->sg_list;
 		for (i = 0; i < num_sge; i++, sge++) {
-			if (qp->is_user && copy_from_user(p, (__user void *)
-					    (uintptr_t)sge->addr, sge->length))
-				return -EFAULT;
-
-			else if (!qp->is_user)
-				memcpy(p, (void *)(uintptr_t)sge->addr,
-				       sge->length);
+			memcpy(p, (void *)(uintptr_t)sge->addr,
+					sge->length);
 
 			p += sge->length;
 		}
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_ib.c b/drivers/infiniband/ulp/ipoib/ipoib_ib.c
index 0060b2f9f659..efe7402f4885 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_ib.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_ib.c
@@ -863,7 +863,6 @@ dev_stop:
 	set_bit(IPOIB_STOP_REAPER, &priv->flags);
 	cancel_delayed_work(&priv->ah_reap_task);
 	set_bit(IPOIB_FLAG_INITIALIZED, &priv->flags);
-	napi_enable(&priv->napi);
 	ipoib_ib_dev_stop(dev);
 	return -1;
 }
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_main.c b/drivers/infiniband/ulp/ipoib/ipoib_main.c
index a115c0b7a310..6e86eeee370e 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_main.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_main.c
@@ -681,7 +681,7 @@ static void push_pseudo_header(struct sk_buff *skb, const char *daddr)
 {
 	struct ipoib_pseudo_header *phdr;
 
-	phdr = (struct ipoib_pseudo_header *)skb_push(skb, sizeof(*phdr));
+	phdr = skb_push(skb, sizeof(*phdr));
 	memcpy(phdr->hwaddr, daddr, INFINIBAND_ALEN);
 }
 
@@ -1129,7 +1129,7 @@ static int ipoib_hard_header(struct sk_buff *skb,
 {
 	struct ipoib_header *header;
 
-	header = (struct ipoib_header *) skb_push(skb, sizeof *header);
+	header = skb_push(skb, sizeof *header);
 
 	header->proto = htons(type);
 	header->reserved = 0;
@@ -1596,6 +1596,8 @@ static void ipoib_dev_uninit_default(struct net_device *dev)
 
 	ipoib_transport_dev_cleanup(dev);
 
+	netif_napi_del(&priv->napi);
+
 	ipoib_cm_dev_cleanup(dev);
 
 	kfree(priv->rx_ring);
@@ -1649,6 +1651,7 @@ out_rx_ring_cleanup:
 	kfree(priv->rx_ring);
 
 out:
+	netif_napi_del(&priv->napi);
 	return -ENOMEM;
 }
 
@@ -1890,6 +1893,7 @@ static struct net_device
 	rn->send = ipoib_send;
 	rn->attach_mcast = ipoib_mcast_attach;
 	rn->detach_mcast = ipoib_mcast_detach;
+	rn->free_rdma_netdev = free_netdev;
 	rn->hca = hca;
 
 	dev->netdev_ops = &ipoib_netdev_default_pf;
@@ -2237,6 +2241,7 @@ event_failed:
 
 device_init_failed:
 	free_netdev(priv->dev);
+	kfree(priv);
 
 alloc_mem_failed:
 	return ERR_PTR(result);
@@ -2277,13 +2282,15 @@ static void ipoib_add_one(struct ib_device *device)
 
 static void ipoib_remove_one(struct ib_device *device, void *client_data)
 {
-	struct ipoib_dev_priv *priv, *tmp;
+	struct ipoib_dev_priv *priv, *tmp, *cpriv, *tcpriv;
 	struct list_head *dev_list = client_data;
 
 	if (!dev_list)
 		return;
 
 	list_for_each_entry_safe(priv, tmp, dev_list, list) {
+		struct rdma_netdev *rn = netdev_priv(priv->dev);
+
 		ib_unregister_event_handler(&priv->event_handler);
 		flush_workqueue(ipoib_workqueue);
 
@@ -2300,7 +2307,11 @@ static void ipoib_remove_one(struct ib_device *device, void *client_data)
 		flush_workqueue(priv->wq);
 
 		unregister_netdev(priv->dev);
-		free_netdev(priv->dev);
+		rn->free_rdma_netdev(priv->dev);
+
+		list_for_each_entry_safe(cpriv, tcpriv, &priv->child_intfs, list)
+			kfree(cpriv);
+
 		kfree(priv);
 	}
 
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_netlink.c b/drivers/infiniband/ulp/ipoib/ipoib_netlink.c
index 28884781311b..3e44087935ae 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_netlink.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_netlink.c
@@ -64,8 +64,9 @@ nla_put_failure:
 	return -EMSGSIZE;
 }
 
-static int ipoib_changelink(struct net_device *dev,
-			    struct nlattr *tb[], struct nlattr *data[])
+static int ipoib_changelink(struct net_device *dev, struct nlattr *tb[],
+			    struct nlattr *data[],
+			    struct netlink_ext_ack *extack)
 {
 	u16 mode, umcast;
 	int ret = 0;
@@ -93,7 +94,8 @@ out_err:
 }
 
 static int ipoib_new_child_link(struct net *src_net, struct net_device *dev,
-			       struct nlattr *tb[], struct nlattr *data[])
+				struct nlattr *tb[], struct nlattr *data[],
+				struct netlink_ext_ack *extack)
 {
 	struct net_device *pdev;
 	struct ipoib_dev_priv *ppriv;
@@ -133,7 +135,7 @@ static int ipoib_new_child_link(struct net *src_net, struct net_device *dev,
 			       child_pkey, IPOIB_RTNL_CHILD);
 
 	if (!err && data)
-		err = ipoib_changelink(dev, tb, data);
+		err = ipoib_changelink(dev, tb, data, extack);
 	return err;
 }
 
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_vlan.c b/drivers/infiniband/ulp/ipoib/ipoib_vlan.c
index 36dc4fcaa3cd..081b33deff1b 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_vlan.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_vlan.c
@@ -133,13 +133,13 @@ int ipoib_vlan_add(struct net_device *pdev, unsigned short pkey)
 	snprintf(intf_name, sizeof intf_name, "%s.%04x",
 		 ppriv->dev->name, pkey);
 
+	if (!rtnl_trylock())
+		return restart_syscall();
+
 	priv = ipoib_intf_alloc(ppriv->ca, ppriv->port, intf_name);
 	if (!priv)
 		return -ENOMEM;
 
-	if (!rtnl_trylock())
-		return restart_syscall();
-
 	down_write(&ppriv->vlan_rwsem);
 
 	/*
@@ -167,8 +167,10 @@ out:
 
 	rtnl_unlock();
 
-	if (result)
+	if (result) {
 		free_netdev(priv->dev);
+		kfree(priv);
+	}
 
 	return result;
 }
@@ -209,6 +211,7 @@ int ipoib_vlan_delete(struct net_device *pdev, unsigned short pkey)
 
 	if (dev) {
 		free_netdev(dev);
+		kfree(priv);
 		return 0;
 	}
 
diff --git a/drivers/infiniband/ulp/opa_vnic/opa_vnic_encap.c b/drivers/infiniband/ulp/opa_vnic/opa_vnic_encap.c
index 2e8fee982436..afa938bd26d6 100644
--- a/drivers/infiniband/ulp/opa_vnic/opa_vnic_encap.c
+++ b/drivers/infiniband/ulp/opa_vnic/opa_vnic_encap.c
@@ -460,7 +460,7 @@ void opa_vnic_encap_skb(struct opa_vnic_adapter *adapter, struct sk_buff *skb)
 	sc = opa_vnic_get_sc(info, skb);
 	l4_hdr = info->vesw.vesw_id;
 
-	mdata = (struct opa_vnic_skb_mdata *)skb_push(skb, sizeof(*mdata));
+	mdata = skb_push(skb, sizeof(*mdata));
 	mdata->vl = opa_vnic_get_vl(adapter, skb);
 	mdata->entropy = entropy;
 	mdata->flags = 0;
diff --git a/drivers/infiniband/ulp/opa_vnic/opa_vnic_ethtool.c b/drivers/infiniband/ulp/opa_vnic/opa_vnic_ethtool.c
index d66540e24885..62390e9e0023 100644
--- a/drivers/infiniband/ulp/opa_vnic/opa_vnic_ethtool.c
+++ b/drivers/infiniband/ulp/opa_vnic/opa_vnic_ethtool.c
@@ -146,15 +146,15 @@ static void vnic_get_ethtool_stats(struct net_device *netdev,
 	int i;
 
 	memset(&vstats, 0, sizeof(vstats));
-	mutex_lock(&adapter->stats_lock);
+	spin_lock(&adapter->stats_lock);
 	adapter->rn_ops->ndo_get_stats64(netdev, &vstats.netstats);
+	spin_unlock(&adapter->stats_lock);
 	for (i = 0; i < VNIC_STATS_LEN; i++) {
 		char *p = (char *)&vstats + vnic_gstrings_stats[i].stat_offset;
 
 		data[i] = (vnic_gstrings_stats[i].sizeof_stat ==
 			   sizeof(u64)) ? *(u64 *)p : *(u32 *)p;
 	}
-	mutex_unlock(&adapter->stats_lock);
 }
 
 /* vnic_get_strings - get strings */
diff --git a/drivers/infiniband/ulp/opa_vnic/opa_vnic_internal.h b/drivers/infiniband/ulp/opa_vnic/opa_vnic_internal.h
index 6bba886bec1f..ca29e6d5aedc 100644
--- a/drivers/infiniband/ulp/opa_vnic/opa_vnic_internal.h
+++ b/drivers/infiniband/ulp/opa_vnic/opa_vnic_internal.h
@@ -214,7 +214,7 @@ struct opa_vnic_adapter {
 	struct mutex mactbl_lock;
 
 	/* Lock used to protect access to vnic counters */
-	struct mutex stats_lock;
+	spinlock_t stats_lock;
 
 	u8 flow_tbl[OPA_VNIC_FLOW_TBL_SIZE];
 
diff --git a/drivers/infiniband/ulp/opa_vnic/opa_vnic_netdev.c b/drivers/infiniband/ulp/opa_vnic/opa_vnic_netdev.c
index 905f39dda5aa..1a3c25364b64 100644
--- a/drivers/infiniband/ulp/opa_vnic/opa_vnic_netdev.c
+++ b/drivers/infiniband/ulp/opa_vnic/opa_vnic_netdev.c
@@ -69,9 +69,9 @@ static void opa_vnic_get_stats64(struct net_device *netdev,
 	struct opa_vnic_stats vstats;
 
 	memset(&vstats, 0, sizeof(vstats));
-	mutex_lock(&adapter->stats_lock);
+	spin_lock(&adapter->stats_lock);
 	adapter->rn_ops->ndo_get_stats64(netdev, &vstats.netstats);
-	mutex_unlock(&adapter->stats_lock);
+	spin_unlock(&adapter->stats_lock);
 	memcpy(stats, &vstats.netstats, sizeof(*stats));
 }
 
@@ -103,7 +103,7 @@ static u16 opa_vnic_select_queue(struct net_device *netdev, struct sk_buff *skb,
 	int rc;
 
 	/* pass entropy and vl as metadata in skb */
-	mdata = (struct opa_vnic_skb_mdata *)skb_push(skb, sizeof(*mdata));
+	mdata = skb_push(skb, sizeof(*mdata));
 	mdata->entropy =  opa_vnic_calc_entropy(adapter, skb);
 	mdata->vl = opa_vnic_get_vl(adapter, skb);
 	rc = adapter->rn_ops->ndo_select_queue(netdev, skb,
@@ -323,13 +323,13 @@ struct opa_vnic_adapter *opa_vnic_add_netdev(struct ib_device *ibdev,
 	else if (IS_ERR(netdev))
 		return ERR_CAST(netdev);
 
+	rn = netdev_priv(netdev);
 	adapter = kzalloc(sizeof(*adapter), GFP_KERNEL);
 	if (!adapter) {
 		rc = -ENOMEM;
 		goto adapter_err;
 	}
 
-	rn = netdev_priv(netdev);
 	rn->clnt_priv = adapter;
 	rn->hca = ibdev;
 	rn->port_num = port_num;
@@ -344,7 +344,7 @@ struct opa_vnic_adapter *opa_vnic_add_netdev(struct ib_device *ibdev,
 	netdev->hard_header_len += OPA_VNIC_SKB_HEADROOM;
 	mutex_init(&adapter->lock);
 	mutex_init(&adapter->mactbl_lock);
-	mutex_init(&adapter->stats_lock);
+	spin_lock_init(&adapter->stats_lock);
 
 	SET_NETDEV_DEV(netdev, ibdev->dev.parent);
 
@@ -364,10 +364,9 @@ struct opa_vnic_adapter *opa_vnic_add_netdev(struct ib_device *ibdev,
 netdev_err:
 	mutex_destroy(&adapter->lock);
 	mutex_destroy(&adapter->mactbl_lock);
-	mutex_destroy(&adapter->stats_lock);
 	kfree(adapter);
 adapter_err:
-	ibdev->free_rdma_netdev(netdev);
+	rn->free_rdma_netdev(netdev);
 
 	return ERR_PTR(rc);
 }
@@ -376,14 +375,13 @@ adapter_err:
 void opa_vnic_rem_netdev(struct opa_vnic_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
-	struct ib_device *ibdev = adapter->ibdev;
+	struct rdma_netdev *rn = netdev_priv(netdev);
 
 	v_info("removing\n");
 	unregister_netdev(netdev);
 	opa_vnic_release_mac_tbl(adapter);
 	mutex_destroy(&adapter->lock);
 	mutex_destroy(&adapter->mactbl_lock);
-	mutex_destroy(&adapter->stats_lock);
 	kfree(adapter);
-	ibdev->free_rdma_netdev(netdev);
+	rn->free_rdma_netdev(netdev);
 }
diff --git a/drivers/infiniband/ulp/opa_vnic/opa_vnic_vema.c b/drivers/infiniband/ulp/opa_vnic/opa_vnic_vema.c
index 875694f9a7f9..cf768dd78d1b 100644
--- a/drivers/infiniband/ulp/opa_vnic/opa_vnic_vema.c
+++ b/drivers/infiniband/ulp/opa_vnic/opa_vnic_vema.c
@@ -794,7 +794,7 @@ void opa_vnic_vema_send_trap(struct opa_vnic_adapter *adapter,
 
 	send_buf = ib_create_send_mad(port->mad_agent, 1, pkey_idx, 0,
 				      IB_MGMT_VENDOR_HDR, IB_MGMT_MAD_DATA,
-				      GFP_KERNEL, OPA_MGMT_BASE_VERSION);
+				      GFP_ATOMIC, OPA_MGMT_BASE_VERSION);
 	if (IS_ERR(send_buf)) {
 		c_err("%s:Couldn't allocate send buf\n", __func__);
 		goto err_sndbuf;
diff --git a/drivers/infiniband/ulp/opa_vnic/opa_vnic_vema_iface.c b/drivers/infiniband/ulp/opa_vnic/opa_vnic_vema_iface.c
index a51bf977f4d6..c2733964379c 100644
--- a/drivers/infiniband/ulp/opa_vnic/opa_vnic_vema_iface.c
+++ b/drivers/infiniband/ulp/opa_vnic/opa_vnic_vema_iface.c
@@ -89,9 +89,9 @@ void opa_vnic_get_summary_counters(struct opa_vnic_adapter *adapter,
 	u64 *src;
 
 	memset(&vstats, 0, sizeof(vstats));
-	mutex_lock(&adapter->stats_lock);
+	spin_lock(&adapter->stats_lock);
 	adapter->rn_ops->ndo_get_stats64(adapter->netdev, &vstats.netstats);
-	mutex_unlock(&adapter->stats_lock);
+	spin_unlock(&adapter->stats_lock);
 
 	cntrs->vp_instance = cpu_to_be16(adapter->vport_num);
 	cntrs->vesw_id = cpu_to_be16(adapter->info.vesw.vesw_id);
@@ -128,9 +128,9 @@ void opa_vnic_get_error_counters(struct opa_vnic_adapter *adapter,
 	struct opa_vnic_stats vstats;
 
 	memset(&vstats, 0, sizeof(vstats));
-	mutex_lock(&adapter->stats_lock);
+	spin_lock(&adapter->stats_lock);
 	adapter->rn_ops->ndo_get_stats64(adapter->netdev, &vstats.netstats);
-	mutex_unlock(&adapter->stats_lock);
+	spin_unlock(&adapter->stats_lock);
 
 	cntrs->vp_instance = cpu_to_be16(adapter->vport_num);
 	cntrs->vesw_id = cpu_to_be16(adapter->info.vesw.vesw_id);
diff --git a/drivers/input/misc/soc_button_array.c b/drivers/input/misc/soc_button_array.c
index e37d37273182..f600f3a7a3c6 100644
--- a/drivers/input/misc/soc_button_array.c
+++ b/drivers/input/misc/soc_button_array.c
@@ -248,7 +248,8 @@ static struct soc_button_info *soc_button_get_button_info(struct device *dev)
 
 	if (!btns_desc) {
 		dev_err(dev, "ACPI Button Descriptors not found\n");
-		return ERR_PTR(-ENODEV);
+		button_info = ERR_PTR(-ENODEV);
+		goto out;
 	}
 
 	/* The first package describes the collection */
@@ -264,24 +265,31 @@ static struct soc_button_info *soc_button_get_button_info(struct device *dev)
 	}
 	if (collection_uid == -1) {
 		dev_err(dev, "Invalid Button Collection Descriptor\n");
-		return ERR_PTR(-ENODEV);
+		button_info = ERR_PTR(-ENODEV);
+		goto out;
 	}
 
 	/* There are package.count - 1 buttons + 1 terminating empty entry */
 	button_info = devm_kcalloc(dev, btns_desc->package.count,
 				   sizeof(*button_info), GFP_KERNEL);
-	if (!button_info)
-		return ERR_PTR(-ENOMEM);
+	if (!button_info) {
+		button_info = ERR_PTR(-ENOMEM);
+		goto out;
+	}
 
 	/* Parse the button descriptors */
 	for (i = 1, btn = 0; i < btns_desc->package.count; i++, btn++) {
 		if (soc_button_parse_btn_desc(dev,
 					      &btns_desc->package.elements[i],
 					      collection_uid,
-					      &button_info[btn]))
-			return ERR_PTR(-ENODEV);
+					      &button_info[btn])) {
+			button_info = ERR_PTR(-ENODEV);
+			goto out;
+		}
 	}
 
+out:
+	kfree(buf.pointer);
 	return button_info;
 }
 
diff --git a/drivers/input/rmi4/rmi_f54.c b/drivers/input/rmi4/rmi_f54.c
index dea63e2db3e6..f5206e2c767e 100644
--- a/drivers/input/rmi4/rmi_f54.c
+++ b/drivers/input/rmi4/rmi_f54.c
@@ -31,9 +31,6 @@
 #define F54_GET_REPORT          1
 #define F54_FORCE_CAL           2
 
-/* Fixed sizes of reports */
-#define F54_QUERY_LEN			27
-
 /* F54 capabilities */
 #define F54_CAP_BASELINE	(1 << 2)
 #define F54_CAP_IMAGE8		(1 << 3)
@@ -95,7 +92,6 @@ struct rmi_f54_reports {
 struct f54_data {
 	struct rmi_function *fn;
 
-	u8 qry[F54_QUERY_LEN];
 	u8 num_rx_electrodes;
 	u8 num_tx_electrodes;
 	u8 capabilities;
@@ -632,22 +628,23 @@ static int rmi_f54_detect(struct rmi_function *fn)
 {
 	int error;
 	struct f54_data *f54;
+	u8 buf[6];
 
 	f54 = dev_get_drvdata(&fn->dev);
 
 	error = rmi_read_block(fn->rmi_dev, fn->fd.query_base_addr,
-			       &f54->qry, sizeof(f54->qry));
+			       buf, sizeof(buf));
 	if (error) {
 		dev_err(&fn->dev, "%s: Failed to query F54 properties\n",
 			__func__);
 		return error;
 	}
 
-	f54->num_rx_electrodes = f54->qry[0];
-	f54->num_tx_electrodes = f54->qry[1];
-	f54->capabilities = f54->qry[2];
-	f54->clock_rate = f54->qry[3] | (f54->qry[4] << 8);
-	f54->family = f54->qry[5];
+	f54->num_rx_electrodes = buf[0];
+	f54->num_tx_electrodes = buf[1];
+	f54->capabilities = buf[2];
+	f54->clock_rate = buf[3] | (buf[4] << 8);
+	f54->family = buf[5];
 
 	rmi_dbg(RMI_DEBUG_FN, &fn->dev, "F54 num_rx_electrodes: %d\n",
 		f54->num_rx_electrodes);
diff --git a/drivers/input/serio/i8042-x86ia64io.h b/drivers/input/serio/i8042-x86ia64io.h
index 09720d950686..f932a83b4990 100644
--- a/drivers/input/serio/i8042-x86ia64io.h
+++ b/drivers/input/serio/i8042-x86ia64io.h
@@ -723,6 +723,13 @@ static const struct dmi_system_id __initconst i8042_dmi_notimeout_table[] = {
 			DMI_MATCH(DMI_PRODUCT_NAME, "LIFEBOOK U574"),
 		},
 	},
+	{
+		/* Fujitsu UH554 laptop */
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "LIFEBOOK UH544"),
+		},
+	},
 	{ }
 };
 
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index 63cacf5d6cf2..5c9759ed22ca 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -3879,11 +3879,9 @@ static void irte_ga_prepare(void *entry,
 			    u8 vector, u32 dest_apicid, int devid)
 {
 	struct irte_ga *irte = (struct irte_ga *) entry;
-	struct iommu_dev_data *dev_data = search_dev_data(devid);
 
 	irte->lo.val                      = 0;
 	irte->hi.val                      = 0;
-	irte->lo.fields_remap.guest_mode  = dev_data ? dev_data->use_vapic : 0;
 	irte->lo.fields_remap.int_type    = delivery_mode;
 	irte->lo.fields_remap.dm          = dest_mode;
 	irte->hi.fields.vector            = vector;
@@ -3939,10 +3937,10 @@ static void irte_ga_set_affinity(void *entry, u16 devid, u16 index,
 	struct irte_ga *irte = (struct irte_ga *) entry;
 	struct iommu_dev_data *dev_data = search_dev_data(devid);
 
-	if (!dev_data || !dev_data->use_vapic) {
+	if (!dev_data || !dev_data->use_vapic ||
+	    !irte->lo.fields_remap.guest_mode) {
 		irte->hi.fields.vector = vector;
 		irte->lo.fields_remap.destination = dest_apicid;
-		irte->lo.fields_remap.guest_mode = 0;
 		modify_irte_ga(devid, index, irte, NULL);
 	}
 }
@@ -4386,21 +4384,29 @@ static void ir_compose_msi_msg(struct irq_data *irq_data, struct msi_msg *msg)
 }
 
 static struct irq_chip amd_ir_chip = {
-	.irq_ack = ir_ack_apic_edge,
-	.irq_set_affinity = amd_ir_set_affinity,
-	.irq_set_vcpu_affinity = amd_ir_set_vcpu_affinity,
-	.irq_compose_msi_msg = ir_compose_msi_msg,
+	.name			= "AMD-IR",
+	.irq_ack		= ir_ack_apic_edge,
+	.irq_set_affinity	= amd_ir_set_affinity,
+	.irq_set_vcpu_affinity	= amd_ir_set_vcpu_affinity,
+	.irq_compose_msi_msg	= ir_compose_msi_msg,
 };
 
 int amd_iommu_create_irq_domain(struct amd_iommu *iommu)
 {
-	iommu->ir_domain = irq_domain_add_tree(NULL, &amd_ir_domain_ops, iommu);
+	struct fwnode_handle *fn;
+
+	fn = irq_domain_alloc_named_id_fwnode("AMD-IR", iommu->index);
+	if (!fn)
+		return -ENOMEM;
+	iommu->ir_domain = irq_domain_create_tree(fn, &amd_ir_domain_ops, iommu);
+	irq_domain_free_fwnode(fn);
 	if (!iommu->ir_domain)
 		return -ENOMEM;
 
 	iommu->ir_domain->parent = arch_get_ir_parent_domain();
-	iommu->msi_domain = arch_create_msi_irq_domain(iommu->ir_domain);
-
+	iommu->msi_domain = arch_create_remap_msi_irq_domain(iommu->ir_domain,
+							     "AMD-IR-MSI",
+							     iommu->index);
 	return 0;
 }
 
diff --git a/drivers/iommu/dmar.c b/drivers/iommu/dmar.c
index cbf7763d8091..c8b0329c85d2 100644
--- a/drivers/iommu/dmar.c
+++ b/drivers/iommu/dmar.c
@@ -1808,10 +1808,9 @@ IOMMU_INIT_POST(detect_intel_iommu);
  * for Directed-IO Architecture Specifiction, Rev 2.2, Section 8.8
  * "Remapping Hardware Unit Hot Plug".
  */
-static u8 dmar_hp_uuid[] = {
-	/* 0000 */    0xA6, 0xA3, 0xC1, 0xD8, 0x9B, 0xBE, 0x9B, 0x4C,
-	/* 0008 */    0x91, 0xBF, 0xC3, 0xCB, 0x81, 0xFC, 0x5D, 0xAF
-};
+static guid_t dmar_hp_guid =
+	GUID_INIT(0xD8C1A3A6, 0xBE9B, 0x4C9B,
+		  0x91, 0xBF, 0xC3, 0xCB, 0x81, 0xFC, 0x5D, 0xAF);
 
 /*
  * Currently there's only one revision and BIOS will not check the revision id,
@@ -1824,7 +1823,7 @@ static u8 dmar_hp_uuid[] = {
 
 static inline bool dmar_detect_dsm(acpi_handle handle, int func)
 {
-	return acpi_check_dsm(handle, dmar_hp_uuid, DMAR_DSM_REV_ID, 1 << func);
+	return acpi_check_dsm(handle, &dmar_hp_guid, DMAR_DSM_REV_ID, 1 << func);
 }
 
 static int dmar_walk_dsm_resource(acpi_handle handle, int func,
@@ -1843,7 +1842,7 @@ static int dmar_walk_dsm_resource(acpi_handle handle, int func,
 	if (!dmar_detect_dsm(handle, func))
 		return 0;
 
-	obj = acpi_evaluate_dsm_typed(handle, dmar_hp_uuid, DMAR_DSM_REV_ID,
+	obj = acpi_evaluate_dsm_typed(handle, &dmar_hp_guid, DMAR_DSM_REV_ID,
 				      func, NULL, ACPI_TYPE_BUFFER);
 	if (!obj)
 		return -ENODEV;
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index fc2765ccdb57..8500deda9175 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -4315,7 +4315,7 @@ int dmar_parse_one_atsr(struct acpi_dmar_header *hdr, void *arg)
 	struct acpi_dmar_atsr *atsr;
 	struct dmar_atsr_unit *atsru;
 
-	if (system_state != SYSTEM_BOOTING && !intel_iommu_enabled)
+	if (system_state >= SYSTEM_RUNNING && !intel_iommu_enabled)
 		return 0;
 
 	atsr = container_of(hdr, struct acpi_dmar_atsr, header);
@@ -4565,7 +4565,7 @@ int dmar_iommu_notify_scope_dev(struct dmar_pci_notify_info *info)
 	struct acpi_dmar_atsr *atsr;
 	struct acpi_dmar_reserved_memory *rmrr;
 
-	if (!intel_iommu_enabled && system_state != SYSTEM_BOOTING)
+	if (!intel_iommu_enabled && system_state >= SYSTEM_RUNNING)
 		return 0;
 
 	list_for_each_entry(rmrru, &dmar_rmrr_units, list) {
diff --git a/drivers/iommu/intel_irq_remapping.c b/drivers/iommu/intel_irq_remapping.c
index a190cbd76ef7..8fc641ea2e41 100644
--- a/drivers/iommu/intel_irq_remapping.c
+++ b/drivers/iommu/intel_irq_remapping.c
@@ -500,8 +500,9 @@ static void iommu_enable_irq_remapping(struct intel_iommu *iommu)
 static int intel_setup_irq_remapping(struct intel_iommu *iommu)
 {
 	struct ir_table *ir_table;
-	struct page *pages;
+	struct fwnode_handle *fn;
 	unsigned long *bitmap;
+	struct page *pages;
 
 	if (iommu->ir_table)
 		return 0;
@@ -525,15 +526,24 @@ static int intel_setup_irq_remapping(struct intel_iommu *iommu)
 		goto out_free_pages;
 	}
 
-	iommu->ir_domain = irq_domain_add_hierarchy(arch_get_ir_parent_domain(),
-						    0, INTR_REMAP_TABLE_ENTRIES,
-						    NULL, &intel_ir_domain_ops,
-						    iommu);
+	fn = irq_domain_alloc_named_id_fwnode("INTEL-IR", iommu->seq_id);
+	if (!fn)
+		goto out_free_bitmap;
+
+	iommu->ir_domain =
+		irq_domain_create_hierarchy(arch_get_ir_parent_domain(),
+					    0, INTR_REMAP_TABLE_ENTRIES,
+					    fn, &intel_ir_domain_ops,
+					    iommu);
+	irq_domain_free_fwnode(fn);
 	if (!iommu->ir_domain) {
 		pr_err("IR%d: failed to allocate irqdomain\n", iommu->seq_id);
 		goto out_free_bitmap;
 	}
-	iommu->ir_msi_domain = arch_create_msi_irq_domain(iommu->ir_domain);
+	iommu->ir_msi_domain =
+		arch_create_remap_msi_irq_domain(iommu->ir_domain,
+						 "INTEL-IR-MSI",
+						 iommu->seq_id);
 
 	ir_table->base = page_address(pages);
 	ir_table->bitmap = bitmap;
@@ -1205,10 +1215,11 @@ static int intel_ir_set_vcpu_affinity(struct irq_data *data, void *info)
 }
 
 static struct irq_chip intel_ir_chip = {
-	.irq_ack = ir_ack_apic_edge,
-	.irq_set_affinity = intel_ir_set_affinity,
-	.irq_compose_msi_msg = intel_ir_compose_msi_msg,
-	.irq_set_vcpu_affinity = intel_ir_set_vcpu_affinity,
+	.name			= "INTEL-IR",
+	.irq_ack		= ir_ack_apic_edge,
+	.irq_set_affinity	= intel_ir_set_affinity,
+	.irq_compose_msi_msg	= intel_ir_compose_msi_msg,
+	.irq_set_vcpu_affinity	= intel_ir_set_vcpu_affinity,
 };
 
 static void intel_irq_remapping_prepare_irte(struct intel_ir_data *data,
diff --git a/drivers/iommu/of_iommu.c b/drivers/iommu/of_iommu.c
index 19779b88a479..8cb60829a7a1 100644
--- a/drivers/iommu/of_iommu.c
+++ b/drivers/iommu/of_iommu.c
@@ -103,7 +103,7 @@ static bool of_iommu_driver_present(struct device_node *np)
 	 * it never will be. We don't want to defer indefinitely, nor attempt
 	 * to dereference __iommu_of_table after it's been freed.
 	 */
-	if (system_state > SYSTEM_BOOTING)
+	if (system_state >= SYSTEM_RUNNING)
 		return false;
 
 	return of_match_node(&__iommu_of_table, np);
diff --git a/drivers/ipack/ipack.c b/drivers/ipack/ipack.c
index 12102448fddd..a1e07a77d4e6 100644
--- a/drivers/ipack/ipack.c
+++ b/drivers/ipack/ipack.c
@@ -212,7 +212,7 @@ struct ipack_bus_device *ipack_bus_register(struct device *parent, int slots,
 	int bus_nr;
 	struct ipack_bus_device *bus;
 
-	bus = kzalloc(sizeof(struct ipack_bus_device), GFP_KERNEL);
+	bus = kzalloc(sizeof(*bus), GFP_KERNEL);
 	if (!bus)
 		return NULL;
 
@@ -402,7 +402,6 @@ static int ipack_device_read_id(struct ipack_device *dev)
 	 * ID ROM contents */
 	dev->id = kmalloc(dev->id_avail, GFP_KERNEL);
 	if (!dev->id) {
-		dev_err(&dev->dev, "dev->id alloc failed.\n");
 		ret = -ENOMEM;
 		goto out;
 	}
diff --git a/drivers/irqchip/Kconfig b/drivers/irqchip/Kconfig
index 478f8ace2664..f1fd5f44d1d4 100644
--- a/drivers/irqchip/Kconfig
+++ b/drivers/irqchip/Kconfig
@@ -39,7 +39,6 @@ config ARM_GIC_V3_ITS
 	bool
 	depends on PCI
 	depends on PCI_MSI
-	select ACPI_IORT if ACPI
 
 config ARM_NVIC
 	bool
@@ -268,6 +267,12 @@ config IRQ_MXS
 	select IRQ_DOMAIN
 	select STMP_DEVICE
 
+config MVEBU_GICP
+	bool
+
+config MVEBU_ICU
+	bool
+
 config MVEBU_ODMI
 	bool
 	select GENERIC_MSI_IRQ_DOMAIN
diff --git a/drivers/irqchip/Makefile b/drivers/irqchip/Makefile
index b64c59b838a0..e88d856cc09c 100644
--- a/drivers/irqchip/Makefile
+++ b/drivers/irqchip/Makefile
@@ -69,10 +69,12 @@ obj-$(CONFIG_ARCH_SA1100)		+= irq-sa11x0.o
 obj-$(CONFIG_INGENIC_IRQ)		+= irq-ingenic.o
 obj-$(CONFIG_IMX_GPCV2)			+= irq-imx-gpcv2.o
 obj-$(CONFIG_PIC32_EVIC)		+= irq-pic32-evic.o
+obj-$(CONFIG_MVEBU_GICP)		+= irq-mvebu-gicp.o
+obj-$(CONFIG_MVEBU_ICU)			+= irq-mvebu-icu.o
 obj-$(CONFIG_MVEBU_ODMI)		+= irq-mvebu-odmi.o
 obj-$(CONFIG_MVEBU_PIC)			+= irq-mvebu-pic.o
 obj-$(CONFIG_LS_SCFG_MSI)		+= irq-ls-scfg-msi.o
 obj-$(CONFIG_EZNPS_GIC)			+= irq-eznps.o
-obj-$(CONFIG_ARCH_ASPEED)		+= irq-aspeed-vic.o
+obj-$(CONFIG_ARCH_ASPEED)		+= irq-aspeed-vic.o irq-aspeed-i2c-ic.o
 obj-$(CONFIG_STM32_EXTI) 		+= irq-stm32-exti.o
 obj-$(CONFIG_QCOM_IRQ_COMBINER)		+= qcom-irq-combiner.o
diff --git a/drivers/irqchip/irq-armada-370-xp.c b/drivers/irqchip/irq-armada-370-xp.c
index eb0d4d41b156..b207b2c3aa55 100644
--- a/drivers/irqchip/irq-armada-370-xp.c
+++ b/drivers/irqchip/irq-armada-370-xp.c
@@ -34,25 +34,104 @@
 #include <asm/smp_plat.h>
 #include <asm/mach/irq.h>
 
-/* Interrupt Controller Registers Map */
-#define ARMADA_370_XP_INT_SET_MASK_OFFS		(0x48)
-#define ARMADA_370_XP_INT_CLEAR_MASK_OFFS	(0x4C)
-#define ARMADA_370_XP_INT_FABRIC_MASK_OFFS	(0x54)
-#define ARMADA_370_XP_INT_CAUSE_PERF(cpu)	(1 << cpu)
+/*
+ * Overall diagram of the Armada XP interrupt controller:
+ *
+ *    To CPU 0                 To CPU 1
+ *
+ *       /\                       /\
+ *       ||                       ||
+ * +---------------+     +---------------+
+ * |               |	 |               |
+ * |    per-CPU    |	 |    per-CPU    |
+ * |  mask/unmask  |	 |  mask/unmask  |
+ * |     CPU0      |	 |     CPU1      |
+ * |               |	 |               |
+ * +---------------+	 +---------------+
+ *        /\                       /\
+ *        ||                       ||
+ *        \\_______________________//
+ *                     ||
+ *            +-------------------+
+ *            |                   |
+ *            | Global interrupt  |
+ *            |    mask/unmask    |
+ *            |                   |
+ *            +-------------------+
+ *                     /\
+ *                     ||
+ *               interrupt from
+ *                   device
+ *
+ * The "global interrupt mask/unmask" is modified using the
+ * ARMADA_370_XP_INT_SET_ENABLE_OFFS and
+ * ARMADA_370_XP_INT_CLEAR_ENABLE_OFFS registers, which are relative
+ * to "main_int_base".
+ *
+ * The "per-CPU mask/unmask" is modified using the
+ * ARMADA_370_XP_INT_SET_MASK_OFFS and
+ * ARMADA_370_XP_INT_CLEAR_MASK_OFFS registers, which are relative to
+ * "per_cpu_int_base". This base address points to a special address,
+ * which automatically accesses the registers of the current CPU.
+ *
+ * The per-CPU mask/unmask can also be adjusted using the global
+ * per-interrupt ARMADA_370_XP_INT_SOURCE_CTL register, which we use
+ * to configure interrupt affinity.
+ *
+ * Due to this model, all interrupts need to be mask/unmasked at two
+ * different levels: at the global level and at the per-CPU level.
+ *
+ * This driver takes the following approach to deal with this:
+ *
+ *  - For global interrupts:
+ *
+ *    At ->map() time, a global interrupt is unmasked at the per-CPU
+ *    mask/unmask level. It is therefore unmasked at this level for
+ *    the current CPU, running the ->map() code. This allows to have
+ *    the interrupt unmasked at this level in non-SMP
+ *    configurations. In SMP configurations, the ->set_affinity()
+ *    callback is called, which using the
+ *    ARMADA_370_XP_INT_SOURCE_CTL() readjusts the per-CPU mask/unmask
+ *    for the interrupt.
+ *
+ *    The ->mask() and ->unmask() operations only mask/unmask the
+ *    interrupt at the "global" level.
+ *
+ *    So, a global interrupt is enabled at the per-CPU level as soon
+ *    as it is mapped. At run time, the masking/unmasking takes place
+ *    at the global level.
+ *
+ *  - For per-CPU interrupts
+ *
+ *    At ->map() time, a per-CPU interrupt is unmasked at the global
+ *    mask/unmask level.
+ *
+ *    The ->mask() and ->unmask() operations mask/unmask the interrupt
+ *    at the per-CPU level.
+ *
+ *    So, a per-CPU interrupt is enabled at the global level as soon
+ *    as it is mapped. At run time, the masking/unmasking takes place
+ *    at the per-CPU level.
+ */
 
+/* Registers relative to main_int_base */
 #define ARMADA_370_XP_INT_CONTROL		(0x00)
+#define ARMADA_370_XP_SW_TRIG_INT_OFFS		(0x04)
 #define ARMADA_370_XP_INT_SET_ENABLE_OFFS	(0x30)
 #define ARMADA_370_XP_INT_CLEAR_ENABLE_OFFS	(0x34)
 #define ARMADA_370_XP_INT_SOURCE_CTL(irq)	(0x100 + irq*4)
 #define ARMADA_370_XP_INT_SOURCE_CPU_MASK	0xF
 #define ARMADA_370_XP_INT_IRQ_FIQ_MASK(cpuid)	((BIT(0) | BIT(8)) << cpuid)
 
-#define ARMADA_370_XP_CPU_INTACK_OFFS		(0x44)
+/* Registers relative to per_cpu_int_base */
+#define ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS	(0x08)
+#define ARMADA_370_XP_IN_DRBEL_MSK_OFFS		(0x0c)
 #define ARMADA_375_PPI_CAUSE			(0x10)
-
-#define ARMADA_370_XP_SW_TRIG_INT_OFFS           (0x4)
-#define ARMADA_370_XP_IN_DRBEL_MSK_OFFS          (0xc)
-#define ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS        (0x8)
+#define ARMADA_370_XP_CPU_INTACK_OFFS		(0x44)
+#define ARMADA_370_XP_INT_SET_MASK_OFFS		(0x48)
+#define ARMADA_370_XP_INT_CLEAR_MASK_OFFS	(0x4C)
+#define ARMADA_370_XP_INT_FABRIC_MASK_OFFS	(0x54)
+#define ARMADA_370_XP_INT_CAUSE_PERF(cpu)	(1 << cpu)
 
 #define ARMADA_370_XP_MAX_PER_CPU_IRQS		(28)
 
@@ -281,13 +360,11 @@ static int armada_370_xp_mpic_irq_map(struct irq_domain *h,
 		irq_set_percpu_devid(virq);
 		irq_set_chip_and_handler(virq, &armada_370_xp_irq_chip,
 					handle_percpu_devid_irq);
-
 	} else {
 		irq_set_chip_and_handler(virq, &armada_370_xp_irq_chip,
 					handle_level_irq);
 	}
 	irq_set_probe(virq);
-	irq_clear_status_flags(virq, IRQ_NOAUTOEN);
 
 	return 0;
 }
@@ -345,16 +422,40 @@ static void armada_mpic_send_doorbell(const struct cpumask *mask,
 		ARMADA_370_XP_SW_TRIG_INT_OFFS);
 }
 
+static void armada_xp_mpic_reenable_percpu(void)
+{
+	unsigned int irq;
+
+	/* Re-enable per-CPU interrupts that were enabled before suspend */
+	for (irq = 0; irq < ARMADA_370_XP_MAX_PER_CPU_IRQS; irq++) {
+		struct irq_data *data;
+		int virq;
+
+		virq = irq_linear_revmap(armada_370_xp_mpic_domain, irq);
+		if (virq == 0)
+			continue;
+
+		data = irq_get_irq_data(virq);
+
+		if (!irq_percpu_is_enabled(virq))
+			continue;
+
+		armada_370_xp_irq_unmask(data);
+	}
+}
+
 static int armada_xp_mpic_starting_cpu(unsigned int cpu)
 {
 	armada_xp_mpic_perf_init();
 	armada_xp_mpic_smp_cpu_init();
+	armada_xp_mpic_reenable_percpu();
 	return 0;
 }
 
 static int mpic_cascaded_starting_cpu(unsigned int cpu)
 {
 	armada_xp_mpic_perf_init();
+	armada_xp_mpic_reenable_percpu();
 	enable_percpu_irq(parent_irq, IRQ_TYPE_NONE);
 	return 0;
 }
@@ -502,16 +603,27 @@ static void armada_370_xp_mpic_resume(void)
 		if (virq == 0)
 			continue;
 
-		if (!is_percpu_irq(irq))
+		data = irq_get_irq_data(virq);
+
+		if (!is_percpu_irq(irq)) {
+			/* Non per-CPU interrupts */
 			writel(irq, per_cpu_int_base +
 			       ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
-		else
+			if (!irqd_irq_disabled(data))
+				armada_370_xp_irq_unmask(data);
+		} else {
+			/* Per-CPU interrupts */
 			writel(irq, main_int_base +
 			       ARMADA_370_XP_INT_SET_ENABLE_OFFS);
 
-		data = irq_get_irq_data(virq);
-		if (!irqd_irq_disabled(data))
-			armada_370_xp_irq_unmask(data);
+			/*
+			 * Re-enable on the current CPU,
+			 * armada_xp_mpic_reenable_percpu() will take
+			 * care of secondary CPUs when they come up.
+			 */
+			if (irq_percpu_is_enabled(virq))
+				armada_370_xp_irq_unmask(data);
+		}
 	}
 
 	/* Reconfigure doorbells for IPIs and MSIs */
@@ -563,7 +675,7 @@ static int __init armada_370_xp_mpic_of_init(struct device_node *node,
 		irq_domain_add_linear(node, nr_irqs,
 				&armada_370_xp_mpic_irq_ops, NULL);
 	BUG_ON(!armada_370_xp_mpic_domain);
-	armada_370_xp_mpic_domain->bus_token = DOMAIN_BUS_WIRED;
+	irq_domain_update_bus_token(armada_370_xp_mpic_domain, DOMAIN_BUS_WIRED);
 
 	/* Setup for the boot CPU */
 	armada_xp_mpic_perf_init();
diff --git a/drivers/irqchip/irq-aspeed-i2c-ic.c b/drivers/irqchip/irq-aspeed-i2c-ic.c
new file mode 100644
index 000000000000..815b88dd18f2
--- /dev/null
+++ b/drivers/irqchip/irq-aspeed-i2c-ic.c
@@ -0,0 +1,115 @@
+/*
+ *  Aspeed 24XX/25XX I2C Interrupt Controller.
+ *
+ *  Copyright (C) 2012-2017 ASPEED Technology Inc.
+ *  Copyright 2017 IBM Corporation
+ *  Copyright 2017 Google, Inc.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ */
+
+#include <linux/irq.h>
+#include <linux/irqchip.h>
+#include <linux/irqchip/chained_irq.h>
+#include <linux/irqdomain.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/io.h>
+
+
+#define ASPEED_I2C_IC_NUM_BUS 14
+
+struct aspeed_i2c_ic {
+	void __iomem		*base;
+	int			parent_irq;
+	struct irq_domain	*irq_domain;
+};
+
+/*
+ * The aspeed chip provides a single hardware interrupt for all of the I2C
+ * busses, so we use a dummy interrupt chip to translate this single interrupt
+ * into multiple interrupts, each associated with a single I2C bus.
+ */
+static void aspeed_i2c_ic_irq_handler(struct irq_desc *desc)
+{
+	struct aspeed_i2c_ic *i2c_ic = irq_desc_get_handler_data(desc);
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+	unsigned long bit, status;
+	unsigned int bus_irq;
+
+	chained_irq_enter(chip, desc);
+	status = readl(i2c_ic->base);
+	for_each_set_bit(bit, &status, ASPEED_I2C_IC_NUM_BUS) {
+		bus_irq = irq_find_mapping(i2c_ic->irq_domain, bit);
+		generic_handle_irq(bus_irq);
+	}
+	chained_irq_exit(chip, desc);
+}
+
+/*
+ * Set simple handler and mark IRQ as valid. Nothing interesting to do here
+ * since we are using a dummy interrupt chip.
+ */
+static int aspeed_i2c_ic_map_irq_domain(struct irq_domain *domain,
+					unsigned int irq, irq_hw_number_t hwirq)
+{
+	irq_set_chip_and_handler(irq, &dummy_irq_chip, handle_simple_irq);
+	irq_set_chip_data(irq, domain->host_data);
+
+	return 0;
+}
+
+static const struct irq_domain_ops aspeed_i2c_ic_irq_domain_ops = {
+	.map = aspeed_i2c_ic_map_irq_domain,
+};
+
+static int __init aspeed_i2c_ic_of_init(struct device_node *node,
+					struct device_node *parent)
+{
+	struct aspeed_i2c_ic *i2c_ic;
+	int ret = 0;
+
+	i2c_ic = kzalloc(sizeof(*i2c_ic), GFP_KERNEL);
+	if (!i2c_ic)
+		return -ENOMEM;
+
+	i2c_ic->base = of_iomap(node, 0);
+	if (IS_ERR(i2c_ic->base)) {
+		ret = PTR_ERR(i2c_ic->base);
+		goto err_free_ic;
+	}
+
+	i2c_ic->parent_irq = irq_of_parse_and_map(node, 0);
+	if (i2c_ic->parent_irq < 0) {
+		ret = i2c_ic->parent_irq;
+		goto err_iounmap;
+	}
+
+	i2c_ic->irq_domain = irq_domain_add_linear(node, ASPEED_I2C_IC_NUM_BUS,
+						   &aspeed_i2c_ic_irq_domain_ops,
+						   NULL);
+	if (!i2c_ic->irq_domain) {
+		ret = -ENOMEM;
+		goto err_iounmap;
+	}
+
+	i2c_ic->irq_domain->name = "aspeed-i2c-domain";
+
+	irq_set_chained_handler_and_data(i2c_ic->parent_irq,
+					 aspeed_i2c_ic_irq_handler, i2c_ic);
+
+	pr_info("i2c controller registered, irq %d\n", i2c_ic->parent_irq);
+
+	return 0;
+
+err_iounmap:
+	iounmap(i2c_ic->base);
+err_free_ic:
+	kfree(i2c_ic);
+	return ret;
+}
+
+IRQCHIP_DECLARE(ast2400_i2c_ic, "aspeed,ast2400-i2c-ic", aspeed_i2c_ic_of_init);
+IRQCHIP_DECLARE(ast2500_i2c_ic, "aspeed,ast2500-i2c-ic", aspeed_i2c_ic_of_init);
diff --git a/drivers/irqchip/irq-aspeed-vic.c b/drivers/irqchip/irq-aspeed-vic.c
index d24451d5bf8a..03ba477ea0d0 100644
--- a/drivers/irqchip/irq-aspeed-vic.c
+++ b/drivers/irqchip/irq-aspeed-vic.c
@@ -186,7 +186,7 @@ static int avic_map(struct irq_domain *d, unsigned int irq,
 	return 0;
 }
 
-static struct irq_domain_ops avic_dom_ops = {
+static const struct irq_domain_ops avic_dom_ops = {
 	.map = avic_map,
 	.xlate = irq_domain_xlate_onetwocell,
 };
@@ -227,4 +227,5 @@ static int __init avic_of_init(struct device_node *node,
 	return 0;
 }
 
-IRQCHIP_DECLARE(aspeed_new_vic, "aspeed,ast2400-vic", avic_of_init);
+IRQCHIP_DECLARE(ast2400_vic, "aspeed,ast2400-vic", avic_of_init);
+IRQCHIP_DECLARE(ast2500_vic, "aspeed,ast2500-vic", avic_of_init);
diff --git a/drivers/irqchip/irq-gic-v2m.c b/drivers/irqchip/irq-gic-v2m.c
index 863e073c6f7f..993a8426a453 100644
--- a/drivers/irqchip/irq-gic-v2m.c
+++ b/drivers/irqchip/irq-gic-v2m.c
@@ -280,7 +280,7 @@ static int gicv2m_allocate_domains(struct irq_domain *parent)
 		return -ENOMEM;
 	}
 
-	inner_domain->bus_token = DOMAIN_BUS_NEXUS;
+	irq_domain_update_bus_token(inner_domain, DOMAIN_BUS_NEXUS);
 	inner_domain->parent = parent;
 	pci_domain = pci_msi_create_irq_domain(v2m->fwnode,
 					       &gicv2m_msi_domain_info,
diff --git a/drivers/irqchip/irq-gic-v3-its-pci-msi.c b/drivers/irqchip/irq-gic-v3-its-pci-msi.c
index aee1c60d7ab5..77931214d954 100644
--- a/drivers/irqchip/irq-gic-v3-its-pci-msi.c
+++ b/drivers/irqchip/irq-gic-v3-its-pci-msi.c
@@ -41,27 +41,22 @@ static struct irq_chip its_msi_irq_chip = {
 	.irq_write_msi_msg	= pci_msi_domain_write_msg,
 };
 
-struct its_pci_alias {
-	struct pci_dev	*pdev;
-	u32		count;
-};
-
-static int its_pci_msi_vec_count(struct pci_dev *pdev)
+static int its_pci_msi_vec_count(struct pci_dev *pdev, void *data)
 {
-	int msi, msix;
+	int msi, msix, *count = data;
 
 	msi = max(pci_msi_vec_count(pdev), 0);
 	msix = max(pci_msix_vec_count(pdev), 0);
+	*count += max(msi, msix);
 
-	return max(msi, msix);
+	return 0;
 }
 
 static int its_get_pci_alias(struct pci_dev *pdev, u16 alias, void *data)
 {
-	struct its_pci_alias *dev_alias = data;
+	struct pci_dev **alias_dev = data;
 
-	if (pdev != dev_alias->pdev)
-		dev_alias->count += its_pci_msi_vec_count(pdev);
+	*alias_dev = pdev;
 
 	return 0;
 }
@@ -69,9 +64,9 @@ static int its_get_pci_alias(struct pci_dev *pdev, u16 alias, void *data)
 static int its_pci_msi_prepare(struct irq_domain *domain, struct device *dev,
 			       int nvec, msi_alloc_info_t *info)
 {
-	struct pci_dev *pdev;
-	struct its_pci_alias dev_alias;
+	struct pci_dev *pdev, *alias_dev;
 	struct msi_domain_info *msi_info;
+	int alias_count = 0;
 
 	if (!dev_is_pci(dev))
 		return -EINVAL;
@@ -79,16 +74,20 @@ static int its_pci_msi_prepare(struct irq_domain *domain, struct device *dev,
 	msi_info = msi_get_domain_info(domain->parent);
 
 	pdev = to_pci_dev(dev);
-	dev_alias.pdev = pdev;
-	dev_alias.count = nvec;
-
-	pci_for_each_dma_alias(pdev, its_get_pci_alias, &dev_alias);
+	/*
+	 * If pdev is downstream of any aliasing bridges, take an upper
+	 * bound of how many other vectors could map to the same DevID.
+	 */
+	pci_for_each_dma_alias(pdev, its_get_pci_alias, &alias_dev);
+	if (alias_dev != pdev && alias_dev->subordinate)
+		pci_walk_bus(alias_dev->subordinate, its_pci_msi_vec_count,
+			     &alias_count);
 
 	/* ITS specific DeviceID, as the core ITS ignores dev. */
 	info->scratchpad[0].ul = pci_msi_domain_get_msi_rid(domain, pdev);
 
 	return msi_info->ops->msi_prepare(domain->parent,
-					  dev, dev_alias.count, info);
+					  dev, max(nvec, alias_count), info);
 }
 
 static struct msi_domain_ops its_pci_msi_ops = {
diff --git a/drivers/irqchip/irq-gic-v3-its-platform-msi.c b/drivers/irqchip/irq-gic-v3-its-platform-msi.c
index 9e9dda33eb17..249240d9a425 100644
--- a/drivers/irqchip/irq-gic-v3-its-platform-msi.c
+++ b/drivers/irqchip/irq-gic-v3-its-platform-msi.c
@@ -86,7 +86,7 @@ static struct msi_domain_info its_pmsi_domain_info = {
 	.chip	= &its_pmsi_irq_chip,
 };
 
-static struct of_device_id its_device_id[] = {
+static const struct of_device_id its_device_id[] = {
 	{	.compatible	= "arm,gic-v3-its",	},
 	{},
 };
diff --git a/drivers/irqchip/irq-gic-v3-its.c b/drivers/irqchip/irq-gic-v3-its.c
index 45ea193325d2..68932873eebc 100644
--- a/drivers/irqchip/irq-gic-v3-its.c
+++ b/drivers/irqchip/irq-gic-v3-its.c
@@ -644,9 +644,12 @@ static int its_set_affinity(struct irq_data *d, const struct cpumask *mask_val,
 	if (cpu >= nr_cpu_ids)
 		return -EINVAL;
 
-	target_col = &its_dev->its->collections[cpu];
-	its_send_movi(its_dev, target_col, id);
-	its_dev->event_map.col_map[id] = cpu;
+	/* don't set the affinity when the target cpu is same as current one */
+	if (cpu != its_dev->event_map.col_map[id]) {
+		target_col = &its_dev->its->collections[cpu];
+		its_send_movi(its_dev, target_col, id);
+		its_dev->event_map.col_map[id] = cpu;
+	}
 
 	return IRQ_SET_MASK_OK_DONE;
 }
@@ -688,9 +691,11 @@ static struct irq_chip its_irq_chip = {
  */
 #define IRQS_PER_CHUNK_SHIFT	5
 #define IRQS_PER_CHUNK		(1 << IRQS_PER_CHUNK_SHIFT)
+#define ITS_MAX_LPI_NRBITS	16 /* 64K LPIs */
 
 static unsigned long *lpi_bitmap;
 static u32 lpi_chunks;
+static u32 lpi_id_bits;
 static DEFINE_SPINLOCK(lpi_lock);
 
 static int its_lpi_to_chunk(int lpi)
@@ -786,17 +791,13 @@ static void its_lpi_free(struct event_lpi_map *map)
 }
 
 /*
- * We allocate 64kB for PROPBASE. That gives us at most 64K LPIs to
+ * We allocate memory for PROPBASE to cover 2 ^ lpi_id_bits LPIs to
  * deal with (one configuration byte per interrupt). PENDBASE has to
  * be 64kB aligned (one bit per LPI, plus 8192 bits for SPI/PPI/SGI).
  */
-#define LPI_PROPBASE_SZ		SZ_64K
-#define LPI_PENDBASE_SZ		(LPI_PROPBASE_SZ / 8 + SZ_1K)
-
-/*
- * This is how many bits of ID we need, including the useless ones.
- */
-#define LPI_NRBITS		ilog2(LPI_PROPBASE_SZ + SZ_8K)
+#define LPI_NRBITS		lpi_id_bits
+#define LPI_PROPBASE_SZ		ALIGN(BIT(LPI_NRBITS), SZ_64K)
+#define LPI_PENDBASE_SZ		ALIGN(BIT(LPI_NRBITS) / 8, SZ_64K)
 
 #define LPI_PROP_DEFAULT_PRIO	0xa0
 
@@ -804,6 +805,7 @@ static int __init its_alloc_lpi_tables(void)
 {
 	phys_addr_t paddr;
 
+	lpi_id_bits = min_t(u32, gic_rdists->id_bits, ITS_MAX_LPI_NRBITS);
 	gic_rdists->prop_page = alloc_pages(GFP_NOWAIT,
 					   get_order(LPI_PROPBASE_SZ));
 	if (!gic_rdists->prop_page) {
@@ -822,7 +824,7 @@ static int __init its_alloc_lpi_tables(void)
 	/* Make sure the GIC will observe the written configuration */
 	gic_flush_dcache_to_poc(page_address(gic_rdists->prop_page), LPI_PROPBASE_SZ);
 
-	return 0;
+	return its_lpi_init(lpi_id_bits);
 }
 
 static const char *its_base_type_string[] = {
@@ -1097,7 +1099,7 @@ static void its_cpu_init_lpis(void)
 		 * hence the 'max(LPI_PENDBASE_SZ, SZ_64K)' below.
 		 */
 		pend_page = alloc_pages(GFP_NOWAIT | __GFP_ZERO,
-					get_order(max(LPI_PENDBASE_SZ, SZ_64K)));
+					get_order(max_t(u32, LPI_PENDBASE_SZ, SZ_64K)));
 		if (!pend_page) {
 			pr_err("Failed to allocate PENDBASE for CPU%d\n",
 			       smp_processor_id());
@@ -1661,7 +1663,7 @@ static int its_init_domain(struct fwnode_handle *handle, struct its_node *its)
 	}
 
 	inner_domain->parent = its_parent;
-	inner_domain->bus_token = DOMAIN_BUS_NEXUS;
+	irq_domain_update_bus_token(inner_domain, DOMAIN_BUS_NEXUS);
 	inner_domain->flags |= IRQ_DOMAIN_FLAG_MSI_REMAP;
 	info->ops = &its_msi_domain_ops;
 	info->data = its;
@@ -1801,7 +1803,7 @@ int its_cpu_init(void)
 	return 0;
 }
 
-static struct of_device_id its_device_id[] = {
+static const struct of_device_id its_device_id[] = {
 	{	.compatible	= "arm,gic-v3-its",	},
 	{},
 };
@@ -1833,6 +1835,78 @@ static int __init its_of_probe(struct device_node *node)
 
 #define ACPI_GICV3_ITS_MEM_SIZE (SZ_128K)
 
+#if defined(CONFIG_ACPI_NUMA) && (ACPI_CA_VERSION >= 0x20170531)
+struct its_srat_map {
+	/* numa node id */
+	u32	numa_node;
+	/* GIC ITS ID */
+	u32	its_id;
+};
+
+static struct its_srat_map its_srat_maps[MAX_NUMNODES] __initdata;
+static int its_in_srat __initdata;
+
+static int __init acpi_get_its_numa_node(u32 its_id)
+{
+	int i;
+
+	for (i = 0; i < its_in_srat; i++) {
+		if (its_id == its_srat_maps[i].its_id)
+			return its_srat_maps[i].numa_node;
+	}
+	return NUMA_NO_NODE;
+}
+
+static int __init gic_acpi_parse_srat_its(struct acpi_subtable_header *header,
+			 const unsigned long end)
+{
+	int node;
+	struct acpi_srat_gic_its_affinity *its_affinity;
+
+	its_affinity = (struct acpi_srat_gic_its_affinity *)header;
+	if (!its_affinity)
+		return -EINVAL;
+
+	if (its_affinity->header.length < sizeof(*its_affinity)) {
+		pr_err("SRAT: Invalid header length %d in ITS affinity\n",
+			its_affinity->header.length);
+		return -EINVAL;
+	}
+
+	if (its_in_srat >= MAX_NUMNODES) {
+		pr_err("SRAT: ITS affinity exceeding max count[%d]\n",
+				MAX_NUMNODES);
+		return -EINVAL;
+	}
+
+	node = acpi_map_pxm_to_node(its_affinity->proximity_domain);
+
+	if (node == NUMA_NO_NODE || node >= MAX_NUMNODES) {
+		pr_err("SRAT: Invalid NUMA node %d in ITS affinity\n", node);
+		return 0;
+	}
+
+	its_srat_maps[its_in_srat].numa_node = node;
+	its_srat_maps[its_in_srat].its_id = its_affinity->its_id;
+	its_in_srat++;
+	pr_info("SRAT: PXM %d -> ITS %d -> Node %d\n",
+		its_affinity->proximity_domain, its_affinity->its_id, node);
+
+	return 0;
+}
+
+static void __init acpi_table_parse_srat_its(void)
+{
+	acpi_table_parse_entries(ACPI_SIG_SRAT,
+			sizeof(struct acpi_table_srat),
+			ACPI_SRAT_TYPE_GIC_ITS_AFFINITY,
+			gic_acpi_parse_srat_its, 0);
+}
+#else
+static void __init acpi_table_parse_srat_its(void)	{ }
+static int __init acpi_get_its_numa_node(u32 its_id) { return NUMA_NO_NODE; }
+#endif
+
 static int __init gic_acpi_parse_madt_its(struct acpi_subtable_header *header,
 					  const unsigned long end)
 {
@@ -1861,7 +1935,8 @@ static int __init gic_acpi_parse_madt_its(struct acpi_subtable_header *header,
 		goto dom_err;
 	}
 
-	err = its_probe_one(&res, dom_handle, NUMA_NO_NODE);
+	err = its_probe_one(&res, dom_handle,
+			acpi_get_its_numa_node(its_entry->translation_id));
 	if (!err)
 		return 0;
 
@@ -1873,6 +1948,7 @@ dom_err:
 
 static void __init its_acpi_probe(void)
 {
+	acpi_table_parse_srat_its();
 	acpi_table_parse_madt(ACPI_MADT_TYPE_GENERIC_TRANSLATOR,
 			      gic_acpi_parse_madt_its, 0);
 }
@@ -1898,8 +1974,5 @@ int __init its_init(struct fwnode_handle *handle, struct rdists *rdists,
 	}
 
 	gic_rdists = rdists;
-	its_alloc_lpi_tables();
-	its_lpi_init(rdists->id_bits);
-
-	return 0;
+	return its_alloc_lpi_tables();
 }
diff --git a/drivers/irqchip/irq-gic-v3.c b/drivers/irqchip/irq-gic-v3.c
index c132f29322cc..dbffb7ab6203 100644
--- a/drivers/irqchip/irq-gic-v3.c
+++ b/drivers/irqchip/irq-gic-v3.c
@@ -645,6 +645,9 @@ static int gic_set_affinity(struct irq_data *d, const struct cpumask *mask_val,
 	int enabled;
 	u64 val;
 
+	if (cpu >= nr_cpu_ids)
+		return -EINVAL;
+
 	if (gic_irq_in_rdist(d))
 		return -EINVAL;
 
diff --git a/drivers/irqchip/irq-i8259.c b/drivers/irqchip/irq-i8259.c
index 1aec12c6d9ac..7aafbb091b67 100644
--- a/drivers/irqchip/irq-i8259.c
+++ b/drivers/irqchip/irq-i8259.c
@@ -307,7 +307,7 @@ static int i8259A_irq_domain_map(struct irq_domain *d, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops i8259A_ops = {
+static const struct irq_domain_ops i8259A_ops = {
 	.map = i8259A_irq_domain_map,
 	.xlate = irq_domain_xlate_onecell,
 };
diff --git a/drivers/irqchip/irq-imx-gpcv2.c b/drivers/irqchip/irq-imx-gpcv2.c
index 9463f3557e82..bb36f572e322 100644
--- a/drivers/irqchip/irq-imx-gpcv2.c
+++ b/drivers/irqchip/irq-imx-gpcv2.c
@@ -200,7 +200,7 @@ static int imx_gpcv2_domain_alloc(struct irq_domain *domain,
 					    &parent_fwspec);
 }
 
-static struct irq_domain_ops gpcv2_irqchip_data_domain_ops = {
+static const struct irq_domain_ops gpcv2_irqchip_data_domain_ops = {
 	.translate	= imx_gpcv2_domain_translate,
 	.alloc		= imx_gpcv2_domain_alloc,
 	.free		= irq_domain_free_irqs_common,
diff --git a/drivers/irqchip/irq-mbigen.c b/drivers/irqchip/irq-mbigen.c
index 31d6b5a582d2..567b29c47608 100644
--- a/drivers/irqchip/irq-mbigen.c
+++ b/drivers/irqchip/irq-mbigen.c
@@ -228,7 +228,7 @@ static int mbigen_irq_domain_alloc(struct irq_domain *domain,
 	return 0;
 }
 
-static struct irq_domain_ops mbigen_domain_ops = {
+static const struct irq_domain_ops mbigen_domain_ops = {
 	.translate	= mbigen_domain_translate,
 	.alloc		= mbigen_irq_domain_alloc,
 	.free		= irq_domain_free_irqs_common,
diff --git a/drivers/irqchip/irq-mips-cpu.c b/drivers/irqchip/irq-mips-cpu.c
index b247f3c743ac..0a8ed1c05518 100644
--- a/drivers/irqchip/irq-mips-cpu.c
+++ b/drivers/irqchip/irq-mips-cpu.c
@@ -240,7 +240,7 @@ static void mips_cpu_register_ipi_domain(struct device_node *of_node)
 					      ipi_domain_state);
 	if (!ipi_domain)
 		panic("Failed to add MIPS CPU IPI domain");
-	ipi_domain->bus_token = DOMAIN_BUS_IPI;
+	irq_domain_update_bus_token(ipi_domain, DOMAIN_BUS_IPI);
 }
 
 #else /* !CONFIG_GENERIC_IRQ_IPI */
diff --git a/drivers/irqchip/irq-mips-gic.c b/drivers/irqchip/irq-mips-gic.c
index eb7fbe159963..832ebf4062f7 100644
--- a/drivers/irqchip/irq-mips-gic.c
+++ b/drivers/irqchip/irq-mips-gic.c
@@ -140,7 +140,7 @@ static inline void gic_map_to_vpe(unsigned int intr, unsigned int vpe)
 }
 
 #ifdef CONFIG_CLKSRC_MIPS_GIC
-u64 gic_read_count(void)
+u64 notrace gic_read_count(void)
 {
 	unsigned int hi, hi2, lo;
 
@@ -167,7 +167,7 @@ unsigned int gic_get_count_width(void)
 	return bits;
 }
 
-void gic_write_compare(u64 cnt)
+void notrace gic_write_compare(u64 cnt)
 {
 	if (mips_cm_is64) {
 		gic_write(GIC_REG(VPE_LOCAL, GIC_VPE_COMPARE), cnt);
@@ -179,7 +179,7 @@ void gic_write_compare(u64 cnt)
 	}
 }
 
-void gic_write_cpu_compare(u64 cnt, int cpu)
+void notrace gic_write_cpu_compare(u64 cnt, int cpu)
 {
 	unsigned long flags;
 
@@ -874,7 +874,7 @@ int gic_ipi_domain_match(struct irq_domain *d, struct device_node *node,
 	}
 }
 
-static struct irq_domain_ops gic_ipi_domain_ops = {
+static const struct irq_domain_ops gic_ipi_domain_ops = {
 	.xlate = gic_ipi_domain_xlate,
 	.alloc = gic_ipi_domain_alloc,
 	.free = gic_ipi_domain_free,
@@ -960,7 +960,7 @@ static void __init __gic_init(unsigned long gic_base_addr,
 		panic("Failed to add GIC IPI domain");
 
 	gic_ipi_domain->name = "mips-gic-ipi";
-	gic_ipi_domain->bus_token = DOMAIN_BUS_IPI;
+	irq_domain_update_bus_token(gic_ipi_domain, DOMAIN_BUS_IPI);
 
 	if (node &&
 	    !of_property_read_u32_array(node, "mti,reserved-ipi-vectors", v, 2)) {
diff --git a/drivers/irqchip/irq-mvebu-gicp.c b/drivers/irqchip/irq-mvebu-gicp.c
new file mode 100644
index 000000000000..b283fc90be1e
--- /dev/null
+++ b/drivers/irqchip/irq-mvebu-gicp.c
@@ -0,0 +1,279 @@
+/*
+ * Copyright (C) 2017 Marvell
+ *
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/msi.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+
+#include <dt-bindings/interrupt-controller/arm-gic.h>
+
+#include "irq-mvebu-gicp.h"
+
+#define GICP_SETSPI_NSR_OFFSET	0x0
+#define GICP_CLRSPI_NSR_OFFSET	0x8
+
+struct mvebu_gicp_spi_range {
+	unsigned int start;
+	unsigned int count;
+};
+
+struct mvebu_gicp {
+	struct mvebu_gicp_spi_range *spi_ranges;
+	unsigned int spi_ranges_cnt;
+	unsigned int spi_cnt;
+	unsigned long *spi_bitmap;
+	spinlock_t spi_lock;
+	struct resource *res;
+	struct device *dev;
+};
+
+static int gicp_idx_to_spi(struct mvebu_gicp *gicp, int idx)
+{
+	int i;
+
+	for (i = 0; i < gicp->spi_ranges_cnt; i++) {
+		struct mvebu_gicp_spi_range *r = &gicp->spi_ranges[i];
+
+		if (idx < r->count)
+			return r->start + idx;
+
+		idx -= r->count;
+	}
+
+	return -EINVAL;
+}
+
+int mvebu_gicp_get_doorbells(struct device_node *dn, phys_addr_t *setspi,
+			     phys_addr_t *clrspi)
+{
+	struct platform_device *pdev;
+	struct mvebu_gicp *gicp;
+
+	pdev = of_find_device_by_node(dn);
+	if (!pdev)
+		return -ENODEV;
+
+	gicp = platform_get_drvdata(pdev);
+	if (!gicp)
+		return -ENODEV;
+
+	*setspi = gicp->res->start + GICP_SETSPI_NSR_OFFSET;
+	*clrspi = gicp->res->start + GICP_CLRSPI_NSR_OFFSET;
+
+	return 0;
+}
+
+static void gicp_compose_msi_msg(struct irq_data *data, struct msi_msg *msg)
+{
+	struct mvebu_gicp *gicp = data->chip_data;
+	phys_addr_t setspi = gicp->res->start + GICP_SETSPI_NSR_OFFSET;
+
+	msg->data = data->hwirq;
+	msg->address_lo = lower_32_bits(setspi);
+	msg->address_hi = upper_32_bits(setspi);
+}
+
+static struct irq_chip gicp_irq_chip = {
+	.name			= "GICP",
+	.irq_mask		= irq_chip_mask_parent,
+	.irq_unmask		= irq_chip_unmask_parent,
+	.irq_eoi		= irq_chip_eoi_parent,
+	.irq_set_affinity	= irq_chip_set_affinity_parent,
+	.irq_set_type		= irq_chip_set_type_parent,
+	.irq_compose_msi_msg	= gicp_compose_msi_msg,
+};
+
+static int gicp_irq_domain_alloc(struct irq_domain *domain, unsigned int virq,
+				 unsigned int nr_irqs, void *args)
+{
+	struct mvebu_gicp *gicp = domain->host_data;
+	struct irq_fwspec fwspec;
+	unsigned int hwirq;
+	int ret;
+
+	spin_lock(&gicp->spi_lock);
+	hwirq = find_first_zero_bit(gicp->spi_bitmap, gicp->spi_cnt);
+	if (hwirq == gicp->spi_cnt) {
+		spin_unlock(&gicp->spi_lock);
+		return -ENOSPC;
+	}
+	__set_bit(hwirq, gicp->spi_bitmap);
+	spin_unlock(&gicp->spi_lock);
+
+	fwspec.fwnode = domain->parent->fwnode;
+	fwspec.param_count = 3;
+	fwspec.param[0] = GIC_SPI;
+	fwspec.param[1] = gicp_idx_to_spi(gicp, hwirq) - 32;
+	/*
+	 * Assume edge rising for now, it will be properly set when
+	 * ->set_type() is called
+	 */
+	fwspec.param[2] = IRQ_TYPE_EDGE_RISING;
+
+	ret = irq_domain_alloc_irqs_parent(domain, virq, 1, &fwspec);
+	if (ret) {
+		dev_err(gicp->dev, "Cannot allocate parent IRQ\n");
+		goto free_hwirq;
+	}
+
+	ret = irq_domain_set_hwirq_and_chip(domain, virq, hwirq,
+					    &gicp_irq_chip, gicp);
+	if (ret)
+		goto free_irqs_parent;
+
+	return 0;
+
+free_irqs_parent:
+	irq_domain_free_irqs_parent(domain, virq, nr_irqs);
+free_hwirq:
+	spin_lock(&gicp->spi_lock);
+	__clear_bit(hwirq, gicp->spi_bitmap);
+	spin_unlock(&gicp->spi_lock);
+	return ret;
+}
+
+static void gicp_irq_domain_free(struct irq_domain *domain,
+				 unsigned int virq, unsigned int nr_irqs)
+{
+	struct mvebu_gicp *gicp = domain->host_data;
+	struct irq_data *d = irq_domain_get_irq_data(domain, virq);
+
+	if (d->hwirq >= gicp->spi_cnt) {
+		dev_err(gicp->dev, "Invalid hwirq %lu\n", d->hwirq);
+		return;
+	}
+
+	irq_domain_free_irqs_parent(domain, virq, nr_irqs);
+
+	spin_lock(&gicp->spi_lock);
+	__clear_bit(d->hwirq, gicp->spi_bitmap);
+	spin_unlock(&gicp->spi_lock);
+}
+
+static const struct irq_domain_ops gicp_domain_ops = {
+	.alloc	= gicp_irq_domain_alloc,
+	.free	= gicp_irq_domain_free,
+};
+
+static struct irq_chip gicp_msi_irq_chip = {
+	.name		= "GICP",
+	.irq_set_type	= irq_chip_set_type_parent,
+};
+
+static struct msi_domain_ops gicp_msi_ops = {
+};
+
+static struct msi_domain_info gicp_msi_domain_info = {
+	.flags	= (MSI_FLAG_USE_DEF_DOM_OPS | MSI_FLAG_USE_DEF_CHIP_OPS),
+	.ops	= &gicp_msi_ops,
+	.chip	= &gicp_msi_irq_chip,
+};
+
+static int mvebu_gicp_probe(struct platform_device *pdev)
+{
+	struct mvebu_gicp *gicp;
+	struct irq_domain *inner_domain, *plat_domain, *parent_domain;
+	struct device_node *node = pdev->dev.of_node;
+	struct device_node *irq_parent_dn;
+	int ret, i;
+
+	gicp = devm_kzalloc(&pdev->dev, sizeof(*gicp), GFP_KERNEL);
+	if (!gicp)
+		return -ENOMEM;
+
+	gicp->dev = &pdev->dev;
+
+	gicp->res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!gicp->res)
+		return -ENODEV;
+
+	ret = of_property_count_u32_elems(node, "marvell,spi-ranges");
+	if (ret < 0)
+		return ret;
+
+	gicp->spi_ranges_cnt = ret / 2;
+
+	gicp->spi_ranges =
+		devm_kzalloc(&pdev->dev,
+			     gicp->spi_ranges_cnt *
+			     sizeof(struct mvebu_gicp_spi_range),
+			     GFP_KERNEL);
+	if (!gicp->spi_ranges)
+		return -ENOMEM;
+
+	for (i = 0; i < gicp->spi_ranges_cnt; i++) {
+		of_property_read_u32_index(node, "marvell,spi-ranges",
+					   i * 2,
+					   &gicp->spi_ranges[i].start);
+
+		of_property_read_u32_index(node, "marvell,spi-ranges",
+					   i * 2 + 1,
+					   &gicp->spi_ranges[i].count);
+
+		gicp->spi_cnt += gicp->spi_ranges[i].count;
+	}
+
+	gicp->spi_bitmap = devm_kzalloc(&pdev->dev,
+				BITS_TO_LONGS(gicp->spi_cnt) * sizeof(long),
+				GFP_KERNEL);
+	if (!gicp->spi_bitmap)
+		return -ENOMEM;
+
+	irq_parent_dn = of_irq_find_parent(node);
+	if (!irq_parent_dn) {
+		dev_err(&pdev->dev, "failed to find parent IRQ node\n");
+		return -ENODEV;
+	}
+
+	parent_domain = irq_find_host(irq_parent_dn);
+	if (!parent_domain) {
+		dev_err(&pdev->dev, "failed to find parent IRQ domain\n");
+		return -ENODEV;
+	}
+
+	inner_domain = irq_domain_create_hierarchy(parent_domain, 0,
+						   gicp->spi_cnt,
+						   of_node_to_fwnode(node),
+						   &gicp_domain_ops, gicp);
+	if (!inner_domain)
+		return -ENOMEM;
+
+
+	plat_domain = platform_msi_create_irq_domain(of_node_to_fwnode(node),
+						     &gicp_msi_domain_info,
+						     inner_domain);
+	if (!plat_domain) {
+		irq_domain_remove(inner_domain);
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, gicp);
+
+	return 0;
+}
+
+static const struct of_device_id mvebu_gicp_of_match[] = {
+	{ .compatible = "marvell,ap806-gicp", },
+	{},
+};
+
+static struct platform_driver mvebu_gicp_driver = {
+	.probe  = mvebu_gicp_probe,
+	.driver = {
+		.name = "mvebu-gicp",
+		.of_match_table = mvebu_gicp_of_match,
+	},
+};
+builtin_platform_driver(mvebu_gicp_driver);
diff --git a/drivers/irqchip/irq-mvebu-gicp.h b/drivers/irqchip/irq-mvebu-gicp.h
new file mode 100644
index 000000000000..98535e886ea5
--- /dev/null
+++ b/drivers/irqchip/irq-mvebu-gicp.h
@@ -0,0 +1,11 @@
+#ifndef __MVEBU_GICP_H__
+#define __MVEBU_GICP_H__
+
+#include <linux/types.h>
+
+struct device_node;
+
+int mvebu_gicp_get_doorbells(struct device_node *dn, phys_addr_t *setspi,
+			     phys_addr_t *clrspi);
+
+#endif /* __MVEBU_GICP_H__ */
diff --git a/drivers/irqchip/irq-mvebu-icu.c b/drivers/irqchip/irq-mvebu-icu.c
new file mode 100644
index 000000000000..e18c48d3a92e
--- /dev/null
+++ b/drivers/irqchip/irq-mvebu-icu.c
@@ -0,0 +1,289 @@
+/*
+ * Copyright (C) 2017 Marvell
+ *
+ * Hanna Hawa <hannah@marvell.com>
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/irqchip.h>
+#include <linux/irqdomain.h>
+#include <linux/kernel.h>
+#include <linux/msi.h>
+#include <linux/of_irq.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+
+#include <dt-bindings/interrupt-controller/mvebu-icu.h>
+
+#include "irq-mvebu-gicp.h"
+
+/* ICU registers */
+#define ICU_SETSPI_NSR_AL	0x10
+#define ICU_SETSPI_NSR_AH	0x14
+#define ICU_CLRSPI_NSR_AL	0x18
+#define ICU_CLRSPI_NSR_AH	0x1c
+#define ICU_INT_CFG(x)          (0x100 + 4 * (x))
+#define   ICU_INT_ENABLE	BIT(24)
+#define   ICU_IS_EDGE		BIT(28)
+#define   ICU_GROUP_SHIFT	29
+
+/* ICU definitions */
+#define ICU_MAX_IRQS		207
+#define ICU_SATA0_ICU_ID	109
+#define ICU_SATA1_ICU_ID	107
+
+struct mvebu_icu {
+	struct irq_chip irq_chip;
+	void __iomem *base;
+	struct irq_domain *domain;
+	struct device *dev;
+};
+
+struct mvebu_icu_irq_data {
+	struct mvebu_icu *icu;
+	unsigned int icu_group;
+	unsigned int type;
+};
+
+static void mvebu_icu_write_msg(struct msi_desc *desc, struct msi_msg *msg)
+{
+	struct irq_data *d = irq_get_irq_data(desc->irq);
+	struct mvebu_icu_irq_data *icu_irqd = d->chip_data;
+	struct mvebu_icu *icu = icu_irqd->icu;
+	unsigned int icu_int;
+
+	if (msg->address_lo || msg->address_hi) {
+		/* Configure the ICU with irq number & type */
+		icu_int = msg->data | ICU_INT_ENABLE;
+		if (icu_irqd->type & IRQ_TYPE_EDGE_RISING)
+			icu_int |= ICU_IS_EDGE;
+		icu_int |= icu_irqd->icu_group << ICU_GROUP_SHIFT;
+	} else {
+		/* De-configure the ICU */
+		icu_int = 0;
+	}
+
+	writel_relaxed(icu_int, icu->base + ICU_INT_CFG(d->hwirq));
+
+	/*
+	 * The SATA unit has 2 ports, and a dedicated ICU entry per
+	 * port. The ahci sata driver supports only one irq interrupt
+	 * per SATA unit. To solve this conflict, we configure the 2
+	 * SATA wired interrupts in the south bridge into 1 GIC
+	 * interrupt in the north bridge. Even if only a single port
+	 * is enabled, if sata node is enabled, both interrupts are
+	 * configured (regardless of which port is actually in use).
+	 */
+	if (d->hwirq == ICU_SATA0_ICU_ID || d->hwirq == ICU_SATA1_ICU_ID) {
+		writel_relaxed(icu_int,
+			       icu->base + ICU_INT_CFG(ICU_SATA0_ICU_ID));
+		writel_relaxed(icu_int,
+			       icu->base + ICU_INT_CFG(ICU_SATA1_ICU_ID));
+	}
+}
+
+static int
+mvebu_icu_irq_domain_translate(struct irq_domain *d, struct irq_fwspec *fwspec,
+			       unsigned long *hwirq, unsigned int *type)
+{
+	struct mvebu_icu *icu = d->host_data;
+	unsigned int icu_group;
+
+	/* Check the count of the parameters in dt */
+	if (WARN_ON(fwspec->param_count < 3)) {
+		dev_err(icu->dev, "wrong ICU parameter count %d\n",
+			fwspec->param_count);
+		return -EINVAL;
+	}
+
+	/* Only ICU group type is handled */
+	icu_group = fwspec->param[0];
+	if (icu_group != ICU_GRP_NSR && icu_group != ICU_GRP_SR &&
+	    icu_group != ICU_GRP_SEI && icu_group != ICU_GRP_REI) {
+		dev_err(icu->dev, "wrong ICU group type %x\n", icu_group);
+		return -EINVAL;
+	}
+
+	*hwirq = fwspec->param[1];
+	if (*hwirq >= ICU_MAX_IRQS) {
+		dev_err(icu->dev, "invalid interrupt number %ld\n", *hwirq);
+		return -EINVAL;
+	}
+
+	/* Mask the type to prevent wrong DT configuration */
+	*type = fwspec->param[2] & IRQ_TYPE_SENSE_MASK;
+
+	return 0;
+}
+
+static int
+mvebu_icu_irq_domain_alloc(struct irq_domain *domain, unsigned int virq,
+			   unsigned int nr_irqs, void *args)
+{
+	int err;
+	unsigned long hwirq;
+	struct irq_fwspec *fwspec = args;
+	struct mvebu_icu *icu = platform_msi_get_host_data(domain);
+	struct mvebu_icu_irq_data *icu_irqd;
+
+	icu_irqd = kmalloc(sizeof(*icu_irqd), GFP_KERNEL);
+	if (!icu_irqd)
+		return -ENOMEM;
+
+	err = mvebu_icu_irq_domain_translate(domain, fwspec, &hwirq,
+					     &icu_irqd->type);
+	if (err) {
+		dev_err(icu->dev, "failed to translate ICU parameters\n");
+		goto free_irqd;
+	}
+
+	icu_irqd->icu_group = fwspec->param[0];
+	icu_irqd->icu = icu;
+
+	err = platform_msi_domain_alloc(domain, virq, nr_irqs);
+	if (err) {
+		dev_err(icu->dev, "failed to allocate ICU interrupt in parent domain\n");
+		goto free_irqd;
+	}
+
+	/* Make sure there is no interrupt left pending by the firmware */
+	err = irq_set_irqchip_state(virq, IRQCHIP_STATE_PENDING, false);
+	if (err)
+		goto free_msi;
+
+	err = irq_domain_set_hwirq_and_chip(domain, virq, hwirq,
+					    &icu->irq_chip, icu_irqd);
+	if (err) {
+		dev_err(icu->dev, "failed to set the data to IRQ domain\n");
+		goto free_msi;
+	}
+
+	return 0;
+
+free_msi:
+	platform_msi_domain_free(domain, virq, nr_irqs);
+free_irqd:
+	kfree(icu_irqd);
+	return err;
+}
+
+static void
+mvebu_icu_irq_domain_free(struct irq_domain *domain, unsigned int virq,
+			  unsigned int nr_irqs)
+{
+	struct irq_data *d = irq_get_irq_data(virq);
+	struct mvebu_icu_irq_data *icu_irqd = d->chip_data;
+
+	kfree(icu_irqd);
+
+	platform_msi_domain_free(domain, virq, nr_irqs);
+}
+
+static const struct irq_domain_ops mvebu_icu_domain_ops = {
+	.translate = mvebu_icu_irq_domain_translate,
+	.alloc     = mvebu_icu_irq_domain_alloc,
+	.free      = mvebu_icu_irq_domain_free,
+};
+
+static int mvebu_icu_probe(struct platform_device *pdev)
+{
+	struct mvebu_icu *icu;
+	struct device_node *node = pdev->dev.of_node;
+	struct device_node *gicp_dn;
+	struct resource *res;
+	phys_addr_t setspi, clrspi;
+	u32 i, icu_int;
+	int ret;
+
+	icu = devm_kzalloc(&pdev->dev, sizeof(struct mvebu_icu),
+			   GFP_KERNEL);
+	if (!icu)
+		return -ENOMEM;
+
+	icu->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	icu->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(icu->base)) {
+		dev_err(&pdev->dev, "Failed to map icu base address.\n");
+		return PTR_ERR(icu->base);
+	}
+
+	icu->irq_chip.name = devm_kasprintf(&pdev->dev, GFP_KERNEL,
+					    "ICU.%x",
+					    (unsigned int)res->start);
+	if (!icu->irq_chip.name)
+		return -ENOMEM;
+
+	icu->irq_chip.irq_mask = irq_chip_mask_parent;
+	icu->irq_chip.irq_unmask = irq_chip_unmask_parent;
+	icu->irq_chip.irq_eoi = irq_chip_eoi_parent;
+	icu->irq_chip.irq_set_type = irq_chip_set_type_parent;
+#ifdef CONFIG_SMP
+	icu->irq_chip.irq_set_affinity = irq_chip_set_affinity_parent;
+#endif
+
+	/*
+	 * We're probed after MSI domains have been resolved, so force
+	 * resolution here.
+	 */
+	pdev->dev.msi_domain = of_msi_get_domain(&pdev->dev, node,
+						 DOMAIN_BUS_PLATFORM_MSI);
+	if (!pdev->dev.msi_domain)
+		return -EPROBE_DEFER;
+
+	gicp_dn = irq_domain_get_of_node(pdev->dev.msi_domain);
+	if (!gicp_dn)
+		return -ENODEV;
+
+	ret = mvebu_gicp_get_doorbells(gicp_dn, &setspi, &clrspi);
+	if (ret)
+		return ret;
+
+	/* Set Clear/Set ICU SPI message address in AP */
+	writel_relaxed(upper_32_bits(setspi), icu->base + ICU_SETSPI_NSR_AH);
+	writel_relaxed(lower_32_bits(setspi), icu->base + ICU_SETSPI_NSR_AL);
+	writel_relaxed(upper_32_bits(clrspi), icu->base + ICU_CLRSPI_NSR_AH);
+	writel_relaxed(lower_32_bits(clrspi), icu->base + ICU_CLRSPI_NSR_AL);
+
+	/*
+	 * Clean all ICU interrupts with type SPI_NSR, required to
+	 * avoid unpredictable SPI assignments done by firmware.
+	 */
+	for (i = 0 ; i < ICU_MAX_IRQS ; i++) {
+		icu_int = readl(icu->base + ICU_INT_CFG(i));
+		if ((icu_int >> ICU_GROUP_SHIFT) == ICU_GRP_NSR)
+			writel_relaxed(0x0, icu->base + ICU_INT_CFG(i));
+	}
+
+	icu->domain =
+		platform_msi_create_device_domain(&pdev->dev, ICU_MAX_IRQS,
+						  mvebu_icu_write_msg,
+						  &mvebu_icu_domain_ops, icu);
+	if (!icu->domain) {
+		dev_err(&pdev->dev, "Failed to create ICU domain\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id mvebu_icu_of_match[] = {
+	{ .compatible = "marvell,cp110-icu", },
+	{},
+};
+
+static struct platform_driver mvebu_icu_driver = {
+	.probe  = mvebu_icu_probe,
+	.driver = {
+		.name = "mvebu-icu",
+		.of_match_table = mvebu_icu_of_match,
+	},
+};
+builtin_platform_driver(mvebu_icu_driver);
diff --git a/drivers/irqchip/irq-or1k-pic.c b/drivers/irqchip/irq-or1k-pic.c
index 6a9a3e79218b..dd9d5d12fea2 100644
--- a/drivers/irqchip/irq-or1k-pic.c
+++ b/drivers/irqchip/irq-or1k-pic.c
@@ -70,7 +70,7 @@ static struct or1k_pic_dev or1k_pic_level = {
 		.name = "or1k-PIC-level",
 		.irq_unmask = or1k_pic_unmask,
 		.irq_mask = or1k_pic_mask,
-		.irq_mask_ack = or1k_pic_mask,
+		.irq_mask_ack = or1k_pic_mask_ack,
 	},
 	.handle = handle_level_irq,
 	.flags = IRQ_LEVEL | IRQ_NOPROBE,
diff --git a/drivers/irqchip/irq-renesas-h8300h.c b/drivers/irqchip/irq-renesas-h8300h.c
index c378768d75b3..b8327590ae52 100644
--- a/drivers/irqchip/irq-renesas-h8300h.c
+++ b/drivers/irqchip/irq-renesas-h8300h.c
@@ -67,7 +67,7 @@ static int irq_map(struct irq_domain *h, unsigned int virq,
        return 0;
 }
 
-static struct irq_domain_ops irq_ops = {
+static const struct irq_domain_ops irq_ops = {
        .map    = irq_map,
        .xlate  = irq_domain_xlate_onecell,
 };
diff --git a/drivers/irqchip/irq-renesas-h8s.c b/drivers/irqchip/irq-renesas-h8s.c
index af8c6c61c824..71d8139be26c 100644
--- a/drivers/irqchip/irq-renesas-h8s.c
+++ b/drivers/irqchip/irq-renesas-h8s.c
@@ -73,7 +73,7 @@ static __init int irq_map(struct irq_domain *h, unsigned int virq,
        return 0;
 }
 
-static struct irq_domain_ops irq_ops = {
+static const struct irq_domain_ops irq_ops = {
        .map    = irq_map,
        .xlate  = irq_domain_xlate_onecell,
 };
diff --git a/drivers/irqchip/irq-sunxi-nmi.c b/drivers/irqchip/irq-sunxi-nmi.c
index 668730c5cb66..a412b5d5d0fa 100644
--- a/drivers/irqchip/irq-sunxi-nmi.c
+++ b/drivers/irqchip/irq-sunxi-nmi.c
@@ -25,6 +25,29 @@
 
 #define SUNXI_NMI_SRC_TYPE_MASK	0x00000003
 
+#define SUNXI_NMI_IRQ_BIT	BIT(0)
+
+#define SUN6I_R_INTC_CTRL	0x0c
+#define SUN6I_R_INTC_PENDING	0x10
+#define SUN6I_R_INTC_ENABLE	0x40
+
+/*
+ * For deprecated sun6i-a31-sc-nmi compatible.
+ * Registers are offset by 0x0c.
+ */
+#define SUN6I_R_INTC_NMI_OFFSET	0x0c
+#define SUN6I_NMI_CTRL		(SUN6I_R_INTC_CTRL - SUN6I_R_INTC_NMI_OFFSET)
+#define SUN6I_NMI_PENDING	(SUN6I_R_INTC_PENDING - SUN6I_R_INTC_NMI_OFFSET)
+#define SUN6I_NMI_ENABLE	(SUN6I_R_INTC_ENABLE - SUN6I_R_INTC_NMI_OFFSET)
+
+#define SUN7I_NMI_CTRL		0x00
+#define SUN7I_NMI_PENDING	0x04
+#define SUN7I_NMI_ENABLE	0x08
+
+#define SUN9I_NMI_CTRL		0x00
+#define SUN9I_NMI_ENABLE	0x04
+#define SUN9I_NMI_PENDING	0x08
+
 enum {
 	SUNXI_SRC_TYPE_LEVEL_LOW = 0,
 	SUNXI_SRC_TYPE_EDGE_FALLING,
@@ -38,22 +61,28 @@ struct sunxi_sc_nmi_reg_offs {
 	u32 enable;
 };
 
-static struct sunxi_sc_nmi_reg_offs sun7i_reg_offs = {
-	.ctrl	= 0x00,
-	.pend	= 0x04,
-	.enable	= 0x08,
+static const struct sunxi_sc_nmi_reg_offs sun6i_r_intc_reg_offs __initconst = {
+	.ctrl	= SUN6I_R_INTC_CTRL,
+	.pend	= SUN6I_R_INTC_PENDING,
+	.enable	= SUN6I_R_INTC_ENABLE,
 };
 
-static struct sunxi_sc_nmi_reg_offs sun6i_reg_offs = {
-	.ctrl	= 0x00,
-	.pend	= 0x04,
-	.enable	= 0x34,
+static const struct sunxi_sc_nmi_reg_offs sun6i_reg_offs __initconst = {
+	.ctrl	= SUN6I_NMI_CTRL,
+	.pend	= SUN6I_NMI_PENDING,
+	.enable	= SUN6I_NMI_ENABLE,
 };
 
-static struct sunxi_sc_nmi_reg_offs sun9i_reg_offs = {
-	.ctrl	= 0x00,
-	.pend	= 0x08,
-	.enable	= 0x04,
+static const struct sunxi_sc_nmi_reg_offs sun7i_reg_offs __initconst = {
+	.ctrl	= SUN7I_NMI_CTRL,
+	.pend	= SUN7I_NMI_PENDING,
+	.enable	= SUN7I_NMI_ENABLE,
+};
+
+static const struct sunxi_sc_nmi_reg_offs sun9i_reg_offs __initconst = {
+	.ctrl	= SUN9I_NMI_CTRL,
+	.pend	= SUN9I_NMI_PENDING,
+	.enable	= SUN9I_NMI_ENABLE,
 };
 
 static inline void sunxi_sc_nmi_write(struct irq_chip_generic *gc, u32 off,
@@ -128,7 +157,7 @@ static int sunxi_sc_nmi_set_type(struct irq_data *data, unsigned int flow_type)
 }
 
 static int __init sunxi_sc_nmi_irq_init(struct device_node *node,
-					struct sunxi_sc_nmi_reg_offs *reg_offs)
+					const struct sunxi_sc_nmi_reg_offs *reg_offs)
 {
 	struct irq_domain *domain;
 	struct irq_chip_generic *gc;
@@ -187,8 +216,11 @@ static int __init sunxi_sc_nmi_irq_init(struct device_node *node,
 	gc->chip_types[1].regs.type		= reg_offs->ctrl;
 	gc->chip_types[1].handler		= handle_edge_irq;
 
+	/* Disable any active interrupts */
 	sunxi_sc_nmi_write(gc, reg_offs->enable, 0);
-	sunxi_sc_nmi_write(gc, reg_offs->pend, 0x1);
+
+	/* Clear any pending NMI interrupts */
+	sunxi_sc_nmi_write(gc, reg_offs->pend, SUNXI_NMI_IRQ_BIT);
 
 	irq_set_chained_handler_and_data(irq, sunxi_sc_nmi_handle_irq, domain);
 
@@ -200,6 +232,14 @@ fail_irqd_remove:
 	return ret;
 }
 
+static int __init sun6i_r_intc_irq_init(struct device_node *node,
+					struct device_node *parent)
+{
+	return sunxi_sc_nmi_irq_init(node, &sun6i_r_intc_reg_offs);
+}
+IRQCHIP_DECLARE(sun6i_r_intc, "allwinner,sun6i-a31-r-intc",
+		sun6i_r_intc_irq_init);
+
 static int __init sun6i_sc_nmi_irq_init(struct device_node *node,
 					struct device_node *parent)
 {
diff --git a/drivers/irqchip/irq-xtensa-mx.c b/drivers/irqchip/irq-xtensa-mx.c
index bb3ac5fe5846..72a391e01011 100644
--- a/drivers/irqchip/irq-xtensa-mx.c
+++ b/drivers/irqchip/irq-xtensa-mx.c
@@ -142,7 +142,7 @@ static struct irq_chip xtensa_mx_irq_chip = {
 int __init xtensa_mx_init_legacy(struct device_node *interrupt_parent)
 {
 	struct irq_domain *root_domain =
-		irq_domain_add_legacy(NULL, NR_IRQS, 0, 0,
+		irq_domain_add_legacy(NULL, NR_IRQS - 1, 1, 0,
 				&xtensa_mx_irq_domain_ops,
 				&xtensa_mx_irq_chip);
 	irq_set_default_host(root_domain);
diff --git a/drivers/irqchip/irq-xtensa-pic.c b/drivers/irqchip/irq-xtensa-pic.c
index 472ae1770964..f728755fa292 100644
--- a/drivers/irqchip/irq-xtensa-pic.c
+++ b/drivers/irqchip/irq-xtensa-pic.c
@@ -89,7 +89,7 @@ static struct irq_chip xtensa_irq_chip = {
 int __init xtensa_pic_init_legacy(struct device_node *interrupt_parent)
 {
 	struct irq_domain *root_domain =
-		irq_domain_add_legacy(NULL, NR_IRQS, 0, 0,
+		irq_domain_add_legacy(NULL, NR_IRQS - 1, 1, 0,
 				&xtensa_irq_domain_ops, &xtensa_irq_chip);
 	irq_set_default_host(root_domain);
 	return 0;
diff --git a/drivers/irqchip/qcom-irq-combiner.c b/drivers/irqchip/qcom-irq-combiner.c
index 226558698344..6aa3ea479214 100644
--- a/drivers/irqchip/qcom-irq-combiner.c
+++ b/drivers/irqchip/qcom-irq-combiner.c
@@ -288,9 +288,4 @@ static struct platform_driver qcom_irq_combiner_probe = {
 	},
 	.probe = combiner_probe,
 };
-
-static int __init register_qcom_irq_combiner(void)
-{
-	return platform_driver_register(&qcom_irq_combiner_probe);
-}
-device_initcall(register_qcom_irq_combiner);
+builtin_platform_driver(qcom_irq_combiner_probe);
diff --git a/drivers/isdn/capi/capi.c b/drivers/isdn/capi/capi.c
index 6a2df3297e77..dde8f46bc254 100644
--- a/drivers/isdn/capi/capi.c
+++ b/drivers/isdn/capi/capi.c
@@ -1058,7 +1058,7 @@ static int capinc_tty_write(struct tty_struct *tty,
 	}
 
 	skb_reserve(skb, CAPI_DATA_B3_REQ_LEN);
-	memcpy(skb_put(skb, count), buf, count);
+	skb_put_data(skb, buf, count);
 
 	__skb_queue_tail(&mp->outqueue, skb);
 	mp->outbytes += skb->len;
@@ -1082,7 +1082,7 @@ static int capinc_tty_put_char(struct tty_struct *tty, unsigned char ch)
 	skb = mp->outskb;
 	if (skb) {
 		if (skb_tailroom(skb) > 0) {
-			*(skb_put(skb, 1)) = ch;
+			skb_put_u8(skb, ch);
 			goto unlock_out;
 		}
 		mp->outskb = NULL;
@@ -1094,7 +1094,7 @@ static int capinc_tty_put_char(struct tty_struct *tty, unsigned char ch)
 	skb = alloc_skb(CAPI_DATA_B3_REQ_LEN + CAPI_MAX_BLKSIZE, GFP_ATOMIC);
 	if (skb) {
 		skb_reserve(skb, CAPI_DATA_B3_REQ_LEN);
-		*(skb_put(skb, 1)) = ch;
+		skb_put_u8(skb, ch);
 		mp->outskb = skb;
 	} else {
 		printk(KERN_ERR "capinc_put_char: char %u lost\n", ch);
diff --git a/drivers/isdn/capi/capidrv.c b/drivers/isdn/capi/capidrv.c
index 85cfa4f8691f..89dd1303a98a 100644
--- a/drivers/isdn/capi/capidrv.c
+++ b/drivers/isdn/capi/capidrv.c
@@ -516,7 +516,7 @@ static void send_message(capidrv_contr *card, _cmsg *cmsg)
 		printk(KERN_ERR "capidrv::send_message: can't allocate mem\n");
 		return;
 	}
-	memcpy(skb_put(skb, len), cmsg->buf, len);
+	skb_put_data(skb, cmsg->buf, len);
 	if (capi20_put_message(&global.ap, skb) != CAPI_NOERROR)
 		kfree_skb(skb);
 }
diff --git a/drivers/isdn/gigaset/asyncdata.c b/drivers/isdn/gigaset/asyncdata.c
index c90dca5abeac..bc208557f783 100644
--- a/drivers/isdn/gigaset/asyncdata.c
+++ b/drivers/isdn/gigaset/asyncdata.c
@@ -264,7 +264,7 @@ byte_stuff:
 				/* skip remainder of packet */
 				bcs->rx_skb = skb = NULL;
 			} else {
-				*__skb_put(skb, 1) = c;
+				__skb_put_u8(skb, c);
 				fcs = crc_ccitt_byte(fcs, c);
 			}
 		}
@@ -315,7 +315,7 @@ static unsigned iraw_loop(unsigned numbytes, struct inbuf_t *inbuf)
 
 		/* regular data byte: append to current skb */
 		inputstate |= INS_have_data;
-		*__skb_put(skb, 1) = bitrev8(c);
+		__skb_put_u8(skb, bitrev8(c));
 	}
 
 	/* pass data up */
@@ -492,33 +492,33 @@ static struct sk_buff *HDLC_Encode(struct sk_buff *skb)
 	hdlc_skb->mac_len = skb->mac_len;
 
 	/* Add flag sequence in front of everything.. */
-	*(skb_put(hdlc_skb, 1)) = PPP_FLAG;
+	skb_put_u8(hdlc_skb, PPP_FLAG);
 
 	/* Perform byte stuffing while copying data. */
 	while (skb->len--) {
 		if (muststuff(*skb->data)) {
-			*(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
-			*(skb_put(hdlc_skb, 1)) = (*skb->data++) ^ PPP_TRANS;
+			skb_put_u8(hdlc_skb, PPP_ESCAPE);
+			skb_put_u8(hdlc_skb, (*skb->data++) ^ PPP_TRANS);
 		} else
-			*(skb_put(hdlc_skb, 1)) = *skb->data++;
+			skb_put_u8(hdlc_skb, *skb->data++);
 	}
 
 	/* Finally add FCS (byte stuffed) and flag sequence */
 	c = (fcs & 0x00ff);	/* least significant byte first */
 	if (muststuff(c)) {
-		*(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
+		skb_put_u8(hdlc_skb, PPP_ESCAPE);
 		c ^= PPP_TRANS;
 	}
-	*(skb_put(hdlc_skb, 1)) = c;
+	skb_put_u8(hdlc_skb, c);
 
 	c = ((fcs >> 8) & 0x00ff);
 	if (muststuff(c)) {
-		*(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
+		skb_put_u8(hdlc_skb, PPP_ESCAPE);
 		c ^= PPP_TRANS;
 	}
-	*(skb_put(hdlc_skb, 1)) = c;
+	skb_put_u8(hdlc_skb, c);
 
-	*(skb_put(hdlc_skb, 1)) = PPP_FLAG;
+	skb_put_u8(hdlc_skb, PPP_FLAG);
 
 	dev_kfree_skb_any(skb);
 	return hdlc_skb;
@@ -561,8 +561,8 @@ static struct sk_buff *iraw_encode(struct sk_buff *skb)
 	while (len--) {
 		c = bitrev8(*cp++);
 		if (c == DLE_FLAG)
-			*(skb_put(iraw_skb, 1)) = c;
-		*(skb_put(iraw_skb, 1)) = c;
+			skb_put_u8(iraw_skb, c);
+		skb_put_u8(iraw_skb, c);
 	}
 	dev_kfree_skb_any(skb);
 	return iraw_skb;
diff --git a/drivers/isdn/gigaset/isocdata.c b/drivers/isdn/gigaset/isocdata.c
index bc29f1d52a2f..97e00118ccfe 100644
--- a/drivers/isdn/gigaset/isocdata.c
+++ b/drivers/isdn/gigaset/isocdata.c
@@ -511,7 +511,7 @@ static inline void hdlc_putbyte(unsigned char c, struct bc_state *bcs)
 		bcs->rx_skb = NULL;
 		return;
 	}
-	*__skb_put(bcs->rx_skb, 1) = c;
+	__skb_put_u8(bcs->rx_skb, c);
 }
 
 /* hdlc_flush
diff --git a/drivers/isdn/hardware/avm/b1.c b/drivers/isdn/hardware/avm/b1.c
index 9fdbd99c7547..b1833d08a5fe 100644
--- a/drivers/isdn/hardware/avm/b1.c
+++ b/drivers/isdn/hardware/avm/b1.c
@@ -529,8 +529,8 @@ irqreturn_t b1_interrupt(int interrupt, void *devptr)
 			printk(KERN_ERR "%s: incoming packet dropped\n",
 			       card->name);
 		} else {
-			memcpy(skb_put(skb, MsgLen), card->msgbuf, MsgLen);
-			memcpy(skb_put(skb, DataB3Len), card->databuf, DataB3Len);
+			skb_put_data(skb, card->msgbuf, MsgLen);
+			skb_put_data(skb, card->databuf, DataB3Len);
 			capi_ctr_handle_message(ctrl, ApplId, skb);
 		}
 		break;
@@ -544,7 +544,7 @@ irqreturn_t b1_interrupt(int interrupt, void *devptr)
 			       card->name);
 			spin_unlock_irqrestore(&card->lock, flags);
 		} else {
-			memcpy(skb_put(skb, MsgLen), card->msgbuf, MsgLen);
+			skb_put_data(skb, card->msgbuf, MsgLen);
 			if (CAPIMSG_CMD(skb->data) == CAPI_DATA_B3_CONF)
 				capilib_data_b3_conf(&cinfo->ncci_head, ApplId,
 						     CAPIMSG_NCCI(skb->data),
diff --git a/drivers/isdn/hardware/avm/b1dma.c b/drivers/isdn/hardware/avm/b1dma.c
index 818bd8f231db..9538a9e5e1a8 100644
--- a/drivers/isdn/hardware/avm/b1dma.c
+++ b/drivers/isdn/hardware/avm/b1dma.c
@@ -474,8 +474,8 @@ static void b1dma_handle_rx(avmcard *card)
 			printk(KERN_ERR "%s: incoming packet dropped\n",
 			       card->name);
 		} else {
-			memcpy(skb_put(skb, MsgLen), card->msgbuf, MsgLen);
-			memcpy(skb_put(skb, DataB3Len), card->databuf, DataB3Len);
+			skb_put_data(skb, card->msgbuf, MsgLen);
+			skb_put_data(skb, card->databuf, DataB3Len);
 			capi_ctr_handle_message(ctrl, ApplId, skb);
 		}
 		break;
@@ -488,7 +488,7 @@ static void b1dma_handle_rx(avmcard *card)
 			printk(KERN_ERR "%s: incoming packet dropped\n",
 			       card->name);
 		} else {
-			memcpy(skb_put(skb, MsgLen), card->msgbuf, MsgLen);
+			skb_put_data(skb, card->msgbuf, MsgLen);
 			if (CAPIMSG_CMD(skb->data) == CAPI_DATA_B3_CONF) {
 				spin_lock(&card->lock);
 				capilib_data_b3_conf(&cinfo->ncci_head, ApplId,
diff --git a/drivers/isdn/hardware/avm/c4.c b/drivers/isdn/hardware/avm/c4.c
index 17beb2869dc1..40c7e2cf423b 100644
--- a/drivers/isdn/hardware/avm/c4.c
+++ b/drivers/isdn/hardware/avm/c4.c
@@ -536,8 +536,8 @@ static void c4_handle_rx(avmcard *card)
 			printk(KERN_ERR "%s: incoming packet dropped\n",
 			       card->name);
 		} else {
-			memcpy(skb_put(skb, MsgLen), card->msgbuf, MsgLen);
-			memcpy(skb_put(skb, DataB3Len), card->databuf, DataB3Len);
+			skb_put_data(skb, card->msgbuf, MsgLen);
+			skb_put_data(skb, card->databuf, DataB3Len);
 			capi_ctr_handle_message(ctrl, ApplId, skb);
 		}
 		break;
@@ -555,7 +555,7 @@ static void c4_handle_rx(avmcard *card)
 			printk(KERN_ERR "%s: incoming packet dropped\n",
 			       card->name);
 		} else {
-			memcpy(skb_put(skb, MsgLen), card->msgbuf, MsgLen);
+			skb_put_data(skb, card->msgbuf, MsgLen);
 			if (CAPIMSG_CMD(skb->data) == CAPI_DATA_B3_CONF)
 				capilib_data_b3_conf(&cinfo->ncci_head, ApplId,
 						     CAPIMSG_NCCI(skb->data),
diff --git a/drivers/isdn/hardware/avm/t1isa.c b/drivers/isdn/hardware/avm/t1isa.c
index 9516203c735f..9f80d20ced87 100644
--- a/drivers/isdn/hardware/avm/t1isa.c
+++ b/drivers/isdn/hardware/avm/t1isa.c
@@ -171,8 +171,8 @@ static irqreturn_t t1isa_interrupt(int interrupt, void *devptr)
 				printk(KERN_ERR "%s: incoming packet dropped\n",
 				       card->name);
 			} else {
-				memcpy(skb_put(skb, MsgLen), card->msgbuf, MsgLen);
-				memcpy(skb_put(skb, DataB3Len), card->databuf, DataB3Len);
+				skb_put_data(skb, card->msgbuf, MsgLen);
+				skb_put_data(skb, card->databuf, DataB3Len);
 				capi_ctr_handle_message(ctrl, ApplId, skb);
 			}
 			break;
@@ -186,7 +186,7 @@ static irqreturn_t t1isa_interrupt(int interrupt, void *devptr)
 				printk(KERN_ERR "%s: incoming packet dropped\n",
 				       card->name);
 			} else {
-				memcpy(skb_put(skb, MsgLen), card->msgbuf, MsgLen);
+				skb_put_data(skb, card->msgbuf, MsgLen);
 				if (CAPIMSG_CMD(skb->data) == CAPI_DATA_B3)
 					capilib_data_b3_conf(&cinfo->ncci_head, ApplId,
 							     CAPIMSG_NCCI(skb->data),
diff --git a/drivers/isdn/hardware/mISDN/hfcmulti.c b/drivers/isdn/hardware/mISDN/hfcmulti.c
index 961c07ee47b7..aea0c9616ea5 100644
--- a/drivers/isdn/hardware/mISDN/hfcmulti.c
+++ b/drivers/isdn/hardware/mISDN/hfcmulti.c
@@ -1926,7 +1926,7 @@ hfcmulti_dtmf(struct hfc_multi *hc)
 			hh = mISDN_HEAD_P(skb);
 			hh->prim = PH_CONTROL_IND;
 			hh->id = DTMF_HFC_COEF;
-			memcpy(skb_put(skb, 512), hc->chan[ch].coeff, 512);
+			skb_put_data(skb, hc->chan[ch].coeff, 512);
 			recv_Bchannel_skb(bch, skb);
 		}
 	}
@@ -2332,8 +2332,7 @@ next_frame:
 				skb = *sp;
 				*sp = mI_alloc_skb(skb->len, GFP_ATOMIC);
 				if (*sp) {
-					memcpy(skb_put(*sp, skb->len),
-					       skb->data, skb->len);
+					skb_put_data(*sp, skb->data, skb->len);
 					skb_trim(skb, 0);
 				} else {
 					printk(KERN_DEBUG "%s: No mem\n",
diff --git a/drivers/isdn/hardware/mISDN/hfcsusb.c b/drivers/isdn/hardware/mISDN/hfcsusb.c
index 114f3bcba1b0..17cc879ad2bb 100644
--- a/drivers/isdn/hardware/mISDN/hfcsusb.c
+++ b/drivers/isdn/hardware/mISDN/hfcsusb.c
@@ -893,7 +893,7 @@ hfcsusb_rx_frame(struct usb_fifo *fifo, __u8 *data, unsigned int len,
 		}
 	}
 
-	memcpy(skb_put(rx_skb, len), data, len);
+	skb_put_data(rx_skb, data, len);
 
 	if (hdlc) {
 		/* we have a complete hdlc packet */
diff --git a/drivers/isdn/hardware/mISDN/mISDNipac.c b/drivers/isdn/hardware/mISDN/mISDNipac.c
index 6742b0dc0821..e240010b93fa 100644
--- a/drivers/isdn/hardware/mISDN/mISDNipac.c
+++ b/drivers/isdn/hardware/mISDN/mISDNipac.c
@@ -364,8 +364,8 @@ afterMONR1:
 			WriteISAC(isac, ISAC_MOCR, isac->mocr);
 			if (isac->mon_txc && (isac->mon_txp >= isac->mon_txc)) {
 				if (isac->monitor)
-					ret = isac->monitor(isac->dch.hw,
-							    MONITOR_TX_0, NULL, 0);
+					isac->monitor(isac->dch.hw,
+						      MONITOR_TX_0, NULL, 0);
 			}
 			kfree(isac->mon_tx);
 			isac->mon_tx = NULL;
@@ -375,8 +375,8 @@ afterMONR1:
 		}
 		if (isac->mon_txc && (isac->mon_txp >= isac->mon_txc)) {
 			if (isac->monitor)
-				ret = isac->monitor(isac->dch.hw,
-						    MONITOR_TX_0, NULL, 0);
+				isac->monitor(isac->dch.hw,
+					      MONITOR_TX_0, NULL, 0);
 			kfree(isac->mon_tx);
 			isac->mon_tx = NULL;
 			isac->mon_txc = 0;
@@ -397,8 +397,8 @@ AfterMOX0:
 			WriteISAC(isac, ISAC_MOCR, isac->mocr);
 			if (isac->mon_txc && (isac->mon_txp >= isac->mon_txc)) {
 				if (isac->monitor)
-					ret = isac->monitor(isac->dch.hw,
-							    MONITOR_TX_1, NULL, 0);
+					isac->monitor(isac->dch.hw,
+						      MONITOR_TX_1, NULL, 0);
 			}
 			kfree(isac->mon_tx);
 			isac->mon_tx = NULL;
@@ -408,8 +408,8 @@ AfterMOX0:
 		}
 		if (isac->mon_txc && (isac->mon_txp >= isac->mon_txc)) {
 			if (isac->monitor)
-				ret = isac->monitor(isac->dch.hw,
-						    MONITOR_TX_1, NULL, 0);
+				isac->monitor(isac->dch.hw,
+					      MONITOR_TX_1, NULL, 0);
 			kfree(isac->mon_tx);
 			isac->mon_tx = NULL;
 			isac->mon_txc = 0;
diff --git a/drivers/isdn/hisax/amd7930_fn.c b/drivers/isdn/hisax/amd7930_fn.c
index 3a4c2f9e19e9..dcf4c2a9fcea 100644
--- a/drivers/isdn/hisax/amd7930_fn.c
+++ b/drivers/isdn/hisax/amd7930_fn.c
@@ -317,7 +317,8 @@ Amd7930_empty_Dfifo(struct IsdnCardState *cs, int flag)
 							debugl1(cs, "%s", cs->dlog);
 						}
 						/* moves received data in sk-buffer */
-						memcpy(skb_put(skb, cs->rcvidx), cs->rcvbuf, cs->rcvidx);
+						skb_put_data(skb, cs->rcvbuf,
+							     cs->rcvidx);
 						skb_queue_tail(&cs->rq, skb);
 					}
 				}
diff --git a/drivers/isdn/hisax/avm_pci.c b/drivers/isdn/hisax/avm_pci.c
index d1427bd6452d..daf3742cdef6 100644
--- a/drivers/isdn/hisax/avm_pci.c
+++ b/drivers/isdn/hisax/avm_pci.c
@@ -378,8 +378,9 @@ HDLC_irq(struct BCState *bcs, u_int stat) {
 					if (!(skb = dev_alloc_skb(bcs->hw.hdlc.rcvidx)))
 						printk(KERN_WARNING "HDLC: receive out of memory\n");
 					else {
-						memcpy(skb_put(skb, bcs->hw.hdlc.rcvidx),
-						       bcs->hw.hdlc.rcvbuf, bcs->hw.hdlc.rcvidx);
+						skb_put_data(skb,
+							     bcs->hw.hdlc.rcvbuf,
+							     bcs->hw.hdlc.rcvidx);
 						skb_queue_tail(&bcs->rqueue, skb);
 					}
 					bcs->hw.hdlc.rcvidx = 0;
diff --git a/drivers/isdn/hisax/diva.c b/drivers/isdn/hisax/diva.c
index 079336e593f9..3fc94e7741ae 100644
--- a/drivers/isdn/hisax/diva.c
+++ b/drivers/isdn/hisax/diva.c
@@ -511,7 +511,8 @@ Memhscx_interrupt(struct IsdnCardState *cs, u_char val, u_char hscx)
 				if (!(skb = dev_alloc_skb(count)))
 					printk(KERN_WARNING "HSCX: receive out of memory\n");
 				else {
-					memcpy(skb_put(skb, count), bcs->hw.hscx.rcvbuf, count);
+					skb_put_data(skb, bcs->hw.hscx.rcvbuf,
+						     count);
 					skb_queue_tail(&bcs->rqueue, skb);
 				}
 			}
@@ -526,7 +527,8 @@ Memhscx_interrupt(struct IsdnCardState *cs, u_char val, u_char hscx)
 			if (!(skb = dev_alloc_skb(fifo_size)))
 				printk(KERN_WARNING "HiSax: receive out of memory\n");
 			else {
-				memcpy(skb_put(skb, fifo_size), bcs->hw.hscx.rcvbuf, fifo_size);
+				skb_put_data(skb, bcs->hw.hscx.rcvbuf,
+					     fifo_size);
 				skb_queue_tail(&bcs->rqueue, skb);
 			}
 			bcs->hw.hscx.rcvidx = 0;
diff --git a/drivers/isdn/hisax/elsa_ser.c b/drivers/isdn/hisax/elsa_ser.c
index a2a358c1dc8e..999effd7a276 100644
--- a/drivers/isdn/hisax/elsa_ser.c
+++ b/drivers/isdn/hisax/elsa_ser.c
@@ -333,8 +333,8 @@ static inline void receive_chars(struct IsdnCardState *cs,
 		if (!(skb = dev_alloc_skb(cs->hw.elsa.rcvcnt)))
 			printk(KERN_WARNING "ElsaSER: receive out of memory\n");
 		else {
-			memcpy(skb_put(skb, cs->hw.elsa.rcvcnt), cs->hw.elsa.rcvbuf,
-			       cs->hw.elsa.rcvcnt);
+			skb_put_data(skb, cs->hw.elsa.rcvbuf,
+				     cs->hw.elsa.rcvcnt);
 			skb_queue_tail(&cs->hw.elsa.bcs->rqueue, skb);
 		}
 		schedule_event(cs->hw.elsa.bcs, B_RCVBUFREADY);
diff --git a/drivers/isdn/hisax/hfc_usb.c b/drivers/isdn/hisax/hfc_usb.c
index 6dbd1f1da14f..ef4748083efd 100644
--- a/drivers/isdn/hisax/hfc_usb.c
+++ b/drivers/isdn/hisax/hfc_usb.c
@@ -799,7 +799,7 @@ collect_rx_frame(usb_fifo *fifo, __u8 *data, int len, int finish)
 	}
 	if (len) {
 		if (fifo->skbuff->len + len < fifo->max_size) {
-			memcpy(skb_put(fifo->skbuff, len), data, len);
+			skb_put_data(fifo->skbuff, data, len);
 		} else {
 			DBG(HFCUSB_DBG_FIFO_ERR,
 			    "HCF-USB: got frame exceeded fifo->max_size(%d) fifo(%d)",
diff --git a/drivers/isdn/hisax/hisax_fcpcipnp.c b/drivers/isdn/hisax/hisax_fcpcipnp.c
index 5e8a5d967162..5a9f39ed1d5d 100644
--- a/drivers/isdn/hisax/hisax_fcpcipnp.c
+++ b/drivers/isdn/hisax/hisax_fcpcipnp.c
@@ -495,8 +495,7 @@ static inline void hdlc_rpr_irq(struct fritz_bcs *bcs, u32 stat)
 			if (!skb) {
 				printk(KERN_WARNING "HDLC: receive out of memory\n");
 			} else {
-				memcpy(skb_put(skb, bcs->rcvidx), bcs->rcvbuf,
-				       bcs->rcvidx);
+				skb_put_data(skb, bcs->rcvbuf, bcs->rcvidx);
 				DBG_SKB(1, skb);
 				B_L1L2(bcs, PH_DATA | INDICATION, skb);
 			}
diff --git a/drivers/isdn/hisax/hisax_isac.c b/drivers/isdn/hisax/hisax_isac.c
index 5154c252a25f..0f36375478c5 100644
--- a/drivers/isdn/hisax/hisax_isac.c
+++ b/drivers/isdn/hisax/hisax_isac.c
@@ -557,7 +557,7 @@ static inline void isac_rme_interrupt(struct isac *isac)
 		DBG(DBG_WARN, "no memory, dropping\n");
 		goto out;
 	}
-	memcpy(skb_put(skb, count), isac->rcvbuf, count);
+	skb_put_data(skb, isac->rcvbuf, count);
 	DBG_SKB(DBG_RPACKET, skb);
 	D_L1L2(isac, PH_DATA | INDICATION, skb);
 out:
@@ -687,7 +687,7 @@ static inline void isacsx_rme_interrupt(struct isac *isac)
 		DBG(DBG_WARN, "no memory, dropping");
 		goto out;
 	}
-	memcpy(skb_put(skb, count), isac->rcvbuf, count);
+	skb_put_data(skb, isac->rcvbuf, count);
 	DBG_SKB(DBG_RPACKET, skb);
 	D_L1L2(isac, PH_DATA | INDICATION, skb);
 out:
diff --git a/drivers/isdn/hisax/hscx_irq.c b/drivers/isdn/hisax/hscx_irq.c
index a8d6188402c6..0d7e783c8bef 100644
--- a/drivers/isdn/hisax/hscx_irq.c
+++ b/drivers/isdn/hisax/hscx_irq.c
@@ -169,7 +169,8 @@ hscx_interrupt(struct IsdnCardState *cs, u_char val, u_char hscx)
 				if (!(skb = dev_alloc_skb(count)))
 					printk(KERN_WARNING "HSCX: receive out of memory\n");
 				else {
-					memcpy(skb_put(skb, count), bcs->hw.hscx.rcvbuf, count);
+					skb_put_data(skb, bcs->hw.hscx.rcvbuf,
+						     count);
 					skb_queue_tail(&bcs->rqueue, skb);
 				}
 			}
@@ -184,7 +185,8 @@ hscx_interrupt(struct IsdnCardState *cs, u_char val, u_char hscx)
 			if (!(skb = dev_alloc_skb(fifo_size)))
 				printk(KERN_WARNING "HiSax: receive out of memory\n");
 			else {
-				memcpy(skb_put(skb, fifo_size), bcs->hw.hscx.rcvbuf, fifo_size);
+				skb_put_data(skb, bcs->hw.hscx.rcvbuf,
+					     fifo_size);
 				skb_queue_tail(&bcs->rqueue, skb);
 			}
 			bcs->hw.hscx.rcvidx = 0;
diff --git a/drivers/isdn/hisax/icc.c b/drivers/isdn/hisax/icc.c
index c7c3797a817e..8d1804572b32 100644
--- a/drivers/isdn/hisax/icc.c
+++ b/drivers/isdn/hisax/icc.c
@@ -217,7 +217,7 @@ icc_interrupt(struct IsdnCardState *cs, u_char val)
 				if (!(skb = alloc_skb(count, GFP_ATOMIC)))
 					printk(KERN_WARNING "HiSax: D receive out of memory\n");
 				else {
-					memcpy(skb_put(skb, count), cs->rcvbuf, count);
+					skb_put_data(skb, cs->rcvbuf, count);
 					skb_queue_tail(&cs->rq, skb);
 				}
 			}
diff --git a/drivers/isdn/hisax/ipacx.c b/drivers/isdn/hisax/ipacx.c
index 43effe7082ed..c426b4fea28a 100644
--- a/drivers/isdn/hisax/ipacx.c
+++ b/drivers/isdn/hisax/ipacx.c
@@ -350,7 +350,7 @@ dch_int(struct IsdnCardState *cs)
 				if (!(skb = dev_alloc_skb(count)))
 					printk(KERN_WARNING "HiSax dch_int(): receive out of memory\n");
 				else {
-					memcpy(skb_put(skb, count), cs->rcvbuf, count);
+					skb_put_data(skb, cs->rcvbuf, count);
 					skb_queue_tail(&cs->rq, skb);
 				}
 			}
@@ -627,7 +627,8 @@ bch_int(struct IsdnCardState *cs, u_char hscx)
 				if (!(skb = dev_alloc_skb(count)))
 					printk(KERN_WARNING "HiSax bch_int(): receive frame out of memory\n");
 				else {
-					memcpy(skb_put(skb, count), bcs->hw.hscx.rcvbuf, count);
+					skb_put_data(skb, bcs->hw.hscx.rcvbuf,
+						     count);
 					skb_queue_tail(&bcs->rqueue, skb);
 				}
 			}
@@ -644,7 +645,8 @@ bch_int(struct IsdnCardState *cs, u_char hscx)
 			if (!(skb = dev_alloc_skb(B_FIFO_SIZE)))
 				printk(KERN_WARNING "HiSax bch_int(): receive transparent out of memory\n");
 			else {
-				memcpy(skb_put(skb, B_FIFO_SIZE), bcs->hw.hscx.rcvbuf, B_FIFO_SIZE);
+				skb_put_data(skb, bcs->hw.hscx.rcvbuf,
+					     B_FIFO_SIZE);
 				skb_queue_tail(&bcs->rqueue, skb);
 			}
 			bcs->hw.hscx.rcvidx = 0;
diff --git a/drivers/isdn/hisax/isac.c b/drivers/isdn/hisax/isac.c
index 4273b4548825..ea965f29a555 100644
--- a/drivers/isdn/hisax/isac.c
+++ b/drivers/isdn/hisax/isac.c
@@ -222,7 +222,7 @@ isac_interrupt(struct IsdnCardState *cs, u_char val)
 				if (!skb)
 					printk(KERN_WARNING "HiSax: D receive out of memory\n");
 				else {
-					memcpy(skb_put(skb, count), cs->rcvbuf, count);
+					skb_put_data(skb, cs->rcvbuf, count);
 					skb_queue_tail(&cs->rq, skb);
 				}
 			}
diff --git a/drivers/isdn/hisax/isar.c b/drivers/isdn/hisax/isar.c
index 0dc60b287c4b..98b4b67ea337 100644
--- a/drivers/isdn/hisax/isar.c
+++ b/drivers/isdn/hisax/isar.c
@@ -458,7 +458,7 @@ send_DLE_ETX(struct BCState *bcs)
 	struct sk_buff *skb;
 
 	if ((skb = dev_alloc_skb(2))) {
-		memcpy(skb_put(skb, 2), dleetx, 2);
+		skb_put_data(skb, dleetx, 2);
 		skb_queue_tail(&bcs->rqueue, skb);
 		schedule_event(bcs, B_RCVBUFREADY);
 	} else {
@@ -550,8 +550,8 @@ isar_rcv_frame(struct IsdnCardState *cs, struct BCState *bcs)
 				} else if (!(skb = dev_alloc_skb(bcs->hw.isar.rcvidx - 2))) {
 					printk(KERN_WARNING "ISAR: receive out of memory\n");
 				} else {
-					memcpy(skb_put(skb, bcs->hw.isar.rcvidx - 2),
-					       bcs->hw.isar.rcvbuf, bcs->hw.isar.rcvidx - 2);
+					skb_put_data(skb, bcs->hw.isar.rcvbuf,
+						     bcs->hw.isar.rcvidx - 2);
 					skb_queue_tail(&bcs->rqueue, skb);
 					schedule_event(bcs, B_RCVBUFREADY);
 				}
diff --git a/drivers/isdn/hisax/isdnl2.c b/drivers/isdn/hisax/isdnl2.c
index c53a53f6efb6..1a40ed04cb52 100644
--- a/drivers/isdn/hisax/isdnl2.c
+++ b/drivers/isdn/hisax/isdnl2.c
@@ -433,7 +433,7 @@ send_uframe(struct PStack *st, u_char cmd, u_char cr)
 		printk(KERN_WARNING "isdl2 can't alloc sbbuff for send_uframe\n");
 		return;
 	}
-	memcpy(skb_put(skb, i), tmp, i);
+	skb_put_data(skb, tmp, i);
 	enqueue_super(st, skb);
 }
 
@@ -894,7 +894,7 @@ enquiry_cr(struct PStack *st, u_char typ, u_char cr, u_char pf)
 		printk(KERN_WARNING "isdl2 can't alloc sbbuff for enquiry_cr\n");
 		return;
 	}
-	memcpy(skb_put(skb, i), tmp, i);
+	skb_put_data(skb, tmp, i);
 	enqueue_super(st, skb);
 }
 
diff --git a/drivers/isdn/hisax/jade_irq.c b/drivers/isdn/hisax/jade_irq.c
index b930da9b5aa6..a89e2df911c5 100644
--- a/drivers/isdn/hisax/jade_irq.c
+++ b/drivers/isdn/hisax/jade_irq.c
@@ -147,7 +147,8 @@ jade_interrupt(struct IsdnCardState *cs, u_char val, u_char jade)
 				if (!(skb = dev_alloc_skb(count)))
 					printk(KERN_WARNING "JADE %s receive out of memory\n", (jade ? "B" : "A"));
 				else {
-					memcpy(skb_put(skb, count), bcs->hw.hscx.rcvbuf, count);
+					skb_put_data(skb, bcs->hw.hscx.rcvbuf,
+						     count);
 					skb_queue_tail(&bcs->rqueue, skb);
 				}
 			}
@@ -162,7 +163,8 @@ jade_interrupt(struct IsdnCardState *cs, u_char val, u_char jade)
 			if (!(skb = dev_alloc_skb(fifo_size)))
 				printk(KERN_WARNING "HiSax: receive out of memory\n");
 			else {
-				memcpy(skb_put(skb, fifo_size), bcs->hw.hscx.rcvbuf, fifo_size);
+				skb_put_data(skb, bcs->hw.hscx.rcvbuf,
+					     fifo_size);
 				skb_queue_tail(&bcs->rqueue, skb);
 			}
 			bcs->hw.hscx.rcvidx = 0;
diff --git a/drivers/isdn/hisax/l3_1tr6.c b/drivers/isdn/hisax/l3_1tr6.c
index 875402e76d0a..da0a1c6aa329 100644
--- a/drivers/isdn/hisax/l3_1tr6.c
+++ b/drivers/isdn/hisax/l3_1tr6.c
@@ -149,7 +149,7 @@ l3_1tr6_setup_req(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	L3DelTimer(&pc->timer);
 	L3AddTimer(&pc->timer, T303, CC_T303);
 	newl3state(pc, 1);
@@ -497,7 +497,7 @@ l3_1tr6_setup_rsp(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	L3DelTimer(&pc->timer);
 	L3AddTimer(&pc->timer, T313, CC_T313);
@@ -543,7 +543,7 @@ l3_1tr6_disconnect_req(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	L3AddTimer(&pc->timer, T305, CC_T305);
 }
@@ -602,7 +602,7 @@ l3_1tr6_t305(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	L3AddTimer(&pc->timer, T308, CC_T308_1);
 }
diff --git a/drivers/isdn/hisax/l3dss1.c b/drivers/isdn/hisax/l3dss1.c
index cda700664e9c..18a3484b1f7e 100644
--- a/drivers/isdn/hisax/l3dss1.c
+++ b/drivers/isdn/hisax/l3dss1.c
@@ -525,7 +525,7 @@ l3dss1_message_cause(struct l3_process *pc, u_char mt, u_char cause)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 }
 
@@ -551,7 +551,7 @@ l3dss1_status_send(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 }
 
@@ -587,7 +587,7 @@ l3dss1_msg_without_setup(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	dss1_release_l3_process(pc);
 }
@@ -944,7 +944,7 @@ l3dss1_msg_with_uus(struct l3_process *pc, u_char cmd)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 } /* l3dss1_msg_with_uus */
 
@@ -1420,7 +1420,7 @@ l3dss1_setup_req(struct l3_process *pc, u_char pr,
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	L3DelTimer(&pc->timer);
 	L3AddTimer(&pc->timer, T303, CC_T303);
 	newl3state(pc, 1);
@@ -1786,7 +1786,7 @@ l3dss1_disconnect_req(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	newl3state(pc, 11);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	L3AddTimer(&pc->timer, T305, CC_T305);
@@ -1848,7 +1848,7 @@ l3dss1_reject_req(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	pc->st->l3.l3l4(pc->st, CC_RELEASE | INDICATION, pc);
 	newl3state(pc, 0);
@@ -2145,7 +2145,7 @@ static void l3dss1_redir_req(struct l3_process *pc, u_char pr, void *arg)
 
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l))) return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 } /* l3dss1_redir_req */
@@ -2216,7 +2216,7 @@ static int l3dss1_cmd_global(struct PStack *st, isdn_ctrl *ic)
 				if (pc) dss1_release_l3_process(pc);
 				return (-2);
 			}
-			memcpy(skb_put(skb, l), temp, l);
+			skb_put_data(skb, temp, l);
 
 			if (pc)
 			{ pc->prot.dss1.invoke_id = id; /* remember id */
@@ -2359,7 +2359,7 @@ l3dss1_t305(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	newl3state(pc, 19);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	L3AddTimer(&pc->timer, T308, CC_T308_1);
@@ -2528,7 +2528,7 @@ l3dss1_suspend_req(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	newl3state(pc, 15);
 	L3AddTimer(&pc->timer, T319, CC_T319);
@@ -2603,7 +2603,7 @@ l3dss1_resume_req(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	newl3state(pc, 17);
 	L3AddTimer(&pc->timer, T318, CC_T318);
@@ -2721,7 +2721,7 @@ l3dss1_global_restart(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	newl3state(pc, 0);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 }
@@ -2929,7 +2929,7 @@ global_handler(struct PStack *st, int mt, struct sk_buff *skb)
 		l = p - tmp;
 		if (!(skb = l3_alloc_skb(l)))
 			return;
-		memcpy(skb_put(skb, l), tmp, l);
+		skb_put_data(skb, tmp, l);
 		l3_msg(proc->st, DL_DATA | REQUEST, skb);
 	} else {
 		if (st->l3.debug & L3_DEB_STATE) {
diff --git a/drivers/isdn/hisax/l3ni1.c b/drivers/isdn/hisax/l3ni1.c
index 8dc791bfaa6f..ea311e7df48e 100644
--- a/drivers/isdn/hisax/l3ni1.c
+++ b/drivers/isdn/hisax/l3ni1.c
@@ -454,7 +454,7 @@ l3ni1_message_plus_chid(struct l3_process *pc, u_char mt)
 
 	if (!(skb = l3_alloc_skb(7)))
 		return;
-	memcpy(skb_put(skb, 7), tmp, 7);
+	skb_put_data(skb, tmp, 7);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 }
 
@@ -475,7 +475,7 @@ l3ni1_message_cause(struct l3_process *pc, u_char mt, u_char cause)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 }
 
@@ -501,7 +501,7 @@ l3ni1_status_send(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 }
 
@@ -537,7 +537,7 @@ l3ni1_msg_without_setup(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	ni1_release_l3_process(pc);
 }
@@ -894,7 +894,7 @@ l3ni1_msg_with_uus(struct l3_process *pc, u_char cmd)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 } /* l3ni1_msg_with_uus */
 
@@ -1274,7 +1274,7 @@ l3ni1_setup_req(struct l3_process *pc, u_char pr,
 	{
 		return;
 	}
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	L3DelTimer(&pc->timer);
 	L3AddTimer(&pc->timer, T303, CC_T303);
 	newl3state(pc, 1);
@@ -1640,7 +1640,7 @@ l3ni1_disconnect_req(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	newl3state(pc, 11);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	L3AddTimer(&pc->timer, T305, CC_T305);
@@ -1704,7 +1704,7 @@ l3ni1_reject_req(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	pc->st->l3.l3l4(pc->st, CC_RELEASE | INDICATION, pc);
 	newl3state(pc, 0);
@@ -2001,7 +2001,7 @@ static void l3ni1_redir_req(struct l3_process *pc, u_char pr, void *arg)
 
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l))) return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 } /* l3ni1_redir_req */
@@ -2076,7 +2076,7 @@ static int l3ni1_cmd_global(struct PStack *st, isdn_ctrl *ic)
 				if (pc) ni1_release_l3_process(pc);
 				return (-2);
 			}
-			memcpy(skb_put(skb, l), temp, l);
+			skb_put_data(skb, temp, l);
 
 			if (pc)
 			{ pc->prot.ni1.invoke_id = id; /* remember id */
@@ -2219,7 +2219,7 @@ l3ni1_t305(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	newl3state(pc, 19);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	L3AddTimer(&pc->timer, T308, CC_T308_1);
@@ -2388,7 +2388,7 @@ l3ni1_suspend_req(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	newl3state(pc, 15);
 	L3AddTimer(&pc->timer, T319, CC_T319);
@@ -2463,7 +2463,7 @@ l3ni1_resume_req(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 	newl3state(pc, 17);
 	L3AddTimer(&pc->timer, T318, CC_T318);
@@ -2582,7 +2582,7 @@ l3ni1_global_restart(struct l3_process *pc, u_char pr, void *arg)
 	l = p - tmp;
 	if (!(skb = l3_alloc_skb(l)))
 		return;
-	memcpy(skb_put(skb, l), tmp, l);
+	skb_put_data(skb, tmp, l);
 	newl3state(pc, 0);
 	l3_msg(pc->st, DL_DATA | REQUEST, skb);
 }
@@ -2655,7 +2655,7 @@ static void l3ni1_SendSpid(struct l3_process *pc, u_char pr, struct sk_buff *skb
 	*p++ = IE_SPID;
 	*p++ = l;
 
-	memcpy(skb_put(skb, l), pSPID, l);
+	skb_put_data(skb, pSPID, l);
 
 	newl3state(pc, iNewState);
 
@@ -2873,7 +2873,7 @@ global_handler(struct PStack *st, int mt, struct sk_buff *skb)
 		l = p - tmp;
 		if (!(skb = l3_alloc_skb(l)))
 			return;
-		memcpy(skb_put(skb, l), tmp, l);
+		skb_put_data(skb, tmp, l);
 		l3_msg(proc->st, DL_DATA | REQUEST, skb);
 	} else {
 		if (st->l3.debug & L3_DEB_STATE) {
diff --git a/drivers/isdn/hisax/netjet.c b/drivers/isdn/hisax/netjet.c
index 233e432e06f6..b7f54fa29228 100644
--- a/drivers/isdn/hisax/netjet.c
+++ b/drivers/isdn/hisax/netjet.c
@@ -383,7 +383,7 @@ static void got_frame(struct BCState *bcs, int count) {
 	if (!(skb = dev_alloc_skb(count)))
 		printk(KERN_WARNING "TIGER: receive out of memory\n");
 	else {
-		memcpy(skb_put(skb, count), bcs->hw.tiger.rcvbuf, count);
+		skb_put_data(skb, bcs->hw.tiger.rcvbuf, count);
 		skb_queue_tail(&bcs->rqueue, skb);
 	}
 	test_and_set_bit(B_RCVBUFREADY, &bcs->event);
diff --git a/drivers/isdn/hisax/st5481_usb.c b/drivers/isdn/hisax/st5481_usb.c
index a0fdbc074b98..1cb9930d5e24 100644
--- a/drivers/isdn/hisax/st5481_usb.c
+++ b/drivers/isdn/hisax/st5481_usb.c
@@ -527,7 +527,7 @@ static void usb_in_complete(struct urb *urb)
 				WARNING("receive out of memory\n");
 				break;
 			}
-			memcpy(skb_put(skb, status), in->rcvbuf, status);
+			skb_put_data(skb, in->rcvbuf, status);
 			in->hisax_if->l1l2(in->hisax_if, PH_DATA | INDICATION, skb);
 		} else if (status == -HDLC_CRC_ERROR) {
 			INFO("CRC error");
diff --git a/drivers/isdn/hisax/w6692.c b/drivers/isdn/hisax/w6692.c
index c99f0ec58a01..6f6733b7c1e4 100644
--- a/drivers/isdn/hisax/w6692.c
+++ b/drivers/isdn/hisax/w6692.c
@@ -309,7 +309,9 @@ W6692B_interrupt(struct IsdnCardState *cs, u_char bchan)
 				if (!(skb = dev_alloc_skb(count)))
 					printk(KERN_WARNING "W6692: Bchan receive out of memory\n");
 				else {
-					memcpy(skb_put(skb, count), bcs->hw.w6692.rcvbuf, count);
+					skb_put_data(skb,
+						     bcs->hw.w6692.rcvbuf,
+						     count);
 					skb_queue_tail(&bcs->rqueue, skb);
 				}
 			}
@@ -332,7 +334,8 @@ W6692B_interrupt(struct IsdnCardState *cs, u_char bchan)
 			if (!(skb = dev_alloc_skb(W_B_FIFO_THRESH)))
 				printk(KERN_WARNING "HiSax: receive out of memory\n");
 			else {
-				memcpy(skb_put(skb, W_B_FIFO_THRESH), bcs->hw.w6692.rcvbuf, W_B_FIFO_THRESH);
+				skb_put_data(skb, bcs->hw.w6692.rcvbuf,
+					     W_B_FIFO_THRESH);
 				skb_queue_tail(&bcs->rqueue, skb);
 			}
 			bcs->hw.w6692.rcvidx = 0;
@@ -441,7 +444,7 @@ StartW6692:
 				if (!(skb = alloc_skb(count, GFP_ATOMIC)))
 					printk(KERN_WARNING "HiSax: D receive out of memory\n");
 				else {
-					memcpy(skb_put(skb, count), cs->rcvbuf, count);
+					skb_put_data(skb, cs->rcvbuf, count);
 					skb_queue_tail(&cs->rq, skb);
 				}
 			}
diff --git a/drivers/isdn/hysdn/hycapi.c b/drivers/isdn/hysdn/hycapi.c
index 93bae94314a6..eac0f51a0f60 100644
--- a/drivers/isdn/hysdn/hycapi.c
+++ b/drivers/isdn/hysdn/hycapi.c
@@ -171,16 +171,16 @@ hycapi_register_internal(struct capi_ctr *ctrl, __u16 appl,
 		       card->myid);
 		return;
 	}
-	memcpy(skb_put(skb, sizeof(__u16)), &len, sizeof(__u16));
-	memcpy(skb_put(skb, sizeof(__u16)), &appl, sizeof(__u16));
-	memcpy(skb_put(skb, sizeof(__u8)), &_command, sizeof(_command));
-	memcpy(skb_put(skb, sizeof(__u8)), &_subcommand, sizeof(_subcommand));
-	memcpy(skb_put(skb, sizeof(__u16)), &MessageNumber, sizeof(__u16));
-	memcpy(skb_put(skb, sizeof(__u16)), &MessageBufferSize, sizeof(__u16));
-	memcpy(skb_put(skb, sizeof(__u16)), &(rp->level3cnt), sizeof(__u16));
-	memcpy(skb_put(skb, sizeof(__u16)), &(rp->datablkcnt), sizeof(__u16));
-	memcpy(skb_put(skb, sizeof(__u16)), &(rp->datablklen), sizeof(__u16));
-	memcpy(skb_put(skb, slen), ExtFeatureDefaults, slen);
+	skb_put_data(skb, &len, sizeof(__u16));
+	skb_put_data(skb, &appl, sizeof(__u16));
+	skb_put_data(skb, &_command, sizeof(__u8));
+	skb_put_data(skb, &_subcommand, sizeof(__u8));
+	skb_put_data(skb, &MessageNumber, sizeof(__u16));
+	skb_put_data(skb, &MessageBufferSize, sizeof(__u16));
+	skb_put_data(skb, &(rp->level3cnt), sizeof(__u16));
+	skb_put_data(skb, &(rp->datablkcnt), sizeof(__u16));
+	skb_put_data(skb, &(rp->datablklen), sizeof(__u16));
+	skb_put_data(skb, ExtFeatureDefaults, slen);
 	hycapi_applications[appl - 1].ctrl_mask |= (1 << (ctrl->cnr - 1));
 	hycapi_send_message(ctrl, skb);
 }
@@ -279,11 +279,11 @@ static void hycapi_release_internal(struct capi_ctr *ctrl, __u16 appl)
 		       card->myid);
 		return;
 	}
-	memcpy(skb_put(skb, sizeof(__u16)), &len, sizeof(__u16));
-	memcpy(skb_put(skb, sizeof(__u16)), &appl, sizeof(__u16));
-	memcpy(skb_put(skb, sizeof(__u8)), &_command, sizeof(_command));
-	memcpy(skb_put(skb, sizeof(__u8)), &_subcommand, sizeof(_subcommand));
-	memcpy(skb_put(skb, sizeof(__u16)), &MessageNumber, sizeof(__u16));
+	skb_put_data(skb, &len, sizeof(__u16));
+	skb_put_data(skb, &appl, sizeof(__u16));
+	skb_put_data(skb, &_command, sizeof(__u8));
+	skb_put_data(skb, &_subcommand, sizeof(__u8));
+	skb_put_data(skb, &MessageNumber, sizeof(__u16));
 	hycapi_send_message(ctrl, skb);
 	hycapi_applications[appl - 1].ctrl_mask &= ~(1 << (ctrl->cnr - 1));
 }
@@ -557,10 +557,9 @@ hycapi_rx_capipkt(hysdn_card *card, unsigned char *buf, unsigned short len)
 			       card->myid);
 			return;
 		}
-		memcpy(skb_put(skb, MsgLen), buf, MsgLen);
-		memcpy(skb_put(skb, 2 * sizeof(__u32)), CP64, 2 * sizeof(__u32));
-		memcpy(skb_put(skb, len - MsgLen), buf + MsgLen,
-		       len - MsgLen);
+		skb_put_data(skb, buf, MsgLen);
+		skb_put_data(skb, CP64, 2 * sizeof(__u32));
+		skb_put_data(skb, buf + MsgLen, len - MsgLen);
 		CAPIMSG_SETLEN(skb->data, 30);
 	} else {
 		if (!(skb = alloc_skb(len, GFP_ATOMIC))) {
@@ -568,7 +567,7 @@ hycapi_rx_capipkt(hysdn_card *card, unsigned char *buf, unsigned short len)
 			       card->myid);
 			return;
 		}
-		memcpy(skb_put(skb, len), buf, len);
+		skb_put_data(skb, buf, len);
 	}
 	switch (CAPIMSG_CMD(skb->data))
 	{
diff --git a/drivers/isdn/hysdn/hysdn_net.c b/drivers/isdn/hysdn/hysdn_net.c
index b93a4e9a8d34..8e9c34f33d86 100644
--- a/drivers/isdn/hysdn/hysdn_net.c
+++ b/drivers/isdn/hysdn/hysdn_net.c
@@ -201,7 +201,7 @@ hysdn_rx_netpkt(hysdn_card *card, unsigned char *buf, unsigned short len)
 		return;
 	}
 	/* copy the data */
-	memcpy(skb_put(skb, len), buf, len);
+	skb_put_data(skb, buf, len);
 
 	/* determine the used protocol */
 	skb->protocol = eth_type_trans(skb, dev);
diff --git a/drivers/isdn/i4l/isdn_audio.c b/drivers/isdn/i4l/isdn_audio.c
index 78ce42214713..b6bcd1eca128 100644
--- a/drivers/isdn/i4l/isdn_audio.c
+++ b/drivers/isdn/i4l/isdn_audio.c
@@ -462,7 +462,7 @@ isdn_audio_goertzel(int *sample, modem_info *info)
 		       info->line);
 		return;
 	}
-	result = (int *) skb_put(skb, sizeof(int) * NCOEFF);
+	result = skb_put(skb, sizeof(int) * NCOEFF);
 	for (k = 0; k < NCOEFF; k++) {
 		sk = sk1 = sk2 = 0;
 		for (n = 0; n < DTMF_NPOINTS; n++) {
@@ -672,7 +672,7 @@ isdn_audio_put_dle_code(modem_info *info, u_char code)
 		       info->line);
 		return;
 	}
-	p = (char *) skb_put(skb, 2);
+	p = skb_put(skb, 2);
 	p[0] = 0x10;
 	p[1] = code;
 	ISDN_AUDIO_SKB_DLECOUNT(skb) = 0;
diff --git a/drivers/isdn/i4l/isdn_bsdcomp.c b/drivers/isdn/i4l/isdn_bsdcomp.c
index 8837ac5a492d..99012c047751 100644
--- a/drivers/isdn/i4l/isdn_bsdcomp.c
+++ b/drivers/isdn/i4l/isdn_bsdcomp.c
@@ -472,7 +472,7 @@ static int bsd_compress(void *state, struct sk_buff *skb_in, struct sk_buff *skb
 		accm |= ((ent) << bitno);				\
 		do	{						\
 			if (skb_out && skb_tailroom(skb_out) > 0)	\
-				*(skb_put(skb_out, 1)) = (unsigned char)(accm >> 24); \
+				skb_put_u8(skb_out, (u8)(accm >> 24));	\
 			accm <<= 8;					\
 			bitno += 8;					\
 		} while (bitno <= 24);					\
@@ -602,7 +602,8 @@ static int bsd_compress(void *state, struct sk_buff *skb_in, struct sk_buff *skb
 	 * Do not emit a completely useless byte of ones.
 	 */
 	if (bitno < 32 && skb_out && skb_tailroom(skb_out) > 0)
-		*(skb_put(skb_out, 1)) = (unsigned char)((accm | (0xff << (bitno - 8))) >> 24);
+		skb_put_u8(skb_out,
+			   (unsigned char)((accm | (0xff << (bitno - 8))) >> 24));
 
 	/*
 	 * Increase code size if we would have without the packet
@@ -698,7 +699,7 @@ static int bsd_decompress(void *state, struct sk_buff *skb_in, struct sk_buff *s
 	db->bytes_out += ilen;
 
 	if (skb_tailroom(skb_out) > 0)
-		*(skb_put(skb_out, 1)) = 0;
+		skb_put_u8(skb_out, 0);
 	else
 		return DECOMP_ERR_NOMEM;
 
@@ -816,7 +817,7 @@ static int bsd_decompress(void *state, struct sk_buff *skb_in, struct sk_buff *s
 #endif
 
 		if (extra)		/* the KwKwK case again */
-			*(skb_put(skb_out, 1)) = finchar;
+			skb_put_u8(skb_out, finchar);
 
 		/*
 		 * If not first code in a packet, and
diff --git a/drivers/isdn/i4l/isdn_common.c b/drivers/isdn/i4l/isdn_common.c
index 9b856e1890d1..89b09c51ab7c 100644
--- a/drivers/isdn/i4l/isdn_common.c
+++ b/drivers/isdn/i4l/isdn_common.c
@@ -1304,9 +1304,6 @@ isdn_ioctl(struct file *file, uint cmd, ulong arg)
 			if (arg) {
 				ulong __user *p = argp;
 				int i;
-				if (!access_ok(VERIFY_WRITE, p,
-					       sizeof(ulong) * ISDN_MAX_CHANNELS * 2))
-					return -EFAULT;
 				for (i = 0; i < ISDN_MAX_CHANNELS; i++) {
 					put_user(dev->ibytes[i], p++);
 					put_user(dev->obytes[i], p++);
@@ -1540,11 +1537,6 @@ isdn_ioctl(struct file *file, uint cmd, ulong arg)
 				char __user *p = argp;
 				int i;
 
-				if (!access_ok(VERIFY_WRITE, argp,
-					       (ISDN_MODEM_NUMREG + ISDN_MSNLEN + ISDN_LMSNLEN)
-					       * ISDN_MAX_CHANNELS))
-					return -EFAULT;
-
 				for (i = 0; i < ISDN_MAX_CHANNELS; i++) {
 					if (copy_to_user(p, dev->mdm.info[i].emu.profile,
 							 ISDN_MODEM_NUMREG))
@@ -1567,11 +1559,6 @@ isdn_ioctl(struct file *file, uint cmd, ulong arg)
 				char __user *p = argp;
 				int i;
 
-				if (!access_ok(VERIFY_READ, argp,
-					       (ISDN_MODEM_NUMREG + ISDN_MSNLEN + ISDN_LMSNLEN)
-					       * ISDN_MAX_CHANNELS))
-					return -EFAULT;
-
 				for (i = 0; i < ISDN_MAX_CHANNELS; i++) {
 					if (copy_from_user(dev->mdm.info[i].emu.profile, p,
 							   ISDN_MODEM_NUMREG))
@@ -1617,8 +1604,6 @@ isdn_ioctl(struct file *file, uint cmd, ulong arg)
 						int j = 0;
 
 						while (1) {
-							if (!access_ok(VERIFY_READ, p, 1))
-								return -EFAULT;
 							get_user(bname[j], p++);
 							switch (bname[j]) {
 							case '\0':
@@ -1685,9 +1670,6 @@ isdn_ioctl(struct file *file, uint cmd, ulong arg)
 					drvidx = 0;
 				if (drvidx == -1)
 					return -ENODEV;
-				if (!access_ok(VERIFY_WRITE, argp,
-					       sizeof(isdn_ioctl_struct)))
-					return -EFAULT;
 				c.driver = drvidx;
 				c.command = ISDN_CMD_IOCTL;
 				c.arg = cmd;
diff --git a/drivers/isdn/i4l/isdn_ppp.c b/drivers/isdn/i4l/isdn_ppp.c
index 8aa158a09180..6c44609fd83a 100644
--- a/drivers/isdn/i4l/isdn_ppp.c
+++ b/drivers/isdn/i4l/isdn_ppp.c
@@ -795,9 +795,6 @@ isdn_ppp_read(int min, struct file *file, char __user *buf, int count)
 	if (!(is->state & IPPP_OPEN))
 		return 0;
 
-	if (!access_ok(VERIFY_WRITE, buf, count))
-		return -EFAULT;
-
 	spin_lock_irqsave(&is->buflock, flags);
 	b = is->first->next;
 	save_buf = b->buf;
@@ -1312,7 +1309,7 @@ isdn_ppp_xmit(struct sk_buff *skb, struct net_device *netdev)
 	/* check if we should pass this packet
 	 * the filter instructions are constructed assuming
 	 * a four-byte PPP header on each packet */
-	*skb_push(skb, 4) = 1; /* indicate outbound */
+	*(u8 *)skb_push(skb, 4) = 1; /* indicate outbound */
 
 	{
 		__be16 *p = (__be16 *)skb->data;
@@ -1509,7 +1506,7 @@ int isdn_ppp_autodial_filter(struct sk_buff *skb, isdn_net_local *lp)
 	 * temporarily remove part of the fake header stuck on
 	 * earlier.
 	 */
-	*skb_pull(skb, IPPP_MAX_HEADER - 4) = 1; /* indicate outbound */
+	*(u8 *)skb_pull(skb, IPPP_MAX_HEADER - 4) = 1; /* indicate outbound */
 
 	{
 		__be16 *p = (__be16 *)skb->data;
@@ -2014,9 +2011,6 @@ isdn_ppp_dev_ioctl_stats(int slot, struct ifreq *ifr, struct net_device *dev)
 	struct ppp_stats t;
 	isdn_net_local *lp = netdev_priv(dev);
 
-	if (!access_ok(VERIFY_WRITE, res, sizeof(struct ppp_stats)))
-		return -EFAULT;
-
 	/* build a temporary stat struct and copy it to user space */
 
 	memset(&t, 0, sizeof(struct ppp_stats));
@@ -2258,8 +2252,7 @@ static void isdn_ppp_ccp_xmit_reset(struct ippp_struct *is, int proto,
 
 	/* Now stuff remaining bytes */
 	if (len) {
-		p = skb_put(skb, len);
-		memcpy(p, data, len);
+		skb_put_data(skb, data, len);
 	}
 
 	/* skb is now ready for xmit */
diff --git a/drivers/isdn/i4l/isdn_tty.c b/drivers/isdn/i4l/isdn_tty.c
index ddd8207e4e54..d30130c8d0f3 100644
--- a/drivers/isdn/i4l/isdn_tty.c
+++ b/drivers/isdn/i4l/isdn_tty.c
@@ -474,7 +474,7 @@ isdn_tty_senddown(modem_info *info)
 		return;
 	}
 	skb_reserve(skb, skb_res);
-	memcpy(skb_put(skb, buflen), info->port.xmit_buf, buflen);
+	skb_put_data(skb, info->port.xmit_buf, buflen);
 	info->xmit_count = 0;
 #ifdef CONFIG_ISDN_AUDIO
 	if (info->vonline & 2) {
diff --git a/drivers/isdn/i4l/isdn_v110.c b/drivers/isdn/i4l/isdn_v110.c
index 52827a80c51f..8b74ce412524 100644
--- a/drivers/isdn/i4l/isdn_v110.c
+++ b/drivers/isdn/i4l/isdn_v110.c
@@ -421,7 +421,7 @@ isdn_v110_sync(isdn_v110_stream *v)
 	}
 	if ((skb = dev_alloc_skb(v->framelen + v->skbres))) {
 		skb_reserve(skb, v->skbres);
-		memcpy(skb_put(skb, v->framelen), v->OfflineFrame, v->framelen);
+		skb_put_data(skb, v->OfflineFrame, v->framelen);
 	}
 	return skb;
 }
@@ -441,7 +441,7 @@ isdn_v110_idle(isdn_v110_stream *v)
 	}
 	if ((skb = dev_alloc_skb(v->framelen + v->skbres))) {
 		skb_reserve(skb, v->skbres);
-		memcpy(skb_put(skb, v->framelen), v->OnlineFrame, v->framelen);
+		skb_put_data(skb, v->OnlineFrame, v->framelen);
 	}
 	return skb;
 }
@@ -486,7 +486,7 @@ isdn_v110_encode(isdn_v110_stream *v, struct sk_buff *skb)
 	}
 	skb_reserve(nskb, v->skbres + sizeof(int));
 	if (skb->len == 0) {
-		memcpy(skb_put(nskb, v->framelen), v->OnlineFrame, v->framelen);
+		skb_put_data(nskb, v->OnlineFrame, v->framelen);
 		*((int *)skb_push(nskb, sizeof(int))) = 0;
 		return nskb;
 	}
diff --git a/drivers/isdn/i4l/isdn_x25iface.c b/drivers/isdn/i4l/isdn_x25iface.c
index ba60076e0b95..48bfbcb4a09d 100644
--- a/drivers/isdn/i4l/isdn_x25iface.c
+++ b/drivers/isdn/i4l/isdn_x25iface.c
@@ -224,7 +224,7 @@ static int isdn_x25iface_connect_ind(struct concap_proto *cprot)
 
 	skb = dev_alloc_skb(1);
 	if (skb) {
-		*(skb_put(skb, 1)) = X25_IFACE_CONNECT;
+		skb_put_u8(skb, X25_IFACE_CONNECT);
 		skb->protocol = x25_type_trans(skb, cprot->net_dev);
 		netif_rx(skb);
 		return 0;
@@ -253,7 +253,7 @@ static int isdn_x25iface_disconn_ind(struct concap_proto *cprot)
 	*state_p = WAN_DISCONNECTED;
 	skb = dev_alloc_skb(1);
 	if (skb) {
-		*(skb_put(skb, 1)) = X25_IFACE_DISCONNECT;
+		skb_put_u8(skb, X25_IFACE_DISCONNECT);
 		skb->protocol = x25_type_trans(skb, cprot->net_dev);
 		netif_rx(skb);
 		return 0;
diff --git a/drivers/isdn/isdnloop/isdnloop.c b/drivers/isdn/isdnloop/isdnloop.c
index ef9c8e4f1fa2..6ffd13466b8c 100644
--- a/drivers/isdn/isdnloop/isdnloop.c
+++ b/drivers/isdn/isdnloop/isdnloop.c
@@ -479,7 +479,7 @@ isdnloop_fake(isdnloop_card *card, char *s, int ch)
 	}
 	if (ch >= 0)
 		sprintf(skb_put(skb, 3), "%02d;", ch);
-	memcpy(skb_put(skb, strlen(s)), s, strlen(s));
+	skb_put_data(skb, s, strlen(s));
 	skb_queue_tail(&card->dqueue, skb);
 	return 0;
 }
@@ -1142,8 +1142,6 @@ isdnloop_command(isdn_ctrl *c, isdnloop_card *card)
 		case ISDNLOOP_IOCTL_DEBUGVAR:
 			return (ulong) card;
 		case ISDNLOOP_IOCTL_STARTUP:
-			if (!access_ok(VERIFY_READ, (void *) a, sizeof(isdnloop_sdef)))
-				return -EFAULT;
 			return isdnloop_start(card, (isdnloop_sdef *) a);
 			break;
 		case ISDNLOOP_IOCTL_ADDCARD:
diff --git a/drivers/isdn/mISDN/dsp_cmx.c b/drivers/isdn/mISDN/dsp_cmx.c
index 8e3aa002767b..d4b6f01a3f0e 100644
--- a/drivers/isdn/mISDN/dsp_cmx.c
+++ b/drivers/isdn/mISDN/dsp_cmx.c
@@ -1595,8 +1595,7 @@ send_packet:
 				thh = mISDN_HEAD_P(txskb);
 				thh->prim = DL_DATA_REQ;
 				thh->id = 0;
-				memcpy(skb_put(txskb, len), nskb->data + preload,
-				       len);
+				skb_put_data(txskb, nskb->data + preload, len);
 				/* queue (trigger later) */
 				skb_queue_tail(&dsp->sendq, txskb);
 			}
diff --git a/drivers/isdn/mISDN/layer2.c b/drivers/isdn/mISDN/layer2.c
index 5eb380a25903..7243a6746f8b 100644
--- a/drivers/isdn/mISDN/layer2.c
+++ b/drivers/isdn/mISDN/layer2.c
@@ -176,7 +176,7 @@ l2up_create(struct layer2 *l2, u_int prim, int len, void *arg)
 	hh->prim = prim;
 	hh->id = (l2->ch.nr << 16) | l2->ch.addr;
 	if (len)
-		memcpy(skb_put(skb, len), arg, len);
+		skb_put_data(skb, arg, len);
 	err = l2->up->send(l2->up, skb);
 	if (err) {
 		printk(KERN_WARNING "%s: dev %s err=%d\n", __func__,
@@ -235,7 +235,7 @@ l2down_create(struct layer2 *l2, u_int prim, u_int id, int len, void *arg)
 	hh->prim = prim;
 	hh->id = id;
 	if (len)
-		memcpy(skb_put(skb, len), arg, len);
+		skb_put_data(skb, arg, len);
 	err = l2down_raw(l2, skb);
 	if (err)
 		dev_kfree_skb(skb);
@@ -640,7 +640,7 @@ send_uframe(struct layer2 *l2, struct sk_buff *skb, u_char cmd, u_char cr)
 			return;
 		}
 	}
-	memcpy(skb_put(skb, i), tmp, i);
+	skb_put_data(skb, tmp, i);
 	enqueue_super(l2, skb);
 }
 
@@ -1125,7 +1125,7 @@ enquiry_cr(struct layer2 *l2, u_char typ, u_char cr, u_char pf)
 		       mISDNDevName4ch(&l2->ch), __func__);
 		return;
 	}
-	memcpy(skb_put(skb, i), tmp, i);
+	skb_put_data(skb, tmp, i);
 	enqueue_super(l2, skb);
 }
 
diff --git a/drivers/isdn/mISDN/socket.c b/drivers/isdn/mISDN/socket.c
index 99e5f9751e8b..c5603d1a07d6 100644
--- a/drivers/isdn/mISDN/socket.c
+++ b/drivers/isdn/mISDN/socket.c
@@ -155,7 +155,7 @@ mISDN_sock_recvmsg(struct socket *sock, struct msghdr *msg, size_t len,
 	copied = skb->len + MISDN_HEADER_LEN;
 	if (len < copied) {
 		if (flags & MSG_PEEK)
-			atomic_dec(&skb->users);
+			refcount_dec(&skb->users);
 		else
 			skb_queue_head(&sk->sk_receive_queue, skb);
 		return -ENOSPC;
diff --git a/drivers/isdn/mISDN/tei.c b/drivers/isdn/mISDN/tei.c
index 592f597d8951..908127efccf8 100644
--- a/drivers/isdn/mISDN/tei.c
+++ b/drivers/isdn/mISDN/tei.c
@@ -312,7 +312,7 @@ teiup_create(struct manager *mgr, u_int prim, int len, void *arg)
 	hh->prim = prim;
 	hh->id = (mgr->ch.nr << 16) | mgr->ch.addr;
 	if (len)
-		memcpy(skb_put(skb, len), arg, len);
+		skb_put_data(skb, arg, len);
 	err = mgr->up->send(mgr->up, skb);
 	if (err) {
 		printk(KERN_WARNING "%s: err=%d\n", __func__, err);
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index 6c2999872090..594b24d410c3 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -590,16 +590,6 @@ config LEDS_KTD2692
 
 	  Say Y to enable this driver.
 
-config LEDS_SEAD3
-	tristate "LED support for the MIPS SEAD 3 board"
-	depends on LEDS_CLASS && MIPS_SEAD3
-	help
-	  Say Y here to include support for the FLED and PLED LEDs on SEAD3 eval
-	  boards.
-
-	  This driver can also be built as a module. If so the module
-	  will be called leds-sead3.
-
 config LEDS_IS31FL319X
 	tristate "LED Support for ISSI IS31FL319x I2C LED controller family"
 	depends on LEDS_CLASS && I2C && OF
@@ -651,14 +641,6 @@ config LEDS_SYSCON
 	  devices. This will only work with device tree enabled
 	  devices.
 
-config LEDS_VERSATILE
-	tristate "LED support for the ARM Versatile and RealView"
-	depends on ARCH_REALVIEW || ARCH_VERSATILE
-	depends on LEDS_CLASS
-	help
-	  This option enabled support for the LEDs on the ARM Versatile
-	  and RealView boards. Say Y to enabled these.
-
 config LEDS_PM8058
 	tristate "LED Support for the Qualcomm PM8058 PMIC"
 	depends on MFD_PM8XXX
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index 45f133962ed8..909dae62ba05 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -62,11 +62,9 @@ obj-$(CONFIG_LEDS_MAX8997)		+= leds-max8997.o
 obj-$(CONFIG_LEDS_LM355x)		+= leds-lm355x.o
 obj-$(CONFIG_LEDS_BLINKM)		+= leds-blinkm.o
 obj-$(CONFIG_LEDS_SYSCON)		+= leds-syscon.o
-obj-$(CONFIG_LEDS_VERSATILE)		+= leds-versatile.o
 obj-$(CONFIG_LEDS_MENF21BMC)		+= leds-menf21bmc.o
 obj-$(CONFIG_LEDS_KTD2692)		+= leds-ktd2692.o
 obj-$(CONFIG_LEDS_POWERNV)		+= leds-powernv.o
-obj-$(CONFIG_LEDS_SEAD3)		+= leds-sead3.o
 obj-$(CONFIG_LEDS_IS31FL319X)		+= leds-is31fl319x.o
 obj-$(CONFIG_LEDS_IS31FL32XX)		+= leds-is31fl32xx.o
 obj-$(CONFIG_LEDS_PM8058)		+= leds-pm8058.o
diff --git a/drivers/leds/leds-aat1290.c b/drivers/leds/leds-aat1290.c
index def3cf9f7e92..a21e19297745 100644
--- a/drivers/leds/leds-aat1290.c
+++ b/drivers/leds/leds-aat1290.c
@@ -503,8 +503,9 @@ static int aat1290_led_probe(struct platform_device *pdev)
 	aat1290_init_v4l2_flash_config(led, &led_cfg, &v4l2_sd_cfg);
 
 	/* Create V4L2 Flash subdev. */
-	led->v4l2_flash = v4l2_flash_init(dev, sub_node, fled_cdev, NULL,
-					  &v4l2_flash_ops, &v4l2_sd_cfg);
+	led->v4l2_flash = v4l2_flash_init(dev, of_fwnode_handle(sub_node),
+					  fled_cdev, NULL, &v4l2_flash_ops,
+					  &v4l2_sd_cfg);
 	if (IS_ERR(led->v4l2_flash)) {
 		ret = PTR_ERR(led->v4l2_flash);
 		goto error_v4l2_flash_init;
diff --git a/drivers/leds/leds-bcm6328.c b/drivers/leds/leds-bcm6328.c
index 1548259297c1..2cfd9389ee96 100644
--- a/drivers/leds/leds-bcm6328.c
+++ b/drivers/leds/leds-bcm6328.c
@@ -242,7 +242,7 @@ static int bcm6328_hwled(struct device *dev, struct device_node *nc, u32 reg,
 
 		spin_lock_irqsave(lock, flags);
 		val = bcm6328_led_read(addr);
-		val |= (BIT(reg) << (((sel % 4) * 4) + 16));
+		val |= (BIT(reg % 4) << (((sel % 4) * 4) + 16));
 		bcm6328_led_write(addr, val);
 		spin_unlock_irqrestore(lock, flags);
 	}
@@ -269,7 +269,7 @@ static int bcm6328_hwled(struct device *dev, struct device_node *nc, u32 reg,
 
 		spin_lock_irqsave(lock, flags);
 		val = bcm6328_led_read(addr);
-		val |= (BIT(reg) << ((sel % 4) * 4));
+		val |= (BIT(reg % 4) << ((sel % 4) * 4));
 		bcm6328_led_write(addr, val);
 		spin_unlock_irqrestore(lock, flags);
 	}
diff --git a/drivers/leds/leds-lp5523.c b/drivers/leds/leds-lp5523.c
index e9ba8cd32d66..924e50aefb00 100644
--- a/drivers/leds/leds-lp5523.c
+++ b/drivers/leds/leds-lp5523.c
@@ -168,13 +168,13 @@ static int lp5523_post_init_device(struct lp55xx_chip *chip)
 static void lp5523_load_engine(struct lp55xx_chip *chip)
 {
 	enum lp55xx_engine_index idx = chip->engine_idx;
-	u8 mask[] = {
+	static const u8 mask[] = {
 		[LP55XX_ENGINE_1] = LP5523_MODE_ENG1_M,
 		[LP55XX_ENGINE_2] = LP5523_MODE_ENG2_M,
 		[LP55XX_ENGINE_3] = LP5523_MODE_ENG3_M,
 	};
 
-	u8 val[] = {
+	static const u8 val[] = {
 		[LP55XX_ENGINE_1] = LP5523_LOAD_ENG1,
 		[LP55XX_ENGINE_2] = LP5523_LOAD_ENG2,
 		[LP55XX_ENGINE_3] = LP5523_LOAD_ENG3,
@@ -188,7 +188,7 @@ static void lp5523_load_engine(struct lp55xx_chip *chip)
 static void lp5523_load_engine_and_select_page(struct lp55xx_chip *chip)
 {
 	enum lp55xx_engine_index idx = chip->engine_idx;
-	u8 page_sel[] = {
+	static const u8 page_sel[] = {
 		[LP55XX_ENGINE_1] = LP5523_PAGE_ENG1,
 		[LP55XX_ENGINE_2] = LP5523_PAGE_ENG2,
 		[LP55XX_ENGINE_3] = LP5523_PAGE_ENG3,
@@ -208,7 +208,7 @@ static void lp5523_stop_all_engines(struct lp55xx_chip *chip)
 static void lp5523_stop_engine(struct lp55xx_chip *chip)
 {
 	enum lp55xx_engine_index idx = chip->engine_idx;
-	u8 mask[] = {
+	static const u8 mask[] = {
 		[LP55XX_ENGINE_1] = LP5523_MODE_ENG1_M,
 		[LP55XX_ENGINE_2] = LP5523_MODE_ENG2_M,
 		[LP55XX_ENGINE_3] = LP5523_MODE_ENG3_M,
@@ -505,7 +505,7 @@ static int lp5523_load_mux(struct lp55xx_chip *chip, u16 mux, int nr)
 {
 	struct lp55xx_engine *engine = &chip->engines[nr - 1];
 	int ret;
-	u8 mux_page[] = {
+	static const u8 mux_page[] = {
 		[LP55XX_ENGINE_1] = LP5523_PAGE_MUX1,
 		[LP55XX_ENGINE_2] = LP5523_PAGE_MUX2,
 		[LP55XX_ENGINE_3] = LP5523_PAGE_MUX3,
diff --git a/drivers/leds/leds-max77693.c b/drivers/leds/leds-max77693.c
index 1eb58ef6aefe..2d3062d53325 100644
--- a/drivers/leds/leds-max77693.c
+++ b/drivers/leds/leds-max77693.c
@@ -930,8 +930,9 @@ static int max77693_register_led(struct max77693_sub_led *sub_led,
 	max77693_init_v4l2_flash_config(sub_led, led_cfg, &v4l2_sd_cfg);
 
 	/* Register in the V4L2 subsystem. */
-	sub_led->v4l2_flash = v4l2_flash_init(dev, sub_node, fled_cdev, NULL,
-					      &v4l2_flash_ops, &v4l2_sd_cfg);
+	sub_led->v4l2_flash = v4l2_flash_init(dev, of_fwnode_handle(sub_node),
+					      fled_cdev, NULL, &v4l2_flash_ops,
+					      &v4l2_sd_cfg);
 	if (IS_ERR(sub_led->v4l2_flash)) {
 		ret = PTR_ERR(sub_led->v4l2_flash);
 		goto err_v4l2_flash_init;
diff --git a/drivers/leds/leds-pca963x.c b/drivers/leds/leds-pca963x.c
index ded1e4dac36a..3bf9a1271819 100644
--- a/drivers/leds/leds-pca963x.c
+++ b/drivers/leds/leds-pca963x.c
@@ -342,6 +342,12 @@ pca963x_dt_init(struct i2c_client *client, struct pca963x_chipdef *chip)
 	if (of_property_read_u32(np, "nxp,period-scale", &chip->scaling))
 		chip->scaling = 1000;
 
+	/* default to non-inverted output, unless inverted is specified */
+	if (of_property_read_bool(np, "nxp,inverted-out"))
+		pdata->dir = PCA963X_INVERTED;
+	else
+		pdata->dir = PCA963X_NORMAL;
+
 	return pdata;
 }
 
@@ -452,11 +458,18 @@ static int pca963x_probe(struct i2c_client *client,
 	i2c_smbus_write_byte_data(client, PCA963X_MODE1, BIT(4));
 
 	if (pdata) {
+		u8 mode2 = i2c_smbus_read_byte_data(pca963x->chip->client,
+						    PCA963X_MODE2);
 		/* Configure output: open-drain or totem pole (push-pull) */
 		if (pdata->outdrv == PCA963X_OPEN_DRAIN)
-			i2c_smbus_write_byte_data(client, PCA963X_MODE2, 0x01);
+			mode2 |= 0x01;
 		else
-			i2c_smbus_write_byte_data(client, PCA963X_MODE2, 0x05);
+			mode2 |= 0x05;
+		/* Configure direction: normal or inverted */
+		if (pdata->dir == PCA963X_INVERTED)
+			mode2 |= 0x10;
+		i2c_smbus_write_byte_data(pca963x->chip->client, PCA963X_MODE2,
+					  mode2);
 	}
 
 	return 0;
diff --git a/drivers/leds/leds-sead3.c b/drivers/leds/leds-sead3.c
deleted file mode 100644
index eb97a3271bb3..000000000000
--- a/drivers/leds/leds-sead3.c
+++ /dev/null
@@ -1,78 +0,0 @@
-/*
- * 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) 2012 MIPS Technologies, Inc.  All rights reserved.
- * Copyright (C) 2015 Imagination Technologies, Inc.
- */
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/leds.h>
-#include <linux/err.h>
-#include <linux/io.h>
-
-#include <asm/mips-boards/sead3-addr.h>
-
-static void sead3_pled_set(struct led_classdev *led_cdev,
-		enum led_brightness value)
-{
-	writel(value, (void __iomem *)SEAD3_CPLD_P_LED);
-}
-
-static void sead3_fled_set(struct led_classdev *led_cdev,
-		enum led_brightness value)
-{
-	writel(value, (void __iomem *)SEAD3_CPLD_F_LED);
-}
-
-static struct led_classdev sead3_pled = {
-	.name		= "sead3::pled",
-	.brightness_set = sead3_pled_set,
-	.flags		= LED_CORE_SUSPENDRESUME,
-};
-
-static struct led_classdev sead3_fled = {
-	.name		= "sead3::fled",
-	.brightness_set = sead3_fled_set,
-	.flags		= LED_CORE_SUSPENDRESUME,
-};
-
-static int sead3_led_probe(struct platform_device *pdev)
-{
-	int ret;
-
-	ret = led_classdev_register(&pdev->dev, &sead3_pled);
-	if (ret < 0)
-		return ret;
-
-	ret = led_classdev_register(&pdev->dev, &sead3_fled);
-	if (ret < 0)
-		led_classdev_unregister(&sead3_pled);
-
-	return ret;
-}
-
-static int sead3_led_remove(struct platform_device *pdev)
-{
-	led_classdev_unregister(&sead3_pled);
-	led_classdev_unregister(&sead3_fled);
-
-	return 0;
-}
-
-static struct platform_driver sead3_led_driver = {
-	.probe		= sead3_led_probe,
-	.remove		= sead3_led_remove,
-	.driver		= {
-		.name		= "sead3-led",
-	},
-};
-
-module_platform_driver(sead3_led_driver);
-
-MODULE_AUTHOR("Kristian Kielhofner <kris@krisk.org>");
-MODULE_DESCRIPTION("SEAD3 LED driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/leds/leds-versatile.c b/drivers/leds/leds-versatile.c
deleted file mode 100644
index 80553022d661..000000000000
--- a/drivers/leds/leds-versatile.c
+++ /dev/null
@@ -1,110 +0,0 @@
-/*
- * Driver for the 8 user LEDs found on the RealViews and Versatiles
- * Based on DaVinci's DM365 board code
- *
- * License terms: GNU General Public License (GPL) version 2
- * Author: Linus Walleij <triad@df.lth.se>
- */
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/io.h>
-#include <linux/slab.h>
-#include <linux/leds.h>
-#include <linux/platform_device.h>
-
-struct versatile_led {
-	void __iomem		*base;
-	struct led_classdev	cdev;
-	u8			mask;
-};
-
-/*
- * The triggers lines up below will only be used if the
- * LED triggers are compiled in.
- */
-static const struct {
-	const char *name;
-	const char *trigger;
-} versatile_leds[] = {
-	{ "versatile:0", "heartbeat", },
-	{ "versatile:1", "mmc0", },
-	{ "versatile:2", "cpu0" },
-	{ "versatile:3", "cpu1" },
-	{ "versatile:4", "cpu2" },
-	{ "versatile:5", "cpu3" },
-	{ "versatile:6", },
-	{ "versatile:7", },
-};
-
-static void versatile_led_set(struct led_classdev *cdev,
-			      enum led_brightness b)
-{
-	struct versatile_led *led = container_of(cdev,
-						 struct versatile_led, cdev);
-	u32 reg = readl(led->base);
-
-	if (b != LED_OFF)
-		reg |= led->mask;
-	else
-		reg &= ~led->mask;
-	writel(reg, led->base);
-}
-
-static enum led_brightness versatile_led_get(struct led_classdev *cdev)
-{
-	struct versatile_led *led = container_of(cdev,
-						 struct versatile_led, cdev);
-	u32 reg = readl(led->base);
-
-	return (reg & led->mask) ? LED_FULL : LED_OFF;
-}
-
-static int versatile_leds_probe(struct platform_device *dev)
-{
-	int i;
-	struct resource *res;
-	void __iomem *base;
-
-	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
-	base = devm_ioremap_resource(&dev->dev, res);
-	if (IS_ERR(base))
-		return PTR_ERR(base);
-
-	/* All off */
-	writel(0, base);
-	for (i = 0; i < ARRAY_SIZE(versatile_leds); i++) {
-		struct versatile_led *led;
-
-		led = kzalloc(sizeof(*led), GFP_KERNEL);
-		if (!led)
-			break;
-
-		led->base = base;
-		led->cdev.name = versatile_leds[i].name;
-		led->cdev.brightness_set = versatile_led_set;
-		led->cdev.brightness_get = versatile_led_get;
-		led->cdev.default_trigger = versatile_leds[i].trigger;
-		led->mask = BIT(i);
-
-		if (led_classdev_register(NULL, &led->cdev) < 0) {
-			kfree(led);
-			break;
-		}
-	}
-
-	return 0;
-}
-
-static struct platform_driver versatile_leds_driver = {
-	.driver = {
-		.name   = "versatile-leds",
-	},
-	.probe = versatile_leds_probe,
-};
-
-module_platform_driver(versatile_leds_driver);
-
-MODULE_AUTHOR("Linus Walleij <linus.walleij@linaro.org>");
-MODULE_DESCRIPTION("ARM Versatile LED driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/trigger/ledtrig-gpio.c b/drivers/leds/trigger/ledtrig-gpio.c
index 51288a45fbcb..8891e88d54dd 100644
--- a/drivers/leds/trigger/ledtrig-gpio.c
+++ b/drivers/leds/trigger/ledtrig-gpio.c
@@ -14,14 +14,12 @@
 #include <linux/init.h>
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
-#include <linux/workqueue.h>
 #include <linux/leds.h>
 #include <linux/slab.h>
 #include "../leds.h"
 
 struct gpio_trig_data {
 	struct led_classdev *led;
-	struct work_struct work;
 
 	unsigned desired_brightness;	/* desired brightness when led is on */
 	unsigned inverted;		/* true when gpio is inverted */
@@ -32,22 +30,8 @@ static irqreturn_t gpio_trig_irq(int irq, void *_led)
 {
 	struct led_classdev *led = _led;
 	struct gpio_trig_data *gpio_data = led->trigger_data;
-
-	/* just schedule_work since gpio_get_value can sleep */
-	schedule_work(&gpio_data->work);
-
-	return IRQ_HANDLED;
-};
-
-static void gpio_trig_work(struct work_struct *work)
-{
-	struct gpio_trig_data *gpio_data = container_of(work,
-			struct gpio_trig_data, work);
 	int tmp;
 
-	if (!gpio_data->gpio)
-		return;
-
 	tmp = gpio_get_value_cansleep(gpio_data->gpio);
 	if (gpio_data->inverted)
 		tmp = !tmp;
@@ -61,6 +45,8 @@ static void gpio_trig_work(struct work_struct *work)
 	} else {
 		led_set_brightness_nosleep(gpio_data->led, LED_OFF);
 	}
+
+	return IRQ_HANDLED;
 }
 
 static ssize_t gpio_trig_brightness_show(struct device *dev,
@@ -120,7 +106,7 @@ static ssize_t gpio_trig_inverted_store(struct device *dev,
 	gpio_data->inverted = inverted;
 
 	/* After inverting, we need to update the LED. */
-	schedule_work(&gpio_data->work);
+	gpio_trig_irq(0, led);
 
 	return n;
 }
@@ -147,7 +133,6 @@ static ssize_t gpio_trig_gpio_store(struct device *dev,
 	ret = sscanf(buf, "%u", &gpio);
 	if (ret < 1) {
 		dev_err(dev, "couldn't read gpio number\n");
-		flush_work(&gpio_data->work);
 		return -EINVAL;
 	}
 
@@ -161,8 +146,8 @@ static ssize_t gpio_trig_gpio_store(struct device *dev,
 		return n;
 	}
 
-	ret = request_irq(gpio_to_irq(gpio), gpio_trig_irq,
-			IRQF_SHARED | IRQF_TRIGGER_RISING
+	ret = request_threaded_irq(gpio_to_irq(gpio), NULL, gpio_trig_irq,
+			IRQF_ONESHOT | IRQF_SHARED | IRQF_TRIGGER_RISING
 			| IRQF_TRIGGER_FALLING, "ledtrig-gpio", led);
 	if (ret) {
 		dev_err(dev, "request_irq failed with error %d\n", ret);
@@ -170,6 +155,8 @@ static ssize_t gpio_trig_gpio_store(struct device *dev,
 		if (gpio_data->gpio != 0)
 			free_irq(gpio_to_irq(gpio_data->gpio), led);
 		gpio_data->gpio = gpio;
+		/* After changing the GPIO, we need to update the LED. */
+		gpio_trig_irq(0, led);
 	}
 
 	return ret ? ret : n;
@@ -199,7 +186,6 @@ static void gpio_trig_activate(struct led_classdev *led)
 
 	gpio_data->led = led;
 	led->trigger_data = gpio_data;
-	INIT_WORK(&gpio_data->work, gpio_trig_work);
 	led->activated = true;
 
 	return;
@@ -222,7 +208,6 @@ static void gpio_trig_deactivate(struct led_classdev *led)
 		device_remove_file(led->dev, &dev_attr_gpio);
 		device_remove_file(led->dev, &dev_attr_inverted);
 		device_remove_file(led->dev, &dev_attr_desired_brightness);
-		flush_work(&gpio_data->work);
 		if (gpio_data->gpio != 0)
 			free_irq(gpio_to_irq(gpio_data->gpio), led);
 		kfree(gpio_data);
diff --git a/drivers/leds/trigger/ledtrig-heartbeat.c b/drivers/leds/trigger/ledtrig-heartbeat.c
index afa3b4099214..e95ea65380c8 100644
--- a/drivers/leds/trigger/ledtrig-heartbeat.c
+++ b/drivers/leds/trigger/ledtrig-heartbeat.c
@@ -20,7 +20,6 @@
 #include <linux/sched/loadavg.h>
 #include <linux/leds.h>
 #include <linux/reboot.h>
-#include <linux/suspend.h>
 #include "../leds.h"
 
 static int panic_heartbeats;
@@ -163,30 +162,6 @@ static struct led_trigger heartbeat_led_trigger = {
 	.deactivate = heartbeat_trig_deactivate,
 };
 
-static int heartbeat_pm_notifier(struct notifier_block *nb,
-				 unsigned long pm_event, void *unused)
-{
-	int rc;
-
-	switch (pm_event) {
-	case PM_SUSPEND_PREPARE:
-	case PM_HIBERNATION_PREPARE:
-	case PM_RESTORE_PREPARE:
-		led_trigger_unregister(&heartbeat_led_trigger);
-		break;
-	case PM_POST_SUSPEND:
-	case PM_POST_HIBERNATION:
-	case PM_POST_RESTORE:
-		rc = led_trigger_register(&heartbeat_led_trigger);
-		if (rc)
-			pr_err("could not re-register heartbeat trigger\n");
-		break;
-	default:
-		break;
-	}
-	return NOTIFY_DONE;
-}
-
 static int heartbeat_reboot_notifier(struct notifier_block *nb,
 				     unsigned long code, void *unused)
 {
@@ -201,10 +176,6 @@ static int heartbeat_panic_notifier(struct notifier_block *nb,
 	return NOTIFY_DONE;
 }
 
-static struct notifier_block heartbeat_pm_nb = {
-	.notifier_call = heartbeat_pm_notifier,
-};
-
 static struct notifier_block heartbeat_reboot_nb = {
 	.notifier_call = heartbeat_reboot_notifier,
 };
@@ -221,14 +192,12 @@ static int __init heartbeat_trig_init(void)
 		atomic_notifier_chain_register(&panic_notifier_list,
 					       &heartbeat_panic_nb);
 		register_reboot_notifier(&heartbeat_reboot_nb);
-		register_pm_notifier(&heartbeat_pm_nb);
 	}
 	return rc;
 }
 
 static void __exit heartbeat_trig_exit(void)
 {
-	unregister_pm_notifier(&heartbeat_pm_nb);
 	unregister_reboot_notifier(&heartbeat_reboot_nb);
 	atomic_notifier_chain_unregister(&panic_notifier_list,
 					 &heartbeat_panic_nb);
diff --git a/drivers/lightnvm/core.c b/drivers/lightnvm/core.c
index 6a4aa608ad95..ddae430b6eae 100644
--- a/drivers/lightnvm/core.c
+++ b/drivers/lightnvm/core.c
@@ -252,8 +252,9 @@ static int nvm_create_tgt(struct nvm_dev *dev, struct nvm_ioctl_create *create)
 	}
 	mutex_unlock(&dev->mlock);
 
-	if (nvm_reserve_luns(dev, s->lun_begin, s->lun_end))
-		return -ENOMEM;
+	ret = nvm_reserve_luns(dev, s->lun_begin, s->lun_end);
+	if (ret)
+		return ret;
 
 	t = kmalloc(sizeof(struct nvm_target), GFP_KERNEL);
 	if (!t) {
@@ -640,6 +641,7 @@ EXPORT_SYMBOL(nvm_max_phys_sects);
 int nvm_submit_io(struct nvm_tgt_dev *tgt_dev, struct nvm_rq *rqd)
 {
 	struct nvm_dev *dev = tgt_dev->parent;
+	int ret;
 
 	if (!dev->ops->submit_io)
 		return -ENODEV;
@@ -647,7 +649,12 @@ int nvm_submit_io(struct nvm_tgt_dev *tgt_dev, struct nvm_rq *rqd)
 	nvm_rq_tgt_to_dev(tgt_dev, rqd);
 
 	rqd->dev = tgt_dev;
-	return dev->ops->submit_io(dev, rqd);
+
+	/* In case of error, fail with right address format */
+	ret = dev->ops->submit_io(dev, rqd);
+	if (ret)
+		nvm_rq_dev_to_tgt(tgt_dev, rqd);
+	return ret;
 }
 EXPORT_SYMBOL(nvm_submit_io);
 
diff --git a/drivers/lightnvm/pblk-cache.c b/drivers/lightnvm/pblk-cache.c
index 59bcea88db84..024a8fc93069 100644
--- a/drivers/lightnvm/pblk-cache.c
+++ b/drivers/lightnvm/pblk-cache.c
@@ -31,9 +31,13 @@ int pblk_write_to_cache(struct pblk *pblk, struct bio *bio, unsigned long flags)
 	 */
 retry:
 	ret = pblk_rb_may_write_user(&pblk->rwb, bio, nr_entries, &bpos);
-	if (ret == NVM_IO_REQUEUE) {
+	switch (ret) {
+	case NVM_IO_REQUEUE:
 		io_schedule();
 		goto retry;
+	case NVM_IO_ERR:
+		pblk_pipeline_stop(pblk);
+		goto out;
 	}
 
 	if (unlikely(!bio_has_data(bio)))
@@ -58,6 +62,8 @@ retry:
 	atomic_long_add(nr_entries, &pblk->req_writes);
 #endif
 
+	pblk_rl_inserted(&pblk->rl, nr_entries);
+
 out:
 	pblk_write_should_kick(pblk);
 	return ret;
diff --git a/drivers/lightnvm/pblk-core.c b/drivers/lightnvm/pblk-core.c
index 5e44768ccffa..11fe0c5b2a9c 100644
--- a/drivers/lightnvm/pblk-core.c
+++ b/drivers/lightnvm/pblk-core.c
@@ -17,7 +17,6 @@
  */
 
 #include "pblk.h"
-#include <linux/time.h>
 
 static void pblk_mark_bb(struct pblk *pblk, struct pblk_line *line,
 			 struct ppa_addr *ppa)
@@ -34,7 +33,7 @@ static void pblk_mark_bb(struct pblk *pblk, struct pblk_line *line,
 		pr_err("pblk: attempted to erase bb: line:%d, pos:%d\n",
 							line->id, pos);
 
-	pblk_line_run_ws(pblk, NULL, ppa, pblk_line_mark_bb);
+	pblk_line_run_ws(pblk, NULL, ppa, pblk_line_mark_bb, pblk->bb_wq);
 }
 
 static void __pblk_end_io_erase(struct pblk *pblk, struct nvm_rq *rqd)
@@ -54,6 +53,8 @@ static void __pblk_end_io_erase(struct pblk *pblk, struct nvm_rq *rqd)
 		*ppa = rqd->ppa_addr;
 		pblk_mark_bb(pblk, line, ppa);
 	}
+
+	atomic_dec(&pblk->inflight_io);
 }
 
 /* Erase completion assumes that only one block is erased at the time */
@@ -61,13 +62,12 @@ static void pblk_end_io_erase(struct nvm_rq *rqd)
 {
 	struct pblk *pblk = rqd->private;
 
-	up(&pblk->erase_sem);
 	__pblk_end_io_erase(pblk, rqd);
-	mempool_free(rqd, pblk->r_rq_pool);
+	mempool_free(rqd, pblk->g_rq_pool);
 }
 
-static void __pblk_map_invalidate(struct pblk *pblk, struct pblk_line *line,
-				  u64 paddr)
+void __pblk_map_invalidate(struct pblk *pblk, struct pblk_line *line,
+			   u64 paddr)
 {
 	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
 	struct list_head *move_list = NULL;
@@ -88,7 +88,7 @@ static void __pblk_map_invalidate(struct pblk *pblk, struct pblk_line *line,
 		spin_unlock(&line->lock);
 		return;
 	}
-	line->vsc--;
+	le32_add_cpu(line->vsc, -1);
 
 	if (line->state == PBLK_LINESTATE_CLOSED)
 		move_list = pblk_line_gc_list(pblk, line);
@@ -130,18 +130,6 @@ void pblk_map_invalidate(struct pblk *pblk, struct ppa_addr ppa)
 	__pblk_map_invalidate(pblk, line, paddr);
 }
 
-void pblk_map_pad_invalidate(struct pblk *pblk, struct pblk_line *line,
-			     u64 paddr)
-{
-	__pblk_map_invalidate(pblk, line, paddr);
-
-	pblk_rb_sync_init(&pblk->rwb, NULL);
-	line->left_ssecs--;
-	if (!line->left_ssecs)
-		pblk_line_run_ws(pblk, line, NULL, pblk_line_close_ws);
-	pblk_rb_sync_end(&pblk->rwb, NULL);
-}
-
 static void pblk_invalidate_range(struct pblk *pblk, sector_t slba,
 				  unsigned int nr_secs)
 {
@@ -172,8 +160,8 @@ struct nvm_rq *pblk_alloc_rqd(struct pblk *pblk, int rw)
 		pool = pblk->w_rq_pool;
 		rq_size = pblk_w_rq_size;
 	} else {
-		pool = pblk->r_rq_pool;
-		rq_size = pblk_r_rq_size;
+		pool = pblk->g_rq_pool;
+		rq_size = pblk_g_rq_size;
 	}
 
 	rqd = mempool_alloc(pool, GFP_KERNEL);
@@ -189,7 +177,7 @@ void pblk_free_rqd(struct pblk *pblk, struct nvm_rq *rqd, int rw)
 	if (rw == WRITE)
 		pool = pblk->w_rq_pool;
 	else
-		pool = pblk->r_rq_pool;
+		pool = pblk->g_rq_pool;
 
 	mempool_free(rqd, pool);
 }
@@ -271,35 +259,26 @@ void pblk_end_io_sync(struct nvm_rq *rqd)
 	complete(waiting);
 }
 
-void pblk_flush_writer(struct pblk *pblk)
+void pblk_wait_for_meta(struct pblk *pblk)
 {
-	struct bio *bio;
-	int ret;
-	DECLARE_COMPLETION_ONSTACK(wait);
-
-	bio = bio_alloc(GFP_KERNEL, 1);
-	if (!bio)
-		return;
-
-	bio->bi_iter.bi_sector = 0; /* internal bio */
-	bio_set_op_attrs(bio, REQ_OP_WRITE, REQ_OP_FLUSH);
-	bio->bi_private = &wait;
-	bio->bi_end_io = pblk_end_bio_sync;
+	do {
+		if (!atomic_read(&pblk->inflight_io))
+			break;
 
-	ret = pblk_write_to_cache(pblk, bio, 0);
-	if (ret == NVM_IO_OK) {
-		if (!wait_for_completion_io_timeout(&wait,
-				msecs_to_jiffies(PBLK_COMMAND_TIMEOUT_MS))) {
-			pr_err("pblk: flush cache timed out\n");
-		}
-	} else if (ret != NVM_IO_DONE) {
-		pr_err("pblk: tear down bio failed\n");
-	}
+		schedule();
+	} while (1);
+}
 
-	if (bio->bi_error)
-		pr_err("pblk: flush sync write failed (%u)\n", bio->bi_error);
+static void pblk_flush_writer(struct pblk *pblk)
+{
+	pblk_rb_flush(&pblk->rwb);
+	do {
+		if (!pblk_rb_sync_count(&pblk->rwb))
+			break;
 
-	bio_put(bio);
+		pblk_write_kick(pblk);
+		schedule();
+	} while (1);
 }
 
 struct list_head *pblk_line_gc_list(struct pblk *pblk, struct pblk_line *line)
@@ -307,28 +286,31 @@ struct list_head *pblk_line_gc_list(struct pblk *pblk, struct pblk_line *line)
 	struct pblk_line_meta *lm = &pblk->lm;
 	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
 	struct list_head *move_list = NULL;
+	int vsc = le32_to_cpu(*line->vsc);
 
-	if (!line->vsc) {
+	lockdep_assert_held(&line->lock);
+
+	if (!vsc) {
 		if (line->gc_group != PBLK_LINEGC_FULL) {
 			line->gc_group = PBLK_LINEGC_FULL;
 			move_list = &l_mg->gc_full_list;
 		}
-	} else if (line->vsc < lm->mid_thrs) {
+	} else if (vsc < lm->high_thrs) {
 		if (line->gc_group != PBLK_LINEGC_HIGH) {
 			line->gc_group = PBLK_LINEGC_HIGH;
 			move_list = &l_mg->gc_high_list;
 		}
-	} else if (line->vsc < lm->high_thrs) {
+	} else if (vsc < lm->mid_thrs) {
 		if (line->gc_group != PBLK_LINEGC_MID) {
 			line->gc_group = PBLK_LINEGC_MID;
 			move_list = &l_mg->gc_mid_list;
 		}
-	} else if (line->vsc < line->sec_in_line) {
+	} else if (vsc < line->sec_in_line) {
 		if (line->gc_group != PBLK_LINEGC_LOW) {
 			line->gc_group = PBLK_LINEGC_LOW;
 			move_list = &l_mg->gc_low_list;
 		}
-	} else if (line->vsc == line->sec_in_line) {
+	} else if (vsc == line->sec_in_line) {
 		if (line->gc_group != PBLK_LINEGC_EMPTY) {
 			line->gc_group = PBLK_LINEGC_EMPTY;
 			move_list = &l_mg->gc_empty_list;
@@ -338,7 +320,7 @@ struct list_head *pblk_line_gc_list(struct pblk *pblk, struct pblk_line *line)
 		line->gc_group = PBLK_LINEGC_NONE;
 		move_list =  &l_mg->corrupt_list;
 		pr_err("pblk: corrupted vsc for line %d, vsc:%d (%d/%d/%d)\n",
-						line->id, line->vsc,
+						line->id, vsc,
 						line->sec_in_line,
 						lm->high_thrs, lm->mid_thrs);
 	}
@@ -397,6 +379,11 @@ void pblk_log_read_err(struct pblk *pblk, struct nvm_rq *rqd)
 #endif
 }
 
+void pblk_set_sec_per_write(struct pblk *pblk, int sec_per_write)
+{
+	pblk->sec_per_write = sec_per_write;
+}
+
 int pblk_submit_io(struct pblk *pblk, struct nvm_rq *rqd)
 {
 	struct nvm_tgt_dev *dev = pblk->dev;
@@ -431,21 +418,23 @@ int pblk_submit_io(struct pblk *pblk, struct nvm_rq *rqd)
 		}
 	}
 #endif
+
+	atomic_inc(&pblk->inflight_io);
+
 	return nvm_submit_io(dev, rqd);
 }
 
 struct bio *pblk_bio_map_addr(struct pblk *pblk, void *data,
 			      unsigned int nr_secs, unsigned int len,
-			      gfp_t gfp_mask)
+			      int alloc_type, gfp_t gfp_mask)
 {
 	struct nvm_tgt_dev *dev = pblk->dev;
-	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
 	void *kaddr = data;
 	struct page *page;
 	struct bio *bio;
 	int i, ret;
 
-	if (l_mg->emeta_alloc_type == PBLK_KMALLOC_META)
+	if (alloc_type == PBLK_KMALLOC_META)
 		return bio_map_kern(dev->q, kaddr, len, gfp_mask);
 
 	bio = bio_kmalloc(gfp_mask, nr_secs);
@@ -478,7 +467,7 @@ out:
 int pblk_calc_secs(struct pblk *pblk, unsigned long secs_avail,
 		   unsigned long secs_to_flush)
 {
-	int max = pblk->max_write_pgs;
+	int max = pblk->sec_per_write;
 	int min = pblk->min_write_pgs;
 	int secs_to_sync = 0;
 
@@ -492,12 +481,26 @@ int pblk_calc_secs(struct pblk *pblk, unsigned long secs_avail,
 	return secs_to_sync;
 }
 
-static u64 __pblk_alloc_page(struct pblk *pblk, struct pblk_line *line,
-			     int nr_secs)
+void pblk_dealloc_page(struct pblk *pblk, struct pblk_line *line, int nr_secs)
+{
+	u64 addr;
+	int i;
+
+	addr = find_next_zero_bit(line->map_bitmap,
+					pblk->lm.sec_per_line, line->cur_sec);
+	line->cur_sec = addr - nr_secs;
+
+	for (i = 0; i < nr_secs; i++, line->cur_sec--)
+		WARN_ON(!test_and_clear_bit(line->cur_sec, line->map_bitmap));
+}
+
+u64 __pblk_alloc_page(struct pblk *pblk, struct pblk_line *line, int nr_secs)
 {
 	u64 addr;
 	int i;
 
+	lockdep_assert_held(&line->lock);
+
 	/* logic error: ppa out-of-bounds. Prevent generating bad address */
 	if (line->cur_sec + nr_secs > pblk->lm.sec_per_line) {
 		WARN(1, "pblk: page allocation out of bounds\n");
@@ -528,27 +531,38 @@ u64 pblk_alloc_page(struct pblk *pblk, struct pblk_line *line, int nr_secs)
 	return addr;
 }
 
+u64 pblk_lookup_page(struct pblk *pblk, struct pblk_line *line)
+{
+	u64 paddr;
+
+	spin_lock(&line->lock);
+	paddr = find_next_zero_bit(line->map_bitmap,
+					pblk->lm.sec_per_line, line->cur_sec);
+	spin_unlock(&line->lock);
+
+	return paddr;
+}
+
 /*
  * Submit emeta to one LUN in the raid line at the time to avoid a deadlock when
  * taking the per LUN semaphore.
  */
 static int pblk_line_submit_emeta_io(struct pblk *pblk, struct pblk_line *line,
-				     u64 paddr, int dir)
+				     void *emeta_buf, u64 paddr, int dir)
 {
 	struct nvm_tgt_dev *dev = pblk->dev;
 	struct nvm_geo *geo = &dev->geo;
+	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
 	struct pblk_line_meta *lm = &pblk->lm;
+	void *ppa_list, *meta_list;
 	struct bio *bio;
 	struct nvm_rq rqd;
-	struct ppa_addr *ppa_list;
-	dma_addr_t dma_ppa_list;
-	void *emeta = line->emeta;
+	dma_addr_t dma_ppa_list, dma_meta_list;
 	int min = pblk->min_write_pgs;
-	int left_ppas = lm->emeta_sec;
+	int left_ppas = lm->emeta_sec[0];
 	int id = line->id;
 	int rq_ppas, rq_len;
 	int cmd_op, bio_op;
-	int flags;
 	int i, j;
 	int ret;
 	DECLARE_COMPLETION_ONSTACK(wait);
@@ -556,25 +570,28 @@ static int pblk_line_submit_emeta_io(struct pblk *pblk, struct pblk_line *line,
 	if (dir == WRITE) {
 		bio_op = REQ_OP_WRITE;
 		cmd_op = NVM_OP_PWRITE;
-		flags = pblk_set_progr_mode(pblk, WRITE);
 	} else if (dir == READ) {
 		bio_op = REQ_OP_READ;
 		cmd_op = NVM_OP_PREAD;
-		flags = pblk_set_read_mode(pblk);
 	} else
 		return -EINVAL;
 
-	ppa_list = nvm_dev_dma_alloc(dev->parent, GFP_KERNEL, &dma_ppa_list);
-	if (!ppa_list)
+	meta_list = nvm_dev_dma_alloc(dev->parent, GFP_KERNEL,
+							&dma_meta_list);
+	if (!meta_list)
 		return -ENOMEM;
 
+	ppa_list = meta_list + pblk_dma_meta_size;
+	dma_ppa_list = dma_meta_list + pblk_dma_meta_size;
+
 next_rq:
 	memset(&rqd, 0, sizeof(struct nvm_rq));
 
 	rq_ppas = pblk_calc_secs(pblk, left_ppas, 0);
 	rq_len = rq_ppas * geo->sec_size;
 
-	bio = pblk_bio_map_addr(pblk, emeta, rq_ppas, rq_len, GFP_KERNEL);
+	bio = pblk_bio_map_addr(pblk, emeta_buf, rq_ppas, rq_len,
+					l_mg->emeta_alloc_type, GFP_KERNEL);
 	if (IS_ERR(bio)) {
 		ret = PTR_ERR(bio);
 		goto free_rqd_dma;
@@ -584,27 +601,38 @@ next_rq:
 	bio_set_op_attrs(bio, bio_op, 0);
 
 	rqd.bio = bio;
-	rqd.opcode = cmd_op;
-	rqd.flags = flags;
-	rqd.nr_ppas = rq_ppas;
+	rqd.meta_list = meta_list;
 	rqd.ppa_list = ppa_list;
+	rqd.dma_meta_list = dma_meta_list;
 	rqd.dma_ppa_list = dma_ppa_list;
+	rqd.opcode = cmd_op;
+	rqd.nr_ppas = rq_ppas;
 	rqd.end_io = pblk_end_io_sync;
 	rqd.private = &wait;
 
 	if (dir == WRITE) {
+		struct pblk_sec_meta *meta_list = rqd.meta_list;
+
+		rqd.flags = pblk_set_progr_mode(pblk, WRITE);
 		for (i = 0; i < rqd.nr_ppas; ) {
 			spin_lock(&line->lock);
 			paddr = __pblk_alloc_page(pblk, line, min);
 			spin_unlock(&line->lock);
-			for (j = 0; j < min; j++, i++, paddr++)
+			for (j = 0; j < min; j++, i++, paddr++) {
+				meta_list[i].lba = cpu_to_le64(ADDR_EMPTY);
 				rqd.ppa_list[i] =
 					addr_to_gen_ppa(pblk, paddr, id);
+			}
 		}
 	} else {
 		for (i = 0; i < rqd.nr_ppas; ) {
 			struct ppa_addr ppa = addr_to_gen_ppa(pblk, paddr, id);
 			int pos = pblk_dev_ppa_to_pos(geo, ppa);
+			int read_type = PBLK_READ_RANDOM;
+
+			if (pblk_io_aligned(pblk, rq_ppas))
+				read_type = PBLK_READ_SEQUENTIAL;
+			rqd.flags = pblk_set_read_mode(pblk, read_type);
 
 			while (test_bit(pos, line->blk_bitmap)) {
 				paddr += min;
@@ -645,9 +673,11 @@ next_rq:
 				msecs_to_jiffies(PBLK_COMMAND_TIMEOUT_MS))) {
 		pr_err("pblk: emeta I/O timed out\n");
 	}
+	atomic_dec(&pblk->inflight_io);
 	reinit_completion(&wait);
 
-	bio_put(bio);
+	if (likely(pblk->l_mg.emeta_alloc_type == PBLK_VMALLOC_META))
+		bio_put(bio);
 
 	if (rqd.error) {
 		if (dir == WRITE)
@@ -656,12 +686,12 @@ next_rq:
 			pblk_log_read_err(pblk, &rqd);
 	}
 
-	emeta += rq_len;
+	emeta_buf += rq_len;
 	left_ppas -= rq_ppas;
 	if (left_ppas)
 		goto next_rq;
 free_rqd_dma:
-	nvm_dev_dma_free(dev->parent, ppa_list, dma_ppa_list);
+	nvm_dev_dma_free(dev->parent, rqd.meta_list, rqd.dma_meta_list);
 	return ret;
 }
 
@@ -697,21 +727,24 @@ static int pblk_line_submit_smeta_io(struct pblk *pblk, struct pblk_line *line,
 		bio_op = REQ_OP_WRITE;
 		cmd_op = NVM_OP_PWRITE;
 		flags = pblk_set_progr_mode(pblk, WRITE);
-		lba_list = pblk_line_emeta_to_lbas(line->emeta);
+		lba_list = emeta_to_lbas(pblk, line->emeta->buf);
 	} else if (dir == READ) {
 		bio_op = REQ_OP_READ;
 		cmd_op = NVM_OP_PREAD;
-		flags = pblk_set_read_mode(pblk);
+		flags = pblk_set_read_mode(pblk, PBLK_READ_SEQUENTIAL);
 	} else
 		return -EINVAL;
 
 	memset(&rqd, 0, sizeof(struct nvm_rq));
 
-	rqd.ppa_list = nvm_dev_dma_alloc(dev->parent, GFP_KERNEL,
-							&rqd.dma_ppa_list);
-	if (!rqd.ppa_list)
+	rqd.meta_list = nvm_dev_dma_alloc(dev->parent, GFP_KERNEL,
+							&rqd.dma_meta_list);
+	if (!rqd.meta_list)
 		return -ENOMEM;
 
+	rqd.ppa_list = rqd.meta_list + pblk_dma_meta_size;
+	rqd.dma_ppa_list = rqd.dma_meta_list + pblk_dma_meta_size;
+
 	bio = bio_map_kern(dev->q, line->smeta, lm->smeta_len, GFP_KERNEL);
 	if (IS_ERR(bio)) {
 		ret = PTR_ERR(bio);
@@ -729,9 +762,15 @@ static int pblk_line_submit_smeta_io(struct pblk *pblk, struct pblk_line *line,
 	rqd.private = &wait;
 
 	for (i = 0; i < lm->smeta_sec; i++, paddr++) {
+		struct pblk_sec_meta *meta_list = rqd.meta_list;
+
 		rqd.ppa_list[i] = addr_to_gen_ppa(pblk, paddr, line->id);
-		if (dir == WRITE)
-			lba_list[paddr] = cpu_to_le64(ADDR_EMPTY);
+
+		if (dir == WRITE) {
+			__le64 addr_empty = cpu_to_le64(ADDR_EMPTY);
+
+			meta_list[i].lba = lba_list[paddr] = addr_empty;
+		}
 	}
 
 	/*
@@ -750,6 +789,7 @@ static int pblk_line_submit_smeta_io(struct pblk *pblk, struct pblk_line *line,
 				msecs_to_jiffies(PBLK_COMMAND_TIMEOUT_MS))) {
 		pr_err("pblk: smeta I/O timed out\n");
 	}
+	atomic_dec(&pblk->inflight_io);
 
 	if (rqd.error) {
 		if (dir == WRITE)
@@ -759,7 +799,7 @@ static int pblk_line_submit_smeta_io(struct pblk *pblk, struct pblk_line *line,
 	}
 
 free_ppa_list:
-	nvm_dev_dma_free(dev->parent, rqd.ppa_list, rqd.dma_ppa_list);
+	nvm_dev_dma_free(dev->parent, rqd.meta_list, rqd.dma_meta_list);
 
 	return ret;
 }
@@ -771,9 +811,11 @@ int pblk_line_read_smeta(struct pblk *pblk, struct pblk_line *line)
 	return pblk_line_submit_smeta_io(pblk, line, bpaddr, READ);
 }
 
-int pblk_line_read_emeta(struct pblk *pblk, struct pblk_line *line)
+int pblk_line_read_emeta(struct pblk *pblk, struct pblk_line *line,
+			 void *emeta_buf)
 {
-	return pblk_line_submit_emeta_io(pblk, line, line->emeta_ssec, READ);
+	return pblk_line_submit_emeta_io(pblk, line, emeta_buf,
+						line->emeta_ssec, READ);
 }
 
 static void pblk_setup_e_rq(struct pblk *pblk, struct nvm_rq *rqd,
@@ -789,7 +831,7 @@ static void pblk_setup_e_rq(struct pblk *pblk, struct nvm_rq *rqd,
 static int pblk_blk_erase_sync(struct pblk *pblk, struct ppa_addr ppa)
 {
 	struct nvm_rq rqd;
-	int ret;
+	int ret = 0;
 	DECLARE_COMPLETION_ONSTACK(wait);
 
 	memset(&rqd, 0, sizeof(struct nvm_rq));
@@ -824,14 +866,14 @@ out:
 	rqd.private = pblk;
 	__pblk_end_io_erase(pblk, &rqd);
 
-	return 0;
+	return ret;
 }
 
 int pblk_line_erase(struct pblk *pblk, struct pblk_line *line)
 {
 	struct pblk_line_meta *lm = &pblk->lm;
 	struct ppa_addr ppa;
-	int bit = -1;
+	int ret, bit = -1;
 
 	/* Erase only good blocks, one at a time */
 	do {
@@ -850,27 +892,59 @@ int pblk_line_erase(struct pblk *pblk, struct pblk_line *line)
 		WARN_ON(test_and_set_bit(bit, line->erase_bitmap));
 		spin_unlock(&line->lock);
 
-		if (pblk_blk_erase_sync(pblk, ppa)) {
+		ret = pblk_blk_erase_sync(pblk, ppa);
+		if (ret) {
 			pr_err("pblk: failed to erase line %d\n", line->id);
-			return -ENOMEM;
+			return ret;
 		}
 	} while (1);
 
 	return 0;
 }
 
+static void pblk_line_setup_metadata(struct pblk_line *line,
+				     struct pblk_line_mgmt *l_mg,
+				     struct pblk_line_meta *lm)
+{
+	int meta_line;
+
+	lockdep_assert_held(&l_mg->free_lock);
+
+retry_meta:
+	meta_line = find_first_zero_bit(&l_mg->meta_bitmap, PBLK_DATA_LINES);
+	if (meta_line == PBLK_DATA_LINES) {
+		spin_unlock(&l_mg->free_lock);
+		io_schedule();
+		spin_lock(&l_mg->free_lock);
+		goto retry_meta;
+	}
+
+	set_bit(meta_line, &l_mg->meta_bitmap);
+	line->meta_line = meta_line;
+
+	line->smeta = l_mg->sline_meta[meta_line];
+	line->emeta = l_mg->eline_meta[meta_line];
+
+	memset(line->smeta, 0, lm->smeta_len);
+	memset(line->emeta->buf, 0, lm->emeta_len[0]);
+
+	line->emeta->mem = 0;
+	atomic_set(&line->emeta->sync, 0);
+}
+
 /* For now lines are always assumed full lines. Thus, smeta former and current
  * lun bitmaps are omitted.
  */
-static int pblk_line_set_metadata(struct pblk *pblk, struct pblk_line *line,
+static int pblk_line_init_metadata(struct pblk *pblk, struct pblk_line *line,
 				  struct pblk_line *cur)
 {
 	struct nvm_tgt_dev *dev = pblk->dev;
 	struct nvm_geo *geo = &dev->geo;
 	struct pblk_line_meta *lm = &pblk->lm;
 	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
-	struct line_smeta *smeta = line->smeta;
-	struct line_emeta *emeta = line->emeta;
+	struct pblk_emeta *emeta = line->emeta;
+	struct line_emeta *emeta_buf = emeta->buf;
+	struct line_smeta *smeta_buf = (struct line_smeta *)line->smeta;
 	int nr_blk_line;
 
 	/* After erasing the line, new bad blocks might appear and we risk
@@ -893,42 +967,44 @@ static int pblk_line_set_metadata(struct pblk *pblk, struct pblk_line *line,
 	}
 
 	/* Run-time metadata */
-	line->lun_bitmap = ((void *)(smeta)) + sizeof(struct line_smeta);
+	line->lun_bitmap = ((void *)(smeta_buf)) + sizeof(struct line_smeta);
 
 	/* Mark LUNs allocated in this line (all for now) */
 	bitmap_set(line->lun_bitmap, 0, lm->lun_bitmap_len);
 
-	smeta->header.identifier = cpu_to_le32(PBLK_MAGIC);
-	memcpy(smeta->header.uuid, pblk->instance_uuid, 16);
-	smeta->header.id = cpu_to_le32(line->id);
-	smeta->header.type = cpu_to_le16(line->type);
-	smeta->header.version = cpu_to_le16(1);
+	smeta_buf->header.identifier = cpu_to_le32(PBLK_MAGIC);
+	memcpy(smeta_buf->header.uuid, pblk->instance_uuid, 16);
+	smeta_buf->header.id = cpu_to_le32(line->id);
+	smeta_buf->header.type = cpu_to_le16(line->type);
+	smeta_buf->header.version = cpu_to_le16(1);
 
 	/* Start metadata */
-	smeta->seq_nr = cpu_to_le64(line->seq_nr);
-	smeta->window_wr_lun = cpu_to_le32(geo->nr_luns);
+	smeta_buf->seq_nr = cpu_to_le64(line->seq_nr);
+	smeta_buf->window_wr_lun = cpu_to_le32(geo->nr_luns);
 
 	/* Fill metadata among lines */
 	if (cur) {
 		memcpy(line->lun_bitmap, cur->lun_bitmap, lm->lun_bitmap_len);
-		smeta->prev_id = cpu_to_le32(cur->id);
-		cur->emeta->next_id = cpu_to_le32(line->id);
+		smeta_buf->prev_id = cpu_to_le32(cur->id);
+		cur->emeta->buf->next_id = cpu_to_le32(line->id);
 	} else {
-		smeta->prev_id = cpu_to_le32(PBLK_LINE_EMPTY);
+		smeta_buf->prev_id = cpu_to_le32(PBLK_LINE_EMPTY);
 	}
 
 	/* All smeta must be set at this point */
-	smeta->header.crc = cpu_to_le32(pblk_calc_meta_header_crc(pblk, smeta));
-	smeta->crc = cpu_to_le32(pblk_calc_smeta_crc(pblk, smeta));
+	smeta_buf->header.crc = cpu_to_le32(
+			pblk_calc_meta_header_crc(pblk, &smeta_buf->header));
+	smeta_buf->crc = cpu_to_le32(pblk_calc_smeta_crc(pblk, smeta_buf));
 
 	/* End metadata */
-	memcpy(&emeta->header, &smeta->header, sizeof(struct line_header));
-	emeta->seq_nr = cpu_to_le64(line->seq_nr);
-	emeta->nr_lbas = cpu_to_le64(line->sec_in_line);
-	emeta->nr_valid_lbas = cpu_to_le64(0);
-	emeta->next_id = cpu_to_le32(PBLK_LINE_EMPTY);
-	emeta->crc = cpu_to_le32(0);
-	emeta->prev_id = smeta->prev_id;
+	memcpy(&emeta_buf->header, &smeta_buf->header,
+						sizeof(struct line_header));
+	emeta_buf->seq_nr = cpu_to_le64(line->seq_nr);
+	emeta_buf->nr_lbas = cpu_to_le64(line->sec_in_line);
+	emeta_buf->nr_valid_lbas = cpu_to_le64(0);
+	emeta_buf->next_id = cpu_to_le32(PBLK_LINE_EMPTY);
+	emeta_buf->crc = cpu_to_le32(0);
+	emeta_buf->prev_id = smeta_buf->prev_id;
 
 	return 1;
 }
@@ -965,7 +1041,6 @@ static int pblk_line_init_bb(struct pblk *pblk, struct pblk_line *line,
 	/* Mark smeta metadata sectors as bad sectors */
 	bit = find_first_zero_bit(line->blk_bitmap, lm->blk_per_line);
 	off = bit * geo->sec_per_pl;
-retry_smeta:
 	bitmap_set(line->map_bitmap, off, lm->smeta_sec);
 	line->sec_in_line -= lm->smeta_sec;
 	line->smeta_ssec = off;
@@ -973,8 +1048,7 @@ retry_smeta:
 
 	if (init && pblk_line_submit_smeta_io(pblk, line, off, WRITE)) {
 		pr_debug("pblk: line smeta I/O failed. Retry\n");
-		off += geo->sec_per_pl;
-		goto retry_smeta;
+		return 1;
 	}
 
 	bitmap_copy(line->invalid_bitmap, line->map_bitmap, lm->sec_per_line);
@@ -983,8 +1057,8 @@ retry_smeta:
 	 * blocks to make sure that there are enough sectors to store emeta
 	 */
 	bit = lm->sec_per_line;
-	off = lm->sec_per_line - lm->emeta_sec;
-	bitmap_set(line->invalid_bitmap, off, lm->emeta_sec);
+	off = lm->sec_per_line - lm->emeta_sec[0];
+	bitmap_set(line->invalid_bitmap, off, lm->emeta_sec[0]);
 	while (nr_bb) {
 		off -= geo->sec_per_pl;
 		if (!test_bit(off, line->invalid_bitmap)) {
@@ -993,9 +1067,11 @@ retry_smeta:
 		}
 	}
 
-	line->sec_in_line -= lm->emeta_sec;
+	line->sec_in_line -= lm->emeta_sec[0];
 	line->emeta_ssec = off;
-	line->vsc = line->left_ssecs = line->left_msecs = line->sec_in_line;
+	line->nr_valid_lbas = 0;
+	line->left_msecs = line->sec_in_line;
+	*line->vsc = cpu_to_le32(line->sec_in_line);
 
 	if (lm->sec_per_line - line->sec_in_line !=
 		bitmap_weight(line->invalid_bitmap, lm->sec_per_line)) {
@@ -1034,14 +1110,20 @@ static int pblk_line_prepare(struct pblk *pblk, struct pblk_line *line)
 
 	spin_lock(&line->lock);
 	if (line->state != PBLK_LINESTATE_FREE) {
+		mempool_free(line->invalid_bitmap, pblk->line_meta_pool);
+		mempool_free(line->map_bitmap, pblk->line_meta_pool);
 		spin_unlock(&line->lock);
-		WARN(1, "pblk: corrupted line state\n");
-		return -EINTR;
+		WARN(1, "pblk: corrupted line %d, state %d\n",
+							line->id, line->state);
+		return -EAGAIN;
 	}
+
 	line->state = PBLK_LINESTATE_OPEN;
 
 	atomic_set(&line->left_eblks, blk_in_line);
 	atomic_set(&line->left_seblks, blk_in_line);
+
+	line->meta_distance = lm->meta_distance;
 	spin_unlock(&line->lock);
 
 	/* Bad blocks do not need to be erased */
@@ -1091,15 +1173,15 @@ struct pblk_line *pblk_line_get(struct pblk *pblk)
 {
 	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
 	struct pblk_line_meta *lm = &pblk->lm;
-	struct pblk_line *line = NULL;
-	int bit;
+	struct pblk_line *line;
+	int ret, bit;
 
 	lockdep_assert_held(&l_mg->free_lock);
 
-retry_get:
+retry:
 	if (list_empty(&l_mg->free_list)) {
 		pr_err("pblk: no free lines\n");
-		goto out;
+		return NULL;
 	}
 
 	line = list_first_entry(&l_mg->free_list, struct pblk_line, list);
@@ -1115,16 +1197,22 @@ retry_get:
 		list_add_tail(&line->list, &l_mg->bad_list);
 
 		pr_debug("pblk: line %d is bad\n", line->id);
-		goto retry_get;
+		goto retry;
 	}
 
-	if (pblk_line_prepare(pblk, line)) {
-		pr_err("pblk: failed to prepare line %d\n", line->id);
-		list_add(&line->list, &l_mg->free_list);
-		return NULL;
+	ret = pblk_line_prepare(pblk, line);
+	if (ret) {
+		if (ret == -EAGAIN) {
+			list_add(&line->list, &l_mg->corrupt_list);
+			goto retry;
+		} else {
+			pr_err("pblk: failed to prepare line %d\n", line->id);
+			list_add(&line->list, &l_mg->free_list);
+			l_mg->nr_free_lines++;
+			return NULL;
+		}
 	}
 
-out:
 	return line;
 }
 
@@ -1134,6 +1222,7 @@ static struct pblk_line *pblk_line_retry(struct pblk *pblk,
 	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
 	struct pblk_line *retry_line;
 
+retry:
 	spin_lock(&l_mg->free_lock);
 	retry_line = pblk_line_get(pblk);
 	if (!retry_line) {
@@ -1150,23 +1239,25 @@ static struct pblk_line *pblk_line_retry(struct pblk *pblk,
 	l_mg->data_line = retry_line;
 	spin_unlock(&l_mg->free_lock);
 
-	if (pblk_line_erase(pblk, retry_line)) {
-		spin_lock(&l_mg->free_lock);
-		l_mg->data_line = NULL;
-		spin_unlock(&l_mg->free_lock);
-		return NULL;
-	}
-
 	pblk_rl_free_lines_dec(&pblk->rl, retry_line);
 
+	if (pblk_line_erase(pblk, retry_line))
+		goto retry;
+
 	return retry_line;
 }
 
+static void pblk_set_space_limit(struct pblk *pblk)
+{
+	struct pblk_rl *rl = &pblk->rl;
+
+	atomic_set(&rl->rb_space, 0);
+}
+
 struct pblk_line *pblk_line_get_first_data(struct pblk *pblk)
 {
 	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
 	struct pblk_line *line;
-	int meta_line;
 	int is_next = 0;
 
 	spin_lock(&l_mg->free_lock);
@@ -1180,30 +1271,37 @@ struct pblk_line *pblk_line_get_first_data(struct pblk *pblk)
 	line->type = PBLK_LINETYPE_DATA;
 	l_mg->data_line = line;
 
-	meta_line = find_first_zero_bit(&l_mg->meta_bitmap, PBLK_DATA_LINES);
-	set_bit(meta_line, &l_mg->meta_bitmap);
-	line->smeta = l_mg->sline_meta[meta_line].meta;
-	line->emeta = l_mg->eline_meta[meta_line].meta;
-	line->meta_line = meta_line;
+	pblk_line_setup_metadata(line, l_mg, &pblk->lm);
 
 	/* Allocate next line for preparation */
 	l_mg->data_next = pblk_line_get(pblk);
-	if (l_mg->data_next) {
+	if (!l_mg->data_next) {
+		/* If we cannot get a new line, we need to stop the pipeline.
+		 * Only allow as many writes in as we can store safely and then
+		 * fail gracefully
+		 */
+		pblk_set_space_limit(pblk);
+
+		l_mg->data_next = NULL;
+	} else {
 		l_mg->data_next->seq_nr = l_mg->d_seq_nr++;
 		l_mg->data_next->type = PBLK_LINETYPE_DATA;
 		is_next = 1;
 	}
 	spin_unlock(&l_mg->free_lock);
 
+	if (pblk_line_erase(pblk, line)) {
+		line = pblk_line_retry(pblk, line);
+		if (!line)
+			return NULL;
+	}
+
 	pblk_rl_free_lines_dec(&pblk->rl, line);
 	if (is_next)
 		pblk_rl_free_lines_dec(&pblk->rl, l_mg->data_next);
 
-	if (pblk_line_erase(pblk, line))
-		return NULL;
-
 retry_setup:
-	if (!pblk_line_set_metadata(pblk, line, NULL)) {
+	if (!pblk_line_init_metadata(pblk, line, NULL)) {
 		line = pblk_line_retry(pblk, line);
 		if (!line)
 			return NULL;
@@ -1222,69 +1320,89 @@ retry_setup:
 	return line;
 }
 
-struct pblk_line *pblk_line_replace_data(struct pblk *pblk)
+static void pblk_stop_writes(struct pblk *pblk, struct pblk_line *line)
+{
+	lockdep_assert_held(&pblk->l_mg.free_lock);
+
+	pblk_set_space_limit(pblk);
+	pblk->state = PBLK_STATE_STOPPING;
+}
+
+void pblk_pipeline_stop(struct pblk *pblk)
+{
+	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
+	int ret;
+
+	spin_lock(&l_mg->free_lock);
+	if (pblk->state == PBLK_STATE_RECOVERING ||
+					pblk->state == PBLK_STATE_STOPPED) {
+		spin_unlock(&l_mg->free_lock);
+		return;
+	}
+	pblk->state = PBLK_STATE_RECOVERING;
+	spin_unlock(&l_mg->free_lock);
+
+	pblk_flush_writer(pblk);
+	pblk_wait_for_meta(pblk);
+
+	ret = pblk_recov_pad(pblk);
+	if (ret) {
+		pr_err("pblk: could not close data on teardown(%d)\n", ret);
+		return;
+	}
+
+	flush_workqueue(pblk->bb_wq);
+	pblk_line_close_meta_sync(pblk);
+
+	spin_lock(&l_mg->free_lock);
+	pblk->state = PBLK_STATE_STOPPED;
+	l_mg->data_line = NULL;
+	l_mg->data_next = NULL;
+	spin_unlock(&l_mg->free_lock);
+}
+
+void pblk_line_replace_data(struct pblk *pblk)
 {
-	struct pblk_line_meta *lm = &pblk->lm;
 	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
 	struct pblk_line *cur, *new;
 	unsigned int left_seblks;
-	int meta_line;
 	int is_next = 0;
 
 	cur = l_mg->data_line;
 	new = l_mg->data_next;
 	if (!new)
-		return NULL;
+		return;
 	l_mg->data_line = new;
 
-retry_line:
+	spin_lock(&l_mg->free_lock);
+	if (pblk->state != PBLK_STATE_RUNNING) {
+		l_mg->data_line = NULL;
+		l_mg->data_next = NULL;
+		spin_unlock(&l_mg->free_lock);
+		return;
+	}
+
+	pblk_line_setup_metadata(new, l_mg, &pblk->lm);
+	spin_unlock(&l_mg->free_lock);
+
+retry_erase:
 	left_seblks = atomic_read(&new->left_seblks);
 	if (left_seblks) {
 		/* If line is not fully erased, erase it */
 		if (atomic_read(&new->left_eblks)) {
 			if (pblk_line_erase(pblk, new))
-				return NULL;
+				return;
 		} else {
 			io_schedule();
 		}
-		goto retry_line;
+		goto retry_erase;
 	}
 
-	spin_lock(&l_mg->free_lock);
-	/* Allocate next line for preparation */
-	l_mg->data_next = pblk_line_get(pblk);
-	if (l_mg->data_next) {
-		l_mg->data_next->seq_nr = l_mg->d_seq_nr++;
-		l_mg->data_next->type = PBLK_LINETYPE_DATA;
-		is_next = 1;
-	}
-
-retry_meta:
-	meta_line = find_first_zero_bit(&l_mg->meta_bitmap, PBLK_DATA_LINES);
-	if (meta_line == PBLK_DATA_LINES) {
-		spin_unlock(&l_mg->free_lock);
-		io_schedule();
-		spin_lock(&l_mg->free_lock);
-		goto retry_meta;
-	}
-
-	set_bit(meta_line, &l_mg->meta_bitmap);
-	new->smeta = l_mg->sline_meta[meta_line].meta;
-	new->emeta = l_mg->eline_meta[meta_line].meta;
-	new->meta_line = meta_line;
-
-	memset(new->smeta, 0, lm->smeta_len);
-	memset(new->emeta, 0, lm->emeta_len);
-	spin_unlock(&l_mg->free_lock);
-
-	if (is_next)
-		pblk_rl_free_lines_dec(&pblk->rl, l_mg->data_next);
-
 retry_setup:
-	if (!pblk_line_set_metadata(pblk, new, cur)) {
+	if (!pblk_line_init_metadata(pblk, new, cur)) {
 		new = pblk_line_retry(pblk, new);
 		if (!new)
-			return NULL;
+			return;
 
 		goto retry_setup;
 	}
@@ -1292,12 +1410,30 @@ retry_setup:
 	if (!pblk_line_init_bb(pblk, new, 1)) {
 		new = pblk_line_retry(pblk, new);
 		if (!new)
-			return NULL;
+			return;
 
 		goto retry_setup;
 	}
 
-	return new;
+	/* Allocate next line for preparation */
+	spin_lock(&l_mg->free_lock);
+	l_mg->data_next = pblk_line_get(pblk);
+	if (!l_mg->data_next) {
+		/* If we cannot get a new line, we need to stop the pipeline.
+		 * Only allow as many writes in as we can store safely and then
+		 * fail gracefully
+		 */
+		pblk_stop_writes(pblk, new);
+		l_mg->data_next = NULL;
+	} else {
+		l_mg->data_next->seq_nr = l_mg->d_seq_nr++;
+		l_mg->data_next->type = PBLK_LINETYPE_DATA;
+		is_next = 1;
+	}
+	spin_unlock(&l_mg->free_lock);
+
+	if (is_next)
+		pblk_rl_free_lines_dec(&pblk->rl, l_mg->data_next);
 }
 
 void pblk_line_free(struct pblk *pblk, struct pblk_line *line)
@@ -1307,6 +1443,8 @@ void pblk_line_free(struct pblk *pblk, struct pblk_line *line)
 	if (line->invalid_bitmap)
 		mempool_free(line->invalid_bitmap, pblk->line_meta_pool);
 
+	*line->vsc = cpu_to_le32(EMPTY_ENTRY);
+
 	line->map_bitmap = NULL;
 	line->invalid_bitmap = NULL;
 	line->smeta = NULL;
@@ -1339,8 +1477,8 @@ int pblk_blk_erase_async(struct pblk *pblk, struct ppa_addr ppa)
 	struct nvm_rq *rqd;
 	int err;
 
-	rqd = mempool_alloc(pblk->r_rq_pool, GFP_KERNEL);
-	memset(rqd, 0, pblk_r_rq_size);
+	rqd = mempool_alloc(pblk->g_rq_pool, GFP_KERNEL);
+	memset(rqd, 0, pblk_g_rq_size);
 
 	pblk_setup_e_rq(pblk, rqd, ppa);
 
@@ -1368,7 +1506,8 @@ struct pblk_line *pblk_line_get_data(struct pblk *pblk)
 	return pblk->l_mg.data_line;
 }
 
-struct pblk_line *pblk_line_get_data_next(struct pblk *pblk)
+/* For now, always erase next line */
+struct pblk_line *pblk_line_get_erase(struct pblk *pblk)
 {
 	return pblk->l_mg.data_next;
 }
@@ -1378,18 +1517,58 @@ int pblk_line_is_full(struct pblk_line *line)
 	return (line->left_msecs == 0);
 }
 
+void pblk_line_close_meta_sync(struct pblk *pblk)
+{
+	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
+	struct pblk_line_meta *lm = &pblk->lm;
+	struct pblk_line *line, *tline;
+	LIST_HEAD(list);
+
+	spin_lock(&l_mg->close_lock);
+	if (list_empty(&l_mg->emeta_list)) {
+		spin_unlock(&l_mg->close_lock);
+		return;
+	}
+
+	list_cut_position(&list, &l_mg->emeta_list, l_mg->emeta_list.prev);
+	spin_unlock(&l_mg->close_lock);
+
+	list_for_each_entry_safe(line, tline, &list, list) {
+		struct pblk_emeta *emeta = line->emeta;
+
+		while (emeta->mem < lm->emeta_len[0]) {
+			int ret;
+
+			ret = pblk_submit_meta_io(pblk, line);
+			if (ret) {
+				pr_err("pblk: sync meta line %d failed (%d)\n",
+							line->id, ret);
+				return;
+			}
+		}
+	}
+
+	pblk_wait_for_meta(pblk);
+	flush_workqueue(pblk->close_wq);
+}
+
+static void pblk_line_should_sync_meta(struct pblk *pblk)
+{
+	if (pblk_rl_is_limit(&pblk->rl))
+		pblk_line_close_meta_sync(pblk);
+}
+
 void pblk_line_close(struct pblk *pblk, struct pblk_line *line)
 {
 	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
 	struct list_head *move_list;
 
-	line->emeta->crc = cpu_to_le32(pblk_calc_emeta_crc(pblk, line->emeta));
-
-	if (pblk_line_submit_emeta_io(pblk, line, line->cur_sec, WRITE))
-		pr_err("pblk: line %d close I/O failed\n", line->id);
+#ifdef CONFIG_NVM_DEBUG
+	struct pblk_line_meta *lm = &pblk->lm;
 
-	WARN(!bitmap_full(line->map_bitmap, line->sec_in_line),
+	WARN(!bitmap_full(line->map_bitmap, lm->sec_per_line),
 				"pblk: corrupt closed line %d\n", line->id);
+#endif
 
 	spin_lock(&l_mg->free_lock);
 	WARN_ON(!test_and_clear_bit(line->meta_line, &l_mg->meta_bitmap));
@@ -1410,6 +1589,31 @@ void pblk_line_close(struct pblk *pblk, struct pblk_line *line)
 
 	spin_unlock(&line->lock);
 	spin_unlock(&l_mg->gc_lock);
+
+	pblk_gc_should_kick(pblk);
+}
+
+void pblk_line_close_meta(struct pblk *pblk, struct pblk_line *line)
+{
+	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
+	struct pblk_line_meta *lm = &pblk->lm;
+	struct pblk_emeta *emeta = line->emeta;
+	struct line_emeta *emeta_buf = emeta->buf;
+
+	/* No need for exact vsc value; avoid a big line lock and take aprox. */
+	memcpy(emeta_to_vsc(pblk, emeta_buf), l_mg->vsc_list, lm->vsc_list_len);
+	memcpy(emeta_to_bb(emeta_buf), line->blk_bitmap, lm->blk_bitmap_len);
+
+	emeta_buf->nr_valid_lbas = cpu_to_le64(line->nr_valid_lbas);
+	emeta_buf->crc = cpu_to_le32(pblk_calc_emeta_crc(pblk, emeta_buf));
+
+	spin_lock(&l_mg->close_lock);
+	spin_lock(&line->lock);
+	list_add_tail(&line->list, &l_mg->emeta_list);
+	spin_unlock(&line->lock);
+	spin_unlock(&l_mg->close_lock);
+
+	pblk_line_should_sync_meta(pblk);
 }
 
 void pblk_line_close_ws(struct work_struct *work)
@@ -1449,7 +1653,8 @@ void pblk_line_mark_bb(struct work_struct *work)
 }
 
 void pblk_line_run_ws(struct pblk *pblk, struct pblk_line *line, void *priv,
-		      void (*work)(struct work_struct *))
+		      void (*work)(struct work_struct *),
+		      struct workqueue_struct *wq)
 {
 	struct pblk_line_ws *line_ws;
 
@@ -1462,7 +1667,7 @@ void pblk_line_run_ws(struct pblk *pblk, struct pblk_line *line, void *priv,
 	line_ws->priv = priv;
 
 	INIT_WORK(&line_ws->ws, work);
-	queue_work(pblk->kw_wq, &line_ws->ws);
+	queue_work(wq, &line_ws->ws);
 }
 
 void pblk_down_rq(struct pblk *pblk, struct ppa_addr *ppa_list, int nr_ppas,
@@ -1471,7 +1676,7 @@ void pblk_down_rq(struct pblk *pblk, struct ppa_addr *ppa_list, int nr_ppas,
 	struct nvm_tgt_dev *dev = pblk->dev;
 	struct nvm_geo *geo = &dev->geo;
 	struct pblk_lun *rlun;
-	int lun_id = ppa_list[0].g.ch * geo->luns_per_chnl + ppa_list[0].g.lun;
+	int pos = pblk_ppa_to_pos(geo, ppa_list[0]);
 	int ret;
 
 	/*
@@ -1488,10 +1693,10 @@ void pblk_down_rq(struct pblk *pblk, struct ppa_addr *ppa_list, int nr_ppas,
 	/* If the LUN has been locked for this same request, do no attempt to
 	 * lock it again
 	 */
-	if (test_and_set_bit(lun_id, lun_bitmap))
+	if (test_and_set_bit(pos, lun_bitmap))
 		return;
 
-	rlun = &pblk->luns[lun_id];
+	rlun = &pblk->luns[pos];
 	ret = down_timeout(&rlun->wr_sem, msecs_to_jiffies(5000));
 	if (ret) {
 		switch (ret) {
diff --git a/drivers/lightnvm/pblk-gc.c b/drivers/lightnvm/pblk-gc.c
index eaf479c6b63c..6090d28f7995 100644
--- a/drivers/lightnvm/pblk-gc.c
+++ b/drivers/lightnvm/pblk-gc.c
@@ -20,8 +20,7 @@
 
 static void pblk_gc_free_gc_rq(struct pblk_gc_rq *gc_rq)
 {
-	kfree(gc_rq->data);
-	kfree(gc_rq->lba_list);
+	vfree(gc_rq->data);
 	kfree(gc_rq);
 }
 
@@ -37,10 +36,8 @@ static int pblk_gc_write(struct pblk *pblk)
 		return 1;
 	}
 
-	list_for_each_entry_safe(gc_rq, tgc_rq, &gc->w_list, list) {
-		list_move_tail(&gc_rq->list, &w_list);
-		gc->w_entries--;
-	}
+	list_cut_position(&w_list, &gc->w_list, gc->w_list.prev);
+	gc->w_entries = 0;
 	spin_unlock(&gc->w_lock);
 
 	list_for_each_entry_safe(gc_rq, tgc_rq, &w_list, list) {
@@ -48,9 +45,8 @@ static int pblk_gc_write(struct pblk *pblk)
 				gc_rq->nr_secs, gc_rq->secs_to_gc,
 				gc_rq->line, PBLK_IOTYPE_GC);
 
-		kref_put(&gc_rq->line->ref, pblk_line_put);
-
 		list_del(&gc_rq->list);
+		kref_put(&gc_rq->line->ref, pblk_line_put);
 		pblk_gc_free_gc_rq(gc_rq);
 	}
 
@@ -66,52 +62,41 @@ static void pblk_gc_writer_kick(struct pblk_gc *gc)
  * Responsible for managing all memory related to a gc request. Also in case of
  * failure
  */
-static int pblk_gc_move_valid_secs(struct pblk *pblk, struct pblk_line *line,
-				   u64 *lba_list, unsigned int nr_secs)
+static int pblk_gc_move_valid_secs(struct pblk *pblk, struct pblk_gc_rq *gc_rq)
 {
 	struct nvm_tgt_dev *dev = pblk->dev;
 	struct nvm_geo *geo = &dev->geo;
 	struct pblk_gc *gc = &pblk->gc;
-	struct pblk_gc_rq *gc_rq;
+	struct pblk_line *line = gc_rq->line;
 	void *data;
 	unsigned int secs_to_gc;
-	int ret = NVM_IO_OK;
+	int ret = 0;
 
-	data = kmalloc(nr_secs * geo->sec_size, GFP_KERNEL);
+	data = vmalloc(gc_rq->nr_secs * geo->sec_size);
 	if (!data) {
-		ret = NVM_IO_ERR;
-		goto free_lba_list;
+		ret = -ENOMEM;
+		goto out;
 	}
 
 	/* Read from GC victim block */
-	if (pblk_submit_read_gc(pblk, lba_list, data, nr_secs,
+	if (pblk_submit_read_gc(pblk, gc_rq->lba_list, data, gc_rq->nr_secs,
 							&secs_to_gc, line)) {
-		ret = NVM_IO_ERR;
+		ret = -EFAULT;
 		goto free_data;
 	}
 
 	if (!secs_to_gc)
-		goto free_data;
-
-	gc_rq = kmalloc(sizeof(struct pblk_gc_rq), GFP_KERNEL);
-	if (!gc_rq) {
-		ret = NVM_IO_ERR;
-		goto free_data;
-	}
+		goto free_rq;
 
-	gc_rq->line = line;
 	gc_rq->data = data;
-	gc_rq->lba_list = lba_list;
-	gc_rq->nr_secs = nr_secs;
 	gc_rq->secs_to_gc = secs_to_gc;
 
-	kref_get(&line->ref);
-
 retry:
 	spin_lock(&gc->w_lock);
-	if (gc->w_entries > 256) {
+	if (gc->w_entries >= PBLK_GC_W_QD) {
 		spin_unlock(&gc->w_lock);
-		usleep_range(256, 1024);
+		pblk_gc_writer_kick(&pblk->gc);
+		usleep_range(128, 256);
 		goto retry;
 	}
 	gc->w_entries++;
@@ -120,13 +105,14 @@ retry:
 
 	pblk_gc_writer_kick(&pblk->gc);
 
-	return NVM_IO_OK;
+	return 0;
 
+free_rq:
+	kfree(gc_rq);
 free_data:
-	kfree(data);
-free_lba_list:
-	kfree(lba_list);
-
+	vfree(data);
+out:
+	kref_put(&line->ref, pblk_line_put);
 	return ret;
 }
 
@@ -150,140 +136,206 @@ static void pblk_put_line_back(struct pblk *pblk, struct pblk_line *line)
 
 static void pblk_gc_line_ws(struct work_struct *work)
 {
+	struct pblk_line_ws *line_rq_ws = container_of(work,
+						struct pblk_line_ws, ws);
+	struct pblk *pblk = line_rq_ws->pblk;
+	struct pblk_gc *gc = &pblk->gc;
+	struct pblk_line *line = line_rq_ws->line;
+	struct pblk_gc_rq *gc_rq = line_rq_ws->priv;
+
+	up(&gc->gc_sem);
+
+	if (pblk_gc_move_valid_secs(pblk, gc_rq)) {
+		pr_err("pblk: could not GC all sectors: line:%d (%d/%d)\n",
+						line->id, *line->vsc,
+						gc_rq->nr_secs);
+	}
+
+	mempool_free(line_rq_ws, pblk->line_ws_pool);
+}
+
+static void pblk_gc_line_prepare_ws(struct work_struct *work)
+{
 	struct pblk_line_ws *line_ws = container_of(work, struct pblk_line_ws,
 									ws);
 	struct pblk *pblk = line_ws->pblk;
-	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
 	struct pblk_line *line = line_ws->line;
+	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
 	struct pblk_line_meta *lm = &pblk->lm;
-	__le64 *lba_list = line_ws->priv;
-	u64 *gc_list;
-	int sec_left;
-	int nr_ppas, bit;
-	int put_line = 1;
+	struct pblk_gc *gc = &pblk->gc;
+	struct line_emeta *emeta_buf;
+	struct pblk_line_ws *line_rq_ws;
+	struct pblk_gc_rq *gc_rq;
+	__le64 *lba_list;
+	int sec_left, nr_secs, bit;
+	int ret;
 
-	pr_debug("pblk: line '%d' being reclaimed for GC\n", line->id);
+	emeta_buf = pblk_malloc(lm->emeta_len[0], l_mg->emeta_alloc_type,
+								GFP_KERNEL);
+	if (!emeta_buf) {
+		pr_err("pblk: cannot use GC emeta\n");
+		return;
+	}
 
-	spin_lock(&line->lock);
-	sec_left = line->vsc;
-	if (!sec_left) {
-		/* Lines are erased before being used (l_mg->data_/log_next) */
-		spin_unlock(&line->lock);
-		goto out;
+	ret = pblk_line_read_emeta(pblk, line, emeta_buf);
+	if (ret) {
+		pr_err("pblk: line %d read emeta failed (%d)\n", line->id, ret);
+		goto fail_free_emeta;
+	}
+
+	/* If this read fails, it means that emeta is corrupted. For now, leave
+	 * the line untouched. TODO: Implement a recovery routine that scans and
+	 * moves all sectors on the line.
+	 */
+	lba_list = pblk_recov_get_lba_list(pblk, emeta_buf);
+	if (!lba_list) {
+		pr_err("pblk: could not interpret emeta (line %d)\n", line->id);
+		goto fail_free_emeta;
 	}
-	spin_unlock(&line->lock);
 
+	sec_left = pblk_line_vsc(line);
 	if (sec_left < 0) {
 		pr_err("pblk: corrupted GC line (%d)\n", line->id);
-		put_line = 0;
-		pblk_put_line_back(pblk, line);
-		goto out;
+		goto fail_free_emeta;
 	}
 
 	bit = -1;
 next_rq:
-	gc_list = kmalloc_array(pblk->max_write_pgs, sizeof(u64), GFP_KERNEL);
-	if (!gc_list) {
-		put_line = 0;
-		pblk_put_line_back(pblk, line);
-		goto out;
-	}
+	gc_rq = kmalloc(sizeof(struct pblk_gc_rq), GFP_KERNEL);
+	if (!gc_rq)
+		goto fail_free_emeta;
 
-	nr_ppas = 0;
+	nr_secs = 0;
 	do {
 		bit = find_next_zero_bit(line->invalid_bitmap, lm->sec_per_line,
 								bit + 1);
 		if (bit > line->emeta_ssec)
 			break;
 
-		gc_list[nr_ppas++] = le64_to_cpu(lba_list[bit]);
-	} while (nr_ppas < pblk->max_write_pgs);
+		gc_rq->lba_list[nr_secs++] = le64_to_cpu(lba_list[bit]);
+	} while (nr_secs < pblk->max_write_pgs);
 
-	if (unlikely(!nr_ppas)) {
-		kfree(gc_list);
+	if (unlikely(!nr_secs)) {
+		kfree(gc_rq);
 		goto out;
 	}
 
-	if (pblk_gc_move_valid_secs(pblk, line, gc_list, nr_ppas)) {
-		pr_err("pblk: could not GC all sectors: line:%d (%d/%d/%d)\n",
-						line->id, line->vsc,
-						nr_ppas, nr_ppas);
-		put_line = 0;
-		pblk_put_line_back(pblk, line);
-		goto out;
-	}
+	gc_rq->nr_secs = nr_secs;
+	gc_rq->line = line;
+
+	line_rq_ws = mempool_alloc(pblk->line_ws_pool, GFP_KERNEL);
+	if (!line_rq_ws)
+		goto fail_free_gc_rq;
 
-	sec_left -= nr_ppas;
+	line_rq_ws->pblk = pblk;
+	line_rq_ws->line = line;
+	line_rq_ws->priv = gc_rq;
+
+	down(&gc->gc_sem);
+	kref_get(&line->ref);
+
+	INIT_WORK(&line_rq_ws->ws, pblk_gc_line_ws);
+	queue_work(gc->gc_line_reader_wq, &line_rq_ws->ws);
+
+	sec_left -= nr_secs;
 	if (sec_left > 0)
 		goto next_rq;
 
 out:
-	pblk_mfree(line->emeta, l_mg->emeta_alloc_type);
+	pblk_mfree(emeta_buf, l_mg->emeta_alloc_type);
 	mempool_free(line_ws, pblk->line_ws_pool);
-	atomic_dec(&pblk->gc.inflight_gc);
-	if (put_line)
-		kref_put(&line->ref, pblk_line_put);
+
+	kref_put(&line->ref, pblk_line_put);
+	atomic_dec(&gc->inflight_gc);
+
+	return;
+
+fail_free_gc_rq:
+	kfree(gc_rq);
+fail_free_emeta:
+	pblk_mfree(emeta_buf, l_mg->emeta_alloc_type);
+	pblk_put_line_back(pblk, line);
+	kref_put(&line->ref, pblk_line_put);
+	mempool_free(line_ws, pblk->line_ws_pool);
+	atomic_dec(&gc->inflight_gc);
+
+	pr_err("pblk: Failed to GC line %d\n", line->id);
 }
 
 static int pblk_gc_line(struct pblk *pblk, struct pblk_line *line)
 {
-	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
-	struct pblk_line_meta *lm = &pblk->lm;
+	struct pblk_gc *gc = &pblk->gc;
 	struct pblk_line_ws *line_ws;
-	__le64 *lba_list;
-	int ret;
 
-	line_ws = mempool_alloc(pblk->line_ws_pool, GFP_KERNEL);
-	line->emeta = pblk_malloc(lm->emeta_len, l_mg->emeta_alloc_type,
-								GFP_KERNEL);
-	if (!line->emeta) {
-		pr_err("pblk: cannot use GC emeta\n");
-		goto fail_free_ws;
-	}
-
-	ret = pblk_line_read_emeta(pblk, line);
-	if (ret) {
-		pr_err("pblk: line %d read emeta failed (%d)\n", line->id, ret);
-		goto fail_free_emeta;
-	}
+	pr_debug("pblk: line '%d' being reclaimed for GC\n", line->id);
 
-	/* If this read fails, it means that emeta is corrupted. For now, leave
-	 * the line untouched. TODO: Implement a recovery routine that scans and
-	 * moves all sectors on the line.
-	 */
-	lba_list = pblk_recov_get_lba_list(pblk, line->emeta);
-	if (!lba_list) {
-		pr_err("pblk: could not interpret emeta (line %d)\n", line->id);
-		goto fail_free_emeta;
-	}
+	line_ws = mempool_alloc(pblk->line_ws_pool, GFP_KERNEL);
+	if (!line_ws)
+		return -ENOMEM;
 
 	line_ws->pblk = pblk;
 	line_ws->line = line;
-	line_ws->priv = lba_list;
 
-	INIT_WORK(&line_ws->ws, pblk_gc_line_ws);
-	queue_work(pblk->gc.gc_reader_wq, &line_ws->ws);
+	INIT_WORK(&line_ws->ws, pblk_gc_line_prepare_ws);
+	queue_work(gc->gc_reader_wq, &line_ws->ws);
 
 	return 0;
+}
 
-fail_free_emeta:
-	pblk_mfree(line->emeta, l_mg->emeta_alloc_type);
-fail_free_ws:
-	mempool_free(line_ws, pblk->line_ws_pool);
-	pblk_put_line_back(pblk, line);
+static int pblk_gc_read(struct pblk *pblk)
+{
+	struct pblk_gc *gc = &pblk->gc;
+	struct pblk_line *line;
+
+	spin_lock(&gc->r_lock);
+	if (list_empty(&gc->r_list)) {
+		spin_unlock(&gc->r_lock);
+		return 1;
+	}
+
+	line = list_first_entry(&gc->r_list, struct pblk_line, list);
+	list_del(&line->list);
+	spin_unlock(&gc->r_lock);
+
+	pblk_gc_kick(pblk);
 
-	return 1;
+	if (pblk_gc_line(pblk, line))
+		pr_err("pblk: failed to GC line %d\n", line->id);
+
+	return 0;
 }
 
-static void pblk_gc_lines(struct pblk *pblk, struct list_head *gc_list)
+static void pblk_gc_reader_kick(struct pblk_gc *gc)
 {
-	struct pblk_line *line, *tline;
+	wake_up_process(gc->gc_reader_ts);
+}
 
-	list_for_each_entry_safe(line, tline, gc_list, list) {
-		if (pblk_gc_line(pblk, line))
-			pr_err("pblk: failed to GC line %d\n", line->id);
-		list_del(&line->list);
+static struct pblk_line *pblk_gc_get_victim_line(struct pblk *pblk,
+						 struct list_head *group_list)
+{
+	struct pblk_line *line, *victim;
+	int line_vsc, victim_vsc;
+
+	victim = list_first_entry(group_list, struct pblk_line, list);
+	list_for_each_entry(line, group_list, list) {
+		line_vsc = le32_to_cpu(*line->vsc);
+		victim_vsc = le32_to_cpu(*victim->vsc);
+		if (line_vsc < victim_vsc)
+			victim = line;
 	}
+
+	return victim;
+}
+
+static bool pblk_gc_should_run(struct pblk_gc *gc, struct pblk_rl *rl)
+{
+	unsigned int nr_blocks_free, nr_blocks_need;
+
+	nr_blocks_need = pblk_rl_high_thrs(rl);
+	nr_blocks_free = pblk_rl_nr_free_blks(rl);
+
+	/* This is not critical, no need to take lock here */
+	return ((gc->gc_active) && (nr_blocks_need > nr_blocks_free));
 }
 
 /*
@@ -296,71 +348,83 @@ static void pblk_gc_run(struct pblk *pblk)
 {
 	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
 	struct pblk_gc *gc = &pblk->gc;
-	struct pblk_line *line, *tline;
-	unsigned int nr_blocks_free, nr_blocks_need;
+	struct pblk_line *line;
 	struct list_head *group_list;
-	int run_gc, gc_group = 0;
-	int prev_gc = 0;
-	int inflight_gc = atomic_read(&gc->inflight_gc);
-	LIST_HEAD(gc_list);
+	bool run_gc;
+	int inflight_gc, gc_group = 0, prev_group = 0;
+
+	do {
+		spin_lock(&l_mg->gc_lock);
+		if (list_empty(&l_mg->gc_full_list)) {
+			spin_unlock(&l_mg->gc_lock);
+			break;
+		}
+
+		line = list_first_entry(&l_mg->gc_full_list,
+							struct pblk_line, list);
 
-	spin_lock(&l_mg->gc_lock);
-	list_for_each_entry_safe(line, tline, &l_mg->gc_full_list, list) {
 		spin_lock(&line->lock);
 		WARN_ON(line->state != PBLK_LINESTATE_CLOSED);
 		line->state = PBLK_LINESTATE_GC;
 		spin_unlock(&line->lock);
 
 		list_del(&line->list);
+		spin_unlock(&l_mg->gc_lock);
+
 		kref_put(&line->ref, pblk_line_put);
-	}
-	spin_unlock(&l_mg->gc_lock);
+	} while (1);
 
-	nr_blocks_need = pblk_rl_gc_thrs(&pblk->rl);
-	nr_blocks_free = pblk_rl_nr_free_blks(&pblk->rl);
-	run_gc = (nr_blocks_need > nr_blocks_free || gc->gc_forced);
+	run_gc = pblk_gc_should_run(&pblk->gc, &pblk->rl);
+	if (!run_gc || (atomic_read(&gc->inflight_gc) >= PBLK_GC_L_QD))
+		return;
 
 next_gc_group:
 	group_list = l_mg->gc_lists[gc_group++];
-	spin_lock(&l_mg->gc_lock);
-	while (run_gc && !list_empty(group_list)) {
-		/* No need to queue up more GC lines than we can handle */
-		if (!run_gc || inflight_gc > gc->gc_jobs_active) {
+
+	do {
+		spin_lock(&l_mg->gc_lock);
+		if (list_empty(group_list)) {
 			spin_unlock(&l_mg->gc_lock);
-			pblk_gc_lines(pblk, &gc_list);
-			return;
+			break;
 		}
 
-		line = list_first_entry(group_list, struct pblk_line, list);
-		nr_blocks_free += atomic_read(&line->blk_in_line);
+		line = pblk_gc_get_victim_line(pblk, group_list);
 
 		spin_lock(&line->lock);
 		WARN_ON(line->state != PBLK_LINESTATE_CLOSED);
 		line->state = PBLK_LINESTATE_GC;
-		list_move_tail(&line->list, &gc_list);
-		atomic_inc(&gc->inflight_gc);
-		inflight_gc++;
 		spin_unlock(&line->lock);
 
-		prev_gc = 1;
-		run_gc = (nr_blocks_need > nr_blocks_free || gc->gc_forced);
-	}
-	spin_unlock(&l_mg->gc_lock);
+		list_del(&line->list);
+		spin_unlock(&l_mg->gc_lock);
+
+		spin_lock(&gc->r_lock);
+		list_add_tail(&line->list, &gc->r_list);
+		spin_unlock(&gc->r_lock);
 
-	pblk_gc_lines(pblk, &gc_list);
+		inflight_gc = atomic_inc_return(&gc->inflight_gc);
+		pblk_gc_reader_kick(gc);
 
-	if (!prev_gc && pblk->rl.rb_state > gc_group &&
-						gc_group < PBLK_NR_GC_LISTS)
+		prev_group = 1;
+
+		/* No need to queue up more GC lines than we can handle */
+		run_gc = pblk_gc_should_run(&pblk->gc, &pblk->rl);
+		if (!run_gc || inflight_gc >= PBLK_GC_L_QD)
+			break;
+	} while (1);
+
+	if (!prev_group && pblk->rl.rb_state > gc_group &&
+						gc_group < PBLK_GC_NR_LISTS)
 		goto next_gc_group;
 }
 
-
-static void pblk_gc_kick(struct pblk *pblk)
+void pblk_gc_kick(struct pblk *pblk)
 {
 	struct pblk_gc *gc = &pblk->gc;
 
 	wake_up_process(gc->gc_ts);
 	pblk_gc_writer_kick(gc);
+	pblk_gc_reader_kick(gc);
 	mod_timer(&gc->gc_timer, jiffies + msecs_to_jiffies(GC_TIME_MSECS));
 }
 
@@ -398,42 +462,34 @@ static int pblk_gc_writer_ts(void *data)
 	return 0;
 }
 
-static void pblk_gc_start(struct pblk *pblk)
+static int pblk_gc_reader_ts(void *data)
 {
-	pblk->gc.gc_active = 1;
+	struct pblk *pblk = data;
 
-	pr_debug("pblk: gc start\n");
+	while (!kthread_should_stop()) {
+		if (!pblk_gc_read(pblk))
+			continue;
+		set_current_state(TASK_INTERRUPTIBLE);
+		io_schedule();
+	}
+
+	return 0;
 }
 
-int pblk_gc_status(struct pblk *pblk)
+static void pblk_gc_start(struct pblk *pblk)
 {
-	struct pblk_gc *gc = &pblk->gc;
-	int ret;
-
-	spin_lock(&gc->lock);
-	ret = gc->gc_active;
-	spin_unlock(&gc->lock);
-
-	return ret;
+	pblk->gc.gc_active = 1;
+	pr_debug("pblk: gc start\n");
 }
 
-static void __pblk_gc_should_start(struct pblk *pblk)
+void pblk_gc_should_start(struct pblk *pblk)
 {
 	struct pblk_gc *gc = &pblk->gc;
 
-	lockdep_assert_held(&gc->lock);
-
 	if (gc->gc_enabled && !gc->gc_active)
 		pblk_gc_start(pblk);
-}
 
-void pblk_gc_should_start(struct pblk *pblk)
-{
-	struct pblk_gc *gc = &pblk->gc;
-
-	spin_lock(&gc->lock);
-	__pblk_gc_should_start(pblk);
-	spin_unlock(&gc->lock);
+	pblk_gc_kick(pblk);
 }
 
 /*
@@ -442,10 +498,7 @@ void pblk_gc_should_start(struct pblk *pblk)
  */
 static void pblk_gc_stop(struct pblk *pblk, int flush_wq)
 {
-	spin_lock(&pblk->gc.lock);
 	pblk->gc.gc_active = 0;
-	spin_unlock(&pblk->gc.lock);
-
 	pr_debug("pblk: gc stop\n");
 }
 
@@ -468,20 +521,25 @@ void pblk_gc_sysfs_state_show(struct pblk *pblk, int *gc_enabled,
 	spin_unlock(&gc->lock);
 }
 
-void pblk_gc_sysfs_force(struct pblk *pblk, int force)
+int pblk_gc_sysfs_force(struct pblk *pblk, int force)
 {
 	struct pblk_gc *gc = &pblk->gc;
-	int rsv = 0;
+
+	if (force < 0 || force > 1)
+		return -EINVAL;
 
 	spin_lock(&gc->lock);
-	if (force) {
-		gc->gc_enabled = 1;
-		rsv = 64;
-	}
-	pblk_rl_set_gc_rsc(&pblk->rl, rsv);
 	gc->gc_forced = force;
-	__pblk_gc_should_start(pblk);
+
+	if (force)
+		gc->gc_enabled = 1;
+	else
+		gc->gc_enabled = 0;
 	spin_unlock(&gc->lock);
+
+	pblk_gc_should_start(pblk);
+
+	return 0;
 }
 
 int pblk_gc_init(struct pblk *pblk)
@@ -503,30 +561,58 @@ int pblk_gc_init(struct pblk *pblk)
 		goto fail_free_main_kthread;
 	}
 
+	gc->gc_reader_ts = kthread_create(pblk_gc_reader_ts, pblk,
+							"pblk-gc-reader-ts");
+	if (IS_ERR(gc->gc_reader_ts)) {
+		pr_err("pblk: could not allocate GC reader kthread\n");
+		ret = PTR_ERR(gc->gc_reader_ts);
+		goto fail_free_writer_kthread;
+	}
+
 	setup_timer(&gc->gc_timer, pblk_gc_timer, (unsigned long)pblk);
 	mod_timer(&gc->gc_timer, jiffies + msecs_to_jiffies(GC_TIME_MSECS));
 
 	gc->gc_active = 0;
 	gc->gc_forced = 0;
 	gc->gc_enabled = 1;
-	gc->gc_jobs_active = 8;
 	gc->w_entries = 0;
 	atomic_set(&gc->inflight_gc, 0);
 
-	gc->gc_reader_wq = alloc_workqueue("pblk-gc-reader-wq",
-			WQ_MEM_RECLAIM | WQ_UNBOUND, gc->gc_jobs_active);
+	/* Workqueue that reads valid sectors from a line and submit them to the
+	 * GC writer to be recycled.
+	 */
+	gc->gc_line_reader_wq = alloc_workqueue("pblk-gc-line-reader-wq",
+			WQ_MEM_RECLAIM | WQ_UNBOUND, PBLK_GC_MAX_READERS);
+	if (!gc->gc_line_reader_wq) {
+		pr_err("pblk: could not allocate GC line reader workqueue\n");
+		ret = -ENOMEM;
+		goto fail_free_reader_kthread;
+	}
+
+	/* Workqueue that prepare lines for GC */
+	gc->gc_reader_wq = alloc_workqueue("pblk-gc-line_wq",
+					WQ_MEM_RECLAIM | WQ_UNBOUND, 1);
 	if (!gc->gc_reader_wq) {
 		pr_err("pblk: could not allocate GC reader workqueue\n");
 		ret = -ENOMEM;
-		goto fail_free_writer_kthread;
+		goto fail_free_reader_line_wq;
 	}
 
 	spin_lock_init(&gc->lock);
 	spin_lock_init(&gc->w_lock);
+	spin_lock_init(&gc->r_lock);
+
+	sema_init(&gc->gc_sem, 128);
+
 	INIT_LIST_HEAD(&gc->w_list);
+	INIT_LIST_HEAD(&gc->r_list);
 
 	return 0;
 
+fail_free_reader_line_wq:
+	destroy_workqueue(gc->gc_line_reader_wq);
+fail_free_reader_kthread:
+	kthread_stop(gc->gc_reader_ts);
 fail_free_writer_kthread:
 	kthread_stop(gc->gc_writer_ts);
 fail_free_main_kthread:
@@ -540,6 +626,7 @@ void pblk_gc_exit(struct pblk *pblk)
 	struct pblk_gc *gc = &pblk->gc;
 
 	flush_workqueue(gc->gc_reader_wq);
+	flush_workqueue(gc->gc_line_reader_wq);
 
 	del_timer(&gc->gc_timer);
 	pblk_gc_stop(pblk, 1);
@@ -547,9 +634,15 @@ void pblk_gc_exit(struct pblk *pblk)
 	if (gc->gc_ts)
 		kthread_stop(gc->gc_ts);
 
-	if (pblk->gc.gc_reader_wq)
-		destroy_workqueue(pblk->gc.gc_reader_wq);
+	if (gc->gc_reader_wq)
+		destroy_workqueue(gc->gc_reader_wq);
+
+	if (gc->gc_line_reader_wq)
+		destroy_workqueue(gc->gc_line_reader_wq);
 
 	if (gc->gc_writer_ts)
 		kthread_stop(gc->gc_writer_ts);
+
+	if (gc->gc_reader_ts)
+		kthread_stop(gc->gc_reader_ts);
 }
diff --git a/drivers/lightnvm/pblk-init.c b/drivers/lightnvm/pblk-init.c
index ae8cd6d5af8b..1b0f61233c21 100644
--- a/drivers/lightnvm/pblk-init.c
+++ b/drivers/lightnvm/pblk-init.c
@@ -20,9 +20,10 @@
 
 #include "pblk.h"
 
-static struct kmem_cache *pblk_blk_ws_cache, *pblk_rec_cache, *pblk_r_rq_cache,
-					*pblk_w_rq_cache, *pblk_line_meta_cache;
+static struct kmem_cache *pblk_blk_ws_cache, *pblk_rec_cache, *pblk_g_rq_cache,
+				*pblk_w_rq_cache, *pblk_line_meta_cache;
 static DECLARE_RWSEM(pblk_lock);
+struct bio_set *pblk_bio_set;
 
 static int pblk_rw_io(struct request_queue *q, struct pblk *pblk,
 			  struct bio *bio)
@@ -33,7 +34,7 @@ static int pblk_rw_io(struct request_queue *q, struct pblk *pblk,
 	 * constraint. Writes can be of arbitrary size.
 	 */
 	if (bio_data_dir(bio) == READ) {
-		blk_queue_split(q, &bio, q->bio_split);
+		blk_queue_split(q, &bio);
 		ret = pblk_submit_read(pblk, bio);
 		if (ret == NVM_IO_DONE && bio_flagged(bio, BIO_CLONED))
 			bio_put(bio);
@@ -46,7 +47,7 @@ static int pblk_rw_io(struct request_queue *q, struct pblk *pblk,
 	 * available for user I/O.
 	 */
 	if (unlikely(pblk_get_secs(bio) >= pblk_rl_sysfs_rate_show(&pblk->rl)))
-		blk_queue_split(q, &bio, q->bio_split);
+		blk_queue_split(q, &bio);
 
 	return pblk_write_to_cache(pblk, bio, PBLK_IOTYPE_USER);
 }
@@ -199,9 +200,9 @@ static int pblk_init_global_caches(struct pblk *pblk)
 		return -ENOMEM;
 	}
 
-	pblk_r_rq_cache = kmem_cache_create("pblk_r_rq", pblk_r_rq_size,
+	pblk_g_rq_cache = kmem_cache_create("pblk_g_rq", pblk_g_rq_size,
 				0, 0, NULL);
-	if (!pblk_r_rq_cache) {
+	if (!pblk_g_rq_cache) {
 		kmem_cache_destroy(pblk_blk_ws_cache);
 		kmem_cache_destroy(pblk_rec_cache);
 		up_write(&pblk_lock);
@@ -213,7 +214,7 @@ static int pblk_init_global_caches(struct pblk *pblk)
 	if (!pblk_w_rq_cache) {
 		kmem_cache_destroy(pblk_blk_ws_cache);
 		kmem_cache_destroy(pblk_rec_cache);
-		kmem_cache_destroy(pblk_r_rq_cache);
+		kmem_cache_destroy(pblk_g_rq_cache);
 		up_write(&pblk_lock);
 		return -ENOMEM;
 	}
@@ -225,7 +226,7 @@ static int pblk_init_global_caches(struct pblk *pblk)
 	if (!pblk_line_meta_cache) {
 		kmem_cache_destroy(pblk_blk_ws_cache);
 		kmem_cache_destroy(pblk_rec_cache);
-		kmem_cache_destroy(pblk_r_rq_cache);
+		kmem_cache_destroy(pblk_g_rq_cache);
 		kmem_cache_destroy(pblk_w_rq_cache);
 		up_write(&pblk_lock);
 		return -ENOMEM;
@@ -239,27 +240,10 @@ static int pblk_core_init(struct pblk *pblk)
 {
 	struct nvm_tgt_dev *dev = pblk->dev;
 	struct nvm_geo *geo = &dev->geo;
-	int max_write_ppas;
-	int mod;
 
-	pblk->min_write_pgs = geo->sec_per_pl * (geo->sec_size / PAGE_SIZE);
-	max_write_ppas = pblk->min_write_pgs * geo->nr_luns;
-	pblk->max_write_pgs = (max_write_ppas < nvm_max_phys_sects(dev)) ?
-				max_write_ppas : nvm_max_phys_sects(dev);
 	pblk->pgs_in_buffer = NVM_MEM_PAGE_WRITE * geo->sec_per_pg *
 						geo->nr_planes * geo->nr_luns;
 
-	if (pblk->max_write_pgs > PBLK_MAX_REQ_ADDRS) {
-		pr_err("pblk: cannot support device max_phys_sect\n");
-		return -EINVAL;
-	}
-
-	div_u64_rem(geo->sec_per_blk, pblk->min_write_pgs, &mod);
-	if (mod) {
-		pr_err("pblk: bad configuration of sectors/pages\n");
-		return -EINVAL;
-	}
-
 	if (pblk_init_global_caches(pblk))
 		return -ENOMEM;
 
@@ -267,7 +251,7 @@ static int pblk_core_init(struct pblk *pblk)
 	if (!pblk->page_pool)
 		return -ENOMEM;
 
-	pblk->line_ws_pool = mempool_create_slab_pool(geo->nr_luns,
+	pblk->line_ws_pool = mempool_create_slab_pool(PBLK_WS_POOL_SIZE,
 							pblk_blk_ws_cache);
 	if (!pblk->line_ws_pool)
 		goto free_page_pool;
@@ -276,41 +260,51 @@ static int pblk_core_init(struct pblk *pblk)
 	if (!pblk->rec_pool)
 		goto free_blk_ws_pool;
 
-	pblk->r_rq_pool = mempool_create_slab_pool(64, pblk_r_rq_cache);
-	if (!pblk->r_rq_pool)
+	pblk->g_rq_pool = mempool_create_slab_pool(PBLK_READ_REQ_POOL_SIZE,
+							pblk_g_rq_cache);
+	if (!pblk->g_rq_pool)
 		goto free_rec_pool;
 
-	pblk->w_rq_pool = mempool_create_slab_pool(64, pblk_w_rq_cache);
+	pblk->w_rq_pool = mempool_create_slab_pool(geo->nr_luns * 2,
+							pblk_w_rq_cache);
 	if (!pblk->w_rq_pool)
-		goto free_r_rq_pool;
+		goto free_g_rq_pool;
 
 	pblk->line_meta_pool =
-			mempool_create_slab_pool(16, pblk_line_meta_cache);
+			mempool_create_slab_pool(PBLK_META_POOL_SIZE,
+							pblk_line_meta_cache);
 	if (!pblk->line_meta_pool)
 		goto free_w_rq_pool;
 
-	pblk->kw_wq = alloc_workqueue("pblk-aux-wq",
-					WQ_MEM_RECLAIM | WQ_UNBOUND, 1);
-	if (!pblk->kw_wq)
+	pblk->close_wq = alloc_workqueue("pblk-close-wq",
+			WQ_MEM_RECLAIM | WQ_UNBOUND, PBLK_NR_CLOSE_JOBS);
+	if (!pblk->close_wq)
 		goto free_line_meta_pool;
 
+	pblk->bb_wq = alloc_workqueue("pblk-bb-wq",
+			WQ_MEM_RECLAIM | WQ_UNBOUND, 0);
+	if (!pblk->bb_wq)
+		goto free_close_wq;
+
 	if (pblk_set_ppaf(pblk))
-		goto free_kw_wq;
+		goto free_bb_wq;
 
 	if (pblk_rwb_init(pblk))
-		goto free_kw_wq;
+		goto free_bb_wq;
 
 	INIT_LIST_HEAD(&pblk->compl_list);
 	return 0;
 
-free_kw_wq:
-	destroy_workqueue(pblk->kw_wq);
+free_bb_wq:
+	destroy_workqueue(pblk->bb_wq);
+free_close_wq:
+	destroy_workqueue(pblk->close_wq);
 free_line_meta_pool:
 	mempool_destroy(pblk->line_meta_pool);
 free_w_rq_pool:
 	mempool_destroy(pblk->w_rq_pool);
-free_r_rq_pool:
-	mempool_destroy(pblk->r_rq_pool);
+free_g_rq_pool:
+	mempool_destroy(pblk->g_rq_pool);
 free_rec_pool:
 	mempool_destroy(pblk->rec_pool);
 free_blk_ws_pool:
@@ -322,19 +316,22 @@ free_page_pool:
 
 static void pblk_core_free(struct pblk *pblk)
 {
-	if (pblk->kw_wq)
-		destroy_workqueue(pblk->kw_wq);
+	if (pblk->close_wq)
+		destroy_workqueue(pblk->close_wq);
+
+	if (pblk->bb_wq)
+		destroy_workqueue(pblk->bb_wq);
 
 	mempool_destroy(pblk->page_pool);
 	mempool_destroy(pblk->line_ws_pool);
 	mempool_destroy(pblk->rec_pool);
-	mempool_destroy(pblk->r_rq_pool);
+	mempool_destroy(pblk->g_rq_pool);
 	mempool_destroy(pblk->w_rq_pool);
 	mempool_destroy(pblk->line_meta_pool);
 
 	kmem_cache_destroy(pblk_blk_ws_cache);
 	kmem_cache_destroy(pblk_rec_cache);
-	kmem_cache_destroy(pblk_r_rq_cache);
+	kmem_cache_destroy(pblk_g_rq_cache);
 	kmem_cache_destroy(pblk_w_rq_cache);
 	kmem_cache_destroy(pblk_line_meta_cache);
 }
@@ -344,6 +341,12 @@ static void pblk_luns_free(struct pblk *pblk)
 	kfree(pblk->luns);
 }
 
+static void pblk_free_line_bitmaps(struct pblk_line *line)
+{
+	kfree(line->blk_bitmap);
+	kfree(line->erase_bitmap);
+}
+
 static void pblk_lines_free(struct pblk *pblk)
 {
 	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
@@ -355,8 +358,7 @@ static void pblk_lines_free(struct pblk *pblk)
 		line = &pblk->lines[i];
 
 		pblk_line_free(pblk, line);
-		kfree(line->blk_bitmap);
-		kfree(line->erase_bitmap);
+		pblk_free_line_bitmaps(line);
 	}
 	spin_unlock(&l_mg->free_lock);
 }
@@ -368,11 +370,15 @@ static void pblk_line_meta_free(struct pblk *pblk)
 
 	kfree(l_mg->bb_template);
 	kfree(l_mg->bb_aux);
+	kfree(l_mg->vsc_list);
 
+	spin_lock(&l_mg->free_lock);
 	for (i = 0; i < PBLK_DATA_LINES; i++) {
-		pblk_mfree(l_mg->sline_meta[i].meta, l_mg->smeta_alloc_type);
-		pblk_mfree(l_mg->eline_meta[i].meta, l_mg->emeta_alloc_type);
+		kfree(l_mg->sline_meta[i]);
+		pblk_mfree(l_mg->eline_meta[i]->buf, l_mg->emeta_alloc_type);
+		kfree(l_mg->eline_meta[i]);
 	}
+	spin_unlock(&l_mg->free_lock);
 
 	kfree(pblk->lines);
 }
@@ -411,13 +417,31 @@ out:
 	return ret;
 }
 
-static int pblk_bb_line(struct pblk *pblk, struct pblk_line *line)
+static int pblk_bb_line(struct pblk *pblk, struct pblk_line *line,
+			int blk_per_line)
 {
-	struct pblk_line_meta *lm = &pblk->lm;
+	struct nvm_tgt_dev *dev = pblk->dev;
+	struct nvm_geo *geo = &dev->geo;
 	struct pblk_lun *rlun;
 	int bb_cnt = 0;
 	int i;
 
+	for (i = 0; i < blk_per_line; i++) {
+		rlun = &pblk->luns[i];
+		if (rlun->bb_list[line->id] == NVM_BLK_T_FREE)
+			continue;
+
+		set_bit(pblk_ppa_to_pos(geo, rlun->bppa), line->blk_bitmap);
+		bb_cnt++;
+	}
+
+	return bb_cnt;
+}
+
+static int pblk_alloc_line_bitmaps(struct pblk *pblk, struct pblk_line *line)
+{
+	struct pblk_line_meta *lm = &pblk->lm;
+
 	line->blk_bitmap = kzalloc(lm->blk_bitmap_len, GFP_KERNEL);
 	if (!line->blk_bitmap)
 		return -ENOMEM;
@@ -428,16 +452,7 @@ static int pblk_bb_line(struct pblk *pblk, struct pblk_line *line)
 		return -ENOMEM;
 	}
 
-	for (i = 0; i < lm->blk_per_line; i++) {
-		rlun = &pblk->luns[i];
-		if (rlun->bb_list[line->id] == NVM_BLK_T_FREE)
-			continue;
-
-		set_bit(i, line->blk_bitmap);
-		bb_cnt++;
-	}
-
-	return bb_cnt;
+	return 0;
 }
 
 static int pblk_luns_init(struct pblk *pblk, struct ppa_addr *luns)
@@ -505,12 +520,32 @@ static int pblk_lines_configure(struct pblk *pblk, int flags)
 }
 
 /* See comment over struct line_emeta definition */
-static unsigned int calc_emeta_len(struct pblk *pblk, struct pblk_line_meta *lm)
+static unsigned int calc_emeta_len(struct pblk *pblk)
 {
-	return (sizeof(struct line_emeta) +
-			((lm->sec_per_line - lm->emeta_sec) * sizeof(u64)) +
-			(pblk->l_mg.nr_lines * sizeof(u32)) +
-			lm->blk_bitmap_len);
+	struct pblk_line_meta *lm = &pblk->lm;
+	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
+	struct nvm_tgt_dev *dev = pblk->dev;
+	struct nvm_geo *geo = &dev->geo;
+
+	/* Round to sector size so that lba_list starts on its own sector */
+	lm->emeta_sec[1] = DIV_ROUND_UP(
+			sizeof(struct line_emeta) + lm->blk_bitmap_len,
+			geo->sec_size);
+	lm->emeta_len[1] = lm->emeta_sec[1] * geo->sec_size;
+
+	/* Round to sector size so that vsc_list starts on its own sector */
+	lm->dsec_per_line = lm->sec_per_line - lm->emeta_sec[0];
+	lm->emeta_sec[2] = DIV_ROUND_UP(lm->dsec_per_line * sizeof(u64),
+			geo->sec_size);
+	lm->emeta_len[2] = lm->emeta_sec[2] * geo->sec_size;
+
+	lm->emeta_sec[3] = DIV_ROUND_UP(l_mg->nr_lines * sizeof(u32),
+			geo->sec_size);
+	lm->emeta_len[3] = lm->emeta_sec[3] * geo->sec_size;
+
+	lm->vsc_list_len = l_mg->nr_lines * sizeof(u32);
+
+	return (lm->emeta_len[1] + lm->emeta_len[2] + lm->emeta_len[3]);
 }
 
 static void pblk_set_provision(struct pblk *pblk, long nr_free_blks)
@@ -534,6 +569,78 @@ static void pblk_set_provision(struct pblk *pblk, long nr_free_blks)
 	atomic_set(&pblk->rl.free_blocks, nr_free_blks);
 }
 
+static int pblk_lines_alloc_metadata(struct pblk *pblk)
+{
+	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
+	struct pblk_line_meta *lm = &pblk->lm;
+	int i;
+
+	/* smeta is always small enough to fit on a kmalloc memory allocation,
+	 * emeta depends on the number of LUNs allocated to the pblk instance
+	 */
+	for (i = 0; i < PBLK_DATA_LINES; i++) {
+		l_mg->sline_meta[i] = kmalloc(lm->smeta_len, GFP_KERNEL);
+		if (!l_mg->sline_meta[i])
+			goto fail_free_smeta;
+	}
+
+	/* emeta allocates three different buffers for managing metadata with
+	 * in-memory and in-media layouts
+	 */
+	for (i = 0; i < PBLK_DATA_LINES; i++) {
+		struct pblk_emeta *emeta;
+
+		emeta = kmalloc(sizeof(struct pblk_emeta), GFP_KERNEL);
+		if (!emeta)
+			goto fail_free_emeta;
+
+		if (lm->emeta_len[0] > KMALLOC_MAX_CACHE_SIZE) {
+			l_mg->emeta_alloc_type = PBLK_VMALLOC_META;
+
+			emeta->buf = vmalloc(lm->emeta_len[0]);
+			if (!emeta->buf) {
+				kfree(emeta);
+				goto fail_free_emeta;
+			}
+
+			emeta->nr_entries = lm->emeta_sec[0];
+			l_mg->eline_meta[i] = emeta;
+		} else {
+			l_mg->emeta_alloc_type = PBLK_KMALLOC_META;
+
+			emeta->buf = kmalloc(lm->emeta_len[0], GFP_KERNEL);
+			if (!emeta->buf) {
+				kfree(emeta);
+				goto fail_free_emeta;
+			}
+
+			emeta->nr_entries = lm->emeta_sec[0];
+			l_mg->eline_meta[i] = emeta;
+		}
+	}
+
+	l_mg->vsc_list = kcalloc(l_mg->nr_lines, sizeof(__le32), GFP_KERNEL);
+	if (!l_mg->vsc_list)
+		goto fail_free_emeta;
+
+	for (i = 0; i < l_mg->nr_lines; i++)
+		l_mg->vsc_list[i] = cpu_to_le32(EMPTY_ENTRY);
+
+	return 0;
+
+fail_free_emeta:
+	while (--i >= 0) {
+		vfree(l_mg->eline_meta[i]->buf);
+		kfree(l_mg->eline_meta[i]);
+	}
+
+fail_free_smeta:
+	for (i = 0; i < PBLK_DATA_LINES; i++)
+		kfree(l_mg->sline_meta[i]);
+
+	return -ENOMEM;
+}
+
 static int pblk_lines_init(struct pblk *pblk)
 {
 	struct nvm_tgt_dev *dev = pblk->dev;
@@ -542,10 +649,32 @@ static int pblk_lines_init(struct pblk *pblk)
 	struct pblk_line_meta *lm = &pblk->lm;
 	struct pblk_line *line;
 	unsigned int smeta_len, emeta_len;
-	long nr_bad_blks, nr_meta_blks, nr_free_blks;
-	int bb_distance;
-	int i;
-	int ret;
+	long nr_bad_blks, nr_free_blks;
+	int bb_distance, max_write_ppas, mod;
+	int i, ret;
+
+	pblk->min_write_pgs = geo->sec_per_pl * (geo->sec_size / PAGE_SIZE);
+	max_write_ppas = pblk->min_write_pgs * geo->nr_luns;
+	pblk->max_write_pgs = (max_write_ppas < nvm_max_phys_sects(dev)) ?
+				max_write_ppas : nvm_max_phys_sects(dev);
+	pblk_set_sec_per_write(pblk, pblk->min_write_pgs);
+
+	if (pblk->max_write_pgs > PBLK_MAX_REQ_ADDRS) {
+		pr_err("pblk: cannot support device max_phys_sect\n");
+		return -EINVAL;
+	}
+
+	div_u64_rem(geo->sec_per_blk, pblk->min_write_pgs, &mod);
+	if (mod) {
+		pr_err("pblk: bad configuration of sectors/pages\n");
+		return -EINVAL;
+	}
+
+	l_mg->nr_lines = geo->blks_per_lun;
+	l_mg->log_line = l_mg->data_line = NULL;
+	l_mg->l_seq_nr = l_mg->d_seq_nr = 0;
+	l_mg->nr_free_lines = 0;
+	bitmap_zero(&l_mg->meta_bitmap, PBLK_DATA_LINES);
 
 	lm->sec_per_line = geo->sec_per_blk * geo->nr_luns;
 	lm->blk_per_line = geo->nr_luns;
@@ -554,20 +683,17 @@ static int pblk_lines_init(struct pblk *pblk)
 	lm->lun_bitmap_len = BITS_TO_LONGS(geo->nr_luns) * sizeof(long);
 	lm->high_thrs = lm->sec_per_line / 2;
 	lm->mid_thrs = lm->sec_per_line / 4;
+	lm->meta_distance = (geo->nr_luns / 2) * pblk->min_write_pgs;
 
 	/* Calculate necessary pages for smeta. See comment over struct
 	 * line_smeta definition
 	 */
-	lm->smeta_len = sizeof(struct line_smeta) +
-				PBLK_LINE_NR_LUN_BITMAP * lm->lun_bitmap_len;
-
 	i = 1;
 add_smeta_page:
 	lm->smeta_sec = i * geo->sec_per_pl;
 	lm->smeta_len = lm->smeta_sec * geo->sec_size;
 
-	smeta_len = sizeof(struct line_smeta) +
-				PBLK_LINE_NR_LUN_BITMAP * lm->lun_bitmap_len;
+	smeta_len = sizeof(struct line_smeta) + lm->lun_bitmap_len;
 	if (smeta_len > lm->smeta_len) {
 		i++;
 		goto add_smeta_page;
@@ -578,66 +704,28 @@ add_smeta_page:
 	 */
 	i = 1;
 add_emeta_page:
-	lm->emeta_sec = i * geo->sec_per_pl;
-	lm->emeta_len = lm->emeta_sec * geo->sec_size;
+	lm->emeta_sec[0] = i * geo->sec_per_pl;
+	lm->emeta_len[0] = lm->emeta_sec[0] * geo->sec_size;
 
-	emeta_len = calc_emeta_len(pblk, lm);
-	if (emeta_len > lm->emeta_len) {
+	emeta_len = calc_emeta_len(pblk);
+	if (emeta_len > lm->emeta_len[0]) {
 		i++;
 		goto add_emeta_page;
 	}
-	lm->emeta_bb = geo->nr_luns - i;
-
-	nr_meta_blks = (lm->smeta_sec + lm->emeta_sec +
-				(geo->sec_per_blk / 2)) / geo->sec_per_blk;
-	lm->min_blk_line = nr_meta_blks + 1;
-
-	l_mg->nr_lines = geo->blks_per_lun;
-	l_mg->log_line = l_mg->data_line = NULL;
-	l_mg->l_seq_nr = l_mg->d_seq_nr = 0;
-	l_mg->nr_free_lines = 0;
-	bitmap_zero(&l_mg->meta_bitmap, PBLK_DATA_LINES);
 
-	/* smeta is always small enough to fit on a kmalloc memory allocation,
-	 * emeta depends on the number of LUNs allocated to the pblk instance
-	 */
-	l_mg->smeta_alloc_type = PBLK_KMALLOC_META;
-	for (i = 0; i < PBLK_DATA_LINES; i++) {
-		l_mg->sline_meta[i].meta = kmalloc(lm->smeta_len, GFP_KERNEL);
-		if (!l_mg->sline_meta[i].meta)
-			while (--i >= 0) {
-				kfree(l_mg->sline_meta[i].meta);
-				ret = -ENOMEM;
-				goto fail;
-			}
+	lm->emeta_bb = geo->nr_luns - i;
+	lm->min_blk_line = 1 + DIV_ROUND_UP(lm->smeta_sec + lm->emeta_sec[0],
+							geo->sec_per_blk);
+	if (lm->min_blk_line > lm->blk_per_line) {
+		pr_err("pblk: config. not supported. Min. LUN in line:%d\n",
+							lm->blk_per_line);
+		ret = -EINVAL;
+		goto fail;
 	}
 
-	if (lm->emeta_len > KMALLOC_MAX_CACHE_SIZE) {
-		l_mg->emeta_alloc_type = PBLK_VMALLOC_META;
-
-		for (i = 0; i < PBLK_DATA_LINES; i++) {
-			l_mg->eline_meta[i].meta = vmalloc(lm->emeta_len);
-			if (!l_mg->eline_meta[i].meta)
-				while (--i >= 0) {
-					vfree(l_mg->eline_meta[i].meta);
-					ret = -ENOMEM;
-					goto fail;
-				}
-		}
-	} else {
-		l_mg->emeta_alloc_type = PBLK_KMALLOC_META;
-
-		for (i = 0; i < PBLK_DATA_LINES; i++) {
-			l_mg->eline_meta[i].meta =
-					kmalloc(lm->emeta_len, GFP_KERNEL);
-			if (!l_mg->eline_meta[i].meta)
-				while (--i >= 0) {
-					kfree(l_mg->eline_meta[i].meta);
-					ret = -ENOMEM;
-					goto fail;
-				}
-		}
-	}
+	ret = pblk_lines_alloc_metadata(pblk);
+	if (ret)
+		goto fail;
 
 	l_mg->bb_template = kzalloc(lm->sec_bitmap_len, GFP_KERNEL);
 	if (!l_mg->bb_template) {
@@ -664,11 +752,14 @@ add_emeta_page:
 	INIT_LIST_HEAD(&l_mg->gc_low_list);
 	INIT_LIST_HEAD(&l_mg->gc_empty_list);
 
+	INIT_LIST_HEAD(&l_mg->emeta_list);
+
 	l_mg->gc_lists[0] = &l_mg->gc_high_list;
 	l_mg->gc_lists[1] = &l_mg->gc_mid_list;
 	l_mg->gc_lists[2] = &l_mg->gc_low_list;
 
 	spin_lock_init(&l_mg->free_lock);
+	spin_lock_init(&l_mg->close_lock);
 	spin_lock_init(&l_mg->gc_lock);
 
 	pblk->lines = kcalloc(l_mg->nr_lines, sizeof(struct pblk_line),
@@ -689,10 +780,16 @@ add_emeta_page:
 		line->type = PBLK_LINETYPE_FREE;
 		line->state = PBLK_LINESTATE_FREE;
 		line->gc_group = PBLK_LINEGC_NONE;
+		line->vsc = &l_mg->vsc_list[i];
 		spin_lock_init(&line->lock);
 
-		nr_bad_blks = pblk_bb_line(pblk, line);
+		ret = pblk_alloc_line_bitmaps(pblk, line);
+		if (ret)
+			goto fail_free_lines;
+
+		nr_bad_blks = pblk_bb_line(pblk, line, lm->blk_per_line);
 		if (nr_bad_blks < 0 || nr_bad_blks > lm->blk_per_line) {
+			pblk_free_line_bitmaps(line);
 			ret = -EINVAL;
 			goto fail_free_lines;
 		}
@@ -713,24 +810,20 @@ add_emeta_page:
 
 	pblk_set_provision(pblk, nr_free_blks);
 
-	sema_init(&pblk->erase_sem, 1);
-
 	/* Cleanup per-LUN bad block lists - managed within lines on run-time */
 	for (i = 0; i < geo->nr_luns; i++)
 		kfree(pblk->luns[i].bb_list);
 
 	return 0;
 fail_free_lines:
-	kfree(pblk->lines);
+	while (--i >= 0)
+		pblk_free_line_bitmaps(&pblk->lines[i]);
 fail_free_bb_aux:
 	kfree(l_mg->bb_aux);
 fail_free_bb_template:
 	kfree(l_mg->bb_template);
 fail_free_meta:
-	for (i = 0; i < PBLK_DATA_LINES; i++) {
-		pblk_mfree(l_mg->sline_meta[i].meta, l_mg->smeta_alloc_type);
-		pblk_mfree(l_mg->eline_meta[i].meta, l_mg->emeta_alloc_type);
-	}
+	pblk_line_meta_free(pblk);
 fail:
 	for (i = 0; i < geo->nr_luns; i++)
 		kfree(pblk->luns[i].bb_list);
@@ -754,6 +847,15 @@ static int pblk_writer_init(struct pblk *pblk)
 
 static void pblk_writer_stop(struct pblk *pblk)
 {
+	/* The pipeline must be stopped and the write buffer emptied before the
+	 * write thread is stopped
+	 */
+	WARN(pblk_rb_read_count(&pblk->rwb),
+			"Stopping not fully persisted write buffer\n");
+
+	WARN(pblk_rb_sync_count(&pblk->rwb),
+			"Stopping not fully synced write buffer\n");
+
 	if (pblk->writer_ts)
 		kthread_stop(pblk->writer_ts);
 	del_timer(&pblk->wtimer);
@@ -772,10 +874,9 @@ static void pblk_free(struct pblk *pblk)
 
 static void pblk_tear_down(struct pblk *pblk)
 {
-	pblk_flush_writer(pblk);
+	pblk_pipeline_stop(pblk);
 	pblk_writer_stop(pblk);
 	pblk_rb_sync_l2p(&pblk->rwb);
-	pblk_recov_pad(pblk);
 	pblk_rwb_free(pblk);
 	pblk_rl_free(&pblk->rl);
 
@@ -821,6 +922,7 @@ static void *pblk_init(struct nvm_tgt_dev *dev, struct gendisk *tdisk,
 
 	pblk->dev = dev;
 	pblk->disk = tdisk;
+	pblk->state = PBLK_STATE_RUNNING;
 
 	spin_lock_init(&pblk->trans_lock);
 	spin_lock_init(&pblk->lock);
@@ -836,8 +938,8 @@ static void *pblk_init(struct nvm_tgt_dev *dev, struct gendisk *tdisk,
 	atomic_long_set(&pblk->req_writes, 0);
 	atomic_long_set(&pblk->sub_writes, 0);
 	atomic_long_set(&pblk->sync_writes, 0);
-	atomic_long_set(&pblk->compl_writes, 0);
 	atomic_long_set(&pblk->inflight_reads, 0);
+	atomic_long_set(&pblk->cache_reads, 0);
 	atomic_long_set(&pblk->sync_reads, 0);
 	atomic_long_set(&pblk->recov_writes, 0);
 	atomic_long_set(&pblk->recov_writes, 0);
@@ -946,11 +1048,20 @@ static struct nvm_tgt_type tt_pblk = {
 
 static int __init pblk_module_init(void)
 {
-	return nvm_register_tgt_type(&tt_pblk);
+	int ret;
+
+	pblk_bio_set = bioset_create(BIO_POOL_SIZE, 0, 0);
+	if (!pblk_bio_set)
+		return -ENOMEM;
+	ret = nvm_register_tgt_type(&tt_pblk);
+	if (ret)
+		bioset_free(pblk_bio_set);
+	return ret;
 }
 
 static void pblk_module_exit(void)
 {
+	bioset_free(pblk_bio_set);
 	nvm_unregister_tgt_type(&tt_pblk);
 }
 
diff --git a/drivers/lightnvm/pblk-map.c b/drivers/lightnvm/pblk-map.c
index 17c16955284d..fddb924f6dde 100644
--- a/drivers/lightnvm/pblk-map.c
+++ b/drivers/lightnvm/pblk-map.c
@@ -25,9 +25,9 @@ static void pblk_map_page_data(struct pblk *pblk, unsigned int sentry,
 			       unsigned int valid_secs)
 {
 	struct pblk_line *line = pblk_line_get_data(pblk);
-	struct line_emeta *emeta = line->emeta;
+	struct pblk_emeta *emeta = line->emeta;
 	struct pblk_w_ctx *w_ctx;
-	__le64 *lba_list = pblk_line_emeta_to_lbas(emeta);
+	__le64 *lba_list = emeta_to_lbas(pblk, emeta->buf);
 	u64 paddr;
 	int nr_secs = pblk->min_write_pgs;
 	int i;
@@ -51,18 +51,20 @@ static void pblk_map_page_data(struct pblk *pblk, unsigned int sentry,
 			w_ctx->ppa = ppa_list[i];
 			meta_list[i].lba = cpu_to_le64(w_ctx->lba);
 			lba_list[paddr] = cpu_to_le64(w_ctx->lba);
-			le64_add_cpu(&line->emeta->nr_valid_lbas, 1);
+			line->nr_valid_lbas++;
 		} else {
-			meta_list[i].lba = cpu_to_le64(ADDR_EMPTY);
-			lba_list[paddr] = cpu_to_le64(ADDR_EMPTY);
-			pblk_map_pad_invalidate(pblk, line, paddr);
+			__le64 addr_empty = cpu_to_le64(ADDR_EMPTY);
+
+			lba_list[paddr] = meta_list[i].lba = addr_empty;
+			__pblk_map_invalidate(pblk, line, paddr);
 		}
 	}
 
 	if (pblk_line_is_full(line)) {
-		line = pblk_line_replace_data(pblk);
-		if (!line)
-			return;
+		struct pblk_line *prev_line = line;
+
+		pblk_line_replace_data(pblk);
+		pblk_line_close_meta(pblk, prev_line);
 	}
 
 	pblk_down_rq(pblk, ppa_list, nr_secs, lun_bitmap);
@@ -91,8 +93,9 @@ void pblk_map_erase_rq(struct pblk *pblk, struct nvm_rq *rqd,
 {
 	struct nvm_tgt_dev *dev = pblk->dev;
 	struct nvm_geo *geo = &dev->geo;
-	struct pblk_line *e_line = pblk_line_get_data_next(pblk);
+	struct pblk_line_meta *lm = &pblk->lm;
 	struct pblk_sec_meta *meta_list = rqd->meta_list;
+	struct pblk_line *e_line, *d_line;
 	unsigned int map_secs;
 	int min = pblk->min_write_pgs;
 	int i, erase_lun;
@@ -102,35 +105,63 @@ void pblk_map_erase_rq(struct pblk *pblk, struct nvm_rq *rqd,
 		pblk_map_page_data(pblk, sentry + i, &rqd->ppa_list[i],
 					lun_bitmap, &meta_list[i], map_secs);
 
-		erase_lun = rqd->ppa_list[i].g.lun * geo->nr_chnls +
-							rqd->ppa_list[i].g.ch;
+		erase_lun = pblk_ppa_to_pos(geo, rqd->ppa_list[i]);
 
-		if (!test_bit(erase_lun, e_line->erase_bitmap)) {
-			if (down_trylock(&pblk->erase_sem))
-				continue;
+		/* line can change after page map. We might also be writing the
+		 * last line.
+		 */
+		e_line = pblk_line_get_erase(pblk);
+		if (!e_line)
+			return pblk_map_rq(pblk, rqd, sentry, lun_bitmap,
+							valid_secs, i + min);
 
+		spin_lock(&e_line->lock);
+		if (!test_bit(erase_lun, e_line->erase_bitmap)) {
 			set_bit(erase_lun, e_line->erase_bitmap);
 			atomic_dec(&e_line->left_eblks);
+
 			*erase_ppa = rqd->ppa_list[i];
 			erase_ppa->g.blk = e_line->id;
 
+			spin_unlock(&e_line->lock);
+
 			/* Avoid evaluating e_line->left_eblks */
 			return pblk_map_rq(pblk, rqd, sentry, lun_bitmap,
 							valid_secs, i + min);
 		}
+		spin_unlock(&e_line->lock);
 	}
 
-	/* Erase blocks that are bad in this line but might not be in next */
-	if (unlikely(ppa_empty(*erase_ppa))) {
-		struct pblk_line_meta *lm = &pblk->lm;
+	d_line = pblk_line_get_data(pblk);
+
+	/* line can change after page map. We might also be writing the
+	 * last line.
+	 */
+	e_line = pblk_line_get_erase(pblk);
+	if (!e_line)
+		return;
 
-		i = find_first_zero_bit(e_line->erase_bitmap, lm->blk_per_line);
-		if (i == lm->blk_per_line)
+	/* Erase blocks that are bad in this line but might not be in next */
+	if (unlikely(ppa_empty(*erase_ppa)) &&
+			bitmap_weight(d_line->blk_bitmap, lm->blk_per_line)) {
+		int bit = -1;
+
+retry:
+		bit = find_next_bit(d_line->blk_bitmap,
+						lm->blk_per_line, bit + 1);
+		if (bit >= lm->blk_per_line)
 			return;
 
-		set_bit(i, e_line->erase_bitmap);
+		spin_lock(&e_line->lock);
+		if (test_bit(bit, e_line->erase_bitmap)) {
+			spin_unlock(&e_line->lock);
+			goto retry;
+		}
+		spin_unlock(&e_line->lock);
+
+		set_bit(bit, e_line->erase_bitmap);
 		atomic_dec(&e_line->left_eblks);
-		*erase_ppa = pblk->luns[i].bppa; /* set ch and lun */
+		*erase_ppa = pblk->luns[bit].bppa; /* set ch and lun */
 		erase_ppa->g.blk = e_line->id;
 	}
 }
diff --git a/drivers/lightnvm/pblk-rb.c b/drivers/lightnvm/pblk-rb.c
index 045384ddc1f9..5ecc154f6831 100644
--- a/drivers/lightnvm/pblk-rb.c
+++ b/drivers/lightnvm/pblk-rb.c
@@ -150,6 +150,7 @@ try:
 	/* Release flags on context. Protect from writes and reads */
 	smp_store_release(&w_ctx->flags, PBLK_WRITABLE_ENTRY);
 	pblk_ppa_set_empty(&w_ctx->ppa);
+	w_ctx->lba = ADDR_EMPTY;
 }
 
 #define pblk_rb_ring_count(head, tail, size) CIRC_CNT(head, tail, size)
@@ -180,6 +181,14 @@ unsigned int pblk_rb_read_count(struct pblk_rb *rb)
 	return pblk_rb_ring_count(mem, subm, rb->nr_entries);
 }
 
+unsigned int pblk_rb_sync_count(struct pblk_rb *rb)
+{
+	unsigned int mem = READ_ONCE(rb->mem);
+	unsigned int sync = READ_ONCE(rb->sync);
+
+	return pblk_rb_ring_count(mem, sync, rb->nr_entries);
+}
+
 unsigned int pblk_rb_read_commit(struct pblk_rb *rb, unsigned int nr_entries)
 {
 	unsigned int subm;
@@ -199,12 +208,22 @@ static int __pblk_rb_update_l2p(struct pblk_rb *rb, unsigned int *l2p_upd,
 	struct pblk_line *line;
 	struct pblk_rb_entry *entry;
 	struct pblk_w_ctx *w_ctx;
+	unsigned int user_io = 0, gc_io = 0;
 	unsigned int i;
+	int flags;
 
 	for (i = 0; i < to_update; i++) {
 		entry = &rb->entries[*l2p_upd];
 		w_ctx = &entry->w_ctx;
 
+		flags = READ_ONCE(entry->w_ctx.flags);
+		if (flags & PBLK_IOTYPE_USER)
+			user_io++;
+		else if (flags & PBLK_IOTYPE_GC)
+			gc_io++;
+		else
+			WARN(1, "pblk: unknown IO type\n");
+
 		pblk_update_map_dev(pblk, w_ctx->lba, w_ctx->ppa,
 							entry->cacheline);
 
@@ -214,6 +233,8 @@ static int __pblk_rb_update_l2p(struct pblk_rb *rb, unsigned int *l2p_upd,
 		*l2p_upd = (*l2p_upd + 1) & (rb->nr_entries - 1);
 	}
 
+	pblk_rl_out(&pblk->rl, user_io, gc_io);
+
 	return 0;
 }
 
@@ -357,6 +378,9 @@ static int pblk_rb_sync_point_set(struct pblk_rb *rb, struct bio *bio,
 	/* Protect syncs */
 	smp_store_release(&rb->sync_point, sync_point);
 
+	if (!bio)
+		return 0;
+
 	spin_lock_irq(&rb->s_lock);
 	bio_list_add(&entry->w_ctx.bios, bio);
 	spin_unlock_irq(&rb->s_lock);
@@ -395,6 +419,17 @@ static int pblk_rb_may_write(struct pblk_rb *rb, unsigned int nr_entries,
 	return 1;
 }
 
+void pblk_rb_flush(struct pblk_rb *rb)
+{
+	struct pblk *pblk = container_of(rb, struct pblk, rwb);
+	unsigned int mem = READ_ONCE(rb->mem);
+
+	if (pblk_rb_sync_point_set(rb, NULL, mem))
+		return;
+
+	pblk_write_should_kick(pblk);
+}
+
 static int pblk_rb_may_write_flush(struct pblk_rb *rb, unsigned int nr_entries,
 				   unsigned int *pos, struct bio *bio,
 				   int *io_ret)
@@ -431,15 +466,16 @@ int pblk_rb_may_write_user(struct pblk_rb *rb, struct bio *bio,
 			   unsigned int nr_entries, unsigned int *pos)
 {
 	struct pblk *pblk = container_of(rb, struct pblk, rwb);
-	int flush_done;
+	int io_ret;
 
 	spin_lock(&rb->w_lock);
-	if (!pblk_rl_user_may_insert(&pblk->rl, nr_entries)) {
+	io_ret = pblk_rl_user_may_insert(&pblk->rl, nr_entries);
+	if (io_ret) {
 		spin_unlock(&rb->w_lock);
-		return NVM_IO_REQUEUE;
+		return io_ret;
 	}
 
-	if (!pblk_rb_may_write_flush(rb, nr_entries, pos, bio, &flush_done)) {
+	if (!pblk_rb_may_write_flush(rb, nr_entries, pos, bio, &io_ret)) {
 		spin_unlock(&rb->w_lock);
 		return NVM_IO_REQUEUE;
 	}
@@ -447,7 +483,7 @@ int pblk_rb_may_write_user(struct pblk_rb *rb, struct bio *bio,
 	pblk_rl_user_in(&pblk->rl, nr_entries);
 	spin_unlock(&rb->w_lock);
 
-	return flush_done;
+	return io_ret;
 }
 
 /*
@@ -521,20 +557,18 @@ out:
  * This function is used by the write thread to form the write bio that will
  * persist data on the write buffer to the media.
  */
-unsigned int pblk_rb_read_to_bio(struct pblk_rb *rb, struct bio *bio,
-				 struct pblk_c_ctx *c_ctx,
-				 unsigned int pos,
-				 unsigned int nr_entries,
-				 unsigned int count)
+unsigned int pblk_rb_read_to_bio(struct pblk_rb *rb, struct nvm_rq *rqd,
+				 struct bio *bio, unsigned int pos,
+				 unsigned int nr_entries, unsigned int count)
 {
 	struct pblk *pblk = container_of(rb, struct pblk, rwb);
+	struct request_queue *q = pblk->dev->q;
+	struct pblk_c_ctx *c_ctx = nvm_rq_to_pdu(rqd);
 	struct pblk_rb_entry *entry;
 	struct page *page;
-	unsigned int pad = 0, read = 0, to_read = nr_entries;
-	unsigned int user_io = 0, gc_io = 0;
+	unsigned int pad = 0, to_read = nr_entries;
 	unsigned int i;
 	int flags;
-	int ret;
 
 	if (count < nr_entries) {
 		pad = nr_entries - count;
@@ -553,15 +587,10 @@ unsigned int pblk_rb_read_to_bio(struct pblk_rb *rb, struct bio *bio,
 		 */
 try:
 		flags = READ_ONCE(entry->w_ctx.flags);
-		if (!(flags & PBLK_WRITTEN_DATA))
+		if (!(flags & PBLK_WRITTEN_DATA)) {
+			io_schedule();
 			goto try;
-
-		if (flags & PBLK_IOTYPE_USER)
-			user_io++;
-		else if (flags & PBLK_IOTYPE_GC)
-			gc_io++;
-		else
-			WARN(1, "pblk: unknown IO type\n");
+		}
 
 		page = virt_to_page(entry->data);
 		if (!page) {
@@ -570,17 +599,17 @@ try:
 			flags |= PBLK_SUBMITTED_ENTRY;
 			/* Release flags on context. Protect from writes */
 			smp_store_release(&entry->w_ctx.flags, flags);
-			goto out;
+			return NVM_IO_ERR;
 		}
 
-		ret = bio_add_page(bio, page, rb->seg_size, 0);
-		if (ret != rb->seg_size) {
+		if (bio_add_pc_page(q, bio, page, rb->seg_size, 0) !=
+								rb->seg_size) {
 			pr_err("pblk: could not add page to write bio\n");
 			flags &= ~PBLK_WRITTEN_DATA;
 			flags |= PBLK_SUBMITTED_ENTRY;
 			/* Release flags on context. Protect from writes */
 			smp_store_release(&entry->w_ctx.flags, flags);
-			goto out;
+			return NVM_IO_ERR;
 		}
 
 		if (flags & PBLK_FLUSH_ENTRY) {
@@ -607,14 +636,19 @@ try:
 		pos = (pos + 1) & (rb->nr_entries - 1);
 	}
 
-	read = to_read;
-	pblk_rl_out(&pblk->rl, user_io, gc_io);
+	if (pad) {
+		if (pblk_bio_add_pages(pblk, bio, GFP_KERNEL, pad)) {
+			pr_err("pblk: could not pad page in write bio\n");
+			return NVM_IO_ERR;
+		}
+	}
+
 #ifdef CONFIG_NVM_DEBUG
 	atomic_long_add(pad, &((struct pblk *)
 			(container_of(rb, struct pblk, rwb)))->padded_writes);
 #endif
-out:
-	return read;
+
+	return NVM_IO_OK;
 }
 
 /*
@@ -623,15 +657,17 @@ out:
  * be directed to disk.
  */
 int pblk_rb_copy_to_bio(struct pblk_rb *rb, struct bio *bio, sector_t lba,
-			u64 pos, int bio_iter)
+			struct ppa_addr ppa, int bio_iter)
 {
+	struct pblk *pblk = container_of(rb, struct pblk, rwb);
 	struct pblk_rb_entry *entry;
 	struct pblk_w_ctx *w_ctx;
+	struct ppa_addr l2p_ppa;
+	u64 pos = pblk_addr_to_cacheline(ppa);
 	void *data;
 	int flags;
 	int ret = 1;
 
-	spin_lock(&rb->w_lock);
 
 #ifdef CONFIG_NVM_DEBUG
 	/* Caller must ensure that the access will not cause an overflow */
@@ -641,8 +677,14 @@ int pblk_rb_copy_to_bio(struct pblk_rb *rb, struct bio *bio, sector_t lba,
 	w_ctx = &entry->w_ctx;
 	flags = READ_ONCE(w_ctx->flags);
 
+	spin_lock(&rb->w_lock);
+	spin_lock(&pblk->trans_lock);
+	l2p_ppa = pblk_trans_map_get(pblk, lba);
+	spin_unlock(&pblk->trans_lock);
+
 	/* Check if the entry has been overwritten or is scheduled to be */
-	if (w_ctx->lba != lba || flags & PBLK_WRITABLE_ENTRY) {
+	if (!pblk_ppa_comp(l2p_ppa, ppa) || w_ctx->lba != lba ||
+						flags & PBLK_WRITABLE_ENTRY) {
 		ret = 0;
 		goto out;
 	}
diff --git a/drivers/lightnvm/pblk-read.c b/drivers/lightnvm/pblk-read.c
index 4a12f14d78c6..4e5c48f3de62 100644
--- a/drivers/lightnvm/pblk-read.c
+++ b/drivers/lightnvm/pblk-read.c
@@ -34,8 +34,7 @@ static int pblk_read_from_cache(struct pblk *pblk, struct bio *bio,
 	BUG_ON(!pblk_addr_in_cache(ppa));
 #endif
 
-	return pblk_rb_copy_to_bio(&pblk->rwb, bio, lba,
-					pblk_addr_to_cacheline(ppa), bio_iter);
+	return pblk_rb_copy_to_bio(&pblk->rwb, bio, lba, ppa, bio_iter);
 }
 
 static void pblk_read_ppalist_rq(struct pblk *pblk, struct nvm_rq *rqd,
@@ -76,6 +75,9 @@ retry:
 			}
 			WARN_ON(test_and_set_bit(i, read_bitmap));
 			advanced_bio = 1;
+#ifdef CONFIG_NVM_DEBUG
+			atomic_long_inc(&pblk->cache_reads);
+#endif
 		} else {
 			/* Read from media non-cached sectors */
 			rqd->ppa_list[j++] = p;
@@ -85,6 +87,11 @@ retry:
 			bio_advance(bio, PBLK_EXPOSED_PAGE_SIZE);
 	}
 
+	if (pblk_io_aligned(pblk, nr_secs))
+		rqd->flags = pblk_set_read_mode(pblk, PBLK_READ_SEQUENTIAL);
+	else
+		rqd->flags = pblk_set_read_mode(pblk, PBLK_READ_RANDOM);
+
 #ifdef CONFIG_NVM_DEBUG
 	atomic_long_add(nr_secs, &pblk->inflight_reads);
 #endif
@@ -94,8 +101,6 @@ static int pblk_submit_read_io(struct pblk *pblk, struct nvm_rq *rqd)
 {
 	int err;
 
-	rqd->flags = pblk_set_read_mode(pblk);
-
 	err = pblk_submit_io(pblk, rqd);
 	if (err)
 		return NVM_IO_ERR;
@@ -107,27 +112,27 @@ static void pblk_end_io_read(struct nvm_rq *rqd)
 {
 	struct pblk *pblk = rqd->private;
 	struct nvm_tgt_dev *dev = pblk->dev;
-	struct pblk_r_ctx *r_ctx = nvm_rq_to_pdu(rqd);
+	struct pblk_g_ctx *r_ctx = nvm_rq_to_pdu(rqd);
 	struct bio *bio = rqd->bio;
 
 	if (rqd->error)
 		pblk_log_read_err(pblk, rqd);
 #ifdef CONFIG_NVM_DEBUG
 	else
-		WARN_ONCE(bio->bi_error, "pblk: corrupted read error\n");
+		WARN_ONCE(bio->bi_status, "pblk: corrupted read error\n");
 #endif
 
-	if (rqd->nr_ppas > 1)
-		nvm_dev_dma_free(dev->parent, rqd->ppa_list, rqd->dma_ppa_list);
+	nvm_dev_dma_free(dev->parent, rqd->meta_list, rqd->dma_meta_list);
 
 	bio_put(bio);
-	if (r_ctx->orig_bio) {
+	if (r_ctx->private) {
+		struct bio *orig_bio = r_ctx->private;
+
 #ifdef CONFIG_NVM_DEBUG
-		WARN_ONCE(r_ctx->orig_bio->bi_error,
-						"pblk: corrupted read bio\n");
+		WARN_ONCE(orig_bio->bi_status, "pblk: corrupted read bio\n");
 #endif
-		bio_endio(r_ctx->orig_bio);
-		bio_put(r_ctx->orig_bio);
+		bio_endio(orig_bio);
+		bio_put(orig_bio);
 	}
 
 #ifdef CONFIG_NVM_DEBUG
@@ -136,6 +141,7 @@ static void pblk_end_io_read(struct nvm_rq *rqd)
 #endif
 
 	pblk_free_rqd(pblk, rqd, READ);
+	atomic_dec(&pblk->inflight_io);
 }
 
 static int pblk_fill_partial_read_bio(struct pblk *pblk, struct nvm_rq *rqd,
@@ -173,6 +179,7 @@ static int pblk_fill_partial_read_bio(struct pblk *pblk, struct nvm_rq *rqd,
 
 	rqd->bio = new_bio;
 	rqd->nr_ppas = nr_holes;
+	rqd->flags = pblk_set_read_mode(pblk, PBLK_READ_RANDOM);
 	rqd->end_io = NULL;
 
 	if (unlikely(nr_secs > 1 && nr_holes == 1)) {
@@ -280,9 +287,14 @@ retry:
 			goto retry;
 		}
 		WARN_ON(test_and_set_bit(0, read_bitmap));
+#ifdef CONFIG_NVM_DEBUG
+			atomic_long_inc(&pblk->cache_reads);
+#endif
 	} else {
 		rqd->ppa_addr = ppa;
 	}
+
+	rqd->flags = pblk_set_read_mode(pblk, PBLK_READ_RANDOM);
 }
 
 int pblk_submit_read(struct pblk *pblk, struct bio *bio)
@@ -316,13 +328,16 @@ int pblk_submit_read(struct pblk *pblk, struct bio *bio)
 	 */
 	bio_init_idx = pblk_get_bi_idx(bio);
 
+	rqd->meta_list = nvm_dev_dma_alloc(dev->parent, GFP_KERNEL,
+							&rqd->dma_meta_list);
+	if (!rqd->meta_list) {
+		pr_err("pblk: not able to allocate ppa list\n");
+		goto fail_rqd_free;
+	}
+
 	if (nr_secs > 1) {
-		rqd->ppa_list = nvm_dev_dma_alloc(dev->parent, GFP_KERNEL,
-						&rqd->dma_ppa_list);
-		if (!rqd->ppa_list) {
-			pr_err("pblk: not able to allocate ppa list\n");
-			goto fail_rqd_free;
-		}
+		rqd->ppa_list = rqd->meta_list + pblk_dma_meta_size;
+		rqd->dma_ppa_list = rqd->dma_meta_list + pblk_dma_meta_size;
 
 		pblk_read_ppalist_rq(pblk, rqd, &read_bitmap);
 	} else {
@@ -332,6 +347,7 @@ int pblk_submit_read(struct pblk *pblk, struct bio *bio)
 	bio_get(bio);
 	if (bitmap_full(&read_bitmap, nr_secs)) {
 		bio_endio(bio);
+		atomic_inc(&pblk->inflight_io);
 		pblk_end_io_read(rqd);
 		return NVM_IO_OK;
 	}
@@ -339,17 +355,17 @@ int pblk_submit_read(struct pblk *pblk, struct bio *bio)
 	/* All sectors are to be read from the device */
 	if (bitmap_empty(&read_bitmap, rqd->nr_ppas)) {
 		struct bio *int_bio = NULL;
-		struct pblk_r_ctx *r_ctx = nvm_rq_to_pdu(rqd);
+		struct pblk_g_ctx *r_ctx = nvm_rq_to_pdu(rqd);
 
 		/* Clone read bio to deal with read errors internally */
-		int_bio = bio_clone_bioset(bio, GFP_KERNEL, fs_bio_set);
+		int_bio = bio_clone_fast(bio, GFP_KERNEL, pblk_bio_set);
 		if (!int_bio) {
 			pr_err("pblk: could not clone read bio\n");
 			return NVM_IO_ERR;
 		}
 
 		rqd->bio = int_bio;
-		r_ctx->orig_bio = bio;
+		r_ctx->private = bio;
 
 		ret = pblk_submit_read_io(pblk, rqd);
 		if (ret) {
@@ -445,7 +461,6 @@ int pblk_submit_read_gc(struct pblk *pblk, u64 *lba_list, void *data,
 {
 	struct nvm_tgt_dev *dev = pblk->dev;
 	struct nvm_geo *geo = &dev->geo;
-	struct request_queue *q = dev->q;
 	struct bio *bio;
 	struct nvm_rq rqd;
 	int ret, data_len;
@@ -453,22 +468,19 @@ int pblk_submit_read_gc(struct pblk *pblk, u64 *lba_list, void *data,
 
 	memset(&rqd, 0, sizeof(struct nvm_rq));
 
+	rqd.meta_list = nvm_dev_dma_alloc(dev->parent, GFP_KERNEL,
+							&rqd.dma_meta_list);
+	if (!rqd.meta_list)
+		return NVM_IO_ERR;
+
 	if (nr_secs > 1) {
-		rqd.ppa_list = nvm_dev_dma_alloc(dev->parent, GFP_KERNEL,
-							&rqd.dma_ppa_list);
-		if (!rqd.ppa_list)
-			return NVM_IO_ERR;
+		rqd.ppa_list = rqd.meta_list + pblk_dma_meta_size;
+		rqd.dma_ppa_list = rqd.dma_meta_list + pblk_dma_meta_size;
 
 		*secs_to_gc = read_ppalist_rq_gc(pblk, &rqd, line, lba_list,
 								nr_secs);
-		if (*secs_to_gc == 1) {
-			struct ppa_addr ppa;
-
-			ppa = rqd.ppa_list[0];
-			nvm_dev_dma_free(dev->parent, rqd.ppa_list,
-							rqd.dma_ppa_list);
-			rqd.ppa_addr = ppa;
-		}
+		if (*secs_to_gc == 1)
+			rqd.ppa_addr = rqd.ppa_list[0];
 	} else {
 		*secs_to_gc = read_rq_gc(pblk, &rqd, line, lba_list[0]);
 	}
@@ -477,7 +489,8 @@ int pblk_submit_read_gc(struct pblk *pblk, u64 *lba_list, void *data,
 		goto out;
 
 	data_len = (*secs_to_gc) * geo->sec_size;
-	bio = bio_map_kern(q, data, data_len, GFP_KERNEL);
+	bio = pblk_bio_map_addr(pblk, data, *secs_to_gc, data_len,
+						PBLK_KMALLOC_META, GFP_KERNEL);
 	if (IS_ERR(bio)) {
 		pr_err("pblk: could not allocate GC bio (%lu)\n", PTR_ERR(bio));
 		goto err_free_dma;
@@ -490,6 +503,7 @@ int pblk_submit_read_gc(struct pblk *pblk, u64 *lba_list, void *data,
 	rqd.end_io = pblk_end_io_sync;
 	rqd.private = &wait;
 	rqd.nr_ppas = *secs_to_gc;
+	rqd.flags = pblk_set_read_mode(pblk, PBLK_READ_RANDOM);
 	rqd.bio = bio;
 
 	ret = pblk_submit_read_io(pblk, &rqd);
@@ -503,6 +517,7 @@ int pblk_submit_read_gc(struct pblk *pblk, u64 *lba_list, void *data,
 				msecs_to_jiffies(PBLK_COMMAND_TIMEOUT_MS))) {
 		pr_err("pblk: GC read I/O timed out\n");
 	}
+	atomic_dec(&pblk->inflight_io);
 
 	if (rqd.error) {
 		atomic_long_inc(&pblk->read_failed_gc);
@@ -518,12 +533,10 @@ int pblk_submit_read_gc(struct pblk *pblk, u64 *lba_list, void *data,
 #endif
 
 out:
-	if (rqd.nr_ppas > 1)
-		nvm_dev_dma_free(dev->parent, rqd.ppa_list, rqd.dma_ppa_list);
+	nvm_dev_dma_free(dev->parent, rqd.meta_list, rqd.dma_meta_list);
 	return NVM_IO_OK;
 
 err_free_dma:
-	if (rqd.nr_ppas > 1)
-		nvm_dev_dma_free(dev->parent, rqd.ppa_list, rqd.dma_ppa_list);
+	nvm_dev_dma_free(dev->parent, rqd.meta_list, rqd.dma_meta_list);
 	return NVM_IO_ERR;
 }
diff --git a/drivers/lightnvm/pblk-recovery.c b/drivers/lightnvm/pblk-recovery.c
index f8f85087cd3c..0e48d3e4e143 100644
--- a/drivers/lightnvm/pblk-recovery.c
+++ b/drivers/lightnvm/pblk-recovery.c
@@ -120,18 +120,18 @@ int pblk_recov_setup_rq(struct pblk *pblk, struct pblk_c_ctx *c_ctx,
 	return 0;
 }
 
-__le64 *pblk_recov_get_lba_list(struct pblk *pblk, struct line_emeta *emeta)
+__le64 *pblk_recov_get_lba_list(struct pblk *pblk, struct line_emeta *emeta_buf)
 {
 	u32 crc;
 
-	crc = pblk_calc_emeta_crc(pblk, emeta);
-	if (le32_to_cpu(emeta->crc) != crc)
+	crc = pblk_calc_emeta_crc(pblk, emeta_buf);
+	if (le32_to_cpu(emeta_buf->crc) != crc)
 		return NULL;
 
-	if (le32_to_cpu(emeta->header.identifier) != PBLK_MAGIC)
+	if (le32_to_cpu(emeta_buf->header.identifier) != PBLK_MAGIC)
 		return NULL;
 
-	return pblk_line_emeta_to_lbas(emeta);
+	return emeta_to_lbas(pblk, emeta_buf);
 }
 
 static int pblk_recov_l2p_from_emeta(struct pblk *pblk, struct pblk_line *line)
@@ -139,19 +139,20 @@ static int pblk_recov_l2p_from_emeta(struct pblk *pblk, struct pblk_line *line)
 	struct nvm_tgt_dev *dev = pblk->dev;
 	struct nvm_geo *geo = &dev->geo;
 	struct pblk_line_meta *lm = &pblk->lm;
-	struct line_emeta *emeta = line->emeta;
+	struct pblk_emeta *emeta = line->emeta;
+	struct line_emeta *emeta_buf = emeta->buf;
 	__le64 *lba_list;
 	int data_start;
 	int nr_data_lbas, nr_valid_lbas, nr_lbas = 0;
 	int i;
 
-	lba_list = pblk_recov_get_lba_list(pblk, emeta);
+	lba_list = pblk_recov_get_lba_list(pblk, emeta_buf);
 	if (!lba_list)
 		return 1;
 
 	data_start = pblk_line_smeta_start(pblk, line) + lm->smeta_sec;
-	nr_data_lbas = lm->sec_per_line - lm->emeta_sec;
-	nr_valid_lbas = le64_to_cpu(emeta->nr_valid_lbas);
+	nr_data_lbas = lm->sec_per_line - lm->emeta_sec[0];
+	nr_valid_lbas = le64_to_cpu(emeta_buf->nr_valid_lbas);
 
 	for (i = data_start; i < nr_data_lbas && nr_lbas < nr_valid_lbas; i++) {
 		struct ppa_addr ppa;
@@ -169,7 +170,7 @@ static int pblk_recov_l2p_from_emeta(struct pblk *pblk, struct pblk_line *line)
 			if (test_and_set_bit(i, line->invalid_bitmap))
 				WARN_ONCE(1, "pblk: rec. double invalidate:\n");
 			else
-				line->vsc--;
+				le32_add_cpu(line->vsc, -1);
 			spin_unlock(&line->lock);
 
 			continue;
@@ -181,7 +182,7 @@ static int pblk_recov_l2p_from_emeta(struct pblk *pblk, struct pblk_line *line)
 
 	if (nr_valid_lbas != nr_lbas)
 		pr_err("pblk: line %d - inconsistent lba list(%llu/%d)\n",
-				line->id, line->emeta->nr_valid_lbas, nr_lbas);
+				line->id, emeta_buf->nr_valid_lbas, nr_lbas);
 
 	line->left_msecs = 0;
 
@@ -195,7 +196,7 @@ static int pblk_calc_sec_in_line(struct pblk *pblk, struct pblk_line *line)
 	struct pblk_line_meta *lm = &pblk->lm;
 	int nr_bb = bitmap_weight(line->blk_bitmap, lm->blk_per_line);
 
-	return lm->sec_per_line - lm->smeta_sec - lm->emeta_sec -
+	return lm->sec_per_line - lm->smeta_sec - lm->emeta_sec[0] -
 				nr_bb * geo->sec_per_blk;
 }
 
@@ -240,7 +241,7 @@ static int pblk_recov_read_oob(struct pblk *pblk, struct pblk_line *line,
 	r_ptr_int = r_ptr;
 
 next_read_rq:
-	memset(rqd, 0, pblk_r_rq_size);
+	memset(rqd, 0, pblk_g_rq_size);
 
 	rq_ppas = pblk_calc_secs(pblk, left_ppas, 0);
 	if (!rq_ppas)
@@ -256,7 +257,6 @@ next_read_rq:
 
 	rqd->bio = bio;
 	rqd->opcode = NVM_OP_PREAD;
-	rqd->flags = pblk_set_read_mode(pblk);
 	rqd->meta_list = meta_list;
 	rqd->nr_ppas = rq_ppas;
 	rqd->ppa_list = ppa_list;
@@ -265,6 +265,11 @@ next_read_rq:
 	rqd->end_io = pblk_end_io_sync;
 	rqd->private = &wait;
 
+	if (pblk_io_aligned(pblk, rq_ppas))
+		rqd->flags = pblk_set_read_mode(pblk, PBLK_READ_SEQUENTIAL);
+	else
+		rqd->flags = pblk_set_read_mode(pblk, PBLK_READ_RANDOM);
+
 	for (i = 0; i < rqd->nr_ppas; ) {
 		struct ppa_addr ppa;
 		int pos;
@@ -295,7 +300,7 @@ next_read_rq:
 		pr_err("pblk: L2P recovery read timed out\n");
 		return -EINTR;
 	}
-
+	atomic_dec(&pblk->inflight_io);
 	reinit_completion(&wait);
 
 	/* At this point, the read should not fail. If it does, it is a problem
@@ -322,47 +327,94 @@ next_read_rq:
 	return 0;
 }
 
+static void pblk_recov_complete(struct kref *ref)
+{
+	struct pblk_pad_rq *pad_rq = container_of(ref, struct pblk_pad_rq, ref);
+
+	complete(&pad_rq->wait);
+}
+
+static void pblk_end_io_recov(struct nvm_rq *rqd)
+{
+	struct pblk_pad_rq *pad_rq = rqd->private;
+	struct pblk *pblk = pad_rq->pblk;
+	struct nvm_tgt_dev *dev = pblk->dev;
+
+	kref_put(&pad_rq->ref, pblk_recov_complete);
+	nvm_dev_dma_free(dev->parent, rqd->meta_list, rqd->dma_meta_list);
+	pblk_free_rqd(pblk, rqd, WRITE);
+}
+
 static int pblk_recov_pad_oob(struct pblk *pblk, struct pblk_line *line,
-			      struct pblk_recov_alloc p, int left_ppas)
+			      int left_ppas)
 {
 	struct nvm_tgt_dev *dev = pblk->dev;
 	struct nvm_geo *geo = &dev->geo;
 	struct ppa_addr *ppa_list;
 	struct pblk_sec_meta *meta_list;
+	struct pblk_pad_rq *pad_rq;
 	struct nvm_rq *rqd;
 	struct bio *bio;
 	void *data;
 	dma_addr_t dma_ppa_list, dma_meta_list;
-	__le64 *lba_list = pblk_line_emeta_to_lbas(line->emeta);
+	__le64 *lba_list = emeta_to_lbas(pblk, line->emeta->buf);
 	u64 w_ptr = line->cur_sec;
-	int left_line_ppas = line->left_msecs;
-	int rq_ppas, rq_len;
+	int left_line_ppas, rq_ppas, rq_len;
 	int i, j;
 	int ret = 0;
-	DECLARE_COMPLETION_ONSTACK(wait);
 
-	ppa_list = p.ppa_list;
-	meta_list = p.meta_list;
-	rqd = p.rqd;
-	data = p.data;
-	dma_ppa_list = p.dma_ppa_list;
-	dma_meta_list = p.dma_meta_list;
+	spin_lock(&line->lock);
+	left_line_ppas = line->left_msecs;
+	spin_unlock(&line->lock);
+
+	pad_rq = kmalloc(sizeof(struct pblk_pad_rq), GFP_KERNEL);
+	if (!pad_rq)
+		return -ENOMEM;
+
+	data = vzalloc(pblk->max_write_pgs * geo->sec_size);
+	if (!data) {
+		ret = -ENOMEM;
+		goto free_rq;
+	}
+
+	pad_rq->pblk = pblk;
+	init_completion(&pad_rq->wait);
+	kref_init(&pad_rq->ref);
 
 next_pad_rq:
 	rq_ppas = pblk_calc_secs(pblk, left_ppas, 0);
-	if (!rq_ppas)
-		rq_ppas = pblk->min_write_pgs;
+	if (rq_ppas < pblk->min_write_pgs) {
+		pr_err("pblk: corrupted pad line %d\n", line->id);
+		goto free_rq;
+	}
+
 	rq_len = rq_ppas * geo->sec_size;
 
+	meta_list = nvm_dev_dma_alloc(dev->parent, GFP_KERNEL, &dma_meta_list);
+	if (!meta_list) {
+		ret = -ENOMEM;
+		goto free_data;
+	}
+
+	ppa_list = (void *)(meta_list) + pblk_dma_meta_size;
+	dma_ppa_list = dma_meta_list + pblk_dma_meta_size;
+
+	rqd = pblk_alloc_rqd(pblk, WRITE);
+	if (IS_ERR(rqd)) {
+		ret = PTR_ERR(rqd);
+		goto fail_free_meta;
+	}
+	memset(rqd, 0, pblk_w_rq_size);
+
 	bio = bio_map_kern(dev->q, data, rq_len, GFP_KERNEL);
-	if (IS_ERR(bio))
-		return PTR_ERR(bio);
+	if (IS_ERR(bio)) {
+		ret = PTR_ERR(bio);
+		goto fail_free_rqd;
+	}
 
 	bio->bi_iter.bi_sector = 0; /* internal bio */
 	bio_set_op_attrs(bio, REQ_OP_WRITE, 0);
 
-	memset(rqd, 0, pblk_r_rq_size);
-
 	rqd->bio = bio;
 	rqd->opcode = NVM_OP_PWRITE;
 	rqd->flags = pblk_set_progr_mode(pblk, WRITE);
@@ -371,8 +423,8 @@ next_pad_rq:
 	rqd->ppa_list = ppa_list;
 	rqd->dma_ppa_list = dma_ppa_list;
 	rqd->dma_meta_list = dma_meta_list;
-	rqd->end_io = pblk_end_io_sync;
-	rqd->private = &wait;
+	rqd->end_io = pblk_end_io_recov;
+	rqd->private = pad_rq;
 
 	for (i = 0; i < rqd->nr_ppas; ) {
 		struct ppa_addr ppa;
@@ -390,34 +442,51 @@ next_pad_rq:
 
 		for (j = 0; j < pblk->min_write_pgs; j++, i++, w_ptr++) {
 			struct ppa_addr dev_ppa;
+			__le64 addr_empty = cpu_to_le64(ADDR_EMPTY);
 
 			dev_ppa = addr_to_gen_ppa(pblk, w_ptr, line->id);
 
 			pblk_map_invalidate(pblk, dev_ppa);
-			meta_list[i].lba = cpu_to_le64(ADDR_EMPTY);
-			lba_list[w_ptr] = cpu_to_le64(ADDR_EMPTY);
+			lba_list[w_ptr] = meta_list[i].lba = addr_empty;
 			rqd->ppa_list[i] = dev_ppa;
 		}
 	}
 
+	kref_get(&pad_rq->ref);
+
 	ret = pblk_submit_io(pblk, rqd);
 	if (ret) {
 		pr_err("pblk: I/O submission failed: %d\n", ret);
-		return ret;
+		goto free_data;
 	}
 
-	if (!wait_for_completion_io_timeout(&wait,
-				msecs_to_jiffies(PBLK_COMMAND_TIMEOUT_MS))) {
-		pr_err("pblk: L2P recovery write timed out\n");
-	}
-	reinit_completion(&wait);
+	atomic_dec(&pblk->inflight_io);
 
 	left_line_ppas -= rq_ppas;
 	left_ppas -= rq_ppas;
-	if (left_ppas > 0 && left_line_ppas)
+	if (left_ppas && left_line_ppas)
 		goto next_pad_rq;
 
-	return 0;
+	kref_put(&pad_rq->ref, pblk_recov_complete);
+
+	if (!wait_for_completion_io_timeout(&pad_rq->wait,
+				msecs_to_jiffies(PBLK_COMMAND_TIMEOUT_MS))) {
+		pr_err("pblk: pad write timed out\n");
+		ret = -ETIME;
+	}
+
+free_rq:
+	kfree(pad_rq);
+free_data:
+	vfree(data);
+	return ret;
+
+fail_free_rqd:
+	pblk_free_rqd(pblk, rqd, WRITE);
+fail_free_meta:
+	nvm_dev_dma_free(dev->parent, meta_list, dma_meta_list);
+	kfree(pad_rq);
+	return ret;
 }
 
 /* When this function is called, it means that not all upper pages have been
@@ -456,7 +525,7 @@ static int pblk_recov_scan_all_oob(struct pblk *pblk, struct pblk_line *line,
 	rec_round = 0;
 
 next_rq:
-	memset(rqd, 0, pblk_r_rq_size);
+	memset(rqd, 0, pblk_g_rq_size);
 
 	rq_ppas = pblk_calc_secs(pblk, left_ppas, 0);
 	if (!rq_ppas)
@@ -472,7 +541,6 @@ next_rq:
 
 	rqd->bio = bio;
 	rqd->opcode = NVM_OP_PREAD;
-	rqd->flags = pblk_set_read_mode(pblk);
 	rqd->meta_list = meta_list;
 	rqd->nr_ppas = rq_ppas;
 	rqd->ppa_list = ppa_list;
@@ -481,6 +549,11 @@ next_rq:
 	rqd->end_io = pblk_end_io_sync;
 	rqd->private = &wait;
 
+	if (pblk_io_aligned(pblk, rq_ppas))
+		rqd->flags = pblk_set_read_mode(pblk, PBLK_READ_SEQUENTIAL);
+	else
+		rqd->flags = pblk_set_read_mode(pblk, PBLK_READ_RANDOM);
+
 	for (i = 0; i < rqd->nr_ppas; ) {
 		struct ppa_addr ppa;
 		int pos;
@@ -510,6 +583,7 @@ next_rq:
 				msecs_to_jiffies(PBLK_COMMAND_TIMEOUT_MS))) {
 		pr_err("pblk: L2P recovery read timed out\n");
 	}
+	atomic_dec(&pblk->inflight_io);
 	reinit_completion(&wait);
 
 	/* This should not happen since the read failed during normal recovery,
@@ -544,7 +618,7 @@ next_rq:
 		if (pad_secs > line->left_msecs)
 			pad_secs = line->left_msecs;
 
-		ret = pblk_recov_pad_oob(pblk, line, p, pad_secs);
+		ret = pblk_recov_pad_oob(pblk, line, pad_secs);
 		if (ret)
 			pr_err("pblk: OOB padding failed (err:%d)\n", ret);
 
@@ -552,7 +626,6 @@ next_rq:
 		if (ret)
 			pr_err("pblk: OOB read failed (err:%d)\n", ret);
 
-		line->left_ssecs = line->left_msecs;
 		left_ppas = 0;
 	}
 
@@ -591,7 +664,7 @@ static int pblk_recov_scan_oob(struct pblk *pblk, struct pblk_line *line,
 	*done = 1;
 
 next_rq:
-	memset(rqd, 0, pblk_r_rq_size);
+	memset(rqd, 0, pblk_g_rq_size);
 
 	rq_ppas = pblk_calc_secs(pblk, left_ppas, 0);
 	if (!rq_ppas)
@@ -607,7 +680,6 @@ next_rq:
 
 	rqd->bio = bio;
 	rqd->opcode = NVM_OP_PREAD;
-	rqd->flags = pblk_set_read_mode(pblk);
 	rqd->meta_list = meta_list;
 	rqd->nr_ppas = rq_ppas;
 	rqd->ppa_list = ppa_list;
@@ -616,6 +688,11 @@ next_rq:
 	rqd->end_io = pblk_end_io_sync;
 	rqd->private = &wait;
 
+	if (pblk_io_aligned(pblk, rq_ppas))
+		rqd->flags = pblk_set_read_mode(pblk, PBLK_READ_SEQUENTIAL);
+	else
+		rqd->flags = pblk_set_read_mode(pblk, PBLK_READ_RANDOM);
+
 	for (i = 0; i < rqd->nr_ppas; ) {
 		struct ppa_addr ppa;
 		int pos;
@@ -646,6 +723,7 @@ next_rq:
 				msecs_to_jiffies(PBLK_COMMAND_TIMEOUT_MS))) {
 		pr_err("pblk: L2P recovery read timed out\n");
 	}
+	atomic_dec(&pblk->inflight_io);
 	reinit_completion(&wait);
 
 	/* Reached the end of the written line */
@@ -658,7 +736,6 @@ next_rq:
 		/* Roll back failed sectors */
 		line->cur_sec -= nr_error_bits;
 		line->left_msecs += nr_error_bits;
-		line->left_ssecs = line->left_msecs;
 		bitmap_clear(line->map_bitmap, line->cur_sec, nr_error_bits);
 
 		left_ppas = 0;
@@ -770,8 +847,9 @@ struct pblk_line *pblk_recov_l2p(struct pblk *pblk)
 	struct pblk_line_meta *lm = &pblk->lm;
 	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
 	struct pblk_line *line, *tline, *data_line = NULL;
-	struct line_smeta *smeta;
-	struct line_emeta *emeta;
+	struct pblk_smeta *smeta;
+	struct pblk_emeta *emeta;
+	struct line_smeta *smeta_buf;
 	int found_lines = 0, recovered_lines = 0, open_lines = 0;
 	int is_next = 0;
 	int meta_line;
@@ -784,8 +862,9 @@ struct pblk_line *pblk_recov_l2p(struct pblk *pblk)
 	spin_lock(&l_mg->free_lock);
 	meta_line = find_first_zero_bit(&l_mg->meta_bitmap, PBLK_DATA_LINES);
 	set_bit(meta_line, &l_mg->meta_bitmap);
-	smeta = l_mg->sline_meta[meta_line].meta;
-	emeta = l_mg->eline_meta[meta_line].meta;
+	smeta = l_mg->sline_meta[meta_line];
+	emeta = l_mg->eline_meta[meta_line];
+	smeta_buf = (struct line_smeta *)smeta;
 	spin_unlock(&l_mg->free_lock);
 
 	/* Order data lines using their sequence number */
@@ -796,33 +875,33 @@ struct pblk_line *pblk_recov_l2p(struct pblk *pblk)
 
 		memset(smeta, 0, lm->smeta_len);
 		line->smeta = smeta;
-		line->lun_bitmap = ((void *)(smeta)) +
+		line->lun_bitmap = ((void *)(smeta_buf)) +
 						sizeof(struct line_smeta);
 
 		/* Lines that cannot be read are assumed as not written here */
 		if (pblk_line_read_smeta(pblk, line))
 			continue;
 
-		crc = pblk_calc_smeta_crc(pblk, smeta);
-		if (le32_to_cpu(smeta->crc) != crc)
+		crc = pblk_calc_smeta_crc(pblk, smeta_buf);
+		if (le32_to_cpu(smeta_buf->crc) != crc)
 			continue;
 
-		if (le32_to_cpu(smeta->header.identifier) != PBLK_MAGIC)
+		if (le32_to_cpu(smeta_buf->header.identifier) != PBLK_MAGIC)
 			continue;
 
-		if (le16_to_cpu(smeta->header.version) != 1) {
+		if (le16_to_cpu(smeta_buf->header.version) != 1) {
 			pr_err("pblk: found incompatible line version %u\n",
-					smeta->header.version);
+					smeta_buf->header.version);
 			return ERR_PTR(-EINVAL);
 		}
 
 		/* The first valid instance uuid is used for initialization */
 		if (!valid_uuid) {
-			memcpy(pblk->instance_uuid, smeta->header.uuid, 16);
+			memcpy(pblk->instance_uuid, smeta_buf->header.uuid, 16);
 			valid_uuid = 1;
 		}
 
-		if (memcmp(pblk->instance_uuid, smeta->header.uuid, 16)) {
+		if (memcmp(pblk->instance_uuid, smeta_buf->header.uuid, 16)) {
 			pr_debug("pblk: ignore line %u due to uuid mismatch\n",
 					i);
 			continue;
@@ -830,9 +909,9 @@ struct pblk_line *pblk_recov_l2p(struct pblk *pblk)
 
 		/* Update line metadata */
 		spin_lock(&line->lock);
-		line->id = le32_to_cpu(line->smeta->header.id);
-		line->type = le16_to_cpu(line->smeta->header.type);
-		line->seq_nr = le64_to_cpu(line->smeta->seq_nr);
+		line->id = le32_to_cpu(smeta_buf->header.id);
+		line->type = le16_to_cpu(smeta_buf->header.type);
+		line->seq_nr = le64_to_cpu(smeta_buf->seq_nr);
 		spin_unlock(&line->lock);
 
 		/* Update general metadata */
@@ -848,7 +927,7 @@ struct pblk_line *pblk_recov_l2p(struct pblk *pblk)
 		pblk_recov_line_add_ordered(&recov_list, line);
 		found_lines++;
 		pr_debug("pblk: recovering data line %d, seq:%llu\n",
-						line->id, smeta->seq_nr);
+						line->id, smeta_buf->seq_nr);
 	}
 
 	if (!found_lines) {
@@ -868,15 +947,15 @@ struct pblk_line *pblk_recov_l2p(struct pblk *pblk)
 
 		recovered_lines++;
 		/* Calculate where emeta starts based on the line bb */
-		off = lm->sec_per_line - lm->emeta_sec;
+		off = lm->sec_per_line - lm->emeta_sec[0];
 		nr_bb = bitmap_weight(line->blk_bitmap, lm->blk_per_line);
 		off -= nr_bb * geo->sec_per_pl;
 
-		memset(emeta, 0, lm->emeta_len);
-		line->emeta = emeta;
 		line->emeta_ssec = off;
+		line->emeta = emeta;
+		memset(line->emeta->buf, 0, lm->emeta_len[0]);
 
-		if (pblk_line_read_emeta(pblk, line)) {
+		if (pblk_line_read_emeta(pblk, line, line->emeta->buf)) {
 			pblk_recov_l2p_from_oob(pblk, line);
 			goto next;
 		}
@@ -941,58 +1020,26 @@ out:
 }
 
 /*
- * Pad until smeta can be read on current data line
+ * Pad current line
  */
-void pblk_recov_pad(struct pblk *pblk)
+int pblk_recov_pad(struct pblk *pblk)
 {
-	struct nvm_tgt_dev *dev = pblk->dev;
-	struct nvm_geo *geo = &dev->geo;
 	struct pblk_line *line;
 	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
-	struct nvm_rq *rqd;
-	struct pblk_recov_alloc p;
-	struct ppa_addr *ppa_list;
-	struct pblk_sec_meta *meta_list;
-	void *data;
-	dma_addr_t dma_ppa_list, dma_meta_list;
+	int left_msecs;
+	int ret = 0;
 
 	spin_lock(&l_mg->free_lock);
 	line = l_mg->data_line;
+	left_msecs = line->left_msecs;
 	spin_unlock(&l_mg->free_lock);
 
-	rqd = pblk_alloc_rqd(pblk, READ);
-	if (IS_ERR(rqd))
-		return;
-
-	meta_list = nvm_dev_dma_alloc(dev->parent, GFP_KERNEL, &dma_meta_list);
-	if (!meta_list)
-		goto free_rqd;
-
-	ppa_list = (void *)(meta_list) + pblk_dma_meta_size;
-	dma_ppa_list = dma_meta_list + pblk_dma_meta_size;
-
-	data = kcalloc(pblk->max_write_pgs, geo->sec_size, GFP_KERNEL);
-	if (!data)
-		goto free_meta_list;
-
-	p.ppa_list = ppa_list;
-	p.meta_list = meta_list;
-	p.rqd = rqd;
-	p.data = data;
-	p.dma_ppa_list = dma_ppa_list;
-	p.dma_meta_list = dma_meta_list;
-
-	if (pblk_recov_pad_oob(pblk, line, p, line->left_msecs)) {
-		pr_err("pblk: Tear down padding failed\n");
-		goto free_data;
+	ret = pblk_recov_pad_oob(pblk, line, left_msecs);
+	if (ret) {
+		pr_err("pblk: Tear down padding failed (%d)\n", ret);
+		return ret;
 	}
 
-	pblk_line_close(pblk, line);
-
-free_data:
-	kfree(data);
-free_meta_list:
-	nvm_dev_dma_free(dev->parent, meta_list, dma_meta_list);
-free_rqd:
-	pblk_free_rqd(pblk, rqd, READ);
+	pblk_line_close_meta(pblk, line);
+	return ret;
 }
diff --git a/drivers/lightnvm/pblk-rl.c b/drivers/lightnvm/pblk-rl.c
index ab7cbb144f3f..2e6a5361baf0 100644
--- a/drivers/lightnvm/pblk-rl.c
+++ b/drivers/lightnvm/pblk-rl.c
@@ -23,11 +23,35 @@ static void pblk_rl_kick_u_timer(struct pblk_rl *rl)
 	mod_timer(&rl->u_timer, jiffies + msecs_to_jiffies(5000));
 }
 
+int pblk_rl_is_limit(struct pblk_rl *rl)
+{
+	int rb_space;
+
+	rb_space = atomic_read(&rl->rb_space);
+
+	return (rb_space == 0);
+}
+
 int pblk_rl_user_may_insert(struct pblk_rl *rl, int nr_entries)
 {
 	int rb_user_cnt = atomic_read(&rl->rb_user_cnt);
+	int rb_space = atomic_read(&rl->rb_space);
 
-	return (!(rb_user_cnt + nr_entries > rl->rb_user_max));
+	if (unlikely(rb_space >= 0) && (rb_space - nr_entries < 0))
+		return NVM_IO_ERR;
+
+	if (rb_user_cnt >= rl->rb_user_max)
+		return NVM_IO_REQUEUE;
+
+	return NVM_IO_OK;
+}
+
+void pblk_rl_inserted(struct pblk_rl *rl, int nr_entries)
+{
+	int rb_space = atomic_read(&rl->rb_space);
+
+	if (unlikely(rb_space >= 0))
+		atomic_sub(nr_entries, &rl->rb_space);
 }
 
 int pblk_rl_gc_may_insert(struct pblk_rl *rl, int nr_entries)
@@ -37,7 +61,7 @@ int pblk_rl_gc_may_insert(struct pblk_rl *rl, int nr_entries)
 
 	/* If there is no user I/O let GC take over space on the write buffer */
 	rb_user_active = READ_ONCE(rl->rb_user_active);
-	return (!(rb_gc_cnt + nr_entries > rl->rb_gc_max && rb_user_active));
+	return (!(rb_gc_cnt >= rl->rb_gc_max && rb_user_active));
 }
 
 void pblk_rl_user_in(struct pblk_rl *rl, int nr_entries)
@@ -77,33 +101,32 @@ static int pblk_rl_update_rates(struct pblk_rl *rl, unsigned long max)
 	unsigned long free_blocks = pblk_rl_nr_free_blks(rl);
 
 	if (free_blocks >= rl->high) {
-		rl->rb_user_max = max - rl->rb_gc_rsv;
-		rl->rb_gc_max = rl->rb_gc_rsv;
+		rl->rb_user_max = max;
+		rl->rb_gc_max = 0;
 		rl->rb_state = PBLK_RL_HIGH;
 	} else if (free_blocks < rl->high) {
 		int shift = rl->high_pw - rl->rb_windows_pw;
 		int user_windows = free_blocks >> shift;
 		int user_max = user_windows << PBLK_MAX_REQ_ADDRS_PW;
-		int gc_max;
 
 		rl->rb_user_max = user_max;
-		gc_max = max - rl->rb_user_max;
-		rl->rb_gc_max = max(gc_max, rl->rb_gc_rsv);
-
-		if (free_blocks > rl->low)
-			rl->rb_state = PBLK_RL_MID;
-		else
-			rl->rb_state = PBLK_RL_LOW;
+		rl->rb_gc_max = max - user_max;
+
+		if (free_blocks <= rl->rsv_blocks) {
+			rl->rb_user_max = 0;
+			rl->rb_gc_max = max;
+		}
+
+		/* In the worst case, we will need to GC lines in the low list
+		 * (high valid sector count). If there are lines to GC on high
+		 * or mid lists, these will be prioritized
+		 */
+		rl->rb_state = PBLK_RL_LOW;
 	}
 
 	return rl->rb_state;
 }
 
-void pblk_rl_set_gc_rsc(struct pblk_rl *rl, int rsv)
-{
-	rl->rb_gc_rsv = rl->rb_gc_max = rsv;
-}
-
 void pblk_rl_free_lines_inc(struct pblk_rl *rl, struct pblk_line *line)
 {
 	struct pblk *pblk = container_of(rl, struct pblk, rl);
@@ -122,11 +145,15 @@ void pblk_rl_free_lines_inc(struct pblk_rl *rl, struct pblk_line *line)
 
 void pblk_rl_free_lines_dec(struct pblk_rl *rl, struct pblk_line *line)
 {
-	struct pblk *pblk = container_of(rl, struct pblk, rl);
 	int blk_in_line = atomic_read(&line->blk_in_line);
-	int ret;
 
 	atomic_sub(blk_in_line, &rl->free_blocks);
+}
+
+void pblk_gc_should_kick(struct pblk *pblk)
+{
+	struct pblk_rl *rl = &pblk->rl;
+	int ret;
 
 	/* Rates will not change that often - no need to lock update */
 	ret = pblk_rl_update_rates(rl, rl->rb_budget);
@@ -136,11 +163,16 @@ void pblk_rl_free_lines_dec(struct pblk_rl *rl, struct pblk_line *line)
 		pblk_gc_should_stop(pblk);
 }
 
-int pblk_rl_gc_thrs(struct pblk_rl *rl)
+int pblk_rl_high_thrs(struct pblk_rl *rl)
 {
 	return rl->high;
 }
 
+int pblk_rl_low_thrs(struct pblk_rl *rl)
+{
+	return rl->low;
+}
+
 int pblk_rl_sysfs_rate_show(struct pblk_rl *rl)
 {
 	return rl->rb_user_max;
@@ -161,24 +193,36 @@ void pblk_rl_free(struct pblk_rl *rl)
 
 void pblk_rl_init(struct pblk_rl *rl, int budget)
 {
+	struct pblk *pblk = container_of(rl, struct pblk, rl);
+	struct pblk_line_meta *lm = &pblk->lm;
+	int min_blocks = lm->blk_per_line * PBLK_GC_RSV_LINE;
 	unsigned int rb_windows;
 
 	rl->high = rl->total_blocks / PBLK_USER_HIGH_THRS;
-	rl->low = rl->total_blocks / PBLK_USER_LOW_THRS;
 	rl->high_pw = get_count_order(rl->high);
 
+	rl->low = rl->total_blocks / PBLK_USER_LOW_THRS;
+	if (rl->low < min_blocks)
+		rl->low = min_blocks;
+
+	rl->rsv_blocks = min_blocks;
+
 	/* This will always be a power-of-2 */
 	rb_windows = budget / PBLK_MAX_REQ_ADDRS;
-	rl->rb_windows_pw = get_count_order(rb_windows) + 1;
+	rl->rb_windows_pw = get_count_order(rb_windows);
 
 	/* To start with, all buffer is available to user I/O writers */
 	rl->rb_budget = budget;
 	rl->rb_user_max = budget;
-	atomic_set(&rl->rb_user_cnt, 0);
 	rl->rb_gc_max = 0;
 	rl->rb_state = PBLK_RL_HIGH;
+
+	atomic_set(&rl->rb_user_cnt, 0);
 	atomic_set(&rl->rb_gc_cnt, 0);
+	atomic_set(&rl->rb_space, -1);
 
 	setup_timer(&rl->u_timer, pblk_rl_u_timer, (unsigned long)rl);
+
 	rl->rb_user_active = 0;
+	rl->rb_gc_active = 0;
 }
diff --git a/drivers/lightnvm/pblk-sysfs.c b/drivers/lightnvm/pblk-sysfs.c
index f0af1d1ceeff..95fb434e2f01 100644
--- a/drivers/lightnvm/pblk-sysfs.c
+++ b/drivers/lightnvm/pblk-sysfs.c
@@ -49,30 +49,26 @@ static ssize_t pblk_sysfs_luns_show(struct pblk *pblk, char *page)
 
 static ssize_t pblk_sysfs_rate_limiter(struct pblk *pblk, char *page)
 {
-	struct nvm_tgt_dev *dev = pblk->dev;
-	struct nvm_geo *geo = &dev->geo;
 	int free_blocks, total_blocks;
 	int rb_user_max, rb_user_cnt;
-	int rb_gc_max, rb_gc_rsv, rb_gc_cnt, rb_budget, rb_state;
+	int rb_gc_max, rb_gc_cnt, rb_budget, rb_state;
 
 	free_blocks = atomic_read(&pblk->rl.free_blocks);
 	rb_user_max = pblk->rl.rb_user_max;
 	rb_user_cnt = atomic_read(&pblk->rl.rb_user_cnt);
 	rb_gc_max = pblk->rl.rb_gc_max;
-	rb_gc_rsv = pblk->rl.rb_gc_rsv;
 	rb_gc_cnt = atomic_read(&pblk->rl.rb_gc_cnt);
 	rb_budget = pblk->rl.rb_budget;
 	rb_state = pblk->rl.rb_state;
 
-	total_blocks = geo->blks_per_lun * geo->nr_luns;
+	total_blocks = pblk->rl.total_blocks;
 
 	return snprintf(page, PAGE_SIZE,
-		"u:%u/%u,gc:%u/%u/%u(%u/%u)(stop:<%u,full:>%u,free:%d/%d)-%d\n",
+		"u:%u/%u,gc:%u/%u(%u/%u)(stop:<%u,full:>%u,free:%d/%d)-%d\n",
 				rb_user_cnt,
 				rb_user_max,
 				rb_gc_cnt,
 				rb_gc_max,
-				rb_gc_rsv,
 				rb_state,
 				rb_budget,
 				pblk->rl.low,
@@ -150,11 +146,11 @@ static ssize_t pblk_sysfs_lines(struct pblk *pblk, char *page)
 	ssize_t sz = 0;
 	int nr_free_lines;
 	int cur_data, cur_log;
-	int free_line_cnt = 0, closed_line_cnt = 0;
+	int free_line_cnt = 0, closed_line_cnt = 0, emeta_line_cnt = 0;
 	int d_line_cnt = 0, l_line_cnt = 0;
 	int gc_full = 0, gc_high = 0, gc_mid = 0, gc_low = 0, gc_empty = 0;
-	int free = 0, bad = 0, cor = 0;
-	int msecs = 0, ssecs = 0, cur_sec = 0, vsc = 0, sec_in_line = 0;
+	int bad = 0, cor = 0;
+	int msecs = 0, cur_sec = 0, vsc = 0, sec_in_line = 0;
 	int map_weight = 0, meta_weight = 0;
 
 	spin_lock(&l_mg->free_lock);
@@ -166,6 +162,11 @@ static ssize_t pblk_sysfs_lines(struct pblk *pblk, char *page)
 		free_line_cnt++;
 	spin_unlock(&l_mg->free_lock);
 
+	spin_lock(&l_mg->close_lock);
+	list_for_each_entry(line, &l_mg->emeta_list, list)
+		emeta_line_cnt++;
+	spin_unlock(&l_mg->close_lock);
+
 	spin_lock(&l_mg->gc_lock);
 	list_for_each_entry(line, &l_mg->gc_full_list, list) {
 		if (line->type == PBLK_LINETYPE_DATA)
@@ -212,8 +213,6 @@ static ssize_t pblk_sysfs_lines(struct pblk *pblk, char *page)
 		gc_empty++;
 	}
 
-	list_for_each_entry(line, &l_mg->free_list, list)
-		free++;
 	list_for_each_entry(line, &l_mg->bad_list, list)
 		bad++;
 	list_for_each_entry(line, &l_mg->corrupt_list, list)
@@ -224,8 +223,7 @@ static ssize_t pblk_sysfs_lines(struct pblk *pblk, char *page)
 	if (l_mg->data_line) {
 		cur_sec = l_mg->data_line->cur_sec;
 		msecs = l_mg->data_line->left_msecs;
-		ssecs = l_mg->data_line->left_ssecs;
-		vsc = l_mg->data_line->vsc;
+		vsc = le32_to_cpu(*l_mg->data_line->vsc);
 		sec_in_line = l_mg->data_line->sec_in_line;
 		meta_weight = bitmap_weight(&l_mg->meta_bitmap,
 							PBLK_DATA_LINES);
@@ -235,17 +233,20 @@ static ssize_t pblk_sysfs_lines(struct pblk *pblk, char *page)
 	spin_unlock(&l_mg->free_lock);
 
 	if (nr_free_lines != free_line_cnt)
-		pr_err("pblk: corrupted free line list\n");
+		pr_err("pblk: corrupted free line list:%d/%d\n",
+						nr_free_lines, free_line_cnt);
 
 	sz = snprintf(page, PAGE_SIZE - sz,
 		"line: nluns:%d, nblks:%d, nsecs:%d\n",
 		geo->nr_luns, lm->blk_per_line, lm->sec_per_line);
 
 	sz += snprintf(page + sz, PAGE_SIZE - sz,
-		"lines:d:%d,l:%d-f:%d(%d),b:%d,co:%d,c:%d(d:%d,l:%d)t:%d\n",
+		"lines:d:%d,l:%d-f:%d,m:%d/%d,c:%d,b:%d,co:%d(d:%d,l:%d)t:%d\n",
 					cur_data, cur_log,
-					free, nr_free_lines, bad, cor,
+					nr_free_lines,
+					emeta_line_cnt, meta_weight,
 					closed_line_cnt,
+					bad, cor,
 					d_line_cnt, l_line_cnt,
 					l_mg->nr_lines);
 
@@ -255,9 +256,10 @@ static ssize_t pblk_sysfs_lines(struct pblk *pblk, char *page)
 			atomic_read(&pblk->gc.inflight_gc));
 
 	sz += snprintf(page + sz, PAGE_SIZE - sz,
-		"data (%d) cur:%d, left:%d/%d, vsc:%d, s:%d, map:%d/%d (%d)\n",
-			cur_data, cur_sec, msecs, ssecs, vsc, sec_in_line,
-			map_weight, lm->sec_per_line, meta_weight);
+		"data (%d) cur:%d, left:%d, vsc:%d, s:%d, map:%d/%d (%d)\n",
+			cur_data, cur_sec, msecs, vsc, sec_in_line,
+			map_weight, lm->sec_per_line,
+			atomic_read(&pblk->inflight_io));
 
 	return sz;
 }
@@ -274,7 +276,7 @@ static ssize_t pblk_sysfs_lines_info(struct pblk *pblk, char *page)
 					lm->smeta_len, lm->smeta_sec);
 	sz += snprintf(page + sz, PAGE_SIZE - sz,
 				"emeta - len:%d, sec:%d, bb_start:%d\n",
-					lm->emeta_len, lm->emeta_sec,
+					lm->emeta_len[0], lm->emeta_sec[0],
 					lm->emeta_bb);
 	sz += snprintf(page + sz, PAGE_SIZE - sz,
 				"bitmap lengths: sec:%d, blk:%d, lun:%d\n",
@@ -290,6 +292,11 @@ static ssize_t pblk_sysfs_lines_info(struct pblk *pblk, char *page)
 	return sz;
 }
 
+static ssize_t pblk_sysfs_get_sec_per_write(struct pblk *pblk, char *page)
+{
+	return snprintf(page, PAGE_SIZE, "%d\n", pblk->sec_per_write);
+}
+
 #ifdef CONFIG_NVM_DEBUG
 static ssize_t pblk_sysfs_stats_debug(struct pblk *pblk, char *page)
 {
@@ -303,52 +310,51 @@ static ssize_t pblk_sysfs_stats_debug(struct pblk *pblk, char *page)
 			atomic_long_read(&pblk->padded_wb),
 			atomic_long_read(&pblk->sub_writes),
 			atomic_long_read(&pblk->sync_writes),
-			atomic_long_read(&pblk->compl_writes),
 			atomic_long_read(&pblk->recov_writes),
 			atomic_long_read(&pblk->recov_gc_writes),
 			atomic_long_read(&pblk->recov_gc_reads),
+			atomic_long_read(&pblk->cache_reads),
 			atomic_long_read(&pblk->sync_reads));
 }
 #endif
 
-static ssize_t pblk_sysfs_rate_store(struct pblk *pblk, const char *page,
-				     size_t len)
+static ssize_t pblk_sysfs_gc_force(struct pblk *pblk, const char *page,
+				   size_t len)
 {
-	struct pblk_gc *gc = &pblk->gc;
 	size_t c_len;
-	int value;
+	int force;
 
 	c_len = strcspn(page, "\n");
 	if (c_len >= len)
 		return -EINVAL;
 
-	if (kstrtouint(page, 0, &value))
+	if (kstrtouint(page, 0, &force))
 		return -EINVAL;
 
-	spin_lock(&gc->lock);
-	pblk_rl_set_gc_rsc(&pblk->rl, value);
-	spin_unlock(&gc->lock);
+	pblk_gc_sysfs_force(pblk, force);
 
 	return len;
 }
 
-static ssize_t pblk_sysfs_gc_force(struct pblk *pblk, const char *page,
-				   size_t len)
+static ssize_t pblk_sysfs_set_sec_per_write(struct pblk *pblk,
+					     const char *page, size_t len)
 {
 	size_t c_len;
-	int force;
+	int sec_per_write;
 
 	c_len = strcspn(page, "\n");
 	if (c_len >= len)
 		return -EINVAL;
 
-	if (kstrtouint(page, 0, &force))
+	if (kstrtouint(page, 0, &sec_per_write))
 		return -EINVAL;
 
-	if (force < 0 || force > 1)
+	if (sec_per_write < pblk->min_write_pgs
+				|| sec_per_write > pblk->max_write_pgs
+				|| sec_per_write % pblk->min_write_pgs != 0)
 		return -EINVAL;
 
-	pblk_gc_sysfs_force(pblk, force);
+	pblk_set_sec_per_write(pblk, sec_per_write);
 
 	return len;
 }
@@ -398,9 +404,9 @@ static struct attribute sys_gc_force = {
 	.mode = 0200,
 };
 
-static struct attribute sys_gc_rl_max = {
-	.name = "gc_rl_max",
-	.mode = 0200,
+static struct attribute sys_max_sec_per_write = {
+	.name = "max_sec_per_write",
+	.mode = 0644,
 };
 
 #ifdef CONFIG_NVM_DEBUG
@@ -416,7 +422,7 @@ static struct attribute *pblk_attrs[] = {
 	&sys_errors_attr,
 	&sys_gc_state,
 	&sys_gc_force,
-	&sys_gc_rl_max,
+	&sys_max_sec_per_write,
 	&sys_rb_attr,
 	&sys_stats_ppaf_attr,
 	&sys_lines_attr,
@@ -448,6 +454,8 @@ static ssize_t pblk_sysfs_show(struct kobject *kobj, struct attribute *attr,
 		return pblk_sysfs_lines(pblk, buf);
 	else if (strcmp(attr->name, "lines_info") == 0)
 		return pblk_sysfs_lines_info(pblk, buf);
+	else if (strcmp(attr->name, "max_sec_per_write") == 0)
+		return pblk_sysfs_get_sec_per_write(pblk, buf);
 #ifdef CONFIG_NVM_DEBUG
 	else if (strcmp(attr->name, "stats") == 0)
 		return pblk_sysfs_stats_debug(pblk, buf);
@@ -460,10 +468,10 @@ static ssize_t pblk_sysfs_store(struct kobject *kobj, struct attribute *attr,
 {
 	struct pblk *pblk = container_of(kobj, struct pblk, kobj);
 
-	if (strcmp(attr->name, "gc_rl_max") == 0)
-		return pblk_sysfs_rate_store(pblk, buf, len);
-	else if (strcmp(attr->name, "gc_force") == 0)
+	if (strcmp(attr->name, "gc_force") == 0)
 		return pblk_sysfs_gc_force(pblk, buf, len);
+	else if (strcmp(attr->name, "max_sec_per_write") == 0)
+		return pblk_sysfs_set_sec_per_write(pblk, buf, len);
 
 	return 0;
 }
diff --git a/drivers/lightnvm/pblk-write.c b/drivers/lightnvm/pblk-write.c
index aef6fd7c4a0c..d62a8f4faaf4 100644
--- a/drivers/lightnvm/pblk-write.c
+++ b/drivers/lightnvm/pblk-write.c
@@ -17,18 +17,6 @@
 
 #include "pblk.h"
 
-static void pblk_sync_line(struct pblk *pblk, struct pblk_line *line)
-{
-#ifdef CONFIG_NVM_DEBUG
-	atomic_long_inc(&pblk->sync_writes);
-#endif
-
-	/* Counter protected by rb sync lock */
-	line->left_ssecs--;
-	if (!line->left_ssecs)
-		pblk_line_run_ws(pblk, line, NULL, pblk_line_close_ws);
-}
-
 static unsigned long pblk_end_w_bio(struct pblk *pblk, struct nvm_rq *rqd,
 				    struct pblk_c_ctx *c_ctx)
 {
@@ -39,21 +27,14 @@ static unsigned long pblk_end_w_bio(struct pblk *pblk, struct nvm_rq *rqd,
 
 	for (i = 0; i < c_ctx->nr_valid; i++) {
 		struct pblk_w_ctx *w_ctx;
-		struct ppa_addr p;
-		struct pblk_line *line;
 
 		w_ctx = pblk_rb_w_ctx(&pblk->rwb, c_ctx->sentry + i);
-
-		p = rqd->ppa_list[i];
-		line = &pblk->lines[pblk_dev_ppa_to_line(p)];
-		pblk_sync_line(pblk, line);
-
 		while ((original_bio = bio_list_pop(&w_ctx->bios)))
 			bio_endio(original_bio);
 	}
 
 #ifdef CONFIG_NVM_DEBUG
-	atomic_long_add(c_ctx->nr_valid, &pblk->compl_writes);
+	atomic_long_add(c_ctx->nr_valid, &pblk->sync_writes);
 #endif
 
 	ret = pblk_rb_sync_advance(&pblk->rwb, c_ctx->nr_valid);
@@ -169,7 +150,7 @@ static void pblk_end_w_fail(struct pblk *pblk, struct nvm_rq *rqd)
 	}
 
 	INIT_WORK(&recovery->ws_rec, pblk_submit_rec);
-	queue_work(pblk->kw_wq, &recovery->ws_rec);
+	queue_work(pblk->close_wq, &recovery->ws_rec);
 
 out:
 	pblk_complete_write(pblk, rqd, c_ctx);
@@ -186,14 +167,50 @@ static void pblk_end_io_write(struct nvm_rq *rqd)
 	}
 #ifdef CONFIG_NVM_DEBUG
 	else
-		WARN_ONCE(rqd->bio->bi_error, "pblk: corrupted write error\n");
+		WARN_ONCE(rqd->bio->bi_status, "pblk: corrupted write error\n");
 #endif
 
 	pblk_complete_write(pblk, rqd, c_ctx);
+	atomic_dec(&pblk->inflight_io);
+}
+
+static void pblk_end_io_write_meta(struct nvm_rq *rqd)
+{
+	struct pblk *pblk = rqd->private;
+	struct nvm_tgt_dev *dev = pblk->dev;
+	struct nvm_geo *geo = &dev->geo;
+	struct pblk_g_ctx *m_ctx = nvm_rq_to_pdu(rqd);
+	struct pblk_line *line = m_ctx->private;
+	struct pblk_emeta *emeta = line->emeta;
+	int pos = pblk_ppa_to_pos(geo, rqd->ppa_list[0]);
+	struct pblk_lun *rlun = &pblk->luns[pos];
+	int sync;
+
+	up(&rlun->wr_sem);
+
+	if (rqd->error) {
+		pblk_log_write_err(pblk, rqd);
+		pr_err("pblk: metadata I/O failed. Line %d\n", line->id);
+	}
+#ifdef CONFIG_NVM_DEBUG
+	else
+		WARN_ONCE(rqd->bio->bi_status, "pblk: corrupted write error\n");
+#endif
+
+	sync = atomic_add_return(rqd->nr_ppas, &emeta->sync);
+	if (sync == emeta->nr_entries)
+		pblk_line_run_ws(pblk, line, NULL, pblk_line_close_ws,
+								pblk->close_wq);
+
+	bio_put(rqd->bio);
+	pblk_free_rqd(pblk, rqd, READ);
+
+	atomic_dec(&pblk->inflight_io);
 }
 
 static int pblk_alloc_w_rq(struct pblk *pblk, struct nvm_rq *rqd,
-			   unsigned int nr_secs)
+			   unsigned int nr_secs,
+			   nvm_end_io_fn(*end_io))
 {
 	struct nvm_tgt_dev *dev = pblk->dev;
 
@@ -202,7 +219,7 @@ static int pblk_alloc_w_rq(struct pblk *pblk, struct nvm_rq *rqd,
 	rqd->nr_ppas = nr_secs;
 	rqd->flags = pblk_set_progr_mode(pblk, WRITE);
 	rqd->private = pblk;
-	rqd->end_io = pblk_end_io_write;
+	rqd->end_io = end_io;
 
 	rqd->meta_list = nvm_dev_dma_alloc(dev->parent, GFP_KERNEL,
 							&rqd->dma_meta_list);
@@ -219,11 +236,10 @@ static int pblk_alloc_w_rq(struct pblk *pblk, struct nvm_rq *rqd,
 }
 
 static int pblk_setup_w_rq(struct pblk *pblk, struct nvm_rq *rqd,
-			   struct pblk_c_ctx *c_ctx)
+			   struct pblk_c_ctx *c_ctx, struct ppa_addr *erase_ppa)
 {
 	struct pblk_line_meta *lm = &pblk->lm;
-	struct pblk_line *e_line = pblk_line_get_data_next(pblk);
-	struct ppa_addr erase_ppa;
+	struct pblk_line *e_line = pblk_line_get_erase(pblk);
 	unsigned int valid = c_ctx->nr_valid;
 	unsigned int padded = c_ctx->nr_padded;
 	unsigned int nr_secs = valid + padded;
@@ -231,40 +247,23 @@ static int pblk_setup_w_rq(struct pblk *pblk, struct nvm_rq *rqd,
 	int ret = 0;
 
 	lun_bitmap = kzalloc(lm->lun_bitmap_len, GFP_KERNEL);
-	if (!lun_bitmap) {
-		ret = -ENOMEM;
-		goto out;
-	}
+	if (!lun_bitmap)
+		return -ENOMEM;
 	c_ctx->lun_bitmap = lun_bitmap;
 
-	ret = pblk_alloc_w_rq(pblk, rqd, nr_secs);
+	ret = pblk_alloc_w_rq(pblk, rqd, nr_secs, pblk_end_io_write);
 	if (ret) {
 		kfree(lun_bitmap);
-		goto out;
+		return ret;
 	}
 
-	ppa_set_empty(&erase_ppa);
 	if (likely(!e_line || !atomic_read(&e_line->left_eblks)))
 		pblk_map_rq(pblk, rqd, c_ctx->sentry, lun_bitmap, valid, 0);
 	else
 		pblk_map_erase_rq(pblk, rqd, c_ctx->sentry, lun_bitmap,
-							valid, &erase_ppa);
-
-out:
-	if (unlikely(e_line && !ppa_empty(erase_ppa))) {
-		if (pblk_blk_erase_async(pblk, erase_ppa)) {
-			struct nvm_tgt_dev *dev = pblk->dev;
-			struct nvm_geo *geo = &dev->geo;
-			int bit;
-
-			atomic_inc(&e_line->left_eblks);
-			bit = erase_ppa.g.lun * geo->nr_chnls + erase_ppa.g.ch;
-			WARN_ON(!test_and_clear_bit(bit, e_line->erase_bitmap));
-			up(&pblk->erase_sem);
-		}
-	}
+							valid, erase_ppa);
 
-	return ret;
+	return 0;
 }
 
 int pblk_setup_w_rec_rq(struct pblk *pblk, struct nvm_rq *rqd,
@@ -280,7 +279,7 @@ int pblk_setup_w_rec_rq(struct pblk *pblk, struct nvm_rq *rqd,
 
 	c_ctx->lun_bitmap = lun_bitmap;
 
-	ret = pblk_alloc_w_rq(pblk, rqd, rqd->nr_ppas);
+	ret = pblk_alloc_w_rq(pblk, rqd, rqd->nr_ppas, pblk_end_io_write);
 	if (ret)
 		return ret;
 
@@ -311,16 +310,237 @@ static int pblk_calc_secs_to_sync(struct pblk *pblk, unsigned int secs_avail,
 	return secs_to_sync;
 }
 
+static inline int pblk_valid_meta_ppa(struct pblk *pblk,
+				      struct pblk_line *meta_line,
+				      struct ppa_addr *ppa_list, int nr_ppas)
+{
+	struct nvm_tgt_dev *dev = pblk->dev;
+	struct nvm_geo *geo = &dev->geo;
+	struct pblk_line *data_line;
+	struct ppa_addr ppa, ppa_opt;
+	u64 paddr;
+	int i;
+
+	data_line = &pblk->lines[pblk_dev_ppa_to_line(ppa_list[0])];
+	paddr = pblk_lookup_page(pblk, meta_line);
+	ppa = addr_to_gen_ppa(pblk, paddr, 0);
+
+	if (test_bit(pblk_ppa_to_pos(geo, ppa), data_line->blk_bitmap))
+		return 1;
+
+	/* Schedule a metadata I/O that is half the distance from the data I/O
+	 * with regards to the number of LUNs forming the pblk instance. This
+	 * balances LUN conflicts across every I/O.
+	 *
+	 * When the LUN configuration changes (e.g., due to GC), this distance
+	 * can align, which would result on a LUN deadlock. In this case, modify
+	 * the distance to not be optimal, but allow metadata I/Os to succeed.
+	 */
+	ppa_opt = addr_to_gen_ppa(pblk, paddr + data_line->meta_distance, 0);
+	if (unlikely(ppa_opt.ppa == ppa.ppa)) {
+		data_line->meta_distance--;
+		return 0;
+	}
+
+	for (i = 0; i < nr_ppas; i += pblk->min_write_pgs)
+		if (ppa_list[i].g.ch == ppa_opt.g.ch &&
+					ppa_list[i].g.lun == ppa_opt.g.lun)
+			return 1;
+
+	if (test_bit(pblk_ppa_to_pos(geo, ppa_opt), data_line->blk_bitmap)) {
+		for (i = 0; i < nr_ppas; i += pblk->min_write_pgs)
+			if (ppa_list[i].g.ch == ppa.g.ch &&
+						ppa_list[i].g.lun == ppa.g.lun)
+				return 0;
+
+		return 1;
+	}
+
+	return 0;
+}
+
+int pblk_submit_meta_io(struct pblk *pblk, struct pblk_line *meta_line)
+{
+	struct nvm_tgt_dev *dev = pblk->dev;
+	struct nvm_geo *geo = &dev->geo;
+	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
+	struct pblk_line_meta *lm = &pblk->lm;
+	struct pblk_emeta *emeta = meta_line->emeta;
+	struct pblk_g_ctx *m_ctx;
+	struct pblk_lun *rlun;
+	struct bio *bio;
+	struct nvm_rq *rqd;
+	void *data;
+	u64 paddr;
+	int rq_ppas = pblk->min_write_pgs;
+	int id = meta_line->id;
+	int rq_len;
+	int i, j;
+	int ret;
+
+	rqd = pblk_alloc_rqd(pblk, READ);
+	if (IS_ERR(rqd)) {
+		pr_err("pblk: cannot allocate write req.\n");
+		return PTR_ERR(rqd);
+	}
+	m_ctx = nvm_rq_to_pdu(rqd);
+	m_ctx->private = meta_line;
+
+	rq_len = rq_ppas * geo->sec_size;
+	data = ((void *)emeta->buf) + emeta->mem;
+
+	bio = pblk_bio_map_addr(pblk, data, rq_ppas, rq_len,
+					l_mg->emeta_alloc_type, GFP_KERNEL);
+	if (IS_ERR(bio)) {
+		ret = PTR_ERR(bio);
+		goto fail_free_rqd;
+	}
+	bio->bi_iter.bi_sector = 0; /* internal bio */
+	bio_set_op_attrs(bio, REQ_OP_WRITE, 0);
+	rqd->bio = bio;
+
+	ret = pblk_alloc_w_rq(pblk, rqd, rq_ppas, pblk_end_io_write_meta);
+	if (ret)
+		goto fail_free_bio;
+
+	for (i = 0; i < rqd->nr_ppas; ) {
+		spin_lock(&meta_line->lock);
+		paddr = __pblk_alloc_page(pblk, meta_line, rq_ppas);
+		spin_unlock(&meta_line->lock);
+		for (j = 0; j < rq_ppas; j++, i++, paddr++)
+			rqd->ppa_list[i] = addr_to_gen_ppa(pblk, paddr, id);
+	}
+
+	rlun = &pblk->luns[pblk_ppa_to_pos(geo, rqd->ppa_list[0])];
+	ret = down_timeout(&rlun->wr_sem, msecs_to_jiffies(5000));
+	if (ret) {
+		pr_err("pblk: lun semaphore timed out (%d)\n", ret);
+		goto fail_free_bio;
+	}
+
+	emeta->mem += rq_len;
+	if (emeta->mem >= lm->emeta_len[0]) {
+		spin_lock(&l_mg->close_lock);
+		list_del(&meta_line->list);
+		WARN(!bitmap_full(meta_line->map_bitmap, lm->sec_per_line),
+				"pblk: corrupt meta line %d\n", meta_line->id);
+		spin_unlock(&l_mg->close_lock);
+	}
+
+	ret = pblk_submit_io(pblk, rqd);
+	if (ret) {
+		pr_err("pblk: emeta I/O submission failed: %d\n", ret);
+		goto fail_rollback;
+	}
+
+	return NVM_IO_OK;
+
+fail_rollback:
+	spin_lock(&l_mg->close_lock);
+	pblk_dealloc_page(pblk, meta_line, rq_ppas);
+	list_add(&meta_line->list, &meta_line->list);
+	spin_unlock(&l_mg->close_lock);
+fail_free_bio:
+	if (likely(l_mg->emeta_alloc_type == PBLK_VMALLOC_META))
+		bio_put(bio);
+fail_free_rqd:
+	pblk_free_rqd(pblk, rqd, READ);
+	return ret;
+}
+
+static int pblk_sched_meta_io(struct pblk *pblk, struct ppa_addr *prev_list,
+			       int prev_n)
+{
+	struct pblk_line_meta *lm = &pblk->lm;
+	struct pblk_line_mgmt *l_mg = &pblk->l_mg;
+	struct pblk_line *meta_line;
+
+	spin_lock(&l_mg->close_lock);
+retry:
+	if (list_empty(&l_mg->emeta_list)) {
+		spin_unlock(&l_mg->close_lock);
+		return 0;
+	}
+	meta_line = list_first_entry(&l_mg->emeta_list, struct pblk_line, list);
+	if (bitmap_full(meta_line->map_bitmap, lm->sec_per_line))
+		goto retry;
+	spin_unlock(&l_mg->close_lock);
+
+	if (!pblk_valid_meta_ppa(pblk, meta_line, prev_list, prev_n))
+		return 0;
+
+	return pblk_submit_meta_io(pblk, meta_line);
+}
+
+static int pblk_submit_io_set(struct pblk *pblk, struct nvm_rq *rqd)
+{
+	struct pblk_c_ctx *c_ctx = nvm_rq_to_pdu(rqd);
+	struct ppa_addr erase_ppa;
+	int err;
+
+	ppa_set_empty(&erase_ppa);
+
+	/* Assign lbas to ppas and populate request structure */
+	err = pblk_setup_w_rq(pblk, rqd, c_ctx, &erase_ppa);
+	if (err) {
+		pr_err("pblk: could not setup write request: %d\n", err);
+		return NVM_IO_ERR;
+	}
+
+	if (likely(ppa_empty(erase_ppa))) {
+		/* Submit metadata write for previous data line */
+		err = pblk_sched_meta_io(pblk, rqd->ppa_list, rqd->nr_ppas);
+		if (err) {
+			pr_err("pblk: metadata I/O submission failed: %d", err);
+			return NVM_IO_ERR;
+		}
+
+		/* Submit data write for current data line */
+		err = pblk_submit_io(pblk, rqd);
+		if (err) {
+			pr_err("pblk: data I/O submission failed: %d\n", err);
+			return NVM_IO_ERR;
+		}
+	} else {
+		/* Submit data write for current data line */
+		err = pblk_submit_io(pblk, rqd);
+		if (err) {
+			pr_err("pblk: data I/O submission failed: %d\n", err);
+			return NVM_IO_ERR;
+		}
+
+		/* Submit available erase for next data line */
+		if (pblk_blk_erase_async(pblk, erase_ppa)) {
+			struct pblk_line *e_line = pblk_line_get_erase(pblk);
+			struct nvm_tgt_dev *dev = pblk->dev;
+			struct nvm_geo *geo = &dev->geo;
+			int bit;
+
+			atomic_inc(&e_line->left_eblks);
+			bit = pblk_ppa_to_pos(geo, erase_ppa);
+			WARN_ON(!test_and_clear_bit(bit, e_line->erase_bitmap));
+		}
+	}
+
+	return NVM_IO_OK;
+}
+
+static void pblk_free_write_rqd(struct pblk *pblk, struct nvm_rq *rqd)
+{
+	struct pblk_c_ctx *c_ctx = nvm_rq_to_pdu(rqd);
+	struct bio *bio = rqd->bio;
+
+	if (c_ctx->nr_padded)
+		pblk_bio_free_pages(pblk, bio, rqd->nr_ppas, c_ctx->nr_padded);
+}
+
 static int pblk_submit_write(struct pblk *pblk)
 {
 	struct bio *bio;
 	struct nvm_rq *rqd;
-	struct pblk_c_ctx *c_ctx;
-	unsigned int pgs_read;
 	unsigned int secs_avail, secs_to_sync, secs_to_com;
 	unsigned int secs_to_flush;
 	unsigned long pos;
-	int err;
 
 	/* If there are no sectors in the cache, flushes (bios without data)
 	 * will be cleared on the cache threads
@@ -338,7 +558,6 @@ static int pblk_submit_write(struct pblk *pblk)
 		pr_err("pblk: cannot allocate write req.\n");
 		return 1;
 	}
-	c_ctx = nvm_rq_to_pdu(rqd);
 
 	bio = bio_alloc(GFP_KERNEL, pblk->max_write_pgs);
 	if (!bio) {
@@ -358,29 +577,14 @@ static int pblk_submit_write(struct pblk *pblk)
 	secs_to_com = (secs_to_sync > secs_avail) ? secs_avail : secs_to_sync;
 	pos = pblk_rb_read_commit(&pblk->rwb, secs_to_com);
 
-	pgs_read = pblk_rb_read_to_bio(&pblk->rwb, bio, c_ctx, pos,
-						secs_to_sync, secs_avail);
-	if (!pgs_read) {
+	if (pblk_rb_read_to_bio(&pblk->rwb, rqd, bio, pos, secs_to_sync,
+								secs_avail)) {
 		pr_err("pblk: corrupted write bio\n");
 		goto fail_put_bio;
 	}
 
-	if (c_ctx->nr_padded)
-		if (pblk_bio_add_pages(pblk, bio, GFP_KERNEL, c_ctx->nr_padded))
-			goto fail_put_bio;
-
-	/* Assign lbas to ppas and populate request structure */
-	err = pblk_setup_w_rq(pblk, rqd, c_ctx);
-	if (err) {
-		pr_err("pblk: could not setup write request\n");
-		goto fail_free_bio;
-	}
-
-	err = pblk_submit_io(pblk, rqd);
-	if (err) {
-		pr_err("pblk: I/O submission failed: %d\n", err);
+	if (pblk_submit_io_set(pblk, rqd))
 		goto fail_free_bio;
-	}
 
 #ifdef CONFIG_NVM_DEBUG
 	atomic_long_add(secs_to_sync, &pblk->sub_writes);
@@ -389,8 +593,7 @@ static int pblk_submit_write(struct pblk *pblk)
 	return 0;
 
 fail_free_bio:
-	if (c_ctx->nr_padded)
-		pblk_bio_free_pages(pblk, bio, secs_to_sync, c_ctx->nr_padded);
+	pblk_free_write_rqd(pblk, rqd);
 fail_put_bio:
 	bio_put(bio);
 fail_free_rqd:
diff --git a/drivers/lightnvm/pblk.h b/drivers/lightnvm/pblk.h
index 99f3186b5288..15931381348c 100644
--- a/drivers/lightnvm/pblk.h
+++ b/drivers/lightnvm/pblk.h
@@ -40,6 +40,12 @@
 #define PBLK_MAX_REQ_ADDRS (64)
 #define PBLK_MAX_REQ_ADDRS_PW (6)
 
+#define PBLK_WS_POOL_SIZE (128)
+#define PBLK_META_POOL_SIZE (128)
+#define PBLK_READ_REQ_POOL_SIZE (1024)
+
+#define PBLK_NR_CLOSE_JOBS (4)
+
 #define PBLK_CACHE_NAME_LEN (DISK_NAME_LEN + 16)
 
 #define PBLK_COMMAND_TIMEOUT_MS 30000
@@ -72,11 +78,15 @@ enum {
 	PBLK_BLK_ST_CLOSED =	0x2,
 };
 
+struct pblk_sec_meta {
+	u64 reserved;
+	__le64 lba;
+};
+
 /* The number of GC lists and the rate-limiter states go together. This way the
  * rate-limiter can dictate how much GC is needed based on resource utilization.
  */
-#define PBLK_NR_GC_LISTS 3
-#define PBLK_MAX_GC_JOBS 32
+#define PBLK_GC_NR_LISTS 3
 
 enum {
 	PBLK_RL_HIGH = 1,
@@ -84,14 +94,9 @@ enum {
 	PBLK_RL_LOW = 3,
 };
 
-struct pblk_sec_meta {
-	u64 reserved;
-	__le64 lba;
-};
-
 #define pblk_dma_meta_size (sizeof(struct pblk_sec_meta) * PBLK_MAX_REQ_ADDRS)
 
-/* write completion context */
+/* write buffer completion context */
 struct pblk_c_ctx {
 	struct list_head list;		/* Head for out-of-order completion */
 
@@ -101,9 +106,16 @@ struct pblk_c_ctx {
 	unsigned int nr_padded;
 };
 
-/* Read context */
-struct pblk_r_ctx {
-	struct bio *orig_bio;
+/* generic context */
+struct pblk_g_ctx {
+	void *private;
+};
+
+/* Pad context */
+struct pblk_pad_rq {
+	struct pblk *pblk;
+	struct completion wait;
+	struct kref ref;
 };
 
 /* Recovery context */
@@ -195,29 +207,39 @@ struct pblk_lun {
 struct pblk_gc_rq {
 	struct pblk_line *line;
 	void *data;
-	u64 *lba_list;
+	u64 lba_list[PBLK_MAX_REQ_ADDRS];
 	int nr_secs;
 	int secs_to_gc;
 	struct list_head list;
 };
 
 struct pblk_gc {
+	/* These states are not protected by a lock since (i) they are in the
+	 * fast path, and (ii) they are not critical.
+	 */
 	int gc_active;
 	int gc_enabled;
 	int gc_forced;
-	int gc_jobs_active;
-	atomic_t inflight_gc;
 
 	struct task_struct *gc_ts;
 	struct task_struct *gc_writer_ts;
+	struct task_struct *gc_reader_ts;
+
+	struct workqueue_struct *gc_line_reader_wq;
 	struct workqueue_struct *gc_reader_wq;
+
 	struct timer_list gc_timer;
 
+	struct semaphore gc_sem;
+	atomic_t inflight_gc;
 	int w_entries;
+
 	struct list_head w_list;
+	struct list_head r_list;
 
 	spinlock_t lock;
 	spinlock_t w_lock;
+	spinlock_t r_lock;
 };
 
 struct pblk_rl {
@@ -229,10 +251,8 @@ struct pblk_rl {
 				 */
 	unsigned int high_pw;	/* High rounded up as a power of 2 */
 
-#define PBLK_USER_HIGH_THRS 2	/* Begin write limit at 50 percent
-				 * available blks
-				 */
-#define PBLK_USER_LOW_THRS 20	/* Aggressive GC at 5% available blocks */
+#define PBLK_USER_HIGH_THRS 8	/* Begin write limit at 12% available blks */
+#define PBLK_USER_LOW_THRS 10	/* Aggressive GC at 10% available blocks */
 
 	int rb_windows_pw;	/* Number of rate windows in the write buffer
 				 * given as a power-of-2. This guarantees that
@@ -244,13 +264,19 @@ struct pblk_rl {
 				 */
 	int rb_budget;		/* Total number of entries available for I/O */
 	int rb_user_max;	/* Max buffer entries available for user I/O */
-	atomic_t rb_user_cnt;	/* User I/O buffer counter */
 	int rb_gc_max;		/* Max buffer entries available for GC I/O */
 	int rb_gc_rsv;		/* Reserved buffer entries for GC I/O */
 	int rb_state;		/* Rate-limiter current state */
+
+	atomic_t rb_user_cnt;	/* User I/O buffer counter */
 	atomic_t rb_gc_cnt;	/* GC I/O buffer counter */
+	atomic_t rb_space;	/* Space limit in case of reaching capacity */
+
+	int rsv_blocks;		/* Reserved blocks for GC */
 
 	int rb_user_active;
+	int rb_gc_active;
+
 	struct timer_list u_timer;
 
 	unsigned long long nr_secs;
@@ -258,8 +284,6 @@ struct pblk_rl {
 	atomic_t free_blocks;
 };
 
-#define PBLK_LINE_NR_LUN_BITMAP 2
-#define PBLK_LINE_NR_SEC_BITMAP 2
 #define PBLK_LINE_EMPTY (~0U)
 
 enum {
@@ -310,16 +334,19 @@ struct line_smeta {
 	__le32 window_wr_lun;	/* Number of parallel LUNs to write */
 
 	__le32 rsvd[2];
+
+	__le64 lun_bitmap[];
 };
 
 /*
- * Metadata Layout:
- *	1. struct pblk_emeta
- *	2. nr_lbas u64 forming lba list
- *	3. nr_lines (all) u32 valid sector count (vsc) (~0U: non-alloc line)
- *	4. nr_luns bits (u64 format) forming line bad block bitmap
- *
- *	3. and 4. will be part of FTL log
+ * Metadata layout in media:
+ *	First sector:
+ *		1. struct line_emeta
+ *		2. bad block bitmap (u64 * window_wr_lun)
+ *	Mid sectors (start at lbas_sector):
+ *		3. nr_lbas (u64) forming lba list
+ *	Last sectors (start at vsc_sector):
+ *		4. u32 valid sector count (vsc) for all lines (~0U: free line)
  */
 struct line_emeta {
 	struct line_header header;
@@ -339,6 +366,23 @@ struct line_emeta {
 	__le32 next_id;		/* Line id for next line */
 	__le64 nr_lbas;		/* Number of lbas mapped in line */
 	__le64 nr_valid_lbas;	/* Number of valid lbas mapped in line */
+	__le64 bb_bitmap[];	/* Updated bad block bitmap for line */
+};
+
+struct pblk_emeta {
+	struct line_emeta *buf;		/* emeta buffer in media format */
+	int mem;			/* Write offset - points to next
+					 * writable entry in memory
+					 */
+	atomic_t sync;			/* Synced - backpointer that signals the
+					 * last entry that has been successfully
+					 * persisted to media
+					 */
+	unsigned int nr_entries;	/* Number of emeta entries */
+};
+
+struct pblk_smeta {
+	struct line_smeta *buf;		/* smeta buffer in persistent format */
 };
 
 struct pblk_line {
@@ -355,9 +399,12 @@ struct pblk_line {
 
 	unsigned long *lun_bitmap;	/* Bitmap for LUNs mapped in line */
 
-	struct line_smeta *smeta;	/* Start metadata */
-	struct line_emeta *emeta;	/* End metadata */
+	struct pblk_smeta *smeta;	/* Start metadata */
+	struct pblk_emeta *emeta;	/* End medatada */
+
 	int meta_line;			/* Metadata line id */
+	int meta_distance;		/* Distance between data and metadata */
+
 	u64 smeta_ssec;			/* Sector where smeta starts */
 	u64 emeta_ssec;			/* Sector where emeta starts */
 
@@ -374,9 +421,10 @@ struct pblk_line {
 	atomic_t left_seblks;		/* Blocks left for sync erasing */
 
 	int left_msecs;			/* Sectors left for mapping */
-	int left_ssecs;			/* Sectors left to sync */
 	unsigned int cur_sec;		/* Sector map pointer */
-	unsigned int vsc;		/* Valid sector count in line */
+	unsigned int nr_valid_lbas;	/* Number of valid lbas in line */
+
+	__le32 *vsc;			/* Valid sector count in line */
 
 	struct kref ref;		/* Write buffer L2P references */
 
@@ -385,13 +433,15 @@ struct pblk_line {
 
 #define PBLK_DATA_LINES 4
 
-enum{
+enum {
 	PBLK_KMALLOC_META = 1,
 	PBLK_VMALLOC_META = 2,
 };
 
-struct pblk_line_metadata {
-	void *meta;
+enum {
+	PBLK_EMETA_TYPE_HEADER = 1,	/* struct line_emeta first sector */
+	PBLK_EMETA_TYPE_LLBA = 2,	/* lba list - type: __le64 */
+	PBLK_EMETA_TYPE_VSC = 3,	/* vsc list - type: __le32 */
 };
 
 struct pblk_line_mgmt {
@@ -404,7 +454,7 @@ struct pblk_line_mgmt {
 	struct list_head bad_list;	/* Full lines bad */
 
 	/* GC lists - use gc_lock */
-	struct list_head *gc_lists[PBLK_NR_GC_LISTS];
+	struct list_head *gc_lists[PBLK_GC_NR_LISTS];
 	struct list_head gc_high_list;	/* Full lines ready to GC, high isc */
 	struct list_head gc_mid_list;	/* Full lines ready to GC, mid isc */
 	struct list_head gc_low_list;	/* Full lines ready to GC, low isc */
@@ -417,13 +467,16 @@ struct pblk_line_mgmt {
 	struct pblk_line *log_next;	/* Next FTL log line */
 	struct pblk_line *data_next;	/* Next data line */
 
+	struct list_head emeta_list;	/* Lines queued to schedule emeta */
+
+	__le32 *vsc_list;		/* Valid sector counts for all lines */
+
 	/* Metadata allocation type: VMALLOC | KMALLOC */
-	int smeta_alloc_type;
 	int emeta_alloc_type;
 
 	/* Pre-allocated metadata for data lines */
-	struct pblk_line_metadata sline_meta[PBLK_DATA_LINES];
-	struct pblk_line_metadata eline_meta[PBLK_DATA_LINES];
+	struct pblk_smeta *sline_meta[PBLK_DATA_LINES];
+	struct pblk_emeta *eline_meta[PBLK_DATA_LINES];
 	unsigned long meta_bitmap;
 
 	/* Helpers for fast bitmap calculations */
@@ -434,25 +487,40 @@ struct pblk_line_mgmt {
 	unsigned long l_seq_nr;		/* Log line unique sequence number */
 
 	spinlock_t free_lock;
+	spinlock_t close_lock;
 	spinlock_t gc_lock;
 };
 
 struct pblk_line_meta {
 	unsigned int smeta_len;		/* Total length for smeta */
-	unsigned int smeta_sec;		/* Sectors needed for smeta*/
-	unsigned int emeta_len;		/* Total length for emeta */
-	unsigned int emeta_sec;		/* Sectors needed for emeta*/
+	unsigned int smeta_sec;		/* Sectors needed for smeta */
+
+	unsigned int emeta_len[4];	/* Lengths for emeta:
+					 *  [0]: Total length
+					 *  [1]: struct line_emeta length
+					 *  [2]: L2P portion length
+					 *  [3]: vsc list length
+					 */
+	unsigned int emeta_sec[4];	/* Sectors needed for emeta. Same layout
+					 * as emeta_len
+					 */
+
 	unsigned int emeta_bb;		/* Boundary for bb that affects emeta */
+
+	unsigned int vsc_list_len;	/* Length for vsc list */
 	unsigned int sec_bitmap_len;	/* Length for sector bitmap in line */
 	unsigned int blk_bitmap_len;	/* Length for block bitmap in line */
 	unsigned int lun_bitmap_len;	/* Length for lun bitmap in line */
 
 	unsigned int blk_per_line;	/* Number of blocks in a full line */
 	unsigned int sec_per_line;	/* Number of sectors in a line */
+	unsigned int dsec_per_line;	/* Number of data sectors in a line */
 	unsigned int min_blk_line;	/* Min. number of good blocks in line */
 
 	unsigned int mid_thrs;		/* Threshold for GC mid list */
 	unsigned int high_thrs;		/* Threshold for GC high list */
+
+	unsigned int meta_distance;	/* Distance between data and metadata */
 };
 
 struct pblk_addr_format {
@@ -470,6 +538,13 @@ struct pblk_addr_format {
 	u8	sec_offset;
 };
 
+enum {
+	PBLK_STATE_RUNNING = 0,
+	PBLK_STATE_STOPPING = 1,
+	PBLK_STATE_RECOVERING = 2,
+	PBLK_STATE_STOPPED = 3,
+};
+
 struct pblk {
 	struct nvm_tgt_dev *dev;
 	struct gendisk *disk;
@@ -487,6 +562,8 @@ struct pblk {
 
 	struct pblk_rb rwb;
 
+	int state;			/* pblk line state */
+
 	int min_write_pgs; /* Minimum amount of pages required by controller */
 	int max_write_pgs; /* Maximum amount of pages supported by controller */
 	int pgs_in_buffer; /* Number of pages that need to be held in buffer to
@@ -499,7 +576,7 @@ struct pblk {
 	/* pblk provisioning values. Used by rate limiter */
 	struct pblk_rl rl;
 
-	struct semaphore erase_sem;
+	int sec_per_write;
 
 	unsigned char instance_uuid[16];
 #ifdef CONFIG_NVM_DEBUG
@@ -511,8 +588,8 @@ struct pblk {
 	atomic_long_t req_writes;	/* Sectors stored on write buffer */
 	atomic_long_t sub_writes;	/* Sectors submitted from buffer */
 	atomic_long_t sync_writes;	/* Sectors synced to media */
-	atomic_long_t compl_writes;	/* Sectors completed in write bio */
 	atomic_long_t inflight_reads;	/* Inflight sector read requests */
+	atomic_long_t cache_reads;	/* Read requests that hit the cache */
 	atomic_long_t sync_reads;	/* Completed sector read requests */
 	atomic_long_t recov_writes;	/* Sectors submitted from recovery */
 	atomic_long_t recov_gc_writes;	/* Sectors submitted from write GC */
@@ -528,6 +605,8 @@ struct pblk {
 	atomic_long_t write_failed;
 	atomic_long_t erase_failed;
 
+	atomic_t inflight_io;		/* General inflight I/O counter */
+
 	struct task_struct *writer_ts;
 
 	/* Simple translation map of logical addresses to physical addresses.
@@ -542,11 +621,13 @@ struct pblk {
 	mempool_t *page_pool;
 	mempool_t *line_ws_pool;
 	mempool_t *rec_pool;
-	mempool_t *r_rq_pool;
+	mempool_t *g_rq_pool;
 	mempool_t *w_rq_pool;
 	mempool_t *line_meta_pool;
 
-	struct workqueue_struct *kw_wq;
+	struct workqueue_struct *close_wq;
+	struct workqueue_struct *bb_wq;
+
 	struct timer_list wtimer;
 
 	struct pblk_gc gc;
@@ -559,7 +640,7 @@ struct pblk_line_ws {
 	struct work_struct ws;
 };
 
-#define pblk_r_rq_size (sizeof(struct nvm_rq) + sizeof(struct pblk_r_ctx))
+#define pblk_g_rq_size (sizeof(struct nvm_rq) + sizeof(struct pblk_g_ctx))
 #define pblk_w_rq_size (sizeof(struct nvm_rq) + sizeof(struct pblk_c_ctx))
 
 /*
@@ -579,18 +660,17 @@ void pblk_rb_write_entry_gc(struct pblk_rb *rb, void *data,
 			    struct pblk_w_ctx w_ctx, struct pblk_line *gc_line,
 			    unsigned int pos);
 struct pblk_w_ctx *pblk_rb_w_ctx(struct pblk_rb *rb, unsigned int pos);
+void pblk_rb_flush(struct pblk_rb *rb);
 
 void pblk_rb_sync_l2p(struct pblk_rb *rb);
-unsigned int pblk_rb_read_to_bio(struct pblk_rb *rb, struct bio *bio,
-				 struct pblk_c_ctx *c_ctx,
-				 unsigned int pos,
-				 unsigned int nr_entries,
-				 unsigned int count);
+unsigned int pblk_rb_read_to_bio(struct pblk_rb *rb, struct nvm_rq *rqd,
+				 struct bio *bio, unsigned int pos,
+				 unsigned int nr_entries, unsigned int count);
 unsigned int pblk_rb_read_to_bio_list(struct pblk_rb *rb, struct bio *bio,
 				      struct list_head *list,
 				      unsigned int max);
 int pblk_rb_copy_to_bio(struct pblk_rb *rb, struct bio *bio, sector_t lba,
-			u64 pos, int bio_iter);
+			struct ppa_addr ppa, int bio_iter);
 unsigned int pblk_rb_read_commit(struct pblk_rb *rb, unsigned int entries);
 
 unsigned int pblk_rb_sync_init(struct pblk_rb *rb, unsigned long *flags);
@@ -601,6 +681,7 @@ void pblk_rb_sync_end(struct pblk_rb *rb, unsigned long *flags);
 unsigned int pblk_rb_sync_point_count(struct pblk_rb *rb);
 
 unsigned int pblk_rb_read_count(struct pblk_rb *rb);
+unsigned int pblk_rb_sync_count(struct pblk_rb *rb);
 unsigned int pblk_rb_wrap_pos(struct pblk_rb *rb, unsigned int pos);
 
 int pblk_rb_tear_down_check(struct pblk_rb *rb);
@@ -612,40 +693,50 @@ ssize_t pblk_rb_sysfs(struct pblk_rb *rb, char *buf);
  * pblk core
  */
 struct nvm_rq *pblk_alloc_rqd(struct pblk *pblk, int rw);
+void pblk_set_sec_per_write(struct pblk *pblk, int sec_per_write);
 int pblk_setup_w_rec_rq(struct pblk *pblk, struct nvm_rq *rqd,
 			struct pblk_c_ctx *c_ctx);
 void pblk_free_rqd(struct pblk *pblk, struct nvm_rq *rqd, int rw);
-void pblk_flush_writer(struct pblk *pblk);
+void pblk_wait_for_meta(struct pblk *pblk);
 struct ppa_addr pblk_get_lba_map(struct pblk *pblk, sector_t lba);
 void pblk_discard(struct pblk *pblk, struct bio *bio);
 void pblk_log_write_err(struct pblk *pblk, struct nvm_rq *rqd);
 void pblk_log_read_err(struct pblk *pblk, struct nvm_rq *rqd);
 int pblk_submit_io(struct pblk *pblk, struct nvm_rq *rqd);
+int pblk_submit_meta_io(struct pblk *pblk, struct pblk_line *meta_line);
 struct bio *pblk_bio_map_addr(struct pblk *pblk, void *data,
 			      unsigned int nr_secs, unsigned int len,
-			      gfp_t gfp_mask);
+			      int alloc_type, gfp_t gfp_mask);
 struct pblk_line *pblk_line_get(struct pblk *pblk);
 struct pblk_line *pblk_line_get_first_data(struct pblk *pblk);
-struct pblk_line *pblk_line_replace_data(struct pblk *pblk);
+void pblk_line_replace_data(struct pblk *pblk);
 int pblk_line_recov_alloc(struct pblk *pblk, struct pblk_line *line);
 void pblk_line_recov_close(struct pblk *pblk, struct pblk_line *line);
 struct pblk_line *pblk_line_get_data(struct pblk *pblk);
-struct pblk_line *pblk_line_get_data_next(struct pblk *pblk);
+struct pblk_line *pblk_line_get_erase(struct pblk *pblk);
 int pblk_line_erase(struct pblk *pblk, struct pblk_line *line);
 int pblk_line_is_full(struct pblk_line *line);
 void pblk_line_free(struct pblk *pblk, struct pblk_line *line);
-void pblk_line_close_ws(struct work_struct *work);
+void pblk_line_close_meta(struct pblk *pblk, struct pblk_line *line);
 void pblk_line_close(struct pblk *pblk, struct pblk_line *line);
+void pblk_line_close_meta_sync(struct pblk *pblk);
+void pblk_line_close_ws(struct work_struct *work);
+void pblk_pipeline_stop(struct pblk *pblk);
 void pblk_line_mark_bb(struct work_struct *work);
 void pblk_line_run_ws(struct pblk *pblk, struct pblk_line *line, void *priv,
-		      void (*work)(struct work_struct *));
+		      void (*work)(struct work_struct *),
+		      struct workqueue_struct *wq);
 u64 pblk_line_smeta_start(struct pblk *pblk, struct pblk_line *line);
 int pblk_line_read_smeta(struct pblk *pblk, struct pblk_line *line);
-int pblk_line_read_emeta(struct pblk *pblk, struct pblk_line *line);
+int pblk_line_read_emeta(struct pblk *pblk, struct pblk_line *line,
+			 void *emeta_buf);
 int pblk_blk_erase_async(struct pblk *pblk, struct ppa_addr erase_ppa);
 void pblk_line_put(struct kref *ref);
 struct list_head *pblk_line_gc_list(struct pblk *pblk, struct pblk_line *line);
+u64 pblk_lookup_page(struct pblk *pblk, struct pblk_line *line);
+void pblk_dealloc_page(struct pblk *pblk, struct pblk_line *line, int nr_secs);
 u64 pblk_alloc_page(struct pblk *pblk, struct pblk_line *line, int nr_secs);
+u64 __pblk_alloc_page(struct pblk *pblk, struct pblk_line *line, int nr_secs);
 int pblk_calc_secs(struct pblk *pblk, unsigned long secs_avail,
 		   unsigned long secs_to_flush);
 void pblk_down_rq(struct pblk *pblk, struct ppa_addr *ppa_list, int nr_ppas,
@@ -656,11 +747,11 @@ void pblk_end_bio_sync(struct bio *bio);
 void pblk_end_io_sync(struct nvm_rq *rqd);
 int pblk_bio_add_pages(struct pblk *pblk, struct bio *bio, gfp_t flags,
 		       int nr_pages);
-void pblk_map_pad_invalidate(struct pblk *pblk, struct pblk_line *line,
-			     u64 paddr);
 void pblk_bio_free_pages(struct pblk *pblk, struct bio *bio, int off,
 			 int nr_pages);
 void pblk_map_invalidate(struct pblk *pblk, struct ppa_addr ppa);
+void __pblk_map_invalidate(struct pblk *pblk, struct pblk_line *line,
+			   u64 paddr);
 void pblk_update_map(struct pblk *pblk, sector_t lba, struct ppa_addr ppa);
 void pblk_update_map_cache(struct pblk *pblk, sector_t lba,
 			   struct ppa_addr ppa);
@@ -702,6 +793,7 @@ void pblk_write_should_kick(struct pblk *pblk);
 /*
  * pblk read path
  */
+extern struct bio_set *pblk_bio_set;
 int pblk_submit_read(struct pblk *pblk, struct bio *bio);
 int pblk_submit_read_gc(struct pblk *pblk, u64 *lba_list, void *data,
 			unsigned int nr_secs, unsigned int *secs_to_gc,
@@ -711,7 +803,7 @@ int pblk_submit_read_gc(struct pblk *pblk, u64 *lba_list, void *data,
  */
 void pblk_submit_rec(struct work_struct *work);
 struct pblk_line *pblk_recov_l2p(struct pblk *pblk);
-void pblk_recov_pad(struct pblk *pblk);
+int pblk_recov_pad(struct pblk *pblk);
 __le64 *pblk_recov_get_lba_list(struct pblk *pblk, struct line_emeta *emeta);
 int pblk_recov_setup_rq(struct pblk *pblk, struct pblk_c_ctx *c_ctx,
 			struct pblk_rec_ctx *recovery, u64 *comp_bits,
@@ -720,33 +812,40 @@ int pblk_recov_setup_rq(struct pblk *pblk, struct pblk_c_ctx *c_ctx,
 /*
  * pblk gc
  */
-#define PBLK_GC_TRIES 3
+#define PBLK_GC_MAX_READERS 8	/* Max number of outstanding GC reader jobs */
+#define PBLK_GC_W_QD 128	/* Queue depth for inflight GC write I/Os */
+#define PBLK_GC_L_QD 4		/* Queue depth for inflight GC lines */
+#define PBLK_GC_RSV_LINE 1	/* Reserved lines for GC */
 
 int pblk_gc_init(struct pblk *pblk);
 void pblk_gc_exit(struct pblk *pblk);
 void pblk_gc_should_start(struct pblk *pblk);
 void pblk_gc_should_stop(struct pblk *pblk);
-int pblk_gc_status(struct pblk *pblk);
+void pblk_gc_should_kick(struct pblk *pblk);
+void pblk_gc_kick(struct pblk *pblk);
 void pblk_gc_sysfs_state_show(struct pblk *pblk, int *gc_enabled,
 			      int *gc_active);
-void pblk_gc_sysfs_force(struct pblk *pblk, int force);
+int pblk_gc_sysfs_force(struct pblk *pblk, int force);
 
 /*
  * pblk rate limiter
  */
 void pblk_rl_init(struct pblk_rl *rl, int budget);
 void pblk_rl_free(struct pblk_rl *rl);
-int pblk_rl_gc_thrs(struct pblk_rl *rl);
+int pblk_rl_high_thrs(struct pblk_rl *rl);
+int pblk_rl_low_thrs(struct pblk_rl *rl);
 unsigned long pblk_rl_nr_free_blks(struct pblk_rl *rl);
 int pblk_rl_user_may_insert(struct pblk_rl *rl, int nr_entries);
+void pblk_rl_inserted(struct pblk_rl *rl, int nr_entries);
 void pblk_rl_user_in(struct pblk_rl *rl, int nr_entries);
 int pblk_rl_gc_may_insert(struct pblk_rl *rl, int nr_entries);
 void pblk_rl_gc_in(struct pblk_rl *rl, int nr_entries);
 void pblk_rl_out(struct pblk_rl *rl, int nr_user, int nr_gc);
-void pblk_rl_set_gc_rsc(struct pblk_rl *rl, int rsv);
 int pblk_rl_sysfs_rate_show(struct pblk_rl *rl);
 void pblk_rl_free_lines_inc(struct pblk_rl *rl, struct pblk_line *line);
 void pblk_rl_free_lines_dec(struct pblk_rl *rl, struct pblk_line *line);
+void pblk_rl_set_space_limit(struct pblk_rl *rl, int entries_left);
+int pblk_rl_is_limit(struct pblk_rl *rl);
 
 /*
  * pblk sysfs
@@ -774,9 +873,30 @@ static inline struct nvm_rq *nvm_rq_from_c_ctx(void *c_ctx)
 	return c_ctx - sizeof(struct nvm_rq);
 }
 
-static inline void *pblk_line_emeta_to_lbas(struct line_emeta *emeta)
+static inline void *emeta_to_bb(struct line_emeta *emeta)
+{
+	return emeta->bb_bitmap;
+}
+
+static inline void *emeta_to_lbas(struct pblk *pblk, struct line_emeta *emeta)
+{
+	return ((void *)emeta + pblk->lm.emeta_len[1]);
+}
+
+static inline void *emeta_to_vsc(struct pblk *pblk, struct line_emeta *emeta)
 {
-	return (emeta) + 1;
+	return (emeta_to_lbas(pblk, emeta) + pblk->lm.emeta_len[2]);
+}
+
+static inline int pblk_line_vsc(struct pblk_line *line)
+{
+	int vsc;
+
+	spin_lock(&line->lock);
+	vsc = le32_to_cpu(*line->vsc);
+	spin_unlock(&line->lock);
+
+	return vsc;
 }
 
 #define NVM_MEM_PAGE_WRITE (8)
@@ -917,6 +1037,14 @@ static inline void pblk_ppa_set_empty(struct ppa_addr *ppa_addr)
 	ppa_addr->ppa = ADDR_EMPTY;
 }
 
+static inline bool pblk_ppa_comp(struct ppa_addr lppa, struct ppa_addr rppa)
+{
+	if (lppa.ppa == rppa.ppa)
+		return true;
+
+	return false;
+}
+
 static inline int pblk_addr_in_cache(struct ppa_addr ppa)
 {
 	return (ppa.ppa != ADDR_EMPTY && ppa.c.is_cached);
@@ -964,11 +1092,11 @@ static inline struct ppa_addr addr_to_pblk_ppa(struct pblk *pblk, u64 paddr,
 }
 
 static inline u32 pblk_calc_meta_header_crc(struct pblk *pblk,
-					    struct line_smeta *smeta)
+					    struct line_header *header)
 {
 	u32 crc = ~(u32)0;
 
-	crc = crc32_le(crc, (unsigned char *)smeta + sizeof(crc),
+	crc = crc32_le(crc, (unsigned char *)header + sizeof(crc),
 				sizeof(struct line_header) - sizeof(crc));
 
 	return crc;
@@ -996,7 +1124,7 @@ static inline u32 pblk_calc_emeta_crc(struct pblk *pblk,
 
 	crc = crc32_le(crc, (unsigned char *)emeta +
 				sizeof(struct line_header) + sizeof(crc),
-				lm->emeta_len -
+				lm->emeta_len[0] -
 				sizeof(struct line_header) - sizeof(crc));
 
 	return crc;
@@ -1016,9 +1144,27 @@ static inline int pblk_set_progr_mode(struct pblk *pblk, int type)
 	return flags;
 }
 
-static inline int pblk_set_read_mode(struct pblk *pblk)
+enum {
+	PBLK_READ_RANDOM	= 0,
+	PBLK_READ_SEQUENTIAL	= 1,
+};
+
+static inline int pblk_set_read_mode(struct pblk *pblk, int type)
+{
+	struct nvm_tgt_dev *dev = pblk->dev;
+	struct nvm_geo *geo = &dev->geo;
+	int flags;
+
+	flags = NVM_IO_SUSPEND | NVM_IO_SCRAMBLE_ENABLE;
+	if (type == PBLK_READ_SEQUENTIAL)
+		flags |= geo->plane_mode >> 1;
+
+	return flags;
+}
+
+static inline int pblk_io_aligned(struct pblk *pblk, int nr_secs)
 {
-	return NVM_IO_SNGL_ACCESS | NVM_IO_SUSPEND | NVM_IO_SCRAMBLE_ENABLE;
+	return !(nr_secs % pblk->min_write_pgs);
 }
 
 #ifdef CONFIG_NVM_DEBUG
diff --git a/drivers/lightnvm/rrpc.c b/drivers/lightnvm/rrpc.c
index cf0e28a0ff61..267f01ae87e4 100644
--- a/drivers/lightnvm/rrpc.c
+++ b/drivers/lightnvm/rrpc.c
@@ -279,8 +279,8 @@ static void rrpc_end_sync_bio(struct bio *bio)
 {
 	struct completion *waiting = bio->bi_private;
 
-	if (bio->bi_error)
-		pr_err("nvm: gc request failed (%u).\n", bio->bi_error);
+	if (bio->bi_status)
+		pr_err("nvm: gc request failed (%u).\n", bio->bi_status);
 
 	complete(waiting);
 }
@@ -359,7 +359,7 @@ try:
 			goto finished;
 		}
 		wait_for_completion_io(&wait);
-		if (bio->bi_error) {
+		if (bio->bi_status) {
 			rrpc_inflight_laddr_release(rrpc, rqd);
 			goto finished;
 		}
@@ -385,7 +385,7 @@ try:
 		wait_for_completion_io(&wait);
 
 		rrpc_inflight_laddr_release(rrpc, rqd);
-		if (bio->bi_error)
+		if (bio->bi_status)
 			goto finished;
 
 		bio_reset(bio);
@@ -994,7 +994,7 @@ static blk_qc_t rrpc_make_rq(struct request_queue *q, struct bio *bio)
 	struct nvm_rq *rqd;
 	int err;
 
-	blk_queue_split(q, &bio, q->bio_split);
+	blk_queue_split(q, &bio);
 
 	if (bio_op(bio) == REQ_OP_DISCARD) {
 		rrpc_discard(rrpc, bio);
diff --git a/drivers/macintosh/adb.c b/drivers/macintosh/adb.c
index fee939efc4fc..039dc8285fc5 100644
--- a/drivers/macintosh/adb.c
+++ b/drivers/macintosh/adb.c
@@ -723,8 +723,6 @@ static ssize_t adb_read(struct file *file, char __user *buf,
 		return -EINVAL;
 	if (count > sizeof(req->reply))
 		count = sizeof(req->reply);
-	if (!access_ok(VERIFY_WRITE, buf, count))
-		return -EFAULT;
 
 	req = NULL;
 	spin_lock_irqsave(&state->lock, flags);
@@ -781,8 +779,6 @@ static ssize_t adb_write(struct file *file, const char __user *buf,
 		return -EINVAL;
 	if (adb_controller == NULL)
 		return -ENXIO;
-	if (!access_ok(VERIFY_READ, buf, count))
-		return -EFAULT;
 
 	req = kmalloc(sizeof(struct adb_request),
 					     GFP_KERNEL);
diff --git a/drivers/macintosh/macio_asic.c b/drivers/macintosh/macio_asic.c
index f757cef293f8..62f541f968f6 100644
--- a/drivers/macintosh/macio_asic.c
+++ b/drivers/macintosh/macio_asic.c
@@ -133,7 +133,7 @@ static int macio_device_resume(struct device * dev)
 	return 0;
 }
 
-extern struct device_attribute macio_dev_attrs[];
+extern const struct attribute_group *macio_dev_groups[];
 
 struct bus_type macio_bus_type = {
        .name	= "macio",
@@ -144,7 +144,7 @@ struct bus_type macio_bus_type = {
        .shutdown = macio_device_shutdown,
        .suspend	= macio_device_suspend,
        .resume	= macio_device_resume,
-       .dev_attrs = macio_dev_attrs,
+       .dev_groups = macio_dev_groups,
 };
 
 static int __init macio_bus_driver_init(void)
diff --git a/drivers/macintosh/macio_sysfs.c b/drivers/macintosh/macio_sysfs.c
index 0b1f9c76c68d..2445274f7e4b 100644
--- a/drivers/macintosh/macio_sysfs.c
+++ b/drivers/macintosh/macio_sysfs.c
@@ -10,7 +10,8 @@ field##_show (struct device *dev, struct device_attribute *attr,	\
 {									\
 	struct macio_dev *mdev = to_macio_device (dev);			\
 	return sprintf (buf, format_string, mdev->ofdev.dev.of_node->field); \
-}
+}									\
+static DEVICE_ATTR_RO(field);
 
 static ssize_t
 compatible_show (struct device *dev, struct device_attribute *attr, char *buf)
@@ -37,6 +38,7 @@ compatible_show (struct device *dev, struct device_attribute *attr, char *buf)
 
 	return length;
 }
+static DEVICE_ATTR_RO(compatible);
 
 static ssize_t modalias_show (struct device *dev, struct device_attribute *attr,
 			      char *buf)
@@ -52,15 +54,26 @@ static ssize_t devspec_show(struct device *dev,
 	ofdev = to_platform_device(dev);
 	return sprintf(buf, "%s\n", ofdev->dev.of_node->full_name);
 }
+static DEVICE_ATTR_RO(modalias);
+static DEVICE_ATTR_RO(devspec);
 
 macio_config_of_attr (name, "%s\n");
 macio_config_of_attr (type, "%s\n");
 
-struct device_attribute macio_dev_attrs[] = {
-	__ATTR_RO(name),
-	__ATTR_RO(type),
-	__ATTR_RO(compatible),
-	__ATTR_RO(modalias),
-	__ATTR_RO(devspec),
-	__ATTR_NULL
+static struct attribute *macio_dev_attrs[] = {
+	&dev_attr_name.attr,
+	&dev_attr_type.attr,
+	&dev_attr_compatible.attr,
+	&dev_attr_modalias.attr,
+	&dev_attr_devspec.attr,
+	NULL,
+};
+
+static const struct attribute_group macio_dev_group = {
+	.attrs = macio_dev_attrs,
+};
+
+const struct attribute_group *macio_dev_groups[] = {
+	&macio_dev_group,
+	NULL,
 };
diff --git a/drivers/mailbox/pcc.c b/drivers/mailbox/pcc.c
index dd9ecd354a3e..ac91fd0d62c6 100644
--- a/drivers/mailbox/pcc.c
+++ b/drivers/mailbox/pcc.c
@@ -203,7 +203,7 @@ static irqreturn_t pcc_mbox_irq(int irq, void *p)
 		struct acpi_pcct_hw_reduced_type2 *pcct2_ss = chan->con_priv;
 		u32 id = chan - pcc_mbox_channels;
 
-		doorbell_ack = &pcct2_ss->doorbell_ack_register;
+		doorbell_ack = &pcct2_ss->platform_ack_register;
 		doorbell_ack_preserve = pcct2_ss->ack_preserve_mask;
 		doorbell_ack_write = pcct2_ss->ack_write_mask;
 
@@ -416,11 +416,11 @@ static int parse_pcc_subspace(struct acpi_subtable_header *header,
 static int pcc_parse_subspace_irq(int id,
 				  struct acpi_pcct_hw_reduced *pcct_ss)
 {
-	pcc_doorbell_irq[id] = pcc_map_interrupt(pcct_ss->doorbell_interrupt,
+	pcc_doorbell_irq[id] = pcc_map_interrupt(pcct_ss->platform_interrupt,
 						 (u32)pcct_ss->flags);
 	if (pcc_doorbell_irq[id] <= 0) {
 		pr_err("PCC GSI %d not registered\n",
-		       pcct_ss->doorbell_interrupt);
+		       pcct_ss->platform_interrupt);
 		return -EINVAL;
 	}
 
@@ -429,8 +429,8 @@ static int pcc_parse_subspace_irq(int id,
 		struct acpi_pcct_hw_reduced_type2 *pcct2_ss = (void *)pcct_ss;
 
 		pcc_doorbell_ack_vaddr[id] = acpi_os_ioremap(
-				pcct2_ss->doorbell_ack_register.address,
-				pcct2_ss->doorbell_ack_register.bit_width / 8);
+				pcct2_ss->platform_ack_register.address,
+				pcct2_ss->platform_ack_register.bit_width / 8);
 		if (!pcc_doorbell_ack_vaddr[id]) {
 			pr_err("Failed to ioremap PCC ACK register\n");
 			return -ENOMEM;
diff --git a/drivers/md/Kconfig b/drivers/md/Kconfig
index 906103c168ea..4a249ee86364 100644
--- a/drivers/md/Kconfig
+++ b/drivers/md/Kconfig
@@ -521,6 +521,23 @@ config DM_INTEGRITY
 	  To compile this code as a module, choose M here: the module will
 	  be called dm-integrity.
 
+config DM_ZONED
+	tristate "Drive-managed zoned block device target support"
+	depends on BLK_DEV_DM
+	depends on BLK_DEV_ZONED
+	---help---
+	  This device-mapper target takes a host-managed or host-aware zoned
+	  block device and exposes most of its capacity as a regular block
+	  device (drive-managed zoned block device) without any write
+	  constraints. This is mainly intended for use with file systems that
+	  do not natively support zoned block devices but still want to
+	  benefit from the increased capacity offered by SMR disks. Other uses
+	  by applications using raw block devices (for example object stores)
+	  are also possible.
+
+	  To compile this code as a module, choose M here: the module will
+	  be called dm-zoned.
+
 	  If unsure, say N.
 
 endif # MD
diff --git a/drivers/md/Makefile b/drivers/md/Makefile
index 913720bd81c1..786ec9e86d65 100644
--- a/drivers/md/Makefile
+++ b/drivers/md/Makefile
@@ -20,6 +20,7 @@ dm-era-y	+= dm-era-target.o
 dm-verity-y	+= dm-verity-target.o
 md-mod-y	+= md.o bitmap.o
 raid456-y	+= raid5.o raid5-cache.o raid5-ppl.o
+dm-zoned-y	+= dm-zoned-target.o dm-zoned-metadata.o dm-zoned-reclaim.o
 
 # Note: link order is important.  All raid personalities
 # and must come before md.o, as they each initialise 
@@ -60,6 +61,7 @@ obj-$(CONFIG_DM_CACHE_SMQ)	+= dm-cache-smq.o
 obj-$(CONFIG_DM_ERA)		+= dm-era.o
 obj-$(CONFIG_DM_LOG_WRITES)	+= dm-log-writes.o
 obj-$(CONFIG_DM_INTEGRITY)	+= dm-integrity.o
+obj-$(CONFIG_DM_ZONED)		+= dm-zoned.o
 
 ifeq ($(CONFIG_DM_UEVENT),y)
 dm-mod-objs			+= dm-uevent.o
diff --git a/drivers/md/bcache/bcache.h b/drivers/md/bcache/bcache.h
index c3ea03c9a1a8..dee542fff68e 100644
--- a/drivers/md/bcache/bcache.h
+++ b/drivers/md/bcache/bcache.h
@@ -849,10 +849,11 @@ static inline void wake_up_allocators(struct cache_set *c)
 
 /* Forward declarations */
 
-void bch_count_io_errors(struct cache *, int, const char *);
+void bch_count_io_errors(struct cache *, blk_status_t, const char *);
 void bch_bbio_count_io_errors(struct cache_set *, struct bio *,
-			      int, const char *);
-void bch_bbio_endio(struct cache_set *, struct bio *, int, const char *);
+			      blk_status_t, const char *);
+void bch_bbio_endio(struct cache_set *, struct bio *, blk_status_t,
+		const char *);
 void bch_bbio_free(struct bio *, struct cache_set *);
 struct bio *bch_bbio_alloc(struct cache_set *);
 
diff --git a/drivers/md/bcache/btree.c b/drivers/md/bcache/btree.c
index 450d0e848ae4..866dcf78ff8e 100644
--- a/drivers/md/bcache/btree.c
+++ b/drivers/md/bcache/btree.c
@@ -307,7 +307,7 @@ static void bch_btree_node_read(struct btree *b)
 	bch_submit_bbio(bio, b->c, &b->key, 0);
 	closure_sync(&cl);
 
-	if (bio->bi_error)
+	if (bio->bi_status)
 		set_btree_node_io_error(b);
 
 	bch_bbio_free(bio, b->c);
@@ -374,10 +374,10 @@ static void btree_node_write_endio(struct bio *bio)
 	struct closure *cl = bio->bi_private;
 	struct btree *b = container_of(cl, struct btree, io);
 
-	if (bio->bi_error)
+	if (bio->bi_status)
 		set_btree_node_io_error(b);
 
-	bch_bbio_count_io_errors(b->c, bio, bio->bi_error, "writing btree");
+	bch_bbio_count_io_errors(b->c, bio, bio->bi_status, "writing btree");
 	closure_put(cl);
 }
 
diff --git a/drivers/md/bcache/btree.h b/drivers/md/bcache/btree.h
index 9b80417cd547..73da1f5626cb 100644
--- a/drivers/md/bcache/btree.h
+++ b/drivers/md/bcache/btree.h
@@ -207,7 +207,7 @@ void bkey_put(struct cache_set *c, struct bkey *k);
 
 struct btree_op {
 	/* for waiting on btree reserve in btree_split() */
-	wait_queue_t		wait;
+	wait_queue_entry_t		wait;
 
 	/* Btree level at which we start taking write locks */
 	short			lock;
diff --git a/drivers/md/bcache/debug.c b/drivers/md/bcache/debug.c
index 06f55056aaae..35a5a7210e51 100644
--- a/drivers/md/bcache/debug.c
+++ b/drivers/md/bcache/debug.c
@@ -110,7 +110,7 @@ void bch_data_verify(struct cached_dev *dc, struct bio *bio)
 	struct bio_vec bv, cbv;
 	struct bvec_iter iter, citer = { 0 };
 
-	check = bio_clone(bio, GFP_NOIO);
+	check = bio_clone_kmalloc(bio, GFP_NOIO);
 	if (!check)
 		return;
 	check->bi_opf = REQ_OP_READ;
diff --git a/drivers/md/bcache/io.c b/drivers/md/bcache/io.c
index db45a88c0ce9..6a9b85095e7b 100644
--- a/drivers/md/bcache/io.c
+++ b/drivers/md/bcache/io.c
@@ -50,7 +50,7 @@ void bch_submit_bbio(struct bio *bio, struct cache_set *c,
 
 /* IO errors */
 
-void bch_count_io_errors(struct cache *ca, int error, const char *m)
+void bch_count_io_errors(struct cache *ca, blk_status_t error, const char *m)
 {
 	/*
 	 * The halflife of an error is:
@@ -103,7 +103,7 @@ void bch_count_io_errors(struct cache *ca, int error, const char *m)
 }
 
 void bch_bbio_count_io_errors(struct cache_set *c, struct bio *bio,
-			      int error, const char *m)
+			      blk_status_t error, const char *m)
 {
 	struct bbio *b = container_of(bio, struct bbio, bio);
 	struct cache *ca = PTR_CACHE(c, &b->key, 0);
@@ -132,7 +132,7 @@ void bch_bbio_count_io_errors(struct cache_set *c, struct bio *bio,
 }
 
 void bch_bbio_endio(struct cache_set *c, struct bio *bio,
-		    int error, const char *m)
+		    blk_status_t error, const char *m)
 {
 	struct closure *cl = bio->bi_private;
 
diff --git a/drivers/md/bcache/journal.c b/drivers/md/bcache/journal.c
index 1198e53d5670..0352d05e495c 100644
--- a/drivers/md/bcache/journal.c
+++ b/drivers/md/bcache/journal.c
@@ -549,7 +549,7 @@ static void journal_write_endio(struct bio *bio)
 {
 	struct journal_write *w = bio->bi_private;
 
-	cache_set_err_on(bio->bi_error, w->c, "journal io error");
+	cache_set_err_on(bio->bi_status, w->c, "journal io error");
 	closure_put(&w->c->journal.io);
 }
 
diff --git a/drivers/md/bcache/movinggc.c b/drivers/md/bcache/movinggc.c
index 13b8a907006d..f633b30c962e 100644
--- a/drivers/md/bcache/movinggc.c
+++ b/drivers/md/bcache/movinggc.c
@@ -63,14 +63,14 @@ static void read_moving_endio(struct bio *bio)
 	struct moving_io *io = container_of(bio->bi_private,
 					    struct moving_io, cl);
 
-	if (bio->bi_error)
-		io->op.error = bio->bi_error;
+	if (bio->bi_status)
+		io->op.status = bio->bi_status;
 	else if (!KEY_DIRTY(&b->key) &&
 		 ptr_stale(io->op.c, &b->key, 0)) {
-		io->op.error = -EINTR;
+		io->op.status = BLK_STS_IOERR;
 	}
 
-	bch_bbio_endio(io->op.c, bio, bio->bi_error, "reading data to move");
+	bch_bbio_endio(io->op.c, bio, bio->bi_status, "reading data to move");
 }
 
 static void moving_init(struct moving_io *io)
@@ -92,7 +92,7 @@ static void write_moving(struct closure *cl)
 	struct moving_io *io = container_of(cl, struct moving_io, cl);
 	struct data_insert_op *op = &io->op;
 
-	if (!op->error) {
+	if (!op->status) {
 		moving_init(io);
 
 		io->bio.bio.bi_iter.bi_sector = KEY_START(&io->w->key);
diff --git a/drivers/md/bcache/request.c b/drivers/md/bcache/request.c
index 709c9cc34369..019b3df9f1c6 100644
--- a/drivers/md/bcache/request.c
+++ b/drivers/md/bcache/request.c
@@ -81,7 +81,7 @@ static void bch_data_insert_keys(struct closure *cl)
 	if (ret == -ESRCH) {
 		op->replace_collision = true;
 	} else if (ret) {
-		op->error		= -ENOMEM;
+		op->status		= BLK_STS_RESOURCE;
 		op->insert_data_done	= true;
 	}
 
@@ -178,17 +178,17 @@ static void bch_data_insert_endio(struct bio *bio)
 	struct closure *cl = bio->bi_private;
 	struct data_insert_op *op = container_of(cl, struct data_insert_op, cl);
 
-	if (bio->bi_error) {
+	if (bio->bi_status) {
 		/* TODO: We could try to recover from this. */
 		if (op->writeback)
-			op->error = bio->bi_error;
+			op->status = bio->bi_status;
 		else if (!op->replace)
 			set_closure_fn(cl, bch_data_insert_error, op->wq);
 		else
 			set_closure_fn(cl, NULL, NULL);
 	}
 
-	bch_bbio_endio(op->c, bio, bio->bi_error, "writing data to cache");
+	bch_bbio_endio(op->c, bio, bio->bi_status, "writing data to cache");
 }
 
 static void bch_data_insert_start(struct closure *cl)
@@ -488,15 +488,15 @@ static void bch_cache_read_endio(struct bio *bio)
 	 * from the backing device.
 	 */
 
-	if (bio->bi_error)
-		s->iop.error = bio->bi_error;
+	if (bio->bi_status)
+		s->iop.status = bio->bi_status;
 	else if (!KEY_DIRTY(&b->key) &&
 		 ptr_stale(s->iop.c, &b->key, 0)) {
 		atomic_long_inc(&s->iop.c->cache_read_races);
-		s->iop.error = -EINTR;
+		s->iop.status = BLK_STS_IOERR;
 	}
 
-	bch_bbio_endio(s->iop.c, bio, bio->bi_error, "reading from cache");
+	bch_bbio_endio(s->iop.c, bio, bio->bi_status, "reading from cache");
 }
 
 /*
@@ -593,9 +593,9 @@ static void request_endio(struct bio *bio)
 {
 	struct closure *cl = bio->bi_private;
 
-	if (bio->bi_error) {
+	if (bio->bi_status) {
 		struct search *s = container_of(cl, struct search, cl);
-		s->iop.error = bio->bi_error;
+		s->iop.status = bio->bi_status;
 		/* Only cache read errors are recoverable */
 		s->recoverable = false;
 	}
@@ -611,7 +611,7 @@ static void bio_complete(struct search *s)
 				    &s->d->disk->part0, s->start_time);
 
 		trace_bcache_request_end(s->d, s->orig_bio);
-		s->orig_bio->bi_error = s->iop.error;
+		s->orig_bio->bi_status = s->iop.status;
 		bio_endio(s->orig_bio);
 		s->orig_bio = NULL;
 	}
@@ -664,7 +664,7 @@ static inline struct search *search_alloc(struct bio *bio,
 	s->iop.inode		= d->id;
 	s->iop.write_point	= hash_long((unsigned long) current, 16);
 	s->iop.write_prio	= 0;
-	s->iop.error		= 0;
+	s->iop.status		= 0;
 	s->iop.flags		= 0;
 	s->iop.flush_journal	= op_is_flush(bio->bi_opf);
 	s->iop.wq		= bcache_wq;
@@ -707,7 +707,7 @@ static void cached_dev_read_error(struct closure *cl)
 		/* Retry from the backing device: */
 		trace_bcache_read_retry(s->orig_bio);
 
-		s->iop.error = 0;
+		s->iop.status = 0;
 		do_bio_hook(s, s->orig_bio);
 
 		/* XXX: invalidate cache */
@@ -767,7 +767,7 @@ static void cached_dev_read_done_bh(struct closure *cl)
 				  !s->cache_miss, s->iop.bypass);
 	trace_bcache_read(s->orig_bio, !s->cache_miss, s->iop.bypass);
 
-	if (s->iop.error)
+	if (s->iop.status)
 		continue_at_nobarrier(cl, cached_dev_read_error, bcache_wq);
 	else if (s->iop.bio || verify(dc, &s->bio.bio))
 		continue_at_nobarrier(cl, cached_dev_read_done, bcache_wq);
diff --git a/drivers/md/bcache/request.h b/drivers/md/bcache/request.h
index 1ff36875c2b3..7689176951ce 100644
--- a/drivers/md/bcache/request.h
+++ b/drivers/md/bcache/request.h
@@ -10,7 +10,7 @@ struct data_insert_op {
 	unsigned		inode;
 	uint16_t		write_point;
 	uint16_t		write_prio;
-	short			error;
+	blk_status_t		status;
 
 	union {
 		uint16_t	flags;
diff --git a/drivers/md/bcache/super.c b/drivers/md/bcache/super.c
index e57353e39168..8352fad765f6 100644
--- a/drivers/md/bcache/super.c
+++ b/drivers/md/bcache/super.c
@@ -271,7 +271,7 @@ static void write_super_endio(struct bio *bio)
 {
 	struct cache *ca = bio->bi_private;
 
-	bch_count_io_errors(ca, bio->bi_error, "writing superblock");
+	bch_count_io_errors(ca, bio->bi_status, "writing superblock");
 	closure_put(&ca->set->sb_write);
 }
 
@@ -321,7 +321,7 @@ static void uuid_endio(struct bio *bio)
 	struct closure *cl = bio->bi_private;
 	struct cache_set *c = container_of(cl, struct cache_set, uuid_write);
 
-	cache_set_err_on(bio->bi_error, c, "accessing uuids");
+	cache_set_err_on(bio->bi_status, c, "accessing uuids");
 	bch_bbio_free(bio, c);
 	closure_put(cl);
 }
@@ -494,7 +494,7 @@ static void prio_endio(struct bio *bio)
 {
 	struct cache *ca = bio->bi_private;
 
-	cache_set_err_on(bio->bi_error, ca->set, "accessing priorities");
+	cache_set_err_on(bio->bi_status, ca->set, "accessing priorities");
 	bch_bbio_free(bio, ca->set);
 	closure_put(&ca->prio);
 }
@@ -782,7 +782,9 @@ static int bcache_device_init(struct bcache_device *d, unsigned block_size,
 
 	minor *= BCACHE_MINORS;
 
-	if (!(d->bio_split = bioset_create(4, offsetof(struct bbio, bio))) ||
+	if (!(d->bio_split = bioset_create(4, offsetof(struct bbio, bio),
+					   BIOSET_NEED_BVECS |
+					   BIOSET_NEED_RESCUER)) ||
 	    !(d->disk = alloc_disk(BCACHE_MINORS))) {
 		ida_simple_remove(&bcache_minor, minor);
 		return -ENOMEM;
@@ -1516,7 +1518,9 @@ struct cache_set *bch_cache_set_alloc(struct cache_sb *sb)
 				sizeof(struct bbio) + sizeof(struct bio_vec) *
 				bucket_pages(c))) ||
 	    !(c->fill_iter = mempool_create_kmalloc_pool(1, iter_size)) ||
-	    !(c->bio_split = bioset_create(4, offsetof(struct bbio, bio))) ||
+	    !(c->bio_split = bioset_create(4, offsetof(struct bbio, bio),
+					   BIOSET_NEED_BVECS |
+					   BIOSET_NEED_RESCUER)) ||
 	    !(c->uuids = alloc_bucket_pages(GFP_KERNEL, c)) ||
 	    !(c->moving_gc_wq = alloc_workqueue("bcache_gc",
 						WQ_MEM_RECLAIM, 0)) ||
diff --git a/drivers/md/bcache/writeback.c b/drivers/md/bcache/writeback.c
index 6ac2e48b9235..42c66e76f05e 100644
--- a/drivers/md/bcache/writeback.c
+++ b/drivers/md/bcache/writeback.c
@@ -167,7 +167,7 @@ static void dirty_endio(struct bio *bio)
 	struct keybuf_key *w = bio->bi_private;
 	struct dirty_io *io = w->private;
 
-	if (bio->bi_error)
+	if (bio->bi_status)
 		SET_KEY_DIRTY(&w->key, false);
 
 	closure_put(&io->cl);
@@ -195,7 +195,7 @@ static void read_dirty_endio(struct bio *bio)
 	struct dirty_io *io = w->private;
 
 	bch_count_io_errors(PTR_CACHE(io->dc->disk.c, &w->key, 0),
-			    bio->bi_error, "reading dirty data from cache");
+			    bio->bi_status, "reading dirty data from cache");
 
 	dirty_endio(bio);
 }
diff --git a/drivers/md/dm-bio-prison-v1.c b/drivers/md/dm-bio-prison-v1.c
index ae7da2c30a57..874841f0fc83 100644
--- a/drivers/md/dm-bio-prison-v1.c
+++ b/drivers/md/dm-bio-prison-v1.c
@@ -116,7 +116,7 @@ static int __bio_detain(struct dm_bio_prison *prison,
 
 	while (*new) {
 		struct dm_bio_prison_cell *cell =
-			container_of(*new, struct dm_bio_prison_cell, node);
+			rb_entry(*new, struct dm_bio_prison_cell, node);
 
 		r = cmp_keys(key, &cell->key);
 
@@ -229,7 +229,7 @@ void dm_cell_release_no_holder(struct dm_bio_prison *prison,
 EXPORT_SYMBOL_GPL(dm_cell_release_no_holder);
 
 void dm_cell_error(struct dm_bio_prison *prison,
-		   struct dm_bio_prison_cell *cell, int error)
+		   struct dm_bio_prison_cell *cell, blk_status_t error)
 {
 	struct bio_list bios;
 	struct bio *bio;
@@ -238,7 +238,7 @@ void dm_cell_error(struct dm_bio_prison *prison,
 	dm_cell_release(prison, cell, &bios);
 
 	while ((bio = bio_list_pop(&bios))) {
-		bio->bi_error = error;
+		bio->bi_status = error;
 		bio_endio(bio);
 	}
 }
diff --git a/drivers/md/dm-bio-prison-v1.h b/drivers/md/dm-bio-prison-v1.h
index cddd4ac07e2c..cec52ac5e1ae 100644
--- a/drivers/md/dm-bio-prison-v1.h
+++ b/drivers/md/dm-bio-prison-v1.h
@@ -91,7 +91,7 @@ void dm_cell_release_no_holder(struct dm_bio_prison *prison,
 			       struct dm_bio_prison_cell *cell,
 			       struct bio_list *inmates);
 void dm_cell_error(struct dm_bio_prison *prison,
-		   struct dm_bio_prison_cell *cell, int error);
+		   struct dm_bio_prison_cell *cell, blk_status_t error);
 
 /*
  * Visits the cell and then releases.  Guarantees no new inmates are
diff --git a/drivers/md/dm-bio-prison-v2.c b/drivers/md/dm-bio-prison-v2.c
index c9b11f799cd8..8ce3a1a588cf 100644
--- a/drivers/md/dm-bio-prison-v2.c
+++ b/drivers/md/dm-bio-prison-v2.c
@@ -120,7 +120,7 @@ static bool __find_or_insert(struct dm_bio_prison_v2 *prison,
 
 	while (*new) {
 		struct dm_bio_prison_cell_v2 *cell =
-			container_of(*new, struct dm_bio_prison_cell_v2, node);
+			rb_entry(*new, struct dm_bio_prison_cell_v2, node);
 
 		r = cmp_keys(key, &cell->key);
 
diff --git a/drivers/md/dm-bufio.c b/drivers/md/dm-bufio.c
index 840c1496b2b1..850ff6c67994 100644
--- a/drivers/md/dm-bufio.c
+++ b/drivers/md/dm-bufio.c
@@ -145,8 +145,8 @@ struct dm_buffer {
 	enum data_mode data_mode;
 	unsigned char list_mode;		/* LIST_* */
 	unsigned hold_count;
-	int read_error;
-	int write_error;
+	blk_status_t read_error;
+	blk_status_t write_error;
 	unsigned long state;
 	unsigned long last_accessed;
 	struct dm_bufio_client *c;
@@ -555,7 +555,7 @@ static void dmio_complete(unsigned long error, void *context)
 {
 	struct dm_buffer *b = context;
 
-	b->bio.bi_error = error ? -EIO : 0;
+	b->bio.bi_status = error ? BLK_STS_IOERR : 0;
 	b->bio.bi_end_io(&b->bio);
 }
 
@@ -588,7 +588,7 @@ static void use_dmio(struct dm_buffer *b, int rw, sector_t sector,
 
 	r = dm_io(&io_req, 1, &region, NULL);
 	if (r) {
-		b->bio.bi_error = r;
+		b->bio.bi_status = errno_to_blk_status(r);
 		end_io(&b->bio);
 	}
 }
@@ -596,7 +596,7 @@ static void use_dmio(struct dm_buffer *b, int rw, sector_t sector,
 static void inline_endio(struct bio *bio)
 {
 	bio_end_io_t *end_fn = bio->bi_private;
-	int error = bio->bi_error;
+	blk_status_t status = bio->bi_status;
 
 	/*
 	 * Reset the bio to free any attached resources
@@ -604,7 +604,7 @@ static void inline_endio(struct bio *bio)
 	 */
 	bio_reset(bio);
 
-	bio->bi_error = error;
+	bio->bi_status = status;
 	end_fn(bio);
 }
 
@@ -685,11 +685,12 @@ static void write_endio(struct bio *bio)
 {
 	struct dm_buffer *b = container_of(bio, struct dm_buffer, bio);
 
-	b->write_error = bio->bi_error;
-	if (unlikely(bio->bi_error)) {
+	b->write_error = bio->bi_status;
+	if (unlikely(bio->bi_status)) {
 		struct dm_bufio_client *c = b->c;
-		int error = bio->bi_error;
-		(void)cmpxchg(&c->async_write_error, 0, error);
+
+		(void)cmpxchg(&c->async_write_error, 0,
+				blk_status_to_errno(bio->bi_status));
 	}
 
 	BUG_ON(!test_bit(B_WRITING, &b->state));
@@ -1063,7 +1064,7 @@ static void read_endio(struct bio *bio)
 {
 	struct dm_buffer *b = container_of(bio, struct dm_buffer, bio);
 
-	b->read_error = bio->bi_error;
+	b->read_error = bio->bi_status;
 
 	BUG_ON(!test_bit(B_READING, &b->state));
 
@@ -1107,7 +1108,7 @@ static void *new_read(struct dm_bufio_client *c, sector_t block,
 	wait_on_bit_io(&b->state, B_READING, TASK_UNINTERRUPTIBLE);
 
 	if (b->read_error) {
-		int error = b->read_error;
+		int error = blk_status_to_errno(b->read_error);
 
 		dm_bufio_release(b);
 
@@ -1257,7 +1258,8 @@ EXPORT_SYMBOL_GPL(dm_bufio_write_dirty_buffers_async);
  */
 int dm_bufio_write_dirty_buffers(struct dm_bufio_client *c)
 {
-	int a, f;
+	blk_status_t a;
+	int f;
 	unsigned long buffers_processed = 0;
 	struct dm_buffer *b, *tmp;
 
diff --git a/drivers/md/dm-cache-target.c b/drivers/md/dm-cache-target.c
index d682a0511381..c5ea03fc7ee1 100644
--- a/drivers/md/dm-cache-target.c
+++ b/drivers/md/dm-cache-target.c
@@ -119,7 +119,7 @@ static void iot_io_end(struct io_tracker *iot, sector_t len)
  */
 struct continuation {
 	struct work_struct ws;
-	int input;
+	blk_status_t input;
 };
 
 static inline void init_continuation(struct continuation *k,
@@ -145,7 +145,7 @@ struct batcher {
 	/*
 	 * The operation that everyone is waiting for.
 	 */
-	int (*commit_op)(void *context);
+	blk_status_t (*commit_op)(void *context);
 	void *commit_context;
 
 	/*
@@ -171,8 +171,7 @@ struct batcher {
 static void __commit(struct work_struct *_ws)
 {
 	struct batcher *b = container_of(_ws, struct batcher, commit_work);
-
-	int r;
+	blk_status_t r;
 	unsigned long flags;
 	struct list_head work_items;
 	struct work_struct *ws, *tmp;
@@ -205,7 +204,7 @@ static void __commit(struct work_struct *_ws)
 
 	while ((bio = bio_list_pop(&bios))) {
 		if (r) {
-			bio->bi_error = r;
+			bio->bi_status = r;
 			bio_endio(bio);
 		} else
 			b->issue_op(bio, b->issue_context);
@@ -213,7 +212,7 @@ static void __commit(struct work_struct *_ws)
 }
 
 static void batcher_init(struct batcher *b,
-			 int (*commit_op)(void *),
+			 blk_status_t (*commit_op)(void *),
 			 void *commit_context,
 			 void (*issue_op)(struct bio *bio, void *),
 			 void *issue_context,
@@ -955,7 +954,7 @@ static void writethrough_endio(struct bio *bio)
 
 	dm_unhook_bio(&pb->hook_info, bio);
 
-	if (bio->bi_error) {
+	if (bio->bi_status) {
 		bio_endio(bio);
 		return;
 	}
@@ -1220,7 +1219,7 @@ static void copy_complete(int read_err, unsigned long write_err, void *context)
 	struct dm_cache_migration *mg = container_of(context, struct dm_cache_migration, k);
 
 	if (read_err || write_err)
-		mg->k.input = -EIO;
+		mg->k.input = BLK_STS_IOERR;
 
 	queue_continuation(mg->cache->wq, &mg->k);
 }
@@ -1266,8 +1265,8 @@ static void overwrite_endio(struct bio *bio)
 
 	dm_unhook_bio(&pb->hook_info, bio);
 
-	if (bio->bi_error)
-		mg->k.input = bio->bi_error;
+	if (bio->bi_status)
+		mg->k.input = bio->bi_status;
 
 	queue_continuation(mg->cache->wq, &mg->k);
 }
@@ -1323,8 +1322,10 @@ static void mg_complete(struct dm_cache_migration *mg, bool success)
 		if (mg->overwrite_bio) {
 			if (success)
 				force_set_dirty(cache, cblock);
+			else if (mg->k.input)
+				mg->overwrite_bio->bi_status = mg->k.input;
 			else
-				mg->overwrite_bio->bi_error = (mg->k.input ? : -EIO);
+				mg->overwrite_bio->bi_status = BLK_STS_IOERR;
 			bio_endio(mg->overwrite_bio);
 		} else {
 			if (success)
@@ -1504,7 +1505,7 @@ static void mg_copy(struct work_struct *ws)
 		r = copy(mg, is_policy_promote);
 		if (r) {
 			DMERR_LIMIT("%s: migration copy failed", cache_device_name(cache));
-			mg->k.input = -EIO;
+			mg->k.input = BLK_STS_IOERR;
 			mg_complete(mg, false);
 		}
 	}
@@ -1907,12 +1908,12 @@ static int commit(struct cache *cache, bool clean_shutdown)
 /*
  * Used by the batcher.
  */
-static int commit_op(void *context)
+static blk_status_t commit_op(void *context)
 {
 	struct cache *cache = context;
 
 	if (dm_cache_changed_this_transaction(cache->cmd))
-		return commit(cache, false);
+		return errno_to_blk_status(commit(cache, false));
 
 	return 0;
 }
@@ -2018,7 +2019,7 @@ static void requeue_deferred_bios(struct cache *cache)
 	bio_list_init(&cache->deferred_bios);
 
 	while ((bio = bio_list_pop(&bios))) {
-		bio->bi_error = DM_ENDIO_REQUEUE;
+		bio->bi_status = BLK_STS_DM_REQUEUE;
 		bio_endio(bio);
 	}
 }
@@ -2820,7 +2821,8 @@ static int cache_map(struct dm_target *ti, struct bio *bio)
 	return r;
 }
 
-static int cache_end_io(struct dm_target *ti, struct bio *bio, int error)
+static int cache_end_io(struct dm_target *ti, struct bio *bio,
+		blk_status_t *error)
 {
 	struct cache *cache = ti->private;
 	unsigned long flags;
@@ -2838,7 +2840,7 @@ static int cache_end_io(struct dm_target *ti, struct bio *bio, int error)
 	bio_drop_shared_lock(cache, bio);
 	accounted_complete(cache, bio);
 
-	return 0;
+	return DM_ENDIO_DONE;
 }
 
 static int write_dirty_bitset(struct cache *cache)
diff --git a/drivers/md/dm-core.h b/drivers/md/dm-core.h
index 52ca8d059e82..24eddbdf2ab4 100644
--- a/drivers/md/dm-core.h
+++ b/drivers/md/dm-core.h
@@ -147,4 +147,7 @@ static inline bool dm_message_test_buffer_overflow(char *result, unsigned maxlen
 	return !maxlen || strlen(result) + 1 >= maxlen;
 }
 
+extern atomic_t dm_global_event_nr;
+extern wait_queue_head_t dm_global_eventq;
+
 #endif
diff --git a/drivers/md/dm-crypt.c b/drivers/md/dm-crypt.c
index ebf9e72d479b..cdf6b1e12460 100644
--- a/drivers/md/dm-crypt.c
+++ b/drivers/md/dm-crypt.c
@@ -71,7 +71,7 @@ struct dm_crypt_io {
 	struct convert_context ctx;
 
 	atomic_t io_pending;
-	int error;
+	blk_status_t error;
 	sector_t sector;
 
 	struct rb_node rb_node;
@@ -246,6 +246,9 @@ static struct crypto_aead *any_tfm_aead(struct crypt_config *cc)
  * plain64: the initial vector is the 64-bit little-endian version of the sector
  *        number, padded with zeros if necessary.
  *
+ * plain64be: the initial vector is the 64-bit big-endian version of the sector
+ *        number, padded with zeros if necessary.
+ *
  * essiv: "encrypted sector|salt initial vector", the sector number is
  *        encrypted with the bulk cipher using a salt as key. The salt
  *        should be derived from the bulk cipher's key via hashing.
@@ -302,6 +305,16 @@ static int crypt_iv_plain64_gen(struct crypt_config *cc, u8 *iv,
 	return 0;
 }
 
+static int crypt_iv_plain64be_gen(struct crypt_config *cc, u8 *iv,
+				  struct dm_crypt_request *dmreq)
+{
+	memset(iv, 0, cc->iv_size);
+	/* iv_size is at least of size u64; usually it is 16 bytes */
+	*(__be64 *)&iv[cc->iv_size - sizeof(u64)] = cpu_to_be64(dmreq->iv_sector);
+
+	return 0;
+}
+
 /* Initialise ESSIV - compute salt but no local memory allocations */
 static int crypt_iv_essiv_init(struct crypt_config *cc)
 {
@@ -835,6 +848,10 @@ static const struct crypt_iv_operations crypt_iv_plain64_ops = {
 	.generator = crypt_iv_plain64_gen
 };
 
+static const struct crypt_iv_operations crypt_iv_plain64be_ops = {
+	.generator = crypt_iv_plain64be_gen
+};
+
 static const struct crypt_iv_operations crypt_iv_essiv_ops = {
 	.ctr       = crypt_iv_essiv_ctr,
 	.dtr       = crypt_iv_essiv_dtr,
@@ -1292,7 +1309,7 @@ static void crypt_free_req(struct crypt_config *cc, void *req, struct bio *base_
 /*
  * Encrypt / decrypt data from one bio to another one (can be the same one)
  */
-static int crypt_convert(struct crypt_config *cc,
+static blk_status_t crypt_convert(struct crypt_config *cc,
 			 struct convert_context *ctx)
 {
 	unsigned int tag_offset = 0;
@@ -1343,13 +1360,13 @@ static int crypt_convert(struct crypt_config *cc,
 		 */
 		case -EBADMSG:
 			atomic_dec(&ctx->cc_pending);
-			return -EILSEQ;
+			return BLK_STS_PROTECTION;
 		/*
 		 * There was an error while processing the request.
 		 */
 		default:
 			atomic_dec(&ctx->cc_pending);
-			return -EIO;
+			return BLK_STS_IOERR;
 		}
 	}
 
@@ -1463,7 +1480,7 @@ static void crypt_dec_pending(struct dm_crypt_io *io)
 {
 	struct crypt_config *cc = io->cc;
 	struct bio *base_bio = io->base_bio;
-	int error = io->error;
+	blk_status_t error = io->error;
 
 	if (!atomic_dec_and_test(&io->io_pending))
 		return;
@@ -1476,7 +1493,7 @@ static void crypt_dec_pending(struct dm_crypt_io *io)
 	else
 		kfree(io->integrity_metadata);
 
-	base_bio->bi_error = error;
+	base_bio->bi_status = error;
 	bio_endio(base_bio);
 }
 
@@ -1502,7 +1519,7 @@ static void crypt_endio(struct bio *clone)
 	struct dm_crypt_io *io = clone->bi_private;
 	struct crypt_config *cc = io->cc;
 	unsigned rw = bio_data_dir(clone);
-	int error;
+	blk_status_t error;
 
 	/*
 	 * free the processed pages
@@ -1510,7 +1527,7 @@ static void crypt_endio(struct bio *clone)
 	if (rw == WRITE)
 		crypt_free_buffer_pages(cc, clone);
 
-	error = clone->bi_error;
+	error = clone->bi_status;
 	bio_put(clone);
 
 	if (rw == READ && !error) {
@@ -1570,7 +1587,7 @@ static void kcryptd_io_read_work(struct work_struct *work)
 
 	crypt_inc_pending(io);
 	if (kcryptd_io_read(io, GFP_NOIO))
-		io->error = -ENOMEM;
+		io->error = BLK_STS_RESOURCE;
 	crypt_dec_pending(io);
 }
 
@@ -1656,7 +1673,7 @@ static void kcryptd_crypt_write_io_submit(struct dm_crypt_io *io, int async)
 	sector_t sector;
 	struct rb_node **rbp, *parent;
 
-	if (unlikely(io->error < 0)) {
+	if (unlikely(io->error)) {
 		crypt_free_buffer_pages(cc, clone);
 		bio_put(clone);
 		crypt_dec_pending(io);
@@ -1697,7 +1714,7 @@ static void kcryptd_crypt_write_convert(struct dm_crypt_io *io)
 	struct bio *clone;
 	int crypt_finished;
 	sector_t sector = io->sector;
-	int r;
+	blk_status_t r;
 
 	/*
 	 * Prevent io from disappearing until this function completes.
@@ -1707,7 +1724,7 @@ static void kcryptd_crypt_write_convert(struct dm_crypt_io *io)
 
 	clone = crypt_alloc_buffer(io, io->base_bio->bi_iter.bi_size);
 	if (unlikely(!clone)) {
-		io->error = -EIO;
+		io->error = BLK_STS_IOERR;
 		goto dec;
 	}
 
@@ -1718,7 +1735,7 @@ static void kcryptd_crypt_write_convert(struct dm_crypt_io *io)
 
 	crypt_inc_pending(io);
 	r = crypt_convert(cc, &io->ctx);
-	if (r < 0)
+	if (r)
 		io->error = r;
 	crypt_finished = atomic_dec_and_test(&io->ctx.cc_pending);
 
@@ -1740,7 +1757,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->cc;
-	int r = 0;
+	blk_status_t r;
 
 	crypt_inc_pending(io);
 
@@ -1748,7 +1765,7 @@ static void kcryptd_crypt_read_convert(struct dm_crypt_io *io)
 			   io->sector);
 
 	r = crypt_convert(cc, &io->ctx);
-	if (r < 0)
+	if (r)
 		io->error = r;
 
 	if (atomic_dec_and_test(&io->ctx.cc_pending))
@@ -1781,9 +1798,9 @@ static void kcryptd_async_done(struct crypto_async_request *async_req,
 	if (error == -EBADMSG) {
 		DMERR_LIMIT("INTEGRITY AEAD ERROR, sector %llu",
 			    (unsigned long long)le64_to_cpu(*org_sector_of_dmreq(cc, dmreq)));
-		io->error = -EILSEQ;
+		io->error = BLK_STS_PROTECTION;
 	} else if (error < 0)
-		io->error = -EIO;
+		io->error = BLK_STS_IOERR;
 
 	crypt_free_req(cc, req_of_dmreq(cc, dmreq), io->base_bio);
 
@@ -2208,6 +2225,8 @@ static int crypt_ctr_ivmode(struct dm_target *ti, const char *ivmode)
 		cc->iv_gen_ops = &crypt_iv_plain_ops;
 	else if (strcmp(ivmode, "plain64") == 0)
 		cc->iv_gen_ops = &crypt_iv_plain64_ops;
+	else if (strcmp(ivmode, "plain64be") == 0)
+		cc->iv_gen_ops = &crypt_iv_plain64be_ops;
 	else if (strcmp(ivmode, "essiv") == 0)
 		cc->iv_gen_ops = &crypt_iv_essiv_ops;
 	else if (strcmp(ivmode, "benbi") == 0)
@@ -2677,7 +2696,8 @@ static int crypt_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 		goto bad;
 	}
 
-	cc->bs = bioset_create(MIN_IOS, 0);
+	cc->bs = bioset_create(MIN_IOS, 0, (BIOSET_NEED_BVECS |
+					    BIOSET_NEED_RESCUER));
 	if (!cc->bs) {
 		ti->error = "Cannot allocate crypt bioset";
 		goto bad;
@@ -2795,10 +2815,10 @@ static int crypt_map(struct dm_target *ti, struct bio *bio)
 	 * and is aligned to this size as defined in IO hints.
 	 */
 	if (unlikely((bio->bi_iter.bi_sector & ((cc->sector_size >> SECTOR_SHIFT) - 1)) != 0))
-		return -EIO;
+		return DM_MAPIO_KILL;
 
 	if (unlikely(bio->bi_iter.bi_size & (cc->sector_size - 1)))
-		return -EIO;
+		return DM_MAPIO_KILL;
 
 	io = dm_per_bio_data(bio, cc->per_bio_data_size);
 	crypt_io_init(io, cc, bio, dm_target_offset(ti, bio->bi_iter.bi_sector));
@@ -2986,7 +3006,7 @@ static void crypt_io_hints(struct dm_target *ti, struct queue_limits *limits)
 
 static struct target_type crypt_target = {
 	.name   = "crypt",
-	.version = {1, 17, 0},
+	.version = {1, 18, 0},
 	.module = THIS_MODULE,
 	.ctr    = crypt_ctr,
 	.dtr    = crypt_dtr,
diff --git a/drivers/md/dm-flakey.c b/drivers/md/dm-flakey.c
index 13305a182611..e2c7234931bc 100644
--- a/drivers/md/dm-flakey.c
+++ b/drivers/md/dm-flakey.c
@@ -275,7 +275,7 @@ static void flakey_map_bio(struct dm_target *ti, struct bio *bio)
 	struct flakey_c *fc = ti->private;
 
 	bio->bi_bdev = fc->dev->bdev;
-	if (bio_sectors(bio))
+	if (bio_sectors(bio) || bio_op(bio) == REQ_OP_ZONE_RESET)
 		bio->bi_iter.bi_sector =
 			flakey_map_sector(ti, bio->bi_iter.bi_sector);
 }
@@ -306,6 +306,14 @@ static int flakey_map(struct dm_target *ti, struct bio *bio)
 	struct per_bio_data *pb = dm_per_bio_data(bio, sizeof(struct per_bio_data));
 	pb->bio_submitted = false;
 
+	/* Do not fail reset zone */
+	if (bio_op(bio) == REQ_OP_ZONE_RESET)
+		goto map_bio;
+
+	/* We need to remap reported zones, so remember the BIO iter */
+	if (bio_op(bio) == REQ_OP_ZONE_REPORT)
+		goto map_bio;
+
 	/* Are we alive ? */
 	elapsed = (jiffies - fc->start_time) / HZ;
 	if (elapsed % (fc->up_interval + fc->down_interval) >= fc->up_interval) {
@@ -321,7 +329,7 @@ static int flakey_map(struct dm_target *ti, struct bio *bio)
 		if (bio_data_dir(bio) == READ) {
 			if (!fc->corrupt_bio_byte && !test_bit(DROP_WRITES, &fc->flags) &&
 			    !test_bit(ERROR_WRITES, &fc->flags))
-				return -EIO;
+				return DM_MAPIO_KILL;
 			goto map_bio;
 		}
 
@@ -349,7 +357,7 @@ static int flakey_map(struct dm_target *ti, struct bio *bio)
 		/*
 		 * By default, error all I/O.
 		 */
-		return -EIO;
+		return DM_MAPIO_KILL;
 	}
 
 map_bio:
@@ -358,12 +366,21 @@ map_bio:
 	return DM_MAPIO_REMAPPED;
 }
 
-static int flakey_end_io(struct dm_target *ti, struct bio *bio, int error)
+static int flakey_end_io(struct dm_target *ti, struct bio *bio,
+			 blk_status_t *error)
 {
 	struct flakey_c *fc = ti->private;
 	struct per_bio_data *pb = dm_per_bio_data(bio, sizeof(struct per_bio_data));
 
-	if (!error && pb->bio_submitted && (bio_data_dir(bio) == READ)) {
+	if (bio_op(bio) == REQ_OP_ZONE_RESET)
+		return DM_ENDIO_DONE;
+
+	if (bio_op(bio) == REQ_OP_ZONE_REPORT) {
+		dm_remap_zone_report(ti, bio, fc->start);
+		return DM_ENDIO_DONE;
+	}
+
+	if (!*error && pb->bio_submitted && (bio_data_dir(bio) == READ)) {
 		if (fc->corrupt_bio_byte && (fc->corrupt_bio_rw == READ) &&
 		    all_corrupt_bio_flags_match(bio, fc)) {
 			/*
@@ -377,11 +394,11 @@ static int flakey_end_io(struct dm_target *ti, struct bio *bio, int error)
 			 * Error read during the down_interval if drop_writes
 			 * and error_writes were not configured.
 			 */
-			return -EIO;
+			*error = BLK_STS_IOERR;
 		}
 	}
 
-	return error;
+	return DM_ENDIO_DONE;
 }
 
 static void flakey_status(struct dm_target *ti, status_type_t type,
@@ -445,7 +462,8 @@ static int flakey_iterate_devices(struct dm_target *ti, iterate_devices_callout_
 
 static struct target_type flakey_target = {
 	.name   = "flakey",
-	.version = {1, 4, 0},
+	.version = {1, 5, 0},
+	.features = DM_TARGET_ZONED_HM,
 	.module = THIS_MODULE,
 	.ctr    = flakey_ctr,
 	.dtr    = flakey_dtr,
diff --git a/drivers/md/dm-integrity.c b/drivers/md/dm-integrity.c
index 7910bfe50da4..1b224aa9cf15 100644
--- a/drivers/md/dm-integrity.c
+++ b/drivers/md/dm-integrity.c
@@ -246,7 +246,7 @@ struct dm_integrity_io {
 	unsigned metadata_offset;
 
 	atomic_t in_flight;
-	int bi_error;
+	blk_status_t bi_status;
 
 	struct completion *completion;
 
@@ -1105,18 +1105,21 @@ static void schedule_autocommit(struct dm_integrity_c *ic)
 static void submit_flush_bio(struct dm_integrity_c *ic, struct dm_integrity_io *dio)
 {
 	struct bio *bio;
-	spin_lock_irq(&ic->endio_wait.lock);
+	unsigned long flags;
+
+	spin_lock_irqsave(&ic->endio_wait.lock, flags);
 	bio = dm_bio_from_per_bio_data(dio, sizeof(struct dm_integrity_io));
 	bio_list_add(&ic->flush_bio_list, bio);
-	spin_unlock_irq(&ic->endio_wait.lock);
+	spin_unlock_irqrestore(&ic->endio_wait.lock, flags);
+
 	queue_work(ic->commit_wq, &ic->commit_work);
 }
 
 static void do_endio(struct dm_integrity_c *ic, struct bio *bio)
 {
 	int r = dm_integrity_failed(ic);
-	if (unlikely(r) && !bio->bi_error)
-		bio->bi_error = r;
+	if (unlikely(r) && !bio->bi_status)
+		bio->bi_status = errno_to_blk_status(r);
 	bio_endio(bio);
 }
 
@@ -1124,7 +1127,7 @@ static void do_endio_flush(struct dm_integrity_c *ic, struct dm_integrity_io *di
 {
 	struct bio *bio = dm_bio_from_per_bio_data(dio, sizeof(struct dm_integrity_io));
 
-	if (unlikely(dio->fua) && likely(!bio->bi_error) && likely(!dm_integrity_failed(ic)))
+	if (unlikely(dio->fua) && likely(!bio->bi_status) && likely(!dm_integrity_failed(ic)))
 		submit_flush_bio(ic, dio);
 	else
 		do_endio(ic, bio);
@@ -1143,9 +1146,9 @@ static void dec_in_flight(struct dm_integrity_io *dio)
 
 		bio = dm_bio_from_per_bio_data(dio, sizeof(struct dm_integrity_io));
 
-		if (unlikely(dio->bi_error) && !bio->bi_error)
-			bio->bi_error = dio->bi_error;
-		if (likely(!bio->bi_error) && unlikely(bio_sectors(bio) != dio->range.n_sectors)) {
+		if (unlikely(dio->bi_status) && !bio->bi_status)
+			bio->bi_status = dio->bi_status;
+		if (likely(!bio->bi_status) && unlikely(bio_sectors(bio) != dio->range.n_sectors)) {
 			dio->range.logical_sector += dio->range.n_sectors;
 			bio_advance(bio, dio->range.n_sectors << SECTOR_SHIFT);
 			INIT_WORK(&dio->work, integrity_bio_wait);
@@ -1319,7 +1322,7 @@ skip_io:
 	dec_in_flight(dio);
 	return;
 error:
-	dio->bi_error = r;
+	dio->bi_status = errno_to_blk_status(r);
 	dec_in_flight(dio);
 }
 
@@ -1332,7 +1335,7 @@ static int dm_integrity_map(struct dm_target *ti, struct bio *bio)
 	sector_t area, offset;
 
 	dio->ic = ic;
-	dio->bi_error = 0;
+	dio->bi_status = 0;
 
 	if (unlikely(bio->bi_opf & REQ_PREFLUSH)) {
 		submit_flush_bio(ic, dio);
@@ -1353,13 +1356,13 @@ static int dm_integrity_map(struct dm_target *ti, struct bio *bio)
 		DMERR("Too big sector number: 0x%llx + 0x%x > 0x%llx",
 		      (unsigned long long)dio->range.logical_sector, bio_sectors(bio),
 		      (unsigned long long)ic->provided_data_sectors);
-		return -EIO;
+		return DM_MAPIO_KILL;
 	}
 	if (unlikely((dio->range.logical_sector | bio_sectors(bio)) & (unsigned)(ic->sectors_per_block - 1))) {
 		DMERR("Bio not aligned on %u sectors: 0x%llx, 0x%x",
 		      ic->sectors_per_block,
 		      (unsigned long long)dio->range.logical_sector, bio_sectors(bio));
-		return -EIO;
+		return DM_MAPIO_KILL;
 	}
 
 	if (ic->sectors_per_block > 1) {
@@ -1369,7 +1372,7 @@ static int dm_integrity_map(struct dm_target *ti, struct bio *bio)
 			if (unlikely((bv.bv_offset | bv.bv_len) & ((ic->sectors_per_block << SECTOR_SHIFT) - 1))) {
 				DMERR("Bio vector (%u,%u) is not aligned on %u-sector boundary",
 					bv.bv_offset, bv.bv_len, ic->sectors_per_block);
-				return -EIO;
+				return DM_MAPIO_KILL;
 			}
 		}
 	}
@@ -1384,18 +1387,18 @@ static int dm_integrity_map(struct dm_target *ti, struct bio *bio)
 				wanted_tag_size *= ic->tag_size;
 			if (unlikely(wanted_tag_size != bip->bip_iter.bi_size)) {
 				DMERR("Invalid integrity data size %u, expected %u", bip->bip_iter.bi_size, wanted_tag_size);
-				return -EIO;
+				return DM_MAPIO_KILL;
 			}
 		}
 	} else {
 		if (unlikely(bip != NULL)) {
 			DMERR("Unexpected integrity data when using internal hash");
-			return -EIO;
+			return DM_MAPIO_KILL;
 		}
 	}
 
 	if (unlikely(ic->mode == 'R') && unlikely(dio->write))
-		return -EIO;
+		return DM_MAPIO_KILL;
 
 	get_area_and_offset(ic, dio->range.logical_sector, &area, &offset);
 	dio->metadata_block = get_metadata_sector_and_offset(ic, area, offset, &dio->metadata_offset);
@@ -3040,6 +3043,11 @@ static int dm_integrity_ctr(struct dm_target *ti, unsigned argc, char **argv)
 		ti->error = "The device is too small";
 		goto bad;
 	}
+	if (ti->len > ic->provided_data_sectors) {
+		r = -EINVAL;
+		ti->error = "Not enough provided sectors for requested mapping size";
+		goto bad;
+	}
 
 	if (!buffer_sectors)
 		buffer_sectors = 1;
diff --git a/drivers/md/dm-io.c b/drivers/md/dm-io.c
index 3702e502466d..25039607f3cb 100644
--- a/drivers/md/dm-io.c
+++ b/drivers/md/dm-io.c
@@ -58,7 +58,8 @@ struct dm_io_client *dm_io_client_create(void)
 	if (!client->pool)
 		goto bad;
 
-	client->bios = bioset_create(min_ios, 0);
+	client->bios = bioset_create(min_ios, 0, (BIOSET_NEED_BVECS |
+						  BIOSET_NEED_RESCUER));
 	if (!client->bios)
 		goto bad;
 
@@ -124,7 +125,7 @@ static void complete_io(struct io *io)
 	fn(error_bits, context);
 }
 
-static void dec_count(struct io *io, unsigned int region, int error)
+static void dec_count(struct io *io, unsigned int region, blk_status_t error)
 {
 	if (error)
 		set_bit(region, &io->error_bits);
@@ -137,9 +138,9 @@ static void endio(struct bio *bio)
 {
 	struct io *io;
 	unsigned region;
-	int error;
+	blk_status_t error;
 
-	if (bio->bi_error && bio_data_dir(bio) == READ)
+	if (bio->bi_status && bio_data_dir(bio) == READ)
 		zero_fill_bio(bio);
 
 	/*
@@ -147,7 +148,7 @@ static void endio(struct bio *bio)
 	 */
 	retrieve_io_and_region_from_bio(bio, &io, &region);
 
-	error = bio->bi_error;
+	error = bio->bi_status;
 	bio_put(bio);
 
 	dec_count(io, region, error);
@@ -317,9 +318,9 @@ static void do_region(int op, int op_flags, unsigned region,
 	else if (op == REQ_OP_WRITE_SAME)
 		special_cmd_max_sectors = q->limits.max_write_same_sectors;
 	if ((op == REQ_OP_DISCARD || op == REQ_OP_WRITE_ZEROES ||
-	     op == REQ_OP_WRITE_SAME)  &&
-	    special_cmd_max_sectors == 0) {
-		dec_count(io, region, -EOPNOTSUPP);
+	     op == REQ_OP_WRITE_SAME) && special_cmd_max_sectors == 0) {
+		atomic_inc(&io->count);
+		dec_count(io, region, BLK_STS_NOTSUPP);
 		return;
 	}
 
diff --git a/drivers/md/dm-ioctl.c b/drivers/md/dm-ioctl.c
index 41852ae287a5..e06f0ef7d2ec 100644
--- a/drivers/md/dm-ioctl.c
+++ b/drivers/md/dm-ioctl.c
@@ -23,6 +23,14 @@
 #define DM_MSG_PREFIX "ioctl"
 #define DM_DRIVER_EMAIL "dm-devel@redhat.com"
 
+struct dm_file {
+	/*
+	 * poll will wait until the global event number is greater than
+	 * this value.
+	 */
+	volatile unsigned global_event_nr;
+};
+
 /*-----------------------------------------------------------------
  * The ioctl interface needs to be able to look up devices by
  * name or uuid.
@@ -456,9 +464,9 @@ void dm_deferred_remove(void)
  * All the ioctl commands get dispatched to functions with this
  * prototype.
  */
-typedef int (*ioctl_fn)(struct dm_ioctl *param, size_t param_size);
+typedef int (*ioctl_fn)(struct file *filp, struct dm_ioctl *param, size_t param_size);
 
-static int remove_all(struct dm_ioctl *param, size_t param_size)
+static int remove_all(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	dm_hash_remove_all(true, !!(param->flags & DM_DEFERRED_REMOVE), false);
 	param->data_size = 0;
@@ -491,13 +499,14 @@ static void *get_result_buffer(struct dm_ioctl *param, size_t param_size,
 	return ((void *) param) + param->data_start;
 }
 
-static int list_devices(struct dm_ioctl *param, size_t param_size)
+static int list_devices(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	unsigned int i;
 	struct hash_cell *hc;
 	size_t len, needed = 0;
 	struct gendisk *disk;
 	struct dm_name_list *nl, *old_nl = NULL;
+	uint32_t *event_nr;
 
 	down_write(&_hash_lock);
 
@@ -510,6 +519,7 @@ static int list_devices(struct dm_ioctl *param, size_t param_size)
 			needed += sizeof(struct dm_name_list);
 			needed += strlen(hc->name) + 1;
 			needed += ALIGN_MASK;
+			needed += (sizeof(uint32_t) + ALIGN_MASK) & ~ALIGN_MASK;
 		}
 	}
 
@@ -539,7 +549,9 @@ static int list_devices(struct dm_ioctl *param, size_t param_size)
 			strcpy(nl->name, hc->name);
 
 			old_nl = nl;
-			nl = align_ptr(((void *) ++nl) + strlen(hc->name) + 1);
+			event_nr = align_ptr(((void *) (nl + 1)) + strlen(hc->name) + 1);
+			*event_nr = dm_get_event_nr(hc->md);
+			nl = align_ptr(event_nr + 1);
 		}
 	}
 
@@ -582,7 +594,7 @@ static void list_version_get_info(struct target_type *tt, void *param)
     info->vers = align_ptr(((void *) ++info->vers) + strlen(tt->name) + 1);
 }
 
-static int list_versions(struct dm_ioctl *param, size_t param_size)
+static int list_versions(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	size_t len, needed = 0;
 	struct dm_target_versions *vers;
@@ -724,7 +736,7 @@ static void __dev_status(struct mapped_device *md, struct dm_ioctl *param)
 	}
 }
 
-static int dev_create(struct dm_ioctl *param, size_t param_size)
+static int dev_create(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	int r, m = DM_ANY_MINOR;
 	struct mapped_device *md;
@@ -816,7 +828,7 @@ static struct mapped_device *find_device(struct dm_ioctl *param)
 	return md;
 }
 
-static int dev_remove(struct dm_ioctl *param, size_t param_size)
+static int dev_remove(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	struct hash_cell *hc;
 	struct mapped_device *md;
@@ -881,7 +893,7 @@ static int invalid_str(char *str, void *end)
 	return -EINVAL;
 }
 
-static int dev_rename(struct dm_ioctl *param, size_t param_size)
+static int dev_rename(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	int r;
 	char *new_data = (char *) param + param->data_start;
@@ -911,7 +923,7 @@ static int dev_rename(struct dm_ioctl *param, size_t param_size)
 	return 0;
 }
 
-static int dev_set_geometry(struct dm_ioctl *param, size_t param_size)
+static int dev_set_geometry(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	int r = -EINVAL, x;
 	struct mapped_device *md;
@@ -1060,7 +1072,7 @@ static int do_resume(struct dm_ioctl *param)
  * Set or unset the suspension state of a device.
  * If the device already is in the requested state we just return its status.
  */
-static int dev_suspend(struct dm_ioctl *param, size_t param_size)
+static int dev_suspend(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	if (param->flags & DM_SUSPEND_FLAG)
 		return do_suspend(param);
@@ -1072,7 +1084,7 @@ static int dev_suspend(struct dm_ioctl *param, size_t param_size)
  * Copies device info back to user space, used by
  * the create and info ioctls.
  */
-static int dev_status(struct dm_ioctl *param, size_t param_size)
+static int dev_status(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	struct mapped_device *md;
 
@@ -1163,7 +1175,7 @@ static void retrieve_status(struct dm_table *table,
 /*
  * Wait for a device to report an event
  */
-static int dev_wait(struct dm_ioctl *param, size_t param_size)
+static int dev_wait(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	int r = 0;
 	struct mapped_device *md;
@@ -1200,6 +1212,19 @@ out:
 	return r;
 }
 
+/*
+ * Remember the global event number and make it possible to poll
+ * for further events.
+ */
+static int dev_arm_poll(struct file *filp, struct dm_ioctl *param, size_t param_size)
+{
+	struct dm_file *priv = filp->private_data;
+
+	priv->global_event_nr = atomic_read(&dm_global_event_nr);
+
+	return 0;
+}
+
 static inline fmode_t get_mode(struct dm_ioctl *param)
 {
 	fmode_t mode = FMODE_READ | FMODE_WRITE;
@@ -1269,7 +1294,7 @@ static bool is_valid_type(enum dm_queue_mode cur, enum dm_queue_mode new)
 	return false;
 }
 
-static int table_load(struct dm_ioctl *param, size_t param_size)
+static int table_load(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	int r;
 	struct hash_cell *hc;
@@ -1356,7 +1381,7 @@ err:
 	return r;
 }
 
-static int table_clear(struct dm_ioctl *param, size_t param_size)
+static int table_clear(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	struct hash_cell *hc;
 	struct mapped_device *md;
@@ -1430,7 +1455,7 @@ static void retrieve_deps(struct dm_table *table,
 	param->data_size = param->data_start + needed;
 }
 
-static int table_deps(struct dm_ioctl *param, size_t param_size)
+static int table_deps(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	struct mapped_device *md;
 	struct dm_table *table;
@@ -1456,7 +1481,7 @@ static int table_deps(struct dm_ioctl *param, size_t param_size)
  * Return the status of a device as a text string for each
  * target.
  */
-static int table_status(struct dm_ioctl *param, size_t param_size)
+static int table_status(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	struct mapped_device *md;
 	struct dm_table *table;
@@ -1511,7 +1536,7 @@ static int message_for_md(struct mapped_device *md, unsigned argc, char **argv,
 /*
  * Pass a message to the target that's at the supplied device offset.
  */
-static int target_message(struct dm_ioctl *param, size_t param_size)
+static int target_message(struct file *filp, struct dm_ioctl *param, size_t param_size)
 {
 	int r, argc;
 	char **argv;
@@ -1628,7 +1653,8 @@ static ioctl_fn lookup_ioctl(unsigned int cmd, int *ioctl_flags)
 		{DM_LIST_VERSIONS_CMD, 0, list_versions},
 
 		{DM_TARGET_MSG_CMD, 0, target_message},
-		{DM_DEV_SET_GEOMETRY_CMD, 0, dev_set_geometry}
+		{DM_DEV_SET_GEOMETRY_CMD, 0, dev_set_geometry},
+		{DM_DEV_ARM_POLL, IOCTL_FLAGS_NO_PARAMS, dev_arm_poll},
 	};
 
 	if (unlikely(cmd >= ARRAY_SIZE(_ioctls)))
@@ -1783,7 +1809,7 @@ static int validate_params(uint cmd, struct dm_ioctl *param)
 	return 0;
 }
 
-static int ctl_ioctl(uint command, struct dm_ioctl __user *user)
+static int ctl_ioctl(struct file *file, uint command, struct dm_ioctl __user *user)
 {
 	int r = 0;
 	int ioctl_flags;
@@ -1837,7 +1863,7 @@ static int ctl_ioctl(uint command, struct dm_ioctl __user *user)
 		goto out;
 
 	param->data_size = offsetof(struct dm_ioctl, data);
-	r = fn(param, input_param_size);
+	r = fn(file, param, input_param_size);
 
 	if (unlikely(param->flags & DM_BUFFER_FULL_FLAG) &&
 	    unlikely(ioctl_flags & IOCTL_FLAGS_NO_PARAMS))
@@ -1856,7 +1882,7 @@ out:
 
 static long dm_ctl_ioctl(struct file *file, uint command, ulong u)
 {
-	return (long)ctl_ioctl(command, (struct dm_ioctl __user *)u);
+	return (long)ctl_ioctl(file, command, (struct dm_ioctl __user *)u);
 }
 
 #ifdef CONFIG_COMPAT
@@ -1868,8 +1894,47 @@ static long dm_compat_ctl_ioctl(struct file *file, uint command, ulong u)
 #define dm_compat_ctl_ioctl NULL
 #endif
 
+static int dm_open(struct inode *inode, struct file *filp)
+{
+	int r;
+	struct dm_file *priv;
+
+	r = nonseekable_open(inode, filp);
+	if (unlikely(r))
+		return r;
+
+	priv = filp->private_data = kmalloc(sizeof(struct dm_file), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->global_event_nr = atomic_read(&dm_global_event_nr);
+
+	return 0;
+}
+
+static int dm_release(struct inode *inode, struct file *filp)
+{
+	kfree(filp->private_data);
+	return 0;
+}
+
+static unsigned dm_poll(struct file *filp, poll_table *wait)
+{
+	struct dm_file *priv = filp->private_data;
+	unsigned mask = 0;
+
+	poll_wait(filp, &dm_global_eventq, wait);
+
+	if ((int)(atomic_read(&dm_global_event_nr) - priv->global_event_nr) > 0)
+		mask |= POLLIN;
+
+	return mask;
+}
+
 static const struct file_operations _ctl_fops = {
-	.open = nonseekable_open,
+	.open    = dm_open,
+	.release = dm_release,
+	.poll    = dm_poll,
 	.unlocked_ioctl	 = dm_ctl_ioctl,
 	.compat_ioctl = dm_compat_ctl_ioctl,
 	.owner	 = THIS_MODULE,
diff --git a/drivers/md/dm-kcopyd.c b/drivers/md/dm-kcopyd.c
index f85846741d50..cf2c67e35eaf 100644
--- a/drivers/md/dm-kcopyd.c
+++ b/drivers/md/dm-kcopyd.c
@@ -356,6 +356,7 @@ struct kcopyd_job {
 	struct mutex lock;
 	atomic_t sub_jobs;
 	sector_t progress;
+	sector_t write_offset;
 
 	struct kcopyd_job *master_job;
 };
@@ -386,6 +387,31 @@ void dm_kcopyd_exit(void)
  * Functions to push and pop a job onto the head of a given job
  * list.
  */
+static struct kcopyd_job *pop_io_job(struct list_head *jobs,
+				     struct dm_kcopyd_client *kc)
+{
+	struct kcopyd_job *job;
+
+	/*
+	 * For I/O jobs, pop any read, any write without sequential write
+	 * constraint and sequential writes that are at the right position.
+	 */
+	list_for_each_entry(job, jobs, list) {
+		if (job->rw == READ || !test_bit(DM_KCOPYD_WRITE_SEQ, &job->flags)) {
+			list_del(&job->list);
+			return job;
+		}
+
+		if (job->write_offset == job->master_job->write_offset) {
+			job->master_job->write_offset += job->source.count;
+			list_del(&job->list);
+			return job;
+		}
+	}
+
+	return NULL;
+}
+
 static struct kcopyd_job *pop(struct list_head *jobs,
 			      struct dm_kcopyd_client *kc)
 {
@@ -395,8 +421,12 @@ static struct kcopyd_job *pop(struct list_head *jobs,
 	spin_lock_irqsave(&kc->job_lock, flags);
 
 	if (!list_empty(jobs)) {
-		job = list_entry(jobs->next, struct kcopyd_job, list);
-		list_del(&job->list);
+		if (jobs == &kc->io_jobs)
+			job = pop_io_job(jobs, kc);
+		else {
+			job = list_entry(jobs->next, struct kcopyd_job, list);
+			list_del(&job->list);
+		}
 	}
 	spin_unlock_irqrestore(&kc->job_lock, flags);
 
@@ -506,6 +536,14 @@ static int run_io_job(struct kcopyd_job *job)
 		.client = job->kc->io_client,
 	};
 
+	/*
+	 * If we need to write sequentially and some reads or writes failed,
+	 * no point in continuing.
+	 */
+	if (test_bit(DM_KCOPYD_WRITE_SEQ, &job->flags) &&
+	    job->master_job->write_err)
+		return -EIO;
+
 	io_job_start(job->kc->throttle);
 
 	if (job->rw == READ)
@@ -655,6 +693,7 @@ static void segment_complete(int read_err, unsigned long write_err,
 		int i;
 
 		*sub_job = *job;
+		sub_job->write_offset = progress;
 		sub_job->source.sector += progress;
 		sub_job->source.count = count;
 
@@ -723,6 +762,27 @@ int dm_kcopyd_copy(struct dm_kcopyd_client *kc, struct dm_io_region *from,
 	job->num_dests = num_dests;
 	memcpy(&job->dests, dests, sizeof(*dests) * num_dests);
 
+	/*
+	 * If one of the destination is a host-managed zoned block device,
+	 * we need to write sequentially. If one of the destination is a
+	 * host-aware device, then leave it to the caller to choose what to do.
+	 */
+	if (!test_bit(DM_KCOPYD_WRITE_SEQ, &job->flags)) {
+		for (i = 0; i < job->num_dests; i++) {
+			if (bdev_zoned_model(dests[i].bdev) == BLK_ZONED_HM) {
+				set_bit(DM_KCOPYD_WRITE_SEQ, &job->flags);
+				break;
+			}
+		}
+	}
+
+	/*
+	 * If we need to write sequentially, errors cannot be ignored.
+	 */
+	if (test_bit(DM_KCOPYD_WRITE_SEQ, &job->flags) &&
+	    test_bit(DM_KCOPYD_IGNORE_ERROR, &job->flags))
+		clear_bit(DM_KCOPYD_IGNORE_ERROR, &job->flags);
+
 	if (from) {
 		job->source = *from;
 		job->pages = NULL;
@@ -746,6 +806,7 @@ int dm_kcopyd_copy(struct dm_kcopyd_client *kc, struct dm_io_region *from,
 	job->fn = fn;
 	job->context = context;
 	job->master_job = job;
+	job->write_offset = 0;
 
 	if (job->source.count <= SUB_JOB_SIZE)
 		dispatch_job(job);
diff --git a/drivers/md/dm-linear.c b/drivers/md/dm-linear.c
index 7d42a9d9f406..c03c203a90b4 100644
--- a/drivers/md/dm-linear.c
+++ b/drivers/md/dm-linear.c
@@ -89,7 +89,7 @@ static void linear_map_bio(struct dm_target *ti, struct bio *bio)
 	struct linear_c *lc = ti->private;
 
 	bio->bi_bdev = lc->dev->bdev;
-	if (bio_sectors(bio))
+	if (bio_sectors(bio) || bio_op(bio) == REQ_OP_ZONE_RESET)
 		bio->bi_iter.bi_sector =
 			linear_map_sector(ti, bio->bi_iter.bi_sector);
 }
@@ -101,6 +101,17 @@ static int linear_map(struct dm_target *ti, struct bio *bio)
 	return DM_MAPIO_REMAPPED;
 }
 
+static int linear_end_io(struct dm_target *ti, struct bio *bio,
+			 blk_status_t *error)
+{
+	struct linear_c *lc = ti->private;
+
+	if (!*error && bio_op(bio) == REQ_OP_ZONE_REPORT)
+		dm_remap_zone_report(ti, bio, lc->start);
+
+	return DM_ENDIO_DONE;
+}
+
 static void linear_status(struct dm_target *ti, status_type_t type,
 			  unsigned status_flags, char *result, unsigned maxlen)
 {
@@ -161,12 +172,13 @@ static long linear_dax_direct_access(struct dm_target *ti, pgoff_t pgoff,
 
 static struct target_type linear_target = {
 	.name   = "linear",
-	.version = {1, 3, 0},
-	.features = DM_TARGET_PASSES_INTEGRITY,
+	.version = {1, 4, 0},
+	.features = DM_TARGET_PASSES_INTEGRITY | DM_TARGET_ZONED_HM,
 	.module = THIS_MODULE,
 	.ctr    = linear_ctr,
 	.dtr    = linear_dtr,
 	.map    = linear_map,
+	.end_io = linear_end_io,
 	.status = linear_status,
 	.prepare_ioctl = linear_prepare_ioctl,
 	.iterate_devices = linear_iterate_devices,
diff --git a/drivers/md/dm-log-writes.c b/drivers/md/dm-log-writes.c
index 4dfe38655a49..a1da0eb58a93 100644
--- a/drivers/md/dm-log-writes.c
+++ b/drivers/md/dm-log-writes.c
@@ -150,10 +150,10 @@ static void log_end_io(struct bio *bio)
 {
 	struct log_writes_c *lc = bio->bi_private;
 
-	if (bio->bi_error) {
+	if (bio->bi_status) {
 		unsigned long flags;
 
-		DMERR("Error writing log block, error=%d", bio->bi_error);
+		DMERR("Error writing log block, error=%d", bio->bi_status);
 		spin_lock_irqsave(&lc->blocks_lock, flags);
 		lc->logging_enabled = false;
 		spin_unlock_irqrestore(&lc->blocks_lock, flags);
@@ -586,7 +586,7 @@ static int log_writes_map(struct dm_target *ti, struct bio *bio)
 		spin_lock_irq(&lc->blocks_lock);
 		lc->logging_enabled = false;
 		spin_unlock_irq(&lc->blocks_lock);
-		return -ENOMEM;
+		return DM_MAPIO_KILL;
 	}
 	INIT_LIST_HEAD(&block->list);
 	pb->block = block;
@@ -639,7 +639,7 @@ static int log_writes_map(struct dm_target *ti, struct bio *bio)
 			spin_lock_irq(&lc->blocks_lock);
 			lc->logging_enabled = false;
 			spin_unlock_irq(&lc->blocks_lock);
-			return -ENOMEM;
+			return DM_MAPIO_KILL;
 		}
 
 		src = kmap_atomic(bv.bv_page);
@@ -664,7 +664,8 @@ map_bio:
 	return DM_MAPIO_REMAPPED;
 }
 
-static int normal_end_io(struct dm_target *ti, struct bio *bio, int error)
+static int normal_end_io(struct dm_target *ti, struct bio *bio,
+		blk_status_t *error)
 {
 	struct log_writes_c *lc = ti->private;
 	struct per_bio_data *pb = dm_per_bio_data(bio, sizeof(struct per_bio_data));
@@ -686,7 +687,7 @@ static int normal_end_io(struct dm_target *ti, struct bio *bio, int error)
 		spin_unlock_irqrestore(&lc->blocks_lock, flags);
 	}
 
-	return error;
+	return DM_ENDIO_DONE;
 }
 
 /*
diff --git a/drivers/md/dm-mpath.c b/drivers/md/dm-mpath.c
index 3df056b73b66..0e8ab5bb3575 100644
--- a/drivers/md/dm-mpath.c
+++ b/drivers/md/dm-mpath.c
@@ -559,13 +559,13 @@ static int __multipath_map_bio(struct multipath *m, struct bio *bio, struct dm_m
 		if (test_bit(MPATHF_QUEUE_IF_NO_PATH, &m->flags))
 			return DM_MAPIO_REQUEUE;
 		dm_report_EIO(m);
-		return -EIO;
+		return DM_MAPIO_KILL;
 	}
 
 	mpio->pgpath = pgpath;
 	mpio->nr_bytes = nr_bytes;
 
-	bio->bi_error = 0;
+	bio->bi_status = 0;
 	bio->bi_bdev = pgpath->path.dev->bdev;
 	bio->bi_opf |= REQ_FAILFAST_TRANSPORT;
 
@@ -621,11 +621,19 @@ static void process_queued_bios(struct work_struct *work)
 	blk_start_plug(&plug);
 	while ((bio = bio_list_pop(&bios))) {
 		r = __multipath_map_bio(m, bio, get_mpio_from_bio(bio));
-		if (r < 0 || r == DM_MAPIO_REQUEUE) {
-			bio->bi_error = r;
+		switch (r) {
+		case DM_MAPIO_KILL:
+			bio->bi_status = BLK_STS_IOERR;
+			bio_endio(bio);
+			break;
+		case DM_MAPIO_REQUEUE:
+			bio->bi_status = BLK_STS_DM_REQUEUE;
 			bio_endio(bio);
-		} else if (r == DM_MAPIO_REMAPPED)
+			break;
+		case DM_MAPIO_REMAPPED:
 			generic_make_request(bio);
+			break;
+		}
 	}
 	blk_finish_plug(&plug);
 }
@@ -1442,22 +1450,15 @@ static void activate_path_work(struct work_struct *work)
 	activate_or_offline_path(pgpath);
 }
 
-static int noretry_error(int error)
+static int noretry_error(blk_status_t error)
 {
 	switch (error) {
-	case -EBADE:
-		/*
-		 * EBADE signals an reservation conflict.
-		 * We shouldn't fail the path here as we can communicate with
-		 * the target.  We should failover to the next path, but in
-		 * doing so we might be causing a ping-pong between paths.
-		 * So just return the reservation conflict error.
-		 */
-	case -EOPNOTSUPP:
-	case -EREMOTEIO:
-	case -EILSEQ:
-	case -ENODATA:
-	case -ENOSPC:
+	case BLK_STS_NOTSUPP:
+	case BLK_STS_NOSPC:
+	case BLK_STS_TARGET:
+	case BLK_STS_NEXUS:
+	case BLK_STS_MEDIUM:
+	case BLK_STS_RESOURCE:
 		return 1;
 	}
 
@@ -1466,7 +1467,7 @@ static int noretry_error(int error)
 }
 
 static int multipath_end_io(struct dm_target *ti, struct request *clone,
-			    int error, union map_info *map_context)
+			    blk_status_t error, union map_info *map_context)
 {
 	struct dm_mpath_io *mpio = get_mpio(map_context);
 	struct pgpath *pgpath = mpio->pgpath;
@@ -1493,7 +1494,7 @@ static int multipath_end_io(struct dm_target *ti, struct request *clone,
 
 		if (atomic_read(&m->nr_valid_paths) == 0 &&
 		    !test_bit(MPATHF_QUEUE_IF_NO_PATH, &m->flags)) {
-			if (error == -EIO)
+			if (error == BLK_STS_IOERR)
 				dm_report_EIO(m);
 			/* complete with the original error */
 			r = DM_ENDIO_DONE;
@@ -1510,24 +1511,26 @@ static int multipath_end_io(struct dm_target *ti, struct request *clone,
 	return r;
 }
 
-static int do_end_io_bio(struct multipath *m, struct bio *clone,
-			 int error, struct dm_mpath_io *mpio)
+static int multipath_end_io_bio(struct dm_target *ti, struct bio *clone,
+		blk_status_t *error)
 {
+	struct multipath *m = ti->private;
+	struct dm_mpath_io *mpio = get_mpio_from_bio(clone);
+	struct pgpath *pgpath = mpio->pgpath;
 	unsigned long flags;
+	int r = DM_ENDIO_DONE;
 
-	if (!error)
-		return 0;	/* I/O complete */
-
-	if (noretry_error(error))
-		return error;
+	if (!*error || noretry_error(*error))
+		goto done;
 
-	if (mpio->pgpath)
-		fail_path(mpio->pgpath);
+	if (pgpath)
+		fail_path(pgpath);
 
 	if (atomic_read(&m->nr_valid_paths) == 0 &&
 	    !test_bit(MPATHF_QUEUE_IF_NO_PATH, &m->flags)) {
 		dm_report_EIO(m);
-		return -EIO;
+		*error = BLK_STS_IOERR;
+		goto done;
 	}
 
 	/* Queue for the daemon to resubmit */
@@ -1539,23 +1542,11 @@ static int do_end_io_bio(struct multipath *m, struct bio *clone,
 	if (!test_bit(MPATHF_QUEUE_IO, &m->flags))
 		queue_work(kmultipathd, &m->process_queued_bios);
 
-	return DM_ENDIO_INCOMPLETE;
-}
-
-static int multipath_end_io_bio(struct dm_target *ti, struct bio *clone, int error)
-{
-	struct multipath *m = ti->private;
-	struct dm_mpath_io *mpio = get_mpio_from_bio(clone);
-	struct pgpath *pgpath;
-	struct path_selector *ps;
-	int r;
-
-	BUG_ON(!mpio);
-
-	r = do_end_io_bio(m, clone, error, mpio);
-	pgpath = mpio->pgpath;
+	r = DM_ENDIO_INCOMPLETE;
+done:
 	if (pgpath) {
-		ps = &pgpath->pg->ps;
+		struct path_selector *ps = &pgpath->pg->ps;
+
 		if (ps->type->end_io)
 			ps->type->end_io(ps, &pgpath->path, mpio->nr_bytes);
 	}
diff --git a/drivers/md/dm-raid.c b/drivers/md/dm-raid.c
index 7d893228c40f..2e10c2f13a34 100644
--- a/drivers/md/dm-raid.c
+++ b/drivers/md/dm-raid.c
@@ -1571,7 +1571,7 @@ static sector_t __rdev_sectors(struct raid_set *rs)
 			return rdev->sectors;
 	}
 
-	BUG(); /* Constructor ensures we got some. */
+	return 0;
 }
 
 /* Calculate the sectors per device and per array used for @rs */
@@ -1927,7 +1927,7 @@ struct dm_raid_superblock {
 	/********************************************************************
 	 * BELOW FOLLOW V1.9.0 EXTENSIONS TO THE PRISTINE SUPERBLOCK FORMAT!!!
 	 *
-	 * FEATURE_FLAG_SUPPORTS_V190 in the features member indicates that those exist
+	 * FEATURE_FLAG_SUPPORTS_V190 in the compat_features member indicates that those exist
 	 */
 
 	__le32 flags; /* Flags defining array states for reshaping */
@@ -2092,6 +2092,11 @@ static void super_sync(struct mddev *mddev, struct md_rdev *rdev)
 	sb->layout = cpu_to_le32(mddev->layout);
 	sb->stripe_sectors = cpu_to_le32(mddev->chunk_sectors);
 
+	/********************************************************************
+	 * BELOW FOLLOW V1.9.0 EXTENSIONS TO THE PRISTINE SUPERBLOCK FORMAT!!!
+	 *
+	 * FEATURE_FLAG_SUPPORTS_V190 in the compat_features member indicates that those exist
+	 */
 	sb->new_level = cpu_to_le32(mddev->new_level);
 	sb->new_layout = cpu_to_le32(mddev->new_layout);
 	sb->new_stripe_sectors = cpu_to_le32(mddev->new_chunk_sectors);
@@ -2438,8 +2443,14 @@ static int super_validate(struct raid_set *rs, struct md_rdev *rdev)
 	mddev->bitmap_info.default_offset = mddev->bitmap_info.offset;
 
 	if (!test_and_clear_bit(FirstUse, &rdev->flags)) {
-		/* Retrieve device size stored in superblock to be prepared for shrink */
-		rdev->sectors = le64_to_cpu(sb->sectors);
+		/*
+		 * Retrieve rdev size stored in superblock to be prepared for shrink.
+		 * Check extended superblock members are present otherwise the size
+		 * will not be set!
+		 */
+		if (le32_to_cpu(sb->compat_features) & FEATURE_FLAG_SUPPORTS_V190)
+			rdev->sectors = le64_to_cpu(sb->sectors);
+
 		rdev->recovery_offset = le64_to_cpu(sb->disk_recovery_offset);
 		if (rdev->recovery_offset == MaxSector)
 			set_bit(In_sync, &rdev->flags);
@@ -2930,7 +2941,7 @@ static int raid_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 	bool resize;
 	struct raid_type *rt;
 	unsigned int num_raid_params, num_raid_devs;
-	sector_t calculated_dev_sectors;
+	sector_t calculated_dev_sectors, rdev_sectors;
 	struct raid_set *rs = NULL;
 	const char *arg;
 	struct rs_layout rs_layout;
@@ -3006,7 +3017,14 @@ static int raid_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 	if (r)
 		goto bad;
 
-	resize = calculated_dev_sectors != __rdev_sectors(rs);
+	rdev_sectors = __rdev_sectors(rs);
+	if (!rdev_sectors) {
+		ti->error = "Invalid rdev size";
+		r = -EINVAL;
+		goto bad;
+	}
+
+	resize = calculated_dev_sectors != rdev_sectors;
 
 	INIT_WORK(&rs->md.event_work, do_table_event);
 	ti->private = rs;
diff --git a/drivers/md/dm-raid1.c b/drivers/md/dm-raid1.c
index e61c45047c25..a4fbd911d566 100644
--- a/drivers/md/dm-raid1.c
+++ b/drivers/md/dm-raid1.c
@@ -145,6 +145,7 @@ static void dispatch_bios(void *context, struct bio_list *bio_list)
 
 struct dm_raid1_bio_record {
 	struct mirror *m;
+	/* if details->bi_bdev == NULL, details were not saved */
 	struct dm_bio_details details;
 	region_t write_region;
 };
@@ -490,9 +491,9 @@ static void hold_bio(struct mirror_set *ms, struct bio *bio)
 		 * If device is suspended, complete the bio.
 		 */
 		if (dm_noflush_suspending(ms->ti))
-			bio->bi_error = DM_ENDIO_REQUEUE;
+			bio->bi_status = BLK_STS_DM_REQUEUE;
 		else
-			bio->bi_error = -EIO;
+			bio->bi_status = BLK_STS_IOERR;
 
 		bio_endio(bio);
 		return;
@@ -626,7 +627,7 @@ static void write_callback(unsigned long error, void *context)
 	 * degrade the array.
 	 */
 	if (bio_op(bio) == REQ_OP_DISCARD) {
-		bio->bi_error = -EOPNOTSUPP;
+		bio->bi_status = BLK_STS_NOTSUPP;
 		bio_endio(bio);
 		return;
 	}
@@ -1198,6 +1199,8 @@ static int mirror_map(struct dm_target *ti, struct bio *bio)
 	struct dm_raid1_bio_record *bio_record =
 	  dm_per_bio_data(bio, sizeof(struct dm_raid1_bio_record));
 
+	bio_record->details.bi_bdev = NULL;
+
 	if (rw == WRITE) {
 		/* Save region for mirror_end_io() handler */
 		bio_record->write_region = dm_rh_bio_to_region(ms->rh, bio);
@@ -1207,14 +1210,14 @@ static int mirror_map(struct dm_target *ti, struct bio *bio)
 
 	r = log->type->in_sync(log, dm_rh_bio_to_region(ms->rh, bio), 0);
 	if (r < 0 && r != -EWOULDBLOCK)
-		return r;
+		return DM_MAPIO_KILL;
 
 	/*
 	 * If region is not in-sync queue the bio.
 	 */
 	if (!r || (r == -EWOULDBLOCK)) {
 		if (bio->bi_opf & REQ_RAHEAD)
-			return -EWOULDBLOCK;
+			return DM_MAPIO_KILL;
 
 		queue_bio(ms, bio, rw);
 		return DM_MAPIO_SUBMITTED;
@@ -1226,7 +1229,7 @@ static int mirror_map(struct dm_target *ti, struct bio *bio)
 	 */
 	m = choose_mirror(ms, bio->bi_iter.bi_sector);
 	if (unlikely(!m))
-		return -EIO;
+		return DM_MAPIO_KILL;
 
 	dm_bio_record(&bio_record->details, bio);
 	bio_record->m = m;
@@ -1236,7 +1239,8 @@ static int mirror_map(struct dm_target *ti, struct bio *bio)
 	return DM_MAPIO_REMAPPED;
 }
 
-static int mirror_end_io(struct dm_target *ti, struct bio *bio, int error)
+static int mirror_end_io(struct dm_target *ti, struct bio *bio,
+		blk_status_t *error)
 {
 	int rw = bio_data_dir(bio);
 	struct mirror_set *ms = (struct mirror_set *) ti->private;
@@ -1252,16 +1256,26 @@ static int mirror_end_io(struct dm_target *ti, struct bio *bio, int error)
 		if (!(bio->bi_opf & REQ_PREFLUSH) &&
 		    bio_op(bio) != REQ_OP_DISCARD)
 			dm_rh_dec(ms->rh, bio_record->write_region);
-		return error;
+		return DM_ENDIO_DONE;
 	}
 
-	if (error == -EOPNOTSUPP)
-		return error;
+	if (*error == BLK_STS_NOTSUPP)
+		goto out;
+
+	if (bio->bi_opf & REQ_RAHEAD)
+		goto out;
 
-	if ((error == -EWOULDBLOCK) && (bio->bi_opf & REQ_RAHEAD))
-		return error;
+	if (unlikely(*error)) {
+		if (!bio_record->details.bi_bdev) {
+			/*
+			 * There wasn't enough memory to record necessary
+			 * information for a retry or there was no other
+			 * mirror in-sync.
+			 */
+			DMERR_LIMIT("Mirror read failed.");
+			return DM_ENDIO_DONE;
+		}
 
-	if (unlikely(error)) {
 		m = bio_record->m;
 
 		DMERR("Mirror read failed from %s. Trying alternative device.",
@@ -1277,7 +1291,8 @@ static int mirror_end_io(struct dm_target *ti, struct bio *bio, int error)
 			bd = &bio_record->details;
 
 			dm_bio_restore(bd, bio);
-			bio->bi_error = 0;
+			bio_record->details.bi_bdev = NULL;
+			bio->bi_status = 0;
 
 			queue_bio(ms, bio, rw);
 			return DM_ENDIO_INCOMPLETE;
@@ -1285,7 +1300,10 @@ static int mirror_end_io(struct dm_target *ti, struct bio *bio, int error)
 		DMERR("All replicated volumes dead, failing I/O");
 	}
 
-	return error;
+out:
+	bio_record->details.bi_bdev = NULL;
+
+	return DM_ENDIO_DONE;
 }
 
 static void mirror_presuspend(struct dm_target *ti)
diff --git a/drivers/md/dm-rq.c b/drivers/md/dm-rq.c
index b639fa7246ee..c6ebc5b1e00e 100644
--- a/drivers/md/dm-rq.c
+++ b/drivers/md/dm-rq.c
@@ -71,7 +71,7 @@ static void dm_old_start_queue(struct request_queue *q)
 
 static void dm_mq_start_queue(struct request_queue *q)
 {
-	blk_mq_start_stopped_hw_queues(q, true);
+	blk_mq_unquiesce_queue(q);
 	blk_mq_kick_requeue_list(q);
 }
 
@@ -119,7 +119,7 @@ static void end_clone_bio(struct bio *clone)
 	struct dm_rq_target_io *tio = info->tio;
 	struct bio *bio = info->orig;
 	unsigned int nr_bytes = info->orig->bi_iter.bi_size;
-	int error = clone->bi_error;
+	blk_status_t error = clone->bi_status;
 
 	bio_put(clone);
 
@@ -158,7 +158,7 @@ static void end_clone_bio(struct bio *clone)
 	 * Do not use blk_end_request() here, because it may complete
 	 * the original request before the clone, and break the ordering.
 	 */
-	blk_update_request(tio->orig, 0, nr_bytes);
+	blk_update_request(tio->orig, BLK_STS_OK, nr_bytes);
 }
 
 static struct dm_rq_target_io *tio_from_request(struct request *rq)
@@ -216,7 +216,7 @@ static void rq_completed(struct mapped_device *md, int rw, bool run_queue)
  * Must be called without clone's queue lock held,
  * see end_clone_request() for more details.
  */
-static void dm_end_request(struct request *clone, int error)
+static void dm_end_request(struct request *clone, blk_status_t error)
 {
 	int rw = rq_data_dir(clone);
 	struct dm_rq_target_io *tio = clone->end_io_data;
@@ -285,7 +285,7 @@ static void dm_requeue_original_request(struct dm_rq_target_io *tio, bool delay_
 	rq_completed(md, rw, false);
 }
 
-static void dm_done(struct request *clone, int error, bool mapped)
+static void dm_done(struct request *clone, blk_status_t error, bool mapped)
 {
 	int r = DM_ENDIO_DONE;
 	struct dm_rq_target_io *tio = clone->end_io_data;
@@ -298,7 +298,7 @@ static void dm_done(struct request *clone, int error, bool mapped)
 			r = rq_end_io(tio->ti, clone, error, &tio->info);
 	}
 
-	if (unlikely(error == -EREMOTEIO)) {
+	if (unlikely(error == BLK_STS_TARGET)) {
 		if (req_op(clone) == REQ_OP_WRITE_SAME &&
 		    !clone->q->limits.max_write_same_sectors)
 			disable_write_same(tio->md);
@@ -358,7 +358,7 @@ static void dm_softirq_done(struct request *rq)
  * Complete the clone and the original request with the error status
  * through softirq context.
  */
-static void dm_complete_request(struct request *rq, int error)
+static void dm_complete_request(struct request *rq, blk_status_t error)
 {
 	struct dm_rq_target_io *tio = tio_from_request(rq);
 
@@ -375,7 +375,7 @@ static void dm_complete_request(struct request *rq, int error)
  * Target's rq_end_io() function isn't called.
  * This may be used when the target's map_rq() or clone_and_map_rq() functions fail.
  */
-static void dm_kill_unmapped_request(struct request *rq, int error)
+static void dm_kill_unmapped_request(struct request *rq, blk_status_t error)
 {
 	rq->rq_flags |= RQF_FAILED;
 	dm_complete_request(rq, error);
@@ -384,7 +384,7 @@ static void dm_kill_unmapped_request(struct request *rq, int error)
 /*
  * Called with the clone's queue lock held (in the case of .request_fn)
  */
-static void end_clone_request(struct request *clone, int error)
+static void end_clone_request(struct request *clone, blk_status_t error)
 {
 	struct dm_rq_target_io *tio = clone->end_io_data;
 
@@ -401,7 +401,7 @@ static void end_clone_request(struct request *clone, int error)
 
 static void dm_dispatch_clone_request(struct request *clone, struct request *rq)
 {
-	int r;
+	blk_status_t r;
 
 	if (blk_queue_io_stat(clone->q))
 		clone->rq_flags |= RQF_IO_STAT;
@@ -506,7 +506,7 @@ static int map_request(struct dm_rq_target_io *tio)
 		break;
 	case DM_MAPIO_KILL:
 		/* The target wants to complete the I/O */
-		dm_kill_unmapped_request(rq, -EIO);
+		dm_kill_unmapped_request(rq, BLK_STS_IOERR);
 		break;
 	default:
 		DMWARN("unimplemented target map return value: %d", r);
@@ -727,7 +727,7 @@ static int dm_mq_init_request(struct blk_mq_tag_set *set, struct request *rq,
 	return __dm_rq_init_rq(set->driver_data, rq);
 }
 
-static int dm_mq_queue_rq(struct blk_mq_hw_ctx *hctx,
+static blk_status_t dm_mq_queue_rq(struct blk_mq_hw_ctx *hctx,
 			  const struct blk_mq_queue_data *bd)
 {
 	struct request *rq = bd->rq;
@@ -744,7 +744,7 @@ static int dm_mq_queue_rq(struct blk_mq_hw_ctx *hctx,
 	}
 
 	if (ti->type->busy && ti->type->busy(ti))
-		return BLK_MQ_RQ_QUEUE_BUSY;
+		return BLK_STS_RESOURCE;
 
 	dm_start_request(md, rq);
 
@@ -762,10 +762,10 @@ static int dm_mq_queue_rq(struct blk_mq_hw_ctx *hctx,
 		rq_end_stats(md, rq);
 		rq_completed(md, rq_data_dir(rq), false);
 		blk_mq_delay_run_hw_queue(hctx, 100/*ms*/);
-		return BLK_MQ_RQ_QUEUE_BUSY;
+		return BLK_STS_RESOURCE;
 	}
 
-	return BLK_MQ_RQ_QUEUE_OK;
+	return BLK_STS_OK;
 }
 
 static const struct blk_mq_ops dm_mq_ops = {
diff --git a/drivers/md/dm-rq.h b/drivers/md/dm-rq.h
index f0020d21b95f..9813922e4fe5 100644
--- a/drivers/md/dm-rq.h
+++ b/drivers/md/dm-rq.h
@@ -24,7 +24,7 @@ struct dm_rq_target_io {
 	struct dm_target *ti;
 	struct request *orig, *clone;
 	struct kthread_work work;
-	int error;
+	blk_status_t error;
 	union map_info info;
 	struct dm_stats_aux stats_aux;
 	unsigned long duration_jiffies;
diff --git a/drivers/md/dm-snap.c b/drivers/md/dm-snap.c
index e152d9817c81..1ba41048b438 100644
--- a/drivers/md/dm-snap.c
+++ b/drivers/md/dm-snap.c
@@ -1590,7 +1590,7 @@ static void full_bio_end_io(struct bio *bio)
 {
 	void *callback_data = bio->bi_private;
 
-	dm_kcopyd_do_callback(callback_data, 0, bio->bi_error ? 1 : 0);
+	dm_kcopyd_do_callback(callback_data, 0, bio->bi_status ? 1 : 0);
 }
 
 static void start_full_bio(struct dm_snap_pending_exception *pe,
@@ -1690,7 +1690,7 @@ static int snapshot_map(struct dm_target *ti, struct bio *bio)
 	/* Full snapshots are not usable */
 	/* To get here the table must be live so s->active is always set. */
 	if (!s->valid)
-		return -EIO;
+		return DM_MAPIO_KILL;
 
 	/* FIXME: should only take write lock if we need
 	 * to copy an exception */
@@ -1698,7 +1698,7 @@ static int snapshot_map(struct dm_target *ti, struct bio *bio)
 
 	if (!s->valid || (unlikely(s->snapshot_overflowed) &&
 	    bio_data_dir(bio) == WRITE)) {
-		r = -EIO;
+		r = DM_MAPIO_KILL;
 		goto out_unlock;
 	}
 
@@ -1723,7 +1723,7 @@ static int snapshot_map(struct dm_target *ti, struct bio *bio)
 
 			if (!s->valid || s->snapshot_overflowed) {
 				free_pending_exception(pe);
-				r = -EIO;
+				r = DM_MAPIO_KILL;
 				goto out_unlock;
 			}
 
@@ -1741,7 +1741,7 @@ static int snapshot_map(struct dm_target *ti, struct bio *bio)
 					DMERR("Snapshot overflowed: Unable to allocate exception.");
 				} else
 					__invalidate_snapshot(s, -ENOMEM);
-				r = -EIO;
+				r = DM_MAPIO_KILL;
 				goto out_unlock;
 			}
 		}
@@ -1851,14 +1851,15 @@ out_unlock:
 	return r;
 }
 
-static int snapshot_end_io(struct dm_target *ti, struct bio *bio, int error)
+static int snapshot_end_io(struct dm_target *ti, struct bio *bio,
+		blk_status_t *error)
 {
 	struct dm_snapshot *s = ti->private;
 
 	if (is_bio_tracked(bio))
 		stop_tracking_chunk(s, bio);
 
-	return 0;
+	return DM_ENDIO_DONE;
 }
 
 static void snapshot_merge_presuspend(struct dm_target *ti)
diff --git a/drivers/md/dm-stripe.c b/drivers/md/dm-stripe.c
index 75152482f3ad..11621a0af887 100644
--- a/drivers/md/dm-stripe.c
+++ b/drivers/md/dm-stripe.c
@@ -375,20 +375,21 @@ static void stripe_status(struct dm_target *ti, status_type_t type,
 	}
 }
 
-static int stripe_end_io(struct dm_target *ti, struct bio *bio, int error)
+static int stripe_end_io(struct dm_target *ti, struct bio *bio,
+		blk_status_t *error)
 {
 	unsigned i;
 	char major_minor[16];
 	struct stripe_c *sc = ti->private;
 
-	if (!error)
-		return 0; /* I/O complete */
+	if (!*error)
+		return DM_ENDIO_DONE; /* I/O complete */
 
-	if ((error == -EWOULDBLOCK) && (bio->bi_opf & REQ_RAHEAD))
-		return error;
+	if (bio->bi_opf & REQ_RAHEAD)
+		return DM_ENDIO_DONE;
 
-	if (error == -EOPNOTSUPP)
-		return error;
+	if (*error == BLK_STS_NOTSUPP)
+		return DM_ENDIO_DONE;
 
 	memset(major_minor, 0, sizeof(major_minor));
 	sprintf(major_minor, "%d:%d",
@@ -409,7 +410,7 @@ static int stripe_end_io(struct dm_target *ti, struct bio *bio, int error)
 				schedule_work(&sc->trigger_event);
 		}
 
-	return error;
+	return DM_ENDIO_DONE;
 }
 
 static int stripe_iterate_devices(struct dm_target *ti,
diff --git a/drivers/md/dm-table.c b/drivers/md/dm-table.c
index 5f5eae41f804..a39bcd9b982a 100644
--- a/drivers/md/dm-table.c
+++ b/drivers/md/dm-table.c
@@ -319,6 +319,39 @@ static int device_area_is_invalid(struct dm_target *ti, struct dm_dev *dev,
 		return 1;
 	}
 
+	/*
+	 * If the target is mapped to zoned block device(s), check
+	 * that the zones are not partially mapped.
+	 */
+	if (bdev_zoned_model(bdev) != BLK_ZONED_NONE) {
+		unsigned int zone_sectors = bdev_zone_sectors(bdev);
+
+		if (start & (zone_sectors - 1)) {
+			DMWARN("%s: start=%llu not aligned to h/w zone size %u of %s",
+			       dm_device_name(ti->table->md),
+			       (unsigned long long)start,
+			       zone_sectors, bdevname(bdev, b));
+			return 1;
+		}
+
+		/*
+		 * Note: The last zone of a zoned block device may be smaller
+		 * than other zones. So for a target mapping the end of a
+		 * zoned block device with such a zone, len would not be zone
+		 * aligned. We do not allow such last smaller zone to be part
+		 * of the mapping here to ensure that mappings with multiple
+		 * devices do not end up with a smaller zone in the middle of
+		 * the sector range.
+		 */
+		if (len & (zone_sectors - 1)) {
+			DMWARN("%s: len=%llu not aligned to h/w zone size %u of %s",
+			       dm_device_name(ti->table->md),
+			       (unsigned long long)len,
+			       zone_sectors, bdevname(bdev, b));
+			return 1;
+		}
+	}
+
 	if (logical_block_size_sectors <= 1)
 		return 0;
 
@@ -456,6 +489,8 @@ static int dm_set_device_limits(struct dm_target *ti, struct dm_dev *dev,
 		       q->limits.alignment_offset,
 		       (unsigned long long) start << SECTOR_SHIFT);
 
+	limits->zoned = blk_queue_zoned_model(q);
+
 	return 0;
 }
 
@@ -1346,6 +1381,88 @@ bool dm_table_has_no_data_devices(struct dm_table *table)
 	return true;
 }
 
+static int device_is_zoned_model(struct dm_target *ti, struct dm_dev *dev,
+				 sector_t start, sector_t len, void *data)
+{
+	struct request_queue *q = bdev_get_queue(dev->bdev);
+	enum blk_zoned_model *zoned_model = data;
+
+	return q && blk_queue_zoned_model(q) == *zoned_model;
+}
+
+static bool dm_table_supports_zoned_model(struct dm_table *t,
+					  enum blk_zoned_model zoned_model)
+{
+	struct dm_target *ti;
+	unsigned i;
+
+	for (i = 0; i < dm_table_get_num_targets(t); i++) {
+		ti = dm_table_get_target(t, i);
+
+		if (zoned_model == BLK_ZONED_HM &&
+		    !dm_target_supports_zoned_hm(ti->type))
+			return false;
+
+		if (!ti->type->iterate_devices ||
+		    !ti->type->iterate_devices(ti, device_is_zoned_model, &zoned_model))
+			return false;
+	}
+
+	return true;
+}
+
+static int device_matches_zone_sectors(struct dm_target *ti, struct dm_dev *dev,
+				       sector_t start, sector_t len, void *data)
+{
+	struct request_queue *q = bdev_get_queue(dev->bdev);
+	unsigned int *zone_sectors = data;
+
+	return q && blk_queue_zone_sectors(q) == *zone_sectors;
+}
+
+static bool dm_table_matches_zone_sectors(struct dm_table *t,
+					  unsigned int zone_sectors)
+{
+	struct dm_target *ti;
+	unsigned i;
+
+	for (i = 0; i < dm_table_get_num_targets(t); i++) {
+		ti = dm_table_get_target(t, i);
+
+		if (!ti->type->iterate_devices ||
+		    !ti->type->iterate_devices(ti, device_matches_zone_sectors, &zone_sectors))
+			return false;
+	}
+
+	return true;
+}
+
+static int validate_hardware_zoned_model(struct dm_table *table,
+					 enum blk_zoned_model zoned_model,
+					 unsigned int zone_sectors)
+{
+	if (zoned_model == BLK_ZONED_NONE)
+		return 0;
+
+	if (!dm_table_supports_zoned_model(table, zoned_model)) {
+		DMERR("%s: zoned model is not consistent across all devices",
+		      dm_device_name(table->md));
+		return -EINVAL;
+	}
+
+	/* Check zone size validity and compatibility */
+	if (!zone_sectors || !is_power_of_2(zone_sectors))
+		return -EINVAL;
+
+	if (!dm_table_matches_zone_sectors(table, zone_sectors)) {
+		DMERR("%s: zone sectors is not consistent across all devices",
+		      dm_device_name(table->md));
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 /*
  * Establish the new table's queue_limits and validate them.
  */
@@ -1355,6 +1472,8 @@ int dm_calculate_queue_limits(struct dm_table *table,
 	struct dm_target *ti;
 	struct queue_limits ti_limits;
 	unsigned i;
+	enum blk_zoned_model zoned_model = BLK_ZONED_NONE;
+	unsigned int zone_sectors = 0;
 
 	blk_set_stacking_limits(limits);
 
@@ -1372,6 +1491,15 @@ int dm_calculate_queue_limits(struct dm_table *table,
 		ti->type->iterate_devices(ti, dm_set_device_limits,
 					  &ti_limits);
 
+		if (zoned_model == BLK_ZONED_NONE && ti_limits.zoned != BLK_ZONED_NONE) {
+			/*
+			 * After stacking all limits, validate all devices
+			 * in table support this zoned model and zone sectors.
+			 */
+			zoned_model = ti_limits.zoned;
+			zone_sectors = ti_limits.chunk_sectors;
+		}
+
 		/* Set I/O hints portion of queue limits */
 		if (ti->type->io_hints)
 			ti->type->io_hints(ti, &ti_limits);
@@ -1396,8 +1524,42 @@ combine_limits:
 			       dm_device_name(table->md),
 			       (unsigned long long) ti->begin,
 			       (unsigned long long) ti->len);
+
+		/*
+		 * FIXME: this should likely be moved to blk_stack_limits(), would
+		 * also eliminate limits->zoned stacking hack in dm_set_device_limits()
+		 */
+		if (limits->zoned == BLK_ZONED_NONE && ti_limits.zoned != BLK_ZONED_NONE) {
+			/*
+			 * By default, the stacked limits zoned model is set to
+			 * BLK_ZONED_NONE in blk_set_stacking_limits(). Update
+			 * this model using the first target model reported
+			 * that is not BLK_ZONED_NONE. This will be either the
+			 * first target device zoned model or the model reported
+			 * by the target .io_hints.
+			 */
+			limits->zoned = ti_limits.zoned;
+		}
 	}
 
+	/*
+	 * Verify that the zoned model and zone sectors, as determined before
+	 * any .io_hints override, are the same across all devices in the table.
+	 * - this is especially relevant if .io_hints is emulating a disk-managed
+	 *   zoned model (aka BLK_ZONED_NONE) on host-managed zoned block devices.
+	 * BUT...
+	 */
+	if (limits->zoned != BLK_ZONED_NONE) {
+		/*
+		 * ...IF the above limits stacking determined a zoned model
+		 * validate that all of the table's devices conform to it.
+		 */
+		zoned_model = limits->zoned;
+		zone_sectors = limits->chunk_sectors;
+	}
+	if (validate_hardware_zoned_model(table, zoned_model, zone_sectors))
+		return -EINVAL;
+
 	return validate_hardware_logical_block_alignment(table, limits);
 }
 
diff --git a/drivers/md/dm-target.c b/drivers/md/dm-target.c
index b242b750542f..c0d7e60820c4 100644
--- a/drivers/md/dm-target.c
+++ b/drivers/md/dm-target.c
@@ -128,7 +128,7 @@ static void io_err_dtr(struct dm_target *tt)
 
 static int io_err_map(struct dm_target *tt, struct bio *bio)
 {
-	return -EIO;
+	return DM_MAPIO_KILL;
 }
 
 static int io_err_clone_and_map_rq(struct dm_target *ti, struct request *rq,
diff --git a/drivers/md/dm-thin.c b/drivers/md/dm-thin.c
index 17ad50daed08..9dec2f8cc739 100644
--- a/drivers/md/dm-thin.c
+++ b/drivers/md/dm-thin.c
@@ -383,8 +383,8 @@ static void end_discard(struct discard_op *op, int r)
 	 * Even if r is set, there could be sub discards in flight that we
 	 * need to wait for.
 	 */
-	if (r && !op->parent_bio->bi_error)
-		op->parent_bio->bi_error = r;
+	if (r && !op->parent_bio->bi_status)
+		op->parent_bio->bi_status = errno_to_blk_status(r);
 	bio_endio(op->parent_bio);
 }
 
@@ -450,22 +450,20 @@ static void cell_release_no_holder(struct pool *pool,
 }
 
 static void cell_error_with_code(struct pool *pool,
-				 struct dm_bio_prison_cell *cell, int error_code)
+		struct dm_bio_prison_cell *cell, blk_status_t error_code)
 {
 	dm_cell_error(pool->prison, cell, error_code);
 	dm_bio_prison_free_cell(pool->prison, cell);
 }
 
-static int get_pool_io_error_code(struct pool *pool)
+static blk_status_t get_pool_io_error_code(struct pool *pool)
 {
-	return pool->out_of_data_space ? -ENOSPC : -EIO;
+	return pool->out_of_data_space ? BLK_STS_NOSPC : BLK_STS_IOERR;
 }
 
 static void cell_error(struct pool *pool, struct dm_bio_prison_cell *cell)
 {
-	int error = get_pool_io_error_code(pool);
-
-	cell_error_with_code(pool, cell, error);
+	cell_error_with_code(pool, cell, get_pool_io_error_code(pool));
 }
 
 static void cell_success(struct pool *pool, struct dm_bio_prison_cell *cell)
@@ -475,7 +473,7 @@ static void cell_success(struct pool *pool, struct dm_bio_prison_cell *cell)
 
 static void cell_requeue(struct pool *pool, struct dm_bio_prison_cell *cell)
 {
-	cell_error_with_code(pool, cell, DM_ENDIO_REQUEUE);
+	cell_error_with_code(pool, cell, BLK_STS_DM_REQUEUE);
 }
 
 /*----------------------------------------------------------------*/
@@ -555,17 +553,18 @@ static void __merge_bio_list(struct bio_list *bios, struct bio_list *master)
 	bio_list_init(master);
 }
 
-static void error_bio_list(struct bio_list *bios, int error)
+static void error_bio_list(struct bio_list *bios, blk_status_t error)
 {
 	struct bio *bio;
 
 	while ((bio = bio_list_pop(bios))) {
-		bio->bi_error = error;
+		bio->bi_status = error;
 		bio_endio(bio);
 	}
 }
 
-static void error_thin_bio_list(struct thin_c *tc, struct bio_list *master, int error)
+static void error_thin_bio_list(struct thin_c *tc, struct bio_list *master,
+		blk_status_t error)
 {
 	struct bio_list bios;
 	unsigned long flags;
@@ -608,11 +607,11 @@ static void requeue_io(struct thin_c *tc)
 	__merge_bio_list(&bios, &tc->retry_on_resume_list);
 	spin_unlock_irqrestore(&tc->lock, flags);
 
-	error_bio_list(&bios, DM_ENDIO_REQUEUE);
+	error_bio_list(&bios, BLK_STS_DM_REQUEUE);
 	requeue_deferred_cells(tc);
 }
 
-static void error_retry_list_with_code(struct pool *pool, int error)
+static void error_retry_list_with_code(struct pool *pool, blk_status_t error)
 {
 	struct thin_c *tc;
 
@@ -624,9 +623,7 @@ static void error_retry_list_with_code(struct pool *pool, int error)
 
 static void error_retry_list(struct pool *pool)
 {
-	int error = get_pool_io_error_code(pool);
-
-	error_retry_list_with_code(pool, error);
+	error_retry_list_with_code(pool, get_pool_io_error_code(pool));
 }
 
 /*
@@ -774,7 +771,7 @@ struct dm_thin_new_mapping {
 	 */
 	atomic_t prepare_actions;
 
-	int err;
+	blk_status_t status;
 	struct thin_c *tc;
 	dm_block_t virt_begin, virt_end;
 	dm_block_t data_block;
@@ -814,7 +811,7 @@ static void copy_complete(int read_err, unsigned long write_err, void *context)
 {
 	struct dm_thin_new_mapping *m = context;
 
-	m->err = read_err || write_err ? -EIO : 0;
+	m->status = read_err || write_err ? BLK_STS_IOERR : 0;
 	complete_mapping_preparation(m);
 }
 
@@ -825,7 +822,7 @@ static void overwrite_endio(struct bio *bio)
 
 	bio->bi_end_io = m->saved_bi_end_io;
 
-	m->err = bio->bi_error;
+	m->status = bio->bi_status;
 	complete_mapping_preparation(m);
 }
 
@@ -925,7 +922,7 @@ static void process_prepared_mapping(struct dm_thin_new_mapping *m)
 	struct bio *bio = m->bio;
 	int r;
 
-	if (m->err) {
+	if (m->status) {
 		cell_error(pool, m->cell);
 		goto out;
 	}
@@ -1094,6 +1091,19 @@ static void process_prepared_discard_passdown_pt1(struct dm_thin_new_mapping *m)
 		return;
 	}
 
+	/*
+	 * Increment the unmapped blocks.  This prevents a race between the
+	 * passdown io and reallocation of freed blocks.
+	 */
+	r = dm_pool_inc_data_range(pool->pmd, m->data_block, data_end);
+	if (r) {
+		metadata_operation_failed(pool, "dm_pool_inc_data_range", r);
+		bio_io_error(m->bio);
+		cell_defer_no_holder(tc, m->cell);
+		mempool_free(m, pool->mapping_pool);
+		return;
+	}
+
 	discard_parent = bio_alloc(GFP_NOIO, 1);
 	if (!discard_parent) {
 		DMWARN("%s: unable to allocate top level discard bio for passdown. Skipping passdown.",
@@ -1114,19 +1124,6 @@ static void process_prepared_discard_passdown_pt1(struct dm_thin_new_mapping *m)
 			end_discard(&op, r);
 		}
 	}
-
-	/*
-	 * Increment the unmapped blocks.  This prevents a race between the
-	 * passdown io and reallocation of freed blocks.
-	 */
-	r = dm_pool_inc_data_range(pool->pmd, m->data_block, data_end);
-	if (r) {
-		metadata_operation_failed(pool, "dm_pool_inc_data_range", r);
-		bio_io_error(m->bio);
-		cell_defer_no_holder(tc, m->cell);
-		mempool_free(m, pool->mapping_pool);
-		return;
-	}
 }
 
 static void process_prepared_discard_passdown_pt2(struct dm_thin_new_mapping *m)
@@ -1495,7 +1492,7 @@ static void retry_on_resume(struct bio *bio)
 	spin_unlock_irqrestore(&tc->lock, flags);
 }
 
-static int should_error_unserviceable_bio(struct pool *pool)
+static blk_status_t should_error_unserviceable_bio(struct pool *pool)
 {
 	enum pool_mode m = get_pool_mode(pool);
 
@@ -1503,27 +1500,27 @@ static int should_error_unserviceable_bio(struct pool *pool)
 	case PM_WRITE:
 		/* Shouldn't get here */
 		DMERR_LIMIT("bio unserviceable, yet pool is in PM_WRITE mode");
-		return -EIO;
+		return BLK_STS_IOERR;
 
 	case PM_OUT_OF_DATA_SPACE:
-		return pool->pf.error_if_no_space ? -ENOSPC : 0;
+		return pool->pf.error_if_no_space ? BLK_STS_NOSPC : 0;
 
 	case PM_READ_ONLY:
 	case PM_FAIL:
-		return -EIO;
+		return BLK_STS_IOERR;
 	default:
 		/* Shouldn't get here */
 		DMERR_LIMIT("bio unserviceable, yet pool has an unknown mode");
-		return -EIO;
+		return BLK_STS_IOERR;
 	}
 }
 
 static void handle_unserviceable_bio(struct pool *pool, struct bio *bio)
 {
-	int error = should_error_unserviceable_bio(pool);
+	blk_status_t error = should_error_unserviceable_bio(pool);
 
 	if (error) {
-		bio->bi_error = error;
+		bio->bi_status = error;
 		bio_endio(bio);
 	} else
 		retry_on_resume(bio);
@@ -1533,7 +1530,7 @@ static void retry_bios_on_resume(struct pool *pool, struct dm_bio_prison_cell *c
 {
 	struct bio *bio;
 	struct bio_list bios;
-	int error;
+	blk_status_t error;
 
 	error = should_error_unserviceable_bio(pool);
 	if (error) {
@@ -2071,7 +2068,8 @@ static void process_thin_deferred_bios(struct thin_c *tc)
 	unsigned count = 0;
 
 	if (tc->requeue_mode) {
-		error_thin_bio_list(tc, &tc->deferred_bio_list, DM_ENDIO_REQUEUE);
+		error_thin_bio_list(tc, &tc->deferred_bio_list,
+				BLK_STS_DM_REQUEUE);
 		return;
 	}
 
@@ -2322,7 +2320,7 @@ static void do_no_space_timeout(struct work_struct *ws)
 	if (get_pool_mode(pool) == PM_OUT_OF_DATA_SPACE && !pool->pf.error_if_no_space) {
 		pool->pf.error_if_no_space = true;
 		notify_of_pool_mode_change_to_oods(pool);
-		error_retry_list_with_code(pool, -ENOSPC);
+		error_retry_list_with_code(pool, BLK_STS_NOSPC);
 	}
 }
 
@@ -2624,7 +2622,7 @@ static int thin_bio_map(struct dm_target *ti, struct bio *bio)
 	thin_hook_bio(tc, bio);
 
 	if (tc->requeue_mode) {
-		bio->bi_error = DM_ENDIO_REQUEUE;
+		bio->bi_status = BLK_STS_DM_REQUEUE;
 		bio_endio(bio);
 		return DM_MAPIO_SUBMITTED;
 	}
@@ -4177,7 +4175,8 @@ static int thin_map(struct dm_target *ti, struct bio *bio)
 	return thin_bio_map(ti, bio);
 }
 
-static int thin_endio(struct dm_target *ti, struct bio *bio, int err)
+static int thin_endio(struct dm_target *ti, struct bio *bio,
+		blk_status_t *err)
 {
 	unsigned long flags;
 	struct dm_thin_endio_hook *h = dm_per_bio_data(bio, sizeof(struct dm_thin_endio_hook));
@@ -4212,7 +4211,7 @@ static int thin_endio(struct dm_target *ti, struct bio *bio, int err)
 	if (h->cell)
 		cell_defer_no_holder(h->tc, h->cell);
 
-	return 0;
+	return DM_ENDIO_DONE;
 }
 
 static void thin_presuspend(struct dm_target *ti)
diff --git a/drivers/md/dm-verity-target.c b/drivers/md/dm-verity-target.c
index 1ec9b2c51c07..b46705ebf01f 100644
--- a/drivers/md/dm-verity-target.c
+++ b/drivers/md/dm-verity-target.c
@@ -538,13 +538,13 @@ static int verity_verify_io(struct dm_verity_io *io)
 /*
  * End one "io" structure with a given error.
  */
-static void verity_finish_io(struct dm_verity_io *io, int error)
+static void verity_finish_io(struct dm_verity_io *io, blk_status_t status)
 {
 	struct dm_verity *v = io->v;
 	struct bio *bio = dm_bio_from_per_bio_data(io, v->ti->per_io_data_size);
 
 	bio->bi_end_io = io->orig_bi_end_io;
-	bio->bi_error = error;
+	bio->bi_status = status;
 
 	verity_fec_finish_io(io);
 
@@ -555,15 +555,15 @@ static void verity_work(struct work_struct *w)
 {
 	struct dm_verity_io *io = container_of(w, struct dm_verity_io, work);
 
-	verity_finish_io(io, verity_verify_io(io));
+	verity_finish_io(io, errno_to_blk_status(verity_verify_io(io)));
 }
 
 static void verity_end_io(struct bio *bio)
 {
 	struct dm_verity_io *io = bio->bi_private;
 
-	if (bio->bi_error && !verity_fec_is_enabled(io->v)) {
-		verity_finish_io(io, bio->bi_error);
+	if (bio->bi_status && !verity_fec_is_enabled(io->v)) {
+		verity_finish_io(io, bio->bi_status);
 		return;
 	}
 
@@ -643,17 +643,17 @@ static int verity_map(struct dm_target *ti, struct bio *bio)
 	if (((unsigned)bio->bi_iter.bi_sector | bio_sectors(bio)) &
 	    ((1 << (v->data_dev_block_bits - SECTOR_SHIFT)) - 1)) {
 		DMERR_LIMIT("unaligned io");
-		return -EIO;
+		return DM_MAPIO_KILL;
 	}
 
 	if (bio_end_sector(bio) >>
 	    (v->data_dev_block_bits - SECTOR_SHIFT) > v->data_blocks) {
 		DMERR_LIMIT("io out of range");
-		return -EIO;
+		return DM_MAPIO_KILL;
 	}
 
 	if (bio_data_dir(bio) == WRITE)
-		return -EIO;
+		return DM_MAPIO_KILL;
 
 	io = dm_per_bio_data(bio, ti->per_io_data_size);
 	io->v = v;
diff --git a/drivers/md/dm-zero.c b/drivers/md/dm-zero.c
index b616f11d8473..b65ca8dcfbdc 100644
--- a/drivers/md/dm-zero.c
+++ b/drivers/md/dm-zero.c
@@ -39,7 +39,7 @@ static int zero_map(struct dm_target *ti, struct bio *bio)
 	case REQ_OP_READ:
 		if (bio->bi_opf & REQ_RAHEAD) {
 			/* readahead of null bytes only wastes buffer cache */
-			return -EIO;
+			return DM_MAPIO_KILL;
 		}
 		zero_fill_bio(bio);
 		break;
@@ -47,7 +47,7 @@ static int zero_map(struct dm_target *ti, struct bio *bio)
 		/* writes get silently dropped */
 		break;
 	default:
-		return -EIO;
+		return DM_MAPIO_KILL;
 	}
 
 	bio_endio(bio);
diff --git a/drivers/md/dm-zoned-metadata.c b/drivers/md/dm-zoned-metadata.c
new file mode 100644
index 000000000000..884ff7c170a0
--- /dev/null
+++ b/drivers/md/dm-zoned-metadata.c
@@ -0,0 +1,2509 @@
+/*
+ * Copyright (C) 2017 Western Digital Corporation or its affiliates.
+ *
+ * This file is released under the GPL.
+ */
+
+#include "dm-zoned.h"
+
+#include <linux/module.h>
+#include <linux/crc32.h>
+
+#define	DM_MSG_PREFIX		"zoned metadata"
+
+/*
+ * Metadata version.
+ */
+#define DMZ_META_VER	1
+
+/*
+ * On-disk super block magic.
+ */
+#define DMZ_MAGIC	((((unsigned int)('D')) << 24) | \
+			 (((unsigned int)('Z')) << 16) | \
+			 (((unsigned int)('B')) <<  8) | \
+			 ((unsigned int)('D')))
+
+/*
+ * On disk super block.
+ * This uses only 512 B but uses on disk a full 4KB block. This block is
+ * followed on disk by the mapping table of chunks to zones and the bitmap
+ * blocks indicating zone block validity.
+ * The overall resulting metadata format is:
+ *    (1) Super block (1 block)
+ *    (2) Chunk mapping table (nr_map_blocks)
+ *    (3) Bitmap blocks (nr_bitmap_blocks)
+ * All metadata blocks are stored in conventional zones, starting from the
+ * the first conventional zone found on disk.
+ */
+struct dmz_super {
+	/* Magic number */
+	__le32		magic;			/*   4 */
+
+	/* Metadata version number */
+	__le32		version;		/*   8 */
+
+	/* Generation number */
+	__le64		gen;			/*  16 */
+
+	/* This block number */
+	__le64		sb_block;		/*  24 */
+
+	/* The number of metadata blocks, including this super block */
+	__le32		nr_meta_blocks;		/*  28 */
+
+	/* The number of sequential zones reserved for reclaim */
+	__le32		nr_reserved_seq;	/*  32 */
+
+	/* The number of entries in the mapping table */
+	__le32		nr_chunks;		/*  36 */
+
+	/* The number of blocks used for the chunk mapping table */
+	__le32		nr_map_blocks;		/*  40 */
+
+	/* The number of blocks used for the block bitmaps */
+	__le32		nr_bitmap_blocks;	/*  44 */
+
+	/* Checksum */
+	__le32		crc;			/*  48 */
+
+	/* Padding to full 512B sector */
+	u8		reserved[464];		/* 512 */
+};
+
+/*
+ * Chunk mapping entry: entries are indexed by chunk number
+ * and give the zone ID (dzone_id) mapping the chunk on disk.
+ * This zone may be sequential or random. If it is a sequential
+ * zone, a second zone (bzone_id) used as a write buffer may
+ * also be specified. This second zone will always be a randomly
+ * writeable zone.
+ */
+struct dmz_map {
+	__le32			dzone_id;
+	__le32			bzone_id;
+};
+
+/*
+ * Chunk mapping table metadata: 512 8-bytes entries per 4KB block.
+ */
+#define DMZ_MAP_ENTRIES		(DMZ_BLOCK_SIZE / sizeof(struct dmz_map))
+#define DMZ_MAP_ENTRIES_SHIFT	(ilog2(DMZ_MAP_ENTRIES))
+#define DMZ_MAP_ENTRIES_MASK	(DMZ_MAP_ENTRIES - 1)
+#define DMZ_MAP_UNMAPPED	UINT_MAX
+
+/*
+ * Meta data block descriptor (for cached metadata blocks).
+ */
+struct dmz_mblock {
+	struct rb_node		node;
+	struct list_head	link;
+	sector_t		no;
+	atomic_t		ref;
+	unsigned long		state;
+	struct page		*page;
+	void			*data;
+};
+
+/*
+ * Metadata block state flags.
+ */
+enum {
+	DMZ_META_DIRTY,
+	DMZ_META_READING,
+	DMZ_META_WRITING,
+	DMZ_META_ERROR,
+};
+
+/*
+ * Super block information (one per metadata set).
+ */
+struct dmz_sb {
+	sector_t		block;
+	struct dmz_mblock	*mblk;
+	struct dmz_super	*sb;
+};
+
+/*
+ * In-memory metadata.
+ */
+struct dmz_metadata {
+	struct dmz_dev		*dev;
+
+	sector_t		zone_bitmap_size;
+	unsigned int		zone_nr_bitmap_blocks;
+
+	unsigned int		nr_bitmap_blocks;
+	unsigned int		nr_map_blocks;
+
+	unsigned int		nr_useable_zones;
+	unsigned int		nr_meta_blocks;
+	unsigned int		nr_meta_zones;
+	unsigned int		nr_data_zones;
+	unsigned int		nr_rnd_zones;
+	unsigned int		nr_reserved_seq;
+	unsigned int		nr_chunks;
+
+	/* Zone information array */
+	struct dm_zone		*zones;
+
+	struct dm_zone		*sb_zone;
+	struct dmz_sb		sb[2];
+	unsigned int		mblk_primary;
+	u64			sb_gen;
+	unsigned int		min_nr_mblks;
+	unsigned int		max_nr_mblks;
+	atomic_t		nr_mblks;
+	struct rw_semaphore	mblk_sem;
+	struct mutex		mblk_flush_lock;
+	spinlock_t		mblk_lock;
+	struct rb_root		mblk_rbtree;
+	struct list_head	mblk_lru_list;
+	struct list_head	mblk_dirty_list;
+	struct shrinker		mblk_shrinker;
+
+	/* Zone allocation management */
+	struct mutex		map_lock;
+	struct dmz_mblock	**map_mblk;
+	unsigned int		nr_rnd;
+	atomic_t		unmap_nr_rnd;
+	struct list_head	unmap_rnd_list;
+	struct list_head	map_rnd_list;
+
+	unsigned int		nr_seq;
+	atomic_t		unmap_nr_seq;
+	struct list_head	unmap_seq_list;
+	struct list_head	map_seq_list;
+
+	atomic_t		nr_reserved_seq_zones;
+	struct list_head	reserved_seq_zones_list;
+
+	wait_queue_head_t	free_wq;
+};
+
+/*
+ * Various accessors
+ */
+unsigned int dmz_id(struct dmz_metadata *zmd, struct dm_zone *zone)
+{
+	return ((unsigned int)(zone - zmd->zones));
+}
+
+sector_t dmz_start_sect(struct dmz_metadata *zmd, struct dm_zone *zone)
+{
+	return (sector_t)dmz_id(zmd, zone) << zmd->dev->zone_nr_sectors_shift;
+}
+
+sector_t dmz_start_block(struct dmz_metadata *zmd, struct dm_zone *zone)
+{
+	return (sector_t)dmz_id(zmd, zone) << zmd->dev->zone_nr_blocks_shift;
+}
+
+unsigned int dmz_nr_chunks(struct dmz_metadata *zmd)
+{
+	return zmd->nr_chunks;
+}
+
+unsigned int dmz_nr_rnd_zones(struct dmz_metadata *zmd)
+{
+	return zmd->nr_rnd;
+}
+
+unsigned int dmz_nr_unmap_rnd_zones(struct dmz_metadata *zmd)
+{
+	return atomic_read(&zmd->unmap_nr_rnd);
+}
+
+/*
+ * Lock/unlock mapping table.
+ * The map lock also protects all the zone lists.
+ */
+void dmz_lock_map(struct dmz_metadata *zmd)
+{
+	mutex_lock(&zmd->map_lock);
+}
+
+void dmz_unlock_map(struct dmz_metadata *zmd)
+{
+	mutex_unlock(&zmd->map_lock);
+}
+
+/*
+ * Lock/unlock metadata access. This is a "read" lock on a semaphore
+ * that prevents metadata flush from running while metadata are being
+ * modified. The actual metadata write mutual exclusion is achieved with
+ * the map lock and zone styate management (active and reclaim state are
+ * mutually exclusive).
+ */
+void dmz_lock_metadata(struct dmz_metadata *zmd)
+{
+	down_read(&zmd->mblk_sem);
+}
+
+void dmz_unlock_metadata(struct dmz_metadata *zmd)
+{
+	up_read(&zmd->mblk_sem);
+}
+
+/*
+ * Lock/unlock flush: prevent concurrent executions
+ * of dmz_flush_metadata as well as metadata modification in reclaim
+ * while flush is being executed.
+ */
+void dmz_lock_flush(struct dmz_metadata *zmd)
+{
+	mutex_lock(&zmd->mblk_flush_lock);
+}
+
+void dmz_unlock_flush(struct dmz_metadata *zmd)
+{
+	mutex_unlock(&zmd->mblk_flush_lock);
+}
+
+/*
+ * Allocate a metadata block.
+ */
+static struct dmz_mblock *dmz_alloc_mblock(struct dmz_metadata *zmd,
+					   sector_t mblk_no)
+{
+	struct dmz_mblock *mblk = NULL;
+
+	/* See if we can reuse cached blocks */
+	if (zmd->max_nr_mblks && atomic_read(&zmd->nr_mblks) > zmd->max_nr_mblks) {
+		spin_lock(&zmd->mblk_lock);
+		mblk = list_first_entry_or_null(&zmd->mblk_lru_list,
+						struct dmz_mblock, link);
+		if (mblk) {
+			list_del_init(&mblk->link);
+			rb_erase(&mblk->node, &zmd->mblk_rbtree);
+			mblk->no = mblk_no;
+		}
+		spin_unlock(&zmd->mblk_lock);
+		if (mblk)
+			return mblk;
+	}
+
+	/* Allocate a new block */
+	mblk = kmalloc(sizeof(struct dmz_mblock), GFP_NOIO);
+	if (!mblk)
+		return NULL;
+
+	mblk->page = alloc_page(GFP_NOIO);
+	if (!mblk->page) {
+		kfree(mblk);
+		return NULL;
+	}
+
+	RB_CLEAR_NODE(&mblk->node);
+	INIT_LIST_HEAD(&mblk->link);
+	atomic_set(&mblk->ref, 0);
+	mblk->state = 0;
+	mblk->no = mblk_no;
+	mblk->data = page_address(mblk->page);
+
+	atomic_inc(&zmd->nr_mblks);
+
+	return mblk;
+}
+
+/*
+ * Free a metadata block.
+ */
+static void dmz_free_mblock(struct dmz_metadata *zmd, struct dmz_mblock *mblk)
+{
+	__free_pages(mblk->page, 0);
+	kfree(mblk);
+
+	atomic_dec(&zmd->nr_mblks);
+}
+
+/*
+ * Insert a metadata block in the rbtree.
+ */
+static void dmz_insert_mblock(struct dmz_metadata *zmd, struct dmz_mblock *mblk)
+{
+	struct rb_root *root = &zmd->mblk_rbtree;
+	struct rb_node **new = &(root->rb_node), *parent = NULL;
+	struct dmz_mblock *b;
+
+	/* Figure out where to put the new node */
+	while (*new) {
+		b = container_of(*new, struct dmz_mblock, node);
+		parent = *new;
+		new = (b->no < mblk->no) ? &((*new)->rb_left) : &((*new)->rb_right);
+	}
+
+	/* Add new node and rebalance tree */
+	rb_link_node(&mblk->node, parent, new);
+	rb_insert_color(&mblk->node, root);
+}
+
+/*
+ * Lookup a metadata block in the rbtree.
+ */
+static struct dmz_mblock *dmz_lookup_mblock(struct dmz_metadata *zmd,
+					    sector_t mblk_no)
+{
+	struct rb_root *root = &zmd->mblk_rbtree;
+	struct rb_node *node = root->rb_node;
+	struct dmz_mblock *mblk;
+
+	while (node) {
+		mblk = container_of(node, struct dmz_mblock, node);
+		if (mblk->no == mblk_no)
+			return mblk;
+		node = (mblk->no < mblk_no) ? node->rb_left : node->rb_right;
+	}
+
+	return NULL;
+}
+
+/*
+ * Metadata block BIO end callback.
+ */
+static void dmz_mblock_bio_end_io(struct bio *bio)
+{
+	struct dmz_mblock *mblk = bio->bi_private;
+	int flag;
+
+	if (bio->bi_status)
+		set_bit(DMZ_META_ERROR, &mblk->state);
+
+	if (bio_op(bio) == REQ_OP_WRITE)
+		flag = DMZ_META_WRITING;
+	else
+		flag = DMZ_META_READING;
+
+	clear_bit_unlock(flag, &mblk->state);
+	smp_mb__after_atomic();
+	wake_up_bit(&mblk->state, flag);
+
+	bio_put(bio);
+}
+
+/*
+ * Read a metadata block from disk.
+ */
+static struct dmz_mblock *dmz_fetch_mblock(struct dmz_metadata *zmd,
+					   sector_t mblk_no)
+{
+	struct dmz_mblock *mblk;
+	sector_t block = zmd->sb[zmd->mblk_primary].block + mblk_no;
+	struct bio *bio;
+
+	/* Get block and insert it */
+	mblk = dmz_alloc_mblock(zmd, mblk_no);
+	if (!mblk)
+		return NULL;
+
+	spin_lock(&zmd->mblk_lock);
+	atomic_inc(&mblk->ref);
+	set_bit(DMZ_META_READING, &mblk->state);
+	dmz_insert_mblock(zmd, mblk);
+	spin_unlock(&zmd->mblk_lock);
+
+	bio = bio_alloc(GFP_NOIO, 1);
+	if (!bio) {
+		dmz_free_mblock(zmd, mblk);
+		return NULL;
+	}
+
+	bio->bi_iter.bi_sector = dmz_blk2sect(block);
+	bio->bi_bdev = zmd->dev->bdev;
+	bio->bi_private = mblk;
+	bio->bi_end_io = dmz_mblock_bio_end_io;
+	bio_set_op_attrs(bio, REQ_OP_READ, REQ_META | REQ_PRIO);
+	bio_add_page(bio, mblk->page, DMZ_BLOCK_SIZE, 0);
+	submit_bio(bio);
+
+	return mblk;
+}
+
+/*
+ * Free metadata blocks.
+ */
+static unsigned long dmz_shrink_mblock_cache(struct dmz_metadata *zmd,
+					     unsigned long limit)
+{
+	struct dmz_mblock *mblk;
+	unsigned long count = 0;
+
+	if (!zmd->max_nr_mblks)
+		return 0;
+
+	while (!list_empty(&zmd->mblk_lru_list) &&
+	       atomic_read(&zmd->nr_mblks) > zmd->min_nr_mblks &&
+	       count < limit) {
+		mblk = list_first_entry(&zmd->mblk_lru_list,
+					struct dmz_mblock, link);
+		list_del_init(&mblk->link);
+		rb_erase(&mblk->node, &zmd->mblk_rbtree);
+		dmz_free_mblock(zmd, mblk);
+		count++;
+	}
+
+	return count;
+}
+
+/*
+ * For mblock shrinker: get the number of unused metadata blocks in the cache.
+ */
+static unsigned long dmz_mblock_shrinker_count(struct shrinker *shrink,
+					       struct shrink_control *sc)
+{
+	struct dmz_metadata *zmd = container_of(shrink, struct dmz_metadata, mblk_shrinker);
+
+	return atomic_read(&zmd->nr_mblks);
+}
+
+/*
+ * For mblock shrinker: scan unused metadata blocks and shrink the cache.
+ */
+static unsigned long dmz_mblock_shrinker_scan(struct shrinker *shrink,
+					      struct shrink_control *sc)
+{
+	struct dmz_metadata *zmd = container_of(shrink, struct dmz_metadata, mblk_shrinker);
+	unsigned long count;
+
+	spin_lock(&zmd->mblk_lock);
+	count = dmz_shrink_mblock_cache(zmd, sc->nr_to_scan);
+	spin_unlock(&zmd->mblk_lock);
+
+	return count ? count : SHRINK_STOP;
+}
+
+/*
+ * Release a metadata block.
+ */
+static void dmz_release_mblock(struct dmz_metadata *zmd,
+			       struct dmz_mblock *mblk)
+{
+
+	if (!mblk)
+		return;
+
+	spin_lock(&zmd->mblk_lock);
+
+	if (atomic_dec_and_test(&mblk->ref)) {
+		if (test_bit(DMZ_META_ERROR, &mblk->state)) {
+			rb_erase(&mblk->node, &zmd->mblk_rbtree);
+			dmz_free_mblock(zmd, mblk);
+		} else if (!test_bit(DMZ_META_DIRTY, &mblk->state)) {
+			list_add_tail(&mblk->link, &zmd->mblk_lru_list);
+			dmz_shrink_mblock_cache(zmd, 1);
+		}
+	}
+
+	spin_unlock(&zmd->mblk_lock);
+}
+
+/*
+ * Get a metadata block from the rbtree. If the block
+ * is not present, read it from disk.
+ */
+static struct dmz_mblock *dmz_get_mblock(struct dmz_metadata *zmd,
+					 sector_t mblk_no)
+{
+	struct dmz_mblock *mblk;
+
+	/* Check rbtree */
+	spin_lock(&zmd->mblk_lock);
+	mblk = dmz_lookup_mblock(zmd, mblk_no);
+	if (mblk) {
+		/* Cache hit: remove block from LRU list */
+		if (atomic_inc_return(&mblk->ref) == 1 &&
+		    !test_bit(DMZ_META_DIRTY, &mblk->state))
+			list_del_init(&mblk->link);
+	}
+	spin_unlock(&zmd->mblk_lock);
+
+	if (!mblk) {
+		/* Cache miss: read the block from disk */
+		mblk = dmz_fetch_mblock(zmd, mblk_no);
+		if (!mblk)
+			return ERR_PTR(-ENOMEM);
+	}
+
+	/* Wait for on-going read I/O and check for error */
+	wait_on_bit_io(&mblk->state, DMZ_META_READING,
+		       TASK_UNINTERRUPTIBLE);
+	if (test_bit(DMZ_META_ERROR, &mblk->state)) {
+		dmz_release_mblock(zmd, mblk);
+		return ERR_PTR(-EIO);
+	}
+
+	return mblk;
+}
+
+/*
+ * Mark a metadata block dirty.
+ */
+static void dmz_dirty_mblock(struct dmz_metadata *zmd, struct dmz_mblock *mblk)
+{
+	spin_lock(&zmd->mblk_lock);
+	if (!test_and_set_bit(DMZ_META_DIRTY, &mblk->state))
+		list_add_tail(&mblk->link, &zmd->mblk_dirty_list);
+	spin_unlock(&zmd->mblk_lock);
+}
+
+/*
+ * Issue a metadata block write BIO.
+ */
+static void dmz_write_mblock(struct dmz_metadata *zmd, struct dmz_mblock *mblk,
+			     unsigned int set)
+{
+	sector_t block = zmd->sb[set].block + mblk->no;
+	struct bio *bio;
+
+	bio = bio_alloc(GFP_NOIO, 1);
+	if (!bio) {
+		set_bit(DMZ_META_ERROR, &mblk->state);
+		return;
+	}
+
+	set_bit(DMZ_META_WRITING, &mblk->state);
+
+	bio->bi_iter.bi_sector = dmz_blk2sect(block);
+	bio->bi_bdev = zmd->dev->bdev;
+	bio->bi_private = mblk;
+	bio->bi_end_io = dmz_mblock_bio_end_io;
+	bio_set_op_attrs(bio, REQ_OP_WRITE, REQ_META | REQ_PRIO);
+	bio_add_page(bio, mblk->page, DMZ_BLOCK_SIZE, 0);
+	submit_bio(bio);
+}
+
+/*
+ * Read/write a metadata block.
+ */
+static int dmz_rdwr_block(struct dmz_metadata *zmd, int op, sector_t block,
+			  struct page *page)
+{
+	struct bio *bio;
+	int ret;
+
+	bio = bio_alloc(GFP_NOIO, 1);
+	if (!bio)
+		return -ENOMEM;
+
+	bio->bi_iter.bi_sector = dmz_blk2sect(block);
+	bio->bi_bdev = zmd->dev->bdev;
+	bio_set_op_attrs(bio, op, REQ_SYNC | REQ_META | REQ_PRIO);
+	bio_add_page(bio, page, DMZ_BLOCK_SIZE, 0);
+	ret = submit_bio_wait(bio);
+	bio_put(bio);
+
+	return ret;
+}
+
+/*
+ * Write super block of the specified metadata set.
+ */
+static int dmz_write_sb(struct dmz_metadata *zmd, unsigned int set)
+{
+	sector_t block = zmd->sb[set].block;
+	struct dmz_mblock *mblk = zmd->sb[set].mblk;
+	struct dmz_super *sb = zmd->sb[set].sb;
+	u64 sb_gen = zmd->sb_gen + 1;
+	int ret;
+
+	sb->magic = cpu_to_le32(DMZ_MAGIC);
+	sb->version = cpu_to_le32(DMZ_META_VER);
+
+	sb->gen = cpu_to_le64(sb_gen);
+
+	sb->sb_block = cpu_to_le64(block);
+	sb->nr_meta_blocks = cpu_to_le32(zmd->nr_meta_blocks);
+	sb->nr_reserved_seq = cpu_to_le32(zmd->nr_reserved_seq);
+	sb->nr_chunks = cpu_to_le32(zmd->nr_chunks);
+
+	sb->nr_map_blocks = cpu_to_le32(zmd->nr_map_blocks);
+	sb->nr_bitmap_blocks = cpu_to_le32(zmd->nr_bitmap_blocks);
+
+	sb->crc = 0;
+	sb->crc = cpu_to_le32(crc32_le(sb_gen, (unsigned char *)sb, DMZ_BLOCK_SIZE));
+
+	ret = dmz_rdwr_block(zmd, REQ_OP_WRITE, block, mblk->page);
+	if (ret == 0)
+		ret = blkdev_issue_flush(zmd->dev->bdev, GFP_KERNEL, NULL);
+
+	return ret;
+}
+
+/*
+ * Write dirty metadata blocks to the specified set.
+ */
+static int dmz_write_dirty_mblocks(struct dmz_metadata *zmd,
+				   struct list_head *write_list,
+				   unsigned int set)
+{
+	struct dmz_mblock *mblk;
+	struct blk_plug plug;
+	int ret = 0;
+
+	/* Issue writes */
+	blk_start_plug(&plug);
+	list_for_each_entry(mblk, write_list, link)
+		dmz_write_mblock(zmd, mblk, set);
+	blk_finish_plug(&plug);
+
+	/* Wait for completion */
+	list_for_each_entry(mblk, write_list, link) {
+		wait_on_bit_io(&mblk->state, DMZ_META_WRITING,
+			       TASK_UNINTERRUPTIBLE);
+		if (test_bit(DMZ_META_ERROR, &mblk->state)) {
+			clear_bit(DMZ_META_ERROR, &mblk->state);
+			ret = -EIO;
+		}
+	}
+
+	/* Flush drive cache (this will also sync data) */
+	if (ret == 0)
+		ret = blkdev_issue_flush(zmd->dev->bdev, GFP_KERNEL, NULL);
+
+	return ret;
+}
+
+/*
+ * Log dirty metadata blocks.
+ */
+static int dmz_log_dirty_mblocks(struct dmz_metadata *zmd,
+				 struct list_head *write_list)
+{
+	unsigned int log_set = zmd->mblk_primary ^ 0x1;
+	int ret;
+
+	/* Write dirty blocks to the log */
+	ret = dmz_write_dirty_mblocks(zmd, write_list, log_set);
+	if (ret)
+		return ret;
+
+	/*
+	 * No error so far: now validate the log by updating the
+	 * log index super block generation.
+	 */
+	ret = dmz_write_sb(zmd, log_set);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+/*
+ * Flush dirty metadata blocks.
+ */
+int dmz_flush_metadata(struct dmz_metadata *zmd)
+{
+	struct dmz_mblock *mblk;
+	struct list_head write_list;
+	int ret;
+
+	if (WARN_ON(!zmd))
+		return 0;
+
+	INIT_LIST_HEAD(&write_list);
+
+	/*
+	 * Make sure that metadata blocks are stable before logging: take
+	 * the write lock on the metadata semaphore to prevent target BIOs
+	 * from modifying metadata.
+	 */
+	down_write(&zmd->mblk_sem);
+
+	/*
+	 * This is called from the target flush work and reclaim work.
+	 * Concurrent execution is not allowed.
+	 */
+	dmz_lock_flush(zmd);
+
+	/* Get dirty blocks */
+	spin_lock(&zmd->mblk_lock);
+	list_splice_init(&zmd->mblk_dirty_list, &write_list);
+	spin_unlock(&zmd->mblk_lock);
+
+	/* If there are no dirty metadata blocks, just flush the device cache */
+	if (list_empty(&write_list)) {
+		ret = blkdev_issue_flush(zmd->dev->bdev, GFP_KERNEL, NULL);
+		goto out;
+	}
+
+	/*
+	 * The primary metadata set is still clean. Keep it this way until
+	 * all updates are successful in the secondary set. That is, use
+	 * the secondary set as a log.
+	 */
+	ret = dmz_log_dirty_mblocks(zmd, &write_list);
+	if (ret)
+		goto out;
+
+	/*
+	 * The log is on disk. It is now safe to update in place
+	 * in the primary metadata set.
+	 */
+	ret = dmz_write_dirty_mblocks(zmd, &write_list, zmd->mblk_primary);
+	if (ret)
+		goto out;
+
+	ret = dmz_write_sb(zmd, zmd->mblk_primary);
+	if (ret)
+		goto out;
+
+	while (!list_empty(&write_list)) {
+		mblk = list_first_entry(&write_list, struct dmz_mblock, link);
+		list_del_init(&mblk->link);
+
+		spin_lock(&zmd->mblk_lock);
+		clear_bit(DMZ_META_DIRTY, &mblk->state);
+		if (atomic_read(&mblk->ref) == 0)
+			list_add_tail(&mblk->link, &zmd->mblk_lru_list);
+		spin_unlock(&zmd->mblk_lock);
+	}
+
+	zmd->sb_gen++;
+out:
+	if (ret && !list_empty(&write_list)) {
+		spin_lock(&zmd->mblk_lock);
+		list_splice(&write_list, &zmd->mblk_dirty_list);
+		spin_unlock(&zmd->mblk_lock);
+	}
+
+	dmz_unlock_flush(zmd);
+	up_write(&zmd->mblk_sem);
+
+	return ret;
+}
+
+/*
+ * Check super block.
+ */
+static int dmz_check_sb(struct dmz_metadata *zmd, struct dmz_super *sb)
+{
+	unsigned int nr_meta_zones, nr_data_zones;
+	struct dmz_dev *dev = zmd->dev;
+	u32 crc, stored_crc;
+	u64 gen;
+
+	gen = le64_to_cpu(sb->gen);
+	stored_crc = le32_to_cpu(sb->crc);
+	sb->crc = 0;
+	crc = crc32_le(gen, (unsigned char *)sb, DMZ_BLOCK_SIZE);
+	if (crc != stored_crc) {
+		dmz_dev_err(dev, "Invalid checksum (needed 0x%08x, got 0x%08x)",
+			    crc, stored_crc);
+		return -ENXIO;
+	}
+
+	if (le32_to_cpu(sb->magic) != DMZ_MAGIC) {
+		dmz_dev_err(dev, "Invalid meta magic (needed 0x%08x, got 0x%08x)",
+			    DMZ_MAGIC, le32_to_cpu(sb->magic));
+		return -ENXIO;
+	}
+
+	if (le32_to_cpu(sb->version) != DMZ_META_VER) {
+		dmz_dev_err(dev, "Invalid meta version (needed %d, got %d)",
+			    DMZ_META_VER, le32_to_cpu(sb->version));
+		return -ENXIO;
+	}
+
+	nr_meta_zones = (le32_to_cpu(sb->nr_meta_blocks) + dev->zone_nr_blocks - 1)
+		>> dev->zone_nr_blocks_shift;
+	if (!nr_meta_zones ||
+	    nr_meta_zones >= zmd->nr_rnd_zones) {
+		dmz_dev_err(dev, "Invalid number of metadata blocks");
+		return -ENXIO;
+	}
+
+	if (!le32_to_cpu(sb->nr_reserved_seq) ||
+	    le32_to_cpu(sb->nr_reserved_seq) >= (zmd->nr_useable_zones - nr_meta_zones)) {
+		dmz_dev_err(dev, "Invalid number of reserved sequential zones");
+		return -ENXIO;
+	}
+
+	nr_data_zones = zmd->nr_useable_zones -
+		(nr_meta_zones * 2 + le32_to_cpu(sb->nr_reserved_seq));
+	if (le32_to_cpu(sb->nr_chunks) > nr_data_zones) {
+		dmz_dev_err(dev, "Invalid number of chunks %u / %u",
+			    le32_to_cpu(sb->nr_chunks), nr_data_zones);
+		return -ENXIO;
+	}
+
+	/* OK */
+	zmd->nr_meta_blocks = le32_to_cpu(sb->nr_meta_blocks);
+	zmd->nr_reserved_seq = le32_to_cpu(sb->nr_reserved_seq);
+	zmd->nr_chunks = le32_to_cpu(sb->nr_chunks);
+	zmd->nr_map_blocks = le32_to_cpu(sb->nr_map_blocks);
+	zmd->nr_bitmap_blocks = le32_to_cpu(sb->nr_bitmap_blocks);
+	zmd->nr_meta_zones = nr_meta_zones;
+	zmd->nr_data_zones = nr_data_zones;
+
+	return 0;
+}
+
+/*
+ * Read the first or second super block from disk.
+ */
+static int dmz_read_sb(struct dmz_metadata *zmd, unsigned int set)
+{
+	return dmz_rdwr_block(zmd, REQ_OP_READ, zmd->sb[set].block,
+			      zmd->sb[set].mblk->page);
+}
+
+/*
+ * Determine the position of the secondary super blocks on disk.
+ * This is used only if a corruption of the primary super block
+ * is detected.
+ */
+static int dmz_lookup_secondary_sb(struct dmz_metadata *zmd)
+{
+	unsigned int zone_nr_blocks = zmd->dev->zone_nr_blocks;
+	struct dmz_mblock *mblk;
+	int i;
+
+	/* Allocate a block */
+	mblk = dmz_alloc_mblock(zmd, 0);
+	if (!mblk)
+		return -ENOMEM;
+
+	zmd->sb[1].mblk = mblk;
+	zmd->sb[1].sb = mblk->data;
+
+	/* Bad first super block: search for the second one */
+	zmd->sb[1].block = zmd->sb[0].block + zone_nr_blocks;
+	for (i = 0; i < zmd->nr_rnd_zones - 1; i++) {
+		if (dmz_read_sb(zmd, 1) != 0)
+			break;
+		if (le32_to_cpu(zmd->sb[1].sb->magic) == DMZ_MAGIC)
+			return 0;
+		zmd->sb[1].block += zone_nr_blocks;
+	}
+
+	dmz_free_mblock(zmd, mblk);
+	zmd->sb[1].mblk = NULL;
+
+	return -EIO;
+}
+
+/*
+ * Read the first or second super block from disk.
+ */
+static int dmz_get_sb(struct dmz_metadata *zmd, unsigned int set)
+{
+	struct dmz_mblock *mblk;
+	int ret;
+
+	/* Allocate a block */
+	mblk = dmz_alloc_mblock(zmd, 0);
+	if (!mblk)
+		return -ENOMEM;
+
+	zmd->sb[set].mblk = mblk;
+	zmd->sb[set].sb = mblk->data;
+
+	/* Read super block */
+	ret = dmz_read_sb(zmd, set);
+	if (ret) {
+		dmz_free_mblock(zmd, mblk);
+		zmd->sb[set].mblk = NULL;
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Recover a metadata set.
+ */
+static int dmz_recover_mblocks(struct dmz_metadata *zmd, unsigned int dst_set)
+{
+	unsigned int src_set = dst_set ^ 0x1;
+	struct page *page;
+	int i, ret;
+
+	dmz_dev_warn(zmd->dev, "Metadata set %u invalid: recovering", dst_set);
+
+	if (dst_set == 0)
+		zmd->sb[0].block = dmz_start_block(zmd, zmd->sb_zone);
+	else {
+		zmd->sb[1].block = zmd->sb[0].block +
+			(zmd->nr_meta_zones << zmd->dev->zone_nr_blocks_shift);
+	}
+
+	page = alloc_page(GFP_KERNEL);
+	if (!page)
+		return -ENOMEM;
+
+	/* Copy metadata blocks */
+	for (i = 1; i < zmd->nr_meta_blocks; i++) {
+		ret = dmz_rdwr_block(zmd, REQ_OP_READ,
+				     zmd->sb[src_set].block + i, page);
+		if (ret)
+			goto out;
+		ret = dmz_rdwr_block(zmd, REQ_OP_WRITE,
+				     zmd->sb[dst_set].block + i, page);
+		if (ret)
+			goto out;
+	}
+
+	/* Finalize with the super block */
+	if (!zmd->sb[dst_set].mblk) {
+		zmd->sb[dst_set].mblk = dmz_alloc_mblock(zmd, 0);
+		if (!zmd->sb[dst_set].mblk) {
+			ret = -ENOMEM;
+			goto out;
+		}
+		zmd->sb[dst_set].sb = zmd->sb[dst_set].mblk->data;
+	}
+
+	ret = dmz_write_sb(zmd, dst_set);
+out:
+	__free_pages(page, 0);
+
+	return ret;
+}
+
+/*
+ * Get super block from disk.
+ */
+static int dmz_load_sb(struct dmz_metadata *zmd)
+{
+	bool sb_good[2] = {false, false};
+	u64 sb_gen[2] = {0, 0};
+	int ret;
+
+	/* Read and check the primary super block */
+	zmd->sb[0].block = dmz_start_block(zmd, zmd->sb_zone);
+	ret = dmz_get_sb(zmd, 0);
+	if (ret) {
+		dmz_dev_err(zmd->dev, "Read primary super block failed");
+		return ret;
+	}
+
+	ret = dmz_check_sb(zmd, zmd->sb[0].sb);
+
+	/* Read and check secondary super block */
+	if (ret == 0) {
+		sb_good[0] = true;
+		zmd->sb[1].block = zmd->sb[0].block +
+			(zmd->nr_meta_zones << zmd->dev->zone_nr_blocks_shift);
+		ret = dmz_get_sb(zmd, 1);
+	} else
+		ret = dmz_lookup_secondary_sb(zmd);
+
+	if (ret) {
+		dmz_dev_err(zmd->dev, "Read secondary super block failed");
+		return ret;
+	}
+
+	ret = dmz_check_sb(zmd, zmd->sb[1].sb);
+	if (ret == 0)
+		sb_good[1] = true;
+
+	/* Use highest generation sb first */
+	if (!sb_good[0] && !sb_good[1]) {
+		dmz_dev_err(zmd->dev, "No valid super block found");
+		return -EIO;
+	}
+
+	if (sb_good[0])
+		sb_gen[0] = le64_to_cpu(zmd->sb[0].sb->gen);
+	else
+		ret = dmz_recover_mblocks(zmd, 0);
+
+	if (sb_good[1])
+		sb_gen[1] = le64_to_cpu(zmd->sb[1].sb->gen);
+	else
+		ret = dmz_recover_mblocks(zmd, 1);
+
+	if (ret) {
+		dmz_dev_err(zmd->dev, "Recovery failed");
+		return -EIO;
+	}
+
+	if (sb_gen[0] >= sb_gen[1]) {
+		zmd->sb_gen = sb_gen[0];
+		zmd->mblk_primary = 0;
+	} else {
+		zmd->sb_gen = sb_gen[1];
+		zmd->mblk_primary = 1;
+	}
+
+	dmz_dev_debug(zmd->dev, "Using super block %u (gen %llu)",
+		      zmd->mblk_primary, zmd->sb_gen);
+
+	return 0;
+}
+
+/*
+ * Initialize a zone descriptor.
+ */
+static int dmz_init_zone(struct dmz_metadata *zmd, struct dm_zone *zone,
+			 struct blk_zone *blkz)
+{
+	struct dmz_dev *dev = zmd->dev;
+
+	/* Ignore the eventual last runt (smaller) zone */
+	if (blkz->len != dev->zone_nr_sectors) {
+		if (blkz->start + blkz->len == dev->capacity)
+			return 0;
+		return -ENXIO;
+	}
+
+	INIT_LIST_HEAD(&zone->link);
+	atomic_set(&zone->refcount, 0);
+	zone->chunk = DMZ_MAP_UNMAPPED;
+
+	if (blkz->type == BLK_ZONE_TYPE_CONVENTIONAL) {
+		set_bit(DMZ_RND, &zone->flags);
+		zmd->nr_rnd_zones++;
+	} else if (blkz->type == BLK_ZONE_TYPE_SEQWRITE_REQ ||
+		   blkz->type == BLK_ZONE_TYPE_SEQWRITE_PREF) {
+		set_bit(DMZ_SEQ, &zone->flags);
+	} else
+		return -ENXIO;
+
+	if (blkz->cond == BLK_ZONE_COND_OFFLINE)
+		set_bit(DMZ_OFFLINE, &zone->flags);
+	else if (blkz->cond == BLK_ZONE_COND_READONLY)
+		set_bit(DMZ_READ_ONLY, &zone->flags);
+
+	if (dmz_is_rnd(zone))
+		zone->wp_block = 0;
+	else
+		zone->wp_block = dmz_sect2blk(blkz->wp - blkz->start);
+
+	if (!dmz_is_offline(zone) && !dmz_is_readonly(zone)) {
+		zmd->nr_useable_zones++;
+		if (dmz_is_rnd(zone)) {
+			zmd->nr_rnd_zones++;
+			if (!zmd->sb_zone) {
+				/* Super block zone */
+				zmd->sb_zone = zone;
+			}
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Free zones descriptors.
+ */
+static void dmz_drop_zones(struct dmz_metadata *zmd)
+{
+	kfree(zmd->zones);
+	zmd->zones = NULL;
+}
+
+/*
+ * The size of a zone report in number of zones.
+ * This results in 4096*64B=256KB report zones commands.
+ */
+#define DMZ_REPORT_NR_ZONES	4096
+
+/*
+ * Allocate and initialize zone descriptors using the zone
+ * information from disk.
+ */
+static int dmz_init_zones(struct dmz_metadata *zmd)
+{
+	struct dmz_dev *dev = zmd->dev;
+	struct dm_zone *zone;
+	struct blk_zone *blkz;
+	unsigned int nr_blkz;
+	sector_t sector = 0;
+	int i, ret = 0;
+
+	/* Init */
+	zmd->zone_bitmap_size = dev->zone_nr_blocks >> 3;
+	zmd->zone_nr_bitmap_blocks = zmd->zone_bitmap_size >> DMZ_BLOCK_SHIFT;
+
+	/* Allocate zone array */
+	zmd->zones = kcalloc(dev->nr_zones, sizeof(struct dm_zone), GFP_KERNEL);
+	if (!zmd->zones)
+		return -ENOMEM;
+
+	dmz_dev_info(dev, "Using %zu B for zone information",
+		     sizeof(struct dm_zone) * dev->nr_zones);
+
+	/* Get zone information */
+	nr_blkz = DMZ_REPORT_NR_ZONES;
+	blkz = kcalloc(nr_blkz, sizeof(struct blk_zone), GFP_KERNEL);
+	if (!blkz) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/*
+	 * Get zone information and initialize zone descriptors.
+	 * At the same time, determine where the super block
+	 * should be: first block of the first randomly writable
+	 * zone.
+	 */
+	zone = zmd->zones;
+	while (sector < dev->capacity) {
+		/* Get zone information */
+		nr_blkz = DMZ_REPORT_NR_ZONES;
+		ret = blkdev_report_zones(dev->bdev, sector, blkz,
+					  &nr_blkz, GFP_KERNEL);
+		if (ret) {
+			dmz_dev_err(dev, "Report zones failed %d", ret);
+			goto out;
+		}
+
+		/* Process report */
+		for (i = 0; i < nr_blkz; i++) {
+			ret = dmz_init_zone(zmd, zone, &blkz[i]);
+			if (ret)
+				goto out;
+			sector += dev->zone_nr_sectors;
+			zone++;
+		}
+	}
+
+	/* The entire zone configuration of the disk should now be known */
+	if (sector < dev->capacity) {
+		dmz_dev_err(dev, "Failed to get correct zone information");
+		ret = -ENXIO;
+	}
+out:
+	kfree(blkz);
+	if (ret)
+		dmz_drop_zones(zmd);
+
+	return ret;
+}
+
+/*
+ * Update a zone information.
+ */
+static int dmz_update_zone(struct dmz_metadata *zmd, struct dm_zone *zone)
+{
+	unsigned int nr_blkz = 1;
+	struct blk_zone blkz;
+	int ret;
+
+	/* Get zone information from disk */
+	ret = blkdev_report_zones(zmd->dev->bdev, dmz_start_sect(zmd, zone),
+				  &blkz, &nr_blkz, GFP_KERNEL);
+	if (ret) {
+		dmz_dev_err(zmd->dev, "Get zone %u report failed",
+			    dmz_id(zmd, zone));
+		return ret;
+	}
+
+	clear_bit(DMZ_OFFLINE, &zone->flags);
+	clear_bit(DMZ_READ_ONLY, &zone->flags);
+	if (blkz.cond == BLK_ZONE_COND_OFFLINE)
+		set_bit(DMZ_OFFLINE, &zone->flags);
+	else if (blkz.cond == BLK_ZONE_COND_READONLY)
+		set_bit(DMZ_READ_ONLY, &zone->flags);
+
+	if (dmz_is_seq(zone))
+		zone->wp_block = dmz_sect2blk(blkz.wp - blkz.start);
+	else
+		zone->wp_block = 0;
+
+	return 0;
+}
+
+/*
+ * Check a zone write pointer position when the zone is marked
+ * with the sequential write error flag.
+ */
+static int dmz_handle_seq_write_err(struct dmz_metadata *zmd,
+				    struct dm_zone *zone)
+{
+	unsigned int wp = 0;
+	int ret;
+
+	wp = zone->wp_block;
+	ret = dmz_update_zone(zmd, zone);
+	if (ret)
+		return ret;
+
+	dmz_dev_warn(zmd->dev, "Processing zone %u write error (zone wp %u/%u)",
+		     dmz_id(zmd, zone), zone->wp_block, wp);
+
+	if (zone->wp_block < wp) {
+		dmz_invalidate_blocks(zmd, zone, zone->wp_block,
+				      wp - zone->wp_block);
+	}
+
+	return 0;
+}
+
+static struct dm_zone *dmz_get(struct dmz_metadata *zmd, unsigned int zone_id)
+{
+	return &zmd->zones[zone_id];
+}
+
+/*
+ * Reset a zone write pointer.
+ */
+static int dmz_reset_zone(struct dmz_metadata *zmd, struct dm_zone *zone)
+{
+	int ret;
+
+	/*
+	 * Ignore offline zones, read only zones,
+	 * and conventional zones.
+	 */
+	if (dmz_is_offline(zone) ||
+	    dmz_is_readonly(zone) ||
+	    dmz_is_rnd(zone))
+		return 0;
+
+	if (!dmz_is_empty(zone) || dmz_seq_write_err(zone)) {
+		struct dmz_dev *dev = zmd->dev;
+
+		ret = blkdev_reset_zones(dev->bdev,
+					 dmz_start_sect(zmd, zone),
+					 dev->zone_nr_sectors, GFP_KERNEL);
+		if (ret) {
+			dmz_dev_err(dev, "Reset zone %u failed %d",
+				    dmz_id(zmd, zone), ret);
+			return ret;
+		}
+	}
+
+	/* Clear write error bit and rewind write pointer position */
+	clear_bit(DMZ_SEQ_WRITE_ERR, &zone->flags);
+	zone->wp_block = 0;
+
+	return 0;
+}
+
+static void dmz_get_zone_weight(struct dmz_metadata *zmd, struct dm_zone *zone);
+
+/*
+ * Initialize chunk mapping.
+ */
+static int dmz_load_mapping(struct dmz_metadata *zmd)
+{
+	struct dmz_dev *dev = zmd->dev;
+	struct dm_zone *dzone, *bzone;
+	struct dmz_mblock *dmap_mblk = NULL;
+	struct dmz_map *dmap;
+	unsigned int i = 0, e = 0, chunk = 0;
+	unsigned int dzone_id;
+	unsigned int bzone_id;
+
+	/* Metadata block array for the chunk mapping table */
+	zmd->map_mblk = kcalloc(zmd->nr_map_blocks,
+				sizeof(struct dmz_mblk *), GFP_KERNEL);
+	if (!zmd->map_mblk)
+		return -ENOMEM;
+
+	/* Get chunk mapping table blocks and initialize zone mapping */
+	while (chunk < zmd->nr_chunks) {
+		if (!dmap_mblk) {
+			/* Get mapping block */
+			dmap_mblk = dmz_get_mblock(zmd, i + 1);
+			if (IS_ERR(dmap_mblk))
+				return PTR_ERR(dmap_mblk);
+			zmd->map_mblk[i] = dmap_mblk;
+			dmap = (struct dmz_map *) dmap_mblk->data;
+			i++;
+			e = 0;
+		}
+
+		/* Check data zone */
+		dzone_id = le32_to_cpu(dmap[e].dzone_id);
+		if (dzone_id == DMZ_MAP_UNMAPPED)
+			goto next;
+
+		if (dzone_id >= dev->nr_zones) {
+			dmz_dev_err(dev, "Chunk %u mapping: invalid data zone ID %u",
+				    chunk, dzone_id);
+			return -EIO;
+		}
+
+		dzone = dmz_get(zmd, dzone_id);
+		set_bit(DMZ_DATA, &dzone->flags);
+		dzone->chunk = chunk;
+		dmz_get_zone_weight(zmd, dzone);
+
+		if (dmz_is_rnd(dzone))
+			list_add_tail(&dzone->link, &zmd->map_rnd_list);
+		else
+			list_add_tail(&dzone->link, &zmd->map_seq_list);
+
+		/* Check buffer zone */
+		bzone_id = le32_to_cpu(dmap[e].bzone_id);
+		if (bzone_id == DMZ_MAP_UNMAPPED)
+			goto next;
+
+		if (bzone_id >= dev->nr_zones) {
+			dmz_dev_err(dev, "Chunk %u mapping: invalid buffer zone ID %u",
+				    chunk, bzone_id);
+			return -EIO;
+		}
+
+		bzone = dmz_get(zmd, bzone_id);
+		if (!dmz_is_rnd(bzone)) {
+			dmz_dev_err(dev, "Chunk %u mapping: invalid buffer zone %u",
+				    chunk, bzone_id);
+			return -EIO;
+		}
+
+		set_bit(DMZ_DATA, &bzone->flags);
+		set_bit(DMZ_BUF, &bzone->flags);
+		bzone->chunk = chunk;
+		bzone->bzone = dzone;
+		dzone->bzone = bzone;
+		dmz_get_zone_weight(zmd, bzone);
+		list_add_tail(&bzone->link, &zmd->map_rnd_list);
+next:
+		chunk++;
+		e++;
+		if (e >= DMZ_MAP_ENTRIES)
+			dmap_mblk = NULL;
+	}
+
+	/*
+	 * At this point, only meta zones and mapped data zones were
+	 * fully initialized. All remaining zones are unmapped data
+	 * zones. Finish initializing those here.
+	 */
+	for (i = 0; i < dev->nr_zones; i++) {
+		dzone = dmz_get(zmd, i);
+		if (dmz_is_meta(dzone))
+			continue;
+
+		if (dmz_is_rnd(dzone))
+			zmd->nr_rnd++;
+		else
+			zmd->nr_seq++;
+
+		if (dmz_is_data(dzone)) {
+			/* Already initialized */
+			continue;
+		}
+
+		/* Unmapped data zone */
+		set_bit(DMZ_DATA, &dzone->flags);
+		dzone->chunk = DMZ_MAP_UNMAPPED;
+		if (dmz_is_rnd(dzone)) {
+			list_add_tail(&dzone->link, &zmd->unmap_rnd_list);
+			atomic_inc(&zmd->unmap_nr_rnd);
+		} else if (atomic_read(&zmd->nr_reserved_seq_zones) < zmd->nr_reserved_seq) {
+			list_add_tail(&dzone->link, &zmd->reserved_seq_zones_list);
+			atomic_inc(&zmd->nr_reserved_seq_zones);
+			zmd->nr_seq--;
+		} else {
+			list_add_tail(&dzone->link, &zmd->unmap_seq_list);
+			atomic_inc(&zmd->unmap_nr_seq);
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Set a data chunk mapping.
+ */
+static void dmz_set_chunk_mapping(struct dmz_metadata *zmd, unsigned int chunk,
+				  unsigned int dzone_id, unsigned int bzone_id)
+{
+	struct dmz_mblock *dmap_mblk = zmd->map_mblk[chunk >> DMZ_MAP_ENTRIES_SHIFT];
+	struct dmz_map *dmap = (struct dmz_map *) dmap_mblk->data;
+	int map_idx = chunk & DMZ_MAP_ENTRIES_MASK;
+
+	dmap[map_idx].dzone_id = cpu_to_le32(dzone_id);
+	dmap[map_idx].bzone_id = cpu_to_le32(bzone_id);
+	dmz_dirty_mblock(zmd, dmap_mblk);
+}
+
+/*
+ * The list of mapped zones is maintained in LRU order.
+ * This rotates a zone at the end of its map list.
+ */
+static void __dmz_lru_zone(struct dmz_metadata *zmd, struct dm_zone *zone)
+{
+	if (list_empty(&zone->link))
+		return;
+
+	list_del_init(&zone->link);
+	if (dmz_is_seq(zone)) {
+		/* LRU rotate sequential zone */
+		list_add_tail(&zone->link, &zmd->map_seq_list);
+	} else {
+		/* LRU rotate random zone */
+		list_add_tail(&zone->link, &zmd->map_rnd_list);
+	}
+}
+
+/*
+ * The list of mapped random zones is maintained
+ * in LRU order. This rotates a zone at the end of the list.
+ */
+static void dmz_lru_zone(struct dmz_metadata *zmd, struct dm_zone *zone)
+{
+	__dmz_lru_zone(zmd, zone);
+	if (zone->bzone)
+		__dmz_lru_zone(zmd, zone->bzone);
+}
+
+/*
+ * Wait for any zone to be freed.
+ */
+static void dmz_wait_for_free_zones(struct dmz_metadata *zmd)
+{
+	DEFINE_WAIT(wait);
+
+	prepare_to_wait(&zmd->free_wq, &wait, TASK_UNINTERRUPTIBLE);
+	dmz_unlock_map(zmd);
+	dmz_unlock_metadata(zmd);
+
+	io_schedule_timeout(HZ);
+
+	dmz_lock_metadata(zmd);
+	dmz_lock_map(zmd);
+	finish_wait(&zmd->free_wq, &wait);
+}
+
+/*
+ * Lock a zone for reclaim (set the zone RECLAIM bit).
+ * Returns false if the zone cannot be locked or if it is already locked
+ * and 1 otherwise.
+ */
+int dmz_lock_zone_reclaim(struct dm_zone *zone)
+{
+	/* Active zones cannot be reclaimed */
+	if (dmz_is_active(zone))
+		return 0;
+
+	return !test_and_set_bit(DMZ_RECLAIM, &zone->flags);
+}
+
+/*
+ * Clear a zone reclaim flag.
+ */
+void dmz_unlock_zone_reclaim(struct dm_zone *zone)
+{
+	WARN_ON(dmz_is_active(zone));
+	WARN_ON(!dmz_in_reclaim(zone));
+
+	clear_bit_unlock(DMZ_RECLAIM, &zone->flags);
+	smp_mb__after_atomic();
+	wake_up_bit(&zone->flags, DMZ_RECLAIM);
+}
+
+/*
+ * Wait for a zone reclaim to complete.
+ */
+static void dmz_wait_for_reclaim(struct dmz_metadata *zmd, struct dm_zone *zone)
+{
+	dmz_unlock_map(zmd);
+	dmz_unlock_metadata(zmd);
+	wait_on_bit_timeout(&zone->flags, DMZ_RECLAIM, TASK_UNINTERRUPTIBLE, HZ);
+	dmz_lock_metadata(zmd);
+	dmz_lock_map(zmd);
+}
+
+/*
+ * Select a random write zone for reclaim.
+ */
+static struct dm_zone *dmz_get_rnd_zone_for_reclaim(struct dmz_metadata *zmd)
+{
+	struct dm_zone *dzone = NULL;
+	struct dm_zone *zone;
+
+	if (list_empty(&zmd->map_rnd_list))
+		return NULL;
+
+	list_for_each_entry(zone, &zmd->map_rnd_list, link) {
+		if (dmz_is_buf(zone))
+			dzone = zone->bzone;
+		else
+			dzone = zone;
+		if (dmz_lock_zone_reclaim(dzone))
+			return dzone;
+	}
+
+	return NULL;
+}
+
+/*
+ * Select a buffered sequential zone for reclaim.
+ */
+static struct dm_zone *dmz_get_seq_zone_for_reclaim(struct dmz_metadata *zmd)
+{
+	struct dm_zone *zone;
+
+	if (list_empty(&zmd->map_seq_list))
+		return NULL;
+
+	list_for_each_entry(zone, &zmd->map_seq_list, link) {
+		if (!zone->bzone)
+			continue;
+		if (dmz_lock_zone_reclaim(zone))
+			return zone;
+	}
+
+	return NULL;
+}
+
+/*
+ * Select a zone for reclaim.
+ */
+struct dm_zone *dmz_get_zone_for_reclaim(struct dmz_metadata *zmd)
+{
+	struct dm_zone *zone;
+
+	/*
+	 * Search for a zone candidate to reclaim: 2 cases are possible.
+	 * (1) There is no free sequential zones. Then a random data zone
+	 *     cannot be reclaimed. So choose a sequential zone to reclaim so
+	 *     that afterward a random zone can be reclaimed.
+	 * (2) At least one free sequential zone is available, then choose
+	 *     the oldest random zone (data or buffer) that can be locked.
+	 */
+	dmz_lock_map(zmd);
+	if (list_empty(&zmd->reserved_seq_zones_list))
+		zone = dmz_get_seq_zone_for_reclaim(zmd);
+	else
+		zone = dmz_get_rnd_zone_for_reclaim(zmd);
+	dmz_unlock_map(zmd);
+
+	return zone;
+}
+
+/*
+ * Activate a zone (increment its reference count).
+ */
+void dmz_activate_zone(struct dm_zone *zone)
+{
+	set_bit(DMZ_ACTIVE, &zone->flags);
+	atomic_inc(&zone->refcount);
+}
+
+/*
+ * Deactivate a zone. This decrement the zone reference counter
+ * and clears the active state of the zone once the count reaches 0,
+ * indicating that all BIOs to the zone have completed. Returns
+ * true if the zone was deactivated.
+ */
+void dmz_deactivate_zone(struct dm_zone *zone)
+{
+	if (atomic_dec_and_test(&zone->refcount)) {
+		WARN_ON(!test_bit(DMZ_ACTIVE, &zone->flags));
+		clear_bit_unlock(DMZ_ACTIVE, &zone->flags);
+		smp_mb__after_atomic();
+	}
+}
+
+/*
+ * Get the zone mapping a chunk, if the chunk is mapped already.
+ * If no mapping exist and the operation is WRITE, a zone is
+ * allocated and used to map the chunk.
+ * The zone returned will be set to the active state.
+ */
+struct dm_zone *dmz_get_chunk_mapping(struct dmz_metadata *zmd, unsigned int chunk, int op)
+{
+	struct dmz_mblock *dmap_mblk = zmd->map_mblk[chunk >> DMZ_MAP_ENTRIES_SHIFT];
+	struct dmz_map *dmap = (struct dmz_map *) dmap_mblk->data;
+	int dmap_idx = chunk & DMZ_MAP_ENTRIES_MASK;
+	unsigned int dzone_id;
+	struct dm_zone *dzone = NULL;
+	int ret = 0;
+
+	dmz_lock_map(zmd);
+again:
+	/* Get the chunk mapping */
+	dzone_id = le32_to_cpu(dmap[dmap_idx].dzone_id);
+	if (dzone_id == DMZ_MAP_UNMAPPED) {
+		/*
+		 * Read or discard in unmapped chunks are fine. But for
+		 * writes, we need a mapping, so get one.
+		 */
+		if (op != REQ_OP_WRITE)
+			goto out;
+
+		/* Alloate a random zone */
+		dzone = dmz_alloc_zone(zmd, DMZ_ALLOC_RND);
+		if (!dzone) {
+			dmz_wait_for_free_zones(zmd);
+			goto again;
+		}
+
+		dmz_map_zone(zmd, dzone, chunk);
+
+	} else {
+		/* The chunk is already mapped: get the mapping zone */
+		dzone = dmz_get(zmd, dzone_id);
+		if (dzone->chunk != chunk) {
+			dzone = ERR_PTR(-EIO);
+			goto out;
+		}
+
+		/* Repair write pointer if the sequential dzone has error */
+		if (dmz_seq_write_err(dzone)) {
+			ret = dmz_handle_seq_write_err(zmd, dzone);
+			if (ret) {
+				dzone = ERR_PTR(-EIO);
+				goto out;
+			}
+			clear_bit(DMZ_SEQ_WRITE_ERR, &dzone->flags);
+		}
+	}
+
+	/*
+	 * If the zone is being reclaimed, the chunk mapping may change
+	 * to a different zone. So wait for reclaim and retry. Otherwise,
+	 * activate the zone (this will prevent reclaim from touching it).
+	 */
+	if (dmz_in_reclaim(dzone)) {
+		dmz_wait_for_reclaim(zmd, dzone);
+		goto again;
+	}
+	dmz_activate_zone(dzone);
+	dmz_lru_zone(zmd, dzone);
+out:
+	dmz_unlock_map(zmd);
+
+	return dzone;
+}
+
+/*
+ * Write and discard change the block validity of data zones and their buffer
+ * zones. Check here that valid blocks are still present. If all blocks are
+ * invalid, the zones can be unmapped on the fly without waiting for reclaim
+ * to do it.
+ */
+void dmz_put_chunk_mapping(struct dmz_metadata *zmd, struct dm_zone *dzone)
+{
+	struct dm_zone *bzone;
+
+	dmz_lock_map(zmd);
+
+	bzone = dzone->bzone;
+	if (bzone) {
+		if (dmz_weight(bzone))
+			dmz_lru_zone(zmd, bzone);
+		else {
+			/* Empty buffer zone: reclaim it */
+			dmz_unmap_zone(zmd, bzone);
+			dmz_free_zone(zmd, bzone);
+			bzone = NULL;
+		}
+	}
+
+	/* Deactivate the data zone */
+	dmz_deactivate_zone(dzone);
+	if (dmz_is_active(dzone) || bzone || dmz_weight(dzone))
+		dmz_lru_zone(zmd, dzone);
+	else {
+		/* Unbuffered inactive empty data zone: reclaim it */
+		dmz_unmap_zone(zmd, dzone);
+		dmz_free_zone(zmd, dzone);
+	}
+
+	dmz_unlock_map(zmd);
+}
+
+/*
+ * Allocate and map a random zone to buffer a chunk
+ * already mapped to a sequential zone.
+ */
+struct dm_zone *dmz_get_chunk_buffer(struct dmz_metadata *zmd,
+				     struct dm_zone *dzone)
+{
+	struct dm_zone *bzone;
+
+	dmz_lock_map(zmd);
+again:
+	bzone = dzone->bzone;
+	if (bzone)
+		goto out;
+
+	/* Alloate a random zone */
+	bzone = dmz_alloc_zone(zmd, DMZ_ALLOC_RND);
+	if (!bzone) {
+		dmz_wait_for_free_zones(zmd);
+		goto again;
+	}
+
+	/* Update the chunk mapping */
+	dmz_set_chunk_mapping(zmd, dzone->chunk, dmz_id(zmd, dzone),
+			      dmz_id(zmd, bzone));
+
+	set_bit(DMZ_BUF, &bzone->flags);
+	bzone->chunk = dzone->chunk;
+	bzone->bzone = dzone;
+	dzone->bzone = bzone;
+	list_add_tail(&bzone->link, &zmd->map_rnd_list);
+out:
+	dmz_unlock_map(zmd);
+
+	return bzone;
+}
+
+/*
+ * Get an unmapped (free) zone.
+ * This must be called with the mapping lock held.
+ */
+struct dm_zone *dmz_alloc_zone(struct dmz_metadata *zmd, unsigned long flags)
+{
+	struct list_head *list;
+	struct dm_zone *zone;
+
+	if (flags & DMZ_ALLOC_RND)
+		list = &zmd->unmap_rnd_list;
+	else
+		list = &zmd->unmap_seq_list;
+again:
+	if (list_empty(list)) {
+		/*
+		 * No free zone: if this is for reclaim, allow using the
+		 * reserved sequential zones.
+		 */
+		if (!(flags & DMZ_ALLOC_RECLAIM) ||
+		    list_empty(&zmd->reserved_seq_zones_list))
+			return NULL;
+
+		zone = list_first_entry(&zmd->reserved_seq_zones_list,
+					struct dm_zone, link);
+		list_del_init(&zone->link);
+		atomic_dec(&zmd->nr_reserved_seq_zones);
+		return zone;
+	}
+
+	zone = list_first_entry(list, struct dm_zone, link);
+	list_del_init(&zone->link);
+
+	if (dmz_is_rnd(zone))
+		atomic_dec(&zmd->unmap_nr_rnd);
+	else
+		atomic_dec(&zmd->unmap_nr_seq);
+
+	if (dmz_is_offline(zone)) {
+		dmz_dev_warn(zmd->dev, "Zone %u is offline", dmz_id(zmd, zone));
+		zone = NULL;
+		goto again;
+	}
+
+	return zone;
+}
+
+/*
+ * Free a zone.
+ * This must be called with the mapping lock held.
+ */
+void dmz_free_zone(struct dmz_metadata *zmd, struct dm_zone *zone)
+{
+	/* If this is a sequential zone, reset it */
+	if (dmz_is_seq(zone))
+		dmz_reset_zone(zmd, zone);
+
+	/* Return the zone to its type unmap list */
+	if (dmz_is_rnd(zone)) {
+		list_add_tail(&zone->link, &zmd->unmap_rnd_list);
+		atomic_inc(&zmd->unmap_nr_rnd);
+	} else if (atomic_read(&zmd->nr_reserved_seq_zones) <
+		   zmd->nr_reserved_seq) {
+		list_add_tail(&zone->link, &zmd->reserved_seq_zones_list);
+		atomic_inc(&zmd->nr_reserved_seq_zones);
+	} else {
+		list_add_tail(&zone->link, &zmd->unmap_seq_list);
+		atomic_inc(&zmd->unmap_nr_seq);
+	}
+
+	wake_up_all(&zmd->free_wq);
+}
+
+/*
+ * Map a chunk to a zone.
+ * This must be called with the mapping lock held.
+ */
+void dmz_map_zone(struct dmz_metadata *zmd, struct dm_zone *dzone,
+		  unsigned int chunk)
+{
+	/* Set the chunk mapping */
+	dmz_set_chunk_mapping(zmd, chunk, dmz_id(zmd, dzone),
+			      DMZ_MAP_UNMAPPED);
+	dzone->chunk = chunk;
+	if (dmz_is_rnd(dzone))
+		list_add_tail(&dzone->link, &zmd->map_rnd_list);
+	else
+		list_add_tail(&dzone->link, &zmd->map_seq_list);
+}
+
+/*
+ * Unmap a zone.
+ * This must be called with the mapping lock held.
+ */
+void dmz_unmap_zone(struct dmz_metadata *zmd, struct dm_zone *zone)
+{
+	unsigned int chunk = zone->chunk;
+	unsigned int dzone_id;
+
+	if (chunk == DMZ_MAP_UNMAPPED) {
+		/* Already unmapped */
+		return;
+	}
+
+	if (test_and_clear_bit(DMZ_BUF, &zone->flags)) {
+		/*
+		 * Unmapping the chunk buffer zone: clear only
+		 * the chunk buffer mapping
+		 */
+		dzone_id = dmz_id(zmd, zone->bzone);
+		zone->bzone->bzone = NULL;
+		zone->bzone = NULL;
+
+	} else {
+		/*
+		 * Unmapping the chunk data zone: the zone must
+		 * not be buffered.
+		 */
+		if (WARN_ON(zone->bzone)) {
+			zone->bzone->bzone = NULL;
+			zone->bzone = NULL;
+		}
+		dzone_id = DMZ_MAP_UNMAPPED;
+	}
+
+	dmz_set_chunk_mapping(zmd, chunk, dzone_id, DMZ_MAP_UNMAPPED);
+
+	zone->chunk = DMZ_MAP_UNMAPPED;
+	list_del_init(&zone->link);
+}
+
+/*
+ * Set @nr_bits bits in @bitmap starting from @bit.
+ * Return the number of bits changed from 0 to 1.
+ */
+static unsigned int dmz_set_bits(unsigned long *bitmap,
+				 unsigned int bit, unsigned int nr_bits)
+{
+	unsigned long *addr;
+	unsigned int end = bit + nr_bits;
+	unsigned int n = 0;
+
+	while (bit < end) {
+		if (((bit & (BITS_PER_LONG - 1)) == 0) &&
+		    ((end - bit) >= BITS_PER_LONG)) {
+			/* Try to set the whole word at once */
+			addr = bitmap + BIT_WORD(bit);
+			if (*addr == 0) {
+				*addr = ULONG_MAX;
+				n += BITS_PER_LONG;
+				bit += BITS_PER_LONG;
+				continue;
+			}
+		}
+
+		if (!test_and_set_bit(bit, bitmap))
+			n++;
+		bit++;
+	}
+
+	return n;
+}
+
+/*
+ * Get the bitmap block storing the bit for chunk_block in zone.
+ */
+static struct dmz_mblock *dmz_get_bitmap(struct dmz_metadata *zmd,
+					 struct dm_zone *zone,
+					 sector_t chunk_block)
+{
+	sector_t bitmap_block = 1 + zmd->nr_map_blocks +
+		(sector_t)(dmz_id(zmd, zone) * zmd->zone_nr_bitmap_blocks) +
+		(chunk_block >> DMZ_BLOCK_SHIFT_BITS);
+
+	return dmz_get_mblock(zmd, bitmap_block);
+}
+
+/*
+ * Copy the valid blocks bitmap of from_zone to the bitmap of to_zone.
+ */
+int dmz_copy_valid_blocks(struct dmz_metadata *zmd, struct dm_zone *from_zone,
+			  struct dm_zone *to_zone)
+{
+	struct dmz_mblock *from_mblk, *to_mblk;
+	sector_t chunk_block = 0;
+
+	/* Get the zones bitmap blocks */
+	while (chunk_block < zmd->dev->zone_nr_blocks) {
+		from_mblk = dmz_get_bitmap(zmd, from_zone, chunk_block);
+		if (IS_ERR(from_mblk))
+			return PTR_ERR(from_mblk);
+		to_mblk = dmz_get_bitmap(zmd, to_zone, chunk_block);
+		if (IS_ERR(to_mblk)) {
+			dmz_release_mblock(zmd, from_mblk);
+			return PTR_ERR(to_mblk);
+		}
+
+		memcpy(to_mblk->data, from_mblk->data, DMZ_BLOCK_SIZE);
+		dmz_dirty_mblock(zmd, to_mblk);
+
+		dmz_release_mblock(zmd, to_mblk);
+		dmz_release_mblock(zmd, from_mblk);
+
+		chunk_block += DMZ_BLOCK_SIZE_BITS;
+	}
+
+	to_zone->weight = from_zone->weight;
+
+	return 0;
+}
+
+/*
+ * Merge the valid blocks bitmap of from_zone into the bitmap of to_zone,
+ * starting from chunk_block.
+ */
+int dmz_merge_valid_blocks(struct dmz_metadata *zmd, struct dm_zone *from_zone,
+			   struct dm_zone *to_zone, sector_t chunk_block)
+{
+	unsigned int nr_blocks;
+	int ret;
+
+	/* Get the zones bitmap blocks */
+	while (chunk_block < zmd->dev->zone_nr_blocks) {
+		/* Get a valid region from the source zone */
+		ret = dmz_first_valid_block(zmd, from_zone, &chunk_block);
+		if (ret <= 0)
+			return ret;
+
+		nr_blocks = ret;
+		ret = dmz_validate_blocks(zmd, to_zone, chunk_block, nr_blocks);
+		if (ret)
+			return ret;
+
+		chunk_block += nr_blocks;
+	}
+
+	return 0;
+}
+
+/*
+ * Validate all the blocks in the range [block..block+nr_blocks-1].
+ */
+int dmz_validate_blocks(struct dmz_metadata *zmd, struct dm_zone *zone,
+			sector_t chunk_block, unsigned int nr_blocks)
+{
+	unsigned int count, bit, nr_bits;
+	unsigned int zone_nr_blocks = zmd->dev->zone_nr_blocks;
+	struct dmz_mblock *mblk;
+	unsigned int n = 0;
+
+	dmz_dev_debug(zmd->dev, "=> VALIDATE zone %u, block %llu, %u blocks",
+		      dmz_id(zmd, zone), (unsigned long long)chunk_block,
+		      nr_blocks);
+
+	WARN_ON(chunk_block + nr_blocks > zone_nr_blocks);
+
+	while (nr_blocks) {
+		/* Get bitmap block */
+		mblk = dmz_get_bitmap(zmd, zone, chunk_block);
+		if (IS_ERR(mblk))
+			return PTR_ERR(mblk);
+
+		/* Set bits */
+		bit = chunk_block & DMZ_BLOCK_MASK_BITS;
+		nr_bits = min(nr_blocks, DMZ_BLOCK_SIZE_BITS - bit);
+
+		count = dmz_set_bits((unsigned long *)mblk->data, bit, nr_bits);
+		if (count) {
+			dmz_dirty_mblock(zmd, mblk);
+			n += count;
+		}
+		dmz_release_mblock(zmd, mblk);
+
+		nr_blocks -= nr_bits;
+		chunk_block += nr_bits;
+	}
+
+	if (likely(zone->weight + n <= zone_nr_blocks))
+		zone->weight += n;
+	else {
+		dmz_dev_warn(zmd->dev, "Zone %u: weight %u should be <= %u",
+			     dmz_id(zmd, zone), zone->weight,
+			     zone_nr_blocks - n);
+		zone->weight = zone_nr_blocks;
+	}
+
+	return 0;
+}
+
+/*
+ * Clear nr_bits bits in bitmap starting from bit.
+ * Return the number of bits cleared.
+ */
+static int dmz_clear_bits(unsigned long *bitmap, int bit, int nr_bits)
+{
+	unsigned long *addr;
+	int end = bit + nr_bits;
+	int n = 0;
+
+	while (bit < end) {
+		if (((bit & (BITS_PER_LONG - 1)) == 0) &&
+		    ((end - bit) >= BITS_PER_LONG)) {
+			/* Try to clear whole word at once */
+			addr = bitmap + BIT_WORD(bit);
+			if (*addr == ULONG_MAX) {
+				*addr = 0;
+				n += BITS_PER_LONG;
+				bit += BITS_PER_LONG;
+				continue;
+			}
+		}
+
+		if (test_and_clear_bit(bit, bitmap))
+			n++;
+		bit++;
+	}
+
+	return n;
+}
+
+/*
+ * Invalidate all the blocks in the range [block..block+nr_blocks-1].
+ */
+int dmz_invalidate_blocks(struct dmz_metadata *zmd, struct dm_zone *zone,
+			  sector_t chunk_block, unsigned int nr_blocks)
+{
+	unsigned int count, bit, nr_bits;
+	struct dmz_mblock *mblk;
+	unsigned int n = 0;
+
+	dmz_dev_debug(zmd->dev, "=> INVALIDATE zone %u, block %llu, %u blocks",
+		      dmz_id(zmd, zone), (u64)chunk_block, nr_blocks);
+
+	WARN_ON(chunk_block + nr_blocks > zmd->dev->zone_nr_blocks);
+
+	while (nr_blocks) {
+		/* Get bitmap block */
+		mblk = dmz_get_bitmap(zmd, zone, chunk_block);
+		if (IS_ERR(mblk))
+			return PTR_ERR(mblk);
+
+		/* Clear bits */
+		bit = chunk_block & DMZ_BLOCK_MASK_BITS;
+		nr_bits = min(nr_blocks, DMZ_BLOCK_SIZE_BITS - bit);
+
+		count = dmz_clear_bits((unsigned long *)mblk->data,
+				       bit, nr_bits);
+		if (count) {
+			dmz_dirty_mblock(zmd, mblk);
+			n += count;
+		}
+		dmz_release_mblock(zmd, mblk);
+
+		nr_blocks -= nr_bits;
+		chunk_block += nr_bits;
+	}
+
+	if (zone->weight >= n)
+		zone->weight -= n;
+	else {
+		dmz_dev_warn(zmd->dev, "Zone %u: weight %u should be >= %u",
+			     dmz_id(zmd, zone), zone->weight, n);
+		zone->weight = 0;
+	}
+
+	return 0;
+}
+
+/*
+ * Get a block bit value.
+ */
+static int dmz_test_block(struct dmz_metadata *zmd, struct dm_zone *zone,
+			  sector_t chunk_block)
+{
+	struct dmz_mblock *mblk;
+	int ret;
+
+	WARN_ON(chunk_block >= zmd->dev->zone_nr_blocks);
+
+	/* Get bitmap block */
+	mblk = dmz_get_bitmap(zmd, zone, chunk_block);
+	if (IS_ERR(mblk))
+		return PTR_ERR(mblk);
+
+	/* Get offset */
+	ret = test_bit(chunk_block & DMZ_BLOCK_MASK_BITS,
+		       (unsigned long *) mblk->data) != 0;
+
+	dmz_release_mblock(zmd, mblk);
+
+	return ret;
+}
+
+/*
+ * Return the number of blocks from chunk_block to the first block with a bit
+ * value specified by set. Search at most nr_blocks blocks from chunk_block.
+ */
+static int dmz_to_next_set_block(struct dmz_metadata *zmd, struct dm_zone *zone,
+				 sector_t chunk_block, unsigned int nr_blocks,
+				 int set)
+{
+	struct dmz_mblock *mblk;
+	unsigned int bit, set_bit, nr_bits;
+	unsigned long *bitmap;
+	int n = 0;
+
+	WARN_ON(chunk_block + nr_blocks > zmd->dev->zone_nr_blocks);
+
+	while (nr_blocks) {
+		/* Get bitmap block */
+		mblk = dmz_get_bitmap(zmd, zone, chunk_block);
+		if (IS_ERR(mblk))
+			return PTR_ERR(mblk);
+
+		/* Get offset */
+		bitmap = (unsigned long *) mblk->data;
+		bit = chunk_block & DMZ_BLOCK_MASK_BITS;
+		nr_bits = min(nr_blocks, DMZ_BLOCK_SIZE_BITS - bit);
+		if (set)
+			set_bit = find_next_bit(bitmap, DMZ_BLOCK_SIZE_BITS, bit);
+		else
+			set_bit = find_next_zero_bit(bitmap, DMZ_BLOCK_SIZE_BITS, bit);
+		dmz_release_mblock(zmd, mblk);
+
+		n += set_bit - bit;
+		if (set_bit < DMZ_BLOCK_SIZE_BITS)
+			break;
+
+		nr_blocks -= nr_bits;
+		chunk_block += nr_bits;
+	}
+
+	return n;
+}
+
+/*
+ * Test if chunk_block is valid. If it is, the number of consecutive
+ * valid blocks from chunk_block will be returned.
+ */
+int dmz_block_valid(struct dmz_metadata *zmd, struct dm_zone *zone,
+		    sector_t chunk_block)
+{
+	int valid;
+
+	valid = dmz_test_block(zmd, zone, chunk_block);
+	if (valid <= 0)
+		return valid;
+
+	/* The block is valid: get the number of valid blocks from block */
+	return dmz_to_next_set_block(zmd, zone, chunk_block,
+				     zmd->dev->zone_nr_blocks - chunk_block, 0);
+}
+
+/*
+ * Find the first valid block from @chunk_block in @zone.
+ * If such a block is found, its number is returned using
+ * @chunk_block and the total number of valid blocks from @chunk_block
+ * is returned.
+ */
+int dmz_first_valid_block(struct dmz_metadata *zmd, struct dm_zone *zone,
+			  sector_t *chunk_block)
+{
+	sector_t start_block = *chunk_block;
+	int ret;
+
+	ret = dmz_to_next_set_block(zmd, zone, start_block,
+				    zmd->dev->zone_nr_blocks - start_block, 1);
+	if (ret < 0)
+		return ret;
+
+	start_block += ret;
+	*chunk_block = start_block;
+
+	return dmz_to_next_set_block(zmd, zone, start_block,
+				     zmd->dev->zone_nr_blocks - start_block, 0);
+}
+
+/*
+ * Count the number of bits set starting from bit up to bit + nr_bits - 1.
+ */
+static int dmz_count_bits(void *bitmap, int bit, int nr_bits)
+{
+	unsigned long *addr;
+	int end = bit + nr_bits;
+	int n = 0;
+
+	while (bit < end) {
+		if (((bit & (BITS_PER_LONG - 1)) == 0) &&
+		    ((end - bit) >= BITS_PER_LONG)) {
+			addr = (unsigned long *)bitmap + BIT_WORD(bit);
+			if (*addr == ULONG_MAX) {
+				n += BITS_PER_LONG;
+				bit += BITS_PER_LONG;
+				continue;
+			}
+		}
+
+		if (test_bit(bit, bitmap))
+			n++;
+		bit++;
+	}
+
+	return n;
+}
+
+/*
+ * Get a zone weight.
+ */
+static void dmz_get_zone_weight(struct dmz_metadata *zmd, struct dm_zone *zone)
+{
+	struct dmz_mblock *mblk;
+	sector_t chunk_block = 0;
+	unsigned int bit, nr_bits;
+	unsigned int nr_blocks = zmd->dev->zone_nr_blocks;
+	void *bitmap;
+	int n = 0;
+
+	while (nr_blocks) {
+		/* Get bitmap block */
+		mblk = dmz_get_bitmap(zmd, zone, chunk_block);
+		if (IS_ERR(mblk)) {
+			n = 0;
+			break;
+		}
+
+		/* Count bits in this block */
+		bitmap = mblk->data;
+		bit = chunk_block & DMZ_BLOCK_MASK_BITS;
+		nr_bits = min(nr_blocks, DMZ_BLOCK_SIZE_BITS - bit);
+		n += dmz_count_bits(bitmap, bit, nr_bits);
+
+		dmz_release_mblock(zmd, mblk);
+
+		nr_blocks -= nr_bits;
+		chunk_block += nr_bits;
+	}
+
+	zone->weight = n;
+}
+
+/*
+ * Cleanup the zoned metadata resources.
+ */
+static void dmz_cleanup_metadata(struct dmz_metadata *zmd)
+{
+	struct rb_root *root;
+	struct dmz_mblock *mblk, *next;
+	int i;
+
+	/* Release zone mapping resources */
+	if (zmd->map_mblk) {
+		for (i = 0; i < zmd->nr_map_blocks; i++)
+			dmz_release_mblock(zmd, zmd->map_mblk[i]);
+		kfree(zmd->map_mblk);
+		zmd->map_mblk = NULL;
+	}
+
+	/* Release super blocks */
+	for (i = 0; i < 2; i++) {
+		if (zmd->sb[i].mblk) {
+			dmz_free_mblock(zmd, zmd->sb[i].mblk);
+			zmd->sb[i].mblk = NULL;
+		}
+	}
+
+	/* Free cached blocks */
+	while (!list_empty(&zmd->mblk_dirty_list)) {
+		mblk = list_first_entry(&zmd->mblk_dirty_list,
+					struct dmz_mblock, link);
+		dmz_dev_warn(zmd->dev, "mblock %llu still in dirty list (ref %u)",
+			     (u64)mblk->no, atomic_read(&mblk->ref));
+		list_del_init(&mblk->link);
+		rb_erase(&mblk->node, &zmd->mblk_rbtree);
+		dmz_free_mblock(zmd, mblk);
+	}
+
+	while (!list_empty(&zmd->mblk_lru_list)) {
+		mblk = list_first_entry(&zmd->mblk_lru_list,
+					struct dmz_mblock, link);
+		list_del_init(&mblk->link);
+		rb_erase(&mblk->node, &zmd->mblk_rbtree);
+		dmz_free_mblock(zmd, mblk);
+	}
+
+	/* Sanity checks: the mblock rbtree should now be empty */
+	root = &zmd->mblk_rbtree;
+	rbtree_postorder_for_each_entry_safe(mblk, next, root, node) {
+		dmz_dev_warn(zmd->dev, "mblock %llu ref %u still in rbtree",
+			     (u64)mblk->no, atomic_read(&mblk->ref));
+		atomic_set(&mblk->ref, 0);
+		dmz_free_mblock(zmd, mblk);
+	}
+
+	/* Free the zone descriptors */
+	dmz_drop_zones(zmd);
+}
+
+/*
+ * Initialize the zoned metadata.
+ */
+int dmz_ctr_metadata(struct dmz_dev *dev, struct dmz_metadata **metadata)
+{
+	struct dmz_metadata *zmd;
+	unsigned int i, zid;
+	struct dm_zone *zone;
+	int ret;
+
+	zmd = kzalloc(sizeof(struct dmz_metadata), GFP_KERNEL);
+	if (!zmd)
+		return -ENOMEM;
+
+	zmd->dev = dev;
+	zmd->mblk_rbtree = RB_ROOT;
+	init_rwsem(&zmd->mblk_sem);
+	mutex_init(&zmd->mblk_flush_lock);
+	spin_lock_init(&zmd->mblk_lock);
+	INIT_LIST_HEAD(&zmd->mblk_lru_list);
+	INIT_LIST_HEAD(&zmd->mblk_dirty_list);
+
+	mutex_init(&zmd->map_lock);
+	atomic_set(&zmd->unmap_nr_rnd, 0);
+	INIT_LIST_HEAD(&zmd->unmap_rnd_list);
+	INIT_LIST_HEAD(&zmd->map_rnd_list);
+
+	atomic_set(&zmd->unmap_nr_seq, 0);
+	INIT_LIST_HEAD(&zmd->unmap_seq_list);
+	INIT_LIST_HEAD(&zmd->map_seq_list);
+
+	atomic_set(&zmd->nr_reserved_seq_zones, 0);
+	INIT_LIST_HEAD(&zmd->reserved_seq_zones_list);
+
+	init_waitqueue_head(&zmd->free_wq);
+
+	/* Initialize zone descriptors */
+	ret = dmz_init_zones(zmd);
+	if (ret)
+		goto err;
+
+	/* Get super block */
+	ret = dmz_load_sb(zmd);
+	if (ret)
+		goto err;
+
+	/* Set metadata zones starting from sb_zone */
+	zid = dmz_id(zmd, zmd->sb_zone);
+	for (i = 0; i < zmd->nr_meta_zones << 1; i++) {
+		zone = dmz_get(zmd, zid + i);
+		if (!dmz_is_rnd(zone))
+			goto err;
+		set_bit(DMZ_META, &zone->flags);
+	}
+
+	/* Load mapping table */
+	ret = dmz_load_mapping(zmd);
+	if (ret)
+		goto err;
+
+	/*
+	 * Cache size boundaries: allow at least 2 super blocks, the chunk map
+	 * blocks and enough blocks to be able to cache the bitmap blocks of
+	 * up to 16 zones when idle (min_nr_mblks). Otherwise, if busy, allow
+	 * the cache to add 512 more metadata blocks.
+	 */
+	zmd->min_nr_mblks = 2 + zmd->nr_map_blocks + zmd->zone_nr_bitmap_blocks * 16;
+	zmd->max_nr_mblks = zmd->min_nr_mblks + 512;
+	zmd->mblk_shrinker.count_objects = dmz_mblock_shrinker_count;
+	zmd->mblk_shrinker.scan_objects = dmz_mblock_shrinker_scan;
+	zmd->mblk_shrinker.seeks = DEFAULT_SEEKS;
+
+	/* Metadata cache shrinker */
+	ret = register_shrinker(&zmd->mblk_shrinker);
+	if (ret) {
+		dmz_dev_err(dev, "Register metadata cache shrinker failed");
+		goto err;
+	}
+
+	dmz_dev_info(dev, "Host-%s zoned block device",
+		     bdev_zoned_model(dev->bdev) == BLK_ZONED_HA ?
+		     "aware" : "managed");
+	dmz_dev_info(dev, "  %llu 512-byte logical sectors",
+		     (u64)dev->capacity);
+	dmz_dev_info(dev, "  %u zones of %llu 512-byte logical sectors",
+		     dev->nr_zones, (u64)dev->zone_nr_sectors);
+	dmz_dev_info(dev, "  %u metadata zones",
+		     zmd->nr_meta_zones * 2);
+	dmz_dev_info(dev, "  %u data zones for %u chunks",
+		     zmd->nr_data_zones, zmd->nr_chunks);
+	dmz_dev_info(dev, "    %u random zones (%u unmapped)",
+		     zmd->nr_rnd, atomic_read(&zmd->unmap_nr_rnd));
+	dmz_dev_info(dev, "    %u sequential zones (%u unmapped)",
+		     zmd->nr_seq, atomic_read(&zmd->unmap_nr_seq));
+	dmz_dev_info(dev, "  %u reserved sequential data zones",
+		     zmd->nr_reserved_seq);
+
+	dmz_dev_debug(dev, "Format:");
+	dmz_dev_debug(dev, "%u metadata blocks per set (%u max cache)",
+		      zmd->nr_meta_blocks, zmd->max_nr_mblks);
+	dmz_dev_debug(dev, "  %u data zone mapping blocks",
+		      zmd->nr_map_blocks);
+	dmz_dev_debug(dev, "  %u bitmap blocks",
+		      zmd->nr_bitmap_blocks);
+
+	*metadata = zmd;
+
+	return 0;
+err:
+	dmz_cleanup_metadata(zmd);
+	kfree(zmd);
+	*metadata = NULL;
+
+	return ret;
+}
+
+/*
+ * Cleanup the zoned metadata resources.
+ */
+void dmz_dtr_metadata(struct dmz_metadata *zmd)
+{
+	unregister_shrinker(&zmd->mblk_shrinker);
+	dmz_cleanup_metadata(zmd);
+	kfree(zmd);
+}
+
+/*
+ * Check zone information on resume.
+ */
+int dmz_resume_metadata(struct dmz_metadata *zmd)
+{
+	struct dmz_dev *dev = zmd->dev;
+	struct dm_zone *zone;
+	sector_t wp_block;
+	unsigned int i;
+	int ret;
+
+	/* Check zones */
+	for (i = 0; i < dev->nr_zones; i++) {
+		zone = dmz_get(zmd, i);
+		if (!zone) {
+			dmz_dev_err(dev, "Unable to get zone %u", i);
+			return -EIO;
+		}
+
+		wp_block = zone->wp_block;
+
+		ret = dmz_update_zone(zmd, zone);
+		if (ret) {
+			dmz_dev_err(dev, "Broken zone %u", i);
+			return ret;
+		}
+
+		if (dmz_is_offline(zone)) {
+			dmz_dev_warn(dev, "Zone %u is offline", i);
+			continue;
+		}
+
+		/* Check write pointer */
+		if (!dmz_is_seq(zone))
+			zone->wp_block = 0;
+		else if (zone->wp_block != wp_block) {
+			dmz_dev_err(dev, "Zone %u: Invalid wp (%llu / %llu)",
+				    i, (u64)zone->wp_block, (u64)wp_block);
+			zone->wp_block = wp_block;
+			dmz_invalidate_blocks(zmd, zone, zone->wp_block,
+					      dev->zone_nr_blocks - zone->wp_block);
+		}
+	}
+
+	return 0;
+}
diff --git a/drivers/md/dm-zoned-reclaim.c b/drivers/md/dm-zoned-reclaim.c
new file mode 100644
index 000000000000..05c0a126f5c8
--- /dev/null
+++ b/drivers/md/dm-zoned-reclaim.c
@@ -0,0 +1,570 @@
+/*
+ * Copyright (C) 2017 Western Digital Corporation or its affiliates.
+ *
+ * This file is released under the GPL.
+ */
+
+#include "dm-zoned.h"
+
+#include <linux/module.h>
+
+#define	DM_MSG_PREFIX		"zoned reclaim"
+
+struct dmz_reclaim {
+	struct dmz_metadata     *metadata;
+	struct dmz_dev		*dev;
+
+	struct delayed_work	work;
+	struct workqueue_struct *wq;
+
+	struct dm_kcopyd_client	*kc;
+	struct dm_kcopyd_throttle kc_throttle;
+	int			kc_err;
+
+	unsigned long		flags;
+
+	/* Last target access time */
+	unsigned long		atime;
+};
+
+/*
+ * Reclaim state flags.
+ */
+enum {
+	DMZ_RECLAIM_KCOPY,
+};
+
+/*
+ * Number of seconds of target BIO inactivity to consider the target idle.
+ */
+#define DMZ_IDLE_PERIOD		(10UL * HZ)
+
+/*
+ * Percentage of unmapped (free) random zones below which reclaim starts
+ * even if the target is busy.
+ */
+#define DMZ_RECLAIM_LOW_UNMAP_RND	30
+
+/*
+ * Percentage of unmapped (free) random zones above which reclaim will
+ * stop if the target is busy.
+ */
+#define DMZ_RECLAIM_HIGH_UNMAP_RND	50
+
+/*
+ * Align a sequential zone write pointer to chunk_block.
+ */
+static int dmz_reclaim_align_wp(struct dmz_reclaim *zrc, struct dm_zone *zone,
+				sector_t block)
+{
+	struct dmz_metadata *zmd = zrc->metadata;
+	sector_t wp_block = zone->wp_block;
+	unsigned int nr_blocks;
+	int ret;
+
+	if (wp_block == block)
+		return 0;
+
+	if (wp_block > block)
+		return -EIO;
+
+	/*
+	 * Zeroout the space between the write
+	 * pointer and the requested position.
+	 */
+	nr_blocks = block - wp_block;
+	ret = blkdev_issue_zeroout(zrc->dev->bdev,
+				   dmz_start_sect(zmd, zone) + dmz_blk2sect(wp_block),
+				   dmz_blk2sect(nr_blocks), GFP_NOFS, false);
+	if (ret) {
+		dmz_dev_err(zrc->dev,
+			    "Align zone %u wp %llu to %llu (wp+%u) blocks failed %d",
+			    dmz_id(zmd, zone), (unsigned long long)wp_block,
+			    (unsigned long long)block, nr_blocks, ret);
+		return ret;
+	}
+
+	zone->wp_block = block;
+
+	return 0;
+}
+
+/*
+ * dm_kcopyd_copy end notification.
+ */
+static void dmz_reclaim_kcopy_end(int read_err, unsigned long write_err,
+				  void *context)
+{
+	struct dmz_reclaim *zrc = context;
+
+	if (read_err || write_err)
+		zrc->kc_err = -EIO;
+	else
+		zrc->kc_err = 0;
+
+	clear_bit_unlock(DMZ_RECLAIM_KCOPY, &zrc->flags);
+	smp_mb__after_atomic();
+	wake_up_bit(&zrc->flags, DMZ_RECLAIM_KCOPY);
+}
+
+/*
+ * Copy valid blocks of src_zone into dst_zone.
+ */
+static int dmz_reclaim_copy(struct dmz_reclaim *zrc,
+			    struct dm_zone *src_zone, struct dm_zone *dst_zone)
+{
+	struct dmz_metadata *zmd = zrc->metadata;
+	struct dmz_dev *dev = zrc->dev;
+	struct dm_io_region src, dst;
+	sector_t block = 0, end_block;
+	sector_t nr_blocks;
+	sector_t src_zone_block;
+	sector_t dst_zone_block;
+	unsigned long flags = 0;
+	int ret;
+
+	if (dmz_is_seq(src_zone))
+		end_block = src_zone->wp_block;
+	else
+		end_block = dev->zone_nr_blocks;
+	src_zone_block = dmz_start_block(zmd, src_zone);
+	dst_zone_block = dmz_start_block(zmd, dst_zone);
+
+	if (dmz_is_seq(dst_zone))
+		set_bit(DM_KCOPYD_WRITE_SEQ, &flags);
+
+	while (block < end_block) {
+		/* Get a valid region from the source zone */
+		ret = dmz_first_valid_block(zmd, src_zone, &block);
+		if (ret <= 0)
+			return ret;
+		nr_blocks = ret;
+
+		/*
+		 * If we are writing in a sequential zone, we must make sure
+		 * that writes are sequential. So Zeroout any eventual hole
+		 * between writes.
+		 */
+		if (dmz_is_seq(dst_zone)) {
+			ret = dmz_reclaim_align_wp(zrc, dst_zone, block);
+			if (ret)
+				return ret;
+		}
+
+		src.bdev = dev->bdev;
+		src.sector = dmz_blk2sect(src_zone_block + block);
+		src.count = dmz_blk2sect(nr_blocks);
+
+		dst.bdev = dev->bdev;
+		dst.sector = dmz_blk2sect(dst_zone_block + block);
+		dst.count = src.count;
+
+		/* Copy the valid region */
+		set_bit(DMZ_RECLAIM_KCOPY, &zrc->flags);
+		ret = dm_kcopyd_copy(zrc->kc, &src, 1, &dst, flags,
+				     dmz_reclaim_kcopy_end, zrc);
+		if (ret)
+			return ret;
+
+		/* Wait for copy to complete */
+		wait_on_bit_io(&zrc->flags, DMZ_RECLAIM_KCOPY,
+			       TASK_UNINTERRUPTIBLE);
+		if (zrc->kc_err)
+			return zrc->kc_err;
+
+		block += nr_blocks;
+		if (dmz_is_seq(dst_zone))
+			dst_zone->wp_block = block;
+	}
+
+	return 0;
+}
+
+/*
+ * Move valid blocks of dzone buffer zone into dzone (after its write pointer)
+ * and free the buffer zone.
+ */
+static int dmz_reclaim_buf(struct dmz_reclaim *zrc, struct dm_zone *dzone)
+{
+	struct dm_zone *bzone = dzone->bzone;
+	sector_t chunk_block = dzone->wp_block;
+	struct dmz_metadata *zmd = zrc->metadata;
+	int ret;
+
+	dmz_dev_debug(zrc->dev,
+		      "Chunk %u, move buf zone %u (weight %u) to data zone %u (weight %u)",
+		      dzone->chunk, dmz_id(zmd, bzone), dmz_weight(bzone),
+		      dmz_id(zmd, dzone), dmz_weight(dzone));
+
+	/* Flush data zone into the buffer zone */
+	ret = dmz_reclaim_copy(zrc, bzone, dzone);
+	if (ret < 0)
+		return ret;
+
+	dmz_lock_flush(zmd);
+
+	/* Validate copied blocks */
+	ret = dmz_merge_valid_blocks(zmd, bzone, dzone, chunk_block);
+	if (ret == 0) {
+		/* Free the buffer zone */
+		dmz_invalidate_blocks(zmd, bzone, 0, zrc->dev->zone_nr_blocks);
+		dmz_lock_map(zmd);
+		dmz_unmap_zone(zmd, bzone);
+		dmz_unlock_zone_reclaim(dzone);
+		dmz_free_zone(zmd, bzone);
+		dmz_unlock_map(zmd);
+	}
+
+	dmz_unlock_flush(zmd);
+
+	return 0;
+}
+
+/*
+ * Merge valid blocks of dzone into its buffer zone and free dzone.
+ */
+static int dmz_reclaim_seq_data(struct dmz_reclaim *zrc, struct dm_zone *dzone)
+{
+	unsigned int chunk = dzone->chunk;
+	struct dm_zone *bzone = dzone->bzone;
+	struct dmz_metadata *zmd = zrc->metadata;
+	int ret = 0;
+
+	dmz_dev_debug(zrc->dev,
+		      "Chunk %u, move data zone %u (weight %u) to buf zone %u (weight %u)",
+		      chunk, dmz_id(zmd, dzone), dmz_weight(dzone),
+		      dmz_id(zmd, bzone), dmz_weight(bzone));
+
+	/* Flush data zone into the buffer zone */
+	ret = dmz_reclaim_copy(zrc, dzone, bzone);
+	if (ret < 0)
+		return ret;
+
+	dmz_lock_flush(zmd);
+
+	/* Validate copied blocks */
+	ret = dmz_merge_valid_blocks(zmd, dzone, bzone, 0);
+	if (ret == 0) {
+		/*
+		 * Free the data zone and remap the chunk to
+		 * the buffer zone.
+		 */
+		dmz_invalidate_blocks(zmd, dzone, 0, zrc->dev->zone_nr_blocks);
+		dmz_lock_map(zmd);
+		dmz_unmap_zone(zmd, bzone);
+		dmz_unmap_zone(zmd, dzone);
+		dmz_unlock_zone_reclaim(dzone);
+		dmz_free_zone(zmd, dzone);
+		dmz_map_zone(zmd, bzone, chunk);
+		dmz_unlock_map(zmd);
+	}
+
+	dmz_unlock_flush(zmd);
+
+	return 0;
+}
+
+/*
+ * Move valid blocks of the random data zone dzone into a free sequential zone.
+ * Once blocks are moved, remap the zone chunk to the sequential zone.
+ */
+static int dmz_reclaim_rnd_data(struct dmz_reclaim *zrc, struct dm_zone *dzone)
+{
+	unsigned int chunk = dzone->chunk;
+	struct dm_zone *szone = NULL;
+	struct dmz_metadata *zmd = zrc->metadata;
+	int ret;
+
+	/* Get a free sequential zone */
+	dmz_lock_map(zmd);
+	szone = dmz_alloc_zone(zmd, DMZ_ALLOC_RECLAIM);
+	dmz_unlock_map(zmd);
+	if (!szone)
+		return -ENOSPC;
+
+	dmz_dev_debug(zrc->dev,
+		      "Chunk %u, move rnd zone %u (weight %u) to seq zone %u",
+		      chunk, dmz_id(zmd, dzone), dmz_weight(dzone),
+		      dmz_id(zmd, szone));
+
+	/* Flush the random data zone into the sequential zone */
+	ret = dmz_reclaim_copy(zrc, dzone, szone);
+
+	dmz_lock_flush(zmd);
+
+	if (ret == 0) {
+		/* Validate copied blocks */
+		ret = dmz_copy_valid_blocks(zmd, dzone, szone);
+	}
+	if (ret) {
+		/* Free the sequential zone */
+		dmz_lock_map(zmd);
+		dmz_free_zone(zmd, szone);
+		dmz_unlock_map(zmd);
+	} else {
+		/* Free the data zone and remap the chunk */
+		dmz_invalidate_blocks(zmd, dzone, 0, zrc->dev->zone_nr_blocks);
+		dmz_lock_map(zmd);
+		dmz_unmap_zone(zmd, dzone);
+		dmz_unlock_zone_reclaim(dzone);
+		dmz_free_zone(zmd, dzone);
+		dmz_map_zone(zmd, szone, chunk);
+		dmz_unlock_map(zmd);
+	}
+
+	dmz_unlock_flush(zmd);
+
+	return 0;
+}
+
+/*
+ * Reclaim an empty zone.
+ */
+static void dmz_reclaim_empty(struct dmz_reclaim *zrc, struct dm_zone *dzone)
+{
+	struct dmz_metadata *zmd = zrc->metadata;
+
+	dmz_lock_flush(zmd);
+	dmz_lock_map(zmd);
+	dmz_unmap_zone(zmd, dzone);
+	dmz_unlock_zone_reclaim(dzone);
+	dmz_free_zone(zmd, dzone);
+	dmz_unlock_map(zmd);
+	dmz_unlock_flush(zmd);
+}
+
+/*
+ * Find a candidate zone for reclaim and process it.
+ */
+static void dmz_reclaim(struct dmz_reclaim *zrc)
+{
+	struct dmz_metadata *zmd = zrc->metadata;
+	struct dm_zone *dzone;
+	struct dm_zone *rzone;
+	unsigned long start;
+	int ret;
+
+	/* Get a data zone */
+	dzone = dmz_get_zone_for_reclaim(zmd);
+	if (!dzone)
+		return;
+
+	start = jiffies;
+
+	if (dmz_is_rnd(dzone)) {
+		if (!dmz_weight(dzone)) {
+			/* Empty zone */
+			dmz_reclaim_empty(zrc, dzone);
+			ret = 0;
+		} else {
+			/*
+			 * Reclaim the random data zone by moving its
+			 * valid data blocks to a free sequential zone.
+			 */
+			ret = dmz_reclaim_rnd_data(zrc, dzone);
+		}
+		rzone = dzone;
+
+	} else {
+		struct dm_zone *bzone = dzone->bzone;
+		sector_t chunk_block = 0;
+
+		ret = dmz_first_valid_block(zmd, bzone, &chunk_block);
+		if (ret < 0)
+			goto out;
+
+		if (ret == 0 || chunk_block >= dzone->wp_block) {
+			/*
+			 * The buffer zone is empty or its valid blocks are
+			 * after the data zone write pointer.
+			 */
+			ret = dmz_reclaim_buf(zrc, dzone);
+			rzone = bzone;
+		} else {
+			/*
+			 * Reclaim the data zone by merging it into the
+			 * buffer zone so that the buffer zone itself can
+			 * be later reclaimed.
+			 */
+			ret = dmz_reclaim_seq_data(zrc, dzone);
+			rzone = dzone;
+		}
+	}
+out:
+	if (ret) {
+		dmz_unlock_zone_reclaim(dzone);
+		return;
+	}
+
+	(void) dmz_flush_metadata(zrc->metadata);
+
+	dmz_dev_debug(zrc->dev, "Reclaimed zone %u in %u ms",
+		      dmz_id(zmd, rzone), jiffies_to_msecs(jiffies - start));
+}
+
+/*
+ * Test if the target device is idle.
+ */
+static inline int dmz_target_idle(struct dmz_reclaim *zrc)
+{
+	return time_is_before_jiffies(zrc->atime + DMZ_IDLE_PERIOD);
+}
+
+/*
+ * Test if reclaim is necessary.
+ */
+static bool dmz_should_reclaim(struct dmz_reclaim *zrc)
+{
+	struct dmz_metadata *zmd = zrc->metadata;
+	unsigned int nr_rnd = dmz_nr_rnd_zones(zmd);
+	unsigned int nr_unmap_rnd = dmz_nr_unmap_rnd_zones(zmd);
+	unsigned int p_unmap_rnd = nr_unmap_rnd * 100 / nr_rnd;
+
+	/* Reclaim when idle */
+	if (dmz_target_idle(zrc) && nr_unmap_rnd < nr_rnd)
+		return true;
+
+	/* If there are still plenty of random zones, do not reclaim */
+	if (p_unmap_rnd >= DMZ_RECLAIM_HIGH_UNMAP_RND)
+		return false;
+
+	/*
+	 * If the percentage of unmappped random zones is low,
+	 * reclaim even if the target is busy.
+	 */
+	return p_unmap_rnd <= DMZ_RECLAIM_LOW_UNMAP_RND;
+}
+
+/*
+ * Reclaim work function.
+ */
+static void dmz_reclaim_work(struct work_struct *work)
+{
+	struct dmz_reclaim *zrc = container_of(work, struct dmz_reclaim, work.work);
+	struct dmz_metadata *zmd = zrc->metadata;
+	unsigned int nr_rnd, nr_unmap_rnd;
+	unsigned int p_unmap_rnd;
+
+	if (!dmz_should_reclaim(zrc)) {
+		mod_delayed_work(zrc->wq, &zrc->work, DMZ_IDLE_PERIOD);
+		return;
+	}
+
+	/*
+	 * We need to start reclaiming random zones: set up zone copy
+	 * throttling to either go fast if we are very low on random zones
+	 * and slower if there are still some free random zones to avoid
+	 * as much as possible to negatively impact the user workload.
+	 */
+	nr_rnd = dmz_nr_rnd_zones(zmd);
+	nr_unmap_rnd = dmz_nr_unmap_rnd_zones(zmd);
+	p_unmap_rnd = nr_unmap_rnd * 100 / nr_rnd;
+	if (dmz_target_idle(zrc) || p_unmap_rnd < DMZ_RECLAIM_LOW_UNMAP_RND / 2) {
+		/* Idle or very low percentage: go fast */
+		zrc->kc_throttle.throttle = 100;
+	} else {
+		/* Busy but we still have some random zone: throttle */
+		zrc->kc_throttle.throttle = min(75U, 100U - p_unmap_rnd / 2);
+	}
+
+	dmz_dev_debug(zrc->dev,
+		      "Reclaim (%u): %s, %u%% free rnd zones (%u/%u)",
+		      zrc->kc_throttle.throttle,
+		      (dmz_target_idle(zrc) ? "Idle" : "Busy"),
+		      p_unmap_rnd, nr_unmap_rnd, nr_rnd);
+
+	dmz_reclaim(zrc);
+
+	dmz_schedule_reclaim(zrc);
+}
+
+/*
+ * Initialize reclaim.
+ */
+int dmz_ctr_reclaim(struct dmz_dev *dev, struct dmz_metadata *zmd,
+		    struct dmz_reclaim **reclaim)
+{
+	struct dmz_reclaim *zrc;
+	int ret;
+
+	zrc = kzalloc(sizeof(struct dmz_reclaim), GFP_KERNEL);
+	if (!zrc)
+		return -ENOMEM;
+
+	zrc->dev = dev;
+	zrc->metadata = zmd;
+	zrc->atime = jiffies;
+
+	/* Reclaim kcopyd client */
+	zrc->kc = dm_kcopyd_client_create(&zrc->kc_throttle);
+	if (IS_ERR(zrc->kc)) {
+		ret = PTR_ERR(zrc->kc);
+		zrc->kc = NULL;
+		goto err;
+	}
+
+	/* Reclaim work */
+	INIT_DELAYED_WORK(&zrc->work, dmz_reclaim_work);
+	zrc->wq = alloc_ordered_workqueue("dmz_rwq_%s", WQ_MEM_RECLAIM,
+					  dev->name);
+	if (!zrc->wq) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	*reclaim = zrc;
+	queue_delayed_work(zrc->wq, &zrc->work, 0);
+
+	return 0;
+err:
+	if (zrc->kc)
+		dm_kcopyd_client_destroy(zrc->kc);
+	kfree(zrc);
+
+	return ret;
+}
+
+/*
+ * Terminate reclaim.
+ */
+void dmz_dtr_reclaim(struct dmz_reclaim *zrc)
+{
+	cancel_delayed_work_sync(&zrc->work);
+	destroy_workqueue(zrc->wq);
+	dm_kcopyd_client_destroy(zrc->kc);
+	kfree(zrc);
+}
+
+/*
+ * Suspend reclaim.
+ */
+void dmz_suspend_reclaim(struct dmz_reclaim *zrc)
+{
+	cancel_delayed_work_sync(&zrc->work);
+}
+
+/*
+ * Resume reclaim.
+ */
+void dmz_resume_reclaim(struct dmz_reclaim *zrc)
+{
+	queue_delayed_work(zrc->wq, &zrc->work, DMZ_IDLE_PERIOD);
+}
+
+/*
+ * BIO accounting.
+ */
+void dmz_reclaim_bio_acc(struct dmz_reclaim *zrc)
+{
+	zrc->atime = jiffies;
+}
+
+/*
+ * Start reclaim if necessary.
+ */
+void dmz_schedule_reclaim(struct dmz_reclaim *zrc)
+{
+	if (dmz_should_reclaim(zrc))
+		mod_delayed_work(zrc->wq, &zrc->work, 0);
+}
+
diff --git a/drivers/md/dm-zoned-target.c b/drivers/md/dm-zoned-target.c
new file mode 100644
index 000000000000..2b538fa817f4
--- /dev/null
+++ b/drivers/md/dm-zoned-target.c
@@ -0,0 +1,967 @@
+/*
+ * Copyright (C) 2017 Western Digital Corporation or its affiliates.
+ *
+ * This file is released under the GPL.
+ */
+
+#include "dm-zoned.h"
+
+#include <linux/module.h>
+
+#define	DM_MSG_PREFIX		"zoned"
+
+#define DMZ_MIN_BIOS		8192
+
+/*
+ * Zone BIO context.
+ */
+struct dmz_bioctx {
+	struct dmz_target	*target;
+	struct dm_zone		*zone;
+	struct bio		*bio;
+	atomic_t		ref;
+	blk_status_t		status;
+};
+
+/*
+ * Chunk work descriptor.
+ */
+struct dm_chunk_work {
+	struct work_struct	work;
+	atomic_t		refcount;
+	struct dmz_target	*target;
+	unsigned int		chunk;
+	struct bio_list		bio_list;
+};
+
+/*
+ * Target descriptor.
+ */
+struct dmz_target {
+	struct dm_dev		*ddev;
+
+	unsigned long		flags;
+
+	/* Zoned block device information */
+	struct dmz_dev		*dev;
+
+	/* For metadata handling */
+	struct dmz_metadata     *metadata;
+
+	/* For reclaim */
+	struct dmz_reclaim	*reclaim;
+
+	/* For chunk work */
+	struct mutex		chunk_lock;
+	struct radix_tree_root	chunk_rxtree;
+	struct workqueue_struct *chunk_wq;
+
+	/* For cloned BIOs to zones */
+	struct bio_set		*bio_set;
+
+	/* For flush */
+	spinlock_t		flush_lock;
+	struct bio_list		flush_list;
+	struct delayed_work	flush_work;
+	struct workqueue_struct *flush_wq;
+};
+
+/*
+ * Flush intervals (seconds).
+ */
+#define DMZ_FLUSH_PERIOD	(10 * HZ)
+
+/*
+ * Target BIO completion.
+ */
+static inline void dmz_bio_endio(struct bio *bio, blk_status_t status)
+{
+	struct dmz_bioctx *bioctx = dm_per_bio_data(bio, sizeof(struct dmz_bioctx));
+
+	if (bioctx->status == BLK_STS_OK && status != BLK_STS_OK)
+		bioctx->status = status;
+	bio_endio(bio);
+}
+
+/*
+ * Partial clone read BIO completion callback. This terminates the
+ * target BIO when there are no more references to its context.
+ */
+static void dmz_read_bio_end_io(struct bio *bio)
+{
+	struct dmz_bioctx *bioctx = bio->bi_private;
+	blk_status_t status = bio->bi_status;
+
+	bio_put(bio);
+	dmz_bio_endio(bioctx->bio, status);
+}
+
+/*
+ * Issue a BIO to a zone. The BIO may only partially process the
+ * original target BIO.
+ */
+static int dmz_submit_read_bio(struct dmz_target *dmz, struct dm_zone *zone,
+			       struct bio *bio, sector_t chunk_block,
+			       unsigned int nr_blocks)
+{
+	struct dmz_bioctx *bioctx = dm_per_bio_data(bio, sizeof(struct dmz_bioctx));
+	sector_t sector;
+	struct bio *clone;
+
+	/* BIO remap sector */
+	sector = dmz_start_sect(dmz->metadata, zone) + dmz_blk2sect(chunk_block);
+
+	/* If the read is not partial, there is no need to clone the BIO */
+	if (nr_blocks == dmz_bio_blocks(bio)) {
+		/* Setup and submit the BIO */
+		bio->bi_iter.bi_sector = sector;
+		atomic_inc(&bioctx->ref);
+		generic_make_request(bio);
+		return 0;
+	}
+
+	/* Partial BIO: we need to clone the BIO */
+	clone = bio_clone_fast(bio, GFP_NOIO, dmz->bio_set);
+	if (!clone)
+		return -ENOMEM;
+
+	/* Setup the clone */
+	clone->bi_iter.bi_sector = sector;
+	clone->bi_iter.bi_size = dmz_blk2sect(nr_blocks) << SECTOR_SHIFT;
+	clone->bi_end_io = dmz_read_bio_end_io;
+	clone->bi_private = bioctx;
+
+	bio_advance(bio, clone->bi_iter.bi_size);
+
+	/* Submit the clone */
+	atomic_inc(&bioctx->ref);
+	generic_make_request(clone);
+
+	return 0;
+}
+
+/*
+ * Zero out pages of discarded blocks accessed by a read BIO.
+ */
+static void dmz_handle_read_zero(struct dmz_target *dmz, struct bio *bio,
+				 sector_t chunk_block, unsigned int nr_blocks)
+{
+	unsigned int size = nr_blocks << DMZ_BLOCK_SHIFT;
+
+	/* Clear nr_blocks */
+	swap(bio->bi_iter.bi_size, size);
+	zero_fill_bio(bio);
+	swap(bio->bi_iter.bi_size, size);
+
+	bio_advance(bio, size);
+}
+
+/*
+ * Process a read BIO.
+ */
+static int dmz_handle_read(struct dmz_target *dmz, struct dm_zone *zone,
+			   struct bio *bio)
+{
+	sector_t chunk_block = dmz_chunk_block(dmz->dev, dmz_bio_block(bio));
+	unsigned int nr_blocks = dmz_bio_blocks(bio);
+	sector_t end_block = chunk_block + nr_blocks;
+	struct dm_zone *rzone, *bzone;
+	int ret;
+
+	/* Read into unmapped chunks need only zeroing the BIO buffer */
+	if (!zone) {
+		zero_fill_bio(bio);
+		return 0;
+	}
+
+	dmz_dev_debug(dmz->dev, "READ chunk %llu -> %s zone %u, block %llu, %u blocks",
+		      (unsigned long long)dmz_bio_chunk(dmz->dev, bio),
+		      (dmz_is_rnd(zone) ? "RND" : "SEQ"),
+		      dmz_id(dmz->metadata, zone),
+		      (unsigned long long)chunk_block, nr_blocks);
+
+	/* Check block validity to determine the read location */
+	bzone = zone->bzone;
+	while (chunk_block < end_block) {
+		nr_blocks = 0;
+		if (dmz_is_rnd(zone) || chunk_block < zone->wp_block) {
+			/* Test block validity in the data zone */
+			ret = dmz_block_valid(dmz->metadata, zone, chunk_block);
+			if (ret < 0)
+				return ret;
+			if (ret > 0) {
+				/* Read data zone blocks */
+				nr_blocks = ret;
+				rzone = zone;
+			}
+		}
+
+		/*
+		 * No valid blocks found in the data zone.
+		 * Check the buffer zone, if there is one.
+		 */
+		if (!nr_blocks && bzone) {
+			ret = dmz_block_valid(dmz->metadata, bzone, chunk_block);
+			if (ret < 0)
+				return ret;
+			if (ret > 0) {
+				/* Read buffer zone blocks */
+				nr_blocks = ret;
+				rzone = bzone;
+			}
+		}
+
+		if (nr_blocks) {
+			/* Valid blocks found: read them */
+			nr_blocks = min_t(unsigned int, nr_blocks, end_block - chunk_block);
+			ret = dmz_submit_read_bio(dmz, rzone, bio, chunk_block, nr_blocks);
+			if (ret)
+				return ret;
+			chunk_block += nr_blocks;
+		} else {
+			/* No valid block: zeroout the current BIO block */
+			dmz_handle_read_zero(dmz, bio, chunk_block, 1);
+			chunk_block++;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Issue a write BIO to a zone.
+ */
+static void dmz_submit_write_bio(struct dmz_target *dmz, struct dm_zone *zone,
+				 struct bio *bio, sector_t chunk_block,
+				 unsigned int nr_blocks)
+{
+	struct dmz_bioctx *bioctx = dm_per_bio_data(bio, sizeof(struct dmz_bioctx));
+
+	/* Setup and submit the BIO */
+	bio->bi_bdev = dmz->dev->bdev;
+	bio->bi_iter.bi_sector = dmz_start_sect(dmz->metadata, zone) + dmz_blk2sect(chunk_block);
+	atomic_inc(&bioctx->ref);
+	generic_make_request(bio);
+
+	if (dmz_is_seq(zone))
+		zone->wp_block += nr_blocks;
+}
+
+/*
+ * Write blocks directly in a data zone, at the write pointer.
+ * If a buffer zone is assigned, invalidate the blocks written
+ * in place.
+ */
+static int dmz_handle_direct_write(struct dmz_target *dmz,
+				   struct dm_zone *zone, struct bio *bio,
+				   sector_t chunk_block,
+				   unsigned int nr_blocks)
+{
+	struct dmz_metadata *zmd = dmz->metadata;
+	struct dm_zone *bzone = zone->bzone;
+	int ret;
+
+	if (dmz_is_readonly(zone))
+		return -EROFS;
+
+	/* Submit write */
+	dmz_submit_write_bio(dmz, zone, bio, chunk_block, nr_blocks);
+
+	/*
+	 * Validate the blocks in the data zone and invalidate
+	 * in the buffer zone, if there is one.
+	 */
+	ret = dmz_validate_blocks(zmd, zone, chunk_block, nr_blocks);
+	if (ret == 0 && bzone)
+		ret = dmz_invalidate_blocks(zmd, bzone, chunk_block, nr_blocks);
+
+	return ret;
+}
+
+/*
+ * Write blocks in the buffer zone of @zone.
+ * If no buffer zone is assigned yet, get one.
+ * Called with @zone write locked.
+ */
+static int dmz_handle_buffered_write(struct dmz_target *dmz,
+				     struct dm_zone *zone, struct bio *bio,
+				     sector_t chunk_block,
+				     unsigned int nr_blocks)
+{
+	struct dmz_metadata *zmd = dmz->metadata;
+	struct dm_zone *bzone;
+	int ret;
+
+	/* Get the buffer zone. One will be allocated if needed */
+	bzone = dmz_get_chunk_buffer(zmd, zone);
+	if (!bzone)
+		return -ENOSPC;
+
+	if (dmz_is_readonly(bzone))
+		return -EROFS;
+
+	/* Submit write */
+	dmz_submit_write_bio(dmz, bzone, bio, chunk_block, nr_blocks);
+
+	/*
+	 * Validate the blocks in the buffer zone
+	 * and invalidate in the data zone.
+	 */
+	ret = dmz_validate_blocks(zmd, bzone, chunk_block, nr_blocks);
+	if (ret == 0 && chunk_block < zone->wp_block)
+		ret = dmz_invalidate_blocks(zmd, zone, chunk_block, nr_blocks);
+
+	return ret;
+}
+
+/*
+ * Process a write BIO.
+ */
+static int dmz_handle_write(struct dmz_target *dmz, struct dm_zone *zone,
+			    struct bio *bio)
+{
+	sector_t chunk_block = dmz_chunk_block(dmz->dev, dmz_bio_block(bio));
+	unsigned int nr_blocks = dmz_bio_blocks(bio);
+
+	if (!zone)
+		return -ENOSPC;
+
+	dmz_dev_debug(dmz->dev, "WRITE chunk %llu -> %s zone %u, block %llu, %u blocks",
+		      (unsigned long long)dmz_bio_chunk(dmz->dev, bio),
+		      (dmz_is_rnd(zone) ? "RND" : "SEQ"),
+		      dmz_id(dmz->metadata, zone),
+		      (unsigned long long)chunk_block, nr_blocks);
+
+	if (dmz_is_rnd(zone) || chunk_block == zone->wp_block) {
+		/*
+		 * zone is a random zone or it is a sequential zone
+		 * and the BIO is aligned to the zone write pointer:
+		 * direct write the zone.
+		 */
+		return dmz_handle_direct_write(dmz, zone, bio, chunk_block, nr_blocks);
+	}
+
+	/*
+	 * This is an unaligned write in a sequential zone:
+	 * use buffered write.
+	 */
+	return dmz_handle_buffered_write(dmz, zone, bio, chunk_block, nr_blocks);
+}
+
+/*
+ * Process a discard BIO.
+ */
+static int dmz_handle_discard(struct dmz_target *dmz, struct dm_zone *zone,
+			      struct bio *bio)
+{
+	struct dmz_metadata *zmd = dmz->metadata;
+	sector_t block = dmz_bio_block(bio);
+	unsigned int nr_blocks = dmz_bio_blocks(bio);
+	sector_t chunk_block = dmz_chunk_block(dmz->dev, block);
+	int ret = 0;
+
+	/* For unmapped chunks, there is nothing to do */
+	if (!zone)
+		return 0;
+
+	if (dmz_is_readonly(zone))
+		return -EROFS;
+
+	dmz_dev_debug(dmz->dev, "DISCARD chunk %llu -> zone %u, block %llu, %u blocks",
+		      (unsigned long long)dmz_bio_chunk(dmz->dev, bio),
+		      dmz_id(zmd, zone),
+		      (unsigned long long)chunk_block, nr_blocks);
+
+	/*
+	 * Invalidate blocks in the data zone and its
+	 * buffer zone if one is mapped.
+	 */
+	if (dmz_is_rnd(zone) || chunk_block < zone->wp_block)
+		ret = dmz_invalidate_blocks(zmd, zone, chunk_block, nr_blocks);
+	if (ret == 0 && zone->bzone)
+		ret = dmz_invalidate_blocks(zmd, zone->bzone,
+					    chunk_block, nr_blocks);
+	return ret;
+}
+
+/*
+ * Process a BIO.
+ */
+static void dmz_handle_bio(struct dmz_target *dmz, struct dm_chunk_work *cw,
+			   struct bio *bio)
+{
+	struct dmz_bioctx *bioctx = dm_per_bio_data(bio, sizeof(struct dmz_bioctx));
+	struct dmz_metadata *zmd = dmz->metadata;
+	struct dm_zone *zone;
+	int ret;
+
+	/*
+	 * Write may trigger a zone allocation. So make sure the
+	 * allocation can succeed.
+	 */
+	if (bio_op(bio) == REQ_OP_WRITE)
+		dmz_schedule_reclaim(dmz->reclaim);
+
+	dmz_lock_metadata(zmd);
+
+	/*
+	 * Get the data zone mapping the chunk. There may be no
+	 * mapping for read and discard. If a mapping is obtained,
+	 + the zone returned will be set to active state.
+	 */
+	zone = dmz_get_chunk_mapping(zmd, dmz_bio_chunk(dmz->dev, bio),
+				     bio_op(bio));
+	if (IS_ERR(zone)) {
+		ret = PTR_ERR(zone);
+		goto out;
+	}
+
+	/* Process the BIO */
+	if (zone) {
+		dmz_activate_zone(zone);
+		bioctx->zone = zone;
+	}
+
+	switch (bio_op(bio)) {
+	case REQ_OP_READ:
+		ret = dmz_handle_read(dmz, zone, bio);
+		break;
+	case REQ_OP_WRITE:
+		ret = dmz_handle_write(dmz, zone, bio);
+		break;
+	case REQ_OP_DISCARD:
+	case REQ_OP_WRITE_ZEROES:
+		ret = dmz_handle_discard(dmz, zone, bio);
+		break;
+	default:
+		dmz_dev_err(dmz->dev, "Unsupported BIO operation 0x%x",
+			    bio_op(bio));
+		ret = -EIO;
+	}
+
+	/*
+	 * Release the chunk mapping. This will check that the mapping
+	 * is still valid, that is, that the zone used still has valid blocks.
+	 */
+	if (zone)
+		dmz_put_chunk_mapping(zmd, zone);
+out:
+	dmz_bio_endio(bio, errno_to_blk_status(ret));
+
+	dmz_unlock_metadata(zmd);
+}
+
+/*
+ * Increment a chunk reference counter.
+ */
+static inline void dmz_get_chunk_work(struct dm_chunk_work *cw)
+{
+	atomic_inc(&cw->refcount);
+}
+
+/*
+ * Decrement a chunk work reference count and
+ * free it if it becomes 0.
+ */
+static void dmz_put_chunk_work(struct dm_chunk_work *cw)
+{
+	if (atomic_dec_and_test(&cw->refcount)) {
+		WARN_ON(!bio_list_empty(&cw->bio_list));
+		radix_tree_delete(&cw->target->chunk_rxtree, cw->chunk);
+		kfree(cw);
+	}
+}
+
+/*
+ * Chunk BIO work function.
+ */
+static void dmz_chunk_work(struct work_struct *work)
+{
+	struct dm_chunk_work *cw = container_of(work, struct dm_chunk_work, work);
+	struct dmz_target *dmz = cw->target;
+	struct bio *bio;
+
+	mutex_lock(&dmz->chunk_lock);
+
+	/* Process the chunk BIOs */
+	while ((bio = bio_list_pop(&cw->bio_list))) {
+		mutex_unlock(&dmz->chunk_lock);
+		dmz_handle_bio(dmz, cw, bio);
+		mutex_lock(&dmz->chunk_lock);
+		dmz_put_chunk_work(cw);
+	}
+
+	/* Queueing the work incremented the work refcount */
+	dmz_put_chunk_work(cw);
+
+	mutex_unlock(&dmz->chunk_lock);
+}
+
+/*
+ * Flush work.
+ */
+static void dmz_flush_work(struct work_struct *work)
+{
+	struct dmz_target *dmz = container_of(work, struct dmz_target, flush_work.work);
+	struct bio *bio;
+	int ret;
+
+	/* Flush dirty metadata blocks */
+	ret = dmz_flush_metadata(dmz->metadata);
+
+	/* Process queued flush requests */
+	while (1) {
+		spin_lock(&dmz->flush_lock);
+		bio = bio_list_pop(&dmz->flush_list);
+		spin_unlock(&dmz->flush_lock);
+
+		if (!bio)
+			break;
+
+		dmz_bio_endio(bio, errno_to_blk_status(ret));
+	}
+
+	queue_delayed_work(dmz->flush_wq, &dmz->flush_work, DMZ_FLUSH_PERIOD);
+}
+
+/*
+ * Get a chunk work and start it to process a new BIO.
+ * If the BIO chunk has no work yet, create one.
+ */
+static void dmz_queue_chunk_work(struct dmz_target *dmz, struct bio *bio)
+{
+	unsigned int chunk = dmz_bio_chunk(dmz->dev, bio);
+	struct dm_chunk_work *cw;
+
+	mutex_lock(&dmz->chunk_lock);
+
+	/* Get the BIO chunk work. If one is not active yet, create one */
+	cw = radix_tree_lookup(&dmz->chunk_rxtree, chunk);
+	if (!cw) {
+		int ret;
+
+		/* Create a new chunk work */
+		cw = kmalloc(sizeof(struct dm_chunk_work), GFP_NOFS);
+		if (!cw)
+			goto out;
+
+		INIT_WORK(&cw->work, dmz_chunk_work);
+		atomic_set(&cw->refcount, 0);
+		cw->target = dmz;
+		cw->chunk = chunk;
+		bio_list_init(&cw->bio_list);
+
+		ret = radix_tree_insert(&dmz->chunk_rxtree, chunk, cw);
+		if (unlikely(ret)) {
+			kfree(cw);
+			cw = NULL;
+			goto out;
+		}
+	}
+
+	bio_list_add(&cw->bio_list, bio);
+	dmz_get_chunk_work(cw);
+
+	if (queue_work(dmz->chunk_wq, &cw->work))
+		dmz_get_chunk_work(cw);
+out:
+	mutex_unlock(&dmz->chunk_lock);
+}
+
+/*
+ * Process a new BIO.
+ */
+static int dmz_map(struct dm_target *ti, struct bio *bio)
+{
+	struct dmz_target *dmz = ti->private;
+	struct dmz_dev *dev = dmz->dev;
+	struct dmz_bioctx *bioctx = dm_per_bio_data(bio, sizeof(struct dmz_bioctx));
+	sector_t sector = bio->bi_iter.bi_sector;
+	unsigned int nr_sectors = bio_sectors(bio);
+	sector_t chunk_sector;
+
+	dmz_dev_debug(dev, "BIO op %d sector %llu + %u => chunk %llu, block %llu, %u blocks",
+		      bio_op(bio), (unsigned long long)sector, nr_sectors,
+		      (unsigned long long)dmz_bio_chunk(dmz->dev, bio),
+		      (unsigned long long)dmz_chunk_block(dmz->dev, dmz_bio_block(bio)),
+		      (unsigned int)dmz_bio_blocks(bio));
+
+	bio->bi_bdev = dev->bdev;
+
+	if (!nr_sectors && (bio_op(bio) != REQ_OP_FLUSH) && (bio_op(bio) != REQ_OP_WRITE))
+		return DM_MAPIO_REMAPPED;
+
+	/* The BIO should be block aligned */
+	if ((nr_sectors & DMZ_BLOCK_SECTORS_MASK) || (sector & DMZ_BLOCK_SECTORS_MASK))
+		return DM_MAPIO_KILL;
+
+	/* Initialize the BIO context */
+	bioctx->target = dmz;
+	bioctx->zone = NULL;
+	bioctx->bio = bio;
+	atomic_set(&bioctx->ref, 1);
+	bioctx->status = BLK_STS_OK;
+
+	/* Set the BIO pending in the flush list */
+	if (bio_op(bio) == REQ_OP_FLUSH || (!nr_sectors && bio_op(bio) == REQ_OP_WRITE)) {
+		spin_lock(&dmz->flush_lock);
+		bio_list_add(&dmz->flush_list, bio);
+		spin_unlock(&dmz->flush_lock);
+		mod_delayed_work(dmz->flush_wq, &dmz->flush_work, 0);
+		return DM_MAPIO_SUBMITTED;
+	}
+
+	/* Split zone BIOs to fit entirely into a zone */
+	chunk_sector = sector & (dev->zone_nr_sectors - 1);
+	if (chunk_sector + nr_sectors > dev->zone_nr_sectors)
+		dm_accept_partial_bio(bio, dev->zone_nr_sectors - chunk_sector);
+
+	/* Now ready to handle this BIO */
+	dmz_reclaim_bio_acc(dmz->reclaim);
+	dmz_queue_chunk_work(dmz, bio);
+
+	return DM_MAPIO_SUBMITTED;
+}
+
+/*
+ * Completed target BIO processing.
+ */
+static int dmz_end_io(struct dm_target *ti, struct bio *bio, blk_status_t *error)
+{
+	struct dmz_bioctx *bioctx = dm_per_bio_data(bio, sizeof(struct dmz_bioctx));
+
+	if (bioctx->status == BLK_STS_OK && *error)
+		bioctx->status = *error;
+
+	if (!atomic_dec_and_test(&bioctx->ref))
+		return DM_ENDIO_INCOMPLETE;
+
+	/* Done */
+	bio->bi_status = bioctx->status;
+
+	if (bioctx->zone) {
+		struct dm_zone *zone = bioctx->zone;
+
+		if (*error && bio_op(bio) == REQ_OP_WRITE) {
+			if (dmz_is_seq(zone))
+				set_bit(DMZ_SEQ_WRITE_ERR, &zone->flags);
+		}
+		dmz_deactivate_zone(zone);
+	}
+
+	return DM_ENDIO_DONE;
+}
+
+/*
+ * Get zoned device information.
+ */
+static int dmz_get_zoned_device(struct dm_target *ti, char *path)
+{
+	struct dmz_target *dmz = ti->private;
+	struct request_queue *q;
+	struct dmz_dev *dev;
+	int ret;
+
+	/* Get the target device */
+	ret = dm_get_device(ti, path, dm_table_get_mode(ti->table), &dmz->ddev);
+	if (ret) {
+		ti->error = "Get target device failed";
+		dmz->ddev = NULL;
+		return ret;
+	}
+
+	dev = kzalloc(sizeof(struct dmz_dev), GFP_KERNEL);
+	if (!dev) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	dev->bdev = dmz->ddev->bdev;
+	(void)bdevname(dev->bdev, dev->name);
+
+	if (bdev_zoned_model(dev->bdev) == BLK_ZONED_NONE) {
+		ti->error = "Not a zoned block device";
+		ret = -EINVAL;
+		goto err;
+	}
+
+	dev->capacity = i_size_read(dev->bdev->bd_inode) >> SECTOR_SHIFT;
+	if (ti->begin || (ti->len != dev->capacity)) {
+		ti->error = "Partial mapping not supported";
+		ret = -EINVAL;
+		goto err;
+	}
+
+	q = bdev_get_queue(dev->bdev);
+	dev->zone_nr_sectors = q->limits.chunk_sectors;
+	dev->zone_nr_sectors_shift = ilog2(dev->zone_nr_sectors);
+
+	dev->zone_nr_blocks = dmz_sect2blk(dev->zone_nr_sectors);
+	dev->zone_nr_blocks_shift = ilog2(dev->zone_nr_blocks);
+
+	dev->nr_zones = (dev->capacity + dev->zone_nr_sectors - 1)
+		>> dev->zone_nr_sectors_shift;
+
+	dmz->dev = dev;
+
+	return 0;
+err:
+	dm_put_device(ti, dmz->ddev);
+	kfree(dev);
+
+	return ret;
+}
+
+/*
+ * Cleanup zoned device information.
+ */
+static void dmz_put_zoned_device(struct dm_target *ti)
+{
+	struct dmz_target *dmz = ti->private;
+
+	dm_put_device(ti, dmz->ddev);
+	kfree(dmz->dev);
+	dmz->dev = NULL;
+}
+
+/*
+ * Setup target.
+ */
+static int dmz_ctr(struct dm_target *ti, unsigned int argc, char **argv)
+{
+	struct dmz_target *dmz;
+	struct dmz_dev *dev;
+	int ret;
+
+	/* Check arguments */
+	if (argc != 1) {
+		ti->error = "Invalid argument count";
+		return -EINVAL;
+	}
+
+	/* Allocate and initialize the target descriptor */
+	dmz = kzalloc(sizeof(struct dmz_target), GFP_KERNEL);
+	if (!dmz) {
+		ti->error = "Unable to allocate the zoned target descriptor";
+		return -ENOMEM;
+	}
+	ti->private = dmz;
+
+	/* Get the target zoned block device */
+	ret = dmz_get_zoned_device(ti, argv[0]);
+	if (ret) {
+		dmz->ddev = NULL;
+		goto err;
+	}
+
+	/* Initialize metadata */
+	dev = dmz->dev;
+	ret = dmz_ctr_metadata(dev, &dmz->metadata);
+	if (ret) {
+		ti->error = "Metadata initialization failed";
+		goto err_dev;
+	}
+
+	/* Set target (no write same support) */
+	ti->max_io_len = dev->zone_nr_sectors << 9;
+	ti->num_flush_bios = 1;
+	ti->num_discard_bios = 1;
+	ti->num_write_zeroes_bios = 1;
+	ti->per_io_data_size = sizeof(struct dmz_bioctx);
+	ti->flush_supported = true;
+	ti->discards_supported = true;
+	ti->split_discard_bios = true;
+
+	/* The exposed capacity is the number of chunks that can be mapped */
+	ti->len = (sector_t)dmz_nr_chunks(dmz->metadata) << dev->zone_nr_sectors_shift;
+
+	/* Zone BIO */
+	dmz->bio_set = bioset_create(DMZ_MIN_BIOS, 0, 0);
+	if (!dmz->bio_set) {
+		ti->error = "Create BIO set failed";
+		ret = -ENOMEM;
+		goto err_meta;
+	}
+
+	/* Chunk BIO work */
+	mutex_init(&dmz->chunk_lock);
+	INIT_RADIX_TREE(&dmz->chunk_rxtree, GFP_NOFS);
+	dmz->chunk_wq = alloc_workqueue("dmz_cwq_%s", WQ_MEM_RECLAIM | WQ_UNBOUND,
+					0, dev->name);
+	if (!dmz->chunk_wq) {
+		ti->error = "Create chunk workqueue failed";
+		ret = -ENOMEM;
+		goto err_bio;
+	}
+
+	/* Flush work */
+	spin_lock_init(&dmz->flush_lock);
+	bio_list_init(&dmz->flush_list);
+	INIT_DELAYED_WORK(&dmz->flush_work, dmz_flush_work);
+	dmz->flush_wq = alloc_ordered_workqueue("dmz_fwq_%s", WQ_MEM_RECLAIM,
+						dev->name);
+	if (!dmz->flush_wq) {
+		ti->error = "Create flush workqueue failed";
+		ret = -ENOMEM;
+		goto err_cwq;
+	}
+	mod_delayed_work(dmz->flush_wq, &dmz->flush_work, DMZ_FLUSH_PERIOD);
+
+	/* Initialize reclaim */
+	ret = dmz_ctr_reclaim(dev, dmz->metadata, &dmz->reclaim);
+	if (ret) {
+		ti->error = "Zone reclaim initialization failed";
+		goto err_fwq;
+	}
+
+	dmz_dev_info(dev, "Target device: %llu 512-byte logical sectors (%llu blocks)",
+		     (unsigned long long)ti->len,
+		     (unsigned long long)dmz_sect2blk(ti->len));
+
+	return 0;
+err_fwq:
+	destroy_workqueue(dmz->flush_wq);
+err_cwq:
+	destroy_workqueue(dmz->chunk_wq);
+err_bio:
+	bioset_free(dmz->bio_set);
+err_meta:
+	dmz_dtr_metadata(dmz->metadata);
+err_dev:
+	dmz_put_zoned_device(ti);
+err:
+	kfree(dmz);
+
+	return ret;
+}
+
+/*
+ * Cleanup target.
+ */
+static void dmz_dtr(struct dm_target *ti)
+{
+	struct dmz_target *dmz = ti->private;
+
+	flush_workqueue(dmz->chunk_wq);
+	destroy_workqueue(dmz->chunk_wq);
+
+	dmz_dtr_reclaim(dmz->reclaim);
+
+	cancel_delayed_work_sync(&dmz->flush_work);
+	destroy_workqueue(dmz->flush_wq);
+
+	(void) dmz_flush_metadata(dmz->metadata);
+
+	dmz_dtr_metadata(dmz->metadata);
+
+	bioset_free(dmz->bio_set);
+
+	dmz_put_zoned_device(ti);
+
+	kfree(dmz);
+}
+
+/*
+ * Setup target request queue limits.
+ */
+static void dmz_io_hints(struct dm_target *ti, struct queue_limits *limits)
+{
+	struct dmz_target *dmz = ti->private;
+	unsigned int chunk_sectors = dmz->dev->zone_nr_sectors;
+
+	limits->logical_block_size = DMZ_BLOCK_SIZE;
+	limits->physical_block_size = DMZ_BLOCK_SIZE;
+
+	blk_limits_io_min(limits, DMZ_BLOCK_SIZE);
+	blk_limits_io_opt(limits, DMZ_BLOCK_SIZE);
+
+	limits->discard_alignment = DMZ_BLOCK_SIZE;
+	limits->discard_granularity = DMZ_BLOCK_SIZE;
+	limits->max_discard_sectors = chunk_sectors;
+	limits->max_hw_discard_sectors = chunk_sectors;
+	limits->max_write_zeroes_sectors = chunk_sectors;
+
+	/* FS hint to try to align to the device zone size */
+	limits->chunk_sectors = chunk_sectors;
+	limits->max_sectors = chunk_sectors;
+
+	/* We are exposing a drive-managed zoned block device */
+	limits->zoned = BLK_ZONED_NONE;
+}
+
+/*
+ * Pass on ioctl to the backend device.
+ */
+static int dmz_prepare_ioctl(struct dm_target *ti,
+			     struct block_device **bdev, fmode_t *mode)
+{
+	struct dmz_target *dmz = ti->private;
+
+	*bdev = dmz->dev->bdev;
+
+	return 0;
+}
+
+/*
+ * Stop works on suspend.
+ */
+static void dmz_suspend(struct dm_target *ti)
+{
+	struct dmz_target *dmz = ti->private;
+
+	flush_workqueue(dmz->chunk_wq);
+	dmz_suspend_reclaim(dmz->reclaim);
+	cancel_delayed_work_sync(&dmz->flush_work);
+}
+
+/*
+ * Restart works on resume or if suspend failed.
+ */
+static void dmz_resume(struct dm_target *ti)
+{
+	struct dmz_target *dmz = ti->private;
+
+	queue_delayed_work(dmz->flush_wq, &dmz->flush_work, DMZ_FLUSH_PERIOD);
+	dmz_resume_reclaim(dmz->reclaim);
+}
+
+static int dmz_iterate_devices(struct dm_target *ti,
+			       iterate_devices_callout_fn fn, void *data)
+{
+	struct dmz_target *dmz = ti->private;
+
+	return fn(ti, dmz->ddev, 0, dmz->dev->capacity, data);
+}
+
+static struct target_type dmz_type = {
+	.name		 = "zoned",
+	.version	 = {1, 0, 0},
+	.features	 = DM_TARGET_SINGLETON | DM_TARGET_ZONED_HM,
+	.module		 = THIS_MODULE,
+	.ctr		 = dmz_ctr,
+	.dtr		 = dmz_dtr,
+	.map		 = dmz_map,
+	.end_io		 = dmz_end_io,
+	.io_hints	 = dmz_io_hints,
+	.prepare_ioctl	 = dmz_prepare_ioctl,
+	.postsuspend	 = dmz_suspend,
+	.resume		 = dmz_resume,
+	.iterate_devices = dmz_iterate_devices,
+};
+
+static int __init dmz_init(void)
+{
+	return dm_register_target(&dmz_type);
+}
+
+static void __exit dmz_exit(void)
+{
+	dm_unregister_target(&dmz_type);
+}
+
+module_init(dmz_init);
+module_exit(dmz_exit);
+
+MODULE_DESCRIPTION(DM_NAME " target for zoned block devices");
+MODULE_AUTHOR("Damien Le Moal <damien.lemoal@wdc.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/md/dm-zoned.h b/drivers/md/dm-zoned.h
new file mode 100644
index 000000000000..12419f0bfe78
--- /dev/null
+++ b/drivers/md/dm-zoned.h
@@ -0,0 +1,228 @@
+/*
+ * Copyright (C) 2017 Western Digital Corporation or its affiliates.
+ *
+ * This file is released under the GPL.
+ */
+
+#ifndef DM_ZONED_H
+#define DM_ZONED_H
+
+#include <linux/types.h>
+#include <linux/blkdev.h>
+#include <linux/device-mapper.h>
+#include <linux/dm-kcopyd.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+#include <linux/rwsem.h>
+#include <linux/rbtree.h>
+#include <linux/radix-tree.h>
+#include <linux/shrinker.h>
+
+/*
+ * dm-zoned creates block devices with 4KB blocks, always.
+ */
+#define DMZ_BLOCK_SHIFT		12
+#define DMZ_BLOCK_SIZE		(1 << DMZ_BLOCK_SHIFT)
+#define DMZ_BLOCK_MASK		(DMZ_BLOCK_SIZE - 1)
+
+#define DMZ_BLOCK_SHIFT_BITS	(DMZ_BLOCK_SHIFT + 3)
+#define DMZ_BLOCK_SIZE_BITS	(1 << DMZ_BLOCK_SHIFT_BITS)
+#define DMZ_BLOCK_MASK_BITS	(DMZ_BLOCK_SIZE_BITS - 1)
+
+#define DMZ_BLOCK_SECTORS_SHIFT	(DMZ_BLOCK_SHIFT - SECTOR_SHIFT)
+#define DMZ_BLOCK_SECTORS	(DMZ_BLOCK_SIZE >> SECTOR_SHIFT)
+#define DMZ_BLOCK_SECTORS_MASK	(DMZ_BLOCK_SECTORS - 1)
+
+/*
+ * 4KB block <-> 512B sector conversion.
+ */
+#define dmz_blk2sect(b)		((sector_t)(b) << DMZ_BLOCK_SECTORS_SHIFT)
+#define dmz_sect2blk(s)		((sector_t)(s) >> DMZ_BLOCK_SECTORS_SHIFT)
+
+#define dmz_bio_block(bio)	dmz_sect2blk((bio)->bi_iter.bi_sector)
+#define dmz_bio_blocks(bio)	dmz_sect2blk(bio_sectors(bio))
+
+/*
+ * Zoned block device information.
+ */
+struct dmz_dev {
+	struct block_device	*bdev;
+
+	char			name[BDEVNAME_SIZE];
+
+	sector_t		capacity;
+
+	unsigned int		nr_zones;
+
+	sector_t		zone_nr_sectors;
+	unsigned int		zone_nr_sectors_shift;
+
+	sector_t		zone_nr_blocks;
+	sector_t		zone_nr_blocks_shift;
+};
+
+#define dmz_bio_chunk(dev, bio)	((bio)->bi_iter.bi_sector >> \
+				 (dev)->zone_nr_sectors_shift)
+#define dmz_chunk_block(dev, b)	((b) & ((dev)->zone_nr_blocks - 1))
+
+/*
+ * Zone descriptor.
+ */
+struct dm_zone {
+	/* For listing the zone depending on its state */
+	struct list_head	link;
+
+	/* Zone type and state */
+	unsigned long		flags;
+
+	/* Zone activation reference count */
+	atomic_t		refcount;
+
+	/* Zone write pointer block (relative to the zone start block) */
+	unsigned int		wp_block;
+
+	/* Zone weight (number of valid blocks in the zone) */
+	unsigned int		weight;
+
+	/* The chunk that the zone maps */
+	unsigned int		chunk;
+
+	/*
+	 * For a sequential data zone, pointer to the random zone
+	 * used as a buffer for processing unaligned writes.
+	 * For a buffer zone, this points back to the data zone.
+	 */
+	struct dm_zone		*bzone;
+};
+
+/*
+ * Zone flags.
+ */
+enum {
+	/* Zone write type */
+	DMZ_RND,
+	DMZ_SEQ,
+
+	/* Zone critical condition */
+	DMZ_OFFLINE,
+	DMZ_READ_ONLY,
+
+	/* How the zone is being used */
+	DMZ_META,
+	DMZ_DATA,
+	DMZ_BUF,
+
+	/* Zone internal state */
+	DMZ_ACTIVE,
+	DMZ_RECLAIM,
+	DMZ_SEQ_WRITE_ERR,
+};
+
+/*
+ * Zone data accessors.
+ */
+#define dmz_is_rnd(z)		test_bit(DMZ_RND, &(z)->flags)
+#define dmz_is_seq(z)		test_bit(DMZ_SEQ, &(z)->flags)
+#define dmz_is_empty(z)		((z)->wp_block == 0)
+#define dmz_is_offline(z)	test_bit(DMZ_OFFLINE, &(z)->flags)
+#define dmz_is_readonly(z)	test_bit(DMZ_READ_ONLY, &(z)->flags)
+#define dmz_is_active(z)	test_bit(DMZ_ACTIVE, &(z)->flags)
+#define dmz_in_reclaim(z)	test_bit(DMZ_RECLAIM, &(z)->flags)
+#define dmz_seq_write_err(z)	test_bit(DMZ_SEQ_WRITE_ERR, &(z)->flags)
+
+#define dmz_is_meta(z)		test_bit(DMZ_META, &(z)->flags)
+#define dmz_is_buf(z)		test_bit(DMZ_BUF, &(z)->flags)
+#define dmz_is_data(z)		test_bit(DMZ_DATA, &(z)->flags)
+
+#define dmz_weight(z)		((z)->weight)
+
+/*
+ * Message functions.
+ */
+#define dmz_dev_info(dev, format, args...)	\
+	DMINFO("(%s): " format, (dev)->name, ## args)
+
+#define dmz_dev_err(dev, format, args...)	\
+	DMERR("(%s): " format, (dev)->name, ## args)
+
+#define dmz_dev_warn(dev, format, args...)	\
+	DMWARN("(%s): " format, (dev)->name, ## args)
+
+#define dmz_dev_debug(dev, format, args...)	\
+	DMDEBUG("(%s): " format, (dev)->name, ## args)
+
+struct dmz_metadata;
+struct dmz_reclaim;
+
+/*
+ * Functions defined in dm-zoned-metadata.c
+ */
+int dmz_ctr_metadata(struct dmz_dev *dev, struct dmz_metadata **zmd);
+void dmz_dtr_metadata(struct dmz_metadata *zmd);
+int dmz_resume_metadata(struct dmz_metadata *zmd);
+
+void dmz_lock_map(struct dmz_metadata *zmd);
+void dmz_unlock_map(struct dmz_metadata *zmd);
+void dmz_lock_metadata(struct dmz_metadata *zmd);
+void dmz_unlock_metadata(struct dmz_metadata *zmd);
+void dmz_lock_flush(struct dmz_metadata *zmd);
+void dmz_unlock_flush(struct dmz_metadata *zmd);
+int dmz_flush_metadata(struct dmz_metadata *zmd);
+
+unsigned int dmz_id(struct dmz_metadata *zmd, struct dm_zone *zone);
+sector_t dmz_start_sect(struct dmz_metadata *zmd, struct dm_zone *zone);
+sector_t dmz_start_block(struct dmz_metadata *zmd, struct dm_zone *zone);
+unsigned int dmz_nr_chunks(struct dmz_metadata *zmd);
+
+#define DMZ_ALLOC_RND		0x01
+#define DMZ_ALLOC_RECLAIM	0x02
+
+struct dm_zone *dmz_alloc_zone(struct dmz_metadata *zmd, unsigned long flags);
+void dmz_free_zone(struct dmz_metadata *zmd, struct dm_zone *zone);
+
+void dmz_map_zone(struct dmz_metadata *zmd, struct dm_zone *zone,
+		  unsigned int chunk);
+void dmz_unmap_zone(struct dmz_metadata *zmd, struct dm_zone *zone);
+unsigned int dmz_nr_rnd_zones(struct dmz_metadata *zmd);
+unsigned int dmz_nr_unmap_rnd_zones(struct dmz_metadata *zmd);
+
+void dmz_activate_zone(struct dm_zone *zone);
+void dmz_deactivate_zone(struct dm_zone *zone);
+
+int dmz_lock_zone_reclaim(struct dm_zone *zone);
+void dmz_unlock_zone_reclaim(struct dm_zone *zone);
+struct dm_zone *dmz_get_zone_for_reclaim(struct dmz_metadata *zmd);
+
+struct dm_zone *dmz_get_chunk_mapping(struct dmz_metadata *zmd,
+				      unsigned int chunk, int op);
+void dmz_put_chunk_mapping(struct dmz_metadata *zmd, struct dm_zone *zone);
+struct dm_zone *dmz_get_chunk_buffer(struct dmz_metadata *zmd,
+				     struct dm_zone *dzone);
+
+int dmz_validate_blocks(struct dmz_metadata *zmd, struct dm_zone *zone,
+			sector_t chunk_block, unsigned int nr_blocks);
+int dmz_invalidate_blocks(struct dmz_metadata *zmd, struct dm_zone *zone,
+			  sector_t chunk_block, unsigned int nr_blocks);
+int dmz_block_valid(struct dmz_metadata *zmd, struct dm_zone *zone,
+		    sector_t chunk_block);
+int dmz_first_valid_block(struct dmz_metadata *zmd, struct dm_zone *zone,
+			  sector_t *chunk_block);
+int dmz_copy_valid_blocks(struct dmz_metadata *zmd, struct dm_zone *from_zone,
+			  struct dm_zone *to_zone);
+int dmz_merge_valid_blocks(struct dmz_metadata *zmd, struct dm_zone *from_zone,
+			   struct dm_zone *to_zone, sector_t chunk_block);
+
+/*
+ * Functions defined in dm-zoned-reclaim.c
+ */
+int dmz_ctr_reclaim(struct dmz_dev *dev, struct dmz_metadata *zmd,
+		    struct dmz_reclaim **zrc);
+void dmz_dtr_reclaim(struct dmz_reclaim *zrc);
+void dmz_suspend_reclaim(struct dmz_reclaim *zrc);
+void dmz_resume_reclaim(struct dmz_reclaim *zrc);
+void dmz_reclaim_bio_acc(struct dmz_reclaim *zrc);
+void dmz_schedule_reclaim(struct dmz_reclaim *zrc);
+
+#endif /* DM_ZONED_H */
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index 37ccd73c79ec..c2afe7a5755f 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -58,12 +58,15 @@ static DECLARE_WORK(deferred_remove_work, do_deferred_remove);
 
 static struct workqueue_struct *deferred_remove_workqueue;
 
+atomic_t dm_global_event_nr = ATOMIC_INIT(0);
+DECLARE_WAIT_QUEUE_HEAD(dm_global_eventq);
+
 /*
  * One of these is allocated per bio.
  */
 struct dm_io {
 	struct mapped_device *md;
-	int error;
+	blk_status_t status;
 	atomic_t io_count;
 	struct bio *bio;
 	unsigned long start_time;
@@ -768,23 +771,24 @@ static int __noflush_suspending(struct mapped_device *md)
  * Decrements the number of outstanding ios that a bio has been
  * cloned into, completing the original io if necc.
  */
-static void dec_pending(struct dm_io *io, int error)
+static void dec_pending(struct dm_io *io, blk_status_t error)
 {
 	unsigned long flags;
-	int io_error;
+	blk_status_t io_error;
 	struct bio *bio;
 	struct mapped_device *md = io->md;
 
 	/* Push-back supersedes any I/O errors */
 	if (unlikely(error)) {
 		spin_lock_irqsave(&io->endio_lock, flags);
-		if (!(io->error > 0 && __noflush_suspending(md)))
-			io->error = error;
+		if (!(io->status == BLK_STS_DM_REQUEUE &&
+				__noflush_suspending(md)))
+			io->status = error;
 		spin_unlock_irqrestore(&io->endio_lock, flags);
 	}
 
 	if (atomic_dec_and_test(&io->io_count)) {
-		if (io->error == DM_ENDIO_REQUEUE) {
+		if (io->status == BLK_STS_DM_REQUEUE) {
 			/*
 			 * Target requested pushing back the I/O.
 			 */
@@ -793,16 +797,16 @@ static void dec_pending(struct dm_io *io, int error)
 				bio_list_add_head(&md->deferred, io->bio);
 			else
 				/* noflush suspend was interrupted. */
-				io->error = -EIO;
+				io->status = BLK_STS_IOERR;
 			spin_unlock_irqrestore(&md->deferred_lock, flags);
 		}
 
-		io_error = io->error;
+		io_error = io->status;
 		bio = io->bio;
 		end_io_acct(io);
 		free_io(md, io);
 
-		if (io_error == DM_ENDIO_REQUEUE)
+		if (io_error == BLK_STS_DM_REQUEUE)
 			return;
 
 		if ((bio->bi_opf & REQ_PREFLUSH) && bio->bi_iter.bi_size) {
@@ -814,7 +818,7 @@ static void dec_pending(struct dm_io *io, int error)
 			queue_io(md, bio);
 		} else {
 			/* done with normal IO or empty flush */
-			bio->bi_error = io_error;
+			bio->bi_status = io_error;
 			bio_endio(bio);
 		}
 	}
@@ -838,31 +842,13 @@ void disable_write_zeroes(struct mapped_device *md)
 
 static void clone_endio(struct bio *bio)
 {
-	int error = bio->bi_error;
-	int r = error;
+	blk_status_t error = bio->bi_status;
 	struct dm_target_io *tio = container_of(bio, struct dm_target_io, clone);
 	struct dm_io *io = tio->io;
 	struct mapped_device *md = tio->io->md;
 	dm_endio_fn endio = tio->ti->type->end_io;
 
-	if (endio) {
-		r = endio(tio->ti, bio, error);
-		if (r < 0 || r == DM_ENDIO_REQUEUE)
-			/*
-			 * error and requeue request are handled
-			 * in dec_pending().
-			 */
-			error = r;
-		else if (r == DM_ENDIO_INCOMPLETE)
-			/* The target will handle the io */
-			return;
-		else if (r) {
-			DMWARN("unimplemented target endio return value: %d", r);
-			BUG();
-		}
-	}
-
-	if (unlikely(r == -EREMOTEIO)) {
+	if (unlikely(error == BLK_STS_TARGET)) {
 		if (bio_op(bio) == REQ_OP_WRITE_SAME &&
 		    !bdev_get_queue(bio->bi_bdev)->limits.max_write_same_sectors)
 			disable_write_same(md);
@@ -871,6 +857,23 @@ static void clone_endio(struct bio *bio)
 			disable_write_zeroes(md);
 	}
 
+	if (endio) {
+		int r = endio(tio->ti, bio, &error);
+		switch (r) {
+		case DM_ENDIO_REQUEUE:
+			error = BLK_STS_DM_REQUEUE;
+			/*FALLTHRU*/
+		case DM_ENDIO_DONE:
+			break;
+		case DM_ENDIO_INCOMPLETE:
+			/* The target will handle the io */
+			return;
+		default:
+			DMWARN("unimplemented target endio return value: %d", r);
+			BUG();
+		}
+	}
+
 	free_tio(tio);
 	dec_pending(io, error);
 }
@@ -1010,6 +1013,85 @@ void dm_accept_partial_bio(struct bio *bio, unsigned n_sectors)
 EXPORT_SYMBOL_GPL(dm_accept_partial_bio);
 
 /*
+ * The zone descriptors obtained with a zone report indicate
+ * zone positions within the target device. The zone descriptors
+ * must be remapped to match their position within the dm device.
+ * A target may call dm_remap_zone_report after completion of a
+ * REQ_OP_ZONE_REPORT bio to remap the zone descriptors obtained
+ * from the target device mapping to the dm device.
+ */
+void dm_remap_zone_report(struct dm_target *ti, struct bio *bio, sector_t start)
+{
+#ifdef CONFIG_BLK_DEV_ZONED
+	struct dm_target_io *tio = container_of(bio, struct dm_target_io, clone);
+	struct bio *report_bio = tio->io->bio;
+	struct blk_zone_report_hdr *hdr = NULL;
+	struct blk_zone *zone;
+	unsigned int nr_rep = 0;
+	unsigned int ofst;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
+	void *addr;
+
+	if (bio->bi_status)
+		return;
+
+	/*
+	 * Remap the start sector of the reported zones. For sequential zones,
+	 * also remap the write pointer position.
+	 */
+	bio_for_each_segment(bvec, report_bio, iter) {
+		addr = kmap_atomic(bvec.bv_page);
+
+		/* Remember the report header in the first page */
+		if (!hdr) {
+			hdr = addr;
+			ofst = sizeof(struct blk_zone_report_hdr);
+		} else
+			ofst = 0;
+
+		/* Set zones start sector */
+		while (hdr->nr_zones && ofst < bvec.bv_len) {
+			zone = addr + ofst;
+			if (zone->start >= start + ti->len) {
+				hdr->nr_zones = 0;
+				break;
+			}
+			zone->start = zone->start + ti->begin - start;
+			if (zone->type != BLK_ZONE_TYPE_CONVENTIONAL) {
+				if (zone->cond == BLK_ZONE_COND_FULL)
+					zone->wp = zone->start + zone->len;
+				else if (zone->cond == BLK_ZONE_COND_EMPTY)
+					zone->wp = zone->start;
+				else
+					zone->wp = zone->wp + ti->begin - start;
+			}
+			ofst += sizeof(struct blk_zone);
+			hdr->nr_zones--;
+			nr_rep++;
+		}
+
+		if (addr != hdr)
+			kunmap_atomic(addr);
+
+		if (!hdr->nr_zones)
+			break;
+	}
+
+	if (hdr) {
+		hdr->nr_zones = nr_rep;
+		kunmap_atomic(hdr);
+	}
+
+	bio_advance(report_bio, report_bio->bi_iter.bi_size);
+
+#else /* !CONFIG_BLK_DEV_ZONED */
+	bio->bi_status = BLK_STS_NOTSUPP;
+#endif
+}
+EXPORT_SYMBOL_GPL(dm_remap_zone_report);
+
+/*
  * Flush current->bio_list when the target map method blocks.
  * This fixes deadlocks in snapshot and possibly in other targets.
  */
@@ -1036,7 +1118,8 @@ static void flush_current_bio_list(struct blk_plug_cb *cb, bool from_schedule)
 
 		while ((bio = bio_list_pop(&list))) {
 			struct bio_set *bs = bio->bi_pool;
-			if (unlikely(!bs) || bs == fs_bio_set) {
+			if (unlikely(!bs) || bs == fs_bio_set ||
+			    !bs->rescue_workqueue) {
 				bio_list_add(&current->bio_list[i], bio);
 				continue;
 			}
@@ -1084,18 +1167,24 @@ static void __map_bio(struct dm_target_io *tio)
 	r = ti->type->map(ti, clone);
 	dm_offload_end(&o);
 
-	if (r == DM_MAPIO_REMAPPED) {
+	switch (r) {
+	case DM_MAPIO_SUBMITTED:
+		break;
+	case DM_MAPIO_REMAPPED:
 		/* the bio has been remapped so dispatch it */
-
 		trace_block_bio_remap(bdev_get_queue(clone->bi_bdev), clone,
 				      tio->io->bio->bi_bdev->bd_dev, sector);
-
 		generic_make_request(clone);
-	} else if (r < 0 || r == DM_MAPIO_REQUEUE) {
-		/* error the io and bail out, or requeue it if needed */
-		dec_pending(tio->io, r);
+		break;
+	case DM_MAPIO_KILL:
+		dec_pending(tio->io, BLK_STS_IOERR);
 		free_tio(tio);
-	} else if (r != DM_MAPIO_SUBMITTED) {
+		break;
+	case DM_MAPIO_REQUEUE:
+		dec_pending(tio->io, BLK_STS_DM_REQUEUE);
+		free_tio(tio);
+		break;
+	default:
 		DMWARN("unimplemented target map return value: %d", r);
 		BUG();
 	}
@@ -1142,7 +1231,8 @@ static int clone_bio(struct dm_target_io *tio, struct bio *bio,
 			return r;
 	}
 
-	bio_advance(clone, to_bytes(sector - clone->bi_iter.bi_sector));
+	if (bio_op(bio) != REQ_OP_ZONE_REPORT)
+		bio_advance(clone, to_bytes(sector - clone->bi_iter.bi_sector));
 	clone->bi_iter.bi_size = to_bytes(len);
 
 	if (unlikely(bio_integrity(bio) != NULL))
@@ -1331,7 +1421,11 @@ static int __split_and_process_non_flush(struct clone_info *ci)
 	if (!dm_target_is_valid(ti))
 		return -EIO;
 
-	len = min_t(sector_t, max_io_len(ci->sector, ti), ci->sector_count);
+	if (bio_op(bio) == REQ_OP_ZONE_REPORT)
+		len = ci->sector_count;
+	else
+		len = min_t(sector_t, max_io_len(ci->sector, ti),
+			    ci->sector_count);
 
 	r = __clone_and_map_data_bio(ci, ti, ci->sector, &len);
 	if (r < 0)
@@ -1360,7 +1454,7 @@ static void __split_and_process_bio(struct mapped_device *md,
 	ci.map = map;
 	ci.md = md;
 	ci.io = alloc_io(md);
-	ci.io->error = 0;
+	ci.io->status = 0;
 	atomic_set(&ci.io->io_count, 1);
 	ci.io->bio = bio;
 	ci.io->md = md;
@@ -1374,6 +1468,10 @@ static void __split_and_process_bio(struct mapped_device *md,
 		ci.sector_count = 0;
 		error = __send_empty_flush(&ci);
 		/* dec_pending submits any data associated with flush */
+	} else if (bio_op(bio) == REQ_OP_ZONE_RESET) {
+		ci.bio = bio;
+		ci.sector_count = 0;
+		error = __split_and_process_non_flush(&ci);
 	} else {
 		ci.bio = bio;
 		ci.sector_count = bio_sectors(bio);
@@ -1527,7 +1625,6 @@ void dm_init_normal_md_queue(struct mapped_device *md)
 	 * Initialize aspects of queue that aren't relevant for blk-mq
 	 */
 	md->queue->backing_dev_info->congested_fn = dm_any_congested;
-	blk_queue_bounce_limit(md->queue, BLK_BOUNCE_ANY);
 }
 
 static void cleanup_mapped_device(struct mapped_device *md)
@@ -1753,7 +1850,9 @@ static void event_callback(void *context)
 	dm_send_uevents(&uevents, &disk_to_dev(md->disk)->kobj);
 
 	atomic_inc(&md->event_nr);
+	atomic_inc(&dm_global_event_nr);
 	wake_up(&md->eventq);
+	wake_up(&dm_global_eventq);
 }
 
 /*
@@ -2654,7 +2753,7 @@ struct dm_md_mempools *dm_alloc_md_mempools(struct mapped_device *md, enum dm_qu
 		BUG();
 	}
 
-	pools->bs = bioset_create_nobvec(pool_size, front_pad);
+	pools->bs = bioset_create(pool_size, front_pad, BIOSET_NEED_RESCUER);
 	if (!pools->bs)
 		goto out;
 
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 87edc342ccb3..31bcbfb09fef 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -185,7 +185,7 @@ static int start_readonly;
 static bool create_on_open = true;
 
 /* bio_clone_mddev
- * like bio_clone, but with a local bio set
+ * like bio_clone_bioset, but with a local bio set
  */
 
 struct bio *bio_alloc_mddev(gfp_t gfp_mask, int nr_iovecs,
@@ -265,7 +265,7 @@ static blk_qc_t md_make_request(struct request_queue *q, struct bio *bio)
 	unsigned int sectors;
 	int cpu;
 
-	blk_queue_split(q, &bio, q->bio_split);
+	blk_queue_split(q, &bio);
 
 	if (mddev == NULL || mddev->pers == NULL) {
 		bio_io_error(bio);
@@ -273,7 +273,7 @@ static blk_qc_t md_make_request(struct request_queue *q, struct bio *bio)
 	}
 	if (mddev->ro == 1 && unlikely(rw == WRITE)) {
 		if (bio_sectors(bio) != 0)
-			bio->bi_error = -EROFS;
+			bio->bi_status = BLK_STS_IOERR;
 		bio_endio(bio);
 		return BLK_QC_T_NONE;
 	}
@@ -719,8 +719,8 @@ static void super_written(struct bio *bio)
 	struct md_rdev *rdev = bio->bi_private;
 	struct mddev *mddev = rdev->mddev;
 
-	if (bio->bi_error) {
-		pr_err("md: super_written gets error=%d\n", bio->bi_error);
+	if (bio->bi_status) {
+		pr_err("md: super_written gets error=%d\n", bio->bi_status);
 		md_error(mddev, rdev);
 		if (!test_bit(Faulty, &rdev->flags)
 		    && (bio->bi_opf & MD_FAILFAST)) {
@@ -801,7 +801,7 @@ int sync_page_io(struct md_rdev *rdev, sector_t sector, int size,
 
 	submit_bio_wait(bio);
 
-	ret = !bio->bi_error;
+	ret = !bio->bi_status;
 	bio_put(bio);
 	return ret;
 }
@@ -825,7 +825,7 @@ fail:
 	return -EINVAL;
 }
 
-static int uuid_equal(mdp_super_t *sb1, mdp_super_t *sb2)
+static int md_uuid_equal(mdp_super_t *sb1, mdp_super_t *sb2)
 {
 	return	sb1->set_uuid0 == sb2->set_uuid0 &&
 		sb1->set_uuid1 == sb2->set_uuid1 &&
@@ -833,7 +833,7 @@ static int uuid_equal(mdp_super_t *sb1, mdp_super_t *sb2)
 		sb1->set_uuid3 == sb2->set_uuid3;
 }
 
-static int sb_equal(mdp_super_t *sb1, mdp_super_t *sb2)
+static int md_sb_equal(mdp_super_t *sb1, mdp_super_t *sb2)
 {
 	int ret;
 	mdp_super_t *tmp1, *tmp2;
@@ -1025,12 +1025,12 @@ static int super_90_load(struct md_rdev *rdev, struct md_rdev *refdev, int minor
 	} else {
 		__u64 ev1, ev2;
 		mdp_super_t *refsb = page_address(refdev->sb_page);
-		if (!uuid_equal(refsb, sb)) {
+		if (!md_uuid_equal(refsb, sb)) {
 			pr_warn("md: %s has different UUID to %s\n",
 				b, bdevname(refdev->bdev,b2));
 			goto abort;
 		}
-		if (!sb_equal(refsb, sb)) {
+		if (!md_sb_equal(refsb, sb)) {
 			pr_warn("md: %s has same UUID but different superblock to %s\n",
 				b, bdevname(refdev->bdev, b2));
 			goto abort;
@@ -5428,7 +5428,7 @@ int md_run(struct mddev *mddev)
 	}
 
 	if (mddev->bio_set == NULL) {
-		mddev->bio_set = bioset_create(BIO_POOL_SIZE, 0);
+		mddev->bio_set = bioset_create(BIO_POOL_SIZE, 0, BIOSET_NEED_BVECS);
 		if (!mddev->bio_set)
 			return -ENOMEM;
 	}
diff --git a/drivers/md/multipath.c b/drivers/md/multipath.c
index e95d521d93e9..68d036e64041 100644
--- a/drivers/md/multipath.c
+++ b/drivers/md/multipath.c
@@ -73,12 +73,12 @@ static void multipath_reschedule_retry (struct multipath_bh *mp_bh)
  * operation and are ready to return a success/failure code to the buffer
  * cache layer.
  */
-static void multipath_end_bh_io (struct multipath_bh *mp_bh, int err)
+static void multipath_end_bh_io(struct multipath_bh *mp_bh, blk_status_t status)
 {
 	struct bio *bio = mp_bh->master_bio;
 	struct mpconf *conf = mp_bh->mddev->private;
 
-	bio->bi_error = err;
+	bio->bi_status = status;
 	bio_endio(bio);
 	mempool_free(mp_bh, conf->pool);
 }
@@ -89,7 +89,7 @@ static void multipath_end_request(struct bio *bio)
 	struct mpconf *conf = mp_bh->mddev->private;
 	struct md_rdev *rdev = conf->multipaths[mp_bh->path].rdev;
 
-	if (!bio->bi_error)
+	if (!bio->bi_status)
 		multipath_end_bh_io(mp_bh, 0);
 	else if (!(bio->bi_opf & REQ_RAHEAD)) {
 		/*
@@ -102,7 +102,7 @@ static void multipath_end_request(struct bio *bio)
 			(unsigned long long)bio->bi_iter.bi_sector);
 		multipath_reschedule_retry(mp_bh);
 	} else
-		multipath_end_bh_io(mp_bh, bio->bi_error);
+		multipath_end_bh_io(mp_bh, bio->bi_status);
 	rdev_dec_pending(rdev, conf->mddev);
 }
 
@@ -347,7 +347,7 @@ static void multipathd(struct md_thread *thread)
 			pr_err("multipath: %s: unrecoverable IO read error for block %llu\n",
 			       bdevname(bio->bi_bdev,b),
 			       (unsigned long long)bio->bi_iter.bi_sector);
-			multipath_end_bh_io(mp_bh, -EIO);
+			multipath_end_bh_io(mp_bh, BLK_STS_IOERR);
 		} else {
 			pr_err("multipath: %s: redirecting sector %llu to another IO path\n",
 			       bdevname(bio->bi_bdev,b),
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index e1a7e3d4c5e4..98ca2c1d3226 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -277,7 +277,7 @@ static void call_bio_endio(struct r1bio *r1_bio)
 	struct r1conf *conf = r1_bio->mddev->private;
 
 	if (!test_bit(R1BIO_Uptodate, &r1_bio->state))
-		bio->bi_error = -EIO;
+		bio->bi_status = BLK_STS_IOERR;
 
 	bio_endio(bio);
 	/*
@@ -335,7 +335,7 @@ static int find_bio_disk(struct r1bio *r1_bio, struct bio *bio)
 
 static void raid1_end_read_request(struct bio *bio)
 {
-	int uptodate = !bio->bi_error;
+	int uptodate = !bio->bi_status;
 	struct r1bio *r1_bio = bio->bi_private;
 	struct r1conf *conf = r1_bio->mddev->private;
 	struct md_rdev *rdev = conf->mirrors[r1_bio->read_disk].rdev;
@@ -426,12 +426,12 @@ static void raid1_end_write_request(struct bio *bio)
 	struct md_rdev *rdev = conf->mirrors[mirror].rdev;
 	bool discard_error;
 
-	discard_error = bio->bi_error && bio_op(bio) == REQ_OP_DISCARD;
+	discard_error = bio->bi_status && bio_op(bio) == REQ_OP_DISCARD;
 
 	/*
 	 * 'one mirror IO has finished' event handler:
 	 */
-	if (bio->bi_error && !discard_error) {
+	if (bio->bi_status && !discard_error) {
 		set_bit(WriteErrorSeen,	&rdev->flags);
 		if (!test_and_set_bit(WantReplacement, &rdev->flags))
 			set_bit(MD_RECOVERY_NEEDED, &
@@ -802,7 +802,7 @@ static void flush_bio_list(struct r1conf *conf, struct bio *bio)
 		bio->bi_next = NULL;
 		bio->bi_bdev = rdev->bdev;
 		if (test_bit(Faulty, &rdev->flags)) {
-			bio->bi_error = -EIO;
+			bio->bi_status = BLK_STS_IOERR;
 			bio_endio(bio);
 		} else if (unlikely((bio_op(bio) == REQ_OP_DISCARD) &&
 				    !blk_queue_discard(bdev_get_queue(bio->bi_bdev))))
@@ -1856,7 +1856,7 @@ static void end_sync_read(struct bio *bio)
 	 * or re-read if the read failed.
 	 * We don't do much here, just schedule handling by raid1d
 	 */
-	if (!bio->bi_error)
+	if (!bio->bi_status)
 		set_bit(R1BIO_Uptodate, &r1_bio->state);
 
 	if (atomic_dec_and_test(&r1_bio->remaining))
@@ -1865,7 +1865,7 @@ static void end_sync_read(struct bio *bio)
 
 static void end_sync_write(struct bio *bio)
 {
-	int uptodate = !bio->bi_error;
+	int uptodate = !bio->bi_status;
 	struct r1bio *r1_bio = get_resync_r1bio(bio);
 	struct mddev *mddev = r1_bio->mddev;
 	struct r1conf *conf = mddev->private;
@@ -2058,7 +2058,7 @@ static int fix_sync_read_error(struct r1bio *r1_bio)
 		idx ++;
 	}
 	set_bit(R1BIO_Uptodate, &r1_bio->state);
-	bio->bi_error = 0;
+	bio->bi_status = 0;
 	return 1;
 }
 
@@ -2082,16 +2082,16 @@ static void process_checks(struct r1bio *r1_bio)
 	for (i = 0; i < conf->raid_disks * 2; i++) {
 		int j;
 		int size;
-		int error;
+		blk_status_t status;
 		struct bio_vec *bi;
 		struct bio *b = r1_bio->bios[i];
 		struct resync_pages *rp = get_resync_pages(b);
 		if (b->bi_end_io != end_sync_read)
 			continue;
 		/* fixup the bio for reuse, but preserve errno */
-		error = b->bi_error;
+		status = b->bi_status;
 		bio_reset(b);
-		b->bi_error = error;
+		b->bi_status = status;
 		b->bi_vcnt = vcnt;
 		b->bi_iter.bi_size = r1_bio->sectors << 9;
 		b->bi_iter.bi_sector = r1_bio->sector +
@@ -2113,7 +2113,7 @@ static void process_checks(struct r1bio *r1_bio)
 	}
 	for (primary = 0; primary < conf->raid_disks * 2; primary++)
 		if (r1_bio->bios[primary]->bi_end_io == end_sync_read &&
-		    !r1_bio->bios[primary]->bi_error) {
+		    !r1_bio->bios[primary]->bi_status) {
 			r1_bio->bios[primary]->bi_end_io = NULL;
 			rdev_dec_pending(conf->mirrors[primary].rdev, mddev);
 			break;
@@ -2123,7 +2123,7 @@ static void process_checks(struct r1bio *r1_bio)
 		int j;
 		struct bio *pbio = r1_bio->bios[primary];
 		struct bio *sbio = r1_bio->bios[i];
-		int error = sbio->bi_error;
+		blk_status_t status = sbio->bi_status;
 		struct page **ppages = get_resync_pages(pbio)->pages;
 		struct page **spages = get_resync_pages(sbio)->pages;
 		struct bio_vec *bi;
@@ -2132,12 +2132,12 @@ static void process_checks(struct r1bio *r1_bio)
 		if (sbio->bi_end_io != end_sync_read)
 			continue;
 		/* Now we can 'fixup' the error value */
-		sbio->bi_error = 0;
+		sbio->bi_status = 0;
 
 		bio_for_each_segment_all(bi, sbio, j)
 			page_len[j] = bi->bv_len;
 
-		if (!error) {
+		if (!status) {
 			for (j = vcnt; j-- ; ) {
 				if (memcmp(page_address(ppages[j]),
 					   page_address(spages[j]),
@@ -2149,7 +2149,7 @@ static void process_checks(struct r1bio *r1_bio)
 		if (j >= 0)
 			atomic64_add(r1_bio->sectors, &mddev->resync_mismatches);
 		if (j < 0 || (test_bit(MD_RECOVERY_CHECK, &mddev->recovery)
-			      && !error)) {
+			      && !status)) {
 			/* No need to write to this device. */
 			sbio->bi_end_io = NULL;
 			rdev_dec_pending(conf->mirrors[i].rdev, mddev);
@@ -2400,11 +2400,11 @@ static void handle_sync_write_finished(struct r1conf *conf, struct r1bio *r1_bio
 		struct bio *bio = r1_bio->bios[m];
 		if (bio->bi_end_io == NULL)
 			continue;
-		if (!bio->bi_error &&
+		if (!bio->bi_status &&
 		    test_bit(R1BIO_MadeGood, &r1_bio->state)) {
 			rdev_clear_badblocks(rdev, r1_bio->sector, s, 0);
 		}
-		if (bio->bi_error &&
+		if (bio->bi_status &&
 		    test_bit(R1BIO_WriteError, &r1_bio->state)) {
 			if (!rdev_set_badblocks(rdev, r1_bio->sector, s, 0))
 				md_error(conf->mddev, rdev);
@@ -2955,7 +2955,7 @@ static struct r1conf *setup_conf(struct mddev *mddev)
 	if (!conf->r1bio_pool)
 		goto abort;
 
-	conf->bio_split = bioset_create(BIO_POOL_SIZE, 0);
+	conf->bio_split = bioset_create(BIO_POOL_SIZE, 0, 0);
 	if (!conf->bio_split)
 		goto abort;
 
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index 797ed60abd5e..57a250fdbbcc 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -336,7 +336,7 @@ static void raid_end_bio_io(struct r10bio *r10_bio)
 	struct r10conf *conf = r10_bio->mddev->private;
 
 	if (!test_bit(R10BIO_Uptodate, &r10_bio->state))
-		bio->bi_error = -EIO;
+		bio->bi_status = BLK_STS_IOERR;
 
 	bio_endio(bio);
 	/*
@@ -389,7 +389,7 @@ static int find_bio_disk(struct r10conf *conf, struct r10bio *r10_bio,
 
 static void raid10_end_read_request(struct bio *bio)
 {
-	int uptodate = !bio->bi_error;
+	int uptodate = !bio->bi_status;
 	struct r10bio *r10_bio = bio->bi_private;
 	int slot, dev;
 	struct md_rdev *rdev;
@@ -477,7 +477,7 @@ static void raid10_end_write_request(struct bio *bio)
 	struct bio *to_put = NULL;
 	bool discard_error;
 
-	discard_error = bio->bi_error && bio_op(bio) == REQ_OP_DISCARD;
+	discard_error = bio->bi_status && bio_op(bio) == REQ_OP_DISCARD;
 
 	dev = find_bio_disk(conf, r10_bio, bio, &slot, &repl);
 
@@ -491,7 +491,7 @@ static void raid10_end_write_request(struct bio *bio)
 	/*
 	 * this branch is our 'one mirror IO has finished' event handler:
 	 */
-	if (bio->bi_error && !discard_error) {
+	if (bio->bi_status && !discard_error) {
 		if (repl)
 			/* Never record new bad blocks to replacement,
 			 * just fail it.
@@ -913,7 +913,7 @@ static void flush_pending_writes(struct r10conf *conf)
 			bio->bi_next = NULL;
 			bio->bi_bdev = rdev->bdev;
 			if (test_bit(Faulty, &rdev->flags)) {
-				bio->bi_error = -EIO;
+				bio->bi_status = BLK_STS_IOERR;
 				bio_endio(bio);
 			} else if (unlikely((bio_op(bio) ==  REQ_OP_DISCARD) &&
 					    !blk_queue_discard(bdev_get_queue(bio->bi_bdev))))
@@ -1098,7 +1098,7 @@ static void raid10_unplug(struct blk_plug_cb *cb, bool from_schedule)
 		bio->bi_next = NULL;
 		bio->bi_bdev = rdev->bdev;
 		if (test_bit(Faulty, &rdev->flags)) {
-			bio->bi_error = -EIO;
+			bio->bi_status = BLK_STS_IOERR;
 			bio_endio(bio);
 		} else if (unlikely((bio_op(bio) ==  REQ_OP_DISCARD) &&
 				    !blk_queue_discard(bdev_get_queue(bio->bi_bdev))))
@@ -1888,7 +1888,7 @@ static void __end_sync_read(struct r10bio *r10_bio, struct bio *bio, int d)
 {
 	struct r10conf *conf = r10_bio->mddev->private;
 
-	if (!bio->bi_error)
+	if (!bio->bi_status)
 		set_bit(R10BIO_Uptodate, &r10_bio->state);
 	else
 		/* The write handler will notice the lack of
@@ -1972,7 +1972,7 @@ static void end_sync_write(struct bio *bio)
 	else
 		rdev = conf->mirrors[d].rdev;
 
-	if (bio->bi_error) {
+	if (bio->bi_status) {
 		if (repl)
 			md_error(mddev, rdev);
 		else {
@@ -2021,7 +2021,7 @@ static void sync_request_write(struct mddev *mddev, struct r10bio *r10_bio)
 
 	/* find the first device with a block */
 	for (i=0; i<conf->copies; i++)
-		if (!r10_bio->devs[i].bio->bi_error)
+		if (!r10_bio->devs[i].bio->bi_status)
 			break;
 
 	if (i == conf->copies)
@@ -2050,7 +2050,7 @@ static void sync_request_write(struct mddev *mddev, struct r10bio *r10_bio)
 		tpages = get_resync_pages(tbio)->pages;
 		d = r10_bio->devs[i].devnum;
 		rdev = conf->mirrors[d].rdev;
-		if (!r10_bio->devs[i].bio->bi_error) {
+		if (!r10_bio->devs[i].bio->bi_status) {
 			/* We know that the bi_io_vec layout is the same for
 			 * both 'first' and 'i', so we just compare them.
 			 * All vec entries are PAGE_SIZE;
@@ -2633,7 +2633,7 @@ static void handle_write_completed(struct r10conf *conf, struct r10bio *r10_bio)
 			rdev = conf->mirrors[dev].rdev;
 			if (r10_bio->devs[m].bio == NULL)
 				continue;
-			if (!r10_bio->devs[m].bio->bi_error) {
+			if (!r10_bio->devs[m].bio->bi_status) {
 				rdev_clear_badblocks(
 					rdev,
 					r10_bio->devs[m].addr,
@@ -2649,7 +2649,7 @@ static void handle_write_completed(struct r10conf *conf, struct r10bio *r10_bio)
 			if (r10_bio->devs[m].repl_bio == NULL)
 				continue;
 
-			if (!r10_bio->devs[m].repl_bio->bi_error) {
+			if (!r10_bio->devs[m].repl_bio->bi_status) {
 				rdev_clear_badblocks(
 					rdev,
 					r10_bio->devs[m].addr,
@@ -2675,7 +2675,7 @@ static void handle_write_completed(struct r10conf *conf, struct r10bio *r10_bio)
 					r10_bio->devs[m].addr,
 					r10_bio->sectors, 0);
 				rdev_dec_pending(rdev, conf->mddev);
-			} else if (bio != NULL && bio->bi_error) {
+			} else if (bio != NULL && bio->bi_status) {
 				fail = true;
 				if (!narrow_write_error(r10_bio, m)) {
 					md_error(conf->mddev, rdev);
@@ -3267,7 +3267,7 @@ static sector_t raid10_sync_request(struct mddev *mddev, sector_t sector_nr,
 				r10_bio->devs[i].repl_bio->bi_end_io = NULL;
 
 			bio = r10_bio->devs[i].bio;
-			bio->bi_error = -EIO;
+			bio->bi_status = BLK_STS_IOERR;
 			rcu_read_lock();
 			rdev = rcu_dereference(conf->mirrors[d].rdev);
 			if (rdev == NULL || test_bit(Faulty, &rdev->flags)) {
@@ -3309,7 +3309,7 @@ static sector_t raid10_sync_request(struct mddev *mddev, sector_t sector_nr,
 
 			/* Need to set up for writing to the replacement */
 			bio = r10_bio->devs[i].repl_bio;
-			bio->bi_error = -EIO;
+			bio->bi_status = BLK_STS_IOERR;
 
 			sector = r10_bio->devs[i].addr;
 			bio->bi_next = biolist;
@@ -3375,7 +3375,7 @@ static sector_t raid10_sync_request(struct mddev *mddev, sector_t sector_nr,
 
 		if (bio->bi_end_io == end_sync_read) {
 			md_sync_acct(bio->bi_bdev, nr_sectors);
-			bio->bi_error = 0;
+			bio->bi_status = 0;
 			generic_make_request(bio);
 		}
 	}
@@ -3552,7 +3552,7 @@ static struct r10conf *setup_conf(struct mddev *mddev)
 	if (!conf->r10bio_pool)
 		goto out;
 
-	conf->bio_split = bioset_create(BIO_POOL_SIZE, 0);
+	conf->bio_split = bioset_create(BIO_POOL_SIZE, 0, 0);
 	if (!conf->bio_split)
 		goto out;
 
@@ -4397,7 +4397,7 @@ read_more:
 	read_bio->bi_end_io = end_reshape_read;
 	bio_set_op_attrs(read_bio, REQ_OP_READ, 0);
 	read_bio->bi_flags &= (~0UL << BIO_RESET_BITS);
-	read_bio->bi_error = 0;
+	read_bio->bi_status = 0;
 	read_bio->bi_vcnt = 0;
 	read_bio->bi_iter.bi_size = 0;
 	r10_bio->master_bio = read_bio;
@@ -4641,7 +4641,7 @@ static void end_reshape_write(struct bio *bio)
 		rdev = conf->mirrors[d].rdev;
 	}
 
-	if (bio->bi_error) {
+	if (bio->bi_status) {
 		/* FIXME should record badblock */
 		md_error(mddev, rdev);
 	}
diff --git a/drivers/md/raid5-cache.c b/drivers/md/raid5-cache.c
index 0a7af8b0a80a..bfa1e907c472 100644
--- a/drivers/md/raid5-cache.c
+++ b/drivers/md/raid5-cache.c
@@ -572,7 +572,7 @@ static void r5l_log_endio(struct bio *bio)
 	struct r5l_log *log = io->log;
 	unsigned long flags;
 
-	if (bio->bi_error)
+	if (bio->bi_status)
 		md_error(log->rdev->mddev, log->rdev);
 
 	bio_put(bio);
@@ -1247,7 +1247,7 @@ static void r5l_log_flush_endio(struct bio *bio)
 	unsigned long flags;
 	struct r5l_io_unit *io;
 
-	if (bio->bi_error)
+	if (bio->bi_status)
 		md_error(log->rdev->mddev, log->rdev);
 
 	spin_lock_irqsave(&log->io_list_lock, flags);
@@ -3063,7 +3063,7 @@ int r5l_init_log(struct r5conf *conf, struct md_rdev *rdev)
 	if (!log->io_pool)
 		goto io_pool;
 
-	log->bs = bioset_create(R5L_POOL_SIZE, 0);
+	log->bs = bioset_create(R5L_POOL_SIZE, 0, BIOSET_NEED_BVECS);
 	if (!log->bs)
 		goto io_bs;
 
diff --git a/drivers/md/raid5-ppl.c b/drivers/md/raid5-ppl.c
index ccce92e68d7f..77cce3573aa8 100644
--- a/drivers/md/raid5-ppl.c
+++ b/drivers/md/raid5-ppl.c
@@ -397,7 +397,7 @@ static void ppl_log_endio(struct bio *bio)
 
 	pr_debug("%s: seq: %llu\n", __func__, io->seq);
 
-	if (bio->bi_error)
+	if (bio->bi_status)
 		md_error(ppl_conf->mddev, log->rdev);
 
 	list_for_each_entry_safe(sh, next, &io->stripe_list, log_list) {
@@ -1150,7 +1150,7 @@ int ppl_init_log(struct r5conf *conf)
 		goto err;
 	}
 
-	ppl_conf->bs = bioset_create(conf->raid_disks, 0);
+	ppl_conf->bs = bioset_create(conf->raid_disks, 0, 0);
 	if (!ppl_conf->bs) {
 		ret = -ENOMEM;
 		goto err;
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index ec0f951ae19f..62c965be97e1 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -2476,7 +2476,7 @@ static void raid5_end_read_request(struct bio * bi)
 
 	pr_debug("end_read_request %llu/%d, count: %d, error %d.\n",
 		(unsigned long long)sh->sector, i, atomic_read(&sh->count),
-		bi->bi_error);
+		bi->bi_status);
 	if (i == disks) {
 		bio_reset(bi);
 		BUG();
@@ -2496,7 +2496,7 @@ static void raid5_end_read_request(struct bio * bi)
 		s = sh->sector + rdev->new_data_offset;
 	else
 		s = sh->sector + rdev->data_offset;
-	if (!bi->bi_error) {
+	if (!bi->bi_status) {
 		set_bit(R5_UPTODATE, &sh->dev[i].flags);
 		if (test_bit(R5_ReadError, &sh->dev[i].flags)) {
 			/* Note that this cannot happen on a
@@ -2613,7 +2613,7 @@ static void raid5_end_write_request(struct bio *bi)
 	}
 	pr_debug("end_write_request %llu/%d, count %d, error: %d.\n",
 		(unsigned long long)sh->sector, i, atomic_read(&sh->count),
-		bi->bi_error);
+		bi->bi_status);
 	if (i == disks) {
 		bio_reset(bi);
 		BUG();
@@ -2621,14 +2621,14 @@ static void raid5_end_write_request(struct bio *bi)
 	}
 
 	if (replacement) {
-		if (bi->bi_error)
+		if (bi->bi_status)
 			md_error(conf->mddev, rdev);
 		else if (is_badblock(rdev, sh->sector,
 				     STRIPE_SECTORS,
 				     &first_bad, &bad_sectors))
 			set_bit(R5_MadeGoodRepl, &sh->dev[i].flags);
 	} else {
-		if (bi->bi_error) {
+		if (bi->bi_status) {
 			set_bit(STRIPE_DEGRADED, &sh->state);
 			set_bit(WriteErrorSeen, &rdev->flags);
 			set_bit(R5_WriteError, &sh->dev[i].flags);
@@ -2649,7 +2649,7 @@ static void raid5_end_write_request(struct bio *bi)
 	}
 	rdev_dec_pending(rdev, conf->mddev);
 
-	if (sh->batch_head && bi->bi_error && !replacement)
+	if (sh->batch_head && bi->bi_status && !replacement)
 		set_bit(STRIPE_BATCH_ERR, &sh->batch_head->state);
 
 	bio_reset(bi);
@@ -3381,7 +3381,7 @@ 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);
 
-			bi->bi_error = -EIO;
+			bi->bi_status = BLK_STS_IOERR;
 			md_write_end(conf->mddev);
 			bio_endio(bi);
 			bi = nextbi;
@@ -3403,7 +3403,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);
 
-			bi->bi_error = -EIO;
+			bi->bi_status = BLK_STS_IOERR;
 			md_write_end(conf->mddev);
 			bio_endio(bi);
 			bi = bi2;
@@ -3429,7 +3429,7 @@ handle_failed_stripe(struct r5conf *conf, struct stripe_head *sh,
 				struct bio *nextbi =
 					r5_next_bio(bi, sh->dev[i].sector);
 
-				bi->bi_error = -EIO;
+				bi->bi_status = BLK_STS_IOERR;
 				bio_endio(bi);
 				bi = nextbi;
 			}
@@ -5154,7 +5154,7 @@ static void raid5_align_endio(struct bio *bi)
 	struct mddev *mddev;
 	struct r5conf *conf;
 	struct md_rdev *rdev;
-	int error = bi->bi_error;
+	blk_status_t error = bi->bi_status;
 
 	bio_put(bi);
 
@@ -5731,7 +5731,7 @@ static void raid5_make_request(struct mddev *mddev, struct bio * bi)
 			release_stripe_plug(mddev, sh);
 		} else {
 			/* cannot get stripe for read-ahead, just give-up */
-			bi->bi_error = -EIO;
+			bi->bi_status = BLK_STS_IOERR;
 			break;
 		}
 	}
@@ -6943,7 +6943,7 @@ static struct r5conf *setup_conf(struct mddev *mddev)
 			goto abort;
 	}
 
-	conf->bio_split = bioset_create(BIO_POOL_SIZE, 0);
+	conf->bio_split = bioset_create(BIO_POOL_SIZE, 0, 0);
 	if (!conf->bio_split)
 		goto abort;
 	conf->mddev = mddev;
diff --git a/drivers/media/cec/Kconfig b/drivers/media/cec/Kconfig
index 4e25a950ae6f..43428cec3a01 100644
--- a/drivers/media/cec/Kconfig
+++ b/drivers/media/cec/Kconfig
@@ -1,5 +1,6 @@
 config MEDIA_CEC_RC
 	bool "HDMI CEC RC integration"
 	depends on CEC_CORE && RC_CORE
+	depends on CEC_CORE=m || RC_CORE=y
 	---help---
 	  Pass on CEC remote control messages to the RC framework.
diff --git a/drivers/media/cec/cec-adap.c b/drivers/media/cec/cec-adap.c
index 9dfc79800c71..bf45977b2823 100644
--- a/drivers/media/cec/cec-adap.c
+++ b/drivers/media/cec/cec-adap.c
@@ -28,6 +28,8 @@
 #include <linux/string.h>
 #include <linux/types.h>
 
+#include <drm/drm_edid.h>
+
 #include "cec-priv.h"
 
 static void cec_fill_msg_report_features(struct cec_adapter *adap,
@@ -366,6 +368,8 @@ int cec_thread_func(void *_adap)
 			 * transmit should be canceled.
 			 */
 			err = wait_event_interruptible_timeout(adap->kthread_waitq,
+				(adap->needs_hpd &&
+				 (!adap->is_configured && !adap->is_configuring)) ||
 				kthread_should_stop() ||
 				(!adap->transmitting &&
 				 !list_empty(&adap->transmit_queue)),
@@ -381,7 +385,9 @@ int cec_thread_func(void *_adap)
 
 		mutex_lock(&adap->lock);
 
-		if (kthread_should_stop()) {
+		if ((adap->needs_hpd &&
+		     (!adap->is_configured && !adap->is_configuring)) ||
+		    kthread_should_stop()) {
 			cec_flush(adap);
 			goto unlock;
 		}
@@ -392,7 +398,7 @@ int cec_thread_func(void *_adap)
 			 * happen and is an indication of a faulty CEC adapter
 			 * driver, or the CEC bus is in some weird state.
 			 */
-			dprintk(0, "message %*ph timed out!\n",
+			dprintk(0, "%s: message %*ph timed out!\n", __func__,
 				adap->transmitting->msg.len,
 				adap->transmitting->msg.msg);
 			/* Just give up on this. */
@@ -468,7 +474,7 @@ void cec_transmit_done(struct cec_adapter *adap, u8 status, u8 arb_lost_cnt,
 	struct cec_msg *msg;
 	u64 ts = ktime_get_ns();
 
-	dprintk(2, "cec_transmit_done %02x\n", status);
+	dprintk(2, "%s: status %02x\n", __func__, status);
 	mutex_lock(&adap->lock);
 	data = adap->transmitting;
 	if (!data) {
@@ -477,7 +483,8 @@ void cec_transmit_done(struct cec_adapter *adap, u8 status, u8 arb_lost_cnt,
 		 * unplugged while the transmit is ongoing. Ignore this
 		 * transmit in that case.
 		 */
-		dprintk(1, "cec_transmit_done without an ongoing transmit!\n");
+		dprintk(1, "%s was called without an ongoing transmit!\n",
+			__func__);
 		goto unlock;
 	}
 
@@ -504,6 +511,12 @@ void cec_transmit_done(struct cec_adapter *adap, u8 status, u8 arb_lost_cnt,
 	    !(status & (CEC_TX_STATUS_MAX_RETRIES | CEC_TX_STATUS_OK))) {
 		/* Retry this message */
 		data->attempts--;
+		if (msg->timeout)
+			dprintk(2, "retransmit: %*ph (attempts: %d, wait for 0x%02x)\n",
+				msg->len, msg->msg, data->attempts, msg->reply);
+		else
+			dprintk(2, "retransmit: %*ph (attempts: %d)\n",
+				msg->len, msg->msg, data->attempts);
 		/* Add the message in front of the transmit queue */
 		list_add(&data->list, &adap->transmit_queue);
 		adap->transmit_queue_sz++;
@@ -544,6 +557,32 @@ unlock:
 }
 EXPORT_SYMBOL_GPL(cec_transmit_done);
 
+void cec_transmit_attempt_done(struct cec_adapter *adap, u8 status)
+{
+	switch (status) {
+	case CEC_TX_STATUS_OK:
+		cec_transmit_done(adap, status, 0, 0, 0, 0);
+		return;
+	case CEC_TX_STATUS_ARB_LOST:
+		cec_transmit_done(adap, status, 1, 0, 0, 0);
+		return;
+	case CEC_TX_STATUS_NACK:
+		cec_transmit_done(adap, status, 0, 1, 0, 0);
+		return;
+	case CEC_TX_STATUS_LOW_DRIVE:
+		cec_transmit_done(adap, status, 0, 0, 1, 0);
+		return;
+	case CEC_TX_STATUS_ERROR:
+		cec_transmit_done(adap, status, 0, 0, 0, 1);
+		return;
+	default:
+		/* Should never happen */
+		WARN(1, "cec-%s: invalid status 0x%02x\n", adap->name, status);
+		return;
+	}
+}
+EXPORT_SYMBOL_GPL(cec_transmit_attempt_done);
+
 /*
  * Called when waiting for a reply times out.
  */
@@ -647,7 +686,7 @@ int cec_transmit_msg_fh(struct cec_adapter *adap, struct cec_msg *msg,
 		return -EINVAL;
 	}
 	if (!adap->is_configured && !adap->is_configuring) {
-		if (msg->msg[0] != 0xf0) {
+		if (adap->needs_hpd || msg->msg[0] != 0xf0) {
 			dprintk(1, "%s: adapter is unconfigured\n", __func__);
 			return -ENONET;
 		}
@@ -911,7 +950,7 @@ void cec_received_msg(struct cec_adapter *adap, struct cec_msg *msg)
 	memset(msg->msg + msg->len, 0, sizeof(msg->msg) - msg->len);
 
 	mutex_lock(&adap->lock);
-	dprintk(2, "cec_received_msg: %*ph\n", msg->len, msg->msg);
+	dprintk(2, "%s: %*ph\n", __func__, msg->len, msg->msg);
 
 	/* Check if this message was for us (directed or broadcast). */
 	if (!cec_msg_is_broadcast(msg))
@@ -1112,9 +1151,6 @@ static int cec_config_log_addr(struct cec_adapter *adap,
 	las->log_addr[idx] = log_addr;
 	las->log_addr_mask |= 1 << log_addr;
 	adap->phys_addrs[log_addr] = adap->phys_addr;
-
-	dprintk(2, "claimed addr %d (%d)\n", log_addr,
-		las->primary_device_type[idx]);
 	return 1;
 }
 
@@ -1126,7 +1162,9 @@ static int cec_config_log_addr(struct cec_adapter *adap,
  */
 static void cec_adap_unconfigure(struct cec_adapter *adap)
 {
-	WARN_ON(adap->ops->adap_log_addr(adap, CEC_LOG_ADDR_INVALID));
+	if (!adap->needs_hpd ||
+	    adap->phys_addr != CEC_PHYS_ADDR_INVALID)
+		WARN_ON(adap->ops->adap_log_addr(adap, CEC_LOG_ADDR_INVALID));
 	adap->log_addrs.log_addr_mask = 0;
 	adap->is_configuring = false;
 	adap->is_configured = false;
@@ -1300,7 +1338,7 @@ configured:
 		/* Report Physical Address */
 		cec_msg_report_physical_addr(&msg, adap->phys_addr,
 					     las->primary_device_type[i]);
-		dprintk(2, "config: la %d pa %x.%x.%x.%x\n",
+		dprintk(1, "config: la %d pa %x.%x.%x.%x\n",
 			las->log_addr[i],
 			cec_phys_addr_exp(adap->phys_addr));
 		cec_transmit_msg_fh(adap, &msg, NULL, false);
@@ -1355,6 +1393,8 @@ void __cec_s_phys_addr(struct cec_adapter *adap, u16 phys_addr, bool block)
 	if (phys_addr == adap->phys_addr || adap->devnode.unregistered)
 		return;
 
+	dprintk(1, "new physical address %x.%x.%x.%x\n",
+		cec_phys_addr_exp(phys_addr));
 	if (phys_addr == CEC_PHYS_ADDR_INVALID ||
 	    adap->phys_addr != CEC_PHYS_ADDR_INVALID) {
 		adap->phys_addr = CEC_PHYS_ADDR_INVALID;
@@ -1364,7 +1404,7 @@ void __cec_s_phys_addr(struct cec_adapter *adap, u16 phys_addr, bool block)
 		if (adap->monitor_all_cnt)
 			WARN_ON(call_op(adap, adap_monitor_all_enable, false));
 		mutex_lock(&adap->devnode.lock);
-		if (list_empty(&adap->devnode.fhs))
+		if (adap->needs_hpd || list_empty(&adap->devnode.fhs))
 			WARN_ON(adap->ops->adap_enable(adap, false));
 		mutex_unlock(&adap->devnode.lock);
 		if (phys_addr == CEC_PHYS_ADDR_INVALID)
@@ -1372,7 +1412,7 @@ void __cec_s_phys_addr(struct cec_adapter *adap, u16 phys_addr, bool block)
 	}
 
 	mutex_lock(&adap->devnode.lock);
-	if (list_empty(&adap->devnode.fhs) &&
+	if ((adap->needs_hpd || list_empty(&adap->devnode.fhs)) &&
 	    adap->ops->adap_enable(adap, true)) {
 		mutex_unlock(&adap->devnode.lock);
 		return;
@@ -1380,7 +1420,7 @@ void __cec_s_phys_addr(struct cec_adapter *adap, u16 phys_addr, bool block)
 
 	if (adap->monitor_all_cnt &&
 	    call_op(adap, adap_monitor_all_enable, true)) {
-		if (list_empty(&adap->devnode.fhs))
+		if (adap->needs_hpd || list_empty(&adap->devnode.fhs))
 			WARN_ON(adap->ops->adap_enable(adap, false));
 		mutex_unlock(&adap->devnode.lock);
 		return;
@@ -1404,6 +1444,18 @@ void cec_s_phys_addr(struct cec_adapter *adap, u16 phys_addr, bool block)
 }
 EXPORT_SYMBOL_GPL(cec_s_phys_addr);
 
+void cec_s_phys_addr_from_edid(struct cec_adapter *adap,
+			       const struct edid *edid)
+{
+	u16 pa = CEC_PHYS_ADDR_INVALID;
+
+	if (edid && edid->extensions)
+		pa = cec_get_edid_phys_addr((const u8 *)edid,
+				EDID_LENGTH * (edid->extensions + 1), NULL);
+	cec_s_phys_addr(adap, pa, false);
+}
+EXPORT_SYMBOL_GPL(cec_s_phys_addr_from_edid);
+
 /*
  * Called from either the ioctl or a driver to set the logical addresses.
  *
@@ -1534,12 +1586,12 @@ int __cec_s_log_addrs(struct cec_adapter *adap,
 		if (log_addrs->num_log_addrs == 2) {
 			if (!(type_mask & ((1 << CEC_LOG_ADDR_TYPE_AUDIOSYSTEM) |
 					   (1 << CEC_LOG_ADDR_TYPE_TV)))) {
-				dprintk(1, "Two LAs is only allowed for audiosystem and TV\n");
+				dprintk(1, "two LAs is only allowed for audiosystem and TV\n");
 				return -EINVAL;
 			}
 			if (!(type_mask & ((1 << CEC_LOG_ADDR_TYPE_PLAYBACK) |
 					   (1 << CEC_LOG_ADDR_TYPE_RECORD)))) {
-				dprintk(1, "An audiosystem/TV can only be combined with record or playback\n");
+				dprintk(1, "an audiosystem/TV can only be combined with record or playback\n");
 				return -EINVAL;
 			}
 		}
@@ -1653,7 +1705,7 @@ static int cec_receive_notify(struct cec_adapter *adap, struct cec_msg *msg,
 	bool from_unregistered = init_laddr == 0xf;
 	struct cec_msg tx_cec_msg = { };
 
-	dprintk(1, "cec_receive_notify: %*ph\n", msg->len, msg->msg);
+	dprintk(2, "%s: %*ph\n", __func__, msg->len, msg->msg);
 
 	/* If this is a CDC-Only device, then ignore any non-CDC messages */
 	if (cec_is_cdc_only(&adap->log_addrs) &&
@@ -1722,7 +1774,7 @@ static int cec_receive_notify(struct cec_adapter *adap, struct cec_msg *msg,
 
 		if (!from_unregistered)
 			adap->phys_addrs[init_laddr] = pa;
-		dprintk(1, "Reported physical address %x.%x.%x.%x for logical address %d\n",
+		dprintk(1, "reported physical address %x.%x.%x.%x for logical address %d\n",
 			cec_phys_addr_exp(pa), init_laddr);
 		break;
 	}
diff --git a/drivers/media/cec/cec-api.c b/drivers/media/cec/cec-api.c
index 0860fb458757..f7eb4c54a354 100644
--- a/drivers/media/cec/cec-api.c
+++ b/drivers/media/cec/cec-api.c
@@ -202,7 +202,8 @@ static long cec_transmit(struct cec_adapter *adap, struct cec_fh *fh,
 		err = -EPERM;
 	else if (adap->is_configuring)
 		err = -ENONET;
-	else if (!adap->is_configured && msg.msg[0] != 0xf0)
+	else if (!adap->is_configured &&
+		 (adap->needs_hpd || msg.msg[0] != 0xf0))
 		err = -ENONET;
 	else if (cec_is_busy(adap, fh))
 		err = -EBUSY;
@@ -271,16 +272,10 @@ static long cec_receive(struct cec_adapter *adap, struct cec_fh *fh,
 			bool block, struct cec_msg __user *parg)
 {
 	struct cec_msg msg = {};
-	long err = 0;
+	long err;
 
 	if (copy_from_user(&msg, parg, sizeof(msg)))
 		return -EFAULT;
-	mutex_lock(&adap->lock);
-	if (!adap->is_configured && fh->mode_follower < CEC_MODE_MONITOR)
-		err = -ENONET;
-	mutex_unlock(&adap->lock);
-	if (err)
-		return err;
 
 	err = cec_receive_msg(fh, &msg, block);
 	if (err)
@@ -521,6 +516,7 @@ static int cec_open(struct inode *inode, struct file *filp)
 
 	mutex_lock(&devnode->lock);
 	if (list_empty(&devnode->fhs) &&
+	    !adap->needs_hpd &&
 	    adap->phys_addr == CEC_PHYS_ADDR_INVALID) {
 		err = adap->ops->adap_enable(adap, true);
 		if (err) {
@@ -565,6 +561,7 @@ static int cec_release(struct inode *inode, struct file *filp)
 	mutex_lock(&devnode->lock);
 	list_del(&fh->list);
 	if (list_empty(&devnode->fhs) &&
+	    !adap->needs_hpd &&
 	    adap->phys_addr == CEC_PHYS_ADDR_INVALID) {
 		WARN_ON(adap->ops->adap_enable(adap, false));
 	}
diff --git a/drivers/media/cec/cec-core.c b/drivers/media/cec/cec-core.c
index 2f87748ba4fc..b516d599d6c4 100644
--- a/drivers/media/cec/cec-core.c
+++ b/drivers/media/cec/cec-core.c
@@ -230,6 +230,7 @@ struct cec_adapter *cec_allocate_adapter(const struct cec_adap_ops *ops,
 	adap->log_addrs.cec_version = CEC_OP_CEC_VERSION_2_0;
 	adap->log_addrs.vendor_id = CEC_VENDOR_ID_NONE;
 	adap->capabilities = caps;
+	adap->needs_hpd = caps & CEC_CAP_NEEDS_HPD;
 	adap->available_log_addrs = available_las;
 	adap->sequence = 0;
 	adap->ops = ops;
diff --git a/drivers/media/dvb-core/dvb_ca_en50221.c b/drivers/media/dvb-core/dvb_ca_en50221.c
index d38bf9bce480..af694f2066a2 100644
--- a/drivers/media/dvb-core/dvb_ca_en50221.c
+++ b/drivers/media/dvb-core/dvb_ca_en50221.c
@@ -193,8 +193,10 @@ static void dvb_ca_private_put(struct dvb_ca_private *ca)
 }
 
 static void dvb_ca_en50221_thread_wakeup(struct dvb_ca_private *ca);
-static int dvb_ca_en50221_read_data(struct dvb_ca_private *ca, int slot, u8 * ebuf, int ecount);
-static int dvb_ca_en50221_write_data(struct dvb_ca_private *ca, int slot, u8 * ebuf, int ecount);
+static int dvb_ca_en50221_read_data(struct dvb_ca_private *ca, int slot,
+				    u8 *ebuf, int ecount);
+static int dvb_ca_en50221_write_data(struct dvb_ca_private *ca, int slot,
+				     u8 *ebuf, int ecount);
 
 
 /**
@@ -206,7 +208,7 @@ static int dvb_ca_en50221_write_data(struct dvb_ca_private *ca, int slot, u8 * e
  * @nlen: Number of bytes in needle.
  * @return Pointer into haystack needle was found at, or NULL if not found.
  */
-static char *findstr(char * haystack, int hlen, char * needle, int nlen)
+static char *findstr(char *haystack, int hlen, char *needle, int nlen)
 {
 	int i;
 
@@ -390,7 +392,8 @@ static int dvb_ca_en50221_link_init(struct dvb_ca_private *ca, int slot)
  * @return 0 on success, nonzero on error.
  */
 static int dvb_ca_en50221_read_tuple(struct dvb_ca_private *ca, int slot,
-				     int *address, int *tupleType, int *tupleLength, u8 * tuple)
+				     int *address, int *tupleType,
+				     int *tupleLength, u8 *tuple)
 {
 	int i;
 	int _tupleType;
@@ -621,7 +624,8 @@ static int dvb_ca_en50221_set_configoption(struct dvb_ca_private *ca, int slot)
  *
  * @return Number of bytes read, or < 0 on error
  */
-static int dvb_ca_en50221_read_data(struct dvb_ca_private *ca, int slot, u8 * ebuf, int ecount)
+static int dvb_ca_en50221_read_data(struct dvb_ca_private *ca, int slot,
+				    u8 *ebuf, int ecount)
 {
 	int bytes_read;
 	int status;
@@ -745,7 +749,8 @@ exit:
  *
  * @return Number of bytes written, or < 0 on error.
  */
-static int dvb_ca_en50221_write_data(struct dvb_ca_private *ca, int slot, u8 * buf, int bytes_write)
+static int dvb_ca_en50221_write_data(struct dvb_ca_private *ca, int slot,
+				     u8 *buf, int bytes_write)
 {
 	int status;
 	int i;
@@ -840,7 +845,6 @@ exit:
 exitnowrite:
 	return status;
 }
-EXPORT_SYMBOL(dvb_ca_en50221_camchange_irq);
 
 
 
@@ -849,7 +853,7 @@ EXPORT_SYMBOL(dvb_ca_en50221_camchange_irq);
 
 
 /**
- * dvb_ca_en50221_camready_irq - A CAM has been removed => shut it down.
+ * dvb_ca_en50221_slot_shutdown - A CAM has been removed => shut it down.
  *
  * @ca: CA instance.
  * @slot: Slot to shut down.
@@ -870,11 +874,10 @@ static int dvb_ca_en50221_slot_shutdown(struct dvb_ca_private *ca, int slot)
 	/* success */
 	return 0;
 }
-EXPORT_SYMBOL(dvb_ca_en50221_camready_irq);
 
 
 /**
- * dvb_ca_en50221_camready_irq - A CAMCHANGE IRQ has occurred.
+ * dvb_ca_en50221_camchange_irq - A CAMCHANGE IRQ has occurred.
  *
  * @ca: CA instance.
  * @slot: Slot concerned.
@@ -899,7 +902,7 @@ void dvb_ca_en50221_camchange_irq(struct dvb_ca_en50221 *pubca, int slot, int ch
 	atomic_inc(&ca->slot_info[slot].camchange_count);
 	dvb_ca_en50221_thread_wakeup(ca);
 }
-EXPORT_SYMBOL(dvb_ca_en50221_frda_irq);
+EXPORT_SYMBOL(dvb_ca_en50221_camchange_irq);
 
 
 /**
@@ -919,10 +922,11 @@ void dvb_ca_en50221_camready_irq(struct dvb_ca_en50221 *pubca, int slot)
 		dvb_ca_en50221_thread_wakeup(ca);
 	}
 }
+EXPORT_SYMBOL(dvb_ca_en50221_camready_irq);
 
 
 /**
- * An FR or DA IRQ has occurred.
+ * dvb_ca_en50221_frda_irq - An FR or DA IRQ has occurred.
  *
  * @ca: CA instance.
  * @slot: Slot concerned.
@@ -949,7 +953,7 @@ void dvb_ca_en50221_frda_irq(struct dvb_ca_en50221 *pubca, int slot)
 		break;
 	}
 }
-
+EXPORT_SYMBOL(dvb_ca_en50221_frda_irq);
 
 
 /* ******************************************************************************** */
@@ -1345,7 +1349,8 @@ static long dvb_ca_en50221_io_ioctl(struct file *file,
  * @return Number of bytes read, or <0 on error.
  */
 static ssize_t dvb_ca_en50221_io_write(struct file *file,
-				       const char __user * buf, size_t count, loff_t * ppos)
+				       const char __user *buf, size_t count,
+				       loff_t *ppos)
 {
 	struct dvb_device *dvbdev = file->private_data;
 	struct dvb_ca_private *ca = dvbdev->priv;
@@ -1485,8 +1490,8 @@ nextslot:
  *
  * @return Number of bytes read, or <0 on error.
  */
-static ssize_t dvb_ca_en50221_io_read(struct file *file, char __user * buf,
-				      size_t count, loff_t * ppos)
+static ssize_t dvb_ca_en50221_io_read(struct file *file, char __user *buf,
+				      size_t count, loff_t *ppos)
 {
 	struct dvb_device *dvbdev = file->private_data;
 	struct dvb_ca_private *ca = dvbdev->priv;
@@ -1664,7 +1669,7 @@ static int dvb_ca_en50221_io_release(struct inode *inode, struct file *file)
  *
  * @return Standard poll mask.
  */
-static unsigned int dvb_ca_en50221_io_poll(struct file *file, poll_table * wait)
+static unsigned int dvb_ca_en50221_io_poll(struct file *file, poll_table *wait)
 {
 	struct dvb_device *dvbdev = file->private_data;
 	struct dvb_ca_private *ca = dvbdev->priv;
diff --git a/drivers/media/dvb-core/dvb_net.c b/drivers/media/dvb-core/dvb_net.c
index 9947b342633e..06b0dcc13695 100644
--- a/drivers/media/dvb-core/dvb_net.c
+++ b/drivers/media/dvb-core/dvb_net.c
@@ -828,8 +828,7 @@ static void dvb_net_ule(struct net_device *dev, const u8 *buf, size_t buf_len)
 
 		/* Copy data into our current skb. */
 		h.how_much = min(h.priv->ule_sndu_remain, (int)h.ts_remain);
-		memcpy(skb_put(h.priv->ule_skb, h.how_much),
-		       h.from_where, h.how_much);
+		skb_put_data(h.priv->ule_skb, h.from_where, h.how_much);
 		h.priv->ule_sndu_remain -= h.how_much;
 		h.ts_remain -= h.how_much;
 		h.from_where += h.how_much;
@@ -964,7 +963,7 @@ static void dvb_net_sec(struct net_device *dev,
 	skb->dev = dev;
 
 	/* copy L3 payload */
-	eth = (u8 *) skb_put(skb, pkt_len - 12 - 4 + 14 - snap);
+	eth = skb_put(skb, pkt_len - 12 - 4 + 14 - snap);
 	memcpy(eth + 14, pkt + 12 + snap, pkt_len - 12 - 4 - snap);
 
 	/* create ethernet header: */
diff --git a/drivers/media/dvb-frontends/Kconfig b/drivers/media/dvb-frontends/Kconfig
index e8c6554a47aa..3a260b82b3e8 100644
--- a/drivers/media/dvb-frontends/Kconfig
+++ b/drivers/media/dvb-frontends/Kconfig
@@ -436,6 +436,7 @@ config DVB_TDA10048
 config DVB_AF9013
 	tristate "Afatech AF9013 demodulator"
 	depends on DVB_CORE && I2C
+	select REGMAP
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Say Y when you want to support this frontend.
diff --git a/drivers/media/dvb-frontends/af9013.c b/drivers/media/dvb-frontends/af9013.c
index b978002af4d8..b8f3ebfc3e27 100644
--- a/drivers/media/dvb-frontends/af9013.c
+++ b/drivers/media/dvb-frontends/af9013.c
@@ -20,13 +20,18 @@
 
 #include "af9013_priv.h"
 
-/* Max transfer size done by I2C transfer functions */
-#define MAX_XFER_SIZE  64
-
 struct af9013_state {
-	struct i2c_adapter *i2c;
+	struct i2c_client *client;
+	struct regmap *regmap;
 	struct dvb_frontend fe;
-	struct af9013_config config;
+	u32 clk;
+	u8 tuner;
+	u32 if_frequency;
+	u8 ts_mode;
+	u8 ts_output_pin;
+	bool spec_inv;
+	u8 api_version[4];
+	u8 gpio[4];
 
 	/* tuner/demod RF and IF AGC limits used for signal strength calc */
 	u8 signal_strength_en, rf_50, rf_80, if_50, if_80;
@@ -44,188 +49,14 @@ struct af9013_state {
 	struct delayed_work statistics_work;
 };
 
-/* write multiple registers */
-static int af9013_wr_regs_i2c(struct af9013_state *priv, u8 mbox, u16 reg,
-	const u8 *val, int len)
-{
-	int ret;
-	u8 buf[MAX_XFER_SIZE];
-	struct i2c_msg msg[1] = {
-		{
-			.addr = priv->config.i2c_addr,
-			.flags = 0,
-			.len = 3 + len,
-			.buf = buf,
-		}
-	};
-
-	if (3 + len > sizeof(buf)) {
-		dev_warn(&priv->i2c->dev,
-			 "%s: i2c wr reg=%04x: len=%d is too big!\n",
-			 KBUILD_MODNAME, reg, len);
-		return -EINVAL;
-	}
-
-	buf[0] = (reg >> 8) & 0xff;
-	buf[1] = (reg >> 0) & 0xff;
-	buf[2] = mbox;
-	memcpy(&buf[3], val, len);
-
-	ret = i2c_transfer(priv->i2c, msg, 1);
-	if (ret == 1) {
-		ret = 0;
-	} else {
-		dev_warn(&priv->i2c->dev, "%s: i2c wr failed=%d reg=%04x " \
-				"len=%d\n", KBUILD_MODNAME, ret, reg, len);
-		ret = -EREMOTEIO;
-	}
-	return ret;
-}
-
-/* read multiple registers */
-static int af9013_rd_regs_i2c(struct af9013_state *priv, u8 mbox, u16 reg,
-	u8 *val, int len)
-{
-	int ret;
-	u8 buf[3];
-	struct i2c_msg msg[2] = {
-		{
-			.addr = priv->config.i2c_addr,
-			.flags = 0,
-			.len = 3,
-			.buf = buf,
-		}, {
-			.addr = priv->config.i2c_addr,
-			.flags = I2C_M_RD,
-			.len = len,
-			.buf = val,
-		}
-	};
-
-	buf[0] = (reg >> 8) & 0xff;
-	buf[1] = (reg >> 0) & 0xff;
-	buf[2] = mbox;
-
-	ret = i2c_transfer(priv->i2c, msg, 2);
-	if (ret == 2) {
-		ret = 0;
-	} else {
-		dev_warn(&priv->i2c->dev, "%s: i2c rd failed=%d reg=%04x " \
-				"len=%d\n", KBUILD_MODNAME, ret, reg, len);
-		ret = -EREMOTEIO;
-	}
-	return ret;
-}
-
-/* write multiple registers */
-static int af9013_wr_regs(struct af9013_state *priv, u16 reg, const u8 *val,
-	int len)
-{
-	int ret, i;
-	u8 mbox = (0 << 7)|(0 << 6)|(1 << 1)|(1 << 0);
-
-	if ((priv->config.ts_mode == AF9013_TS_USB) &&
-		((reg & 0xff00) != 0xff00) && ((reg & 0xff00) != 0xae00)) {
-		mbox |= ((len - 1) << 2);
-		ret = af9013_wr_regs_i2c(priv, mbox, reg, val, len);
-	} else {
-		for (i = 0; i < len; i++) {
-			ret = af9013_wr_regs_i2c(priv, mbox, reg+i, val+i, 1);
-			if (ret)
-				goto err;
-		}
-	}
-
-err:
-	return 0;
-}
-
-/* read multiple registers */
-static int af9013_rd_regs(struct af9013_state *priv, u16 reg, u8 *val, int len)
-{
-	int ret, i;
-	u8 mbox = (0 << 7)|(0 << 6)|(1 << 1)|(0 << 0);
-
-	if ((priv->config.ts_mode == AF9013_TS_USB) &&
-		((reg & 0xff00) != 0xff00) && ((reg & 0xff00) != 0xae00)) {
-		mbox |= ((len - 1) << 2);
-		ret = af9013_rd_regs_i2c(priv, mbox, reg, val, len);
-	} else {
-		for (i = 0; i < len; i++) {
-			ret = af9013_rd_regs_i2c(priv, mbox, reg+i, val+i, 1);
-			if (ret)
-				goto err;
-		}
-	}
-
-err:
-	return 0;
-}
-
-/* write single register */
-static int af9013_wr_reg(struct af9013_state *priv, u16 reg, u8 val)
-{
-	return af9013_wr_regs(priv, reg, &val, 1);
-}
-
-/* read single register */
-static int af9013_rd_reg(struct af9013_state *priv, u16 reg, u8 *val)
-{
-	return af9013_rd_regs(priv, reg, val, 1);
-}
-
-static int af9013_write_ofsm_regs(struct af9013_state *state, u16 reg, u8 *val,
-	u8 len)
-{
-	u8 mbox = (1 << 7)|(1 << 6)|((len - 1) << 2)|(1 << 1)|(1 << 0);
-	return af9013_wr_regs_i2c(state, mbox, reg, val, len);
-}
-
-static int af9013_wr_reg_bits(struct af9013_state *state, u16 reg, int pos,
-	int len, u8 val)
-{
-	int ret;
-	u8 tmp, mask;
-
-	/* no need for read if whole reg is written */
-	if (len != 8) {
-		ret = af9013_rd_reg(state, reg, &tmp);
-		if (ret)
-			return ret;
-
-		mask = (0xff >> (8 - len)) << pos;
-		val <<= pos;
-		tmp &= ~mask;
-		val |= tmp;
-	}
-
-	return af9013_wr_reg(state, reg, val);
-}
-
-static int af9013_rd_reg_bits(struct af9013_state *state, u16 reg, int pos,
-	int len, u8 *val)
-{
-	int ret;
-	u8 tmp;
-
-	ret = af9013_rd_reg(state, reg, &tmp);
-	if (ret)
-		return ret;
-
-	*val = (tmp >> pos);
-	*val &= (0xff >> (8 - len));
-
-	return 0;
-}
-
 static int af9013_set_gpio(struct af9013_state *state, u8 gpio, u8 gpioval)
 {
+	struct i2c_client *client = state->client;
 	int ret;
 	u8 pos;
 	u16 addr;
 
-	dev_dbg(&state->i2c->dev, "%s: gpio=%d gpioval=%02x\n",
-			__func__, gpio, gpioval);
+	dev_dbg(&client->dev, "gpio %u, gpioval %02x\n", gpio, gpioval);
 
 	/*
 	 * GPIO0 & GPIO1 0xd735
@@ -243,8 +74,6 @@ static int af9013_set_gpio(struct af9013_state *state, u8 gpio, u8 gpioval)
 		break;
 
 	default:
-		dev_err(&state->i2c->dev, "%s: invalid gpio=%d\n",
-				KBUILD_MODNAME, gpio);
 		ret = -EINVAL;
 		goto err;
 	}
@@ -261,197 +90,124 @@ static int af9013_set_gpio(struct af9013_state *state, u8 gpio, u8 gpioval)
 		break;
 	}
 
-	ret = af9013_wr_reg_bits(state, addr, pos, 4, gpioval);
+	ret = regmap_update_bits(state->regmap, addr, 0x0f << pos,
+				 gpioval << pos);
 	if (ret)
 		goto err;
 
-	return ret;
-err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
-	return ret;
-}
-
-static u32 af9013_div(struct af9013_state *state, u32 a, u32 b, u32 x)
-{
-	u32 r = 0, c = 0, i;
-
-	dev_dbg(&state->i2c->dev, "%s: a=%d b=%d x=%d\n", __func__, a, b, x);
-
-	if (a > b) {
-		c = a / b;
-		a = a - c * b;
-	}
-
-	for (i = 0; i < x; i++) {
-		if (a >= b) {
-			r += 1;
-			a -= b;
-		}
-		a <<= 1;
-		r <<= 1;
-	}
-	r = (c << (u32)x) + r;
-
-	dev_dbg(&state->i2c->dev, "%s: a=%d b=%d x=%d r=%d r=%x\n",
-			__func__, a, b, x, r, r);
-
-	return r;
-}
-
-static int af9013_power_ctrl(struct af9013_state *state, u8 onoff)
-{
-	int ret, i;
-	u8 tmp;
-
-	dev_dbg(&state->i2c->dev, "%s: onoff=%d\n", __func__, onoff);
-
-	/* enable reset */
-	ret = af9013_wr_reg_bits(state, 0xd417, 4, 1, 1);
-	if (ret)
-		goto err;
-
-	/* start reset mechanism */
-	ret = af9013_wr_reg(state, 0xaeff, 1);
-	if (ret)
-		goto err;
-
-	/* wait reset performs */
-	for (i = 0; i < 150; i++) {
-		ret = af9013_rd_reg_bits(state, 0xd417, 1, 1, &tmp);
-		if (ret)
-			goto err;
-
-		if (tmp)
-			break; /* reset done */
-
-		usleep_range(5000, 25000);
-	}
-
-	if (!tmp)
-		return -ETIMEDOUT;
-
-	if (onoff) {
-		/* clear reset */
-		ret = af9013_wr_reg_bits(state, 0xd417, 1, 1, 0);
-		if (ret)
-			goto err;
-
-		/* disable reset */
-		ret = af9013_wr_reg_bits(state, 0xd417, 4, 1, 0);
-
-		/* power on */
-		ret = af9013_wr_reg_bits(state, 0xd73a, 3, 1, 0);
-	} else {
-		/* power off */
-		ret = af9013_wr_reg_bits(state, 0xd73a, 3, 1, 1);
-	}
-
-	return ret;
+	return 0;
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed %d\n", ret);
 	return ret;
 }
 
 static int af9013_statistics_ber_unc_start(struct dvb_frontend *fe)
 {
 	struct af9013_state *state = fe->demodulator_priv;
+	struct i2c_client *client = state->client;
 	int ret;
 
-	dev_dbg(&state->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	/* reset and start BER counter */
-	ret = af9013_wr_reg_bits(state, 0xd391, 4, 1, 1);
+	ret = regmap_update_bits(state->regmap, 0xd391, 0x10, 0x10);
 	if (ret)
 		goto err;
 
-	return ret;
+	return 0;
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed %d\n", ret);
 	return ret;
 }
 
 static int af9013_statistics_ber_unc_result(struct dvb_frontend *fe)
 {
 	struct af9013_state *state = fe->demodulator_priv;
+	struct i2c_client *client = state->client;
 	int ret;
+	unsigned int utmp;
 	u8 buf[5];
 
-	dev_dbg(&state->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	/* check if error bit count is ready */
-	ret = af9013_rd_reg_bits(state, 0xd391, 4, 1, &buf[0]);
+	ret = regmap_read(state->regmap, 0xd391, &utmp);
 	if (ret)
 		goto err;
 
-	if (!buf[0]) {
-		dev_dbg(&state->i2c->dev, "%s: not ready\n", __func__);
+	if (!((utmp >> 4) & 0x01)) {
+		dev_dbg(&client->dev, "not ready\n");
 		return 0;
 	}
 
-	ret = af9013_rd_regs(state, 0xd387, buf, 5);
+	ret = regmap_bulk_read(state->regmap, 0xd387, buf, 5);
 	if (ret)
 		goto err;
 
 	state->ber = (buf[2] << 16) | (buf[1] << 8) | buf[0];
 	state->ucblocks += (buf[4] << 8) | buf[3];
 
-	return ret;
+	return 0;
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed %d\n", ret);
 	return ret;
 }
 
 static int af9013_statistics_snr_start(struct dvb_frontend *fe)
 {
 	struct af9013_state *state = fe->demodulator_priv;
+	struct i2c_client *client = state->client;
 	int ret;
 
-	dev_dbg(&state->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	/* start SNR meas */
-	ret = af9013_wr_reg_bits(state, 0xd2e1, 3, 1, 1);
+	ret = regmap_update_bits(state->regmap, 0xd2e1, 0x08, 0x08);
 	if (ret)
 		goto err;
 
-	return ret;
+	return 0;
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed %d\n", ret);
 	return ret;
 }
 
 static int af9013_statistics_snr_result(struct dvb_frontend *fe)
 {
 	struct af9013_state *state = fe->demodulator_priv;
+	struct i2c_client *client = state->client;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret, i, len;
-	u8 buf[3], tmp;
+	unsigned int utmp;
+	u8 buf[3];
 	u32 snr_val;
 	const struct af9013_snr *uninitialized_var(snr_lut);
 
-	dev_dbg(&state->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	/* check if SNR ready */
-	ret = af9013_rd_reg_bits(state, 0xd2e1, 3, 1, &tmp);
+	ret = regmap_read(state->regmap, 0xd2e1, &utmp);
 	if (ret)
 		goto err;
 
-	if (!tmp) {
-		dev_dbg(&state->i2c->dev, "%s: not ready\n", __func__);
+	if (!((utmp >> 3) & 0x01)) {
+		dev_dbg(&client->dev, "not ready\n");
 		return 0;
 	}
 
 	/* read value */
-	ret = af9013_rd_regs(state, 0xd2e3, buf, 3);
+	ret = regmap_bulk_read(state->regmap, 0xd2e3, buf, 3);
 	if (ret)
 		goto err;
 
 	snr_val = (buf[2] << 16) | (buf[1] << 8) | buf[0];
 
 	/* read current modulation */
-	ret = af9013_rd_reg(state, 0xd3c1, &tmp);
+	ret = regmap_read(state->regmap, 0xd3c1, &utmp);
 	if (ret)
 		goto err;
 
-	switch ((tmp >> 6) & 3) {
+	switch ((utmp >> 6) & 3) {
 	case 0:
 		len = ARRAY_SIZE(qpsk_snr_lut);
 		snr_lut = qpsk_snr_lut;
@@ -469,32 +225,36 @@ static int af9013_statistics_snr_result(struct dvb_frontend *fe)
 	}
 
 	for (i = 0; i < len; i++) {
-		tmp = snr_lut[i].snr;
+		utmp = snr_lut[i].snr;
 
 		if (snr_val < snr_lut[i].val)
 			break;
 	}
-	state->snr = tmp * 10; /* dB/10 */
+	state->snr = utmp * 10; /* dB/10 */
 
-	return ret;
+	c->cnr.stat[0].svalue = 1000 * utmp;
+	c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+
+	return 0;
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed %d\n", ret);
 	return ret;
 }
 
 static int af9013_statistics_signal_strength(struct dvb_frontend *fe)
 {
 	struct af9013_state *state = fe->demodulator_priv;
+	struct i2c_client *client = state->client;
 	int ret = 0;
 	u8 buf[2], rf_gain, if_gain;
 	int signal_strength;
 
-	dev_dbg(&state->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	if (!state->signal_strength_en)
 		return 0;
 
-	ret = af9013_rd_regs(state, 0xd07c, buf, 2);
+	ret = regmap_bulk_read(state->regmap, 0xd07c, buf, 2);
 	if (ret)
 		goto err;
 
@@ -513,9 +273,9 @@ static int af9013_statistics_signal_strength(struct dvb_frontend *fe)
 
 	state->signal_strength = signal_strength;
 
-	return ret;
+	return 0;
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed %d\n", ret);
 	return ret;
 }
 
@@ -535,6 +295,7 @@ static void af9013_statistics_work(struct work_struct *work)
 	switch (state->statistics_step) {
 	default:
 		state->statistics_step = 0;
+		/* fall-through */
 	case 0:
 		af9013_statistics_signal_strength(&state->fe);
 		state->statistics_step++;
@@ -579,61 +340,72 @@ static int af9013_get_tune_settings(struct dvb_frontend *fe,
 static int af9013_set_frontend(struct dvb_frontend *fe)
 {
 	struct af9013_state *state = fe->demodulator_priv;
+	struct i2c_client *client = state->client;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret, i, sampling_freq;
 	bool auto_mode, spec_inv;
 	u8 buf[6];
 	u32 if_frequency, freq_cw;
 
-	dev_dbg(&state->i2c->dev, "%s: frequency=%d bandwidth_hz=%d\n",
-			__func__, c->frequency, c->bandwidth_hz);
+	dev_dbg(&client->dev, "frequency %u, bandwidth_hz %u\n",
+		c->frequency, c->bandwidth_hz);
 
 	/* program tuner */
-	if (fe->ops.tuner_ops.set_params)
-		fe->ops.tuner_ops.set_params(fe);
+	if (fe->ops.tuner_ops.set_params) {
+		ret = fe->ops.tuner_ops.set_params(fe);
+		if (ret)
+			goto err;
+	}
 
 	/* program CFOE coefficients */
 	if (c->bandwidth_hz != state->bandwidth_hz) {
 		for (i = 0; i < ARRAY_SIZE(coeff_lut); i++) {
-			if (coeff_lut[i].clock == state->config.clock &&
+			if (coeff_lut[i].clock == state->clk &&
 				coeff_lut[i].bandwidth_hz == c->bandwidth_hz) {
 				break;
 			}
 		}
 
 		/* Return an error if can't find bandwidth or the right clock */
-		if (i == ARRAY_SIZE(coeff_lut))
-			return -EINVAL;
+		if (i == ARRAY_SIZE(coeff_lut)) {
+			ret = -EINVAL;
+			goto err;
+		}
 
-		ret = af9013_wr_regs(state, 0xae00, coeff_lut[i].val,
-			sizeof(coeff_lut[i].val));
+		ret = regmap_bulk_write(state->regmap, 0xae00, coeff_lut[i].val,
+					sizeof(coeff_lut[i].val));
+		if (ret)
+			goto err;
 	}
 
 	/* program frequency control */
 	if (c->bandwidth_hz != state->bandwidth_hz || state->first_tune) {
 		/* get used IF frequency */
-		if (fe->ops.tuner_ops.get_if_frequency)
-			fe->ops.tuner_ops.get_if_frequency(fe, &if_frequency);
-		else
-			if_frequency = state->config.if_frequency;
+		if (fe->ops.tuner_ops.get_if_frequency) {
+			ret = fe->ops.tuner_ops.get_if_frequency(fe,
+								 &if_frequency);
+			if (ret)
+				goto err;
+		} else {
+			if_frequency = state->if_frequency;
+		}
 
-		dev_dbg(&state->i2c->dev, "%s: if_frequency=%d\n",
-				__func__, if_frequency);
+		dev_dbg(&client->dev, "if_frequency %u\n", if_frequency);
 
 		sampling_freq = if_frequency;
 
-		while (sampling_freq > (state->config.clock / 2))
-			sampling_freq -= state->config.clock;
+		while (sampling_freq > (state->clk / 2))
+			sampling_freq -= state->clk;
 
 		if (sampling_freq < 0) {
 			sampling_freq *= -1;
-			spec_inv = state->config.spec_inv;
+			spec_inv = state->spec_inv;
 		} else {
-			spec_inv = !state->config.spec_inv;
+			spec_inv = !state->spec_inv;
 		}
 
-		freq_cw = af9013_div(state, sampling_freq, state->config.clock,
-				23);
+		freq_cw = DIV_ROUND_CLOSEST_ULL((u64)sampling_freq * 0x800000,
+						state->clk);
 
 		if (spec_inv)
 			freq_cw = 0x800000 - freq_cw;
@@ -648,32 +420,32 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 		buf[4] = (freq_cw >>  8) & 0xff;
 		buf[5] = (freq_cw >> 16) & 0x7f;
 
-		ret = af9013_wr_regs(state, 0xd140, buf, 3);
+		ret = regmap_bulk_write(state->regmap, 0xd140, buf, 3);
 		if (ret)
 			goto err;
 
-		ret = af9013_wr_regs(state, 0x9be7, buf, 6);
+		ret = regmap_bulk_write(state->regmap, 0x9be7, buf, 6);
 		if (ret)
 			goto err;
 	}
 
 	/* clear TPS lock flag */
-	ret = af9013_wr_reg_bits(state, 0xd330, 3, 1, 1);
+	ret = regmap_update_bits(state->regmap, 0xd330, 0x08, 0x08);
 	if (ret)
 		goto err;
 
 	/* clear MPEG2 lock flag */
-	ret = af9013_wr_reg_bits(state, 0xd507, 6, 1, 0);
+	ret = regmap_update_bits(state->regmap, 0xd507, 0x40, 0x00);
 	if (ret)
 		goto err;
 
 	/* empty channel function */
-	ret = af9013_wr_reg_bits(state, 0x9bfe, 0, 1, 0);
+	ret = regmap_update_bits(state->regmap, 0x9bfe, 0x01, 0x00);
 	if (ret)
 		goto err;
 
 	/* empty DVB-T channel function */
-	ret = af9013_wr_reg_bits(state, 0x9bc2, 0, 1, 0);
+	ret = regmap_update_bits(state->regmap, 0x9bc2, 0x01, 0x00);
 	if (ret)
 		goto err;
 
@@ -691,8 +463,7 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 		buf[0] |= (1 << 0);
 		break;
 	default:
-		dev_dbg(&state->i2c->dev, "%s: invalid transmission_mode\n",
-				__func__);
+		dev_dbg(&client->dev, "invalid transmission_mode\n");
 		auto_mode = true;
 	}
 
@@ -712,8 +483,7 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 		buf[0] |= (3 << 2);
 		break;
 	default:
-		dev_dbg(&state->i2c->dev, "%s: invalid guard_interval\n",
-				__func__);
+		dev_dbg(&client->dev, "invalid guard_interval\n");
 		auto_mode = true;
 	}
 
@@ -733,7 +503,7 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 		buf[0] |= (3 << 4);
 		break;
 	default:
-		dev_dbg(&state->i2c->dev, "%s: invalid hierarchy\n", __func__);
+		dev_dbg(&client->dev, "invalid hierarchy\n");
 		auto_mode = true;
 	}
 
@@ -750,7 +520,7 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 		buf[1] |= (2 << 6);
 		break;
 	default:
-		dev_dbg(&state->i2c->dev, "%s: invalid modulation\n", __func__);
+		dev_dbg(&client->dev, "invalid modulation\n");
 		auto_mode = true;
 	}
 
@@ -776,8 +546,7 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 		buf[2] |= (4 << 0);
 		break;
 	default:
-		dev_dbg(&state->i2c->dev, "%s: invalid code_rate_HP\n",
-				__func__);
+		dev_dbg(&client->dev, "invalid code_rate_HP\n");
 		auto_mode = true;
 	}
 
@@ -802,8 +571,7 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 	case FEC_NONE:
 		break;
 	default:
-		dev_dbg(&state->i2c->dev, "%s: invalid code_rate_LP\n",
-				__func__);
+		dev_dbg(&client->dev, "invalid code_rate_LP\n");
 		auto_mode = true;
 	}
 
@@ -817,38 +585,37 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 		buf[1] |= (2 << 2);
 		break;
 	default:
-		dev_dbg(&state->i2c->dev, "%s: invalid bandwidth_hz\n",
-				__func__);
+		dev_dbg(&client->dev, "invalid bandwidth_hz\n");
 		ret = -EINVAL;
 		goto err;
 	}
 
-	ret = af9013_wr_regs(state, 0xd3c0, buf, 3);
+	ret = regmap_bulk_write(state->regmap, 0xd3c0, buf, 3);
 	if (ret)
 		goto err;
 
 	if (auto_mode) {
 		/* clear easy mode flag */
-		ret = af9013_wr_reg(state, 0xaefd, 0);
+		ret = regmap_write(state->regmap, 0xaefd, 0x00);
 		if (ret)
 			goto err;
 
-		dev_dbg(&state->i2c->dev, "%s: auto params\n", __func__);
+		dev_dbg(&client->dev, "auto params\n");
 	} else {
 		/* set easy mode flag */
-		ret = af9013_wr_reg(state, 0xaefd, 1);
+		ret = regmap_write(state->regmap, 0xaefd, 0x01);
 		if (ret)
 			goto err;
 
-		ret = af9013_wr_reg(state, 0xaefe, 0);
+		ret = regmap_write(state->regmap, 0xaefe, 0x00);
 		if (ret)
 			goto err;
 
-		dev_dbg(&state->i2c->dev, "%s: manual params\n", __func__);
+		dev_dbg(&client->dev, "manual params\n");
 	}
 
-	/* tune */
-	ret = af9013_wr_reg(state, 0xffff, 0);
+	/* Reset FSM */
+	ret = regmap_write(state->regmap, 0xffff, 0x00);
 	if (ret)
 		goto err;
 
@@ -856,9 +623,9 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 	state->set_frontend_jiffies = jiffies;
 	state->first_tune = false;
 
-	return ret;
+	return 0;
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed %d\n", ret);
 	return ret;
 }
 
@@ -866,12 +633,13 @@ static int af9013_get_frontend(struct dvb_frontend *fe,
 			       struct dtv_frontend_properties *c)
 {
 	struct af9013_state *state = fe->demodulator_priv;
+	struct i2c_client *client = state->client;
 	int ret;
 	u8 buf[3];
 
-	dev_dbg(&state->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
-	ret = af9013_rd_regs(state, 0xd3c0, buf, 3);
+	ret = regmap_bulk_read(state->regmap, 0xd3c0, buf, 3);
 	if (ret)
 		goto err;
 
@@ -973,17 +741,18 @@ static int af9013_get_frontend(struct dvb_frontend *fe,
 		break;
 	}
 
-	return ret;
+	return 0;
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed %d\n", ret);
 	return ret;
 }
 
 static int af9013_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct af9013_state *state = fe->demodulator_priv;
+	struct i2c_client *client = state->client;
 	int ret;
-	u8 tmp;
+	unsigned int utmp;
 
 	/*
 	 * Return status from the cache if it is younger than 2000ms with the
@@ -1001,21 +770,21 @@ static int af9013_read_status(struct dvb_frontend *fe, enum fe_status *status)
 	}
 
 	/* MPEG2 lock */
-	ret = af9013_rd_reg_bits(state, 0xd507, 6, 1, &tmp);
+	ret = regmap_read(state->regmap, 0xd507, &utmp);
 	if (ret)
 		goto err;
 
-	if (tmp)
+	if ((utmp >> 6) & 0x01)
 		*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER | FE_HAS_VITERBI |
 			FE_HAS_SYNC | FE_HAS_LOCK;
 
 	if (!*status) {
 		/* TPS lock */
-		ret = af9013_rd_reg_bits(state, 0xd330, 3, 1, &tmp);
+		ret = regmap_read(state->regmap, 0xd330, &utmp);
 		if (ret)
 			goto err;
 
-		if (tmp)
+		if ((utmp >> 3) & 0x01)
 			*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER |
 				FE_HAS_VITERBI;
 	}
@@ -1023,9 +792,9 @@ static int af9013_read_status(struct dvb_frontend *fe, enum fe_status *status)
 	state->fe_status = *status;
 	state->read_status_jiffies = jiffies;
 
-	return ret;
+	return 0;
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed %d\n", ret);
 	return ret;
 }
 
@@ -1060,118 +829,82 @@ static int af9013_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 static int af9013_init(struct dvb_frontend *fe)
 {
 	struct af9013_state *state = fe->demodulator_priv;
+	struct i2c_client *client = state->client;
 	int ret, i, len;
-	u8 buf[3], tmp;
-	u32 adc_cw;
+	unsigned int utmp;
+	u8 buf[3];
 	const struct af9013_reg_bit *init;
 
-	dev_dbg(&state->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
+
+	/* ADC on */
+	ret = regmap_update_bits(state->regmap, 0xd73a, 0x08, 0x00);
+	if (ret)
+		goto err;
 
-	/* power on */
-	ret = af9013_power_ctrl(state, 1);
+	/* Clear reset */
+	ret = regmap_update_bits(state->regmap, 0xd417, 0x02, 0x00);
 	if (ret)
 		goto err;
 
-	/* enable ADC */
-	ret = af9013_wr_reg(state, 0xd73a, 0xa4);
+	/* Disable reset */
+	ret = regmap_update_bits(state->regmap, 0xd417, 0x10, 0x00);
 	if (ret)
 		goto err;
 
 	/* write API version to firmware */
-	ret = af9013_wr_regs(state, 0x9bf2, state->config.api_version, 4);
+	ret = regmap_bulk_write(state->regmap, 0x9bf2, state->api_version, 4);
 	if (ret)
 		goto err;
 
 	/* program ADC control */
-	switch (state->config.clock) {
+	switch (state->clk) {
 	case 28800000: /* 28.800 MHz */
-		tmp = 0;
+		utmp = 0;
 		break;
 	case 20480000: /* 20.480 MHz */
-		tmp = 1;
+		utmp = 1;
 		break;
 	case 28000000: /* 28.000 MHz */
-		tmp = 2;
+		utmp = 2;
 		break;
 	case 25000000: /* 25.000 MHz */
-		tmp = 3;
+		utmp = 3;
 		break;
 	default:
-		dev_err(&state->i2c->dev, "%s: invalid clock\n",
-				KBUILD_MODNAME);
-		return -EINVAL;
-	}
-
-	adc_cw = af9013_div(state, state->config.clock, 1000000ul, 19);
-	buf[0] = (adc_cw >>  0) & 0xff;
-	buf[1] = (adc_cw >>  8) & 0xff;
-	buf[2] = (adc_cw >> 16) & 0xff;
-
-	ret = af9013_wr_regs(state, 0xd180, buf, 3);
-	if (ret)
-		goto err;
-
-	ret = af9013_wr_reg_bits(state, 0x9bd2, 0, 4, tmp);
-	if (ret)
-		goto err;
-
-	/* set I2C master clock */
-	ret = af9013_wr_reg(state, 0xd416, 0x14);
-	if (ret)
-		goto err;
-
-	/* set 16 embx */
-	ret = af9013_wr_reg_bits(state, 0xd700, 1, 1, 1);
-	if (ret)
-		goto err;
-
-	/* set no trigger */
-	ret = af9013_wr_reg_bits(state, 0xd700, 2, 1, 0);
-	if (ret)
+		ret = -EINVAL;
 		goto err;
+	}
 
-	/* set read-update bit for constellation */
-	ret = af9013_wr_reg_bits(state, 0xd371, 1, 1, 1);
+	ret = regmap_update_bits(state->regmap, 0x9bd2, 0x0f, utmp);
 	if (ret)
 		goto err;
 
-	/* settings for mp2if */
-	if (state->config.ts_mode == AF9013_TS_USB) {
-		/* AF9015 split PSB to 1.5k + 0.5k */
-		ret = af9013_wr_reg_bits(state, 0xd50b, 2, 1, 1);
-		if (ret)
-			goto err;
-	} else {
-		/* AF9013 change the output bit to data7 */
-		ret = af9013_wr_reg_bits(state, 0xd500, 3, 1, 1);
-		if (ret)
-			goto err;
-
-		/* AF9013 set mpeg to full speed */
-		ret = af9013_wr_reg_bits(state, 0xd502, 4, 1, 1);
-		if (ret)
-			goto err;
-	}
-
-	ret = af9013_wr_reg_bits(state, 0xd520, 4, 1, 1);
+	utmp = div_u64((u64)state->clk * 0x80000, 1000000);
+	buf[0] = (utmp >>  0) & 0xff;
+	buf[1] = (utmp >>  8) & 0xff;
+	buf[2] = (utmp >> 16) & 0xff;
+	ret = regmap_bulk_write(state->regmap, 0xd180, buf, 3);
 	if (ret)
 		goto err;
 
 	/* load OFSM settings */
-	dev_dbg(&state->i2c->dev, "%s: load ofsm settings\n", __func__);
+	dev_dbg(&client->dev, "load ofsm settings\n");
 	len = ARRAY_SIZE(ofsm_init);
 	init = ofsm_init;
 	for (i = 0; i < len; i++) {
-		ret = af9013_wr_reg_bits(state, init[i].addr, init[i].pos,
-			init[i].len, init[i].val);
+		u16 reg = init[i].addr;
+		u8 mask = GENMASK(init[i].pos + init[i].len - 1, init[i].pos);
+		u8 val = init[i].val << init[i].pos;
+
+		ret = regmap_update_bits(state->regmap, reg, mask, val);
 		if (ret)
 			goto err;
 	}
 
 	/* load tuner specific settings */
-	dev_dbg(&state->i2c->dev, "%s: load tuner specific settings\n",
-			__func__);
-	switch (state->config.tuner) {
+	dev_dbg(&client->dev, "load tuner specific settings\n");
+	switch (state->tuner) {
 	case AF9013_TUNER_MXL5003D:
 		len = ARRAY_SIZE(tuner_init_mxl5003d);
 		init = tuner_init_mxl5003d;
@@ -1216,98 +949,126 @@ static int af9013_init(struct dvb_frontend *fe)
 	}
 
 	for (i = 0; i < len; i++) {
-		ret = af9013_wr_reg_bits(state, init[i].addr, init[i].pos,
-			init[i].len, init[i].val);
+		u16 reg = init[i].addr;
+		u8 mask = GENMASK(init[i].pos + init[i].len - 1, init[i].pos);
+		u8 val = init[i].val << init[i].pos;
+
+		ret = regmap_update_bits(state->regmap, reg, mask, val);
 		if (ret)
 			goto err;
 	}
 
-	/* TS mode */
-	ret = af9013_wr_reg_bits(state, 0xd500, 1, 2, state->config.ts_mode);
+	/* TS interface */
+	if (state->ts_output_pin == 7)
+		utmp = 1 << 3 | state->ts_mode << 1;
+	else
+		utmp = 0 << 3 | state->ts_mode << 1;
+	ret = regmap_update_bits(state->regmap, 0xd500, 0x0e, utmp);
 	if (ret)
 		goto err;
 
 	/* enable lock led */
-	ret = af9013_wr_reg_bits(state, 0xd730, 0, 1, 1);
+	ret = regmap_update_bits(state->regmap, 0xd730, 0x01, 0x01);
 	if (ret)
 		goto err;
 
 	/* check if we support signal strength */
 	if (!state->signal_strength_en) {
-		ret = af9013_rd_reg_bits(state, 0x9bee, 0, 1,
-			&state->signal_strength_en);
+		ret = regmap_read(state->regmap, 0x9bee, &utmp);
 		if (ret)
 			goto err;
+
+		state->signal_strength_en = (utmp >> 0) & 0x01;
 	}
 
 	/* read values needed for signal strength calculation */
 	if (state->signal_strength_en && !state->rf_50) {
-		ret = af9013_rd_reg(state, 0x9bbd, &state->rf_50);
+		ret = regmap_bulk_read(state->regmap, 0x9bbd, &state->rf_50, 1);
 		if (ret)
 			goto err;
-
-		ret = af9013_rd_reg(state, 0x9bd0, &state->rf_80);
+		ret = regmap_bulk_read(state->regmap, 0x9bd0, &state->rf_80, 1);
 		if (ret)
 			goto err;
-
-		ret = af9013_rd_reg(state, 0x9be2, &state->if_50);
+		ret = regmap_bulk_read(state->regmap, 0x9be2, &state->if_50, 1);
 		if (ret)
 			goto err;
-
-		ret = af9013_rd_reg(state, 0x9be4, &state->if_80);
+		ret = regmap_bulk_read(state->regmap, 0x9be4, &state->if_80, 1);
 		if (ret)
 			goto err;
 	}
 
 	/* SNR */
-	ret = af9013_wr_reg(state, 0xd2e2, 1);
+	ret = regmap_write(state->regmap, 0xd2e2, 0x01);
 	if (ret)
 		goto err;
 
 	/* BER / UCB */
 	buf[0] = (10000 >> 0) & 0xff;
 	buf[1] = (10000 >> 8) & 0xff;
-	ret = af9013_wr_regs(state, 0xd385, buf, 2);
+	ret = regmap_bulk_write(state->regmap, 0xd385, buf, 2);
 	if (ret)
 		goto err;
 
 	/* enable FEC monitor */
-	ret = af9013_wr_reg_bits(state, 0xd392, 1, 1, 1);
+	ret = regmap_update_bits(state->regmap, 0xd392, 0x02, 0x02);
 	if (ret)
 		goto err;
 
 	state->first_tune = true;
 	schedule_delayed_work(&state->statistics_work, msecs_to_jiffies(400));
 
-	return ret;
+	return 0;
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed %d\n", ret);
 	return ret;
 }
 
 static int af9013_sleep(struct dvb_frontend *fe)
 {
 	struct af9013_state *state = fe->demodulator_priv;
+	struct i2c_client *client = state->client;
 	int ret;
+	unsigned int utmp;
 
-	dev_dbg(&state->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	/* stop statistics polling */
 	cancel_delayed_work_sync(&state->statistics_work);
 
 	/* disable lock led */
-	ret = af9013_wr_reg_bits(state, 0xd730, 0, 1, 0);
+	ret = regmap_update_bits(state->regmap, 0xd730, 0x01, 0x00);
 	if (ret)
 		goto err;
 
-	/* power off */
-	ret = af9013_power_ctrl(state, 0);
+	/* Enable reset */
+	ret = regmap_update_bits(state->regmap, 0xd417, 0x10, 0x10);
 	if (ret)
 		goto err;
 
-	return ret;
+	/* Start reset execution */
+	ret = regmap_write(state->regmap, 0xaeff, 0x01);
+	if (ret)
+		goto err;
+
+	/* Wait reset performs */
+	ret = regmap_read_poll_timeout(state->regmap, 0xd417, utmp,
+				       (utmp >> 1) & 0x01, 5000, 1000000);
+	if (ret)
+		goto err;
+
+	if (!((utmp >> 1) & 0x01)) {
+		ret = -ETIMEDOUT;
+		goto err;
+	}
+
+	/* ADC off */
+	ret = regmap_update_bits(state->regmap, 0xd73a, 0x08, 0x08);
+	if (ret)
+		goto err;
+
+	return 0;
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed %d\n", ret);
 	return ret;
 }
 
@@ -1315,200 +1076,174 @@ static int af9013_i2c_gate_ctrl(struct dvb_frontend *fe, int enable)
 {
 	int ret;
 	struct af9013_state *state = fe->demodulator_priv;
+	struct i2c_client *client = state->client;
 
-	dev_dbg(&state->i2c->dev, "%s: enable=%d\n", __func__, enable);
+	dev_dbg(&client->dev, "enable %d\n", enable);
 
 	/* gate already open or close */
 	if (state->i2c_gate_state == enable)
 		return 0;
 
-	if (state->config.ts_mode == AF9013_TS_USB)
-		ret = af9013_wr_reg_bits(state, 0xd417, 3, 1, enable);
+	if (state->ts_mode == AF9013_TS_MODE_USB)
+		ret = regmap_update_bits(state->regmap, 0xd417, 0x08,
+					 enable << 3);
 	else
-		ret = af9013_wr_reg_bits(state, 0xd607, 2, 1, enable);
+		ret = regmap_update_bits(state->regmap, 0xd607, 0x04,
+					 enable << 2);
 	if (ret)
 		goto err;
 
 	state->i2c_gate_state = enable;
 
-	return ret;
+	return 0;
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed %d\n", ret);
 	return ret;
 }
 
 static void af9013_release(struct dvb_frontend *fe)
 {
 	struct af9013_state *state = fe->demodulator_priv;
+	struct i2c_client *client = state->client;
 
-	/* stop statistics polling */
-	cancel_delayed_work_sync(&state->statistics_work);
+	dev_dbg(&client->dev, "\n");
 
-	kfree(state);
+	i2c_unregister_device(client);
 }
 
 static const struct dvb_frontend_ops af9013_ops;
 
 static int af9013_download_firmware(struct af9013_state *state)
 {
-	int i, len, remaining, ret;
-	const struct firmware *fw;
+	struct i2c_client *client = state->client;
+	int ret, i, len, rem;
+	unsigned int utmp;
+	u8 buf[4];
 	u16 checksum = 0;
-	u8 val;
-	u8 fw_params[4];
-	u8 *fw_file = AF9013_FIRMWARE;
+	const struct firmware *firmware;
+	const char *name = AF9013_FIRMWARE;
 
-	msleep(100);
-	/* check whether firmware is already running */
-	ret = af9013_rd_reg(state, 0x98be, &val);
+	dev_dbg(&client->dev, "\n");
+
+	/* Check whether firmware is already running */
+	ret = regmap_read(state->regmap, 0x98be, &utmp);
 	if (ret)
 		goto err;
-	else
-		dev_dbg(&state->i2c->dev, "%s: firmware status=%02x\n",
-				__func__, val);
 
-	if (val == 0x0c) /* fw is running, no need for download */
-		goto exit;
+	dev_dbg(&client->dev, "firmware status %02x\n", utmp);
 
-	dev_info(&state->i2c->dev, "%s: found a '%s' in cold state, will try " \
-			"to load a firmware\n",
-			KBUILD_MODNAME, af9013_ops.info.name);
+	if (utmp == 0x0c)
+		return 0;
 
-	/* request the firmware, this will block and timeout */
-	ret = request_firmware(&fw, fw_file, state->i2c->dev.parent);
+	dev_info(&client->dev, "found a '%s' in cold state, will try to load a firmware\n",
+		 af9013_ops.info.name);
+
+	/* Request the firmware, will block and timeout */
+	ret = request_firmware(&firmware, name, &client->dev);
 	if (ret) {
-		dev_info(&state->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);
+		dev_info(&client->dev, "firmware file '%s' not found %d\n",
+			 name, ret);
 		goto err;
 	}
 
-	dev_info(&state->i2c->dev, "%s: downloading firmware from file '%s'\n",
-			KBUILD_MODNAME, fw_file);
-
-	/* calc checksum */
-	for (i = 0; i < fw->size; i++)
-		checksum += fw->data[i];
+	dev_info(&client->dev, "downloading firmware from file '%s'\n",
+		 name);
 
-	fw_params[0] = checksum >> 8;
-	fw_params[1] = checksum & 0xff;
-	fw_params[2] = fw->size >> 8;
-	fw_params[3] = fw->size & 0xff;
+	/* Write firmware checksum & size */
+	for (i = 0; i < firmware->size; i++)
+		checksum += firmware->data[i];
 
-	/* write fw checksum & size */
-	ret = af9013_write_ofsm_regs(state, 0x50fc,
-		fw_params, sizeof(fw_params));
+	buf[0] = (checksum >> 8) & 0xff;
+	buf[1] = (checksum >> 0) & 0xff;
+	buf[2] = (firmware->size >> 8) & 0xff;
+	buf[3] = (firmware->size >> 0) & 0xff;
+	ret = regmap_bulk_write(state->regmap, 0x50fc, buf, 4);
 	if (ret)
-		goto err_release;
-
-	#define FW_ADDR 0x5100 /* firmware start address */
-	#define LEN_MAX 16 /* max packet size */
-	for (remaining = fw->size; remaining > 0; remaining -= LEN_MAX) {
-		len = remaining;
-		if (len > LEN_MAX)
-			len = LEN_MAX;
-
-		ret = af9013_write_ofsm_regs(state,
-			FW_ADDR + fw->size - remaining,
-			(u8 *) &fw->data[fw->size - remaining], len);
+		goto err_release_firmware;
+
+	/* Download firmware */
+	#define LEN_MAX 16
+	for (rem = firmware->size; rem > 0; rem -= LEN_MAX) {
+		len = min(LEN_MAX, rem);
+		ret = regmap_bulk_write(state->regmap,
+					0x5100 + firmware->size - rem,
+					&firmware->data[firmware->size - rem],
+					len);
 		if (ret) {
-			dev_err(&state->i2c->dev,
-					"%s: firmware download failed=%d\n",
-					KBUILD_MODNAME, ret);
-			goto err_release;
+			dev_err(&client->dev, "firmware download failed %d\n",
+				ret);
+			goto err_release_firmware;
 		}
 	}
 
-	/* request boot firmware */
-	ret = af9013_wr_reg(state, 0xe205, 1);
-	if (ret)
-		goto err_release;
-
-	for (i = 0; i < 15; i++) {
-		msleep(100);
+	release_firmware(firmware);
 
-		/* check firmware status */
-		ret = af9013_rd_reg(state, 0x98be, &val);
-		if (ret)
-			goto err_release;
+	/* Boot firmware */
+	ret = regmap_write(state->regmap, 0xe205, 0x01);
+	if (ret)
+		goto err;
 
-		dev_dbg(&state->i2c->dev, "%s: firmware status=%02x\n",
-				__func__, val);
+	/* Check firmware status. 0c=OK, 04=fail */
+	ret = regmap_read_poll_timeout(state->regmap, 0x98be, utmp,
+				       (utmp == 0x0c || utmp == 0x04),
+				       5000, 1000000);
+	if (ret)
+		goto err;
 
-		if (val == 0x0c || val == 0x04) /* success or fail */
-			break;
-	}
+	dev_dbg(&client->dev, "firmware status %02x\n", utmp);
 
-	if (val == 0x04) {
-		dev_err(&state->i2c->dev, "%s: firmware did not run\n",
-				KBUILD_MODNAME);
+	if (utmp == 0x04) {
 		ret = -ENODEV;
-	} else if (val != 0x0c) {
-		dev_err(&state->i2c->dev, "%s: firmware boot timeout\n",
-				KBUILD_MODNAME);
+		dev_err(&client->dev, "firmware did not run\n");
+		goto err;
+	} else if (utmp != 0x0c) {
 		ret = -ENODEV;
+		dev_err(&client->dev, "firmware boot timeout\n");
+		goto err;
 	}
 
-err_release:
-	release_firmware(fw);
+	dev_info(&client->dev, "found a '%s' in warm state\n",
+		 af9013_ops.info.name);
+
+	return 0;
+err_release_firmware:
+	release_firmware(firmware);
 err:
-exit:
-	if (!ret)
-		dev_info(&state->i2c->dev, "%s: found a '%s' in warm state\n",
-				KBUILD_MODNAME, af9013_ops.info.name);
+	dev_dbg(&client->dev, "failed %d\n", ret);
 	return ret;
 }
 
+/*
+ * XXX: That is wrapper to af9013_probe() via driver core in order to provide
+ * proper I2C client for legacy media attach binding.
+ * New users must use I2C client binding directly!
+ */
 struct dvb_frontend *af9013_attach(const struct af9013_config *config,
-	struct i2c_adapter *i2c)
+				   struct i2c_adapter *i2c)
 {
-	int ret;
-	struct af9013_state *state = NULL;
-	u8 buf[4], i;
-
-	/* allocate memory for the internal state */
-	state = kzalloc(sizeof(struct af9013_state), GFP_KERNEL);
-	if (state == NULL)
-		goto err;
-
-	/* setup the state */
-	state->i2c = i2c;
-	memcpy(&state->config, config, sizeof(struct af9013_config));
-
-	/* download firmware */
-	if (state->config.ts_mode != AF9013_TS_USB) {
-		ret = af9013_download_firmware(state);
-		if (ret)
-			goto err;
-	}
-
-	/* firmware version */
-	ret = af9013_rd_regs(state, 0x5103, buf, 4);
-	if (ret)
-		goto err;
-
-	dev_info(&state->i2c->dev, "%s: firmware version %d.%d.%d.%d\n",
-			KBUILD_MODNAME, buf[0], buf[1], buf[2], buf[3]);
-
-	/* set GPIOs */
-	for (i = 0; i < sizeof(state->config.gpio); i++) {
-		ret = af9013_set_gpio(state, i, state->config.gpio[i]);
-		if (ret)
-			goto err;
-	}
-
-	/* create dvb_frontend */
-	memcpy(&state->fe.ops, &af9013_ops,
-		sizeof(struct dvb_frontend_ops));
-	state->fe.demodulator_priv = state;
-
-	INIT_DELAYED_WORK(&state->statistics_work, af9013_statistics_work);
-
-	return &state->fe;
-err:
-	kfree(state);
-	return NULL;
+	struct i2c_client *client;
+	struct i2c_board_info board_info;
+	struct af9013_platform_data pdata;
+
+	pdata.clk = config->clock;
+	pdata.tuner = config->tuner;
+	pdata.if_frequency = config->if_frequency;
+	pdata.ts_mode = config->ts_mode;
+	pdata.ts_output_pin = 7;
+	pdata.spec_inv = config->spec_inv;
+	memcpy(&pdata.api_version, config->api_version, sizeof(pdata.api_version));
+	memcpy(&pdata.gpio, config->gpio, sizeof(pdata.gpio));
+	pdata.attach_in_use = true;
+
+	memset(&board_info, 0, sizeof(board_info));
+	strlcpy(board_info.type, "af9013", sizeof(board_info.type));
+	board_info.addr = config->i2c_addr;
+	board_info.platform_data = &pdata;
+	client = i2c_new_device(i2c, &board_info);
+	if (!client || !client->dev.driver)
+		return NULL;
+
+	return pdata.get_dvb_frontend(client);
 }
 EXPORT_SYMBOL(af9013_attach);
 
@@ -1555,6 +1290,279 @@ static const struct dvb_frontend_ops af9013_ops = {
 	.i2c_gate_ctrl = af9013_i2c_gate_ctrl,
 };
 
+static struct dvb_frontend *af9013_get_dvb_frontend(struct i2c_client *client)
+{
+	struct af9013_state *state = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "\n");
+
+	return &state->fe;
+}
+
+/* Own I2C access routines needed for regmap as chip uses extra command byte */
+static int af9013_wregs(struct i2c_client *client, u8 cmd, u16 reg,
+			const u8 *val, int len)
+{
+	int ret;
+	u8 buf[21];
+	struct i2c_msg msg[1] = {
+		{
+			.addr = client->addr,
+			.flags = 0,
+			.len = 3 + len,
+			.buf = buf,
+		}
+	};
+
+	if (3 + len > sizeof(buf)) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	buf[0] = (reg >> 8) & 0xff;
+	buf[1] = (reg >> 0) & 0xff;
+	buf[2] = cmd;
+	memcpy(&buf[3], val, len);
+	ret = i2c_transfer(client->adapter, msg, 1);
+	if (ret < 0) {
+		goto err;
+	} else if (ret != 1) {
+		ret = -EREMOTEIO;
+		goto err;
+	}
+
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed %d\n", ret);
+	return ret;
+}
+
+static int af9013_rregs(struct i2c_client *client, u8 cmd, u16 reg,
+			u8 *val, int len)
+{
+	int ret;
+	u8 buf[3];
+	struct i2c_msg msg[2] = {
+		{
+			.addr = client->addr,
+			.flags = 0,
+			.len = 3,
+			.buf = buf,
+		}, {
+			.addr = client->addr,
+			.flags = I2C_M_RD,
+			.len = len,
+			.buf = val,
+		}
+	};
+
+	buf[0] = (reg >> 8) & 0xff;
+	buf[1] = (reg >> 0) & 0xff;
+	buf[2] = cmd;
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret < 0) {
+		goto err;
+	} else if (ret != 2) {
+		ret = -EREMOTEIO;
+		goto err;
+	}
+
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed %d\n", ret);
+	return ret;
+}
+
+static int af9013_regmap_write(void *context, const void *data, size_t count)
+{
+	struct i2c_client *client = context;
+	struct af9013_state *state = i2c_get_clientdata(client);
+	int ret, i;
+	u8 cmd;
+	u16 reg = ((u8 *)data)[0] << 8|((u8 *)data)[1] << 0;
+	u8 *val = &((u8 *)data)[2];
+	const unsigned int len = count - 2;
+
+	if (state->ts_mode == AF9013_TS_MODE_USB && (reg & 0xff00) != 0xae00) {
+		cmd = 0 << 7|0 << 6|(len - 1) << 2|1 << 1|1 << 0;
+		ret = af9013_wregs(client, cmd, reg, val, len);
+		if (ret)
+			goto err;
+	} else if (reg >= 0x5100 && reg < 0x8fff) {
+		/* Firmware download */
+		cmd = 1 << 7|1 << 6|(len - 1) << 2|1 << 1|1 << 0;
+		ret = af9013_wregs(client, cmd, reg, val, len);
+		if (ret)
+			goto err;
+	} else {
+		cmd = 0 << 7|0 << 6|(1 - 1) << 2|1 << 1|1 << 0;
+		for (i = 0; i < len; i++) {
+			ret = af9013_wregs(client, cmd, reg + i, val + i, 1);
+			if (ret)
+				goto err;
+		}
+	}
+
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed %d\n", ret);
+	return ret;
+}
+
+static int af9013_regmap_read(void *context, const void *reg_buf,
+			      size_t reg_size, void *val_buf, size_t val_size)
+{
+	struct i2c_client *client = context;
+	struct af9013_state *state = i2c_get_clientdata(client);
+	int ret, i;
+	u8 cmd;
+	u16 reg = ((u8 *)reg_buf)[0] << 8|((u8 *)reg_buf)[1] << 0;
+	u8 *val = &((u8 *)val_buf)[0];
+	const unsigned int len = val_size;
+
+	if (state->ts_mode == AF9013_TS_MODE_USB && (reg & 0xff00) != 0xae00) {
+		cmd = 0 << 7|0 << 6|(len - 1) << 2|1 << 1|0 << 0;
+		ret = af9013_rregs(client, cmd, reg, val_buf, len);
+		if (ret)
+			goto err;
+	} else {
+		cmd = 0 << 7|0 << 6|(1 - 1) << 2|1 << 1|0 << 0;
+		for (i = 0; i < len; i++) {
+			ret = af9013_rregs(client, cmd, reg + i, val + i, 1);
+			if (ret)
+				goto err;
+		}
+	}
+
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed %d\n", ret);
+	return ret;
+}
+
+static int af9013_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct af9013_state *state;
+	struct af9013_platform_data *pdata = client->dev.platform_data;
+	struct dtv_frontend_properties *c;
+	int ret, i;
+	u8 firmware_version[4];
+	static const struct regmap_bus regmap_bus = {
+		.read = af9013_regmap_read,
+		.write = af9013_regmap_write,
+	};
+	static const struct regmap_config regmap_config = {
+		.reg_bits    =  16,
+		.val_bits    =  8,
+	};
+
+	state = kzalloc(sizeof(*state), GFP_KERNEL);
+	if (!state) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	/* Setup the state */
+	state->client = client;
+	i2c_set_clientdata(client, state);
+	state->clk = pdata->clk;
+	state->tuner = pdata->tuner;
+	state->if_frequency = pdata->if_frequency;
+	state->ts_mode = pdata->ts_mode;
+	state->ts_output_pin = pdata->ts_output_pin;
+	state->spec_inv = pdata->spec_inv;
+	memcpy(&state->api_version, pdata->api_version, sizeof(state->api_version));
+	memcpy(&state->gpio, pdata->gpio, sizeof(state->gpio));
+	INIT_DELAYED_WORK(&state->statistics_work, af9013_statistics_work);
+	state->regmap = regmap_init(&client->dev, &regmap_bus, client,
+				  &regmap_config);
+	if (IS_ERR(state->regmap)) {
+		ret = PTR_ERR(state->regmap);
+		goto err_kfree;
+	}
+
+	/* Download firmware */
+	if (state->ts_mode != AF9013_TS_MODE_USB) {
+		ret = af9013_download_firmware(state);
+		if (ret)
+			goto err_regmap_exit;
+	}
+
+	/* Firmware version */
+	ret = regmap_bulk_read(state->regmap, 0x5103, firmware_version,
+			       sizeof(firmware_version));
+	if (ret)
+		goto err_regmap_exit;
+
+	/* Set GPIOs */
+	for (i = 0; i < sizeof(state->gpio); i++) {
+		ret = af9013_set_gpio(state, i, state->gpio[i]);
+		if (ret)
+			goto err_regmap_exit;
+	}
+
+	/* Create dvb frontend */
+	memcpy(&state->fe.ops, &af9013_ops, sizeof(state->fe.ops));
+	if (!pdata->attach_in_use)
+		state->fe.ops.release = NULL;
+	state->fe.demodulator_priv = state;
+
+	/* Setup callbacks */
+	pdata->get_dvb_frontend = af9013_get_dvb_frontend;
+
+	/* Init stats to indicate which stats are supported */
+	c = &state->fe.dtv_property_cache;
+	c->cnr.len = 1;
+
+	dev_info(&client->dev, "Afatech AF9013 successfully attached\n");
+	dev_info(&client->dev, "firmware version: %d.%d.%d.%d\n",
+		 firmware_version[0], firmware_version[1],
+		 firmware_version[2], firmware_version[3]);
+	return 0;
+err_regmap_exit:
+	regmap_exit(state->regmap);
+err_kfree:
+	kfree(state);
+err:
+	dev_dbg(&client->dev, "failed %d\n", ret);
+	return ret;
+}
+
+static int af9013_remove(struct i2c_client *client)
+{
+	struct af9013_state *state = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "\n");
+
+	/* Stop statistics polling */
+	cancel_delayed_work_sync(&state->statistics_work);
+
+	regmap_exit(state->regmap);
+
+	kfree(state);
+
+	return 0;
+}
+
+static const struct i2c_device_id af9013_id_table[] = {
+	{"af9013", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, af9013_id_table);
+
+static struct i2c_driver af9013_driver = {
+	.driver = {
+		.name	= "af9013",
+		.suppress_bind_attrs = true,
+	},
+	.probe		= af9013_probe,
+	.remove		= af9013_remove,
+	.id_table	= af9013_id_table,
+};
+
+module_i2c_driver(af9013_driver);
+
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
 MODULE_DESCRIPTION("Afatech AF9013 DVB-T demodulator driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb-frontends/af9013.h b/drivers/media/dvb-frontends/af9013.h
index 277112863719..353274524f1b 100644
--- a/drivers/media/dvb-frontends/af9013.h
+++ b/drivers/media/dvb-frontends/af9013.h
@@ -23,29 +23,27 @@
 
 #include <linux/dvb/frontend.h>
 
-/* AF9013/5 GPIOs (mostly guessed)
-   demod#1-gpio#0 - set demod#2 i2c-addr for dual devices
-   demod#1-gpio#1 - xtal setting (?)
-   demod#1-gpio#3 - tuner#1
-   demod#2-gpio#0 - tuner#2
-   demod#2-gpio#1 - xtal setting (?)
-*/
-
-struct af9013_config {
-	/*
-	 * I2C address
-	 */
-	u8 i2c_addr;
+/*
+ * I2C address: 0x1c, 0x1d
+ */
 
+/**
+ * struct af9013_platform_data - Platform data for the af9013 driver
+ * @clk: Clock frequency.
+ * @tuner: Used tuner model.
+ * @if_frequency: IF frequency.
+ * @ts_mode: TS mode.
+ * @ts_output_pin: TS output pin.
+ * @spec_inv: Input spectrum inverted.
+ * @api_version: Firmware API version.
+ * @gpio: GPIOs.
+ * @get_dvb_frontend: Get DVB frontend callback.
+ */
+struct af9013_platform_data {
 	/*
-	 * clock
 	 * 20480000, 25000000, 28000000, 28800000
 	 */
-	u32 clock;
-
-	/*
-	 * tuner
-	 */
+	u32 clk;
 #define AF9013_TUNER_MXL5003D      3 /* MaxLinear */
 #define AF9013_TUNER_MXL5005D     13 /* MaxLinear */
 #define AF9013_TUNER_MXL5005R     30 /* MaxLinear */
@@ -60,33 +58,14 @@ struct af9013_config {
 #define AF9013_TUNER_MXL5007T    177 /* MaxLinear */
 #define AF9013_TUNER_TDA18218    179 /* NXP */
 	u8 tuner;
-
-	/*
-	 * IF frequency
-	 */
 	u32 if_frequency;
-
-	/*
-	 * TS settings
-	 */
-#define AF9013_TS_USB       0
-#define AF9013_TS_PARALLEL  1
-#define AF9013_TS_SERIAL    2
-	u8 ts_mode:2;
-
-	/*
-	 * input spectrum inversion
-	 */
+#define AF9013_TS_MODE_USB       0
+#define AF9013_TS_MODE_PARALLEL  1
+#define AF9013_TS_MODE_SERIAL    2
+	u8 ts_mode;
+	u8 ts_output_pin;
 	bool spec_inv;
-
-	/*
-	 * firmware API version
-	 */
 	u8 api_version[4];
-
-	/*
-	 * GPIOs
-	 */
 #define AF9013_GPIO_ON (1 << 0)
 #define AF9013_GPIO_EN (1 << 1)
 #define AF9013_GPIO_O  (1 << 2)
@@ -96,8 +75,29 @@ struct af9013_config {
 #define AF9013_GPIO_TUNER_ON  (AF9013_GPIO_ON|AF9013_GPIO_EN)
 #define AF9013_GPIO_TUNER_OFF (AF9013_GPIO_ON|AF9013_GPIO_EN|AF9013_GPIO_O)
 	u8 gpio[4];
+
+	struct dvb_frontend* (*get_dvb_frontend)(struct i2c_client *);
+
+/* private: For legacy media attach wrapper. Do not set value. */
+	bool attach_in_use;
+	u8 i2c_addr;
+	u32 clock;
 };
 
+#define af9013_config       af9013_platform_data
+#define AF9013_TS_USB       AF9013_TS_MODE_USB
+#define AF9013_TS_PARALLEL  AF9013_TS_MODE_PARALLEL
+#define AF9013_TS_SERIAL    AF9013_TS_MODE_SERIAL
+
+/*
+ * AF9013/5 GPIOs (mostly guessed)
+ * demod#1-gpio#0 - set demod#2 i2c-addr for dual devices
+ * demod#1-gpio#1 - xtal setting (?)
+ * demod#1-gpio#3 - tuner#1
+ * demod#2-gpio#0 - tuner#2
+ * demod#2-gpio#1 - xtal setting (?)
+ */
+
 #if IS_REACHABLE(CONFIG_DVB_AF9013)
 extern struct dvb_frontend *af9013_attach(const struct af9013_config *config,
 	struct i2c_adapter *i2c);
diff --git a/drivers/media/dvb-frontends/af9013_priv.h b/drivers/media/dvb-frontends/af9013_priv.h
index 31d6538abfae..35ca5c9bcacd 100644
--- a/drivers/media/dvb-frontends/af9013_priv.h
+++ b/drivers/media/dvb-frontends/af9013_priv.h
@@ -24,6 +24,8 @@
 #include "dvb_frontend.h"
 #include "af9013.h"
 #include <linux/firmware.h>
+#include <linux/math64.h>
+#include <linux/regmap.h>
 
 #define AF9013_FIRMWARE "dvb-fe-af9013.fw"
 
diff --git a/drivers/media/dvb-frontends/au8522_common.c b/drivers/media/dvb-frontends/au8522_common.c
index cf4ac240a01f..6722838c3707 100644
--- a/drivers/media/dvb-frontends/au8522_common.c
+++ b/drivers/media/dvb-frontends/au8522_common.c
@@ -234,6 +234,7 @@ int au8522_init(struct dvb_frontend *fe)
 	   chip, so that when it gets powered back up it won't think
 	   that it is already tuned */
 	state->current_frequency = 0;
+	state->current_modulation = VSB_8;
 
 	au8522_writereg(state, 0xa4, 1 << 5);
 
diff --git a/drivers/media/dvb-frontends/au8522_decoder.c b/drivers/media/dvb-frontends/au8522_decoder.c
index a2e771305008..343dc92ef54e 100644
--- a/drivers/media/dvb-frontends/au8522_decoder.c
+++ b/drivers/media/dvb-frontends/au8522_decoder.c
@@ -17,7 +17,6 @@
 
 /* Developer notes:
  *
- * VBI support is not yet working
  * Enough is implemented here for CVBS and S-Video inputs, but the actual
  *  analog demodulator code isn't implemented (not needed for xc5000 since it
  *  has its own demodulator and outputs CVBS)
@@ -179,42 +178,6 @@ static inline struct au8522_state *to_state(struct v4l2_subdev *sd)
 	return container_of(sd, struct au8522_state, sd);
 }
 
-static void setup_vbi(struct au8522_state *state, int aud_input)
-{
-	int i;
-
-	/* These are set to zero regardless of what mode we're in */
-	au8522_writereg(state, AU8522_TVDEC_VBI_CTRL_H_REG017H, 0x00);
-	au8522_writereg(state, AU8522_TVDEC_VBI_CTRL_L_REG018H, 0x00);
-	au8522_writereg(state, AU8522_TVDEC_VBI_USER_TOTAL_BITS_REG019H, 0x00);
-	au8522_writereg(state, AU8522_TVDEC_VBI_USER_TUNIT_H_REG01AH, 0x00);
-	au8522_writereg(state, AU8522_TVDEC_VBI_USER_TUNIT_L_REG01BH, 0x00);
-	au8522_writereg(state, AU8522_TVDEC_VBI_USER_THRESH1_REG01CH, 0x00);
-	au8522_writereg(state, AU8522_TVDEC_VBI_USER_FRAME_PAT2_REG01EH, 0x00);
-	au8522_writereg(state, AU8522_TVDEC_VBI_USER_FRAME_PAT1_REG01FH, 0x00);
-	au8522_writereg(state, AU8522_TVDEC_VBI_USER_FRAME_PAT0_REG020H, 0x00);
-	au8522_writereg(state, AU8522_TVDEC_VBI_USER_FRAME_MASK2_REG021H,
-			0x00);
-	au8522_writereg(state, AU8522_TVDEC_VBI_USER_FRAME_MASK1_REG022H,
-			0x00);
-	au8522_writereg(state, AU8522_TVDEC_VBI_USER_FRAME_MASK0_REG023H,
-			0x00);
-
-	/* Setup the VBI registers */
-	for (i = 0x30; i < 0x60; i++)
-		au8522_writereg(state, i, 0x40);
-
-	/* For some reason, every register is 0x40 except register 0x44
-	   (confirmed via the HVR-950q USB capture) */
-	au8522_writereg(state, 0x44, 0x60);
-
-	/* Enable VBI (we always do this regardless of whether the user is
-	   viewing closed caption info) */
-	au8522_writereg(state, AU8522_TVDEC_VBI_CTRL_H_REG017H,
-			AU8522_TVDEC_VBI_CTRL_H_REG017H_CCON);
-
-}
-
 static void setup_decoder_defaults(struct au8522_state *state, bool is_svideo)
 {
 	int i;
@@ -317,8 +280,6 @@ static void setup_decoder_defaults(struct au8522_state *state, bool is_svideo)
 			AU8522_TOREGAAGC_REG0E5H_CVBS);
 	au8522_writereg(state, AU8522_REG016H, AU8522_REG016H_CVBS);
 
-	setup_vbi(state, 0);
-
 	if (is_svideo) {
 		/* Despite what the table says, for the HVR-950q we still need
 		   to be in CVBS mode for the S-Video input (reason unknown). */
@@ -456,30 +417,29 @@ static void set_audio_input(struct au8522_state *state)
 				lpfilter_coef[i].reg_val[0]);
 	}
 
-	/* Setup audio */
-	au8522_writereg(state, AU8522_AUDIO_VOLUME_L_REG0F2H, 0x00);
-	au8522_writereg(state, AU8522_AUDIO_VOLUME_R_REG0F3H, 0x00);
-	au8522_writereg(state, AU8522_AUDIO_VOLUME_REG0F4H, 0x00);
-	au8522_writereg(state, AU8522_I2C_CONTROL_REG1_REG091H, 0x80);
-	au8522_writereg(state, AU8522_I2C_CONTROL_REG0_REG090H, 0x84);
-	msleep(150);
-	au8522_writereg(state, AU8522_SYSTEM_MODULE_CONTROL_0_REG0A4H, 0x00);
-	msleep(10);
-	au8522_writereg(state, AU8522_SYSTEM_MODULE_CONTROL_0_REG0A4H,
-			AU8522_SYSTEM_MODULE_CONTROL_0_REG0A4H_CVBS);
-	msleep(50);
+	/* Set the volume */
 	au8522_writereg(state, AU8522_AUDIO_VOLUME_L_REG0F2H, 0x7F);
 	au8522_writereg(state, AU8522_AUDIO_VOLUME_R_REG0F3H, 0x7F);
 	au8522_writereg(state, AU8522_AUDIO_VOLUME_REG0F4H, 0xff);
-	msleep(80);
-	au8522_writereg(state, AU8522_AUDIO_VOLUME_L_REG0F2H, 0x7F);
-	au8522_writereg(state, AU8522_AUDIO_VOLUME_R_REG0F3H, 0x7F);
+
+	/* Not sure what this does */
 	au8522_writereg(state, AU8522_REG0F9H, AU8522_REG0F9H_AUDIO);
+
+	/* Setup the audio mode to stereo DBX */
 	au8522_writereg(state, AU8522_AUDIO_MODE_REG0F1H, 0x82);
 	msleep(70);
-	au8522_writereg(state, AU8522_SYSTEM_MODULE_CONTROL_1_REG0A5H, 0x09);
+
+	/* Start the audio processing module */
+	au8522_writereg(state, AU8522_SYSTEM_MODULE_CONTROL_0_REG0A4H, 0x9d);
+
+	/* Set the audio frequency to 48 KHz */
 	au8522_writereg(state, AU8522_AUDIOFREQ_REG606H, 0x03);
+
+	/* Set the I2S parameters (WS, LSB, mode, sample rate */
 	au8522_writereg(state, AU8522_I2S_CTRL_2_REG112H, 0xc2);
+
+	/* Enable the I2S output */
+	au8522_writereg(state, AU8522_SYSTEM_MODULE_CONTROL_1_REG0A5H, 0x09);
 }
 
 /* ----------------------------------------------------------------------- */
@@ -663,10 +623,12 @@ static int au8522_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	int val = 0;
 	struct au8522_state *state = to_state(sd);
 	u8 lock_status;
+	u8 pll_status;
 
 	/* Interrogate the decoder to see if we are getting a real signal */
 	lock_status = au8522_readreg(state, 0x00);
-	if (lock_status == 0xa2)
+	pll_status = au8522_readreg(state, 0x7e);
+	if ((lock_status == 0xa2) && (pll_status & 0x10))
 		vt->signal = 0xffff;
 	else
 		vt->signal = 0x00;
diff --git a/drivers/media/dvb-frontends/au8522_dig.c b/drivers/media/dvb-frontends/au8522_dig.c
index 7ed326e43fc4..3f3635f5a06a 100644
--- a/drivers/media/dvb-frontends/au8522_dig.c
+++ b/drivers/media/dvb-frontends/au8522_dig.c
@@ -271,9 +271,9 @@ static int au8522_set_if(struct dvb_frontend *fe, enum au8522_if_freq if_freq)
 		return -EINVAL;
 	}
 	dprintk("%s() %s MHz\n", __func__, ifmhz);
-	au8522_writereg(state, 0x80b5, r0b5);
-	au8522_writereg(state, 0x80b6, r0b6);
-	au8522_writereg(state, 0x80b7, r0b7);
+	au8522_writereg(state, 0x00b5, r0b5);
+	au8522_writereg(state, 0x00b6, r0b6);
+	au8522_writereg(state, 0x00b7, r0b7);
 
 	return 0;
 }
@@ -283,33 +283,32 @@ static struct {
 	u16 reg;
 	u16 data;
 } VSB_mod_tab[] = {
-	{ 0x8090, 0x84 },
-	{ 0x4092, 0x11 },
+	{ 0x0090, 0x84 },
 	{ 0x2005, 0x00 },
-	{ 0x8091, 0x80 },
-	{ 0x80a3, 0x0c },
-	{ 0x80a4, 0xe8 },
-	{ 0x8081, 0xc4 },
-	{ 0x80a5, 0x40 },
-	{ 0x80a7, 0x40 },
-	{ 0x80a6, 0x67 },
-	{ 0x8262, 0x20 },
-	{ 0x821c, 0x30 },
-	{ 0x80d8, 0x1a },
-	{ 0x8227, 0xa0 },
-	{ 0x8121, 0xff },
-	{ 0x80a8, 0xf0 },
-	{ 0x80a9, 0x05 },
-	{ 0x80aa, 0x77 },
-	{ 0x80ab, 0xf0 },
-	{ 0x80ac, 0x05 },
-	{ 0x80ad, 0x77 },
-	{ 0x80ae, 0x41 },
-	{ 0x80af, 0x66 },
-	{ 0x821b, 0xcc },
-	{ 0x821d, 0x80 },
-	{ 0x80a4, 0xe8 },
-	{ 0x8231, 0x13 },
+	{ 0x0091, 0x80 },
+	{ 0x00a3, 0x0c },
+	{ 0x00a4, 0xe8 },
+	{ 0x0081, 0xc4 },
+	{ 0x00a5, 0x40 },
+	{ 0x00a7, 0x40 },
+	{ 0x00a6, 0x67 },
+	{ 0x0262, 0x20 },
+	{ 0x021c, 0x30 },
+	{ 0x00d8, 0x1a },
+	{ 0x0227, 0xa0 },
+	{ 0x0121, 0xff },
+	{ 0x00a8, 0xf0 },
+	{ 0x00a9, 0x05 },
+	{ 0x00aa, 0x77 },
+	{ 0x00ab, 0xf0 },
+	{ 0x00ac, 0x05 },
+	{ 0x00ad, 0x77 },
+	{ 0x00ae, 0x41 },
+	{ 0x00af, 0x66 },
+	{ 0x021b, 0xcc },
+	{ 0x021d, 0x80 },
+	{ 0x00a4, 0xe8 },
+	{ 0x0231, 0x13 },
 };
 
 /* QAM64 Modulation table */
@@ -396,78 +395,78 @@ static struct {
 	u16 reg;
 	u16 data;
 } QAM256_mod_tab[] = {
-	{ 0x80a3, 0x09 },
-	{ 0x80a4, 0x00 },
-	{ 0x8081, 0xc4 },
-	{ 0x80a5, 0x40 },
-	{ 0x80aa, 0x77 },
-	{ 0x80ad, 0x77 },
-	{ 0x80a6, 0x67 },
-	{ 0x8262, 0x20 },
-	{ 0x821c, 0x30 },
-	{ 0x80b8, 0x3e },
-	{ 0x80b9, 0xf0 },
-	{ 0x80ba, 0x01 },
-	{ 0x80bb, 0x18 },
-	{ 0x80bc, 0x50 },
-	{ 0x80bd, 0x00 },
-	{ 0x80be, 0xea },
-	{ 0x80bf, 0xef },
-	{ 0x80c0, 0xfc },
-	{ 0x80c1, 0xbd },
-	{ 0x80c2, 0x1f },
-	{ 0x80c3, 0xfc },
-	{ 0x80c4, 0xdd },
-	{ 0x80c5, 0xaf },
-	{ 0x80c6, 0x00 },
-	{ 0x80c7, 0x38 },
-	{ 0x80c8, 0x30 },
-	{ 0x80c9, 0x05 },
-	{ 0x80ca, 0x4a },
-	{ 0x80cb, 0xd0 },
-	{ 0x80cc, 0x01 },
-	{ 0x80cd, 0xd9 },
-	{ 0x80ce, 0x6f },
-	{ 0x80cf, 0xf9 },
-	{ 0x80d0, 0x70 },
-	{ 0x80d1, 0xdf },
-	{ 0x80d2, 0xf7 },
-	{ 0x80d3, 0xc2 },
-	{ 0x80d4, 0xdf },
-	{ 0x80d5, 0x02 },
-	{ 0x80d6, 0x9a },
-	{ 0x80d7, 0xd0 },
-	{ 0x8250, 0x0d },
-	{ 0x8251, 0xcd },
-	{ 0x8252, 0xe0 },
-	{ 0x8253, 0x05 },
-	{ 0x8254, 0xa7 },
-	{ 0x8255, 0xff },
-	{ 0x8256, 0xed },
-	{ 0x8257, 0x5b },
-	{ 0x8258, 0xae },
-	{ 0x8259, 0xe6 },
-	{ 0x825a, 0x3d },
-	{ 0x825b, 0x0f },
-	{ 0x825c, 0x0d },
-	{ 0x825d, 0xea },
-	{ 0x825e, 0xf2 },
-	{ 0x825f, 0x51 },
-	{ 0x8260, 0xf5 },
-	{ 0x8261, 0x06 },
-	{ 0x821a, 0x00 },
-	{ 0x8546, 0x40 },
-	{ 0x8210, 0x26 },
-	{ 0x8211, 0xf6 },
-	{ 0x8212, 0x84 },
-	{ 0x8213, 0x02 },
-	{ 0x8502, 0x01 },
-	{ 0x8121, 0x04 },
-	{ 0x8122, 0x04 },
-	{ 0x852e, 0x10 },
-	{ 0x80a4, 0xca },
-	{ 0x80a7, 0x40 },
-	{ 0x8526, 0x01 },
+	{ 0x00a3, 0x09 },
+	{ 0x00a4, 0x00 },
+	{ 0x0081, 0xc4 },
+	{ 0x00a5, 0x40 },
+	{ 0x00aa, 0x77 },
+	{ 0x00ad, 0x77 },
+	{ 0x00a6, 0x67 },
+	{ 0x0262, 0x20 },
+	{ 0x021c, 0x30 },
+	{ 0x00b8, 0x3e },
+	{ 0x00b9, 0xf0 },
+	{ 0x00ba, 0x01 },
+	{ 0x00bb, 0x18 },
+	{ 0x00bc, 0x50 },
+	{ 0x00bd, 0x00 },
+	{ 0x00be, 0xea },
+	{ 0x00bf, 0xef },
+	{ 0x00c0, 0xfc },
+	{ 0x00c1, 0xbd },
+	{ 0x00c2, 0x1f },
+	{ 0x00c3, 0xfc },
+	{ 0x00c4, 0xdd },
+	{ 0x00c5, 0xaf },
+	{ 0x00c6, 0x00 },
+	{ 0x00c7, 0x38 },
+	{ 0x00c8, 0x30 },
+	{ 0x00c9, 0x05 },
+	{ 0x00ca, 0x4a },
+	{ 0x00cb, 0xd0 },
+	{ 0x00cc, 0x01 },
+	{ 0x00cd, 0xd9 },
+	{ 0x00ce, 0x6f },
+	{ 0x00cf, 0xf9 },
+	{ 0x00d0, 0x70 },
+	{ 0x00d1, 0xdf },
+	{ 0x00d2, 0xf7 },
+	{ 0x00d3, 0xc2 },
+	{ 0x00d4, 0xdf },
+	{ 0x00d5, 0x02 },
+	{ 0x00d6, 0x9a },
+	{ 0x00d7, 0xd0 },
+	{ 0x0250, 0x0d },
+	{ 0x0251, 0xcd },
+	{ 0x0252, 0xe0 },
+	{ 0x0253, 0x05 },
+	{ 0x0254, 0xa7 },
+	{ 0x0255, 0xff },
+	{ 0x0256, 0xed },
+	{ 0x0257, 0x5b },
+	{ 0x0258, 0xae },
+	{ 0x0259, 0xe6 },
+	{ 0x025a, 0x3d },
+	{ 0x025b, 0x0f },
+	{ 0x025c, 0x0d },
+	{ 0x025d, 0xea },
+	{ 0x025e, 0xf2 },
+	{ 0x025f, 0x51 },
+	{ 0x0260, 0xf5 },
+	{ 0x0261, 0x06 },
+	{ 0x021a, 0x00 },
+	{ 0x0546, 0x40 },
+	{ 0x0210, 0x26 },
+	{ 0x0211, 0xf6 },
+	{ 0x0212, 0x84 },
+	{ 0x0213, 0x02 },
+	{ 0x0502, 0x01 },
+	{ 0x0121, 0x04 },
+	{ 0x0122, 0x04 },
+	{ 0x052e, 0x10 },
+	{ 0x00a4, 0xca },
+	{ 0x00a7, 0x40 },
+	{ 0x0526, 0x01 },
 };
 
 static struct {
@@ -654,12 +653,12 @@ static int au8522_read_status(struct dvb_frontend *fe, enum fe_status *status)
 
 	if (state->current_modulation == VSB_8) {
 		dprintk("%s() Checking VSB_8\n", __func__);
-		reg = au8522_readreg(state, 0x4088);
+		reg = au8522_readreg(state, 0x0088);
 		if ((reg & 0x03) == 0x03)
 			*status |= FE_HAS_LOCK | FE_HAS_SYNC | FE_HAS_VITERBI;
 	} else {
 		dprintk("%s() Checking QAM\n", __func__);
-		reg = au8522_readreg(state, 0x4541);
+		reg = au8522_readreg(state, 0x0541);
 		if (reg & 0x80)
 			*status |= FE_HAS_VITERBI;
 		if (reg & 0x20)
@@ -745,17 +744,17 @@ static int au8522_read_snr(struct dvb_frontend *fe, u16 *snr)
 	if (state->current_modulation == QAM_256)
 		ret = au8522_mse2snr_lookup(qam256_mse2snr_tab,
 					    ARRAY_SIZE(qam256_mse2snr_tab),
-					    au8522_readreg(state, 0x4522),
+					    au8522_readreg(state, 0x0522),
 					    snr);
 	else if (state->current_modulation == QAM_64)
 		ret = au8522_mse2snr_lookup(qam64_mse2snr_tab,
 					    ARRAY_SIZE(qam64_mse2snr_tab),
-					    au8522_readreg(state, 0x4522),
+					    au8522_readreg(state, 0x0522),
 					    snr);
 	else /* VSB_8 */
 		ret = au8522_mse2snr_lookup(vsb_mse2snr_tab,
 					    ARRAY_SIZE(vsb_mse2snr_tab),
-					    au8522_readreg(state, 0x4311),
+					    au8522_readreg(state, 0x0311),
 					    snr);
 
 	if (state->config.led_cfg)
@@ -804,9 +803,9 @@ static int au8522_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 	struct au8522_state *state = fe->demodulator_priv;
 
 	if (state->current_modulation == VSB_8)
-		*ucblocks = au8522_readreg(state, 0x4087);
+		*ucblocks = au8522_readreg(state, 0x0087);
 	else
-		*ucblocks = au8522_readreg(state, 0x4543);
+		*ucblocks = au8522_readreg(state, 0x0543);
 
 	return 0;
 }
diff --git a/drivers/media/dvb-frontends/bcm3510.c b/drivers/media/dvb-frontends/bcm3510.c
index 617c5e29f919..ba63ad170d3c 100644
--- a/drivers/media/dvb-frontends/bcm3510.c
+++ b/drivers/media/dvb-frontends/bcm3510.c
@@ -538,6 +538,7 @@ static int bcm3510_set_frontend(struct dvb_frontend *fe)
 			cmd.ACQUIRE0.MODE = 0x9;
 			cmd.ACQUIRE1.SYM_RATE = 0x0;
 			cmd.ACQUIRE1.IF_FREQ = 0x0;
+			break;
 		default:
 			return -EINVAL;
 	}
@@ -772,7 +773,8 @@ static int bcm3510_init(struct dvb_frontend* fe)
 			deb_info("attempting to download firmware\n");
 			if ((ret = bcm3510_init_cold(st)) < 0)
 				return ret;
-		case JDEC_EEPROM_LOAD_WAIT: /* fall-through is wanted */
+			/* fall-through */
+		case JDEC_EEPROM_LOAD_WAIT:
 			deb_info("firmware is loaded\n");
 			bcm3510_check_firmware_version(st);
 			break;
diff --git a/drivers/media/dvb-frontends/cxd2841er.c b/drivers/media/dvb-frontends/cxd2841er.c
index ce37dc2e89c7..08f67d60a7d9 100644
--- a/drivers/media/dvb-frontends/cxd2841er.c
+++ b/drivers/media/dvb-frontends/cxd2841er.c
@@ -38,6 +38,8 @@
 #define MAX_WRITE_REGSIZE	16
 #define LOG2_E_100X 144
 
+#define INTLOG10X100(x) ((u32) (((u64) intlog10(x) * 100) >> 24))
+
 /* DVB-C constellation */
 enum sony_dvbc_constellation_t {
 	SONY_DVBC_CONSTELLATION_16QAM,
@@ -65,6 +67,7 @@ struct cxd2841er_priv {
 	u8				system;
 	enum cxd2841er_xtal		xtal;
 	enum fe_caps caps;
+	u32				flags;
 };
 
 static const struct cxd2841er_cnr_data s_cn_data[] = {
@@ -201,11 +204,6 @@ static const struct cxd2841er_cnr_data s2_cn_data[] = {
 	{ 0x0016, 19700 }, { 0x0015, 19900 }, { 0x0014, 20000 },
 };
 
-#define MAKE_IFFREQ_CONFIG(iffreq) ((u32)(((iffreq)/41.0)*16777216.0 + 0.5))
-#define MAKE_IFFREQ_CONFIG_XTAL(xtal, iffreq) ((xtal == SONY_XTAL_24000) ? \
-		(u32)(((iffreq)/48.0)*16777216.0 + 0.5) : \
-		(u32)(((iffreq)/41.0)*16777216.0 + 0.5))
-
 static int cxd2841er_freeze_regs(struct cxd2841er_priv *priv);
 static int cxd2841er_unfreeze_regs(struct cxd2841er_priv *priv);
 
@@ -214,10 +212,8 @@ static void cxd2841er_i2c_debug(struct cxd2841er_priv *priv,
 				const u8 *data, u32 len)
 {
 	dev_dbg(&priv->i2c->dev,
-		"cxd2841er: I2C %s addr %02x reg 0x%02x size %d\n",
-		(write == 0 ? "read" : "write"), addr, reg, len);
-	print_hex_dump_bytes("cxd2841er: I2C data: ",
-		DUMP_PREFIX_OFFSET, data, len);
+		"cxd2841er: I2C %s addr %02x reg 0x%02x size %d data %*ph\n",
+		(write == 0 ? "read" : "write"), addr, reg, len, len, data);
 }
 
 static int cxd2841er_write_regs(struct cxd2841er_priv *priv,
@@ -284,17 +280,8 @@ static int cxd2841er_read_regs(struct cxd2841er_priv *priv,
 		}
 	};
 
-	ret = i2c_transfer(priv->i2c, &msg[0], 1);
-	if (ret >= 0 && ret != 1)
-		ret = -EIO;
-	if (ret < 0) {
-		dev_warn(&priv->i2c->dev,
-			"%s: i2c rw failed=%d addr=%02x reg=%02x\n",
-			KBUILD_MODNAME, ret, i2c_addr, reg);
-		return ret;
-	}
-	ret = i2c_transfer(priv->i2c, &msg[1], 1);
-	if (ret >= 0 && ret != 1)
+	ret = i2c_transfer(priv->i2c, msg, 2);
+	if (ret >= 0 && ret != 2)
 		ret = -EIO;
 	if (ret < 0) {
 		dev_warn(&priv->i2c->dev,
@@ -327,6 +314,49 @@ static int cxd2841er_set_reg_bits(struct cxd2841er_priv *priv,
 	return cxd2841er_write_reg(priv, addr, reg, data);
 }
 
+static u32 cxd2841er_calc_iffreq_xtal(enum cxd2841er_xtal xtal, u32 ifhz)
+{
+	u64 tmp;
+
+	tmp = (u64) ifhz * 16777216;
+	do_div(tmp, ((xtal == SONY_XTAL_24000) ? 48000000 : 41000000));
+
+	return (u32) tmp;
+}
+
+static u32 cxd2841er_calc_iffreq(u32 ifhz)
+{
+	return cxd2841er_calc_iffreq_xtal(SONY_XTAL_20500, ifhz);
+}
+
+static int cxd2841er_get_if_hz(struct cxd2841er_priv *priv, u32 def_hz)
+{
+	u32 hz;
+
+	if (priv->frontend.ops.tuner_ops.get_if_frequency
+			&& (priv->flags & CXD2841ER_AUTO_IFHZ))
+		priv->frontend.ops.tuner_ops.get_if_frequency(
+			&priv->frontend, &hz);
+	else
+		hz = def_hz;
+
+	return hz;
+}
+
+static int cxd2841er_tuner_set(struct dvb_frontend *fe)
+{
+	struct cxd2841er_priv *priv = fe->demodulator_priv;
+
+	if ((priv->flags & CXD2841ER_USE_GATECTRL) && fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 1);
+	if (fe->ops.tuner_ops.set_params)
+		fe->ops.tuner_ops.set_params(fe);
+	if ((priv->flags & CXD2841ER_USE_GATECTRL) && fe->ops.i2c_gate_ctrl)
+		fe->ops.i2c_gate_ctrl(fe, 0);
+
+	return 0;
+}
+
 static int cxd2841er_dvbs2_set_symbol_rate(struct cxd2841er_priv *priv,
 					   u32 symbol_rate)
 {
@@ -884,6 +914,18 @@ static void cxd2841er_set_ts_clock_mode(struct cxd2841er_priv *priv,
 
 	/*
 	 * slave    Bank    Addr    Bit    default    Name
+	 * <SLV-T>  00h     C4h     [1:0]  2'b??      OSERCKMODE
+	 */
+	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xc4,
+		((priv->flags & CXD2841ER_TS_SERIAL) ? 0x01 : 0x00), 0x03);
+	/*
+	 * slave    Bank    Addr    Bit    default    Name
+	 * <SLV-T>  00h     D1h     [1:0]  2'b??      OSERDUTYMODE
+	 */
+	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xd1,
+		((priv->flags & CXD2841ER_TS_SERIAL) ? 0x01 : 0x00), 0x03);
+	/*
+	 * slave    Bank    Addr    Bit    default    Name
 	 * <SLV-T>  00h     D9h     [7:0]  8'h08      OTSCKPERIOD
 	 */
 	cxd2841er_write_reg(priv, I2C_SLVT, 0xd9, 0x08);
@@ -897,7 +939,8 @@ static void cxd2841er_set_ts_clock_mode(struct cxd2841er_priv *priv,
 	 * slave    Bank    Addr    Bit    default    Name
 	 * <SLV-T>  00h     33h     [1:0]  2'b01      OREG_CKSEL_TSIF
 	 */
-	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0x33, 0x00, 0x03);
+	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0x33,
+		((priv->flags & CXD2841ER_TS_SERIAL) ? 0x01 : 0x00), 0x03);
 	/*
 	 * Enable TS IF Clock
 	 * slave    Bank    Addr    Bit    default    Name
@@ -1421,11 +1464,11 @@ static int cxd2841er_read_ber_i(struct cxd2841er_priv *priv,
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x60);
 	cxd2841er_read_regs(priv, I2C_SLVT, 0x5B, pktnum, sizeof(pktnum));
 	cxd2841er_read_regs(priv, I2C_SLVT, 0x16, data, sizeof(data));
+	cxd2841er_unfreeze_regs(priv);
 
 	if (!pktnum[0] && !pktnum[1]) {
 		dev_dbg(&priv->i2c->dev,
 				"%s(): no valid BER data\n", __func__);
-		cxd2841er_unfreeze_regs(priv);
 		return -EINVAL;
 	}
 
@@ -1435,7 +1478,6 @@ static int cxd2841er_read_ber_i(struct cxd2841er_priv *priv,
 	dev_dbg(&priv->i2c->dev, "%s(): bit_error=%u bit_count=%u\n",
 			__func__, *bit_error, *bit_count);
 
-	cxd2841er_unfreeze_regs(priv);
 	return 0;
 }
 
@@ -1645,6 +1687,8 @@ static u32 cxd2841er_dvbs_read_snr(struct cxd2841er_priv *priv,
 	 * <SLV-T>    A1h       12h       [7:0]   ICPM_QUICKCNDT[7:0]
 	 */
 	cxd2841er_read_regs(priv, I2C_SLVT, 0x10, data, 3);
+	cxd2841er_unfreeze_regs(priv);
+
 	if (data[0] & 0x01) {
 		value = ((u32)(data[1] & 0x1F) << 8) | (u32)(data[2] & 0xFF);
 		min_index = 0;
@@ -1687,11 +1731,9 @@ static u32 cxd2841er_dvbs_read_snr(struct cxd2841er_priv *priv,
 	} else {
 		dev_dbg(&priv->i2c->dev,
 			"%s(): no data available\n", __func__);
-		cxd2841er_unfreeze_regs(priv);
 		return -EINVAL;
 	}
 done:
-	cxd2841er_unfreeze_regs(priv);
 	*snr = res;
 	return 0;
 }
@@ -1720,12 +1762,12 @@ static int cxd2841er_read_snr_c(struct cxd2841er_priv *priv, u32 *snr)
 	cxd2841er_read_regs(priv, I2C_SLVT, 0x19, data, 1);
 	qam = (enum sony_dvbc_constellation_t) (data[0] & 0x07);
 	cxd2841er_read_regs(priv, I2C_SLVT, 0x4C, data, 2);
+	cxd2841er_unfreeze_regs(priv);
 
 	reg = ((u32)(data[0]&0x1f) << 8) | (u32)data[1];
 	if (reg == 0) {
 		dev_dbg(&priv->i2c->dev,
 				"%s(): reg value out of range\n", __func__);
-		cxd2841er_unfreeze_regs(priv);
 		return 0;
 	}
 
@@ -1746,11 +1788,9 @@ static int cxd2841er_read_snr_c(struct cxd2841er_priv *priv, u32 *snr)
 		*snr = -88 * (int32_t)sony_log(reg) + 86999;
 		break;
 	default:
-		cxd2841er_unfreeze_regs(priv);
 		return -EINVAL;
 	}
 
-	cxd2841er_unfreeze_regs(priv);
 	return 0;
 }
 
@@ -1769,17 +1809,17 @@ static int cxd2841er_read_snr_t(struct cxd2841er_priv *priv, u32 *snr)
 	cxd2841er_freeze_regs(priv);
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x10);
 	cxd2841er_read_regs(priv, I2C_SLVT, 0x28, data, sizeof(data));
+	cxd2841er_unfreeze_regs(priv);
+
 	reg = ((u32)data[0] << 8) | (u32)data[1];
 	if (reg == 0) {
 		dev_dbg(&priv->i2c->dev,
 			"%s(): reg value out of range\n", __func__);
-		cxd2841er_unfreeze_regs(priv);
 		return 0;
 	}
 	if (reg > 4996)
 		reg = 4996;
-	*snr = 10000 * ((intlog10(reg) - intlog10(5350 - reg)) >> 24) + 28500;
-	cxd2841er_unfreeze_regs(priv);
+	*snr = 100 * ((INTLOG10X100(reg) - INTLOG10X100(5350 - reg)) + 285);
 	return 0;
 }
 
@@ -1798,18 +1838,17 @@ static int cxd2841er_read_snr_t2(struct cxd2841er_priv *priv, u32 *snr)
 	cxd2841er_freeze_regs(priv);
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x20);
 	cxd2841er_read_regs(priv, I2C_SLVT, 0x28, data, sizeof(data));
+	cxd2841er_unfreeze_regs(priv);
+
 	reg = ((u32)data[0] << 8) | (u32)data[1];
 	if (reg == 0) {
 		dev_dbg(&priv->i2c->dev,
 			"%s(): reg value out of range\n", __func__);
-		cxd2841er_unfreeze_regs(priv);
 		return 0;
 	}
 	if (reg > 10876)
 		reg = 10876;
-	*snr = 10000 * ((intlog10(reg) -
-		intlog10(12600 - reg)) >> 24) + 32000;
-	cxd2841er_unfreeze_regs(priv);
+	*snr = 100 * ((INTLOG10X100(reg) - INTLOG10X100(12600 - reg)) + 320);
 	return 0;
 }
 
@@ -1829,15 +1868,15 @@ static int cxd2841er_read_snr_i(struct cxd2841er_priv *priv, u32 *snr)
 	cxd2841er_freeze_regs(priv);
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x60);
 	cxd2841er_read_regs(priv, I2C_SLVT, 0x28, data, sizeof(data));
+	cxd2841er_unfreeze_regs(priv);
+
 	reg = ((u32)data[0] << 8) | (u32)data[1];
 	if (reg == 0) {
 		dev_dbg(&priv->i2c->dev,
 				"%s(): reg value out of range\n", __func__);
-		cxd2841er_unfreeze_regs(priv);
 		return 0;
 	}
 	*snr = 10000 * (intlog10(reg) >> 24) - 9031;
-	cxd2841er_unfreeze_regs(priv);
 	return 0;
 }
 
@@ -2136,7 +2175,7 @@ static int cxd2841er_dvbt2_set_plp_config(struct cxd2841er_priv *priv,
 static int cxd2841er_sleep_tc_to_active_t2_band(struct cxd2841er_priv *priv,
 						u32 bandwidth)
 {
-	u32 iffreq;
+	u32 iffreq, ifhz;
 	u8 data[MAX_WRITE_REGSIZE];
 
 	const uint8_t nominalRate8bw[3][5] = {
@@ -2239,10 +2278,12 @@ static int cxd2841er_sleep_tc_to_active_t2_band(struct cxd2841er_priv *priv,
 		/* Group delay equaliser settings for
 		 * ASCOT2D, ASCOT2E and ASCOT3 tuners
 		 */
-		cxd2841er_write_regs(priv, I2C_SLVT,
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(priv, I2C_SLVT,
 				0xA6, itbCoef8bw[priv->xtal], 14);
 		/* <IF freq setting> */
-		iffreq = MAKE_IFFREQ_CONFIG_XTAL(priv->xtal, 4.80);
+		ifhz = cxd2841er_get_if_hz(priv, 4800000);
+		iffreq = cxd2841er_calc_iffreq_xtal(priv->xtal, ifhz);
 		data[0] = (u8) ((iffreq >> 16) & 0xff);
 		data[1] = (u8)((iffreq >> 8) & 0xff);
 		data[2] = (u8)(iffreq & 0xff);
@@ -2267,10 +2308,12 @@ static int cxd2841er_sleep_tc_to_active_t2_band(struct cxd2841er_priv *priv,
 		/* Group delay equaliser settings for
 		 * ASCOT2D, ASCOT2E and ASCOT3 tuners
 		 */
-		cxd2841er_write_regs(priv, I2C_SLVT,
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(priv, I2C_SLVT,
 				0xA6, itbCoef7bw[priv->xtal], 14);
 		/* <IF freq setting> */
-		iffreq = MAKE_IFFREQ_CONFIG_XTAL(priv->xtal, 4.20);
+		ifhz = cxd2841er_get_if_hz(priv, 4200000);
+		iffreq = cxd2841er_calc_iffreq_xtal(priv->xtal, ifhz);
 		data[0] = (u8) ((iffreq >> 16) & 0xff);
 		data[1] = (u8)((iffreq >> 8) & 0xff);
 		data[2] = (u8)(iffreq & 0xff);
@@ -2295,10 +2338,12 @@ static int cxd2841er_sleep_tc_to_active_t2_band(struct cxd2841er_priv *priv,
 		/* Group delay equaliser settings for
 		 * ASCOT2D, ASCOT2E and ASCOT3 tuners
 		 */
-		cxd2841er_write_regs(priv, I2C_SLVT,
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(priv, I2C_SLVT,
 				0xA6, itbCoef6bw[priv->xtal], 14);
 		/* <IF freq setting> */
-		iffreq = MAKE_IFFREQ_CONFIG_XTAL(priv->xtal, 3.60);
+		ifhz = cxd2841er_get_if_hz(priv, 3600000);
+		iffreq = cxd2841er_calc_iffreq_xtal(priv->xtal, ifhz);
 		data[0] = (u8) ((iffreq >> 16) & 0xff);
 		data[1] = (u8)((iffreq >> 8) & 0xff);
 		data[2] = (u8)(iffreq & 0xff);
@@ -2323,10 +2368,12 @@ static int cxd2841er_sleep_tc_to_active_t2_band(struct cxd2841er_priv *priv,
 		/* Group delay equaliser settings for
 		 * ASCOT2D, ASCOT2E and ASCOT3 tuners
 		 */
-		cxd2841er_write_regs(priv, I2C_SLVT,
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(priv, I2C_SLVT,
 				0xA6, itbCoef5bw[priv->xtal], 14);
 		/* <IF freq setting> */
-		iffreq = MAKE_IFFREQ_CONFIG_XTAL(priv->xtal, 3.60);
+		ifhz = cxd2841er_get_if_hz(priv, 3600000);
+		iffreq = cxd2841er_calc_iffreq_xtal(priv->xtal, ifhz);
 		data[0] = (u8) ((iffreq >> 16) & 0xff);
 		data[1] = (u8)((iffreq >> 8) & 0xff);
 		data[2] = (u8)(iffreq & 0xff);
@@ -2351,10 +2398,12 @@ static int cxd2841er_sleep_tc_to_active_t2_band(struct cxd2841er_priv *priv,
 		/* Group delay equaliser settings for
 		 * ASCOT2D, ASCOT2E and ASCOT3 tuners
 		 */
-		cxd2841er_write_regs(priv, I2C_SLVT,
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(priv, I2C_SLVT,
 				0xA6, itbCoef17bw[priv->xtal], 14);
 		/* <IF freq setting> */
-		iffreq = MAKE_IFFREQ_CONFIG_XTAL(priv->xtal, 3.50);
+		ifhz = cxd2841er_get_if_hz(priv, 3500000);
+		iffreq = cxd2841er_calc_iffreq_xtal(priv->xtal, ifhz);
 		data[0] = (u8) ((iffreq >> 16) & 0xff);
 		data[1] = (u8)((iffreq >> 8) & 0xff);
 		data[2] = (u8)(iffreq & 0xff);
@@ -2373,7 +2422,7 @@ static int cxd2841er_sleep_tc_to_active_t_band(
 		struct cxd2841er_priv *priv, u32 bandwidth)
 {
 	u8 data[MAX_WRITE_REGSIZE];
-	u32 iffreq;
+	u32 iffreq, ifhz;
 	u8 nominalRate8bw[3][5] = {
 		/* TRCG Nominal Rate [37:0] */
 		{0x11, 0xF0, 0x00, 0x00, 0x00}, /* 20.5MHz XTal */
@@ -2450,10 +2499,12 @@ static int cxd2841er_sleep_tc_to_active_t_band(
 		/* Group delay equaliser settings for
 		 * ASCOT2D, ASCOT2E and ASCOT3 tuners
 		*/
-		cxd2841er_write_regs(priv, I2C_SLVT,
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(priv, I2C_SLVT,
 				0xA6, itbCoef8bw[priv->xtal], 14);
 		/* <IF freq setting> */
-		iffreq = MAKE_IFFREQ_CONFIG_XTAL(priv->xtal, 4.80);
+		ifhz = cxd2841er_get_if_hz(priv, 4800000);
+		iffreq = cxd2841er_calc_iffreq_xtal(priv->xtal, ifhz);
 		data[0] = (u8) ((iffreq >> 16) & 0xff);
 		data[1] = (u8)((iffreq >> 8) & 0xff);
 		data[2] = (u8)(iffreq & 0xff);
@@ -2485,10 +2536,12 @@ static int cxd2841er_sleep_tc_to_active_t_band(
 		/* Group delay equaliser settings for
 		 * ASCOT2D, ASCOT2E and ASCOT3 tuners
 		*/
-		cxd2841er_write_regs(priv, I2C_SLVT,
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(priv, I2C_SLVT,
 				0xA6, itbCoef7bw[priv->xtal], 14);
 		/* <IF freq setting> */
-		iffreq = MAKE_IFFREQ_CONFIG_XTAL(priv->xtal, 4.20);
+		ifhz = cxd2841er_get_if_hz(priv, 4200000);
+		iffreq = cxd2841er_calc_iffreq_xtal(priv->xtal, ifhz);
 		data[0] = (u8) ((iffreq >> 16) & 0xff);
 		data[1] = (u8)((iffreq >> 8) & 0xff);
 		data[2] = (u8)(iffreq & 0xff);
@@ -2520,10 +2573,12 @@ static int cxd2841er_sleep_tc_to_active_t_band(
 		/* Group delay equaliser settings for
 		 * ASCOT2D, ASCOT2E and ASCOT3 tuners
 		*/
-		cxd2841er_write_regs(priv, I2C_SLVT,
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(priv, I2C_SLVT,
 				0xA6, itbCoef6bw[priv->xtal], 14);
 		/* <IF freq setting> */
-		iffreq = MAKE_IFFREQ_CONFIG_XTAL(priv->xtal, 3.60);
+		ifhz = cxd2841er_get_if_hz(priv, 3600000);
+		iffreq = cxd2841er_calc_iffreq_xtal(priv->xtal, ifhz);
 		data[0] = (u8) ((iffreq >> 16) & 0xff);
 		data[1] = (u8)((iffreq >> 8) & 0xff);
 		data[2] = (u8)(iffreq & 0xff);
@@ -2555,10 +2610,12 @@ static int cxd2841er_sleep_tc_to_active_t_band(
 		/* Group delay equaliser settings for
 		 * ASCOT2D, ASCOT2E and ASCOT3 tuners
 		*/
-		cxd2841er_write_regs(priv, I2C_SLVT,
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(priv, I2C_SLVT,
 				0xA6, itbCoef5bw[priv->xtal], 14);
 		/* <IF freq setting> */
-		iffreq = MAKE_IFFREQ_CONFIG_XTAL(priv->xtal, 3.60);
+		ifhz = cxd2841er_get_if_hz(priv, 3600000);
+		iffreq = cxd2841er_calc_iffreq_xtal(priv->xtal, ifhz);
 		data[0] = (u8) ((iffreq >> 16) & 0xff);
 		data[1] = (u8)((iffreq >> 8) & 0xff);
 		data[2] = (u8)(iffreq & 0xff);
@@ -2591,7 +2648,7 @@ static int cxd2841er_sleep_tc_to_active_t_band(
 static int cxd2841er_sleep_tc_to_active_i_band(
 		struct cxd2841er_priv *priv, u32 bandwidth)
 {
-	u32 iffreq;
+	u32 iffreq, ifhz;
 	u8 data[3];
 
 	/* TRCG Nominal Rate */
@@ -2656,11 +2713,13 @@ static int cxd2841er_sleep_tc_to_active_i_band(
 		cxd2841er_write_regs(priv, I2C_SLVT,
 				0x9F, nominalRate8bw[priv->xtal], 5);
 		/*  Group delay equaliser settings for ASCOT tuners optimized */
-		cxd2841er_write_regs(priv, I2C_SLVT,
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(priv, I2C_SLVT,
 				0xA6, itbCoef8bw[priv->xtal], 14);
 
 		/* IF freq setting */
-		iffreq = MAKE_IFFREQ_CONFIG_XTAL(priv->xtal, 4.75);
+		ifhz = cxd2841er_get_if_hz(priv, 4750000);
+		iffreq = cxd2841er_calc_iffreq_xtal(priv->xtal, ifhz);
 		data[0] = (u8) ((iffreq >> 16) & 0xff);
 		data[1] = (u8)((iffreq >> 8) & 0xff);
 		data[2] = (u8)(iffreq & 0xff);
@@ -2685,11 +2744,13 @@ static int cxd2841er_sleep_tc_to_active_i_band(
 		cxd2841er_write_regs(priv, I2C_SLVT,
 				0x9F, nominalRate7bw[priv->xtal], 5);
 		/*  Group delay equaliser settings for ASCOT tuners optimized */
-		cxd2841er_write_regs(priv, I2C_SLVT,
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(priv, I2C_SLVT,
 				0xA6, itbCoef7bw[priv->xtal], 14);
 
 		/* IF freq setting */
-		iffreq = MAKE_IFFREQ_CONFIG_XTAL(priv->xtal, 4.15);
+		ifhz = cxd2841er_get_if_hz(priv, 4150000);
+		iffreq = cxd2841er_calc_iffreq_xtal(priv->xtal, ifhz);
 		data[0] = (u8) ((iffreq >> 16) & 0xff);
 		data[1] = (u8)((iffreq >> 8) & 0xff);
 		data[2] = (u8)(iffreq & 0xff);
@@ -2714,11 +2775,13 @@ static int cxd2841er_sleep_tc_to_active_i_band(
 		cxd2841er_write_regs(priv, I2C_SLVT,
 				0x9F, nominalRate6bw[priv->xtal], 5);
 		/*  Group delay equaliser settings for ASCOT tuners optimized */
-		cxd2841er_write_regs(priv, I2C_SLVT,
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(priv, I2C_SLVT,
 				0xA6, itbCoef6bw[priv->xtal], 14);
 
 		/* IF freq setting */
-		iffreq = MAKE_IFFREQ_CONFIG_XTAL(priv->xtal, 3.55);
+		ifhz = cxd2841er_get_if_hz(priv, 3550000);
+		iffreq = cxd2841er_calc_iffreq_xtal(priv->xtal, ifhz);
 		data[0] = (u8) ((iffreq >> 16) & 0xff);
 		data[1] = (u8)((iffreq >> 8) & 0xff);
 		data[2] = (u8)(iffreq & 0xff);
@@ -2761,7 +2824,7 @@ static int cxd2841er_sleep_tc_to_active_c_band(struct cxd2841er_priv *priv,
 		0x27, 0xA7, 0x28, 0xB3, 0x02, 0xF0, 0x01, 0xE8,
 		0x00, 0xCF, 0x00, 0xE6, 0x23, 0xA4 };
 	u8 b10_b6[3];
-	u32 iffreq;
+	u32 iffreq, ifhz;
 
 	if (bandwidth != 6000000 &&
 			bandwidth != 7000000 &&
@@ -2776,16 +2839,20 @@ static int cxd2841er_sleep_tc_to_active_c_band(struct cxd2841er_priv *priv,
 	switch (bandwidth) {
 	case 8000000:
 	case 7000000:
-		cxd2841er_write_regs(
-			priv, I2C_SLVT, 0xa6,
-			bw7_8mhz_b10_a6, sizeof(bw7_8mhz_b10_a6));
-		iffreq = MAKE_IFFREQ_CONFIG(4.9);
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(
+				priv, I2C_SLVT, 0xa6,
+				bw7_8mhz_b10_a6, sizeof(bw7_8mhz_b10_a6));
+		ifhz = cxd2841er_get_if_hz(priv, 4900000);
+		iffreq = cxd2841er_calc_iffreq(ifhz);
 		break;
 	case 6000000:
-		cxd2841er_write_regs(
-			priv, I2C_SLVT, 0xa6,
-			bw6mhz_b10_a6, sizeof(bw6mhz_b10_a6));
-		iffreq = MAKE_IFFREQ_CONFIG(3.7);
+		if (priv->flags & CXD2841ER_ASCOT)
+			cxd2841er_write_regs(
+				priv, I2C_SLVT, 0xa6,
+				bw6mhz_b10_a6, sizeof(bw6mhz_b10_a6));
+		ifhz = cxd2841er_get_if_hz(priv, 3700000);
+		iffreq = cxd2841er_calc_iffreq(ifhz);
 		break;
 	default:
 		dev_err(&priv->i2c->dev, "%s(): unsupported bandwidth %d\n",
@@ -2872,8 +2939,9 @@ static int cxd2841er_sleep_tc_to_active_t(struct cxd2841er_priv *priv,
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x6a, 0x50);
 	/* Set SLV-T Bank : 0x10 */
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x10);
-	/* ASCOT setting ON */
-	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xa5, 0x01, 0x01);
+	/* ASCOT setting */
+	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xa5,
+		((priv->flags & CXD2841ER_ASCOT) ? 0x01 : 0x00), 0x01);
 	/* Set SLV-T Bank : 0x18 */
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x18);
 	/* Pre-RS BER moniter setting */
@@ -2950,8 +3018,9 @@ static int cxd2841er_sleep_tc_to_active_t2(struct cxd2841er_priv *priv,
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x6a, 0x50);
 	/* Set SLV-T Bank : 0x10 */
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x10);
-	/* ASCOT setting ON */
-	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xa5, 0x01, 0x01);
+	/* ASCOT setting */
+	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xa5,
+		((priv->flags & CXD2841ER_ASCOT) ? 0x01 : 0x00), 0x01);
 	/* Set SLV-T Bank : 0x20 */
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x20);
 	/* Acquisition optimization setting */
@@ -3088,8 +3157,9 @@ static int cxd2841er_sleep_tc_to_active_i(struct cxd2841er_priv *priv,
 	cxd2841er_write_regs(priv, I2C_SLVT, 0x43, data, 2);
 	/* Enable ADC 4 */
 	cxd2841er_write_reg(priv, I2C_SLVX, 0x18, 0x00);
-	/* ASCOT setting ON */
-	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xa5, 0x01, 0x01);
+	/* ASCOT setting */
+	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xa5,
+		((priv->flags & CXD2841ER_ASCOT) ? 0x01 : 0x00), 0x01);
 	/* FEC Auto Recovery setting */
 	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0x30, 0x01, 0x01);
 	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0x31, 0x00, 0x01);
@@ -3173,8 +3243,9 @@ static int cxd2841er_sleep_tc_to_active_c(struct cxd2841er_priv *priv,
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x6a, 0x48);
 	/* Set SLV-T Bank : 0x10 */
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x10);
-	/* ASCOT setting ON */
-	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xa5, 0x01, 0x01);
+	/* ASCOT setting */
+	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xa5,
+		((priv->flags & CXD2841ER_ASCOT) ? 0x01 : 0x00), 0x01);
 	/* Set SLV-T Bank : 0x40 */
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x40);
 	/* Demod setting */
@@ -3236,6 +3307,10 @@ static int cxd2841er_set_frontend_s(struct dvb_frontend *fe)
 		__func__,
 		(p->delivery_system == SYS_DVBS ? "DVB-S" : "DVB-S2"),
 		 p->frequency, symbol_rate, priv->xtal);
+
+	if (priv->flags & CXD2841ER_EARLY_TUNE)
+		cxd2841er_tuner_set(fe);
+
 	switch (priv->state) {
 	case STATE_SLEEP_S:
 		ret = cxd2841er_sleep_s_to_active_s(
@@ -3254,12 +3329,10 @@ static int cxd2841er_set_frontend_s(struct dvb_frontend *fe)
 		dev_dbg(&priv->i2c->dev, "%s(): tune failed\n", __func__);
 		goto done;
 	}
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 1);
-	if (fe->ops.tuner_ops.set_params)
-		fe->ops.tuner_ops.set_params(fe);
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0);
+
+	if (!(priv->flags & CXD2841ER_EARLY_TUNE))
+		cxd2841er_tuner_set(fe);
+
 	cxd2841er_tune_done(priv);
 	timeout = ((3000000 + (symbol_rate - 1)) / symbol_rate) + 150;
 	for (i = 0; i < timeout / CXD2841ER_DVBS_POLLING_INVL; i++) {
@@ -3298,6 +3371,10 @@ static int cxd2841er_set_frontend_tc(struct dvb_frontend *fe)
 
 	dev_dbg(&priv->i2c->dev, "%s() delivery_system=%d bandwidth_hz=%d\n",
 		 __func__, p->delivery_system, p->bandwidth_hz);
+
+	if (priv->flags & CXD2841ER_EARLY_TUNE)
+		cxd2841er_tuner_set(fe);
+
 	if (p->delivery_system == SYS_DVBT) {
 		priv->system = SYS_DVBT;
 		switch (priv->state) {
@@ -3379,13 +3456,15 @@ static int cxd2841er_set_frontend_tc(struct dvb_frontend *fe)
 	}
 	if (ret)
 		goto done;
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 1);
-	if (fe->ops.tuner_ops.set_params)
-		fe->ops.tuner_ops.set_params(fe);
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0);
+
+	if (!(priv->flags & CXD2841ER_EARLY_TUNE))
+		cxd2841er_tuner_set(fe);
+
 	cxd2841er_tune_done(priv);
+
+	if (priv->flags & CXD2841ER_NO_WAIT_LOCK)
+		goto done;
+
 	timeout = 2500;
 	while (timeout > 0) {
 		ret = cxd2841er_read_status_tc(fe, &status);
@@ -3705,14 +3784,20 @@ static int cxd2841er_init_tc(struct dvb_frontend *fe)
 	dev_dbg(&priv->i2c->dev, "%s() bandwidth_hz=%d\n",
 			__func__, p->bandwidth_hz);
 	cxd2841er_shutdown_to_sleep_tc(priv);
-	/* SONY_DEMOD_CONFIG_IFAGCNEG = 1 */
+	/* SONY_DEMOD_CONFIG_IFAGCNEG = 1 (0 for NO_AGCNEG */
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x10);
-	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xcb, 0x40, 0x40);
+	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xcb,
+		((priv->flags & CXD2841ER_NO_AGCNEG) ? 0x00 : 0x40), 0x40);
 	/* SONY_DEMOD_CONFIG_IFAGC_ADC_FS = 0 */
 	cxd2841er_write_reg(priv, I2C_SLVT, 0xcd, 0x50);
 	/* SONY_DEMOD_CONFIG_PARALLEL_SEL = 1 */
 	cxd2841er_write_reg(priv, I2C_SLVT, 0x00, 0x00);
-	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xc4, 0x00, 0x80);
+	cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xc4,
+		((priv->flags & CXD2841ER_TS_SERIAL) ? 0x80 : 0x00), 0x80);
+
+	/* clear TSCFG bits 3+4 */
+	if (priv->flags & CXD2841ER_TSBITS)
+		cxd2841er_set_reg_bits(priv, I2C_SLVT, 0xc4, 0x00, 0x18);
 
 	cxd2841er_init_stats(fe);
 
@@ -3740,6 +3825,7 @@ static struct dvb_frontend *cxd2841er_attach(struct cxd2841er_config *cfg,
 	priv->i2c_addr_slvx = (cfg->i2c_addr + 4) >> 1;
 	priv->i2c_addr_slvt = (cfg->i2c_addr) >> 1;
 	priv->xtal = cfg->xtal;
+	priv->flags = cfg->flags;
 	priv->frontend.demodulator_priv = priv;
 	dev_info(&priv->i2c->dev,
 		"%s(): I2C adapter %p SLVX addr %x SLVT addr %x\n",
@@ -3747,16 +3833,39 @@ static struct dvb_frontend *cxd2841er_attach(struct cxd2841er_config *cfg,
 		priv->i2c_addr_slvx, priv->i2c_addr_slvt);
 	chip_id = cxd2841er_chip_id(priv);
 	switch (chip_id) {
+	case CXD2837ER_CHIP_ID:
+		snprintf(cxd2841er_t_c_ops.info.name, 128,
+				"Sony CXD2837ER DVB-T/T2/C demodulator");
+		name = "CXD2837ER";
+		type = "C/T/T2";
+		break;
+	case CXD2838ER_CHIP_ID:
+		snprintf(cxd2841er_t_c_ops.info.name, 128,
+				"Sony CXD2838ER ISDB-T demodulator");
+		cxd2841er_t_c_ops.delsys[0] = SYS_ISDBT;
+		cxd2841er_t_c_ops.delsys[1] = SYS_UNDEFINED;
+		cxd2841er_t_c_ops.delsys[2] = SYS_UNDEFINED;
+		name = "CXD2838ER";
+		type = "ISDB-T";
+		break;
 	case CXD2841ER_CHIP_ID:
 		snprintf(cxd2841er_t_c_ops.info.name, 128,
 				"Sony CXD2841ER DVB-T/T2/C demodulator");
 		name = "CXD2841ER";
+		type = "T/T2/C/ISDB-T";
+		break;
+	case CXD2843ER_CHIP_ID:
+		snprintf(cxd2841er_t_c_ops.info.name, 128,
+				"Sony CXD2843ER DVB-T/T2/C/C2 demodulator");
+		name = "CXD2843ER";
+		type = "C/C2/T/T2";
 		break;
 	case CXD2854ER_CHIP_ID:
 		snprintf(cxd2841er_t_c_ops.info.name, 128,
 				"Sony CXD2854ER DVB-T/T2/C and ISDB-T demodulator");
 		cxd2841er_t_c_ops.delsys[3] = SYS_ISDBT;
 		name = "CXD2854ER";
+		type = "C/C2/T/T2/ISDB-T";
 		break;
 	default:
 		dev_err(&priv->i2c->dev, "%s(): invalid chip ID 0x%02x\n",
@@ -3776,7 +3885,6 @@ static struct dvb_frontend *cxd2841er_attach(struct cxd2841er_config *cfg,
 		memcpy(&priv->frontend.ops,
 			&cxd2841er_t_c_ops,
 			sizeof(struct dvb_frontend_ops));
-		type = "T/T2/C/ISDB-T";
 	}
 
 	dev_info(&priv->i2c->dev,
diff --git a/drivers/media/dvb-frontends/cxd2841er.h b/drivers/media/dvb-frontends/cxd2841er.h
index 7f1acfb8f4f5..dc32f5fb6662 100644
--- a/drivers/media/dvb-frontends/cxd2841er.h
+++ b/drivers/media/dvb-frontends/cxd2841er.h
@@ -24,6 +24,15 @@
 
 #include <linux/dvb/frontend.h>
 
+#define CXD2841ER_USE_GATECTRL	1	/* bit 0 */
+#define CXD2841ER_AUTO_IFHZ	2	/* bit 1 */
+#define CXD2841ER_TS_SERIAL	4	/* bit 2 */
+#define CXD2841ER_ASCOT		8	/* bit 3 */
+#define CXD2841ER_EARLY_TUNE	16	/* bit 4 */
+#define CXD2841ER_NO_WAIT_LOCK	32	/* bit 5 */
+#define CXD2841ER_NO_AGCNEG	64	/* bit 6 */
+#define CXD2841ER_TSBITS	128	/* bit 7 */
+
 enum cxd2841er_xtal {
 	SONY_XTAL_20500, /* 20.5 MHz */
 	SONY_XTAL_24000, /* 24 MHz */
@@ -33,6 +42,7 @@ enum cxd2841er_xtal {
 struct cxd2841er_config {
 	u8	i2c_addr;
 	enum cxd2841er_xtal	xtal;
+	u32	flags;
 };
 
 #if IS_REACHABLE(CONFIG_DVB_CXD2841ER)
diff --git a/drivers/media/dvb-frontends/cxd2841er_priv.h b/drivers/media/dvb-frontends/cxd2841er_priv.h
index 0bbce451149f..6a7126480889 100644
--- a/drivers/media/dvb-frontends/cxd2841er_priv.h
+++ b/drivers/media/dvb-frontends/cxd2841er_priv.h
@@ -25,7 +25,10 @@
 #define I2C_SLVX			0
 #define I2C_SLVT			1
 
+#define CXD2837ER_CHIP_ID		0xb1
+#define CXD2838ER_CHIP_ID		0xb0
 #define CXD2841ER_CHIP_ID		0xa7
+#define CXD2843ER_CHIP_ID		0xa4
 #define CXD2854ER_CHIP_ID		0xc1
 
 #define CXD2841ER_DVBS_POLLING_INVL	10
diff --git a/drivers/media/dvb-frontends/dib7000p.c b/drivers/media/dvb-frontends/dib7000p.c
index 3815ea515364..1caa04d8f60f 100644
--- a/drivers/media/dvb-frontends/dib7000p.c
+++ b/drivers/media/dvb-frontends/dib7000p.c
@@ -279,10 +279,10 @@ static int dib7000p_set_power_mode(struct dib7000p_state *state, enum dib7000p_p
 		if (state->version != SOC7090)
 			reg_1280 &= ~((1 << 11));
 		reg_1280 &= ~(1 << 6);
-		/* fall through wanted to enable the interfaces */
-
+		/* fall-through */
+	case DIB7000P_POWER_INTERFACE_ONLY:
 		/* just leave power on the control-interfaces: GPIO and (I2C or SDIO) */
-	case DIB7000P_POWER_INTERFACE_ONLY:	/* TODO power up either SDIO or I2C */
+		/* TODO power up either SDIO or I2C */
 		if (state->version == SOC7090)
 			reg_1280 &= ~((1 << 7) | (1 << 5));
 		else
diff --git a/drivers/media/dvb-frontends/drx39xyj/drxj.c b/drivers/media/dvb-frontends/drx39xyj/drxj.c
index daeaf965dd56..14040c915dbb 100644
--- a/drivers/media/dvb-frontends/drx39xyj/drxj.c
+++ b/drivers/media/dvb-frontends/drx39xyj/drxj.c
@@ -2837,7 +2837,8 @@ ctrl_set_cfg_mpeg_output(struct drx_demod_instance *demod, struct drx_cfg_mpeg_o
 			/* coef = 188/204                          */
 			max_bit_rate =
 			    (ext_attr->curr_symbol_rate / 8) * nr_bits * 188;
-			/* pass through b/c Annex A/c need following settings */
+			/* pass through as b/c Annex A/c need following settings */
+			/* fall-through */
 		case DRX_STANDARD_ITU_B:
 			rc = drxj_dap_write_reg16(dev_addr, FEC_OC_FCT_USAGE__A, FEC_OC_FCT_USAGE__PRE, 0);
 			if (rc != 0) {
@@ -4776,9 +4777,9 @@ set_frequency(struct drx_demod_instance *demod,
 	   No need to account for mirroring on RF
 	 */
 	switch (ext_attr->standard) {
-	case DRX_STANDARD_ITU_A:	/* fallthrough */
-	case DRX_STANDARD_ITU_C:	/* fallthrough */
-	case DRX_STANDARD_PAL_SECAM_LP:	/* fallthrough */
+	case DRX_STANDARD_ITU_A:
+	case DRX_STANDARD_ITU_C:
+	case DRX_STANDARD_PAL_SECAM_LP:
 	case DRX_STANDARD_8VSB:
 		select_pos_image = true;
 		break;
@@ -4787,11 +4788,12 @@ set_frequency(struct drx_demod_instance *demod,
 		   Sound carrier is already 3Mhz above centre frequency due
 		   to tuner setting so now add an extra shift of 1MHz... */
 		fm_frequency_shift = 1000;
-	case DRX_STANDARD_ITU_B:	/* fallthrough */
-	case DRX_STANDARD_NTSC:	/* fallthrough */
-	case DRX_STANDARD_PAL_SECAM_BG:	/* fallthrough */
-	case DRX_STANDARD_PAL_SECAM_DK:	/* fallthrough */
-	case DRX_STANDARD_PAL_SECAM_I:	/* fallthrough */
+		/*fall through */
+	case DRX_STANDARD_ITU_B:
+	case DRX_STANDARD_NTSC:
+	case DRX_STANDARD_PAL_SECAM_BG:
+	case DRX_STANDARD_PAL_SECAM_DK:
+	case DRX_STANDARD_PAL_SECAM_I:
 	case DRX_STANDARD_PAL_SECAM_L:
 		select_pos_image = false;
 		break;
diff --git a/drivers/media/dvb-frontends/drxd_hard.c b/drivers/media/dvb-frontends/drxd_hard.c
index 71910561005f..17638e08835a 100644
--- a/drivers/media/dvb-frontends/drxd_hard.c
+++ b/drivers/media/dvb-frontends/drxd_hard.c
@@ -1517,12 +1517,14 @@ static int SetDeviceTypeId(struct drxd_state *state)
 			switch (deviceId) {
 			case 4:
 				state->diversity = 1;
+				/* fall through */
 			case 3:
 			case 7:
 				state->PGA = 1;
 				break;
 			case 6:
 				state->diversity = 1;
+				/* fall through */
 			case 5:
 			case 8:
 				break;
@@ -1969,7 +1971,8 @@ static int DRX_Start(struct drxd_state *state, s32 off)
 		switch (p->transmission_mode) {
 		default:	/* Not set, detect it automatically */
 			operationMode |= SC_RA_RAM_OP_AUTO_MODE__M;
-			/* fall through , try first guess DRX_FFTMODE_8K */
+			/* try first guess DRX_FFTMODE_8K */
+			/* fall through */
 		case TRANSMISSION_MODE_8K:
 			transmissionParams |= SC_RA_RAM_OP_PARAM_MODE_8K;
 			if (state->type_A) {
@@ -2143,8 +2146,8 @@ static int DRX_Start(struct drxd_state *state, s32 off)
 		switch (p->modulation) {
 		default:
 			operationMode |= SC_RA_RAM_OP_AUTO_CONST__M;
-			/* fall through , try first guess
-			   DRX_CONSTELLATION_QAM64 */
+			/* try first guess DRX_CONSTELLATION_QAM64 */
+			/* fall through */
 		case QAM_64:
 			transmissionParams |= SC_RA_RAM_OP_PARAM_CONST_QAM64;
 			if (state->type_A) {
@@ -2280,6 +2283,7 @@ static int DRX_Start(struct drxd_state *state, s32 off)
 			break;
 		default:
 			operationMode |= SC_RA_RAM_OP_AUTO_RATE__M;
+			/* fall through */
 		case FEC_2_3:
 			transmissionParams |= SC_RA_RAM_OP_PARAM_RATE_2_3;
 			if (state->type_A) {
diff --git a/drivers/media/dvb-frontends/drxk_hard.c b/drivers/media/dvb-frontends/drxk_hard.c
index 050fe34342d3..48a8aad47a74 100644
--- a/drivers/media/dvb-frontends/drxk_hard.c
+++ b/drivers/media/dvb-frontends/drxk_hard.c
@@ -3271,10 +3271,12 @@ static int dvbt_sc_command(struct drxk_state *state,
 	case OFDM_SC_RA_RAM_CMD_PROGRAM_PARAM:
 		status |= write16(state, OFDM_SC_RA_RAM_PARAM1__A, param1);
 		/* All commands using 1 parameters */
+		/* fall through */
 	case OFDM_SC_RA_RAM_CMD_SET_ECHO_TIMING:
 	case OFDM_SC_RA_RAM_CMD_USER_IO:
 		status |= write16(state, OFDM_SC_RA_RAM_PARAM0__A, param0);
 		/* All commands using 0 parameters */
+		/* fall through */
 	case OFDM_SC_RA_RAM_CMD_GET_OP_PARAM:
 	case OFDM_SC_RA_RAM_CMD_NULL:
 		/* Write command */
@@ -3782,7 +3784,8 @@ static int set_dvbt(struct drxk_state *state, u16 intermediate_freqk_hz,
 	case TRANSMISSION_MODE_AUTO:
 	default:
 		operation_mode |= OFDM_SC_RA_RAM_OP_AUTO_MODE__M;
-		/* fall through , try first guess DRX_FFTMODE_8K */
+		/* try first guess DRX_FFTMODE_8K */
+		/* fall through */
 	case TRANSMISSION_MODE_8K:
 		transmission_params |= OFDM_SC_RA_RAM_OP_PARAM_MODE_8K;
 		break;
@@ -3796,7 +3799,8 @@ static int set_dvbt(struct drxk_state *state, u16 intermediate_freqk_hz,
 	default:
 	case GUARD_INTERVAL_AUTO:
 		operation_mode |= OFDM_SC_RA_RAM_OP_AUTO_GUARD__M;
-		/* fall through , try first guess DRX_GUARD_1DIV4 */
+		/* try first guess DRX_GUARD_1DIV4 */
+		/* fall through */
 	case GUARD_INTERVAL_1_4:
 		transmission_params |= OFDM_SC_RA_RAM_OP_PARAM_GUARD_4;
 		break;
@@ -3817,9 +3821,9 @@ static int set_dvbt(struct drxk_state *state, u16 intermediate_freqk_hz,
 	case HIERARCHY_NONE:
 	default:
 		operation_mode |= OFDM_SC_RA_RAM_OP_AUTO_HIER__M;
-		/* fall through , try first guess SC_RA_RAM_OP_PARAM_HIER_NO */
+		/* try first guess SC_RA_RAM_OP_PARAM_HIER_NO */
 		/* transmission_params |= OFDM_SC_RA_RAM_OP_PARAM_HIER_NO; */
-		/* break; */
+		/* fall through */
 	case HIERARCHY_1:
 		transmission_params |= OFDM_SC_RA_RAM_OP_PARAM_HIER_A1;
 		break;
@@ -3837,7 +3841,8 @@ static int set_dvbt(struct drxk_state *state, u16 intermediate_freqk_hz,
 	case QAM_AUTO:
 	default:
 		operation_mode |= OFDM_SC_RA_RAM_OP_AUTO_CONST__M;
-		/* fall through , try first guess DRX_CONSTELLATION_QAM64 */
+		/* try first guess DRX_CONSTELLATION_QAM64 */
+		/* fall through */
 	case QAM_64:
 		transmission_params |= OFDM_SC_RA_RAM_OP_PARAM_CONST_QAM64;
 		break;
@@ -3880,7 +3885,8 @@ static int set_dvbt(struct drxk_state *state, u16 intermediate_freqk_hz,
 	case FEC_AUTO:
 	default:
 		operation_mode |= OFDM_SC_RA_RAM_OP_AUTO_RATE__M;
-		/* fall through , try first guess DRX_CODERATE_2DIV3 */
+		/* try first guess DRX_CODERATE_2DIV3 */
+		/* fall through */
 	case FEC_2_3:
 		transmission_params |= OFDM_SC_RA_RAM_OP_PARAM_RATE_2_3;
 		break;
@@ -3914,7 +3920,7 @@ static int set_dvbt(struct drxk_state *state, u16 intermediate_freqk_hz,
 	switch (state->props.bandwidth_hz) {
 	case 0:
 		state->props.bandwidth_hz = 8000000;
-		/* fall though */
+		/* fall through */
 	case 8000000:
 		bandwidth = DRXK_BANDWIDTH_8MHZ_IN_HZ;
 		status = write16(state, OFDM_SC_RA_RAM_SRMM_FIX_FACT_8K__A,
diff --git a/drivers/media/dvb-frontends/mt352.c b/drivers/media/dvb-frontends/mt352.c
index e127090f2d22..d5fa96f0a6cd 100644
--- a/drivers/media/dvb-frontends/mt352.c
+++ b/drivers/media/dvb-frontends/mt352.c
@@ -211,6 +211,7 @@ static int mt352_set_parameters(struct dvb_frontend *fe)
 			if (op->hierarchy == HIERARCHY_AUTO ||
 			    op->hierarchy == HIERARCHY_NONE)
 				break;
+			/* fall through */
 		default:
 			return -EINVAL;
 	}
diff --git a/drivers/media/dvb-frontends/or51132.c b/drivers/media/dvb-frontends/or51132.c
index 62aa00767015..5f2549c48eb0 100644
--- a/drivers/media/dvb-frontends/or51132.c
+++ b/drivers/media/dvb-frontends/or51132.c
@@ -493,8 +493,8 @@ start:
 	switch (reg&0xff) {
 	case 0x06:
 		if (reg & 0x1000) usK = 3 << 24;
-		/* Fall through to QAM64 case */
-	case 0x43:
+		/* fall through */
+	case 0x43: /* QAM64 */
 		c = 150204167;
 		break;
 	case 0x45:
diff --git a/drivers/media/dvb-frontends/s5h1411.c b/drivers/media/dvb-frontends/s5h1411.c
index f29750a96196..dd09336a135b 100644
--- a/drivers/media/dvb-frontends/s5h1411.c
+++ b/drivers/media/dvb-frontends/s5h1411.c
@@ -51,7 +51,7 @@ static int debug;
 #define dprintk(arg...) do {	\
 	if (debug)		\
 		printk(arg);	\
-	} while (0)
+} while (0)
 
 /* Register values to initialise the demod, defaults to VSB */
 static struct init_tab {
@@ -410,7 +410,7 @@ static int s5h1411_set_if_freq(struct dvb_frontend *fe, int KHz)
 	default:
 		dprintk("%s(%d KHz) Invalid, defaulting to 5380\n",
 			__func__, KHz);
-		/* no break, need to continue */
+		/* fall through */
 	case 5380:
 	case 44000:
 		s5h1411_writereg(state, S5H1411_I2C_TOP_ADDR, 0x38, 0x1be4);
diff --git a/drivers/media/dvb-frontends/stv0367.c b/drivers/media/dvb-frontends/stv0367.c
index fd49c436a36d..e726c2e00460 100644
--- a/drivers/media/dvb-frontends/stv0367.c
+++ b/drivers/media/dvb-frontends/stv0367.c
@@ -26,6 +26,7 @@
 #include <linux/i2c.h>
 
 #include "stv0367.h"
+#include "stv0367_defs.h"
 #include "stv0367_regs.h"
 #include "stv0367_priv.h"
 
@@ -45,6 +46,8 @@ module_param_named(i2c_debug, i2cdebug, int, 0644);
 	} while (0)
 	/* DVB-C */
 
+enum active_demod_state { demod_none, demod_ter, demod_cab };
+
 struct stv0367cab_state {
 	enum stv0367_cab_signal_type	state;
 	u32	mclk;
@@ -56,6 +59,7 @@ struct stv0367cab_state {
 	u32 freq_khz;			/* found frequency (in kHz)	*/
 	u32 symbol_rate;		/* found symbol rate (in Bds)	*/
 	enum fe_spectral_inversion spect_inv; /* Spectrum Inversion	*/
+	u32 qamfec_status_reg;          /* status reg to poll for FEC Lock */
 };
 
 struct stv0367ter_state {
@@ -89,461 +93,12 @@ struct stv0367_state {
 	struct stv0367cab_state *cab_state;
 	/* DVB-T */
 	struct stv0367ter_state *ter_state;
-};
-
-struct st_register {
-	u16	addr;
-	u8	value;
-};
-
-/* values for STV4100 XTAL=30M int clk=53.125M*/
-static struct st_register def0367ter[STV0367TER_NBREGS] = {
-	{R367TER_ID,		0x60},
-	{R367TER_I2CRPT,	0xa0},
-	/* {R367TER_I2CRPT,	0x22},*/
-	{R367TER_TOPCTRL,	0x00},/* for xc5000; was 0x02 */
-	{R367TER_IOCFG0,	0x40},
-	{R367TER_DAC0R,		0x00},
-	{R367TER_IOCFG1,	0x00},
-	{R367TER_DAC1R,		0x00},
-	{R367TER_IOCFG2,	0x62},
-	{R367TER_SDFR,		0x00},
-	{R367TER_STATUS,	0xf8},
-	{R367TER_AUX_CLK,	0x0a},
-	{R367TER_FREESYS1,	0x00},
-	{R367TER_FREESYS2,	0x00},
-	{R367TER_FREESYS3,	0x00},
-	{R367TER_GPIO_CFG,	0x55},
-	{R367TER_GPIO_CMD,	0x00},
-	{R367TER_AGC2MAX,	0xff},
-	{R367TER_AGC2MIN,	0x00},
-	{R367TER_AGC1MAX,	0xff},
-	{R367TER_AGC1MIN,	0x00},
-	{R367TER_AGCR,		0xbc},
-	{R367TER_AGC2TH,	0x00},
-	{R367TER_AGC12C,	0x00},
-	{R367TER_AGCCTRL1,	0x85},
-	{R367TER_AGCCTRL2,	0x1f},
-	{R367TER_AGC1VAL1,	0x00},
-	{R367TER_AGC1VAL2,	0x00},
-	{R367TER_AGC2VAL1,	0x6f},
-	{R367TER_AGC2VAL2,	0x05},
-	{R367TER_AGC2PGA,	0x00},
-	{R367TER_OVF_RATE1,	0x00},
-	{R367TER_OVF_RATE2,	0x00},
-	{R367TER_GAIN_SRC1,	0xaa},/* for xc5000; was 0x2b */
-	{R367TER_GAIN_SRC2,	0xd6},/* for xc5000; was 0x04 */
-	{R367TER_INC_DEROT1,	0x55},
-	{R367TER_INC_DEROT2,	0x55},
-	{R367TER_PPM_CPAMP_DIR,	0x2c},
-	{R367TER_PPM_CPAMP_INV,	0x00},
-	{R367TER_FREESTFE_1,	0x00},
-	{R367TER_FREESTFE_2,	0x1c},
-	{R367TER_DCOFFSET,	0x00},
-	{R367TER_EN_PROCESS,	0x05},
-	{R367TER_SDI_SMOOTHER,	0x80},
-	{R367TER_FE_LOOP_OPEN,	0x1c},
-	{R367TER_FREQOFF1,	0x00},
-	{R367TER_FREQOFF2,	0x00},
-	{R367TER_FREQOFF3,	0x00},
-	{R367TER_TIMOFF1,	0x00},
-	{R367TER_TIMOFF2,	0x00},
-	{R367TER_EPQ,		0x02},
-	{R367TER_EPQAUTO,	0x01},
-	{R367TER_SYR_UPDATE,	0xf5},
-	{R367TER_CHPFREE,	0x00},
-	{R367TER_PPM_STATE_MAC,	0x23},
-	{R367TER_INR_THRESHOLD,	0xff},
-	{R367TER_EPQ_TPS_ID_CELL, 0xf9},
-	{R367TER_EPQ_CFG,	0x00},
-	{R367TER_EPQ_STATUS,	0x01},
-	{R367TER_AUTORELOCK,	0x81},
-	{R367TER_BER_THR_VMSB,	0x00},
-	{R367TER_BER_THR_MSB,	0x00},
-	{R367TER_BER_THR_LSB,	0x00},
-	{R367TER_CCD,		0x83},
-	{R367TER_SPECTR_CFG,	0x00},
-	{R367TER_CHC_DUMMY,	0x18},
-	{R367TER_INC_CTL,	0x88},
-	{R367TER_INCTHRES_COR1,	0xb4},
-	{R367TER_INCTHRES_COR2,	0x96},
-	{R367TER_INCTHRES_DET1,	0x0e},
-	{R367TER_INCTHRES_DET2,	0x11},
-	{R367TER_IIR_CELLNB,	0x8d},
-	{R367TER_IIRCX_COEFF1_MSB, 0x00},
-	{R367TER_IIRCX_COEFF1_LSB, 0x00},
-	{R367TER_IIRCX_COEFF2_MSB, 0x09},
-	{R367TER_IIRCX_COEFF2_LSB, 0x18},
-	{R367TER_IIRCX_COEFF3_MSB, 0x14},
-	{R367TER_IIRCX_COEFF3_LSB, 0x9c},
-	{R367TER_IIRCX_COEFF4_MSB, 0x00},
-	{R367TER_IIRCX_COEFF4_LSB, 0x00},
-	{R367TER_IIRCX_COEFF5_MSB, 0x36},
-	{R367TER_IIRCX_COEFF5_LSB, 0x42},
-	{R367TER_FEPATH_CFG,	0x00},
-	{R367TER_PMC1_FUNC,	0x65},
-	{R367TER_PMC1_FOR,	0x00},
-	{R367TER_PMC2_FUNC,	0x00},
-	{R367TER_STATUS_ERR_DA,	0xe0},
-	{R367TER_DIG_AGC_R,	0xfe},
-	{R367TER_COMAGC_TARMSB,	0x0b},
-	{R367TER_COM_AGC_TAR_ENMODE, 0x41},
-	{R367TER_COM_AGC_CFG,	0x3e},
-	{R367TER_COM_AGC_GAIN1, 0x39},
-	{R367TER_AUT_AGC_TARGETMSB, 0x0b},
-	{R367TER_LOCK_DET_MSB,	0x01},
-	{R367TER_AGCTAR_LOCK_LSBS, 0x40},
-	{R367TER_AUT_GAIN_EN,	0xf4},
-	{R367TER_AUT_CFG,	0xf0},
-	{R367TER_LOCKN,		0x23},
-	{R367TER_INT_X_3,	0x00},
-	{R367TER_INT_X_2,	0x03},
-	{R367TER_INT_X_1,	0x8d},
-	{R367TER_INT_X_0,	0xa0},
-	{R367TER_MIN_ERRX_MSB,	0x00},
-	{R367TER_COR_CTL,	0x23},
-	{R367TER_COR_STAT,	0xf6},
-	{R367TER_COR_INTEN,	0x00},
-	{R367TER_COR_INTSTAT,	0x3f},
-	{R367TER_COR_MODEGUARD,	0x03},
-	{R367TER_AGC_CTL,	0x08},
-	{R367TER_AGC_MANUAL1,	0x00},
-	{R367TER_AGC_MANUAL2,	0x00},
-	{R367TER_AGC_TARG,	0x16},
-	{R367TER_AGC_GAIN1,	0x53},
-	{R367TER_AGC_GAIN2,	0x1d},
-	{R367TER_RESERVED_1,	0x00},
-	{R367TER_RESERVED_2,	0x00},
-	{R367TER_RESERVED_3,	0x00},
-	{R367TER_CAS_CTL,	0x44},
-	{R367TER_CAS_FREQ,	0xb3},
-	{R367TER_CAS_DAGCGAIN,	0x12},
-	{R367TER_SYR_CTL,	0x04},
-	{R367TER_SYR_STAT,	0x10},
-	{R367TER_SYR_NCO1,	0x00},
-	{R367TER_SYR_NCO2,	0x00},
-	{R367TER_SYR_OFFSET1,	0x00},
-	{R367TER_SYR_OFFSET2,	0x00},
-	{R367TER_FFT_CTL,	0x00},
-	{R367TER_SCR_CTL,	0x70},
-	{R367TER_PPM_CTL1,	0xf8},
-	{R367TER_TRL_CTL,	0x14},/* for xc5000; was 0xac */
-	{R367TER_TRL_NOMRATE1,	0xae},/* for xc5000; was 0x1e */
-	{R367TER_TRL_NOMRATE2,	0x56},/* for xc5000; was 0x58 */
-	{R367TER_TRL_TIME1,	0x1d},
-	{R367TER_TRL_TIME2,	0xfc},
-	{R367TER_CRL_CTL,	0x24},
-	{R367TER_CRL_FREQ1,	0xad},
-	{R367TER_CRL_FREQ2,	0x9d},
-	{R367TER_CRL_FREQ3,	0xff},
-	{R367TER_CHC_CTL,	0x01},
-	{R367TER_CHC_SNR,	0xf0},
-	{R367TER_BDI_CTL,	0x00},
-	{R367TER_DMP_CTL,	0x00},
-	{R367TER_TPS_RCVD1,	0x30},
-	{R367TER_TPS_RCVD2,	0x02},
-	{R367TER_TPS_RCVD3,	0x01},
-	{R367TER_TPS_RCVD4,	0x00},
-	{R367TER_TPS_ID_CELL1,	0x00},
-	{R367TER_TPS_ID_CELL2,	0x00},
-	{R367TER_TPS_RCVD5_SET1, 0x02},
-	{R367TER_TPS_SET2,	0x02},
-	{R367TER_TPS_SET3,	0x01},
-	{R367TER_TPS_CTL,	0x00},
-	{R367TER_CTL_FFTOSNUM,	0x34},
-	{R367TER_TESTSELECT,	0x09},
-	{R367TER_MSC_REV,	0x0a},
-	{R367TER_PIR_CTL,	0x00},
-	{R367TER_SNR_CARRIER1,	0xa1},
-	{R367TER_SNR_CARRIER2,	0x9a},
-	{R367TER_PPM_CPAMP,	0x2c},
-	{R367TER_TSM_AP0,	0x00},
-	{R367TER_TSM_AP1,	0x00},
-	{R367TER_TSM_AP2 ,	0x00},
-	{R367TER_TSM_AP3,	0x00},
-	{R367TER_TSM_AP4,	0x00},
-	{R367TER_TSM_AP5,	0x00},
-	{R367TER_TSM_AP6,	0x00},
-	{R367TER_TSM_AP7,	0x00},
-	{R367TER_TSTRES,	0x00},
-	{R367TER_ANACTRL,	0x0D},/* PLL stoped, restart at init!!! */
-	{R367TER_TSTBUS,	0x00},
-	{R367TER_TSTRATE,	0x00},
-	{R367TER_CONSTMODE,	0x01},
-	{R367TER_CONSTCARR1,	0x00},
-	{R367TER_CONSTCARR2,	0x00},
-	{R367TER_ICONSTEL,	0x0a},
-	{R367TER_QCONSTEL,	0x15},
-	{R367TER_TSTBISTRES0,	0x00},
-	{R367TER_TSTBISTRES1,	0x00},
-	{R367TER_TSTBISTRES2,	0x28},
-	{R367TER_TSTBISTRES3,	0x00},
-	{R367TER_RF_AGC1,	0xff},
-	{R367TER_RF_AGC2,	0x83},
-	{R367TER_ANADIGCTRL,	0x19},
-	{R367TER_PLLMDIV,	0x01},/* for xc5000; was 0x0c */
-	{R367TER_PLLNDIV,	0x06},/* for xc5000; was 0x55 */
-	{R367TER_PLLSETUP,	0x18},
-	{R367TER_DUAL_AD12,	0x0C},/* for xc5000 AGC voltage 1.6V */
-	{R367TER_TSTBIST,	0x00},
-	{R367TER_PAD_COMP_CTRL,	0x00},
-	{R367TER_PAD_COMP_WR,	0x00},
-	{R367TER_PAD_COMP_RD,	0xe0},
-	{R367TER_SYR_TARGET_FFTADJT_MSB, 0x00},
-	{R367TER_SYR_TARGET_FFTADJT_LSB, 0x00},
-	{R367TER_SYR_TARGET_CHCADJT_MSB, 0x00},
-	{R367TER_SYR_TARGET_CHCADJT_LSB, 0x00},
-	{R367TER_SYR_FLAG,	0x00},
-	{R367TER_CRL_TARGET1,	0x00},
-	{R367TER_CRL_TARGET2,	0x00},
-	{R367TER_CRL_TARGET3,	0x00},
-	{R367TER_CRL_TARGET4,	0x00},
-	{R367TER_CRL_FLAG,	0x00},
-	{R367TER_TRL_TARGET1,	0x00},
-	{R367TER_TRL_TARGET2,	0x00},
-	{R367TER_TRL_CHC,	0x00},
-	{R367TER_CHC_SNR_TARG,	0x00},
-	{R367TER_TOP_TRACK,	0x00},
-	{R367TER_TRACKER_FREE1,	0x00},
-	{R367TER_ERROR_CRL1,	0x00},
-	{R367TER_ERROR_CRL2,	0x00},
-	{R367TER_ERROR_CRL3,	0x00},
-	{R367TER_ERROR_CRL4,	0x00},
-	{R367TER_DEC_NCO1,	0x2c},
-	{R367TER_DEC_NCO2,	0x0f},
-	{R367TER_DEC_NCO3,	0x20},
-	{R367TER_SNR,		0xf1},
-	{R367TER_SYR_FFTADJ1,	0x00},
-	{R367TER_SYR_FFTADJ2,	0x00},
-	{R367TER_SYR_CHCADJ1,	0x00},
-	{R367TER_SYR_CHCADJ2,	0x00},
-	{R367TER_SYR_OFF,	0x00},
-	{R367TER_PPM_OFFSET1,	0x00},
-	{R367TER_PPM_OFFSET2,	0x03},
-	{R367TER_TRACKER_FREE2,	0x00},
-	{R367TER_DEBG_LT10,	0x00},
-	{R367TER_DEBG_LT11,	0x00},
-	{R367TER_DEBG_LT12,	0x00},
-	{R367TER_DEBG_LT13,	0x00},
-	{R367TER_DEBG_LT14,	0x00},
-	{R367TER_DEBG_LT15,	0x00},
-	{R367TER_DEBG_LT16,	0x00},
-	{R367TER_DEBG_LT17,	0x00},
-	{R367TER_DEBG_LT18,	0x00},
-	{R367TER_DEBG_LT19,	0x00},
-	{R367TER_DEBG_LT1A,	0x00},
-	{R367TER_DEBG_LT1B,	0x00},
-	{R367TER_DEBG_LT1C,	0x00},
-	{R367TER_DEBG_LT1D,	0x00},
-	{R367TER_DEBG_LT1E,	0x00},
-	{R367TER_DEBG_LT1F,	0x00},
-	{R367TER_RCCFGH,	0x00},
-	{R367TER_RCCFGM,	0x00},
-	{R367TER_RCCFGL,	0x00},
-	{R367TER_RCINSDELH,	0x00},
-	{R367TER_RCINSDELM,	0x00},
-	{R367TER_RCINSDELL,	0x00},
-	{R367TER_RCSTATUS,	0x00},
-	{R367TER_RCSPEED,	0x6f},
-	{R367TER_RCDEBUGM,	0xe7},
-	{R367TER_RCDEBUGL,	0x9b},
-	{R367TER_RCOBSCFG,	0x00},
-	{R367TER_RCOBSM,	0x00},
-	{R367TER_RCOBSL,	0x00},
-	{R367TER_RCFECSPY,	0x00},
-	{R367TER_RCFSPYCFG,	0x00},
-	{R367TER_RCFSPYDATA,	0x00},
-	{R367TER_RCFSPYOUT,	0x00},
-	{R367TER_RCFSTATUS,	0x00},
-	{R367TER_RCFGOODPACK,	0x00},
-	{R367TER_RCFPACKCNT,	0x00},
-	{R367TER_RCFSPYMISC,	0x00},
-	{R367TER_RCFBERCPT4,	0x00},
-	{R367TER_RCFBERCPT3,	0x00},
-	{R367TER_RCFBERCPT2,	0x00},
-	{R367TER_RCFBERCPT1,	0x00},
-	{R367TER_RCFBERCPT0,	0x00},
-	{R367TER_RCFBERERR2,	0x00},
-	{R367TER_RCFBERERR1,	0x00},
-	{R367TER_RCFBERERR0,	0x00},
-	{R367TER_RCFSTATESM,	0x00},
-	{R367TER_RCFSTATESL,	0x00},
-	{R367TER_RCFSPYBER,	0x00},
-	{R367TER_RCFSPYDISTM,	0x00},
-	{R367TER_RCFSPYDISTL,	0x00},
-	{R367TER_RCFSPYOBS7,	0x00},
-	{R367TER_RCFSPYOBS6,	0x00},
-	{R367TER_RCFSPYOBS5,	0x00},
-	{R367TER_RCFSPYOBS4,	0x00},
-	{R367TER_RCFSPYOBS3,	0x00},
-	{R367TER_RCFSPYOBS2,	0x00},
-	{R367TER_RCFSPYOBS1,	0x00},
-	{R367TER_RCFSPYOBS0,	0x00},
-	{R367TER_TSGENERAL,	0x00},
-	{R367TER_RC1SPEED,	0x6f},
-	{R367TER_TSGSTATUS,	0x18},
-	{R367TER_FECM,		0x01},
-	{R367TER_VTH12,		0xff},
-	{R367TER_VTH23,		0xa1},
-	{R367TER_VTH34,		0x64},
-	{R367TER_VTH56,		0x40},
-	{R367TER_VTH67,		0x00},
-	{R367TER_VTH78,		0x2c},
-	{R367TER_VITCURPUN,	0x12},
-	{R367TER_VERROR,	0x01},
-	{R367TER_PRVIT,		0x3f},
-	{R367TER_VAVSRVIT,	0x00},
-	{R367TER_VSTATUSVIT,	0xbd},
-	{R367TER_VTHINUSE,	0xa1},
-	{R367TER_KDIV12,	0x20},
-	{R367TER_KDIV23,	0x40},
-	{R367TER_KDIV34,	0x20},
-	{R367TER_KDIV56,	0x30},
-	{R367TER_KDIV67,	0x00},
-	{R367TER_KDIV78,	0x30},
-	{R367TER_SIGPOWER,	0x54},
-	{R367TER_DEMAPVIT,	0x40},
-	{R367TER_VITSCALE,	0x00},
-	{R367TER_FFEC1PRG,	0x00},
-	{R367TER_FVITCURPUN,	0x12},
-	{R367TER_FVERROR,	0x01},
-	{R367TER_FVSTATUSVIT,	0xbd},
-	{R367TER_DEBUG_LT1,	0x00},
-	{R367TER_DEBUG_LT2,	0x00},
-	{R367TER_DEBUG_LT3,	0x00},
-	{R367TER_TSTSFMET,	0x00},
-	{R367TER_SELOUT,	0x00},
-	{R367TER_TSYNC,		0x00},
-	{R367TER_TSTERR,	0x00},
-	{R367TER_TSFSYNC,	0x00},
-	{R367TER_TSTSFERR,	0x00},
-	{R367TER_TSTTSSF1,	0x01},
-	{R367TER_TSTTSSF2,	0x1f},
-	{R367TER_TSTTSSF3,	0x00},
-	{R367TER_TSTTS1,	0x00},
-	{R367TER_TSTTS2,	0x1f},
-	{R367TER_TSTTS3,	0x01},
-	{R367TER_TSTTS4,	0x00},
-	{R367TER_TSTTSRC,	0x00},
-	{R367TER_TSTTSRS,	0x00},
-	{R367TER_TSSTATEM,	0xb0},
-	{R367TER_TSSTATEL,	0x40},
-	{R367TER_TSCFGH,	0xC0},
-	{R367TER_TSCFGM,	0xc0},/* for xc5000; was 0x00 */
-	{R367TER_TSCFGL,	0x20},
-	{R367TER_TSSYNC,	0x00},
-	{R367TER_TSINSDELH,	0x00},
-	{R367TER_TSINSDELM,	0x00},
-	{R367TER_TSINSDELL,	0x00},
-	{R367TER_TSDIVN,	0x03},
-	{R367TER_TSDIVPM,	0x00},
-	{R367TER_TSDIVPL,	0x00},
-	{R367TER_TSDIVQM,	0x00},
-	{R367TER_TSDIVQL,	0x00},
-	{R367TER_TSDILSTKM,	0x00},
-	{R367TER_TSDILSTKL,	0x00},
-	{R367TER_TSSPEED,	0x40},/* for xc5000; was 0x6f */
-	{R367TER_TSSTATUS,	0x81},
-	{R367TER_TSSTATUS2,	0x6a},
-	{R367TER_TSBITRATEM,	0x0f},
-	{R367TER_TSBITRATEL,	0xc6},
-	{R367TER_TSPACKLENM,	0x00},
-	{R367TER_TSPACKLENL,	0xfc},
-	{R367TER_TSBLOCLENM,	0x0a},
-	{R367TER_TSBLOCLENL,	0x80},
-	{R367TER_TSDLYH,	0x90},
-	{R367TER_TSDLYM,	0x68},
-	{R367TER_TSDLYL,	0x01},
-	{R367TER_TSNPDAV,	0x00},
-	{R367TER_TSBUFSTATH,	0x00},
-	{R367TER_TSBUFSTATM,	0x00},
-	{R367TER_TSBUFSTATL,	0x00},
-	{R367TER_TSDEBUGM,	0xcf},
-	{R367TER_TSDEBUGL,	0x1e},
-	{R367TER_TSDLYSETH,	0x00},
-	{R367TER_TSDLYSETM,	0x68},
-	{R367TER_TSDLYSETL,	0x00},
-	{R367TER_TSOBSCFG,	0x00},
-	{R367TER_TSOBSM,	0x47},
-	{R367TER_TSOBSL,	0x1f},
-	{R367TER_ERRCTRL1,	0x95},
-	{R367TER_ERRCNT1H,	0x80},
-	{R367TER_ERRCNT1M,	0x00},
-	{R367TER_ERRCNT1L,	0x00},
-	{R367TER_ERRCTRL2,	0x95},
-	{R367TER_ERRCNT2H,	0x00},
-	{R367TER_ERRCNT2M,	0x00},
-	{R367TER_ERRCNT2L,	0x00},
-	{R367TER_FECSPY,	0x88},
-	{R367TER_FSPYCFG,	0x2c},
-	{R367TER_FSPYDATA,	0x3a},
-	{R367TER_FSPYOUT,	0x06},
-	{R367TER_FSTATUS,	0x61},
-	{R367TER_FGOODPACK,	0xff},
-	{R367TER_FPACKCNT,	0xff},
-	{R367TER_FSPYMISC,	0x66},
-	{R367TER_FBERCPT4,	0x00},
-	{R367TER_FBERCPT3,	0x00},
-	{R367TER_FBERCPT2,	0x36},
-	{R367TER_FBERCPT1,	0x36},
-	{R367TER_FBERCPT0,	0x14},
-	{R367TER_FBERERR2,	0x00},
-	{R367TER_FBERERR1,	0x03},
-	{R367TER_FBERERR0,	0x28},
-	{R367TER_FSTATESM,	0x00},
-	{R367TER_FSTATESL,	0x02},
-	{R367TER_FSPYBER,	0x00},
-	{R367TER_FSPYDISTM,	0x01},
-	{R367TER_FSPYDISTL,	0x9f},
-	{R367TER_FSPYOBS7,	0xc9},
-	{R367TER_FSPYOBS6,	0x99},
-	{R367TER_FSPYOBS5,	0x08},
-	{R367TER_FSPYOBS4,	0xec},
-	{R367TER_FSPYOBS3,	0x01},
-	{R367TER_FSPYOBS2,	0x0f},
-	{R367TER_FSPYOBS1,	0xf5},
-	{R367TER_FSPYOBS0,	0x08},
-	{R367TER_SFDEMAP,	0x40},
-	{R367TER_SFERROR,	0x00},
-	{R367TER_SFAVSR,	0x30},
-	{R367TER_SFECSTATUS,	0xcc},
-	{R367TER_SFKDIV12,	0x20},
-	{R367TER_SFKDIV23,	0x40},
-	{R367TER_SFKDIV34,	0x20},
-	{R367TER_SFKDIV56,	0x20},
-	{R367TER_SFKDIV67,	0x00},
-	{R367TER_SFKDIV78,	0x20},
-	{R367TER_SFDILSTKM,	0x00},
-	{R367TER_SFDILSTKL,	0x00},
-	{R367TER_SFSTATUS,	0xb5},
-	{R367TER_SFDLYH,	0x90},
-	{R367TER_SFDLYM,	0x60},
-	{R367TER_SFDLYL,	0x01},
-	{R367TER_SFDLYSETH,	0xc0},
-	{R367TER_SFDLYSETM,	0x60},
-	{R367TER_SFDLYSETL,	0x00},
-	{R367TER_SFOBSCFG,	0x00},
-	{R367TER_SFOBSM,	0x47},
-	{R367TER_SFOBSL,	0x05},
-	{R367TER_SFECINFO,	0x40},
-	{R367TER_SFERRCTRL,	0x74},
-	{R367TER_SFERRCNTH,	0x80},
-	{R367TER_SFERRCNTM ,	0x00},
-	{R367TER_SFERRCNTL,	0x00},
-	{R367TER_SYMBRATEM,	0x2f},
-	{R367TER_SYMBRATEL,	0x50},
-	{R367TER_SYMBSTATUS,	0x7f},
-	{R367TER_SYMBCFG,	0x00},
-	{R367TER_SYMBFIFOM,	0xf4},
-	{R367TER_SYMBFIFOL,	0x0d},
-	{R367TER_SYMBOFFSM,	0xf0},
-	{R367TER_SYMBOFFSL,	0x2d},
-	{R367TER_DEBUG_LT4,	0x00},
-	{R367TER_DEBUG_LT5,	0x00},
-	{R367TER_DEBUG_LT6,	0x00},
-	{R367TER_DEBUG_LT7,	0x00},
-	{R367TER_DEBUG_LT8,	0x00},
-	{R367TER_DEBUG_LT9,	0x00},
+	/* flags for operation control */
+	u8 use_i2c_gatectrl;
+	u8 deftabs;
+	u8 reinit_on_setfrontend;
+	u8 auto_if_khz;
+	enum active_demod_state activedemod;
 };
 
 #define RF_LOOKUP_TABLE_SIZE  31
@@ -571,197 +126,6 @@ static const s32 stv0367cab_RF_LookUp2[RF_LOOKUP_TABLE2_SIZE][RF_LOOKUP_TABLE2_S
 	}
 };
 
-static struct st_register def0367cab[STV0367CAB_NBREGS] = {
-	{R367CAB_ID,		0x60},
-	{R367CAB_I2CRPT,	0xa0},
-	/*{R367CAB_I2CRPT,	0x22},*/
-	{R367CAB_TOPCTRL,	0x10},
-	{R367CAB_IOCFG0,	0x80},
-	{R367CAB_DAC0R,		0x00},
-	{R367CAB_IOCFG1,	0x00},
-	{R367CAB_DAC1R,		0x00},
-	{R367CAB_IOCFG2,	0x00},
-	{R367CAB_SDFR,		0x00},
-	{R367CAB_AUX_CLK,	0x00},
-	{R367CAB_FREESYS1,	0x00},
-	{R367CAB_FREESYS2,	0x00},
-	{R367CAB_FREESYS3,	0x00},
-	{R367CAB_GPIO_CFG,	0x55},
-	{R367CAB_GPIO_CMD,	0x01},
-	{R367CAB_TSTRES,	0x00},
-	{R367CAB_ANACTRL,	0x0d},/* was 0x00 need to check - I.M.L.*/
-	{R367CAB_TSTBUS,	0x00},
-	{R367CAB_RF_AGC1,	0xea},
-	{R367CAB_RF_AGC2,	0x82},
-	{R367CAB_ANADIGCTRL,	0x0b},
-	{R367CAB_PLLMDIV,	0x01},
-	{R367CAB_PLLNDIV,	0x08},
-	{R367CAB_PLLSETUP,	0x18},
-	{R367CAB_DUAL_AD12,	0x0C}, /* for xc5000 AGC voltage 1.6V */
-	{R367CAB_TSTBIST,	0x00},
-	{R367CAB_CTRL_1,	0x00},
-	{R367CAB_CTRL_2,	0x03},
-	{R367CAB_IT_STATUS1,	0x2b},
-	{R367CAB_IT_STATUS2,	0x08},
-	{R367CAB_IT_EN1,	0x00},
-	{R367CAB_IT_EN2,	0x00},
-	{R367CAB_CTRL_STATUS,	0x04},
-	{R367CAB_TEST_CTL,	0x00},
-	{R367CAB_AGC_CTL,	0x73},
-	{R367CAB_AGC_IF_CFG,	0x50},
-	{R367CAB_AGC_RF_CFG,	0x00},
-	{R367CAB_AGC_PWM_CFG,	0x03},
-	{R367CAB_AGC_PWR_REF_L,	0x5a},
-	{R367CAB_AGC_PWR_REF_H,	0x00},
-	{R367CAB_AGC_RF_TH_L,	0xff},
-	{R367CAB_AGC_RF_TH_H,	0x07},
-	{R367CAB_AGC_IF_LTH_L,	0x00},
-	{R367CAB_AGC_IF_LTH_H,	0x08},
-	{R367CAB_AGC_IF_HTH_L,	0xff},
-	{R367CAB_AGC_IF_HTH_H,	0x07},
-	{R367CAB_AGC_PWR_RD_L,	0xa0},
-	{R367CAB_AGC_PWR_RD_M,	0xe9},
-	{R367CAB_AGC_PWR_RD_H,	0x03},
-	{R367CAB_AGC_PWM_IFCMD_L,	0xe4},
-	{R367CAB_AGC_PWM_IFCMD_H,	0x00},
-	{R367CAB_AGC_PWM_RFCMD_L,	0xff},
-	{R367CAB_AGC_PWM_RFCMD_H,	0x07},
-	{R367CAB_IQDEM_CFG,	0x01},
-	{R367CAB_MIX_NCO_LL,	0x22},
-	{R367CAB_MIX_NCO_HL,	0x96},
-	{R367CAB_MIX_NCO_HH,	0x55},
-	{R367CAB_SRC_NCO_LL,	0xff},
-	{R367CAB_SRC_NCO_LH,	0x0c},
-	{R367CAB_SRC_NCO_HL,	0xf5},
-	{R367CAB_SRC_NCO_HH,	0x20},
-	{R367CAB_IQDEM_GAIN_SRC_L,	0x06},
-	{R367CAB_IQDEM_GAIN_SRC_H,	0x01},
-	{R367CAB_IQDEM_DCRM_CFG_LL,	0xfe},
-	{R367CAB_IQDEM_DCRM_CFG_LH,	0xff},
-	{R367CAB_IQDEM_DCRM_CFG_HL,	0x0f},
-	{R367CAB_IQDEM_DCRM_CFG_HH,	0x00},
-	{R367CAB_IQDEM_ADJ_COEFF0,	0x34},
-	{R367CAB_IQDEM_ADJ_COEFF1,	0xae},
-	{R367CAB_IQDEM_ADJ_COEFF2,	0x46},
-	{R367CAB_IQDEM_ADJ_COEFF3,	0x77},
-	{R367CAB_IQDEM_ADJ_COEFF4,	0x96},
-	{R367CAB_IQDEM_ADJ_COEFF5,	0x69},
-	{R367CAB_IQDEM_ADJ_COEFF6,	0xc7},
-	{R367CAB_IQDEM_ADJ_COEFF7,	0x01},
-	{R367CAB_IQDEM_ADJ_EN,	0x04},
-	{R367CAB_IQDEM_ADJ_AGC_REF,	0x94},
-	{R367CAB_ALLPASSFILT1,	0xc9},
-	{R367CAB_ALLPASSFILT2,	0x2d},
-	{R367CAB_ALLPASSFILT3,	0xa3},
-	{R367CAB_ALLPASSFILT4,	0xfb},
-	{R367CAB_ALLPASSFILT5,	0xf6},
-	{R367CAB_ALLPASSFILT6,	0x45},
-	{R367CAB_ALLPASSFILT7,	0x6f},
-	{R367CAB_ALLPASSFILT8,	0x7e},
-	{R367CAB_ALLPASSFILT9,	0x05},
-	{R367CAB_ALLPASSFILT10,	0x0a},
-	{R367CAB_ALLPASSFILT11,	0x51},
-	{R367CAB_TRL_AGC_CFG,	0x20},
-	{R367CAB_TRL_LPF_CFG,	0x28},
-	{R367CAB_TRL_LPF_ACQ_GAIN,	0x44},
-	{R367CAB_TRL_LPF_TRK_GAIN,	0x22},
-	{R367CAB_TRL_LPF_OUT_GAIN,	0x03},
-	{R367CAB_TRL_LOCKDET_LTH,	0x04},
-	{R367CAB_TRL_LOCKDET_HTH,	0x11},
-	{R367CAB_TRL_LOCKDET_TRGVAL,	0x20},
-	{R367CAB_IQ_QAM,	0x01},
-	{R367CAB_FSM_STATE,	0xa0},
-	{R367CAB_FSM_CTL,	0x08},
-	{R367CAB_FSM_STS,	0x0c},
-	{R367CAB_FSM_SNR0_HTH,	0x00},
-	{R367CAB_FSM_SNR1_HTH,	0x00},
-	{R367CAB_FSM_SNR2_HTH,	0x23},/* 0x00 */
-	{R367CAB_FSM_SNR0_LTH,	0x00},
-	{R367CAB_FSM_SNR1_LTH,	0x00},
-	{R367CAB_FSM_EQA1_HTH,	0x00},
-	{R367CAB_FSM_TEMPO,	0x32},
-	{R367CAB_FSM_CONFIG,	0x03},
-	{R367CAB_EQU_I_TESTTAP_L,	0x11},
-	{R367CAB_EQU_I_TESTTAP_M,	0x00},
-	{R367CAB_EQU_I_TESTTAP_H,	0x00},
-	{R367CAB_EQU_TESTAP_CFG,	0x00},
-	{R367CAB_EQU_Q_TESTTAP_L,	0xff},
-	{R367CAB_EQU_Q_TESTTAP_M,	0x00},
-	{R367CAB_EQU_Q_TESTTAP_H,	0x00},
-	{R367CAB_EQU_TAP_CTRL,	0x00},
-	{R367CAB_EQU_CTR_CRL_CONTROL_L,	0x11},
-	{R367CAB_EQU_CTR_CRL_CONTROL_H,	0x05},
-	{R367CAB_EQU_CTR_HIPOW_L,	0x00},
-	{R367CAB_EQU_CTR_HIPOW_H,	0x00},
-	{R367CAB_EQU_I_EQU_LO,	0xef},
-	{R367CAB_EQU_I_EQU_HI,	0x00},
-	{R367CAB_EQU_Q_EQU_LO,	0xee},
-	{R367CAB_EQU_Q_EQU_HI,	0x00},
-	{R367CAB_EQU_MAPPER,	0xc5},
-	{R367CAB_EQU_SWEEP_RATE,	0x80},
-	{R367CAB_EQU_SNR_LO,	0x64},
-	{R367CAB_EQU_SNR_HI,	0x03},
-	{R367CAB_EQU_GAMMA_LO,	0x00},
-	{R367CAB_EQU_GAMMA_HI,	0x00},
-	{R367CAB_EQU_ERR_GAIN,	0x36},
-	{R367CAB_EQU_RADIUS,	0xaa},
-	{R367CAB_EQU_FFE_MAINTAP,	0x00},
-	{R367CAB_EQU_FFE_LEAKAGE,	0x63},
-	{R367CAB_EQU_FFE_MAINTAP_POS,	0xdf},
-	{R367CAB_EQU_GAIN_WIDE,	0x88},
-	{R367CAB_EQU_GAIN_NARROW,	0x41},
-	{R367CAB_EQU_CTR_LPF_GAIN,	0xd1},
-	{R367CAB_EQU_CRL_LPF_GAIN,	0xa7},
-	{R367CAB_EQU_GLOBAL_GAIN,	0x06},
-	{R367CAB_EQU_CRL_LD_SEN,	0x85},
-	{R367CAB_EQU_CRL_LD_VAL,	0xe2},
-	{R367CAB_EQU_CRL_TFR,	0x20},
-	{R367CAB_EQU_CRL_BISTH_LO,	0x00},
-	{R367CAB_EQU_CRL_BISTH_HI,	0x00},
-	{R367CAB_EQU_SWEEP_RANGE_LO,	0x00},
-	{R367CAB_EQU_SWEEP_RANGE_HI,	0x00},
-	{R367CAB_EQU_CRL_LIMITER,	0x40},
-	{R367CAB_EQU_MODULUS_MAP,	0x90},
-	{R367CAB_EQU_PNT_GAIN,	0xa7},
-	{R367CAB_FEC_AC_CTR_0,	0x16},
-	{R367CAB_FEC_AC_CTR_1,	0x0b},
-	{R367CAB_FEC_AC_CTR_2,	0x88},
-	{R367CAB_FEC_AC_CTR_3,	0x02},
-	{R367CAB_FEC_STATUS,	0x12},
-	{R367CAB_RS_COUNTER_0,	0x7d},
-	{R367CAB_RS_COUNTER_1,	0xd0},
-	{R367CAB_RS_COUNTER_2,	0x19},
-	{R367CAB_RS_COUNTER_3,	0x0b},
-	{R367CAB_RS_COUNTER_4,	0xa3},
-	{R367CAB_RS_COUNTER_5,	0x00},
-	{R367CAB_BERT_0,	0x01},
-	{R367CAB_BERT_1,	0x25},
-	{R367CAB_BERT_2,	0x41},
-	{R367CAB_BERT_3,	0x39},
-	{R367CAB_OUTFORMAT_0,	0xc2},
-	{R367CAB_OUTFORMAT_1,	0x22},
-	{R367CAB_SMOOTHER_2,	0x28},
-	{R367CAB_TSMF_CTRL_0,	0x01},
-	{R367CAB_TSMF_CTRL_1,	0xc6},
-	{R367CAB_TSMF_CTRL_3,	0x43},
-	{R367CAB_TS_ON_ID_0,	0x00},
-	{R367CAB_TS_ON_ID_1,	0x00},
-	{R367CAB_TS_ON_ID_2,	0x00},
-	{R367CAB_TS_ON_ID_3,	0x00},
-	{R367CAB_RE_STATUS_0,	0x00},
-	{R367CAB_RE_STATUS_1,	0x00},
-	{R367CAB_RE_STATUS_2,	0x00},
-	{R367CAB_RE_STATUS_3,	0x00},
-	{R367CAB_TS_STATUS_0,	0x00},
-	{R367CAB_TS_STATUS_1,	0x00},
-	{R367CAB_TS_STATUS_2,	0xa0},
-	{R367CAB_TS_STATUS_3,	0x00},
-	{R367CAB_T_O_ID_0,	0x00},
-	{R367CAB_T_O_ID_1,	0x00},
-	{R367CAB_T_O_ID_2,	0x00},
-	{R367CAB_T_O_ID_3,	0x00},
-};
-
 static
 int stv0367_writeregs(struct stv0367_state *state, u16 reg, u8 *data, int len)
 {
@@ -899,6 +263,78 @@ static u8 stv0367_getbits(u8 reg, u32 label)
 	return (reg & mask) >> pos;
 }
 #endif
+
+static void stv0367_write_table(struct stv0367_state *state,
+				const struct st_register *deftab)
+{
+	int i = 0;
+
+	while (1) {
+		if (!deftab[i].addr)
+			break;
+		stv0367_writereg(state, deftab[i].addr, deftab[i].value);
+		i++;
+	}
+}
+
+static void stv0367_pll_setup(struct stv0367_state *state,
+				u32 icspeed, u32 xtal)
+{
+	/* note on regs: R367TER_* and R367CAB_* defines each point to
+	 * 0xf0d8, so just use R367TER_ for both cases
+	 */
+
+	switch (icspeed) {
+	case STV0367_ICSPEED_58000:
+		switch (xtal) {
+		default:
+		case 27000000:
+			dprintk("STV0367 SetCLKgen for 58MHz IC and 27Mhz crystal\n");
+			/* PLLMDIV: 27, PLLNDIV: 232 */
+			stv0367_writereg(state, R367TER_PLLMDIV, 0x1b);
+			stv0367_writereg(state, R367TER_PLLNDIV, 0xe8);
+			break;
+		}
+		break;
+	default:
+	case STV0367_ICSPEED_53125:
+		switch (xtal) {
+			/* set internal freq to 53.125MHz */
+		case 16000000:
+			stv0367_writereg(state, R367TER_PLLMDIV, 0x2);
+			stv0367_writereg(state, R367TER_PLLNDIV, 0x1b);
+			break;
+		case 25000000:
+			stv0367_writereg(state, R367TER_PLLMDIV, 0xa);
+			stv0367_writereg(state, R367TER_PLLNDIV, 0x55);
+			break;
+		default:
+		case 27000000:
+			dprintk("FE_STV0367TER_SetCLKgen for 27Mhz\n");
+			stv0367_writereg(state, R367TER_PLLMDIV, 0x1);
+			stv0367_writereg(state, R367TER_PLLNDIV, 0x8);
+			break;
+		case 30000000:
+			stv0367_writereg(state, R367TER_PLLMDIV, 0xc);
+			stv0367_writereg(state, R367TER_PLLNDIV, 0x55);
+			break;
+		}
+	}
+
+	stv0367_writereg(state, R367TER_PLLSETUP, 0x18);
+}
+
+static int stv0367_get_if_khz(struct stv0367_state *state, u32 *ifkhz)
+{
+	if (state->auto_if_khz && state->fe.ops.tuner_ops.get_if_frequency) {
+		state->fe.ops.tuner_ops.get_if_frequency(&state->fe, ifkhz);
+		*ifkhz = *ifkhz / 1000; /* hz -> khz */
+	} else
+		*ifkhz = state->config->if_khz;
+
+	return 0;
+}
+
 static int stv0367ter_gate_ctrl(struct dvb_frontend *fe, int enable)
 {
 	struct stv0367_state *state = fe->demodulator_priv;
@@ -1260,9 +696,9 @@ stv0367_ter_signal_type stv0367ter_check_cpamp(struct stv0367_state *state,
 	dprintk("******last CPAMPvalue= %d at wd=%d\n", CPAMPvalue, wd);
 	if (CPAMPvalue < CPAMPMin) {
 		CPAMPStatus = FE_TER_NOCPAMP;
-		printk(KERN_ERR "CPAMP failed\n");
+		dprintk("%s: CPAMP failed\n", __func__);
 	} else {
-		printk(KERN_ERR "CPAMP OK !\n");
+		dprintk("%s: CPAMP OK !\n", __func__);
 		CPAMPStatus = FE_TER_CPAMPOK;
 	}
 
@@ -1538,41 +974,15 @@ static int stv0367ter_init(struct dvb_frontend *fe)
 {
 	struct stv0367_state *state = fe->demodulator_priv;
 	struct stv0367ter_state *ter_state = state->ter_state;
-	int i;
 
 	dprintk("%s:\n", __func__);
 
 	ter_state->pBER = 0;
 
-	for (i = 0; i < STV0367TER_NBREGS; i++)
-		stv0367_writereg(state, def0367ter[i].addr,
-					def0367ter[i].value);
+	stv0367_write_table(state,
+		stv0367_deftabs[state->deftabs][STV0367_TAB_TER]);
 
-	switch (state->config->xtal) {
-		/*set internal freq to 53.125MHz */
-	case 16000000:
-		stv0367_writereg(state, R367TER_PLLMDIV, 0x2);
-		stv0367_writereg(state, R367TER_PLLNDIV, 0x1b);
-		stv0367_writereg(state, R367TER_PLLSETUP, 0x18);
-		break;
-	case 25000000:
-		stv0367_writereg(state, R367TER_PLLMDIV, 0xa);
-		stv0367_writereg(state, R367TER_PLLNDIV, 0x55);
-		stv0367_writereg(state, R367TER_PLLSETUP, 0x18);
-		break;
-	default:
-	case 27000000:
-		dprintk("FE_STV0367TER_SetCLKgen for 27Mhz\n");
-		stv0367_writereg(state, R367TER_PLLMDIV, 0x1);
-		stv0367_writereg(state, R367TER_PLLNDIV, 0x8);
-		stv0367_writereg(state, R367TER_PLLSETUP, 0x18);
-		break;
-	case 30000000:
-		stv0367_writereg(state, R367TER_PLLMDIV, 0xc);
-		stv0367_writereg(state, R367TER_PLLNDIV, 0x55);
-		stv0367_writereg(state, R367TER_PLLSETUP, 0x18);
-		break;
-	}
+	stv0367_pll_setup(state, STV0367_ICSPEED_53125, state->config->xtal);
 
 	stv0367_writereg(state, R367TER_I2CRPT, 0xa0);
 	stv0367_writereg(state, R367TER_ANACTRL, 0x00);
@@ -1598,10 +1008,12 @@ static int stv0367ter_algo(struct dvb_frontend *fe)
 	u8 /*constell,*/ counter;
 	s8 step;
 	s32 timing_offset = 0;
-	u32 trl_nomrate = 0, InternalFreq = 0, temp = 0;
+	u32 trl_nomrate = 0, InternalFreq = 0, temp = 0, ifkhz = 0;
 
 	dprintk("%s:\n", __func__);
 
+	stv0367_get_if_khz(state, &ifkhz);
+
 	ter_state->frequency = p->frequency;
 	ter_state->force = FE_TER_FORCENONE
 			+ stv0367_readbits(state, F367TER_FORCE) * 2;
@@ -1704,8 +1116,7 @@ static int stv0367ter_algo(struct dvb_frontend *fe)
 			stv0367_readbits(state, F367TER_GAIN_SRC_LO);
 
 	temp = (int)
-		((InternalFreq - state->config->if_khz) * (1 << 16)
-							/ (InternalFreq));
+		((InternalFreq - ifkhz) * (1 << 16) / (InternalFreq));
 
 	dprintk("DEROT temp=0x%x\n", temp);
 	stv0367_writebits(state, F367TER_INC_DEROT_HI, temp / 256);
@@ -1824,13 +1235,14 @@ static int stv0367ter_set_frontend(struct dvb_frontend *fe)
 	s8 num_trials, index;
 	u8 SenseTrials[] = { INVERSION_ON, INVERSION_OFF };
 
-	stv0367ter_init(fe);
+	if (state->reinit_on_setfrontend)
+		stv0367ter_init(fe);
 
 	if (fe->ops.tuner_ops.set_params) {
-		if (fe->ops.i2c_gate_ctrl)
+		if (state->use_i2c_gatectrl && fe->ops.i2c_gate_ctrl)
 			fe->ops.i2c_gate_ctrl(fe, 1);
 		fe->ops.tuner_ops.set_params(fe);
-		if (fe->ops.i2c_gate_ctrl)
+		if (state->use_i2c_gatectrl && fe->ops.i2c_gate_ctrl)
 			fe->ops.i2c_gate_ctrl(fe, 0);
 	}
 
@@ -2321,6 +1733,12 @@ struct dvb_frontend *stv0367ter_attach(const struct stv0367_config *config,
 	state->fe.demodulator_priv = state;
 	state->chip_id = stv0367_readreg(state, 0xf000);
 
+	/* demod operation options */
+	state->use_i2c_gatectrl = 1;
+	state->deftabs = STV0367_DEFTAB_GENERIC;
+	state->reinit_on_setfrontend = 1;
+	state->auto_if_khz = 0;
+
 	dprintk("%s: chip_id = 0x%x\n", __func__, state->chip_id);
 
 	/* check if the demod is there */
@@ -2423,11 +1841,11 @@ static enum stv0367cab_mod stv0367cab_SetQamSize(struct stv0367_state *state,
 	case FE_CAB_MOD_QAM64:
 		stv0367_writereg(state, R367CAB_IQDEM_ADJ_AGC_REF, 0x82);
 		stv0367_writereg(state, R367CAB_AGC_PWR_REF_L, 0x5a);
-		if (SymbolRate > 45000000) {
+		if (SymbolRate > 4500000) {
 			stv0367_writereg(state, R367CAB_FSM_STATE, 0xb0);
 			stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xc1);
 			stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0xa5);
-		} else if (SymbolRate > 25000000) {
+		} else if (SymbolRate > 2500000) {
 			stv0367_writereg(state, R367CAB_FSM_STATE, 0xa0);
 			stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xc1);
 			stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0xa6);
@@ -2445,9 +1863,9 @@ static enum stv0367cab_mod stv0367cab_SetQamSize(struct stv0367_state *state,
 		stv0367_writereg(state, R367CAB_AGC_PWR_REF_L, 0x76);
 		stv0367_writereg(state, R367CAB_FSM_STATE, 0x90);
 		stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xb1);
-		if (SymbolRate > 45000000)
+		if (SymbolRate > 4500000)
 			stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0xa7);
-		else if (SymbolRate > 25000000)
+		else if (SymbolRate > 2500000)
 			stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0xa6);
 		else
 			stv0367_writereg(state, R367CAB_EQU_CRL_LPF_GAIN, 0x97);
@@ -2460,9 +1878,9 @@ static enum stv0367cab_mod stv0367cab_SetQamSize(struct stv0367_state *state,
 		stv0367_writereg(state, R367CAB_IQDEM_ADJ_AGC_REF, 0x94);
 		stv0367_writereg(state, R367CAB_AGC_PWR_REF_L, 0x5a);
 		stv0367_writereg(state, R367CAB_FSM_STATE, 0xa0);
-		if (SymbolRate > 45000000)
+		if (SymbolRate > 4500000)
 			stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xc1);
-		else if (SymbolRate > 25000000)
+		else if (SymbolRate > 2500000)
 			stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xc1);
 		else
 			stv0367_writereg(state, R367CAB_EQU_CTR_LPF_GAIN, 0xd1);
@@ -2731,7 +2149,8 @@ static int stv0367cab_read_status(struct dvb_frontend *fe,
 
 	*status = 0;
 
-	if (stv0367_readbits(state, F367CAB_QAMFEC_LOCK)) {
+	if (stv0367_readbits(state, (state->cab_state->qamfec_status_reg ?
+		state->cab_state->qamfec_status_reg : F367CAB_QAMFEC_LOCK))) {
 		*status |= FE_HAS_LOCK;
 		dprintk("%s: stv0367 has locked\n", __func__);
 	}
@@ -2777,13 +2196,11 @@ static int stv0367cab_init(struct dvb_frontend *fe)
 {
 	struct stv0367_state *state = fe->demodulator_priv;
 	struct stv0367cab_state *cab_state = state->cab_state;
-	int i;
 
 	dprintk("%s:\n", __func__);
 
-	for (i = 0; i < STV0367CAB_NBREGS; i++)
-		stv0367_writereg(state, def0367cab[i].addr,
-						def0367cab[i].value);
+	stv0367_write_table(state,
+		stv0367_deftabs[state->deftabs][STV0367_TAB_CAB]);
 
 	switch (state->config->ts_mode) {
 	case STV0367_DVBCI_CLOCK:
@@ -2831,7 +2248,7 @@ enum stv0367_cab_signal_type stv0367cab_algo(struct stv0367_state *state,
 {
 	struct stv0367cab_state *cab_state = state->cab_state;
 	enum stv0367_cab_signal_type signalType = FE_CAB_NOAGC;
-	u32	QAMFEC_Lock, QAM_Lock, u32_tmp,
+	u32	QAMFEC_Lock, QAM_Lock, u32_tmp, ifkhz,
 		LockTime, TRLTimeOut, AGCTimeOut, CRLSymbols,
 		CRLTimeOut, EQLTimeOut, DemodTimeOut, FECTimeOut;
 	u8	TrackAGCAccum;
@@ -2839,6 +2256,8 @@ enum stv0367_cab_signal_type stv0367cab_algo(struct stv0367_state *state,
 
 	dprintk("%s:\n", __func__);
 
+	stv0367_get_if_khz(state, &ifkhz);
+
 	/* Timeouts calculation */
 	/* A max lock time of 25 ms is allowed for delayed AGC */
 	AGCTimeOut = 25;
@@ -2917,7 +2336,7 @@ enum stv0367_cab_signal_type stv0367cab_algo(struct stv0367_state *state,
 	/* The sweep function is never used, Sweep rate must be set to 0 */
 	/* Set the derotator frequency in Hz */
 	stv0367cab_set_derot_freq(state, cab_state->adc_clk,
-		(1000 * (s32)state->config->if_khz + cab_state->derot_offset));
+		(1000 * (s32)ifkhz + cab_state->derot_offset));
 	/* Disable the Allpass Filter when the symbol rate is out of range */
 	if ((p->symbol_rate > 10800000) | (p->symbol_rate < 1800000)) {
 		stv0367_writebits(state, F367CAB_ADJ_EN, 0);
@@ -2996,7 +2415,9 @@ enum stv0367_cab_signal_type stv0367cab_algo(struct stv0367_state *state,
 			usleep_range(5000, 7000);
 			LockTime += 5;
 			QAMFEC_Lock = stv0367_readbits(state,
-							F367CAB_QAMFEC_LOCK);
+				(state->cab_state->qamfec_status_reg ?
+				state->cab_state->qamfec_status_reg :
+				F367CAB_QAMFEC_LOCK));
 		} while (!QAMFEC_Lock && (LockTime < FECTimeOut));
 	} else
 		QAMFEC_Lock = 0;
@@ -3007,17 +2428,17 @@ enum stv0367_cab_signal_type stv0367cab_algo(struct stv0367_state *state,
 							F367CAB_QUAD_INV);
 #if 0
 /* not clear for me */
-		if (state->config->if_khz != 0) {
-			if (state->config->if_khz > cab_state->adc_clk / 1000) {
+		if (ifkhz != 0) {
+			if (ifkhz > cab_state->adc_clk / 1000) {
 				cab_state->freq_khz =
 					FE_Cab_TunerGetFrequency(pIntParams->hTuner)
 				- stv0367cab_get_derot_freq(state, cab_state->adc_clk)
-				- cab_state->adc_clk / 1000 + state->config->if_khz;
+				- cab_state->adc_clk / 1000 + ifkhz;
 			} else {
 				cab_state->freq_khz =
 						FE_Cab_TunerGetFrequency(pIntParams->hTuner)
 						- stv0367cab_get_derot_freq(state, cab_state->adc_clk)
-										+ state->config->if_khz;
+						+ ifkhz;
 			}
 		} else {
 			cab_state->freq_khz =
@@ -3116,14 +2537,15 @@ static int stv0367cab_set_frontend(struct dvb_frontend *fe)
 		break;
 	}
 
-	stv0367cab_init(fe);
+	if (state->reinit_on_setfrontend)
+		stv0367cab_init(fe);
 
 	/* Tuner Frequency Setting */
 	if (fe->ops.tuner_ops.set_params) {
-		if (fe->ops.i2c_gate_ctrl)
+		if (state->use_i2c_gatectrl && fe->ops.i2c_gate_ctrl)
 			fe->ops.i2c_gate_ctrl(fe, 1);
 		fe->ops.tuner_ops.set_params(fe);
-		if (fe->ops.i2c_gate_ctrl)
+		if (state->use_i2c_gatectrl && fe->ops.i2c_gate_ctrl)
 			fe->ops.i2c_gate_ctrl(fe, 0);
 	}
 
@@ -3147,11 +2569,13 @@ static int stv0367cab_get_frontend(struct dvb_frontend *fe,
 {
 	struct stv0367_state *state = fe->demodulator_priv;
 	struct stv0367cab_state *cab_state = state->cab_state;
+	u32 ifkhz = 0;
 
 	enum stv0367cab_mod QAMSize;
 
 	dprintk("%s:\n", __func__);
 
+	stv0367_get_if_khz(state, &ifkhz);
 	p->symbol_rate = stv0367cab_GetSymbolRate(state, cab_state->mclk);
 
 	QAMSize = stv0367_readbits(state, F367CAB_QAM_MODE);
@@ -3179,19 +2603,19 @@ static int stv0367cab_get_frontend(struct dvb_frontend *fe,
 
 	dprintk("%s: tuner frequency = %d\n", __func__, p->frequency);
 
-	if (state->config->if_khz == 0) {
+	if (ifkhz == 0) {
 		p->frequency +=
 			(stv0367cab_get_derot_freq(state, cab_state->adc_clk) -
 			cab_state->adc_clk / 4000);
 		return 0;
 	}
 
-	if (state->config->if_khz > cab_state->adc_clk / 1000)
-		p->frequency += (state->config->if_khz
+	if (ifkhz > cab_state->adc_clk / 1000)
+		p->frequency += (ifkhz
 			- stv0367cab_get_derot_freq(state, cab_state->adc_clk)
 			- cab_state->adc_clk / 1000);
 	else
-		p->frequency += (state->config->if_khz
+		p->frequency += (ifkhz
 			- stv0367cab_get_derot_freq(state, cab_state->adc_clk));
 
 	return 0;
@@ -3432,11 +2856,18 @@ struct dvb_frontend *stv0367cab_attach(const struct stv0367_config *config,
 	state->i2c = i2c;
 	state->config = config;
 	cab_state->search_range = 280000;
+	cab_state->qamfec_status_reg = F367CAB_QAMFEC_LOCK;
 	state->cab_state = cab_state;
 	state->fe.ops = stv0367cab_ops;
 	state->fe.demodulator_priv = state;
 	state->chip_id = stv0367_readreg(state, 0xf000);
 
+	/* demod operation options */
+	state->use_i2c_gatectrl = 1;
+	state->deftabs = STV0367_DEFTAB_GENERIC;
+	state->reinit_on_setfrontend = 1;
+	state->auto_if_khz = 0;
+
 	dprintk("%s: chip_id = 0x%x\n", __func__, state->chip_id);
 
 	/* check if the demod is there */
@@ -3452,6 +2883,327 @@ error:
 }
 EXPORT_SYMBOL(stv0367cab_attach);
 
+/*
+ * Functions for operation on Digital Devices hardware
+ */
+
+static void stv0367ddb_setup_ter(struct stv0367_state *state)
+{
+	stv0367_writereg(state, R367TER_DEBUG_LT4, 0x00);
+	stv0367_writereg(state, R367TER_DEBUG_LT5, 0x00);
+	stv0367_writereg(state, R367TER_DEBUG_LT6, 0x00); /* R367CAB_CTRL_1 */
+	stv0367_writereg(state, R367TER_DEBUG_LT7, 0x00); /* R367CAB_CTRL_2 */
+	stv0367_writereg(state, R367TER_DEBUG_LT8, 0x00);
+	stv0367_writereg(state, R367TER_DEBUG_LT9, 0x00);
+
+	/* Tuner Setup */
+	/* Buffer Q disabled, I Enabled, unsigned ADC */
+	stv0367_writereg(state, R367TER_ANADIGCTRL, 0x89);
+	stv0367_writereg(state, R367TER_DUAL_AD12, 0x04); /* ADCQ disabled */
+
+	/* Clock setup */
+	/* PLL bypassed and disabled */
+	stv0367_writereg(state, R367TER_ANACTRL, 0x0D);
+	stv0367_writereg(state, R367TER_TOPCTRL, 0x00); /* Set OFDM */
+
+	/* IC runs at 54 MHz with a 27 MHz crystal */
+	stv0367_pll_setup(state, STV0367_ICSPEED_53125, state->config->xtal);
+
+	msleep(50);
+	/* PLL enabled and used */
+	stv0367_writereg(state, R367TER_ANACTRL, 0x00);
+
+	state->activedemod = demod_ter;
+}
+
+static void stv0367ddb_setup_cab(struct stv0367_state *state)
+{
+	stv0367_writereg(state, R367TER_DEBUG_LT4, 0x00);
+	stv0367_writereg(state, R367TER_DEBUG_LT5, 0x01);
+	stv0367_writereg(state, R367TER_DEBUG_LT6, 0x06); /* R367CAB_CTRL_1 */
+	stv0367_writereg(state, R367TER_DEBUG_LT7, 0x03); /* R367CAB_CTRL_2 */
+	stv0367_writereg(state, R367TER_DEBUG_LT8, 0x00);
+	stv0367_writereg(state, R367TER_DEBUG_LT9, 0x00);
+
+	/* Tuner Setup */
+	/* Buffer Q disabled, I Enabled, signed ADC */
+	stv0367_writereg(state, R367TER_ANADIGCTRL, 0x8B);
+	/* ADCQ disabled */
+	stv0367_writereg(state, R367TER_DUAL_AD12, 0x04);
+
+	/* Clock setup */
+	/* PLL bypassed and disabled */
+	stv0367_writereg(state, R367TER_ANACTRL, 0x0D);
+	/* Set QAM */
+	stv0367_writereg(state, R367TER_TOPCTRL, 0x10);
+
+	/* IC runs at 58 MHz with a 27 MHz crystal */
+	stv0367_pll_setup(state, STV0367_ICSPEED_58000, state->config->xtal);
+
+	msleep(50);
+	/* PLL enabled and used */
+	stv0367_writereg(state, R367TER_ANACTRL, 0x00);
+
+	state->cab_state->mclk = stv0367cab_get_mclk(&state->fe,
+		state->config->xtal);
+	state->cab_state->adc_clk = stv0367cab_get_adc_freq(&state->fe,
+		state->config->xtal);
+
+	state->activedemod = demod_cab;
+}
+
+static int stv0367ddb_set_frontend(struct dvb_frontend *fe)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+
+	switch (fe->dtv_property_cache.delivery_system) {
+	case SYS_DVBT:
+		if (state->activedemod != demod_ter)
+			stv0367ddb_setup_ter(state);
+
+		return stv0367ter_set_frontend(fe);
+	case SYS_DVBC_ANNEX_A:
+		if (state->activedemod != demod_cab)
+			stv0367ddb_setup_cab(state);
+
+		/* protect against division error oopses */
+		if (fe->dtv_property_cache.symbol_rate == 0) {
+			printk(KERN_ERR "Invalid symbol rate\n");
+			return -EINVAL;
+		}
+
+		return stv0367cab_set_frontend(fe);
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static int stv0367ddb_read_status(struct dvb_frontend *fe,
+				  enum fe_status *status)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+
+	switch (state->activedemod) {
+	case demod_ter:
+		return stv0367ter_read_status(fe, status);
+	case demod_cab:
+		return stv0367cab_read_status(fe, status);
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static int stv0367ddb_get_frontend(struct dvb_frontend *fe,
+				   struct dtv_frontend_properties *p)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+
+	switch (state->activedemod) {
+	case demod_ter:
+		return stv0367ter_get_frontend(fe, p);
+	case demod_cab:
+		return stv0367cab_get_frontend(fe, p);
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static int stv0367ddb_sleep(struct dvb_frontend *fe)
+{
+	struct stv0367_state *state = fe->demodulator_priv;
+
+	switch (state->activedemod) {
+	case demod_ter:
+		state->activedemod = demod_none;
+		return stv0367ter_sleep(fe);
+	case demod_cab:
+		state->activedemod = demod_none;
+		return stv0367cab_sleep(fe);
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static int stv0367ddb_init(struct stv0367_state *state)
+{
+	struct stv0367ter_state *ter_state = state->ter_state;
+
+	stv0367_writereg(state, R367TER_TOPCTRL, 0x10);
+
+	if (stv0367_deftabs[state->deftabs][STV0367_TAB_BASE])
+		stv0367_write_table(state,
+			stv0367_deftabs[state->deftabs][STV0367_TAB_BASE]);
+
+	stv0367_write_table(state,
+		stv0367_deftabs[state->deftabs][STV0367_TAB_CAB]);
+
+	stv0367_writereg(state, R367TER_TOPCTRL, 0x00);
+	stv0367_write_table(state,
+		stv0367_deftabs[state->deftabs][STV0367_TAB_TER]);
+
+	stv0367_writereg(state, R367TER_GAIN_SRC1, 0x2A);
+	stv0367_writereg(state, R367TER_GAIN_SRC2, 0xD6);
+	stv0367_writereg(state, R367TER_INC_DEROT1, 0x55);
+	stv0367_writereg(state, R367TER_INC_DEROT2, 0x55);
+	stv0367_writereg(state, R367TER_TRL_CTL, 0x14);
+	stv0367_writereg(state, R367TER_TRL_NOMRATE1, 0xAE);
+	stv0367_writereg(state, R367TER_TRL_NOMRATE2, 0x56);
+	stv0367_writereg(state, R367TER_FEPATH_CFG, 0x0);
+
+	/* OFDM TS Setup */
+
+	stv0367_writereg(state, R367TER_TSCFGH, 0x70);
+	stv0367_writereg(state, R367TER_TSCFGM, 0xC0);
+	stv0367_writereg(state, R367TER_TSCFGL, 0x20);
+	stv0367_writereg(state, R367TER_TSSPEED, 0x40); /* Fixed at 54 MHz */
+
+	stv0367_writereg(state, R367TER_TSCFGH, 0x71);
+	stv0367_writereg(state, R367TER_TSCFGH, 0x70);
+
+	stv0367_writereg(state, R367TER_TOPCTRL, 0x10);
+
+	/* Also needed for QAM */
+	stv0367_writereg(state, R367TER_AGC12C, 0x01); /* AGC Pin setup */
+
+	stv0367_writereg(state, R367TER_AGCCTRL1, 0x8A);
+
+	/* QAM TS setup, note exact format also depends on descrambler */
+	/* settings */
+	/* Inverted Clock, Swap, serial */
+	stv0367_writereg(state, R367CAB_OUTFORMAT_0, 0x85);
+
+	/* Clock setup (PLL bypassed and disabled) */
+	stv0367_writereg(state, R367TER_ANACTRL, 0x0D);
+
+	/* IC runs at 58 MHz with a 27 MHz crystal */
+	stv0367_pll_setup(state, STV0367_ICSPEED_58000, state->config->xtal);
+
+	/* Tuner setup */
+	/* Buffer Q disabled, I Enabled, signed ADC */
+	stv0367_writereg(state, R367TER_ANADIGCTRL, 0x8b);
+	stv0367_writereg(state, R367TER_DUAL_AD12, 0x04); /* ADCQ disabled */
+
+	/* Improves the C/N lock limit */
+	stv0367_writereg(state, R367CAB_FSM_SNR2_HTH, 0x23);
+	/* ZIF/IF Automatic mode */
+	stv0367_writereg(state, R367CAB_IQ_QAM, 0x01);
+	/* Improving burst noise performances */
+	stv0367_writereg(state, R367CAB_EQU_FFE_LEAKAGE, 0x83);
+	/* Improving ACI performances */
+	stv0367_writereg(state, R367CAB_IQDEM_ADJ_EN, 0x05);
+
+	/* PLL enabled and used */
+	stv0367_writereg(state, R367TER_ANACTRL, 0x00);
+
+	stv0367_writereg(state, R367TER_I2CRPT, (0x08 | ((5 & 0x07) << 4)));
+
+	ter_state->pBER = 0;
+	ter_state->first_lock = 0;
+	ter_state->unlock_counter = 2;
+
+	return 0;
+}
+
+static const struct dvb_frontend_ops stv0367ddb_ops = {
+	.delsys = { SYS_DVBC_ANNEX_A, SYS_DVBT },
+	.info = {
+		.name			= "ST STV0367 DDB DVB-C/T",
+		.frequency_min		= 47000000,
+		.frequency_max		= 865000000,
+		.frequency_stepsize	= 166667,
+		.frequency_tolerance	= 0,
+		.symbol_rate_min	= 870000,
+		.symbol_rate_max	= 11700000,
+		.caps = /* DVB-C */
+			0x400 |/* FE_CAN_QAM_4 */
+			FE_CAN_QAM_16 | FE_CAN_QAM_32  |
+			FE_CAN_QAM_64 | FE_CAN_QAM_128 |
+			FE_CAN_QAM_256 | FE_CAN_FEC_AUTO |
+			/* DVB-T */
+			FE_CAN_FEC_1_2 | FE_CAN_FEC_2_3 |
+			FE_CAN_FEC_3_4 | FE_CAN_FEC_5_6 | FE_CAN_FEC_7_8 |
+			FE_CAN_FEC_AUTO |
+			FE_CAN_QPSK | FE_CAN_QAM_16 | FE_CAN_QAM_64 |
+			FE_CAN_QAM_128 | FE_CAN_QAM_256 | FE_CAN_QAM_AUTO |
+			FE_CAN_TRANSMISSION_MODE_AUTO | FE_CAN_RECOVER |
+			FE_CAN_INVERSION_AUTO |
+			FE_CAN_MUTE_TS
+	},
+	.release = stv0367_release,
+	.sleep = stv0367ddb_sleep,
+	.i2c_gate_ctrl = stv0367cab_gate_ctrl, /* valid for TER and CAB */
+	.set_frontend = stv0367ddb_set_frontend,
+	.get_frontend = stv0367ddb_get_frontend,
+	.get_tune_settings = stv0367_get_tune_settings,
+	.read_status = stv0367ddb_read_status,
+};
+
+struct dvb_frontend *stv0367ddb_attach(const struct stv0367_config *config,
+				   struct i2c_adapter *i2c)
+{
+	struct stv0367_state *state = NULL;
+	struct stv0367ter_state *ter_state = NULL;
+	struct stv0367cab_state *cab_state = NULL;
+
+	/* allocate memory for the internal state */
+	state = kzalloc(sizeof(struct stv0367_state), GFP_KERNEL);
+	if (state == NULL)
+		goto error;
+	ter_state = kzalloc(sizeof(struct stv0367ter_state), GFP_KERNEL);
+	if (ter_state == NULL)
+		goto error;
+	cab_state = kzalloc(sizeof(struct stv0367cab_state), GFP_KERNEL);
+	if (cab_state == NULL)
+		goto error;
+
+	/* setup the state */
+	state->i2c = i2c;
+	state->config = config;
+	state->ter_state = ter_state;
+	cab_state->search_range = 280000;
+	cab_state->qamfec_status_reg = F367CAB_DESCR_SYNCSTATE;
+	state->cab_state = cab_state;
+	state->fe.ops = stv0367ddb_ops;
+	state->fe.demodulator_priv = state;
+	state->chip_id = stv0367_readreg(state, R367TER_ID);
+
+	/* demod operation options */
+	state->use_i2c_gatectrl = 0;
+	state->deftabs = STV0367_DEFTAB_DDB;
+	state->reinit_on_setfrontend = 0;
+	state->auto_if_khz = 1;
+	state->activedemod = demod_none;
+
+	dprintk("%s: chip_id = 0x%x\n", __func__, state->chip_id);
+
+	/* check if the demod is there */
+	if ((state->chip_id != 0x50) && (state->chip_id != 0x60))
+		goto error;
+
+	dev_info(&i2c->dev, "Found %s with ChipID %02X at adr %02X\n",
+		state->fe.ops.info.name, state->chip_id,
+		config->demod_address);
+
+	stv0367ddb_init(state);
+
+	return &state->fe;
+
+error:
+	kfree(cab_state);
+	kfree(ter_state);
+	kfree(state);
+	return NULL;
+}
+EXPORT_SYMBOL(stv0367ddb_attach);
+
 MODULE_PARM_DESC(debug, "Set debug");
 MODULE_PARM_DESC(i2c_debug, "Set i2c debug");
 
diff --git a/drivers/media/dvb-frontends/stv0367.h b/drivers/media/dvb-frontends/stv0367.h
index 26c38a0503c8..8f7a31481744 100644
--- a/drivers/media/dvb-frontends/stv0367.h
+++ b/drivers/media/dvb-frontends/stv0367.h
@@ -25,6 +25,9 @@
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
+#define STV0367_ICSPEED_53125	53125000
+#define STV0367_ICSPEED_58000	58000000
+
 struct stv0367_config {
 	u8 demod_address;
 	u32 xtal;
@@ -41,6 +44,9 @@ dvb_frontend *stv0367ter_attach(const struct stv0367_config *config,
 extern struct
 dvb_frontend *stv0367cab_attach(const struct stv0367_config *config,
 					struct i2c_adapter *i2c);
+extern struct
+dvb_frontend *stv0367ddb_attach(const struct stv0367_config *config,
+					struct i2c_adapter *i2c);
 #else
 static inline struct
 dvb_frontend *stv0367ter_attach(const struct stv0367_config *config,
@@ -56,6 +62,13 @@ dvb_frontend *stv0367cab_attach(const struct stv0367_config *config,
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return NULL;
 }
+static inline struct
+dvb_frontend *stv0367ddb_attach(const struct stv0367_config *config,
+					struct i2c_adapter *i2c)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
 #endif
 
 #endif
diff --git a/drivers/media/dvb-frontends/stv0367_defs.h b/drivers/media/dvb-frontends/stv0367_defs.h
new file mode 100644
index 000000000000..277d2971ed3f
--- /dev/null
+++ b/drivers/media/dvb-frontends/stv0367_defs.h
@@ -0,0 +1,1301 @@
+/*
+ * stv0367_defs.h
+ *
+ * Driver for ST STV0367 DVB-T & DVB-C demodulator IC.
+ *
+ * Copyright (C) ST Microelectronics.
+ * Copyright (C) 2010,2011 NetUP Inc.
+ * Copyright (C) 2010,2011 Igor M. Liplianin <liplianin@netup.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ * GNU General Public License for more details.
+ */
+
+#ifndef STV0367_DEFS_H
+#define STV0367_DEFS_H
+
+#include "stv0367_regs.h"
+
+#define STV0367_DEFTAB_GENERIC	0
+#define STV0367_DEFTAB_DDB	1
+#define STV0367_DEFTAB_MAX	2
+
+#define STV0367_TAB_TER		0
+#define STV0367_TAB_CAB		1
+#define STV0367_TAB_BASE	2
+#define STV0367_TAB_MAX		3
+
+struct st_register {
+	u16	addr;
+	u8	value;
+};
+
+/* values for STV4100 XTAL=30M int clk=53.125M*/
+static const struct st_register def0367ter[] = {
+	{R367TER_ID,		0x60},
+	{R367TER_I2CRPT,	0xa0},
+	/* {R367TER_I2CRPT,	0x22},*/
+	{R367TER_TOPCTRL,	0x00},/* for xc5000; was 0x02 */
+	{R367TER_IOCFG0,	0x40},
+	{R367TER_DAC0R,		0x00},
+	{R367TER_IOCFG1,	0x00},
+	{R367TER_DAC1R,		0x00},
+	{R367TER_IOCFG2,	0x62},
+	{R367TER_SDFR,		0x00},
+	{R367TER_STATUS,	0xf8},
+	{R367TER_AUX_CLK,	0x0a},
+	{R367TER_FREESYS1,	0x00},
+	{R367TER_FREESYS2,	0x00},
+	{R367TER_FREESYS3,	0x00},
+	{R367TER_GPIO_CFG,	0x55},
+	{R367TER_GPIO_CMD,	0x00},
+	{R367TER_AGC2MAX,	0xff},
+	{R367TER_AGC2MIN,	0x00},
+	{R367TER_AGC1MAX,	0xff},
+	{R367TER_AGC1MIN,	0x00},
+	{R367TER_AGCR,		0xbc},
+	{R367TER_AGC2TH,	0x00},
+	{R367TER_AGC12C,	0x00},
+	{R367TER_AGCCTRL1,	0x85},
+	{R367TER_AGCCTRL2,	0x1f},
+	{R367TER_AGC1VAL1,	0x00},
+	{R367TER_AGC1VAL2,	0x00},
+	{R367TER_AGC2VAL1,	0x6f},
+	{R367TER_AGC2VAL2,	0x05},
+	{R367TER_AGC2PGA,	0x00},
+	{R367TER_OVF_RATE1,	0x00},
+	{R367TER_OVF_RATE2,	0x00},
+	{R367TER_GAIN_SRC1,	0xaa},/* for xc5000; was 0x2b */
+	{R367TER_GAIN_SRC2,	0xd6},/* for xc5000; was 0x04 */
+	{R367TER_INC_DEROT1,	0x55},
+	{R367TER_INC_DEROT2,	0x55},
+	{R367TER_PPM_CPAMP_DIR,	0x2c},
+	{R367TER_PPM_CPAMP_INV,	0x00},
+	{R367TER_FREESTFE_1,	0x00},
+	{R367TER_FREESTFE_2,	0x1c},
+	{R367TER_DCOFFSET,	0x00},
+	{R367TER_EN_PROCESS,	0x05},
+	{R367TER_SDI_SMOOTHER,	0x80},
+	{R367TER_FE_LOOP_OPEN,	0x1c},
+	{R367TER_FREQOFF1,	0x00},
+	{R367TER_FREQOFF2,	0x00},
+	{R367TER_FREQOFF3,	0x00},
+	{R367TER_TIMOFF1,	0x00},
+	{R367TER_TIMOFF2,	0x00},
+	{R367TER_EPQ,		0x02},
+	{R367TER_EPQAUTO,	0x01},
+	{R367TER_SYR_UPDATE,	0xf5},
+	{R367TER_CHPFREE,	0x00},
+	{R367TER_PPM_STATE_MAC,	0x23},
+	{R367TER_INR_THRESHOLD,	0xff},
+	{R367TER_EPQ_TPS_ID_CELL, 0xf9},
+	{R367TER_EPQ_CFG,	0x00},
+	{R367TER_EPQ_STATUS,	0x01},
+	{R367TER_AUTORELOCK,	0x81},
+	{R367TER_BER_THR_VMSB,	0x00},
+	{R367TER_BER_THR_MSB,	0x00},
+	{R367TER_BER_THR_LSB,	0x00},
+	{R367TER_CCD,		0x83},
+	{R367TER_SPECTR_CFG,	0x00},
+	{R367TER_CHC_DUMMY,	0x18},
+	{R367TER_INC_CTL,	0x88},
+	{R367TER_INCTHRES_COR1,	0xb4},
+	{R367TER_INCTHRES_COR2,	0x96},
+	{R367TER_INCTHRES_DET1,	0x0e},
+	{R367TER_INCTHRES_DET2,	0x11},
+	{R367TER_IIR_CELLNB,	0x8d},
+	{R367TER_IIRCX_COEFF1_MSB, 0x00},
+	{R367TER_IIRCX_COEFF1_LSB, 0x00},
+	{R367TER_IIRCX_COEFF2_MSB, 0x09},
+	{R367TER_IIRCX_COEFF2_LSB, 0x18},
+	{R367TER_IIRCX_COEFF3_MSB, 0x14},
+	{R367TER_IIRCX_COEFF3_LSB, 0x9c},
+	{R367TER_IIRCX_COEFF4_MSB, 0x00},
+	{R367TER_IIRCX_COEFF4_LSB, 0x00},
+	{R367TER_IIRCX_COEFF5_MSB, 0x36},
+	{R367TER_IIRCX_COEFF5_LSB, 0x42},
+	{R367TER_FEPATH_CFG,	0x00},
+	{R367TER_PMC1_FUNC,	0x65},
+	{R367TER_PMC1_FOR,	0x00},
+	{R367TER_PMC2_FUNC,	0x00},
+	{R367TER_STATUS_ERR_DA,	0xe0},
+	{R367TER_DIG_AGC_R,	0xfe},
+	{R367TER_COMAGC_TARMSB,	0x0b},
+	{R367TER_COM_AGC_TAR_ENMODE, 0x41},
+	{R367TER_COM_AGC_CFG,	0x3e},
+	{R367TER_COM_AGC_GAIN1, 0x39},
+	{R367TER_AUT_AGC_TARGETMSB, 0x0b},
+	{R367TER_LOCK_DET_MSB,	0x01},
+	{R367TER_AGCTAR_LOCK_LSBS, 0x40},
+	{R367TER_AUT_GAIN_EN,	0xf4},
+	{R367TER_AUT_CFG,	0xf0},
+	{R367TER_LOCKN,		0x23},
+	{R367TER_INT_X_3,	0x00},
+	{R367TER_INT_X_2,	0x03},
+	{R367TER_INT_X_1,	0x8d},
+	{R367TER_INT_X_0,	0xa0},
+	{R367TER_MIN_ERRX_MSB,	0x00},
+	{R367TER_COR_CTL,	0x23},
+	{R367TER_COR_STAT,	0xf6},
+	{R367TER_COR_INTEN,	0x00},
+	{R367TER_COR_INTSTAT,	0x3f},
+	{R367TER_COR_MODEGUARD,	0x03},
+	{R367TER_AGC_CTL,	0x08},
+	{R367TER_AGC_MANUAL1,	0x00},
+	{R367TER_AGC_MANUAL2,	0x00},
+	{R367TER_AGC_TARG,	0x16},
+	{R367TER_AGC_GAIN1,	0x53},
+	{R367TER_AGC_GAIN2,	0x1d},
+	{R367TER_RESERVED_1,	0x00},
+	{R367TER_RESERVED_2,	0x00},
+	{R367TER_RESERVED_3,	0x00},
+	{R367TER_CAS_CTL,	0x44},
+	{R367TER_CAS_FREQ,	0xb3},
+	{R367TER_CAS_DAGCGAIN,	0x12},
+	{R367TER_SYR_CTL,	0x04},
+	{R367TER_SYR_STAT,	0x10},
+	{R367TER_SYR_NCO1,	0x00},
+	{R367TER_SYR_NCO2,	0x00},
+	{R367TER_SYR_OFFSET1,	0x00},
+	{R367TER_SYR_OFFSET2,	0x00},
+	{R367TER_FFT_CTL,	0x00},
+	{R367TER_SCR_CTL,	0x70},
+	{R367TER_PPM_CTL1,	0xf8},
+	{R367TER_TRL_CTL,	0x14},/* for xc5000; was 0xac */
+	{R367TER_TRL_NOMRATE1,	0xae},/* for xc5000; was 0x1e */
+	{R367TER_TRL_NOMRATE2,	0x56},/* for xc5000; was 0x58 */
+	{R367TER_TRL_TIME1,	0x1d},
+	{R367TER_TRL_TIME2,	0xfc},
+	{R367TER_CRL_CTL,	0x24},
+	{R367TER_CRL_FREQ1,	0xad},
+	{R367TER_CRL_FREQ2,	0x9d},
+	{R367TER_CRL_FREQ3,	0xff},
+	{R367TER_CHC_CTL,	0x01},
+	{R367TER_CHC_SNR,	0xf0},
+	{R367TER_BDI_CTL,	0x00},
+	{R367TER_DMP_CTL,	0x00},
+	{R367TER_TPS_RCVD1,	0x30},
+	{R367TER_TPS_RCVD2,	0x02},
+	{R367TER_TPS_RCVD3,	0x01},
+	{R367TER_TPS_RCVD4,	0x00},
+	{R367TER_TPS_ID_CELL1,	0x00},
+	{R367TER_TPS_ID_CELL2,	0x00},
+	{R367TER_TPS_RCVD5_SET1, 0x02},
+	{R367TER_TPS_SET2,	0x02},
+	{R367TER_TPS_SET3,	0x01},
+	{R367TER_TPS_CTL,	0x00},
+	{R367TER_CTL_FFTOSNUM,	0x34},
+	{R367TER_TESTSELECT,	0x09},
+	{R367TER_MSC_REV,	0x0a},
+	{R367TER_PIR_CTL,	0x00},
+	{R367TER_SNR_CARRIER1,	0xa1},
+	{R367TER_SNR_CARRIER2,	0x9a},
+	{R367TER_PPM_CPAMP,	0x2c},
+	{R367TER_TSM_AP0,	0x00},
+	{R367TER_TSM_AP1,	0x00},
+	{R367TER_TSM_AP2,	0x00},
+	{R367TER_TSM_AP3,	0x00},
+	{R367TER_TSM_AP4,	0x00},
+	{R367TER_TSM_AP5,	0x00},
+	{R367TER_TSM_AP6,	0x00},
+	{R367TER_TSM_AP7,	0x00},
+	{R367TER_TSTRES,	0x00},
+	{R367TER_ANACTRL,	0x0D},/* PLL stopped, restart at init!!! */
+	{R367TER_TSTBUS,	0x00},
+	{R367TER_TSTRATE,	0x00},
+	{R367TER_CONSTMODE,	0x01},
+	{R367TER_CONSTCARR1,	0x00},
+	{R367TER_CONSTCARR2,	0x00},
+	{R367TER_ICONSTEL,	0x0a},
+	{R367TER_QCONSTEL,	0x15},
+	{R367TER_TSTBISTRES0,	0x00},
+	{R367TER_TSTBISTRES1,	0x00},
+	{R367TER_TSTBISTRES2,	0x28},
+	{R367TER_TSTBISTRES3,	0x00},
+	{R367TER_RF_AGC1,	0xff},
+	{R367TER_RF_AGC2,	0x83},
+	{R367TER_ANADIGCTRL,	0x19},
+	{R367TER_PLLMDIV,	0x01},/* for xc5000; was 0x0c */
+	{R367TER_PLLNDIV,	0x06},/* for xc5000; was 0x55 */
+	{R367TER_PLLSETUP,	0x18},
+	{R367TER_DUAL_AD12,	0x0C},/* for xc5000 AGC voltage 1.6V */
+	{R367TER_TSTBIST,	0x00},
+	{R367TER_PAD_COMP_CTRL,	0x00},
+	{R367TER_PAD_COMP_WR,	0x00},
+	{R367TER_PAD_COMP_RD,	0xe0},
+	{R367TER_SYR_TARGET_FFTADJT_MSB, 0x00},
+	{R367TER_SYR_TARGET_FFTADJT_LSB, 0x00},
+	{R367TER_SYR_TARGET_CHCADJT_MSB, 0x00},
+	{R367TER_SYR_TARGET_CHCADJT_LSB, 0x00},
+	{R367TER_SYR_FLAG,	0x00},
+	{R367TER_CRL_TARGET1,	0x00},
+	{R367TER_CRL_TARGET2,	0x00},
+	{R367TER_CRL_TARGET3,	0x00},
+	{R367TER_CRL_TARGET4,	0x00},
+	{R367TER_CRL_FLAG,	0x00},
+	{R367TER_TRL_TARGET1,	0x00},
+	{R367TER_TRL_TARGET2,	0x00},
+	{R367TER_TRL_CHC,	0x00},
+	{R367TER_CHC_SNR_TARG,	0x00},
+	{R367TER_TOP_TRACK,	0x00},
+	{R367TER_TRACKER_FREE1,	0x00},
+	{R367TER_ERROR_CRL1,	0x00},
+	{R367TER_ERROR_CRL2,	0x00},
+	{R367TER_ERROR_CRL3,	0x00},
+	{R367TER_ERROR_CRL4,	0x00},
+	{R367TER_DEC_NCO1,	0x2c},
+	{R367TER_DEC_NCO2,	0x0f},
+	{R367TER_DEC_NCO3,	0x20},
+	{R367TER_SNR,		0xf1},
+	{R367TER_SYR_FFTADJ1,	0x00},
+	{R367TER_SYR_FFTADJ2,	0x00},
+	{R367TER_SYR_CHCADJ1,	0x00},
+	{R367TER_SYR_CHCADJ2,	0x00},
+	{R367TER_SYR_OFF,	0x00},
+	{R367TER_PPM_OFFSET1,	0x00},
+	{R367TER_PPM_OFFSET2,	0x03},
+	{R367TER_TRACKER_FREE2,	0x00},
+	{R367TER_DEBG_LT10,	0x00},
+	{R367TER_DEBG_LT11,	0x00},
+	{R367TER_DEBG_LT12,	0x00},
+	{R367TER_DEBG_LT13,	0x00},
+	{R367TER_DEBG_LT14,	0x00},
+	{R367TER_DEBG_LT15,	0x00},
+	{R367TER_DEBG_LT16,	0x00},
+	{R367TER_DEBG_LT17,	0x00},
+	{R367TER_DEBG_LT18,	0x00},
+	{R367TER_DEBG_LT19,	0x00},
+	{R367TER_DEBG_LT1A,	0x00},
+	{R367TER_DEBG_LT1B,	0x00},
+	{R367TER_DEBG_LT1C,	0x00},
+	{R367TER_DEBG_LT1D,	0x00},
+	{R367TER_DEBG_LT1E,	0x00},
+	{R367TER_DEBG_LT1F,	0x00},
+	{R367TER_RCCFGH,	0x00},
+	{R367TER_RCCFGM,	0x00},
+	{R367TER_RCCFGL,	0x00},
+	{R367TER_RCINSDELH,	0x00},
+	{R367TER_RCINSDELM,	0x00},
+	{R367TER_RCINSDELL,	0x00},
+	{R367TER_RCSTATUS,	0x00},
+	{R367TER_RCSPEED,	0x6f},
+	{R367TER_RCDEBUGM,	0xe7},
+	{R367TER_RCDEBUGL,	0x9b},
+	{R367TER_RCOBSCFG,	0x00},
+	{R367TER_RCOBSM,	0x00},
+	{R367TER_RCOBSL,	0x00},
+	{R367TER_RCFECSPY,	0x00},
+	{R367TER_RCFSPYCFG,	0x00},
+	{R367TER_RCFSPYDATA,	0x00},
+	{R367TER_RCFSPYOUT,	0x00},
+	{R367TER_RCFSTATUS,	0x00},
+	{R367TER_RCFGOODPACK,	0x00},
+	{R367TER_RCFPACKCNT,	0x00},
+	{R367TER_RCFSPYMISC,	0x00},
+	{R367TER_RCFBERCPT4,	0x00},
+	{R367TER_RCFBERCPT3,	0x00},
+	{R367TER_RCFBERCPT2,	0x00},
+	{R367TER_RCFBERCPT1,	0x00},
+	{R367TER_RCFBERCPT0,	0x00},
+	{R367TER_RCFBERERR2,	0x00},
+	{R367TER_RCFBERERR1,	0x00},
+	{R367TER_RCFBERERR0,	0x00},
+	{R367TER_RCFSTATESM,	0x00},
+	{R367TER_RCFSTATESL,	0x00},
+	{R367TER_RCFSPYBER,	0x00},
+	{R367TER_RCFSPYDISTM,	0x00},
+	{R367TER_RCFSPYDISTL,	0x00},
+	{R367TER_RCFSPYOBS7,	0x00},
+	{R367TER_RCFSPYOBS6,	0x00},
+	{R367TER_RCFSPYOBS5,	0x00},
+	{R367TER_RCFSPYOBS4,	0x00},
+	{R367TER_RCFSPYOBS3,	0x00},
+	{R367TER_RCFSPYOBS2,	0x00},
+	{R367TER_RCFSPYOBS1,	0x00},
+	{R367TER_RCFSPYOBS0,	0x00},
+	{R367TER_TSGENERAL,	0x00},
+	{R367TER_RC1SPEED,	0x6f},
+	{R367TER_TSGSTATUS,	0x18},
+	{R367TER_FECM,		0x01},
+	{R367TER_VTH12,		0xff},
+	{R367TER_VTH23,		0xa1},
+	{R367TER_VTH34,		0x64},
+	{R367TER_VTH56,		0x40},
+	{R367TER_VTH67,		0x00},
+	{R367TER_VTH78,		0x2c},
+	{R367TER_VITCURPUN,	0x12},
+	{R367TER_VERROR,	0x01},
+	{R367TER_PRVIT,		0x3f},
+	{R367TER_VAVSRVIT,	0x00},
+	{R367TER_VSTATUSVIT,	0xbd},
+	{R367TER_VTHINUSE,	0xa1},
+	{R367TER_KDIV12,	0x20},
+	{R367TER_KDIV23,	0x40},
+	{R367TER_KDIV34,	0x20},
+	{R367TER_KDIV56,	0x30},
+	{R367TER_KDIV67,	0x00},
+	{R367TER_KDIV78,	0x30},
+	{R367TER_SIGPOWER,	0x54},
+	{R367TER_DEMAPVIT,	0x40},
+	{R367TER_VITSCALE,	0x00},
+	{R367TER_FFEC1PRG,	0x00},
+	{R367TER_FVITCURPUN,	0x12},
+	{R367TER_FVERROR,	0x01},
+	{R367TER_FVSTATUSVIT,	0xbd},
+	{R367TER_DEBUG_LT1,	0x00},
+	{R367TER_DEBUG_LT2,	0x00},
+	{R367TER_DEBUG_LT3,	0x00},
+	{R367TER_TSTSFMET,	0x00},
+	{R367TER_SELOUT,	0x00},
+	{R367TER_TSYNC,		0x00},
+	{R367TER_TSTERR,	0x00},
+	{R367TER_TSFSYNC,	0x00},
+	{R367TER_TSTSFERR,	0x00},
+	{R367TER_TSTTSSF1,	0x01},
+	{R367TER_TSTTSSF2,	0x1f},
+	{R367TER_TSTTSSF3,	0x00},
+	{R367TER_TSTTS1,	0x00},
+	{R367TER_TSTTS2,	0x1f},
+	{R367TER_TSTTS3,	0x01},
+	{R367TER_TSTTS4,	0x00},
+	{R367TER_TSTTSRC,	0x00},
+	{R367TER_TSTTSRS,	0x00},
+	{R367TER_TSSTATEM,	0xb0},
+	{R367TER_TSSTATEL,	0x40},
+	{R367TER_TSCFGH,	0xC0},
+	{R367TER_TSCFGM,	0xc0},/* for xc5000; was 0x00 */
+	{R367TER_TSCFGL,	0x20},
+	{R367TER_TSSYNC,	0x00},
+	{R367TER_TSINSDELH,	0x00},
+	{R367TER_TSINSDELM,	0x00},
+	{R367TER_TSINSDELL,	0x00},
+	{R367TER_TSDIVN,	0x03},
+	{R367TER_TSDIVPM,	0x00},
+	{R367TER_TSDIVPL,	0x00},
+	{R367TER_TSDIVQM,	0x00},
+	{R367TER_TSDIVQL,	0x00},
+	{R367TER_TSDILSTKM,	0x00},
+	{R367TER_TSDILSTKL,	0x00},
+	{R367TER_TSSPEED,	0x40},/* for xc5000; was 0x6f */
+	{R367TER_TSSTATUS,	0x81},
+	{R367TER_TSSTATUS2,	0x6a},
+	{R367TER_TSBITRATEM,	0x0f},
+	{R367TER_TSBITRATEL,	0xc6},
+	{R367TER_TSPACKLENM,	0x00},
+	{R367TER_TSPACKLENL,	0xfc},
+	{R367TER_TSBLOCLENM,	0x0a},
+	{R367TER_TSBLOCLENL,	0x80},
+	{R367TER_TSDLYH,	0x90},
+	{R367TER_TSDLYM,	0x68},
+	{R367TER_TSDLYL,	0x01},
+	{R367TER_TSNPDAV,	0x00},
+	{R367TER_TSBUFSTATH,	0x00},
+	{R367TER_TSBUFSTATM,	0x00},
+	{R367TER_TSBUFSTATL,	0x00},
+	{R367TER_TSDEBUGM,	0xcf},
+	{R367TER_TSDEBUGL,	0x1e},
+	{R367TER_TSDLYSETH,	0x00},
+	{R367TER_TSDLYSETM,	0x68},
+	{R367TER_TSDLYSETL,	0x00},
+	{R367TER_TSOBSCFG,	0x00},
+	{R367TER_TSOBSM,	0x47},
+	{R367TER_TSOBSL,	0x1f},
+	{R367TER_ERRCTRL1,	0x95},
+	{R367TER_ERRCNT1H,	0x80},
+	{R367TER_ERRCNT1M,	0x00},
+	{R367TER_ERRCNT1L,	0x00},
+	{R367TER_ERRCTRL2,	0x95},
+	{R367TER_ERRCNT2H,	0x00},
+	{R367TER_ERRCNT2M,	0x00},
+	{R367TER_ERRCNT2L,	0x00},
+	{R367TER_FECSPY,	0x88},
+	{R367TER_FSPYCFG,	0x2c},
+	{R367TER_FSPYDATA,	0x3a},
+	{R367TER_FSPYOUT,	0x06},
+	{R367TER_FSTATUS,	0x61},
+	{R367TER_FGOODPACK,	0xff},
+	{R367TER_FPACKCNT,	0xff},
+	{R367TER_FSPYMISC,	0x66},
+	{R367TER_FBERCPT4,	0x00},
+	{R367TER_FBERCPT3,	0x00},
+	{R367TER_FBERCPT2,	0x36},
+	{R367TER_FBERCPT1,	0x36},
+	{R367TER_FBERCPT0,	0x14},
+	{R367TER_FBERERR2,	0x00},
+	{R367TER_FBERERR1,	0x03},
+	{R367TER_FBERERR0,	0x28},
+	{R367TER_FSTATESM,	0x00},
+	{R367TER_FSTATESL,	0x02},
+	{R367TER_FSPYBER,	0x00},
+	{R367TER_FSPYDISTM,	0x01},
+	{R367TER_FSPYDISTL,	0x9f},
+	{R367TER_FSPYOBS7,	0xc9},
+	{R367TER_FSPYOBS6,	0x99},
+	{R367TER_FSPYOBS5,	0x08},
+	{R367TER_FSPYOBS4,	0xec},
+	{R367TER_FSPYOBS3,	0x01},
+	{R367TER_FSPYOBS2,	0x0f},
+	{R367TER_FSPYOBS1,	0xf5},
+	{R367TER_FSPYOBS0,	0x08},
+	{R367TER_SFDEMAP,	0x40},
+	{R367TER_SFERROR,	0x00},
+	{R367TER_SFAVSR,	0x30},
+	{R367TER_SFECSTATUS,	0xcc},
+	{R367TER_SFKDIV12,	0x20},
+	{R367TER_SFKDIV23,	0x40},
+	{R367TER_SFKDIV34,	0x20},
+	{R367TER_SFKDIV56,	0x20},
+	{R367TER_SFKDIV67,	0x00},
+	{R367TER_SFKDIV78,	0x20},
+	{R367TER_SFDILSTKM,	0x00},
+	{R367TER_SFDILSTKL,	0x00},
+	{R367TER_SFSTATUS,	0xb5},
+	{R367TER_SFDLYH,	0x90},
+	{R367TER_SFDLYM,	0x60},
+	{R367TER_SFDLYL,	0x01},
+	{R367TER_SFDLYSETH,	0xc0},
+	{R367TER_SFDLYSETM,	0x60},
+	{R367TER_SFDLYSETL,	0x00},
+	{R367TER_SFOBSCFG,	0x00},
+	{R367TER_SFOBSM,	0x47},
+	{R367TER_SFOBSL,	0x05},
+	{R367TER_SFECINFO,	0x40},
+	{R367TER_SFERRCTRL,	0x74},
+	{R367TER_SFERRCNTH,	0x80},
+	{R367TER_SFERRCNTM,	0x00},
+	{R367TER_SFERRCNTL,	0x00},
+	{R367TER_SYMBRATEM,	0x2f},
+	{R367TER_SYMBRATEL,	0x50},
+	{R367TER_SYMBSTATUS,	0x7f},
+	{R367TER_SYMBCFG,	0x00},
+	{R367TER_SYMBFIFOM,	0xf4},
+	{R367TER_SYMBFIFOL,	0x0d},
+	{R367TER_SYMBOFFSM,	0xf0},
+	{R367TER_SYMBOFFSL,	0x2d},
+	{R367TER_DEBUG_LT4,	0x00},
+	{R367TER_DEBUG_LT5,	0x00},
+	{R367TER_DEBUG_LT6,	0x00},
+	{R367TER_DEBUG_LT7,	0x00},
+	{R367TER_DEBUG_LT8,	0x00},
+	{R367TER_DEBUG_LT9,	0x00},
+	{0x0000,		0x00},
+};
+
+static const struct st_register def0367cab[] = {
+	{R367CAB_ID,		0x60},
+	{R367CAB_I2CRPT,	0xa0},
+	/*{R367CAB_I2CRPT,	0x22},*/
+	{R367CAB_TOPCTRL,	0x10},
+	{R367CAB_IOCFG0,	0x80},
+	{R367CAB_DAC0R,		0x00},
+	{R367CAB_IOCFG1,	0x00},
+	{R367CAB_DAC1R,		0x00},
+	{R367CAB_IOCFG2,	0x00},
+	{R367CAB_SDFR,		0x00},
+	{R367CAB_AUX_CLK,	0x00},
+	{R367CAB_FREESYS1,	0x00},
+	{R367CAB_FREESYS2,	0x00},
+	{R367CAB_FREESYS3,	0x00},
+	{R367CAB_GPIO_CFG,	0x55},
+	{R367CAB_GPIO_CMD,	0x01},
+	{R367CAB_TSTRES,	0x00},
+	{R367CAB_ANACTRL,	0x0d},/* was 0x00 need to check - I.M.L.*/
+	{R367CAB_TSTBUS,	0x00},
+	{R367CAB_RF_AGC1,	0xea},
+	{R367CAB_RF_AGC2,	0x82},
+	{R367CAB_ANADIGCTRL,	0x0b},
+	{R367CAB_PLLMDIV,	0x01},
+	{R367CAB_PLLNDIV,	0x08},
+	{R367CAB_PLLSETUP,	0x18},
+	{R367CAB_DUAL_AD12,	0x0C}, /* for xc5000 AGC voltage 1.6V */
+	{R367CAB_TSTBIST,	0x00},
+	{R367CAB_CTRL_1,	0x00},
+	{R367CAB_CTRL_2,	0x03},
+	{R367CAB_IT_STATUS1,	0x2b},
+	{R367CAB_IT_STATUS2,	0x08},
+	{R367CAB_IT_EN1,	0x00},
+	{R367CAB_IT_EN2,	0x00},
+	{R367CAB_CTRL_STATUS,	0x04},
+	{R367CAB_TEST_CTL,	0x00},
+	{R367CAB_AGC_CTL,	0x73},
+	{R367CAB_AGC_IF_CFG,	0x50},
+	{R367CAB_AGC_RF_CFG,	0x00},
+	{R367CAB_AGC_PWM_CFG,	0x03},
+	{R367CAB_AGC_PWR_REF_L,	0x5a},
+	{R367CAB_AGC_PWR_REF_H,	0x00},
+	{R367CAB_AGC_RF_TH_L,	0xff},
+	{R367CAB_AGC_RF_TH_H,	0x07},
+	{R367CAB_AGC_IF_LTH_L,	0x00},
+	{R367CAB_AGC_IF_LTH_H,	0x08},
+	{R367CAB_AGC_IF_HTH_L,	0xff},
+	{R367CAB_AGC_IF_HTH_H,	0x07},
+	{R367CAB_AGC_PWR_RD_L,	0xa0},
+	{R367CAB_AGC_PWR_RD_M,	0xe9},
+	{R367CAB_AGC_PWR_RD_H,	0x03},
+	{R367CAB_AGC_PWM_IFCMD_L,	0xe4},
+	{R367CAB_AGC_PWM_IFCMD_H,	0x00},
+	{R367CAB_AGC_PWM_RFCMD_L,	0xff},
+	{R367CAB_AGC_PWM_RFCMD_H,	0x07},
+	{R367CAB_IQDEM_CFG,	0x01},
+	{R367CAB_MIX_NCO_LL,	0x22},
+	{R367CAB_MIX_NCO_HL,	0x96},
+	{R367CAB_MIX_NCO_HH,	0x55},
+	{R367CAB_SRC_NCO_LL,	0xff},
+	{R367CAB_SRC_NCO_LH,	0x0c},
+	{R367CAB_SRC_NCO_HL,	0xf5},
+	{R367CAB_SRC_NCO_HH,	0x20},
+	{R367CAB_IQDEM_GAIN_SRC_L,	0x06},
+	{R367CAB_IQDEM_GAIN_SRC_H,	0x01},
+	{R367CAB_IQDEM_DCRM_CFG_LL,	0xfe},
+	{R367CAB_IQDEM_DCRM_CFG_LH,	0xff},
+	{R367CAB_IQDEM_DCRM_CFG_HL,	0x0f},
+	{R367CAB_IQDEM_DCRM_CFG_HH,	0x00},
+	{R367CAB_IQDEM_ADJ_COEFF0,	0x34},
+	{R367CAB_IQDEM_ADJ_COEFF1,	0xae},
+	{R367CAB_IQDEM_ADJ_COEFF2,	0x46},
+	{R367CAB_IQDEM_ADJ_COEFF3,	0x77},
+	{R367CAB_IQDEM_ADJ_COEFF4,	0x96},
+	{R367CAB_IQDEM_ADJ_COEFF5,	0x69},
+	{R367CAB_IQDEM_ADJ_COEFF6,	0xc7},
+	{R367CAB_IQDEM_ADJ_COEFF7,	0x01},
+	{R367CAB_IQDEM_ADJ_EN,	0x04},
+	{R367CAB_IQDEM_ADJ_AGC_REF,	0x94},
+	{R367CAB_ALLPASSFILT1,	0xc9},
+	{R367CAB_ALLPASSFILT2,	0x2d},
+	{R367CAB_ALLPASSFILT3,	0xa3},
+	{R367CAB_ALLPASSFILT4,	0xfb},
+	{R367CAB_ALLPASSFILT5,	0xf6},
+	{R367CAB_ALLPASSFILT6,	0x45},
+	{R367CAB_ALLPASSFILT7,	0x6f},
+	{R367CAB_ALLPASSFILT8,	0x7e},
+	{R367CAB_ALLPASSFILT9,	0x05},
+	{R367CAB_ALLPASSFILT10,	0x0a},
+	{R367CAB_ALLPASSFILT11,	0x51},
+	{R367CAB_TRL_AGC_CFG,	0x20},
+	{R367CAB_TRL_LPF_CFG,	0x28},
+	{R367CAB_TRL_LPF_ACQ_GAIN,	0x44},
+	{R367CAB_TRL_LPF_TRK_GAIN,	0x22},
+	{R367CAB_TRL_LPF_OUT_GAIN,	0x03},
+	{R367CAB_TRL_LOCKDET_LTH,	0x04},
+	{R367CAB_TRL_LOCKDET_HTH,	0x11},
+	{R367CAB_TRL_LOCKDET_TRGVAL,	0x20},
+	{R367CAB_IQ_QAM,	0x01},
+	{R367CAB_FSM_STATE,	0xa0},
+	{R367CAB_FSM_CTL,	0x08},
+	{R367CAB_FSM_STS,	0x0c},
+	{R367CAB_FSM_SNR0_HTH,	0x00},
+	{R367CAB_FSM_SNR1_HTH,	0x00},
+	{R367CAB_FSM_SNR2_HTH,	0x23},/* 0x00 */
+	{R367CAB_FSM_SNR0_LTH,	0x00},
+	{R367CAB_FSM_SNR1_LTH,	0x00},
+	{R367CAB_FSM_EQA1_HTH,	0x00},
+	{R367CAB_FSM_TEMPO,	0x32},
+	{R367CAB_FSM_CONFIG,	0x03},
+	{R367CAB_EQU_I_TESTTAP_L,	0x11},
+	{R367CAB_EQU_I_TESTTAP_M,	0x00},
+	{R367CAB_EQU_I_TESTTAP_H,	0x00},
+	{R367CAB_EQU_TESTAP_CFG,	0x00},
+	{R367CAB_EQU_Q_TESTTAP_L,	0xff},
+	{R367CAB_EQU_Q_TESTTAP_M,	0x00},
+	{R367CAB_EQU_Q_TESTTAP_H,	0x00},
+	{R367CAB_EQU_TAP_CTRL,	0x00},
+	{R367CAB_EQU_CTR_CRL_CONTROL_L,	0x11},
+	{R367CAB_EQU_CTR_CRL_CONTROL_H,	0x05},
+	{R367CAB_EQU_CTR_HIPOW_L,	0x00},
+	{R367CAB_EQU_CTR_HIPOW_H,	0x00},
+	{R367CAB_EQU_I_EQU_LO,	0xef},
+	{R367CAB_EQU_I_EQU_HI,	0x00},
+	{R367CAB_EQU_Q_EQU_LO,	0xee},
+	{R367CAB_EQU_Q_EQU_HI,	0x00},
+	{R367CAB_EQU_MAPPER,	0xc5},
+	{R367CAB_EQU_SWEEP_RATE,	0x80},
+	{R367CAB_EQU_SNR_LO,	0x64},
+	{R367CAB_EQU_SNR_HI,	0x03},
+	{R367CAB_EQU_GAMMA_LO,	0x00},
+	{R367CAB_EQU_GAMMA_HI,	0x00},
+	{R367CAB_EQU_ERR_GAIN,	0x36},
+	{R367CAB_EQU_RADIUS,	0xaa},
+	{R367CAB_EQU_FFE_MAINTAP,	0x00},
+	{R367CAB_EQU_FFE_LEAKAGE,	0x63},
+	{R367CAB_EQU_FFE_MAINTAP_POS,	0xdf},
+	{R367CAB_EQU_GAIN_WIDE,	0x88},
+	{R367CAB_EQU_GAIN_NARROW,	0x41},
+	{R367CAB_EQU_CTR_LPF_GAIN,	0xd1},
+	{R367CAB_EQU_CRL_LPF_GAIN,	0xa7},
+	{R367CAB_EQU_GLOBAL_GAIN,	0x06},
+	{R367CAB_EQU_CRL_LD_SEN,	0x85},
+	{R367CAB_EQU_CRL_LD_VAL,	0xe2},
+	{R367CAB_EQU_CRL_TFR,	0x20},
+	{R367CAB_EQU_CRL_BISTH_LO,	0x00},
+	{R367CAB_EQU_CRL_BISTH_HI,	0x00},
+	{R367CAB_EQU_SWEEP_RANGE_LO,	0x00},
+	{R367CAB_EQU_SWEEP_RANGE_HI,	0x00},
+	{R367CAB_EQU_CRL_LIMITER,	0x40},
+	{R367CAB_EQU_MODULUS_MAP,	0x90},
+	{R367CAB_EQU_PNT_GAIN,	0xa7},
+	{R367CAB_FEC_AC_CTR_0,	0x16},
+	{R367CAB_FEC_AC_CTR_1,	0x0b},
+	{R367CAB_FEC_AC_CTR_2,	0x88},
+	{R367CAB_FEC_AC_CTR_3,	0x02},
+	{R367CAB_FEC_STATUS,	0x12},
+	{R367CAB_RS_COUNTER_0,	0x7d},
+	{R367CAB_RS_COUNTER_1,	0xd0},
+	{R367CAB_RS_COUNTER_2,	0x19},
+	{R367CAB_RS_COUNTER_3,	0x0b},
+	{R367CAB_RS_COUNTER_4,	0xa3},
+	{R367CAB_RS_COUNTER_5,	0x00},
+	{R367CAB_BERT_0,	0x01},
+	{R367CAB_BERT_1,	0x25},
+	{R367CAB_BERT_2,	0x41},
+	{R367CAB_BERT_3,	0x39},
+	{R367CAB_OUTFORMAT_0,	0xc2},
+	{R367CAB_OUTFORMAT_1,	0x22},
+	{R367CAB_SMOOTHER_2,	0x28},
+	{R367CAB_TSMF_CTRL_0,	0x01},
+	{R367CAB_TSMF_CTRL_1,	0xc6},
+	{R367CAB_TSMF_CTRL_3,	0x43},
+	{R367CAB_TS_ON_ID_0,	0x00},
+	{R367CAB_TS_ON_ID_1,	0x00},
+	{R367CAB_TS_ON_ID_2,	0x00},
+	{R367CAB_TS_ON_ID_3,	0x00},
+	{R367CAB_RE_STATUS_0,	0x00},
+	{R367CAB_RE_STATUS_1,	0x00},
+	{R367CAB_RE_STATUS_2,	0x00},
+	{R367CAB_RE_STATUS_3,	0x00},
+	{R367CAB_TS_STATUS_0,	0x00},
+	{R367CAB_TS_STATUS_1,	0x00},
+	{R367CAB_TS_STATUS_2,	0xa0},
+	{R367CAB_TS_STATUS_3,	0x00},
+	{R367CAB_T_O_ID_0,	0x00},
+	{R367CAB_T_O_ID_1,	0x00},
+	{R367CAB_T_O_ID_2,	0x00},
+	{R367CAB_T_O_ID_3,	0x00},
+	{0x0000,		0x00},
+};
+
+/**************
+ *
+ * Defaults / Tables for Digital Devices C/T Cine/Flex devices
+ *
+ **************/
+
+static const struct st_register def0367dd_ofdm[] = {
+	{R367TER_AGC2MAX,                0xff},
+	{R367TER_AGC2MIN,                0x00},
+	{R367TER_AGC1MAX,                0xff},
+	{R367TER_AGC1MIN,                0x00},
+	{R367TER_AGCR,                   0xbc},
+	{R367TER_AGC2TH,                 0x00},
+	{R367TER_AGCCTRL1,               0x85},
+	{R367TER_AGCCTRL2,               0x1f},
+	{R367TER_AGC1VAL1,               0x00},
+	{R367TER_AGC1VAL2,               0x00},
+	{R367TER_AGC2VAL1,               0x6f},
+	{R367TER_AGC2VAL2,               0x05},
+	{R367TER_AGC2PGA,                0x00},
+	{R367TER_OVF_RATE1,              0x00},
+	{R367TER_OVF_RATE2,              0x00},
+	{R367TER_GAIN_SRC1,              0x2b},
+	{R367TER_GAIN_SRC2,              0x04},
+	{R367TER_INC_DEROT1,             0x55},
+	{R367TER_INC_DEROT2,             0x55},
+	{R367TER_PPM_CPAMP_DIR,          0x2c},
+	{R367TER_PPM_CPAMP_INV,          0x00},
+	{R367TER_FREESTFE_1,             0x00},
+	{R367TER_FREESTFE_2,             0x1c},
+	{R367TER_DCOFFSET,               0x00},
+	{R367TER_EN_PROCESS,             0x05},
+	{R367TER_SDI_SMOOTHER,           0x80},
+	{R367TER_FE_LOOP_OPEN,           0x1c},
+	{R367TER_FREQOFF1,               0x00},
+	{R367TER_FREQOFF2,               0x00},
+	{R367TER_FREQOFF3,               0x00},
+	{R367TER_TIMOFF1,                0x00},
+	{R367TER_TIMOFF2,                0x00},
+	{R367TER_EPQ,                    0x02},
+	{R367TER_EPQAUTO,                0x01},
+	{R367TER_SYR_UPDATE,             0xf5},
+	{R367TER_CHPFREE,                0x00},
+	{R367TER_PPM_STATE_MAC,          0x23},
+	{R367TER_INR_THRESHOLD,          0xff},
+	{R367TER_EPQ_TPS_ID_CELL,        0xf9},
+	{R367TER_EPQ_CFG,                0x00},
+	{R367TER_EPQ_STATUS,             0x01},
+	{R367TER_AUTORELOCK,             0x81},
+	{R367TER_BER_THR_VMSB,           0x00},
+	{R367TER_BER_THR_MSB,            0x00},
+	{R367TER_BER_THR_LSB,            0x00},
+	{R367TER_CCD,                    0x83},
+	{R367TER_SPECTR_CFG,             0x00},
+	{R367TER_CHC_DUMMY,              0x18},
+	{R367TER_INC_CTL,                0x88},
+	{R367TER_INCTHRES_COR1,          0xb4},
+	{R367TER_INCTHRES_COR2,          0x96},
+	{R367TER_INCTHRES_DET1,          0x0e},
+	{R367TER_INCTHRES_DET2,          0x11},
+	{R367TER_IIR_CELLNB,             0x8d},
+	{R367TER_IIRCX_COEFF1_MSB,       0x00},
+	{R367TER_IIRCX_COEFF1_LSB,       0x00},
+	{R367TER_IIRCX_COEFF2_MSB,       0x09},
+	{R367TER_IIRCX_COEFF2_LSB,       0x18},
+	{R367TER_IIRCX_COEFF3_MSB,       0x14},
+	{R367TER_IIRCX_COEFF3_LSB,       0x9c},
+	{R367TER_IIRCX_COEFF4_MSB,       0x00},
+	{R367TER_IIRCX_COEFF4_LSB,       0x00},
+	{R367TER_IIRCX_COEFF5_MSB,       0x36},
+	{R367TER_IIRCX_COEFF5_LSB,       0x42},
+	{R367TER_FEPATH_CFG,             0x00},
+	{R367TER_PMC1_FUNC,              0x65},
+	{R367TER_PMC1_FOR,               0x00},
+	{R367TER_PMC2_FUNC,              0x00},
+	{R367TER_STATUS_ERR_DA,          0xe0},
+	{R367TER_DIG_AGC_R,              0xfe},
+	{R367TER_COMAGC_TARMSB,          0x0b},
+	{R367TER_COM_AGC_TAR_ENMODE,     0x41},
+	{R367TER_COM_AGC_CFG,            0x3e},
+	{R367TER_COM_AGC_GAIN1,          0x39},
+	{R367TER_AUT_AGC_TARGETMSB,      0x0b},
+	{R367TER_LOCK_DET_MSB,           0x01},
+	{R367TER_AGCTAR_LOCK_LSBS,       0x40},
+	{R367TER_AUT_GAIN_EN,            0xf4},
+	{R367TER_AUT_CFG,                0xf0},
+	{R367TER_LOCKN,                  0x23},
+	{R367TER_INT_X_3,                0x00},
+	{R367TER_INT_X_2,                0x03},
+	{R367TER_INT_X_1,                0x8d},
+	{R367TER_INT_X_0,                0xa0},
+	{R367TER_MIN_ERRX_MSB,           0x00},
+	{R367TER_COR_CTL,                0x00},
+	{R367TER_COR_STAT,               0xf6},
+	{R367TER_COR_INTEN,              0x00},
+	{R367TER_COR_INTSTAT,            0x3f},
+	{R367TER_COR_MODEGUARD,          0x03},
+	{R367TER_AGC_CTL,                0x08},
+	{R367TER_AGC_MANUAL1,            0x00},
+	{R367TER_AGC_MANUAL2,            0x00},
+	{R367TER_AGC_TARG,               0x16},
+	{R367TER_AGC_GAIN1,              0x53},
+	{R367TER_AGC_GAIN2,              0x1d},
+	{R367TER_RESERVED_1,             0x00},
+	{R367TER_RESERVED_2,             0x00},
+	{R367TER_RESERVED_3,             0x00},
+	{R367TER_CAS_CTL,                0x44},
+	{R367TER_CAS_FREQ,               0xb3},
+	{R367TER_CAS_DAGCGAIN,           0x12},
+	{R367TER_SYR_CTL,                0x04},
+	{R367TER_SYR_STAT,               0x10},
+	{R367TER_SYR_NCO1,               0x00},
+	{R367TER_SYR_NCO2,               0x00},
+	{R367TER_SYR_OFFSET1,            0x00},
+	{R367TER_SYR_OFFSET2,            0x00},
+	{R367TER_FFT_CTL,                0x00},
+	{R367TER_SCR_CTL,                0x70},
+	{R367TER_PPM_CTL1,               0xf8},
+	{R367TER_TRL_CTL,                0xac},
+	{R367TER_TRL_NOMRATE1,           0x1e},
+	{R367TER_TRL_NOMRATE2,           0x58},
+	{R367TER_TRL_TIME1,              0x1d},
+	{R367TER_TRL_TIME2,              0xfc},
+	{R367TER_CRL_CTL,                0x24},
+	{R367TER_CRL_FREQ1,              0xad},
+	{R367TER_CRL_FREQ2,              0x9d},
+	{R367TER_CRL_FREQ3,              0xff},
+	{R367TER_CHC_CTL,                0x01},
+	{R367TER_CHC_SNR,                0xf0},
+	{R367TER_BDI_CTL,                0x00},
+	{R367TER_DMP_CTL,                0x00},
+	{R367TER_TPS_RCVD1,              0x30},
+	{R367TER_TPS_RCVD2,              0x02},
+	{R367TER_TPS_RCVD3,              0x01},
+	{R367TER_TPS_RCVD4,              0x00},
+	{R367TER_TPS_ID_CELL1,           0x00},
+	{R367TER_TPS_ID_CELL2,           0x00},
+	{R367TER_TPS_RCVD5_SET1,         0x02},
+	{R367TER_TPS_SET2,               0x02},
+	{R367TER_TPS_SET3,               0x01},
+	{R367TER_TPS_CTL,                0x00},
+	{R367TER_CTL_FFTOSNUM,           0x34},
+	{R367TER_TESTSELECT,             0x09},
+	{R367TER_MSC_REV,                0x0a},
+	{R367TER_PIR_CTL,                0x00},
+	{R367TER_SNR_CARRIER1,           0xa1},
+	{R367TER_SNR_CARRIER2,           0x9a},
+	{R367TER_PPM_CPAMP,              0x2c},
+	{R367TER_TSM_AP0,                0x00},
+	{R367TER_TSM_AP1,                0x00},
+	{R367TER_TSM_AP2,                0x00},
+	{R367TER_TSM_AP3,                0x00},
+	{R367TER_TSM_AP4,                0x00},
+	{R367TER_TSM_AP5,                0x00},
+	{R367TER_TSM_AP6,                0x00},
+	{R367TER_TSM_AP7,                0x00},
+	{R367TER_CONSTMODE,              0x01},
+	{R367TER_CONSTCARR1,             0x00},
+	{R367TER_CONSTCARR2,             0x00},
+	{R367TER_ICONSTEL,               0x0a},
+	{R367TER_QCONSTEL,               0x15},
+	{R367TER_TSTBISTRES0,            0x00},
+	{R367TER_TSTBISTRES1,            0x00},
+	{R367TER_TSTBISTRES2,            0x28},
+	{R367TER_TSTBISTRES3,            0x00},
+	{R367TER_SYR_TARGET_FFTADJT_MSB, 0x00},
+	{R367TER_SYR_TARGET_FFTADJT_LSB, 0x00},
+	{R367TER_SYR_TARGET_CHCADJT_MSB, 0x00},
+	{R367TER_SYR_TARGET_CHCADJT_LSB, 0x00},
+	{R367TER_SYR_FLAG,               0x00},
+	{R367TER_CRL_TARGET1,            0x00},
+	{R367TER_CRL_TARGET2,            0x00},
+	{R367TER_CRL_TARGET3,            0x00},
+	{R367TER_CRL_TARGET4,            0x00},
+	{R367TER_CRL_FLAG,               0x00},
+	{R367TER_TRL_TARGET1,            0x00},
+	{R367TER_TRL_TARGET2,            0x00},
+	{R367TER_TRL_CHC,                0x00},
+	{R367TER_CHC_SNR_TARG,           0x00},
+	{R367TER_TOP_TRACK,              0x00},
+	{R367TER_TRACKER_FREE1,          0x00},
+	{R367TER_ERROR_CRL1,             0x00},
+	{R367TER_ERROR_CRL2,             0x00},
+	{R367TER_ERROR_CRL3,             0x00},
+	{R367TER_ERROR_CRL4,             0x00},
+	{R367TER_DEC_NCO1,               0x2c},
+	{R367TER_DEC_NCO2,               0x0f},
+	{R367TER_DEC_NCO3,               0x20},
+	{R367TER_SNR,                    0xf1},
+	{R367TER_SYR_FFTADJ1,            0x00},
+	{R367TER_SYR_FFTADJ2,            0x00},
+	{R367TER_SYR_CHCADJ1,            0x00},
+	{R367TER_SYR_CHCADJ2,            0x00},
+	{R367TER_SYR_OFF,                0x00},
+	{R367TER_PPM_OFFSET1,            0x00},
+	{R367TER_PPM_OFFSET2,            0x03},
+	{R367TER_TRACKER_FREE2,          0x00},
+	{R367TER_DEBG_LT10,              0x00},
+	{R367TER_DEBG_LT11,              0x00},
+	{R367TER_DEBG_LT12,              0x00},
+	{R367TER_DEBG_LT13,              0x00},
+	{R367TER_DEBG_LT14,              0x00},
+	{R367TER_DEBG_LT15,              0x00},
+	{R367TER_DEBG_LT16,              0x00},
+	{R367TER_DEBG_LT17,              0x00},
+	{R367TER_DEBG_LT18,              0x00},
+	{R367TER_DEBG_LT19,              0x00},
+	{R367TER_DEBG_LT1A,              0x00},
+	{R367TER_DEBG_LT1B,              0x00},
+	{R367TER_DEBG_LT1C,              0x00},
+	{R367TER_DEBG_LT1D,              0x00},
+	{R367TER_DEBG_LT1E,              0x00},
+	{R367TER_DEBG_LT1F,              0x00},
+	{R367TER_RCCFGH,                 0x00},
+	{R367TER_RCCFGM,                 0x00},
+	{R367TER_RCCFGL,                 0x00},
+	{R367TER_RCINSDELH,              0x00},
+	{R367TER_RCINSDELM,              0x00},
+	{R367TER_RCINSDELL,              0x00},
+	{R367TER_RCSTATUS,               0x00},
+	{R367TER_RCSPEED,                0x6f},
+	{R367TER_RCDEBUGM,               0xe7},
+	{R367TER_RCDEBUGL,               0x9b},
+	{R367TER_RCOBSCFG,               0x00},
+	{R367TER_RCOBSM,                 0x00},
+	{R367TER_RCOBSL,                 0x00},
+	{R367TER_RCFECSPY,               0x00},
+	{R367TER_RCFSPYCFG,              0x00},
+	{R367TER_RCFSPYDATA,             0x00},
+	{R367TER_RCFSPYOUT,              0x00},
+	{R367TER_RCFSTATUS,              0x00},
+	{R367TER_RCFGOODPACK,            0x00},
+	{R367TER_RCFPACKCNT,             0x00},
+	{R367TER_RCFSPYMISC,             0x00},
+	{R367TER_RCFBERCPT4,             0x00},
+	{R367TER_RCFBERCPT3,             0x00},
+	{R367TER_RCFBERCPT2,             0x00},
+	{R367TER_RCFBERCPT1,             0x00},
+	{R367TER_RCFBERCPT0,             0x00},
+	{R367TER_RCFBERERR2,             0x00},
+	{R367TER_RCFBERERR1,             0x00},
+	{R367TER_RCFBERERR0,             0x00},
+	{R367TER_RCFSTATESM,             0x00},
+	{R367TER_RCFSTATESL,             0x00},
+	{R367TER_RCFSPYBER,              0x00},
+	{R367TER_RCFSPYDISTM,            0x00},
+	{R367TER_RCFSPYDISTL,            0x00},
+	{R367TER_RCFSPYOBS7,             0x00},
+	{R367TER_RCFSPYOBS6,             0x00},
+	{R367TER_RCFSPYOBS5,             0x00},
+	{R367TER_RCFSPYOBS4,             0x00},
+	{R367TER_RCFSPYOBS3,             0x00},
+	{R367TER_RCFSPYOBS2,             0x00},
+	{R367TER_RCFSPYOBS1,             0x00},
+	{R367TER_RCFSPYOBS0,             0x00},
+	{R367TER_FECM,                   0x01},
+	{R367TER_VTH12,                  0xff},
+	{R367TER_VTH23,                  0xa1},
+	{R367TER_VTH34,                  0x64},
+	{R367TER_VTH56,                  0x40},
+	{R367TER_VTH67,                  0x00},
+	{R367TER_VTH78,                  0x2c},
+	{R367TER_VITCURPUN,              0x12},
+	{R367TER_VERROR,                 0x01},
+	{R367TER_PRVIT,                  0x3f},
+	{R367TER_VAVSRVIT,               0x00},
+	{R367TER_VSTATUSVIT,             0xbd},
+	{R367TER_VTHINUSE,               0xa1},
+	{R367TER_KDIV12,                 0x20},
+	{R367TER_KDIV23,                 0x40},
+	{R367TER_KDIV34,                 0x20},
+	{R367TER_KDIV56,                 0x30},
+	{R367TER_KDIV67,                 0x00},
+	{R367TER_KDIV78,                 0x30},
+	{R367TER_SIGPOWER,               0x54},
+	{R367TER_DEMAPVIT,               0x40},
+	{R367TER_VITSCALE,               0x00},
+	{R367TER_FFEC1PRG,               0x00},
+	{R367TER_FVITCURPUN,             0x12},
+	{R367TER_FVERROR,                0x01},
+	{R367TER_FVSTATUSVIT,            0xbd},
+	{R367TER_DEBUG_LT1,              0x00},
+	{R367TER_DEBUG_LT2,              0x00},
+	{R367TER_DEBUG_LT3,              0x00},
+	{R367TER_TSTSFMET,               0x00},
+	{R367TER_SELOUT,                 0x00},
+	{R367TER_TSYNC,                  0x00},
+	{R367TER_TSTERR,                 0x00},
+	{R367TER_TSFSYNC,                0x00},
+	{R367TER_TSTSFERR,               0x00},
+	{R367TER_TSTTSSF1,               0x01},
+	{R367TER_TSTTSSF2,               0x1f},
+	{R367TER_TSTTSSF3,               0x00},
+	{R367TER_TSTTS1,                 0x00},
+	{R367TER_TSTTS2,                 0x1f},
+	{R367TER_TSTTS3,                 0x01},
+	{R367TER_TSTTS4,                 0x00},
+	{R367TER_TSTTSRC,                0x00},
+	{R367TER_TSTTSRS,                0x00},
+	{R367TER_TSSTATEM,               0xb0},
+	{R367TER_TSSTATEL,               0x40},
+	{R367TER_TSCFGH,                 0x80},
+	{R367TER_TSCFGM,                 0x00},
+	{R367TER_TSCFGL,                 0x20},
+	{R367TER_TSSYNC,                 0x00},
+	{R367TER_TSINSDELH,              0x00},
+	{R367TER_TSINSDELM,              0x00},
+	{R367TER_TSINSDELL,              0x00},
+	{R367TER_TSDIVN,                 0x03},
+	{R367TER_TSDIVPM,                0x00},
+	{R367TER_TSDIVPL,                0x00},
+	{R367TER_TSDIVQM,                0x00},
+	{R367TER_TSDIVQL,                0x00},
+	{R367TER_TSDILSTKM,              0x00},
+	{R367TER_TSDILSTKL,              0x00},
+	{R367TER_TSSPEED,                0x6f},
+	{R367TER_TSSTATUS,               0x81},
+	{R367TER_TSSTATUS2,              0x6a},
+	{R367TER_TSBITRATEM,             0x0f},
+	{R367TER_TSBITRATEL,             0xc6},
+	{R367TER_TSPACKLENM,             0x00},
+	{R367TER_TSPACKLENL,             0xfc},
+	{R367TER_TSBLOCLENM,             0x0a},
+	{R367TER_TSBLOCLENL,             0x80},
+	{R367TER_TSDLYH,                 0x90},
+	{R367TER_TSDLYM,                 0x68},
+	{R367TER_TSDLYL,                 0x01},
+	{R367TER_TSNPDAV,                0x00},
+	{R367TER_TSBUFSTATH,             0x00},
+	{R367TER_TSBUFSTATM,             0x00},
+	{R367TER_TSBUFSTATL,             0x00},
+	{R367TER_TSDEBUGM,               0xcf},
+	{R367TER_TSDEBUGL,               0x1e},
+	{R367TER_TSDLYSETH,              0x00},
+	{R367TER_TSDLYSETM,              0x68},
+	{R367TER_TSDLYSETL,              0x00},
+	{R367TER_TSOBSCFG,               0x00},
+	{R367TER_TSOBSM,                 0x47},
+	{R367TER_TSOBSL,                 0x1f},
+	{R367TER_ERRCTRL1,               0x95},
+	{R367TER_ERRCNT1H,               0x80},
+	{R367TER_ERRCNT1M,               0x00},
+	{R367TER_ERRCNT1L,               0x00},
+	{R367TER_ERRCTRL2,               0x95},
+	{R367TER_ERRCNT2H,               0x00},
+	{R367TER_ERRCNT2M,               0x00},
+	{R367TER_ERRCNT2L,               0x00},
+	{R367TER_FECSPY,                 0x88},
+	{R367TER_FSPYCFG,                0x2c},
+	{R367TER_FSPYDATA,               0x3a},
+	{R367TER_FSPYOUT,                0x06},
+	{R367TER_FSTATUS,                0x61},
+	{R367TER_FGOODPACK,              0xff},
+	{R367TER_FPACKCNT,               0xff},
+	{R367TER_FSPYMISC,               0x66},
+	{R367TER_FBERCPT4,               0x00},
+	{R367TER_FBERCPT3,               0x00},
+	{R367TER_FBERCPT2,               0x36},
+	{R367TER_FBERCPT1,               0x36},
+	{R367TER_FBERCPT0,               0x14},
+	{R367TER_FBERERR2,               0x00},
+	{R367TER_FBERERR1,               0x03},
+	{R367TER_FBERERR0,               0x28},
+	{R367TER_FSTATESM,               0x00},
+	{R367TER_FSTATESL,               0x02},
+	{R367TER_FSPYBER,                0x00},
+	{R367TER_FSPYDISTM,              0x01},
+	{R367TER_FSPYDISTL,              0x9f},
+	{R367TER_FSPYOBS7,               0xc9},
+	{R367TER_FSPYOBS6,               0x99},
+	{R367TER_FSPYOBS5,               0x08},
+	{R367TER_FSPYOBS4,               0xec},
+	{R367TER_FSPYOBS3,               0x01},
+	{R367TER_FSPYOBS2,               0x0f},
+	{R367TER_FSPYOBS1,               0xf5},
+	{R367TER_FSPYOBS0,               0x08},
+	{R367TER_SFDEMAP,                0x40},
+	{R367TER_SFERROR,                0x00},
+	{R367TER_SFAVSR,                 0x30},
+	{R367TER_SFECSTATUS,             0xcc},
+	{R367TER_SFKDIV12,               0x20},
+	{R367TER_SFKDIV23,               0x40},
+	{R367TER_SFKDIV34,               0x20},
+	{R367TER_SFKDIV56,               0x20},
+	{R367TER_SFKDIV67,               0x00},
+	{R367TER_SFKDIV78,               0x20},
+	{R367TER_SFDILSTKM,              0x00},
+	{R367TER_SFDILSTKL,              0x00},
+	{R367TER_SFSTATUS,               0xb5},
+	{R367TER_SFDLYH,                 0x90},
+	{R367TER_SFDLYM,                 0x60},
+	{R367TER_SFDLYL,                 0x01},
+	{R367TER_SFDLYSETH,              0xc0},
+	{R367TER_SFDLYSETM,              0x60},
+	{R367TER_SFDLYSETL,              0x00},
+	{R367TER_SFOBSCFG,               0x00},
+	{R367TER_SFOBSM,                 0x47},
+	{R367TER_SFOBSL,                 0x05},
+	{R367TER_SFECINFO,               0x40},
+	{R367TER_SFERRCTRL,              0x74},
+	{R367TER_SFERRCNTH,              0x80},
+	{R367TER_SFERRCNTM,              0x00},
+	{R367TER_SFERRCNTL,              0x00},
+	{R367TER_SYMBRATEM,              0x2f},
+	{R367TER_SYMBRATEL,              0x50},
+	{R367TER_SYMBSTATUS,             0x7f},
+	{R367TER_SYMBCFG,                0x00},
+	{R367TER_SYMBFIFOM,              0xf4},
+	{R367TER_SYMBFIFOL,              0x0d},
+	{R367TER_SYMBOFFSM,              0xf0},
+	{R367TER_SYMBOFFSL,              0x2d},
+	{0x0000, 0x00} /* EOT */
+};
+
+static const struct st_register def0367dd_qam[] = {
+	{R367CAB_CTRL_1,                  0x06}, /* Orginal 0x04 */
+	{R367CAB_CTRL_2,                  0x03},
+	{R367CAB_IT_STATUS1,              0x2b},
+	{R367CAB_IT_STATUS2,              0x08},
+	{R367CAB_IT_EN1,                  0x00},
+	{R367CAB_IT_EN2,                  0x00},
+	{R367CAB_CTRL_STATUS,             0x04},
+	{R367CAB_TEST_CTL,                0x00},
+	{R367CAB_AGC_CTL,                 0x73},
+	{R367CAB_AGC_IF_CFG,              0x50},
+	{R367CAB_AGC_RF_CFG,              0x02}, /* RF Freeze */
+	{R367CAB_AGC_PWM_CFG,             0x03},
+	{R367CAB_AGC_PWR_REF_L,           0x5a},
+	{R367CAB_AGC_PWR_REF_H,           0x00},
+	{R367CAB_AGC_RF_TH_L,             0xff},
+	{R367CAB_AGC_RF_TH_H,             0x07},
+	{R367CAB_AGC_IF_LTH_L,            0x00},
+	{R367CAB_AGC_IF_LTH_H,            0x08},
+	{R367CAB_AGC_IF_HTH_L,            0xff},
+	{R367CAB_AGC_IF_HTH_H,            0x07},
+	{R367CAB_AGC_PWR_RD_L,            0xa0},
+	{R367CAB_AGC_PWR_RD_M,            0xe9},
+	{R367CAB_AGC_PWR_RD_H,            0x03},
+	{R367CAB_AGC_PWM_IFCMD_L,         0xe4},
+	{R367CAB_AGC_PWM_IFCMD_H,         0x00},
+	{R367CAB_AGC_PWM_RFCMD_L,         0xff},
+	{R367CAB_AGC_PWM_RFCMD_H,         0x07},
+	{R367CAB_IQDEM_CFG,               0x01},
+	{R367CAB_MIX_NCO_LL,              0x22},
+	{R367CAB_MIX_NCO_HL,              0x96},
+	{R367CAB_MIX_NCO_HH,              0x55},
+	{R367CAB_SRC_NCO_LL,              0xff},
+	{R367CAB_SRC_NCO_LH,              0x0c},
+	{R367CAB_SRC_NCO_HL,              0xf5},
+	{R367CAB_SRC_NCO_HH,              0x20},
+	{R367CAB_IQDEM_GAIN_SRC_L,        0x06},
+	{R367CAB_IQDEM_GAIN_SRC_H,        0x01},
+	{R367CAB_IQDEM_DCRM_CFG_LL,       0xfe},
+	{R367CAB_IQDEM_DCRM_CFG_LH,       0xff},
+	{R367CAB_IQDEM_DCRM_CFG_HL,       0x0f},
+	{R367CAB_IQDEM_DCRM_CFG_HH,       0x00},
+	{R367CAB_IQDEM_ADJ_COEFF0,        0x34},
+	{R367CAB_IQDEM_ADJ_COEFF1,        0xae},
+	{R367CAB_IQDEM_ADJ_COEFF2,        0x46},
+	{R367CAB_IQDEM_ADJ_COEFF3,        0x77},
+	{R367CAB_IQDEM_ADJ_COEFF4,        0x96},
+	{R367CAB_IQDEM_ADJ_COEFF5,        0x69},
+	{R367CAB_IQDEM_ADJ_COEFF6,        0xc7},
+	{R367CAB_IQDEM_ADJ_COEFF7,        0x01},
+	{R367CAB_IQDEM_ADJ_EN,            0x04},
+	{R367CAB_IQDEM_ADJ_AGC_REF,       0x94},
+	{R367CAB_ALLPASSFILT1,            0xc9},
+	{R367CAB_ALLPASSFILT2,            0x2d},
+	{R367CAB_ALLPASSFILT3,            0xa3},
+	{R367CAB_ALLPASSFILT4,            0xfb},
+	{R367CAB_ALLPASSFILT5,            0xf6},
+	{R367CAB_ALLPASSFILT6,            0x45},
+	{R367CAB_ALLPASSFILT7,            0x6f},
+	{R367CAB_ALLPASSFILT8,            0x7e},
+	{R367CAB_ALLPASSFILT9,            0x05},
+	{R367CAB_ALLPASSFILT10,           0x0a},
+	{R367CAB_ALLPASSFILT11,           0x51},
+	{R367CAB_TRL_AGC_CFG,             0x20},
+	{R367CAB_TRL_LPF_CFG,             0x28},
+	{R367CAB_TRL_LPF_ACQ_GAIN,        0x44},
+	{R367CAB_TRL_LPF_TRK_GAIN,        0x22},
+	{R367CAB_TRL_LPF_OUT_GAIN,        0x03},
+	{R367CAB_TRL_LOCKDET_LTH,         0x04},
+	{R367CAB_TRL_LOCKDET_HTH,         0x11},
+	{R367CAB_TRL_LOCKDET_TRGVAL,      0x20},
+	{R367CAB_IQ_QAM,                  0x01},
+	{R367CAB_FSM_STATE,               0xa0},
+	{R367CAB_FSM_CTL,                 0x08},
+	{R367CAB_FSM_STS,                 0x0c},
+	{R367CAB_FSM_SNR0_HTH,            0x00},
+	{R367CAB_FSM_SNR1_HTH,            0x00},
+	{R367CAB_FSM_SNR2_HTH,            0x00},
+	{R367CAB_FSM_SNR0_LTH,            0x00},
+	{R367CAB_FSM_SNR1_LTH,            0x00},
+	{R367CAB_FSM_EQA1_HTH,            0x00},
+	{R367CAB_FSM_TEMPO,               0x32},
+	{R367CAB_FSM_CONFIG,              0x03},
+	{R367CAB_EQU_I_TESTTAP_L,         0x11},
+	{R367CAB_EQU_I_TESTTAP_M,         0x00},
+	{R367CAB_EQU_I_TESTTAP_H,         0x00},
+	{R367CAB_EQU_TESTAP_CFG,          0x00},
+	{R367CAB_EQU_Q_TESTTAP_L,         0xff},
+	{R367CAB_EQU_Q_TESTTAP_M,         0x00},
+	{R367CAB_EQU_Q_TESTTAP_H,         0x00},
+	{R367CAB_EQU_TAP_CTRL,            0x00},
+	{R367CAB_EQU_CTR_CRL_CONTROL_L,   0x11},
+	{R367CAB_EQU_CTR_CRL_CONTROL_H,   0x05},
+	{R367CAB_EQU_CTR_HIPOW_L,         0x00},
+	{R367CAB_EQU_CTR_HIPOW_H,         0x00},
+	{R367CAB_EQU_I_EQU_LO,            0xef},
+	{R367CAB_EQU_I_EQU_HI,            0x00},
+	{R367CAB_EQU_Q_EQU_LO,            0xee},
+	{R367CAB_EQU_Q_EQU_HI,            0x00},
+	{R367CAB_EQU_MAPPER,              0xc5},
+	{R367CAB_EQU_SWEEP_RATE,          0x80},
+	{R367CAB_EQU_SNR_LO,              0x64},
+	{R367CAB_EQU_SNR_HI,              0x03},
+	{R367CAB_EQU_GAMMA_LO,            0x00},
+	{R367CAB_EQU_GAMMA_HI,            0x00},
+	{R367CAB_EQU_ERR_GAIN,            0x36},
+	{R367CAB_EQU_RADIUS,              0xaa},
+	{R367CAB_EQU_FFE_MAINTAP,         0x00},
+	{R367CAB_EQU_FFE_LEAKAGE,         0x63},
+	{R367CAB_EQU_FFE_MAINTAP_POS,     0xdf},
+	{R367CAB_EQU_GAIN_WIDE,           0x88},
+	{R367CAB_EQU_GAIN_NARROW,         0x41},
+	{R367CAB_EQU_CTR_LPF_GAIN,        0xd1},
+	{R367CAB_EQU_CRL_LPF_GAIN,        0xa7},
+	{R367CAB_EQU_GLOBAL_GAIN,         0x06},
+	{R367CAB_EQU_CRL_LD_SEN,          0x85},
+	{R367CAB_EQU_CRL_LD_VAL,          0xe2},
+	{R367CAB_EQU_CRL_TFR,             0x20},
+	{R367CAB_EQU_CRL_BISTH_LO,        0x00},
+	{R367CAB_EQU_CRL_BISTH_HI,        0x00},
+	{R367CAB_EQU_SWEEP_RANGE_LO,      0x00},
+	{R367CAB_EQU_SWEEP_RANGE_HI,      0x00},
+	{R367CAB_EQU_CRL_LIMITER,         0x40},
+	{R367CAB_EQU_MODULUS_MAP,         0x90},
+	{R367CAB_EQU_PNT_GAIN,            0xa7},
+	{R367CAB_FEC_AC_CTR_0,            0x16},
+	{R367CAB_FEC_AC_CTR_1,            0x0b},
+	{R367CAB_FEC_AC_CTR_2,            0x88},
+	{R367CAB_FEC_AC_CTR_3,            0x02},
+	{R367CAB_FEC_STATUS,              0x12},
+	{R367CAB_RS_COUNTER_0,            0x7d},
+	{R367CAB_RS_COUNTER_1,            0xd0},
+	{R367CAB_RS_COUNTER_2,            0x19},
+	{R367CAB_RS_COUNTER_3,            0x0b},
+	{R367CAB_RS_COUNTER_4,            0xa3},
+	{R367CAB_RS_COUNTER_5,            0x00},
+	{R367CAB_BERT_0,                  0x01},
+	{R367CAB_BERT_1,                  0x25},
+	{R367CAB_BERT_2,                  0x41},
+	{R367CAB_BERT_3,                  0x39},
+	{R367CAB_OUTFORMAT_0,             0xc2},
+	{R367CAB_OUTFORMAT_1,             0x22},
+	{R367CAB_SMOOTHER_2,              0x28},
+	{R367CAB_TSMF_CTRL_0,             0x01},
+	{R367CAB_TSMF_CTRL_1,             0xc6},
+	{R367CAB_TSMF_CTRL_3,             0x43},
+	{R367CAB_TS_ON_ID_0,              0x00},
+	{R367CAB_TS_ON_ID_1,              0x00},
+	{R367CAB_TS_ON_ID_2,              0x00},
+	{R367CAB_TS_ON_ID_3,              0x00},
+	{R367CAB_RE_STATUS_0,             0x00},
+	{R367CAB_RE_STATUS_1,             0x00},
+	{R367CAB_RE_STATUS_2,             0x00},
+	{R367CAB_RE_STATUS_3,             0x00},
+	{R367CAB_TS_STATUS_0,             0x00},
+	{R367CAB_TS_STATUS_1,             0x00},
+	{R367CAB_TS_STATUS_2,             0xa0},
+	{R367CAB_TS_STATUS_3,             0x00},
+	{R367CAB_T_O_ID_0,                0x00},
+	{R367CAB_T_O_ID_1,                0x00},
+	{R367CAB_T_O_ID_2,                0x00},
+	{R367CAB_T_O_ID_3,                0x00},
+	{0x0000, 0x00} /* EOT */
+};
+
+static const struct st_register def0367dd_base[] = {
+	{R367TER_IOCFG0,     0x80},
+	{R367TER_DAC0R,      0x00},
+	{R367TER_IOCFG1,     0x00},
+	{R367TER_DAC1R,      0x00},
+	{R367TER_IOCFG2,     0x00},
+	{R367TER_SDFR,       0x00},
+	{R367TER_AUX_CLK,    0x00},
+	{R367TER_FREESYS1,   0x00},
+	{R367TER_FREESYS2,   0x00},
+	{R367TER_FREESYS3,   0x00},
+	{R367TER_GPIO_CFG,   0x55},
+	{R367TER_GPIO_CMD,   0x01},
+	{R367TER_TSTRES,     0x00},
+	{R367TER_ANACTRL,    0x00},
+	{R367TER_TSTBUS,     0x00},
+	{R367TER_RF_AGC2,    0x20},
+	{R367TER_ANADIGCTRL, 0x0b},
+	{R367TER_PLLMDIV,    0x01},
+	{R367TER_PLLNDIV,    0x08},
+	{R367TER_PLLSETUP,   0x18},
+	{R367TER_DUAL_AD12,  0x04},
+	{R367TER_TSTBIST,    0x00},
+	{0x0000, 0x00} /* EOT */
+};
+
+/*
+ * Tables combined
+ */
+
+static const struct
+st_register *stv0367_deftabs[STV0367_DEFTAB_MAX][STV0367_TAB_MAX] = {
+	/* generic default/init tabs */
+	{ def0367ter, def0367cab, NULL },
+	/* default tabs for digital devices cards/flex modules */
+	{ def0367dd_ofdm, def0367dd_qam, def0367dd_base },
+};
+
+#endif
diff --git a/drivers/media/dvb-frontends/stv0367_regs.h b/drivers/media/dvb-frontends/stv0367_regs.h
index 1d1586221239..cc66d93c5a1f 100644
--- a/drivers/media/dvb-frontends/stv0367_regs.h
+++ b/drivers/media/dvb-frontends/stv0367_regs.h
@@ -2639,8 +2639,6 @@
 #define	R367TER_DEBUG_LT9	0xf405
 #define	F367TER_F_DEBUG_LT9	0xf40500ff
 
-#define STV0367TER_NBREGS	445
-
 /* ID */
 #define	R367CAB_ID	0xf000
 #define	F367CAB_IDENTIFICATIONREGISTER	0xf00000ff
@@ -3605,6 +3603,4 @@
 #define	R367CAB_T_O_ID_3	0xf4d3
 #define	F367CAB_TS_ID_I_H	0xf4d300ff
 
-#define STV0367CAB_NBREGS	187
-
 #endif
diff --git a/drivers/media/dvb-frontends/zl10353.c b/drivers/media/dvb-frontends/zl10353.c
index 47c0549eb7b2..1c689f7f4ab8 100644
--- a/drivers/media/dvb-frontends/zl10353.c
+++ b/drivers/media/dvb-frontends/zl10353.c
@@ -211,7 +211,7 @@ static int zl10353_set_parameters(struct dvb_frontend *fe)
 		break;
 	default:
 		c->bandwidth_hz = 8000000;
-		/* fall though */
+		/* fall through */
 	case 8000000:
 		zl10353_single_write(fe, MCLK_RATIO, 0x75);
 		zl10353_single_write(fe, 0x64, 0x36);
@@ -268,6 +268,7 @@ static int zl10353_set_parameters(struct dvb_frontend *fe)
 		if (c->hierarchy == HIERARCHY_AUTO ||
 		    c->hierarchy == HIERARCHY_NONE)
 			break;
+		/* fall through */
 	default:
 		return -EINVAL;
 	}
diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index aaa9471c7d11..121b3b5394cb 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -209,6 +209,7 @@ config VIDEO_ADV7604
 	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
 	depends on GPIOLIB || COMPILE_TEST
 	select HDMI
+	select V4L2_FWNODE
 	---help---
 	  Support for the Analog Devices ADV7604 video decoder.
 
@@ -302,6 +303,16 @@ config VIDEO_AD5820
 	  This is a driver for the AD5820 camera lens voice coil.
 	  It is used for example in Nokia N900 (RX-51).
 
+config VIDEO_DW9714
+	tristate "DW9714 lens voice coil support"
+	depends on I2C && VIDEO_V4L2 && MEDIA_CONTROLLER
+	depends on VIDEO_V4L2_SUBDEV_API
+	---help---
+	  This is a driver for the DW9714 camera lens voice coil.
+	  DW9714 is a 10 bit DAC with 120mA output current sink
+	  capability. This is designed for linear control of
+	  voice coil motors, controlled via I2C serial interface.
+
 config VIDEO_SAA7110
 	tristate "Philips SAA7110 video decoder"
 	depends on VIDEO_V4L2 && I2C
@@ -324,6 +335,7 @@ config VIDEO_TC358743
 	tristate "Toshiba TC358743 decoder"
 	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
 	select HDMI
+	select V4L2_FWNODE
 	---help---
 	  Support for the Toshiba TC358743 HDMI to MIPI CSI-2 bridge.
 
@@ -333,6 +345,7 @@ config VIDEO_TC358743
 config VIDEO_TVP514X
 	tristate "Texas Instruments TVP514x video decoder"
 	depends on VIDEO_V4L2 && I2C
+	select V4L2_FWNODE
 	---help---
 	  This is a Video4Linux2 sensor-level driver for the TI TVP5146/47
 	  decoder. It is currently working with the TI OMAP3 camera
@@ -344,6 +357,7 @@ config VIDEO_TVP514X
 config VIDEO_TVP5150
 	tristate "Texas Instruments TVP5150 video decoder"
 	depends on VIDEO_V4L2 && I2C
+	select V4L2_FWNODE
 	---help---
 	  Support for the Texas Instruments TVP5150 video decoder.
 
@@ -353,6 +367,7 @@ config VIDEO_TVP5150
 config VIDEO_TVP7002
 	tristate "Texas Instruments TVP7002 video decoder"
 	depends on VIDEO_V4L2 && I2C
+	select V4L2_FWNODE
 	---help---
 	  Support for the Texas Instruments TVP7002 video decoder.
 
@@ -535,6 +550,7 @@ config VIDEO_OV2659
 	tristate "OmniVision OV2659 sensor support"
 	depends on VIDEO_V4L2 && I2C
 	depends on MEDIA_CAMERA_SUPPORT
+	select V4L2_FWNODE
 	---help---
 	  This is a Video4Linux2 sensor-level driver for the OmniVision
 	  OV2659 camera.
@@ -542,11 +558,22 @@ config VIDEO_OV2659
 	  To compile this driver as a module, choose M here: the
 	  module will be called ov2659.
 
+config VIDEO_OV5640
+	tristate "OmniVision OV5640 sensor support"
+	depends on OF
+	depends on GPIOLIB && VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
+	select V4L2_FWNODE
+	---help---
+	  This is a Video4Linux2 sensor-level driver for the Omnivision
+	  OV5640 camera sensor with a MIPI CSI-2 interface.
+
 config VIDEO_OV5645
 	tristate "OmniVision OV5645 sensor support"
 	depends on OF
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
 	depends on MEDIA_CAMERA_SUPPORT
+	select V4L2_FWNODE
 	---help---
 	  This is a Video4Linux2 sensor-level driver for the OmniVision
 	  OV5645 camera.
@@ -558,6 +585,7 @@ config VIDEO_OV5647
 	tristate "OmniVision OV5647 sensor support"
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
 	depends on MEDIA_CAMERA_SUPPORT
+	select V4L2_FWNODE
 	---help---
 	  This is a Video4Linux2 sensor-level driver for the OmniVision
 	  OV5647 camera.
@@ -592,6 +620,14 @@ config VIDEO_OV9650
 	  This is a V4L2 sensor-level driver for the Omnivision
 	  OV9650 and OV9652 camera sensors.
 
+config VIDEO_OV13858
+	tristate "OmniVision OV13858 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 OmniVision
+	  OV13858 camera.
+
 config VIDEO_VS6624
 	tristate "ST VS6624 sensor support"
 	depends on VIDEO_V4L2 && I2C
@@ -650,6 +686,7 @@ config VIDEO_MT9V032
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
 	depends on MEDIA_CAMERA_SUPPORT
 	select REGMAP_I2C
+	select V4L2_FWNODE
 	---help---
 	  This is a Video4Linux2 sensor-level driver for the Micron
 	  MT9V032 752x480 CMOS sensor.
@@ -697,6 +734,7 @@ config VIDEO_S5K4ECGX
 config VIDEO_S5K5BAF
 	tristate "Samsung S5K5BAF sensor support"
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	select V4L2_FWNODE
 	---help---
 	  This is a V4L2 sensor-level driver for Samsung S5K5BAF 2M
 	  camera sensor with an embedded SoC image signal processor.
@@ -707,6 +745,7 @@ source "drivers/media/i2c/et8ek8/Kconfig"
 config VIDEO_S5C73M3
 	tristate "Samsung S5C73M3 sensor support"
 	depends on I2C && SPI && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	select V4L2_FWNODE
 	---help---
 	  This is a V4L2 sensor-level driver for Samsung S5C73M3
 	  8 Mpixel camera.
@@ -785,6 +824,18 @@ config VIDEO_SAA6752HS
 	  To compile this driver as a module, choose M here: the
 	  module will be called saa6752hs.
 
+comment "SDR tuner chips"
+
+config SDR_MAX2175
+	tristate "Maxim 2175 RF to Bits tuner"
+	depends on VIDEO_V4L2 && MEDIA_SDR_SUPPORT && I2C
+	---help---
+	  Support for Maxim 2175 tuner. It is an advanced analog/digital
+	  radio receiver with RF-to-Bits front-end designed for SDR solutions.
+
+	  To compile this driver as a module, choose M here; the
+	  module will be called max2175.
+
 comment "Miscellaneous helper chips"
 
 config VIDEO_THS7303
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index 62323ec66be8..2c0868fa6034 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -21,6 +21,7 @@ obj-$(CONFIG_VIDEO_SAA7127) += saa7127.o
 obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
 obj-$(CONFIG_VIDEO_SAA6752HS) += saa6752hs.o
 obj-$(CONFIG_VIDEO_AD5820)  += ad5820.o
+obj-$(CONFIG_VIDEO_DW9714)  += dw9714.o
 obj-$(CONFIG_VIDEO_ADV7170) += adv7170.o
 obj-$(CONFIG_VIDEO_ADV7175) += adv7175.o
 obj-$(CONFIG_VIDEO_ADV7180) += adv7180.o
@@ -58,11 +59,13 @@ obj-$(CONFIG_VIDEO_SONY_BTF_MPX) += sony-btf-mpx.o
 obj-$(CONFIG_VIDEO_UPD64031A) += upd64031a.o
 obj-$(CONFIG_VIDEO_UPD64083) += upd64083.o
 obj-$(CONFIG_VIDEO_OV2640) += ov2640.o
+obj-$(CONFIG_VIDEO_OV5640) += ov5640.o
 obj-$(CONFIG_VIDEO_OV5645) += ov5645.o
 obj-$(CONFIG_VIDEO_OV5647) += ov5647.o
 obj-$(CONFIG_VIDEO_OV7640) += ov7640.o
 obj-$(CONFIG_VIDEO_OV7670) += ov7670.o
 obj-$(CONFIG_VIDEO_OV9650) += ov9650.o
+obj-$(CONFIG_VIDEO_OV13858) += ov13858.o
 obj-$(CONFIG_VIDEO_MT9M032) += mt9m032.o
 obj-$(CONFIG_VIDEO_MT9M111) += mt9m111.o
 obj-$(CONFIG_VIDEO_MT9P031) += mt9p031.o
@@ -86,3 +89,5 @@ obj-$(CONFIG_VIDEO_IR_I2C)  += ir-kbd-i2c.o
 obj-$(CONFIG_VIDEO_ML86V7667)	+= ml86v7667.o
 obj-$(CONFIG_VIDEO_OV2659)	+= ov2659.o
 obj-$(CONFIG_VIDEO_TC358743)	+= tc358743.o
+
+obj-$(CONFIG_SDR_MAX2175) += max2175.o
diff --git a/drivers/media/i2c/ad5820.c b/drivers/media/i2c/ad5820.c
index 3d2a3c6b67d8..034ebf754007 100644
--- a/drivers/media/i2c/ad5820.c
+++ b/drivers/media/i2c/ad5820.c
@@ -341,7 +341,7 @@ static int ad5820_remove(struct i2c_client *client)
 	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
 	struct ad5820_device *coil = to_ad5820_device(subdev);
 
-	v4l2_device_unregister_subdev(&coil->subdev);
+	v4l2_async_unregister_subdev(&coil->subdev);
 	v4l2_ctrl_handler_free(&coil->ctrls);
 	media_entity_cleanup(&coil->subdev.entity);
 	mutex_destroy(&coil->power_lock);
diff --git a/drivers/media/i2c/adv7180.c b/drivers/media/i2c/adv7180.c
index bdbbf8cf27e4..78de7ddf5081 100644
--- a/drivers/media/i2c/adv7180.c
+++ b/drivers/media/i2c/adv7180.c
@@ -1452,6 +1452,8 @@ static SIMPLE_DEV_PM_OPS(adv7180_pm_ops, adv7180_suspend, adv7180_resume);
 #ifdef CONFIG_OF
 static const struct of_device_id adv7180_of_id[] = {
 	{ .compatible = "adi,adv7180", },
+	{ .compatible = "adi,adv7180cp", },
+	{ .compatible = "adi,adv7180st", },
 	{ .compatible = "adi,adv7182", },
 	{ .compatible = "adi,adv7280", },
 	{ .compatible = "adi,adv7280-m", },
diff --git a/drivers/media/i2c/adv7604.c b/drivers/media/i2c/adv7604.c
index f1fa9cec489f..660bacb8f7d9 100644
--- a/drivers/media/i2c/adv7604.c
+++ b/drivers/media/i2c/adv7604.c
@@ -33,6 +33,7 @@
 #include <linux/i2c.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/of_graph.h>
 #include <linux/slab.h>
 #include <linux/v4l2-dv-timings.h>
 #include <linux/videodev2.h>
@@ -45,7 +46,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-event.h>
 #include <media/v4l2-dv-timings.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 
 static int debug;
 module_param(debug, int, 0644);
@@ -3069,7 +3070,7 @@ MODULE_DEVICE_TABLE(of, adv76xx_of_id);
 
 static int adv76xx_parse_dt(struct adv76xx_state *state)
 {
-	struct v4l2_of_endpoint bus_cfg;
+	struct v4l2_fwnode_endpoint bus_cfg;
 	struct device_node *endpoint;
 	struct device_node *np;
 	unsigned int flags;
@@ -3083,7 +3084,7 @@ static int adv76xx_parse_dt(struct adv76xx_state *state)
 	if (!endpoint)
 		return -EINVAL;
 
-	ret = v4l2_of_parse_endpoint(endpoint, &bus_cfg);
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint), &bus_cfg);
 	if (ret) {
 		of_node_put(endpoint);
 		return ret;
diff --git a/drivers/media/i2c/as3645a.c b/drivers/media/i2c/as3645a.c
index b6aeceea9850..af5db71a0888 100644
--- a/drivers/media/i2c/as3645a.c
+++ b/drivers/media/i2c/as3645a.c
@@ -294,8 +294,8 @@ static int as3645a_read_fault(struct as3645a *flash)
 		dev_dbg(&client->dev, "Inductor Peak limit fault\n");
 
 	if (rval & AS_FAULT_INFO_INDICATOR_LED)
-		dev_dbg(&client->dev, "Indicator LED fault: "
-			"Short circuit or open loop\n");
+		dev_dbg(&client->dev,
+			"Indicator LED fault: Short circuit or open loop\n");
 
 	dev_dbg(&client->dev, "%u connected LEDs\n",
 		rval & AS_FAULT_INFO_LED_AMOUNT ? 2 : 1);
@@ -310,8 +310,8 @@ static int as3645a_read_fault(struct as3645a *flash)
 		dev_dbg(&client->dev, "Short circuit fault\n");
 
 	if (rval & AS_FAULT_INFO_OVER_VOLTAGE)
-		dev_dbg(&client->dev, "Over voltage fault: "
-			"Indicates missing capacitor or open connection\n");
+		dev_dbg(&client->dev,
+			"Over voltage fault: Indicates missing capacitor or open connection\n");
 
 	return rval;
 }
@@ -583,8 +583,8 @@ static int as3645a_registered(struct v4l2_subdev *sd)
 
 	/* Verify the chip model and version. */
 	if (model != 0x01 || rfu != 0x00) {
-		dev_err(&client->dev, "AS3645A not detected "
-			"(model %d rfu %d)\n", model, rfu);
+		dev_err(&client->dev,
+			"AS3645A not detected (model %d rfu %d)\n", model, rfu);
 		rval = -ENODEV;
 		goto power_off;
 	}
diff --git a/drivers/media/i2c/cx25840/cx25840-core.c b/drivers/media/i2c/cx25840/cx25840-core.c
index b8d3c070bfc1..39f51daa7558 100644
--- a/drivers/media/i2c/cx25840/cx25840-core.c
+++ b/drivers/media/i2c/cx25840/cx25840-core.c
@@ -416,11 +416,13 @@ static void cx25840_initialize(struct i2c_client *client)
 	INIT_WORK(&state->fw_work, cx25840_work_handler);
 	init_waitqueue_head(&state->fw_wait);
 	q = create_singlethread_workqueue("cx25840_fw");
-	prepare_to_wait(&state->fw_wait, &wait, TASK_UNINTERRUPTIBLE);
-	queue_work(q, &state->fw_work);
-	schedule();
-	finish_wait(&state->fw_wait, &wait);
-	destroy_workqueue(q);
+	if (q) {
+		prepare_to_wait(&state->fw_wait, &wait, TASK_UNINTERRUPTIBLE);
+		queue_work(q, &state->fw_work);
+		schedule();
+		finish_wait(&state->fw_wait, &wait);
+		destroy_workqueue(q);
+	}
 
 	/* 6. */
 	cx25840_write(client, 0x115, 0x8c);
@@ -630,11 +632,13 @@ static void cx23885_initialize(struct i2c_client *client)
 	INIT_WORK(&state->fw_work, cx25840_work_handler);
 	init_waitqueue_head(&state->fw_wait);
 	q = create_singlethread_workqueue("cx25840_fw");
-	prepare_to_wait(&state->fw_wait, &wait, TASK_UNINTERRUPTIBLE);
-	queue_work(q, &state->fw_work);
-	schedule();
-	finish_wait(&state->fw_wait, &wait);
-	destroy_workqueue(q);
+	if (q) {
+		prepare_to_wait(&state->fw_wait, &wait, TASK_UNINTERRUPTIBLE);
+		queue_work(q, &state->fw_work);
+		schedule();
+		finish_wait(&state->fw_wait, &wait);
+		destroy_workqueue(q);
+	}
 
 	/* Call the cx23888 specific std setup func, we no longer rely on
 	 * the generic cx24840 func.
@@ -748,11 +752,13 @@ static void cx231xx_initialize(struct i2c_client *client)
 	INIT_WORK(&state->fw_work, cx25840_work_handler);
 	init_waitqueue_head(&state->fw_wait);
 	q = create_singlethread_workqueue("cx25840_fw");
-	prepare_to_wait(&state->fw_wait, &wait, TASK_UNINTERRUPTIBLE);
-	queue_work(q, &state->fw_work);
-	schedule();
-	finish_wait(&state->fw_wait, &wait);
-	destroy_workqueue(q);
+	if (q) {
+		prepare_to_wait(&state->fw_wait, &wait, TASK_UNINTERRUPTIBLE);
+		queue_work(q, &state->fw_work);
+		schedule();
+		finish_wait(&state->fw_wait, &wait);
+		destroy_workqueue(q);
+	}
 
 	cx25840_std_setup(client);
 
diff --git a/drivers/media/i2c/dw9714.c b/drivers/media/i2c/dw9714.c
new file mode 100644
index 000000000000..6a607d7f82de
--- /dev/null
+++ b/drivers/media/i2c/dw9714.c
@@ -0,0 +1,291 @@
+/*
+ * Copyright (c) 2015--2017 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+
+#define DW9714_NAME		"dw9714"
+#define DW9714_MAX_FOCUS_POS	1023
+/*
+ * This acts as the minimum granularity of lens movement.
+ * Keep this value power of 2, so the control steps can be
+ * uniformly adjusted for gradual lens movement, with desired
+ * number of control steps.
+ */
+#define DW9714_CTRL_STEPS	16
+#define DW9714_CTRL_DELAY_US	1000
+/*
+ * S[3:2] = 0x00, codes per step for "Linear Slope Control"
+ * S[1:0] = 0x00, step period
+ */
+#define DW9714_DEFAULT_S 0x0
+#define DW9714_VAL(data, s) ((data) << 4 | (s))
+
+/* dw9714 device structure */
+struct dw9714_device {
+	struct i2c_client *client;
+	struct v4l2_ctrl_handler ctrls_vcm;
+	struct v4l2_subdev sd;
+	u16 current_val;
+};
+
+static inline struct dw9714_device *to_dw9714_vcm(struct v4l2_ctrl *ctrl)
+{
+	return container_of(ctrl->handler, struct dw9714_device, ctrls_vcm);
+}
+
+static inline struct dw9714_device *sd_to_dw9714_vcm(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct dw9714_device, sd);
+}
+
+static int dw9714_i2c_write(struct i2c_client *client, u16 data)
+{
+	int ret;
+	u16 val = cpu_to_be16(data);
+
+	ret = i2c_master_send(client, (const char *)&val, sizeof(val));
+	if (ret != sizeof(val)) {
+		dev_err(&client->dev, "I2C write fail\n");
+		return -EIO;
+	}
+	return 0;
+}
+
+static int dw9714_t_focus_vcm(struct dw9714_device *dw9714_dev, u16 val)
+{
+	struct i2c_client *client = dw9714_dev->client;
+
+	dw9714_dev->current_val = val;
+
+	return dw9714_i2c_write(client, DW9714_VAL(val, DW9714_DEFAULT_S));
+}
+
+static int dw9714_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct dw9714_device *dev_vcm = to_dw9714_vcm(ctrl);
+
+	if (ctrl->id == V4L2_CID_FOCUS_ABSOLUTE)
+		return dw9714_t_focus_vcm(dev_vcm, ctrl->val);
+
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops dw9714_vcm_ctrl_ops = {
+	.s_ctrl = dw9714_set_ctrl,
+};
+
+static int dw9714_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct dw9714_device *dw9714_dev = sd_to_dw9714_vcm(sd);
+	struct device *dev = &dw9714_dev->client->dev;
+	int rval;
+
+	rval = pm_runtime_get_sync(dev);
+	if (rval < 0) {
+		pm_runtime_put_noidle(dev);
+		return rval;
+	}
+
+	return 0;
+}
+
+static int dw9714_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct dw9714_device *dw9714_dev = sd_to_dw9714_vcm(sd);
+	struct device *dev = &dw9714_dev->client->dev;
+
+	pm_runtime_put(dev);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops dw9714_int_ops = {
+	.open = dw9714_open,
+	.close = dw9714_close,
+};
+
+static const struct v4l2_subdev_ops dw9714_ops = { };
+
+static void dw9714_subdev_cleanup(struct dw9714_device *dw9714_dev)
+{
+	v4l2_async_unregister_subdev(&dw9714_dev->sd);
+	v4l2_ctrl_handler_free(&dw9714_dev->ctrls_vcm);
+	media_entity_cleanup(&dw9714_dev->sd.entity);
+}
+
+static int dw9714_init_controls(struct dw9714_device *dev_vcm)
+{
+	struct v4l2_ctrl_handler *hdl = &dev_vcm->ctrls_vcm;
+	const struct v4l2_ctrl_ops *ops = &dw9714_vcm_ctrl_ops;
+	struct i2c_client *client = dev_vcm->client;
+
+	v4l2_ctrl_handler_init(hdl, 1);
+
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FOCUS_ABSOLUTE,
+			  0, DW9714_MAX_FOCUS_POS, DW9714_CTRL_STEPS, 0);
+
+	if (hdl->error)
+		dev_err(&client->dev, "%s fail error: 0x%x\n",
+			__func__, hdl->error);
+	dev_vcm->sd.ctrl_handler = hdl;
+	return hdl->error;
+}
+
+static int dw9714_probe(struct i2c_client *client,
+			const struct i2c_device_id *devid)
+{
+	struct dw9714_device *dw9714_dev;
+	int rval;
+
+	dw9714_dev = devm_kzalloc(&client->dev, sizeof(*dw9714_dev),
+				  GFP_KERNEL);
+	if (dw9714_dev == NULL)
+		return -ENOMEM;
+
+	dw9714_dev->client = client;
+
+	v4l2_i2c_subdev_init(&dw9714_dev->sd, client, &dw9714_ops);
+	dw9714_dev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	dw9714_dev->sd.internal_ops = &dw9714_int_ops;
+
+	rval = dw9714_init_controls(dw9714_dev);
+	if (rval)
+		goto err_cleanup;
+
+	rval = media_entity_pads_init(&dw9714_dev->sd.entity, 0, NULL);
+	if (rval < 0)
+		goto err_cleanup;
+
+	dw9714_dev->sd.entity.function = MEDIA_ENT_F_LENS;
+
+	rval = v4l2_async_register_subdev(&dw9714_dev->sd);
+	if (rval < 0)
+		goto err_cleanup;
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+
+	return 0;
+
+err_cleanup:
+	dw9714_subdev_cleanup(dw9714_dev);
+	dev_err(&client->dev, "Probe failed: %d\n", rval);
+	return rval;
+}
+
+static int dw9714_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct dw9714_device *dw9714_dev = sd_to_dw9714_vcm(sd);
+
+	pm_runtime_disable(&client->dev);
+	dw9714_subdev_cleanup(dw9714_dev);
+
+	return 0;
+}
+
+/*
+ * This function sets the vcm position, so it consumes least current
+ * The lens position is gradually moved in units of DW9714_CTRL_STEPS,
+ * to make the movements smoothly.
+ */
+static int __maybe_unused dw9714_vcm_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct dw9714_device *dw9714_dev = sd_to_dw9714_vcm(sd);
+	int ret, val;
+
+	for (val = dw9714_dev->current_val & ~(DW9714_CTRL_STEPS - 1);
+	     val >= 0; val -= DW9714_CTRL_STEPS) {
+		ret = dw9714_i2c_write(client,
+				       DW9714_VAL(val, DW9714_DEFAULT_S));
+		if (ret)
+			dev_err_once(dev, "%s I2C failure: %d", __func__, ret);
+		usleep_range(DW9714_CTRL_DELAY_US, DW9714_CTRL_DELAY_US + 10);
+	}
+	return 0;
+}
+
+/*
+ * This function sets the vcm position to the value set by the user
+ * through v4l2_ctrl_ops s_ctrl handler
+ * The lens position is gradually moved in units of DW9714_CTRL_STEPS,
+ * to make the movements smoothly.
+ */
+static int  __maybe_unused dw9714_vcm_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct dw9714_device *dw9714_dev = sd_to_dw9714_vcm(sd);
+	int ret, val;
+
+	for (val = dw9714_dev->current_val % DW9714_CTRL_STEPS;
+	     val < dw9714_dev->current_val + DW9714_CTRL_STEPS - 1;
+	     val += DW9714_CTRL_STEPS) {
+		ret = dw9714_i2c_write(client,
+				       DW9714_VAL(val, DW9714_DEFAULT_S));
+		if (ret)
+			dev_err_ratelimited(dev, "%s I2C failure: %d",
+						__func__, ret);
+		usleep_range(DW9714_CTRL_DELAY_US, DW9714_CTRL_DELAY_US + 10);
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id dw9714_acpi_match[] = {
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, dw9714_acpi_match);
+#endif
+
+static const struct i2c_device_id dw9714_id_table[] = {
+	{DW9714_NAME, 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, dw9714_id_table);
+
+static const struct dev_pm_ops dw9714_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(dw9714_vcm_suspend, dw9714_vcm_resume)
+	SET_RUNTIME_PM_OPS(dw9714_vcm_suspend, dw9714_vcm_resume, NULL)
+};
+
+static struct i2c_driver dw9714_i2c_driver = {
+	.driver = {
+		.name = DW9714_NAME,
+		.pm = &dw9714_pm_ops,
+		.acpi_match_table = ACPI_PTR(dw9714_acpi_match),
+	},
+	.probe = dw9714_probe,
+	.remove = dw9714_remove,
+	.id_table = dw9714_id_table,
+};
+
+module_i2c_driver(dw9714_i2c_driver);
+
+MODULE_AUTHOR("Tianshu Qiu <tian.shu.qiu@intel.com>");
+MODULE_AUTHOR("Jian Xu Zheng <jian.xu.zheng@intel.com>");
+MODULE_AUTHOR("Yuning Pu <yuning.pu@intel.com>");
+MODULE_AUTHOR("Jouni Ukkonen <jouni.ukkonen@intel.com>");
+MODULE_AUTHOR("Tommi Franttila <tommi.franttila@intel.com>");
+MODULE_DESCRIPTION("DW9714 VCM driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/max2175.c b/drivers/media/i2c/max2175.c
new file mode 100644
index 000000000000..a4736a8a7792
--- /dev/null
+++ b/drivers/media/i2c/max2175.c
@@ -0,0 +1,1453 @@
+/*
+ * Maxim Integrated MAX2175 RF to Bits tuner driver
+ *
+ * This driver & most of the hard coded values are based on the reference
+ * application delivered by Maxim for this device.
+ *
+ * Copyright (C) 2016 Maxim Integrated Products
+ * Copyright (C) 2017 Renesas Electronics Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/math64.h>
+#include <linux/max2175.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+
+#include "max2175.h"
+
+#define DRIVER_NAME "max2175"
+
+#define mxm_dbg(ctx, fmt, arg...) dev_dbg(&ctx->client->dev, fmt, ## arg)
+#define mxm_err(ctx, fmt, arg...) dev_err(&ctx->client->dev, fmt, ## arg)
+
+/* Rx mode */
+struct max2175_rxmode {
+	enum max2175_band band;		/* Associated band */
+	u32 freq;			/* Default freq in Hz */
+	u8 i2s_word_size;		/* Bit value */
+};
+
+/* Register map to define preset values */
+struct max2175_reg_map {
+	u8 idx;				/* Register index */
+	u8 val;				/* Register value */
+};
+
+static const struct max2175_rxmode eu_rx_modes[] = {
+	/* EU modes */
+	[MAX2175_EU_FM_1_2] = { MAX2175_BAND_FM, 98256000, 1 },
+	[MAX2175_DAB_1_2]   = { MAX2175_BAND_VHF, 182640000, 0 },
+};
+
+static const struct max2175_rxmode na_rx_modes[] = {
+	/* NA modes */
+	[MAX2175_NA_FM_1_0] = { MAX2175_BAND_FM, 98255520, 1 },
+	[MAX2175_NA_FM_2_0] = { MAX2175_BAND_FM, 98255520, 6 },
+};
+
+/*
+ * Preset values:
+ * Based on Maxim MAX2175 Register Table revision: 130p10
+ */
+static const u8 full_fm_eu_1p0[] = {
+	0x15, 0x04, 0xb8, 0xe3, 0x35, 0x18, 0x7c, 0x00,
+	0x00, 0x7d, 0x40, 0x08, 0x70, 0x7a, 0x88, 0x91,
+	0x61, 0x61, 0x61, 0x61, 0x5a, 0x0f, 0x34, 0x1c,
+	0x14, 0x88, 0x33, 0x02, 0x00, 0x09, 0x00, 0x65,
+	0x9f, 0x2b, 0x80, 0x00, 0x95, 0x05, 0x2c, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40,
+	0x4a, 0x08, 0xa8, 0x0e, 0x0e, 0x2f, 0x7e, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0xab, 0x5e, 0xa9,
+	0xae, 0xbb, 0x57, 0x18, 0x3b, 0x03, 0x3b, 0x64,
+	0x40, 0x60, 0x00, 0x2a, 0xbf, 0x3f, 0xff, 0x9f,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a, 0x00,
+	0xff, 0xfc, 0xef, 0x1c, 0x40, 0x00, 0x00, 0x02,
+	0x00, 0x00, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0xac, 0x40, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x75, 0x00, 0x00,
+	0x00, 0x47, 0x00, 0x00, 0x11, 0x3f, 0x22, 0x00,
+	0xf1, 0x00, 0x41, 0x03, 0xb0, 0x00, 0x00, 0x00,
+	0x1b,
+};
+
+static const u8 full_fm_na_1p0[] = {
+	0x13, 0x08, 0x8d, 0xc0, 0x35, 0x18, 0x7d, 0x3f,
+	0x7d, 0x75, 0x40, 0x08, 0x70, 0x7a, 0x88, 0x91,
+	0x61, 0x61, 0x61, 0x61, 0x5c, 0x0f, 0x34, 0x1c,
+	0x14, 0x88, 0x33, 0x02, 0x00, 0x01, 0x00, 0x65,
+	0x9f, 0x2b, 0x80, 0x00, 0x95, 0x05, 0x2c, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40,
+	0x4a, 0x08, 0xa8, 0x0e, 0x0e, 0xaf, 0x7e, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0xab, 0x5e, 0xa9,
+	0xae, 0xbb, 0x57, 0x18, 0x3b, 0x03, 0x3b, 0x64,
+	0x40, 0x60, 0x00, 0x2a, 0xbf, 0x3f, 0xff, 0x9f,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a, 0x00,
+	0xff, 0xfc, 0xef, 0x1c, 0x40, 0x00, 0x00, 0x02,
+	0x00, 0x00, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0xa6, 0x40, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x75, 0x00, 0x00,
+	0x00, 0x35, 0x00, 0x00, 0x11, 0x3f, 0x22, 0x00,
+	0xf1, 0x00, 0x41, 0x03, 0xb0, 0x00, 0x00, 0x00,
+	0x1b,
+};
+
+/* DAB1.2 settings */
+static const struct max2175_reg_map dab12_map[] = {
+	{ 0x01, 0x13 }, { 0x02, 0x0d }, { 0x03, 0x15 }, { 0x04, 0x55 },
+	{ 0x05, 0x0a }, { 0x06, 0xa0 }, { 0x07, 0x40 }, { 0x08, 0x00 },
+	{ 0x09, 0x00 }, { 0x0a, 0x7d }, { 0x0b, 0x4a }, { 0x0c, 0x28 },
+	{ 0x0e, 0x43 }, { 0x0f, 0xb5 }, { 0x10, 0x31 }, { 0x11, 0x9e },
+	{ 0x12, 0x68 }, { 0x13, 0x9e }, { 0x14, 0x68 }, { 0x15, 0x58 },
+	{ 0x16, 0x2f }, { 0x17, 0x3f }, { 0x18, 0x40 }, { 0x1a, 0x88 },
+	{ 0x1b, 0xaa }, { 0x1c, 0x9a }, { 0x1d, 0x00 }, { 0x1e, 0x00 },
+	{ 0x23, 0x80 }, { 0x24, 0x00 }, { 0x25, 0x00 }, { 0x26, 0x00 },
+	{ 0x27, 0x00 }, { 0x32, 0x08 }, { 0x33, 0xf8 }, { 0x36, 0x2d },
+	{ 0x37, 0x7e }, { 0x55, 0xaf }, { 0x56, 0x3f }, { 0x57, 0xf8 },
+	{ 0x58, 0x99 }, { 0x76, 0x00 }, { 0x77, 0x00 }, { 0x78, 0x02 },
+	{ 0x79, 0x40 }, { 0x82, 0x00 }, { 0x83, 0x00 }, { 0x85, 0x00 },
+	{ 0x86, 0x20 },
+};
+
+/* EU FM 1.2 settings */
+static const struct max2175_reg_map fmeu1p2_map[] = {
+	{ 0x01, 0x15 }, { 0x02, 0x04 }, { 0x03, 0xb8 }, { 0x04, 0xe3 },
+	{ 0x05, 0x35 }, { 0x06, 0x18 }, { 0x07, 0x7c }, { 0x08, 0x00 },
+	{ 0x09, 0x00 }, { 0x0a, 0x73 }, { 0x0b, 0x40 }, { 0x0c, 0x08 },
+	{ 0x0e, 0x7a }, { 0x0f, 0x88 }, { 0x10, 0x91 }, { 0x11, 0x61 },
+	{ 0x12, 0x61 }, { 0x13, 0x61 }, { 0x14, 0x61 }, { 0x15, 0x5a },
+	{ 0x16, 0x0f }, { 0x17, 0x34 }, { 0x18, 0x1c }, { 0x1a, 0x88 },
+	{ 0x1b, 0x33 }, { 0x1c, 0x02 }, { 0x1d, 0x00 }, { 0x1e, 0x01 },
+	{ 0x23, 0x80 }, { 0x24, 0x00 }, { 0x25, 0x95 }, { 0x26, 0x05 },
+	{ 0x27, 0x2c }, { 0x32, 0x08 }, { 0x33, 0xa8 }, { 0x36, 0x2f },
+	{ 0x37, 0x7e }, { 0x55, 0xbf }, { 0x56, 0x3f }, { 0x57, 0xff },
+	{ 0x58, 0x9f }, { 0x76, 0xac }, { 0x77, 0x40 }, { 0x78, 0x00 },
+	{ 0x79, 0x00 }, { 0x82, 0x47 }, { 0x83, 0x00 }, { 0x85, 0x11 },
+	{ 0x86, 0x3f },
+};
+
+/* FM NA 1.0 settings */
+static const struct max2175_reg_map fmna1p0_map[] = {
+	{ 0x01, 0x13 }, { 0x02, 0x08 }, { 0x03, 0x8d }, { 0x04, 0xc0 },
+	{ 0x05, 0x35 }, { 0x06, 0x18 }, { 0x07, 0x7d }, { 0x08, 0x3f },
+	{ 0x09, 0x7d }, { 0x0a, 0x75 }, { 0x0b, 0x40 }, { 0x0c, 0x08 },
+	{ 0x0e, 0x7a }, { 0x0f, 0x88 }, { 0x10, 0x91 }, { 0x11, 0x61 },
+	{ 0x12, 0x61 }, { 0x13, 0x61 }, { 0x14, 0x61 }, { 0x15, 0x5c },
+	{ 0x16, 0x0f }, { 0x17, 0x34 }, { 0x18, 0x1c }, { 0x1a, 0x88 },
+	{ 0x1b, 0x33 }, { 0x1c, 0x02 }, { 0x1d, 0x00 }, { 0x1e, 0x01 },
+	{ 0x23, 0x80 }, { 0x24, 0x00 }, { 0x25, 0x95 }, { 0x26, 0x05 },
+	{ 0x27, 0x2c }, { 0x32, 0x08 }, { 0x33, 0xa8 }, { 0x36, 0xaf },
+	{ 0x37, 0x7e }, { 0x55, 0xbf }, { 0x56, 0x3f }, { 0x57, 0xff },
+	{ 0x58, 0x9f }, { 0x76, 0xa6 }, { 0x77, 0x40 }, { 0x78, 0x00 },
+	{ 0x79, 0x00 }, { 0x82, 0x35 }, { 0x83, 0x00 }, { 0x85, 0x11 },
+	{ 0x86, 0x3f },
+};
+
+/* FM NA 2.0 settings */
+static const struct max2175_reg_map fmna2p0_map[] = {
+	{ 0x01, 0x13 }, { 0x02, 0x08 }, { 0x03, 0x8d }, { 0x04, 0xc0 },
+	{ 0x05, 0x35 }, { 0x06, 0x18 }, { 0x07, 0x7c }, { 0x08, 0x54 },
+	{ 0x09, 0xa7 }, { 0x0a, 0x55 }, { 0x0b, 0x42 }, { 0x0c, 0x48 },
+	{ 0x0e, 0x7a }, { 0x0f, 0x88 }, { 0x10, 0x91 }, { 0x11, 0x61 },
+	{ 0x12, 0x61 }, { 0x13, 0x61 }, { 0x14, 0x61 }, { 0x15, 0x5c },
+	{ 0x16, 0x0f }, { 0x17, 0x34 }, { 0x18, 0x1c }, { 0x1a, 0x88 },
+	{ 0x1b, 0x33 }, { 0x1c, 0x02 }, { 0x1d, 0x00 }, { 0x1e, 0x01 },
+	{ 0x23, 0x80 }, { 0x24, 0x00 }, { 0x25, 0x95 }, { 0x26, 0x05 },
+	{ 0x27, 0x2c }, { 0x32, 0x08 }, { 0x33, 0xa8 }, { 0x36, 0xaf },
+	{ 0x37, 0x7e }, { 0x55, 0xbf }, { 0x56, 0x3f }, { 0x57, 0xff },
+	{ 0x58, 0x9f }, { 0x76, 0xac }, { 0x77, 0xc0 }, { 0x78, 0x00 },
+	{ 0x79, 0x00 }, { 0x82, 0x6b }, { 0x83, 0x00 }, { 0x85, 0x11 },
+	{ 0x86, 0x3f },
+};
+
+static const u16 ch_coeff_dab1[] = {
+	0x001c, 0x0007, 0xffcd, 0x0056, 0xffa4, 0x0033, 0x0027, 0xff61,
+	0x010e, 0xfec0, 0x0106, 0xffb8, 0xff1c, 0x023c, 0xfcb2, 0x039b,
+	0xfd4e, 0x0055, 0x036a, 0xf7de, 0x0d21, 0xee72, 0x1499, 0x6a51,
+};
+
+static const u16 ch_coeff_fmeu[] = {
+	0x0000, 0xffff, 0x0001, 0x0002, 0xfffa, 0xffff, 0x0015, 0xffec,
+	0xffde, 0x0054, 0xfff9, 0xff52, 0x00b8, 0x00a2, 0xfe0a, 0x00af,
+	0x02e3, 0xfc14, 0xfe89, 0x089d, 0xfa2e, 0xf30f, 0x25be, 0x4eb6,
+};
+
+static const u16 eq_coeff_fmeu1_ra02_m6db[] = {
+	0x0040, 0xffc6, 0xfffa, 0x002c, 0x000d, 0xff90, 0x0037, 0x006e,
+	0xffc0, 0xff5b, 0x006a, 0x00f0, 0xff57, 0xfe94, 0x0112, 0x0252,
+	0xfe0c, 0xfc6a, 0x0385, 0x0553, 0xfa49, 0xf789, 0x0b91, 0x1a10,
+};
+
+static const u16 ch_coeff_fmna[] = {
+	0x0001, 0x0003, 0xfffe, 0xfff4, 0x0000, 0x001f, 0x000c, 0xffbc,
+	0xffd3, 0x007d, 0x0075, 0xff33, 0xff01, 0x0131, 0x01ef, 0xfe60,
+	0xfc7a, 0x020e, 0x0656, 0xfd94, 0xf395, 0x02ab, 0x2857, 0x3d3f,
+};
+
+static const u16 eq_coeff_fmna1_ra02_m6db[] = {
+	0xfff1, 0xffe1, 0xffef, 0x000e, 0x0030, 0x002f, 0xfff6, 0xffa7,
+	0xff9d, 0x000a, 0x00a2, 0x00b5, 0xffea, 0xfed9, 0xfec5, 0x003d,
+	0x0217, 0x021b, 0xff5a, 0xfc2b, 0xfcbd, 0x02c4, 0x0ac3, 0x0e85,
+};
+
+static const u8 adc_presets[2][23] = {
+	{
+		0x83, 0x00, 0xcf, 0xb4, 0x0f, 0x2c, 0x0c, 0x49,
+		0x00, 0x00, 0x00, 0x8c,	0x02, 0x02, 0x00, 0x04,
+		0xec, 0x82, 0x4b, 0xcc, 0x01, 0x88, 0x0c,
+	},
+	{
+		0x83, 0x00, 0xcf, 0xb4,	0x0f, 0x2c, 0x0c, 0x49,
+		0x00, 0x00, 0x00, 0x8c,	0x02, 0x20, 0x33, 0x8c,
+		0x57, 0xd7, 0x59, 0xb7,	0x65, 0x0e, 0x0c,
+	},
+};
+
+/* Tuner bands */
+static const struct v4l2_frequency_band eu_bands_rf = {
+	.tuner = 0,
+	.type = V4L2_TUNER_RF,
+	.index = 0,
+	.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+	.rangelow   = 65000000,
+	.rangehigh  = 240000000,
+};
+
+static const struct v4l2_frequency_band na_bands_rf = {
+	.tuner = 0,
+	.type = V4L2_TUNER_RF,
+	.index = 0,
+	.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+	.rangelow   = 65000000,
+	.rangehigh  = 108000000,
+};
+
+/* Regmap settings */
+static const struct regmap_range max2175_regmap_volatile_range[] = {
+	regmap_reg_range(0x30, 0x35),
+	regmap_reg_range(0x3a, 0x45),
+	regmap_reg_range(0x59, 0x5e),
+	regmap_reg_range(0x73, 0x75),
+};
+
+static const struct regmap_access_table max2175_volatile_regs = {
+	.yes_ranges = max2175_regmap_volatile_range,
+	.n_yes_ranges = ARRAY_SIZE(max2175_regmap_volatile_range),
+};
+
+static const struct reg_default max2175_reg_defaults[] = {
+	{ 0x00, 0x07},
+};
+
+static const struct regmap_config max2175_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = 0xff,
+	.reg_defaults = max2175_reg_defaults,
+	.num_reg_defaults = ARRAY_SIZE(max2175_reg_defaults),
+	.volatile_table = &max2175_volatile_regs,
+	.cache_type = REGCACHE_FLAT,
+};
+
+struct max2175 {
+	struct v4l2_subdev sd;		/* Sub-device */
+	struct i2c_client *client;	/* I2C client */
+
+	/* Controls */
+	struct v4l2_ctrl_handler ctrl_hdl;
+	struct v4l2_ctrl *lna_gain;	/* LNA gain value */
+	struct v4l2_ctrl *if_gain;	/* I/F gain value */
+	struct v4l2_ctrl *pll_lock;	/* PLL lock */
+	struct v4l2_ctrl *i2s_en;	/* I2S output enable */
+	struct v4l2_ctrl *hsls;		/* High-side/Low-side polarity */
+	struct v4l2_ctrl *rx_mode;	/* Receive mode */
+
+	/* Regmap */
+	struct regmap *regmap;
+
+	/* Cached configuration */
+	u32 freq;			/* Tuned freq In Hz */
+	const struct max2175_rxmode *rx_modes;		/* EU or NA modes */
+	const struct v4l2_frequency_band *bands_rf;	/* EU or NA bands */
+
+	/* Device settings */
+	unsigned long xtal_freq;	/* Ref Oscillator freq in Hz */
+	u32 decim_ratio;
+	bool master;			/* Master/Slave */
+	bool am_hiz;			/* AM Hi-Z filter */
+
+	/* ROM values */
+	u8 rom_bbf_bw_am;
+	u8 rom_bbf_bw_fm;
+	u8 rom_bbf_bw_dab;
+
+	/* Driver private variables */
+	bool mode_resolved;		/* Flag to sanity check settings */
+};
+
+static inline struct max2175 *max2175_from_sd(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct max2175, sd);
+}
+
+static inline struct max2175 *max2175_from_ctrl_hdl(struct v4l2_ctrl_handler *h)
+{
+	return container_of(h, struct max2175, ctrl_hdl);
+}
+
+/* Get bitval of a given val */
+static inline u8 max2175_get_bitval(u8 val, u8 msb, u8 lsb)
+{
+	return (val & GENMASK(msb, lsb)) >> lsb;
+}
+
+/* Read/Write bit(s) on top of regmap */
+static int max2175_read(struct max2175 *ctx, u8 idx, u8 *val)
+{
+	u32 regval;
+	int ret;
+
+	ret = regmap_read(ctx->regmap, idx, &regval);
+	if (ret)
+		mxm_err(ctx, "read ret(%d): idx 0x%02x\n", ret, idx);
+	else
+		*val = regval;
+
+	return ret;
+}
+
+static int max2175_write(struct max2175 *ctx, u8 idx, u8 val)
+{
+	int ret;
+
+	ret = regmap_write(ctx->regmap, idx, val);
+	if (ret)
+		mxm_err(ctx, "write ret(%d): idx 0x%02x val 0x%02x\n",
+			ret, idx, val);
+
+	return ret;
+}
+
+static u8 max2175_read_bits(struct max2175 *ctx, u8 idx, u8 msb, u8 lsb)
+{
+	u8 val;
+
+	if (max2175_read(ctx, idx, &val))
+		return 0;
+
+	return max2175_get_bitval(val, msb, lsb);
+}
+
+static int max2175_write_bits(struct max2175 *ctx, u8 idx,
+			     u8 msb, u8 lsb, u8 newval)
+{
+	int ret = regmap_update_bits(ctx->regmap, idx, GENMASK(msb, lsb),
+				     newval << lsb);
+
+	if (ret)
+		mxm_err(ctx, "wbits ret(%d): idx 0x%02x\n", ret, idx);
+
+	return ret;
+}
+
+static int max2175_write_bit(struct max2175 *ctx, u8 idx, u8 bit, u8 newval)
+{
+	return max2175_write_bits(ctx, idx, bit, bit, newval);
+}
+
+/* Checks expected pattern every msec until timeout */
+static int max2175_poll_timeout(struct max2175 *ctx, u8 idx, u8 msb, u8 lsb,
+				u8 exp_bitval, u32 timeout_us)
+{
+	unsigned int val;
+
+	return regmap_read_poll_timeout(ctx->regmap, idx, val,
+			(max2175_get_bitval(val, msb, lsb) == exp_bitval),
+			1000, timeout_us);
+}
+
+static int max2175_poll_csm_ready(struct max2175 *ctx)
+{
+	int ret;
+
+	ret = max2175_poll_timeout(ctx, 69, 1, 1, 0, 50000);
+	if (ret)
+		mxm_err(ctx, "csm not ready\n");
+
+	return ret;
+}
+
+#define MAX2175_IS_BAND_AM(ctx)		\
+	(max2175_read_bits(ctx, 5, 1, 0) == MAX2175_BAND_AM)
+
+#define MAX2175_IS_BAND_VHF(ctx)	\
+	(max2175_read_bits(ctx, 5, 1, 0) == MAX2175_BAND_VHF)
+
+#define MAX2175_IS_FM_MODE(ctx)		\
+	(max2175_read_bits(ctx, 12, 5, 4) == 0)
+
+#define MAX2175_IS_FMHD_MODE(ctx)	\
+	(max2175_read_bits(ctx, 12, 5, 4) == 1)
+
+#define MAX2175_IS_DAB_MODE(ctx)	\
+	(max2175_read_bits(ctx, 12, 5, 4) == 2)
+
+static int max2175_band_from_freq(u32 freq)
+{
+	if (freq >= 144000 && freq <= 26100000)
+		return MAX2175_BAND_AM;
+	else if (freq >= 65000000 && freq <= 108000000)
+		return MAX2175_BAND_FM;
+
+	return MAX2175_BAND_VHF;
+}
+
+static void max2175_i2s_enable(struct max2175 *ctx, bool enable)
+{
+	if (enable)
+		/* Stuff bits are zeroed */
+		max2175_write_bits(ctx, 104, 3, 0, 2);
+	else
+		/* Keep SCK alive */
+		max2175_write_bits(ctx, 104, 3, 0, 9);
+	mxm_dbg(ctx, "i2s %sabled\n", enable ? "en" : "dis");
+}
+
+static void max2175_set_filter_coeffs(struct max2175 *ctx, u8 m_sel,
+				      u8 bank, const u16 *coeffs)
+{
+	unsigned int i;
+	u8 coeff_addr, upper_address = 24;
+
+	mxm_dbg(ctx, "set_filter_coeffs: m_sel %d bank %d\n", m_sel, bank);
+	max2175_write_bits(ctx, 114, 5, 4, m_sel);
+
+	if (m_sel == 2)
+		upper_address = 12;
+
+	for (i = 0; i < upper_address; i++) {
+		coeff_addr = i + bank * 24;
+		max2175_write(ctx, 115, coeffs[i] >> 8);
+		max2175_write(ctx, 116, coeffs[i]);
+		max2175_write(ctx, 117, coeff_addr | 1 << 7);
+	}
+	max2175_write_bit(ctx, 117, 7, 0);
+}
+
+static void max2175_load_fmeu_1p2(struct max2175 *ctx)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(fmeu1p2_map); i++)
+		max2175_write(ctx, fmeu1p2_map[i].idx, fmeu1p2_map[i].val);
+
+	ctx->decim_ratio = 36;
+
+	/* Load the Channel Filter Coefficients into channel filter bank #2 */
+	max2175_set_filter_coeffs(ctx, MAX2175_CH_MSEL, 0, ch_coeff_fmeu);
+	max2175_set_filter_coeffs(ctx, MAX2175_EQ_MSEL, 0,
+				  eq_coeff_fmeu1_ra02_m6db);
+}
+
+static void max2175_load_dab_1p2(struct max2175 *ctx)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(dab12_map); i++)
+		max2175_write(ctx, dab12_map[i].idx, dab12_map[i].val);
+
+	ctx->decim_ratio = 1;
+
+	/* Load the Channel Filter Coefficients into channel filter bank #2 */
+	max2175_set_filter_coeffs(ctx, MAX2175_CH_MSEL, 2, ch_coeff_dab1);
+}
+
+static void max2175_load_fmna_1p0(struct max2175 *ctx)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(fmna1p0_map); i++)
+		max2175_write(ctx, fmna1p0_map[i].idx, fmna1p0_map[i].val);
+}
+
+static void max2175_load_fmna_2p0(struct max2175 *ctx)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(fmna2p0_map); i++)
+		max2175_write(ctx, fmna2p0_map[i].idx, fmna2p0_map[i].val);
+}
+
+static void max2175_set_bbfilter(struct max2175 *ctx)
+{
+	if (MAX2175_IS_BAND_AM(ctx)) {
+		max2175_write_bits(ctx, 12, 3, 0, ctx->rom_bbf_bw_am);
+		mxm_dbg(ctx, "set_bbfilter AM: rom %d\n", ctx->rom_bbf_bw_am);
+	} else if (MAX2175_IS_DAB_MODE(ctx)) {
+		max2175_write_bits(ctx, 12, 3, 0, ctx->rom_bbf_bw_dab);
+		mxm_dbg(ctx, "set_bbfilter DAB: rom %d\n", ctx->rom_bbf_bw_dab);
+	} else {
+		max2175_write_bits(ctx, 12, 3, 0, ctx->rom_bbf_bw_fm);
+		mxm_dbg(ctx, "set_bbfilter FM: rom %d\n", ctx->rom_bbf_bw_fm);
+	}
+}
+
+static bool max2175_set_csm_mode(struct max2175 *ctx,
+			  enum max2175_csm_mode new_mode)
+{
+	int ret = max2175_poll_csm_ready(ctx);
+
+	if (ret)
+		return ret;
+
+	max2175_write_bits(ctx, 0, 2, 0, new_mode);
+	mxm_dbg(ctx, "set csm new mode %d\n", new_mode);
+
+	/* Wait for a fixed settle down time depending on new mode */
+	switch (new_mode) {
+	case MAX2175_PRESET_TUNE:
+		usleep_range(51100, 51500);	/* 51.1ms */
+		break;
+	/*
+	 * Other mode switches need different sleep values depending on band &
+	 * mode
+	 */
+	default:
+		break;
+	}
+
+	return max2175_poll_csm_ready(ctx);
+}
+
+static int max2175_csm_action(struct max2175 *ctx,
+			      enum max2175_csm_mode action)
+{
+	int ret;
+
+	mxm_dbg(ctx, "csm_action: %d\n", action);
+
+	/* Other actions can be added in future when needed */
+	ret = max2175_set_csm_mode(ctx, MAX2175_LOAD_TO_BUFFER);
+	if (ret)
+		return ret;
+
+	return max2175_set_csm_mode(ctx, MAX2175_PRESET_TUNE);
+}
+
+static int max2175_set_lo_freq(struct max2175 *ctx, u32 lo_freq)
+{
+	u8 lo_mult, loband_bits = 0, vcodiv_bits = 0;
+	u32 int_desired, frac_desired;
+	enum max2175_band band;
+	int ret;
+
+	band = max2175_read_bits(ctx, 5, 1, 0);
+	switch (band) {
+	case MAX2175_BAND_AM:
+		lo_mult = 16;
+		break;
+	case MAX2175_BAND_FM:
+		if (lo_freq <= 74700000) {
+			lo_mult = 16;
+		} else if (lo_freq > 74700000 && lo_freq <= 110000000) {
+			loband_bits = 1;
+			lo_mult = 8;
+		} else {
+			loband_bits = 1;
+			vcodiv_bits = 3;
+			lo_mult = 8;
+		}
+		break;
+	case MAX2175_BAND_VHF:
+		if (lo_freq <= 210000000)
+			vcodiv_bits = 2;
+		else
+			vcodiv_bits = 1;
+
+		loband_bits = 2;
+		lo_mult = 4;
+		break;
+	default:
+		loband_bits = 3;
+		vcodiv_bits = 2;
+		lo_mult = 2;
+		break;
+	}
+
+	if (band == MAX2175_BAND_L)
+		lo_freq /= lo_mult;
+	else
+		lo_freq *= lo_mult;
+
+	int_desired = lo_freq / ctx->xtal_freq;
+	frac_desired = div_u64((u64)(lo_freq % ctx->xtal_freq) << 20,
+			       ctx->xtal_freq);
+
+	/* Check CSM is not busy */
+	ret = max2175_poll_csm_ready(ctx);
+	if (ret)
+		return ret;
+
+	mxm_dbg(ctx, "lo_mult %u int %u  frac %u\n",
+		lo_mult, int_desired, frac_desired);
+
+	/* Write the calculated values to the appropriate registers */
+	max2175_write(ctx, 1, int_desired);
+	max2175_write_bits(ctx, 2, 3, 0, (frac_desired >> 16) & 0xf);
+	max2175_write(ctx, 3, frac_desired >> 8);
+	max2175_write(ctx, 4, frac_desired);
+	max2175_write_bits(ctx, 5, 3, 2, loband_bits);
+	max2175_write_bits(ctx, 6, 7, 6, vcodiv_bits);
+
+	return ret;
+}
+
+/*
+ * Helper similar to DIV_ROUND_CLOSEST but an inline function that accepts s64
+ * dividend and s32 divisor
+ */
+static inline s64 max2175_round_closest(s64 dividend, s32 divisor)
+{
+	if ((dividend > 0 && divisor > 0) || (dividend < 0 && divisor < 0))
+		return div_s64(dividend + divisor / 2, divisor);
+
+	return div_s64(dividend - divisor / 2, divisor);
+}
+
+static int max2175_set_nco_freq(struct max2175 *ctx, s32 nco_freq)
+{
+	s32 clock_rate = ctx->xtal_freq / ctx->decim_ratio;
+	u32 nco_reg, abs_nco_freq = abs(nco_freq);
+	s64 nco_val_desired;
+	int ret;
+
+	if (abs_nco_freq < clock_rate / 2) {
+		nco_val_desired = 2 * nco_freq;
+	} else {
+		nco_val_desired = 2 * (clock_rate - abs_nco_freq);
+		if (nco_freq < 0)
+			nco_val_desired = -nco_val_desired;
+	}
+
+	nco_reg = max2175_round_closest(nco_val_desired << 20, clock_rate);
+
+	if (nco_freq < 0)
+		nco_reg += 0x200000;
+
+	/* Check CSM is not busy */
+	ret = max2175_poll_csm_ready(ctx);
+	if (ret)
+		return ret;
+
+	mxm_dbg(ctx, "freq %d desired %lld reg %u\n",
+		nco_freq, nco_val_desired, nco_reg);
+
+	/* Write the calculated values to the appropriate registers */
+	max2175_write_bits(ctx, 7, 4, 0, (nco_reg >> 16) & 0x1f);
+	max2175_write(ctx, 8, nco_reg >> 8);
+	max2175_write(ctx, 9, nco_reg);
+
+	return ret;
+}
+
+static int max2175_set_rf_freq_non_am_bands(struct max2175 *ctx, u64 freq,
+					    u32 lo_pos)
+{
+	s64 adj_freq, low_if_freq;
+	int ret;
+
+	mxm_dbg(ctx, "rf_freq: non AM bands\n");
+
+	if (MAX2175_IS_FM_MODE(ctx))
+		low_if_freq = 128000;
+	else if (MAX2175_IS_FMHD_MODE(ctx))
+		low_if_freq = 228000;
+	else
+		return max2175_set_lo_freq(ctx, freq);
+
+	if (MAX2175_IS_BAND_VHF(ctx) == (lo_pos == MAX2175_LO_ABOVE_DESIRED))
+		adj_freq = freq + low_if_freq;
+	else
+		adj_freq = freq - low_if_freq;
+
+	ret = max2175_set_lo_freq(ctx, adj_freq);
+	if (ret)
+		return ret;
+
+	return max2175_set_nco_freq(ctx, -low_if_freq);
+}
+
+static int max2175_set_rf_freq(struct max2175 *ctx, u64 freq, u32 lo_pos)
+{
+	int ret;
+
+	if (MAX2175_IS_BAND_AM(ctx))
+		ret = max2175_set_nco_freq(ctx, freq);
+	else
+		ret = max2175_set_rf_freq_non_am_bands(ctx, freq, lo_pos);
+
+	mxm_dbg(ctx, "set_rf_freq: ret %d freq %llu\n", ret, freq);
+
+	return ret;
+}
+
+static int max2175_tune_rf_freq(struct max2175 *ctx, u64 freq, u32 hsls)
+{
+	int ret;
+
+	ret = max2175_set_rf_freq(ctx, freq, hsls);
+	if (ret)
+		return ret;
+
+	ret = max2175_csm_action(ctx, MAX2175_BUFFER_PLUS_PRESET_TUNE);
+	if (ret)
+		return ret;
+
+	mxm_dbg(ctx, "tune_rf_freq: old %u new %llu\n", ctx->freq, freq);
+	ctx->freq = freq;
+
+	return ret;
+}
+
+static void max2175_set_hsls(struct max2175 *ctx, u32 lo_pos)
+{
+	mxm_dbg(ctx, "set_hsls: lo_pos %u\n", lo_pos);
+
+	if ((lo_pos == MAX2175_LO_BELOW_DESIRED) == MAX2175_IS_BAND_VHF(ctx))
+		max2175_write_bit(ctx, 5, 4, 1);
+	else
+		max2175_write_bit(ctx, 5, 4, 0);
+}
+
+static void max2175_set_eu_rx_mode(struct max2175 *ctx, u32 rx_mode)
+{
+	switch (rx_mode) {
+	case MAX2175_EU_FM_1_2:
+		max2175_load_fmeu_1p2(ctx);
+		break;
+
+	case MAX2175_DAB_1_2:
+		max2175_load_dab_1p2(ctx);
+		break;
+	}
+	/* Master is the default setting */
+	if (!ctx->master)
+		max2175_write_bit(ctx, 30, 7, 1);
+}
+
+static void max2175_set_na_rx_mode(struct max2175 *ctx, u32 rx_mode)
+{
+	switch (rx_mode) {
+	case MAX2175_NA_FM_1_0:
+		max2175_load_fmna_1p0(ctx);
+		break;
+	case MAX2175_NA_FM_2_0:
+		max2175_load_fmna_2p0(ctx);
+		break;
+	}
+	/* Master is the default setting */
+	if (!ctx->master)
+		max2175_write_bit(ctx, 30, 7, 1);
+
+	ctx->decim_ratio = 27;
+
+	/* Load the Channel Filter Coefficients into channel filter bank #2 */
+	max2175_set_filter_coeffs(ctx, MAX2175_CH_MSEL, 0, ch_coeff_fmna);
+	max2175_set_filter_coeffs(ctx, MAX2175_EQ_MSEL, 0,
+				  eq_coeff_fmna1_ra02_m6db);
+}
+
+static int max2175_set_rx_mode(struct max2175 *ctx, u32 rx_mode)
+{
+	mxm_dbg(ctx, "set_rx_mode: %u am_hiz %u\n", rx_mode, ctx->am_hiz);
+	if (ctx->xtal_freq == MAX2175_EU_XTAL_FREQ)
+		max2175_set_eu_rx_mode(ctx, rx_mode);
+	else
+		max2175_set_na_rx_mode(ctx, rx_mode);
+
+	if (ctx->am_hiz) {
+		mxm_dbg(ctx, "setting AM HiZ related config\n");
+		max2175_write_bit(ctx, 50, 5, 1);
+		max2175_write_bit(ctx, 90, 7, 1);
+		max2175_write_bits(ctx, 73, 1, 0, 2);
+		max2175_write_bits(ctx, 80, 5, 0, 33);
+	}
+
+	/* Load BB filter trim values saved in ROM */
+	max2175_set_bbfilter(ctx);
+
+	/* Set HSLS */
+	max2175_set_hsls(ctx, ctx->hsls->cur.val);
+
+	/* Use i2s enable settings */
+	max2175_i2s_enable(ctx, ctx->i2s_en->cur.val);
+
+	ctx->mode_resolved = true;
+
+	return 0;
+}
+
+static int max2175_rx_mode_from_freq(struct max2175 *ctx, u32 freq, u32 *mode)
+{
+	unsigned int i;
+	int band = max2175_band_from_freq(freq);
+
+	/* Pick the first match always */
+	for (i = 0; i <= ctx->rx_mode->maximum; i++) {
+		if (ctx->rx_modes[i].band == band) {
+			*mode = i;
+			mxm_dbg(ctx, "rx_mode_from_freq: freq %u mode %d\n",
+				freq, *mode);
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static bool max2175_freq_rx_mode_valid(struct max2175 *ctx,
+					 u32 mode, u32 freq)
+{
+	int band = max2175_band_from_freq(freq);
+
+	return (ctx->rx_modes[mode].band == band);
+}
+
+static void max2175_load_adc_presets(struct max2175 *ctx)
+{
+	unsigned int i, j;
+
+	for (i = 0; i < ARRAY_SIZE(adc_presets); i++)
+		for (j = 0; j < ARRAY_SIZE(adc_presets[0]); j++)
+			max2175_write(ctx, 146 + j + i * 55, adc_presets[i][j]);
+}
+
+static int max2175_init_power_manager(struct max2175 *ctx)
+{
+	int ret;
+
+	/* Execute on-chip power-up/calibration */
+	max2175_write_bit(ctx, 99, 2, 0);
+	usleep_range(1000, 1500);
+	max2175_write_bit(ctx, 99, 2, 1);
+
+	/* Wait for the power manager to finish. */
+	ret = max2175_poll_timeout(ctx, 69, 7, 7, 1, 50000);
+	if (ret)
+		mxm_err(ctx, "init pm failed\n");
+
+	return ret;
+}
+
+static int max2175_recalibrate_adc(struct max2175 *ctx)
+{
+	int ret;
+
+	/* ADC Re-calibration */
+	max2175_write(ctx, 150, 0xff);
+	max2175_write(ctx, 205, 0xff);
+	max2175_write(ctx, 147, 0x20);
+	max2175_write(ctx, 147, 0x00);
+	max2175_write(ctx, 202, 0x20);
+	max2175_write(ctx, 202, 0x00);
+
+	ret = max2175_poll_timeout(ctx, 69, 4, 3, 3, 50000);
+	if (ret)
+		mxm_err(ctx, "adc recalibration failed\n");
+
+	return ret;
+}
+
+static u8 max2175_read_rom(struct max2175 *ctx, u8 row)
+{
+	u8 data = 0;
+
+	max2175_write_bit(ctx, 56, 4, 0);
+	max2175_write_bits(ctx, 56, 3, 0, row);
+
+	usleep_range(2000, 2500);
+	max2175_read(ctx, 58, &data);
+
+	max2175_write_bits(ctx, 56, 3, 0, 0);
+
+	mxm_dbg(ctx, "read_rom: row %d data 0x%02x\n", row, data);
+
+	return data;
+}
+
+static void max2175_load_from_rom(struct max2175 *ctx)
+{
+	u8 data = 0;
+
+	data = max2175_read_rom(ctx, 0);
+	ctx->rom_bbf_bw_am = data & 0x0f;
+	max2175_write_bits(ctx, 81, 3, 0, data >> 4);
+
+	data = max2175_read_rom(ctx, 1);
+	ctx->rom_bbf_bw_fm = data & 0x0f;
+	ctx->rom_bbf_bw_dab = data >> 4;
+
+	data = max2175_read_rom(ctx, 2);
+	max2175_write_bits(ctx, 82, 4, 0, data & 0x1f);
+	max2175_write_bits(ctx, 82, 7, 5, data >> 5);
+
+	data = max2175_read_rom(ctx, 3);
+	if (ctx->am_hiz) {
+		data &= 0x0f;
+		data |= (max2175_read_rom(ctx, 7) & 0x40) >> 2;
+		if (!data)
+			data |= 2;
+	} else {
+		data = (data & 0xf0) >> 4;
+		data |= (max2175_read_rom(ctx, 7) & 0x80) >> 3;
+		if (!data)
+			data |= 30;
+	}
+	max2175_write_bits(ctx, 80, 5, 0, data + 31);
+
+	data = max2175_read_rom(ctx, 6);
+	max2175_write_bits(ctx, 81, 7, 6, data >> 6);
+}
+
+static void max2175_load_full_fm_eu_1p0(struct max2175 *ctx)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(full_fm_eu_1p0); i++)
+		max2175_write(ctx, i + 1, full_fm_eu_1p0[i]);
+
+	usleep_range(5000, 5500);
+	ctx->decim_ratio = 36;
+}
+
+static void max2175_load_full_fm_na_1p0(struct max2175 *ctx)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(full_fm_na_1p0); i++)
+		max2175_write(ctx, i + 1, full_fm_na_1p0[i]);
+
+	usleep_range(5000, 5500);
+	ctx->decim_ratio = 27;
+}
+
+static int max2175_core_init(struct max2175 *ctx, u32 refout_bits)
+{
+	int ret;
+
+	/* MAX2175 uses 36.864MHz clock for EU & 40.154MHz for NA region */
+	if (ctx->xtal_freq == MAX2175_EU_XTAL_FREQ)
+		max2175_load_full_fm_eu_1p0(ctx);
+	else
+		max2175_load_full_fm_na_1p0(ctx);
+
+	/* The default settings assume master */
+	if (!ctx->master)
+		max2175_write_bit(ctx, 30, 7, 1);
+
+	mxm_dbg(ctx, "refout_bits %u\n", refout_bits);
+
+	/* Set REFOUT */
+	max2175_write_bits(ctx, 56, 7, 5, refout_bits);
+
+	/* ADC Reset */
+	max2175_write_bit(ctx, 99, 1, 0);
+	usleep_range(1000, 1500);
+	max2175_write_bit(ctx, 99, 1, 1);
+
+	/* Load ADC preset values */
+	max2175_load_adc_presets(ctx);
+
+	/* Initialize the power management state machine */
+	ret = max2175_init_power_manager(ctx);
+	if (ret)
+		return ret;
+
+	/* Recalibrate ADC */
+	ret = max2175_recalibrate_adc(ctx);
+	if (ret)
+		return ret;
+
+	/* Load ROM values to appropriate registers */
+	max2175_load_from_rom(ctx);
+
+	if (ctx->xtal_freq == MAX2175_EU_XTAL_FREQ) {
+		/* Load FIR coefficients into bank 0 */
+		max2175_set_filter_coeffs(ctx, MAX2175_CH_MSEL, 0,
+					  ch_coeff_fmeu);
+		max2175_set_filter_coeffs(ctx, MAX2175_EQ_MSEL, 0,
+					  eq_coeff_fmeu1_ra02_m6db);
+	} else {
+		/* Load FIR coefficients into bank 0 */
+		max2175_set_filter_coeffs(ctx, MAX2175_CH_MSEL, 0,
+					  ch_coeff_fmna);
+		max2175_set_filter_coeffs(ctx, MAX2175_EQ_MSEL, 0,
+					  eq_coeff_fmna1_ra02_m6db);
+	}
+	mxm_dbg(ctx, "core initialized\n");
+
+	return 0;
+}
+
+static void max2175_s_ctrl_rx_mode(struct max2175 *ctx, u32 rx_mode)
+{
+	/* Load mode. Range check already done */
+	max2175_set_rx_mode(ctx, rx_mode);
+
+	mxm_dbg(ctx, "s_ctrl_rx_mode: %u curr freq %u\n", rx_mode, ctx->freq);
+
+	/* Check if current freq valid for mode & update */
+	if (max2175_freq_rx_mode_valid(ctx, rx_mode, ctx->freq))
+		max2175_tune_rf_freq(ctx, ctx->freq, ctx->hsls->cur.val);
+	else
+		/* Use default freq of mode if current freq is not valid */
+		max2175_tune_rf_freq(ctx, ctx->rx_modes[rx_mode].freq,
+				     ctx->hsls->cur.val);
+}
+
+static int max2175_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct max2175 *ctx = max2175_from_ctrl_hdl(ctrl->handler);
+
+	mxm_dbg(ctx, "s_ctrl: id 0x%x, val %u\n", ctrl->id, ctrl->val);
+	switch (ctrl->id) {
+	case V4L2_CID_MAX2175_I2S_ENABLE:
+		max2175_i2s_enable(ctx, ctrl->val);
+		break;
+	case V4L2_CID_MAX2175_HSLS:
+		max2175_set_hsls(ctx, ctrl->val);
+		break;
+	case V4L2_CID_MAX2175_RX_MODE:
+		max2175_s_ctrl_rx_mode(ctx, ctrl->val);
+		break;
+	}
+
+	return 0;
+}
+
+static u32 max2175_get_lna_gain(struct max2175 *ctx)
+{
+	enum max2175_band band = max2175_read_bits(ctx, 5, 1, 0);
+
+	switch (band) {
+	case MAX2175_BAND_AM:
+		return max2175_read_bits(ctx, 51, 3, 0);
+	case MAX2175_BAND_FM:
+		return max2175_read_bits(ctx, 50, 3, 0);
+	case MAX2175_BAND_VHF:
+		return max2175_read_bits(ctx, 52, 5, 0);
+	default:
+		return 0;
+	}
+}
+
+static int max2175_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct max2175 *ctx = max2175_from_ctrl_hdl(ctrl->handler);
+
+	switch (ctrl->id) {
+	case V4L2_CID_RF_TUNER_LNA_GAIN:
+		ctrl->val = max2175_get_lna_gain(ctx);
+		break;
+	case V4L2_CID_RF_TUNER_IF_GAIN:
+		ctrl->val = max2175_read_bits(ctx, 49, 4, 0);
+		break;
+	case V4L2_CID_RF_TUNER_PLL_LOCK:
+		ctrl->val = (max2175_read_bits(ctx, 60, 7, 6) == 3);
+		break;
+	}
+
+	return 0;
+};
+
+static int max2175_set_freq_and_mode(struct max2175 *ctx, u32 freq)
+{
+	u32 rx_mode;
+	int ret;
+
+	/* Get band from frequency */
+	ret = max2175_rx_mode_from_freq(ctx, freq, &rx_mode);
+	if (ret)
+		return ret;
+
+	mxm_dbg(ctx, "set_freq_and_mode: freq %u rx_mode %d\n", freq, rx_mode);
+
+	/* Load mode */
+	max2175_set_rx_mode(ctx, rx_mode);
+	ctx->rx_mode->cur.val = rx_mode;
+
+	/* Tune to the new freq given */
+	return max2175_tune_rf_freq(ctx, freq, ctx->hsls->cur.val);
+}
+
+static int max2175_s_frequency(struct v4l2_subdev *sd,
+			       const struct v4l2_frequency *vf)
+{
+	struct max2175 *ctx = max2175_from_sd(sd);
+	u32 freq;
+	int ret = 0;
+
+	mxm_dbg(ctx, "s_freq: new %u curr %u, mode_resolved %d\n",
+		vf->frequency, ctx->freq, ctx->mode_resolved);
+
+	if (vf->tuner != 0)
+		return -EINVAL;
+
+	freq = clamp(vf->frequency, ctx->bands_rf->rangelow,
+		     ctx->bands_rf->rangehigh);
+
+	/* Check new freq valid for rx_mode if already resolved */
+	if (ctx->mode_resolved &&
+	    max2175_freq_rx_mode_valid(ctx, ctx->rx_mode->cur.val, freq))
+		ret = max2175_tune_rf_freq(ctx, freq, ctx->hsls->cur.val);
+	else
+		/* Find default rx_mode for freq and tune to it */
+		ret = max2175_set_freq_and_mode(ctx, freq);
+
+	mxm_dbg(ctx, "s_freq: ret %d curr %u mode_resolved %d mode %u\n",
+		ret, ctx->freq, ctx->mode_resolved, ctx->rx_mode->cur.val);
+
+	return ret;
+}
+
+static int max2175_g_frequency(struct v4l2_subdev *sd,
+			       struct v4l2_frequency *vf)
+{
+	struct max2175 *ctx = max2175_from_sd(sd);
+	int ret = 0;
+
+	if (vf->tuner != 0)
+		return -EINVAL;
+
+	/* RF freq */
+	vf->type = V4L2_TUNER_RF;
+	vf->frequency = ctx->freq;
+
+	return ret;
+}
+
+static int max2175_enum_freq_bands(struct v4l2_subdev *sd,
+			    struct v4l2_frequency_band *band)
+{
+	struct max2175 *ctx = max2175_from_sd(sd);
+
+	if (band->tuner != 0 || band->index != 0)
+		return -EINVAL;
+
+	*band = *ctx->bands_rf;
+
+	return 0;
+}
+
+static int max2175_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
+{
+	struct max2175 *ctx = max2175_from_sd(sd);
+
+	if (vt->index > 0)
+		return -EINVAL;
+
+	strlcpy(vt->name, "RF", sizeof(vt->name));
+	vt->type = V4L2_TUNER_RF;
+	vt->capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
+	vt->rangelow = ctx->bands_rf->rangelow;
+	vt->rangehigh = ctx->bands_rf->rangehigh;
+
+	return 0;
+}
+
+static int max2175_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *vt)
+{
+	/* Check tuner index is valid */
+	if (vt->index > 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_tuner_ops max2175_tuner_ops = {
+	.s_frequency = max2175_s_frequency,
+	.g_frequency = max2175_g_frequency,
+	.enum_freq_bands = max2175_enum_freq_bands,
+	.g_tuner = max2175_g_tuner,
+	.s_tuner = max2175_s_tuner,
+};
+
+static const struct v4l2_subdev_ops max2175_ops = {
+	.tuner = &max2175_tuner_ops,
+};
+
+static const struct v4l2_ctrl_ops max2175_ctrl_ops = {
+	.s_ctrl = max2175_s_ctrl,
+	.g_volatile_ctrl = max2175_g_volatile_ctrl,
+};
+
+/*
+ * I2S output enable/disable configuration. This is a private control.
+ * Refer to Documentation/media/v4l-drivers/max2175 for more details.
+ */
+static const struct v4l2_ctrl_config max2175_i2s_en = {
+	.ops = &max2175_ctrl_ops,
+	.id = V4L2_CID_MAX2175_I2S_ENABLE,
+	.name = "I2S Enable",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+	.is_private = 1,
+};
+
+/*
+ * HSLS value control LO freq adjacent location configuration.
+ * Refer to Documentation/media/v4l-drivers/max2175 for more details.
+ */
+static const struct v4l2_ctrl_config max2175_hsls = {
+	.ops = &max2175_ctrl_ops,
+	.id = V4L2_CID_MAX2175_HSLS,
+	.name = "HSLS Above/Below Desired",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+/*
+ * Rx modes below are a set of preset configurations that decides the tuner's
+ * sck and sample rate of transmission. They are separate for EU & NA regions.
+ * Refer to Documentation/media/v4l-drivers/max2175 for more details.
+ */
+static const char * const max2175_ctrl_eu_rx_modes[] = {
+	[MAX2175_EU_FM_1_2]	= "EU FM 1.2",
+	[MAX2175_DAB_1_2]	= "DAB 1.2",
+};
+
+static const char * const max2175_ctrl_na_rx_modes[] = {
+	[MAX2175_NA_FM_1_0]	= "NA FM 1.0",
+	[MAX2175_NA_FM_2_0]	= "NA FM 2.0",
+};
+
+static const struct v4l2_ctrl_config max2175_eu_rx_mode = {
+	.ops = &max2175_ctrl_ops,
+	.id = V4L2_CID_MAX2175_RX_MODE,
+	.name = "RX Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(max2175_ctrl_eu_rx_modes) - 1,
+	.def = 0,
+	.qmenu = max2175_ctrl_eu_rx_modes,
+};
+
+static const struct v4l2_ctrl_config max2175_na_rx_mode = {
+	.ops = &max2175_ctrl_ops,
+	.id = V4L2_CID_MAX2175_RX_MODE,
+	.name = "RX Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(max2175_ctrl_na_rx_modes) - 1,
+	.def = 0,
+	.qmenu = max2175_ctrl_na_rx_modes,
+};
+
+static int max2175_refout_load_to_bits(struct i2c_client *client, u32 load,
+				       u32 *bits)
+{
+	if (load <= 40)
+		*bits = load / 10;
+	else if (load >= 60 && load <= 70)
+		*bits = load / 10 - 1;
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
+static int max2175_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	bool master = true, am_hiz = false;
+	u32 refout_load, refout_bits = 0;	/* REFOUT disabled */
+	struct v4l2_ctrl_handler *hdl;
+	struct fwnode_handle *fwnode;
+	struct device_node *np;
+	struct v4l2_subdev *sd;
+	struct regmap *regmap;
+	struct max2175 *ctx;
+	struct clk *clk;
+	int ret;
+
+	/* Parse DT properties */
+	np = of_parse_phandle(client->dev.of_node, "maxim,master", 0);
+	if (np) {
+		master = false;			/* Slave tuner */
+		of_node_put(np);
+	}
+
+	fwnode = of_fwnode_handle(client->dev.of_node);
+	if (fwnode_property_present(fwnode, "maxim,am-hiz-filter"))
+		am_hiz = true;
+
+	if (!fwnode_property_read_u32(fwnode, "maxim,refout-load",
+				      &refout_load)) {
+		ret = max2175_refout_load_to_bits(client, refout_load,
+						  &refout_bits);
+		if (ret) {
+			dev_err(&client->dev, "invalid refout_load %u\n",
+				refout_load);
+			return -EINVAL;
+		}
+	}
+
+	clk = devm_clk_get(&client->dev, NULL);
+	if (IS_ERR(clk)) {
+		ret = PTR_ERR(clk);
+		dev_err(&client->dev, "cannot get clock %d\n", ret);
+		return -ENODEV;
+	}
+
+	regmap = devm_regmap_init_i2c(client, &max2175_regmap_config);
+	if (IS_ERR(regmap)) {
+		ret = PTR_ERR(regmap);
+		dev_err(&client->dev, "regmap init failed %d\n", ret);
+		return -ENODEV;
+	}
+
+	/* Alloc tuner context */
+	ctx = devm_kzalloc(&client->dev, sizeof(*ctx), GFP_KERNEL);
+	if (ctx == NULL)
+		return -ENOMEM;
+
+	sd = &ctx->sd;
+	ctx->master = master;
+	ctx->am_hiz = am_hiz;
+	ctx->mode_resolved = false;
+	ctx->regmap = regmap;
+	ctx->xtal_freq = clk_get_rate(clk);
+	dev_info(&client->dev, "xtal freq %luHz\n", ctx->xtal_freq);
+
+	v4l2_i2c_subdev_init(sd, client, &max2175_ops);
+	ctx->client = client;
+
+	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	/* Controls */
+	hdl = &ctx->ctrl_hdl;
+	ret = v4l2_ctrl_handler_init(hdl, 7);
+	if (ret)
+		return ret;
+
+	ctx->lna_gain = v4l2_ctrl_new_std(hdl, &max2175_ctrl_ops,
+					  V4L2_CID_RF_TUNER_LNA_GAIN,
+					  0, 63, 1, 0);
+	ctx->lna_gain->flags |= (V4L2_CTRL_FLAG_VOLATILE |
+				 V4L2_CTRL_FLAG_READ_ONLY);
+	ctx->if_gain = v4l2_ctrl_new_std(hdl, &max2175_ctrl_ops,
+					 V4L2_CID_RF_TUNER_IF_GAIN,
+					 0, 31, 1, 0);
+	ctx->if_gain->flags |= (V4L2_CTRL_FLAG_VOLATILE |
+				V4L2_CTRL_FLAG_READ_ONLY);
+	ctx->pll_lock = v4l2_ctrl_new_std(hdl, &max2175_ctrl_ops,
+					  V4L2_CID_RF_TUNER_PLL_LOCK,
+					  0, 1, 1, 0);
+	ctx->pll_lock->flags |= (V4L2_CTRL_FLAG_VOLATILE |
+				 V4L2_CTRL_FLAG_READ_ONLY);
+	ctx->i2s_en = v4l2_ctrl_new_custom(hdl, &max2175_i2s_en, NULL);
+	ctx->hsls = v4l2_ctrl_new_custom(hdl, &max2175_hsls, NULL);
+
+	if (ctx->xtal_freq == MAX2175_EU_XTAL_FREQ) {
+		ctx->rx_mode = v4l2_ctrl_new_custom(hdl,
+						    &max2175_eu_rx_mode, NULL);
+		ctx->rx_modes = eu_rx_modes;
+		ctx->bands_rf = &eu_bands_rf;
+	} else {
+		ctx->rx_mode = v4l2_ctrl_new_custom(hdl,
+						    &max2175_na_rx_mode, NULL);
+		ctx->rx_modes = na_rx_modes;
+		ctx->bands_rf = &na_bands_rf;
+	}
+	ctx->sd.ctrl_handler = &ctx->ctrl_hdl;
+
+	/* Set the defaults */
+	ctx->freq = ctx->bands_rf->rangelow;
+
+	/* Register subdev */
+	ret = v4l2_async_register_subdev(sd);
+	if (ret) {
+		dev_err(&client->dev, "register subdev failed\n");
+		goto err_reg;
+	}
+
+	/* Initialize device */
+	ret = max2175_core_init(ctx, refout_bits);
+	if (ret)
+		goto err_init;
+
+	ret = v4l2_ctrl_handler_setup(hdl);
+	if (ret)
+		goto err_init;
+
+	return 0;
+
+err_init:
+	v4l2_async_unregister_subdev(sd);
+err_reg:
+	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
+
+	return ret;
+}
+
+static int max2175_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct max2175 *ctx = max2175_from_sd(sd);
+
+	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
+	v4l2_async_unregister_subdev(sd);
+
+	return 0;
+}
+
+static const struct i2c_device_id max2175_id[] = {
+	{ DRIVER_NAME, 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, max2175_id);
+
+static const struct of_device_id max2175_of_ids[] = {
+	{ .compatible = "maxim,max2175", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, max2175_of_ids);
+
+static struct i2c_driver max2175_driver = {
+	.driver = {
+		.name	= DRIVER_NAME,
+		.of_match_table = max2175_of_ids,
+	},
+	.probe		= max2175_probe,
+	.remove		= max2175_remove,
+	.id_table	= max2175_id,
+};
+
+module_i2c_driver(max2175_driver);
+
+MODULE_DESCRIPTION("Maxim MAX2175 RF to Bits tuner driver");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Ramesh Shanmugasundaram <ramesh.shanmugasundaram@bp.renesas.com>");
diff --git a/drivers/media/i2c/max2175.h b/drivers/media/i2c/max2175.h
new file mode 100644
index 000000000000..eb43373ce7e2
--- /dev/null
+++ b/drivers/media/i2c/max2175.h
@@ -0,0 +1,109 @@
+/*
+ * Maxim Integrated MAX2175 RF to Bits tuner driver
+ *
+ * This driver & most of the hard coded values are based on the reference
+ * application delivered by Maxim for this device.
+ *
+ * Copyright (C) 2016 Maxim Integrated Products
+ * Copyright (C) 2017 Renesas Electronics Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MAX2175_H__
+#define __MAX2175_H__
+
+#define MAX2175_EU_XTAL_FREQ	36864000	/* In Hz */
+#define MAX2175_NA_XTAL_FREQ	40186125	/* In Hz */
+
+enum max2175_region {
+	MAX2175_REGION_EU = 0,	/* Europe */
+	MAX2175_REGION_NA,	/* North America */
+};
+
+enum max2175_band {
+	MAX2175_BAND_AM = 0,
+	MAX2175_BAND_FM,
+	MAX2175_BAND_VHF,
+	MAX2175_BAND_L,
+};
+
+enum max2175_eu_mode {
+	/* EU modes */
+	MAX2175_EU_FM_1_2 = 0,
+	MAX2175_DAB_1_2,
+
+	/*
+	 * Other possible modes to add in future
+	 * MAX2175_DAB_1_0,
+	 * MAX2175_DAB_1_3,
+	 * MAX2175_EU_FM_2_2,
+	 * MAX2175_EU_FMHD_4_0,
+	 * MAX2175_EU_AM_1_0,
+	 * MAX2175_EU_AM_2_2,
+	 */
+};
+
+enum max2175_na_mode {
+	/* NA modes */
+	MAX2175_NA_FM_1_0 = 0,
+	MAX2175_NA_FM_2_0,
+
+	/*
+	 * Other possible modes to add in future
+	 * MAX2175_NA_FMHD_1_0,
+	 * MAX2175_NA_FMHD_1_2,
+	 * MAX2175_NA_AM_1_0,
+	 * MAX2175_NA_AM_1_2,
+	 */
+};
+
+/* Supported I2S modes */
+enum {
+	MAX2175_I2S_MODE0 = 0,
+	MAX2175_I2S_MODE1,
+	MAX2175_I2S_MODE2,
+	MAX2175_I2S_MODE3,
+	MAX2175_I2S_MODE4,
+};
+
+/* Coefficient table groups */
+enum {
+	MAX2175_CH_MSEL = 0,
+	MAX2175_EQ_MSEL,
+	MAX2175_AA_MSEL,
+};
+
+/* HSLS LO injection polarity */
+enum {
+	MAX2175_LO_BELOW_DESIRED = 0,
+	MAX2175_LO_ABOVE_DESIRED,
+};
+
+/* Channel FSM modes */
+enum max2175_csm_mode {
+	MAX2175_LOAD_TO_BUFFER = 0,
+	MAX2175_PRESET_TUNE,
+	MAX2175_SEARCH,
+	MAX2175_AF_UPDATE,
+	MAX2175_JUMP_FAST_TUNE,
+	MAX2175_CHECK,
+	MAX2175_LOAD_AND_SWAP,
+	MAX2175_END,
+	MAX2175_BUFFER_PLUS_PRESET_TUNE,
+	MAX2175_BUFFER_PLUS_SEARCH,
+	MAX2175_BUFFER_PLUS_AF_UPDATE,
+	MAX2175_BUFFER_PLUS_JUMP_FAST_TUNE,
+	MAX2175_BUFFER_PLUS_CHECK,
+	MAX2175_BUFFER_PLUS_LOAD_AND_SWAP,
+	MAX2175_NO_ACTION
+};
+
+#endif /* __MAX2175_H__ */
diff --git a/drivers/media/i2c/msp3400-kthreads.c b/drivers/media/i2c/msp3400-kthreads.c
index 11fc593ed908..4dd01e9f553b 100644
--- a/drivers/media/i2c/msp3400-kthreads.c
+++ b/drivers/media/i2c/msp3400-kthreads.c
@@ -655,6 +655,7 @@ restart:
 			break;
 		case 0: /* 4.5 */
 			state->detected_std = V4L2_STD_MN;
+			/* fall-through */
 		default:
 no_second:
 			state->second = msp3400c_carrier_detect_main[max1].cdo;
diff --git a/drivers/media/i2c/mt9v032.c b/drivers/media/i2c/mt9v032.c
index 2e7a6e62a358..8a430640c85d 100644
--- a/drivers/media/i2c/mt9v032.c
+++ b/drivers/media/i2c/mt9v032.c
@@ -19,6 +19,7 @@
 #include <linux/log2.h>
 #include <linux/mutex.h>
 #include <linux/of.h>
+#include <linux/of_graph.h>
 #include <linux/regmap.h>
 #include <linux/slab.h>
 #include <linux/videodev2.h>
@@ -28,7 +29,7 @@
 #include <media/i2c/mt9v032.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 #include <media/v4l2-subdev.h>
 
 /* The first four rows are black rows. The active area spans 753x481 pixels. */
@@ -979,7 +980,7 @@ static struct mt9v032_platform_data *
 mt9v032_get_pdata(struct i2c_client *client)
 {
 	struct mt9v032_platform_data *pdata = NULL;
-	struct v4l2_of_endpoint endpoint;
+	struct v4l2_fwnode_endpoint endpoint;
 	struct device_node *np;
 	struct property *prop;
 
@@ -990,7 +991,7 @@ mt9v032_get_pdata(struct i2c_client *client)
 	if (!np)
 		return NULL;
 
-	if (v4l2_of_parse_endpoint(np, &endpoint) < 0)
+	if (v4l2_fwnode_endpoint_parse(of_fwnode_handle(np), &endpoint) < 0)
 		goto done;
 
 	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
diff --git a/drivers/media/i2c/ov13858.c b/drivers/media/i2c/ov13858.c
new file mode 100644
index 000000000000..86550d8ddfee
--- /dev/null
+++ b/drivers/media/i2c/ov13858.c
@@ -0,0 +1,1816 @@
+/*
+ * Copyright (c) 2017 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/acpi.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+
+#define OV13858_REG_VALUE_08BIT		1
+#define OV13858_REG_VALUE_16BIT		2
+#define OV13858_REG_VALUE_24BIT		3
+
+#define OV13858_REG_MODE_SELECT		0x0100
+#define OV13858_MODE_STANDBY		0x00
+#define OV13858_MODE_STREAMING		0x01
+
+#define OV13858_REG_SOFTWARE_RST	0x0103
+#define OV13858_SOFTWARE_RST		0x01
+
+/* PLL1 generates PCLK and MIPI_PHY_CLK */
+#define OV13858_REG_PLL1_CTRL_0		0x0300
+#define OV13858_REG_PLL1_CTRL_1		0x0301
+#define OV13858_REG_PLL1_CTRL_2		0x0302
+#define OV13858_REG_PLL1_CTRL_3		0x0303
+#define OV13858_REG_PLL1_CTRL_4		0x0304
+#define OV13858_REG_PLL1_CTRL_5		0x0305
+
+/* PLL2 generates DAC_CLK, SCLK and SRAM_CLK */
+#define OV13858_REG_PLL2_CTRL_B		0x030b
+#define OV13858_REG_PLL2_CTRL_C		0x030c
+#define OV13858_REG_PLL2_CTRL_D		0x030d
+#define OV13858_REG_PLL2_CTRL_E		0x030e
+#define OV13858_REG_PLL2_CTRL_F		0x030f
+#define OV13858_REG_PLL2_CTRL_12	0x0312
+#define OV13858_REG_MIPI_SC_CTRL0	0x3016
+#define OV13858_REG_MIPI_SC_CTRL1	0x3022
+
+/* Chip ID */
+#define OV13858_REG_CHIP_ID		0x300a
+#define OV13858_CHIP_ID			0x00d855
+
+/* V_TIMING internal */
+#define OV13858_REG_VTS			0x380e
+#define OV13858_VTS_30FPS		0x0c8e /* 30 fps */
+#define OV13858_VTS_60FPS		0x0648 /* 60 fps */
+#define OV13858_VTS_MAX			0x7fff
+#define OV13858_VBLANK_MIN		56
+
+/* HBLANK control - read only */
+#define OV13858_PPL_540MHZ		2244
+#define OV13858_PPL_1080MHZ		4488
+
+/* Exposure control */
+#define OV13858_REG_EXPOSURE		0x3500
+#define OV13858_EXPOSURE_MIN		4
+#define OV13858_EXPOSURE_MAX		(OV13858_VTS_MAX - 8)
+#define OV13858_EXPOSURE_STEP		1
+#define OV13858_EXPOSURE_DEFAULT	0x640
+
+/* Analog gain control */
+#define OV13858_REG_ANALOG_GAIN		0x3508
+#define OV13858_ANA_GAIN_MIN		0
+#define OV13858_ANA_GAIN_MAX		0x1fff
+#define OV13858_ANA_GAIN_STEP		1
+#define OV13858_ANA_GAIN_DEFAULT	0x80
+
+/* Digital gain control */
+#define OV13858_REG_DIGITAL_GAIN	0x350a
+#define OV13858_DGTL_GAIN_MASK		0xf3
+#define OV13858_DGTL_GAIN_SHIFT		2
+#define OV13858_DGTL_GAIN_MIN		1
+#define OV13858_DGTL_GAIN_MAX		4
+#define OV13858_DGTL_GAIN_STEP		1
+#define OV13858_DGTL_GAIN_DEFAULT	1
+
+/* Test Pattern Control */
+#define OV13858_REG_TEST_PATTERN	0x4503
+#define OV13858_TEST_PATTERN_ENABLE	BIT(7)
+#define OV13858_TEST_PATTERN_MASK	0xfc
+
+/* Number of frames to skip */
+#define OV13858_NUM_OF_SKIP_FRAMES	2
+
+struct ov13858_reg {
+	u16 address;
+	u8 val;
+};
+
+struct ov13858_reg_list {
+	u32 num_of_regs;
+	const struct ov13858_reg *regs;
+};
+
+/* Link frequency config */
+struct ov13858_link_freq_config {
+	u32 pixel_rate;
+	u32 pixels_per_line;
+
+	/* PLL registers for this link frequency */
+	struct ov13858_reg_list reg_list;
+};
+
+/* Mode : resolution and related config&values */
+struct ov13858_mode {
+	/* Frame width */
+	u32 width;
+	/* Frame height */
+	u32 height;
+
+	/* V-timing */
+	u32 vts;
+
+	/* Index of Link frequency config to be used */
+	u32 link_freq_index;
+	/* Default register values */
+	struct ov13858_reg_list reg_list;
+};
+
+/* 4224x3136 needs 1080Mbps/lane, 4 lanes */
+static const struct ov13858_reg mipi_data_rate_1080mbps[] = {
+	/* PLL1 registers */
+	{OV13858_REG_PLL1_CTRL_0, 0x07},
+	{OV13858_REG_PLL1_CTRL_1, 0x01},
+	{OV13858_REG_PLL1_CTRL_2, 0xc2},
+	{OV13858_REG_PLL1_CTRL_3, 0x00},
+	{OV13858_REG_PLL1_CTRL_4, 0x00},
+	{OV13858_REG_PLL1_CTRL_5, 0x01},
+
+	/* PLL2 registers */
+	{OV13858_REG_PLL2_CTRL_B, 0x05},
+	{OV13858_REG_PLL2_CTRL_C, 0x01},
+	{OV13858_REG_PLL2_CTRL_D, 0x0e},
+	{OV13858_REG_PLL2_CTRL_E, 0x05},
+	{OV13858_REG_PLL2_CTRL_F, 0x01},
+	{OV13858_REG_PLL2_CTRL_12, 0x01},
+	{OV13858_REG_MIPI_SC_CTRL0, 0x72},
+	{OV13858_REG_MIPI_SC_CTRL1, 0x01},
+};
+
+/*
+ * 2112x1568, 2112x1188, 1056x784 need 540Mbps/lane,
+ * 4 lanes
+ */
+static const struct ov13858_reg mipi_data_rate_540mbps[] = {
+	/* PLL1 registers */
+	{OV13858_REG_PLL1_CTRL_0, 0x07},
+	{OV13858_REG_PLL1_CTRL_1, 0x01},
+	{OV13858_REG_PLL1_CTRL_2, 0xc2},
+	{OV13858_REG_PLL1_CTRL_3, 0x01},
+	{OV13858_REG_PLL1_CTRL_4, 0x00},
+	{OV13858_REG_PLL1_CTRL_5, 0x01},
+
+	/* PLL2 registers */
+	{OV13858_REG_PLL2_CTRL_B, 0x05},
+	{OV13858_REG_PLL2_CTRL_C, 0x01},
+	{OV13858_REG_PLL2_CTRL_D, 0x0e},
+	{OV13858_REG_PLL2_CTRL_E, 0x05},
+	{OV13858_REG_PLL2_CTRL_F, 0x01},
+	{OV13858_REG_PLL2_CTRL_12, 0x01},
+	{OV13858_REG_MIPI_SC_CTRL0, 0x72},
+	{OV13858_REG_MIPI_SC_CTRL1, 0x01},
+};
+
+static const struct ov13858_reg mode_4224x3136_regs[] = {
+	{0x3013, 0x32},
+	{0x301b, 0xf0},
+	{0x301f, 0xd0},
+	{0x3106, 0x15},
+	{0x3107, 0x23},
+	{0x350a, 0x00},
+	{0x350e, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3600, 0x2b},
+	{0x3601, 0x52},
+	{0x3602, 0x60},
+	{0x3612, 0x05},
+	{0x3613, 0xa4},
+	{0x3620, 0x80},
+	{0x3621, 0x10},
+	{0x3622, 0x30},
+	{0x3624, 0x1c},
+	{0x3640, 0x10},
+	{0x3641, 0x70},
+	{0x3661, 0x80},
+	{0x3662, 0x12},
+	{0x3664, 0x73},
+	{0x3665, 0xa7},
+	{0x366e, 0xff},
+	{0x366f, 0xf4},
+	{0x3674, 0x00},
+	{0x3679, 0x0c},
+	{0x367f, 0x01},
+	{0x3680, 0x0c},
+	{0x3681, 0x50},
+	{0x3682, 0x50},
+	{0x3683, 0xa9},
+	{0x3684, 0xa9},
+	{0x3709, 0x5f},
+	{0x3714, 0x24},
+	{0x371a, 0x3e},
+	{0x3737, 0x04},
+	{0x3738, 0xcc},
+	{0x3739, 0x12},
+	{0x373d, 0x26},
+	{0x3764, 0x20},
+	{0x3765, 0x20},
+	{0x37a1, 0x36},
+	{0x37a8, 0x3b},
+	{0x37ab, 0x31},
+	{0x37c2, 0x04},
+	{0x37c3, 0xf1},
+	{0x37c5, 0x00},
+	{0x37d8, 0x03},
+	{0x37d9, 0x0c},
+	{0x37da, 0xc2},
+	{0x37dc, 0x02},
+	{0x37e0, 0x00},
+	{0x37e1, 0x0a},
+	{0x37e2, 0x14},
+	{0x37e3, 0x04},
+	{0x37e4, 0x2a},
+	{0x37e5, 0x03},
+	{0x37e6, 0x04},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x10},
+	{0x3805, 0x9f},
+	{0x3806, 0x0c},
+	{0x3807, 0x5f},
+	{0x3808, 0x10},
+	{0x3809, 0x80},
+	{0x380a, 0x0c},
+	{0x380b, 0x40},
+	{0x380c, 0x04},
+	{0x380d, 0x62},
+	{0x380e, 0x0c},
+	{0x380f, 0x8e},
+	{0x3811, 0x04},
+	{0x3813, 0x05},
+	{0x3814, 0x01},
+	{0x3815, 0x01},
+	{0x3816, 0x01},
+	{0x3817, 0x01},
+	{0x3820, 0xa8},
+	{0x3821, 0x00},
+	{0x3822, 0xc2},
+	{0x3823, 0x18},
+	{0x3826, 0x11},
+	{0x3827, 0x1c},
+	{0x3829, 0x03},
+	{0x3832, 0x00},
+	{0x3c80, 0x00},
+	{0x3c87, 0x01},
+	{0x3c8c, 0x19},
+	{0x3c8d, 0x1c},
+	{0x3c90, 0x00},
+	{0x3c91, 0x00},
+	{0x3c92, 0x00},
+	{0x3c93, 0x00},
+	{0x3c94, 0x40},
+	{0x3c95, 0x54},
+	{0x3c96, 0x34},
+	{0x3c97, 0x04},
+	{0x3c98, 0x00},
+	{0x3d8c, 0x73},
+	{0x3d8d, 0xc0},
+	{0x3f00, 0x0b},
+	{0x3f03, 0x00},
+	{0x4001, 0xe0},
+	{0x4008, 0x00},
+	{0x4009, 0x0f},
+	{0x4011, 0xf0},
+	{0x4017, 0x08},
+	{0x4050, 0x04},
+	{0x4051, 0x0b},
+	{0x4052, 0x00},
+	{0x4053, 0x80},
+	{0x4054, 0x00},
+	{0x4055, 0x80},
+	{0x4056, 0x00},
+	{0x4057, 0x80},
+	{0x4058, 0x00},
+	{0x4059, 0x80},
+	{0x405e, 0x20},
+	{0x4500, 0x07},
+	{0x4503, 0x00},
+	{0x450a, 0x04},
+	{0x4809, 0x04},
+	{0x480c, 0x12},
+	{0x481f, 0x30},
+	{0x4833, 0x10},
+	{0x4837, 0x0e},
+	{0x4902, 0x01},
+	{0x4d00, 0x03},
+	{0x4d01, 0xc9},
+	{0x4d02, 0xbc},
+	{0x4d03, 0xd7},
+	{0x4d04, 0xf0},
+	{0x4d05, 0xa2},
+	{0x5000, 0xfd},
+	{0x5001, 0x01},
+	{0x5040, 0x39},
+	{0x5041, 0x10},
+	{0x5042, 0x10},
+	{0x5043, 0x84},
+	{0x5044, 0x62},
+	{0x5180, 0x00},
+	{0x5181, 0x10},
+	{0x5182, 0x02},
+	{0x5183, 0x0f},
+	{0x5200, 0x1b},
+	{0x520b, 0x07},
+	{0x520c, 0x0f},
+	{0x5300, 0x04},
+	{0x5301, 0x0c},
+	{0x5302, 0x0c},
+	{0x5303, 0x0f},
+	{0x5304, 0x00},
+	{0x5305, 0x70},
+	{0x5306, 0x00},
+	{0x5307, 0x80},
+	{0x5308, 0x00},
+	{0x5309, 0xa5},
+	{0x530a, 0x00},
+	{0x530b, 0xd3},
+	{0x530c, 0x00},
+	{0x530d, 0xf0},
+	{0x530e, 0x01},
+	{0x530f, 0x10},
+	{0x5310, 0x01},
+	{0x5311, 0x20},
+	{0x5312, 0x01},
+	{0x5313, 0x20},
+	{0x5314, 0x01},
+	{0x5315, 0x20},
+	{0x5316, 0x08},
+	{0x5317, 0x08},
+	{0x5318, 0x10},
+	{0x5319, 0x88},
+	{0x531a, 0x88},
+	{0x531b, 0xa9},
+	{0x531c, 0xaa},
+	{0x531d, 0x0a},
+	{0x5405, 0x02},
+	{0x5406, 0x67},
+	{0x5407, 0x01},
+	{0x5408, 0x4a},
+};
+
+static const struct ov13858_reg mode_2112x1568_regs[] = {
+	{0x3013, 0x32},
+	{0x301b, 0xf0},
+	{0x301f, 0xd0},
+	{0x3106, 0x15},
+	{0x3107, 0x23},
+	{0x350a, 0x00},
+	{0x350e, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3600, 0x2b},
+	{0x3601, 0x52},
+	{0x3602, 0x60},
+	{0x3612, 0x05},
+	{0x3613, 0xa4},
+	{0x3620, 0x80},
+	{0x3621, 0x10},
+	{0x3622, 0x30},
+	{0x3624, 0x1c},
+	{0x3640, 0x10},
+	{0x3641, 0x70},
+	{0x3661, 0x80},
+	{0x3662, 0x10},
+	{0x3664, 0x73},
+	{0x3665, 0xa7},
+	{0x366e, 0xff},
+	{0x366f, 0xf4},
+	{0x3674, 0x00},
+	{0x3679, 0x0c},
+	{0x367f, 0x01},
+	{0x3680, 0x0c},
+	{0x3681, 0x50},
+	{0x3682, 0x50},
+	{0x3683, 0xa9},
+	{0x3684, 0xa9},
+	{0x3709, 0x5f},
+	{0x3714, 0x28},
+	{0x371a, 0x3e},
+	{0x3737, 0x08},
+	{0x3738, 0xcc},
+	{0x3739, 0x20},
+	{0x373d, 0x26},
+	{0x3764, 0x20},
+	{0x3765, 0x20},
+	{0x37a1, 0x36},
+	{0x37a8, 0x3b},
+	{0x37ab, 0x31},
+	{0x37c2, 0x14},
+	{0x37c3, 0xf1},
+	{0x37c5, 0x00},
+	{0x37d8, 0x03},
+	{0x37d9, 0x0c},
+	{0x37da, 0xc2},
+	{0x37dc, 0x02},
+	{0x37e0, 0x00},
+	{0x37e1, 0x0a},
+	{0x37e2, 0x14},
+	{0x37e3, 0x08},
+	{0x37e4, 0x38},
+	{0x37e5, 0x03},
+	{0x37e6, 0x08},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x10},
+	{0x3805, 0x9f},
+	{0x3806, 0x0c},
+	{0x3807, 0x5f},
+	{0x3808, 0x08},
+	{0x3809, 0x40},
+	{0x380a, 0x06},
+	{0x380b, 0x20},
+	{0x380c, 0x04},
+	{0x380d, 0x62},
+	{0x380e, 0x0c},
+	{0x380f, 0x8e},
+	{0x3811, 0x04},
+	{0x3813, 0x05},
+	{0x3814, 0x03},
+	{0x3815, 0x01},
+	{0x3816, 0x03},
+	{0x3817, 0x01},
+	{0x3820, 0xab},
+	{0x3821, 0x00},
+	{0x3822, 0xc2},
+	{0x3823, 0x18},
+	{0x3826, 0x04},
+	{0x3827, 0x90},
+	{0x3829, 0x07},
+	{0x3832, 0x00},
+	{0x3c80, 0x00},
+	{0x3c87, 0x01},
+	{0x3c8c, 0x19},
+	{0x3c8d, 0x1c},
+	{0x3c90, 0x00},
+	{0x3c91, 0x00},
+	{0x3c92, 0x00},
+	{0x3c93, 0x00},
+	{0x3c94, 0x40},
+	{0x3c95, 0x54},
+	{0x3c96, 0x34},
+	{0x3c97, 0x04},
+	{0x3c98, 0x00},
+	{0x3d8c, 0x73},
+	{0x3d8d, 0xc0},
+	{0x3f00, 0x0b},
+	{0x3f03, 0x00},
+	{0x4001, 0xe0},
+	{0x4008, 0x00},
+	{0x4009, 0x0d},
+	{0x4011, 0xf0},
+	{0x4017, 0x08},
+	{0x4050, 0x04},
+	{0x4051, 0x0b},
+	{0x4052, 0x00},
+	{0x4053, 0x80},
+	{0x4054, 0x00},
+	{0x4055, 0x80},
+	{0x4056, 0x00},
+	{0x4057, 0x80},
+	{0x4058, 0x00},
+	{0x4059, 0x80},
+	{0x405e, 0x20},
+	{0x4500, 0x07},
+	{0x4503, 0x00},
+	{0x450a, 0x04},
+	{0x4809, 0x04},
+	{0x480c, 0x12},
+	{0x481f, 0x30},
+	{0x4833, 0x10},
+	{0x4837, 0x1c},
+	{0x4902, 0x01},
+	{0x4d00, 0x03},
+	{0x4d01, 0xc9},
+	{0x4d02, 0xbc},
+	{0x4d03, 0xd7},
+	{0x4d04, 0xf0},
+	{0x4d05, 0xa2},
+	{0x5000, 0xfd},
+	{0x5001, 0x01},
+	{0x5040, 0x39},
+	{0x5041, 0x10},
+	{0x5042, 0x10},
+	{0x5043, 0x84},
+	{0x5044, 0x62},
+	{0x5180, 0x00},
+	{0x5181, 0x10},
+	{0x5182, 0x02},
+	{0x5183, 0x0f},
+	{0x5200, 0x1b},
+	{0x520b, 0x07},
+	{0x520c, 0x0f},
+	{0x5300, 0x04},
+	{0x5301, 0x0c},
+	{0x5302, 0x0c},
+	{0x5303, 0x0f},
+	{0x5304, 0x00},
+	{0x5305, 0x70},
+	{0x5306, 0x00},
+	{0x5307, 0x80},
+	{0x5308, 0x00},
+	{0x5309, 0xa5},
+	{0x530a, 0x00},
+	{0x530b, 0xd3},
+	{0x530c, 0x00},
+	{0x530d, 0xf0},
+	{0x530e, 0x01},
+	{0x530f, 0x10},
+	{0x5310, 0x01},
+	{0x5311, 0x20},
+	{0x5312, 0x01},
+	{0x5313, 0x20},
+	{0x5314, 0x01},
+	{0x5315, 0x20},
+	{0x5316, 0x08},
+	{0x5317, 0x08},
+	{0x5318, 0x10},
+	{0x5319, 0x88},
+	{0x531a, 0x88},
+	{0x531b, 0xa9},
+	{0x531c, 0xaa},
+	{0x531d, 0x0a},
+	{0x5405, 0x02},
+	{0x5406, 0x67},
+	{0x5407, 0x01},
+	{0x5408, 0x4a},
+};
+
+static const struct ov13858_reg mode_2112x1188_regs[] = {
+	{0x3013, 0x32},
+	{0x301b, 0xf0},
+	{0x301f, 0xd0},
+	{0x3106, 0x15},
+	{0x3107, 0x23},
+	{0x350a, 0x00},
+	{0x350e, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3600, 0x2b},
+	{0x3601, 0x52},
+	{0x3602, 0x60},
+	{0x3612, 0x05},
+	{0x3613, 0xa4},
+	{0x3620, 0x80},
+	{0x3621, 0x10},
+	{0x3622, 0x30},
+	{0x3624, 0x1c},
+	{0x3640, 0x10},
+	{0x3641, 0x70},
+	{0x3661, 0x80},
+	{0x3662, 0x10},
+	{0x3664, 0x73},
+	{0x3665, 0xa7},
+	{0x366e, 0xff},
+	{0x366f, 0xf4},
+	{0x3674, 0x00},
+	{0x3679, 0x0c},
+	{0x367f, 0x01},
+	{0x3680, 0x0c},
+	{0x3681, 0x50},
+	{0x3682, 0x50},
+	{0x3683, 0xa9},
+	{0x3684, 0xa9},
+	{0x3709, 0x5f},
+	{0x3714, 0x28},
+	{0x371a, 0x3e},
+	{0x3737, 0x08},
+	{0x3738, 0xcc},
+	{0x3739, 0x20},
+	{0x373d, 0x26},
+	{0x3764, 0x20},
+	{0x3765, 0x20},
+	{0x37a1, 0x36},
+	{0x37a8, 0x3b},
+	{0x37ab, 0x31},
+	{0x37c2, 0x14},
+	{0x37c3, 0xf1},
+	{0x37c5, 0x00},
+	{0x37d8, 0x03},
+	{0x37d9, 0x0c},
+	{0x37da, 0xc2},
+	{0x37dc, 0x02},
+	{0x37e0, 0x00},
+	{0x37e1, 0x0a},
+	{0x37e2, 0x14},
+	{0x37e3, 0x08},
+	{0x37e4, 0x38},
+	{0x37e5, 0x03},
+	{0x37e6, 0x08},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x01},
+	{0x3803, 0x84},
+	{0x3804, 0x10},
+	{0x3805, 0x9f},
+	{0x3806, 0x0a},
+	{0x3807, 0xd3},
+	{0x3808, 0x08},
+	{0x3809, 0x40},
+	{0x380a, 0x04},
+	{0x380b, 0xa4},
+	{0x380c, 0x04},
+	{0x380d, 0x62},
+	{0x380e, 0x0c},
+	{0x380f, 0x8e},
+	{0x3811, 0x08},
+	{0x3813, 0x03},
+	{0x3814, 0x03},
+	{0x3815, 0x01},
+	{0x3816, 0x03},
+	{0x3817, 0x01},
+	{0x3820, 0xab},
+	{0x3821, 0x00},
+	{0x3822, 0xc2},
+	{0x3823, 0x18},
+	{0x3826, 0x04},
+	{0x3827, 0x90},
+	{0x3829, 0x07},
+	{0x3832, 0x00},
+	{0x3c80, 0x00},
+	{0x3c87, 0x01},
+	{0x3c8c, 0x19},
+	{0x3c8d, 0x1c},
+	{0x3c90, 0x00},
+	{0x3c91, 0x00},
+	{0x3c92, 0x00},
+	{0x3c93, 0x00},
+	{0x3c94, 0x40},
+	{0x3c95, 0x54},
+	{0x3c96, 0x34},
+	{0x3c97, 0x04},
+	{0x3c98, 0x00},
+	{0x3d8c, 0x73},
+	{0x3d8d, 0xc0},
+	{0x3f00, 0x0b},
+	{0x3f03, 0x00},
+	{0x4001, 0xe0},
+	{0x4008, 0x00},
+	{0x4009, 0x0d},
+	{0x4011, 0xf0},
+	{0x4017, 0x08},
+	{0x4050, 0x04},
+	{0x4051, 0x0b},
+	{0x4052, 0x00},
+	{0x4053, 0x80},
+	{0x4054, 0x00},
+	{0x4055, 0x80},
+	{0x4056, 0x00},
+	{0x4057, 0x80},
+	{0x4058, 0x00},
+	{0x4059, 0x80},
+	{0x405e, 0x20},
+	{0x4500, 0x07},
+	{0x4503, 0x00},
+	{0x450a, 0x04},
+	{0x4809, 0x04},
+	{0x480c, 0x12},
+	{0x481f, 0x30},
+	{0x4833, 0x10},
+	{0x4837, 0x1c},
+	{0x4902, 0x01},
+	{0x4d00, 0x03},
+	{0x4d01, 0xc9},
+	{0x4d02, 0xbc},
+	{0x4d03, 0xd7},
+	{0x4d04, 0xf0},
+	{0x4d05, 0xa2},
+	{0x5000, 0xfd},
+	{0x5001, 0x01},
+	{0x5040, 0x39},
+	{0x5041, 0x10},
+	{0x5042, 0x10},
+	{0x5043, 0x84},
+	{0x5044, 0x62},
+	{0x5180, 0x00},
+	{0x5181, 0x10},
+	{0x5182, 0x02},
+	{0x5183, 0x0f},
+	{0x5200, 0x1b},
+	{0x520b, 0x07},
+	{0x520c, 0x0f},
+	{0x5300, 0x04},
+	{0x5301, 0x0c},
+	{0x5302, 0x0c},
+	{0x5303, 0x0f},
+	{0x5304, 0x00},
+	{0x5305, 0x70},
+	{0x5306, 0x00},
+	{0x5307, 0x80},
+	{0x5308, 0x00},
+	{0x5309, 0xa5},
+	{0x530a, 0x00},
+	{0x530b, 0xd3},
+	{0x530c, 0x00},
+	{0x530d, 0xf0},
+	{0x530e, 0x01},
+	{0x530f, 0x10},
+	{0x5310, 0x01},
+	{0x5311, 0x20},
+	{0x5312, 0x01},
+	{0x5313, 0x20},
+	{0x5314, 0x01},
+	{0x5315, 0x20},
+	{0x5316, 0x08},
+	{0x5317, 0x08},
+	{0x5318, 0x10},
+	{0x5319, 0x88},
+	{0x531a, 0x88},
+	{0x531b, 0xa9},
+	{0x531c, 0xaa},
+	{0x531d, 0x0a},
+	{0x5405, 0x02},
+	{0x5406, 0x67},
+	{0x5407, 0x01},
+	{0x5408, 0x4a},
+};
+
+static const struct ov13858_reg mode_1056x784_regs[] = {
+	{0x3013, 0x32},
+	{0x301b, 0xf0},
+	{0x301f, 0xd0},
+	{0x3106, 0x15},
+	{0x3107, 0x23},
+	{0x350a, 0x00},
+	{0x350e, 0x00},
+	{0x3510, 0x00},
+	{0x3511, 0x02},
+	{0x3512, 0x00},
+	{0x3600, 0x2b},
+	{0x3601, 0x52},
+	{0x3602, 0x60},
+	{0x3612, 0x05},
+	{0x3613, 0xa4},
+	{0x3620, 0x80},
+	{0x3621, 0x10},
+	{0x3622, 0x30},
+	{0x3624, 0x1c},
+	{0x3640, 0x10},
+	{0x3641, 0x70},
+	{0x3661, 0x80},
+	{0x3662, 0x08},
+	{0x3664, 0x73},
+	{0x3665, 0xa7},
+	{0x366e, 0xff},
+	{0x366f, 0xf4},
+	{0x3674, 0x00},
+	{0x3679, 0x0c},
+	{0x367f, 0x01},
+	{0x3680, 0x0c},
+	{0x3681, 0x50},
+	{0x3682, 0x50},
+	{0x3683, 0xa9},
+	{0x3684, 0xa9},
+	{0x3709, 0x5f},
+	{0x3714, 0x30},
+	{0x371a, 0x3e},
+	{0x3737, 0x08},
+	{0x3738, 0xcc},
+	{0x3739, 0x20},
+	{0x373d, 0x26},
+	{0x3764, 0x20},
+	{0x3765, 0x20},
+	{0x37a1, 0x36},
+	{0x37a8, 0x3b},
+	{0x37ab, 0x31},
+	{0x37c2, 0x2c},
+	{0x37c3, 0xf1},
+	{0x37c5, 0x00},
+	{0x37d8, 0x03},
+	{0x37d9, 0x06},
+	{0x37da, 0xc2},
+	{0x37dc, 0x02},
+	{0x37e0, 0x00},
+	{0x37e1, 0x0a},
+	{0x37e2, 0x14},
+	{0x37e3, 0x08},
+	{0x37e4, 0x36},
+	{0x37e5, 0x03},
+	{0x37e6, 0x08},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x10},
+	{0x3805, 0x9f},
+	{0x3806, 0x0c},
+	{0x3807, 0x5f},
+	{0x3808, 0x04},
+	{0x3809, 0x20},
+	{0x380a, 0x03},
+	{0x380b, 0x10},
+	{0x380c, 0x04},
+	{0x380d, 0x62},
+	{0x380e, 0x0c},
+	{0x380f, 0x8e},
+	{0x3811, 0x04},
+	{0x3813, 0x05},
+	{0x3814, 0x07},
+	{0x3815, 0x01},
+	{0x3816, 0x07},
+	{0x3817, 0x01},
+	{0x3820, 0xac},
+	{0x3821, 0x00},
+	{0x3822, 0xc2},
+	{0x3823, 0x18},
+	{0x3826, 0x04},
+	{0x3827, 0x48},
+	{0x3829, 0x03},
+	{0x3832, 0x00},
+	{0x3c80, 0x00},
+	{0x3c87, 0x01},
+	{0x3c8c, 0x19},
+	{0x3c8d, 0x1c},
+	{0x3c90, 0x00},
+	{0x3c91, 0x00},
+	{0x3c92, 0x00},
+	{0x3c93, 0x00},
+	{0x3c94, 0x40},
+	{0x3c95, 0x54},
+	{0x3c96, 0x34},
+	{0x3c97, 0x04},
+	{0x3c98, 0x00},
+	{0x3d8c, 0x73},
+	{0x3d8d, 0xc0},
+	{0x3f00, 0x0b},
+	{0x3f03, 0x00},
+	{0x4001, 0xe0},
+	{0x4008, 0x00},
+	{0x4009, 0x05},
+	{0x4011, 0xf0},
+	{0x4017, 0x08},
+	{0x4050, 0x02},
+	{0x4051, 0x05},
+	{0x4052, 0x00},
+	{0x4053, 0x80},
+	{0x4054, 0x00},
+	{0x4055, 0x80},
+	{0x4056, 0x00},
+	{0x4057, 0x80},
+	{0x4058, 0x00},
+	{0x4059, 0x80},
+	{0x405e, 0x20},
+	{0x4500, 0x07},
+	{0x4503, 0x00},
+	{0x450a, 0x04},
+	{0x4809, 0x04},
+	{0x480c, 0x12},
+	{0x481f, 0x30},
+	{0x4833, 0x10},
+	{0x4837, 0x1e},
+	{0x4902, 0x02},
+	{0x4d00, 0x03},
+	{0x4d01, 0xc9},
+	{0x4d02, 0xbc},
+	{0x4d03, 0xd7},
+	{0x4d04, 0xf0},
+	{0x4d05, 0xa2},
+	{0x5000, 0xfd},
+	{0x5001, 0x01},
+	{0x5040, 0x39},
+	{0x5041, 0x10},
+	{0x5042, 0x10},
+	{0x5043, 0x84},
+	{0x5044, 0x62},
+	{0x5180, 0x00},
+	{0x5181, 0x10},
+	{0x5182, 0x02},
+	{0x5183, 0x0f},
+	{0x5200, 0x1b},
+	{0x520b, 0x07},
+	{0x520c, 0x0f},
+	{0x5300, 0x04},
+	{0x5301, 0x0c},
+	{0x5302, 0x0c},
+	{0x5303, 0x0f},
+	{0x5304, 0x00},
+	{0x5305, 0x70},
+	{0x5306, 0x00},
+	{0x5307, 0x80},
+	{0x5308, 0x00},
+	{0x5309, 0xa5},
+	{0x530a, 0x00},
+	{0x530b, 0xd3},
+	{0x530c, 0x00},
+	{0x530d, 0xf0},
+	{0x530e, 0x01},
+	{0x530f, 0x10},
+	{0x5310, 0x01},
+	{0x5311, 0x20},
+	{0x5312, 0x01},
+	{0x5313, 0x20},
+	{0x5314, 0x01},
+	{0x5315, 0x20},
+	{0x5316, 0x08},
+	{0x5317, 0x08},
+	{0x5318, 0x10},
+	{0x5319, 0x88},
+	{0x531a, 0x88},
+	{0x531b, 0xa9},
+	{0x531c, 0xaa},
+	{0x531d, 0x0a},
+	{0x5405, 0x02},
+	{0x5406, 0x67},
+	{0x5407, 0x01},
+	{0x5408, 0x4a},
+};
+
+static const char * const ov13858_test_pattern_menu[] = {
+	"Disabled",
+	"Vertical Color Bar Type 1",
+	"Vertical Color Bar Type 2",
+	"Vertical Color Bar Type 3",
+	"Vertical Color Bar Type 4"
+};
+
+/* Configurations for supported link frequencies */
+#define OV13858_NUM_OF_LINK_FREQS	2
+#define OV13858_LINK_FREQ_1080MBPS	1080000000
+#define OV13858_LINK_FREQ_540MBPS	540000000
+#define OV13858_LINK_FREQ_INDEX_0	0
+#define OV13858_LINK_FREQ_INDEX_1	1
+
+/* Menu items for LINK_FREQ V4L2 control */
+static const s64 link_freq_menu_items[OV13858_NUM_OF_LINK_FREQS] = {
+	OV13858_LINK_FREQ_1080MBPS,
+	OV13858_LINK_FREQ_540MBPS
+};
+
+/* Link frequency configs */
+static const struct ov13858_link_freq_config
+			link_freq_configs[OV13858_NUM_OF_LINK_FREQS] = {
+	{
+		.pixel_rate = 864000000,
+		.pixels_per_line = OV13858_PPL_1080MHZ,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_1080mbps),
+			.regs = mipi_data_rate_1080mbps,
+		}
+	},
+	{
+		.pixel_rate = 432000000,
+		.pixels_per_line = OV13858_PPL_540MHZ,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_540mbps),
+			.regs = mipi_data_rate_540mbps,
+		}
+	}
+};
+
+/* Mode configs */
+static const struct ov13858_mode supported_modes[] = {
+	{
+		.width = 4224,
+		.height = 3136,
+		.vts = OV13858_VTS_30FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_4224x3136_regs),
+			.regs = mode_4224x3136_regs,
+		},
+		.link_freq_index = OV13858_LINK_FREQ_INDEX_0,
+	},
+	{
+		.width = 2112,
+		.height = 1568,
+		.vts = OV13858_VTS_30FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_2112x1568_regs),
+			.regs = mode_2112x1568_regs,
+		},
+		.link_freq_index = OV13858_LINK_FREQ_INDEX_1,
+	},
+	{
+		.width = 2112,
+		.height = 1188,
+		.vts = OV13858_VTS_30FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_2112x1188_regs),
+			.regs = mode_2112x1188_regs,
+		},
+		.link_freq_index = OV13858_LINK_FREQ_INDEX_1,
+	},
+	{
+		.width = 1056,
+		.height = 784,
+		.vts = OV13858_VTS_30FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1056x784_regs),
+			.regs = mode_1056x784_regs,
+		},
+		.link_freq_index = OV13858_LINK_FREQ_INDEX_1,
+	}
+};
+
+struct ov13858 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	/* Current mode */
+	const struct ov13858_mode *cur_mode;
+
+	/* Mutex for serialized access */
+	struct mutex mutex;
+
+	/* Streaming on/off */
+	bool streaming;
+};
+
+#define to_ov13858(_sd)	container_of(_sd, struct ov13858, sd)
+
+/* Read registers up to 4 at a time */
+static int ov13858_read_reg(struct ov13858 *ov13858, u16 reg, u32 len, u32 *val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	struct i2c_msg msgs[2];
+	u8 *data_be_p;
+	int ret;
+	u32 data_be = 0;
+	u16 reg_addr_be = cpu_to_be16(reg);
+
+	if (len > 4)
+		return -EINVAL;
+
+	data_be_p = (u8 *)&data_be;
+	/* Write register address */
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = 2;
+	msgs[0].buf = (u8 *)&reg_addr_be;
+
+	/* Read data from register */
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len = len;
+	msgs[1].buf = &data_be_p[4 - len];
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
+
+	*val = be32_to_cpu(data_be);
+
+	return 0;
+}
+
+/* Write registers up to 4 at a time */
+static int ov13858_write_reg(struct ov13858 *ov13858, u16 reg, u32 len, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	int buf_i, val_i;
+	u8 buf[6], *val_p;
+
+	if (len > 4)
+		return -EINVAL;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	val = cpu_to_be32(val);
+	val_p = (u8 *)&val;
+	buf_i = 2;
+	val_i = 4 - len;
+
+	while (val_i < 4)
+		buf[buf_i++] = val_p[val_i++];
+
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+/* Write a list of registers */
+static int ov13858_write_regs(struct ov13858 *ov13858,
+			      const struct ov13858_reg *regs, u32 len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	int ret;
+	u32 i;
+
+	for (i = 0; i < len; i++) {
+		ret = ov13858_write_reg(ov13858, regs[i].address, 1,
+					regs[i].val);
+		if (ret) {
+			dev_err_ratelimited(
+				&client->dev,
+				"Failed to write reg 0x%4.4x. error = %d\n",
+				regs[i].address, ret);
+
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ov13858_write_reg_list(struct ov13858 *ov13858,
+				  const struct ov13858_reg_list *r_list)
+{
+	return ov13858_write_regs(ov13858, r_list->regs, r_list->num_of_regs);
+}
+
+/* Open sub-device */
+static int ov13858_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct ov13858 *ov13858 = to_ov13858(sd);
+	struct v4l2_mbus_framefmt *try_fmt = v4l2_subdev_get_try_format(sd,
+									fh->pad,
+									0);
+
+	mutex_lock(&ov13858->mutex);
+
+	/* Initialize try_fmt */
+	try_fmt->width = ov13858->cur_mode->width;
+	try_fmt->height = ov13858->cur_mode->height;
+	try_fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	try_fmt->field = V4L2_FIELD_NONE;
+
+	/* No crop or compose */
+	mutex_unlock(&ov13858->mutex);
+
+	return 0;
+}
+
+static int ov13858_update_digital_gain(struct ov13858 *ov13858, u32 d_gain)
+{
+	int ret;
+	u32 val;
+
+	if (d_gain == 3)
+		return -EINVAL;
+
+	ret = ov13858_read_reg(ov13858, OV13858_REG_DIGITAL_GAIN,
+			       OV13858_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	val &= OV13858_DGTL_GAIN_MASK;
+	val |= (d_gain - 1) << OV13858_DGTL_GAIN_SHIFT;
+
+	return ov13858_write_reg(ov13858, OV13858_REG_DIGITAL_GAIN,
+				 OV13858_REG_VALUE_08BIT, val);
+}
+
+static int ov13858_enable_test_pattern(struct ov13858 *ov13858, u32 pattern)
+{
+	int ret;
+	u32 val;
+
+	ret = ov13858_read_reg(ov13858, OV13858_REG_TEST_PATTERN,
+			       OV13858_REG_VALUE_08BIT, &val);
+	if (ret)
+		return ret;
+
+	if (pattern) {
+		val &= OV13858_TEST_PATTERN_MASK;
+		val |= (pattern - 1) | OV13858_TEST_PATTERN_ENABLE;
+	} else {
+		val &= ~OV13858_TEST_PATTERN_ENABLE;
+	}
+
+	return ov13858_write_reg(ov13858, OV13858_REG_TEST_PATTERN,
+				 OV13858_REG_VALUE_08BIT, val);
+}
+
+static int ov13858_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov13858 *ov13858 = container_of(ctrl->handler,
+					       struct ov13858, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	s64 max;
+	int ret;
+
+	/* Propagate change of current control to all related controls */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		/* Update max exposure while meeting expected vblanking */
+		max = ov13858->cur_mode->height + ctrl->val - 8;
+		__v4l2_ctrl_modify_range(ov13858->exposure,
+					 ov13858->exposure->minimum,
+					 max, ov13858->exposure->step, max);
+		break;
+	};
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is up for streaming
+	 */
+	if (pm_runtime_get_if_in_use(&client->dev) <= 0)
+		return 0;
+
+	ret = 0;
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = ov13858_write_reg(ov13858, OV13858_REG_ANALOG_GAIN,
+					OV13858_REG_VALUE_16BIT, ctrl->val);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = ov13858_update_digital_gain(ov13858, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		ret = ov13858_write_reg(ov13858, OV13858_REG_EXPOSURE,
+					OV13858_REG_VALUE_24BIT,
+					ctrl->val << 4);
+		break;
+	case V4L2_CID_VBLANK:
+		/* Update VTS that meets expected vertical blanking */
+		ret = ov13858_write_reg(ov13858, OV13858_REG_VTS,
+					OV13858_REG_VALUE_16BIT,
+					ov13858->cur_mode->height
+					  + ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov13858_enable_test_pattern(ov13858, ctrl->val);
+		break;
+	default:
+		dev_info(&client->dev,
+			 "ctrl(id:0x%x,val:0x%x) is not handled\n",
+			 ctrl->id, ctrl->val);
+		break;
+	};
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov13858_ctrl_ops = {
+	.s_ctrl = ov13858_set_ctrl,
+};
+
+static int ov13858_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	/* Only one bayer order(GRBG) is supported */
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int ov13858_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_pad_config *cfg,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static void ov13858_update_pad_format(const struct ov13858_mode *mode,
+				      struct v4l2_subdev_format *fmt)
+{
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->format.field = V4L2_FIELD_NONE;
+}
+
+static int ov13858_do_get_pad_format(struct ov13858 *ov13858,
+				     struct v4l2_subdev_pad_config *cfg,
+				     struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *framefmt;
+	struct v4l2_subdev *sd = &ov13858->sd;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
+		fmt->format = *framefmt;
+	} else {
+		ov13858_update_pad_format(ov13858->cur_mode, fmt);
+	}
+
+	return 0;
+}
+
+static int ov13858_get_pad_format(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_format *fmt)
+{
+	struct ov13858 *ov13858 = to_ov13858(sd);
+	int ret;
+
+	mutex_lock(&ov13858->mutex);
+	ret = ov13858_do_get_pad_format(ov13858, cfg, fmt);
+	mutex_unlock(&ov13858->mutex);
+
+	return ret;
+}
+
+/*
+ * Calculate resolution distance
+ */
+static int
+ov13858_get_resolution_dist(const struct ov13858_mode *mode,
+			    struct v4l2_mbus_framefmt *framefmt)
+{
+	return abs(mode->width - framefmt->width) +
+	       abs(mode->height - framefmt->height);
+}
+
+/*
+ * Find the closest supported resolution to the requested resolution
+ */
+static const struct ov13858_mode *
+ov13858_find_best_fit(struct ov13858 *ov13858,
+		      struct v4l2_subdev_format *fmt)
+{
+	int i, dist, cur_best_fit = 0, cur_best_fit_dist = -1;
+	struct v4l2_mbus_framefmt *framefmt = &fmt->format;
+
+	for (i = 0; i < ARRAY_SIZE(supported_modes); i++) {
+		dist = ov13858_get_resolution_dist(&supported_modes[i],
+						   framefmt);
+		if (cur_best_fit_dist == -1 || dist < cur_best_fit_dist) {
+			cur_best_fit_dist = dist;
+			cur_best_fit = i;
+		}
+	}
+
+	return &supported_modes[cur_best_fit];
+}
+
+static int
+ov13858_set_pad_format(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_format *fmt)
+{
+	struct ov13858 *ov13858 = to_ov13858(sd);
+	const struct ov13858_mode *mode;
+	struct v4l2_mbus_framefmt *framefmt;
+	s64 h_blank;
+
+	mutex_lock(&ov13858->mutex);
+
+	/* Only one raw bayer(GRBG) order is supported */
+	if (fmt->format.code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	mode = ov13858_find_best_fit(ov13858, fmt);
+	ov13858_update_pad_format(mode, fmt);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		framefmt = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
+		*framefmt = fmt->format;
+	} else {
+		ov13858->cur_mode = mode;
+		__v4l2_ctrl_s_ctrl(ov13858->link_freq, mode->link_freq_index);
+		__v4l2_ctrl_s_ctrl_int64(
+			ov13858->pixel_rate,
+			link_freq_configs[mode->link_freq_index].pixel_rate);
+		/* Update limits and set FPS to default */
+		__v4l2_ctrl_modify_range(
+			ov13858->vblank, OV13858_VBLANK_MIN,
+			OV13858_VTS_MAX - ov13858->cur_mode->height, 1,
+			ov13858->cur_mode->vts - ov13858->cur_mode->height);
+		h_blank =
+			link_freq_configs[mode->link_freq_index].pixels_per_line
+			 - ov13858->cur_mode->width;
+		__v4l2_ctrl_modify_range(ov13858->hblank, h_blank,
+					 h_blank, 1, h_blank);
+	}
+
+	mutex_unlock(&ov13858->mutex);
+
+	return 0;
+}
+
+static int ov13858_get_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+	*frames = OV13858_NUM_OF_SKIP_FRAMES;
+
+	return 0;
+}
+
+/* Start streaming */
+static int ov13858_start_streaming(struct ov13858 *ov13858)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	const struct ov13858_reg_list *reg_list;
+	int ret, link_freq_index;
+
+	/* Get out of from software reset */
+	ret = ov13858_write_reg(ov13858, OV13858_REG_SOFTWARE_RST,
+				OV13858_REG_VALUE_08BIT, OV13858_SOFTWARE_RST);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set powerup registers\n",
+			__func__);
+		return ret;
+	}
+
+	/* Setup PLL */
+	link_freq_index = ov13858->cur_mode->link_freq_index;
+	reg_list = &link_freq_configs[link_freq_index].reg_list;
+	ret = ov13858_write_reg_list(ov13858, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set plls\n", __func__);
+		return ret;
+	}
+
+	/* Apply default values of current mode */
+	reg_list = &ov13858->cur_mode->reg_list;
+	ret = ov13858_write_reg_list(ov13858, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set mode\n", __func__);
+		return ret;
+	}
+
+	/* Apply customized values from user */
+	ret =  __v4l2_ctrl_handler_setup(ov13858->sd.ctrl_handler);
+	if (ret)
+		return ret;
+
+	return ov13858_write_reg(ov13858, OV13858_REG_MODE_SELECT,
+				 OV13858_REG_VALUE_08BIT,
+				 OV13858_MODE_STREAMING);
+}
+
+/* Stop streaming */
+static int ov13858_stop_streaming(struct ov13858 *ov13858)
+{
+	return ov13858_write_reg(ov13858, OV13858_REG_MODE_SELECT,
+				 OV13858_REG_VALUE_08BIT, OV13858_MODE_STANDBY);
+}
+
+static int ov13858_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov13858 *ov13858 = to_ov13858(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&ov13858->mutex);
+	if (ov13858->streaming == enable) {
+		mutex_unlock(&ov13858->mutex);
+		return 0;
+	}
+
+	if (enable) {
+		ret = pm_runtime_get_sync(&client->dev);
+		if (ret < 0) {
+			pm_runtime_put_noidle(&client->dev);
+			goto err_unlock;
+		}
+
+		/*
+		 * Apply default & customized values
+		 * and then start streaming.
+		 */
+		ret = ov13858_start_streaming(ov13858);
+		if (ret)
+			goto err_rpm_put;
+	} else {
+		ov13858_stop_streaming(ov13858);
+		pm_runtime_put(&client->dev);
+	}
+
+	ov13858->streaming = enable;
+	mutex_unlock(&ov13858->mutex);
+
+	return ret;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+err_unlock:
+	mutex_unlock(&ov13858->mutex);
+
+	return ret;
+}
+
+static int __maybe_unused ov13858_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov13858 *ov13858 = to_ov13858(sd);
+
+	if (ov13858->streaming)
+		ov13858_stop_streaming(ov13858);
+
+	return 0;
+}
+
+static int __maybe_unused ov13858_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov13858 *ov13858 = to_ov13858(sd);
+	int ret;
+
+	if (ov13858->streaming) {
+		ret = ov13858_start_streaming(ov13858);
+		if (ret)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	ov13858_stop_streaming(ov13858);
+	ov13858->streaming = 0;
+	return ret;
+}
+
+/* Verify chip ID */
+static int ov13858_identify_module(struct ov13858 *ov13858)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	int ret;
+	u32 val;
+
+	ret = ov13858_read_reg(ov13858, OV13858_REG_CHIP_ID,
+			       OV13858_REG_VALUE_24BIT, &val);
+	if (ret)
+		return ret;
+
+	if (val != OV13858_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+			OV13858_CHIP_ID, val);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov13858_video_ops = {
+	.s_stream = ov13858_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov13858_pad_ops = {
+	.enum_mbus_code = ov13858_enum_mbus_code,
+	.get_fmt = ov13858_get_pad_format,
+	.set_fmt = ov13858_set_pad_format,
+	.enum_frame_size = ov13858_enum_frame_size,
+};
+
+static const struct v4l2_subdev_sensor_ops ov13858_sensor_ops = {
+	.g_skip_frames = ov13858_get_skip_frames,
+};
+
+static const struct v4l2_subdev_ops ov13858_subdev_ops = {
+	.video = &ov13858_video_ops,
+	.pad = &ov13858_pad_ops,
+	.sensor = &ov13858_sensor_ops,
+};
+
+static const struct media_entity_operations ov13858_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops ov13858_internal_ops = {
+	.open = ov13858_open,
+};
+
+/* Initialize control handlers */
+static int ov13858_init_controls(struct ov13858 *ov13858)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov13858->sd);
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	int ret;
+
+	ctrl_hdlr = &ov13858->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 8);
+	if (ret)
+		return ret;
+
+	mutex_init(&ov13858->mutex);
+	ctrl_hdlr->lock = &ov13858->mutex;
+	ov13858->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+				&ov13858_ctrl_ops,
+				V4L2_CID_LINK_FREQ,
+				OV13858_NUM_OF_LINK_FREQS - 1,
+				0,
+				link_freq_menu_items);
+	ov13858->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* By default, PIXEL_RATE is read only */
+	ov13858->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov13858_ctrl_ops,
+					V4L2_CID_PIXEL_RATE, 0,
+					link_freq_configs[0].pixel_rate, 1,
+					link_freq_configs[0].pixel_rate);
+
+	ov13858->vblank = v4l2_ctrl_new_std(
+				ctrl_hdlr, &ov13858_ctrl_ops, V4L2_CID_VBLANK,
+				OV13858_VBLANK_MIN,
+				OV13858_VTS_MAX - ov13858->cur_mode->height, 1,
+				ov13858->cur_mode->vts
+				  - ov13858->cur_mode->height);
+
+	ov13858->hblank = v4l2_ctrl_new_std(
+				ctrl_hdlr, &ov13858_ctrl_ops, V4L2_CID_HBLANK,
+				OV13858_PPL_1080MHZ - ov13858->cur_mode->width,
+				OV13858_PPL_1080MHZ - ov13858->cur_mode->width,
+				1,
+				OV13858_PPL_1080MHZ - ov13858->cur_mode->width);
+	ov13858->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	ov13858->exposure = v4l2_ctrl_new_std(
+				ctrl_hdlr, &ov13858_ctrl_ops,
+				V4L2_CID_EXPOSURE, OV13858_EXPOSURE_MIN,
+				OV13858_EXPOSURE_MAX, OV13858_EXPOSURE_STEP,
+				OV13858_EXPOSURE_DEFAULT);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov13858_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OV13858_ANA_GAIN_MIN, OV13858_ANA_GAIN_MAX,
+			  OV13858_ANA_GAIN_STEP, OV13858_ANA_GAIN_DEFAULT);
+
+	/* Digital gain */
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov13858_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV13858_DGTL_GAIN_MIN, OV13858_DGTL_GAIN_MAX,
+			  OV13858_DGTL_GAIN_STEP, OV13858_DGTL_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &ov13858_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov13858_test_pattern_menu) - 1,
+				     0, 0, ov13858_test_pattern_menu);
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		dev_err(&client->dev, "%s control init failed (%d)\n",
+			__func__, ret);
+		goto error;
+	}
+
+	ov13858->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+	mutex_destroy(&ov13858->mutex);
+
+	return ret;
+}
+
+static void ov13858_free_controls(struct ov13858 *ov13858)
+{
+	v4l2_ctrl_handler_free(ov13858->sd.ctrl_handler);
+	mutex_destroy(&ov13858->mutex);
+}
+
+static int ov13858_probe(struct i2c_client *client,
+			 const struct i2c_device_id *devid)
+{
+	struct ov13858 *ov13858;
+	int ret;
+	u32 val = 0;
+
+	device_property_read_u32(&client->dev, "clock-frequency", &val);
+	if (val != 19200000)
+		return -EINVAL;
+
+	ov13858 = devm_kzalloc(&client->dev, sizeof(*ov13858), GFP_KERNEL);
+	if (!ov13858)
+		return -ENOMEM;
+
+	/* Initialize subdev */
+	v4l2_i2c_subdev_init(&ov13858->sd, client, &ov13858_subdev_ops);
+
+	/* Check module identity */
+	ret = ov13858_identify_module(ov13858);
+	if (ret) {
+		dev_err(&client->dev, "failed to find sensor: %d\n", ret);
+		return ret;
+	}
+
+	/* Set default mode to max resolution */
+	ov13858->cur_mode = &supported_modes[0];
+
+	ret = ov13858_init_controls(ov13858);
+	if (ret)
+		return ret;
+
+	/* Initialize subdev */
+	ov13858->sd.internal_ops = &ov13858_internal_ops;
+	ov13858->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov13858->sd.entity.ops = &ov13858_subdev_entity_ops;
+	ov13858->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	ov13858->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&ov13858->sd.entity, 1, &ov13858->pad);
+	if (ret) {
+		dev_err(&client->dev, "%s failed:%d\n", __func__, ret);
+		goto error_handler_free;
+	}
+
+	ret = v4l2_async_register_subdev(&ov13858->sd);
+	if (ret < 0)
+		goto error_media_entity;
+
+	/*
+	 * Device is already turned on by i2c-core with ACPI domain PM.
+	 * Enable runtime PM and turn off the device.
+	 */
+	pm_runtime_get_noresume(&client->dev);
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+	pm_runtime_put(&client->dev);
+
+	return 0;
+
+error_media_entity:
+	media_entity_cleanup(&ov13858->sd.entity);
+
+error_handler_free:
+	ov13858_free_controls(ov13858);
+	dev_err(&client->dev, "%s failed:%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int ov13858_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov13858 *ov13858 = to_ov13858(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	ov13858_free_controls(ov13858);
+
+	/*
+	 * Disable runtime PM but keep the device turned on.
+	 * i2c-core with ACPI domain PM will turn off the device.
+	 */
+	pm_runtime_get_sync(&client->dev);
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+
+	return 0;
+}
+
+static const struct i2c_device_id ov13858_id_table[] = {
+	{"ov13858", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, ov13858_id_table);
+
+static const struct dev_pm_ops ov13858_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(ov13858_suspend, ov13858_resume)
+};
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id ov13858_acpi_ids[] = {
+	{"OVTID858"},
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(acpi, ov13858_acpi_ids);
+#endif
+
+static struct i2c_driver ov13858_i2c_driver = {
+	.driver = {
+		.name = "ov13858",
+		.owner = THIS_MODULE,
+		.pm = &ov13858_pm_ops,
+		.acpi_match_table = ACPI_PTR(ov13858_acpi_ids),
+	},
+	.probe = ov13858_probe,
+	.remove = ov13858_remove,
+	.id_table = ov13858_id_table,
+};
+
+module_i2c_driver(ov13858_i2c_driver);
+
+MODULE_AUTHOR("Kan, Chris <chris.kan@intel.com>");
+MODULE_AUTHOR("Rapolu, Chiranjeevi <chiranjeevi.rapolu@intel.com>");
+MODULE_AUTHOR("Yang, Hyungwoo <hyungwoo.yang@intel.com>");
+MODULE_DESCRIPTION("Omnivision ov13858 sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/ov2659.c b/drivers/media/i2c/ov2659.c
index 6e6367214d40..122dd6c5eb38 100644
--- a/drivers/media/i2c/ov2659.c
+++ b/drivers/media/i2c/ov2659.c
@@ -42,9 +42,9 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
 #include <media/v4l2-image-sizes.h>
 #include <media/v4l2-mediabus.h>
-#include <media/v4l2-of.h>
 #include <media/v4l2-subdev.h>
 
 #define DRIVER_NAME "ov2659"
@@ -1308,7 +1308,8 @@ static const struct v4l2_subdev_internal_ops ov2659_subdev_internal_ops = {
 static int ov2659_detect(struct v4l2_subdev *sd)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	u8 pid, ver;
+	u8 pid = 0;
+	u8 ver = 0;
 	int ret;
 
 	dev_dbg(&client->dev, "%s:\n", __func__);
@@ -1346,7 +1347,7 @@ static struct ov2659_platform_data *
 ov2659_get_pdata(struct i2c_client *client)
 {
 	struct ov2659_platform_data *pdata;
-	struct v4l2_of_endpoint *bus_cfg;
+	struct v4l2_fwnode_endpoint *bus_cfg;
 	struct device_node *endpoint;
 
 	if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
@@ -1356,7 +1357,7 @@ ov2659_get_pdata(struct i2c_client *client)
 	if (!endpoint)
 		return NULL;
 
-	bus_cfg = v4l2_of_alloc_parse_endpoint(endpoint);
+	bus_cfg = v4l2_fwnode_endpoint_alloc_parse(of_fwnode_handle(endpoint));
 	if (IS_ERR(bus_cfg)) {
 		pdata = NULL;
 		goto done;
@@ -1376,7 +1377,7 @@ ov2659_get_pdata(struct i2c_client *client)
 	pdata->link_frequency = bus_cfg->link_frequencies[0];
 
 done:
-	v4l2_of_free_endpoint(bus_cfg);
+	v4l2_fwnode_endpoint_free(bus_cfg);
 	of_node_put(endpoint);
 	return pdata;
 }
diff --git a/drivers/media/i2c/ov5640.c b/drivers/media/i2c/ov5640.c
new file mode 100644
index 000000000000..1f5b483cf334
--- /dev/null
+++ b/drivers/media/i2c/ov5640.c
@@ -0,0 +1,2344 @@
+/*
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2014-2017 Mentor Graphics Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/gpio/consumer.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+/* min/typical/max system clock (xclk) frequencies */
+#define OV5640_XCLK_MIN  6000000
+#define OV5640_XCLK_MAX 24000000
+
+#define OV5640_DEFAULT_SLAVE_ID 0x3c
+
+#define OV5640_REG_CHIP_ID		0x300a
+#define OV5640_REG_PAD_OUTPUT00		0x3019
+#define OV5640_REG_SC_PLL_CTRL0		0x3034
+#define OV5640_REG_SC_PLL_CTRL1		0x3035
+#define OV5640_REG_SC_PLL_CTRL2		0x3036
+#define OV5640_REG_SC_PLL_CTRL3		0x3037
+#define OV5640_REG_SLAVE_ID		0x3100
+#define OV5640_REG_SYS_ROOT_DIVIDER	0x3108
+#define OV5640_REG_AWB_R_GAIN		0x3400
+#define OV5640_REG_AWB_G_GAIN		0x3402
+#define OV5640_REG_AWB_B_GAIN		0x3404
+#define OV5640_REG_AWB_MANUAL_CTRL	0x3406
+#define OV5640_REG_AEC_PK_EXPOSURE_HI	0x3500
+#define OV5640_REG_AEC_PK_EXPOSURE_MED	0x3501
+#define OV5640_REG_AEC_PK_EXPOSURE_LO	0x3502
+#define OV5640_REG_AEC_PK_MANUAL	0x3503
+#define OV5640_REG_AEC_PK_REAL_GAIN	0x350a
+#define OV5640_REG_AEC_PK_VTS		0x350c
+#define OV5640_REG_TIMING_HTS		0x380c
+#define OV5640_REG_TIMING_VTS		0x380e
+#define OV5640_REG_TIMING_TC_REG21	0x3821
+#define OV5640_REG_AEC_CTRL00		0x3a00
+#define OV5640_REG_AEC_B50_STEP		0x3a08
+#define OV5640_REG_AEC_B60_STEP		0x3a0a
+#define OV5640_REG_AEC_CTRL0D		0x3a0d
+#define OV5640_REG_AEC_CTRL0E		0x3a0e
+#define OV5640_REG_AEC_CTRL0F		0x3a0f
+#define OV5640_REG_AEC_CTRL10		0x3a10
+#define OV5640_REG_AEC_CTRL11		0x3a11
+#define OV5640_REG_AEC_CTRL1B		0x3a1b
+#define OV5640_REG_AEC_CTRL1E		0x3a1e
+#define OV5640_REG_AEC_CTRL1F		0x3a1f
+#define OV5640_REG_HZ5060_CTRL00	0x3c00
+#define OV5640_REG_HZ5060_CTRL01	0x3c01
+#define OV5640_REG_SIGMADELTA_CTRL0C	0x3c0c
+#define OV5640_REG_FRAME_CTRL01		0x4202
+#define OV5640_REG_MIPI_CTRL00		0x4800
+#define OV5640_REG_DEBUG_MODE		0x4814
+#define OV5640_REG_PRE_ISP_TEST_SET1	0x503d
+#define OV5640_REG_SDE_CTRL0		0x5580
+#define OV5640_REG_SDE_CTRL1		0x5581
+#define OV5640_REG_SDE_CTRL3		0x5583
+#define OV5640_REG_SDE_CTRL4		0x5584
+#define OV5640_REG_SDE_CTRL5		0x5585
+#define OV5640_REG_AVG_READOUT		0x56a1
+
+enum ov5640_mode_id {
+	OV5640_MODE_QCIF_176_144 = 0,
+	OV5640_MODE_QVGA_320_240,
+	OV5640_MODE_VGA_640_480,
+	OV5640_MODE_NTSC_720_480,
+	OV5640_MODE_PAL_720_576,
+	OV5640_MODE_XGA_1024_768,
+	OV5640_MODE_720P_1280_720,
+	OV5640_MODE_1080P_1920_1080,
+	OV5640_MODE_QSXGA_2592_1944,
+	OV5640_NUM_MODES,
+};
+
+enum ov5640_frame_rate {
+	OV5640_15_FPS = 0,
+	OV5640_30_FPS,
+	OV5640_NUM_FRAMERATES,
+};
+
+/*
+ * FIXME: remove this when a subdev API becomes available
+ * to set the MIPI CSI-2 virtual channel.
+ */
+static unsigned int virtual_channel;
+module_param(virtual_channel, int, 0);
+MODULE_PARM_DESC(virtual_channel,
+		 "MIPI CSI-2 virtual channel (0..3), default 0");
+
+static const int ov5640_framerates[] = {
+	[OV5640_15_FPS] = 15,
+	[OV5640_30_FPS] = 30,
+};
+
+/* regulator supplies */
+static const char * const ov5640_supply_name[] = {
+	"DOVDD", /* Digital I/O (1.8V) suppply */
+	"DVDD",  /* Digital Core (1.5V) supply */
+	"AVDD",  /* Analog (2.8V) supply */
+};
+
+#define OV5640_NUM_SUPPLIES ARRAY_SIZE(ov5640_supply_name)
+
+/*
+ * Image size under 1280 * 960 are SUBSAMPLING
+ * Image size upper 1280 * 960 are SCALING
+ */
+enum ov5640_downsize_mode {
+	SUBSAMPLING,
+	SCALING,
+};
+
+struct reg_value {
+	u16 reg_addr;
+	u8 val;
+	u8 mask;
+	u32 delay_ms;
+};
+
+struct ov5640_mode_info {
+	enum ov5640_mode_id id;
+	enum ov5640_downsize_mode dn_mode;
+	u32 width;
+	u32 height;
+	const struct reg_value *reg_data;
+	u32 reg_data_size;
+};
+
+struct ov5640_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct {
+		struct v4l2_ctrl *auto_exp;
+		struct v4l2_ctrl *exposure;
+	};
+	struct {
+		struct v4l2_ctrl *auto_wb;
+		struct v4l2_ctrl *blue_balance;
+		struct v4l2_ctrl *red_balance;
+	};
+	struct {
+		struct v4l2_ctrl *auto_gain;
+		struct v4l2_ctrl *gain;
+	};
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *saturation;
+	struct v4l2_ctrl *contrast;
+	struct v4l2_ctrl *hue;
+	struct v4l2_ctrl *test_pattern;
+};
+
+struct ov5640_dev {
+	struct i2c_client *i2c_client;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
+	struct clk *xclk; /* system clock to OV5640 */
+	u32 xclk_freq;
+
+	struct regulator_bulk_data supplies[OV5640_NUM_SUPPLIES];
+	struct gpio_desc *reset_gpio;
+	struct gpio_desc *pwdn_gpio;
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	int power_count;
+
+	struct v4l2_mbus_framefmt fmt;
+
+	const struct ov5640_mode_info *current_mode;
+	enum ov5640_frame_rate current_fr;
+	struct v4l2_fract frame_interval;
+
+	struct ov5640_ctrls ctrls;
+
+	u32 prev_sysclk, prev_hts;
+	u32 ae_low, ae_high, ae_target;
+
+	bool pending_mode_change;
+	bool streaming;
+};
+
+static inline struct ov5640_dev *to_ov5640_dev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov5640_dev, sd);
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct ov5640_dev,
+			     ctrls.handler)->sd;
+}
+
+/*
+ * FIXME: all of these register tables are likely filled with
+ * entries that set the register to their power-on default values,
+ * and which are otherwise not touched by this driver. Those entries
+ * should be identified and removed to speed register load time
+ * over i2c.
+ */
+
+static const struct reg_value ov5640_init_setting_30fps_VGA[] = {
+
+	{0x3103, 0x11, 0, 0}, {0x3008, 0x82, 0, 5}, {0x3008, 0x42, 0, 0},
+	{0x3103, 0x03, 0, 0}, {0x3017, 0x00, 0, 0}, {0x3018, 0x00, 0, 0},
+	{0x3034, 0x18, 0, 0}, {0x3035, 0x14, 0, 0}, {0x3036, 0x38, 0, 0},
+	{0x3037, 0x13, 0, 0}, {0x3108, 0x01, 0, 0}, {0x3630, 0x36, 0, 0},
+	{0x3631, 0x0e, 0, 0}, {0x3632, 0xe2, 0, 0}, {0x3633, 0x12, 0, 0},
+	{0x3621, 0xe0, 0, 0}, {0x3704, 0xa0, 0, 0}, {0x3703, 0x5a, 0, 0},
+	{0x3715, 0x78, 0, 0}, {0x3717, 0x01, 0, 0}, {0x370b, 0x60, 0, 0},
+	{0x3705, 0x1a, 0, 0}, {0x3905, 0x02, 0, 0}, {0x3906, 0x10, 0, 0},
+	{0x3901, 0x0a, 0, 0}, {0x3731, 0x12, 0, 0}, {0x3600, 0x08, 0, 0},
+	{0x3601, 0x33, 0, 0}, {0x302d, 0x60, 0, 0}, {0x3620, 0x52, 0, 0},
+	{0x371b, 0x20, 0, 0}, {0x471c, 0x50, 0, 0}, {0x3a13, 0x43, 0, 0},
+	{0x3a18, 0x00, 0, 0}, {0x3a19, 0xf8, 0, 0}, {0x3635, 0x13, 0, 0},
+	{0x3636, 0x03, 0, 0}, {0x3634, 0x40, 0, 0}, {0x3622, 0x01, 0, 0},
+	{0x3c01, 0xa4, 0, 0}, {0x3c04, 0x28, 0, 0}, {0x3c05, 0x98, 0, 0},
+	{0x3c06, 0x00, 0, 0}, {0x3c07, 0x08, 0, 0}, {0x3c08, 0x00, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x02, 0, 0}, {0x3809, 0x80, 0, 0}, {0x380a, 0x01, 0, 0},
+	{0x380b, 0xe0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x03, 0, 0}, {0x380f, 0xd8, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x3000, 0x00, 0, 0},
+	{0x3002, 0x1c, 0, 0}, {0x3004, 0xff, 0, 0}, {0x3006, 0xc3, 0, 0},
+	{0x300e, 0x45, 0, 0}, {0x302e, 0x08, 0, 0}, {0x4300, 0x3f, 0, 0},
+	{0x501f, 0x00, 0, 0}, {0x4713, 0x03, 0, 0}, {0x4407, 0x04, 0, 0},
+	{0x440e, 0x00, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x4837, 0x0a, 0, 0}, {0x4800, 0x04, 0, 0}, {0x3824, 0x02, 0, 0},
+	{0x5000, 0xa7, 0, 0}, {0x5001, 0xa3, 0, 0}, {0x5180, 0xff, 0, 0},
+	{0x5181, 0xf2, 0, 0}, {0x5182, 0x00, 0, 0}, {0x5183, 0x14, 0, 0},
+	{0x5184, 0x25, 0, 0}, {0x5185, 0x24, 0, 0}, {0x5186, 0x09, 0, 0},
+	{0x5187, 0x09, 0, 0}, {0x5188, 0x09, 0, 0}, {0x5189, 0x88, 0, 0},
+	{0x518a, 0x54, 0, 0}, {0x518b, 0xee, 0, 0}, {0x518c, 0xb2, 0, 0},
+	{0x518d, 0x50, 0, 0}, {0x518e, 0x34, 0, 0}, {0x518f, 0x6b, 0, 0},
+	{0x5190, 0x46, 0, 0}, {0x5191, 0xf8, 0, 0}, {0x5192, 0x04, 0, 0},
+	{0x5193, 0x70, 0, 0}, {0x5194, 0xf0, 0, 0}, {0x5195, 0xf0, 0, 0},
+	{0x5196, 0x03, 0, 0}, {0x5197, 0x01, 0, 0}, {0x5198, 0x04, 0, 0},
+	{0x5199, 0x6c, 0, 0}, {0x519a, 0x04, 0, 0}, {0x519b, 0x00, 0, 0},
+	{0x519c, 0x09, 0, 0}, {0x519d, 0x2b, 0, 0}, {0x519e, 0x38, 0, 0},
+	{0x5381, 0x1e, 0, 0}, {0x5382, 0x5b, 0, 0}, {0x5383, 0x08, 0, 0},
+	{0x5384, 0x0a, 0, 0}, {0x5385, 0x7e, 0, 0}, {0x5386, 0x88, 0, 0},
+	{0x5387, 0x7c, 0, 0}, {0x5388, 0x6c, 0, 0}, {0x5389, 0x10, 0, 0},
+	{0x538a, 0x01, 0, 0}, {0x538b, 0x98, 0, 0}, {0x5300, 0x08, 0, 0},
+	{0x5301, 0x30, 0, 0}, {0x5302, 0x10, 0, 0}, {0x5303, 0x00, 0, 0},
+	{0x5304, 0x08, 0, 0}, {0x5305, 0x30, 0, 0}, {0x5306, 0x08, 0, 0},
+	{0x5307, 0x16, 0, 0}, {0x5309, 0x08, 0, 0}, {0x530a, 0x30, 0, 0},
+	{0x530b, 0x04, 0, 0}, {0x530c, 0x06, 0, 0}, {0x5480, 0x01, 0, 0},
+	{0x5481, 0x08, 0, 0}, {0x5482, 0x14, 0, 0}, {0x5483, 0x28, 0, 0},
+	{0x5484, 0x51, 0, 0}, {0x5485, 0x65, 0, 0}, {0x5486, 0x71, 0, 0},
+	{0x5487, 0x7d, 0, 0}, {0x5488, 0x87, 0, 0}, {0x5489, 0x91, 0, 0},
+	{0x548a, 0x9a, 0, 0}, {0x548b, 0xaa, 0, 0}, {0x548c, 0xb8, 0, 0},
+	{0x548d, 0xcd, 0, 0}, {0x548e, 0xdd, 0, 0}, {0x548f, 0xea, 0, 0},
+	{0x5490, 0x1d, 0, 0}, {0x5580, 0x02, 0, 0}, {0x5583, 0x40, 0, 0},
+	{0x5584, 0x10, 0, 0}, {0x5589, 0x10, 0, 0}, {0x558a, 0x00, 0, 0},
+	{0x558b, 0xf8, 0, 0}, {0x5800, 0x23, 0, 0}, {0x5801, 0x14, 0, 0},
+	{0x5802, 0x0f, 0, 0}, {0x5803, 0x0f, 0, 0}, {0x5804, 0x12, 0, 0},
+	{0x5805, 0x26, 0, 0}, {0x5806, 0x0c, 0, 0}, {0x5807, 0x08, 0, 0},
+	{0x5808, 0x05, 0, 0}, {0x5809, 0x05, 0, 0}, {0x580a, 0x08, 0, 0},
+	{0x580b, 0x0d, 0, 0}, {0x580c, 0x08, 0, 0}, {0x580d, 0x03, 0, 0},
+	{0x580e, 0x00, 0, 0}, {0x580f, 0x00, 0, 0}, {0x5810, 0x03, 0, 0},
+	{0x5811, 0x09, 0, 0}, {0x5812, 0x07, 0, 0}, {0x5813, 0x03, 0, 0},
+	{0x5814, 0x00, 0, 0}, {0x5815, 0x01, 0, 0}, {0x5816, 0x03, 0, 0},
+	{0x5817, 0x08, 0, 0}, {0x5818, 0x0d, 0, 0}, {0x5819, 0x08, 0, 0},
+	{0x581a, 0x05, 0, 0}, {0x581b, 0x06, 0, 0}, {0x581c, 0x08, 0, 0},
+	{0x581d, 0x0e, 0, 0}, {0x581e, 0x29, 0, 0}, {0x581f, 0x17, 0, 0},
+	{0x5820, 0x11, 0, 0}, {0x5821, 0x11, 0, 0}, {0x5822, 0x15, 0, 0},
+	{0x5823, 0x28, 0, 0}, {0x5824, 0x46, 0, 0}, {0x5825, 0x26, 0, 0},
+	{0x5826, 0x08, 0, 0}, {0x5827, 0x26, 0, 0}, {0x5828, 0x64, 0, 0},
+	{0x5829, 0x26, 0, 0}, {0x582a, 0x24, 0, 0}, {0x582b, 0x22, 0, 0},
+	{0x582c, 0x24, 0, 0}, {0x582d, 0x24, 0, 0}, {0x582e, 0x06, 0, 0},
+	{0x582f, 0x22, 0, 0}, {0x5830, 0x40, 0, 0}, {0x5831, 0x42, 0, 0},
+	{0x5832, 0x24, 0, 0}, {0x5833, 0x26, 0, 0}, {0x5834, 0x24, 0, 0},
+	{0x5835, 0x22, 0, 0}, {0x5836, 0x22, 0, 0}, {0x5837, 0x26, 0, 0},
+	{0x5838, 0x44, 0, 0}, {0x5839, 0x24, 0, 0}, {0x583a, 0x26, 0, 0},
+	{0x583b, 0x28, 0, 0}, {0x583c, 0x42, 0, 0}, {0x583d, 0xce, 0, 0},
+	{0x5025, 0x00, 0, 0}, {0x3a0f, 0x30, 0, 0}, {0x3a10, 0x28, 0, 0},
+	{0x3a1b, 0x30, 0, 0}, {0x3a1e, 0x26, 0, 0}, {0x3a11, 0x60, 0, 0},
+	{0x3a1f, 0x14, 0, 0}, {0x3008, 0x02, 0, 0}, {0x3c00, 0x04, 0, 300},
+};
+
+static const struct reg_value ov5640_setting_30fps_VGA_640_480[] = {
+
+	{0x3035, 0x14, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x02, 0, 0}, {0x3809, 0x80, 0, 0}, {0x380a, 0x01, 0, 0},
+	{0x380b, 0xe0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x04, 0, 0}, {0x380f, 0x38, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x0e, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0}, {0x3503, 0x00, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_15fps_VGA_640_480[] = {
+	{0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x02, 0, 0}, {0x3809, 0x80, 0, 0}, {0x380a, 0x01, 0, 0},
+	{0x380b, 0xe0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x03, 0, 0}, {0x380f, 0xd8, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_30fps_XGA_1024_768[] = {
+
+	{0x3035, 0x14, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x02, 0, 0}, {0x3809, 0x80, 0, 0}, {0x380a, 0x01, 0, 0},
+	{0x380b, 0xe0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x04, 0, 0}, {0x380f, 0x38, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x0e, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0}, {0x3503, 0x00, 0, 0},
+	{0x3808, 0x04, 0, 0}, {0x3809, 0x00, 0, 0}, {0x380a, 0x03, 0, 0},
+	{0x380b, 0x00, 0, 0}, {0x3035, 0x12, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_15fps_XGA_1024_768[] = {
+	{0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x02, 0, 0}, {0x3809, 0x80, 0, 0}, {0x380a, 0x01, 0, 0},
+	{0x380b, 0xe0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x03, 0, 0}, {0x380f, 0xd8, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0}, {0x3808, 0x04, 0, 0},
+	{0x3809, 0x00, 0, 0}, {0x380a, 0x03, 0, 0}, {0x380b, 0x00, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_30fps_QVGA_320_240[] = {
+	{0x3035, 0x14, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x01, 0, 0}, {0x3809, 0x40, 0, 0}, {0x380a, 0x00, 0, 0},
+	{0x380b, 0xf0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x03, 0, 0}, {0x380f, 0xd8, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_15fps_QVGA_320_240[] = {
+	{0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x01, 0, 0}, {0x3809, 0x40, 0, 0}, {0x380a, 0x00, 0, 0},
+	{0x380b, 0xf0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x03, 0, 0}, {0x380f, 0xd8, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_30fps_QCIF_176_144[] = {
+	{0x3035, 0x14, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x00, 0, 0}, {0x3809, 0xb0, 0, 0}, {0x380a, 0x00, 0, 0},
+	{0x380b, 0x90, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x03, 0, 0}, {0x380f, 0xd8, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+static const struct reg_value ov5640_setting_15fps_QCIF_176_144[] = {
+	{0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x00, 0, 0}, {0x3809, 0xb0, 0, 0}, {0x380a, 0x00, 0, 0},
+	{0x380b, 0x90, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x03, 0, 0}, {0x380f, 0xd8, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_30fps_NTSC_720_480[] = {
+	{0x3035, 0x12, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x02, 0, 0}, {0x3809, 0xd0, 0, 0}, {0x380a, 0x01, 0, 0},
+	{0x380b, 0xe0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x03, 0, 0}, {0x380f, 0xd8, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x3c, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_15fps_NTSC_720_480[] = {
+	{0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x02, 0, 0}, {0x3809, 0xd0, 0, 0}, {0x380a, 0x01, 0, 0},
+	{0x380b, 0xe0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x03, 0, 0}, {0x380f, 0xd8, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x3c, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_30fps_PAL_720_576[] = {
+	{0x3035, 0x12, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x02, 0, 0}, {0x3809, 0xd0, 0, 0}, {0x380a, 0x02, 0, 0},
+	{0x380b, 0x40, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x03, 0, 0}, {0x380f, 0xd8, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x38, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_15fps_PAL_720_576[] = {
+	{0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+	{0x3808, 0x02, 0, 0}, {0x3809, 0xd0, 0, 0}, {0x380a, 0x02, 0, 0},
+	{0x380b, 0x40, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x68, 0, 0},
+	{0x380e, 0x03, 0, 0}, {0x380f, 0xd8, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x38, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_30fps_720P_1280_720[] = {
+	{0x3008, 0x42, 0, 0},
+	{0x3035, 0x21, 0, 0}, {0x3036, 0x54, 0, 0}, {0x3c07, 0x07, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0xfa, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x06, 0, 0}, {0x3807, 0xa9, 0, 0},
+	{0x3808, 0x05, 0, 0}, {0x3809, 0x00, 0, 0}, {0x380a, 0x02, 0, 0},
+	{0x380b, 0xd0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x64, 0, 0},
+	{0x380e, 0x02, 0, 0}, {0x380f, 0xe4, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x02, 0, 0},
+	{0x3a03, 0xe4, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0xbc, 0, 0},
+	{0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x72, 0, 0}, {0x3a0e, 0x01, 0, 0},
+	{0x3a0d, 0x02, 0, 0}, {0x3a14, 0x02, 0, 0}, {0x3a15, 0xe4, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x02, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0},
+	{0x3824, 0x04, 0, 0}, {0x5001, 0x83, 0, 0}, {0x4005, 0x1a, 0, 0},
+	{0x3008, 0x02, 0, 0}, {0x3503, 0,    0, 0},
+};
+
+static const struct reg_value ov5640_setting_15fps_720P_1280_720[] = {
+	{0x3035, 0x41, 0, 0}, {0x3036, 0x54, 0, 0}, {0x3c07, 0x07, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0xfa, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x06, 0, 0}, {0x3807, 0xa9, 0, 0},
+	{0x3808, 0x05, 0, 0}, {0x3809, 0x00, 0, 0}, {0x380a, 0x02, 0, 0},
+	{0x380b, 0xd0, 0, 0}, {0x380c, 0x07, 0, 0}, {0x380d, 0x64, 0, 0},
+	{0x380e, 0x02, 0, 0}, {0x380f, 0xe4, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+	{0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x02, 0, 0},
+	{0x3a03, 0xe4, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0xbc, 0, 0},
+	{0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x72, 0, 0}, {0x3a0e, 0x01, 0, 0},
+	{0x3a0d, 0x02, 0, 0}, {0x3a14, 0x02, 0, 0}, {0x3a15, 0xe4, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x02, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0},
+	{0x3824, 0x04, 0, 0}, {0x5001, 0x83, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_30fps_1080P_1920_1080[] = {
+	{0x3008, 0x42, 0, 0},
+	{0x3035, 0x21, 0, 0}, {0x3036, 0x54, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x40, 0, 0}, {0x3821, 0x06, 0, 0}, {0x3814, 0x11, 0, 0},
+	{0x3815, 0x11, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x00, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9f, 0, 0},
+	{0x3808, 0x0a, 0, 0}, {0x3809, 0x20, 0, 0}, {0x380a, 0x07, 0, 0},
+	{0x380b, 0x98, 0, 0}, {0x380c, 0x0b, 0, 0}, {0x380d, 0x1c, 0, 0},
+	{0x380e, 0x07, 0, 0}, {0x380f, 0xb0, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+	{0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
+	{0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 0}, {0x3035, 0x11, 0, 0},
+	{0x3036, 0x54, 0, 0}, {0x3c07, 0x07, 0, 0}, {0x3c08, 0x00, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3800, 0x01, 0, 0}, {0x3801, 0x50, 0, 0}, {0x3802, 0x01, 0, 0},
+	{0x3803, 0xb2, 0, 0}, {0x3804, 0x08, 0, 0}, {0x3805, 0xef, 0, 0},
+	{0x3806, 0x05, 0, 0}, {0x3807, 0xf1, 0, 0}, {0x3808, 0x07, 0, 0},
+	{0x3809, 0x80, 0, 0}, {0x380a, 0x04, 0, 0}, {0x380b, 0x38, 0, 0},
+	{0x380c, 0x09, 0, 0}, {0x380d, 0xc4, 0, 0}, {0x380e, 0x04, 0, 0},
+	{0x380f, 0x60, 0, 0}, {0x3612, 0x2b, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3a02, 0x04, 0, 0}, {0x3a03, 0x60, 0, 0}, {0x3a08, 0x01, 0, 0},
+	{0x3a09, 0x50, 0, 0}, {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x18, 0, 0},
+	{0x3a0e, 0x03, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x04, 0, 0},
+	{0x3a15, 0x60, 0, 0}, {0x4713, 0x02, 0, 0}, {0x4407, 0x04, 0, 0},
+	{0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3824, 0x04, 0, 0},
+	{0x4005, 0x1a, 0, 0}, {0x3008, 0x02, 0, 0},
+	{0x3503, 0, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_15fps_1080P_1920_1080[] = {
+	{0x3008, 0x42, 0, 0},
+	{0x3035, 0x21, 0, 0}, {0x3036, 0x54, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x40, 0, 0}, {0x3821, 0x06, 0, 0}, {0x3814, 0x11, 0, 0},
+	{0x3815, 0x11, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x00, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9f, 0, 0},
+	{0x3808, 0x0a, 0, 0}, {0x3809, 0x20, 0, 0}, {0x380a, 0x07, 0, 0},
+	{0x380b, 0x98, 0, 0}, {0x380c, 0x0b, 0, 0}, {0x380d, 0x1c, 0, 0},
+	{0x380e, 0x07, 0, 0}, {0x380f, 0xb0, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+	{0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
+	{0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 0}, {0x3035, 0x21, 0, 0},
+	{0x3036, 0x54, 0, 1}, {0x3c07, 0x07, 0, 0}, {0x3c08, 0x00, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3800, 0x01, 0, 0}, {0x3801, 0x50, 0, 0}, {0x3802, 0x01, 0, 0},
+	{0x3803, 0xb2, 0, 0}, {0x3804, 0x08, 0, 0}, {0x3805, 0xef, 0, 0},
+	{0x3806, 0x05, 0, 0}, {0x3807, 0xf1, 0, 0}, {0x3808, 0x07, 0, 0},
+	{0x3809, 0x80, 0, 0}, {0x380a, 0x04, 0, 0}, {0x380b, 0x38, 0, 0},
+	{0x380c, 0x09, 0, 0}, {0x380d, 0xc4, 0, 0}, {0x380e, 0x04, 0, 0},
+	{0x380f, 0x60, 0, 0}, {0x3612, 0x2b, 0, 0}, {0x3708, 0x64, 0, 0},
+	{0x3a02, 0x04, 0, 0}, {0x3a03, 0x60, 0, 0}, {0x3a08, 0x01, 0, 0},
+	{0x3a09, 0x50, 0, 0}, {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x18, 0, 0},
+	{0x3a0e, 0x03, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x04, 0, 0},
+	{0x3a15, 0x60, 0, 0}, {0x4713, 0x02, 0, 0}, {0x4407, 0x04, 0, 0},
+	{0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3824, 0x04, 0, 0},
+	{0x4005, 0x1a, 0, 0}, {0x3008, 0x02, 0, 0}, {0x3503, 0, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_15fps_QSXGA_2592_1944[] = {
+	{0x3820, 0x40, 0, 0}, {0x3821, 0x06, 0, 0},
+	{0x3035, 0x21, 0, 0}, {0x3036, 0x54, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+	{0x3820, 0x40, 0, 0}, {0x3821, 0x06, 0, 0}, {0x3814, 0x11, 0, 0},
+	{0x3815, 0x11, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+	{0x3802, 0x00, 0, 0}, {0x3803, 0x00, 0, 0}, {0x3804, 0x0a, 0, 0},
+	{0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9f, 0, 0},
+	{0x3808, 0x0a, 0, 0}, {0x3809, 0x20, 0, 0}, {0x380a, 0x07, 0, 0},
+	{0x380b, 0x98, 0, 0}, {0x380c, 0x0b, 0, 0}, {0x380d, 0x1c, 0, 0},
+	{0x380e, 0x07, 0, 0}, {0x380f, 0xb0, 0, 0}, {0x3810, 0x00, 0, 0},
+	{0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+	{0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
+	{0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
+	{0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+	{0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+	{0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+	{0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0}, {0x4713, 0x03, 0, 0},
+	{0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+	{0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 70},
+};
+
+/* power-on sensor init reg table */
+static const struct ov5640_mode_info ov5640_mode_init_data = {
+	0, SUBSAMPLING, 640, 480, ov5640_init_setting_30fps_VGA,
+	ARRAY_SIZE(ov5640_init_setting_30fps_VGA),
+};
+
+static const struct ov5640_mode_info
+ov5640_mode_data[OV5640_NUM_FRAMERATES][OV5640_NUM_MODES] = {
+	{
+		{OV5640_MODE_QCIF_176_144, SUBSAMPLING, 176, 144,
+		 ov5640_setting_15fps_QCIF_176_144,
+		 ARRAY_SIZE(ov5640_setting_15fps_QCIF_176_144)},
+		{OV5640_MODE_QVGA_320_240, SUBSAMPLING, 320,  240,
+		 ov5640_setting_15fps_QVGA_320_240,
+		 ARRAY_SIZE(ov5640_setting_15fps_QVGA_320_240)},
+		{OV5640_MODE_VGA_640_480, SUBSAMPLING, 640,  480,
+		 ov5640_setting_15fps_VGA_640_480,
+		 ARRAY_SIZE(ov5640_setting_15fps_VGA_640_480)},
+		{OV5640_MODE_NTSC_720_480, SUBSAMPLING, 720, 480,
+		 ov5640_setting_15fps_NTSC_720_480,
+		 ARRAY_SIZE(ov5640_setting_15fps_NTSC_720_480)},
+		{OV5640_MODE_PAL_720_576, SUBSAMPLING, 720, 576,
+		 ov5640_setting_15fps_PAL_720_576,
+		 ARRAY_SIZE(ov5640_setting_15fps_PAL_720_576)},
+		{OV5640_MODE_XGA_1024_768, SUBSAMPLING, 1024, 768,
+		 ov5640_setting_15fps_XGA_1024_768,
+		 ARRAY_SIZE(ov5640_setting_15fps_XGA_1024_768)},
+		{OV5640_MODE_720P_1280_720, SUBSAMPLING, 1280, 720,
+		 ov5640_setting_15fps_720P_1280_720,
+		 ARRAY_SIZE(ov5640_setting_15fps_720P_1280_720)},
+		{OV5640_MODE_1080P_1920_1080, SCALING, 1920, 1080,
+		 ov5640_setting_15fps_1080P_1920_1080,
+		 ARRAY_SIZE(ov5640_setting_15fps_1080P_1920_1080)},
+		{OV5640_MODE_QSXGA_2592_1944, SCALING, 2592, 1944,
+		 ov5640_setting_15fps_QSXGA_2592_1944,
+		 ARRAY_SIZE(ov5640_setting_15fps_QSXGA_2592_1944)},
+	}, {
+		{OV5640_MODE_QCIF_176_144, SUBSAMPLING, 176, 144,
+		 ov5640_setting_30fps_QCIF_176_144,
+		 ARRAY_SIZE(ov5640_setting_30fps_QCIF_176_144)},
+		{OV5640_MODE_QVGA_320_240, SUBSAMPLING, 320,  240,
+		 ov5640_setting_30fps_QVGA_320_240,
+		 ARRAY_SIZE(ov5640_setting_30fps_QVGA_320_240)},
+		{OV5640_MODE_VGA_640_480, SUBSAMPLING, 640,  480,
+		 ov5640_setting_30fps_VGA_640_480,
+		 ARRAY_SIZE(ov5640_setting_30fps_VGA_640_480)},
+		{OV5640_MODE_NTSC_720_480, SUBSAMPLING, 720, 480,
+		 ov5640_setting_30fps_NTSC_720_480,
+		 ARRAY_SIZE(ov5640_setting_30fps_NTSC_720_480)},
+		{OV5640_MODE_PAL_720_576, SUBSAMPLING, 720, 576,
+		 ov5640_setting_30fps_PAL_720_576,
+		 ARRAY_SIZE(ov5640_setting_30fps_PAL_720_576)},
+		{OV5640_MODE_XGA_1024_768, SUBSAMPLING, 1024, 768,
+		 ov5640_setting_30fps_XGA_1024_768,
+		 ARRAY_SIZE(ov5640_setting_30fps_XGA_1024_768)},
+		{OV5640_MODE_720P_1280_720, SUBSAMPLING, 1280, 720,
+		 ov5640_setting_30fps_720P_1280_720,
+		 ARRAY_SIZE(ov5640_setting_30fps_720P_1280_720)},
+		{OV5640_MODE_1080P_1920_1080, SCALING, 1920, 1080,
+		 ov5640_setting_30fps_1080P_1920_1080,
+		 ARRAY_SIZE(ov5640_setting_30fps_1080P_1920_1080)},
+		{OV5640_MODE_QSXGA_2592_1944, -1, 0, 0, NULL, 0},
+	},
+};
+
+static int ov5640_init_slave_id(struct ov5640_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg;
+	u8 buf[3];
+	int ret;
+
+	if (client->addr == OV5640_DEFAULT_SLAVE_ID)
+		return 0;
+
+	buf[0] = OV5640_REG_SLAVE_ID >> 8;
+	buf[1] = OV5640_REG_SLAVE_ID & 0xff;
+	buf[2] = client->addr << 1;
+
+	msg.addr = OV5640_DEFAULT_SLAVE_ID;
+	msg.flags = 0;
+	msg.buf = buf;
+	msg.len = sizeof(buf);
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: failed with %d\n", __func__, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5640_write_reg(struct ov5640_dev *sensor, u16 reg, u8 val)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg;
+	u8 buf[3];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+	buf[2] = val;
+
+	msg.addr = client->addr;
+	msg.flags = client->flags;
+	msg.buf = buf;
+	msg.len = sizeof(buf);
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		v4l2_err(&sensor->sd, "%s: error: reg=%x, val=%x\n",
+			__func__, reg, val);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5640_read_reg(struct ov5640_dev *sensor, u16 reg, u8 *val)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct i2c_msg msg[2];
+	u8 buf[2];
+	int ret;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags;
+	msg[0].buf = buf;
+	msg[0].len = sizeof(buf);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = client->flags | I2C_M_RD;
+	msg[1].buf = buf;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret < 0)
+		return ret;
+
+	*val = buf[0];
+	return 0;
+}
+
+static int ov5640_read_reg16(struct ov5640_dev *sensor, u16 reg, u16 *val)
+{
+	u8 hi, lo;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, reg, &hi);
+	if (ret)
+		return ret;
+	ret = ov5640_read_reg(sensor, reg+1, &lo);
+	if (ret)
+		return ret;
+
+	*val = ((u16)hi << 8) | (u16)lo;
+	return 0;
+}
+
+static int ov5640_write_reg16(struct ov5640_dev *sensor, u16 reg, u16 val)
+{
+	int ret;
+
+	ret = ov5640_write_reg(sensor, reg, val >> 8);
+	if (ret)
+		return ret;
+
+	return ov5640_write_reg(sensor, reg + 1, val & 0xff);
+}
+
+static int ov5640_mod_reg(struct ov5640_dev *sensor, u16 reg,
+			  u8 mask, u8 val)
+{
+	u8 readval;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, reg, &readval);
+	if (ret)
+		return ret;
+
+	readval &= ~mask;
+	val &= mask;
+	val |= readval;
+
+	return ov5640_write_reg(sensor, reg, val);
+}
+
+/* download ov5640 settings to sensor through i2c */
+static int ov5640_load_regs(struct ov5640_dev *sensor,
+			    const struct ov5640_mode_info *mode)
+{
+	const struct reg_value *regs = mode->reg_data;
+	unsigned int i;
+	u32 delay_ms;
+	u16 reg_addr;
+	u8 mask, val;
+	int ret = 0;
+
+	for (i = 0; i < mode->reg_data_size; ++i, ++regs) {
+		delay_ms = regs->delay_ms;
+		reg_addr = regs->reg_addr;
+		val = regs->val;
+		mask = regs->mask;
+
+		if (mask)
+			ret = ov5640_mod_reg(sensor, reg_addr, mask, val);
+		else
+			ret = ov5640_write_reg(sensor, reg_addr, val);
+		if (ret)
+			break;
+
+		if (delay_ms)
+			usleep_range(1000*delay_ms, 1000*delay_ms+100);
+	}
+
+	return ret;
+}
+
+/* read exposure, in number of line periods */
+static int ov5640_get_exposure(struct ov5640_dev *sensor)
+{
+	int exp, ret;
+	u8 temp;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_HI, &temp);
+	if (ret)
+		return ret;
+	exp = ((int)temp & 0x0f) << 16;
+	ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_MED, &temp);
+	if (ret)
+		return ret;
+	exp |= ((int)temp << 8);
+	ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_LO, &temp);
+	if (ret)
+		return ret;
+	exp |= (int)temp;
+
+	return exp >> 4;
+}
+
+/* write exposure, given number of line periods */
+static int ov5640_set_exposure(struct ov5640_dev *sensor, u32 exposure)
+{
+	int ret;
+
+	exposure <<= 4;
+
+	ret = ov5640_write_reg(sensor,
+			       OV5640_REG_AEC_PK_EXPOSURE_LO,
+			       exposure & 0xff);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor,
+			       OV5640_REG_AEC_PK_EXPOSURE_MED,
+			       (exposure >> 8) & 0xff);
+	if (ret)
+		return ret;
+	return ov5640_write_reg(sensor,
+				OV5640_REG_AEC_PK_EXPOSURE_HI,
+				(exposure >> 16) & 0x0f);
+}
+
+static int ov5640_get_gain(struct ov5640_dev *sensor)
+{
+	u16 gain;
+	int ret;
+
+	ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN, &gain);
+	if (ret)
+		return ret;
+
+	return gain & 0x3ff;
+}
+
+static int ov5640_set_stream(struct ov5640_dev *sensor, bool on)
+{
+	int ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_MIPI_CTRL00, BIT(5),
+			     on ? 0 : BIT(5));
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT00,
+			       on ? 0x00 : 0x70);
+	if (ret)
+		return ret;
+
+	return ov5640_write_reg(sensor, OV5640_REG_FRAME_CTRL01,
+				on ? 0x00 : 0x0f);
+}
+
+static int ov5640_get_sysclk(struct ov5640_dev *sensor)
+{
+	 /* calculate sysclk */
+	u32 xvclk = sensor->xclk_freq / 10000;
+	u32 multiplier, prediv, VCO, sysdiv, pll_rdiv;
+	u32 sclk_rdiv_map[] = {1, 2, 4, 8};
+	u32 bit_div2x = 1, sclk_rdiv, sysclk;
+	u8 temp1, temp2;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL0, &temp1);
+	if (ret)
+		return ret;
+	temp2 = temp1 & 0x0f;
+	if (temp2 == 8 || temp2 == 10)
+		bit_div2x = temp2 / 2;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL1, &temp1);
+	if (ret)
+		return ret;
+	sysdiv = temp1 >> 4;
+	if (sysdiv == 0)
+		sysdiv = 16;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL2, &temp1);
+	if (ret)
+		return ret;
+	multiplier = temp1;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL3, &temp1);
+	if (ret)
+		return ret;
+	prediv = temp1 & 0x0f;
+	pll_rdiv = ((temp1 >> 4) & 0x01) + 1;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, &temp1);
+	if (ret)
+		return ret;
+	temp2 = temp1 & 0x03;
+	sclk_rdiv = sclk_rdiv_map[temp2];
+
+	if (!prediv || !sysdiv || !pll_rdiv || !bit_div2x)
+		return -EINVAL;
+
+	VCO = xvclk * multiplier / prediv;
+
+	sysclk = VCO / sysdiv / pll_rdiv * 2 / bit_div2x / sclk_rdiv;
+
+	return sysclk;
+}
+
+static int ov5640_set_night_mode(struct ov5640_dev *sensor)
+{
+	 /* read HTS from register settings */
+	u8 mode;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_AEC_CTRL00, &mode);
+	if (ret)
+		return ret;
+	mode &= 0xfb;
+	return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL00, mode);
+}
+
+static int ov5640_get_hts(struct ov5640_dev *sensor)
+{
+	/* read HTS from register settings */
+	u16 hts;
+	int ret;
+
+	ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_HTS, &hts);
+	if (ret)
+		return ret;
+	return hts;
+}
+
+static int ov5640_get_vts(struct ov5640_dev *sensor)
+{
+	u16 vts;
+	int ret;
+
+	ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_VTS, &vts);
+	if (ret)
+		return ret;
+	return vts;
+}
+
+static int ov5640_set_vts(struct ov5640_dev *sensor, int vts)
+{
+	return ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS, vts);
+}
+
+static int ov5640_get_light_freq(struct ov5640_dev *sensor)
+{
+	/* get banding filter value */
+	int ret, light_freq = 0;
+	u8 temp, temp1;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL01, &temp);
+	if (ret)
+		return ret;
+
+	if (temp & 0x80) {
+		/* manual */
+		ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL00,
+				      &temp1);
+		if (ret)
+			return ret;
+		if (temp1 & 0x04) {
+			/* 50Hz */
+			light_freq = 50;
+		} else {
+			/* 60Hz */
+			light_freq = 60;
+		}
+	} else {
+		/* auto */
+		ret = ov5640_read_reg(sensor, OV5640_REG_SIGMADELTA_CTRL0C,
+				      &temp1);
+		if (ret)
+			return ret;
+
+		if (temp1 & 0x01) {
+			/* 50Hz */
+			light_freq = 50;
+		} else {
+			/* 60Hz */
+		}
+	}
+
+	return light_freq;
+}
+
+static int ov5640_set_bandingfilter(struct ov5640_dev *sensor)
+{
+	u32 band_step60, max_band60, band_step50, max_band50, prev_vts;
+	int ret;
+
+	/* read preview PCLK */
+	ret = ov5640_get_sysclk(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		return -EINVAL;
+	sensor->prev_sysclk = ret;
+	/* read preview HTS */
+	ret = ov5640_get_hts(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		return -EINVAL;
+	sensor->prev_hts = ret;
+
+	/* read preview VTS */
+	ret = ov5640_get_vts(sensor);
+	if (ret < 0)
+		return ret;
+	prev_vts = ret;
+
+
+	/* calculate banding filter */
+	/* 60Hz */
+	band_step60 = sensor->prev_sysclk * 100 / sensor->prev_hts * 100 / 120;
+	ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B60_STEP, band_step60);
+	if (ret)
+		return ret;
+	if (!band_step60)
+		return -EINVAL;
+	max_band60 = (int)((prev_vts - 4) / band_step60);
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0D, max_band60);
+	if (ret)
+		return ret;
+
+	/* 50Hz */
+	band_step50 = sensor->prev_sysclk * 100 / sensor->prev_hts;
+	ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B50_STEP, band_step50);
+	if (ret)
+		return ret;
+	if (!band_step50)
+		return -EINVAL;
+	max_band50 = (int)((prev_vts - 4) / band_step50);
+	return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0E, max_band50);
+}
+
+static int ov5640_set_ae_target(struct ov5640_dev *sensor, int target)
+{
+	/* stable in high */
+	u32 fast_high, fast_low;
+	int ret;
+
+	sensor->ae_low = target * 23 / 25;	/* 0.92 */
+	sensor->ae_high = target * 27 / 25;	/* 1.08 */
+
+	fast_high = sensor->ae_high << 1;
+	if (fast_high > 255)
+		fast_high = 255;
+
+	fast_low = sensor->ae_low >> 1;
+
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0F, sensor->ae_high);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL10, sensor->ae_low);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1B, sensor->ae_high);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1E, sensor->ae_low);
+	if (ret)
+		return ret;
+	ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL11, fast_high);
+	if (ret)
+		return ret;
+	return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1F, fast_low);
+}
+
+static int ov5640_binning_on(struct ov5640_dev *sensor)
+{
+	u8 temp;
+	int ret;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_TIMING_TC_REG21, &temp);
+	if (ret)
+		return ret;
+	temp &= 0xfe;
+	return temp ? 1 : 0;
+}
+
+static int ov5640_set_virtual_channel(struct ov5640_dev *sensor)
+{
+	u8 temp, channel = virtual_channel;
+	int ret;
+
+	if (channel > 3)
+		return -EINVAL;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_DEBUG_MODE, &temp);
+	if (ret)
+		return ret;
+	temp &= ~(3 << 6);
+	temp |= (channel << 6);
+	return ov5640_write_reg(sensor, OV5640_REG_DEBUG_MODE, temp);
+}
+
+static const struct ov5640_mode_info *
+ov5640_find_mode(struct ov5640_dev *sensor, enum ov5640_frame_rate fr,
+		 int width, int height, bool nearest)
+{
+	const struct ov5640_mode_info *mode = NULL;
+	int i;
+
+	for (i = OV5640_NUM_MODES - 1; i >= 0; i--) {
+		mode = &ov5640_mode_data[fr][i];
+
+		if (!mode->reg_data)
+			continue;
+
+		if ((nearest && mode->width <= width &&
+		     mode->height <= height) ||
+		    (!nearest && mode->width == width &&
+		     mode->height == height))
+			break;
+	}
+
+	if (nearest && i < 0)
+		mode = &ov5640_mode_data[fr][0];
+
+	return mode;
+}
+
+/*
+ * sensor changes between scaling and subsampling, go through
+ * exposure calculation
+ */
+static int ov5640_set_mode_exposure_calc(
+	struct ov5640_dev *sensor, const struct ov5640_mode_info *mode)
+{
+	u32 prev_shutter, prev_gain16;
+	u32 cap_shutter, cap_gain16;
+	u32 cap_sysclk, cap_hts, cap_vts;
+	u32 light_freq, cap_bandfilt, cap_maxband;
+	u32 cap_gain16_shutter;
+	u8 average;
+	int ret;
+
+	if (mode->reg_data == NULL)
+		return -EINVAL;
+
+	/* read preview shutter */
+	ret = ov5640_get_exposure(sensor);
+	if (ret < 0)
+		return ret;
+	prev_shutter = ret;
+	ret = ov5640_binning_on(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret && mode->id != OV5640_MODE_720P_1280_720 &&
+	    mode->id != OV5640_MODE_1080P_1920_1080)
+		prev_shutter *= 2;
+
+	/* read preview gain */
+	ret = ov5640_get_gain(sensor);
+	if (ret < 0)
+		return ret;
+	prev_gain16 = ret;
+
+	/* get average */
+	ret = ov5640_read_reg(sensor, OV5640_REG_AVG_READOUT, &average);
+	if (ret)
+		return ret;
+
+	/* turn off night mode for capture */
+	ret = ov5640_set_night_mode(sensor);
+	if (ret < 0)
+		return ret;
+
+	/* Write capture setting */
+	ret = ov5640_load_regs(sensor, mode);
+	if (ret < 0)
+		return ret;
+
+	/* read capture VTS */
+	ret = ov5640_get_vts(sensor);
+	if (ret < 0)
+		return ret;
+	cap_vts = ret;
+	ret = ov5640_get_hts(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		return -EINVAL;
+	cap_hts = ret;
+
+	ret = ov5640_get_sysclk(sensor);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		return -EINVAL;
+	cap_sysclk = ret;
+
+	/* calculate capture banding filter */
+	ret = ov5640_get_light_freq(sensor);
+	if (ret < 0)
+		return ret;
+	light_freq = ret;
+
+	if (light_freq == 60) {
+		/* 60Hz */
+		cap_bandfilt = cap_sysclk * 100 / cap_hts * 100 / 120;
+	} else {
+		/* 50Hz */
+		cap_bandfilt = cap_sysclk * 100 / cap_hts;
+	}
+
+	if (!sensor->prev_sysclk) {
+		ret = ov5640_get_sysclk(sensor);
+		if (ret < 0)
+			return ret;
+		if (ret == 0)
+			return -EINVAL;
+		sensor->prev_sysclk = ret;
+	}
+
+	if (!cap_bandfilt)
+		return -EINVAL;
+
+	cap_maxband = (int)((cap_vts - 4) / cap_bandfilt);
+
+	/* calculate capture shutter/gain16 */
+	if (average > sensor->ae_low && average < sensor->ae_high) {
+		/* in stable range */
+		cap_gain16_shutter =
+			prev_gain16 * prev_shutter *
+			cap_sysclk / sensor->prev_sysclk *
+			sensor->prev_hts / cap_hts *
+			sensor->ae_target / average;
+	} else {
+		cap_gain16_shutter =
+			prev_gain16 * prev_shutter *
+			cap_sysclk / sensor->prev_sysclk *
+			sensor->prev_hts / cap_hts;
+	}
+
+	/* gain to shutter */
+	if (cap_gain16_shutter < (cap_bandfilt * 16)) {
+		/* shutter < 1/100 */
+		cap_shutter = cap_gain16_shutter / 16;
+		if (cap_shutter < 1)
+			cap_shutter = 1;
+
+		cap_gain16 = cap_gain16_shutter / cap_shutter;
+		if (cap_gain16 < 16)
+			cap_gain16 = 16;
+	} else {
+		if (cap_gain16_shutter > (cap_bandfilt * cap_maxband * 16)) {
+			/* exposure reach max */
+			cap_shutter = cap_bandfilt * cap_maxband;
+			if (!cap_shutter)
+				return -EINVAL;
+
+			cap_gain16 = cap_gain16_shutter / cap_shutter;
+		} else {
+			/* 1/100 < (cap_shutter = n/100) =< max */
+			cap_shutter =
+				((int)(cap_gain16_shutter / 16 / cap_bandfilt))
+				* cap_bandfilt;
+			if (!cap_shutter)
+				return -EINVAL;
+
+			cap_gain16 = cap_gain16_shutter / cap_shutter;
+		}
+	}
+
+	/* set capture gain */
+	ret = __v4l2_ctrl_s_ctrl(sensor->ctrls.gain, cap_gain16);
+	if (ret)
+		return ret;
+
+	/* write capture shutter */
+	if (cap_shutter > (cap_vts - 4)) {
+		cap_vts = cap_shutter + 4;
+		ret = ov5640_set_vts(sensor, cap_vts);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* set exposure */
+	return __v4l2_ctrl_s_ctrl(sensor->ctrls.exposure, cap_shutter);
+}
+
+/*
+ * if sensor changes inside scaling or subsampling
+ * change mode directly
+ */
+static int ov5640_set_mode_direct(struct ov5640_dev *sensor,
+				  const struct ov5640_mode_info *mode)
+{
+	int ret;
+
+	if (mode->reg_data == NULL)
+		return -EINVAL;
+
+	/* Write capture setting */
+	ret = ov5640_load_regs(sensor, mode);
+	if (ret < 0)
+		return ret;
+
+	/* turn auto gain/exposure back on for direct mode */
+	ret = __v4l2_ctrl_s_ctrl(sensor->ctrls.auto_gain, 1);
+	if (ret)
+		return ret;
+	return __v4l2_ctrl_s_ctrl(sensor->ctrls.auto_exp, V4L2_EXPOSURE_AUTO);
+}
+
+static int ov5640_set_mode(struct ov5640_dev *sensor,
+			   const struct ov5640_mode_info *orig_mode)
+{
+	const struct ov5640_mode_info *mode = sensor->current_mode;
+	enum ov5640_downsize_mode dn_mode, orig_dn_mode;
+	int ret;
+
+	dn_mode = mode->dn_mode;
+	orig_dn_mode = orig_mode->dn_mode;
+
+	/* auto gain and exposure must be turned off when changing modes */
+	ret = __v4l2_ctrl_s_ctrl(sensor->ctrls.auto_gain, 0);
+	if (ret)
+		return ret;
+	ret = __v4l2_ctrl_s_ctrl(sensor->ctrls.auto_exp, V4L2_EXPOSURE_MANUAL);
+	if (ret)
+		return ret;
+
+	if ((dn_mode == SUBSAMPLING && orig_dn_mode == SCALING) ||
+	    (dn_mode == SCALING && orig_dn_mode == SUBSAMPLING)) {
+		/*
+		 * change between subsampling and scaling
+		 * go through exposure calucation
+		 */
+		ret = ov5640_set_mode_exposure_calc(sensor, mode);
+	} else {
+		/*
+		 * change inside subsampling or scaling
+		 * download firmware directly
+		 */
+		ret = ov5640_set_mode_direct(sensor, mode);
+	}
+
+	if (ret < 0)
+		return ret;
+
+	ret = ov5640_set_ae_target(sensor, sensor->ae_target);
+	if (ret < 0)
+		return ret;
+	ret = ov5640_get_light_freq(sensor);
+	if (ret < 0)
+		return ret;
+	ret = ov5640_set_bandingfilter(sensor);
+	if (ret < 0)
+		return ret;
+	ret = ov5640_set_virtual_channel(sensor);
+	if (ret < 0)
+		return ret;
+
+	sensor->pending_mode_change = false;
+
+	return 0;
+}
+
+/* restore the last set video mode after chip power-on */
+static int ov5640_restore_mode(struct ov5640_dev *sensor)
+{
+	int ret;
+
+	/* first load the initial register values */
+	ret = ov5640_load_regs(sensor, &ov5640_mode_init_data);
+	if (ret < 0)
+		return ret;
+
+	/* now restore the last capture mode */
+	return ov5640_set_mode(sensor, &ov5640_mode_init_data);
+}
+
+static void ov5640_power(struct ov5640_dev *sensor, bool enable)
+{
+	if (sensor->pwdn_gpio)
+		gpiod_set_value(sensor->pwdn_gpio, enable ? 0 : 1);
+}
+
+static void ov5640_reset(struct ov5640_dev *sensor)
+{
+	if (!sensor->reset_gpio)
+		return;
+
+	gpiod_set_value(sensor->reset_gpio, 0);
+
+	/* camera power cycle */
+	ov5640_power(sensor, false);
+	usleep_range(5000, 10000);
+	ov5640_power(sensor, true);
+	usleep_range(5000, 10000);
+
+	gpiod_set_value(sensor->reset_gpio, 1);
+	usleep_range(1000, 2000);
+
+	gpiod_set_value(sensor->reset_gpio, 0);
+	usleep_range(5000, 10000);
+}
+
+static int ov5640_set_power(struct ov5640_dev *sensor, bool on)
+{
+	int ret = 0;
+
+	if (on) {
+		clk_prepare_enable(sensor->xclk);
+
+		ret = regulator_bulk_enable(OV5640_NUM_SUPPLIES,
+					    sensor->supplies);
+		if (ret)
+			goto xclk_off;
+
+		ov5640_reset(sensor);
+		ov5640_power(sensor, true);
+
+		ret = ov5640_init_slave_id(sensor);
+		if (ret)
+			goto power_off;
+
+		ret = ov5640_restore_mode(sensor);
+		if (ret)
+			goto power_off;
+
+		/*
+		 * start streaming briefly followed by stream off in
+		 * order to coax the clock lane into LP-11 state.
+		 */
+		ret = ov5640_set_stream(sensor, true);
+		if (ret)
+			goto power_off;
+		usleep_range(1000, 2000);
+		ret = ov5640_set_stream(sensor, false);
+		if (ret)
+			goto power_off;
+
+		return 0;
+	}
+
+power_off:
+	ov5640_power(sensor, false);
+	regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
+xclk_off:
+	clk_disable_unprepare(sensor->xclk);
+	return ret;
+}
+
+/* --------------- Subdev Operations --------------- */
+
+static int ov5640_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	int ret = 0;
+
+	mutex_lock(&sensor->lock);
+
+	/*
+	 * If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (sensor->power_count == !on) {
+		ret = ov5640_set_power(sensor, !!on);
+		if (ret)
+			goto out;
+	}
+
+	/* Update the power count. */
+	sensor->power_count += on ? 1 : -1;
+	WARN_ON(sensor->power_count < 0);
+out:
+	mutex_unlock(&sensor->lock);
+
+	if (on && !ret && sensor->power_count == 1) {
+		/* restore controls */
+		ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+	}
+
+	return ret;
+}
+
+static int ov5640_try_frame_interval(struct ov5640_dev *sensor,
+				     struct v4l2_fract *fi,
+				     u32 width, u32 height)
+{
+	const struct ov5640_mode_info *mode;
+	u32 minfps, maxfps, fps;
+	int ret;
+
+	minfps = ov5640_framerates[OV5640_15_FPS];
+	maxfps = ov5640_framerates[OV5640_30_FPS];
+
+	if (fi->numerator == 0) {
+		fi->denominator = maxfps;
+		fi->numerator = 1;
+		return OV5640_30_FPS;
+	}
+
+	fps = DIV_ROUND_CLOSEST(fi->denominator, fi->numerator);
+
+	fi->numerator = 1;
+	if (fps > maxfps)
+		fi->denominator = maxfps;
+	else if (fps < minfps)
+		fi->denominator = minfps;
+	else if (2 * fps >= 2 * minfps + (maxfps - minfps))
+		fi->denominator = maxfps;
+	else
+		fi->denominator = minfps;
+
+	ret = (fi->denominator == minfps) ? OV5640_15_FPS : OV5640_30_FPS;
+
+	mode = ov5640_find_mode(sensor, ret, width, height, false);
+	return mode ? ret : -EINVAL;
+}
+
+static int ov5640_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	if (format->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt = v4l2_subdev_get_try_format(&sensor->sd, cfg,
+						 format->pad);
+	else
+		fmt = &sensor->fmt;
+
+	format->format = *fmt;
+
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int ov5640_try_fmt_internal(struct v4l2_subdev *sd,
+				   struct v4l2_mbus_framefmt *fmt,
+				   enum ov5640_frame_rate fr,
+				   const struct ov5640_mode_info **new_mode)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	const struct ov5640_mode_info *mode;
+
+	mode = ov5640_find_mode(sensor, fr, fmt->width, fmt->height, true);
+	if (!mode)
+		return -EINVAL;
+
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = sensor->fmt.code;
+
+	if (new_mode)
+		*new_mode = mode;
+	return 0;
+}
+
+static int ov5640_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *format)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	const struct ov5640_mode_info *new_mode;
+	int ret;
+
+	if (format->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	ret = ov5640_try_fmt_internal(sd, &format->format,
+				      sensor->current_fr, &new_mode);
+	if (ret)
+		goto out;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt =
+			v4l2_subdev_get_try_format(sd, cfg, 0);
+
+		*fmt = format->format;
+		goto out;
+	}
+
+	sensor->current_mode = new_mode;
+	sensor->fmt = format->format;
+	sensor->pending_mode_change = true;
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+
+/*
+ * Sensor Controls.
+ */
+
+static int ov5640_set_ctrl_hue(struct ov5640_dev *sensor, int value)
+{
+	int ret;
+
+	if (value) {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
+				     BIT(0), BIT(0));
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg16(sensor, OV5640_REG_SDE_CTRL1, value);
+	} else {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(0), 0);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_contrast(struct ov5640_dev *sensor, int value)
+{
+	int ret;
+
+	if (value) {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
+				     BIT(2), BIT(2));
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL5,
+				       value & 0xff);
+	} else {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(2), 0);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_saturation(struct ov5640_dev *sensor, int value)
+{
+	int ret;
+
+	if (value) {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
+				     BIT(1), BIT(1));
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL3,
+				       value & 0xff);
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL4,
+				       value & 0xff);
+	} else {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(1), 0);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_white_balance(struct ov5640_dev *sensor, int awb)
+{
+	int ret;
+
+	ret = ov5640_mod_reg(sensor, OV5640_REG_AWB_MANUAL_CTRL,
+			     BIT(0), awb ? 0 : 1);
+	if (ret)
+		return ret;
+
+	if (!awb) {
+		u16 red = (u16)sensor->ctrls.red_balance->val;
+		u16 blue = (u16)sensor->ctrls.blue_balance->val;
+
+		ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_R_GAIN, red);
+		if (ret)
+			return ret;
+		ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_B_GAIN, blue);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_exposure(struct ov5640_dev *sensor, int exp)
+{
+	struct ov5640_ctrls *ctrls = &sensor->ctrls;
+	bool auto_exposure = (exp == V4L2_EXPOSURE_AUTO);
+	int ret = 0;
+
+	if (ctrls->auto_exp->is_new) {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
+				     BIT(0), auto_exposure ? 0 : BIT(0));
+		if (ret)
+			return ret;
+	}
+
+	if (!auto_exposure && ctrls->exposure->is_new) {
+		u16 max_exp;
+
+		ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_VTS,
+					&max_exp);
+		if (ret)
+			return ret;
+		ret = ov5640_get_vts(sensor);
+		if (ret < 0)
+			return ret;
+		max_exp += ret;
+
+		if (ctrls->exposure->val < max_exp)
+			ret = ov5640_set_exposure(sensor, ctrls->exposure->val);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_gain(struct ov5640_dev *sensor, int auto_gain)
+{
+	struct ov5640_ctrls *ctrls = &sensor->ctrls;
+	int ret = 0;
+
+	if (ctrls->auto_gain->is_new) {
+		ret = ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
+				     BIT(1), ctrls->auto_gain->val ? 0 : BIT(1));
+		if (ret)
+			return ret;
+	}
+
+	if (!auto_gain && ctrls->gain->is_new) {
+		u16 gain = (u16)ctrls->gain->val;
+
+		ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN,
+					 gain & 0x3ff);
+	}
+
+	return ret;
+}
+
+static int ov5640_set_ctrl_test_pattern(struct ov5640_dev *sensor, int value)
+{
+	return ov5640_mod_reg(sensor, OV5640_REG_PRE_ISP_TEST_SET1,
+			      0xa4, value ? 0xa4 : 0);
+}
+
+static int ov5640_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	int val;
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		if (!ctrl->val)
+			return 0;
+		val = ov5640_get_gain(sensor);
+		if (val < 0)
+			return val;
+		sensor->ctrls.gain->val = val;
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		if (ctrl->val == V4L2_EXPOSURE_MANUAL)
+			return 0;
+		val = ov5640_get_exposure(sensor);
+		if (val < 0)
+			return val;
+		sensor->ctrls.exposure->val = val;
+		break;
+	}
+
+	return 0;
+}
+
+static int ov5640_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	int ret;
+
+	/* v4l2_ctrl_lock() locks our own mutex */
+
+	/*
+	 * If the device is not powered up by the host driver do
+	 * not apply any controls to H/W at this time. Instead
+	 * the controls will be restored right after power-up.
+	 */
+	if (sensor->power_count == 0)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		ret = ov5640_set_ctrl_gain(sensor, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = ov5640_set_ctrl_exposure(sensor, ctrl->val);
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = ov5640_set_ctrl_white_balance(sensor, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		ret = ov5640_set_ctrl_hue(sensor, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		ret = ov5640_set_ctrl_contrast(sensor, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		ret = ov5640_set_ctrl_saturation(sensor, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov5640_set_ctrl_test_pattern(sensor, ctrl->val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov5640_ctrl_ops = {
+	.g_volatile_ctrl = ov5640_g_volatile_ctrl,
+	.s_ctrl = ov5640_s_ctrl,
+};
+
+static const char * const test_pattern_menu[] = {
+	"Disabled",
+	"Color bars",
+};
+
+static int ov5640_init_controls(struct ov5640_dev *sensor)
+{
+	const struct v4l2_ctrl_ops *ops = &ov5640_ctrl_ops;
+	struct ov5640_ctrls *ctrls = &sensor->ctrls;
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 32);
+
+	/* we can use our own mutex for the ctrl lock */
+	hdl->lock = &sensor->lock;
+
+	/* Auto/manual white balance */
+	ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
+					   V4L2_CID_AUTO_WHITE_BALANCE,
+					   0, 1, 1, 1);
+	ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+						0, 4095, 1, 0);
+	ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+					       0, 4095, 1, 0);
+	/* Auto/manual exposure */
+	ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
+						 V4L2_CID_EXPOSURE_AUTO,
+						 V4L2_EXPOSURE_MANUAL, 0,
+						 V4L2_EXPOSURE_AUTO);
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+					    0, 65535, 1, 0);
+	/* Auto/manual gain */
+	ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
+					     0, 1, 1, 1);
+	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
+					0, 1023, 1, 0);
+
+	ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
+					      0, 255, 1, 64);
+	ctrls->hue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HUE,
+				       0, 359, 1, 0);
+	ctrls->contrast = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST,
+					    0, 255, 1, 0);
+	ctrls->test_pattern =
+		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+					     ARRAY_SIZE(test_pattern_menu) - 1,
+					     0, 0, test_pattern_menu);
+
+	if (hdl->error) {
+		ret = hdl->error;
+		goto free_ctrls;
+	}
+
+	ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
+
+	sensor->sd.ctrl_handler = hdl;
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(hdl);
+	return ret;
+}
+
+static int ov5640_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->pad != 0)
+		return -EINVAL;
+	if (fse->index >= OV5640_NUM_MODES)
+		return -EINVAL;
+
+	fse->min_width = fse->max_width =
+		ov5640_mode_data[0][fse->index].width;
+	fse->min_height = fse->max_height =
+		ov5640_mode_data[0][fse->index].height;
+
+	return 0;
+}
+
+static int ov5640_enum_frame_interval(
+	struct v4l2_subdev *sd,
+	struct v4l2_subdev_pad_config *cfg,
+	struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	struct v4l2_fract tpf;
+	int ret;
+
+	if (fie->pad != 0)
+		return -EINVAL;
+	if (fie->index >= OV5640_NUM_FRAMERATES)
+		return -EINVAL;
+
+	tpf.numerator = 1;
+	tpf.denominator = ov5640_framerates[fie->index];
+
+	ret = ov5640_try_frame_interval(sensor, &tpf,
+					fie->width, fie->height);
+	if (ret < 0)
+		return -EINVAL;
+
+	fie->interval = tpf;
+	return 0;
+}
+
+static int ov5640_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+
+	mutex_lock(&sensor->lock);
+	fi->interval = sensor->frame_interval;
+	mutex_unlock(&sensor->lock);
+
+	return 0;
+}
+
+static int ov5640_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	const struct ov5640_mode_info *mode;
+	int frame_rate, ret = 0;
+
+	if (fi->pad != 0)
+		return -EINVAL;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	mode = sensor->current_mode;
+
+	frame_rate = ov5640_try_frame_interval(sensor, &fi->interval,
+					       mode->width, mode->height);
+	if (frame_rate < 0)
+		frame_rate = OV5640_15_FPS;
+
+	sensor->current_fr = frame_rate;
+	sensor->frame_interval = fi->interval;
+	sensor->pending_mode_change = true;
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static int ov5640_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+
+	if (code->pad != 0)
+		return -EINVAL;
+	if (code->index != 0)
+		return -EINVAL;
+
+	code->code = sensor->fmt.code;
+
+	return 0;
+}
+
+static int ov5640_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+	int ret = 0;
+
+	mutex_lock(&sensor->lock);
+
+	if (sensor->streaming == !enable) {
+		if (enable && sensor->pending_mode_change) {
+			ret = ov5640_set_mode(sensor, sensor->current_mode);
+			if (ret)
+				goto out;
+		}
+
+		ret = ov5640_set_stream(sensor, enable);
+		if (!ret)
+			sensor->streaming = enable;
+	}
+out:
+	mutex_unlock(&sensor->lock);
+	return ret;
+}
+
+static const struct v4l2_subdev_core_ops ov5640_core_ops = {
+	.s_power = ov5640_s_power,
+};
+
+static const struct v4l2_subdev_video_ops ov5640_video_ops = {
+	.g_frame_interval = ov5640_g_frame_interval,
+	.s_frame_interval = ov5640_s_frame_interval,
+	.s_stream = ov5640_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov5640_pad_ops = {
+	.enum_mbus_code = ov5640_enum_mbus_code,
+	.get_fmt = ov5640_get_fmt,
+	.set_fmt = ov5640_set_fmt,
+	.enum_frame_size = ov5640_enum_frame_size,
+	.enum_frame_interval = ov5640_enum_frame_interval,
+};
+
+static const struct v4l2_subdev_ops ov5640_subdev_ops = {
+	.core = &ov5640_core_ops,
+	.video = &ov5640_video_ops,
+	.pad = &ov5640_pad_ops,
+};
+
+static int ov5640_get_regulators(struct ov5640_dev *sensor)
+{
+	int i;
+
+	for (i = 0; i < OV5640_NUM_SUPPLIES; i++)
+		sensor->supplies[i].supply = ov5640_supply_name[i];
+
+	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
+				       OV5640_NUM_SUPPLIES,
+				       sensor->supplies);
+}
+
+static int ov5640_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	struct fwnode_handle *endpoint;
+	struct ov5640_dev *sensor;
+	int ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->i2c_client = client;
+	sensor->fmt.code = MEDIA_BUS_FMT_UYVY8_2X8;
+	sensor->fmt.width = 640;
+	sensor->fmt.height = 480;
+	sensor->fmt.field = V4L2_FIELD_NONE;
+	sensor->frame_interval.numerator = 1;
+	sensor->frame_interval.denominator = ov5640_framerates[OV5640_30_FPS];
+	sensor->current_fr = OV5640_30_FPS;
+	sensor->current_mode =
+		&ov5640_mode_data[OV5640_30_FPS][OV5640_MODE_VGA_640_480];
+	sensor->pending_mode_change = true;
+
+	sensor->ae_target = 52;
+
+	endpoint = fwnode_graph_get_next_endpoint(
+		of_fwnode_handle(client->dev.of_node), NULL);
+	if (!endpoint) {
+		dev_err(dev, "endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep);
+	fwnode_handle_put(endpoint);
+	if (ret) {
+		dev_err(dev, "Could not parse endpoint\n");
+		return ret;
+	}
+
+	if (sensor->ep.bus_type != V4L2_MBUS_CSI2) {
+		dev_err(dev, "invalid bus type, must be MIPI CSI2\n");
+		return -EINVAL;
+	}
+
+	/* get system clock (xclk) */
+	sensor->xclk = devm_clk_get(dev, "xclk");
+	if (IS_ERR(sensor->xclk)) {
+		dev_err(dev, "failed to get xclk\n");
+		return PTR_ERR(sensor->xclk);
+	}
+
+	sensor->xclk_freq = clk_get_rate(sensor->xclk);
+	if (sensor->xclk_freq < OV5640_XCLK_MIN ||
+	    sensor->xclk_freq > OV5640_XCLK_MAX) {
+		dev_err(dev, "xclk frequency out of range: %d Hz\n",
+			sensor->xclk_freq);
+		return -EINVAL;
+	}
+
+	/* request optional power down pin */
+	sensor->pwdn_gpio = devm_gpiod_get_optional(dev, "powerdown",
+						    GPIOD_OUT_HIGH);
+	/* request optional reset pin */
+	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						     GPIOD_OUT_HIGH);
+
+	v4l2_i2c_subdev_init(&sensor->sd, client, &ov5640_subdev_ops);
+
+	sensor->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+	if (ret)
+		return ret;
+
+	ret = ov5640_get_regulators(sensor);
+	if (ret)
+		return ret;
+
+	mutex_init(&sensor->lock);
+
+	ret = ov5640_init_controls(sensor);
+	if (ret)
+		goto entity_cleanup;
+
+	ret = v4l2_async_register_subdev(&sensor->sd);
+	if (ret)
+		goto free_ctrls;
+
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+entity_cleanup:
+	mutex_destroy(&sensor->lock);
+	media_entity_cleanup(&sensor->sd.entity);
+	return ret;
+}
+
+static int ov5640_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov5640_dev *sensor = to_ov5640_dev(sd);
+
+	v4l2_async_unregister_subdev(&sensor->sd);
+	mutex_destroy(&sensor->lock);
+	media_entity_cleanup(&sensor->sd.entity);
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+
+	return 0;
+}
+
+static const struct i2c_device_id ov5640_id[] = {
+	{"ov5640", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, ov5640_id);
+
+static const struct of_device_id ov5640_dt_ids[] = {
+	{ .compatible = "ovti,ov5640" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov5640_dt_ids);
+
+static struct i2c_driver ov5640_i2c_driver = {
+	.driver = {
+		.name  = "ov5640",
+		.of_match_table	= ov5640_dt_ids,
+	},
+	.id_table = ov5640_id,
+	.probe    = ov5640_probe,
+	.remove   = ov5640_remove,
+};
+
+module_i2c_driver(ov5640_i2c_driver);
+
+MODULE_DESCRIPTION("OV5640 MIPI Camera Subdev Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ov5645.c b/drivers/media/i2c/ov5645.c
index 57bd591ea54b..d1e844f7f03f 100644
--- a/drivers/media/i2c/ov5645.c
+++ b/drivers/media/i2c/ov5645.c
@@ -39,7 +39,7 @@
 #include <linux/slab.h>
 #include <linux/types.h>
 #include <media/v4l2-ctrls.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 #include <media/v4l2-subdev.h>
 
 #define OV5645_VOLTAGE_ANALOG               2800000
@@ -87,7 +87,7 @@ struct ov5645 {
 	struct device *dev;
 	struct v4l2_subdev sd;
 	struct media_pad pad;
-	struct v4l2_of_endpoint ep;
+	struct v4l2_fwnode_endpoint ep;
 	struct v4l2_mbus_framefmt fmt;
 	struct v4l2_rect crop;
 	struct clk *xclk;
@@ -1102,7 +1102,8 @@ static int ov5645_probe(struct i2c_client *client,
 		return -EINVAL;
 	}
 
-	ret = v4l2_of_parse_endpoint(endpoint, &ov5645->ep);
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint),
+					 &ov5645->ep);
 	if (ret < 0) {
 		dev_err(dev, "parsing endpoint node failed\n");
 		return ret;
diff --git a/drivers/media/i2c/ov5647.c b/drivers/media/i2c/ov5647.c
index f57a0b354cf6..95ce90fdb876 100644
--- a/drivers/media/i2c/ov5647.c
+++ b/drivers/media/i2c/ov5647.c
@@ -25,12 +25,13 @@
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/module.h>
+#include <linux/of_graph.h>
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
 #include <media/v4l2-image-sizes.h>
 #include <media/v4l2-mediabus.h>
-#include <media/v4l2-of.h>
 
 #define SENSOR_NAME "ov5647"
 
@@ -510,7 +511,7 @@ static const struct v4l2_subdev_internal_ops ov5647_subdev_internal_ops = {
 
 static int ov5647_parse_dt(struct device_node *np)
 {
-	struct v4l2_of_endpoint bus_cfg;
+	struct v4l2_fwnode_endpoint bus_cfg;
 	struct device_node *ep;
 
 	int ret;
@@ -519,7 +520,7 @@ static int ov5647_parse_dt(struct device_node *np)
 	if (!ep)
 		return -EINVAL;
 
-	ret = v4l2_of_parse_endpoint(ep, &bus_cfg);
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &bus_cfg);
 
 	of_node_put(ep);
 	return ret;
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-core.c b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
index 3844853ab0a0..f434fb2ee6fc 100644
--- a/drivers/media/i2c/s5c73m3/s5c73m3-core.c
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
@@ -24,6 +24,7 @@
 #include <linux/media.h>
 #include <linux/module.h>
 #include <linux/of_gpio.h>
+#include <linux/of_graph.h>
 #include <linux/regulator/consumer.h>
 #include <linux/sizes.h>
 #include <linux/slab.h>
@@ -35,7 +36,7 @@
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-mediabus.h>
 #include <media/i2c/s5c73m3.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 
 #include "s5c73m3.h"
 
@@ -1602,7 +1603,7 @@ static int s5c73m3_get_platform_data(struct s5c73m3 *state)
 	const struct s5c73m3_platform_data *pdata = dev->platform_data;
 	struct device_node *node = dev->of_node;
 	struct device_node *node_ep;
-	struct v4l2_of_endpoint ep;
+	struct v4l2_fwnode_endpoint ep;
 	int ret;
 
 	if (!node) {
@@ -1639,7 +1640,7 @@ static int s5c73m3_get_platform_data(struct s5c73m3 *state)
 		return 0;
 	}
 
-	ret = v4l2_of_parse_endpoint(node_ep, &ep);
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(node_ep), &ep);
 	of_node_put(node_ep);
 	if (ret)
 		return ret;
diff --git a/drivers/media/i2c/s5k5baf.c b/drivers/media/i2c/s5k5baf.c
index db82ed05792e..962051b9939d 100644
--- a/drivers/media/i2c/s5k5baf.c
+++ b/drivers/media/i2c/s5k5baf.c
@@ -30,7 +30,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-mediabus.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 
 static int debug;
 module_param(debug, int, 0644);
@@ -1841,7 +1841,7 @@ static int s5k5baf_parse_device_node(struct s5k5baf *state, struct device *dev)
 {
 	struct device_node *node = dev->of_node;
 	struct device_node *node_ep;
-	struct v4l2_of_endpoint ep;
+	struct v4l2_fwnode_endpoint ep;
 	int ret;
 
 	if (!node) {
@@ -1868,7 +1868,7 @@ static int s5k5baf_parse_device_node(struct s5k5baf *state, struct device *dev)
 		return -EINVAL;
 	}
 
-	ret = v4l2_of_parse_endpoint(node_ep, &ep);
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(node_ep), &ep);
 	of_node_put(node_ep);
 	if (ret)
 		return ret;
diff --git a/drivers/media/i2c/s5k6aa.c b/drivers/media/i2c/s5k6aa.c
index faee11383cb7..9fd254a8e20d 100644
--- a/drivers/media/i2c/s5k6aa.c
+++ b/drivers/media/i2c/s5k6aa.c
@@ -838,7 +838,7 @@ static int __s5k6aa_power_on(struct s5k6aa *s5k6aa)
 
 	if (s5k6aa->s_power)
 		ret = s5k6aa->s_power(1);
-	usleep_range(4000, 4000);
+	usleep_range(4000, 5000);
 
 	if (s5k6aa_gpio_deassert(s5k6aa, RST))
 		msleep(20);
diff --git a/drivers/media/i2c/smiapp/Kconfig b/drivers/media/i2c/smiapp/Kconfig
index 3149cda1d0db..f59718d8e51e 100644
--- a/drivers/media/i2c/smiapp/Kconfig
+++ b/drivers/media/i2c/smiapp/Kconfig
@@ -3,5 +3,6 @@ config VIDEO_SMIAPP
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && HAVE_CLK
 	depends on MEDIA_CAMERA_SUPPORT
 	select VIDEO_SMIAPP_PLL
+	select V4L2_FWNODE
 	---help---
 	  This is a generic driver for SMIA++/SMIA camera modules.
diff --git a/drivers/media/i2c/smiapp/smiapp-core.c b/drivers/media/i2c/smiapp/smiapp-core.c
index f4e92bdfe192..e0b0c032c4ac 100644
--- a/drivers/media/i2c/smiapp/smiapp-core.c
+++ b/drivers/media/i2c/smiapp/smiapp-core.c
@@ -27,12 +27,13 @@
 #include <linux/gpio/consumer.h>
 #include <linux/module.h>
 #include <linux/pm_runtime.h>
+#include <linux/property.h>
 #include <linux/regulator/consumer.h>
 #include <linux/slab.h>
 #include <linux/smiapp.h>
 #include <linux/v4l2-mediabus.h>
+#include <media/v4l2-fwnode.h>
 #include <media/v4l2-device.h>
-#include <media/v4l2-of.h>
 
 #include "smiapp.h"
 
@@ -2784,19 +2785,20 @@ static int __maybe_unused smiapp_resume(struct device *dev)
 static struct smiapp_hwconfig *smiapp_get_hwconfig(struct device *dev)
 {
 	struct smiapp_hwconfig *hwcfg;
-	struct v4l2_of_endpoint *bus_cfg;
-	struct device_node *ep;
+	struct v4l2_fwnode_endpoint *bus_cfg;
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
 	int i;
 	int rval;
 
-	if (!dev->of_node)
+	if (!fwnode)
 		return dev->platform_data;
 
-	ep = of_graph_get_next_endpoint(dev->of_node, NULL);
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
 	if (!ep)
 		return NULL;
 
-	bus_cfg = v4l2_of_alloc_parse_endpoint(ep);
+	bus_cfg = v4l2_fwnode_endpoint_alloc_parse(ep);
 	if (IS_ERR(bus_cfg))
 		goto out_err;
 
@@ -2817,11 +2819,10 @@ static struct smiapp_hwconfig *smiapp_get_hwconfig(struct device *dev)
 	dev_dbg(dev, "lanes %u\n", hwcfg->lanes);
 
 	/* NVM size is not mandatory */
-	of_property_read_u32(dev->of_node, "nokia,nvm-size",
-				    &hwcfg->nvm_size);
+	fwnode_property_read_u32(fwnode, "nokia,nvm-size", &hwcfg->nvm_size);
 
-	rval = of_property_read_u32(dev->of_node, "clock-frequency",
-				    &hwcfg->ext_clk);
+	rval = fwnode_property_read_u32(fwnode, "clock-frequency",
+					&hwcfg->ext_clk);
 	if (rval) {
 		dev_warn(dev, "can't get clock-frequency\n");
 		goto out_err;
@@ -2846,13 +2847,13 @@ static struct smiapp_hwconfig *smiapp_get_hwconfig(struct device *dev)
 		dev_dbg(dev, "freq %d: %lld\n", i, hwcfg->op_sys_clock[i]);
 	}
 
-	v4l2_of_free_endpoint(bus_cfg);
-	of_node_put(ep);
+	v4l2_fwnode_endpoint_free(bus_cfg);
+	fwnode_handle_put(ep);
 	return hwcfg;
 
 out_err:
-	v4l2_of_free_endpoint(bus_cfg);
-	of_node_put(ep);
+	v4l2_fwnode_endpoint_free(bus_cfg);
+	fwnode_handle_put(ep);
 	return NULL;
 }
 
diff --git a/drivers/media/i2c/soc_camera/ov6650.c b/drivers/media/i2c/soc_camera/ov6650.c
index dbd6d92c589f..d2be64d54b22 100644
--- a/drivers/media/i2c/soc_camera/ov6650.c
+++ b/drivers/media/i2c/soc_camera/ov6650.c
@@ -709,6 +709,7 @@ static int ov6650_set_fmt(struct v4l2_subdev *sd,
 	switch (mf->code) {
 	case MEDIA_BUS_FMT_Y10_1X10:
 		mf->code = MEDIA_BUS_FMT_Y8_1X8;
+		/* fall through */
 	case MEDIA_BUS_FMT_Y8_1X8:
 	case MEDIA_BUS_FMT_YVYU8_2X8:
 	case MEDIA_BUS_FMT_YUYV8_2X8:
@@ -718,6 +719,7 @@ static int ov6650_set_fmt(struct v4l2_subdev *sd,
 		break;
 	default:
 		mf->code = MEDIA_BUS_FMT_SBGGR8_1X8;
+		/* fall through */
 	case MEDIA_BUS_FMT_SBGGR8_1X8:
 		mf->colorspace = V4L2_COLORSPACE_SRGB;
 		break;
diff --git a/drivers/media/i2c/soc_camera/ov772x.c b/drivers/media/i2c/soc_camera/ov772x.c
index 0f7b9d1b9c57..806383500313 100644
--- a/drivers/media/i2c/soc_camera/ov772x.c
+++ b/drivers/media/i2c/soc_camera/ov772x.c
@@ -1047,11 +1047,13 @@ static int ov772x_probe(struct i2c_client *client,
 		return -EINVAL;
 	}
 
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA |
+					      I2C_FUNC_PROTOCOL_MANGLING)) {
 		dev_err(&adapter->dev,
-			"I2C-Adapter doesn't support I2C_FUNC_SMBUS_BYTE_DATA\n");
+			"I2C-Adapter doesn't support SMBUS_BYTE_DATA or PROTOCOL_MANGLING\n");
 		return -EIO;
 	}
+	client->flags |= I2C_CLIENT_SCCB;
 
 	priv = devm_kzalloc(&client->dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
diff --git a/drivers/media/i2c/tc358743.c b/drivers/media/i2c/tc358743.c
index acef4eca269f..5788af238b86 100644
--- a/drivers/media/i2c/tc358743.c
+++ b/drivers/media/i2c/tc358743.c
@@ -33,6 +33,8 @@
 #include <linux/delay.h>
 #include <linux/gpio/consumer.h>
 #include <linux/interrupt.h>
+#include <linux/timer.h>
+#include <linux/of_graph.h>
 #include <linux/videodev2.h>
 #include <linux/workqueue.h>
 #include <linux/v4l2-dv-timings.h>
@@ -41,7 +43,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-event.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 #include <media/i2c/tc358743.h>
 
 #include "tc358743_regs.h"
@@ -61,6 +63,8 @@ MODULE_LICENSE("GPL");
 
 #define I2C_MAX_XFER_SIZE  (EDID_BLOCK_SIZE + 2)
 
+#define POLL_INTERVAL_MS	1000
+
 static const struct v4l2_dv_timings_cap tc358743_timings_cap = {
 	.type = V4L2_DV_BT_656_1120,
 	/* keep this initialization for compatibility with GCC < 4.4.6 */
@@ -76,7 +80,7 @@ static const struct v4l2_dv_timings_cap tc358743_timings_cap = {
 
 struct tc358743_state {
 	struct tc358743_platform_data pdata;
-	struct v4l2_of_bus_mipi_csi2 bus;
+	struct v4l2_fwnode_bus_mipi_csi2 bus;
 	struct v4l2_subdev sd;
 	struct media_pad pad;
 	struct v4l2_ctrl_handler hdl;
@@ -91,6 +95,9 @@ struct tc358743_state {
 
 	struct delayed_work delayed_work_enable_hotplug;
 
+	struct timer_list timer;
+	struct work_struct work_i2c_poll;
+
 	/* edid  */
 	u8 edid_blocks_written;
 
@@ -223,7 +230,7 @@ static void i2c_wr8(struct v4l2_subdev *sd, u16 reg, u8 val)
 static void i2c_wr8_and_or(struct v4l2_subdev *sd, u16 reg,
 		u8 mask, u8 val)
 {
-	i2c_wrreg(sd, reg, (i2c_rdreg(sd, reg, 2) & mask) | val, 2);
+	i2c_wrreg(sd, reg, (i2c_rdreg(sd, reg, 1) & mask) | val, 1);
 }
 
 static u16 i2c_rd16(struct v4l2_subdev *sd, u16 reg)
@@ -1296,7 +1303,6 @@ static int tc358743_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 			tc358743_csi_err_int_handler(sd, handled);
 
 		i2c_wr16(sd, INTSTATUS, MASK_CSI_INT);
-		intstatus &= ~MASK_CSI_INT;
 	}
 
 	intstatus = i2c_rd16(sd, INTSTATUS);
@@ -1319,6 +1325,24 @@ static irqreturn_t tc358743_irq_handler(int irq, void *dev_id)
 	return handled ? IRQ_HANDLED : IRQ_NONE;
 }
 
+static void tc358743_irq_poll_timer(unsigned long arg)
+{
+	struct tc358743_state *state = (struct tc358743_state *)arg;
+
+	schedule_work(&state->work_i2c_poll);
+
+	mod_timer(&state->timer, jiffies + msecs_to_jiffies(POLL_INTERVAL_MS));
+}
+
+static void tc358743_work_i2c_poll(struct work_struct *work)
+{
+	struct tc358743_state *state = container_of(work,
+			struct tc358743_state, work_i2c_poll);
+	bool handled;
+
+	tc358743_isr(&state->sd, 0, &handled);
+}
+
 static int tc358743_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
 				    struct v4l2_event_subscription *sub)
 {
@@ -1473,6 +1497,23 @@ static int tc358743_s_stream(struct v4l2_subdev *sd, int enable)
 
 /* --------------- PAD OPS --------------- */
 
+static int tc358743_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	switch (code->index) {
+	case 0:
+		code->code = MEDIA_BUS_FMT_RGB888_1X24;
+		break;
+	case 1:
+		code->code = MEDIA_BUS_FMT_UYVY8_1X16;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
 static int tc358743_get_fmt(struct v4l2_subdev *sd,
 		struct v4l2_subdev_pad_config *cfg,
 		struct v4l2_subdev_format *format)
@@ -1642,6 +1683,7 @@ static const struct v4l2_subdev_video_ops tc358743_video_ops = {
 };
 
 static const struct v4l2_subdev_pad_ops tc358743_pad_ops = {
+	.enum_mbus_code = tc358743_enum_mbus_code,
 	.set_fmt = tc358743_set_fmt,
 	.get_fmt = tc358743_get_fmt,
 	.get_edid = tc358743_g_edid,
@@ -1695,7 +1737,7 @@ static void tc358743_gpio_reset(struct tc358743_state *state)
 static int tc358743_probe_of(struct tc358743_state *state)
 {
 	struct device *dev = &state->i2c_client->dev;
-	struct v4l2_of_endpoint *endpoint;
+	struct v4l2_fwnode_endpoint *endpoint;
 	struct device_node *ep;
 	struct clk *refclk;
 	u32 bps_pr_lane;
@@ -1715,7 +1757,7 @@ static int tc358743_probe_of(struct tc358743_state *state)
 		return -EINVAL;
 	}
 
-	endpoint = v4l2_of_alloc_parse_endpoint(ep);
+	endpoint = v4l2_fwnode_endpoint_alloc_parse(of_fwnode_handle(ep));
 	if (IS_ERR(endpoint)) {
 		dev_err(dev, "failed to parse endpoint\n");
 		return PTR_ERR(endpoint);
@@ -1730,7 +1772,11 @@ static int tc358743_probe_of(struct tc358743_state *state)
 
 	state->bus = endpoint->bus.mipi_csi2;
 
-	clk_prepare_enable(refclk);
+	ret = clk_prepare_enable(refclk);
+	if (ret) {
+		dev_err(dev, "Failed! to enable clock\n");
+		goto free_endpoint;
+	}
 
 	state->pdata.refclk_hz = clk_get_rate(refclk);
 	state->pdata.ddc5v_delay = DDC5V_DELAY_100_MS;
@@ -1803,7 +1849,7 @@ static int tc358743_probe_of(struct tc358743_state *state)
 disable_clk:
 	clk_disable_unprepare(refclk);
 free_endpoint:
-	v4l2_of_free_endpoint(endpoint);
+	v4l2_fwnode_endpoint_free(endpoint);
 	return ret;
 }
 #else
@@ -1887,6 +1933,8 @@ static int tc358743_probe(struct i2c_client *client,
 	if (err < 0)
 		goto err_hdl;
 
+	state->mbus_fmt_code = MEDIA_BUS_FMT_RGB888_1X24;
+
 	sd->dev = &client->dev;
 	err = v4l2_async_register_subdev(sd);
 	if (err < 0)
@@ -1901,7 +1949,6 @@ static int tc358743_probe(struct i2c_client *client,
 
 	tc358743_s_dv_timings(sd, &default_timing);
 
-	state->mbus_fmt_code = MEDIA_BUS_FMT_RGB888_1X24;
 	tc358743_set_csi_color_space(sd);
 
 	tc358743_init_interrupts(sd);
@@ -1914,6 +1961,14 @@ static int tc358743_probe(struct i2c_client *client,
 						"tc358743", state);
 		if (err)
 			goto err_work_queues;
+	} else {
+		INIT_WORK(&state->work_i2c_poll,
+			  tc358743_work_i2c_poll);
+		state->timer.data = (unsigned long)state;
+		state->timer.function = tc358743_irq_poll_timer;
+		state->timer.expires = jiffies +
+				       msecs_to_jiffies(POLL_INTERVAL_MS);
+		add_timer(&state->timer);
 	}
 
 	tc358743_enable_interrupts(sd, tx_5v_power_present(sd));
@@ -1929,6 +1984,8 @@ static int tc358743_probe(struct i2c_client *client,
 	return 0;
 
 err_work_queues:
+	if (!state->i2c_client->irq)
+		flush_work(&state->work_i2c_poll);
 	cancel_delayed_work(&state->delayed_work_enable_hotplug);
 	mutex_destroy(&state->confctl_mutex);
 err_hdl:
@@ -1942,6 +1999,10 @@ static int tc358743_remove(struct i2c_client *client)
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	struct tc358743_state *state = to_state(sd);
 
+	if (!state->i2c_client->irq) {
+		del_timer_sync(&state->timer);
+		flush_work(&state->work_i2c_poll);
+	}
 	cancel_delayed_work(&state->delayed_work_enable_hotplug);
 	v4l2_async_unregister_subdev(sd);
 	v4l2_device_unregister_subdev(sd);
diff --git a/drivers/media/i2c/tvp514x.c b/drivers/media/i2c/tvp514x.c
index 07853d2252aa..ad2df998f9c5 100644
--- a/drivers/media/i2c/tvp514x.c
+++ b/drivers/media/i2c/tvp514x.c
@@ -38,7 +38,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-mediabus.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 #include <media/v4l2-ctrls.h>
 #include <media/i2c/tvp514x.h>
 #include <media/media-entity.h>
@@ -998,7 +998,7 @@ static struct tvp514x_platform_data *
 tvp514x_get_pdata(struct i2c_client *client)
 {
 	struct tvp514x_platform_data *pdata = NULL;
-	struct v4l2_of_endpoint bus_cfg;
+	struct v4l2_fwnode_endpoint bus_cfg;
 	struct device_node *endpoint;
 	unsigned int flags;
 
@@ -1009,7 +1009,7 @@ tvp514x_get_pdata(struct i2c_client *client)
 	if (!endpoint)
 		return NULL;
 
-	if (v4l2_of_parse_endpoint(endpoint, &bus_cfg))
+	if (v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint), &bus_cfg))
 		goto done;
 
 	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
diff --git a/drivers/media/i2c/tvp5150.c b/drivers/media/i2c/tvp5150.c
index 04e96b3057bb..9da4bf4f2c7a 100644
--- a/drivers/media/i2c/tvp5150.c
+++ b/drivers/media/i2c/tvp5150.c
@@ -12,10 +12,11 @@
 #include <linux/delay.h>
 #include <linux/gpio/consumer.h>
 #include <linux/module.h>
+#include <linux/of_graph.h>
 #include <media/v4l2-async.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 #include <media/v4l2-mc.h>
 
 #include "tvp5150_reg.h"
@@ -1358,7 +1359,7 @@ static int tvp5150_init(struct i2c_client *c)
 
 static int tvp5150_parse_dt(struct tvp5150 *decoder, struct device_node *np)
 {
-	struct v4l2_of_endpoint bus_cfg;
+	struct v4l2_fwnode_endpoint bus_cfg;
 	struct device_node *ep;
 #ifdef CONFIG_MEDIA_CONTROLLER
 	struct device_node *connectors, *child;
@@ -1373,7 +1374,7 @@ static int tvp5150_parse_dt(struct tvp5150 *decoder, struct device_node *np)
 	if (!ep)
 		return -EINVAL;
 
-	ret = v4l2_of_parse_endpoint(ep, &bus_cfg);
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &bus_cfg);
 	if (ret)
 		goto err;
 
diff --git a/drivers/media/i2c/tvp7002.c b/drivers/media/i2c/tvp7002.c
index 4c1190127c85..a26c1a3f7183 100644
--- a/drivers/media/i2c/tvp7002.c
+++ b/drivers/media/i2c/tvp7002.c
@@ -33,7 +33,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 
 #include "tvp7002_reg.h"
 
@@ -889,7 +889,7 @@ static const struct v4l2_subdev_ops tvp7002_ops = {
 static struct tvp7002_config *
 tvp7002_get_pdata(struct i2c_client *client)
 {
-	struct v4l2_of_endpoint bus_cfg;
+	struct v4l2_fwnode_endpoint bus_cfg;
 	struct tvp7002_config *pdata = NULL;
 	struct device_node *endpoint;
 	unsigned int flags;
@@ -901,7 +901,7 @@ tvp7002_get_pdata(struct i2c_client *client)
 	if (!endpoint)
 		return NULL;
 
-	if (v4l2_of_parse_endpoint(endpoint, &bus_cfg))
+	if (v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint), &bus_cfg))
 		goto done;
 
 	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
diff --git a/drivers/media/media-entity.c b/drivers/media/media-entity.c
index bc44193efa47..dd0f0ead9516 100644
--- a/drivers/media/media-entity.c
+++ b/drivers/media/media-entity.c
@@ -18,6 +18,7 @@
 
 #include <linux/bitmap.h>
 #include <linux/module.h>
+#include <linux/property.h>
 #include <linux/slab.h>
 #include <media/media-entity.h>
 #include <media/media-device.h>
@@ -386,6 +387,41 @@ struct media_entity *media_graph_walk_next(struct media_graph *graph)
 }
 EXPORT_SYMBOL_GPL(media_graph_walk_next);
 
+int media_entity_get_fwnode_pad(struct media_entity *entity,
+				struct fwnode_handle *fwnode,
+				unsigned long direction_flags)
+{
+	struct fwnode_endpoint endpoint;
+	unsigned int i;
+	int ret;
+
+	if (!entity->ops || !entity->ops->get_fwnode_pad) {
+		for (i = 0; i < entity->num_pads; i++) {
+			if (entity->pads[i].flags & direction_flags)
+				return i;
+		}
+
+		return -ENXIO;
+	}
+
+	ret = fwnode_graph_parse_endpoint(fwnode, &endpoint);
+	if (ret)
+		return ret;
+
+	ret = entity->ops->get_fwnode_pad(&endpoint);
+	if (ret < 0)
+		return ret;
+
+	if (ret >= entity->num_pads)
+		return -ENXIO;
+
+	if (!(entity->pads[ret].flags & direction_flags))
+		return -ENXIO;
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(media_entity_get_fwnode_pad);
+
 /* -----------------------------------------------------------------------------
  * Pipeline management
  */
@@ -530,8 +566,13 @@ void __media_pipeline_stop(struct media_entity *entity)
 	struct media_graph *graph = &entity->pipe->graph;
 	struct media_pipeline *pipe = entity->pipe;
 
+	/*
+	 * If the following check fails, the driver has performed an
+	 * unbalanced call to media_pipeline_stop()
+	 */
+	if (WARN_ON(!pipe))
+		return;
 
-	WARN_ON(!pipe->streaming_count);
 	media_graph_walk_start(graph, entity);
 
 	while ((entity = media_graph_walk_next(graph))) {
diff --git a/drivers/media/pci/bt8xx/dst_ca.c b/drivers/media/pci/bt8xx/dst_ca.c
index 04d06c564602..90f4263452d3 100644
--- a/drivers/media/pci/bt8xx/dst_ca.c
+++ b/drivers/media/pci/bt8xx/dst_ca.c
@@ -637,6 +637,7 @@ static long dst_ca_ioctl(struct file *file, unsigned int cmd, unsigned long ioct
 			goto free_mem_and_exit;
 		}
 		dprintk(verbose, DST_CA_INFO, 1, " -->CA_SET_PID Success !");
+		break;
 	default:
 		result = -EOPNOTSUPP;
 	}
diff --git a/drivers/media/pci/cobalt/cobalt-driver.c b/drivers/media/pci/cobalt/cobalt-driver.c
index d5c911c09e2b..f8e173f3e9e2 100644
--- a/drivers/media/pci/cobalt/cobalt-driver.c
+++ b/drivers/media/pci/cobalt/cobalt-driver.c
@@ -205,6 +205,8 @@ void cobalt_pcie_status_show(struct cobalt *cobalt)
 
 	offset = pci_find_capability(pci_dev, PCI_CAP_ID_EXP);
 	bus_offset = pci_find_capability(pci_bus_dev, PCI_CAP_ID_EXP);
+	if (!offset || !bus_offset)
+		return;
 
 	/* Device */
 	pci_read_config_dword(pci_dev, offset + PCI_EXP_DEVCAP, &capa);
diff --git a/drivers/media/pci/cx18/cx18-alsa-pcm.c b/drivers/media/pci/cx18/cx18-alsa-pcm.c
index 205a98da877c..f68ee57a9ae2 100644
--- a/drivers/media/pci/cx18/cx18-alsa-pcm.c
+++ b/drivers/media/pci/cx18/cx18-alsa-pcm.c
@@ -257,14 +257,16 @@ static int snd_cx18_pcm_hw_free(struct snd_pcm_substream *substream)
 {
 	struct snd_cx18_card *cxsc = snd_pcm_substream_chip(substream);
 	unsigned long flags;
+	unsigned char *dma_area = NULL;
 
 	spin_lock_irqsave(&cxsc->slock, flags);
 	if (substream->runtime->dma_area) {
 		dprintk("freeing pcm capture region\n");
-		vfree(substream->runtime->dma_area);
+		dma_area = substream->runtime->dma_area;
 		substream->runtime->dma_area = NULL;
 	}
 	spin_unlock_irqrestore(&cxsc->slock, flags);
+	vfree(dma_area);
 
 	return 0;
 }
diff --git a/drivers/media/pci/cx18/cx18-dvb.c b/drivers/media/pci/cx18/cx18-dvb.c
index d130d65828b0..53f4d6bf81fb 100644
--- a/drivers/media/pci/cx18/cx18-dvb.c
+++ b/drivers/media/pci/cx18/cx18-dvb.c
@@ -151,7 +151,7 @@ static int yuan_mpc718_mt352_reqfw(struct cx18_stream *stream,
 	}
 
 	if (ret) {
-		CX18_ERR("The MPC718 board variant with the MT352 DVB-Tdemodualtor will not work without it\n");
+		CX18_ERR("The MPC718 board variant with the MT352 DVB-T demodulator will not work without it\n");
 		CX18_ERR("Run 'linux/Documentation/dvb/get_dvb_firmware mpc718' if you need the firmware\n");
 	}
 	return ret;
diff --git a/drivers/media/pci/cx23885/cx23885-cards.c b/drivers/media/pci/cx23885/cx23885-cards.c
index 9e39aea85df6..c48fa8e25a70 100644
--- a/drivers/media/pci/cx23885/cx23885-cards.c
+++ b/drivers/media/pci/cx23885/cx23885-cards.c
@@ -2081,7 +2081,7 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 		ts2->gen_ctrl_val  = 0xc; /* Serial bus + punctured clock */
 		ts2->ts_clk_en_val = 0x1; /* Enable TS_CLK */
 		ts2->src_sel_val   = CX23885_SRC_SEL_PARALLEL_MPEG_VIDEO;
-		/* break omitted intentionally */
+		/* fall-through */
 	case CX23885_BOARD_DVICO_FUSIONHDTV_5_EXP:
 		ts1->gen_ctrl_val  = 0xc; /* Serial bus + punctured clock */
 		ts1->ts_clk_en_val = 0x1; /* Enable TS_CLK */
@@ -2238,6 +2238,7 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 		/* Currently only enabled for the integrated IR controller */
 		if (!enable_885_ir)
 			break;
+		/* fall-through */
 	case CX23885_BOARD_HAUPPAUGE_HVR1250:
 	case CX23885_BOARD_HAUPPAUGE_HVR1800:
 	case CX23885_BOARD_HAUPPAUGE_IMPACTVCBE:
diff --git a/drivers/media/pci/cx88/cx88-cards.c b/drivers/media/pci/cx88/cx88-cards.c
index 73cc7a67a8bc..6df21b29ea17 100644
--- a/drivers/media/pci/cx88/cx88-cards.c
+++ b/drivers/media/pci/cx88/cx88-cards.c
@@ -3681,7 +3681,14 @@ struct cx88_core *cx88_core_create(struct pci_dev *pci, int nr)
 	core->nr = nr;
 	sprintf(core->name, "cx88[%d]", core->nr);
 
-	core->tvnorm = V4L2_STD_NTSC_M;
+	/*
+	 * Note: Setting initial standard here would cause first call to
+	 * cx88_set_tvnorm() to return without programming any registers.  Leave
+	 * it blank for at this point and it will get set later in
+	 * cx8800_initdev()
+	 */
+	core->tvnorm  = 0;
+
 	core->width   = 320;
 	core->height  = 240;
 	core->field   = V4L2_FIELD_INTERLACED;
diff --git a/drivers/media/pci/cx88/cx88-video.c b/drivers/media/pci/cx88/cx88-video.c
index c7d4e87ccb64..7d25ecd4404b 100644
--- a/drivers/media/pci/cx88/cx88-video.c
+++ b/drivers/media/pci/cx88/cx88-video.c
@@ -1420,7 +1420,7 @@ static int cx8800_initdev(struct pci_dev *pci_dev,
 		request_module("rtc-isl1208");
 		core->i2c_rtc = i2c_new_device(&core->i2c_adap, &rtc_info);
 	}
-		/* break intentionally omitted */
+		/* fall-through */
 	case CX88_BOARD_DVICO_FUSIONHDTV_5_PCI_NANO:
 		request_module("ir-kbd-i2c");
 	}
@@ -1435,7 +1435,7 @@ static int cx8800_initdev(struct pci_dev *pci_dev,
 
 	/* initial device configuration */
 	mutex_lock(&core->lock);
-	cx88_set_tvnorm(core, core->tvnorm);
+	cx88_set_tvnorm(core, V4L2_STD_NTSC_M);
 	v4l2_ctrl_handler_setup(&core->video_hdl);
 	v4l2_ctrl_handler_setup(&core->audio_hdl);
 	cx88_video_mux(core, 0);
diff --git a/drivers/media/pci/ddbridge/Kconfig b/drivers/media/pci/ddbridge/Kconfig
index 44e5dc15e60a..ffed78c2ffb4 100644
--- a/drivers/media/pci/ddbridge/Kconfig
+++ b/drivers/media/pci/ddbridge/Kconfig
@@ -6,6 +6,9 @@ config DVB_DDBRIDGE
 	select DVB_STV090x if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_DRXK if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_TDA18271C2DD if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV0367 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_CXD2841ER if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_TDA18212 if MEDIA_SUBDRV_AUTOSELECT
 	---help---
 	  Support for cards with the Digital Devices PCI express bridge:
 	  - Octopus PCIe Bridge
@@ -14,5 +17,8 @@ config DVB_DDBRIDGE
 	  - DuoFlex S2 Octopus
 	  - DuoFlex CT Octopus
 	  - cineS2(v6)
+	  - CineCTv6 and DuoFlex CT (STV0367-based)
+	  - CineCTv7 and DuoFlex CT2/C2T2/C2T2I (Sony CXD28xx-based)
+	  - MaxA8 series
 
 	  Say Y if you own such a card and want to use it.
diff --git a/drivers/media/pci/ddbridge/ddbridge-core.c b/drivers/media/pci/ddbridge/ddbridge-core.c
index 340cff02dee2..9420479bee9a 100644
--- a/drivers/media/pci/ddbridge/ddbridge-core.c
+++ b/drivers/media/pci/ddbridge/ddbridge-core.c
@@ -39,6 +39,14 @@
 #include "stv090x.h"
 #include "lnbh24.h"
 #include "drxk.h"
+#include "stv0367.h"
+#include "stv0367_priv.h"
+#include "cxd2841er.h"
+#include "tda18212.h"
+
+static int xo2_speed = 2;
+module_param(xo2_speed, int, 0444);
+MODULE_PARM_DESC(xo2_speed, "default transfer speed for xo2 based duoflex, 0=55,1=75,2=90,3=104 MBit/s, default=2, use attribute to change for individual cards");
 
 DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
@@ -47,6 +55,24 @@ DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
 /******************************************************************************/
 
+static int i2c_io(struct i2c_adapter *adapter, u8 adr,
+		  u8 *wbuf, u32 wlen, u8 *rbuf, u32 rlen)
+{
+	struct i2c_msg msgs[2] = {{.addr = adr,  .flags = 0,
+				   .buf  = wbuf, .len   = wlen },
+				  {.addr = adr,  .flags = I2C_M_RD,
+				   .buf  = rbuf,  .len   = rlen } };
+	return (i2c_transfer(adapter, msgs, 2) == 2) ? 0 : -1;
+}
+
+static int i2c_write(struct i2c_adapter *adap, u8 adr, u8 *data, int len)
+{
+	struct i2c_msg msg = {.addr = adr, .flags = 0,
+			      .buf = data, .len = len};
+
+	return (i2c_transfer(adap, &msg, 1) == 1) ? 0 : -1;
+}
+
 static int i2c_read(struct i2c_adapter *adapter, u8 adr, u8 *val)
 {
 	struct i2c_msg msgs[1] = {{.addr = adr,  .flags = I2C_M_RD,
@@ -54,15 +80,21 @@ static int i2c_read(struct i2c_adapter *adapter, u8 adr, u8 *val)
 	return (i2c_transfer(adapter, msgs, 1) == 1) ? 0 : -1;
 }
 
-static int i2c_read_reg(struct i2c_adapter *adapter, u8 adr, u8 reg, u8 *val)
+static int i2c_read_regs(struct i2c_adapter *adapter,
+			 u8 adr, u8 reg, u8 *val, u8 len)
 {
 	struct i2c_msg msgs[2] = {{.addr = adr,  .flags = 0,
 				   .buf  = &reg, .len   = 1 },
 				  {.addr = adr,  .flags = I2C_M_RD,
-				   .buf  = val,  .len   = 1 } };
+				   .buf  = val,  .len   = len } };
 	return (i2c_transfer(adapter, msgs, 2) == 2) ? 0 : -1;
 }
 
+static int i2c_read_reg(struct i2c_adapter *adapter, u8 adr, u8 reg, u8 *val)
+{
+	return i2c_read_regs(adapter, adr, reg, val, 1);
+}
+
 static int i2c_read_reg16(struct i2c_adapter *adapter, u8 adr,
 			  u16 reg, u8 *val)
 {
@@ -74,6 +106,14 @@ static int i2c_read_reg16(struct i2c_adapter *adapter, u8 adr,
 	return (i2c_transfer(adapter, msgs, 2) == 2) ? 0 : -1;
 }
 
+static int i2c_write_reg(struct i2c_adapter *adap, u8 adr,
+			 u8 reg, u8 val)
+{
+	u8 msg[2] = {reg, val};
+
+	return i2c_write(adap, adr, msg, 2);
+}
+
 static int ddb_i2c_cmd(struct ddb_i2c *i2c, u32 adr, u32 cmd)
 {
 	struct ddb *dev = i2c->dev;
@@ -609,6 +649,151 @@ static int tuner_attach_tda18271(struct ddb_input *input)
 /******************************************************************************/
 /******************************************************************************/
 
+static struct stv0367_config ddb_stv0367_config[] = {
+	{
+		.demod_address = 0x1f,
+		.xtal = 27000000,
+		.if_khz = 0,
+		.if_iq_mode = FE_TER_NORMAL_IF_TUNER,
+		.ts_mode = STV0367_SERIAL_PUNCT_CLOCK,
+		.clk_pol = STV0367_CLOCKPOLARITY_DEFAULT,
+	}, {
+		.demod_address = 0x1e,
+		.xtal = 27000000,
+		.if_khz = 0,
+		.if_iq_mode = FE_TER_NORMAL_IF_TUNER,
+		.ts_mode = STV0367_SERIAL_PUNCT_CLOCK,
+		.clk_pol = STV0367_CLOCKPOLARITY_DEFAULT,
+	},
+};
+
+static int demod_attach_stv0367(struct ddb_input *input)
+{
+	struct i2c_adapter *i2c = &input->port->i2c->adap;
+
+	/* attach frontend */
+	input->fe = dvb_attach(stv0367ddb_attach,
+		&ddb_stv0367_config[(input->nr & 1)], i2c);
+
+	if (!input->fe) {
+		printk(KERN_ERR "stv0367ddb_attach failed (not found?)\n");
+		return -ENODEV;
+	}
+
+	input->fe->sec_priv = input;
+	input->gate_ctrl = input->fe->ops.i2c_gate_ctrl;
+	input->fe->ops.i2c_gate_ctrl = drxk_gate_ctrl;
+
+	return 0;
+}
+
+static int tuner_tda18212_ping(struct ddb_input *input, unsigned short adr)
+{
+	struct i2c_adapter *adapter = &input->port->i2c->adap;
+	u8 tda_id[2];
+	u8 subaddr = 0x00;
+
+	printk(KERN_DEBUG "stv0367-tda18212 tuner ping\n");
+	if (input->fe->ops.i2c_gate_ctrl)
+		input->fe->ops.i2c_gate_ctrl(input->fe, 1);
+
+	if (i2c_read_regs(adapter, adr, subaddr, tda_id, sizeof(tda_id)) < 0)
+		printk(KERN_DEBUG "tda18212 ping 1 fail\n");
+	if (i2c_read_regs(adapter, adr, subaddr, tda_id, sizeof(tda_id)) < 0)
+		printk(KERN_DEBUG "tda18212 ping 2 fail\n");
+
+	if (input->fe->ops.i2c_gate_ctrl)
+		input->fe->ops.i2c_gate_ctrl(input->fe, 0);
+
+	return 0;
+}
+
+static int demod_attach_cxd28xx(struct ddb_input *input, int par, int osc24)
+{
+	struct i2c_adapter *i2c = &input->port->i2c->adap;
+	struct cxd2841er_config cfg;
+
+	/* the cxd2841er driver expects 8bit/shifted I2C addresses */
+	cfg.i2c_addr = ((input->nr & 1) ? 0x6d : 0x6c) << 1;
+
+	cfg.xtal = osc24 ? SONY_XTAL_24000 : SONY_XTAL_20500;
+	cfg.flags = CXD2841ER_AUTO_IFHZ | CXD2841ER_EARLY_TUNE |
+		CXD2841ER_NO_WAIT_LOCK | CXD2841ER_NO_AGCNEG |
+		CXD2841ER_TSBITS;
+
+	if (!par)
+		cfg.flags |= CXD2841ER_TS_SERIAL;
+
+	/* attach frontend */
+	input->fe = dvb_attach(cxd2841er_attach_t_c, &cfg, i2c);
+
+	if (!input->fe) {
+		printk(KERN_ERR "No Sony CXD28xx found!\n");
+		return -ENODEV;
+	}
+
+	input->fe->sec_priv = input;
+	input->gate_ctrl = input->fe->ops.i2c_gate_ctrl;
+	input->fe->ops.i2c_gate_ctrl = drxk_gate_ctrl;
+
+	return 0;
+}
+
+static int tuner_attach_tda18212(struct ddb_input *input, u32 porttype)
+{
+	struct i2c_adapter *adapter = &input->port->i2c->adap;
+	struct i2c_client *client;
+	struct tda18212_config config = {
+		.fe = input->fe,
+		.if_dvbt_6 = 3550,
+		.if_dvbt_7 = 3700,
+		.if_dvbt_8 = 4150,
+		.if_dvbt2_6 = 3250,
+		.if_dvbt2_7 = 4000,
+		.if_dvbt2_8 = 4000,
+		.if_dvbc = 5000,
+	};
+	struct i2c_board_info board_info = {
+		.type = "tda18212",
+		.platform_data = &config,
+	};
+
+	if (input->nr & 1)
+		board_info.addr = 0x63;
+	else
+		board_info.addr = 0x60;
+
+	/* due to a hardware quirk with the I2C gate on the stv0367+tda18212
+	 * combo, the tda18212 must be probed by reading it's id _twice_ when
+	 * cold started, or it very likely will fail.
+	 */
+	if (porttype == DDB_TUNER_DVBCT_ST)
+		tuner_tda18212_ping(input, board_info.addr);
+
+	request_module(board_info.type);
+
+	/* perform tuner init/attach */
+	client = i2c_new_device(adapter, &board_info);
+	if (client == NULL || client->dev.driver == NULL)
+		goto err;
+
+	if (!try_module_get(client->dev.driver->owner)) {
+		i2c_unregister_device(client);
+		goto err;
+	}
+
+	input->i2c_client[0] = client;
+
+	return 0;
+err:
+	printk(KERN_INFO "TDA18212 tuner not found. Device is not fully operational.\n");
+	return -ENODEV;
+}
+
+/******************************************************************************/
+/******************************************************************************/
+/******************************************************************************/
+
 static struct stv090x_config stv0900 = {
 	.device         = STV0900,
 	.demod_mode     = STV090x_DUAL,
@@ -779,19 +964,28 @@ static void dvb_input_detach(struct ddb_input *input)
 {
 	struct dvb_adapter *adap = &input->adap;
 	struct dvb_demux *dvbdemux = &input->demux;
+	struct i2c_client *client;
 
 	switch (input->attached) {
 	case 5:
-		if (input->fe2)
+		client = input->i2c_client[0];
+		if (client) {
+			module_put(client->dev.driver->owner);
+			i2c_unregister_device(client);
+		}
+		if (input->fe2) {
 			dvb_unregister_frontend(input->fe2);
+			input->fe2 = NULL;
+		}
 		if (input->fe) {
 			dvb_unregister_frontend(input->fe);
 			dvb_frontend_detach(input->fe);
 			input->fe = NULL;
 		}
+		/* fall-through */
 	case 4:
 		dvb_net_release(&input->dvbnet);
-
+		/* fall-through */
 	case 3:
 		dvbdemux->dmx.close(&dvbdemux->dmx);
 		dvbdemux->dmx.remove_frontend(&dvbdemux->dmx,
@@ -799,10 +993,10 @@ static void dvb_input_detach(struct ddb_input *input)
 		dvbdemux->dmx.remove_frontend(&dvbdemux->dmx,
 					      &input->mem_frontend);
 		dvb_dmxdev_release(&input->dmxdev);
-
+		/* fall-through */
 	case 2:
 		dvb_dmx_release(&input->demux);
-
+		/* fall-through */
 	case 1:
 		dvb_unregister_adapter(adap);
 	}
@@ -815,6 +1009,7 @@ static int dvb_input_attach(struct ddb_input *input)
 	struct ddb_port *port = input->port;
 	struct dvb_adapter *adap = &input->adap;
 	struct dvb_demux *dvbdemux = &input->demux;
+	int sony_osc24 = 0, sony_tspar = 0;
 
 	ret = dvb_register_adapter(adap, "DDBridge", THIS_MODULE,
 				   &input->port->dev->pdev->dev,
@@ -882,7 +1077,56 @@ static int dvb_input_attach(struct ddb_input *input)
 			       sizeof(struct dvb_tuner_ops));
 		}
 		break;
+	case DDB_TUNER_DVBCT_ST:
+		if (demod_attach_stv0367(input) < 0)
+			return -ENODEV;
+		if (tuner_attach_tda18212(input, port->type) < 0)
+			return -ENODEV;
+		if (input->fe) {
+			if (dvb_register_frontend(adap, input->fe) < 0)
+				return -ENODEV;
+		}
+		break;
+	case DDB_TUNER_DVBC2T2I_SONY_P:
+	case DDB_TUNER_DVBCT2_SONY_P:
+	case DDB_TUNER_DVBC2T2_SONY_P:
+	case DDB_TUNER_ISDBT_SONY_P:
+		if (port->type == DDB_TUNER_DVBC2T2I_SONY_P)
+			sony_osc24 = 1;
+		if (input->port->dev->info->ts_quirks & TS_QUIRK_ALT_OSC)
+			sony_osc24 = 0;
+		if (input->port->dev->info->ts_quirks & TS_QUIRK_SERIAL)
+			sony_tspar = 0;
+		else
+			sony_tspar = 1;
+
+		if (demod_attach_cxd28xx(input, sony_tspar, sony_osc24) < 0)
+			return -ENODEV;
+		if (tuner_attach_tda18212(input, port->type) < 0)
+			return -ENODEV;
+		if (input->fe) {
+			if (dvb_register_frontend(adap, input->fe) < 0)
+				return -ENODEV;
+		}
+		break;
+	case DDB_TUNER_XO2_DVBC2T2I_SONY:
+	case DDB_TUNER_XO2_DVBCT2_SONY:
+	case DDB_TUNER_XO2_DVBC2T2_SONY:
+	case DDB_TUNER_XO2_ISDBT_SONY:
+		if (port->type == DDB_TUNER_XO2_DVBC2T2I_SONY)
+			sony_osc24 = 1;
+
+		if (demod_attach_cxd28xx(input, 0, sony_osc24) < 0)
+			return -ENODEV;
+		if (tuner_attach_tda18212(input, port->type) < 0)
+			return -ENODEV;
+		if (input->fe) {
+			if (dvb_register_frontend(adap, input->fe) < 0)
+				return -ENODEV;
+		}
+		break;
 	}
+
 	input->attached = 5;
 	return 0;
 }
@@ -1130,6 +1374,70 @@ static void ddb_ports_detach(struct ddb *dev)
 /****************************************************************************/
 /****************************************************************************/
 
+static int init_xo2(struct ddb_port *port)
+{
+	struct i2c_adapter *i2c = &port->i2c->adap;
+	u8 val, data[2];
+	int res;
+
+	res = i2c_read_regs(i2c, 0x10, 0x04, data, 2);
+	if (res < 0)
+		return res;
+
+	if (data[0] != 0x01)  {
+		pr_info("Port %d: invalid XO2\n", port->nr);
+		return -1;
+	}
+
+	i2c_read_reg(i2c, 0x10, 0x08, &val);
+	if (val != 0) {
+		i2c_write_reg(i2c, 0x10, 0x08, 0x00);
+		msleep(100);
+	}
+	/* Enable tuner power, disable pll, reset demods */
+	i2c_write_reg(i2c, 0x10, 0x08, 0x04);
+	usleep_range(2000, 3000);
+	/* Release demod resets */
+	i2c_write_reg(i2c, 0x10, 0x08, 0x07);
+
+	/* speed: 0=55,1=75,2=90,3=104 MBit/s */
+	i2c_write_reg(i2c, 0x10, 0x09,
+		((xo2_speed >= 0 && xo2_speed <= 3) ? xo2_speed : 2));
+
+	i2c_write_reg(i2c, 0x10, 0x0a, 0x01);
+	i2c_write_reg(i2c, 0x10, 0x0b, 0x01);
+
+	usleep_range(2000, 3000);
+	/* Start XO2 PLL */
+	i2c_write_reg(i2c, 0x10, 0x08, 0x87);
+
+	return 0;
+}
+
+static int port_has_xo2(struct ddb_port *port, u8 *type, u8 *id)
+{
+	u8 probe[1] = { 0x00 }, data[4];
+
+	*type = DDB_XO2_TYPE_NONE;
+
+	if (i2c_io(&port->i2c->adap, 0x10, probe, 1, data, 4))
+		return 0;
+	if (data[0] == 'D' && data[1] == 'F') {
+		*id = data[2];
+		*type = DDB_XO2_TYPE_DUOFLEX;
+		return 1;
+	}
+	if (data[0] == 'C' && data[1] == 'I') {
+		*id = data[2];
+		*type = DDB_XO2_TYPE_CI;
+		return 1;
+	}
+	return 0;
+}
+
+/****************************************************************************/
+/****************************************************************************/
+
 static int port_has_ci(struct ddb_port *port)
 {
 	u8 val;
@@ -1162,10 +1470,39 @@ static int port_has_drxks(struct ddb_port *port)
 	return 1;
 }
 
+static int port_has_stv0367(struct ddb_port *port)
+{
+	u8 val;
+	if (i2c_read_reg16(&port->i2c->adap, 0x1e, 0xf000, &val) < 0)
+		return 0;
+	if (val != 0x60)
+		return 0;
+	if (i2c_read_reg16(&port->i2c->adap, 0x1f, 0xf000, &val) < 0)
+		return 0;
+	if (val != 0x60)
+		return 0;
+	return 1;
+}
+
+static int port_has_cxd28xx(struct ddb_port *port, u8 *id)
+{
+	struct i2c_adapter *i2c = &port->i2c->adap;
+	int status;
+
+	status = i2c_write_reg(&port->i2c->adap, 0x6e, 0, 0);
+	if (status)
+		return 0;
+	status = i2c_read_reg(i2c, 0x6e, 0xfd, id);
+	if (status)
+		return 0;
+	return 1;
+}
+
 static void ddb_port_probe(struct ddb_port *port)
 {
 	struct ddb *dev = port->dev;
 	char *modname = "NO MODULE";
+	u8 xo2_type, xo2_id, cxd_id;
 
 	port->class = DDB_PORT_NONE;
 
@@ -1173,6 +1510,85 @@ static void ddb_port_probe(struct ddb_port *port)
 		modname = "CI";
 		port->class = DDB_PORT_CI;
 		ddbwritel(I2C_SPEED_400, port->i2c->regs + I2C_TIMING);
+	} else if (port_has_xo2(port, &xo2_type, &xo2_id)) {
+		printk(KERN_INFO "Port %d (TAB %d): XO2 type: %d, id: %d\n",
+			port->nr, port->nr+1, xo2_type, xo2_id);
+
+		ddbwritel(I2C_SPEED_400, port->i2c->regs + I2C_TIMING);
+
+		switch (xo2_type) {
+		case DDB_XO2_TYPE_DUOFLEX:
+			init_xo2(port);
+			switch (xo2_id >> 2) {
+			case 0:
+				modname = "DUAL DVB-S2 (unsupported)";
+				port->class = DDB_PORT_NONE;
+				port->type = DDB_TUNER_XO2_DVBS_STV0910;
+				break;
+			case 1:
+				modname = "DUAL DVB-C/T/T2";
+				port->class = DDB_PORT_TUNER;
+				port->type = DDB_TUNER_XO2_DVBCT2_SONY;
+				break;
+			case 2:
+				modname = "DUAL DVB-ISDBT";
+				port->class = DDB_PORT_TUNER;
+				port->type = DDB_TUNER_XO2_ISDBT_SONY;
+				break;
+			case 3:
+				modname = "DUAL DVB-C/C2/T/T2";
+				port->class = DDB_PORT_TUNER;
+				port->type = DDB_TUNER_XO2_DVBC2T2_SONY;
+				break;
+			case 4:
+				modname = "DUAL ATSC (unsupported)";
+				port->class = DDB_PORT_NONE;
+				port->type = DDB_TUNER_XO2_ATSC_ST;
+				break;
+			case 5:
+				modname = "DUAL DVB-C/C2/T/T2/ISDBT";
+				port->class = DDB_PORT_TUNER;
+				port->type = DDB_TUNER_XO2_DVBC2T2I_SONY;
+				break;
+			default:
+				modname = "Unknown XO2 DuoFlex module\n";
+				break;
+			}
+			break;
+		case DDB_XO2_TYPE_CI:
+			printk(KERN_INFO "DuoFlex CI modules not supported\n");
+			break;
+		default:
+			printk(KERN_INFO "Unknown XO2 DuoFlex module\n");
+			break;
+		}
+	} else if (port_has_cxd28xx(port, &cxd_id)) {
+		switch (cxd_id) {
+		case 0xa4:
+			modname = "DUAL DVB-C2T2 CXD2843";
+			port->class = DDB_PORT_TUNER;
+			port->type = DDB_TUNER_DVBC2T2_SONY_P;
+			break;
+		case 0xb1:
+			modname = "DUAL DVB-CT2 CXD2837";
+			port->class = DDB_PORT_TUNER;
+			port->type = DDB_TUNER_DVBCT2_SONY_P;
+			break;
+		case 0xb0:
+			modname = "DUAL ISDB-T CXD2838";
+			port->class = DDB_PORT_TUNER;
+			port->type = DDB_TUNER_ISDBT_SONY_P;
+			break;
+		case 0xc1:
+			modname = "DUAL DVB-C2T2 ISDB-T CXD2854";
+			port->class = DDB_PORT_TUNER;
+			port->type = DDB_TUNER_DVBC2T2I_SONY_P;
+			break;
+		default:
+			modname = "Unknown CXD28xx tuner";
+			break;
+		}
+		ddbwritel(I2C_SPEED_400, port->i2c->regs + I2C_TIMING);
 	} else if (port_has_stv0900(port)) {
 		modname = "DUAL DVB-S2";
 		port->class = DDB_PORT_TUNER;
@@ -1188,7 +1604,13 @@ static void ddb_port_probe(struct ddb_port *port)
 		port->class = DDB_PORT_TUNER;
 		port->type = DDB_TUNER_DVBCT_TR;
 		ddbwritel(I2C_SPEED_400, port->i2c->regs + I2C_TIMING);
+	} else if (port_has_stv0367(port)) {
+		modname = "DUAL DVB-C/T";
+		port->class = DDB_PORT_TUNER;
+		port->type = DDB_TUNER_DVBCT_ST;
+		ddbwritel(I2C_SPEED_100, port->i2c->regs + I2C_TIMING);
 	}
+
 	printk(KERN_INFO "Port %d (TAB %d): %s\n",
 			 port->nr, port->nr+1, modname);
 }
@@ -1601,6 +2023,19 @@ static int ddb_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	ddbwritel(0xfff0f, INTERRUPT_ENABLE);
 	ddbwritel(0, MSI1_ENABLE);
 
+	/* board control */
+	if (dev->info->board_control) {
+		ddbwritel(0, DDB_LINK_TAG(0) | BOARD_CONTROL);
+		msleep(100);
+		ddbwritel(dev->info->board_control_2,
+			DDB_LINK_TAG(0) | BOARD_CONTROL);
+		usleep_range(2000, 3000);
+		ddbwritel(dev->info->board_control_2
+			| dev->info->board_control,
+			DDB_LINK_TAG(0) | BOARD_CONTROL);
+		usleep_range(2000, 3000);
+	}
+
 	if (ddb_i2c_init(dev) < 0)
 		goto fail1;
 	ddb_ports_init(dev);
@@ -1655,6 +2090,12 @@ static const struct ddb_info ddb_octopus_le = {
 	.port_num = 2,
 };
 
+static const struct ddb_info ddb_octopus_oem = {
+	.type     = DDB_OCTOPUS,
+	.name     = "Digital Devices Octopus OEM",
+	.port_num = 4,
+};
+
 static const struct ddb_info ddb_octopus_mini = {
 	.type     = DDB_OCTOPUS,
 	.name     = "Digital Devices Octopus Mini",
@@ -1678,6 +2119,14 @@ static const struct ddb_info ddb_dvbct = {
 	.port_num = 3,
 };
 
+static const struct ddb_info ddb_ctv7 = {
+	.type     = DDB_OCTOPUS,
+	.name     = "Digital Devices Cine CT V7 DVB adapter",
+	.port_num = 4,
+	.board_control   = 3,
+	.board_control_2 = 4,
+};
+
 static const struct ddb_info ddb_satixS2v3 = {
 	.type     = DDB_OCTOPUS,
 	.name     = "Mystique SaTiX-S2 V3 DVB adapter",
@@ -1690,6 +2139,55 @@ static const struct ddb_info ddb_octopusv3 = {
 	.port_num = 4,
 };
 
+/*** MaxA8 adapters ***********************************************************/
+
+static struct ddb_info ddb_ct2_8 = {
+	.type     = DDB_OCTOPUS_MAX_CT,
+	.name     = "Digital Devices MAX A8 CT2",
+	.port_num = 4,
+	.board_control   = 0x0ff,
+	.board_control_2 = 0xf00,
+	.ts_quirks = TS_QUIRK_SERIAL,
+};
+
+static struct ddb_info ddb_c2t2_8 = {
+	.type     = DDB_OCTOPUS_MAX_CT,
+	.name     = "Digital Devices MAX A8 C2T2",
+	.port_num = 4,
+	.board_control   = 0x0ff,
+	.board_control_2 = 0xf00,
+	.ts_quirks = TS_QUIRK_SERIAL,
+};
+
+static struct ddb_info ddb_isdbt_8 = {
+	.type     = DDB_OCTOPUS_MAX_CT,
+	.name     = "Digital Devices MAX A8 ISDBT",
+	.port_num = 4,
+	.board_control   = 0x0ff,
+	.board_control_2 = 0xf00,
+	.ts_quirks = TS_QUIRK_SERIAL,
+};
+
+static struct ddb_info ddb_c2t2i_v0_8 = {
+	.type     = DDB_OCTOPUS_MAX_CT,
+	.name     = "Digital Devices MAX A8 C2T2I V0",
+	.port_num = 4,
+	.board_control   = 0x0ff,
+	.board_control_2 = 0xf00,
+	.ts_quirks = TS_QUIRK_SERIAL | TS_QUIRK_ALT_OSC,
+};
+
+static struct ddb_info ddb_c2t2i_8 = {
+	.type     = DDB_OCTOPUS_MAX_CT,
+	.name     = "Digital Devices MAX A8 C2T2I",
+	.port_num = 4,
+	.board_control   = 0x0ff,
+	.board_control_2 = 0xf00,
+	.ts_quirks = TS_QUIRK_SERIAL,
+};
+
+/******************************************************************************/
+
 #define DDVID 0xdd01 /* Digital Devices Vendor ID */
 
 #define DDB_ID(_vend, _dev, _subvend, _subdev, _driverdata) {	\
@@ -1700,15 +2198,34 @@ static const struct ddb_info ddb_octopusv3 = {
 static const struct pci_device_id ddb_id_tbl[] = {
 	DDB_ID(DDVID, 0x0002, DDVID, 0x0001, ddb_octopus),
 	DDB_ID(DDVID, 0x0003, DDVID, 0x0001, ddb_octopus),
+	DDB_ID(DDVID, 0x0005, DDVID, 0x0004, ddb_octopusv3),
 	DDB_ID(DDVID, 0x0003, DDVID, 0x0002, ddb_octopus_le),
+	DDB_ID(DDVID, 0x0003, DDVID, 0x0003, ddb_octopus_oem),
 	DDB_ID(DDVID, 0x0003, DDVID, 0x0010, ddb_octopus_mini),
+	DDB_ID(DDVID, 0x0005, DDVID, 0x0011, ddb_octopus_mini),
 	DDB_ID(DDVID, 0x0003, DDVID, 0x0020, ddb_v6),
 	DDB_ID(DDVID, 0x0003, DDVID, 0x0021, ddb_v6_5),
 	DDB_ID(DDVID, 0x0003, DDVID, 0x0030, ddb_dvbct),
 	DDB_ID(DDVID, 0x0003, DDVID, 0xdb03, ddb_satixS2v3),
-	DDB_ID(DDVID, 0x0005, DDVID, 0x0004, ddb_octopusv3),
+	DDB_ID(DDVID, 0x0006, DDVID, 0x0031, ddb_ctv7),
+	DDB_ID(DDVID, 0x0006, DDVID, 0x0032, ddb_ctv7),
+	DDB_ID(DDVID, 0x0006, DDVID, 0x0033, ddb_ctv7),
+	DDB_ID(DDVID, 0x0008, DDVID, 0x0034, ddb_ct2_8),
+	DDB_ID(DDVID, 0x0008, DDVID, 0x0035, ddb_c2t2_8),
+	DDB_ID(DDVID, 0x0008, DDVID, 0x0036, ddb_isdbt_8),
+	DDB_ID(DDVID, 0x0008, DDVID, 0x0037, ddb_c2t2i_v0_8),
+	DDB_ID(DDVID, 0x0008, DDVID, 0x0038, ddb_c2t2i_8),
+	DDB_ID(DDVID, 0x0006, DDVID, 0x0039, ddb_ctv7),
 	/* in case sub-ids got deleted in flash */
 	DDB_ID(DDVID, 0x0003, PCI_ANY_ID, PCI_ANY_ID, ddb_none),
+	DDB_ID(DDVID, 0x0005, PCI_ANY_ID, PCI_ANY_ID, ddb_none),
+	DDB_ID(DDVID, 0x0006, PCI_ANY_ID, PCI_ANY_ID, ddb_none),
+	DDB_ID(DDVID, 0x0007, PCI_ANY_ID, PCI_ANY_ID, ddb_none),
+	DDB_ID(DDVID, 0x0008, PCI_ANY_ID, PCI_ANY_ID, ddb_none),
+	DDB_ID(DDVID, 0x0011, PCI_ANY_ID, PCI_ANY_ID, ddb_none),
+	DDB_ID(DDVID, 0x0013, PCI_ANY_ID, PCI_ANY_ID, ddb_none),
+	DDB_ID(DDVID, 0x0201, PCI_ANY_ID, PCI_ANY_ID, ddb_none),
+	DDB_ID(DDVID, 0x0320, PCI_ANY_ID, PCI_ANY_ID, ddb_none),
 	{0}
 };
 MODULE_DEVICE_TABLE(pci, ddb_id_tbl);
diff --git a/drivers/media/pci/ddbridge/ddbridge-regs.h b/drivers/media/pci/ddbridge/ddbridge-regs.h
index 6ae810324b4e..98cebb97d64f 100644
--- a/drivers/media/pci/ddbridge/ddbridge-regs.h
+++ b/drivers/media/pci/ddbridge/ddbridge-regs.h
@@ -34,6 +34,10 @@
 
 /* ------------------------------------------------------------------------- */
 
+#define BOARD_CONTROL    0x30
+
+/* ------------------------------------------------------------------------- */
+
 /* Interrupt controller                                     */
 /* How many MSI's are available depends on HW (Min 2 max 8) */
 /* How many are usable also depends on Host platform        */
diff --git a/drivers/media/pci/ddbridge/ddbridge.h b/drivers/media/pci/ddbridge/ddbridge.h
index 185b423818d3..4a0e3283d646 100644
--- a/drivers/media/pci/ddbridge/ddbridge.h
+++ b/drivers/media/pci/ddbridge/ddbridge.h
@@ -43,14 +43,29 @@
 #define DDB_MAX_PORT    4
 #define DDB_MAX_INPUT   8
 #define DDB_MAX_OUTPUT  4
+#define DDB_MAX_LINK    4
+#define DDB_LINK_SHIFT 28
+
+#define DDB_LINK_TAG(_x) (_x << DDB_LINK_SHIFT)
+
+#define DDB_XO2_TYPE_NONE	0
+#define DDB_XO2_TYPE_DUOFLEX	1
+#define DDB_XO2_TYPE_CI		2
 
 struct ddb_info {
 	int   type;
-#define DDB_NONE         0
-#define DDB_OCTOPUS      1
+#define DDB_NONE		0
+#define DDB_OCTOPUS		1
+#define DDB_OCTOPUS_MAX_CT	6
 	char *name;
 	int   port_num;
 	u32   port_type[DDB_MAX_PORT];
+	u32   board_control;
+	u32   board_control_2;
+	u8    ts_quirks;
+#define TS_QUIRK_SERIAL   1
+#define TS_QUIRK_REVERSED 2
+#define TS_QUIRK_ALT_OSC  8
 };
 
 /* DMA_SIZE MUST be divisible by 188 and 128 !!! */
@@ -86,6 +101,7 @@ struct ddb_input {
 
 	struct dvb_adapter     adap;
 	struct dvb_device     *dev;
+	struct i2c_client     *i2c_client[1];
 	struct dvb_frontend   *fe;
 	struct dvb_frontend   *fe2;
 	struct dmxdev          dmxdev;
@@ -138,11 +154,22 @@ struct ddb_port {
 #define DDB_PORT_CI             1
 #define DDB_PORT_TUNER          2
 	u32                    type;
-#define DDB_TUNER_NONE          0
-#define DDB_TUNER_DVBS_ST       1
-#define DDB_TUNER_DVBS_ST_AA    2
-#define DDB_TUNER_DVBCT_TR     16
-#define DDB_TUNER_DVBCT_ST     17
+#define DDB_TUNER_NONE			0
+#define DDB_TUNER_DVBS_ST		1
+#define DDB_TUNER_DVBS_ST_AA		2
+#define DDB_TUNER_DVBCT2_SONY_P		7
+#define DDB_TUNER_DVBC2T2_SONY_P	8
+#define DDB_TUNER_ISDBT_SONY_P		9
+#define DDB_TUNER_DVBC2T2I_SONY_P	15
+#define DDB_TUNER_DVBCT_TR		16
+#define DDB_TUNER_DVBCT_ST		17
+#define DDB_TUNER_XO2_DVBS_STV0910	32
+#define DDB_TUNER_XO2_DVBCT2_SONY	33
+#define DDB_TUNER_XO2_ISDBT_SONY	34
+#define DDB_TUNER_XO2_DVBC2T2_SONY	35
+#define DDB_TUNER_XO2_ATSC_ST		36
+#define DDB_TUNER_XO2_DVBC2T2I_SONY	37
+
 	u32                    adr;
 
 	struct ddb_input      *input[2];
diff --git a/drivers/media/pci/ivtv/ivtv-alsa-pcm.c b/drivers/media/pci/ivtv/ivtv-alsa-pcm.c
index 807ead20d212..417d03da01f0 100644
--- a/drivers/media/pci/ivtv/ivtv-alsa-pcm.c
+++ b/drivers/media/pci/ivtv/ivtv-alsa-pcm.c
@@ -262,14 +262,16 @@ static int snd_ivtv_pcm_hw_free(struct snd_pcm_substream *substream)
 {
 	struct snd_ivtv_card *itvsc = snd_pcm_substream_chip(substream);
 	unsigned long flags;
+	unsigned char *dma_area = NULL;
 
 	spin_lock_irqsave(&itvsc->slock, flags);
 	if (substream->runtime->dma_area) {
 		dprintk("freeing pcm capture region\n");
-		vfree(substream->runtime->dma_area);
+		dma_area = substream->runtime->dma_area;
 		substream->runtime->dma_area = NULL;
 	}
 	spin_unlock_irqrestore(&itvsc->slock, flags);
+	vfree(dma_area);
 
 	return 0;
 }
diff --git a/drivers/media/pci/netup_unidvb/netup_unidvb_core.c b/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
index 9444483fb942..5c0a4e614413 100644
--- a/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
+++ b/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
@@ -122,7 +122,8 @@ static void netup_unidvb_queue_cleanup(struct netup_dma *dma);
 
 static struct cxd2841er_config demod_config = {
 	.i2c_addr = 0xc8,
-	.xtal = SONY_XTAL_24000
+	.xtal = SONY_XTAL_24000,
+	.flags = CXD2841ER_USE_GATECTRL | CXD2841ER_ASCOT
 };
 
 static struct horus3a_config horus3a_conf = {
diff --git a/drivers/media/pci/saa7134/saa7134-cards.c b/drivers/media/pci/saa7134/saa7134-cards.c
index f79380faf499..9965d3531c80 100644
--- a/drivers/media/pci/saa7134/saa7134-cards.c
+++ b/drivers/media/pci/saa7134/saa7134-cards.c
@@ -7806,7 +7806,7 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 				dev->name, saa7134_boards[dev->board].name);
 			break;
 		}
-		/* break intentionally omitted */
+		/* fall-through */
 	case SAA7134_BOARD_VIDEOMATE_DVBT_300:
 	case SAA7134_BOARD_ASUS_EUROPA2_HYBRID:
 	case SAA7134_BOARD_ASUS_EUROPA_HYBRID:
@@ -7864,7 +7864,7 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 		break;
 	case SAA7134_BOARD_HAUPPAUGE_HVR1110:
 		hauppauge_eeprom(dev, dev->eedata+0x80);
-		/* break intentionally omitted */
+		/* fall-through */
 	case SAA7134_BOARD_PINNACLE_PCTV_310i:
 	case SAA7134_BOARD_KWORLD_DVBT_210:
 	case SAA7134_BOARD_TEVION_DVBT_220RF:
diff --git a/drivers/media/pci/saa7164/saa7164-bus.c b/drivers/media/pci/saa7164/saa7164-bus.c
index b2ff82fa7116..ecfeac5cdbed 100644
--- a/drivers/media/pci/saa7164/saa7164-bus.c
+++ b/drivers/media/pci/saa7164/saa7164-bus.c
@@ -389,11 +389,11 @@ int saa7164_bus_get(struct saa7164_dev *dev, struct tmComResInfo* msg,
 	msg_tmp.size = le16_to_cpu((__force __le16)msg_tmp.size);
 	msg_tmp.command = le32_to_cpu((__force __le32)msg_tmp.command);
 	msg_tmp.controlselector = le16_to_cpu((__force __le16)msg_tmp.controlselector);
+	memcpy(msg, &msg_tmp, sizeof(*msg));
 
 	/* No need to update the read positions, because this was a peek */
 	/* If the caller specifically want to peek, return */
 	if (peekonly) {
-		memcpy(msg, &msg_tmp, sizeof(*msg));
 		goto peekout;
 	}
 
@@ -438,21 +438,15 @@ int saa7164_bus_get(struct saa7164_dev *dev, struct tmComResInfo* msg,
 		space_rem = bus->m_dwSizeGetRing - curr_grp;
 
 		if (space_rem < sizeof(*msg)) {
-			/* msg wraps around the ring */
-			memcpy_fromio(msg, bus->m_pdwGetRing + curr_grp, space_rem);
-			memcpy_fromio((u8 *)msg + space_rem, bus->m_pdwGetRing,
-				sizeof(*msg) - space_rem);
 			if (buf)
 				memcpy_fromio(buf, bus->m_pdwGetRing + sizeof(*msg) -
 					space_rem, buf_size);
 
 		} else if (space_rem == sizeof(*msg)) {
-			memcpy_fromio(msg, bus->m_pdwGetRing + curr_grp, sizeof(*msg));
 			if (buf)
 				memcpy_fromio(buf, bus->m_pdwGetRing, buf_size);
 		} else {
 			/* Additional data wraps around the ring */
-			memcpy_fromio(msg, bus->m_pdwGetRing + curr_grp, sizeof(*msg));
 			if (buf) {
 				memcpy_fromio(buf, bus->m_pdwGetRing + curr_grp +
 					sizeof(*msg), space_rem - sizeof(*msg));
@@ -465,15 +459,10 @@ int saa7164_bus_get(struct saa7164_dev *dev, struct tmComResInfo* msg,
 
 	} else {
 		/* No wrapping */
-		memcpy_fromio(msg, bus->m_pdwGetRing + curr_grp, sizeof(*msg));
 		if (buf)
 			memcpy_fromio(buf, bus->m_pdwGetRing + curr_grp + sizeof(*msg),
 				buf_size);
 	}
-	/* Convert from little endian to CPU */
-	msg->size = le16_to_cpu((__force __le16)msg->size);
-	msg->command = le32_to_cpu((__force __le32)msg->command);
-	msg->controlselector = le16_to_cpu((__force __le16)msg->controlselector);
 
 	/* Update the read positions, adjusting the ring */
 	saa7164_writel(bus->m_dwGetReadPos, new_grp);
diff --git a/drivers/media/pci/saa7164/saa7164-cmd.c b/drivers/media/pci/saa7164/saa7164-cmd.c
index 175015ca79f2..dfebd77ada59 100644
--- a/drivers/media/pci/saa7164/saa7164-cmd.c
+++ b/drivers/media/pci/saa7164/saa7164-cmd.c
@@ -506,6 +506,8 @@ int saa7164_cmd_send(struct saa7164_dev *dev, u8 id, enum tmComResCmd command,
 				dprintk(DBGLVL_CMD,
 					"%s() UNKNOWN OR INVALID CONTROL\n",
 					__func__);
+				ret = SAA_ERR_NOT_SUPPORTED;
+				break;
 			default:
 				dprintk(DBGLVL_CMD, "%s() UNKNOWN\n", __func__);
 				ret = SAA_ERR_NOT_SUPPORTED;
diff --git a/drivers/media/pci/solo6x10/solo6x10-core.c b/drivers/media/pci/solo6x10/solo6x10-core.c
index f50d07229236..ca0873e47bea 100644
--- a/drivers/media/pci/solo6x10/solo6x10-core.c
+++ b/drivers/media/pci/solo6x10/solo6x10-core.c
@@ -511,6 +511,7 @@ static int solo_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	default:
 		dev_warn(&pdev->dev, "Invalid chip_id 0x%02x, assuming 4 ch\n",
 			 chip_id);
+		/* fall through */
 	case 5:
 		solo_dev->nr_chans = 4;
 		solo_dev->nr_ext = 1;
diff --git a/drivers/media/pci/solo6x10/solo6x10-g723.c b/drivers/media/pci/solo6x10/solo6x10-g723.c
index 36e93540bb49..3ca947092775 100644
--- a/drivers/media/pci/solo6x10/solo6x10-g723.c
+++ b/drivers/media/pci/solo6x10/solo6x10-g723.c
@@ -223,9 +223,9 @@ static snd_pcm_uframes_t snd_solo_pcm_pointer(struct snd_pcm_substream *ss)
 	return idx * G723_FRAMES_PER_PAGE;
 }
 
-static int snd_solo_pcm_copy(struct snd_pcm_substream *ss, int channel,
-			     snd_pcm_uframes_t pos, void __user *dst,
-			     snd_pcm_uframes_t count)
+static int __snd_solo_pcm_copy(struct snd_pcm_substream *ss,
+			       unsigned long pos, void *dst,
+			       unsigned long count, bool in_kernel)
 {
 	struct solo_snd_pcm *solo_pcm = snd_pcm_substream_chip(ss);
 	struct solo_dev *solo_dev = solo_pcm->solo_dev;
@@ -242,16 +242,31 @@ static int snd_solo_pcm_copy(struct snd_pcm_substream *ss, int channel,
 		if (err)
 			return err;
 
-		err = copy_to_user(dst + (i * G723_PERIOD_BYTES),
-				   solo_pcm->g723_buf, G723_PERIOD_BYTES);
-
-		if (err)
+		if (in_kernel)
+			memcpy(dst, solo_pcm->g723_buf, G723_PERIOD_BYTES);
+		else if (copy_to_user((void __user *)dst,
+				      solo_pcm->g723_buf, G723_PERIOD_BYTES))
 			return -EFAULT;
+		dst += G723_PERIOD_BYTES;
 	}
 
 	return 0;
 }
 
+static int snd_solo_pcm_copy_user(struct snd_pcm_substream *ss, int channel,
+				  unsigned long pos, void __user *dst,
+				  unsigned long count)
+{
+	return __snd_solo_pcm_copy(ss, pos, (void *)dst, count, false);
+}
+
+static int snd_solo_pcm_copy_kernel(struct snd_pcm_substream *ss, int channel,
+				    unsigned long pos, void *dst,
+				    unsigned long count)
+{
+	return __snd_solo_pcm_copy(ss, pos, dst, count, true);
+}
+
 static const struct snd_pcm_ops snd_solo_pcm_ops = {
 	.open = snd_solo_pcm_open,
 	.close = snd_solo_pcm_close,
@@ -261,7 +276,8 @@ static const struct snd_pcm_ops snd_solo_pcm_ops = {
 	.prepare = snd_solo_pcm_prepare,
 	.trigger = snd_solo_pcm_trigger,
 	.pointer = snd_solo_pcm_pointer,
-	.copy = snd_solo_pcm_copy,
+	.copy_user = snd_solo_pcm_copy_user,
+	.copy_kernel = snd_solo_pcm_copy_kernel,
 };
 
 static int snd_solo_capture_volume_info(struct snd_kcontrol *kcontrol,
diff --git a/drivers/media/pci/solo6x10/solo6x10-i2c.c b/drivers/media/pci/solo6x10/solo6x10-i2c.c
index e83bb79f9349..89f2f2a493c2 100644
--- a/drivers/media/pci/solo6x10/solo6x10-i2c.c
+++ b/drivers/media/pci/solo6x10/solo6x10-i2c.c
@@ -192,6 +192,7 @@ int solo_i2c_isr(struct solo_dev *solo_dev)
 		}
 
 		solo_dev->i2c_state = IIC_STATE_WRITE;
+		/* fall through */
 	case IIC_STATE_WRITE:
 		ret = solo_i2c_handle_write(solo_dev);
 		break;
diff --git a/drivers/media/pci/ttpci/av7110.c b/drivers/media/pci/ttpci/av7110.c
index df9395c87178..f2905bd80366 100644
--- a/drivers/media/pci/ttpci/av7110.c
+++ b/drivers/media/pci/ttpci/av7110.c
@@ -336,6 +336,7 @@ static int DvbDmxFilterCallback(u8 *buffer1, size_t buffer1_len,
 			av7110_p2t_write(buffer1, buffer1_len,
 					 dvbdmxfilter->feed->pid,
 					 &av7110->p2t_filter[dvbdmxfilter->index]);
+		return 0;
 	default:
 		return 0;
 	}
@@ -451,8 +452,12 @@ static void debiirq(unsigned long cookie)
 
 	case DATA_CI_PUT:
 		dprintk(4, "debi DATA_CI_PUT\n");
+		xfer = TX_BUFF;
+		break;
 	case DATA_MPEG_PLAY:
 		dprintk(4, "debi DATA_MPEG_PLAY\n");
+		xfer = TX_BUFF;
+		break;
 	case DATA_BMP_LOAD:
 		dprintk(4, "debi DATA_BMP_LOAD\n");
 		xfer = TX_BUFF;
diff --git a/drivers/media/pci/zoran/zoran_driver.c b/drivers/media/pci/zoran/zoran_driver.c
index 180f3d7af3e1..a11cb501c550 100644
--- a/drivers/media/pci/zoran/zoran_driver.c
+++ b/drivers/media/pci/zoran/zoran_driver.c
@@ -534,6 +534,7 @@ static int zoran_v4l_queue_frame(struct zoran_fh *fh, int num)
 				KERN_WARNING
 				"%s: %s - queueing buffer %d in state DONE!?\n",
 				ZR_DEVNAME(zr), __func__, num);
+			/* fall through */
 		case BUZ_STATE_USER:
 			/* since there is at least one unused buffer there's room for at least
 			 * one more pend[] entry */
@@ -693,6 +694,7 @@ static int zoran_jpg_queue_frame(struct zoran_fh *fh, int num,
 				KERN_WARNING
 				"%s: %s - queing frame in BUZ_STATE_DONE state!?\n",
 				ZR_DEVNAME(zr), __func__);
+			/* fall through */
 		case BUZ_STATE_USER:
 			/* since there is at least one unused buffer there's room for at
 			 *least one more pend[] entry */
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index 041cb80a26b1..1313cd533436 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -74,6 +74,13 @@ config VIDEO_M32R_AR_M64278
 	  To compile this driver as a module, choose M here: the
 	  module will be called arv.
 
+config VIDEO_MUX
+	tristate "Video Multiplexer"
+	depends on OF && VIDEO_V4L2_SUBDEV_API && MEDIA_CONTROLLER
+	select REGMAP
+	help
+	  This driver provides support for N:1 video bus multiplexers.
+
 config VIDEO_OMAP3
 	tristate "OMAP 3 Camera support"
 	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API && ARCH_OMAP3
@@ -82,6 +89,7 @@ config VIDEO_OMAP3
 	select ARM_DMA_USE_IOMMU
 	select VIDEOBUF2_DMA_CONTIG
 	select MFD_SYSCON
+	select V4L2_FWNODE
 	---help---
 	  Driver for an OMAP 3 camera controller.
 
@@ -97,6 +105,7 @@ config VIDEO_PXA27x
 	depends on PXA27x || COMPILE_TEST
 	select VIDEOBUF2_DMA_SG
 	select SG_SPLIT
+	select V4L2_FWNODE
 	---help---
 	  This is a v4l2 driver for the PXA27x Quick Capture Interface
 
@@ -114,6 +123,19 @@ config VIDEO_S3C_CAMIF
 	  To compile this driver as a module, choose M here: the module
 	  will be called s3c-camif.
 
+config VIDEO_STM32_DCMI
+	tristate "STM32 Digital Camera Memory Interface (DCMI) support"
+	depends on VIDEO_V4L2 && OF && HAS_DMA
+	depends on ARCH_STM32 || COMPILE_TEST
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_FWNODE
+	---help---
+	  This module makes the STM32 Digital Camera Memory Interface (DCMI)
+	  available as a v4l2 device.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called stm32-dcmi.
+
 source "drivers/media/platform/soc_camera/Kconfig"
 source "drivers/media/platform/exynos4-is/Kconfig"
 source "drivers/media/platform/am437x/Kconfig"
@@ -127,6 +149,7 @@ config VIDEO_TI_CAL
 	depends on SOC_DRA7XX || COMPILE_TEST
 	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_FWNODE
 	default n
 	---help---
 	  Support for the TI CAL (Camera Adaptation Layer) block
@@ -448,6 +471,20 @@ config VIDEO_TI_VPE_DEBUG
 	---help---
 	  Enable debug messages on VPE driver.
 
+config VIDEO_QCOM_VENUS
+	tristate "Qualcomm Venus V4L2 encoder/decoder driver"
+	depends on VIDEO_DEV && VIDEO_V4L2 && HAS_DMA
+	depends on (ARCH_QCOM && IOMMU_DMA) || COMPILE_TEST
+	select QCOM_MDT_LOADER if (ARM || ARM64)
+	select QCOM_SCM if (ARM || ARM64)
+	select VIDEOBUF2_DMA_SG
+	select V4L2_MEM2MEM_DEV
+	---help---
+	  This is a V4L2 driver for Qualcomm Venus video accelerator
+	  hardware. It accelerates encoding and decoding operations
+	  on various Qualcomm SoCs.
+	  To compile this driver as a module choose m here.
+
 endif # V4L_MEM2MEM_DRIVERS
 
 # TI VIDEO PORT Helper Modules
@@ -521,4 +558,41 @@ config VIDEO_STI_HDMI_CEC
          CEC bus is present in the HDMI connector and enables communication
          between compatible devices.
 
+config VIDEO_STM32_HDMI_CEC
+       tristate "STMicroelectronics STM32 HDMI CEC driver"
+       depends on ARCH_STM32 || COMPILE_TEST
+       select REGMAP
+       select REGMAP_MMIO
+       select CEC_CORE
+       ---help---
+         This is a driver for STM32 interface. It uses the
+         generic CEC framework interface.
+         CEC bus is present in the HDMI connector and enables communication
+         between compatible devices.
+
 endif #CEC_PLATFORM_DRIVERS
+
+menuconfig SDR_PLATFORM_DRIVERS
+	bool "SDR platform devices"
+	depends on MEDIA_SDR_SUPPORT
+	default n
+	---help---
+	  Say Y here to enable support for platform-specific SDR Drivers.
+
+if SDR_PLATFORM_DRIVERS
+
+config VIDEO_RCAR_DRIF
+	tristate "Renesas Digitial Radio Interface (DRIF)"
+	depends on VIDEO_V4L2 && HAS_DMA
+	depends on ARCH_RENESAS || COMPILE_TEST
+	select VIDEOBUF2_VMALLOC
+	---help---
+	  Say Y if you want to enable R-Car Gen3 DRIF support. DRIF is Digital
+	  Radio Interface that interfaces with an RF front end chip. It is a
+	  receiver of digital data which uses DMA to transfer received data to
+	  a configured location for an application to use.
+
+	  To compile this driver as a module, choose M here; the module
+	  will be called rcar_drif.
+
+endif # SDR_PLATFORM_DRIVERS
diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile
index 63303d63c64c..9beadc760467 100644
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -28,6 +28,8 @@ obj-$(CONFIG_VIDEO_SH_VEU)		+= sh_veu.o
 
 obj-$(CONFIG_VIDEO_MEM2MEM_DEINTERLACE)	+= m2m-deinterlace.o
 
+obj-$(CONFIG_VIDEO_MUX)			+= video-mux.o
+
 obj-$(CONFIG_VIDEO_S3C_CAMIF) 		+= s3c-camif/
 obj-$(CONFIG_VIDEO_SAMSUNG_EXYNOS4_IS) 	+= exynos4-is/
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_JPEG)	+= s5p-jpeg/
@@ -44,14 +46,17 @@ obj-$(CONFIG_VIDEO_STI_HDMI_CEC) 	+= sti/cec/
 
 obj-$(CONFIG_VIDEO_STI_DELTA)		+= sti/delta/
 
-obj-$(CONFIG_BLACKFIN)                  += blackfin/
+obj-y 					+= stm32/
+
+obj-y                                   += blackfin/
 
-obj-$(CONFIG_ARCH_DAVINCI)		+= davinci/
+obj-y					+= davinci/
 
 obj-$(CONFIG_VIDEO_SH_VOU)		+= sh_vou.o
 
 obj-$(CONFIG_SOC_CAMERA)		+= soc_camera/
 
+obj-$(CONFIG_VIDEO_RCAR_DRIF)		+= rcar_drif.o
 obj-$(CONFIG_VIDEO_RENESAS_FCP) 	+= rcar-fcp.o
 obj-$(CONFIG_VIDEO_RENESAS_FDP1)	+= rcar_fdp1.o
 obj-$(CONFIG_VIDEO_RENESAS_JPU) 	+= rcar_jpu.o
@@ -68,6 +73,8 @@ obj-$(CONFIG_VIDEO_RCAR_VIN)		+= rcar-vin/
 obj-$(CONFIG_VIDEO_ATMEL_ISC)		+= atmel/
 obj-$(CONFIG_VIDEO_ATMEL_ISI)		+= atmel/
 
+obj-$(CONFIG_VIDEO_STM32_DCMI)		+= stm32/
+
 ccflags-y += -I$(srctree)/drivers/media/i2c
 
 obj-$(CONFIG_VIDEO_MEDIATEK_VPU)	+= mtk-vpu/
@@ -77,3 +84,5 @@ obj-$(CONFIG_VIDEO_MEDIATEK_VCODEC)	+= mtk-vcodec/
 obj-$(CONFIG_VIDEO_MEDIATEK_MDP)	+= mtk-mdp/
 
 obj-$(CONFIG_VIDEO_MEDIATEK_JPEG)	+= mtk-jpeg/
+
+obj-$(CONFIG_VIDEO_QCOM_VENUS)		+= qcom/venus/
diff --git a/drivers/media/platform/am437x/Kconfig b/drivers/media/platform/am437x/Kconfig
index 42d9c186710a..160e77e9a0fb 100644
--- a/drivers/media/platform/am437x/Kconfig
+++ b/drivers/media/platform/am437x/Kconfig
@@ -3,6 +3,7 @@ config VIDEO_AM437X_VPFE
 	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && HAS_DMA
 	depends on SOC_AM43XX || COMPILE_TEST
 	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_FWNODE
 	help
 	   Support for AM437x Video Processing Front End based Video
 	   Capture Driver.
diff --git a/drivers/media/platform/am437x/am437x-vpfe.c b/drivers/media/platform/am437x/am437x-vpfe.c
index 05489a401c5c..466aba8b0e00 100644
--- a/drivers/media/platform/am437x/am437x-vpfe.c
+++ b/drivers/media/platform/am437x/am437x-vpfe.c
@@ -26,6 +26,7 @@
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/module.h>
+#include <linux/of_graph.h>
 #include <linux/pinctrl/consumer.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
@@ -36,7 +37,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-event.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 
 #include "am437x-vpfe.h"
 
@@ -2303,7 +2304,8 @@ vpfe_async_bound(struct v4l2_async_notifier *notifier,
 	vpfe_dbg(1, vpfe, "vpfe_async_bound\n");
 
 	for (i = 0; i < ARRAY_SIZE(vpfe->cfg->asd); i++) {
-		if (vpfe->cfg->asd[i]->match.of.node == asd[i].match.of.node) {
+		if (vpfe->cfg->asd[i]->match.fwnode.fwnode ==
+		    asd[i].match.fwnode.fwnode) {
 			sdinfo = &vpfe->cfg->sub_devs[i];
 			vpfe->sd[i] = subdev;
 			vpfe->sd[i]->grp_id = sdinfo->grp_id;
@@ -2419,7 +2421,7 @@ static struct vpfe_config *
 vpfe_get_pdata(struct platform_device *pdev)
 {
 	struct device_node *endpoint = NULL;
-	struct v4l2_of_endpoint bus_cfg;
+	struct v4l2_fwnode_endpoint bus_cfg;
 	struct vpfe_subdev_info *sdinfo;
 	struct vpfe_config *pdata;
 	unsigned int flags;
@@ -2463,7 +2465,8 @@ vpfe_get_pdata(struct platform_device *pdev)
 			sdinfo->vpfe_param.if_type = VPFE_RAW_BAYER;
 		}
 
-		err = v4l2_of_parse_endpoint(endpoint, &bus_cfg);
+		err = v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint),
+						 &bus_cfg);
 		if (err) {
 			dev_err(&pdev->dev, "Could not parse the endpoint\n");
 			goto done;
@@ -2501,8 +2504,8 @@ vpfe_get_pdata(struct platform_device *pdev)
 			goto done;
 		}
 
-		pdata->asd[i]->match_type = V4L2_ASYNC_MATCH_OF;
-		pdata->asd[i]->match.of.node = rem;
+		pdata->asd[i]->match_type = V4L2_ASYNC_MATCH_FWNODE;
+		pdata->asd[i]->match.fwnode.fwnode = of_fwnode_handle(rem);
 		of_node_put(rem);
 	}
 
diff --git a/drivers/media/platform/atmel/Kconfig b/drivers/media/platform/atmel/Kconfig
index 9bd0f19b127f..55de751e5f51 100644
--- a/drivers/media/platform/atmel/Kconfig
+++ b/drivers/media/platform/atmel/Kconfig
@@ -4,6 +4,7 @@ config VIDEO_ATMEL_ISC
 	depends on ARCH_AT91 || COMPILE_TEST
 	select VIDEOBUF2_DMA_CONTIG
 	select REGMAP_MMIO
+	select V4L2_FWNODE
 	help
 	   This module makes the ATMEL Image Sensor Controller available
 	   as a v4l2 device.
@@ -13,6 +14,7 @@ config VIDEO_ATMEL_ISI
 	depends on VIDEO_V4L2 && OF && HAS_DMA
 	depends on ARCH_AT91 || COMPILE_TEST
 	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_FWNODE
 	---help---
 	  This module makes the ATMEL Image Sensor Interface available
 	  as a v4l2 device.
diff --git a/drivers/media/platform/atmel/atmel-isc.c b/drivers/media/platform/atmel/atmel-isc.c
index c4b2115559a5..d6534252cdcd 100644
--- a/drivers/media/platform/atmel/atmel-isc.c
+++ b/drivers/media/platform/atmel/atmel-isc.c
@@ -32,6 +32,7 @@
 #include <linux/math64.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/of_graph.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/regmap.h>
@@ -42,7 +43,7 @@
 #include <media/v4l2-event.h>
 #include <media/v4l2-image-sizes.h>
 #include <media/v4l2-ioctl.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 #include <media/v4l2-subdev.h>
 #include <media/videobuf2-dma-contig.h>
 
@@ -239,13 +240,11 @@ static struct isc_format isc_formats[] = {
 
 	{ V4L2_PIX_FMT_YUV420, 0x0, 12,
 	  ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_YYCC,
-	  ISC_DCFG_IMODE_YC420P | ISC_DCFG_YMBSIZE_BEATS8 |
-	  ISC_DCFG_CMBSIZE_BEATS8, ISC_DCTRL_DVIEW_PLANAR, 0x7fb,
+	  ISC_DCFG_IMODE_YC420P, ISC_DCTRL_DVIEW_PLANAR, 0x7fb,
 	  false, false },
 	{ V4L2_PIX_FMT_YUV422P, 0x0, 16,
 	  ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_YYCC,
-	  ISC_DCFG_IMODE_YC422P | ISC_DCFG_YMBSIZE_BEATS8 |
-	  ISC_DCFG_CMBSIZE_BEATS8, ISC_DCTRL_DVIEW_PLANAR, 0x3fb,
+	  ISC_DCFG_IMODE_YC422P, ISC_DCTRL_DVIEW_PLANAR, 0x3fb,
 	  false, false },
 	{ V4L2_PIX_FMT_RGB565, MEDIA_BUS_FMT_RGB565_2X8_LE, 16,
 	  ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_RGB565,
@@ -700,8 +699,10 @@ static void isc_set_histogram(struct isc_device *isc)
 }
 
 static inline void isc_get_param(const struct isc_format *fmt,
-				     u32 *rlp_mode, u32 *dcfg_imode)
+				  u32 *rlp_mode, u32 *dcfg)
 {
+	*dcfg = ISC_DCFG_YMBSIZE_BEATS8;
+
 	switch (fmt->fourcc) {
 	case V4L2_PIX_FMT_SBGGR10:
 	case V4L2_PIX_FMT_SGBRG10:
@@ -712,11 +713,11 @@ static inline void isc_get_param(const struct isc_format *fmt,
 	case V4L2_PIX_FMT_SGRBG12:
 	case V4L2_PIX_FMT_SRGGB12:
 		*rlp_mode = fmt->reg_rlp_mode;
-		*dcfg_imode = fmt->reg_dcfg_imode;
+		*dcfg |= fmt->reg_dcfg_imode;
 		break;
 	default:
 		*rlp_mode = ISC_RLP_CFG_MODE_DAT8;
-		*dcfg_imode = ISC_DCFG_IMODE_PACKED8;
+		*dcfg |= ISC_DCFG_IMODE_PACKED8;
 		break;
 	}
 }
@@ -726,18 +727,19 @@ static int isc_configure(struct isc_device *isc)
 	struct regmap *regmap = isc->regmap;
 	const struct isc_format *current_fmt = isc->current_fmt;
 	struct isc_subdev_entity *subdev = isc->current_subdev;
-	u32 pfe_cfg0, rlp_mode, dcfg_imode, mask, pipeline;
+	u32 pfe_cfg0, rlp_mode, dcfg, mask, pipeline;
 
 	if (sensor_is_preferred(current_fmt)) {
 		pfe_cfg0 = current_fmt->reg_bps;
 		pipeline = 0x0;
-		isc_get_param(current_fmt, &rlp_mode, &dcfg_imode);
+		isc_get_param(current_fmt, &rlp_mode, &dcfg);
 		isc->ctrls.hist_stat = HIST_INIT;
 	} else {
 		pfe_cfg0  = isc->raw_fmt->reg_bps;
 		pipeline = current_fmt->pipeline;
 		rlp_mode = current_fmt->reg_rlp_mode;
-		dcfg_imode = current_fmt->reg_dcfg_imode;
+		dcfg = current_fmt->reg_dcfg_imode | ISC_DCFG_YMBSIZE_BEATS8 |
+		       ISC_DCFG_CMBSIZE_BEATS8;
 	}
 
 	pfe_cfg0  |= subdev->pfe_cfg0 | ISC_PFE_CFG0_MODE_PROGRESSIVE;
@@ -750,7 +752,7 @@ static int isc_configure(struct isc_device *isc)
 	regmap_update_bits(regmap, ISC_RLP_CFG, ISC_RLP_CFG_MODE_MASK,
 			   rlp_mode);
 
-	regmap_update_bits(regmap, ISC_DCFG, ISC_DCFG_IMODE_MASK, dcfg_imode);
+	regmap_write(regmap, ISC_DCFG, dcfg);
 
 	/* Set the pipeline */
 	isc_set_pipeline(isc, pipeline);
@@ -1684,7 +1686,7 @@ static int isc_parse_dt(struct device *dev, struct isc_device *isc)
 {
 	struct device_node *np = dev->of_node;
 	struct device_node *epn = NULL, *rem;
-	struct v4l2_of_endpoint v4l2_epn;
+	struct v4l2_fwnode_endpoint v4l2_epn;
 	struct isc_subdev_entity *subdev_entity;
 	unsigned int flags;
 	int ret;
@@ -1703,7 +1705,8 @@ static int isc_parse_dt(struct device *dev, struct isc_device *isc)
 			continue;
 		}
 
-		ret = v4l2_of_parse_endpoint(epn, &v4l2_epn);
+		ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(epn),
+						 &v4l2_epn);
 		if (ret) {
 			of_node_put(rem);
 			ret = -EINVAL;
@@ -1738,8 +1741,9 @@ static int isc_parse_dt(struct device *dev, struct isc_device *isc)
 		if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
 			subdev_entity->pfe_cfg0 |= ISC_PFE_CFG0_PPOL_LOW;
 
-		subdev_entity->asd->match_type = V4L2_ASYNC_MATCH_OF;
-		subdev_entity->asd->match.of.node = rem;
+		subdev_entity->asd->match_type = V4L2_ASYNC_MATCH_FWNODE;
+		subdev_entity->asd->match.fwnode.fwnode =
+			of_fwnode_handle(rem);
 		list_add_tail(&subdev_entity->list, &isc->subdev_entities);
 	}
 
diff --git a/drivers/media/platform/atmel/atmel-isi.c b/drivers/media/platform/atmel/atmel-isi.c
index e4867f84514c..891fa2505efa 100644
--- a/drivers/media/platform/atmel/atmel-isi.c
+++ b/drivers/media/platform/atmel/atmel-isi.c
@@ -19,6 +19,7 @@
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/of_graph.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
@@ -30,14 +31,14 @@
 #include <media/v4l2-dev.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 #include <media/videobuf2-dma-contig.h>
 #include <media/v4l2-image-sizes.h>
 
 #include "atmel-isi.h"
 
-#define MAX_SUPPORT_WIDTH		2048
-#define MAX_SUPPORT_HEIGHT		2048
+#define MAX_SUPPORT_WIDTH		2048U
+#define MAX_SUPPORT_HEIGHT		2048U
 #define MIN_FRAME_RATE			15
 #define FRAME_INTERVAL_MILLI_SEC	(1000 / MIN_FRAME_RATE)
 
@@ -424,6 +425,8 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 	struct frame_buffer *buf, *node;
 	int ret;
 
+	pm_runtime_get_sync(isi->dev);
+
 	/* Enable stream on the sub device */
 	ret = v4l2_subdev_call(isi->entity.subdev, video, s_stream, 1);
 	if (ret && ret != -ENOIOCTLCMD) {
@@ -431,8 +434,6 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 		goto err_start_stream;
 	}
 
-	pm_runtime_get_sync(isi->dev);
-
 	/* Reset ISI */
 	ret = atmel_isi_wait_status(isi, WAIT_ISI_RESET);
 	if (ret < 0) {
@@ -455,10 +456,11 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 	return 0;
 
 err_reset:
-	pm_runtime_put(isi->dev);
 	v4l2_subdev_call(isi->entity.subdev, video, s_stream, 0);
 
 err_start_stream:
+	pm_runtime_put(isi->dev);
+
 	spin_lock_irq(&isi->irqlock);
 	isi->active = NULL;
 	/* Release all active buffers */
@@ -566,20 +568,15 @@ static int isi_try_fmt(struct atmel_isi *isi, struct v4l2_format *f,
 	};
 	int ret;
 
-	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
 	isi_fmt = find_format_by_fourcc(isi, pixfmt->pixelformat);
 	if (!isi_fmt) {
 		isi_fmt = isi->user_formats[isi->num_user_formats - 1];
 		pixfmt->pixelformat = isi_fmt->fourcc;
 	}
 
-	/* Limit to Atmel ISC hardware capabilities */
-	if (pixfmt->width > MAX_SUPPORT_WIDTH)
-		pixfmt->width = MAX_SUPPORT_WIDTH;
-	if (pixfmt->height > MAX_SUPPORT_HEIGHT)
-		pixfmt->height = MAX_SUPPORT_HEIGHT;
+	/* Limit to Atmel ISI hardware capabilities */
+	pixfmt->width = clamp(pixfmt->width, 0U, MAX_SUPPORT_WIDTH);
+	pixfmt->height = clamp(pixfmt->height, 0U, MAX_SUPPORT_HEIGHT);
 
 	v4l2_fill_mbus_format(&format.format, pixfmt, isi_fmt->mbus_code);
 	ret = v4l2_subdev_call(isi->entity.subdev, pad, set_fmt,
@@ -801,7 +798,7 @@ static int atmel_isi_parse_dt(struct atmel_isi *isi,
 			struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
-	struct v4l2_of_endpoint ep;
+	struct v4l2_fwnode_endpoint ep;
 	int err;
 
 	/* Default settings for ISI */
@@ -814,7 +811,7 @@ static int atmel_isi_parse_dt(struct atmel_isi *isi,
 		return -EINVAL;
 	}
 
-	err = v4l2_of_parse_endpoint(np, &ep);
+	err = v4l2_fwnode_endpoint_parse(of_fwnode_handle(np), &ep);
 	of_node_put(np);
 	if (err) {
 		dev_err(&pdev->dev, "Could not parse the endpoint\n");
@@ -1058,7 +1055,7 @@ static int isi_graph_notify_complete(struct v4l2_async_notifier *notifier)
 	struct atmel_isi *isi = notifier_to_isi(notifier);
 	int ret;
 
-	isi->vdev->ctrl_handler	= isi->entity.subdev->ctrl_handler;
+	isi->vdev->ctrl_handler = isi->entity.subdev->ctrl_handler;
 	ret = isi_formats_init(isi);
 	if (ret) {
 		dev_err(isi->dev, "No supported mediabus format found\n");
@@ -1126,8 +1123,8 @@ static int isi_graph_parse(struct atmel_isi *isi, struct device_node *node)
 
 		/* Remote node to connect */
 		isi->entity.node = remote;
-		isi->entity.asd.match_type = V4L2_ASYNC_MATCH_OF;
-		isi->entity.asd.match.of.node = remote;
+		isi->entity.asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
+		isi->entity.asd.match.fwnode.fwnode = of_fwnode_handle(remote);
 		return 0;
 	}
 }
diff --git a/drivers/media/platform/coda/coda-bit.c b/drivers/media/platform/coda/coda-bit.c
index 403214e00e95..25cbf9e5ac5a 100644
--- a/drivers/media/platform/coda/coda-bit.c
+++ b/drivers/media/platform/coda/coda-bit.c
@@ -427,14 +427,16 @@ static int coda_alloc_framebuffers(struct coda_ctx *ctx,
 
 	/* Register frame buffers in the parameter buffer */
 	for (i = 0; i < ctx->num_internal_frames; i++) {
-		u32 y, cb, cr;
+		u32 y, cb, cr, mvcol;
 
 		/* Start addresses of Y, Cb, Cr planes */
 		y = ctx->internal_frames[i].paddr;
 		cb = y + ysize;
 		cr = y + ysize + ysize/4;
+		mvcol = y + ysize + ysize/4 + ysize/4;
 		if (ctx->tiled_map_type == GDI_TILED_FRAME_MB_RASTER_MAP) {
 			cb = round_up(cb, 4096);
+			mvcol = cb + ysize/2;
 			cr = 0;
 			/* Packed 20-bit MSB of base addresses */
 			/* YYYYYCCC, CCyyyyyc, cccc.... */
@@ -448,9 +450,7 @@ static int coda_alloc_framebuffers(struct coda_ctx *ctx,
 		/* mvcol buffer for h.264 */
 		if (ctx->codec->src_fourcc == V4L2_PIX_FMT_H264 &&
 		    dev->devtype->product != CODA_DX6)
-			coda_parabuf_write(ctx, 96 + i,
-					   ctx->internal_frames[i].paddr +
-					   ysize + ysize/4 + ysize/4);
+			coda_parabuf_write(ctx, 96 + i, mvcol);
 	}
 
 	/* mvcol buffer for mpeg4 */
@@ -1247,12 +1247,18 @@ static int coda_prepare_encode(struct coda_ctx *ctx)
 	dst_buf->sequence = ctx->osequence;
 	ctx->osequence++;
 
+	force_ipicture = ctx->params.force_ipicture;
+	if (force_ipicture)
+		ctx->params.force_ipicture = false;
+	else if ((src_buf->sequence % ctx->params.gop_size) == 0)
+		force_ipicture = 1;
+
 	/*
 	 * Workaround coda firmware BUG that only marks the first
 	 * frame as IDR. This is a problem for some decoders that can't
 	 * recover when a frame is lost.
 	 */
-	if (src_buf->sequence % ctx->params.gop_size) {
+	if (!force_ipicture) {
 		src_buf->flags |= V4L2_BUF_FLAG_PFRAME;
 		src_buf->flags &= ~V4L2_BUF_FLAG_KEYFRAME;
 	} else {
@@ -1264,10 +1270,10 @@ static int coda_prepare_encode(struct coda_ctx *ctx)
 		coda_set_gdi_regs(ctx);
 
 	/*
-	 * Copy headers at the beginning of the first frame for H.264 only.
-	 * In MPEG4 they are already copied by the coda.
+	 * Copy headers in front of the first frame and forced I frames for
+	 * H.264 only. In MPEG4 they are already copied by the CODA.
 	 */
-	if (src_buf->sequence == 0) {
+	if (src_buf->sequence == 0 || force_ipicture) {
 		pic_stream_buffer_addr =
 			vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0) +
 			ctx->vpu_header_size[0] +
@@ -1291,8 +1297,7 @@ static int coda_prepare_encode(struct coda_ctx *ctx)
 		pic_stream_buffer_size = q_data_dst->sizeimage;
 	}
 
-	if (src_buf->flags & V4L2_BUF_FLAG_KEYFRAME) {
-		force_ipicture = 1;
+	if (force_ipicture) {
 		switch (dst_fourcc) {
 		case V4L2_PIX_FMT_H264:
 			quant_param = ctx->params.h264_intra_qp;
@@ -1309,7 +1314,6 @@ static int coda_prepare_encode(struct coda_ctx *ctx)
 			break;
 		}
 	} else {
-		force_ipicture = 0;
 		switch (dst_fourcc) {
 		case V4L2_PIX_FMT_H264:
 			quant_param = ctx->params.h264_inter_qp;
@@ -1382,7 +1386,8 @@ static void coda_finish_encode(struct coda_ctx *ctx)
 	wr_ptr = coda_read(dev, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
 
 	/* Calculate bytesused field */
-	if (dst_buf->sequence == 0) {
+	if (dst_buf->sequence == 0 ||
+	    src_buf->flags & V4L2_BUF_FLAG_KEYFRAME) {
 		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, wr_ptr - start_ptr +
 					ctx->vpu_header_size[0] +
 					ctx->vpu_header_size[1] +
@@ -2193,12 +2198,32 @@ static void coda_finish_decode(struct coda_ctx *ctx)
 	ctx->display_idx = display_idx;
 }
 
+static void coda_error_decode(struct coda_ctx *ctx)
+{
+	struct vb2_v4l2_buffer *dst_buf;
+
+	/*
+	 * For now this only handles the case where we would deadlock with
+	 * userspace, i.e. userspace issued DEC_CMD_STOP and waits for EOS,
+	 * but after a failed decode run we would hold the context and wait for
+	 * userspace to queue more buffers.
+	 */
+	if (!(ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG))
+		return;
+
+	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+	dst_buf->sequence = ctx->qsequence - 1;
+
+	coda_m2m_buf_done(ctx, dst_buf, VB2_BUF_STATE_ERROR);
+}
+
 const struct coda_context_ops coda_bit_decode_ops = {
 	.queue_init = coda_decoder_queue_init,
 	.reqbufs = coda_decoder_reqbufs,
 	.start_streaming = coda_start_decoding,
 	.prepare_run = coda_prepare_decode,
 	.finish_run = coda_finish_decode,
+	.error_run = coda_error_decode,
 	.seq_end_work = coda_seq_end_work,
 	.release = coda_bit_release,
 };
diff --git a/drivers/media/platform/coda/coda-common.c b/drivers/media/platform/coda/coda-common.c
index d523e990d509..f92cc7df58fb 100644
--- a/drivers/media/platform/coda/coda-common.c
+++ b/drivers/media/platform/coda/coda-common.c
@@ -430,10 +430,10 @@ static int coda_g_fmt(struct file *file, void *priv,
 	f->fmt.pix.bytesperline = q_data->bytesperline;
 
 	f->fmt.pix.sizeimage	= q_data->sizeimage;
-	if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG)
-		f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
-	else
-		f->fmt.pix.colorspace = ctx->colorspace;
+	f->fmt.pix.colorspace	= ctx->colorspace;
+	f->fmt.pix.xfer_func	= ctx->xfer_func;
+	f->fmt.pix.ycbcr_enc	= ctx->ycbcr_enc;
+	f->fmt.pix.quantization	= ctx->quantization;
 
 	return 0;
 }
@@ -599,6 +599,9 @@ static int coda_try_fmt_vid_cap(struct file *file, void *priv,
 	}
 
 	f->fmt.pix.colorspace = ctx->colorspace;
+	f->fmt.pix.xfer_func = ctx->xfer_func;
+	f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc;
+	f->fmt.pix.quantization = ctx->quantization;
 
 	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
 	codec = coda_find_codec(ctx->dev, q_data_src->fourcc,
@@ -612,7 +615,6 @@ static int coda_try_fmt_vid_cap(struct file *file, void *priv,
 
 	/* The h.264 decoder only returns complete 16x16 macroblocks */
 	if (codec && codec->src_fourcc == V4L2_PIX_FMT_H264) {
-		f->fmt.pix.width = f->fmt.pix.width;
 		f->fmt.pix.height = round_up(f->fmt.pix.height, 16);
 		f->fmt.pix.bytesperline = round_up(f->fmt.pix.width, 16);
 		f->fmt.pix.sizeimage = f->fmt.pix.bytesperline *
@@ -635,6 +637,23 @@ static int coda_try_fmt_vid_cap(struct file *file, void *priv,
 	return 0;
 }
 
+static void coda_set_default_colorspace(struct v4l2_pix_format *fmt)
+{
+	enum v4l2_colorspace colorspace;
+
+	if (fmt->pixelformat == V4L2_PIX_FMT_JPEG)
+		colorspace = V4L2_COLORSPACE_JPEG;
+	else if (fmt->width <= 720 && fmt->height <= 576)
+		colorspace = V4L2_COLORSPACE_SMPTE170M;
+	else
+		colorspace = V4L2_COLORSPACE_REC709;
+
+	fmt->colorspace = colorspace;
+	fmt->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+	fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	fmt->quantization = V4L2_QUANTIZATION_DEFAULT;
+}
+
 static int coda_try_fmt_vid_out(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
@@ -648,16 +667,8 @@ static int coda_try_fmt_vid_out(struct file *file, void *priv,
 	if (ret < 0)
 		return ret;
 
-	switch (f->fmt.pix.colorspace) {
-	case V4L2_COLORSPACE_REC709:
-	case V4L2_COLORSPACE_JPEG:
-		break;
-	default:
-		if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG)
-			f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
-		else
-			f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
-	}
+	if (f->fmt.pix.colorspace == V4L2_COLORSPACE_DEFAULT)
+		coda_set_default_colorspace(&f->fmt.pix);
 
 	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
 	codec = coda_find_codec(dev, f->fmt.pix.pixelformat, q_data_dst->fourcc);
@@ -772,6 +783,9 @@ static int coda_s_fmt_vid_out(struct file *file, void *priv,
 		return ret;
 
 	ctx->colorspace = f->fmt.pix.colorspace;
+	ctx->xfer_func = f->fmt.pix.xfer_func;
+	ctx->ycbcr_enc = f->fmt.pix.ycbcr_enc;
+	ctx->quantization = f->fmt.pix.quantization;
 
 	memset(&f_cap, 0, sizeof(f_cap));
 	f_cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
@@ -1149,6 +1163,9 @@ static void coda_pic_run_work(struct work_struct *work)
 		ctx->hold = true;
 
 		coda_hw_reset(ctx);
+
+		if (ctx->ops->error_run)
+			ctx->ops->error_run(ctx);
 	} else if (!ctx->aborting) {
 		ctx->ops->finish_run(ctx);
 	}
@@ -1282,7 +1299,13 @@ static void set_default_params(struct coda_ctx *ctx)
 	csize = coda_estimate_sizeimage(ctx, usize, max_w, max_h);
 
 	ctx->params.codec_mode = ctx->codec->mode;
-	ctx->colorspace = V4L2_COLORSPACE_REC709;
+	if (ctx->cvd->src_formats[0] == V4L2_PIX_FMT_JPEG)
+		ctx->colorspace = V4L2_COLORSPACE_JPEG;
+	else
+		ctx->colorspace = V4L2_COLORSPACE_REC709;
+	ctx->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+	ctx->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	ctx->quantization = V4L2_QUANTIZATION_DEFAULT;
 	ctx->params.framerate = 30;
 
 	/* Default formats for output and input queues */
@@ -1680,6 +1703,9 @@ static int coda_s_ctrl(struct v4l2_ctrl *ctrl)
 	case V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB:
 		ctx->params.intra_refresh = ctrl->val;
 		break;
+	case V4L2_CID_MPEG_VIDEO_FORCE_KEY_FRAME:
+		ctx->params.force_ipicture = true;
+		break;
 	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
 		coda_set_jpeg_compression_quality(ctx, ctrl->val);
 		break;
@@ -2063,8 +2089,7 @@ static int coda_hw_init(struct coda_dev *dev)
 	if (ret)
 		goto err_clk_ahb;
 
-	if (dev->rstc)
-		reset_control_reset(dev->rstc);
+	reset_control_reset(dev->rstc);
 
 	/*
 	 * Copy the first CODA_ISRAM_SIZE in the internal SRAM.
@@ -2448,13 +2473,8 @@ static int coda_probe(struct platform_device *pdev)
 	dev->rstc = devm_reset_control_get_optional(&pdev->dev, NULL);
 	if (IS_ERR(dev->rstc)) {
 		ret = PTR_ERR(dev->rstc);
-		if (ret == -ENOENT || ret == -ENOTSUPP) {
-			dev->rstc = NULL;
-		} else {
-			dev_err(&pdev->dev, "failed get reset control: %d\n",
-				ret);
-			return ret;
-		}
+		dev_err(&pdev->dev, "failed get reset control: %d\n", ret);
+		return ret;
 	}
 
 	/* Get IRAM pool from device tree or platform data */
diff --git a/drivers/media/platform/coda/coda.h b/drivers/media/platform/coda/coda.h
index 20222befb9b2..40fe22f0d757 100644
--- a/drivers/media/platform/coda/coda.h
+++ b/drivers/media/platform/coda/coda.h
@@ -135,6 +135,7 @@ struct coda_params {
 	u32			vbv_size;
 	u32			slice_max_bits;
 	u32			slice_max_mb;
+	bool			force_ipicture;
 };
 
 struct coda_buffer_meta {
@@ -182,6 +183,7 @@ struct coda_context_ops {
 	int (*start_streaming)(struct coda_ctx *ctx);
 	int (*prepare_run)(struct coda_ctx *ctx);
 	void (*finish_run)(struct coda_ctx *ctx);
+	void (*error_run)(struct coda_ctx *ctx);
 	void (*seq_end_work)(struct work_struct *work);
 	void (*release)(struct coda_ctx *ctx);
 };
@@ -206,6 +208,9 @@ struct coda_ctx {
 	enum coda_inst_type		inst_type;
 	const struct coda_codec		*codec;
 	enum v4l2_colorspace		colorspace;
+	enum v4l2_xfer_func		xfer_func;
+	enum v4l2_ycbcr_encoding	ycbcr_enc;
+	enum v4l2_quantization		quantization;
 	struct coda_params		params;
 	struct v4l2_ctrl_handler	ctrls;
 	struct v4l2_fh			fh;
diff --git a/drivers/media/platform/coda/imx-vdoa.c b/drivers/media/platform/coda/imx-vdoa.c
index 669a4c82f1ff..df9b71621420 100644
--- a/drivers/media/platform/coda/imx-vdoa.c
+++ b/drivers/media/platform/coda/imx-vdoa.c
@@ -101,6 +101,8 @@ struct vdoa_ctx {
 	struct vdoa_data	*vdoa;
 	struct completion	completion;
 	struct vdoa_q_data	q_data[2];
+	unsigned int		submitted_job;
+	unsigned int		completed_job;
 };
 
 static irqreturn_t vdoa_irq_handler(int irq, void *data)
@@ -114,7 +116,7 @@ static irqreturn_t vdoa_irq_handler(int irq, void *data)
 
 	curr_ctx = vdoa->curr_ctx;
 	if (!curr_ctx) {
-		dev_dbg(vdoa->dev,
+		dev_warn(vdoa->dev,
 			"Instance released before the end of transaction\n");
 		return IRQ_HANDLED;
 	}
@@ -127,19 +129,44 @@ static irqreturn_t vdoa_irq_handler(int irq, void *data)
 	} else if (!(val & VDOAIST_EOT)) {
 		dev_warn(vdoa->dev, "Spurious interrupt\n");
 	}
+	curr_ctx->completed_job++;
 	complete(&curr_ctx->completion);
 
 	return IRQ_HANDLED;
 }
 
+int vdoa_wait_for_completion(struct vdoa_ctx *ctx)
+{
+	struct vdoa_data *vdoa = ctx->vdoa;
+
+	if (ctx->submitted_job == ctx->completed_job)
+		return 0;
+
+	if (!wait_for_completion_timeout(&ctx->completion,
+					 msecs_to_jiffies(300))) {
+		dev_err(vdoa->dev,
+			"Timeout waiting for transfer result\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(vdoa_wait_for_completion);
+
 void vdoa_device_run(struct vdoa_ctx *ctx, dma_addr_t dst, dma_addr_t src)
 {
 	struct vdoa_q_data *src_q_data, *dst_q_data;
 	struct vdoa_data *vdoa = ctx->vdoa;
 	u32 val;
 
+	if (vdoa->curr_ctx)
+		vdoa_wait_for_completion(vdoa->curr_ctx);
+
 	vdoa->curr_ctx = ctx;
 
+	reinit_completion(&ctx->completion);
+	ctx->submitted_job++;
+
 	src_q_data = &ctx->q_data[V4L2_M2M_SRC];
 	dst_q_data = &ctx->q_data[V4L2_M2M_DST];
 
@@ -177,21 +204,6 @@ void vdoa_device_run(struct vdoa_ctx *ctx, dma_addr_t dst, dma_addr_t src)
 }
 EXPORT_SYMBOL(vdoa_device_run);
 
-int vdoa_wait_for_completion(struct vdoa_ctx *ctx)
-{
-	struct vdoa_data *vdoa = ctx->vdoa;
-
-	if (!wait_for_completion_timeout(&ctx->completion,
-					 msecs_to_jiffies(300))) {
-		dev_err(vdoa->dev,
-			"Timeout waiting for transfer result\n");
-		return -ETIMEDOUT;
-	}
-
-	return 0;
-}
-EXPORT_SYMBOL(vdoa_wait_for_completion);
-
 struct vdoa_ctx *vdoa_context_create(struct vdoa_data *vdoa)
 {
 	struct vdoa_ctx *ctx;
@@ -218,6 +230,11 @@ void vdoa_context_destroy(struct vdoa_ctx *ctx)
 {
 	struct vdoa_data *vdoa = ctx->vdoa;
 
+	if (vdoa->curr_ctx == ctx) {
+		vdoa_wait_for_completion(vdoa->curr_ctx);
+		vdoa->curr_ctx = NULL;
+	}
+
 	clk_disable_unprepare(vdoa->vdoa_clk);
 	kfree(ctx);
 }
diff --git a/drivers/media/platform/davinci/Kconfig b/drivers/media/platform/davinci/Kconfig
index 554e710de487..55982e681d77 100644
--- a/drivers/media/platform/davinci/Kconfig
+++ b/drivers/media/platform/davinci/Kconfig
@@ -22,6 +22,7 @@ config VIDEO_DAVINCI_VPIF_CAPTURE
 	depends on HAS_DMA
 	depends on I2C
 	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_FWNODE
 	help
 	  Enables Davinci VPIF module used for capture devices.
 	  This module is used for capture on TI DM6467/DA850/OMAPL138
diff --git a/drivers/media/platform/davinci/vpif.c b/drivers/media/platform/davinci/vpif.c
index 1b02a6363f77..07e89a4985a6 100644
--- a/drivers/media/platform/davinci/vpif.c
+++ b/drivers/media/platform/davinci/vpif.c
@@ -26,6 +26,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/spinlock.h>
 #include <linux/v4l2-dv-timings.h>
+#include <linux/of_graph.h>
 
 #include "vpif.h"
 
@@ -423,7 +424,9 @@ EXPORT_SYMBOL(vpif_channel_getfid);
 
 static int vpif_probe(struct platform_device *pdev)
 {
-	static struct resource	*res;
+	static struct resource	*res, *res_irq;
+	struct platform_device *pdev_capture, *pdev_display;
+	struct device_node *endpoint = NULL;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	vpif_base = devm_ioremap_resource(&pdev->dev, res);
@@ -435,6 +438,58 @@ static int vpif_probe(struct platform_device *pdev)
 
 	spin_lock_init(&vpif_lock);
 	dev_info(&pdev->dev, "vpif probe success\n");
+
+	/*
+	 * If VPIF Node has endpoints, assume "new" DT support,
+	 * where capture and display drivers don't have DT nodes
+	 * so their devices need to be registered manually here
+	 * for their legacy platform_drivers to work.
+	 */
+	endpoint = of_graph_get_next_endpoint(pdev->dev.of_node,
+					      endpoint);
+	if (!endpoint)
+		return 0;
+
+	/*
+	 * For DT platforms, manually create platform_devices for
+	 * capture/display drivers.
+	 */
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq) {
+		dev_warn(&pdev->dev, "Missing IRQ resource.\n");
+		return -EINVAL;
+	}
+
+	pdev_capture = devm_kzalloc(&pdev->dev, sizeof(*pdev_capture),
+				    GFP_KERNEL);
+	if (pdev_capture) {
+		pdev_capture->name = "vpif_capture";
+		pdev_capture->id = -1;
+		pdev_capture->resource = res_irq;
+		pdev_capture->num_resources = 1;
+		pdev_capture->dev.dma_mask = pdev->dev.dma_mask;
+		pdev_capture->dev.coherent_dma_mask = pdev->dev.coherent_dma_mask;
+		pdev_capture->dev.parent = &pdev->dev;
+		platform_device_register(pdev_capture);
+	} else {
+		dev_warn(&pdev->dev, "Unable to allocate memory for pdev_capture.\n");
+	}
+
+	pdev_display = devm_kzalloc(&pdev->dev, sizeof(*pdev_display),
+				    GFP_KERNEL);
+	if (pdev_display) {
+		pdev_display->name = "vpif_display";
+		pdev_display->id = -1;
+		pdev_display->resource = res_irq;
+		pdev_display->num_resources = 1;
+		pdev_display->dev.dma_mask = pdev->dev.dma_mask;
+		pdev_display->dev.coherent_dma_mask = pdev->dev.coherent_dma_mask;
+		pdev_display->dev.parent = &pdev->dev;
+		platform_device_register(pdev_display);
+	} else {
+		dev_warn(&pdev->dev, "Unable to allocate memory for pdev_display.\n");
+	}
+
 	return 0;
 }
 
diff --git a/drivers/media/platform/davinci/vpif_capture.c b/drivers/media/platform/davinci/vpif_capture.c
index 44f702752d3a..d78580f9e431 100644
--- a/drivers/media/platform/davinci/vpif_capture.c
+++ b/drivers/media/platform/davinci/vpif_capture.c
@@ -18,10 +18,16 @@
 
 #include <linux/module.h>
 #include <linux/interrupt.h>
+#include <linux/of_graph.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 
+#include <media/v4l2-fwnode.h>
 #include <media/v4l2-ioctl.h>
+#include <media/i2c/tvp514x.h>
+#include <media/v4l2-mediabus.h>
+
+#include <linux/videodev2.h>
 
 #include "vpif.h"
 #include "vpif_capture.h"
@@ -385,7 +391,8 @@ static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
 		common = &ch->common[i];
 		/* skip If streaming is not started in this channel */
 		/* Check the field format */
-		if (1 == ch->vpifparams.std_info.frm_fmt) {
+		if (1 == ch->vpifparams.std_info.frm_fmt ||
+		    common->fmt.fmt.pix.field == V4L2_FIELD_NONE) {
 			/* Progressive mode */
 			spin_lock(&common->irqlock);
 			if (list_empty(&common->dma_queue)) {
@@ -466,9 +473,38 @@ static int vpif_update_std_info(struct channel_obj *ch)
 	struct vpif_channel_config_params *std_info = &vpifparams->std_info;
 	struct video_obj *vid_ch = &ch->video;
 	int index;
+	struct v4l2_pix_format *pixfmt = &common->fmt.fmt.pix;
 
 	vpif_dbg(2, debug, "vpif_update_std_info\n");
 
+	/*
+	 * if called after try_fmt or g_fmt, there will already be a size
+	 * so use that by default.
+	 */
+	if (pixfmt->width && pixfmt->height) {
+		if (pixfmt->field == V4L2_FIELD_ANY ||
+		    pixfmt->field == V4L2_FIELD_NONE)
+			pixfmt->field = V4L2_FIELD_NONE;
+
+		vpifparams->iface.if_type = VPIF_IF_BT656;
+		if (pixfmt->pixelformat == V4L2_PIX_FMT_SGRBG10 ||
+		    pixfmt->pixelformat == V4L2_PIX_FMT_SBGGR8)
+			vpifparams->iface.if_type = VPIF_IF_RAW_BAYER;
+
+		if (pixfmt->pixelformat == V4L2_PIX_FMT_SGRBG10)
+			vpifparams->params.data_sz = 1; /* 10 bits/pixel.  */
+
+		/*
+		 * For raw formats from camera sensors, we don't need
+		 * the std_info from table lookup, so nothing else to do here.
+		 */
+		if (vpifparams->iface.if_type == VPIF_IF_RAW_BAYER) {
+			memset(std_info, 0, sizeof(struct vpif_channel_config_params));
+			vpifparams->std_info.capture_format = 1; /* CCD/raw mode */
+			return 0;
+		}
+	}
+
 	for (index = 0; index < vpif_ch_params_count; index++) {
 		config = &vpif_ch_params[index];
 		if (config->hd_sd == 0) {
@@ -513,7 +549,7 @@ static int vpif_update_std_info(struct channel_obj *ch)
 	if (ch->vpifparams.iface.if_type == VPIF_IF_RAW_BAYER)
 		common->fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SBGGR8;
 	else
-		common->fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422P;
+		common->fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_NV16;
 
 	common->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
@@ -655,7 +691,7 @@ static int vpif_input_to_subdev(
 	/* loop through the sub device list to get the sub device info */
 	for (i = 0; i < vpif_cfg->subdev_count; i++) {
 		subdev_info = &vpif_cfg->subdev_info[i];
-		if (!strcmp(subdev_info->name, subdev_name))
+		if (subdev_info && !strcmp(subdev_info->name, subdev_name))
 			return i;
 	}
 	return -1;
@@ -917,8 +953,8 @@ static int vpif_enum_fmt_vid_cap(struct file *file, void  *priv,
 		fmt->pixelformat = V4L2_PIX_FMT_SBGGR8;
 	} else {
 		fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		strcpy(fmt->description, "YCbCr4:2:2 YC Planar");
-		fmt->pixelformat = V4L2_PIX_FMT_YUV422P;
+		strcpy(fmt->description, "YCbCr4:2:2 Semi-Planar");
+		fmt->pixelformat = V4L2_PIX_FMT_NV16;
 	}
 	return 0;
 }
@@ -936,22 +972,8 @@ static int vpif_try_fmt_vid_cap(struct file *file, void *priv,
 	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
 	struct common_obj *common = &(ch->common[VPIF_VIDEO_INDEX]);
-	struct vpif_params *vpif_params = &ch->vpifparams;
-
-	/*
-	 * to supress v4l-compliance warnings silently correct
-	 * the pixelformat
-	 */
-	if (vpif_params->iface.if_type == VPIF_IF_RAW_BAYER) {
-		if (pixfmt->pixelformat != V4L2_PIX_FMT_SBGGR8)
-			pixfmt->pixelformat = V4L2_PIX_FMT_SBGGR8;
-	} else {
-		if (pixfmt->pixelformat != V4L2_PIX_FMT_YUV422P)
-			pixfmt->pixelformat = V4L2_PIX_FMT_YUV422P;
-	}
-
-	common->fmt.fmt.pix.pixelformat = pixfmt->pixelformat;
 
+	common->fmt = *fmt;
 	vpif_update_std_info(ch);
 
 	pixfmt->field = common->fmt.fmt.pix.field;
@@ -960,8 +982,17 @@ static int vpif_try_fmt_vid_cap(struct file *file, void *priv,
 	pixfmt->width = common->fmt.fmt.pix.width;
 	pixfmt->height = common->fmt.fmt.pix.height;
 	pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height * 2;
+	if (pixfmt->pixelformat == V4L2_PIX_FMT_SGRBG10) {
+		pixfmt->bytesperline = common->fmt.fmt.pix.width * 2;
+		pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
+	}
 	pixfmt->priv = 0;
 
+	dev_dbg(vpif_dev, "%s: %d x %d; pitch=%d pixelformat=0x%08x, field=%d, size=%d\n", __func__,
+		pixfmt->width, pixfmt->height,
+		pixfmt->bytesperline, pixfmt->pixelformat,
+		pixfmt->field, pixfmt->sizeimage);
+
 	return 0;
 }
 
@@ -978,13 +1009,47 @@ static int vpif_g_fmt_vid_cap(struct file *file, void *priv,
 	struct video_device *vdev = video_devdata(file);
 	struct channel_obj *ch = video_get_drvdata(vdev);
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct v4l2_pix_format *pix_fmt = &fmt->fmt.pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mbus_fmt = &format.format;
+	int ret;
 
 	/* Check the validity of the buffer type */
 	if (common->fmt.type != fmt->type)
 		return -EINVAL;
 
-	/* Fill in the information about format */
+	/* By default, use currently set fmt */
 	*fmt = common->fmt;
+
+	/* If subdev has get_fmt, use that to override */
+	ret = v4l2_subdev_call(ch->sd, pad, get_fmt, NULL, &format);
+	if (!ret && mbus_fmt->code) {
+		v4l2_fill_pix_format(pix_fmt, mbus_fmt);
+		pix_fmt->bytesperline = pix_fmt->width;
+		if (mbus_fmt->code == MEDIA_BUS_FMT_SGRBG10_1X10) {
+			/* e.g. mt9v032 */
+			pix_fmt->pixelformat = V4L2_PIX_FMT_SGRBG10;
+			pix_fmt->bytesperline = pix_fmt->width * 2;
+		} else if (mbus_fmt->code == MEDIA_BUS_FMT_UYVY8_2X8) {
+			/* e.g. tvp514x */
+			pix_fmt->pixelformat = V4L2_PIX_FMT_NV16;
+			pix_fmt->bytesperline = pix_fmt->width * 2;
+		} else {
+			dev_warn(vpif_dev, "%s: Unhandled media-bus format 0x%x\n",
+				 __func__, mbus_fmt->code);
+		}
+		pix_fmt->sizeimage = pix_fmt->bytesperline * pix_fmt->height;
+		dev_dbg(vpif_dev, "%s: %d x %d; pitch=%d, pixelformat=0x%08x, code=0x%x, field=%d, size=%d\n", __func__,
+			pix_fmt->width, pix_fmt->height,
+			pix_fmt->bytesperline, pix_fmt->pixelformat,
+			mbus_fmt->code, pix_fmt->field, pix_fmt->sizeimage);
+
+		common->fmt = *fmt;
+		vpif_update_std_info(ch);
+	}
+
 	return 0;
 }
 
@@ -1323,6 +1388,22 @@ static int vpif_async_bound(struct v4l2_async_notifier *notifier,
 {
 	int i;
 
+	for (i = 0; i < vpif_obj.config->asd_sizes[0]; i++) {
+		struct v4l2_async_subdev *_asd = vpif_obj.config->asd[i];
+		const struct fwnode_handle *fwnode = _asd->match.fwnode.fwnode;
+
+		if (fwnode == subdev->fwnode) {
+			vpif_obj.sd[i] = subdev;
+			vpif_obj.config->chan_config->inputs[i].subdev_name =
+				(char *)to_of_node(subdev->fwnode)->full_name;
+			vpif_dbg(2, debug,
+				 "%s: setting input %d subdev_name = %s\n",
+				 __func__, i,
+				 to_of_node(subdev->fwnode)->full_name);
+			return 0;
+		}
+	}
+
 	for (i = 0; i < vpif_obj.config->subdev_count; i++)
 		if (!strcmp(vpif_obj.config->subdev_info[i].name,
 			    subdev->name)) {
@@ -1356,6 +1437,7 @@ static int vpif_probe_complete(void)
 		/* set initial format */
 		ch->video.stdid = V4L2_STD_525_60;
 		memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings));
+		common->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 		vpif_update_std_info(ch);
 
 		/* Initialize vb2 queue */
@@ -1418,6 +1500,106 @@ static int vpif_async_complete(struct v4l2_async_notifier *notifier)
 	return vpif_probe_complete();
 }
 
+static struct vpif_capture_config *
+vpif_capture_get_pdata(struct platform_device *pdev)
+{
+	struct device_node *endpoint = NULL;
+	struct v4l2_fwnode_endpoint bus_cfg;
+	struct vpif_capture_config *pdata;
+	struct vpif_subdev_info *sdinfo;
+	struct vpif_capture_chan_config *chan;
+	unsigned int i;
+
+	/*
+	 * DT boot: OF node from parent device contains
+	 * video ports & endpoints data.
+	 */
+	if (pdev->dev.parent && pdev->dev.parent->of_node)
+		pdev->dev.of_node = pdev->dev.parent->of_node;
+	if (!IS_ENABLED(CONFIG_OF) || !pdev->dev.of_node)
+		return pdev->dev.platform_data;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return NULL;
+	pdata->subdev_info =
+		devm_kzalloc(&pdev->dev, sizeof(*pdata->subdev_info) *
+			     VPIF_CAPTURE_NUM_CHANNELS, GFP_KERNEL);
+
+	if (!pdata->subdev_info)
+		return NULL;
+
+	for (i = 0; i < VPIF_CAPTURE_NUM_CHANNELS; i++) {
+		struct device_node *rem;
+		unsigned int flags;
+		int err;
+
+		endpoint = of_graph_get_next_endpoint(pdev->dev.of_node,
+						      endpoint);
+		if (!endpoint)
+			break;
+
+		sdinfo = &pdata->subdev_info[i];
+		chan = &pdata->chan_config[i];
+		chan->inputs = devm_kzalloc(&pdev->dev,
+					    sizeof(*chan->inputs) *
+					    VPIF_CAPTURE_NUM_CHANNELS,
+					    GFP_KERNEL);
+
+		chan->input_count++;
+		chan->inputs[i].input.type = V4L2_INPUT_TYPE_CAMERA;
+		chan->inputs[i].input.std = V4L2_STD_ALL;
+		chan->inputs[i].input.capabilities = V4L2_IN_CAP_STD;
+
+		err = v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint),
+						 &bus_cfg);
+		if (err) {
+			dev_err(&pdev->dev, "Could not parse the endpoint\n");
+			goto done;
+		}
+		dev_dbg(&pdev->dev, "Endpoint %s, bus_width = %d\n",
+			endpoint->full_name, bus_cfg.bus.parallel.bus_width);
+		flags = bus_cfg.bus.parallel.flags;
+
+		if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+			chan->vpif_if.hd_pol = 1;
+
+		if (flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+			chan->vpif_if.vd_pol = 1;
+
+		rem = of_graph_get_remote_port_parent(endpoint);
+		if (!rem) {
+			dev_dbg(&pdev->dev, "Remote device at %s not found\n",
+				endpoint->full_name);
+			goto done;
+		}
+
+		dev_dbg(&pdev->dev, "Remote device %s, %s found\n",
+			rem->name, rem->full_name);
+		sdinfo->name = rem->full_name;
+
+		pdata->asd[i] = devm_kzalloc(&pdev->dev,
+					     sizeof(struct v4l2_async_subdev),
+					     GFP_KERNEL);
+		if (!pdata->asd[i]) {
+			of_node_put(rem);
+			pdata = NULL;
+			goto done;
+		}
+
+		pdata->asd[i]->match_type = V4L2_ASYNC_MATCH_FWNODE;
+		pdata->asd[i]->match.fwnode.fwnode = of_fwnode_handle(rem);
+		of_node_put(rem);
+	}
+
+done:
+	pdata->asd_sizes[0] = i;
+	pdata->subdev_count = i;
+	pdata->card_name = "DA850/OMAP-L138 Video Capture";
+
+	return pdata;
+}
+
 /**
  * vpif_probe : This function probes the vpif capture driver
  * @pdev: platform device pointer
@@ -1434,6 +1616,12 @@ static __init int vpif_probe(struct platform_device *pdev)
 	int res_idx = 0;
 	int i, err;
 
+	pdev->dev.platform_data = vpif_capture_get_pdata(pdev);
+	if (!pdev->dev.platform_data) {
+		dev_warn(&pdev->dev, "Missing platform data.  Giving up.\n");
+		return -EINVAL;
+	}
+
 	if (!pdev->dev.platform_data) {
 		dev_warn(&pdev->dev, "Missing platform data.  Giving up.\n");
 		return -EINVAL;
@@ -1474,7 +1662,7 @@ static __init int vpif_probe(struct platform_device *pdev)
 		goto vpif_unregister;
 	}
 
-	if (!vpif_obj.config->asd_sizes) {
+	if (!vpif_obj.config->asd_sizes[0]) {
 		int i2c_id = vpif_obj.config->i2c_adapter_id;
 
 		i2c_adap = i2c_get_adapter(i2c_id);
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
index 7e5cf9923c8d..b5ac6ce626b3 100644
--- a/drivers/media/platform/davinci/vpif_display.c
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -1250,6 +1250,11 @@ static __init int vpif_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
+	if (!pdev->dev.platform_data) {
+		dev_warn(&pdev->dev, "Missing platform data.  Giving up.\n");
+		return -EINVAL;
+	}
+
 	vpif_dev = &pdev->dev;
 	err = initialize_vpif();
 
diff --git a/drivers/media/platform/exynos-gsc/gsc-core.c b/drivers/media/platform/exynos-gsc/gsc-core.c
index 59a634201830..43801509dabb 100644
--- a/drivers/media/platform/exynos-gsc/gsc-core.c
+++ b/drivers/media/platform/exynos-gsc/gsc-core.c
@@ -454,6 +454,7 @@ int gsc_try_fmt_mplane(struct gsc_ctx *ctx, struct v4l2_format *f)
 	} else {
 		min_w = variant->pix_min->target_rot_dis_w;
 		min_h = variant->pix_min->target_rot_dis_h;
+		pix_mp->colorspace = ctx->out_colorspace;
 	}
 
 	pr_debug("mod_x: %d, mod_y: %d, max_w: %d, max_h = %d",
@@ -472,10 +473,8 @@ int gsc_try_fmt_mplane(struct gsc_ctx *ctx, struct v4l2_format *f)
 
 	pix_mp->num_planes = fmt->num_planes;
 
-	if (pix_mp->width >= 1280) /* HD */
-		pix_mp->colorspace = V4L2_COLORSPACE_REC709;
-	else /* SD */
-		pix_mp->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	if (V4L2_TYPE_IS_OUTPUT(f->type))
+		ctx->out_colorspace = pix_mp->colorspace;
 
 	for (i = 0; i < pix_mp->num_planes; ++i) {
 		struct v4l2_plane_pix_format *plane_fmt = &pix_mp->plane_fmt[i];
@@ -519,8 +518,8 @@ int gsc_g_fmt_mplane(struct gsc_ctx *ctx, struct v4l2_format *f)
 	pix_mp->height		= frame->f_height;
 	pix_mp->field		= V4L2_FIELD_NONE;
 	pix_mp->pixelformat	= frame->fmt->pixelformat;
-	pix_mp->colorspace	= V4L2_COLORSPACE_REC709;
 	pix_mp->num_planes	= frame->fmt->num_planes;
+	pix_mp->colorspace = ctx->out_colorspace;
 
 	for (i = 0; i < pix_mp->num_planes; ++i) {
 		pix_mp->plane_fmt[i].bytesperline = (frame->f_width *
@@ -569,9 +568,9 @@ int gsc_try_crop(struct gsc_ctx *ctx, struct v4l2_crop *cr)
 	}
 	pr_debug("user put w: %d, h: %d", cr->c.width, cr->c.height);
 
-	if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+	if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		f = &ctx->d_frame;
-	else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+	else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
 		f = &ctx->s_frame;
 	else
 		return -EINVAL;
diff --git a/drivers/media/platform/exynos-gsc/gsc-core.h b/drivers/media/platform/exynos-gsc/gsc-core.h
index 696217e9af66..715d9c9d8d30 100644
--- a/drivers/media/platform/exynos-gsc/gsc-core.h
+++ b/drivers/media/platform/exynos-gsc/gsc-core.h
@@ -376,6 +376,7 @@ struct gsc_ctx {
 	struct v4l2_ctrl_handler ctrl_handler;
 	struct gsc_ctrls	gsc_ctrls;
 	bool			ctrls_rdy;
+	enum v4l2_colorspace out_colorspace;
 };
 
 void gsc_set_prefbuf(struct gsc_dev *gsc, struct gsc_frame *frm);
diff --git a/drivers/media/platform/exynos-gsc/gsc-m2m.c b/drivers/media/platform/exynos-gsc/gsc-m2m.c
index 82505025d96c..33611a46ce35 100644
--- a/drivers/media/platform/exynos-gsc/gsc-m2m.c
+++ b/drivers/media/platform/exynos-gsc/gsc-m2m.c
@@ -460,8 +460,8 @@ static int gsc_m2m_g_selection(struct file *file, void *fh,
 	struct gsc_frame *frame;
 	struct gsc_ctx *ctx = fh_to_ctx(fh);
 
-	if ((s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) &&
-	    (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE))
+	if ((s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
+	    (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT))
 		return -EINVAL;
 
 	frame = ctx_get_frame(ctx, s->type);
@@ -503,8 +503,8 @@ static int gsc_m2m_s_selection(struct file *file, void *fh,
 	cr.type = s->type;
 	cr.c = s->r;
 
-	if ((s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) &&
-	    (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE))
+	if ((s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
+	    (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT))
 		return -EINVAL;
 
 	ret = gsc_try_crop(ctx, &cr);
diff --git a/drivers/media/platform/exynos4-is/Kconfig b/drivers/media/platform/exynos4-is/Kconfig
index 57d42c6172c5..c480efb755f5 100644
--- a/drivers/media/platform/exynos4-is/Kconfig
+++ b/drivers/media/platform/exynos4-is/Kconfig
@@ -4,6 +4,7 @@ config VIDEO_SAMSUNG_EXYNOS4_IS
 	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
 	depends on ARCH_S5PV210 || ARCH_EXYNOS || COMPILE_TEST
 	depends on OF && COMMON_CLK
+	select V4L2_FWNODE
 	help
 	  Say Y here to enable camera host interface devices for
 	  Samsung S5P and EXYNOS SoC series.
@@ -32,6 +33,7 @@ config VIDEO_S5P_MIPI_CSIS
 	tristate "S5P/EXYNOS MIPI-CSI2 receiver (MIPI-CSIS) driver"
 	depends on REGULATOR
 	select GENERIC_PHY
+	select V4L2_FWNODE
 	help
 	  This is a V4L2 driver for Samsung S5P and EXYNOS4 SoC MIPI-CSI2
 	  receiver (MIPI-CSIS) devices.
diff --git a/drivers/media/platform/exynos4-is/fimc-capture.c b/drivers/media/platform/exynos4-is/fimc-capture.c
index 8a7cd07dbe28..948fe01f6c96 100644
--- a/drivers/media/platform/exynos4-is/fimc-capture.c
+++ b/drivers/media/platform/exynos4-is/fimc-capture.c
@@ -1270,13 +1270,14 @@ static int fimc_cap_g_selection(struct file *file, void *fh,
 	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
 	struct fimc_frame *f = &ctx->s_frame;
 
-	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
 	switch (s->target) {
 	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
 	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 		f = &ctx->d_frame;
+		/* fall through */
 	case V4L2_SEL_TGT_CROP_BOUNDS:
 	case V4L2_SEL_TGT_CROP_DEFAULT:
 		s->r.left = 0;
@@ -1287,6 +1288,7 @@ static int fimc_cap_g_selection(struct file *file, void *fh,
 
 	case V4L2_SEL_TGT_COMPOSE:
 		f = &ctx->d_frame;
+		/* fall through */
 	case V4L2_SEL_TGT_CROP:
 		s->r.left = f->offs_h;
 		s->r.top = f->offs_v;
@@ -1320,7 +1322,7 @@ static int fimc_cap_s_selection(struct file *file, void *fh,
 	struct fimc_frame *f;
 	unsigned long flags;
 
-	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
 	if (s->target == V4L2_SEL_TGT_COMPOSE)
@@ -1610,6 +1612,7 @@ static int fimc_subdev_get_selection(struct v4l2_subdev *sd,
 	switch (sel->target) {
 	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 		f = &ctx->d_frame;
+		/* fall through */
 	case V4L2_SEL_TGT_CROP_BOUNDS:
 		r->width = f->o_width;
 		r->height = f->o_height;
diff --git a/drivers/media/platform/exynos4-is/fimc-is.c b/drivers/media/platform/exynos4-is/fimc-is.c
index 7f92144a1de3..340d906db370 100644
--- a/drivers/media/platform/exynos4-is/fimc-is.c
+++ b/drivers/media/platform/exynos4-is/fimc-is.c
@@ -854,7 +854,7 @@ static int fimc_is_probe(struct platform_device *pdev)
 
 	vb2_dma_contig_set_max_seg_size(dev, DMA_BIT_MASK(32));
 
-	ret = of_platform_populate(dev->of_node, NULL, NULL, dev);
+	ret = devm_of_platform_populate(dev);
 	if (ret < 0)
 		goto err_pm;
 
@@ -864,7 +864,7 @@ static int fimc_is_probe(struct platform_device *pdev)
 	 */
 	ret = fimc_is_register_subdevs(is);
 	if (ret < 0)
-		goto err_of_dep;
+		goto err_pm;
 
 	ret = fimc_is_debugfs_create(is);
 	if (ret < 0)
@@ -883,8 +883,6 @@ err_dfs:
 	fimc_is_debugfs_remove(is);
 err_sd:
 	fimc_is_unregister_subdevs(is);
-err_of_dep:
-	of_platform_depopulate(dev);
 err_pm:
 	if (!pm_runtime_enabled(dev))
 		fimc_is_runtime_suspend(dev);
@@ -946,7 +944,6 @@ static int fimc_is_remove(struct platform_device *pdev)
 	if (!pm_runtime_status_suspended(dev))
 		fimc_is_runtime_suspend(dev);
 	free_irq(is->irq, is);
-	of_platform_depopulate(dev);
 	fimc_is_unregister_subdevs(is);
 	vb2_dma_contig_clear_max_seg_size(dev);
 	fimc_is_put_clocks(is);
diff --git a/drivers/media/platform/exynos4-is/fimc-lite.c b/drivers/media/platform/exynos4-is/fimc-lite.c
index b4c4a33784c4..7d3ec5cc6608 100644
--- a/drivers/media/platform/exynos4-is/fimc-lite.c
+++ b/drivers/media/platform/exynos4-is/fimc-lite.c
@@ -901,7 +901,7 @@ static int fimc_lite_g_selection(struct file *file, void *fh,
 	struct fimc_lite *fimc = video_drvdata(file);
 	struct flite_frame *f = &fimc->out_frame;
 
-	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
 	switch (sel->target) {
@@ -929,7 +929,7 @@ static int fimc_lite_s_selection(struct file *file, void *fh,
 	struct v4l2_rect rect = sel->r;
 	unsigned long flags;
 
-	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ||
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
 	    sel->target != V4L2_SEL_TGT_COMPOSE)
 		return -EINVAL;
 
diff --git a/drivers/media/platform/exynos4-is/media-dev.c b/drivers/media/platform/exynos4-is/media-dev.c
index e82450e90a67..7d1cf78846c4 100644
--- a/drivers/media/platform/exynos4-is/media-dev.c
+++ b/drivers/media/platform/exynos4-is/media-dev.c
@@ -29,7 +29,7 @@
 #include <linux/slab.h>
 #include <media/v4l2-async.h>
 #include <media/v4l2-ctrls.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 #include <media/media-device.h>
 #include <media/drv-intf/exynos-fimc.h>
 
@@ -388,7 +388,7 @@ static int fimc_md_parse_port_node(struct fimc_md *fmd,
 {
 	struct fimc_source_info *pd = &fmd->sensor[index].pdata;
 	struct device_node *rem, *ep, *np;
-	struct v4l2_of_endpoint endpoint;
+	struct v4l2_fwnode_endpoint endpoint;
 	int ret;
 
 	/* Assume here a port node can have only one endpoint node. */
@@ -396,7 +396,7 @@ static int fimc_md_parse_port_node(struct fimc_md *fmd,
 	if (!ep)
 		return 0;
 
-	ret = v4l2_of_parse_endpoint(ep, &endpoint);
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &endpoint);
 	if (ret) {
 		of_node_put(ep);
 		return ret;
@@ -453,8 +453,8 @@ static int fimc_md_parse_port_node(struct fimc_md *fmd,
 		return -EINVAL;
 	}
 
-	fmd->sensor[index].asd.match_type = V4L2_ASYNC_MATCH_OF;
-	fmd->sensor[index].asd.match.of.node = rem;
+	fmd->sensor[index].asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
+	fmd->sensor[index].asd.match.fwnode.fwnode = of_fwnode_handle(rem);
 	fmd->async_subdevs[index] = &fmd->sensor[index].asd;
 
 	fmd->num_sensors++;
@@ -1361,7 +1361,8 @@ static int subdev_notifier_bound(struct v4l2_async_notifier *notifier,
 
 	/* Find platform data for this sensor subdev */
 	for (i = 0; i < ARRAY_SIZE(fmd->sensor); i++)
-		if (fmd->sensor[i].asd.match.of.node == subdev->dev->of_node)
+		if (fmd->sensor[i].asd.match.fwnode.fwnode ==
+		    of_fwnode_handle(subdev->dev->of_node))
 			si = &fmd->sensor[i];
 
 	if (si == NULL)
diff --git a/drivers/media/platform/exynos4-is/mipi-csis.c b/drivers/media/platform/exynos4-is/mipi-csis.c
index f819b29efc38..98c89873c2dc 100644
--- a/drivers/media/platform/exynos4-is/mipi-csis.c
+++ b/drivers/media/platform/exynos4-is/mipi-csis.c
@@ -30,7 +30,7 @@
 #include <linux/spinlock.h>
 #include <linux/videodev2.h>
 #include <media/drv-intf/exynos-fimc.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 #include <media/v4l2-subdev.h>
 
 #include "mipi-csis.h"
@@ -718,7 +718,7 @@ static int s5pcsis_parse_dt(struct platform_device *pdev,
 			    struct csis_state *state)
 {
 	struct device_node *node = pdev->dev.of_node;
-	struct v4l2_of_endpoint endpoint;
+	struct v4l2_fwnode_endpoint endpoint;
 	int ret;
 
 	if (of_property_read_u32(node, "clock-frequency",
@@ -735,7 +735,7 @@ static int s5pcsis_parse_dt(struct platform_device *pdev,
 		return -EINVAL;
 	}
 	/* Get port node and validate MIPI-CSI channel id. */
-	ret = v4l2_of_parse_endpoint(node, &endpoint);
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &endpoint);
 	if (ret)
 		goto err;
 
diff --git a/drivers/media/platform/marvell-ccic/mcam-core.c b/drivers/media/platform/marvell-ccic/mcam-core.c
index a8bda6679422..8cac2f202099 100644
--- a/drivers/media/platform/marvell-ccic/mcam-core.c
+++ b/drivers/media/platform/marvell-ccic/mcam-core.c
@@ -393,6 +393,7 @@ static int mcam_alloc_dma_bufs(struct mcam_camera *cam, int loadtime)
 		dma_free_coherent(cam->dev, cam->dma_buf_size,
 				cam->dma_bufs[0], cam->dma_handles[0]);
 		cam->nbufs = 0;
+		/* fall-through */
 	case 0:
 		cam_err(cam, "Insufficient DMA buffers, cannot operate\n");
 		return -ENOMEM;
diff --git a/drivers/media/platform/mtk-mdp/mtk_mdp_core.c b/drivers/media/platform/mtk-mdp/mtk_mdp_core.c
index 9e4eb7dcc424..81347558b24a 100644
--- a/drivers/media/platform/mtk-mdp/mtk_mdp_core.c
+++ b/drivers/media/platform/mtk-mdp/mtk_mdp_core.c
@@ -103,7 +103,7 @@ static int mtk_mdp_probe(struct platform_device *pdev)
 {
 	struct mtk_mdp_dev *mdp;
 	struct device *dev = &pdev->dev;
-	struct device_node *node;
+	struct device_node *node, *parent;
 	int i, ret = 0;
 
 	mdp = devm_kzalloc(dev, sizeof(*mdp), GFP_KERNEL);
@@ -117,8 +117,16 @@ static int mtk_mdp_probe(struct platform_device *pdev)
 	mutex_init(&mdp->lock);
 	mutex_init(&mdp->vpulock);
 
+	/* Old dts had the components as child nodes */
+	if (of_get_next_child(dev->of_node, NULL)) {
+		parent = dev->of_node;
+		dev_warn(dev, "device tree is out of date\n");
+	} else {
+		parent = dev->of_node->parent;
+	}
+
 	/* Iterate over sibling MDP function blocks */
-	for_each_child_of_node(dev->of_node, node) {
+	for_each_child_of_node(parent, node) {
 		const struct of_device_id *of_id;
 		enum mtk_mdp_comp_type comp_type;
 		int comp_id;
diff --git a/drivers/media/platform/mtk-vcodec/mtk_vcodec_dec.c b/drivers/media/platform/mtk-vcodec/mtk_vcodec_dec.c
index a60b538686ea..843510979ad8 100644
--- a/drivers/media/platform/mtk-vcodec/mtk_vcodec_dec.c
+++ b/drivers/media/platform/mtk-vcodec/mtk_vcodec_dec.c
@@ -278,7 +278,7 @@ static void mtk_vdec_flush_decoder(struct mtk_vcodec_ctx *ctx)
 	clean_free_buffer(ctx);
 }
 
-static void mtk_vdec_pic_info_update(struct mtk_vcodec_ctx *ctx)
+static int mtk_vdec_pic_info_update(struct mtk_vcodec_ctx *ctx)
 {
 	unsigned int dpbsize = 0;
 	int ret;
@@ -288,7 +288,7 @@ static void mtk_vdec_pic_info_update(struct mtk_vcodec_ctx *ctx)
 				&ctx->last_decoded_picinfo)) {
 		mtk_v4l2_err("[%d]Error!! Cannot get param : GET_PARAM_PICTURE_INFO ERR",
 				ctx->id);
-		return;
+		return -EINVAL;
 	}
 
 	if (ctx->last_decoded_picinfo.pic_w == 0 ||
@@ -296,12 +296,12 @@ static void mtk_vdec_pic_info_update(struct mtk_vcodec_ctx *ctx)
 		ctx->last_decoded_picinfo.buf_w == 0 ||
 		ctx->last_decoded_picinfo.buf_h == 0) {
 		mtk_v4l2_err("Cannot get correct pic info");
-		return;
+		return -EINVAL;
 	}
 
 	if ((ctx->last_decoded_picinfo.pic_w == ctx->picinfo.pic_w) ||
 	    (ctx->last_decoded_picinfo.pic_h == ctx->picinfo.pic_h))
-		return;
+		return 0;
 
 	mtk_v4l2_debug(1,
 			"[%d]-> new(%d,%d), old(%d,%d), real(%d,%d)",
@@ -316,6 +316,8 @@ static void mtk_vdec_pic_info_update(struct mtk_vcodec_ctx *ctx)
 		mtk_v4l2_err("Incorrect dpb size, ret=%d", ret);
 
 	ctx->dpb_size = dpbsize;
+
+	return ret;
 }
 
 static void mtk_vdec_worker(struct work_struct *work)
diff --git a/drivers/media/platform/mtk-vcodec/mtk_vcodec_util.h b/drivers/media/platform/mtk-vcodec/mtk_vcodec_util.h
index 237e144c194f..06c254f5c171 100644
--- a/drivers/media/platform/mtk-vcodec/mtk_vcodec_util.h
+++ b/drivers/media/platform/mtk-vcodec/mtk_vcodec_util.h
@@ -32,6 +32,15 @@ extern int mtk_v4l2_dbg_level;
 extern bool mtk_vcodec_dbg;
 
 
+#define mtk_v4l2_err(fmt, args...)                \
+	pr_err("[MTK_V4L2][ERROR] %s:%d: " fmt "\n", __func__, __LINE__, \
+	       ##args)
+
+#define mtk_vcodec_err(h, fmt, args...)					\
+	pr_err("[MTK_VCODEC][ERROR][%d]: %s() " fmt "\n",		\
+	       ((struct mtk_vcodec_ctx *)h->ctx)->id, __func__, ##args)
+
+
 #if defined(DEBUG)
 
 #define mtk_v4l2_debug(level, fmt, args...)				 \
@@ -41,11 +50,6 @@ extern bool mtk_vcodec_dbg;
 				level, __func__, __LINE__, ##args);	 \
 	} while (0)
 
-#define mtk_v4l2_err(fmt, args...)                \
-	pr_err("[MTK_V4L2][ERROR] %s:%d: " fmt "\n", __func__, __LINE__, \
-	       ##args)
-
-
 #define mtk_v4l2_debug_enter()  mtk_v4l2_debug(3, "+")
 #define mtk_v4l2_debug_leave()  mtk_v4l2_debug(3, "-")
 
@@ -57,22 +61,16 @@ extern bool mtk_vcodec_dbg;
 				__func__, ##args);			\
 	} while (0)
 
-#define mtk_vcodec_err(h, fmt, args...)					\
-	pr_err("[MTK_VCODEC][ERROR][%d]: %s() " fmt "\n",		\
-	       ((struct mtk_vcodec_ctx *)h->ctx)->id, __func__, ##args)
-
 #define mtk_vcodec_debug_enter(h)  mtk_vcodec_debug(h, "+")
 #define mtk_vcodec_debug_leave(h)  mtk_vcodec_debug(h, "-")
 
 #else
 
 #define mtk_v4l2_debug(level, fmt, args...) {}
-#define mtk_v4l2_err(fmt, args...) {}
 #define mtk_v4l2_debug_enter() {}
 #define mtk_v4l2_debug_leave() {}
 
 #define mtk_vcodec_debug(h, fmt, args...) {}
-#define mtk_vcodec_err(h, fmt, args...) {}
 #define mtk_vcodec_debug_enter(h) {}
 #define mtk_vcodec_debug_leave(h) {}
 
diff --git a/drivers/media/platform/omap3isp/isp.c b/drivers/media/platform/omap3isp/isp.c
index 0d984a28a003..9df64c189883 100644
--- a/drivers/media/platform/omap3isp/isp.c
+++ b/drivers/media/platform/omap3isp/isp.c
@@ -55,6 +55,7 @@
 #include <linux/module.h>
 #include <linux/omap-iommu.h>
 #include <linux/platform_device.h>
+#include <linux/property.h>
 #include <linux/regulator/consumer.h>
 #include <linux/slab.h>
 #include <linux/sched.h>
@@ -63,9 +64,9 @@
 #include <asm/dma-iommu.h>
 
 #include <media/v4l2-common.h>
+#include <media/v4l2-fwnode.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-mc.h>
-#include <media/v4l2-of.h>
 
 #include "isp.h"
 #include "ispreg.h"
@@ -2007,20 +2008,20 @@ enum isp_of_phy {
 	ISP_OF_PHY_CSIPHY2,
 };
 
-static int isp_of_parse_node(struct device *dev, struct device_node *node,
-			     struct isp_async_subdev *isd)
+static int isp_fwnode_parse(struct device *dev, struct fwnode_handle *fwnode,
+			    struct isp_async_subdev *isd)
 {
 	struct isp_bus_cfg *buscfg = &isd->bus;
-	struct v4l2_of_endpoint vep;
+	struct v4l2_fwnode_endpoint vep;
 	unsigned int i;
 	int ret;
 
-	ret = v4l2_of_parse_endpoint(node, &vep);
+	ret = v4l2_fwnode_endpoint_parse(fwnode, &vep);
 	if (ret)
 		return ret;
 
-	dev_dbg(dev, "parsing endpoint %s, interface %u\n", node->full_name,
-		vep.base.port);
+	dev_dbg(dev, "parsing endpoint %s, interface %u\n",
+		to_of_node(fwnode)->full_name, vep.base.port);
 
 	switch (vep.base.port) {
 	case ISP_OF_PHY_PARALLEL:
@@ -2077,18 +2078,18 @@ static int isp_of_parse_node(struct device *dev, struct device_node *node,
 		break;
 
 	default:
-		dev_warn(dev, "%s: invalid interface %u\n", node->full_name,
-			 vep.base.port);
+		dev_warn(dev, "%s: invalid interface %u\n",
+			 to_of_node(fwnode)->full_name, vep.base.port);
 		break;
 	}
 
 	return 0;
 }
 
-static int isp_of_parse_nodes(struct device *dev,
-			      struct v4l2_async_notifier *notifier)
+static int isp_fwnodes_parse(struct device *dev,
+			     struct v4l2_async_notifier *notifier)
 {
-	struct device_node *node = NULL;
+	struct fwnode_handle *fwnode = NULL;
 
 	notifier->subdevs = devm_kcalloc(
 		dev, ISP_MAX_SUBDEVS, sizeof(*notifier->subdevs), GFP_KERNEL);
@@ -2096,7 +2097,8 @@ static int isp_of_parse_nodes(struct device *dev,
 		return -ENOMEM;
 
 	while (notifier->num_subdevs < ISP_MAX_SUBDEVS &&
-	       (node = of_graph_get_next_endpoint(dev->of_node, node))) {
+	       (fwnode = fwnode_graph_get_next_endpoint(
+			of_fwnode_handle(dev->of_node), fwnode))) {
 		struct isp_async_subdev *isd;
 
 		isd = devm_kzalloc(dev, sizeof(*isd), GFP_KERNEL);
@@ -2105,23 +2107,24 @@ static int isp_of_parse_nodes(struct device *dev,
 
 		notifier->subdevs[notifier->num_subdevs] = &isd->asd;
 
-		if (isp_of_parse_node(dev, node, isd))
+		if (isp_fwnode_parse(dev, fwnode, isd))
 			goto error;
 
-		isd->asd.match.of.node = of_graph_get_remote_port_parent(node);
-		if (!isd->asd.match.of.node) {
+		isd->asd.match.fwnode.fwnode =
+			fwnode_graph_get_remote_port_parent(fwnode);
+		if (!isd->asd.match.fwnode.fwnode) {
 			dev_warn(dev, "bad remote port parent\n");
 			goto error;
 		}
 
-		isd->asd.match_type = V4L2_ASYNC_MATCH_OF;
+		isd->asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
 		notifier->num_subdevs++;
 	}
 
 	return notifier->num_subdevs;
 
 error:
-	of_node_put(node);
+	fwnode_handle_put(fwnode);
 	return -EINVAL;
 }
 
@@ -2192,8 +2195,8 @@ static int isp_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	ret = of_property_read_u32(pdev->dev.of_node, "ti,phy-type",
-				   &isp->phy_type);
+	ret = fwnode_property_read_u32(of_fwnode_handle(pdev->dev.of_node),
+				       "ti,phy-type", &isp->phy_type);
 	if (ret)
 		return ret;
 
@@ -2202,12 +2205,12 @@ static int isp_probe(struct platform_device *pdev)
 	if (IS_ERR(isp->syscon))
 		return PTR_ERR(isp->syscon);
 
-	ret = of_property_read_u32_index(pdev->dev.of_node, "syscon", 1,
-					 &isp->syscon_offset);
+	ret = of_property_read_u32_index(pdev->dev.of_node,
+					 "syscon", 1, &isp->syscon_offset);
 	if (ret)
 		return ret;
 
-	ret = isp_of_parse_nodes(&pdev->dev, &isp->notifier);
+	ret = isp_fwnodes_parse(&pdev->dev, &isp->notifier);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/media/platform/pxa_camera.c b/drivers/media/platform/pxa_camera.c
index 929006f65cc7..399095170b6e 100644
--- a/drivers/media/platform/pxa_camera.c
+++ b/drivers/media/platform/pxa_camera.c
@@ -25,6 +25,7 @@
 #include <linux/mm.h>
 #include <linux/moduleparam.h>
 #include <linux/of.h>
+#include <linux/of_graph.h>
 #include <linux/time.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
@@ -37,9 +38,11 @@
 #include <media/v4l2-async.h>
 #include <media/v4l2-clk.h>
 #include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-ioctl.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 
 #include <media/videobuf2-dma-sg.h>
 
@@ -345,6 +348,36 @@ static const struct pxa_mbus_lookup mbus_fmt[] = {
 		.layout			= PXA_MBUS_LAYOUT_PACKED,
 	},
 }, {
+	.code = MEDIA_BUS_FMT_SGBRG8_1X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGBRG8,
+		.name			= "Bayer 8 GBRG",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_NONE,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SGRBG8_1X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGRBG8,
+		.name			= "Bayer 8 GRBG",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_NONE,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SRGGB8_1X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SRGGB8,
+		.name			= "Bayer 8 RGGB",
+		.bits_per_sample	= 8,
+		.packing		= PXA_MBUS_PACKING_NONE,
+		.order			= PXA_MBUS_ORDER_LE,
+		.layout			= PXA_MBUS_LAYOUT_PACKED,
+	},
+}, {
 	.code = MEDIA_BUS_FMT_SBGGR10_1X10,
 	.fmt = {
 		.fourcc			= V4L2_PIX_FMT_SBGGR10,
@@ -445,16 +478,6 @@ static const struct pxa_mbus_lookup mbus_fmt[] = {
 		.layout			= PXA_MBUS_LAYOUT_PACKED,
 	},
 }, {
-	.code = MEDIA_BUS_FMT_SGRBG8_1X8,
-	.fmt = {
-		.fourcc			= V4L2_PIX_FMT_SGRBG8,
-		.name			= "Bayer 8 GRBG",
-		.bits_per_sample	= 8,
-		.packing		= PXA_MBUS_PACKING_NONE,
-		.order			= PXA_MBUS_ORDER_LE,
-		.layout			= PXA_MBUS_LAYOUT_PACKED,
-	},
-}, {
 	.code = MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
 	.fmt = {
 		.fourcc			= V4L2_PIX_FMT_SGRBG10DPCM8,
@@ -555,6 +578,9 @@ static s32 pxa_mbus_bytes_per_line(u32 width, const struct pxa_mbus_pixelfmt *mf
 static s32 pxa_mbus_image_size(const struct pxa_mbus_pixelfmt *mf,
 			u32 bytes_per_line, u32 height)
 {
+	if (mf->layout == PXA_MBUS_LAYOUT_PACKED)
+		return bytes_per_line * height;
+
 	switch (mf->packing) {
 	case PXA_MBUS_PACKING_2X8_PADHI:
 		return bytes_per_line * height * 2;
@@ -1099,7 +1125,7 @@ static u32 mclk_get_divisor(struct platform_device *pdev,
 	/* mclk <= ciclk / 4 (27.4.2) */
 	if (mclk > lcdclk / 4) {
 		mclk = lcdclk / 4;
-		dev_warn(pcdev_to_dev(pcdev),
+		dev_warn(&pdev->dev,
 			 "Limiting master clock to %lu\n", mclk);
 	}
 
@@ -1110,7 +1136,7 @@ static u32 mclk_get_divisor(struct platform_device *pdev,
 	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
 		pcdev->mclk = lcdclk / (2 * (div + 1));
 
-	dev_dbg(pcdev_to_dev(pcdev), "LCD clock %luHz, target freq %luHz, divisor %u\n",
+	dev_dbg(&pdev->dev, "LCD clock %luHz, target freq %luHz, divisor %u\n",
 		lcdclk, mclk, div);
 
 	return div;
@@ -1291,6 +1317,7 @@ static void pxa_camera_setup_cicr(struct pxa_camera_dev *pcdev,
 		 * transformation. Note that UYVY is the only format that
 		 * should be used if pxa framebuffer Overlay2 is used.
 		 */
+		/* fall through */
 	case V4L2_PIX_FMT_UYVY:
 	case V4L2_PIX_FMT_VYUY:
 	case V4L2_PIX_FMT_YUYV:
@@ -2066,6 +2093,8 @@ static const struct v4l2_ioctl_ops pxa_camera_ioctl_ops = {
 	.vidioc_g_register		= pxac_vidioc_g_register,
 	.vidioc_s_register		= pxac_vidioc_s_register,
 #endif
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
 };
 
 static struct v4l2_clk_ops pxa_camera_mclk_ops = {
@@ -2177,6 +2206,12 @@ static void pxa_camera_sensor_unbind(struct v4l2_async_notifier *notifier,
 	pxa_dma_stop_channels(pcdev);
 
 	pxa_camera_destroy_formats(pcdev);
+
+	if (pcdev->mclk_clk) {
+		v4l2_clk_unregister(pcdev->mclk_clk);
+		pcdev->mclk_clk = NULL;
+	}
+
 	video_unregister_device(&pcdev->vdev);
 	pcdev->sensor = NULL;
 
@@ -2236,7 +2271,7 @@ static int pxa_camera_pdata_from_dt(struct device *dev,
 {
 	u32 mclk_rate;
 	struct device_node *remote, *np = dev->of_node;
-	struct v4l2_of_endpoint ep;
+	struct v4l2_fwnode_endpoint ep;
 	int err = of_property_read_u32(np, "clock-frequency",
 				       &mclk_rate);
 	if (!err) {
@@ -2250,7 +2285,7 @@ static int pxa_camera_pdata_from_dt(struct device *dev,
 		return -EINVAL;
 	}
 
-	err = v4l2_of_parse_endpoint(np, &ep);
+	err = v4l2_fwnode_endpoint_parse(of_fwnode_handle(np), &ep);
 	if (err) {
 		dev_err(dev, "could not parse endpoint\n");
 		goto out;
@@ -2287,10 +2322,10 @@ static int pxa_camera_pdata_from_dt(struct device *dev,
 	if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
 		pcdev->platform_flags |= PXA_CAMERA_PCLK_EN;
 
-	asd->match_type = V4L2_ASYNC_MATCH_OF;
+	asd->match_type = V4L2_ASYNC_MATCH_FWNODE;
 	remote = of_graph_get_remote_port(np);
 	if (remote) {
-		asd->match.of.node = remote;
+		asd->match.fwnode.fwnode = of_fwnode_handle(remote);
 		of_node_put(remote);
 	} else {
 		dev_notice(dev, "no remote for %s\n", of_node_full_name(np));
@@ -2501,7 +2536,13 @@ static int pxa_camera_remove(struct platform_device *pdev)
 	dma_release_channel(pcdev->dma_chans[1]);
 	dma_release_channel(pcdev->dma_chans[2]);
 
-	v4l2_clk_unregister(pcdev->mclk_clk);
+	v4l2_async_notifier_unregister(&pcdev->notifier);
+
+	if (pcdev->mclk_clk) {
+		v4l2_clk_unregister(pcdev->mclk_clk);
+		pcdev->mclk_clk = NULL;
+	}
+
 	v4l2_device_unregister(&pcdev->v4l2_dev);
 
 	dev_info(&pdev->dev, "PXA Camera driver unloaded\n");
diff --git a/drivers/media/platform/qcom/venus/Makefile b/drivers/media/platform/qcom/venus/Makefile
new file mode 100644
index 000000000000..0fe9afb83697
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/Makefile
@@ -0,0 +1,11 @@
+# Makefile for Qualcomm Venus driver
+
+venus-core-objs += core.o helpers.o firmware.o \
+		   hfi_venus.o hfi_msgs.o hfi_cmds.o hfi.o
+
+venus-dec-objs += vdec.o vdec_ctrls.o
+venus-enc-objs += venc.o venc_ctrls.o
+
+obj-$(CONFIG_VIDEO_QCOM_VENUS) += venus-core.o
+obj-$(CONFIG_VIDEO_QCOM_VENUS) += venus-dec.o
+obj-$(CONFIG_VIDEO_QCOM_VENUS) += venus-enc.o
diff --git a/drivers/media/platform/qcom/venus/core.c b/drivers/media/platform/qcom/venus/core.c
new file mode 100644
index 000000000000..776d2bae6979
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/core.c
@@ -0,0 +1,390 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/clk.h>
+#include <linux/init.h>
+#include <linux/ioctl.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/pm_runtime.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-ioctl.h>
+
+#include "core.h"
+#include "vdec.h"
+#include "venc.h"
+#include "firmware.h"
+
+static void venus_event_notify(struct venus_core *core, u32 event)
+{
+	struct venus_inst *inst;
+
+	switch (event) {
+	case EVT_SYS_WATCHDOG_TIMEOUT:
+	case EVT_SYS_ERROR:
+		break;
+	default:
+		return;
+	}
+
+	mutex_lock(&core->lock);
+	core->sys_error = true;
+	list_for_each_entry(inst, &core->instances, list)
+		inst->ops->event_notify(inst, EVT_SESSION_ERROR, NULL);
+	mutex_unlock(&core->lock);
+
+	disable_irq_nosync(core->irq);
+
+	/*
+	 * Delay recovery to ensure venus has completed any pending cache
+	 * operations. Without this sleep, we see device reset when firmware is
+	 * unloaded after a system error.
+	 */
+	schedule_delayed_work(&core->work, msecs_to_jiffies(100));
+}
+
+static const struct hfi_core_ops venus_core_ops = {
+	.event_notify = venus_event_notify,
+};
+
+static void venus_sys_error_handler(struct work_struct *work)
+{
+	struct venus_core *core =
+			container_of(work, struct venus_core, work.work);
+	int ret = 0;
+
+	dev_warn(core->dev, "system error has occurred, starting recovery!\n");
+
+	pm_runtime_get_sync(core->dev);
+
+	hfi_core_deinit(core, true);
+	hfi_destroy(core);
+	mutex_lock(&core->lock);
+	venus_shutdown(&core->dev_fw);
+
+	pm_runtime_put_sync(core->dev);
+
+	ret |= hfi_create(core, &venus_core_ops);
+
+	pm_runtime_get_sync(core->dev);
+
+	ret |= venus_boot(core->dev, &core->dev_fw, core->res->fwname);
+
+	ret |= hfi_core_resume(core, true);
+
+	enable_irq(core->irq);
+
+	mutex_unlock(&core->lock);
+
+	ret |= hfi_core_init(core);
+
+	pm_runtime_put_sync(core->dev);
+
+	if (ret) {
+		disable_irq_nosync(core->irq);
+		dev_warn(core->dev, "recovery failed (%d)\n", ret);
+		schedule_delayed_work(&core->work, msecs_to_jiffies(10));
+		return;
+	}
+
+	mutex_lock(&core->lock);
+	core->sys_error = false;
+	mutex_unlock(&core->lock);
+}
+
+static int venus_clks_get(struct venus_core *core)
+{
+	const struct venus_resources *res = core->res;
+	struct device *dev = core->dev;
+	unsigned int i;
+
+	for (i = 0; i < res->clks_num; i++) {
+		core->clks[i] = devm_clk_get(dev, res->clks[i]);
+		if (IS_ERR(core->clks[i]))
+			return PTR_ERR(core->clks[i]);
+	}
+
+	return 0;
+}
+
+static int venus_clks_enable(struct venus_core *core)
+{
+	const struct venus_resources *res = core->res;
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < res->clks_num; i++) {
+		ret = clk_prepare_enable(core->clks[i]);
+		if (ret)
+			goto err;
+	}
+
+	return 0;
+err:
+	while (--i)
+		clk_disable_unprepare(core->clks[i]);
+
+	return ret;
+}
+
+static void venus_clks_disable(struct venus_core *core)
+{
+	const struct venus_resources *res = core->res;
+	unsigned int i = res->clks_num;
+
+	while (i--)
+		clk_disable_unprepare(core->clks[i]);
+}
+
+static int venus_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct venus_core *core;
+	struct resource *r;
+	int ret;
+
+	core = devm_kzalloc(dev, sizeof(*core), GFP_KERNEL);
+	if (!core)
+		return -ENOMEM;
+
+	core->dev = dev;
+	platform_set_drvdata(pdev, core);
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	core->base = devm_ioremap_resource(dev, r);
+	if (IS_ERR(core->base))
+		return PTR_ERR(core->base);
+
+	core->irq = platform_get_irq(pdev, 0);
+	if (core->irq < 0)
+		return core->irq;
+
+	core->res = of_device_get_match_data(dev);
+	if (!core->res)
+		return -ENODEV;
+
+	ret = venus_clks_get(core);
+	if (ret)
+		return ret;
+
+	ret = dma_set_mask_and_coherent(dev, core->res->dma_mask);
+	if (ret)
+		return ret;
+
+	INIT_LIST_HEAD(&core->instances);
+	mutex_init(&core->lock);
+	INIT_DELAYED_WORK(&core->work, venus_sys_error_handler);
+
+	ret = devm_request_threaded_irq(dev, core->irq, hfi_isr, hfi_isr_thread,
+					IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
+					"venus", core);
+	if (ret)
+		return ret;
+
+	ret = hfi_create(core, &venus_core_ops);
+	if (ret)
+		return ret;
+
+	pm_runtime_enable(dev);
+
+	ret = pm_runtime_get_sync(dev);
+	if (ret < 0)
+		goto err_runtime_disable;
+
+	ret = venus_boot(dev, &core->dev_fw, core->res->fwname);
+	if (ret)
+		goto err_runtime_disable;
+
+	ret = hfi_core_resume(core, true);
+	if (ret)
+		goto err_venus_shutdown;
+
+	ret = hfi_core_init(core);
+	if (ret)
+		goto err_venus_shutdown;
+
+	ret = v4l2_device_register(dev, &core->v4l2_dev);
+	if (ret)
+		goto err_core_deinit;
+
+	ret = of_platform_populate(dev->of_node, NULL, NULL, dev);
+	if (ret)
+		goto err_dev_unregister;
+
+	ret = pm_runtime_put_sync(dev);
+	if (ret)
+		goto err_dev_unregister;
+
+	return 0;
+
+err_dev_unregister:
+	v4l2_device_unregister(&core->v4l2_dev);
+err_core_deinit:
+	hfi_core_deinit(core, false);
+err_venus_shutdown:
+	venus_shutdown(&core->dev_fw);
+err_runtime_disable:
+	pm_runtime_set_suspended(dev);
+	pm_runtime_disable(dev);
+	hfi_destroy(core);
+	return ret;
+}
+
+static int venus_remove(struct platform_device *pdev)
+{
+	struct venus_core *core = platform_get_drvdata(pdev);
+	struct device *dev = core->dev;
+	int ret;
+
+	ret = pm_runtime_get_sync(dev);
+	WARN_ON(ret < 0);
+
+	ret = hfi_core_deinit(core, true);
+	WARN_ON(ret);
+
+	hfi_destroy(core);
+	venus_shutdown(&core->dev_fw);
+	of_platform_depopulate(dev);
+
+	pm_runtime_put_sync(dev);
+	pm_runtime_disable(dev);
+
+	v4l2_device_unregister(&core->v4l2_dev);
+
+	return ret;
+}
+
+#ifdef CONFIG_PM
+static int venus_runtime_suspend(struct device *dev)
+{
+	struct venus_core *core = dev_get_drvdata(dev);
+	int ret;
+
+	ret = hfi_core_suspend(core);
+
+	venus_clks_disable(core);
+
+	return ret;
+}
+
+static int venus_runtime_resume(struct device *dev)
+{
+	struct venus_core *core = dev_get_drvdata(dev);
+	int ret;
+
+	ret = venus_clks_enable(core);
+	if (ret)
+		return ret;
+
+	ret = hfi_core_resume(core, false);
+	if (ret)
+		goto err_clks_disable;
+
+	return 0;
+
+err_clks_disable:
+	venus_clks_disable(core);
+	return ret;
+}
+#endif
+
+static const struct dev_pm_ops venus_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(venus_runtime_suspend, venus_runtime_resume, NULL)
+};
+
+static const struct freq_tbl msm8916_freq_table[] = {
+	{ 352800, 228570000 },	/* 1920x1088 @ 30 + 1280x720 @ 30 */
+	{ 244800, 160000000 },	/* 1920x1088 @ 30 */
+	{ 108000, 100000000 },	/* 1280x720 @ 30 */
+};
+
+static const struct reg_val msm8916_reg_preset[] = {
+	{ 0xe0020, 0x05555556 },
+	{ 0xe0024, 0x05555556 },
+	{ 0x80124, 0x00000003 },
+};
+
+static const struct venus_resources msm8916_res = {
+	.freq_tbl = msm8916_freq_table,
+	.freq_tbl_size = ARRAY_SIZE(msm8916_freq_table),
+	.reg_tbl = msm8916_reg_preset,
+	.reg_tbl_size = ARRAY_SIZE(msm8916_reg_preset),
+	.clks = { "core", "iface", "bus", },
+	.clks_num = 3,
+	.max_load = 352800, /* 720p@30 + 1080p@30 */
+	.hfi_version = HFI_VERSION_1XX,
+	.vmem_id = VIDC_RESOURCE_NONE,
+	.vmem_size = 0,
+	.vmem_addr = 0,
+	.dma_mask = 0xddc00000 - 1,
+	.fwname = "qcom/venus-1.8/venus.mdt",
+};
+
+static const struct freq_tbl msm8996_freq_table[] = {
+	{ 1944000, 490000000 },	/* 4k UHD @ 60 */
+	{  972000, 320000000 },	/* 4k UHD @ 30 */
+	{  489600, 150000000 },	/* 1080p @ 60 */
+	{  244800,  75000000 },	/* 1080p @ 30 */
+};
+
+static const struct reg_val msm8996_reg_preset[] = {
+	{ 0x80010, 0xffffffff },
+	{ 0x80018, 0x00001556 },
+	{ 0x8001C, 0x00001556 },
+};
+
+static const struct venus_resources msm8996_res = {
+	.freq_tbl = msm8996_freq_table,
+	.freq_tbl_size = ARRAY_SIZE(msm8996_freq_table),
+	.reg_tbl = msm8996_reg_preset,
+	.reg_tbl_size = ARRAY_SIZE(msm8996_reg_preset),
+	.clks = {"core", "iface", "bus", "mbus" },
+	.clks_num = 4,
+	.max_load = 2563200,
+	.hfi_version = HFI_VERSION_3XX,
+	.vmem_id = VIDC_RESOURCE_NONE,
+	.vmem_size = 0,
+	.vmem_addr = 0,
+	.dma_mask = 0xddc00000 - 1,
+	.fwname = "qcom/venus-4.2/venus.mdt",
+};
+
+static const struct of_device_id venus_dt_match[] = {
+	{ .compatible = "qcom,msm8916-venus", .data = &msm8916_res, },
+	{ .compatible = "qcom,msm8996-venus", .data = &msm8996_res, },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, venus_dt_match);
+
+static struct platform_driver qcom_venus_driver = {
+	.probe = venus_probe,
+	.remove = venus_remove,
+	.driver = {
+		.name = "qcom-venus",
+		.of_match_table = venus_dt_match,
+		.pm = &venus_pm_ops,
+	},
+};
+module_platform_driver(qcom_venus_driver);
+
+MODULE_ALIAS("platform:qcom-venus");
+MODULE_DESCRIPTION("Qualcomm Venus video encoder and decoder driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/qcom/venus/core.h b/drivers/media/platform/qcom/venus/core.h
new file mode 100644
index 000000000000..e542700eee32
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/core.h
@@ -0,0 +1,324 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef __VENUS_CORE_H_
+#define __VENUS_CORE_H_
+
+#include <linux/list.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+
+#include "hfi.h"
+
+#define VIDC_CLKS_NUM_MAX	4
+
+struct freq_tbl {
+	unsigned int load;
+	unsigned long freq;
+};
+
+struct reg_val {
+	u32 reg;
+	u32 value;
+};
+
+struct venus_resources {
+	u64 dma_mask;
+	const struct freq_tbl *freq_tbl;
+	unsigned int freq_tbl_size;
+	const struct reg_val *reg_tbl;
+	unsigned int reg_tbl_size;
+	const char * const clks[VIDC_CLKS_NUM_MAX];
+	unsigned int clks_num;
+	enum hfi_version hfi_version;
+	u32 max_load;
+	unsigned int vmem_id;
+	u32 vmem_size;
+	u32 vmem_addr;
+	const char *fwname;
+};
+
+struct venus_format {
+	u32 pixfmt;
+	unsigned int num_planes;
+	u32 type;
+};
+
+/**
+ * struct venus_core - holds core parameters valid for all instances
+ *
+ * @base:	IO memory base address
+ * @irq:		Venus irq
+ * @clks:	an array of struct clk pointers
+ * @core0_clk:	a struct clk pointer for core0
+ * @core1_clk:	a struct clk pointer for core1
+ * @vdev_dec:	a reference to video device structure for decoder instances
+ * @vdev_enc:	a reference to video device structure for encoder instances
+ * @v4l2_dev:	a holder for v4l2 device structure
+ * @res:		a reference to venus resources structure
+ * @dev:		convenience struct device pointer
+ * @dev_dec:	convenience struct device pointer for decoder device
+ * @dev_enc:	convenience struct device pointer for encoder device
+ * @lock:	a lock for this strucure
+ * @instances:	a list_head of all instances
+ * @insts_count:	num of instances
+ * @state:	the state of the venus core
+ * @done:	a completion for sync HFI operations
+ * @error:	an error returned during last HFI sync operations
+ * @sys_error:	an error flag that signal system error event
+ * @core_ops:	the core operations
+ * @enc_codecs:	encoders supported by this core
+ * @dec_codecs:	decoders supported by this core
+ * @max_sessions_supported:	holds the maximum number of sessions
+ * @core_caps:	core capabilities
+ * @priv:	a private filed for HFI operations
+ * @ops:		the core HFI operations
+ * @work:	a delayed work for handling system fatal error
+ */
+struct venus_core {
+	void __iomem *base;
+	int irq;
+	struct clk *clks[VIDC_CLKS_NUM_MAX];
+	struct clk *core0_clk;
+	struct clk *core1_clk;
+	struct video_device *vdev_dec;
+	struct video_device *vdev_enc;
+	struct v4l2_device v4l2_dev;
+	const struct venus_resources *res;
+	struct device *dev;
+	struct device *dev_dec;
+	struct device *dev_enc;
+	struct device dev_fw;
+	struct mutex lock;
+	struct list_head instances;
+	atomic_t insts_count;
+	unsigned int state;
+	struct completion done;
+	unsigned int error;
+	bool sys_error;
+	const struct hfi_core_ops *core_ops;
+	u32 enc_codecs;
+	u32 dec_codecs;
+	unsigned int max_sessions_supported;
+#define ENC_ROTATION_CAPABILITY		0x1
+#define ENC_SCALING_CAPABILITY		0x2
+#define ENC_DEINTERLACE_CAPABILITY	0x4
+#define DEC_MULTI_STREAM_CAPABILITY	0x8
+	unsigned int core_caps;
+	void *priv;
+	const struct hfi_ops *ops;
+	struct delayed_work work;
+};
+
+struct vdec_controls {
+	u32 post_loop_deb_mode;
+	u32 profile;
+	u32 level;
+};
+
+struct venc_controls {
+	u16 gop_size;
+	u32 num_p_frames;
+	u32 num_b_frames;
+	u32 bitrate_mode;
+	u32 bitrate;
+	u32 bitrate_peak;
+
+	u32 h264_i_period;
+	u32 h264_entropy_mode;
+	u32 h264_i_qp;
+	u32 h264_p_qp;
+	u32 h264_b_qp;
+	u32 h264_min_qp;
+	u32 h264_max_qp;
+	u32 h264_loop_filter_mode;
+	u32 h264_loop_filter_alpha;
+	u32 h264_loop_filter_beta;
+
+	u32 vp8_min_qp;
+	u32 vp8_max_qp;
+
+	u32 multi_slice_mode;
+	u32 multi_slice_max_bytes;
+	u32 multi_slice_max_mb;
+
+	u32 header_mode;
+
+	struct {
+		u32 mpeg4;
+		u32 h264;
+		u32 vpx;
+	} profile;
+	struct {
+		u32 mpeg4;
+		u32 h264;
+	} level;
+};
+
+struct venus_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+	dma_addr_t dma_addr;
+	u32 size;
+	struct list_head reg_list;
+	u32 flags;
+	struct list_head ref_list;
+};
+
+#define to_venus_buffer(ptr)	container_of(ptr, struct venus_buffer, vb)
+
+/**
+ * struct venus_inst - holds per instance paramerters
+ *
+ * @list:	used for attach an instance to the core
+ * @lock:	instance lock
+ * @core:	a reference to the core struct
+ * @internalbufs:	a list of internal bufferes
+ * @registeredbufs:	a list of registered capture bufferes
+ * @delayed_process	a list of delayed buffers
+ * @delayed_process_work:	a work_struct for process delayed buffers
+ * @ctrl_handler:	v4l control handler
+ * @controls:	a union of decoder and encoder control parameters
+ * @fh:	 a holder of v4l file handle structure
+ * @streamon_cap: stream on flag for capture queue
+ * @streamon_out: stream on flag for output queue
+ * @cmd_stop:	a flag to signal encoder/decoder commands
+ * @width:	current capture width
+ * @height:	current capture height
+ * @out_width:	current output width
+ * @out_height:	current output height
+ * @colorspace:	current color space
+ * @quantization:	current quantization
+ * @xfer_func:	current xfer function
+ * @fps:		holds current FPS
+ * @timeperframe:	holds current time per frame structure
+ * @fmt_out:	a reference to output format structure
+ * @fmt_cap:	a reference to capture format structure
+ * @num_input_bufs:	holds number of input buffers
+ * @num_output_bufs:	holds number of output buffers
+ * @input_buf_size	holds input buffer size
+ * @output_buf_size:	holds output buffer size
+ * @reconfig:	a flag raised by decoder when the stream resolution changed
+ * @reconfig_width:	holds the new width
+ * @reconfig_height:	holds the new height
+ * @sequence_cap:	a sequence counter for capture queue
+ * @sequence_out:	a sequence counter for output queue
+ * @m2m_dev:	a reference to m2m device structure
+ * @m2m_ctx:	a reference to m2m context structure
+ * @state:	current state of the instance
+ * @done:	a completion for sync HFI operation
+ * @error:	an error returned during last HFI sync operation
+ * @session_error:	a flag rised by HFI interface in case of session error
+ * @ops:		HFI operations
+ * @priv:	a private for HFI operations callbacks
+ * @session_type:	the type of the session (decoder or encoder)
+ * @hprop:	a union used as a holder by get property
+ * @cap_width:	width capability
+ * @cap_height:	height capability
+ * @cap_mbs_per_frame:	macroblocks per frame capability
+ * @cap_mbs_per_sec:	macroblocks per second capability
+ * @cap_framerate:	framerate capability
+ * @cap_scale_x:		horizontal scaling capability
+ * @cap_scale_y:		vertical scaling capability
+ * @cap_bitrate:		bitrate capability
+ * @cap_hier_p:		hier capability
+ * @cap_ltr_count:	LTR count capability
+ * @cap_secure_output2_threshold: secure OUTPUT2 threshold capability
+ * @cap_bufs_mode_static:	buffers allocation mode capability
+ * @cap_bufs_mode_dynamic:	buffers allocation mode capability
+ * @pl_count:	count of supported profiles/levels
+ * @pl:		supported profiles/levels
+ * @bufreq:	holds buffer requirements
+ */
+struct venus_inst {
+	struct list_head list;
+	struct mutex lock;
+	struct venus_core *core;
+	struct list_head internalbufs;
+	struct list_head registeredbufs;
+	struct list_head delayed_process;
+	struct work_struct delayed_process_work;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	union {
+		struct vdec_controls dec;
+		struct venc_controls enc;
+	} controls;
+	struct v4l2_fh fh;
+	unsigned int streamon_cap, streamon_out;
+	bool cmd_stop;
+	u32 width;
+	u32 height;
+	u32 out_width;
+	u32 out_height;
+	u32 colorspace;
+	u8 ycbcr_enc;
+	u8 quantization;
+	u8 xfer_func;
+	u64 fps;
+	struct v4l2_fract timeperframe;
+	const struct venus_format *fmt_out;
+	const struct venus_format *fmt_cap;
+	unsigned int num_input_bufs;
+	unsigned int num_output_bufs;
+	unsigned int input_buf_size;
+	unsigned int output_buf_size;
+	bool reconfig;
+	u32 reconfig_width;
+	u32 reconfig_height;
+	u32 sequence_cap;
+	u32 sequence_out;
+	struct v4l2_m2m_dev *m2m_dev;
+	struct v4l2_m2m_ctx *m2m_ctx;
+	unsigned int state;
+	struct completion done;
+	unsigned int error;
+	bool session_error;
+	const struct hfi_inst_ops *ops;
+	u32 session_type;
+	union hfi_get_property hprop;
+	struct hfi_capability cap_width;
+	struct hfi_capability cap_height;
+	struct hfi_capability cap_mbs_per_frame;
+	struct hfi_capability cap_mbs_per_sec;
+	struct hfi_capability cap_framerate;
+	struct hfi_capability cap_scale_x;
+	struct hfi_capability cap_scale_y;
+	struct hfi_capability cap_bitrate;
+	struct hfi_capability cap_hier_p;
+	struct hfi_capability cap_ltr_count;
+	struct hfi_capability cap_secure_output2_threshold;
+	bool cap_bufs_mode_static;
+	bool cap_bufs_mode_dynamic;
+	unsigned int pl_count;
+	struct hfi_profile_level pl[HFI_MAX_PROFILE_COUNT];
+	struct hfi_buffer_requirements bufreq[HFI_BUFFER_TYPE_MAX];
+};
+
+#define ctrl_to_inst(ctrl)	\
+	container_of((ctrl)->handler, struct venus_inst, ctrl_handler)
+
+static inline struct venus_inst *to_inst(struct file *filp)
+{
+	return container_of(filp->private_data, struct venus_inst, fh);
+}
+
+static inline void *to_hfi_priv(struct venus_core *core)
+{
+	return core->priv;
+}
+
+#endif
diff --git a/drivers/media/platform/qcom/venus/firmware.c b/drivers/media/platform/qcom/venus/firmware.c
new file mode 100644
index 000000000000..1b1a4f355918
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/firmware.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/firmware.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_reserved_mem.h>
+#include <linux/slab.h>
+#include <linux/qcom_scm.h>
+#include <linux/soc/qcom/mdt_loader.h>
+
+#include "firmware.h"
+
+#define VENUS_PAS_ID			9
+#define VENUS_FW_MEM_SIZE		SZ_8M
+
+static void device_release_dummy(struct device *dev)
+{
+	of_reserved_mem_device_release(dev);
+}
+
+int venus_boot(struct device *parent, struct device *fw_dev, const char *fwname)
+{
+	const struct firmware *mdt;
+	phys_addr_t mem_phys;
+	ssize_t fw_size;
+	size_t mem_size;
+	void *mem_va;
+	int ret;
+
+	if (!qcom_scm_is_available())
+		return -EPROBE_DEFER;
+
+	fw_dev->parent = parent;
+	fw_dev->release = device_release_dummy;
+
+	ret = dev_set_name(fw_dev, "%s:%s", dev_name(parent), "firmware");
+	if (ret)
+		return ret;
+
+	ret = device_register(fw_dev);
+	if (ret < 0)
+		return ret;
+
+	ret = of_reserved_mem_device_init_by_idx(fw_dev, parent->of_node, 0);
+	if (ret)
+		goto err_unreg_device;
+
+	mem_size = VENUS_FW_MEM_SIZE;
+
+	mem_va = dmam_alloc_coherent(fw_dev, mem_size, &mem_phys, GFP_KERNEL);
+	if (!mem_va) {
+		ret = -ENOMEM;
+		goto err_unreg_device;
+	}
+
+	ret = request_firmware(&mdt, fwname, fw_dev);
+	if (ret < 0)
+		goto err_unreg_device;
+
+	fw_size = qcom_mdt_get_size(mdt);
+	if (fw_size < 0) {
+		ret = fw_size;
+		release_firmware(mdt);
+		goto err_unreg_device;
+	}
+
+	ret = qcom_mdt_load(fw_dev, mdt, fwname, VENUS_PAS_ID, mem_va, mem_phys,
+			    mem_size);
+
+	release_firmware(mdt);
+
+	if (ret)
+		goto err_unreg_device;
+
+	ret = qcom_scm_pas_auth_and_reset(VENUS_PAS_ID);
+	if (ret)
+		goto err_unreg_device;
+
+	return 0;
+
+err_unreg_device:
+	device_unregister(fw_dev);
+	return ret;
+}
+
+int venus_shutdown(struct device *fw_dev)
+{
+	int ret;
+
+	ret = qcom_scm_pas_shutdown(VENUS_PAS_ID);
+	device_unregister(fw_dev);
+	memset(fw_dev, 0, sizeof(*fw_dev));
+
+	return ret;
+}
diff --git a/drivers/media/platform/qcom/venus/firmware.h b/drivers/media/platform/qcom/venus/firmware.h
new file mode 100644
index 000000000000..f81a98979798
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/firmware.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __VENUS_FIRMWARE_H__
+#define __VENUS_FIRMWARE_H__
+
+struct device;
+
+int venus_boot(struct device *parent, struct device *fw_dev,
+	       const char *fwname);
+int venus_shutdown(struct device *fw_dev);
+
+#endif
diff --git a/drivers/media/platform/qcom/venus/helpers.c b/drivers/media/platform/qcom/venus/helpers.c
new file mode 100644
index 000000000000..5f4434c0a8f1
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/helpers.c
@@ -0,0 +1,725 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/clk.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <media/videobuf2-dma-sg.h>
+#include <media/v4l2-mem2mem.h>
+#include <asm/div64.h>
+
+#include "core.h"
+#include "helpers.h"
+#include "hfi_helper.h"
+
+struct intbuf {
+	struct list_head list;
+	u32 type;
+	size_t size;
+	void *va;
+	dma_addr_t da;
+	unsigned long attrs;
+};
+
+static int intbufs_set_buffer(struct venus_inst *inst, u32 type)
+{
+	struct venus_core *core = inst->core;
+	struct device *dev = core->dev;
+	struct hfi_buffer_requirements bufreq;
+	struct hfi_buffer_desc bd;
+	struct intbuf *buf;
+	unsigned int i;
+	int ret;
+
+	ret = venus_helper_get_bufreq(inst, type, &bufreq);
+	if (ret)
+		return 0;
+
+	if (!bufreq.size)
+		return 0;
+
+	for (i = 0; i < bufreq.count_actual; i++) {
+		buf = kzalloc(sizeof(*buf), GFP_KERNEL);
+		if (!buf) {
+			ret = -ENOMEM;
+			goto fail;
+		}
+
+		buf->type = bufreq.type;
+		buf->size = bufreq.size;
+		buf->attrs = DMA_ATTR_WRITE_COMBINE |
+			     DMA_ATTR_NO_KERNEL_MAPPING;
+		buf->va = dma_alloc_attrs(dev, buf->size, &buf->da, GFP_KERNEL,
+					  buf->attrs);
+		if (!buf->va) {
+			ret = -ENOMEM;
+			goto fail;
+		}
+
+		memset(&bd, 0, sizeof(bd));
+		bd.buffer_size = buf->size;
+		bd.buffer_type = buf->type;
+		bd.num_buffers = 1;
+		bd.device_addr = buf->da;
+
+		ret = hfi_session_set_buffers(inst, &bd);
+		if (ret) {
+			dev_err(dev, "set session buffers failed\n");
+			goto dma_free;
+		}
+
+		list_add_tail(&buf->list, &inst->internalbufs);
+	}
+
+	return 0;
+
+dma_free:
+	dma_free_attrs(dev, buf->size, buf->va, buf->da, buf->attrs);
+fail:
+	kfree(buf);
+	return ret;
+}
+
+static int intbufs_unset_buffers(struct venus_inst *inst)
+{
+	struct hfi_buffer_desc bd = {0};
+	struct intbuf *buf, *n;
+	int ret = 0;
+
+	list_for_each_entry_safe(buf, n, &inst->internalbufs, list) {
+		bd.buffer_size = buf->size;
+		bd.buffer_type = buf->type;
+		bd.num_buffers = 1;
+		bd.device_addr = buf->da;
+		bd.response_required = true;
+
+		ret = hfi_session_unset_buffers(inst, &bd);
+
+		list_del_init(&buf->list);
+		dma_free_attrs(inst->core->dev, buf->size, buf->va, buf->da,
+			       buf->attrs);
+		kfree(buf);
+	}
+
+	return ret;
+}
+
+static const unsigned int intbuf_types[] = {
+	HFI_BUFFER_INTERNAL_SCRATCH,
+	HFI_BUFFER_INTERNAL_SCRATCH_1,
+	HFI_BUFFER_INTERNAL_SCRATCH_2,
+	HFI_BUFFER_INTERNAL_PERSIST,
+	HFI_BUFFER_INTERNAL_PERSIST_1,
+};
+
+static int intbufs_alloc(struct venus_inst *inst)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(intbuf_types); i++) {
+		ret = intbufs_set_buffer(inst, intbuf_types[i]);
+		if (ret)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	intbufs_unset_buffers(inst);
+	return ret;
+}
+
+static int intbufs_free(struct venus_inst *inst)
+{
+	return intbufs_unset_buffers(inst);
+}
+
+static u32 load_per_instance(struct venus_inst *inst)
+{
+	u32 mbs;
+
+	if (!inst || !(inst->state >= INST_INIT && inst->state < INST_STOP))
+		return 0;
+
+	mbs = (ALIGN(inst->width, 16) / 16) * (ALIGN(inst->height, 16) / 16);
+
+	return mbs * inst->fps;
+}
+
+static u32 load_per_type(struct venus_core *core, u32 session_type)
+{
+	struct venus_inst *inst = NULL;
+	u32 mbs_per_sec = 0;
+
+	mutex_lock(&core->lock);
+	list_for_each_entry(inst, &core->instances, list) {
+		if (inst->session_type != session_type)
+			continue;
+
+		mbs_per_sec += load_per_instance(inst);
+	}
+	mutex_unlock(&core->lock);
+
+	return mbs_per_sec;
+}
+
+static int load_scale_clocks(struct venus_core *core)
+{
+	const struct freq_tbl *table = core->res->freq_tbl;
+	unsigned int num_rows = core->res->freq_tbl_size;
+	unsigned long freq = table[0].freq;
+	struct clk *clk = core->clks[0];
+	struct device *dev = core->dev;
+	u32 mbs_per_sec;
+	unsigned int i;
+	int ret;
+
+	mbs_per_sec = load_per_type(core, VIDC_SESSION_TYPE_ENC) +
+		      load_per_type(core, VIDC_SESSION_TYPE_DEC);
+
+	if (mbs_per_sec > core->res->max_load)
+		dev_warn(dev, "HW is overloaded, needed: %d max: %d\n",
+			 mbs_per_sec, core->res->max_load);
+
+	if (!mbs_per_sec && num_rows > 1) {
+		freq = table[num_rows - 1].freq;
+		goto set_freq;
+	}
+
+	for (i = 0; i < num_rows; i++) {
+		if (mbs_per_sec > table[i].load)
+			break;
+		freq = table[i].freq;
+	}
+
+set_freq:
+
+	if (core->res->hfi_version == HFI_VERSION_3XX) {
+		ret = clk_set_rate(clk, freq);
+		ret |= clk_set_rate(core->core0_clk, freq);
+		ret |= clk_set_rate(core->core1_clk, freq);
+	} else {
+		ret = clk_set_rate(clk, freq);
+	}
+
+	if (ret) {
+		dev_err(dev, "failed to set clock rate %lu (%d)\n", freq, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void fill_buffer_desc(const struct venus_buffer *buf,
+			     struct hfi_buffer_desc *bd, bool response)
+{
+	memset(bd, 0, sizeof(*bd));
+	bd->buffer_type = HFI_BUFFER_OUTPUT;
+	bd->buffer_size = buf->size;
+	bd->num_buffers = 1;
+	bd->device_addr = buf->dma_addr;
+	bd->response_required = response;
+}
+
+static void return_buf_error(struct venus_inst *inst,
+			     struct vb2_v4l2_buffer *vbuf)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+
+	if (vbuf->vb2_buf.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		v4l2_m2m_src_buf_remove_by_buf(m2m_ctx, vbuf);
+	else
+		v4l2_m2m_src_buf_remove_by_buf(m2m_ctx, vbuf);
+
+	v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
+}
+
+static int
+session_process_buf(struct venus_inst *inst, struct vb2_v4l2_buffer *vbuf)
+{
+	struct venus_buffer *buf = to_venus_buffer(vbuf);
+	struct vb2_buffer *vb = &vbuf->vb2_buf;
+	unsigned int type = vb->type;
+	struct hfi_frame_data fdata;
+	int ret;
+
+	memset(&fdata, 0, sizeof(fdata));
+	fdata.alloc_len = buf->size;
+	fdata.device_addr = buf->dma_addr;
+	fdata.timestamp = vb->timestamp;
+	do_div(fdata.timestamp, NSEC_PER_USEC);
+	fdata.flags = 0;
+	fdata.clnt_data = vbuf->vb2_buf.index;
+
+	if (!fdata.timestamp)
+		fdata.flags |= HFI_BUFFERFLAG_TIMESTAMPINVALID;
+
+	if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		fdata.buffer_type = HFI_BUFFER_INPUT;
+		fdata.filled_len = vb2_get_plane_payload(vb, 0);
+		fdata.offset = vb->planes[0].data_offset;
+
+		if (vbuf->flags & V4L2_BUF_FLAG_LAST || !fdata.filled_len)
+			fdata.flags |= HFI_BUFFERFLAG_EOS;
+	} else if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		fdata.buffer_type = HFI_BUFFER_OUTPUT;
+		fdata.filled_len = 0;
+		fdata.offset = 0;
+	}
+
+	ret = hfi_session_process_buf(inst, &fdata);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static inline int is_reg_unreg_needed(struct venus_inst *inst)
+{
+	if (inst->session_type == VIDC_SESSION_TYPE_DEC &&
+	    inst->core->res->hfi_version == HFI_VERSION_3XX)
+		return 0;
+
+	if (inst->session_type == VIDC_SESSION_TYPE_DEC &&
+	    inst->cap_bufs_mode_dynamic &&
+	    inst->core->res->hfi_version == HFI_VERSION_1XX)
+		return 0;
+
+	return 1;
+}
+
+static int session_unregister_bufs(struct venus_inst *inst)
+{
+	struct venus_buffer *buf, *n;
+	struct hfi_buffer_desc bd;
+	int ret = 0;
+
+	if (!is_reg_unreg_needed(inst))
+		return 0;
+
+	list_for_each_entry_safe(buf, n, &inst->registeredbufs, reg_list) {
+		fill_buffer_desc(buf, &bd, true);
+		ret = hfi_session_unset_buffers(inst, &bd);
+		list_del_init(&buf->reg_list);
+	}
+
+	return ret;
+}
+
+static int session_register_bufs(struct venus_inst *inst)
+{
+	struct venus_core *core = inst->core;
+	struct device *dev = core->dev;
+	struct hfi_buffer_desc bd;
+	struct venus_buffer *buf;
+	int ret = 0;
+
+	if (!is_reg_unreg_needed(inst))
+		return 0;
+
+	list_for_each_entry(buf, &inst->registeredbufs, reg_list) {
+		fill_buffer_desc(buf, &bd, false);
+		ret = hfi_session_set_buffers(inst, &bd);
+		if (ret) {
+			dev_err(dev, "%s: set buffer failed\n", __func__);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+int venus_helper_get_bufreq(struct venus_inst *inst, u32 type,
+			    struct hfi_buffer_requirements *req)
+{
+	u32 ptype = HFI_PROPERTY_CONFIG_BUFFER_REQUIREMENTS;
+	union hfi_get_property hprop;
+	unsigned int i;
+	int ret;
+
+	if (req)
+		memset(req, 0, sizeof(*req));
+
+	ret = hfi_session_get_property(inst, ptype, &hprop);
+	if (ret)
+		return ret;
+
+	ret = -EINVAL;
+
+	for (i = 0; i < HFI_BUFFER_TYPE_MAX; i++) {
+		if (hprop.bufreq[i].type != type)
+			continue;
+
+		if (req)
+			memcpy(req, &hprop.bufreq[i], sizeof(*req));
+		ret = 0;
+		break;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(venus_helper_get_bufreq);
+
+int venus_helper_set_input_resolution(struct venus_inst *inst,
+				      unsigned int width, unsigned int height)
+{
+	u32 ptype = HFI_PROPERTY_PARAM_FRAME_SIZE;
+	struct hfi_framesize fs;
+
+	fs.buffer_type = HFI_BUFFER_INPUT;
+	fs.width = width;
+	fs.height = height;
+
+	return hfi_session_set_property(inst, ptype, &fs);
+}
+EXPORT_SYMBOL_GPL(venus_helper_set_input_resolution);
+
+int venus_helper_set_output_resolution(struct venus_inst *inst,
+				       unsigned int width, unsigned int height)
+{
+	u32 ptype = HFI_PROPERTY_PARAM_FRAME_SIZE;
+	struct hfi_framesize fs;
+
+	fs.buffer_type = HFI_BUFFER_OUTPUT;
+	fs.width = width;
+	fs.height = height;
+
+	return hfi_session_set_property(inst, ptype, &fs);
+}
+EXPORT_SYMBOL_GPL(venus_helper_set_output_resolution);
+
+int venus_helper_set_num_bufs(struct venus_inst *inst, unsigned int input_bufs,
+			      unsigned int output_bufs)
+{
+	u32 ptype = HFI_PROPERTY_PARAM_BUFFER_COUNT_ACTUAL;
+	struct hfi_buffer_count_actual buf_count;
+	int ret;
+
+	buf_count.type = HFI_BUFFER_INPUT;
+	buf_count.count_actual = input_bufs;
+
+	ret = hfi_session_set_property(inst, ptype, &buf_count);
+	if (ret)
+		return ret;
+
+	buf_count.type = HFI_BUFFER_OUTPUT;
+	buf_count.count_actual = output_bufs;
+
+	return hfi_session_set_property(inst, ptype, &buf_count);
+}
+EXPORT_SYMBOL_GPL(venus_helper_set_num_bufs);
+
+int venus_helper_set_color_format(struct venus_inst *inst, u32 pixfmt)
+{
+	struct hfi_uncompressed_format_select fmt;
+	u32 ptype = HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SELECT;
+	int ret;
+
+	if (inst->session_type == VIDC_SESSION_TYPE_DEC)
+		fmt.buffer_type = HFI_BUFFER_OUTPUT;
+	else if (inst->session_type == VIDC_SESSION_TYPE_ENC)
+		fmt.buffer_type = HFI_BUFFER_INPUT;
+	else
+		return -EINVAL;
+
+	switch (pixfmt) {
+	case V4L2_PIX_FMT_NV12:
+		fmt.format = HFI_COLOR_FORMAT_NV12;
+		break;
+	case V4L2_PIX_FMT_NV21:
+		fmt.format = HFI_COLOR_FORMAT_NV21;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = hfi_session_set_property(inst, ptype, &fmt);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(venus_helper_set_color_format);
+
+static void delayed_process_buf_func(struct work_struct *work)
+{
+	struct venus_buffer *buf, *n;
+	struct venus_inst *inst;
+	int ret;
+
+	inst = container_of(work, struct venus_inst, delayed_process_work);
+
+	mutex_lock(&inst->lock);
+
+	if (!(inst->streamon_out & inst->streamon_cap))
+		goto unlock;
+
+	list_for_each_entry_safe(buf, n, &inst->delayed_process, ref_list) {
+		if (buf->flags & HFI_BUFFERFLAG_READONLY)
+			continue;
+
+		ret = session_process_buf(inst, &buf->vb);
+		if (ret)
+			return_buf_error(inst, &buf->vb);
+
+		list_del_init(&buf->ref_list);
+	}
+unlock:
+	mutex_unlock(&inst->lock);
+}
+
+void venus_helper_release_buf_ref(struct venus_inst *inst, unsigned int idx)
+{
+	struct venus_buffer *buf;
+
+	list_for_each_entry(buf, &inst->registeredbufs, reg_list) {
+		if (buf->vb.vb2_buf.index == idx) {
+			buf->flags &= ~HFI_BUFFERFLAG_READONLY;
+			schedule_work(&inst->delayed_process_work);
+			break;
+		}
+	}
+}
+EXPORT_SYMBOL_GPL(venus_helper_release_buf_ref);
+
+void venus_helper_acquire_buf_ref(struct vb2_v4l2_buffer *vbuf)
+{
+	struct venus_buffer *buf = to_venus_buffer(vbuf);
+
+	buf->flags |= HFI_BUFFERFLAG_READONLY;
+}
+EXPORT_SYMBOL_GPL(venus_helper_acquire_buf_ref);
+
+static int is_buf_refed(struct venus_inst *inst, struct vb2_v4l2_buffer *vbuf)
+{
+	struct venus_buffer *buf = to_venus_buffer(vbuf);
+
+	if (buf->flags & HFI_BUFFERFLAG_READONLY) {
+		list_add_tail(&buf->ref_list, &inst->delayed_process);
+		schedule_work(&inst->delayed_process_work);
+		return 1;
+	}
+
+	return 0;
+}
+
+struct vb2_v4l2_buffer *
+venus_helper_find_buf(struct venus_inst *inst, unsigned int type, u32 idx)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+
+	if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return v4l2_m2m_src_buf_remove_by_idx(m2m_ctx, idx);
+	else
+		return v4l2_m2m_dst_buf_remove_by_idx(m2m_ctx, idx);
+}
+EXPORT_SYMBOL_GPL(venus_helper_find_buf);
+
+int venus_helper_vb2_buf_init(struct vb2_buffer *vb)
+{
+	struct venus_inst *inst = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct venus_buffer *buf = to_venus_buffer(vbuf);
+	struct sg_table *sgt;
+
+	sgt = vb2_dma_sg_plane_desc(vb, 0);
+	if (!sgt)
+		return -EFAULT;
+
+	buf->size = vb2_plane_size(vb, 0);
+	buf->dma_addr = sg_dma_address(sgt->sgl);
+
+	if (vb->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		list_add_tail(&buf->reg_list, &inst->registeredbufs);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(venus_helper_vb2_buf_init);
+
+int venus_helper_vb2_buf_prepare(struct vb2_buffer *vb)
+{
+	struct venus_inst *inst = vb2_get_drv_priv(vb->vb2_queue);
+
+	if (vb->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+	    vb2_plane_size(vb, 0) < inst->output_buf_size)
+		return -EINVAL;
+	if (vb->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
+	    vb2_plane_size(vb, 0) < inst->input_buf_size)
+		return -EINVAL;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(venus_helper_vb2_buf_prepare);
+
+void venus_helper_vb2_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct venus_inst *inst = vb2_get_drv_priv(vb->vb2_queue);
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	int ret;
+
+	mutex_lock(&inst->lock);
+
+	if (inst->cmd_stop) {
+		vbuf->flags |= V4L2_BUF_FLAG_LAST;
+		v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_DONE);
+		inst->cmd_stop = false;
+		goto unlock;
+	}
+
+	v4l2_m2m_buf_queue(m2m_ctx, vbuf);
+
+	if (!(inst->streamon_out & inst->streamon_cap))
+		goto unlock;
+
+	ret = is_buf_refed(inst, vbuf);
+	if (ret)
+		goto unlock;
+
+	ret = session_process_buf(inst, vbuf);
+	if (ret)
+		return_buf_error(inst, vbuf);
+
+unlock:
+	mutex_unlock(&inst->lock);
+}
+EXPORT_SYMBOL_GPL(venus_helper_vb2_buf_queue);
+
+void venus_helper_buffers_done(struct venus_inst *inst,
+			       enum vb2_buffer_state state)
+{
+	struct vb2_v4l2_buffer *buf;
+
+	while ((buf = v4l2_m2m_src_buf_remove(inst->m2m_ctx)))
+		v4l2_m2m_buf_done(buf, state);
+	while ((buf = v4l2_m2m_dst_buf_remove(inst->m2m_ctx)))
+		v4l2_m2m_buf_done(buf, state);
+}
+EXPORT_SYMBOL_GPL(venus_helper_buffers_done);
+
+void venus_helper_vb2_stop_streaming(struct vb2_queue *q)
+{
+	struct venus_inst *inst = vb2_get_drv_priv(q);
+	struct venus_core *core = inst->core;
+	int ret;
+
+	mutex_lock(&inst->lock);
+
+	if (inst->streamon_out & inst->streamon_cap) {
+		ret = hfi_session_stop(inst);
+		ret |= hfi_session_unload_res(inst);
+		ret |= session_unregister_bufs(inst);
+		ret |= intbufs_free(inst);
+		ret |= hfi_session_deinit(inst);
+
+		if (inst->session_error || core->sys_error)
+			ret = -EIO;
+
+		if (ret)
+			hfi_session_abort(inst);
+
+		load_scale_clocks(core);
+	}
+
+	venus_helper_buffers_done(inst, VB2_BUF_STATE_ERROR);
+
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		inst->streamon_out = 0;
+	else
+		inst->streamon_cap = 0;
+
+	mutex_unlock(&inst->lock);
+}
+EXPORT_SYMBOL_GPL(venus_helper_vb2_stop_streaming);
+
+int venus_helper_vb2_start_streaming(struct venus_inst *inst)
+{
+	struct venus_core *core = inst->core;
+	int ret;
+
+	ret = intbufs_alloc(inst);
+	if (ret)
+		return ret;
+
+	ret = session_register_bufs(inst);
+	if (ret)
+		goto err_bufs_free;
+
+	load_scale_clocks(core);
+
+	ret = hfi_session_load_res(inst);
+	if (ret)
+		goto err_unreg_bufs;
+
+	ret = hfi_session_start(inst);
+	if (ret)
+		goto err_unload_res;
+
+	return 0;
+
+err_unload_res:
+	hfi_session_unload_res(inst);
+err_unreg_bufs:
+	session_unregister_bufs(inst);
+err_bufs_free:
+	intbufs_free(inst);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(venus_helper_vb2_start_streaming);
+
+void venus_helper_m2m_device_run(void *priv)
+{
+	struct venus_inst *inst = priv;
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct v4l2_m2m_buffer *buf, *n;
+	int ret;
+
+	mutex_lock(&inst->lock);
+
+	v4l2_m2m_for_each_dst_buf_safe(m2m_ctx, buf, n) {
+		ret = session_process_buf(inst, &buf->vb);
+		if (ret)
+			return_buf_error(inst, &buf->vb);
+	}
+
+	v4l2_m2m_for_each_src_buf_safe(m2m_ctx, buf, n) {
+		ret = session_process_buf(inst, &buf->vb);
+		if (ret)
+			return_buf_error(inst, &buf->vb);
+	}
+
+	mutex_unlock(&inst->lock);
+}
+EXPORT_SYMBOL_GPL(venus_helper_m2m_device_run);
+
+void venus_helper_m2m_job_abort(void *priv)
+{
+	struct venus_inst *inst = priv;
+
+	v4l2_m2m_job_finish(inst->m2m_dev, inst->m2m_ctx);
+}
+EXPORT_SYMBOL_GPL(venus_helper_m2m_job_abort);
+
+void venus_helper_init_instance(struct venus_inst *inst)
+{
+	if (inst->session_type == VIDC_SESSION_TYPE_DEC) {
+		INIT_LIST_HEAD(&inst->delayed_process);
+		INIT_WORK(&inst->delayed_process_work,
+			  delayed_process_buf_func);
+	}
+}
+EXPORT_SYMBOL_GPL(venus_helper_init_instance);
diff --git a/drivers/media/platform/qcom/venus/helpers.h b/drivers/media/platform/qcom/venus/helpers.h
new file mode 100644
index 000000000000..6a061b417a93
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/helpers.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __VENUS_HELPERS_H__
+#define __VENUS_HELPERS_H__
+
+#include <media/videobuf2-v4l2.h>
+
+struct venus_inst;
+
+struct vb2_v4l2_buffer *venus_helper_find_buf(struct venus_inst *inst,
+					      unsigned int type, u32 idx);
+void venus_helper_buffers_done(struct venus_inst *inst,
+			       enum vb2_buffer_state state);
+int venus_helper_vb2_buf_init(struct vb2_buffer *vb);
+int venus_helper_vb2_buf_prepare(struct vb2_buffer *vb);
+void venus_helper_vb2_buf_queue(struct vb2_buffer *vb);
+void venus_helper_vb2_stop_streaming(struct vb2_queue *q);
+int venus_helper_vb2_start_streaming(struct venus_inst *inst);
+void venus_helper_m2m_device_run(void *priv);
+void venus_helper_m2m_job_abort(void *priv);
+int venus_helper_get_bufreq(struct venus_inst *inst, u32 type,
+			    struct hfi_buffer_requirements *req);
+int venus_helper_set_input_resolution(struct venus_inst *inst,
+				      unsigned int width, unsigned int height);
+int venus_helper_set_output_resolution(struct venus_inst *inst,
+				       unsigned int width, unsigned int height);
+int venus_helper_set_num_bufs(struct venus_inst *inst, unsigned int input_bufs,
+			      unsigned int output_bufs);
+int venus_helper_set_color_format(struct venus_inst *inst, u32 fmt);
+void venus_helper_acquire_buf_ref(struct vb2_v4l2_buffer *vbuf);
+void venus_helper_release_buf_ref(struct venus_inst *inst, unsigned int idx);
+void venus_helper_init_instance(struct venus_inst *inst);
+#endif
diff --git a/drivers/media/platform/qcom/venus/hfi.c b/drivers/media/platform/qcom/venus/hfi.c
new file mode 100644
index 000000000000..c09490876516
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/hfi.c
@@ -0,0 +1,522 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/list.h>
+#include <linux/completion.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+
+#include "core.h"
+#include "hfi.h"
+#include "hfi_cmds.h"
+#include "hfi_venus.h"
+
+#define TIMEOUT		msecs_to_jiffies(1000)
+
+static u32 to_codec_type(u32 pixfmt)
+{
+	switch (pixfmt) {
+	case V4L2_PIX_FMT_H264:
+	case V4L2_PIX_FMT_H264_NO_SC:
+		return HFI_VIDEO_CODEC_H264;
+	case V4L2_PIX_FMT_H263:
+		return HFI_VIDEO_CODEC_H263;
+	case V4L2_PIX_FMT_MPEG1:
+		return HFI_VIDEO_CODEC_MPEG1;
+	case V4L2_PIX_FMT_MPEG2:
+		return HFI_VIDEO_CODEC_MPEG2;
+	case V4L2_PIX_FMT_MPEG4:
+		return HFI_VIDEO_CODEC_MPEG4;
+	case V4L2_PIX_FMT_VC1_ANNEX_G:
+	case V4L2_PIX_FMT_VC1_ANNEX_L:
+		return HFI_VIDEO_CODEC_VC1;
+	case V4L2_PIX_FMT_VP8:
+		return HFI_VIDEO_CODEC_VP8;
+	case V4L2_PIX_FMT_VP9:
+		return HFI_VIDEO_CODEC_VP9;
+	case V4L2_PIX_FMT_XVID:
+		return HFI_VIDEO_CODEC_DIVX;
+	default:
+		return 0;
+	}
+}
+
+int hfi_core_init(struct venus_core *core)
+{
+	int ret = 0;
+
+	mutex_lock(&core->lock);
+
+	if (core->state >= CORE_INIT)
+		goto unlock;
+
+	reinit_completion(&core->done);
+
+	ret = core->ops->core_init(core);
+	if (ret)
+		goto unlock;
+
+	ret = wait_for_completion_timeout(&core->done, TIMEOUT);
+	if (!ret) {
+		ret = -ETIMEDOUT;
+		goto unlock;
+	}
+
+	ret = 0;
+
+	if (core->error != HFI_ERR_NONE) {
+		ret = -EIO;
+		goto unlock;
+	}
+
+	core->state = CORE_INIT;
+unlock:
+	mutex_unlock(&core->lock);
+	return ret;
+}
+
+static int core_deinit_wait_atomic_t(atomic_t *p)
+{
+	schedule();
+	return 0;
+}
+
+int hfi_core_deinit(struct venus_core *core, bool blocking)
+{
+	int ret = 0, empty;
+
+	mutex_lock(&core->lock);
+
+	if (core->state == CORE_UNINIT)
+		goto unlock;
+
+	empty = list_empty(&core->instances);
+
+	if (!empty && !blocking) {
+		ret = -EBUSY;
+		goto unlock;
+	}
+
+	if (!empty) {
+		mutex_unlock(&core->lock);
+		wait_on_atomic_t(&core->insts_count, core_deinit_wait_atomic_t,
+				 TASK_UNINTERRUPTIBLE);
+		mutex_lock(&core->lock);
+	}
+
+	ret = core->ops->core_deinit(core);
+
+	if (!ret)
+		core->state = CORE_UNINIT;
+
+unlock:
+	mutex_unlock(&core->lock);
+	return ret;
+}
+
+int hfi_core_suspend(struct venus_core *core)
+{
+	if (core->state != CORE_INIT)
+		return 0;
+
+	return core->ops->suspend(core);
+}
+
+int hfi_core_resume(struct venus_core *core, bool force)
+{
+	if (!force && core->state != CORE_INIT)
+		return 0;
+
+	return core->ops->resume(core);
+}
+
+int hfi_core_trigger_ssr(struct venus_core *core, u32 type)
+{
+	return core->ops->core_trigger_ssr(core, type);
+}
+
+int hfi_core_ping(struct venus_core *core)
+{
+	int ret;
+
+	mutex_lock(&core->lock);
+
+	ret = core->ops->core_ping(core, 0xbeef);
+	if (ret)
+		goto unlock;
+
+	ret = wait_for_completion_timeout(&core->done, TIMEOUT);
+	if (!ret) {
+		ret = -ETIMEDOUT;
+		goto unlock;
+	}
+	ret = 0;
+	if (core->error != HFI_ERR_NONE)
+		ret = -ENODEV;
+unlock:
+	mutex_unlock(&core->lock);
+	return ret;
+}
+
+static int wait_session_msg(struct venus_inst *inst)
+{
+	int ret;
+
+	ret = wait_for_completion_timeout(&inst->done, TIMEOUT);
+	if (!ret)
+		return -ETIMEDOUT;
+
+	if (inst->error != HFI_ERR_NONE)
+		return -EIO;
+
+	return 0;
+}
+
+int hfi_session_create(struct venus_inst *inst, const struct hfi_inst_ops *ops)
+{
+	struct venus_core *core = inst->core;
+
+	if (!ops)
+		return -EINVAL;
+
+	inst->state = INST_UNINIT;
+	init_completion(&inst->done);
+	inst->ops = ops;
+
+	mutex_lock(&core->lock);
+	list_add_tail(&inst->list, &core->instances);
+	atomic_inc(&core->insts_count);
+	mutex_unlock(&core->lock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(hfi_session_create);
+
+int hfi_session_init(struct venus_inst *inst, u32 pixfmt)
+{
+	struct venus_core *core = inst->core;
+	const struct hfi_ops *ops = core->ops;
+	u32 codec;
+	int ret;
+
+	codec = to_codec_type(pixfmt);
+	reinit_completion(&inst->done);
+
+	ret = ops->session_init(inst, inst->session_type, codec);
+	if (ret)
+		return ret;
+
+	ret = wait_session_msg(inst);
+	if (ret)
+		return ret;
+
+	inst->state = INST_INIT;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(hfi_session_init);
+
+void hfi_session_destroy(struct venus_inst *inst)
+{
+	struct venus_core *core = inst->core;
+
+	mutex_lock(&core->lock);
+	list_del_init(&inst->list);
+	atomic_dec(&core->insts_count);
+	wake_up_atomic_t(&core->insts_count);
+	mutex_unlock(&core->lock);
+}
+EXPORT_SYMBOL_GPL(hfi_session_destroy);
+
+int hfi_session_deinit(struct venus_inst *inst)
+{
+	const struct hfi_ops *ops = inst->core->ops;
+	int ret;
+
+	if (inst->state == INST_UNINIT)
+		return 0;
+
+	if (inst->state < INST_INIT)
+		return -EINVAL;
+
+	reinit_completion(&inst->done);
+
+	ret = ops->session_end(inst);
+	if (ret)
+		return ret;
+
+	ret = wait_session_msg(inst);
+	if (ret)
+		return ret;
+
+	inst->state = INST_UNINIT;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(hfi_session_deinit);
+
+int hfi_session_start(struct venus_inst *inst)
+{
+	const struct hfi_ops *ops = inst->core->ops;
+	int ret;
+
+	if (inst->state != INST_LOAD_RESOURCES)
+		return -EINVAL;
+
+	reinit_completion(&inst->done);
+
+	ret = ops->session_start(inst);
+	if (ret)
+		return ret;
+
+	ret = wait_session_msg(inst);
+	if (ret)
+		return ret;
+
+	inst->state = INST_START;
+
+	return 0;
+}
+
+int hfi_session_stop(struct venus_inst *inst)
+{
+	const struct hfi_ops *ops = inst->core->ops;
+	int ret;
+
+	if (inst->state != INST_START)
+		return -EINVAL;
+
+	reinit_completion(&inst->done);
+
+	ret = ops->session_stop(inst);
+	if (ret)
+		return ret;
+
+	ret = wait_session_msg(inst);
+	if (ret)
+		return ret;
+
+	inst->state = INST_STOP;
+
+	return 0;
+}
+
+int hfi_session_continue(struct venus_inst *inst)
+{
+	struct venus_core *core = inst->core;
+
+	if (core->res->hfi_version != HFI_VERSION_3XX)
+		return 0;
+
+	return core->ops->session_continue(inst);
+}
+EXPORT_SYMBOL_GPL(hfi_session_continue);
+
+int hfi_session_abort(struct venus_inst *inst)
+{
+	const struct hfi_ops *ops = inst->core->ops;
+	int ret;
+
+	reinit_completion(&inst->done);
+
+	ret = ops->session_abort(inst);
+	if (ret)
+		return ret;
+
+	ret = wait_session_msg(inst);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+int hfi_session_load_res(struct venus_inst *inst)
+{
+	const struct hfi_ops *ops = inst->core->ops;
+	int ret;
+
+	if (inst->state != INST_INIT)
+		return -EINVAL;
+
+	reinit_completion(&inst->done);
+
+	ret = ops->session_load_res(inst);
+	if (ret)
+		return ret;
+
+	ret = wait_session_msg(inst);
+	if (ret)
+		return ret;
+
+	inst->state = INST_LOAD_RESOURCES;
+
+	return 0;
+}
+
+int hfi_session_unload_res(struct venus_inst *inst)
+{
+	const struct hfi_ops *ops = inst->core->ops;
+	int ret;
+
+	if (inst->state != INST_STOP)
+		return -EINVAL;
+
+	reinit_completion(&inst->done);
+
+	ret = ops->session_release_res(inst);
+	if (ret)
+		return ret;
+
+	ret = wait_session_msg(inst);
+	if (ret)
+		return ret;
+
+	inst->state = INST_RELEASE_RESOURCES;
+
+	return 0;
+}
+
+int hfi_session_flush(struct venus_inst *inst)
+{
+	const struct hfi_ops *ops = inst->core->ops;
+	int ret;
+
+	reinit_completion(&inst->done);
+
+	ret = ops->session_flush(inst, HFI_FLUSH_ALL);
+	if (ret)
+		return ret;
+
+	ret = wait_session_msg(inst);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(hfi_session_flush);
+
+int hfi_session_set_buffers(struct venus_inst *inst, struct hfi_buffer_desc *bd)
+{
+	const struct hfi_ops *ops = inst->core->ops;
+
+	return ops->session_set_buffers(inst, bd);
+}
+
+int hfi_session_unset_buffers(struct venus_inst *inst,
+			      struct hfi_buffer_desc *bd)
+{
+	const struct hfi_ops *ops = inst->core->ops;
+	int ret;
+
+	reinit_completion(&inst->done);
+
+	ret = ops->session_unset_buffers(inst, bd);
+	if (ret)
+		return ret;
+
+	if (!bd->response_required)
+		return 0;
+
+	ret = wait_session_msg(inst);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+int hfi_session_get_property(struct venus_inst *inst, u32 ptype,
+			     union hfi_get_property *hprop)
+{
+	const struct hfi_ops *ops = inst->core->ops;
+	int ret;
+
+	if (inst->state < INST_INIT || inst->state >= INST_STOP)
+		return -EINVAL;
+
+	reinit_completion(&inst->done);
+
+	ret = ops->session_get_property(inst, ptype);
+	if (ret)
+		return ret;
+
+	ret = wait_session_msg(inst);
+	if (ret)
+		return ret;
+
+	*hprop = inst->hprop;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(hfi_session_get_property);
+
+int hfi_session_set_property(struct venus_inst *inst, u32 ptype, void *pdata)
+{
+	const struct hfi_ops *ops = inst->core->ops;
+
+	if (inst->state < INST_INIT || inst->state >= INST_STOP)
+		return -EINVAL;
+
+	return ops->session_set_property(inst, ptype, pdata);
+}
+EXPORT_SYMBOL_GPL(hfi_session_set_property);
+
+int hfi_session_process_buf(struct venus_inst *inst, struct hfi_frame_data *fd)
+{
+	const struct hfi_ops *ops = inst->core->ops;
+
+	if (fd->buffer_type == HFI_BUFFER_INPUT)
+		return ops->session_etb(inst, fd);
+	else if (fd->buffer_type == HFI_BUFFER_OUTPUT)
+		return ops->session_ftb(inst, fd);
+
+	return -EINVAL;
+}
+
+irqreturn_t hfi_isr_thread(int irq, void *dev_id)
+{
+	struct venus_core *core = dev_id;
+
+	return core->ops->isr_thread(core);
+}
+
+irqreturn_t hfi_isr(int irq, void *dev)
+{
+	struct venus_core *core = dev;
+
+	return core->ops->isr(core);
+}
+
+int hfi_create(struct venus_core *core, const struct hfi_core_ops *ops)
+{
+	int ret;
+
+	if (!ops)
+		return -EINVAL;
+
+	atomic_set(&core->insts_count, 0);
+	core->core_ops = ops;
+	core->state = CORE_UNINIT;
+	init_completion(&core->done);
+	pkt_set_version(core->res->hfi_version);
+	ret = venus_hfi_create(core);
+
+	return ret;
+}
+
+void hfi_destroy(struct venus_core *core)
+{
+	venus_hfi_destroy(core);
+}
diff --git a/drivers/media/platform/qcom/venus/hfi.h b/drivers/media/platform/qcom/venus/hfi.h
new file mode 100644
index 000000000000..5466b7d60dd0
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/hfi.h
@@ -0,0 +1,175 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __HFI_H__
+#define __HFI_H__
+
+#include <linux/interrupt.h>
+
+#include "hfi_helper.h"
+
+#define VIDC_SESSION_TYPE_VPE			0
+#define VIDC_SESSION_TYPE_ENC			1
+#define VIDC_SESSION_TYPE_DEC			2
+
+#define VIDC_RESOURCE_NONE			0
+#define VIDC_RESOURCE_OCMEM			1
+#define VIDC_RESOURCE_VMEM			2
+
+struct hfi_buffer_desc {
+	u32 buffer_type;
+	u32 buffer_size;
+	u32 num_buffers;
+	u32 device_addr;
+	u32 extradata_addr;
+	u32 extradata_size;
+	u32 response_required;
+};
+
+struct hfi_frame_data {
+	u32 buffer_type;
+	u32 device_addr;
+	u32 extradata_addr;
+	u64 timestamp;
+	u32 flags;
+	u32 offset;
+	u32 alloc_len;
+	u32 filled_len;
+	u32 mark_target;
+	u32 mark_data;
+	u32 clnt_data;
+	u32 extradata_size;
+};
+
+union hfi_get_property {
+	struct hfi_profile_level profile_level;
+	struct hfi_buffer_requirements bufreq[HFI_BUFFER_TYPE_MAX];
+};
+
+/* HFI events */
+#define EVT_SYS_EVENT_CHANGE			1
+#define EVT_SYS_WATCHDOG_TIMEOUT		2
+#define EVT_SYS_ERROR				3
+#define EVT_SESSION_ERROR			4
+
+/* HFI event callback structure */
+struct hfi_event_data {
+	u32 error;
+	u32 height;
+	u32 width;
+	u32 event_type;
+	u32 packet_buffer;
+	u32 extradata_buffer;
+	u32 tag;
+	u32 profile;
+	u32 level;
+};
+
+/* define core states */
+#define CORE_UNINIT				0
+#define CORE_INIT				1
+
+/* define instance states */
+#define INST_UNINIT				2
+#define INST_INIT				3
+#define INST_LOAD_RESOURCES			4
+#define INST_START				5
+#define INST_STOP				6
+#define INST_RELEASE_RESOURCES			7
+
+struct venus_core;
+struct venus_inst;
+
+struct hfi_core_ops {
+	void (*event_notify)(struct venus_core *core, u32 event);
+};
+
+struct hfi_inst_ops {
+	void (*buf_done)(struct venus_inst *inst, unsigned int buf_type,
+			 u32 tag, u32 bytesused, u32 data_offset, u32 flags,
+			 u32 hfi_flags, u64 timestamp_us);
+	void (*event_notify)(struct venus_inst *inst, u32 event,
+			     struct hfi_event_data *data);
+};
+
+struct hfi_ops {
+	int (*core_init)(struct venus_core *core);
+	int (*core_deinit)(struct venus_core *core);
+	int (*core_ping)(struct venus_core *core, u32 cookie);
+	int (*core_trigger_ssr)(struct venus_core *core, u32 trigger_type);
+
+	int (*session_init)(struct venus_inst *inst, u32 session_type,
+			    u32 codec);
+	int (*session_end)(struct venus_inst *inst);
+	int (*session_abort)(struct venus_inst *inst);
+	int (*session_flush)(struct venus_inst *inst, u32 flush_mode);
+	int (*session_start)(struct venus_inst *inst);
+	int (*session_stop)(struct venus_inst *inst);
+	int (*session_continue)(struct venus_inst *inst);
+	int (*session_etb)(struct venus_inst *inst, struct hfi_frame_data *fd);
+	int (*session_ftb)(struct venus_inst *inst, struct hfi_frame_data *fd);
+	int (*session_set_buffers)(struct venus_inst *inst,
+				   struct hfi_buffer_desc *bd);
+	int (*session_unset_buffers)(struct venus_inst *inst,
+				     struct hfi_buffer_desc *bd);
+	int (*session_load_res)(struct venus_inst *inst);
+	int (*session_release_res)(struct venus_inst *inst);
+	int (*session_parse_seq_hdr)(struct venus_inst *inst, u32 seq_hdr,
+				     u32 seq_hdr_len);
+	int (*session_get_seq_hdr)(struct venus_inst *inst, u32 seq_hdr,
+				   u32 seq_hdr_len);
+	int (*session_set_property)(struct venus_inst *inst, u32 ptype,
+				    void *pdata);
+	int (*session_get_property)(struct venus_inst *inst, u32 ptype);
+
+	int (*resume)(struct venus_core *core);
+	int (*suspend)(struct venus_core *core);
+
+	/* interrupt operations */
+	irqreturn_t (*isr)(struct venus_core *core);
+	irqreturn_t (*isr_thread)(struct venus_core *core);
+};
+
+int hfi_create(struct venus_core *core, const struct hfi_core_ops *ops);
+void hfi_destroy(struct venus_core *core);
+
+int hfi_core_init(struct venus_core *core);
+int hfi_core_deinit(struct venus_core *core, bool blocking);
+int hfi_core_suspend(struct venus_core *core);
+int hfi_core_resume(struct venus_core *core, bool force);
+int hfi_core_trigger_ssr(struct venus_core *core, u32 type);
+int hfi_core_ping(struct venus_core *core);
+int hfi_session_create(struct venus_inst *inst, const struct hfi_inst_ops *ops);
+void hfi_session_destroy(struct venus_inst *inst);
+int hfi_session_init(struct venus_inst *inst, u32 pixfmt);
+int hfi_session_deinit(struct venus_inst *inst);
+int hfi_session_start(struct venus_inst *inst);
+int hfi_session_stop(struct venus_inst *inst);
+int hfi_session_continue(struct venus_inst *inst);
+int hfi_session_abort(struct venus_inst *inst);
+int hfi_session_load_res(struct venus_inst *inst);
+int hfi_session_unload_res(struct venus_inst *inst);
+int hfi_session_flush(struct venus_inst *inst);
+int hfi_session_set_buffers(struct venus_inst *inst,
+			    struct hfi_buffer_desc *bd);
+int hfi_session_unset_buffers(struct venus_inst *inst,
+			      struct hfi_buffer_desc *bd);
+int hfi_session_get_property(struct venus_inst *inst, u32 ptype,
+			     union hfi_get_property *hprop);
+int hfi_session_set_property(struct venus_inst *inst, u32 ptype, void *pdata);
+int hfi_session_process_buf(struct venus_inst *inst, struct hfi_frame_data *f);
+irqreturn_t hfi_isr_thread(int irq, void *dev_id);
+irqreturn_t hfi_isr(int irq, void *dev);
+
+#endif
diff --git a/drivers/media/platform/qcom/venus/hfi_cmds.c b/drivers/media/platform/qcom/venus/hfi_cmds.c
new file mode 100644
index 000000000000..b83c5b8ddccb
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/hfi_cmds.c
@@ -0,0 +1,1259 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/errno.h>
+#include <linux/hash.h>
+
+#include "hfi_cmds.h"
+
+static enum hfi_version hfi_ver;
+
+void pkt_sys_init(struct hfi_sys_init_pkt *pkt, u32 arch_type)
+{
+	pkt->hdr.size = sizeof(*pkt);
+	pkt->hdr.pkt_type = HFI_CMD_SYS_INIT;
+	pkt->arch_type = arch_type;
+}
+
+void pkt_sys_pc_prep(struct hfi_sys_pc_prep_pkt *pkt)
+{
+	pkt->hdr.size = sizeof(*pkt);
+	pkt->hdr.pkt_type = HFI_CMD_SYS_PC_PREP;
+}
+
+void pkt_sys_idle_indicator(struct hfi_sys_set_property_pkt *pkt, u32 enable)
+{
+	struct hfi_enable *hfi = (struct hfi_enable *)&pkt->data[1];
+
+	pkt->hdr.size = sizeof(*pkt) + sizeof(*hfi) + sizeof(u32);
+	pkt->hdr.pkt_type = HFI_CMD_SYS_SET_PROPERTY;
+	pkt->num_properties = 1;
+	pkt->data[0] = HFI_PROPERTY_SYS_IDLE_INDICATOR;
+	hfi->enable = enable;
+}
+
+void pkt_sys_debug_config(struct hfi_sys_set_property_pkt *pkt, u32 mode,
+			  u32 config)
+{
+	struct hfi_debug_config *hfi;
+
+	pkt->hdr.size = sizeof(*pkt) + sizeof(*hfi) + sizeof(u32);
+	pkt->hdr.pkt_type = HFI_CMD_SYS_SET_PROPERTY;
+	pkt->num_properties = 1;
+	pkt->data[0] = HFI_PROPERTY_SYS_DEBUG_CONFIG;
+	hfi = (struct hfi_debug_config *)&pkt->data[1];
+	hfi->config = config;
+	hfi->mode = mode;
+}
+
+void pkt_sys_coverage_config(struct hfi_sys_set_property_pkt *pkt, u32 mode)
+{
+	pkt->hdr.size = sizeof(*pkt) + sizeof(u32);
+	pkt->hdr.pkt_type = HFI_CMD_SYS_SET_PROPERTY;
+	pkt->num_properties = 1;
+	pkt->data[0] = HFI_PROPERTY_SYS_CONFIG_COVERAGE;
+	pkt->data[1] = mode;
+}
+
+int pkt_sys_set_resource(struct hfi_sys_set_resource_pkt *pkt, u32 id, u32 size,
+			 u32 addr, void *cookie)
+{
+	pkt->hdr.size = sizeof(*pkt);
+	pkt->hdr.pkt_type = HFI_CMD_SYS_SET_RESOURCE;
+	pkt->resource_handle = hash32_ptr(cookie);
+
+	switch (id) {
+	case VIDC_RESOURCE_OCMEM:
+	case VIDC_RESOURCE_VMEM: {
+		struct hfi_resource_ocmem *res =
+			(struct hfi_resource_ocmem *)&pkt->resource_data[0];
+
+		res->size = size;
+		res->mem = addr;
+		pkt->resource_type = HFI_RESOURCE_OCMEM;
+		pkt->hdr.size += sizeof(*res) - sizeof(u32);
+		break;
+	}
+	case VIDC_RESOURCE_NONE:
+	default:
+		return -ENOTSUPP;
+	}
+
+	return 0;
+}
+
+int pkt_sys_unset_resource(struct hfi_sys_release_resource_pkt *pkt, u32 id,
+			   u32 size, void *cookie)
+{
+	pkt->hdr.size = sizeof(*pkt);
+	pkt->hdr.pkt_type = HFI_CMD_SYS_RELEASE_RESOURCE;
+	pkt->resource_handle = hash32_ptr(cookie);
+
+	switch (id) {
+	case VIDC_RESOURCE_OCMEM:
+	case VIDC_RESOURCE_VMEM:
+		pkt->resource_type = HFI_RESOURCE_OCMEM;
+		break;
+	case VIDC_RESOURCE_NONE:
+		break;
+	default:
+		return -ENOTSUPP;
+	}
+
+	return 0;
+}
+
+void pkt_sys_ping(struct hfi_sys_ping_pkt *pkt, u32 cookie)
+{
+	pkt->hdr.size = sizeof(*pkt);
+	pkt->hdr.pkt_type = HFI_CMD_SYS_PING;
+	pkt->client_data = cookie;
+}
+
+void pkt_sys_power_control(struct hfi_sys_set_property_pkt *pkt, u32 enable)
+{
+	struct hfi_enable *hfi = (struct hfi_enable *)&pkt->data[1];
+
+	pkt->hdr.size = sizeof(*pkt) + sizeof(*hfi) + sizeof(u32);
+	pkt->hdr.pkt_type = HFI_CMD_SYS_SET_PROPERTY;
+	pkt->num_properties = 1;
+	pkt->data[0] = HFI_PROPERTY_SYS_CODEC_POWER_PLANE_CTRL;
+	hfi->enable = enable;
+}
+
+int pkt_sys_ssr_cmd(struct hfi_sys_test_ssr_pkt *pkt, u32 trigger_type)
+{
+	switch (trigger_type) {
+	case HFI_TEST_SSR_SW_ERR_FATAL:
+	case HFI_TEST_SSR_SW_DIV_BY_ZERO:
+	case HFI_TEST_SSR_HW_WDOG_IRQ:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	pkt->hdr.size = sizeof(*pkt);
+	pkt->hdr.pkt_type = HFI_CMD_SYS_TEST_SSR;
+	pkt->trigger_type = trigger_type;
+
+	return 0;
+}
+
+void pkt_sys_image_version(struct hfi_sys_get_property_pkt *pkt)
+{
+	pkt->hdr.size = sizeof(*pkt);
+	pkt->hdr.pkt_type = HFI_CMD_SYS_GET_PROPERTY;
+	pkt->num_properties = 1;
+	pkt->data[0] = HFI_PROPERTY_SYS_IMAGE_VERSION;
+}
+
+int pkt_session_init(struct hfi_session_init_pkt *pkt, void *cookie,
+		     u32 session_type, u32 codec)
+{
+	if (!pkt || !cookie || !codec)
+		return -EINVAL;
+
+	pkt->shdr.hdr.size = sizeof(*pkt);
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SYS_SESSION_INIT;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+	pkt->session_domain = session_type;
+	pkt->session_codec = codec;
+
+	return 0;
+}
+
+void pkt_session_cmd(struct hfi_session_pkt *pkt, u32 pkt_type, void *cookie)
+{
+	pkt->shdr.hdr.size = sizeof(*pkt);
+	pkt->shdr.hdr.pkt_type = pkt_type;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+}
+
+int pkt_session_set_buffers(struct hfi_session_set_buffers_pkt *pkt,
+			    void *cookie, struct hfi_buffer_desc *bd)
+{
+	unsigned int i;
+
+	if (!cookie || !pkt || !bd)
+		return -EINVAL;
+
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_SET_BUFFERS;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+	pkt->buffer_size = bd->buffer_size;
+	pkt->min_buffer_size = bd->buffer_size;
+	pkt->num_buffers = bd->num_buffers;
+
+	if (bd->buffer_type == HFI_BUFFER_OUTPUT ||
+	    bd->buffer_type == HFI_BUFFER_OUTPUT2) {
+		struct hfi_buffer_info *bi;
+
+		pkt->extradata_size = bd->extradata_size;
+		pkt->shdr.hdr.size = sizeof(*pkt) - sizeof(u32) +
+			(bd->num_buffers * sizeof(*bi));
+		bi = (struct hfi_buffer_info *)pkt->buffer_info;
+		for (i = 0; i < pkt->num_buffers; i++) {
+			bi->buffer_addr = bd->device_addr;
+			bi->extradata_addr = bd->extradata_addr;
+		}
+	} else {
+		pkt->extradata_size = 0;
+		pkt->shdr.hdr.size = sizeof(*pkt) +
+			((bd->num_buffers - 1) * sizeof(u32));
+		for (i = 0; i < pkt->num_buffers; i++)
+			pkt->buffer_info[i] = bd->device_addr;
+	}
+
+	pkt->buffer_type = bd->buffer_type;
+
+	return 0;
+}
+
+int pkt_session_unset_buffers(struct hfi_session_release_buffer_pkt *pkt,
+			      void *cookie, struct hfi_buffer_desc *bd)
+{
+	unsigned int i;
+
+	if (!cookie || !pkt || !bd)
+		return -EINVAL;
+
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_RELEASE_BUFFERS;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+	pkt->buffer_size = bd->buffer_size;
+	pkt->num_buffers = bd->num_buffers;
+
+	if (bd->buffer_type == HFI_BUFFER_OUTPUT ||
+	    bd->buffer_type == HFI_BUFFER_OUTPUT2) {
+		struct hfi_buffer_info *bi;
+
+		bi = (struct hfi_buffer_info *)pkt->buffer_info;
+		for (i = 0; i < pkt->num_buffers; i++) {
+			bi->buffer_addr = bd->device_addr;
+			bi->extradata_addr = bd->extradata_addr;
+		}
+		pkt->shdr.hdr.size =
+				sizeof(struct hfi_session_set_buffers_pkt) -
+				sizeof(u32) + (bd->num_buffers * sizeof(*bi));
+	} else {
+		for (i = 0; i < pkt->num_buffers; i++)
+			pkt->buffer_info[i] = bd->device_addr;
+
+		pkt->extradata_size = 0;
+		pkt->shdr.hdr.size =
+				sizeof(struct hfi_session_set_buffers_pkt) +
+				((bd->num_buffers - 1) * sizeof(u32));
+	}
+
+	pkt->response_req = bd->response_required;
+	pkt->buffer_type = bd->buffer_type;
+
+	return 0;
+}
+
+int pkt_session_etb_decoder(struct hfi_session_empty_buffer_compressed_pkt *pkt,
+			    void *cookie, struct hfi_frame_data *in_frame)
+{
+	if (!cookie || !in_frame->device_addr)
+		return -EINVAL;
+
+	pkt->shdr.hdr.size = sizeof(*pkt);
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_EMPTY_BUFFER;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+	pkt->time_stamp_hi = upper_32_bits(in_frame->timestamp);
+	pkt->time_stamp_lo = lower_32_bits(in_frame->timestamp);
+	pkt->flags = in_frame->flags;
+	pkt->mark_target = in_frame->mark_target;
+	pkt->mark_data = in_frame->mark_data;
+	pkt->offset = in_frame->offset;
+	pkt->alloc_len = in_frame->alloc_len;
+	pkt->filled_len = in_frame->filled_len;
+	pkt->input_tag = in_frame->clnt_data;
+	pkt->packet_buffer = in_frame->device_addr;
+
+	return 0;
+}
+
+int pkt_session_etb_encoder(
+		struct hfi_session_empty_buffer_uncompressed_plane0_pkt *pkt,
+		void *cookie, struct hfi_frame_data *in_frame)
+{
+	if (!cookie || !in_frame->device_addr)
+		return -EINVAL;
+
+	pkt->shdr.hdr.size = sizeof(*pkt);
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_EMPTY_BUFFER;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+	pkt->view_id = 0;
+	pkt->time_stamp_hi = upper_32_bits(in_frame->timestamp);
+	pkt->time_stamp_lo = lower_32_bits(in_frame->timestamp);
+	pkt->flags = in_frame->flags;
+	pkt->mark_target = in_frame->mark_target;
+	pkt->mark_data = in_frame->mark_data;
+	pkt->offset = in_frame->offset;
+	pkt->alloc_len = in_frame->alloc_len;
+	pkt->filled_len = in_frame->filled_len;
+	pkt->input_tag = in_frame->clnt_data;
+	pkt->packet_buffer = in_frame->device_addr;
+	pkt->extradata_buffer = in_frame->extradata_addr;
+
+	return 0;
+}
+
+int pkt_session_ftb(struct hfi_session_fill_buffer_pkt *pkt, void *cookie,
+		    struct hfi_frame_data *out_frame)
+{
+	if (!cookie || !out_frame || !out_frame->device_addr)
+		return -EINVAL;
+
+	pkt->shdr.hdr.size = sizeof(*pkt);
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_FILL_BUFFER;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+
+	if (out_frame->buffer_type == HFI_BUFFER_OUTPUT)
+		pkt->stream_id = 0;
+	else if (out_frame->buffer_type == HFI_BUFFER_OUTPUT2)
+		pkt->stream_id = 1;
+
+	pkt->output_tag = out_frame->clnt_data;
+	pkt->packet_buffer = out_frame->device_addr;
+	pkt->extradata_buffer = out_frame->extradata_addr;
+	pkt->alloc_len = out_frame->alloc_len;
+	pkt->filled_len = out_frame->filled_len;
+	pkt->offset = out_frame->offset;
+	pkt->data[0] = out_frame->extradata_size;
+
+	return 0;
+}
+
+int pkt_session_parse_seq_header(
+		struct hfi_session_parse_sequence_header_pkt *pkt,
+		void *cookie, u32 seq_hdr, u32 seq_hdr_len)
+{
+	if (!cookie || !seq_hdr || !seq_hdr_len)
+		return -EINVAL;
+
+	pkt->shdr.hdr.size = sizeof(*pkt);
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_PARSE_SEQUENCE_HEADER;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+	pkt->header_len = seq_hdr_len;
+	pkt->packet_buffer = seq_hdr;
+
+	return 0;
+}
+
+int pkt_session_get_seq_hdr(struct hfi_session_get_sequence_header_pkt *pkt,
+			    void *cookie, u32 seq_hdr, u32 seq_hdr_len)
+{
+	if (!cookie || !seq_hdr || !seq_hdr_len)
+		return -EINVAL;
+
+	pkt->shdr.hdr.size = sizeof(*pkt);
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_GET_SEQUENCE_HEADER;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+	pkt->buffer_len = seq_hdr_len;
+	pkt->packet_buffer = seq_hdr;
+
+	return 0;
+}
+
+int pkt_session_flush(struct hfi_session_flush_pkt *pkt, void *cookie, u32 type)
+{
+	switch (type) {
+	case HFI_FLUSH_INPUT:
+	case HFI_FLUSH_OUTPUT:
+	case HFI_FLUSH_OUTPUT2:
+	case HFI_FLUSH_ALL:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	pkt->shdr.hdr.size = sizeof(*pkt);
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_FLUSH;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+	pkt->flush_type = type;
+
+	return 0;
+}
+
+static int pkt_session_get_property_1x(struct hfi_session_get_property_pkt *pkt,
+				       void *cookie, u32 ptype)
+{
+	switch (ptype) {
+	case HFI_PROPERTY_PARAM_PROFILE_LEVEL_CURRENT:
+	case HFI_PROPERTY_CONFIG_BUFFER_REQUIREMENTS:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	pkt->shdr.hdr.size = sizeof(*pkt);
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_GET_PROPERTY;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+	pkt->num_properties = 1;
+	pkt->data[0] = ptype;
+
+	return 0;
+}
+
+static int pkt_session_set_property_1x(struct hfi_session_set_property_pkt *pkt,
+				       void *cookie, u32 ptype, void *pdata)
+{
+	void *prop_data;
+	int ret = 0;
+
+	if (!pkt || !cookie || !pdata)
+		return -EINVAL;
+
+	prop_data = &pkt->data[1];
+
+	pkt->shdr.hdr.size = sizeof(*pkt);
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_SET_PROPERTY;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+	pkt->num_properties = 1;
+
+	switch (ptype) {
+	case HFI_PROPERTY_CONFIG_FRAME_RATE: {
+		struct hfi_framerate *in = pdata, *frate = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_FRAME_RATE;
+		frate->buffer_type = in->buffer_type;
+		frate->framerate = in->framerate;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*frate);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SELECT: {
+		struct hfi_uncompressed_format_select *in = pdata;
+		struct hfi_uncompressed_format_select *hfi = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SELECT;
+		hfi->buffer_type = in->buffer_type;
+		hfi->format = in->format;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*hfi);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_FRAME_SIZE: {
+		struct hfi_framesize *in = pdata, *fsize = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_FRAME_SIZE;
+		fsize->buffer_type = in->buffer_type;
+		fsize->height = in->height;
+		fsize->width = in->width;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*fsize);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_REALTIME: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_REALTIME;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) * 2;
+		break;
+	}
+	case HFI_PROPERTY_PARAM_BUFFER_COUNT_ACTUAL: {
+		struct hfi_buffer_count_actual *in = pdata, *count = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_BUFFER_COUNT_ACTUAL;
+		count->count_actual = in->count_actual;
+		count->type = in->type;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*count);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_BUFFER_SIZE_ACTUAL: {
+		struct hfi_buffer_size_actual *in = pdata, *sz = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_BUFFER_SIZE_ACTUAL;
+		sz->size = in->size;
+		sz->type = in->type;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*sz);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_BUFFER_DISPLAY_HOLD_COUNT_ACTUAL: {
+		struct hfi_buffer_display_hold_count_actual *in = pdata;
+		struct hfi_buffer_display_hold_count_actual *count = prop_data;
+
+		pkt->data[0] =
+			HFI_PROPERTY_PARAM_BUFFER_DISPLAY_HOLD_COUNT_ACTUAL;
+		count->hold_count = in->hold_count;
+		count->type = in->type;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*count);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_NAL_STREAM_FORMAT_SELECT: {
+		struct hfi_nal_stream_format_select *in = pdata;
+		struct hfi_nal_stream_format_select *fmt = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_NAL_STREAM_FORMAT_SELECT;
+		fmt->format = in->format;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*fmt);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VDEC_OUTPUT_ORDER: {
+		u32 *in = pdata;
+
+		switch (*in) {
+		case HFI_OUTPUT_ORDER_DECODE:
+		case HFI_OUTPUT_ORDER_DISPLAY:
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VDEC_OUTPUT_ORDER;
+		pkt->data[1] = *in;
+		pkt->shdr.hdr.size += sizeof(u32) * 2;
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VDEC_PICTURE_TYPE_DECODE: {
+		struct hfi_enable_picture *in = pdata, *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VDEC_PICTURE_TYPE_DECODE;
+		en->picture_type = in->picture_type;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VDEC_OUTPUT2_KEEP_ASPECT_RATIO: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] =
+			HFI_PROPERTY_PARAM_VDEC_OUTPUT2_KEEP_ASPECT_RATIO;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VDEC_POST_LOOP_DEBLOCKER: {
+		struct hfi_enable *in = pdata;
+		struct hfi_enable *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VDEC_POST_LOOP_DEBLOCKER;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VDEC_MULTI_STREAM: {
+		struct hfi_multi_stream *in = pdata, *multi = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VDEC_MULTI_STREAM;
+		multi->buffer_type = in->buffer_type;
+		multi->enable = in->enable;
+		multi->width = in->width;
+		multi->height = in->height;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*multi);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VDEC_DISPLAY_PICTURE_BUFFER_COUNT: {
+		struct hfi_display_picture_buffer_count *in = pdata;
+		struct hfi_display_picture_buffer_count *count = prop_data;
+
+		pkt->data[0] =
+			HFI_PROPERTY_PARAM_VDEC_DISPLAY_PICTURE_BUFFER_COUNT;
+		count->count = in->count;
+		count->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*count);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_DIVX_FORMAT: {
+		u32 *in = pdata;
+
+		switch (*in) {
+		case HFI_DIVX_FORMAT_4:
+		case HFI_DIVX_FORMAT_5:
+		case HFI_DIVX_FORMAT_6:
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_DIVX_FORMAT;
+		pkt->data[1] = *in;
+		pkt->shdr.hdr.size += sizeof(u32) * 2;
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VDEC_MB_ERROR_MAP_REPORTING: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VDEC_MB_ERROR_MAP_REPORTING;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VDEC_CONTINUE_DATA_TRANSFER: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VDEC_CONTINUE_DATA_TRANSFER;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VDEC_THUMBNAIL_MODE: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VDEC_THUMBNAIL_MODE;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VENC_SYNC_FRAME_SEQUENCE_HEADER: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] =
+			HFI_PROPERTY_CONFIG_VENC_SYNC_FRAME_SEQUENCE_HEADER;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VENC_REQUEST_SYNC_FRAME:
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VENC_REQUEST_SYNC_FRAME;
+		pkt->shdr.hdr.size += sizeof(u32);
+		break;
+	case HFI_PROPERTY_PARAM_VENC_MPEG4_SHORT_HEADER:
+		break;
+	case HFI_PROPERTY_PARAM_VENC_MPEG4_AC_PREDICTION:
+		break;
+	case HFI_PROPERTY_CONFIG_VENC_TARGET_BITRATE: {
+		struct hfi_bitrate *in = pdata, *brate = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VENC_TARGET_BITRATE;
+		brate->bitrate = in->bitrate;
+		brate->layer_id = in->layer_id;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*brate);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VENC_MAX_BITRATE: {
+		struct hfi_bitrate *in = pdata, *hfi = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VENC_MAX_BITRATE;
+		hfi->bitrate = in->bitrate;
+		hfi->layer_id = in->layer_id;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*hfi);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_PROFILE_LEVEL_CURRENT: {
+		struct hfi_profile_level *in = pdata, *pl = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_PROFILE_LEVEL_CURRENT;
+		pl->level = in->level;
+		pl->profile = in->profile;
+		if (pl->profile <= 0)
+			/* Profile not supported, falling back to high */
+			pl->profile = HFI_H264_PROFILE_HIGH;
+
+		if (!pl->level)
+			/* Level not supported, falling back to 1 */
+			pl->level = 1;
+
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*pl);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_H264_ENTROPY_CONTROL: {
+		struct hfi_h264_entropy_control *in = pdata, *hfi = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_H264_ENTROPY_CONTROL;
+		hfi->entropy_mode = in->entropy_mode;
+		if (hfi->entropy_mode == HFI_H264_ENTROPY_CABAC)
+			hfi->cabac_model = in->cabac_model;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*hfi);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_RATE_CONTROL: {
+		u32 *in = pdata;
+
+		switch (*in) {
+		case HFI_RATE_CONTROL_OFF:
+		case HFI_RATE_CONTROL_CBR_CFR:
+		case HFI_RATE_CONTROL_CBR_VFR:
+		case HFI_RATE_CONTROL_VBR_CFR:
+		case HFI_RATE_CONTROL_VBR_VFR:
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_RATE_CONTROL;
+		pkt->data[1] = *in;
+		pkt->shdr.hdr.size += sizeof(u32) * 2;
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_MPEG4_TIME_RESOLUTION: {
+		struct hfi_mpeg4_time_resolution *in = pdata, *res = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_MPEG4_TIME_RESOLUTION;
+		res->time_increment_resolution = in->time_increment_resolution;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*res);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_MPEG4_HEADER_EXTENSION: {
+		struct hfi_mpeg4_header_extension *in = pdata, *ext = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_MPEG4_HEADER_EXTENSION;
+		ext->header_extension = in->header_extension;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*ext);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_H264_DEBLOCK_CONTROL: {
+		struct hfi_h264_db_control *in = pdata, *db = prop_data;
+
+		switch (in->mode) {
+		case HFI_H264_DB_MODE_DISABLE:
+		case HFI_H264_DB_MODE_SKIP_SLICE_BOUNDARY:
+		case HFI_H264_DB_MODE_ALL_BOUNDARY:
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_H264_DEBLOCK_CONTROL;
+		db->mode = in->mode;
+		db->slice_alpha_offset = in->slice_alpha_offset;
+		db->slice_beta_offset = in->slice_beta_offset;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*db);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_SESSION_QP: {
+		struct hfi_quantization *in = pdata, *quant = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_SESSION_QP;
+		quant->qp_i = in->qp_i;
+		quant->qp_p = in->qp_p;
+		quant->qp_b = in->qp_b;
+		quant->layer_id = in->layer_id;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*quant);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_SESSION_QP_RANGE: {
+		struct hfi_quantization_range *in = pdata, *range = prop_data;
+		u32 min_qp, max_qp;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_SESSION_QP_RANGE;
+		min_qp = in->min_qp;
+		max_qp = in->max_qp;
+
+		/* We'll be packing in the qp, so make sure we
+		 * won't be losing data when masking
+		 */
+		if (min_qp > 0xff || max_qp > 0xff) {
+			ret = -ERANGE;
+			break;
+		}
+
+		/* When creating the packet, pack the qp value as
+		 * 0xiippbb, where ii = qp range for I-frames,
+		 * pp = qp range for P-frames, etc.
+		 */
+		range->min_qp = min_qp | min_qp << 8 | min_qp << 16;
+		range->max_qp = max_qp | max_qp << 8 | max_qp << 16;
+		range->layer_id = in->layer_id;
+
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*range);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_VC1_PERF_CFG: {
+		struct hfi_vc1e_perf_cfg_type *in = pdata, *perf = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_VC1_PERF_CFG;
+
+		memcpy(perf->search_range_x_subsampled,
+		       in->search_range_x_subsampled,
+		       sizeof(perf->search_range_x_subsampled));
+		memcpy(perf->search_range_y_subsampled,
+		       in->search_range_y_subsampled,
+		       sizeof(perf->search_range_y_subsampled));
+
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*perf);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_MAX_NUM_B_FRAMES: {
+		struct hfi_max_num_b_frames *bframes = prop_data;
+		u32 *in = pdata;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_MAX_NUM_B_FRAMES;
+		bframes->max_num_b_frames = *in;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*bframes);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VENC_INTRA_PERIOD: {
+		struct hfi_intra_period *in = pdata, *intra = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VENC_INTRA_PERIOD;
+		intra->pframes = in->pframes;
+		intra->bframes = in->bframes;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*intra);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VENC_IDR_PERIOD: {
+		struct hfi_idr_period *in = pdata, *idr = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VENC_IDR_PERIOD;
+		idr->idr_period = in->idr_period;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*idr);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VDEC_CONCEAL_COLOR: {
+		struct hfi_conceal_color *color = prop_data;
+		u32 *in = pdata;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VDEC_CONCEAL_COLOR;
+		color->conceal_color = *in;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*color);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VPE_OPERATIONS: {
+		struct hfi_operations_type *in = pdata, *ops = prop_data;
+
+		switch (in->rotation) {
+		case HFI_ROTATE_NONE:
+		case HFI_ROTATE_90:
+		case HFI_ROTATE_180:
+		case HFI_ROTATE_270:
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		switch (in->flip) {
+		case HFI_FLIP_NONE:
+		case HFI_FLIP_HORIZONTAL:
+		case HFI_FLIP_VERTICAL:
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VPE_OPERATIONS;
+		ops->rotation = in->rotation;
+		ops->flip = in->flip;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*ops);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_INTRA_REFRESH: {
+		struct hfi_intra_refresh *in = pdata, *intra = prop_data;
+
+		switch (in->mode) {
+		case HFI_INTRA_REFRESH_NONE:
+		case HFI_INTRA_REFRESH_ADAPTIVE:
+		case HFI_INTRA_REFRESH_CYCLIC:
+		case HFI_INTRA_REFRESH_CYCLIC_ADAPTIVE:
+		case HFI_INTRA_REFRESH_RANDOM:
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_INTRA_REFRESH;
+		intra->mode = in->mode;
+		intra->air_mbs = in->air_mbs;
+		intra->air_ref = in->air_ref;
+		intra->cir_mbs = in->cir_mbs;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*intra);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_MULTI_SLICE_CONTROL: {
+		struct hfi_multi_slice_control *in = pdata, *multi = prop_data;
+
+		switch (in->multi_slice) {
+		case HFI_MULTI_SLICE_OFF:
+		case HFI_MULTI_SLICE_GOB:
+		case HFI_MULTI_SLICE_BY_MB_COUNT:
+		case HFI_MULTI_SLICE_BY_BYTE_COUNT:
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_MULTI_SLICE_CONTROL;
+		multi->multi_slice = in->multi_slice;
+		multi->slice_size = in->slice_size;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*multi);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_SLICE_DELIVERY_MODE: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_SLICE_DELIVERY_MODE;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_H264_VUI_TIMING_INFO: {
+		struct hfi_h264_vui_timing_info *in = pdata, *vui = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_H264_VUI_TIMING_INFO;
+		vui->enable = in->enable;
+		vui->fixed_framerate = in->fixed_framerate;
+		vui->time_scale = in->time_scale;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*vui);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VPE_DEINTERLACE: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VPE_DEINTERLACE;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_H264_GENERATE_AUDNAL: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_H264_GENERATE_AUDNAL;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_BUFFER_ALLOC_MODE: {
+		struct hfi_buffer_alloc_mode *in = pdata, *mode = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_BUFFER_ALLOC_MODE;
+		mode->type = in->type;
+		mode->mode = in->mode;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*mode);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VDEC_FRAME_ASSEMBLY: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VDEC_FRAME_ASSEMBLY;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_H264_VUI_BITSTREAM_RESTRC: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] =
+			HFI_PROPERTY_PARAM_VENC_H264_VUI_BITSTREAM_RESTRC;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_PRESERVE_TEXT_QUALITY: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_PRESERVE_TEXT_QUALITY;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VDEC_SCS_THRESHOLD: {
+		struct hfi_scs_threshold *thres = prop_data;
+		u32 *in = pdata;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VDEC_SCS_THRESHOLD;
+		thres->threshold_value = *in;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*thres);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_MVC_BUFFER_LAYOUT: {
+		struct hfi_mvc_buffer_layout_descp_type *in = pdata;
+		struct hfi_mvc_buffer_layout_descp_type *mvc = prop_data;
+
+		switch (in->layout_type) {
+		case HFI_MVC_BUFFER_LAYOUT_TOP_BOTTOM:
+		case HFI_MVC_BUFFER_LAYOUT_SEQ:
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_MVC_BUFFER_LAYOUT;
+		mvc->layout_type = in->layout_type;
+		mvc->bright_view_first = in->bright_view_first;
+		mvc->ngap = in->ngap;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*mvc);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_LTRMODE: {
+		struct hfi_ltr_mode *in = pdata, *ltr = prop_data;
+
+		switch (in->ltr_mode) {
+		case HFI_LTR_MODE_DISABLE:
+		case HFI_LTR_MODE_MANUAL:
+		case HFI_LTR_MODE_PERIODIC:
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_LTRMODE;
+		ltr->ltr_mode = in->ltr_mode;
+		ltr->ltr_count = in->ltr_count;
+		ltr->trust_mode = in->trust_mode;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*ltr);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VENC_USELTRFRAME: {
+		struct hfi_ltr_use *in = pdata, *ltr_use = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VENC_USELTRFRAME;
+		ltr_use->frames = in->frames;
+		ltr_use->ref_ltr = in->ref_ltr;
+		ltr_use->use_constrnt = in->use_constrnt;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*ltr_use);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VENC_MARKLTRFRAME: {
+		struct hfi_ltr_mark *in = pdata, *ltr_mark = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VENC_MARKLTRFRAME;
+		ltr_mark->mark_frame = in->mark_frame;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*ltr_mark);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_HIER_P_MAX_NUM_ENH_LAYER: {
+		u32 *in = pdata;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_HIER_P_MAX_NUM_ENH_LAYER;
+		pkt->data[1] = *in;
+		pkt->shdr.hdr.size += sizeof(u32) * 2;
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VENC_HIER_P_ENH_LAYER: {
+		u32 *in = pdata;
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VENC_HIER_P_ENH_LAYER;
+		pkt->data[1] = *in;
+		pkt->shdr.hdr.size += sizeof(u32) * 2;
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_DISABLE_RC_TIMESTAMP: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_DISABLE_RC_TIMESTAMP;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_INITIAL_QP: {
+		struct hfi_initial_quantization *in = pdata, *quant = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_INITIAL_QP;
+		quant->init_qp_enable = in->init_qp_enable;
+		quant->qp_i = in->qp_i;
+		quant->qp_p = in->qp_p;
+		quant->qp_b = in->qp_b;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*quant);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VPE_COLOR_SPACE_CONVERSION: {
+		struct hfi_vpe_color_space_conversion *in = pdata;
+		struct hfi_vpe_color_space_conversion *csc = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VPE_COLOR_SPACE_CONVERSION;
+		memcpy(csc->csc_matrix, in->csc_matrix,
+		       sizeof(csc->csc_matrix));
+		memcpy(csc->csc_bias, in->csc_bias, sizeof(csc->csc_bias));
+		memcpy(csc->csc_limit, in->csc_limit, sizeof(csc->csc_limit));
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*csc);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_VPX_ERROR_RESILIENCE_MODE: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] =
+			HFI_PROPERTY_PARAM_VENC_VPX_ERROR_RESILIENCE_MODE;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_H264_NAL_SVC_EXT: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_H264_NAL_SVC_EXT;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VENC_PERF_MODE: {
+		u32 *in = pdata;
+
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VENC_PERF_MODE;
+		pkt->data[1] = *in;
+		pkt->shdr.hdr.size += sizeof(u32) * 2;
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_HIER_B_MAX_NUM_ENH_LAYER: {
+		u32 *in = pdata;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_HIER_B_MAX_NUM_ENH_LAYER;
+		pkt->data[1] = *in;
+		pkt->shdr.hdr.size += sizeof(u32) * 2;
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VDEC_NONCP_OUTPUT2: {
+		struct hfi_enable *in = pdata, *en = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VDEC_NONCP_OUTPUT2;
+		en->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_HIER_P_HYBRID_MODE: {
+		struct hfi_hybrid_hierp *in = pdata, *hierp = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_HIER_P_HYBRID_MODE;
+		hierp->layers = in->layers;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*hierp);
+		break;
+	}
+
+	/* FOLLOWING PROPERTIES ARE NOT IMPLEMENTED IN CORE YET */
+	case HFI_PROPERTY_CONFIG_BUFFER_REQUIREMENTS:
+	case HFI_PROPERTY_CONFIG_PRIORITY:
+	case HFI_PROPERTY_CONFIG_BATCH_INFO:
+	case HFI_PROPERTY_SYS_IDLE_INDICATOR:
+	case HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SUPPORTED:
+	case HFI_PROPERTY_PARAM_INTERLACE_FORMAT_SUPPORTED:
+	case HFI_PROPERTY_PARAM_CHROMA_SITE:
+	case HFI_PROPERTY_PARAM_PROPERTIES_SUPPORTED:
+	case HFI_PROPERTY_PARAM_PROFILE_LEVEL_SUPPORTED:
+	case HFI_PROPERTY_PARAM_CAPABILITY_SUPPORTED:
+	case HFI_PROPERTY_PARAM_NAL_STREAM_FORMAT_SUPPORTED:
+	case HFI_PROPERTY_PARAM_MULTI_VIEW_FORMAT:
+	case HFI_PROPERTY_PARAM_MAX_SEQUENCE_HEADER_SIZE:
+	case HFI_PROPERTY_PARAM_CODEC_SUPPORTED:
+	case HFI_PROPERTY_PARAM_VDEC_MULTI_VIEW_SELECT:
+	case HFI_PROPERTY_PARAM_VDEC_MB_QUANTIZATION:
+	case HFI_PROPERTY_PARAM_VDEC_NUM_CONCEALED_MB:
+	case HFI_PROPERTY_PARAM_VDEC_H264_ENTROPY_SWITCHING:
+	case HFI_PROPERTY_PARAM_VENC_MULTI_SLICE_INFO:
+	default:
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static int
+pkt_session_get_property_3xx(struct hfi_session_get_property_pkt *pkt,
+			     void *cookie, u32 ptype)
+{
+	int ret = 0;
+
+	if (!pkt || !cookie)
+		return -EINVAL;
+
+	pkt->shdr.hdr.size = sizeof(struct hfi_session_get_property_pkt);
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_GET_PROPERTY;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+	pkt->num_properties = 1;
+
+	switch (ptype) {
+	case HFI_PROPERTY_CONFIG_VDEC_ENTROPY:
+		pkt->data[0] = HFI_PROPERTY_CONFIG_VDEC_ENTROPY;
+		break;
+	default:
+		ret = pkt_session_get_property_1x(pkt, cookie, ptype);
+		break;
+	}
+
+	return ret;
+}
+
+static int
+pkt_session_set_property_3xx(struct hfi_session_set_property_pkt *pkt,
+			     void *cookie, u32 ptype, void *pdata)
+{
+	void *prop_data;
+	int ret = 0;
+
+	if (!pkt || !cookie || !pdata)
+		return -EINVAL;
+
+	prop_data = &pkt->data[1];
+
+	pkt->shdr.hdr.size = sizeof(*pkt);
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_SET_PROPERTY;
+	pkt->shdr.session_id = hash32_ptr(cookie);
+	pkt->num_properties = 1;
+
+	/*
+	 * Any session set property which is different in 3XX packetization
+	 * should be added as a new case below. All unchanged session set
+	 * properties will be handled in the default case.
+	 */
+	switch (ptype) {
+	case HFI_PROPERTY_PARAM_VDEC_MULTI_STREAM: {
+		struct hfi_multi_stream *in = pdata;
+		struct hfi_multi_stream_3x *multi = prop_data;
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VDEC_MULTI_STREAM;
+		multi->buffer_type = in->buffer_type;
+		multi->enable = in->enable;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*multi);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VENC_INTRA_REFRESH: {
+		struct hfi_intra_refresh *in = pdata;
+		struct hfi_intra_refresh_3x *intra = prop_data;
+
+		switch (in->mode) {
+		case HFI_INTRA_REFRESH_NONE:
+		case HFI_INTRA_REFRESH_ADAPTIVE:
+		case HFI_INTRA_REFRESH_CYCLIC:
+		case HFI_INTRA_REFRESH_CYCLIC_ADAPTIVE:
+		case HFI_INTRA_REFRESH_RANDOM:
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		pkt->data[0] = HFI_PROPERTY_PARAM_VENC_INTRA_REFRESH;
+		intra->mode = in->mode;
+		intra->mbs = in->cir_mbs;
+		pkt->shdr.hdr.size += sizeof(u32) + sizeof(*intra);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VDEC_CONTINUE_DATA_TRANSFER:
+		/* for 3xx fw version session_continue is used */
+		break;
+	default:
+		ret = pkt_session_set_property_1x(pkt, cookie, ptype, pdata);
+		break;
+	}
+
+	return ret;
+}
+
+int pkt_session_get_property(struct hfi_session_get_property_pkt *pkt,
+			     void *cookie, u32 ptype)
+{
+	if (hfi_ver == HFI_VERSION_1XX)
+		return pkt_session_get_property_1x(pkt, cookie, ptype);
+
+	return pkt_session_get_property_3xx(pkt, cookie, ptype);
+}
+
+int pkt_session_set_property(struct hfi_session_set_property_pkt *pkt,
+			     void *cookie, u32 ptype, void *pdata)
+{
+	if (hfi_ver == HFI_VERSION_1XX)
+		return pkt_session_set_property_1x(pkt, cookie, ptype, pdata);
+
+	return pkt_session_set_property_3xx(pkt, cookie, ptype, pdata);
+}
+
+void pkt_set_version(enum hfi_version version)
+{
+	hfi_ver = version;
+}
diff --git a/drivers/media/platform/qcom/venus/hfi_cmds.h b/drivers/media/platform/qcom/venus/hfi_cmds.h
new file mode 100644
index 000000000000..f7617cf59914
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/hfi_cmds.h
@@ -0,0 +1,304 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __VENUS_HFI_CMDS_H__
+#define __VENUS_HFI_CMDS_H__
+
+#include "hfi.h"
+
+/* commands */
+#define HFI_CMD_SYS_INIT			0x10001
+#define HFI_CMD_SYS_PC_PREP			0x10002
+#define HFI_CMD_SYS_SET_RESOURCE		0x10003
+#define HFI_CMD_SYS_RELEASE_RESOURCE		0x10004
+#define HFI_CMD_SYS_SET_PROPERTY		0x10005
+#define HFI_CMD_SYS_GET_PROPERTY		0x10006
+#define HFI_CMD_SYS_SESSION_INIT		0x10007
+#define HFI_CMD_SYS_SESSION_END			0x10008
+#define HFI_CMD_SYS_SET_BUFFERS			0x10009
+#define HFI_CMD_SYS_TEST_SSR			0x10101
+
+#define HFI_CMD_SESSION_SET_PROPERTY		0x11001
+#define HFI_CMD_SESSION_SET_BUFFERS		0x11002
+#define HFI_CMD_SESSION_GET_SEQUENCE_HEADER	0x11003
+
+#define HFI_CMD_SYS_SESSION_ABORT		0x210001
+#define HFI_CMD_SYS_PING			0x210002
+
+#define HFI_CMD_SESSION_LOAD_RESOURCES		0x211001
+#define HFI_CMD_SESSION_START			0x211002
+#define HFI_CMD_SESSION_STOP			0x211003
+#define HFI_CMD_SESSION_EMPTY_BUFFER		0x211004
+#define HFI_CMD_SESSION_FILL_BUFFER		0x211005
+#define HFI_CMD_SESSION_SUSPEND			0x211006
+#define HFI_CMD_SESSION_RESUME			0x211007
+#define HFI_CMD_SESSION_FLUSH			0x211008
+#define HFI_CMD_SESSION_GET_PROPERTY		0x211009
+#define HFI_CMD_SESSION_PARSE_SEQUENCE_HEADER	0x21100a
+#define HFI_CMD_SESSION_RELEASE_BUFFERS		0x21100b
+#define HFI_CMD_SESSION_RELEASE_RESOURCES	0x21100c
+#define HFI_CMD_SESSION_CONTINUE		0x21100d
+#define HFI_CMD_SESSION_SYNC			0x21100e
+
+/* command packets */
+struct hfi_sys_init_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 arch_type;
+};
+
+struct hfi_sys_pc_prep_pkt {
+	struct hfi_pkt_hdr hdr;
+};
+
+struct hfi_sys_set_resource_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 resource_handle;
+	u32 resource_type;
+	u32 resource_data[1];
+};
+
+struct hfi_sys_release_resource_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 resource_type;
+	u32 resource_handle;
+};
+
+struct hfi_sys_set_property_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 num_properties;
+	u32 data[1];
+};
+
+struct hfi_sys_get_property_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 num_properties;
+	u32 data[1];
+};
+
+struct hfi_sys_set_buffers_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 buffer_type;
+	u32 buffer_size;
+	u32 num_buffers;
+	u32 buffer_addr[1];
+};
+
+struct hfi_sys_ping_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 client_data;
+};
+
+struct hfi_session_init_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 session_domain;
+	u32 session_codec;
+};
+
+struct hfi_session_end_pkt {
+	struct hfi_session_hdr_pkt shdr;
+};
+
+struct hfi_session_abort_pkt {
+	struct hfi_session_hdr_pkt shdr;
+};
+
+struct hfi_session_set_property_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 num_properties;
+	u32 data[0];
+};
+
+struct hfi_session_set_buffers_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 buffer_type;
+	u32 buffer_size;
+	u32 extradata_size;
+	u32 min_buffer_size;
+	u32 num_buffers;
+	u32 buffer_info[1];
+};
+
+struct hfi_session_get_sequence_header_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 buffer_len;
+	u32 packet_buffer;
+};
+
+struct hfi_session_load_resources_pkt {
+	struct hfi_session_hdr_pkt shdr;
+};
+
+struct hfi_session_start_pkt {
+	struct hfi_session_hdr_pkt shdr;
+};
+
+struct hfi_session_stop_pkt {
+	struct hfi_session_hdr_pkt shdr;
+};
+
+struct hfi_session_empty_buffer_compressed_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 time_stamp_hi;
+	u32 time_stamp_lo;
+	u32 flags;
+	u32 mark_target;
+	u32 mark_data;
+	u32 offset;
+	u32 alloc_len;
+	u32 filled_len;
+	u32 input_tag;
+	u32 packet_buffer;
+	u32 extradata_buffer;
+	u32 data[1];
+};
+
+struct hfi_session_empty_buffer_uncompressed_plane0_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 view_id;
+	u32 time_stamp_hi;
+	u32 time_stamp_lo;
+	u32 flags;
+	u32 mark_target;
+	u32 mark_data;
+	u32 alloc_len;
+	u32 filled_len;
+	u32 offset;
+	u32 input_tag;
+	u32 packet_buffer;
+	u32 extradata_buffer;
+	u32 data[1];
+};
+
+struct hfi_session_empty_buffer_uncompressed_plane1_pkt {
+	u32 flags;
+	u32 alloc_len;
+	u32 filled_len;
+	u32 offset;
+	u32 packet_buffer2;
+	u32 data[1];
+};
+
+struct hfi_session_empty_buffer_uncompressed_plane2_pkt {
+	u32 flags;
+	u32 alloc_len;
+	u32 filled_len;
+	u32 offset;
+	u32 packet_buffer3;
+	u32 data[1];
+};
+
+struct hfi_session_fill_buffer_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 stream_id;
+	u32 offset;
+	u32 alloc_len;
+	u32 filled_len;
+	u32 output_tag;
+	u32 packet_buffer;
+	u32 extradata_buffer;
+	u32 data[1];
+};
+
+struct hfi_session_flush_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 flush_type;
+};
+
+struct hfi_session_suspend_pkt {
+	struct hfi_session_hdr_pkt shdr;
+};
+
+struct hfi_session_resume_pkt {
+	struct hfi_session_hdr_pkt shdr;
+};
+
+struct hfi_session_get_property_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 num_properties;
+	u32 data[1];
+};
+
+struct hfi_session_release_buffer_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 buffer_type;
+	u32 buffer_size;
+	u32 extradata_size;
+	u32 response_req;
+	u32 num_buffers;
+	u32 buffer_info[1];
+};
+
+struct hfi_session_release_resources_pkt {
+	struct hfi_session_hdr_pkt shdr;
+};
+
+struct hfi_session_parse_sequence_header_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 header_len;
+	u32 packet_buffer;
+};
+
+struct hfi_sfr {
+	u32 buf_size;
+	u8 data[1];
+};
+
+struct hfi_sys_test_ssr_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 trigger_type;
+};
+
+void pkt_set_version(enum hfi_version version);
+
+void pkt_sys_init(struct hfi_sys_init_pkt *pkt, u32 arch_type);
+void pkt_sys_pc_prep(struct hfi_sys_pc_prep_pkt *pkt);
+void pkt_sys_idle_indicator(struct hfi_sys_set_property_pkt *pkt, u32 enable);
+void pkt_sys_power_control(struct hfi_sys_set_property_pkt *pkt, u32 enable);
+int pkt_sys_set_resource(struct hfi_sys_set_resource_pkt *pkt, u32 id, u32 size,
+			 u32 addr, void *cookie);
+int pkt_sys_unset_resource(struct hfi_sys_release_resource_pkt *pkt, u32 id,
+			   u32 size, void *cookie);
+void pkt_sys_debug_config(struct hfi_sys_set_property_pkt *pkt, u32 mode,
+			  u32 config);
+void pkt_sys_coverage_config(struct hfi_sys_set_property_pkt *pkt, u32 mode);
+void pkt_sys_ping(struct hfi_sys_ping_pkt *pkt, u32 cookie);
+void pkt_sys_image_version(struct hfi_sys_get_property_pkt *pkt);
+int pkt_sys_ssr_cmd(struct hfi_sys_test_ssr_pkt *pkt, u32 trigger_type);
+int pkt_session_init(struct hfi_session_init_pkt *pkt, void *cookie,
+		     u32 session_type, u32 codec);
+void pkt_session_cmd(struct hfi_session_pkt *pkt, u32 pkt_type, void *cookie);
+int pkt_session_set_buffers(struct hfi_session_set_buffers_pkt *pkt,
+			    void *cookie, struct hfi_buffer_desc *bd);
+int pkt_session_unset_buffers(struct hfi_session_release_buffer_pkt *pkt,
+			      void *cookie, struct hfi_buffer_desc *bd);
+int pkt_session_etb_decoder(struct hfi_session_empty_buffer_compressed_pkt *pkt,
+			    void *cookie, struct hfi_frame_data *input_frame);
+int pkt_session_etb_encoder(
+		struct hfi_session_empty_buffer_uncompressed_plane0_pkt *pkt,
+		void *cookie, struct hfi_frame_data *input_frame);
+int pkt_session_ftb(struct hfi_session_fill_buffer_pkt *pkt,
+		    void *cookie, struct hfi_frame_data *output_frame);
+int pkt_session_parse_seq_header(
+		struct hfi_session_parse_sequence_header_pkt *pkt,
+		void *cookie, u32 seq_hdr, u32 seq_hdr_len);
+int pkt_session_get_seq_hdr(struct hfi_session_get_sequence_header_pkt *pkt,
+			    void *cookie, u32 seq_hdr, u32 seq_hdr_len);
+int pkt_session_flush(struct hfi_session_flush_pkt *pkt, void *cookie,
+		      u32 flush_mode);
+int pkt_session_get_property(struct hfi_session_get_property_pkt *pkt,
+			     void *cookie, u32 ptype);
+int pkt_session_set_property(struct hfi_session_set_property_pkt *pkt,
+			     void *cookie, u32 ptype, void *pdata);
+
+#endif
diff --git a/drivers/media/platform/qcom/venus/hfi_helper.h b/drivers/media/platform/qcom/venus/hfi_helper.h
new file mode 100644
index 000000000000..8d282dba9e57
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/hfi_helper.h
@@ -0,0 +1,1050 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __VENUS_HFI_HELPER_H__
+#define __VENUS_HFI_HELPER_H__
+
+#define HFI_DOMAIN_BASE_COMMON				0
+
+#define HFI_DOMAIN_BASE_VDEC				0x1000000
+#define HFI_DOMAIN_BASE_VENC				0x2000000
+#define HFI_DOMAIN_BASE_VPE				0x3000000
+
+#define HFI_VIDEO_ARCH_OX				0x1
+
+#define HFI_ARCH_COMMON_OFFSET				0
+#define HFI_ARCH_OX_OFFSET				0x200000
+
+#define HFI_OX_BASE					0x1000000
+
+#define HFI_CMD_START_OFFSET				0x10000
+#define HFI_MSG_START_OFFSET				0x20000
+
+#define HFI_ERR_NONE					0x0
+#define HFI_ERR_SYS_FATAL				0x1
+#define HFI_ERR_SYS_INVALID_PARAMETER			0x2
+#define HFI_ERR_SYS_VERSION_MISMATCH			0x3
+#define HFI_ERR_SYS_INSUFFICIENT_RESOURCES		0x4
+#define HFI_ERR_SYS_MAX_SESSIONS_REACHED		0x5
+#define HFI_ERR_SYS_UNSUPPORTED_CODEC			0x6
+#define HFI_ERR_SYS_SESSION_IN_USE			0x7
+#define HFI_ERR_SYS_SESSION_ID_OUT_OF_RANGE		0x8
+#define HFI_ERR_SYS_UNSUPPORTED_DOMAIN			0x9
+
+#define HFI_ERR_SESSION_FATAL				0x1001
+#define HFI_ERR_SESSION_INVALID_PARAMETER		0x1002
+#define HFI_ERR_SESSION_BAD_POINTER			0x1003
+#define HFI_ERR_SESSION_INVALID_SESSION_ID		0x1004
+#define HFI_ERR_SESSION_INVALID_STREAM_ID		0x1005
+#define HFI_ERR_SESSION_INCORRECT_STATE_OPERATION	0x1006
+#define HFI_ERR_SESSION_UNSUPPORTED_PROPERTY		0x1007
+#define HFI_ERR_SESSION_UNSUPPORTED_SETTING		0x1008
+#define HFI_ERR_SESSION_INSUFFICIENT_RESOURCES		0x1009
+#define HFI_ERR_SESSION_STREAM_CORRUPT_OUTPUT_STALLED	0x100a
+#define HFI_ERR_SESSION_STREAM_CORRUPT			0x100b
+#define HFI_ERR_SESSION_ENC_OVERFLOW			0x100c
+#define HFI_ERR_SESSION_UNSUPPORTED_STREAM		0x100d
+#define HFI_ERR_SESSION_CMDSIZE				0x100e
+#define HFI_ERR_SESSION_UNSUPPORT_CMD			0x100f
+#define HFI_ERR_SESSION_UNSUPPORT_BUFFERTYPE		0x1010
+#define HFI_ERR_SESSION_BUFFERCOUNT_TOOSMALL		0x1011
+#define HFI_ERR_SESSION_INVALID_SCALE_FACTOR		0x1012
+#define HFI_ERR_SESSION_UPSCALE_NOT_SUPPORTED		0x1013
+
+#define HFI_EVENT_SYS_ERROR				0x1
+#define HFI_EVENT_SESSION_ERROR				0x2
+
+#define HFI_EVENT_DATA_SEQUENCE_CHANGED_SUFFICIENT_BUF_RESOURCES   0x1000001
+#define HFI_EVENT_DATA_SEQUENCE_CHANGED_INSUFFICIENT_BUF_RESOURCES 0x1000002
+#define HFI_EVENT_SESSION_SEQUENCE_CHANGED			   0x1000003
+#define HFI_EVENT_SESSION_PROPERTY_CHANGED			   0x1000004
+#define HFI_EVENT_SESSION_LTRUSE_FAILED				   0x1000005
+#define HFI_EVENT_RELEASE_BUFFER_REFERENCE			   0x1000006
+
+#define HFI_BUFFERFLAG_EOS				0x00000001
+#define HFI_BUFFERFLAG_STARTTIME			0x00000002
+#define HFI_BUFFERFLAG_DECODEONLY			0x00000004
+#define HFI_BUFFERFLAG_DATACORRUPT			0x00000008
+#define HFI_BUFFERFLAG_ENDOFFRAME			0x00000010
+#define HFI_BUFFERFLAG_SYNCFRAME			0x00000020
+#define HFI_BUFFERFLAG_EXTRADATA			0x00000040
+#define HFI_BUFFERFLAG_CODECCONFIG			0x00000080
+#define HFI_BUFFERFLAG_TIMESTAMPINVALID			0x00000100
+#define HFI_BUFFERFLAG_READONLY				0x00000200
+#define HFI_BUFFERFLAG_ENDOFSUBFRAME			0x00000400
+#define HFI_BUFFERFLAG_EOSEQ				0x00200000
+#define HFI_BUFFERFLAG_MBAFF				0x08000000
+#define HFI_BUFFERFLAG_VPE_YUV_601_709_CSC_CLAMP	0x10000000
+#define HFI_BUFFERFLAG_DROP_FRAME			0x20000000
+#define HFI_BUFFERFLAG_TEI				0x40000000
+#define HFI_BUFFERFLAG_DISCONTINUITY			0x80000000
+
+#define HFI_ERR_SESSION_EMPTY_BUFFER_DONE_OUTPUT_PENDING	0x1001001
+#define HFI_ERR_SESSION_SAME_STATE_OPERATION			0x1001002
+#define HFI_ERR_SESSION_SYNC_FRAME_NOT_DETECTED			0x1001003
+#define HFI_ERR_SESSION_START_CODE_NOT_FOUND			0x1001004
+
+#define HFI_FLUSH_INPUT					0x1000001
+#define HFI_FLUSH_OUTPUT				0x1000002
+#define HFI_FLUSH_OUTPUT2				0x1000003
+#define HFI_FLUSH_ALL					0x1000004
+
+#define HFI_EXTRADATA_NONE				0x00000000
+#define HFI_EXTRADATA_MB_QUANTIZATION			0x00000001
+#define HFI_EXTRADATA_INTERLACE_VIDEO			0x00000002
+#define HFI_EXTRADATA_VC1_FRAMEDISP			0x00000003
+#define HFI_EXTRADATA_VC1_SEQDISP			0x00000004
+#define HFI_EXTRADATA_TIMESTAMP				0x00000005
+#define HFI_EXTRADATA_S3D_FRAME_PACKING			0x00000006
+#define HFI_EXTRADATA_FRAME_RATE			0x00000007
+#define HFI_EXTRADATA_PANSCAN_WINDOW			0x00000008
+#define HFI_EXTRADATA_RECOVERY_POINT_SEI		0x00000009
+#define HFI_EXTRADATA_MPEG2_SEQDISP			0x0000000d
+#define HFI_EXTRADATA_STREAM_USERDATA			0x0000000e
+#define HFI_EXTRADATA_FRAME_QP				0x0000000f
+#define HFI_EXTRADATA_FRAME_BITS_INFO			0x00000010
+#define HFI_EXTRADATA_MULTISLICE_INFO			0x7f100000
+#define HFI_EXTRADATA_NUM_CONCEALED_MB			0x7f100001
+#define HFI_EXTRADATA_INDEX				0x7f100002
+#define HFI_EXTRADATA_METADATA_LTR			0x7f100004
+#define HFI_EXTRADATA_METADATA_FILLER			0x7fe00002
+
+#define HFI_INDEX_EXTRADATA_INPUT_CROP			0x0700000e
+#define HFI_INDEX_EXTRADATA_DIGITAL_ZOOM		0x07000010
+#define HFI_INDEX_EXTRADATA_ASPECT_RATIO		0x7f100003
+
+#define HFI_INTERLACE_FRAME_PROGRESSIVE			0x01
+#define HFI_INTERLACE_INTERLEAVE_FRAME_TOPFIELDFIRST	0x02
+#define HFI_INTERLACE_INTERLEAVE_FRAME_BOTTOMFIELDFIRST	0x04
+#define HFI_INTERLACE_FRAME_TOPFIELDFIRST		0x08
+#define HFI_INTERLACE_FRAME_BOTTOMFIELDFIRST		0x10
+
+/*
+ * HFI_PROPERTY_PARAM_OX_START
+ * HFI_DOMAIN_BASE_COMMON + HFI_ARCH_OX_OFFSET + 0x1000
+ */
+#define HFI_PROPERTY_PARAM_BUFFER_COUNT_ACTUAL				0x201001
+#define HFI_PROPERTY_PARAM_UNCOMPRESSED_PLANE_ACTUAL_CONSTRAINTS_INFO	0x201002
+#define HFI_PROPERTY_PARAM_INTERLACE_FORMAT_SUPPORTED			0x201003
+#define HFI_PROPERTY_PARAM_CHROMA_SITE					0x201004
+#define HFI_PROPERTY_PARAM_EXTRA_DATA_HEADER_CONFIG			0x201005
+#define HFI_PROPERTY_PARAM_INDEX_EXTRADATA				0x201006
+#define HFI_PROPERTY_PARAM_DIVX_FORMAT					0x201007
+#define HFI_PROPERTY_PARAM_BUFFER_ALLOC_MODE				0x201008
+#define HFI_PROPERTY_PARAM_S3D_FRAME_PACKING_EXTRADATA			0x201009
+#define HFI_PROPERTY_PARAM_ERR_DETECTION_CODE_EXTRADATA			0x20100a
+#define HFI_PROPERTY_PARAM_BUFFER_ALLOC_MODE_SUPPORTED			0x20100b
+#define HFI_PROPERTY_PARAM_BUFFER_SIZE_ACTUAL				0x20100c
+#define HFI_PROPERTY_PARAM_BUFFER_DISPLAY_HOLD_COUNT_ACTUAL		0x20100d
+
+/*
+ * HFI_PROPERTY_CONFIG_OX_START
+ * HFI_DOMAIN_BASE_COMMON + HFI_ARCH_OX_OFFSET + 0x2000
+ */
+#define HFI_PROPERTY_CONFIG_BUFFER_REQUIREMENTS		0x202001
+#define HFI_PROPERTY_CONFIG_REALTIME			0x202002
+#define HFI_PROPERTY_CONFIG_PRIORITY			0x202003
+#define HFI_PROPERTY_CONFIG_BATCH_INFO			0x202004
+
+/*
+ * HFI_PROPERTY_PARAM_VDEC_OX_START	\
+ * HFI_DOMAIN_BASE_VDEC + HFI_ARCH_OX_OFFSET + 0x3000
+ */
+#define HFI_PROPERTY_PARAM_VDEC_CONTINUE_DATA_TRANSFER		0x1203001
+#define HFI_PROPERTY_PARAM_VDEC_DISPLAY_PICTURE_BUFFER_COUNT	0x1203002
+#define HFI_PROPERTY_PARAM_VDEC_MULTI_VIEW_SELECT		0x1203003
+#define HFI_PROPERTY_PARAM_VDEC_PICTURE_TYPE_DECODE		0x1203004
+#define HFI_PROPERTY_PARAM_VDEC_OUTPUT_ORDER			0x1203005
+#define HFI_PROPERTY_PARAM_VDEC_MB_QUANTIZATION			0x1203006
+#define HFI_PROPERTY_PARAM_VDEC_NUM_CONCEALED_MB		0x1203007
+#define HFI_PROPERTY_PARAM_VDEC_H264_ENTROPY_SWITCHING		0x1203008
+#define HFI_PROPERTY_PARAM_VDEC_OUTPUT2_KEEP_ASPECT_RATIO	0x1203009
+#define HFI_PROPERTY_PARAM_VDEC_FRAME_RATE_EXTRADATA		0x120300a
+#define HFI_PROPERTY_PARAM_VDEC_PANSCAN_WNDW_EXTRADATA		0x120300b
+#define HFI_PROPERTY_PARAM_VDEC_RECOVERY_POINT_SEI_EXTRADATA	0x120300c
+#define HFI_PROPERTY_PARAM_VDEC_THUMBNAIL_MODE			0x120300d
+#define HFI_PROPERTY_PARAM_VDEC_FRAME_ASSEMBLY			0x120300e
+#define HFI_PROPERTY_PARAM_VDEC_VC1_FRAMEDISP_EXTRADATA		0x1203011
+#define HFI_PROPERTY_PARAM_VDEC_VC1_SEQDISP_EXTRADATA		0x1203012
+#define HFI_PROPERTY_PARAM_VDEC_TIMESTAMP_EXTRADATA		0x1203013
+#define HFI_PROPERTY_PARAM_VDEC_INTERLACE_VIDEO_EXTRADATA	0x1203014
+#define HFI_PROPERTY_PARAM_VDEC_AVC_SESSION_SELECT		0x1203015
+#define HFI_PROPERTY_PARAM_VDEC_MPEG2_SEQDISP_EXTRADATA		0x1203016
+#define HFI_PROPERTY_PARAM_VDEC_STREAM_USERDATA_EXTRADATA	0x1203017
+#define HFI_PROPERTY_PARAM_VDEC_FRAME_QP_EXTRADATA		0x1203018
+#define HFI_PROPERTY_PARAM_VDEC_FRAME_BITS_INFO_EXTRADATA	0x1203019
+#define HFI_PROPERTY_PARAM_VDEC_SCS_THRESHOLD			0x120301a
+
+/*
+ * HFI_PROPERTY_CONFIG_VDEC_OX_START
+ * HFI_DOMAIN_BASE_VDEC + HFI_ARCH_OX_OFFSET + 0x0000
+ */
+#define HFI_PROPERTY_CONFIG_VDEC_POST_LOOP_DEBLOCKER		0x1200001
+#define HFI_PROPERTY_CONFIG_VDEC_MB_ERROR_MAP_REPORTING		0x1200002
+#define HFI_PROPERTY_CONFIG_VDEC_MB_ERROR_MAP			0x1200003
+
+#define HFI_PROPERTY_CONFIG_VDEC_ENTROPY			0x1204004
+
+/*
+ * HFI_PROPERTY_PARAM_VENC_OX_START
+ * HFI_DOMAIN_BASE_VENC + HFI_ARCH_OX_OFFSET + 0x5000
+ */
+#define  HFI_PROPERTY_PARAM_VENC_MULTI_SLICE_INFO		0x2205001
+#define  HFI_PROPERTY_PARAM_VENC_H264_IDR_S3D_FRAME_PACKING_NAL	0x2205002
+#define  HFI_PROPERTY_PARAM_VENC_LTR_INFO			0x2205003
+#define  HFI_PROPERTY_PARAM_VENC_MBI_DUMPING			0x2205005
+
+/*
+ * HFI_PROPERTY_CONFIG_VENC_OX_START
+ * HFI_DOMAIN_BASE_VENC + HFI_ARCH_OX_OFFSET + 0x6000
+ */
+#define HFI_PROPERTY_CONFIG_VENC_FRAME_QP			0x2206001
+
+/*
+ * HFI_PROPERTY_PARAM_VPE_OX_START
+ * HFI_DOMAIN_BASE_VPE + HFI_ARCH_OX_OFFSET + 0x7000
+ */
+#define HFI_PROPERTY_PARAM_VPE_COLOR_SPACE_CONVERSION		0x3207001
+
+#define HFI_PROPERTY_CONFIG_VPE_OX_START	\
+	(HFI_DOMAIN_BASE_VPE + HFI_ARCH_OX_OFFSET + 0x8000)
+
+#define HFI_CHROMA_SITE_0			0x1000001
+#define HFI_CHROMA_SITE_1			0x1000002
+#define HFI_CHROMA_SITE_2			0x1000003
+#define HFI_CHROMA_SITE_3			0x1000004
+#define HFI_CHROMA_SITE_4			0x1000005
+#define HFI_CHROMA_SITE_5			0x1000006
+
+#define HFI_PRIORITY_LOW			10
+#define HFI_PRIOIRTY_MEDIUM			20
+#define HFI_PRIORITY_HIGH			30
+
+#define HFI_OUTPUT_ORDER_DISPLAY		0x1000001
+#define HFI_OUTPUT_ORDER_DECODE			0x1000002
+
+#define HFI_RATE_CONTROL_OFF			0x1000001
+#define HFI_RATE_CONTROL_VBR_VFR		0x1000002
+#define HFI_RATE_CONTROL_VBR_CFR		0x1000003
+#define HFI_RATE_CONTROL_CBR_VFR		0x1000004
+#define HFI_RATE_CONTROL_CBR_CFR		0x1000005
+
+#define HFI_VIDEO_CODEC_H264			0x00000002
+#define HFI_VIDEO_CODEC_H263			0x00000004
+#define HFI_VIDEO_CODEC_MPEG1			0x00000008
+#define HFI_VIDEO_CODEC_MPEG2			0x00000010
+#define HFI_VIDEO_CODEC_MPEG4			0x00000020
+#define HFI_VIDEO_CODEC_DIVX_311		0x00000040
+#define HFI_VIDEO_CODEC_DIVX			0x00000080
+#define HFI_VIDEO_CODEC_VC1			0x00000100
+#define HFI_VIDEO_CODEC_SPARK			0x00000200
+#define HFI_VIDEO_CODEC_VP8			0x00001000
+#define HFI_VIDEO_CODEC_HEVC			0x00002000
+#define HFI_VIDEO_CODEC_VP9			0x00004000
+#define HFI_VIDEO_CODEC_HEVC_HYBRID		0x80000000
+
+#define HFI_H264_PROFILE_BASELINE		0x00000001
+#define HFI_H264_PROFILE_MAIN			0x00000002
+#define HFI_H264_PROFILE_HIGH			0x00000004
+#define HFI_H264_PROFILE_STEREO_HIGH		0x00000008
+#define HFI_H264_PROFILE_MULTIVIEW_HIGH		0x00000010
+#define HFI_H264_PROFILE_CONSTRAINED_BASE	0x00000020
+#define HFI_H264_PROFILE_CONSTRAINED_HIGH	0x00000040
+
+#define HFI_H264_LEVEL_1			0x00000001
+#define HFI_H264_LEVEL_1b			0x00000002
+#define HFI_H264_LEVEL_11			0x00000004
+#define HFI_H264_LEVEL_12			0x00000008
+#define HFI_H264_LEVEL_13			0x00000010
+#define HFI_H264_LEVEL_2			0x00000020
+#define HFI_H264_LEVEL_21			0x00000040
+#define HFI_H264_LEVEL_22			0x00000080
+#define HFI_H264_LEVEL_3			0x00000100
+#define HFI_H264_LEVEL_31			0x00000200
+#define HFI_H264_LEVEL_32			0x00000400
+#define HFI_H264_LEVEL_4			0x00000800
+#define HFI_H264_LEVEL_41			0x00001000
+#define HFI_H264_LEVEL_42			0x00002000
+#define HFI_H264_LEVEL_5			0x00004000
+#define HFI_H264_LEVEL_51			0x00008000
+#define HFI_H264_LEVEL_52			0x00010000
+
+#define HFI_H263_PROFILE_BASELINE		0x00000001
+
+#define HFI_H263_LEVEL_10			0x00000001
+#define HFI_H263_LEVEL_20			0x00000002
+#define HFI_H263_LEVEL_30			0x00000004
+#define HFI_H263_LEVEL_40			0x00000008
+#define HFI_H263_LEVEL_45			0x00000010
+#define HFI_H263_LEVEL_50			0x00000020
+#define HFI_H263_LEVEL_60			0x00000040
+#define HFI_H263_LEVEL_70			0x00000080
+
+#define HFI_MPEG2_PROFILE_SIMPLE		0x00000001
+#define HFI_MPEG2_PROFILE_MAIN			0x00000002
+#define HFI_MPEG2_PROFILE_422			0x00000004
+#define HFI_MPEG2_PROFILE_SNR			0x00000008
+#define HFI_MPEG2_PROFILE_SPATIAL		0x00000010
+#define HFI_MPEG2_PROFILE_HIGH			0x00000020
+
+#define HFI_MPEG2_LEVEL_LL			0x00000001
+#define HFI_MPEG2_LEVEL_ML			0x00000002
+#define HFI_MPEG2_LEVEL_H14			0x00000004
+#define HFI_MPEG2_LEVEL_HL			0x00000008
+
+#define HFI_MPEG4_PROFILE_SIMPLE		0x00000001
+#define HFI_MPEG4_PROFILE_ADVANCEDSIMPLE	0x00000002
+
+#define HFI_MPEG4_LEVEL_0			0x00000001
+#define HFI_MPEG4_LEVEL_0b			0x00000002
+#define HFI_MPEG4_LEVEL_1			0x00000004
+#define HFI_MPEG4_LEVEL_2			0x00000008
+#define HFI_MPEG4_LEVEL_3			0x00000010
+#define HFI_MPEG4_LEVEL_4			0x00000020
+#define HFI_MPEG4_LEVEL_4a			0x00000040
+#define HFI_MPEG4_LEVEL_5			0x00000080
+#define HFI_MPEG4_LEVEL_6			0x00000100
+#define HFI_MPEG4_LEVEL_7			0x00000200
+#define HFI_MPEG4_LEVEL_8			0x00000400
+#define HFI_MPEG4_LEVEL_9			0x00000800
+#define HFI_MPEG4_LEVEL_3b			0x00001000
+
+#define HFI_VC1_PROFILE_SIMPLE			0x00000001
+#define HFI_VC1_PROFILE_MAIN			0x00000002
+#define HFI_VC1_PROFILE_ADVANCED		0x00000004
+
+#define HFI_VC1_LEVEL_LOW			0x00000001
+#define HFI_VC1_LEVEL_MEDIUM			0x00000002
+#define HFI_VC1_LEVEL_HIGH			0x00000004
+#define HFI_VC1_LEVEL_0				0x00000008
+#define HFI_VC1_LEVEL_1				0x00000010
+#define HFI_VC1_LEVEL_2				0x00000020
+#define HFI_VC1_LEVEL_3				0x00000040
+#define HFI_VC1_LEVEL_4				0x00000080
+
+#define HFI_VPX_PROFILE_SIMPLE			0x00000001
+#define HFI_VPX_PROFILE_ADVANCED		0x00000002
+#define HFI_VPX_PROFILE_VERSION_0		0x00000004
+#define HFI_VPX_PROFILE_VERSION_1		0x00000008
+#define HFI_VPX_PROFILE_VERSION_2		0x00000010
+#define HFI_VPX_PROFILE_VERSION_3		0x00000020
+
+#define HFI_DIVX_FORMAT_4			0x1
+#define HFI_DIVX_FORMAT_5			0x2
+#define HFI_DIVX_FORMAT_6			0x3
+
+#define HFI_DIVX_PROFILE_QMOBILE		0x00000001
+#define HFI_DIVX_PROFILE_MOBILE			0x00000002
+#define HFI_DIVX_PROFILE_MT			0x00000004
+#define HFI_DIVX_PROFILE_HT			0x00000008
+#define HFI_DIVX_PROFILE_HD			0x00000010
+
+#define HFI_HEVC_PROFILE_MAIN			0x00000001
+#define HFI_HEVC_PROFILE_MAIN10			0x00000002
+#define HFI_HEVC_PROFILE_MAIN_STILL_PIC		0x00000004
+
+#define HFI_HEVC_LEVEL_1			0x00000001
+#define HFI_HEVC_LEVEL_2			0x00000002
+#define HFI_HEVC_LEVEL_21			0x00000004
+#define HFI_HEVC_LEVEL_3			0x00000008
+#define HFI_HEVC_LEVEL_31			0x00000010
+#define HFI_HEVC_LEVEL_4			0x00000020
+#define HFI_HEVC_LEVEL_41			0x00000040
+#define HFI_HEVC_LEVEL_5			0x00000080
+#define HFI_HEVC_LEVEL_51			0x00000100
+#define HFI_HEVC_LEVEL_52			0x00000200
+#define HFI_HEVC_LEVEL_6			0x00000400
+#define HFI_HEVC_LEVEL_61			0x00000800
+#define HFI_HEVC_LEVEL_62			0x00001000
+
+#define HFI_HEVC_TIER_MAIN			0x1
+#define HFI_HEVC_TIER_HIGH0			0x2
+
+#define HFI_BUFFER_INPUT			0x1
+#define HFI_BUFFER_OUTPUT			0x2
+#define HFI_BUFFER_OUTPUT2			0x3
+#define HFI_BUFFER_INTERNAL_PERSIST		0x4
+#define HFI_BUFFER_INTERNAL_PERSIST_1		0x5
+#define HFI_BUFFER_INTERNAL_SCRATCH		0x1000001
+#define HFI_BUFFER_EXTRADATA_INPUT		0x1000002
+#define HFI_BUFFER_EXTRADATA_OUTPUT		0x1000003
+#define HFI_BUFFER_EXTRADATA_OUTPUT2		0x1000004
+#define HFI_BUFFER_INTERNAL_SCRATCH_1		0x1000005
+#define HFI_BUFFER_INTERNAL_SCRATCH_2		0x1000006
+
+#define HFI_BUFFER_TYPE_MAX			11
+
+#define HFI_BUFFER_MODE_STATIC			0x1000001
+#define HFI_BUFFER_MODE_RING			0x1000002
+#define HFI_BUFFER_MODE_DYNAMIC			0x1000003
+
+#define HFI_VENC_PERFMODE_MAX_QUALITY		0x1
+#define HFI_VENC_PERFMODE_POWER_SAVE		0x2
+
+/*
+ * HFI_PROPERTY_SYS_COMMON_START
+ * HFI_DOMAIN_BASE_COMMON + HFI_ARCH_COMMON_OFFSET + 0x0000
+ */
+#define HFI_PROPERTY_SYS_DEBUG_CONFIG				0x1
+#define HFI_PROPERTY_SYS_RESOURCE_OCMEM_REQUIREMENT_INFO	0x2
+#define HFI_PROPERTY_SYS_CONFIG_VCODEC_CLKFREQ			0x3
+#define HFI_PROPERTY_SYS_IDLE_INDICATOR				0x4
+#define HFI_PROPERTY_SYS_CODEC_POWER_PLANE_CTRL			0x5
+#define HFI_PROPERTY_SYS_IMAGE_VERSION				0x6
+#define HFI_PROPERTY_SYS_CONFIG_COVERAGE			0x7
+
+/*
+ * HFI_PROPERTY_PARAM_COMMON_START
+ * HFI_DOMAIN_BASE_COMMON + HFI_ARCH_COMMON_OFFSET + 0x1000
+ */
+#define HFI_PROPERTY_PARAM_FRAME_SIZE				0x1001
+#define HFI_PROPERTY_PARAM_UNCOMPRESSED_PLANE_ACTUAL_INFO	0x1002
+#define HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SELECT		0x1003
+#define HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SUPPORTED	0x1004
+#define HFI_PROPERTY_PARAM_PROFILE_LEVEL_CURRENT		0x1005
+#define HFI_PROPERTY_PARAM_PROFILE_LEVEL_SUPPORTED		0x1006
+#define HFI_PROPERTY_PARAM_CAPABILITY_SUPPORTED			0x1007
+#define HFI_PROPERTY_PARAM_PROPERTIES_SUPPORTED			0x1008
+#define HFI_PROPERTY_PARAM_CODEC_SUPPORTED			0x1009
+#define HFI_PROPERTY_PARAM_NAL_STREAM_FORMAT_SUPPORTED		0x100a
+#define HFI_PROPERTY_PARAM_NAL_STREAM_FORMAT_SELECT		0x100b
+#define HFI_PROPERTY_PARAM_MULTI_VIEW_FORMAT			0x100c
+#define HFI_PROPERTY_PARAM_MAX_SEQUENCE_HEADER_SIZE		0x100d
+#define HFI_PROPERTY_PARAM_CODEC_MASK_SUPPORTED			0x100e
+#define HFI_PROPERTY_PARAM_MVC_BUFFER_LAYOUT			0x100f
+#define HFI_PROPERTY_PARAM_MAX_SESSIONS_SUPPORTED		0x1010
+
+/*
+ * HFI_PROPERTY_CONFIG_COMMON_START
+ * HFI_DOMAIN_BASE_COMMON + HFI_ARCH_COMMON_OFFSET + 0x2000
+ */
+#define HFI_PROPERTY_CONFIG_FRAME_RATE				0x2001
+
+/*
+ * HFI_PROPERTY_PARAM_VDEC_COMMON_START
+ * HFI_DOMAIN_BASE_VDEC + HFI_ARCH_COMMON_OFFSET + 0x3000
+ */
+#define HFI_PROPERTY_PARAM_VDEC_MULTI_STREAM			0x1003001
+#define HFI_PROPERTY_PARAM_VDEC_CONCEAL_COLOR			0x1003002
+#define HFI_PROPERTY_PARAM_VDEC_NONCP_OUTPUT2			0x1003003
+
+/*
+ * HFI_PROPERTY_CONFIG_VDEC_COMMON_START
+ * HFI_DOMAIN_BASE_VDEC + HFI_ARCH_COMMON_OFFSET + 0x4000
+ */
+
+/*
+ * HFI_PROPERTY_PARAM_VENC_COMMON_START
+ * HFI_DOMAIN_BASE_VENC + HFI_ARCH_COMMON_OFFSET + 0x5000
+ */
+#define HFI_PROPERTY_PARAM_VENC_SLICE_DELIVERY_MODE		0x2005001
+#define HFI_PROPERTY_PARAM_VENC_H264_ENTROPY_CONTROL		0x2005002
+#define HFI_PROPERTY_PARAM_VENC_H264_DEBLOCK_CONTROL		0x2005003
+#define HFI_PROPERTY_PARAM_VENC_RATE_CONTROL			0x2005004
+#define HFI_PROPERTY_PARAM_VENC_H264_PICORDER_CNT_TYPE		0x2005005
+#define HFI_PROPERTY_PARAM_VENC_SESSION_QP			0x2005006
+#define HFI_PROPERTY_PARAM_VENC_MPEG4_AC_PREDICTION		0x2005007
+#define HFI_PROPERTY_PARAM_VENC_SESSION_QP_RANGE		0x2005008
+#define HFI_PROPERTY_PARAM_VENC_MPEG4_TIME_RESOLUTION		0x2005009
+#define HFI_PROPERTY_PARAM_VENC_MPEG4_SHORT_HEADER		0x200500a
+#define HFI_PROPERTY_PARAM_VENC_MPEG4_HEADER_EXTENSION		0x200500b
+#define HFI_PROPERTY_PARAM_VENC_OPEN_GOP			0x200500c
+#define HFI_PROPERTY_PARAM_VENC_INTRA_REFRESH			0x200500d
+#define HFI_PROPERTY_PARAM_VENC_MULTI_SLICE_CONTROL		0x200500e
+#define HFI_PROPERTY_PARAM_VENC_VBV_HRD_BUF_SIZE		0x200500f
+#define HFI_PROPERTY_PARAM_VENC_QUALITY_VS_SPEED		0x2005010
+#define HFI_PROPERTY_PARAM_VENC_ADVANCED			0x2005012
+#define HFI_PROPERTY_PARAM_VENC_H264_SPS_ID			0x2005014
+#define HFI_PROPERTY_PARAM_VENC_H264_PPS_ID			0x2005015
+#define HFI_PROPERTY_PARAM_VENC_H264_GENERATE_AUDNAL		0x2005016
+#define HFI_PROPERTY_PARAM_VENC_ASPECT_RATIO			0x2005017
+#define HFI_PROPERTY_PARAM_VENC_NUMREF				0x2005018
+#define HFI_PROPERTY_PARAM_VENC_MULTIREF_P			0x2005019
+#define HFI_PROPERTY_PARAM_VENC_H264_NAL_SVC_EXT		0x200501b
+#define HFI_PROPERTY_PARAM_VENC_LTRMODE				0x200501c
+#define HFI_PROPERTY_PARAM_VENC_VIDEO_FULL_RANGE		0x200501d
+#define HFI_PROPERTY_PARAM_VENC_H264_VUI_TIMING_INFO		0x200501e
+#define HFI_PROPERTY_PARAM_VENC_VC1_PERF_CFG			0x200501f
+#define HFI_PROPERTY_PARAM_VENC_MAX_NUM_B_FRAMES		0x2005020
+#define HFI_PROPERTY_PARAM_VENC_H264_VUI_BITSTREAM_RESTRC	0x2005021
+#define HFI_PROPERTY_PARAM_VENC_PRESERVE_TEXT_QUALITY		0x2005023
+#define HFI_PROPERTY_PARAM_VENC_HIER_P_MAX_NUM_ENH_LAYER	0x2005026
+#define HFI_PROPERTY_PARAM_VENC_DISABLE_RC_TIMESTAMP		0x2005027
+#define HFI_PROPERTY_PARAM_VENC_INITIAL_QP			0x2005028
+#define HFI_PROPERTY_PARAM_VENC_VPX_ERROR_RESILIENCE_MODE	0x2005029
+#define HFI_PROPERTY_PARAM_VENC_HIER_B_MAX_NUM_ENH_LAYER	0x200502c
+#define HFI_PROPERTY_PARAM_VENC_HIER_P_HYBRID_MODE		0x200502f
+
+/*
+ * HFI_PROPERTY_CONFIG_VENC_COMMON_START
+ * HFI_DOMAIN_BASE_VENC + HFI_ARCH_COMMON_OFFSET + 0x6000
+ */
+#define HFI_PROPERTY_CONFIG_VENC_TARGET_BITRATE			0x2006001
+#define HFI_PROPERTY_CONFIG_VENC_IDR_PERIOD			0x2006002
+#define HFI_PROPERTY_CONFIG_VENC_INTRA_PERIOD			0x2006003
+#define HFI_PROPERTY_CONFIG_VENC_REQUEST_SYNC_FRAME		0x2006004
+#define HFI_PROPERTY_CONFIG_VENC_SLICE_SIZE			0x2006005
+#define HFI_PROPERTY_CONFIG_VENC_MAX_BITRATE			0x2006007
+#define HFI_PROPERTY_CONFIG_VENC_SYNC_FRAME_SEQUENCE_HEADER	0x2006008
+#define HFI_PROPERTY_CONFIG_VENC_MARKLTRFRAME			0x2006009
+#define HFI_PROPERTY_CONFIG_VENC_USELTRFRAME			0x200600a
+#define HFI_PROPERTY_CONFIG_VENC_HIER_P_ENH_LAYER		0x200600b
+#define HFI_PROPERTY_CONFIG_VENC_LTRPERIOD			0x200600c
+#define HFI_PROPERTY_CONFIG_VENC_PERF_MODE			0x200600e
+
+/*
+ * HFI_PROPERTY_PARAM_VPE_COMMON_START
+ * HFI_DOMAIN_BASE_VPE + HFI_ARCH_COMMON_OFFSET + 0x7000
+ */
+
+/*
+ * HFI_PROPERTY_CONFIG_VPE_COMMON_START
+ * HFI_DOMAIN_BASE_VPE + HFI_ARCH_COMMON_OFFSET + 0x8000
+ */
+#define HFI_PROPERTY_CONFIG_VPE_DEINTERLACE			0x3008001
+#define HFI_PROPERTY_CONFIG_VPE_OPERATIONS			0x3008002
+
+enum hfi_version {
+	HFI_VERSION_1XX,
+	HFI_VERSION_3XX,
+};
+
+struct hfi_buffer_info {
+	u32 buffer_addr;
+	u32 extradata_addr;
+};
+
+struct hfi_bitrate {
+	u32 bitrate;
+	u32 layer_id;
+};
+
+#define HFI_CAPABILITY_FRAME_WIDTH			0x01
+#define HFI_CAPABILITY_FRAME_HEIGHT			0x02
+#define HFI_CAPABILITY_MBS_PER_FRAME			0x03
+#define HFI_CAPABILITY_MBS_PER_SECOND			0x04
+#define HFI_CAPABILITY_FRAMERATE			0x05
+#define HFI_CAPABILITY_SCALE_X				0x06
+#define HFI_CAPABILITY_SCALE_Y				0x07
+#define HFI_CAPABILITY_BITRATE				0x08
+#define HFI_CAPABILITY_BFRAME				0x09
+#define HFI_CAPABILITY_PEAKBITRATE			0x0a
+#define HFI_CAPABILITY_HIER_P_NUM_ENH_LAYERS		0x10
+#define HFI_CAPABILITY_ENC_LTR_COUNT			0x11
+#define HFI_CAPABILITY_CP_OUTPUT2_THRESH		0x12
+#define HFI_CAPABILITY_HIER_B_NUM_ENH_LAYERS		0x13
+#define HFI_CAPABILITY_LCU_SIZE				0x14
+#define HFI_CAPABILITY_HIER_P_HYBRID_NUM_ENH_LAYERS	0x15
+#define HFI_CAPABILITY_MBS_PER_SECOND_POWERSAVE		0x16
+
+struct hfi_capability {
+	u32 capability_type;
+	u32 min;
+	u32 max;
+	u32 step_size;
+};
+
+struct hfi_capabilities {
+	u32 num_capabilities;
+	struct hfi_capability data[1];
+};
+
+#define HFI_DEBUG_MSG_LOW	0x01
+#define HFI_DEBUG_MSG_MEDIUM	0x02
+#define HFI_DEBUG_MSG_HIGH	0x04
+#define HFI_DEBUG_MSG_ERROR	0x08
+#define HFI_DEBUG_MSG_FATAL	0x10
+#define HFI_DEBUG_MSG_PERF	0x20
+
+#define HFI_DEBUG_MODE_QUEUE	0x01
+#define HFI_DEBUG_MODE_QDSS	0x02
+
+struct hfi_debug_config {
+	u32 config;
+	u32 mode;
+};
+
+struct hfi_enable {
+	u32 enable;
+};
+
+#define HFI_H264_DB_MODE_DISABLE		0x1
+#define HFI_H264_DB_MODE_SKIP_SLICE_BOUNDARY	0x2
+#define HFI_H264_DB_MODE_ALL_BOUNDARY		0x3
+
+struct hfi_h264_db_control {
+	u32 mode;
+	u32 slice_alpha_offset;
+	u32 slice_beta_offset;
+};
+
+#define HFI_H264_ENTROPY_CAVLC			0x1
+#define HFI_H264_ENTROPY_CABAC			0x2
+
+#define HFI_H264_CABAC_MODEL_0			0x1
+#define HFI_H264_CABAC_MODEL_1			0x2
+#define HFI_H264_CABAC_MODEL_2			0x3
+
+struct hfi_h264_entropy_control {
+	u32 entropy_mode;
+	u32 cabac_model;
+};
+
+struct hfi_framerate {
+	u32 buffer_type;
+	u32 framerate;
+};
+
+#define HFI_INTRA_REFRESH_NONE			0x1
+#define HFI_INTRA_REFRESH_CYCLIC		0x2
+#define HFI_INTRA_REFRESH_ADAPTIVE		0x3
+#define HFI_INTRA_REFRESH_CYCLIC_ADAPTIVE	0x4
+#define HFI_INTRA_REFRESH_RANDOM		0x5
+
+struct hfi_intra_refresh {
+	u32 mode;
+	u32 air_mbs;
+	u32 air_ref;
+	u32 cir_mbs;
+};
+
+struct hfi_intra_refresh_3x {
+	u32 mode;
+	u32 mbs;
+};
+
+struct hfi_idr_period {
+	u32 idr_period;
+};
+
+struct hfi_operations_type {
+	u32 rotation;
+	u32 flip;
+};
+
+struct hfi_max_num_b_frames {
+	u32 max_num_b_frames;
+};
+
+struct hfi_vc1e_perf_cfg_type {
+	u32 search_range_x_subsampled[3];
+	u32 search_range_y_subsampled[3];
+};
+
+struct hfi_conceal_color {
+	u32 conceal_color;
+};
+
+struct hfi_intra_period {
+	u32 pframes;
+	u32 bframes;
+};
+
+struct hfi_mpeg4_header_extension {
+	u32 header_extension;
+};
+
+struct hfi_mpeg4_time_resolution {
+	u32 time_increment_resolution;
+};
+
+struct hfi_multi_stream {
+	u32 buffer_type;
+	u32 enable;
+	u32 width;
+	u32 height;
+};
+
+struct hfi_multi_stream_3x {
+	u32 buffer_type;
+	u32 enable;
+};
+
+struct hfi_multi_view_format {
+	u32 views;
+	u32 view_order[1];
+};
+
+#define HFI_MULTI_SLICE_OFF			0x1
+#define HFI_MULTI_SLICE_BY_MB_COUNT		0x2
+#define HFI_MULTI_SLICE_BY_BYTE_COUNT		0x3
+#define HFI_MULTI_SLICE_GOB			0x4
+
+struct hfi_multi_slice_control {
+	u32 multi_slice;
+	u32 slice_size;
+};
+
+#define HFI_NAL_FORMAT_STARTCODES		0x01
+#define HFI_NAL_FORMAT_ONE_NAL_PER_BUFFER	0x02
+#define HFI_NAL_FORMAT_ONE_BYTE_LENGTH		0x04
+#define HFI_NAL_FORMAT_TWO_BYTE_LENGTH		0x08
+#define HFI_NAL_FORMAT_FOUR_BYTE_LENGTH		0x10
+
+struct hfi_nal_stream_format {
+	u32 format;
+};
+
+struct hfi_nal_stream_format_select {
+	u32 format;
+};
+
+#define HFI_PICTURE_TYPE_I			0x01
+#define HFI_PICTURE_TYPE_P			0x02
+#define HFI_PICTURE_TYPE_B			0x04
+#define HFI_PICTURE_TYPE_IDR			0x08
+
+struct hfi_profile_level {
+	u32 profile;
+	u32 level;
+};
+
+#define HFI_MAX_PROFILE_COUNT			16
+
+struct hfi_profile_level_supported {
+	u32 profile_count;
+	struct hfi_profile_level profile_level[1];
+};
+
+struct hfi_quality_vs_speed {
+	u32 quality_vs_speed;
+};
+
+struct hfi_quantization {
+	u32 qp_i;
+	u32 qp_p;
+	u32 qp_b;
+	u32 layer_id;
+};
+
+struct hfi_initial_quantization {
+	u32 qp_i;
+	u32 qp_p;
+	u32 qp_b;
+	u32 init_qp_enable;
+};
+
+struct hfi_quantization_range {
+	u32 min_qp;
+	u32 max_qp;
+	u32 layer_id;
+};
+
+#define HFI_LTR_MODE_DISABLE	0x0
+#define HFI_LTR_MODE_MANUAL	0x1
+#define HFI_LTR_MODE_PERIODIC	0x2
+
+struct hfi_ltr_mode {
+	u32 ltr_mode;
+	u32 ltr_count;
+	u32 trust_mode;
+};
+
+struct hfi_ltr_use {
+	u32 ref_ltr;
+	u32 use_constrnt;
+	u32 frames;
+};
+
+struct hfi_ltr_mark {
+	u32 mark_frame;
+};
+
+struct hfi_framesize {
+	u32 buffer_type;
+	u32 width;
+	u32 height;
+};
+
+struct hfi_h264_vui_timing_info {
+	u32 enable;
+	u32 fixed_framerate;
+	u32 time_scale;
+};
+
+#define HFI_COLOR_FORMAT_MONOCHROME		0x01
+#define HFI_COLOR_FORMAT_NV12			0x02
+#define HFI_COLOR_FORMAT_NV21			0x03
+#define HFI_COLOR_FORMAT_NV12_4x4TILE		0x04
+#define HFI_COLOR_FORMAT_NV21_4x4TILE		0x05
+#define HFI_COLOR_FORMAT_YUYV			0x06
+#define HFI_COLOR_FORMAT_YVYU			0x07
+#define HFI_COLOR_FORMAT_UYVY			0x08
+#define HFI_COLOR_FORMAT_VYUY			0x09
+#define HFI_COLOR_FORMAT_RGB565			0x0a
+#define HFI_COLOR_FORMAT_BGR565			0x0b
+#define HFI_COLOR_FORMAT_RGB888			0x0c
+#define HFI_COLOR_FORMAT_BGR888			0x0d
+#define HFI_COLOR_FORMAT_YUV444			0x0e
+#define HFI_COLOR_FORMAT_RGBA8888		0x10
+
+#define HFI_COLOR_FORMAT_UBWC_BASE		0x8000
+#define HFI_COLOR_FORMAT_10_BIT_BASE		0x4000
+
+#define HFI_COLOR_FORMAT_YUV420_TP10		0x4002
+#define HFI_COLOR_FORMAT_NV12_UBWC		0x8002
+#define HFI_COLOR_FORMAT_YUV420_TP10_UBWC	0xc002
+#define HFI_COLOR_FORMAT_RGBA8888_UBWC		0x8010
+
+struct hfi_uncompressed_format_select {
+	u32 buffer_type;
+	u32 format;
+};
+
+struct hfi_uncompressed_format_supported {
+	u32 buffer_type;
+	u32 format_entries;
+	u32 format_info[1];
+};
+
+struct hfi_uncompressed_plane_actual {
+	int actual_stride;
+	u32 actual_plane_buffer_height;
+};
+
+struct hfi_uncompressed_plane_actual_info {
+	u32 buffer_type;
+	u32 num_planes;
+	struct hfi_uncompressed_plane_actual plane_format[1];
+};
+
+struct hfi_uncompressed_plane_constraints {
+	u32 stride_multiples;
+	u32 max_stride;
+	u32 min_plane_buffer_height_multiple;
+	u32 buffer_alignment;
+};
+
+struct hfi_uncompressed_plane_info {
+	u32 format;
+	u32 num_planes;
+	struct hfi_uncompressed_plane_constraints plane_format[1];
+};
+
+struct hfi_uncompressed_plane_actual_constraints_info {
+	u32 buffer_type;
+	u32 num_planes;
+	struct hfi_uncompressed_plane_constraints plane_format[1];
+};
+
+struct hfi_codec_supported {
+	u32 dec_codecs;
+	u32 enc_codecs;
+};
+
+struct hfi_properties_supported {
+	u32 num_properties;
+	u32 properties[1];
+};
+
+struct hfi_max_sessions_supported {
+	u32 max_sessions;
+};
+
+#define HFI_MAX_MATRIX_COEFFS	9
+#define HFI_MAX_BIAS_COEFFS	3
+#define HFI_MAX_LIMIT_COEFFS	6
+
+struct hfi_vpe_color_space_conversion {
+	u32 csc_matrix[HFI_MAX_MATRIX_COEFFS];
+	u32 csc_bias[HFI_MAX_BIAS_COEFFS];
+	u32 csc_limit[HFI_MAX_LIMIT_COEFFS];
+};
+
+#define HFI_ROTATE_NONE		0x1
+#define HFI_ROTATE_90		0x2
+#define HFI_ROTATE_180		0x3
+#define HFI_ROTATE_270		0x4
+
+#define HFI_FLIP_NONE		0x1
+#define HFI_FLIP_HORIZONTAL	0x2
+#define HFI_FLIP_VERTICAL	0x3
+
+struct hfi_operations {
+	u32 rotate;
+	u32 flip;
+};
+
+#define HFI_RESOURCE_OCMEM	0x1
+
+struct hfi_resource_ocmem {
+	u32 size;
+	u32 mem;
+};
+
+struct hfi_resource_ocmem_requirement {
+	u32 session_domain;
+	u32 width;
+	u32 height;
+	u32 size;
+};
+
+struct hfi_resource_ocmem_requirement_info {
+	u32 num_entries;
+	struct hfi_resource_ocmem_requirement requirements[1];
+};
+
+struct hfi_property_sys_image_version_info_type {
+	u32 string_size;
+	u8  str_image_version[1];
+};
+
+struct hfi_codec_mask_supported {
+	u32 codecs;
+	u32 video_domains;
+};
+
+struct hfi_seq_header_info {
+	u32 max_hader_len;
+};
+
+struct hfi_aspect_ratio {
+	u32 aspect_width;
+	u32 aspect_height;
+};
+
+#define HFI_MVC_BUFFER_LAYOUT_TOP_BOTTOM	0
+#define HFI_MVC_BUFFER_LAYOUT_SIDEBYSIDE	1
+#define HFI_MVC_BUFFER_LAYOUT_SEQ		2
+
+struct hfi_mvc_buffer_layout_descp_type {
+	u32 layout_type;
+	u32 bright_view_first;
+	u32 ngap;
+};
+
+struct hfi_scs_threshold {
+	u32 threshold_value;
+};
+
+#define HFI_TEST_SSR_SW_ERR_FATAL	0x1
+#define HFI_TEST_SSR_SW_DIV_BY_ZERO	0x2
+#define HFI_TEST_SSR_HW_WDOG_IRQ	0x3
+
+struct hfi_buffer_alloc_mode {
+	u32 type;
+	u32 mode;
+};
+
+struct hfi_index_extradata_config {
+	u32 enable;
+	u32 index_extra_data_id;
+};
+
+struct hfi_extradata_header {
+	u32 size;
+	u32 version;
+	u32 port_index;
+	u32 type;
+	u32 data_size;
+	u8 data[1];
+};
+
+struct hfi_batch_info {
+	u32 input_batch_count;
+	u32 output_batch_count;
+};
+
+struct hfi_buffer_count_actual {
+	u32 type;
+	u32 count_actual;
+};
+
+struct hfi_buffer_size_actual {
+	u32 type;
+	u32 size;
+};
+
+struct hfi_buffer_display_hold_count_actual {
+	u32 type;
+	u32 hold_count;
+};
+
+struct hfi_buffer_requirements {
+	u32 type;
+	u32 size;
+	u32 region_size;
+	u32 hold_count;
+	u32 count_min;
+	u32 count_actual;
+	u32 contiguous;
+	u32 alignment;
+};
+
+struct hfi_data_payload {
+	u32 size;
+	u8 data[1];
+};
+
+struct hfi_enable_picture {
+	u32 picture_type;
+};
+
+struct hfi_display_picture_buffer_count {
+	int enable;
+	u32 count;
+};
+
+struct hfi_extra_data_header_config {
+	u32 type;
+	u32 buffer_type;
+	u32 version;
+	u32 port_index;
+	u32 client_extra_data_id;
+};
+
+struct hfi_interlace_format_supported {
+	u32 buffer_type;
+	u32 format;
+};
+
+struct hfi_buffer_alloc_mode_supported {
+	u32 buffer_type;
+	u32 num_entries;
+	u32 data[1];
+};
+
+struct hfi_mb_error_map {
+	u32 error_map_size;
+	u8 error_map[1];
+};
+
+struct hfi_metadata_pass_through {
+	int enable;
+	u32 size;
+};
+
+struct hfi_multi_view_select {
+	u32 view_index;
+};
+
+struct hfi_hybrid_hierp {
+	u32 layers;
+};
+
+struct hfi_pkt_hdr {
+	u32 size;
+	u32 pkt_type;
+};
+
+struct hfi_session_hdr_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 session_id;
+};
+
+struct hfi_session_pkt {
+	struct hfi_session_hdr_pkt shdr;
+};
+
+#endif
diff --git a/drivers/media/platform/qcom/venus/hfi_msgs.c b/drivers/media/platform/qcom/venus/hfi_msgs.c
new file mode 100644
index 000000000000..f8841713e417
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/hfi_msgs.c
@@ -0,0 +1,1052 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/hash.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <media/videobuf2-v4l2.h>
+
+#include "core.h"
+#include "hfi.h"
+#include "hfi_helper.h"
+#include "hfi_msgs.h"
+
+static void event_seq_changed(struct venus_core *core, struct venus_inst *inst,
+			      struct hfi_msg_event_notify_pkt *pkt)
+{
+	struct hfi_event_data event = {0};
+	int num_properties_changed;
+	struct hfi_framesize *frame_sz;
+	struct hfi_profile_level *profile_level;
+	u8 *data_ptr;
+	u32 ptype;
+
+	inst->error = HFI_ERR_NONE;
+
+	switch (pkt->event_data1) {
+	case HFI_EVENT_DATA_SEQUENCE_CHANGED_SUFFICIENT_BUF_RESOURCES:
+	case HFI_EVENT_DATA_SEQUENCE_CHANGED_INSUFFICIENT_BUF_RESOURCES:
+		break;
+	default:
+		inst->error = HFI_ERR_SESSION_INVALID_PARAMETER;
+		goto done;
+	}
+
+	event.event_type = pkt->event_data1;
+
+	num_properties_changed = pkt->event_data2;
+	if (!num_properties_changed) {
+		inst->error = HFI_ERR_SESSION_INSUFFICIENT_RESOURCES;
+		goto done;
+	}
+
+	data_ptr = (u8 *)&pkt->ext_event_data[0];
+	do {
+		ptype = *((u32 *)data_ptr);
+		switch (ptype) {
+		case HFI_PROPERTY_PARAM_FRAME_SIZE:
+			data_ptr += sizeof(u32);
+			frame_sz = (struct hfi_framesize *)data_ptr;
+			event.width = frame_sz->width;
+			event.height = frame_sz->height;
+			data_ptr += sizeof(frame_sz);
+			break;
+		case HFI_PROPERTY_PARAM_PROFILE_LEVEL_CURRENT:
+			data_ptr += sizeof(u32);
+			profile_level = (struct hfi_profile_level *)data_ptr;
+			event.profile = profile_level->profile;
+			event.level = profile_level->level;
+			data_ptr += sizeof(profile_level);
+			break;
+		default:
+			break;
+		}
+		num_properties_changed--;
+	} while (num_properties_changed > 0);
+
+done:
+	inst->ops->event_notify(inst, EVT_SYS_EVENT_CHANGE, &event);
+}
+
+static void event_release_buffer_ref(struct venus_core *core,
+				     struct venus_inst *inst,
+				     struct hfi_msg_event_notify_pkt *pkt)
+{
+	struct hfi_event_data event = {0};
+	struct hfi_msg_event_release_buffer_ref_pkt *data;
+
+	data = (struct hfi_msg_event_release_buffer_ref_pkt *)
+		pkt->ext_event_data;
+
+	event.event_type = HFI_EVENT_RELEASE_BUFFER_REFERENCE;
+	event.packet_buffer = data->packet_buffer;
+	event.extradata_buffer = data->extradata_buffer;
+	event.tag = data->output_tag;
+
+	inst->error = HFI_ERR_NONE;
+	inst->ops->event_notify(inst, EVT_SYS_EVENT_CHANGE, &event);
+}
+
+static void event_sys_error(struct venus_core *core, u32 event,
+			    struct hfi_msg_event_notify_pkt *pkt)
+{
+	if (pkt)
+		dev_dbg(core->dev,
+			"sys error (session id:%x, data1:%x, data2:%x)\n",
+			pkt->shdr.session_id, pkt->event_data1,
+			pkt->event_data2);
+
+	core->core_ops->event_notify(core, event);
+}
+
+static void
+event_session_error(struct venus_core *core, struct venus_inst *inst,
+		    struct hfi_msg_event_notify_pkt *pkt)
+{
+	struct device *dev = core->dev;
+
+	dev_dbg(dev, "session error: event id:%x, session id:%x\n",
+		pkt->event_data1, pkt->shdr.session_id);
+
+	if (!inst)
+		return;
+
+	switch (pkt->event_data1) {
+	/* non fatal session errors */
+	case HFI_ERR_SESSION_INVALID_SCALE_FACTOR:
+	case HFI_ERR_SESSION_UNSUPPORT_BUFFERTYPE:
+	case HFI_ERR_SESSION_UNSUPPORTED_SETTING:
+	case HFI_ERR_SESSION_UPSCALE_NOT_SUPPORTED:
+		inst->error = HFI_ERR_NONE;
+		break;
+	default:
+		dev_err(dev, "session error: event id:%x (%x), session id:%x\n",
+			pkt->event_data1, pkt->event_data2,
+			pkt->shdr.session_id);
+
+		inst->error = pkt->event_data1;
+		inst->ops->event_notify(inst, EVT_SESSION_ERROR, NULL);
+		break;
+	}
+}
+
+static void hfi_event_notify(struct venus_core *core, struct venus_inst *inst,
+			     void *packet)
+{
+	struct hfi_msg_event_notify_pkt *pkt = packet;
+
+	if (!packet)
+		return;
+
+	switch (pkt->event_id) {
+	case HFI_EVENT_SYS_ERROR:
+		event_sys_error(core, EVT_SYS_ERROR, pkt);
+		break;
+	case HFI_EVENT_SESSION_ERROR:
+		event_session_error(core, inst, pkt);
+		break;
+	case HFI_EVENT_SESSION_SEQUENCE_CHANGED:
+		event_seq_changed(core, inst, pkt);
+		break;
+	case HFI_EVENT_RELEASE_BUFFER_REFERENCE:
+		event_release_buffer_ref(core, inst, pkt);
+		break;
+	case HFI_EVENT_SESSION_PROPERTY_CHANGED:
+		break;
+	default:
+		break;
+	}
+}
+
+static void hfi_sys_init_done(struct venus_core *core, struct venus_inst *inst,
+			      void *packet)
+{
+	struct hfi_msg_sys_init_done_pkt *pkt = packet;
+	u32 rem_bytes, read_bytes = 0, num_properties;
+	u32 error, ptype;
+	u8 *data;
+
+	error = pkt->error_type;
+	if (error != HFI_ERR_NONE)
+		goto err_no_prop;
+
+	num_properties = pkt->num_properties;
+
+	if (!num_properties) {
+		error = HFI_ERR_SYS_INVALID_PARAMETER;
+		goto err_no_prop;
+	}
+
+	rem_bytes = pkt->hdr.size - sizeof(*pkt) + sizeof(u32);
+
+	if (!rem_bytes) {
+		/* missing property data */
+		error = HFI_ERR_SYS_INSUFFICIENT_RESOURCES;
+		goto err_no_prop;
+	}
+
+	data = (u8 *)&pkt->data[0];
+
+	if (core->res->hfi_version == HFI_VERSION_3XX)
+		goto err_no_prop;
+
+	while (num_properties && rem_bytes >= sizeof(u32)) {
+		ptype = *((u32 *)data);
+		data += sizeof(u32);
+
+		switch (ptype) {
+		case HFI_PROPERTY_PARAM_CODEC_SUPPORTED: {
+			struct hfi_codec_supported *prop;
+
+			prop = (struct hfi_codec_supported *)data;
+
+			if (rem_bytes < sizeof(*prop)) {
+				error = HFI_ERR_SYS_INSUFFICIENT_RESOURCES;
+				break;
+			}
+
+			read_bytes += sizeof(*prop) + sizeof(u32);
+			core->dec_codecs = prop->dec_codecs;
+			core->enc_codecs = prop->enc_codecs;
+			break;
+		}
+		case HFI_PROPERTY_PARAM_MAX_SESSIONS_SUPPORTED: {
+			struct hfi_max_sessions_supported *prop;
+
+			if (rem_bytes < sizeof(*prop)) {
+				error = HFI_ERR_SYS_INSUFFICIENT_RESOURCES;
+				break;
+			}
+
+			prop = (struct hfi_max_sessions_supported *)data;
+			read_bytes += sizeof(*prop) + sizeof(u32);
+			core->max_sessions_supported = prop->max_sessions;
+			break;
+		}
+		default:
+			error = HFI_ERR_SYS_INVALID_PARAMETER;
+			break;
+		}
+
+		if (!error) {
+			rem_bytes -= read_bytes;
+			data += read_bytes;
+			num_properties--;
+		}
+	}
+
+err_no_prop:
+	core->error = error;
+	complete(&core->done);
+}
+
+static void
+sys_get_prop_image_version(struct device *dev,
+			   struct hfi_msg_sys_property_info_pkt *pkt)
+{
+	int req_bytes;
+
+	req_bytes = pkt->hdr.size - sizeof(*pkt);
+
+	if (req_bytes < 128 || !pkt->data[1] || pkt->num_properties > 1)
+		/* bad packet */
+		return;
+
+	dev_dbg(dev, "F/W version: %s\n", (u8 *)&pkt->data[1]);
+}
+
+static void hfi_sys_property_info(struct venus_core *core,
+				  struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_sys_property_info_pkt *pkt = packet;
+	struct device *dev = core->dev;
+
+	if (!pkt->num_properties) {
+		dev_dbg(dev, "%s: no properties\n", __func__);
+		return;
+	}
+
+	switch (pkt->data[0]) {
+	case HFI_PROPERTY_SYS_IMAGE_VERSION:
+		sys_get_prop_image_version(dev, pkt);
+		break;
+	default:
+		dev_dbg(dev, "%s: unknown property data\n", __func__);
+		break;
+	}
+}
+
+static void hfi_sys_rel_resource_done(struct venus_core *core,
+				      struct venus_inst *inst,
+				      void *packet)
+{
+	struct hfi_msg_sys_release_resource_done_pkt *pkt = packet;
+
+	core->error = pkt->error_type;
+	complete(&core->done);
+}
+
+static void hfi_sys_ping_done(struct venus_core *core, struct venus_inst *inst,
+			      void *packet)
+{
+	struct hfi_msg_sys_ping_ack_pkt *pkt = packet;
+
+	core->error = HFI_ERR_NONE;
+
+	if (pkt->client_data != 0xbeef)
+		core->error = HFI_ERR_SYS_FATAL;
+
+	complete(&core->done);
+}
+
+static void hfi_sys_idle_done(struct venus_core *core, struct venus_inst *inst,
+			      void *packet)
+{
+	dev_dbg(core->dev, "sys idle\n");
+}
+
+static void hfi_sys_pc_prepare_done(struct venus_core *core,
+				    struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_sys_pc_prep_done_pkt *pkt = packet;
+
+	dev_dbg(core->dev, "pc prepare done (error %x)\n", pkt->error_type);
+}
+
+static void
+hfi_copy_cap_prop(struct hfi_capability *in, struct venus_inst *inst)
+{
+	if (!in || !inst)
+		return;
+
+	switch (in->capability_type) {
+	case HFI_CAPABILITY_FRAME_WIDTH:
+		inst->cap_width = *in;
+		break;
+	case HFI_CAPABILITY_FRAME_HEIGHT:
+		inst->cap_height = *in;
+		break;
+	case HFI_CAPABILITY_MBS_PER_FRAME:
+		inst->cap_mbs_per_frame = *in;
+		break;
+	case HFI_CAPABILITY_MBS_PER_SECOND:
+		inst->cap_mbs_per_sec = *in;
+		break;
+	case HFI_CAPABILITY_FRAMERATE:
+		inst->cap_framerate = *in;
+		break;
+	case HFI_CAPABILITY_SCALE_X:
+		inst->cap_scale_x = *in;
+		break;
+	case HFI_CAPABILITY_SCALE_Y:
+		inst->cap_scale_y = *in;
+		break;
+	case HFI_CAPABILITY_BITRATE:
+		inst->cap_bitrate = *in;
+		break;
+	case HFI_CAPABILITY_HIER_P_NUM_ENH_LAYERS:
+		inst->cap_hier_p = *in;
+		break;
+	case HFI_CAPABILITY_ENC_LTR_COUNT:
+		inst->cap_ltr_count = *in;
+		break;
+	case HFI_CAPABILITY_CP_OUTPUT2_THRESH:
+		inst->cap_secure_output2_threshold = *in;
+		break;
+	default:
+		break;
+	}
+}
+
+static unsigned int
+session_get_prop_profile_level(struct hfi_msg_session_property_info_pkt *pkt,
+			       struct hfi_profile_level *profile_level)
+{
+	struct hfi_profile_level *hfi;
+	u32 req_bytes;
+
+	req_bytes = pkt->shdr.hdr.size - sizeof(*pkt);
+
+	if (!req_bytes || req_bytes % sizeof(struct hfi_profile_level))
+		/* bad packet */
+		return HFI_ERR_SESSION_INVALID_PARAMETER;
+
+	hfi = (struct hfi_profile_level *)&pkt->data[1];
+	profile_level->profile = hfi->profile;
+	profile_level->level = hfi->level;
+
+	return HFI_ERR_NONE;
+}
+
+static unsigned int
+session_get_prop_buf_req(struct hfi_msg_session_property_info_pkt *pkt,
+			 struct hfi_buffer_requirements *bufreq)
+{
+	struct hfi_buffer_requirements *buf_req;
+	u32 req_bytes;
+	unsigned int idx = 0;
+
+	req_bytes = pkt->shdr.hdr.size - sizeof(*pkt);
+
+	if (!req_bytes || req_bytes % sizeof(*buf_req) || !pkt->data[1])
+		/* bad packet */
+		return HFI_ERR_SESSION_INVALID_PARAMETER;
+
+	buf_req = (struct hfi_buffer_requirements *)&pkt->data[1];
+	if (!buf_req)
+		return HFI_ERR_SESSION_INVALID_PARAMETER;
+
+	while (req_bytes) {
+		memcpy(&bufreq[idx], buf_req, sizeof(*bufreq));
+		idx++;
+
+		if (idx > HFI_BUFFER_TYPE_MAX)
+			return HFI_ERR_SESSION_INVALID_PARAMETER;
+
+		req_bytes -= sizeof(struct hfi_buffer_requirements);
+		buf_req++;
+	}
+
+	return HFI_ERR_NONE;
+}
+
+static void hfi_session_prop_info(struct venus_core *core,
+				  struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_session_property_info_pkt *pkt = packet;
+	struct device *dev = core->dev;
+	union hfi_get_property *hprop = &inst->hprop;
+	unsigned int error = HFI_ERR_NONE;
+
+	if (!pkt->num_properties) {
+		error = HFI_ERR_SESSION_INVALID_PARAMETER;
+		dev_err(dev, "%s: no properties\n", __func__);
+		goto done;
+	}
+
+	switch (pkt->data[0]) {
+	case HFI_PROPERTY_CONFIG_BUFFER_REQUIREMENTS:
+		memset(hprop->bufreq, 0, sizeof(hprop->bufreq));
+		error = session_get_prop_buf_req(pkt, hprop->bufreq);
+		break;
+	case HFI_PROPERTY_PARAM_PROFILE_LEVEL_CURRENT:
+		memset(&hprop->profile_level, 0, sizeof(hprop->profile_level));
+		error = session_get_prop_profile_level(pkt,
+						       &hprop->profile_level);
+		break;
+	case HFI_PROPERTY_CONFIG_VDEC_ENTROPY:
+		break;
+	default:
+		dev_dbg(dev, "%s: unknown property id:%x\n", __func__,
+			pkt->data[0]);
+		return;
+	}
+
+done:
+	inst->error = error;
+	complete(&inst->done);
+}
+
+static u32 init_done_read_prop(struct venus_core *core, struct venus_inst *inst,
+			       struct hfi_msg_session_init_done_pkt *pkt)
+{
+	struct device *dev = core->dev;
+	u32 rem_bytes, num_props;
+	u32 ptype, next_offset = 0;
+	u32 err;
+	u8 *data;
+
+	rem_bytes = pkt->shdr.hdr.size - sizeof(*pkt) + sizeof(u32);
+	if (!rem_bytes) {
+		dev_err(dev, "%s: missing property info\n", __func__);
+		return HFI_ERR_SESSION_INSUFFICIENT_RESOURCES;
+	}
+
+	err = pkt->error_type;
+	if (err)
+		return err;
+
+	data = (u8 *)&pkt->data[0];
+	num_props = pkt->num_properties;
+
+	while (err == HFI_ERR_NONE && num_props && rem_bytes >= sizeof(u32)) {
+		ptype = *((u32 *)data);
+		next_offset = sizeof(u32);
+
+		switch (ptype) {
+		case HFI_PROPERTY_PARAM_CODEC_MASK_SUPPORTED: {
+			struct hfi_codec_mask_supported *masks =
+				(struct hfi_codec_mask_supported *)
+				(data + next_offset);
+
+			next_offset += sizeof(*masks);
+			num_props--;
+			break;
+		}
+		case HFI_PROPERTY_PARAM_CAPABILITY_SUPPORTED: {
+			struct hfi_capabilities *caps;
+			struct hfi_capability *cap;
+			u32 num_caps;
+
+			if ((rem_bytes - next_offset) < sizeof(*cap)) {
+				err = HFI_ERR_SESSION_INVALID_PARAMETER;
+				break;
+			}
+
+			caps = (struct hfi_capabilities *)(data + next_offset);
+
+			num_caps = caps->num_capabilities;
+			cap = &caps->data[0];
+			next_offset += sizeof(u32);
+
+			while (num_caps &&
+			       (rem_bytes - next_offset) >= sizeof(u32)) {
+				hfi_copy_cap_prop(cap, inst);
+				cap++;
+				next_offset += sizeof(*cap);
+				num_caps--;
+			}
+			num_props--;
+			break;
+		}
+		case HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SUPPORTED: {
+			struct hfi_uncompressed_format_supported *prop =
+				(struct hfi_uncompressed_format_supported *)
+				(data + next_offset);
+			u32 num_fmt_entries;
+			u8 *fmt;
+			struct hfi_uncompressed_plane_info *inf;
+
+			if ((rem_bytes - next_offset) < sizeof(*prop)) {
+				err = HFI_ERR_SESSION_INVALID_PARAMETER;
+				break;
+			}
+
+			num_fmt_entries = prop->format_entries;
+			next_offset = sizeof(*prop) - sizeof(u32);
+			fmt = (u8 *)&prop->format_info[0];
+
+			dev_dbg(dev, "uncomm format support num entries:%u\n",
+				num_fmt_entries);
+
+			while (num_fmt_entries) {
+				struct hfi_uncompressed_plane_constraints *cnts;
+				u32 bytes_to_skip;
+
+				inf = (struct hfi_uncompressed_plane_info *)fmt;
+
+				if ((rem_bytes - next_offset) < sizeof(*inf)) {
+					err = HFI_ERR_SESSION_INVALID_PARAMETER;
+					break;
+				}
+
+				dev_dbg(dev, "plane info: fmt:%x, planes:%x\n",
+					inf->format, inf->num_planes);
+
+				cnts = &inf->plane_format[0];
+				dev_dbg(dev, "%u %u %u %u\n",
+					cnts->stride_multiples,
+					cnts->max_stride,
+					cnts->min_plane_buffer_height_multiple,
+					cnts->buffer_alignment);
+
+				bytes_to_skip = sizeof(*inf) - sizeof(*cnts) +
+						inf->num_planes * sizeof(*cnts);
+
+				fmt += bytes_to_skip;
+				next_offset += bytes_to_skip;
+				num_fmt_entries--;
+			}
+			num_props--;
+			break;
+		}
+		case HFI_PROPERTY_PARAM_PROPERTIES_SUPPORTED: {
+			struct hfi_properties_supported *prop =
+				(struct hfi_properties_supported *)
+				(data + next_offset);
+
+			next_offset += sizeof(*prop) - sizeof(u32)
+					+ prop->num_properties * sizeof(u32);
+			num_props--;
+			break;
+		}
+		case HFI_PROPERTY_PARAM_PROFILE_LEVEL_SUPPORTED: {
+			struct hfi_profile_level_supported *prop =
+				(struct hfi_profile_level_supported *)
+				(data + next_offset);
+			struct hfi_profile_level *pl;
+			unsigned int prop_count = 0;
+			unsigned int count = 0;
+			u8 *ptr;
+
+			ptr = (u8 *)&prop->profile_level[0];
+			prop_count = prop->profile_count;
+
+			if (prop_count > HFI_MAX_PROFILE_COUNT)
+				prop_count = HFI_MAX_PROFILE_COUNT;
+
+			while (prop_count) {
+				ptr++;
+				pl = (struct hfi_profile_level *)ptr;
+
+				inst->pl[count].profile = pl->profile;
+				inst->pl[count].level = pl->level;
+				prop_count--;
+				count++;
+				ptr += sizeof(*pl) / sizeof(u32);
+			}
+
+			inst->pl_count = count;
+			next_offset += sizeof(*prop) - sizeof(*pl) +
+				       prop->profile_count * sizeof(*pl);
+
+			num_props--;
+			break;
+		}
+		case HFI_PROPERTY_PARAM_INTERLACE_FORMAT_SUPPORTED: {
+			next_offset +=
+				sizeof(struct hfi_interlace_format_supported);
+			num_props--;
+			break;
+		}
+		case HFI_PROPERTY_PARAM_NAL_STREAM_FORMAT_SUPPORTED: {
+			struct hfi_nal_stream_format *nal =
+				(struct hfi_nal_stream_format *)
+				(data + next_offset);
+			dev_dbg(dev, "NAL format: %x\n", nal->format);
+			next_offset += sizeof(*nal);
+			num_props--;
+			break;
+		}
+		case HFI_PROPERTY_PARAM_NAL_STREAM_FORMAT_SELECT: {
+			next_offset += sizeof(u32);
+			num_props--;
+			break;
+		}
+		case HFI_PROPERTY_PARAM_MAX_SEQUENCE_HEADER_SIZE: {
+			u32 *max_seq_sz = (u32 *)(data + next_offset);
+
+			dev_dbg(dev, "max seq header sz: %x\n", *max_seq_sz);
+			next_offset += sizeof(u32);
+			num_props--;
+			break;
+		}
+		case HFI_PROPERTY_PARAM_VENC_INTRA_REFRESH: {
+			next_offset += sizeof(struct hfi_intra_refresh);
+			num_props--;
+			break;
+		}
+		case HFI_PROPERTY_PARAM_BUFFER_ALLOC_MODE_SUPPORTED: {
+			struct hfi_buffer_alloc_mode_supported *prop =
+				(struct hfi_buffer_alloc_mode_supported *)
+				(data + next_offset);
+			unsigned int i;
+
+			for (i = 0; i < prop->num_entries; i++) {
+				if (prop->buffer_type == HFI_BUFFER_OUTPUT ||
+				    prop->buffer_type == HFI_BUFFER_OUTPUT2) {
+					switch (prop->data[i]) {
+					case HFI_BUFFER_MODE_STATIC:
+						inst->cap_bufs_mode_static = 1;
+						break;
+					case HFI_BUFFER_MODE_DYNAMIC:
+						inst->cap_bufs_mode_dynamic = 1;
+						break;
+					default:
+						break;
+					}
+				}
+			}
+			next_offset += sizeof(*prop) -
+				sizeof(u32) + prop->num_entries * sizeof(u32);
+			num_props--;
+			break;
+		}
+		default:
+			dev_dbg(dev, "%s: default case %#x\n", __func__, ptype);
+			break;
+		}
+
+		rem_bytes -= next_offset;
+		data += next_offset;
+	}
+
+	return err;
+}
+
+static void hfi_session_init_done(struct venus_core *core,
+				  struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_session_init_done_pkt *pkt = packet;
+	unsigned int error;
+
+	error = pkt->error_type;
+	if (error != HFI_ERR_NONE)
+		goto done;
+
+	if (core->res->hfi_version != HFI_VERSION_1XX)
+		goto done;
+
+	error = init_done_read_prop(core, inst, pkt);
+
+done:
+	inst->error = error;
+	complete(&inst->done);
+}
+
+static void hfi_session_load_res_done(struct venus_core *core,
+				      struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_session_load_resources_done_pkt *pkt = packet;
+
+	inst->error = pkt->error_type;
+	complete(&inst->done);
+}
+
+static void hfi_session_flush_done(struct venus_core *core,
+				   struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_session_flush_done_pkt *pkt = packet;
+
+	inst->error = pkt->error_type;
+	complete(&inst->done);
+}
+
+static void hfi_session_etb_done(struct venus_core *core,
+				 struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_session_empty_buffer_done_pkt *pkt = packet;
+
+	inst->error = pkt->error_type;
+	inst->ops->buf_done(inst, HFI_BUFFER_INPUT, pkt->input_tag,
+			    pkt->filled_len, pkt->offset, 0, 0, 0);
+}
+
+static void hfi_session_ftb_done(struct venus_core *core,
+				 struct venus_inst *inst, void *packet)
+{
+	u32 session_type = inst->session_type;
+	u64 timestamp_us = 0;
+	u32 timestamp_hi = 0, timestamp_lo = 0;
+	unsigned int error;
+	u32 flags = 0, hfi_flags = 0, offset = 0, filled_len = 0;
+	u32 pic_type = 0, buffer_type = 0, output_tag = -1;
+
+	if (session_type == VIDC_SESSION_TYPE_ENC) {
+		struct hfi_msg_session_fbd_compressed_pkt *pkt = packet;
+
+		timestamp_hi = pkt->time_stamp_hi;
+		timestamp_lo = pkt->time_stamp_lo;
+		hfi_flags = pkt->flags;
+		offset = pkt->offset;
+		filled_len = pkt->filled_len;
+		pic_type = pkt->picture_type;
+		output_tag = pkt->output_tag;
+		buffer_type = HFI_BUFFER_OUTPUT;
+
+		error = pkt->error_type;
+	} else if (session_type == VIDC_SESSION_TYPE_DEC) {
+		struct hfi_msg_session_fbd_uncompressed_plane0_pkt *pkt =
+			packet;
+
+		timestamp_hi = pkt->time_stamp_hi;
+		timestamp_lo = pkt->time_stamp_lo;
+		hfi_flags = pkt->flags;
+		offset = pkt->offset;
+		filled_len = pkt->filled_len;
+		pic_type = pkt->picture_type;
+		output_tag = pkt->output_tag;
+
+		if (pkt->stream_id == 0)
+			buffer_type = HFI_BUFFER_OUTPUT;
+		else if (pkt->stream_id == 1)
+			buffer_type = HFI_BUFFER_OUTPUT2;
+
+		error = pkt->error_type;
+	} else {
+		error = HFI_ERR_SESSION_INVALID_PARAMETER;
+	}
+
+	if (buffer_type != HFI_BUFFER_OUTPUT)
+		goto done;
+
+	if (hfi_flags & HFI_BUFFERFLAG_EOS)
+		flags |= V4L2_BUF_FLAG_LAST;
+
+	switch (pic_type) {
+	case HFI_PICTURE_IDR:
+	case HFI_PICTURE_I:
+		flags |= V4L2_BUF_FLAG_KEYFRAME;
+		break;
+	case HFI_PICTURE_P:
+		flags |= V4L2_BUF_FLAG_PFRAME;
+		break;
+	case HFI_PICTURE_B:
+		flags |= V4L2_BUF_FLAG_BFRAME;
+		break;
+	case HFI_FRAME_NOTCODED:
+	case HFI_UNUSED_PICT:
+	case HFI_FRAME_YUV:
+	default:
+		break;
+	}
+
+	if (!(hfi_flags & HFI_BUFFERFLAG_TIMESTAMPINVALID) && filled_len) {
+		timestamp_us = timestamp_hi;
+		timestamp_us = (timestamp_us << 32) | timestamp_lo;
+	}
+
+done:
+	inst->error = error;
+	inst->ops->buf_done(inst, buffer_type, output_tag, filled_len,
+			    offset, flags, hfi_flags, timestamp_us);
+}
+
+static void hfi_session_start_done(struct venus_core *core,
+				   struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_session_start_done_pkt *pkt = packet;
+
+	inst->error = pkt->error_type;
+	complete(&inst->done);
+}
+
+static void hfi_session_stop_done(struct venus_core *core,
+				  struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_session_stop_done_pkt *pkt = packet;
+
+	inst->error = pkt->error_type;
+	complete(&inst->done);
+}
+
+static void hfi_session_rel_res_done(struct venus_core *core,
+				     struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_session_release_resources_done_pkt *pkt = packet;
+
+	inst->error = pkt->error_type;
+	complete(&inst->done);
+}
+
+static void hfi_session_rel_buf_done(struct venus_core *core,
+				     struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_session_release_buffers_done_pkt *pkt = packet;
+
+	inst->error = pkt->error_type;
+	complete(&inst->done);
+}
+
+static void hfi_session_end_done(struct venus_core *core,
+				 struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_session_end_done_pkt *pkt = packet;
+
+	inst->error = pkt->error_type;
+	complete(&inst->done);
+}
+
+static void hfi_session_abort_done(struct venus_core *core,
+				   struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_sys_session_abort_done_pkt *pkt = packet;
+
+	inst->error = pkt->error_type;
+	complete(&inst->done);
+}
+
+static void hfi_session_get_seq_hdr_done(struct venus_core *core,
+					 struct venus_inst *inst, void *packet)
+{
+	struct hfi_msg_session_get_sequence_hdr_done_pkt *pkt = packet;
+
+	inst->error = pkt->error_type;
+	complete(&inst->done);
+}
+
+struct hfi_done_handler {
+	u32 pkt;
+	u32 pkt_sz;
+	u32 pkt_sz2;
+	void (*done)(struct venus_core *, struct venus_inst *, void *);
+	bool is_sys_pkt;
+};
+
+static const struct hfi_done_handler handlers[] = {
+	{.pkt = HFI_MSG_EVENT_NOTIFY,
+	 .pkt_sz = sizeof(struct hfi_msg_event_notify_pkt),
+	 .done = hfi_event_notify,
+	},
+	{.pkt = HFI_MSG_SYS_INIT,
+	 .pkt_sz = sizeof(struct hfi_msg_sys_init_done_pkt),
+	 .done = hfi_sys_init_done,
+	 .is_sys_pkt = true,
+	},
+	{.pkt = HFI_MSG_SYS_PROPERTY_INFO,
+	 .pkt_sz = sizeof(struct hfi_msg_sys_property_info_pkt),
+	 .done = hfi_sys_property_info,
+	 .is_sys_pkt = true,
+	},
+	{.pkt = HFI_MSG_SYS_RELEASE_RESOURCE,
+	 .pkt_sz = sizeof(struct hfi_msg_sys_release_resource_done_pkt),
+	 .done = hfi_sys_rel_resource_done,
+	 .is_sys_pkt = true,
+	},
+	{.pkt = HFI_MSG_SYS_PING_ACK,
+	 .pkt_sz = sizeof(struct hfi_msg_sys_ping_ack_pkt),
+	 .done = hfi_sys_ping_done,
+	 .is_sys_pkt = true,
+	},
+	{.pkt = HFI_MSG_SYS_IDLE,
+	 .pkt_sz = sizeof(struct hfi_msg_sys_idle_pkt),
+	 .done = hfi_sys_idle_done,
+	 .is_sys_pkt = true,
+	},
+	{.pkt = HFI_MSG_SYS_PC_PREP,
+	 .pkt_sz = sizeof(struct hfi_msg_sys_pc_prep_done_pkt),
+	 .done = hfi_sys_pc_prepare_done,
+	 .is_sys_pkt = true,
+	},
+	{.pkt = HFI_MSG_SYS_SESSION_INIT,
+	 .pkt_sz = sizeof(struct hfi_msg_session_init_done_pkt),
+	 .done = hfi_session_init_done,
+	},
+	{.pkt = HFI_MSG_SYS_SESSION_END,
+	 .pkt_sz = sizeof(struct hfi_msg_session_end_done_pkt),
+	 .done = hfi_session_end_done,
+	},
+	{.pkt = HFI_MSG_SESSION_LOAD_RESOURCES,
+	 .pkt_sz = sizeof(struct hfi_msg_session_load_resources_done_pkt),
+	 .done = hfi_session_load_res_done,
+	},
+	{.pkt = HFI_MSG_SESSION_START,
+	 .pkt_sz = sizeof(struct hfi_msg_session_start_done_pkt),
+	 .done = hfi_session_start_done,
+	},
+	{.pkt = HFI_MSG_SESSION_STOP,
+	 .pkt_sz = sizeof(struct hfi_msg_session_stop_done_pkt),
+	 .done = hfi_session_stop_done,
+	},
+	{.pkt = HFI_MSG_SYS_SESSION_ABORT,
+	 .pkt_sz = sizeof(struct hfi_msg_sys_session_abort_done_pkt),
+	 .done = hfi_session_abort_done,
+	},
+	{.pkt = HFI_MSG_SESSION_EMPTY_BUFFER,
+	 .pkt_sz = sizeof(struct hfi_msg_session_empty_buffer_done_pkt),
+	 .done = hfi_session_etb_done,
+	},
+	{.pkt = HFI_MSG_SESSION_FILL_BUFFER,
+	 .pkt_sz = sizeof(struct hfi_msg_session_fbd_uncompressed_plane0_pkt),
+	 .pkt_sz2 = sizeof(struct hfi_msg_session_fbd_compressed_pkt),
+	 .done = hfi_session_ftb_done,
+	},
+	{.pkt = HFI_MSG_SESSION_FLUSH,
+	 .pkt_sz = sizeof(struct hfi_msg_session_flush_done_pkt),
+	 .done = hfi_session_flush_done,
+	},
+	{.pkt = HFI_MSG_SESSION_PROPERTY_INFO,
+	 .pkt_sz = sizeof(struct hfi_msg_session_property_info_pkt),
+	 .done = hfi_session_prop_info,
+	},
+	{.pkt = HFI_MSG_SESSION_RELEASE_RESOURCES,
+	 .pkt_sz = sizeof(struct hfi_msg_session_release_resources_done_pkt),
+	 .done = hfi_session_rel_res_done,
+	},
+	{.pkt = HFI_MSG_SESSION_GET_SEQUENCE_HEADER,
+	 .pkt_sz = sizeof(struct hfi_msg_session_get_sequence_hdr_done_pkt),
+	 .done = hfi_session_get_seq_hdr_done,
+	},
+	{.pkt = HFI_MSG_SESSION_RELEASE_BUFFERS,
+	 .pkt_sz = sizeof(struct hfi_msg_session_release_buffers_done_pkt),
+	 .done = hfi_session_rel_buf_done,
+	},
+};
+
+void hfi_process_watchdog_timeout(struct venus_core *core)
+{
+	event_sys_error(core, EVT_SYS_WATCHDOG_TIMEOUT, NULL);
+}
+
+static struct venus_inst *to_instance(struct venus_core *core, u32 session_id)
+{
+	struct venus_inst *inst;
+
+	mutex_lock(&core->lock);
+	list_for_each_entry(inst, &core->instances, list)
+		if (hash32_ptr(inst) == session_id) {
+			mutex_unlock(&core->lock);
+			return inst;
+		}
+	mutex_unlock(&core->lock);
+
+	return NULL;
+}
+
+u32 hfi_process_msg_packet(struct venus_core *core, struct hfi_pkt_hdr *hdr)
+{
+	const struct hfi_done_handler *handler;
+	struct device *dev = core->dev;
+	struct venus_inst *inst;
+	bool found = false;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(handlers); i++) {
+		handler = &handlers[i];
+		if (handler->pkt != hdr->pkt_type)
+			continue;
+		found = true;
+		break;
+	}
+
+	if (!found)
+		return hdr->pkt_type;
+
+	if (hdr->size && hdr->size < handler->pkt_sz &&
+	    hdr->size < handler->pkt_sz2) {
+		dev_err(dev, "bad packet size (%d should be %d, pkt type:%x)\n",
+			hdr->size, handler->pkt_sz, hdr->pkt_type);
+
+		return hdr->pkt_type;
+	}
+
+	if (handler->is_sys_pkt) {
+		inst = NULL;
+	} else {
+		struct hfi_session_pkt *pkt;
+
+		pkt = (struct hfi_session_pkt *)hdr;
+		inst = to_instance(core, pkt->shdr.session_id);
+
+		if (!inst)
+			dev_warn(dev, "no valid instance(pkt session_id:%x, pkt:%x)\n",
+				 pkt->shdr.session_id,
+				 handler ? handler->pkt : 0);
+
+		/*
+		 * Event of type HFI_EVENT_SYS_ERROR will not have any session
+		 * associated with it
+		 */
+		if (!inst && hdr->pkt_type != HFI_MSG_EVENT_NOTIFY) {
+			dev_err(dev, "got invalid session id:%x\n",
+				pkt->shdr.session_id);
+			goto invalid_session;
+		}
+	}
+
+	handler->done(core, inst, hdr);
+
+invalid_session:
+	return hdr->pkt_type;
+}
diff --git a/drivers/media/platform/qcom/venus/hfi_msgs.h b/drivers/media/platform/qcom/venus/hfi_msgs.h
new file mode 100644
index 000000000000..14d9a3979b14
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/hfi_msgs.h
@@ -0,0 +1,283 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __VENUS_HFI_MSGS_H__
+#define __VENUS_HFI_MSGS_H__
+
+/* message calls */
+#define HFI_MSG_SYS_INIT			0x20001
+#define HFI_MSG_SYS_PC_PREP			0x20002
+#define HFI_MSG_SYS_RELEASE_RESOURCE		0x20003
+#define HFI_MSG_SYS_DEBUG			0x20004
+#define HFI_MSG_SYS_SESSION_INIT		0x20006
+#define HFI_MSG_SYS_SESSION_END			0x20007
+#define HFI_MSG_SYS_IDLE			0x20008
+#define HFI_MSG_SYS_COV				0x20009
+#define HFI_MSG_SYS_PROPERTY_INFO		0x2000a
+
+#define HFI_MSG_EVENT_NOTIFY			0x21001
+#define HFI_MSG_SESSION_GET_SEQUENCE_HEADER	0x21002
+
+#define HFI_MSG_SYS_PING_ACK			0x220002
+#define HFI_MSG_SYS_SESSION_ABORT		0x220004
+
+#define HFI_MSG_SESSION_LOAD_RESOURCES		0x221001
+#define HFI_MSG_SESSION_START			0x221002
+#define HFI_MSG_SESSION_STOP			0x221003
+#define HFI_MSG_SESSION_SUSPEND			0x221004
+#define HFI_MSG_SESSION_RESUME			0x221005
+#define HFI_MSG_SESSION_FLUSH			0x221006
+#define HFI_MSG_SESSION_EMPTY_BUFFER		0x221007
+#define HFI_MSG_SESSION_FILL_BUFFER		0x221008
+#define HFI_MSG_SESSION_PROPERTY_INFO		0x221009
+#define HFI_MSG_SESSION_RELEASE_RESOURCES	0x22100a
+#define HFI_MSG_SESSION_PARSE_SEQUENCE_HEADER	0x22100b
+#define HFI_MSG_SESSION_RELEASE_BUFFERS		0x22100c
+
+#define HFI_PICTURE_I				0x00000001
+#define HFI_PICTURE_P				0x00000002
+#define HFI_PICTURE_B				0x00000004
+#define HFI_PICTURE_IDR				0x00000008
+#define HFI_FRAME_NOTCODED			0x7f002000
+#define HFI_FRAME_YUV				0x7f004000
+#define HFI_UNUSED_PICT				0x10000000
+
+/* message packets */
+struct hfi_msg_event_notify_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 event_id;
+	u32 event_data1;
+	u32 event_data2;
+	u32 ext_event_data[1];
+};
+
+struct hfi_msg_event_release_buffer_ref_pkt {
+	u32 packet_buffer;
+	u32 extradata_buffer;
+	u32 output_tag;
+};
+
+struct hfi_msg_sys_init_done_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 error_type;
+	u32 num_properties;
+	u32 data[1];
+};
+
+struct hfi_msg_sys_pc_prep_done_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 error_type;
+};
+
+struct hfi_msg_sys_release_resource_done_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 resource_handle;
+	u32 error_type;
+};
+
+struct hfi_msg_session_init_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+	u32 num_properties;
+	u32 data[1];
+};
+
+struct hfi_msg_session_end_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+};
+
+struct hfi_msg_session_get_sequence_hdr_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+	u32 header_len;
+	u32 sequence_header;
+};
+
+struct hfi_msg_sys_session_abort_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+};
+
+struct hfi_msg_sys_idle_pkt {
+	struct hfi_pkt_hdr hdr;
+};
+
+struct hfi_msg_sys_ping_ack_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 client_data;
+};
+
+struct hfi_msg_sys_property_info_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 num_properties;
+	u32 data[1];
+};
+
+struct hfi_msg_session_load_resources_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+};
+
+struct hfi_msg_session_start_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+};
+
+struct hfi_msg_session_stop_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+};
+
+struct hfi_msg_session_suspend_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+};
+
+struct hfi_msg_session_resume_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+};
+
+struct hfi_msg_session_flush_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+	u32 flush_type;
+};
+
+struct hfi_msg_session_empty_buffer_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+	u32 offset;
+	u32 filled_len;
+	u32 input_tag;
+	u32 packet_buffer;
+	u32 extradata_buffer;
+	u32 data[0];
+};
+
+struct hfi_msg_session_fbd_compressed_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 time_stamp_hi;
+	u32 time_stamp_lo;
+	u32 error_type;
+	u32 flags;
+	u32 mark_target;
+	u32 mark_data;
+	u32 stats;
+	u32 offset;
+	u32 alloc_len;
+	u32 filled_len;
+	u32 input_tag;
+	u32 output_tag;
+	u32 picture_type;
+	u32 packet_buffer;
+	u32 extradata_buffer;
+	u32 data[0];
+};
+
+struct hfi_msg_session_fbd_uncompressed_plane0_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 stream_id;
+	u32 view_id;
+	u32 error_type;
+	u32 time_stamp_hi;
+	u32 time_stamp_lo;
+	u32 flags;
+	u32 mark_target;
+	u32 mark_data;
+	u32 stats;
+	u32 alloc_len;
+	u32 filled_len;
+	u32 offset;
+	u32 frame_width;
+	u32 frame_height;
+	u32 start_x_coord;
+	u32 start_y_coord;
+	u32 input_tag;
+	u32 input_tag2;
+	u32 output_tag;
+	u32 picture_type;
+	u32 packet_buffer;
+	u32 extradata_buffer;
+	u32 data[0];
+};
+
+struct hfi_msg_session_fbd_uncompressed_plane1_pkt {
+	u32 flags;
+	u32 alloc_len;
+	u32 filled_len;
+	u32 offset;
+	u32 packet_buffer2;
+	u32 data[0];
+};
+
+struct hfi_msg_session_fbd_uncompressed_plane2_pkt {
+	u32 flags;
+	u32 alloc_len;
+	u32 filled_len;
+	u32 offset;
+	u32 packet_buffer3;
+	u32 data[0];
+};
+
+struct hfi_msg_session_parse_sequence_header_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+	u32 num_properties;
+	u32 data[1];
+};
+
+struct hfi_msg_session_property_info_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 num_properties;
+	u32 data[1];
+};
+
+struct hfi_msg_session_release_resources_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+};
+
+struct hfi_msg_session_release_buffers_done_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+	u32 num_buffers;
+	u32 buffer_info[1];
+};
+
+struct hfi_msg_sys_debug_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 msg_type;
+	u32 msg_size;
+	u32 time_stamp_hi;
+	u32 time_stamp_lo;
+	u8 msg_data[1];
+};
+
+struct hfi_msg_sys_coverage_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 msg_size;
+	u32 time_stamp_hi;
+	u32 time_stamp_lo;
+	u8 msg_data[1];
+};
+
+struct venus_core;
+struct hfi_pkt_hdr;
+
+void hfi_process_watchdog_timeout(struct venus_core *core);
+u32 hfi_process_msg_packet(struct venus_core *core, struct hfi_pkt_hdr *hdr);
+
+#endif
diff --git a/drivers/media/platform/qcom/venus/hfi_venus.c b/drivers/media/platform/qcom/venus/hfi_venus.c
new file mode 100644
index 000000000000..1caae8feaa36
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/hfi_venus.c
@@ -0,0 +1,1572 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/interrupt.h>
+#include <linux/iopoll.h>
+#include <linux/kernel.h>
+#include <linux/qcom_scm.h>
+#include <linux/slab.h>
+
+#include "core.h"
+#include "hfi_cmds.h"
+#include "hfi_msgs.h"
+#include "hfi_venus.h"
+#include "hfi_venus_io.h"
+
+#define HFI_MASK_QHDR_TX_TYPE		0xff000000
+#define HFI_MASK_QHDR_RX_TYPE		0x00ff0000
+#define HFI_MASK_QHDR_PRI_TYPE		0x0000ff00
+#define HFI_MASK_QHDR_ID_TYPE		0x000000ff
+
+#define HFI_HOST_TO_CTRL_CMD_Q		0
+#define HFI_CTRL_TO_HOST_MSG_Q		1
+#define HFI_CTRL_TO_HOST_DBG_Q		2
+#define HFI_MASK_QHDR_STATUS		0x000000ff
+
+#define IFACEQ_NUM			3
+#define IFACEQ_CMD_IDX			0
+#define IFACEQ_MSG_IDX			1
+#define IFACEQ_DBG_IDX			2
+#define IFACEQ_MAX_BUF_COUNT		50
+#define IFACEQ_MAX_PARALLEL_CLNTS	16
+#define IFACEQ_DFLT_QHDR		0x01010000
+
+#define POLL_INTERVAL_US		50
+
+#define IFACEQ_MAX_PKT_SIZE		1024
+#define IFACEQ_MED_PKT_SIZE		768
+#define IFACEQ_MIN_PKT_SIZE		8
+#define IFACEQ_VAR_SMALL_PKT_SIZE	100
+#define IFACEQ_VAR_LARGE_PKT_SIZE	512
+#define IFACEQ_VAR_HUGE_PKT_SIZE	(1024 * 12)
+
+enum tzbsp_video_state {
+	TZBSP_VIDEO_STATE_SUSPEND = 0,
+	TZBSP_VIDEO_STATE_RESUME
+};
+
+struct hfi_queue_table_header {
+	u32 version;
+	u32 size;
+	u32 qhdr0_offset;
+	u32 qhdr_size;
+	u32 num_q;
+	u32 num_active_q;
+};
+
+struct hfi_queue_header {
+	u32 status;
+	u32 start_addr;
+	u32 type;
+	u32 q_size;
+	u32 pkt_size;
+	u32 pkt_drop_cnt;
+	u32 rx_wm;
+	u32 tx_wm;
+	u32 rx_req;
+	u32 tx_req;
+	u32 rx_irq_status;
+	u32 tx_irq_status;
+	u32 read_idx;
+	u32 write_idx;
+};
+
+#define IFACEQ_TABLE_SIZE	\
+	(sizeof(struct hfi_queue_table_header) +	\
+	 sizeof(struct hfi_queue_header) * IFACEQ_NUM)
+
+#define IFACEQ_QUEUE_SIZE	(IFACEQ_MAX_PKT_SIZE *	\
+	IFACEQ_MAX_BUF_COUNT * IFACEQ_MAX_PARALLEL_CLNTS)
+
+#define IFACEQ_GET_QHDR_START_ADDR(ptr, i)	\
+	(void *)(((ptr) + sizeof(struct hfi_queue_table_header)) +	\
+		((i) * sizeof(struct hfi_queue_header)))
+
+#define QDSS_SIZE		SZ_4K
+#define SFR_SIZE		SZ_4K
+#define QUEUE_SIZE		\
+	(IFACEQ_TABLE_SIZE + (IFACEQ_QUEUE_SIZE * IFACEQ_NUM))
+
+#define ALIGNED_QDSS_SIZE	ALIGN(QDSS_SIZE, SZ_4K)
+#define ALIGNED_SFR_SIZE	ALIGN(SFR_SIZE, SZ_4K)
+#define ALIGNED_QUEUE_SIZE	ALIGN(QUEUE_SIZE, SZ_4K)
+#define SHARED_QSIZE		ALIGN(ALIGNED_SFR_SIZE + ALIGNED_QUEUE_SIZE + \
+				      ALIGNED_QDSS_SIZE, SZ_1M)
+
+struct mem_desc {
+	dma_addr_t da;	/* device address */
+	void *kva;	/* kernel virtual address */
+	u32 size;
+	unsigned long attrs;
+};
+
+struct iface_queue {
+	struct hfi_queue_header *qhdr;
+	struct mem_desc qmem;
+};
+
+enum venus_state {
+	VENUS_STATE_DEINIT = 1,
+	VENUS_STATE_INIT,
+};
+
+struct venus_hfi_device {
+	struct venus_core *core;
+	u32 irq_status;
+	u32 last_packet_type;
+	bool power_enabled;
+	bool suspended;
+	enum venus_state state;
+	/* serialize read / write to the shared memory */
+	struct mutex lock;
+	struct completion pwr_collapse_prep;
+	struct completion release_resource;
+	struct mem_desc ifaceq_table;
+	struct mem_desc sfr;
+	struct iface_queue queues[IFACEQ_NUM];
+	u8 pkt_buf[IFACEQ_VAR_HUGE_PKT_SIZE];
+	u8 dbg_buf[IFACEQ_VAR_HUGE_PKT_SIZE];
+};
+
+static bool venus_pkt_debug;
+static int venus_fw_debug = HFI_DEBUG_MSG_ERROR | HFI_DEBUG_MSG_FATAL;
+static bool venus_sys_idle_indicator;
+static bool venus_fw_low_power_mode = true;
+static int venus_hw_rsp_timeout = 1000;
+static bool venus_fw_coverage;
+
+static void venus_set_state(struct venus_hfi_device *hdev,
+			    enum venus_state state)
+{
+	mutex_lock(&hdev->lock);
+	hdev->state = state;
+	mutex_unlock(&hdev->lock);
+}
+
+static bool venus_is_valid_state(struct venus_hfi_device *hdev)
+{
+	return hdev->state != VENUS_STATE_DEINIT;
+}
+
+static void venus_dump_packet(struct venus_hfi_device *hdev, const void *packet)
+{
+	size_t pkt_size = *(u32 *)packet;
+
+	if (!venus_pkt_debug)
+		return;
+
+	print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 16, 1, packet,
+		       pkt_size, true);
+}
+
+static int venus_write_queue(struct venus_hfi_device *hdev,
+			     struct iface_queue *queue,
+			     void *packet, u32 *rx_req)
+{
+	struct hfi_queue_header *qhdr;
+	u32 dwords, new_wr_idx;
+	u32 empty_space, rd_idx, wr_idx, qsize;
+	u32 *wr_ptr;
+
+	if (!queue->qmem.kva)
+		return -EINVAL;
+
+	qhdr = queue->qhdr;
+	if (!qhdr)
+		return -EINVAL;
+
+	venus_dump_packet(hdev, packet);
+
+	dwords = (*(u32 *)packet) >> 2;
+	if (!dwords)
+		return -EINVAL;
+
+	rd_idx = qhdr->read_idx;
+	wr_idx = qhdr->write_idx;
+	qsize = qhdr->q_size;
+	/* ensure rd/wr indices's are read from memory */
+	rmb();
+
+	if (wr_idx >= rd_idx)
+		empty_space = qsize - (wr_idx - rd_idx);
+	else
+		empty_space = rd_idx - wr_idx;
+
+	if (empty_space <= dwords) {
+		qhdr->tx_req = 1;
+		/* ensure tx_req is updated in memory */
+		wmb();
+		return -ENOSPC;
+	}
+
+	qhdr->tx_req = 0;
+	/* ensure tx_req is updated in memory */
+	wmb();
+
+	new_wr_idx = wr_idx + dwords;
+	wr_ptr = (u32 *)(queue->qmem.kva + (wr_idx << 2));
+	if (new_wr_idx < qsize) {
+		memcpy(wr_ptr, packet, dwords << 2);
+	} else {
+		size_t len;
+
+		new_wr_idx -= qsize;
+		len = (dwords - new_wr_idx) << 2;
+		memcpy(wr_ptr, packet, len);
+		memcpy(queue->qmem.kva, packet + len, new_wr_idx << 2);
+	}
+
+	/* make sure packet is written before updating the write index */
+	wmb();
+
+	qhdr->write_idx = new_wr_idx;
+	*rx_req = qhdr->rx_req ? 1 : 0;
+
+	/* make sure write index is updated before an interrupt is raised */
+	mb();
+
+	return 0;
+}
+
+static int venus_read_queue(struct venus_hfi_device *hdev,
+			    struct iface_queue *queue, void *pkt, u32 *tx_req)
+{
+	struct hfi_queue_header *qhdr;
+	u32 dwords, new_rd_idx;
+	u32 rd_idx, wr_idx, type, qsize;
+	u32 *rd_ptr;
+	u32 recv_request = 0;
+	int ret = 0;
+
+	if (!queue->qmem.kva)
+		return -EINVAL;
+
+	qhdr = queue->qhdr;
+	if (!qhdr)
+		return -EINVAL;
+
+	type = qhdr->type;
+	rd_idx = qhdr->read_idx;
+	wr_idx = qhdr->write_idx;
+	qsize = qhdr->q_size;
+
+	/* make sure data is valid before using it */
+	rmb();
+
+	/*
+	 * Do not set receive request for debug queue, if set, Venus generates
+	 * interrupt for debug messages even when there is no response message
+	 * available. In general debug queue will not become full as it is being
+	 * emptied out for every interrupt from Venus. Venus will anyway
+	 * generates interrupt if it is full.
+	 */
+	if (type & HFI_CTRL_TO_HOST_MSG_Q)
+		recv_request = 1;
+
+	if (rd_idx == wr_idx) {
+		qhdr->rx_req = recv_request;
+		*tx_req = 0;
+		/* update rx_req field in memory */
+		wmb();
+		return -ENODATA;
+	}
+
+	rd_ptr = (u32 *)(queue->qmem.kva + (rd_idx << 2));
+	dwords = *rd_ptr >> 2;
+	if (!dwords)
+		return -EINVAL;
+
+	new_rd_idx = rd_idx + dwords;
+	if (((dwords << 2) <= IFACEQ_VAR_HUGE_PKT_SIZE) && rd_idx <= qsize) {
+		if (new_rd_idx < qsize) {
+			memcpy(pkt, rd_ptr, dwords << 2);
+		} else {
+			size_t len;
+
+			new_rd_idx -= qsize;
+			len = (dwords - new_rd_idx) << 2;
+			memcpy(pkt, rd_ptr, len);
+			memcpy(pkt + len, queue->qmem.kva, new_rd_idx << 2);
+		}
+	} else {
+		/* bad packet received, dropping */
+		new_rd_idx = qhdr->write_idx;
+		ret = -EBADMSG;
+	}
+
+	/* ensure the packet is read before updating read index */
+	rmb();
+
+	qhdr->read_idx = new_rd_idx;
+	/* ensure updating read index */
+	wmb();
+
+	rd_idx = qhdr->read_idx;
+	wr_idx = qhdr->write_idx;
+	/* ensure rd/wr indices are read from memory */
+	rmb();
+
+	if (rd_idx != wr_idx)
+		qhdr->rx_req = 0;
+	else
+		qhdr->rx_req = recv_request;
+
+	*tx_req = qhdr->tx_req ? 1 : 0;
+
+	/* ensure rx_req is stored to memory and tx_req is loaded from memory */
+	mb();
+
+	venus_dump_packet(hdev, pkt);
+
+	return ret;
+}
+
+static int venus_alloc(struct venus_hfi_device *hdev, struct mem_desc *desc,
+		       u32 size)
+{
+	struct device *dev = hdev->core->dev;
+
+	desc->attrs = DMA_ATTR_WRITE_COMBINE;
+	desc->size = ALIGN(size, SZ_4K);
+
+	desc->kva = dma_alloc_attrs(dev, size, &desc->da, GFP_KERNEL,
+				    desc->attrs);
+	if (!desc->kva)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void venus_free(struct venus_hfi_device *hdev, struct mem_desc *mem)
+{
+	struct device *dev = hdev->core->dev;
+
+	dma_free_attrs(dev, mem->size, mem->kva, mem->da, mem->attrs);
+}
+
+static void venus_writel(struct venus_hfi_device *hdev, u32 reg, u32 value)
+{
+	writel(value, hdev->core->base + reg);
+}
+
+static u32 venus_readl(struct venus_hfi_device *hdev, u32 reg)
+{
+	return readl(hdev->core->base + reg);
+}
+
+static void venus_set_registers(struct venus_hfi_device *hdev)
+{
+	const struct venus_resources *res = hdev->core->res;
+	const struct reg_val *tbl = res->reg_tbl;
+	unsigned int count = res->reg_tbl_size;
+	unsigned int i;
+
+	for (i = 0; i < count; i++)
+		venus_writel(hdev, tbl[i].reg, tbl[i].value);
+}
+
+static void venus_soft_int(struct venus_hfi_device *hdev)
+{
+	venus_writel(hdev, CPU_IC_SOFTINT, BIT(CPU_IC_SOFTINT_H2A_SHIFT));
+}
+
+static int venus_iface_cmdq_write_nolock(struct venus_hfi_device *hdev,
+					 void *pkt)
+{
+	struct device *dev = hdev->core->dev;
+	struct hfi_pkt_hdr *cmd_packet;
+	struct iface_queue *queue;
+	u32 rx_req;
+	int ret;
+
+	if (!venus_is_valid_state(hdev))
+		return -EINVAL;
+
+	cmd_packet = (struct hfi_pkt_hdr *)pkt;
+	hdev->last_packet_type = cmd_packet->pkt_type;
+
+	queue = &hdev->queues[IFACEQ_CMD_IDX];
+
+	ret = venus_write_queue(hdev, queue, pkt, &rx_req);
+	if (ret) {
+		dev_err(dev, "write to iface cmd queue failed (%d)\n", ret);
+		return ret;
+	}
+
+	if (rx_req)
+		venus_soft_int(hdev);
+
+	return 0;
+}
+
+static int venus_iface_cmdq_write(struct venus_hfi_device *hdev, void *pkt)
+{
+	int ret;
+
+	mutex_lock(&hdev->lock);
+	ret = venus_iface_cmdq_write_nolock(hdev, pkt);
+	mutex_unlock(&hdev->lock);
+
+	return ret;
+}
+
+static int venus_hfi_core_set_resource(struct venus_core *core, u32 id,
+				       u32 size, u32 addr, void *cookie)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(core);
+	struct hfi_sys_set_resource_pkt *pkt;
+	u8 packet[IFACEQ_VAR_SMALL_PKT_SIZE];
+	int ret;
+
+	if (id == VIDC_RESOURCE_NONE)
+		return 0;
+
+	pkt = (struct hfi_sys_set_resource_pkt *)packet;
+
+	ret = pkt_sys_set_resource(pkt, id, size, addr, cookie);
+	if (ret)
+		return ret;
+
+	ret = venus_iface_cmdq_write(hdev, pkt);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int venus_boot_core(struct venus_hfi_device *hdev)
+{
+	struct device *dev = hdev->core->dev;
+	static const unsigned int max_tries = 100;
+	u32 ctrl_status = 0;
+	unsigned int count = 0;
+	int ret = 0;
+
+	venus_writel(hdev, VIDC_CTRL_INIT, BIT(VIDC_CTRL_INIT_CTRL_SHIFT));
+	venus_writel(hdev, WRAPPER_INTR_MASK, WRAPPER_INTR_MASK_A2HVCODEC_MASK);
+	venus_writel(hdev, CPU_CS_SCIACMDARG3, 1);
+
+	while (!ctrl_status && count < max_tries) {
+		ctrl_status = venus_readl(hdev, CPU_CS_SCIACMDARG0);
+		if ((ctrl_status & CPU_CS_SCIACMDARG0_ERROR_STATUS_MASK) == 4) {
+			dev_err(dev, "invalid setting for UC_REGION\n");
+			ret = -EINVAL;
+			break;
+		}
+
+		usleep_range(500, 1000);
+		count++;
+	}
+
+	if (count >= max_tries)
+		ret = -ETIMEDOUT;
+
+	return ret;
+}
+
+static u32 venus_hwversion(struct venus_hfi_device *hdev)
+{
+	struct device *dev = hdev->core->dev;
+	u32 ver = venus_readl(hdev, WRAPPER_HW_VERSION);
+	u32 major, minor, step;
+
+	major = ver & WRAPPER_HW_VERSION_MAJOR_VERSION_MASK;
+	major = major >> WRAPPER_HW_VERSION_MAJOR_VERSION_SHIFT;
+	minor = ver & WRAPPER_HW_VERSION_MINOR_VERSION_MASK;
+	minor = minor >> WRAPPER_HW_VERSION_MINOR_VERSION_SHIFT;
+	step = ver & WRAPPER_HW_VERSION_STEP_VERSION_MASK;
+
+	dev_dbg(dev, "venus hw version %x.%x.%x\n", major, minor, step);
+
+	return major;
+}
+
+static int venus_run(struct venus_hfi_device *hdev)
+{
+	struct device *dev = hdev->core->dev;
+	int ret;
+
+	/*
+	 * Re-program all of the registers that get reset as a result of
+	 * regulator_disable() and _enable()
+	 */
+	venus_set_registers(hdev);
+
+	venus_writel(hdev, UC_REGION_ADDR, hdev->ifaceq_table.da);
+	venus_writel(hdev, UC_REGION_SIZE, SHARED_QSIZE);
+	venus_writel(hdev, CPU_CS_SCIACMDARG2, hdev->ifaceq_table.da);
+	venus_writel(hdev, CPU_CS_SCIACMDARG1, 0x01);
+	if (hdev->sfr.da)
+		venus_writel(hdev, SFR_ADDR, hdev->sfr.da);
+
+	ret = venus_boot_core(hdev);
+	if (ret) {
+		dev_err(dev, "failed to reset venus core\n");
+		return ret;
+	}
+
+	venus_hwversion(hdev);
+
+	return 0;
+}
+
+static int venus_halt_axi(struct venus_hfi_device *hdev)
+{
+	void __iomem *base = hdev->core->base;
+	struct device *dev = hdev->core->dev;
+	u32 val;
+	int ret;
+
+	/* Halt AXI and AXI IMEM VBIF Access */
+	val = venus_readl(hdev, VBIF_AXI_HALT_CTRL0);
+	val |= VBIF_AXI_HALT_CTRL0_HALT_REQ;
+	venus_writel(hdev, VBIF_AXI_HALT_CTRL0, val);
+
+	/* Request for AXI bus port halt */
+	ret = readl_poll_timeout(base + VBIF_AXI_HALT_CTRL1, val,
+				 val & VBIF_AXI_HALT_CTRL1_HALT_ACK,
+				 POLL_INTERVAL_US,
+				 VBIF_AXI_HALT_ACK_TIMEOUT_US);
+	if (ret) {
+		dev_err(dev, "AXI bus port halt timeout\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int venus_power_off(struct venus_hfi_device *hdev)
+{
+	int ret;
+
+	if (!hdev->power_enabled)
+		return 0;
+
+	ret = qcom_scm_set_remote_state(TZBSP_VIDEO_STATE_SUSPEND, 0);
+	if (ret)
+		return ret;
+
+	ret = venus_halt_axi(hdev);
+	if (ret)
+		return ret;
+
+	hdev->power_enabled = false;
+
+	return 0;
+}
+
+static int venus_power_on(struct venus_hfi_device *hdev)
+{
+	int ret;
+
+	if (hdev->power_enabled)
+		return 0;
+
+	ret = qcom_scm_set_remote_state(TZBSP_VIDEO_STATE_RESUME, 0);
+	if (ret)
+		goto err;
+
+	ret = venus_run(hdev);
+	if (ret)
+		goto err_suspend;
+
+	hdev->power_enabled = true;
+
+	return 0;
+
+err_suspend:
+	qcom_scm_set_remote_state(TZBSP_VIDEO_STATE_SUSPEND, 0);
+err:
+	hdev->power_enabled = false;
+	return ret;
+}
+
+static int venus_iface_msgq_read_nolock(struct venus_hfi_device *hdev,
+					void *pkt)
+{
+	struct iface_queue *queue;
+	u32 tx_req;
+	int ret;
+
+	if (!venus_is_valid_state(hdev))
+		return -EINVAL;
+
+	queue = &hdev->queues[IFACEQ_MSG_IDX];
+
+	ret = venus_read_queue(hdev, queue, pkt, &tx_req);
+	if (ret)
+		return ret;
+
+	if (tx_req)
+		venus_soft_int(hdev);
+
+	return 0;
+}
+
+static int venus_iface_msgq_read(struct venus_hfi_device *hdev, void *pkt)
+{
+	int ret;
+
+	mutex_lock(&hdev->lock);
+	ret = venus_iface_msgq_read_nolock(hdev, pkt);
+	mutex_unlock(&hdev->lock);
+
+	return ret;
+}
+
+static int venus_iface_dbgq_read_nolock(struct venus_hfi_device *hdev,
+					void *pkt)
+{
+	struct iface_queue *queue;
+	u32 tx_req;
+	int ret;
+
+	ret = venus_is_valid_state(hdev);
+	if (!ret)
+		return -EINVAL;
+
+	queue = &hdev->queues[IFACEQ_DBG_IDX];
+
+	ret = venus_read_queue(hdev, queue, pkt, &tx_req);
+	if (ret)
+		return ret;
+
+	if (tx_req)
+		venus_soft_int(hdev);
+
+	return 0;
+}
+
+static int venus_iface_dbgq_read(struct venus_hfi_device *hdev, void *pkt)
+{
+	int ret;
+
+	if (!pkt)
+		return -EINVAL;
+
+	mutex_lock(&hdev->lock);
+	ret = venus_iface_dbgq_read_nolock(hdev, pkt);
+	mutex_unlock(&hdev->lock);
+
+	return ret;
+}
+
+static void venus_set_qhdr_defaults(struct hfi_queue_header *qhdr)
+{
+	qhdr->status = 1;
+	qhdr->type = IFACEQ_DFLT_QHDR;
+	qhdr->q_size = IFACEQ_QUEUE_SIZE / 4;
+	qhdr->pkt_size = 0;
+	qhdr->rx_wm = 1;
+	qhdr->tx_wm = 1;
+	qhdr->rx_req = 1;
+	qhdr->tx_req = 0;
+	qhdr->rx_irq_status = 0;
+	qhdr->tx_irq_status = 0;
+	qhdr->read_idx = 0;
+	qhdr->write_idx = 0;
+}
+
+static void venus_interface_queues_release(struct venus_hfi_device *hdev)
+{
+	mutex_lock(&hdev->lock);
+
+	venus_free(hdev, &hdev->ifaceq_table);
+	venus_free(hdev, &hdev->sfr);
+
+	memset(hdev->queues, 0, sizeof(hdev->queues));
+	memset(&hdev->ifaceq_table, 0, sizeof(hdev->ifaceq_table));
+	memset(&hdev->sfr, 0, sizeof(hdev->sfr));
+
+	mutex_unlock(&hdev->lock);
+}
+
+static int venus_interface_queues_init(struct venus_hfi_device *hdev)
+{
+	struct hfi_queue_table_header *tbl_hdr;
+	struct iface_queue *queue;
+	struct hfi_sfr *sfr;
+	struct mem_desc desc = {0};
+	unsigned int offset;
+	unsigned int i;
+	int ret;
+
+	ret = venus_alloc(hdev, &desc, ALIGNED_QUEUE_SIZE);
+	if (ret)
+		return ret;
+
+	hdev->ifaceq_table.kva = desc.kva;
+	hdev->ifaceq_table.da = desc.da;
+	hdev->ifaceq_table.size = IFACEQ_TABLE_SIZE;
+	offset = hdev->ifaceq_table.size;
+
+	for (i = 0; i < IFACEQ_NUM; i++) {
+		queue = &hdev->queues[i];
+		queue->qmem.da = desc.da + offset;
+		queue->qmem.kva = desc.kva + offset;
+		queue->qmem.size = IFACEQ_QUEUE_SIZE;
+		offset += queue->qmem.size;
+		queue->qhdr =
+			IFACEQ_GET_QHDR_START_ADDR(hdev->ifaceq_table.kva, i);
+
+		venus_set_qhdr_defaults(queue->qhdr);
+
+		queue->qhdr->start_addr = queue->qmem.da;
+
+		if (i == IFACEQ_CMD_IDX)
+			queue->qhdr->type |= HFI_HOST_TO_CTRL_CMD_Q;
+		else if (i == IFACEQ_MSG_IDX)
+			queue->qhdr->type |= HFI_CTRL_TO_HOST_MSG_Q;
+		else if (i == IFACEQ_DBG_IDX)
+			queue->qhdr->type |= HFI_CTRL_TO_HOST_DBG_Q;
+	}
+
+	tbl_hdr = hdev->ifaceq_table.kva;
+	tbl_hdr->version = 0;
+	tbl_hdr->size = IFACEQ_TABLE_SIZE;
+	tbl_hdr->qhdr0_offset = sizeof(struct hfi_queue_table_header);
+	tbl_hdr->qhdr_size = sizeof(struct hfi_queue_header);
+	tbl_hdr->num_q = IFACEQ_NUM;
+	tbl_hdr->num_active_q = IFACEQ_NUM;
+
+	/*
+	 * Set receive request to zero on debug queue as there is no
+	 * need of interrupt from video hardware for debug messages
+	 */
+	queue = &hdev->queues[IFACEQ_DBG_IDX];
+	queue->qhdr->rx_req = 0;
+
+	ret = venus_alloc(hdev, &desc, ALIGNED_SFR_SIZE);
+	if (ret) {
+		hdev->sfr.da = 0;
+	} else {
+		hdev->sfr.da = desc.da;
+		hdev->sfr.kva = desc.kva;
+		hdev->sfr.size = ALIGNED_SFR_SIZE;
+		sfr = hdev->sfr.kva;
+		sfr->buf_size = ALIGNED_SFR_SIZE;
+	}
+
+	/* ensure table and queue header structs are settled in memory */
+	wmb();
+
+	return 0;
+}
+
+static int venus_sys_set_debug(struct venus_hfi_device *hdev, u32 debug)
+{
+	struct hfi_sys_set_property_pkt *pkt;
+	u8 packet[IFACEQ_VAR_SMALL_PKT_SIZE];
+	int ret;
+
+	pkt = (struct hfi_sys_set_property_pkt *)packet;
+
+	pkt_sys_debug_config(pkt, HFI_DEBUG_MODE_QUEUE, debug);
+
+	ret = venus_iface_cmdq_write(hdev, pkt);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int venus_sys_set_coverage(struct venus_hfi_device *hdev, u32 mode)
+{
+	struct hfi_sys_set_property_pkt *pkt;
+	u8 packet[IFACEQ_VAR_SMALL_PKT_SIZE];
+	int ret;
+
+	pkt = (struct hfi_sys_set_property_pkt *)packet;
+
+	pkt_sys_coverage_config(pkt, mode);
+
+	ret = venus_iface_cmdq_write(hdev, pkt);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int venus_sys_set_idle_message(struct venus_hfi_device *hdev,
+				      bool enable)
+{
+	struct hfi_sys_set_property_pkt *pkt;
+	u8 packet[IFACEQ_VAR_SMALL_PKT_SIZE];
+	int ret;
+
+	if (!enable)
+		return 0;
+
+	pkt = (struct hfi_sys_set_property_pkt *)packet;
+
+	pkt_sys_idle_indicator(pkt, enable);
+
+	ret = venus_iface_cmdq_write(hdev, pkt);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int venus_sys_set_power_control(struct venus_hfi_device *hdev,
+				       bool enable)
+{
+	struct hfi_sys_set_property_pkt *pkt;
+	u8 packet[IFACEQ_VAR_SMALL_PKT_SIZE];
+	int ret;
+
+	pkt = (struct hfi_sys_set_property_pkt *)packet;
+
+	pkt_sys_power_control(pkt, enable);
+
+	ret = venus_iface_cmdq_write(hdev, pkt);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int venus_get_queue_size(struct venus_hfi_device *hdev,
+				unsigned int index)
+{
+	struct hfi_queue_header *qhdr;
+
+	if (index >= IFACEQ_NUM)
+		return -EINVAL;
+
+	qhdr = hdev->queues[index].qhdr;
+	if (!qhdr)
+		return -EINVAL;
+
+	return abs(qhdr->read_idx - qhdr->write_idx);
+}
+
+static int venus_sys_set_default_properties(struct venus_hfi_device *hdev)
+{
+	struct device *dev = hdev->core->dev;
+	int ret;
+
+	ret = venus_sys_set_debug(hdev, venus_fw_debug);
+	if (ret)
+		dev_warn(dev, "setting fw debug msg ON failed (%d)\n", ret);
+
+	ret = venus_sys_set_idle_message(hdev, venus_sys_idle_indicator);
+	if (ret)
+		dev_warn(dev, "setting idle response ON failed (%d)\n", ret);
+
+	ret = venus_sys_set_power_control(hdev, venus_fw_low_power_mode);
+	if (ret)
+		dev_warn(dev, "setting hw power collapse ON failed (%d)\n",
+			 ret);
+
+	return ret;
+}
+
+static int venus_session_cmd(struct venus_inst *inst, u32 pkt_type)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(inst->core);
+	struct hfi_session_pkt pkt;
+
+	pkt_session_cmd(&pkt, pkt_type, inst);
+
+	return venus_iface_cmdq_write(hdev, &pkt);
+}
+
+static void venus_flush_debug_queue(struct venus_hfi_device *hdev)
+{
+	struct device *dev = hdev->core->dev;
+	void *packet = hdev->dbg_buf;
+
+	while (!venus_iface_dbgq_read(hdev, packet)) {
+		struct hfi_msg_sys_coverage_pkt *pkt = packet;
+
+		if (pkt->hdr.pkt_type != HFI_MSG_SYS_COV) {
+			struct hfi_msg_sys_debug_pkt *pkt = packet;
+
+			dev_dbg(dev, "%s", pkt->msg_data);
+		}
+	}
+}
+
+static int venus_prepare_power_collapse(struct venus_hfi_device *hdev,
+					bool wait)
+{
+	unsigned long timeout = msecs_to_jiffies(venus_hw_rsp_timeout);
+	struct hfi_sys_pc_prep_pkt pkt;
+	int ret;
+
+	init_completion(&hdev->pwr_collapse_prep);
+
+	pkt_sys_pc_prep(&pkt);
+
+	ret = venus_iface_cmdq_write(hdev, &pkt);
+	if (ret)
+		return ret;
+
+	if (!wait)
+		return 0;
+
+	ret = wait_for_completion_timeout(&hdev->pwr_collapse_prep, timeout);
+	if (!ret) {
+		venus_flush_debug_queue(hdev);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int venus_are_queues_empty(struct venus_hfi_device *hdev)
+{
+	int ret1, ret2;
+
+	ret1 = venus_get_queue_size(hdev, IFACEQ_MSG_IDX);
+	if (ret1 < 0)
+		return ret1;
+
+	ret2 = venus_get_queue_size(hdev, IFACEQ_CMD_IDX);
+	if (ret2 < 0)
+		return ret2;
+
+	if (!ret1 && !ret2)
+		return 1;
+
+	return 0;
+}
+
+static void venus_sfr_print(struct venus_hfi_device *hdev)
+{
+	struct device *dev = hdev->core->dev;
+	struct hfi_sfr *sfr = hdev->sfr.kva;
+	void *p;
+
+	if (!sfr)
+		return;
+
+	p = memchr(sfr->data, '\0', sfr->buf_size);
+	/*
+	 * SFR isn't guaranteed to be NULL terminated since SYS_ERROR indicates
+	 * that Venus is in the process of crashing.
+	 */
+	if (!p)
+		sfr->data[sfr->buf_size - 1] = '\0';
+
+	dev_err_ratelimited(dev, "SFR message from FW: %s\n", sfr->data);
+}
+
+static void venus_process_msg_sys_error(struct venus_hfi_device *hdev,
+					void *packet)
+{
+	struct hfi_msg_event_notify_pkt *event_pkt = packet;
+
+	if (event_pkt->event_id != HFI_EVENT_SYS_ERROR)
+		return;
+
+	venus_set_state(hdev, VENUS_STATE_DEINIT);
+
+	/*
+	 * Once SYS_ERROR received from HW, it is safe to halt the AXI.
+	 * With SYS_ERROR, Venus FW may have crashed and HW might be
+	 * active and causing unnecessary transactions. Hence it is
+	 * safe to stop all AXI transactions from venus subsystem.
+	 */
+	venus_halt_axi(hdev);
+	venus_sfr_print(hdev);
+}
+
+static irqreturn_t venus_isr_thread(struct venus_core *core)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(core);
+	const struct venus_resources *res;
+	void *pkt;
+	u32 msg_ret;
+
+	if (!hdev)
+		return IRQ_NONE;
+
+	res = hdev->core->res;
+	pkt = hdev->pkt_buf;
+
+	if (hdev->irq_status & WRAPPER_INTR_STATUS_A2HWD_MASK) {
+		venus_sfr_print(hdev);
+		hfi_process_watchdog_timeout(core);
+	}
+
+	while (!venus_iface_msgq_read(hdev, pkt)) {
+		msg_ret = hfi_process_msg_packet(core, pkt);
+		switch (msg_ret) {
+		case HFI_MSG_EVENT_NOTIFY:
+			venus_process_msg_sys_error(hdev, pkt);
+			break;
+		case HFI_MSG_SYS_INIT:
+			venus_hfi_core_set_resource(core, res->vmem_id,
+						    res->vmem_size,
+						    res->vmem_addr,
+						    hdev);
+			break;
+		case HFI_MSG_SYS_RELEASE_RESOURCE:
+			complete(&hdev->release_resource);
+			break;
+		case HFI_MSG_SYS_PC_PREP:
+			complete(&hdev->pwr_collapse_prep);
+			break;
+		default:
+			break;
+		}
+	}
+
+	venus_flush_debug_queue(hdev);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t venus_isr(struct venus_core *core)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(core);
+	u32 status;
+
+	if (!hdev)
+		return IRQ_NONE;
+
+	status = venus_readl(hdev, WRAPPER_INTR_STATUS);
+
+	if (status & WRAPPER_INTR_STATUS_A2H_MASK ||
+	    status & WRAPPER_INTR_STATUS_A2HWD_MASK ||
+	    status & CPU_CS_SCIACMDARG0_INIT_IDLE_MSG_MASK)
+		hdev->irq_status = status;
+
+	venus_writel(hdev, CPU_CS_A2HSOFTINTCLR, 1);
+	venus_writel(hdev, WRAPPER_INTR_CLEAR, status);
+
+	return IRQ_WAKE_THREAD;
+}
+
+static int venus_core_init(struct venus_core *core)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(core);
+	struct device *dev = core->dev;
+	struct hfi_sys_get_property_pkt version_pkt;
+	struct hfi_sys_init_pkt pkt;
+	int ret;
+
+	pkt_sys_init(&pkt, HFI_VIDEO_ARCH_OX);
+
+	venus_set_state(hdev, VENUS_STATE_INIT);
+
+	ret = venus_iface_cmdq_write(hdev, &pkt);
+	if (ret)
+		return ret;
+
+	pkt_sys_image_version(&version_pkt);
+
+	ret = venus_iface_cmdq_write(hdev, &version_pkt);
+	if (ret)
+		dev_warn(dev, "failed to send image version pkt to fw\n");
+
+	return 0;
+}
+
+static int venus_core_deinit(struct venus_core *core)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(core);
+
+	venus_set_state(hdev, VENUS_STATE_DEINIT);
+	hdev->suspended = true;
+	hdev->power_enabled = false;
+
+	return 0;
+}
+
+static int venus_core_ping(struct venus_core *core, u32 cookie)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(core);
+	struct hfi_sys_ping_pkt pkt;
+
+	pkt_sys_ping(&pkt, cookie);
+
+	return venus_iface_cmdq_write(hdev, &pkt);
+}
+
+static int venus_core_trigger_ssr(struct venus_core *core, u32 trigger_type)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(core);
+	struct hfi_sys_test_ssr_pkt pkt;
+	int ret;
+
+	ret = pkt_sys_ssr_cmd(&pkt, trigger_type);
+	if (ret)
+		return ret;
+
+	return venus_iface_cmdq_write(hdev, &pkt);
+}
+
+static int venus_session_init(struct venus_inst *inst, u32 session_type,
+			      u32 codec)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(inst->core);
+	struct hfi_session_init_pkt pkt;
+	int ret;
+
+	ret = venus_sys_set_default_properties(hdev);
+	if (ret)
+		return ret;
+
+	ret = pkt_session_init(&pkt, inst, session_type, codec);
+	if (ret)
+		goto err;
+
+	ret = venus_iface_cmdq_write(hdev, &pkt);
+	if (ret)
+		goto err;
+
+	return 0;
+
+err:
+	venus_flush_debug_queue(hdev);
+	return ret;
+}
+
+static int venus_session_end(struct venus_inst *inst)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(inst->core);
+	struct device *dev = hdev->core->dev;
+
+	if (venus_fw_coverage) {
+		if (venus_sys_set_coverage(hdev, venus_fw_coverage))
+			dev_warn(dev, "fw coverage msg ON failed\n");
+	}
+
+	return venus_session_cmd(inst, HFI_CMD_SYS_SESSION_END);
+}
+
+static int venus_session_abort(struct venus_inst *inst)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(inst->core);
+
+	venus_flush_debug_queue(hdev);
+
+	return venus_session_cmd(inst, HFI_CMD_SYS_SESSION_ABORT);
+}
+
+static int venus_session_flush(struct venus_inst *inst, u32 flush_mode)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(inst->core);
+	struct hfi_session_flush_pkt pkt;
+	int ret;
+
+	ret = pkt_session_flush(&pkt, inst, flush_mode);
+	if (ret)
+		return ret;
+
+	return venus_iface_cmdq_write(hdev, &pkt);
+}
+
+static int venus_session_start(struct venus_inst *inst)
+{
+	return venus_session_cmd(inst, HFI_CMD_SESSION_START);
+}
+
+static int venus_session_stop(struct venus_inst *inst)
+{
+	return venus_session_cmd(inst, HFI_CMD_SESSION_STOP);
+}
+
+static int venus_session_continue(struct venus_inst *inst)
+{
+	return venus_session_cmd(inst, HFI_CMD_SESSION_CONTINUE);
+}
+
+static int venus_session_etb(struct venus_inst *inst,
+			     struct hfi_frame_data *in_frame)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(inst->core);
+	u32 session_type = inst->session_type;
+	int ret;
+
+	if (session_type == VIDC_SESSION_TYPE_DEC) {
+		struct hfi_session_empty_buffer_compressed_pkt pkt;
+
+		ret = pkt_session_etb_decoder(&pkt, inst, in_frame);
+		if (ret)
+			return ret;
+
+		ret = venus_iface_cmdq_write(hdev, &pkt);
+	} else if (session_type == VIDC_SESSION_TYPE_ENC) {
+		struct hfi_session_empty_buffer_uncompressed_plane0_pkt pkt;
+
+		ret = pkt_session_etb_encoder(&pkt, inst, in_frame);
+		if (ret)
+			return ret;
+
+		ret = venus_iface_cmdq_write(hdev, &pkt);
+	} else {
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int venus_session_ftb(struct venus_inst *inst,
+			     struct hfi_frame_data *out_frame)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(inst->core);
+	struct hfi_session_fill_buffer_pkt pkt;
+	int ret;
+
+	ret = pkt_session_ftb(&pkt, inst, out_frame);
+	if (ret)
+		return ret;
+
+	return venus_iface_cmdq_write(hdev, &pkt);
+}
+
+static int venus_session_set_buffers(struct venus_inst *inst,
+				     struct hfi_buffer_desc *bd)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(inst->core);
+	struct hfi_session_set_buffers_pkt *pkt;
+	u8 packet[IFACEQ_VAR_LARGE_PKT_SIZE];
+	int ret;
+
+	if (bd->buffer_type == HFI_BUFFER_INPUT)
+		return 0;
+
+	pkt = (struct hfi_session_set_buffers_pkt *)packet;
+
+	ret = pkt_session_set_buffers(pkt, inst, bd);
+	if (ret)
+		return ret;
+
+	return venus_iface_cmdq_write(hdev, pkt);
+}
+
+static int venus_session_unset_buffers(struct venus_inst *inst,
+				       struct hfi_buffer_desc *bd)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(inst->core);
+	struct hfi_session_release_buffer_pkt *pkt;
+	u8 packet[IFACEQ_VAR_LARGE_PKT_SIZE];
+	int ret;
+
+	if (bd->buffer_type == HFI_BUFFER_INPUT)
+		return 0;
+
+	pkt = (struct hfi_session_release_buffer_pkt *)packet;
+
+	ret = pkt_session_unset_buffers(pkt, inst, bd);
+	if (ret)
+		return ret;
+
+	return venus_iface_cmdq_write(hdev, pkt);
+}
+
+static int venus_session_load_res(struct venus_inst *inst)
+{
+	return venus_session_cmd(inst, HFI_CMD_SESSION_LOAD_RESOURCES);
+}
+
+static int venus_session_release_res(struct venus_inst *inst)
+{
+	return venus_session_cmd(inst, HFI_CMD_SESSION_RELEASE_RESOURCES);
+}
+
+static int venus_session_parse_seq_hdr(struct venus_inst *inst, u32 seq_hdr,
+				       u32 seq_hdr_len)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(inst->core);
+	struct hfi_session_parse_sequence_header_pkt *pkt;
+	u8 packet[IFACEQ_VAR_SMALL_PKT_SIZE];
+	int ret;
+
+	pkt = (struct hfi_session_parse_sequence_header_pkt *)packet;
+
+	ret = pkt_session_parse_seq_header(pkt, inst, seq_hdr, seq_hdr_len);
+	if (ret)
+		return ret;
+
+	ret = venus_iface_cmdq_write(hdev, pkt);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int venus_session_get_seq_hdr(struct venus_inst *inst, u32 seq_hdr,
+				     u32 seq_hdr_len)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(inst->core);
+	struct hfi_session_get_sequence_header_pkt *pkt;
+	u8 packet[IFACEQ_VAR_SMALL_PKT_SIZE];
+	int ret;
+
+	pkt = (struct hfi_session_get_sequence_header_pkt *)packet;
+
+	ret = pkt_session_get_seq_hdr(pkt, inst, seq_hdr, seq_hdr_len);
+	if (ret)
+		return ret;
+
+	return venus_iface_cmdq_write(hdev, pkt);
+}
+
+static int venus_session_set_property(struct venus_inst *inst, u32 ptype,
+				      void *pdata)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(inst->core);
+	struct hfi_session_set_property_pkt *pkt;
+	u8 packet[IFACEQ_VAR_LARGE_PKT_SIZE];
+	int ret;
+
+	pkt = (struct hfi_session_set_property_pkt *)packet;
+
+	ret = pkt_session_set_property(pkt, inst, ptype, pdata);
+	if (ret)
+		return ret;
+
+	return venus_iface_cmdq_write(hdev, pkt);
+}
+
+static int venus_session_get_property(struct venus_inst *inst, u32 ptype)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(inst->core);
+	struct hfi_session_get_property_pkt pkt;
+	int ret;
+
+	ret = pkt_session_get_property(&pkt, inst, ptype);
+	if (ret)
+		return ret;
+
+	return venus_iface_cmdq_write(hdev, &pkt);
+}
+
+static int venus_resume(struct venus_core *core)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(core);
+	int ret = 0;
+
+	mutex_lock(&hdev->lock);
+
+	if (!hdev->suspended)
+		goto unlock;
+
+	ret = venus_power_on(hdev);
+
+unlock:
+	if (!ret)
+		hdev->suspended = false;
+
+	mutex_unlock(&hdev->lock);
+
+	return ret;
+}
+
+static int venus_suspend_1xx(struct venus_core *core)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(core);
+	struct device *dev = core->dev;
+	u32 ctrl_status;
+	int ret;
+
+	if (!hdev->power_enabled || hdev->suspended)
+		return 0;
+
+	mutex_lock(&hdev->lock);
+	ret = venus_is_valid_state(hdev);
+	mutex_unlock(&hdev->lock);
+
+	if (!ret) {
+		dev_err(dev, "bad state, cannot suspend\n");
+		return -EINVAL;
+	}
+
+	ret = venus_prepare_power_collapse(hdev, true);
+	if (ret) {
+		dev_err(dev, "prepare for power collapse fail (%d)\n", ret);
+		return ret;
+	}
+
+	mutex_lock(&hdev->lock);
+
+	if (hdev->last_packet_type != HFI_CMD_SYS_PC_PREP) {
+		mutex_unlock(&hdev->lock);
+		return -EINVAL;
+	}
+
+	ret = venus_are_queues_empty(hdev);
+	if (ret < 0 || !ret) {
+		mutex_unlock(&hdev->lock);
+		return -EINVAL;
+	}
+
+	ctrl_status = venus_readl(hdev, CPU_CS_SCIACMDARG0);
+	if (!(ctrl_status & CPU_CS_SCIACMDARG0_PC_READY)) {
+		mutex_unlock(&hdev->lock);
+		return -EINVAL;
+	}
+
+	ret = venus_power_off(hdev);
+	if (ret) {
+		mutex_unlock(&hdev->lock);
+		return ret;
+	}
+
+	hdev->suspended = true;
+
+	mutex_unlock(&hdev->lock);
+
+	return 0;
+}
+
+static int venus_suspend_3xx(struct venus_core *core)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(core);
+	struct device *dev = core->dev;
+	u32 ctrl_status, wfi_status;
+	int ret;
+	int cnt = 100;
+
+	if (!hdev->power_enabled || hdev->suspended)
+		return 0;
+
+	mutex_lock(&hdev->lock);
+	ret = venus_is_valid_state(hdev);
+	mutex_unlock(&hdev->lock);
+
+	if (!ret) {
+		dev_err(dev, "bad state, cannot suspend\n");
+		return -EINVAL;
+	}
+
+	ctrl_status = venus_readl(hdev, CPU_CS_SCIACMDARG0);
+	if (!(ctrl_status & CPU_CS_SCIACMDARG0_PC_READY)) {
+		wfi_status = venus_readl(hdev, WRAPPER_CPU_STATUS);
+		ctrl_status = venus_readl(hdev, CPU_CS_SCIACMDARG0);
+
+		ret = venus_prepare_power_collapse(hdev, false);
+		if (ret) {
+			dev_err(dev, "prepare for power collapse fail (%d)\n",
+				ret);
+			return ret;
+		}
+
+		cnt = 100;
+		while (cnt--) {
+			wfi_status = venus_readl(hdev, WRAPPER_CPU_STATUS);
+			ctrl_status = venus_readl(hdev, CPU_CS_SCIACMDARG0);
+			if (ctrl_status & CPU_CS_SCIACMDARG0_PC_READY &&
+			    wfi_status & BIT(0))
+				break;
+			usleep_range(1000, 1500);
+		}
+	}
+
+	mutex_lock(&hdev->lock);
+
+	ret = venus_power_off(hdev);
+	if (ret) {
+		dev_err(dev, "venus_power_off (%d)\n", ret);
+		mutex_unlock(&hdev->lock);
+		return ret;
+	}
+
+	hdev->suspended = true;
+
+	mutex_unlock(&hdev->lock);
+
+	return 0;
+}
+
+static int venus_suspend(struct venus_core *core)
+{
+	if (core->res->hfi_version == HFI_VERSION_3XX)
+		return venus_suspend_3xx(core);
+
+	return venus_suspend_1xx(core);
+}
+
+static const struct hfi_ops venus_hfi_ops = {
+	.core_init			= venus_core_init,
+	.core_deinit			= venus_core_deinit,
+	.core_ping			= venus_core_ping,
+	.core_trigger_ssr		= venus_core_trigger_ssr,
+
+	.session_init			= venus_session_init,
+	.session_end			= venus_session_end,
+	.session_abort			= venus_session_abort,
+	.session_flush			= venus_session_flush,
+	.session_start			= venus_session_start,
+	.session_stop			= venus_session_stop,
+	.session_continue		= venus_session_continue,
+	.session_etb			= venus_session_etb,
+	.session_ftb			= venus_session_ftb,
+	.session_set_buffers		= venus_session_set_buffers,
+	.session_unset_buffers		= venus_session_unset_buffers,
+	.session_load_res		= venus_session_load_res,
+	.session_release_res		= venus_session_release_res,
+	.session_parse_seq_hdr		= venus_session_parse_seq_hdr,
+	.session_get_seq_hdr		= venus_session_get_seq_hdr,
+	.session_set_property		= venus_session_set_property,
+	.session_get_property		= venus_session_get_property,
+
+	.resume				= venus_resume,
+	.suspend			= venus_suspend,
+
+	.isr				= venus_isr,
+	.isr_thread			= venus_isr_thread,
+};
+
+void venus_hfi_destroy(struct venus_core *core)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(core);
+
+	venus_interface_queues_release(hdev);
+	mutex_destroy(&hdev->lock);
+	kfree(hdev);
+	core->priv = NULL;
+	core->ops = NULL;
+}
+
+int venus_hfi_create(struct venus_core *core)
+{
+	struct venus_hfi_device *hdev;
+	int ret;
+
+	hdev = kzalloc(sizeof(*hdev), GFP_KERNEL);
+	if (!hdev)
+		return -ENOMEM;
+
+	mutex_init(&hdev->lock);
+
+	hdev->core = core;
+	hdev->suspended = true;
+	core->priv = hdev;
+	core->ops = &venus_hfi_ops;
+	core->core_caps = ENC_ROTATION_CAPABILITY | ENC_SCALING_CAPABILITY |
+			  ENC_DEINTERLACE_CAPABILITY |
+			  DEC_MULTI_STREAM_CAPABILITY;
+
+	ret = venus_interface_queues_init(hdev);
+	if (ret)
+		goto err_kfree;
+
+	return 0;
+
+err_kfree:
+	kfree(hdev);
+	core->priv = NULL;
+	core->ops = NULL;
+	return ret;
+}
diff --git a/drivers/media/platform/qcom/venus/hfi_venus.h b/drivers/media/platform/qcom/venus/hfi_venus.h
new file mode 100644
index 000000000000..885923354033
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/hfi_venus.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __VENUS_HFI_VENUS_H__
+#define __VENUS_HFI_VENUS_H__
+
+struct venus_core;
+
+void venus_hfi_destroy(struct venus_core *core);
+int venus_hfi_create(struct venus_core *core);
+
+#endif
diff --git a/drivers/media/platform/qcom/venus/hfi_venus_io.h b/drivers/media/platform/qcom/venus/hfi_venus_io.h
new file mode 100644
index 000000000000..98cc350113ab
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/hfi_venus_io.h
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __VENUS_HFI_VENUS_IO_H__
+#define __VENUS_HFI_VENUS_IO_H__
+
+#define VBIF_BASE				0x80000
+
+#define VBIF_AXI_HALT_CTRL0			(VBIF_BASE + 0x208)
+#define VBIF_AXI_HALT_CTRL1			(VBIF_BASE + 0x20c)
+
+#define VBIF_AXI_HALT_CTRL0_HALT_REQ		BIT(0)
+#define VBIF_AXI_HALT_CTRL1_HALT_ACK		BIT(0)
+#define VBIF_AXI_HALT_ACK_TIMEOUT_US		500000
+
+#define CPU_BASE				0xc0000
+#define CPU_CS_BASE				(CPU_BASE + 0x12000)
+#define CPU_IC_BASE				(CPU_BASE + 0x1f000)
+
+#define CPU_CS_A2HSOFTINTCLR			(CPU_CS_BASE + 0x1c)
+
+#define VIDC_CTRL_INIT				(CPU_CS_BASE + 0x48)
+#define VIDC_CTRL_INIT_RESERVED_BITS31_1_MASK	0xfffffffe
+#define VIDC_CTRL_INIT_RESERVED_BITS31_1_SHIFT	1
+#define VIDC_CTRL_INIT_CTRL_MASK		0x1
+#define VIDC_CTRL_INIT_CTRL_SHIFT		0
+
+/* HFI control status */
+#define CPU_CS_SCIACMDARG0			(CPU_CS_BASE + 0x4c)
+#define CPU_CS_SCIACMDARG0_MASK			0xff
+#define CPU_CS_SCIACMDARG0_SHIFT		0x0
+#define CPU_CS_SCIACMDARG0_ERROR_STATUS_MASK	0xfe
+#define CPU_CS_SCIACMDARG0_ERROR_STATUS_SHIFT	0x1
+#define CPU_CS_SCIACMDARG0_INIT_STATUS_MASK	0x1
+#define CPU_CS_SCIACMDARG0_INIT_STATUS_SHIFT	0x0
+#define CPU_CS_SCIACMDARG0_PC_READY		BIT(8)
+#define CPU_CS_SCIACMDARG0_INIT_IDLE_MSG_MASK	BIT(30)
+
+/* HFI queue table info */
+#define CPU_CS_SCIACMDARG1			(CPU_CS_BASE + 0x50)
+
+/* HFI queue table address */
+#define CPU_CS_SCIACMDARG2			(CPU_CS_BASE + 0x54)
+
+/* Venus cpu */
+#define CPU_CS_SCIACMDARG3			(CPU_CS_BASE + 0x58)
+
+#define SFR_ADDR				(CPU_CS_BASE + 0x5c)
+#define MMAP_ADDR				(CPU_CS_BASE + 0x60)
+#define UC_REGION_ADDR				(CPU_CS_BASE + 0x64)
+#define UC_REGION_SIZE				(CPU_CS_BASE + 0x68)
+
+#define CPU_IC_SOFTINT				(CPU_IC_BASE + 0x18)
+#define CPU_IC_SOFTINT_H2A_MASK			0x8000
+#define CPU_IC_SOFTINT_H2A_SHIFT		0xf
+
+/* Venus wrapper */
+#define WRAPPER_BASE				0x000e0000
+
+#define WRAPPER_HW_VERSION			(WRAPPER_BASE + 0x00)
+#define WRAPPER_HW_VERSION_MAJOR_VERSION_MASK	0x78000000
+#define WRAPPER_HW_VERSION_MAJOR_VERSION_SHIFT	28
+#define WRAPPER_HW_VERSION_MINOR_VERSION_MASK	0xfff0000
+#define WRAPPER_HW_VERSION_MINOR_VERSION_SHIFT	16
+#define WRAPPER_HW_VERSION_STEP_VERSION_MASK	0xffff
+
+#define WRAPPER_CLOCK_CONFIG			(WRAPPER_BASE + 0x04)
+
+#define WRAPPER_INTR_STATUS			(WRAPPER_BASE + 0x0c)
+#define WRAPPER_INTR_STATUS_A2HWD_MASK		0x10
+#define WRAPPER_INTR_STATUS_A2HWD_SHIFT		0x4
+#define WRAPPER_INTR_STATUS_A2H_MASK		0x4
+#define WRAPPER_INTR_STATUS_A2H_SHIFT		0x2
+
+#define WRAPPER_INTR_MASK			(WRAPPER_BASE + 0x10)
+#define WRAPPER_INTR_MASK_A2HWD_BASK		0x10
+#define WRAPPER_INTR_MASK_A2HWD_SHIFT		0x4
+#define WRAPPER_INTR_MASK_A2HVCODEC_MASK	0x8
+#define WRAPPER_INTR_MASK_A2HVCODEC_SHIFT	0x3
+#define WRAPPER_INTR_MASK_A2HCPU_MASK		0x4
+#define WRAPPER_INTR_MASK_A2HCPU_SHIFT		0x2
+
+#define WRAPPER_INTR_CLEAR			(WRAPPER_BASE + 0x14)
+#define WRAPPER_INTR_CLEAR_A2HWD_MASK		0x10
+#define WRAPPER_INTR_CLEAR_A2HWD_SHIFT		0x4
+#define WRAPPER_INTR_CLEAR_A2H_MASK		0x4
+#define WRAPPER_INTR_CLEAR_A2H_SHIFT		0x2
+
+#define WRAPPER_POWER_STATUS			(WRAPPER_BASE + 0x44)
+#define WRAPPER_VDEC_VCODEC_POWER_CONTROL	(WRAPPER_BASE + 0x48)
+#define WRAPPER_VENC_VCODEC_POWER_CONTROL	(WRAPPER_BASE + 0x4c)
+#define WRAPPER_VDEC_VENC_AHB_BRIDGE_SYNC_RESET	(WRAPPER_BASE + 0x64)
+
+#define WRAPPER_CPU_CLOCK_CONFIG		(WRAPPER_BASE + 0x2000)
+#define WRAPPER_CPU_AXI_HALT			(WRAPPER_BASE + 0x2008)
+#define WRAPPER_CPU_AXI_HALT_STATUS		(WRAPPER_BASE + 0x200c)
+
+#define WRAPPER_CPU_CGC_DIS			(WRAPPER_BASE + 0x2010)
+#define WRAPPER_CPU_STATUS			(WRAPPER_BASE + 0x2014)
+#define WRAPPER_SW_RESET			(WRAPPER_BASE + 0x3000)
+
+#endif
diff --git a/drivers/media/platform/qcom/venus/vdec.c b/drivers/media/platform/qcom/venus/vdec.c
new file mode 100644
index 000000000000..eb0c1c51cfef
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/vdec.c
@@ -0,0 +1,1162 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/clk.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include "hfi_venus_io.h"
+#include "core.h"
+#include "helpers.h"
+#include "vdec.h"
+
+static u32 get_framesize_uncompressed(unsigned int plane, u32 width, u32 height)
+{
+	u32 y_stride, uv_stride, y_plane;
+	u32 y_sclines, uv_sclines, uv_plane;
+	u32 size;
+
+	y_stride = ALIGN(width, 128);
+	uv_stride = ALIGN(width, 128);
+	y_sclines = ALIGN(height, 32);
+	uv_sclines = ALIGN(((height + 1) >> 1), 16);
+
+	y_plane = y_stride * y_sclines;
+	uv_plane = uv_stride * uv_sclines + SZ_4K;
+	size = y_plane + uv_plane + SZ_8K;
+
+	return ALIGN(size, SZ_4K);
+}
+
+static u32 get_framesize_compressed(unsigned int width, unsigned int height)
+{
+	return ((width * height * 3 / 2) / 2) + 128;
+}
+
+/*
+ * Three resons to keep MPLANE formats (despite that the number of planes
+ * currently is one):
+ * - the MPLANE formats allow only one plane to be used
+ * - the downstream driver use MPLANE formats too
+ * - future firmware versions could add support for >1 planes
+ */
+static const struct venus_format vdec_formats[] = {
+	{
+		.pixfmt = V4L2_PIX_FMT_NV12,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_MPEG4,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_MPEG2,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_H263,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_VC1_ANNEX_G,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_VC1_ANNEX_L,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_H264,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_VP8,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_VP9,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_XVID,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+	},
+};
+
+static const struct venus_format *find_format(u32 pixfmt, u32 type)
+{
+	const struct venus_format *fmt = vdec_formats;
+	unsigned int size = ARRAY_SIZE(vdec_formats);
+	unsigned int i;
+
+	for (i = 0; i < size; i++) {
+		if (fmt[i].pixfmt == pixfmt)
+			break;
+	}
+
+	if (i == size || fmt[i].type != type)
+		return NULL;
+
+	return &fmt[i];
+}
+
+static const struct venus_format *
+find_format_by_index(unsigned int index, u32 type)
+{
+	const struct venus_format *fmt = vdec_formats;
+	unsigned int size = ARRAY_SIZE(vdec_formats);
+	unsigned int i, k = 0;
+
+	if (index > size)
+		return NULL;
+
+	for (i = 0; i < size; i++) {
+		if (fmt[i].type != type)
+			continue;
+		if (k == index)
+			break;
+		k++;
+	}
+
+	if (i == size)
+		return NULL;
+
+	return &fmt[i];
+}
+
+static const struct venus_format *
+vdec_try_fmt_common(struct venus_inst *inst, struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pixmp = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *pfmt = pixmp->plane_fmt;
+	const struct venus_format *fmt;
+	unsigned int p;
+
+	memset(pfmt[0].reserved, 0, sizeof(pfmt[0].reserved));
+	memset(pixmp->reserved, 0, sizeof(pixmp->reserved));
+
+	fmt = find_format(pixmp->pixelformat, f->type);
+	if (!fmt) {
+		if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+			pixmp->pixelformat = V4L2_PIX_FMT_NV12;
+		else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+			pixmp->pixelformat = V4L2_PIX_FMT_H264;
+		else
+			return NULL;
+		fmt = find_format(pixmp->pixelformat, f->type);
+		pixmp->width = 1280;
+		pixmp->height = 720;
+	}
+
+	pixmp->width = clamp(pixmp->width, inst->cap_width.min,
+			     inst->cap_width.max);
+	pixmp->height = clamp(pixmp->height, inst->cap_height.min,
+			      inst->cap_height.max);
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		pixmp->height = ALIGN(pixmp->height, 32);
+
+	if (pixmp->field == V4L2_FIELD_ANY)
+		pixmp->field = V4L2_FIELD_NONE;
+	pixmp->num_planes = fmt->num_planes;
+	pixmp->flags = 0;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		for (p = 0; p < pixmp->num_planes; p++) {
+			pfmt[p].sizeimage =
+				get_framesize_uncompressed(p, pixmp->width,
+							   pixmp->height);
+			pfmt[p].bytesperline = ALIGN(pixmp->width, 128);
+		}
+	} else {
+		pfmt[0].sizeimage = get_framesize_compressed(pixmp->width,
+							     pixmp->height);
+		pfmt[0].bytesperline = 0;
+	}
+
+	return fmt;
+}
+
+static int vdec_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct venus_inst *inst = to_inst(file);
+
+	vdec_try_fmt_common(inst, f);
+
+	return 0;
+}
+
+static int vdec_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct venus_inst *inst = to_inst(file);
+	const struct venus_format *fmt = NULL;
+	struct v4l2_pix_format_mplane *pixmp = &f->fmt.pix_mp;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		fmt = inst->fmt_cap;
+	else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		fmt = inst->fmt_out;
+
+	if (inst->reconfig) {
+		struct v4l2_format format = {};
+
+		inst->out_width = inst->reconfig_width;
+		inst->out_height = inst->reconfig_height;
+		inst->reconfig = false;
+
+		format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+		format.fmt.pix_mp.pixelformat = inst->fmt_cap->pixfmt;
+		format.fmt.pix_mp.width = inst->out_width;
+		format.fmt.pix_mp.height = inst->out_height;
+
+		vdec_try_fmt_common(inst, &format);
+
+		inst->width = format.fmt.pix_mp.width;
+		inst->height = format.fmt.pix_mp.height;
+	}
+
+	pixmp->pixelformat = fmt->pixfmt;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		pixmp->width = inst->width;
+		pixmp->height = inst->height;
+		pixmp->colorspace = inst->colorspace;
+		pixmp->ycbcr_enc = inst->ycbcr_enc;
+		pixmp->quantization = inst->quantization;
+		pixmp->xfer_func = inst->xfer_func;
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		pixmp->width = inst->out_width;
+		pixmp->height = inst->out_height;
+	}
+
+	vdec_try_fmt_common(inst, f);
+
+	return 0;
+}
+
+static int vdec_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct venus_inst *inst = to_inst(file);
+	struct v4l2_pix_format_mplane *pixmp = &f->fmt.pix_mp;
+	struct v4l2_pix_format_mplane orig_pixmp;
+	const struct venus_format *fmt;
+	struct v4l2_format format;
+	u32 pixfmt_out = 0, pixfmt_cap = 0;
+
+	orig_pixmp = *pixmp;
+
+	fmt = vdec_try_fmt_common(inst, f);
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		pixfmt_out = pixmp->pixelformat;
+		pixfmt_cap = inst->fmt_cap->pixfmt;
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		pixfmt_cap = pixmp->pixelformat;
+		pixfmt_out = inst->fmt_out->pixfmt;
+	}
+
+	memset(&format, 0, sizeof(format));
+
+	format.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	format.fmt.pix_mp.pixelformat = pixfmt_out;
+	format.fmt.pix_mp.width = orig_pixmp.width;
+	format.fmt.pix_mp.height = orig_pixmp.height;
+	vdec_try_fmt_common(inst, &format);
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		inst->out_width = format.fmt.pix_mp.width;
+		inst->out_height = format.fmt.pix_mp.height;
+		inst->colorspace = pixmp->colorspace;
+		inst->ycbcr_enc = pixmp->ycbcr_enc;
+		inst->quantization = pixmp->quantization;
+		inst->xfer_func = pixmp->xfer_func;
+	}
+
+	memset(&format, 0, sizeof(format));
+
+	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	format.fmt.pix_mp.pixelformat = pixfmt_cap;
+	format.fmt.pix_mp.width = orig_pixmp.width;
+	format.fmt.pix_mp.height = orig_pixmp.height;
+	vdec_try_fmt_common(inst, &format);
+
+	inst->width = format.fmt.pix_mp.width;
+	inst->height = format.fmt.pix_mp.height;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		inst->fmt_out = fmt;
+	else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		inst->fmt_cap = fmt;
+
+	return 0;
+}
+
+static int
+vdec_g_selection(struct file *file, void *fh, struct v4l2_selection *s)
+{
+	struct venus_inst *inst = to_inst(file);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		s->r.width = inst->out_width;
+		s->r.height = inst->out_height;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_PADDED:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		s->r.width = inst->width;
+		s->r.height = inst->height;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		s->r.width = inst->out_width;
+		s->r.height = inst->out_height;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	s->r.top = 0;
+	s->r.left = 0;
+
+	return 0;
+}
+
+static int
+vdec_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
+{
+	strlcpy(cap->driver, "qcom-venus", sizeof(cap->driver));
+	strlcpy(cap->card, "Qualcomm Venus video decoder", sizeof(cap->card));
+	strlcpy(cap->bus_info, "platform:qcom-venus", sizeof(cap->bus_info));
+
+	return 0;
+}
+
+static int vdec_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	const struct venus_format *fmt;
+
+	memset(f->reserved, 0, sizeof(f->reserved));
+
+	fmt = find_format_by_index(f->index, f->type);
+	if (!fmt)
+		return -EINVAL;
+
+	f->pixelformat = fmt->pixfmt;
+
+	return 0;
+}
+
+static int vdec_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	struct venus_inst *inst = to_inst(file);
+	struct v4l2_captureparm *cap = &a->parm.capture;
+	struct v4l2_fract *timeperframe = &cap->timeperframe;
+	u64 us_per_frame, fps;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+	    a->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return -EINVAL;
+
+	memset(cap->reserved, 0, sizeof(cap->reserved));
+	if (!timeperframe->denominator)
+		timeperframe->denominator = inst->timeperframe.denominator;
+	if (!timeperframe->numerator)
+		timeperframe->numerator = inst->timeperframe.numerator;
+	cap->readbuffers = 0;
+	cap->extendedmode = 0;
+	cap->capability = V4L2_CAP_TIMEPERFRAME;
+	us_per_frame = timeperframe->numerator * (u64)USEC_PER_SEC;
+	do_div(us_per_frame, timeperframe->denominator);
+
+	if (!us_per_frame)
+		return -EINVAL;
+
+	fps = (u64)USEC_PER_SEC;
+	do_div(fps, us_per_frame);
+
+	inst->fps = fps;
+	inst->timeperframe = *timeperframe;
+
+	return 0;
+}
+
+static int vdec_enum_framesizes(struct file *file, void *fh,
+				struct v4l2_frmsizeenum *fsize)
+{
+	struct venus_inst *inst = to_inst(file);
+	const struct venus_format *fmt;
+
+	fmt = find_format(fsize->pixel_format,
+			  V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (!fmt) {
+		fmt = find_format(fsize->pixel_format,
+				  V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+		if (!fmt)
+			return -EINVAL;
+	}
+
+	if (fsize->index)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+
+	fsize->stepwise.min_width = inst->cap_width.min;
+	fsize->stepwise.max_width = inst->cap_width.max;
+	fsize->stepwise.step_width = inst->cap_width.step_size;
+	fsize->stepwise.min_height = inst->cap_height.min;
+	fsize->stepwise.max_height = inst->cap_height.max;
+	fsize->stepwise.step_height = inst->cap_height.step_size;
+
+	return 0;
+}
+
+static int vdec_subscribe_event(struct v4l2_fh *fh,
+				const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_EOS:
+		return v4l2_event_subscribe(fh, sub, 2, NULL);
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subscribe(fh, sub);
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int
+vdec_try_decoder_cmd(struct file *file, void *fh, struct v4l2_decoder_cmd *cmd)
+{
+	if (cmd->cmd != V4L2_DEC_CMD_STOP)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int
+vdec_decoder_cmd(struct file *file, void *fh, struct v4l2_decoder_cmd *cmd)
+{
+	struct venus_inst *inst = to_inst(file);
+	int ret;
+
+	ret = vdec_try_decoder_cmd(file, fh, cmd);
+	if (ret)
+		return ret;
+
+	mutex_lock(&inst->lock);
+	inst->cmd_stop = true;
+	mutex_unlock(&inst->lock);
+
+	hfi_session_flush(inst);
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops vdec_ioctl_ops = {
+	.vidioc_querycap = vdec_querycap,
+	.vidioc_enum_fmt_vid_cap_mplane = vdec_enum_fmt,
+	.vidioc_enum_fmt_vid_out_mplane = vdec_enum_fmt,
+	.vidioc_s_fmt_vid_cap_mplane = vdec_s_fmt,
+	.vidioc_s_fmt_vid_out_mplane = vdec_s_fmt,
+	.vidioc_g_fmt_vid_cap_mplane = vdec_g_fmt,
+	.vidioc_g_fmt_vid_out_mplane = vdec_g_fmt,
+	.vidioc_try_fmt_vid_cap_mplane = vdec_try_fmt,
+	.vidioc_try_fmt_vid_out_mplane = vdec_try_fmt,
+	.vidioc_g_selection = vdec_g_selection,
+	.vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf = v4l2_m2m_ioctl_querybuf,
+	.vidioc_create_bufs = v4l2_m2m_ioctl_create_bufs,
+	.vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf,
+	.vidioc_qbuf = v4l2_m2m_ioctl_qbuf,
+	.vidioc_expbuf = v4l2_m2m_ioctl_expbuf,
+	.vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf,
+	.vidioc_streamon = v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff = v4l2_m2m_ioctl_streamoff,
+	.vidioc_s_parm = vdec_s_parm,
+	.vidioc_enum_framesizes = vdec_enum_framesizes,
+	.vidioc_subscribe_event = vdec_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+	.vidioc_try_decoder_cmd = vdec_try_decoder_cmd,
+	.vidioc_decoder_cmd = vdec_decoder_cmd,
+};
+
+static int vdec_set_properties(struct venus_inst *inst)
+{
+	struct vdec_controls *ctr = &inst->controls.dec;
+	struct venus_core *core = inst->core;
+	struct hfi_enable en = { .enable = 1 };
+	u32 ptype;
+	int ret;
+
+	if (core->res->hfi_version == HFI_VERSION_1XX) {
+		ptype = HFI_PROPERTY_PARAM_VDEC_CONTINUE_DATA_TRANSFER;
+		ret = hfi_session_set_property(inst, ptype, &en);
+		if (ret)
+			return ret;
+	}
+
+	if (core->res->hfi_version == HFI_VERSION_3XX ||
+	    inst->cap_bufs_mode_dynamic) {
+		struct hfi_buffer_alloc_mode mode;
+
+		ptype = HFI_PROPERTY_PARAM_BUFFER_ALLOC_MODE;
+		mode.type = HFI_BUFFER_OUTPUT;
+		mode.mode = HFI_BUFFER_MODE_DYNAMIC;
+
+		ret = hfi_session_set_property(inst, ptype, &mode);
+		if (ret)
+			return ret;
+	}
+
+	if (ctr->post_loop_deb_mode) {
+		ptype = HFI_PROPERTY_CONFIG_VDEC_POST_LOOP_DEBLOCKER;
+		en.enable = 1;
+		ret = hfi_session_set_property(inst, ptype, &en);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int vdec_init_session(struct venus_inst *inst)
+{
+	int ret;
+
+	ret = hfi_session_init(inst, inst->fmt_out->pixfmt);
+	if (ret)
+		return ret;
+
+	ret = venus_helper_set_input_resolution(inst, inst->out_width,
+						inst->out_height);
+	if (ret)
+		goto deinit;
+
+	ret = venus_helper_set_color_format(inst, inst->fmt_cap->pixfmt);
+	if (ret)
+		goto deinit;
+
+	return 0;
+deinit:
+	hfi_session_deinit(inst);
+	return ret;
+}
+
+static int vdec_cap_num_buffers(struct venus_inst *inst, unsigned int *num)
+{
+	struct hfi_buffer_requirements bufreq;
+	int ret;
+
+	ret = vdec_init_session(inst);
+	if (ret)
+		return ret;
+
+	ret = venus_helper_get_bufreq(inst, HFI_BUFFER_OUTPUT, &bufreq);
+
+	*num = bufreq.count_actual;
+
+	hfi_session_deinit(inst);
+
+	return ret;
+}
+
+static int vdec_queue_setup(struct vb2_queue *q,
+			    unsigned int *num_buffers, unsigned int *num_planes,
+			    unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct venus_inst *inst = vb2_get_drv_priv(q);
+	unsigned int p, num;
+	int ret = 0;
+
+	if (*num_planes) {
+		if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
+		    *num_planes != inst->fmt_out->num_planes)
+			return -EINVAL;
+
+		if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+		    *num_planes != inst->fmt_cap->num_planes)
+			return -EINVAL;
+
+		if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
+		    sizes[0] < inst->input_buf_size)
+			return -EINVAL;
+
+		if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+		    sizes[0] < inst->output_buf_size)
+			return -EINVAL;
+
+		return 0;
+	}
+
+	switch (q->type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		*num_planes = inst->fmt_out->num_planes;
+		sizes[0] = get_framesize_compressed(inst->out_width,
+						    inst->out_height);
+		inst->input_buf_size = sizes[0];
+		inst->num_input_bufs = *num_buffers;
+
+		ret = vdec_cap_num_buffers(inst, &num);
+		if (ret)
+			break;
+
+		inst->num_output_bufs = num;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		*num_planes = inst->fmt_cap->num_planes;
+
+		ret = vdec_cap_num_buffers(inst, &num);
+		if (ret)
+			break;
+
+		*num_buffers = max(*num_buffers, num);
+
+		for (p = 0; p < *num_planes; p++)
+			sizes[p] = get_framesize_uncompressed(p, inst->width,
+							      inst->height);
+
+		inst->num_output_bufs = *num_buffers;
+		inst->output_buf_size = sizes[0];
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int vdec_verify_conf(struct venus_inst *inst)
+{
+	struct hfi_buffer_requirements bufreq;
+	int ret;
+
+	if (!inst->num_input_bufs || !inst->num_output_bufs)
+		return -EINVAL;
+
+	ret = venus_helper_get_bufreq(inst, HFI_BUFFER_OUTPUT, &bufreq);
+	if (ret)
+		return ret;
+
+	if (inst->num_output_bufs < bufreq.count_actual ||
+	    inst->num_output_bufs < bufreq.count_min)
+		return -EINVAL;
+
+	ret = venus_helper_get_bufreq(inst, HFI_BUFFER_INPUT, &bufreq);
+	if (ret)
+		return ret;
+
+	if (inst->num_input_bufs < bufreq.count_min)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int vdec_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct venus_inst *inst = vb2_get_drv_priv(q);
+	struct venus_core *core = inst->core;
+	u32 ptype;
+	int ret;
+
+	mutex_lock(&inst->lock);
+
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		inst->streamon_out = 1;
+	else
+		inst->streamon_cap = 1;
+
+	if (!(inst->streamon_out & inst->streamon_cap)) {
+		mutex_unlock(&inst->lock);
+		return 0;
+	}
+
+	venus_helper_init_instance(inst);
+
+	inst->reconfig = false;
+	inst->sequence_cap = 0;
+	inst->sequence_out = 0;
+	inst->cmd_stop = false;
+
+	ret = vdec_init_session(inst);
+	if (ret)
+		goto bufs_done;
+
+	ret = vdec_set_properties(inst);
+	if (ret)
+		goto deinit_sess;
+
+	if (core->res->hfi_version == HFI_VERSION_3XX) {
+		struct hfi_buffer_size_actual buf_sz;
+
+		ptype = HFI_PROPERTY_PARAM_BUFFER_SIZE_ACTUAL;
+		buf_sz.type = HFI_BUFFER_OUTPUT;
+		buf_sz.size = inst->output_buf_size;
+
+		ret = hfi_session_set_property(inst, ptype, &buf_sz);
+		if (ret)
+			goto deinit_sess;
+	}
+
+	ret = vdec_verify_conf(inst);
+	if (ret)
+		goto deinit_sess;
+
+	ret = venus_helper_set_num_bufs(inst, inst->num_input_bufs,
+					VB2_MAX_FRAME);
+	if (ret)
+		goto deinit_sess;
+
+	ret = venus_helper_vb2_start_streaming(inst);
+	if (ret)
+		goto deinit_sess;
+
+	mutex_unlock(&inst->lock);
+
+	return 0;
+
+deinit_sess:
+	hfi_session_deinit(inst);
+bufs_done:
+	venus_helper_buffers_done(inst, VB2_BUF_STATE_QUEUED);
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		inst->streamon_out = 0;
+	else
+		inst->streamon_cap = 0;
+	mutex_unlock(&inst->lock);
+	return ret;
+}
+
+static const struct vb2_ops vdec_vb2_ops = {
+	.queue_setup = vdec_queue_setup,
+	.buf_init = venus_helper_vb2_buf_init,
+	.buf_prepare = venus_helper_vb2_buf_prepare,
+	.start_streaming = vdec_start_streaming,
+	.stop_streaming = venus_helper_vb2_stop_streaming,
+	.buf_queue = venus_helper_vb2_buf_queue,
+};
+
+static void vdec_buf_done(struct venus_inst *inst, unsigned int buf_type,
+			  u32 tag, u32 bytesused, u32 data_offset, u32 flags,
+			  u32 hfi_flags, u64 timestamp_us)
+{
+	enum vb2_buffer_state state = VB2_BUF_STATE_DONE;
+	struct vb2_v4l2_buffer *vbuf;
+	struct vb2_buffer *vb;
+	unsigned int type;
+
+	if (buf_type == HFI_BUFFER_INPUT)
+		type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	else
+		type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+
+	vbuf = venus_helper_find_buf(inst, type, tag);
+	if (!vbuf)
+		return;
+
+	vbuf->flags = flags;
+	vbuf->field = V4L2_FIELD_NONE;
+
+	if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		vb = &vbuf->vb2_buf;
+		vb->planes[0].bytesused =
+			max_t(unsigned int, inst->output_buf_size, bytesused);
+		vb->planes[0].data_offset = data_offset;
+		vb->timestamp = timestamp_us * NSEC_PER_USEC;
+		vbuf->sequence = inst->sequence_cap++;
+
+		if (inst->cmd_stop) {
+			vbuf->flags |= V4L2_BUF_FLAG_LAST;
+			inst->cmd_stop = false;
+		}
+
+		if (vbuf->flags & V4L2_BUF_FLAG_LAST) {
+			const struct v4l2_event ev = { .type = V4L2_EVENT_EOS };
+
+			v4l2_event_queue_fh(&inst->fh, &ev);
+		}
+	} else {
+		vbuf->sequence = inst->sequence_out++;
+	}
+
+	if (hfi_flags & HFI_BUFFERFLAG_READONLY)
+		venus_helper_acquire_buf_ref(vbuf);
+
+	if (hfi_flags & HFI_BUFFERFLAG_DATACORRUPT)
+		state = VB2_BUF_STATE_ERROR;
+
+	v4l2_m2m_buf_done(vbuf, state);
+}
+
+static void vdec_event_notify(struct venus_inst *inst, u32 event,
+			      struct hfi_event_data *data)
+{
+	struct venus_core *core = inst->core;
+	struct device *dev = core->dev_dec;
+	static const struct v4l2_event ev = {
+		.type = V4L2_EVENT_SOURCE_CHANGE,
+		.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION };
+
+	switch (event) {
+	case EVT_SESSION_ERROR:
+		inst->session_error = true;
+		dev_err(dev, "dec: event session error %x\n", inst->error);
+		break;
+	case EVT_SYS_EVENT_CHANGE:
+		switch (data->event_type) {
+		case HFI_EVENT_DATA_SEQUENCE_CHANGED_SUFFICIENT_BUF_RESOURCES:
+			hfi_session_continue(inst);
+			dev_dbg(dev, "event sufficient resources\n");
+			break;
+		case HFI_EVENT_DATA_SEQUENCE_CHANGED_INSUFFICIENT_BUF_RESOURCES:
+			inst->reconfig_height = data->height;
+			inst->reconfig_width = data->width;
+			inst->reconfig = true;
+
+			v4l2_event_queue_fh(&inst->fh, &ev);
+
+			dev_dbg(dev, "event not sufficient resources (%ux%u)\n",
+				data->width, data->height);
+			break;
+		case HFI_EVENT_RELEASE_BUFFER_REFERENCE:
+			venus_helper_release_buf_ref(inst, data->tag);
+			break;
+		default:
+			break;
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+static const struct hfi_inst_ops vdec_hfi_ops = {
+	.buf_done = vdec_buf_done,
+	.event_notify = vdec_event_notify,
+};
+
+static void vdec_inst_init(struct venus_inst *inst)
+{
+	inst->fmt_out = &vdec_formats[6];
+	inst->fmt_cap = &vdec_formats[0];
+	inst->width = 1280;
+	inst->height = ALIGN(720, 32);
+	inst->out_width = 1280;
+	inst->out_height = 720;
+	inst->fps = 30;
+	inst->timeperframe.numerator = 1;
+	inst->timeperframe.denominator = 30;
+
+	inst->cap_width.min = 64;
+	inst->cap_width.max = 1920;
+	if (inst->core->res->hfi_version == HFI_VERSION_3XX)
+		inst->cap_width.max = 3840;
+	inst->cap_width.step_size = 1;
+	inst->cap_height.min = 64;
+	inst->cap_height.max = ALIGN(1080, 32);
+	if (inst->core->res->hfi_version == HFI_VERSION_3XX)
+		inst->cap_height.max = ALIGN(2160, 32);
+	inst->cap_height.step_size = 1;
+	inst->cap_framerate.min = 1;
+	inst->cap_framerate.max = 30;
+	inst->cap_framerate.step_size = 1;
+	inst->cap_mbs_per_frame.min = 16;
+	inst->cap_mbs_per_frame.max = 8160;
+}
+
+static const struct v4l2_m2m_ops vdec_m2m_ops = {
+	.device_run = venus_helper_m2m_device_run,
+	.job_abort = venus_helper_m2m_job_abort,
+};
+
+static int m2m_queue_init(void *priv, struct vb2_queue *src_vq,
+			  struct vb2_queue *dst_vq)
+{
+	struct venus_inst *inst = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->ops = &vdec_vb2_ops;
+	src_vq->mem_ops = &vb2_dma_sg_memops;
+	src_vq->drv_priv = inst;
+	src_vq->buf_struct_size = sizeof(struct venus_buffer);
+	src_vq->allow_zero_bytesused = 1;
+	src_vq->min_buffers_needed = 1;
+	src_vq->dev = inst->core->dev;
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->ops = &vdec_vb2_ops;
+	dst_vq->mem_ops = &vb2_dma_sg_memops;
+	dst_vq->drv_priv = inst;
+	dst_vq->buf_struct_size = sizeof(struct venus_buffer);
+	dst_vq->allow_zero_bytesused = 1;
+	dst_vq->min_buffers_needed = 1;
+	dst_vq->dev = inst->core->dev;
+	ret = vb2_queue_init(dst_vq);
+	if (ret) {
+		vb2_queue_release(src_vq);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int vdec_open(struct file *file)
+{
+	struct venus_core *core = video_drvdata(file);
+	struct venus_inst *inst;
+	int ret;
+
+	inst = kzalloc(sizeof(*inst), GFP_KERNEL);
+	if (!inst)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&inst->registeredbufs);
+	INIT_LIST_HEAD(&inst->internalbufs);
+	INIT_LIST_HEAD(&inst->list);
+	mutex_init(&inst->lock);
+
+	inst->core = core;
+	inst->session_type = VIDC_SESSION_TYPE_DEC;
+	inst->num_output_bufs = 1;
+
+	venus_helper_init_instance(inst);
+
+	ret = pm_runtime_get_sync(core->dev_dec);
+	if (ret < 0)
+		goto err_free_inst;
+
+	ret = vdec_ctrl_init(inst);
+	if (ret)
+		goto err_put_sync;
+
+	ret = hfi_session_create(inst, &vdec_hfi_ops);
+	if (ret)
+		goto err_ctrl_deinit;
+
+	vdec_inst_init(inst);
+
+	/*
+	 * create m2m device for every instance, the m2m context scheduling
+	 * is made by firmware side so we do not need to care about.
+	 */
+	inst->m2m_dev = v4l2_m2m_init(&vdec_m2m_ops);
+	if (IS_ERR(inst->m2m_dev)) {
+		ret = PTR_ERR(inst->m2m_dev);
+		goto err_session_destroy;
+	}
+
+	inst->m2m_ctx = v4l2_m2m_ctx_init(inst->m2m_dev, inst, m2m_queue_init);
+	if (IS_ERR(inst->m2m_ctx)) {
+		ret = PTR_ERR(inst->m2m_ctx);
+		goto err_m2m_release;
+	}
+
+	v4l2_fh_init(&inst->fh, core->vdev_dec);
+
+	inst->fh.ctrl_handler = &inst->ctrl_handler;
+	v4l2_fh_add(&inst->fh);
+	inst->fh.m2m_ctx = inst->m2m_ctx;
+	file->private_data = &inst->fh;
+
+	return 0;
+
+err_m2m_release:
+	v4l2_m2m_release(inst->m2m_dev);
+err_session_destroy:
+	hfi_session_destroy(inst);
+err_ctrl_deinit:
+	vdec_ctrl_deinit(inst);
+err_put_sync:
+	pm_runtime_put_sync(core->dev_dec);
+err_free_inst:
+	kfree(inst);
+	return ret;
+}
+
+static int vdec_close(struct file *file)
+{
+	struct venus_inst *inst = to_inst(file);
+
+	v4l2_m2m_ctx_release(inst->m2m_ctx);
+	v4l2_m2m_release(inst->m2m_dev);
+	vdec_ctrl_deinit(inst);
+	hfi_session_destroy(inst);
+	mutex_destroy(&inst->lock);
+	v4l2_fh_del(&inst->fh);
+	v4l2_fh_exit(&inst->fh);
+
+	pm_runtime_put_sync(inst->core->dev_dec);
+
+	kfree(inst);
+	return 0;
+}
+
+static const struct v4l2_file_operations vdec_fops = {
+	.owner = THIS_MODULE,
+	.open = vdec_open,
+	.release = vdec_close,
+	.unlocked_ioctl = video_ioctl2,
+	.poll = v4l2_m2m_fop_poll,
+	.mmap = v4l2_m2m_fop_mmap,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl32 = v4l2_compat_ioctl32,
+#endif
+};
+
+static int vdec_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct video_device *vdev;
+	struct venus_core *core;
+	int ret;
+
+	if (!dev->parent)
+		return -EPROBE_DEFER;
+
+	core = dev_get_drvdata(dev->parent);
+	if (!core)
+		return -EPROBE_DEFER;
+
+	if (core->res->hfi_version == HFI_VERSION_3XX) {
+		core->core0_clk = devm_clk_get(dev, "core");
+		if (IS_ERR(core->core0_clk))
+			return PTR_ERR(core->core0_clk);
+	}
+
+	platform_set_drvdata(pdev, core);
+
+	vdev = video_device_alloc();
+	if (!vdev)
+		return -ENOMEM;
+
+	vdev->release = video_device_release;
+	vdev->fops = &vdec_fops;
+	vdev->ioctl_ops = &vdec_ioctl_ops;
+	vdev->vfl_dir = VFL_DIR_M2M;
+	vdev->v4l2_dev = &core->v4l2_dev;
+	vdev->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING;
+
+	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+	if (ret)
+		goto err_vdev_release;
+
+	core->vdev_dec = vdev;
+	core->dev_dec = dev;
+
+	video_set_drvdata(vdev, core);
+	pm_runtime_enable(dev);
+
+	return 0;
+
+err_vdev_release:
+	video_device_release(vdev);
+	return ret;
+}
+
+static int vdec_remove(struct platform_device *pdev)
+{
+	struct venus_core *core = dev_get_drvdata(pdev->dev.parent);
+
+	video_unregister_device(core->vdev_dec);
+	pm_runtime_disable(core->dev_dec);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int vdec_runtime_suspend(struct device *dev)
+{
+	struct venus_core *core = dev_get_drvdata(dev);
+
+	if (core->res->hfi_version == HFI_VERSION_1XX)
+		return 0;
+
+	writel(0, core->base + WRAPPER_VDEC_VCODEC_POWER_CONTROL);
+	clk_disable_unprepare(core->core0_clk);
+	writel(1, core->base + WRAPPER_VDEC_VCODEC_POWER_CONTROL);
+
+	return 0;
+}
+
+static int vdec_runtime_resume(struct device *dev)
+{
+	struct venus_core *core = dev_get_drvdata(dev);
+	int ret;
+
+	if (core->res->hfi_version == HFI_VERSION_1XX)
+		return 0;
+
+	writel(0, core->base + WRAPPER_VDEC_VCODEC_POWER_CONTROL);
+	ret = clk_prepare_enable(core->core0_clk);
+	writel(1, core->base + WRAPPER_VDEC_VCODEC_POWER_CONTROL);
+
+	return ret;
+}
+#endif
+
+static const struct dev_pm_ops vdec_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(vdec_runtime_suspend, vdec_runtime_resume, NULL)
+};
+
+static const struct of_device_id vdec_dt_match[] = {
+	{ .compatible = "venus-decoder" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, vdec_dt_match);
+
+static struct platform_driver qcom_venus_dec_driver = {
+	.probe = vdec_probe,
+	.remove = vdec_remove,
+	.driver = {
+		.name = "qcom-venus-decoder",
+		.of_match_table = vdec_dt_match,
+		.pm = &vdec_pm_ops,
+	},
+};
+module_platform_driver(qcom_venus_dec_driver);
+
+MODULE_ALIAS("platform:qcom-venus-decoder");
+MODULE_DESCRIPTION("Qualcomm Venus video decoder driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/qcom/venus/vdec.h b/drivers/media/platform/qcom/venus/vdec.h
new file mode 100644
index 000000000000..84b672c54d02
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/vdec.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __VENUS_VDEC_H__
+#define __VENUS_VDEC_H__
+
+struct venus_inst;
+
+int vdec_ctrl_init(struct venus_inst *inst);
+void vdec_ctrl_deinit(struct venus_inst *inst);
+
+#endif
diff --git a/drivers/media/platform/qcom/venus/vdec_ctrls.c b/drivers/media/platform/qcom/venus/vdec_ctrls.c
new file mode 100644
index 000000000000..032839bbc967
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/vdec_ctrls.c
@@ -0,0 +1,158 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/types.h>
+#include <media/v4l2-ctrls.h>
+
+#include "core.h"
+#include "vdec.h"
+
+static int vdec_op_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct venus_inst *inst = ctrl_to_inst(ctrl);
+	struct vdec_controls *ctr = &inst->controls.dec;
+
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_VIDEO_DECODER_MPEG4_DEBLOCK_FILTER:
+		ctr->post_loop_deb_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
+	case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
+	case V4L2_CID_MPEG_VIDEO_VPX_PROFILE:
+		ctr->profile = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
+	case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
+		ctr->level = ctrl->val;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vdec_op_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct venus_inst *inst = ctrl_to_inst(ctrl);
+	struct vdec_controls *ctr = &inst->controls.dec;
+	union hfi_get_property hprop;
+	u32 ptype = HFI_PROPERTY_PARAM_PROFILE_LEVEL_CURRENT;
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
+	case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
+	case V4L2_CID_MPEG_VIDEO_VPX_PROFILE:
+		ret = hfi_session_get_property(inst, ptype, &hprop);
+		if (!ret)
+			ctr->profile = hprop.profile_level.profile;
+		ctrl->val = ctr->profile;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
+	case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
+		ret = hfi_session_get_property(inst, ptype, &hprop);
+		if (!ret)
+			ctr->level = hprop.profile_level.level;
+		ctrl->val = ctr->level;
+		break;
+	case V4L2_CID_MPEG_VIDEO_DECODER_MPEG4_DEBLOCK_FILTER:
+		ctrl->val = ctr->post_loop_deb_mode;
+		break;
+	case V4L2_CID_MIN_BUFFERS_FOR_CAPTURE:
+		ctrl->val = inst->num_output_bufs;
+		break;
+	default:
+		return -EINVAL;
+	};
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vdec_ctrl_ops = {
+	.s_ctrl = vdec_op_s_ctrl,
+	.g_volatile_ctrl = vdec_op_g_volatile_ctrl,
+};
+
+int vdec_ctrl_init(struct venus_inst *inst)
+{
+	struct v4l2_ctrl *ctrl;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(&inst->ctrl_handler, 7);
+	if (ret)
+		return ret;
+
+	ctrl = v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &vdec_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE,
+		V4L2_MPEG_VIDEO_MPEG4_PROFILE_ADVANCED_CODING_EFFICIENCY,
+		~((1 << V4L2_MPEG_VIDEO_MPEG4_PROFILE_SIMPLE) |
+		  (1 << V4L2_MPEG_VIDEO_MPEG4_PROFILE_ADVANCED_SIMPLE)),
+		V4L2_MPEG_VIDEO_MPEG4_PROFILE_SIMPLE);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	ctrl = v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &vdec_ctrl_ops,
+				      V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL,
+				      V4L2_MPEG_VIDEO_MPEG4_LEVEL_5,
+				      0, V4L2_MPEG_VIDEO_MPEG4_LEVEL_0);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	ctrl = v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &vdec_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_PROFILE,
+		V4L2_MPEG_VIDEO_H264_PROFILE_MULTIVIEW_HIGH,
+		~((1 << V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
+		  (1 << V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE) |
+		  (1 << V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) |
+		  (1 << V4L2_MPEG_VIDEO_H264_PROFILE_HIGH) |
+		  (1 << V4L2_MPEG_VIDEO_H264_PROFILE_STEREO_HIGH) |
+		  (1 << V4L2_MPEG_VIDEO_H264_PROFILE_MULTIVIEW_HIGH)),
+		V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	ctrl = v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &vdec_ctrl_ops,
+				      V4L2_CID_MPEG_VIDEO_H264_LEVEL,
+				      V4L2_MPEG_VIDEO_H264_LEVEL_5_1,
+				      0, V4L2_MPEG_VIDEO_H264_LEVEL_1_0);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	ctrl = v4l2_ctrl_new_std(&inst->ctrl_handler, &vdec_ctrl_ops,
+				 V4L2_CID_MPEG_VIDEO_VPX_PROFILE, 0, 3, 1, 0);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &vdec_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_DECODER_MPEG4_DEBLOCK_FILTER, 0, 1, 1, 0);
+
+	ctrl = v4l2_ctrl_new_std(&inst->ctrl_handler, &vdec_ctrl_ops,
+		V4L2_CID_MIN_BUFFERS_FOR_CAPTURE, 1, 32, 1, 1);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+	ret = inst->ctrl_handler.error;
+	if (ret) {
+		v4l2_ctrl_handler_free(&inst->ctrl_handler);
+		return ret;
+	}
+
+	return 0;
+}
+
+void vdec_ctrl_deinit(struct venus_inst *inst)
+{
+	v4l2_ctrl_handler_free(&inst->ctrl_handler);
+}
diff --git a/drivers/media/platform/qcom/venus/venc.c b/drivers/media/platform/qcom/venus/venc.c
new file mode 100644
index 000000000000..39748e7a08e4
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/venc.c
@@ -0,0 +1,1283 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/clk.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-sg.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ctrls.h>
+
+#include "hfi_venus_io.h"
+#include "core.h"
+#include "helpers.h"
+#include "venc.h"
+
+#define NUM_B_FRAMES_MAX	4
+
+static u32 get_framesize_uncompressed(unsigned int plane, u32 width, u32 height)
+{
+	u32 y_stride, uv_stride, y_plane;
+	u32 y_sclines, uv_sclines, uv_plane;
+	u32 size;
+
+	y_stride = ALIGN(width, 128);
+	uv_stride = ALIGN(width, 128);
+	y_sclines = ALIGN(height, 32);
+	uv_sclines = ALIGN(((height + 1) >> 1), 16);
+
+	y_plane = y_stride * y_sclines;
+	uv_plane = uv_stride * uv_sclines + SZ_4K;
+	size = y_plane + uv_plane + SZ_8K;
+	size = ALIGN(size, SZ_4K);
+
+	return size;
+}
+
+static u32 get_framesize_compressed(u32 width, u32 height)
+{
+	u32 sz = ALIGN(height, 32) * ALIGN(width, 32) * 3 / 2 / 2;
+
+	return ALIGN(sz, SZ_4K);
+}
+
+/*
+ * Three resons to keep MPLANE formats (despite that the number of planes
+ * currently is one):
+ * - the MPLANE formats allow only one plane to be used
+ * - the downstream driver use MPLANE formats too
+ * - future firmware versions could add support for >1 planes
+ */
+static const struct venus_format venc_formats[] = {
+	{
+		.pixfmt = V4L2_PIX_FMT_NV12,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_MPEG4,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_H263,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_H264,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_VP8,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+	}, {
+		.pixfmt = V4L2_PIX_FMT_VP9,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+	},
+};
+
+static const struct venus_format *find_format(u32 pixfmt, u32 type)
+{
+	const struct venus_format *fmt = venc_formats;
+	unsigned int size = ARRAY_SIZE(venc_formats);
+	unsigned int i;
+
+	for (i = 0; i < size; i++) {
+		if (fmt[i].pixfmt == pixfmt)
+			break;
+	}
+
+	if (i == size || fmt[i].type != type)
+		return NULL;
+
+	return &fmt[i];
+}
+
+static const struct venus_format *
+find_format_by_index(unsigned int index, u32 type)
+{
+	const struct venus_format *fmt = venc_formats;
+	unsigned int size = ARRAY_SIZE(venc_formats);
+	unsigned int i, k = 0;
+
+	if (index > size)
+		return NULL;
+
+	for (i = 0; i < size; i++) {
+		if (fmt[i].type != type)
+			continue;
+		if (k == index)
+			break;
+		k++;
+	}
+
+	if (i == size)
+		return NULL;
+
+	return &fmt[i];
+}
+
+static int venc_v4l2_to_hfi(int id, int value)
+{
+	switch (id) {
+	case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
+		switch (value) {
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_0:
+		default:
+			return HFI_MPEG4_LEVEL_0;
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_0B:
+			return HFI_MPEG4_LEVEL_0b;
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_1:
+			return HFI_MPEG4_LEVEL_1;
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_2:
+			return HFI_MPEG4_LEVEL_2;
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_3:
+			return HFI_MPEG4_LEVEL_3;
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_4:
+			return HFI_MPEG4_LEVEL_4;
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_5:
+			return HFI_MPEG4_LEVEL_5;
+		}
+	case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
+		switch (value) {
+		case V4L2_MPEG_VIDEO_MPEG4_PROFILE_SIMPLE:
+		default:
+			return HFI_MPEG4_PROFILE_SIMPLE;
+		case V4L2_MPEG_VIDEO_MPEG4_PROFILE_ADVANCED_SIMPLE:
+			return HFI_MPEG4_PROFILE_ADVANCEDSIMPLE;
+		}
+	case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
+		switch (value) {
+		case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
+			return HFI_H264_PROFILE_BASELINE;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
+			return HFI_H264_PROFILE_CONSTRAINED_BASE;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
+			return HFI_H264_PROFILE_MAIN;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
+		default:
+			return HFI_H264_PROFILE_HIGH;
+		}
+	case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
+		switch (value) {
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_0:
+			return HFI_H264_LEVEL_1;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1B:
+			return HFI_H264_LEVEL_1b;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_1:
+			return HFI_H264_LEVEL_11;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_2:
+			return HFI_H264_LEVEL_12;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_3:
+			return HFI_H264_LEVEL_13;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_0:
+			return HFI_H264_LEVEL_2;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_1:
+			return HFI_H264_LEVEL_21;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_2:
+			return HFI_H264_LEVEL_22;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_0:
+			return HFI_H264_LEVEL_3;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_1:
+			return HFI_H264_LEVEL_31;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_2:
+			return HFI_H264_LEVEL_32;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_4_0:
+			return HFI_H264_LEVEL_4;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_4_1:
+			return HFI_H264_LEVEL_41;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_4_2:
+			return HFI_H264_LEVEL_42;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_5_0:
+		default:
+			return HFI_H264_LEVEL_5;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_5_1:
+			return HFI_H264_LEVEL_51;
+		}
+	case V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE:
+		switch (value) {
+		case V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CAVLC:
+		default:
+			return HFI_H264_ENTROPY_CAVLC;
+		case V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC:
+			return HFI_H264_ENTROPY_CABAC;
+		}
+	case V4L2_CID_MPEG_VIDEO_VPX_PROFILE:
+		switch (value) {
+		case 0:
+		default:
+			return HFI_VPX_PROFILE_VERSION_0;
+		case 1:
+			return HFI_VPX_PROFILE_VERSION_1;
+		case 2:
+			return HFI_VPX_PROFILE_VERSION_2;
+		case 3:
+			return HFI_VPX_PROFILE_VERSION_3;
+		}
+	}
+
+	return 0;
+}
+
+static int
+venc_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
+{
+	strlcpy(cap->driver, "qcom-venus", sizeof(cap->driver));
+	strlcpy(cap->card, "Qualcomm Venus video encoder", sizeof(cap->card));
+	strlcpy(cap->bus_info, "platform:qcom-venus", sizeof(cap->bus_info));
+
+	return 0;
+}
+
+static int venc_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	const struct venus_format *fmt;
+
+	fmt = find_format_by_index(f->index, f->type);
+
+	memset(f->reserved, 0, sizeof(f->reserved));
+
+	if (!fmt)
+		return -EINVAL;
+
+	f->pixelformat = fmt->pixfmt;
+
+	return 0;
+}
+
+static const struct venus_format *
+venc_try_fmt_common(struct venus_inst *inst, struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pixmp = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *pfmt = pixmp->plane_fmt;
+	const struct venus_format *fmt;
+	unsigned int p;
+
+	memset(pfmt[0].reserved, 0, sizeof(pfmt[0].reserved));
+	memset(pixmp->reserved, 0, sizeof(pixmp->reserved));
+
+	fmt = find_format(pixmp->pixelformat, f->type);
+	if (!fmt) {
+		if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+			pixmp->pixelformat = V4L2_PIX_FMT_H264;
+		else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+			pixmp->pixelformat = V4L2_PIX_FMT_NV12;
+		else
+			return NULL;
+		fmt = find_format(pixmp->pixelformat, f->type);
+		pixmp->width = 1280;
+		pixmp->height = 720;
+	}
+
+	pixmp->width = clamp(pixmp->width, inst->cap_width.min,
+			     inst->cap_width.max);
+	pixmp->height = clamp(pixmp->height, inst->cap_height.min,
+			      inst->cap_height.max);
+
+	if (inst->core->res->hfi_version == HFI_VERSION_1XX)
+		pixmp->height = ALIGN(pixmp->height, 32);
+
+	pixmp->width = ALIGN(pixmp->width, 2);
+	pixmp->height = ALIGN(pixmp->height, 2);
+
+	if (pixmp->field == V4L2_FIELD_ANY)
+		pixmp->field = V4L2_FIELD_NONE;
+	pixmp->num_planes = fmt->num_planes;
+	pixmp->flags = 0;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		for (p = 0; p < pixmp->num_planes; p++) {
+			pfmt[p].sizeimage =
+				get_framesize_uncompressed(p, pixmp->width,
+							   pixmp->height);
+
+			pfmt[p].bytesperline = ALIGN(pixmp->width, 128);
+		}
+	} else {
+		pfmt[0].sizeimage = get_framesize_compressed(pixmp->width,
+							     pixmp->height);
+		pfmt[0].bytesperline = 0;
+	}
+
+	return fmt;
+}
+
+static int venc_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct venus_inst *inst = to_inst(file);
+
+	venc_try_fmt_common(inst, f);
+
+	return 0;
+}
+
+static int venc_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct venus_inst *inst = to_inst(file);
+	struct v4l2_pix_format_mplane *pixmp = &f->fmt.pix_mp;
+	struct v4l2_pix_format_mplane orig_pixmp;
+	const struct venus_format *fmt;
+	struct v4l2_format format;
+	u32 pixfmt_out = 0, pixfmt_cap = 0;
+
+	orig_pixmp = *pixmp;
+
+	fmt = venc_try_fmt_common(inst, f);
+	if (!fmt)
+		return -EINVAL;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		pixfmt_out = pixmp->pixelformat;
+		pixfmt_cap = inst->fmt_cap->pixfmt;
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		pixfmt_cap = pixmp->pixelformat;
+		pixfmt_out = inst->fmt_out->pixfmt;
+	}
+
+	memset(&format, 0, sizeof(format));
+
+	format.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	format.fmt.pix_mp.pixelformat = pixfmt_out;
+	format.fmt.pix_mp.width = orig_pixmp.width;
+	format.fmt.pix_mp.height = orig_pixmp.height;
+	venc_try_fmt_common(inst, &format);
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		inst->out_width = format.fmt.pix_mp.width;
+		inst->out_height = format.fmt.pix_mp.height;
+		inst->colorspace = pixmp->colorspace;
+		inst->ycbcr_enc = pixmp->ycbcr_enc;
+		inst->quantization = pixmp->quantization;
+		inst->xfer_func = pixmp->xfer_func;
+	}
+
+	memset(&format, 0, sizeof(format));
+
+	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	format.fmt.pix_mp.pixelformat = pixfmt_cap;
+	format.fmt.pix_mp.width = orig_pixmp.width;
+	format.fmt.pix_mp.height = orig_pixmp.height;
+	venc_try_fmt_common(inst, &format);
+
+	inst->width = format.fmt.pix_mp.width;
+	inst->height = format.fmt.pix_mp.height;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		inst->fmt_out = fmt;
+	else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		inst->fmt_cap = fmt;
+
+	return 0;
+}
+
+static int venc_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pixmp = &f->fmt.pix_mp;
+	struct venus_inst *inst = to_inst(file);
+	const struct venus_format *fmt;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		fmt = inst->fmt_cap;
+	else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		fmt = inst->fmt_out;
+	else
+		return -EINVAL;
+
+	pixmp->pixelformat = fmt->pixfmt;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		pixmp->width = inst->width;
+		pixmp->height = inst->height;
+		pixmp->colorspace = inst->colorspace;
+		pixmp->ycbcr_enc = inst->ycbcr_enc;
+		pixmp->quantization = inst->quantization;
+		pixmp->xfer_func = inst->xfer_func;
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		pixmp->width = inst->out_width;
+		pixmp->height = inst->out_height;
+	}
+
+	venc_try_fmt_common(inst, f);
+
+	return 0;
+}
+
+static int
+venc_g_selection(struct file *file, void *fh, struct v4l2_selection *s)
+{
+	struct venus_inst *inst = to_inst(file);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		s->r.width = inst->width;
+		s->r.height = inst->height;
+		break;
+	case V4L2_SEL_TGT_CROP:
+		s->r.width = inst->out_width;
+		s->r.height = inst->out_height;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	s->r.top = 0;
+	s->r.left = 0;
+
+	return 0;
+}
+
+static int
+venc_s_selection(struct file *file, void *fh, struct v4l2_selection *s)
+{
+	struct venus_inst *inst = to_inst(file);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		if (s->r.width != inst->out_width ||
+		    s->r.height != inst->out_height ||
+		    s->r.top != 0 || s->r.left != 0)
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int venc_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	struct venus_inst *inst = to_inst(file);
+	struct v4l2_outputparm *out = &a->parm.output;
+	struct v4l2_fract *timeperframe = &out->timeperframe;
+	u64 us_per_frame, fps;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+	    a->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return -EINVAL;
+
+	memset(out->reserved, 0, sizeof(out->reserved));
+
+	if (!timeperframe->denominator)
+		timeperframe->denominator = inst->timeperframe.denominator;
+	if (!timeperframe->numerator)
+		timeperframe->numerator = inst->timeperframe.numerator;
+
+	out->capability = V4L2_CAP_TIMEPERFRAME;
+
+	us_per_frame = timeperframe->numerator * (u64)USEC_PER_SEC;
+	do_div(us_per_frame, timeperframe->denominator);
+
+	if (!us_per_frame)
+		return -EINVAL;
+
+	fps = (u64)USEC_PER_SEC;
+	do_div(fps, us_per_frame);
+
+	inst->timeperframe = *timeperframe;
+	inst->fps = fps;
+
+	return 0;
+}
+
+static int venc_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	struct venus_inst *inst = to_inst(file);
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+	    a->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return -EINVAL;
+
+	a->parm.output.capability |= V4L2_CAP_TIMEPERFRAME;
+	a->parm.output.timeperframe = inst->timeperframe;
+
+	return 0;
+}
+
+static int venc_enum_framesizes(struct file *file, void *fh,
+				struct v4l2_frmsizeenum *fsize)
+{
+	struct venus_inst *inst = to_inst(file);
+	const struct venus_format *fmt;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+
+	fmt = find_format(fsize->pixel_format,
+			  V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (!fmt) {
+		fmt = find_format(fsize->pixel_format,
+				  V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+		if (!fmt)
+			return -EINVAL;
+	}
+
+	if (fsize->index)
+		return -EINVAL;
+
+	fsize->stepwise.min_width = inst->cap_width.min;
+	fsize->stepwise.max_width = inst->cap_width.max;
+	fsize->stepwise.step_width = inst->cap_width.step_size;
+	fsize->stepwise.min_height = inst->cap_height.min;
+	fsize->stepwise.max_height = inst->cap_height.max;
+	fsize->stepwise.step_height = inst->cap_height.step_size;
+
+	return 0;
+}
+
+static int venc_enum_frameintervals(struct file *file, void *fh,
+				    struct v4l2_frmivalenum *fival)
+{
+	struct venus_inst *inst = to_inst(file);
+	const struct venus_format *fmt;
+
+	fival->type = V4L2_FRMIVAL_TYPE_STEPWISE;
+
+	fmt = find_format(fival->pixel_format,
+			  V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (!fmt) {
+		fmt = find_format(fival->pixel_format,
+				  V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+		if (!fmt)
+			return -EINVAL;
+	}
+
+	if (fival->index)
+		return -EINVAL;
+
+	if (!fival->width || !fival->height)
+		return -EINVAL;
+
+	if (fival->width > inst->cap_width.max ||
+	    fival->width < inst->cap_width.min ||
+	    fival->height > inst->cap_height.max ||
+	    fival->height < inst->cap_height.min)
+		return -EINVAL;
+
+	fival->stepwise.min.numerator = 1;
+	fival->stepwise.min.denominator = inst->cap_framerate.max;
+	fival->stepwise.max.numerator = 1;
+	fival->stepwise.max.denominator = inst->cap_framerate.min;
+	fival->stepwise.step.numerator = 1;
+	fival->stepwise.step.denominator = inst->cap_framerate.max;
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops venc_ioctl_ops = {
+	.vidioc_querycap = venc_querycap,
+	.vidioc_enum_fmt_vid_cap_mplane = venc_enum_fmt,
+	.vidioc_enum_fmt_vid_out_mplane = venc_enum_fmt,
+	.vidioc_s_fmt_vid_cap_mplane = venc_s_fmt,
+	.vidioc_s_fmt_vid_out_mplane = venc_s_fmt,
+	.vidioc_g_fmt_vid_cap_mplane = venc_g_fmt,
+	.vidioc_g_fmt_vid_out_mplane = venc_g_fmt,
+	.vidioc_try_fmt_vid_cap_mplane = venc_try_fmt,
+	.vidioc_try_fmt_vid_out_mplane = venc_try_fmt,
+	.vidioc_g_selection = venc_g_selection,
+	.vidioc_s_selection = venc_s_selection,
+	.vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf = v4l2_m2m_ioctl_querybuf,
+	.vidioc_create_bufs = v4l2_m2m_ioctl_create_bufs,
+	.vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf,
+	.vidioc_qbuf = v4l2_m2m_ioctl_qbuf,
+	.vidioc_expbuf = v4l2_m2m_ioctl_expbuf,
+	.vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf,
+	.vidioc_streamon = v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff = v4l2_m2m_ioctl_streamoff,
+	.vidioc_s_parm = venc_s_parm,
+	.vidioc_g_parm = venc_g_parm,
+	.vidioc_enum_framesizes = venc_enum_framesizes,
+	.vidioc_enum_frameintervals = venc_enum_frameintervals,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static int venc_set_properties(struct venus_inst *inst)
+{
+	struct venc_controls *ctr = &inst->controls.enc;
+	struct hfi_intra_period intra_period;
+	struct hfi_profile_level pl;
+	struct hfi_framerate frate;
+	struct hfi_bitrate brate;
+	struct hfi_idr_period idrp;
+	u32 ptype, rate_control, bitrate, profile = 0, level = 0;
+	int ret;
+
+	ptype = HFI_PROPERTY_CONFIG_FRAME_RATE;
+	frate.buffer_type = HFI_BUFFER_OUTPUT;
+	frate.framerate = inst->fps * (1 << 16);
+
+	ret = hfi_session_set_property(inst, ptype, &frate);
+	if (ret)
+		return ret;
+
+	if (inst->fmt_cap->pixfmt == V4L2_PIX_FMT_H264) {
+		struct hfi_h264_vui_timing_info info;
+
+		ptype = HFI_PROPERTY_PARAM_VENC_H264_VUI_TIMING_INFO;
+		info.enable = 1;
+		info.fixed_framerate = 1;
+		info.time_scale = NSEC_PER_SEC;
+
+		ret = hfi_session_set_property(inst, ptype, &info);
+		if (ret)
+			return ret;
+	}
+
+	ptype = HFI_PROPERTY_CONFIG_VENC_IDR_PERIOD;
+	idrp.idr_period = ctr->gop_size;
+	ret = hfi_session_set_property(inst, ptype, &idrp);
+	if (ret)
+		return ret;
+
+	if (ctr->num_b_frames) {
+		u32 max_num_b_frames = NUM_B_FRAMES_MAX;
+
+		ptype = HFI_PROPERTY_PARAM_VENC_MAX_NUM_B_FRAMES;
+		ret = hfi_session_set_property(inst, ptype, &max_num_b_frames);
+		if (ret)
+			return ret;
+	}
+
+	/* intra_period = pframes + bframes + 1 */
+	if (!ctr->num_p_frames)
+		ctr->num_p_frames = 2 * 15 - 1,
+
+	ptype = HFI_PROPERTY_CONFIG_VENC_INTRA_PERIOD;
+	intra_period.pframes = ctr->num_p_frames;
+	intra_period.bframes = ctr->num_b_frames;
+
+	ret = hfi_session_set_property(inst, ptype, &intra_period);
+	if (ret)
+		return ret;
+
+	if (ctr->bitrate_mode == V4L2_MPEG_VIDEO_BITRATE_MODE_VBR)
+		rate_control = HFI_RATE_CONTROL_VBR_CFR;
+	else
+		rate_control = HFI_RATE_CONTROL_CBR_CFR;
+
+	ptype = HFI_PROPERTY_PARAM_VENC_RATE_CONTROL;
+	ret = hfi_session_set_property(inst, ptype, &rate_control);
+	if (ret)
+		return ret;
+
+	if (!ctr->bitrate)
+		bitrate = 64000;
+	else
+		bitrate = ctr->bitrate;
+
+	ptype = HFI_PROPERTY_CONFIG_VENC_TARGET_BITRATE;
+	brate.bitrate = bitrate;
+	brate.layer_id = 0;
+
+	ret = hfi_session_set_property(inst, ptype, &brate);
+	if (ret)
+		return ret;
+
+	if (!ctr->bitrate_peak)
+		bitrate *= 2;
+	else
+		bitrate = ctr->bitrate_peak;
+
+	ptype = HFI_PROPERTY_CONFIG_VENC_MAX_BITRATE;
+	brate.bitrate = bitrate;
+	brate.layer_id = 0;
+
+	ret = hfi_session_set_property(inst, ptype, &brate);
+	if (ret)
+		return ret;
+
+	if (inst->fmt_cap->pixfmt == V4L2_PIX_FMT_H264) {
+		profile = venc_v4l2_to_hfi(V4L2_CID_MPEG_VIDEO_H264_PROFILE,
+					   ctr->profile.h264);
+		level = venc_v4l2_to_hfi(V4L2_CID_MPEG_VIDEO_H264_LEVEL,
+					 ctr->level.h264);
+	} else if (inst->fmt_cap->pixfmt == V4L2_PIX_FMT_VP8) {
+		profile = venc_v4l2_to_hfi(V4L2_CID_MPEG_VIDEO_VPX_PROFILE,
+					   ctr->profile.vpx);
+		level = 0;
+	} else if (inst->fmt_cap->pixfmt == V4L2_PIX_FMT_MPEG4) {
+		profile = venc_v4l2_to_hfi(V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE,
+					   ctr->profile.mpeg4);
+		level = venc_v4l2_to_hfi(V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL,
+					 ctr->level.mpeg4);
+	} else if (inst->fmt_cap->pixfmt == V4L2_PIX_FMT_H263) {
+		profile = 0;
+		level = 0;
+	}
+
+	ptype = HFI_PROPERTY_PARAM_PROFILE_LEVEL_CURRENT;
+	pl.profile = profile;
+	pl.level = level;
+
+	ret = hfi_session_set_property(inst, ptype, &pl);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int venc_init_session(struct venus_inst *inst)
+{
+	int ret;
+
+	ret = hfi_session_init(inst, inst->fmt_cap->pixfmt);
+	if (ret)
+		return ret;
+
+	ret = venus_helper_set_input_resolution(inst, inst->out_width,
+						inst->out_height);
+	if (ret)
+		goto deinit;
+
+	ret = venus_helper_set_output_resolution(inst, inst->width,
+						 inst->height);
+	if (ret)
+		goto deinit;
+
+	ret = venus_helper_set_color_format(inst, inst->fmt_out->pixfmt);
+	if (ret)
+		goto deinit;
+
+	return 0;
+deinit:
+	hfi_session_deinit(inst);
+	return ret;
+}
+
+static int venc_out_num_buffers(struct venus_inst *inst, unsigned int *num)
+{
+	struct hfi_buffer_requirements bufreq;
+	int ret;
+
+	ret = venc_init_session(inst);
+	if (ret)
+		return ret;
+
+	ret = venus_helper_get_bufreq(inst, HFI_BUFFER_INPUT, &bufreq);
+
+	*num = bufreq.count_actual;
+
+	hfi_session_deinit(inst);
+
+	return ret;
+}
+
+static int venc_queue_setup(struct vb2_queue *q,
+			    unsigned int *num_buffers, unsigned int *num_planes,
+			    unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct venus_inst *inst = vb2_get_drv_priv(q);
+	unsigned int p, num, min = 4;
+	int ret = 0;
+
+	if (*num_planes) {
+		if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
+		    *num_planes != inst->fmt_out->num_planes)
+			return -EINVAL;
+
+		if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+		    *num_planes != inst->fmt_cap->num_planes)
+			return -EINVAL;
+
+		if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
+		    sizes[0] < inst->input_buf_size)
+			return -EINVAL;
+
+		if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+		    sizes[0] < inst->output_buf_size)
+			return -EINVAL;
+
+		return 0;
+	}
+
+	switch (q->type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		*num_planes = inst->fmt_out->num_planes;
+
+		ret = venc_out_num_buffers(inst, &num);
+		if (ret)
+			break;
+
+		num = max(num, min);
+		*num_buffers = max(*num_buffers, num);
+		inst->num_input_bufs = *num_buffers;
+
+		for (p = 0; p < *num_planes; ++p)
+			sizes[p] = get_framesize_uncompressed(p, inst->width,
+							      inst->height);
+		inst->input_buf_size = sizes[0];
+		break;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		*num_planes = inst->fmt_cap->num_planes;
+		*num_buffers = max(*num_buffers, min);
+		inst->num_output_bufs = *num_buffers;
+		sizes[0] = get_framesize_compressed(inst->width, inst->height);
+		inst->output_buf_size = sizes[0];
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int venc_verify_conf(struct venus_inst *inst)
+{
+	struct hfi_buffer_requirements bufreq;
+	int ret;
+
+	if (!inst->num_input_bufs || !inst->num_output_bufs)
+		return -EINVAL;
+
+	ret = venus_helper_get_bufreq(inst, HFI_BUFFER_OUTPUT, &bufreq);
+	if (ret)
+		return ret;
+
+	if (inst->num_output_bufs < bufreq.count_actual ||
+	    inst->num_output_bufs < bufreq.count_min)
+		return -EINVAL;
+
+	ret = venus_helper_get_bufreq(inst, HFI_BUFFER_INPUT, &bufreq);
+	if (ret)
+		return ret;
+
+	if (inst->num_input_bufs < bufreq.count_actual ||
+	    inst->num_input_bufs < bufreq.count_min)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int venc_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct venus_inst *inst = vb2_get_drv_priv(q);
+	int ret;
+
+	mutex_lock(&inst->lock);
+
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		inst->streamon_out = 1;
+	else
+		inst->streamon_cap = 1;
+
+	if (!(inst->streamon_out & inst->streamon_cap)) {
+		mutex_unlock(&inst->lock);
+		return 0;
+	}
+
+	venus_helper_init_instance(inst);
+
+	inst->sequence_cap = 0;
+	inst->sequence_out = 0;
+
+	ret = venc_init_session(inst);
+	if (ret)
+		goto bufs_done;
+
+	ret = venc_set_properties(inst);
+	if (ret)
+		goto deinit_sess;
+
+	ret = venc_verify_conf(inst);
+	if (ret)
+		goto deinit_sess;
+
+	ret = venus_helper_set_num_bufs(inst, inst->num_input_bufs,
+					inst->num_output_bufs);
+	if (ret)
+		goto deinit_sess;
+
+	ret = venus_helper_vb2_start_streaming(inst);
+	if (ret)
+		goto deinit_sess;
+
+	mutex_unlock(&inst->lock);
+
+	return 0;
+
+deinit_sess:
+	hfi_session_deinit(inst);
+bufs_done:
+	venus_helper_buffers_done(inst, VB2_BUF_STATE_QUEUED);
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		inst->streamon_out = 0;
+	else
+		inst->streamon_cap = 0;
+	mutex_unlock(&inst->lock);
+	return ret;
+}
+
+static const struct vb2_ops venc_vb2_ops = {
+	.queue_setup = venc_queue_setup,
+	.buf_init = venus_helper_vb2_buf_init,
+	.buf_prepare = venus_helper_vb2_buf_prepare,
+	.start_streaming = venc_start_streaming,
+	.stop_streaming = venus_helper_vb2_stop_streaming,
+	.buf_queue = venus_helper_vb2_buf_queue,
+};
+
+static void venc_buf_done(struct venus_inst *inst, unsigned int buf_type,
+			  u32 tag, u32 bytesused, u32 data_offset, u32 flags,
+			  u32 hfi_flags, u64 timestamp_us)
+{
+	struct vb2_v4l2_buffer *vbuf;
+	struct vb2_buffer *vb;
+	unsigned int type;
+
+	if (buf_type == HFI_BUFFER_INPUT)
+		type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	else
+		type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+
+	vbuf = venus_helper_find_buf(inst, type, tag);
+	if (!vbuf)
+		return;
+
+	vb = &vbuf->vb2_buf;
+	vb->planes[0].bytesused = bytesused;
+	vb->planes[0].data_offset = data_offset;
+
+	vbuf->flags = flags;
+
+	if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		vb->timestamp = timestamp_us * NSEC_PER_USEC;
+		vbuf->sequence = inst->sequence_cap++;
+	} else {
+		vbuf->sequence = inst->sequence_out++;
+	}
+
+	v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_DONE);
+}
+
+static void venc_event_notify(struct venus_inst *inst, u32 event,
+			      struct hfi_event_data *data)
+{
+	struct device *dev = inst->core->dev_enc;
+
+	if (event == EVT_SESSION_ERROR) {
+		inst->session_error = true;
+		dev_err(dev, "enc: event session error %x\n", inst->error);
+	}
+}
+
+static const struct hfi_inst_ops venc_hfi_ops = {
+	.buf_done = venc_buf_done,
+	.event_notify = venc_event_notify,
+};
+
+static const struct v4l2_m2m_ops venc_m2m_ops = {
+	.device_run = venus_helper_m2m_device_run,
+	.job_abort = venus_helper_m2m_job_abort,
+};
+
+static int m2m_queue_init(void *priv, struct vb2_queue *src_vq,
+			  struct vb2_queue *dst_vq)
+{
+	struct venus_inst *inst = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->ops = &venc_vb2_ops;
+	src_vq->mem_ops = &vb2_dma_sg_memops;
+	src_vq->drv_priv = inst;
+	src_vq->buf_struct_size = sizeof(struct venus_buffer);
+	src_vq->allow_zero_bytesused = 1;
+	src_vq->min_buffers_needed = 1;
+	src_vq->dev = inst->core->dev;
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->ops = &venc_vb2_ops;
+	dst_vq->mem_ops = &vb2_dma_sg_memops;
+	dst_vq->drv_priv = inst;
+	dst_vq->buf_struct_size = sizeof(struct venus_buffer);
+	dst_vq->allow_zero_bytesused = 1;
+	dst_vq->min_buffers_needed = 1;
+	dst_vq->dev = inst->core->dev;
+	ret = vb2_queue_init(dst_vq);
+	if (ret) {
+		vb2_queue_release(src_vq);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void venc_inst_init(struct venus_inst *inst)
+{
+	inst->fmt_cap = &venc_formats[2];
+	inst->fmt_out = &venc_formats[0];
+	inst->width = 1280;
+	inst->height = ALIGN(720, 32);
+	inst->out_width = 1280;
+	inst->out_height = 720;
+	inst->fps = 15;
+	inst->timeperframe.numerator = 1;
+	inst->timeperframe.denominator = 15;
+
+	inst->cap_width.min = 96;
+	inst->cap_width.max = 1920;
+	if (inst->core->res->hfi_version == HFI_VERSION_3XX)
+		inst->cap_width.max = 3840;
+	inst->cap_width.step_size = 2;
+	inst->cap_height.min = 64;
+	inst->cap_height.max = ALIGN(1080, 32);
+	if (inst->core->res->hfi_version == HFI_VERSION_3XX)
+		inst->cap_height.max = ALIGN(2160, 32);
+	inst->cap_height.step_size = 2;
+	inst->cap_framerate.min = 1;
+	inst->cap_framerate.max = 30;
+	inst->cap_framerate.step_size = 1;
+	inst->cap_mbs_per_frame.min = 24;
+	inst->cap_mbs_per_frame.max = 8160;
+}
+
+static int venc_open(struct file *file)
+{
+	struct venus_core *core = video_drvdata(file);
+	struct venus_inst *inst;
+	int ret;
+
+	inst = kzalloc(sizeof(*inst), GFP_KERNEL);
+	if (!inst)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&inst->registeredbufs);
+	INIT_LIST_HEAD(&inst->internalbufs);
+	INIT_LIST_HEAD(&inst->list);
+	mutex_init(&inst->lock);
+
+	inst->core = core;
+	inst->session_type = VIDC_SESSION_TYPE_ENC;
+
+	venus_helper_init_instance(inst);
+
+	ret = pm_runtime_get_sync(core->dev_enc);
+	if (ret < 0)
+		goto err_free_inst;
+
+	ret = venc_ctrl_init(inst);
+	if (ret)
+		goto err_put_sync;
+
+	ret = hfi_session_create(inst, &venc_hfi_ops);
+	if (ret)
+		goto err_ctrl_deinit;
+
+	venc_inst_init(inst);
+
+	/*
+	 * create m2m device for every instance, the m2m context scheduling
+	 * is made by firmware side so we do not need to care about.
+	 */
+	inst->m2m_dev = v4l2_m2m_init(&venc_m2m_ops);
+	if (IS_ERR(inst->m2m_dev)) {
+		ret = PTR_ERR(inst->m2m_dev);
+		goto err_session_destroy;
+	}
+
+	inst->m2m_ctx = v4l2_m2m_ctx_init(inst->m2m_dev, inst, m2m_queue_init);
+	if (IS_ERR(inst->m2m_ctx)) {
+		ret = PTR_ERR(inst->m2m_ctx);
+		goto err_m2m_release;
+	}
+
+	v4l2_fh_init(&inst->fh, core->vdev_enc);
+
+	inst->fh.ctrl_handler = &inst->ctrl_handler;
+	v4l2_fh_add(&inst->fh);
+	inst->fh.m2m_ctx = inst->m2m_ctx;
+	file->private_data = &inst->fh;
+
+	return 0;
+
+err_m2m_release:
+	v4l2_m2m_release(inst->m2m_dev);
+err_session_destroy:
+	hfi_session_destroy(inst);
+err_ctrl_deinit:
+	venc_ctrl_deinit(inst);
+err_put_sync:
+	pm_runtime_put_sync(core->dev_enc);
+err_free_inst:
+	kfree(inst);
+	return ret;
+}
+
+static int venc_close(struct file *file)
+{
+	struct venus_inst *inst = to_inst(file);
+
+	v4l2_m2m_ctx_release(inst->m2m_ctx);
+	v4l2_m2m_release(inst->m2m_dev);
+	venc_ctrl_deinit(inst);
+	hfi_session_destroy(inst);
+	mutex_destroy(&inst->lock);
+	v4l2_fh_del(&inst->fh);
+	v4l2_fh_exit(&inst->fh);
+
+	pm_runtime_put_sync(inst->core->dev_enc);
+
+	kfree(inst);
+	return 0;
+}
+
+static const struct v4l2_file_operations venc_fops = {
+	.owner = THIS_MODULE,
+	.open = venc_open,
+	.release = venc_close,
+	.unlocked_ioctl = video_ioctl2,
+	.poll = v4l2_m2m_fop_poll,
+	.mmap = v4l2_m2m_fop_mmap,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl32 = v4l2_compat_ioctl32,
+#endif
+};
+
+static int venc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct video_device *vdev;
+	struct venus_core *core;
+	int ret;
+
+	if (!dev->parent)
+		return -EPROBE_DEFER;
+
+	core = dev_get_drvdata(dev->parent);
+	if (!core)
+		return -EPROBE_DEFER;
+
+	if (core->res->hfi_version == HFI_VERSION_3XX) {
+		core->core1_clk = devm_clk_get(dev, "core");
+		if (IS_ERR(core->core1_clk))
+			return PTR_ERR(core->core1_clk);
+	}
+
+	platform_set_drvdata(pdev, core);
+
+	vdev = video_device_alloc();
+	if (!vdev)
+		return -ENOMEM;
+
+	vdev->release = video_device_release;
+	vdev->fops = &venc_fops;
+	vdev->ioctl_ops = &venc_ioctl_ops;
+	vdev->vfl_dir = VFL_DIR_M2M;
+	vdev->v4l2_dev = &core->v4l2_dev;
+	vdev->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING;
+
+	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+	if (ret)
+		goto err_vdev_release;
+
+	core->vdev_enc = vdev;
+	core->dev_enc = dev;
+
+	video_set_drvdata(vdev, core);
+	pm_runtime_enable(dev);
+
+	return 0;
+
+err_vdev_release:
+	video_device_release(vdev);
+	return ret;
+}
+
+static int venc_remove(struct platform_device *pdev)
+{
+	struct venus_core *core = dev_get_drvdata(pdev->dev.parent);
+
+	video_unregister_device(core->vdev_enc);
+	pm_runtime_disable(core->dev_enc);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int venc_runtime_suspend(struct device *dev)
+{
+	struct venus_core *core = dev_get_drvdata(dev);
+
+	if (core->res->hfi_version == HFI_VERSION_1XX)
+		return 0;
+
+	writel(0, core->base + WRAPPER_VENC_VCODEC_POWER_CONTROL);
+	clk_disable_unprepare(core->core1_clk);
+	writel(1, core->base + WRAPPER_VENC_VCODEC_POWER_CONTROL);
+
+	return 0;
+}
+
+static int venc_runtime_resume(struct device *dev)
+{
+	struct venus_core *core = dev_get_drvdata(dev);
+	int ret;
+
+	if (core->res->hfi_version == HFI_VERSION_1XX)
+		return 0;
+
+	writel(0, core->base + WRAPPER_VENC_VCODEC_POWER_CONTROL);
+	ret = clk_prepare_enable(core->core1_clk);
+	writel(1, core->base + WRAPPER_VENC_VCODEC_POWER_CONTROL);
+
+	return ret;
+}
+#endif
+
+static const struct dev_pm_ops venc_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(venc_runtime_suspend, venc_runtime_resume, NULL)
+};
+
+static const struct of_device_id venc_dt_match[] = {
+	{ .compatible = "venus-encoder" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, venc_dt_match);
+
+static struct platform_driver qcom_venus_enc_driver = {
+	.probe = venc_probe,
+	.remove = venc_remove,
+	.driver = {
+		.name = "qcom-venus-encoder",
+		.of_match_table = venc_dt_match,
+		.pm = &venc_pm_ops,
+	},
+};
+module_platform_driver(qcom_venus_enc_driver);
+
+MODULE_ALIAS("platform:qcom-venus-encoder");
+MODULE_DESCRIPTION("Qualcomm Venus video encoder driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/qcom/venus/venc.h b/drivers/media/platform/qcom/venus/venc.h
new file mode 100644
index 000000000000..9daca669f307
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/venc.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __VENUS_VENC_H__
+#define __VENUS_VENC_H__
+
+struct venus_inst;
+
+int venc_ctrl_init(struct venus_inst *inst);
+void venc_ctrl_deinit(struct venus_inst *inst);
+
+#endif
diff --git a/drivers/media/platform/qcom/venus/venc_ctrls.c b/drivers/media/platform/qcom/venus/venc_ctrls.c
new file mode 100644
index 000000000000..ab0fe51ff0f7
--- /dev/null
+++ b/drivers/media/platform/qcom/venus/venc_ctrls.c
@@ -0,0 +1,270 @@
+/*
+ * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/types.h>
+#include <media/v4l2-ctrls.h>
+
+#include "core.h"
+#include "venc.h"
+
+#define BITRATE_MIN		32000
+#define BITRATE_MAX		160000000
+#define BITRATE_DEFAULT		1000000
+#define BITRATE_DEFAULT_PEAK	(BITRATE_DEFAULT * 2)
+#define BITRATE_STEP		100
+#define SLICE_BYTE_SIZE_MAX	1024
+#define SLICE_BYTE_SIZE_MIN	1024
+#define SLICE_MB_SIZE_MAX	300
+#define INTRA_REFRESH_MBS_MAX	300
+#define AT_SLICE_BOUNDARY	\
+	V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_DISABLED_AT_SLICE_BOUNDARY
+
+static int venc_op_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct venus_inst *inst = ctrl_to_inst(ctrl);
+	struct venc_controls *ctr = &inst->controls.enc;
+
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
+		ctr->bitrate_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		ctr->bitrate = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE_PEAK:
+		ctr->bitrate_peak = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE:
+		ctr->h264_entropy_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
+		ctr->profile.mpeg4 = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
+		ctr->profile.h264 = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_PROFILE:
+		ctr->profile.vpx = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
+		ctr->level.mpeg4 = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
+		ctr->level.h264 = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP:
+		ctr->h264_i_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP:
+		ctr->h264_p_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_B_FRAME_QP:
+		ctr->h264_b_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:
+		ctr->h264_min_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_MAX_QP:
+		ctr->h264_max_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE:
+		ctr->multi_slice_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES:
+		ctr->multi_slice_max_bytes = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB:
+		ctr->multi_slice_max_mb = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA:
+		ctr->h264_loop_filter_alpha = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA:
+		ctr->h264_loop_filter_beta = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE:
+		ctr->h264_loop_filter_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_HEADER_MODE:
+		ctr->header_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB:
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		ctr->gop_size = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_I_PERIOD:
+		ctr->h264_i_period = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_MIN_QP:
+		ctr->vp8_min_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_MAX_QP:
+		ctr->vp8_max_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_B_FRAMES:
+		ctr->num_b_frames = ctrl->val;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops venc_ctrl_ops = {
+	.s_ctrl = venc_op_s_ctrl,
+};
+
+int venc_ctrl_init(struct venus_inst *inst)
+{
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(&inst->ctrl_handler, 27);
+	if (ret)
+		return ret;
+
+	v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
+		V4L2_MPEG_VIDEO_BITRATE_MODE_CBR,
+		~((1 << V4L2_MPEG_VIDEO_BITRATE_MODE_VBR) |
+		  (1 << V4L2_MPEG_VIDEO_BITRATE_MODE_CBR)),
+		V4L2_MPEG_VIDEO_BITRATE_MODE_VBR);
+
+	v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE,
+		V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC,
+		0, V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CAVLC);
+
+	v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE,
+		V4L2_MPEG_VIDEO_MPEG4_PROFILE_ADVANCED_CODING_EFFICIENCY,
+		~((1 << V4L2_MPEG_VIDEO_MPEG4_PROFILE_SIMPLE) |
+		  (1 << V4L2_MPEG_VIDEO_MPEG4_PROFILE_ADVANCED_SIMPLE)),
+		V4L2_MPEG_VIDEO_MPEG4_PROFILE_SIMPLE);
+
+	v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL,
+		V4L2_MPEG_VIDEO_MPEG4_LEVEL_5,
+		0, V4L2_MPEG_VIDEO_MPEG4_LEVEL_0);
+
+	v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_PROFILE,
+		V4L2_MPEG_VIDEO_H264_PROFILE_MULTIVIEW_HIGH,
+		~((1 << V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
+		  (1 << V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE) |
+		  (1 << V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) |
+		  (1 << V4L2_MPEG_VIDEO_H264_PROFILE_HIGH) |
+		  (1 << V4L2_MPEG_VIDEO_H264_PROFILE_STEREO_HIGH) |
+		  (1 << V4L2_MPEG_VIDEO_H264_PROFILE_MULTIVIEW_HIGH)),
+		V4L2_MPEG_VIDEO_H264_PROFILE_HIGH);
+
+	v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_LEVEL,
+		V4L2_MPEG_VIDEO_H264_LEVEL_5_1,
+		0, V4L2_MPEG_VIDEO_H264_LEVEL_1_0);
+
+	v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE,
+		AT_SLICE_BOUNDARY,
+		0, V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_DISABLED);
+
+	v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_HEADER_MODE,
+		V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME,
+		1 << V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME,
+		V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE);
+
+	v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE,
+		V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES,
+		0, V4L2_MPEG_VIDEO_MULTI_SLICE_MODE_SINGLE);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE, BITRATE_MIN, BITRATE_MAX,
+		BITRATE_STEP, BITRATE_DEFAULT);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE_PEAK, BITRATE_MIN, BITRATE_MAX,
+		BITRATE_STEP, BITRATE_DEFAULT_PEAK);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_VPX_PROFILE, 0, 3, 1, 0);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP, 1, 51, 1, 26);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP, 1, 51, 1, 28);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_B_FRAME_QP, 1, 51, 1, 30);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_MIN_QP, 1, 51, 1, 1);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_MAX_QP, 1, 51, 1, 51);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES, SLICE_BYTE_SIZE_MIN,
+		SLICE_BYTE_SIZE_MAX, 1, SLICE_BYTE_SIZE_MIN);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB, 1,
+		SLICE_MB_SIZE_MAX, 1, 1);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA, -6, 6, 1, 0);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA, -6, 6, 1, 0);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB,
+		0, INTRA_REFRESH_MBS_MAX, 1, 0);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_GOP_SIZE, 0, (1 << 16) - 1, 1, 12);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_VPX_MIN_QP, 1, 128, 1, 1);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_VPX_MAX_QP, 1, 128, 1, 128);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_B_FRAMES, 0, 4, 1, 0);
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_I_PERIOD, 0, (1 << 16) - 1, 1, 0);
+
+	ret = inst->ctrl_handler.error;
+	if (ret)
+		goto err;
+
+	ret = v4l2_ctrl_handler_setup(&inst->ctrl_handler);
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	v4l2_ctrl_handler_free(&inst->ctrl_handler);
+	return ret;
+}
+
+void venc_ctrl_deinit(struct venus_inst *inst)
+{
+	v4l2_ctrl_handler_free(&inst->ctrl_handler);
+}
diff --git a/drivers/media/platform/rcar-vin/Kconfig b/drivers/media/platform/rcar-vin/Kconfig
index 111d2a151f6a..af4c98b44d2e 100644
--- a/drivers/media/platform/rcar-vin/Kconfig
+++ b/drivers/media/platform/rcar-vin/Kconfig
@@ -3,6 +3,7 @@ config VIDEO_RCAR_VIN
 	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && OF && HAS_DMA && MEDIA_CONTROLLER
 	depends on ARCH_RENESAS || COMPILE_TEST
 	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_FWNODE
 	---help---
 	  Support for Renesas R-Car Video Input (VIN) driver.
 	  Supports R-Car Gen2 SoCs.
diff --git a/drivers/media/platform/rcar-vin/rcar-core.c b/drivers/media/platform/rcar-vin/rcar-core.c
index 098a0b1cc10a..77dff047c41c 100644
--- a/drivers/media/platform/rcar-vin/rcar-core.c
+++ b/drivers/media/platform/rcar-vin/rcar-core.c
@@ -21,7 +21,7 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 
 #include "rcar-vin.h"
 
@@ -31,6 +31,20 @@
 
 #define notifier_to_vin(n) container_of(n, struct rvin_dev, notifier)
 
+static int rvin_find_pad(struct v4l2_subdev *sd, int direction)
+{
+	unsigned int pad;
+
+	if (sd->entity.num_pads <= 1)
+		return 0;
+
+	for (pad = 0; pad < sd->entity.num_pads; pad++)
+		if (sd->entity.pads[pad].flags & direction)
+			return pad;
+
+	return -EINVAL;
+}
+
 static bool rvin_mbus_supported(struct rvin_graph_entity *entity)
 {
 	struct v4l2_subdev *sd = entity->subdev;
@@ -39,6 +53,7 @@ static bool rvin_mbus_supported(struct rvin_graph_entity *entity)
 	};
 
 	code.index = 0;
+	code.pad = entity->source_pad;
 	while (!v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code)) {
 		code.index++;
 		switch (code.code) {
@@ -86,14 +101,9 @@ static void rvin_digital_notify_unbind(struct v4l2_async_notifier *notifier,
 {
 	struct rvin_dev *vin = notifier_to_vin(notifier);
 
-	if (vin->digital.subdev == subdev) {
-		vin_dbg(vin, "unbind digital subdev %s\n", subdev->name);
-		rvin_v4l2_remove(vin);
-		vin->digital.subdev = NULL;
-		return;
-	}
-
-	vin_err(vin, "no entity for subdev %s to unbind\n", subdev->name);
+	vin_dbg(vin, "unbind digital subdev %s\n", subdev->name);
+	rvin_v4l2_remove(vin);
+	vin->digital.subdev = NULL;
 }
 
 static int rvin_digital_notify_bound(struct v4l2_async_notifier *notifier,
@@ -101,27 +111,37 @@ static int rvin_digital_notify_bound(struct v4l2_async_notifier *notifier,
 				     struct v4l2_async_subdev *asd)
 {
 	struct rvin_dev *vin = notifier_to_vin(notifier);
+	int ret;
 
 	v4l2_set_subdev_hostdata(subdev, vin);
 
-	if (vin->digital.asd.match.of.node == subdev->dev->of_node) {
-		vin_dbg(vin, "bound digital subdev %s\n", subdev->name);
-		vin->digital.subdev = subdev;
-		return 0;
-	}
+	/* Find source and sink pad of remote subdevice */
 
-	vin_err(vin, "no entity for subdev %s to bind\n", subdev->name);
-	return -EINVAL;
+	ret = rvin_find_pad(subdev, MEDIA_PAD_FL_SOURCE);
+	if (ret < 0)
+		return ret;
+	vin->digital.source_pad = ret;
+
+	ret = rvin_find_pad(subdev, MEDIA_PAD_FL_SINK);
+	vin->digital.sink_pad = ret < 0 ? 0 : ret;
+
+	vin->digital.subdev = subdev;
+
+	vin_dbg(vin, "bound subdev %s source pad: %u sink pad: %u\n",
+		subdev->name, vin->digital.source_pad,
+		vin->digital.sink_pad);
+
+	return 0;
 }
 
 static int rvin_digitial_parse_v4l2(struct rvin_dev *vin,
 				    struct device_node *ep,
 				    struct v4l2_mbus_config *mbus_cfg)
 {
-	struct v4l2_of_endpoint v4l2_ep;
+	struct v4l2_fwnode_endpoint v4l2_ep;
 	int ret;
 
-	ret = v4l2_of_parse_endpoint(ep, &v4l2_ep);
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &v4l2_ep);
 	if (ret) {
 		vin_err(vin, "Could not parse v4l2 endpoint\n");
 		return -EINVAL;
@@ -151,7 +171,7 @@ static int rvin_digital_graph_parse(struct rvin_dev *vin)
 	struct device_node *ep, *np;
 	int ret;
 
-	vin->digital.asd.match.of.node = NULL;
+	vin->digital.asd.match.fwnode.fwnode = NULL;
 	vin->digital.subdev = NULL;
 
 	/*
@@ -175,8 +195,8 @@ static int rvin_digital_graph_parse(struct rvin_dev *vin)
 	if (ret)
 		return ret;
 
-	vin->digital.asd.match.of.node = np;
-	vin->digital.asd.match_type = V4L2_ASYNC_MATCH_OF;
+	vin->digital.asd.match.fwnode.fwnode = of_fwnode_handle(np);
+	vin->digital.asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
 
 	return 0;
 }
@@ -190,7 +210,7 @@ static int rvin_digital_graph_init(struct rvin_dev *vin)
 	if (ret)
 		return ret;
 
-	if (!vin->digital.asd.match.of.node) {
+	if (!vin->digital.asd.match.fwnode.fwnode) {
 		vin_dbg(vin, "No digital subdevice found\n");
 		return -ENODEV;
 	}
@@ -203,7 +223,7 @@ static int rvin_digital_graph_init(struct rvin_dev *vin)
 	subdevs[0] = &vin->digital.asd;
 
 	vin_dbg(vin, "Found digital subdevice %s\n",
-		of_node_full_name(subdevs[0]->match.of.node));
+		of_node_full_name(to_of_node(subdevs[0]->match.fwnode.fwnode)));
 
 	vin->notifier.num_subdevs = 1;
 	vin->notifier.subdevs = subdevs;
diff --git a/drivers/media/platform/rcar-vin/rcar-dma.c b/drivers/media/platform/rcar-vin/rcar-dma.c
index 9ccd5ff55e19..b136844499f6 100644
--- a/drivers/media/platform/rcar-vin/rcar-dma.c
+++ b/drivers/media/platform/rcar-vin/rcar-dma.c
@@ -119,6 +119,15 @@
 #define VNDMR2_FTEV		(1 << 17)
 #define VNDMR2_VLV(n)		((n & 0xf) << 12)
 
+struct rvin_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+};
+
+#define to_buf_list(vb2_buffer) (&container_of(vb2_buffer, \
+					       struct rvin_buffer, \
+					       vb)->list)
+
 static void rvin_write(struct rvin_dev *vin, u32 value, u32 offset)
 {
 	iowrite32(value, vin->base + offset);
@@ -269,48 +278,6 @@ static int rvin_setup(struct rvin_dev *vin)
 	return 0;
 }
 
-static void rvin_capture_on(struct rvin_dev *vin)
-{
-	vin_dbg(vin, "Capture on in %s mode\n",
-		vin->continuous ? "continuous" : "single");
-
-	if (vin->continuous)
-		/* Continuous Frame Capture Mode */
-		rvin_write(vin, VNFC_C_FRAME, VNFC_REG);
-	else
-		/* Single Frame Capture Mode */
-		rvin_write(vin, VNFC_S_FRAME, VNFC_REG);
-}
-
-static void rvin_capture_off(struct rvin_dev *vin)
-{
-	/* Set continuous & single transfer off */
-	rvin_write(vin, 0, VNFC_REG);
-}
-
-static int rvin_capture_start(struct rvin_dev *vin)
-{
-	int ret;
-
-	rvin_crop_scale_comp(vin);
-
-	ret = rvin_setup(vin);
-	if (ret)
-		return ret;
-
-	rvin_capture_on(vin);
-
-	return 0;
-}
-
-static void rvin_capture_stop(struct rvin_dev *vin)
-{
-	rvin_capture_off(vin);
-
-	/* Disable module */
-	rvin_write(vin, rvin_read(vin, VNMC_REG) & ~VNMC_ME, VNMC_REG);
-}
-
 static void rvin_disable_interrupts(struct rvin_dev *vin)
 {
 	rvin_write(vin, 0, VNIE_REG);
@@ -377,6 +344,99 @@ static void rvin_set_slot_addr(struct rvin_dev *vin, int slot, dma_addr_t addr)
 	rvin_write(vin, offset, VNMB_REG(slot));
 }
 
+/* Moves a buffer from the queue to the HW slots */
+static bool rvin_fill_hw_slot(struct rvin_dev *vin, int slot)
+{
+	struct rvin_buffer *buf;
+	struct vb2_v4l2_buffer *vbuf;
+	dma_addr_t phys_addr_top;
+
+	if (vin->queue_buf[slot] != NULL)
+		return true;
+
+	if (list_empty(&vin->buf_list))
+		return false;
+
+	vin_dbg(vin, "Filling HW slot: %d\n", slot);
+
+	/* Keep track of buffer we give to HW */
+	buf = list_entry(vin->buf_list.next, struct rvin_buffer, list);
+	vbuf = &buf->vb;
+	list_del_init(to_buf_list(vbuf));
+	vin->queue_buf[slot] = vbuf;
+
+	/* Setup DMA */
+	phys_addr_top = vb2_dma_contig_plane_dma_addr(&vbuf->vb2_buf, 0);
+	rvin_set_slot_addr(vin, slot, phys_addr_top);
+
+	return true;
+}
+
+static bool rvin_fill_hw(struct rvin_dev *vin)
+{
+	int slot, limit;
+
+	limit = vin->continuous ? HW_BUFFER_NUM : 1;
+
+	for (slot = 0; slot < limit; slot++)
+		if (!rvin_fill_hw_slot(vin, slot))
+			return false;
+	return true;
+}
+
+static void rvin_capture_on(struct rvin_dev *vin)
+{
+	vin_dbg(vin, "Capture on in %s mode\n",
+		vin->continuous ? "continuous" : "single");
+
+	if (vin->continuous)
+		/* Continuous Frame Capture Mode */
+		rvin_write(vin, VNFC_C_FRAME, VNFC_REG);
+	else
+		/* Single Frame Capture Mode */
+		rvin_write(vin, VNFC_S_FRAME, VNFC_REG);
+}
+
+static int rvin_capture_start(struct rvin_dev *vin)
+{
+	struct rvin_buffer *buf, *node;
+	int bufs, ret;
+
+	/* Count number of free buffers */
+	bufs = 0;
+	list_for_each_entry_safe(buf, node, &vin->buf_list, list)
+		bufs++;
+
+	/* Continuous capture requires more buffers then there are HW slots */
+	vin->continuous = bufs > HW_BUFFER_NUM;
+
+	if (!rvin_fill_hw(vin)) {
+		vin_err(vin, "HW not ready to start, not enough buffers available\n");
+		return -EINVAL;
+	}
+
+	rvin_crop_scale_comp(vin);
+
+	ret = rvin_setup(vin);
+	if (ret)
+		return ret;
+
+	rvin_capture_on(vin);
+
+	vin->state = RUNNING;
+
+	return 0;
+}
+
+static void rvin_capture_stop(struct rvin_dev *vin)
+{
+	/* Set continuous & single transfer off */
+	rvin_write(vin, 0, VNFC_REG);
+
+	/* Disable module */
+	rvin_write(vin, rvin_read(vin, VNMC_REG) & ~VNMC_ME, VNMC_REG);
+}
+
 /* -----------------------------------------------------------------------------
  * Crop and Scaling Gen2
  */
@@ -839,61 +899,12 @@ void rvin_scale_try(struct rvin_dev *vin, struct v4l2_pix_format *pix,
 #define RVIN_TIMEOUT_MS 100
 #define RVIN_RETRIES 10
 
-struct rvin_buffer {
-	struct vb2_v4l2_buffer vb;
-	struct list_head list;
-};
-
-#define to_buf_list(vb2_buffer) (&container_of(vb2_buffer, \
-					       struct rvin_buffer, \
-					       vb)->list)
-
-/* Moves a buffer from the queue to the HW slots */
-static bool rvin_fill_hw_slot(struct rvin_dev *vin, int slot)
-{
-	struct rvin_buffer *buf;
-	struct vb2_v4l2_buffer *vbuf;
-	dma_addr_t phys_addr_top;
-
-	if (vin->queue_buf[slot] != NULL)
-		return true;
-
-	if (list_empty(&vin->buf_list))
-		return false;
-
-	vin_dbg(vin, "Filling HW slot: %d\n", slot);
-
-	/* Keep track of buffer we give to HW */
-	buf = list_entry(vin->buf_list.next, struct rvin_buffer, list);
-	vbuf = &buf->vb;
-	list_del_init(to_buf_list(vbuf));
-	vin->queue_buf[slot] = vbuf;
-
-	/* Setup DMA */
-	phys_addr_top = vb2_dma_contig_plane_dma_addr(&vbuf->vb2_buf, 0);
-	rvin_set_slot_addr(vin, slot, phys_addr_top);
-
-	return true;
-}
-
-static bool rvin_fill_hw(struct rvin_dev *vin)
-{
-	int slot, limit;
-
-	limit = vin->continuous ? HW_BUFFER_NUM : 1;
-
-	for (slot = 0; slot < limit; slot++)
-		if (!rvin_fill_hw_slot(vin, slot))
-			return false;
-	return true;
-}
-
 static irqreturn_t rvin_irq(int irq, void *data)
 {
 	struct rvin_dev *vin = data;
 	u32 int_status, vnms;
 	int slot;
-	unsigned int sequence, handled = 0;
+	unsigned int i, sequence, handled = 0;
 	unsigned long flags;
 
 	spin_lock_irqsave(&vin->qlock, flags);
@@ -955,8 +966,20 @@ static irqreturn_t rvin_irq(int irq, void *data)
 		 * the VnMBm registers.
 		 */
 		if (vin->continuous) {
-			rvin_capture_off(vin);
+			rvin_capture_stop(vin);
 			vin_dbg(vin, "IRQ %02d: hw not ready stop\n", sequence);
+
+			/* Maybe we can continue in single capture mode */
+			for (i = 0; i < HW_BUFFER_NUM; i++) {
+				if (vin->queue_buf[i]) {
+					list_add(to_buf_list(vin->queue_buf[i]),
+						 &vin->buf_list);
+					vin->queue_buf[i] = NULL;
+				}
+			}
+
+			if (!list_empty(&vin->buf_list))
+				rvin_capture_start(vin);
 		}
 	} else {
 		/*
@@ -1041,8 +1064,7 @@ static void rvin_buffer_queue(struct vb2_buffer *vb)
 	 * capturing if HW is ready to continue.
 	 */
 	if (vin->state == STALLED)
-		if (rvin_fill_hw(vin))
-			rvin_capture_on(vin);
+		rvin_capture_start(vin);
 
 	spin_unlock_irqrestore(&vin->qlock, flags);
 }
@@ -1059,25 +1081,9 @@ static int rvin_start_streaming(struct vb2_queue *vq, unsigned int count)
 
 	spin_lock_irqsave(&vin->qlock, flags);
 
-	vin->state = RUNNING;
 	vin->sequence = 0;
 
-	/* Continuous capture requires more buffers then there are HW slots */
-	vin->continuous = count > HW_BUFFER_NUM;
-
-	/*
-	 * This should never happen but if we don't have enough
-	 * buffers for HW bail out
-	 */
-	if (!rvin_fill_hw(vin)) {
-		vin_err(vin, "HW not ready to start, not enough buffers available\n");
-		ret = -EINVAL;
-		goto out;
-	}
-
 	ret = rvin_capture_start(vin);
-out:
-	/* Return all buffers if something went wrong */
 	if (ret) {
 		return_all_buffers(vin, VB2_BUF_STATE_QUEUED);
 		v4l2_subdev_call(sd, video, s_stream, 0);
@@ -1183,7 +1189,7 @@ int rvin_dma_probe(struct rvin_dev *vin, int irq)
 	q->ops = &rvin_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-	q->min_buffers_needed = 2;
+	q->min_buffers_needed = 1;
 	q->dev = vin->dev;
 
 	ret = vb2_queue_init(q);
diff --git a/drivers/media/platform/rcar-vin/rcar-v4l2.c b/drivers/media/platform/rcar-vin/rcar-v4l2.c
index 2bbe6d495fa6..dd37ea811680 100644
--- a/drivers/media/platform/rcar-vin/rcar-v4l2.c
+++ b/drivers/media/platform/rcar-vin/rcar-v4l2.c
@@ -111,7 +111,7 @@ static int rvin_reset_format(struct rvin_dev *vin)
 	struct v4l2_mbus_framefmt *mf = &fmt.format;
 	int ret;
 
-	fmt.pad = vin->src_pad_idx;
+	fmt.pad = vin->digital.source_pad;
 
 	ret = v4l2_subdev_call(vin_to_source(vin), pad, get_fmt, NULL, &fmt);
 	if (ret)
@@ -151,6 +151,9 @@ static int rvin_reset_format(struct rvin_dev *vin)
 
 	rvin_reset_crop_compose(vin);
 
+	vin->format.bytesperline = rvin_format_bytesperline(&vin->format);
+	vin->format.sizeimage = rvin_format_sizeimage(&vin->format);
+
 	return 0;
 }
 
@@ -175,7 +178,7 @@ static int __rvin_try_format_source(struct rvin_dev *vin,
 	if (pad_cfg == NULL)
 		return -ENOMEM;
 
-	format.pad = vin->src_pad_idx;
+	format.pad = vin->digital.source_pad;
 
 	field = pix->field;
 
@@ -203,8 +206,8 @@ static int __rvin_try_format(struct rvin_dev *vin,
 			     struct v4l2_pix_format *pix,
 			     struct rvin_source_fmt *source)
 {
-	const struct rvin_video_format *info;
 	u32 rwidth, rheight, walign;
+	int ret;
 
 	/* Requested */
 	rwidth = pix->width;
@@ -214,17 +217,11 @@ static int __rvin_try_format(struct rvin_dev *vin,
 	if (pix->field == V4L2_FIELD_ANY)
 		pix->field = vin->format.field;
 
-	/*
-	 * Retrieve format information and select the current format if the
-	 * requested format isn't supported.
-	 */
-	info = rvin_format_from_pixel(pix->pixelformat);
-	if (!info) {
-		vin_dbg(vin, "Format %x not found, keeping %x\n",
-			pix->pixelformat, vin->format.pixelformat);
-		*pix = vin->format;
-		pix->width = rwidth;
-		pix->height = rheight;
+	/* If requested format is not supported fallback to the default */
+	if (!rvin_format_from_pixel(pix->pixelformat)) {
+		vin_dbg(vin, "Format 0x%x not found, using default 0x%x\n",
+			pix->pixelformat, RVIN_DEFAULT_FORMAT);
+		pix->pixelformat = RVIN_DEFAULT_FORMAT;
 	}
 
 	/* Always recalculate */
@@ -232,7 +229,9 @@ static int __rvin_try_format(struct rvin_dev *vin,
 	pix->sizeimage = 0;
 
 	/* Limit to source capabilities */
-	__rvin_try_format_source(vin, which, pix, source);
+	ret = __rvin_try_format_source(vin, which, pix, source);
+	if (ret)
+		return ret;
 
 	switch (pix->field) {
 	case V4L2_FIELD_TOP:
@@ -480,10 +479,14 @@ static int rvin_enum_input(struct file *file, void *priv,
 		return ret;
 
 	i->type = V4L2_INPUT_TYPE_CAMERA;
-	i->std = vin->vdev.tvnorms;
 
-	if (v4l2_subdev_has_op(sd, pad, dv_timings_cap))
+	if (v4l2_subdev_has_op(sd, pad, dv_timings_cap)) {
 		i->capabilities = V4L2_IN_CAP_DV_TIMINGS;
+		i->std = 0;
+	} else {
+		i->capabilities = V4L2_IN_CAP_STD;
+		i->std = vin->vdev.tvnorms;
+	}
 
 	strlcpy(i->name, "Camera", sizeof(i->name));
 
@@ -547,14 +550,16 @@ static int rvin_enum_dv_timings(struct file *file, void *priv_fh,
 {
 	struct rvin_dev *vin = video_drvdata(file);
 	struct v4l2_subdev *sd = vin_to_source(vin);
-	int pad, ret;
+	int ret;
+
+	if (timings->pad)
+		return -EINVAL;
 
-	pad = timings->pad;
-	timings->pad = vin->sink_pad_idx;
+	timings->pad = vin->digital.sink_pad;
 
 	ret = v4l2_subdev_call(sd, pad, enum_dv_timings, timings);
 
-	timings->pad = pad;
+	timings->pad = 0;
 
 	return ret;
 }
@@ -570,12 +575,8 @@ static int rvin_s_dv_timings(struct file *file, void *priv_fh,
 	if (ret)
 		return ret;
 
-	vin->source.width = timings->bt.width;
-	vin->source.height = timings->bt.height;
-	vin->format.width = timings->bt.width;
-	vin->format.height = timings->bt.height;
-
-	return 0;
+	/* Changing the timings will change the width/height */
+	return rvin_reset_format(vin);
 }
 
 static int rvin_g_dv_timings(struct file *file, void *priv_fh,
@@ -601,14 +602,16 @@ static int rvin_dv_timings_cap(struct file *file, void *priv_fh,
 {
 	struct rvin_dev *vin = video_drvdata(file);
 	struct v4l2_subdev *sd = vin_to_source(vin);
-	int pad, ret;
+	int ret;
+
+	if (cap->pad)
+		return -EINVAL;
 
-	pad = cap->pad;
-	cap->pad = vin->sink_pad_idx;
+	cap->pad = vin->digital.sink_pad;
 
 	ret = v4l2_subdev_call(sd, pad, dv_timings_cap, cap);
 
-	cap->pad = pad;
+	cap->pad = 0;
 
 	return ret;
 }
@@ -617,17 +620,16 @@ static int rvin_g_edid(struct file *file, void *fh, struct v4l2_edid *edid)
 {
 	struct rvin_dev *vin = video_drvdata(file);
 	struct v4l2_subdev *sd = vin_to_source(vin);
-	int input, ret;
+	int ret;
 
 	if (edid->pad)
 		return -EINVAL;
 
-	input = edid->pad;
-	edid->pad = vin->sink_pad_idx;
+	edid->pad = vin->digital.sink_pad;
 
 	ret = v4l2_subdev_call(sd, pad, get_edid, edid);
 
-	edid->pad = input;
+	edid->pad = 0;
 
 	return ret;
 }
@@ -636,17 +638,16 @@ static int rvin_s_edid(struct file *file, void *fh, struct v4l2_edid *edid)
 {
 	struct rvin_dev *vin = video_drvdata(file);
 	struct v4l2_subdev *sd = vin_to_source(vin);
-	int input, ret;
+	int ret;
 
 	if (edid->pad)
 		return -EINVAL;
 
-	input = edid->pad;
-	edid->pad = vin->sink_pad_idx;
+	edid->pad = vin->digital.sink_pad;
 
 	ret = v4l2_subdev_call(sd, pad, set_edid, edid);
 
-	edid->pad = input;
+	edid->pad = 0;
 
 	return ret;
 }
@@ -869,7 +870,7 @@ int rvin_v4l2_probe(struct rvin_dev *vin)
 {
 	struct video_device *vdev = &vin->vdev;
 	struct v4l2_subdev *sd = vin_to_source(vin);
-	int pad_idx, ret;
+	int ret;
 
 	v4l2_set_subdev_hostdata(sd, vin);
 
@@ -915,22 +916,6 @@ int rvin_v4l2_probe(struct rvin_dev *vin)
 	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
 		V4L2_CAP_READWRITE;
 
-	vin->src_pad_idx = 0;
-	for (pad_idx = 0; pad_idx < sd->entity.num_pads; pad_idx++)
-		if (sd->entity.pads[pad_idx].flags == MEDIA_PAD_FL_SOURCE)
-			break;
-	if (pad_idx >= sd->entity.num_pads)
-		return -EINVAL;
-
-	vin->src_pad_idx = pad_idx;
-
-	vin->sink_pad_idx = 0;
-	for (pad_idx = 0; pad_idx < sd->entity.num_pads; pad_idx++)
-		if (sd->entity.pads[pad_idx].flags == MEDIA_PAD_FL_SINK) {
-			vin->sink_pad_idx = pad_idx;
-			break;
-		}
-
 	vin->format.pixelformat	= RVIN_DEFAULT_FORMAT;
 	rvin_reset_format(vin);
 
diff --git a/drivers/media/platform/rcar-vin/rcar-vin.h b/drivers/media/platform/rcar-vin/rcar-vin.h
index 727e215c0871..9bfb5a7c4dc4 100644
--- a/drivers/media/platform/rcar-vin/rcar-vin.h
+++ b/drivers/media/platform/rcar-vin/rcar-vin.h
@@ -74,6 +74,8 @@ struct rvin_video_format {
  * @subdev:	subdevice matched using async framework
  * @code:	Media bus format from source
  * @mbus_cfg:	Media bus format from DT
+ * @source_pad:	source pad of remote subdevice
+ * @sink_pad:	sink pad of remote subdevice
  */
 struct rvin_graph_entity {
 	struct v4l2_async_subdev asd;
@@ -81,6 +83,9 @@ struct rvin_graph_entity {
 
 	u32 code;
 	struct v4l2_mbus_config mbus_cfg;
+
+	unsigned int source_pad;
+	unsigned int sink_pad;
 };
 
 /**
@@ -91,8 +96,6 @@ struct rvin_graph_entity {
  *
  * @vdev:		V4L2 video device associated with VIN
  * @v4l2_dev:		V4L2 device
- * @src_pad_idx:	source pad index for media controller drivers
- * @sink_pad_idx:	sink pad index for media controller drivers
  * @ctrl_handler:	V4L2 control handler
  * @notifier:		V4L2 asynchronous subdevs notifier
  * @digital:		entity in the DT for local digital subdevice
@@ -121,8 +124,6 @@ struct rvin_dev {
 
 	struct video_device vdev;
 	struct v4l2_device v4l2_dev;
-	int src_pad_idx;
-	int sink_pad_idx;
 	struct v4l2_ctrl_handler ctrl_handler;
 	struct v4l2_async_notifier notifier;
 	struct rvin_graph_entity digital;
diff --git a/drivers/media/platform/rcar_drif.c b/drivers/media/platform/rcar_drif.c
new file mode 100644
index 000000000000..522364ff0d5d
--- /dev/null
+++ b/drivers/media/platform/rcar_drif.c
@@ -0,0 +1,1498 @@
+/*
+ * R-Car Gen3 Digital Radio Interface (DRIF) driver
+ *
+ * Copyright (C) 2017 Renesas Electronics Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+/*
+ * The R-Car DRIF is a receive only MSIOF like controller with an
+ * external master device driving the SCK. It receives data into a FIFO,
+ * then this driver uses the SYS-DMAC engine to move the data from
+ * the device to memory.
+ *
+ * Each DRIF channel DRIFx (as per datasheet) contains two internal
+ * channels DRIFx0 & DRIFx1 within itself with each having its own resources
+ * like module clk, register set, irq and dma. These internal channels share
+ * common CLK & SYNC from master. The two data pins D0 & D1 shall be
+ * considered to represent the two internal channels. This internal split
+ * is not visible to the master device.
+ *
+ * Depending on the master device, a DRIF channel can use
+ *  (1) both internal channels (D0 & D1) to receive data in parallel (or)
+ *  (2) one internal channel (D0 or D1) to receive data
+ *
+ * The primary design goal of this controller is to act as a Digital Radio
+ * Interface that receives digital samples from a tuner device. Hence the
+ * driver exposes the device as a V4L2 SDR device. In order to qualify as
+ * a V4L2 SDR device, it should possess a tuner interface as mandated by the
+ * framework. This driver expects a tuner driver (sub-device) to bind
+ * asynchronously with this device and the combined drivers shall expose
+ * a V4L2 compliant SDR device. The DRIF driver is independent of the
+ * tuner vendor.
+ *
+ * The DRIF h/w can support I2S mode and Frame start synchronization pulse mode.
+ * This driver is tested for I2S mode only because of the availability of
+ * suitable master devices. Hence, not all configurable options of DRIF h/w
+ * like lsb/msb first, syncdl, dtdl etc. are exposed via DT and I2S defaults
+ * are used. These can be exposed later if needed after testing.
+ */
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/ioctl.h>
+#include <linux/iopoll.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-vmalloc.h>
+
+/* DRIF register offsets */
+#define RCAR_DRIF_SITMDR1			0x00
+#define RCAR_DRIF_SITMDR2			0x04
+#define RCAR_DRIF_SITMDR3			0x08
+#define RCAR_DRIF_SIRMDR1			0x10
+#define RCAR_DRIF_SIRMDR2			0x14
+#define RCAR_DRIF_SIRMDR3			0x18
+#define RCAR_DRIF_SICTR				0x28
+#define RCAR_DRIF_SIFCTR			0x30
+#define RCAR_DRIF_SISTR				0x40
+#define RCAR_DRIF_SIIER				0x44
+#define RCAR_DRIF_SIRFDR			0x60
+
+#define RCAR_DRIF_RFOVF			BIT(3)	/* Receive FIFO overflow */
+#define RCAR_DRIF_RFUDF			BIT(4)	/* Receive FIFO underflow */
+#define RCAR_DRIF_RFSERR		BIT(5)	/* Receive frame sync error */
+#define RCAR_DRIF_REOF			BIT(7)	/* Frame reception end */
+#define RCAR_DRIF_RDREQ			BIT(12) /* Receive data xfer req */
+#define RCAR_DRIF_RFFUL			BIT(13)	/* Receive FIFO full */
+
+/* SIRMDR1 */
+#define RCAR_DRIF_SIRMDR1_SYNCMD_FRAME		(0 << 28)
+#define RCAR_DRIF_SIRMDR1_SYNCMD_LR		(3 << 28)
+
+#define RCAR_DRIF_SIRMDR1_SYNCAC_POL_HIGH	(0 << 25)
+#define RCAR_DRIF_SIRMDR1_SYNCAC_POL_LOW	(1 << 25)
+
+#define RCAR_DRIF_SIRMDR1_MSB_FIRST		(0 << 24)
+#define RCAR_DRIF_SIRMDR1_LSB_FIRST		(1 << 24)
+
+#define RCAR_DRIF_SIRMDR1_DTDL_0		(0 << 20)
+#define RCAR_DRIF_SIRMDR1_DTDL_1		(1 << 20)
+#define RCAR_DRIF_SIRMDR1_DTDL_2		(2 << 20)
+#define RCAR_DRIF_SIRMDR1_DTDL_0PT5		(5 << 20)
+#define RCAR_DRIF_SIRMDR1_DTDL_1PT5		(6 << 20)
+
+#define RCAR_DRIF_SIRMDR1_SYNCDL_0		(0 << 20)
+#define RCAR_DRIF_SIRMDR1_SYNCDL_1		(1 << 20)
+#define RCAR_DRIF_SIRMDR1_SYNCDL_2		(2 << 20)
+#define RCAR_DRIF_SIRMDR1_SYNCDL_3		(3 << 20)
+#define RCAR_DRIF_SIRMDR1_SYNCDL_0PT5		(5 << 20)
+#define RCAR_DRIF_SIRMDR1_SYNCDL_1PT5		(6 << 20)
+
+#define RCAR_DRIF_MDR_GRPCNT(n)			(((n) - 1) << 30)
+#define RCAR_DRIF_MDR_BITLEN(n)			(((n) - 1) << 24)
+#define RCAR_DRIF_MDR_WDCNT(n)			(((n) - 1) << 16)
+
+/* Hidden Transmit register that controls CLK & SYNC */
+#define RCAR_DRIF_SITMDR1_PCON			BIT(30)
+
+#define RCAR_DRIF_SICTR_RX_RISING_EDGE		BIT(26)
+#define RCAR_DRIF_SICTR_RX_EN			BIT(8)
+#define RCAR_DRIF_SICTR_RESET			BIT(0)
+
+/* Constants */
+#define RCAR_DRIF_NUM_HWBUFS			32
+#define RCAR_DRIF_MAX_DEVS			4
+#define RCAR_DRIF_DEFAULT_NUM_HWBUFS		16
+#define RCAR_DRIF_DEFAULT_HWBUF_SIZE		(4 * PAGE_SIZE)
+#define RCAR_DRIF_MAX_CHANNEL			2
+#define RCAR_SDR_BUFFER_SIZE			SZ_64K
+
+/* Internal buffer status flags */
+#define RCAR_DRIF_BUF_DONE			BIT(0)	/* DMA completed */
+#define RCAR_DRIF_BUF_OVERFLOW			BIT(1)	/* Overflow detected */
+
+#define to_rcar_drif_buf_pair(sdr, ch_num, idx)			\
+	(&((sdr)->ch[!(ch_num)]->buf[(idx)]))
+
+#define for_each_rcar_drif_channel(ch, ch_mask)			\
+	for_each_set_bit(ch, ch_mask, RCAR_DRIF_MAX_CHANNEL)
+
+/* Debug */
+#define rdrif_dbg(sdr, fmt, arg...)				\
+	dev_dbg(sdr->v4l2_dev.dev, fmt, ## arg)
+
+#define rdrif_err(sdr, fmt, arg...)				\
+	dev_err(sdr->v4l2_dev.dev, fmt, ## arg)
+
+/* Stream formats */
+struct rcar_drif_format {
+	u32	pixelformat;
+	u32	buffersize;
+	u32	bitlen;
+	u32	wdcnt;
+	u32	num_ch;
+};
+
+/* Format descriptions for capture */
+static const struct rcar_drif_format formats[] = {
+	{
+		.pixelformat	= V4L2_SDR_FMT_PCU16BE,
+		.buffersize	= RCAR_SDR_BUFFER_SIZE,
+		.bitlen		= 16,
+		.wdcnt		= 1,
+		.num_ch		= 2,
+	},
+	{
+		.pixelformat	= V4L2_SDR_FMT_PCU18BE,
+		.buffersize	= RCAR_SDR_BUFFER_SIZE,
+		.bitlen		= 18,
+		.wdcnt		= 1,
+		.num_ch		= 2,
+	},
+	{
+		.pixelformat	= V4L2_SDR_FMT_PCU20BE,
+		.buffersize	= RCAR_SDR_BUFFER_SIZE,
+		.bitlen		= 20,
+		.wdcnt		= 1,
+		.num_ch		= 2,
+	},
+};
+
+/* Buffer for a received frame from one or both internal channels */
+struct rcar_drif_frame_buf {
+	/* Common v4l buffer stuff -- must be first */
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+};
+
+/* OF graph endpoint's V4L2 async data */
+struct rcar_drif_graph_ep {
+	struct v4l2_subdev *subdev;	/* Async matched subdev */
+	struct v4l2_async_subdev asd;	/* Async sub-device descriptor */
+};
+
+/* DMA buffer */
+struct rcar_drif_hwbuf {
+	void *addr;			/* CPU-side address */
+	unsigned int status;		/* Buffer status flags */
+};
+
+/* Internal channel */
+struct rcar_drif {
+	struct rcar_drif_sdr *sdr;	/* Group device */
+	struct platform_device *pdev;	/* Channel's pdev */
+	void __iomem *base;		/* Base register address */
+	resource_size_t start;		/* I/O resource offset */
+	struct dma_chan *dmach;		/* Reserved DMA channel */
+	struct clk *clk;		/* Module clock */
+	struct rcar_drif_hwbuf buf[RCAR_DRIF_NUM_HWBUFS]; /* H/W bufs */
+	dma_addr_t dma_handle;		/* Handle for all bufs */
+	unsigned int num;		/* Channel number */
+	bool acting_sdr;		/* Channel acting as SDR device */
+};
+
+/* DRIF V4L2 SDR */
+struct rcar_drif_sdr {
+	struct device *dev;		/* Platform device */
+	struct video_device *vdev;	/* V4L2 SDR device */
+	struct v4l2_device v4l2_dev;	/* V4L2 device */
+
+	/* Videobuf2 queue and queued buffers list */
+	struct vb2_queue vb_queue;
+	struct list_head queued_bufs;
+	spinlock_t queued_bufs_lock;	/* Protects queued_bufs */
+	spinlock_t dma_lock;		/* To serialize DMA cb of channels */
+
+	struct mutex v4l2_mutex;	/* To serialize ioctls */
+	struct mutex vb_queue_mutex;	/* To serialize streaming ioctls */
+	struct v4l2_ctrl_handler ctrl_hdl;	/* SDR control handler */
+	struct v4l2_async_notifier notifier;	/* For subdev (tuner) */
+	struct rcar_drif_graph_ep ep;	/* Endpoint V4L2 async data */
+
+	/* Current V4L2 SDR format ptr */
+	const struct rcar_drif_format *fmt;
+
+	/* Device tree SYNC properties */
+	u32 mdr1;
+
+	/* Internals */
+	struct rcar_drif *ch[RCAR_DRIF_MAX_CHANNEL]; /* DRIFx0,1 */
+	unsigned long hw_ch_mask;	/* Enabled channels per DT */
+	unsigned long cur_ch_mask;	/* Used channels for an SDR FMT */
+	u32 num_hw_ch;			/* Num of DT enabled channels */
+	u32 num_cur_ch;			/* Num of used channels */
+	u32 hwbuf_size;			/* Each DMA buffer size */
+	u32 produced;			/* Buffers produced by sdr dev */
+};
+
+/* Register access functions */
+static void rcar_drif_write(struct rcar_drif *ch, u32 offset, u32 data)
+{
+	writel(data, ch->base + offset);
+}
+
+static u32 rcar_drif_read(struct rcar_drif *ch, u32 offset)
+{
+	return readl(ch->base + offset);
+}
+
+/* Release DMA channels */
+static void rcar_drif_release_dmachannels(struct rcar_drif_sdr *sdr)
+{
+	unsigned int i;
+
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask)
+		if (sdr->ch[i]->dmach) {
+			dma_release_channel(sdr->ch[i]->dmach);
+			sdr->ch[i]->dmach = NULL;
+		}
+}
+
+/* Allocate DMA channels */
+static int rcar_drif_alloc_dmachannels(struct rcar_drif_sdr *sdr)
+{
+	struct dma_slave_config dma_cfg;
+	unsigned int i;
+	int ret = -ENODEV;
+
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask) {
+		struct rcar_drif *ch = sdr->ch[i];
+
+		ch->dmach = dma_request_slave_channel(&ch->pdev->dev, "rx");
+		if (!ch->dmach) {
+			rdrif_err(sdr, "ch%u: dma channel req failed\n", i);
+			goto dmach_error;
+		}
+
+		/* Configure slave */
+		memset(&dma_cfg, 0, sizeof(dma_cfg));
+		dma_cfg.src_addr = (phys_addr_t)(ch->start + RCAR_DRIF_SIRFDR);
+		dma_cfg.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+		ret = dmaengine_slave_config(ch->dmach, &dma_cfg);
+		if (ret) {
+			rdrif_err(sdr, "ch%u: dma slave config failed\n", i);
+			goto dmach_error;
+		}
+	}
+	return 0;
+
+dmach_error:
+	rcar_drif_release_dmachannels(sdr);
+	return ret;
+}
+
+/* Release queued vb2 buffers */
+static void rcar_drif_release_queued_bufs(struct rcar_drif_sdr *sdr,
+					  enum vb2_buffer_state state)
+{
+	struct rcar_drif_frame_buf *fbuf, *tmp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&sdr->queued_bufs_lock, flags);
+	list_for_each_entry_safe(fbuf, tmp, &sdr->queued_bufs, list) {
+		list_del(&fbuf->list);
+		vb2_buffer_done(&fbuf->vb.vb2_buf, state);
+	}
+	spin_unlock_irqrestore(&sdr->queued_bufs_lock, flags);
+}
+
+/* Set MDR defaults */
+static inline void rcar_drif_set_mdr1(struct rcar_drif_sdr *sdr)
+{
+	unsigned int i;
+
+	/* Set defaults for enabled internal channels */
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask) {
+		/* Refer MSIOF section in manual for this register setting */
+		rcar_drif_write(sdr->ch[i], RCAR_DRIF_SITMDR1,
+				RCAR_DRIF_SITMDR1_PCON);
+
+		/* Setup MDR1 value */
+		rcar_drif_write(sdr->ch[i], RCAR_DRIF_SIRMDR1, sdr->mdr1);
+
+		rdrif_dbg(sdr, "ch%u: mdr1 = 0x%08x",
+			  i, rcar_drif_read(sdr->ch[i], RCAR_DRIF_SIRMDR1));
+	}
+}
+
+/* Set DRIF receive format */
+static int rcar_drif_set_format(struct rcar_drif_sdr *sdr)
+{
+	unsigned int i;
+
+	rdrif_dbg(sdr, "setfmt: bitlen %u wdcnt %u num_ch %u\n",
+		  sdr->fmt->bitlen, sdr->fmt->wdcnt, sdr->fmt->num_ch);
+
+	/* Sanity check */
+	if (sdr->fmt->num_ch > sdr->num_cur_ch) {
+		rdrif_err(sdr, "fmt num_ch %u cur_ch %u mismatch\n",
+			  sdr->fmt->num_ch, sdr->num_cur_ch);
+		return -EINVAL;
+	}
+
+	/* Setup group, bitlen & wdcnt */
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask) {
+		u32 mdr;
+
+		/* Two groups */
+		mdr = RCAR_DRIF_MDR_GRPCNT(2) |
+			RCAR_DRIF_MDR_BITLEN(sdr->fmt->bitlen) |
+			RCAR_DRIF_MDR_WDCNT(sdr->fmt->wdcnt);
+		rcar_drif_write(sdr->ch[i], RCAR_DRIF_SIRMDR2, mdr);
+
+		mdr = RCAR_DRIF_MDR_BITLEN(sdr->fmt->bitlen) |
+			RCAR_DRIF_MDR_WDCNT(sdr->fmt->wdcnt);
+		rcar_drif_write(sdr->ch[i], RCAR_DRIF_SIRMDR3, mdr);
+
+		rdrif_dbg(sdr, "ch%u: new mdr[2,3] = 0x%08x, 0x%08x\n",
+			  i, rcar_drif_read(sdr->ch[i], RCAR_DRIF_SIRMDR2),
+			  rcar_drif_read(sdr->ch[i], RCAR_DRIF_SIRMDR3));
+	}
+	return 0;
+}
+
+/* Release DMA buffers */
+static void rcar_drif_release_buf(struct rcar_drif_sdr *sdr)
+{
+	unsigned int i;
+
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask) {
+		struct rcar_drif *ch = sdr->ch[i];
+
+		/* First entry contains the dma buf ptr */
+		if (ch->buf[0].addr) {
+			dma_free_coherent(&ch->pdev->dev,
+				sdr->hwbuf_size * RCAR_DRIF_NUM_HWBUFS,
+				ch->buf[0].addr, ch->dma_handle);
+			ch->buf[0].addr = NULL;
+		}
+	}
+}
+
+/* Request DMA buffers */
+static int rcar_drif_request_buf(struct rcar_drif_sdr *sdr)
+{
+	int ret = -ENOMEM;
+	unsigned int i, j;
+	void *addr;
+
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask) {
+		struct rcar_drif *ch = sdr->ch[i];
+
+		/* Allocate DMA buffers */
+		addr = dma_alloc_coherent(&ch->pdev->dev,
+				sdr->hwbuf_size * RCAR_DRIF_NUM_HWBUFS,
+				&ch->dma_handle, GFP_KERNEL);
+		if (!addr) {
+			rdrif_err(sdr,
+			"ch%u: dma alloc failed. num hwbufs %u size %u\n",
+			i, RCAR_DRIF_NUM_HWBUFS, sdr->hwbuf_size);
+			goto error;
+		}
+
+		/* Split the chunk and populate bufctxt */
+		for (j = 0; j < RCAR_DRIF_NUM_HWBUFS; j++) {
+			ch->buf[j].addr = addr + (j * sdr->hwbuf_size);
+			ch->buf[j].status = 0;
+		}
+	}
+	return 0;
+error:
+	return ret;
+}
+
+/* Setup vb_queue minimum buffer requirements */
+static int rcar_drif_queue_setup(struct vb2_queue *vq,
+			unsigned int *num_buffers, unsigned int *num_planes,
+			unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct rcar_drif_sdr *sdr = vb2_get_drv_priv(vq);
+
+	/* Need at least 16 buffers */
+	if (vq->num_buffers + *num_buffers < 16)
+		*num_buffers = 16 - vq->num_buffers;
+
+	*num_planes = 1;
+	sizes[0] = PAGE_ALIGN(sdr->fmt->buffersize);
+	rdrif_dbg(sdr, "num_bufs %d sizes[0] %d\n", *num_buffers, sizes[0]);
+
+	return 0;
+}
+
+/* Enqueue buffer */
+static void rcar_drif_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct rcar_drif_sdr *sdr = vb2_get_drv_priv(vb->vb2_queue);
+	struct rcar_drif_frame_buf *fbuf =
+			container_of(vbuf, struct rcar_drif_frame_buf, vb);
+	unsigned long flags;
+
+	rdrif_dbg(sdr, "buf_queue idx %u\n", vb->index);
+	spin_lock_irqsave(&sdr->queued_bufs_lock, flags);
+	list_add_tail(&fbuf->list, &sdr->queued_bufs);
+	spin_unlock_irqrestore(&sdr->queued_bufs_lock, flags);
+}
+
+/* Get a frame buf from list */
+static struct rcar_drif_frame_buf *
+rcar_drif_get_fbuf(struct rcar_drif_sdr *sdr)
+{
+	struct rcar_drif_frame_buf *fbuf;
+	unsigned long flags;
+
+	spin_lock_irqsave(&sdr->queued_bufs_lock, flags);
+	fbuf = list_first_entry_or_null(&sdr->queued_bufs, struct
+					rcar_drif_frame_buf, list);
+	if (!fbuf) {
+		/*
+		 * App is late in enqueing buffers. Samples lost & there will
+		 * be a gap in sequence number when app recovers
+		 */
+		rdrif_dbg(sdr, "\napp late: prod %u\n", sdr->produced);
+		spin_unlock_irqrestore(&sdr->queued_bufs_lock, flags);
+		return NULL;
+	}
+	list_del(&fbuf->list);
+	spin_unlock_irqrestore(&sdr->queued_bufs_lock, flags);
+
+	return fbuf;
+}
+
+/* Helpers to set/clear buf pair status */
+static inline bool rcar_drif_bufs_done(struct rcar_drif_hwbuf **buf)
+{
+	return (buf[0]->status & buf[1]->status & RCAR_DRIF_BUF_DONE);
+}
+
+static inline bool rcar_drif_bufs_overflow(struct rcar_drif_hwbuf **buf)
+{
+	return ((buf[0]->status | buf[1]->status) & RCAR_DRIF_BUF_OVERFLOW);
+}
+
+static inline void rcar_drif_bufs_clear(struct rcar_drif_hwbuf **buf,
+					unsigned int bit)
+{
+	unsigned int i;
+
+	for (i = 0; i < RCAR_DRIF_MAX_CHANNEL; i++)
+		buf[i]->status &= ~bit;
+}
+
+/* Channel DMA complete */
+static void rcar_drif_channel_complete(struct rcar_drif *ch, u32 idx)
+{
+	u32 str;
+
+	ch->buf[idx].status |= RCAR_DRIF_BUF_DONE;
+
+	/* Check for DRIF errors */
+	str = rcar_drif_read(ch, RCAR_DRIF_SISTR);
+	if (unlikely(str & RCAR_DRIF_RFOVF)) {
+		/* Writing the same clears it */
+		rcar_drif_write(ch, RCAR_DRIF_SISTR, str);
+
+		/* Overflow: some samples are lost */
+		ch->buf[idx].status |= RCAR_DRIF_BUF_OVERFLOW;
+	}
+}
+
+/* DMA callback for each stage */
+static void rcar_drif_dma_complete(void *dma_async_param)
+{
+	struct rcar_drif *ch = dma_async_param;
+	struct rcar_drif_sdr *sdr = ch->sdr;
+	struct rcar_drif_hwbuf *buf[RCAR_DRIF_MAX_CHANNEL];
+	struct rcar_drif_frame_buf *fbuf;
+	bool overflow = false;
+	u32 idx, produced;
+	unsigned int i;
+
+	spin_lock(&sdr->dma_lock);
+
+	/* DMA can be terminated while the callback was waiting on lock */
+	if (!vb2_is_streaming(&sdr->vb_queue)) {
+		spin_unlock(&sdr->dma_lock);
+		return;
+	}
+
+	idx = sdr->produced % RCAR_DRIF_NUM_HWBUFS;
+	rcar_drif_channel_complete(ch, idx);
+
+	if (sdr->num_cur_ch == RCAR_DRIF_MAX_CHANNEL) {
+		buf[0] = ch->num ? to_rcar_drif_buf_pair(sdr, ch->num, idx) :
+				&ch->buf[idx];
+		buf[1] = ch->num ? &ch->buf[idx] :
+				to_rcar_drif_buf_pair(sdr, ch->num, idx);
+
+		/* Check if both DMA buffers are done */
+		if (!rcar_drif_bufs_done(buf)) {
+			spin_unlock(&sdr->dma_lock);
+			return;
+		}
+
+		/* Clear buf done status */
+		rcar_drif_bufs_clear(buf, RCAR_DRIF_BUF_DONE);
+
+		if (rcar_drif_bufs_overflow(buf)) {
+			overflow = true;
+			/* Clear the flag in status */
+			rcar_drif_bufs_clear(buf, RCAR_DRIF_BUF_OVERFLOW);
+		}
+	} else {
+		buf[0] = &ch->buf[idx];
+		if (buf[0]->status & RCAR_DRIF_BUF_OVERFLOW) {
+			overflow = true;
+			/* Clear the flag in status */
+			buf[0]->status &= ~RCAR_DRIF_BUF_OVERFLOW;
+		}
+	}
+
+	/* Buffer produced for consumption */
+	produced = sdr->produced++;
+	spin_unlock(&sdr->dma_lock);
+
+	rdrif_dbg(sdr, "ch%u: prod %u\n", ch->num, produced);
+
+	/* Get fbuf */
+	fbuf = rcar_drif_get_fbuf(sdr);
+	if (!fbuf)
+		return;
+
+	for (i = 0; i < RCAR_DRIF_MAX_CHANNEL; i++)
+		memcpy(vb2_plane_vaddr(&fbuf->vb.vb2_buf, 0) +
+		       i * sdr->hwbuf_size, buf[i]->addr, sdr->hwbuf_size);
+
+	fbuf->vb.field = V4L2_FIELD_NONE;
+	fbuf->vb.sequence = produced;
+	fbuf->vb.vb2_buf.timestamp = ktime_get_ns();
+	vb2_set_plane_payload(&fbuf->vb.vb2_buf, 0, sdr->fmt->buffersize);
+
+	/* Set error state on overflow */
+	vb2_buffer_done(&fbuf->vb.vb2_buf,
+			overflow ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+}
+
+static int rcar_drif_qbuf(struct rcar_drif *ch)
+{
+	struct rcar_drif_sdr *sdr = ch->sdr;
+	dma_addr_t addr = ch->dma_handle;
+	struct dma_async_tx_descriptor *rxd;
+	dma_cookie_t cookie;
+	int ret = -EIO;
+
+	/* Setup cyclic DMA with given buffers */
+	rxd = dmaengine_prep_dma_cyclic(ch->dmach, addr,
+					sdr->hwbuf_size * RCAR_DRIF_NUM_HWBUFS,
+					sdr->hwbuf_size, DMA_DEV_TO_MEM,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!rxd) {
+		rdrif_err(sdr, "ch%u: prep dma cyclic failed\n", ch->num);
+		return ret;
+	}
+
+	/* Submit descriptor */
+	rxd->callback = rcar_drif_dma_complete;
+	rxd->callback_param = ch;
+	cookie = dmaengine_submit(rxd);
+	if (dma_submit_error(cookie)) {
+		rdrif_err(sdr, "ch%u: dma submit failed\n", ch->num);
+		return ret;
+	}
+
+	dma_async_issue_pending(ch->dmach);
+	return 0;
+}
+
+/* Enable reception */
+static int rcar_drif_enable_rx(struct rcar_drif_sdr *sdr)
+{
+	unsigned int i;
+	u32 ctr;
+	int ret;
+
+	/*
+	 * When both internal channels are enabled, they can be synchronized
+	 * only by the master
+	 */
+
+	/* Enable receive */
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask) {
+		ctr = rcar_drif_read(sdr->ch[i], RCAR_DRIF_SICTR);
+		ctr |= (RCAR_DRIF_SICTR_RX_RISING_EDGE |
+			 RCAR_DRIF_SICTR_RX_EN);
+		rcar_drif_write(sdr->ch[i], RCAR_DRIF_SICTR, ctr);
+	}
+
+	/* Check receive enabled */
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask) {
+		ret = readl_poll_timeout(sdr->ch[i]->base + RCAR_DRIF_SICTR,
+				ctr, ctr & RCAR_DRIF_SICTR_RX_EN, 7, 100000);
+		if (ret) {
+			rdrif_err(sdr, "ch%u: rx en failed. ctr 0x%08x\n", i,
+				  rcar_drif_read(sdr->ch[i], RCAR_DRIF_SICTR));
+			break;
+		}
+	}
+	return ret;
+}
+
+/* Disable reception */
+static void rcar_drif_disable_rx(struct rcar_drif_sdr *sdr)
+{
+	unsigned int i;
+	u32 ctr;
+	int ret;
+
+	/* Disable receive */
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask) {
+		ctr = rcar_drif_read(sdr->ch[i], RCAR_DRIF_SICTR);
+		ctr &= ~RCAR_DRIF_SICTR_RX_EN;
+		rcar_drif_write(sdr->ch[i], RCAR_DRIF_SICTR, ctr);
+	}
+
+	/* Check receive disabled */
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask) {
+		ret = readl_poll_timeout(sdr->ch[i]->base + RCAR_DRIF_SICTR,
+				ctr, !(ctr & RCAR_DRIF_SICTR_RX_EN), 7, 100000);
+		if (ret)
+			dev_warn(&sdr->vdev->dev,
+			"ch%u: failed to disable rx. ctr 0x%08x\n",
+			i, rcar_drif_read(sdr->ch[i], RCAR_DRIF_SICTR));
+	}
+}
+
+/* Stop channel */
+static void rcar_drif_stop_channel(struct rcar_drif *ch)
+{
+	/* Disable DMA receive interrupt */
+	rcar_drif_write(ch, RCAR_DRIF_SIIER, 0x00000000);
+
+	/* Terminate all DMA transfers */
+	dmaengine_terminate_sync(ch->dmach);
+}
+
+/* Stop receive operation */
+static void rcar_drif_stop(struct rcar_drif_sdr *sdr)
+{
+	unsigned int i;
+
+	/* Disable Rx */
+	rcar_drif_disable_rx(sdr);
+
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask)
+		rcar_drif_stop_channel(sdr->ch[i]);
+}
+
+/* Start channel */
+static int rcar_drif_start_channel(struct rcar_drif *ch)
+{
+	struct rcar_drif_sdr *sdr = ch->sdr;
+	u32 ctr, str;
+	int ret;
+
+	/* Reset receive */
+	rcar_drif_write(ch, RCAR_DRIF_SICTR, RCAR_DRIF_SICTR_RESET);
+	ret = readl_poll_timeout(ch->base + RCAR_DRIF_SICTR, ctr,
+				 !(ctr & RCAR_DRIF_SICTR_RESET), 7, 100000);
+	if (ret) {
+		rdrif_err(sdr, "ch%u: failed to reset rx. ctr 0x%08x\n",
+			  ch->num, rcar_drif_read(ch, RCAR_DRIF_SICTR));
+		return ret;
+	}
+
+	/* Queue buffers for DMA */
+	ret = rcar_drif_qbuf(ch);
+	if (ret)
+		return ret;
+
+	/* Clear status register flags */
+	str = RCAR_DRIF_RFFUL | RCAR_DRIF_REOF | RCAR_DRIF_RFSERR |
+		RCAR_DRIF_RFUDF | RCAR_DRIF_RFOVF;
+	rcar_drif_write(ch, RCAR_DRIF_SISTR, str);
+
+	/* Enable DMA receive interrupt */
+	rcar_drif_write(ch, RCAR_DRIF_SIIER, 0x00009000);
+
+	return ret;
+}
+
+/* Start receive operation */
+static int rcar_drif_start(struct rcar_drif_sdr *sdr)
+{
+	unsigned long enabled = 0;
+	unsigned int i;
+	int ret;
+
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask) {
+		ret = rcar_drif_start_channel(sdr->ch[i]);
+		if (ret)
+			goto start_error;
+		enabled |= BIT(i);
+	}
+
+	ret = rcar_drif_enable_rx(sdr);
+	if (ret)
+		goto enable_error;
+
+	sdr->produced = 0;
+	return ret;
+
+enable_error:
+	rcar_drif_disable_rx(sdr);
+start_error:
+	for_each_rcar_drif_channel(i, &enabled)
+		rcar_drif_stop_channel(sdr->ch[i]);
+
+	return ret;
+}
+
+/* Start streaming */
+static int rcar_drif_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct rcar_drif_sdr *sdr = vb2_get_drv_priv(vq);
+	unsigned long enabled = 0;
+	unsigned int i;
+	int ret;
+
+	mutex_lock(&sdr->v4l2_mutex);
+
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask) {
+		ret = clk_prepare_enable(sdr->ch[i]->clk);
+		if (ret)
+			goto error;
+		enabled |= BIT(i);
+	}
+
+	/* Set default MDRx settings */
+	rcar_drif_set_mdr1(sdr);
+
+	/* Set new format */
+	ret = rcar_drif_set_format(sdr);
+	if (ret)
+		goto error;
+
+	if (sdr->num_cur_ch == RCAR_DRIF_MAX_CHANNEL)
+		sdr->hwbuf_size = sdr->fmt->buffersize / RCAR_DRIF_MAX_CHANNEL;
+	else
+		sdr->hwbuf_size = sdr->fmt->buffersize;
+
+	rdrif_dbg(sdr, "num hwbufs %u, hwbuf_size %u\n",
+		RCAR_DRIF_NUM_HWBUFS, sdr->hwbuf_size);
+
+	/* Alloc DMA channel */
+	ret = rcar_drif_alloc_dmachannels(sdr);
+	if (ret)
+		goto error;
+
+	/* Request buffers */
+	ret = rcar_drif_request_buf(sdr);
+	if (ret)
+		goto error;
+
+	/* Start Rx */
+	ret = rcar_drif_start(sdr);
+	if (ret)
+		goto error;
+
+	mutex_unlock(&sdr->v4l2_mutex);
+
+	return ret;
+
+error:
+	rcar_drif_release_queued_bufs(sdr, VB2_BUF_STATE_QUEUED);
+	rcar_drif_release_buf(sdr);
+	rcar_drif_release_dmachannels(sdr);
+	for_each_rcar_drif_channel(i, &enabled)
+		clk_disable_unprepare(sdr->ch[i]->clk);
+
+	mutex_unlock(&sdr->v4l2_mutex);
+
+	return ret;
+}
+
+/* Stop streaming */
+static void rcar_drif_stop_streaming(struct vb2_queue *vq)
+{
+	struct rcar_drif_sdr *sdr = vb2_get_drv_priv(vq);
+	unsigned int i;
+
+	mutex_lock(&sdr->v4l2_mutex);
+
+	/* Stop hardware streaming */
+	rcar_drif_stop(sdr);
+
+	/* Return all queued buffers to vb2 */
+	rcar_drif_release_queued_bufs(sdr, VB2_BUF_STATE_ERROR);
+
+	/* Release buf */
+	rcar_drif_release_buf(sdr);
+
+	/* Release DMA channel resources */
+	rcar_drif_release_dmachannels(sdr);
+
+	for_each_rcar_drif_channel(i, &sdr->cur_ch_mask)
+		clk_disable_unprepare(sdr->ch[i]->clk);
+
+	mutex_unlock(&sdr->v4l2_mutex);
+}
+
+/* Vb2 ops */
+static const struct vb2_ops rcar_drif_vb2_ops = {
+	.queue_setup            = rcar_drif_queue_setup,
+	.buf_queue              = rcar_drif_buf_queue,
+	.start_streaming        = rcar_drif_start_streaming,
+	.stop_streaming         = rcar_drif_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+static int rcar_drif_querycap(struct file *file, void *fh,
+			      struct v4l2_capability *cap)
+{
+	struct rcar_drif_sdr *sdr = video_drvdata(file);
+
+	strlcpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
+	strlcpy(cap->card, sdr->vdev->name, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 sdr->vdev->name);
+
+	return 0;
+}
+
+static int rcar_drif_set_default_format(struct rcar_drif_sdr *sdr)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(formats); i++) {
+		/* Matching fmt based on required channels is set as default */
+		if (sdr->num_hw_ch == formats[i].num_ch) {
+			sdr->fmt = &formats[i];
+			sdr->cur_ch_mask = sdr->hw_ch_mask;
+			sdr->num_cur_ch = sdr->num_hw_ch;
+			dev_dbg(sdr->dev, "default fmt[%u]: mask %lu num %u\n",
+				i, sdr->cur_ch_mask, sdr->num_cur_ch);
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+static int rcar_drif_enum_fmt_sdr_cap(struct file *file, void *priv,
+				      struct v4l2_fmtdesc *f)
+{
+	if (f->index >= ARRAY_SIZE(formats))
+		return -EINVAL;
+
+	f->pixelformat = formats[f->index].pixelformat;
+
+	return 0;
+}
+
+static int rcar_drif_g_fmt_sdr_cap(struct file *file, void *priv,
+				   struct v4l2_format *f)
+{
+	struct rcar_drif_sdr *sdr = video_drvdata(file);
+
+	f->fmt.sdr.pixelformat = sdr->fmt->pixelformat;
+	f->fmt.sdr.buffersize = sdr->fmt->buffersize;
+
+	return 0;
+}
+
+static int rcar_drif_s_fmt_sdr_cap(struct file *file, void *priv,
+				   struct v4l2_format *f)
+{
+	struct rcar_drif_sdr *sdr = video_drvdata(file);
+	struct vb2_queue *q = &sdr->vb_queue;
+	unsigned int i;
+
+	if (vb2_is_busy(q))
+		return -EBUSY;
+
+	for (i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].pixelformat == f->fmt.sdr.pixelformat)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(formats))
+		i = 0;		/* Set the 1st format as default on no match */
+
+	sdr->fmt = &formats[i];
+	f->fmt.sdr.pixelformat = sdr->fmt->pixelformat;
+	f->fmt.sdr.buffersize = formats[i].buffersize;
+	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+
+	/*
+	 * If a format demands one channel only out of two
+	 * enabled channels, pick the 0th channel.
+	 */
+	if (formats[i].num_ch < sdr->num_hw_ch) {
+		sdr->cur_ch_mask = BIT(0);
+		sdr->num_cur_ch = formats[i].num_ch;
+	} else {
+		sdr->cur_ch_mask = sdr->hw_ch_mask;
+		sdr->num_cur_ch = sdr->num_hw_ch;
+	}
+
+	rdrif_dbg(sdr, "cur: idx %u mask %lu num %u\n",
+		  i, sdr->cur_ch_mask, sdr->num_cur_ch);
+
+	return 0;
+}
+
+static int rcar_drif_try_fmt_sdr_cap(struct file *file, void *priv,
+				     struct v4l2_format *f)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].pixelformat == f->fmt.sdr.pixelformat) {
+			f->fmt.sdr.buffersize = formats[i].buffersize;
+			return 0;
+		}
+	}
+
+	f->fmt.sdr.pixelformat = formats[0].pixelformat;
+	f->fmt.sdr.buffersize = formats[0].buffersize;
+	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+
+	return 0;
+}
+
+/* Tuner subdev ioctls */
+static int rcar_drif_enum_freq_bands(struct file *file, void *priv,
+				     struct v4l2_frequency_band *band)
+{
+	struct rcar_drif_sdr *sdr = video_drvdata(file);
+
+	return v4l2_subdev_call(sdr->ep.subdev, tuner, enum_freq_bands, band);
+}
+
+static int rcar_drif_g_frequency(struct file *file, void *priv,
+				 struct v4l2_frequency *f)
+{
+	struct rcar_drif_sdr *sdr = video_drvdata(file);
+
+	return v4l2_subdev_call(sdr->ep.subdev, tuner, g_frequency, f);
+}
+
+static int rcar_drif_s_frequency(struct file *file, void *priv,
+				 const struct v4l2_frequency *f)
+{
+	struct rcar_drif_sdr *sdr = video_drvdata(file);
+
+	return v4l2_subdev_call(sdr->ep.subdev, tuner, s_frequency, f);
+}
+
+static int rcar_drif_g_tuner(struct file *file, void *priv,
+			     struct v4l2_tuner *vt)
+{
+	struct rcar_drif_sdr *sdr = video_drvdata(file);
+
+	return v4l2_subdev_call(sdr->ep.subdev, tuner, g_tuner, vt);
+}
+
+static int rcar_drif_s_tuner(struct file *file, void *priv,
+			     const struct v4l2_tuner *vt)
+{
+	struct rcar_drif_sdr *sdr = video_drvdata(file);
+
+	return v4l2_subdev_call(sdr->ep.subdev, tuner, s_tuner, vt);
+}
+
+static const struct v4l2_ioctl_ops rcar_drif_ioctl_ops = {
+	.vidioc_querycap          = rcar_drif_querycap,
+
+	.vidioc_enum_fmt_sdr_cap  = rcar_drif_enum_fmt_sdr_cap,
+	.vidioc_g_fmt_sdr_cap     = rcar_drif_g_fmt_sdr_cap,
+	.vidioc_s_fmt_sdr_cap     = rcar_drif_s_fmt_sdr_cap,
+	.vidioc_try_fmt_sdr_cap   = rcar_drif_try_fmt_sdr_cap,
+
+	.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_streamon          = vb2_ioctl_streamon,
+	.vidioc_streamoff         = vb2_ioctl_streamoff,
+
+	.vidioc_s_frequency       = rcar_drif_s_frequency,
+	.vidioc_g_frequency       = rcar_drif_g_frequency,
+	.vidioc_s_tuner		  = rcar_drif_s_tuner,
+	.vidioc_g_tuner		  = rcar_drif_g_tuner,
+	.vidioc_enum_freq_bands   = rcar_drif_enum_freq_bands,
+	.vidioc_subscribe_event   = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+	.vidioc_log_status        = v4l2_ctrl_log_status,
+};
+
+static const struct v4l2_file_operations rcar_drif_fops = {
+	.owner                    = THIS_MODULE,
+	.open                     = v4l2_fh_open,
+	.release                  = vb2_fop_release,
+	.read                     = vb2_fop_read,
+	.poll                     = vb2_fop_poll,
+	.mmap                     = vb2_fop_mmap,
+	.unlocked_ioctl           = video_ioctl2,
+};
+
+static int rcar_drif_sdr_register(struct rcar_drif_sdr *sdr)
+{
+	int ret;
+
+	/* Init video_device structure */
+	sdr->vdev = video_device_alloc();
+	if (!sdr->vdev)
+		return -ENOMEM;
+
+	snprintf(sdr->vdev->name, sizeof(sdr->vdev->name), "R-Car DRIF");
+	sdr->vdev->fops = &rcar_drif_fops;
+	sdr->vdev->ioctl_ops = &rcar_drif_ioctl_ops;
+	sdr->vdev->release = video_device_release;
+	sdr->vdev->lock = &sdr->v4l2_mutex;
+	sdr->vdev->queue = &sdr->vb_queue;
+	sdr->vdev->queue->lock = &sdr->vb_queue_mutex;
+	sdr->vdev->ctrl_handler = &sdr->ctrl_hdl;
+	sdr->vdev->v4l2_dev = &sdr->v4l2_dev;
+	sdr->vdev->device_caps = V4L2_CAP_SDR_CAPTURE | V4L2_CAP_TUNER |
+		V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+	video_set_drvdata(sdr->vdev, sdr);
+
+	/* Register V4L2 SDR device */
+	ret = video_register_device(sdr->vdev, VFL_TYPE_SDR, -1);
+	if (ret) {
+		video_device_release(sdr->vdev);
+		sdr->vdev = NULL;
+		dev_err(sdr->dev, "failed video_register_device (%d)\n", ret);
+	}
+
+	return ret;
+}
+
+static void rcar_drif_sdr_unregister(struct rcar_drif_sdr *sdr)
+{
+	video_unregister_device(sdr->vdev);
+	sdr->vdev = NULL;
+}
+
+/* Sub-device bound callback */
+static int rcar_drif_notify_bound(struct v4l2_async_notifier *notifier,
+				   struct v4l2_subdev *subdev,
+				   struct v4l2_async_subdev *asd)
+{
+	struct rcar_drif_sdr *sdr =
+		container_of(notifier, struct rcar_drif_sdr, notifier);
+
+	if (sdr->ep.asd.match.fwnode.fwnode !=
+	    of_fwnode_handle(subdev->dev->of_node)) {
+		rdrif_err(sdr, "subdev %s cannot bind\n", subdev->name);
+		return -EINVAL;
+	}
+
+	v4l2_set_subdev_hostdata(subdev, sdr);
+	sdr->ep.subdev = subdev;
+	rdrif_dbg(sdr, "bound asd %s\n", subdev->name);
+
+	return 0;
+}
+
+/* Sub-device unbind callback */
+static void rcar_drif_notify_unbind(struct v4l2_async_notifier *notifier,
+				   struct v4l2_subdev *subdev,
+				   struct v4l2_async_subdev *asd)
+{
+	struct rcar_drif_sdr *sdr =
+		container_of(notifier, struct rcar_drif_sdr, notifier);
+
+	if (sdr->ep.subdev != subdev) {
+		rdrif_err(sdr, "subdev %s is not bound\n", subdev->name);
+		return;
+	}
+
+	/* Free ctrl handler if initialized */
+	v4l2_ctrl_handler_free(&sdr->ctrl_hdl);
+	sdr->v4l2_dev.ctrl_handler = NULL;
+	sdr->ep.subdev = NULL;
+
+	rcar_drif_sdr_unregister(sdr);
+	rdrif_dbg(sdr, "unbind asd %s\n", subdev->name);
+}
+
+/* Sub-device registered notification callback */
+static int rcar_drif_notify_complete(struct v4l2_async_notifier *notifier)
+{
+	struct rcar_drif_sdr *sdr =
+		container_of(notifier, struct rcar_drif_sdr, notifier);
+	int ret;
+
+	/*
+	 * The subdev tested at this point uses 4 controls. Using 10 as a worst
+	 * case scenario hint. When less controls are needed there will be some
+	 * unused memory and when more controls are needed the framework uses
+	 * hash to manage controls within this number.
+	 */
+	ret = v4l2_ctrl_handler_init(&sdr->ctrl_hdl, 10);
+	if (ret)
+		return -ENOMEM;
+
+	sdr->v4l2_dev.ctrl_handler = &sdr->ctrl_hdl;
+	ret = v4l2_device_register_subdev_nodes(&sdr->v4l2_dev);
+	if (ret) {
+		rdrif_err(sdr, "failed: register subdev nodes ret %d\n", ret);
+		goto error;
+	}
+
+	ret = v4l2_ctrl_add_handler(&sdr->ctrl_hdl,
+				    sdr->ep.subdev->ctrl_handler, NULL);
+	if (ret) {
+		rdrif_err(sdr, "failed: ctrl add hdlr ret %d\n", ret);
+		goto error;
+	}
+
+	ret = rcar_drif_sdr_register(sdr);
+	if (ret)
+		goto error;
+
+	return ret;
+
+error:
+	v4l2_ctrl_handler_free(&sdr->ctrl_hdl);
+
+	return ret;
+}
+
+/* Read endpoint properties */
+static void rcar_drif_get_ep_properties(struct rcar_drif_sdr *sdr,
+					struct fwnode_handle *fwnode)
+{
+	u32 val;
+
+	/* Set the I2S defaults for SIRMDR1*/
+	sdr->mdr1 = RCAR_DRIF_SIRMDR1_SYNCMD_LR | RCAR_DRIF_SIRMDR1_MSB_FIRST |
+		RCAR_DRIF_SIRMDR1_DTDL_1 | RCAR_DRIF_SIRMDR1_SYNCDL_0;
+
+	/* Parse sync polarity from endpoint */
+	if (!fwnode_property_read_u32(fwnode, "sync-active", &val))
+		sdr->mdr1 |= val ? RCAR_DRIF_SIRMDR1_SYNCAC_POL_HIGH :
+			RCAR_DRIF_SIRMDR1_SYNCAC_POL_LOW;
+	else
+		sdr->mdr1 |= RCAR_DRIF_SIRMDR1_SYNCAC_POL_HIGH; /* default */
+
+	dev_dbg(sdr->dev, "mdr1 0x%08x\n", sdr->mdr1);
+}
+
+/* Parse sub-devs (tuner) to find a matching device */
+static int rcar_drif_parse_subdevs(struct rcar_drif_sdr *sdr)
+{
+	struct v4l2_async_notifier *notifier = &sdr->notifier;
+	struct fwnode_handle *fwnode, *ep;
+
+	notifier->subdevs = devm_kzalloc(sdr->dev, sizeof(*notifier->subdevs),
+					 GFP_KERNEL);
+	if (!notifier->subdevs)
+		return -ENOMEM;
+
+	ep = fwnode_graph_get_next_endpoint(of_fwnode_handle(sdr->dev->of_node),
+					    NULL);
+	if (!ep)
+		return 0;
+
+	notifier->subdevs[notifier->num_subdevs] = &sdr->ep.asd;
+	fwnode = fwnode_graph_get_remote_port_parent(ep);
+	if (!fwnode) {
+		dev_warn(sdr->dev, "bad remote port parent\n");
+		fwnode_handle_put(ep);
+		return -EINVAL;
+	}
+
+	sdr->ep.asd.match.fwnode.fwnode = fwnode;
+	sdr->ep.asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
+	notifier->num_subdevs++;
+
+	/* Get the endpoint properties */
+	rcar_drif_get_ep_properties(sdr, ep);
+
+	fwnode_handle_put(fwnode);
+	fwnode_handle_put(ep);
+
+	return 0;
+}
+
+/* Check if the given device is the primary bond */
+static bool rcar_drif_primary_bond(struct platform_device *pdev)
+{
+	return of_property_read_bool(pdev->dev.of_node, "renesas,primary-bond");
+}
+
+/* Check if both devices of the bond are enabled */
+static struct device_node *rcar_drif_bond_enabled(struct platform_device *p)
+{
+	struct device_node *np;
+
+	np = of_parse_phandle(p->dev.of_node, "renesas,bonding", 0);
+	if (np && of_device_is_available(np))
+		return np;
+
+	return NULL;
+}
+
+/* Check if the bonded device is probed */
+static int rcar_drif_bond_available(struct rcar_drif_sdr *sdr,
+				    struct device_node *np)
+{
+	struct platform_device *pdev;
+	struct rcar_drif *ch;
+	int ret = 0;
+
+	pdev = of_find_device_by_node(np);
+	if (!pdev) {
+		dev_err(sdr->dev, "failed to get bonded device from node\n");
+		return -ENODEV;
+	}
+
+	device_lock(&pdev->dev);
+	ch = platform_get_drvdata(pdev);
+	if (ch) {
+		/* Update sdr data in the bonded device */
+		ch->sdr = sdr;
+
+		/* Update sdr with bonded device data */
+		sdr->ch[ch->num] = ch;
+		sdr->hw_ch_mask |= BIT(ch->num);
+	} else {
+		/* Defer */
+		dev_info(sdr->dev, "defer probe\n");
+		ret = -EPROBE_DEFER;
+	}
+	device_unlock(&pdev->dev);
+
+	put_device(&pdev->dev);
+
+	return ret;
+}
+
+/* V4L2 SDR device probe */
+static int rcar_drif_sdr_probe(struct rcar_drif_sdr *sdr)
+{
+	int ret;
+
+	/* Validate any supported format for enabled channels */
+	ret = rcar_drif_set_default_format(sdr);
+	if (ret) {
+		dev_err(sdr->dev, "failed to set default format\n");
+		return ret;
+	}
+
+	/* Set defaults */
+	sdr->hwbuf_size = RCAR_DRIF_DEFAULT_HWBUF_SIZE;
+
+	mutex_init(&sdr->v4l2_mutex);
+	mutex_init(&sdr->vb_queue_mutex);
+	spin_lock_init(&sdr->queued_bufs_lock);
+	spin_lock_init(&sdr->dma_lock);
+	INIT_LIST_HEAD(&sdr->queued_bufs);
+
+	/* Init videobuf2 queue structure */
+	sdr->vb_queue.type = V4L2_BUF_TYPE_SDR_CAPTURE;
+	sdr->vb_queue.io_modes = VB2_READ | VB2_MMAP | VB2_DMABUF;
+	sdr->vb_queue.drv_priv = sdr;
+	sdr->vb_queue.buf_struct_size = sizeof(struct rcar_drif_frame_buf);
+	sdr->vb_queue.ops = &rcar_drif_vb2_ops;
+	sdr->vb_queue.mem_ops = &vb2_vmalloc_memops;
+	sdr->vb_queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+
+	/* Init videobuf2 queue */
+	ret = vb2_queue_init(&sdr->vb_queue);
+	if (ret) {
+		dev_err(sdr->dev, "failed: vb2_queue_init ret %d\n", ret);
+		return ret;
+	}
+
+	/* Register the v4l2_device */
+	ret = v4l2_device_register(sdr->dev, &sdr->v4l2_dev);
+	if (ret) {
+		dev_err(sdr->dev, "failed: v4l2_device_register ret %d\n", ret);
+		return ret;
+	}
+
+	/*
+	 * Parse subdevs after v4l2_device_register because if the subdev
+	 * is already probed, bound and complete will be called immediately
+	 */
+	ret = rcar_drif_parse_subdevs(sdr);
+	if (ret)
+		goto error;
+
+	sdr->notifier.bound = rcar_drif_notify_bound;
+	sdr->notifier.unbind = rcar_drif_notify_unbind;
+	sdr->notifier.complete = rcar_drif_notify_complete;
+
+	/* Register notifier */
+	ret = v4l2_async_notifier_register(&sdr->v4l2_dev, &sdr->notifier);
+	if (ret < 0) {
+		dev_err(sdr->dev, "failed: notifier register ret %d\n", ret);
+		goto error;
+	}
+
+	return ret;
+
+error:
+	v4l2_device_unregister(&sdr->v4l2_dev);
+
+	return ret;
+}
+
+/* V4L2 SDR device remove */
+static void rcar_drif_sdr_remove(struct rcar_drif_sdr *sdr)
+{
+	v4l2_async_notifier_unregister(&sdr->notifier);
+	v4l2_device_unregister(&sdr->v4l2_dev);
+}
+
+/* DRIF channel probe */
+static int rcar_drif_probe(struct platform_device *pdev)
+{
+	struct rcar_drif_sdr *sdr;
+	struct device_node *np;
+	struct rcar_drif *ch;
+	struct resource	*res;
+	int ret;
+
+	/* Reserve memory for enabled channel */
+	ch = devm_kzalloc(&pdev->dev, sizeof(*ch), GFP_KERNEL);
+	if (!ch)
+		return -ENOMEM;
+
+	ch->pdev = pdev;
+
+	/* Module clock */
+	ch->clk = devm_clk_get(&pdev->dev, "fck");
+	if (IS_ERR(ch->clk)) {
+		ret = PTR_ERR(ch->clk);
+		dev_err(&pdev->dev, "clk get failed (%d)\n", ret);
+		return ret;
+	}
+
+	/* Register map */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	ch->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(ch->base)) {
+		ret = PTR_ERR(ch->base);
+		dev_err(&pdev->dev, "ioremap failed (%d)\n", ret);
+		return ret;
+	}
+	ch->start = res->start;
+	platform_set_drvdata(pdev, ch);
+
+	/* Check if both channels of the bond are enabled */
+	np = rcar_drif_bond_enabled(pdev);
+	if (np) {
+		/* Check if current channel acting as primary-bond */
+		if (!rcar_drif_primary_bond(pdev)) {
+			ch->num = 1;	/* Primary bond is channel 0 always */
+			of_node_put(np);
+			return 0;
+		}
+	}
+
+	/* Reserve memory for SDR structure */
+	sdr = devm_kzalloc(&pdev->dev, sizeof(*sdr), GFP_KERNEL);
+	if (!sdr) {
+		of_node_put(np);
+		return -ENOMEM;
+	}
+	ch->sdr = sdr;
+	sdr->dev = &pdev->dev;
+
+	/* Establish links between SDR and channel(s) */
+	sdr->ch[ch->num] = ch;
+	sdr->hw_ch_mask = BIT(ch->num);
+	if (np) {
+		/* Check if bonded device is ready */
+		ret = rcar_drif_bond_available(sdr, np);
+		of_node_put(np);
+		if (ret)
+			return ret;
+	}
+	sdr->num_hw_ch = hweight_long(sdr->hw_ch_mask);
+
+	return rcar_drif_sdr_probe(sdr);
+}
+
+/* DRIF channel remove */
+static int rcar_drif_remove(struct platform_device *pdev)
+{
+	struct rcar_drif *ch = platform_get_drvdata(pdev);
+	struct rcar_drif_sdr *sdr = ch->sdr;
+
+	/* Channel 0 will be the SDR instance */
+	if (ch->num)
+		return 0;
+
+	/* SDR instance */
+	rcar_drif_sdr_remove(sdr);
+
+	return 0;
+}
+
+/* FIXME: Implement suspend/resume support */
+static int __maybe_unused rcar_drif_suspend(struct device *dev)
+{
+	return 0;
+}
+
+static int __maybe_unused rcar_drif_resume(struct device *dev)
+{
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(rcar_drif_pm_ops, rcar_drif_suspend,
+			 rcar_drif_resume);
+
+static const struct of_device_id rcar_drif_of_table[] = {
+	{ .compatible = "renesas,rcar-gen3-drif" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, rcar_drif_of_table);
+
+#define RCAR_DRIF_DRV_NAME "rcar_drif"
+static struct platform_driver rcar_drif_driver = {
+	.driver = {
+		.name = RCAR_DRIF_DRV_NAME,
+		.of_match_table = of_match_ptr(rcar_drif_of_table),
+		.pm = &rcar_drif_pm_ops,
+		},
+	.probe = rcar_drif_probe,
+	.remove = rcar_drif_remove,
+};
+
+module_platform_driver(rcar_drif_driver);
+
+MODULE_DESCRIPTION("Renesas R-Car Gen3 DRIF driver");
+MODULE_ALIAS("platform:" RCAR_DRIF_DRV_NAME);
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Ramesh Shanmugasundaram <ramesh.shanmugasundaram@bp.renesas.com>");
diff --git a/drivers/media/platform/rcar_fdp1.c b/drivers/media/platform/rcar_fdp1.c
index 42f25d241edd..3ee51fc3bb50 100644
--- a/drivers/media/platform/rcar_fdp1.c
+++ b/drivers/media/platform/rcar_fdp1.c
@@ -1,5 +1,5 @@
 /*
- * Renesas RCar Fine Display Processor
+ * Renesas R-Car Fine Display Processor
  *
  * Video format converter and frame deinterlacer device.
  *
@@ -258,8 +258,9 @@ MODULE_PARM_DESC(debug, "activate debug info");
 
 /* Internal Data (HW Version) */
 #define FD1_IP_INTDATA			0x0800
-#define FD1_IP_H3			0x02010101
+#define FD1_IP_H3_ES1			0x02010101
 #define FD1_IP_M3W			0x02010202
+#define FD1_IP_H3			0x02010203
 
 /* LUTs */
 #define FD1_LUT_DIF_ADJ			0x1000
@@ -2359,12 +2360,15 @@ static int fdp1_probe(struct platform_device *pdev)
 
 	hw_version = fdp1_read(fdp1, FD1_IP_INTDATA);
 	switch (hw_version) {
-	case FD1_IP_H3:
-		dprintk(fdp1, "FDP1 Version R-Car H3\n");
+	case FD1_IP_H3_ES1:
+		dprintk(fdp1, "FDP1 Version R-Car H3 ES1\n");
 		break;
 	case FD1_IP_M3W:
 		dprintk(fdp1, "FDP1 Version R-Car M3-W\n");
 		break;
+	case FD1_IP_H3:
+		dprintk(fdp1, "FDP1 Version R-Car H3\n");
+		break;
 	default:
 		dev_err(fdp1->dev, "FDP1 Unidentifiable (0x%08x)\n",
 				hw_version);
diff --git a/drivers/media/platform/s3c-camif/camif-capture.c b/drivers/media/platform/s3c-camif/camif-capture.c
index 1b30be72f4f9..25c7a7d42292 100644
--- a/drivers/media/platform/s3c-camif/camif-capture.c
+++ b/drivers/media/platform/s3c-camif/camif-capture.c
@@ -80,7 +80,7 @@ static int s3c_camif_hw_init(struct camif_dev *camif, struct camif_vp *vp)
 	camif_hw_set_test_pattern(camif, camif->test_pattern);
 	if (variant->has_img_effect)
 		camif_hw_set_effect(camif, camif->colorfx,
-				camif->colorfx_cb, camif->colorfx_cr);
+				camif->colorfx_cr, camif->colorfx_cb);
 	if (variant->ip_revision == S3C6410_CAMIF_IP_REV)
 		camif_hw_set_input_path(vp);
 	camif_cfg_video_path(vp);
@@ -364,7 +364,7 @@ irqreturn_t s3c_camif_irq_handler(int irq, void *priv)
 		camif_hw_set_test_pattern(camif, camif->test_pattern);
 		if (camif->variant->has_img_effect)
 			camif_hw_set_effect(camif, camif->colorfx,
-				    camif->colorfx_cb, camif->colorfx_cr);
+				    camif->colorfx_cr, camif->colorfx_cb);
 		vp->state &= ~ST_VP_CONFIG;
 	}
 unlock:
diff --git a/drivers/media/platform/s5p-cec/s5p_cec.c b/drivers/media/platform/s5p-cec/s5p_cec.c
index 664937b61fa4..8e06071a7977 100644
--- a/drivers/media/platform/s5p-cec/s5p_cec.c
+++ b/drivers/media/platform/s5p-cec/s5p_cec.c
@@ -173,6 +173,7 @@ static int s5p_cec_probe(struct platform_device *pdev)
 	struct platform_device *hdmi_dev;
 	struct resource *res;
 	struct s5p_cec_dev *cec;
+	bool needs_hpd = of_property_read_bool(pdev->dev.of_node, "needs-hpd");
 	int ret;
 
 	np = of_parse_phandle(pdev->dev.of_node, "hdmi-phandle", 0);
@@ -221,7 +222,8 @@ static int s5p_cec_probe(struct platform_device *pdev)
 	cec->adap = cec_allocate_adapter(&s5p_cec_adap_ops, cec,
 		CEC_NAME,
 		CEC_CAP_LOG_ADDRS | CEC_CAP_TRANSMIT |
-		CEC_CAP_PASSTHROUGH | CEC_CAP_RC, 1);
+		CEC_CAP_PASSTHROUGH | CEC_CAP_RC |
+		(needs_hpd ? CEC_CAP_NEEDS_HPD : 0), 1);
 	ret = PTR_ERR_OR_ZERO(cec->adap);
 	if (ret)
 		return ret;
@@ -235,7 +237,7 @@ static int s5p_cec_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, cec);
 	pm_runtime_enable(dev);
 
-	dev_dbg(dev, "successfuly probed\n");
+	dev_dbg(dev, "successfully probed\n");
 	return 0;
 
 err_delete_adapter:
diff --git a/drivers/media/platform/s5p-cec/s5p_cec.h b/drivers/media/platform/s5p-cec/s5p_cec.h
index 7015845c1caa..8bcd8dc1aeb9 100644
--- a/drivers/media/platform/s5p-cec/s5p_cec.h
+++ b/drivers/media/platform/s5p-cec/s5p_cec.h
@@ -22,7 +22,6 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/timer.h>
-#include <linux/version.h>
 #include <linux/workqueue.h>
 #include <media/cec.h>
 
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
index 52dc7941db65..d1e3ebb22577 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -1099,10 +1099,10 @@ static bool s5p_jpeg_parse_hdr(struct s5p_jpeg_q_data *result,
 			       struct s5p_jpeg_ctx *ctx)
 {
 	int c, components = 0, notfound, n_dht = 0, n_dqt = 0;
-	unsigned int height, width, word, subsampling = 0, sos = 0, sof = 0,
-		     sof_len = 0;
-	unsigned int dht[S5P_JPEG_MAX_MARKER], dht_len[S5P_JPEG_MAX_MARKER],
-		     dqt[S5P_JPEG_MAX_MARKER], dqt_len[S5P_JPEG_MAX_MARKER];
+	unsigned int height = 0, width = 0, word, subsampling = 0;
+	unsigned int sos = 0, sof = 0, sof_len = 0;
+	unsigned int dht[S5P_JPEG_MAX_MARKER], dht_len[S5P_JPEG_MAX_MARKER];
+	unsigned int dqt[S5P_JPEG_MAX_MARKER], dqt_len[S5P_JPEG_MAX_MARKER];
 	long length;
 	struct s5p_jpeg_buffer jpeg_buffer;
 
@@ -2642,13 +2642,13 @@ static irqreturn_t s5p_jpeg_irq(int irq, void *dev_id)
 	if (curr_ctx->mode == S5P_JPEG_ENCODE)
 		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, payload_size);
 	v4l2_m2m_buf_done(dst_buf, state);
-	v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->fh.m2m_ctx);
 
 	curr_ctx->subsampling = s5p_jpeg_get_subsampling_mode(jpeg->regs);
 	spin_unlock(&jpeg->slock);
 
 	s5p_jpeg_clear_int(jpeg->regs);
 
+	v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->fh.m2m_ctx);
 	return IRQ_HANDLED;
 }
 
@@ -2707,11 +2707,12 @@ static irqreturn_t exynos4_jpeg_irq(int irq, void *priv)
 		v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_ERROR);
 	}
 
-	v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->fh.m2m_ctx);
 	if (jpeg->variant->version == SJPEG_EXYNOS4)
 		curr_ctx->subsampling = exynos4_jpeg_get_frame_fmt(jpeg->regs);
 
 	spin_unlock(&jpeg->slock);
+
+	v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->fh.m2m_ctx);
 	return IRQ_HANDLED;
 }
 
@@ -2770,10 +2771,15 @@ static irqreturn_t exynos3250_jpeg_irq(int irq, void *dev_id)
 	if (curr_ctx->mode == S5P_JPEG_ENCODE)
 		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, payload_size);
 	v4l2_m2m_buf_done(dst_buf, state);
-	v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->fh.m2m_ctx);
 
 	curr_ctx->subsampling =
 			exynos3250_jpeg_get_subsampling_mode(jpeg->regs);
+
+	spin_unlock(&jpeg->slock);
+
+	v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->fh.m2m_ctx);
+	return IRQ_HANDLED;
+
 exit_unlock:
 	spin_unlock(&jpeg->slock);
 	return IRQ_HANDLED;
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
index b41ee608c171..0913881219ff 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
@@ -1293,7 +1293,7 @@ static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
 	 * First set the output frame buffers
 	 */
 	if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
-		mfc_err("It seems that not all destionation buffers were mmaped\nMFC requires that all destination are mmaped before starting processing\n");
+		mfc_err("It seems that not all destination buffers were mmaped\nMFC requires that all destination are mmaped before starting processing\n");
 		return -EAGAIN;
 	}
 	if (list_empty(&ctx->src_queue)) {
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
index 85880e9106be..88dbb9c341ec 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -1650,7 +1650,7 @@ static inline int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
 	 * s5p_mfc_alloc_dec_buffers(ctx); */
 
 	if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
-		mfc_err("It seems that not all destionation buffers were\n"
+		mfc_err("It seems that not all destination buffers were\n"
 			"mmaped.MFC requires that all destination are mmaped\n"
 			"before starting processing.\n");
 		return -EAGAIN;
diff --git a/drivers/media/platform/sh_vou.c b/drivers/media/platform/sh_vou.c
index 992d61a8b961..871da2a2a91c 100644
--- a/drivers/media/platform/sh_vou.c
+++ b/drivers/media/platform/sh_vou.c
@@ -229,6 +229,7 @@ static void sh_vou_stream_config(struct sh_vou_device *vou_dev)
 		break;
 	case V4L2_PIX_FMT_RGB565:
 		dataswap ^= 1;
+		/* fall through */
 	case V4L2_PIX_FMT_RGB565X:
 		row_coeff = 2;
 		break;
@@ -815,6 +816,7 @@ static u32 sh_vou_ntsc_mode(enum sh_vou_bus_fmt bus_fmt)
 	default:
 		pr_warn("%s(): Invalid bus-format code %d, using default 8-bit\n",
 			__func__, bus_fmt);
+		/* fall through */
 	case SH_VOU_BUS_8BIT:
 		return 1;
 	case SH_VOU_BUS_16BIT:
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index 3c9421f4d8e3..45a0429d75bb 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -23,6 +23,7 @@
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/mutex.h>
+#include <linux/of_graph.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/regulator/consumer.h>
@@ -36,7 +37,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-dev.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 #include <media/videobuf2-v4l2.h>
 
 /* Default to VGA resolution */
@@ -1512,8 +1513,8 @@ static int soc_of_bind(struct soc_camera_host *ici,
 	if (!info)
 		return -ENOMEM;
 
-	info->sasd.asd.match.of.node = remote;
-	info->sasd.asd.match_type = V4L2_ASYNC_MATCH_OF;
+	info->sasd.asd.match.fwnode.fwnode = of_fwnode_handle(remote);
+	info->sasd.asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
 	info->subdev = &info->sasd.asd;
 
 	/* Or shall this be managed by the soc-camera device? */
diff --git a/drivers/media/platform/soc_camera/soc_mediabus.c b/drivers/media/platform/soc_camera/soc_mediabus.c
index e3e665e1c503..57581f626f4c 100644
--- a/drivers/media/platform/soc_camera/soc_mediabus.c
+++ b/drivers/media/platform/soc_camera/soc_mediabus.c
@@ -494,6 +494,7 @@ unsigned int soc_mbus_config_compatible(const struct v4l2_mbus_config *cfg,
 					V4L2_MBUS_HSYNC_ACTIVE_LOW);
 		vsync = common_flags & (V4L2_MBUS_VSYNC_ACTIVE_HIGH |
 					V4L2_MBUS_VSYNC_ACTIVE_LOW);
+		/* fall through */
 	case V4L2_MBUS_BT656:
 		pclk = common_flags & (V4L2_MBUS_PCLK_SAMPLE_RISING |
 				       V4L2_MBUS_PCLK_SAMPLE_FALLING);
diff --git a/drivers/media/platform/sti/cec/stih-cec.c b/drivers/media/platform/sti/cec/stih-cec.c
index 39ff55145a82..dccbdaebb7a8 100644
--- a/drivers/media/platform/sti/cec/stih-cec.c
+++ b/drivers/media/platform/sti/cec/stih-cec.c
@@ -1,6 +1,6 @@
 /*
  * STIH4xx CEC driver
- * Copyright (C) STMicroelectronic SA 2016
+ * Copyright (C) STMicroelectronics SA 2016
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -213,7 +213,8 @@ static int stih_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
 	for (i = 0; i < msg->len; i++)
 		writeb(msg->msg[i], cec->regs + CEC_TX_DATA_BASE + i);
 
-	/* Start transmission, configure hardware to add start and stop bits
+	/*
+	 * Start transmission, configure hardware to add start and stop bits
 	 * Signal free time is handled by the hardware
 	 */
 	writel(CEC_TX_AUTO_SOM_EN | CEC_TX_AUTO_EOM_EN | CEC_TX_START |
@@ -225,22 +226,21 @@ static int stih_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
 static void stih_tx_done(struct stih_cec *cec, u32 status)
 {
 	if (status & CEC_TX_ERROR) {
-		cec_transmit_done(cec->adap, CEC_TX_STATUS_ERROR, 0, 0, 0, 1);
+		cec_transmit_attempt_done(cec->adap, CEC_TX_STATUS_ERROR);
 		return;
 	}
 
 	if (status & CEC_TX_ARB_ERROR) {
-		cec_transmit_done(cec->adap,
-				  CEC_TX_STATUS_ARB_LOST, 1, 0, 0, 0);
+		cec_transmit_attempt_done(cec->adap, CEC_TX_STATUS_ARB_LOST);
 		return;
 	}
 
 	if (!(status & CEC_TX_ACK_GET_STS)) {
-		cec_transmit_done(cec->adap, CEC_TX_STATUS_NACK, 0, 1, 0, 0);
+		cec_transmit_attempt_done(cec->adap, CEC_TX_STATUS_NACK);
 		return;
 	}
 
-	cec_transmit_done(cec->adap, CEC_TX_STATUS_OK, 0, 0, 0, 0);
+	cec_transmit_attempt_done(cec->adap, CEC_TX_STATUS_OK);
 }
 
 static void stih_rx_done(struct stih_cec *cec, u32 status)
@@ -353,7 +353,7 @@ static int stih_cec_probe(struct platform_device *pdev)
 	cec->adap = cec_allocate_adapter(&sti_cec_adap_ops, cec,
 			CEC_NAME,
 			CEC_CAP_LOG_ADDRS | CEC_CAP_PASSTHROUGH |
-			CEC_CAP_TRANSMIT, 1);
+			CEC_CAP_TRANSMIT, CEC_MAX_LOG_ADDRS);
 	ret = PTR_ERR_OR_ZERO(cec->adap);
 	if (ret)
 		return ret;
diff --git a/drivers/media/platform/stm32/Makefile b/drivers/media/platform/stm32/Makefile
new file mode 100644
index 000000000000..07355091376b
--- /dev/null
+++ b/drivers/media/platform/stm32/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_VIDEO_STM32_DCMI) += stm32-dcmi.o
+obj-$(CONFIG_VIDEO_STM32_HDMI_CEC) += stm32-cec.o
diff --git a/drivers/media/platform/stm32/stm32-cec.c b/drivers/media/platform/stm32/stm32-cec.c
new file mode 100644
index 000000000000..9ab896b01ee8
--- /dev/null
+++ b/drivers/media/platform/stm32/stm32-cec.c
@@ -0,0 +1,362 @@
+/*
+ * STM32 CEC driver
+ * Copyright (C) STMicroelectronics SA 2017
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#include <media/cec.h>
+
+#define CEC_NAME	"stm32-cec"
+
+/* CEC registers  */
+#define CEC_CR		0x0000 /* Control Register */
+#define CEC_CFGR	0x0004 /* ConFiGuration Register */
+#define CEC_TXDR	0x0008 /* Rx data Register */
+#define CEC_RXDR	0x000C /* Rx data Register */
+#define CEC_ISR		0x0010 /* Interrupt and status Register */
+#define CEC_IER		0x0014 /* Interrupt enable Register */
+
+#define TXEOM		BIT(2)
+#define TXSOM		BIT(1)
+#define CECEN		BIT(0)
+
+#define LSTN		BIT(31)
+#define OAR		GENMASK(30, 16)
+#define SFTOP		BIT(8)
+#define BRDNOGEN	BIT(7)
+#define LBPEGEN		BIT(6)
+#define BREGEN		BIT(5)
+#define BRESTP		BIT(4)
+#define RXTOL		BIT(3)
+#define SFT		GENMASK(2, 0)
+#define FULL_CFG	(LSTN | SFTOP | BRDNOGEN | LBPEGEN | BREGEN | BRESTP \
+			 | RXTOL)
+
+#define TXACKE		BIT(12)
+#define TXERR		BIT(11)
+#define TXUDR		BIT(10)
+#define TXEND		BIT(9)
+#define TXBR		BIT(8)
+#define ARBLST		BIT(7)
+#define RXACKE		BIT(6)
+#define RXOVR		BIT(2)
+#define RXEND		BIT(1)
+#define RXBR		BIT(0)
+
+#define ALL_TX_IT	(TXEND | TXBR | TXACKE | TXERR | TXUDR | ARBLST)
+#define ALL_RX_IT	(RXEND | RXBR | RXACKE | RXOVR)
+
+struct stm32_cec {
+	struct cec_adapter	*adap;
+	struct device		*dev;
+	struct clk		*clk_cec;
+	struct clk		*clk_hdmi_cec;
+	struct reset_control	*rstc;
+	struct regmap		*regmap;
+	int			irq;
+	u32			irq_status;
+	struct cec_msg		rx_msg;
+	struct cec_msg		tx_msg;
+	int			tx_cnt;
+};
+
+static void cec_hw_init(struct stm32_cec *cec)
+{
+	regmap_update_bits(cec->regmap, CEC_CR, TXEOM | TXSOM | CECEN, 0);
+
+	regmap_update_bits(cec->regmap, CEC_IER, ALL_TX_IT | ALL_RX_IT,
+			   ALL_TX_IT | ALL_RX_IT);
+
+	regmap_update_bits(cec->regmap, CEC_CFGR, FULL_CFG, FULL_CFG);
+}
+
+static void stm32_tx_done(struct stm32_cec *cec, u32 status)
+{
+	if (status & (TXERR | TXUDR)) {
+		cec_transmit_done(cec->adap, CEC_TX_STATUS_ERROR,
+				  0, 0, 0, 1);
+		return;
+	}
+
+	if (status & ARBLST) {
+		cec_transmit_done(cec->adap, CEC_TX_STATUS_ARB_LOST,
+				  1, 0, 0, 0);
+		return;
+	}
+
+	if (status & TXACKE) {
+		cec_transmit_done(cec->adap, CEC_TX_STATUS_NACK,
+				  0, 1, 0, 0);
+		return;
+	}
+
+	if (cec->irq_status & TXBR) {
+		/* send next byte */
+		if (cec->tx_cnt < cec->tx_msg.len)
+			regmap_write(cec->regmap, CEC_TXDR,
+				     cec->tx_msg.msg[cec->tx_cnt++]);
+
+		/* TXEOM is set to command transmission of the last byte */
+		if (cec->tx_cnt == cec->tx_msg.len)
+			regmap_update_bits(cec->regmap, CEC_CR, TXEOM, TXEOM);
+	}
+
+	if (cec->irq_status & TXEND)
+		cec_transmit_done(cec->adap, CEC_TX_STATUS_OK, 0, 0, 0, 0);
+}
+
+static void stm32_rx_done(struct stm32_cec *cec, u32 status)
+{
+	if (cec->irq_status & (RXACKE | RXOVR)) {
+		cec->rx_msg.len = 0;
+		return;
+	}
+
+	if (cec->irq_status & RXBR) {
+		u32 val;
+
+		regmap_read(cec->regmap, CEC_RXDR, &val);
+		cec->rx_msg.msg[cec->rx_msg.len++] = val & 0xFF;
+	}
+
+	if (cec->irq_status & RXEND) {
+		cec_received_msg(cec->adap, &cec->rx_msg);
+		cec->rx_msg.len = 0;
+	}
+}
+
+static irqreturn_t stm32_cec_irq_thread(int irq, void *arg)
+{
+	struct stm32_cec *cec = arg;
+
+	if (cec->irq_status & ALL_TX_IT)
+		stm32_tx_done(cec, cec->irq_status);
+
+	if (cec->irq_status & ALL_RX_IT)
+		stm32_rx_done(cec, cec->irq_status);
+
+	cec->irq_status = 0;
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t stm32_cec_irq_handler(int irq, void *arg)
+{
+	struct stm32_cec *cec = arg;
+
+	regmap_read(cec->regmap, CEC_ISR, &cec->irq_status);
+
+	regmap_update_bits(cec->regmap, CEC_ISR,
+			   ALL_TX_IT | ALL_RX_IT,
+			   ALL_TX_IT | ALL_RX_IT);
+
+	return IRQ_WAKE_THREAD;
+}
+
+static int stm32_cec_adap_enable(struct cec_adapter *adap, bool enable)
+{
+	struct stm32_cec *cec = adap->priv;
+	int ret = 0;
+
+	if (enable) {
+		ret = clk_enable(cec->clk_cec);
+		if (ret)
+			dev_err(cec->dev, "fail to enable cec clock\n");
+
+		clk_enable(cec->clk_hdmi_cec);
+		regmap_update_bits(cec->regmap, CEC_CR, CECEN, CECEN);
+	} else {
+		clk_disable(cec->clk_cec);
+		clk_disable(cec->clk_hdmi_cec);
+		regmap_update_bits(cec->regmap, CEC_CR, CECEN, 0);
+	}
+
+	return ret;
+}
+
+static int stm32_cec_adap_log_addr(struct cec_adapter *adap, u8 logical_addr)
+{
+	struct stm32_cec *cec = adap->priv;
+	u32 oar = (1 << logical_addr) << 16;
+
+	regmap_update_bits(cec->regmap, CEC_CR, CECEN, 0);
+
+	if (logical_addr == CEC_LOG_ADDR_INVALID)
+		regmap_update_bits(cec->regmap, CEC_CFGR, OAR, 0);
+	else
+		regmap_update_bits(cec->regmap, CEC_CFGR, oar, oar);
+
+	regmap_update_bits(cec->regmap, CEC_CR, CECEN, CECEN);
+
+	return 0;
+}
+
+static int stm32_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
+				   u32 signal_free_time, struct cec_msg *msg)
+{
+	struct stm32_cec *cec = adap->priv;
+
+	/* Copy message */
+	cec->tx_msg = *msg;
+	cec->tx_cnt = 0;
+
+	/*
+	 * If the CEC message consists of only one byte,
+	 * TXEOM must be set before of TXSOM.
+	 */
+	if (cec->tx_msg.len == 1)
+		regmap_update_bits(cec->regmap, CEC_CR, TXEOM, TXEOM);
+
+	/* TXSOM is set to command transmission of the first byte */
+	regmap_update_bits(cec->regmap, CEC_CR, TXSOM, TXSOM);
+
+	/* Write the header (first byte of message) */
+	regmap_write(cec->regmap, CEC_TXDR, cec->tx_msg.msg[0]);
+	cec->tx_cnt++;
+
+	return 0;
+}
+
+static const struct cec_adap_ops stm32_cec_adap_ops = {
+	.adap_enable = stm32_cec_adap_enable,
+	.adap_log_addr = stm32_cec_adap_log_addr,
+	.adap_transmit = stm32_cec_adap_transmit,
+};
+
+static const struct regmap_config stm32_cec_regmap_cfg = {
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_stride = sizeof(u32),
+	.max_register = 0x14,
+	.fast_io = true,
+};
+
+static int stm32_cec_probe(struct platform_device *pdev)
+{
+	u32 caps = CEC_CAP_LOG_ADDRS | CEC_CAP_PASSTHROUGH |
+		   CEC_CAP_TRANSMIT | CEC_CAP_RC | CEC_CAP_PHYS_ADDR |
+		   CEC_MODE_MONITOR_ALL;
+	struct resource *res;
+	struct stm32_cec *cec;
+	void __iomem *mmio;
+	int ret;
+
+	cec = devm_kzalloc(&pdev->dev, sizeof(*cec), GFP_KERNEL);
+	if (!cec)
+		return -ENOMEM;
+
+	cec->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mmio = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(mmio))
+		return PTR_ERR(mmio);
+
+	cec->regmap = devm_regmap_init_mmio_clk(&pdev->dev, "cec", mmio,
+						&stm32_cec_regmap_cfg);
+
+	if (IS_ERR(cec->regmap))
+		return PTR_ERR(cec->regmap);
+
+	cec->irq = platform_get_irq(pdev, 0);
+	if (cec->irq < 0)
+		return cec->irq;
+
+	ret = devm_request_threaded_irq(&pdev->dev, cec->irq,
+					stm32_cec_irq_handler,
+					stm32_cec_irq_thread,
+					0,
+					pdev->name, cec);
+	if (ret)
+		return ret;
+
+	cec->clk_cec = devm_clk_get(&pdev->dev, "cec");
+	if (IS_ERR(cec->clk_cec)) {
+		dev_err(&pdev->dev, "Cannot get cec clock\n");
+		return PTR_ERR(cec->clk_cec);
+	}
+
+	ret = clk_prepare(cec->clk_cec);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to prepare cec clock\n");
+		return ret;
+	}
+
+	cec->clk_hdmi_cec = devm_clk_get(&pdev->dev, "hdmi-cec");
+	if (!IS_ERR(cec->clk_hdmi_cec)) {
+		ret = clk_prepare(cec->clk_hdmi_cec);
+		if (ret) {
+			dev_err(&pdev->dev, "Unable to prepare hdmi-cec clock\n");
+			return ret;
+		}
+	}
+
+	/*
+	 * CEC_CAP_PHYS_ADDR caps should be removed when a cec notifier is
+	 * available for example when a drm driver can provide edid
+	 */
+	cec->adap = cec_allocate_adapter(&stm32_cec_adap_ops, cec,
+			CEC_NAME, caps,	CEC_MAX_LOG_ADDRS);
+	ret = PTR_ERR_OR_ZERO(cec->adap);
+	if (ret)
+		return ret;
+
+	ret = cec_register_adapter(cec->adap, &pdev->dev);
+	if (ret) {
+		cec_delete_adapter(cec->adap);
+		return ret;
+	}
+
+	cec_hw_init(cec);
+
+	platform_set_drvdata(pdev, cec);
+
+	return 0;
+}
+
+static int stm32_cec_remove(struct platform_device *pdev)
+{
+	struct stm32_cec *cec = platform_get_drvdata(pdev);
+
+	clk_unprepare(cec->clk_cec);
+	clk_unprepare(cec->clk_hdmi_cec);
+
+	cec_unregister_adapter(cec->adap);
+
+	return 0;
+}
+
+static const struct of_device_id stm32_cec_of_match[] = {
+	{ .compatible = "st,stm32-cec" },
+	{ /* end node */ }
+};
+MODULE_DEVICE_TABLE(of, stm32_cec_of_match);
+
+static struct platform_driver stm32_cec_driver = {
+	.probe  = stm32_cec_probe,
+	.remove = stm32_cec_remove,
+	.driver = {
+		.name		= CEC_NAME,
+		.of_match_table = stm32_cec_of_match,
+	},
+};
+
+module_platform_driver(stm32_cec_driver);
+
+MODULE_AUTHOR("Benjamin Gaignard <benjamin.gaignard@st.com>");
+MODULE_AUTHOR("Yannick Fertre <yannick.fertre@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics STM32 Consumer Electronics Control");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/stm32/stm32-dcmi.c b/drivers/media/platform/stm32/stm32-dcmi.c
new file mode 100644
index 000000000000..83d32a5d0f40
--- /dev/null
+++ b/drivers/media/platform/stm32/stm32-dcmi.c
@@ -0,0 +1,1404 @@
+/*
+ * Driver for STM32 Digital Camera Memory Interface
+ *
+ * Copyright (C) STMicroelectronics SA 2017
+ * Authors: Yannick Fertre <yannick.fertre@st.com>
+ *          Hugues Fruchet <hugues.fruchet@st.com>
+ *          for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * This driver is based on atmel_isi.c
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/dmaengine.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <linux/reset.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+
+#define DRV_NAME "stm32-dcmi"
+
+/* Registers offset for DCMI */
+#define DCMI_CR		0x00 /* Control Register */
+#define DCMI_SR		0x04 /* Status Register */
+#define DCMI_RIS	0x08 /* Raw Interrupt Status register */
+#define DCMI_IER	0x0C /* Interrupt Enable Register */
+#define DCMI_MIS	0x10 /* Masked Interrupt Status register */
+#define DCMI_ICR	0x14 /* Interrupt Clear Register */
+#define DCMI_ESCR	0x18 /* Embedded Synchronization Code Register */
+#define DCMI_ESUR	0x1C /* Embedded Synchronization Unmask Register */
+#define DCMI_CWSTRT	0x20 /* Crop Window STaRT */
+#define DCMI_CWSIZE	0x24 /* Crop Window SIZE */
+#define DCMI_DR		0x28 /* Data Register */
+#define DCMI_IDR	0x2C /* IDentifier Register */
+
+/* Bits definition for control register (DCMI_CR) */
+#define CR_CAPTURE	BIT(0)
+#define CR_CM		BIT(1)
+#define CR_CROP		BIT(2)
+#define CR_JPEG		BIT(3)
+#define CR_ESS		BIT(4)
+#define CR_PCKPOL	BIT(5)
+#define CR_HSPOL	BIT(6)
+#define CR_VSPOL	BIT(7)
+#define CR_FCRC_0	BIT(8)
+#define CR_FCRC_1	BIT(9)
+#define CR_EDM_0	BIT(10)
+#define CR_EDM_1	BIT(11)
+#define CR_ENABLE	BIT(14)
+
+/* Bits definition for status register (DCMI_SR) */
+#define SR_HSYNC	BIT(0)
+#define SR_VSYNC	BIT(1)
+#define SR_FNE		BIT(2)
+
+/*
+ * Bits definition for interrupt registers
+ * (DCMI_RIS, DCMI_IER, DCMI_MIS, DCMI_ICR)
+ */
+#define IT_FRAME	BIT(0)
+#define IT_OVR		BIT(1)
+#define IT_ERR		BIT(2)
+#define IT_VSYNC	BIT(3)
+#define IT_LINE		BIT(4)
+
+enum state {
+	STOPPED = 0,
+	RUNNING,
+	STOPPING,
+};
+
+#define MIN_WIDTH	16U
+#define MAX_WIDTH	2048U
+#define MIN_HEIGHT	16U
+#define MAX_HEIGHT	2048U
+
+#define TIMEOUT_MS	1000
+
+struct dcmi_graph_entity {
+	struct device_node *node;
+
+	struct v4l2_async_subdev asd;
+	struct v4l2_subdev *subdev;
+};
+
+struct dcmi_format {
+	u32	fourcc;
+	u32	mbus_code;
+	u8	bpp;
+};
+
+struct dcmi_buf {
+	struct vb2_v4l2_buffer	vb;
+	bool			prepared;
+	dma_addr_t		paddr;
+	size_t			size;
+	struct list_head	list;
+};
+
+struct stm32_dcmi {
+	/* Protects the access of variables shared within the interrupt */
+	spinlock_t			irqlock;
+	struct device			*dev;
+	void __iomem			*regs;
+	struct resource			*res;
+	struct reset_control		*rstc;
+	int				sequence;
+	struct list_head		buffers;
+	struct dcmi_buf			*active;
+
+	struct v4l2_device		v4l2_dev;
+	struct video_device		*vdev;
+	struct v4l2_async_notifier	notifier;
+	struct dcmi_graph_entity	entity;
+	struct v4l2_format		fmt;
+
+	const struct dcmi_format	**user_formats;
+	unsigned int			num_user_formats;
+	const struct dcmi_format	*current_fmt;
+
+	/* Protect this data structure */
+	struct mutex			lock;
+	struct vb2_queue		queue;
+
+	struct v4l2_fwnode_bus_parallel	bus;
+	struct completion		complete;
+	struct clk			*mclk;
+	enum state			state;
+	struct dma_chan			*dma_chan;
+	dma_cookie_t			dma_cookie;
+	u32				misr;
+	int				errors_count;
+	int				buffers_count;
+};
+
+static inline struct stm32_dcmi *notifier_to_dcmi(struct v4l2_async_notifier *n)
+{
+	return container_of(n, struct stm32_dcmi, notifier);
+}
+
+static inline u32 reg_read(void __iomem *base, u32 reg)
+{
+	return readl_relaxed(base + reg);
+}
+
+static inline void reg_write(void __iomem *base, u32 reg, u32 val)
+{
+	writel_relaxed(val, base + reg);
+}
+
+static inline void reg_set(void __iomem *base, u32 reg, u32 mask)
+{
+	reg_write(base, reg, reg_read(base, reg) | mask);
+}
+
+static inline void reg_clear(void __iomem *base, u32 reg, u32 mask)
+{
+	reg_write(base, reg, reg_read(base, reg) & ~mask);
+}
+
+static int dcmi_start_capture(struct stm32_dcmi *dcmi);
+
+static void dcmi_dma_callback(void *param)
+{
+	struct stm32_dcmi *dcmi = (struct stm32_dcmi *)param;
+	struct dma_chan *chan = dcmi->dma_chan;
+	struct dma_tx_state state;
+	enum dma_status status;
+
+	spin_lock(&dcmi->irqlock);
+
+	/* Check DMA status */
+	status = dmaengine_tx_status(chan, dcmi->dma_cookie, &state);
+
+	switch (status) {
+	case DMA_IN_PROGRESS:
+		dev_dbg(dcmi->dev, "%s: Received DMA_IN_PROGRESS\n", __func__);
+		break;
+	case DMA_PAUSED:
+		dev_err(dcmi->dev, "%s: Received DMA_PAUSED\n", __func__);
+		break;
+	case DMA_ERROR:
+		dev_err(dcmi->dev, "%s: Received DMA_ERROR\n", __func__);
+		break;
+	case DMA_COMPLETE:
+		dev_dbg(dcmi->dev, "%s: Received DMA_COMPLETE\n", __func__);
+
+		if (dcmi->active) {
+			struct dcmi_buf *buf = dcmi->active;
+			struct vb2_v4l2_buffer *vbuf = &dcmi->active->vb;
+
+			vbuf->sequence = dcmi->sequence++;
+			vbuf->field = V4L2_FIELD_NONE;
+			vbuf->vb2_buf.timestamp = ktime_get_ns();
+			vb2_set_plane_payload(&vbuf->vb2_buf, 0, buf->size);
+			vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
+			dev_dbg(dcmi->dev, "buffer[%d] done seq=%d\n",
+				vbuf->vb2_buf.index, vbuf->sequence);
+
+			dcmi->buffers_count++;
+			dcmi->active = NULL;
+		}
+
+		/* Restart a new DMA transfer with next buffer */
+		if (dcmi->state == RUNNING) {
+			if (list_empty(&dcmi->buffers)) {
+				dev_err(dcmi->dev, "%s: No more buffer queued, cannot capture buffer",
+					__func__);
+				dcmi->errors_count++;
+				dcmi->active = NULL;
+
+				spin_unlock(&dcmi->irqlock);
+				return;
+			}
+
+			dcmi->active = list_entry(dcmi->buffers.next,
+						  struct dcmi_buf, list);
+
+			list_del_init(&dcmi->active->list);
+
+			if (dcmi_start_capture(dcmi)) {
+				dev_err(dcmi->dev, "%s: Cannot restart capture on DMA complete",
+					__func__);
+
+				spin_unlock(&dcmi->irqlock);
+				return;
+			}
+
+			/* Enable capture */
+			reg_set(dcmi->regs, DCMI_CR, CR_CAPTURE);
+		}
+
+		break;
+	default:
+		dev_err(dcmi->dev, "%s: Received unknown status\n", __func__);
+		break;
+	}
+
+	spin_unlock(&dcmi->irqlock);
+}
+
+static int dcmi_start_dma(struct stm32_dcmi *dcmi,
+			  struct dcmi_buf *buf)
+{
+	struct dma_async_tx_descriptor *desc = NULL;
+	struct dma_slave_config config;
+	int ret;
+
+	memset(&config, 0, sizeof(config));
+
+	config.src_addr = (dma_addr_t)dcmi->res->start + DCMI_DR;
+	config.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	config.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	config.dst_maxburst = 4;
+
+	/* Configure DMA channel */
+	ret = dmaengine_slave_config(dcmi->dma_chan, &config);
+	if (ret < 0) {
+		dev_err(dcmi->dev, "%s: DMA channel config failed (%d)\n",
+			__func__, ret);
+		return ret;
+	}
+
+	/* Prepare a DMA transaction */
+	desc = dmaengine_prep_slave_single(dcmi->dma_chan, buf->paddr,
+					   buf->size,
+					   DMA_DEV_TO_MEM, DMA_PREP_INTERRUPT);
+	if (!desc) {
+		dev_err(dcmi->dev, "%s: DMA dmaengine_prep_slave_single failed for buffer size %zu\n",
+			__func__, buf->size);
+		return -EINVAL;
+	}
+
+	/* Set completion callback routine for notification */
+	desc->callback = dcmi_dma_callback;
+	desc->callback_param = dcmi;
+
+	/* Push current DMA transaction in the pending queue */
+	dcmi->dma_cookie = dmaengine_submit(desc);
+
+	dma_async_issue_pending(dcmi->dma_chan);
+
+	return 0;
+}
+
+static int dcmi_start_capture(struct stm32_dcmi *dcmi)
+{
+	int ret;
+	struct dcmi_buf *buf = dcmi->active;
+
+	if (!buf)
+		return -EINVAL;
+
+	ret = dcmi_start_dma(dcmi, buf);
+	if (ret) {
+		dcmi->errors_count++;
+		return ret;
+	}
+
+	/* Enable capture */
+	reg_set(dcmi->regs, DCMI_CR, CR_CAPTURE);
+
+	return 0;
+}
+
+static irqreturn_t dcmi_irq_thread(int irq, void *arg)
+{
+	struct stm32_dcmi *dcmi = arg;
+
+	spin_lock(&dcmi->irqlock);
+
+	/* Stop capture is required */
+	if (dcmi->state == STOPPING) {
+		reg_clear(dcmi->regs, DCMI_IER, IT_FRAME | IT_OVR | IT_ERR);
+
+		dcmi->state = STOPPED;
+
+		complete(&dcmi->complete);
+
+		spin_unlock(&dcmi->irqlock);
+		return IRQ_HANDLED;
+	}
+
+	if ((dcmi->misr & IT_OVR) || (dcmi->misr & IT_ERR)) {
+		/*
+		 * An overflow or an error has been detected,
+		 * stop current DMA transfert & restart it
+		 */
+		dev_warn(dcmi->dev, "%s: Overflow or error detected\n",
+			 __func__);
+
+		dcmi->errors_count++;
+		dmaengine_terminate_all(dcmi->dma_chan);
+
+		reg_set(dcmi->regs, DCMI_ICR, IT_FRAME | IT_OVR | IT_ERR);
+
+		dev_dbg(dcmi->dev, "Restarting capture after DCMI error\n");
+
+		if (dcmi_start_capture(dcmi)) {
+			dev_err(dcmi->dev, "%s: Cannot restart capture on overflow or error\n",
+				__func__);
+
+			spin_unlock(&dcmi->irqlock);
+			return IRQ_HANDLED;
+		}
+	}
+
+	spin_unlock(&dcmi->irqlock);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t dcmi_irq_callback(int irq, void *arg)
+{
+	struct stm32_dcmi *dcmi = arg;
+
+	spin_lock(&dcmi->irqlock);
+
+	dcmi->misr = reg_read(dcmi->regs, DCMI_MIS);
+
+	/* Clear interrupt */
+	reg_set(dcmi->regs, DCMI_ICR, IT_FRAME | IT_OVR | IT_ERR);
+
+	spin_unlock(&dcmi->irqlock);
+
+	return IRQ_WAKE_THREAD;
+}
+
+static int dcmi_queue_setup(struct vb2_queue *vq,
+			    unsigned int *nbuffers,
+			    unsigned int *nplanes,
+			    unsigned int sizes[],
+			    struct device *alloc_devs[])
+{
+	struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq);
+	unsigned int size;
+
+	size = dcmi->fmt.fmt.pix.sizeimage;
+
+	/* Make sure the image size is large enough */
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
+
+	*nplanes = 1;
+	sizes[0] = size;
+
+	dcmi->active = NULL;
+
+	dev_dbg(dcmi->dev, "Setup queue, count=%d, size=%d\n",
+		*nbuffers, size);
+
+	return 0;
+}
+
+static int dcmi_buf_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct dcmi_buf *buf = container_of(vbuf, struct dcmi_buf, vb);
+
+	INIT_LIST_HEAD(&buf->list);
+
+	return 0;
+}
+
+static int dcmi_buf_prepare(struct vb2_buffer *vb)
+{
+	struct stm32_dcmi *dcmi =  vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct dcmi_buf *buf = container_of(vbuf, struct dcmi_buf, vb);
+	unsigned long size;
+
+	size = dcmi->fmt.fmt.pix.sizeimage;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		dev_err(dcmi->dev, "%s data will not fit into plane (%lu < %lu)\n",
+			__func__, vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+
+	if (!buf->prepared) {
+		/* Get memory addresses */
+		buf->paddr =
+			vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
+		buf->size = vb2_plane_size(&buf->vb.vb2_buf, 0);
+		buf->prepared = true;
+
+		vb2_set_plane_payload(&buf->vb.vb2_buf, 0, buf->size);
+
+		dev_dbg(dcmi->dev, "buffer[%d] phy=0x%pad size=%zu\n",
+			vb->index, &buf->paddr, buf->size);
+	}
+
+	return 0;
+}
+
+static void dcmi_buf_queue(struct vb2_buffer *vb)
+{
+	struct stm32_dcmi *dcmi =  vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct dcmi_buf *buf = container_of(vbuf, struct dcmi_buf, vb);
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&dcmi->irqlock, flags);
+
+	if ((dcmi->state == RUNNING) && (!dcmi->active)) {
+		dcmi->active = buf;
+
+		dev_dbg(dcmi->dev, "Starting capture on buffer[%d] queued\n",
+			buf->vb.vb2_buf.index);
+
+		if (dcmi_start_capture(dcmi)) {
+			dev_err(dcmi->dev, "%s: Cannot restart capture on overflow or error\n",
+				__func__);
+
+			spin_unlock_irqrestore(&dcmi->irqlock, flags);
+			return;
+		}
+	} else {
+		/* Enqueue to video buffers list */
+		list_add_tail(&buf->list, &dcmi->buffers);
+	}
+
+	spin_unlock_irqrestore(&dcmi->irqlock, flags);
+}
+
+static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq);
+	struct dcmi_buf *buf, *node;
+	u32 val;
+	int ret;
+
+	ret = clk_enable(dcmi->mclk);
+	if (ret) {
+		dev_err(dcmi->dev, "%s: Failed to start streaming, cannot enable clock",
+			__func__);
+		goto err_release_buffers;
+	}
+
+	/* Enable stream on the sub device */
+	ret = v4l2_subdev_call(dcmi->entity.subdev, video, s_stream, 1);
+	if (ret && ret != -ENOIOCTLCMD) {
+		dev_err(dcmi->dev, "%s: Failed to start streaming, subdev streamon error",
+			__func__);
+		goto err_disable_clock;
+	}
+
+	spin_lock_irq(&dcmi->irqlock);
+
+	val = reg_read(dcmi->regs, DCMI_CR);
+
+	val &= ~(CR_PCKPOL | CR_HSPOL | CR_VSPOL |
+		 CR_EDM_0 | CR_EDM_1 | CR_FCRC_0 |
+		 CR_FCRC_1 | CR_JPEG | CR_ESS);
+
+	/* Set bus width */
+	switch (dcmi->bus.bus_width) {
+	case 14:
+		val &= CR_EDM_0 + CR_EDM_1;
+		break;
+	case 12:
+		val &= CR_EDM_1;
+		break;
+	case 10:
+		val &= CR_EDM_0;
+		break;
+	default:
+		/* Set bus width to 8 bits by default */
+		break;
+	}
+
+	/* Set vertical synchronization polarity */
+	if (dcmi->bus.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+		val |= CR_VSPOL;
+
+	/* Set horizontal synchronization polarity */
+	if (dcmi->bus.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		val |= CR_HSPOL;
+
+	/* Set pixel clock polarity */
+	if (dcmi->bus.flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		val |= CR_PCKPOL;
+
+	reg_write(dcmi->regs, DCMI_CR, val);
+
+	/* Enable dcmi */
+	reg_set(dcmi->regs, DCMI_CR, CR_ENABLE);
+
+	dcmi->state = RUNNING;
+
+	dcmi->sequence = 0;
+	dcmi->errors_count = 0;
+	dcmi->buffers_count = 0;
+	dcmi->active = NULL;
+
+	/*
+	 * Start transfer if at least one buffer has been queued,
+	 * otherwise transfer is deferred at buffer queueing
+	 */
+	if (list_empty(&dcmi->buffers)) {
+		dev_dbg(dcmi->dev, "Start streaming is deferred to next buffer queueing\n");
+		spin_unlock_irq(&dcmi->irqlock);
+		return 0;
+	}
+
+	dcmi->active = list_entry(dcmi->buffers.next, struct dcmi_buf, list);
+	list_del_init(&dcmi->active->list);
+
+	dev_dbg(dcmi->dev, "Start streaming, starting capture\n");
+
+	ret = dcmi_start_capture(dcmi);
+	if (ret) {
+		dev_err(dcmi->dev, "%s: Start streaming failed, cannot start capture",
+			__func__);
+
+		spin_unlock_irq(&dcmi->irqlock);
+		goto err_subdev_streamoff;
+	}
+
+	/* Enable interruptions */
+	reg_set(dcmi->regs, DCMI_IER, IT_FRAME | IT_OVR | IT_ERR);
+
+	spin_unlock_irq(&dcmi->irqlock);
+
+	return 0;
+
+err_subdev_streamoff:
+	v4l2_subdev_call(dcmi->entity.subdev, video, s_stream, 0);
+
+err_disable_clock:
+	clk_disable(dcmi->mclk);
+
+err_release_buffers:
+	spin_lock_irq(&dcmi->irqlock);
+	/*
+	 * Return all buffers to vb2 in QUEUED state.
+	 * This will give ownership back to userspace
+	 */
+	if (dcmi->active) {
+		buf = dcmi->active;
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
+		dcmi->active = NULL;
+	}
+	list_for_each_entry_safe(buf, node, &dcmi->buffers, list) {
+		list_del_init(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
+	}
+	spin_unlock_irq(&dcmi->irqlock);
+
+	return ret;
+}
+
+static void dcmi_stop_streaming(struct vb2_queue *vq)
+{
+	struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq);
+	struct dcmi_buf *buf, *node;
+	unsigned long time_ms = msecs_to_jiffies(TIMEOUT_MS);
+	long timeout;
+	int ret;
+
+	/* Disable stream on the sub device */
+	ret = v4l2_subdev_call(dcmi->entity.subdev, video, s_stream, 0);
+	if (ret && ret != -ENOIOCTLCMD)
+		dev_err(dcmi->dev, "stream off failed in subdev\n");
+
+	dcmi->state = STOPPING;
+
+	timeout = wait_for_completion_interruptible_timeout(&dcmi->complete,
+							    time_ms);
+
+	spin_lock_irq(&dcmi->irqlock);
+
+	/* Disable interruptions */
+	reg_clear(dcmi->regs, DCMI_IER, IT_FRAME | IT_OVR | IT_ERR);
+
+	/* Disable DCMI */
+	reg_clear(dcmi->regs, DCMI_CR, CR_ENABLE);
+
+	if (!timeout) {
+		dev_err(dcmi->dev, "Timeout during stop streaming\n");
+		dcmi->state = STOPPED;
+	}
+
+	/* Return all queued buffers to vb2 in ERROR state */
+	if (dcmi->active) {
+		buf = dcmi->active;
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+		dcmi->active = NULL;
+	}
+	list_for_each_entry_safe(buf, node, &dcmi->buffers, list) {
+		list_del_init(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+
+	spin_unlock_irq(&dcmi->irqlock);
+
+	/* Stop all pending DMA operations */
+	dmaengine_terminate_all(dcmi->dma_chan);
+
+	clk_disable(dcmi->mclk);
+
+	dev_dbg(dcmi->dev, "Stop streaming, errors=%d buffers=%d\n",
+		dcmi->errors_count, dcmi->buffers_count);
+}
+
+static struct vb2_ops dcmi_video_qops = {
+	.queue_setup		= dcmi_queue_setup,
+	.buf_init		= dcmi_buf_init,
+	.buf_prepare		= dcmi_buf_prepare,
+	.buf_queue		= dcmi_buf_queue,
+	.start_streaming	= dcmi_start_streaming,
+	.stop_streaming		= dcmi_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+static int dcmi_g_fmt_vid_cap(struct file *file, void *priv,
+			      struct v4l2_format *fmt)
+{
+	struct stm32_dcmi *dcmi = video_drvdata(file);
+
+	*fmt = dcmi->fmt;
+
+	return 0;
+}
+
+static const struct dcmi_format *find_format_by_fourcc(struct stm32_dcmi *dcmi,
+						       unsigned int fourcc)
+{
+	unsigned int num_formats = dcmi->num_user_formats;
+	const struct dcmi_format *fmt;
+	unsigned int i;
+
+	for (i = 0; i < num_formats; i++) {
+		fmt = dcmi->user_formats[i];
+		if (fmt->fourcc == fourcc)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+static int dcmi_try_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f,
+			const struct dcmi_format **current_fmt)
+{
+	const struct dcmi_format *dcmi_fmt;
+	struct v4l2_pix_format *pixfmt = &f->fmt.pix;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	int ret;
+
+	dcmi_fmt = find_format_by_fourcc(dcmi, pixfmt->pixelformat);
+	if (!dcmi_fmt) {
+		dcmi_fmt = dcmi->user_formats[dcmi->num_user_formats - 1];
+		pixfmt->pixelformat = dcmi_fmt->fourcc;
+	}
+
+	/* Limit to hardware capabilities */
+	pixfmt->width = clamp(pixfmt->width, MIN_WIDTH, MAX_WIDTH);
+	pixfmt->height = clamp(pixfmt->height, MIN_HEIGHT, MAX_HEIGHT);
+
+	v4l2_fill_mbus_format(&format.format, pixfmt, dcmi_fmt->mbus_code);
+	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, set_fmt,
+			       &pad_cfg, &format);
+	if (ret < 0)
+		return ret;
+
+	v4l2_fill_pix_format(pixfmt, &format.format);
+
+	pixfmt->field = V4L2_FIELD_NONE;
+	pixfmt->bytesperline = pixfmt->width * dcmi_fmt->bpp;
+	pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
+
+	if (current_fmt)
+		*current_fmt = dcmi_fmt;
+
+	return 0;
+}
+
+static int dcmi_set_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f)
+{
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	const struct dcmi_format *current_fmt;
+	int ret;
+
+	ret = dcmi_try_fmt(dcmi, f, &current_fmt);
+	if (ret)
+		return ret;
+
+	v4l2_fill_mbus_format(&format.format, &f->fmt.pix,
+			      current_fmt->mbus_code);
+	ret = v4l2_subdev_call(dcmi->entity.subdev, pad,
+			       set_fmt, NULL, &format);
+	if (ret < 0)
+		return ret;
+
+	dcmi->fmt = *f;
+	dcmi->current_fmt = current_fmt;
+
+	return 0;
+}
+
+static int dcmi_s_fmt_vid_cap(struct file *file, void *priv,
+			      struct v4l2_format *f)
+{
+	struct stm32_dcmi *dcmi = video_drvdata(file);
+
+	if (vb2_is_streaming(&dcmi->queue))
+		return -EBUSY;
+
+	return dcmi_set_fmt(dcmi, f);
+}
+
+static int dcmi_try_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct stm32_dcmi *dcmi = video_drvdata(file);
+
+	return dcmi_try_fmt(dcmi, f, NULL);
+}
+
+static int dcmi_enum_fmt_vid_cap(struct file *file, void  *priv,
+				 struct v4l2_fmtdesc *f)
+{
+	struct stm32_dcmi *dcmi = video_drvdata(file);
+
+	if (f->index >= dcmi->num_user_formats)
+		return -EINVAL;
+
+	f->pixelformat = dcmi->user_formats[f->index]->fourcc;
+	return 0;
+}
+
+static int dcmi_querycap(struct file *file, void *priv,
+			 struct v4l2_capability *cap)
+{
+	strlcpy(cap->driver, DRV_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, "STM32 Camera Memory Interface",
+		sizeof(cap->card));
+	strlcpy(cap->bus_info, "platform:dcmi", sizeof(cap->bus_info));
+	return 0;
+}
+
+static int dcmi_enum_input(struct file *file, void *priv,
+			   struct v4l2_input *i)
+{
+	if (i->index != 0)
+		return -EINVAL;
+
+	i->type = V4L2_INPUT_TYPE_CAMERA;
+	strlcpy(i->name, "Camera", sizeof(i->name));
+	return 0;
+}
+
+static int dcmi_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int dcmi_s_input(struct file *file, void *priv, unsigned int i)
+{
+	if (i > 0)
+		return -EINVAL;
+	return 0;
+}
+
+static int dcmi_enum_framesizes(struct file *file, void *fh,
+				struct v4l2_frmsizeenum *fsize)
+{
+	struct stm32_dcmi *dcmi = video_drvdata(file);
+	const struct dcmi_format *dcmi_fmt;
+	struct v4l2_subdev_frame_size_enum fse = {
+		.index = fsize->index,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	dcmi_fmt = find_format_by_fourcc(dcmi, fsize->pixel_format);
+	if (!dcmi_fmt)
+		return -EINVAL;
+
+	fse.code = dcmi_fmt->mbus_code;
+
+	ret = v4l2_subdev_call(dcmi->entity.subdev, pad, enum_frame_size,
+			       NULL, &fse);
+	if (ret)
+		return ret;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = fse.max_width;
+	fsize->discrete.height = fse.max_height;
+
+	return 0;
+}
+
+static int dcmi_enum_frameintervals(struct file *file, void *fh,
+				    struct v4l2_frmivalenum *fival)
+{
+	struct stm32_dcmi *dcmi = video_drvdata(file);
+	const struct dcmi_format *dcmi_fmt;
+	struct v4l2_subdev_frame_interval_enum fie = {
+		.index = fival->index,
+		.width = fival->width,
+		.height = fival->height,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	dcmi_fmt = find_format_by_fourcc(dcmi, fival->pixel_format);
+	if (!dcmi_fmt)
+		return -EINVAL;
+
+	fie.code = dcmi_fmt->mbus_code;
+
+	ret = v4l2_subdev_call(dcmi->entity.subdev, pad,
+			       enum_frame_interval, NULL, &fie);
+	if (ret)
+		return ret;
+
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete = fie.interval;
+
+	return 0;
+}
+
+static const struct of_device_id stm32_dcmi_of_match[] = {
+	{ .compatible = "st,stm32-dcmi"},
+	{ /* end node */ },
+};
+MODULE_DEVICE_TABLE(of, stm32_dcmi_of_match);
+
+static int dcmi_open(struct file *file)
+{
+	struct stm32_dcmi *dcmi = video_drvdata(file);
+	struct v4l2_subdev *sd = dcmi->entity.subdev;
+	int ret;
+
+	if (mutex_lock_interruptible(&dcmi->lock))
+		return -ERESTARTSYS;
+
+	ret = v4l2_fh_open(file);
+	if (ret < 0)
+		goto unlock;
+
+	if (!v4l2_fh_is_singular_file(file))
+		goto fh_rel;
+
+	ret = v4l2_subdev_call(sd, core, s_power, 1);
+	if (ret < 0 && ret != -ENOIOCTLCMD)
+		goto fh_rel;
+
+	ret = dcmi_set_fmt(dcmi, &dcmi->fmt);
+	if (ret)
+		v4l2_subdev_call(sd, core, s_power, 0);
+fh_rel:
+	if (ret)
+		v4l2_fh_release(file);
+unlock:
+	mutex_unlock(&dcmi->lock);
+	return ret;
+}
+
+static int dcmi_release(struct file *file)
+{
+	struct stm32_dcmi *dcmi = video_drvdata(file);
+	struct v4l2_subdev *sd = dcmi->entity.subdev;
+	bool fh_singular;
+	int ret;
+
+	mutex_lock(&dcmi->lock);
+
+	fh_singular = v4l2_fh_is_singular_file(file);
+
+	ret = _vb2_fop_release(file, NULL);
+
+	if (fh_singular)
+		v4l2_subdev_call(sd, core, s_power, 0);
+
+	mutex_unlock(&dcmi->lock);
+
+	return ret;
+}
+
+static const struct v4l2_ioctl_ops dcmi_ioctl_ops = {
+	.vidioc_querycap		= dcmi_querycap,
+
+	.vidioc_try_fmt_vid_cap		= dcmi_try_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		= dcmi_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= dcmi_s_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_cap	= dcmi_enum_fmt_vid_cap,
+
+	.vidioc_enum_input		= dcmi_enum_input,
+	.vidioc_g_input			= dcmi_g_input,
+	.vidioc_s_input			= dcmi_s_input,
+
+	.vidioc_enum_framesizes		= dcmi_enum_framesizes,
+	.vidioc_enum_frameintervals	= dcmi_enum_frameintervals,
+
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.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,
+};
+
+static const struct v4l2_file_operations dcmi_fops = {
+	.owner		= THIS_MODULE,
+	.unlocked_ioctl	= video_ioctl2,
+	.open		= dcmi_open,
+	.release	= dcmi_release,
+	.poll		= vb2_fop_poll,
+	.mmap		= vb2_fop_mmap,
+#ifndef CONFIG_MMU
+	.get_unmapped_area = vb2_fop_get_unmapped_area,
+#endif
+	.read		= vb2_fop_read,
+};
+
+static int dcmi_set_default_fmt(struct stm32_dcmi *dcmi)
+{
+	struct v4l2_format f = {
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+		.fmt.pix = {
+			.width		= CIF_WIDTH,
+			.height		= CIF_HEIGHT,
+			.field		= V4L2_FIELD_NONE,
+			.pixelformat	= dcmi->user_formats[0]->fourcc,
+		},
+	};
+	int ret;
+
+	ret = dcmi_try_fmt(dcmi, &f, NULL);
+	if (ret)
+		return ret;
+	dcmi->current_fmt = dcmi->user_formats[0];
+	dcmi->fmt = f;
+	return 0;
+}
+
+static const struct dcmi_format dcmi_formats[] = {
+	{
+		.fourcc = V4L2_PIX_FMT_RGB565,
+		.mbus_code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+		.bpp = 2,
+	}, {
+		.fourcc = V4L2_PIX_FMT_YUYV,
+		.mbus_code = MEDIA_BUS_FMT_YUYV8_2X8,
+		.bpp = 2,
+	}, {
+		.fourcc = V4L2_PIX_FMT_UYVY,
+		.mbus_code = MEDIA_BUS_FMT_UYVY8_2X8,
+		.bpp = 2,
+	},
+};
+
+static int dcmi_formats_init(struct stm32_dcmi *dcmi)
+{
+	const struct dcmi_format *dcmi_fmts[ARRAY_SIZE(dcmi_formats)];
+	unsigned int num_fmts = 0, i, j;
+	struct v4l2_subdev *subdev = dcmi->entity.subdev;
+	struct v4l2_subdev_mbus_code_enum mbus_code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+
+	while (!v4l2_subdev_call(subdev, pad, enum_mbus_code,
+				 NULL, &mbus_code)) {
+		for (i = 0; i < ARRAY_SIZE(dcmi_formats); i++) {
+			if (dcmi_formats[i].mbus_code != mbus_code.code)
+				continue;
+
+			/* Code supported, have we got this fourcc yet? */
+			for (j = 0; j < num_fmts; j++)
+				if (dcmi_fmts[j]->fourcc ==
+						dcmi_formats[i].fourcc)
+					/* Already available */
+					break;
+			if (j == num_fmts)
+				/* New */
+				dcmi_fmts[num_fmts++] = dcmi_formats + i;
+		}
+		mbus_code.index++;
+	}
+
+	if (!num_fmts)
+		return -ENXIO;
+
+	dcmi->num_user_formats = num_fmts;
+	dcmi->user_formats = devm_kcalloc(dcmi->dev,
+					 num_fmts, sizeof(struct dcmi_format *),
+					 GFP_KERNEL);
+	if (!dcmi->user_formats) {
+		dev_err(dcmi->dev, "could not allocate memory\n");
+		return -ENOMEM;
+	}
+
+	memcpy(dcmi->user_formats, dcmi_fmts,
+	       num_fmts * sizeof(struct dcmi_format *));
+	dcmi->current_fmt = dcmi->user_formats[0];
+
+	return 0;
+}
+
+static int dcmi_graph_notify_complete(struct v4l2_async_notifier *notifier)
+{
+	struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);
+	int ret;
+
+	dcmi->vdev->ctrl_handler = dcmi->entity.subdev->ctrl_handler;
+	ret = dcmi_formats_init(dcmi);
+	if (ret) {
+		dev_err(dcmi->dev, "No supported mediabus format found\n");
+		return ret;
+	}
+
+	ret = dcmi_set_default_fmt(dcmi);
+	if (ret) {
+		dev_err(dcmi->dev, "Could not set default format\n");
+		return ret;
+	}
+
+	ret = video_register_device(dcmi->vdev, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		dev_err(dcmi->dev, "Failed to register video device\n");
+		return ret;
+	}
+
+	dev_dbg(dcmi->dev, "Device registered as %s\n",
+		video_device_node_name(dcmi->vdev));
+	return 0;
+}
+
+static void dcmi_graph_notify_unbind(struct v4l2_async_notifier *notifier,
+				     struct v4l2_subdev *sd,
+				     struct v4l2_async_subdev *asd)
+{
+	struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);
+
+	dev_dbg(dcmi->dev, "Removing %s\n", video_device_node_name(dcmi->vdev));
+
+	/* Checks internaly if vdev has been init or not */
+	video_unregister_device(dcmi->vdev);
+}
+
+static int dcmi_graph_notify_bound(struct v4l2_async_notifier *notifier,
+				   struct v4l2_subdev *subdev,
+				   struct v4l2_async_subdev *asd)
+{
+	struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);
+
+	dev_dbg(dcmi->dev, "Subdev %s bound\n", subdev->name);
+
+	dcmi->entity.subdev = subdev;
+
+	return 0;
+}
+
+static int dcmi_graph_parse(struct stm32_dcmi *dcmi, struct device_node *node)
+{
+	struct device_node *ep = NULL;
+	struct device_node *remote;
+
+	while (1) {
+		ep = of_graph_get_next_endpoint(node, ep);
+		if (!ep)
+			return -EINVAL;
+
+		remote = of_graph_get_remote_port_parent(ep);
+		if (!remote) {
+			of_node_put(ep);
+			return -EINVAL;
+		}
+
+		/* Remote node to connect */
+		dcmi->entity.node = remote;
+		dcmi->entity.asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
+		dcmi->entity.asd.match.fwnode.fwnode = of_fwnode_handle(remote);
+		return 0;
+	}
+}
+
+static int dcmi_graph_init(struct stm32_dcmi *dcmi)
+{
+	struct v4l2_async_subdev **subdevs = NULL;
+	int ret;
+
+	/* Parse the graph to extract a list of subdevice DT nodes. */
+	ret = dcmi_graph_parse(dcmi, dcmi->dev->of_node);
+	if (ret < 0) {
+		dev_err(dcmi->dev, "Graph parsing failed\n");
+		return ret;
+	}
+
+	/* Register the subdevices notifier. */
+	subdevs = devm_kzalloc(dcmi->dev, sizeof(*subdevs), GFP_KERNEL);
+	if (!subdevs) {
+		of_node_put(dcmi->entity.node);
+		return -ENOMEM;
+	}
+
+	subdevs[0] = &dcmi->entity.asd;
+
+	dcmi->notifier.subdevs = subdevs;
+	dcmi->notifier.num_subdevs = 1;
+	dcmi->notifier.bound = dcmi_graph_notify_bound;
+	dcmi->notifier.unbind = dcmi_graph_notify_unbind;
+	dcmi->notifier.complete = dcmi_graph_notify_complete;
+
+	ret = v4l2_async_notifier_register(&dcmi->v4l2_dev, &dcmi->notifier);
+	if (ret < 0) {
+		dev_err(dcmi->dev, "Notifier registration failed\n");
+		of_node_put(dcmi->entity.node);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int dcmi_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	const struct of_device_id *match = NULL;
+	struct v4l2_fwnode_endpoint ep;
+	struct stm32_dcmi *dcmi;
+	struct vb2_queue *q;
+	struct dma_chan *chan;
+	struct clk *mclk;
+	int irq;
+	int ret = 0;
+
+	match = of_match_device(of_match_ptr(stm32_dcmi_of_match), &pdev->dev);
+	if (!match) {
+		dev_err(&pdev->dev, "Could not find a match in devicetree\n");
+		return -ENODEV;
+	}
+
+	dcmi = devm_kzalloc(&pdev->dev, sizeof(struct stm32_dcmi), GFP_KERNEL);
+	if (!dcmi)
+		return -ENOMEM;
+
+	dcmi->rstc = devm_reset_control_get(&pdev->dev, NULL);
+	if (IS_ERR(dcmi->rstc)) {
+		dev_err(&pdev->dev, "Could not get reset control\n");
+		return -ENODEV;
+	}
+
+	/* Get bus characteristics from devicetree */
+	np = of_graph_get_next_endpoint(np, NULL);
+	if (!np) {
+		dev_err(&pdev->dev, "Could not find the endpoint\n");
+		of_node_put(np);
+		return -ENODEV;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(np), &ep);
+	if (ret) {
+		dev_err(&pdev->dev, "Could not parse the endpoint\n");
+		of_node_put(np);
+		return -ENODEV;
+	}
+
+	if (ep.bus_type == V4L2_MBUS_CSI2) {
+		dev_err(&pdev->dev, "CSI bus not supported\n");
+		of_node_put(np);
+		return -ENODEV;
+	}
+	dcmi->bus.flags = ep.bus.parallel.flags;
+	dcmi->bus.bus_width = ep.bus.parallel.bus_width;
+	dcmi->bus.data_shift = ep.bus.parallel.data_shift;
+
+	of_node_put(np);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq <= 0) {
+		dev_err(&pdev->dev, "Could not get irq\n");
+		return -ENODEV;
+	}
+
+	dcmi->res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!dcmi->res) {
+		dev_err(&pdev->dev, "Could not get resource\n");
+		return -ENODEV;
+	}
+
+	dcmi->regs = devm_ioremap_resource(&pdev->dev, dcmi->res);
+	if (IS_ERR(dcmi->regs)) {
+		dev_err(&pdev->dev, "Could not map registers\n");
+		return PTR_ERR(dcmi->regs);
+	}
+
+	ret = devm_request_threaded_irq(&pdev->dev, irq, dcmi_irq_callback,
+					dcmi_irq_thread, IRQF_ONESHOT,
+					dev_name(&pdev->dev), dcmi);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to request irq %d\n", irq);
+		return -ENODEV;
+	}
+
+	mclk = devm_clk_get(&pdev->dev, "mclk");
+	if (IS_ERR(mclk)) {
+		dev_err(&pdev->dev, "Unable to get mclk\n");
+		return PTR_ERR(mclk);
+	}
+
+	chan = dma_request_slave_channel(&pdev->dev, "tx");
+	if (!chan) {
+		dev_info(&pdev->dev, "Unable to request DMA channel, defer probing\n");
+		return -EPROBE_DEFER;
+	}
+
+	ret = clk_prepare(mclk);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to prepare mclk %p\n", mclk);
+		goto err_dma_release;
+	}
+
+	spin_lock_init(&dcmi->irqlock);
+	mutex_init(&dcmi->lock);
+	init_completion(&dcmi->complete);
+	INIT_LIST_HEAD(&dcmi->buffers);
+
+	dcmi->dev = &pdev->dev;
+	dcmi->mclk = mclk;
+	dcmi->state = STOPPED;
+	dcmi->dma_chan = chan;
+
+	q = &dcmi->queue;
+
+	/* Initialize the top-level structure */
+	ret = v4l2_device_register(&pdev->dev, &dcmi->v4l2_dev);
+	if (ret)
+		goto err_clk_unprepare;
+
+	dcmi->vdev = video_device_alloc();
+	if (!dcmi->vdev) {
+		ret = -ENOMEM;
+		goto err_device_unregister;
+	}
+
+	/* Video node */
+	dcmi->vdev->fops = &dcmi_fops;
+	dcmi->vdev->v4l2_dev = &dcmi->v4l2_dev;
+	dcmi->vdev->queue = &dcmi->queue;
+	strlcpy(dcmi->vdev->name, KBUILD_MODNAME, sizeof(dcmi->vdev->name));
+	dcmi->vdev->release = video_device_release;
+	dcmi->vdev->ioctl_ops = &dcmi_ioctl_ops;
+	dcmi->vdev->lock = &dcmi->lock;
+	dcmi->vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
+				  V4L2_CAP_READWRITE;
+	video_set_drvdata(dcmi->vdev, dcmi);
+
+	/* Buffer queue */
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_READ | VB2_DMABUF;
+	q->lock = &dcmi->lock;
+	q->drv_priv = dcmi;
+	q->buf_struct_size = sizeof(struct dcmi_buf);
+	q->ops = &dcmi_video_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->min_buffers_needed = 2;
+	q->dev = &pdev->dev;
+
+	ret = vb2_queue_init(q);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to initialize vb2 queue\n");
+		goto err_device_release;
+	}
+
+	ret = dcmi_graph_init(dcmi);
+	if (ret < 0)
+		goto err_device_release;
+
+	/* Reset device */
+	ret = reset_control_assert(dcmi->rstc);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to assert the reset line\n");
+		goto err_device_release;
+	}
+
+	usleep_range(3000, 5000);
+
+	ret = reset_control_deassert(dcmi->rstc);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to deassert the reset line\n");
+		goto err_device_release;
+	}
+
+	dev_info(&pdev->dev, "Probe done\n");
+
+	platform_set_drvdata(pdev, dcmi);
+	return 0;
+
+err_device_release:
+	video_device_release(dcmi->vdev);
+err_device_unregister:
+	v4l2_device_unregister(&dcmi->v4l2_dev);
+err_clk_unprepare:
+	clk_unprepare(dcmi->mclk);
+err_dma_release:
+	dma_release_channel(dcmi->dma_chan);
+
+	return ret;
+}
+
+static int dcmi_remove(struct platform_device *pdev)
+{
+	struct stm32_dcmi *dcmi = platform_get_drvdata(pdev);
+
+	v4l2_async_notifier_unregister(&dcmi->notifier);
+	v4l2_device_unregister(&dcmi->v4l2_dev);
+	clk_unprepare(dcmi->mclk);
+	dma_release_channel(dcmi->dma_chan);
+
+	return 0;
+}
+
+static struct platform_driver stm32_dcmi_driver = {
+	.probe		= dcmi_probe,
+	.remove		= dcmi_remove,
+	.driver		= {
+		.name = DRV_NAME,
+		.of_match_table = of_match_ptr(stm32_dcmi_of_match),
+	},
+};
+
+module_platform_driver(stm32_dcmi_driver);
+
+MODULE_AUTHOR("Yannick Fertre <yannick.fertre@st.com>");
+MODULE_AUTHOR("Hugues Fruchet <hugues.fruchet@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics STM32 Digital Camera Memory Interface driver");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/platform/ti-vpe/cal.c b/drivers/media/platform/ti-vpe/cal.c
index 7a058b6e03d0..177faa36bc16 100644
--- a/drivers/media/platform/ti-vpe/cal.c
+++ b/drivers/media/platform/ti-vpe/cal.c
@@ -21,7 +21,7 @@
 #include <linux/of_device.h>
 #include <linux/of_graph.h>
 
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 #include <media/v4l2-async.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
@@ -270,7 +270,7 @@ struct cal_ctx {
 	struct video_device	vdev;
 	struct v4l2_async_notifier notifier;
 	struct v4l2_subdev	*sensor;
-	struct v4l2_of_endpoint	endpoint;
+	struct v4l2_fwnode_endpoint	endpoint;
 
 	struct v4l2_async_subdev asd;
 	struct v4l2_async_subdev *asd_list[1];
@@ -608,7 +608,8 @@ static void csi2_lane_config(struct cal_ctx *ctx)
 	u32 val = reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port));
 	u32 lane_mask = CAL_CSI2_COMPLEXIO_CFG_CLOCK_POSITION_MASK;
 	u32 polarity_mask = CAL_CSI2_COMPLEXIO_CFG_CLOCK_POL_MASK;
-	struct v4l2_of_bus_mipi_csi2 *mipi_csi2 = &ctx->endpoint.bus.mipi_csi2;
+	struct v4l2_fwnode_bus_mipi_csi2 *mipi_csi2 =
+		&ctx->endpoint.bus.mipi_csi2;
 	int lane;
 
 	set_field(&val, mipi_csi2->clock_lane + 1, lane_mask);
@@ -1643,7 +1644,7 @@ static int of_cal_create_instance(struct cal_ctx *ctx, int inst)
 	struct platform_device *pdev = ctx->dev->pdev;
 	struct device_node *ep_node, *port, *remote_ep,
 			*sensor_node, *parent;
-	struct v4l2_of_endpoint *endpoint;
+	struct v4l2_fwnode_endpoint *endpoint;
 	struct v4l2_async_subdev *asd;
 	u32 regval = 0;
 	int ret, index, found_port = 0, lane;
@@ -1698,15 +1699,15 @@ static int of_cal_create_instance(struct cal_ctx *ctx, int inst)
 		ctx_dbg(3, ctx, "can't get remote parent\n");
 		goto cleanup_exit;
 	}
-	asd->match_type = V4L2_ASYNC_MATCH_OF;
-	asd->match.of.node = sensor_node;
+	asd->match_type = V4L2_ASYNC_MATCH_FWNODE;
+	asd->match.fwnode.fwnode = of_fwnode_handle(sensor_node);
 
 	remote_ep = of_parse_phandle(ep_node, "remote-endpoint", 0);
 	if (!remote_ep) {
 		ctx_dbg(3, ctx, "can't get remote-endpoint\n");
 		goto cleanup_exit;
 	}
-	v4l2_of_parse_endpoint(remote_ep, endpoint);
+	v4l2_fwnode_endpoint_parse(of_fwnode_handle(remote_ep), endpoint);
 
 	if (endpoint->bus_type != V4L2_MBUS_CSI2) {
 		ctx_err(ctx, "Port:%d sub-device %s is not a CSI2 device\n",
diff --git a/drivers/media/platform/video-mux.c b/drivers/media/platform/video-mux.c
new file mode 100644
index 000000000000..665744716f73
--- /dev/null
+++ b/drivers/media/platform/video-mux.c
@@ -0,0 +1,334 @@
+/*
+ * video stream multiplexer controlled via mux control
+ *
+ * Copyright (C) 2013 Pengutronix, Sascha Hauer <kernel@pengutronix.de>
+ * Copyright (C) 2016-2017 Pengutronix, Philipp Zabel <kernel@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/regmap.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+struct video_mux {
+	struct v4l2_subdev subdev;
+	struct media_pad *pads;
+	struct v4l2_mbus_framefmt *format_mbus;
+	struct regmap_field *field;
+	struct mutex lock;
+	int active;
+};
+
+static inline struct video_mux *v4l2_subdev_to_video_mux(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct video_mux, subdev);
+}
+
+static int video_mux_link_setup(struct media_entity *entity,
+				const struct media_pad *local,
+				const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
+	int ret = 0;
+
+	/*
+	 * The mux state is determined by the enabled sink pad link.
+	 * Enabling or disabling the source pad link has no effect.
+	 */
+	if (local->flags & MEDIA_PAD_FL_SOURCE)
+		return 0;
+
+	dev_dbg(sd->dev, "link setup '%s':%d->'%s':%d[%d]",
+		remote->entity->name, remote->index, local->entity->name,
+		local->index, flags & MEDIA_LNK_FL_ENABLED);
+
+	mutex_lock(&vmux->lock);
+
+	if (flags & MEDIA_LNK_FL_ENABLED) {
+		if (vmux->active == local->index)
+			goto out;
+
+		if (vmux->active >= 0) {
+			ret = -EBUSY;
+			goto out;
+		}
+
+		dev_dbg(sd->dev, "setting %d active\n", local->index);
+		ret = regmap_field_write(vmux->field, local->index);
+		if (ret < 0)
+			goto out;
+		vmux->active = local->index;
+	} else {
+		if (vmux->active != local->index)
+			goto out;
+
+		dev_dbg(sd->dev, "going inactive\n");
+		vmux->active = -1;
+	}
+
+out:
+	mutex_unlock(&vmux->lock);
+	return ret;
+}
+
+static const struct media_entity_operations video_mux_ops = {
+	.link_setup = video_mux_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int video_mux_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
+	struct v4l2_subdev *upstream_sd;
+	struct media_pad *pad;
+
+	if (vmux->active == -1) {
+		dev_err(sd->dev, "Can not start streaming on inactive mux\n");
+		return -EINVAL;
+	}
+
+	pad = media_entity_remote_pad(&sd->entity.pads[vmux->active]);
+	if (!pad) {
+		dev_err(sd->dev, "Failed to find remote source pad\n");
+		return -ENOLINK;
+	}
+
+	if (!is_media_entity_v4l2_subdev(pad->entity)) {
+		dev_err(sd->dev, "Upstream entity is not a v4l2 subdev\n");
+		return -ENODEV;
+	}
+
+	upstream_sd = media_entity_to_v4l2_subdev(pad->entity);
+
+	return v4l2_subdev_call(upstream_sd, video, s_stream, enable);
+}
+
+static const struct v4l2_subdev_video_ops video_mux_subdev_video_ops = {
+	.s_stream = video_mux_s_stream,
+};
+
+static struct v4l2_mbus_framefmt *
+__video_mux_get_pad_format(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_pad_config *cfg,
+			   unsigned int pad, u32 which)
+{
+	struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
+
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(sd, cfg, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &vmux->format_mbus[pad];
+	default:
+		return NULL;
+	}
+}
+
+static int video_mux_get_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_format *sdformat)
+{
+	struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
+
+	mutex_lock(&vmux->lock);
+
+	sdformat->format = *__video_mux_get_pad_format(sd, cfg, sdformat->pad,
+						       sdformat->which);
+
+	mutex_unlock(&vmux->lock);
+
+	return 0;
+}
+
+static int video_mux_set_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_format *sdformat)
+{
+	struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
+	struct v4l2_mbus_framefmt *mbusformat;
+	struct media_pad *pad = &vmux->pads[sdformat->pad];
+
+	mbusformat = __video_mux_get_pad_format(sd, cfg, sdformat->pad,
+					    sdformat->which);
+	if (!mbusformat)
+		return -EINVAL;
+
+	mutex_lock(&vmux->lock);
+
+	/* Source pad mirrors active sink pad, no limitations on sink pads */
+	if ((pad->flags & MEDIA_PAD_FL_SOURCE) && vmux->active >= 0)
+		sdformat->format = vmux->format_mbus[vmux->active];
+
+	*mbusformat = sdformat->format;
+
+	mutex_unlock(&vmux->lock);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops video_mux_pad_ops = {
+	.get_fmt = video_mux_get_format,
+	.set_fmt = video_mux_set_format,
+};
+
+static const struct v4l2_subdev_ops video_mux_subdev_ops = {
+	.pad = &video_mux_pad_ops,
+	.video = &video_mux_subdev_video_ops,
+};
+
+static int video_mux_probe_mmio_mux(struct video_mux *vmux)
+{
+	struct device *dev = vmux->subdev.dev;
+	struct of_phandle_args args;
+	struct reg_field field;
+	struct regmap *regmap;
+	u32 reg, mask;
+	int ret;
+
+	ret = of_parse_phandle_with_args(dev->of_node, "mux-controls",
+					 "#mux-control-cells", 0, &args);
+	if (ret)
+		return ret;
+
+	if (!of_device_is_compatible(args.np, "mmio-mux"))
+		return -EINVAL;
+
+	regmap = syscon_node_to_regmap(args.np->parent);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	ret = of_property_read_u32_index(args.np, "mux-reg-masks",
+					 2 * args.args[0], &reg);
+	if (!ret)
+		ret = of_property_read_u32_index(args.np, "mux-reg-masks",
+						 2 * args.args[0] + 1, &mask);
+	if (ret < 0)
+		return ret;
+
+	field.reg = reg;
+	field.msb = fls(mask) - 1;
+	field.lsb = ffs(mask) - 1;
+
+	vmux->field = devm_regmap_field_alloc(dev, regmap, field);
+	if (IS_ERR(vmux->field))
+		return PTR_ERR(vmux->field);
+
+	return 0;
+}
+
+static int video_mux_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct device *dev = &pdev->dev;
+	struct device_node *ep;
+	struct video_mux *vmux;
+	unsigned int num_pads = 0;
+	int ret;
+	int i;
+
+	vmux = devm_kzalloc(dev, sizeof(*vmux), GFP_KERNEL);
+	if (!vmux)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, vmux);
+
+	v4l2_subdev_init(&vmux->subdev, &video_mux_subdev_ops);
+	snprintf(vmux->subdev.name, sizeof(vmux->subdev.name), "%s", np->name);
+	vmux->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	vmux->subdev.dev = dev;
+
+	/*
+	 * The largest numbered port is the output port. It determines
+	 * total number of pads.
+	 */
+	for_each_endpoint_of_node(np, ep) {
+		struct of_endpoint endpoint;
+
+		of_graph_parse_endpoint(ep, &endpoint);
+		num_pads = max(num_pads, endpoint.port + 1);
+	}
+
+	if (num_pads < 2) {
+		dev_err(dev, "Not enough ports %d\n", num_pads);
+		return -EINVAL;
+	}
+
+	ret = video_mux_probe_mmio_mux(vmux);
+	if (ret) {
+		if (ret != -EPROBE_DEFER)
+			dev_err(dev, "Failed to get mux: %d\n", ret);
+		return ret;
+	}
+
+	mutex_init(&vmux->lock);
+	vmux->active = -1;
+	vmux->pads = devm_kcalloc(dev, num_pads, sizeof(*vmux->pads),
+				  GFP_KERNEL);
+	vmux->format_mbus = devm_kcalloc(dev, num_pads,
+					 sizeof(*vmux->format_mbus),
+					 GFP_KERNEL);
+
+	for (i = 0; i < num_pads - 1; i++)
+		vmux->pads[i].flags = MEDIA_PAD_FL_SINK;
+	vmux->pads[num_pads - 1].flags = MEDIA_PAD_FL_SOURCE;
+
+	vmux->subdev.entity.function = MEDIA_ENT_F_VID_MUX;
+	ret = media_entity_pads_init(&vmux->subdev.entity, num_pads,
+				     vmux->pads);
+	if (ret < 0)
+		return ret;
+
+	vmux->subdev.entity.ops = &video_mux_ops;
+
+	return v4l2_async_register_subdev(&vmux->subdev);
+}
+
+static int video_mux_remove(struct platform_device *pdev)
+{
+	struct video_mux *vmux = platform_get_drvdata(pdev);
+	struct v4l2_subdev *sd = &vmux->subdev;
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+
+	return 0;
+}
+
+static const struct of_device_id video_mux_dt_ids[] = {
+	{ .compatible = "video-mux", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, video_mux_dt_ids);
+
+static struct platform_driver video_mux_driver = {
+	.probe		= video_mux_probe,
+	.remove		= video_mux_remove,
+	.driver		= {
+		.of_match_table = video_mux_dt_ids,
+		.name = "video-mux",
+	},
+};
+
+module_platform_driver(video_mux_driver);
+
+MODULE_DESCRIPTION("video stream multiplexer");
+MODULE_AUTHOR("Sascha Hauer, Pengutronix");
+MODULE_AUTHOR("Philipp Zabel, Pengutronix");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/vimc/Kconfig b/drivers/media/platform/vimc/Kconfig
index a18f6352c422..71c9fe7d3370 100644
--- a/drivers/media/platform/vimc/Kconfig
+++ b/drivers/media/platform/vimc/Kconfig
@@ -2,6 +2,7 @@ config VIDEO_VIMC
 	tristate "Virtual Media Controller Driver (VIMC)"
 	depends on VIDEO_DEV && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
 	select VIDEOBUF2_VMALLOC
+	select VIDEO_V4L2_TPG
 	default n
 	---help---
 	  Skeleton driver for Virtual Media Controller
diff --git a/drivers/media/platform/vimc/Makefile b/drivers/media/platform/vimc/Makefile
index c45195e5e05c..68c5d9804c11 100644
--- a/drivers/media/platform/vimc/Makefile
+++ b/drivers/media/platform/vimc/Makefile
@@ -1,3 +1,9 @@
-vimc-objs := vimc-core.o vimc-capture.o vimc-sensor.o
+vimc-objs := vimc-core.o
+vimc_capture-objs := vimc-capture.o
+vimc_common-objs := vimc-common.o
+vimc_debayer-objs := vimc-debayer.o
+vimc_scaler-objs := vimc-scaler.o
+vimc_sensor-objs := vimc-sensor.o
 
-obj-$(CONFIG_VIDEO_VIMC) += vimc.o
+obj-$(CONFIG_VIDEO_VIMC) += vimc.o vimc_capture.o vimc_common.o vimc-debayer.o \
+				vimc_scaler.o vimc_sensor.o
diff --git a/drivers/media/platform/vimc/vimc-capture.c b/drivers/media/platform/vimc/vimc-capture.c
index 9adb06d7e13d..14cb32e21130 100644
--- a/drivers/media/platform/vimc/vimc-capture.c
+++ b/drivers/media/platform/vimc/vimc-capture.c
@@ -15,15 +15,21 @@
  *
  */
 
+#include <linux/component.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/videobuf2-core.h>
 #include <media/videobuf2-vmalloc.h>
 
-#include "vimc-capture.h"
+#include "vimc-common.h"
+
+#define VIMC_CAP_DRV_NAME "vimc-capture"
 
 struct vimc_cap_device {
 	struct vimc_ent_device ved;
 	struct video_device vdev;
+	struct device *dev;
 	struct v4l2_pix_format format;
 	struct vb2_queue queue;
 	struct list_head buf_list;
@@ -40,6 +46,14 @@ struct vimc_cap_device {
 	struct media_pipeline pipe;
 };
 
+static const struct v4l2_pix_format fmt_default = {
+	.width = 640,
+	.height = 480,
+	.pixelformat = V4L2_PIX_FMT_RGB24,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = V4L2_COLORSPACE_DEFAULT,
+};
+
 struct vimc_cap_buffer {
 	/*
 	 * struct vb2_v4l2_buffer must be the first element
@@ -64,7 +78,16 @@ static int vimc_cap_querycap(struct file *file, void *priv,
 	return 0;
 }
 
-static int vimc_cap_fmt_vid_cap(struct file *file, void *priv,
+static void vimc_cap_get_format(struct vimc_ent_device *ved,
+				struct v4l2_pix_format *fmt)
+{
+	struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
+						    ved);
+
+	*fmt = vcap->format;
+}
+
+static int vimc_cap_g_fmt_vid_cap(struct file *file, void *priv,
 				  struct v4l2_format *f)
 {
 	struct vimc_cap_device *vcap = video_drvdata(file);
@@ -74,16 +97,98 @@ static int vimc_cap_fmt_vid_cap(struct file *file, void *priv,
 	return 0;
 }
 
+static int vimc_cap_try_fmt_vid_cap(struct file *file, void *priv,
+				    struct v4l2_format *f)
+{
+	struct v4l2_pix_format *format = &f->fmt.pix;
+	const struct vimc_pix_map *vpix;
+
+	format->width = clamp_t(u32, format->width, VIMC_FRAME_MIN_WIDTH,
+				VIMC_FRAME_MAX_WIDTH) & ~1;
+	format->height = clamp_t(u32, format->height, VIMC_FRAME_MIN_HEIGHT,
+				 VIMC_FRAME_MAX_HEIGHT) & ~1;
+
+	/* Don't accept a pixelformat that is not on the table */
+	vpix = vimc_pix_map_by_pixelformat(format->pixelformat);
+	if (!vpix) {
+		format->pixelformat = fmt_default.pixelformat;
+		vpix = vimc_pix_map_by_pixelformat(format->pixelformat);
+	}
+	/* TODO: Add support for custom bytesperline values */
+	format->bytesperline = format->width * vpix->bpp;
+	format->sizeimage = format->bytesperline * format->height;
+
+	if (format->field == V4L2_FIELD_ANY)
+		format->field = fmt_default.field;
+
+	vimc_colorimetry_clamp(format);
+
+	return 0;
+}
+
+static int vimc_cap_s_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct vimc_cap_device *vcap = video_drvdata(file);
+
+	/* Do not change the format while stream is on */
+	if (vb2_is_busy(&vcap->queue))
+		return -EBUSY;
+
+	vimc_cap_try_fmt_vid_cap(file, priv, f);
+
+	dev_dbg(vcap->dev, "%s: format update: "
+		"old:%dx%d (0x%x, %d, %d, %d, %d) "
+		"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vcap->vdev.name,
+		/* old */
+		vcap->format.width, vcap->format.height,
+		vcap->format.pixelformat, vcap->format.colorspace,
+		vcap->format.quantization, vcap->format.xfer_func,
+		vcap->format.ycbcr_enc,
+		/* new */
+		f->fmt.pix.width, f->fmt.pix.height,
+		f->fmt.pix.pixelformat,	f->fmt.pix.colorspace,
+		f->fmt.pix.quantization, f->fmt.pix.xfer_func,
+		f->fmt.pix.ycbcr_enc);
+
+	vcap->format = f->fmt.pix;
+
+	return 0;
+}
+
 static int vimc_cap_enum_fmt_vid_cap(struct file *file, void *priv,
 				     struct v4l2_fmtdesc *f)
 {
-	struct vimc_cap_device *vcap = video_drvdata(file);
+	const struct vimc_pix_map *vpix = vimc_pix_map_by_index(f->index);
+
+	if (!vpix)
+		return -EINVAL;
+
+	f->pixelformat = vpix->pixelformat;
+
+	return 0;
+}
+
+static int vimc_cap_enum_framesizes(struct file *file, void *fh,
+				    struct v4l2_frmsizeenum *fsize)
+{
+	const struct vimc_pix_map *vpix;
+
+	if (fsize->index)
+		return -EINVAL;
 
-	if (f->index > 0)
+	/* Only accept code in the pix map table */
+	vpix = vimc_pix_map_by_code(fsize->pixel_format);
+	if (!vpix)
 		return -EINVAL;
 
-	/* We only support one format for now */
-	f->pixelformat = vcap->format.pixelformat;
+	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+	fsize->stepwise.min_width = VIMC_FRAME_MIN_WIDTH;
+	fsize->stepwise.max_width = VIMC_FRAME_MAX_WIDTH;
+	fsize->stepwise.min_height = VIMC_FRAME_MIN_HEIGHT;
+	fsize->stepwise.max_height = VIMC_FRAME_MAX_HEIGHT;
+	fsize->stepwise.step_width = 2;
+	fsize->stepwise.step_height = 2;
 
 	return 0;
 }
@@ -101,10 +206,11 @@ static const struct v4l2_file_operations vimc_cap_fops = {
 static const struct v4l2_ioctl_ops vimc_cap_ioctl_ops = {
 	.vidioc_querycap = vimc_cap_querycap,
 
-	.vidioc_g_fmt_vid_cap = vimc_cap_fmt_vid_cap,
-	.vidioc_s_fmt_vid_cap = vimc_cap_fmt_vid_cap,
-	.vidioc_try_fmt_vid_cap = vimc_cap_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap = vimc_cap_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap = vimc_cap_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap = vimc_cap_try_fmt_vid_cap,
 	.vidioc_enum_fmt_vid_cap = vimc_cap_enum_fmt_vid_cap,
+	.vidioc_enum_framesizes = vimc_cap_enum_framesizes,
 
 	.vidioc_reqbufs = vb2_ioctl_reqbufs,
 	.vidioc_create_bufs = vb2_ioctl_create_bufs,
@@ -132,31 +238,6 @@ static void vimc_cap_return_all_buffers(struct vimc_cap_device *vcap,
 	spin_unlock(&vcap->qlock);
 }
 
-static int vimc_cap_pipeline_s_stream(struct vimc_cap_device *vcap, int enable)
-{
-	struct v4l2_subdev *sd;
-	struct media_pad *pad;
-	int ret;
-
-	/* Start the stream in the subdevice direct connected */
-	pad = media_entity_remote_pad(&vcap->vdev.entity.pads[0]);
-
-	/*
-	 * if it is a raw node from vimc-core, there is nothing to activate
-	 * TODO: remove this when there are no more raw nodes in the
-	 * core and return error instead
-	 */
-	if (pad->entity->obj_type == MEDIA_ENTITY_TYPE_BASE)
-		return 0;
-
-	sd = media_entity_to_v4l2_subdev(pad->entity);
-	ret = v4l2_subdev_call(sd, video, s_stream, enable);
-	if (ret && ret != -ENOIOCTLCMD)
-		return ret;
-
-	return 0;
-}
-
 static int vimc_cap_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
 	struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
@@ -173,7 +254,7 @@ static int vimc_cap_start_streaming(struct vb2_queue *vq, unsigned int count)
 	}
 
 	/* Enable streaming from the pipe */
-	ret = vimc_cap_pipeline_s_stream(vcap, 1);
+	ret = vimc_pipeline_s_stream(&vcap->vdev.entity, 1);
 	if (ret) {
 		media_pipeline_stop(entity);
 		vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED);
@@ -192,7 +273,7 @@ static void vimc_cap_stop_streaming(struct vb2_queue *vq)
 	struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
 
 	/* Disable streaming from the pipe */
-	vimc_cap_pipeline_s_stream(vcap, 0);
+	vimc_pipeline_s_stream(&vcap->vdev.entity, 0);
 
 	/* Stop the media pipeline */
 	media_pipeline_stop(&vcap->vdev.entity);
@@ -234,8 +315,7 @@ static int vimc_cap_buffer_prepare(struct vb2_buffer *vb)
 	unsigned long size = vcap->format.sizeimage;
 
 	if (vb2_plane_size(vb, 0) < size) {
-		dev_err(vcap->vdev.v4l2_dev->dev,
-			"%s: buffer too small (%lu < %lu)\n",
+		dev_err(vcap->dev, "%s: buffer too small (%lu < %lu)\n",
 			vcap->vdev.name, vb2_plane_size(vb, 0), size);
 		return -EINVAL;
 	}
@@ -256,78 +336,14 @@ static const struct vb2_ops vimc_cap_qops = {
 	.wait_finish		= vb2_ops_wait_finish,
 };
 
-/*
- * NOTE: this function is a copy of v4l2_subdev_link_validate_get_format
- * maybe the v4l2 function should be public
- */
-static int vimc_cap_v4l2_subdev_link_validate_get_format(struct media_pad *pad,
-						struct v4l2_subdev_format *fmt)
-{
-	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(pad->entity);
-
-	fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE;
-	fmt->pad = pad->index;
-
-	return v4l2_subdev_call(sd, pad, get_fmt, NULL, fmt);
-}
-
-static int vimc_cap_link_validate(struct media_link *link)
-{
-	struct v4l2_subdev_format source_fmt;
-	const struct vimc_pix_map *vpix;
-	struct vimc_cap_device *vcap = container_of(link->sink->entity,
-						    struct vimc_cap_device,
-						    vdev.entity);
-	struct v4l2_pix_format *sink_fmt = &vcap->format;
-	int ret;
-
-	/*
-	 * if it is a raw node from vimc-core, ignore the link for now
-	 * TODO: remove this when there are no more raw nodes in the
-	 * core and return error instead
-	 */
-	if (link->source->entity->obj_type == MEDIA_ENTITY_TYPE_BASE)
-		return 0;
-
-	/* Get the the format of the subdev */
-	ret = vimc_cap_v4l2_subdev_link_validate_get_format(link->source,
-							    &source_fmt);
-	if (ret)
-		return ret;
-
-	dev_dbg(vcap->vdev.v4l2_dev->dev,
-		"%s: link validate formats src:%dx%d %d sink:%dx%d %d\n",
-		vcap->vdev.name,
-		source_fmt.format.width, source_fmt.format.height,
-		source_fmt.format.code,
-		sink_fmt->width, sink_fmt->height,
-		sink_fmt->pixelformat);
-
-	/* The width, height and code must match. */
-	vpix = vimc_pix_map_by_pixelformat(sink_fmt->pixelformat);
-	if (source_fmt.format.width != sink_fmt->width
-	    || source_fmt.format.height != sink_fmt->height
-	    || vpix->code != source_fmt.format.code)
-		return -EPIPE;
-
-	/*
-	 * The field order must match, or the sink field order must be NONE
-	 * to support interlaced hardware connected to bridges that support
-	 * progressive formats only.
-	 */
-	if (source_fmt.format.field != sink_fmt->field &&
-	    sink_fmt->field != V4L2_FIELD_NONE)
-		return -EPIPE;
-
-	return 0;
-}
-
 static const struct media_entity_operations vimc_cap_mops = {
-	.link_validate		= vimc_cap_link_validate,
+	.link_validate		= vimc_link_validate,
 };
 
-static void vimc_cap_destroy(struct vimc_ent_device *ved)
+static void vimc_cap_comp_unbind(struct device *comp, struct device *master,
+				 void *master_data)
 {
+	struct vimc_ent_device *ved = dev_get_drvdata(comp);
 	struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
 						    ved);
 
@@ -376,42 +392,35 @@ static void vimc_cap_process_frame(struct vimc_ent_device *ved,
 	vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE);
 }
 
-struct vimc_ent_device *vimc_cap_create(struct v4l2_device *v4l2_dev,
-					const char *const name,
-					u16 num_pads,
-					const unsigned long *pads_flag)
+static int vimc_cap_comp_bind(struct device *comp, struct device *master,
+			      void *master_data)
 {
+	struct v4l2_device *v4l2_dev = master_data;
+	struct vimc_platform_data *pdata = comp->platform_data;
 	const struct vimc_pix_map *vpix;
 	struct vimc_cap_device *vcap;
 	struct video_device *vdev;
 	struct vb2_queue *q;
 	int ret;
 
-	/*
-	 * Check entity configuration params
-	 * NOTE: we only support a single sink pad
-	 */
-	if (!name || num_pads != 1 || !pads_flag ||
-	    !(pads_flag[0] & MEDIA_PAD_FL_SINK))
-		return ERR_PTR(-EINVAL);
-
 	/* Allocate the vimc_cap_device struct */
 	vcap = kzalloc(sizeof(*vcap), GFP_KERNEL);
 	if (!vcap)
-		return ERR_PTR(-ENOMEM);
+		return -ENOMEM;
 
 	/* Allocate the pads */
-	vcap->ved.pads = vimc_pads_init(num_pads, pads_flag);
+	vcap->ved.pads =
+		vimc_pads_init(1, (const unsigned long[1]) {MEDIA_PAD_FL_SINK});
 	if (IS_ERR(vcap->ved.pads)) {
 		ret = PTR_ERR(vcap->ved.pads);
 		goto err_free_vcap;
 	}
 
 	/* Initialize the media entity */
-	vcap->vdev.entity.name = name;
+	vcap->vdev.entity.name = pdata->entity_name;
 	vcap->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
 	ret = media_entity_pads_init(&vcap->vdev.entity,
-				     num_pads, vcap->ved.pads);
+				     1, vcap->ved.pads);
 	if (ret)
 		goto err_clean_pads;
 
@@ -432,9 +441,8 @@ struct vimc_ent_device *vimc_cap_create(struct v4l2_device *v4l2_dev,
 
 	ret = vb2_queue_init(q);
 	if (ret) {
-		dev_err(vcap->vdev.v4l2_dev->dev,
-			"%s: vb2 queue init failed (err=%d)\n",
-			vcap->vdev.name, ret);
+		dev_err(comp, "%s: vb2 queue init failed (err=%d)\n",
+			pdata->entity_name, ret);
 		goto err_clean_m_ent;
 	}
 
@@ -442,23 +450,19 @@ struct vimc_ent_device *vimc_cap_create(struct v4l2_device *v4l2_dev,
 	INIT_LIST_HEAD(&vcap->buf_list);
 	spin_lock_init(&vcap->qlock);
 
-	/* Set the frame format (this is hardcoded for now) */
-	vcap->format.width = 640;
-	vcap->format.height = 480;
-	vcap->format.pixelformat = V4L2_PIX_FMT_RGB24;
-	vcap->format.field = V4L2_FIELD_NONE;
-	vcap->format.colorspace = V4L2_COLORSPACE_SRGB;
-
+	/* Set default frame format */
+	vcap->format = fmt_default;
 	vpix = vimc_pix_map_by_pixelformat(vcap->format.pixelformat);
-
 	vcap->format.bytesperline = vcap->format.width * vpix->bpp;
 	vcap->format.sizeimage = vcap->format.bytesperline *
 				 vcap->format.height;
 
 	/* Fill the vimc_ent_device struct */
-	vcap->ved.destroy = vimc_cap_destroy;
 	vcap->ved.ent = &vcap->vdev.entity;
 	vcap->ved.process_frame = vimc_cap_process_frame;
+	vcap->ved.vdev_get_format = vimc_cap_get_format;
+	dev_set_drvdata(comp, &vcap->ved);
+	vcap->dev = comp;
 
 	/* Initialize the video_device struct */
 	vdev = &vcap->vdev;
@@ -471,19 +475,18 @@ struct vimc_ent_device *vimc_cap_create(struct v4l2_device *v4l2_dev,
 	vdev->queue = q;
 	vdev->v4l2_dev = v4l2_dev;
 	vdev->vfl_dir = VFL_DIR_RX;
-	strlcpy(vdev->name, name, sizeof(vdev->name));
+	strlcpy(vdev->name, pdata->entity_name, sizeof(vdev->name));
 	video_set_drvdata(vdev, &vcap->ved);
 
 	/* Register the video_device with the v4l2 and the media framework */
 	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
 	if (ret) {
-		dev_err(vcap->vdev.v4l2_dev->dev,
-			"%s: video register failed (err=%d)\n",
+		dev_err(comp, "%s: video register failed (err=%d)\n",
 			vcap->vdev.name, ret);
 		goto err_release_queue;
 	}
 
-	return &vcap->ved;
+	return 0;
 
 err_release_queue:
 	vb2_queue_release(q);
@@ -494,5 +497,45 @@ err_clean_pads:
 err_free_vcap:
 	kfree(vcap);
 
-	return ERR_PTR(ret);
+	return ret;
+}
+
+static const struct component_ops vimc_cap_comp_ops = {
+	.bind = vimc_cap_comp_bind,
+	.unbind = vimc_cap_comp_unbind,
+};
+
+static int vimc_cap_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &vimc_cap_comp_ops);
 }
+
+static int vimc_cap_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &vimc_cap_comp_ops);
+
+	return 0;
+}
+
+static struct platform_driver vimc_cap_pdrv = {
+	.probe		= vimc_cap_probe,
+	.remove		= vimc_cap_remove,
+	.driver		= {
+		.name	= VIMC_CAP_DRV_NAME,
+	},
+};
+
+static const struct platform_device_id vimc_cap_driver_ids[] = {
+	{
+		.name           = VIMC_CAP_DRV_NAME,
+	},
+	{ }
+};
+
+module_platform_driver(vimc_cap_pdrv);
+
+MODULE_DEVICE_TABLE(platform, vimc_cap_driver_ids);
+
+MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Capture");
+MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/vimc/vimc-capture.h b/drivers/media/platform/vimc/vimc-capture.h
deleted file mode 100644
index 581a813abdf1..000000000000
--- a/drivers/media/platform/vimc/vimc-capture.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
- * vimc-capture.h Virtual Media Controller Driver
- *
- * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- */
-
-#ifndef _VIMC_CAPTURE_H_
-#define _VIMC_CAPTURE_H_
-
-#include "vimc-core.h"
-
-struct vimc_ent_device *vimc_cap_create(struct v4l2_device *v4l2_dev,
-					const char *const name,
-					u16 num_pads,
-					const unsigned long *pads_flag);
-
-#endif
diff --git a/drivers/media/platform/vimc/vimc-common.c b/drivers/media/platform/vimc/vimc-common.c
new file mode 100644
index 000000000000..9d63c84a9876
--- /dev/null
+++ b/drivers/media/platform/vimc/vimc-common.c
@@ -0,0 +1,473 @@
+/*
+ * vimc-common.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+
+#include "vimc-common.h"
+
+/*
+ * NOTE: non-bayer formats need to come first (necessary for enum_mbus_code
+ * in the scaler)
+ */
+static const struct vimc_pix_map vimc_pix_map_list[] = {
+	/* TODO: add all missing formats */
+
+	/* RGB formats */
+	{
+		.code = MEDIA_BUS_FMT_BGR888_1X24,
+		.pixelformat = V4L2_PIX_FMT_BGR24,
+		.bpp = 3,
+		.bayer = false,
+	},
+	{
+		.code = MEDIA_BUS_FMT_RGB888_1X24,
+		.pixelformat = V4L2_PIX_FMT_RGB24,
+		.bpp = 3,
+		.bayer = false,
+	},
+	{
+		.code = MEDIA_BUS_FMT_ARGB8888_1X32,
+		.pixelformat = V4L2_PIX_FMT_ARGB32,
+		.bpp = 4,
+		.bayer = false,
+	},
+
+	/* Bayer formats */
+	{
+		.code = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.pixelformat = V4L2_PIX_FMT_SBGGR8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGBRG8_1X8,
+		.pixelformat = V4L2_PIX_FMT_SGBRG8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGRBG8_1X8,
+		.pixelformat = V4L2_PIX_FMT_SGRBG8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SRGGB8_1X8,
+		.pixelformat = V4L2_PIX_FMT_SRGGB8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SBGGR10_1X10,
+		.pixelformat = V4L2_PIX_FMT_SBGGR10,
+		.bpp = 2,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGBRG10_1X10,
+		.pixelformat = V4L2_PIX_FMT_SGBRG10,
+		.bpp = 2,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.pixelformat = V4L2_PIX_FMT_SGRBG10,
+		.bpp = 2,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.pixelformat = V4L2_PIX_FMT_SRGGB10,
+		.bpp = 2,
+		.bayer = true,
+	},
+
+	/* 10bit raw bayer a-law compressed to 8 bits */
+	{
+		.code = MEDIA_BUS_FMT_SBGGR10_ALAW8_1X8,
+		.pixelformat = V4L2_PIX_FMT_SBGGR10ALAW8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGBRG10_ALAW8_1X8,
+		.pixelformat = V4L2_PIX_FMT_SGBRG10ALAW8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGRBG10_ALAW8_1X8,
+		.pixelformat = V4L2_PIX_FMT_SGRBG10ALAW8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SRGGB10_ALAW8_1X8,
+		.pixelformat = V4L2_PIX_FMT_SRGGB10ALAW8,
+		.bpp = 1,
+		.bayer = true,
+	},
+
+	/* 10bit raw bayer DPCM compressed to 8 bits */
+	{
+		.code = MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8,
+		.pixelformat = V4L2_PIX_FMT_SBGGR10DPCM8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8,
+		.pixelformat = V4L2_PIX_FMT_SGBRG10DPCM8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+		.pixelformat = V4L2_PIX_FMT_SGRBG10DPCM8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8,
+		.pixelformat = V4L2_PIX_FMT_SRGGB10DPCM8,
+		.bpp = 1,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SBGGR12_1X12,
+		.pixelformat = V4L2_PIX_FMT_SBGGR12,
+		.bpp = 2,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGBRG12_1X12,
+		.pixelformat = V4L2_PIX_FMT_SGBRG12,
+		.bpp = 2,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGRBG12_1X12,
+		.pixelformat = V4L2_PIX_FMT_SGRBG12,
+		.bpp = 2,
+		.bayer = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SRGGB12_1X12,
+		.pixelformat = V4L2_PIX_FMT_SRGGB12,
+		.bpp = 2,
+		.bayer = true,
+	},
+};
+
+const struct vimc_pix_map *vimc_pix_map_by_index(unsigned int i)
+{
+	if (i >= ARRAY_SIZE(vimc_pix_map_list))
+		return NULL;
+
+	return &vimc_pix_map_list[i];
+}
+EXPORT_SYMBOL_GPL(vimc_pix_map_by_index);
+
+const struct vimc_pix_map *vimc_pix_map_by_code(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
+		if (vimc_pix_map_list[i].code == code)
+			return &vimc_pix_map_list[i];
+	}
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(vimc_pix_map_by_code);
+
+const struct vimc_pix_map *vimc_pix_map_by_pixelformat(u32 pixelformat)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
+		if (vimc_pix_map_list[i].pixelformat == pixelformat)
+			return &vimc_pix_map_list[i];
+	}
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(vimc_pix_map_by_pixelformat);
+
+int vimc_propagate_frame(struct media_pad *src, const void *frame)
+{
+	struct media_link *link;
+
+	if (!(src->flags & MEDIA_PAD_FL_SOURCE))
+		return -EINVAL;
+
+	/* Send this frame to all sink pads that are direct linked */
+	list_for_each_entry(link, &src->entity->links, list) {
+		if (link->source == src &&
+		    (link->flags & MEDIA_LNK_FL_ENABLED)) {
+			struct vimc_ent_device *ved = NULL;
+			struct media_entity *entity = link->sink->entity;
+
+			if (is_media_entity_v4l2_subdev(entity)) {
+				struct v4l2_subdev *sd =
+					container_of(entity, struct v4l2_subdev,
+						     entity);
+				ved = v4l2_get_subdevdata(sd);
+			} else if (is_media_entity_v4l2_video_device(entity)) {
+				struct video_device *vdev =
+					container_of(entity,
+						     struct video_device,
+						     entity);
+				ved = video_get_drvdata(vdev);
+			}
+			if (ved && ved->process_frame)
+				ved->process_frame(ved, link->sink, frame);
+		}
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vimc_propagate_frame);
+
+/* Helper function to allocate and initialize pads */
+struct media_pad *vimc_pads_init(u16 num_pads, const unsigned long *pads_flag)
+{
+	struct media_pad *pads;
+	unsigned int i;
+
+	/* Allocate memory for the pads */
+	pads = kcalloc(num_pads, sizeof(*pads), GFP_KERNEL);
+	if (!pads)
+		return ERR_PTR(-ENOMEM);
+
+	/* Initialize the pads */
+	for (i = 0; i < num_pads; i++) {
+		pads[i].index = i;
+		pads[i].flags = pads_flag[i];
+	}
+
+	return pads;
+}
+EXPORT_SYMBOL_GPL(vimc_pads_init);
+
+int vimc_pipeline_s_stream(struct media_entity *ent, int enable)
+{
+	struct v4l2_subdev *sd;
+	struct media_pad *pad;
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < ent->num_pads; i++) {
+		if (ent->pads[i].flags & MEDIA_PAD_FL_SOURCE)
+			continue;
+
+		/* Start the stream in the subdevice direct connected */
+		pad = media_entity_remote_pad(&ent->pads[i]);
+
+		if (!is_media_entity_v4l2_subdev(pad->entity))
+			return -EINVAL;
+
+		sd = media_entity_to_v4l2_subdev(pad->entity);
+		ret = v4l2_subdev_call(sd, video, s_stream, enable);
+		if (ret && ret != -ENOIOCTLCMD)
+			return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vimc_pipeline_s_stream);
+
+static int vimc_get_mbus_format(struct media_pad *pad,
+				struct v4l2_subdev_format *fmt)
+{
+	if (is_media_entity_v4l2_subdev(pad->entity)) {
+		struct v4l2_subdev *sd =
+			media_entity_to_v4l2_subdev(pad->entity);
+		int ret;
+
+		fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		fmt->pad = pad->index;
+
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, fmt);
+		if (ret)
+			return ret;
+
+	} else if (is_media_entity_v4l2_video_device(pad->entity)) {
+		struct video_device *vdev = container_of(pad->entity,
+							 struct video_device,
+							 entity);
+		struct vimc_ent_device *ved = video_get_drvdata(vdev);
+		const struct vimc_pix_map *vpix;
+		struct v4l2_pix_format vdev_fmt;
+
+		if (!ved->vdev_get_format)
+			return -ENOIOCTLCMD;
+
+		ved->vdev_get_format(ved, &vdev_fmt);
+		vpix = vimc_pix_map_by_pixelformat(vdev_fmt.pixelformat);
+		v4l2_fill_mbus_format(&fmt->format, &vdev_fmt, vpix->code);
+	} else {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int vimc_link_validate(struct media_link *link)
+{
+	struct v4l2_subdev_format source_fmt, sink_fmt;
+	int ret;
+
+	ret = vimc_get_mbus_format(link->source, &source_fmt);
+	if (ret)
+		return ret;
+
+	ret = vimc_get_mbus_format(link->sink, &sink_fmt);
+	if (ret)
+		return ret;
+
+	pr_info("vimc link validate: "
+		"%s:src:%dx%d (0x%x, %d, %d, %d, %d) "
+		"%s:snk:%dx%d (0x%x, %d, %d, %d, %d)\n",
+		/* src */
+		link->source->entity->name,
+		source_fmt.format.width, source_fmt.format.height,
+		source_fmt.format.code, source_fmt.format.colorspace,
+		source_fmt.format.quantization, source_fmt.format.xfer_func,
+		source_fmt.format.ycbcr_enc,
+		/* sink */
+		link->sink->entity->name,
+		sink_fmt.format.width, sink_fmt.format.height,
+		sink_fmt.format.code, sink_fmt.format.colorspace,
+		sink_fmt.format.quantization, sink_fmt.format.xfer_func,
+		sink_fmt.format.ycbcr_enc);
+
+	/* The width, height and code must match. */
+	if (source_fmt.format.width != sink_fmt.format.width
+	    || source_fmt.format.height != sink_fmt.format.height
+	    || source_fmt.format.code != sink_fmt.format.code)
+		return -EPIPE;
+
+	/*
+	 * The field order must match, or the sink field order must be NONE
+	 * to support interlaced hardware connected to bridges that support
+	 * progressive formats only.
+	 */
+	if (source_fmt.format.field != sink_fmt.format.field &&
+	    sink_fmt.format.field != V4L2_FIELD_NONE)
+		return -EPIPE;
+
+	/*
+	 * If colorspace is DEFAULT, then assume all the colorimetry is also
+	 * DEFAULT, return 0 to skip comparing the other colorimetry parameters
+	 */
+	if (source_fmt.format.colorspace == V4L2_COLORSPACE_DEFAULT
+	    || sink_fmt.format.colorspace == V4L2_COLORSPACE_DEFAULT)
+		return 0;
+
+	/* Colorspace must match. */
+	if (source_fmt.format.colorspace != sink_fmt.format.colorspace)
+		return -EPIPE;
+
+	/* Colorimetry must match if they are not set to DEFAULT */
+	if (source_fmt.format.ycbcr_enc != V4L2_YCBCR_ENC_DEFAULT
+	    && sink_fmt.format.ycbcr_enc != V4L2_YCBCR_ENC_DEFAULT
+	    && source_fmt.format.ycbcr_enc != sink_fmt.format.ycbcr_enc)
+		return -EPIPE;
+
+	if (source_fmt.format.quantization != V4L2_QUANTIZATION_DEFAULT
+	    && sink_fmt.format.quantization != V4L2_QUANTIZATION_DEFAULT
+	    && source_fmt.format.quantization != sink_fmt.format.quantization)
+		return -EPIPE;
+
+	if (source_fmt.format.xfer_func != V4L2_XFER_FUNC_DEFAULT
+	    && sink_fmt.format.xfer_func != V4L2_XFER_FUNC_DEFAULT
+	    && source_fmt.format.xfer_func != sink_fmt.format.xfer_func)
+		return -EPIPE;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vimc_link_validate);
+
+static const struct media_entity_operations vimc_ent_sd_mops = {
+	.link_validate = vimc_link_validate,
+};
+
+int vimc_ent_sd_register(struct vimc_ent_device *ved,
+			 struct v4l2_subdev *sd,
+			 struct v4l2_device *v4l2_dev,
+			 const char *const name,
+			 u32 function,
+			 u16 num_pads,
+			 const unsigned long *pads_flag,
+			 const struct v4l2_subdev_ops *sd_ops)
+{
+	int ret;
+
+	/* Allocate the pads */
+	ved->pads = vimc_pads_init(num_pads, pads_flag);
+	if (IS_ERR(ved->pads))
+		return PTR_ERR(ved->pads);
+
+	/* Fill the vimc_ent_device struct */
+	ved->ent = &sd->entity;
+
+	/* Initialize the subdev */
+	v4l2_subdev_init(sd, sd_ops);
+	sd->entity.function = function;
+	sd->entity.ops = &vimc_ent_sd_mops;
+	sd->owner = THIS_MODULE;
+	strlcpy(sd->name, name, sizeof(sd->name));
+	v4l2_set_subdevdata(sd, ved);
+
+	/* Expose this subdev to user space */
+	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	/* Initialize the media entity */
+	ret = media_entity_pads_init(&sd->entity, num_pads, ved->pads);
+	if (ret)
+		goto err_clean_pads;
+
+	/* Register the subdev with the v4l2 and the media framework */
+	ret = v4l2_device_register_subdev(v4l2_dev, sd);
+	if (ret) {
+		dev_err(v4l2_dev->dev,
+			"%s: subdev register failed (err=%d)\n",
+			name, ret);
+		goto err_clean_m_ent;
+	}
+
+	return 0;
+
+err_clean_m_ent:
+	media_entity_cleanup(&sd->entity);
+err_clean_pads:
+	vimc_pads_cleanup(ved->pads);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vimc_ent_sd_register);
+
+void vimc_ent_sd_unregister(struct vimc_ent_device *ved, struct v4l2_subdev *sd)
+{
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(ved->ent);
+	vimc_pads_cleanup(ved->pads);
+}
+EXPORT_SYMBOL_GPL(vimc_ent_sd_unregister);
+
+MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Common");
+MODULE_AUTHOR("Helen Koike <helen.fornazier@gmail.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/vimc/vimc-common.h b/drivers/media/platform/vimc/vimc-common.h
new file mode 100644
index 000000000000..dca528a316e7
--- /dev/null
+++ b/drivers/media/platform/vimc/vimc-common.h
@@ -0,0 +1,229 @@
+/*
+ * vimc-common.h Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _VIMC_COMMON_H_
+#define _VIMC_COMMON_H_
+
+#include <linux/slab.h>
+#include <media/media-device.h>
+#include <media/v4l2-device.h>
+
+#define VIMC_FRAME_MAX_WIDTH 4096
+#define VIMC_FRAME_MAX_HEIGHT 2160
+#define VIMC_FRAME_MIN_WIDTH 16
+#define VIMC_FRAME_MIN_HEIGHT 16
+
+#define VIMC_FRAME_INDEX(lin, col, width, bpp) ((lin * width + col) * bpp)
+
+/**
+ * struct vimc_colorimetry_clamp - Adjust colorimetry parameters
+ *
+ * @fmt:		the pointer to struct v4l2_pix_format or
+ *			struct v4l2_mbus_framefmt
+ *
+ * Entities must check if colorimetry given by the userspace is valid, if not
+ * then set them as DEFAULT
+ */
+#define vimc_colorimetry_clamp(fmt)					\
+do {									\
+	if ((fmt)->colorspace == V4L2_COLORSPACE_DEFAULT		\
+	    || (fmt)->colorspace > V4L2_COLORSPACE_DCI_P3) {		\
+		(fmt)->colorspace = V4L2_COLORSPACE_DEFAULT;		\
+		(fmt)->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;		\
+		(fmt)->quantization = V4L2_QUANTIZATION_DEFAULT;	\
+		(fmt)->xfer_func = V4L2_XFER_FUNC_DEFAULT;		\
+	}								\
+	if ((fmt)->ycbcr_enc > V4L2_YCBCR_ENC_SMPTE240M)		\
+		(fmt)->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;		\
+	if ((fmt)->quantization > V4L2_QUANTIZATION_LIM_RANGE)		\
+		(fmt)->quantization = V4L2_QUANTIZATION_DEFAULT;	\
+	if ((fmt)->xfer_func > V4L2_XFER_FUNC_SMPTE2084)		\
+		(fmt)->xfer_func = V4L2_XFER_FUNC_DEFAULT;		\
+} while (0)
+
+/**
+ * struct vimc_platform_data - platform data to components
+ *
+ * @entity_name:	The name of the entity to be created
+ *
+ * Board setup code will often provide additional information using the device's
+ * platform_data field to hold additional information.
+ * When injecting a new platform_device in the component system the core needs
+ * to provide to the corresponding submodules the name of the entity that should
+ * be used when registering the subdevice in the Media Controller system.
+ */
+struct vimc_platform_data {
+	char entity_name[32];
+};
+
+/**
+ * struct vimc_pix_map - maps media bus code with v4l2 pixel format
+ *
+ * @code:		media bus format code defined by MEDIA_BUS_FMT_* macros
+ * @bbp:		number of bytes each pixel occupies
+ * @pixelformat:	pixel format devined by V4L2_PIX_FMT_* macros
+ *
+ * Struct which matches the MEDIA_BUS_FMT_* codes with the corresponding
+ * V4L2_PIX_FMT_* fourcc pixelformat and its bytes per pixel (bpp)
+ */
+struct vimc_pix_map {
+	unsigned int code;
+	unsigned int bpp;
+	u32 pixelformat;
+	bool bayer;
+};
+
+/**
+ * struct vimc_ent_device - core struct that represents a node in the topology
+ *
+ * @ent:		the pointer to struct media_entity for the node
+ * @pads:		the list of pads of the node
+ * @process_frame:	callback send a frame to that node
+ * @vdev_get_format:	callback that returns the current format a pad, used
+ *			only when is_media_entity_v4l2_video_device(ent) returns
+ *			true
+ *
+ * Each node of the topology must create a vimc_ent_device struct. Depending on
+ * the node it will be of an instance of v4l2_subdev or video_device struct
+ * where both contains a struct media_entity.
+ * Those structures should embedded the vimc_ent_device struct through
+ * v4l2_set_subdevdata() and video_set_drvdata() respectivaly, allowing the
+ * vimc_ent_device struct to be retrieved from the corresponding struct
+ * media_entity
+ */
+struct vimc_ent_device {
+	struct media_entity *ent;
+	struct media_pad *pads;
+	void (*process_frame)(struct vimc_ent_device *ved,
+			      struct media_pad *sink, const void *frame);
+	void (*vdev_get_format)(struct vimc_ent_device *ved,
+			      struct v4l2_pix_format *fmt);
+};
+
+/**
+ * vimc_propagate_frame - propagate a frame through the topology
+ *
+ * @src:	the source pad where the frame is being originated
+ * @frame:	the frame to be propagated
+ *
+ * This function will call the process_frame callback from the vimc_ent_device
+ * struct of the nodes directly connected to the @src pad
+ */
+int vimc_propagate_frame(struct media_pad *src, const void *frame);
+
+/**
+ * vimc_pads_init - initialize pads
+ *
+ * @num_pads:	number of pads to initialize
+ * @pads_flags:	flags to use in each pad
+ *
+ * Helper functions to allocate/initialize pads
+ */
+struct media_pad *vimc_pads_init(u16 num_pads,
+				 const unsigned long *pads_flag);
+
+/**
+ * vimc_pads_cleanup - free pads
+ *
+ * @pads: pointer to the pads
+ *
+ * Helper function to free the pads initialized with vimc_pads_init
+ */
+static inline void vimc_pads_cleanup(struct media_pad *pads)
+{
+	kfree(pads);
+}
+
+/**
+ * vimc_pipeline_s_stream - start stream through the pipeline
+ *
+ * @ent:		the pointer to struct media_entity for the node
+ * @enable:		1 to start the stream and 0 to stop
+ *
+ * Helper function to call the s_stream of the subdevices connected
+ * in all the sink pads of the entity
+ */
+int vimc_pipeline_s_stream(struct media_entity *ent, int enable);
+
+/**
+ * vimc_pix_map_by_index - get vimc_pix_map struct by its index
+ *
+ * @i:			index of the vimc_pix_map struct in vimc_pix_map_list
+ */
+const struct vimc_pix_map *vimc_pix_map_by_index(unsigned int i);
+
+/**
+ * vimc_pix_map_by_code - get vimc_pix_map struct by media bus code
+ *
+ * @code:		media bus format code defined by MEDIA_BUS_FMT_* macros
+ */
+const struct vimc_pix_map *vimc_pix_map_by_code(u32 code);
+
+/**
+ * vimc_pix_map_by_pixelformat - get vimc_pix_map struct by v4l2 pixel format
+ *
+ * @pixelformat:	pixel format devined by V4L2_PIX_FMT_* macros
+ */
+const struct vimc_pix_map *vimc_pix_map_by_pixelformat(u32 pixelformat);
+
+/**
+ * vimc_ent_sd_register - initialize and register a subdev node
+ *
+ * @ved:	the vimc_ent_device struct to be initialize
+ * @sd:		the v4l2_subdev struct to be initialize and registered
+ * @v4l2_dev:	the v4l2 device to register the v4l2_subdev
+ * @name:	name of the sub-device. Please notice that the name must be
+ *		unique.
+ * @function:	media entity function defined by MEDIA_ENT_F_* macros
+ * @num_pads:	number of pads to initialize
+ * @pads_flag:	flags to use in each pad
+ * @sd_ops:	pointer to &struct v4l2_subdev_ops.
+ *
+ * Helper function initialize and register the struct vimc_ent_device and struct
+ * v4l2_subdev which represents a subdev node in the topology
+ */
+int vimc_ent_sd_register(struct vimc_ent_device *ved,
+			 struct v4l2_subdev *sd,
+			 struct v4l2_device *v4l2_dev,
+			 const char *const name,
+			 u32 function,
+			 u16 num_pads,
+			 const unsigned long *pads_flag,
+			 const struct v4l2_subdev_ops *sd_ops);
+
+/**
+ * vimc_ent_sd_unregister - cleanup and unregister a subdev node
+ *
+ * @ved:	the vimc_ent_device struct to be cleaned up
+ * @sd:		the v4l2_subdev struct to be unregistered
+ *
+ * Helper function cleanup and unregister the struct vimc_ent_device and struct
+ * v4l2_subdev which represents a subdev node in the topology
+ */
+void vimc_ent_sd_unregister(struct vimc_ent_device *ved,
+			    struct v4l2_subdev *sd);
+
+/**
+ * vimc_link_validate - validates a media link
+ *
+ * @link: pointer to &struct media_link
+ *
+ * This function calls validates if a media link is valid for streaming.
+ */
+int vimc_link_validate(struct media_link *link);
+
+#endif
diff --git a/drivers/media/platform/vimc/vimc-core.c b/drivers/media/platform/vimc/vimc-core.c
index bc107da8fbd5..51c0eee61ca6 100644
--- a/drivers/media/platform/vimc/vimc-core.c
+++ b/drivers/media/platform/vimc/vimc-core.c
@@ -15,15 +15,14 @@
  *
  */
 
+#include <linux/component.h>
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <media/media-device.h>
 #include <media/v4l2-device.h>
 
-#include "vimc-capture.h"
-#include "vimc-core.h"
-#include "vimc-sensor.h"
+#include "vimc-common.h"
 
 #define VIMC_PDEV_NAME "vimc"
 #define VIMC_MDEV_MODEL_NAME "VIMC MDEV"
@@ -37,10 +36,10 @@
 }
 
 struct vimc_device {
-	/*
-	 * The pipeline configuration
-	 * (filled before calling vimc_device_register)
-	 */
+	/* The platform device */
+	struct platform_device pdev;
+
+	/* The pipeline configuration */
 	const struct vimc_pipeline_config *pipe_cfg;
 
 	/* The Associated media_device parent */
@@ -49,43 +48,14 @@ struct vimc_device {
 	/* Internal v4l2 parent device*/
 	struct v4l2_device v4l2_dev;
 
-	/* Internal topology */
-	struct vimc_ent_device **ved;
-};
-
-/**
- * enum vimc_ent_node - Select the functionality of a node in the topology
- * @VIMC_ENT_NODE_SENSOR:	A node of type SENSOR simulates a camera sensor
- *				generating internal images in bayer format and
- *				propagating those images through the pipeline
- * @VIMC_ENT_NODE_CAPTURE:	A node of type CAPTURE is a v4l2 video_device
- *				that exposes the received image from the
- *				pipeline to the user space
- * @VIMC_ENT_NODE_INPUT:	A node of type INPUT is a v4l2 video_device that
- *				receives images from the user space and
- *				propagates them through the pipeline
- * @VIMC_ENT_NODE_DEBAYER:	A node type DEBAYER expects to receive a frame
- *				in bayer format converts it to RGB
- * @VIMC_ENT_NODE_SCALER:	A node of type SCALER scales the received image
- *				by a given multiplier
- *
- * This enum is used in the entity configuration struct to allow the definition
- * of a custom topology specifying the role of each node on it.
- */
-enum vimc_ent_node {
-	VIMC_ENT_NODE_SENSOR,
-	VIMC_ENT_NODE_CAPTURE,
-	VIMC_ENT_NODE_INPUT,
-	VIMC_ENT_NODE_DEBAYER,
-	VIMC_ENT_NODE_SCALER,
+	/* Subdevices */
+	struct platform_device **subdevs;
 };
 
 /* Structure which describes individual configuration for each entity */
 struct vimc_ent_config {
 	const char *name;
-	size_t pads_qty;
-	const unsigned long *pads_flag;
-	enum vimc_ent_node node;
+	const char *drv;
 };
 
 /* Structure which describes links between entities */
@@ -112,60 +82,40 @@ struct vimc_pipeline_config {
 static const struct vimc_ent_config ent_config[] = {
 	{
 		.name = "Sensor A",
-		.pads_qty = 1,
-		.pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SOURCE},
-		.node = VIMC_ENT_NODE_SENSOR,
+		.drv = "vimc-sensor",
 	},
 	{
 		.name = "Sensor B",
-		.pads_qty = 1,
-		.pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SOURCE},
-		.node = VIMC_ENT_NODE_SENSOR,
+		.drv = "vimc-sensor",
 	},
 	{
 		.name = "Debayer A",
-		.pads_qty = 2,
-		.pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SINK,
-						     MEDIA_PAD_FL_SOURCE},
-		.node = VIMC_ENT_NODE_DEBAYER,
+		.drv = "vimc-debayer",
 	},
 	{
 		.name = "Debayer B",
-		.pads_qty = 2,
-		.pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SINK,
-						     MEDIA_PAD_FL_SOURCE},
-		.node = VIMC_ENT_NODE_DEBAYER,
+		.drv = "vimc-debayer",
 	},
 	{
 		.name = "Raw Capture 0",
-		.pads_qty = 1,
-		.pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SINK},
-		.node = VIMC_ENT_NODE_CAPTURE,
+		.drv = "vimc-capture",
 	},
 	{
 		.name = "Raw Capture 1",
-		.pads_qty = 1,
-		.pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SINK},
-		.node = VIMC_ENT_NODE_CAPTURE,
+		.drv = "vimc-capture",
 	},
 	{
 		.name = "RGB/YUV Input",
-		.pads_qty = 1,
-		.pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SOURCE},
-		.node = VIMC_ENT_NODE_INPUT,
+		/* TODO: change this to vimc-input when it is implemented */
+		.drv = "vimc-sensor",
 	},
 	{
 		.name = "Scaler",
-		.pads_qty = 2,
-		.pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SINK,
-						     MEDIA_PAD_FL_SOURCE},
-		.node = VIMC_ENT_NODE_SCALER,
+		.drv = "vimc-scaler",
 	},
 	{
 		.name = "RGB/YUV Capture",
-		.pads_qty = 1,
-		.pads_flag = (const unsigned long[]){MEDIA_PAD_FL_SINK},
-		.node = VIMC_ENT_NODE_CAPTURE,
+		.drv = "vimc-capture",
 	},
 };
 
@@ -197,314 +147,40 @@ static const struct vimc_pipeline_config pipe_cfg = {
 
 /* -------------------------------------------------------------------------- */
 
-static const struct vimc_pix_map vimc_pix_map_list[] = {
-	/* TODO: add all missing formats */
-
-	/* RGB formats */
-	{
-		.code = MEDIA_BUS_FMT_BGR888_1X24,
-		.pixelformat = V4L2_PIX_FMT_BGR24,
-		.bpp = 3,
-	},
-	{
-		.code = MEDIA_BUS_FMT_RGB888_1X24,
-		.pixelformat = V4L2_PIX_FMT_RGB24,
-		.bpp = 3,
-	},
-	{
-		.code = MEDIA_BUS_FMT_ARGB8888_1X32,
-		.pixelformat = V4L2_PIX_FMT_ARGB32,
-		.bpp = 4,
-	},
-
-	/* Bayer formats */
-	{
-		.code = MEDIA_BUS_FMT_SBGGR8_1X8,
-		.pixelformat = V4L2_PIX_FMT_SBGGR8,
-		.bpp = 1,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SGBRG8_1X8,
-		.pixelformat = V4L2_PIX_FMT_SGBRG8,
-		.bpp = 1,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SGRBG8_1X8,
-		.pixelformat = V4L2_PIX_FMT_SGRBG8,
-		.bpp = 1,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SRGGB8_1X8,
-		.pixelformat = V4L2_PIX_FMT_SRGGB8,
-		.bpp = 1,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SBGGR10_1X10,
-		.pixelformat = V4L2_PIX_FMT_SBGGR10,
-		.bpp = 2,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SGBRG10_1X10,
-		.pixelformat = V4L2_PIX_FMT_SGBRG10,
-		.bpp = 2,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
-		.pixelformat = V4L2_PIX_FMT_SGRBG10,
-		.bpp = 2,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
-		.pixelformat = V4L2_PIX_FMT_SRGGB10,
-		.bpp = 2,
-	},
-
-	/* 10bit raw bayer a-law compressed to 8 bits */
-	{
-		.code = MEDIA_BUS_FMT_SBGGR10_ALAW8_1X8,
-		.pixelformat = V4L2_PIX_FMT_SBGGR10ALAW8,
-		.bpp = 1,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SGBRG10_ALAW8_1X8,
-		.pixelformat = V4L2_PIX_FMT_SGBRG10ALAW8,
-		.bpp = 1,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SGRBG10_ALAW8_1X8,
-		.pixelformat = V4L2_PIX_FMT_SGRBG10ALAW8,
-		.bpp = 1,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SRGGB10_ALAW8_1X8,
-		.pixelformat = V4L2_PIX_FMT_SRGGB10ALAW8,
-		.bpp = 1,
-	},
-
-	/* 10bit raw bayer DPCM compressed to 8 bits */
-	{
-		.code = MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8,
-		.pixelformat = V4L2_PIX_FMT_SBGGR10DPCM8,
-		.bpp = 1,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8,
-		.pixelformat = V4L2_PIX_FMT_SGBRG10DPCM8,
-		.bpp = 1,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
-		.pixelformat = V4L2_PIX_FMT_SGRBG10DPCM8,
-		.bpp = 1,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8,
-		.pixelformat = V4L2_PIX_FMT_SRGGB10DPCM8,
-		.bpp = 1,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SBGGR12_1X12,
-		.pixelformat = V4L2_PIX_FMT_SBGGR12,
-		.bpp = 2,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SGBRG12_1X12,
-		.pixelformat = V4L2_PIX_FMT_SGBRG12,
-		.bpp = 2,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SGRBG12_1X12,
-		.pixelformat = V4L2_PIX_FMT_SGRBG12,
-		.bpp = 2,
-	},
-	{
-		.code = MEDIA_BUS_FMT_SRGGB12_1X12,
-		.pixelformat = V4L2_PIX_FMT_SRGGB12,
-		.bpp = 2,
-	},
-};
-
-const struct vimc_pix_map *vimc_pix_map_by_code(u32 code)
-{
-	unsigned int i;
-
-	for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
-		if (vimc_pix_map_list[i].code == code)
-			return &vimc_pix_map_list[i];
-	}
-	return NULL;
-}
-
-const struct vimc_pix_map *vimc_pix_map_by_pixelformat(u32 pixelformat)
-{
-	unsigned int i;
-
-	for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
-		if (vimc_pix_map_list[i].pixelformat == pixelformat)
-			return &vimc_pix_map_list[i];
-	}
-	return NULL;
-}
-
-int vimc_propagate_frame(struct media_pad *src, const void *frame)
-{
-	struct media_link *link;
-
-	if (!(src->flags & MEDIA_PAD_FL_SOURCE))
-		return -EINVAL;
-
-	/* Send this frame to all sink pads that are direct linked */
-	list_for_each_entry(link, &src->entity->links, list) {
-		if (link->source == src &&
-		    (link->flags & MEDIA_LNK_FL_ENABLED)) {
-			struct vimc_ent_device *ved = NULL;
-			struct media_entity *entity = link->sink->entity;
-
-			if (is_media_entity_v4l2_subdev(entity)) {
-				struct v4l2_subdev *sd =
-					container_of(entity, struct v4l2_subdev,
-						     entity);
-				ved = v4l2_get_subdevdata(sd);
-			} else if (is_media_entity_v4l2_video_device(entity)) {
-				struct video_device *vdev =
-					container_of(entity,
-						     struct video_device,
-						     entity);
-				ved = video_get_drvdata(vdev);
-			}
-			if (ved && ved->process_frame)
-				ved->process_frame(ved, link->sink, frame);
-		}
-	}
-
-	return 0;
-}
-
-static void vimc_device_unregister(struct vimc_device *vimc)
-{
-	unsigned int i;
-
-	media_device_unregister(&vimc->mdev);
-	/* Cleanup (only initialized) entities */
-	for (i = 0; i < vimc->pipe_cfg->num_ents; i++) {
-		if (vimc->ved[i] && vimc->ved[i]->destroy)
-			vimc->ved[i]->destroy(vimc->ved[i]);
-
-		vimc->ved[i] = NULL;
-	}
-	v4l2_device_unregister(&vimc->v4l2_dev);
-	media_device_cleanup(&vimc->mdev);
-}
-
-/* Helper function to allocate and initialize pads */
-struct media_pad *vimc_pads_init(u16 num_pads, const unsigned long *pads_flag)
+static int vimc_create_links(struct vimc_device *vimc)
 {
-	struct media_pad *pads;
 	unsigned int i;
-
-	/* Allocate memory for the pads */
-	pads = kcalloc(num_pads, sizeof(*pads), GFP_KERNEL);
-	if (!pads)
-		return ERR_PTR(-ENOMEM);
-
-	/* Initialize the pads */
-	for (i = 0; i < num_pads; i++) {
-		pads[i].index = i;
-		pads[i].flags = pads_flag[i];
-	}
-
-	return pads;
-}
-
-/*
- * TODO: remove this function when all the
- * entities specific code are implemented
- */
-static void vimc_raw_destroy(struct vimc_ent_device *ved)
-{
-	media_device_unregister_entity(ved->ent);
-
-	media_entity_cleanup(ved->ent);
-
-	vimc_pads_cleanup(ved->pads);
-
-	kfree(ved->ent);
-
-	kfree(ved);
-}
-
-/*
- * TODO: remove this function when all the
- * entities specific code are implemented
- */
-static struct vimc_ent_device *vimc_raw_create(struct v4l2_device *v4l2_dev,
-					       const char *const name,
-					       u16 num_pads,
-					       const unsigned long *pads_flag)
-{
-	struct vimc_ent_device *ved;
 	int ret;
 
-	/* Allocate the main ved struct */
-	ved = kzalloc(sizeof(*ved), GFP_KERNEL);
-	if (!ved)
-		return ERR_PTR(-ENOMEM);
-
-	/* Allocate the media entity */
-	ved->ent = kzalloc(sizeof(*ved->ent), GFP_KERNEL);
-	if (!ved->ent) {
-		ret = -ENOMEM;
-		goto err_free_ved;
-	}
-
-	/* Allocate the pads */
-	ved->pads = vimc_pads_init(num_pads, pads_flag);
-	if (IS_ERR(ved->pads)) {
-		ret = PTR_ERR(ved->pads);
-		goto err_free_ent;
+	/* Initialize the links between entities */
+	for (i = 0; i < vimc->pipe_cfg->num_links; i++) {
+		const struct vimc_ent_link *link = &vimc->pipe_cfg->links[i];
+		/*
+		 * TODO: Check another way of retrieving ved struct without
+		 * relying on platform_get_drvdata
+		 */
+		struct vimc_ent_device *ved_src =
+			platform_get_drvdata(vimc->subdevs[link->src_ent]);
+		struct vimc_ent_device *ved_sink =
+			platform_get_drvdata(vimc->subdevs[link->sink_ent]);
+
+		ret = media_create_pad_link(ved_src->ent, link->src_pad,
+					    ved_sink->ent, link->sink_pad,
+					    link->flags);
+		if (ret)
+			return ret;
 	}
 
-	/* Initialize the media entity */
-	ved->ent->name = name;
-	ved->ent->function = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
-	ret = media_entity_pads_init(ved->ent, num_pads, ved->pads);
-	if (ret)
-		goto err_cleanup_pads;
-
-	/* Register the media entity */
-	ret = media_device_register_entity(v4l2_dev->mdev, ved->ent);
-	if (ret)
-		goto err_cleanup_entity;
-
-	/* Fill out the destroy function and return */
-	ved->destroy = vimc_raw_destroy;
-	return ved;
-
-err_cleanup_entity:
-	media_entity_cleanup(ved->ent);
-err_cleanup_pads:
-	vimc_pads_cleanup(ved->pads);
-err_free_ent:
-	kfree(ved->ent);
-err_free_ved:
-	kfree(ved);
-
-	return ERR_PTR(ret);
+	return 0;
 }
 
-static int vimc_device_register(struct vimc_device *vimc)
+static int vimc_comp_bind(struct device *master)
 {
-	unsigned int i;
+	struct vimc_device *vimc = container_of(to_platform_device(master),
+						struct vimc_device, pdev);
 	int ret;
 
-	/* Allocate memory for the vimc_ent_devices pointers */
-	vimc->ved = devm_kcalloc(vimc->mdev.dev, vimc->pipe_cfg->num_ents,
-				 sizeof(*vimc->ved), GFP_KERNEL);
-	if (!vimc->ved)
-		return -ENOMEM;
-
-	/* Link the media device within the v4l2_device */
-	vimc->v4l2_dev.mdev = &vimc->mdev;
+	dev_dbg(master, "bind");
 
 	/* Register the v4l2 struct */
 	ret = v4l2_device_register(vimc->mdev.dev, &vimc->v4l2_dev);
@@ -514,66 +190,22 @@ static int vimc_device_register(struct vimc_device *vimc)
 		return ret;
 	}
 
-	/* Initialize entities */
-	for (i = 0; i < vimc->pipe_cfg->num_ents; i++) {
-		struct vimc_ent_device *(*create_func)(struct v4l2_device *,
-						       const char *const,
-						       u16,
-						       const unsigned long *);
-
-		/* Register the specific node */
-		switch (vimc->pipe_cfg->ents[i].node) {
-		case VIMC_ENT_NODE_SENSOR:
-			create_func = vimc_sen_create;
-			break;
-
-		case VIMC_ENT_NODE_CAPTURE:
-			create_func = vimc_cap_create;
-			break;
-
-		/* TODO: Instantiate the specific topology node */
-		case VIMC_ENT_NODE_INPUT:
-		case VIMC_ENT_NODE_DEBAYER:
-		case VIMC_ENT_NODE_SCALER:
-		default:
-			/*
-			 * TODO: remove this when all the entities specific
-			 * code are implemented
-			 */
-			create_func = vimc_raw_create;
-			break;
-		}
-
-		vimc->ved[i] = create_func(&vimc->v4l2_dev,
-					   vimc->pipe_cfg->ents[i].name,
-					   vimc->pipe_cfg->ents[i].pads_qty,
-					   vimc->pipe_cfg->ents[i].pads_flag);
-		if (IS_ERR(vimc->ved[i])) {
-			ret = PTR_ERR(vimc->ved[i]);
-			vimc->ved[i] = NULL;
-			goto err;
-		}
-	}
-
-	/* Initialize the links between entities */
-	for (i = 0; i < vimc->pipe_cfg->num_links; i++) {
-		const struct vimc_ent_link *link = &vimc->pipe_cfg->links[i];
+	/* Bind subdevices */
+	ret = component_bind_all(master, &vimc->v4l2_dev);
+	if (ret)
+		goto err_v4l2_unregister;
 
-		ret = media_create_pad_link(vimc->ved[link->src_ent]->ent,
-					    link->src_pad,
-					    vimc->ved[link->sink_ent]->ent,
-					    link->sink_pad,
-					    link->flags);
-		if (ret)
-			goto err;
-	}
+	/* Initialize links */
+	ret = vimc_create_links(vimc);
+	if (ret)
+		goto err_comp_unbind_all;
 
 	/* Register the media device */
 	ret = media_device_register(&vimc->mdev);
 	if (ret) {
 		dev_err(vimc->mdev.dev,
 			"media device register failed (err=%d)\n", ret);
-		return ret;
+		goto err_comp_unbind_all;
 	}
 
 	/* Expose all subdev's nodes*/
@@ -582,32 +214,106 @@ static int vimc_device_register(struct vimc_device *vimc)
 		dev_err(vimc->mdev.dev,
 			"vimc subdev nodes registration failed (err=%d)\n",
 			ret);
-		goto err;
+		goto err_mdev_unregister;
 	}
 
 	return 0;
 
-err:
-	/* Destroy the so far created topology */
-	vimc_device_unregister(vimc);
+err_mdev_unregister:
+	media_device_unregister(&vimc->mdev);
+err_comp_unbind_all:
+	component_unbind_all(master, NULL);
+err_v4l2_unregister:
+	v4l2_device_unregister(&vimc->v4l2_dev);
 
 	return ret;
 }
 
+static void vimc_comp_unbind(struct device *master)
+{
+	struct vimc_device *vimc = container_of(to_platform_device(master),
+						struct vimc_device, pdev);
+
+	dev_dbg(master, "unbind");
+
+	media_device_unregister(&vimc->mdev);
+	component_unbind_all(master, NULL);
+	v4l2_device_unregister(&vimc->v4l2_dev);
+}
+
+static int vimc_comp_compare(struct device *comp, void *data)
+{
+	const struct platform_device *pdev = to_platform_device(comp);
+	const char *name = data;
+
+	return !strcmp(pdev->dev.platform_data, name);
+}
+
+static struct component_match *vimc_add_subdevs(struct vimc_device *vimc)
+{
+	struct component_match *match = NULL;
+	struct vimc_platform_data pdata;
+	int i;
+
+	for (i = 0; i < vimc->pipe_cfg->num_ents; i++) {
+		dev_dbg(&vimc->pdev.dev, "new pdev for %s\n",
+			vimc->pipe_cfg->ents[i].drv);
+
+		strlcpy(pdata.entity_name, vimc->pipe_cfg->ents[i].name,
+			sizeof(pdata.entity_name));
+
+		vimc->subdevs[i] = platform_device_register_data(&vimc->pdev.dev,
+						vimc->pipe_cfg->ents[i].drv,
+						PLATFORM_DEVID_AUTO,
+						&pdata,
+						sizeof(pdata));
+		if (!vimc->subdevs[i]) {
+			while (--i >= 0)
+				platform_device_unregister(vimc->subdevs[i]);
+
+			return ERR_PTR(-ENOMEM);
+		}
+
+		component_match_add(&vimc->pdev.dev, &match, vimc_comp_compare,
+				    (void *)vimc->pipe_cfg->ents[i].name);
+	}
+
+	return match;
+}
+
+static void vimc_rm_subdevs(struct vimc_device *vimc)
+{
+	unsigned int i;
+
+	for (i = 0; i < vimc->pipe_cfg->num_ents; i++)
+		platform_device_unregister(vimc->subdevs[i]);
+}
+
+static const struct component_master_ops vimc_comp_ops = {
+	.bind = vimc_comp_bind,
+	.unbind = vimc_comp_unbind,
+};
+
 static int vimc_probe(struct platform_device *pdev)
 {
-	struct vimc_device *vimc;
+	struct vimc_device *vimc = container_of(pdev, struct vimc_device, pdev);
+	struct component_match *match = NULL;
 	int ret;
 
-	/* Prepare the vimc topology structure */
+	dev_dbg(&pdev->dev, "probe");
 
-	/* Allocate memory for the vimc structure */
-	vimc = kzalloc(sizeof(*vimc), GFP_KERNEL);
-	if (!vimc)
+	/* Create platform_device for each entity in the topology*/
+	vimc->subdevs = devm_kcalloc(&vimc->pdev.dev, vimc->pipe_cfg->num_ents,
+				     sizeof(*vimc->subdevs), GFP_KERNEL);
+	if (!vimc->subdevs)
 		return -ENOMEM;
 
-	/* Set the pipeline configuration struct */
-	vimc->pipe_cfg = &pipe_cfg;
+	match = vimc_add_subdevs(vimc);
+	if (IS_ERR(match))
+		return PTR_ERR(match);
+
+	/* Link the media device within the v4l2_device */
+	vimc->v4l2_dev.mdev = &vimc->mdev;
 
 	/* Initialize media device */
 	strlcpy(vimc->mdev.model, VIMC_MDEV_MODEL_NAME,
@@ -615,28 +321,27 @@ static int vimc_probe(struct platform_device *pdev)
 	vimc->mdev.dev = &pdev->dev;
 	media_device_init(&vimc->mdev);
 
-	/* Create vimc topology */
-	ret = vimc_device_register(vimc);
+	/* Add self to the component system */
+	ret = component_master_add_with_match(&pdev->dev, &vimc_comp_ops,
+					      match);
 	if (ret) {
-		dev_err(vimc->mdev.dev,
-			"vimc device registration failed (err=%d)\n", ret);
+		media_device_cleanup(&vimc->mdev);
+		vimc_rm_subdevs(vimc);
 		kfree(vimc);
 		return ret;
 	}
 
-	/* Link the topology object with the platform device object */
-	platform_set_drvdata(pdev, vimc);
-
 	return 0;
 }
 
 static int vimc_remove(struct platform_device *pdev)
 {
-	struct vimc_device *vimc = platform_get_drvdata(pdev);
+	struct vimc_device *vimc = container_of(pdev, struct vimc_device, pdev);
+
+	dev_dbg(&pdev->dev, "remove");
 
-	/* Destroy all the topology */
-	vimc_device_unregister(vimc);
-	kfree(vimc);
+	component_master_del(&pdev->dev, &vimc_comp_ops);
+	vimc_rm_subdevs(vimc);
 
 	return 0;
 }
@@ -645,9 +350,12 @@ static void vimc_dev_release(struct device *dev)
 {
 }
 
-static struct platform_device vimc_pdev = {
-	.name		= VIMC_PDEV_NAME,
-	.dev.release	= vimc_dev_release,
+static struct vimc_device vimc_dev = {
+	.pipe_cfg = &pipe_cfg,
+	.pdev = {
+		.name = VIMC_PDEV_NAME,
+		.dev.release = vimc_dev_release,
+	}
 };
 
 static struct platform_driver vimc_pdrv = {
@@ -662,29 +370,29 @@ static int __init vimc_init(void)
 {
 	int ret;
 
-	ret = platform_device_register(&vimc_pdev);
+	ret = platform_device_register(&vimc_dev.pdev);
 	if (ret) {
-		dev_err(&vimc_pdev.dev,
+		dev_err(&vimc_dev.pdev.dev,
 			"platform device registration failed (err=%d)\n", ret);
 		return ret;
 	}
 
 	ret = platform_driver_register(&vimc_pdrv);
 	if (ret) {
-		dev_err(&vimc_pdev.dev,
+		dev_err(&vimc_dev.pdev.dev,
 			"platform driver registration failed (err=%d)\n", ret);
-
-		platform_device_unregister(&vimc_pdev);
+		platform_driver_unregister(&vimc_pdrv);
+		return ret;
 	}
 
-	return ret;
+	return 0;
 }
 
 static void __exit vimc_exit(void)
 {
 	platform_driver_unregister(&vimc_pdrv);
 
-	platform_device_unregister(&vimc_pdev);
+	platform_device_unregister(&vimc_dev.pdev);
 }
 
 module_init(vimc_init);
diff --git a/drivers/media/platform/vimc/vimc-core.h b/drivers/media/platform/vimc/vimc-core.h
deleted file mode 100644
index 4525d23211ca..000000000000
--- a/drivers/media/platform/vimc/vimc-core.h
+++ /dev/null
@@ -1,112 +0,0 @@
-/*
- * vimc-core.h Virtual Media Controller Driver
- *
- * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- */
-
-#ifndef _VIMC_CORE_H_
-#define _VIMC_CORE_H_
-
-#include <linux/slab.h>
-#include <media/v4l2-device.h>
-
-/**
- * struct vimc_pix_map - maps media bus code with v4l2 pixel format
- *
- * @code:		media bus format code defined by MEDIA_BUS_FMT_* macros
- * @bbp:		number of bytes each pixel occupies
- * @pixelformat:	pixel format devined by V4L2_PIX_FMT_* macros
- *
- * Struct which matches the MEDIA_BUS_FMT_* codes with the corresponding
- * V4L2_PIX_FMT_* fourcc pixelformat and its bytes per pixel (bpp)
- */
-struct vimc_pix_map {
-	unsigned int code;
-	unsigned int bpp;
-	u32 pixelformat;
-};
-
-/**
- * struct vimc_ent_device - core struct that represents a node in the topology
- *
- * @ent:		the pointer to struct media_entity for the node
- * @pads:		the list of pads of the node
- * @destroy:		callback to destroy the node
- * @process_frame:	callback send a frame to that node
- *
- * Each node of the topology must create a vimc_ent_device struct. Depending on
- * the node it will be of an instance of v4l2_subdev or video_device struct
- * where both contains a struct media_entity.
- * Those structures should embedded the vimc_ent_device struct through
- * v4l2_set_subdevdata() and video_set_drvdata() respectivaly, allowing the
- * vimc_ent_device struct to be retrieved from the corresponding struct
- * media_entity
- */
-struct vimc_ent_device {
-	struct media_entity *ent;
-	struct media_pad *pads;
-	void (*destroy)(struct vimc_ent_device *);
-	void (*process_frame)(struct vimc_ent_device *ved,
-			      struct media_pad *sink, const void *frame);
-};
-
-/**
- * vimc_propagate_frame - propagate a frame through the topology
- *
- * @src:	the source pad where the frame is being originated
- * @frame:	the frame to be propagated
- *
- * This function will call the process_frame callback from the vimc_ent_device
- * struct of the nodes directly connected to the @src pad
- */
-int vimc_propagate_frame(struct media_pad *src, const void *frame);
-
-/**
- * vimc_pads_init - initialize pads
- *
- * @num_pads:	number of pads to initialize
- * @pads_flags:	flags to use in each pad
- *
- * Helper functions to allocate/initialize pads
- */
-struct media_pad *vimc_pads_init(u16 num_pads,
-				 const unsigned long *pads_flag);
-
-/**
- * vimc_pads_cleanup - free pads
- *
- * @pads: pointer to the pads
- *
- * Helper function to free the pads initialized with vimc_pads_init
- */
-static inline void vimc_pads_cleanup(struct media_pad *pads)
-{
-	kfree(pads);
-}
-
-/**
- * vimc_pix_map_by_code - get vimc_pix_map struct by media bus code
- *
- * @code:		media bus format code defined by MEDIA_BUS_FMT_* macros
- */
-const struct vimc_pix_map *vimc_pix_map_by_code(u32 code);
-
-/**
- * vimc_pix_map_by_pixelformat - get vimc_pix_map struct by v4l2 pixel format
- *
- * @pixelformat:	pixel format devined by V4L2_PIX_FMT_* macros
- */
-const struct vimc_pix_map *vimc_pix_map_by_pixelformat(u32 pixelformat);
-
-#endif
diff --git a/drivers/media/platform/vimc/vimc-debayer.c b/drivers/media/platform/vimc/vimc-debayer.c
new file mode 100644
index 000000000000..35b15bd4d61d
--- /dev/null
+++ b/drivers/media/platform/vimc/vimc-debayer.c
@@ -0,0 +1,601 @@
+/*
+ * vimc-debayer.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/component.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/vmalloc.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/v4l2-subdev.h>
+
+#include "vimc-common.h"
+
+#define VIMC_DEB_DRV_NAME "vimc-debayer"
+
+static unsigned int deb_mean_win_size = 3;
+module_param(deb_mean_win_size, uint, 0000);
+MODULE_PARM_DESC(deb_mean_win_size, " the window size to calculate the mean.\n"
+	"NOTE: the window size need to be an odd number, as the main pixel "
+	"stays in the center of the window, otherwise the next odd number "
+	"is considered");
+
+#define IS_SINK(pad) (!pad)
+#define IS_SRC(pad)  (pad)
+
+enum vimc_deb_rgb_colors {
+	VIMC_DEB_RED = 0,
+	VIMC_DEB_GREEN = 1,
+	VIMC_DEB_BLUE = 2,
+};
+
+struct vimc_deb_pix_map {
+	u32 code;
+	enum vimc_deb_rgb_colors order[2][2];
+};
+
+struct vimc_deb_device {
+	struct vimc_ent_device ved;
+	struct v4l2_subdev sd;
+	struct device *dev;
+	/* The active format */
+	struct v4l2_mbus_framefmt sink_fmt;
+	u32 src_code;
+	void (*set_rgb_src)(struct vimc_deb_device *vdeb, unsigned int lin,
+			    unsigned int col, unsigned int rgb[3]);
+	/* Values calculated when the stream starts */
+	u8 *src_frame;
+	const struct vimc_deb_pix_map *sink_pix_map;
+	unsigned int sink_bpp;
+};
+
+static const struct v4l2_mbus_framefmt sink_fmt_default = {
+	.width = 640,
+	.height = 480,
+	.code = MEDIA_BUS_FMT_RGB888_1X24,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = V4L2_COLORSPACE_DEFAULT,
+};
+
+static const struct vimc_deb_pix_map vimc_deb_pix_map_list[] = {
+	{
+		.code = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.order = { { VIMC_DEB_BLUE, VIMC_DEB_GREEN },
+			   { VIMC_DEB_GREEN, VIMC_DEB_RED } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGBRG8_1X8,
+		.order = { { VIMC_DEB_GREEN, VIMC_DEB_BLUE },
+			   { VIMC_DEB_RED, VIMC_DEB_GREEN } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGRBG8_1X8,
+		.order = { { VIMC_DEB_GREEN, VIMC_DEB_RED },
+			   { VIMC_DEB_BLUE, VIMC_DEB_GREEN } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SRGGB8_1X8,
+		.order = { { VIMC_DEB_RED, VIMC_DEB_GREEN },
+			   { VIMC_DEB_GREEN, VIMC_DEB_BLUE } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SBGGR10_1X10,
+		.order = { { VIMC_DEB_BLUE, VIMC_DEB_GREEN },
+			   { VIMC_DEB_GREEN, VIMC_DEB_RED } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGBRG10_1X10,
+		.order = { { VIMC_DEB_GREEN, VIMC_DEB_BLUE },
+			   { VIMC_DEB_RED, VIMC_DEB_GREEN } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.order = { { VIMC_DEB_GREEN, VIMC_DEB_RED },
+			   { VIMC_DEB_BLUE, VIMC_DEB_GREEN } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.order = { { VIMC_DEB_RED, VIMC_DEB_GREEN },
+			   { VIMC_DEB_GREEN, VIMC_DEB_BLUE } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SBGGR12_1X12,
+		.order = { { VIMC_DEB_BLUE, VIMC_DEB_GREEN },
+			   { VIMC_DEB_GREEN, VIMC_DEB_RED } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGBRG12_1X12,
+		.order = { { VIMC_DEB_GREEN, VIMC_DEB_BLUE },
+			   { VIMC_DEB_RED, VIMC_DEB_GREEN } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGRBG12_1X12,
+		.order = { { VIMC_DEB_GREEN, VIMC_DEB_RED },
+			   { VIMC_DEB_BLUE, VIMC_DEB_GREEN } }
+	},
+	{
+		.code = MEDIA_BUS_FMT_SRGGB12_1X12,
+		.order = { { VIMC_DEB_RED, VIMC_DEB_GREEN },
+			   { VIMC_DEB_GREEN, VIMC_DEB_BLUE } }
+	},
+};
+
+static const struct vimc_deb_pix_map *vimc_deb_pix_map_by_code(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(vimc_deb_pix_map_list); i++)
+		if (vimc_deb_pix_map_list[i].code == code)
+			return &vimc_deb_pix_map_list[i];
+
+	return NULL;
+}
+
+static int vimc_deb_init_cfg(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_pad_config *cfg)
+{
+	struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf;
+	unsigned int i;
+
+	mf = v4l2_subdev_get_try_format(sd, cfg, 0);
+	*mf = sink_fmt_default;
+
+	for (i = 1; i < sd->entity.num_pads; i++) {
+		mf = v4l2_subdev_get_try_format(sd, cfg, i);
+		*mf = sink_fmt_default;
+		mf->code = vdeb->src_code;
+	}
+
+	return 0;
+}
+
+static int vimc_deb_enum_mbus_code(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_pad_config *cfg,
+				   struct v4l2_subdev_mbus_code_enum *code)
+{
+	/* We only support one format for source pads */
+	if (IS_SRC(code->pad)) {
+		struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
+
+		if (code->index)
+			return -EINVAL;
+
+		code->code = vdeb->src_code;
+	} else {
+		if (code->index >= ARRAY_SIZE(vimc_deb_pix_map_list))
+			return -EINVAL;
+
+		code->code = vimc_deb_pix_map_list[code->index].code;
+	}
+
+	return 0;
+}
+
+static int vimc_deb_enum_frame_size(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_pad_config *cfg,
+				    struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
+
+	if (fse->index)
+		return -EINVAL;
+
+	if (IS_SINK(fse->pad)) {
+		const struct vimc_deb_pix_map *vpix =
+			vimc_deb_pix_map_by_code(fse->code);
+
+		if (!vpix)
+			return -EINVAL;
+	} else if (fse->code != vdeb->src_code) {
+		return -EINVAL;
+	}
+
+	fse->min_width = VIMC_FRAME_MIN_WIDTH;
+	fse->max_width = VIMC_FRAME_MAX_WIDTH;
+	fse->min_height = VIMC_FRAME_MIN_HEIGHT;
+	fse->max_height = VIMC_FRAME_MAX_HEIGHT;
+
+	return 0;
+}
+
+static int vimc_deb_get_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
+
+	/* Get the current sink format */
+	fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
+		      *v4l2_subdev_get_try_format(sd, cfg, 0) :
+		      vdeb->sink_fmt;
+
+	/* Set the right code for the source pad */
+	if (IS_SRC(fmt->pad))
+		fmt->format.code = vdeb->src_code;
+
+	return 0;
+}
+
+static void vimc_deb_adjust_sink_fmt(struct v4l2_mbus_framefmt *fmt)
+{
+	const struct vimc_deb_pix_map *vpix;
+
+	/* Don't accept a code that is not on the debayer table */
+	vpix = vimc_deb_pix_map_by_code(fmt->code);
+	if (!vpix)
+		fmt->code = sink_fmt_default.code;
+
+	fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
+			     VIMC_FRAME_MAX_WIDTH) & ~1;
+	fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
+			      VIMC_FRAME_MAX_HEIGHT) & ~1;
+
+	if (fmt->field == V4L2_FIELD_ANY)
+		fmt->field = sink_fmt_default.field;
+
+	vimc_colorimetry_clamp(fmt);
+}
+
+static int vimc_deb_set_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *sink_fmt;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		/* Do not change the format while stream is on */
+		if (vdeb->src_frame)
+			return -EBUSY;
+
+		sink_fmt = &vdeb->sink_fmt;
+	} else {
+		sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+	}
+
+	/*
+	 * Do not change the format of the source pad,
+	 * it is propagated from the sink
+	 */
+	if (IS_SRC(fmt->pad)) {
+		fmt->format = *sink_fmt;
+		/* TODO: Add support for other formats */
+		fmt->format.code = vdeb->src_code;
+	} else {
+		/* Set the new format in the sink pad */
+		vimc_deb_adjust_sink_fmt(&fmt->format);
+
+		dev_dbg(vdeb->dev, "%s: sink format update: "
+			"old:%dx%d (0x%x, %d, %d, %d, %d) "
+			"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vdeb->sd.name,
+			/* old */
+			sink_fmt->width, sink_fmt->height, sink_fmt->code,
+			sink_fmt->colorspace, sink_fmt->quantization,
+			sink_fmt->xfer_func, sink_fmt->ycbcr_enc,
+			/* new */
+			fmt->format.width, fmt->format.height, fmt->format.code,
+			fmt->format.colorspace,	fmt->format.quantization,
+			fmt->format.xfer_func, fmt->format.ycbcr_enc);
+
+		*sink_fmt = fmt->format;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops vimc_deb_pad_ops = {
+	.init_cfg		= vimc_deb_init_cfg,
+	.enum_mbus_code		= vimc_deb_enum_mbus_code,
+	.enum_frame_size	= vimc_deb_enum_frame_size,
+	.get_fmt		= vimc_deb_get_fmt,
+	.set_fmt		= vimc_deb_set_fmt,
+};
+
+static void vimc_deb_set_rgb_mbus_fmt_rgb888_1x24(struct vimc_deb_device *vdeb,
+						  unsigned int lin,
+						  unsigned int col,
+						  unsigned int rgb[3])
+{
+	unsigned int i, index;
+
+	index = VIMC_FRAME_INDEX(lin, col, vdeb->sink_fmt.width, 3);
+	for (i = 0; i < 3; i++)
+		vdeb->src_frame[index + i] = rgb[i];
+}
+
+static int vimc_deb_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (enable) {
+		const struct vimc_pix_map *vpix;
+		unsigned int frame_size;
+
+		if (vdeb->src_frame)
+			return 0;
+
+		/* Calculate the frame size of the source pad */
+		vpix = vimc_pix_map_by_code(vdeb->src_code);
+		frame_size = vdeb->sink_fmt.width * vdeb->sink_fmt.height *
+				vpix->bpp;
+
+		/* Save the bytes per pixel of the sink */
+		vpix = vimc_pix_map_by_code(vdeb->sink_fmt.code);
+		vdeb->sink_bpp = vpix->bpp;
+
+		/* Get the corresponding pixel map from the table */
+		vdeb->sink_pix_map =
+			vimc_deb_pix_map_by_code(vdeb->sink_fmt.code);
+
+		/*
+		 * Allocate the frame buffer. Use vmalloc to be able to
+		 * allocate a large amount of memory
+		 */
+		vdeb->src_frame = vmalloc(frame_size);
+		if (!vdeb->src_frame)
+			return -ENOMEM;
+
+		/* Turn the stream on in the subdevices directly connected */
+		ret = vimc_pipeline_s_stream(&vdeb->sd.entity, 1);
+		if (ret) {
+			vfree(vdeb->src_frame);
+			vdeb->src_frame = NULL;
+			return ret;
+		}
+	} else {
+		if (!vdeb->src_frame)
+			return 0;
+
+		/* Disable streaming from the pipe */
+		ret = vimc_pipeline_s_stream(&vdeb->sd.entity, 0);
+		if (ret)
+			return ret;
+
+		vfree(vdeb->src_frame);
+		vdeb->src_frame = NULL;
+	}
+
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops vimc_deb_video_ops = {
+	.s_stream = vimc_deb_s_stream,
+};
+
+static const struct v4l2_subdev_ops vimc_deb_ops = {
+	.pad = &vimc_deb_pad_ops,
+	.video = &vimc_deb_video_ops,
+};
+
+static unsigned int vimc_deb_get_val(const u8 *bytes,
+				     const unsigned int n_bytes)
+{
+	unsigned int i;
+	unsigned int acc = 0;
+
+	for (i = 0; i < n_bytes; i++)
+		acc = acc + (bytes[i] << (8 * i));
+
+	return acc;
+}
+
+static void vimc_deb_calc_rgb_sink(struct vimc_deb_device *vdeb,
+				   const u8 *frame,
+				   const unsigned int lin,
+				   const unsigned int col,
+				   unsigned int rgb[3])
+{
+	unsigned int i, seek, wlin, wcol;
+	unsigned int n_rgb[3] = {0, 0, 0};
+
+	for (i = 0; i < 3; i++)
+		rgb[i] = 0;
+
+	/*
+	 * Calculate how many we need to subtract to get to the pixel in
+	 * the top left corner of the mean window (considering the current
+	 * pixel as the center)
+	 */
+	seek = deb_mean_win_size / 2;
+
+	/* Sum the values of the colors in the mean window */
+
+	dev_dbg(vdeb->dev,
+		"deb: %s: --- Calc pixel %dx%d, window mean %d, seek %d ---\n",
+		vdeb->sd.name, lin, col, vdeb->sink_fmt.height, seek);
+
+	/*
+	 * Iterate through all the lines in the mean window, start
+	 * with zero if the pixel is outside the frame and don't pass
+	 * the height when the pixel is in the bottom border of the
+	 * frame
+	 */
+	for (wlin = seek > lin ? 0 : lin - seek;
+	     wlin < lin + seek + 1 && wlin < vdeb->sink_fmt.height;
+	     wlin++) {
+
+		/*
+		 * Iterate through all the columns in the mean window, start
+		 * with zero if the pixel is outside the frame and don't pass
+		 * the width when the pixel is in the right border of the
+		 * frame
+		 */
+		for (wcol = seek > col ? 0 : col - seek;
+		     wcol < col + seek + 1 && wcol < vdeb->sink_fmt.width;
+		     wcol++) {
+			enum vimc_deb_rgb_colors color;
+			unsigned int index;
+
+			/* Check which color this pixel is */
+			color = vdeb->sink_pix_map->order[wlin % 2][wcol % 2];
+
+			index = VIMC_FRAME_INDEX(wlin, wcol,
+						 vdeb->sink_fmt.width,
+						 vdeb->sink_bpp);
+
+			dev_dbg(vdeb->dev,
+				"deb: %s: RGB CALC: frame index %d, win pos %dx%d, color %d\n",
+				vdeb->sd.name, index, wlin, wcol, color);
+
+			/* Get its value */
+			rgb[color] = rgb[color] +
+				vimc_deb_get_val(&frame[index], vdeb->sink_bpp);
+
+			/* Save how many values we already added */
+			n_rgb[color]++;
+
+			dev_dbg(vdeb->dev, "deb: %s: RGB CALC: val %d, n %d\n",
+				vdeb->sd.name, rgb[color], n_rgb[color]);
+		}
+	}
+
+	/* Calculate the mean */
+	for (i = 0; i < 3; i++) {
+		dev_dbg(vdeb->dev,
+			"deb: %s: PRE CALC: %dx%d Color %d, val %d, n %d\n",
+			vdeb->sd.name, lin, col, i, rgb[i], n_rgb[i]);
+
+		if (n_rgb[i])
+			rgb[i] = rgb[i] / n_rgb[i];
+
+		dev_dbg(vdeb->dev,
+			"deb: %s: FINAL CALC: %dx%d Color %d, val %d\n",
+			vdeb->sd.name, lin, col, i, rgb[i]);
+	}
+}
+
+static void vimc_deb_process_frame(struct vimc_ent_device *ved,
+				   struct media_pad *sink,
+				   const void *sink_frame)
+{
+	struct vimc_deb_device *vdeb = container_of(ved, struct vimc_deb_device,
+						    ved);
+	unsigned int rgb[3];
+	unsigned int i, j;
+
+	/* If the stream in this node is not active, just return */
+	if (!vdeb->src_frame)
+		return;
+
+	for (i = 0; i < vdeb->sink_fmt.height; i++)
+		for (j = 0; j < vdeb->sink_fmt.width; j++) {
+			vimc_deb_calc_rgb_sink(vdeb, sink_frame, i, j, rgb);
+			vdeb->set_rgb_src(vdeb, i, j, rgb);
+		}
+
+	/* Propagate the frame through all source pads */
+	for (i = 1; i < vdeb->sd.entity.num_pads; i++) {
+		struct media_pad *pad = &vdeb->sd.entity.pads[i];
+
+		vimc_propagate_frame(pad, vdeb->src_frame);
+	}
+}
+
+static void vimc_deb_comp_unbind(struct device *comp, struct device *master,
+				 void *master_data)
+{
+	struct vimc_ent_device *ved = dev_get_drvdata(comp);
+	struct vimc_deb_device *vdeb = container_of(ved, struct vimc_deb_device,
+						    ved);
+
+	vimc_ent_sd_unregister(ved, &vdeb->sd);
+	kfree(vdeb);
+}
+
+static int vimc_deb_comp_bind(struct device *comp, struct device *master,
+			      void *master_data)
+{
+	struct v4l2_device *v4l2_dev = master_data;
+	struct vimc_platform_data *pdata = comp->platform_data;
+	struct vimc_deb_device *vdeb;
+	int ret;
+
+	/* Allocate the vdeb struct */
+	vdeb = kzalloc(sizeof(*vdeb), GFP_KERNEL);
+	if (!vdeb)
+		return -ENOMEM;
+
+	/* Initialize ved and sd */
+	ret = vimc_ent_sd_register(&vdeb->ved, &vdeb->sd, v4l2_dev,
+				   pdata->entity_name,
+				   MEDIA_ENT_F_ATV_DECODER, 2,
+				   (const unsigned long[2]) {MEDIA_PAD_FL_SINK,
+				   MEDIA_PAD_FL_SOURCE},
+				   &vimc_deb_ops);
+	if (ret) {
+		kfree(vdeb);
+		return ret;
+	}
+
+	vdeb->ved.process_frame = vimc_deb_process_frame;
+	dev_set_drvdata(comp, &vdeb->ved);
+	vdeb->dev = comp;
+
+	/* Initialize the frame format */
+	vdeb->sink_fmt = sink_fmt_default;
+	/*
+	 * TODO: Add support for more output formats, we only support
+	 * RGB888 for now
+	 * NOTE: the src format is always the same as the sink, except
+	 * for the code
+	 */
+	vdeb->src_code = MEDIA_BUS_FMT_RGB888_1X24;
+	vdeb->set_rgb_src = vimc_deb_set_rgb_mbus_fmt_rgb888_1x24;
+
+	return 0;
+}
+
+static const struct component_ops vimc_deb_comp_ops = {
+	.bind = vimc_deb_comp_bind,
+	.unbind = vimc_deb_comp_unbind,
+};
+
+static int vimc_deb_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &vimc_deb_comp_ops);
+}
+
+static int vimc_deb_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &vimc_deb_comp_ops);
+
+	return 0;
+}
+
+static struct platform_driver vimc_deb_pdrv = {
+	.probe		= vimc_deb_probe,
+	.remove		= vimc_deb_remove,
+	.driver		= {
+		.name	= VIMC_DEB_DRV_NAME,
+	},
+};
+
+static const struct platform_device_id vimc_deb_driver_ids[] = {
+	{
+		.name           = VIMC_DEB_DRV_NAME,
+	},
+	{ }
+};
+
+module_platform_driver(vimc_deb_pdrv);
+
+MODULE_DEVICE_TABLE(platform, vimc_deb_driver_ids);
+
+MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Debayer");
+MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/vimc/vimc-scaler.c b/drivers/media/platform/vimc/vimc-scaler.c
new file mode 100644
index 000000000000..fe77505d2679
--- /dev/null
+++ b/drivers/media/platform/vimc/vimc-scaler.c
@@ -0,0 +1,455 @@
+/*
+ * vimc-scaler.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/component.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/vmalloc.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/v4l2-subdev.h>
+
+#include "vimc-common.h"
+
+#define VIMC_SCA_DRV_NAME "vimc-scaler"
+
+static unsigned int sca_mult = 3;
+module_param(sca_mult, uint, 0000);
+MODULE_PARM_DESC(sca_mult, " the image size multiplier");
+
+#define IS_SINK(pad)	(!pad)
+#define IS_SRC(pad)	(pad)
+#define MAX_ZOOM	8
+
+struct vimc_sca_device {
+	struct vimc_ent_device ved;
+	struct v4l2_subdev sd;
+	struct device *dev;
+	/* NOTE: the source fmt is the same as the sink
+	 * with the width and hight multiplied by mult
+	 */
+	struct v4l2_mbus_framefmt sink_fmt;
+	/* Values calculated when the stream starts */
+	u8 *src_frame;
+	unsigned int src_line_size;
+	unsigned int bpp;
+};
+
+static const struct v4l2_mbus_framefmt sink_fmt_default = {
+	.width = 640,
+	.height = 480,
+	.code = MEDIA_BUS_FMT_RGB888_1X24,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = V4L2_COLORSPACE_DEFAULT,
+};
+
+static int vimc_sca_init_cfg(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_pad_config *cfg)
+{
+	struct v4l2_mbus_framefmt *mf;
+	unsigned int i;
+
+	mf = v4l2_subdev_get_try_format(sd, cfg, 0);
+	*mf = sink_fmt_default;
+
+	for (i = 1; i < sd->entity.num_pads; i++) {
+		mf = v4l2_subdev_get_try_format(sd, cfg, i);
+		*mf = sink_fmt_default;
+		mf->width = mf->width * sca_mult;
+		mf->height = mf->height * sca_mult;
+	}
+
+	return 0;
+}
+
+static int vimc_sca_enum_mbus_code(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_pad_config *cfg,
+				   struct v4l2_subdev_mbus_code_enum *code)
+{
+	const struct vimc_pix_map *vpix = vimc_pix_map_by_index(code->index);
+
+	/* We don't support bayer format */
+	if (!vpix || vpix->bayer)
+		return -EINVAL;
+
+	code->code = vpix->code;
+
+	return 0;
+}
+
+static int vimc_sca_enum_frame_size(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_pad_config *cfg,
+				    struct v4l2_subdev_frame_size_enum *fse)
+{
+	const struct vimc_pix_map *vpix;
+
+	if (fse->index)
+		return -EINVAL;
+
+	/* Only accept code in the pix map table in non bayer format */
+	vpix = vimc_pix_map_by_code(fse->code);
+	if (!vpix || vpix->bayer)
+		return -EINVAL;
+
+	fse->min_width = VIMC_FRAME_MIN_WIDTH;
+	fse->min_height = VIMC_FRAME_MIN_HEIGHT;
+
+	if (IS_SINK(fse->pad)) {
+		fse->max_width = VIMC_FRAME_MAX_WIDTH;
+		fse->max_height = VIMC_FRAME_MAX_HEIGHT;
+	} else {
+		fse->max_width = VIMC_FRAME_MAX_WIDTH * MAX_ZOOM;
+		fse->max_height = VIMC_FRAME_MAX_HEIGHT * MAX_ZOOM;
+	}
+
+	return 0;
+}
+
+static int vimc_sca_get_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_format *format)
+{
+	struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
+
+	/* Get the current sink format */
+	format->format = (format->which == V4L2_SUBDEV_FORMAT_TRY) ?
+			 *v4l2_subdev_get_try_format(sd, cfg, 0) :
+			 vsca->sink_fmt;
+
+	/* Scale the frame size for the source pad */
+	if (IS_SRC(format->pad)) {
+		format->format.width = vsca->sink_fmt.width * sca_mult;
+		format->format.height = vsca->sink_fmt.height * sca_mult;
+	}
+
+	return 0;
+}
+
+static void vimc_sca_adjust_sink_fmt(struct v4l2_mbus_framefmt *fmt)
+{
+	const struct vimc_pix_map *vpix;
+
+	/* Only accept code in the pix map table in non bayer format */
+	vpix = vimc_pix_map_by_code(fmt->code);
+	if (!vpix || vpix->bayer)
+		fmt->code = sink_fmt_default.code;
+
+	fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
+			     VIMC_FRAME_MAX_WIDTH) & ~1;
+	fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
+			      VIMC_FRAME_MAX_HEIGHT) & ~1;
+
+	if (fmt->field == V4L2_FIELD_ANY)
+		fmt->field = sink_fmt_default.field;
+
+	vimc_colorimetry_clamp(fmt);
+}
+
+static int vimc_sca_set_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *sink_fmt;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		/* Do not change the format while stream is on */
+		if (vsca->src_frame)
+			return -EBUSY;
+
+		sink_fmt = &vsca->sink_fmt;
+	} else {
+		sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+	}
+
+	/*
+	 * Do not change the format of the source pad,
+	 * it is propagated from the sink
+	 */
+	if (IS_SRC(fmt->pad)) {
+		fmt->format = *sink_fmt;
+		fmt->format.width = sink_fmt->width * sca_mult;
+		fmt->format.height = sink_fmt->height * sca_mult;
+	} else {
+		/* Set the new format in the sink pad */
+		vimc_sca_adjust_sink_fmt(&fmt->format);
+
+		dev_dbg(vsca->dev, "%s: sink format update: "
+			"old:%dx%d (0x%x, %d, %d, %d, %d) "
+			"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsca->sd.name,
+			/* old */
+			sink_fmt->width, sink_fmt->height, sink_fmt->code,
+			sink_fmt->colorspace, sink_fmt->quantization,
+			sink_fmt->xfer_func, sink_fmt->ycbcr_enc,
+			/* new */
+			fmt->format.width, fmt->format.height, fmt->format.code,
+			fmt->format.colorspace,	fmt->format.quantization,
+			fmt->format.xfer_func, fmt->format.ycbcr_enc);
+
+		*sink_fmt = fmt->format;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops vimc_sca_pad_ops = {
+	.init_cfg		= vimc_sca_init_cfg,
+	.enum_mbus_code		= vimc_sca_enum_mbus_code,
+	.enum_frame_size	= vimc_sca_enum_frame_size,
+	.get_fmt		= vimc_sca_get_fmt,
+	.set_fmt		= vimc_sca_set_fmt,
+};
+
+static int vimc_sca_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (enable) {
+		const struct vimc_pix_map *vpix;
+		unsigned int frame_size;
+
+		if (vsca->src_frame)
+			return 0;
+
+		/* Save the bytes per pixel of the sink */
+		vpix = vimc_pix_map_by_code(vsca->sink_fmt.code);
+		vsca->bpp = vpix->bpp;
+
+		/* Calculate the width in bytes of the src frame */
+		vsca->src_line_size = vsca->sink_fmt.width *
+				      sca_mult * vsca->bpp;
+
+		/* Calculate the frame size of the source pad */
+		frame_size = vsca->src_line_size * vsca->sink_fmt.height *
+			     sca_mult;
+
+		/* Allocate the frame buffer. Use vmalloc to be able to
+		 * allocate a large amount of memory
+		 */
+		vsca->src_frame = vmalloc(frame_size);
+		if (!vsca->src_frame)
+			return -ENOMEM;
+
+		/* Turn the stream on in the subdevices directly connected */
+		ret = vimc_pipeline_s_stream(&vsca->sd.entity, 1);
+		if (ret) {
+			vfree(vsca->src_frame);
+			vsca->src_frame = NULL;
+			return ret;
+		}
+	} else {
+		if (!vsca->src_frame)
+			return 0;
+
+		/* Disable streaming from the pipe */
+		ret = vimc_pipeline_s_stream(&vsca->sd.entity, 0);
+		if (ret)
+			return ret;
+
+		vfree(vsca->src_frame);
+		vsca->src_frame = NULL;
+	}
+
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops vimc_sca_video_ops = {
+	.s_stream = vimc_sca_s_stream,
+};
+
+static const struct v4l2_subdev_ops vimc_sca_ops = {
+	.pad = &vimc_sca_pad_ops,
+	.video = &vimc_sca_video_ops,
+};
+
+static void vimc_sca_fill_pix(u8 *const ptr,
+			      const u8 *const pixel,
+			      const unsigned int bpp)
+{
+	unsigned int i;
+
+	/* copy the pixel to the pointer */
+	for (i = 0; i < bpp; i++)
+		ptr[i] = pixel[i];
+}
+
+static void vimc_sca_scale_pix(const struct vimc_sca_device *const vsca,
+			       const unsigned int lin, const unsigned int col,
+			       const u8 *const sink_frame)
+{
+	unsigned int i, j, index;
+	const u8 *pixel;
+
+	/* Point to the pixel value in position (lin, col) in the sink frame */
+	index = VIMC_FRAME_INDEX(lin, col,
+				 vsca->sink_fmt.width,
+				 vsca->bpp);
+	pixel = &sink_frame[index];
+
+	dev_dbg(vsca->dev,
+		"sca: %s: --- scale_pix sink pos %dx%d, index %d ---\n",
+		vsca->sd.name, lin, col, index);
+
+	/* point to the place we are going to put the first pixel
+	 * in the scaled src frame
+	 */
+	index = VIMC_FRAME_INDEX(lin * sca_mult, col * sca_mult,
+				 vsca->sink_fmt.width * sca_mult, vsca->bpp);
+
+	dev_dbg(vsca->dev, "sca: %s: scale_pix src pos %dx%d, index %d\n",
+		vsca->sd.name, lin * sca_mult, col * sca_mult, index);
+
+	/* Repeat this pixel mult times */
+	for (i = 0; i < sca_mult; i++) {
+		/* Iterate through each beginning of a
+		 * pixel repetition in a line
+		 */
+		for (j = 0; j < sca_mult * vsca->bpp; j += vsca->bpp) {
+			dev_dbg(vsca->dev,
+				"sca: %s: sca: scale_pix src pos %d\n",
+				vsca->sd.name, index + j);
+
+			/* copy the pixel to the position index + j */
+			vimc_sca_fill_pix(&vsca->src_frame[index + j],
+					  pixel, vsca->bpp);
+		}
+
+		/* move the index to the next line */
+		index += vsca->src_line_size;
+	}
+}
+
+static void vimc_sca_fill_src_frame(const struct vimc_sca_device *const vsca,
+				    const u8 *const sink_frame)
+{
+	unsigned int i, j;
+
+	/* Scale each pixel from the original sink frame */
+	/* TODO: implement scale down, only scale up is supported for now */
+	for (i = 0; i < vsca->sink_fmt.height; i++)
+		for (j = 0; j < vsca->sink_fmt.width; j++)
+			vimc_sca_scale_pix(vsca, i, j, sink_frame);
+}
+
+static void vimc_sca_process_frame(struct vimc_ent_device *ved,
+				   struct media_pad *sink,
+				   const void *sink_frame)
+{
+	struct vimc_sca_device *vsca = container_of(ved, struct vimc_sca_device,
+						    ved);
+	unsigned int i;
+
+	/* If the stream in this node is not active, just return */
+	if (!vsca->src_frame)
+		return;
+
+	vimc_sca_fill_src_frame(vsca, sink_frame);
+
+	/* Propagate the frame through all source pads */
+	for (i = 1; i < vsca->sd.entity.num_pads; i++) {
+		struct media_pad *pad = &vsca->sd.entity.pads[i];
+
+		vimc_propagate_frame(pad, vsca->src_frame);
+	}
+};
+
+static void vimc_sca_comp_unbind(struct device *comp, struct device *master,
+				 void *master_data)
+{
+	struct vimc_ent_device *ved = dev_get_drvdata(comp);
+	struct vimc_sca_device *vsca = container_of(ved, struct vimc_sca_device,
+						    ved);
+
+	vimc_ent_sd_unregister(ved, &vsca->sd);
+	kfree(vsca);
+}
+
+
+static int vimc_sca_comp_bind(struct device *comp, struct device *master,
+			      void *master_data)
+{
+	struct v4l2_device *v4l2_dev = master_data;
+	struct vimc_platform_data *pdata = comp->platform_data;
+	struct vimc_sca_device *vsca;
+	int ret;
+
+	/* Allocate the vsca struct */
+	vsca = kzalloc(sizeof(*vsca), GFP_KERNEL);
+	if (!vsca)
+		return -ENOMEM;
+
+	/* Initialize ved and sd */
+	ret = vimc_ent_sd_register(&vsca->ved, &vsca->sd, v4l2_dev,
+				   pdata->entity_name,
+				   MEDIA_ENT_F_ATV_DECODER, 2,
+				   (const unsigned long[2]) {MEDIA_PAD_FL_SINK,
+				   MEDIA_PAD_FL_SOURCE},
+				   &vimc_sca_ops);
+	if (ret) {
+		kfree(vsca);
+		return ret;
+	}
+
+	vsca->ved.process_frame = vimc_sca_process_frame;
+	dev_set_drvdata(comp, &vsca->ved);
+	vsca->dev = comp;
+
+	/* Initialize the frame format */
+	vsca->sink_fmt = sink_fmt_default;
+
+	return 0;
+}
+
+static const struct component_ops vimc_sca_comp_ops = {
+	.bind = vimc_sca_comp_bind,
+	.unbind = vimc_sca_comp_unbind,
+};
+
+static int vimc_sca_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &vimc_sca_comp_ops);
+}
+
+static int vimc_sca_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &vimc_sca_comp_ops);
+
+	return 0;
+}
+
+static struct platform_driver vimc_sca_pdrv = {
+	.probe		= vimc_sca_probe,
+	.remove		= vimc_sca_remove,
+	.driver		= {
+		.name	= VIMC_SCA_DRV_NAME,
+	},
+};
+
+static const struct platform_device_id vimc_sca_driver_ids[] = {
+	{
+		.name           = VIMC_SCA_DRV_NAME,
+	},
+	{ }
+};
+
+module_platform_driver(vimc_sca_pdrv);
+
+MODULE_DEVICE_TABLE(platform, vimc_sca_driver_ids);
+
+MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Scaler");
+MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/vimc/vimc-sensor.c b/drivers/media/platform/vimc/vimc-sensor.c
index 591f6a4f8bd3..ebdbbe8c05ed 100644
--- a/drivers/media/platform/vimc/vimc-sensor.c
+++ b/drivers/media/platform/vimc/vimc-sensor.c
@@ -15,36 +15,64 @@
  *
  */
 
+#include <linux/component.h>
 #include <linux/freezer.h>
 #include <linux/kthread.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
 #include <linux/v4l2-mediabus.h>
 #include <linux/vmalloc.h>
 #include <media/v4l2-subdev.h>
+#include <media/v4l2-tpg.h>
 
-#include "vimc-sensor.h"
+#include "vimc-common.h"
+
+#define VIMC_SEN_DRV_NAME "vimc-sensor"
 
 struct vimc_sen_device {
 	struct vimc_ent_device ved;
 	struct v4l2_subdev sd;
+	struct device *dev;
+	struct tpg_data tpg;
 	struct task_struct *kthread_sen;
 	u8 *frame;
 	/* The active format */
 	struct v4l2_mbus_framefmt mbus_format;
-	int frame_size;
 };
 
+static const struct v4l2_mbus_framefmt fmt_default = {
+	.width = 640,
+	.height = 480,
+	.code = MEDIA_BUS_FMT_RGB888_1X24,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = V4L2_COLORSPACE_DEFAULT,
+};
+
+static int vimc_sen_init_cfg(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_pad_config *cfg)
+{
+	unsigned int i;
+
+	for (i = 0; i < sd->entity.num_pads; i++) {
+		struct v4l2_mbus_framefmt *mf;
+
+		mf = v4l2_subdev_get_try_format(sd, cfg, i);
+		*mf = fmt_default;
+	}
+
+	return 0;
+}
+
 static int vimc_sen_enum_mbus_code(struct v4l2_subdev *sd,
 				   struct v4l2_subdev_pad_config *cfg,
 				   struct v4l2_subdev_mbus_code_enum *code)
 {
-	struct vimc_sen_device *vsen =
-				container_of(sd, struct vimc_sen_device, sd);
+	const struct vimc_pix_map *vpix = vimc_pix_map_by_index(code->index);
 
-	/* TODO: Add support for other codes */
-	if (code->index)
+	if (!vpix)
 		return -EINVAL;
 
-	code->code = vsen->mbus_format.code;
+	code->code = vpix->code;
 
 	return 0;
 }
@@ -53,51 +81,123 @@ static int vimc_sen_enum_frame_size(struct v4l2_subdev *sd,
 				    struct v4l2_subdev_pad_config *cfg,
 				    struct v4l2_subdev_frame_size_enum *fse)
 {
-	struct vimc_sen_device *vsen =
-				container_of(sd, struct vimc_sen_device, sd);
+	const struct vimc_pix_map *vpix;
 
-	/* TODO: Add support to other formats */
 	if (fse->index)
 		return -EINVAL;
 
-	/* TODO: Add support for other codes */
-	if (fse->code != vsen->mbus_format.code)
+	/* Only accept code in the pix map table */
+	vpix = vimc_pix_map_by_code(fse->code);
+	if (!vpix)
 		return -EINVAL;
 
-	fse->min_width = vsen->mbus_format.width;
-	fse->max_width = vsen->mbus_format.width;
-	fse->min_height = vsen->mbus_format.height;
-	fse->max_height = vsen->mbus_format.height;
+	fse->min_width = VIMC_FRAME_MIN_WIDTH;
+	fse->max_width = VIMC_FRAME_MAX_WIDTH;
+	fse->min_height = VIMC_FRAME_MIN_HEIGHT;
+	fse->max_height = VIMC_FRAME_MAX_HEIGHT;
 
 	return 0;
 }
 
 static int vimc_sen_get_fmt(struct v4l2_subdev *sd,
 			    struct v4l2_subdev_pad_config *cfg,
-			    struct v4l2_subdev_format *format)
+			    struct v4l2_subdev_format *fmt)
 {
 	struct vimc_sen_device *vsen =
 				container_of(sd, struct vimc_sen_device, sd);
 
-	format->format = vsen->mbus_format;
+	fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
+		      *v4l2_subdev_get_try_format(sd, cfg, fmt->pad) :
+		      vsen->mbus_format;
+
+	return 0;
+}
+
+static void vimc_sen_tpg_s_format(struct vimc_sen_device *vsen)
+{
+	const struct vimc_pix_map *vpix =
+				vimc_pix_map_by_code(vsen->mbus_format.code);
+
+	tpg_reset_source(&vsen->tpg, vsen->mbus_format.width,
+			 vsen->mbus_format.height, vsen->mbus_format.field);
+	tpg_s_bytesperline(&vsen->tpg, 0, vsen->mbus_format.width * vpix->bpp);
+	tpg_s_buf_height(&vsen->tpg, vsen->mbus_format.height);
+	tpg_s_fourcc(&vsen->tpg, vpix->pixelformat);
+	/* TODO: add support for V4L2_FIELD_ALTERNATE */
+	tpg_s_field(&vsen->tpg, vsen->mbus_format.field, false);
+	tpg_s_colorspace(&vsen->tpg, vsen->mbus_format.colorspace);
+	tpg_s_ycbcr_enc(&vsen->tpg, vsen->mbus_format.ycbcr_enc);
+	tpg_s_quantization(&vsen->tpg, vsen->mbus_format.quantization);
+	tpg_s_xfer_func(&vsen->tpg, vsen->mbus_format.xfer_func);
+}
+
+static void vimc_sen_adjust_fmt(struct v4l2_mbus_framefmt *fmt)
+{
+	const struct vimc_pix_map *vpix;
+
+	/* Only accept code in the pix map table */
+	vpix = vimc_pix_map_by_code(fmt->code);
+	if (!vpix)
+		fmt->code = fmt_default.code;
+
+	fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
+			     VIMC_FRAME_MAX_WIDTH) & ~1;
+	fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
+			      VIMC_FRAME_MAX_HEIGHT) & ~1;
+
+	/* TODO: add support for V4L2_FIELD_ALTERNATE */
+	if (fmt->field == V4L2_FIELD_ANY || fmt->field == V4L2_FIELD_ALTERNATE)
+		fmt->field = fmt_default.field;
+
+	vimc_colorimetry_clamp(fmt);
+}
+
+static int vimc_sen_set_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_format *fmt)
+{
+	struct vimc_sen_device *vsen = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		/* Do not change the format while stream is on */
+		if (vsen->frame)
+			return -EBUSY;
+
+		mf = &vsen->mbus_format;
+	} else {
+		mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
+	}
+
+	/* Set the new format */
+	vimc_sen_adjust_fmt(&fmt->format);
+
+	dev_dbg(vsen->dev, "%s: format update: "
+		"old:%dx%d (0x%x, %d, %d, %d, %d) "
+		"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsen->sd.name,
+		/* old */
+		mf->width, mf->height, mf->code,
+		mf->colorspace,	mf->quantization,
+		mf->xfer_func, mf->ycbcr_enc,
+		/* new */
+		fmt->format.width, fmt->format.height, fmt->format.code,
+		fmt->format.colorspace, fmt->format.quantization,
+		fmt->format.xfer_func, fmt->format.ycbcr_enc);
+
+	*mf = fmt->format;
 
 	return 0;
 }
 
 static const struct v4l2_subdev_pad_ops vimc_sen_pad_ops = {
+	.init_cfg		= vimc_sen_init_cfg,
 	.enum_mbus_code		= vimc_sen_enum_mbus_code,
 	.enum_frame_size	= vimc_sen_enum_frame_size,
 	.get_fmt		= vimc_sen_get_fmt,
-	/* TODO: Add support to other formats */
-	.set_fmt		= vimc_sen_get_fmt,
+	.set_fmt		= vimc_sen_set_fmt,
 };
 
-/* media operations */
-static const struct media_entity_operations vimc_sen_mops = {
-	.link_validate = v4l2_subdev_link_validate,
-};
-
-static int vimc_thread_sen(void *data)
+static int vimc_sen_tpg_thread(void *data)
 {
 	struct vimc_sen_device *vsen = data;
 	unsigned int i;
@@ -110,7 +210,7 @@ static int vimc_thread_sen(void *data)
 		if (kthread_should_stop())
 			break;
 
-		memset(vsen->frame, 100, vsen->frame_size);
+		tpg_fill_plane_buffer(&vsen->tpg, 0, 0, vsen->frame);
 
 		/* Send the frame to all source pads */
 		for (i = 0; i < vsen->sd.entity.num_pads; i++)
@@ -132,50 +232,57 @@ static int vimc_sen_s_stream(struct v4l2_subdev *sd, int enable)
 
 	if (enable) {
 		const struct vimc_pix_map *vpix;
+		unsigned int frame_size;
 
 		if (vsen->kthread_sen)
-			return -EINVAL;
+			/* tpg is already executing */
+			return 0;
 
 		/* Calculate the frame size */
 		vpix = vimc_pix_map_by_code(vsen->mbus_format.code);
-		vsen->frame_size = vsen->mbus_format.width * vpix->bpp *
-				   vsen->mbus_format.height;
+		frame_size = vsen->mbus_format.width * vpix->bpp *
+			     vsen->mbus_format.height;
 
 		/*
 		 * Allocate the frame buffer. Use vmalloc to be able to
 		 * allocate a large amount of memory
 		 */
-		vsen->frame = vmalloc(vsen->frame_size);
+		vsen->frame = vmalloc(frame_size);
 		if (!vsen->frame)
 			return -ENOMEM;
 
+		/* configure the test pattern generator */
+		vimc_sen_tpg_s_format(vsen);
+
 		/* Initialize the image generator thread */
-		vsen->kthread_sen = kthread_run(vimc_thread_sen, vsen, "%s-sen",
-						vsen->sd.v4l2_dev->name);
+		vsen->kthread_sen = kthread_run(vimc_sen_tpg_thread, vsen,
+					"%s-sen", vsen->sd.v4l2_dev->name);
 		if (IS_ERR(vsen->kthread_sen)) {
-			dev_err(vsen->sd.v4l2_dev->dev,
-				"%s: kernel_thread() failed\n",	vsen->sd.name);
+			dev_err(vsen->dev, "%s: kernel_thread() failed\n",
+				vsen->sd.name);
 			vfree(vsen->frame);
 			vsen->frame = NULL;
 			return PTR_ERR(vsen->kthread_sen);
 		}
 	} else {
 		if (!vsen->kthread_sen)
-			return -EINVAL;
+			return 0;
 
 		/* Stop image generator */
 		ret = kthread_stop(vsen->kthread_sen);
-		vsen->kthread_sen = NULL;
+		if (ret)
+			return ret;
 
+		vsen->kthread_sen = NULL;
 		vfree(vsen->frame);
 		vsen->frame = NULL;
-		return ret;
+		return 0;
 	}
 
 	return 0;
 }
 
-struct v4l2_subdev_video_ops vimc_sen_video_ops = {
+static struct v4l2_subdev_video_ops vimc_sen_video_ops = {
 	.s_stream = vimc_sen_s_stream,
 };
 
@@ -184,93 +291,99 @@ static const struct v4l2_subdev_ops vimc_sen_ops = {
 	.video = &vimc_sen_video_ops,
 };
 
-static void vimc_sen_destroy(struct vimc_ent_device *ved)
+static void vimc_sen_comp_unbind(struct device *comp, struct device *master,
+				 void *master_data)
 {
+	struct vimc_ent_device *ved = dev_get_drvdata(comp);
 	struct vimc_sen_device *vsen =
 				container_of(ved, struct vimc_sen_device, ved);
 
-	v4l2_device_unregister_subdev(&vsen->sd);
-	media_entity_cleanup(ved->ent);
+	vimc_ent_sd_unregister(ved, &vsen->sd);
+	tpg_free(&vsen->tpg);
 	kfree(vsen);
 }
 
-struct vimc_ent_device *vimc_sen_create(struct v4l2_device *v4l2_dev,
-					const char *const name,
-					u16 num_pads,
-					const unsigned long *pads_flag)
+static int vimc_sen_comp_bind(struct device *comp, struct device *master,
+			      void *master_data)
 {
+	struct v4l2_device *v4l2_dev = master_data;
+	struct vimc_platform_data *pdata = comp->platform_data;
 	struct vimc_sen_device *vsen;
-	unsigned int i;
 	int ret;
 
-	/* NOTE: a sensor node may be created with more then one pad */
-	if (!name || !num_pads || !pads_flag)
-		return ERR_PTR(-EINVAL);
-
-	/* check if all pads are sources */
-	for (i = 0; i < num_pads; i++)
-		if (!(pads_flag[i] & MEDIA_PAD_FL_SOURCE))
-			return ERR_PTR(-EINVAL);
-
 	/* Allocate the vsen struct */
 	vsen = kzalloc(sizeof(*vsen), GFP_KERNEL);
 	if (!vsen)
-		return ERR_PTR(-ENOMEM);
-
-	/* Allocate the pads */
-	vsen->ved.pads = vimc_pads_init(num_pads, pads_flag);
-	if (IS_ERR(vsen->ved.pads)) {
-		ret = PTR_ERR(vsen->ved.pads);
+		return -ENOMEM;
+
+	/* Initialize ved and sd */
+	ret = vimc_ent_sd_register(&vsen->ved, &vsen->sd, v4l2_dev,
+				   pdata->entity_name,
+				   MEDIA_ENT_F_ATV_DECODER, 1,
+				   (const unsigned long[1]) {MEDIA_PAD_FL_SOURCE},
+				   &vimc_sen_ops);
+	if (ret)
 		goto err_free_vsen;
-	}
-
-	/* Fill the vimc_ent_device struct */
-	vsen->ved.destroy = vimc_sen_destroy;
-	vsen->ved.ent = &vsen->sd.entity;
 
-	/* Initialize the subdev */
-	v4l2_subdev_init(&vsen->sd, &vimc_sen_ops);
-	vsen->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
-	vsen->sd.entity.ops = &vimc_sen_mops;
-	vsen->sd.owner = THIS_MODULE;
-	strlcpy(vsen->sd.name, name, sizeof(vsen->sd.name));
-	v4l2_set_subdevdata(&vsen->sd, &vsen->ved);
+	dev_set_drvdata(comp, &vsen->ved);
+	vsen->dev = comp;
 
-	/* Expose this subdev to user space */
-	vsen->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	/* Initialize the frame format */
+	vsen->mbus_format = fmt_default;
 
-	/* Initialize the media entity */
-	ret = media_entity_pads_init(&vsen->sd.entity,
-				     num_pads, vsen->ved.pads);
+	/* Initialize the test pattern generator */
+	tpg_init(&vsen->tpg, vsen->mbus_format.width,
+		 vsen->mbus_format.height);
+	ret = tpg_alloc(&vsen->tpg, VIMC_FRAME_MAX_WIDTH);
 	if (ret)
-		goto err_clean_pads;
-
-	/* Set the active frame format (this is hardcoded for now) */
-	vsen->mbus_format.width = 640;
-	vsen->mbus_format.height = 480;
-	vsen->mbus_format.code = MEDIA_BUS_FMT_RGB888_1X24;
-	vsen->mbus_format.field = V4L2_FIELD_NONE;
-	vsen->mbus_format.colorspace = V4L2_COLORSPACE_SRGB;
-	vsen->mbus_format.quantization = V4L2_QUANTIZATION_FULL_RANGE;
-	vsen->mbus_format.xfer_func = V4L2_XFER_FUNC_SRGB;
-
-	/* Register the subdev with the v4l2 and the media framework */
-	ret = v4l2_device_register_subdev(v4l2_dev, &vsen->sd);
-	if (ret) {
-		dev_err(vsen->sd.v4l2_dev->dev,
-			"%s: subdev register failed (err=%d)\n",
-			vsen->sd.name, ret);
-		goto err_clean_m_ent;
-	}
+		goto err_unregister_ent_sd;
 
-	return &vsen->ved;
+	return 0;
 
-err_clean_m_ent:
-	media_entity_cleanup(&vsen->sd.entity);
-err_clean_pads:
-	vimc_pads_cleanup(vsen->ved.pads);
+err_unregister_ent_sd:
+	vimc_ent_sd_unregister(&vsen->ved,  &vsen->sd);
 err_free_vsen:
 	kfree(vsen);
 
-	return ERR_PTR(ret);
+	return ret;
 }
+
+static const struct component_ops vimc_sen_comp_ops = {
+	.bind = vimc_sen_comp_bind,
+	.unbind = vimc_sen_comp_unbind,
+};
+
+static int vimc_sen_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &vimc_sen_comp_ops);
+}
+
+static int vimc_sen_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &vimc_sen_comp_ops);
+
+	return 0;
+}
+
+static struct platform_driver vimc_sen_pdrv = {
+	.probe		= vimc_sen_probe,
+	.remove		= vimc_sen_remove,
+	.driver		= {
+		.name	= VIMC_SEN_DRV_NAME,
+	},
+};
+
+static const struct platform_device_id vimc_sen_driver_ids[] = {
+	{
+		.name           = VIMC_SEN_DRV_NAME,
+	},
+	{ }
+};
+
+module_platform_driver(vimc_sen_pdrv);
+
+MODULE_DEVICE_TABLE(platform, vimc_sen_driver_ids);
+
+MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Sensor");
+MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/vimc/vimc-sensor.h b/drivers/media/platform/vimc/vimc-sensor.h
deleted file mode 100644
index 505310e8aeb7..000000000000
--- a/drivers/media/platform/vimc/vimc-sensor.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
- * vimc-sensor.h Virtual Media Controller Driver
- *
- * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- */
-
-#ifndef _VIMC_SENSOR_H_
-#define _VIMC_SENSOR_H_
-
-#include "vimc-core.h"
-
-struct vimc_ent_device *vimc_sen_create(struct v4l2_device *v4l2_dev,
-					const char *const name,
-					u16 num_pads,
-					const unsigned long *pads_flag);
-
-#endif
diff --git a/drivers/media/platform/vivid/vivid-cec.c b/drivers/media/platform/vivid/vivid-cec.c
index 653f4099f737..e15705758969 100644
--- a/drivers/media/platform/vivid/vivid-cec.c
+++ b/drivers/media/platform/vivid/vivid-cec.c
@@ -34,7 +34,7 @@ void vivid_cec_bus_free_work(struct vivid_dev *dev)
 		cancel_delayed_work_sync(&cw->work);
 		spin_lock(&dev->cec_slock);
 		list_del(&cw->list);
-		cec_transmit_done(cw->adap, CEC_TX_STATUS_LOW_DRIVE, 0, 0, 1, 0);
+		cec_transmit_attempt_done(cw->adap, CEC_TX_STATUS_LOW_DRIVE);
 		kfree(cw);
 	}
 	spin_unlock(&dev->cec_slock);
@@ -84,7 +84,7 @@ static void vivid_cec_xfer_done_worker(struct work_struct *work)
 	dev->cec_xfer_start_jiffies = 0;
 	list_del(&cw->list);
 	spin_unlock(&dev->cec_slock);
-	cec_transmit_done(cw->adap, cw->tx_status, 0, valid_dest ? 0 : 1, 0, 0);
+	cec_transmit_attempt_done(cw->adap, cw->tx_status);
 
 	/* Broadcast message */
 	if (adap != dev->cec_rx_adap)
@@ -105,7 +105,7 @@ static void vivid_cec_xfer_try_worker(struct work_struct *work)
 	if (dev->cec_xfer_time_jiffies) {
 		list_del(&cw->list);
 		spin_unlock(&dev->cec_slock);
-		cec_transmit_done(cw->adap, CEC_TX_STATUS_ARB_LOST, 1, 0, 0, 0);
+		cec_transmit_attempt_done(cw->adap, CEC_TX_STATUS_ARB_LOST);
 		kfree(cw);
 	} else {
 		INIT_DELAYED_WORK(&cw->work, vivid_cec_xfer_done_worker);
diff --git a/drivers/media/platform/xilinx/Kconfig b/drivers/media/platform/xilinx/Kconfig
index 84bae795b70d..a5d21b7c6e0b 100644
--- a/drivers/media/platform/xilinx/Kconfig
+++ b/drivers/media/platform/xilinx/Kconfig
@@ -2,6 +2,7 @@ config VIDEO_XILINX
 	tristate "Xilinx Video IP (EXPERIMENTAL)"
 	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && OF && HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_FWNODE
 	---help---
 	  Driver for Xilinx Video IP Pipelines
 
diff --git a/drivers/media/platform/xilinx/xilinx-vipp.c b/drivers/media/platform/xilinx/xilinx-vipp.c
index feb3b2f1d874..ac4704388920 100644
--- a/drivers/media/platform/xilinx/xilinx-vipp.c
+++ b/drivers/media/platform/xilinx/xilinx-vipp.c
@@ -22,7 +22,7 @@
 #include <media/v4l2-async.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
-#include <media/v4l2-of.h>
+#include <media/v4l2-fwnode.h>
 
 #include "xilinx-dma.h"
 #include "xilinx-vipp.h"
@@ -74,7 +74,7 @@ static int xvip_graph_build_one(struct xvip_composite_device *xdev,
 	struct media_pad *local_pad;
 	struct media_pad *remote_pad;
 	struct xvip_graph_entity *ent;
-	struct v4l2_of_link link;
+	struct v4l2_fwnode_link link;
 	struct device_node *ep = NULL;
 	struct device_node *next;
 	int ret = 0;
@@ -92,7 +92,7 @@ static int xvip_graph_build_one(struct xvip_composite_device *xdev,
 
 		dev_dbg(xdev->dev, "processing endpoint %s\n", ep->full_name);
 
-		ret = v4l2_of_parse_link(ep, &link);
+		ret = v4l2_fwnode_parse_link(of_fwnode_handle(ep), &link);
 		if (ret < 0) {
 			dev_err(xdev->dev, "failed to parse link for %s\n",
 				ep->full_name);
@@ -103,9 +103,10 @@ static int xvip_graph_build_one(struct xvip_composite_device *xdev,
 		 * the link.
 		 */
 		if (link.local_port >= local->num_pads) {
-			dev_err(xdev->dev, "invalid port number %u on %s\n",
-				link.local_port, link.local_node->full_name);
-			v4l2_of_put_link(&link);
+			dev_err(xdev->dev, "invalid port number %u for %s\n",
+				link.local_port,
+				to_of_node(link.local_node)->full_name);
+			v4l2_fwnode_put_link(&link);
 			ret = -EINVAL;
 			break;
 		}
@@ -114,25 +115,28 @@ static int xvip_graph_build_one(struct xvip_composite_device *xdev,
 
 		if (local_pad->flags & MEDIA_PAD_FL_SINK) {
 			dev_dbg(xdev->dev, "skipping sink port %s:%u\n",
-				link.local_node->full_name, link.local_port);
-			v4l2_of_put_link(&link);
+				to_of_node(link.local_node)->full_name,
+				link.local_port);
+			v4l2_fwnode_put_link(&link);
 			continue;
 		}
 
 		/* Skip DMA engines, they will be processed separately. */
-		if (link.remote_node == xdev->dev->of_node) {
+		if (link.remote_node == of_fwnode_handle(xdev->dev->of_node)) {
 			dev_dbg(xdev->dev, "skipping DMA port %s:%u\n",
-				link.local_node->full_name, link.local_port);
-			v4l2_of_put_link(&link);
+				to_of_node(link.local_node)->full_name,
+				link.local_port);
+			v4l2_fwnode_put_link(&link);
 			continue;
 		}
 
 		/* Find the remote entity. */
-		ent = xvip_graph_find_entity(xdev, link.remote_node);
+		ent = xvip_graph_find_entity(xdev,
+					     to_of_node(link.remote_node));
 		if (ent == NULL) {
 			dev_err(xdev->dev, "no entity found for %s\n",
-				link.remote_node->full_name);
-			v4l2_of_put_link(&link);
+				to_of_node(link.remote_node)->full_name);
+			v4l2_fwnode_put_link(&link);
 			ret = -ENODEV;
 			break;
 		}
@@ -141,15 +145,16 @@ static int xvip_graph_build_one(struct xvip_composite_device *xdev,
 
 		if (link.remote_port >= remote->num_pads) {
 			dev_err(xdev->dev, "invalid port number %u on %s\n",
-				link.remote_port, link.remote_node->full_name);
-			v4l2_of_put_link(&link);
+				link.remote_port,
+				to_of_node(link.remote_node)->full_name);
+			v4l2_fwnode_put_link(&link);
 			ret = -EINVAL;
 			break;
 		}
 
 		remote_pad = &remote->pads[link.remote_port];
 
-		v4l2_of_put_link(&link);
+		v4l2_fwnode_put_link(&link);
 
 		/* Create the media link. */
 		dev_dbg(xdev->dev, "creating %s:%u -> %s:%u link\n",
@@ -194,7 +199,7 @@ static int xvip_graph_build_dma(struct xvip_composite_device *xdev)
 	struct media_pad *source_pad;
 	struct media_pad *sink_pad;
 	struct xvip_graph_entity *ent;
-	struct v4l2_of_link link;
+	struct v4l2_fwnode_link link;
 	struct device_node *ep = NULL;
 	struct device_node *next;
 	struct xvip_dma *dma;
@@ -213,7 +218,7 @@ static int xvip_graph_build_dma(struct xvip_composite_device *xdev)
 
 		dev_dbg(xdev->dev, "processing endpoint %s\n", ep->full_name);
 
-		ret = v4l2_of_parse_link(ep, &link);
+		ret = v4l2_fwnode_parse_link(of_fwnode_handle(ep), &link);
 		if (ret < 0) {
 			dev_err(xdev->dev, "failed to parse link for %s\n",
 				ep->full_name);
@@ -225,7 +230,7 @@ static int xvip_graph_build_dma(struct xvip_composite_device *xdev)
 		if (dma == NULL) {
 			dev_err(xdev->dev, "no DMA engine found for port %u\n",
 				link.local_port);
-			v4l2_of_put_link(&link);
+			v4l2_fwnode_put_link(&link);
 			ret = -EINVAL;
 			break;
 		}
@@ -234,19 +239,21 @@ static int xvip_graph_build_dma(struct xvip_composite_device *xdev)
 			dma->video.name);
 
 		/* Find the remote entity. */
-		ent = xvip_graph_find_entity(xdev, link.remote_node);
+		ent = xvip_graph_find_entity(xdev,
+					     to_of_node(link.remote_node));
 		if (ent == NULL) {
 			dev_err(xdev->dev, "no entity found for %s\n",
-				link.remote_node->full_name);
-			v4l2_of_put_link(&link);
+				to_of_node(link.remote_node)->full_name);
+			v4l2_fwnode_put_link(&link);
 			ret = -ENODEV;
 			break;
 		}
 
 		if (link.remote_port >= ent->entity->num_pads) {
 			dev_err(xdev->dev, "invalid port number %u on %s\n",
-				link.remote_port, link.remote_node->full_name);
-			v4l2_of_put_link(&link);
+				link.remote_port,
+				to_of_node(link.remote_node)->full_name);
+			v4l2_fwnode_put_link(&link);
 			ret = -EINVAL;
 			break;
 		}
@@ -263,7 +270,7 @@ static int xvip_graph_build_dma(struct xvip_composite_device *xdev)
 			sink_pad = &dma->pad;
 		}
 
-		v4l2_of_put_link(&link);
+		v4l2_fwnode_put_link(&link);
 
 		/* Create the media link. */
 		dev_dbg(xdev->dev, "creating %s:%u -> %s:%u link\n",
@@ -383,8 +390,8 @@ static int xvip_graph_parse_one(struct xvip_composite_device *xdev,
 		}
 
 		entity->node = remote;
-		entity->asd.match_type = V4L2_ASYNC_MATCH_OF;
-		entity->asd.match.of.node = remote;
+		entity->asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
+		entity->asd.match.fwnode.fwnode = of_fwnode_handle(remote);
 		list_add_tail(&entity->list, &xdev->entities);
 		xdev->num_subdevs++;
 	}
diff --git a/drivers/media/radio/wl128x/fmdrv_common.c b/drivers/media/radio/wl128x/fmdrv_common.c
index 588e2d61c3b4..ab3428bf63fe 100644
--- a/drivers/media/radio/wl128x/fmdrv_common.c
+++ b/drivers/media/radio/wl128x/fmdrv_common.c
@@ -416,7 +416,7 @@ static int fm_send_cmd(struct fmdev *fmdev, u8 fm_op, u16 type,	void *payload,
 	if (!test_bit(FM_FW_DW_INPROGRESS, &fmdev->flag) ||
 			test_bit(FM_INTTASK_RUNNING, &fmdev->flag)) {
 		/* Fill command header info */
-		hdr = (struct fm_cmd_msg_hdr *)skb_put(skb, FM_CMD_MSG_HDR_SIZE);
+		hdr = skb_put(skb, FM_CMD_MSG_HDR_SIZE);
 		hdr->hdr = FM_PKT_LOGICAL_CHAN_NUMBER;	/* 0x08 */
 
 		/* 3 (fm_opcode,rd_wr,dlen) + payload len) */
@@ -442,7 +442,7 @@ static int fm_send_cmd(struct fmdev *fmdev, u8 fm_op, u16 type,	void *payload,
 		fm_cb(skb)->fm_op = *((u8 *)payload + 2);
 	}
 	if (payload != NULL)
-		memcpy(skb_put(skb, payload_len), payload, payload_len);
+		skb_put_data(skb, payload, payload_len);
 
 	fm_cb(skb)->completion = wait_completion;
 	skb_queue_tail(&fmdev->tx_q, skb);
diff --git a/drivers/media/rc/Kconfig b/drivers/media/rc/Kconfig
index e422f3d56f76..5e83b76495f7 100644
--- a/drivers/media/rc/Kconfig
+++ b/drivers/media/rc/Kconfig
@@ -169,11 +169,11 @@ config IR_HIX5HD2
 	tristate "Hisilicon hix5hd2 IR remote control"
 	depends on RC_CORE
 	help
-	 Say Y here if you want to use hisilicon hix5hd2 remote control.
-	 To compile this driver as a module, choose M here: the module will be
-	 called ir-hix5hd2.
+	   Say Y here if you want to use hisilicon hix5hd2 remote control.
+	   To compile this driver as a module, choose M here: the module will be
+	   called ir-hix5hd2.
 
-	 If you're not sure, select N here
+	   If you're not sure, select N here
 
 config IR_IMON
 	tristate "SoundGraph iMON Receiver and Display"
diff --git a/drivers/media/rc/ati_remote.c b/drivers/media/rc/ati_remote.c
index 9cf3e69de16a..a4c6ad4f67c1 100644
--- a/drivers/media/rc/ati_remote.c
+++ b/drivers/media/rc/ati_remote.c
@@ -904,9 +904,6 @@ static int ati_remote_probe(struct usb_interface *interface,
 	if (err)
 		goto exit_kill_urbs;
 
-	/* use our delay for rc_dev */
-	ati_remote->rdev->input_dev->rep[REP_DELAY] = repeat_delay;
-
 	/* Set up and register mouse input device */
 	if (mouse) {
 		input_dev = input_allocate_device();
diff --git a/drivers/media/rc/iguanair.c b/drivers/media/rc/iguanair.c
index ccf24fd7ec1b..8711a7ff55cc 100644
--- a/drivers/media/rc/iguanair.c
+++ b/drivers/media/rc/iguanair.c
@@ -113,6 +113,7 @@ static void process_ir_data(struct iguanair *ir, unsigned len)
 			break;
 		case CMD_TX_OVERFLOW:
 			ir->tx_overflow = true;
+			/* fall through */
 		case CMD_RECEIVER_OFF:
 		case CMD_RECEIVER_ON:
 		case CMD_SEND:
diff --git a/drivers/media/rc/img-ir/img-ir-hw.c b/drivers/media/rc/img-ir/img-ir-hw.c
index 431d33b36fb0..8d1439622533 100644
--- a/drivers/media/rc/img-ir/img-ir-hw.c
+++ b/drivers/media/rc/img-ir/img-ir-hw.c
@@ -702,10 +702,6 @@ static void img_ir_set_protocol(struct img_ir_priv *priv, u64 proto)
 {
 	struct rc_dev *rdev = priv->hw.rdev;
 
-	spin_lock_irq(&rdev->rc_map.lock);
-	rdev->rc_map.rc_type = __ffs64(proto);
-	spin_unlock_irq(&rdev->rc_map.lock);
-
 	mutex_lock(&rdev->lock);
 	rdev->enabled_protocols = proto;
 	rdev->allowed_wakeup_protocols = proto;
diff --git a/drivers/media/rc/imon.c b/drivers/media/rc/imon.c
index 3489010601b5..bd76534a2749 100644
--- a/drivers/media/rc/imon.c
+++ b/drivers/media/rc/imon.c
@@ -1722,7 +1722,7 @@ static void imon_incoming_scancode(struct imon_context *ictx,
 	if (kc == KEY_KEYBOARD && !ictx->release_code) {
 		ictx->last_keycode = kc;
 		if (!nomouse) {
-			ictx->pad_mouse = ~(ictx->pad_mouse) & 0x1;
+			ictx->pad_mouse = !ictx->pad_mouse;
 			dev_dbg(dev, "toggling to %s mode\n",
 				ictx->pad_mouse ? "mouse" : "keyboard");
 			spin_unlock_irqrestore(&ictx->kc_lock, flags);
diff --git a/drivers/media/rc/ir-lirc-codec.c b/drivers/media/rc/ir-lirc-codec.c
index de85f1d7ce43..a30af91710fe 100644
--- a/drivers/media/rc/ir-lirc-codec.c
+++ b/drivers/media/rc/ir-lirc-codec.c
@@ -327,16 +327,6 @@ static long ir_lirc_ioctl(struct file *filep, unsigned int cmd,
 	return ret;
 }
 
-static int ir_lirc_open(void *data)
-{
-	return 0;
-}
-
-static void ir_lirc_close(void *data)
-{
-	return;
-}
-
 static const struct file_operations lirc_fops = {
 	.owner		= THIS_MODULE,
 	.write		= ir_lirc_transmit_ir,
@@ -354,7 +344,6 @@ static const struct file_operations lirc_fops = {
 static int ir_lirc_register(struct rc_dev *dev)
 {
 	struct lirc_driver *drv;
-	struct lirc_buffer *rbuf;
 	int rc = -ENOMEM;
 	unsigned long features = 0;
 
@@ -362,19 +351,12 @@ static int ir_lirc_register(struct rc_dev *dev)
 	if (!drv)
 		return rc;
 
-	rbuf = kzalloc(sizeof(struct lirc_buffer), GFP_KERNEL);
-	if (!rbuf)
-		goto rbuf_alloc_failed;
-
-	rc = lirc_buffer_init(rbuf, sizeof(int), LIRCBUF_SIZE);
-	if (rc)
-		goto rbuf_init_failed;
-
 	if (dev->driver_type != RC_DRIVER_IR_RAW_TX) {
 		features |= LIRC_CAN_REC_MODE2;
 		if (dev->rx_resolution)
 			features |= LIRC_CAN_GET_REC_RESOLUTION;
 	}
+
 	if (dev->tx_ir) {
 		features |= LIRC_CAN_SEND_PULSE;
 		if (dev->s_tx_mask)
@@ -403,10 +385,10 @@ static int ir_lirc_register(struct rc_dev *dev)
 	drv->minor = -1;
 	drv->features = features;
 	drv->data = &dev->raw->lirc;
-	drv->rbuf = rbuf;
-	drv->set_use_inc = &ir_lirc_open;
-	drv->set_use_dec = &ir_lirc_close;
+	drv->rbuf = NULL;
 	drv->code_length = sizeof(struct ir_raw_event) * 8;
+	drv->chunk_size = sizeof(int);
+	drv->buffer_size = LIRCBUF_SIZE;
 	drv->fops = &lirc_fops;
 	drv->dev = &dev->dev;
 	drv->rdev = dev;
@@ -415,19 +397,15 @@ static int ir_lirc_register(struct rc_dev *dev)
 	drv->minor = lirc_register_driver(drv);
 	if (drv->minor < 0) {
 		rc = -ENODEV;
-		goto lirc_register_failed;
+		goto out;
 	}
 
 	dev->raw->lirc.drv = drv;
 	dev->raw->lirc.dev = dev;
 	return 0;
 
-lirc_register_failed:
-rbuf_init_failed:
-	kfree(rbuf);
-rbuf_alloc_failed:
+out:
 	kfree(drv);
-
 	return rc;
 }
 
@@ -436,9 +414,8 @@ static int ir_lirc_unregister(struct rc_dev *dev)
 	struct lirc_codec *lirc = &dev->raw->lirc;
 
 	lirc_unregister_driver(lirc->drv->minor);
-	lirc_buffer_free(lirc->drv->rbuf);
-	kfree(lirc->drv->rbuf);
 	kfree(lirc->drv);
+	lirc->drv = NULL;
 
 	return 0;
 }
diff --git a/drivers/media/rc/ir-spi.c b/drivers/media/rc/ir-spi.c
index c8863f36686a..7e383b3fedd5 100644
--- a/drivers/media/rc/ir-spi.c
+++ b/drivers/media/rc/ir-spi.c
@@ -57,10 +57,13 @@ static int ir_spi_tx(struct rc_dev *dev,
 
 	/* convert the pulse/space signal to raw binary signal */
 	for (i = 0; i < count; i++) {
+		unsigned int periods;
 		int j;
-		u16 val = ((i + 1) % 2) ? idata->pulse : idata->space;
+		u16 val;
 
-		if (len + buffer[i] >= IR_SPI_MAX_BUFSIZE)
+		periods = DIV_ROUND_CLOSEST(buffer[i] * idata->freq, 1000000);
+
+		if (len + periods >= IR_SPI_MAX_BUFSIZE)
 			return -EINVAL;
 
 		/*
@@ -69,13 +72,13 @@ static int ir_spi_tx(struct rc_dev *dev,
 		 * contain a space duration.
 		 */
 		val = (i % 2) ? idata->space : idata->pulse;
-		for (j = 0; j < buffer[i]; j++)
+		for (j = 0; j < periods; j++)
 			idata->tx_buf[len++] = val;
 	}
 
 	memset(&xfer, 0, sizeof(xfer));
 
-	xfer.speed_hz = idata->freq;
+	xfer.speed_hz = idata->freq * 16;
 	xfer.len = len * sizeof(*idata->tx_buf);
 	xfer.tx_buf = idata->tx_buf;
 
diff --git a/drivers/media/rc/lirc_dev.c b/drivers/media/rc/lirc_dev.c
index 8d60c9f00df9..db1e7b70c998 100644
--- a/drivers/media/rc/lirc_dev.c
+++ b/drivers/media/rc/lirc_dev.c
@@ -18,18 +18,10 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/module.h>
-#include <linux/kernel.h>
 #include <linux/sched/signal.h>
-#include <linux/errno.h>
 #include <linux/ioctl.h>
-#include <linux/fs.h>
 #include <linux/poll.h>
-#include <linux/completion.h>
 #include <linux/mutex.h>
-#include <linux/wait.h>
-#include <linux/unistd.h>
-#include <linux/kthread.h>
-#include <linux/bitops.h>
 #include <linux/device.h>
 #include <linux/cdev.h>
 
@@ -37,9 +29,6 @@
 #include <media/lirc.h>
 #include <media/lirc_dev.h>
 
-static bool debug;
-
-#define IRCTL_DEV_NAME	"BaseRemoteCtl"
 #define NOPLUG		-1
 #define LOGHEAD		"lirc_dev (%s[%d]): "
 
@@ -52,13 +41,11 @@ struct irctl {
 
 	struct mutex irctl_lock;
 	struct lirc_buffer *buf;
+	bool buf_internal;
 	unsigned int chunk_size;
 
 	struct device dev;
 	struct cdev cdev;
-
-	struct task_struct *task;
-	long jiffies_to_wait;
 };
 
 static DEFINE_MUTEX(lirc_dev_lock);
@@ -68,22 +55,11 @@ static struct irctl *irctls[MAX_IRCTL_DEVICES];
 /* Only used for sysfs but defined to void otherwise */
 static struct class *lirc_class;
 
-/*  helper function
- *  initializes the irctl structure
- */
-static void lirc_irctl_init(struct irctl *ir)
-{
-	mutex_init(&ir->irctl_lock);
-	ir->d.minor = NOPLUG;
-}
-
 static void lirc_release(struct device *ld)
 {
 	struct irctl *ir = container_of(ld, struct irctl, dev);
 
-	put_device(ir->dev.parent);
-
-	if (ir->buf != ir->d.rbuf) {
+	if (ir->buf_internal) {
 		lirc_buffer_free(ir->buf);
 		kfree(ir->buf);
 	}
@@ -94,93 +70,6 @@ static void lirc_release(struct device *ld)
 	kfree(ir);
 }
 
-/*  helper function
- *  reads key codes from driver and puts them into buffer
- *  returns 0 on success
- */
-static int lirc_add_to_buf(struct irctl *ir)
-{
-	int res;
-	int got_data = -1;
-
-	if (!ir->d.add_to_buf)
-		return 0;
-
-	/*
-	 * service the device as long as it is returning
-	 * data and we have space
-	 */
-	do {
-		got_data++;
-		res = ir->d.add_to_buf(ir->d.data, ir->buf);
-	} while (!res);
-
-	if (res == -ENODEV)
-		kthread_stop(ir->task);
-
-	return got_data ? 0 : res;
-}
-
-/* main function of the polling thread
- */
-static int lirc_thread(void *irctl)
-{
-	struct irctl *ir = irctl;
-
-	do {
-		if (ir->open) {
-			if (ir->jiffies_to_wait) {
-				set_current_state(TASK_INTERRUPTIBLE);
-				schedule_timeout(ir->jiffies_to_wait);
-			}
-			if (kthread_should_stop())
-				break;
-			if (!lirc_add_to_buf(ir))
-				wake_up_interruptible(&ir->buf->wait_poll);
-		} else {
-			set_current_state(TASK_INTERRUPTIBLE);
-			schedule();
-		}
-	} while (!kthread_should_stop());
-
-	return 0;
-}
-
-
-static const struct file_operations lirc_dev_fops = {
-	.owner		= THIS_MODULE,
-	.read		= lirc_dev_fop_read,
-	.write		= lirc_dev_fop_write,
-	.poll		= lirc_dev_fop_poll,
-	.unlocked_ioctl	= lirc_dev_fop_ioctl,
-	.open		= lirc_dev_fop_open,
-	.release	= lirc_dev_fop_close,
-	.llseek		= noop_llseek,
-};
-
-static int lirc_cdev_add(struct irctl *ir)
-{
-	struct lirc_driver *d = &ir->d;
-	struct cdev *cdev;
-	int retval;
-
-	cdev = &ir->cdev;
-
-	if (d->fops) {
-		cdev_init(cdev, d->fops);
-		cdev->owner = d->owner;
-	} else {
-		cdev_init(cdev, &lirc_dev_fops);
-		cdev->owner = THIS_MODULE;
-	}
-	retval = kobject_set_name(&cdev->kobj, "lirc%d", d->minor);
-	if (retval)
-		return retval;
-
-	cdev->kobj.parent = &ir->dev.kobj;
-	return cdev_add(cdev, ir->dev.devt, 1);
-}
-
 static int lirc_allocate_buffer(struct irctl *ir)
 {
 	int err = 0;
@@ -189,8 +78,6 @@ static int lirc_allocate_buffer(struct irctl *ir)
 	unsigned int buffer_size;
 	struct lirc_driver *d = &ir->d;
 
-	mutex_lock(&lirc_dev_lock);
-
 	bytes_in_key = BITS_TO_LONGS(d->code_length) +
 						(d->code_length % 8 ? 1 : 0);
 	buffer_size = d->buffer_size ? d->buffer_size : BUFLEN / bytes_in_key;
@@ -198,6 +85,7 @@ static int lirc_allocate_buffer(struct irctl *ir)
 
 	if (d->rbuf) {
 		ir->buf = d->rbuf;
+		ir->buf_internal = false;
 	} else {
 		ir->buf = kmalloc(sizeof(struct lirc_buffer), GFP_KERNEL);
 		if (!ir->buf) {
@@ -208,18 +96,20 @@ static int lirc_allocate_buffer(struct irctl *ir)
 		err = lirc_buffer_init(ir->buf, chunk_size, buffer_size);
 		if (err) {
 			kfree(ir->buf);
+			ir->buf = NULL;
 			goto out;
 		}
+
+		ir->buf_internal = true;
+		d->rbuf = ir->buf;
 	}
 	ir->chunk_size = ir->buf->chunk_size;
 
 out:
-	mutex_unlock(&lirc_dev_lock);
-
 	return err;
 }
 
-static int lirc_allocate_driver(struct lirc_driver *d)
+int lirc_register_driver(struct lirc_driver *d)
 {
 	struct irctl *ir;
 	int minor;
@@ -235,6 +125,11 @@ static int lirc_allocate_driver(struct lirc_driver *d)
 		return -EINVAL;
 	}
 
+	if (!d->fops) {
+		pr_err("fops pointer not filled in!\n");
+		return -EINVAL;
+	}
+
 	if (d->minor >= MAX_IRCTL_DEVICES) {
 		dev_err(d->dev, "minor must be between 0 and %d!\n",
 						MAX_IRCTL_DEVICES - 1);
@@ -247,18 +142,8 @@ static int lirc_allocate_driver(struct lirc_driver *d)
 		return -EBADRQC;
 	}
 
-	if (d->sample_rate) {
-		if (2 > d->sample_rate || HZ < d->sample_rate) {
-			dev_err(d->dev, "invalid %d sample rate\n",
-							d->sample_rate);
-			return -EBADRQC;
-		}
-		if (!d->add_to_buf) {
-			dev_err(d->dev, "add_to_buf not set\n");
-			return -EBADRQC;
-		}
-	} else if (!d->rbuf && !(d->fops && d->fops->read &&
-				d->fops->poll && d->fops->unlocked_ioctl)) {
+	if (!d->rbuf && !(d->fops && d->fops->read &&
+			  d->fops->poll && d->fops->unlocked_ioctl)) {
 		dev_err(d->dev, "undefined read, poll, ioctl\n");
 		return -EBADRQC;
 	}
@@ -288,7 +173,8 @@ static int lirc_allocate_driver(struct lirc_driver *d)
 		err = -ENOMEM;
 		goto out_lock;
 	}
-	lirc_irctl_init(ir);
+
+	mutex_init(&ir->irctl_lock);
 	irctls[minor] = ir;
 	d->minor = minor;
 
@@ -300,32 +186,29 @@ static int lirc_allocate_driver(struct lirc_driver *d)
 
 	ir->d = *d;
 
+	if (LIRC_CAN_REC(d->features)) {
+		err = lirc_allocate_buffer(irctls[minor]);
+		if (err) {
+			kfree(ir);
+			goto out_lock;
+		}
+		d->rbuf = ir->buf;
+	}
+
+	device_initialize(&ir->dev);
 	ir->dev.devt = MKDEV(MAJOR(lirc_base_dev), ir->d.minor);
 	ir->dev.class = lirc_class;
 	ir->dev.parent = d->dev;
 	ir->dev.release = lirc_release;
 	dev_set_name(&ir->dev, "lirc%d", ir->d.minor);
-	device_initialize(&ir->dev);
 
-	if (d->sample_rate) {
-		ir->jiffies_to_wait = HZ / d->sample_rate;
+	cdev_init(&ir->cdev, d->fops);
+	ir->cdev.owner = ir->d.owner;
+	ir->cdev.kobj.parent = &ir->dev.kobj;
 
-		/* try to fire up polling thread */
-		ir->task = kthread_run(lirc_thread, (void *)ir, "lirc_dev");
-		if (IS_ERR(ir->task)) {
-			dev_err(d->dev, "cannot run thread for minor = %d\n",
-								d->minor);
-			err = -ECHILD;
-			goto out_sysfs;
-		}
-	} else {
-		/* it means - wait for external event in task queue */
-		ir->jiffies_to_wait = 0;
-	}
-
-	err = lirc_cdev_add(ir);
+	err = cdev_add(&ir->cdev, ir->dev.devt, 1);
 	if (err)
-		goto out_sysfs;
+		goto out_free_dev;
 
 	ir->attached = 1;
 
@@ -335,37 +218,20 @@ static int lirc_allocate_driver(struct lirc_driver *d)
 
 	mutex_unlock(&lirc_dev_lock);
 
-	get_device(ir->dev.parent);
-
 	dev_info(ir->d.dev, "lirc_dev: driver %s registered at minor = %d\n",
 		 ir->d.name, ir->d.minor);
+
 	return minor;
+
 out_cdev:
 	cdev_del(&ir->cdev);
-out_sysfs:
+out_free_dev:
 	put_device(&ir->dev);
 out_lock:
 	mutex_unlock(&lirc_dev_lock);
 
 	return err;
 }
-
-int lirc_register_driver(struct lirc_driver *d)
-{
-	int minor, err = 0;
-
-	minor = lirc_allocate_driver(d);
-	if (minor < 0)
-		return minor;
-
-	if (LIRC_CAN_REC(d->features)) {
-		err = lirc_allocate_buffer(irctls[minor]);
-		if (err)
-			lirc_unregister_driver(minor);
-	}
-
-	return err ? err : minor;
-}
 EXPORT_SYMBOL(lirc_register_driver);
 
 int lirc_unregister_driver(int minor)
@@ -393,10 +259,6 @@ int lirc_unregister_driver(int minor)
 		return -ENOENT;
 	}
 
-	/* end up polling thread */
-	if (ir->task)
-		kthread_stop(ir->task);
-
 	dev_dbg(ir->d.dev, "lirc_dev: driver %s unregistered from minor = %d\n",
 		ir->d.name, ir->d.minor);
 
@@ -407,12 +269,6 @@ int lirc_unregister_driver(int minor)
 		wake_up_interruptible(&ir->buf->wait_poll);
 	}
 
-	mutex_lock(&ir->irctl_lock);
-
-	if (ir->d.set_use_dec)
-		ir->d.set_use_dec(ir->d.data);
-
-	mutex_unlock(&ir->irctl_lock);
 	mutex_unlock(&lirc_dev_lock);
 
 	device_del(&ir->dev);
@@ -462,17 +318,10 @@ int lirc_dev_fop_open(struct inode *inode, struct file *file)
 			goto error;
 	}
 
+	if (ir->buf)
+		lirc_buffer_clear(ir->buf);
+
 	ir->open++;
-	if (ir->d.set_use_inc)
-		retval = ir->d.set_use_inc(ir->d.data);
-	if (retval) {
-		ir->open--;
-	} else {
-		if (ir->buf)
-			lirc_buffer_clear(ir->buf);
-		if (ir->task)
-			wake_up_process(ir->task);
-	}
 
 error:
 	nonseekable_open(inode, file);
@@ -497,8 +346,6 @@ int lirc_dev_fop_close(struct inode *inode, struct file *file)
 	rc_close(ir->d.rdev);
 
 	ir->open--;
-	if (ir->d.set_use_dec)
-		ir->d.set_use_dec(ir->d.data);
 	if (!ret)
 		mutex_unlock(&lirc_dev_lock);
 
@@ -517,7 +364,7 @@ unsigned int lirc_dev_fop_poll(struct file *file, poll_table *wait)
 	}
 
 	if (!ir->attached)
-		return POLLERR;
+		return POLLHUP | POLLERR;
 
 	if (ir->buf) {
 		poll_wait(file, &ir->buf->wait_poll, wait);
@@ -729,24 +576,6 @@ void *lirc_get_pdata(struct file *file)
 EXPORT_SYMBOL(lirc_get_pdata);
 
 
-ssize_t lirc_dev_fop_write(struct file *file, const char __user *buffer,
-			   size_t length, loff_t *ppos)
-{
-	struct irctl *ir = irctls[iminor(file_inode(file))];
-
-	if (!ir) {
-		pr_err("called with invalid irctl\n");
-		return -ENODEV;
-	}
-
-	if (!ir->attached)
-		return -ENODEV;
-
-	return -EINVAL;
-}
-EXPORT_SYMBOL(lirc_dev_fop_write);
-
-
 static int __init lirc_dev_init(void)
 {
 	int retval;
@@ -758,7 +587,7 @@ static int __init lirc_dev_init(void)
 	}
 
 	retval = alloc_chrdev_region(&lirc_base_dev, 0, MAX_IRCTL_DEVICES,
-				     IRCTL_DEV_NAME);
+				     "BaseRemoteCtl");
 	if (retval) {
 		class_destroy(lirc_class);
 		pr_err("alloc_chrdev_region failed\n");
@@ -784,6 +613,3 @@ module_exit(lirc_dev_exit);
 MODULE_DESCRIPTION("LIRC base driver module");
 MODULE_AUTHOR("Artur Lipowski");
 MODULE_LICENSE("GPL");
-
-module_param(debug, bool, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "Enable debugging messages");
diff --git a/drivers/media/rc/mceusb.c b/drivers/media/rc/mceusb.c
index 93b16fe3ab38..eb130694bbb8 100644
--- a/drivers/media/rc/mceusb.c
+++ b/drivers/media/rc/mceusb.c
@@ -36,18 +36,18 @@
 #include <linux/device.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include <linux/workqueue.h>
 #include <linux/usb.h>
 #include <linux/usb/input.h>
 #include <linux/pm_wakeup.h>
 #include <media/rc-core.h>
 
-#define DRIVER_VERSION	"1.92"
+#define DRIVER_VERSION	"1.93"
 #define DRIVER_AUTHOR	"Jarod Wilson <jarod@redhat.com>"
 #define DRIVER_DESC	"Windows Media Center Ed. eHome Infrared Transceiver " \
 			"device driver"
 #define DRIVER_NAME	"mceusb"
 
-#define USB_BUFLEN		32 /* USB reception buffer length */
 #define USB_CTRL_MSG_SZ		2  /* Size of usb ctrl msg on gen1 hw */
 #define MCE_G1_INIT_MSGS	40 /* Init messages on gen1 hw to throw out */
 
@@ -417,7 +417,9 @@ struct mceusb_dev {
 	/* usb */
 	struct usb_device *usbdev;
 	struct urb *urb_in;
+	unsigned int pipe_in;
 	struct usb_endpoint_descriptor *usb_ep_out;
+	unsigned int pipe_out;
 
 	/* buffers and dma */
 	unsigned char *buf_in;
@@ -454,6 +456,16 @@ struct mceusb_dev {
 	u8 num_rxports;		/* number of receive sensors */
 	u8 txports_cabled;	/* bitmask of transmitters with cable */
 	u8 rxports_active;	/* bitmask of active receive sensors */
+
+	/*
+	 * support for async error handler mceusb_deferred_kevent()
+	 * where usb_clear_halt(), usb_reset_configuration(),
+	 * usb_reset_device(), etc. must be done in process context
+	 */
+	struct work_struct kevent;
+	unsigned long kevent_flags;
+#		define EVENT_TX_HALT	0
+#		define EVENT_RX_HALT	1
 };
 
 /* MCE Device Command Strings, generally a port and command pair */
@@ -527,7 +539,7 @@ static int mceusb_cmd_datasize(u8 cmd, u8 subcmd)
 }
 
 static void mceusb_dev_printdata(struct mceusb_dev *ir, char *buf,
-				 int offset, int len, bool out)
+				 int buf_len, int offset, int len, bool out)
 {
 #if defined(DEBUG) || defined(CONFIG_DYNAMIC_DEBUG)
 	char *inout;
@@ -544,7 +556,8 @@ static void mceusb_dev_printdata(struct mceusb_dev *ir, char *buf,
 		return;
 
 	dev_dbg(dev, "%cx data: %*ph (length=%d)",
-		(out ? 't' : 'r'), min(len, USB_BUFLEN), buf, len);
+		(out ? 't' : 'r'),
+		min(len, buf_len - offset), buf + offset, len);
 
 	inout = out ? "Request" : "Got";
 
@@ -686,6 +699,21 @@ static void mceusb_dev_printdata(struct mceusb_dev *ir, char *buf,
 #endif
 }
 
+/*
+ * Schedule work that can't be done in interrupt handlers
+ * (mceusb_dev_recv() and mce_async_callback()) nor tasklets.
+ * Invokes mceusb_deferred_kevent() for recovering from
+ * error events specified by the kevent bit field.
+ */
+static void mceusb_defer_kevent(struct mceusb_dev *ir, int kevent)
+{
+	set_bit(kevent, &ir->kevent_flags);
+	if (!schedule_work(&ir->kevent))
+		dev_err(ir->dev, "kevent %d may have been dropped", kevent);
+	else
+		dev_dbg(ir->dev, "kevent %d scheduled", kevent);
+}
+
 static void mce_async_callback(struct urb *urb)
 {
 	struct mceusb_dev *ir;
@@ -701,7 +729,8 @@ static void mce_async_callback(struct urb *urb)
 	case 0:
 		len = urb->actual_length;
 
-		mceusb_dev_printdata(ir, urb->transfer_buffer, 0, len, true);
+		mceusb_dev_printdata(ir, urb->transfer_buffer, len,
+				     0, len, true);
 		break;
 
 	case -ECONNRESET:
@@ -711,6 +740,11 @@ static void mce_async_callback(struct urb *urb)
 		break;
 
 	case -EPIPE:
+		dev_err(ir->dev, "Error: request urb status = %d (TX HALT)",
+			urb->status);
+		mceusb_defer_kevent(ir, EVENT_TX_HALT);
+		break;
+
 	default:
 		dev_err(ir->dev, "Error: request urb status = %d", urb->status);
 		break;
@@ -721,18 +755,18 @@ static void mce_async_callback(struct urb *urb)
 	usb_free_urb(urb);
 }
 
-/* request incoming or send outgoing usb packet - used to initialize remote */
+/* request outgoing (send) usb packet - used to initialize remote */
 static void mce_request_packet(struct mceusb_dev *ir, unsigned char *data,
 								int size)
 {
-	int res, pipe;
+	int res;
 	struct urb *async_urb;
 	struct device *dev = ir->dev;
 	unsigned char *async_buf;
 
 	async_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (unlikely(!async_urb)) {
-		dev_err(dev, "Error, couldn't allocate urb!\n");
+		dev_err(dev, "Error, couldn't allocate urb!");
 		return;
 	}
 
@@ -743,32 +777,26 @@ static void mce_request_packet(struct mceusb_dev *ir, unsigned char *data,
 	}
 
 	/* outbound data */
-	if (usb_endpoint_xfer_int(ir->usb_ep_out)) {
-		pipe = usb_sndintpipe(ir->usbdev,
-				 ir->usb_ep_out->bEndpointAddress);
-		usb_fill_int_urb(async_urb, ir->usbdev, pipe, async_buf,
-				 size, mce_async_callback, ir,
+	if (usb_endpoint_xfer_int(ir->usb_ep_out))
+		usb_fill_int_urb(async_urb, ir->usbdev, ir->pipe_out,
+				 async_buf, size, mce_async_callback, ir,
 				 ir->usb_ep_out->bInterval);
-	} else {
-		pipe = usb_sndbulkpipe(ir->usbdev,
-				 ir->usb_ep_out->bEndpointAddress);
-		usb_fill_bulk_urb(async_urb, ir->usbdev, pipe,
-				 async_buf, size, mce_async_callback,
-				 ir);
-	}
-	memcpy(async_buf, data, size);
+	else
+		usb_fill_bulk_urb(async_urb, ir->usbdev, ir->pipe_out,
+				  async_buf, size, mce_async_callback, ir);
 
-	dev_dbg(dev, "receive request called (size=%#x)", size);
+	memcpy(async_buf, data, size);
 
-	async_urb->transfer_buffer_length = size;
-	async_urb->dev = ir->usbdev;
+	dev_dbg(dev, "send request called (size=%#x)", size);
 
 	res = usb_submit_urb(async_urb, GFP_ATOMIC);
 	if (res) {
-		dev_err(dev, "receive request FAILED! (res=%d)", res);
+		dev_err(dev, "send request FAILED! (res=%d)", res);
+		kfree(async_buf);
+		usb_free_urb(async_urb);
 		return;
 	}
-	dev_dbg(dev, "receive request complete (res=%d)", res);
+	dev_dbg(dev, "send request complete (res=%d)", res);
 }
 
 static void mce_async_out(struct mceusb_dev *ir, unsigned char *data, int size)
@@ -974,7 +1002,7 @@ static void mceusb_process_ir_data(struct mceusb_dev *ir, int buf_len)
 		switch (ir->parser_state) {
 		case SUBCMD:
 			ir->rem = mceusb_cmd_datasize(ir->cmd, ir->buf_in[i]);
-			mceusb_dev_printdata(ir, ir->buf_in, i - 1,
+			mceusb_dev_printdata(ir, ir->buf_in, buf_len, i - 1,
 					     ir->rem + 2, false);
 			mceusb_handle_command(ir, i);
 			ir->parser_state = CMD_DATA;
@@ -986,7 +1014,7 @@ static void mceusb_process_ir_data(struct mceusb_dev *ir, int buf_len)
 			rawir.duration = (ir->buf_in[i] & MCE_PULSE_MASK)
 					 * US_TO_NS(MCE_TIME_UNIT);
 
-			dev_dbg(ir->dev, "Storing %s with duration %d",
+			dev_dbg(ir->dev, "Storing %s with duration %u",
 				rawir.pulse ? "pulse" : "space",
 				rawir.duration);
 
@@ -1007,7 +1035,7 @@ static void mceusb_process_ir_data(struct mceusb_dev *ir, int buf_len)
 				continue;
 			}
 			ir->rem = (ir->cmd & MCE_PACKET_LENGTH_MASK);
-			mceusb_dev_printdata(ir, ir->buf_in,
+			mceusb_dev_printdata(ir, ir->buf_in, buf_len,
 					     i, ir->rem + 1, false);
 			if (ir->rem)
 				ir->parser_state = PARSE_IRDATA;
@@ -1052,6 +1080,11 @@ static void mceusb_dev_recv(struct urb *urb)
 		return;
 
 	case -EPIPE:
+		dev_err(ir->dev, "Error: urb status = %d (RX HALT)",
+			urb->status);
+		mceusb_defer_kevent(ir, EVENT_RX_HALT);
+		return;
+
 	default:
 		dev_err(ir->dev, "Error: urb status = %d", urb->status);
 		break;
@@ -1170,6 +1203,45 @@ static void mceusb_flash_led(struct mceusb_dev *ir)
 	mce_async_out(ir, FLASH_LED, sizeof(FLASH_LED));
 }
 
+/*
+ * Workqueue function
+ * for resetting or recovering device after occurrence of error events
+ * specified in ir->kevent bit field.
+ * Function runs (via schedule_work()) in non-interrupt context, for
+ * calls here (such as usb_clear_halt()) requiring non-interrupt context.
+ */
+static void mceusb_deferred_kevent(struct work_struct *work)
+{
+	struct mceusb_dev *ir =
+		container_of(work, struct mceusb_dev, kevent);
+	int status;
+
+	if (test_bit(EVENT_RX_HALT, &ir->kevent_flags)) {
+		usb_unlink_urb(ir->urb_in);
+		status = usb_clear_halt(ir->usbdev, ir->pipe_in);
+		if (status < 0) {
+			dev_err(ir->dev, "rx clear halt error %d",
+				status);
+		}
+		clear_bit(EVENT_RX_HALT, &ir->kevent_flags);
+		if (status == 0) {
+			status = usb_submit_urb(ir->urb_in, GFP_KERNEL);
+			if (status < 0) {
+				dev_err(ir->dev,
+					"rx unhalt submit urb error %d",
+					status);
+			}
+		}
+	}
+
+	if (test_bit(EVENT_TX_HALT, &ir->kevent_flags)) {
+		status = usb_clear_halt(ir->usbdev, ir->pipe_out);
+		if (status < 0)
+			dev_err(ir->dev, "tx clear halt error %d", status);
+		clear_bit(EVENT_TX_HALT, &ir->kevent_flags);
+	}
+}
+
 static struct rc_dev *mceusb_init_rc_dev(struct mceusb_dev *ir)
 {
 	struct usb_device *udev = ir->usbdev;
@@ -1303,6 +1375,7 @@ static int mceusb_dev_probe(struct usb_interface *intf,
 	if (!ir)
 		goto mem_alloc_fail;
 
+	ir->pipe_in = pipe;
 	ir->buf_in = usb_alloc_coherent(dev, maxp, GFP_ATOMIC, &ir->dma_in);
 	if (!ir->buf_in)
 		goto buf_in_alloc_fail;
@@ -1321,6 +1394,12 @@ static int mceusb_dev_probe(struct usb_interface *intf,
 
 	/* Saving usb interface data for use by the transmitter routine */
 	ir->usb_ep_out = ep_out;
+	if (usb_endpoint_xfer_int(ep_out))
+		ir->pipe_out = usb_sndintpipe(ir->usbdev,
+					      ep_out->bEndpointAddress);
+	else
+		ir->pipe_out = usb_sndbulkpipe(ir->usbdev,
+					       ep_out->bEndpointAddress);
 
 	if (dev->descriptor.iManufacturer
 	    && usb_string(dev, dev->descriptor.iManufacturer,
@@ -1332,21 +1411,32 @@ static int mceusb_dev_probe(struct usb_interface *intf,
 		snprintf(name + strlen(name), sizeof(name) - strlen(name),
 			 " %s", buf);
 
+	/*
+	 * Initialize async USB error handler before registering
+	 * or activating any mceusb RX and TX functions
+	 */
+	INIT_WORK(&ir->kevent, mceusb_deferred_kevent);
+
 	ir->rc = mceusb_init_rc_dev(ir);
 	if (!ir->rc)
 		goto rc_dev_fail;
 
 	/* wire up inbound data handler */
-	usb_fill_int_urb(ir->urb_in, dev, pipe, ir->buf_in, maxp,
-				mceusb_dev_recv, ir, ep_in->bInterval);
+	if (usb_endpoint_xfer_int(ep_in))
+		usb_fill_int_urb(ir->urb_in, dev, pipe, ir->buf_in, maxp,
+				 mceusb_dev_recv, ir, ep_in->bInterval);
+	else
+		usb_fill_bulk_urb(ir->urb_in, dev, pipe, ir->buf_in, maxp,
+				  mceusb_dev_recv, ir);
+
 	ir->urb_in->transfer_dma = ir->dma_in;
 	ir->urb_in->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 
 	/* flush buffers on the device */
-	dev_dbg(&intf->dev, "Flushing receive buffers\n");
+	dev_dbg(&intf->dev, "Flushing receive buffers");
 	res = usb_submit_urb(ir->urb_in, GFP_KERNEL);
 	if (res)
-		dev_err(&intf->dev, "failed to flush buffers: %d\n", res);
+		dev_err(&intf->dev, "failed to flush buffers: %d", res);
 
 	/* figure out which firmware/emulator version this hardware has */
 	mceusb_get_emulator_version(ir);
@@ -1380,6 +1470,7 @@ static int mceusb_dev_probe(struct usb_interface *intf,
 
 	/* Error-handling path */
 rc_dev_fail:
+	cancel_work_sync(&ir->kevent);
 	usb_put_dev(ir->usbdev);
 	usb_kill_urb(ir->urb_in);
 	usb_free_urb(ir->urb_in);
@@ -1405,6 +1496,7 @@ static void mceusb_dev_disconnect(struct usb_interface *intf)
 		return;
 
 	ir->usbdev = NULL;
+	cancel_work_sync(&ir->kevent);
 	rc_unregister_device(ir->rc);
 	usb_kill_urb(ir->urb_in);
 	usb_free_urb(ir->urb_in);
diff --git a/drivers/media/rc/meson-ir.c b/drivers/media/rc/meson-ir.c
index 5576dbd6b1a4..65566d569cb1 100644
--- a/drivers/media/rc/meson-ir.c
+++ b/drivers/media/rc/meson-ir.c
@@ -19,6 +19,7 @@
 #include <linux/of_platform.h>
 #include <linux/platform_device.h>
 #include <linux/spinlock.h>
+#include <linux/bitfield.h>
 
 #include <media/rc-core.h>
 
@@ -36,7 +37,7 @@
 /* only available on Meson 8b and newer */
 #define IR_DEC_REG2		0x20
 
-#define REG0_RATE_MASK		(BIT(11) - 1)
+#define REG0_RATE_MASK		GENMASK(11, 0)
 
 #define DECODE_MODE_NEC		0x0
 #define DECODE_MODE_RAW		0x2
@@ -49,14 +50,13 @@
 #define REG2_MODE_MASK		GENMASK(3, 0)
 #define REG2_MODE_SHIFT		0
 
-#define REG1_TIME_IV_SHIFT	16
-#define REG1_TIME_IV_MASK	((BIT(13) - 1) << REG1_TIME_IV_SHIFT)
+#define REG1_TIME_IV_MASK	GENMASK(28, 16)
 
-#define REG1_IRQSEL_MASK	(BIT(2) | BIT(3))
-#define REG1_IRQSEL_NEC_MODE	(0 << 2)
-#define REG1_IRQSEL_RISE_FALL	(1 << 2)
-#define REG1_IRQSEL_FALL	(2 << 2)
-#define REG1_IRQSEL_RISE	(3 << 2)
+#define REG1_IRQSEL_MASK	GENMASK(3, 2)
+#define REG1_IRQSEL_NEC_MODE	0
+#define REG1_IRQSEL_RISE_FALL	1
+#define REG1_IRQSEL_FALL	2
+#define REG1_IRQSEL_RISE	3
 
 #define REG1_RESET		BIT(0)
 #define REG1_ENABLE		BIT(15)
@@ -68,7 +68,6 @@
 struct meson_ir {
 	void __iomem	*reg;
 	struct rc_dev	*rc;
-	int		irq;
 	spinlock_t	lock;
 };
 
@@ -86,18 +85,19 @@ static void meson_ir_set_mask(struct meson_ir *ir, unsigned int reg,
 static irqreturn_t meson_ir_irq(int irqno, void *dev_id)
 {
 	struct meson_ir *ir = dev_id;
-	u32 duration;
+	u32 duration, status;
 	DEFINE_IR_RAW_EVENT(rawir);
 
 	spin_lock(&ir->lock);
 
-	duration = readl(ir->reg + IR_DEC_REG1);
-	duration = (duration & REG1_TIME_IV_MASK) >> REG1_TIME_IV_SHIFT;
+	duration = readl_relaxed(ir->reg + IR_DEC_REG1);
+	duration = FIELD_GET(REG1_TIME_IV_MASK, duration);
 	rawir.duration = US_TO_NS(duration * MESON_TRATE);
 
-	rawir.pulse = !!(readl(ir->reg + IR_DEC_STATUS) & STATUS_IR_DEC_IN);
+	status = readl_relaxed(ir->reg + IR_DEC_STATUS);
+	rawir.pulse = !!(status & STATUS_IR_DEC_IN);
 
-	ir_raw_event_store_with_filter(ir->rc, &rawir);
+	ir_raw_event_store(ir->rc, &rawir);
 	ir_raw_event_handle(ir->rc);
 
 	spin_unlock(&ir->lock);
@@ -112,7 +112,7 @@ static int meson_ir_probe(struct platform_device *pdev)
 	struct resource *res;
 	const char *map_name;
 	struct meson_ir *ir;
-	int ret;
+	int irq, ret;
 
 	ir = devm_kzalloc(dev, sizeof(struct meson_ir), GFP_KERNEL);
 	if (!ir)
@@ -125,13 +125,13 @@ static int meson_ir_probe(struct platform_device *pdev)
 		return PTR_ERR(ir->reg);
 	}
 
-	ir->irq = platform_get_irq(pdev, 0);
-	if (ir->irq < 0) {
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
 		dev_err(dev, "no irq resource\n");
-		return ir->irq;
+		return irq;
 	}
 
-	ir->rc = rc_allocate_device(RC_DRIVER_IR_RAW);
+	ir->rc = devm_rc_allocate_device(dev, RC_DRIVER_IR_RAW);
 	if (!ir->rc) {
 		dev_err(dev, "failed to allocate rc device\n");
 		return -ENOMEM;
@@ -143,7 +143,6 @@ static int meson_ir_probe(struct platform_device *pdev)
 	ir->rc->input_id.bustype = BUS_HOST;
 	map_name = of_get_property(node, "linux,rc-map-name", NULL);
 	ir->rc->map_name = map_name ? map_name : RC_MAP_EMPTY;
-	ir->rc->dev.parent = dev;
 	ir->rc->allowed_protocols = RC_BIT_ALL_IR_DECODER;
 	ir->rc->rx_resolution = US_TO_NS(MESON_TRATE);
 	ir->rc->timeout = MS_TO_NS(200);
@@ -152,16 +151,16 @@ static int meson_ir_probe(struct platform_device *pdev)
 	spin_lock_init(&ir->lock);
 	platform_set_drvdata(pdev, ir);
 
-	ret = rc_register_device(ir->rc);
+	ret = devm_rc_register_device(dev, ir->rc);
 	if (ret) {
 		dev_err(dev, "failed to register rc device\n");
-		goto out_free;
+		return ret;
 	}
 
-	ret = devm_request_irq(dev, ir->irq, meson_ir_irq, 0, "ir-meson", ir);
+	ret = devm_request_irq(dev, irq, meson_ir_irq, 0, NULL, ir);
 	if (ret) {
 		dev_err(dev, "failed to request irq\n");
-		goto out_unreg;
+		return ret;
 	}
 
 	/* Reset the decoder */
@@ -171,29 +170,22 @@ static int meson_ir_probe(struct platform_device *pdev)
 	/* Set general operation mode (= raw/software decoding) */
 	if (of_device_is_compatible(node, "amlogic,meson6-ir"))
 		meson_ir_set_mask(ir, IR_DEC_REG1, REG1_MODE_MASK,
-				  DECODE_MODE_RAW << REG1_MODE_SHIFT);
+				  FIELD_PREP(REG1_MODE_MASK, DECODE_MODE_RAW));
 	else
 		meson_ir_set_mask(ir, IR_DEC_REG2, REG2_MODE_MASK,
-				  DECODE_MODE_RAW << REG2_MODE_SHIFT);
+				  FIELD_PREP(REG2_MODE_MASK, DECODE_MODE_RAW));
 
 	/* Set rate */
 	meson_ir_set_mask(ir, IR_DEC_REG0, REG0_RATE_MASK, MESON_TRATE - 1);
 	/* IRQ on rising and falling edges */
 	meson_ir_set_mask(ir, IR_DEC_REG1, REG1_IRQSEL_MASK,
-			  REG1_IRQSEL_RISE_FALL);
+			  FIELD_PREP(REG1_IRQSEL_MASK, REG1_IRQSEL_RISE_FALL));
 	/* Enable the decoder */
 	meson_ir_set_mask(ir, IR_DEC_REG1, REG1_ENABLE, REG1_ENABLE);
 
 	dev_info(dev, "receiver initialized\n");
 
 	return 0;
-out_unreg:
-	rc_unregister_device(ir->rc);
-	ir->rc = NULL;
-out_free:
-	rc_free_device(ir->rc);
-
-	return ret;
 }
 
 static int meson_ir_remove(struct platform_device *pdev)
@@ -206,11 +198,35 @@ static int meson_ir_remove(struct platform_device *pdev)
 	meson_ir_set_mask(ir, IR_DEC_REG1, REG1_ENABLE, 0);
 	spin_unlock_irqrestore(&ir->lock, flags);
 
-	rc_unregister_device(ir->rc);
-
 	return 0;
 }
 
+static void meson_ir_shutdown(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *node = dev->of_node;
+	struct meson_ir *ir = platform_get_drvdata(pdev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&ir->lock, flags);
+
+	/*
+	 * Set operation mode to NEC/hardware decoding to give
+	 * bootloader a chance to power the system back on
+	 */
+	if (of_device_is_compatible(node, "amlogic,meson6-ir"))
+		meson_ir_set_mask(ir, IR_DEC_REG1, REG1_MODE_MASK,
+				  DECODE_MODE_NEC << REG1_MODE_SHIFT);
+	else
+		meson_ir_set_mask(ir, IR_DEC_REG2, REG2_MODE_MASK,
+				  DECODE_MODE_NEC << REG2_MODE_SHIFT);
+
+	/* Set rate to default value */
+	meson_ir_set_mask(ir, IR_DEC_REG0, REG0_RATE_MASK, 0x13);
+
+	spin_unlock_irqrestore(&ir->lock, flags);
+}
+
 static const struct of_device_id meson_ir_match[] = {
 	{ .compatible = "amlogic,meson6-ir" },
 	{ .compatible = "amlogic,meson8b-ir" },
@@ -222,6 +238,7 @@ MODULE_DEVICE_TABLE(of, meson_ir_match);
 static struct platform_driver meson_ir_driver = {
 	.probe		= meson_ir_probe,
 	.remove		= meson_ir_remove,
+	.shutdown	= meson_ir_shutdown,
 	.driver = {
 		.name		= DRIVER_NAME,
 		.of_match_table	= meson_ir_match,
diff --git a/drivers/media/rc/rc-core-priv.h b/drivers/media/rc/rc-core-priv.h
index 0455b273c2fc..b3e7cac2c3ee 100644
--- a/drivers/media/rc/rc-core-priv.h
+++ b/drivers/media/rc/rc-core-priv.h
@@ -263,7 +263,9 @@ int ir_raw_gen_pl(struct ir_raw_event **ev, unsigned int max,
  * Routines from rc-raw.c to be used internally and by decoders
  */
 u64 ir_raw_get_allowed_protocols(void);
+int ir_raw_event_prepare(struct rc_dev *dev);
 int ir_raw_event_register(struct rc_dev *dev);
+void ir_raw_event_free(struct rc_dev *dev);
 void ir_raw_event_unregister(struct rc_dev *dev);
 int ir_raw_handler_register(struct ir_raw_handler *ir_raw_handler);
 void ir_raw_handler_unregister(struct ir_raw_handler *ir_raw_handler);
diff --git a/drivers/media/rc/rc-ir-raw.c b/drivers/media/rc/rc-ir-raw.c
index a2fc1a1d58b0..b6d256f03847 100644
--- a/drivers/media/rc/rc-ir-raw.c
+++ b/drivers/media/rc/rc-ir-raw.c
@@ -486,15 +486,18 @@ EXPORT_SYMBOL(ir_raw_encode_scancode);
 /*
  * Used to (un)register raw event clients
  */
-int ir_raw_event_register(struct rc_dev *dev)
+int ir_raw_event_prepare(struct rc_dev *dev)
 {
-	int rc;
-	struct ir_raw_handler *handler;
-	struct task_struct *thread;
+	static bool raw_init; /* 'false' default value, raw decoders loaded? */
 
 	if (!dev)
 		return -EINVAL;
 
+	if (!raw_init) {
+		request_module("ir-lirc-codec");
+		raw_init = true;
+	}
+
 	dev->raw = kzalloc(sizeof(*dev->raw), GFP_KERNEL);
 	if (!dev->raw)
 		return -ENOMEM;
@@ -503,6 +506,14 @@ int ir_raw_event_register(struct rc_dev *dev)
 	dev->change_protocol = change_protocol;
 	INIT_KFIFO(dev->raw->kfifo);
 
+	return 0;
+}
+
+int ir_raw_event_register(struct rc_dev *dev)
+{
+	struct ir_raw_handler *handler;
+	struct task_struct *thread;
+
 	/*
 	 * raw transmitters do not need any event registration
 	 * because the event is coming from userspace
@@ -511,10 +522,8 @@ int ir_raw_event_register(struct rc_dev *dev)
 		thread = kthread_run(ir_raw_event_thread, dev->raw, "rc%u",
 				     dev->minor);
 
-		if (IS_ERR(thread)) {
-			rc = PTR_ERR(thread);
-			goto out;
-		}
+		if (IS_ERR(thread))
+			return PTR_ERR(thread);
 
 		dev->raw->thread = thread;
 	}
@@ -527,11 +536,15 @@ int ir_raw_event_register(struct rc_dev *dev)
 	mutex_unlock(&ir_raw_handler_lock);
 
 	return 0;
+}
+
+void ir_raw_event_free(struct rc_dev *dev)
+{
+	if (!dev)
+		return;
 
-out:
 	kfree(dev->raw);
 	dev->raw = NULL;
-	return rc;
 }
 
 void ir_raw_event_unregister(struct rc_dev *dev)
@@ -550,8 +563,7 @@ void ir_raw_event_unregister(struct rc_dev *dev)
 			handler->raw_unregister(dev);
 	mutex_unlock(&ir_raw_handler_lock);
 
-	kfree(dev->raw);
-	dev->raw = NULL;
+	ir_raw_event_free(dev);
 }
 
 /*
diff --git a/drivers/media/rc/rc-main.c b/drivers/media/rc/rc-main.c
index 6ec73357fa47..a9eba0013525 100644
--- a/drivers/media/rc/rc-main.c
+++ b/drivers/media/rc/rc-main.c
@@ -15,7 +15,6 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <media/rc-core.h>
-#include <linux/atomic.h>
 #include <linux/spinlock.h>
 #include <linux/delay.h>
 #include <linux/input.h>
@@ -934,8 +933,8 @@ static bool lirc_is_present(void)
  * It returns the protocol names of supported protocols.
  * Enabled protocols are printed in brackets.
  *
- * dev->lock is taken to guard against races between device
- * registration, store_protocols and show_protocols.
+ * dev->lock is taken to guard against races between
+ * store_protocols and show_protocols.
  */
 static ssize_t show_protocols(struct device *device,
 			      struct device_attribute *mattr, char *buf)
@@ -945,13 +944,6 @@ static ssize_t show_protocols(struct device *device,
 	char *tmp = buf;
 	int i;
 
-	/* Device is being removed */
-	if (!dev)
-		return -EINVAL;
-
-	if (!atomic_read(&dev->initialized))
-		return -ERESTARTSYS;
-
 	mutex_lock(&dev->lock);
 
 	enabled = dev->enabled_protocols;
@@ -1106,8 +1098,8 @@ static void ir_raw_load_modules(u64 *protocols)
  * See parse_protocol_change() for the valid commands.
  * Returns @len on success or a negative error code.
  *
- * dev->lock is taken to guard against races between device
- * registration, store_protocols and show_protocols.
+ * dev->lock is taken to guard against races between
+ * store_protocols and show_protocols.
  */
 static ssize_t store_protocols(struct device *device,
 			       struct device_attribute *mattr,
@@ -1119,13 +1111,6 @@ static ssize_t store_protocols(struct device *device,
 	u64 old_protocols, new_protocols;
 	ssize_t rc;
 
-	/* Device is being removed */
-	if (!dev)
-		return -EINVAL;
-
-	if (!atomic_read(&dev->initialized))
-		return -ERESTARTSYS;
-
 	IR_dprintk(1, "Normal protocol change requested\n");
 	current_protocols = &dev->enabled_protocols;
 	filter = &dev->scancode_filter;
@@ -1200,7 +1185,7 @@ out:
  * Bits of the filter value corresponding to set bits in the filter mask are
  * compared against input scancodes and non-matching scancodes are discarded.
  *
- * dev->lock is taken to guard against races between device registration,
+ * dev->lock is taken to guard against races between
  * store_filter and show_filter.
  */
 static ssize_t show_filter(struct device *device,
@@ -1212,13 +1197,6 @@ static ssize_t show_filter(struct device *device,
 	struct rc_scancode_filter *filter;
 	u32 val;
 
-	/* Device is being removed */
-	if (!dev)
-		return -EINVAL;
-
-	if (!atomic_read(&dev->initialized))
-		return -ERESTARTSYS;
-
 	mutex_lock(&dev->lock);
 
 	if (fattr->type == RC_FILTER_NORMAL)
@@ -1251,7 +1229,7 @@ static ssize_t show_filter(struct device *device,
  * Bits of the filter value corresponding to set bits in the filter mask are
  * compared against input scancodes and non-matching scancodes are discarded.
  *
- * dev->lock is taken to guard against races between device registration,
+ * dev->lock is taken to guard against races between
  * store_filter and show_filter.
  */
 static ssize_t store_filter(struct device *device,
@@ -1265,13 +1243,6 @@ static ssize_t store_filter(struct device *device,
 	unsigned long val;
 	int (*set_filter)(struct rc_dev *dev, struct rc_scancode_filter *filter);
 
-	/* Device is being removed */
-	if (!dev)
-		return -EINVAL;
-
-	if (!atomic_read(&dev->initialized))
-		return -ERESTARTSYS;
-
 	ret = kstrtoul(buf, 0, &val);
 	if (ret < 0)
 		return ret;
@@ -1372,8 +1343,8 @@ static const char * const proto_variant_names[] = {
  * It returns the protocol names of supported protocols.
  * The enabled protocols are printed in brackets.
  *
- * dev->lock is taken to guard against races between device
- * registration, store_protocols and show_protocols.
+ * dev->lock is taken to guard against races between
+ * store_wakeup_protocols and show_wakeup_protocols.
  */
 static ssize_t show_wakeup_protocols(struct device *device,
 				     struct device_attribute *mattr,
@@ -1385,13 +1356,6 @@ static ssize_t show_wakeup_protocols(struct device *device,
 	char *tmp = buf;
 	int i;
 
-	/* Device is being removed */
-	if (!dev)
-		return -EINVAL;
-
-	if (!atomic_read(&dev->initialized))
-		return -ERESTARTSYS;
-
 	mutex_lock(&dev->lock);
 
 	allowed = dev->allowed_wakeup_protocols;
@@ -1431,8 +1395,8 @@ static ssize_t show_wakeup_protocols(struct device *device,
  * It is trigged by writing to /sys/class/rc/rc?/wakeup_protocols.
  * Returns @len on success or a negative error code.
  *
- * dev->lock is taken to guard against races between device
- * registration, store_protocols and show_protocols.
+ * dev->lock is taken to guard against races between
+ * store_wakeup_protocols and show_wakeup_protocols.
  */
 static ssize_t store_wakeup_protocols(struct device *device,
 				      struct device_attribute *mattr,
@@ -1444,13 +1408,6 @@ static ssize_t store_wakeup_protocols(struct device *device,
 	u64 allowed;
 	int i;
 
-	/* Device is being removed */
-	if (!dev)
-		return -EINVAL;
-
-	if (!atomic_read(&dev->initialized))
-		return -ERESTARTSYS;
-
 	mutex_lock(&dev->lock);
 
 	allowed = dev->allowed_wakeup_protocols;
@@ -1663,7 +1620,7 @@ struct rc_dev *devm_rc_allocate_device(struct device *dev,
 }
 EXPORT_SYMBOL_GPL(devm_rc_allocate_device);
 
-static int rc_setup_rx_device(struct rc_dev *dev)
+static int rc_prepare_rx_device(struct rc_dev *dev)
 {
 	int rc;
 	struct rc_map *rc_map;
@@ -1703,6 +1660,28 @@ static int rc_setup_rx_device(struct rc_dev *dev)
 	if (dev->close)
 		dev->input_dev->close = ir_close;
 
+	dev->input_dev->dev.parent = &dev->dev;
+	memcpy(&dev->input_dev->id, &dev->input_id, sizeof(dev->input_id));
+	dev->input_dev->phys = dev->input_phys;
+	dev->input_dev->name = dev->input_name;
+
+	return 0;
+
+out_table:
+	ir_free_table(&dev->rc_map);
+
+	return rc;
+}
+
+static int rc_setup_rx_device(struct rc_dev *dev)
+{
+	int rc;
+
+	/* rc_open will be called here */
+	rc = input_register_device(dev->input_dev);
+	if (rc)
+		return rc;
+
 	/*
 	 * Default delay of 250ms is too short for some protocols, especially
 	 * since the timeout is currently set to 250ms. Increase it to 500ms,
@@ -1718,38 +1697,24 @@ static int rc_setup_rx_device(struct rc_dev *dev)
 	 */
 	dev->input_dev->rep[REP_PERIOD] = 125;
 
-	dev->input_dev->dev.parent = &dev->dev;
-	memcpy(&dev->input_dev->id, &dev->input_id, sizeof(dev->input_id));
-	dev->input_dev->phys = dev->input_phys;
-	dev->input_dev->name = dev->input_name;
-
-	/* rc_open will be called here */
-	rc = input_register_device(dev->input_dev);
-	if (rc)
-		goto out_table;
-
 	return 0;
-
-out_table:
-	ir_free_table(&dev->rc_map);
-
-	return rc;
 }
 
 static void rc_free_rx_device(struct rc_dev *dev)
 {
-	if (!dev || dev->driver_type == RC_DRIVER_IR_RAW_TX)
+	if (!dev)
 		return;
 
-	ir_free_table(&dev->rc_map);
+	if (dev->input_dev) {
+		input_unregister_device(dev->input_dev);
+		dev->input_dev = NULL;
+	}
 
-	input_unregister_device(dev->input_dev);
-	dev->input_dev = NULL;
+	ir_free_table(&dev->rc_map);
 }
 
 int rc_register_device(struct rc_dev *dev)
 {
-	static bool raw_init; /* 'false' default value, raw decoders loaded? */
 	const char *path;
 	int attr = 0;
 	int minor;
@@ -1765,7 +1730,6 @@ int rc_register_device(struct rc_dev *dev)
 	dev->minor = minor;
 	dev_set_name(&dev->dev, "rc%u", dev->minor);
 	dev_set_drvdata(&dev->dev, dev);
-	atomic_set(&dev->initialized, 0);
 
 	dev->dev.groups = dev->sysfs_groups;
 	if (dev->driver_type != RC_DRIVER_IR_RAW_TX)
@@ -1776,34 +1740,40 @@ int rc_register_device(struct rc_dev *dev)
 		dev->sysfs_groups[attr++] = &rc_dev_wakeup_filter_attr_grp;
 	dev->sysfs_groups[attr++] = NULL;
 
+	if (dev->driver_type == RC_DRIVER_IR_RAW ||
+	    dev->driver_type == RC_DRIVER_IR_RAW_TX) {
+		rc = ir_raw_event_prepare(dev);
+		if (rc < 0)
+			goto out_minor;
+	}
+
+	if (dev->driver_type != RC_DRIVER_IR_RAW_TX) {
+		rc = rc_prepare_rx_device(dev);
+		if (rc)
+			goto out_raw;
+	}
+
 	rc = device_add(&dev->dev);
 	if (rc)
-		goto out_unlock;
+		goto out_rx_free;
 
 	path = kobject_get_path(&dev->dev.kobj, GFP_KERNEL);
 	dev_info(&dev->dev, "%s as %s\n",
 		dev->input_name ?: "Unspecified device", path ?: "N/A");
 	kfree(path);
 
-	if (dev->driver_type == RC_DRIVER_IR_RAW ||
-	    dev->driver_type == RC_DRIVER_IR_RAW_TX) {
-		if (!raw_init) {
-			request_module_nowait("ir-lirc-codec");
-			raw_init = true;
-		}
-		rc = ir_raw_event_register(dev);
-		if (rc < 0)
-			goto out_dev;
-	}
-
 	if (dev->driver_type != RC_DRIVER_IR_RAW_TX) {
 		rc = rc_setup_rx_device(dev);
 		if (rc)
-			goto out_raw;
+			goto out_dev;
 	}
 
-	/* Allow the RC sysfs nodes to be accessible */
-	atomic_set(&dev->initialized, 1);
+	if (dev->driver_type == RC_DRIVER_IR_RAW ||
+	    dev->driver_type == RC_DRIVER_IR_RAW_TX) {
+		rc = ir_raw_event_register(dev);
+		if (rc < 0)
+			goto out_rx;
+	}
 
 	IR_dprintk(1, "Registered rc%u (driver: %s)\n",
 		   dev->minor,
@@ -1811,11 +1781,15 @@ int rc_register_device(struct rc_dev *dev)
 
 	return 0;
 
-out_raw:
-	ir_raw_event_unregister(dev);
+out_rx:
+	rc_free_rx_device(dev);
 out_dev:
 	device_del(&dev->dev);
-out_unlock:
+out_rx_free:
+	ir_free_table(&dev->rc_map);
+out_raw:
+	ir_raw_event_free(dev);
+out_minor:
 	ida_simple_remove(&rc_ida, minor);
 	return rc;
 }
diff --git a/drivers/media/rc/sir_ir.c b/drivers/media/rc/sir_ir.c
index e12ec50bf0bf..20234ba0b318 100644
--- a/drivers/media/rc/sir_ir.c
+++ b/drivers/media/rc/sir_ir.c
@@ -53,16 +53,13 @@ static DEFINE_SPINLOCK(hardware_lock);
 
 /* Communication with user-space */
 static void add_read_queue(int flag, unsigned long val);
-static int init_chrdev(void);
 /* Hardware */
 static irqreturn_t sir_interrupt(int irq, void *dev_id);
 static void send_space(unsigned long len);
 static void send_pulse(unsigned long len);
-static int init_hardware(void);
+static void init_hardware(void);
 static void drop_hardware(void);
 /* Initialisation */
-static int init_port(void);
-static void drop_port(void);
 
 static inline unsigned int sinp(int offset)
 {
@@ -122,28 +119,6 @@ static void add_read_queue(int flag, unsigned long val)
 	ir_raw_event_store_with_filter(rcdev, &ev);
 }
 
-static int init_chrdev(void)
-{
-	rcdev = devm_rc_allocate_device(&sir_ir_dev->dev, RC_DRIVER_IR_RAW);
-	if (!rcdev)
-		return -ENOMEM;
-
-	rcdev->input_name = "SIR IrDA port";
-	rcdev->input_phys = KBUILD_MODNAME "/input0";
-	rcdev->input_id.bustype = BUS_HOST;
-	rcdev->input_id.vendor = 0x0001;
-	rcdev->input_id.product = 0x0001;
-	rcdev->input_id.version = 0x0100;
-	rcdev->tx_ir = sir_tx_ir;
-	rcdev->allowed_protocols = RC_BIT_ALL_IR_DECODER;
-	rcdev->driver_name = KBUILD_MODNAME;
-	rcdev->map_name = RC_MAP_RC6_MCE;
-	rcdev->timeout = IR_DEFAULT_TIMEOUT;
-	rcdev->dev.parent = &sir_ir_dev->dev;
-
-	return devm_rc_register_device(&sir_ir_dev->dev, rcdev);
-}
-
 /* SECTION: Hardware */
 static void sir_timeout(unsigned long data)
 {
@@ -183,9 +158,15 @@ static irqreturn_t sir_interrupt(int irq, void *dev_id)
 	static unsigned long delt;
 	unsigned long deltintr;
 	unsigned long flags;
+	int counter = 0;
 	int iir, lsr;
 
 	while ((iir = inb(io + UART_IIR) & UART_IIR_ID)) {
+		if (++counter > 256) {
+			dev_err(&sir_ir_dev->dev, "Trapped in interrupt");
+			break;
+		}
+
 		switch (iir & UART_IIR_ID) { /* FIXME toto treba preriedit */
 		case UART_IIR_MSI:
 			(void)inb(io + UART_MSR);
@@ -282,7 +263,7 @@ static void send_pulse(unsigned long len)
 	}
 }
 
-static int init_hardware(void)
+static void init_hardware(void)
 {
 	unsigned long flags;
 
@@ -304,7 +285,6 @@ static int init_hardware(void)
 	/* turn on UART */
 	outb(UART_MCR_DTR | UART_MCR_RTS | UART_MCR_OUT2, io + UART_MCR);
 	spin_unlock_irqrestore(&hardware_lock, flags);
-	return 0;
 }
 
 static void drop_hardware(void)
@@ -320,61 +300,55 @@ static void drop_hardware(void)
 }
 
 /* SECTION: Initialisation */
-
-static int init_port(void)
+static int sir_ir_probe(struct platform_device *dev)
 {
 	int retval;
 
+	rcdev = devm_rc_allocate_device(&sir_ir_dev->dev, RC_DRIVER_IR_RAW);
+	if (!rcdev)
+		return -ENOMEM;
+
+	rcdev->input_name = "SIR IrDA port";
+	rcdev->input_phys = KBUILD_MODNAME "/input0";
+	rcdev->input_id.bustype = BUS_HOST;
+	rcdev->input_id.vendor = 0x0001;
+	rcdev->input_id.product = 0x0001;
+	rcdev->input_id.version = 0x0100;
+	rcdev->tx_ir = sir_tx_ir;
+	rcdev->allowed_protocols = RC_BIT_ALL_IR_DECODER;
+	rcdev->driver_name = KBUILD_MODNAME;
+	rcdev->map_name = RC_MAP_RC6_MCE;
+	rcdev->timeout = IR_DEFAULT_TIMEOUT;
+	rcdev->dev.parent = &sir_ir_dev->dev;
+
 	setup_timer(&timerlist, sir_timeout, 0);
 
 	/* get I/O port access and IRQ line */
-	if (!request_region(io, 8, KBUILD_MODNAME)) {
+	if (!devm_request_region(&sir_ir_dev->dev, io, 8, KBUILD_MODNAME)) {
 		pr_err("i/o port 0x%.4x already in use.\n", io);
 		return -EBUSY;
 	}
-	retval = request_irq(irq, sir_interrupt, 0,
-			     KBUILD_MODNAME, NULL);
+	retval = devm_request_irq(&sir_ir_dev->dev, irq, sir_interrupt, 0,
+				  KBUILD_MODNAME, NULL);
 	if (retval < 0) {
-		release_region(io, 8);
 		pr_err("IRQ %d already in use.\n", irq);
 		return retval;
 	}
 	pr_info("I/O port 0x%.4x, IRQ %d.\n", io, irq);
 
-	return 0;
-}
-
-static void drop_port(void)
-{
-	free_irq(irq, NULL);
-	del_timer_sync(&timerlist);
-	release_region(io, 8);
-}
-
-static int init_sir_ir(void)
-{
-	int retval;
-
-	retval = init_port();
+	retval = devm_rc_register_device(&sir_ir_dev->dev, rcdev);
 	if (retval < 0)
 		return retval;
-	init_hardware();
-	return 0;
-}
-
-static int sir_ir_probe(struct platform_device *dev)
-{
-	int retval;
 
-	retval = init_chrdev();
-	if (retval < 0)
-		return retval;
+	init_hardware();
 
-	return init_sir_ir();
+	return 0;
 }
 
 static int sir_ir_remove(struct platform_device *dev)
 {
+	drop_hardware();
+	del_timer_sync(&timerlist);
 	return 0;
 }
 
@@ -415,8 +389,6 @@ pdev_alloc_fail:
 
 static void __exit sir_ir_exit(void)
 {
-	drop_hardware();
-	drop_port();
 	platform_device_unregister(sir_ir_dev);
 	platform_driver_unregister(&sir_ir_driver);
 }
@@ -428,10 +400,10 @@ MODULE_DESCRIPTION("Infrared receiver driver for SIR type serial ports");
 MODULE_AUTHOR("Milan Pikula");
 MODULE_LICENSE("GPL");
 
-module_param(io, int, 0444);
+module_param_hw(io, int, ioport, 0444);
 MODULE_PARM_DESC(io, "I/O address base (0x3f8 or 0x2f8)");
 
-module_param(irq, int, 0444);
+module_param_hw(irq, int, irq, 0444);
 MODULE_PARM_DESC(irq, "Interrupt (4 or 3)");
 
 module_param(threshold, int, 0444);
diff --git a/drivers/media/tuners/tda18271-fe.c b/drivers/media/tuners/tda18271-fe.c
index b4e5fa2ff5e5..147155553648 100644
--- a/drivers/media/tuners/tda18271-fe.c
+++ b/drivers/media/tuners/tda18271-fe.c
@@ -960,7 +960,7 @@ static int tda18271_set_params(struct dvb_frontend *fe)
 		break;
 	case SYS_DVBC_ANNEX_B:
 		bw = 6000000;
-		/* falltrough */
+		/* fall through */
 	case SYS_DVBC_ANNEX_A:
 	case SYS_DVBC_ANNEX_C:
 		if (bw <= 6000000) {
diff --git a/drivers/media/tuners/xc5000.c b/drivers/media/tuners/xc5000.c
index e823aafce276..0e7e4fdf9e50 100644
--- a/drivers/media/tuners/xc5000.c
+++ b/drivers/media/tuners/xc5000.c
@@ -565,38 +565,16 @@ static int xc_get_totalgain(struct xc5000_priv *priv, u16 *totalgain)
 	return xc5000_readreg(priv, XREG_TOTALGAIN, totalgain);
 }
 
-static u16 wait_for_lock(struct xc5000_priv *priv)
-{
-	u16 lock_state = 0;
-	int watch_dog_count = 40;
-
-	while ((lock_state == 0) && (watch_dog_count > 0)) {
-		xc_get_lock_status(priv, &lock_state);
-		if (lock_state != 1) {
-			msleep(5);
-			watch_dog_count--;
-		}
-	}
-	return lock_state;
-}
-
 #define XC_TUNE_ANALOG  0
 #define XC_TUNE_DIGITAL 1
 static int xc_tune_channel(struct xc5000_priv *priv, u32 freq_hz, int mode)
 {
-	int found = 0;
-
 	dprintk(1, "%s(%u)\n", __func__, freq_hz);
 
 	if (xc_set_rf_frequency(priv, freq_hz) != 0)
-		return 0;
-
-	if (mode == XC_TUNE_ANALOG) {
-		if (wait_for_lock(priv) == 1)
-			found = 1;
-	}
+		return -EREMOTEIO;
 
-	return found;
+	return 0;
 }
 
 static int xc_set_xtal(struct dvb_frontend *fe)
@@ -788,6 +766,7 @@ static int xc5000_set_digital_params(struct dvb_frontend *fe)
 		if (!bw)
 			bw = 6000000;
 		/* fall to OFDM handling */
+		/* fall through */
 	case SYS_DMBTH:
 	case SYS_DVBT:
 	case SYS_DVBT2:
diff --git a/drivers/media/usb/au0828/au0828-dvb.c b/drivers/media/usb/au0828/au0828-dvb.c
index 7e0c9b795e52..34dc7e062471 100644
--- a/drivers/media/usb/au0828/au0828-dvb.c
+++ b/drivers/media/usb/au0828/au0828-dvb.c
@@ -105,6 +105,15 @@ static struct tda18271_config hauppauge_woodbury_tunerconfig = {
 
 static void au0828_restart_dvb_streaming(struct work_struct *work);
 
+static void au0828_bulk_timeout(unsigned long data)
+{
+	struct au0828_dev *dev = (struct au0828_dev *) data;
+
+	dprintk(1, "%s called\n", __func__);
+	dev->bulk_timeout_running = 0;
+	schedule_work(&dev->restart_streaming);
+}
+
 /*-------------------------------------------------------------------*/
 static void urb_completion(struct urb *purb)
 {
@@ -138,6 +147,13 @@ static void urb_completion(struct urb *purb)
 			ptr[0], purb->actual_length);
 		schedule_work(&dev->restart_streaming);
 		return;
+	} else if (dev->bulk_timeout_running == 1) {
+		/* The URB handler has fired, so cancel timer which would
+		 * restart endpoint if we hadn't
+		 */
+		dprintk(1, "%s cancelling bulk timeout\n", __func__);
+		dev->bulk_timeout_running = 0;
+		del_timer(&dev->bulk_timeout);
 	}
 
 	/* Feed the transport payload into the kernel demux */
@@ -160,6 +176,11 @@ static int stop_urb_transfer(struct au0828_dev *dev)
 	if (!dev->urb_streaming)
 		return 0;
 
+	if (dev->bulk_timeout_running == 1) {
+		dev->bulk_timeout_running = 0;
+		del_timer(&dev->bulk_timeout);
+	}
+
 	dev->urb_streaming = false;
 	for (i = 0; i < URB_COUNT; i++) {
 		if (dev->urbs[i]) {
@@ -232,6 +253,11 @@ static int start_urb_transfer(struct au0828_dev *dev)
 	}
 
 	dev->urb_streaming = true;
+
+	/* If we don't valid data within 1 second, restart stream */
+	mod_timer(&dev->bulk_timeout, jiffies + (HZ));
+	dev->bulk_timeout_running = 1;
+
 	return 0;
 }
 
@@ -622,6 +648,10 @@ int au0828_dvb_register(struct au0828_dev *dev)
 		return ret;
 	}
 
+	dev->bulk_timeout.function = au0828_bulk_timeout;
+	dev->bulk_timeout.data = (unsigned long) dev;
+	init_timer(&dev->bulk_timeout);
+
 	return 0;
 }
 
diff --git a/drivers/media/usb/au0828/au0828.h b/drivers/media/usb/au0828/au0828.h
index 88e59748ebc2..05e445fe0b77 100644
--- a/drivers/media/usb/au0828/au0828.h
+++ b/drivers/media/usb/au0828/au0828.h
@@ -195,6 +195,8 @@ struct au0828_dev {
 	/* Digital */
 	struct au0828_dvb		dvb;
 	struct work_struct              restart_streaming;
+	struct timer_list               bulk_timeout;
+	int                             bulk_timeout_running;
 
 #ifdef CONFIG_VIDEO_AU0828_V4L2
 	/* Analog */
diff --git a/drivers/media/usb/cpia2/cpia2_core.c b/drivers/media/usb/cpia2/cpia2_core.c
index b1d13444ff30..0efba0da0a45 100644
--- a/drivers/media/usb/cpia2/cpia2_core.c
+++ b/drivers/media/usb/cpia2/cpia2_core.c
@@ -173,7 +173,8 @@ int cpia2_do_command(struct camera_data *cam,
 		cmd.start = CPIA2_VP_DEVICEH;
 		break;
 	case CPIA2_CMD_SET_VP_BRIGHTNESS:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_VP_BRIGHTNESS:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
 		cmd.reg_count = 1;
@@ -183,14 +184,16 @@ int cpia2_do_command(struct camera_data *cam,
 			cmd.start = CPIA2_VP5_EXPOSURE_TARGET;
 		break;
 	case CPIA2_CMD_SET_CONTRAST:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_CONTRAST:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
 		cmd.reg_count = 1;
 		cmd.start = CPIA2_VP_YRANGE;
 		break;
 	case CPIA2_CMD_SET_VP_SATURATION:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_VP_SATURATION:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
 		cmd.reg_count = 1;
@@ -200,28 +203,32 @@ int cpia2_do_command(struct camera_data *cam,
 			cmd.start = CPIA2_VP5_MCUVSATURATION;
 		break;
 	case CPIA2_CMD_SET_VP_GPIO_DATA:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_VP_GPIO_DATA:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
 		cmd.reg_count = 1;
 		cmd.start = CPIA2_VP_GPIO_DATA;
 		break;
 	case CPIA2_CMD_SET_VP_GPIO_DIRECTION:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_VP_GPIO_DIRECTION:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
 		cmd.reg_count = 1;
 		cmd.start = CPIA2_VP_GPIO_DIRECTION;
 		break;
 	case CPIA2_CMD_SET_VC_MP_GPIO_DATA:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_VC_MP_GPIO_DATA:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
 		cmd.reg_count = 1;
 		cmd.start = CPIA2_VC_MP_DATA;
 		break;
 	case CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/*fall through */
 	case CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
 		cmd.reg_count = 1;
@@ -235,7 +242,8 @@ int cpia2_do_command(struct camera_data *cam,
 		cmd.buffer.block_data[0] = param;
 		break;
 	case CPIA2_CMD_SET_FLICKER_MODES:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_FLICKER_MODES:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
 		cmd.reg_count = 1;
@@ -280,8 +288,9 @@ int cpia2_do_command(struct camera_data *cam,
 		cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
 		cmd.buffer.block_data[0] = CPIA2_SYSTEM_CONTROL_CLEAR_ERR;
 		break;
-	case CPIA2_CMD_SET_USER_MODE:   /* Then fall through */
+	case CPIA2_CMD_SET_USER_MODE:
 		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_USER_MODE:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
 		cmd.reg_count = 1;
@@ -300,14 +309,16 @@ int cpia2_do_command(struct camera_data *cam,
 		cmd.buffer.block_data[0] = param;
 		break;
 	case CPIA2_CMD_SET_WAKEUP:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_WAKEUP:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
 		cmd.reg_count = 1;
 		cmd.start = CPIA2_VC_WAKEUP;
 		break;
 	case CPIA2_CMD_SET_PW_CONTROL:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_PW_CONTROL:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
 		cmd.reg_count = 1;
@@ -319,7 +330,8 @@ int cpia2_do_command(struct camera_data *cam,
 		cmd.start = CPIA2_VP_SYSTEMSTATE;
 		break;
 	case CPIA2_CMD_SET_SYSTEM_CTRL:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_SYSTEM_CTRL:
 		cmd.req_mode =
 		    CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
@@ -327,21 +339,24 @@ int cpia2_do_command(struct camera_data *cam,
 		cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
 		break;
 	case CPIA2_CMD_SET_VP_SYSTEM_CTRL:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_VP_SYSTEM_CTRL:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
 		cmd.reg_count = 1;
 		cmd.start = CPIA2_VP_SYSTEMCTRL;
 		break;
 	case CPIA2_CMD_SET_VP_EXP_MODES:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_VP_EXP_MODES:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
 		cmd.reg_count = 1;
 		cmd.start = CPIA2_VP_EXPOSURE_MODES;
 		break;
 	case CPIA2_CMD_SET_DEVICE_CONFIG:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_DEVICE_CONFIG:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
 		cmd.reg_count = 1;
@@ -361,7 +376,8 @@ int cpia2_do_command(struct camera_data *cam,
 		cmd.start = CPIA2_SENSOR_CR1;
 		break;
 	case CPIA2_CMD_SET_VC_CONTROL:
-		cmd.buffer.block_data[0] = param;	/* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_VC_CONTROL:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
 		cmd.reg_count = 1;
@@ -395,7 +411,8 @@ int cpia2_do_command(struct camera_data *cam,
 	case CPIA2_CMD_SET_USER_EFFECTS:  /* Note: Be careful with this as
 					     this register can also affect
 					     flicker modes */
-		cmd.buffer.block_data[0] = param;      /* Then fall through */
+		cmd.buffer.block_data[0] = param;
+		/* fall through */
 	case CPIA2_CMD_GET_USER_EFFECTS:
 		cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
 		cmd.reg_count = 1;
diff --git a/drivers/media/usb/cx231xx/Kconfig b/drivers/media/usb/cx231xx/Kconfig
index 58de80bff4c7..6276d9b2198b 100644
--- a/drivers/media/usb/cx231xx/Kconfig
+++ b/drivers/media/usb/cx231xx/Kconfig
@@ -52,6 +52,8 @@ config VIDEO_CX231XX_DVB
 	select DVB_SI2165 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_SI2168 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_SI2157 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_MN88473 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_R820T if MEDIA_SUBDRV_AUTOSELECT
 
 	---help---
 	  This adds support for DVB cards based on the
diff --git a/drivers/media/usb/cx231xx/cx231xx-cards.c b/drivers/media/usb/cx231xx/cx231xx-cards.c
index a1007d005290..e0daa9b6c2a0 100644
--- a/drivers/media/usb/cx231xx/cx231xx-cards.c
+++ b/drivers/media/usb/cx231xx/cx231xx-cards.c
@@ -868,6 +868,33 @@ struct cx231xx_board cx231xx_boards[] = {
 			.amux = CX231XX_AMUX_LINE_IN,
 		} },
 	},
+	[CX231XX_BOARD_ASTROMETA_T2HYBRID] = {
+		.name = "Astrometa T2hybrid",
+		.tuner_type = TUNER_ABSENT,
+		.has_dvb = 1,
+		.output_mode = OUT_MODE_VIP11,
+		.agc_analog_digital_select_gpio = 0x01,
+		.ctl_pin_status_mask = 0xffffffc4,
+		.demod_addr = 0x18, /* 0x30 >> 1 */
+		.demod_i2c_master = I2C_1_MUX_1,
+		.gpio_pin_status_mask = 0xa,
+		.norm = V4L2_STD_NTSC,
+		.tuner_addr = 0x3a, /* 0x74 >> 1 */
+		.tuner_i2c_master = I2C_1_MUX_3,
+		.tuner_scl_gpio = 0x1a,
+		.tuner_sda_gpio = 0x1b,
+		.tuner_sif_gpio = 0x05,
+		.input = {{
+				.type = CX231XX_VMUX_TELEVISION,
+				.vmux = CX231XX_VIN_1_1,
+				.amux = CX231XX_AMUX_VIDEO,
+			}, {
+				.type = CX231XX_VMUX_COMPOSITE1,
+				.vmux = CX231XX_VIN_2_1,
+				.amux = CX231XX_AMUX_LINE_IN,
+			},
+		},
+	},
 };
 const unsigned int cx231xx_bcount = ARRAY_SIZE(cx231xx_boards);
 
@@ -937,6 +964,8 @@ struct usb_device_id cx231xx_id_table[] = {
 	 .driver_info = CX231XX_BOARD_TERRATEC_GRABBY},
 	{USB_DEVICE(0x1b80, 0xd3b2),
 	.driver_info = CX231XX_BOARD_EVROMEDIA_FULL_HYBRID_FULLHD},
+	{USB_DEVICE(0x15f4, 0x0135),
+	.driver_info = CX231XX_BOARD_ASTROMETA_T2HYBRID},
 	{},
 };
 
@@ -1013,6 +1042,11 @@ void cx231xx_pre_card_setup(struct cx231xx *dev)
 	dev_info(dev->dev, "Identified as %s (card=%d)\n",
 		dev->board.name, dev->model);
 
+	if (CX231XX_BOARD_ASTROMETA_T2HYBRID == dev->model) {
+		/* turn on demodulator chip */
+		cx231xx_set_gpio_value(dev, 0x03, 0x01);
+	}
+
 	/* set the direction for GPIO pins */
 	if (dev->board.tuner_gpio) {
 		cx231xx_set_gpio_direction(dev, dev->board.tuner_gpio->bit, 1);
diff --git a/drivers/media/usb/cx231xx/cx231xx-dvb.c b/drivers/media/usb/cx231xx/cx231xx-dvb.c
index 46427fd3b220..ee3eeeb600f8 100644
--- a/drivers/media/usb/cx231xx/cx231xx-dvb.c
+++ b/drivers/media/usb/cx231xx/cx231xx-dvb.c
@@ -37,6 +37,8 @@
 #include "mb86a20s.h"
 #include "si2157.h"
 #include "lgdt3306a.h"
+#include "r820t.h"
+#include "mn88473.h"
 
 MODULE_DESCRIPTION("driver for cx231xx based DVB cards");
 MODULE_AUTHOR("Srinivasa Deevi <srinivasa.deevi@conexant.com>");
@@ -164,6 +166,13 @@ static struct lgdt3306a_config hauppauge_955q_lgdt3306a_config = {
 	.xtalMHz            = 25,
 };
 
+static struct r820t_config astrometa_t2hybrid_r820t_config = {
+	.i2c_addr		= 0x3a, /* 0x74 >> 1 */
+	.xtal			= 16000000,
+	.rafael_chip		= CHIP_R828D,
+	.max_i2c_msg_len	= 2,
+};
+
 static inline void print_err_status(struct cx231xx *dev, int packet, int status)
 {
 	char *errmsg = "Unknown";
@@ -1019,6 +1028,46 @@ static int dvb_init(struct cx231xx *dev)
 		dev->dvb->i2c_client_tuner = client;
 		break;
 	}
+	case CX231XX_BOARD_ASTROMETA_T2HYBRID:
+	{
+		struct i2c_client *client;
+		struct i2c_board_info info = {};
+		struct mn88473_config mn88473_config = {};
+
+		/* attach demodulator chip */
+		mn88473_config.i2c_wr_max = 16;
+		mn88473_config.xtal = 25000000;
+		mn88473_config.fe = &dev->dvb->frontend;
+
+		strlcpy(info.type, "mn88473", sizeof(info.type));
+		info.addr = dev->board.demod_addr;
+		info.platform_data = &mn88473_config;
+
+		request_module(info.type);
+		client = i2c_new_device(demod_i2c, &info);
+
+		if (client == NULL || client->dev.driver == NULL) {
+			result = -ENODEV;
+			goto out_free;
+		}
+
+		if (!try_module_get(client->dev.driver->owner)) {
+			i2c_unregister_device(client);
+			result = -ENODEV;
+			goto out_free;
+		}
+
+		dvb->i2c_client_demod = client;
+
+		/* define general-purpose callback pointer */
+		dvb->frontend->callback = cx231xx_tuner_callback;
+
+		/* attach tuner chip */
+		dvb_attach(r820t_attach, dev->dvb->frontend,
+			   tuner_i2c,
+			   &astrometa_t2hybrid_r820t_config);
+		break;
+	}
 	default:
 		dev_err(dev->dev,
 			"%s/2: The frontend of your DVB/ATSC card isn't supported yet\n",
diff --git a/drivers/media/usb/cx231xx/cx231xx-input.c b/drivers/media/usb/cx231xx/cx231xx-input.c
index 6e80f3c573f3..eecf074b0a48 100644
--- a/drivers/media/usb/cx231xx/cx231xx-input.c
+++ b/drivers/media/usb/cx231xx/cx231xx-input.c
@@ -30,7 +30,7 @@ static int get_key_isdbt(struct IR_i2c *ir, enum rc_type *protocol,
 	int	rc;
 	u8	cmd, scancode;
 
-	dev_dbg(&ir->rc->input_dev->dev, "%s\n", __func__);
+	dev_dbg(&ir->rc->dev, "%s\n", __func__);
 
 		/* poll IR chip */
 	rc = i2c_master_recv(ir->c, &cmd, 1);
@@ -48,8 +48,7 @@ static int get_key_isdbt(struct IR_i2c *ir, enum rc_type *protocol,
 
 	scancode = bitrev8(cmd);
 
-	dev_dbg(&ir->rc->input_dev->dev, "cmd %02x, scan = %02x\n",
-		cmd, scancode);
+	dev_dbg(&ir->rc->dev, "cmd %02x, scan = %02x\n", cmd, scancode);
 
 	*protocol = RC_TYPE_OTHER;
 	*pscancode = scancode;
diff --git a/drivers/media/usb/cx231xx/cx231xx-video.c b/drivers/media/usb/cx231xx/cx231xx-video.c
index 6414188ffdfa..f67f86876625 100644
--- a/drivers/media/usb/cx231xx/cx231xx-video.c
+++ b/drivers/media/usb/cx231xx/cx231xx-video.c
@@ -1134,7 +1134,7 @@ void cx231xx_v4l2_create_entities(struct cx231xx *dev)
 			/* The DVB core will handle it */
 			if (dev->tuner_type == TUNER_ABSENT)
 				continue;
-			/* fall though */
+			/* fall through */
 		default: /* just to shut up a gcc warning */
 			ent->function = MEDIA_ENT_F_CONN_RF;
 			break;
diff --git a/drivers/media/usb/cx231xx/cx231xx.h b/drivers/media/usb/cx231xx/cx231xx.h
index d9792ea4bbc6..986c64ba5b56 100644
--- a/drivers/media/usb/cx231xx/cx231xx.h
+++ b/drivers/media/usb/cx231xx/cx231xx.h
@@ -79,6 +79,7 @@
 #define CX231XX_BOARD_HAUPPAUGE_955Q 21
 #define CX231XX_BOARD_TERRATEC_GRABBY 22
 #define CX231XX_BOARD_EVROMEDIA_FULL_HYBRID_FULLHD 23
+#define CX231XX_BOARD_ASTROMETA_T2HYBRID 24
 
 /* Limits minimum and default number of buffers */
 #define CX231XX_MIN_BUF                 4
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.c b/drivers/media/usb/dvb-usb-v2/af9015.c
index caa1e6101f58..23bbbf367b51 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.c
+++ b/drivers/media/usb/dvb-usb-v2/af9015.c
@@ -36,7 +36,7 @@ static int af9015_ctrl_msg(struct dvb_usb_device *d, struct req_t *req)
 
 	state->buf[0] = req->cmd;
 	state->buf[1] = state->seq++;
-	state->buf[2] = req->i2c_addr;
+	state->buf[2] = req->i2c_addr << 1;
 	state->buf[3] = req->addr >> 8;
 	state->buf[4] = req->addr & 0xff;
 	state->buf[5] = req->mbox;
@@ -52,6 +52,7 @@ static int af9015_ctrl_msg(struct dvb_usb_device *d, struct req_t *req)
 	case READ_I2C:
 		write = 0;
 		state->buf[2] |= 0x01; /* set I2C direction */
+		/* fall through */
 	case WRITE_I2C:
 		state->buf[0] = READ_WRITE_I2C;
 		break;
@@ -205,9 +206,9 @@ static int af9015_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
 	struct af9015_state *state = d_to_priv(d);
-	int ret = 0, i = 0;
+	int ret;
 	u16 addr;
-	u8 uninitialized_var(mbox), addr_len;
+	u8 mbox, addr_len;
 	struct req_t req;
 
 /*
@@ -232,84 +233,89 @@ Due to that the only way to select correct tuner is use demodulator I2C-gate.
 				| addr 0x3a  |                 |  addr 0xc6 |
 				|____________|                 |____________|
 */
-	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
-		return -EAGAIN;
 
-	while (i < num) {
-		if (msg[i].addr == state->af9013_config[0].i2c_addr ||
-		    msg[i].addr == state->af9013_config[1].i2c_addr) {
-			addr = msg[i].buf[0] << 8;
-			addr += msg[i].buf[1];
-			mbox = msg[i].buf[2];
-			addr_len = 3;
-		} else {
-			addr = msg[i].buf[0];
-			addr_len = 1;
-			/* mbox is don't care in that case */
-		}
+	if (msg[0].len == 0 || msg[0].flags & I2C_M_RD) {
+		addr = 0x0000;
+		mbox = 0;
+		addr_len = 0;
+	} else if (msg[0].len == 1) {
+		addr = msg[0].buf[0];
+		mbox = 0;
+		addr_len = 1;
+	} else if (msg[0].len == 2) {
+		addr = msg[0].buf[0] << 8|msg[0].buf[1] << 0;
+		mbox = 0;
+		addr_len = 2;
+	} else {
+		addr = msg[0].buf[0] << 8|msg[0].buf[1] << 0;
+		mbox = msg[0].buf[2];
+		addr_len = 3;
+	}
 
-		if (num > i + 1 && (msg[i+1].flags & I2C_M_RD)) {
-			if (msg[i].len > 3 || msg[i+1].len > 61) {
-				ret = -EOPNOTSUPP;
-				goto error;
-			}
-			if (msg[i].addr == state->af9013_config[0].i2c_addr)
-				req.cmd = READ_MEMORY;
-			else
-				req.cmd = READ_I2C;
-			req.i2c_addr = msg[i].addr;
-			req.addr = addr;
-			req.mbox = mbox;
-			req.addr_len = addr_len;
-			req.data_len = msg[i+1].len;
-			req.data = &msg[i+1].buf[0];
-			ret = af9015_ctrl_msg(d, &req);
-			i += 2;
-		} else if (msg[i].flags & I2C_M_RD) {
-			if (msg[i].len > 61) {
-				ret = -EOPNOTSUPP;
-				goto error;
-			}
-			if (msg[i].addr == state->af9013_config[0].i2c_addr) {
-				ret = -EINVAL;
-				goto error;
-			}
+	if (num == 1 && !(msg[0].flags & I2C_M_RD)) {
+		/* i2c write */
+		if (msg[0].len > 21) {
+			ret = -EOPNOTSUPP;
+			goto err;
+		}
+		if (msg[0].addr == state->af9013_config[0].i2c_addr)
+			req.cmd = WRITE_MEMORY;
+		else
+			req.cmd = WRITE_I2C;
+		req.i2c_addr = msg[0].addr;
+		req.addr = addr;
+		req.mbox = mbox;
+		req.addr_len = addr_len;
+		req.data_len = msg[0].len-addr_len;
+		req.data = &msg[0].buf[addr_len];
+		ret = af9015_ctrl_msg(d, &req);
+	} else if (num == 2 && !(msg[0].flags & I2C_M_RD) &&
+		   (msg[1].flags & I2C_M_RD)) {
+		/* i2c write + read */
+		if (msg[0].len > 3 || msg[1].len > 61) {
+			ret = -EOPNOTSUPP;
+			goto err;
+		}
+		if (msg[0].addr == state->af9013_config[0].i2c_addr)
+			req.cmd = READ_MEMORY;
+		else
 			req.cmd = READ_I2C;
-			req.i2c_addr = msg[i].addr;
-			req.addr = addr;
-			req.mbox = mbox;
-			req.addr_len = addr_len;
-			req.data_len = msg[i].len;
-			req.data = &msg[i].buf[0];
-			ret = af9015_ctrl_msg(d, &req);
-			i += 1;
-		} else {
-			if (msg[i].len > 21) {
-				ret = -EOPNOTSUPP;
-				goto error;
-			}
-			if (msg[i].addr == state->af9013_config[0].i2c_addr)
-				req.cmd = WRITE_MEMORY;
-			else
-				req.cmd = WRITE_I2C;
-			req.i2c_addr = msg[i].addr;
-			req.addr = addr;
-			req.mbox = mbox;
-			req.addr_len = addr_len;
-			req.data_len = msg[i].len-addr_len;
-			req.data = &msg[i].buf[addr_len];
-			ret = af9015_ctrl_msg(d, &req);
-			i += 1;
+		req.i2c_addr = msg[0].addr;
+		req.addr = addr;
+		req.mbox = mbox;
+		req.addr_len = addr_len;
+		req.data_len = msg[1].len;
+		req.data = &msg[1].buf[0];
+		ret = af9015_ctrl_msg(d, &req);
+	} else if (num == 1 && (msg[0].flags & I2C_M_RD)) {
+		/* i2c read */
+		if (msg[0].len > 61) {
+			ret = -EOPNOTSUPP;
+			goto err;
 		}
-		if (ret)
-			goto error;
-
+		if (msg[0].addr == state->af9013_config[0].i2c_addr) {
+			ret = -EINVAL;
+			goto err;
+		}
+		req.cmd = READ_I2C;
+		req.i2c_addr = msg[0].addr;
+		req.addr = addr;
+		req.mbox = mbox;
+		req.addr_len = addr_len;
+		req.data_len = msg[0].len;
+		req.data = &msg[0].buf[0];
+		ret = af9015_ctrl_msg(d, &req);
+	} else {
+		ret = -EOPNOTSUPP;
+		dev_dbg(&d->udev->dev, "%s: unknown msg, num %u\n",
+			__func__, num);
 	}
-	ret = i;
-
-error:
-	mutex_unlock(&d->i2c_mutex);
+	if (ret)
+		goto err;
 
+	return num;
+err:
+	dev_dbg(&d->udev->dev, "%s: failed %d\n", __func__, ret);
 	return ret;
 }
 
@@ -471,6 +477,8 @@ static int af9015_read_config(struct dvb_usb_device *d)
 	if (d->udev->speed == USB_SPEED_FULL)
 		state->dual_mode = 0;
 
+	state->af9013_config[0].i2c_addr = AF9015_I2C_DEMOD;
+
 	if (state->dual_mode) {
 		/* read 2nd demodulator I2C address */
 		req.addr = AF9015_EEPROM_DEMOD2_I2C;
@@ -478,7 +486,7 @@ static int af9015_read_config(struct dvb_usb_device *d)
 		if (ret)
 			goto error;
 
-		state->af9013_config[1].i2c_addr = val;
+		state->af9013_config[1].i2c_addr = val >> 1;
 	}
 
 	for (i = 0; i < state->dual_mode + 1; i++) {
@@ -733,9 +741,6 @@ static int af9015_copy_firmware(struct dvb_usb_device *d)
 	fw_params[2] = state->firmware_checksum >> 8;
 	fw_params[3] = state->firmware_checksum & 0xff;
 
-	/* wait 2nd demodulator ready */
-	msleep(100);
-
 	ret = af9015_read_reg_i2c(d, state->af9013_config[1].i2c_addr,
 			0x98be, &val);
 	if (ret)
@@ -823,6 +828,9 @@ static int af9015_af9013_frontend_attach(struct dvb_usb_adapter *adap)
 
 		/* copy firmware to 2nd demodulator */
 		if (state->dual_mode) {
+			/* Wait 2nd demodulator ready */
+			msleep(100);
+
 			ret = af9015_copy_firmware(adap_to_d(adap));
 			if (ret) {
 				dev_err(&adap_to_d(adap)->udev->dev,
@@ -870,12 +878,12 @@ static int af9015_af9013_frontend_attach(struct dvb_usb_adapter *adap)
 }
 
 static struct mt2060_config af9015_mt2060_config = {
-	.i2c_address = 0xc0,
+	.i2c_address = 0x60,
 	.clock_out = 0,
 };
 
 static struct qt1010_config af9015_qt1010_config = {
-	.i2c_address = 0xc4,
+	.i2c_address = 0x62,
 };
 
 static struct tda18271_config af9015_tda18271_config = {
@@ -884,7 +892,7 @@ static struct tda18271_config af9015_tda18271_config = {
 };
 
 static struct mxl5005s_config af9015_mxl5003_config = {
-	.i2c_address     = 0xc6,
+	.i2c_address     = 0x63,
 	.if_freq         = IF_FREQ_4570000HZ,
 	.xtal_freq       = CRYSTAL_FREQ_16000000HZ,
 	.agc_mode        = MXL_SINGLE_AGC,
@@ -901,7 +909,7 @@ static struct mxl5005s_config af9015_mxl5003_config = {
 };
 
 static struct mxl5005s_config af9015_mxl5005_config = {
-	.i2c_address     = 0xc6,
+	.i2c_address     = 0x63,
 	.if_freq         = IF_FREQ_4570000HZ,
 	.xtal_freq       = CRYSTAL_FREQ_16000000HZ,
 	.agc_mode        = MXL_SINGLE_AGC,
@@ -918,12 +926,12 @@ static struct mxl5005s_config af9015_mxl5005_config = {
 };
 
 static struct mc44s803_config af9015_mc44s803_config = {
-	.i2c_address = 0xc0,
+	.i2c_address = 0x60,
 	.dig_out = 1,
 };
 
 static struct tda18218_config af9015_tda18218_config = {
-	.i2c_address = 0xc0,
+	.i2c_address = 0x60,
 	.i2c_wr_max = 21, /* max wr bytes AF9015 I2C adap can handle at once */
 };
 
@@ -954,7 +962,7 @@ static int af9015_tuner_attach(struct dvb_usb_adapter *adap)
 			&af9015_qt1010_config) == NULL ? -ENODEV : 0;
 		break;
 	case AF9013_TUNER_TDA18271:
-		ret = dvb_attach(tda18271_attach, adap->fe[0], 0xc0,
+		ret = dvb_attach(tda18271_attach, adap->fe[0], 0x60,
 			&adap_to_d(adap)->i2c_adap,
 			&af9015_tda18271_config) == NULL ? -ENODEV : 0;
 		break;
@@ -975,7 +983,7 @@ static int af9015_tuner_attach(struct dvb_usb_adapter *adap)
 			&af9015_mxl5005_config) == NULL ? -ENODEV : 0;
 		break;
 	case AF9013_TUNER_ENV77H11D5:
-		ret = dvb_attach(dvb_pll_attach, adap->fe[0], 0xc0,
+		ret = dvb_attach(dvb_pll_attach, adap->fe[0], 0x60,
 			&adap_to_d(adap)->i2c_adap,
 			DVB_PLL_TDA665X) == NULL ? -ENODEV : 0;
 		break;
@@ -987,7 +995,7 @@ static int af9015_tuner_attach(struct dvb_usb_adapter *adap)
 	case AF9013_TUNER_MXL5007T:
 		ret = dvb_attach(mxl5007t_attach, adap->fe[0],
 			&adap_to_d(adap)->i2c_adap,
-			0xc0, &af9015_mxl5007t_config) == NULL ? -ENODEV : 0;
+			0x60, &af9015_mxl5007t_config) == NULL ? -ENODEV : 0;
 		break;
 	case AF9013_TUNER_UNKNOWN:
 	default:
@@ -1124,10 +1132,21 @@ static int af9015_init_endpoint(struct dvb_usb_device *d)
 	}
 
 	/* enable / disable mp2if2 */
-	if (state->dual_mode)
+	if (state->dual_mode) {
 		ret = af9015_set_reg_bit(d, 0xd50b, 0);
-	else
+		if (ret)
+			goto error;
+		ret = af9015_set_reg_bit(d, 0xd520, 4);
+		if (ret)
+			goto error;
+	} else {
 		ret = af9015_clear_reg_bit(d, 0xd50b, 0);
+		if (ret)
+			goto error;
+		ret = af9015_clear_reg_bit(d, 0xd520, 4);
+		if (ret)
+			goto error;
+	}
 
 error:
 	if (ret)
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.h b/drivers/media/usb/dvb-usb-v2/af9015.h
index 2dd9231a8ece..3a9d9815ab7a 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.h
+++ b/drivers/media/usb/dvb-usb-v2/af9015.h
@@ -47,8 +47,8 @@
 #define TS_USB20_MAX_PACKET_SIZE  512
 #define TS_USB11_MAX_PACKET_SIZE   64
 
-#define AF9015_I2C_EEPROM  0xa0
-#define AF9015_I2C_DEMOD   0x38
+#define AF9015_I2C_EEPROM  0x50
+#define AF9015_I2C_DEMOD   0x1c
 #define AF9015_USB_TIMEOUT 2000
 
 /* EEPROM locations */
diff --git a/drivers/media/usb/dvb-usb-v2/lmedm04.c b/drivers/media/usb/dvb-usb-v2/lmedm04.c
index 924adfdb660d..594360a63c18 100644
--- a/drivers/media/usb/dvb-usb-v2/lmedm04.c
+++ b/drivers/media/usb/dvb-usb-v2/lmedm04.c
@@ -1065,6 +1065,7 @@ static int dm04_lme2510_frontend_attach(struct dvb_usb_adapter *adap)
 			}
 			break;
 		}
+		/* fall through */
 	case 0x22f0:
 		st->i2c_gate = 5;
 		adap->fe[0] = dvb_attach(m88rs2000_attach,
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-i2c.c b/drivers/media/usb/dvb-usb-v2/mxl111sf-i2c.c
index ffb49c28b15a..0eb33e043079 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-i2c.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-i2c.c
@@ -316,7 +316,7 @@ fail:
 static int mxl111sf_i2c_send_data(struct mxl111sf_state *state,
 				  u8 index, u8 *wdata)
 {
-	int ret = mxl111sf_ctrl_msg(state->d, wdata[0],
+	int ret = mxl111sf_ctrl_msg(state, wdata[0],
 				    &wdata[1], 25, NULL, 0);
 	mxl_fail(ret);
 
@@ -326,7 +326,7 @@ static int mxl111sf_i2c_send_data(struct mxl111sf_state *state,
 static int mxl111sf_i2c_get_data(struct mxl111sf_state *state,
 				 u8 index, u8 *wdata, u8 *rdata)
 {
-	int ret = mxl111sf_ctrl_msg(state->d, wdata[0],
+	int ret = mxl111sf_ctrl_msg(state, wdata[0],
 				    &wdata[1], 25, rdata, 24);
 	mxl_fail(ret);
 
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf.c b/drivers/media/usb/dvb-usb-v2/mxl111sf.c
index abf69d8fa469..b0d5904a4ea6 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf.c
@@ -24,9 +24,6 @@
 #include "lgdt3305.h"
 #include "lg2160.h"
 
-/* Max transfer size done by I2C transfer functions */
-#define MAX_XFER_SIZE  64
-
 int dvb_usb_mxl111sf_debug;
 module_param_named(debug, dvb_usb_mxl111sf_debug, int, 0644);
 MODULE_PARM_DESC(debug, "set debugging level (1=info, 2=xfer, 4=i2c, 8=reg, 16=adv (or-able)).");
@@ -55,27 +52,34 @@ MODULE_PARM_DESC(rfswitch, "force rf switch position (0=auto, 1=ext, 2=int).");
 
 DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
-int mxl111sf_ctrl_msg(struct dvb_usb_device *d,
+int mxl111sf_ctrl_msg(struct mxl111sf_state *state,
 		      u8 cmd, u8 *wbuf, int wlen, u8 *rbuf, int rlen)
 {
+	struct dvb_usb_device *d = state->d;
 	int wo = (rbuf == NULL || rlen == 0); /* write-only */
 	int ret;
-	u8 sndbuf[MAX_XFER_SIZE];
 
-	if (1 + wlen > sizeof(sndbuf)) {
+	if (1 + wlen > MXL_MAX_XFER_SIZE) {
 		pr_warn("%s: len=%d is too big!\n", __func__, wlen);
 		return -EOPNOTSUPP;
 	}
 
 	pr_debug("%s(wlen = %d, rlen = %d)\n", __func__, wlen, rlen);
 
-	memset(sndbuf, 0, 1+wlen);
+	mutex_lock(&state->msg_lock);
+	memset(state->sndbuf, 0, 1+wlen);
+	memset(state->rcvbuf, 0, rlen);
+
+	state->sndbuf[0] = cmd;
+	memcpy(&state->sndbuf[1], wbuf, wlen);
 
-	sndbuf[0] = cmd;
-	memcpy(&sndbuf[1], wbuf, wlen);
+	ret = (wo) ? dvb_usbv2_generic_write(d, state->sndbuf, 1+wlen) :
+		dvb_usbv2_generic_rw(d, state->sndbuf, 1+wlen, state->rcvbuf,
+				     rlen);
+
+	memcpy(rbuf, state->rcvbuf, rlen);
+	mutex_unlock(&state->msg_lock);
 
-	ret = (wo) ? dvb_usbv2_generic_write(d, sndbuf, 1+wlen) :
-		dvb_usbv2_generic_rw(d, sndbuf, 1+wlen, rbuf, rlen);
 	mxl_fail(ret);
 
 	return ret;
@@ -91,7 +95,7 @@ int mxl111sf_read_reg(struct mxl111sf_state *state, u8 addr, u8 *data)
 	u8 buf[2];
 	int ret;
 
-	ret = mxl111sf_ctrl_msg(state->d, MXL_CMD_REG_READ, &addr, 1, buf, 2);
+	ret = mxl111sf_ctrl_msg(state, MXL_CMD_REG_READ, &addr, 1, buf, 2);
 	if (mxl_fail(ret)) {
 		mxl_debug("error reading reg: 0x%02x", addr);
 		goto fail;
@@ -117,7 +121,7 @@ int mxl111sf_write_reg(struct mxl111sf_state *state, u8 addr, u8 data)
 
 	pr_debug("W: (0x%02x, 0x%02x)\n", addr, data);
 
-	ret = mxl111sf_ctrl_msg(state->d, MXL_CMD_REG_WRITE, buf, 2, NULL, 0);
+	ret = mxl111sf_ctrl_msg(state, MXL_CMD_REG_WRITE, buf, 2, NULL, 0);
 	if (mxl_fail(ret))
 		pr_err("error writing reg: 0x%02x, val: 0x%02x", addr, data);
 	return ret;
@@ -926,6 +930,8 @@ static int mxl111sf_init(struct dvb_usb_device *d)
 		  .len = sizeof(eeprom), .buf = eeprom },
 	};
 
+	mutex_init(&state->msg_lock);
+
 	ret = get_chip_info(state);
 	if (mxl_fail(ret))
 		pr_err("failed to get chip info during probe");
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf.h b/drivers/media/usb/dvb-usb-v2/mxl111sf.h
index 846260e0eec0..3e6f5880bd1e 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf.h
@@ -19,6 +19,9 @@
 #include <media/tveeprom.h>
 #include <media/media-entity.h>
 
+/* Max transfer size done by I2C transfer functions */
+#define MXL_MAX_XFER_SIZE  64
+
 #define MXL_EP1_REG_READ     1
 #define MXL_EP2_REG_WRITE    2
 #define MXL_EP3_INTERRUPT    3
@@ -86,6 +89,9 @@ struct mxl111sf_state {
 	struct mutex fe_lock;
 	u8 num_frontends;
 	struct mxl111sf_adap_state adap_state[3];
+	u8 sndbuf[MXL_MAX_XFER_SIZE];
+	u8 rcvbuf[MXL_MAX_XFER_SIZE];
+	struct mutex msg_lock;
 #ifdef CONFIG_MEDIA_CONTROLLER_DVB
 	struct media_entity tuner;
 	struct media_pad tuner_pads[2];
@@ -108,7 +114,7 @@ int mxl111sf_ctrl_program_regs(struct mxl111sf_state *state,
 
 /* needed for hardware i2c functions in mxl111sf-i2c.c:
  * mxl111sf_i2c_send_data / mxl111sf_i2c_get_data */
-int mxl111sf_ctrl_msg(struct dvb_usb_device *d,
+int mxl111sf_ctrl_msg(struct mxl111sf_state *state,
 		      u8 cmd, u8 *wbuf, int wlen, u8 *rbuf, int rlen);
 
 #define mxl_printk(kern, fmt, arg...) \
diff --git a/drivers/media/usb/dvb-usb/dib0700_devices.c b/drivers/media/usb/dvb-usb/dib0700_devices.c
index 85ab3fa48f9a..6a57fc6d3472 100644
--- a/drivers/media/usb/dvb-usb/dib0700_devices.c
+++ b/drivers/media/usb/dvb-usb/dib0700_devices.c
@@ -1659,6 +1659,7 @@ static int dib8096_set_param_override(struct dvb_frontend *fe)
 	switch (band) {
 	default:
 			deb_info("Warning : Rf frequency  (%iHz) is not in the supported range, using VHF switch ", fe->dtv_property_cache.frequency);
+			/* fall through */
 	case BAND_VHF:
 			state->dib8000_ops.set_gpio(fe, 3, 0, 1);
 			break;
diff --git a/drivers/media/usb/dvb-usb/dvb-usb-remote.c b/drivers/media/usb/dvb-usb/dvb-usb-remote.c
index 059ded59208e..f05f1fc80729 100644
--- a/drivers/media/usb/dvb-usb/dvb-usb-remote.c
+++ b/drivers/media/usb/dvb-usb/dvb-usb-remote.c
@@ -131,6 +131,11 @@ static void legacy_dvb_usb_read_remote_control(struct work_struct *work)
 		case REMOTE_KEY_PRESSED:
 			deb_rc("key pressed\n");
 			d->last_event = event;
+			input_event(d->input_dev, EV_KEY, event, 1);
+			input_sync(d->input_dev);
+			input_event(d->input_dev, EV_KEY, d->last_event, 0);
+			input_sync(d->input_dev);
+			break;
 		case REMOTE_KEY_REPEAT:
 			deb_rc("key repeated\n");
 			input_event(d->input_dev, EV_KEY, event, 1);
diff --git a/drivers/media/usb/dvb-usb/dw2102.c b/drivers/media/usb/dvb-usb/dw2102.c
index 6e654e5026dd..57b187240110 100644
--- a/drivers/media/usb/dvb-usb/dw2102.c
+++ b/drivers/media/usb/dvb-usb/dw2102.c
@@ -1840,11 +1840,12 @@ static int dw2102_load_firmware(struct usb_device *dev,
 		switch (le16_to_cpu(dev->descriptor.idProduct)) {
 		case USB_PID_TEVII_S650:
 			dw2104_properties.rc.core.rc_codes = RC_MAP_TEVII_NEC;
+			/* fall through */
 		case USB_PID_DW2104:
 			reset = 1;
 			dw210x_op_rw(dev, 0xc4, 0x0000, 0, &reset, 1,
 					DW210X_WRITE_MSG);
-			/* break omitted intentionally */
+			/* fall through */
 		case USB_PID_DW3101:
 			reset = 0;
 			dw210x_op_rw(dev, 0xbf, 0x0040, 0, &reset, 0,
@@ -1877,6 +1878,7 @@ static int dw2102_load_firmware(struct usb_device *dev,
 					break;
 				}
 			}
+			/* fall through */
 		case 0x2101:
 			dw210x_op_rw(dev, 0xbc, 0x0030, 0, &reset16[0], 2,
 					DW210X_READ_MSG);
diff --git a/drivers/media/usb/em28xx/em28xx-cards.c b/drivers/media/usb/em28xx/em28xx-cards.c
index a12b599a1fa2..146341aeb782 100644
--- a/drivers/media/usb/em28xx/em28xx-cards.c
+++ b/drivers/media/usb/em28xx/em28xx-cards.c
@@ -2855,7 +2855,7 @@ static int em28xx_hint_board(struct em28xx *dev)
 				"Your board has no unique USB ID.\n"
 				"A hint were successfully done, based on eeprom hash.\n"
 				"This method is not 100%% failproof.\n"
-				"If the board were missdetected, please email this log to:\n"
+				"If the board were misdetected, please email this log to:\n"
 				"\tV4L Mailing List  <linux-media@vger.kernel.org>\n"
 				"Board detected as %s\n",
 			       em28xx_boards[dev->model].name);
@@ -2885,7 +2885,7 @@ static int em28xx_hint_board(struct em28xx *dev)
 				"Your board has no unique USB ID.\n"
 				"A hint were successfully done, based on i2c devicelist hash.\n"
 				"This method is not 100%% failproof.\n"
-				"If the board were missdetected, please email this log to:\n"
+				"If the board were misdetected, please email this log to:\n"
 				"\tV4L Mailing List  <linux-media@vger.kernel.org>\n"
 				"Board detected as %s\n",
 				em28xx_boards[dev->model].name);
diff --git a/drivers/media/usb/em28xx/em28xx-core.c b/drivers/media/usb/em28xx/em28xx-core.c
index 19ccff41c7eb..1d0d8cc06103 100644
--- a/drivers/media/usb/em28xx/em28xx-core.c
+++ b/drivers/media/usb/em28xx/em28xx-core.c
@@ -91,22 +91,16 @@ int em28xx_read_reg_req_len(struct em28xx *dev, u8 req, u16 reg,
 	if (len > URB_MAX_CTRL_SIZE)
 		return -EINVAL;
 
-	em28xx_regdbg("(pipe 0x%08x): IN:  %02x %02x %02x %02x %02x %02x %02x %02x ",
-		     pipe, USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-		     req, 0, 0,
-		     reg & 0xff, reg >> 8,
-		     len & 0xff, len >> 8);
-
 	mutex_lock(&dev->ctrl_urb_lock);
 	ret = usb_control_msg(udev, pipe, req,
 			      USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
 			      0x0000, reg, dev->urb_buf, len, HZ);
 	if (ret < 0) {
-		em28xx_regdbg("(pipe 0x%08x): IN:  %02x %02x %02x %02x %02x %02x %02x %02x  failed\n",
+		em28xx_regdbg("(pipe 0x%08x): IN:  %02x %02x %02x %02x %02x %02x %02x %02x  failed with error %i\n",
 			     pipe, USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
 			     req, 0, 0,
 			     reg & 0xff, reg >> 8,
-			     len & 0xff, len >> 8);
+			     len & 0xff, len >> 8, ret);
 		mutex_unlock(&dev->ctrl_urb_lock);
 		return usb_translate_errors(ret);
 	}
@@ -116,7 +110,7 @@ int em28xx_read_reg_req_len(struct em28xx *dev, u8 req, u16 reg,
 
 	mutex_unlock(&dev->ctrl_urb_lock);
 
-	em28xx_regdbg("(pipe 0x%08x): IN:  %02x %02x %02x %02x %02x %02x %02x %02x  failed <<< %*ph\n",
+	em28xx_regdbg("(pipe 0x%08x): IN:  %02x %02x %02x %02x %02x %02x %02x %02x <<< %*ph\n",
 		     pipe, USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
 		     req, 0, 0,
 		     reg & 0xff, reg >> 8,
@@ -164,13 +158,6 @@ int em28xx_write_regs_req(struct em28xx *dev, u8 req, u16 reg, char *buf,
 	if ((len < 1) || (len > URB_MAX_CTRL_SIZE))
 		return -EINVAL;
 
-	em28xx_regdbg("(pipe 0x%08x): OUT: %02x %02x %02x %02x %02x %02x %02x %02x >>> %*ph\n",
-		      pipe,
-		      USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-		      req, 0, 0,
-		      reg & 0xff, reg >> 8,
-		      len & 0xff, len >> 8, len, buf);
-
 	mutex_lock(&dev->ctrl_urb_lock);
 	memcpy(dev->urb_buf, buf, len);
 	ret = usb_control_msg(udev, pipe, req,
@@ -178,8 +165,22 @@ int em28xx_write_regs_req(struct em28xx *dev, u8 req, u16 reg, char *buf,
 			      0x0000, reg, dev->urb_buf, len, HZ);
 	mutex_unlock(&dev->ctrl_urb_lock);
 
-	if (ret < 0)
+	if (ret < 0) {
+		em28xx_regdbg("(pipe 0x%08x): OUT:  %02x %02x %02x %02x %02x %02x %02x %02x >>> %*ph  failed with error %i\n",
+			      pipe,
+			      USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+			      req, 0, 0,
+			      reg & 0xff, reg >> 8,
+			      len & 0xff, len >> 8, len, buf, ret);
 		return usb_translate_errors(ret);
+	}
+
+	em28xx_regdbg("(pipe 0x%08x): OUT:  %02x %02x %02x %02x %02x %02x %02x %02x >>> %*ph\n",
+		      pipe,
+		      USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+		      req, 0, 0,
+		      reg & 0xff, reg >> 8,
+		      len & 0xff, len >> 8, len, buf);
 
 	if (dev->wait_after_write)
 		msleep(dev->wait_after_write);
diff --git a/drivers/media/usb/gspca/m5602/m5602_s5k83a.c b/drivers/media/usb/gspca/m5602/m5602_s5k83a.c
index be5e25d1a2e8..6ad8d4849680 100644
--- a/drivers/media/usb/gspca/m5602/m5602_s5k83a.c
+++ b/drivers/media/usb/gspca/m5602/m5602_s5k83a.c
@@ -345,6 +345,11 @@ int s5k83a_start(struct sd *sd)
 	   to assume that there is no better way of accomplishing this */
 	sd->rotation_thread = kthread_create(rotation_thread_function,
 					     sd, "rotation thread");
+	if (IS_ERR(sd->rotation_thread)) {
+		err = PTR_ERR(sd->rotation_thread);
+		sd->rotation_thread = NULL;
+		return err;
+	}
 	wake_up_process(sd->rotation_thread);
 
 	/* Preinit the sensor */
diff --git a/drivers/media/usb/gspca/ov519.c b/drivers/media/usb/gspca/ov519.c
index f4c41f043cda..cdb79c5f0c38 100644
--- a/drivers/media/usb/gspca/ov519.c
+++ b/drivers/media/usb/gspca/ov519.c
@@ -3526,7 +3526,8 @@ static void ov511_mode_init_regs(struct sd *sd)
 				sd->clockdiv = 0;
 				break;
 			}
-			/* Fall through for 640x480 case */
+			/* For 640x480 case */
+			/* fall through */
 		default:
 /*		case 20: */
 /*		case 15: */
diff --git a/drivers/media/usb/pulse8-cec/pulse8-cec.c b/drivers/media/usb/pulse8-cec/pulse8-cec.c
index 1dfc2de1fe77..c843070f24c1 100644
--- a/drivers/media/usb/pulse8-cec/pulse8-cec.c
+++ b/drivers/media/usb/pulse8-cec/pulse8-cec.c
@@ -148,18 +148,15 @@ static void pulse8_irq_work_handler(struct work_struct *work)
 		cec_received_msg(pulse8->adap, &pulse8->rx_msg);
 		break;
 	case MSGCODE_TRANSMIT_SUCCEEDED:
-		cec_transmit_done(pulse8->adap, CEC_TX_STATUS_OK,
-				  0, 0, 0, 0);
+		cec_transmit_attempt_done(pulse8->adap, CEC_TX_STATUS_OK);
 		break;
 	case MSGCODE_TRANSMIT_FAILED_ACK:
-		cec_transmit_done(pulse8->adap, CEC_TX_STATUS_NACK,
-				  0, 1, 0, 0);
+		cec_transmit_attempt_done(pulse8->adap, CEC_TX_STATUS_NACK);
 		break;
 	case MSGCODE_TRANSMIT_FAILED_LINE:
 	case MSGCODE_TRANSMIT_FAILED_TIMEOUT_DATA:
 	case MSGCODE_TRANSMIT_FAILED_TIMEOUT_LINE:
-		cec_transmit_done(pulse8->adap, CEC_TX_STATUS_ERROR,
-				  0, 0, 0, 1);
+		cec_transmit_attempt_done(pulse8->adap, CEC_TX_STATUS_ERROR);
 		break;
 	}
 }
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-i2c-core.c b/drivers/media/usb/pvrusb2/pvrusb2-i2c-core.c
index f727b54a53c6..20a52b785fff 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-i2c-core.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-i2c-core.c
@@ -488,7 +488,7 @@ static int pvr2_i2c_xfer(struct i2c_adapter *i2c_adap,
 			if ((ret > 0) || !(msgs[idx].flags & I2C_M_RD)) {
 				if (cnt > 8) cnt = 8;
 				printk(KERN_CONT " [");
-				for (offs = 0; offs < (cnt>8?8:cnt); offs++) {
+				for (offs = 0; offs < cnt; offs++) {
 					if (offs) printk(KERN_CONT " ");
 					printk(KERN_CONT "%02x",msgs[idx].buf[offs]);
 				}
diff --git a/drivers/media/usb/pwc/pwc-v4l.c b/drivers/media/usb/pwc/pwc-v4l.c
index 92f04db6bbae..043b2b97cee6 100644
--- a/drivers/media/usb/pwc/pwc-v4l.c
+++ b/drivers/media/usb/pwc/pwc-v4l.c
@@ -568,7 +568,8 @@ static int pwc_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 		pdev->gain_valid = true;
 		if (!DEVICE_USE_CODEC3(pdev->type))
 			break;
-		/* Fall through for CODEC3 where autogain also controls expo */
+		/* For CODEC3 where autogain also controls expo */
+		/* fall through */
 	case V4L2_CID_EXPOSURE_AUTO:
 		if (pdev->exposure_valid && time_before(jiffies,
 				pdev->last_exposure_update + HZ / 4)) {
diff --git a/drivers/media/usb/rainshadow-cec/rainshadow-cec.c b/drivers/media/usb/rainshadow-cec/rainshadow-cec.c
index 71bd68548c9c..f203699e9c1b 100644
--- a/drivers/media/usb/rainshadow-cec/rainshadow-cec.c
+++ b/drivers/media/usb/rainshadow-cec/rainshadow-cec.c
@@ -98,16 +98,13 @@ static void rain_process_msg(struct rain *rain)
 
 	switch (stat) {
 	case 1:
-		cec_transmit_done(rain->adap, CEC_TX_STATUS_OK,
-				  0, 0, 0, 0);
+		cec_transmit_attempt_done(rain->adap, CEC_TX_STATUS_OK);
 		break;
 	case 2:
-		cec_transmit_done(rain->adap, CEC_TX_STATUS_NACK,
-				  0, 1, 0, 0);
+		cec_transmit_attempt_done(rain->adap, CEC_TX_STATUS_NACK);
 		break;
 	default:
-		cec_transmit_done(rain->adap, CEC_TX_STATUS_LOW_DRIVE,
-				  0, 0, 0, 1);
+		cec_transmit_attempt_done(rain->adap, CEC_TX_STATUS_LOW_DRIVE);
 		break;
 	}
 }
@@ -123,11 +120,12 @@ static void rain_irq_work_handler(struct work_struct *work)
 		char data;
 
 		spin_lock_irqsave(&rain->buf_lock, flags);
-		exit_loop = rain->buf_len == 0;
 		if (rain->buf_len) {
 			data = rain->buf[rain->buf_rd_idx];
 			rain->buf_len--;
 			rain->buf_rd_idx = (rain->buf_rd_idx + 1) & 0xff;
+		} else {
+			exit_loop = true;
 		}
 		spin_unlock_irqrestore(&rain->buf_lock, flags);
 
@@ -296,7 +294,7 @@ static int rain_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
 			 cec_msg_destination(msg), msg->msg[1]);
 		for (i = 2; i < msg->len; i++) {
 			snprintf(hex, sizeof(hex), "%02x", msg->msg[i]);
-			strncat(cmd, hex, sizeof(cmd));
+			strlcat(cmd, hex, sizeof(cmd));
 		}
 	}
 	mutex_lock(&rain->write_lock);
@@ -336,6 +334,7 @@ static int rain_connect(struct serio *serio, struct serio_driver *drv)
 	serio_set_drvdata(serio, rain);
 	INIT_WORK(&rain->work, rain_irq_work_handler);
 	mutex_init(&rain->write_lock);
+	spin_lock_init(&rain->buf_lock);
 
 	err = serio_open(serio, drv);
 	if (err)
diff --git a/drivers/media/usb/s2255/s2255drv.c b/drivers/media/usb/s2255/s2255drv.c
index a9d4484f7626..6a88b1dbb3a0 100644
--- a/drivers/media/usb/s2255/s2255drv.c
+++ b/drivers/media/usb/s2255/s2255drv.c
@@ -1803,6 +1803,8 @@ static int save_frame(struct s2255_dev *dev, struct s2255_pipeinfo *pipe_info)
 				default:
 					pr_info("s2255 unknown resp\n");
 				}
+				pdata++;
+				break;
 			default:
 				pdata++;
 				break;
diff --git a/drivers/media/usb/tm6000/tm6000-input.c b/drivers/media/usb/tm6000/tm6000-input.c
index 39c15bb2b20c..1a033f57fcc1 100644
--- a/drivers/media/usb/tm6000/tm6000-input.c
+++ b/drivers/media/usb/tm6000/tm6000-input.c
@@ -63,7 +63,6 @@ struct tm6000_IR {
 	u8			wait:1;
 	u8			pwled:2;
 	u8			submit_urb:1;
-	u16			key_addr;
 	struct urb		*int_urb;
 
 	/* IR device properties */
@@ -321,9 +320,6 @@ static int tm6000_ir_change_protocol(struct rc_dev *rc, u64 *rc_type)
 
 	dprintk(2, "%s\n",__func__);
 
-	if ((rc->rc_map.scan) && (*rc_type == RC_BIT_NEC))
-		ir->key_addr = ((rc->rc_map.scan[0].scancode >> 8) & 0xffff);
-
 	ir->rc_type = *rc_type;
 
 	tm6000_ir_config(ir);
diff --git a/drivers/media/usb/usbvision/usbvision-i2c.c b/drivers/media/usb/usbvision/usbvision-i2c.c
index 5a3f788ad033..fdf6b6e285da 100644
--- a/drivers/media/usb/usbvision/usbvision-i2c.c
+++ b/drivers/media/usb/usbvision/usbvision-i2c.c
@@ -311,10 +311,13 @@ usbvision_i2c_read_max4(struct usb_usbvision *usbvision, unsigned char addr,
 	switch (len) {
 	case 4:
 		buf[3] = usbvision_read_reg(usbvision, USBVISION_SER_DAT4);
+		/* fall through */
 	case 3:
 		buf[2] = usbvision_read_reg(usbvision, USBVISION_SER_DAT3);
+		/* fall through */
 	case 2:
 		buf[1] = usbvision_read_reg(usbvision, USBVISION_SER_DAT2);
+		/* fall through */
 	case 1:
 		buf[0] = usbvision_read_reg(usbvision, USBVISION_SER_DAT1);
 		break;
diff --git a/drivers/media/usb/usbvision/usbvision-video.c b/drivers/media/usb/usbvision/usbvision-video.c
index f9c3325aa4d4..756322c4ac05 100644
--- a/drivers/media/usb/usbvision/usbvision-video.c
+++ b/drivers/media/usb/usbvision/usbvision-video.c
@@ -1427,8 +1427,8 @@ static int usbvision_probe(struct usb_interface *intf,
 	int model, i, ret;
 
 	PDEBUG(DBG_PROBE, "VID=%#04x, PID=%#04x, ifnum=%u",
-				dev->descriptor.idVendor,
-				dev->descriptor.idProduct, ifnum);
+				le16_to_cpu(dev->descriptor.idVendor),
+				le16_to_cpu(dev->descriptor.idProduct), ifnum);
 
 	model = devid->driver_info;
 	if (model < 0 || model >= usbvision_device_data_size) {
diff --git a/drivers/media/usb/uvc/uvc_driver.c b/drivers/media/usb/uvc/uvc_driver.c
index 46d6be0bb316..70842c5af05b 100644
--- a/drivers/media/usb/uvc/uvc_driver.c
+++ b/drivers/media/usb/uvc/uvc_driver.c
@@ -2013,6 +2013,7 @@ static int uvc_probe(struct usb_interface *intf,
 {
 	struct usb_device *udev = interface_to_usbdev(intf);
 	struct uvc_device *dev;
+	int function;
 	int ret;
 
 	if (id->idVendor && id->idProduct)
@@ -2044,9 +2045,27 @@ static int uvc_probe(struct usb_interface *intf,
 		strlcpy(dev->name, udev->product, sizeof dev->name);
 	else
 		snprintf(dev->name, sizeof dev->name,
-			"UVC Camera (%04x:%04x)",
-			le16_to_cpu(udev->descriptor.idVendor),
-			le16_to_cpu(udev->descriptor.idProduct));
+			 "UVC Camera (%04x:%04x)",
+			 le16_to_cpu(udev->descriptor.idVendor),
+			 le16_to_cpu(udev->descriptor.idProduct));
+
+	/*
+	 * Add iFunction or iInterface to names when available as additional
+	 * distinguishers between interfaces. iFunction is prioritized over
+	 * iInterface which matches Windows behavior at the point of writing.
+	 */
+	if (intf->intf_assoc && intf->intf_assoc->iFunction != 0)
+		function = intf->intf_assoc->iFunction;
+	else
+		function = intf->cur_altsetting->desc.iInterface;
+	if (function != 0) {
+		size_t len;
+
+		strlcat(dev->name, ": ", sizeof(dev->name));
+		len = strlen(dev->name);
+		usb_string(udev, function, dev->name + len,
+			   sizeof(dev->name) - len);
+	}
 
 	/* Parse the Video Class control descriptor. */
 	if (uvc_parse_control(dev) < 0) {
@@ -2441,6 +2460,15 @@ static struct usb_device_id uvc_ids[] = {
 	  .bInterfaceProtocol	= 0,
 	  .driver_info 		= UVC_QUIRK_PROBE_MINMAX
 				| UVC_QUIRK_BUILTIN_ISIGHT },
+	/* Apple Built-In iSight via iBridge */
+	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
+				| USB_DEVICE_ID_MATCH_INT_INFO,
+	  .idVendor		= 0x05ac,
+	  .idProduct		= 0x8600,
+	  .bInterfaceClass	= USB_CLASS_VIDEO,
+	  .bInterfaceSubClass	= 1,
+	  .bInterfaceProtocol	= 0,
+	  .driver_info		= UVC_QUIRK_PROBE_DEF },
 	/* Foxlink ("HP Webcam" on HP Mini 5103) */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
diff --git a/drivers/media/usb/uvc/uvc_video.c b/drivers/media/usb/uvc/uvc_video.c
index 47d93a938dde..fb86d6af398d 100644
--- a/drivers/media/usb/uvc/uvc_video.c
+++ b/drivers/media/usb/uvc/uvc_video.c
@@ -1323,11 +1323,11 @@ static void uvc_video_complete(struct urb *urb)
 	default:
 		uvc_printk(KERN_WARNING, "Non-zero status (%d) in video "
 			"completion handler.\n", urb->status);
-
+		/* fall through */
 	case -ENOENT:		/* usb_kill_urb() called. */
 		if (stream->frozen)
 			return;
-
+		/* fall through */
 	case -ECONNRESET:	/* usb_unlink_urb() called. */
 	case -ESHUTDOWN:	/* The endpoint is being disabled. */
 		uvc_queue_cancel(queue, urb->status == -ESHUTDOWN);
diff --git a/drivers/media/v4l2-core/Kconfig b/drivers/media/v4l2-core/Kconfig
index 6b1b78ff1417..a35c33686abf 100644
--- a/drivers/media/v4l2-core/Kconfig
+++ b/drivers/media/v4l2-core/Kconfig
@@ -55,6 +55,9 @@ config V4L2_FLASH_LED_CLASS
 
 	  When in doubt, say N.
 
+config V4L2_FWNODE
+	tristate
+
 # Used by drivers that need Videobuf modules
 config VIDEOBUF_GEN
 	tristate
diff --git a/drivers/media/v4l2-core/Makefile b/drivers/media/v4l2-core/Makefile
index 795a5352761d..098ad5fd5231 100644
--- a/drivers/media/v4l2-core/Makefile
+++ b/drivers/media/v4l2-core/Makefile
@@ -10,9 +10,7 @@ videodev-objs	:=	v4l2-dev.o v4l2-ioctl.o v4l2-device.o v4l2-fh.o \
 ifeq ($(CONFIG_COMPAT),y)
   videodev-objs += v4l2-compat-ioctl32.o
 endif
-ifeq ($(CONFIG_OF),y)
-  videodev-objs += v4l2-of.o
-endif
+obj-$(CONFIG_V4L2_FWNODE) += v4l2-fwnode.o
 ifeq ($(CONFIG_TRACEPOINTS),y)
   videodev-objs += vb2-trace.o v4l2-trace.o
 endif
diff --git a/drivers/media/v4l2-core/v4l2-async.c b/drivers/media/v4l2-core/v4l2-async.c
index 96cc733f35ef..851f128eba22 100644
--- a/drivers/media/v4l2-core/v4l2-async.c
+++ b/drivers/media/v4l2-core/v4l2-async.c
@@ -12,8 +12,10 @@
 #include <linux/err.h>
 #include <linux/i2c.h>
 #include <linux/list.h>
+#include <linux/mm.h>
 #include <linux/module.h>
 #include <linux/mutex.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/types.h>
@@ -40,10 +42,14 @@ static bool match_devname(struct v4l2_subdev *sd,
 	return !strcmp(asd->match.device_name.name, dev_name(sd->dev));
 }
 
-static bool match_of(struct v4l2_subdev *sd, struct v4l2_async_subdev *asd)
+static bool match_fwnode(struct v4l2_subdev *sd, struct v4l2_async_subdev *asd)
 {
-	return !of_node_cmp(of_node_full_name(sd->of_node),
-			    of_node_full_name(asd->match.of.node));
+	if (!is_of_node(sd->fwnode) || !is_of_node(asd->match.fwnode.fwnode))
+		return sd->fwnode == asd->match.fwnode.fwnode;
+
+	return !of_node_cmp(of_node_full_name(to_of_node(sd->fwnode)),
+			    of_node_full_name(
+				    to_of_node(asd->match.fwnode.fwnode)));
 }
 
 static bool match_custom(struct v4l2_subdev *sd, struct v4l2_async_subdev *asd)
@@ -77,8 +83,8 @@ static struct v4l2_async_subdev *v4l2_async_belongs(struct v4l2_async_notifier *
 		case V4L2_ASYNC_MATCH_I2C:
 			match = match_i2c;
 			break;
-		case V4L2_ASYNC_MATCH_OF:
-			match = match_of;
+		case V4L2_ASYNC_MATCH_FWNODE:
+			match = match_fwnode;
 			break;
 		default:
 			/* Cannot happen, unless someone breaks us */
@@ -143,7 +149,8 @@ int v4l2_async_notifier_register(struct v4l2_device *v4l2_dev,
 	struct v4l2_async_subdev *asd;
 	int i;
 
-	if (!notifier->num_subdevs || notifier->num_subdevs > V4L2_MAX_SUBDEVS)
+	if (!v4l2_dev || !notifier->num_subdevs ||
+	    notifier->num_subdevs > V4L2_MAX_SUBDEVS)
 		return -EINVAL;
 
 	notifier->v4l2_dev = v4l2_dev;
@@ -157,7 +164,7 @@ int v4l2_async_notifier_register(struct v4l2_device *v4l2_dev,
 		case V4L2_ASYNC_MATCH_CUSTOM:
 		case V4L2_ASYNC_MATCH_DEVNAME:
 		case V4L2_ASYNC_MATCH_I2C:
-		case V4L2_ASYNC_MATCH_OF:
+		case V4L2_ASYNC_MATCH_FWNODE:
 			break;
 		default:
 			dev_err(notifier->v4l2_dev ? notifier->v4l2_dev->dev : NULL,
@@ -204,7 +211,7 @@ void v4l2_async_notifier_unregister(struct v4l2_async_notifier *notifier)
 	if (!notifier->v4l2_dev)
 		return;
 
-	dev = kmalloc_array(n_subdev, sizeof(*dev), GFP_KERNEL);
+	dev = kvmalloc_array(n_subdev, sizeof(*dev), GFP_KERNEL);
 	if (!dev) {
 		dev_err(notifier->v4l2_dev->dev,
 			"Failed to allocate device cache!\n");
@@ -260,7 +267,7 @@ void v4l2_async_notifier_unregister(struct v4l2_async_notifier *notifier)
 		}
 		put_device(d);
 	}
-	kfree(dev);
+	kvfree(dev);
 
 	notifier->v4l2_dev = NULL;
 
@@ -280,8 +287,8 @@ int v4l2_async_register_subdev(struct v4l2_subdev *sd)
 	 * (struct v4l2_subdev.dev), and async sub-device does not
 	 * exist independently of the device at any point of time.
 	 */
-	if (!sd->of_node && sd->dev)
-		sd->of_node = sd->dev->of_node;
+	if (!sd->fwnode && sd->dev)
+		sd->fwnode = dev_fwnode(sd->dev);
 
 	mutex_lock(&list_lock);
 
diff --git a/drivers/media/v4l2-core/v4l2-ctrls.c b/drivers/media/v4l2-core/v4l2-ctrls.c
index ec42872d11cf..dd1db678718c 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls.c
@@ -19,6 +19,7 @@
  */
 
 #include <linux/ctype.h>
+#include <linux/mm.h>
 #include <linux/slab.h>
 #include <linux/export.h>
 #include <media/v4l2-ioctl.h>
@@ -886,6 +887,7 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_PIXEL_RATE:		return "Pixel Rate";
 	case V4L2_CID_TEST_PATTERN:		return "Test Pattern";
 	case V4L2_CID_DEINTERLACING_MODE:	return "Deinterlacing Mode";
+	case V4L2_CID_DIGITAL_GAIN:		return "Digital Gain";
 
 	/* DV controls */
 	/* Keep the order of the 'case's the same as in v4l2-controls.h! */
@@ -1739,14 +1741,15 @@ int v4l2_ctrl_handler_init_class(struct v4l2_ctrl_handler *hdl,
 				 unsigned nr_of_controls_hint,
 				 struct lock_class_key *key, const char *name)
 {
+	mutex_init(&hdl->_lock);
 	hdl->lock = &hdl->_lock;
-	mutex_init(hdl->lock);
 	lockdep_set_class_and_name(hdl->lock, key, name);
 	INIT_LIST_HEAD(&hdl->ctrls);
 	INIT_LIST_HEAD(&hdl->ctrl_refs);
 	hdl->nr_of_buckets = 1 + nr_of_controls_hint / 8;
-	hdl->buckets = kcalloc(hdl->nr_of_buckets, sizeof(hdl->buckets[0]),
-			       GFP_KERNEL);
+	hdl->buckets = kvmalloc_array(hdl->nr_of_buckets,
+				      sizeof(hdl->buckets[0]),
+				      GFP_KERNEL | __GFP_ZERO);
 	hdl->error = hdl->buckets ? 0 : -ENOMEM;
 	return hdl->error;
 }
@@ -1773,13 +1776,14 @@ void v4l2_ctrl_handler_free(struct v4l2_ctrl_handler *hdl)
 		list_del(&ctrl->node);
 		list_for_each_entry_safe(sev, next_sev, &ctrl->ev_subs, node)
 			list_del(&sev->node);
-		kfree(ctrl);
+		kvfree(ctrl);
 	}
-	kfree(hdl->buckets);
+	kvfree(hdl->buckets);
 	hdl->buckets = NULL;
 	hdl->cached = NULL;
 	hdl->error = 0;
 	mutex_unlock(hdl->lock);
+	mutex_destroy(&hdl->_lock);
 }
 EXPORT_SYMBOL(v4l2_ctrl_handler_free);
 
@@ -2022,7 +2026,7 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 		 is_array)
 		sz_extra += 2 * tot_ctrl_size;
 
-	ctrl = kzalloc(sizeof(*ctrl) + sz_extra, GFP_KERNEL);
+	ctrl = kvzalloc(sizeof(*ctrl) + sz_extra, GFP_KERNEL);
 	if (ctrl == NULL) {
 		handler_set_err(hdl, -ENOMEM);
 		return NULL;
@@ -2071,7 +2075,7 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 	}
 
 	if (handler_new_ref(hdl, ctrl)) {
-		kfree(ctrl);
+		kvfree(ctrl);
 		return NULL;
 	}
 	mutex_lock(hdl->lock);
@@ -2444,14 +2448,16 @@ int v4l2_ctrl_subdev_log_status(struct v4l2_subdev *sd)
 EXPORT_SYMBOL(v4l2_ctrl_subdev_log_status);
 
 /* Call s_ctrl for all controls owned by the handler */
-int v4l2_ctrl_handler_setup(struct v4l2_ctrl_handler *hdl)
+int __v4l2_ctrl_handler_setup(struct v4l2_ctrl_handler *hdl)
 {
 	struct v4l2_ctrl *ctrl;
 	int ret = 0;
 
 	if (hdl == NULL)
 		return 0;
-	mutex_lock(hdl->lock);
+
+	lockdep_assert_held(hdl->lock);
+
 	list_for_each_entry(ctrl, &hdl->ctrls, node)
 		ctrl->done = false;
 
@@ -2476,7 +2482,22 @@ int v4l2_ctrl_handler_setup(struct v4l2_ctrl_handler *hdl)
 		if (ret)
 			break;
 	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(__v4l2_ctrl_handler_setup);
+
+int v4l2_ctrl_handler_setup(struct v4l2_ctrl_handler *hdl)
+{
+	int ret;
+
+	if (hdl == NULL)
+		return 0;
+
+	mutex_lock(hdl->lock);
+	ret = __v4l2_ctrl_handler_setup(hdl);
 	mutex_unlock(hdl->lock);
+
 	return ret;
 }
 EXPORT_SYMBOL(v4l2_ctrl_handler_setup);
@@ -2824,8 +2845,8 @@ int v4l2_g_ext_ctrls(struct v4l2_ctrl_handler *hdl, struct v4l2_ext_controls *cs
 		return class_check(hdl, cs->which);
 
 	if (cs->count > ARRAY_SIZE(helper)) {
-		helpers = kmalloc_array(cs->count, sizeof(helper[0]),
-					GFP_KERNEL);
+		helpers = kvmalloc_array(cs->count, sizeof(helper[0]),
+					 GFP_KERNEL);
 		if (helpers == NULL)
 			return -ENOMEM;
 	}
@@ -2877,7 +2898,7 @@ int v4l2_g_ext_ctrls(struct v4l2_ctrl_handler *hdl, struct v4l2_ext_controls *cs
 	}
 
 	if (cs->count > ARRAY_SIZE(helper))
-		kfree(helpers);
+		kvfree(helpers);
 	return ret;
 }
 EXPORT_SYMBOL(v4l2_g_ext_ctrls);
@@ -3079,8 +3100,8 @@ static int try_set_ext_ctrls(struct v4l2_fh *fh, struct v4l2_ctrl_handler *hdl,
 		return class_check(hdl, cs->which);
 
 	if (cs->count > ARRAY_SIZE(helper)) {
-		helpers = kmalloc_array(cs->count, sizeof(helper[0]),
-					GFP_KERNEL);
+		helpers = kvmalloc_array(cs->count, sizeof(helper[0]),
+					 GFP_KERNEL);
 		if (!helpers)
 			return -ENOMEM;
 	}
@@ -3157,7 +3178,7 @@ static int try_set_ext_ctrls(struct v4l2_fh *fh, struct v4l2_ctrl_handler *hdl,
 	}
 
 	if (cs->count > ARRAY_SIZE(helper))
-		kfree(helpers);
+		kvfree(helpers);
 	return ret;
 }
 
diff --git a/drivers/media/v4l2-core/v4l2-event.c b/drivers/media/v4l2-core/v4l2-event.c
index a75df6cb141f..968c2eb08b5a 100644
--- a/drivers/media/v4l2-core/v4l2-event.c
+++ b/drivers/media/v4l2-core/v4l2-event.c
@@ -21,6 +21,7 @@
 #include <media/v4l2-fh.h>
 #include <media/v4l2-event.h>
 
+#include <linux/mm.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/export.h>
@@ -214,7 +215,8 @@ int v4l2_event_subscribe(struct v4l2_fh *fh,
 	if (elems < 1)
 		elems = 1;
 
-	sev = kzalloc(sizeof(*sev) + sizeof(struct v4l2_kevent) * elems, GFP_KERNEL);
+	sev = kvzalloc(sizeof(*sev) + sizeof(struct v4l2_kevent) * elems,
+		       GFP_KERNEL);
 	if (!sev)
 		return -ENOMEM;
 	for (i = 0; i < elems; i++)
@@ -232,7 +234,7 @@ int v4l2_event_subscribe(struct v4l2_fh *fh,
 	spin_unlock_irqrestore(&fh->vdev->fh_lock, flags);
 
 	if (found_ev) {
-		kfree(sev);
+		kvfree(sev);
 		return 0; /* Already listening */
 	}
 
@@ -304,7 +306,7 @@ int v4l2_event_unsubscribe(struct v4l2_fh *fh,
 	if (sev && sev->ops && sev->ops->del)
 		sev->ops->del(sev);
 
-	kfree(sev);
+	kvfree(sev);
 
 	return 0;
 }
diff --git a/drivers/media/v4l2-core/v4l2-flash-led-class.c b/drivers/media/v4l2-core/v4l2-flash-led-class.c
index 794e563f24f8..7b8288108e8a 100644
--- a/drivers/media/v4l2-core/v4l2-flash-led-class.c
+++ b/drivers/media/v4l2-core/v4l2-flash-led-class.c
@@ -12,7 +12,7 @@
 #include <linux/led-class-flash.h>
 #include <linux/module.h>
 #include <linux/mutex.h>
-#include <linux/of.h>
+#include <linux/property.h>
 #include <linux/slab.h>
 #include <linux/types.h>
 #include <media/v4l2-flash-led-class.h>
@@ -612,7 +612,7 @@ static const struct v4l2_subdev_internal_ops v4l2_flash_subdev_internal_ops = {
 static const struct v4l2_subdev_ops v4l2_flash_subdev_ops;
 
 struct v4l2_flash *v4l2_flash_init(
-	struct device *dev, struct device_node *of_node,
+	struct device *dev, struct fwnode_handle *fwn,
 	struct led_classdev_flash *fled_cdev,
 	struct led_classdev_flash *iled_cdev,
 	const struct v4l2_flash_ops *ops,
@@ -638,7 +638,7 @@ struct v4l2_flash *v4l2_flash_init(
 	v4l2_flash->iled_cdev = iled_cdev;
 	v4l2_flash->ops = ops;
 	sd->dev = dev;
-	sd->of_node = of_node ? of_node : led_cdev->dev->of_node;
+	sd->fwnode = fwn ? fwn : dev_fwnode(led_cdev->dev);
 	v4l2_subdev_init(sd, &v4l2_flash_subdev_ops);
 	sd->internal_ops = &v4l2_flash_subdev_internal_ops;
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
@@ -654,7 +654,7 @@ struct v4l2_flash *v4l2_flash_init(
 	if (ret < 0)
 		goto err_init_controls;
 
-	of_node_get(sd->of_node);
+	fwnode_handle_get(sd->fwnode);
 
 	ret = v4l2_async_register_subdev(sd);
 	if (ret < 0)
@@ -663,7 +663,7 @@ struct v4l2_flash *v4l2_flash_init(
 	return v4l2_flash;
 
 err_async_register_sd:
-	of_node_put(sd->of_node);
+	fwnode_handle_put(sd->fwnode);
 	v4l2_ctrl_handler_free(sd->ctrl_handler);
 err_init_controls:
 	media_entity_cleanup(&sd->entity);
@@ -683,7 +683,7 @@ void v4l2_flash_release(struct v4l2_flash *v4l2_flash)
 
 	v4l2_async_unregister_subdev(sd);
 
-	of_node_put(sd->of_node);
+	fwnode_handle_put(sd->fwnode);
 
 	v4l2_ctrl_handler_free(sd->ctrl_handler);
 	media_entity_cleanup(&sd->entity);
diff --git a/drivers/media/v4l2-core/v4l2-fwnode.c b/drivers/media/v4l2-core/v4l2-fwnode.c
new file mode 100644
index 000000000000..153c53ca3925
--- /dev/null
+++ b/drivers/media/v4l2-core/v4l2-fwnode.c
@@ -0,0 +1,345 @@
+/*
+ * V4L2 fwnode binding parsing library
+ *
+ * The origins of the V4L2 fwnode library are in V4L2 OF library that
+ * formerly was located in v4l2-of.c.
+ *
+ * Copyright (c) 2016 Intel Corporation.
+ * Author: Sakari Ailus <sakari.ailus@linux.intel.com>
+ *
+ * Copyright (C) 2012 - 2013 Samsung Electronics Co., Ltd.
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * Copyright (C) 2012 Renesas Electronics Corp.
+ * Author: Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ */
+#include <linux/acpi.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/property.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/types.h>
+
+#include <media/v4l2-fwnode.h>
+
+static int v4l2_fwnode_endpoint_parse_csi_bus(struct fwnode_handle *fwnode,
+					      struct v4l2_fwnode_endpoint *vep)
+{
+	struct v4l2_fwnode_bus_mipi_csi2 *bus = &vep->bus.mipi_csi2;
+	bool have_clk_lane = false;
+	unsigned int flags = 0, lanes_used = 0;
+	unsigned int i;
+	u32 v;
+	int rval;
+
+	rval = fwnode_property_read_u32_array(fwnode, "data-lanes", NULL, 0);
+	if (rval > 0) {
+		u32 array[ARRAY_SIZE(bus->data_lanes)];
+
+		bus->num_data_lanes =
+			min_t(int, ARRAY_SIZE(bus->data_lanes), rval);
+
+		fwnode_property_read_u32_array(fwnode, "data-lanes", array,
+					       bus->num_data_lanes);
+
+		for (i = 0; i < bus->num_data_lanes; i++) {
+			if (lanes_used & BIT(array[i]))
+				pr_warn("duplicated lane %u in data-lanes\n",
+					array[i]);
+			lanes_used |= BIT(array[i]);
+
+			bus->data_lanes[i] = array[i];
+		}
+	}
+
+	rval = fwnode_property_read_u32_array(fwnode, "lane-polarities", NULL,
+					      0);
+	if (rval > 0) {
+		u32 array[ARRAY_SIZE(bus->lane_polarities)];
+
+		if (rval < 1 + bus->num_data_lanes /* clock + data */) {
+			pr_warn("too few lane-polarities entries (need %u, got %u)\n",
+				1 + bus->num_data_lanes, rval);
+			return -EINVAL;
+		}
+
+		fwnode_property_read_u32_array(fwnode, "lane-polarities", array,
+					       1 + bus->num_data_lanes);
+
+		for (i = 0; i < 1 + bus->num_data_lanes; i++)
+			bus->lane_polarities[i] = array[i];
+	}
+
+	if (!fwnode_property_read_u32(fwnode, "clock-lanes", &v)) {
+		if (lanes_used & BIT(v))
+			pr_warn("duplicated lane %u in clock-lanes\n", v);
+		lanes_used |= BIT(v);
+
+		bus->clock_lane = v;
+		have_clk_lane = true;
+	}
+
+	if (fwnode_property_present(fwnode, "clock-noncontinuous"))
+		flags |= V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
+	else if (have_clk_lane || bus->num_data_lanes > 0)
+		flags |= V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
+
+	bus->flags = flags;
+	vep->bus_type = V4L2_MBUS_CSI2;
+
+	return 0;
+}
+
+static void v4l2_fwnode_endpoint_parse_parallel_bus(
+	struct fwnode_handle *fwnode, struct v4l2_fwnode_endpoint *vep)
+{
+	struct v4l2_fwnode_bus_parallel *bus = &vep->bus.parallel;
+	unsigned int flags = 0;
+	u32 v;
+
+	if (!fwnode_property_read_u32(fwnode, "hsync-active", &v))
+		flags |= v ? V4L2_MBUS_HSYNC_ACTIVE_HIGH :
+			V4L2_MBUS_HSYNC_ACTIVE_LOW;
+
+	if (!fwnode_property_read_u32(fwnode, "vsync-active", &v))
+		flags |= v ? V4L2_MBUS_VSYNC_ACTIVE_HIGH :
+			V4L2_MBUS_VSYNC_ACTIVE_LOW;
+
+	if (!fwnode_property_read_u32(fwnode, "field-even-active", &v))
+		flags |= v ? V4L2_MBUS_FIELD_EVEN_HIGH :
+			V4L2_MBUS_FIELD_EVEN_LOW;
+	if (flags)
+		vep->bus_type = V4L2_MBUS_PARALLEL;
+	else
+		vep->bus_type = V4L2_MBUS_BT656;
+
+	if (!fwnode_property_read_u32(fwnode, "pclk-sample", &v))
+		flags |= v ? V4L2_MBUS_PCLK_SAMPLE_RISING :
+			V4L2_MBUS_PCLK_SAMPLE_FALLING;
+
+	if (!fwnode_property_read_u32(fwnode, "data-active", &v))
+		flags |= v ? V4L2_MBUS_DATA_ACTIVE_HIGH :
+			V4L2_MBUS_DATA_ACTIVE_LOW;
+
+	if (fwnode_property_present(fwnode, "slave-mode"))
+		flags |= V4L2_MBUS_SLAVE;
+	else
+		flags |= V4L2_MBUS_MASTER;
+
+	if (!fwnode_property_read_u32(fwnode, "bus-width", &v))
+		bus->bus_width = v;
+
+	if (!fwnode_property_read_u32(fwnode, "data-shift", &v))
+		bus->data_shift = v;
+
+	if (!fwnode_property_read_u32(fwnode, "sync-on-green-active", &v))
+		flags |= v ? V4L2_MBUS_VIDEO_SOG_ACTIVE_HIGH :
+			V4L2_MBUS_VIDEO_SOG_ACTIVE_LOW;
+
+	bus->flags = flags;
+
+}
+
+/**
+ * v4l2_fwnode_endpoint_parse() - parse all fwnode node properties
+ * @fwnode: pointer to the endpoint's fwnode handle
+ * @vep: pointer to the V4L2 fwnode data structure
+ *
+ * All properties are optional. If none are found, we don't set any flags. This
+ * means the port has a static configuration and no properties have to be
+ * specified explicitly. If any properties that identify the bus as parallel
+ * are found and slave-mode isn't set, we set V4L2_MBUS_MASTER. Similarly, if
+ * we recognise the bus as serial CSI-2 and clock-noncontinuous isn't set, we
+ * set the V4L2_MBUS_CSI2_CONTINUOUS_CLOCK flag. The caller should hold a
+ * reference to @fwnode.
+ *
+ * NOTE: This function does not parse properties the size of which is variable
+ * without a low fixed limit. Please use v4l2_fwnode_endpoint_alloc_parse() in
+ * new drivers instead.
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int v4l2_fwnode_endpoint_parse(struct fwnode_handle *fwnode,
+			       struct v4l2_fwnode_endpoint *vep)
+{
+	int rval;
+
+	fwnode_graph_parse_endpoint(fwnode, &vep->base);
+
+	/* Zero fields from bus_type to until the end */
+	memset(&vep->bus_type, 0, sizeof(*vep) -
+	       offsetof(typeof(*vep), bus_type));
+
+	rval = v4l2_fwnode_endpoint_parse_csi_bus(fwnode, vep);
+	if (rval)
+		return rval;
+	/*
+	 * Parse the parallel video bus properties only if none
+	 * of the MIPI CSI-2 specific properties were found.
+	 */
+	if (vep->bus.mipi_csi2.flags == 0)
+		v4l2_fwnode_endpoint_parse_parallel_bus(fwnode, vep);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(v4l2_fwnode_endpoint_parse);
+
+/*
+ * v4l2_fwnode_endpoint_free() - free the V4L2 fwnode acquired by
+ * v4l2_fwnode_endpoint_alloc_parse()
+ * @vep - the V4L2 fwnode the resources of which are to be released
+ *
+ * It is safe to call this function with NULL argument or on a V4L2 fwnode the
+ * parsing of which failed.
+ */
+void v4l2_fwnode_endpoint_free(struct v4l2_fwnode_endpoint *vep)
+{
+	if (IS_ERR_OR_NULL(vep))
+		return;
+
+	kfree(vep->link_frequencies);
+	kfree(vep);
+}
+EXPORT_SYMBOL_GPL(v4l2_fwnode_endpoint_free);
+
+/**
+ * v4l2_fwnode_endpoint_alloc_parse() - parse all fwnode node properties
+ * @fwnode: pointer to the endpoint's fwnode handle
+ *
+ * All properties are optional. If none are found, we don't set any flags. This
+ * means the port has a static configuration and no properties have to be
+ * specified explicitly. If any properties that identify the bus as parallel
+ * are found and slave-mode isn't set, we set V4L2_MBUS_MASTER. Similarly, if
+ * we recognise the bus as serial CSI-2 and clock-noncontinuous isn't set, we
+ * set the V4L2_MBUS_CSI2_CONTINUOUS_CLOCK flag. The caller should hold a
+ * reference to @fwnode.
+ *
+ * v4l2_fwnode_endpoint_alloc_parse() has two important differences to
+ * v4l2_fwnode_endpoint_parse():
+ *
+ * 1. It also parses variable size data.
+ *
+ * 2. The memory it has allocated to store the variable size data must be freed
+ *    using v4l2_fwnode_endpoint_free() when no longer needed.
+ *
+ * Return: Pointer to v4l2_fwnode_endpoint if successful, on an error pointer
+ * on error.
+ */
+struct v4l2_fwnode_endpoint *v4l2_fwnode_endpoint_alloc_parse(
+	struct fwnode_handle *fwnode)
+{
+	struct v4l2_fwnode_endpoint *vep;
+	int rval;
+
+	vep = kzalloc(sizeof(*vep), GFP_KERNEL);
+	if (!vep)
+		return ERR_PTR(-ENOMEM);
+
+	rval = v4l2_fwnode_endpoint_parse(fwnode, vep);
+	if (rval < 0)
+		goto out_err;
+
+	rval = fwnode_property_read_u64_array(fwnode, "link-frequencies",
+					      NULL, 0);
+	if (rval < 0)
+		goto out_err;
+
+	vep->link_frequencies =
+		kmalloc_array(rval, sizeof(*vep->link_frequencies), GFP_KERNEL);
+	if (!vep->link_frequencies) {
+		rval = -ENOMEM;
+		goto out_err;
+	}
+
+	vep->nr_of_link_frequencies = rval;
+
+	rval = fwnode_property_read_u64_array(fwnode, "link-frequencies",
+					      vep->link_frequencies,
+					      vep->nr_of_link_frequencies);
+	if (rval < 0)
+		goto out_err;
+
+	return vep;
+
+out_err:
+	v4l2_fwnode_endpoint_free(vep);
+	return ERR_PTR(rval);
+}
+EXPORT_SYMBOL_GPL(v4l2_fwnode_endpoint_alloc_parse);
+
+/**
+ * v4l2_fwnode_endpoint_parse_link() - parse a link between two endpoints
+ * @__fwnode: pointer to the endpoint's fwnode at the local end of the link
+ * @link: pointer to the V4L2 fwnode link data structure
+ *
+ * Fill the link structure with the local and remote nodes and port numbers.
+ * The local_node and remote_node fields are set to point to the local and
+ * remote port's parent nodes respectively (the port parent node being the
+ * parent node of the port node if that node isn't a 'ports' node, or the
+ * grand-parent node of the port node otherwise).
+ *
+ * A reference is taken to both the local and remote nodes, the caller must use
+ * v4l2_fwnode_endpoint_put_link() to drop the references when done with the
+ * link.
+ *
+ * Return: 0 on success, or -ENOLINK if the remote endpoint fwnode can't be
+ * found.
+ */
+int v4l2_fwnode_parse_link(struct fwnode_handle *__fwnode,
+			   struct v4l2_fwnode_link *link)
+{
+	const char *port_prop = is_of_node(__fwnode) ? "reg" : "port";
+	struct fwnode_handle *fwnode;
+
+	memset(link, 0, sizeof(*link));
+
+	fwnode = fwnode_get_parent(__fwnode);
+	fwnode_property_read_u32(fwnode, port_prop, &link->local_port);
+	fwnode = fwnode_get_next_parent(fwnode);
+	if (is_of_node(fwnode) &&
+	    of_node_cmp(to_of_node(fwnode)->name, "ports") == 0)
+		fwnode = fwnode_get_next_parent(fwnode);
+	link->local_node = fwnode;
+
+	fwnode = fwnode_graph_get_remote_endpoint(__fwnode);
+	if (!fwnode) {
+		fwnode_handle_put(fwnode);
+		return -ENOLINK;
+	}
+
+	fwnode = fwnode_get_parent(fwnode);
+	fwnode_property_read_u32(fwnode, port_prop, &link->remote_port);
+	fwnode = fwnode_get_next_parent(fwnode);
+	if (is_of_node(fwnode) &&
+	    of_node_cmp(to_of_node(fwnode)->name, "ports") == 0)
+		fwnode = fwnode_get_next_parent(fwnode);
+	link->remote_node = fwnode;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(v4l2_fwnode_parse_link);
+
+/**
+ * v4l2_fwnode_put_link() - drop references to nodes in a link
+ * @link: pointer to the V4L2 fwnode link data structure
+ *
+ * Drop references to the local and remote nodes in the link. This function
+ * must be called on every link parsed with v4l2_fwnode_parse_link().
+ */
+void v4l2_fwnode_put_link(struct v4l2_fwnode_link *link)
+{
+	fwnode_handle_put(link->local_node);
+	fwnode_handle_put(link->remote_node);
+}
+EXPORT_SYMBOL_GPL(v4l2_fwnode_put_link);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@linux.intel.com>");
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index e5a2187381db..cab63bb49c97 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -12,6 +12,7 @@
  *              Mauro Carvalho Chehab <mchehab@infradead.org> (version 2)
  */
 
+#include <linux/mm.h>
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/types.h>
@@ -1229,6 +1230,9 @@ static void v4l_fill_fmtdesc(struct v4l2_fmtdesc *fmt)
 	case V4L2_SDR_FMT_CS8:		descr = "Complex S8"; break;
 	case V4L2_SDR_FMT_CS14LE:	descr = "Complex S14LE"; break;
 	case V4L2_SDR_FMT_RU12LE:	descr = "Real U12LE"; break;
+	case V4L2_SDR_FMT_PCU16BE:	descr = "Planar Complex U16BE"; break;
+	case V4L2_SDR_FMT_PCU18BE:	descr = "Planar Complex U18BE"; break;
+	case V4L2_SDR_FMT_PCU20BE:	descr = "Planar Complex U20BE"; break;
 	case V4L2_TCH_FMT_DELTA_TD16:	descr = "16-bit signed deltas"; break;
 	case V4L2_TCH_FMT_DELTA_TD08:	descr = "8-bit signed deltas"; break;
 	case V4L2_TCH_FMT_TU16:		descr = "16-bit unsigned touch data"; break;
@@ -2141,6 +2145,47 @@ static int v4l_try_ext_ctrls(const struct v4l2_ioctl_ops *ops,
 					-EINVAL;
 }
 
+/*
+ * The selection API specified originally that the _MPLANE buffer types
+ * shouldn't be used. The reasons for this are lost in the mists of time
+ * (or just really crappy memories). Regardless, this is really annoying
+ * for userspace. So to keep things simple we map _MPLANE buffer types
+ * to their 'regular' counterparts before calling the driver. And we
+ * restore it afterwards. This way applications can use either buffer
+ * type and drivers don't need to check for both.
+ */
+static int v4l_g_selection(const struct v4l2_ioctl_ops *ops,
+			   struct file *file, void *fh, void *arg)
+{
+	struct v4l2_selection *p = arg;
+	u32 old_type = p->type;
+	int ret;
+
+	if (p->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		p->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	else if (p->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		p->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	ret = ops->vidioc_g_selection(file, fh, p);
+	p->type = old_type;
+	return ret;
+}
+
+static int v4l_s_selection(const struct v4l2_ioctl_ops *ops,
+			   struct file *file, void *fh, void *arg)
+{
+	struct v4l2_selection *p = arg;
+	u32 old_type = p->type;
+	int ret;
+
+	if (p->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		p->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	else if (p->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		p->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	ret = ops->vidioc_s_selection(file, fh, p);
+	p->type = old_type;
+	return ret;
+}
+
 static int v4l_g_crop(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
@@ -2160,7 +2205,7 @@ static int v4l_g_crop(const struct v4l2_ioctl_ops *ops,
 	else
 		s.target = V4L2_SEL_TGT_CROP_ACTIVE;
 
-	ret = ops->vidioc_g_selection(file, fh, &s);
+	ret = v4l_g_selection(ops, file, fh, &s);
 
 	/* copying results to old structure on success */
 	if (!ret)
@@ -2187,7 +2232,7 @@ static int v4l_s_crop(const struct v4l2_ioctl_ops *ops,
 	else
 		s.target = V4L2_SEL_TGT_CROP_ACTIVE;
 
-	return ops->vidioc_s_selection(file, fh, &s);
+	return v4l_s_selection(ops, file, fh, &s);
 }
 
 static int v4l_cropcap(const struct v4l2_ioctl_ops *ops,
@@ -2229,7 +2274,7 @@ static int v4l_cropcap(const struct v4l2_ioctl_ops *ops,
 	else
 		s.target = V4L2_SEL_TGT_CROP_BOUNDS;
 
-	ret = ops->vidioc_g_selection(file, fh, &s);
+	ret = v4l_g_selection(ops, file, fh, &s);
 	if (ret)
 		return ret;
 	p->bounds = s.r;
@@ -2240,7 +2285,7 @@ static int v4l_cropcap(const struct v4l2_ioctl_ops *ops,
 	else
 		s.target = V4L2_SEL_TGT_CROP_DEFAULT;
 
-	ret = ops->vidioc_g_selection(file, fh, &s);
+	ret = v4l_g_selection(ops, file, fh, &s);
 	if (ret)
 		return ret;
 	p->defrect = s.r;
@@ -2472,20 +2517,22 @@ struct v4l2_ioctl_info {
 };
 
 /* This control needs a priority check */
-#define INFO_FL_PRIO	(1 << 0)
+#define INFO_FL_PRIO		(1 << 0)
 /* This control can be valid if the filehandle passes a control handler. */
-#define INFO_FL_CTRL	(1 << 1)
+#define INFO_FL_CTRL		(1 << 1)
 /* This is a standard ioctl, no need for special code */
-#define INFO_FL_STD	(1 << 2)
+#define INFO_FL_STD		(1 << 2)
 /* This is ioctl has its own function */
-#define INFO_FL_FUNC	(1 << 3)
+#define INFO_FL_FUNC		(1 << 3)
 /* Queuing ioctl */
-#define INFO_FL_QUEUE	(1 << 4)
+#define INFO_FL_QUEUE		(1 << 4)
+/* Always copy back result, even on error */
+#define INFO_FL_ALWAYS_COPY	(1 << 5)
 /* 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 INFO_FL_CLEAR_MASK 	(_IOC_SIZEMASK << 16)
 
 #define IOCTL_INFO_STD(_ioctl, _vidioc, _debug, _flags)			\
 	[_IOC_NR(_ioctl)] = {						\
@@ -2536,8 +2583,8 @@ static struct v4l2_ioctl_info v4l2_ioctls[] = {
 	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_EDID, vidioc_g_edid, v4l_print_edid, 0),
-	IOCTL_INFO_STD(VIDIOC_S_EDID, vidioc_s_edid, v4l_print_edid, INFO_FL_PRIO),
+	IOCTL_INFO_STD(VIDIOC_G_EDID, vidioc_g_edid, v4l_print_edid, INFO_FL_ALWAYS_COPY),
+	IOCTL_INFO_STD(VIDIOC_S_EDID, vidioc_s_edid, v4l_print_edid, INFO_FL_PRIO | INFO_FL_ALWAYS_COPY),
 	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)),
@@ -2550,8 +2597,8 @@ static struct v4l2_ioctl_info v4l2_ioctls[] = {
 	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, INFO_FL_CLEAR(v4l2_selection, r)),
-	IOCTL_INFO_STD(VIDIOC_S_SELECTION, vidioc_s_selection, v4l_print_selection, INFO_FL_PRIO | INFO_FL_CLEAR(v4l2_selection, r)),
+	IOCTL_INFO_FNC(VIDIOC_G_SELECTION, v4l_g_selection, v4l_print_selection, INFO_FL_CLEAR(v4l2_selection, r)),
+	IOCTL_INFO_FNC(VIDIOC_S_SELECTION, v4l_s_selection, v4l_print_selection, INFO_FL_PRIO | INFO_FL_CLEAR(v4l2_selection, r)),
 	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),
@@ -2583,7 +2630,7 @@ static struct v4l2_ioctl_info v4l2_ioctls[] = {
 	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, INFO_FL_CLEAR(v4l2_enum_dv_timings, pad)),
-	IOCTL_INFO_STD(VIDIOC_QUERY_DV_TIMINGS, vidioc_query_dv_timings, v4l_print_dv_timings, 0),
+	IOCTL_INFO_STD(VIDIOC_QUERY_DV_TIMINGS, vidioc_query_dv_timings, v4l_print_dv_timings, INFO_FL_ALWAYS_COPY),
 	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),
 	IOCTL_INFO_FNC(VIDIOC_DBG_G_CHIP_INFO, v4l_dbg_g_chip_info, v4l_print_dbg_chip_info, INFO_FL_CLEAR(v4l2_dbg_chip_info, match)),
@@ -2801,6 +2848,7 @@ video_usercopy(struct file *file, unsigned int cmd, unsigned long arg,
 	void	*parg = (void *)arg;
 	long	err  = -EINVAL;
 	bool	has_array_args;
+	bool	always_copy = false;
 	size_t  array_size = 0;
 	void __user *user_ptr = NULL;
 	void	**kernel_ptr = NULL;
@@ -2811,7 +2859,7 @@ video_usercopy(struct file *file, unsigned int cmd, unsigned long arg,
 			parg = sbuf;
 		} else {
 			/* too big to allocate from stack */
-			mbuf = kmalloc(_IOC_SIZE(cmd), GFP_KERNEL);
+			mbuf = kvmalloc(_IOC_SIZE(cmd), GFP_KERNEL);
 			if (NULL == mbuf)
 				return -ENOMEM;
 			parg = mbuf;
@@ -2830,8 +2878,10 @@ video_usercopy(struct file *file, unsigned int cmd, unsigned long arg,
 			 */
 			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;
+				always_copy = flags & INFO_FL_ALWAYS_COPY;
 			}
 
 			if (copy_from_user(parg, (void __user *)arg, n))
@@ -2858,7 +2908,7 @@ video_usercopy(struct file *file, unsigned int cmd, unsigned long arg,
 		 * array) fits into sbuf (so that mbuf will still remain
 		 * unused up to here).
 		 */
-		mbuf = kmalloc(array_size, GFP_KERNEL);
+		mbuf = kvmalloc(array_size, GFP_KERNEL);
 		err = -ENOMEM;
 		if (NULL == mbuf)
 			goto out_array_args;
@@ -2885,9 +2935,11 @@ video_usercopy(struct file *file, unsigned int cmd, unsigned long arg,
 			err = -EFAULT;
 		goto out_array_args;
 	}
-	/* VIDIOC_QUERY_DV_TIMINGS can return an error, but still have valid
-	   results that must be returned. */
-	if (err < 0 && cmd != VIDIOC_QUERY_DV_TIMINGS)
+	/*
+	 * Some ioctls can return an error, but still have valid
+	 * results that must be returned.
+	 */
+	if (err < 0 && !always_copy)
 		goto out;
 
 out_array_args:
@@ -2901,7 +2953,7 @@ out_array_args:
 	}
 
 out:
-	kfree(mbuf);
+	kvfree(mbuf);
 	return err;
 }
 EXPORT_SYMBOL(video_usercopy);
diff --git a/drivers/media/v4l2-core/v4l2-mem2mem.c b/drivers/media/v4l2-core/v4l2-mem2mem.c
index 6bc27e7b2a33..f62e68aa04c4 100644
--- a/drivers/media/v4l2-core/v4l2-mem2mem.c
+++ b/drivers/media/v4l2-core/v4l2-mem2mem.c
@@ -126,6 +126,43 @@ void *v4l2_m2m_buf_remove(struct v4l2_m2m_queue_ctx *q_ctx)
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_buf_remove);
 
+void v4l2_m2m_buf_remove_by_buf(struct v4l2_m2m_queue_ctx *q_ctx,
+				struct vb2_v4l2_buffer *vbuf)
+{
+	struct v4l2_m2m_buffer *b;
+	unsigned long flags;
+
+	spin_lock_irqsave(&q_ctx->rdy_spinlock, flags);
+	b = container_of(vbuf, struct v4l2_m2m_buffer, vb);
+	list_del(&b->list);
+	q_ctx->num_rdy--;
+	spin_unlock_irqrestore(&q_ctx->rdy_spinlock, flags);
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_buf_remove_by_buf);
+
+struct vb2_v4l2_buffer *
+v4l2_m2m_buf_remove_by_idx(struct v4l2_m2m_queue_ctx *q_ctx, unsigned int idx)
+
+{
+	struct v4l2_m2m_buffer *b, *tmp;
+	struct vb2_v4l2_buffer *ret = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&q_ctx->rdy_spinlock, flags);
+	list_for_each_entry_safe(b, tmp, &q_ctx->rdy_queue, list) {
+		if (b->vb.vb2_buf.index == idx) {
+			list_del(&b->list);
+			q_ctx->num_rdy--;
+			ret = &b->vb;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&q_ctx->rdy_spinlock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_buf_remove_by_idx);
+
 /*
  * Scheduling handlers
  */
diff --git a/drivers/media/v4l2-core/v4l2-of.c b/drivers/media/v4l2-core/v4l2-of.c
deleted file mode 100644
index 4f59f442dd0a..000000000000
--- a/drivers/media/v4l2-core/v4l2-of.c
+++ /dev/null
@@ -1,327 +0,0 @@
-/*
- * V4L2 OF binding parsing library
- *
- * Copyright (C) 2012 - 2013 Samsung Electronics Co., Ltd.
- * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
- *
- * Copyright (C) 2012 Renesas Electronics Corp.
- * Author: Guennadi Liakhovetski <g.liakhovetski@gmx.de>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- */
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/of.h>
-#include <linux/slab.h>
-#include <linux/string.h>
-#include <linux/types.h>
-
-#include <media/v4l2-of.h>
-
-static int v4l2_of_parse_csi_bus(const struct device_node *node,
-				 struct v4l2_of_endpoint *endpoint)
-{
-	struct v4l2_of_bus_mipi_csi2 *bus = &endpoint->bus.mipi_csi2;
-	struct property *prop;
-	bool have_clk_lane = false;
-	unsigned int flags = 0, lanes_used = 0;
-	u32 v;
-
-	prop = of_find_property(node, "data-lanes", NULL);
-	if (prop) {
-		const __be32 *lane = NULL;
-		unsigned int i;
-
-		for (i = 0; i < ARRAY_SIZE(bus->data_lanes); i++) {
-			lane = of_prop_next_u32(prop, lane, &v);
-			if (!lane)
-				break;
-
-			if (lanes_used & BIT(v))
-				pr_warn("%s: duplicated lane %u in data-lanes\n",
-					node->full_name, v);
-			lanes_used |= BIT(v);
-
-			bus->data_lanes[i] = v;
-		}
-		bus->num_data_lanes = i;
-	}
-
-	prop = of_find_property(node, "lane-polarities", NULL);
-	if (prop) {
-		const __be32 *polarity = NULL;
-		unsigned int i;
-
-		for (i = 0; i < ARRAY_SIZE(bus->lane_polarities); i++) {
-			polarity = of_prop_next_u32(prop, polarity, &v);
-			if (!polarity)
-				break;
-			bus->lane_polarities[i] = v;
-		}
-
-		if (i < 1 + bus->num_data_lanes /* clock + data */) {
-			pr_warn("%s: too few lane-polarities entries (need %u, got %u)\n",
-				node->full_name, 1 + bus->num_data_lanes, i);
-			return -EINVAL;
-		}
-	}
-
-	if (!of_property_read_u32(node, "clock-lanes", &v)) {
-		if (lanes_used & BIT(v))
-			pr_warn("%s: duplicated lane %u in clock-lanes\n",
-				node->full_name, v);
-		lanes_used |= BIT(v);
-
-		bus->clock_lane = v;
-		have_clk_lane = true;
-	}
-
-	if (of_get_property(node, "clock-noncontinuous", &v))
-		flags |= V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
-	else if (have_clk_lane || bus->num_data_lanes > 0)
-		flags |= V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
-
-	bus->flags = flags;
-	endpoint->bus_type = V4L2_MBUS_CSI2;
-
-	return 0;
-}
-
-static void v4l2_of_parse_parallel_bus(const struct device_node *node,
-				       struct v4l2_of_endpoint *endpoint)
-{
-	struct v4l2_of_bus_parallel *bus = &endpoint->bus.parallel;
-	unsigned int flags = 0;
-	u32 v;
-
-	if (!of_property_read_u32(node, "hsync-active", &v))
-		flags |= v ? V4L2_MBUS_HSYNC_ACTIVE_HIGH :
-			V4L2_MBUS_HSYNC_ACTIVE_LOW;
-
-	if (!of_property_read_u32(node, "vsync-active", &v))
-		flags |= v ? V4L2_MBUS_VSYNC_ACTIVE_HIGH :
-			V4L2_MBUS_VSYNC_ACTIVE_LOW;
-
-	if (!of_property_read_u32(node, "field-even-active", &v))
-		flags |= v ? V4L2_MBUS_FIELD_EVEN_HIGH :
-			V4L2_MBUS_FIELD_EVEN_LOW;
-	if (flags)
-		endpoint->bus_type = V4L2_MBUS_PARALLEL;
-	else
-		endpoint->bus_type = V4L2_MBUS_BT656;
-
-	if (!of_property_read_u32(node, "pclk-sample", &v))
-		flags |= v ? V4L2_MBUS_PCLK_SAMPLE_RISING :
-			V4L2_MBUS_PCLK_SAMPLE_FALLING;
-
-	if (!of_property_read_u32(node, "data-active", &v))
-		flags |= v ? V4L2_MBUS_DATA_ACTIVE_HIGH :
-			V4L2_MBUS_DATA_ACTIVE_LOW;
-
-	if (of_get_property(node, "slave-mode", &v))
-		flags |= V4L2_MBUS_SLAVE;
-	else
-		flags |= V4L2_MBUS_MASTER;
-
-	if (!of_property_read_u32(node, "bus-width", &v))
-		bus->bus_width = v;
-
-	if (!of_property_read_u32(node, "data-shift", &v))
-		bus->data_shift = v;
-
-	if (!of_property_read_u32(node, "sync-on-green-active", &v))
-		flags |= v ? V4L2_MBUS_VIDEO_SOG_ACTIVE_HIGH :
-			V4L2_MBUS_VIDEO_SOG_ACTIVE_LOW;
-
-	bus->flags = flags;
-
-}
-
-/**
- * v4l2_of_parse_endpoint() - parse all endpoint node properties
- * @node: pointer to endpoint device_node
- * @endpoint: pointer to the V4L2 OF endpoint data structure
- *
- * All properties are optional. If none are found, we don't set any flags.
- * This means the port has a static configuration and no properties have
- * to be specified explicitly.
- * If any properties that identify the bus as parallel are found and
- * slave-mode isn't set, we set V4L2_MBUS_MASTER. Similarly, if we recognise
- * the bus as serial CSI-2 and clock-noncontinuous isn't set, we set the
- * V4L2_MBUS_CSI2_CONTINUOUS_CLOCK flag.
- * The caller should hold a reference to @node.
- *
- * NOTE: This function does not parse properties the size of which is
- * variable without a low fixed limit. Please use
- * v4l2_of_alloc_parse_endpoint() in new drivers instead.
- *
- * Return: 0 on success or a negative error code on failure.
- */
-int v4l2_of_parse_endpoint(const struct device_node *node,
-			   struct v4l2_of_endpoint *endpoint)
-{
-	int rval;
-
-	of_graph_parse_endpoint(node, &endpoint->base);
-	/* Zero fields from bus_type to until the end */
-	memset(&endpoint->bus_type, 0, sizeof(*endpoint) -
-	       offsetof(typeof(*endpoint), bus_type));
-
-	rval = v4l2_of_parse_csi_bus(node, endpoint);
-	if (rval)
-		return rval;
-	/*
-	 * Parse the parallel video bus properties only if none
-	 * of the MIPI CSI-2 specific properties were found.
-	 */
-	if (endpoint->bus.mipi_csi2.flags == 0)
-		v4l2_of_parse_parallel_bus(node, endpoint);
-
-	return 0;
-}
-EXPORT_SYMBOL(v4l2_of_parse_endpoint);
-
-/*
- * v4l2_of_free_endpoint() - free the endpoint acquired by
- * v4l2_of_alloc_parse_endpoint()
- * @endpoint - the endpoint the resources of which are to be released
- *
- * It is safe to call this function with NULL argument or on an
- * endpoint the parsing of which failed.
- */
-void v4l2_of_free_endpoint(struct v4l2_of_endpoint *endpoint)
-{
-	if (IS_ERR_OR_NULL(endpoint))
-		return;
-
-	kfree(endpoint->link_frequencies);
-	kfree(endpoint);
-}
-EXPORT_SYMBOL(v4l2_of_free_endpoint);
-
-/**
- * v4l2_of_alloc_parse_endpoint() - parse all endpoint node properties
- * @node: pointer to endpoint device_node
- *
- * All properties are optional. If none are found, we don't set any flags.
- * This means the port has a static configuration and no properties have
- * to be specified explicitly.
- * If any properties that identify the bus as parallel are found and
- * slave-mode isn't set, we set V4L2_MBUS_MASTER. Similarly, if we recognise
- * the bus as serial CSI-2 and clock-noncontinuous isn't set, we set the
- * V4L2_MBUS_CSI2_CONTINUOUS_CLOCK flag.
- * The caller should hold a reference to @node.
- *
- * v4l2_of_alloc_parse_endpoint() has two important differences to
- * v4l2_of_parse_endpoint():
- *
- * 1. It also parses variable size data and
- *
- * 2. The memory it has allocated to store the variable size data must
- *    be freed using v4l2_of_free_endpoint() when no longer needed.
- *
- * Return: Pointer to v4l2_of_endpoint if successful, on error a
- * negative error code.
- */
-struct v4l2_of_endpoint *v4l2_of_alloc_parse_endpoint(
-	const struct device_node *node)
-{
-	struct v4l2_of_endpoint *endpoint;
-	int len;
-	int rval;
-
-	endpoint = kzalloc(sizeof(*endpoint), GFP_KERNEL);
-	if (!endpoint)
-		return ERR_PTR(-ENOMEM);
-
-	rval = v4l2_of_parse_endpoint(node, endpoint);
-	if (rval < 0)
-		goto out_err;
-
-	if (of_get_property(node, "link-frequencies", &len)) {
-		endpoint->link_frequencies = kmalloc(len, GFP_KERNEL);
-		if (!endpoint->link_frequencies) {
-			rval = -ENOMEM;
-			goto out_err;
-		}
-
-		endpoint->nr_of_link_frequencies =
-			len / sizeof(*endpoint->link_frequencies);
-
-		rval = of_property_read_u64_array(
-			node, "link-frequencies", endpoint->link_frequencies,
-			endpoint->nr_of_link_frequencies);
-		if (rval < 0)
-			goto out_err;
-	}
-
-	return endpoint;
-
-out_err:
-	v4l2_of_free_endpoint(endpoint);
-	return ERR_PTR(rval);
-}
-EXPORT_SYMBOL(v4l2_of_alloc_parse_endpoint);
-
-/**
- * v4l2_of_parse_link() - parse a link between two endpoints
- * @node: pointer to the endpoint at the local end of the link
- * @link: pointer to the V4L2 OF link data structure
- *
- * Fill the link structure with the local and remote nodes and port numbers.
- * The local_node and remote_node fields are set to point to the local and
- * remote port's parent nodes respectively (the port parent node being the
- * parent node of the port node if that node isn't a 'ports' node, or the
- * grand-parent node of the port node otherwise).
- *
- * A reference is taken to both the local and remote nodes, the caller must use
- * v4l2_of_put_link() to drop the references when done with the link.
- *
- * Return: 0 on success, or -ENOLINK if the remote endpoint can't be found.
- */
-int v4l2_of_parse_link(const struct device_node *node,
-		       struct v4l2_of_link *link)
-{
-	struct device_node *np;
-
-	memset(link, 0, sizeof(*link));
-
-	np = of_get_parent(node);
-	of_property_read_u32(np, "reg", &link->local_port);
-	np = of_get_next_parent(np);
-	if (of_node_cmp(np->name, "ports") == 0)
-		np = of_get_next_parent(np);
-	link->local_node = np;
-
-	np = of_parse_phandle(node, "remote-endpoint", 0);
-	if (!np) {
-		of_node_put(link->local_node);
-		return -ENOLINK;
-	}
-
-	np = of_get_parent(np);
-	of_property_read_u32(np, "reg", &link->remote_port);
-	np = of_get_next_parent(np);
-	if (of_node_cmp(np->name, "ports") == 0)
-		np = of_get_next_parent(np);
-	link->remote_node = np;
-
-	return 0;
-}
-EXPORT_SYMBOL(v4l2_of_parse_link);
-
-/**
- * v4l2_of_put_link() - drop references to nodes in a link
- * @link: pointer to the V4L2 OF link data structure
- *
- * Drop references to the local and remote nodes in the link. This function must
- * be called on every link parsed with v4l2_of_parse_link().
- */
-void v4l2_of_put_link(struct v4l2_of_link *link)
-{
-	of_node_put(link->local_node);
-	of_node_put(link->remote_node);
-}
-EXPORT_SYMBOL(v4l2_of_put_link);
diff --git a/drivers/media/v4l2-core/v4l2-subdev.c b/drivers/media/v4l2-core/v4l2-subdev.c
index da78497ae5ed..43fefa73e0a3 100644
--- a/drivers/media/v4l2-core/v4l2-subdev.c
+++ b/drivers/media/v4l2-core/v4l2-subdev.c
@@ -17,6 +17,7 @@
  */
 
 #include <linux/ioctl.h>
+#include <linux/mm.h>
 #include <linux/slab.h>
 #include <linux/types.h>
 #include <linux/videodev2.h>
@@ -577,13 +578,14 @@ v4l2_subdev_alloc_pad_config(struct v4l2_subdev *sd)
 	if (!sd->entity.num_pads)
 		return NULL;
 
-	cfg = kcalloc(sd->entity.num_pads, sizeof(*cfg), GFP_KERNEL);
+	cfg = kvmalloc_array(sd->entity.num_pads, sizeof(*cfg),
+			     GFP_KERNEL | __GFP_ZERO);
 	if (!cfg)
 		return NULL;
 
 	ret = v4l2_subdev_call(sd, pad, init_cfg, cfg);
 	if (ret < 0 && ret != -ENOIOCTLCMD) {
-		kfree(cfg);
+		kvfree(cfg);
 		return NULL;
 	}
 
@@ -593,7 +595,7 @@ EXPORT_SYMBOL_GPL(v4l2_subdev_alloc_pad_config);
 
 void v4l2_subdev_free_pad_config(struct v4l2_subdev_pad_config *cfg)
 {
-	kfree(cfg);
+	kvfree(cfg);
 }
 EXPORT_SYMBOL_GPL(v4l2_subdev_free_pad_config);
 #endif /* CONFIG_MEDIA_CONTROLLER */
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index 94afbbf92807..14f83cecfa92 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -210,7 +210,7 @@ static int __vb2_buf_mem_alloc(struct vb2_buffer *vb)
 		mem_priv = call_ptr_memop(vb, alloc,
 				q->alloc_devs[plane] ? : q->dev,
 				q->dma_attrs, size, dma_dir, q->gfp_flags);
-		if (IS_ERR(mem_priv)) {
+		if (IS_ERR_OR_NULL(mem_priv)) {
 			if (mem_priv)
 				ret = PTR_ERR(mem_priv);
 			goto free;
@@ -868,7 +868,7 @@ EXPORT_SYMBOL_GPL(vb2_core_create_bufs);
 
 void *vb2_plane_vaddr(struct vb2_buffer *vb, unsigned int plane_no)
 {
-	if (plane_no > vb->num_planes || !vb->planes[plane_no].mem_priv)
+	if (plane_no >= vb->num_planes || !vb->planes[plane_no].mem_priv)
 		return NULL;
 
 	return call_ptr_memop(vb, vaddr, vb->planes[plane_no].mem_priv);
@@ -956,9 +956,9 @@ void vb2_discard_done(struct vb2_queue *q)
 EXPORT_SYMBOL_GPL(vb2_discard_done);
 
 /**
- * __qbuf_mmap() - handle qbuf of an MMAP buffer
+ * __prepare_mmap() - prepare an MMAP buffer
  */
-static int __qbuf_mmap(struct vb2_buffer *vb, const void *pb)
+static int __prepare_mmap(struct vb2_buffer *vb, const void *pb)
 {
 	int ret = 0;
 
@@ -969,9 +969,9 @@ static int __qbuf_mmap(struct vb2_buffer *vb, const void *pb)
 }
 
 /**
- * __qbuf_userptr() - handle qbuf of a USERPTR buffer
+ * __prepare_userptr() - prepare a USERPTR buffer
  */
-static int __qbuf_userptr(struct vb2_buffer *vb, const void *pb)
+static int __prepare_userptr(struct vb2_buffer *vb, const void *pb)
 {
 	struct vb2_plane planes[VB2_MAX_PLANES];
 	struct vb2_queue *q = vb->vb2_queue;
@@ -1087,9 +1087,9 @@ err:
 }
 
 /**
- * __qbuf_dmabuf() - handle qbuf of a DMABUF buffer
+ * __prepare_dmabuf() - prepare a DMABUF buffer
  */
-static int __qbuf_dmabuf(struct vb2_buffer *vb, const void *pb)
+static int __prepare_dmabuf(struct vb2_buffer *vb, const void *pb)
 {
 	struct vb2_plane planes[VB2_MAX_PLANES];
 	struct vb2_queue *q = vb->vb2_queue;
@@ -1227,23 +1227,19 @@ err:
 static void __enqueue_in_driver(struct vb2_buffer *vb)
 {
 	struct vb2_queue *q = vb->vb2_queue;
-	unsigned int plane;
 
 	vb->state = VB2_BUF_STATE_ACTIVE;
 	atomic_inc(&q->owned_by_drv_count);
 
 	trace_vb2_buf_queue(q, vb);
 
-	/* sync buffers */
-	for (plane = 0; plane < vb->num_planes; ++plane)
-		call_void_memop(vb, prepare, vb->planes[plane].mem_priv);
-
 	call_void_vb_qop(vb, buf_queue, vb);
 }
 
 static int __buf_prepare(struct vb2_buffer *vb, const void *pb)
 {
 	struct vb2_queue *q = vb->vb2_queue;
+	unsigned int plane;
 	int ret;
 
 	if (q->error) {
@@ -1255,24 +1251,32 @@ static int __buf_prepare(struct vb2_buffer *vb, const void *pb)
 
 	switch (q->memory) {
 	case VB2_MEMORY_MMAP:
-		ret = __qbuf_mmap(vb, pb);
+		ret = __prepare_mmap(vb, pb);
 		break;
 	case VB2_MEMORY_USERPTR:
-		ret = __qbuf_userptr(vb, pb);
+		ret = __prepare_userptr(vb, pb);
 		break;
 	case VB2_MEMORY_DMABUF:
-		ret = __qbuf_dmabuf(vb, pb);
+		ret = __prepare_dmabuf(vb, pb);
 		break;
 	default:
 		WARN(1, "Invalid queue type\n");
 		ret = -EINVAL;
 	}
 
-	if (ret)
+	if (ret) {
 		dprintk(1, "buffer preparation failed: %d\n", ret);
-	vb->state = ret ? VB2_BUF_STATE_DEQUEUED : VB2_BUF_STATE_PREPARED;
+		vb->state = VB2_BUF_STATE_DEQUEUED;
+		return ret;
+	}
 
-	return ret;
+	/* sync buffers */
+	for (plane = 0; plane < vb->num_planes; ++plane)
+		call_void_memop(vb, prepare, vb->planes[plane].mem_priv);
+
+	vb->state = VB2_BUF_STATE_PREPARED;
+
+	return 0;
 }
 
 int vb2_core_prepare_buf(struct vb2_queue *q, unsigned int index, void *pb)
diff --git a/drivers/media/v4l2-core/videobuf2-dma-sg.c b/drivers/media/v4l2-core/videobuf2-dma-sg.c
index 8e8798a74760..5defa1f22ca2 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-sg.c
@@ -120,8 +120,8 @@ static void *vb2_dma_sg_alloc(struct device *dev, unsigned long dma_attrs,
 	buf->num_pages = size >> PAGE_SHIFT;
 	buf->dma_sgt = &buf->sg_table;
 
-	buf->pages = kzalloc(buf->num_pages * sizeof(struct page *),
-			     GFP_KERNEL);
+	buf->pages = kvmalloc_array(buf->num_pages, sizeof(struct page *),
+				    GFP_KERNEL | __GFP_ZERO);
 	if (!buf->pages)
 		goto fail_pages_array_alloc;
 
@@ -165,7 +165,7 @@ fail_table_alloc:
 	while (num_pages--)
 		__free_page(buf->pages[num_pages]);
 fail_pages_alloc:
-	kfree(buf->pages);
+	kvfree(buf->pages);
 fail_pages_array_alloc:
 	kfree(buf);
 	return ERR_PTR(-ENOMEM);
@@ -187,7 +187,7 @@ static void vb2_dma_sg_put(void *buf_priv)
 		sg_free_table(buf->dma_sgt);
 		while (--i >= 0)
 			__free_page(buf->pages[i]);
-		kfree(buf->pages);
+		kvfree(buf->pages);
 		put_device(buf->dev);
 		kfree(buf);
 	}
diff --git a/drivers/memory/omap-gpmc.c b/drivers/memory/omap-gpmc.c
index 6d1b4b707cc2..a80e17de906d 100644
--- a/drivers/memory/omap-gpmc.c
+++ b/drivers/memory/omap-gpmc.c
@@ -460,12 +460,12 @@ static int get_gpmc_timing_reg(
 		if (l)
 			time_ns_min = gpmc_clk_ticks_to_ns(l - 1, cs, cd) + 1;
 		time_ns = gpmc_clk_ticks_to_ns(l, cs, cd);
-		pr_info("gpmc,%s = <%u> /* %u ns - %u ns; %i ticks%s*/\n",
+		pr_info("gpmc,%s = <%u>; /* %u ns - %u ns; %i ticks%s*/\n",
 			name, time_ns, time_ns_min, time_ns, l,
 			invalid ? "; invalid " : " ");
 	} else {
 		/* raw format */
-		pr_info("gpmc,%s = <%u>%s\n", name, l,
+		pr_info("gpmc,%s = <%u>;%s\n", name, l,
 			invalid ? " /* invalid */" : "");
 	}
 
@@ -2083,8 +2083,11 @@ static int gpmc_probe_generic_child(struct platform_device *pdev,
 	} else {
 		ret = of_property_read_u32(child, "bank-width",
 					   &gpmc_s.device_width);
-		if (ret < 0)
+		if (ret < 0) {
+			dev_err(&pdev->dev, "%s has no 'bank-width' property\n",
+				child->full_name);
 			goto err;
+		}
 	}
 
 	/* Reserve wait pin if it is required and valid */
diff --git a/drivers/memory/ti-aemif.c b/drivers/memory/ti-aemif.c
index 22c1aeeb6421..2744b1b91b57 100644
--- a/drivers/memory/ti-aemif.c
+++ b/drivers/memory/ti-aemif.c
@@ -357,7 +357,10 @@ static int aemif_probe(struct platform_device *pdev)
 		return PTR_ERR(aemif->clk);
 	}
 
-	clk_prepare_enable(aemif->clk);
+	ret = clk_prepare_enable(aemif->clk);
+	if (ret)
+		return ret;
+
 	aemif->clk_rate = clk_get_rate(aemif->clk) / MSEC_PER_SEC;
 
 	if (of_device_is_compatible(np, "ti,da850-aemif"))
diff --git a/drivers/memstick/core/ms_block.c b/drivers/memstick/core/ms_block.c
index 99e651c27fb7..22de7f5ed032 100644
--- a/drivers/memstick/core/ms_block.c
+++ b/drivers/memstick/core/ms_block.c
@@ -1921,12 +1921,13 @@ static void msb_io_work(struct work_struct *work)
 		spin_lock_irqsave(&msb->q_lock, flags);
 
 		if (len)
-			if (!__blk_end_request(msb->req, 0, len))
+			if (!__blk_end_request(msb->req, BLK_STS_OK, len))
 				msb->req = NULL;
 
 		if (error && msb->req) {
+			blk_status_t ret = errno_to_blk_status(error);
 			dbg_verbose("IO: ending one sector of the request with error");
-			if (!__blk_end_request(msb->req, error, msb->page_size))
+			if (!__blk_end_request(msb->req, ret, msb->page_size))
 				msb->req = NULL;
 		}
 
@@ -2014,7 +2015,7 @@ static void msb_submit_req(struct request_queue *q)
 		WARN_ON(!msb->io_queue_stopped);
 
 		while ((req = blk_fetch_request(q)) != NULL)
-			__blk_end_request_all(req, -ENODEV);
+			__blk_end_request_all(req, BLK_STS_IOERR);
 		return;
 	}
 
diff --git a/drivers/memstick/core/mspro_block.c b/drivers/memstick/core/mspro_block.c
index c00d8a266878..8897962781bb 100644
--- a/drivers/memstick/core/mspro_block.c
+++ b/drivers/memstick/core/mspro_block.c
@@ -709,7 +709,8 @@ try_again:
 					       msb->req_sg);
 
 		if (!msb->seg_count) {
-			chunk = __blk_end_request_cur(msb->block_req, -ENOMEM);
+			chunk = __blk_end_request_cur(msb->block_req,
+					BLK_STS_RESOURCE);
 			continue;
 		}
 
@@ -776,7 +777,8 @@ static int mspro_block_complete_req(struct memstick_dev *card, int error)
 		if (error && !t_len)
 			t_len = blk_rq_cur_bytes(msb->block_req);
 
-		chunk = __blk_end_request(msb->block_req, error, t_len);
+		chunk = __blk_end_request(msb->block_req,
+				errno_to_blk_status(error), t_len);
 
 		error = mspro_block_issue_req(card, chunk);
 
@@ -838,7 +840,7 @@ static void mspro_block_submit_req(struct request_queue *q)
 
 	if (msb->eject) {
 		while ((req = blk_fetch_request(q)) != NULL)
-			__blk_end_request_all(req, -ENODEV);
+			__blk_end_request_all(req, BLK_STS_IOERR);
 
 		return;
 	}
diff --git a/drivers/mfd/arizona-core.c b/drivers/mfd/arizona-core.c
index 75488e65cd96..8d46e3ad9529 100644
--- a/drivers/mfd/arizona-core.c
+++ b/drivers/mfd/arizona-core.c
@@ -245,8 +245,7 @@ static int arizona_poll_reg(struct arizona *arizona,
 	int ret;
 
 	ret = regmap_read_poll_timeout(arizona->regmap,
-				       ARIZONA_INTERRUPT_RAW_STATUS_5, val,
-				       ((val & mask) == target),
+				       reg, val, ((val & mask) == target),
 				       ARIZONA_REG_POLL_DELAY_US,
 				       timeout_ms * 1000);
 	if (ret)
diff --git a/drivers/mfd/tps65910.c b/drivers/mfd/tps65910.c
index 11cab1582f2f..8263605f6d2f 100644
--- a/drivers/mfd/tps65910.c
+++ b/drivers/mfd/tps65910.c
@@ -328,11 +328,7 @@ static int tps65910_sleepinit(struct tps65910 *tps65910,
 		goto err_sleep_init;
 	}
 
-	/* Return if there is no sleep keepon data. */
-	if (!pmic_pdata->slp_keepon)
-		return 0;
-
-	if (pmic_pdata->slp_keepon->therm_keepon) {
+	if (pmic_pdata->slp_keepon.therm_keepon) {
 		ret = tps65910_reg_set_bits(tps65910,
 				TPS65910_SLEEP_KEEP_RES_ON,
 				SLEEP_KEEP_RES_ON_THERM_KEEPON_MASK);
@@ -342,7 +338,7 @@ static int tps65910_sleepinit(struct tps65910 *tps65910,
 		}
 	}
 
-	if (pmic_pdata->slp_keepon->clkout32k_keepon) {
+	if (pmic_pdata->slp_keepon.clkout32k_keepon) {
 		ret = tps65910_reg_set_bits(tps65910,
 				TPS65910_SLEEP_KEEP_RES_ON,
 				SLEEP_KEEP_RES_ON_CLKOUT32K_KEEPON_MASK);
@@ -352,7 +348,7 @@ static int tps65910_sleepinit(struct tps65910 *tps65910,
 		}
 	}
 
-	if (pmic_pdata->slp_keepon->i2chs_keepon) {
+	if (pmic_pdata->slp_keepon.i2chs_keepon) {
 		ret = tps65910_reg_set_bits(tps65910,
 				TPS65910_SLEEP_KEEP_RES_ON,
 				SLEEP_KEEP_RES_ON_I2CHS_KEEPON_MASK);
@@ -415,6 +411,18 @@ static struct tps65910_board *tps65910_parse_dt(struct i2c_client *client,
 	prop = of_property_read_bool(np, "ti,en-ck32k-xtal");
 	board_info->en_ck32k_xtal = prop;
 
+	prop = of_property_read_bool(np, "ti,sleep-enable");
+	board_info->en_dev_slp = prop;
+
+	prop = of_property_read_bool(np, "ti,sleep-keep-therm");
+	board_info->slp_keepon.therm_keepon = prop;
+
+	prop = of_property_read_bool(np, "ti,sleep-keep-ck32k");
+	board_info->slp_keepon.clkout32k_keepon = prop;
+
+	prop = of_property_read_bool(np, "ti,sleep-keep-hsclk");
+	board_info->slp_keepon.i2chs_keepon = prop;
+
 	board_info->irq = client->irq;
 	board_info->irq_base = -1;
 	board_info->pm_off = of_property_read_bool(np,
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 07bbd4cc1852..8136dc7e863d 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -490,6 +490,14 @@ config ASPEED_LPC_CTRL
 	  ioctl()s, the driver also provides a read/write interface to a BMC ram
 	  region where the host LPC read/write region can be buffered.
 
+config ASPEED_LPC_SNOOP
+	tristate "Aspeed ast2500 HOST LPC snoop support"
+	depends on (ARCH_ASPEED || COMPILE_TEST) && REGMAP && MFD_SYSCON
+	help
+	  Provides a driver to control the LPC snoop interface which
+	  allows the BMC to listen on and save the data written by
+	  the host to an arbitrary LPC I/O port.
+
 config PCI_ENDPOINT_TEST
 	depends on PCI
 	select CRC32
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 81ef3e67acc9..b0b766416306 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -53,6 +53,7 @@ obj-$(CONFIG_ECHO)		+= echo/
 obj-$(CONFIG_VEXPRESS_SYSCFG)	+= vexpress-syscfg.o
 obj-$(CONFIG_CXL_BASE)		+= cxl/
 obj-$(CONFIG_ASPEED_LPC_CTRL)	+= aspeed-lpc-ctrl.o
+obj-$(CONFIG_ASPEED_LPC_SNOOP)	+= aspeed-lpc-snoop.o
 obj-$(CONFIG_PCI_ENDPOINT_TEST)	+= pci_endpoint_test.o
 
 lkdtm-$(CONFIG_LKDTM)		+= lkdtm_core.o
diff --git a/drivers/misc/apds990x.c b/drivers/misc/apds990x.c
index dfb72ecfa604..84e5b949399e 100644
--- a/drivers/misc/apds990x.c
+++ b/drivers/misc/apds990x.c
@@ -32,7 +32,7 @@
 #include <linux/delay.h>
 #include <linux/wait.h>
 #include <linux/slab.h>
-#include <linux/i2c/apds990x.h>
+#include <linux/platform_data/apds990x.h>
 
 /* Register map */
 #define APDS990X_ENABLE	 0x00 /* Enable of states and interrupts */
@@ -841,7 +841,7 @@ static ssize_t apds990x_prox_enable_store(struct device *dev,
 static DEVICE_ATTR(prox0_raw_en, S_IRUGO | S_IWUSR, apds990x_prox_enable_show,
 						   apds990x_prox_enable_store);
 
-static const char reporting_modes[][9] = {"trigger", "periodic"};
+static const char *reporting_modes[] = {"trigger", "periodic"};
 
 static ssize_t apds990x_prox_reporting_mode_show(struct device *dev,
 				   struct device_attribute *attr, char *buf)
@@ -856,13 +856,13 @@ static ssize_t apds990x_prox_reporting_mode_store(struct device *dev,
 				  const char *buf, size_t len)
 {
 	struct apds990x_chip *chip =  dev_get_drvdata(dev);
+	int ret;
 
-	if (sysfs_streq(buf, reporting_modes[0]))
-		chip->prox_continuous_mode = 0;
-	else if (sysfs_streq(buf, reporting_modes[1]))
-		chip->prox_continuous_mode = 1;
-	else
-		return -EINVAL;
+	ret = sysfs_match_string(reporting_modes, buf);
+	if (ret < 0)
+		return ret;
+
+	chip->prox_continuous_mode = ret;
 	return len;
 }
 
diff --git a/drivers/misc/aspeed-lpc-snoop.c b/drivers/misc/aspeed-lpc-snoop.c
new file mode 100644
index 000000000000..593905565b74
--- /dev/null
+++ b/drivers/misc/aspeed-lpc-snoop.c
@@ -0,0 +1,261 @@
+/*
+ * Copyright 2017 Google Inc
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ *
+ * Provides a simple driver to control the ASPEED LPC snoop interface which
+ * allows the BMC to listen on and save the data written by
+ * the host to an arbitrary LPC I/O port.
+ *
+ * Typically used by the BMC to "watch" host boot progress via port
+ * 0x80 writes made by the BIOS during the boot process.
+ */
+
+#include <linux/bitops.h>
+#include <linux/interrupt.h>
+#include <linux/kfifo.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#define DEVICE_NAME	"aspeed-lpc-snoop"
+
+#define NUM_SNOOP_CHANNELS 2
+#define SNOOP_FIFO_SIZE 2048
+
+#define HICR5	0x0
+#define HICR5_EN_SNP0W		BIT(0)
+#define HICR5_ENINT_SNP0W	BIT(1)
+#define HICR5_EN_SNP1W		BIT(2)
+#define HICR5_ENINT_SNP1W	BIT(3)
+
+#define HICR6	0x4
+#define HICR6_STR_SNP0W		BIT(0)
+#define HICR6_STR_SNP1W		BIT(1)
+#define SNPWADR	0x10
+#define SNPWADR_CH0_MASK	GENMASK(15, 0)
+#define SNPWADR_CH0_SHIFT	0
+#define SNPWADR_CH1_MASK	GENMASK(31, 16)
+#define SNPWADR_CH1_SHIFT	16
+#define SNPWDR	0x14
+#define SNPWDR_CH0_MASK		GENMASK(7, 0)
+#define SNPWDR_CH0_SHIFT	0
+#define SNPWDR_CH1_MASK		GENMASK(15, 8)
+#define SNPWDR_CH1_SHIFT	8
+#define HICRB	0x80
+#define HICRB_ENSNP0D		BIT(14)
+#define HICRB_ENSNP1D		BIT(15)
+
+struct aspeed_lpc_snoop {
+	struct regmap		*regmap;
+	int			irq;
+	struct kfifo		snoop_fifo[NUM_SNOOP_CHANNELS];
+};
+
+/* Save a byte to a FIFO and discard the oldest byte if FIFO is full */
+static void put_fifo_with_discard(struct kfifo *fifo, u8 val)
+{
+	if (!kfifo_initialized(fifo))
+		return;
+	if (kfifo_is_full(fifo))
+		kfifo_skip(fifo);
+	kfifo_put(fifo, val);
+}
+
+static irqreturn_t aspeed_lpc_snoop_irq(int irq, void *arg)
+{
+	struct aspeed_lpc_snoop *lpc_snoop = arg;
+	u32 reg, data;
+
+	if (regmap_read(lpc_snoop->regmap, HICR6, &reg))
+		return IRQ_NONE;
+
+	/* Check if one of the snoop channels is interrupting */
+	reg &= (HICR6_STR_SNP0W | HICR6_STR_SNP1W);
+	if (!reg)
+		return IRQ_NONE;
+
+	/* Ack pending IRQs */
+	regmap_write(lpc_snoop->regmap, HICR6, reg);
+
+	/* Read and save most recent snoop'ed data byte to FIFO */
+	regmap_read(lpc_snoop->regmap, SNPWDR, &data);
+
+	if (reg & HICR6_STR_SNP0W) {
+		u8 val = (data & SNPWDR_CH0_MASK) >> SNPWDR_CH0_SHIFT;
+
+		put_fifo_with_discard(&lpc_snoop->snoop_fifo[0], val);
+	}
+	if (reg & HICR6_STR_SNP1W) {
+		u8 val = (data & SNPWDR_CH1_MASK) >> SNPWDR_CH1_SHIFT;
+
+		put_fifo_with_discard(&lpc_snoop->snoop_fifo[1], val);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int aspeed_lpc_snoop_config_irq(struct aspeed_lpc_snoop *lpc_snoop,
+				       struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	int rc;
+
+	lpc_snoop->irq = platform_get_irq(pdev, 0);
+	if (!lpc_snoop->irq)
+		return -ENODEV;
+
+	rc = devm_request_irq(dev, lpc_snoop->irq,
+			      aspeed_lpc_snoop_irq, IRQF_SHARED,
+			      DEVICE_NAME, lpc_snoop);
+	if (rc < 0) {
+		dev_warn(dev, "Unable to request IRQ %d\n", lpc_snoop->irq);
+		lpc_snoop->irq = 0;
+		return rc;
+	}
+
+	return 0;
+}
+
+static int aspeed_lpc_enable_snoop(struct aspeed_lpc_snoop *lpc_snoop,
+				  int channel, u16 lpc_port)
+{
+	int rc = 0;
+	u32 hicr5_en, snpwadr_mask, snpwadr_shift, hicrb_en;
+
+	/* Create FIFO datastructure */
+	rc = kfifo_alloc(&lpc_snoop->snoop_fifo[channel],
+			 SNOOP_FIFO_SIZE, GFP_KERNEL);
+	if (rc)
+		return rc;
+
+	/* Enable LPC snoop channel at requested port */
+	switch (channel) {
+	case 0:
+		hicr5_en = HICR5_EN_SNP0W | HICR5_ENINT_SNP0W;
+		snpwadr_mask = SNPWADR_CH0_MASK;
+		snpwadr_shift = SNPWADR_CH0_SHIFT;
+		hicrb_en = HICRB_ENSNP0D;
+		break;
+	case 1:
+		hicr5_en = HICR5_EN_SNP1W | HICR5_ENINT_SNP1W;
+		snpwadr_mask = SNPWADR_CH1_MASK;
+		snpwadr_shift = SNPWADR_CH1_SHIFT;
+		hicrb_en = HICRB_ENSNP1D;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	regmap_update_bits(lpc_snoop->regmap, HICR5, hicr5_en, hicr5_en);
+	regmap_update_bits(lpc_snoop->regmap, SNPWADR, snpwadr_mask,
+			   lpc_port << snpwadr_shift);
+	regmap_update_bits(lpc_snoop->regmap, HICRB, hicrb_en, hicrb_en);
+
+	return rc;
+}
+
+static void aspeed_lpc_disable_snoop(struct aspeed_lpc_snoop *lpc_snoop,
+				     int channel)
+{
+	switch (channel) {
+	case 0:
+		regmap_update_bits(lpc_snoop->regmap, HICR5,
+				   HICR5_EN_SNP0W | HICR5_ENINT_SNP0W,
+				   0);
+		break;
+	case 1:
+		regmap_update_bits(lpc_snoop->regmap, HICR5,
+				   HICR5_EN_SNP1W | HICR5_ENINT_SNP1W,
+				   0);
+		break;
+	default:
+		return;
+	}
+
+	kfifo_free(&lpc_snoop->snoop_fifo[channel]);
+}
+
+static int aspeed_lpc_snoop_probe(struct platform_device *pdev)
+{
+	struct aspeed_lpc_snoop *lpc_snoop;
+	struct device *dev;
+	u32 port;
+	int rc;
+
+	dev = &pdev->dev;
+
+	lpc_snoop = devm_kzalloc(dev, sizeof(*lpc_snoop), GFP_KERNEL);
+	if (!lpc_snoop)
+		return -ENOMEM;
+
+	lpc_snoop->regmap = syscon_node_to_regmap(
+			pdev->dev.parent->of_node);
+	if (IS_ERR(lpc_snoop->regmap)) {
+		dev_err(dev, "Couldn't get regmap\n");
+		return -ENODEV;
+	}
+
+	dev_set_drvdata(&pdev->dev, lpc_snoop);
+
+	rc = of_property_read_u32_index(dev->of_node, "snoop-ports", 0, &port);
+	if (rc) {
+		dev_err(dev, "no snoop ports configured\n");
+		return -ENODEV;
+	}
+
+	rc = aspeed_lpc_snoop_config_irq(lpc_snoop, pdev);
+	if (rc)
+		return rc;
+
+	rc = aspeed_lpc_enable_snoop(lpc_snoop, 0, port);
+	if (rc)
+		return rc;
+
+	/* Configuration of 2nd snoop channel port is optional */
+	if (of_property_read_u32_index(dev->of_node, "snoop-ports",
+				       1, &port) == 0) {
+		rc = aspeed_lpc_enable_snoop(lpc_snoop, 1, port);
+		if (rc)
+			aspeed_lpc_disable_snoop(lpc_snoop, 0);
+	}
+
+	return rc;
+}
+
+static int aspeed_lpc_snoop_remove(struct platform_device *pdev)
+{
+	struct aspeed_lpc_snoop *lpc_snoop = dev_get_drvdata(&pdev->dev);
+
+	/* Disable both snoop channels */
+	aspeed_lpc_disable_snoop(lpc_snoop, 0);
+	aspeed_lpc_disable_snoop(lpc_snoop, 1);
+
+	return 0;
+}
+
+static const struct of_device_id aspeed_lpc_snoop_match[] = {
+	{ .compatible = "aspeed,ast2500-lpc-snoop" },
+	{ },
+};
+
+static struct platform_driver aspeed_lpc_snoop_driver = {
+	.driver = {
+		.name		= DEVICE_NAME,
+		.of_match_table = aspeed_lpc_snoop_match,
+	},
+	.probe = aspeed_lpc_snoop_probe,
+	.remove = aspeed_lpc_snoop_remove,
+};
+
+module_platform_driver(aspeed_lpc_snoop_driver);
+
+MODULE_DEVICE_TABLE(of, aspeed_lpc_snoop_match);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Robert Lippert <rlippert@google.com>");
+MODULE_DESCRIPTION("Linux driver to control Aspeed LPC snoop functionality");
diff --git a/drivers/misc/bh1770glc.c b/drivers/misc/bh1770glc.c
index 845466e45b95..38fcfe219d1c 100644
--- a/drivers/misc/bh1770glc.c
+++ b/drivers/misc/bh1770glc.c
@@ -27,7 +27,7 @@
 #include <linux/i2c.h>
 #include <linux/interrupt.h>
 #include <linux/mutex.h>
-#include <linux/i2c/bh1770glc.h>
+#include <linux/platform_data/bh1770glc.h>
 #include <linux/regulator/consumer.h>
 #include <linux/pm_runtime.h>
 #include <linux/workqueue.h>
diff --git a/drivers/misc/cxl/context.c b/drivers/misc/cxl/context.c
index 4472ce11f98d..8c32040b9c09 100644
--- a/drivers/misc/cxl/context.c
+++ b/drivers/misc/cxl/context.c
@@ -45,7 +45,7 @@ int cxl_context_init(struct cxl_context *ctx, struct cxl_afu *afu, bool master)
 	mutex_init(&ctx->mapping_lock);
 	ctx->mapping = NULL;
 
-	if (cxl_is_psl8(afu)) {
+	if (cxl_is_power8()) {
 		spin_lock_init(&ctx->sste_lock);
 
 		/*
@@ -189,7 +189,7 @@ int cxl_context_iomap(struct cxl_context *ctx, struct vm_area_struct *vma)
 		if (start + len > ctx->afu->adapter->ps_size)
 			return -EINVAL;
 
-		if (cxl_is_psl9(ctx->afu)) {
+		if (cxl_is_power9()) {
 			/*
 			 * Make sure there is a valid problem state
 			 * area space for this AFU.
@@ -324,7 +324,7 @@ static void reclaim_ctx(struct rcu_head *rcu)
 {
 	struct cxl_context *ctx = container_of(rcu, struct cxl_context, rcu);
 
-	if (cxl_is_psl8(ctx->afu))
+	if (cxl_is_power8())
 		free_page((u64)ctx->sstp);
 	if (ctx->ff_page)
 		__free_page(ctx->ff_page);
diff --git a/drivers/misc/cxl/cxl.h b/drivers/misc/cxl/cxl.h
index c8568ea7c518..a03f8e7535e5 100644
--- a/drivers/misc/cxl/cxl.h
+++ b/drivers/misc/cxl/cxl.h
@@ -357,6 +357,7 @@ static const cxl_p2n_reg_t CXL_PSL_WED_An     = {0x0A0};
 #define CXL_PSL9_DSISR_An_PF_RGP  0x0000000000000090ULL  /* PTE not found (Radix Guest (parent)) 0b10010000 */
 #define CXL_PSL9_DSISR_An_PF_HRH  0x0000000000000094ULL  /* PTE not found (HPT/Radix Host)       0b10010100 */
 #define CXL_PSL9_DSISR_An_PF_STEG 0x000000000000009CULL  /* PTE not found (STEG VA)              0b10011100 */
+#define CXL_PSL9_DSISR_An_URTCH   0x00000000000000B4ULL  /* Unsupported Radix Tree Configuration 0b10110100 */
 
 /****** CXL_PSL_TFC_An ******************************************************/
 #define CXL_PSL_TFC_An_A  (1ull << (63-28)) /* Acknowledge non-translation fault */
@@ -844,24 +845,15 @@ static inline bool cxl_is_power8(void)
 
 static inline bool cxl_is_power9(void)
 {
-	/* intermediate solution */
-	if (!cxl_is_power8() &&
-	   (cpu_has_feature(CPU_FTRS_POWER9) ||
-	    cpu_has_feature(CPU_FTR_POWER9_DD1)))
+	if (pvr_version_is(PVR_POWER9))
 		return true;
 	return false;
 }
 
-static inline bool cxl_is_psl8(struct cxl_afu *afu)
+static inline bool cxl_is_power9_dd1(void)
 {
-	if (afu->adapter->caia_major == 1)
-		return true;
-	return false;
-}
-
-static inline bool cxl_is_psl9(struct cxl_afu *afu)
-{
-	if (afu->adapter->caia_major == 2)
+	if ((pvr_version_is(PVR_POWER9)) &&
+	    cpu_has_feature(CPU_FTR_POWER9_DD1))
 		return true;
 	return false;
 }
diff --git a/drivers/misc/cxl/fault.c b/drivers/misc/cxl/fault.c
index 5344448f514e..c79e39bad7a4 100644
--- a/drivers/misc/cxl/fault.c
+++ b/drivers/misc/cxl/fault.c
@@ -187,7 +187,7 @@ static struct mm_struct *get_mem_context(struct cxl_context *ctx)
 
 static bool cxl_is_segment_miss(struct cxl_context *ctx, u64 dsisr)
 {
-	if ((cxl_is_psl8(ctx->afu)) && (dsisr & CXL_PSL_DSISR_An_DS))
+	if ((cxl_is_power8() && (dsisr & CXL_PSL_DSISR_An_DS)))
 		return true;
 
 	return false;
@@ -195,16 +195,23 @@ static bool cxl_is_segment_miss(struct cxl_context *ctx, u64 dsisr)
 
 static bool cxl_is_page_fault(struct cxl_context *ctx, u64 dsisr)
 {
-	if ((cxl_is_psl8(ctx->afu)) && (dsisr & CXL_PSL_DSISR_An_DM))
-		return true;
+	u64 crs; /* Translation Checkout Response Status */
 
-	if ((cxl_is_psl9(ctx->afu)) &&
-	   ((dsisr & CXL_PSL9_DSISR_An_CO_MASK) &
-		(CXL_PSL9_DSISR_An_PF_SLR | CXL_PSL9_DSISR_An_PF_RGC |
-		 CXL_PSL9_DSISR_An_PF_RGP | CXL_PSL9_DSISR_An_PF_HRH |
-		 CXL_PSL9_DSISR_An_PF_STEG)))
+	if ((cxl_is_power8()) && (dsisr & CXL_PSL_DSISR_An_DM))
 		return true;
 
+	if (cxl_is_power9()) {
+		crs = (dsisr & CXL_PSL9_DSISR_An_CO_MASK);
+		if ((crs == CXL_PSL9_DSISR_An_PF_SLR) ||
+		    (crs == CXL_PSL9_DSISR_An_PF_RGC) ||
+		    (crs == CXL_PSL9_DSISR_An_PF_RGP) ||
+		    (crs == CXL_PSL9_DSISR_An_PF_HRH) ||
+		    (crs == CXL_PSL9_DSISR_An_PF_STEG) ||
+		    (crs == CXL_PSL9_DSISR_An_URTCH)) {
+			return true;
+		}
+	}
+
 	return false;
 }
 
diff --git a/drivers/misc/cxl/main.c b/drivers/misc/cxl/main.c
index 1703655072b1..c1ba0d42cbc8 100644
--- a/drivers/misc/cxl/main.c
+++ b/drivers/misc/cxl/main.c
@@ -329,8 +329,15 @@ static int __init init_cxl(void)
 
 	cxl_debugfs_init();
 
-	if ((rc = register_cxl_calls(&cxl_calls)))
-		goto err;
+	/*
+	 * we don't register the callback on P9. slb callack is only
+	 * used for the PSL8 MMU and CX4.
+	 */
+	if (cxl_is_power8()) {
+		rc = register_cxl_calls(&cxl_calls);
+		if (rc)
+			goto err;
+	}
 
 	if (cpu_has_feature(CPU_FTR_HVMODE)) {
 		cxl_ops = &cxl_native_ops;
@@ -347,7 +354,8 @@ static int __init init_cxl(void)
 
 	return 0;
 err1:
-	unregister_cxl_calls(&cxl_calls);
+	if (cxl_is_power8())
+		unregister_cxl_calls(&cxl_calls);
 err:
 	cxl_debugfs_exit();
 	cxl_file_exit();
@@ -366,7 +374,8 @@ static void exit_cxl(void)
 
 	cxl_debugfs_exit();
 	cxl_file_exit();
-	unregister_cxl_calls(&cxl_calls);
+	if (cxl_is_power8())
+		unregister_cxl_calls(&cxl_calls);
 	idr_destroy(&cxl_adapter_idr);
 }
 
diff --git a/drivers/misc/cxl/native.c b/drivers/misc/cxl/native.c
index 8d6ea9712dbd..2b2f8894149d 100644
--- a/drivers/misc/cxl/native.c
+++ b/drivers/misc/cxl/native.c
@@ -105,11 +105,16 @@ static int native_afu_reset(struct cxl_afu *afu)
 			   CXL_AFU_Cntl_An_RS_MASK | CXL_AFU_Cntl_An_ES_MASK,
 			   false);
 
-	/* Re-enable any masked interrupts */
-	serr = cxl_p1n_read(afu, CXL_PSL_SERR_An);
-	serr &= ~CXL_PSL_SERR_An_IRQ_MASKS;
-	cxl_p1n_write(afu, CXL_PSL_SERR_An, serr);
-
+	/*
+	 * Re-enable any masked interrupts when the AFU is not
+	 * activated to avoid side effects after attaching a process
+	 * in dedicated mode.
+	 */
+	if (afu->current_mode == 0) {
+		serr = cxl_p1n_read(afu, CXL_PSL_SERR_An);
+		serr &= ~CXL_PSL_SERR_An_IRQ_MASKS;
+		cxl_p1n_write(afu, CXL_PSL_SERR_An, serr);
+	}
 
 	return rc;
 }
@@ -139,9 +144,9 @@ int cxl_psl_purge(struct cxl_afu *afu)
 
 	pr_devel("PSL purge request\n");
 
-	if (cxl_is_psl8(afu))
+	if (cxl_is_power8())
 		trans_fault = CXL_PSL_DSISR_TRANS;
-	if (cxl_is_psl9(afu))
+	if (cxl_is_power9())
 		trans_fault = CXL_PSL9_DSISR_An_TF;
 
 	if (!cxl_ops->link_ok(afu->adapter, afu)) {
@@ -603,7 +608,7 @@ static u64 calculate_sr(struct cxl_context *ctx)
 		if (!test_tsk_thread_flag(current, TIF_32BIT))
 			sr |= CXL_PSL_SR_An_SF;
 	}
-	if (cxl_is_psl9(ctx->afu)) {
+	if (cxl_is_power9()) {
 		if (radix_enabled())
 			sr |= CXL_PSL_SR_An_XLAT_ror;
 		else
@@ -1117,10 +1122,10 @@ static irqreturn_t native_handle_psl_slice_error(struct cxl_context *ctx,
 
 static bool cxl_is_translation_fault(struct cxl_afu *afu, u64 dsisr)
 {
-	if ((cxl_is_psl8(afu)) && (dsisr & CXL_PSL_DSISR_TRANS))
+	if ((cxl_is_power8()) && (dsisr & CXL_PSL_DSISR_TRANS))
 		return true;
 
-	if ((cxl_is_psl9(afu)) && (dsisr & CXL_PSL9_DSISR_An_TF))
+	if ((cxl_is_power9()) && (dsisr & CXL_PSL9_DSISR_An_TF))
 		return true;
 
 	return false;
@@ -1194,10 +1199,10 @@ static void native_irq_wait(struct cxl_context *ctx)
 		if (ph != ctx->pe)
 			return;
 		dsisr = cxl_p2n_read(ctx->afu, CXL_PSL_DSISR_An);
-		if (cxl_is_psl8(ctx->afu) &&
+		if (cxl_is_power8() &&
 		   ((dsisr & CXL_PSL_DSISR_PENDING) == 0))
 			return;
-		if (cxl_is_psl9(ctx->afu) &&
+		if (cxl_is_power9() &&
 		   ((dsisr & CXL_PSL9_DSISR_PENDING) == 0))
 			return;
 		/*
diff --git a/drivers/misc/cxl/pci.c b/drivers/misc/cxl/pci.c
index 6dc1ee5b92c9..1eb9859809bf 100644
--- a/drivers/misc/cxl/pci.c
+++ b/drivers/misc/cxl/pci.c
@@ -436,7 +436,7 @@ static int init_implementation_adapter_regs_psl9(struct cxl *adapter, struct pci
 	/* nMMU_ID Defaults to: b’000001001’*/
 	xsl_dsnctl |= ((u64)0x09 << (63-28));
 
-	if (cxl_is_power9() && !cpu_has_feature(CPU_FTR_POWER9_DD1)) {
+	if (!(cxl_is_power9_dd1())) {
 		/*
 		 * Used to identify CAPI packets which should be sorted into
 		 * the Non-Blocking queues by the PHB. This field should match
@@ -491,7 +491,7 @@ static int init_implementation_adapter_regs_psl9(struct cxl *adapter, struct pci
 	cxl_p1_write(adapter, CXL_PSL9_APCDEDTYPE, 0x40000003FFFF0000ULL);
 
 	/* Disable vc dd1 fix */
-	if ((cxl_is_power9() && cpu_has_feature(CPU_FTR_POWER9_DD1)))
+	if (cxl_is_power9_dd1())
 		cxl_p1_write(adapter, CXL_PSL9_GP_CT, 0x0400000000000001ULL);
 
 	return 0;
@@ -1439,8 +1439,7 @@ int cxl_pci_reset(struct cxl *adapter)
 	 * The adapter is about to be reset, so ignore errors.
 	 * Not supported on P9 DD1
 	 */
-	if ((cxl_is_power8()) ||
-	    ((cxl_is_power9() && !cpu_has_feature(CPU_FTR_POWER9_DD1))))
+	if ((cxl_is_power8()) || (!(cxl_is_power9_dd1())))
 		cxl_data_cache_flush(adapter);
 
 	/* pcie_warm_reset requests a fundamental pci reset which includes a
@@ -1750,7 +1749,6 @@ static const struct cxl_service_layer_ops psl9_ops = {
 	.debugfs_add_adapter_regs = cxl_debugfs_add_adapter_regs_psl9,
 	.debugfs_add_afu_regs = cxl_debugfs_add_afu_regs_psl9,
 	.psl_irq_dump_registers = cxl_native_irq_dump_regs_psl9,
-	.err_irq_dump_registers = cxl_native_err_irq_dump_regs,
 	.debugfs_stop_trace = cxl_stop_trace_psl9,
 	.write_timebase_ctrl = write_timebase_ctrl_psl9,
 	.timebase_read = timebase_read_psl9,
@@ -1889,8 +1887,7 @@ static void cxl_pci_remove_adapter(struct cxl *adapter)
 	 * Flush adapter datacache as its about to be removed.
 	 * Not supported on P9 DD1.
 	 */
-	if ((cxl_is_power8()) ||
-	    ((cxl_is_power9() && !cpu_has_feature(CPU_FTR_POWER9_DD1))))
+	if ((cxl_is_power8()) || (!(cxl_is_power9_dd1())))
 		cxl_data_cache_flush(adapter);
 
 	cxl_deconfigure_adapter(adapter);
diff --git a/drivers/misc/mei/bus.c b/drivers/misc/mei/bus.c
index 07aad8576334..40c79089e548 100644
--- a/drivers/misc/mei/bus.c
+++ b/drivers/misc/mei/bus.c
@@ -1040,7 +1040,7 @@ static void mei_cl_bus_dev_init(struct mei_device *bus,
  *
  * @bus: mei device
  */
-void mei_cl_bus_rescan(struct mei_device *bus)
+static void mei_cl_bus_rescan(struct mei_device *bus)
 {
 	struct mei_cl_device *cldev, *n;
 	struct mei_me_client *me_cl;
diff --git a/drivers/misc/mei/hw.h b/drivers/misc/mei/hw.h
index e1e4d47d4d7d..5c8286b40b62 100644
--- a/drivers/misc/mei/hw.h
+++ b/drivers/misc/mei/hw.h
@@ -65,7 +65,7 @@
 #define HBM_MAJOR_VERSION_DOT              2
 
 /*
- * MEI version with notifcation support
+ * MEI version with notification support
  */
 #define HBM_MINOR_VERSION_EV               0
 #define HBM_MAJOR_VERSION_EV               2
diff --git a/drivers/misc/mei/init.c b/drivers/misc/mei/init.c
index c8ad9ee7cb80..d2f691424dd1 100644
--- a/drivers/misc/mei/init.c
+++ b/drivers/misc/mei/init.c
@@ -215,12 +215,6 @@ int mei_start(struct mei_device *dev)
 		}
 	} while (ret);
 
-	/* we cannot start the device w/o hbm start message completed */
-	if (dev->dev_state == MEI_DEV_DISABLED) {
-		dev_err(dev->dev, "reset failed");
-		goto err;
-	}
-
 	if (mei_hbm_start_wait(dev)) {
 		dev_err(dev->dev, "HBM haven't started");
 		goto err;
diff --git a/drivers/misc/mei/interrupt.c b/drivers/misc/mei/interrupt.c
index c14e35201721..b0b8f18a85e3 100644
--- a/drivers/misc/mei/interrupt.c
+++ b/drivers/misc/mei/interrupt.c
@@ -235,6 +235,17 @@ static inline bool hdr_is_fixed(struct mei_msg_hdr *mei_hdr)
 	return mei_hdr->host_addr == 0 && mei_hdr->me_addr != 0;
 }
 
+static inline int hdr_is_valid(u32 msg_hdr)
+{
+	struct mei_msg_hdr *mei_hdr;
+
+	mei_hdr = (struct mei_msg_hdr *)&msg_hdr;
+	if (!msg_hdr || mei_hdr->reserved)
+		return -EBADMSG;
+
+	return 0;
+}
+
 /**
  * mei_irq_read_handler - bottom half read routine after ISR to
  * handle the read processing.
@@ -256,17 +267,18 @@ int mei_irq_read_handler(struct mei_device *dev,
 		dev->rd_msg_hdr = mei_read_hdr(dev);
 		(*slots)--;
 		dev_dbg(dev->dev, "slots =%08x.\n", *slots);
-	}
-	mei_hdr = (struct mei_msg_hdr *) &dev->rd_msg_hdr;
-	dev_dbg(dev->dev, MEI_HDR_FMT, MEI_HDR_PRM(mei_hdr));
 
-	if (mei_hdr->reserved || !dev->rd_msg_hdr) {
-		dev_err(dev->dev, "corrupted message header 0x%08X\n",
+		ret = hdr_is_valid(dev->rd_msg_hdr);
+		if (ret) {
+			dev_err(dev->dev, "corrupted message header 0x%08X\n",
 				dev->rd_msg_hdr);
-		ret = -EBADMSG;
-		goto end;
+			goto end;
+		}
 	}
 
+	mei_hdr = (struct mei_msg_hdr *)&dev->rd_msg_hdr;
+	dev_dbg(dev->dev, MEI_HDR_FMT, MEI_HDR_PRM(mei_hdr));
+
 	if (mei_slots2data(*slots) < mei_hdr->length) {
 		dev_err(dev->dev, "less data available than length=%08x.\n",
 				*slots);
diff --git a/drivers/misc/mei/mei_dev.h b/drivers/misc/mei/mei_dev.h
index 63a67c99fc78..ebcd5132e447 100644
--- a/drivers/misc/mei/mei_dev.h
+++ b/drivers/misc/mei/mei_dev.h
@@ -306,7 +306,6 @@ struct mei_hw_ops {
 };
 
 /* MEI bus API*/
-void mei_cl_bus_rescan(struct mei_device *bus);
 void mei_cl_bus_rescan_work(struct work_struct *work);
 void mei_cl_bus_dev_fixup(struct mei_cl_device *dev);
 ssize_t __mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length,
diff --git a/drivers/misc/sram-exec.c b/drivers/misc/sram-exec.c
index 3d528a13b8fc..426ad912b441 100644
--- a/drivers/misc/sram-exec.c
+++ b/drivers/misc/sram-exec.c
@@ -19,6 +19,7 @@
 #include <linux/mm.h>
 #include <linux/sram.h>
 
+#include <asm/fncpy.h>
 #include <asm/set_memory.h>
 
 #include "sram.h"
@@ -58,20 +59,32 @@ int sram_add_protect_exec(struct sram_partition *part)
  * @src: Source address for the data to copy
  * @size: Size of copy to perform, which starting from dst, must reside in pool
  *
+ * Return: Address for copied data that can safely be called through function
+ *	   pointer, or NULL if problem.
+ *
  * This helper function allows sram driver to act as central control location
  * of 'protect-exec' pools which are normal sram pools but are always set
  * read-only and executable except when copying data to them, at which point
  * they are set to read-write non-executable, to make sure no memory is
  * writeable and executable at the same time. This region must be page-aligned
  * and is checked during probe, otherwise page attribute manipulation would
- * not be possible.
+ * not be possible. Care must be taken to only call the returned address as
+ * dst address is not guaranteed to be safely callable.
+ *
+ * NOTE: This function uses the fncpy macro to move code to the executable
+ * region. Some architectures have strict requirements for relocating
+ * executable code, so fncpy is a macro that must be defined by any arch
+ * making use of this functionality that guarantees a safe copy of exec
+ * data and returns a safe address that can be called as a C function
+ * pointer.
  */
-int sram_exec_copy(struct gen_pool *pool, void *dst, void *src,
-		   size_t size)
+void *sram_exec_copy(struct gen_pool *pool, void *dst, void *src,
+		     size_t size)
 {
 	struct sram_partition *part = NULL, *p;
 	unsigned long base;
 	int pages;
+	void *dst_cpy;
 
 	mutex_lock(&exec_pool_list_mutex);
 	list_for_each_entry(p, &exec_pool_list, list) {
@@ -81,10 +94,10 @@ int sram_exec_copy(struct gen_pool *pool, void *dst, void *src,
 	mutex_unlock(&exec_pool_list_mutex);
 
 	if (!part)
-		return -EINVAL;
+		return NULL;
 
 	if (!addr_in_gen_pool(pool, (unsigned long)dst, size))
-		return -EINVAL;
+		return NULL;
 
 	base = (unsigned long)part->base;
 	pages = PAGE_ALIGN(size) / PAGE_SIZE;
@@ -94,13 +107,13 @@ int sram_exec_copy(struct gen_pool *pool, void *dst, void *src,
 	set_memory_nx((unsigned long)base, pages);
 	set_memory_rw((unsigned long)base, pages);
 
-	memcpy(dst, src, size);
+	dst_cpy = fncpy(dst, src, size);
 
 	set_memory_ro((unsigned long)base, pages);
 	set_memory_x((unsigned long)base, pages);
 
 	mutex_unlock(&part->lock);
 
-	return 0;
+	return dst_cpy;
 }
 EXPORT_SYMBOL_GPL(sram_exec_copy);
diff --git a/drivers/misc/ti-st/st_core.c b/drivers/misc/ti-st/st_core.c
index 00051590e00f..eda8d407be28 100644
--- a/drivers/misc/ti-st/st_core.c
+++ b/drivers/misc/ti-st/st_core.c
@@ -262,7 +262,7 @@ void st_int_recv(void *disc_data,
 	while (count) {
 		if (st_gdata->rx_count) {
 			len = min_t(unsigned int, st_gdata->rx_count, count);
-			memcpy(skb_put(st_gdata->rx_skb, len), ptr, len);
+			skb_put_data(st_gdata->rx_skb, ptr, len);
 			st_gdata->rx_count -= len;
 			count -= len;
 			ptr += len;
diff --git a/drivers/misc/ti-st/st_kim.c b/drivers/misc/ti-st/st_kim.c
index bf0d7708beac..e74413f882ab 100644
--- a/drivers/misc/ti-st/st_kim.c
+++ b/drivers/misc/ti-st/st_kim.c
@@ -152,7 +152,7 @@ static void kim_int_recv(struct kim_data_s *kim_gdata,
 	while (count) {
 		if (kim_gdata->rx_count) {
 			len = min_t(unsigned int, kim_gdata->rx_count, count);
-			memcpy(skb_put(kim_gdata->rx_skb, len), ptr, len);
+			skb_put_data(kim_gdata->rx_skb, ptr, len);
 			kim_gdata->rx_count -= len;
 			count -= len;
 			ptr += len;
diff --git a/drivers/mmc/core/Kconfig b/drivers/mmc/core/Kconfig
index fc1ecdaaa9ca..42e89060cd41 100644
--- a/drivers/mmc/core/Kconfig
+++ b/drivers/mmc/core/Kconfig
@@ -61,24 +61,6 @@ config MMC_BLOCK_MINORS
 
 	  If unsure, say 8 here.
 
-config MMC_BLOCK_BOUNCE
-	bool "Use bounce buffer for simple hosts"
-	depends on MMC_BLOCK
-	default y
-	help
-	  SD/MMC is a high latency protocol where it is crucial to
-	  send large requests in order to get high performance. Many
-	  controllers, however, are restricted to continuous memory
-	  (i.e. they can't do scatter-gather), something the kernel
-	  rarely can provide.
-
-	  Say Y here to help these restricted hosts by bouncing
-	  requests back and forth from a large buffer. You will get
-	  a big performance gain at the cost of up to 64 KiB of
-	  physical memory.
-
-	  If unsure, say Y here.
-
 config SDIO_UART
 	tristate "SDIO UART/GPS class support"
 	depends on TTY
diff --git a/drivers/mmc/core/block.c b/drivers/mmc/core/block.c
index 8273b078686d..0cfac2d39107 100644
--- a/drivers/mmc/core/block.c
+++ b/drivers/mmc/core/block.c
@@ -127,14 +127,6 @@ MODULE_PARM_DESC(perdev_minors, "Minors numbers to allocate per device");
 
 static inline int mmc_blk_part_switch(struct mmc_card *card,
 				      struct mmc_blk_data *md);
-static int get_card_status(struct mmc_card *card, u32 *status, int retries);
-
-static void mmc_blk_requeue(struct request_queue *q, struct request *req)
-{
-	spin_lock_irq(q->queue_lock);
-	blk_requeue_request(q, req);
-	spin_unlock_irq(q->queue_lock);
-}
 
 static struct mmc_blk_data *mmc_blk_get(struct gendisk *disk)
 {
@@ -197,6 +189,8 @@ static ssize_t power_ro_lock_store(struct device *dev,
 	int ret;
 	struct mmc_blk_data *md, *part_md;
 	struct mmc_card *card;
+	struct mmc_queue *mq;
+	struct request *req;
 	unsigned long set;
 
 	if (kstrtoul(buf, 0, &set))
@@ -206,20 +200,14 @@ static ssize_t power_ro_lock_store(struct device *dev,
 		return count;
 
 	md = mmc_blk_get(dev_to_disk(dev));
+	mq = &md->queue;
 	card = md->queue.card;
 
-	mmc_get_card(card);
-
-	ret = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL, EXT_CSD_BOOT_WP,
-				card->ext_csd.boot_ro_lock |
-				EXT_CSD_BOOT_WP_B_PWR_WP_EN,
-				card->ext_csd.part_time);
-	if (ret)
-		pr_err("%s: Locking boot partition ro until next power on failed: %d\n", md->disk->disk_name, ret);
-	else
-		card->ext_csd.boot_ro_lock |= EXT_CSD_BOOT_WP_B_PWR_WP_EN;
-
-	mmc_put_card(card);
+	/* Dispatch locking to the block layer */
+	req = blk_get_request(mq->queue, REQ_OP_DRV_OUT, __GFP_RECLAIM);
+	req_to_mmc_queue_req(req)->drv_op = MMC_DRV_OP_BOOT_WP;
+	blk_execute_rq(mq->queue, NULL, req, 0);
+	ret = req_to_mmc_queue_req(req)->drv_op_result;
 
 	if (!ret) {
 		pr_info("%s: Locking boot partition ro until next power on\n",
@@ -392,7 +380,7 @@ static int ioctl_rpmb_card_status_poll(struct mmc_card *card, u32 *status,
 		return -EINVAL;
 
 	do {
-		err = get_card_status(card, status, 5);
+		err = __mmc_send_status(card, status, 5);
 		if (err)
 			break;
 
@@ -450,7 +438,7 @@ static int __mmc_blk_ioctl_cmd(struct mmc_card *card, struct mmc_blk_data *md,
 	struct mmc_request mrq = {};
 	struct scatterlist sg;
 	int err;
-	int is_rpmb = false;
+	bool is_rpmb = false;
 	u32 status = 0;
 
 	if (!card || !md || !idata)
@@ -570,9 +558,12 @@ static int mmc_blk_ioctl_cmd(struct block_device *bdev,
 			     struct mmc_ioc_cmd __user *ic_ptr)
 {
 	struct mmc_blk_ioc_data *idata;
+	struct mmc_blk_ioc_data *idatas[1];
 	struct mmc_blk_data *md;
+	struct mmc_queue *mq;
 	struct mmc_card *card;
 	int err = 0, ioc_err = 0;
+	struct request *req;
 
 	/*
 	 * The caller must have CAP_SYS_RAWIO, and must be calling this on the
@@ -598,17 +589,21 @@ static int mmc_blk_ioctl_cmd(struct block_device *bdev,
 		goto cmd_done;
 	}
 
-	mmc_get_card(card);
-
-	ioc_err = __mmc_blk_ioctl_cmd(card, md, idata);
-
-	/* Always switch back to main area after RPMB access */
-	if (md->area_type & MMC_BLK_DATA_AREA_RPMB)
-		mmc_blk_part_switch(card, dev_get_drvdata(&card->dev));
-
-	mmc_put_card(card);
-
+	/*
+	 * Dispatch the ioctl() into the block request queue.
+	 */
+	mq = &md->queue;
+	req = blk_get_request(mq->queue,
+		idata->ic.write_flag ? REQ_OP_DRV_OUT : REQ_OP_DRV_IN,
+		__GFP_RECLAIM);
+	idatas[0] = idata;
+	req_to_mmc_queue_req(req)->drv_op = MMC_DRV_OP_IOCTL;
+	req_to_mmc_queue_req(req)->idata = idatas;
+	req_to_mmc_queue_req(req)->ioc_count = 1;
+	blk_execute_rq(mq->queue, NULL, req, 0);
+	ioc_err = req_to_mmc_queue_req(req)->drv_op_result;
 	err = mmc_blk_ioctl_copy_to_user(ic_ptr, idata);
+	blk_put_request(req);
 
 cmd_done:
 	mmc_blk_put(md);
@@ -625,8 +620,10 @@ static int mmc_blk_ioctl_multi_cmd(struct block_device *bdev,
 	struct mmc_ioc_cmd __user *cmds = user->cmds;
 	struct mmc_card *card;
 	struct mmc_blk_data *md;
+	struct mmc_queue *mq;
 	int i, err = 0, ioc_err = 0;
 	__u64 num_of_cmds;
+	struct request *req;
 
 	/*
 	 * The caller must have CAP_SYS_RAWIO, and must be calling this on the
@@ -668,21 +665,26 @@ static int mmc_blk_ioctl_multi_cmd(struct block_device *bdev,
 		goto cmd_done;
 	}
 
-	mmc_get_card(card);
-
-	for (i = 0; i < num_of_cmds && !ioc_err; i++)
-		ioc_err = __mmc_blk_ioctl_cmd(card, md, idata[i]);
-
-	/* Always switch back to main area after RPMB access */
-	if (md->area_type & MMC_BLK_DATA_AREA_RPMB)
-		mmc_blk_part_switch(card, dev_get_drvdata(&card->dev));
 
-	mmc_put_card(card);
+	/*
+	 * Dispatch the ioctl()s into the block request queue.
+	 */
+	mq = &md->queue;
+	req = blk_get_request(mq->queue,
+		idata[0]->ic.write_flag ? REQ_OP_DRV_OUT : REQ_OP_DRV_IN,
+		__GFP_RECLAIM);
+	req_to_mmc_queue_req(req)->drv_op = MMC_DRV_OP_IOCTL;
+	req_to_mmc_queue_req(req)->idata = idata;
+	req_to_mmc_queue_req(req)->ioc_count = num_of_cmds;
+	blk_execute_rq(mq->queue, NULL, req, 0);
+	ioc_err = req_to_mmc_queue_req(req)->drv_op_result;
 
 	/* copy to user if data and response */
 	for (i = 0; i < num_of_cmds && !err; i++)
 		err = mmc_blk_ioctl_copy_to_user(&cmds[i], idata[i]);
 
+	blk_put_request(req);
+
 cmd_done:
 	mmc_blk_put(md);
 cmd_err:
@@ -852,21 +854,6 @@ static int mmc_sd_num_wr_blocks(struct mmc_card *card, u32 *written_blocks)
 	return 0;
 }
 
-static int get_card_status(struct mmc_card *card, u32 *status, int retries)
-{
-	struct mmc_command cmd = {};
-	int err;
-
-	cmd.opcode = MMC_SEND_STATUS;
-	if (!mmc_host_is_spi(card->host))
-		cmd.arg = card->rca << 16;
-	cmd.flags = MMC_RSP_SPI_R2 | MMC_RSP_R1 | MMC_CMD_AC;
-	err = mmc_wait_for_cmd(card->host, &cmd, retries);
-	if (err == 0)
-		*status = cmd.resp[0];
-	return err;
-}
-
 static int card_busy_detect(struct mmc_card *card, unsigned int timeout_ms,
 		bool hw_busy_detect, struct request *req, bool *gen_err)
 {
@@ -875,7 +862,7 @@ static int card_busy_detect(struct mmc_card *card, unsigned int timeout_ms,
 	u32 status;
 
 	do {
-		err = get_card_status(card, &status, 5);
+		err = __mmc_send_status(card, &status, 5);
 		if (err) {
 			pr_err("%s: error %d requesting status\n",
 			       req->rq_disk->disk_name, err);
@@ -1043,7 +1030,7 @@ static int mmc_blk_cmd_recovery(struct mmc_card *card, struct request *req,
 	 * we can't be sure the returned status is for the r/w command.
 	 */
 	for (retry = 2; retry >= 0; retry--) {
-		err = get_card_status(card, &status, 0);
+		err = __mmc_send_status(card, &status, 0);
 		if (!err)
 			break;
 
@@ -1178,15 +1165,64 @@ int mmc_access_rpmb(struct mmc_queue *mq)
 	return false;
 }
 
+/*
+ * The non-block commands come back from the block layer after it queued it and
+ * processed it with all other requests and then they get issued in this
+ * function.
+ */
+static void mmc_blk_issue_drv_op(struct mmc_queue *mq, struct request *req)
+{
+	struct mmc_queue_req *mq_rq;
+	struct mmc_card *card = mq->card;
+	struct mmc_blk_data *md = mq->blkdata;
+	int ret;
+	int i;
+
+	mq_rq = req_to_mmc_queue_req(req);
+
+	switch (mq_rq->drv_op) {
+	case MMC_DRV_OP_IOCTL:
+		for (i = 0; i < mq_rq->ioc_count; i++) {
+			ret = __mmc_blk_ioctl_cmd(card, md, mq_rq->idata[i]);
+			if (ret)
+				break;
+		}
+		/* Always switch back to main area after RPMB access */
+		if (md->area_type & MMC_BLK_DATA_AREA_RPMB)
+			mmc_blk_part_switch(card, dev_get_drvdata(&card->dev));
+		break;
+	case MMC_DRV_OP_BOOT_WP:
+		ret = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL, EXT_CSD_BOOT_WP,
+				 card->ext_csd.boot_ro_lock |
+				 EXT_CSD_BOOT_WP_B_PWR_WP_EN,
+				 card->ext_csd.part_time);
+		if (ret)
+			pr_err("%s: Locking boot partition ro until next power on failed: %d\n",
+			       md->disk->disk_name, ret);
+		else
+			card->ext_csd.boot_ro_lock |=
+				EXT_CSD_BOOT_WP_B_PWR_WP_EN;
+		break;
+	default:
+		pr_err("%s: unknown driver specific operation\n",
+		       md->disk->disk_name);
+		ret = -EINVAL;
+		break;
+	}
+	mq_rq->drv_op_result = ret;
+	blk_end_request_all(req, ret);
+}
+
 static void mmc_blk_issue_discard_rq(struct mmc_queue *mq, struct request *req)
 {
 	struct mmc_blk_data *md = mq->blkdata;
 	struct mmc_card *card = md->queue.card;
 	unsigned int from, nr, arg;
 	int err = 0, type = MMC_BLK_DISCARD;
+	blk_status_t status = BLK_STS_OK;
 
 	if (!mmc_can_erase(card)) {
-		err = -EOPNOTSUPP;
+		status = BLK_STS_NOTSUPP;
 		goto fail;
 	}
 
@@ -1212,10 +1248,12 @@ static void mmc_blk_issue_discard_rq(struct mmc_queue *mq, struct request *req)
 		if (!err)
 			err = mmc_erase(card, from, nr, arg);
 	} while (err == -EIO && !mmc_blk_reset(md, card->host, type));
-	if (!err)
+	if (err)
+		status = BLK_STS_IOERR;
+	else
 		mmc_blk_reset_success(md, type);
 fail:
-	blk_end_request(req, err, blk_rq_bytes(req));
+	blk_end_request(req, status, blk_rq_bytes(req));
 }
 
 static void mmc_blk_issue_secdiscard_rq(struct mmc_queue *mq,
@@ -1225,9 +1263,10 @@ static void mmc_blk_issue_secdiscard_rq(struct mmc_queue *mq,
 	struct mmc_card *card = md->queue.card;
 	unsigned int from, nr, arg;
 	int err = 0, type = MMC_BLK_SECDISCARD;
+	blk_status_t status = BLK_STS_OK;
 
 	if (!(mmc_can_secure_erase_trim(card))) {
-		err = -EOPNOTSUPP;
+		status = BLK_STS_NOTSUPP;
 		goto out;
 	}
 
@@ -1254,8 +1293,10 @@ retry:
 	err = mmc_erase(card, from, nr, arg);
 	if (err == -EIO)
 		goto out_retry;
-	if (err)
+	if (err) {
+		status = BLK_STS_IOERR;
 		goto out;
+	}
 
 	if (arg == MMC_SECURE_TRIM1_ARG) {
 		if (card->quirks & MMC_QUIRK_INAND_CMD38) {
@@ -1270,8 +1311,10 @@ retry:
 		err = mmc_erase(card, from, nr, MMC_SECURE_TRIM2_ARG);
 		if (err == -EIO)
 			goto out_retry;
-		if (err)
+		if (err) {
+			status = BLK_STS_IOERR;
 			goto out;
+		}
 	}
 
 out_retry:
@@ -1280,7 +1323,7 @@ out_retry:
 	if (!err)
 		mmc_blk_reset_success(md, type);
 out:
-	blk_end_request(req, err, blk_rq_bytes(req));
+	blk_end_request(req, status, blk_rq_bytes(req));
 }
 
 static void mmc_blk_issue_flush(struct mmc_queue *mq, struct request *req)
@@ -1290,10 +1333,7 @@ static void mmc_blk_issue_flush(struct mmc_queue *mq, struct request *req)
 	int ret = 0;
 
 	ret = mmc_flush_cache(card);
-	if (ret)
-		ret = -EIO;
-
-	blk_end_request_all(req, ret);
+	blk_end_request_all(req, ret ? BLK_STS_IOERR : BLK_STS_OK);
 }
 
 /*
@@ -1324,16 +1364,25 @@ static inline void mmc_apply_rel_rw(struct mmc_blk_request *brq,
 	 R1_ADDRESS_ERROR |	/* Misaligned address */		\
 	 R1_BLOCK_LEN_ERROR |	/* Transferred block length incorrect */\
 	 R1_WP_VIOLATION |	/* Tried to write to protected block */	\
+	 R1_CARD_ECC_FAILED |	/* Card ECC failed */			\
 	 R1_CC_ERROR |		/* Card controller error */		\
 	 R1_ERROR)		/* General/unknown error */
 
+static bool mmc_blk_has_cmd_err(struct mmc_command *cmd)
+{
+	if (!cmd->error && cmd->resp[0] & CMD_ERRORS)
+		cmd->error = -EIO;
+
+	return cmd->error;
+}
+
 static enum mmc_blk_status mmc_blk_err_check(struct mmc_card *card,
 					     struct mmc_async_req *areq)
 {
 	struct mmc_queue_req *mq_mrq = container_of(areq, struct mmc_queue_req,
 						    areq);
 	struct mmc_blk_request *brq = &mq_mrq->brq;
-	struct request *req = mq_mrq->req;
+	struct request *req = mmc_queue_req_to_req(mq_mrq);
 	int need_retune = card->host->need_retune;
 	bool ecc_err = false;
 	bool gen_err = false;
@@ -1348,7 +1397,7 @@ static enum mmc_blk_status mmc_blk_err_check(struct mmc_card *card,
 	 * stop.error indicates a problem with the stop command.  Data
 	 * may have been transferred, or may still be transferring.
 	 */
-	if (brq->sbc.error || brq->cmd.error || brq->stop.error ||
+	if (brq->sbc.error || brq->cmd.error || mmc_blk_has_cmd_err(&brq->stop) ||
 	    brq->data.error) {
 		switch (mmc_blk_cmd_recovery(card, req, brq, &ecc_err, &gen_err)) {
 		case ERR_RETRY:
@@ -1402,7 +1451,8 @@ static enum mmc_blk_status mmc_blk_err_check(struct mmc_card *card,
 		return MMC_BLK_RETRY;
 	}
 
-	if (brq->data.error) {
+	/* Some errors (ECC) are flagged on the next commmand, so check stop, too */
+	if (brq->data.error || brq->stop.error) {
 		if (need_retune && !brq->retune_retry_done) {
 			pr_debug("%s: retrying because a re-tune was needed\n",
 				 req->rq_disk->disk_name);
@@ -1410,7 +1460,7 @@ static enum mmc_blk_status mmc_blk_err_check(struct mmc_card *card,
 			return MMC_BLK_RETRY;
 		}
 		pr_err("%s: error %d transferring data, sector %u, nr %u, cmd response %#x, card status %#x\n",
-		       req->rq_disk->disk_name, brq->data.error,
+		       req->rq_disk->disk_name, brq->data.error ?: brq->stop.error,
 		       (unsigned)blk_rq_pos(req),
 		       (unsigned)blk_rq_sectors(req),
 		       brq->cmd.resp[0], brq->stop.resp[0]);
@@ -1440,7 +1490,7 @@ static void mmc_blk_data_prep(struct mmc_queue *mq, struct mmc_queue_req *mqrq,
 	struct mmc_blk_data *md = mq->blkdata;
 	struct mmc_card *card = md->queue.card;
 	struct mmc_blk_request *brq = &mqrq->brq;
-	struct request *req = mqrq->req;
+	struct request *req = mmc_queue_req_to_req(mqrq);
 
 	/*
 	 * Reliable writes are used to implement Forced Unit Access and
@@ -1545,7 +1595,7 @@ static void mmc_blk_rw_rq_prep(struct mmc_queue_req *mqrq,
 {
 	u32 readcmd, writecmd;
 	struct mmc_blk_request *brq = &mqrq->brq;
-	struct request *req = mqrq->req;
+	struct request *req = mmc_queue_req_to_req(mqrq);
 	struct mmc_blk_data *md = mq->blkdata;
 	bool do_rel_wr, do_data_tag;
 
@@ -1641,8 +1691,8 @@ static void mmc_blk_rw_cmd_abort(struct mmc_queue *mq, struct mmc_card *card,
 {
 	if (mmc_card_removed(card))
 		req->rq_flags |= RQF_QUIET;
-	while (blk_end_request(req, -EIO, blk_rq_cur_bytes(req)));
-	mmc_queue_req_free(mq, mqrq);
+	while (blk_end_request(req, BLK_STS_IOERR, blk_rq_cur_bytes(req)));
+	mq->qcnt--;
 }
 
 /**
@@ -1661,8 +1711,8 @@ static void mmc_blk_rw_try_restart(struct mmc_queue *mq, struct request *req,
 	 */
 	if (mmc_card_removed(mq->card)) {
 		req->rq_flags |= RQF_QUIET;
-		blk_end_request_all(req, -EIO);
-		mmc_queue_req_free(mq, mqrq);
+		blk_end_request_all(req, BLK_STS_IOERR);
+		mq->qcnt--; /* FIXME: just set to 0? */
 		return;
 	}
 	/* Else proceed and try to restart the current async request */
@@ -1685,12 +1735,8 @@ static void mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *new_req)
 	bool req_pending = true;
 
 	if (new_req) {
-		mqrq_cur = mmc_queue_req_find(mq, new_req);
-		if (!mqrq_cur) {
-			WARN_ON(1);
-			mmc_blk_requeue(mq->queue, new_req);
-			new_req = NULL;
-		}
+		mqrq_cur = req_to_mmc_queue_req(new_req);
+		mq->qcnt++;
 	}
 
 	if (!mq->qcnt)
@@ -1731,7 +1777,7 @@ static void mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *new_req)
 		 */
 		mq_rq =	container_of(old_areq, struct mmc_queue_req, areq);
 		brq = &mq_rq->brq;
-		old_req = mq_rq->req;
+		old_req = mmc_queue_req_to_req(mq_rq);
 		type = rq_data_dir(old_req) == READ ? MMC_BLK_READ : MMC_BLK_WRITE;
 		mmc_queue_bounce_post(mq_rq);
 
@@ -1743,7 +1789,7 @@ static void mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *new_req)
 			 */
 			mmc_blk_reset_success(md, type);
 
-			req_pending = blk_end_request(old_req, 0,
+			req_pending = blk_end_request(old_req, BLK_STS_OK,
 						      brq->data.bytes_xfered);
 			/*
 			 * If the blk_end_request function returns non-zero even
@@ -1764,12 +1810,12 @@ static void mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *new_req)
 				if (req_pending)
 					mmc_blk_rw_cmd_abort(mq, card, old_req, mq_rq);
 				else
-					mmc_queue_req_free(mq, mq_rq);
+					mq->qcnt--;
 				mmc_blk_rw_try_restart(mq, new_req, mqrq_cur);
 				return;
 			}
 			if (!req_pending) {
-				mmc_queue_req_free(mq, mq_rq);
+				mq->qcnt--;
 				mmc_blk_rw_try_restart(mq, new_req, mqrq_cur);
 				return;
 			}
@@ -1811,10 +1857,10 @@ static void mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *new_req)
 			 * time, so we only reach here after trying to
 			 * read a single sector.
 			 */
-			req_pending = blk_end_request(old_req, -EIO,
+			req_pending = blk_end_request(old_req, BLK_STS_IOERR,
 						      brq->data.blksz);
 			if (!req_pending) {
-				mmc_queue_req_free(mq, mq_rq);
+				mq->qcnt--;
 				mmc_blk_rw_try_restart(mq, new_req, mqrq_cur);
 				return;
 			}
@@ -1844,7 +1890,7 @@ static void mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *new_req)
 		}
 	} while (req_pending);
 
-	mmc_queue_req_free(mq, mq_rq);
+	mq->qcnt--;
 }
 
 void mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
@@ -1860,28 +1906,59 @@ void mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
 	ret = mmc_blk_part_switch(card, md);
 	if (ret) {
 		if (req) {
-			blk_end_request_all(req, -EIO);
+			blk_end_request_all(req, BLK_STS_IOERR);
 		}
 		goto out;
 	}
 
-	if (req && req_op(req) == REQ_OP_DISCARD) {
-		/* complete ongoing async transfer before issuing discard */
-		if (mq->qcnt)
-			mmc_blk_issue_rw_rq(mq, NULL);
-		mmc_blk_issue_discard_rq(mq, req);
-	} else if (req && req_op(req) == REQ_OP_SECURE_ERASE) {
-		/* complete ongoing async transfer before issuing secure erase*/
-		if (mq->qcnt)
-			mmc_blk_issue_rw_rq(mq, NULL);
-		mmc_blk_issue_secdiscard_rq(mq, req);
-	} else if (req && req_op(req) == REQ_OP_FLUSH) {
-		/* complete ongoing async transfer before issuing flush */
-		if (mq->qcnt)
-			mmc_blk_issue_rw_rq(mq, NULL);
-		mmc_blk_issue_flush(mq, req);
+	if (req) {
+		switch (req_op(req)) {
+		case REQ_OP_DRV_IN:
+		case REQ_OP_DRV_OUT:
+			/*
+			 * Complete ongoing async transfer before issuing
+			 * ioctl()s
+			 */
+			if (mq->qcnt)
+				mmc_blk_issue_rw_rq(mq, NULL);
+			mmc_blk_issue_drv_op(mq, req);
+			break;
+		case REQ_OP_DISCARD:
+			/*
+			 * Complete ongoing async transfer before issuing
+			 * discard.
+			 */
+			if (mq->qcnt)
+				mmc_blk_issue_rw_rq(mq, NULL);
+			mmc_blk_issue_discard_rq(mq, req);
+			break;
+		case REQ_OP_SECURE_ERASE:
+			/*
+			 * Complete ongoing async transfer before issuing
+			 * secure erase.
+			 */
+			if (mq->qcnt)
+				mmc_blk_issue_rw_rq(mq, NULL);
+			mmc_blk_issue_secdiscard_rq(mq, req);
+			break;
+		case REQ_OP_FLUSH:
+			/*
+			 * Complete ongoing async transfer before issuing
+			 * flush.
+			 */
+			if (mq->qcnt)
+				mmc_blk_issue_rw_rq(mq, NULL);
+			mmc_blk_issue_flush(mq, req);
+			break;
+		default:
+			/* Normal request, just issue it */
+			mmc_blk_issue_rw_rq(mq, req);
+			card->host->context_info.is_waiting_last_req = false;
+			break;
+		}
 	} else {
-		mmc_blk_issue_rw_rq(mq, req);
+		/* No request, flushing the pipeline with NULL */
+		mmc_blk_issue_rw_rq(mq, NULL);
 		card->host->context_info.is_waiting_last_req = false;
 	}
 
@@ -2166,7 +2243,6 @@ static int mmc_blk_probe(struct mmc_card *card)
 {
 	struct mmc_blk_data *md, *part_md;
 	char cap_str[10];
-	int ret;
 
 	/*
 	 * Check that the card supports the command class(es) we need.
@@ -2176,15 +2252,9 @@ static int mmc_blk_probe(struct mmc_card *card)
 
 	mmc_fixup_device(card, mmc_blk_fixups);
 
-	ret = mmc_queue_alloc_shared_queue(card);
-	if (ret)
-		return ret;
-
 	md = mmc_blk_alloc(card);
-	if (IS_ERR(md)) {
-		mmc_queue_free_shared_queue(card);
+	if (IS_ERR(md))
 		return PTR_ERR(md);
-	}
 
 	string_get_size((u64)get_capacity(md->disk), 512, STRING_UNITS_2,
 			cap_str, sizeof(cap_str));
@@ -2222,7 +2292,6 @@ static int mmc_blk_probe(struct mmc_card *card)
  out:
 	mmc_blk_remove_parts(card, md);
 	mmc_blk_remove_req(md);
-	mmc_queue_free_shared_queue(card);
 	return 0;
 }
 
@@ -2240,7 +2309,6 @@ static void mmc_blk_remove(struct mmc_card *card)
 	pm_runtime_put_noidle(&card->dev);
 	mmc_blk_remove_req(md);
 	dev_set_drvdata(&card->dev, NULL);
-	mmc_queue_free_shared_queue(card);
 }
 
 static int _mmc_blk_suspend(struct mmc_card *card)
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 82c45ddfa202..26431267a3e2 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -53,12 +53,6 @@
 /* If the device is not responding */
 #define MMC_CORE_TIMEOUT_MS	(10 * 60 * 1000) /* 10 minute timeout */
 
-/*
- * Background operations can take a long time, depending on the housekeeping
- * operations the card has to perform.
- */
-#define MMC_BKOPS_MAX_TIMEOUT	(4 * 60 * 1000) /* max time to wait in ms */
-
 /* The max erase timeout, used when host->max_busy_timeout isn't specified */
 #define MMC_ERASE_TIMEOUT_MS	(60 * 1000) /* 60 s */
 
@@ -362,74 +356,6 @@ static int mmc_start_request(struct mmc_host *host, struct mmc_request *mrq)
 	return 0;
 }
 
-/**
- *	mmc_start_bkops - start BKOPS for supported cards
- *	@card: MMC card to start BKOPS
- *	@form_exception: A flag to indicate if this function was
- *			 called due to an exception raised by the card
- *
- *	Start background operations whenever requested.
- *	When the urgent BKOPS bit is set in a R1 command response
- *	then background operations should be started immediately.
-*/
-void mmc_start_bkops(struct mmc_card *card, bool from_exception)
-{
-	int err;
-	int timeout;
-	bool use_busy_signal;
-
-	if (!card->ext_csd.man_bkops_en || mmc_card_doing_bkops(card))
-		return;
-
-	err = mmc_read_bkops_status(card);
-	if (err) {
-		pr_err("%s: Failed to read bkops status: %d\n",
-		       mmc_hostname(card->host), err);
-		return;
-	}
-
-	if (!card->ext_csd.raw_bkops_status)
-		return;
-
-	if (card->ext_csd.raw_bkops_status < EXT_CSD_BKOPS_LEVEL_2 &&
-	    from_exception)
-		return;
-
-	mmc_claim_host(card->host);
-	if (card->ext_csd.raw_bkops_status >= EXT_CSD_BKOPS_LEVEL_2) {
-		timeout = MMC_BKOPS_MAX_TIMEOUT;
-		use_busy_signal = true;
-	} else {
-		timeout = 0;
-		use_busy_signal = false;
-	}
-
-	mmc_retune_hold(card->host);
-
-	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-			EXT_CSD_BKOPS_START, 1, timeout, 0,
-			use_busy_signal, true, false);
-	if (err) {
-		pr_warn("%s: Error %d starting bkops\n",
-			mmc_hostname(card->host), err);
-		mmc_retune_release(card->host);
-		goto out;
-	}
-
-	/*
-	 * For urgent bkops status (LEVEL_2 and more)
-	 * bkops executed synchronously, otherwise
-	 * the operation is in progress
-	 */
-	if (!use_busy_signal)
-		mmc_card_set_doing_bkops(card);
-	else
-		mmc_retune_release(card->host);
-out:
-	mmc_release_host(card->host);
-}
-EXPORT_SYMBOL(mmc_start_bkops);
-
 /*
  * mmc_wait_data_done() - done callback for data request
  * @mrq: done data request
@@ -749,71 +675,6 @@ void mmc_wait_for_req(struct mmc_host *host, struct mmc_request *mrq)
 EXPORT_SYMBOL(mmc_wait_for_req);
 
 /**
- *	mmc_interrupt_hpi - Issue for High priority Interrupt
- *	@card: the MMC card associated with the HPI transfer
- *
- *	Issued High Priority Interrupt, and check for card status
- *	until out-of prg-state.
- */
-int mmc_interrupt_hpi(struct mmc_card *card)
-{
-	int err;
-	u32 status;
-	unsigned long prg_wait;
-
-	if (!card->ext_csd.hpi_en) {
-		pr_info("%s: HPI enable bit unset\n", mmc_hostname(card->host));
-		return 1;
-	}
-
-	mmc_claim_host(card->host);
-	err = mmc_send_status(card, &status);
-	if (err) {
-		pr_err("%s: Get card status fail\n", mmc_hostname(card->host));
-		goto out;
-	}
-
-	switch (R1_CURRENT_STATE(status)) {
-	case R1_STATE_IDLE:
-	case R1_STATE_READY:
-	case R1_STATE_STBY:
-	case R1_STATE_TRAN:
-		/*
-		 * In idle and transfer 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_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);
-	return err;
-}
-EXPORT_SYMBOL(mmc_interrupt_hpi);
-
-/**
  *	mmc_wait_for_cmd - start a command and wait for completion
  *	@host: MMC host to start command
  *	@cmd: MMC command to start
@@ -843,53 +704,6 @@ int mmc_wait_for_cmd(struct mmc_host *host, struct mmc_command *cmd, int retries
 EXPORT_SYMBOL(mmc_wait_for_cmd);
 
 /**
- *	mmc_stop_bkops - stop ongoing BKOPS
- *	@card: MMC card to check BKOPS
- *
- *	Send HPI command to stop ongoing background operations to
- *	allow rapid servicing of foreground operations, e.g. read/
- *	writes. Wait until the card comes out of the programming state
- *	to avoid errors in servicing read/write requests.
- */
-int mmc_stop_bkops(struct mmc_card *card)
-{
-	int err = 0;
-
-	err = mmc_interrupt_hpi(card);
-
-	/*
-	 * If err is EINVAL, we can't issue an HPI.
-	 * It should complete the BKOPS.
-	 */
-	if (!err || (err == -EINVAL)) {
-		mmc_card_clr_doing_bkops(card);
-		mmc_retune_release(card->host);
-		err = 0;
-	}
-
-	return err;
-}
-EXPORT_SYMBOL(mmc_stop_bkops);
-
-int mmc_read_bkops_status(struct mmc_card *card)
-{
-	int err;
-	u8 *ext_csd;
-
-	mmc_claim_host(card->host);
-	err = mmc_get_ext_csd(card, &ext_csd);
-	mmc_release_host(card->host);
-	if (err)
-		return err;
-
-	card->ext_csd.raw_bkops_status = ext_csd[EXT_CSD_BKOPS_STATUS];
-	card->ext_csd.raw_exception_status = ext_csd[EXT_CSD_EXP_EVENTS_STATUS];
-	kfree(ext_csd);
-	return 0;
-}
-EXPORT_SYMBOL(mmc_read_bkops_status);
-
-/**
  *	mmc_set_data_timeout - set the timeout for a data command
  *	@data: data phase for command
  *	@card: the MMC card associated with the data transfer
@@ -2597,6 +2411,8 @@ EXPORT_SYMBOL(mmc_set_blockcount);
 
 static void mmc_hw_reset_for_init(struct mmc_host *host)
 {
+	mmc_pwrseq_reset(host);
+
 	if (!(host->caps & MMC_CAP_HW_RESET) || !host->ops->hw_reset)
 		return;
 	host->ops->hw_reset(host);
@@ -2836,8 +2652,11 @@ void mmc_stop_host(struct mmc_host *host)
 	host->removed = 1;
 	spin_unlock_irqrestore(&host->lock, flags);
 #endif
-	if (host->slot.cd_irq >= 0)
+	if (host->slot.cd_irq >= 0) {
+		if (host->slot.cd_wake_enabled)
+			disable_irq_wake(host->slot.cd_irq);
 		disable_irq(host->slot.cd_irq);
+	}
 
 	host->rescan_disable = 1;
 	cancel_delayed_work_sync(&host->detect);
@@ -2913,27 +2732,6 @@ int mmc_power_restore_host(struct mmc_host *host)
 }
 EXPORT_SYMBOL(mmc_power_restore_host);
 
-/*
- * Flush the cache to the non-volatile storage.
- */
-int mmc_flush_cache(struct mmc_card *card)
-{
-	int err = 0;
-
-	if (mmc_card_mmc(card) &&
-			(card->ext_csd.cache_size > 0) &&
-			(card->ext_csd.cache_ctrl & 1)) {
-		err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-				EXT_CSD_FLUSH_CACHE, 1, 0);
-		if (err)
-			pr_err("%s: cache flush error %d\n",
-					mmc_hostname(card->host), err);
-	}
-
-	return err;
-}
-EXPORT_SYMBOL(mmc_flush_cache);
-
 #ifdef CONFIG_PM_SLEEP
 /* Do the card removal on suspend if card is assumed removeable
  * Do that in pm notifier while userspace isn't yet frozen, so we will be able
diff --git a/drivers/mmc/core/host.c b/drivers/mmc/core/host.c
index 3f8c85d5aa09..1503412f826c 100644
--- a/drivers/mmc/core/host.c
+++ b/drivers/mmc/core/host.c
@@ -30,6 +30,7 @@
 #include "host.h"
 #include "slot-gpio.h"
 #include "pwrseq.h"
+#include "sdio_ops.h"
 
 #define cls_dev_to_mmc_host(d)	container_of(d, struct mmc_host, class_dev)
 
@@ -176,19 +177,17 @@ static void mmc_retune_timer(unsigned long data)
  */
 int mmc_of_parse(struct mmc_host *host)
 {
-	struct device_node *np;
+	struct device *dev = host->parent;
 	u32 bus_width;
 	int ret;
 	bool cd_cap_invert, cd_gpio_invert = false;
 	bool ro_cap_invert, ro_gpio_invert = false;
 
-	if (!host->parent || !host->parent->of_node)
+	if (!dev || !dev_fwnode(dev))
 		return 0;
 
-	np = host->parent->of_node;
-
 	/* "bus-width" is translated to MMC_CAP_*_BIT_DATA flags */
-	if (of_property_read_u32(np, "bus-width", &bus_width) < 0) {
+	if (device_property_read_u32(dev, "bus-width", &bus_width) < 0) {
 		dev_dbg(host->parent,
 			"\"bus-width\" property is missing, assuming 1 bit.\n");
 		bus_width = 1;
@@ -210,7 +209,7 @@ int mmc_of_parse(struct mmc_host *host)
 	}
 
 	/* f_max is obtained from the optional "max-frequency" property */
-	of_property_read_u32(np, "max-frequency", &host->f_max);
+	device_property_read_u32(dev, "max-frequency", &host->f_max);
 
 	/*
 	 * Configure CD and WP pins. They are both by default active low to
@@ -225,12 +224,12 @@ int mmc_of_parse(struct mmc_host *host)
 	 */
 
 	/* Parse Card Detection */
-	if (of_property_read_bool(np, "non-removable")) {
+	if (device_property_read_bool(dev, "non-removable")) {
 		host->caps |= MMC_CAP_NONREMOVABLE;
 	} else {
-		cd_cap_invert = of_property_read_bool(np, "cd-inverted");
+		cd_cap_invert = device_property_read_bool(dev, "cd-inverted");
 
-		if (of_property_read_bool(np, "broken-cd"))
+		if (device_property_read_bool(dev, "broken-cd"))
 			host->caps |= MMC_CAP_NEEDS_POLL;
 
 		ret = mmc_gpiod_request_cd(host, "cd", 0, true,
@@ -256,7 +255,7 @@ int mmc_of_parse(struct mmc_host *host)
 	}
 
 	/* Parse Write Protection */
-	ro_cap_invert = of_property_read_bool(np, "wp-inverted");
+	ro_cap_invert = device_property_read_bool(dev, "wp-inverted");
 
 	ret = mmc_gpiod_request_ro(host, "wp", 0, false, 0, &ro_gpio_invert);
 	if (!ret)
@@ -264,64 +263,64 @@ int mmc_of_parse(struct mmc_host *host)
 	else if (ret != -ENOENT && ret != -ENOSYS)
 		return ret;
 
-	if (of_property_read_bool(np, "disable-wp"))
+	if (device_property_read_bool(dev, "disable-wp"))
 		host->caps2 |= MMC_CAP2_NO_WRITE_PROTECT;
 
 	/* See the comment on CD inversion above */
 	if (ro_cap_invert ^ ro_gpio_invert)
 		host->caps2 |= MMC_CAP2_RO_ACTIVE_HIGH;
 
-	if (of_property_read_bool(np, "cap-sd-highspeed"))
+	if (device_property_read_bool(dev, "cap-sd-highspeed"))
 		host->caps |= MMC_CAP_SD_HIGHSPEED;
-	if (of_property_read_bool(np, "cap-mmc-highspeed"))
+	if (device_property_read_bool(dev, "cap-mmc-highspeed"))
 		host->caps |= MMC_CAP_MMC_HIGHSPEED;
-	if (of_property_read_bool(np, "sd-uhs-sdr12"))
+	if (device_property_read_bool(dev, "sd-uhs-sdr12"))
 		host->caps |= MMC_CAP_UHS_SDR12;
-	if (of_property_read_bool(np, "sd-uhs-sdr25"))
+	if (device_property_read_bool(dev, "sd-uhs-sdr25"))
 		host->caps |= MMC_CAP_UHS_SDR25;
-	if (of_property_read_bool(np, "sd-uhs-sdr50"))
+	if (device_property_read_bool(dev, "sd-uhs-sdr50"))
 		host->caps |= MMC_CAP_UHS_SDR50;
-	if (of_property_read_bool(np, "sd-uhs-sdr104"))
+	if (device_property_read_bool(dev, "sd-uhs-sdr104"))
 		host->caps |= MMC_CAP_UHS_SDR104;
-	if (of_property_read_bool(np, "sd-uhs-ddr50"))
+	if (device_property_read_bool(dev, "sd-uhs-ddr50"))
 		host->caps |= MMC_CAP_UHS_DDR50;
-	if (of_property_read_bool(np, "cap-power-off-card"))
+	if (device_property_read_bool(dev, "cap-power-off-card"))
 		host->caps |= MMC_CAP_POWER_OFF_CARD;
-	if (of_property_read_bool(np, "cap-mmc-hw-reset"))
+	if (device_property_read_bool(dev, "cap-mmc-hw-reset"))
 		host->caps |= MMC_CAP_HW_RESET;
-	if (of_property_read_bool(np, "cap-sdio-irq"))
+	if (device_property_read_bool(dev, "cap-sdio-irq"))
 		host->caps |= MMC_CAP_SDIO_IRQ;
-	if (of_property_read_bool(np, "full-pwr-cycle"))
+	if (device_property_read_bool(dev, "full-pwr-cycle"))
 		host->caps2 |= MMC_CAP2_FULL_PWR_CYCLE;
-	if (of_property_read_bool(np, "keep-power-in-suspend"))
+	if (device_property_read_bool(dev, "keep-power-in-suspend"))
 		host->pm_caps |= MMC_PM_KEEP_POWER;
-	if (of_property_read_bool(np, "wakeup-source") ||
-	    of_property_read_bool(np, "enable-sdio-wakeup")) /* legacy */
+	if (device_property_read_bool(dev, "wakeup-source") ||
+	    device_property_read_bool(dev, "enable-sdio-wakeup")) /* legacy */
 		host->pm_caps |= MMC_PM_WAKE_SDIO_IRQ;
-	if (of_property_read_bool(np, "mmc-ddr-3_3v"))
+	if (device_property_read_bool(dev, "mmc-ddr-3_3v"))
 		host->caps |= MMC_CAP_3_3V_DDR;
-	if (of_property_read_bool(np, "mmc-ddr-1_8v"))
+	if (device_property_read_bool(dev, "mmc-ddr-1_8v"))
 		host->caps |= MMC_CAP_1_8V_DDR;
-	if (of_property_read_bool(np, "mmc-ddr-1_2v"))
+	if (device_property_read_bool(dev, "mmc-ddr-1_2v"))
 		host->caps |= MMC_CAP_1_2V_DDR;
-	if (of_property_read_bool(np, "mmc-hs200-1_8v"))
+	if (device_property_read_bool(dev, "mmc-hs200-1_8v"))
 		host->caps2 |= MMC_CAP2_HS200_1_8V_SDR;
-	if (of_property_read_bool(np, "mmc-hs200-1_2v"))
+	if (device_property_read_bool(dev, "mmc-hs200-1_2v"))
 		host->caps2 |= MMC_CAP2_HS200_1_2V_SDR;
-	if (of_property_read_bool(np, "mmc-hs400-1_8v"))
+	if (device_property_read_bool(dev, "mmc-hs400-1_8v"))
 		host->caps2 |= MMC_CAP2_HS400_1_8V | MMC_CAP2_HS200_1_8V_SDR;
-	if (of_property_read_bool(np, "mmc-hs400-1_2v"))
+	if (device_property_read_bool(dev, "mmc-hs400-1_2v"))
 		host->caps2 |= MMC_CAP2_HS400_1_2V | MMC_CAP2_HS200_1_2V_SDR;
-	if (of_property_read_bool(np, "mmc-hs400-enhanced-strobe"))
+	if (device_property_read_bool(dev, "mmc-hs400-enhanced-strobe"))
 		host->caps2 |= MMC_CAP2_HS400_ES;
-	if (of_property_read_bool(np, "no-sdio"))
+	if (device_property_read_bool(dev, "no-sdio"))
 		host->caps2 |= MMC_CAP2_NO_SDIO;
-	if (of_property_read_bool(np, "no-sd"))
+	if (device_property_read_bool(dev, "no-sd"))
 		host->caps2 |= MMC_CAP2_NO_SD;
-	if (of_property_read_bool(np, "no-mmc"))
+	if (device_property_read_bool(dev, "no-mmc"))
 		host->caps2 |= MMC_CAP2_NO_MMC;
 
-	host->dsr_req = !of_property_read_u32(np, "dsr", &host->dsr);
+	host->dsr_req = !device_property_read_u32(dev, "dsr", &host->dsr);
 	if (host->dsr_req && (host->dsr & ~0xffff)) {
 		dev_err(host->parent,
 			"device tree specified broken value for DSR: 0x%x, ignoring\n",
@@ -379,6 +378,7 @@ struct mmc_host *mmc_alloc_host(int extra, struct device *dev)
 	spin_lock_init(&host->lock);
 	init_waitqueue_head(&host->wq);
 	INIT_DELAYED_WORK(&host->detect, mmc_rescan);
+	INIT_DELAYED_WORK(&host->sdio_irq_work, sdio_irq_work);
 	setup_timer(&host->retune_timer, mmc_retune_timer, (unsigned long)host);
 
 	/*
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index 2c87dede5841..4ffea14b7eb6 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -27,6 +27,7 @@
 #include "mmc_ops.h"
 #include "quirks.h"
 #include "sd_ops.h"
+#include "pwrseq.h"
 
 #define DEFAULT_CMD6_TIMEOUT_MS	500
 
@@ -1555,10 +1556,7 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 	/*
 	 * Fetch CID from card.
 	 */
-	if (mmc_host_is_spi(host))
-		err = mmc_send_cid(host, cid);
-	else
-		err = mmc_all_send_cid(host, cid);
+	err = mmc_send_cid(host, cid);
 	if (err)
 		goto err;
 
@@ -1653,12 +1651,8 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 		mmc_set_erase_size(card);
 	}
 
-	/*
-	 * If enhanced_area_en is TRUE, host needs to enable ERASE_GRP_DEF
-	 * bit.  This bit will be lost every time after a reset or power off.
-	 */
-	if (card->ext_csd.partition_setting_completed ||
-	    (card->ext_csd.rev >= 3 && (host->caps2 & MMC_CAP2_HC_ERASE_SZ))) {
+	/* Enable ERASE_GRP_DEF. This bit is lost after a reset or power off. */
+	if (card->ext_csd.rev >= 3) {
 		err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
 				 EXT_CSD_ERASE_GROUP_DEF, 1,
 				 card->ext_csd.generic_cmd6_time);
@@ -2096,7 +2090,7 @@ static int mmc_runtime_resume(struct mmc_host *host)
 	return 0;
 }
 
-int mmc_can_reset(struct mmc_card *card)
+static int mmc_can_reset(struct mmc_card *card)
 {
 	u8 rst_n_function;
 
@@ -2105,7 +2099,6 @@ int mmc_can_reset(struct mmc_card *card)
 		return 0;
 	return 1;
 }
-EXPORT_SYMBOL(mmc_can_reset);
 
 static int mmc_reset(struct mmc_host *host)
 {
@@ -2127,6 +2120,7 @@ static int mmc_reset(struct mmc_host *host)
 	} else {
 		/* Do a brute force power cycle */
 		mmc_power_cycle(host, card->ocr);
+		mmc_pwrseq_reset(host);
 	}
 	return mmc_init_card(host, card->ocr, card);
 }
diff --git a/drivers/mmc/core/mmc_ops.c b/drivers/mmc/core/mmc_ops.c
index 78f75f00efc5..5f7c5920231a 100644
--- a/drivers/mmc/core/mmc_ops.c
+++ b/drivers/mmc/core/mmc_ops.c
@@ -19,6 +19,7 @@
 #include <linux/mmc/mmc.h>
 
 #include "core.h"
+#include "card.h"
 #include "host.h"
 #include "mmc_ops.h"
 
@@ -54,7 +55,7 @@ static const u8 tuning_blk_pattern_8bit[] = {
 	0xff, 0x77, 0x77, 0xff, 0x77, 0xbb, 0xdd, 0xee,
 };
 
-int mmc_send_status(struct mmc_card *card, u32 *status)
+int __mmc_send_status(struct mmc_card *card, u32 *status, unsigned int retries)
 {
 	int err;
 	struct mmc_command cmd = {};
@@ -64,7 +65,7 @@ int mmc_send_status(struct mmc_card *card, u32 *status)
 		cmd.arg = card->rca << 16;
 	cmd.flags = MMC_RSP_SPI_R2 | MMC_RSP_R1 | MMC_CMD_AC;
 
-	err = mmc_wait_for_cmd(card->host, &cmd, MMC_CMD_RETRIES);
+	err = mmc_wait_for_cmd(card->host, &cmd, retries);
 	if (err)
 		return err;
 
@@ -76,6 +77,12 @@ int mmc_send_status(struct mmc_card *card, u32 *status)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(__mmc_send_status);
+
+int mmc_send_status(struct mmc_card *card, u32 *status)
+{
+	return __mmc_send_status(card, status, MMC_CMD_RETRIES);
+}
 
 static int _mmc_select_card(struct mmc_host *host, struct mmc_card *card)
 {
@@ -200,24 +207,6 @@ int mmc_send_op_cond(struct mmc_host *host, u32 ocr, u32 *rocr)
 	return err;
 }
 
-int mmc_all_send_cid(struct mmc_host *host, u32 *cid)
-{
-	int err;
-	struct mmc_command cmd = {};
-
-	cmd.opcode = MMC_ALL_SEND_CID;
-	cmd.arg = 0;
-	cmd.flags = MMC_RSP_R2 | MMC_CMD_BCR;
-
-	err = mmc_wait_for_cmd(host, &cmd, MMC_CMD_RETRIES);
-	if (err)
-		return err;
-
-	memcpy(cid, cmd.resp, sizeof(u32) * 4);
-
-	return 0;
-}
-
 int mmc_set_relative_addr(struct mmc_card *card)
 {
 	struct mmc_command cmd = {};
@@ -302,15 +291,11 @@ mmc_send_cxd_data(struct mmc_card *card, struct mmc_host *host,
 	return 0;
 }
 
-int mmc_send_csd(struct mmc_card *card, u32 *csd)
+static int mmc_spi_send_csd(struct mmc_card *card, u32 *csd)
 {
 	int ret, i;
 	__be32 *csd_tmp;
 
-	if (!mmc_host_is_spi(card->host))
-		return mmc_send_cxd_native(card->host, card->rca << 16,
-				csd, MMC_SEND_CSD);
-
 	csd_tmp = kzalloc(16, GFP_KERNEL);
 	if (!csd_tmp)
 		return -ENOMEM;
@@ -327,18 +312,20 @@ err:
 	return ret;
 }
 
-int mmc_send_cid(struct mmc_host *host, u32 *cid)
+int mmc_send_csd(struct mmc_card *card, u32 *csd)
+{
+	if (mmc_host_is_spi(card->host))
+		return mmc_spi_send_csd(card, csd);
+
+	return mmc_send_cxd_native(card->host, card->rca << 16,	csd,
+				MMC_SEND_CSD);
+}
+
+static int mmc_spi_send_cid(struct mmc_host *host, u32 *cid)
 {
 	int ret, i;
 	__be32 *cid_tmp;
 
-	if (!mmc_host_is_spi(host)) {
-		if (!host->card)
-			return -EINVAL;
-		return mmc_send_cxd_native(host, host->card->rca << 16,
-				cid, MMC_SEND_CID);
-	}
-
 	cid_tmp = kzalloc(16, GFP_KERNEL);
 	if (!cid_tmp)
 		return -ENOMEM;
@@ -355,6 +342,14 @@ err:
 	return ret;
 }
 
+int mmc_send_cid(struct mmc_host *host, u32 *cid)
+{
+	if (mmc_host_is_spi(host))
+		return mmc_spi_send_cid(host, cid);
+
+	return mmc_send_cxd_native(host, 0, cid, MMC_ALL_SEND_CID);
+}
+
 int mmc_get_ext_csd(struct mmc_card *card, u8 **new_ext_csd)
 {
 	int err;
@@ -800,7 +795,7 @@ int mmc_bus_test(struct mmc_card *card, u8 bus_width)
 	return mmc_send_bus_test(card, card->host, MMC_BUS_TEST_R, width);
 }
 
-int mmc_send_hpi_cmd(struct mmc_card *card, u32 *status)
+static int mmc_send_hpi_cmd(struct mmc_card *card, u32 *status)
 {
 	struct mmc_command cmd = {};
 	unsigned int opcode;
@@ -834,11 +829,208 @@ int mmc_send_hpi_cmd(struct mmc_card *card, u32 *status)
 	return 0;
 }
 
+/**
+ *	mmc_interrupt_hpi - Issue for High priority Interrupt
+ *	@card: the MMC card associated with the HPI transfer
+ *
+ *	Issued High Priority Interrupt, and check for card status
+ *	until out-of prg-state.
+ */
+int mmc_interrupt_hpi(struct mmc_card *card)
+{
+	int err;
+	u32 status;
+	unsigned long prg_wait;
+
+	if (!card->ext_csd.hpi_en) {
+		pr_info("%s: HPI enable bit unset\n", mmc_hostname(card->host));
+		return 1;
+	}
+
+	mmc_claim_host(card->host);
+	err = mmc_send_status(card, &status);
+	if (err) {
+		pr_err("%s: Get card status fail\n", mmc_hostname(card->host));
+		goto out;
+	}
+
+	switch (R1_CURRENT_STATE(status)) {
+	case R1_STATE_IDLE:
+	case R1_STATE_READY:
+	case R1_STATE_STBY:
+	case R1_STATE_TRAN:
+		/*
+		 * In idle and transfer 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_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);
+	return err;
+}
+
 int mmc_can_ext_csd(struct mmc_card *card)
 {
 	return (card && card->csd.mmca_vsn > CSD_SPEC_VER_3);
 }
 
+/**
+ *	mmc_stop_bkops - stop ongoing BKOPS
+ *	@card: MMC card to check BKOPS
+ *
+ *	Send HPI command to stop ongoing background operations to
+ *	allow rapid servicing of foreground operations, e.g. read/
+ *	writes. Wait until the card comes out of the programming state
+ *	to avoid errors in servicing read/write requests.
+ */
+int mmc_stop_bkops(struct mmc_card *card)
+{
+	int err = 0;
+
+	err = mmc_interrupt_hpi(card);
+
+	/*
+	 * If err is EINVAL, we can't issue an HPI.
+	 * It should complete the BKOPS.
+	 */
+	if (!err || (err == -EINVAL)) {
+		mmc_card_clr_doing_bkops(card);
+		mmc_retune_release(card->host);
+		err = 0;
+	}
+
+	return err;
+}
+
+static int mmc_read_bkops_status(struct mmc_card *card)
+{
+	int err;
+	u8 *ext_csd;
+
+	mmc_claim_host(card->host);
+	err = mmc_get_ext_csd(card, &ext_csd);
+	mmc_release_host(card->host);
+	if (err)
+		return err;
+
+	card->ext_csd.raw_bkops_status = ext_csd[EXT_CSD_BKOPS_STATUS];
+	card->ext_csd.raw_exception_status = ext_csd[EXT_CSD_EXP_EVENTS_STATUS];
+	kfree(ext_csd);
+	return 0;
+}
+
+/**
+ *	mmc_start_bkops - start BKOPS for supported cards
+ *	@card: MMC card to start BKOPS
+ *	@form_exception: A flag to indicate if this function was
+ *			 called due to an exception raised by the card
+ *
+ *	Start background operations whenever requested.
+ *	When the urgent BKOPS bit is set in a R1 command response
+ *	then background operations should be started immediately.
+*/
+void mmc_start_bkops(struct mmc_card *card, bool from_exception)
+{
+	int err;
+	int timeout;
+	bool use_busy_signal;
+
+	if (!card->ext_csd.man_bkops_en || mmc_card_doing_bkops(card))
+		return;
+
+	err = mmc_read_bkops_status(card);
+	if (err) {
+		pr_err("%s: Failed to read bkops status: %d\n",
+		       mmc_hostname(card->host), err);
+		return;
+	}
+
+	if (!card->ext_csd.raw_bkops_status)
+		return;
+
+	if (card->ext_csd.raw_bkops_status < EXT_CSD_BKOPS_LEVEL_2 &&
+	    from_exception)
+		return;
+
+	mmc_claim_host(card->host);
+	if (card->ext_csd.raw_bkops_status >= EXT_CSD_BKOPS_LEVEL_2) {
+		timeout = MMC_OPS_TIMEOUT_MS;
+		use_busy_signal = true;
+	} else {
+		timeout = 0;
+		use_busy_signal = false;
+	}
+
+	mmc_retune_hold(card->host);
+
+	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+			EXT_CSD_BKOPS_START, 1, timeout, 0,
+			use_busy_signal, true, false);
+	if (err) {
+		pr_warn("%s: Error %d starting bkops\n",
+			mmc_hostname(card->host), err);
+		mmc_retune_release(card->host);
+		goto out;
+	}
+
+	/*
+	 * For urgent bkops status (LEVEL_2 and more)
+	 * bkops executed synchronously, otherwise
+	 * the operation is in progress
+	 */
+	if (!use_busy_signal)
+		mmc_card_set_doing_bkops(card);
+	else
+		mmc_retune_release(card->host);
+out:
+	mmc_release_host(card->host);
+}
+
+/*
+ * Flush the cache to the non-volatile storage.
+ */
+int mmc_flush_cache(struct mmc_card *card)
+{
+	int err = 0;
+
+	if (mmc_card_mmc(card) &&
+			(card->ext_csd.cache_size > 0) &&
+			(card->ext_csd.cache_ctrl & 1)) {
+		err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+				EXT_CSD_FLUSH_CACHE, 1, 0);
+		if (err)
+			pr_err("%s: cache flush error %d\n",
+					mmc_hostname(card->host), err);
+	}
+
+	return err;
+}
+EXPORT_SYMBOL(mmc_flush_cache);
+
 static int mmc_cmdq_switch(struct mmc_card *card, bool enable)
 {
 	u8 val = enable ? EXT_CSD_CMDQ_MODE_ENABLED : 0;
diff --git a/drivers/mmc/core/mmc_ops.h b/drivers/mmc/core/mmc_ops.h
index 978bd2e60f8a..a1390d486381 100644
--- a/drivers/mmc/core/mmc_ops.h
+++ b/drivers/mmc/core/mmc_ops.h
@@ -22,15 +22,14 @@ int mmc_deselect_cards(struct mmc_host *host);
 int mmc_set_dsr(struct mmc_host *host);
 int mmc_go_idle(struct mmc_host *host);
 int mmc_send_op_cond(struct mmc_host *host, u32 ocr, u32 *rocr);
-int mmc_all_send_cid(struct mmc_host *host, u32 *cid);
 int mmc_set_relative_addr(struct mmc_card *card);
 int mmc_send_csd(struct mmc_card *card, u32 *csd);
+int __mmc_send_status(struct mmc_card *card, u32 *status, unsigned int retries);
 int mmc_send_status(struct mmc_card *card, u32 *status);
 int mmc_send_cid(struct mmc_host *host, u32 *cid);
 int mmc_spi_read_ocr(struct mmc_host *host, int highcap, u32 *ocrp);
 int mmc_spi_set_crc(struct mmc_host *host, int use_crc);
 int mmc_bus_test(struct mmc_card *card, u8 bus_width);
-int mmc_send_hpi_cmd(struct mmc_card *card, u32 *status);
 int mmc_interrupt_hpi(struct mmc_card *card);
 int mmc_can_ext_csd(struct mmc_card *card);
 int mmc_get_ext_csd(struct mmc_card *card, u8 **new_ext_csd);
@@ -42,9 +41,7 @@ int __mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
 int mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
 		unsigned int timeout_ms);
 int mmc_stop_bkops(struct mmc_card *card);
-int mmc_read_bkops_status(struct mmc_card *card);
 void mmc_start_bkops(struct mmc_card *card, bool from_exception);
-int mmc_can_reset(struct mmc_card *card);
 int mmc_flush_cache(struct mmc_card *card);
 int mmc_cmdq_enable(struct mmc_card *card);
 int mmc_cmdq_disable(struct mmc_card *card);
diff --git a/drivers/mmc/core/mmc_test.c b/drivers/mmc/core/mmc_test.c
index fd1b4b8510b9..7a304a6e5bf1 100644
--- a/drivers/mmc/core/mmc_test.c
+++ b/drivers/mmc/core/mmc_test.c
@@ -3220,8 +3220,6 @@ static int __mmc_test_register_dbgfs_file(struct mmc_card *card,
 	df = kmalloc(sizeof(*df), GFP_KERNEL);
 	if (!df) {
 		debugfs_remove(file);
-		dev_err(&card->dev,
-			"Can't allocate memory for internal usage.\n");
 		return -ENOMEM;
 	}
 
diff --git a/drivers/mmc/core/pwrseq.c b/drivers/mmc/core/pwrseq.c
index 9386c4771814..e3ad30fa8307 100644
--- a/drivers/mmc/core/pwrseq.c
+++ b/drivers/mmc/core/pwrseq.c
@@ -76,6 +76,14 @@ void mmc_pwrseq_power_off(struct mmc_host *host)
 		pwrseq->ops->power_off(host);
 }
 
+void mmc_pwrseq_reset(struct mmc_host *host)
+{
+	struct mmc_pwrseq *pwrseq = host->pwrseq;
+
+	if (pwrseq && pwrseq->ops->reset)
+		pwrseq->ops->reset(host);
+}
+
 void mmc_pwrseq_free(struct mmc_host *host)
 {
 	struct mmc_pwrseq *pwrseq = host->pwrseq;
diff --git a/drivers/mmc/core/pwrseq.h b/drivers/mmc/core/pwrseq.h
index 39c911aa6ebb..819386f4ec61 100644
--- a/drivers/mmc/core/pwrseq.h
+++ b/drivers/mmc/core/pwrseq.h
@@ -18,6 +18,7 @@ struct mmc_pwrseq_ops {
 	void (*pre_power_on)(struct mmc_host *host);
 	void (*post_power_on)(struct mmc_host *host);
 	void (*power_off)(struct mmc_host *host);
+	void (*reset)(struct mmc_host *host);
 };
 
 struct mmc_pwrseq {
@@ -36,6 +37,7 @@ int mmc_pwrseq_alloc(struct mmc_host *host);
 void mmc_pwrseq_pre_power_on(struct mmc_host *host);
 void mmc_pwrseq_post_power_on(struct mmc_host *host);
 void mmc_pwrseq_power_off(struct mmc_host *host);
+void mmc_pwrseq_reset(struct mmc_host *host);
 void mmc_pwrseq_free(struct mmc_host *host);
 
 #else
@@ -49,6 +51,7 @@ static inline int mmc_pwrseq_alloc(struct mmc_host *host) { return 0; }
 static inline void mmc_pwrseq_pre_power_on(struct mmc_host *host) {}
 static inline void mmc_pwrseq_post_power_on(struct mmc_host *host) {}
 static inline void mmc_pwrseq_power_off(struct mmc_host *host) {}
+static inline void mmc_pwrseq_reset(struct mmc_host *host) {}
 static inline void mmc_pwrseq_free(struct mmc_host *host) {}
 
 #endif
diff --git a/drivers/mmc/core/pwrseq_emmc.c b/drivers/mmc/core/pwrseq_emmc.c
index adc9c0c614fb..efb8a7965dd4 100644
--- a/drivers/mmc/core/pwrseq_emmc.c
+++ b/drivers/mmc/core/pwrseq_emmc.c
@@ -56,7 +56,7 @@ static int mmc_pwrseq_emmc_reset_nb(struct notifier_block *this,
 }
 
 static const struct mmc_pwrseq_ops mmc_pwrseq_emmc_ops = {
-	.post_power_on = mmc_pwrseq_emmc_reset,
+	.reset = mmc_pwrseq_emmc_reset,
 };
 
 static int mmc_pwrseq_emmc_probe(struct platform_device *pdev)
diff --git a/drivers/mmc/core/queue.c b/drivers/mmc/core/queue.c
index 5c37b6be3e7b..affa7370ba82 100644
--- a/drivers/mmc/core/queue.c
+++ b/drivers/mmc/core/queue.c
@@ -40,35 +40,6 @@ static int mmc_prep_request(struct request_queue *q, struct request *req)
 	return BLKPREP_OK;
 }
 
-struct mmc_queue_req *mmc_queue_req_find(struct mmc_queue *mq,
-					 struct request *req)
-{
-	struct mmc_queue_req *mqrq;
-	int i = ffz(mq->qslots);
-
-	if (i >= mq->qdepth)
-		return NULL;
-
-	mqrq = &mq->mqrq[i];
-	WARN_ON(mqrq->req || mq->qcnt >= mq->qdepth ||
-		test_bit(mqrq->task_id, &mq->qslots));
-	mqrq->req = req;
-	mq->qcnt += 1;
-	__set_bit(mqrq->task_id, &mq->qslots);
-
-	return mqrq;
-}
-
-void mmc_queue_req_free(struct mmc_queue *mq,
-			struct mmc_queue_req *mqrq)
-{
-	WARN_ON(!mqrq->req || mq->qcnt < 1 ||
-		!test_bit(mqrq->task_id, &mq->qslots));
-	mqrq->req = NULL;
-	mq->qcnt -= 1;
-	__clear_bit(mqrq->task_id, &mq->qslots);
-}
-
 static int mmc_queue_thread(void *d)
 {
 	struct mmc_queue *mq = d;
@@ -133,7 +104,7 @@ static void mmc_request_fn(struct request_queue *q)
 	if (!mq) {
 		while ((req = blk_fetch_request(q)) != NULL) {
 			req->rq_flags |= RQF_QUIET;
-			__blk_end_request_all(req, -EIO);
+			__blk_end_request_all(req, BLK_STS_IOERR);
 		}
 		return;
 	}
@@ -149,11 +120,11 @@ static void mmc_request_fn(struct request_queue *q)
 		wake_up_process(mq->thread);
 }
 
-static struct scatterlist *mmc_alloc_sg(int sg_len)
+static struct scatterlist *mmc_alloc_sg(int sg_len, gfp_t gfp)
 {
 	struct scatterlist *sg;
 
-	sg = kmalloc_array(sg_len, sizeof(*sg), GFP_KERNEL);
+	sg = kmalloc_array(sg_len, sizeof(*sg), gfp);
 	if (sg)
 		sg_init_table(sg, sg_len);
 
@@ -179,86 +150,11 @@ static void mmc_queue_setup_discard(struct request_queue *q,
 		queue_flag_set_unlocked(QUEUE_FLAG_SECERASE, q);
 }
 
-static void mmc_queue_req_free_bufs(struct mmc_queue_req *mqrq)
-{
-	kfree(mqrq->bounce_sg);
-	mqrq->bounce_sg = NULL;
-
-	kfree(mqrq->sg);
-	mqrq->sg = NULL;
-
-	kfree(mqrq->bounce_buf);
-	mqrq->bounce_buf = NULL;
-}
-
-static void mmc_queue_reqs_free_bufs(struct mmc_queue_req *mqrq, int qdepth)
-{
-	int i;
-
-	for (i = 0; i < qdepth; i++)
-		mmc_queue_req_free_bufs(&mqrq[i]);
-}
-
-static void mmc_queue_free_mqrqs(struct mmc_queue_req *mqrq, int qdepth)
-{
-	mmc_queue_reqs_free_bufs(mqrq, qdepth);
-	kfree(mqrq);
-}
-
-static struct mmc_queue_req *mmc_queue_alloc_mqrqs(int qdepth)
-{
-	struct mmc_queue_req *mqrq;
-	int i;
-
-	mqrq = kcalloc(qdepth, sizeof(*mqrq), GFP_KERNEL);
-	if (mqrq) {
-		for (i = 0; i < qdepth; i++)
-			mqrq[i].task_id = i;
-	}
-
-	return mqrq;
-}
-
-#ifdef CONFIG_MMC_BLOCK_BOUNCE
-static int mmc_queue_alloc_bounce_bufs(struct mmc_queue_req *mqrq, int qdepth,
-				       unsigned int bouncesz)
-{
-	int i;
-
-	for (i = 0; i < qdepth; i++) {
-		mqrq[i].bounce_buf = kmalloc(bouncesz, GFP_KERNEL);
-		if (!mqrq[i].bounce_buf)
-			return -ENOMEM;
-
-		mqrq[i].sg = mmc_alloc_sg(1);
-		if (!mqrq[i].sg)
-			return -ENOMEM;
-
-		mqrq[i].bounce_sg = mmc_alloc_sg(bouncesz / 512);
-		if (!mqrq[i].bounce_sg)
-			return -ENOMEM;
-	}
-
-	return 0;
-}
-
-static bool mmc_queue_alloc_bounce(struct mmc_queue_req *mqrq, int qdepth,
-				   unsigned int bouncesz)
-{
-	int ret;
-
-	ret = mmc_queue_alloc_bounce_bufs(mqrq, qdepth, bouncesz);
-	if (ret)
-		mmc_queue_reqs_free_bufs(mqrq, qdepth);
-
-	return !ret;
-}
-
 static unsigned int mmc_queue_calc_bouncesz(struct mmc_host *host)
 {
 	unsigned int bouncesz = MMC_QUEUE_BOUNCESZ;
 
-	if (host->max_segs != 1)
+	if (host->max_segs != 1 || (host->caps & MMC_CAP_NO_BOUNCE_BUFF))
 		return 0;
 
 	if (bouncesz > host->max_req_size)
@@ -273,84 +169,58 @@ static unsigned int mmc_queue_calc_bouncesz(struct mmc_host *host)
 
 	return bouncesz;
 }
-#else
-static inline bool mmc_queue_alloc_bounce(struct mmc_queue_req *mqrq,
-					  int qdepth, unsigned int bouncesz)
-{
-	return false;
-}
 
-static unsigned int mmc_queue_calc_bouncesz(struct mmc_host *host)
-{
-	return 0;
-}
-#endif
-
-static int mmc_queue_alloc_sgs(struct mmc_queue_req *mqrq, int qdepth,
-			       int max_segs)
+/**
+ * mmc_init_request() - initialize the MMC-specific per-request data
+ * @q: the request queue
+ * @req: the request
+ * @gfp: memory allocation policy
+ */
+static int mmc_init_request(struct request_queue *q, struct request *req,
+			    gfp_t gfp)
 {
-	int i;
+	struct mmc_queue_req *mq_rq = req_to_mmc_queue_req(req);
+	struct mmc_queue *mq = q->queuedata;
+	struct mmc_card *card = mq->card;
+	struct mmc_host *host = card->host;
 
-	for (i = 0; i < qdepth; i++) {
-		mqrq[i].sg = mmc_alloc_sg(max_segs);
-		if (!mqrq[i].sg)
+	if (card->bouncesz) {
+		mq_rq->bounce_buf = kmalloc(card->bouncesz, gfp);
+		if (!mq_rq->bounce_buf)
+			return -ENOMEM;
+		if (card->bouncesz > 512) {
+			mq_rq->sg = mmc_alloc_sg(1, gfp);
+			if (!mq_rq->sg)
+				return -ENOMEM;
+			mq_rq->bounce_sg = mmc_alloc_sg(card->bouncesz / 512,
+							gfp);
+			if (!mq_rq->bounce_sg)
+				return -ENOMEM;
+		}
+	} else {
+		mq_rq->bounce_buf = NULL;
+		mq_rq->bounce_sg = NULL;
+		mq_rq->sg = mmc_alloc_sg(host->max_segs, gfp);
+		if (!mq_rq->sg)
 			return -ENOMEM;
 	}
 
 	return 0;
 }
 
-void mmc_queue_free_shared_queue(struct mmc_card *card)
+static void mmc_exit_request(struct request_queue *q, struct request *req)
 {
-	if (card->mqrq) {
-		mmc_queue_free_mqrqs(card->mqrq, card->qdepth);
-		card->mqrq = NULL;
-	}
-}
-
-static int __mmc_queue_alloc_shared_queue(struct mmc_card *card, int qdepth)
-{
-	struct mmc_host *host = card->host;
-	struct mmc_queue_req *mqrq;
-	unsigned int bouncesz;
-	int ret = 0;
-
-	if (card->mqrq)
-		return -EINVAL;
+	struct mmc_queue_req *mq_rq = req_to_mmc_queue_req(req);
 
-	mqrq = mmc_queue_alloc_mqrqs(qdepth);
-	if (!mqrq)
-		return -ENOMEM;
-
-	card->mqrq = mqrq;
-	card->qdepth = qdepth;
-
-	bouncesz = mmc_queue_calc_bouncesz(host);
-
-	if (bouncesz && !mmc_queue_alloc_bounce(mqrq, qdepth, bouncesz)) {
-		bouncesz = 0;
-		pr_warn("%s: unable to allocate bounce buffers\n",
-			mmc_card_name(card));
-	}
+	/* It is OK to kfree(NULL) so this will be smooth */
+	kfree(mq_rq->bounce_sg);
+	mq_rq->bounce_sg = NULL;
 
-	card->bouncesz = bouncesz;
+	kfree(mq_rq->bounce_buf);
+	mq_rq->bounce_buf = NULL;
 
-	if (!bouncesz) {
-		ret = mmc_queue_alloc_sgs(mqrq, qdepth, host->max_segs);
-		if (ret)
-			goto out_err;
-	}
-
-	return ret;
-
-out_err:
-	mmc_queue_free_shared_queue(card);
-	return ret;
-}
-
-int mmc_queue_alloc_shared_queue(struct mmc_card *card)
-{
-	return __mmc_queue_alloc_shared_queue(card, 2);
+	kfree(mq_rq->sg);
+	mq_rq->sg = NULL;
 }
 
 /**
@@ -373,13 +243,21 @@ int mmc_init_queue(struct mmc_queue *mq, struct mmc_card *card,
 		limit = (u64)dma_max_pfn(mmc_dev(host)) << PAGE_SHIFT;
 
 	mq->card = card;
-	mq->queue = blk_init_queue(mmc_request_fn, lock);
+	mq->queue = blk_alloc_queue(GFP_KERNEL);
 	if (!mq->queue)
 		return -ENOMEM;
-
-	mq->mqrq = card->mqrq;
-	mq->qdepth = card->qdepth;
+	mq->queue->queue_lock = lock;
+	mq->queue->request_fn = mmc_request_fn;
+	mq->queue->init_rq_fn = mmc_init_request;
+	mq->queue->exit_rq_fn = mmc_exit_request;
+	mq->queue->cmd_size = sizeof(struct mmc_queue_req);
 	mq->queue->queuedata = mq;
+	mq->qcnt = 0;
+	ret = blk_init_allocated_queue(mq->queue);
+	if (ret) {
+		blk_cleanup_queue(mq->queue);
+		return ret;
+	}
 
 	blk_queue_prep_rq(mq->queue, mmc_prep_request);
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, mq->queue);
@@ -387,8 +265,8 @@ int mmc_init_queue(struct mmc_queue *mq, struct mmc_card *card,
 	if (mmc_can_erase(card))
 		mmc_queue_setup_discard(mq->queue, card);
 
+	card->bouncesz = mmc_queue_calc_bouncesz(host);
 	if (card->bouncesz) {
-		blk_queue_bounce_limit(mq->queue, BLK_BOUNCE_ANY);
 		blk_queue_max_hw_sectors(mq->queue, card->bouncesz / 512);
 		blk_queue_max_segments(mq->queue, card->bouncesz / 512);
 		blk_queue_max_segment_size(mq->queue, card->bouncesz);
@@ -413,7 +291,6 @@ int mmc_init_queue(struct mmc_queue *mq, struct mmc_card *card,
 	return 0;
 
 cleanup_queue:
-	mq->mqrq = NULL;
 	blk_cleanup_queue(mq->queue);
 	return ret;
 }
@@ -435,7 +312,6 @@ void mmc_cleanup_queue(struct mmc_queue *mq)
 	blk_start_queue(q);
 	spin_unlock_irqrestore(q->queue_lock, flags);
 
-	mq->mqrq = NULL;
 	mq->card = NULL;
 }
 EXPORT_SYMBOL(mmc_cleanup_queue);
@@ -492,12 +368,13 @@ unsigned int mmc_queue_map_sg(struct mmc_queue *mq, struct mmc_queue_req *mqrq)
 	unsigned int sg_len;
 	size_t buflen;
 	struct scatterlist *sg;
+	struct request *req = mmc_queue_req_to_req(mqrq);
 	int i;
 
 	if (!mqrq->bounce_buf)
-		return blk_rq_map_sg(mq->queue, mqrq->req, mqrq->sg);
+		return blk_rq_map_sg(mq->queue, req, mqrq->sg);
 
-	sg_len = blk_rq_map_sg(mq->queue, mqrq->req, mqrq->bounce_sg);
+	sg_len = blk_rq_map_sg(mq->queue, req, mqrq->bounce_sg);
 
 	mqrq->bounce_sg_len = sg_len;
 
@@ -519,7 +396,7 @@ void mmc_queue_bounce_pre(struct mmc_queue_req *mqrq)
 	if (!mqrq->bounce_buf)
 		return;
 
-	if (rq_data_dir(mqrq->req) != WRITE)
+	if (rq_data_dir(mmc_queue_req_to_req(mqrq)) != WRITE)
 		return;
 
 	sg_copy_to_buffer(mqrq->bounce_sg, mqrq->bounce_sg_len,
@@ -535,7 +412,7 @@ void mmc_queue_bounce_post(struct mmc_queue_req *mqrq)
 	if (!mqrq->bounce_buf)
 		return;
 
-	if (rq_data_dir(mqrq->req) != READ)
+	if (rq_data_dir(mmc_queue_req_to_req(mqrq)) != READ)
 		return;
 
 	sg_copy_from_buffer(mqrq->bounce_sg, mqrq->bounce_sg_len,
diff --git a/drivers/mmc/core/queue.h b/drivers/mmc/core/queue.h
index 871796c3f406..361b46408e0f 100644
--- a/drivers/mmc/core/queue.h
+++ b/drivers/mmc/core/queue.h
@@ -3,19 +3,25 @@
 
 #include <linux/types.h>
 #include <linux/blkdev.h>
+#include <linux/blk-mq.h>
 #include <linux/mmc/core.h>
 #include <linux/mmc/host.h>
 
-static inline bool mmc_req_is_special(struct request *req)
+static inline struct mmc_queue_req *req_to_mmc_queue_req(struct request *rq)
 {
-	return req &&
-		(req_op(req) == REQ_OP_FLUSH ||
-		 req_op(req) == REQ_OP_DISCARD ||
-		 req_op(req) == REQ_OP_SECURE_ERASE);
+	return blk_mq_rq_to_pdu(rq);
+}
+
+struct mmc_queue_req;
+
+static inline struct request *mmc_queue_req_to_req(struct mmc_queue_req *mqr)
+{
+	return blk_mq_rq_from_pdu(mqr);
 }
 
 struct task_struct;
 struct mmc_blk_data;
+struct mmc_blk_ioc_data;
 
 struct mmc_blk_request {
 	struct mmc_request	mrq;
@@ -26,15 +32,27 @@ struct mmc_blk_request {
 	int			retune_retry_done;
 };
 
+/**
+ * enum mmc_drv_op - enumerates the operations in the mmc_queue_req
+ * @MMC_DRV_OP_IOCTL: ioctl operation
+ * @MMC_DRV_OP_BOOT_WP: write protect boot partitions
+ */
+enum mmc_drv_op {
+	MMC_DRV_OP_IOCTL,
+	MMC_DRV_OP_BOOT_WP,
+};
+
 struct mmc_queue_req {
-	struct request		*req;
 	struct mmc_blk_request	brq;
 	struct scatterlist	*sg;
 	char			*bounce_buf;
 	struct scatterlist	*bounce_sg;
 	unsigned int		bounce_sg_len;
 	struct mmc_async_req	areq;
-	int			task_id;
+	enum mmc_drv_op		drv_op;
+	int			drv_op_result;
+	struct mmc_blk_ioc_data	**idata;
+	unsigned int		ioc_count;
 };
 
 struct mmc_queue {
@@ -45,14 +63,15 @@ struct mmc_queue {
 	bool			asleep;
 	struct mmc_blk_data	*blkdata;
 	struct request_queue	*queue;
-	struct mmc_queue_req	*mqrq;
-	int			qdepth;
+	/*
+	 * FIXME: this counter is not a very reliable way of keeping
+	 * track of how many requests that are ongoing. Switch to just
+	 * letting the block core keep track of requests and per-request
+	 * associated mmc_queue_req data.
+	 */
 	int			qcnt;
-	unsigned long		qslots;
 };
 
-extern int mmc_queue_alloc_shared_queue(struct mmc_card *card);
-extern void mmc_queue_free_shared_queue(struct mmc_card *card);
 extern int mmc_init_queue(struct mmc_queue *, struct mmc_card *, spinlock_t *,
 			  const char *);
 extern void mmc_cleanup_queue(struct mmc_queue *);
@@ -66,8 +85,4 @@ extern void mmc_queue_bounce_post(struct mmc_queue_req *);
 
 extern int mmc_access_rpmb(struct mmc_queue *);
 
-extern struct mmc_queue_req *mmc_queue_req_find(struct mmc_queue *,
-						struct request *);
-extern void mmc_queue_req_free(struct mmc_queue *, struct mmc_queue_req *);
-
 #endif
diff --git a/drivers/mmc/core/sd.c b/drivers/mmc/core/sd.c
index d109634fbfce..a1b0aa14d5e3 100644
--- a/drivers/mmc/core/sd.c
+++ b/drivers/mmc/core/sd.c
@@ -294,12 +294,8 @@ static int mmc_read_switch(struct mmc_card *card)
 	err = -EIO;
 
 	status = kmalloc(64, GFP_KERNEL);
-	if (!status) {
-		pr_err("%s: could not allocate a buffer for "
-			"switch capabilities.\n",
-			mmc_hostname(card->host));
+	if (!status)
 		return -ENOMEM;
-	}
 
 	/*
 	 * Find out the card's support bits with a mode 0 operation.
@@ -359,11 +355,8 @@ int mmc_sd_switch_hs(struct mmc_card *card)
 		return 0;
 
 	status = kmalloc(64, GFP_KERNEL);
-	if (!status) {
-		pr_err("%s: could not allocate a buffer for "
-			"switch capabilities.\n", mmc_hostname(card->host));
+	if (!status)
 		return -ENOMEM;
-	}
 
 	err = mmc_sd_switch(card, 1, 0, 1, status);
 	if (err)
@@ -596,11 +589,8 @@ static int mmc_sd_init_uhs_card(struct mmc_card *card)
 		return 0;
 
 	status = kmalloc(64, GFP_KERNEL);
-	if (!status) {
-		pr_err("%s: could not allocate a buffer for "
-			"switch capabilities.\n", mmc_hostname(card->host));
+	if (!status)
 		return -ENOMEM;
-	}
 
 	/* Set 4-bit bus width */
 	if ((card->host->caps & MMC_CAP_4_BIT_DATA) &&
@@ -798,11 +788,7 @@ try_again:
 		}
 	}
 
-	if (mmc_host_is_spi(host))
-		err = mmc_send_cid(host, cid);
-	else
-		err = mmc_all_send_cid(host, cid);
-
+	err = mmc_send_cid(host, cid);
 	return err;
 }
 
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index fae732c870a9..cc43687ca241 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -1104,6 +1104,12 @@ int mmc_attach_sdio(struct mmc_host *host)
 	 */
 	if (host->caps & MMC_CAP_POWER_OFF_CARD) {
 		/*
+		 * Do not allow runtime suspend until after SDIO function
+		 * devices are added.
+		 */
+		pm_runtime_get_noresume(&card->dev);
+
+		/*
 		 * Let runtime PM core know our card is active
 		 */
 		err = pm_runtime_set_active(&card->dev);
@@ -1155,19 +1161,23 @@ int mmc_attach_sdio(struct mmc_host *host)
 			goto remove_added;
 	}
 
+	if (host->caps & MMC_CAP_POWER_OFF_CARD)
+		pm_runtime_put(&card->dev);
+
 	mmc_claim_host(host);
 	return 0;
 
 
-remove_added:
-	/* Remove without lock if the device has been added. */
-	mmc_sdio_remove(host);
-	mmc_claim_host(host);
 remove:
-	/* And with lock if it hasn't been added. */
 	mmc_release_host(host);
-	if (host->card)
-		mmc_sdio_remove(host);
+remove_added:
+	/*
+	 * The devices are being deleted so it is not necessary to disable
+	 * runtime PM. Similarly we also don't pm_runtime_put() the SDIO card
+	 * because it needs to be active to remove any function devices that
+	 * were probed, and after that it gets deleted.
+	 */
+	mmc_sdio_remove(host);
 	mmc_claim_host(host);
 err:
 	mmc_detach_bus(host);
diff --git a/drivers/mmc/core/sdio_irq.c b/drivers/mmc/core/sdio_irq.c
index 6d4b72080d51..c771843e4c15 100644
--- a/drivers/mmc/core/sdio_irq.c
+++ b/drivers/mmc/core/sdio_irq.c
@@ -95,12 +95,30 @@ static int process_sdio_pending_irqs(struct mmc_host *host)
 void sdio_run_irqs(struct mmc_host *host)
 {
 	mmc_claim_host(host);
-	host->sdio_irq_pending = true;
-	process_sdio_pending_irqs(host);
+	if (host->sdio_irqs) {
+		host->sdio_irq_pending = true;
+		process_sdio_pending_irqs(host);
+		if (host->ops->ack_sdio_irq)
+			host->ops->ack_sdio_irq(host);
+	}
 	mmc_release_host(host);
 }
 EXPORT_SYMBOL_GPL(sdio_run_irqs);
 
+void sdio_irq_work(struct work_struct *work)
+{
+	struct mmc_host *host =
+		container_of(work, struct mmc_host, sdio_irq_work.work);
+
+	sdio_run_irqs(host);
+}
+
+void sdio_signal_irq(struct mmc_host *host)
+{
+	queue_delayed_work(system_wq, &host->sdio_irq_work, 0);
+}
+EXPORT_SYMBOL_GPL(sdio_signal_irq);
+
 static int sdio_irq_thread(void *_host)
 {
 	struct mmc_host *host = _host;
diff --git a/drivers/mmc/core/sdio_ops.h b/drivers/mmc/core/sdio_ops.h
index ee35cb4d170e..96945cafbf0b 100644
--- a/drivers/mmc/core/sdio_ops.h
+++ b/drivers/mmc/core/sdio_ops.h
@@ -17,6 +17,7 @@
 
 struct mmc_host;
 struct mmc_card;
+struct work_struct;
 
 int mmc_send_io_op_cond(struct mmc_host *host, u32 ocr, u32 *rocr);
 int mmc_io_rw_direct(struct mmc_card *card, int write, unsigned fn,
@@ -25,6 +26,7 @@ int mmc_io_rw_extended(struct mmc_card *card, int write, unsigned fn,
 	unsigned addr, int incr_addr, u8 *buf, unsigned blocks, unsigned blksz);
 int sdio_reset(struct mmc_host *host);
 unsigned int mmc_align_data_size(struct mmc_card *card, unsigned int sz);
+void sdio_irq_work(struct work_struct *work);
 
 static inline bool sdio_is_io_busy(u32 opcode, u32 arg)
 {
diff --git a/drivers/mmc/core/slot-gpio.c b/drivers/mmc/core/slot-gpio.c
index a8450a8701e4..863f1dbbfc1b 100644
--- a/drivers/mmc/core/slot-gpio.c
+++ b/drivers/mmc/core/slot-gpio.c
@@ -151,6 +151,8 @@ void mmc_gpiod_request_cd_irq(struct mmc_host *host)
 
 	if (irq < 0)
 		host->caps |= MMC_CAP_NEEDS_POLL;
+	else if ((host->caps & MMC_CAP_CD_WAKE) && !enable_irq_wake(irq))
+		host->slot.cd_wake_enabled = true;
 }
 EXPORT_SYMBOL(mmc_gpiod_request_cd_irq);
 
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index 2db84dd664d7..5755b69f2f72 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -408,11 +408,11 @@ config MMC_AU1X
 
 config MMC_ATMELMCI
 	tristate "Atmel SD/MMC Driver (Multimedia Card Interface)"
-	depends on AVR32 || ARCH_AT91
+	depends on ARCH_AT91
 	help
-	  This selects the Atmel Multimedia Card Interface driver. If
-	  you have an AT32 (AVR32) or AT91 platform with a Multimedia
-	  Card slot, say Y or M here.
+	  This selects the Atmel Multimedia Card Interface driver.
+	  If you have an AT91 platform with a Multimedia Card slot,
+	  say Y or M here.
 
 	  If unsure, say N.
 
@@ -571,13 +571,13 @@ config MMC_TMIO
 	  T7L66XB and also HTC ASIC3
 
 config MMC_SDHI
-	tristate "SH-Mobile SDHI SD/SDIO controller support"
+	tristate "Renesas SDHI SD/SDIO controller support"
 	depends on SUPERH || ARM || ARM64
 	depends on SUPERH || ARCH_RENESAS || COMPILE_TEST
 	select MMC_TMIO_CORE
 	help
 	  This provides support for the SDHI SD/SDIO controller found in
-	  SuperH and ARM SH-Mobile SoCs
+	  Renesas SuperH, ARM and ARM64 based SoCs
 
 config MMC_CB710
 	tristate "ENE CB710 MMC/SD Interface support"
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index 926347c2eeb4..4d4547116311 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -36,9 +36,7 @@ obj-$(CONFIG_MMC_S3C)   	+= s3cmci.o
 obj-$(CONFIG_MMC_SDRICOH_CS)	+= sdricoh_cs.o
 obj-$(CONFIG_MMC_TMIO)		+= tmio_mmc.o
 obj-$(CONFIG_MMC_TMIO_CORE)	+= tmio_mmc_core.o
-tmio_mmc_core-y			:= tmio_mmc_pio.o
-tmio_mmc_core-$(subst m,y,$(CONFIG_MMC_SDHI))	+= tmio_mmc_dma.o
-obj-$(CONFIG_MMC_SDHI)		+= sh_mobile_sdhi.o
+obj-$(CONFIG_MMC_SDHI)		+= renesas_sdhi_core.o renesas_sdhi_sys_dmac.o
 obj-$(CONFIG_MMC_CB710)		+= cb710-mmc.o
 obj-$(CONFIG_MMC_VIA_SDMMC)	+= via-sdmmc.o
 obj-$(CONFIG_SDH_BFIN)		+= bfin_sdh.o
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index 388e4a3f13e6..97de2d32ba84 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -44,7 +44,7 @@
 #include <asm/unaligned.h>
 
 /*
- * Superset of MCI IP registers integrated in Atmel AVR32 and AT91 Processors
+ * Superset of MCI IP registers integrated in Atmel AT91 Processor
  * Registers and bitfields marked with [2] are only available in MCI2
  */
 
@@ -172,13 +172,6 @@
 #define	atmci_writel(port, reg, value)			\
 	__raw_writel((value), (port)->regs + reg)
 
-/* On AVR chips the Peripheral DMA Controller is not connected to MCI. */
-#ifdef CONFIG_AVR32
-#	define ATMCI_PDC_CONNECTED	0
-#else
-#	define ATMCI_PDC_CONNECTED	1
-#endif
-
 #define AUTOSUSPEND_DELAY	50
 
 #define ATMCI_DATA_ERROR_FLAGS	(ATMCI_DCRCE | ATMCI_DTOE | ATMCI_OVRE | ATMCI_UNRE)
@@ -667,10 +660,8 @@ atmci_of_init(struct platform_device *pdev)
 	}
 
 	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
-	if (!pdata) {
-		dev_err(&pdev->dev, "could not allocate memory for pdata\n");
+	if (!pdata)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	for_each_child_of_node(np, cnp) {
 		if (of_property_read_u32(cnp, "reg", &slot_id)) {
@@ -1549,21 +1540,8 @@ static void atmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 			mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, ios->vdd);
 		break;
 	default:
-		/*
-		 * TODO: None of the currently available AVR32-based
-		 * boards allow MMC power to be turned off. Implement
-		 * power control when this can be tested properly.
-		 *
-		 * We also need to hook this into the clock management
-		 * somehow so that newly inserted cards aren't
-		 * subjected to a fast clock before we have a chance
-		 * to figure out what the maximum rate is. Currently,
-		 * there's no way to avoid this, and there never will
-		 * be for boards that don't support power control.
-		 */
 		break;
 	}
-
 }
 
 static int atmci_get_ro(struct mmc_host *mmc)
@@ -2464,7 +2442,7 @@ static void atmci_get_cap(struct atmel_mci *host)
 			"version: 0x%x\n", version);
 
 	host->caps.has_dma_conf_reg = 0;
-	host->caps.has_pdc = ATMCI_PDC_CONNECTED;
+	host->caps.has_pdc = 1;
 	host->caps.has_cfg_reg = 0;
 	host->caps.has_cstor_reg = 0;
 	host->caps.has_highspeed = 0;
diff --git a/drivers/mmc/host/bcm2835.c b/drivers/mmc/host/bcm2835.c
index 1f343a477b3d..abba9a2a78b8 100644
--- a/drivers/mmc/host/bcm2835.c
+++ b/drivers/mmc/host/bcm2835.c
@@ -1172,7 +1172,10 @@ static void bcm2835_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	if (mrq->data && !is_power_of_2(mrq->data->blksz)) {
 		dev_err(dev, "unsupported block size (%d bytes)\n",
 			mrq->data->blksz);
-		mrq->cmd->error = -EINVAL;
+
+		if (mrq->cmd)
+			mrq->cmd->error = -EINVAL;
+
 		mmc_request_done(mmc, mrq);
 		return;
 	}
@@ -1194,7 +1197,10 @@ static void bcm2835_request(struct mmc_host *mmc, struct mmc_request *mrq)
 			readl(host->ioaddr + SDCMD) & SDCMD_CMD_MASK,
 			edm);
 		bcm2835_dumpregs(host);
-		mrq->cmd->error = -EILSEQ;
+
+		if (mrq->cmd)
+			mrq->cmd->error = -EILSEQ;
+
 		bcm2835_finish_request(host);
 		mutex_unlock(&host->mutex);
 		return;
@@ -1207,7 +1213,7 @@ static void bcm2835_request(struct mmc_host *mmc, struct mmc_request *mrq)
 			if (!host->use_busy)
 				bcm2835_finish_command(host);
 		}
-	} else if (bcm2835_send_command(host, mrq->cmd)) {
+	} else if (mrq->cmd && bcm2835_send_command(host, mrq->cmd)) {
 		if (host->data && host->dma_desc) {
 			/* DMA transfer starts now, PIO starts after irq */
 			bcm2835_start_dma(host);
diff --git a/drivers/mmc/host/cavium.c b/drivers/mmc/host/cavium.c
index b8aaf0fdb77c..3686d77c717b 100644
--- a/drivers/mmc/host/cavium.c
+++ b/drivers/mmc/host/cavium.c
@@ -1035,10 +1035,12 @@ int cvm_mmc_of_slot_probe(struct device *dev, struct cvm_mmc_host *host)
 	 * We only have a 3.3v supply, we cannot support any
 	 * of the UHS modes. We do support the high speed DDR
 	 * modes up to 52MHz.
+	 *
+	 * Disable bounce buffers for max_segs = 1
 	 */
 	mmc->caps |= MMC_CAP_MMC_HIGHSPEED | MMC_CAP_SD_HIGHSPEED |
 		     MMC_CAP_ERASE | MMC_CAP_CMD23 | MMC_CAP_POWER_OFF_CARD |
-		     MMC_CAP_3_3V_DDR;
+		     MMC_CAP_3_3V_DDR | MMC_CAP_NO_BOUNCE_BUFF;
 
 	if (host->use_sg)
 		mmc->max_segs = 16;
diff --git a/drivers/mmc/host/dw_mmc-exynos.c b/drivers/mmc/host/dw_mmc-exynos.c
index 25691cca1881..35026795be28 100644
--- a/drivers/mmc/host/dw_mmc-exynos.c
+++ b/drivers/mmc/host/dw_mmc-exynos.c
@@ -157,8 +157,8 @@ static void dw_mci_exynos_set_clksel_timing(struct dw_mci *host, u32 timing)
 	 * HOLD register should be bypassed in case there is no phase shift
 	 * applied on CMD/DATA that is sent to the card.
 	 */
-	if (!SDMMC_CLKSEL_GET_DRV_WD3(clksel) && host->cur_slot)
-		set_bit(DW_MMC_CARD_NO_USE_HOLD, &host->cur_slot->flags);
+	if (!SDMMC_CLKSEL_GET_DRV_WD3(clksel) && host->slot)
+		set_bit(DW_MMC_CARD_NO_USE_HOLD, &host->slot->flags);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/mmc/host/dw_mmc-rockchip.c b/drivers/mmc/host/dw_mmc-rockchip.c
index 372fb6e948c1..a3f1c2b30145 100644
--- a/drivers/mmc/host/dw_mmc-rockchip.c
+++ b/drivers/mmc/host/dw_mmc-rockchip.c
@@ -25,6 +25,7 @@ struct dw_mci_rockchip_priv_data {
 	struct clk		*drv_clk;
 	struct clk		*sample_clk;
 	int			default_sample_phase;
+	int			num_phases;
 };
 
 static void dw_mci_rk3288_set_ios(struct dw_mci *host, struct mmc_ios *ios)
@@ -133,8 +134,8 @@ static void dw_mci_rk3288_set_ios(struct dw_mci *host, struct mmc_ios *ios)
 	}
 }
 
-#define NUM_PHASES			360
-#define TUNING_ITERATION_TO_PHASE(i)	(DIV_ROUND_UP((i) * 360, NUM_PHASES))
+#define TUNING_ITERATION_TO_PHASE(i, num_phases) \
+		(DIV_ROUND_UP((i) * 360, num_phases))
 
 static int dw_mci_rk3288_execute_tuning(struct dw_mci_slot *slot, u32 opcode)
 {
@@ -159,13 +160,15 @@ static int dw_mci_rk3288_execute_tuning(struct dw_mci_slot *slot, u32 opcode)
 		return -EIO;
 	}
 
-	ranges = kmalloc_array(NUM_PHASES / 2 + 1, sizeof(*ranges), GFP_KERNEL);
+	ranges = kmalloc_array(priv->num_phases / 2 + 1,
+			       sizeof(*ranges), GFP_KERNEL);
 	if (!ranges)
 		return -ENOMEM;
 
 	/* Try each phase and extract good ranges */
-	for (i = 0; i < NUM_PHASES; ) {
-		clk_set_phase(priv->sample_clk, TUNING_ITERATION_TO_PHASE(i));
+	for (i = 0; i < priv->num_phases; ) {
+		clk_set_phase(priv->sample_clk,
+			      TUNING_ITERATION_TO_PHASE(i, priv->num_phases));
 
 		v = !mmc_send_tuning(mmc, opcode, NULL);
 
@@ -179,7 +182,7 @@ static int dw_mci_rk3288_execute_tuning(struct dw_mci_slot *slot, u32 opcode)
 		if (v) {
 			ranges[range_count-1].end = i;
 			i++;
-		} else if (i == NUM_PHASES - 1) {
+		} else if (i == priv->num_phases - 1) {
 			/* No extra skipping rules if we're at the end */
 			i++;
 		} else {
@@ -188,11 +191,11 @@ static int dw_mci_rk3288_execute_tuning(struct dw_mci_slot *slot, u32 opcode)
 			 * one since testing bad phases is slow.  Skip
 			 * 20 degrees.
 			 */
-			i += DIV_ROUND_UP(20 * NUM_PHASES, 360);
+			i += DIV_ROUND_UP(20 * priv->num_phases, 360);
 
 			/* Always test the last one */
-			if (i >= NUM_PHASES)
-				i = NUM_PHASES - 1;
+			if (i >= priv->num_phases)
+				i = priv->num_phases - 1;
 		}
 
 		prev_v = v;
@@ -210,7 +213,7 @@ static int dw_mci_rk3288_execute_tuning(struct dw_mci_slot *slot, u32 opcode)
 		range_count--;
 	}
 
-	if (ranges[0].start == 0 && ranges[0].end == NUM_PHASES - 1) {
+	if (ranges[0].start == 0 && ranges[0].end == priv->num_phases - 1) {
 		clk_set_phase(priv->sample_clk, priv->default_sample_phase);
 		dev_info(host->dev, "All phases work, using default phase %d.",
 			 priv->default_sample_phase);
@@ -222,7 +225,7 @@ static int dw_mci_rk3288_execute_tuning(struct dw_mci_slot *slot, u32 opcode)
 		int len = (ranges[i].end - ranges[i].start + 1);
 
 		if (len < 0)
-			len += NUM_PHASES;
+			len += priv->num_phases;
 
 		if (longest_range_len < len) {
 			longest_range_len = len;
@@ -230,25 +233,30 @@ static int dw_mci_rk3288_execute_tuning(struct dw_mci_slot *slot, u32 opcode)
 		}
 
 		dev_dbg(host->dev, "Good phase range %d-%d (%d len)\n",
-			TUNING_ITERATION_TO_PHASE(ranges[i].start),
-			TUNING_ITERATION_TO_PHASE(ranges[i].end),
+			TUNING_ITERATION_TO_PHASE(ranges[i].start,
+						  priv->num_phases),
+			TUNING_ITERATION_TO_PHASE(ranges[i].end,
+						  priv->num_phases),
 			len
 		);
 	}
 
 	dev_dbg(host->dev, "Best phase range %d-%d (%d len)\n",
-		TUNING_ITERATION_TO_PHASE(ranges[longest_range].start),
-		TUNING_ITERATION_TO_PHASE(ranges[longest_range].end),
+		TUNING_ITERATION_TO_PHASE(ranges[longest_range].start,
+					  priv->num_phases),
+		TUNING_ITERATION_TO_PHASE(ranges[longest_range].end,
+					  priv->num_phases),
 		longest_range_len
 	);
 
 	middle_phase = ranges[longest_range].start + longest_range_len / 2;
-	middle_phase %= NUM_PHASES;
+	middle_phase %= priv->num_phases;
 	dev_info(host->dev, "Successfully tuned phase to %d\n",
-		 TUNING_ITERATION_TO_PHASE(middle_phase));
+		 TUNING_ITERATION_TO_PHASE(middle_phase, priv->num_phases));
 
 	clk_set_phase(priv->sample_clk,
-		      TUNING_ITERATION_TO_PHASE(middle_phase));
+		      TUNING_ITERATION_TO_PHASE(middle_phase,
+						priv->num_phases));
 
 free:
 	kfree(ranges);
@@ -264,6 +272,10 @@ static int dw_mci_rk3288_parse_dt(struct dw_mci *host)
 	if (!priv)
 		return -ENOMEM;
 
+	if (of_property_read_u32(np, "rockchip,desired-num-phases",
+					&priv->num_phases))
+		priv->num_phases = 360;
+
 	if (of_property_read_u32(np, "rockchip,default-sample-phase",
 					&priv->default_sample_phase))
 		priv->default_sample_phase = 0;
diff --git a/drivers/mmc/host/dw_mmc.c b/drivers/mmc/host/dw_mmc.c
index e45129f48174..a9dfb26972f2 100644
--- a/drivers/mmc/host/dw_mmc.c
+++ b/drivers/mmc/host/dw_mmc.c
@@ -392,7 +392,7 @@ static u32 dw_mci_prep_stop_abort(struct dw_mci *host, struct mmc_command *cmd)
 	cmdr = stop->opcode | SDMMC_CMD_STOP |
 		SDMMC_CMD_RESP_CRC | SDMMC_CMD_RESP_EXP;
 
-	if (!test_bit(DW_MMC_CARD_NO_USE_HOLD, &host->cur_slot->flags))
+	if (!test_bit(DW_MMC_CARD_NO_USE_HOLD, &host->slot->flags))
 		cmdr |= SDMMC_CMD_USE_HOLD_REG;
 
 	return cmdr;
@@ -480,7 +480,7 @@ static void dw_mci_dmac_complete_dma(void *arg)
 	if ((host->use_dma == TRANS_MODE_EDMAC) &&
 	    data && (data->flags & MMC_DATA_READ))
 		/* Invalidate cache after read */
-		dma_sync_sg_for_cpu(mmc_dev(host->cur_slot->mmc),
+		dma_sync_sg_for_cpu(mmc_dev(host->slot->mmc),
 				    data->sg,
 				    data->sg_len,
 				    DMA_FROM_DEVICE);
@@ -820,7 +820,7 @@ static int dw_mci_edmac_start_dma(struct dw_mci *host,
 
 	/* Flush cache before write */
 	if (host->data->flags & MMC_DATA_WRITE)
-		dma_sync_sg_for_device(mmc_dev(host->cur_slot->mmc), sgl,
+		dma_sync_sg_for_device(mmc_dev(host->slot->mmc), sgl,
 				       sg_elems, DMA_TO_DEVICE);
 
 	dma_async_issue_pending(host->dms->ch);
@@ -1282,7 +1282,6 @@ static void __dw_mci_start_request(struct dw_mci *host,
 
 	mrq = slot->mrq;
 
-	host->cur_slot = slot;
 	host->mrq = mrq;
 
 	host->pending_events = 0;
@@ -1621,16 +1620,10 @@ static void dw_mci_init_card(struct mmc_host *mmc, struct mmc_card *card)
 
 		if (card->type == MMC_TYPE_SDIO ||
 		    card->type == MMC_TYPE_SD_COMBO) {
-			if (!test_bit(DW_MMC_CARD_NO_LOW_PWR, &slot->flags)) {
-				pm_runtime_get_noresume(mmc->parent);
-				set_bit(DW_MMC_CARD_NO_LOW_PWR, &slot->flags);
-			}
+			set_bit(DW_MMC_CARD_NO_LOW_PWR, &slot->flags);
 			clk_en_a = clk_en_a_old & ~clken_low_pwr;
 		} else {
-			if (test_bit(DW_MMC_CARD_NO_LOW_PWR, &slot->flags)) {
-				pm_runtime_put_noidle(mmc->parent);
-				clear_bit(DW_MMC_CARD_NO_LOW_PWR, &slot->flags);
-			}
+			clear_bit(DW_MMC_CARD_NO_LOW_PWR, &slot->flags);
 			clk_en_a = clk_en_a_old | clken_low_pwr;
 		}
 
@@ -1642,9 +1635,8 @@ static void dw_mci_init_card(struct mmc_host *mmc, struct mmc_card *card)
 	}
 }
 
-static void dw_mci_enable_sdio_irq(struct mmc_host *mmc, int enb)
+static void __dw_mci_enable_sdio_irq(struct dw_mci_slot *slot, int enb)
 {
-	struct dw_mci_slot *slot = mmc_priv(mmc);
 	struct dw_mci *host = slot->host;
 	unsigned long irqflags;
 	u32 int_mask;
@@ -1662,6 +1654,27 @@ static void dw_mci_enable_sdio_irq(struct mmc_host *mmc, int enb)
 	spin_unlock_irqrestore(&host->irq_lock, irqflags);
 }
 
+static void dw_mci_enable_sdio_irq(struct mmc_host *mmc, int enb)
+{
+	struct dw_mci_slot *slot = mmc_priv(mmc);
+	struct dw_mci *host = slot->host;
+
+	__dw_mci_enable_sdio_irq(slot, enb);
+
+	/* Avoid runtime suspending the device when SDIO IRQ is enabled */
+	if (enb)
+		pm_runtime_get_noresume(host->dev);
+	else
+		pm_runtime_put_noidle(host->dev);
+}
+
+static void dw_mci_ack_sdio_irq(struct mmc_host *mmc)
+{
+	struct dw_mci_slot *slot = mmc_priv(mmc);
+
+	__dw_mci_enable_sdio_irq(slot, 1);
+}
+
 static int dw_mci_execute_tuning(struct mmc_host *mmc, u32 opcode)
 {
 	struct dw_mci_slot *slot = mmc_priv(mmc);
@@ -1749,7 +1762,7 @@ static bool dw_mci_reset(struct dw_mci *host)
 
 ciu_out:
 	/* After a CTRL reset we need to have CIU set clock registers  */
-	mci_send_cmd(host->cur_slot, SDMMC_CMD_UPD_CLK, 0);
+	mci_send_cmd(host->slot, SDMMC_CMD_UPD_CLK, 0);
 
 	return ret;
 }
@@ -1763,6 +1776,7 @@ static const struct mmc_host_ops dw_mci_ops = {
 	.get_cd			= dw_mci_get_cd,
 	.hw_reset               = dw_mci_hw_reset,
 	.enable_sdio_irq	= dw_mci_enable_sdio_irq,
+	.ack_sdio_irq		= dw_mci_ack_sdio_irq,
 	.execute_tuning		= dw_mci_execute_tuning,
 	.card_busy		= dw_mci_card_busy,
 	.start_signal_voltage_switch = dw_mci_switch_voltage,
@@ -1775,11 +1789,11 @@ static void dw_mci_request_end(struct dw_mci *host, struct mmc_request *mrq)
 	__acquires(&host->lock)
 {
 	struct dw_mci_slot *slot;
-	struct mmc_host	*prev_mmc = host->cur_slot->mmc;
+	struct mmc_host	*prev_mmc = host->slot->mmc;
 
 	WARN_ON(host->cmd || host->data);
 
-	host->cur_slot->mrq = NULL;
+	host->slot->mrq = NULL;
 	host->mrq = NULL;
 	if (!list_empty(&host->queue)) {
 		slot = list_entry(host->queue.next,
@@ -1929,7 +1943,7 @@ static void dw_mci_tasklet_func(unsigned long priv)
 			err = dw_mci_command_complete(host, cmd);
 			if (cmd == mrq->sbc && !err) {
 				prev_state = state = STATE_SENDING_CMD;
-				__dw_mci_start_request(host, host->cur_slot,
+				__dw_mci_start_request(host, host->slot,
 						       mrq->cmd);
 				goto unlock;
 			}
@@ -2548,26 +2562,19 @@ static void dw_mci_cmd_interrupt(struct dw_mci *host, u32 status)
 
 static void dw_mci_handle_cd(struct dw_mci *host)
 {
-	int i;
+	struct dw_mci_slot *slot = host->slot;
 
-	for (i = 0; i < host->num_slots; i++) {
-		struct dw_mci_slot *slot = host->slot[i];
-
-		if (!slot)
-			continue;
-
-		if (slot->mmc->ops->card_event)
-			slot->mmc->ops->card_event(slot->mmc);
-		mmc_detect_change(slot->mmc,
-			msecs_to_jiffies(host->pdata->detect_delay_ms));
-	}
+	if (slot->mmc->ops->card_event)
+		slot->mmc->ops->card_event(slot->mmc);
+	mmc_detect_change(slot->mmc,
+		msecs_to_jiffies(host->pdata->detect_delay_ms));
 }
 
 static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 {
 	struct dw_mci *host = dev_id;
 	u32 pending;
-	int i;
+	struct dw_mci_slot *slot = host->slot;
 
 	pending = mci_readl(host, MINTSTS); /* read-only mask reg */
 
@@ -2644,18 +2651,11 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 			dw_mci_handle_cd(host);
 		}
 
-		/* Handle SDIO Interrupts */
-		for (i = 0; i < host->num_slots; i++) {
-			struct dw_mci_slot *slot = host->slot[i];
-
-			if (!slot)
-				continue;
-
-			if (pending & SDMMC_INT_SDIO(slot->sdio_id)) {
-				mci_writel(host, RINTSTS,
-					   SDMMC_INT_SDIO(slot->sdio_id));
-				mmc_signal_sdio_irq(slot->mmc);
-			}
+		if (pending & SDMMC_INT_SDIO(slot->sdio_id)) {
+			mci_writel(host, RINTSTS,
+				   SDMMC_INT_SDIO(slot->sdio_id));
+			__dw_mci_enable_sdio_irq(slot, 0);
+			sdio_signal_irq(slot->mmc);
 		}
 
 	}
@@ -2687,7 +2687,7 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
+static int dw_mci_init_slot(struct dw_mci *host)
 {
 	struct mmc_host *mmc;
 	struct dw_mci_slot *slot;
@@ -2700,15 +2700,15 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 		return -ENOMEM;
 
 	slot = mmc_priv(mmc);
-	slot->id = id;
-	slot->sdio_id = host->sdio_id0 + id;
+	slot->id = 0;
+	slot->sdio_id = host->sdio_id0 + slot->id;
 	slot->mmc = mmc;
 	slot->host = host;
-	host->slot[id] = slot;
+	host->slot = slot;
 
 	mmc->ops = &dw_mci_ops;
-	if (of_property_read_u32_array(host->dev->of_node,
-				       "clock-freq-min-max", freq, 2)) {
+	if (device_property_read_u32_array(host->dev, "clock-freq-min-max",
+					   freq, 2)) {
 		mmc->f_min = DW_MCI_FREQ_MIN;
 		mmc->f_max = DW_MCI_FREQ_MAX;
 	} else {
@@ -2755,6 +2755,10 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	if (ret)
 		goto err_host_allocated;
 
+	/* Process SDIO IRQs through the sdio_irq_work. */
+	if (mmc->caps & MMC_CAP_SDIO_IRQ)
+		mmc->caps2 |= MMC_CAP2_SDIO_IRQ_NOTHREAD;
+
 	/* Useful defaults if platform data is unset. */
 	if (host->use_dma == TRANS_MODE_IDMAC) {
 		mmc->max_segs = host->ring_size;
@@ -2796,11 +2800,11 @@ err_host_allocated:
 	return ret;
 }
 
-static void dw_mci_cleanup_slot(struct dw_mci_slot *slot, unsigned int id)
+static void dw_mci_cleanup_slot(struct dw_mci_slot *slot)
 {
 	/* Debugfs stuff is cleaned up by mmc core */
 	mmc_remove_host(slot->mmc);
-	slot->host->slot[id] = NULL;
+	slot->host->slot = NULL;
 	mmc_free_host(slot->mmc);
 }
 
@@ -2808,7 +2812,6 @@ static void dw_mci_init_dma(struct dw_mci *host)
 {
 	int addr_config;
 	struct device *dev = host->dev;
-	struct device_node *np = dev->of_node;
 
 	/*
 	* Check tansfer mode from HCON[17:16]
@@ -2869,8 +2872,9 @@ static void dw_mci_init_dma(struct dw_mci *host)
 		dev_info(host->dev, "Using internal DMA controller.\n");
 	} else {
 		/* TRANS_MODE_EDMAC: check dma bindings again */
-		if ((of_property_count_strings(np, "dma-names") < 0) ||
-		    (!of_find_property(np, "dmas", NULL))) {
+		if ((device_property_read_string_array(dev, "dma-names",
+						       NULL, 0) < 0) ||
+		    !device_property_present(dev, "dmas")) {
 			goto no_dma;
 		}
 		host->dma_ops = &dw_mci_edmac_ops;
@@ -2937,7 +2941,6 @@ static struct dw_mci_board *dw_mci_parse_dt(struct dw_mci *host)
 {
 	struct dw_mci_board *pdata;
 	struct device *dev = host->dev;
-	struct device_node *np = dev->of_node;
 	const struct dw_mci_drv_data *drv_data = host->drv_data;
 	int ret;
 	u32 clock_frequency;
@@ -2954,20 +2957,22 @@ static struct dw_mci_board *dw_mci_parse_dt(struct dw_mci *host)
 	}
 
 	/* find out number of slots supported */
-	of_property_read_u32(np, "num-slots", &pdata->num_slots);
+	if (device_property_read_u32(dev, "num-slots", &pdata->num_slots))
+		dev_info(dev, "'num-slots' was deprecated.\n");
 
-	if (of_property_read_u32(np, "fifo-depth", &pdata->fifo_depth))
+	if (device_property_read_u32(dev, "fifo-depth", &pdata->fifo_depth))
 		dev_info(dev,
 			 "fifo-depth property not found, using value of FIFOTH register as default\n");
 
-	of_property_read_u32(np, "card-detect-delay", &pdata->detect_delay_ms);
+	device_property_read_u32(dev, "card-detect-delay",
+				 &pdata->detect_delay_ms);
 
-	of_property_read_u32(np, "data-addr", &host->data_addr_override);
+	device_property_read_u32(dev, "data-addr", &host->data_addr_override);
 
-	if (of_get_property(np, "fifo-watermark-aligned", NULL))
+	if (device_property_present(dev, "fifo-watermark-aligned"))
 		host->wm_aligned = true;
 
-	if (!of_property_read_u32(np, "clock-frequency", &clock_frequency))
+	if (!device_property_read_u32(dev, "clock-frequency", &clock_frequency))
 		pdata->bus_hz = clock_frequency;
 
 	if (drv_data && drv_data->parse_dt) {
@@ -2990,29 +2995,21 @@ static void dw_mci_enable_cd(struct dw_mci *host)
 {
 	unsigned long irqflags;
 	u32 temp;
-	int i;
-	struct dw_mci_slot *slot;
 
 	/*
 	 * No need for CD if all slots have a non-error GPIO
 	 * as well as broken card detection is found.
 	 */
-	for (i = 0; i < host->num_slots; i++) {
-		slot = host->slot[i];
-		if (slot->mmc->caps & MMC_CAP_NEEDS_POLL)
-			return;
-
-		if (mmc_gpio_get_cd(slot->mmc) < 0)
-			break;
-	}
-	if (i == host->num_slots)
+	if (host->slot->mmc->caps & MMC_CAP_NEEDS_POLL)
 		return;
 
-	spin_lock_irqsave(&host->irq_lock, irqflags);
-	temp = mci_readl(host, INTMASK);
-	temp  |= SDMMC_INT_CD;
-	mci_writel(host, INTMASK, temp);
-	spin_unlock_irqrestore(&host->irq_lock, irqflags);
+	if (mmc_gpio_get_cd(host->slot->mmc) < 0) {
+		spin_lock_irqsave(&host->irq_lock, irqflags);
+		temp = mci_readl(host, INTMASK);
+		temp  |= SDMMC_INT_CD;
+		mci_writel(host, INTMASK, temp);
+		spin_unlock_irqrestore(&host->irq_lock, irqflags);
+	}
 }
 
 int dw_mci_probe(struct dw_mci *host)
@@ -3020,7 +3017,6 @@ int dw_mci_probe(struct dw_mci *host)
 	const struct dw_mci_drv_data *drv_data = host->drv_data;
 	int width, i, ret = 0;
 	u32 fifo_size;
-	int init_slots = 0;
 
 	if (!host->pdata) {
 		host->pdata = dw_mci_parse_dt(host);
@@ -3183,19 +3179,6 @@ int dw_mci_probe(struct dw_mci *host)
 	if (ret)
 		goto err_dmaunmap;
 
-	if (host->pdata->num_slots)
-		host->num_slots = host->pdata->num_slots;
-	else
-		host->num_slots = 1;
-
-	if (host->num_slots < 1 ||
-	    host->num_slots > SDMMC_GET_SLOT_NUM(mci_readl(host, HCON))) {
-		dev_err(host->dev,
-			"Platform data must supply correct num_slots.\n");
-		ret = -ENODEV;
-		goto err_clk_ciu;
-	}
-
 	/*
 	 * Enable interrupts for command done, data over, data empty,
 	 * receive ready and error such as transmit, receive timeout, crc error
@@ -3211,20 +3194,9 @@ int dw_mci_probe(struct dw_mci *host)
 		 host->irq, width, fifo_size);
 
 	/* We need at least one slot to succeed */
-	for (i = 0; i < host->num_slots; i++) {
-		ret = dw_mci_init_slot(host, i);
-		if (ret)
-			dev_dbg(host->dev, "slot %d init failed\n", i);
-		else
-			init_slots++;
-	}
-
-	if (init_slots) {
-		dev_info(host->dev, "%d slots initialized\n", init_slots);
-	} else {
-		dev_dbg(host->dev,
-			"attempted to initialize %d slots, but failed on all\n",
-			host->num_slots);
+	ret = dw_mci_init_slot(host);
+	if (ret) {
+		dev_dbg(host->dev, "slot %d init failed\n", i);
 		goto err_dmaunmap;
 	}
 
@@ -3252,13 +3224,9 @@ EXPORT_SYMBOL(dw_mci_probe);
 
 void dw_mci_remove(struct dw_mci *host)
 {
-	int i;
-
-	for (i = 0; i < host->num_slots; i++) {
-		dev_dbg(host->dev, "remove slot %d\n", i);
-		if (host->slot[i])
-			dw_mci_cleanup_slot(host->slot[i], i);
-	}
+	dev_dbg(host->dev, "remove slot\n");
+	if (host->slot)
+		dw_mci_cleanup_slot(host->slot);
 
 	mci_writel(host, RINTSTS, 0xFFFFFFFF);
 	mci_writel(host, INTMASK, 0); /* disable all mmc interrupt first */
@@ -3290,9 +3258,9 @@ int dw_mci_runtime_suspend(struct device *dev)
 
 	clk_disable_unprepare(host->ciu_clk);
 
-	if (host->cur_slot &&
-	    (mmc_can_gpio_cd(host->cur_slot->mmc) ||
-	     !mmc_card_is_removable(host->cur_slot->mmc)))
+	if (host->slot &&
+	    (mmc_can_gpio_cd(host->slot->mmc) ||
+	     !mmc_card_is_removable(host->slot->mmc)))
 		clk_disable_unprepare(host->biu_clk);
 
 	return 0;
@@ -3301,12 +3269,12 @@ EXPORT_SYMBOL(dw_mci_runtime_suspend);
 
 int dw_mci_runtime_resume(struct device *dev)
 {
-	int i, ret = 0;
+	int ret = 0;
 	struct dw_mci *host = dev_get_drvdata(dev);
 
-	if (host->cur_slot &&
-	    (mmc_can_gpio_cd(host->cur_slot->mmc) ||
-	     !mmc_card_is_removable(host->cur_slot->mmc))) {
+	if (host->slot &&
+	    (mmc_can_gpio_cd(host->slot->mmc) ||
+	     !mmc_card_is_removable(host->slot->mmc))) {
 		ret = clk_prepare_enable(host->biu_clk);
 		if (ret)
 			return ret;
@@ -3341,17 +3309,12 @@ int dw_mci_runtime_resume(struct device *dev)
 		   DW_MCI_ERROR_FLAGS);
 	mci_writel(host, CTRL, SDMMC_CTRL_INT_ENABLE);
 
-	for (i = 0; i < host->num_slots; i++) {
-		struct dw_mci_slot *slot = host->slot[i];
 
-		if (!slot)
-			continue;
-		if (slot->mmc->pm_flags & MMC_PM_KEEP_POWER)
-			dw_mci_set_ios(slot->mmc, &slot->mmc->ios);
+	if (host->slot->mmc->pm_flags & MMC_PM_KEEP_POWER)
+		dw_mci_set_ios(host->slot->mmc, &host->slot->mmc->ios);
 
-		/* Force setup bus to guarantee available clock output */
-		dw_mci_setup_bus(slot, true);
-	}
+	/* Force setup bus to guarantee available clock output */
+	dw_mci_setup_bus(host->slot, true);
 
 	/* Now that slots are all setup, we can enable card detect */
 	dw_mci_enable_cd(host);
@@ -3359,9 +3322,9 @@ int dw_mci_runtime_resume(struct device *dev)
 	return 0;
 
 err:
-	if (host->cur_slot &&
-	    (mmc_can_gpio_cd(host->cur_slot->mmc) ||
-	     !mmc_card_is_removable(host->cur_slot->mmc)))
+	if (host->slot &&
+	    (mmc_can_gpio_cd(host->slot->mmc) ||
+	     !mmc_card_is_removable(host->slot->mmc)))
 		clk_disable_unprepare(host->biu_clk);
 
 	return ret;
diff --git a/drivers/mmc/host/dw_mmc.h b/drivers/mmc/host/dw_mmc.h
index ce347361f3dc..75da3756955d 100644
--- a/drivers/mmc/host/dw_mmc.h
+++ b/drivers/mmc/host/dw_mmc.h
@@ -20,8 +20,6 @@
 #include <linux/reset.h>
 #include <linux/interrupt.h>
 
-#define MAX_MCI_SLOTS	2
-
 enum dw_mci_state {
 	STATE_IDLE = 0,
 	STATE_SENDING_CMD,
@@ -134,7 +132,6 @@ struct dw_mci_dma_slave {
  * =======
  *
  * @lock is a softirq-safe spinlock protecting @queue as well as
- * @cur_slot, @mrq and @state. These must always be updated
  * at the same time while holding @lock.
  *
  * @irq_lock is an irq-safe spinlock protecting the INTMASK register
@@ -170,7 +167,6 @@ struct dw_mci {
 	struct scatterlist	*sg;
 	struct sg_mapping_iter	sg_miter;
 
-	struct dw_mci_slot	*cur_slot;
 	struct mmc_request	*mrq;
 	struct mmc_command	*cmd;
 	struct mmc_data		*data;
@@ -206,7 +202,6 @@ struct dw_mci {
 
 	u32			bus_hz;
 	u32			current_speed;
-	u32			num_slots;
 	u32			fifoth_val;
 	u16			verid;
 	struct device		*dev;
@@ -215,7 +210,7 @@ struct dw_mci {
 	void			*priv;
 	struct clk		*biu_clk;
 	struct clk		*ciu_clk;
-	struct dw_mci_slot	*slot[MAX_MCI_SLOTS];
+	struct dw_mci_slot	*slot;
 
 	/* FIFO push and pull */
 	int			fifo_depth;
diff --git a/drivers/mmc/host/jz4740_mmc.c b/drivers/mmc/host/jz4740_mmc.c
index 57e254aac48d..7db8c7a8d38d 100644
--- a/drivers/mmc/host/jz4740_mmc.c
+++ b/drivers/mmc/host/jz4740_mmc.c
@@ -20,6 +20,7 @@
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/module.h>
+#include <linux/pinctrl/consumer.h>
 #include <linux/platform_device.h>
 #include <linux/delay.h>
 #include <linux/scatterlist.h>
@@ -27,7 +28,6 @@
 
 #include <linux/bitops.h>
 #include <linux/gpio.h>
-#include <asm/mach-jz4740/gpio.h>
 #include <asm/cacheflush.h>
 #include <linux/dma-mapping.h>
 #include <linux/dmaengine.h>
@@ -901,15 +901,6 @@ static const struct mmc_host_ops jz4740_mmc_ops = {
 	.enable_sdio_irq = jz4740_mmc_enable_sdio_irq,
 };
 
-static const struct jz_gpio_bulk_request jz4740_mmc_pins[] = {
-	JZ_GPIO_BULK_PIN(MSC_CMD),
-	JZ_GPIO_BULK_PIN(MSC_CLK),
-	JZ_GPIO_BULK_PIN(MSC_DATA0),
-	JZ_GPIO_BULK_PIN(MSC_DATA1),
-	JZ_GPIO_BULK_PIN(MSC_DATA2),
-	JZ_GPIO_BULK_PIN(MSC_DATA3),
-};
-
 static int jz4740_mmc_request_gpio(struct device *dev, int gpio,
 	const char *name, bool output, int value)
 {
@@ -973,15 +964,6 @@ static void jz4740_mmc_free_gpios(struct platform_device *pdev)
 		gpio_free(pdata->gpio_power);
 }
 
-static inline size_t jz4740_mmc_num_pins(struct jz4740_mmc_host *host)
-{
-	size_t num_pins = ARRAY_SIZE(jz4740_mmc_pins);
-	if (host->pdata && host->pdata->data_1bit)
-		num_pins -= 3;
-
-	return num_pins;
-}
-
 static int jz4740_mmc_probe(struct platform_device* pdev)
 {
 	int ret;
@@ -1022,15 +1004,9 @@ static int jz4740_mmc_probe(struct platform_device* pdev)
 		goto err_free_host;
 	}
 
-	ret = jz_gpio_bulk_request(jz4740_mmc_pins, jz4740_mmc_num_pins(host));
-	if (ret) {
-		dev_err(&pdev->dev, "Failed to request mmc pins: %d\n", ret);
-		goto err_free_host;
-	}
-
 	ret = jz4740_mmc_request_gpios(mmc, pdev);
 	if (ret)
-		goto err_gpio_bulk_free;
+		goto err_release_dma;
 
 	mmc->ops = &jz4740_mmc_ops;
 	mmc->f_min = JZ_MMC_CLK_RATE / 128;
@@ -1086,10 +1062,9 @@ err_free_irq:
 	free_irq(host->irq, host);
 err_free_gpios:
 	jz4740_mmc_free_gpios(pdev);
-err_gpio_bulk_free:
+err_release_dma:
 	if (host->use_dma)
 		jz4740_mmc_release_dma_channels(host);
-	jz_gpio_bulk_free(jz4740_mmc_pins, jz4740_mmc_num_pins(host));
 err_free_host:
 	mmc_free_host(mmc);
 
@@ -1109,7 +1084,6 @@ static int jz4740_mmc_remove(struct platform_device *pdev)
 	free_irq(host->irq, host);
 
 	jz4740_mmc_free_gpios(pdev);
-	jz_gpio_bulk_free(jz4740_mmc_pins, jz4740_mmc_num_pins(host));
 
 	if (host->use_dma)
 		jz4740_mmc_release_dma_channels(host);
@@ -1123,20 +1097,12 @@ static int jz4740_mmc_remove(struct platform_device *pdev)
 
 static int jz4740_mmc_suspend(struct device *dev)
 {
-	struct jz4740_mmc_host *host = dev_get_drvdata(dev);
-
-	jz_gpio_bulk_suspend(jz4740_mmc_pins, jz4740_mmc_num_pins(host));
-
-	return 0;
+	return pinctrl_pm_select_sleep_state(dev);
 }
 
 static int jz4740_mmc_resume(struct device *dev)
 {
-	struct jz4740_mmc_host *host = dev_get_drvdata(dev);
-
-	jz_gpio_bulk_resume(jz4740_mmc_pins, jz4740_mmc_num_pins(host));
-
-	return 0;
+	return pinctrl_pm_select_default_state(dev);
 }
 
 static SIMPLE_DEV_PM_OPS(jz4740_mmc_pm_ops, jz4740_mmc_suspend,
diff --git a/drivers/mmc/host/meson-gx-mmc.c b/drivers/mmc/host/meson-gx-mmc.c
index 1842ed341af1..de962c2d5e00 100644
--- a/drivers/mmc/host/meson-gx-mmc.c
+++ b/drivers/mmc/host/meson-gx-mmc.c
@@ -210,6 +210,15 @@ static void meson_mmc_get_transfer_mode(struct mmc_host *mmc,
 	int i;
 	bool use_desc_chain_mode = true;
 
+	/*
+	 * Broken SDIO with AP6255-based WiFi on Khadas VIM Pro has been
+	 * reported. For some strange reason this occurs in descriptor
+	 * chain mode only. So let's fall back to bounce buffer mode
+	 * for command SD_IO_RW_EXTENDED.
+	 */
+	if (mrq->cmd->opcode == SD_IO_RW_EXTENDED)
+		return;
+
 	for_each_sg(data->sg, sg, data->sg_len, i)
 		/* check for 8 byte alignment */
 		if (sg->offset & 7) {
diff --git a/drivers/mmc/host/mtk-sd.c b/drivers/mmc/host/mtk-sd.c
index 5c1e178fc5f9..5a672a5218ad 100644
--- a/drivers/mmc/host/mtk-sd.c
+++ b/drivers/mmc/host/mtk-sd.c
@@ -1774,7 +1774,7 @@ static int msdc_drv_remove(struct platform_device *pdev)
 	pm_runtime_disable(host->dev);
 	pm_runtime_put_noidle(host->dev);
 	dma_free_coherent(&pdev->dev,
-			sizeof(struct mt_gpdma_desc),
+			2 * sizeof(struct mt_gpdma_desc),
 			host->dma.gpd, host->dma.gpd_addr);
 	dma_free_coherent(&pdev->dev, MAX_BD_NUM * sizeof(struct mt_bdma_desc),
 			host->dma.bd, host->dma.bd_addr);
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index 8c39dccacf39..7c12f3715676 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -250,14 +250,14 @@ static int omap_hsmmc_enable_supply(struct mmc_host *mmc)
 	struct omap_hsmmc_host *host = mmc_priv(mmc);
 	struct mmc_ios *ios = &mmc->ios;
 
-	if (mmc->supply.vmmc) {
+	if (!IS_ERR(mmc->supply.vmmc)) {
 		ret = mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, ios->vdd);
 		if (ret)
 			return ret;
 	}
 
 	/* Enable interface voltage rail, if needed */
-	if (mmc->supply.vqmmc && !host->vqmmc_enabled) {
+	if (!IS_ERR(mmc->supply.vqmmc) && !host->vqmmc_enabled) {
 		ret = regulator_enable(mmc->supply.vqmmc);
 		if (ret) {
 			dev_err(mmc_dev(mmc), "vmmc_aux reg enable failed\n");
@@ -269,7 +269,7 @@ static int omap_hsmmc_enable_supply(struct mmc_host *mmc)
 	return 0;
 
 err_vqmmc:
-	if (mmc->supply.vmmc)
+	if (!IS_ERR(mmc->supply.vmmc))
 		mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, 0);
 
 	return ret;
@@ -281,7 +281,7 @@ static int omap_hsmmc_disable_supply(struct mmc_host *mmc)
 	int status;
 	struct omap_hsmmc_host *host = mmc_priv(mmc);
 
-	if (mmc->supply.vqmmc && host->vqmmc_enabled) {
+	if (!IS_ERR(mmc->supply.vqmmc) && host->vqmmc_enabled) {
 		ret = regulator_disable(mmc->supply.vqmmc);
 		if (ret) {
 			dev_err(mmc_dev(mmc), "vmmc_aux reg disable failed\n");
@@ -290,7 +290,7 @@ static int omap_hsmmc_disable_supply(struct mmc_host *mmc)
 		host->vqmmc_enabled = 0;
 	}
 
-	if (mmc->supply.vmmc) {
+	if (!IS_ERR(mmc->supply.vmmc)) {
 		ret = mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, 0);
 		if (ret)
 			goto err_set_ocr;
@@ -299,7 +299,7 @@ static int omap_hsmmc_disable_supply(struct mmc_host *mmc)
 	return 0;
 
 err_set_ocr:
-	if (mmc->supply.vqmmc) {
+	if (!IS_ERR(mmc->supply.vqmmc)) {
 		status = regulator_enable(mmc->supply.vqmmc);
 		if (status)
 			dev_err(mmc_dev(mmc), "vmmc_aux re-enable failed\n");
@@ -313,7 +313,7 @@ static int omap_hsmmc_set_pbias(struct omap_hsmmc_host *host, bool power_on,
 {
 	int ret;
 
-	if (!host->pbias)
+	if (IS_ERR(host->pbias))
 		return 0;
 
 	if (power_on) {
@@ -363,7 +363,7 @@ static int omap_hsmmc_set_power(struct omap_hsmmc_host *host, int power_on,
 	 * If we don't see a Vcc regulator, assume it's a fixed
 	 * voltage always-on regulator.
 	 */
-	if (!mmc->supply.vmmc)
+	if (IS_ERR(mmc->supply.vmmc))
 		return 0;
 
 	if (mmc_pdata(host)->before_set_reg)
@@ -415,7 +415,7 @@ static int omap_hsmmc_disable_boot_regulator(struct regulator *reg)
 {
 	int ret;
 
-	if (!reg)
+	if (IS_ERR(reg))
 		return 0;
 
 	if (regulator_is_enabled(reg)) {
@@ -466,36 +466,27 @@ static int omap_hsmmc_disable_boot_regulators(struct omap_hsmmc_host *host)
 
 static int omap_hsmmc_reg_get(struct omap_hsmmc_host *host)
 {
-	int ocr_value = 0;
 	int ret;
 	struct mmc_host *mmc = host->mmc;
 
 	if (mmc_pdata(host)->set_power)
 		return 0;
 
-	mmc->supply.vmmc = devm_regulator_get_optional(host->dev, "vmmc");
-	if (IS_ERR(mmc->supply.vmmc)) {
-		ret = PTR_ERR(mmc->supply.vmmc);
-		if ((ret != -ENODEV) && host->dev->of_node)
-			return ret;
-		dev_dbg(host->dev, "unable to get vmmc regulator %ld\n",
-			PTR_ERR(mmc->supply.vmmc));
-		mmc->supply.vmmc = NULL;
-	} else {
-		ocr_value = mmc_regulator_get_ocrmask(mmc->supply.vmmc);
-		if (ocr_value > 0)
-			mmc_pdata(host)->ocr_mask = ocr_value;
-	}
+	ret = mmc_regulator_get_supply(mmc);
+	if (ret == -EPROBE_DEFER)
+		return ret;
 
 	/* Allow an aux regulator */
-	mmc->supply.vqmmc = devm_regulator_get_optional(host->dev, "vmmc_aux");
 	if (IS_ERR(mmc->supply.vqmmc)) {
-		ret = PTR_ERR(mmc->supply.vqmmc);
-		if ((ret != -ENODEV) && host->dev->of_node)
-			return ret;
-		dev_dbg(host->dev, "unable to get vmmc_aux regulator %ld\n",
-			PTR_ERR(mmc->supply.vqmmc));
-		mmc->supply.vqmmc = NULL;
+		mmc->supply.vqmmc = devm_regulator_get_optional(host->dev,
+								"vmmc_aux");
+		if (IS_ERR(mmc->supply.vqmmc)) {
+			ret = PTR_ERR(mmc->supply.vqmmc);
+			if ((ret != -ENODEV) && host->dev->of_node)
+				return ret;
+			dev_dbg(host->dev, "unable to get vmmc_aux regulator %ld\n",
+				PTR_ERR(mmc->supply.vqmmc));
+		}
 	}
 
 	host->pbias = devm_regulator_get_optional(host->dev, "pbias");
@@ -508,7 +499,6 @@ static int omap_hsmmc_reg_get(struct omap_hsmmc_host *host)
 		}
 		dev_dbg(host->dev, "unable to get pbias regulator %ld\n",
 			PTR_ERR(host->pbias));
-		host->pbias = NULL;
 	}
 
 	/* For eMMC do not power off when not in sleep state */
@@ -2146,7 +2136,8 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 	if (ret)
 		goto err_irq;
 
-	mmc->ocr_avail = mmc_pdata(host)->ocr_mask;
+	if (!mmc->ocr_avail)
+		mmc->ocr_avail = mmc_pdata(host)->ocr_mask;
 
 	omap_hsmmc_disable_irq(host);
 
diff --git a/drivers/mmc/host/pxamci.c b/drivers/mmc/host/pxamci.c
index c763b404510f..59ab194cb009 100644
--- a/drivers/mmc/host/pxamci.c
+++ b/drivers/mmc/host/pxamci.c
@@ -702,7 +702,11 @@ static int pxamci_probe(struct platform_device *pdev)
 
 	pxamci_init_ocr(host);
 
-	mmc->caps = 0;
+	/*
+	 * This architecture used to disable bounce buffers through its
+	 * defconfig, now it is done at runtime as a host property.
+	 */
+	mmc->caps = MMC_CAP_NO_BOUNCE_BUFF;
 	host->cmdat = 0;
 	if (!cpu_is_pxa25x()) {
 		mmc->caps |= MMC_CAP_4_BIT_DATA | MMC_CAP_SDIO_IRQ;
diff --git a/drivers/mmc/host/renesas_sdhi.h b/drivers/mmc/host/renesas_sdhi.h
new file mode 100644
index 000000000000..ca83acc113b8
--- /dev/null
+++ b/drivers/mmc/host/renesas_sdhi.h
@@ -0,0 +1,39 @@
+/*
+ * Renesas Mobile SDHI
+ *
+ * Copyright (C) 2017 Horms Solutions Ltd., Simon Horman
+ * Copyright (C) 2017 Renesas Electronics Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef RENESAS_SDHI_H
+#define RENESAS_SDHI_H
+
+#include <linux/platform_device.h>
+#include "tmio_mmc.h"
+
+struct renesas_sdhi_scc {
+	unsigned long clk_rate;	/* clock rate for SDR104 */
+	u32 tap;		/* sampling clock position for SDR104 */
+};
+
+struct renesas_sdhi_of_data {
+	unsigned long tmio_flags;
+	u32	      tmio_ocr_mask;
+	unsigned long capabilities;
+	unsigned long capabilities2;
+	enum dma_slave_buswidth dma_buswidth;
+	dma_addr_t dma_rx_offset;
+	unsigned int bus_shift;
+	int scc_offset;
+	struct renesas_sdhi_scc *taps;
+	int taps_num;
+};
+
+int renesas_sdhi_probe(struct platform_device *pdev,
+		       const struct tmio_mmc_dma_ops *dma_ops);
+int renesas_sdhi_remove(struct platform_device *pdev);
+#endif
diff --git a/drivers/mmc/host/sh_mobile_sdhi.c b/drivers/mmc/host/renesas_sdhi_core.c
index bc6be0dbea39..a4fb07d0ea91 100644
--- a/drivers/mmc/host/sh_mobile_sdhi.c
+++ b/drivers/mmc/host/renesas_sdhi_core.c
@@ -1,8 +1,9 @@
 /*
- * SuperH Mobile SDHI
+ * Renesas SDHI
  *
- * Copyright (C) 2016 Sang Engineering, Wolfram Sang
- * Copyright (C) 2015-16 Renesas Electronics Corporation
+ * Copyright (C) 2015-17 Renesas Electronics Corporation
+ * Copyright (C) 2016-17 Sang Engineering, Wolfram Sang
+ * Copyright (C) 2016-17 Horms Solutions, Simon Horman
  * Copyright (C) 2009 Magnus Damm
  *
  * This program is free software; you can redistribute it and/or modify
@@ -23,8 +24,6 @@
 #include <linux/kernel.h>
 #include <linux/clk.h>
 #include <linux/slab.h>
-#include <linux/mod_devicetable.h>
-#include <linux/module.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/mmc/host.h>
@@ -35,6 +34,7 @@
 #include <linux/pinctrl/pinctrl-state.h>
 #include <linux/regulator/consumer.h>
 
+#include "renesas_sdhi.h"
 #include "tmio_mmc.h"
 
 #define EXT_ACC           0xe4
@@ -45,103 +45,10 @@
 #define SDHI_VER_GEN3_SD	0xcc10
 #define SDHI_VER_GEN3_SDMMC	0xcd10
 
-#define host_to_priv(host) container_of((host)->pdata, struct sh_mobile_sdhi, mmc_data)
+#define host_to_priv(host) \
+	container_of((host)->pdata, struct renesas_sdhi, mmc_data)
 
-struct sh_mobile_sdhi_scc {
-	unsigned long clk_rate;	/* clock rate for SDR104 */
-	u32 tap;		/* sampling clock position for SDR104 */
-};
-
-struct sh_mobile_sdhi_of_data {
-	unsigned long tmio_flags;
-	u32	      tmio_ocr_mask;
-	unsigned long capabilities;
-	unsigned long capabilities2;
-	enum dma_slave_buswidth dma_buswidth;
-	dma_addr_t dma_rx_offset;
-	unsigned bus_shift;
-	int scc_offset;
-	struct sh_mobile_sdhi_scc *taps;
-	int taps_num;
-};
-
-static const struct sh_mobile_sdhi_of_data of_default_cfg = {
-	.tmio_flags = TMIO_MMC_HAS_IDLE_WAIT,
-};
-
-static const struct sh_mobile_sdhi_of_data of_rz_compatible = {
-	.tmio_flags	= TMIO_MMC_HAS_IDLE_WAIT | TMIO_MMC_32BIT_DATA_PORT,
-	.tmio_ocr_mask	= MMC_VDD_32_33,
-	.capabilities	= MMC_CAP_SD_HIGHSPEED | MMC_CAP_SDIO_IRQ,
-};
-
-static const struct sh_mobile_sdhi_of_data of_rcar_gen1_compatible = {
-	.tmio_flags	= TMIO_MMC_HAS_IDLE_WAIT | TMIO_MMC_WRPROTECT_DISABLE |
-			  TMIO_MMC_CLK_ACTUAL,
-	.capabilities	= MMC_CAP_SD_HIGHSPEED | MMC_CAP_SDIO_IRQ,
-};
-
-/* Definitions for sampling clocks */
-static struct sh_mobile_sdhi_scc rcar_gen2_scc_taps[] = {
-	{
-		.clk_rate = 156000000,
-		.tap = 0x00000703,
-	},
-	{
-		.clk_rate = 0,
-		.tap = 0x00000300,
-	},
-};
-
-static const struct sh_mobile_sdhi_of_data of_rcar_gen2_compatible = {
-	.tmio_flags	= TMIO_MMC_HAS_IDLE_WAIT | TMIO_MMC_WRPROTECT_DISABLE |
-			  TMIO_MMC_CLK_ACTUAL | TMIO_MMC_MIN_RCAR2,
-	.capabilities	= MMC_CAP_SD_HIGHSPEED | MMC_CAP_SDIO_IRQ,
-	.dma_buswidth	= DMA_SLAVE_BUSWIDTH_4_BYTES,
-	.dma_rx_offset	= 0x2000,
-	.scc_offset	= 0x0300,
-	.taps		= rcar_gen2_scc_taps,
-	.taps_num	= ARRAY_SIZE(rcar_gen2_scc_taps),
-};
-
-/* Definitions for sampling clocks */
-static struct sh_mobile_sdhi_scc rcar_gen3_scc_taps[] = {
-	{
-		.clk_rate = 0,
-		.tap = 0x00000300,
-	},
-};
-
-static const struct sh_mobile_sdhi_of_data of_rcar_gen3_compatible = {
-	.tmio_flags	= TMIO_MMC_HAS_IDLE_WAIT | TMIO_MMC_WRPROTECT_DISABLE |
-			  TMIO_MMC_CLK_ACTUAL | TMIO_MMC_MIN_RCAR2,
-	.capabilities	= MMC_CAP_SD_HIGHSPEED | MMC_CAP_SDIO_IRQ,
-	.bus_shift	= 2,
-	.scc_offset	= 0x1000,
-	.taps		= rcar_gen3_scc_taps,
-	.taps_num	= ARRAY_SIZE(rcar_gen3_scc_taps),
-};
-
-static const struct of_device_id sh_mobile_sdhi_of_match[] = {
-	{ .compatible = "renesas,sdhi-shmobile" },
-	{ .compatible = "renesas,sdhi-sh73a0", .data = &of_default_cfg, },
-	{ .compatible = "renesas,sdhi-r8a73a4", .data = &of_default_cfg, },
-	{ .compatible = "renesas,sdhi-r8a7740", .data = &of_default_cfg, },
-	{ .compatible = "renesas,sdhi-r7s72100", .data = &of_rz_compatible, },
-	{ .compatible = "renesas,sdhi-r8a7778", .data = &of_rcar_gen1_compatible, },
-	{ .compatible = "renesas,sdhi-r8a7779", .data = &of_rcar_gen1_compatible, },
-	{ .compatible = "renesas,sdhi-r8a7790", .data = &of_rcar_gen2_compatible, },
-	{ .compatible = "renesas,sdhi-r8a7791", .data = &of_rcar_gen2_compatible, },
-	{ .compatible = "renesas,sdhi-r8a7792", .data = &of_rcar_gen2_compatible, },
-	{ .compatible = "renesas,sdhi-r8a7793", .data = &of_rcar_gen2_compatible, },
-	{ .compatible = "renesas,sdhi-r8a7794", .data = &of_rcar_gen2_compatible, },
-	{ .compatible = "renesas,sdhi-r8a7795", .data = &of_rcar_gen3_compatible, },
-	{ .compatible = "renesas,sdhi-r8a7796", .data = &of_rcar_gen3_compatible, },
-	{},
-};
-MODULE_DEVICE_TABLE(of, sh_mobile_sdhi_of_match);
-
-struct sh_mobile_sdhi {
+struct renesas_sdhi {
 	struct clk *clk;
 	struct clk *clk_cd;
 	struct tmio_mmc_data mmc_data;
@@ -151,13 +58,13 @@ struct sh_mobile_sdhi {
 	void __iomem *scc_ctl;
 };
 
-static void sh_mobile_sdhi_sdbuf_width(struct tmio_mmc_host *host, int width)
+static void renesas_sdhi_sdbuf_width(struct tmio_mmc_host *host, int width)
 {
 	u32 val;
 
 	/*
 	 * see also
-	 *	sh_mobile_sdhi_of_data :: dma_buswidth
+	 *	renesas_sdhi_of_data :: dma_buswidth
 	 */
 	switch (sd_ctrl_read16(host, CTL_VERSION)) {
 	case SDHI_VER_GEN2_SDR50:
@@ -183,11 +90,12 @@ static void sh_mobile_sdhi_sdbuf_width(struct tmio_mmc_host *host, int width)
 	sd_ctrl_write16(host, EXT_ACC, val);
 }
 
-static int sh_mobile_sdhi_clk_enable(struct tmio_mmc_host *host)
+static int renesas_sdhi_clk_enable(struct tmio_mmc_host *host)
 {
 	struct mmc_host *mmc = host->mmc;
-	struct sh_mobile_sdhi *priv = host_to_priv(host);
+	struct renesas_sdhi *priv = host_to_priv(host);
 	int ret = clk_prepare_enable(priv->clk);
+
 	if (ret < 0)
 		return ret;
 
@@ -213,19 +121,19 @@ static int sh_mobile_sdhi_clk_enable(struct tmio_mmc_host *host)
 	mmc->f_min = max(clk_round_rate(priv->clk, 1) / 512, 1L);
 
 	/* enable 16bit data access on SDBUF as default */
-	sh_mobile_sdhi_sdbuf_width(host, 16);
+	renesas_sdhi_sdbuf_width(host, 16);
 
 	return 0;
 }
 
-static unsigned int sh_mobile_sdhi_clk_update(struct tmio_mmc_host *host,
-					      unsigned int new_clock)
+static unsigned int renesas_sdhi_clk_update(struct tmio_mmc_host *host,
+					    unsigned int new_clock)
 {
-	struct sh_mobile_sdhi *priv = host_to_priv(host);
+	struct renesas_sdhi *priv = host_to_priv(host);
 	unsigned int freq, diff, best_freq = 0, diff_min = ~0;
 	int i, ret;
 
-	/* tested only on RCar Gen2+ currently; may work for others */
+	/* tested only on R-Car Gen2+ currently; may work for others */
 	if (!(host->pdata->flags & TMIO_MMC_MIN_RCAR2))
 		return clk_get_rate(priv->clk);
 
@@ -257,26 +165,27 @@ static unsigned int sh_mobile_sdhi_clk_update(struct tmio_mmc_host *host,
 	return ret == 0 ? best_freq : clk_get_rate(priv->clk);
 }
 
-static void sh_mobile_sdhi_clk_disable(struct tmio_mmc_host *host)
+static void renesas_sdhi_clk_disable(struct tmio_mmc_host *host)
 {
-	struct sh_mobile_sdhi *priv = host_to_priv(host);
+	struct renesas_sdhi *priv = host_to_priv(host);
 
 	clk_disable_unprepare(priv->clk);
 	clk_disable_unprepare(priv->clk_cd);
 }
 
-static int sh_mobile_sdhi_card_busy(struct mmc_host *mmc)
+static int renesas_sdhi_card_busy(struct mmc_host *mmc)
 {
 	struct tmio_mmc_host *host = mmc_priv(mmc);
 
-	return !(sd_ctrl_read16_and_16_as_32(host, CTL_STATUS) & TMIO_STAT_DAT0);
+	return !(sd_ctrl_read16_and_16_as_32(host, CTL_STATUS) &
+		 TMIO_STAT_DAT0);
 }
 
-static int sh_mobile_sdhi_start_signal_voltage_switch(struct mmc_host *mmc,
-						      struct mmc_ios *ios)
+static int renesas_sdhi_start_signal_voltage_switch(struct mmc_host *mmc,
+						    struct mmc_ios *ios)
 {
 	struct tmio_mmc_host *host = mmc_priv(mmc);
-	struct sh_mobile_sdhi *priv = host_to_priv(host);
+	struct renesas_sdhi *priv = host_to_priv(host);
 	struct pinctrl_state *pin_state;
 	int ret;
 
@@ -327,21 +236,21 @@ static int sh_mobile_sdhi_start_signal_voltage_switch(struct mmc_host *mmc,
 #define SH_MOBILE_SDHI_SCC_RVSREQ_RVSERR	BIT(2)
 
 static inline u32 sd_scc_read32(struct tmio_mmc_host *host,
-				struct sh_mobile_sdhi *priv, int addr)
+				struct renesas_sdhi *priv, int addr)
 {
 	return readl(priv->scc_ctl + (addr << host->bus_shift));
 }
 
 static inline void sd_scc_write32(struct tmio_mmc_host *host,
-				  struct sh_mobile_sdhi *priv,
+				  struct renesas_sdhi *priv,
 				  int addr, u32 val)
 {
 	writel(val, priv->scc_ctl + (addr << host->bus_shift));
 }
 
-static unsigned int sh_mobile_sdhi_init_tuning(struct tmio_mmc_host *host)
+static unsigned int renesas_sdhi_init_tuning(struct tmio_mmc_host *host)
 {
-	struct sh_mobile_sdhi *priv;
+	struct renesas_sdhi *priv;
 
 	priv = host_to_priv(host);
 
@@ -378,10 +287,10 @@ static unsigned int sh_mobile_sdhi_init_tuning(struct tmio_mmc_host *host)
 		SH_MOBILE_SDHI_SCC_DTCNTL_TAPNUM_MASK;
 }
 
-static void sh_mobile_sdhi_prepare_tuning(struct tmio_mmc_host *host,
-					 unsigned long tap)
+static void renesas_sdhi_prepare_tuning(struct tmio_mmc_host *host,
+					unsigned long tap)
 {
-	struct sh_mobile_sdhi *priv = host_to_priv(host);
+	struct renesas_sdhi *priv = host_to_priv(host);
 
 	/* Set sampling clock position */
 	sd_scc_write32(host, priv, SH_MOBILE_SDHI_SCC_TAPSET, tap);
@@ -389,9 +298,9 @@ static void sh_mobile_sdhi_prepare_tuning(struct tmio_mmc_host *host,
 
 #define SH_MOBILE_SDHI_MAX_TAP 3
 
-static int sh_mobile_sdhi_select_tuning(struct tmio_mmc_host *host)
+static int renesas_sdhi_select_tuning(struct tmio_mmc_host *host)
 {
-	struct sh_mobile_sdhi *priv = host_to_priv(host);
+	struct renesas_sdhi *priv = host_to_priv(host);
 	unsigned long tap_cnt;  /* counter of tuning success */
 	unsigned long tap_set;  /* tap position */
 	unsigned long tap_start;/* start position of tuning success */
@@ -412,9 +321,9 @@ static int sh_mobile_sdhi_select_tuning(struct tmio_mmc_host *host)
 	tap_start = 0;
 	tap_end = 0;
 	for (i = 0; i < host->tap_num * 2; i++) {
-		if (test_bit(i, host->taps))
+		if (test_bit(i, host->taps)) {
 			ntap++;
-		else {
+		} else {
 			if (ntap > tap_cnt) {
 				tap_start = i - ntap;
 				tap_end = i - 1;
@@ -446,10 +355,9 @@ static int sh_mobile_sdhi_select_tuning(struct tmio_mmc_host *host)
 	return 0;
 }
 
-
-static bool sh_mobile_sdhi_check_scc_error(struct tmio_mmc_host *host)
+static bool renesas_sdhi_check_scc_error(struct tmio_mmc_host *host)
 {
-	struct sh_mobile_sdhi *priv = host_to_priv(host);
+	struct renesas_sdhi *priv = host_to_priv(host);
 
 	/* Check SCC error */
 	if (sd_scc_read32(host, priv, SH_MOBILE_SDHI_SCC_RVSCNTL) &
@@ -464,9 +372,9 @@ static bool sh_mobile_sdhi_check_scc_error(struct tmio_mmc_host *host)
 	return false;
 }
 
-static void sh_mobile_sdhi_hw_reset(struct tmio_mmc_host *host)
+static void renesas_sdhi_hw_reset(struct tmio_mmc_host *host)
 {
-	struct sh_mobile_sdhi *priv;
+	struct renesas_sdhi *priv;
 
 	priv = host_to_priv(host);
 
@@ -490,7 +398,7 @@ static void sh_mobile_sdhi_hw_reset(struct tmio_mmc_host *host)
 		       sd_scc_read32(host, priv, SH_MOBILE_SDHI_SCC_RVSCNTL));
 }
 
-static int sh_mobile_sdhi_wait_idle(struct tmio_mmc_host *host)
+static int renesas_sdhi_wait_idle(struct tmio_mmc_host *host)
 {
 	int timeout = 1000;
 
@@ -506,10 +414,9 @@ static int sh_mobile_sdhi_wait_idle(struct tmio_mmc_host *host)
 	return 0;
 }
 
-static int sh_mobile_sdhi_write16_hook(struct tmio_mmc_host *host, int addr)
+static int renesas_sdhi_write16_hook(struct tmio_mmc_host *host, int addr)
 {
-	switch (addr)
-	{
+	switch (addr) {
 	case CTL_SD_CMD:
 	case CTL_STOP_INTERNAL_ACTION:
 	case CTL_XFER_BLK_COUNT:
@@ -519,14 +426,14 @@ static int sh_mobile_sdhi_write16_hook(struct tmio_mmc_host *host, int addr)
 	case CTL_TRANSACTION_CTL:
 	case CTL_DMA_ENABLE:
 	case EXT_ACC:
-		return sh_mobile_sdhi_wait_idle(host);
+		return renesas_sdhi_wait_idle(host);
 	}
 
 	return 0;
 }
 
-static int sh_mobile_sdhi_multi_io_quirk(struct mmc_card *card,
-					 unsigned int direction, int blk_size)
+static int renesas_sdhi_multi_io_quirk(struct mmc_card *card,
+				       unsigned int direction, int blk_size)
 {
 	/*
 	 * In Renesas controllers, when performing a
@@ -543,30 +450,34 @@ static int sh_mobile_sdhi_multi_io_quirk(struct mmc_card *card,
 	return blk_size;
 }
 
-static void sh_mobile_sdhi_enable_dma(struct tmio_mmc_host *host, bool enable)
+static void renesas_sdhi_enable_dma(struct tmio_mmc_host *host, bool enable)
 {
 	sd_ctrl_write16(host, CTL_DMA_ENABLE, enable ? 2 : 0);
 
 	/* enable 32bit access if DMA mode if possibile */
-	sh_mobile_sdhi_sdbuf_width(host, enable ? 32 : 16);
+	renesas_sdhi_sdbuf_width(host, enable ? 32 : 16);
 }
 
-static int sh_mobile_sdhi_probe(struct platform_device *pdev)
+int renesas_sdhi_probe(struct platform_device *pdev,
+		       const struct tmio_mmc_dma_ops *dma_ops)
 {
-	const struct sh_mobile_sdhi_of_data *of_data = of_device_get_match_data(&pdev->dev);
-	struct sh_mobile_sdhi *priv;
-	struct tmio_mmc_data *mmc_data;
 	struct tmio_mmc_data *mmd = pdev->dev.platform_data;
+	const struct renesas_sdhi_of_data *of_data;
+	struct tmio_mmc_data *mmc_data;
+	struct tmio_mmc_dma *dma_priv;
 	struct tmio_mmc_host *host;
+	struct renesas_sdhi *priv;
 	struct resource *res;
 	int irq, ret, i;
-	struct tmio_mmc_dma *dma_priv;
+
+	of_data = of_device_get_match_data(&pdev->dev);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res)
 		return -EINVAL;
 
-	priv = devm_kzalloc(&pdev->dev, sizeof(struct sh_mobile_sdhi), GFP_KERNEL);
+	priv = devm_kzalloc(&pdev->dev, sizeof(struct renesas_sdhi),
+			    GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
@@ -609,7 +520,6 @@ static int sh_mobile_sdhi_probe(struct platform_device *pdev)
 		goto eprobe;
 	}
 
-
 	if (of_data) {
 		mmc_data->flags |= of_data->tmio_flags;
 		mmc_data->ocr_mask = of_data->tmio_ocr_mask;
@@ -621,18 +531,18 @@ static int sh_mobile_sdhi_probe(struct platform_device *pdev)
 	}
 
 	host->dma		= dma_priv;
-	host->write16_hook	= sh_mobile_sdhi_write16_hook;
-	host->clk_enable	= sh_mobile_sdhi_clk_enable;
-	host->clk_update	= sh_mobile_sdhi_clk_update;
-	host->clk_disable	= sh_mobile_sdhi_clk_disable;
-	host->multi_io_quirk	= sh_mobile_sdhi_multi_io_quirk;
+	host->write16_hook	= renesas_sdhi_write16_hook;
+	host->clk_enable	= renesas_sdhi_clk_enable;
+	host->clk_update	= renesas_sdhi_clk_update;
+	host->clk_disable	= renesas_sdhi_clk_disable;
+	host->multi_io_quirk	= renesas_sdhi_multi_io_quirk;
 
 	/* SDR speeds are only available on Gen2+ */
 	if (mmc_data->flags & TMIO_MMC_MIN_RCAR2) {
 		/* card_busy caused issues on r8a73a4 (pre-Gen2) CD-less SDHI */
-		host->card_busy	= sh_mobile_sdhi_card_busy;
+		host->card_busy	= renesas_sdhi_card_busy;
 		host->start_signal_voltage_switch =
-			sh_mobile_sdhi_start_signal_voltage_switch;
+			renesas_sdhi_start_signal_voltage_switch;
 	}
 
 	/* Orginally registers were 16 bit apart, could be 32 or 64 nowadays */
@@ -643,7 +553,7 @@ static int sh_mobile_sdhi_probe(struct platform_device *pdev)
 		*mmc_data = *mmd;
 
 	dma_priv->filter = shdma_chan_filter;
-	dma_priv->enable = sh_mobile_sdhi_enable_dma;
+	dma_priv->enable = renesas_sdhi_enable_dma;
 
 	mmc_data->alignment_shift = 1; /* 2-byte alignment */
 	mmc_data->capabilities |= MMC_CAP_MMC_HIGHSPEED;
@@ -659,15 +569,13 @@ static int sh_mobile_sdhi_probe(struct platform_device *pdev)
 	 */
 	mmc_data->flags |= TMIO_MMC_SDIO_IRQ;
 
-	/*
-	 * All SDHI have CMD12 controll bit
-	 */
+	/* All SDHI have CMD12 control bit */
 	mmc_data->flags |= TMIO_MMC_HAVE_CMD12_CTRL;
 
 	/* All SDHI have SDIO status bits which must be 1 */
 	mmc_data->flags |= TMIO_MMC_SDIO_STATUS_SETBITS;
 
-	ret = tmio_mmc_host_probe(host, mmc_data);
+	ret = tmio_mmc_host_probe(host, mmc_data, dma_ops);
 	if (ret < 0)
 		goto efree;
 
@@ -675,7 +583,7 @@ static int sh_mobile_sdhi_probe(struct platform_device *pdev)
 	if (of_data && of_data->scc_offset &&
 	    (host->mmc->caps & MMC_CAP_UHS_SDR104 ||
 	     host->mmc->caps2 & MMC_CAP2_HS200_1_8V_SDR)) {
-		const struct sh_mobile_sdhi_scc *taps = of_data->taps;
+		const struct renesas_sdhi_scc *taps = of_data->taps;
 		bool hit = false;
 
 		host->mmc->caps |= MMC_CAP_HW_RESET;
@@ -693,11 +601,11 @@ static int sh_mobile_sdhi_probe(struct platform_device *pdev)
 			dev_warn(&host->pdev->dev, "Unknown clock rate for SDR104\n");
 
 		priv->scc_ctl = host->ctl + of_data->scc_offset;
-		host->init_tuning = sh_mobile_sdhi_init_tuning;
-		host->prepare_tuning = sh_mobile_sdhi_prepare_tuning;
-		host->select_tuning = sh_mobile_sdhi_select_tuning;
-		host->check_scc_error = sh_mobile_sdhi_check_scc_error;
-		host->hw_reset = sh_mobile_sdhi_hw_reset;
+		host->init_tuning = renesas_sdhi_init_tuning;
+		host->prepare_tuning = renesas_sdhi_prepare_tuning;
+		host->select_tuning = renesas_sdhi_select_tuning;
+		host->check_scc_error = renesas_sdhi_check_scc_error;
+		host->hw_reset = renesas_sdhi_hw_reset;
 	}
 
 	i = 0;
@@ -707,7 +615,7 @@ static int sh_mobile_sdhi_probe(struct platform_device *pdev)
 			break;
 		i++;
 		ret = devm_request_irq(&pdev->dev, irq, tmio_mmc_irq, 0,
-				  dev_name(&pdev->dev), host);
+				       dev_name(&pdev->dev), host);
 		if (ret)
 			goto eirq;
 	}
@@ -732,8 +640,9 @@ efree:
 eprobe:
 	return ret;
 }
+EXPORT_SYMBOL_GPL(renesas_sdhi_probe);
 
-static int sh_mobile_sdhi_remove(struct platform_device *pdev)
+int renesas_sdhi_remove(struct platform_device *pdev)
 {
 	struct mmc_host *mmc = platform_get_drvdata(pdev);
 	struct tmio_mmc_host *host = mmc_priv(mmc);
@@ -742,28 +651,4 @@ static int sh_mobile_sdhi_remove(struct platform_device *pdev)
 
 	return 0;
 }
-
-static const struct dev_pm_ops tmio_mmc_dev_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
-			pm_runtime_force_resume)
-	SET_RUNTIME_PM_OPS(tmio_mmc_host_runtime_suspend,
-			tmio_mmc_host_runtime_resume,
-			NULL)
-};
-
-static struct platform_driver sh_mobile_sdhi_driver = {
-	.driver		= {
-		.name	= "sh_mobile_sdhi",
-		.pm	= &tmio_mmc_dev_pm_ops,
-		.of_match_table = sh_mobile_sdhi_of_match,
-	},
-	.probe		= sh_mobile_sdhi_probe,
-	.remove		= sh_mobile_sdhi_remove,
-};
-
-module_platform_driver(sh_mobile_sdhi_driver);
-
-MODULE_DESCRIPTION("SuperH Mobile SDHI driver");
-MODULE_AUTHOR("Magnus Damm");
-MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("platform:sh_mobile_sdhi");
+EXPORT_SYMBOL_GPL(renesas_sdhi_remove);
diff --git a/drivers/mmc/host/tmio_mmc_dma.c b/drivers/mmc/host/renesas_sdhi_sys_dmac.c
index e2093db2b7ff..642a0dcc8c5c 100644
--- a/drivers/mmc/host/tmio_mmc_dma.c
+++ b/drivers/mmc/host/renesas_sdhi_sys_dmac.c
@@ -1,13 +1,14 @@
 /*
- * linux/drivers/mmc/tmio_mmc_dma.c
+ * DMA function for TMIO MMC implementations
  *
+ * Copyright (C) 2016-17 Renesas Electronics Corporation
+ * Copyright (C) 2016-17 Sang Engineering, Wolfram Sang
+ * Copyright (C) 2017 Horms Solutions, Simon Horman
  * Copyright (C) 2010-2011 Guennadi Liakhovetski
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * DMA function for TMIO MMC implementations
  */
 
 #include <linux/device.h>
@@ -15,14 +16,96 @@
 #include <linux/dmaengine.h>
 #include <linux/mfd/tmio.h>
 #include <linux/mmc/host.h>
+#include <linux/mod_devicetable.h>
+#include <linux/module.h>
 #include <linux/pagemap.h>
 #include <linux/scatterlist.h>
 
+#include "renesas_sdhi.h"
 #include "tmio_mmc.h"
 
 #define TMIO_MMC_MIN_DMA_LEN 8
 
-void tmio_mmc_enable_dma(struct tmio_mmc_host *host, bool enable)
+static const struct renesas_sdhi_of_data of_default_cfg = {
+	.tmio_flags = TMIO_MMC_HAS_IDLE_WAIT,
+};
+
+static const struct renesas_sdhi_of_data of_rz_compatible = {
+	.tmio_flags	= TMIO_MMC_HAS_IDLE_WAIT | TMIO_MMC_32BIT_DATA_PORT,
+	.tmio_ocr_mask	= MMC_VDD_32_33,
+	.capabilities	= MMC_CAP_SD_HIGHSPEED | MMC_CAP_SDIO_IRQ,
+};
+
+static const struct renesas_sdhi_of_data of_rcar_gen1_compatible = {
+	.tmio_flags	= TMIO_MMC_HAS_IDLE_WAIT | TMIO_MMC_WRPROTECT_DISABLE |
+			  TMIO_MMC_CLK_ACTUAL,
+	.capabilities	= MMC_CAP_SD_HIGHSPEED | MMC_CAP_SDIO_IRQ,
+};
+
+/* Definitions for sampling clocks */
+static struct renesas_sdhi_scc rcar_gen2_scc_taps[] = {
+	{
+		.clk_rate = 156000000,
+		.tap = 0x00000703,
+	},
+	{
+		.clk_rate = 0,
+		.tap = 0x00000300,
+	},
+};
+
+static const struct renesas_sdhi_of_data of_rcar_gen2_compatible = {
+	.tmio_flags	= TMIO_MMC_HAS_IDLE_WAIT | TMIO_MMC_WRPROTECT_DISABLE |
+			  TMIO_MMC_CLK_ACTUAL | TMIO_MMC_MIN_RCAR2,
+	.capabilities	= MMC_CAP_SD_HIGHSPEED | MMC_CAP_SDIO_IRQ |
+			  MMC_CAP_CMD23,
+	.dma_buswidth	= DMA_SLAVE_BUSWIDTH_4_BYTES,
+	.dma_rx_offset	= 0x2000,
+	.scc_offset	= 0x0300,
+	.taps		= rcar_gen2_scc_taps,
+	.taps_num	= ARRAY_SIZE(rcar_gen2_scc_taps),
+};
+
+/* Definitions for sampling clocks */
+static struct renesas_sdhi_scc rcar_gen3_scc_taps[] = {
+	{
+		.clk_rate = 0,
+		.tap = 0x00000300,
+	},
+};
+
+static const struct renesas_sdhi_of_data of_rcar_gen3_compatible = {
+	.tmio_flags	= TMIO_MMC_HAS_IDLE_WAIT | TMIO_MMC_WRPROTECT_DISABLE |
+			  TMIO_MMC_CLK_ACTUAL | TMIO_MMC_MIN_RCAR2,
+	.capabilities	= MMC_CAP_SD_HIGHSPEED | MMC_CAP_SDIO_IRQ |
+			  MMC_CAP_CMD23,
+	.bus_shift	= 2,
+	.scc_offset	= 0x1000,
+	.taps		= rcar_gen3_scc_taps,
+	.taps_num	= ARRAY_SIZE(rcar_gen3_scc_taps),
+};
+
+static const struct of_device_id renesas_sdhi_sys_dmac_of_match[] = {
+	{ .compatible = "renesas,sdhi-shmobile" },
+	{ .compatible = "renesas,sdhi-sh73a0", .data = &of_default_cfg, },
+	{ .compatible = "renesas,sdhi-r8a73a4", .data = &of_default_cfg, },
+	{ .compatible = "renesas,sdhi-r8a7740", .data = &of_default_cfg, },
+	{ .compatible = "renesas,sdhi-r7s72100", .data = &of_rz_compatible, },
+	{ .compatible = "renesas,sdhi-r8a7778", .data = &of_rcar_gen1_compatible, },
+	{ .compatible = "renesas,sdhi-r8a7779", .data = &of_rcar_gen1_compatible, },
+	{ .compatible = "renesas,sdhi-r8a7790", .data = &of_rcar_gen2_compatible, },
+	{ .compatible = "renesas,sdhi-r8a7791", .data = &of_rcar_gen2_compatible, },
+	{ .compatible = "renesas,sdhi-r8a7792", .data = &of_rcar_gen2_compatible, },
+	{ .compatible = "renesas,sdhi-r8a7793", .data = &of_rcar_gen2_compatible, },
+	{ .compatible = "renesas,sdhi-r8a7794", .data = &of_rcar_gen2_compatible, },
+	{ .compatible = "renesas,sdhi-r8a7795", .data = &of_rcar_gen3_compatible, },
+	{ .compatible = "renesas,sdhi-r8a7796", .data = &of_rcar_gen3_compatible, },
+	{},
+};
+MODULE_DEVICE_TABLE(of, renesas_sdhi_sys_dmac_of_match);
+
+static void renesas_sdhi_sys_dmac_enable_dma(struct tmio_mmc_host *host,
+					     bool enable)
 {
 	if (!host->chan_tx || !host->chan_rx)
 		return;
@@ -31,19 +114,19 @@ void tmio_mmc_enable_dma(struct tmio_mmc_host *host, bool enable)
 		host->dma->enable(host, enable);
 }
 
-void tmio_mmc_abort_dma(struct tmio_mmc_host *host)
+static void renesas_sdhi_sys_dmac_abort_dma(struct tmio_mmc_host *host)
 {
-	tmio_mmc_enable_dma(host, false);
+	renesas_sdhi_sys_dmac_enable_dma(host, false);
 
 	if (host->chan_rx)
 		dmaengine_terminate_all(host->chan_rx);
 	if (host->chan_tx)
 		dmaengine_terminate_all(host->chan_tx);
 
-	tmio_mmc_enable_dma(host, true);
+	renesas_sdhi_sys_dmac_enable_dma(host, true);
 }
 
-static void tmio_mmc_dma_callback(void *arg)
+static void renesas_sdhi_sys_dmac_dma_callback(void *arg)
 {
 	struct tmio_mmc_host *host = arg;
 
@@ -71,7 +154,7 @@ out:
 	spin_unlock_irq(&host->lock);
 }
 
-static void tmio_mmc_start_dma_rx(struct tmio_mmc_host *host)
+static void renesas_sdhi_sys_dmac_start_dma_rx(struct tmio_mmc_host *host)
 {
 	struct scatterlist *sg = host->sg_ptr, *sg_tmp;
 	struct dma_async_tx_descriptor *desc = NULL;
@@ -112,12 +195,12 @@ static void tmio_mmc_start_dma_rx(struct tmio_mmc_host *host)
 
 	ret = dma_map_sg(chan->device->dev, sg, host->sg_len, DMA_FROM_DEVICE);
 	if (ret > 0)
-		desc = dmaengine_prep_slave_sg(chan, sg, ret,
-			DMA_DEV_TO_MEM, DMA_CTRL_ACK);
+		desc = dmaengine_prep_slave_sg(chan, sg, ret, DMA_DEV_TO_MEM,
+					       DMA_CTRL_ACK);
 
 	if (desc) {
 		reinit_completion(&host->dma_dataend);
-		desc->callback = tmio_mmc_dma_callback;
+		desc->callback = renesas_sdhi_sys_dmac_dma_callback;
 		desc->callback_param = host;
 
 		cookie = dmaengine_submit(desc);
@@ -129,7 +212,7 @@ static void tmio_mmc_start_dma_rx(struct tmio_mmc_host *host)
 pio:
 	if (!desc) {
 		/* DMA failed, fall back to PIO */
-		tmio_mmc_enable_dma(host, false);
+		renesas_sdhi_sys_dmac_enable_dma(host, false);
 		if (ret >= 0)
 			ret = -EIO;
 		host->chan_rx = NULL;
@@ -145,7 +228,7 @@ pio:
 	}
 }
 
-static void tmio_mmc_start_dma_tx(struct tmio_mmc_host *host)
+static void renesas_sdhi_sys_dmac_start_dma_tx(struct tmio_mmc_host *host)
 {
 	struct scatterlist *sg = host->sg_ptr, *sg_tmp;
 	struct dma_async_tx_descriptor *desc = NULL;
@@ -181,6 +264,7 @@ static void tmio_mmc_start_dma_tx(struct tmio_mmc_host *host)
 	if (!aligned) {
 		unsigned long flags;
 		void *sg_vaddr = tmio_mmc_kmap_atomic(sg, &flags);
+
 		sg_init_one(&host->bounce_sg, host->bounce_buf, sg->length);
 		memcpy(host->bounce_buf, sg_vaddr, host->bounce_sg.length);
 		tmio_mmc_kunmap_atomic(sg, &flags, sg_vaddr);
@@ -190,12 +274,12 @@ static void tmio_mmc_start_dma_tx(struct tmio_mmc_host *host)
 
 	ret = dma_map_sg(chan->device->dev, sg, host->sg_len, DMA_TO_DEVICE);
 	if (ret > 0)
-		desc = dmaengine_prep_slave_sg(chan, sg, ret,
-			DMA_MEM_TO_DEV, DMA_CTRL_ACK);
+		desc = dmaengine_prep_slave_sg(chan, sg, ret, DMA_MEM_TO_DEV,
+					       DMA_CTRL_ACK);
 
 	if (desc) {
 		reinit_completion(&host->dma_dataend);
-		desc->callback = tmio_mmc_dma_callback;
+		desc->callback = renesas_sdhi_sys_dmac_dma_callback;
 		desc->callback_param = host;
 
 		cookie = dmaengine_submit(desc);
@@ -207,7 +291,7 @@ static void tmio_mmc_start_dma_tx(struct tmio_mmc_host *host)
 pio:
 	if (!desc) {
 		/* DMA failed, fall back to PIO */
-		tmio_mmc_enable_dma(host, false);
+		renesas_sdhi_sys_dmac_enable_dma(host, false);
 		if (ret >= 0)
 			ret = -EIO;
 		host->chan_tx = NULL;
@@ -223,19 +307,19 @@ pio:
 	}
 }
 
-void tmio_mmc_start_dma(struct tmio_mmc_host *host,
-			       struct mmc_data *data)
+static void renesas_sdhi_sys_dmac_start_dma(struct tmio_mmc_host *host,
+					    struct mmc_data *data)
 {
 	if (data->flags & MMC_DATA_READ) {
 		if (host->chan_rx)
-			tmio_mmc_start_dma_rx(host);
+			renesas_sdhi_sys_dmac_start_dma_rx(host);
 	} else {
 		if (host->chan_tx)
-			tmio_mmc_start_dma_tx(host);
+			renesas_sdhi_sys_dmac_start_dma_tx(host);
 	}
 }
 
-static void tmio_mmc_issue_tasklet_fn(unsigned long priv)
+static void renesas_sdhi_sys_dmac_issue_tasklet_fn(unsigned long priv)
 {
 	struct tmio_mmc_host *host = (struct tmio_mmc_host *)priv;
 	struct dma_chan *chan = NULL;
@@ -257,11 +341,12 @@ static void tmio_mmc_issue_tasklet_fn(unsigned long priv)
 		dma_async_issue_pending(chan);
 }
 
-void tmio_mmc_request_dma(struct tmio_mmc_host *host, struct tmio_mmc_data *pdata)
+static void renesas_sdhi_sys_dmac_request_dma(struct tmio_mmc_host *host,
+					      struct tmio_mmc_data *pdata)
 {
 	/* We can only either use DMA for both Tx and Rx or not use it at all */
 	if (!host->dma || (!host->pdev->dev.of_node &&
-		(!pdata->chan_priv_tx || !pdata->chan_priv_rx)))
+			   (!pdata->chan_priv_tx || !pdata->chan_priv_rx)))
 		return;
 
 	if (!host->chan_tx && !host->chan_rx) {
@@ -287,7 +372,8 @@ void tmio_mmc_request_dma(struct tmio_mmc_host *host, struct tmio_mmc_data *pdat
 			return;
 
 		cfg.direction = DMA_MEM_TO_DEV;
-		cfg.dst_addr = res->start + (CTL_SD_DATA_PORT << host->bus_shift);
+		cfg.dst_addr = res->start +
+			(CTL_SD_DATA_PORT << host->bus_shift);
 		cfg.dst_addr_width = host->dma->dma_buswidth;
 		if (!cfg.dst_addr_width)
 			cfg.dst_addr_width = DMA_SLAVE_BUSWIDTH_2_BYTES;
@@ -320,10 +406,12 @@ void tmio_mmc_request_dma(struct tmio_mmc_host *host, struct tmio_mmc_data *pdat
 			goto ebouncebuf;
 
 		init_completion(&host->dma_dataend);
-		tasklet_init(&host->dma_issue, tmio_mmc_issue_tasklet_fn, (unsigned long)host);
+		tasklet_init(&host->dma_issue,
+			     renesas_sdhi_sys_dmac_issue_tasklet_fn,
+			     (unsigned long)host);
 	}
 
-	tmio_mmc_enable_dma(host, true);
+	renesas_sdhi_sys_dmac_enable_dma(host, true);
 
 	return;
 
@@ -337,15 +425,17 @@ ecfgtx:
 	host->chan_tx = NULL;
 }
 
-void tmio_mmc_release_dma(struct tmio_mmc_host *host)
+static void renesas_sdhi_sys_dmac_release_dma(struct tmio_mmc_host *host)
 {
 	if (host->chan_tx) {
 		struct dma_chan *chan = host->chan_tx;
+
 		host->chan_tx = NULL;
 		dma_release_channel(chan);
 	}
 	if (host->chan_rx) {
 		struct dma_chan *chan = host->chan_rx;
+
 		host->chan_rx = NULL;
 		dma_release_channel(chan);
 	}
@@ -354,3 +444,41 @@ void tmio_mmc_release_dma(struct tmio_mmc_host *host)
 		host->bounce_buf = NULL;
 	}
 }
+
+static const struct tmio_mmc_dma_ops renesas_sdhi_sys_dmac_dma_ops = {
+	.start = renesas_sdhi_sys_dmac_start_dma,
+	.enable = renesas_sdhi_sys_dmac_enable_dma,
+	.request = renesas_sdhi_sys_dmac_request_dma,
+	.release = renesas_sdhi_sys_dmac_release_dma,
+	.abort = renesas_sdhi_sys_dmac_abort_dma,
+};
+
+static int renesas_sdhi_sys_dmac_probe(struct platform_device *pdev)
+{
+	return renesas_sdhi_probe(pdev, &renesas_sdhi_sys_dmac_dma_ops);
+}
+
+static const struct dev_pm_ops renesas_sdhi_sys_dmac_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(tmio_mmc_host_runtime_suspend,
+			   tmio_mmc_host_runtime_resume,
+			   NULL)
+};
+
+static struct platform_driver renesas_sys_dmac_sdhi_driver = {
+	.driver		= {
+		.name	= "sh_mobile_sdhi",
+		.pm	= &renesas_sdhi_sys_dmac_dev_pm_ops,
+		.of_match_table = renesas_sdhi_sys_dmac_of_match,
+	},
+	.probe		= renesas_sdhi_sys_dmac_probe,
+	.remove		= renesas_sdhi_remove,
+};
+
+module_platform_driver(renesas_sys_dmac_sdhi_driver);
+
+MODULE_DESCRIPTION("Renesas SDHI driver");
+MODULE_AUTHOR("Magnus Damm");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:sh_mobile_sdhi");
diff --git a/drivers/mmc/host/sdhci-acpi.c b/drivers/mmc/host/sdhci-acpi.c
index c6a9a1bfaa22..cf66a3db71b8 100644
--- a/drivers/mmc/host/sdhci-acpi.c
+++ b/drivers/mmc/host/sdhci-acpi.c
@@ -274,7 +274,6 @@ static const struct sdhci_acpi_slot sdhci_acpi_slot_int_emmc = {
 	.caps    = MMC_CAP_8_BIT_DATA | MMC_CAP_NONREMOVABLE |
 		   MMC_CAP_HW_RESET | MMC_CAP_1_8V_DDR |
 		   MMC_CAP_CMD_DURING_TFR | MMC_CAP_WAIT_WHILE_BUSY,
-	.caps2   = MMC_CAP2_HC_ERASE_SZ,
 	.flags   = SDHCI_ACPI_RUNTIME_PM,
 	.quirks  = SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC,
 	.quirks2 = SDHCI_QUIRK2_PRESET_VALUE_BROKEN |
@@ -396,9 +395,6 @@ static int sdhci_acpi_probe(struct platform_device *pdev)
 		if (child->status.present && child->status.enabled)
 			acpi_device_fix_up_power(child);
 
-	if (acpi_bus_get_status(device) || !device->status.present)
-		return -ENODEV;
-
 	if (sdhci_acpi_byt_defer(dev))
 		return -EPROBE_DEFER;
 
diff --git a/drivers/mmc/host/sdhci-brcmstb.c b/drivers/mmc/host/sdhci-brcmstb.c
index 242c5dc7a81e..e2f638338e8f 100644
--- a/drivers/mmc/host/sdhci-brcmstb.c
+++ b/drivers/mmc/host/sdhci-brcmstb.c
@@ -89,9 +89,6 @@ static int sdhci_brcmstb_probe(struct platform_device *pdev)
 		goto err_clk;
 	}
 
-	/* Enable MMC_CAP2_HC_ERASE_SZ for better max discard calculations */
-	host->mmc->caps2 |= MMC_CAP2_HC_ERASE_SZ;
-
 	sdhci_get_of_property(pdev);
 	mmc_of_parse(host->mmc);
 
diff --git a/drivers/mmc/host/sdhci-esdhc-imx.c b/drivers/mmc/host/sdhci-esdhc-imx.c
index 23d8b8a73ae9..85140c9af581 100644
--- a/drivers/mmc/host/sdhci-esdhc-imx.c
+++ b/drivers/mmc/host/sdhci-esdhc-imx.c
@@ -95,7 +95,7 @@
 #define ESDHC_CTRL_BUSWIDTH_MASK	(0x3 << 1)
 
 /*
- * There is an INT DMA ERR mis-match between eSDHC and STD SDHC SPEC:
+ * There is an INT DMA ERR mismatch between eSDHC and STD SDHC SPEC:
  * Bit25 is used in STD SPEC, and is reserved in fsl eSDHC design,
  * but bit28 is used as the INT DMA ERR in fsl eSDHC design.
  * Define this macro DMA error INT for fsl eSDHC
@@ -110,16 +110,11 @@
  * In exact block transfer, the controller doesn't complete the
  * operations automatically as required at the end of the
  * transfer and remains on hold if the abort command is not sent.
- * As a result, the TC flag is not asserted and SW  received timeout
- * exeception. Bit1 of Vendor Spec registor is used to fix it.
+ * As a result, the TC flag is not asserted and SW received timeout
+ * exception. Bit1 of Vendor Spec register is used to fix it.
  */
 #define ESDHC_FLAG_MULTIBLK_NO_INT	BIT(1)
 /*
- * The flag enables the workaround for ESDHC errata ENGcm07207 which
- * affects i.MX25 and i.MX35.
- */
-#define ESDHC_FLAG_ENGCM07207		BIT(2)
-/*
  * The flag tells that the ESDHC controller is an USDHC block that is
  * integrated on the i.MX6 series.
  */
@@ -131,9 +126,11 @@
 /* The IP has SDHCI_CAPABILITIES_1 register */
 #define ESDHC_FLAG_HAVE_CAP1		BIT(6)
 /*
- * The IP has errata ERR004536
+ * The IP has erratum ERR004536
  * uSDHC: ADMA Length Mismatch Error occurs if the AHB read access is slow,
  * when reading data from the card
+ * This flag is also set for i.MX25 and i.MX35 in order to get
+ * SDHCI_QUIRK_BROKEN_ADMA, but for different reasons (ADMA capability bits).
  */
 #define ESDHC_FLAG_ERR004536		BIT(7)
 /* The IP supports HS200 mode */
@@ -141,7 +138,7 @@
 /* The IP supports HS400 mode */
 #define ESDHC_FLAG_HS400		BIT(9)
 
-/* A higher clock ferquency than this rate requires strobell dll control */
+/* A clock frequency higher than this rate requires strobe dll control */
 #define ESDHC_STROBE_DLL_CLK_FREQ	100000000
 
 struct esdhc_soc_data {
@@ -149,11 +146,11 @@ struct esdhc_soc_data {
 };
 
 static struct esdhc_soc_data esdhc_imx25_data = {
-	.flags = ESDHC_FLAG_ENGCM07207,
+	.flags = ESDHC_FLAG_ERR004536,
 };
 
 static struct esdhc_soc_data esdhc_imx35_data = {
-	.flags = ESDHC_FLAG_ENGCM07207,
+	.flags = ESDHC_FLAG_ERR004536,
 };
 
 static struct esdhc_soc_data esdhc_imx51_data = {
@@ -197,7 +194,7 @@ struct pltfm_imx_data {
 	struct clk *clk_ahb;
 	struct clk *clk_per;
 	enum {
-		NO_CMD_PENDING,      /* no multiblock command pending*/
+		NO_CMD_PENDING,      /* no multiblock command pending */
 		MULTIBLK_IN_PROCESS, /* exact multiblock cmd in process */
 		WAIT_FOR_INT,        /* sent CMD12, waiting for response INT */
 	} multiblock_status;
@@ -286,7 +283,7 @@ static u32 esdhc_readl_le(struct sdhci_host *host, int reg)
 		 * ADMA2 capability of esdhc, but this bit is messed up on
 		 * some SOCs (e.g. on MX25, MX35 this bit is set, but they
 		 * don't actually support ADMA2). So set the BROKEN_ADMA
-		 * uirk on MX25/35 platforms.
+		 * quirk on MX25/35 platforms.
 		 */
 
 		if (val & SDHCI_CAN_DO_ADMA1) {
@@ -351,7 +348,7 @@ static void esdhc_writel_le(struct sdhci_host *host, u32 val, int reg)
 		if ((val & SDHCI_INT_CARD_INT) && !esdhc_is_usdhc(imx_data)) {
 			/*
 			 * Clear and then set D3CD bit to avoid missing the
-			 * card interrupt.  This is a eSDHC controller problem
+			 * card interrupt. This is an eSDHC controller problem
 			 * so we need to apply the following workaround: clear
 			 * and set D3CD bit will make eSDHC re-sample the card
 			 * interrupt. In case a card interrupt was lost,
@@ -579,7 +576,7 @@ 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 = sdhci_pltfm_priv(pltfm_host);
-	u32 new_val;
+	u32 new_val = 0;
 	u32 mask;
 
 	switch (reg) {
@@ -604,35 +601,52 @@ static void esdhc_writeb_le(struct sdhci_host *host, u8 val, int reg)
 		 * Do not touch buswidth bits here. This is done in
 		 * esdhc_pltfm_bus_width.
 		 * Do not touch the D3CD bit either which is used for the
-		 * SDIO interrupt errata workaround.
+		 * SDIO interrupt erratum workaround.
 		 */
 		mask = 0xffff & ~(ESDHC_CTRL_BUSWIDTH_MASK | ESDHC_CTRL_D3CD);
 
 		esdhc_clrset_le(host, mask, new_val, reg);
 		return;
+	case SDHCI_SOFTWARE_RESET:
+		if (val & SDHCI_RESET_DATA)
+			new_val = readl(host->ioaddr + SDHCI_HOST_CONTROL);
+		break;
 	}
 	esdhc_clrset_le(host, 0xff, val, reg);
 
-	/*
-	 * The esdhc has a design violation to SDHC spec which tells
-	 * that software reset should not affect card detection circuit.
-	 * But esdhc clears its SYSCTL register bits [0..2] during the
-	 * software reset.  This will stop those clocks that card detection
-	 * circuit relies on.  To work around it, we turn the clocks on back
-	 * to keep card detection circuit functional.
-	 */
-	if ((reg == SDHCI_SOFTWARE_RESET) && (val & 1)) {
-		esdhc_clrset_le(host, 0x7, 0x7, ESDHC_SYSTEM_CONTROL);
-		/*
-		 * The reset on usdhc fails to clear MIX_CTRL register.
-		 * Do it manually here.
-		 */
-		if (esdhc_is_usdhc(imx_data)) {
-			/* the tuning bits should be kept during reset */
-			new_val = readl(host->ioaddr + ESDHC_MIX_CTRL);
-			writel(new_val & ESDHC_MIX_CTRL_TUNING_MASK,
-					host->ioaddr + ESDHC_MIX_CTRL);
-			imx_data->is_ddr = 0;
+	if (reg == SDHCI_SOFTWARE_RESET) {
+		if (val & SDHCI_RESET_ALL) {
+			/*
+			 * The esdhc has a design violation to SDHC spec which
+			 * tells that software reset should not affect card
+			 * detection circuit. But esdhc clears its SYSCTL
+			 * register bits [0..2] during the software reset. This
+			 * will stop those clocks that card detection circuit
+			 * relies on. To work around it, we turn the clocks on
+			 * back to keep card detection circuit functional.
+			 */
+			esdhc_clrset_le(host, 0x7, 0x7, ESDHC_SYSTEM_CONTROL);
+			/*
+			 * The reset on usdhc fails to clear MIX_CTRL register.
+			 * Do it manually here.
+			 */
+			if (esdhc_is_usdhc(imx_data)) {
+				/*
+				 * the tuning bits should be kept during reset
+				 */
+				new_val = readl(host->ioaddr + ESDHC_MIX_CTRL);
+				writel(new_val & ESDHC_MIX_CTRL_TUNING_MASK,
+						host->ioaddr + ESDHC_MIX_CTRL);
+				imx_data->is_ddr = 0;
+			}
+		} else if (val & SDHCI_RESET_DATA) {
+			/*
+			 * The eSDHC DAT line software reset clears at least the
+			 * data transfer width on i.MX25, so make sure that the
+			 * Host Control register is unaffected.
+			 */
+			esdhc_clrset_le(host, 0xff, new_val,
+					SDHCI_HOST_CONTROL);
 		}
 	}
 }
@@ -657,7 +671,8 @@ static inline void esdhc_pltfm_set_clock(struct sdhci_host *host,
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 	struct pltfm_imx_data *imx_data = sdhci_pltfm_priv(pltfm_host);
 	unsigned int host_clock = pltfm_host->clock;
-	int pre_div = 2;
+	int ddr_pre_div = imx_data->is_ddr ? 2 : 1;
+	int pre_div = 1;
 	int div = 1;
 	u32 temp, val;
 
@@ -672,28 +687,23 @@ static inline void esdhc_pltfm_set_clock(struct sdhci_host *host,
 		return;
 	}
 
-	if (esdhc_is_usdhc(imx_data) && !imx_data->is_ddr)
-		pre_div = 1;
-
 	temp = sdhci_readl(host, ESDHC_SYSTEM_CONTROL);
 	temp &= ~(ESDHC_CLOCK_IPGEN | ESDHC_CLOCK_HCKEN | ESDHC_CLOCK_PEREN
 		| ESDHC_CLOCK_MASK);
 	sdhci_writel(host, temp, ESDHC_SYSTEM_CONTROL);
 
-	while (host_clock / pre_div / 16 > clock && pre_div < 256)
+	while (host_clock / (16 * pre_div * ddr_pre_div) > clock &&
+			pre_div < 256)
 		pre_div *= 2;
 
-	while (host_clock / pre_div / div > clock && div < 16)
+	while (host_clock / (div * pre_div * ddr_pre_div) > clock && div < 16)
 		div++;
 
-	host->mmc->actual_clock = host_clock / pre_div / div;
+	host->mmc->actual_clock = host_clock / (div * pre_div * ddr_pre_div);
 	dev_dbg(mmc_dev(host->mmc), "desired SD clock: %d, actual: %d\n",
 		clock, host->mmc->actual_clock);
 
-	if (imx_data->is_ddr)
-		pre_div >>= 2;
-	else
-		pre_div >>= 1;
+	pre_div >>= 1;
 	div--;
 
 	temp = sdhci_readl(host, ESDHC_SYSTEM_CONTROL);
@@ -763,7 +773,7 @@ static void esdhc_prepare_tuning(struct sdhci_host *host, u32 val)
 	writel(reg, host->ioaddr + ESDHC_MIX_CTRL);
 	writel(val << 8, host->ioaddr + ESDHC_TUNE_CTRL_STATUS);
 	dev_dbg(mmc_dev(host->mmc),
-		"tunning with delay 0x%x ESDHC_TUNE_CTRL_STATUS 0x%x\n",
+		"tuning with delay 0x%x ESDHC_TUNE_CTRL_STATUS 0x%x\n",
 			val, readl(host->ioaddr + ESDHC_TUNE_CTRL_STATUS));
 }
 
@@ -807,7 +817,7 @@ static int esdhc_executing_tuning(struct sdhci_host *host, u32 opcode)
 	ret = mmc_send_tuning(host->mmc, opcode, NULL);
 	esdhc_post_tuning(host);
 
-	dev_dbg(mmc_dev(host->mmc), "tunning %s at 0x%x ret %d\n",
+	dev_dbg(mmc_dev(host->mmc), "tuning %s at 0x%x ret %d\n",
 		ret ? "failed" : "passed", avg, ret);
 
 	return ret;
@@ -847,15 +857,15 @@ static int esdhc_change_pinstate(struct sdhci_host *host,
 }
 
 /*
- * For HS400 eMMC, there is a data_strobe line, this signal is generated
+ * For HS400 eMMC, there is a data_strobe line. This signal is generated
  * by the device and used for data output and CRC status response output
  * in HS400 mode. The frequency of this signal follows the frequency of
- * CLK generated by host. Host receive the data which is aligned to the
+ * CLK generated by host. The host receives the data which is aligned to the
  * edge of data_strobe line. Due to the time delay between CLK line and
  * data_strobe line, if the delay time is larger than one clock cycle,
- * then CLK and data_strobe line will misaligned, read error shows up.
+ * then CLK and data_strobe line will be misaligned, read error shows up.
  * So when the CLK is higher than 100MHz, each clock cycle is short enough,
- * host should config the delay target.
+ * host should configure the delay target.
  */
 static void esdhc_set_strobe_dll(struct sdhci_host *host)
 {
@@ -895,7 +905,7 @@ static void esdhc_reset_tuning(struct sdhci_host *host)
 	struct pltfm_imx_data *imx_data = sdhci_pltfm_priv(pltfm_host);
 	u32 ctrl;
 
-	/* Rest the tuning circurt */
+	/* Reset the tuning circuit */
 	if (esdhc_is_usdhc(imx_data)) {
 		if (imx_data->socdata->flags & ESDHC_FLAG_MAN_TUNING) {
 			ctrl = readl(host->ioaddr + ESDHC_MIX_CTRL);
@@ -976,7 +986,7 @@ static unsigned int esdhc_get_max_timeout_count(struct sdhci_host *host)
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 	struct pltfm_imx_data *imx_data = sdhci_pltfm_priv(pltfm_host);
 
-	/* Doc Errata: the uSDHC actual maximum timeout count is 1 << 29 */
+	/* Doc Erratum: the uSDHC actual maximum timeout count is 1 << 29 */
 	return esdhc_is_usdhc(imx_data) ? 1 << 29 : 1 << 27;
 }
 
@@ -1032,10 +1042,10 @@ static void sdhci_esdhc_imx_hwinit(struct sdhci_host *host)
 
 		/*
 		 * ROM code will change the bit burst_length_enable setting
-		 * to zero if this usdhc is choosed to boot system. Change
+		 * to zero if this usdhc is chosen to boot system. Change
 		 * it back here, otherwise it will impact the performance a
 		 * lot. This bit is used to enable/disable the burst length
-		 * for the external AHB2AXI bridge, it's usefully especially
+		 * for the external AHB2AXI bridge. It's useful especially
 		 * for INCR transfer because without burst length indicator,
 		 * the AHB2AXI bridge does not know the burst length in
 		 * advance. And without burst length indicator, AHB INCR
@@ -1045,7 +1055,7 @@ static void sdhci_esdhc_imx_hwinit(struct sdhci_host *host)
 			| ESDHC_BURST_LEN_EN_INCR,
 			host->ioaddr + SDHCI_HOST_CONTROL);
 		/*
-		* errata ESDHC_FLAG_ERR004536 fix for MX6Q TO1.2 and MX6DL
+		* erratum ESDHC_FLAG_ERR004536 fix for MX6Q TO1.2 and MX6DL
 		* TO1.1, it's harmless for MX6SL
 		*/
 		writel(readl(host->ioaddr + 0x6c) | BIT(7),
@@ -1104,7 +1114,7 @@ sdhci_esdhc_imx_probe_dt(struct platform_device *pdev,
 
 	mmc_of_parse_voltage(np, &host->ocr_mask);
 
-	/* sdr50 and sdr104 needs work on 1.8v signal voltage */
+	/* sdr50 and sdr104 need work on 1.8v signal voltage */
 	if ((boarddata->support_vsel) && esdhc_is_usdhc(imx_data) &&
 	    !IS_ERR(imx_data->pins_default)) {
 		imx_data->pins_100mhz = pinctrl_lookup_state(imx_data->pinctrl,
@@ -1116,7 +1126,8 @@ sdhci_esdhc_imx_probe_dt(struct platform_device *pdev,
 			dev_warn(mmc_dev(host->mmc),
 				"could not get ultra high speed state, work on normal mode\n");
 			/*
-			 * fall back to not support uhs by specify no 1.8v quirk
+			 * fall back to not supporting uhs by specifying no
+			 * 1.8v quirk
 			 */
 			host->quirks2 |= SDHCI_QUIRK2_NO_1_8_V;
 		}
@@ -1250,14 +1261,20 @@ static int sdhci_esdhc_imx_probe(struct platform_device *pdev)
 
 	pltfm_host->clk = imx_data->clk_per;
 	pltfm_host->clock = clk_get_rate(pltfm_host->clk);
-	clk_prepare_enable(imx_data->clk_per);
-	clk_prepare_enable(imx_data->clk_ipg);
-	clk_prepare_enable(imx_data->clk_ahb);
+	err = clk_prepare_enable(imx_data->clk_per);
+	if (err)
+		goto free_sdhci;
+	err = clk_prepare_enable(imx_data->clk_ipg);
+	if (err)
+		goto disable_per_clk;
+	err = clk_prepare_enable(imx_data->clk_ahb);
+	if (err)
+		goto disable_ipg_clk;
 
 	imx_data->pinctrl = devm_pinctrl_get(&pdev->dev);
 	if (IS_ERR(imx_data->pinctrl)) {
 		err = PTR_ERR(imx_data->pinctrl);
-		goto disable_clk;
+		goto disable_ahb_clk;
 	}
 
 	imx_data->pins_default = pinctrl_lookup_state(imx_data->pinctrl,
@@ -1265,11 +1282,6 @@ static int sdhci_esdhc_imx_probe(struct platform_device *pdev)
 	if (IS_ERR(imx_data->pins_default))
 		dev_warn(mmc_dev(host->mmc), "could not get default state\n");
 
-	if (imx_data->socdata->flags & ESDHC_FLAG_ENGCM07207)
-		/* Fix errata ENGcm07207 present on i.MX25 and i.MX35 */
-		host->quirks |= SDHCI_QUIRK_NO_MULTIBLOCK
-			| SDHCI_QUIRK_BROKEN_ADMA;
-
 	if (esdhc_is_usdhc(imx_data)) {
 		host->quirks2 |= SDHCI_QUIRK2_PRESET_VALUE_BROKEN;
 		host->mmc->caps |= MMC_CAP_1_8V_DDR;
@@ -1297,13 +1309,13 @@ static int sdhci_esdhc_imx_probe(struct platform_device *pdev)
 	else
 		err = sdhci_esdhc_imx_probe_nondt(pdev, host, imx_data);
 	if (err)
-		goto disable_clk;
+		goto disable_ahb_clk;
 
 	sdhci_esdhc_imx_hwinit(host);
 
 	err = sdhci_add_host(host);
 	if (err)
-		goto disable_clk;
+		goto disable_ahb_clk;
 
 	pm_runtime_set_active(&pdev->dev);
 	pm_runtime_set_autosuspend_delay(&pdev->dev, 50);
@@ -1313,10 +1325,12 @@ static int sdhci_esdhc_imx_probe(struct platform_device *pdev)
 
 	return 0;
 
-disable_clk:
-	clk_disable_unprepare(imx_data->clk_per);
-	clk_disable_unprepare(imx_data->clk_ipg);
+disable_ahb_clk:
 	clk_disable_unprepare(imx_data->clk_ahb);
+disable_ipg_clk:
+	clk_disable_unprepare(imx_data->clk_ipg);
+disable_per_clk:
+	clk_disable_unprepare(imx_data->clk_per);
 free_sdhci:
 	sdhci_pltfm_free(pdev);
 	return err;
@@ -1393,14 +1407,34 @@ static int sdhci_esdhc_runtime_resume(struct device *dev)
 	struct sdhci_host *host = dev_get_drvdata(dev);
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 	struct pltfm_imx_data *imx_data = sdhci_pltfm_priv(pltfm_host);
+	int err;
 
 	if (!sdhci_sdio_irq_enabled(host)) {
-		clk_prepare_enable(imx_data->clk_per);
-		clk_prepare_enable(imx_data->clk_ipg);
+		err = clk_prepare_enable(imx_data->clk_per);
+		if (err)
+			return err;
+		err = clk_prepare_enable(imx_data->clk_ipg);
+		if (err)
+			goto disable_per_clk;
 	}
-	clk_prepare_enable(imx_data->clk_ahb);
+	err = clk_prepare_enable(imx_data->clk_ahb);
+	if (err)
+		goto disable_ipg_clk;
+	err = sdhci_runtime_resume_host(host);
+	if (err)
+		goto disable_ahb_clk;
+
+	return 0;
 
-	return sdhci_runtime_resume_host(host);
+disable_ahb_clk:
+	clk_disable_unprepare(imx_data->clk_ahb);
+disable_ipg_clk:
+	if (!sdhci_sdio_irq_enabled(host))
+		clk_disable_unprepare(imx_data->clk_ipg);
+disable_per_clk:
+	if (!sdhci_sdio_irq_enabled(host))
+		clk_disable_unprepare(imx_data->clk_per);
+	return err;
 }
 #endif
 
diff --git a/drivers/mmc/host/sdhci-esdhc.h b/drivers/mmc/host/sdhci-esdhc.h
index c4bbd7485987..e7893f21b65e 100644
--- a/drivers/mmc/host/sdhci-esdhc.h
+++ b/drivers/mmc/host/sdhci-esdhc.h
@@ -19,6 +19,7 @@
  */
 
 #define ESDHC_DEFAULT_QUIRKS	(SDHCI_QUIRK_FORCE_BLK_SZ_2048 | \
+				SDHCI_QUIRK_32BIT_DMA_ADDR | \
 				SDHCI_QUIRK_NO_BUSY_IRQ | \
 				SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK | \
 				SDHCI_QUIRK_PIO_NEEDS_DELAY | \
diff --git a/drivers/mmc/host/sdhci-of-arasan.c b/drivers/mmc/host/sdhci-of-arasan.c
index ea6b36c88ae7..b13c0a7d50e4 100644
--- a/drivers/mmc/host/sdhci-of-arasan.c
+++ b/drivers/mmc/host/sdhci-of-arasan.c
@@ -638,7 +638,7 @@ static int sdhci_arasan_probe(struct platform_device *pdev)
 
 	ret = mmc_of_parse(host->mmc);
 	if (ret) {
-		dev_err(&pdev->dev, "parsing dt failed (%u)\n", ret);
+		dev_err(&pdev->dev, "parsing dt failed (%d)\n", ret);
 		goto unreg_clk;
 	}
 
diff --git a/drivers/mmc/host/sdhci-pci-core.c b/drivers/mmc/host/sdhci-pci-core.c
index 92fc3f7c538d..e1721ac37919 100644
--- a/drivers/mmc/host/sdhci-pci-core.c
+++ b/drivers/mmc/host/sdhci-pci-core.c
@@ -347,8 +347,7 @@ static inline void sdhci_pci_remove_own_cd(struct sdhci_pci_slot *slot)
 static int mfd_emmc_probe_slot(struct sdhci_pci_slot *slot)
 {
 	slot->host->mmc->caps |= MMC_CAP_8_BIT_DATA | MMC_CAP_NONREMOVABLE;
-	slot->host->mmc->caps2 |= MMC_CAP2_BOOTPART_NOACC |
-				  MMC_CAP2_HC_ERASE_SZ;
+	slot->host->mmc->caps2 |= MMC_CAP2_BOOTPART_NOACC;
 	return 0;
 }
 
@@ -404,10 +403,9 @@ struct intel_host {
 	bool	d3_retune;
 };
 
-const u8 intel_dsm_uuid[] = {
-	0xA5, 0x3E, 0xC1, 0xF6, 0xCD, 0x65, 0x1F, 0x46,
-	0xAB, 0x7A, 0x29, 0xF7, 0xE8, 0xD5, 0xBD, 0x61,
-};
+static const guid_t intel_dsm_guid =
+	GUID_INIT(0xF6C13EA5, 0x65CD, 0x461F,
+		  0xAB, 0x7A, 0x29, 0xF7, 0xE8, 0xD5, 0xBD, 0x61);
 
 static int __intel_dsm(struct intel_host *intel_host, struct device *dev,
 		       unsigned int fn, u32 *result)
@@ -416,7 +414,7 @@ static int __intel_dsm(struct intel_host *intel_host, struct device *dev,
 	int err = 0;
 	size_t len;
 
-	obj = acpi_evaluate_dsm(ACPI_HANDLE(dev), intel_dsm_uuid, 0, fn, NULL);
+	obj = acpi_evaluate_dsm(ACPI_HANDLE(dev), &intel_dsm_guid, 0, fn, NULL);
 	if (!obj)
 		return -EOPNOTSUPP;
 
@@ -543,6 +541,23 @@ static void sdhci_intel_set_power(struct sdhci_host *host, unsigned char mode,
 	}
 }
 
+#define INTEL_HS400_ES_REG 0x78
+#define INTEL_HS400_ES_BIT BIT(0)
+
+static void intel_hs400_enhanced_strobe(struct mmc_host *mmc,
+					struct mmc_ios *ios)
+{
+	struct sdhci_host *host = mmc_priv(mmc);
+	u32 val;
+
+	val = sdhci_readl(host, INTEL_HS400_ES_REG);
+	if (ios->enhanced_strobe)
+		val |= INTEL_HS400_ES_BIT;
+	else
+		val &= ~INTEL_HS400_ES_BIT;
+	sdhci_writel(host, val, INTEL_HS400_ES_REG);
+}
+
 static const struct sdhci_ops sdhci_intel_byt_ops = {
 	.set_clock		= sdhci_set_clock,
 	.set_power		= sdhci_intel_set_power,
@@ -570,7 +585,6 @@ static int byt_emmc_probe_slot(struct sdhci_pci_slot *slot)
 				 MMC_CAP_HW_RESET | MMC_CAP_1_8V_DDR |
 				 MMC_CAP_CMD_DURING_TFR |
 				 MMC_CAP_WAIT_WHILE_BUSY;
-	slot->host->mmc->caps2 |= MMC_CAP2_HC_ERASE_SZ;
 	slot->hw_reset = sdhci_pci_int_hw_reset;
 	if (slot->chip->pdev->device == PCI_DEVICE_ID_INTEL_BSW_EMMC)
 		slot->host->timeout_clk = 1000; /* 1000 kHz i.e. 1 MHz */
@@ -579,6 +593,19 @@ static int byt_emmc_probe_slot(struct sdhci_pci_slot *slot)
 	return 0;
 }
 
+static int glk_emmc_probe_slot(struct sdhci_pci_slot *slot)
+{
+	int ret = byt_emmc_probe_slot(slot);
+
+	if (slot->chip->pdev->device != PCI_DEVICE_ID_INTEL_GLK_EMMC) {
+		slot->host->mmc->caps2 |= MMC_CAP2_HS400_ES,
+		slot->host->mmc_host_ops.hs400_enhanced_strobe =
+						intel_hs400_enhanced_strobe;
+	}
+
+	return ret;
+}
+
 #ifdef CONFIG_ACPI
 static int ni_set_max_freq(struct sdhci_pci_slot *slot)
 {
@@ -631,7 +658,7 @@ static int byt_sd_probe_slot(struct sdhci_pci_slot *slot)
 {
 	byt_read_dsm(slot);
 	slot->host->mmc->caps |= MMC_CAP_WAIT_WHILE_BUSY |
-				 MMC_CAP_AGGRESSIVE_PM;
+				 MMC_CAP_AGGRESSIVE_PM | MMC_CAP_CD_WAKE;
 	slot->cd_idx = 0;
 	slot->cd_override_level = true;
 	if (slot->chip->pdev->device == PCI_DEVICE_ID_INTEL_BXT_SD ||
@@ -654,6 +681,17 @@ static const struct sdhci_pci_fixes sdhci_intel_byt_emmc = {
 	.priv_size	= sizeof(struct intel_host),
 };
 
+static const struct sdhci_pci_fixes sdhci_intel_glk_emmc = {
+	.allow_runtime_pm	= true,
+	.probe_slot		= glk_emmc_probe_slot,
+	.quirks			= SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC,
+	.quirks2		= SDHCI_QUIRK2_PRESET_VALUE_BROKEN |
+				  SDHCI_QUIRK2_CAPS_BIT63_FOR_HS400 |
+				  SDHCI_QUIRK2_STOP_WITH_TC,
+	.ops			= &sdhci_intel_byt_ops,
+	.priv_size		= sizeof(struct intel_host),
+};
+
 static const struct sdhci_pci_fixes sdhci_ni_byt_sdio = {
 	.quirks		= SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC,
 	.quirks2	= SDHCI_QUIRK2_HOST_OFF_CARD_ON |
@@ -1171,554 +1209,79 @@ static const struct sdhci_pci_fixes sdhci_amd = {
 };
 
 static const struct pci_device_id pci_ids[] = {
-	{
-		.vendor		= PCI_VENDOR_ID_RICOH,
-		.device		= PCI_DEVICE_ID_RICOH_R5C822,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_ricoh,
-	},
-
-	{
-		.vendor         = PCI_VENDOR_ID_RICOH,
-		.device         = 0x843,
-		.subvendor      = PCI_ANY_ID,
-		.subdevice      = PCI_ANY_ID,
-		.driver_data    = (kernel_ulong_t)&sdhci_ricoh_mmc,
-	},
-
-	{
-		.vendor         = PCI_VENDOR_ID_RICOH,
-		.device         = 0xe822,
-		.subvendor      = PCI_ANY_ID,
-		.subdevice      = PCI_ANY_ID,
-		.driver_data    = (kernel_ulong_t)&sdhci_ricoh_mmc,
-	},
-
-	{
-		.vendor         = PCI_VENDOR_ID_RICOH,
-		.device         = 0xe823,
-		.subvendor      = PCI_ANY_ID,
-		.subdevice      = PCI_ANY_ID,
-		.driver_data    = (kernel_ulong_t)&sdhci_ricoh_mmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_ENE,
-		.device		= PCI_DEVICE_ID_ENE_CB712_SD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_ene_712,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_ENE,
-		.device		= PCI_DEVICE_ID_ENE_CB712_SD_2,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_ene_712,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_ENE,
-		.device		= PCI_DEVICE_ID_ENE_CB714_SD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_ene_714,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_ENE,
-		.device		= PCI_DEVICE_ID_ENE_CB714_SD_2,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_ene_714,
-	},
-
-	{
-		.vendor         = PCI_VENDOR_ID_MARVELL,
-		.device         = PCI_DEVICE_ID_MARVELL_88ALP01_SD,
-		.subvendor      = PCI_ANY_ID,
-		.subdevice      = PCI_ANY_ID,
-		.driver_data    = (kernel_ulong_t)&sdhci_cafe,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_JMICRON,
-		.device		= PCI_DEVICE_ID_JMICRON_JMB38X_SD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_jmicron,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_JMICRON,
-		.device		= PCI_DEVICE_ID_JMICRON_JMB38X_MMC,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_jmicron,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_JMICRON,
-		.device		= PCI_DEVICE_ID_JMICRON_JMB388_SD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_jmicron,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_JMICRON,
-		.device		= PCI_DEVICE_ID_JMICRON_JMB388_ESD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_jmicron,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_SYSKONNECT,
-		.device		= 0x8000,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_syskt,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_VIA,
-		.device		= 0x95d0,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_via,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_REALTEK,
-		.device		= 0x5250,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_rtsx,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_QRK_SD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_qrk,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_MRST_SD0,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mrst_hc0,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_MRST_SD1,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mrst_hc1_hc2,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_MRST_SD2,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mrst_hc1_hc2,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_MFD_SD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mfd_sd,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_MFD_SDIO1,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mfd_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_MFD_SDIO2,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mfd_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_MFD_EMMC0,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mfd_emmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_MFD_EMMC1,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mfd_emmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_PCH_SDIO0,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_pch_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_PCH_SDIO1,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_pch_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BYT_EMMC,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_emmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BYT_SDIO,
-		.subvendor	= PCI_VENDOR_ID_NI,
-		.subdevice	= 0x7884,
-		.driver_data	= (kernel_ulong_t)&sdhci_ni_byt_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BYT_SDIO,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BYT_SD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sd,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BYT_EMMC2,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_emmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BSW_EMMC,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_emmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BSW_SDIO,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BSW_SD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sd,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_CLV_SDIO0,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mfd_sd,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_CLV_SDIO1,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mfd_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_CLV_SDIO2,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mfd_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_CLV_EMMC0,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mfd_emmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_CLV_EMMC1,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mfd_emmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_MRFLD_MMC,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_mrfld_mmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_SPT_EMMC,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_emmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_SPT_SDIO,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_SPT_SD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sd,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_DNV_EMMC,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_emmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BXT_EMMC,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_emmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BXT_SDIO,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BXT_SD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sd,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BXTM_EMMC,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_emmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BXTM_SDIO,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_BXTM_SD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sd,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_APL_EMMC,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_emmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_APL_SDIO,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_APL_SD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sd,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_GLK_EMMC,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_emmc,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_GLK_SDIO,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sdio,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_INTEL,
-		.device		= PCI_DEVICE_ID_INTEL_GLK_SD,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sd,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_O2,
-		.device		= PCI_DEVICE_ID_O2_8120,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_o2,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_O2,
-		.device		= PCI_DEVICE_ID_O2_8220,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_o2,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_O2,
-		.device		= PCI_DEVICE_ID_O2_8221,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_o2,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_O2,
-		.device		= PCI_DEVICE_ID_O2_8320,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_o2,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_O2,
-		.device		= PCI_DEVICE_ID_O2_8321,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_o2,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_O2,
-		.device		= PCI_DEVICE_ID_O2_FUJIN2,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_o2,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_O2,
-		.device		= PCI_DEVICE_ID_O2_SDS0,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_o2,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_O2,
-		.device		= PCI_DEVICE_ID_O2_SDS1,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_o2,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_O2,
-		.device		= PCI_DEVICE_ID_O2_SEABIRD0,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_o2,
-	},
-
-	{
-		.vendor		= PCI_VENDOR_ID_O2,
-		.device		= PCI_DEVICE_ID_O2_SEABIRD1,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_o2,
-	},
-	{
-		.vendor		= PCI_VENDOR_ID_AMD,
-		.device		= PCI_ANY_ID,
-		.class		= PCI_CLASS_SYSTEM_SDHCI << 8,
-		.class_mask	= 0xFFFF00,
-		.subvendor	= PCI_ANY_ID,
-		.subdevice	= PCI_ANY_ID,
-		.driver_data	= (kernel_ulong_t)&sdhci_amd,
-	},
-	{	/* Generic SD host controller */
-		PCI_DEVICE_CLASS((PCI_CLASS_SYSTEM_SDHCI << 8), 0xFFFF00)
-	},
-
+	SDHCI_PCI_DEVICE(RICOH, R5C822,  ricoh),
+	SDHCI_PCI_DEVICE(RICOH, R5C843,  ricoh_mmc),
+	SDHCI_PCI_DEVICE(RICOH, R5CE822, ricoh_mmc),
+	SDHCI_PCI_DEVICE(RICOH, R5CE823, ricoh_mmc),
+	SDHCI_PCI_DEVICE(ENE, CB712_SD,   ene_712),
+	SDHCI_PCI_DEVICE(ENE, CB712_SD_2, ene_712),
+	SDHCI_PCI_DEVICE(ENE, CB714_SD,   ene_714),
+	SDHCI_PCI_DEVICE(ENE, CB714_SD_2, ene_714),
+	SDHCI_PCI_DEVICE(MARVELL, 88ALP01_SD, cafe),
+	SDHCI_PCI_DEVICE(JMICRON, JMB38X_SD,  jmicron),
+	SDHCI_PCI_DEVICE(JMICRON, JMB38X_MMC, jmicron),
+	SDHCI_PCI_DEVICE(JMICRON, JMB388_SD,  jmicron),
+	SDHCI_PCI_DEVICE(JMICRON, JMB388_ESD, jmicron),
+	SDHCI_PCI_DEVICE(SYSKONNECT, 8000, syskt),
+	SDHCI_PCI_DEVICE(VIA, 95D0, via),
+	SDHCI_PCI_DEVICE(REALTEK, 5250, rtsx),
+	SDHCI_PCI_DEVICE(INTEL, QRK_SD,    intel_qrk),
+	SDHCI_PCI_DEVICE(INTEL, MRST_SD0,  intel_mrst_hc0),
+	SDHCI_PCI_DEVICE(INTEL, MRST_SD1,  intel_mrst_hc1_hc2),
+	SDHCI_PCI_DEVICE(INTEL, MRST_SD2,  intel_mrst_hc1_hc2),
+	SDHCI_PCI_DEVICE(INTEL, MFD_SD,    intel_mfd_sd),
+	SDHCI_PCI_DEVICE(INTEL, MFD_SDIO1, intel_mfd_sdio),
+	SDHCI_PCI_DEVICE(INTEL, MFD_SDIO2, intel_mfd_sdio),
+	SDHCI_PCI_DEVICE(INTEL, MFD_EMMC0, intel_mfd_emmc),
+	SDHCI_PCI_DEVICE(INTEL, MFD_EMMC1, intel_mfd_emmc),
+	SDHCI_PCI_DEVICE(INTEL, PCH_SDIO0, intel_pch_sdio),
+	SDHCI_PCI_DEVICE(INTEL, PCH_SDIO1, intel_pch_sdio),
+	SDHCI_PCI_DEVICE(INTEL, BYT_EMMC,  intel_byt_emmc),
+	SDHCI_PCI_SUBDEVICE(INTEL, BYT_SDIO, NI, 7884, ni_byt_sdio),
+	SDHCI_PCI_DEVICE(INTEL, BYT_SDIO,  intel_byt_sdio),
+	SDHCI_PCI_DEVICE(INTEL, BYT_SD,    intel_byt_sd),
+	SDHCI_PCI_DEVICE(INTEL, BYT_EMMC2, intel_byt_emmc),
+	SDHCI_PCI_DEVICE(INTEL, BSW_EMMC,  intel_byt_emmc),
+	SDHCI_PCI_DEVICE(INTEL, BSW_SDIO,  intel_byt_sdio),
+	SDHCI_PCI_DEVICE(INTEL, BSW_SD,    intel_byt_sd),
+	SDHCI_PCI_DEVICE(INTEL, CLV_SDIO0, intel_mfd_sd),
+	SDHCI_PCI_DEVICE(INTEL, CLV_SDIO1, intel_mfd_sdio),
+	SDHCI_PCI_DEVICE(INTEL, CLV_SDIO2, intel_mfd_sdio),
+	SDHCI_PCI_DEVICE(INTEL, CLV_EMMC0, intel_mfd_emmc),
+	SDHCI_PCI_DEVICE(INTEL, CLV_EMMC1, intel_mfd_emmc),
+	SDHCI_PCI_DEVICE(INTEL, MRFLD_MMC, intel_mrfld_mmc),
+	SDHCI_PCI_DEVICE(INTEL, SPT_EMMC,  intel_byt_emmc),
+	SDHCI_PCI_DEVICE(INTEL, SPT_SDIO,  intel_byt_sdio),
+	SDHCI_PCI_DEVICE(INTEL, SPT_SD,    intel_byt_sd),
+	SDHCI_PCI_DEVICE(INTEL, DNV_EMMC,  intel_byt_emmc),
+	SDHCI_PCI_DEVICE(INTEL, BXT_EMMC,  intel_byt_emmc),
+	SDHCI_PCI_DEVICE(INTEL, BXT_SDIO,  intel_byt_sdio),
+	SDHCI_PCI_DEVICE(INTEL, BXT_SD,    intel_byt_sd),
+	SDHCI_PCI_DEVICE(INTEL, BXTM_EMMC, intel_byt_emmc),
+	SDHCI_PCI_DEVICE(INTEL, BXTM_SDIO, intel_byt_sdio),
+	SDHCI_PCI_DEVICE(INTEL, BXTM_SD,   intel_byt_sd),
+	SDHCI_PCI_DEVICE(INTEL, APL_EMMC,  intel_byt_emmc),
+	SDHCI_PCI_DEVICE(INTEL, APL_SDIO,  intel_byt_sdio),
+	SDHCI_PCI_DEVICE(INTEL, APL_SD,    intel_byt_sd),
+	SDHCI_PCI_DEVICE(INTEL, GLK_EMMC,  intel_glk_emmc),
+	SDHCI_PCI_DEVICE(INTEL, GLK_SDIO,  intel_byt_sdio),
+	SDHCI_PCI_DEVICE(INTEL, GLK_SD,    intel_byt_sd),
+	SDHCI_PCI_DEVICE(INTEL, CNP_EMMC,  intel_glk_emmc),
+	SDHCI_PCI_DEVICE(INTEL, CNP_SD,    intel_byt_sd),
+	SDHCI_PCI_DEVICE(INTEL, CNPH_SD,   intel_byt_sd),
+	SDHCI_PCI_DEVICE(O2, 8120,     o2),
+	SDHCI_PCI_DEVICE(O2, 8220,     o2),
+	SDHCI_PCI_DEVICE(O2, 8221,     o2),
+	SDHCI_PCI_DEVICE(O2, 8320,     o2),
+	SDHCI_PCI_DEVICE(O2, 8321,     o2),
+	SDHCI_PCI_DEVICE(O2, FUJIN2,   o2),
+	SDHCI_PCI_DEVICE(O2, SDS0,     o2),
+	SDHCI_PCI_DEVICE(O2, SDS1,     o2),
+	SDHCI_PCI_DEVICE(O2, SEABIRD0, o2),
+	SDHCI_PCI_DEVICE(O2, SEABIRD1, o2),
+	SDHCI_PCI_DEVICE_CLASS(AMD, SYSTEM_SDHCI, PCI_CLASS_MASK, amd),
+	/* Generic SD host controller */
+	{PCI_DEVICE_CLASS(SYSTEM_SDHCI, PCI_CLASS_MASK)},
 	{ /* end: all zeroes */ },
 };
 
diff --git a/drivers/mmc/host/sdhci-pci.h b/drivers/mmc/host/sdhci-pci.h
index 37766d20a600..75196a2b5289 100644
--- a/drivers/mmc/host/sdhci-pci.h
+++ b/drivers/mmc/host/sdhci-pci.h
@@ -2,7 +2,7 @@
 #define __SDHCI_PCI_H
 
 /*
- * PCI device IDs
+ * PCI device IDs, sub IDs
  */
 
 #define PCI_DEVICE_ID_INTEL_PCH_SDIO0	0x8809
@@ -37,6 +37,50 @@
 #define PCI_DEVICE_ID_INTEL_GLK_SD	0x31ca
 #define PCI_DEVICE_ID_INTEL_GLK_EMMC	0x31cc
 #define PCI_DEVICE_ID_INTEL_GLK_SDIO	0x31d0
+#define PCI_DEVICE_ID_INTEL_CNP_EMMC	0x9dc4
+#define PCI_DEVICE_ID_INTEL_CNP_SD	0x9df5
+#define PCI_DEVICE_ID_INTEL_CNPH_SD	0xa375
+
+#define PCI_DEVICE_ID_SYSKONNECT_8000	0x8000
+#define PCI_DEVICE_ID_VIA_95D0		0x95d0
+#define PCI_DEVICE_ID_REALTEK_5250	0x5250
+
+#define PCI_SUBDEVICE_ID_NI_7884	0x7884
+
+/*
+ * PCI device class and mask
+ */
+
+#define SYSTEM_SDHCI			(PCI_CLASS_SYSTEM_SDHCI << 8)
+#define PCI_CLASS_MASK			0xFFFF00
+
+/*
+ * Macros for PCI device-description
+ */
+
+#define _PCI_VEND(vend) PCI_VENDOR_ID_##vend
+#define _PCI_DEV(vend, dev) PCI_DEVICE_ID_##vend##_##dev
+#define _PCI_SUBDEV(subvend, subdev) PCI_SUBDEVICE_ID_##subvend##_##subdev
+
+#define SDHCI_PCI_DEVICE(vend, dev, cfg) { \
+	.vendor = _PCI_VEND(vend), .device = _PCI_DEV(vend, dev), \
+	.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID, \
+	.driver_data = (kernel_ulong_t)&(sdhci_##cfg) \
+}
+
+#define SDHCI_PCI_SUBDEVICE(vend, dev, subvend, subdev, cfg) { \
+	.vendor = _PCI_VEND(vend), .device = _PCI_DEV(vend, dev), \
+	.subvendor = _PCI_VEND(subvend), \
+	.subdevice = _PCI_SUBDEV(subvend, subdev), \
+	.driver_data = (kernel_ulong_t)&(sdhci_##cfg) \
+}
+
+#define SDHCI_PCI_DEVICE_CLASS(vend, cl, cl_msk, cfg) { \
+	.vendor = _PCI_VEND(vend), .device = PCI_ANY_ID, \
+	.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID, \
+	.class = (cl), .class_mask = (cl_msk), \
+	.driver_data = (kernel_ulong_t)&(sdhci_##cfg) \
+}
 
 /*
  * PCI registers
diff --git a/drivers/mmc/host/sdricoh_cs.c b/drivers/mmc/host/sdricoh_cs.c
index 5ff26ab81eb1..70cb00aa79a0 100644
--- a/drivers/mmc/host/sdricoh_cs.c
+++ b/drivers/mmc/host/sdricoh_cs.c
@@ -256,9 +256,6 @@ static int sdricoh_blockio(struct sdricoh_host *host, int read,
 		}
 	}
 
-	if (len)
-		return -EIO;
-
 	return 0;
 }
 
diff --git a/drivers/mmc/host/tmio_mmc.c b/drivers/mmc/host/tmio_mmc.c
index e897e7fc3b14..64b7e9f18361 100644
--- a/drivers/mmc/host/tmio_mmc.c
+++ b/drivers/mmc/host/tmio_mmc.c
@@ -1,16 +1,16 @@
 /*
- * linux/drivers/mmc/host/tmio_mmc.c
+ * Driver for the MMC / SD / SDIO cell found in:
+ *
+ * TC6393XB TC6391XB TC6387XB T7L66XB ASIC3
  *
+ * Copyright (C) 2017 Renesas Electronics Corporation
+ * Copyright (C) 2017 Horms Solutions, Simon Horman
  * Copyright (C) 2007 Ian Molton
  * Copyright (C) 2004 Ian Molton
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * Driver for the MMC / SD / SDIO cell found in:
- *
- * TC6393XB TC6391XB TC6387XB T7L66XB ASIC3
  */
 
 #include <linux/device.h>
@@ -99,13 +99,13 @@ static int tmio_mmc_probe(struct platform_device *pdev)
 	/* SD control register space size is 0x200, 0x400 for bus_shift=1 */
 	host->bus_shift = resource_size(res) >> 10;
 
-	ret = tmio_mmc_host_probe(host, pdata);
+	ret = tmio_mmc_host_probe(host, pdata, NULL);
 	if (ret)
 		goto host_free;
 
 	ret = devm_request_irq(&pdev->dev, irq, tmio_mmc_irq,
-				IRQF_TRIGGER_FALLING,
-				dev_name(&pdev->dev), host);
+			       IRQF_TRIGGER_FALLING,
+			       dev_name(&pdev->dev), host);
 	if (ret)
 		goto host_remove;
 
@@ -132,6 +132,7 @@ static int tmio_mmc_remove(struct platform_device *pdev)
 
 	if (mmc) {
 		struct tmio_mmc_host *host = mmc_priv(mmc);
+
 		tmio_mmc_host_remove(host);
 		if (cell->disable)
 			cell->disable(pdev);
@@ -145,8 +146,7 @@ static int tmio_mmc_remove(struct platform_device *pdev)
 static const struct dev_pm_ops tmio_mmc_dev_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(tmio_mmc_suspend, tmio_mmc_resume)
 	SET_RUNTIME_PM_OPS(tmio_mmc_host_runtime_suspend,
-			tmio_mmc_host_runtime_resume,
-			NULL)
+			   tmio_mmc_host_runtime_resume, NULL)
 };
 
 static struct platform_driver tmio_mmc_driver = {
diff --git a/drivers/mmc/host/tmio_mmc.h b/drivers/mmc/host/tmio_mmc.h
index d0edb5730d3f..6ad6704175dc 100644
--- a/drivers/mmc/host/tmio_mmc.h
+++ b/drivers/mmc/host/tmio_mmc.h
@@ -1,8 +1,11 @@
 /*
- * linux/drivers/mmc/host/tmio_mmc.h
+ * Driver for the MMC / SD / SDIO cell found in:
+ *
+ * TC6393XB TC6391XB TC6387XB T7L66XB ASIC3
  *
- * Copyright (C) 2016 Sang Engineering, Wolfram Sang
- * Copyright (C) 2015-16 Renesas Electronics Corporation
+ * Copyright (C) 2015-17 Renesas Electronics Corporation
+ * Copyright (C) 2016-17 Sang Engineering, Wolfram Sang
+ * Copyright (C) 2016-17 Horms Solutions, Simon Horman
  * Copyright (C) 2007 Ian Molton
  * Copyright (C) 2004 Ian Molton
  *
@@ -10,9 +13,6 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  *
- * Driver for the MMC / SD / SDIO cell found in:
- *
- * TC6393XB TC6391XB TC6387XB T7L66XB ASIC3
  */
 
 #ifndef TMIO_MMC_H
@@ -115,6 +115,15 @@ struct tmio_mmc_dma {
 	void (*enable)(struct tmio_mmc_host *host, bool enable);
 };
 
+struct tmio_mmc_dma_ops {
+	void (*start)(struct tmio_mmc_host *host, struct mmc_data *data);
+	void (*enable)(struct tmio_mmc_host *host, bool enable);
+	void (*request)(struct tmio_mmc_host *host,
+			struct tmio_mmc_data *pdata);
+	void (*release)(struct tmio_mmc_host *host);
+	void (*abort)(struct tmio_mmc_host *host);
+};
+
 struct tmio_mmc_host {
 	void __iomem *ctl;
 	struct mmc_command      *cmd;
@@ -189,12 +198,15 @@ struct tmio_mmc_host {
 	/* Tuning values: 1 for success, 0 for failure */
 	DECLARE_BITMAP(taps, BITS_PER_BYTE * sizeof(long));
 	unsigned int tap_num;
+
+	const struct tmio_mmc_dma_ops *dma_ops;
 };
 
 struct tmio_mmc_host *tmio_mmc_host_alloc(struct platform_device *pdev);
 void tmio_mmc_host_free(struct tmio_mmc_host *host);
 int tmio_mmc_host_probe(struct tmio_mmc_host *host,
-			struct tmio_mmc_data *pdata);
+			struct tmio_mmc_data *pdata,
+			const struct tmio_mmc_dma_ops *dma_ops);
 void tmio_mmc_host_remove(struct tmio_mmc_host *host);
 void tmio_mmc_do_data_irq(struct tmio_mmc_host *host);
 
@@ -216,38 +228,6 @@ static inline void tmio_mmc_kunmap_atomic(struct scatterlist *sg,
 	local_irq_restore(*flags);
 }
 
-#if defined(CONFIG_MMC_SDHI) || defined(CONFIG_MMC_SDHI_MODULE)
-void tmio_mmc_start_dma(struct tmio_mmc_host *host, struct mmc_data *data);
-void tmio_mmc_enable_dma(struct tmio_mmc_host *host, bool enable);
-void tmio_mmc_request_dma(struct tmio_mmc_host *host, struct tmio_mmc_data *pdata);
-void tmio_mmc_release_dma(struct tmio_mmc_host *host);
-void tmio_mmc_abort_dma(struct tmio_mmc_host *host);
-#else
-static inline void tmio_mmc_start_dma(struct tmio_mmc_host *host,
-			       struct mmc_data *data)
-{
-}
-
-static inline void tmio_mmc_enable_dma(struct tmio_mmc_host *host, bool enable)
-{
-}
-
-static inline void tmio_mmc_request_dma(struct tmio_mmc_host *host,
-				 struct tmio_mmc_data *pdata)
-{
-	host->chan_tx = NULL;
-	host->chan_rx = NULL;
-}
-
-static inline void tmio_mmc_release_dma(struct tmio_mmc_host *host)
-{
-}
-
-static inline void tmio_mmc_abort_dma(struct tmio_mmc_host *host)
-{
-}
-#endif
-
 #ifdef CONFIG_PM
 int tmio_mmc_host_runtime_suspend(struct device *dev);
 int tmio_mmc_host_runtime_resume(struct device *dev);
@@ -259,24 +239,26 @@ static inline u16 sd_ctrl_read16(struct tmio_mmc_host *host, int addr)
 }
 
 static inline void sd_ctrl_read16_rep(struct tmio_mmc_host *host, int addr,
-		u16 *buf, int count)
+				      u16 *buf, int count)
 {
 	readsw(host->ctl + (addr << host->bus_shift), buf, count);
 }
 
-static inline u32 sd_ctrl_read16_and_16_as_32(struct tmio_mmc_host *host, int addr)
+static inline u32 sd_ctrl_read16_and_16_as_32(struct tmio_mmc_host *host,
+					      int addr)
 {
 	return readw(host->ctl + (addr << host->bus_shift)) |
 	       readw(host->ctl + ((addr + 2) << host->bus_shift)) << 16;
 }
 
 static inline void sd_ctrl_read32_rep(struct tmio_mmc_host *host, int addr,
-		u32 *buf, int count)
+				      u32 *buf, int count)
 {
 	readsl(host->ctl + (addr << host->bus_shift), buf, count);
 }
 
-static inline void sd_ctrl_write16(struct tmio_mmc_host *host, int addr, u16 val)
+static inline void sd_ctrl_write16(struct tmio_mmc_host *host, int addr,
+				   u16 val)
 {
 	/* If there is a hook and it returns non-zero then there
 	 * is an error and the write should be skipped
@@ -287,19 +269,20 @@ static inline void sd_ctrl_write16(struct tmio_mmc_host *host, int addr, u16 val
 }
 
 static inline void sd_ctrl_write16_rep(struct tmio_mmc_host *host, int addr,
-		u16 *buf, int count)
+				       u16 *buf, int count)
 {
 	writesw(host->ctl + (addr << host->bus_shift), buf, count);
 }
 
-static inline void sd_ctrl_write32_as_16_and_16(struct tmio_mmc_host *host, int addr, u32 val)
+static inline void sd_ctrl_write32_as_16_and_16(struct tmio_mmc_host *host,
+						int addr, u32 val)
 {
 	writew(val & 0xffff, host->ctl + (addr << host->bus_shift));
 	writew(val >> 16, host->ctl + ((addr + 2) << host->bus_shift));
 }
 
 static inline void sd_ctrl_write32_rep(struct tmio_mmc_host *host, int addr,
-		const u32 *buf, int count)
+				       const u32 *buf, int count)
 {
 	writesl(host->ctl + (addr << host->bus_shift), buf, count);
 }
diff --git a/drivers/mmc/host/tmio_mmc_pio.c b/drivers/mmc/host/tmio_mmc_core.c
index a2d92f10501b..82b80d42f7ae 100644
--- a/drivers/mmc/host/tmio_mmc_pio.c
+++ b/drivers/mmc/host/tmio_mmc_core.c
@@ -1,8 +1,11 @@
 /*
- * linux/drivers/mmc/host/tmio_mmc_pio.c
+ * Driver for the MMC / SD / SDIO IP found in:
+ *
+ * TC6393XB, TC6391XB, TC6387XB, T7L66XB, ASIC3, SH-Mobile SoCs
  *
- * Copyright (C) 2016 Sang Engineering, Wolfram Sang
- * Copyright (C) 2015-16 Renesas Electronics Corporation
+ * Copyright (C) 2015-17 Renesas Electronics Corporation
+ * Copyright (C) 2016-17 Sang Engineering, Wolfram Sang
+ * Copyright (C) 2017 Horms Solutions, Simon Horman
  * Copyright (C) 2011 Guennadi Liakhovetski
  * Copyright (C) 2007 Ian Molton
  * Copyright (C) 2004 Ian Molton
@@ -11,10 +14,6 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  *
- * Driver for the MMC / SD / SDIO IP found in:
- *
- * TC6393XB, TC6391XB, TC6387XB, T7L66XB, ASIC3, SH-Mobile SoCs
- *
  * This driver draws mainly on scattered spec sheets, Reverse engineering
  * of the toshiba e800  SD driver and some parts of the 2.4 ASIC3 driver (4 bit
  * support). (Further 4 bit support from a later datasheet).
@@ -52,17 +51,55 @@
 
 #include "tmio_mmc.h"
 
+static inline void tmio_mmc_start_dma(struct tmio_mmc_host *host,
+				      struct mmc_data *data)
+{
+	if (host->dma_ops)
+		host->dma_ops->start(host, data);
+}
+
+static inline void tmio_mmc_enable_dma(struct tmio_mmc_host *host, bool enable)
+{
+	if (host->dma_ops)
+		host->dma_ops->enable(host, enable);
+}
+
+static inline void tmio_mmc_request_dma(struct tmio_mmc_host *host,
+					struct tmio_mmc_data *pdata)
+{
+	if (host->dma_ops) {
+		host->dma_ops->request(host, pdata);
+	} else {
+		host->chan_tx = NULL;
+		host->chan_rx = NULL;
+	}
+}
+
+static inline void tmio_mmc_release_dma(struct tmio_mmc_host *host)
+{
+	if (host->dma_ops)
+		host->dma_ops->release(host);
+}
+
+static inline void tmio_mmc_abort_dma(struct tmio_mmc_host *host)
+{
+	if (host->dma_ops)
+		host->dma_ops->abort(host);
+}
+
 void tmio_mmc_enable_mmc_irqs(struct tmio_mmc_host *host, u32 i)
 {
 	host->sdcard_irq_mask &= ~(i & TMIO_MASK_IRQ);
 	sd_ctrl_write32_as_16_and_16(host, CTL_IRQ_MASK, host->sdcard_irq_mask);
 }
+EXPORT_SYMBOL_GPL(tmio_mmc_enable_mmc_irqs);
 
 void tmio_mmc_disable_mmc_irqs(struct tmio_mmc_host *host, u32 i)
 {
 	host->sdcard_irq_mask |= (i & TMIO_MASK_IRQ);
 	sd_ctrl_write32_as_16_and_16(host, CTL_IRQ_MASK, host->sdcard_irq_mask);
 }
+EXPORT_SYMBOL_GPL(tmio_mmc_disable_mmc_irqs);
 
 static void tmio_mmc_ack_mmc_irqs(struct tmio_mmc_host *host, u32 i)
 {
@@ -90,16 +127,17 @@ static int tmio_mmc_next_sg(struct tmio_mmc_host *host)
 
 #define STATUS_TO_TEXT(a, status, i) \
 	do { \
-		if (status & TMIO_STAT_##a) { \
-			if (i++) \
-				printk(" | "); \
-			printk(#a); \
+		if ((status) & TMIO_STAT_##a) { \
+			if ((i)++) \
+				printk(KERN_DEBUG " | "); \
+			printk(KERN_DEBUG #a); \
 		} \
 	} while (0)
 
 static void pr_debug_status(u32 status)
 {
 	int i = 0;
+
 	pr_debug("status: %08x = ", status);
 	STATUS_TO_TEXT(CARD_REMOVE, status, i);
 	STATUS_TO_TEXT(CARD_INSERT, status, i);
@@ -140,8 +178,7 @@ static void tmio_mmc_enable_sdio_irq(struct mmc_host *mmc, int enable)
 		pm_runtime_get_sync(mmc_dev(mmc));
 
 		host->sdio_irq_enabled = true;
-		host->sdio_irq_mask = TMIO_SDIO_MASK_ALL &
-					~TMIO_SDIO_STAT_IOIRQ;
+		host->sdio_irq_mask = TMIO_SDIO_MASK_ALL & ~TMIO_SDIO_STAT_IOIRQ;
 
 		/* Clear obsolete interrupts before enabling */
 		sdio_status = sd_ctrl_read16(host, CTL_SDIO_STATUS) & ~TMIO_SDIO_MASK_ALL;
@@ -185,7 +222,7 @@ static void tmio_mmc_clk_stop(struct tmio_mmc_host *host)
 }
 
 static void tmio_mmc_set_clock(struct tmio_mmc_host *host,
-				unsigned int new_clock)
+			       unsigned int new_clock)
 {
 	u32 clk = 0, clock;
 
@@ -229,6 +266,12 @@ static void tmio_mmc_reset(struct tmio_mmc_host *host)
 	if (host->pdata->flags & TMIO_MMC_HAVE_HIGH_REG)
 		sd_ctrl_write16(host, CTL_RESET_SDIO, 0x0001);
 	msleep(10);
+
+	if (host->pdata->flags & TMIO_MMC_SDIO_IRQ) {
+		sd_ctrl_write16(host, CTL_SDIO_IRQ_MASK, host->sdio_irq_mask);
+		sd_ctrl_write16(host, CTL_TRANSACTION_CTL, 0x0001);
+	}
+
 }
 
 static void tmio_mmc_reset_work(struct work_struct *work)
@@ -246,16 +289,16 @@ static void tmio_mmc_reset_work(struct work_struct *work)
 	 * cancel_delayed_work(), it can happen, that a .set_ios() call preempts
 	 * us, so, have to check for IS_ERR(host->mrq)
 	 */
-	if (IS_ERR_OR_NULL(mrq)
-	    || time_is_after_jiffies(host->last_req_ts +
-		msecs_to_jiffies(CMDREQ_TIMEOUT))) {
+	if (IS_ERR_OR_NULL(mrq) ||
+	    time_is_after_jiffies(host->last_req_ts +
+				  msecs_to_jiffies(CMDREQ_TIMEOUT))) {
 		spin_unlock_irqrestore(&host->lock, flags);
 		return;
 	}
 
 	dev_warn(&host->pdev->dev,
-		"timeout waiting for hardware interrupt (CMD%u)\n",
-		mrq->cmd->opcode);
+		 "timeout waiting for hardware interrupt (CMD%u)\n",
+		 mrq->cmd->opcode);
 
 	if (host->data)
 		host->data->error = -ETIMEDOUT;
@@ -279,45 +322,6 @@ static void tmio_mmc_reset_work(struct work_struct *work)
 	mmc_request_done(host->mmc, mrq);
 }
 
-/* called with host->lock held, interrupts disabled */
-static void tmio_mmc_finish_request(struct tmio_mmc_host *host)
-{
-	struct mmc_request *mrq;
-	unsigned long flags;
-
-	spin_lock_irqsave(&host->lock, flags);
-
-	mrq = host->mrq;
-	if (IS_ERR_OR_NULL(mrq)) {
-		spin_unlock_irqrestore(&host->lock, flags);
-		return;
-	}
-
-	host->cmd = NULL;
-	host->data = NULL;
-	host->force_pio = false;
-
-	cancel_delayed_work(&host->delayed_reset_work);
-
-	host->mrq = NULL;
-	spin_unlock_irqrestore(&host->lock, flags);
-
-	if (mrq->cmd->error || (mrq->data && mrq->data->error))
-		tmio_mmc_abort_dma(host);
-
-	if (host->check_scc_error)
-		host->check_scc_error(host);
-
-	mmc_request_done(host->mmc, mrq);
-}
-
-static void tmio_mmc_done_work(struct work_struct *work)
-{
-	struct tmio_mmc_host *host = container_of(work, struct tmio_mmc_host,
-						  done);
-	tmio_mmc_finish_request(host);
-}
-
 /* These are the bitmasks the tmio chip requires to implement the MMC response
  * types. Note that R1 and R6 are the same in this scheme. */
 #define APP_CMD        0x0040
@@ -332,7 +336,8 @@ static void tmio_mmc_done_work(struct work_struct *work)
 #define SECURITY_CMD   0x4000
 #define NO_CMD12_ISSUE 0x4000 /* TMIO_MMC_HAVE_CMD12_CTRL */
 
-static int tmio_mmc_start_command(struct tmio_mmc_host *host, struct mmc_command *cmd)
+static int tmio_mmc_start_command(struct tmio_mmc_host *host,
+				  struct mmc_command *cmd)
 {
 	struct mmc_data *data = host->data;
 	int c = cmd->opcode;
@@ -371,11 +376,11 @@ static int tmio_mmc_start_command(struct tmio_mmc_host *host, struct mmc_command
 			c |= TRANSFER_MULTI;
 
 			/*
-			 * Disable auto CMD12 at IO_RW_EXTENDED when
-			 * multiple block transfer
+			 * Disable auto CMD12 at IO_RW_EXTENDED and
+			 * SET_BLOCK_COUNT when doing multiple block transfer
 			 */
 			if ((host->pdata->flags & TMIO_MMC_HAVE_CMD12_CTRL) &&
-			    (cmd->opcode == SD_IO_RW_EXTENDED))
+			    (cmd->opcode == SD_IO_RW_EXTENDED || host->mrq->sbc))
 				c |= NO_CMD12_ISSUE;
 		}
 		if (data->flags & MMC_DATA_READ)
@@ -497,8 +502,6 @@ static void tmio_mmc_pio_irq(struct tmio_mmc_host *host)
 
 	if (host->sg_off == host->sg_ptr->length)
 		tmio_mmc_next_sg(host);
-
-	return;
 }
 
 static void tmio_mmc_check_bounce_buffer(struct tmio_mmc_host *host)
@@ -506,6 +509,7 @@ static void tmio_mmc_check_bounce_buffer(struct tmio_mmc_host *host)
 	if (host->sg_ptr == &host->bounce_sg) {
 		unsigned long flags;
 		void *sg_vaddr = tmio_mmc_kmap_atomic(host->sg_orig, &flags);
+
 		memcpy(sg_vaddr, host->bounce_buf, host->bounce_sg.length);
 		tmio_mmc_kunmap_atomic(host->sg_orig, &flags, sg_vaddr);
 	}
@@ -552,7 +556,7 @@ void tmio_mmc_do_data_irq(struct tmio_mmc_host *host)
 			host->mrq);
 	}
 
-	if (stop) {
+	if (stop && !host->mrq->sbc) {
 		if (stop->opcode != MMC_STOP_TRANSMISSION || stop->arg)
 			dev_err(&host->pdev->dev, "unsupported stop: CMD%u,0x%x. We did CMD12,0\n",
 				stop->opcode, stop->arg);
@@ -565,10 +569,12 @@ void tmio_mmc_do_data_irq(struct tmio_mmc_host *host)
 
 	schedule_work(&host->done);
 }
+EXPORT_SYMBOL_GPL(tmio_mmc_do_data_irq);
 
 static void tmio_mmc_data_irq(struct tmio_mmc_host *host, unsigned int stat)
 {
 	struct mmc_data *data;
+
 	spin_lock(&host->lock);
 	data = host->data;
 
@@ -613,8 +619,7 @@ out:
 	spin_unlock(&host->lock);
 }
 
-static void tmio_mmc_cmd_irq(struct tmio_mmc_host *host,
-	unsigned int stat)
+static void tmio_mmc_cmd_irq(struct tmio_mmc_host *host, unsigned int stat)
 {
 	struct mmc_command *cmd = host->cmd;
 	int i, addr;
@@ -675,7 +680,7 @@ out:
 }
 
 static bool __tmio_mmc_card_detect_irq(struct tmio_mmc_host *host,
-				      int ireg, int status)
+				       int ireg, int status)
 {
 	struct mmc_host *mmc = host->mmc;
 
@@ -693,14 +698,13 @@ static bool __tmio_mmc_card_detect_irq(struct tmio_mmc_host *host,
 	return false;
 }
 
-static bool __tmio_mmc_sdcard_irq(struct tmio_mmc_host *host,
-				 int ireg, int status)
+static bool __tmio_mmc_sdcard_irq(struct tmio_mmc_host *host, int ireg,
+				  int status)
 {
 	/* Command completion */
 	if (ireg & (TMIO_STAT_CMDRESPEND | TMIO_STAT_CMDTIMEOUT)) {
-		tmio_mmc_ack_mmc_irqs(host,
-			     TMIO_STAT_CMDRESPEND |
-			     TMIO_STAT_CMDTIMEOUT);
+		tmio_mmc_ack_mmc_irqs(host, TMIO_STAT_CMDRESPEND |
+				      TMIO_STAT_CMDTIMEOUT);
 		tmio_mmc_cmd_irq(host, status);
 		return true;
 	}
@@ -768,10 +772,10 @@ irqreturn_t tmio_mmc_irq(int irq, void *devid)
 
 	return IRQ_HANDLED;
 }
-EXPORT_SYMBOL(tmio_mmc_irq);
+EXPORT_SYMBOL_GPL(tmio_mmc_irq);
 
 static int tmio_mmc_start_data(struct tmio_mmc_host *host,
-	struct mmc_data *data)
+			       struct mmc_data *data)
 {
 	struct tmio_mmc_data *pdata = host->pdata;
 
@@ -826,7 +830,7 @@ static int tmio_mmc_execute_tuning(struct mmc_host *mmc, u32 opcode)
 
 	if (host->tap_num * 2 >= sizeof(host->taps) * BITS_PER_BYTE) {
 		dev_warn_once(&host->pdev->dev,
-		      "Too many taps, skipping tuning. Please consider updating size of taps field of tmio_mmc_host\n");
+			"Too many taps, skipping tuning. Please consider updating size of taps field of tmio_mmc_host\n");
 		goto out;
 	}
 
@@ -857,12 +861,43 @@ out:
 	return ret;
 }
 
+static void tmio_process_mrq(struct tmio_mmc_host *host,
+			     struct mmc_request *mrq)
+{
+	struct mmc_command *cmd;
+	int ret;
+
+	if (mrq->sbc && host->cmd != mrq->sbc) {
+		cmd = mrq->sbc;
+	} else {
+		cmd = mrq->cmd;
+		if (mrq->data) {
+			ret = tmio_mmc_start_data(host, mrq->data);
+			if (ret)
+				goto fail;
+		}
+	}
+
+	ret = tmio_mmc_start_command(host, cmd);
+	if (ret)
+		goto fail;
+
+	schedule_delayed_work(&host->delayed_reset_work,
+			      msecs_to_jiffies(CMDREQ_TIMEOUT));
+	return;
+
+fail:
+	host->force_pio = false;
+	host->mrq = NULL;
+	mrq->cmd->error = ret;
+	mmc_request_done(host->mmc, mrq);
+}
+
 /* Process requests from the MMC layer */
 static void tmio_mmc_request(struct mmc_host *mmc, struct mmc_request *mrq)
 {
 	struct tmio_mmc_host *host = mmc_priv(mmc);
 	unsigned long flags;
-	int ret;
 
 	spin_lock_irqsave(&host->lock, flags);
 
@@ -882,24 +917,54 @@ static void tmio_mmc_request(struct mmc_host *mmc, struct mmc_request *mrq)
 
 	spin_unlock_irqrestore(&host->lock, flags);
 
-	if (mrq->data) {
-		ret = tmio_mmc_start_data(host, mrq->data);
-		if (ret)
-			goto fail;
+	tmio_process_mrq(host, mrq);
+}
+
+static void tmio_mmc_finish_request(struct tmio_mmc_host *host)
+{
+	struct mmc_request *mrq;
+	unsigned long flags;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	mrq = host->mrq;
+	if (IS_ERR_OR_NULL(mrq)) {
+		spin_unlock_irqrestore(&host->lock, flags);
+		return;
+	}
+
+	/* If not SET_BLOCK_COUNT, clear old data */
+	if (host->cmd != mrq->sbc) {
+		host->cmd = NULL;
+		host->data = NULL;
+		host->force_pio = false;
+		host->mrq = NULL;
 	}
 
-	ret = tmio_mmc_start_command(host, mrq->cmd);
-	if (!ret) {
-		schedule_delayed_work(&host->delayed_reset_work,
-				      msecs_to_jiffies(CMDREQ_TIMEOUT));
+	cancel_delayed_work(&host->delayed_reset_work);
+
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	if (mrq->cmd->error || (mrq->data && mrq->data->error))
+		tmio_mmc_abort_dma(host);
+
+	if (host->check_scc_error)
+		host->check_scc_error(host);
+
+	/* If SET_BLOCK_COUNT, continue with main command */
+	if (host->mrq) {
+		tmio_process_mrq(host, mrq);
 		return;
 	}
 
-fail:
-	host->force_pio = false;
-	host->mrq = NULL;
-	mrq->cmd->error = ret;
-	mmc_request_done(mmc, mrq);
+	mmc_request_done(host->mmc, mrq);
+}
+
+static void tmio_mmc_done_work(struct work_struct *work)
+{
+	struct tmio_mmc_host *host = container_of(work, struct tmio_mmc_host,
+						  done);
+	tmio_mmc_finish_request(host);
 }
 
 static int tmio_mmc_clk_enable(struct tmio_mmc_host *host)
@@ -965,7 +1030,7 @@ static void tmio_mmc_power_off(struct tmio_mmc_host *host)
 }
 
 static void tmio_mmc_set_bus_width(struct tmio_mmc_host *host,
-				unsigned char bus_width)
+				   unsigned char bus_width)
 {
 	u16 reg = sd_ctrl_read16(host, CTL_SD_MEM_CARD_OPT)
 				& ~(CARD_OPT_WIDTH | CARD_OPT_WIDTH8);
@@ -1005,7 +1070,8 @@ static void tmio_mmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 			dev_dbg(dev,
 				"%s.%d: CMD%u active since %lu, now %lu!\n",
 				current->comm, task_pid_nr(current),
-				host->mrq->cmd->opcode, host->last_req_ts, jiffies);
+				host->mrq->cmd->opcode, host->last_req_ts,
+				jiffies);
 		}
 		spin_unlock_irqrestore(&host->lock, flags);
 
@@ -1052,6 +1118,7 @@ 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;
 
@@ -1108,6 +1175,7 @@ static void tmio_mmc_of_parse(struct platform_device *pdev,
 			      struct tmio_mmc_data *pdata)
 {
 	const struct device_node *np = pdev->dev.of_node;
+
 	if (!np)
 		return;
 
@@ -1131,16 +1199,17 @@ tmio_mmc_host_alloc(struct platform_device *pdev)
 
 	return host;
 }
-EXPORT_SYMBOL(tmio_mmc_host_alloc);
+EXPORT_SYMBOL_GPL(tmio_mmc_host_alloc);
 
 void tmio_mmc_host_free(struct tmio_mmc_host *host)
 {
 	mmc_free_host(host->mmc);
 }
-EXPORT_SYMBOL(tmio_mmc_host_free);
+EXPORT_SYMBOL_GPL(tmio_mmc_host_free);
 
 int tmio_mmc_host_probe(struct tmio_mmc_host *_host,
-			struct tmio_mmc_data *pdata)
+			struct tmio_mmc_data *pdata,
+			const struct tmio_mmc_dma_ops *dma_ops)
 {
 	struct platform_device *pdev = _host->pdev;
 	struct mmc_host *mmc = _host->mmc;
@@ -1177,7 +1246,8 @@ int tmio_mmc_host_probe(struct tmio_mmc_host *_host,
 		return -ENOMEM;
 
 	tmio_mmc_ops.card_busy = _host->card_busy;
-	tmio_mmc_ops.start_signal_voltage_switch = _host->start_signal_voltage_switch;
+	tmio_mmc_ops.start_signal_voltage_switch =
+		_host->start_signal_voltage_switch;
 	mmc->ops = &tmio_mmc_ops;
 
 	mmc->caps |= MMC_CAP_4_BIT_DATA | pdata->capabilities;
@@ -1221,6 +1291,10 @@ int tmio_mmc_host_probe(struct tmio_mmc_host *_host,
 	if (_host->native_hotplug)
 		pm_runtime_get_noresume(&pdev->dev);
 
+	_host->sdio_irq_enabled = false;
+	if (pdata->flags & TMIO_MMC_SDIO_IRQ)
+		_host->sdio_irq_mask = TMIO_SDIO_MASK_ALL;
+
 	tmio_mmc_clk_stop(_host);
 	tmio_mmc_reset(_host);
 
@@ -1237,13 +1311,6 @@ int tmio_mmc_host_probe(struct tmio_mmc_host *_host,
 
 	_host->sdcard_irq_mask &= ~irq_mask;
 
-	_host->sdio_irq_enabled = false;
-	if (pdata->flags & TMIO_MMC_SDIO_IRQ) {
-		_host->sdio_irq_mask = TMIO_SDIO_MASK_ALL;
-		sd_ctrl_write16(_host, CTL_SDIO_IRQ_MASK, _host->sdio_irq_mask);
-		sd_ctrl_write16(_host, CTL_TRANSACTION_CTL, 0x0001);
-	}
-
 	spin_lock_init(&_host->lock);
 	mutex_init(&_host->ios_lock);
 
@@ -1252,6 +1319,7 @@ int tmio_mmc_host_probe(struct tmio_mmc_host *_host,
 	INIT_WORK(&_host->done, tmio_mmc_done_work);
 
 	/* See if we also get DMA */
+	_host->dma_ops = dma_ops;
 	tmio_mmc_request_dma(_host, pdata);
 
 	pm_runtime_set_active(&pdev->dev);
@@ -1278,7 +1346,7 @@ int tmio_mmc_host_probe(struct tmio_mmc_host *_host,
 
 	return 0;
 }
-EXPORT_SYMBOL(tmio_mmc_host_probe);
+EXPORT_SYMBOL_GPL(tmio_mmc_host_probe);
 
 void tmio_mmc_host_remove(struct tmio_mmc_host *host)
 {
@@ -1303,7 +1371,7 @@ void tmio_mmc_host_remove(struct tmio_mmc_host *host)
 
 	tmio_mmc_clk_disable(host);
 }
-EXPORT_SYMBOL(tmio_mmc_host_remove);
+EXPORT_SYMBOL_GPL(tmio_mmc_host_remove);
 
 #ifdef CONFIG_PM
 int tmio_mmc_host_runtime_suspend(struct device *dev)
@@ -1320,7 +1388,7 @@ int tmio_mmc_host_runtime_suspend(struct device *dev)
 
 	return 0;
 }
-EXPORT_SYMBOL(tmio_mmc_host_runtime_suspend);
+EXPORT_SYMBOL_GPL(tmio_mmc_host_runtime_suspend);
 
 static bool tmio_mmc_can_retune(struct tmio_mmc_host *host)
 {
@@ -1345,7 +1413,7 @@ int tmio_mmc_host_runtime_resume(struct device *dev)
 
 	return 0;
 }
-EXPORT_SYMBOL(tmio_mmc_host_runtime_resume);
+EXPORT_SYMBOL_GPL(tmio_mmc_host_runtime_resume);
 #endif
 
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/mmc/host/vub300.c b/drivers/mmc/host/vub300.c
index c061e7c704be..fbeea1a491a6 100644
--- a/drivers/mmc/host/vub300.c
+++ b/drivers/mmc/host/vub300.c
@@ -2107,7 +2107,8 @@ static int vub300_probe(struct usb_interface *interface,
 	usb_string(udev, udev->descriptor.iSerialNumber, serial_number,
 		   sizeof(serial_number));
 	dev_info(&udev->dev, "probing VID:PID(%04X:%04X) %s %s %s\n",
-		 udev->descriptor.idVendor, udev->descriptor.idProduct,
+		 le16_to_cpu(udev->descriptor.idVendor),
+		 le16_to_cpu(udev->descriptor.idProduct),
 		 manufacturer, product, serial_number);
 	command_out_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!command_out_urb) {
diff --git a/drivers/mtd/mtd_blkdevs.c b/drivers/mtd/mtd_blkdevs.c
index 6b8d5cd7dbf6..f336a9b85576 100644
--- a/drivers/mtd/mtd_blkdevs.c
+++ b/drivers/mtd/mtd_blkdevs.c
@@ -73,7 +73,7 @@ static void blktrans_dev_put(struct mtd_blktrans_dev *dev)
 }
 
 
-static int do_blktrans_request(struct mtd_blktrans_ops *tr,
+static blk_status_t do_blktrans_request(struct mtd_blktrans_ops *tr,
 			       struct mtd_blktrans_dev *dev,
 			       struct request *req)
 {
@@ -84,33 +84,37 @@ static int do_blktrans_request(struct mtd_blktrans_ops *tr,
 	nsect = blk_rq_cur_bytes(req) >> tr->blkshift;
 	buf = bio_data(req->bio);
 
-	if (req_op(req) == REQ_OP_FLUSH)
-		return tr->flush(dev);
+	if (req_op(req) == REQ_OP_FLUSH) {
+		if (tr->flush(dev))
+			return BLK_STS_IOERR;
+		return BLK_STS_OK;
+	}
 
 	if (blk_rq_pos(req) + blk_rq_cur_sectors(req) >
 	    get_capacity(req->rq_disk))
-		return -EIO;
+		return BLK_STS_IOERR;
 
 	switch (req_op(req)) {
 	case REQ_OP_DISCARD:
-		return tr->discard(dev, block, nsect);
+		if (tr->discard(dev, block, nsect))
+			return BLK_STS_IOERR;
+		return BLK_STS_OK;
 	case REQ_OP_READ:
 		for (; nsect > 0; nsect--, block++, buf += tr->blksize)
 			if (tr->readsect(dev, block, buf))
-				return -EIO;
+				return BLK_STS_IOERR;
 		rq_flush_dcache_pages(req);
-		return 0;
+		return BLK_STS_OK;
 	case REQ_OP_WRITE:
 		if (!tr->writesect)
-			return -EIO;
+			return BLK_STS_IOERR;
 
 		rq_flush_dcache_pages(req);
 		for (; nsect > 0; nsect--, block++, buf += tr->blksize)
 			if (tr->writesect(dev, block, buf))
-				return -EIO;
-		return 0;
+				return BLK_STS_IOERR;
 	default:
-		return -EIO;
+		return BLK_STS_IOERR;
 	}
 }
 
@@ -132,7 +136,7 @@ static void mtd_blktrans_work(struct work_struct *work)
 	spin_lock_irq(rq->queue_lock);
 
 	while (1) {
-		int res;
+		blk_status_t res;
 
 		dev->bg_stop = false;
 		if (!req && !(req = blk_fetch_request(rq))) {
@@ -178,7 +182,7 @@ static void mtd_blktrans_request(struct request_queue *rq)
 
 	if (!dev)
 		while ((req = blk_fetch_request(rq)) != NULL)
-			__blk_end_request_all(req, -ENODEV);
+			__blk_end_request_all(req, BLK_STS_IOERR);
 	else
 		queue_work(dev->wq, &dev->work);
 }
@@ -413,6 +417,7 @@ int add_mtd_blktrans_dev(struct mtd_blktrans_dev *new)
 	new->rq->queuedata = new;
 	blk_queue_logical_block_size(new->rq, tr->blksize);
 
+	blk_queue_bounce_limit(new->rq, BLK_BOUNCE_HIGH);
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, new->rq);
 	queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, new->rq);
 
diff --git a/drivers/mtd/nand/jz4740_nand.c b/drivers/mtd/nand/jz4740_nand.c
index 5551c36adbdf..0d06a1f07d82 100644
--- a/drivers/mtd/nand/jz4740_nand.c
+++ b/drivers/mtd/nand/jz4740_nand.c
@@ -25,7 +25,6 @@
 
 #include <linux/gpio.h>
 
-#include <asm/mach-jz4740/gpio.h>
 #include <asm/mach-jz4740/jz4740_nand.h>
 
 #define JZ_REG_NAND_CTRL	0x50
@@ -310,34 +309,20 @@ static int jz_nand_detect_bank(struct platform_device *pdev,
 			       uint8_t *nand_dev_id)
 {
 	int ret;
-	int gpio;
-	char gpio_name[9];
 	char res_name[6];
 	uint32_t ctrl;
 	struct nand_chip *chip = &nand->chip;
 	struct mtd_info *mtd = nand_to_mtd(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;
+		return ret;
 
 	/* 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);
@@ -377,12 +362,8 @@ 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;
 }
 
@@ -503,7 +484,6 @@ err_nand_release:
 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]);
 	}
@@ -530,7 +510,6 @@ static int jz_nand_remove(struct platform_device *pdev)
 		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);
 		}
 	}
 
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index b1dd12729f19..bf8486c406d3 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -502,10 +502,12 @@ static int nand_default_block_markbad(struct mtd_info *mtd, loff_t ofs)
  * specify how to write bad block markers to OOB (chip->block_markbad).
  *
  * We try operations in the following order:
+ *
  *  (1) erase the affected block, to allow OOB marker to be written cleanly
  *  (2) write bad block marker to OOB area of affected block (unless flag
  *      NAND_BBT_NO_OOB_BBM is present)
  *  (3) update the BBT
+ *
  * Note that we retain the first error encountered in (2) or (3), finish the
  * procedures, and dump the error in the end.
 */
@@ -1219,9 +1221,10 @@ int nand_reset(struct nand_chip *chip, int chipnr)
  * @mtd: mtd info
  * @ofs: offset to start unlock from
  * @len: length to unlock
- * @invert: when = 0, unlock the range of blocks within the lower and
+ * @invert:
+ *        - when = 0, unlock the range of blocks within the lower and
  *                    upper boundary address
- *          when = 1, unlock the range of blocks outside the boundaries
+ *        - when = 1, unlock the range of blocks outside the boundaries
  *                    of the lower and upper boundary address
  *
  * Returs unlock status.
diff --git a/drivers/mtd/ubi/block.c b/drivers/mtd/ubi/block.c
index 5497e65439df..c3963f880448 100644
--- a/drivers/mtd/ubi/block.c
+++ b/drivers/mtd/ubi/block.c
@@ -313,10 +313,10 @@ static void ubiblock_do_work(struct work_struct *work)
 	ret = ubiblock_read(pdu);
 	rq_flush_dcache_pages(req);
 
-	blk_mq_end_request(req, ret);
+	blk_mq_end_request(req, errno_to_blk_status(ret));
 }
 
-static int ubiblock_queue_rq(struct blk_mq_hw_ctx *hctx,
+static blk_status_t ubiblock_queue_rq(struct blk_mq_hw_ctx *hctx,
 			     const struct blk_mq_queue_data *bd)
 {
 	struct request *req = bd->rq;
@@ -327,9 +327,9 @@ static int ubiblock_queue_rq(struct blk_mq_hw_ctx *hctx,
 	case REQ_OP_READ:
 		ubi_sgl_init(&pdu->usgl);
 		queue_work(dev->wq, &pdu->work);
-		return BLK_MQ_RQ_QUEUE_OK;
+		return BLK_STS_OK;
 	default:
-		return BLK_MQ_RQ_QUEUE_ERROR;
+		return BLK_STS_IOERR;
 	}
 
 }
diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index 93e5d251a9e4..d854521962ef 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -104,23 +104,25 @@ DEFINE_MUTEX(ubi_devices_mutex);
 static DEFINE_SPINLOCK(ubi_devices_lock);
 
 /* "Show" method for files in '/<sysfs>/class/ubi/' */
-static ssize_t ubi_version_show(struct class *class,
-				struct class_attribute *attr, char *buf)
+/* UBI version attribute ('/<sysfs>/class/ubi/version') */
+static ssize_t version_show(struct class *class, struct class_attribute *attr,
+			    char *buf)
 {
 	return sprintf(buf, "%d\n", UBI_VERSION);
 }
+static CLASS_ATTR_RO(version);
 
-/* UBI version attribute ('/<sysfs>/class/ubi/version') */
-static struct class_attribute ubi_class_attrs[] = {
-	__ATTR(version, S_IRUGO, ubi_version_show, NULL),
-	__ATTR_NULL
+static struct attribute *ubi_class_attrs[] = {
+	&class_attr_version.attr,
+	NULL,
 };
+ATTRIBUTE_GROUPS(ubi_class);
 
 /* Root UBI "class" object (corresponds to '/<sysfs>/class/ubi/') */
 struct class ubi_class = {
 	.name		= UBI_NAME_STR,
 	.owner		= THIS_MODULE,
-	.class_attrs	= ubi_class_attrs,
+	.class_groups	= ubi_class_groups,
 };
 
 static ssize_t dev_attribute_show(struct device *dev,
diff --git a/drivers/mux/Kconfig b/drivers/mux/Kconfig
new file mode 100644
index 000000000000..7c754a0f14bb
--- /dev/null
+++ b/drivers/mux/Kconfig
@@ -0,0 +1,59 @@
+#
+# Multiplexer devices
+#
+
+menuconfig MULTIPLEXER
+	tristate "Multiplexer subsystem"
+	help
+	  Multiplexer controller subsystem. Multiplexers are used in a
+	  variety of settings, and this subsystem abstracts their use
+	  so that the rest of the kernel sees a common interface. When
+	  multiple parallel multiplexers are controlled by one single
+	  multiplexer controller, this subsystem also coordinates the
+	  multiplexer accesses.
+
+	  To compile the subsystem as a module, choose M here: the module will
+	  be called mux-core.
+
+if MULTIPLEXER
+
+config MUX_ADG792A
+	tristate "Analog Devices ADG792A/ADG792G Multiplexers"
+	depends on I2C
+	help
+	  ADG792A and ADG792G Wide Bandwidth Triple 4:1 Multiplexers
+
+	  The driver supports both operating the three multiplexers in
+	  parallel and operating them independently.
+
+	  To compile the driver as a module, choose M here: the module will
+	  be called mux-adg792a.
+
+config MUX_GPIO
+	tristate "GPIO-controlled Multiplexer"
+	depends on GPIOLIB || COMPILE_TEST
+	help
+	  GPIO-controlled Multiplexer controller.
+
+	  The driver builds a single multiplexer controller using a number
+	  of gpio pins. For N pins, there will be 2^N possible multiplexer
+	  states. The GPIO pins can be connected (by the hardware) to several
+	  multiplexers, which in that case will be operated in parallel.
+
+	  To compile the driver as a module, choose M here: the module will
+	  be called mux-gpio.
+
+config MUX_MMIO
+	tristate "MMIO register bitfield-controlled Multiplexer"
+	depends on (OF && MFD_SYSCON) || COMPILE_TEST
+	help
+	  MMIO register bitfield-controlled Multiplexer controller.
+
+	  The driver builds multiplexer controllers for bitfields in a syscon
+	  register. For N bit wide bitfields, there will be 2^N possible
+	  multiplexer states.
+
+	  To compile the driver as a module, choose M here: the module will
+	  be called mux-mmio.
+
+endif
diff --git a/drivers/mux/Makefile b/drivers/mux/Makefile
new file mode 100644
index 000000000000..6bac5b0fea13
--- /dev/null
+++ b/drivers/mux/Makefile
@@ -0,0 +1,8 @@
+#
+# Makefile for multiplexer devices.
+#
+
+obj-$(CONFIG_MULTIPLEXER)	+= mux-core.o
+obj-$(CONFIG_MUX_ADG792A)	+= mux-adg792a.o
+obj-$(CONFIG_MUX_GPIO)		+= mux-gpio.o
+obj-$(CONFIG_MUX_MMIO)		+= mux-mmio.o
diff --git a/drivers/mux/mux-adg792a.c b/drivers/mux/mux-adg792a.c
new file mode 100644
index 000000000000..12aa221ab90d
--- /dev/null
+++ b/drivers/mux/mux-adg792a.c
@@ -0,0 +1,157 @@
+/*
+ * Multiplexer driver for Analog Devices ADG792A/G Triple 4:1 mux
+ *
+ * Copyright (C) 2017 Axentia Technologies AB
+ *
+ * Author: Peter Rosin <peda@axentia.se>
+ *
+ * This program is free software; you can 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/mux/driver.h>
+#include <linux/property.h>
+
+#define ADG792A_LDSW		BIT(0)
+#define ADG792A_RESETB		BIT(1)
+#define ADG792A_DISABLE(mux)	(0x50 | (mux))
+#define ADG792A_DISABLE_ALL	(0x5f)
+#define ADG792A_MUX(mux, state)	(0xc0 | (((mux) + 1) << 2) | (state))
+#define ADG792A_MUX_ALL(state)	(0xc0 | (state))
+
+static int adg792a_write_cmd(struct i2c_client *i2c, u8 cmd, int reset)
+{
+	u8 data = ADG792A_RESETB | ADG792A_LDSW;
+
+	/* ADG792A_RESETB is active low, the chip resets when it is zero. */
+	if (reset)
+		data &= ~ADG792A_RESETB;
+
+	return i2c_smbus_write_byte_data(i2c, cmd, data);
+}
+
+static int adg792a_set(struct mux_control *mux, int state)
+{
+	struct i2c_client *i2c = to_i2c_client(mux->chip->dev.parent);
+	u8 cmd;
+
+	if (mux->chip->controllers == 1) {
+		/* parallel mux controller operation */
+		if (state == MUX_IDLE_DISCONNECT)
+			cmd = ADG792A_DISABLE_ALL;
+		else
+			cmd = ADG792A_MUX_ALL(state);
+	} else {
+		unsigned int controller = mux_control_get_index(mux);
+
+		if (state == MUX_IDLE_DISCONNECT)
+			cmd = ADG792A_DISABLE(controller);
+		else
+			cmd = ADG792A_MUX(controller, state);
+	}
+
+	return adg792a_write_cmd(i2c, cmd, 0);
+}
+
+static const struct mux_control_ops adg792a_ops = {
+	.set = adg792a_set,
+};
+
+static int adg792a_probe(struct i2c_client *i2c,
+			 const struct i2c_device_id *id)
+{
+	struct device *dev = &i2c->dev;
+	struct mux_chip *mux_chip;
+	s32 idle_state[3];
+	u32 cells;
+	int ret;
+	int i;
+
+	if (!i2c_check_functionality(i2c->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	ret = device_property_read_u32(dev, "#mux-control-cells", &cells);
+	if (ret < 0)
+		return ret;
+	if (cells >= 2)
+		return -EINVAL;
+
+	mux_chip = devm_mux_chip_alloc(dev, cells ? 3 : 1, 0);
+	if (IS_ERR(mux_chip))
+		return PTR_ERR(mux_chip);
+
+	mux_chip->ops = &adg792a_ops;
+
+	ret = adg792a_write_cmd(i2c, ADG792A_DISABLE_ALL, 1);
+	if (ret < 0)
+		return ret;
+
+	ret = device_property_read_u32_array(dev, "idle-state",
+					     (u32 *)idle_state,
+					     mux_chip->controllers);
+	if (ret < 0) {
+		idle_state[0] = MUX_IDLE_AS_IS;
+		idle_state[1] = MUX_IDLE_AS_IS;
+		idle_state[2] = MUX_IDLE_AS_IS;
+	}
+
+	for (i = 0; i < mux_chip->controllers; ++i) {
+		struct mux_control *mux = &mux_chip->mux[i];
+
+		mux->states = 4;
+
+		switch (idle_state[i]) {
+		case MUX_IDLE_DISCONNECT:
+		case MUX_IDLE_AS_IS:
+		case 0 ... 4:
+			mux->idle_state = idle_state[i];
+			break;
+		default:
+			dev_err(dev, "invalid idle-state %d\n", idle_state[i]);
+			return -EINVAL;
+		}
+	}
+
+	ret = devm_mux_chip_register(dev, mux_chip);
+	if (ret < 0)
+		return ret;
+
+	if (cells)
+		dev_info(dev, "3x single pole quadruple throw muxes registered\n");
+	else
+		dev_info(dev, "triple pole quadruple throw mux registered\n");
+
+	return 0;
+}
+
+static const struct i2c_device_id adg792a_id[] = {
+	{ .name = "adg792a", },
+	{ .name = "adg792g", },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, adg792a_id);
+
+static const struct of_device_id adg792a_of_match[] = {
+	{ .compatible = "adi,adg792a", },
+	{ .compatible = "adi,adg792g", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, adg792a_of_match);
+
+static struct i2c_driver adg792a_driver = {
+	.driver		= {
+		.name		= "adg792a",
+		.of_match_table = of_match_ptr(adg792a_of_match),
+	},
+	.probe		= adg792a_probe,
+	.id_table	= adg792a_id,
+};
+module_i2c_driver(adg792a_driver);
+
+MODULE_DESCRIPTION("Analog Devices ADG792A/G Triple 4:1 mux driver");
+MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mux/mux-core.c b/drivers/mux/mux-core.c
new file mode 100644
index 000000000000..90b8995f07cb
--- /dev/null
+++ b/drivers/mux/mux-core.c
@@ -0,0 +1,547 @@
+/*
+ * Multiplexer subsystem
+ *
+ * Copyright (C) 2017 Axentia Technologies AB
+ *
+ * Author: Peter Rosin <peda@axentia.se>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#define pr_fmt(fmt) "mux-core: " fmt
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/export.h>
+#include <linux/idr.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/mux/consumer.h>
+#include <linux/mux/driver.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/slab.h>
+
+/*
+ * The idle-as-is "state" is not an actual state that may be selected, it
+ * only implies that the state should not be changed. So, use that state
+ * as indication that the cached state of the multiplexer is unknown.
+ */
+#define MUX_CACHE_UNKNOWN MUX_IDLE_AS_IS
+
+static struct class mux_class = {
+	.name = "mux",
+	.owner = THIS_MODULE,
+};
+
+static DEFINE_IDA(mux_ida);
+
+static int __init mux_init(void)
+{
+	ida_init(&mux_ida);
+	return class_register(&mux_class);
+}
+
+static void __exit mux_exit(void)
+{
+	class_register(&mux_class);
+	ida_destroy(&mux_ida);
+}
+
+static void mux_chip_release(struct device *dev)
+{
+	struct mux_chip *mux_chip = to_mux_chip(dev);
+
+	ida_simple_remove(&mux_ida, mux_chip->id);
+	kfree(mux_chip);
+}
+
+static struct device_type mux_type = {
+	.name = "mux-chip",
+	.release = mux_chip_release,
+};
+
+/**
+ * mux_chip_alloc() - Allocate a mux-chip.
+ * @dev: The parent device implementing the mux interface.
+ * @controllers: The number of mux controllers to allocate for this chip.
+ * @sizeof_priv: Size of extra memory area for private use by the caller.
+ *
+ * After allocating the mux-chip with the desired number of mux controllers
+ * but before registering the chip, the mux driver is required to configure
+ * the number of valid mux states in the mux_chip->mux[N].states members and
+ * the desired idle state in the returned mux_chip->mux[N].idle_state members.
+ * The default idle state is MUX_IDLE_AS_IS. The mux driver also needs to
+ * provide a pointer to the operations struct in the mux_chip->ops member
+ * before registering the mux-chip with mux_chip_register.
+ *
+ * Return: A pointer to the new mux-chip, or an ERR_PTR with a negative errno.
+ */
+struct mux_chip *mux_chip_alloc(struct device *dev,
+				unsigned int controllers, size_t sizeof_priv)
+{
+	struct mux_chip *mux_chip;
+	int i;
+
+	if (WARN_ON(!dev || !controllers))
+		return ERR_PTR(-EINVAL);
+
+	mux_chip = kzalloc(sizeof(*mux_chip) +
+			   controllers * sizeof(*mux_chip->mux) +
+			   sizeof_priv, GFP_KERNEL);
+	if (!mux_chip)
+		return ERR_PTR(-ENOMEM);
+
+	mux_chip->mux = (struct mux_control *)(mux_chip + 1);
+	mux_chip->dev.class = &mux_class;
+	mux_chip->dev.type = &mux_type;
+	mux_chip->dev.parent = dev;
+	mux_chip->dev.of_node = dev->of_node;
+	dev_set_drvdata(&mux_chip->dev, mux_chip);
+
+	mux_chip->id = ida_simple_get(&mux_ida, 0, 0, GFP_KERNEL);
+	if (mux_chip->id < 0) {
+		int err = mux_chip->id;
+
+		pr_err("muxchipX failed to get a device id\n");
+		kfree(mux_chip);
+		return ERR_PTR(err);
+	}
+	dev_set_name(&mux_chip->dev, "muxchip%d", mux_chip->id);
+
+	mux_chip->controllers = controllers;
+	for (i = 0; i < controllers; ++i) {
+		struct mux_control *mux = &mux_chip->mux[i];
+
+		mux->chip = mux_chip;
+		sema_init(&mux->lock, 1);
+		mux->cached_state = MUX_CACHE_UNKNOWN;
+		mux->idle_state = MUX_IDLE_AS_IS;
+	}
+
+	device_initialize(&mux_chip->dev);
+
+	return mux_chip;
+}
+EXPORT_SYMBOL_GPL(mux_chip_alloc);
+
+static int mux_control_set(struct mux_control *mux, int state)
+{
+	int ret = mux->chip->ops->set(mux, state);
+
+	mux->cached_state = ret < 0 ? MUX_CACHE_UNKNOWN : state;
+
+	return ret;
+}
+
+/**
+ * mux_chip_register() - Register a mux-chip, thus readying the controllers
+ *			 for use.
+ * @mux_chip: The mux-chip to register.
+ *
+ * Do not retry registration of the same mux-chip on failure. You should
+ * instead put it away with mux_chip_free() and allocate a new one, if you
+ * for some reason would like to retry registration.
+ *
+ * Return: Zero on success or a negative errno on error.
+ */
+int mux_chip_register(struct mux_chip *mux_chip)
+{
+	int i;
+	int ret;
+
+	for (i = 0; i < mux_chip->controllers; ++i) {
+		struct mux_control *mux = &mux_chip->mux[i];
+
+		if (mux->idle_state == mux->cached_state)
+			continue;
+
+		ret = mux_control_set(mux, mux->idle_state);
+		if (ret < 0) {
+			dev_err(&mux_chip->dev, "unable to set idle state\n");
+			return ret;
+		}
+	}
+
+	ret = device_add(&mux_chip->dev);
+	if (ret < 0)
+		dev_err(&mux_chip->dev,
+			"device_add failed in %s: %d\n", __func__, ret);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mux_chip_register);
+
+/**
+ * mux_chip_unregister() - Take the mux-chip off-line.
+ * @mux_chip: The mux-chip to unregister.
+ *
+ * mux_chip_unregister() reverses the effects of mux_chip_register().
+ * But not completely, you should not try to call mux_chip_register()
+ * on a mux-chip that has been registered before.
+ */
+void mux_chip_unregister(struct mux_chip *mux_chip)
+{
+	device_del(&mux_chip->dev);
+}
+EXPORT_SYMBOL_GPL(mux_chip_unregister);
+
+/**
+ * mux_chip_free() - Free the mux-chip for good.
+ * @mux_chip: The mux-chip to free.
+ *
+ * mux_chip_free() reverses the effects of mux_chip_alloc().
+ */
+void mux_chip_free(struct mux_chip *mux_chip)
+{
+	if (!mux_chip)
+		return;
+
+	put_device(&mux_chip->dev);
+}
+EXPORT_SYMBOL_GPL(mux_chip_free);
+
+static void devm_mux_chip_release(struct device *dev, void *res)
+{
+	struct mux_chip *mux_chip = *(struct mux_chip **)res;
+
+	mux_chip_free(mux_chip);
+}
+
+/**
+ * devm_mux_chip_alloc() - Resource-managed version of mux_chip_alloc().
+ * @dev: The parent device implementing the mux interface.
+ * @controllers: The number of mux controllers to allocate for this chip.
+ * @sizeof_priv: Size of extra memory area for private use by the caller.
+ *
+ * See mux_chip_alloc() for more details.
+ *
+ * Return: A pointer to the new mux-chip, or an ERR_PTR with a negative errno.
+ */
+struct mux_chip *devm_mux_chip_alloc(struct device *dev,
+				     unsigned int controllers,
+				     size_t sizeof_priv)
+{
+	struct mux_chip **ptr, *mux_chip;
+
+	ptr = devres_alloc(devm_mux_chip_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	mux_chip = mux_chip_alloc(dev, controllers, sizeof_priv);
+	if (IS_ERR(mux_chip)) {
+		devres_free(ptr);
+		return mux_chip;
+	}
+
+	*ptr = mux_chip;
+	devres_add(dev, ptr);
+
+	return mux_chip;
+}
+EXPORT_SYMBOL_GPL(devm_mux_chip_alloc);
+
+static void devm_mux_chip_reg_release(struct device *dev, void *res)
+{
+	struct mux_chip *mux_chip = *(struct mux_chip **)res;
+
+	mux_chip_unregister(mux_chip);
+}
+
+/**
+ * devm_mux_chip_register() - Resource-managed version mux_chip_register().
+ * @dev: The parent device implementing the mux interface.
+ * @mux_chip: The mux-chip to register.
+ *
+ * See mux_chip_register() for more details.
+ *
+ * Return: Zero on success or a negative errno on error.
+ */
+int devm_mux_chip_register(struct device *dev,
+			   struct mux_chip *mux_chip)
+{
+	struct mux_chip **ptr;
+	int res;
+
+	ptr = devres_alloc(devm_mux_chip_reg_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return -ENOMEM;
+
+	res = mux_chip_register(mux_chip);
+	if (res) {
+		devres_free(ptr);
+		return res;
+	}
+
+	*ptr = mux_chip;
+	devres_add(dev, ptr);
+
+	return res;
+}
+EXPORT_SYMBOL_GPL(devm_mux_chip_register);
+
+/**
+ * mux_control_states() - Query the number of multiplexer states.
+ * @mux: The mux-control to query.
+ *
+ * Return: The number of multiplexer states.
+ */
+unsigned int mux_control_states(struct mux_control *mux)
+{
+	return mux->states;
+}
+EXPORT_SYMBOL_GPL(mux_control_states);
+
+/*
+ * The mux->lock must be down when calling this function.
+ */
+static int __mux_control_select(struct mux_control *mux, int state)
+{
+	int ret;
+
+	if (WARN_ON(state < 0 || state >= mux->states))
+		return -EINVAL;
+
+	if (mux->cached_state == state)
+		return 0;
+
+	ret = mux_control_set(mux, state);
+	if (ret >= 0)
+		return 0;
+
+	/* The mux update failed, try to revert if appropriate... */
+	if (mux->idle_state != MUX_IDLE_AS_IS)
+		mux_control_set(mux, mux->idle_state);
+
+	return ret;
+}
+
+/**
+ * mux_control_select() - Select the given multiplexer state.
+ * @mux: The mux-control to request a change of state from.
+ * @state: The new requested state.
+ *
+ * On successfully selecting the mux-control state, it will be locked until
+ * there is a call to mux_control_deselect(). If the mux-control is already
+ * selected when mux_control_select() is called, the caller will be blocked
+ * until mux_control_deselect() is called (by someone else).
+ *
+ * Therefore, make sure to call mux_control_deselect() when the operation is
+ * complete and the mux-control is free for others to use, but do not call
+ * mux_control_deselect() if mux_control_select() fails.
+ *
+ * Return: 0 when the mux-control state has the requested state or a negative
+ * errno on error.
+ */
+int mux_control_select(struct mux_control *mux, unsigned int state)
+{
+	int ret;
+
+	ret = down_killable(&mux->lock);
+	if (ret < 0)
+		return ret;
+
+	ret = __mux_control_select(mux, state);
+
+	if (ret < 0)
+		up(&mux->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mux_control_select);
+
+/**
+ * mux_control_try_select() - Try to select the given multiplexer state.
+ * @mux: The mux-control to request a change of state from.
+ * @state: The new requested state.
+ *
+ * On successfully selecting the mux-control state, it will be locked until
+ * mux_control_deselect() called.
+ *
+ * Therefore, make sure to call mux_control_deselect() when the operation is
+ * complete and the mux-control is free for others to use, but do not call
+ * mux_control_deselect() if mux_control_try_select() fails.
+ *
+ * Return: 0 when the mux-control state has the requested state or a negative
+ * errno on error. Specifically -EBUSY if the mux-control is contended.
+ */
+int mux_control_try_select(struct mux_control *mux, unsigned int state)
+{
+	int ret;
+
+	if (down_trylock(&mux->lock))
+		return -EBUSY;
+
+	ret = __mux_control_select(mux, state);
+
+	if (ret < 0)
+		up(&mux->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mux_control_try_select);
+
+/**
+ * mux_control_deselect() - Deselect the previously selected multiplexer state.
+ * @mux: The mux-control to deselect.
+ *
+ * It is required that a single call is made to mux_control_deselect() for
+ * each and every successful call made to either of mux_control_select() or
+ * mux_control_try_select().
+ *
+ * Return: 0 on success and a negative errno on error. An error can only
+ * occur if the mux has an idle state. Note that even if an error occurs, the
+ * mux-control is unlocked and is thus free for the next access.
+ */
+int mux_control_deselect(struct mux_control *mux)
+{
+	int ret = 0;
+
+	if (mux->idle_state != MUX_IDLE_AS_IS &&
+	    mux->idle_state != mux->cached_state)
+		ret = mux_control_set(mux, mux->idle_state);
+
+	up(&mux->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mux_control_deselect);
+
+static int of_dev_node_match(struct device *dev, const void *data)
+{
+	return dev->of_node == data;
+}
+
+static struct mux_chip *of_find_mux_chip_by_node(struct device_node *np)
+{
+	struct device *dev;
+
+	dev = class_find_device(&mux_class, NULL, np, of_dev_node_match);
+
+	return dev ? to_mux_chip(dev) : NULL;
+}
+
+/**
+ * mux_control_get() - Get the mux-control for a device.
+ * @dev: The device that needs a mux-control.
+ * @mux_name: The name identifying the mux-control.
+ *
+ * Return: A pointer to the mux-control, or an ERR_PTR with a negative errno.
+ */
+struct mux_control *mux_control_get(struct device *dev, const char *mux_name)
+{
+	struct device_node *np = dev->of_node;
+	struct of_phandle_args args;
+	struct mux_chip *mux_chip;
+	unsigned int controller;
+	int index = 0;
+	int ret;
+
+	if (mux_name) {
+		index = of_property_match_string(np, "mux-control-names",
+						 mux_name);
+		if (index < 0) {
+			dev_err(dev, "mux controller '%s' not found\n",
+				mux_name);
+			return ERR_PTR(index);
+		}
+	}
+
+	ret = of_parse_phandle_with_args(np,
+					 "mux-controls", "#mux-control-cells",
+					 index, &args);
+	if (ret) {
+		dev_err(dev, "%s: failed to get mux-control %s(%i)\n",
+			np->full_name, mux_name ?: "", index);
+		return ERR_PTR(ret);
+	}
+
+	mux_chip = of_find_mux_chip_by_node(args.np);
+	of_node_put(args.np);
+	if (!mux_chip)
+		return ERR_PTR(-EPROBE_DEFER);
+
+	if (args.args_count > 1 ||
+	    (!args.args_count && (mux_chip->controllers > 1))) {
+		dev_err(dev, "%s: wrong #mux-control-cells for %s\n",
+			np->full_name, args.np->full_name);
+		return ERR_PTR(-EINVAL);
+	}
+
+	controller = 0;
+	if (args.args_count)
+		controller = args.args[0];
+
+	if (controller >= mux_chip->controllers) {
+		dev_err(dev, "%s: bad mux controller %u specified in %s\n",
+			np->full_name, controller, args.np->full_name);
+		return ERR_PTR(-EINVAL);
+	}
+
+	get_device(&mux_chip->dev);
+	return &mux_chip->mux[controller];
+}
+EXPORT_SYMBOL_GPL(mux_control_get);
+
+/**
+ * mux_control_put() - Put away the mux-control for good.
+ * @mux: The mux-control to put away.
+ *
+ * mux_control_put() reverses the effects of mux_control_get().
+ */
+void mux_control_put(struct mux_control *mux)
+{
+	put_device(&mux->chip->dev);
+}
+EXPORT_SYMBOL_GPL(mux_control_put);
+
+static void devm_mux_control_release(struct device *dev, void *res)
+{
+	struct mux_control *mux = *(struct mux_control **)res;
+
+	mux_control_put(mux);
+}
+
+/**
+ * devm_mux_control_get() - Get the mux-control for a device, with resource
+ *			    management.
+ * @dev: The device that needs a mux-control.
+ * @mux_name: The name identifying the mux-control.
+ *
+ * Return: Pointer to the mux-control, or an ERR_PTR with a negative errno.
+ */
+struct mux_control *devm_mux_control_get(struct device *dev,
+					 const char *mux_name)
+{
+	struct mux_control **ptr, *mux;
+
+	ptr = devres_alloc(devm_mux_control_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	mux = mux_control_get(dev, mux_name);
+	if (IS_ERR(mux)) {
+		devres_free(ptr);
+		return mux;
+	}
+
+	*ptr = mux;
+	devres_add(dev, ptr);
+
+	return mux;
+}
+EXPORT_SYMBOL_GPL(devm_mux_control_get);
+
+/*
+ * Using subsys_initcall instead of module_init here to try to ensure - for
+ * the non-modular case - that the subsystem is initialized when mux consumers
+ * and mux controllers start to use it.
+ * For the modular case, the ordering is ensured with module dependencies.
+ */
+subsys_initcall(mux_init);
+module_exit(mux_exit);
+
+MODULE_DESCRIPTION("Multiplexer subsystem");
+MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mux/mux-gpio.c b/drivers/mux/mux-gpio.c
new file mode 100644
index 000000000000..468bf1709606
--- /dev/null
+++ b/drivers/mux/mux-gpio.c
@@ -0,0 +1,114 @@
+/*
+ * GPIO-controlled multiplexer driver
+ *
+ * Copyright (C) 2017 Axentia Technologies AB
+ *
+ * Author: Peter Rosin <peda@axentia.se>
+ *
+ * This program is free software; you can 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/consumer.h>
+#include <linux/module.h>
+#include <linux/mux/driver.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/property.h>
+
+struct mux_gpio {
+	struct gpio_descs *gpios;
+	int *val;
+};
+
+static int mux_gpio_set(struct mux_control *mux, int state)
+{
+	struct mux_gpio *mux_gpio = mux_chip_priv(mux->chip);
+	int i;
+
+	for (i = 0; i < mux_gpio->gpios->ndescs; i++)
+		mux_gpio->val[i] = (state >> i) & 1;
+
+	gpiod_set_array_value_cansleep(mux_gpio->gpios->ndescs,
+				       mux_gpio->gpios->desc,
+				       mux_gpio->val);
+
+	return 0;
+}
+
+static const struct mux_control_ops mux_gpio_ops = {
+	.set = mux_gpio_set,
+};
+
+static const struct of_device_id mux_gpio_dt_ids[] = {
+	{ .compatible = "gpio-mux", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mux_gpio_dt_ids);
+
+static int mux_gpio_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct mux_chip *mux_chip;
+	struct mux_gpio *mux_gpio;
+	int pins;
+	s32 idle_state;
+	int ret;
+
+	pins = gpiod_count(dev, "mux");
+	if (pins < 0)
+		return pins;
+
+	mux_chip = devm_mux_chip_alloc(dev, 1, sizeof(*mux_gpio) +
+				       pins * sizeof(*mux_gpio->val));
+	if (IS_ERR(mux_chip))
+		return PTR_ERR(mux_chip);
+
+	mux_gpio = mux_chip_priv(mux_chip);
+	mux_gpio->val = (int *)(mux_gpio + 1);
+	mux_chip->ops = &mux_gpio_ops;
+
+	mux_gpio->gpios = devm_gpiod_get_array(dev, "mux", GPIOD_OUT_LOW);
+	if (IS_ERR(mux_gpio->gpios)) {
+		ret = PTR_ERR(mux_gpio->gpios);
+		if (ret != -EPROBE_DEFER)
+			dev_err(dev, "failed to get gpios\n");
+		return ret;
+	}
+	WARN_ON(pins != mux_gpio->gpios->ndescs);
+	mux_chip->mux->states = 1 << pins;
+
+	ret = device_property_read_u32(dev, "idle-state", (u32 *)&idle_state);
+	if (ret >= 0 && idle_state != MUX_IDLE_AS_IS) {
+		if (idle_state < 0 || idle_state >= mux_chip->mux->states) {
+			dev_err(dev, "invalid idle-state %u\n", idle_state);
+			return -EINVAL;
+		}
+
+		mux_chip->mux->idle_state = idle_state;
+	}
+
+	ret = devm_mux_chip_register(dev, mux_chip);
+	if (ret < 0)
+		return ret;
+
+	dev_info(dev, "%u-way mux-controller registered\n",
+		 mux_chip->mux->states);
+
+	return 0;
+}
+
+static struct platform_driver mux_gpio_driver = {
+	.driver = {
+		.name = "gpio-mux",
+		.of_match_table	= of_match_ptr(mux_gpio_dt_ids),
+	},
+	.probe = mux_gpio_probe,
+};
+module_platform_driver(mux_gpio_driver);
+
+MODULE_DESCRIPTION("GPIO-controlled multiplexer driver");
+MODULE_AUTHOR("Peter Rosin <peda@axentia.se>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mux/mux-mmio.c b/drivers/mux/mux-mmio.c
new file mode 100644
index 000000000000..37c1de359a70
--- /dev/null
+++ b/drivers/mux/mux-mmio.c
@@ -0,0 +1,141 @@
+/*
+ * MMIO register bitfield-controlled multiplexer driver
+ *
+ * Copyright (C) 2017 Pengutronix, Philipp Zabel <kernel@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/mux/driver.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/property.h>
+#include <linux/regmap.h>
+
+static int mux_mmio_set(struct mux_control *mux, int state)
+{
+	struct regmap_field **fields = mux_chip_priv(mux->chip);
+
+	return regmap_field_write(fields[mux_control_get_index(mux)], state);
+}
+
+static const struct mux_control_ops mux_mmio_ops = {
+	.set = mux_mmio_set,
+};
+
+static const struct of_device_id mux_mmio_dt_ids[] = {
+	{ .compatible = "mmio-mux", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mux_mmio_dt_ids);
+
+static int mux_mmio_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct regmap_field **fields;
+	struct mux_chip *mux_chip;
+	struct regmap *regmap;
+	int num_fields;
+	int ret;
+	int i;
+
+	regmap = syscon_node_to_regmap(np->parent);
+	if (IS_ERR(regmap)) {
+		ret = PTR_ERR(regmap);
+		dev_err(dev, "failed to get regmap: %d\n", ret);
+		return ret;
+	}
+
+	ret = of_property_count_u32_elems(np, "mux-reg-masks");
+	if (ret == 0 || ret % 2)
+		ret = -EINVAL;
+	if (ret < 0) {
+		dev_err(dev, "mux-reg-masks property missing or invalid: %d\n",
+			ret);
+		return ret;
+	}
+	num_fields = ret / 2;
+
+	mux_chip = devm_mux_chip_alloc(dev, num_fields, num_fields *
+				       sizeof(*fields));
+	if (IS_ERR(mux_chip))
+		return PTR_ERR(mux_chip);
+
+	fields = mux_chip_priv(mux_chip);
+
+	for (i = 0; i < num_fields; i++) {
+		struct mux_control *mux = &mux_chip->mux[i];
+		struct reg_field field;
+		s32 idle_state = MUX_IDLE_AS_IS;
+		u32 reg, mask;
+		int bits;
+
+		ret = of_property_read_u32_index(np, "mux-reg-masks",
+						 2 * i, &reg);
+		if (!ret)
+			ret = of_property_read_u32_index(np, "mux-reg-masks",
+							 2 * i + 1, &mask);
+		if (ret < 0) {
+			dev_err(dev, "bitfield %d: failed to read mux-reg-masks property: %d\n",
+				i, ret);
+			return ret;
+		}
+
+		field.reg = reg;
+		field.msb = fls(mask) - 1;
+		field.lsb = ffs(mask) - 1;
+
+		if (mask != GENMASK(field.msb, field.lsb)) {
+			dev_err(dev, "bitfield %d: invalid mask 0x%x\n",
+				i, mask);
+			return -EINVAL;
+		}
+
+		fields[i] = devm_regmap_field_alloc(dev, regmap, field);
+		if (IS_ERR(fields[i])) {
+			ret = PTR_ERR(fields[i]);
+			dev_err(dev, "bitfield %d: failed allocate: %d\n",
+				i, ret);
+			return ret;
+		}
+
+		bits = 1 + field.msb - field.lsb;
+		mux->states = 1 << bits;
+
+		of_property_read_u32_index(np, "idle-states", i,
+					   (u32 *)&idle_state);
+		if (idle_state != MUX_IDLE_AS_IS) {
+			if (idle_state < 0 || idle_state >= mux->states) {
+				dev_err(dev, "bitfield: %d: out of range idle state %d\n",
+					i, idle_state);
+				return -EINVAL;
+			}
+
+			mux->idle_state = idle_state;
+		}
+	}
+
+	mux_chip->ops = &mux_mmio_ops;
+
+	return devm_mux_chip_register(dev, mux_chip);
+}
+
+static struct platform_driver mux_mmio_driver = {
+	.driver = {
+		.name = "mmio-mux",
+		.of_match_table	= of_match_ptr(mux_mmio_dt_ids),
+	},
+	.probe = mux_mmio_probe,
+};
+module_platform_driver(mux_mmio_driver);
+
+MODULE_DESCRIPTION("MMIO register bitfield-controlled multiplexer driver");
+MODULE_AUTHOR("Philipp Zabel <p.zabel@pengutronix.de>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/arcnet/arc-rawmode.c b/drivers/net/arcnet/arc-rawmode.c
index d78f30186642..8c651fdee039 100644
--- a/drivers/net/arcnet/arc-rawmode.c
+++ b/drivers/net/arcnet/arc-rawmode.c
@@ -85,7 +85,7 @@ static int build_header(struct sk_buff *skb, struct net_device *dev,
 			unsigned short type, uint8_t daddr)
 {
 	int hdr_size = ARC_HDR_SIZE;
-	struct archdr *pkt = (struct archdr *)skb_push(skb, hdr_size);
+	struct archdr *pkt = skb_push(skb, hdr_size);
 
 	/* Set the source hardware address.
 	 *
diff --git a/drivers/net/arcnet/arcdevice.h b/drivers/net/arcnet/arcdevice.h
index 20bfb9ba83ea..cbb4f8566bbe 100644
--- a/drivers/net/arcnet/arcdevice.h
+++ b/drivers/net/arcnet/arcdevice.h
@@ -269,6 +269,10 @@ struct arcnet_local {
 
 	struct timer_list	timer;
 
+	struct net_device *dev;
+	int reply_status;
+	struct tasklet_struct reply_tasklet;
+
 	/*
 	 * Buffer management: an ARCnet card has 4 x 512-byte buffers, each of
 	 * which can be used for either sending or receiving.  The new dynamic
diff --git a/drivers/net/arcnet/arcnet.c b/drivers/net/arcnet/arcnet.c
index 62ee439d5882..fcfccbb3d9a2 100644
--- a/drivers/net/arcnet/arcnet.c
+++ b/drivers/net/arcnet/arcnet.c
@@ -51,6 +51,7 @@
 #include <net/arp.h>
 #include <linux/init.h>
 #include <linux/jiffies.h>
+#include <linux/errqueue.h>
 
 #include <linux/leds.h>
 
@@ -391,6 +392,52 @@ static void arcnet_timer(unsigned long data)
 	}
 }
 
+static void arcnet_reply_tasklet(unsigned long data)
+{
+	struct arcnet_local *lp = (struct arcnet_local *)data;
+
+	struct sk_buff *ackskb, *skb;
+	struct sock_exterr_skb *serr;
+	struct sock *sk;
+	int ret;
+
+	local_irq_disable();
+	skb = lp->outgoing.skb;
+	if (!skb || !skb->sk) {
+		local_irq_enable();
+		return;
+	}
+
+	sock_hold(skb->sk);
+	sk = skb->sk;
+	ackskb = skb_clone_sk(skb);
+	sock_put(skb->sk);
+
+	if (!ackskb) {
+		local_irq_enable();
+		return;
+	}
+
+	serr = SKB_EXT_ERR(ackskb);
+	memset(serr, 0, sizeof(*serr));
+	serr->ee.ee_errno = ENOMSG;
+	serr->ee.ee_origin = SO_EE_ORIGIN_TXSTATUS;
+	serr->ee.ee_data = skb_shinfo(skb)->tskey;
+	serr->ee.ee_info = lp->reply_status;
+
+	/* finally erasing outgoing skb */
+	dev_kfree_skb(lp->outgoing.skb);
+	lp->outgoing.skb = NULL;
+
+	ackskb->dev = lp->dev;
+
+	ret = sock_queue_err_skb(sk, ackskb);
+	if (ret)
+		kfree_skb(ackskb);
+
+	local_irq_enable();
+};
+
 struct net_device *alloc_arcdev(const char *name)
 {
 	struct net_device *dev;
@@ -401,6 +448,7 @@ struct net_device *alloc_arcdev(const char *name)
 	if (dev) {
 		struct arcnet_local *lp = netdev_priv(dev);
 
+		lp->dev = dev;
 		spin_lock_init(&lp->lock);
 		init_timer(&lp->timer);
 		lp->timer.data = (unsigned long) dev;
@@ -436,6 +484,9 @@ int arcnet_open(struct net_device *dev)
 		arc_cont(D_PROTO, "\n");
 	}
 
+	tasklet_init(&lp->reply_tasklet, arcnet_reply_tasklet,
+		     (unsigned long)lp);
+
 	arc_printk(D_INIT, dev, "arcnet_open: resetting card.\n");
 
 	/* try to put the card in a defined state - if it fails the first
@@ -527,6 +578,8 @@ int arcnet_close(struct net_device *dev)
 	netif_stop_queue(dev);
 	netif_carrier_off(dev);
 
+	tasklet_kill(&lp->reply_tasklet);
+
 	/* flush TX and disable RX */
 	lp->hw.intmask(dev, 0);
 	lp->hw.command(dev, NOTXcmd);	/* stop transmit */
@@ -635,13 +688,13 @@ netdev_tx_t arcnet_send_packet(struct sk_buff *skb,
 		txbuf = -1;
 
 	if (txbuf != -1) {
+		lp->outgoing.skb = skb;
 		if (proto->prepare_tx(dev, pkt, skb->len, txbuf) &&
 		    !proto->ack_tx) {
 			/* done right away and we don't want to acknowledge
 			 *  the package later - forget about it now
 			 */
 			dev->stats.tx_bytes += skb->len;
-			dev_kfree_skb(skb);
 		} else {
 			/* do it the 'split' way */
 			lp->outgoing.proto = proto;
@@ -756,6 +809,7 @@ irqreturn_t arcnet_interrupt(int irq, void *dev_id)
 	struct net_device *dev = dev_id;
 	struct arcnet_local *lp;
 	int recbuf, status, diagstatus, didsomething, boguscount;
+	unsigned long flags;
 	int retval = IRQ_NONE;
 
 	arc_printk(D_DURING, dev, "\n");
@@ -765,7 +819,7 @@ irqreturn_t arcnet_interrupt(int irq, void *dev_id)
 	lp = netdev_priv(dev);
 	BUG_ON(!lp);
 
-	spin_lock(&lp->lock);
+	spin_lock_irqsave(&lp->lock, flags);
 
 	/* RESET flag was enabled - if device is not running, we must
 	 * clear it right away (but nothing else).
@@ -774,7 +828,7 @@ irqreturn_t arcnet_interrupt(int irq, void *dev_id)
 		if (lp->hw.status(dev) & RESETflag)
 			lp->hw.command(dev, CFLAGScmd | RESETclear);
 		lp->hw.intmask(dev, 0);
-		spin_unlock(&lp->lock);
+		spin_unlock_irqrestore(&lp->lock, flags);
 		return retval;
 	}
 
@@ -842,8 +896,16 @@ irqreturn_t arcnet_interrupt(int irq, void *dev_id)
 
 		/* a transmit finished, and we're interested in it. */
 		if ((status & lp->intmask & TXFREEflag) || lp->timed_out) {
+			int ackstatus;
 			lp->intmask &= ~(TXFREEflag | EXCNAKflag);
 
+			if (status & TXACKflag)
+				ackstatus = 2;
+			else if (lp->excnak_pending)
+				ackstatus = 1;
+			else
+				ackstatus = 0;
+
 			arc_printk(D_DURING, dev, "TX IRQ (stat=%Xh)\n",
 				   status);
 
@@ -866,18 +928,11 @@ irqreturn_t arcnet_interrupt(int irq, void *dev_id)
 
 				if (lp->outgoing.proto &&
 				    lp->outgoing.proto->ack_tx) {
-					int ackstatus;
-
-					if (status & TXACKflag)
-						ackstatus = 2;
-					else if (lp->excnak_pending)
-						ackstatus = 1;
-					else
-						ackstatus = 0;
-
 					lp->outgoing.proto
 						->ack_tx(dev, ackstatus);
 				}
+				lp->reply_status = ackstatus;
+				tasklet_hi_schedule(&lp->reply_tasklet);
 			}
 			if (lp->cur_tx != -1)
 				release_arcbuf(dev, lp->cur_tx);
@@ -998,7 +1053,7 @@ irqreturn_t arcnet_interrupt(int irq, void *dev_id)
 	udelay(1);
 	lp->hw.intmask(dev, lp->intmask);
 
-	spin_unlock(&lp->lock);
+	spin_unlock_irqrestore(&lp->lock, flags);
 	return retval;
 }
 EXPORT_SYMBOL(arcnet_interrupt);
diff --git a/drivers/net/arcnet/capmode.c b/drivers/net/arcnet/capmode.c
index 2056878fb087..b780be6f41ff 100644
--- a/drivers/net/arcnet/capmode.c
+++ b/drivers/net/arcnet/capmode.c
@@ -101,7 +101,7 @@ static int build_header(struct sk_buff *skb,
 			uint8_t daddr)
 {
 	int hdr_size = ARC_HDR_SIZE;
-	struct archdr *pkt = (struct archdr *)skb_push(skb, hdr_size);
+	struct archdr *pkt = skb_push(skb, hdr_size);
 
 	arc_printk(D_PROTO, dev, "Preparing header for cap packet %x.\n",
 		   *((int *)&pkt->soft.cap.cookie[0]));
@@ -212,7 +212,7 @@ static int ack_tx(struct net_device *dev, int acked)
 	ackpkt->soft.cap.proto = 0; /* using protocol 0 for acknowledge */
 	ackpkt->soft.cap.mes.ack = acked;
 
-	arc_printk(D_PROTO, dev, "Ackknowledge for cap packet %x.\n",
+	arc_printk(D_PROTO, dev, "Acknowledge for cap packet %x.\n",
 		   *((int *)&ackpkt->soft.cap.cookie[0]));
 
 	ackskb->protocol = cpu_to_be16(ETH_P_ARCNET);
diff --git a/drivers/net/arcnet/com20020-pci.c b/drivers/net/arcnet/com20020-pci.c
index 239de38fbd6a..2d956cb59d06 100644
--- a/drivers/net/arcnet/com20020-pci.c
+++ b/drivers/net/arcnet/com20020-pci.c
@@ -93,6 +93,27 @@ static void led_recon_set(struct led_classdev *led_cdev,
 	outb(!!value, priv->misc + ci->leds[card->index].red);
 }
 
+static ssize_t backplane_mode_show(struct device *dev,
+				   struct device_attribute *attr,
+				   char *buf)
+{
+	struct net_device *net_dev = to_net_dev(dev);
+	struct arcnet_local *lp = netdev_priv(net_dev);
+
+	return sprintf(buf, "%s\n", lp->backplane ? "true" : "false");
+}
+static DEVICE_ATTR_RO(backplane_mode);
+
+static struct attribute *com20020_state_attrs[] = {
+	&dev_attr_backplane_mode.attr,
+	NULL,
+};
+
+static struct attribute_group com20020_state_group = {
+	.name = NULL,
+	.attrs = com20020_state_attrs,
+};
+
 static void com20020pci_remove(struct pci_dev *pdev);
 
 static int com20020pci_probe(struct pci_dev *pdev,
@@ -135,6 +156,7 @@ static int com20020pci_probe(struct pci_dev *pdev,
 	for (i = 0; i < ci->devcount; i++) {
 		struct com20020_pci_channel_map *cm = &ci->chan_map_tbl[i];
 		struct com20020_dev *card;
+		int dev_id_mask = 0xf;
 
 		dev = alloc_arcdev(device);
 		if (!dev) {
@@ -166,8 +188,10 @@ static int com20020pci_probe(struct pci_dev *pdev,
 		arcnet_outb(0x00, ioaddr, COM20020_REG_W_COMMAND);
 		arcnet_inb(ioaddr, COM20020_REG_R_DIAGSTAT);
 
+		SET_NETDEV_DEV(dev, &pdev->dev);
 		dev->base_addr = ioaddr;
 		dev->dev_addr[0] = node;
+		dev->sysfs_groups[0] = &com20020_state_group;
 		dev->irq = pdev->irq;
 		lp->card_name = "PCI COM20020";
 		lp->card_flags = ci->flags;
@@ -177,10 +201,15 @@ static int com20020pci_probe(struct pci_dev *pdev,
 		lp->timeout = timeout;
 		lp->hw.owner = THIS_MODULE;
 
+		lp->backplane = (inb(priv->misc) >> (2 + i)) & 0x1;
+
+		if (!strncmp(ci->name, "EAE PLX-PCI FB2", 15))
+			lp->backplane = 1;
+
 		/* Get the dev_id from the PLX rotary coder */
 		if (!strncmp(ci->name, "EAE PLX-PCI MA1", 15))
-			dev->dev_id = 0xc;
-		dev->dev_id ^= inb(priv->misc + ci->rotary) >> 4;
+			dev_id_mask = 0x3;
+		dev->dev_id = (inb(priv->misc + ci->rotary) >> 4) & dev_id_mask;
 
 		snprintf(dev->name, sizeof(dev->name), "arc%d-%d", dev->dev_id, i);
 
@@ -361,6 +390,31 @@ static struct com20020_pci_card_info card_info_eae_ma1 = {
 	.flags = ARC_CAN_10MBIT,
 };
 
+static struct com20020_pci_card_info card_info_eae_fb2 = {
+	.name = "EAE PLX-PCI FB2",
+	.devcount = 1,
+	.chan_map_tbl = {
+		{
+			.bar = 2,
+			.offset = 0x00,
+			.size = 0x08,
+		},
+	},
+	.misc_map = {
+		.bar = 2,
+		.offset = 0x10,
+		.size = 0x04,
+	},
+	.leds = {
+		{
+			.green = 0x0,
+			.red = 0x1,
+		},
+	},
+	.rotary = 0x0,
+	.flags = ARC_CAN_10MBIT,
+};
+
 static const struct pci_device_id com20020pci_id_table[] = {
 	{
 		0x1571, 0xa001,
@@ -507,6 +561,12 @@ static const struct pci_device_id com20020pci_id_table[] = {
 		(kernel_ulong_t)&card_info_eae_ma1
 	},
 	{
+		0x10B5, 0x9050,
+		0x10B5, 0x3294,
+		0, 0,
+		(kernel_ulong_t)&card_info_eae_fb2
+	},
+	{
 		0x14BA, 0x6000,
 		PCI_ANY_ID, PCI_ANY_ID,
 		0, 0,
diff --git a/drivers/net/arcnet/com20020.c b/drivers/net/arcnet/com20020.c
index 13d9ad4b3f5c..78043a9c5981 100644
--- a/drivers/net/arcnet/com20020.c
+++ b/drivers/net/arcnet/com20020.c
@@ -246,8 +246,6 @@ int com20020_found(struct net_device *dev, int shared)
 		return -ENODEV;
 	}
 
-	dev->base_addr = ioaddr;
-
 	arc_printk(D_NORMAL, dev, "%s: station %02Xh found at %03lXh, IRQ %d.\n",
 		   lp->card_name, dev->dev_addr[0], dev->base_addr, dev->irq);
 
diff --git a/drivers/net/arcnet/rfc1051.c b/drivers/net/arcnet/rfc1051.c
index 4b1a75469cb1..a7752a5b647f 100644
--- a/drivers/net/arcnet/rfc1051.c
+++ b/drivers/net/arcnet/rfc1051.c
@@ -162,7 +162,7 @@ static int build_header(struct sk_buff *skb, struct net_device *dev,
 			unsigned short type, uint8_t daddr)
 {
 	int hdr_size = ARC_HDR_SIZE + RFC1051_HDR_SIZE;
-	struct archdr *pkt = (struct archdr *)skb_push(skb, hdr_size);
+	struct archdr *pkt = skb_push(skb, hdr_size);
 	struct arc_rfc1051 *soft = &pkt->soft.rfc1051;
 
 	/* set the protocol ID according to RFC1051 */
diff --git a/drivers/net/arcnet/rfc1201.c b/drivers/net/arcnet/rfc1201.c
index 566da5ecdc9d..a4c856282674 100644
--- a/drivers/net/arcnet/rfc1201.c
+++ b/drivers/net/arcnet/rfc1201.c
@@ -379,7 +379,7 @@ static int build_header(struct sk_buff *skb, struct net_device *dev,
 {
 	struct arcnet_local *lp = netdev_priv(dev);
 	int hdr_size = ARC_HDR_SIZE + RFC1201_HDR_SIZE;
-	struct archdr *pkt = (struct archdr *)skb_push(skb, hdr_size);
+	struct archdr *pkt = skb_push(skb, hdr_size);
 	struct arc_rfc1201 *soft = &pkt->soft.rfc1201;
 
 	/* set the protocol ID according to RFC1201 */
diff --git a/drivers/net/bonding/bond_3ad.c b/drivers/net/bonding/bond_3ad.c
index b44a6aeb346d..f43fb2f958a5 100644
--- a/drivers/net/bonding/bond_3ad.c
+++ b/drivers/net/bonding/bond_3ad.c
@@ -90,10 +90,13 @@ enum ad_link_speed_type {
 	AD_LINK_SPEED_100MBPS,
 	AD_LINK_SPEED_1000MBPS,
 	AD_LINK_SPEED_2500MBPS,
+	AD_LINK_SPEED_5000MBPS,
 	AD_LINK_SPEED_10000MBPS,
+	AD_LINK_SPEED_14000MBPS,
 	AD_LINK_SPEED_20000MBPS,
 	AD_LINK_SPEED_25000MBPS,
 	AD_LINK_SPEED_40000MBPS,
+	AD_LINK_SPEED_50000MBPS,
 	AD_LINK_SPEED_56000MBPS,
 	AD_LINK_SPEED_100000MBPS,
 };
@@ -259,10 +262,13 @@ static inline int __check_agg_selection_timer(struct port *port)
  *     %AD_LINK_SPEED_100MBPS,
  *     %AD_LINK_SPEED_1000MBPS,
  *     %AD_LINK_SPEED_2500MBPS,
+ *     %AD_LINK_SPEED_5000MBPS,
  *     %AD_LINK_SPEED_10000MBPS
+ *     %AD_LINK_SPEED_14000MBPS,
  *     %AD_LINK_SPEED_20000MBPS
  *     %AD_LINK_SPEED_25000MBPS
  *     %AD_LINK_SPEED_40000MBPS
+ *     %AD_LINK_SPEED_50000MBPS
  *     %AD_LINK_SPEED_56000MBPS
  *     %AD_LINK_SPEED_100000MBPS
  */
@@ -296,10 +302,18 @@ static u16 __get_link_speed(struct port *port)
 			speed = AD_LINK_SPEED_2500MBPS;
 			break;
 
+		case SPEED_5000:
+			speed = AD_LINK_SPEED_5000MBPS;
+			break;
+
 		case SPEED_10000:
 			speed = AD_LINK_SPEED_10000MBPS;
 			break;
 
+		case SPEED_14000:
+			speed = AD_LINK_SPEED_14000MBPS;
+			break;
+
 		case SPEED_20000:
 			speed = AD_LINK_SPEED_20000MBPS;
 			break;
@@ -312,6 +326,10 @@ static u16 __get_link_speed(struct port *port)
 			speed = AD_LINK_SPEED_40000MBPS;
 			break;
 
+		case SPEED_50000:
+			speed = AD_LINK_SPEED_50000MBPS;
+			break;
+
 		case SPEED_56000:
 			speed = AD_LINK_SPEED_56000MBPS;
 			break;
@@ -322,6 +340,11 @@ static u16 __get_link_speed(struct port *port)
 
 		default:
 			/* unknown speed value from ethtool. shouldn't happen */
+			if (slave->speed != SPEED_UNKNOWN)
+				pr_warn_once("%s: unknown ethtool speed (%d) for port %d (set it to 0)\n",
+					     slave->bond->dev->name,
+					     slave->speed,
+					     port->actor_port_number);
 			speed = 0;
 			break;
 		}
@@ -707,9 +730,15 @@ static u32 __get_agg_bandwidth(struct aggregator *aggregator)
 		case AD_LINK_SPEED_2500MBPS:
 			bandwidth = nports * 2500;
 			break;
+		case AD_LINK_SPEED_5000MBPS:
+			bandwidth = nports * 5000;
+			break;
 		case AD_LINK_SPEED_10000MBPS:
 			bandwidth = nports * 10000;
 			break;
+		case AD_LINK_SPEED_14000MBPS:
+			bandwidth = nports * 14000;
+			break;
 		case AD_LINK_SPEED_20000MBPS:
 			bandwidth = nports * 20000;
 			break;
@@ -719,6 +748,9 @@ static u32 __get_agg_bandwidth(struct aggregator *aggregator)
 		case AD_LINK_SPEED_40000MBPS:
 			bandwidth = nports * 40000;
 			break;
+		case AD_LINK_SPEED_50000MBPS:
+			bandwidth = nports * 50000;
+			break;
 		case AD_LINK_SPEED_56000MBPS:
 			bandwidth = nports * 56000;
 			break;
@@ -825,7 +857,7 @@ static int ad_lacpdu_send(struct port *port)
 	skb->protocol = PKT_TYPE_LACPDU;
 	skb->priority = TC_PRIO_CONTROL;
 
-	lacpdu_header = (struct lacpdu_header *)skb_put(skb, length);
+	lacpdu_header = skb_put(skb, length);
 
 	ether_addr_copy(lacpdu_header->hdr.h_dest, lacpdu_mcast_addr);
 	/* Note: source address is set to be the member's PERMANENT address,
@@ -867,7 +899,7 @@ static int ad_marker_send(struct port *port, struct bond_marker *marker)
 	skb->network_header = skb->mac_header + ETH_HLEN;
 	skb->protocol = PKT_TYPE_LACPDU;
 
-	marker_header = (struct bond_marker_header *)skb_put(skb, length);
+	marker_header = skb_put(skb, length);
 
 	ether_addr_copy(marker_header->hdr.h_dest, lacpdu_mcast_addr);
 	/* Note: source address is set to be the member's PERMANENT address,
diff --git a/drivers/net/bonding/bond_alb.c b/drivers/net/bonding/bond_alb.c
index 7d7a3cec149a..c02cc817a490 100644
--- a/drivers/net/bonding/bond_alb.c
+++ b/drivers/net/bonding/bond_alb.c
@@ -925,7 +925,6 @@ static void alb_send_lp_vid(struct slave *slave, u8 mac_addr[],
 	struct learning_pkt pkt;
 	struct sk_buff *skb;
 	int size = sizeof(struct learning_pkt);
-	char *data;
 
 	memset(&pkt, 0, size);
 	ether_addr_copy(pkt.mac_dst, mac_addr);
@@ -936,8 +935,7 @@ static void alb_send_lp_vid(struct slave *slave, u8 mac_addr[],
 	if (!skb)
 		return;
 
-	data = skb_put(skb, size);
-	memcpy(data, &pkt, size);
+	skb_put_data(skb, &pkt, size);
 
 	skb_reset_mac_header(skb);
 	skb->network_header = skb->mac_header + ETH_HLEN;
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index 2359478b977f..2865f31c6076 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -3488,7 +3488,8 @@ static int bond_do_ioctl(struct net_device *bond_dev, struct ifreq *ifr, int cmd
 	case BOND_CHANGE_ACTIVE_OLD:
 	case SIOCBONDCHANGEACTIVE:
 		bond_opt_initstr(&newval, slave_dev->name);
-		res = __bond_opt_set(bond, BOND_OPT_ACTIVE_SLAVE, &newval);
+		res = __bond_opt_set_notify(bond, BOND_OPT_ACTIVE_SLAVE,
+					    &newval);
 		break;
 	default:
 		res = -EOPNOTSUPP;
@@ -4174,12 +4175,6 @@ static const struct net_device_ops bond_netdev_ops = {
 	.ndo_add_slave		= bond_enslave,
 	.ndo_del_slave		= bond_release,
 	.ndo_fix_features	= bond_fix_features,
-	.ndo_bridge_setlink	= switchdev_port_bridge_setlink,
-	.ndo_bridge_getlink	= switchdev_port_bridge_getlink,
-	.ndo_bridge_dellink	= switchdev_port_bridge_dellink,
-	.ndo_fdb_add		= switchdev_port_fdb_add,
-	.ndo_fdb_del		= switchdev_port_fdb_del,
-	.ndo_fdb_dump		= switchdev_port_fdb_dump,
 	.ndo_features_check	= passthru_features_check,
 };
 
@@ -4192,7 +4187,6 @@ static void bond_destructor(struct net_device *bond_dev)
 	struct bonding *bond = netdev_priv(bond_dev);
 	if (bond->wq)
 		destroy_workqueue(bond->wq);
-	free_netdev(bond_dev);
 }
 
 void bond_setup(struct net_device *bond_dev)
@@ -4212,7 +4206,8 @@ void bond_setup(struct net_device *bond_dev)
 	bond_dev->netdev_ops = &bond_netdev_ops;
 	bond_dev->ethtool_ops = &bond_ethtool_ops;
 
-	bond_dev->destructor = bond_destructor;
+	bond_dev->needs_free_netdev = true;
+	bond_dev->priv_destructor = bond_destructor;
 
 	SET_NETDEV_DEVTYPE(bond_dev, &bond_type);
 
@@ -4736,7 +4731,7 @@ int bond_create(struct net *net, const char *name)
 
 	rtnl_unlock();
 	if (res < 0)
-		bond_destructor(bond_dev);
+		free_netdev(bond_dev);
 	return res;
 }
 
diff --git a/drivers/net/bonding/bond_netlink.c b/drivers/net/bonding/bond_netlink.c
index 47a8103610bc..a1b33aa6054a 100644
--- a/drivers/net/bonding/bond_netlink.c
+++ b/drivers/net/bonding/bond_netlink.c
@@ -118,7 +118,8 @@ static const struct nla_policy bond_slave_policy[IFLA_BOND_SLAVE_MAX + 1] = {
 	[IFLA_BOND_SLAVE_QUEUE_ID]	= { .type = NLA_U16 },
 };
 
-static int bond_validate(struct nlattr *tb[], struct nlattr *data[])
+static int bond_validate(struct nlattr *tb[], struct nlattr *data[],
+			 struct netlink_ext_ack *extack)
 {
 	if (tb[IFLA_ADDRESS]) {
 		if (nla_len(tb[IFLA_ADDRESS]) != ETH_ALEN)
@@ -131,7 +132,8 @@ static int bond_validate(struct nlattr *tb[], struct nlattr *data[])
 
 static int bond_slave_changelink(struct net_device *bond_dev,
 				 struct net_device *slave_dev,
-				 struct nlattr *tb[], struct nlattr *data[])
+				 struct nlattr *tb[], struct nlattr *data[],
+				 struct netlink_ext_ack *extack)
 {
 	struct bonding *bond = netdev_priv(bond_dev);
 	struct bond_opt_value newval;
@@ -156,8 +158,9 @@ static int bond_slave_changelink(struct net_device *bond_dev,
 	return 0;
 }
 
-static int bond_changelink(struct net_device *bond_dev,
-			   struct nlattr *tb[], struct nlattr *data[])
+static int bond_changelink(struct net_device *bond_dev, struct nlattr *tb[],
+			   struct nlattr *data[],
+			   struct netlink_ext_ack *extack)
 {
 	struct bonding *bond = netdev_priv(bond_dev);
 	struct bond_opt_value newval;
@@ -438,11 +441,12 @@ static int bond_changelink(struct net_device *bond_dev,
 }
 
 static int bond_newlink(struct net *src_net, struct net_device *bond_dev,
-			struct nlattr *tb[], struct nlattr *data[])
+			struct nlattr *tb[], struct nlattr *data[],
+			struct netlink_ext_ack *extack)
 {
 	int err;
 
-	err = bond_changelink(bond_dev, tb, data);
+	err = bond_changelink(bond_dev, tb, data, extack);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/net/bonding/bond_options.c b/drivers/net/bonding/bond_options.c
index 1bcbb8913e17..a12d603d41c6 100644
--- a/drivers/net/bonding/bond_options.c
+++ b/drivers/net/bonding/bond_options.c
@@ -464,7 +464,7 @@ const struct bond_opt_value *bond_opt_get_val(unsigned int option, u64 val)
 
 /* Searches for a value in opt's values[] table which matches the flagmask */
 static const struct bond_opt_value *bond_opt_get_flags(const struct bond_option *opt,
-						 u32 flagmask)
+						       u32 flagmask)
 {
 	int i;
 
@@ -673,7 +673,30 @@ int __bond_opt_set(struct bonding *bond,
 out:
 	if (ret)
 		bond_opt_error_interpret(bond, opt, ret, val);
-	else if (bond->dev->reg_state == NETREG_REGISTERED)
+
+	return ret;
+}
+/**
+ * __bond_opt_set_notify - set a bonding option
+ * @bond: target bond device
+ * @option: option to set
+ * @val: value to set it to
+ *
+ * This function is used to change the bond's option value and trigger
+ * a notification to user sapce. It can be used for both enabling/changing
+ * an option and for disabling it. RTNL lock must be obtained before calling
+ * this function.
+ */
+int __bond_opt_set_notify(struct bonding *bond,
+			  unsigned int option, struct bond_opt_value *val)
+{
+	int ret = -ENOENT;
+
+	ASSERT_RTNL();
+
+	ret = __bond_opt_set(bond, option, val);
+
+	if (!ret && (bond->dev->reg_state == NETREG_REGISTERED))
 		call_netdevice_notifiers(NETDEV_CHANGEINFODATA, bond->dev);
 
 	return ret;
@@ -696,7 +719,7 @@ int bond_opt_tryset_rtnl(struct bonding *bond, unsigned int option, char *buf)
 	if (!rtnl_trylock())
 		return restart_syscall();
 	bond_opt_initstr(&optval, buf);
-	ret = __bond_opt_set(bond, option, &optval);
+	ret = __bond_opt_set_notify(bond, option, &optval);
 	rtnl_unlock();
 
 	return ret;
@@ -721,14 +744,14 @@ static int bond_option_mode_set(struct bonding *bond,
 				const struct bond_opt_value *newval)
 {
 	if (!bond_mode_uses_arp(newval->value) && bond->params.arp_interval) {
-		netdev_info(bond->dev, "%s mode is incompatible with arp monitoring, start mii monitoring\n",
-			    newval->string);
+		netdev_dbg(bond->dev, "%s mode is incompatible with arp monitoring, start mii monitoring\n",
+			   newval->string);
 		/* disable arp monitoring */
 		bond->params.arp_interval = 0;
 		/* set miimon to default value */
 		bond->params.miimon = BOND_DEFAULT_MIIMON;
-		netdev_info(bond->dev, "Setting MII monitoring interval to %d\n",
-			    bond->params.miimon);
+		netdev_dbg(bond->dev, "Setting MII monitoring interval to %d\n",
+			   bond->params.miimon);
 	}
 
 	/* don't cache arp_validate between modes */
@@ -771,7 +794,7 @@ static int bond_option_active_slave_set(struct bonding *bond,
 	block_netpoll_tx();
 	/* check to see if we are clearing active */
 	if (!slave_dev) {
-		netdev_info(bond->dev, "Clearing current active slave\n");
+		netdev_dbg(bond->dev, "Clearing current active slave\n");
 		RCU_INIT_POINTER(bond->curr_active_slave, NULL);
 		bond_select_active_slave(bond);
 	} else {
@@ -782,13 +805,13 @@ static int bond_option_active_slave_set(struct bonding *bond,
 
 		if (new_active == old_active) {
 			/* do nothing */
-			netdev_info(bond->dev, "%s is already the current active slave\n",
-				    new_active->dev->name);
+			netdev_dbg(bond->dev, "%s is already the current active slave\n",
+				   new_active->dev->name);
 		} else {
 			if (old_active && (new_active->link == BOND_LINK_UP) &&
 			    bond_slave_is_up(new_active)) {
-				netdev_info(bond->dev, "Setting %s as active slave\n",
-					    new_active->dev->name);
+				netdev_dbg(bond->dev, "Setting %s as active slave\n",
+					   new_active->dev->name);
 				bond_change_active_slave(bond, new_active);
 			} else {
 				netdev_err(bond->dev, "Could not set %s as active slave; either %s is down or the link is down\n",
@@ -810,17 +833,17 @@ static int bond_option_active_slave_set(struct bonding *bond,
 static int bond_option_miimon_set(struct bonding *bond,
 				  const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting MII monitoring interval to %llu\n",
-		    newval->value);
+	netdev_dbg(bond->dev, "Setting MII monitoring interval to %llu\n",
+		   newval->value);
 	bond->params.miimon = newval->value;
 	if (bond->params.updelay)
-		netdev_info(bond->dev, "Note: Updating updelay (to %d) since it is a multiple of the miimon value\n",
-			bond->params.updelay * bond->params.miimon);
+		netdev_dbg(bond->dev, "Note: Updating updelay (to %d) since it is a multiple of the miimon value\n",
+			   bond->params.updelay * bond->params.miimon);
 	if (bond->params.downdelay)
-		netdev_info(bond->dev, "Note: Updating downdelay (to %d) since it is a multiple of the miimon value\n",
-			    bond->params.downdelay * bond->params.miimon);
+		netdev_dbg(bond->dev, "Note: Updating downdelay (to %d) since it is a multiple of the miimon value\n",
+			   bond->params.downdelay * bond->params.miimon);
 	if (newval->value && bond->params.arp_interval) {
-		netdev_info(bond->dev, "MII monitoring cannot be used with ARP monitoring - disabling ARP monitoring...\n");
+		netdev_dbg(bond->dev, "MII monitoring cannot be used with ARP monitoring - disabling ARP monitoring...\n");
 		bond->params.arp_interval = 0;
 		if (bond->params.arp_validate)
 			bond->params.arp_validate = BOND_ARP_VALIDATE_NONE;
@@ -862,8 +885,8 @@ static int bond_option_updelay_set(struct bonding *bond,
 			    bond->params.miimon);
 	}
 	bond->params.updelay = value / bond->params.miimon;
-	netdev_info(bond->dev, "Setting up delay to %d\n",
-		    bond->params.updelay * bond->params.miimon);
+	netdev_dbg(bond->dev, "Setting up delay to %d\n",
+		   bond->params.updelay * bond->params.miimon);
 
 	return 0;
 }
@@ -884,8 +907,8 @@ static int bond_option_downdelay_set(struct bonding *bond,
 			    bond->params.miimon);
 	}
 	bond->params.downdelay = value / bond->params.miimon;
-	netdev_info(bond->dev, "Setting down delay to %d\n",
-		    bond->params.downdelay * bond->params.miimon);
+	netdev_dbg(bond->dev, "Setting down delay to %d\n",
+		   bond->params.downdelay * bond->params.miimon);
 
 	return 0;
 }
@@ -893,8 +916,8 @@ static int bond_option_downdelay_set(struct bonding *bond,
 static int bond_option_use_carrier_set(struct bonding *bond,
 				       const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting use_carrier to %llu\n",
-		    newval->value);
+	netdev_dbg(bond->dev, "Setting use_carrier to %llu\n",
+		   newval->value);
 	bond->params.use_carrier = newval->value;
 
 	return 0;
@@ -907,16 +930,16 @@ static int bond_option_use_carrier_set(struct bonding *bond,
 static int bond_option_arp_interval_set(struct bonding *bond,
 					const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting ARP monitoring interval to %llu\n",
-		    newval->value);
+	netdev_dbg(bond->dev, "Setting ARP monitoring interval to %llu\n",
+		   newval->value);
 	bond->params.arp_interval = newval->value;
 	if (newval->value) {
 		if (bond->params.miimon) {
-			netdev_info(bond->dev, "ARP monitoring cannot be used with MII monitoring. Disabling MII monitoring\n");
+			netdev_dbg(bond->dev, "ARP monitoring cannot be used with MII monitoring. Disabling MII monitoring\n");
 			bond->params.miimon = 0;
 		}
 		if (!bond->params.arp_targets[0])
-			netdev_info(bond->dev, "ARP monitoring has been set up, but no ARP targets have been specified\n");
+			netdev_dbg(bond->dev, "ARP monitoring has been set up, but no ARP targets have been specified\n");
 	}
 	if (bond->dev->flags & IFF_UP) {
 		/* If the interface is up, we may need to fire off
@@ -977,7 +1000,7 @@ static int _bond_option_arp_ip_target_add(struct bonding *bond, __be32 target)
 		return -EINVAL;
 	}
 
-	netdev_info(bond->dev, "Adding ARP target %pI4\n", &target);
+	netdev_dbg(bond->dev, "Adding ARP target %pI4\n", &target);
 
 	_bond_options_arp_ip_target_set(bond, ind, target, jiffies);
 
@@ -1013,7 +1036,7 @@ static int bond_option_arp_ip_target_rem(struct bonding *bond, __be32 target)
 	if (ind == 0 && !targets[1] && bond->params.arp_interval)
 		netdev_warn(bond->dev, "Removing last arp target with arp_interval on\n");
 
-	netdev_info(bond->dev, "Removing ARP target %pI4\n", &target);
+	netdev_dbg(bond->dev, "Removing ARP target %pI4\n", &target);
 
 	bond_for_each_slave(bond, slave, iter) {
 		targets_rx = slave->target_last_arp_rx;
@@ -1065,8 +1088,8 @@ static int bond_option_arp_ip_targets_set(struct bonding *bond,
 static int bond_option_arp_validate_set(struct bonding *bond,
 					const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting arp_validate to %s (%llu)\n",
-		    newval->string, newval->value);
+	netdev_dbg(bond->dev, "Setting arp_validate to %s (%llu)\n",
+		   newval->string, newval->value);
 
 	if (bond->dev->flags & IFF_UP) {
 		if (!newval->value)
@@ -1082,8 +1105,8 @@ static int bond_option_arp_validate_set(struct bonding *bond,
 static int bond_option_arp_all_targets_set(struct bonding *bond,
 					   const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting arp_all_targets to %s (%llu)\n",
-		    newval->string, newval->value);
+	netdev_dbg(bond->dev, "Setting arp_all_targets to %s (%llu)\n",
+		   newval->string, newval->value);
 	bond->params.arp_all_targets = newval->value;
 
 	return 0;
@@ -1103,7 +1126,7 @@ static int bond_option_primary_set(struct bonding *bond,
 		*p = '\0';
 	/* check to see if we are clearing primary */
 	if (!strlen(primary)) {
-		netdev_info(bond->dev, "Setting primary slave to None\n");
+		netdev_dbg(bond->dev, "Setting primary slave to None\n");
 		RCU_INIT_POINTER(bond->primary_slave, NULL);
 		memset(bond->params.primary, 0, sizeof(bond->params.primary));
 		bond_select_active_slave(bond);
@@ -1112,8 +1135,8 @@ static int bond_option_primary_set(struct bonding *bond,
 
 	bond_for_each_slave(bond, slave, iter) {
 		if (strncmp(slave->dev->name, primary, IFNAMSIZ) == 0) {
-			netdev_info(bond->dev, "Setting %s as primary slave\n",
-				    slave->dev->name);
+			netdev_dbg(bond->dev, "Setting %s as primary slave\n",
+				   slave->dev->name);
 			rcu_assign_pointer(bond->primary_slave, slave);
 			strcpy(bond->params.primary, slave->dev->name);
 			bond_select_active_slave(bond);
@@ -1122,15 +1145,15 @@ static int bond_option_primary_set(struct bonding *bond,
 	}
 
 	if (rtnl_dereference(bond->primary_slave)) {
-		netdev_info(bond->dev, "Setting primary slave to None\n");
+		netdev_dbg(bond->dev, "Setting primary slave to None\n");
 		RCU_INIT_POINTER(bond->primary_slave, NULL);
 		bond_select_active_slave(bond);
 	}
 	strncpy(bond->params.primary, primary, IFNAMSIZ);
 	bond->params.primary[IFNAMSIZ - 1] = 0;
 
-	netdev_info(bond->dev, "Recording %s as primary, but it has not been enslaved to %s yet\n",
-		    primary, bond->dev->name);
+	netdev_dbg(bond->dev, "Recording %s as primary, but it has not been enslaved to %s yet\n",
+		   primary, bond->dev->name);
 
 out:
 	unblock_netpoll_tx();
@@ -1141,8 +1164,8 @@ out:
 static int bond_option_primary_reselect_set(struct bonding *bond,
 					    const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting primary_reselect to %s (%llu)\n",
-		    newval->string, newval->value);
+	netdev_dbg(bond->dev, "Setting primary_reselect to %s (%llu)\n",
+		   newval->string, newval->value);
 	bond->params.primary_reselect = newval->value;
 
 	block_netpoll_tx();
@@ -1155,8 +1178,8 @@ static int bond_option_primary_reselect_set(struct bonding *bond,
 static int bond_option_fail_over_mac_set(struct bonding *bond,
 					 const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting fail_over_mac to %s (%llu)\n",
-		    newval->string, newval->value);
+	netdev_dbg(bond->dev, "Setting fail_over_mac to %s (%llu)\n",
+		   newval->string, newval->value);
 	bond->params.fail_over_mac = newval->value;
 
 	return 0;
@@ -1165,8 +1188,8 @@ static int bond_option_fail_over_mac_set(struct bonding *bond,
 static int bond_option_xmit_hash_policy_set(struct bonding *bond,
 					    const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting xmit hash policy to %s (%llu)\n",
-		    newval->string, newval->value);
+	netdev_dbg(bond->dev, "Setting xmit hash policy to %s (%llu)\n",
+		   newval->string, newval->value);
 	bond->params.xmit_policy = newval->value;
 
 	return 0;
@@ -1175,8 +1198,8 @@ static int bond_option_xmit_hash_policy_set(struct bonding *bond,
 static int bond_option_resend_igmp_set(struct bonding *bond,
 				       const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting resend_igmp to %llu\n",
-		    newval->value);
+	netdev_dbg(bond->dev, "Setting resend_igmp to %llu\n",
+		   newval->value);
 	bond->params.resend_igmp = newval->value;
 
 	return 0;
@@ -1214,8 +1237,8 @@ static int bond_option_all_slaves_active_set(struct bonding *bond,
 static int bond_option_min_links_set(struct bonding *bond,
 				     const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting min links value to %llu\n",
-		    newval->value);
+	netdev_dbg(bond->dev, "Setting min links value to %llu\n",
+		   newval->value);
 	bond->params.min_links = newval->value;
 	bond_set_carrier(bond);
 
@@ -1233,6 +1256,8 @@ static int bond_option_lp_interval_set(struct bonding *bond,
 static int bond_option_pps_set(struct bonding *bond,
 			       const struct bond_opt_value *newval)
 {
+	netdev_dbg(bond->dev, "Setting packets per slave to %llu\n",
+		   newval->value);
 	bond->params.packets_per_slave = newval->value;
 	if (newval->value > 0) {
 		bond->params.reciprocal_packets_per_slave =
@@ -1251,8 +1276,8 @@ static int bond_option_pps_set(struct bonding *bond,
 static int bond_option_lacp_rate_set(struct bonding *bond,
 				     const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting LACP rate to %s (%llu)\n",
-		    newval->string, newval->value);
+	netdev_dbg(bond->dev, "Setting LACP rate to %s (%llu)\n",
+		   newval->string, newval->value);
 	bond->params.lacp_fast = newval->value;
 	bond_3ad_update_lacp_rate(bond);
 
@@ -1262,8 +1287,8 @@ static int bond_option_lacp_rate_set(struct bonding *bond,
 static int bond_option_ad_select_set(struct bonding *bond,
 				     const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting ad_select to %s (%llu)\n",
-		    newval->string, newval->value);
+	netdev_dbg(bond->dev, "Setting ad_select to %s (%llu)\n",
+		   newval->string, newval->value);
 	bond->params.ad_select = newval->value;
 
 	return 0;
@@ -1324,7 +1349,7 @@ out:
 	return ret;
 
 err_no_cmd:
-	netdev_info(bond->dev, "invalid input for queue_id set\n");
+	netdev_dbg(bond->dev, "invalid input for queue_id set\n");
 	ret = -EPERM;
 	goto out;
 
@@ -1346,20 +1371,20 @@ static int bond_option_slaves_set(struct bonding *bond,
 
 	dev = __dev_get_by_name(dev_net(bond->dev), ifname);
 	if (!dev) {
-		netdev_info(bond->dev, "interface %s does not exist!\n",
-			    ifname);
+		netdev_dbg(bond->dev, "interface %s does not exist!\n",
+			   ifname);
 		ret = -ENODEV;
 		goto out;
 	}
 
 	switch (command[0]) {
 	case '+':
-		netdev_info(bond->dev, "Adding slave %s\n", dev->name);
+		netdev_dbg(bond->dev, "Adding slave %s\n", dev->name);
 		ret = bond_enslave(bond->dev, dev);
 		break;
 
 	case '-':
-		netdev_info(bond->dev, "Removing slave %s\n", dev->name);
+		netdev_dbg(bond->dev, "Removing slave %s\n", dev->name);
 		ret = bond_release(bond->dev, dev);
 		break;
 
@@ -1379,8 +1404,8 @@ err_no_cmd:
 static int bond_option_tlb_dynamic_lb_set(struct bonding *bond,
 					  const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting dynamic-lb to %s (%llu)\n",
-		    newval->string, newval->value);
+	netdev_dbg(bond->dev, "Setting dynamic-lb to %s (%llu)\n",
+		   newval->string, newval->value);
 	bond->params.tlb_dynamic_lb = newval->value;
 
 	return 0;
@@ -1389,8 +1414,8 @@ static int bond_option_tlb_dynamic_lb_set(struct bonding *bond,
 static int bond_option_ad_actor_sys_prio_set(struct bonding *bond,
 					     const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting ad_actor_sys_prio to %llu\n",
-		    newval->value);
+	netdev_dbg(bond->dev, "Setting ad_actor_sys_prio to %llu\n",
+		   newval->value);
 
 	bond->params.ad_actor_sys_prio = newval->value;
 	bond_3ad_update_ad_actor_settings(bond);
@@ -1419,7 +1444,7 @@ static int bond_option_ad_actor_system_set(struct bonding *bond,
 	if (!is_valid_ether_addr(mac))
 		goto err;
 
-	netdev_info(bond->dev, "Setting ad_actor_system to %pM\n", mac);
+	netdev_dbg(bond->dev, "Setting ad_actor_system to %pM\n", mac);
 	ether_addr_copy(bond->params.ad_actor_system, mac);
 	bond_3ad_update_ad_actor_settings(bond);
 
@@ -1433,8 +1458,8 @@ err:
 static int bond_option_ad_user_port_key_set(struct bonding *bond,
 					    const struct bond_opt_value *newval)
 {
-	netdev_info(bond->dev, "Setting ad_user_port_key to %llu\n",
-		    newval->value);
+	netdev_dbg(bond->dev, "Setting ad_user_port_key to %llu\n",
+		   newval->value);
 
 	bond->params.ad_user_port_key = newval->value;
 	return 0;
diff --git a/drivers/net/caif/caif_hsi.c b/drivers/net/caif/caif_hsi.c
index ddabce759456..438966bf51c2 100644
--- a/drivers/net/caif/caif_hsi.c
+++ b/drivers/net/caif/caif_hsi.c
@@ -426,7 +426,6 @@ static int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 	/* Check for embedded CAIF frame. */
 	if (desc->offset) {
 		struct sk_buff *skb;
-		u8 *dst = NULL;
 		int len = 0;
 		pfrm = ((u8 *)desc) + desc->offset;
 
@@ -454,8 +453,7 @@ static int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 		}
 		caif_assert(skb != NULL);
 
-		dst = skb_put(skb, len);
-		memcpy(dst, pfrm, len);
+		skb_put_data(skb, pfrm, len);
 
 		skb->protocol = htons(ETH_P_CAIF);
 		skb_reset_mac_header(skb);
@@ -556,7 +554,6 @@ static int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 	/* Parse payload. */
 	while (nfrms < CFHSI_MAX_PKTS && *plen) {
 		struct sk_buff *skb;
-		u8 *dst = NULL;
 		u8 *pcffrm = NULL;
 		int len;
 
@@ -585,8 +582,7 @@ static int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 		}
 		caif_assert(skb != NULL);
 
-		dst = skb_put(skb, len);
-		memcpy(dst, pcffrm, len);
+		skb_put_data(skb, pcffrm, len);
 
 		skb->protocol = htons(ETH_P_CAIF);
 		skb_reset_mac_header(skb);
@@ -1121,7 +1117,7 @@ static void cfhsi_setup(struct net_device *dev)
 	dev->flags = IFF_POINTOPOINT | IFF_NOARP;
 	dev->mtu = CFHSI_MAX_CAIF_FRAME_SZ;
 	dev->priv_flags |= IFF_NO_QUEUE;
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 	dev->netdev_ops = &cfhsi_netdevops;
 	for (i = 0; i < CFHSI_PRIO_LAST; ++i)
 		skb_queue_head_init(&cfhsi->qhead[i]);
@@ -1356,7 +1352,8 @@ static void cfhsi_netlink_parms(struct nlattr *data[], struct cfhsi *cfhsi)
 }
 
 static int caif_hsi_changelink(struct net_device *dev, struct nlattr *tb[],
-				struct nlattr *data[])
+			       struct nlattr *data[],
+			       struct netlink_ext_ack *extack)
 {
 	cfhsi_netlink_parms(data, netdev_priv(dev));
 	netdev_state_change(dev);
@@ -1403,7 +1400,8 @@ static int caif_hsi_fill_info(struct sk_buff *skb, const struct net_device *dev)
 }
 
 static int caif_hsi_newlink(struct net *src_net, struct net_device *dev,
-			  struct nlattr *tb[], struct nlattr *data[])
+			    struct nlattr *tb[], struct nlattr *data[],
+			    struct netlink_ext_ack *extack)
 {
 	struct cfhsi *cfhsi = NULL;
 	struct cfhsi_ops *(*get_ops)(void);
diff --git a/drivers/net/caif/caif_serial.c b/drivers/net/caif/caif_serial.c
index c2dea4916e5d..709838e4c062 100644
--- a/drivers/net/caif/caif_serial.c
+++ b/drivers/net/caif/caif_serial.c
@@ -171,7 +171,6 @@ static void ldisc_receive(struct tty_struct *tty, const u8 *data,
 	struct sk_buff *skb = NULL;
 	struct ser_device *ser;
 	int ret;
-	u8 *p;
 
 	ser = tty->disc_data;
 
@@ -198,8 +197,7 @@ static void ldisc_receive(struct tty_struct *tty, const u8 *data,
 	skb = netdev_alloc_skb(ser->dev, count+1);
 	if (skb == NULL)
 		return;
-	p = skb_put(skb, count);
-	memcpy(p, data, count);
+	skb_put_data(skb, data, count);
 
 	skb->protocol = htons(ETH_P_CAIF);
 	skb_reset_mac_header(skb);
@@ -428,7 +426,7 @@ static void caifdev_setup(struct net_device *dev)
 	dev->flags = IFF_POINTOPOINT | IFF_NOARP;
 	dev->mtu = CAIF_MAX_MTU;
 	dev->priv_flags |= IFF_NO_QUEUE;
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 	skb_queue_head_init(&serdev->head);
 	serdev->common.link_select = CAIF_LINK_LOW_LATENCY;
 	serdev->common.use_frag = true;
diff --git a/drivers/net/caif/caif_spi.c b/drivers/net/caif/caif_spi.c
index 3a529fbe539f..980eace53d44 100644
--- a/drivers/net/caif/caif_spi.c
+++ b/drivers/net/caif/caif_spi.c
@@ -289,44 +289,44 @@ static LIST_HEAD(cfspi_list);
 static spinlock_t cfspi_list_lock;
 
 /* SPI uplink head alignment. */
-static ssize_t show_up_head_align(struct device_driver *driver, char *buf)
+static ssize_t up_head_align_show(struct device_driver *driver, char *buf)
 {
 	return sprintf(buf, "%d\n", spi_up_head_align);
 }
 
-static DRIVER_ATTR(up_head_align, S_IRUSR, show_up_head_align, NULL);
+static DRIVER_ATTR_RO(up_head_align);
 
 /* SPI uplink tail alignment. */
-static ssize_t show_up_tail_align(struct device_driver *driver, char *buf)
+static ssize_t up_tail_align_show(struct device_driver *driver, char *buf)
 {
 	return sprintf(buf, "%d\n", spi_up_tail_align);
 }
 
-static DRIVER_ATTR(up_tail_align, S_IRUSR, show_up_tail_align, NULL);
+static DRIVER_ATTR_RO(up_tail_align);
 
 /* SPI downlink head alignment. */
-static ssize_t show_down_head_align(struct device_driver *driver, char *buf)
+static ssize_t down_head_align_show(struct device_driver *driver, char *buf)
 {
 	return sprintf(buf, "%d\n", spi_down_head_align);
 }
 
-static DRIVER_ATTR(down_head_align, S_IRUSR, show_down_head_align, NULL);
+static DRIVER_ATTR_RO(down_head_align);
 
 /* SPI downlink tail alignment. */
-static ssize_t show_down_tail_align(struct device_driver *driver, char *buf)
+static ssize_t down_tail_align_show(struct device_driver *driver, char *buf)
 {
 	return sprintf(buf, "%d\n", spi_down_tail_align);
 }
 
-static DRIVER_ATTR(down_tail_align, S_IRUSR, show_down_tail_align, NULL);
+static DRIVER_ATTR_RO(down_tail_align);
 
 /* SPI frame alignment. */
-static ssize_t show_frame_align(struct device_driver *driver, char *buf)
+static ssize_t frame_align_show(struct device_driver *driver, char *buf)
 {
 	return sprintf(buf, "%d\n", spi_frm_align);
 }
 
-static DRIVER_ATTR(frame_align, S_IRUSR, show_frame_align, NULL);
+static DRIVER_ATTR_RO(frame_align);
 
 int cfspi_xmitfrm(struct cfspi *cfspi, u8 *buf, size_t len)
 {
@@ -526,7 +526,6 @@ int cfspi_rxfrm(struct cfspi *cfspi, u8 *buf, size_t len)
 		struct sk_buff *skb = NULL;
 		int spad = 0;
 		int epad = 0;
-		u8 *dst = NULL;
 		int pkt_len = 0;
 
 		/*
@@ -548,8 +547,7 @@ int cfspi_rxfrm(struct cfspi *cfspi, u8 *buf, size_t len)
 		skb = netdev_alloc_skb(cfspi->ndev, pkt_len + 1);
 		caif_assert(skb != NULL);
 
-		dst = skb_put(skb, pkt_len);
-		memcpy(dst, src, pkt_len);
+		skb_put_data(skb, src, pkt_len);
 		src += pkt_len;
 
 		skb->protocol = htons(ETH_P_CAIF);
@@ -712,7 +710,7 @@ static void cfspi_setup(struct net_device *dev)
 	dev->flags = IFF_NOARP | IFF_POINTOPOINT;
 	dev->priv_flags |= IFF_NO_QUEUE;
 	dev->mtu = SPI_MAX_PAYLOAD_SIZE;
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 	skb_queue_head_init(&cfspi->qhead);
 	skb_queue_head_init(&cfspi->chead);
 	cfspi->cfdev.link_select = CAIF_LINK_HIGH_BANDW;
diff --git a/drivers/net/caif/caif_virtio.c b/drivers/net/caif/caif_virtio.c
index 6122768c8644..c3d104feee13 100644
--- a/drivers/net/caif/caif_virtio.c
+++ b/drivers/net/caif/caif_virtio.c
@@ -242,7 +242,7 @@ static struct sk_buff *cfv_alloc_and_copy_skb(int *err,
 
 	skb_reserve(skb, cfv->rx_hr + pad_len);
 
-	memcpy(skb_put(skb, cfpkt_len), frm + cfv->rx_hr, cfpkt_len);
+	skb_put_data(skb, frm + cfv->rx_hr, cfpkt_len);
 	return skb;
 }
 
@@ -617,7 +617,7 @@ static void cfv_netdev_setup(struct net_device *netdev)
 	netdev->tx_queue_len = 100;
 	netdev->flags = IFF_POINTOPOINT | IFF_NOARP;
 	netdev->mtu = CFV_DEF_MTU_SIZE;
-	netdev->destructor = free_netdev;
+	netdev->needs_free_netdev = true;
 }
 
 /* Create debugfs counters for the device */
diff --git a/drivers/net/can/dev.c b/drivers/net/can/dev.c
index 611d16a7061d..365a8cc62405 100644
--- a/drivers/net/can/dev.c
+++ b/drivers/net/can/dev.c
@@ -391,6 +391,9 @@ void can_change_state(struct net_device *dev, struct can_frame *cf,
 	can_update_state_error_stats(dev, new_state);
 	priv->state = new_state;
 
+	if (!cf)
+		return;
+
 	if (unlikely(new_state == CAN_STATE_BUS_OFF)) {
 		cf->can_id |= CAN_ERR_BUSOFF;
 		return;
@@ -645,7 +648,7 @@ struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf)
 	can_skb_prv(skb)->ifindex = dev->ifindex;
 	can_skb_prv(skb)->skbcnt = 0;
 
-	*cf = (struct can_frame *)skb_put(skb, sizeof(struct can_frame));
+	*cf = skb_put(skb, sizeof(struct can_frame));
 	memset(*cf, 0, sizeof(struct can_frame));
 
 	return skb;
@@ -674,7 +677,7 @@ struct sk_buff *alloc_canfd_skb(struct net_device *dev,
 	can_skb_prv(skb)->ifindex = dev->ifindex;
 	can_skb_prv(skb)->skbcnt = 0;
 
-	*cfd = (struct canfd_frame *)skb_put(skb, sizeof(struct canfd_frame));
+	*cfd = skb_put(skb, sizeof(struct canfd_frame));
 	memset(*cfd, 0, sizeof(struct canfd_frame));
 
 	return skb;
@@ -845,7 +848,8 @@ static const struct nla_policy can_policy[IFLA_CAN_MAX + 1] = {
 				= { .len = sizeof(struct can_bittiming_const) },
 };
 
-static int can_validate(struct nlattr *tb[], struct nlattr *data[])
+static int can_validate(struct nlattr *tb[], struct nlattr *data[],
+			struct netlink_ext_ack *extack)
 {
 	bool is_can_fd = false;
 
@@ -877,8 +881,9 @@ static int can_validate(struct nlattr *tb[], struct nlattr *data[])
 	return 0;
 }
 
-static int can_changelink(struct net_device *dev,
-			  struct nlattr *tb[], struct nlattr *data[])
+static int can_changelink(struct net_device *dev, struct nlattr *tb[],
+			  struct nlattr *data[],
+			  struct netlink_ext_ack *extack)
 {
 	struct can_priv *priv = netdev_priv(dev);
 	int err;
@@ -1143,7 +1148,8 @@ nla_put_failure:
 }
 
 static int can_newlink(struct net *src_net, struct net_device *dev,
-		       struct nlattr *tb[], struct nlattr *data[])
+		       struct nlattr *tb[], struct nlattr *data[],
+		       struct netlink_ext_ack *extack)
 {
 	return -EOPNOTSUPP;
 }
diff --git a/drivers/net/can/m_can/m_can.c b/drivers/net/can/m_can/m_can.c
index bf8fdaeb955e..f4947a74b65f 100644
--- a/drivers/net/can/m_can/m_can.c
+++ b/drivers/net/can/m_can/m_can.c
@@ -621,10 +621,8 @@ static int __m_can_get_berr_counter(const struct net_device *dev,
 	return 0;
 }
 
-static int m_can_get_berr_counter(const struct net_device *dev,
-				  struct can_berr_counter *bec)
+static int m_can_clk_start(struct m_can_priv *priv)
 {
-	struct m_can_priv *priv = netdev_priv(dev);
 	int err;
 
 	err = clk_prepare_enable(priv->hclk);
@@ -632,15 +630,31 @@ static int m_can_get_berr_counter(const struct net_device *dev,
 		return err;
 
 	err = clk_prepare_enable(priv->cclk);
-	if (err) {
+	if (err)
 		clk_disable_unprepare(priv->hclk);
-		return err;
-	}
 
-	__m_can_get_berr_counter(dev, bec);
+	return err;
+}
 
+static void m_can_clk_stop(struct m_can_priv *priv)
+{
 	clk_disable_unprepare(priv->cclk);
 	clk_disable_unprepare(priv->hclk);
+}
+
+static int m_can_get_berr_counter(const struct net_device *dev,
+				  struct can_berr_counter *bec)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+	int err;
+
+	err = m_can_clk_start(priv);
+	if (err)
+		return err;
+
+	__m_can_get_berr_counter(dev, bec);
+
+	m_can_clk_stop(priv);
 
 	return 0;
 }
@@ -1276,19 +1290,15 @@ static int m_can_open(struct net_device *dev)
 	struct m_can_priv *priv = netdev_priv(dev);
 	int err;
 
-	err = clk_prepare_enable(priv->hclk);
+	err = m_can_clk_start(priv);
 	if (err)
 		return err;
 
-	err = clk_prepare_enable(priv->cclk);
-	if (err)
-		goto exit_disable_hclk;
-
 	/* open the can device */
 	err = open_candev(dev);
 	if (err) {
 		netdev_err(dev, "failed to open can device\n");
-		goto exit_disable_cclk;
+		goto exit_disable_clks;
 	}
 
 	/* register interrupt handler */
@@ -1310,10 +1320,8 @@ static int m_can_open(struct net_device *dev)
 
 exit_irq_fail:
 	close_candev(dev);
-exit_disable_cclk:
-	clk_disable_unprepare(priv->cclk);
-exit_disable_hclk:
-	clk_disable_unprepare(priv->hclk);
+exit_disable_clks:
+	m_can_clk_stop(priv);
 	return err;
 }
 
@@ -1324,9 +1332,6 @@ static void m_can_stop(struct net_device *dev)
 	/* disable all interrupts */
 	m_can_disable_all_interrupts(priv);
 
-	clk_disable_unprepare(priv->hclk);
-	clk_disable_unprepare(priv->cclk);
-
 	/* set the state as STOPPED */
 	priv->can.state = CAN_STATE_STOPPED;
 }
@@ -1338,6 +1343,7 @@ static int m_can_close(struct net_device *dev)
 	netif_stop_queue(dev);
 	napi_disable(&priv->napi);
 	m_can_stop(dev);
+	m_can_clk_stop(priv);
 	free_irq(dev->irq, dev);
 	close_candev(dev);
 	can_led_event(dev, CAN_LED_EVENT_STOP);
@@ -1489,11 +1495,23 @@ static int register_m_can_dev(struct net_device *dev)
 	return register_candev(dev);
 }
 
+static void m_can_init_ram(struct m_can_priv *priv)
+{
+	int end, i, start;
+
+	/* initialize the entire Message RAM in use to avoid possible
+	 * ECC/parity checksum errors when reading an uninitialized buffer
+	 */
+	start = priv->mcfg[MRAM_SIDF].off;
+	end = priv->mcfg[MRAM_TXB].off +
+		priv->mcfg[MRAM_TXB].num * TXB_ELEMENT_SIZE;
+	for (i = start; i < end; i += 4)
+		writel(0x0, priv->mram_base + i);
+}
+
 static void m_can_of_parse_mram(struct m_can_priv *priv,
 				const u32 *mram_config_vals)
 {
-	int i, start, end;
-
 	priv->mcfg[MRAM_SIDF].off = mram_config_vals[0];
 	priv->mcfg[MRAM_SIDF].num = mram_config_vals[1];
 	priv->mcfg[MRAM_XIDF].off = priv->mcfg[MRAM_SIDF].off +
@@ -1529,15 +1547,7 @@ static void m_can_of_parse_mram(struct m_can_priv *priv,
 		priv->mcfg[MRAM_TXE].off, priv->mcfg[MRAM_TXE].num,
 		priv->mcfg[MRAM_TXB].off, priv->mcfg[MRAM_TXB].num);
 
-	/* initialize the entire Message RAM in use to avoid possible
-	 * ECC/parity checksum errors when reading an uninitialized buffer
-	 */
-	start = priv->mcfg[MRAM_SIDF].off;
-	end = priv->mcfg[MRAM_TXB].off +
-		priv->mcfg[MRAM_TXB].num * TXB_ELEMENT_SIZE;
-	for (i = start; i < end; i += 4)
-		writel(0x0, priv->mram_base + i);
-
+	m_can_init_ram(priv);
 }
 
 static int m_can_plat_probe(struct platform_device *pdev)
@@ -1658,6 +1668,8 @@ failed_ret:
 	return ret;
 }
 
+/* TODO: runtime PM with power down or sleep mode  */
+
 static __maybe_unused int m_can_suspend(struct device *dev)
 {
 	struct net_device *ndev = dev_get_drvdata(dev);
@@ -1666,10 +1678,10 @@ static __maybe_unused int m_can_suspend(struct device *dev)
 	if (netif_running(ndev)) {
 		netif_stop_queue(ndev);
 		netif_device_detach(ndev);
+		m_can_stop(ndev);
+		m_can_clk_stop(priv);
 	}
 
-	/* TODO: enter low power */
-
 	priv->can.state = CAN_STATE_SLEEPING;
 
 	return 0;
@@ -1680,11 +1692,18 @@ static __maybe_unused int m_can_resume(struct device *dev)
 	struct net_device *ndev = dev_get_drvdata(dev);
 	struct m_can_priv *priv = netdev_priv(ndev);
 
-	/* TODO: exit low power */
+	m_can_init_ram(priv);
 
 	priv->can.state = CAN_STATE_ERROR_ACTIVE;
 
 	if (netif_running(ndev)) {
+		int ret;
+
+		ret = m_can_clk_start(priv);
+		if (ret)
+			return ret;
+
+		m_can_start(ndev);
 		netif_device_attach(ndev);
 		netif_start_queue(ndev);
 	}
diff --git a/drivers/net/can/peak_canfd/peak_canfd.c b/drivers/net/can/peak_canfd/peak_canfd.c
index 0d57be5ea97b..85268be0c913 100644
--- a/drivers/net/can/peak_canfd/peak_canfd.c
+++ b/drivers/net/can/peak_canfd/peak_canfd.c
@@ -489,7 +489,7 @@ int peak_canfd_handle_msgs_list(struct peak_canfd_priv *priv,
 				struct pucan_rx_msg *msg_list, int msg_count)
 {
 	void *msg_ptr = msg_list;
-	int i, msg_size;
+	int i, msg_size = 0;
 
 	for (i = 0; i < msg_count; i++) {
 		msg_size = peak_canfd_handle_msg(priv, msg_ptr);
diff --git a/drivers/net/can/slcan.c b/drivers/net/can/slcan.c
index eb7173713bbc..5d067c1b987f 100644
--- a/drivers/net/can/slcan.c
+++ b/drivers/net/can/slcan.c
@@ -216,8 +216,7 @@ static void slc_bump(struct slcan *sl)
 	can_skb_prv(skb)->ifindex = sl->dev->ifindex;
 	can_skb_prv(skb)->skbcnt = 0;
 
-	memcpy(skb_put(skb, sizeof(struct can_frame)),
-	       &cf, sizeof(struct can_frame));
+	skb_put_data(skb, &cf, sizeof(struct can_frame));
 
 	sl->dev->stats.rx_packets++;
 	sl->dev->stats.rx_bytes += cf.can_dlc;
@@ -417,7 +416,7 @@ static int slc_open(struct net_device *dev)
 static void slc_free_netdev(struct net_device *dev)
 {
 	int i = dev->base_addr;
-	free_netdev(dev);
+
 	slcan_devs[i] = NULL;
 }
 
@@ -436,7 +435,8 @@ static const struct net_device_ops slc_netdev_ops = {
 static void slc_setup(struct net_device *dev)
 {
 	dev->netdev_ops		= &slc_netdev_ops;
-	dev->destructor		= slc_free_netdev;
+	dev->needs_free_netdev	= true;
+	dev->priv_destructor	= slc_free_netdev;
 
 	dev->hard_header_len	= 0;
 	dev->addr_len		= 0;
@@ -761,8 +761,6 @@ static void __exit slcan_exit(void)
 		if (sl->tty) {
 			printk(KERN_ERR "%s: tty discipline still running\n",
 			       dev->name);
-			/* Intentionally leak the control block. */
-			dev->destructor = NULL;
 		}
 
 		unregister_netdev(dev);
diff --git a/drivers/net/can/usb/gs_usb.c b/drivers/net/can/usb/gs_usb.c
index eecee7f8dfb7..afcc1312dbaf 100644
--- a/drivers/net/can/usb/gs_usb.c
+++ b/drivers/net/can/usb/gs_usb.c
@@ -265,6 +265,8 @@ static int gs_cmd_reset(struct gs_usb *gsusb, struct gs_can *gsdev)
 			     sizeof(*dm),
 			     1000);
 
+	kfree(dm);
+
 	return rc;
 }
 
diff --git a/drivers/net/can/usb/peak_usb/pcan_usb_core.c b/drivers/net/can/usb/peak_usb/pcan_usb_core.c
index 57913dbbae0a..1ca76e03e965 100644
--- a/drivers/net/can/usb/peak_usb/pcan_usb_core.c
+++ b/drivers/net/can/usb/peak_usb/pcan_usb_core.c
@@ -908,8 +908,6 @@ static int peak_usb_probe(struct usb_interface *intf,
 	const struct peak_usb_adapter *peak_usb_adapter = NULL;
 	int i, err = -ENOMEM;
 
-	usb_dev = interface_to_usbdev(intf);
-
 	/* get corresponding PCAN-USB adapter */
 	for (i = 0; i < ARRAY_SIZE(peak_usb_adapters_list); i++)
 		if (peak_usb_adapters_list[i]->device_id == usb_id_product) {
@@ -920,7 +918,7 @@ static int peak_usb_probe(struct usb_interface *intf,
 	if (!peak_usb_adapter) {
 		/* should never come except device_id bad usage in this file */
 		pr_err("%s: didn't find device id. 0x%x in devices list\n",
-			PCAN_USB_DRIVER_NAME, usb_dev->descriptor.idProduct);
+			PCAN_USB_DRIVER_NAME, usb_id_product);
 		return -ENODEV;
 	}
 
diff --git a/drivers/net/can/vcan.c b/drivers/net/can/vcan.c
index facca33d53e9..a8cb33264ff1 100644
--- a/drivers/net/can/vcan.c
+++ b/drivers/net/can/vcan.c
@@ -152,7 +152,7 @@ static const struct net_device_ops vcan_netdev_ops = {
 static void vcan_setup(struct net_device *dev)
 {
 	dev->type		= ARPHRD_CAN;
-	dev->mtu		= CAN_MTU;
+	dev->mtu		= CANFD_MTU;
 	dev->hard_header_len	= 0;
 	dev->addr_len		= 0;
 	dev->tx_queue_len	= 0;
@@ -163,7 +163,7 @@ static void vcan_setup(struct net_device *dev)
 		dev->flags |= IFF_ECHO;
 
 	dev->netdev_ops		= &vcan_netdev_ops;
-	dev->destructor		= free_netdev;
+	dev->needs_free_netdev	= true;
 }
 
 static struct rtnl_link_ops vcan_link_ops __read_mostly = {
diff --git a/drivers/net/can/vxcan.c b/drivers/net/can/vxcan.c
index 7fbb24795681..8404e8852a0f 100644
--- a/drivers/net/can/vxcan.c
+++ b/drivers/net/can/vxcan.c
@@ -150,20 +150,21 @@ static const struct net_device_ops vxcan_netdev_ops = {
 static void vxcan_setup(struct net_device *dev)
 {
 	dev->type		= ARPHRD_CAN;
-	dev->mtu		= CAN_MTU;
+	dev->mtu		= CANFD_MTU;
 	dev->hard_header_len	= 0;
 	dev->addr_len		= 0;
 	dev->tx_queue_len	= 0;
 	dev->flags		= (IFF_NOARP|IFF_ECHO);
 	dev->netdev_ops		= &vxcan_netdev_ops;
-	dev->destructor		= free_netdev;
+	dev->needs_free_netdev	= true;
 }
 
 /* forward declaration for rtnl_create_link() */
 static struct rtnl_link_ops vxcan_link_ops;
 
 static int vxcan_newlink(struct net *net, struct net_device *dev,
-			 struct nlattr *tb[], struct nlattr *data[])
+			 struct nlattr *tb[], struct nlattr *data[],
+			 struct netlink_ext_ack *extack)
 {
 	struct vxcan_priv *priv;
 	struct net_device *peer;
diff --git a/drivers/net/cris/eth_v10.c b/drivers/net/cris/eth_v10.c
index da020418a652..017f48cdcab9 100644
--- a/drivers/net/cris/eth_v10.c
+++ b/drivers/net/cris/eth_v10.c
@@ -1417,10 +1417,9 @@ static int e100_get_link_ksettings(struct net_device *dev,
 {
 	struct net_local *np = netdev_priv(dev);
 	u32 supported;
-	int err;
 
 	spin_lock_irq(&np->lock);
-	err = mii_ethtool_get_link_ksettings(&np->mii_if, cmd);
+	mii_ethtool_get_link_ksettings(&np->mii_if, cmd);
 	spin_unlock_irq(&np->lock);
 
 	/* The PHY may support 1000baseT, but the Etrax100 does not.  */
@@ -1432,7 +1431,7 @@ static int e100_get_link_ksettings(struct net_device *dev,
 	ethtool_convert_legacy_u32_to_link_mode(cmd->link_modes.supported,
 						supported);
 
-	return err;
+	return 0;
 }
 
 static int e100_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/dsa/Kconfig b/drivers/net/dsa/Kconfig
index 862ee22303c2..83a9bc892a3b 100644
--- a/drivers/net/dsa/Kconfig
+++ b/drivers/net/dsa/Kconfig
@@ -1,13 +1,7 @@
 menu "Distributed Switch Architecture drivers"
 	depends on HAVE_NET_DSA
 
-config NET_DSA_MV88E6060
-	tristate "Marvell 88E6060 ethernet switch chip support"
-	depends on NET_DSA
-	select NET_DSA_TAG_TRAILER
-	---help---
-	  This enables support for the Marvell 88E6060 ethernet switch
-	  chip.
+source "drivers/net/dsa/b53/Kconfig"
 
 config NET_DSA_BCM_SF2
 	tristate "Broadcom Starfighter 2 Ethernet switch support"
@@ -21,19 +15,6 @@ config NET_DSA_BCM_SF2
 	  This enables support for the Broadcom Starfighter 2 Ethernet
 	  switch chips.
 
-source "drivers/net/dsa/b53/Kconfig"
-
-source "drivers/net/dsa/mv88e6xxx/Kconfig"
-
-config NET_DSA_QCA8K
-	tristate "Qualcomm Atheros QCA8K Ethernet switch family support"
-	depends on NET_DSA
-	select NET_DSA_TAG_QCA
-	select REGMAP
-	---help---
-	  This enables support for the Qualcomm Atheros QCA8K Ethernet
-	  switch chips.
-
 config NET_DSA_LOOP
 	tristate "DSA mock-up Ethernet switch chip support"
 	depends on NET_DSA
@@ -50,6 +31,27 @@ config NET_DSA_MT7530
 	  This enables support for the Mediatek MT7530 Ethernet switch
 	  chip.
 
+config NET_DSA_MV88E6060
+	tristate "Marvell 88E6060 ethernet switch chip support"
+	depends on NET_DSA
+	select NET_DSA_TAG_TRAILER
+	---help---
+	  This enables support for the Marvell 88E6060 ethernet switch
+	  chip.
+
+source "drivers/net/dsa/microchip/Kconfig"
+
+source "drivers/net/dsa/mv88e6xxx/Kconfig"
+
+config NET_DSA_QCA8K
+	tristate "Qualcomm Atheros QCA8K Ethernet switch family support"
+	depends on NET_DSA
+	select NET_DSA_TAG_QCA
+	select REGMAP
+	---help---
+	  This enables support for the Qualcomm Atheros QCA8K Ethernet
+	  switch chips.
+
 config NET_DSA_SMSC_LAN9303
 	tristate
 	select NET_DSA_TAG_LAN9303
diff --git a/drivers/net/dsa/Makefile b/drivers/net/dsa/Makefile
index edd630361736..4a5b5bd297ee 100644
--- a/drivers/net/dsa/Makefile
+++ b/drivers/net/dsa/Makefile
@@ -1,11 +1,12 @@
-obj-$(CONFIG_NET_DSA_MV88E6060) += mv88e6060.o
 obj-$(CONFIG_NET_DSA_BCM_SF2)	+= bcm-sf2.o
 bcm-sf2-objs			:= bcm_sf2.o bcm_sf2_cfp.o
-obj-$(CONFIG_NET_DSA_QCA8K)	+= qca8k.o
+obj-$(CONFIG_NET_DSA_LOOP)	+= dsa_loop.o dsa_loop_bdinfo.o
 obj-$(CONFIG_NET_DSA_MT7530)	+= mt7530.o
+obj-$(CONFIG_NET_DSA_MV88E6060) += mv88e6060.o
+obj-$(CONFIG_NET_DSA_QCA8K)	+= qca8k.o
 obj-$(CONFIG_NET_DSA_SMSC_LAN9303) += lan9303-core.o
 obj-$(CONFIG_NET_DSA_SMSC_LAN9303_I2C) += lan9303_i2c.o
 obj-$(CONFIG_NET_DSA_SMSC_LAN9303_MDIO) += lan9303_mdio.o
 obj-y				+= b53/
+obj-y				+= microchip/
 obj-y				+= mv88e6xxx/
-obj-$(CONFIG_NET_DSA_LOOP)	+= dsa_loop.o dsa_loop_bdinfo.o
diff --git a/drivers/net/dsa/b53/b53_common.c b/drivers/net/dsa/b53/b53_common.c
index fa0eece21eef..e68d368e20ac 100644
--- a/drivers/net/dsa/b53/b53_common.c
+++ b/drivers/net/dsa/b53/b53_common.c
@@ -29,7 +29,6 @@
 #include <linux/etherdevice.h>
 #include <linux/if_bridge.h>
 #include <net/dsa.h>
-#include <net/switchdev.h>
 
 #include "b53_regs.h"
 #include "b53_priv.h"
@@ -1056,7 +1055,7 @@ EXPORT_SYMBOL(b53_vlan_del);
 
 int b53_vlan_dump(struct dsa_switch *ds, int port,
 		  struct switchdev_obj_port_vlan *vlan,
-		  int (*cb)(struct switchdev_obj *obj))
+		  switchdev_obj_dump_cb_t *cb)
 {
 	struct b53_device *dev = ds->priv;
 	u16 vid, vid_start = 0, pvid;
@@ -1282,10 +1281,9 @@ static void b53_arl_search_rd(struct b53_device *dev, u8 idx,
 	b53_arl_to_entry(ent, mac_vid, fwd_entry);
 }
 
-static int b53_fdb_copy(struct net_device *dev, int port,
-			const struct b53_arl_entry *ent,
+static int b53_fdb_copy(int port, const struct b53_arl_entry *ent,
 			struct switchdev_obj_port_fdb *fdb,
-			int (*cb)(struct switchdev_obj *obj))
+			switchdev_obj_dump_cb_t *cb)
 {
 	if (!ent->is_valid)
 		return 0;
@@ -1302,10 +1300,9 @@ static int b53_fdb_copy(struct net_device *dev, int port,
 
 int b53_fdb_dump(struct dsa_switch *ds, int port,
 		 struct switchdev_obj_port_fdb *fdb,
-		 int (*cb)(struct switchdev_obj *obj))
+		 switchdev_obj_dump_cb_t *cb)
 {
 	struct b53_device *priv = ds->priv;
-	struct net_device *dev = ds->ports[port].netdev;
 	struct b53_arl_entry results[2];
 	unsigned int count = 0;
 	int ret;
@@ -1321,13 +1318,13 @@ int b53_fdb_dump(struct dsa_switch *ds, int port,
 			return ret;
 
 		b53_arl_search_rd(priv, 0, &results[0]);
-		ret = b53_fdb_copy(dev, port, &results[0], fdb, cb);
+		ret = b53_fdb_copy(port, &results[0], fdb, cb);
 		if (ret)
 			return ret;
 
 		if (priv->num_arl_entries > 2) {
 			b53_arl_search_rd(priv, 1, &results[1]);
-			ret = b53_fdb_copy(dev, port, &results[1], fdb, cb);
+			ret = b53_fdb_copy(port, &results[1], fdb, cb);
 			if (ret)
 				return ret;
 
@@ -1344,7 +1341,7 @@ EXPORT_SYMBOL(b53_fdb_dump);
 int b53_br_join(struct dsa_switch *ds, int port, struct net_device *br)
 {
 	struct b53_device *dev = ds->priv;
-	s8 cpu_port = ds->dst->cpu_port;
+	s8 cpu_port = ds->dst->cpu_dp->index;
 	u16 pvlan, reg;
 	unsigned int i;
 
@@ -1390,7 +1387,7 @@ void b53_br_leave(struct dsa_switch *ds, int port, struct net_device *br)
 {
 	struct b53_device *dev = ds->priv;
 	struct b53_vlan *vl = &dev->vlans[0];
-	s8 cpu_port = ds->dst->cpu_port;
+	s8 cpu_port = ds->dst->cpu_dp->index;
 	unsigned int i;
 	u16 pvlan, reg, pvid;
 
@@ -1992,7 +1989,7 @@ int b53_switch_register(struct b53_device *dev)
 
 	pr_info("found switch: %s, rev %i\n", dev->name, dev->core_rev);
 
-	return dsa_register_switch(dev->ds, dev->ds->dev);
+	return dsa_register_switch(dev->ds);
 }
 EXPORT_SYMBOL(b53_switch_register);
 
diff --git a/drivers/net/dsa/b53/b53_priv.h b/drivers/net/dsa/b53/b53_priv.h
index a9dc90a01438..155a9c48c317 100644
--- a/drivers/net/dsa/b53/b53_priv.h
+++ b/drivers/net/dsa/b53/b53_priv.h
@@ -395,7 +395,7 @@ int b53_vlan_del(struct dsa_switch *ds, int port,
 		 const struct switchdev_obj_port_vlan *vlan);
 int b53_vlan_dump(struct dsa_switch *ds, int port,
 		  struct switchdev_obj_port_vlan *vlan,
-		  int (*cb)(struct switchdev_obj *obj));
+		  switchdev_obj_dump_cb_t *cb);
 int b53_fdb_prepare(struct dsa_switch *ds, int port,
 		    const struct switchdev_obj_port_fdb *fdb,
 		    struct switchdev_trans *trans);
@@ -406,7 +406,7 @@ int b53_fdb_del(struct dsa_switch *ds, int port,
 		const struct switchdev_obj_port_fdb *fdb);
 int b53_fdb_dump(struct dsa_switch *ds, int port,
 		 struct switchdev_obj_port_fdb *fdb,
-		 int (*cb)(struct switchdev_obj *obj));
+		 switchdev_obj_dump_cb_t *cb);
 int b53_mirror_add(struct dsa_switch *ds, int port,
 		   struct dsa_mall_mirror_tc_entry *mirror, bool ingress);
 void b53_mirror_del(struct dsa_switch *ds, int port,
diff --git a/drivers/net/dsa/b53/b53_srab.c b/drivers/net/dsa/b53/b53_srab.c
index 8a62b6a69703..c37ffd1b6833 100644
--- a/drivers/net/dsa/b53/b53_srab.c
+++ b/drivers/net/dsa/b53/b53_srab.c
@@ -364,6 +364,7 @@ static const struct of_device_id b53_srab_of_match[] = {
 	{ .compatible = "brcm,bcm53018-srab" },
 	{ .compatible = "brcm,bcm53019-srab" },
 	{ .compatible = "brcm,bcm5301x-srab" },
+	{ .compatible = "brcm,bcm11360-srab", .data = (void *)BCM58XX_DEVICE_ID },
 	{ .compatible = "brcm,bcm58522-srab", .data = (void *)BCM58XX_DEVICE_ID },
 	{ .compatible = "brcm,bcm58525-srab", .data = (void *)BCM58XX_DEVICE_ID },
 	{ .compatible = "brcm,bcm58535-srab", .data = (void *)BCM58XX_DEVICE_ID },
@@ -371,6 +372,7 @@ static const struct of_device_id b53_srab_of_match[] = {
 	{ .compatible = "brcm,bcm58623-srab", .data = (void *)BCM58XX_DEVICE_ID },
 	{ .compatible = "brcm,bcm58625-srab", .data = (void *)BCM58XX_DEVICE_ID },
 	{ .compatible = "brcm,bcm88312-srab", .data = (void *)BCM58XX_DEVICE_ID },
+	{ .compatible = "brcm,cygnus-srab", .data = (void *)BCM58XX_DEVICE_ID },
 	{ .compatible = "brcm,nsp-srab", .data = (void *)BCM58XX_DEVICE_ID },
 	{ /* sentinel */ },
 };
diff --git a/drivers/net/dsa/bcm_sf2.c b/drivers/net/dsa/bcm_sf2.c
index 2be963252ca5..648f91b58d1e 100644
--- a/drivers/net/dsa/bcm_sf2.c
+++ b/drivers/net/dsa/bcm_sf2.c
@@ -28,7 +28,6 @@
 #include <linux/if_bridge.h>
 #include <linux/brcmphy.h>
 #include <linux/etherdevice.h>
-#include <net/switchdev.h>
 #include <linux/platform_data/b53.h>
 
 #include "bcm_sf2.h"
@@ -228,7 +227,7 @@ static int bcm_sf2_port_setup(struct dsa_switch *ds, int port,
 			      struct phy_device *phy)
 {
 	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
-	s8 cpu_port = ds->dst[ds->index].cpu_port;
+	s8 cpu_port = ds->dst->cpu_dp->index;
 	unsigned int i;
 	u32 reg;
 
@@ -499,10 +498,8 @@ static void bcm_sf2_identify_ports(struct bcm_sf2_priv *priv,
 				   struct device_node *dn)
 {
 	struct device_node *port;
-	const char *phy_mode_str;
 	int mode;
 	unsigned int port_num;
-	int ret;
 
 	priv->moca_port = -1;
 
@@ -516,15 +513,11 @@ static void bcm_sf2_identify_ports(struct bcm_sf2_priv *priv,
 		 * time
 		 */
 		mode = of_get_phy_mode(port);
-		if (mode < 0) {
-			ret = of_property_read_string(port, "phy-mode",
-						      &phy_mode_str);
-			if (ret < 0)
-				continue;
-
-			if (!strcasecmp(phy_mode_str, "internal"))
-				priv->int_phy_mask |= 1 << port_num;
-		}
+		if (mode < 0)
+			continue;
+
+		if (mode == PHY_INTERFACE_MODE_INTERNAL)
+			priv->int_phy_mask |= 1 << port_num;
 
 		if (mode == PHY_INTERFACE_MODE_MOCA)
 			priv->moca_port = port_num;
@@ -807,7 +800,7 @@ static int bcm_sf2_sw_resume(struct dsa_switch *ds)
 static void bcm_sf2_sw_get_wol(struct dsa_switch *ds, int port,
 			       struct ethtool_wolinfo *wol)
 {
-	struct net_device *p = ds->dst[ds->index].master_netdev;
+	struct net_device *p = ds->dst[ds->index].cpu_dp->netdev;
 	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
 	struct ethtool_wolinfo pwol;
 
@@ -830,9 +823,9 @@ static void bcm_sf2_sw_get_wol(struct dsa_switch *ds, int port,
 static int bcm_sf2_sw_set_wol(struct dsa_switch *ds, int port,
 			      struct ethtool_wolinfo *wol)
 {
-	struct net_device *p = ds->dst[ds->index].master_netdev;
+	struct net_device *p = ds->dst[ds->index].cpu_dp->netdev;
 	struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds);
-	s8 cpu_port = ds->dst[ds->index].cpu_port;
+	s8 cpu_port = ds->dst->cpu_dp->index;
 	struct ethtool_wolinfo pwol;
 
 	p->ethtool_ops->get_wol(p, &pwol);
diff --git a/drivers/net/dsa/dsa_loop.c b/drivers/net/dsa/dsa_loop.c
index a19e1781e9bb..fdd8f3872102 100644
--- a/drivers/net/dsa/dsa_loop.c
+++ b/drivers/net/dsa/dsa_loop.c
@@ -14,10 +14,10 @@
 #include <linux/phy.h>
 #include <linux/phy_fixed.h>
 #include <linux/export.h>
+#include <linux/ethtool.h>
 #include <linux/workqueue.h>
 #include <linux/module.h>
 #include <linux/if_bridge.h>
-#include <net/switchdev.h>
 #include <net/dsa.h>
 
 #include "dsa_loop.h"
@@ -27,6 +27,30 @@ struct dsa_loop_vlan {
 	u16 untagged;
 };
 
+struct dsa_loop_mib_entry {
+	char name[ETH_GSTRING_LEN];
+	unsigned long val;
+};
+
+enum dsa_loop_mib_counters {
+	DSA_LOOP_PHY_READ_OK,
+	DSA_LOOP_PHY_READ_ERR,
+	DSA_LOOP_PHY_WRITE_OK,
+	DSA_LOOP_PHY_WRITE_ERR,
+	__DSA_LOOP_CNT_MAX,
+};
+
+static struct dsa_loop_mib_entry dsa_loop_mibs[] = {
+	[DSA_LOOP_PHY_READ_OK]	= { "phy_read_ok", },
+	[DSA_LOOP_PHY_READ_ERR]	= { "phy_read_err", },
+	[DSA_LOOP_PHY_WRITE_OK] = { "phy_write_ok", },
+	[DSA_LOOP_PHY_WRITE_ERR] = { "phy_write_err", },
+};
+
+struct dsa_loop_port {
+	struct dsa_loop_mib_entry mib[__DSA_LOOP_CNT_MAX];
+};
+
 #define DSA_LOOP_VLANS	5
 
 struct dsa_loop_priv {
@@ -34,6 +58,7 @@ struct dsa_loop_priv {
 	unsigned int	port_base;
 	struct dsa_loop_vlan vlans[DSA_LOOP_VLANS];
 	struct net_device *netdev;
+	struct dsa_loop_port ports[DSA_MAX_PORTS];
 	u16 pvid;
 };
 
@@ -48,11 +73,43 @@ static enum dsa_tag_protocol dsa_loop_get_protocol(struct dsa_switch *ds)
 
 static int dsa_loop_setup(struct dsa_switch *ds)
 {
+	struct dsa_loop_priv *ps = ds->priv;
+	unsigned int i;
+
+	for (i = 0; i < ds->num_ports; i++)
+		memcpy(ps->ports[i].mib, dsa_loop_mibs,
+		       sizeof(dsa_loop_mibs));
+
 	dev_dbg(ds->dev, "%s\n", __func__);
 
 	return 0;
 }
 
+static int dsa_loop_get_sset_count(struct dsa_switch *ds)
+{
+	return __DSA_LOOP_CNT_MAX;
+}
+
+static void dsa_loop_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
+{
+	struct dsa_loop_priv *ps = ds->priv;
+	unsigned int i;
+
+	for (i = 0; i < __DSA_LOOP_CNT_MAX; i++)
+		memcpy(data + i * ETH_GSTRING_LEN,
+		       ps->ports[port].mib[i].name, ETH_GSTRING_LEN);
+}
+
+static void dsa_loop_get_ethtool_stats(struct dsa_switch *ds, int port,
+				       uint64_t *data)
+{
+	struct dsa_loop_priv *ps = ds->priv;
+	unsigned int i;
+
+	for (i = 0; i < __DSA_LOOP_CNT_MAX; i++)
+		data[i] = ps->ports[port].mib[i].val;
+}
+
 static int dsa_loop_set_addr(struct dsa_switch *ds, u8 *addr)
 {
 	dev_dbg(ds->dev, "%s\n", __func__);
@@ -64,10 +121,17 @@ static int dsa_loop_phy_read(struct dsa_switch *ds, int port, int regnum)
 {
 	struct dsa_loop_priv *ps = ds->priv;
 	struct mii_bus *bus = ps->bus;
+	int ret;
 
 	dev_dbg(ds->dev, "%s\n", __func__);
 
-	return mdiobus_read_nested(bus, ps->port_base + port, regnum);
+	ret = mdiobus_read_nested(bus, ps->port_base + port, regnum);
+	if (ret < 0)
+		ps->ports[port].mib[DSA_LOOP_PHY_READ_ERR].val++;
+	else
+		ps->ports[port].mib[DSA_LOOP_PHY_READ_OK].val++;
+
+	return ret;
 }
 
 static int dsa_loop_phy_write(struct dsa_switch *ds, int port,
@@ -75,10 +139,17 @@ static int dsa_loop_phy_write(struct dsa_switch *ds, int port,
 {
 	struct dsa_loop_priv *ps = ds->priv;
 	struct mii_bus *bus = ps->bus;
+	int ret;
 
 	dev_dbg(ds->dev, "%s\n", __func__);
 
-	return mdiobus_write_nested(bus, ps->port_base + port, regnum, value);
+	ret = mdiobus_write_nested(bus, ps->port_base + port, regnum, value);
+	if (ret < 0)
+		ps->ports[port].mib[DSA_LOOP_PHY_WRITE_ERR].val++;
+	else
+		ps->ports[port].mib[DSA_LOOP_PHY_WRITE_OK].val++;
+
+	return ret;
 }
 
 static int dsa_loop_port_bridge_join(struct dsa_switch *ds, int port,
@@ -188,7 +259,7 @@ static int dsa_loop_port_vlan_del(struct dsa_switch *ds, int port,
 
 static int dsa_loop_port_vlan_dump(struct dsa_switch *ds, int port,
 				   struct switchdev_obj_port_vlan *vlan,
-				   int (*cb)(struct switchdev_obj *obj))
+				   switchdev_obj_dump_cb_t *cb)
 {
 	struct dsa_loop_priv *ps = ds->priv;
 	struct mii_bus *bus = ps->bus;
@@ -226,6 +297,9 @@ static int dsa_loop_port_vlan_dump(struct dsa_switch *ds, int port,
 static struct dsa_switch_ops dsa_loop_driver = {
 	.get_tag_protocol	= dsa_loop_get_protocol,
 	.setup			= dsa_loop_setup,
+	.get_strings		= dsa_loop_get_strings,
+	.get_ethtool_stats	= dsa_loop_get_ethtool_stats,
+	.get_sset_count		= dsa_loop_get_sset_count,
 	.set_addr		= dsa_loop_set_addr,
 	.phy_read		= dsa_loop_phy_read,
 	.phy_write		= dsa_loop_phy_write,
@@ -272,7 +346,7 @@ static int dsa_loop_drv_probe(struct mdio_device *mdiodev)
 
 	dev_set_drvdata(&mdiodev->dev, ds);
 
-	return dsa_register_switch(ds, ds->dev);
+	return dsa_register_switch(ds);
 }
 
 static void dsa_loop_drv_remove(struct mdio_device *mdiodev)
@@ -294,15 +368,6 @@ static struct mdio_driver dsa_loop_drv = {
 
 #define NUM_FIXED_PHYS	(DSA_LOOP_NUM_PORTS - 2)
 
-static void unregister_fixed_phys(void)
-{
-	unsigned int i;
-
-	for (i = 0; i < NUM_FIXED_PHYS; i++)
-		if (phydevs[i])
-			fixed_phy_unregister(phydevs[i]);
-}
-
 static int __init dsa_loop_init(void)
 {
 	struct fixed_phy_status status = {
@@ -321,8 +386,12 @@ module_init(dsa_loop_init);
 
 static void __exit dsa_loop_exit(void)
 {
+	unsigned int i;
+
 	mdio_driver_unregister(&dsa_loop_drv);
-	unregister_fixed_phys();
+	for (i = 0; i < NUM_FIXED_PHYS; i++)
+		if (phydevs[i])
+			fixed_phy_unregister(phydevs[i]);
 }
 module_exit(dsa_loop_exit);
 
diff --git a/drivers/net/dsa/lan9303-core.c b/drivers/net/dsa/lan9303-core.c
index c8b2423c8ef7..cd76e61f1fca 100644
--- a/drivers/net/dsa/lan9303-core.c
+++ b/drivers/net/dsa/lan9303-core.c
@@ -802,7 +802,7 @@ static int lan9303_register_switch(struct lan9303 *chip)
 	chip->ds->ops = &lan9303_switch_ops;
 	chip->ds->phys_mii_mask = chip->phy_addr_sel_strap ? 0xe : 0x7;
 
-	return dsa_register_switch(chip->ds, chip->dev);
+	return dsa_register_switch(chip->ds);
 }
 
 static void lan9303_probe_reset_gpio(struct lan9303 *chip,
diff --git a/drivers/net/dsa/microchip/Kconfig b/drivers/net/dsa/microchip/Kconfig
new file mode 100644
index 000000000000..a8b8f59099ce
--- /dev/null
+++ b/drivers/net/dsa/microchip/Kconfig
@@ -0,0 +1,12 @@
+menuconfig MICROCHIP_KSZ
+	tristate "Microchip KSZ series switch support"
+	depends on NET_DSA
+	select NET_DSA_TAG_KSZ
+	help
+	  This driver adds support for Microchip KSZ switch chips.
+
+config MICROCHIP_KSZ_SPI_DRIVER
+	tristate "KSZ series SPI connected switch driver"
+	depends on MICROCHIP_KSZ && SPI
+	help
+	  Select to enable support for registering switches configured through SPI.
diff --git a/drivers/net/dsa/microchip/Makefile b/drivers/net/dsa/microchip/Makefile
new file mode 100644
index 000000000000..ed335e29fae8
--- /dev/null
+++ b/drivers/net/dsa/microchip/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_MICROCHIP_KSZ)	        += ksz_common.o
+obj-$(CONFIG_MICROCHIP_KSZ_SPI_DRIVER)	+= ksz_spi.o
diff --git a/drivers/net/dsa/microchip/ksz_9477_reg.h b/drivers/net/dsa/microchip/ksz_9477_reg.h
new file mode 100644
index 000000000000..6aa6752035a1
--- /dev/null
+++ b/drivers/net/dsa/microchip/ksz_9477_reg.h
@@ -0,0 +1,1676 @@
+/*
+ * Microchip KSZ9477 register definitions
+ *
+ * Copyright (C) 2017
+ *
+ * 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 __KSZ9477_REGS_H
+#define __KSZ9477_REGS_H
+
+#define KS_PRIO_M			0x7
+#define KS_PRIO_S			4
+
+/* 0 - Operation */
+#define REG_CHIP_ID0__1			0x0000
+
+#define REG_CHIP_ID1__1			0x0001
+
+#define FAMILY_ID			0x95
+#define FAMILY_ID_94			0x94
+#define FAMILY_ID_95			0x95
+#define FAMILY_ID_85			0x85
+#define FAMILY_ID_98			0x98
+#define FAMILY_ID_88			0x88
+
+#define REG_CHIP_ID2__1			0x0002
+
+#define CHIP_ID_63			0x63
+#define CHIP_ID_66			0x66
+#define CHIP_ID_67			0x67
+#define CHIP_ID_77			0x77
+#define CHIP_ID_93			0x93
+#define CHIP_ID_96			0x96
+#define CHIP_ID_97			0x97
+
+#define REG_CHIP_ID3__1			0x0003
+
+#define SWITCH_REVISION_M		0x0F
+#define SWITCH_REVISION_S		4
+#define SWITCH_RESET			0x01
+
+#define REG_SW_PME_CTRL			0x0006
+
+#define PME_ENABLE			BIT(1)
+#define PME_POLARITY			BIT(0)
+
+#define REG_GLOBAL_OPTIONS		0x000F
+
+#define SW_GIGABIT_ABLE			BIT(6)
+#define SW_REDUNDANCY_ABLE		BIT(5)
+#define SW_AVB_ABLE			BIT(4)
+#define SW_9567_RL_5_2			0xC
+#define SW_9477_SL_5_2			0xD
+
+#define SW_9896_GL_5_1			0xB
+#define SW_9896_RL_5_1			0x8
+#define SW_9896_SL_5_1			0x9
+
+#define SW_9895_GL_4_1			0x7
+#define SW_9895_RL_4_1			0x4
+#define SW_9895_SL_4_1			0x5
+
+#define SW_9896_RL_4_2			0x6
+
+#define SW_9893_RL_2_1			0x0
+#define SW_9893_SL_2_1			0x1
+#define SW_9893_GL_2_1			0x3
+
+#define SW_QW_ABLE			BIT(5)
+#define SW_9893_RN_2_1			0xC
+
+#define REG_SW_INT_STATUS__4		0x0010
+#define REG_SW_INT_MASK__4		0x0014
+
+#define LUE_INT				BIT(31)
+#define TRIG_TS_INT			BIT(30)
+#define APB_TIMEOUT_INT			BIT(29)
+
+#define SWITCH_INT_MASK			(TRIG_TS_INT | APB_TIMEOUT_INT)
+
+#define REG_SW_PORT_INT_STATUS__4	0x0018
+#define REG_SW_PORT_INT_MASK__4		0x001C
+#define REG_SW_PHY_INT_STATUS		0x0020
+#define REG_SW_PHY_INT_ENABLE		0x0024
+
+/* 1 - Global */
+#define REG_SW_GLOBAL_SERIAL_CTRL_0	0x0100
+#define SW_SPARE_REG_2			BIT(7)
+#define SW_SPARE_REG_1			BIT(6)
+#define SW_SPARE_REG_0			BIT(5)
+#define SW_BIG_ENDIAN			BIT(4)
+#define SPI_AUTO_EDGE_DETECTION		BIT(1)
+#define SPI_CLOCK_OUT_RISING_EDGE	BIT(0)
+
+#define REG_SW_GLOBAL_OUTPUT_CTRL__1	0x0103
+#define SW_ENABLE_REFCLKO		BIT(1)
+#define SW_REFCLKO_IS_125MHZ		BIT(0)
+
+#define REG_SW_IBA__4			0x0104
+
+#define SW_IBA_ENABLE			BIT(31)
+#define SW_IBA_DA_MATCH			BIT(30)
+#define SW_IBA_INIT			BIT(29)
+#define SW_IBA_QID_M			0xF
+#define SW_IBA_QID_S			22
+#define SW_IBA_PORT_M			0x2F
+#define SW_IBA_PORT_S			16
+#define SW_IBA_FRAME_TPID_M		0xFFFF
+
+#define REG_SW_APB_TIMEOUT_ADDR__4	0x0108
+
+#define APB_TIMEOUT_ACKNOWLEDGE		BIT(31)
+
+#define REG_SW_IBA_SYNC__1		0x010C
+
+#define REG_SW_IO_STRENGTH__1		0x010D
+#define SW_DRIVE_STRENGTH_M		0x7
+#define SW_DRIVE_STRENGTH_2MA		0
+#define SW_DRIVE_STRENGTH_4MA		1
+#define SW_DRIVE_STRENGTH_8MA		2
+#define SW_DRIVE_STRENGTH_12MA		3
+#define SW_DRIVE_STRENGTH_16MA		4
+#define SW_DRIVE_STRENGTH_20MA		5
+#define SW_DRIVE_STRENGTH_24MA		6
+#define SW_DRIVE_STRENGTH_28MA		7
+#define SW_HI_SPEED_DRIVE_STRENGTH_S	4
+#define SW_LO_SPEED_DRIVE_STRENGTH_S	0
+
+#define REG_SW_IBA_STATUS__4		0x0110
+
+#define SW_IBA_REQ			BIT(31)
+#define SW_IBA_RESP			BIT(30)
+#define SW_IBA_DA_MISMATCH		BIT(14)
+#define SW_IBA_FMT_MISMATCH		BIT(13)
+#define SW_IBA_CODE_ERROR		BIT(12)
+#define SW_IBA_CMD_ERROR		BIT(11)
+#define SW_IBA_CMD_LOC_M		(BIT(6) - 1)
+
+#define REG_SW_IBA_STATES__4		0x0114
+
+#define SW_IBA_BUF_STATE_S		30
+#define SW_IBA_CMD_STATE_S		28
+#define SW_IBA_RESP_STATE_S		26
+#define SW_IBA_STATE_M			0x3
+#define SW_IBA_PACKET_SIZE_M		0x7F
+#define SW_IBA_PACKET_SIZE_S		16
+#define SW_IBA_FMT_ID_M			0xFFFF
+
+#define REG_SW_IBA_RESULT__4		0x0118
+
+#define SW_IBA_SIZE_S			24
+
+#define SW_IBA_RETRY_CNT_M		(BIT(5) - 1)
+
+/* 2 - PHY */
+#define REG_SW_POWER_MANAGEMENT_CTRL	0x0201
+
+#define SW_PLL_POWER_DOWN		BIT(5)
+#define SW_POWER_DOWN_MODE		0x3
+#define SW_ENERGY_DETECTION		1
+#define SW_SOFT_POWER_DOWN		2
+#define SW_POWER_SAVING			3
+
+/* 3 - Operation Control */
+#define REG_SW_OPERATION		0x0300
+
+#define SW_DOUBLE_TAG			BIT(7)
+#define SW_RESET			BIT(1)
+#define SW_START			BIT(0)
+
+#define REG_SW_MAC_ADDR_0		0x0302
+#define REG_SW_MAC_ADDR_1		0x0303
+#define REG_SW_MAC_ADDR_2		0x0304
+#define REG_SW_MAC_ADDR_3		0x0305
+#define REG_SW_MAC_ADDR_4		0x0306
+#define REG_SW_MAC_ADDR_5		0x0307
+
+#define REG_SW_MTU__2			0x0308
+
+#define REG_SW_ISP_TPID__2		0x030A
+
+#define REG_SW_HSR_TPID__2		0x030C
+
+#define REG_AVB_STRATEGY__2		0x030E
+
+#define SW_SHAPING_CREDIT_ACCT		BIT(1)
+#define SW_POLICING_CREDIT_ACCT		BIT(0)
+
+#define REG_SW_LUE_CTRL_0		0x0310
+
+#define SW_VLAN_ENABLE			BIT(7)
+#define SW_DROP_INVALID_VID		BIT(6)
+#define SW_AGE_CNT_M			0x7
+#define SW_AGE_CNT_S			3
+#define SW_RESV_MCAST_ENABLE		BIT(2)
+#define SW_HASH_OPTION_M		0x03
+#define SW_HASH_OPTION_CRC		1
+#define SW_HASH_OPTION_XOR		2
+#define SW_HASH_OPTION_DIRECT		3
+
+#define REG_SW_LUE_CTRL_1		0x0311
+
+#define UNICAST_LEARN_DISABLE		BIT(7)
+#define SW_SRC_ADDR_FILTER		BIT(6)
+#define SW_FLUSH_STP_TABLE		BIT(5)
+#define SW_FLUSH_MSTP_TABLE		BIT(4)
+#define SW_FWD_MCAST_SRC_ADDR		BIT(3)
+#define SW_AGING_ENABLE			BIT(2)
+#define SW_FAST_AGING			BIT(1)
+#define SW_LINK_AUTO_AGING		BIT(0)
+
+#define REG_SW_LUE_CTRL_2		0x0312
+
+#define SW_TRAP_DOUBLE_TAG		BIT(6)
+#define SW_EGRESS_VLAN_FILTER_DYN	BIT(5)
+#define SW_EGRESS_VLAN_FILTER_STA	BIT(4)
+#define SW_FLUSH_OPTION_M		0x3
+#define SW_FLUSH_OPTION_S		2
+#define SW_FLUSH_OPTION_DYN_MAC		1
+#define SW_FLUSH_OPTION_STA_MAC		2
+#define SW_FLUSH_OPTION_BOTH		3
+#define SW_PRIO_M			0x3
+#define SW_PRIO_DA			0
+#define SW_PRIO_SA			1
+#define SW_PRIO_HIGHEST_DA_SA		2
+#define SW_PRIO_LOWEST_DA_SA		3
+
+#define REG_SW_LUE_CTRL_3		0x0313
+
+#define REG_SW_LUE_INT_STATUS		0x0314
+#define REG_SW_LUE_INT_ENABLE		0x0315
+
+#define LEARN_FAIL_INT			BIT(2)
+#define ALMOST_FULL_INT			BIT(1)
+#define WRITE_FAIL_INT			BIT(0)
+
+#define REG_SW_LUE_INDEX_0__2		0x0316
+
+#define ENTRY_INDEX_M			0x0FFF
+
+#define REG_SW_LUE_INDEX_1__2		0x0318
+
+#define FAIL_INDEX_M			0x03FF
+
+#define REG_SW_LUE_INDEX_2__2		0x031A
+
+#define REG_SW_LUE_UNK_UCAST_CTRL__4	0x0320
+
+#define SW_UNK_UCAST_ENABLE		BIT(31)
+
+#define REG_SW_LUE_UNK_MCAST_CTRL__4	0x0324
+
+#define SW_UNK_MCAST_ENABLE		BIT(31)
+
+#define REG_SW_LUE_UNK_VID_CTRL__4	0x0328
+
+#define SW_UNK_VID_ENABLE		BIT(31)
+
+#define REG_SW_MAC_CTRL_0		0x0330
+
+#define SW_NEW_BACKOFF			BIT(7)
+#define SW_CHECK_LENGTH			BIT(3)
+#define SW_PAUSE_UNH_MODE		BIT(1)
+#define SW_AGGR_BACKOFF			BIT(0)
+
+#define REG_SW_MAC_CTRL_1		0x0331
+
+#define MULTICAST_STORM_DISABLE		BIT(6)
+#define SW_BACK_PRESSURE		BIT(5)
+#define FAIR_FLOW_CTRL			BIT(4)
+#define NO_EXC_COLLISION_DROP		BIT(3)
+#define SW_JUMBO_PACKET			BIT(2)
+#define SW_LEGAL_PACKET_DISABLE		BIT(1)
+#define SW_PASS_SHORT_FRAME		BIT(0)
+
+#define REG_SW_MAC_CTRL_2		0x0332
+
+#define SW_REPLACE_VID			BIT(3)
+#define BROADCAST_STORM_RATE_HI		0x07
+
+#define REG_SW_MAC_CTRL_3		0x0333
+
+#define BROADCAST_STORM_RATE_LO		0xFF
+#define BROADCAST_STORM_RATE		0x07FF
+
+#define REG_SW_MAC_CTRL_4		0x0334
+
+#define SW_PASS_PAUSE			BIT(3)
+
+#define REG_SW_MAC_CTRL_5		0x0335
+
+#define SW_OUT_RATE_LIMIT_QUEUE_BASED	BIT(3)
+
+#define REG_SW_MAC_CTRL_6		0x0336
+
+#define SW_MIB_COUNTER_FLUSH		BIT(7)
+#define SW_MIB_COUNTER_FREEZE		BIT(6)
+
+#define REG_SW_MAC_802_1P_MAP_0		0x0338
+#define REG_SW_MAC_802_1P_MAP_1		0x0339
+#define REG_SW_MAC_802_1P_MAP_2		0x033A
+#define REG_SW_MAC_802_1P_MAP_3		0x033B
+
+#define SW_802_1P_MAP_M			KS_PRIO_M
+#define SW_802_1P_MAP_S			KS_PRIO_S
+
+#define REG_SW_MAC_ISP_CTRL		0x033C
+
+#define REG_SW_MAC_TOS_CTRL		0x033E
+
+#define SW_TOS_DSCP_REMARK		BIT(1)
+#define SW_TOS_DSCP_REMAP		BIT(0)
+
+#define REG_SW_MAC_TOS_PRIO_0		0x0340
+#define REG_SW_MAC_TOS_PRIO_1		0x0341
+#define REG_SW_MAC_TOS_PRIO_2		0x0342
+#define REG_SW_MAC_TOS_PRIO_3		0x0343
+#define REG_SW_MAC_TOS_PRIO_4		0x0344
+#define REG_SW_MAC_TOS_PRIO_5		0x0345
+#define REG_SW_MAC_TOS_PRIO_6		0x0346
+#define REG_SW_MAC_TOS_PRIO_7		0x0347
+#define REG_SW_MAC_TOS_PRIO_8		0x0348
+#define REG_SW_MAC_TOS_PRIO_9		0x0349
+#define REG_SW_MAC_TOS_PRIO_10		0x034A
+#define REG_SW_MAC_TOS_PRIO_11		0x034B
+#define REG_SW_MAC_TOS_PRIO_12		0x034C
+#define REG_SW_MAC_TOS_PRIO_13		0x034D
+#define REG_SW_MAC_TOS_PRIO_14		0x034E
+#define REG_SW_MAC_TOS_PRIO_15		0x034F
+#define REG_SW_MAC_TOS_PRIO_16		0x0350
+#define REG_SW_MAC_TOS_PRIO_17		0x0351
+#define REG_SW_MAC_TOS_PRIO_18		0x0352
+#define REG_SW_MAC_TOS_PRIO_19		0x0353
+#define REG_SW_MAC_TOS_PRIO_20		0x0354
+#define REG_SW_MAC_TOS_PRIO_21		0x0355
+#define REG_SW_MAC_TOS_PRIO_22		0x0356
+#define REG_SW_MAC_TOS_PRIO_23		0x0357
+#define REG_SW_MAC_TOS_PRIO_24		0x0358
+#define REG_SW_MAC_TOS_PRIO_25		0x0359
+#define REG_SW_MAC_TOS_PRIO_26		0x035A
+#define REG_SW_MAC_TOS_PRIO_27		0x035B
+#define REG_SW_MAC_TOS_PRIO_28		0x035C
+#define REG_SW_MAC_TOS_PRIO_29		0x035D
+#define REG_SW_MAC_TOS_PRIO_30		0x035E
+#define REG_SW_MAC_TOS_PRIO_31		0x035F
+
+#define REG_SW_MRI_CTRL_0		0x0370
+
+#define SW_IGMP_SNOOP			BIT(6)
+#define SW_IPV6_MLD_OPTION		BIT(3)
+#define SW_IPV6_MLD_SNOOP		BIT(2)
+#define SW_MIRROR_RX_TX			BIT(0)
+
+#define REG_SW_CLASS_D_IP_CTRL__4	0x0374
+
+#define SW_CLASS_D_IP_ENABLE		BIT(31)
+
+#define REG_SW_MRI_CTRL_8		0x0378
+
+#define SW_NO_COLOR_S			6
+#define SW_RED_COLOR_S			4
+#define SW_YELLOW_COLOR_S		2
+#define SW_GREEN_COLOR_S		0
+#define SW_COLOR_M			0x3
+
+#define REG_SW_QM_CTRL__4		0x0390
+
+#define PRIO_SCHEME_SELECT_M		KS_PRIO_M
+#define PRIO_SCHEME_SELECT_S		6
+#define PRIO_MAP_3_HI			0
+#define PRIO_MAP_2_HI			2
+#define PRIO_MAP_0_LO			3
+#define UNICAST_VLAN_BOUNDARY		BIT(1)
+
+#define REG_SW_EEE_QM_CTRL__2		0x03C0
+
+#define REG_SW_EEE_TXQ_WAIT_TIME__2	0x03C2
+
+/* 4 - */
+#define REG_SW_VLAN_ENTRY__4		0x0400
+
+#define VLAN_VALID			BIT(31)
+#define VLAN_FORWARD_OPTION		BIT(27)
+#define VLAN_PRIO_M			KS_PRIO_M
+#define VLAN_PRIO_S			24
+#define VLAN_MSTP_M			0x7
+#define VLAN_MSTP_S			12
+#define VLAN_FID_M			0x7F
+
+#define REG_SW_VLAN_ENTRY_UNTAG__4	0x0404
+#define REG_SW_VLAN_ENTRY_PORTS__4	0x0408
+
+#define REG_SW_VLAN_ENTRY_INDEX__2	0x040C
+
+#define VLAN_INDEX_M			0x0FFF
+
+#define REG_SW_VLAN_CTRL		0x040E
+
+#define VLAN_START			BIT(7)
+#define VLAN_ACTION			0x3
+#define VLAN_WRITE			1
+#define VLAN_READ			2
+#define VLAN_CLEAR			3
+
+#define REG_SW_ALU_INDEX_0		0x0410
+
+#define ALU_FID_INDEX_S			16
+#define ALU_MAC_ADDR_HI			0xFFFF
+
+#define REG_SW_ALU_INDEX_1		0x0414
+
+#define ALU_DIRECT_INDEX_M		(BIT(12) - 1)
+
+#define REG_SW_ALU_CTRL__4		0x0418
+
+#define ALU_VALID_CNT_M			(BIT(14) - 1)
+#define ALU_VALID_CNT_S			16
+#define ALU_START			BIT(7)
+#define ALU_VALID			BIT(6)
+#define ALU_DIRECT			BIT(2)
+#define ALU_ACTION			0x3
+#define ALU_WRITE			1
+#define ALU_READ			2
+#define ALU_SEARCH			3
+
+#define REG_SW_ALU_STAT_CTRL__4		0x041C
+
+#define ALU_STAT_INDEX_M		(BIT(4) - 1)
+#define ALU_STAT_INDEX_S		16
+#define ALU_RESV_MCAST_INDEX_M		(BIT(6) - 1)
+#define ALU_STAT_START			BIT(7)
+#define ALU_RESV_MCAST_ADDR		BIT(1)
+#define ALU_STAT_READ			BIT(0)
+
+#define REG_SW_ALU_VAL_A		0x0420
+
+#define ALU_V_STATIC_VALID		BIT(31)
+#define ALU_V_SRC_FILTER		BIT(30)
+#define ALU_V_DST_FILTER		BIT(29)
+#define ALU_V_PRIO_AGE_CNT_M		(BIT(3) - 1)
+#define ALU_V_PRIO_AGE_CNT_S		26
+#define ALU_V_MSTP_M			0x7
+
+#define REG_SW_ALU_VAL_B		0x0424
+
+#define ALU_V_OVERRIDE			BIT(31)
+#define ALU_V_USE_FID			BIT(30)
+#define ALU_V_PORT_MAP			(BIT(24) - 1)
+
+#define REG_SW_ALU_VAL_C		0x0428
+
+#define ALU_V_FID_M			(BIT(16) - 1)
+#define ALU_V_FID_S			16
+#define ALU_V_MAC_ADDR_HI		0xFFFF
+
+#define REG_SW_ALU_VAL_D		0x042C
+
+#define REG_HSR_ALU_INDEX_0		0x0440
+
+#define REG_HSR_ALU_INDEX_1		0x0444
+
+#define HSR_DST_MAC_INDEX_LO_S		16
+#define HSR_SRC_MAC_INDEX_HI		0xFFFF
+
+#define REG_HSR_ALU_INDEX_2		0x0448
+
+#define HSR_INDEX_MAX			BIT(9)
+#define HSR_DIRECT_INDEX_M		(HSR_INDEX_MAX - 1)
+
+#define REG_HSR_ALU_INDEX_3		0x044C
+
+#define HSR_PATH_INDEX_M		(BIT(4) - 1)
+
+#define REG_HSR_ALU_CTRL__4		0x0450
+
+#define HSR_VALID_CNT_M			(BIT(14) - 1)
+#define HSR_VALID_CNT_S			16
+#define HSR_START			BIT(7)
+#define HSR_VALID			BIT(6)
+#define HSR_SEARCH_END			BIT(5)
+#define HSR_DIRECT			BIT(2)
+#define HSR_ACTION			0x3
+#define HSR_WRITE			1
+#define HSR_READ			2
+#define HSR_SEARCH			3
+
+#define REG_HSR_ALU_VAL_A		0x0454
+
+#define HSR_V_STATIC_VALID		BIT(31)
+#define HSR_V_AGE_CNT_M			(BIT(3) - 1)
+#define HSR_V_AGE_CNT_S			26
+#define HSR_V_PATH_ID_M			(BIT(4) - 1)
+
+#define REG_HSR_ALU_VAL_B		0x0458
+
+#define REG_HSR_ALU_VAL_C		0x045C
+
+#define HSR_V_DST_MAC_ADDR_LO_S		16
+#define HSR_V_SRC_MAC_ADDR_HI		0xFFFF
+
+#define REG_HSR_ALU_VAL_D		0x0460
+
+#define REG_HSR_ALU_VAL_E		0x0464
+
+#define HSR_V_START_SEQ_1_S		16
+#define HSR_V_START_SEQ_2_S		0
+
+#define REG_HSR_ALU_VAL_F		0x0468
+
+#define HSR_V_EXP_SEQ_1_S		16
+#define HSR_V_EXP_SEQ_2_S		0
+
+#define REG_HSR_ALU_VAL_G		0x046C
+
+#define HSR_V_SEQ_CNT_1_S		16
+#define HSR_V_SEQ_CNT_2_S		0
+
+#define HSR_V_SEQ_M			(BIT(16) - 1)
+
+/* 5 - PTP Clock */
+#define REG_PTP_CLK_CTRL		0x0500
+
+#define PTP_STEP_ADJ			BIT(6)
+#define PTP_STEP_DIR			BIT(5)
+#define PTP_READ_TIME			BIT(4)
+#define PTP_LOAD_TIME			BIT(3)
+#define PTP_CLK_ADJ_ENABLE		BIT(2)
+#define PTP_CLK_ENABLE			BIT(1)
+#define PTP_CLK_RESET			BIT(0)
+
+#define REG_PTP_RTC_SUB_NANOSEC__2	0x0502
+
+#define PTP_RTC_SUB_NANOSEC_M		0x0007
+
+#define REG_PTP_RTC_NANOSEC		0x0504
+#define REG_PTP_RTC_NANOSEC_H		0x0504
+#define REG_PTP_RTC_NANOSEC_L		0x0506
+
+#define REG_PTP_RTC_SEC			0x0508
+#define REG_PTP_RTC_SEC_H		0x0508
+#define REG_PTP_RTC_SEC_L		0x050A
+
+#define REG_PTP_SUBNANOSEC_RATE		0x050C
+#define REG_PTP_SUBNANOSEC_RATE_H	0x050C
+
+#define PTP_RATE_DIR			BIT(31)
+#define PTP_TMP_RATE_ENABLE		BIT(30)
+
+#define REG_PTP_SUBNANOSEC_RATE_L	0x050E
+
+#define REG_PTP_RATE_DURATION		0x0510
+#define REG_PTP_RATE_DURATION_H		0x0510
+#define REG_PTP_RATE_DURATION_L		0x0512
+
+#define REG_PTP_MSG_CONF1		0x0514
+
+#define PTP_802_1AS			BIT(7)
+#define PTP_ENABLE			BIT(6)
+#define PTP_ETH_ENABLE			BIT(5)
+#define PTP_IPV4_UDP_ENABLE		BIT(4)
+#define PTP_IPV6_UDP_ENABLE		BIT(3)
+#define PTP_TC_P2P			BIT(2)
+#define PTP_MASTER			BIT(1)
+#define PTP_1STEP			BIT(0)
+
+#define REG_PTP_MSG_CONF2		0x0516
+
+#define PTP_UNICAST_ENABLE		BIT(12)
+#define PTP_ALTERNATE_MASTER		BIT(11)
+#define PTP_ALL_HIGH_PRIO		BIT(10)
+#define PTP_SYNC_CHECK			BIT(9)
+#define PTP_DELAY_CHECK			BIT(8)
+#define PTP_PDELAY_CHECK		BIT(7)
+#define PTP_DROP_SYNC_DELAY_REQ		BIT(5)
+#define PTP_DOMAIN_CHECK		BIT(4)
+#define PTP_UDP_CHECKSUM		BIT(2)
+
+#define REG_PTP_DOMAIN_VERSION		0x0518
+#define PTP_VERSION_M			0xFF00
+#define PTP_DOMAIN_M			0x00FF
+
+#define REG_PTP_UNIT_INDEX__4		0x0520
+
+#define PTP_UNIT_M			0xF
+
+#define PTP_GPIO_INDEX_S		16
+#define PTP_TSI_INDEX_S			8
+#define PTP_TOU_INDEX_S			0
+
+#define REG_PTP_TRIG_STATUS__4		0x0524
+
+#define TRIG_ERROR_S			16
+#define TRIG_DONE_S			0
+
+#define REG_PTP_INT_STATUS__4		0x0528
+
+#define TRIG_INT_S			16
+#define TS_INT_S			0
+
+#define TRIG_UNIT_M			0x7
+#define TS_UNIT_M			0x3
+
+#define REG_PTP_CTRL_STAT__4		0x052C
+
+#define GPIO_IN				BIT(7)
+#define GPIO_OUT			BIT(6)
+#define TS_INT_ENABLE			BIT(5)
+#define TRIG_ACTIVE			BIT(4)
+#define TRIG_ENABLE			BIT(3)
+#define TRIG_RESET			BIT(2)
+#define TS_ENABLE			BIT(1)
+#define TS_RESET			BIT(0)
+
+#define GPIO_CTRL_M			(GPIO_IN | GPIO_OUT)
+
+#define TRIG_CTRL_M			\
+	(TRIG_ACTIVE | TRIG_ENABLE | TRIG_RESET)
+
+#define TS_CTRL_M			\
+	(TS_INT_ENABLE | TS_ENABLE | TS_RESET)
+
+#define REG_TRIG_TARGET_NANOSEC		0x0530
+#define REG_TRIG_TARGET_SEC		0x0534
+
+#define REG_TRIG_CTRL__4		0x0538
+
+#define TRIG_CASCADE_ENABLE		BIT(31)
+#define TRIG_CASCADE_TAIL		BIT(30)
+#define TRIG_CASCADE_UPS_M		0xF
+#define TRIG_CASCADE_UPS_S		26
+#define TRIG_NOW			BIT(25)
+#define TRIG_NOTIFY			BIT(24)
+#define TRIG_EDGE			BIT(23)
+#define TRIG_PATTERN_S			20
+#define TRIG_PATTERN_M			0x7
+#define TRIG_NEG_EDGE			0
+#define TRIG_POS_EDGE			1
+#define TRIG_NEG_PULSE			2
+#define TRIG_POS_PULSE			3
+#define TRIG_NEG_PERIOD			4
+#define TRIG_POS_PERIOD			5
+#define TRIG_REG_OUTPUT			6
+#define TRIG_GPO_S			16
+#define TRIG_GPO_M			0xF
+#define TRIG_CASCADE_ITERATE_CNT_M	0xFFFF
+
+#define REG_TRIG_CYCLE_WIDTH		0x053C
+
+#define REG_TRIG_CYCLE_CNT		0x0540
+
+#define TRIG_CYCLE_CNT_M		0xFFFF
+#define TRIG_CYCLE_CNT_S		16
+#define TRIG_BIT_PATTERN_M		0xFFFF
+
+#define REG_TRIG_ITERATE_TIME		0x0544
+
+#define REG_TRIG_PULSE_WIDTH__4		0x0548
+
+#define TRIG_PULSE_WIDTH_M		0x00FFFFFF
+
+#define REG_TS_CTRL_STAT__4		0x0550
+
+#define TS_EVENT_DETECT_M		0xF
+#define TS_EVENT_DETECT_S		17
+#define TS_EVENT_OVERFLOW		BIT(16)
+#define TS_GPI_M			0xF
+#define TS_GPI_S			8
+#define TS_DETECT_RISE			BIT(7)
+#define TS_DETECT_FALL			BIT(6)
+#define TS_DETECT_S			6
+#define TS_CASCADE_TAIL			BIT(5)
+#define TS_CASCADE_UPS_M		0xF
+#define TS_CASCADE_UPS_S		1
+#define TS_CASCADE_ENABLE		BIT(0)
+
+#define DETECT_RISE			(TS_DETECT_RISE >> TS_DETECT_S)
+#define DETECT_FALL			(TS_DETECT_FALL >> TS_DETECT_S)
+
+#define REG_TS_EVENT_0_NANOSEC		0x0554
+#define REG_TS_EVENT_0_SEC		0x0558
+#define REG_TS_EVENT_0_SUB_NANOSEC	0x055C
+
+#define REG_TS_EVENT_1_NANOSEC		0x0560
+#define REG_TS_EVENT_1_SEC		0x0564
+#define REG_TS_EVENT_1_SUB_NANOSEC	0x0568
+
+#define REG_TS_EVENT_2_NANOSEC		0x056C
+#define REG_TS_EVENT_2_SEC		0x0570
+#define REG_TS_EVENT_2_SUB_NANOSEC	0x0574
+
+#define REG_TS_EVENT_3_NANOSEC		0x0578
+#define REG_TS_EVENT_3_SEC		0x057C
+#define REG_TS_EVENT_3_SUB_NANOSEC	0x0580
+
+#define REG_TS_EVENT_4_NANOSEC		0x0584
+#define REG_TS_EVENT_4_SEC		0x0588
+#define REG_TS_EVENT_4_SUB_NANOSEC	0x058C
+
+#define REG_TS_EVENT_5_NANOSEC		0x0590
+#define REG_TS_EVENT_5_SEC		0x0594
+#define REG_TS_EVENT_5_SUB_NANOSEC	0x0598
+
+#define REG_TS_EVENT_6_NANOSEC		0x059C
+#define REG_TS_EVENT_6_SEC		0x05A0
+#define REG_TS_EVENT_6_SUB_NANOSEC	0x05A4
+
+#define REG_TS_EVENT_7_NANOSEC		0x05A8
+#define REG_TS_EVENT_7_SEC		0x05AC
+#define REG_TS_EVENT_7_SUB_NANOSEC	0x05B0
+
+#define TS_EVENT_EDGE_M			0x1
+#define TS_EVENT_EDGE_S			30
+#define TS_EVENT_NANOSEC_M		(BIT(30) - 1)
+
+#define TS_EVENT_SUB_NANOSEC_M		0x7
+
+#define TS_EVENT_SAMPLE			\
+	(REG_TS_EVENT_1_NANOSEC - REG_TS_EVENT_0_NANOSEC)
+
+#define PORT_CTRL_ADDR(port, addr)	((addr) | (((port) + 1) << 12))
+
+#define REG_GLOBAL_RR_INDEX__1		0x0600
+
+/* DLR */
+#define REG_DLR_SRC_PORT__4		0x0604
+
+#define DLR_SRC_PORT_UNICAST		BIT(31)
+#define DLR_SRC_PORT_M			0x3
+#define DLR_SRC_PORT_BOTH		0
+#define DLR_SRC_PORT_EACH		1
+
+#define REG_DLR_IP_ADDR__4		0x0608
+
+#define REG_DLR_CTRL__1			0x0610
+
+#define DLR_RESET_SEQ_ID		BIT(3)
+#define DLR_BACKUP_AUTO_ON		BIT(2)
+#define DLR_BEACON_TX_ENABLE		BIT(1)
+#define DLR_ASSIST_ENABLE		BIT(0)
+
+#define REG_DLR_STATE__1		0x0611
+
+#define DLR_NODE_STATE_M		0x3
+#define DLR_NODE_STATE_S		1
+#define DLR_NODE_STATE_IDLE		0
+#define DLR_NODE_STATE_FAULT		1
+#define DLR_NODE_STATE_NORMAL		2
+#define DLR_RING_STATE_FAULT		0
+#define DLR_RING_STATE_NORMAL		1
+
+#define REG_DLR_PRECEDENCE__1		0x0612
+
+#define REG_DLR_BEACON_INTERVAL__4	0x0614
+
+#define REG_DLR_BEACON_TIMEOUT__4	0x0618
+
+#define REG_DLR_TIMEOUT_WINDOW__4	0x061C
+
+#define DLR_TIMEOUT_WINDOW_M		(BIT(22) - 1)
+
+#define REG_DLR_VLAN_ID__2		0x0620
+
+#define DLR_VLAN_ID_M			(BIT(12) - 1)
+
+#define REG_DLR_DEST_ADDR_0		0x0622
+#define REG_DLR_DEST_ADDR_1		0x0623
+#define REG_DLR_DEST_ADDR_2		0x0624
+#define REG_DLR_DEST_ADDR_3		0x0625
+#define REG_DLR_DEST_ADDR_4		0x0626
+#define REG_DLR_DEST_ADDR_5		0x0627
+
+#define REG_DLR_PORT_MAP__4		0x0628
+
+#define REG_DLR_CLASS__1		0x062C
+
+#define DLR_FRAME_QID_M			0x3
+
+/* HSR */
+#define REG_HSR_PORT_MAP__4		0x0640
+
+#define REG_HSR_ALU_CTRL_0__1		0x0644
+
+#define HSR_DUPLICATE_DISCARD		BIT(7)
+#define HSR_NODE_UNICAST		BIT(6)
+#define HSR_AGE_CNT_DEFAULT_M		0x7
+#define HSR_AGE_CNT_DEFAULT_S		3
+#define HSR_LEARN_MCAST_DISABLE		BIT(2)
+#define HSR_HASH_OPTION_M		0x3
+#define HSR_HASH_DISABLE		0
+#define HSR_HASH_UPPER_BITS		1
+#define HSR_HASH_LOWER_BITS		2
+#define HSR_HASH_XOR_BOTH_BITS		3
+
+#define REG_HSR_ALU_CTRL_1__1		0x0645
+
+#define HSR_LEARN_UCAST_DISABLE		BIT(7)
+#define HSR_FLUSH_TABLE			BIT(5)
+#define HSR_PROC_MCAST_SRC		BIT(3)
+#define HSR_AGING_ENABLE		BIT(2)
+
+#define REG_HSR_ALU_CTRL_2__2		0x0646
+
+#define REG_HSR_ALU_AGE_PERIOD__4	0x0648
+
+#define REG_HSR_ALU_INT_STATUS__1	0x064C
+#define REG_HSR_ALU_INT_MASK__1		0x064D
+
+#define HSR_WINDOW_OVERFLOW_INT		BIT(3)
+#define HSR_LEARN_FAIL_INT		BIT(2)
+#define HSR_ALMOST_FULL_INT		BIT(1)
+#define HSR_WRITE_FAIL_INT		BIT(0)
+
+#define REG_HSR_ALU_ENTRY_0__2		0x0650
+
+#define HSR_ENTRY_INDEX_M		(BIT(10) - 1)
+#define HSR_FAIL_INDEX_M		(BIT(8) - 1)
+
+#define REG_HSR_ALU_ENTRY_1__2		0x0652
+
+#define HSR_FAIL_LEARN_INDEX_M		(BIT(8) - 1)
+
+#define REG_HSR_ALU_ENTRY_3__2		0x0654
+
+#define HSR_CPU_ACCESS_ENTRY_INDEX_M	(BIT(8) - 1)
+
+/* 0 - Operation */
+#define REG_PORT_DEFAULT_VID		0x0000
+
+#define REG_PORT_CUSTOM_VID		0x0002
+#define REG_PORT_AVB_SR_1_VID		0x0004
+#define REG_PORT_AVB_SR_2_VID		0x0006
+
+#define REG_PORT_AVB_SR_1_TYPE		0x0008
+#define REG_PORT_AVB_SR_2_TYPE		0x000A
+
+#define REG_PORT_PME_STATUS		0x0013
+#define REG_PORT_PME_CTRL		0x0017
+
+#define PME_WOL_MAGICPKT		BIT(2)
+#define PME_WOL_LINKUP			BIT(1)
+#define PME_WOL_ENERGY			BIT(0)
+
+#define REG_PORT_INT_STATUS		0x001B
+#define REG_PORT_INT_MASK		0x001F
+
+#define PORT_SGMII_INT			BIT(3)
+#define PORT_PTP_INT			BIT(2)
+#define PORT_PHY_INT			BIT(1)
+#define PORT_ACL_INT			BIT(0)
+
+#define PORT_INT_MASK			\
+	(PORT_SGMII_INT | PORT_PTP_INT | PORT_PHY_INT | PORT_ACL_INT)
+
+#define REG_PORT_CTRL_0			0x0020
+
+#define PORT_MAC_LOOPBACK		BIT(7)
+#define PORT_FORCE_TX_FLOW_CTRL		BIT(4)
+#define PORT_FORCE_RX_FLOW_CTRL		BIT(3)
+#define PORT_TAIL_TAG_ENABLE		BIT(2)
+#define PORT_QUEUE_SPLIT_ENABLE		0x3
+
+#define REG_PORT_CTRL_1			0x0021
+
+#define PORT_SRP_ENABLE			0x3
+
+#define REG_PORT_STATUS_0		0x0030
+
+#define PORT_INTF_SPEED_M		0x3
+#define PORT_INTF_SPEED_S		3
+#define PORT_INTF_FULL_DUPLEX		BIT(2)
+#define PORT_TX_FLOW_CTRL		BIT(1)
+#define PORT_RX_FLOW_CTRL		BIT(0)
+
+#define REG_PORT_STATUS_1		0x0034
+
+/* 1 - PHY */
+#define REG_PORT_PHY_CTRL		0x0100
+
+#define PORT_PHY_RESET			BIT(15)
+#define PORT_PHY_LOOPBACK		BIT(14)
+#define PORT_SPEED_100MBIT		BIT(13)
+#define PORT_AUTO_NEG_ENABLE		BIT(12)
+#define PORT_POWER_DOWN			BIT(11)
+#define PORT_ISOLATE			BIT(10)
+#define PORT_AUTO_NEG_RESTART		BIT(9)
+#define PORT_FULL_DUPLEX		BIT(8)
+#define PORT_COLLISION_TEST		BIT(7)
+#define PORT_SPEED_1000MBIT		BIT(6)
+
+#define REG_PORT_PHY_STATUS		0x0102
+
+#define PORT_100BT4_CAPABLE		BIT(15)
+#define PORT_100BTX_FD_CAPABLE		BIT(14)
+#define PORT_100BTX_CAPABLE		BIT(13)
+#define PORT_10BT_FD_CAPABLE		BIT(12)
+#define PORT_10BT_CAPABLE		BIT(11)
+#define PORT_EXTENDED_STATUS		BIT(8)
+#define PORT_MII_SUPPRESS_CAPABLE	BIT(6)
+#define PORT_AUTO_NEG_ACKNOWLEDGE	BIT(5)
+#define PORT_REMOTE_FAULT		BIT(4)
+#define PORT_AUTO_NEG_CAPABLE		BIT(3)
+#define PORT_LINK_STATUS		BIT(2)
+#define PORT_JABBER_DETECT		BIT(1)
+#define PORT_EXTENDED_CAPABILITY	BIT(0)
+
+#define REG_PORT_PHY_ID_HI		0x0104
+#define REG_PORT_PHY_ID_LO		0x0106
+
+#define KSZ9477_ID_HI			0x0022
+#define KSZ9477_ID_LO			0x1622
+
+#define REG_PORT_PHY_AUTO_NEGOTIATION	0x0108
+
+#define PORT_AUTO_NEG_NEXT_PAGE		BIT(15)
+#define PORT_AUTO_NEG_REMOTE_FAULT	BIT(13)
+#define PORT_AUTO_NEG_ASYM_PAUSE	BIT(11)
+#define PORT_AUTO_NEG_SYM_PAUSE		BIT(10)
+#define PORT_AUTO_NEG_100BT4		BIT(9)
+#define PORT_AUTO_NEG_100BTX_FD		BIT(8)
+#define PORT_AUTO_NEG_100BTX		BIT(7)
+#define PORT_AUTO_NEG_10BT_FD		BIT(6)
+#define PORT_AUTO_NEG_10BT		BIT(5)
+#define PORT_AUTO_NEG_SELECTOR		0x001F
+#define PORT_AUTO_NEG_802_3		0x0001
+
+#define PORT_AUTO_NEG_PAUSE		\
+	(PORT_AUTO_NEG_ASYM_PAUSE | PORT_AUTO_NEG_SYM_PAUSE)
+
+#define REG_PORT_PHY_REMOTE_CAPABILITY	0x010A
+
+#define PORT_REMOTE_NEXT_PAGE		BIT(15)
+#define PORT_REMOTE_ACKNOWLEDGE		BIT(14)
+#define PORT_REMOTE_REMOTE_FAULT	BIT(13)
+#define PORT_REMOTE_ASYM_PAUSE		BIT(11)
+#define PORT_REMOTE_SYM_PAUSE		BIT(10)
+#define PORT_REMOTE_100BTX_FD		BIT(8)
+#define PORT_REMOTE_100BTX		BIT(7)
+#define PORT_REMOTE_10BT_FD		BIT(6)
+#define PORT_REMOTE_10BT		BIT(5)
+
+#define REG_PORT_PHY_1000_CTRL		0x0112
+
+#define PORT_AUTO_NEG_MANUAL		BIT(12)
+#define PORT_AUTO_NEG_MASTER		BIT(11)
+#define PORT_AUTO_NEG_MASTER_PREFERRED	BIT(10)
+#define PORT_AUTO_NEG_1000BT_FD		BIT(9)
+#define PORT_AUTO_NEG_1000BT		BIT(8)
+
+#define REG_PORT_PHY_1000_STATUS	0x0114
+
+#define PORT_MASTER_FAULT		BIT(15)
+#define PORT_LOCAL_MASTER		BIT(14)
+#define PORT_LOCAL_RX_OK		BIT(13)
+#define PORT_REMOTE_RX_OK		BIT(12)
+#define PORT_REMOTE_1000BT_FD		BIT(11)
+#define PORT_REMOTE_1000BT		BIT(10)
+#define PORT_REMOTE_IDLE_CNT_M		0x0F
+
+#define PORT_PHY_1000_STATIC_STATUS	\
+	(PORT_LOCAL_RX_OK |		\
+	PORT_REMOTE_RX_OK |		\
+	PORT_REMOTE_1000BT_FD |		\
+	PORT_REMOTE_1000BT)
+
+#define REG_PORT_PHY_MMD_SETUP		0x011A
+
+#define PORT_MMD_OP_MODE_M		0x3
+#define PORT_MMD_OP_MODE_S		14
+#define PORT_MMD_OP_INDEX		0
+#define PORT_MMD_OP_DATA_NO_INCR	1
+#define PORT_MMD_OP_DATA_INCR_RW	2
+#define PORT_MMD_OP_DATA_INCR_W		3
+#define PORT_MMD_DEVICE_ID_M		0x1F
+
+#define MMD_SETUP(mode, dev)		\
+	(((u16)(mode) << PORT_MMD_OP_MODE_S) | (dev))
+
+#define REG_PORT_PHY_MMD_INDEX_DATA	0x011C
+
+#define MMD_DEVICE_ID_DSP		1
+
+#define MMD_DSP_SQI_CHAN_A		0xAC
+#define MMD_DSP_SQI_CHAN_B		0xAD
+#define MMD_DSP_SQI_CHAN_C		0xAE
+#define MMD_DSP_SQI_CHAN_D		0xAF
+
+#define DSP_SQI_ERR_DETECTED		BIT(15)
+#define DSP_SQI_AVG_ERR			0x7FFF
+
+#define MMD_DEVICE_ID_COMMON		2
+
+#define MMD_DEVICE_ID_EEE_ADV		7
+
+#define MMD_EEE_ADV			0x3C
+#define EEE_ADV_100MBIT			BIT(1)
+#define EEE_ADV_1GBIT			BIT(2)
+
+#define MMD_EEE_LP_ADV			0x3D
+#define MMD_EEE_MSG_CODE		0x3F
+
+#define MMD_DEVICE_ID_AFED		0x1C
+
+#define REG_PORT_PHY_EXTENDED_STATUS	0x011E
+
+#define PORT_100BTX_FD_ABLE		BIT(15)
+#define PORT_100BTX_ABLE		BIT(14)
+#define PORT_10BT_FD_ABLE		BIT(13)
+#define PORT_10BT_ABLE			BIT(12)
+
+#define REG_PORT_SGMII_ADDR__4		0x0200
+#define PORT_SGMII_AUTO_INCR		BIT(23)
+#define PORT_SGMII_DEVICE_ID_M		0x1F
+#define PORT_SGMII_DEVICE_ID_S		16
+#define PORT_SGMII_ADDR_M		(BIT(21) - 1)
+
+#define REG_PORT_SGMII_DATA__4		0x0204
+#define PORT_SGMII_DATA_M		(BIT(16) - 1)
+
+#define MMD_DEVICE_ID_PMA		0x01
+#define MMD_DEVICE_ID_PCS		0x03
+#define MMD_DEVICE_ID_PHY_XS		0x04
+#define MMD_DEVICE_ID_DTE_XS		0x05
+#define MMD_DEVICE_ID_AN		0x07
+#define MMD_DEVICE_ID_VENDOR_CTRL	0x1E
+#define MMD_DEVICE_ID_VENDOR_MII	0x1F
+
+#define SR_MII				MMD_DEVICE_ID_VENDOR_MII
+
+#define MMD_SR_MII_CTRL			0x0000
+
+#define SR_MII_RESET			BIT(15)
+#define SR_MII_LOOPBACK			BIT(14)
+#define SR_MII_SPEED_100MBIT		BIT(13)
+#define SR_MII_AUTO_NEG_ENABLE		BIT(12)
+#define SR_MII_POWER_DOWN		BIT(11)
+#define SR_MII_AUTO_NEG_RESTART		BIT(9)
+#define SR_MII_FULL_DUPLEX		BIT(8)
+#define SR_MII_SPEED_1000MBIT		BIT(6)
+
+#define MMD_SR_MII_STATUS		0x0001
+#define MMD_SR_MII_ID_1			0x0002
+#define MMD_SR_MII_ID_2			0x0003
+#define MMD_SR_MII_AUTO_NEGOTIATION	0x0004
+
+#define SR_MII_AUTO_NEG_NEXT_PAGE	BIT(15)
+#define SR_MII_AUTO_NEG_REMOTE_FAULT_M	0x3
+#define SR_MII_AUTO_NEG_REMOTE_FAULT_S	12
+#define SR_MII_AUTO_NEG_NO_ERROR	0
+#define SR_MII_AUTO_NEG_OFFLINE		1
+#define SR_MII_AUTO_NEG_LINK_FAILURE	2
+#define SR_MII_AUTO_NEG_ERROR		3
+#define SR_MII_AUTO_NEG_PAUSE_M		0x3
+#define SR_MII_AUTO_NEG_PAUSE_S		7
+#define SR_MII_AUTO_NEG_NO_PAUSE	0
+#define SR_MII_AUTO_NEG_ASYM_PAUSE_TX	1
+#define SR_MII_AUTO_NEG_SYM_PAUSE	2
+#define SR_MII_AUTO_NEG_ASYM_PAUSE_RX	3
+#define SR_MII_AUTO_NEG_HALF_DUPLEX	BIT(6)
+#define SR_MII_AUTO_NEG_FULL_DUPLEX	BIT(5)
+
+#define MMD_SR_MII_REMOTE_CAPABILITY	0x0005
+#define MMD_SR_MII_AUTO_NEG_EXP		0x0006
+#define MMD_SR_MII_AUTO_NEG_EXT		0x000F
+
+#define MMD_SR_MII_DIGITAL_CTRL_1	0x8000
+
+#define MMD_SR_MII_AUTO_NEG_CTRL	0x8001
+
+#define SR_MII_8_BIT			BIT(8)
+#define SR_MII_SGMII_LINK_UP		BIT(4)
+#define SR_MII_TX_CFG_PHY_MASTER	BIT(3)
+#define SR_MII_PCS_MODE_M		0x3
+#define SR_MII_PCS_MODE_S		1
+#define SR_MII_PCS_SGMII		2
+#define SR_MII_AUTO_NEG_COMPLETE_INTR	BIT(0)
+
+#define MMD_SR_MII_AUTO_NEG_STATUS	0x8002
+
+#define SR_MII_STAT_LINK_UP		BIT(4)
+#define SR_MII_STAT_M			0x3
+#define SR_MII_STAT_S			2
+#define SR_MII_STAT_10_MBPS		0
+#define SR_MII_STAT_100_MBPS		1
+#define SR_MII_STAT_1000_MBPS		2
+#define SR_MII_STAT_FULL_DUPLEX		BIT(1)
+
+#define MMD_SR_MII_PHY_CTRL		0x80A0
+
+#define SR_MII_PHY_LANE_SEL_M		0xF
+#define SR_MII_PHY_LANE_SEL_S		8
+#define SR_MII_PHY_WRITE		BIT(1)
+#define SR_MII_PHY_START_BUSY		BIT(0)
+
+#define MMD_SR_MII_PHY_ADDR		0x80A1
+
+#define SR_MII_PHY_ADDR_M		(BIT(16) - 1)
+
+#define MMD_SR_MII_PHY_DATA		0x80A2
+
+#define SR_MII_PHY_DATA_M		(BIT(16) - 1)
+
+#define SR_MII_PHY_JTAG_CHIP_ID_HI	0x000C
+#define SR_MII_PHY_JTAG_CHIP_ID_LO	0x000D
+
+#define REG_PORT_PHY_REMOTE_LB_LED	0x0122
+
+#define PORT_REMOTE_LOOPBACK		BIT(8)
+#define PORT_LED_SELECT			(3 << 6)
+#define PORT_LED_CTRL			(3 << 4)
+#define PORT_LED_CTRL_TEST		BIT(3)
+#define PORT_10BT_PREAMBLE		BIT(2)
+#define PORT_LINK_MD_10BT_ENABLE	BIT(1)
+#define PORT_LINK_MD_PASS		BIT(0)
+
+#define REG_PORT_PHY_LINK_MD		0x0124
+
+#define PORT_START_CABLE_DIAG		BIT(15)
+#define PORT_TX_DISABLE			BIT(14)
+#define PORT_CABLE_DIAG_PAIR_M		0x3
+#define PORT_CABLE_DIAG_PAIR_S		12
+#define PORT_CABLE_DIAG_SELECT_M	0x3
+#define PORT_CABLE_DIAG_SELECT_S	10
+#define PORT_CABLE_DIAG_RESULT_M	0x3
+#define PORT_CABLE_DIAG_RESULT_S	8
+#define PORT_CABLE_STAT_NORMAL		0
+#define PORT_CABLE_STAT_OPEN		1
+#define PORT_CABLE_STAT_SHORT		2
+#define PORT_CABLE_STAT_FAILED		3
+#define PORT_CABLE_FAULT_COUNTER	0x00FF
+
+#define REG_PORT_PHY_PMA_STATUS		0x0126
+
+#define PORT_1000_LINK_GOOD		BIT(1)
+#define PORT_100_LINK_GOOD		BIT(0)
+
+#define REG_PORT_PHY_DIGITAL_STATUS	0x0128
+
+#define PORT_LINK_DETECT		BIT(14)
+#define PORT_SIGNAL_DETECT		BIT(13)
+#define PORT_PHY_STAT_MDI		BIT(12)
+#define PORT_PHY_STAT_MASTER		BIT(11)
+
+#define REG_PORT_PHY_RXER_COUNTER	0x012A
+
+#define REG_PORT_PHY_INT_ENABLE		0x0136
+#define REG_PORT_PHY_INT_STATUS		0x0137
+
+#define JABBER_INT			BIT(7)
+#define RX_ERR_INT			BIT(6)
+#define PAGE_RX_INT			BIT(5)
+#define PARALLEL_DETECT_FAULT_INT	BIT(4)
+#define LINK_PARTNER_ACK_INT		BIT(3)
+#define LINK_DOWN_INT			BIT(2)
+#define REMOTE_FAULT_INT		BIT(1)
+#define LINK_UP_INT			BIT(0)
+
+#define REG_PORT_PHY_DIGITAL_DEBUG_1	0x0138
+
+#define PORT_REG_CLK_SPEED_25_MHZ	BIT(14)
+#define PORT_PHY_FORCE_MDI		BIT(7)
+#define PORT_PHY_AUTO_MDIX_DISABLE	BIT(6)
+
+/* Same as PORT_PHY_LOOPBACK */
+#define PORT_PHY_PCS_LOOPBACK		BIT(0)
+
+#define REG_PORT_PHY_DIGITAL_DEBUG_2	0x013A
+
+#define REG_PORT_PHY_DIGITAL_DEBUG_3	0x013C
+
+#define PORT_100BT_FIXED_LATENCY	BIT(15)
+
+#define REG_PORT_PHY_PHY_CTRL		0x013E
+
+#define PORT_INT_PIN_HIGH		BIT(14)
+#define PORT_ENABLE_JABBER		BIT(9)
+#define PORT_STAT_SPEED_1000MBIT	BIT(6)
+#define PORT_STAT_SPEED_100MBIT		BIT(5)
+#define PORT_STAT_SPEED_10MBIT		BIT(4)
+#define PORT_STAT_FULL_DUPLEX		BIT(3)
+
+/* Same as PORT_PHY_STAT_MASTER */
+#define PORT_STAT_MASTER		BIT(2)
+#define PORT_RESET			BIT(1)
+#define PORT_LINK_STATUS_FAIL		BIT(0)
+
+/* 3 - xMII */
+#define REG_PORT_XMII_CTRL_0		0x0300
+
+#define PORT_SGMII_SEL			BIT(7)
+#define PORT_MII_FULL_DUPLEX		BIT(6)
+#define PORT_MII_100MBIT		BIT(4)
+#define PORT_GRXC_ENABLE		BIT(0)
+
+#define REG_PORT_XMII_CTRL_1		0x0301
+
+#define PORT_RMII_CLK_SEL		BIT(7)
+/* S1 */
+#define PORT_MII_1000MBIT_S1		BIT(6)
+/* S2 */
+#define PORT_MII_NOT_1GBIT		BIT(6)
+#define PORT_MII_SEL_EDGE		BIT(5)
+#define PORT_RGMII_ID_IG_ENABLE		BIT(4)
+#define PORT_RGMII_ID_EG_ENABLE		BIT(3)
+#define PORT_MII_MAC_MODE		BIT(2)
+#define PORT_MII_SEL_M			0x3
+/* S1 */
+#define PORT_MII_SEL_S1			0x0
+#define PORT_RMII_SEL_S1		0x1
+#define PORT_GMII_SEL_S1		0x2
+#define PORT_RGMII_SEL_S1		0x3
+/* S2 */
+#define PORT_RGMII_SEL			0x0
+#define PORT_RMII_SEL			0x1
+#define PORT_GMII_SEL			0x2
+#define PORT_MII_SEL			0x3
+
+/* 4 - MAC */
+#define REG_PORT_MAC_CTRL_0		0x0400
+
+#define PORT_BROADCAST_STORM		BIT(1)
+#define PORT_JUMBO_FRAME		BIT(0)
+
+#define REG_PORT_MAC_CTRL_1		0x0401
+
+#define PORT_BACK_PRESSURE		BIT(3)
+#define PORT_PASS_ALL			BIT(0)
+
+#define REG_PORT_MAC_CTRL_2		0x0402
+
+#define PORT_100BT_EEE_DISABLE		BIT(7)
+#define PORT_1000BT_EEE_DISABLE		BIT(6)
+
+#define REG_PORT_MAC_IN_RATE_LIMIT	0x0403
+
+#define PORT_IN_PORT_BASED_S		6
+#define PORT_RATE_PACKET_BASED_S	5
+#define PORT_IN_FLOW_CTRL_S		4
+#define PORT_COUNT_IFG_S		1
+#define PORT_COUNT_PREAMBLE_S		0
+#define PORT_IN_PORT_BASED		BIT(6)
+#define PORT_IN_PACKET_BASED		BIT(5)
+#define PORT_IN_FLOW_CTRL		BIT(4)
+#define PORT_IN_LIMIT_MODE_M		0x3
+#define PORT_IN_LIMIT_MODE_S		2
+#define PORT_IN_ALL			0
+#define PORT_IN_UNICAST			1
+#define PORT_IN_MULTICAST		2
+#define PORT_IN_BROADCAST		3
+#define PORT_COUNT_IFG			BIT(1)
+#define PORT_COUNT_PREAMBLE		BIT(0)
+
+#define REG_PORT_IN_RATE_0		0x0410
+#define REG_PORT_IN_RATE_1		0x0411
+#define REG_PORT_IN_RATE_2		0x0412
+#define REG_PORT_IN_RATE_3		0x0413
+#define REG_PORT_IN_RATE_4		0x0414
+#define REG_PORT_IN_RATE_5		0x0415
+#define REG_PORT_IN_RATE_6		0x0416
+#define REG_PORT_IN_RATE_7		0x0417
+
+#define REG_PORT_OUT_RATE_0		0x0420
+#define REG_PORT_OUT_RATE_1		0x0421
+#define REG_PORT_OUT_RATE_2		0x0422
+#define REG_PORT_OUT_RATE_3		0x0423
+
+#define PORT_RATE_LIMIT_M		(BIT(7) - 1)
+
+/* 5 - MIB Counters */
+#define REG_PORT_MIB_CTRL_STAT__4	0x0500
+
+#define MIB_COUNTER_OVERFLOW		BIT(31)
+#define MIB_COUNTER_VALID		BIT(30)
+#define MIB_COUNTER_READ		BIT(25)
+#define MIB_COUNTER_FLUSH_FREEZE	BIT(24)
+#define MIB_COUNTER_INDEX_M		(BIT(8) - 1)
+#define MIB_COUNTER_INDEX_S		16
+#define MIB_COUNTER_DATA_HI_M		0xF
+
+#define REG_PORT_MIB_DATA		0x0504
+
+/* 6 - ACL */
+#define REG_PORT_ACL_0			0x0600
+
+#define ACL_FIRST_RULE_M		0xF
+
+#define REG_PORT_ACL_1			0x0601
+
+#define ACL_MODE_M			0x3
+#define ACL_MODE_S			4
+#define ACL_MODE_DISABLE		0
+#define ACL_MODE_LAYER_2		1
+#define ACL_MODE_LAYER_3		2
+#define ACL_MODE_LAYER_4		3
+#define ACL_ENABLE_M			0x3
+#define ACL_ENABLE_S			2
+#define ACL_ENABLE_2_COUNT		0
+#define ACL_ENABLE_2_TYPE		1
+#define ACL_ENABLE_2_MAC		2
+#define ACL_ENABLE_2_BOTH		3
+#define ACL_ENABLE_3_IP			1
+#define ACL_ENABLE_3_SRC_DST_COMP	2
+#define ACL_ENABLE_4_PROTOCOL		0
+#define ACL_ENABLE_4_TCP_PORT_COMP	1
+#define ACL_ENABLE_4_UDP_PORT_COMP	2
+#define ACL_ENABLE_4_TCP_SEQN_COMP	3
+#define ACL_SRC				BIT(1)
+#define ACL_EQUAL			BIT(0)
+
+#define REG_PORT_ACL_2			0x0602
+#define REG_PORT_ACL_3			0x0603
+
+#define ACL_MAX_PORT			0xFFFF
+
+#define REG_PORT_ACL_4			0x0604
+#define REG_PORT_ACL_5			0x0605
+
+#define ACL_MIN_PORT			0xFFFF
+#define ACL_IP_ADDR			0xFFFFFFFF
+#define ACL_TCP_SEQNUM			0xFFFFFFFF
+
+#define REG_PORT_ACL_6			0x0606
+
+#define ACL_RESERVED			0xF8
+#define ACL_PORT_MODE_M			0x3
+#define ACL_PORT_MODE_S			1
+#define ACL_PORT_MODE_DISABLE		0
+#define ACL_PORT_MODE_EITHER		1
+#define ACL_PORT_MODE_IN_RANGE		2
+#define ACL_PORT_MODE_OUT_OF_RANGE	3
+
+#define REG_PORT_ACL_7			0x0607
+
+#define ACL_TCP_FLAG_ENABLE		BIT(0)
+
+#define REG_PORT_ACL_8			0x0608
+
+#define ACL_TCP_FLAG_M			0xFF
+
+#define REG_PORT_ACL_9			0x0609
+
+#define ACL_TCP_FLAG			0xFF
+#define ACL_ETH_TYPE			0xFFFF
+#define ACL_IP_M			0xFFFFFFFF
+
+#define REG_PORT_ACL_A			0x060A
+
+#define ACL_PRIO_MODE_M			0x3
+#define ACL_PRIO_MODE_S			6
+#define ACL_PRIO_MODE_DISABLE		0
+#define ACL_PRIO_MODE_HIGHER		1
+#define ACL_PRIO_MODE_LOWER		2
+#define ACL_PRIO_MODE_REPLACE		3
+#define ACL_PRIO_M			KS_PRIO_M
+#define ACL_PRIO_S			3
+#define ACL_VLAN_PRIO_REPLACE		BIT(2)
+#define ACL_VLAN_PRIO_M			KS_PRIO_M
+#define ACL_VLAN_PRIO_HI_M		0x3
+
+#define REG_PORT_ACL_B			0x060B
+
+#define ACL_VLAN_PRIO_LO_M		0x8
+#define ACL_VLAN_PRIO_S			7
+#define ACL_MAP_MODE_M			0x3
+#define ACL_MAP_MODE_S			5
+#define ACL_MAP_MODE_DISABLE		0
+#define ACL_MAP_MODE_OR			1
+#define ACL_MAP_MODE_AND		2
+#define ACL_MAP_MODE_REPLACE		3
+
+#define ACL_CNT_M			(BIT(11) - 1)
+#define ACL_CNT_S			5
+
+#define REG_PORT_ACL_C			0x060C
+
+#define REG_PORT_ACL_D			0x060D
+#define ACL_MSEC_UNIT			BIT(6)
+#define ACL_INTR_MODE			BIT(5)
+#define ACL_PORT_MAP			0x7F
+
+#define REG_PORT_ACL_E			0x060E
+#define REG_PORT_ACL_F			0x060F
+
+#define REG_PORT_ACL_BYTE_EN_MSB	0x0610
+#define REG_PORT_ACL_BYTE_EN_LSB	0x0611
+
+#define ACL_ACTION_START		0xA
+#define ACL_ACTION_LEN			4
+#define ACL_INTR_CNT_START		0xD
+#define ACL_RULESET_START		0xE
+#define ACL_RULESET_LEN			2
+#define ACL_TABLE_LEN			16
+
+#define ACL_ACTION_ENABLE		0x003C
+#define ACL_MATCH_ENABLE		0x7FC3
+#define ACL_RULESET_ENABLE		0x8003
+#define ACL_BYTE_ENABLE			0xFFFF
+
+#define REG_PORT_ACL_CTRL_0		0x0612
+
+#define PORT_ACL_WRITE_DONE		BIT(6)
+#define PORT_ACL_READ_DONE		BIT(5)
+#define PORT_ACL_WRITE			BIT(4)
+#define PORT_ACL_INDEX_M		0xF
+
+#define REG_PORT_ACL_CTRL_1		0x0613
+
+/* 8 - Classification and Policing */
+#define REG_PORT_MRI_MIRROR_CTRL	0x0800
+
+#define PORT_MIRROR_RX			BIT(6)
+#define PORT_MIRROR_TX			BIT(5)
+#define PORT_MIRROR_SNIFFER		BIT(1)
+
+#define REG_PORT_MRI_PRIO_CTRL		0x0801
+
+#define PORT_HIGHEST_PRIO		BIT(7)
+#define PORT_OR_PRIO			BIT(6)
+#define PORT_MAC_PRIO_ENABLE		BIT(4)
+#define PORT_VLAN_PRIO_ENABLE		BIT(3)
+#define PORT_802_1P_PRIO_ENABLE		BIT(2)
+#define PORT_DIFFSERV_PRIO_ENABLE	BIT(1)
+#define PORT_ACL_PRIO_ENABLE		BIT(0)
+
+#define REG_PORT_MRI_MAC_CTRL		0x0802
+
+#define PORT_USER_PRIO_CEILING		BIT(7)
+#define PORT_DROP_NON_VLAN		BIT(4)
+#define PORT_DROP_TAG			BIT(3)
+#define PORT_BASED_PRIO_M		KS_PRIO_M
+#define PORT_BASED_PRIO_S		0
+
+#define REG_PORT_MRI_AUTHEN_CTRL	0x0803
+
+#define PORT_ACL_ENABLE			BIT(2)
+#define PORT_AUTHEN_MODE		0x3
+#define PORT_AUTHEN_PASS		0
+#define PORT_AUTHEN_BLOCK		1
+#define PORT_AUTHEN_TRAP		2
+
+#define REG_PORT_MRI_INDEX__4		0x0804
+
+#define MRI_INDEX_P_M			0x7
+#define MRI_INDEX_P_S			16
+#define MRI_INDEX_Q_M			0x3
+#define MRI_INDEX_Q_S			0
+
+#define REG_PORT_MRI_TC_MAP__4		0x0808
+
+#define PORT_TC_MAP_M			0xf
+#define PORT_TC_MAP_S			4
+
+#define REG_PORT_MRI_POLICE_CTRL__4	0x080C
+
+#define POLICE_DROP_ALL			BIT(10)
+#define POLICE_PACKET_TYPE_M		0x3
+#define POLICE_PACKET_TYPE_S		8
+#define POLICE_PACKET_DROPPED		0
+#define POLICE_PACKET_GREEN		1
+#define POLICE_PACKET_YELLOW		2
+#define POLICE_PACKET_RED		3
+#define PORT_BASED_POLICING		BIT(7)
+#define NON_DSCP_COLOR_M		0x3
+#define NON_DSCP_COLOR_S		5
+#define COLOR_MARK_ENABLE		BIT(4)
+#define COLOR_REMAP_ENABLE		BIT(3)
+#define POLICE_DROP_SRP			BIT(2)
+#define POLICE_COLOR_NOT_AWARE		BIT(1)
+#define POLICE_ENABLE			BIT(0)
+
+#define REG_PORT_POLICE_COLOR_0__4	0x0810
+#define REG_PORT_POLICE_COLOR_1__4	0x0814
+#define REG_PORT_POLICE_COLOR_2__4	0x0818
+#define REG_PORT_POLICE_COLOR_3__4	0x081C
+
+#define POLICE_COLOR_MAP_S		2
+#define POLICE_COLOR_MAP_M		(BIT(POLICE_COLOR_MAP_S) - 1)
+
+#define REG_PORT_POLICE_RATE__4		0x0820
+
+#define POLICE_CIR_S			16
+#define POLICE_PIR_S			0
+
+#define REG_PORT_POLICE_BURST_SIZE__4	0x0824
+
+#define POLICE_BURST_SIZE_M		0x3FFF
+#define POLICE_CBS_S			16
+#define POLICE_PBS_S			0
+
+#define REG_PORT_WRED_PM_CTRL_0__4	0x0830
+
+#define WRED_PM_CTRL_M			(BIT(11) - 1)
+
+#define WRED_PM_MAX_THRESHOLD_S		16
+#define WRED_PM_MIN_THRESHOLD_S		0
+
+#define REG_PORT_WRED_PM_CTRL_1__4	0x0834
+
+#define WRED_PM_MULTIPLIER_S		16
+#define WRED_PM_AVG_QUEUE_SIZE_S	0
+
+#define REG_PORT_WRED_QUEUE_CTRL_0__4	0x0840
+#define REG_PORT_WRED_QUEUE_CTRL_1__4	0x0844
+
+#define REG_PORT_WRED_QUEUE_PMON__4	0x0848
+
+#define WRED_RANDOM_DROP_ENABLE		BIT(31)
+#define WRED_PMON_FLUSH			BIT(30)
+#define WRED_DROP_GYR_DISABLE		BIT(29)
+#define WRED_DROP_YR_DISABLE		BIT(28)
+#define WRED_DROP_R_DISABLE		BIT(27)
+#define WRED_DROP_ALL			BIT(26)
+#define WRED_PMON_M			(BIT(24) - 1)
+
+/* 9 - Shaping */
+
+#define REG_PORT_MTI_QUEUE_INDEX__4	0x0900
+
+#define REG_PORT_MTI_QUEUE_CTRL_0__4	0x0904
+
+#define MTI_PVID_REPLACE		BIT(0)
+
+#define REG_PORT_MTI_QUEUE_CTRL_0	0x0914
+
+#define MTI_SCHEDULE_MODE_M		0x3
+#define MTI_SCHEDULE_MODE_S		6
+#define MTI_SCHEDULE_STRICT_PRIO	0
+#define MTI_SCHEDULE_WRR		2
+#define MTI_SHAPING_M			0x3
+#define MTI_SHAPING_S			4
+#define MTI_SHAPING_OFF			0
+#define MTI_SHAPING_SRP			1
+#define MTI_SHAPING_TIME_AWARE		2
+
+#define REG_PORT_MTI_QUEUE_CTRL_1	0x0915
+
+#define MTI_TX_RATIO_M			(BIT(7) - 1)
+
+#define REG_PORT_MTI_QUEUE_CTRL_2__2	0x0916
+#define REG_PORT_MTI_HI_WATER_MARK	0x0916
+#define REG_PORT_MTI_QUEUE_CTRL_3__2	0x0918
+#define REG_PORT_MTI_LO_WATER_MARK	0x0918
+#define REG_PORT_MTI_QUEUE_CTRL_4__2	0x091A
+#define REG_PORT_MTI_CREDIT_INCREMENT	0x091A
+
+/* A - QM */
+
+#define REG_PORT_QM_CTRL__4		0x0A00
+
+#define PORT_QM_DROP_PRIO_M		0x3
+
+#define REG_PORT_VLAN_MEMBERSHIP__4	0x0A04
+
+#define REG_PORT_QM_QUEUE_INDEX__4	0x0A08
+
+#define PORT_QM_QUEUE_INDEX_S		24
+#define PORT_QM_BURST_SIZE_S		16
+#define PORT_QM_MIN_RESV_SPACE_M	(BIT(11) - 1)
+
+#define REG_PORT_QM_WATER_MARK__4	0x0A0C
+
+#define PORT_QM_HI_WATER_MARK_S		16
+#define PORT_QM_LO_WATER_MARK_S		0
+#define PORT_QM_WATER_MARK_M		(BIT(11) - 1)
+
+#define REG_PORT_QM_TX_CNT_0__4		0x0A10
+
+#define PORT_QM_TX_CNT_USED_S		0
+#define PORT_QM_TX_CNT_M		(BIT(11) - 1)
+
+#define REG_PORT_QM_TX_CNT_1__4		0x0A14
+
+#define PORT_QM_TX_CNT_CALCULATED_S	16
+#define PORT_QM_TX_CNT_AVAIL_S		0
+
+/* B - LUE */
+#define REG_PORT_LUE_CTRL		0x0B00
+
+#define PORT_VLAN_LOOKUP_VID_0		BIT(7)
+#define PORT_INGRESS_FILTER		BIT(6)
+#define PORT_DISCARD_NON_VID		BIT(5)
+#define PORT_MAC_BASED_802_1X		BIT(4)
+#define PORT_SRC_ADDR_FILTER		BIT(3)
+
+#define REG_PORT_LUE_MSTP_INDEX		0x0B01
+
+#define REG_PORT_LUE_MSTP_STATE		0x0B04
+
+#define PORT_TX_ENABLE			BIT(2)
+#define PORT_RX_ENABLE			BIT(1)
+#define PORT_LEARN_DISABLE		BIT(0)
+
+/* C - PTP */
+
+#define REG_PTP_PORT_RX_DELAY__2	0x0C00
+#define REG_PTP_PORT_TX_DELAY__2	0x0C02
+#define REG_PTP_PORT_ASYM_DELAY__2	0x0C04
+
+#define REG_PTP_PORT_XDELAY_TS		0x0C08
+#define REG_PTP_PORT_XDELAY_TS_H	0x0C08
+#define REG_PTP_PORT_XDELAY_TS_L	0x0C0A
+
+#define REG_PTP_PORT_SYNC_TS		0x0C0C
+#define REG_PTP_PORT_SYNC_TS_H		0x0C0C
+#define REG_PTP_PORT_SYNC_TS_L		0x0C0E
+
+#define REG_PTP_PORT_PDRESP_TS		0x0C10
+#define REG_PTP_PORT_PDRESP_TS_H	0x0C10
+#define REG_PTP_PORT_PDRESP_TS_L	0x0C12
+
+#define REG_PTP_PORT_TX_INT_STATUS__2	0x0C14
+#define REG_PTP_PORT_TX_INT_ENABLE__2	0x0C16
+
+#define PTP_PORT_SYNC_INT		BIT(15)
+#define PTP_PORT_XDELAY_REQ_INT		BIT(14)
+#define PTP_PORT_PDELAY_RESP_INT	BIT(13)
+
+#define REG_PTP_PORT_LINK_DELAY__4	0x0C18
+
+#define PRIO_QUEUES			4
+#define RX_PRIO_QUEUES			8
+
+#define KS_PRIO_IN_REG			2
+
+#define TOTAL_PORT_NUM			7
+
+#define KSZ9477_COUNTER_NUM		0x20
+#define TOTAL_KSZ9477_COUNTER_NUM	(KSZ9477_COUNTER_NUM + 2 + 2)
+
+#define SWITCH_COUNTER_NUM		KSZ9477_COUNTER_NUM
+#define TOTAL_SWITCH_COUNTER_NUM	TOTAL_KSZ9477_COUNTER_NUM
+
+#define P_BCAST_STORM_CTRL		REG_PORT_MAC_CTRL_0
+#define P_PRIO_CTRL			REG_PORT_MRI_PRIO_CTRL
+#define P_MIRROR_CTRL			REG_PORT_MRI_MIRROR_CTRL
+#define P_STP_CTRL			REG_PORT_LUE_MSTP_STATE
+#define P_PHY_CTRL			REG_PORT_PHY_CTRL
+#define P_NEG_RESTART_CTRL		REG_PORT_PHY_CTRL
+#define P_LINK_STATUS			REG_PORT_PHY_STATUS
+#define P_SPEED_STATUS			REG_PORT_PHY_PHY_CTRL
+#define P_RATE_LIMIT_CTRL		REG_PORT_MAC_IN_RATE_LIMIT
+
+#define S_LINK_AGING_CTRL		REG_SW_LUE_CTRL_1
+#define S_MIRROR_CTRL			REG_SW_MRI_CTRL_0
+#define S_REPLACE_VID_CTRL		REG_SW_MAC_CTRL_2
+#define S_802_1P_PRIO_CTRL		REG_SW_MAC_802_1P_MAP_0
+#define S_TOS_PRIO_CTRL			REG_SW_MAC_TOS_PRIO_0
+#define S_FLUSH_TABLE_CTRL		REG_SW_LUE_CTRL_1
+
+#define SW_FLUSH_DYN_MAC_TABLE		SW_FLUSH_MSTP_TABLE
+
+#define MAX_TIMESTAMP_UNIT		2
+#define MAX_TRIG_UNIT			3
+#define MAX_TIMESTAMP_EVENT_UNIT	8
+#define MAX_GPIO			4
+
+#define PTP_TRIG_UNIT_M			(BIT(MAX_TRIG_UNIT) - 1)
+#define PTP_TS_UNIT_M			(BIT(MAX_TIMESTAMP_UNIT) - 1)
+
+/* Driver set switch broadcast storm protection at 10% rate. */
+#define BROADCAST_STORM_PROT_RATE	10
+
+/* 148,800 frames * 67 ms / 100 */
+#define BROADCAST_STORM_VALUE		9969
+
+#endif /* KSZ9477_REGS_H */
diff --git a/drivers/net/dsa/microchip/ksz_common.c b/drivers/net/dsa/microchip/ksz_common.c
new file mode 100644
index 000000000000..b313ecdf2919
--- /dev/null
+++ b/drivers/net/dsa/microchip/ksz_common.c
@@ -0,0 +1,1279 @@
+/*
+ * Microchip switch driver main logic
+ *
+ * Copyright (C) 2017
+ *
+ * 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/delay.h>
+#include <linux/export.h>
+#include <linux/gpio.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_data/microchip-ksz.h>
+#include <linux/phy.h>
+#include <linux/etherdevice.h>
+#include <linux/if_bridge.h>
+#include <net/dsa.h>
+#include <net/switchdev.h>
+
+#include "ksz_priv.h"
+
+static const struct {
+	int index;
+	char string[ETH_GSTRING_LEN];
+} mib_names[TOTAL_SWITCH_COUNTER_NUM] = {
+	{ 0x00, "rx_hi" },
+	{ 0x01, "rx_undersize" },
+	{ 0x02, "rx_fragments" },
+	{ 0x03, "rx_oversize" },
+	{ 0x04, "rx_jabbers" },
+	{ 0x05, "rx_symbol_err" },
+	{ 0x06, "rx_crc_err" },
+	{ 0x07, "rx_align_err" },
+	{ 0x08, "rx_mac_ctrl" },
+	{ 0x09, "rx_pause" },
+	{ 0x0A, "rx_bcast" },
+	{ 0x0B, "rx_mcast" },
+	{ 0x0C, "rx_ucast" },
+	{ 0x0D, "rx_64_or_less" },
+	{ 0x0E, "rx_65_127" },
+	{ 0x0F, "rx_128_255" },
+	{ 0x10, "rx_256_511" },
+	{ 0x11, "rx_512_1023" },
+	{ 0x12, "rx_1024_1522" },
+	{ 0x13, "rx_1523_2000" },
+	{ 0x14, "rx_2001" },
+	{ 0x15, "tx_hi" },
+	{ 0x16, "tx_late_col" },
+	{ 0x17, "tx_pause" },
+	{ 0x18, "tx_bcast" },
+	{ 0x19, "tx_mcast" },
+	{ 0x1A, "tx_ucast" },
+	{ 0x1B, "tx_deferred" },
+	{ 0x1C, "tx_total_col" },
+	{ 0x1D, "tx_exc_col" },
+	{ 0x1E, "tx_single_col" },
+	{ 0x1F, "tx_mult_col" },
+	{ 0x80, "rx_total" },
+	{ 0x81, "tx_total" },
+	{ 0x82, "rx_discards" },
+	{ 0x83, "tx_discards" },
+};
+
+static void ksz_cfg(struct ksz_device *dev, u32 addr, u8 bits, bool set)
+{
+	u8 data;
+
+	ksz_read8(dev, addr, &data);
+	if (set)
+		data |= bits;
+	else
+		data &= ~bits;
+	ksz_write8(dev, addr, data);
+}
+
+static void ksz_cfg32(struct ksz_device *dev, u32 addr, u32 bits, bool set)
+{
+	u32 data;
+
+	ksz_read32(dev, addr, &data);
+	if (set)
+		data |= bits;
+	else
+		data &= ~bits;
+	ksz_write32(dev, addr, data);
+}
+
+static void ksz_port_cfg(struct ksz_device *dev, int port, int offset, u8 bits,
+			 bool set)
+{
+	u32 addr;
+	u8 data;
+
+	addr = PORT_CTRL_ADDR(port, offset);
+	ksz_read8(dev, addr, &data);
+
+	if (set)
+		data |= bits;
+	else
+		data &= ~bits;
+
+	ksz_write8(dev, addr, data);
+}
+
+static void ksz_port_cfg32(struct ksz_device *dev, int port, int offset,
+			   u32 bits, bool set)
+{
+	u32 addr;
+	u32 data;
+
+	addr = PORT_CTRL_ADDR(port, offset);
+	ksz_read32(dev, addr, &data);
+
+	if (set)
+		data |= bits;
+	else
+		data &= ~bits;
+
+	ksz_write32(dev, addr, data);
+}
+
+static int wait_vlan_ctrl_ready(struct ksz_device *dev, u32 waiton, int timeout)
+{
+	u8 data;
+
+	do {
+		ksz_read8(dev, REG_SW_VLAN_CTRL, &data);
+		if (!(data & waiton))
+			break;
+		usleep_range(1, 10);
+	} while (timeout-- > 0);
+
+	if (timeout <= 0)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+static int get_vlan_table(struct dsa_switch *ds, u16 vid, u32 *vlan_table)
+{
+	struct ksz_device *dev = ds->priv;
+	int ret;
+
+	mutex_lock(&dev->vlan_mutex);
+
+	ksz_write16(dev, REG_SW_VLAN_ENTRY_INDEX__2, vid & VLAN_INDEX_M);
+	ksz_write8(dev, REG_SW_VLAN_CTRL, VLAN_READ | VLAN_START);
+
+	/* wait to be cleared */
+	ret = wait_vlan_ctrl_ready(dev, VLAN_START, 1000);
+	if (ret < 0) {
+		dev_dbg(dev->dev, "Failed to read vlan table\n");
+		goto exit;
+	}
+
+	ksz_read32(dev, REG_SW_VLAN_ENTRY__4, &vlan_table[0]);
+	ksz_read32(dev, REG_SW_VLAN_ENTRY_UNTAG__4, &vlan_table[1]);
+	ksz_read32(dev, REG_SW_VLAN_ENTRY_PORTS__4, &vlan_table[2]);
+
+	ksz_write8(dev, REG_SW_VLAN_CTRL, 0);
+
+exit:
+	mutex_unlock(&dev->vlan_mutex);
+
+	return ret;
+}
+
+static int set_vlan_table(struct dsa_switch *ds, u16 vid, u32 *vlan_table)
+{
+	struct ksz_device *dev = ds->priv;
+	int ret;
+
+	mutex_lock(&dev->vlan_mutex);
+
+	ksz_write32(dev, REG_SW_VLAN_ENTRY__4, vlan_table[0]);
+	ksz_write32(dev, REG_SW_VLAN_ENTRY_UNTAG__4, vlan_table[1]);
+	ksz_write32(dev, REG_SW_VLAN_ENTRY_PORTS__4, vlan_table[2]);
+
+	ksz_write16(dev, REG_SW_VLAN_ENTRY_INDEX__2, vid & VLAN_INDEX_M);
+	ksz_write8(dev, REG_SW_VLAN_CTRL, VLAN_START | VLAN_WRITE);
+
+	/* wait to be cleared */
+	ret = wait_vlan_ctrl_ready(dev, VLAN_START, 1000);
+	if (ret < 0) {
+		dev_dbg(dev->dev, "Failed to write vlan table\n");
+		goto exit;
+	}
+
+	ksz_write8(dev, REG_SW_VLAN_CTRL, 0);
+
+	/* update vlan cache table */
+	dev->vlan_cache[vid].table[0] = vlan_table[0];
+	dev->vlan_cache[vid].table[1] = vlan_table[1];
+	dev->vlan_cache[vid].table[2] = vlan_table[2];
+
+exit:
+	mutex_unlock(&dev->vlan_mutex);
+
+	return ret;
+}
+
+static void read_table(struct dsa_switch *ds, u32 *table)
+{
+	struct ksz_device *dev = ds->priv;
+
+	ksz_read32(dev, REG_SW_ALU_VAL_A, &table[0]);
+	ksz_read32(dev, REG_SW_ALU_VAL_B, &table[1]);
+	ksz_read32(dev, REG_SW_ALU_VAL_C, &table[2]);
+	ksz_read32(dev, REG_SW_ALU_VAL_D, &table[3]);
+}
+
+static void write_table(struct dsa_switch *ds, u32 *table)
+{
+	struct ksz_device *dev = ds->priv;
+
+	ksz_write32(dev, REG_SW_ALU_VAL_A, table[0]);
+	ksz_write32(dev, REG_SW_ALU_VAL_B, table[1]);
+	ksz_write32(dev, REG_SW_ALU_VAL_C, table[2]);
+	ksz_write32(dev, REG_SW_ALU_VAL_D, table[3]);
+}
+
+static int wait_alu_ready(struct ksz_device *dev, u32 waiton, int timeout)
+{
+	u32 data;
+
+	do {
+		ksz_read32(dev, REG_SW_ALU_CTRL__4, &data);
+		if (!(data & waiton))
+			break;
+		usleep_range(1, 10);
+	} while (timeout-- > 0);
+
+	if (timeout <= 0)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+static int wait_alu_sta_ready(struct ksz_device *dev, u32 waiton, int timeout)
+{
+	u32 data;
+
+	do {
+		ksz_read32(dev, REG_SW_ALU_STAT_CTRL__4, &data);
+		if (!(data & waiton))
+			break;
+		usleep_range(1, 10);
+	} while (timeout-- > 0);
+
+	if (timeout <= 0)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+static int ksz_reset_switch(struct dsa_switch *ds)
+{
+	struct ksz_device *dev = ds->priv;
+	u8 data8;
+	u16 data16;
+	u32 data32;
+
+	/* reset switch */
+	ksz_cfg(dev, REG_SW_OPERATION, SW_RESET, true);
+
+	/* turn off SPI DO Edge select */
+	ksz_read8(dev, REG_SW_GLOBAL_SERIAL_CTRL_0, &data8);
+	data8 &= ~SPI_AUTO_EDGE_DETECTION;
+	ksz_write8(dev, REG_SW_GLOBAL_SERIAL_CTRL_0, data8);
+
+	/* default configuration */
+	ksz_read8(dev, REG_SW_LUE_CTRL_1, &data8);
+	data8 = SW_AGING_ENABLE | SW_LINK_AUTO_AGING |
+	      SW_SRC_ADDR_FILTER | SW_FLUSH_STP_TABLE | SW_FLUSH_MSTP_TABLE;
+	ksz_write8(dev, REG_SW_LUE_CTRL_1, data8);
+
+	/* disable interrupts */
+	ksz_write32(dev, REG_SW_INT_MASK__4, SWITCH_INT_MASK);
+	ksz_write32(dev, REG_SW_PORT_INT_MASK__4, 0x7F);
+	ksz_read32(dev, REG_SW_PORT_INT_STATUS__4, &data32);
+
+	/* set broadcast storm protection 10% rate */
+	ksz_read16(dev, REG_SW_MAC_CTRL_2, &data16);
+	data16 &= ~BROADCAST_STORM_RATE;
+	data16 |= (BROADCAST_STORM_VALUE * BROADCAST_STORM_PROT_RATE) / 100;
+	ksz_write16(dev, REG_SW_MAC_CTRL_2, data16);
+
+	return 0;
+}
+
+static void port_setup(struct ksz_device *dev, int port, bool cpu_port)
+{
+	u8 data8;
+	u16 data16;
+
+	/* enable tag tail for host port */
+	if (cpu_port)
+		ksz_port_cfg(dev, port, REG_PORT_CTRL_0, PORT_TAIL_TAG_ENABLE,
+			     true);
+
+	ksz_port_cfg(dev, port, REG_PORT_CTRL_0, PORT_MAC_LOOPBACK, false);
+
+	/* set back pressure */
+	ksz_port_cfg(dev, port, REG_PORT_MAC_CTRL_1, PORT_BACK_PRESSURE, true);
+
+	/* set flow control */
+	ksz_port_cfg(dev, port, REG_PORT_CTRL_0,
+		     PORT_FORCE_TX_FLOW_CTRL | PORT_FORCE_RX_FLOW_CTRL, true);
+
+	/* enable broadcast storm limit */
+	ksz_port_cfg(dev, port, P_BCAST_STORM_CTRL, PORT_BROADCAST_STORM, true);
+
+	/* disable DiffServ priority */
+	ksz_port_cfg(dev, port, P_PRIO_CTRL, PORT_DIFFSERV_PRIO_ENABLE, false);
+
+	/* replace priority */
+	ksz_port_cfg(dev, port, REG_PORT_MRI_MAC_CTRL, PORT_USER_PRIO_CEILING,
+		     false);
+	ksz_port_cfg32(dev, port, REG_PORT_MTI_QUEUE_CTRL_0__4,
+		       MTI_PVID_REPLACE, false);
+
+	/* enable 802.1p priority */
+	ksz_port_cfg(dev, port, P_PRIO_CTRL, PORT_802_1P_PRIO_ENABLE, true);
+
+	/* configure MAC to 1G & RGMII mode */
+	ksz_pread8(dev, port, REG_PORT_XMII_CTRL_1, &data8);
+	data8 |= PORT_RGMII_ID_EG_ENABLE;
+	data8 &= ~PORT_MII_NOT_1GBIT;
+	data8 &= ~PORT_MII_SEL_M;
+	data8 |= PORT_RGMII_SEL;
+	ksz_pwrite8(dev, port, REG_PORT_XMII_CTRL_1, data8);
+
+	/* clear pending interrupts */
+	ksz_pread16(dev, port, REG_PORT_PHY_INT_ENABLE, &data16);
+}
+
+static void ksz_config_cpu_port(struct dsa_switch *ds)
+{
+	struct ksz_device *dev = ds->priv;
+	int i;
+
+	ds->num_ports = dev->port_cnt;
+
+	for (i = 0; i < ds->num_ports; i++) {
+		if (dsa_is_cpu_port(ds, i) && (dev->cpu_ports & (1 << i))) {
+			dev->cpu_port = i;
+
+			/* enable cpu port */
+			port_setup(dev, i, true);
+		}
+	}
+}
+
+static int ksz_setup(struct dsa_switch *ds)
+{
+	struct ksz_device *dev = ds->priv;
+	int ret = 0;
+
+	dev->vlan_cache = devm_kcalloc(dev->dev, sizeof(struct vlan_table),
+				       dev->num_vlans, GFP_KERNEL);
+	if (!dev->vlan_cache)
+		return -ENOMEM;
+
+	ret = ksz_reset_switch(ds);
+	if (ret) {
+		dev_err(ds->dev, "failed to reset switch\n");
+		return ret;
+	}
+
+	/* accept packet up to 2000bytes */
+	ksz_cfg(dev, REG_SW_MAC_CTRL_1, SW_LEGAL_PACKET_DISABLE, true);
+
+	ksz_config_cpu_port(ds);
+
+	ksz_cfg(dev, REG_SW_MAC_CTRL_1, MULTICAST_STORM_DISABLE, true);
+
+	/* queue based egress rate limit */
+	ksz_cfg(dev, REG_SW_MAC_CTRL_5, SW_OUT_RATE_LIMIT_QUEUE_BASED, true);
+
+	/* start switch */
+	ksz_cfg(dev, REG_SW_OPERATION, SW_START, true);
+
+	return 0;
+}
+
+static enum dsa_tag_protocol ksz_get_tag_protocol(struct dsa_switch *ds)
+{
+	return DSA_TAG_PROTO_KSZ;
+}
+
+static int ksz_phy_read16(struct dsa_switch *ds, int addr, int reg)
+{
+	struct ksz_device *dev = ds->priv;
+	u16 val = 0;
+
+	ksz_pread16(dev, addr, 0x100 + (reg << 1), &val);
+
+	return val;
+}
+
+static int ksz_phy_write16(struct dsa_switch *ds, int addr, int reg, u16 val)
+{
+	struct ksz_device *dev = ds->priv;
+
+	ksz_pwrite16(dev, addr, 0x100 + (reg << 1), val);
+
+	return 0;
+}
+
+static int ksz_enable_port(struct dsa_switch *ds, int port,
+			   struct phy_device *phy)
+{
+	struct ksz_device *dev = ds->priv;
+
+	/* setup slave port */
+	port_setup(dev, port, false);
+
+	return 0;
+}
+
+static void ksz_disable_port(struct dsa_switch *ds, int port,
+			     struct phy_device *phy)
+{
+	struct ksz_device *dev = ds->priv;
+
+	/* there is no port disable */
+	ksz_port_cfg(dev, port, REG_PORT_CTRL_0, PORT_MAC_LOOPBACK, true);
+}
+
+static int ksz_sset_count(struct dsa_switch *ds)
+{
+	return TOTAL_SWITCH_COUNTER_NUM;
+}
+
+static void ksz_get_strings(struct dsa_switch *ds, int port, uint8_t *buf)
+{
+	int i;
+
+	for (i = 0; i < TOTAL_SWITCH_COUNTER_NUM; i++) {
+		memcpy(buf + i * ETH_GSTRING_LEN, mib_names[i].string,
+		       ETH_GSTRING_LEN);
+	}
+}
+
+static void ksz_get_ethtool_stats(struct dsa_switch *ds, int port,
+				  uint64_t *buf)
+{
+	struct ksz_device *dev = ds->priv;
+	int i;
+	u32 data;
+	int timeout;
+
+	mutex_lock(&dev->stats_mutex);
+
+	for (i = 0; i < TOTAL_SWITCH_COUNTER_NUM; i++) {
+		data = MIB_COUNTER_READ;
+		data |= ((mib_names[i].index & 0xFF) << MIB_COUNTER_INDEX_S);
+		ksz_pwrite32(dev, port, REG_PORT_MIB_CTRL_STAT__4, data);
+
+		timeout = 1000;
+		do {
+			ksz_pread32(dev, port, REG_PORT_MIB_CTRL_STAT__4,
+				    &data);
+			usleep_range(1, 10);
+			if (!(data & MIB_COUNTER_READ))
+				break;
+		} while (timeout-- > 0);
+
+		/* failed to read MIB. get out of loop */
+		if (!timeout) {
+			dev_dbg(dev->dev, "Failed to get MIB\n");
+			break;
+		}
+
+		/* count resets upon read */
+		ksz_pread32(dev, port, REG_PORT_MIB_DATA, &data);
+
+		dev->mib_value[i] += (uint64_t)data;
+		buf[i] = dev->mib_value[i];
+	}
+
+	mutex_unlock(&dev->stats_mutex);
+}
+
+static void ksz_port_stp_state_set(struct dsa_switch *ds, int port, u8 state)
+{
+	struct ksz_device *dev = ds->priv;
+	u8 data;
+
+	ksz_pread8(dev, port, P_STP_CTRL, &data);
+	data &= ~(PORT_TX_ENABLE | PORT_RX_ENABLE | PORT_LEARN_DISABLE);
+
+	switch (state) {
+	case BR_STATE_DISABLED:
+		data |= PORT_LEARN_DISABLE;
+		break;
+	case BR_STATE_LISTENING:
+		data |= (PORT_RX_ENABLE | PORT_LEARN_DISABLE);
+		break;
+	case BR_STATE_LEARNING:
+		data |= PORT_RX_ENABLE;
+		break;
+	case BR_STATE_FORWARDING:
+		data |= (PORT_TX_ENABLE | PORT_RX_ENABLE);
+		break;
+	case BR_STATE_BLOCKING:
+		data |= PORT_LEARN_DISABLE;
+		break;
+	default:
+		dev_err(ds->dev, "invalid STP state: %d\n", state);
+		return;
+	}
+
+	ksz_pwrite8(dev, port, P_STP_CTRL, data);
+}
+
+static void ksz_port_fast_age(struct dsa_switch *ds, int port)
+{
+	struct ksz_device *dev = ds->priv;
+	u8 data8;
+
+	ksz_read8(dev, REG_SW_LUE_CTRL_1, &data8);
+	data8 |= SW_FAST_AGING;
+	ksz_write8(dev, REG_SW_LUE_CTRL_1, data8);
+
+	data8 &= ~SW_FAST_AGING;
+	ksz_write8(dev, REG_SW_LUE_CTRL_1, data8);
+}
+
+static int ksz_port_vlan_filtering(struct dsa_switch *ds, int port, bool flag)
+{
+	struct ksz_device *dev = ds->priv;
+
+	if (flag) {
+		ksz_port_cfg(dev, port, REG_PORT_LUE_CTRL,
+			     PORT_VLAN_LOOKUP_VID_0, true);
+		ksz_cfg32(dev, REG_SW_QM_CTRL__4, UNICAST_VLAN_BOUNDARY, true);
+		ksz_cfg(dev, REG_SW_LUE_CTRL_0, SW_VLAN_ENABLE, true);
+	} else {
+		ksz_cfg(dev, REG_SW_LUE_CTRL_0, SW_VLAN_ENABLE, false);
+		ksz_cfg32(dev, REG_SW_QM_CTRL__4, UNICAST_VLAN_BOUNDARY, false);
+		ksz_port_cfg(dev, port, REG_PORT_LUE_CTRL,
+			     PORT_VLAN_LOOKUP_VID_0, false);
+	}
+
+	return 0;
+}
+
+static int ksz_port_vlan_prepare(struct dsa_switch *ds, int port,
+				 const struct switchdev_obj_port_vlan *vlan,
+				 struct switchdev_trans *trans)
+{
+	/* nothing needed */
+
+	return 0;
+}
+
+static void ksz_port_vlan_add(struct dsa_switch *ds, int port,
+			      const struct switchdev_obj_port_vlan *vlan,
+			      struct switchdev_trans *trans)
+{
+	struct ksz_device *dev = ds->priv;
+	u32 vlan_table[3];
+	u16 vid;
+	bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
+
+	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
+		if (get_vlan_table(ds, vid, vlan_table)) {
+			dev_dbg(dev->dev, "Failed to get vlan table\n");
+			return;
+		}
+
+		vlan_table[0] = VLAN_VALID | (vid & VLAN_FID_M);
+		if (untagged)
+			vlan_table[1] |= BIT(port);
+		else
+			vlan_table[1] &= ~BIT(port);
+		vlan_table[1] &= ~(BIT(dev->cpu_port));
+
+		vlan_table[2] |= BIT(port) | BIT(dev->cpu_port);
+
+		if (set_vlan_table(ds, vid, vlan_table)) {
+			dev_dbg(dev->dev, "Failed to set vlan table\n");
+			return;
+		}
+
+		/* change PVID */
+		if (vlan->flags & BRIDGE_VLAN_INFO_PVID)
+			ksz_pwrite16(dev, port, REG_PORT_DEFAULT_VID, vid);
+	}
+}
+
+static int ksz_port_vlan_del(struct dsa_switch *ds, int port,
+			     const struct switchdev_obj_port_vlan *vlan)
+{
+	struct ksz_device *dev = ds->priv;
+	bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
+	u32 vlan_table[3];
+	u16 vid;
+	u16 pvid;
+
+	ksz_pread16(dev, port, REG_PORT_DEFAULT_VID, &pvid);
+	pvid = pvid & 0xFFF;
+
+	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
+		if (get_vlan_table(ds, vid, vlan_table)) {
+			dev_dbg(dev->dev, "Failed to get vlan table\n");
+			return -ETIMEDOUT;
+		}
+
+		vlan_table[2] &= ~BIT(port);
+
+		if (pvid == vid)
+			pvid = 1;
+
+		if (untagged)
+			vlan_table[1] &= ~BIT(port);
+
+		if (set_vlan_table(ds, vid, vlan_table)) {
+			dev_dbg(dev->dev, "Failed to set vlan table\n");
+			return -ETIMEDOUT;
+		}
+	}
+
+	ksz_pwrite16(dev, port, REG_PORT_DEFAULT_VID, pvid);
+
+	return 0;
+}
+
+static int ksz_port_vlan_dump(struct dsa_switch *ds, int port,
+			      struct switchdev_obj_port_vlan *vlan,
+			      switchdev_obj_dump_cb_t *cb)
+{
+	struct ksz_device *dev = ds->priv;
+	u16 vid;
+	u16 data;
+	struct vlan_table *vlan_cache;
+	int err = 0;
+
+	mutex_lock(&dev->vlan_mutex);
+
+	/* use dev->vlan_cache due to lack of searching valid vlan entry */
+	for (vid = vlan->vid_begin; vid < dev->num_vlans; vid++) {
+		vlan_cache = &dev->vlan_cache[vid];
+
+		if (!(vlan_cache->table[0] & VLAN_VALID))
+			continue;
+
+		vlan->vid_begin = vid;
+		vlan->vid_end = vid;
+		vlan->flags = 0;
+		if (vlan_cache->table[2] & BIT(port)) {
+			if (vlan_cache->table[1] & BIT(port))
+				vlan->flags |= BRIDGE_VLAN_INFO_UNTAGGED;
+			ksz_pread16(dev, port, REG_PORT_DEFAULT_VID, &data);
+			if (vid == (data & 0xFFFFF))
+				vlan->flags |= BRIDGE_VLAN_INFO_PVID;
+
+			err = cb(&vlan->obj);
+			if (err)
+				break;
+		}
+	}
+
+	mutex_unlock(&dev->vlan_mutex);
+
+	return err;
+}
+
+static int ksz_port_fdb_prepare(struct dsa_switch *ds, int port,
+				const struct switchdev_obj_port_fdb *fdb,
+				struct switchdev_trans *trans)
+{
+	/* nothing needed */
+
+	return 0;
+}
+
+struct alu_struct {
+	/* entry 1 */
+	u8	is_static:1;
+	u8	is_src_filter:1;
+	u8	is_dst_filter:1;
+	u8	prio_age:3;
+	u32	_reserv_0_1:23;
+	u8	mstp:3;
+	/* entry 2 */
+	u8	is_override:1;
+	u8	is_use_fid:1;
+	u32	_reserv_1_1:23;
+	u8	port_forward:7;
+	/* entry 3 & 4*/
+	u32	_reserv_2_1:9;
+	u8	fid:7;
+	u8	mac[ETH_ALEN];
+};
+
+static void ksz_port_fdb_add(struct dsa_switch *ds, int port,
+			     const struct switchdev_obj_port_fdb *fdb,
+			     struct switchdev_trans *trans)
+{
+	struct ksz_device *dev = ds->priv;
+	u32 alu_table[4];
+	u32 data;
+
+	mutex_lock(&dev->alu_mutex);
+
+	/* find any entry with mac & vid */
+	data = fdb->vid << ALU_FID_INDEX_S;
+	data |= ((fdb->addr[0] << 8) | fdb->addr[1]);
+	ksz_write32(dev, REG_SW_ALU_INDEX_0, data);
+
+	data = ((fdb->addr[2] << 24) | (fdb->addr[3] << 16));
+	data |= ((fdb->addr[4] << 8) | fdb->addr[5]);
+	ksz_write32(dev, REG_SW_ALU_INDEX_1, data);
+
+	/* start read operation */
+	ksz_write32(dev, REG_SW_ALU_CTRL__4, ALU_READ | ALU_START);
+
+	/* wait to be finished */
+	if (wait_alu_ready(dev, ALU_START, 1000) < 0) {
+		dev_dbg(dev->dev, "Failed to read ALU\n");
+		goto exit;
+	}
+
+	/* read ALU entry */
+	read_table(ds, alu_table);
+
+	/* update ALU entry */
+	alu_table[0] = ALU_V_STATIC_VALID;
+	alu_table[1] |= BIT(port);
+	if (fdb->vid)
+		alu_table[1] |= ALU_V_USE_FID;
+	alu_table[2] = (fdb->vid << ALU_V_FID_S);
+	alu_table[2] |= ((fdb->addr[0] << 8) | fdb->addr[1]);
+	alu_table[3] = ((fdb->addr[2] << 24) | (fdb->addr[3] << 16));
+	alu_table[3] |= ((fdb->addr[4] << 8) | fdb->addr[5]);
+
+	write_table(ds, alu_table);
+
+	ksz_write32(dev, REG_SW_ALU_CTRL__4, ALU_WRITE | ALU_START);
+
+	/* wait to be finished */
+	if (wait_alu_ready(dev, ALU_START, 1000) < 0)
+		dev_dbg(dev->dev, "Failed to read ALU\n");
+
+exit:
+	mutex_unlock(&dev->alu_mutex);
+}
+
+static int ksz_port_fdb_del(struct dsa_switch *ds, int port,
+			    const struct switchdev_obj_port_fdb *fdb)
+{
+	struct ksz_device *dev = ds->priv;
+	u32 alu_table[4];
+	u32 data;
+	int ret = 0;
+
+	mutex_lock(&dev->alu_mutex);
+
+	/* read any entry with mac & vid */
+	data = fdb->vid << ALU_FID_INDEX_S;
+	data |= ((fdb->addr[0] << 8) | fdb->addr[1]);
+	ksz_write32(dev, REG_SW_ALU_INDEX_0, data);
+
+	data = ((fdb->addr[2] << 24) | (fdb->addr[3] << 16));
+	data |= ((fdb->addr[4] << 8) | fdb->addr[5]);
+	ksz_write32(dev, REG_SW_ALU_INDEX_1, data);
+
+	/* start read operation */
+	ksz_write32(dev, REG_SW_ALU_CTRL__4, ALU_READ | ALU_START);
+
+	/* wait to be finished */
+	ret = wait_alu_ready(dev, ALU_START, 1000);
+	if (ret < 0) {
+		dev_dbg(dev->dev, "Failed to read ALU\n");
+		goto exit;
+	}
+
+	ksz_read32(dev, REG_SW_ALU_VAL_A, &alu_table[0]);
+	if (alu_table[0] & ALU_V_STATIC_VALID) {
+		ksz_read32(dev, REG_SW_ALU_VAL_B, &alu_table[1]);
+		ksz_read32(dev, REG_SW_ALU_VAL_C, &alu_table[2]);
+		ksz_read32(dev, REG_SW_ALU_VAL_D, &alu_table[3]);
+
+		/* clear forwarding port */
+		alu_table[2] &= ~BIT(port);
+
+		/* if there is no port to forward, clear table */
+		if ((alu_table[2] & ALU_V_PORT_MAP) == 0) {
+			alu_table[0] = 0;
+			alu_table[1] = 0;
+			alu_table[2] = 0;
+			alu_table[3] = 0;
+		}
+	} else {
+		alu_table[0] = 0;
+		alu_table[1] = 0;
+		alu_table[2] = 0;
+		alu_table[3] = 0;
+	}
+
+	write_table(ds, alu_table);
+
+	ksz_write32(dev, REG_SW_ALU_CTRL__4, ALU_WRITE | ALU_START);
+
+	/* wait to be finished */
+	ret = wait_alu_ready(dev, ALU_START, 1000);
+	if (ret < 0)
+		dev_dbg(dev->dev, "Failed to write ALU\n");
+
+exit:
+	mutex_unlock(&dev->alu_mutex);
+
+	return ret;
+}
+
+static void convert_alu(struct alu_struct *alu, u32 *alu_table)
+{
+	alu->is_static = !!(alu_table[0] & ALU_V_STATIC_VALID);
+	alu->is_src_filter = !!(alu_table[0] & ALU_V_SRC_FILTER);
+	alu->is_dst_filter = !!(alu_table[0] & ALU_V_DST_FILTER);
+	alu->prio_age = (alu_table[0] >> ALU_V_PRIO_AGE_CNT_S) &
+			ALU_V_PRIO_AGE_CNT_M;
+	alu->mstp = alu_table[0] & ALU_V_MSTP_M;
+
+	alu->is_override = !!(alu_table[1] & ALU_V_OVERRIDE);
+	alu->is_use_fid = !!(alu_table[1] & ALU_V_USE_FID);
+	alu->port_forward = alu_table[1] & ALU_V_PORT_MAP;
+
+	alu->fid = (alu_table[2] >> ALU_V_FID_S) & ALU_V_FID_M;
+
+	alu->mac[0] = (alu_table[2] >> 8) & 0xFF;
+	alu->mac[1] = alu_table[2] & 0xFF;
+	alu->mac[2] = (alu_table[3] >> 24) & 0xFF;
+	alu->mac[3] = (alu_table[3] >> 16) & 0xFF;
+	alu->mac[4] = (alu_table[3] >> 8) & 0xFF;
+	alu->mac[5] = alu_table[3] & 0xFF;
+}
+
+static int ksz_port_fdb_dump(struct dsa_switch *ds, int port,
+			     struct switchdev_obj_port_fdb *fdb,
+			     switchdev_obj_dump_cb_t *cb)
+{
+	struct ksz_device *dev = ds->priv;
+	int ret = 0;
+	u32 data;
+	u32 alu_table[4];
+	struct alu_struct alu;
+	int timeout;
+
+	mutex_lock(&dev->alu_mutex);
+
+	/* start ALU search */
+	ksz_write32(dev, REG_SW_ALU_CTRL__4, ALU_START | ALU_SEARCH);
+
+	do {
+		timeout = 1000;
+		do {
+			ksz_read32(dev, REG_SW_ALU_CTRL__4, &data);
+			if ((data & ALU_VALID) || !(data & ALU_START))
+				break;
+			usleep_range(1, 10);
+		} while (timeout-- > 0);
+
+		if (!timeout) {
+			dev_dbg(dev->dev, "Failed to search ALU\n");
+			ret = -ETIMEDOUT;
+			goto exit;
+		}
+
+		/* read ALU table */
+		read_table(ds, alu_table);
+
+		convert_alu(&alu, alu_table);
+
+		if (alu.port_forward & BIT(port)) {
+			fdb->vid = alu.fid;
+			if (alu.is_static)
+				fdb->ndm_state = NUD_NOARP;
+			else
+				fdb->ndm_state = NUD_REACHABLE;
+			ether_addr_copy(fdb->addr, alu.mac);
+
+			ret = cb(&fdb->obj);
+			if (ret)
+				goto exit;
+		}
+	} while (data & ALU_START);
+
+exit:
+
+	/* stop ALU search */
+	ksz_write32(dev, REG_SW_ALU_CTRL__4, 0);
+
+	mutex_unlock(&dev->alu_mutex);
+
+	return ret;
+}
+
+static int ksz_port_mdb_prepare(struct dsa_switch *ds, int port,
+				const struct switchdev_obj_port_mdb *mdb,
+				struct switchdev_trans *trans)
+{
+	/* nothing to do */
+	return 0;
+}
+
+static void ksz_port_mdb_add(struct dsa_switch *ds, int port,
+			     const struct switchdev_obj_port_mdb *mdb,
+			     struct switchdev_trans *trans)
+{
+	struct ksz_device *dev = ds->priv;
+	u32 static_table[4];
+	u32 data;
+	int index;
+	u32 mac_hi, mac_lo;
+
+	mac_hi = ((mdb->addr[0] << 8) | mdb->addr[1]);
+	mac_lo = ((mdb->addr[2] << 24) | (mdb->addr[3] << 16));
+	mac_lo |= ((mdb->addr[4] << 8) | mdb->addr[5]);
+
+	mutex_lock(&dev->alu_mutex);
+
+	for (index = 0; index < dev->num_statics; index++) {
+		/* find empty slot first */
+		data = (index << ALU_STAT_INDEX_S) |
+			ALU_STAT_READ | ALU_STAT_START;
+		ksz_write32(dev, REG_SW_ALU_STAT_CTRL__4, data);
+
+		/* wait to be finished */
+		if (wait_alu_sta_ready(dev, ALU_STAT_START, 1000) < 0) {
+			dev_dbg(dev->dev, "Failed to read ALU STATIC\n");
+			goto exit;
+		}
+
+		/* read ALU static table */
+		read_table(ds, static_table);
+
+		if (static_table[0] & ALU_V_STATIC_VALID) {
+			/* check this has same vid & mac address */
+			if (((static_table[2] >> ALU_V_FID_S) == (mdb->vid)) &&
+			    ((static_table[2] & ALU_V_MAC_ADDR_HI) == mac_hi) &&
+			    (static_table[3] == mac_lo)) {
+				/* found matching one */
+				break;
+			}
+		} else {
+			/* found empty one */
+			break;
+		}
+	}
+
+	/* no available entry */
+	if (index == dev->num_statics)
+		goto exit;
+
+	/* add entry */
+	static_table[0] = ALU_V_STATIC_VALID;
+	static_table[1] |= BIT(port);
+	if (mdb->vid)
+		static_table[1] |= ALU_V_USE_FID;
+	static_table[2] = (mdb->vid << ALU_V_FID_S);
+	static_table[2] |= mac_hi;
+	static_table[3] = mac_lo;
+
+	write_table(ds, static_table);
+
+	data = (index << ALU_STAT_INDEX_S) | ALU_STAT_START;
+	ksz_write32(dev, REG_SW_ALU_STAT_CTRL__4, data);
+
+	/* wait to be finished */
+	if (wait_alu_sta_ready(dev, ALU_STAT_START, 1000) < 0)
+		dev_dbg(dev->dev, "Failed to read ALU STATIC\n");
+
+exit:
+	mutex_unlock(&dev->alu_mutex);
+}
+
+static int ksz_port_mdb_del(struct dsa_switch *ds, int port,
+			    const struct switchdev_obj_port_mdb *mdb)
+{
+	struct ksz_device *dev = ds->priv;
+	u32 static_table[4];
+	u32 data;
+	int index;
+	int ret = 0;
+	u32 mac_hi, mac_lo;
+
+	mac_hi = ((mdb->addr[0] << 8) | mdb->addr[1]);
+	mac_lo = ((mdb->addr[2] << 24) | (mdb->addr[3] << 16));
+	mac_lo |= ((mdb->addr[4] << 8) | mdb->addr[5]);
+
+	mutex_lock(&dev->alu_mutex);
+
+	for (index = 0; index < dev->num_statics; index++) {
+		/* find empty slot first */
+		data = (index << ALU_STAT_INDEX_S) |
+			ALU_STAT_READ | ALU_STAT_START;
+		ksz_write32(dev, REG_SW_ALU_STAT_CTRL__4, data);
+
+		/* wait to be finished */
+		ret = wait_alu_sta_ready(dev, ALU_STAT_START, 1000);
+		if (ret < 0) {
+			dev_dbg(dev->dev, "Failed to read ALU STATIC\n");
+			goto exit;
+		}
+
+		/* read ALU static table */
+		read_table(ds, static_table);
+
+		if (static_table[0] & ALU_V_STATIC_VALID) {
+			/* check this has same vid & mac address */
+
+			if (((static_table[2] >> ALU_V_FID_S) == (mdb->vid)) &&
+			    ((static_table[2] & ALU_V_MAC_ADDR_HI) == mac_hi) &&
+			    (static_table[3] == mac_lo)) {
+				/* found matching one */
+				break;
+			}
+		}
+	}
+
+	/* no available entry */
+	if (index == dev->num_statics) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	/* clear port */
+	static_table[1] &= ~BIT(port);
+
+	if ((static_table[1] & ALU_V_PORT_MAP) == 0) {
+		/* delete entry */
+		static_table[0] = 0;
+		static_table[1] = 0;
+		static_table[2] = 0;
+		static_table[3] = 0;
+	}
+
+	write_table(ds, static_table);
+
+	data = (index << ALU_STAT_INDEX_S) | ALU_STAT_START;
+	ksz_write32(dev, REG_SW_ALU_STAT_CTRL__4, data);
+
+	/* wait to be finished */
+	ret = wait_alu_sta_ready(dev, ALU_STAT_START, 1000);
+	if (ret < 0)
+		dev_dbg(dev->dev, "Failed to read ALU STATIC\n");
+
+exit:
+	mutex_unlock(&dev->alu_mutex);
+
+	return ret;
+}
+
+static int ksz_port_mdb_dump(struct dsa_switch *ds, int port,
+			     struct switchdev_obj_port_mdb *mdb,
+			     switchdev_obj_dump_cb_t *cb)
+{
+	/* this is not called by switch layer */
+	return 0;
+}
+
+static int ksz_port_mirror_add(struct dsa_switch *ds, int port,
+			       struct dsa_mall_mirror_tc_entry *mirror,
+			       bool ingress)
+{
+	struct ksz_device *dev = ds->priv;
+
+	if (ingress)
+		ksz_port_cfg(dev, port, P_MIRROR_CTRL, PORT_MIRROR_RX, true);
+	else
+		ksz_port_cfg(dev, port, P_MIRROR_CTRL, PORT_MIRROR_TX, true);
+
+	ksz_port_cfg(dev, port, P_MIRROR_CTRL, PORT_MIRROR_SNIFFER, false);
+
+	/* configure mirror port */
+	ksz_port_cfg(dev, mirror->to_local_port, P_MIRROR_CTRL,
+		     PORT_MIRROR_SNIFFER, true);
+
+	ksz_cfg(dev, S_MIRROR_CTRL, SW_MIRROR_RX_TX, false);
+
+	return 0;
+}
+
+static void ksz_port_mirror_del(struct dsa_switch *ds, int port,
+				struct dsa_mall_mirror_tc_entry *mirror)
+{
+	struct ksz_device *dev = ds->priv;
+	u8 data;
+
+	if (mirror->ingress)
+		ksz_port_cfg(dev, port, P_MIRROR_CTRL, PORT_MIRROR_RX, false);
+	else
+		ksz_port_cfg(dev, port, P_MIRROR_CTRL, PORT_MIRROR_TX, false);
+
+	ksz_pread8(dev, port, P_MIRROR_CTRL, &data);
+
+	if (!(data & (PORT_MIRROR_RX | PORT_MIRROR_TX)))
+		ksz_port_cfg(dev, mirror->to_local_port, P_MIRROR_CTRL,
+			     PORT_MIRROR_SNIFFER, false);
+}
+
+static const struct dsa_switch_ops ksz_switch_ops = {
+	.get_tag_protocol	= ksz_get_tag_protocol,
+	.setup			= ksz_setup,
+	.phy_read		= ksz_phy_read16,
+	.phy_write		= ksz_phy_write16,
+	.port_enable		= ksz_enable_port,
+	.port_disable		= ksz_disable_port,
+	.get_strings		= ksz_get_strings,
+	.get_ethtool_stats	= ksz_get_ethtool_stats,
+	.get_sset_count		= ksz_sset_count,
+	.port_stp_state_set	= ksz_port_stp_state_set,
+	.port_fast_age		= ksz_port_fast_age,
+	.port_vlan_filtering	= ksz_port_vlan_filtering,
+	.port_vlan_prepare	= ksz_port_vlan_prepare,
+	.port_vlan_add		= ksz_port_vlan_add,
+	.port_vlan_del		= ksz_port_vlan_del,
+	.port_vlan_dump		= ksz_port_vlan_dump,
+	.port_fdb_prepare	= ksz_port_fdb_prepare,
+	.port_fdb_dump		= ksz_port_fdb_dump,
+	.port_fdb_add		= ksz_port_fdb_add,
+	.port_fdb_del		= ksz_port_fdb_del,
+	.port_mdb_prepare       = ksz_port_mdb_prepare,
+	.port_mdb_add           = ksz_port_mdb_add,
+	.port_mdb_del           = ksz_port_mdb_del,
+	.port_mdb_dump          = ksz_port_mdb_dump,
+	.port_mirror_add	= ksz_port_mirror_add,
+	.port_mirror_del	= ksz_port_mirror_del,
+};
+
+struct ksz_chip_data {
+	u32 chip_id;
+	const char *dev_name;
+	int num_vlans;
+	int num_alus;
+	int num_statics;
+	int cpu_ports;
+	int port_cnt;
+};
+
+static const struct ksz_chip_data ksz_switch_chips[] = {
+	{
+		.chip_id = 0x00947700,
+		.dev_name = "KSZ9477",
+		.num_vlans = 4096,
+		.num_alus = 4096,
+		.num_statics = 16,
+		.cpu_ports = 0x7F,	/* can be configured as cpu port */
+		.port_cnt = 7,		/* total physical port count */
+	},
+};
+
+static int ksz_switch_init(struct ksz_device *dev)
+{
+	int i;
+
+	mutex_init(&dev->reg_mutex);
+	mutex_init(&dev->stats_mutex);
+	mutex_init(&dev->alu_mutex);
+	mutex_init(&dev->vlan_mutex);
+
+	dev->ds->ops = &ksz_switch_ops;
+
+	for (i = 0; i < ARRAY_SIZE(ksz_switch_chips); i++) {
+		const struct ksz_chip_data *chip = &ksz_switch_chips[i];
+
+		if (dev->chip_id == chip->chip_id) {
+			dev->name = chip->dev_name;
+			dev->num_vlans = chip->num_vlans;
+			dev->num_alus = chip->num_alus;
+			dev->num_statics = chip->num_statics;
+			dev->port_cnt = chip->port_cnt;
+			dev->cpu_ports = chip->cpu_ports;
+
+			break;
+		}
+	}
+
+	/* no switch found */
+	if (!dev->port_cnt)
+		return -ENODEV;
+
+	return 0;
+}
+
+struct ksz_device *ksz_switch_alloc(struct device *base,
+				    const struct ksz_io_ops *ops,
+				    void *priv)
+{
+	struct dsa_switch *ds;
+	struct ksz_device *swdev;
+
+	ds = dsa_switch_alloc(base, DSA_MAX_PORTS);
+	if (!ds)
+		return NULL;
+
+	swdev = devm_kzalloc(base, sizeof(*swdev), GFP_KERNEL);
+	if (!swdev)
+		return NULL;
+
+	ds->priv = swdev;
+	swdev->dev = base;
+
+	swdev->ds = ds;
+	swdev->priv = priv;
+	swdev->ops = ops;
+
+	return swdev;
+}
+EXPORT_SYMBOL(ksz_switch_alloc);
+
+int ksz_switch_detect(struct ksz_device *dev)
+{
+	u8 data8;
+	u32 id32;
+	int ret;
+
+	/* turn off SPI DO Edge select */
+	ret = ksz_read8(dev, REG_SW_GLOBAL_SERIAL_CTRL_0, &data8);
+	if (ret)
+		return ret;
+
+	data8 &= ~SPI_AUTO_EDGE_DETECTION;
+	ret = ksz_write8(dev, REG_SW_GLOBAL_SERIAL_CTRL_0, data8);
+	if (ret)
+		return ret;
+
+	/* read chip id */
+	ret = ksz_read32(dev, REG_CHIP_ID0__1, &id32);
+	if (ret)
+		return ret;
+
+	dev->chip_id = id32;
+
+	return 0;
+}
+EXPORT_SYMBOL(ksz_switch_detect);
+
+int ksz_switch_register(struct ksz_device *dev)
+{
+	int ret;
+
+	if (dev->pdata)
+		dev->chip_id = dev->pdata->chip_id;
+
+	if (ksz_switch_detect(dev))
+		return -EINVAL;
+
+	ret = ksz_switch_init(dev);
+	if (ret)
+		return ret;
+
+	return dsa_register_switch(dev->ds);
+}
+EXPORT_SYMBOL(ksz_switch_register);
+
+void ksz_switch_remove(struct ksz_device *dev)
+{
+	dsa_unregister_switch(dev->ds);
+}
+EXPORT_SYMBOL(ksz_switch_remove);
+
+MODULE_AUTHOR("Woojung Huh <Woojung.Huh@microchip.com>");
+MODULE_DESCRIPTION("Microchip KSZ Series Switch DSA Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/dsa/microchip/ksz_priv.h b/drivers/net/dsa/microchip/ksz_priv.h
new file mode 100644
index 000000000000..2a98dbd51456
--- /dev/null
+++ b/drivers/net/dsa/microchip/ksz_priv.h
@@ -0,0 +1,210 @@
+/*
+ * Microchip KSZ series switch common definitions
+ *
+ * Copyright (C) 2017
+ *
+ * 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 __KSZ_PRIV_H
+#define __KSZ_PRIV_H
+
+#include <linux/kernel.h>
+#include <linux/mutex.h>
+#include <linux/phy.h>
+#include <linux/etherdevice.h>
+#include <net/dsa.h>
+
+#include "ksz_9477_reg.h"
+
+struct ksz_io_ops;
+
+struct vlan_table {
+	u32 table[3];
+};
+
+struct ksz_device {
+	struct dsa_switch *ds;
+	struct ksz_platform_data *pdata;
+	const char *name;
+
+	struct mutex reg_mutex;		/* register access */
+	struct mutex stats_mutex;	/* status access */
+	struct mutex alu_mutex;		/* ALU access */
+	struct mutex vlan_mutex;	/* vlan access */
+	const struct ksz_io_ops *ops;
+
+	struct device *dev;
+
+	void *priv;
+
+	/* chip specific data */
+	u32 chip_id;
+	int num_vlans;
+	int num_alus;
+	int num_statics;
+	int cpu_port;			/* port connected to CPU */
+	int cpu_ports;			/* port bitmap can be cpu port */
+	int port_cnt;
+
+	struct vlan_table *vlan_cache;
+
+	u64 mib_value[TOTAL_SWITCH_COUNTER_NUM];
+};
+
+struct ksz_io_ops {
+	int (*read8)(struct ksz_device *dev, u32 reg, u8 *value);
+	int (*read16)(struct ksz_device *dev, u32 reg, u16 *value);
+	int (*read24)(struct ksz_device *dev, u32 reg, u32 *value);
+	int (*read32)(struct ksz_device *dev, u32 reg, u32 *value);
+	int (*write8)(struct ksz_device *dev, u32 reg, u8 value);
+	int (*write16)(struct ksz_device *dev, u32 reg, u16 value);
+	int (*write24)(struct ksz_device *dev, u32 reg, u32 value);
+	int (*write32)(struct ksz_device *dev, u32 reg, u32 value);
+	int (*phy_read16)(struct ksz_device *dev, int addr, int reg,
+			  u16 *value);
+	int (*phy_write16)(struct ksz_device *dev, int addr, int reg,
+			   u16 value);
+};
+
+struct ksz_device *ksz_switch_alloc(struct device *base,
+				    const struct ksz_io_ops *ops, void *priv);
+int ksz_switch_detect(struct ksz_device *dev);
+int ksz_switch_register(struct ksz_device *dev);
+void ksz_switch_remove(struct ksz_device *dev);
+
+static inline int ksz_read8(struct ksz_device *dev, u32 reg, u8 *val)
+{
+	int ret;
+
+	mutex_lock(&dev->reg_mutex);
+	ret = dev->ops->read8(dev, reg, val);
+	mutex_unlock(&dev->reg_mutex);
+
+	return ret;
+}
+
+static inline int ksz_read16(struct ksz_device *dev, u32 reg, u16 *val)
+{
+	int ret;
+
+	mutex_lock(&dev->reg_mutex);
+	ret = dev->ops->read16(dev, reg, val);
+	mutex_unlock(&dev->reg_mutex);
+
+	return ret;
+}
+
+static inline int ksz_read24(struct ksz_device *dev, u32 reg, u32 *val)
+{
+	int ret;
+
+	mutex_lock(&dev->reg_mutex);
+	ret = dev->ops->read24(dev, reg, val);
+	mutex_unlock(&dev->reg_mutex);
+
+	return ret;
+}
+
+static inline int ksz_read32(struct ksz_device *dev, u32 reg, u32 *val)
+{
+	int ret;
+
+	mutex_lock(&dev->reg_mutex);
+	ret = dev->ops->read32(dev, reg, val);
+	mutex_unlock(&dev->reg_mutex);
+
+	return ret;
+}
+
+static inline int ksz_write8(struct ksz_device *dev, u32 reg, u8 value)
+{
+	int ret;
+
+	mutex_lock(&dev->reg_mutex);
+	ret = dev->ops->write8(dev, reg, value);
+	mutex_unlock(&dev->reg_mutex);
+
+	return ret;
+}
+
+static inline int ksz_write16(struct ksz_device *dev, u32 reg, u16 value)
+{
+	int ret;
+
+	mutex_lock(&dev->reg_mutex);
+	ret = dev->ops->write16(dev, reg, value);
+	mutex_unlock(&dev->reg_mutex);
+
+	return ret;
+}
+
+static inline int ksz_write24(struct ksz_device *dev, u32 reg, u32 value)
+{
+	int ret;
+
+	mutex_lock(&dev->reg_mutex);
+	ret = dev->ops->write24(dev, reg, value);
+	mutex_unlock(&dev->reg_mutex);
+
+	return ret;
+}
+
+static inline int ksz_write32(struct ksz_device *dev, u32 reg, u32 value)
+{
+	int ret;
+
+	mutex_lock(&dev->reg_mutex);
+	ret = dev->ops->write32(dev, reg, value);
+	mutex_unlock(&dev->reg_mutex);
+
+	return ret;
+}
+
+static inline void ksz_pread8(struct ksz_device *dev, int port, int offset,
+			      u8 *data)
+{
+	ksz_read8(dev, PORT_CTRL_ADDR(port, offset), data);
+}
+
+static inline void ksz_pread16(struct ksz_device *dev, int port, int offset,
+			       u16 *data)
+{
+	ksz_read16(dev, PORT_CTRL_ADDR(port, offset), data);
+}
+
+static inline void ksz_pread32(struct ksz_device *dev, int port, int offset,
+			       u32 *data)
+{
+	ksz_read32(dev, PORT_CTRL_ADDR(port, offset), data);
+}
+
+static inline void ksz_pwrite8(struct ksz_device *dev, int port, int offset,
+			       u8 data)
+{
+	ksz_write8(dev, PORT_CTRL_ADDR(port, offset), data);
+}
+
+static inline void ksz_pwrite16(struct ksz_device *dev, int port, int offset,
+				u16 data)
+{
+	ksz_write16(dev, PORT_CTRL_ADDR(port, offset), data);
+}
+
+static inline void ksz_pwrite32(struct ksz_device *dev, int port, int offset,
+				u32 data)
+{
+	ksz_write32(dev, PORT_CTRL_ADDR(port, offset), data);
+}
+
+#endif
diff --git a/drivers/net/dsa/microchip/ksz_spi.c b/drivers/net/dsa/microchip/ksz_spi.c
new file mode 100644
index 000000000000..c51946983bed
--- /dev/null
+++ b/drivers/net/dsa/microchip/ksz_spi.c
@@ -0,0 +1,216 @@
+/*
+ * Microchip KSZ series register access through SPI
+ *
+ * Copyright (C) 2017
+ *
+ * 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 <asm/unaligned.h>
+
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/spi/spi.h>
+
+#include "ksz_priv.h"
+
+/* SPI frame opcodes */
+#define KS_SPIOP_RD			3
+#define KS_SPIOP_WR			2
+
+#define SPI_ADDR_SHIFT			24
+#define SPI_ADDR_MASK			(BIT(SPI_ADDR_SHIFT) - 1)
+#define SPI_TURNAROUND_SHIFT		5
+
+static int ksz_spi_read_reg(struct spi_device *spi, u32 reg, u8 *val,
+			    unsigned int len)
+{
+	u32 txbuf;
+	int ret;
+
+	txbuf = reg & SPI_ADDR_MASK;
+	txbuf |= KS_SPIOP_RD << SPI_ADDR_SHIFT;
+	txbuf <<= SPI_TURNAROUND_SHIFT;
+	txbuf = cpu_to_be32(txbuf);
+
+	ret = spi_write_then_read(spi, &txbuf, 4, val, len);
+	return ret;
+}
+
+static int ksz_spi_read(struct ksz_device *dev, u32 reg, u8 *data,
+			unsigned int len)
+{
+	struct spi_device *spi = dev->priv;
+
+	return ksz_spi_read_reg(spi, reg, data, len);
+}
+
+static int ksz_spi_read8(struct ksz_device *dev, u32 reg, u8 *val)
+{
+	return ksz_spi_read(dev, reg, val, 1);
+}
+
+static int ksz_spi_read16(struct ksz_device *dev, u32 reg, u16 *val)
+{
+	int ret = ksz_spi_read(dev, reg, (u8 *)val, 2);
+
+	if (!ret)
+		*val = be16_to_cpu(*val);
+
+	return ret;
+}
+
+static int ksz_spi_read24(struct ksz_device *dev, u32 reg, u32 *val)
+{
+	int ret;
+
+	*val = 0;
+	ret = ksz_spi_read(dev, reg, (u8 *)val, 3);
+	if (!ret) {
+		*val = be32_to_cpu(*val);
+		/* convert to 24bit */
+		*val >>= 8;
+	}
+
+	return ret;
+}
+
+static int ksz_spi_read32(struct ksz_device *dev, u32 reg, u32 *val)
+{
+	int ret = ksz_spi_read(dev, reg, (u8 *)val, 4);
+
+	if (!ret)
+		*val = be32_to_cpu(*val);
+
+	return ret;
+}
+
+static int ksz_spi_write_reg(struct spi_device *spi, u32 reg, u8 *val,
+			     unsigned int len)
+{
+	u32 txbuf;
+	u8 data[12];
+	int i;
+
+	txbuf = reg & SPI_ADDR_MASK;
+	txbuf |= (KS_SPIOP_WR << SPI_ADDR_SHIFT);
+	txbuf <<= SPI_TURNAROUND_SHIFT;
+	txbuf = cpu_to_be32(txbuf);
+
+	data[0] = txbuf & 0xFF;
+	data[1] = (txbuf & 0xFF00) >> 8;
+	data[2] = (txbuf & 0xFF0000) >> 16;
+	data[3] = (txbuf & 0xFF000000) >> 24;
+	for (i = 0; i < len; i++)
+		data[i + 4] = val[i];
+
+	return spi_write(spi, &data, 4 + len);
+}
+
+static int ksz_spi_write8(struct ksz_device *dev, u32 reg, u8 value)
+{
+	struct spi_device *spi = dev->priv;
+
+	return ksz_spi_write_reg(spi, reg, &value, 1);
+}
+
+static int ksz_spi_write16(struct ksz_device *dev, u32 reg, u16 value)
+{
+	struct spi_device *spi = dev->priv;
+
+	value = cpu_to_be16(value);
+	return ksz_spi_write_reg(spi, reg, (u8 *)&value, 2);
+}
+
+static int ksz_spi_write24(struct ksz_device *dev, u32 reg, u32 value)
+{
+	struct spi_device *spi = dev->priv;
+
+	/* make it to big endian 24bit from MSB */
+	value <<= 8;
+	value = cpu_to_be32(value);
+	return ksz_spi_write_reg(spi, reg, (u8 *)&value, 3);
+}
+
+static int ksz_spi_write32(struct ksz_device *dev, u32 reg, u32 value)
+{
+	struct spi_device *spi = dev->priv;
+
+	value = cpu_to_be32(value);
+	return ksz_spi_write_reg(spi, reg, (u8 *)&value, 4);
+}
+
+static const struct ksz_io_ops ksz_spi_ops = {
+	.read8 = ksz_spi_read8,
+	.read16 = ksz_spi_read16,
+	.read24 = ksz_spi_read24,
+	.read32 = ksz_spi_read32,
+	.write8 = ksz_spi_write8,
+	.write16 = ksz_spi_write16,
+	.write24 = ksz_spi_write24,
+	.write32 = ksz_spi_write32,
+};
+
+static int ksz_spi_probe(struct spi_device *spi)
+{
+	struct ksz_device *dev;
+	int ret;
+
+	dev = ksz_switch_alloc(&spi->dev, &ksz_spi_ops, spi);
+	if (!dev)
+		return -ENOMEM;
+
+	if (spi->dev.platform_data)
+		dev->pdata = spi->dev.platform_data;
+
+	ret = ksz_switch_register(dev);
+	if (ret)
+		return ret;
+
+	spi_set_drvdata(spi, dev);
+
+	return 0;
+}
+
+static int ksz_spi_remove(struct spi_device *spi)
+{
+	struct ksz_device *dev = spi_get_drvdata(spi);
+
+	if (dev)
+		ksz_switch_remove(dev);
+
+	return 0;
+}
+
+static const struct of_device_id ksz_dt_ids[] = {
+	{ .compatible = "microchip,ksz9477" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ksz_dt_ids);
+
+static struct spi_driver ksz_spi_driver = {
+	.driver = {
+		.name	= "ksz9477-switch",
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(ksz_dt_ids),
+	},
+	.probe	= ksz_spi_probe,
+	.remove	= ksz_spi_remove,
+};
+
+module_spi_driver(ksz_spi_driver);
+
+MODULE_AUTHOR("Woojung Huh <Woojung.Huh@microchip.com>");
+MODULE_DESCRIPTION("Microchip KSZ Series Switch SPI access Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/dsa/mt7530.c b/drivers/net/dsa/mt7530.c
index b070c167e70f..1e46418a3b74 100644
--- a/drivers/net/dsa/mt7530.c
+++ b/drivers/net/dsa/mt7530.c
@@ -28,7 +28,6 @@
 #include <linux/reset.h>
 #include <linux/gpio/consumer.h>
 #include <net/dsa.h>
-#include <net/switchdev.h>
 
 #include "mt7530.h"
 
@@ -854,7 +853,7 @@ mt7530_port_fdb_del(struct dsa_switch *ds, int port,
 static int
 mt7530_port_fdb_dump(struct dsa_switch *ds, int port,
 		     struct switchdev_obj_port_fdb *fdb,
-		     int (*cb)(struct switchdev_obj *obj))
+		     switchdev_obj_dump_cb_t *cb)
 {
 	struct mt7530_priv *priv = ds->priv;
 	struct mt7530_fdb _fdb = { 0 };
@@ -913,11 +912,11 @@ mt7530_setup(struct dsa_switch *ds)
 	struct device_node *dn;
 	struct mt7530_dummy_poll p;
 
-	/* The parent node of master_netdev which holds the common system
+	/* The parent node of cpu_dp->netdev which holds the common system
 	 * controller also is the container for two GMACs nodes representing
 	 * as two netdev instances.
 	 */
-	dn = ds->master_netdev->dev.of_node->parent;
+	dn = ds->dst->cpu_dp->netdev->dev.of_node->parent;
 	priv->ethernet = syscon_node_to_regmap(dn);
 	if (IS_ERR(priv->ethernet))
 		return PTR_ERR(priv->ethernet);
@@ -1081,7 +1080,7 @@ mt7530_probe(struct mdio_device *mdiodev)
 	mutex_init(&priv->reg_mutex);
 	dev_set_drvdata(&mdiodev->dev, priv);
 
-	return dsa_register_switch(priv->ds, &mdiodev->dev);
+	return dsa_register_switch(priv->ds);
 }
 
 static void
diff --git a/drivers/net/dsa/mv88e6060.c b/drivers/net/dsa/mv88e6060.c
index 5934b7a4c448..dce7fa57eb55 100644
--- a/drivers/net/dsa/mv88e6060.c
+++ b/drivers/net/dsa/mv88e6060.c
@@ -176,7 +176,7 @@ static int mv88e6060_setup_port(struct dsa_switch *ds, int p)
 		  ((p & 0xf) << PORT_VLAN_MAP_DBNUM_SHIFT) |
 		   (dsa_is_cpu_port(ds, p) ?
 			ds->enabled_port_mask :
-			BIT(ds->dst->cpu_port)));
+			BIT(ds->dst->cpu_dp->index)));
 
 	/* Port Association Vector: when learning source addresses
 	 * of packets, add the address to the address database using
diff --git a/drivers/net/dsa/mv88e6xxx/Makefile b/drivers/net/dsa/mv88e6xxx/Makefile
index 6edd869c8d6f..5cd5551461e3 100644
--- a/drivers/net/dsa/mv88e6xxx/Makefile
+++ b/drivers/net/dsa/mv88e6xxx/Makefile
@@ -4,4 +4,6 @@ mv88e6xxx-objs += global1.o
 mv88e6xxx-objs += global1_atu.o
 mv88e6xxx-objs += global1_vtu.o
 mv88e6xxx-$(CONFIG_NET_DSA_MV88E6XXX_GLOBAL2) += global2.o
+mv88e6xxx-objs += phy.o
 mv88e6xxx-objs += port.o
+mv88e6xxx-objs += serdes.o
diff --git a/drivers/net/dsa/mv88e6xxx/chip.c b/drivers/net/dsa/mv88e6xxx/chip.c
index d034d8cd7d22..53b088166c28 100644
--- a/drivers/net/dsa/mv88e6xxx/chip.c
+++ b/drivers/net/dsa/mv88e6xxx/chip.c
@@ -32,12 +32,13 @@
 #include <linux/gpio/consumer.h>
 #include <linux/phy.h>
 #include <net/dsa.h>
-#include <net/switchdev.h>
 
-#include "mv88e6xxx.h"
+#include "chip.h"
 #include "global1.h"
 #include "global2.h"
+#include "phy.h"
 #include "port.h"
+#include "serdes.h"
 
 static void assert_reg_lock(struct mv88e6xxx_chip *chip)
 {
@@ -222,21 +223,7 @@ int mv88e6xxx_write(struct mv88e6xxx_chip *chip, int addr, int reg, u16 val)
 	return 0;
 }
 
-static int mv88e6165_phy_read(struct mv88e6xxx_chip *chip,
-			      struct mii_bus *bus,
-			      int addr, int reg, u16 *val)
-{
-	return mv88e6xxx_read(chip, addr, reg, val);
-}
-
-static int mv88e6165_phy_write(struct mv88e6xxx_chip *chip,
-			       struct mii_bus *bus,
-			       int addr, int reg, u16 val)
-{
-	return mv88e6xxx_write(chip, addr, reg, val);
-}
-
-static struct mii_bus *mv88e6xxx_default_mdio_bus(struct mv88e6xxx_chip *chip)
+struct mii_bus *mv88e6xxx_default_mdio_bus(struct mv88e6xxx_chip *chip)
 {
 	struct mv88e6xxx_mdio_bus *mdio_bus;
 
@@ -248,106 +235,6 @@ static struct mii_bus *mv88e6xxx_default_mdio_bus(struct mv88e6xxx_chip *chip)
 	return mdio_bus->bus;
 }
 
-static int mv88e6xxx_phy_read(struct mv88e6xxx_chip *chip, int phy,
-			      int reg, u16 *val)
-{
-	int addr = phy; /* PHY devices addresses start at 0x0 */
-	struct mii_bus *bus;
-
-	bus = mv88e6xxx_default_mdio_bus(chip);
-	if (!bus)
-		return -EOPNOTSUPP;
-
-	if (!chip->info->ops->phy_read)
-		return -EOPNOTSUPP;
-
-	return chip->info->ops->phy_read(chip, bus, addr, reg, val);
-}
-
-static int mv88e6xxx_phy_write(struct mv88e6xxx_chip *chip, int phy,
-			       int reg, u16 val)
-{
-	int addr = phy; /* PHY devices addresses start at 0x0 */
-	struct mii_bus *bus;
-
-	bus = mv88e6xxx_default_mdio_bus(chip);
-	if (!bus)
-		return -EOPNOTSUPP;
-
-	if (!chip->info->ops->phy_write)
-		return -EOPNOTSUPP;
-
-	return chip->info->ops->phy_write(chip, bus, addr, reg, val);
-}
-
-static int mv88e6xxx_phy_page_get(struct mv88e6xxx_chip *chip, int phy, u8 page)
-{
-	if (!mv88e6xxx_has(chip, MV88E6XXX_FLAG_PHY_PAGE))
-		return -EOPNOTSUPP;
-
-	return mv88e6xxx_phy_write(chip, phy, PHY_PAGE, page);
-}
-
-static void mv88e6xxx_phy_page_put(struct mv88e6xxx_chip *chip, int phy)
-{
-	int err;
-
-	/* Restore PHY page Copper 0x0 for access via the registered MDIO bus */
-	err = mv88e6xxx_phy_write(chip, phy, PHY_PAGE, PHY_PAGE_COPPER);
-	if (unlikely(err)) {
-		dev_err(chip->dev, "failed to restore PHY %d page Copper (%d)\n",
-			phy, err);
-	}
-}
-
-static int mv88e6xxx_phy_page_read(struct mv88e6xxx_chip *chip, int phy,
-				   u8 page, int reg, u16 *val)
-{
-	int err;
-
-	/* There is no paging for registers 22 */
-	if (reg == PHY_PAGE)
-		return -EINVAL;
-
-	err = mv88e6xxx_phy_page_get(chip, phy, page);
-	if (!err) {
-		err = mv88e6xxx_phy_read(chip, phy, reg, val);
-		mv88e6xxx_phy_page_put(chip, phy);
-	}
-
-	return err;
-}
-
-static int mv88e6xxx_phy_page_write(struct mv88e6xxx_chip *chip, int phy,
-				    u8 page, int reg, u16 val)
-{
-	int err;
-
-	/* There is no paging for registers 22 */
-	if (reg == PHY_PAGE)
-		return -EINVAL;
-
-	err = mv88e6xxx_phy_page_get(chip, phy, page);
-	if (!err) {
-		err = mv88e6xxx_phy_write(chip, phy, PHY_PAGE, page);
-		mv88e6xxx_phy_page_put(chip, phy);
-	}
-
-	return err;
-}
-
-static int mv88e6xxx_serdes_read(struct mv88e6xxx_chip *chip, int reg, u16 *val)
-{
-	return mv88e6xxx_phy_page_read(chip, ADDR_SERDES, SERDES_PAGE_FIBER,
-				       reg, val);
-}
-
-static int mv88e6xxx_serdes_write(struct mv88e6xxx_chip *chip, int reg, u16 val)
-{
-	return mv88e6xxx_phy_page_write(chip, ADDR_SERDES, SERDES_PAGE_FIBER,
-					reg, val);
-}
-
 static void mv88e6xxx_g1_irq_mask(struct irq_data *d)
 {
 	struct mv88e6xxx_chip *chip = irq_data_get_irq_chip_data(d);
@@ -374,7 +261,7 @@ static irqreturn_t mv88e6xxx_g1_irq_thread_fn(int irq, void *dev_id)
 	int err;
 
 	mutex_lock(&chip->reg_lock);
-	err = mv88e6xxx_g1_read(chip, GLOBAL_STATUS, &reg);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_STS, &reg);
 	mutex_unlock(&chip->reg_lock);
 
 	if (err)
@@ -405,14 +292,14 @@ static void mv88e6xxx_g1_irq_bus_sync_unlock(struct irq_data *d)
 	u16 reg;
 	int err;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_CONTROL, &reg);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_CTL1, &reg);
 	if (err)
 		goto out;
 
 	reg &= ~mask;
 	reg |= (~chip->g1_irq.masked & mask);
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_CONTROL, reg);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL1, reg);
 	if (err)
 		goto out;
 
@@ -451,9 +338,9 @@ static void mv88e6xxx_g1_irq_free(struct mv88e6xxx_chip *chip)
 	int irq, virq;
 	u16 mask;
 
-	mv88e6xxx_g1_read(chip, GLOBAL_CONTROL, &mask);
+	mv88e6xxx_g1_read(chip, MV88E6XXX_G1_CTL1, &mask);
 	mask |= GENMASK(chip->g1_irq.nirqs, 0);
-	mv88e6xxx_g1_write(chip, GLOBAL_CONTROL, mask);
+	mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL1, mask);
 
 	free_irq(chip->irq, chip);
 
@@ -483,18 +370,18 @@ static int mv88e6xxx_g1_irq_setup(struct mv88e6xxx_chip *chip)
 	chip->g1_irq.chip = mv88e6xxx_g1_irq_chip;
 	chip->g1_irq.masked = ~0;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_CONTROL, &mask);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_CTL1, &mask);
 	if (err)
 		goto out_mapping;
 
 	mask &= ~GENMASK(chip->g1_irq.nirqs, 0);
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_CONTROL, mask);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL1, mask);
 	if (err)
 		goto out_disable;
 
 	/* Reading the interrupt status clears (most of) them */
-	err = mv88e6xxx_g1_read(chip, GLOBAL_STATUS, &reg);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_STS, &reg);
 	if (err)
 		goto out_disable;
 
@@ -509,7 +396,7 @@ static int mv88e6xxx_g1_irq_setup(struct mv88e6xxx_chip *chip)
 
 out_disable:
 	mask |= GENMASK(chip->g1_irq.nirqs, 0);
-	mv88e6xxx_g1_write(chip, GLOBAL_CONTROL, mask);
+	mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL1, mask);
 
 out_mapping:
 	for (irq = 0; irq < 16; irq++) {
@@ -561,122 +448,6 @@ int mv88e6xxx_update(struct mv88e6xxx_chip *chip, int addr, int reg, u16 update)
 	return mv88e6xxx_write(chip, addr, reg, val);
 }
 
-static int mv88e6xxx_ppu_disable(struct mv88e6xxx_chip *chip)
-{
-	if (!chip->info->ops->ppu_disable)
-		return 0;
-
-	return chip->info->ops->ppu_disable(chip);
-}
-
-static int mv88e6xxx_ppu_enable(struct mv88e6xxx_chip *chip)
-{
-	if (!chip->info->ops->ppu_enable)
-		return 0;
-
-	return chip->info->ops->ppu_enable(chip);
-}
-
-static void mv88e6xxx_ppu_reenable_work(struct work_struct *ugly)
-{
-	struct mv88e6xxx_chip *chip;
-
-	chip = container_of(ugly, struct mv88e6xxx_chip, ppu_work);
-
-	mutex_lock(&chip->reg_lock);
-
-	if (mutex_trylock(&chip->ppu_mutex)) {
-		if (mv88e6xxx_ppu_enable(chip) == 0)
-			chip->ppu_disabled = 0;
-		mutex_unlock(&chip->ppu_mutex);
-	}
-
-	mutex_unlock(&chip->reg_lock);
-}
-
-static void mv88e6xxx_ppu_reenable_timer(unsigned long _ps)
-{
-	struct mv88e6xxx_chip *chip = (void *)_ps;
-
-	schedule_work(&chip->ppu_work);
-}
-
-static int mv88e6xxx_ppu_access_get(struct mv88e6xxx_chip *chip)
-{
-	int ret;
-
-	mutex_lock(&chip->ppu_mutex);
-
-	/* If the PHY polling unit is enabled, disable it so that
-	 * we can access the PHY registers.  If it was already
-	 * disabled, cancel the timer that is going to re-enable
-	 * it.
-	 */
-	if (!chip->ppu_disabled) {
-		ret = mv88e6xxx_ppu_disable(chip);
-		if (ret < 0) {
-			mutex_unlock(&chip->ppu_mutex);
-			return ret;
-		}
-		chip->ppu_disabled = 1;
-	} else {
-		del_timer(&chip->ppu_timer);
-		ret = 0;
-	}
-
-	return ret;
-}
-
-static void mv88e6xxx_ppu_access_put(struct mv88e6xxx_chip *chip)
-{
-	/* Schedule a timer to re-enable the PHY polling unit. */
-	mod_timer(&chip->ppu_timer, jiffies + msecs_to_jiffies(10));
-	mutex_unlock(&chip->ppu_mutex);
-}
-
-static void mv88e6xxx_ppu_state_init(struct mv88e6xxx_chip *chip)
-{
-	mutex_init(&chip->ppu_mutex);
-	INIT_WORK(&chip->ppu_work, mv88e6xxx_ppu_reenable_work);
-	setup_timer(&chip->ppu_timer, mv88e6xxx_ppu_reenable_timer,
-		    (unsigned long)chip);
-}
-
-static void mv88e6xxx_ppu_state_destroy(struct mv88e6xxx_chip *chip)
-{
-	del_timer_sync(&chip->ppu_timer);
-}
-
-static int mv88e6xxx_phy_ppu_read(struct mv88e6xxx_chip *chip,
-				  struct mii_bus *bus,
-				  int addr, int reg, u16 *val)
-{
-	int err;
-
-	err = mv88e6xxx_ppu_access_get(chip);
-	if (!err) {
-		err = mv88e6xxx_read(chip, addr, reg, val);
-		mv88e6xxx_ppu_access_put(chip);
-	}
-
-	return err;
-}
-
-static int mv88e6xxx_phy_ppu_write(struct mv88e6xxx_chip *chip,
-				   struct mii_bus *bus,
-				   int addr, int reg, u16 val)
-{
-	int err;
-
-	err = mv88e6xxx_ppu_access_get(chip);
-	if (!err) {
-		err = mv88e6xxx_write(chip, addr, reg, val);
-		mv88e6xxx_ppu_access_put(chip);
-	}
-
-	return err;
-}
-
 static int mv88e6xxx_port_setup_mac(struct mv88e6xxx_chip *chip, int port,
 				    int link, int speed, int duplex,
 				    phy_interface_t mode)
@@ -718,8 +489,7 @@ static int mv88e6xxx_port_setup_mac(struct mv88e6xxx_chip *chip, int port,
 	err = 0;
 restore_link:
 	if (chip->info->ops->port_set_link(chip, port, link))
-		netdev_err(chip->ds->ports[port].netdev,
-			   "failed to restore MAC's link\n");
+		dev_err(chip->dev, "p%d: failed to restore MAC's link\n", port);
 
 	return err;
 }
@@ -743,7 +513,7 @@ static void mv88e6xxx_adjust_link(struct dsa_switch *ds, int port,
 	mutex_unlock(&chip->reg_lock);
 
 	if (err && err != -EOPNOTSUPP)
-		netdev_err(ds->ports[port].netdev, "failed to configure MAC\n");
+		dev_err(ds->dev, "p%d: failed to configure MAC\n", port);
 }
 
 static int mv88e6xxx_stats_snapshot(struct mv88e6xxx_chip *chip, int port)
@@ -955,7 +725,7 @@ static void mv88e6095_stats_get_stats(struct mv88e6xxx_chip *chip, int port,
 {
 	return mv88e6xxx_stats_get_stats(chip, port, data,
 					 STATS_TYPE_BANK0 | STATS_TYPE_PORT,
-					 0, GLOBAL_STATS_OP_HIST_RX_TX);
+					 0, MV88E6XXX_G1_STATS_OP_HIST_RX_TX);
 }
 
 static void mv88e6320_stats_get_stats(struct mv88e6xxx_chip *chip, int port,
@@ -963,8 +733,8 @@ static void mv88e6320_stats_get_stats(struct mv88e6xxx_chip *chip, int port,
 {
 	return mv88e6xxx_stats_get_stats(chip, port, data,
 					 STATS_TYPE_BANK0 | STATS_TYPE_BANK1,
-					 GLOBAL_STATS_OP_BANK_1_BIT_9,
-					 GLOBAL_STATS_OP_HIST_RX_TX);
+					 MV88E6XXX_G1_STATS_OP_BANK_1_BIT_9,
+					 MV88E6XXX_G1_STATS_OP_HIST_RX_TX);
 }
 
 static void mv88e6390_stats_get_stats(struct mv88e6xxx_chip *chip, int port,
@@ -972,7 +742,8 @@ static void mv88e6390_stats_get_stats(struct mv88e6xxx_chip *chip, int port,
 {
 	return mv88e6xxx_stats_get_stats(chip, port, data,
 					 STATS_TYPE_BANK0 | STATS_TYPE_BANK1,
-					 GLOBAL_STATS_OP_BANK_1_BIT_10, 0);
+					 MV88E6XXX_G1_STATS_OP_BANK_1_BIT_10,
+					 0);
 }
 
 static void mv88e6xxx_get_stats(struct mv88e6xxx_chip *chip, int port,
@@ -1058,11 +829,11 @@ static int mv88e6xxx_get_eee(struct dsa_switch *ds, int port,
 	e->eee_enabled = !!(reg & 0x0200);
 	e->tx_lpi_enabled = !!(reg & 0x0100);
 
-	err = mv88e6xxx_port_read(chip, port, PORT_STATUS, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_STS, &reg);
 	if (err)
 		goto out;
 
-	e->eee_active = !!(reg & PORT_STATUS_EEE);
+	e->eee_active = !!(reg & MV88E6352_PORT_STS_EEE);
 out:
 	mutex_unlock(&chip->reg_lock);
 
@@ -1145,32 +916,14 @@ static void mv88e6xxx_port_stp_state_set(struct dsa_switch *ds, int port,
 					 u8 state)
 {
 	struct mv88e6xxx_chip *chip = ds->priv;
-	int stp_state;
 	int err;
 
-	switch (state) {
-	case BR_STATE_DISABLED:
-		stp_state = PORT_CONTROL_STATE_DISABLED;
-		break;
-	case BR_STATE_BLOCKING:
-	case BR_STATE_LISTENING:
-		stp_state = PORT_CONTROL_STATE_BLOCKING;
-		break;
-	case BR_STATE_LEARNING:
-		stp_state = PORT_CONTROL_STATE_LEARNING;
-		break;
-	case BR_STATE_FORWARDING:
-	default:
-		stp_state = PORT_CONTROL_STATE_FORWARDING;
-		break;
-	}
-
 	mutex_lock(&chip->reg_lock);
-	err = mv88e6xxx_port_set_state(chip, port, stp_state);
+	err = mv88e6xxx_port_set_state(chip, port, state);
 	mutex_unlock(&chip->reg_lock);
 
 	if (err)
-		netdev_err(ds->ports[port].netdev, "failed to update state\n");
+		dev_err(ds->dev, "p%d: failed to update state\n", port);
 }
 
 static int mv88e6xxx_atu_setup(struct mv88e6xxx_chip *chip)
@@ -1188,6 +941,26 @@ static int mv88e6xxx_atu_setup(struct mv88e6xxx_chip *chip)
 	return mv88e6xxx_g1_atu_set_age_time(chip, 300000);
 }
 
+static int mv88e6xxx_irl_setup(struct mv88e6xxx_chip *chip)
+{
+	int port;
+	int err;
+
+	if (!chip->info->ops->irl_init_all)
+		return 0;
+
+	for (port = 0; port < mv88e6xxx_num_ports(chip); port++) {
+		/* Disable ingress rate limiting by resetting all per port
+		 * ingress rate limit resources to their initial state.
+		 */
+		err = chip->info->ops->irl_init_all(chip, port);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
 static int mv88e6xxx_pvt_map(struct mv88e6xxx_chip *chip, int dev, int port)
 {
 	u16 pvlan = 0;
@@ -1238,7 +1011,7 @@ static void mv88e6xxx_port_fast_age(struct dsa_switch *ds, int port)
 	mutex_unlock(&chip->reg_lock);
 
 	if (err)
-		netdev_err(ds->ports[port].netdev, "failed to flush ATU\n");
+		dev_err(ds->dev, "p%d: failed to flush ATU\n", port);
 }
 
 static int mv88e6xxx_vtu_setup(struct mv88e6xxx_chip *chip)
@@ -1269,7 +1042,7 @@ static int mv88e6xxx_vtu_loadpurge(struct mv88e6xxx_chip *chip,
 
 static int mv88e6xxx_port_vlan_dump(struct dsa_switch *ds, int port,
 				    struct switchdev_obj_port_vlan *vlan,
-				    int (*cb)(struct switchdev_obj *obj))
+				    switchdev_obj_dump_cb_t *cb)
 {
 	struct mv88e6xxx_chip *chip = ds->priv;
 	struct mv88e6xxx_vtu_entry next = {
@@ -1295,7 +1068,8 @@ static int mv88e6xxx_port_vlan_dump(struct dsa_switch *ds, int port,
 		if (!next.valid)
 			break;
 
-		if (next.member[port] == GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER)
+		if (next.member[port] ==
+		    MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_NON_MEMBER)
 			continue;
 
 		/* reinit and dump this VLAN obj */
@@ -1303,7 +1077,8 @@ static int mv88e6xxx_port_vlan_dump(struct dsa_switch *ds, int port,
 		vlan->vid_end = next.vid;
 		vlan->flags = 0;
 
-		if (next.member[port] == GLOBAL_VTU_DATA_MEMBER_TAG_UNTAGGED)
+		if (next.member[port] ==
+		    MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_UNTAGGED)
 			vlan->flags |= BRIDGE_VLAN_INFO_UNTAGGED;
 
 		if (next.vid == pvid)
@@ -1388,11 +1163,10 @@ static int mv88e6xxx_vtu_get(struct mv88e6xxx_chip *chip, u16 vid,
 		entry->valid = true;
 		entry->vid = vid;
 
-		/* Include only CPU and DSA ports */
+		/* Exclude all ports */
 		for (i = 0; i < mv88e6xxx_num_ports(chip); ++i)
-			entry->member[i] = dsa_is_normal_port(chip->ds, i) ?
-				GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER :
-				GLOBAL_VTU_DATA_MEMBER_TAG_UNMODIFIED;
+			entry->member[i] =
+				MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_NON_MEMBER;
 
 		return mv88e6xxx_atu_new(chip, &entry->fid);
 	}
@@ -1434,7 +1208,7 @@ static int mv88e6xxx_port_check_hw_vlan(struct dsa_switch *ds, int port,
 				continue;
 
 			if (vlan.member[i] ==
-			    GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER)
+			    MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_NON_MEMBER)
 				continue;
 
 			if (ds->ports[i].bridge_dev ==
@@ -1444,10 +1218,9 @@ static int mv88e6xxx_port_check_hw_vlan(struct dsa_switch *ds, int port,
 			if (!ds->ports[i].bridge_dev)
 				continue;
 
-			netdev_warn(ds->ports[port].netdev,
-				    "hardware VLAN %d already used by %s\n",
-				    vlan.vid,
-				    netdev_name(ds->ports[i].bridge_dev));
+			dev_err(ds->dev, "p%d: hw VLAN %d already used by %s\n",
+				port, vlan.vid,
+				netdev_name(ds->ports[i].bridge_dev));
 			err = -EOPNOTSUPP;
 			goto unlock;
 		}
@@ -1463,8 +1236,8 @@ static int mv88e6xxx_port_vlan_filtering(struct dsa_switch *ds, int port,
 					 bool vlan_filtering)
 {
 	struct mv88e6xxx_chip *chip = ds->priv;
-	u16 mode = vlan_filtering ? PORT_CONTROL_2_8021Q_SECURE :
-		PORT_CONTROL_2_8021Q_DISABLED;
+	u16 mode = vlan_filtering ? MV88E6XXX_PORT_CTL2_8021Q_MODE_SECURE :
+		MV88E6XXX_PORT_CTL2_8021Q_MODE_DISABLED;
 	int err;
 
 	if (!chip->info->max_vid)
@@ -1503,7 +1276,7 @@ mv88e6xxx_port_vlan_prepare(struct dsa_switch *ds, int port,
 }
 
 static int _mv88e6xxx_port_vlan_add(struct mv88e6xxx_chip *chip, int port,
-				    u16 vid, bool untagged)
+				    u16 vid, u8 member)
 {
 	struct mv88e6xxx_vtu_entry vlan;
 	int err;
@@ -1512,9 +1285,7 @@ static int _mv88e6xxx_port_vlan_add(struct mv88e6xxx_chip *chip, int port,
 	if (err)
 		return err;
 
-	vlan.member[port] = untagged ?
-		GLOBAL_VTU_DATA_MEMBER_TAG_UNTAGGED :
-		GLOBAL_VTU_DATA_MEMBER_TAG_TAGGED;
+	vlan.member[port] = member;
 
 	return mv88e6xxx_vtu_loadpurge(chip, &vlan);
 }
@@ -1526,22 +1297,29 @@ static void mv88e6xxx_port_vlan_add(struct dsa_switch *ds, int port,
 	struct mv88e6xxx_chip *chip = ds->priv;
 	bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
 	bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
+	u8 member;
 	u16 vid;
 
 	if (!chip->info->max_vid)
 		return;
 
+	if (dsa_is_dsa_port(ds, port) || dsa_is_cpu_port(ds, port))
+		member = MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_UNMODIFIED;
+	else if (untagged)
+		member = MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_UNTAGGED;
+	else
+		member = MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_TAGGED;
+
 	mutex_lock(&chip->reg_lock);
 
 	for (vid = vlan->vid_begin; vid <= vlan->vid_end; ++vid)
-		if (_mv88e6xxx_port_vlan_add(chip, port, vid, untagged))
-			netdev_err(ds->ports[port].netdev,
-				   "failed to add VLAN %d%c\n",
-				   vid, untagged ? 'u' : 't');
+		if (_mv88e6xxx_port_vlan_add(chip, port, vid, member))
+			dev_err(ds->dev, "p%d: failed to add VLAN %d%c\n", port,
+				vid, untagged ? 'u' : 't');
 
 	if (pvid && mv88e6xxx_port_set_pvid(chip, port, vlan->vid_end))
-		netdev_err(ds->ports[port].netdev, "failed to set PVID %d\n",
-			   vlan->vid_end);
+		dev_err(ds->dev, "p%d: failed to set PVID %d\n", port,
+			vlan->vid_end);
 
 	mutex_unlock(&chip->reg_lock);
 }
@@ -1549,7 +1327,6 @@ static void mv88e6xxx_port_vlan_add(struct dsa_switch *ds, int port,
 static int _mv88e6xxx_port_vlan_del(struct mv88e6xxx_chip *chip,
 				    int port, u16 vid)
 {
-	struct dsa_switch *ds = chip->ds;
 	struct mv88e6xxx_vtu_entry vlan;
 	int i, err;
 
@@ -1558,18 +1335,16 @@ static int _mv88e6xxx_port_vlan_del(struct mv88e6xxx_chip *chip,
 		return err;
 
 	/* Tell switchdev if this VLAN is handled in software */
-	if (vlan.member[port] == GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER)
+	if (vlan.member[port] == MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_NON_MEMBER)
 		return -EOPNOTSUPP;
 
-	vlan.member[port] = GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER;
+	vlan.member[port] = MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_NON_MEMBER;
 
 	/* keep the VLAN unless all ports are excluded */
 	vlan.valid = false;
 	for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
-		if (dsa_is_cpu_port(ds, i) || dsa_is_dsa_port(ds, i))
-			continue;
-
-		if (vlan.member[i] != GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER) {
+		if (vlan.member[i] !=
+		    MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_NON_MEMBER) {
 			vlan.valid = true;
 			break;
 		}
@@ -1632,7 +1407,7 @@ static int mv88e6xxx_port_db_load_purge(struct mv88e6xxx_chip *chip, int port,
 	if (err)
 		return err;
 
-	entry.state = GLOBAL_ATU_DATA_STATE_UNUSED;
+	entry.state = MV88E6XXX_G1_ATU_DATA_STATE_UNUSED;
 	ether_addr_copy(entry.mac, addr);
 	eth_addr_dec(entry.mac);
 
@@ -1641,17 +1416,17 @@ static int mv88e6xxx_port_db_load_purge(struct mv88e6xxx_chip *chip, int port,
 		return err;
 
 	/* Initialize a fresh ATU entry if it isn't found */
-	if (entry.state == GLOBAL_ATU_DATA_STATE_UNUSED ||
+	if (entry.state == MV88E6XXX_G1_ATU_DATA_STATE_UNUSED ||
 	    !ether_addr_equal(entry.mac, addr)) {
 		memset(&entry, 0, sizeof(entry));
 		ether_addr_copy(entry.mac, addr);
 	}
 
 	/* Purge the ATU entry only if no port is using it anymore */
-	if (state == GLOBAL_ATU_DATA_STATE_UNUSED) {
+	if (state == MV88E6XXX_G1_ATU_DATA_STATE_UNUSED) {
 		entry.portvec &= ~BIT(port);
 		if (!entry.portvec)
-			entry.state = GLOBAL_ATU_DATA_STATE_UNUSED;
+			entry.state = MV88E6XXX_G1_ATU_DATA_STATE_UNUSED;
 	} else {
 		entry.portvec |= BIT(port);
 		entry.state = state;
@@ -1678,8 +1453,9 @@ static void mv88e6xxx_port_fdb_add(struct dsa_switch *ds, int port,
 
 	mutex_lock(&chip->reg_lock);
 	if (mv88e6xxx_port_db_load_purge(chip, port, fdb->addr, fdb->vid,
-					 GLOBAL_ATU_DATA_STATE_UC_STATIC))
-		netdev_err(ds->ports[port].netdev, "failed to load unicast MAC address\n");
+					 MV88E6XXX_G1_ATU_DATA_STATE_UC_STATIC))
+		dev_err(ds->dev, "p%d: failed to load unicast MAC address\n",
+			port);
 	mutex_unlock(&chip->reg_lock);
 }
 
@@ -1691,7 +1467,7 @@ static int mv88e6xxx_port_fdb_del(struct dsa_switch *ds, int port,
 
 	mutex_lock(&chip->reg_lock);
 	err = mv88e6xxx_port_db_load_purge(chip, port, fdb->addr, fdb->vid,
-					   GLOBAL_ATU_DATA_STATE_UNUSED);
+					   MV88E6XXX_G1_ATU_DATA_STATE_UNUSED);
 	mutex_unlock(&chip->reg_lock);
 
 	return err;
@@ -1700,12 +1476,12 @@ static int mv88e6xxx_port_fdb_del(struct dsa_switch *ds, int port,
 static int mv88e6xxx_port_db_dump_fid(struct mv88e6xxx_chip *chip,
 				      u16 fid, u16 vid, int port,
 				      struct switchdev_obj *obj,
-				      int (*cb)(struct switchdev_obj *obj))
+				      switchdev_obj_dump_cb_t *cb)
 {
 	struct mv88e6xxx_atu_entry addr;
 	int err;
 
-	addr.state = GLOBAL_ATU_DATA_STATE_UNUSED;
+	addr.state = MV88E6XXX_G1_ATU_DATA_STATE_UNUSED;
 	eth_broadcast_addr(addr.mac);
 
 	do {
@@ -1713,7 +1489,7 @@ static int mv88e6xxx_port_db_dump_fid(struct mv88e6xxx_chip *chip,
 		if (err)
 			return err;
 
-		if (addr.state == GLOBAL_ATU_DATA_STATE_UNUSED)
+		if (addr.state == MV88E6XXX_G1_ATU_DATA_STATE_UNUSED)
 			break;
 
 		if (addr.trunk || (addr.portvec & BIT(port)) == 0)
@@ -1728,7 +1504,7 @@ static int mv88e6xxx_port_db_dump_fid(struct mv88e6xxx_chip *chip,
 			fdb = SWITCHDEV_OBJ_PORT_FDB(obj);
 			fdb->vid = vid;
 			ether_addr_copy(fdb->addr, addr.mac);
-			if (addr.state == GLOBAL_ATU_DATA_STATE_UC_STATIC)
+			if (addr.state == MV88E6XXX_G1_ATU_DATA_STATE_UC_STATIC)
 				fdb->ndm_state = NUD_NOARP;
 			else
 				fdb->ndm_state = NUD_REACHABLE;
@@ -1755,7 +1531,7 @@ static int mv88e6xxx_port_db_dump_fid(struct mv88e6xxx_chip *chip,
 
 static int mv88e6xxx_port_db_dump(struct mv88e6xxx_chip *chip, int port,
 				  struct switchdev_obj *obj,
-				  int (*cb)(struct switchdev_obj *obj))
+				  switchdev_obj_dump_cb_t *cb)
 {
 	struct mv88e6xxx_vtu_entry vlan = {
 		.vid = chip->info->max_vid,
@@ -1792,7 +1568,7 @@ static int mv88e6xxx_port_db_dump(struct mv88e6xxx_chip *chip, int port,
 
 static int mv88e6xxx_port_fdb_dump(struct dsa_switch *ds, int port,
 				   struct switchdev_obj_port_fdb *fdb,
-				   int (*cb)(struct switchdev_obj *obj))
+				   switchdev_obj_dump_cb_t *cb)
 {
 	struct mv88e6xxx_chip *chip = ds->priv;
 	int err;
@@ -1924,8 +1700,7 @@ static int mv88e6xxx_disable_ports(struct mv88e6xxx_chip *chip)
 
 	/* Set all ports to the Disabled state */
 	for (i = 0; i < mv88e6xxx_num_ports(chip); i++) {
-		err = mv88e6xxx_port_set_state(chip, i,
-					       PORT_CONTROL_STATE_DISABLED);
+		err = mv88e6xxx_port_set_state(chip, i, BR_STATE_DISABLED);
 		if (err)
 			return err;
 	}
@@ -1951,27 +1726,9 @@ static int mv88e6xxx_switch_reset(struct mv88e6xxx_chip *chip)
 	return mv88e6xxx_software_reset(chip);
 }
 
-static int mv88e6xxx_serdes_power_on(struct mv88e6xxx_chip *chip)
-{
-	u16 val;
-	int err;
-
-	/* Clear Power Down bit */
-	err = mv88e6xxx_serdes_read(chip, MII_BMCR, &val);
-	if (err)
-		return err;
-
-	if (val & BMCR_PDOWN) {
-		val &= ~BMCR_PDOWN;
-		err = mv88e6xxx_serdes_write(chip, MII_BMCR, val);
-	}
-
-	return err;
-}
-
 static int mv88e6xxx_set_port_mode(struct mv88e6xxx_chip *chip, int port,
-				   enum mv88e6xxx_frame_mode frame, u16 egress,
-				   u16 etype)
+				   enum mv88e6xxx_frame_mode frame,
+				   enum mv88e6xxx_egress_mode egress, u16 etype)
 {
 	int err;
 
@@ -1995,22 +1752,23 @@ static int mv88e6xxx_set_port_mode(struct mv88e6xxx_chip *chip, int port,
 static int mv88e6xxx_set_port_mode_normal(struct mv88e6xxx_chip *chip, int port)
 {
 	return mv88e6xxx_set_port_mode(chip, port, MV88E6XXX_FRAME_MODE_NORMAL,
-				       PORT_CONTROL_EGRESS_UNMODIFIED,
-				       PORT_ETH_TYPE_DEFAULT);
+				       MV88E6XXX_EGRESS_MODE_UNMODIFIED,
+				       MV88E6XXX_PORT_ETH_TYPE_DEFAULT);
 }
 
 static int mv88e6xxx_set_port_mode_dsa(struct mv88e6xxx_chip *chip, int port)
 {
 	return mv88e6xxx_set_port_mode(chip, port, MV88E6XXX_FRAME_MODE_DSA,
-				       PORT_CONTROL_EGRESS_UNMODIFIED,
-				       PORT_ETH_TYPE_DEFAULT);
+				       MV88E6XXX_EGRESS_MODE_UNMODIFIED,
+				       MV88E6XXX_PORT_ETH_TYPE_DEFAULT);
 }
 
 static int mv88e6xxx_set_port_mode_edsa(struct mv88e6xxx_chip *chip, int port)
 {
 	return mv88e6xxx_set_port_mode(chip, port,
 				       MV88E6XXX_FRAME_MODE_ETHERTYPE,
-				       PORT_CONTROL_EGRESS_ADD_TAG, ETH_P_EDSA);
+				       MV88E6XXX_EGRESS_MODE_ETHERTYPE,
+				       ETH_P_EDSA);
 }
 
 static int mv88e6xxx_setup_port_mode(struct mv88e6xxx_chip *chip, int port)
@@ -2050,6 +1808,15 @@ static int mv88e6xxx_setup_egress_floods(struct mv88e6xxx_chip *chip, int port)
 	return 0;
 }
 
+static int mv88e6xxx_serdes_power(struct mv88e6xxx_chip *chip, int port,
+				  bool on)
+{
+	if (chip->info->ops->serdes_power)
+		return chip->info->ops->serdes_power(chip, port, on);
+
+	return 0;
+}
+
 static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 {
 	struct dsa_switch *ds = chip->ds;
@@ -2085,10 +1852,10 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 	 * If this is the upstream port for this switch, enable
 	 * forwarding of unknown unicasts and multicasts.
 	 */
-	reg = PORT_CONTROL_IGMP_MLD_SNOOP |
-		PORT_CONTROL_USE_TAG | PORT_CONTROL_USE_IP |
-		PORT_CONTROL_STATE_FORWARDING;
-	err = mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
+	reg = MV88E6XXX_PORT_CTL0_IGMP_MLD_SNOOP |
+		MV88E6185_PORT_CTL0_USE_TAG | MV88E6185_PORT_CTL0_USE_IP |
+		MV88E6XXX_PORT_CTL0_STATE_FORWARDING;
+	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 	if (err)
 		return err;
 
@@ -2100,21 +1867,14 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 	if (err)
 		return err;
 
-	/* If this port is connected to a SerDes, make sure the SerDes is not
-	 * powered down.
+	/* Enable the SERDES interface for DSA and CPU ports. Normal
+	 * ports SERDES are enabled when the port is enabled, thus
+	 * saving a bit of power.
 	 */
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAGS_SERDES)) {
-		err = mv88e6xxx_port_read(chip, port, PORT_STATUS, &reg);
+	if ((dsa_is_cpu_port(ds, port) || dsa_is_dsa_port(ds, port))) {
+		err = mv88e6xxx_serdes_power(chip, port, true);
 		if (err)
 			return err;
-		reg &= PORT_STATUS_CMODE_MASK;
-		if ((reg == PORT_STATUS_CMODE_100BASE_X) ||
-		    (reg == PORT_STATUS_CMODE_1000BASE_X) ||
-		    (reg == PORT_STATUS_CMODE_SGMII)) {
-			err = mv88e6xxx_serdes_power_on(chip);
-			if (err < 0)
-				return err;
-		}
 	}
 
 	/* Port Control 2: don't force a good FCS, set the maximum frame size to
@@ -2136,12 +1896,12 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 	}
 
 	err = mv88e6xxx_port_set_8021q_mode(chip, port,
-					    PORT_CONTROL_2_8021Q_DISABLED);
+				MV88E6XXX_PORT_CTL2_8021Q_MODE_DISABLED);
 	if (err)
 		return err;
 
-	if (chip->info->ops->port_jumbo_config) {
-		err = chip->info->ops->port_jumbo_config(chip, port);
+	if (chip->info->ops->port_set_jumbo_size) {
+		err = chip->info->ops->port_set_jumbo_size(chip, port, 10240);
 		if (err)
 			return err;
 	}
@@ -2156,17 +1916,19 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 	if (dsa_is_cpu_port(ds, port))
 		reg = 0;
 
-	err = mv88e6xxx_port_write(chip, port, PORT_ASSOC_VECTOR, reg);
+	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_ASSOC_VECTOR,
+				   reg);
 	if (err)
 		return err;
 
 	/* Egress rate control 2: disable egress rate control. */
-	err = mv88e6xxx_port_write(chip, port, PORT_RATE_CONTROL_2, 0x0000);
+	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_EGRESS_RATE_CTL2,
+				   0x0000);
 	if (err)
 		return err;
 
-	if (chip->info->ops->port_pause_config) {
-		err = chip->info->ops->port_pause_config(chip, port);
+	if (chip->info->ops->port_pause_limit) {
+		err = chip->info->ops->port_pause_limit(chip, port, 0, 0);
 		if (err)
 			return err;
 	}
@@ -2214,26 +1976,31 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 	/* Default VLAN ID and priority: don't set a default VLAN
 	 * ID, and set the default packet priority to zero.
 	 */
-	return mv88e6xxx_port_write(chip, port, PORT_DEFAULT_VLAN, 0x0000);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_DEFAULT_VLAN, 0);
 }
 
-static int mv88e6xxx_g1_set_switch_mac(struct mv88e6xxx_chip *chip, u8 *addr)
+static int mv88e6xxx_port_enable(struct dsa_switch *ds, int port,
+				 struct phy_device *phydev)
 {
+	struct mv88e6xxx_chip *chip = ds->priv;
 	int err;
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_MAC_01, (addr[0] << 8) | addr[1]);
-	if (err)
-		return err;
+	mutex_lock(&chip->reg_lock);
+	err = mv88e6xxx_serdes_power(chip, port, true);
+	mutex_unlock(&chip->reg_lock);
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_MAC_23, (addr[2] << 8) | addr[3]);
-	if (err)
-		return err;
+	return err;
+}
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_MAC_45, (addr[4] << 8) | addr[5]);
-	if (err)
-		return err;
+static void mv88e6xxx_port_disable(struct dsa_switch *ds, int port,
+				   struct phy_device *phydev)
+{
+	struct mv88e6xxx_chip *chip = ds->priv;
 
-	return 0;
+	mutex_lock(&chip->reg_lock);
+	if (mv88e6xxx_serdes_power(chip, port, false))
+		dev_err(chip->dev, "failed to power off SERDES\n");
+	mutex_unlock(&chip->reg_lock);
 }
 
 static int mv88e6xxx_set_ageing_time(struct dsa_switch *ds,
@@ -2255,60 +2022,53 @@ static int mv88e6xxx_g1_setup(struct mv88e6xxx_chip *chip)
 	u32 upstream_port = dsa_upstream_port(ds);
 	int err;
 
-	/* Enable the PHY Polling Unit if present, don't discard any packets,
-	 * and mask all interrupt sources.
-	 */
-	err = mv88e6xxx_ppu_enable(chip);
-	if (err)
-		return err;
-
-	if (chip->info->ops->g1_set_cpu_port) {
-		err = chip->info->ops->g1_set_cpu_port(chip, upstream_port);
+	if (chip->info->ops->set_cpu_port) {
+		err = chip->info->ops->set_cpu_port(chip, upstream_port);
 		if (err)
 			return err;
 	}
 
-	if (chip->info->ops->g1_set_egress_port) {
-		err = chip->info->ops->g1_set_egress_port(chip, upstream_port);
+	if (chip->info->ops->set_egress_port) {
+		err = chip->info->ops->set_egress_port(chip, upstream_port);
 		if (err)
 			return err;
 	}
 
 	/* Disable remote management, and set the switch's DSA device number. */
-	err = mv88e6xxx_g1_write(chip, GLOBAL_CONTROL_2,
-				 GLOBAL_CONTROL_2_MULTIPLE_CASCADE |
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL2,
+				 MV88E6XXX_G1_CTL2_MULTIPLE_CASCADE |
 				 (ds->index & 0x1f));
 	if (err)
 		return err;
 
 	/* Configure the IP ToS mapping registers. */
-	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_0, 0x0000);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_IP_PRI_0, 0x0000);
 	if (err)
 		return err;
-	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_1, 0x0000);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_IP_PRI_1, 0x0000);
 	if (err)
 		return err;
-	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_2, 0x5555);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_IP_PRI_2, 0x5555);
 	if (err)
 		return err;
-	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_3, 0x5555);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_IP_PRI_3, 0x5555);
 	if (err)
 		return err;
-	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_4, 0xaaaa);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_IP_PRI_4, 0xaaaa);
 	if (err)
 		return err;
-	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_5, 0xaaaa);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_IP_PRI_5, 0xaaaa);
 	if (err)
 		return err;
-	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_6, 0xffff);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_IP_PRI_6, 0xffff);
 	if (err)
 		return err;
-	err = mv88e6xxx_g1_write(chip, GLOBAL_IP_PRI_7, 0xffff);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_IP_PRI_7, 0xffff);
 	if (err)
 		return err;
 
 	/* Configure the IEEE 802.1p priority mapping register. */
-	err = mv88e6xxx_g1_write(chip, GLOBAL_IEEE_PRI, 0xfa41);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_IEEE_PRI, 0xfa41);
 	if (err)
 		return err;
 
@@ -2318,8 +2078,9 @@ static int mv88e6xxx_g1_setup(struct mv88e6xxx_chip *chip)
 		return err;
 
 	/* Clear the statistics counters for all ports */
-	err = mv88e6xxx_g1_write(chip, GLOBAL_STATS_OP,
-				 GLOBAL_STATS_OP_FLUSH_ALL);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_STATS_OP,
+				 MV88E6XXX_G1_STATS_OP_BUSY |
+				 MV88E6XXX_G1_STATS_OP_FLUSH_ALL);
 	if (err)
 		return err;
 
@@ -2361,6 +2122,14 @@ static int mv88e6xxx_setup(struct dsa_switch *ds)
 			goto unlock;
 	}
 
+	err = mv88e6xxx_irl_setup(chip);
+	if (err)
+		goto unlock;
+
+	err = mv88e6xxx_phy_setup(chip);
+	if (err)
+		goto unlock;
+
 	err = mv88e6xxx_vtu_setup(chip);
 	if (err)
 		goto unlock;
@@ -2424,7 +2193,7 @@ static int mv88e6xxx_mdio_read(struct mii_bus *bus, int phy, int reg)
 		 * the mv88e6390 family model number instead.
 		 */
 		if (!(val & 0x3f0))
-			val |= PORT_SWITCH_ID_PROD_NUM_6390;
+			val |= MV88E6XXX_PORT_SWITCH_ID_PROD_6390 >> 4;
 	}
 
 	return err ? err : val;
@@ -2594,9 +2363,10 @@ static int mv88e6xxx_set_eeprom(struct dsa_switch *ds,
 
 static const struct mv88e6xxx_ops mv88e6085_ops = {
 	/* MV88E6XXX_FAMILY_6097 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.set_switch_mac = mv88e6xxx_g1_set_switch_mac,
-	.phy_read = mv88e6xxx_phy_ppu_read,
-	.phy_write = mv88e6xxx_phy_ppu_write,
+	.phy_read = mv88e6185_phy_ppu_read,
+	.phy_write = mv88e6185_phy_ppu_write,
 	.port_set_link = mv88e6xxx_port_set_link,
 	.port_set_duplex = mv88e6xxx_port_set_duplex,
 	.port_set_speed = mv88e6185_port_set_speed,
@@ -2605,15 +2375,15 @@ static const struct mv88e6xxx_ops mv88e6085_ops = {
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6xxx_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.ppu_enable = mv88e6185_g1_ppu_enable,
@@ -2626,8 +2396,8 @@ static const struct mv88e6xxx_ops mv88e6085_ops = {
 static const struct mv88e6xxx_ops mv88e6095_ops = {
 	/* MV88E6XXX_FAMILY_6095 */
 	.set_switch_mac = mv88e6xxx_g1_set_switch_mac,
-	.phy_read = mv88e6xxx_phy_ppu_read,
-	.phy_write = mv88e6xxx_phy_ppu_write,
+	.phy_read = mv88e6185_phy_ppu_read,
+	.phy_write = mv88e6185_phy_ppu_write,
 	.port_set_link = mv88e6xxx_port_set_link,
 	.port_set_duplex = mv88e6xxx_port_set_duplex,
 	.port_set_speed = mv88e6185_port_set_speed,
@@ -2648,6 +2418,7 @@ static const struct mv88e6xxx_ops mv88e6095_ops = {
 
 static const struct mv88e6xxx_ops mv88e6097_ops = {
 	/* MV88E6XXX_FAMILY_6097 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
 	.phy_read = mv88e6xxx_g2_smi_phy_read,
 	.phy_write = mv88e6xxx_g2_smi_phy_write,
@@ -2658,17 +2429,17 @@ static const struct mv88e6xxx_ops mv88e6097_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6095_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6xxx_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
@@ -2678,9 +2449,10 @@ static const struct mv88e6xxx_ops mv88e6097_ops = {
 
 static const struct mv88e6xxx_ops mv88e6123_ops = {
 	/* MV88E6XXX_FAMILY_6165 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
-	.phy_read = mv88e6165_phy_read,
-	.phy_write = mv88e6165_phy_write,
+	.phy_read = mv88e6xxx_g2_smi_phy_read,
+	.phy_write = mv88e6xxx_g2_smi_phy_write,
 	.port_set_link = mv88e6xxx_port_set_link,
 	.port_set_duplex = mv88e6xxx_port_set_duplex,
 	.port_set_speed = mv88e6185_port_set_speed,
@@ -2688,12 +2460,12 @@ static const struct mv88e6xxx_ops mv88e6123_ops = {
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
-	.stats_snapshot = mv88e6xxx_g1_stats_snapshot,
+	.stats_snapshot = mv88e6320_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
@@ -2704,8 +2476,8 @@ static const struct mv88e6xxx_ops mv88e6123_ops = {
 static const struct mv88e6xxx_ops mv88e6131_ops = {
 	/* MV88E6XXX_FAMILY_6185 */
 	.set_switch_mac = mv88e6xxx_g1_set_switch_mac,
-	.phy_read = mv88e6xxx_phy_ppu_read,
-	.phy_write = mv88e6xxx_phy_ppu_write,
+	.phy_read = mv88e6185_phy_ppu_read,
+	.phy_write = mv88e6185_phy_ppu_write,
 	.port_set_link = mv88e6xxx_port_set_link,
 	.port_set_duplex = mv88e6xxx_port_set_duplex,
 	.port_set_speed = mv88e6185_port_set_speed,
@@ -2714,15 +2486,15 @@ static const struct mv88e6xxx_ops mv88e6131_ops = {
 	.port_set_egress_floods = mv88e6185_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
 	.port_set_upstream_port = mv88e6095_port_set_upstream_port,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.stats_snapshot = mv88e6xxx_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.ppu_enable = mv88e6185_g1_ppu_enable,
@@ -2734,6 +2506,7 @@ static const struct mv88e6xxx_ops mv88e6131_ops = {
 
 static const struct mv88e6xxx_ops mv88e6141_ops = {
 	/* MV88E6XXX_FAMILY_6341 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom8,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom8,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -2747,17 +2520,17 @@ static const struct mv88e6xxx_ops mv88e6141_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6390_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6320_stats_get_sset_count,
 	.stats_get_strings = mv88e6320_stats_get_strings,
 	.stats_get_stats = mv88e6390_stats_get_stats,
-	.g1_set_cpu_port = mv88e6390_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6390_g1_set_egress_port,
+	.set_cpu_port = mv88e6390_g1_set_cpu_port,
+	.set_egress_port = mv88e6390_g1_set_egress_port,
 	.watchdog_ops = &mv88e6390_watchdog_ops,
 	.mgmt_rsvd2cpu =  mv88e6390_g1_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
@@ -2767,9 +2540,10 @@ static const struct mv88e6xxx_ops mv88e6141_ops = {
 
 static const struct mv88e6xxx_ops mv88e6161_ops = {
 	/* MV88E6XXX_FAMILY_6165 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
-	.phy_read = mv88e6165_phy_read,
-	.phy_write = mv88e6165_phy_write,
+	.phy_read = mv88e6xxx_g2_smi_phy_read,
+	.phy_write = mv88e6xxx_g2_smi_phy_write,
 	.port_set_link = mv88e6xxx_port_set_link,
 	.port_set_duplex = mv88e6xxx_port_set_duplex,
 	.port_set_speed = mv88e6185_port_set_speed,
@@ -2777,17 +2551,17 @@ static const struct mv88e6xxx_ops mv88e6161_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
-	.stats_snapshot = mv88e6xxx_g1_stats_snapshot,
+	.stats_snapshot = mv88e6320_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
@@ -2797,6 +2571,7 @@ static const struct mv88e6xxx_ops mv88e6161_ops = {
 
 static const struct mv88e6xxx_ops mv88e6165_ops = {
 	/* MV88E6XXX_FAMILY_6165 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
 	.phy_read = mv88e6165_phy_read,
 	.phy_write = mv88e6165_phy_write,
@@ -2809,8 +2584,8 @@ static const struct mv88e6xxx_ops mv88e6165_ops = {
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
@@ -2820,6 +2595,7 @@ static const struct mv88e6xxx_ops mv88e6165_ops = {
 
 static const struct mv88e6xxx_ops mv88e6171_ops = {
 	/* MV88E6XXX_FAMILY_6351 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
 	.phy_read = mv88e6xxx_g2_smi_phy_read,
 	.phy_write = mv88e6xxx_g2_smi_phy_write,
@@ -2831,17 +2607,17 @@ static const struct mv88e6xxx_ops mv88e6171_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6320_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
@@ -2851,6 +2627,7 @@ static const struct mv88e6xxx_ops mv88e6171_ops = {
 
 static const struct mv88e6xxx_ops mv88e6172_ops = {
 	/* MV88E6XXX_FAMILY_6352 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom16,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom16,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -2864,26 +2641,28 @@ static const struct mv88e6xxx_ops mv88e6172_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6320_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
 	.vtu_getnext = mv88e6352_g1_vtu_getnext,
 	.vtu_loadpurge = mv88e6352_g1_vtu_loadpurge,
+	.serdes_power = mv88e6352_serdes_power,
 };
 
 static const struct mv88e6xxx_ops mv88e6175_ops = {
 	/* MV88E6XXX_FAMILY_6351 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
 	.phy_read = mv88e6xxx_g2_smi_phy_read,
 	.phy_write = mv88e6xxx_g2_smi_phy_write,
@@ -2895,17 +2674,17 @@ static const struct mv88e6xxx_ops mv88e6175_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6320_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
@@ -2915,6 +2694,7 @@ static const struct mv88e6xxx_ops mv88e6175_ops = {
 
 static const struct mv88e6xxx_ops mv88e6176_ops = {
 	/* MV88E6XXX_FAMILY_6352 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom16,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom16,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -2928,29 +2708,30 @@ static const struct mv88e6xxx_ops mv88e6176_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6320_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
 	.vtu_getnext = mv88e6352_g1_vtu_getnext,
 	.vtu_loadpurge = mv88e6352_g1_vtu_loadpurge,
+	.serdes_power = mv88e6352_serdes_power,
 };
 
 static const struct mv88e6xxx_ops mv88e6185_ops = {
 	/* MV88E6XXX_FAMILY_6185 */
 	.set_switch_mac = mv88e6xxx_g1_set_switch_mac,
-	.phy_read = mv88e6xxx_phy_ppu_read,
-	.phy_write = mv88e6xxx_phy_ppu_write,
+	.phy_read = mv88e6185_phy_ppu_read,
+	.phy_write = mv88e6185_phy_ppu_write,
 	.port_set_link = mv88e6xxx_port_set_link,
 	.port_set_duplex = mv88e6xxx_port_set_duplex,
 	.port_set_speed = mv88e6185_port_set_speed,
@@ -2962,8 +2743,8 @@ static const struct mv88e6xxx_ops mv88e6185_ops = {
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.ppu_enable = mv88e6185_g1_ppu_enable,
@@ -2975,6 +2756,7 @@ static const struct mv88e6xxx_ops mv88e6185_ops = {
 
 static const struct mv88e6xxx_ops mv88e6190_ops = {
 	/* MV88E6XXX_FAMILY_6390 */
+	.irl_init_all = mv88e6390_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom8,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom8,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -2988,7 +2770,7 @@ static const struct mv88e6xxx_ops mv88e6190_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_pause_config = mv88e6390_port_pause_config,
+	.port_pause_limit = mv88e6390_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6390_g1_stats_snapshot,
@@ -2996,17 +2778,19 @@ static const struct mv88e6xxx_ops mv88e6190_ops = {
 	.stats_get_sset_count = mv88e6320_stats_get_sset_count,
 	.stats_get_strings = mv88e6320_stats_get_strings,
 	.stats_get_stats = mv88e6390_stats_get_stats,
-	.g1_set_cpu_port = mv88e6390_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6390_g1_set_egress_port,
+	.set_cpu_port = mv88e6390_g1_set_cpu_port,
+	.set_egress_port = mv88e6390_g1_set_egress_port,
 	.watchdog_ops = &mv88e6390_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6390_g1_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
 	.vtu_getnext = mv88e6390_g1_vtu_getnext,
 	.vtu_loadpurge = mv88e6390_g1_vtu_loadpurge,
+	.serdes_power = mv88e6390_serdes_power,
 };
 
 static const struct mv88e6xxx_ops mv88e6190x_ops = {
 	/* MV88E6XXX_FAMILY_6390 */
+	.irl_init_all = mv88e6390_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom8,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom8,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -3020,7 +2804,7 @@ static const struct mv88e6xxx_ops mv88e6190x_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_pause_config = mv88e6390_port_pause_config,
+	.port_pause_limit = mv88e6390_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6390_g1_stats_snapshot,
@@ -3028,17 +2812,19 @@ static const struct mv88e6xxx_ops mv88e6190x_ops = {
 	.stats_get_sset_count = mv88e6320_stats_get_sset_count,
 	.stats_get_strings = mv88e6320_stats_get_strings,
 	.stats_get_stats = mv88e6390_stats_get_stats,
-	.g1_set_cpu_port = mv88e6390_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6390_g1_set_egress_port,
+	.set_cpu_port = mv88e6390_g1_set_cpu_port,
+	.set_egress_port = mv88e6390_g1_set_egress_port,
 	.watchdog_ops = &mv88e6390_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6390_g1_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
 	.vtu_getnext = mv88e6390_g1_vtu_getnext,
 	.vtu_loadpurge = mv88e6390_g1_vtu_loadpurge,
+	.serdes_power = mv88e6390_serdes_power,
 };
 
 static const struct mv88e6xxx_ops mv88e6191_ops = {
 	/* MV88E6XXX_FAMILY_6390 */
+	.irl_init_all = mv88e6390_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom8,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom8,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -3052,7 +2838,7 @@ static const struct mv88e6xxx_ops mv88e6191_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_pause_config = mv88e6390_port_pause_config,
+	.port_pause_limit = mv88e6390_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6390_g1_stats_snapshot,
@@ -3060,17 +2846,19 @@ static const struct mv88e6xxx_ops mv88e6191_ops = {
 	.stats_get_sset_count = mv88e6320_stats_get_sset_count,
 	.stats_get_strings = mv88e6320_stats_get_strings,
 	.stats_get_stats = mv88e6390_stats_get_stats,
-	.g1_set_cpu_port = mv88e6390_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6390_g1_set_egress_port,
+	.set_cpu_port = mv88e6390_g1_set_cpu_port,
+	.set_egress_port = mv88e6390_g1_set_egress_port,
 	.watchdog_ops = &mv88e6390_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6390_g1_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
 	.vtu_getnext = mv88e6390_g1_vtu_getnext,
 	.vtu_loadpurge = mv88e6390_g1_vtu_loadpurge,
+	.serdes_power = mv88e6390_serdes_power,
 };
 
 static const struct mv88e6xxx_ops mv88e6240_ops = {
 	/* MV88E6XXX_FAMILY_6352 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom16,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom16,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -3084,26 +2872,28 @@ static const struct mv88e6xxx_ops mv88e6240_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6320_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
 	.vtu_getnext = mv88e6352_g1_vtu_getnext,
 	.vtu_loadpurge = mv88e6352_g1_vtu_loadpurge,
+	.serdes_power = mv88e6352_serdes_power,
 };
 
 static const struct mv88e6xxx_ops mv88e6290_ops = {
 	/* MV88E6XXX_FAMILY_6390 */
+	.irl_init_all = mv88e6390_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom8,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom8,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -3117,7 +2907,7 @@ static const struct mv88e6xxx_ops mv88e6290_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_pause_config = mv88e6390_port_pause_config,
+	.port_pause_limit = mv88e6390_port_pause_limit,
 	.port_set_cmode = mv88e6390x_port_set_cmode,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
@@ -3126,17 +2916,19 @@ static const struct mv88e6xxx_ops mv88e6290_ops = {
 	.stats_get_sset_count = mv88e6320_stats_get_sset_count,
 	.stats_get_strings = mv88e6320_stats_get_strings,
 	.stats_get_stats = mv88e6390_stats_get_stats,
-	.g1_set_cpu_port = mv88e6390_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6390_g1_set_egress_port,
+	.set_cpu_port = mv88e6390_g1_set_cpu_port,
+	.set_egress_port = mv88e6390_g1_set_egress_port,
 	.watchdog_ops = &mv88e6390_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6390_g1_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
 	.vtu_getnext = mv88e6390_g1_vtu_getnext,
 	.vtu_loadpurge = mv88e6390_g1_vtu_loadpurge,
+	.serdes_power = mv88e6390_serdes_power,
 };
 
 static const struct mv88e6xxx_ops mv88e6320_ops = {
 	/* MV88E6XXX_FAMILY_6320 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom16,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom16,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -3149,17 +2941,17 @@ static const struct mv88e6xxx_ops mv88e6320_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6320_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6320_stats_get_sset_count,
 	.stats_get_strings = mv88e6320_stats_get_strings,
 	.stats_get_stats = mv88e6320_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
 	.vtu_getnext = mv88e6185_g1_vtu_getnext,
@@ -3168,6 +2960,7 @@ static const struct mv88e6xxx_ops mv88e6320_ops = {
 
 static const struct mv88e6xxx_ops mv88e6321_ops = {
 	/* MV88E6XXX_FAMILY_6321 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom16,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom16,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -3180,17 +2973,17 @@ static const struct mv88e6xxx_ops mv88e6321_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6320_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6320_stats_get_sset_count,
 	.stats_get_strings = mv88e6320_stats_get_strings,
 	.stats_get_stats = mv88e6320_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.reset = mv88e6352_g1_reset,
 	.vtu_getnext = mv88e6185_g1_vtu_getnext,
 	.vtu_loadpurge = mv88e6185_g1_vtu_loadpurge,
@@ -3198,6 +2991,7 @@ static const struct mv88e6xxx_ops mv88e6321_ops = {
 
 static const struct mv88e6xxx_ops mv88e6341_ops = {
 	/* MV88E6XXX_FAMILY_6341 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom8,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom8,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -3211,17 +3005,17 @@ static const struct mv88e6xxx_ops mv88e6341_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6390_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6320_stats_get_sset_count,
 	.stats_get_strings = mv88e6320_stats_get_strings,
 	.stats_get_stats = mv88e6390_stats_get_stats,
-	.g1_set_cpu_port = mv88e6390_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6390_g1_set_egress_port,
+	.set_cpu_port = mv88e6390_g1_set_cpu_port,
+	.set_egress_port = mv88e6390_g1_set_egress_port,
 	.watchdog_ops = &mv88e6390_watchdog_ops,
 	.mgmt_rsvd2cpu =  mv88e6390_g1_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
@@ -3231,6 +3025,7 @@ static const struct mv88e6xxx_ops mv88e6341_ops = {
 
 static const struct mv88e6xxx_ops mv88e6350_ops = {
 	/* MV88E6XXX_FAMILY_6351 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
 	.phy_read = mv88e6xxx_g2_smi_phy_read,
 	.phy_write = mv88e6xxx_g2_smi_phy_write,
@@ -3242,17 +3037,17 @@ static const struct mv88e6xxx_ops mv88e6350_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6320_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
@@ -3262,6 +3057,7 @@ static const struct mv88e6xxx_ops mv88e6350_ops = {
 
 static const struct mv88e6xxx_ops mv88e6351_ops = {
 	/* MV88E6XXX_FAMILY_6351 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
 	.phy_read = mv88e6xxx_g2_smi_phy_read,
 	.phy_write = mv88e6xxx_g2_smi_phy_write,
@@ -3273,17 +3069,17 @@ static const struct mv88e6xxx_ops mv88e6351_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6320_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
@@ -3293,6 +3089,7 @@ static const struct mv88e6xxx_ops mv88e6351_ops = {
 
 static const struct mv88e6xxx_ops mv88e6352_ops = {
 	/* MV88E6XXX_FAMILY_6352 */
+	.irl_init_all = mv88e6352_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom16,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom16,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -3306,26 +3103,28 @@ static const struct mv88e6xxx_ops mv88e6352_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6097_port_pause_config,
+	.port_pause_limit = mv88e6097_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6320_g1_stats_snapshot,
 	.stats_get_sset_count = mv88e6095_stats_get_sset_count,
 	.stats_get_strings = mv88e6095_stats_get_strings,
 	.stats_get_stats = mv88e6095_stats_get_stats,
-	.g1_set_cpu_port = mv88e6095_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6095_g1_set_egress_port,
+	.set_cpu_port = mv88e6095_g1_set_cpu_port,
+	.set_egress_port = mv88e6095_g1_set_egress_port,
 	.watchdog_ops = &mv88e6097_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6095_g2_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
 	.vtu_getnext = mv88e6352_g1_vtu_getnext,
 	.vtu_loadpurge = mv88e6352_g1_vtu_loadpurge,
+	.serdes_power = mv88e6352_serdes_power,
 };
 
 static const struct mv88e6xxx_ops mv88e6390_ops = {
 	/* MV88E6XXX_FAMILY_6390 */
+	.irl_init_all = mv88e6390_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom8,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom8,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -3339,9 +3138,9 @@ static const struct mv88e6xxx_ops mv88e6390_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6390_port_pause_config,
+	.port_pause_limit = mv88e6390_port_pause_limit,
 	.port_set_cmode = mv88e6390x_port_set_cmode,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
@@ -3350,17 +3149,19 @@ static const struct mv88e6xxx_ops mv88e6390_ops = {
 	.stats_get_sset_count = mv88e6320_stats_get_sset_count,
 	.stats_get_strings = mv88e6320_stats_get_strings,
 	.stats_get_stats = mv88e6390_stats_get_stats,
-	.g1_set_cpu_port = mv88e6390_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6390_g1_set_egress_port,
+	.set_cpu_port = mv88e6390_g1_set_cpu_port,
+	.set_egress_port = mv88e6390_g1_set_egress_port,
 	.watchdog_ops = &mv88e6390_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6390_g1_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
 	.vtu_getnext = mv88e6390_g1_vtu_getnext,
 	.vtu_loadpurge = mv88e6390_g1_vtu_loadpurge,
+	.serdes_power = mv88e6390_serdes_power,
 };
 
 static const struct mv88e6xxx_ops mv88e6390x_ops = {
 	/* MV88E6XXX_FAMILY_6390 */
+	.irl_init_all = mv88e6390_g2_irl_init_all,
 	.get_eeprom = mv88e6xxx_g2_get_eeprom8,
 	.set_eeprom = mv88e6xxx_g2_set_eeprom8,
 	.set_switch_mac = mv88e6xxx_g2_set_switch_mac,
@@ -3374,9 +3175,9 @@ static const struct mv88e6xxx_ops mv88e6390x_ops = {
 	.port_set_frame_mode = mv88e6351_port_set_frame_mode,
 	.port_set_egress_floods = mv88e6352_port_set_egress_floods,
 	.port_set_ether_type = mv88e6351_port_set_ether_type,
-	.port_jumbo_config = mv88e6165_port_jumbo_config,
+	.port_set_jumbo_size = mv88e6165_port_set_jumbo_size,
 	.port_egress_rate_limiting = mv88e6097_port_egress_rate_limiting,
-	.port_pause_config = mv88e6390_port_pause_config,
+	.port_pause_limit = mv88e6390_port_pause_limit,
 	.port_disable_learn_limit = mv88e6xxx_port_disable_learn_limit,
 	.port_disable_pri_override = mv88e6xxx_port_disable_pri_override,
 	.stats_snapshot = mv88e6390_g1_stats_snapshot,
@@ -3384,18 +3185,19 @@ static const struct mv88e6xxx_ops mv88e6390x_ops = {
 	.stats_get_sset_count = mv88e6320_stats_get_sset_count,
 	.stats_get_strings = mv88e6320_stats_get_strings,
 	.stats_get_stats = mv88e6390_stats_get_stats,
-	.g1_set_cpu_port = mv88e6390_g1_set_cpu_port,
-	.g1_set_egress_port = mv88e6390_g1_set_egress_port,
+	.set_cpu_port = mv88e6390_g1_set_cpu_port,
+	.set_egress_port = mv88e6390_g1_set_egress_port,
 	.watchdog_ops = &mv88e6390_watchdog_ops,
 	.mgmt_rsvd2cpu = mv88e6390_g1_mgmt_rsvd2cpu,
 	.reset = mv88e6352_g1_reset,
 	.vtu_getnext = mv88e6390_g1_vtu_getnext,
 	.vtu_loadpurge = mv88e6390_g1_vtu_loadpurge,
+	.serdes_power = mv88e6390_serdes_power,
 };
 
 static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	[MV88E6085] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6085,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6085,
 		.family = MV88E6XXX_FAMILY_6097,
 		.name = "Marvell 88E6085",
 		.num_databases = 4096,
@@ -3413,7 +3215,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6095] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6095,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6095,
 		.family = MV88E6XXX_FAMILY_6095,
 		.name = "Marvell 88E6095/88E6095F",
 		.num_databases = 256,
@@ -3430,7 +3232,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6097] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6097,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6097,
 		.family = MV88E6XXX_FAMILY_6097,
 		.name = "Marvell 88E6097/88E6097F",
 		.num_databases = 4096,
@@ -3448,7 +3250,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6123] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6123,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6123,
 		.family = MV88E6XXX_FAMILY_6165,
 		.name = "Marvell 88E6123",
 		.num_databases = 4096,
@@ -3460,13 +3262,13 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.g1_irqs = 9,
 		.atu_move_port_mask = 0xf,
 		.pvt = true,
-		.tag_protocol = DSA_TAG_PROTO_DSA,
+		.tag_protocol = DSA_TAG_PROTO_EDSA,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6165,
 		.ops = &mv88e6123_ops,
 	},
 
 	[MV88E6131] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6131,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6131,
 		.family = MV88E6XXX_FAMILY_6185,
 		.name = "Marvell 88E6131",
 		.num_databases = 256,
@@ -3483,7 +3285,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6141] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6141,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6141,
 		.family = MV88E6XXX_FAMILY_6341,
 		.name = "Marvell 88E6341",
 		.num_databases = 4096,
@@ -3500,7 +3302,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6161] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6161,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6161,
 		.family = MV88E6XXX_FAMILY_6165,
 		.name = "Marvell 88E6161",
 		.num_databases = 4096,
@@ -3512,13 +3314,13 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.g1_irqs = 9,
 		.atu_move_port_mask = 0xf,
 		.pvt = true,
-		.tag_protocol = DSA_TAG_PROTO_DSA,
+		.tag_protocol = DSA_TAG_PROTO_EDSA,
 		.flags = MV88E6XXX_FLAGS_FAMILY_6165,
 		.ops = &mv88e6161_ops,
 	},
 
 	[MV88E6165] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6165,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6165,
 		.family = MV88E6XXX_FAMILY_6165,
 		.name = "Marvell 88E6165",
 		.num_databases = 4096,
@@ -3536,7 +3338,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6171] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6171,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6171,
 		.family = MV88E6XXX_FAMILY_6351,
 		.name = "Marvell 88E6171",
 		.num_databases = 4096,
@@ -3554,7 +3356,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6172] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6172,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6172,
 		.family = MV88E6XXX_FAMILY_6352,
 		.name = "Marvell 88E6172",
 		.num_databases = 4096,
@@ -3572,7 +3374,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6175] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6175,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6175,
 		.family = MV88E6XXX_FAMILY_6351,
 		.name = "Marvell 88E6175",
 		.num_databases = 4096,
@@ -3590,7 +3392,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6176] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6176,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6176,
 		.family = MV88E6XXX_FAMILY_6352,
 		.name = "Marvell 88E6176",
 		.num_databases = 4096,
@@ -3608,7 +3410,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6185] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6185,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6185,
 		.family = MV88E6XXX_FAMILY_6185,
 		.name = "Marvell 88E6185",
 		.num_databases = 256,
@@ -3625,7 +3427,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6190] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6190,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6190,
 		.family = MV88E6XXX_FAMILY_6390,
 		.name = "Marvell 88E6190",
 		.num_databases = 4096,
@@ -3643,7 +3445,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6190X] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6190X,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6190X,
 		.family = MV88E6XXX_FAMILY_6390,
 		.name = "Marvell 88E6190X",
 		.num_databases = 4096,
@@ -3661,7 +3463,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6191] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6191,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6191,
 		.family = MV88E6XXX_FAMILY_6390,
 		.name = "Marvell 88E6191",
 		.num_databases = 4096,
@@ -3679,7 +3481,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6240] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6240,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6240,
 		.family = MV88E6XXX_FAMILY_6352,
 		.name = "Marvell 88E6240",
 		.num_databases = 4096,
@@ -3697,7 +3499,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6290] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6290,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6290,
 		.family = MV88E6XXX_FAMILY_6390,
 		.name = "Marvell 88E6290",
 		.num_databases = 4096,
@@ -3715,7 +3517,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6320] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6320,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6320,
 		.family = MV88E6XXX_FAMILY_6320,
 		.name = "Marvell 88E6320",
 		.num_databases = 4096,
@@ -3733,7 +3535,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6321] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6321,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6321,
 		.family = MV88E6XXX_FAMILY_6320,
 		.name = "Marvell 88E6321",
 		.num_databases = 4096,
@@ -3750,7 +3552,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6341] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6341,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6341,
 		.family = MV88E6XXX_FAMILY_6341,
 		.name = "Marvell 88E6341",
 		.num_databases = 4096,
@@ -3767,7 +3569,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6350] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6350,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6350,
 		.family = MV88E6XXX_FAMILY_6351,
 		.name = "Marvell 88E6350",
 		.num_databases = 4096,
@@ -3785,7 +3587,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6351] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6351,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6351,
 		.family = MV88E6XXX_FAMILY_6351,
 		.name = "Marvell 88E6351",
 		.num_databases = 4096,
@@ -3803,7 +3605,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 	},
 
 	[MV88E6352] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6352,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6352,
 		.family = MV88E6XXX_FAMILY_6352,
 		.name = "Marvell 88E6352",
 		.num_databases = 4096,
@@ -3820,7 +3622,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.ops = &mv88e6352_ops,
 	},
 	[MV88E6390] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6390,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6390,
 		.family = MV88E6XXX_FAMILY_6390,
 		.name = "Marvell 88E6390",
 		.num_databases = 4096,
@@ -3837,7 +3639,7 @@ static const struct mv88e6xxx_info mv88e6xxx_table[] = {
 		.ops = &mv88e6390_ops,
 	},
 	[MV88E6390X] = {
-		.prod_num = PORT_SWITCH_ID_PROD_NUM_6390X,
+		.prod_num = MV88E6XXX_PORT_SWITCH_ID_PROD_6390X,
 		.family = MV88E6XXX_FAMILY_6390,
 		.name = "Marvell 88E6390X",
 		.num_databases = 4096,
@@ -3874,13 +3676,13 @@ static int mv88e6xxx_detect(struct mv88e6xxx_chip *chip)
 	int err;
 
 	mutex_lock(&chip->reg_lock);
-	err = mv88e6xxx_port_read(chip, 0, PORT_SWITCH_ID, &id);
+	err = mv88e6xxx_port_read(chip, 0, MV88E6XXX_PORT_SWITCH_ID, &id);
 	mutex_unlock(&chip->reg_lock);
 	if (err)
 		return err;
 
-	prod_num = (id & 0xfff0) >> 4;
-	rev = id & 0x000f;
+	prod_num = id & MV88E6XXX_PORT_SWITCH_ID_PROD_MASK;
+	rev = id & MV88E6XXX_PORT_SWITCH_ID_REV_MASK;
 
 	info = mv88e6xxx_lookup_info(prod_num);
 	if (!info)
@@ -3915,18 +3717,6 @@ static struct mv88e6xxx_chip *mv88e6xxx_alloc_chip(struct device *dev)
 	return chip;
 }
 
-static void mv88e6xxx_phy_init(struct mv88e6xxx_chip *chip)
-{
-	if (chip->info->ops->ppu_enable && chip->info->ops->ppu_disable)
-		mv88e6xxx_ppu_state_init(chip);
-}
-
-static void mv88e6xxx_phy_destroy(struct mv88e6xxx_chip *chip)
-{
-	if (chip->info->ops->ppu_enable && chip->info->ops->ppu_disable)
-		mv88e6xxx_ppu_state_destroy(chip);
-}
-
 static int mv88e6xxx_smi_init(struct mv88e6xxx_chip *chip,
 			      struct mii_bus *bus, int sw_addr)
 {
@@ -4017,8 +3807,9 @@ static void mv88e6xxx_port_mdb_add(struct dsa_switch *ds, int port,
 
 	mutex_lock(&chip->reg_lock);
 	if (mv88e6xxx_port_db_load_purge(chip, port, mdb->addr, mdb->vid,
-					 GLOBAL_ATU_DATA_STATE_MC_STATIC))
-		netdev_err(ds->ports[port].netdev, "failed to load multicast MAC address\n");
+					 MV88E6XXX_G1_ATU_DATA_STATE_MC_STATIC))
+		dev_err(ds->dev, "p%d: failed to load multicast MAC address\n",
+			port);
 	mutex_unlock(&chip->reg_lock);
 }
 
@@ -4030,7 +3821,7 @@ static int mv88e6xxx_port_mdb_del(struct dsa_switch *ds, int port,
 
 	mutex_lock(&chip->reg_lock);
 	err = mv88e6xxx_port_db_load_purge(chip, port, mdb->addr, mdb->vid,
-					   GLOBAL_ATU_DATA_STATE_UNUSED);
+					   MV88E6XXX_G1_ATU_DATA_STATE_UNUSED);
 	mutex_unlock(&chip->reg_lock);
 
 	return err;
@@ -4038,7 +3829,7 @@ static int mv88e6xxx_port_mdb_del(struct dsa_switch *ds, int port,
 
 static int mv88e6xxx_port_mdb_dump(struct dsa_switch *ds, int port,
 				   struct switchdev_obj_port_mdb *mdb,
-				   int (*cb)(struct switchdev_obj *obj))
+				   switchdev_obj_dump_cb_t *cb)
 {
 	struct mv88e6xxx_chip *chip = ds->priv;
 	int err;
@@ -4059,6 +3850,8 @@ static const struct dsa_switch_ops mv88e6xxx_switch_ops = {
 	.get_strings		= mv88e6xxx_get_strings,
 	.get_ethtool_stats	= mv88e6xxx_get_ethtool_stats,
 	.get_sset_count		= mv88e6xxx_get_sset_count,
+	.port_enable		= mv88e6xxx_port_enable,
+	.port_disable		= mv88e6xxx_port_disable,
 	.set_eee		= mv88e6xxx_set_eee,
 	.get_eee		= mv88e6xxx_get_eee,
 	.get_eeprom_len		= mv88e6xxx_get_eeprom_len,
@@ -4108,7 +3901,7 @@ static int mv88e6xxx_register_switch(struct mv88e6xxx_chip *chip)
 
 	dev_set_drvdata(dev, ds);
 
-	return dsa_register_switch(ds, dev);
+	return dsa_register_switch(ds);
 }
 
 static void mv88e6xxx_unregister_switch(struct mv88e6xxx_chip *chip)
diff --git a/drivers/net/dsa/mv88e6xxx/chip.h b/drivers/net/dsa/mv88e6xxx/chip.h
new file mode 100644
index 000000000000..086444016352
--- /dev/null
+++ b/drivers/net/dsa/mv88e6xxx/chip.h
@@ -0,0 +1,518 @@
+/*
+ * Marvell 88E6xxx Ethernet switch single-chip definition
+ *
+ * Copyright (c) 2008 Marvell Semiconductor
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef _MV88E6XXX_CHIP_H
+#define _MV88E6XXX_CHIP_H
+
+#include <linux/if_vlan.h>
+#include <linux/irq.h>
+#include <linux/gpio/consumer.h>
+#include <linux/phy.h>
+#include <net/dsa.h>
+
+#ifndef UINT64_MAX
+#define UINT64_MAX		(u64)(~((u64)0))
+#endif
+
+#define SMI_CMD			0x00
+#define SMI_CMD_BUSY		BIT(15)
+#define SMI_CMD_CLAUSE_22	BIT(12)
+#define SMI_CMD_OP_22_WRITE	((1 << 10) | SMI_CMD_BUSY | SMI_CMD_CLAUSE_22)
+#define SMI_CMD_OP_22_READ	((2 << 10) | SMI_CMD_BUSY | SMI_CMD_CLAUSE_22)
+#define SMI_CMD_OP_45_WRITE_ADDR	((0 << 10) | SMI_CMD_BUSY)
+#define SMI_CMD_OP_45_WRITE_DATA	((1 << 10) | SMI_CMD_BUSY)
+#define SMI_CMD_OP_45_READ_DATA		((2 << 10) | SMI_CMD_BUSY)
+#define SMI_CMD_OP_45_READ_DATA_INC	((3 << 10) | SMI_CMD_BUSY)
+#define SMI_DATA		0x01
+
+#define MV88E6XXX_N_FID		4096
+
+/* PVT limits for 4-bit port and 5-bit switch */
+#define MV88E6XXX_MAX_PVT_SWITCHES	32
+#define MV88E6XXX_MAX_PVT_PORTS		16
+
+enum mv88e6xxx_egress_mode {
+	MV88E6XXX_EGRESS_MODE_UNMODIFIED,
+	MV88E6XXX_EGRESS_MODE_UNTAGGED,
+	MV88E6XXX_EGRESS_MODE_TAGGED,
+	MV88E6XXX_EGRESS_MODE_ETHERTYPE,
+};
+
+enum mv88e6xxx_frame_mode {
+	MV88E6XXX_FRAME_MODE_NORMAL,
+	MV88E6XXX_FRAME_MODE_DSA,
+	MV88E6XXX_FRAME_MODE_PROVIDER,
+	MV88E6XXX_FRAME_MODE_ETHERTYPE,
+};
+
+/* List of supported models */
+enum mv88e6xxx_model {
+	MV88E6085,
+	MV88E6095,
+	MV88E6097,
+	MV88E6123,
+	MV88E6131,
+	MV88E6141,
+	MV88E6161,
+	MV88E6165,
+	MV88E6171,
+	MV88E6172,
+	MV88E6175,
+	MV88E6176,
+	MV88E6185,
+	MV88E6190,
+	MV88E6190X,
+	MV88E6191,
+	MV88E6240,
+	MV88E6290,
+	MV88E6320,
+	MV88E6321,
+	MV88E6341,
+	MV88E6350,
+	MV88E6351,
+	MV88E6352,
+	MV88E6390,
+	MV88E6390X,
+};
+
+enum mv88e6xxx_family {
+	MV88E6XXX_FAMILY_NONE,
+	MV88E6XXX_FAMILY_6065,	/* 6031 6035 6061 6065 */
+	MV88E6XXX_FAMILY_6095,	/* 6092 6095 */
+	MV88E6XXX_FAMILY_6097,	/* 6046 6085 6096 6097 */
+	MV88E6XXX_FAMILY_6165,	/* 6123 6161 6165 */
+	MV88E6XXX_FAMILY_6185,	/* 6108 6121 6122 6131 6152 6155 6182 6185 */
+	MV88E6XXX_FAMILY_6320,	/* 6320 6321 */
+	MV88E6XXX_FAMILY_6341,	/* 6141 6341 */
+	MV88E6XXX_FAMILY_6351,	/* 6171 6175 6350 6351 */
+	MV88E6XXX_FAMILY_6352,	/* 6172 6176 6240 6352 */
+	MV88E6XXX_FAMILY_6390,  /* 6190 6190X 6191 6290 6390 6390X */
+};
+
+enum mv88e6xxx_cap {
+	/* Energy Efficient Ethernet.
+	 */
+	MV88E6XXX_CAP_EEE,
+
+	/* Multi-chip Addressing Mode.
+	 * Some chips respond to only 2 registers of its own SMI device address
+	 * when it is non-zero, and use indirect access to internal registers.
+	 */
+	MV88E6XXX_CAP_SMI_CMD,		/* (0x00) SMI Command */
+	MV88E6XXX_CAP_SMI_DATA,		/* (0x01) SMI Data */
+
+	/* Switch Global (1) Registers.
+	 */
+	MV88E6XXX_CAP_G1_ATU_FID,	/* (0x01) ATU FID Register */
+	MV88E6XXX_CAP_G1_VTU_FID,	/* (0x02) VTU FID Register */
+
+	/* Switch Global 2 Registers.
+	 * The device contains a second set of global 16-bit registers.
+	 */
+	MV88E6XXX_CAP_GLOBAL2,
+	MV88E6XXX_CAP_G2_INT,		/* (0x00) Interrupt Status */
+	MV88E6XXX_CAP_G2_MGMT_EN_2X,	/* (0x02) MGMT Enable Register 2x */
+	MV88E6XXX_CAP_G2_MGMT_EN_0X,	/* (0x03) MGMT Enable Register 0x */
+	MV88E6XXX_CAP_G2_POT,		/* (0x0f) Priority Override Table */
+
+	/* Per VLAN Spanning Tree Unit (STU).
+	 * The Port State database, if present, is accessed through VTU
+	 * operations and dedicated SID registers. See MV88E6352_G1_VTU_SID.
+	 */
+	MV88E6XXX_CAP_STU,
+
+	/* VLAN Table Unit.
+	 * The VTU is used to program 802.1Q VLANs. See MV88E6XXX_G1_VTU_OP.
+	 */
+	MV88E6XXX_CAP_VTU,
+};
+
+/* Bitmask of capabilities */
+#define MV88E6XXX_FLAG_EEE		BIT_ULL(MV88E6XXX_CAP_EEE)
+
+#define MV88E6XXX_FLAG_SMI_CMD		BIT_ULL(MV88E6XXX_CAP_SMI_CMD)
+#define MV88E6XXX_FLAG_SMI_DATA		BIT_ULL(MV88E6XXX_CAP_SMI_DATA)
+
+#define MV88E6XXX_FLAG_G1_VTU_FID	BIT_ULL(MV88E6XXX_CAP_G1_VTU_FID)
+
+#define MV88E6XXX_FLAG_GLOBAL2		BIT_ULL(MV88E6XXX_CAP_GLOBAL2)
+#define MV88E6XXX_FLAG_G2_INT		BIT_ULL(MV88E6XXX_CAP_G2_INT)
+#define MV88E6XXX_FLAG_G2_MGMT_EN_2X	BIT_ULL(MV88E6XXX_CAP_G2_MGMT_EN_2X)
+#define MV88E6XXX_FLAG_G2_MGMT_EN_0X	BIT_ULL(MV88E6XXX_CAP_G2_MGMT_EN_0X)
+#define MV88E6XXX_FLAG_G2_POT		BIT_ULL(MV88E6XXX_CAP_G2_POT)
+
+/* Multi-chip Addressing Mode */
+#define MV88E6XXX_FLAGS_MULTI_CHIP	\
+	(MV88E6XXX_FLAG_SMI_CMD |	\
+	 MV88E6XXX_FLAG_SMI_DATA)
+
+#define MV88E6XXX_FLAGS_FAMILY_6095	\
+	(MV88E6XXX_FLAG_GLOBAL2 |	\
+	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
+	 MV88E6XXX_FLAGS_MULTI_CHIP)
+
+#define MV88E6XXX_FLAGS_FAMILY_6097	\
+	(MV88E6XXX_FLAG_G1_VTU_FID |	\
+	 MV88E6XXX_FLAG_GLOBAL2 |	\
+	 MV88E6XXX_FLAG_G2_INT |        \
+	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
+	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
+	 MV88E6XXX_FLAG_G2_POT |	\
+	 MV88E6XXX_FLAGS_MULTI_CHIP)
+
+#define MV88E6XXX_FLAGS_FAMILY_6165	\
+	(MV88E6XXX_FLAG_G1_VTU_FID |	\
+	 MV88E6XXX_FLAG_GLOBAL2 |	\
+	 MV88E6XXX_FLAG_G2_INT |	\
+	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
+	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
+	 MV88E6XXX_FLAG_G2_POT |	\
+	 MV88E6XXX_FLAGS_MULTI_CHIP)
+
+#define MV88E6XXX_FLAGS_FAMILY_6185	\
+	(MV88E6XXX_FLAG_GLOBAL2 |	\
+	 MV88E6XXX_FLAG_G2_INT |	\
+	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
+	 MV88E6XXX_FLAGS_MULTI_CHIP)
+
+#define MV88E6XXX_FLAGS_FAMILY_6320	\
+	(MV88E6XXX_FLAG_EEE |		\
+	 MV88E6XXX_FLAG_GLOBAL2 |	\
+	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
+	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
+	 MV88E6XXX_FLAG_G2_POT |	\
+	 MV88E6XXX_FLAGS_MULTI_CHIP)
+
+#define MV88E6XXX_FLAGS_FAMILY_6341	\
+	(MV88E6XXX_FLAG_EEE |		\
+	 MV88E6XXX_FLAG_G1_VTU_FID |	\
+	 MV88E6XXX_FLAG_GLOBAL2 |	\
+	 MV88E6XXX_FLAG_G2_INT |	\
+	 MV88E6XXX_FLAG_G2_POT |	\
+	 MV88E6XXX_FLAGS_MULTI_CHIP)
+
+#define MV88E6XXX_FLAGS_FAMILY_6351	\
+	(MV88E6XXX_FLAG_G1_VTU_FID |	\
+	 MV88E6XXX_FLAG_GLOBAL2 |	\
+	 MV88E6XXX_FLAG_G2_INT |	\
+	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
+	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
+	 MV88E6XXX_FLAG_G2_POT |	\
+	 MV88E6XXX_FLAGS_MULTI_CHIP)
+
+#define MV88E6XXX_FLAGS_FAMILY_6352	\
+	(MV88E6XXX_FLAG_EEE |		\
+	 MV88E6XXX_FLAG_G1_VTU_FID |	\
+	 MV88E6XXX_FLAG_GLOBAL2 |	\
+	 MV88E6XXX_FLAG_G2_INT |	\
+	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
+	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
+	 MV88E6XXX_FLAG_G2_POT |	\
+	 MV88E6XXX_FLAGS_MULTI_CHIP)
+
+#define MV88E6XXX_FLAGS_FAMILY_6390	\
+	(MV88E6XXX_FLAG_EEE |		\
+	 MV88E6XXX_FLAG_GLOBAL2 |	\
+	 MV88E6XXX_FLAG_G2_INT |        \
+	 MV88E6XXX_FLAGS_MULTI_CHIP)
+
+struct mv88e6xxx_ops;
+
+struct mv88e6xxx_info {
+	enum mv88e6xxx_family family;
+	u16 prod_num;
+	const char *name;
+	unsigned int num_databases;
+	unsigned int num_ports;
+	unsigned int max_vid;
+	unsigned int port_base_addr;
+	unsigned int global1_addr;
+	unsigned int age_time_coeff;
+	unsigned int g1_irqs;
+	bool pvt;
+	enum dsa_tag_protocol tag_protocol;
+	unsigned long long flags;
+
+	/* Mask for FromPort and ToPort value of PortVec used in ATU Move
+	 * operation. 0 means that the ATU Move operation is not supported.
+	 */
+	u8 atu_move_port_mask;
+	const struct mv88e6xxx_ops *ops;
+};
+
+struct mv88e6xxx_atu_entry {
+	u8	state;
+	bool	trunk;
+	u16	portvec;
+	u8	mac[ETH_ALEN];
+};
+
+struct mv88e6xxx_vtu_entry {
+	u16	vid;
+	u16	fid;
+	u8	sid;
+	bool	valid;
+	u8	member[DSA_MAX_PORTS];
+	u8	state[DSA_MAX_PORTS];
+};
+
+struct mv88e6xxx_bus_ops;
+struct mv88e6xxx_irq_ops;
+
+struct mv88e6xxx_irq {
+	u16 masked;
+	struct irq_chip chip;
+	struct irq_domain *domain;
+	unsigned int nirqs;
+};
+
+struct mv88e6xxx_chip {
+	const struct mv88e6xxx_info *info;
+
+	/* The dsa_switch this private structure is related to */
+	struct dsa_switch *ds;
+
+	/* The device this structure is associated to */
+	struct device *dev;
+
+	/* This mutex protects the access to the switch registers */
+	struct mutex reg_lock;
+
+	/* The MII bus and the address on the bus that is used to
+	 * communication with the switch
+	 */
+	const struct mv88e6xxx_bus_ops *smi_ops;
+	struct mii_bus *bus;
+	int sw_addr;
+
+	/* Handles automatic disabling and re-enabling of the PHY
+	 * polling unit.
+	 */
+	const struct mv88e6xxx_bus_ops *phy_ops;
+	struct mutex		ppu_mutex;
+	int			ppu_disabled;
+	struct work_struct	ppu_work;
+	struct timer_list	ppu_timer;
+
+	/* This mutex serialises access to the statistics unit.
+	 * Hold this mutex over snapshot + dump sequences.
+	 */
+	struct mutex	stats_mutex;
+
+	/* A switch may have a GPIO line tied to its reset pin. Parse
+	 * this from the device tree, and use it before performing
+	 * switch soft reset.
+	 */
+	struct gpio_desc *reset;
+
+	/* set to size of eeprom if supported by the switch */
+	int		eeprom_len;
+
+	/* List of mdio busses */
+	struct list_head mdios;
+
+	/* There can be two interrupt controllers, which are chained
+	 * off a GPIO as interrupt source
+	 */
+	struct mv88e6xxx_irq g1_irq;
+	struct mv88e6xxx_irq g2_irq;
+	int irq;
+	int device_irq;
+	int watchdog_irq;
+};
+
+struct mv88e6xxx_bus_ops {
+	int (*read)(struct mv88e6xxx_chip *chip, int addr, int reg, u16 *val);
+	int (*write)(struct mv88e6xxx_chip *chip, int addr, int reg, u16 val);
+};
+
+struct mv88e6xxx_mdio_bus {
+	struct mii_bus *bus;
+	struct mv88e6xxx_chip *chip;
+	struct list_head list;
+	bool external;
+};
+
+struct mv88e6xxx_ops {
+	/* Ingress Rate Limit unit (IRL) operations */
+	int (*irl_init_all)(struct mv88e6xxx_chip *chip, int port);
+
+	int (*get_eeprom)(struct mv88e6xxx_chip *chip,
+			  struct ethtool_eeprom *eeprom, u8 *data);
+	int (*set_eeprom)(struct mv88e6xxx_chip *chip,
+			  struct ethtool_eeprom *eeprom, u8 *data);
+
+	int (*set_switch_mac)(struct mv88e6xxx_chip *chip, u8 *addr);
+
+	int (*phy_read)(struct mv88e6xxx_chip *chip,
+			struct mii_bus *bus,
+			int addr, int reg, u16 *val);
+	int (*phy_write)(struct mv88e6xxx_chip *chip,
+			 struct mii_bus *bus,
+			 int addr, int reg, u16 val);
+
+	/* PHY Polling Unit (PPU) operations */
+	int (*ppu_enable)(struct mv88e6xxx_chip *chip);
+	int (*ppu_disable)(struct mv88e6xxx_chip *chip);
+
+	/* Switch Software Reset */
+	int (*reset)(struct mv88e6xxx_chip *chip);
+
+	/* RGMII Receive/Transmit Timing Control
+	 * Add delay on PHY_INTERFACE_MODE_RGMII_*ID, no delay otherwise.
+	 */
+	int (*port_set_rgmii_delay)(struct mv88e6xxx_chip *chip, int port,
+				    phy_interface_t mode);
+
+#define LINK_FORCED_DOWN	0
+#define LINK_FORCED_UP		1
+#define LINK_UNFORCED		-2
+
+	/* Port's MAC link state
+	 * Use LINK_FORCED_UP or LINK_FORCED_DOWN to force link up or down,
+	 * or LINK_UNFORCED for normal link detection.
+	 */
+	int (*port_set_link)(struct mv88e6xxx_chip *chip, int port, int link);
+
+#define DUPLEX_UNFORCED		-2
+
+	/* Port's MAC duplex mode
+	 *
+	 * Use DUPLEX_HALF or DUPLEX_FULL to force half or full duplex,
+	 * or DUPLEX_UNFORCED for normal duplex detection.
+	 */
+	int (*port_set_duplex)(struct mv88e6xxx_chip *chip, int port, int dup);
+
+#define SPEED_MAX		INT_MAX
+#define SPEED_UNFORCED		-2
+
+	/* Port's MAC speed (in Mbps)
+	 *
+	 * Depending on the chip, 10, 100, 200, 1000, 2500, 10000 are valid.
+	 * Use SPEED_UNFORCED for normal detection, SPEED_MAX for max value.
+	 */
+	int (*port_set_speed)(struct mv88e6xxx_chip *chip, int port, int speed);
+
+	int (*port_tag_remap)(struct mv88e6xxx_chip *chip, int port);
+
+	int (*port_set_frame_mode)(struct mv88e6xxx_chip *chip, int port,
+				   enum mv88e6xxx_frame_mode mode);
+	int (*port_set_egress_floods)(struct mv88e6xxx_chip *chip, int port,
+				      bool unicast, bool multicast);
+	int (*port_set_ether_type)(struct mv88e6xxx_chip *chip, int port,
+				   u16 etype);
+	int (*port_set_jumbo_size)(struct mv88e6xxx_chip *chip, int port,
+				   size_t size);
+
+	int (*port_egress_rate_limiting)(struct mv88e6xxx_chip *chip, int port);
+	int (*port_pause_limit)(struct mv88e6xxx_chip *chip, int port, u8 in,
+				u8 out);
+	int (*port_disable_learn_limit)(struct mv88e6xxx_chip *chip, int port);
+	int (*port_disable_pri_override)(struct mv88e6xxx_chip *chip, int port);
+
+	/* CMODE control what PHY mode the MAC will use, eg. SGMII, RGMII, etc.
+	 * Some chips allow this to be configured on specific ports.
+	 */
+	int (*port_set_cmode)(struct mv88e6xxx_chip *chip, int port,
+			      phy_interface_t mode);
+
+	/* Some devices have a per port register indicating what is
+	 * the upstream port this port should forward to.
+	 */
+	int (*port_set_upstream_port)(struct mv88e6xxx_chip *chip, int port,
+				      int upstream_port);
+
+	/* Snapshot the statistics for a port. The statistics can then
+	 * be read back a leisure but still with a consistent view.
+	 */
+	int (*stats_snapshot)(struct mv88e6xxx_chip *chip, int port);
+
+	/* Set the histogram mode for statistics, when the control registers
+	 * are separated out of the STATS_OP register.
+	 */
+	int (*stats_set_histogram)(struct mv88e6xxx_chip *chip);
+
+	/* Return the number of strings describing statistics */
+	int (*stats_get_sset_count)(struct mv88e6xxx_chip *chip);
+	void (*stats_get_strings)(struct mv88e6xxx_chip *chip,  uint8_t *data);
+	void (*stats_get_stats)(struct mv88e6xxx_chip *chip,  int port,
+				uint64_t *data);
+	int (*set_cpu_port)(struct mv88e6xxx_chip *chip, int port);
+	int (*set_egress_port)(struct mv88e6xxx_chip *chip, int port);
+	const struct mv88e6xxx_irq_ops *watchdog_ops;
+
+	/* Can be either in g1 or g2, so don't use a prefix */
+	int (*mgmt_rsvd2cpu)(struct mv88e6xxx_chip *chip);
+
+	/* Power on/off a SERDES interface */
+	int (*serdes_power)(struct mv88e6xxx_chip *chip, int port, bool on);
+
+	/* VLAN Translation Unit operations */
+	int (*vtu_getnext)(struct mv88e6xxx_chip *chip,
+			   struct mv88e6xxx_vtu_entry *entry);
+	int (*vtu_loadpurge)(struct mv88e6xxx_chip *chip,
+			     struct mv88e6xxx_vtu_entry *entry);
+};
+
+struct mv88e6xxx_irq_ops {
+	/* Action to be performed when the interrupt happens */
+	int (*irq_action)(struct mv88e6xxx_chip *chip, int irq);
+	/* Setup the hardware to generate the interrupt */
+	int (*irq_setup)(struct mv88e6xxx_chip *chip);
+	/* Reset the hardware to stop generating the interrupt */
+	void (*irq_free)(struct mv88e6xxx_chip *chip);
+};
+
+#define STATS_TYPE_PORT		BIT(0)
+#define STATS_TYPE_BANK0	BIT(1)
+#define STATS_TYPE_BANK1	BIT(2)
+
+struct mv88e6xxx_hw_stat {
+	char string[ETH_GSTRING_LEN];
+	int sizeof_stat;
+	int reg;
+	int type;
+};
+
+static inline bool mv88e6xxx_has(struct mv88e6xxx_chip *chip,
+				 unsigned long flags)
+{
+	return (chip->info->flags & flags) == flags;
+}
+
+static inline bool mv88e6xxx_has_pvt(struct mv88e6xxx_chip *chip)
+{
+	return chip->info->pvt;
+}
+
+static inline unsigned int mv88e6xxx_num_databases(struct mv88e6xxx_chip *chip)
+{
+	return chip->info->num_databases;
+}
+
+static inline unsigned int mv88e6xxx_num_ports(struct mv88e6xxx_chip *chip)
+{
+	return chip->info->num_ports;
+}
+
+static inline u16 mv88e6xxx_port_mask(struct mv88e6xxx_chip *chip)
+{
+	return GENMASK(mv88e6xxx_num_ports(chip) - 1, 0);
+}
+
+int mv88e6xxx_read(struct mv88e6xxx_chip *chip, int addr, int reg, u16 *val);
+int mv88e6xxx_write(struct mv88e6xxx_chip *chip, int addr, int reg, u16 val);
+int mv88e6xxx_update(struct mv88e6xxx_chip *chip, int addr, int reg,
+		     u16 update);
+int mv88e6xxx_wait(struct mv88e6xxx_chip *chip, int addr, int reg, u16 mask);
+struct mii_bus *mv88e6xxx_default_mdio_bus(struct mv88e6xxx_chip *chip);
+
+#endif /* _MV88E6XXX_CHIP_H */
diff --git a/drivers/net/dsa/mv88e6xxx/global1.c b/drivers/net/dsa/mv88e6xxx/global1.c
index 39825837a1c9..d76d7c7ea819 100644
--- a/drivers/net/dsa/mv88e6xxx/global1.c
+++ b/drivers/net/dsa/mv88e6xxx/global1.c
@@ -12,7 +12,9 @@
  * (at your option) any later version.
  */
 
-#include "mv88e6xxx.h"
+#include <linux/bitfield.h>
+
+#include "chip.h"
 #include "global1.h"
 
 int mv88e6xxx_g1_read(struct mv88e6xxx_chip *chip, int reg, u16 *val)
@@ -42,13 +44,13 @@ static int mv88e6185_g1_wait_ppu_disabled(struct mv88e6xxx_chip *chip)
 	int i, err;
 
 	for (i = 0; i < 16; i++) {
-		err = mv88e6xxx_g1_read(chip, GLOBAL_STATUS, &state);
+		err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_STS, &state);
 		if (err)
 			return err;
 
 		/* Check the value of the PPUState bits 15:14 */
-		state &= GLOBAL_STATUS_PPU_STATE_MASK;
-		if (state != GLOBAL_STATUS_PPU_STATE_POLLING)
+		state &= MV88E6185_G1_STS_PPU_STATE_MASK;
+		if (state != MV88E6185_G1_STS_PPU_STATE_POLLING)
 			return 0;
 
 		usleep_range(1000, 2000);
@@ -63,13 +65,13 @@ static int mv88e6185_g1_wait_ppu_polling(struct mv88e6xxx_chip *chip)
 	int i, err;
 
 	for (i = 0; i < 16; ++i) {
-		err = mv88e6xxx_g1_read(chip, GLOBAL_STATUS, &state);
+		err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_STS, &state);
 		if (err)
 			return err;
 
 		/* Check the value of the PPUState bits 15:14 */
-		state &= GLOBAL_STATUS_PPU_STATE_MASK;
-		if (state == GLOBAL_STATUS_PPU_STATE_POLLING)
+		state &= MV88E6185_G1_STS_PPU_STATE_MASK;
+		if (state == MV88E6185_G1_STS_PPU_STATE_POLLING)
 			return 0;
 
 		usleep_range(1000, 2000);
@@ -84,12 +86,12 @@ static int mv88e6352_g1_wait_ppu_polling(struct mv88e6xxx_chip *chip)
 	int i, err;
 
 	for (i = 0; i < 16; ++i) {
-		err = mv88e6xxx_g1_read(chip, GLOBAL_STATUS, &state);
+		err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_STS, &state);
 		if (err)
 			return err;
 
 		/* Check the value of the PPUState (or InitState) bit 15 */
-		if (state & GLOBAL_STATUS_PPU_STATE)
+		if (state & MV88E6352_G1_STS_PPU_STATE)
 			return 0;
 
 		usleep_range(1000, 2000);
@@ -109,11 +111,11 @@ static int mv88e6xxx_g1_wait_init_ready(struct mv88e6xxx_chip *chip)
 	 * have finished their initialization and are ready to accept frames.
 	 */
 	while (time_before(jiffies, timeout)) {
-		err = mv88e6xxx_g1_read(chip, GLOBAL_STATUS, &val);
+		err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_STS, &val);
 		if (err)
 			return err;
 
-		if (val & GLOBAL_STATUS_INIT_READY)
+		if (val & MV88E6XXX_G1_STS_INIT_READY)
 			break;
 
 		usleep_range(1000, 2000);
@@ -125,6 +127,33 @@ static int mv88e6xxx_g1_wait_init_ready(struct mv88e6xxx_chip *chip)
 	return 0;
 }
 
+/* Offset 0x01: Switch MAC Address Register Bytes 0 & 1
+ * Offset 0x02: Switch MAC Address Register Bytes 2 & 3
+ * Offset 0x03: Switch MAC Address Register Bytes 4 & 5
+ */
+int mv88e6xxx_g1_set_switch_mac(struct mv88e6xxx_chip *chip, u8 *addr)
+{
+	u16 reg;
+	int err;
+
+	reg = (addr[0] << 8) | addr[1];
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_MAC_01, reg);
+	if (err)
+		return err;
+
+	reg = (addr[2] << 8) | addr[3];
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_MAC_23, reg);
+	if (err)
+		return err;
+
+	reg = (addr[4] << 8) | addr[5];
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_MAC_45, reg);
+	if (err)
+		return err;
+
+	return 0;
+}
+
 /* Offset 0x04: Switch Global Control Register */
 
 int mv88e6185_g1_reset(struct mv88e6xxx_chip *chip)
@@ -135,14 +164,14 @@ int mv88e6185_g1_reset(struct mv88e6xxx_chip *chip)
 	/* Set the SWReset bit 15 along with the PPUEn bit 14, to also restart
 	 * the PPU, including re-doing PHY detection and initialization
 	 */
-	err = mv88e6xxx_g1_read(chip, GLOBAL_CONTROL, &val);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_CTL1, &val);
 	if (err)
 		return err;
 
-	val |= GLOBAL_CONTROL_SW_RESET;
-	val |= GLOBAL_CONTROL_PPU_ENABLE;
+	val |= MV88E6XXX_G1_CTL1_SW_RESET;
+	val |= MV88E6XXX_G1_CTL1_PPU_ENABLE;
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_CONTROL, val);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL1, val);
 	if (err)
 		return err;
 
@@ -159,13 +188,13 @@ int mv88e6352_g1_reset(struct mv88e6xxx_chip *chip)
 	int err;
 
 	/* Set the SWReset bit 15 */
-	err = mv88e6xxx_g1_read(chip, GLOBAL_CONTROL, &val);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_CTL1, &val);
 	if (err)
 		return err;
 
-	val |= GLOBAL_CONTROL_SW_RESET;
+	val |= MV88E6XXX_G1_CTL1_SW_RESET;
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_CONTROL, val);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL1, val);
 	if (err)
 		return err;
 
@@ -181,13 +210,13 @@ int mv88e6185_g1_ppu_enable(struct mv88e6xxx_chip *chip)
 	u16 val;
 	int err;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_CONTROL, &val);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_CTL1, &val);
 	if (err)
 		return err;
 
-	val |= GLOBAL_CONTROL_PPU_ENABLE;
+	val |= MV88E6XXX_G1_CTL1_PPU_ENABLE;
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_CONTROL, val);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL1, val);
 	if (err)
 		return err;
 
@@ -199,13 +228,13 @@ int mv88e6185_g1_ppu_disable(struct mv88e6xxx_chip *chip)
 	u16 val;
 	int err;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_CONTROL, &val);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_CTL1, &val);
 	if (err)
 		return err;
 
-	val &= ~GLOBAL_CONTROL_PPU_ENABLE;
+	val &= ~MV88E6XXX_G1_CTL1_PPU_ENABLE;
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_CONTROL, val);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL1, val);
 	if (err)
 		return err;
 
@@ -220,17 +249,17 @@ int mv88e6095_g1_set_egress_port(struct mv88e6xxx_chip *chip, int port)
 	u16 reg;
 	int err;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_MONITOR_CONTROL, &reg);
+	err = mv88e6xxx_g1_read(chip, MV88E6185_G1_MONITOR_CTL, &reg);
 	if (err)
 		return err;
 
-	reg &= ~(GLOBAL_MONITOR_CONTROL_INGRESS_MASK |
-		 GLOBAL_MONITOR_CONTROL_EGRESS_MASK);
+	reg &= ~(MV88E6185_G1_MONITOR_CTL_INGRESS_DEST_MASK |
+		 MV88E6185_G1_MONITOR_CTL_EGRESS_DEST_MASK);
 
-	reg |= port << GLOBAL_MONITOR_CONTROL_INGRESS_SHIFT |
-		port << GLOBAL_MONITOR_CONTROL_EGRESS_SHIFT;
+	reg |= port << __bf_shf(MV88E6185_G1_MONITOR_CTL_INGRESS_DEST_MASK) |
+		port << __bf_shf(MV88E6185_G1_MONITOR_CTL_EGRESS_DEST_MASK);
 
-	return mv88e6xxx_g1_write(chip, GLOBAL_MONITOR_CONTROL, reg);
+	return mv88e6xxx_g1_write(chip, MV88E6185_G1_MONITOR_CTL, reg);
 }
 
 /* Older generations also call this the ARP destination. It has been
@@ -242,14 +271,14 @@ int mv88e6095_g1_set_cpu_port(struct mv88e6xxx_chip *chip, int port)
 	u16 reg;
 	int err;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_MONITOR_CONTROL, &reg);
+	err = mv88e6xxx_g1_read(chip, MV88E6185_G1_MONITOR_CTL, &reg);
 	if (err)
 		return err;
 
-	reg &= ~GLOBAL_MONITOR_CONTROL_ARP_MASK;
-	reg |= port << GLOBAL_MONITOR_CONTROL_ARP_SHIFT;
+	reg &= ~MV88E6185_G1_MONITOR_CTL_ARP_DEST_MASK;
+	reg |= port << __bf_shf(MV88E6185_G1_MONITOR_CTL_ARP_DEST_MASK);
 
-	return mv88e6xxx_g1_write(chip, GLOBAL_MONITOR_CONTROL, reg);
+	return mv88e6xxx_g1_write(chip, MV88E6185_G1_MONITOR_CTL, reg);
 }
 
 static int mv88e6390_g1_monitor_write(struct mv88e6xxx_chip *chip,
@@ -257,55 +286,66 @@ static int mv88e6390_g1_monitor_write(struct mv88e6xxx_chip *chip,
 {
 	u16 reg;
 
-	reg = GLOBAL_MONITOR_CONTROL_UPDATE | pointer | data;
+	reg = MV88E6390_G1_MONITOR_MGMT_CTL_UPDATE | pointer | data;
 
-	return mv88e6xxx_g1_write(chip, GLOBAL_MONITOR_CONTROL, reg);
+	return mv88e6xxx_g1_write(chip, MV88E6390_G1_MONITOR_MGMT_CTL, reg);
 }
 
 int mv88e6390_g1_set_egress_port(struct mv88e6xxx_chip *chip, int port)
 {
+	u16 ptr;
 	int err;
 
-	err = mv88e6390_g1_monitor_write(chip, GLOBAL_MONITOR_CONTROL_INGRESS,
-					 port);
+	ptr = MV88E6390_G1_MONITOR_MGMT_CTL_PTR_INGRESS_DEST;
+	err = mv88e6390_g1_monitor_write(chip, ptr, port);
+	if (err)
+		return err;
+
+	ptr = MV88E6390_G1_MONITOR_MGMT_CTL_PTR_EGRESS_DEST;
+	err = mv88e6390_g1_monitor_write(chip, ptr, port);
 	if (err)
 		return err;
 
-	return mv88e6390_g1_monitor_write(chip, GLOBAL_MONITOR_CONTROL_EGRESS,
-					  port);
+	return 0;
 }
 
 int mv88e6390_g1_set_cpu_port(struct mv88e6xxx_chip *chip, int port)
 {
-	return mv88e6390_g1_monitor_write(chip, GLOBAL_MONITOR_CONTROL_CPU_DEST,
-					  port);
+	u16 ptr = MV88E6390_G1_MONITOR_MGMT_CTL_PTR_CPU_DEST;
+
+	return mv88e6390_g1_monitor_write(chip, ptr, port);
 }
 
 int mv88e6390_g1_mgmt_rsvd2cpu(struct mv88e6xxx_chip *chip)
 {
+	u16 ptr;
 	int err;
 
 	/* 01:c2:80:00:00:00:00-01:c2:80:00:00:00:07 are Management */
-	err = mv88e6390_g1_monitor_write(
-		chip, GLOBAL_MONITOR_CONTROL_0180C280000000XLO, 0xff);
+	ptr = MV88E6390_G1_MONITOR_MGMT_CTL_PTR_0180C280000000XLO;
+	err = mv88e6390_g1_monitor_write(chip, ptr, 0xff);
 	if (err)
 		return err;
 
 	/* 01:c2:80:00:00:00:08-01:c2:80:00:00:00:0f are Management */
-	err = mv88e6390_g1_monitor_write(
-		chip, GLOBAL_MONITOR_CONTROL_0180C280000000XHI, 0xff);
+	ptr = MV88E6390_G1_MONITOR_MGMT_CTL_PTR_0180C280000000XHI;
+	err = mv88e6390_g1_monitor_write(chip, ptr, 0xff);
 	if (err)
 		return err;
 
 	/* 01:c2:80:00:00:00:20-01:c2:80:00:00:00:27 are Management */
-	err = mv88e6390_g1_monitor_write(
-		chip, GLOBAL_MONITOR_CONTROL_0180C280000002XLO, 0xff);
+	ptr = MV88E6390_G1_MONITOR_MGMT_CTL_PTR_0180C280000002XLO;
+	err = mv88e6390_g1_monitor_write(chip, ptr, 0xff);
 	if (err)
 		return err;
 
 	/* 01:c2:80:00:00:00:28-01:c2:80:00:00:00:2f are Management */
-	return mv88e6390_g1_monitor_write(
-		chip, GLOBAL_MONITOR_CONTROL_0180C280000002XHI, 0xff);
+	ptr = MV88E6390_G1_MONITOR_MGMT_CTL_PTR_0180C280000002XHI;
+	err = mv88e6390_g1_monitor_write(chip, ptr, 0xff);
+	if (err)
+		return err;
+
+	return 0;
 }
 
 /* Offset 0x1c: Global Control 2 */
@@ -315,13 +355,13 @@ int mv88e6390_g1_stats_set_histogram(struct mv88e6xxx_chip *chip)
 	u16 val;
 	int err;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_CONTROL_2, &val);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_CTL2, &val);
 	if (err)
 		return err;
 
-	val |= GLOBAL_CONTROL_2_HIST_RX_TX;
+	val |= MV88E6XXX_G1_CTL2_HIST_RX_TX;
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_CONTROL_2, val);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL2, val);
 
 	return err;
 }
@@ -330,7 +370,8 @@ int mv88e6390_g1_stats_set_histogram(struct mv88e6xxx_chip *chip)
 
 int mv88e6xxx_g1_stats_wait(struct mv88e6xxx_chip *chip)
 {
-	return mv88e6xxx_g1_wait(chip, GLOBAL_STATS_OP, GLOBAL_STATS_OP_BUSY);
+	return mv88e6xxx_g1_wait(chip, MV88E6XXX_G1_STATS_OP,
+				 MV88E6XXX_G1_STATS_OP_BUSY);
 }
 
 int mv88e6xxx_g1_stats_snapshot(struct mv88e6xxx_chip *chip, int port)
@@ -338,9 +379,10 @@ int mv88e6xxx_g1_stats_snapshot(struct mv88e6xxx_chip *chip, int port)
 	int err;
 
 	/* Snapshot the hardware statistics counters for this port. */
-	err = mv88e6xxx_g1_write(chip, GLOBAL_STATS_OP,
-				 GLOBAL_STATS_OP_CAPTURE_PORT |
-				 GLOBAL_STATS_OP_HIST_RX_TX | port);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_STATS_OP,
+				 MV88E6XXX_G1_STATS_OP_BUSY |
+				 MV88E6XXX_G1_STATS_OP_CAPTURE_PORT |
+				 MV88E6XXX_G1_STATS_OP_HIST_RX_TX | port);
 	if (err)
 		return err;
 
@@ -362,8 +404,9 @@ int mv88e6390_g1_stats_snapshot(struct mv88e6xxx_chip *chip, int port)
 	port = (port + 1) << 5;
 
 	/* Snapshot the hardware statistics counters for this port. */
-	err = mv88e6xxx_g1_write(chip, GLOBAL_STATS_OP,
-				 GLOBAL_STATS_OP_CAPTURE_PORT | port);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_STATS_OP,
+				 MV88E6XXX_G1_STATS_OP_BUSY |
+				 MV88E6XXX_G1_STATS_OP_CAPTURE_PORT | port);
 	if (err)
 		return err;
 
@@ -379,8 +422,9 @@ void mv88e6xxx_g1_stats_read(struct mv88e6xxx_chip *chip, int stat, u32 *val)
 
 	*val = 0;
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_STATS_OP,
-				 GLOBAL_STATS_OP_READ_CAPTURED | stat);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_STATS_OP,
+				 MV88E6XXX_G1_STATS_OP_BUSY |
+				 MV88E6XXX_G1_STATS_OP_READ_CAPTURED | stat);
 	if (err)
 		return;
 
@@ -388,13 +432,13 @@ void mv88e6xxx_g1_stats_read(struct mv88e6xxx_chip *chip, int stat, u32 *val)
 	if (err)
 		return;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_STATS_COUNTER_32, &reg);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_STATS_COUNTER_32, &reg);
 	if (err)
 		return;
 
 	value = reg << 16;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_STATS_COUNTER_01, &reg);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_STATS_COUNTER_01, &reg);
 	if (err)
 		return;
 
diff --git a/drivers/net/dsa/mv88e6xxx/global1.h b/drivers/net/dsa/mv88e6xxx/global1.h
index 46a4ea0f8c47..950b914f9251 100644
--- a/drivers/net/dsa/mv88e6xxx/global1.h
+++ b/drivers/net/dsa/mv88e6xxx/global1.h
@@ -15,12 +15,216 @@
 #ifndef _MV88E6XXX_GLOBAL1_H
 #define _MV88E6XXX_GLOBAL1_H
 
-#include "mv88e6xxx.h"
+#include "chip.h"
+
+/* Offset 0x00: Switch Global Status Register */
+#define MV88E6XXX_G1_STS				0x00
+#define MV88E6352_G1_STS_PPU_STATE			0x8000
+#define MV88E6185_G1_STS_PPU_STATE_MASK			0xc000
+#define MV88E6185_G1_STS_PPU_STATE_DISABLED_RST		0x0000
+#define MV88E6185_G1_STS_PPU_STATE_INITIALIZING		0x4000
+#define MV88E6185_G1_STS_PPU_STATE_DISABLED		0x8000
+#define MV88E6185_G1_STS_PPU_STATE_POLLING		0xc000
+#define MV88E6XXX_G1_STS_INIT_READY			0x0800
+#define MV88E6XXX_G1_STS_IRQ_AVB			8
+#define MV88E6XXX_G1_STS_IRQ_DEVICE			7
+#define MV88E6XXX_G1_STS_IRQ_STATS			6
+#define MV88E6XXX_G1_STS_IRQ_VTU_PROBLEM		5
+#define MV88E6XXX_G1_STS_IRQ_VTU_DONE			4
+#define MV88E6XXX_G1_STS_IRQ_ATU_PROBLEM		3
+#define MV88E6XXX_G1_STS_IRQ_ATU_DONE			2
+#define MV88E6XXX_G1_STS_IRQ_TCAM_DONE			1
+#define MV88E6XXX_G1_STS_IRQ_EEPROM_DONE		0
+
+/* Offset 0x01: Switch MAC Address Register Bytes 0 & 1
+ * Offset 0x02: Switch MAC Address Register Bytes 2 & 3
+ * Offset 0x03: Switch MAC Address Register Bytes 4 & 5
+ */
+#define MV88E6XXX_G1_MAC_01		0x01
+#define MV88E6XXX_G1_MAC_23		0x02
+#define MV88E6XXX_G1_MAC_45		0x03
+
+/* Offset 0x01: ATU FID Register */
+#define MV88E6352_G1_ATU_FID		0x01
+
+/* Offset 0x02: VTU FID Register */
+#define MV88E6352_G1_VTU_FID		0x02
+#define MV88E6352_G1_VTU_FID_MASK	0x0fff
+
+/* Offset 0x03: VTU SID Register */
+#define MV88E6352_G1_VTU_SID		0x03
+#define MV88E6352_G1_VTU_SID_MASK	0x3f
+
+/* Offset 0x04: Switch Global Control Register */
+#define MV88E6XXX_G1_CTL1			0x04
+#define MV88E6XXX_G1_CTL1_SW_RESET		0x8000
+#define MV88E6XXX_G1_CTL1_PPU_ENABLE		0x4000
+#define MV88E6352_G1_CTL1_DISCARD_EXCESS	0x2000
+#define MV88E6185_G1_CTL1_SCHED_PRIO		0x0800
+#define MV88E6185_G1_CTL1_MAX_FRAME_1632	0x0400
+#define MV88E6185_G1_CTL1_RELOAD_EEPROM		0x0200
+#define MV88E6XXX_G1_CTL1_DEVICE_EN		0x0080
+#define MV88E6XXX_G1_CTL1_STATS_DONE_EN		0x0040
+#define MV88E6XXX_G1_CTL1_VTU_PROBLEM_EN	0x0020
+#define MV88E6XXX_G1_CTL1_VTU_DONE_EN		0x0010
+#define MV88E6XXX_G1_CTL1_ATU_PROBLEM_EN	0x0008
+#define MV88E6XXX_G1_CTL1_ATU_DONE_EN		0x0004
+#define MV88E6XXX_G1_CTL1_TCAM_EN		0x0002
+#define MV88E6XXX_G1_CTL1_EEPROM_DONE_EN	0x0001
+
+/* Offset 0x05: VTU Operation Register */
+#define MV88E6XXX_G1_VTU_OP			0x05
+#define MV88E6XXX_G1_VTU_OP_BUSY		0x8000
+#define MV88E6XXX_G1_VTU_OP_MASK		0x7000
+#define MV88E6XXX_G1_VTU_OP_FLUSH_ALL		0x1000
+#define MV88E6XXX_G1_VTU_OP_NOOP		0x2000
+#define MV88E6XXX_G1_VTU_OP_VTU_LOAD_PURGE	0x3000
+#define MV88E6XXX_G1_VTU_OP_VTU_GET_NEXT	0x4000
+#define MV88E6XXX_G1_VTU_OP_STU_LOAD_PURGE	0x5000
+#define MV88E6XXX_G1_VTU_OP_STU_GET_NEXT	0x6000
+
+/* Offset 0x06: VTU VID Register */
+#define MV88E6XXX_G1_VTU_VID		0x06
+#define MV88E6XXX_G1_VTU_VID_MASK	0x0fff
+#define MV88E6390_G1_VTU_VID_PAGE	0x2000
+#define MV88E6XXX_G1_VTU_VID_VALID	0x1000
+
+/* Offset 0x07: VTU/STU Data Register 1
+ * Offset 0x08: VTU/STU Data Register 2
+ * Offset 0x09: VTU/STU Data Register 3
+ */
+#define MV88E6XXX_G1_VTU_DATA1				0x07
+#define MV88E6XXX_G1_VTU_DATA2				0x08
+#define MV88E6XXX_G1_VTU_DATA3				0x09
+#define MV88E6XXX_G1_VTU_STU_DATA_MASK			0x0003
+#define MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_UNMODIFIED	0x0000
+#define MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_UNTAGGED	0x0001
+#define MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_TAGGED		0x0002
+#define MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_NON_MEMBER	0x0003
+#define MV88E6XXX_G1_STU_DATA_PORT_STATE_DISABLED	0x0000
+#define MV88E6XXX_G1_STU_DATA_PORT_STATE_BLOCKING	0x0001
+#define MV88E6XXX_G1_STU_DATA_PORT_STATE_LEARNING	0x0002
+#define MV88E6XXX_G1_STU_DATA_PORT_STATE_FORWARDING	0x0003
+
+/* Offset 0x0A: ATU Control Register */
+#define MV88E6XXX_G1_ATU_CTL		0x0a
+#define MV88E6XXX_G1_ATU_CTL_LEARN2ALL	0x0008
+
+/* Offset 0x0B: ATU Operation Register */
+#define MV88E6XXX_G1_ATU_OP				0x0b
+#define MV88E6XXX_G1_ATU_OP_BUSY			0x8000
+#define MV88E6XXX_G1_ATU_OP_MASK			0x7000
+#define MV88E6XXX_G1_ATU_OP_NOOP			0x0000
+#define MV88E6XXX_G1_ATU_OP_FLUSH_MOVE_ALL		0x1000
+#define MV88E6XXX_G1_ATU_OP_FLUSH_MOVE_NON_STATIC	0x2000
+#define MV88E6XXX_G1_ATU_OP_LOAD_DB			0x3000
+#define MV88E6XXX_G1_ATU_OP_GET_NEXT_DB			0x4000
+#define MV88E6XXX_G1_ATU_OP_FLUSH_MOVE_ALL_DB		0x5000
+#define MV88E6XXX_G1_ATU_OP_FLUSH_MOVE_NON_STATIC_DB	0x6000
+#define MV88E6XXX_G1_ATU_OP_GET_CLR_VIOLATION		0x7000
+
+/* Offset 0x0C: ATU Data Register */
+#define MV88E6XXX_G1_ATU_DATA				0x0c
+#define MV88E6XXX_G1_ATU_DATA_TRUNK			0x8000
+#define MV88E6XXX_G1_ATU_DATA_TRUNK_ID_MASK		0x00f0
+#define MV88E6XXX_G1_ATU_DATA_PORT_VECTOR_MASK		0x3ff0
+#define MV88E6XXX_G1_ATU_DATA_STATE_MASK		0x000f
+#define MV88E6XXX_G1_ATU_DATA_STATE_UNUSED		0x0000
+#define MV88E6XXX_G1_ATU_DATA_STATE_UC_MGMT		0x000d
+#define MV88E6XXX_G1_ATU_DATA_STATE_UC_STATIC		0x000e
+#define MV88E6XXX_G1_ATU_DATA_STATE_UC_PRIO_OVER	0x000f
+#define MV88E6XXX_G1_ATU_DATA_STATE_MC_NONE_RATE	0x0005
+#define MV88E6XXX_G1_ATU_DATA_STATE_MC_STATIC		0x0007
+#define MV88E6XXX_G1_ATU_DATA_STATE_MC_MGMT		0x000e
+#define MV88E6XXX_G1_ATU_DATA_STATE_MC_PRIO_OVER	0x000f
+
+/* Offset 0x0D: ATU MAC Address Register Bytes 0 & 1
+ * Offset 0x0E: ATU MAC Address Register Bytes 2 & 3
+ * Offset 0x0F: ATU MAC Address Register Bytes 4 & 5
+ */
+#define MV88E6XXX_G1_ATU_MAC01		0x0d
+#define MV88E6XXX_G1_ATU_MAC23		0x0e
+#define MV88E6XXX_G1_ATU_MAC45		0x0f
+
+/* Offset 0x10: IP-PRI Mapping Register 0
+ * Offset 0x11: IP-PRI Mapping Register 1
+ * Offset 0x12: IP-PRI Mapping Register 2
+ * Offset 0x13: IP-PRI Mapping Register 3
+ * Offset 0x14: IP-PRI Mapping Register 4
+ * Offset 0x15: IP-PRI Mapping Register 5
+ * Offset 0x16: IP-PRI Mapping Register 6
+ * Offset 0x17: IP-PRI Mapping Register 7
+ */
+#define MV88E6XXX_G1_IP_PRI_0	0x10
+#define MV88E6XXX_G1_IP_PRI_1	0x11
+#define MV88E6XXX_G1_IP_PRI_2	0x12
+#define MV88E6XXX_G1_IP_PRI_3	0x13
+#define MV88E6XXX_G1_IP_PRI_4	0x14
+#define MV88E6XXX_G1_IP_PRI_5	0x15
+#define MV88E6XXX_G1_IP_PRI_6	0x16
+#define MV88E6XXX_G1_IP_PRI_7	0x17
+
+/* Offset 0x18: IEEE-PRI Register */
+#define MV88E6XXX_G1_IEEE_PRI	0x18
+
+/* Offset 0x19: Core Tag Type */
+#define MV88E6185_G1_CORE_TAG_TYPE	0x19
+
+/* Offset 0x1A: Monitor Control */
+#define MV88E6185_G1_MONITOR_CTL			0x1a
+#define MV88E6185_G1_MONITOR_CTL_INGRESS_DEST_MASK	0xf000
+#define MV88E6185_G1_MONITOR_CTL_EGRESS_DEST_MASK	0x0f00
+#define MV88E6185_G1_MONITOR_CTL_ARP_DEST_MASK	        0x00f0
+#define MV88E6352_G1_MONITOR_CTL_CPU_DEST_MASK	        0x00f0
+#define MV88E6352_G1_MONITOR_CTL_MIRROR_DEST_MASK	0x000f
+
+/* Offset 0x1A: Monitor & MGMT Control Register */
+#define MV88E6390_G1_MONITOR_MGMT_CTL				0x1a
+#define MV88E6390_G1_MONITOR_MGMT_CTL_UPDATE			0x8000
+#define MV88E6390_G1_MONITOR_MGMT_CTL_PTR_MASK			0x3f00
+#define MV88E6390_G1_MONITOR_MGMT_CTL_PTR_0180C280000000XLO	0x0000
+#define MV88E6390_G1_MONITOR_MGMT_CTL_PTR_0180C280000000XHI	0x0100
+#define MV88E6390_G1_MONITOR_MGMT_CTL_PTR_0180C280000002XLO	0x0200
+#define MV88E6390_G1_MONITOR_MGMT_CTL_PTR_0180C280000002XHI	0x0300
+#define MV88E6390_G1_MONITOR_MGMT_CTL_PTR_INGRESS_DEST		0x2000
+#define MV88E6390_G1_MONITOR_MGMT_CTL_PTR_EGRESS_DEST		0x2100
+#define MV88E6390_G1_MONITOR_MGMT_CTL_PTR_CPU_DEST		0x3000
+#define MV88E6390_G1_MONITOR_MGMT_CTL_DATA_MASK			0x00ff
+
+/* Offset 0x1C: Global Control 2 */
+#define MV88E6XXX_G1_CTL2			0x1c
+#define MV88E6XXX_G1_CTL2_NO_CASCADE		0xe000
+#define MV88E6XXX_G1_CTL2_MULTIPLE_CASCADE	0xf000
+#define MV88E6XXX_G1_CTL2_HIST_RX		0x0040
+#define MV88E6XXX_G1_CTL2_HIST_TX		0x0080
+#define MV88E6XXX_G1_CTL2_HIST_RX_TX		0x00c0
+
+/* Offset 0x1D: Stats Operation Register */
+#define MV88E6XXX_G1_STATS_OP			0x1d
+#define MV88E6XXX_G1_STATS_OP_BUSY		0x8000
+#define MV88E6XXX_G1_STATS_OP_NOP		0x0000
+#define MV88E6XXX_G1_STATS_OP_FLUSH_ALL		0x1000
+#define MV88E6XXX_G1_STATS_OP_FLUSH_PORT	0x2000
+#define MV88E6XXX_G1_STATS_OP_READ_CAPTURED	0x4000
+#define MV88E6XXX_G1_STATS_OP_CAPTURE_PORT	0x5000
+#define MV88E6XXX_G1_STATS_OP_HIST_RX		0x0400
+#define MV88E6XXX_G1_STATS_OP_HIST_TX		0x0800
+#define MV88E6XXX_G1_STATS_OP_HIST_RX_TX	0x0c00
+#define MV88E6XXX_G1_STATS_OP_BANK_1_BIT_9	0x0200
+#define MV88E6XXX_G1_STATS_OP_BANK_1_BIT_10	0x0400
+
+/* Offset 0x1E: Stats Counter Register Bytes 3 & 2
+ * Offset 0x1F: Stats Counter Register Bytes 1 & 0
+ */
+#define MV88E6XXX_G1_STATS_COUNTER_32	0x1e
+#define MV88E6XXX_G1_STATS_COUNTER_01	0x1f
 
 int mv88e6xxx_g1_read(struct mv88e6xxx_chip *chip, int reg, u16 *val);
 int mv88e6xxx_g1_write(struct mv88e6xxx_chip *chip, int reg, u16 val);
 int mv88e6xxx_g1_wait(struct mv88e6xxx_chip *chip, int reg, u16 mask);
 
+int mv88e6xxx_g1_set_switch_mac(struct mv88e6xxx_chip *chip, u8 *addr);
+
 int mv88e6185_g1_reset(struct mv88e6xxx_chip *chip);
 int mv88e6352_g1_reset(struct mv88e6xxx_chip *chip);
 
diff --git a/drivers/net/dsa/mv88e6xxx/global1_atu.c b/drivers/net/dsa/mv88e6xxx/global1_atu.c
index fa7e7db5171b..efeef4b01442 100644
--- a/drivers/net/dsa/mv88e6xxx/global1_atu.c
+++ b/drivers/net/dsa/mv88e6xxx/global1_atu.c
@@ -10,14 +10,14 @@
  * (at your option) any later version.
  */
 
-#include "mv88e6xxx.h"
+#include "chip.h"
 #include "global1.h"
 
 /* Offset 0x01: ATU FID Register */
 
 static int mv88e6xxx_g1_atu_fid_write(struct mv88e6xxx_chip *chip, u16 fid)
 {
-	return mv88e6xxx_g1_write(chip, GLOBAL_ATU_FID, fid & 0xfff);
+	return mv88e6xxx_g1_write(chip, MV88E6352_G1_ATU_FID, fid & 0xfff);
 }
 
 /* Offset 0x0A: ATU Control Register */
@@ -27,16 +27,16 @@ int mv88e6xxx_g1_atu_set_learn2all(struct mv88e6xxx_chip *chip, bool learn2all)
 	u16 val;
 	int err;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_ATU_CONTROL, &val);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_ATU_CTL, &val);
 	if (err)
 		return err;
 
 	if (learn2all)
-		val |= GLOBAL_ATU_CONTROL_LEARN2ALL;
+		val |= MV88E6XXX_G1_ATU_CTL_LEARN2ALL;
 	else
-		val &= ~GLOBAL_ATU_CONTROL_LEARN2ALL;
+		val &= ~MV88E6XXX_G1_ATU_CTL_LEARN2ALL;
 
-	return mv88e6xxx_g1_write(chip, GLOBAL_ATU_CONTROL, val);
+	return mv88e6xxx_g1_write(chip, MV88E6XXX_G1_ATU_CTL, val);
 }
 
 int mv88e6xxx_g1_atu_set_age_time(struct mv88e6xxx_chip *chip,
@@ -55,7 +55,7 @@ int mv88e6xxx_g1_atu_set_age_time(struct mv88e6xxx_chip *chip,
 	/* Round to nearest multiple of coeff */
 	age_time = (msecs + coeff / 2) / coeff;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_ATU_CONTROL, &val);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_ATU_CTL, &val);
 	if (err)
 		return err;
 
@@ -63,7 +63,7 @@ int mv88e6xxx_g1_atu_set_age_time(struct mv88e6xxx_chip *chip,
 	val &= ~0xff0;
 	val |= age_time << 4;
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_ATU_CONTROL, val);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_ATU_CTL, val);
 	if (err)
 		return err;
 
@@ -77,7 +77,8 @@ int mv88e6xxx_g1_atu_set_age_time(struct mv88e6xxx_chip *chip,
 
 static int mv88e6xxx_g1_atu_op_wait(struct mv88e6xxx_chip *chip)
 {
-	return mv88e6xxx_g1_wait(chip, GLOBAL_ATU_OP, GLOBAL_ATU_OP_BUSY);
+	return mv88e6xxx_g1_wait(chip, MV88E6XXX_G1_ATU_OP,
+				 MV88E6XXX_G1_ATU_OP_BUSY);
 }
 
 static int mv88e6xxx_g1_atu_op(struct mv88e6xxx_chip *chip, u16 fid, u16 op)
@@ -93,12 +94,14 @@ static int mv88e6xxx_g1_atu_op(struct mv88e6xxx_chip *chip, u16 fid, u16 op)
 	} else {
 		if (mv88e6xxx_num_databases(chip) > 16) {
 			/* ATU DBNum[7:4] are located in ATU Control 15:12 */
-			err = mv88e6xxx_g1_read(chip, GLOBAL_ATU_CONTROL, &val);
+			err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_ATU_CTL,
+						&val);
 			if (err)
 				return err;
 
 			val = (val & 0x0fff) | ((fid << 8) & 0xf000);
-			err = mv88e6xxx_g1_write(chip, GLOBAL_ATU_CONTROL, val);
+			err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_ATU_CTL,
+						 val);
 			if (err)
 				return err;
 		}
@@ -107,7 +110,8 @@ static int mv88e6xxx_g1_atu_op(struct mv88e6xxx_chip *chip, u16 fid, u16 op)
 		op |= fid & 0xf;
 	}
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_ATU_OP, op);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_ATU_OP,
+				 MV88E6XXX_G1_ATU_OP_BUSY | op);
 	if (err)
 		return err;
 
@@ -122,13 +126,13 @@ static int mv88e6xxx_g1_atu_data_read(struct mv88e6xxx_chip *chip,
 	u16 val;
 	int err;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_ATU_DATA, &val);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_ATU_DATA, &val);
 	if (err)
 		return err;
 
 	entry->state = val & 0xf;
-	if (entry->state != GLOBAL_ATU_DATA_STATE_UNUSED) {
-		entry->trunk = !!(val & GLOBAL_ATU_DATA_TRUNK);
+	if (entry->state != MV88E6XXX_G1_ATU_DATA_STATE_UNUSED) {
+		entry->trunk = !!(val & MV88E6XXX_G1_ATU_DATA_TRUNK);
 		entry->portvec = (val >> 4) & mv88e6xxx_port_mask(chip);
 	}
 
@@ -140,14 +144,14 @@ static int mv88e6xxx_g1_atu_data_write(struct mv88e6xxx_chip *chip,
 {
 	u16 data = entry->state & 0xf;
 
-	if (entry->state != GLOBAL_ATU_DATA_STATE_UNUSED) {
+	if (entry->state != MV88E6XXX_G1_ATU_DATA_STATE_UNUSED) {
 		if (entry->trunk)
-			data |= GLOBAL_ATU_DATA_TRUNK;
+			data |= MV88E6XXX_G1_ATU_DATA_TRUNK;
 
 		data |= (entry->portvec & mv88e6xxx_port_mask(chip)) << 4;
 	}
 
-	return mv88e6xxx_g1_write(chip, GLOBAL_ATU_DATA, data);
+	return mv88e6xxx_g1_write(chip, MV88E6XXX_G1_ATU_DATA, data);
 }
 
 /* Offset 0x0D: ATU MAC Address Register Bytes 0 & 1
@@ -162,7 +166,7 @@ static int mv88e6xxx_g1_atu_mac_read(struct mv88e6xxx_chip *chip,
 	int i, err;
 
 	for (i = 0; i < 3; i++) {
-		err = mv88e6xxx_g1_read(chip, GLOBAL_ATU_MAC_01 + i, &val);
+		err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_ATU_MAC01 + i, &val);
 		if (err)
 			return err;
 
@@ -181,7 +185,7 @@ static int mv88e6xxx_g1_atu_mac_write(struct mv88e6xxx_chip *chip,
 
 	for (i = 0; i < 3; i++) {
 		val = (entry->mac[i * 2] << 8) | entry->mac[i * 2 + 1];
-		err = mv88e6xxx_g1_write(chip, GLOBAL_ATU_MAC_01 + i, val);
+		err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_ATU_MAC01 + i, val);
 		if (err)
 			return err;
 	}
@@ -201,13 +205,13 @@ int mv88e6xxx_g1_atu_getnext(struct mv88e6xxx_chip *chip, u16 fid,
 		return err;
 
 	/* Write the MAC address to iterate from only once */
-	if (entry->state == GLOBAL_ATU_DATA_STATE_UNUSED) {
+	if (entry->state == MV88E6XXX_G1_ATU_DATA_STATE_UNUSED) {
 		err = mv88e6xxx_g1_atu_mac_write(chip, entry);
 		if (err)
 			return err;
 	}
 
-	err = mv88e6xxx_g1_atu_op(chip, fid, GLOBAL_ATU_OP_GET_NEXT_DB);
+	err = mv88e6xxx_g1_atu_op(chip, fid, MV88E6XXX_G1_ATU_OP_GET_NEXT_DB);
 	if (err)
 		return err;
 
@@ -235,7 +239,7 @@ int mv88e6xxx_g1_atu_loadpurge(struct mv88e6xxx_chip *chip, u16 fid,
 	if (err)
 		return err;
 
-	return mv88e6xxx_g1_atu_op(chip, fid, GLOBAL_ATU_OP_LOAD_DB);
+	return mv88e6xxx_g1_atu_op(chip, fid, MV88E6XXX_G1_ATU_OP_LOAD_DB);
 }
 
 static int mv88e6xxx_g1_atu_flushmove(struct mv88e6xxx_chip *chip, u16 fid,
@@ -255,13 +259,13 @@ static int mv88e6xxx_g1_atu_flushmove(struct mv88e6xxx_chip *chip, u16 fid,
 
 	/* Flush/Move all or non-static entries from all or a given database */
 	if (all && fid)
-		op = GLOBAL_ATU_OP_FLUSH_MOVE_ALL_DB;
+		op = MV88E6XXX_G1_ATU_OP_FLUSH_MOVE_ALL_DB;
 	else if (fid)
-		op = GLOBAL_ATU_OP_FLUSH_MOVE_NON_STATIC_DB;
+		op = MV88E6XXX_G1_ATU_OP_FLUSH_MOVE_NON_STATIC_DB;
 	else if (all)
-		op = GLOBAL_ATU_OP_FLUSH_MOVE_ALL;
+		op = MV88E6XXX_G1_ATU_OP_FLUSH_MOVE_ALL;
 	else
-		op = GLOBAL_ATU_OP_FLUSH_MOVE_NON_STATIC;
+		op = MV88E6XXX_G1_ATU_OP_FLUSH_MOVE_NON_STATIC;
 
 	return mv88e6xxx_g1_atu_op(chip, fid, op);
 }
diff --git a/drivers/net/dsa/mv88e6xxx/global1_vtu.c b/drivers/net/dsa/mv88e6xxx/global1_vtu.c
index 9aea22d4c9e2..8c8a0ec3d6e9 100644
--- a/drivers/net/dsa/mv88e6xxx/global1_vtu.c
+++ b/drivers/net/dsa/mv88e6xxx/global1_vtu.c
@@ -11,7 +11,7 @@
  * (at your option) any later version.
  */
 
-#include "mv88e6xxx.h"
+#include "chip.h"
 #include "global1.h"
 
 /* Offset 0x02: VTU FID Register */
@@ -22,11 +22,11 @@ static int mv88e6xxx_g1_vtu_fid_read(struct mv88e6xxx_chip *chip,
 	u16 val;
 	int err;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_VTU_FID, &val);
+	err = mv88e6xxx_g1_read(chip, MV88E6352_G1_VTU_FID, &val);
 	if (err)
 		return err;
 
-	entry->fid = val & GLOBAL_VTU_FID_MASK;
+	entry->fid = val & MV88E6352_G1_VTU_FID_MASK;
 
 	return 0;
 }
@@ -34,9 +34,9 @@ static int mv88e6xxx_g1_vtu_fid_read(struct mv88e6xxx_chip *chip,
 static int mv88e6xxx_g1_vtu_fid_write(struct mv88e6xxx_chip *chip,
 				      struct mv88e6xxx_vtu_entry *entry)
 {
-	u16 val = entry->fid & GLOBAL_VTU_FID_MASK;
+	u16 val = entry->fid & MV88E6352_G1_VTU_FID_MASK;
 
-	return mv88e6xxx_g1_write(chip, GLOBAL_VTU_FID, val);
+	return mv88e6xxx_g1_write(chip, MV88E6352_G1_VTU_FID, val);
 }
 
 /* Offset 0x03: VTU SID Register */
@@ -47,11 +47,11 @@ static int mv88e6xxx_g1_vtu_sid_read(struct mv88e6xxx_chip *chip,
 	u16 val;
 	int err;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_VTU_SID, &val);
+	err = mv88e6xxx_g1_read(chip, MV88E6352_G1_VTU_SID, &val);
 	if (err)
 		return err;
 
-	entry->sid = val & GLOBAL_VTU_SID_MASK;
+	entry->sid = val & MV88E6352_G1_VTU_SID_MASK;
 
 	return 0;
 }
@@ -59,23 +59,25 @@ static int mv88e6xxx_g1_vtu_sid_read(struct mv88e6xxx_chip *chip,
 static int mv88e6xxx_g1_vtu_sid_write(struct mv88e6xxx_chip *chip,
 				      struct mv88e6xxx_vtu_entry *entry)
 {
-	u16 val = entry->sid & GLOBAL_VTU_SID_MASK;
+	u16 val = entry->sid & MV88E6352_G1_VTU_SID_MASK;
 
-	return mv88e6xxx_g1_write(chip, GLOBAL_VTU_SID, val);
+	return mv88e6xxx_g1_write(chip, MV88E6352_G1_VTU_SID, val);
 }
 
 /* Offset 0x05: VTU Operation Register */
 
 static int mv88e6xxx_g1_vtu_op_wait(struct mv88e6xxx_chip *chip)
 {
-	return mv88e6xxx_g1_wait(chip, GLOBAL_VTU_OP, GLOBAL_VTU_OP_BUSY);
+	return mv88e6xxx_g1_wait(chip, MV88E6XXX_G1_VTU_OP,
+				 MV88E6XXX_G1_VTU_OP_BUSY);
 }
 
 static int mv88e6xxx_g1_vtu_op(struct mv88e6xxx_chip *chip, u16 op)
 {
 	int err;
 
-	err = mv88e6xxx_g1_write(chip, GLOBAL_VTU_OP, op);
+	err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_VTU_OP,
+				 MV88E6XXX_G1_VTU_OP_BUSY | op);
 	if (err)
 		return err;
 
@@ -90,16 +92,16 @@ static int mv88e6xxx_g1_vtu_vid_read(struct mv88e6xxx_chip *chip,
 	u16 val;
 	int err;
 
-	err = mv88e6xxx_g1_read(chip, GLOBAL_VTU_VID, &val);
+	err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_VTU_VID, &val);
 	if (err)
 		return err;
 
 	entry->vid = val & 0xfff;
 
-	if (val & GLOBAL_VTU_VID_PAGE)
+	if (val & MV88E6390_G1_VTU_VID_PAGE)
 		entry->vid |= 0x1000;
 
-	entry->valid = !!(val & GLOBAL_VTU_VID_VALID);
+	entry->valid = !!(val & MV88E6XXX_G1_VTU_VID_VALID);
 
 	return 0;
 }
@@ -110,12 +112,12 @@ static int mv88e6xxx_g1_vtu_vid_write(struct mv88e6xxx_chip *chip,
 	u16 val = entry->vid & 0xfff;
 
 	if (entry->vid & 0x1000)
-		val |= GLOBAL_VTU_VID_PAGE;
+		val |= MV88E6390_G1_VTU_VID_PAGE;
 
 	if (entry->valid)
-		val |= GLOBAL_VTU_VID_VALID;
+		val |= MV88E6XXX_G1_VTU_VID_VALID;
 
-	return mv88e6xxx_g1_write(chip, GLOBAL_VTU_VID, val);
+	return mv88e6xxx_g1_write(chip, MV88E6XXX_G1_VTU_VID, val);
 }
 
 /* Offset 0x07: VTU/STU Data Register 1
@@ -134,7 +136,7 @@ static int mv88e6185_g1_vtu_data_read(struct mv88e6xxx_chip *chip,
 		u16 *reg = &regs[i];
 		int err;
 
-		err = mv88e6xxx_g1_read(chip, GLOBAL_VTU_DATA_0_3 + i, reg);
+		err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_VTU_DATA1 + i, reg);
 		if (err)
 			return err;
 	}
@@ -171,7 +173,7 @@ static int mv88e6185_g1_vtu_data_write(struct mv88e6xxx_chip *chip,
 		u16 reg = regs[i];
 		int err;
 
-		err = mv88e6xxx_g1_write(chip, GLOBAL_VTU_DATA_0_3 + i, reg);
+		err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_VTU_DATA1 + i, reg);
 		if (err)
 			return err;
 	}
@@ -189,7 +191,7 @@ static int mv88e6390_g1_vtu_data_read(struct mv88e6xxx_chip *chip, u8 *data)
 		u16 *reg = &regs[i];
 		int err;
 
-		err = mv88e6xxx_g1_read(chip, GLOBAL_VTU_DATA_0_3 + i, reg);
+		err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_VTU_DATA1 + i, reg);
 		if (err)
 			return err;
 	}
@@ -221,7 +223,7 @@ static int mv88e6390_g1_vtu_data_write(struct mv88e6xxx_chip *chip, u8 *data)
 		u16 reg = regs[i];
 		int err;
 
-		err = mv88e6xxx_g1_write(chip, GLOBAL_VTU_DATA_0_3 + i, reg);
+		err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_VTU_DATA1 + i, reg);
 		if (err)
 			return err;
 	}
@@ -240,7 +242,7 @@ static int mv88e6xxx_g1_vtu_stu_getnext(struct mv88e6xxx_chip *chip,
 	if (err)
 		return err;
 
-	err = mv88e6xxx_g1_vtu_op(chip, GLOBAL_VTU_OP_STU_GET_NEXT);
+	err = mv88e6xxx_g1_vtu_op(chip, MV88E6XXX_G1_VTU_OP_STU_GET_NEXT);
 	if (err)
 		return err;
 
@@ -295,7 +297,7 @@ static int mv88e6xxx_g1_vtu_getnext(struct mv88e6xxx_chip *chip,
 			return err;
 	}
 
-	err = mv88e6xxx_g1_vtu_op(chip, GLOBAL_VTU_OP_VTU_GET_NEXT);
+	err = mv88e6xxx_g1_vtu_op(chip, MV88E6XXX_G1_VTU_OP_VTU_GET_NEXT);
 	if (err)
 		return err;
 
@@ -320,7 +322,7 @@ int mv88e6185_g1_vtu_getnext(struct mv88e6xxx_chip *chip,
 		/* VTU DBNum[3:0] are located in VTU Operation 3:0
 		 * VTU DBNum[7:4] are located in VTU Operation 11:8
 		 */
-		err = mv88e6xxx_g1_read(chip, GLOBAL_VTU_OP, &val);
+		err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_VTU_OP, &val);
 		if (err)
 			return err;
 
@@ -394,7 +396,7 @@ int mv88e6390_g1_vtu_getnext(struct mv88e6xxx_chip *chip,
 int mv88e6185_g1_vtu_loadpurge(struct mv88e6xxx_chip *chip,
 			       struct mv88e6xxx_vtu_entry *entry)
 {
-	u16 op = GLOBAL_VTU_OP_VTU_LOAD_PURGE;
+	u16 op = MV88E6XXX_G1_VTU_OP_VTU_LOAD_PURGE;
 	int err;
 
 	err = mv88e6xxx_g1_vtu_op_wait(chip);
@@ -444,7 +446,8 @@ int mv88e6352_g1_vtu_loadpurge(struct mv88e6xxx_chip *chip,
 			return err;
 
 		/* Load STU entry */
-		err = mv88e6xxx_g1_vtu_op(chip, GLOBAL_VTU_OP_STU_LOAD_PURGE);
+		err = mv88e6xxx_g1_vtu_op(chip,
+					  MV88E6XXX_G1_VTU_OP_STU_LOAD_PURGE);
 		if (err)
 			return err;
 
@@ -454,7 +457,7 @@ int mv88e6352_g1_vtu_loadpurge(struct mv88e6xxx_chip *chip,
 	}
 
 	/* Load/Purge VTU entry */
-	return mv88e6xxx_g1_vtu_op(chip, GLOBAL_VTU_OP_VTU_LOAD_PURGE);
+	return mv88e6xxx_g1_vtu_op(chip, MV88E6XXX_G1_VTU_OP_VTU_LOAD_PURGE);
 }
 
 int mv88e6390_g1_vtu_loadpurge(struct mv88e6xxx_chip *chip,
@@ -481,7 +484,8 @@ int mv88e6390_g1_vtu_loadpurge(struct mv88e6xxx_chip *chip,
 			return err;
 
 		/* Load STU entry */
-		err = mv88e6xxx_g1_vtu_op(chip, GLOBAL_VTU_OP_STU_LOAD_PURGE);
+		err = mv88e6xxx_g1_vtu_op(chip,
+					  MV88E6XXX_G1_VTU_OP_STU_LOAD_PURGE);
 		if (err)
 			return err;
 
@@ -496,7 +500,7 @@ int mv88e6390_g1_vtu_loadpurge(struct mv88e6xxx_chip *chip,
 	}
 
 	/* Load/Purge VTU entry */
-	return mv88e6xxx_g1_vtu_op(chip, GLOBAL_VTU_OP_VTU_LOAD_PURGE);
+	return mv88e6xxx_g1_vtu_op(chip, MV88E6XXX_G1_VTU_OP_VTU_LOAD_PURGE);
 }
 
 int mv88e6xxx_g1_vtu_flush(struct mv88e6xxx_chip *chip)
@@ -507,5 +511,5 @@ int mv88e6xxx_g1_vtu_flush(struct mv88e6xxx_chip *chip)
 	if (err)
 		return err;
 
-	return mv88e6xxx_g1_vtu_op(chip, GLOBAL_VTU_OP_FLUSH_ALL);
+	return mv88e6xxx_g1_vtu_op(chip, MV88E6XXX_G1_VTU_OP_FLUSH_ALL);
 }
diff --git a/drivers/net/dsa/mv88e6xxx/global2.c b/drivers/net/dsa/mv88e6xxx/global2.c
index b3fea55071e3..158d0f499874 100644
--- a/drivers/net/dsa/mv88e6xxx/global2.c
+++ b/drivers/net/dsa/mv88e6xxx/global2.c
@@ -1,6 +1,5 @@
 /*
- * Marvell 88E6xxx Switch Global 2 Registers support (device address
- * 0x1C)
+ * Marvell 88E6xxx Switch Global 2 Registers support
  *
  * Copyright (c) 2008 Marvell Semiconductor
  *
@@ -13,31 +12,32 @@
  * (at your option) any later version.
  */
 
+#include <linux/bitfield.h>
 #include <linux/interrupt.h>
 #include <linux/irqdomain.h>
-#include "mv88e6xxx.h"
-#include "global2.h"
 
-#define ADDR_GLOBAL2	0x1c
+#include "chip.h"
+#include "global1.h" /* for MV88E6XXX_G1_STS_IRQ_DEVICE */
+#include "global2.h"
 
 static int mv88e6xxx_g2_read(struct mv88e6xxx_chip *chip, int reg, u16 *val)
 {
-	return mv88e6xxx_read(chip, ADDR_GLOBAL2, reg, val);
+	return mv88e6xxx_read(chip, MV88E6XXX_G2, reg, val);
 }
 
 static int mv88e6xxx_g2_write(struct mv88e6xxx_chip *chip, int reg, u16 val)
 {
-	return mv88e6xxx_write(chip, ADDR_GLOBAL2, reg, val);
+	return mv88e6xxx_write(chip, MV88E6XXX_G2, reg, val);
 }
 
 static int mv88e6xxx_g2_update(struct mv88e6xxx_chip *chip, int reg, u16 update)
 {
-	return mv88e6xxx_update(chip, ADDR_GLOBAL2, reg, update);
+	return mv88e6xxx_update(chip, MV88E6XXX_G2, reg, update);
 }
 
 static int mv88e6xxx_g2_wait(struct mv88e6xxx_chip *chip, int reg, u16 mask)
 {
-	return mv88e6xxx_wait(chip, ADDR_GLOBAL2, reg, mask);
+	return mv88e6xxx_wait(chip, MV88E6XXX_G2, reg, mask);
 }
 
 /* Offset 0x02: Management Enable 2x */
@@ -51,7 +51,7 @@ int mv88e6095_g2_mgmt_rsvd2cpu(struct mv88e6xxx_chip *chip)
 	 * addresses matching 01:80:c2:00:00:2x as MGMT.
 	 */
 	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_MGMT_EN_2X)) {
-		err = mv88e6xxx_g2_write(chip, GLOBAL2_MGMT_EN_2X, 0xffff);
+		err = mv88e6xxx_g2_write(chip, MV88E6XXX_G2_MGMT_EN_2X, 0xffff);
 		if (err)
 			return err;
 	}
@@ -60,7 +60,8 @@ int mv88e6095_g2_mgmt_rsvd2cpu(struct mv88e6xxx_chip *chip)
 	 * addresses matching 01:80:c2:00:00:0x as MGMT.
 	 */
 	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_MGMT_EN_0X))
-		return mv88e6xxx_g2_write(chip, GLOBAL2_MGMT_EN_0X, 0xffff);
+		return mv88e6xxx_g2_write(chip, MV88E6XXX_G2_MGMT_EN_0X,
+					  0xffff);
 
 	return 0;
 }
@@ -72,7 +73,7 @@ static int mv88e6xxx_g2_device_mapping_write(struct mv88e6xxx_chip *chip,
 {
 	u16 val = (target << 8) | (port & 0xf);
 
-	return mv88e6xxx_g2_update(chip, GLOBAL2_DEVICE_MAPPING, val);
+	return mv88e6xxx_g2_update(chip, MV88E6XXX_G2_DEVICE_MAPPING, val);
 }
 
 static int mv88e6xxx_g2_set_device_mapping(struct mv88e6xxx_chip *chip)
@@ -101,15 +102,14 @@ static int mv88e6xxx_g2_set_device_mapping(struct mv88e6xxx_chip *chip)
 /* Offset 0x07: Trunk Mask Table register */
 
 static int mv88e6xxx_g2_trunk_mask_write(struct mv88e6xxx_chip *chip, int num,
-					 bool hask, u16 mask)
+					 bool hash, u16 mask)
 {
-	const u16 port_mask = BIT(mv88e6xxx_num_ports(chip)) - 1;
-	u16 val = (num << 12) | (mask & port_mask);
+	u16 val = (num << 12) | (mask & mv88e6xxx_port_mask(chip));
 
-	if (hask)
-		val |= GLOBAL2_TRUNK_MASK_HASK;
+	if (hash)
+		val |= MV88E6XXX_G2_TRUNK_MASK_HASH;
 
-	return mv88e6xxx_g2_update(chip, GLOBAL2_TRUNK_MASK, val);
+	return mv88e6xxx_g2_update(chip, MV88E6XXX_G2_TRUNK_MASK, val);
 }
 
 /* Offset 0x08: Trunk Mapping Table register */
@@ -120,7 +120,7 @@ static int mv88e6xxx_g2_trunk_mapping_write(struct mv88e6xxx_chip *chip, int id,
 	const u16 port_mask = BIT(mv88e6xxx_num_ports(chip)) - 1;
 	u16 val = (id << 11) | (map & port_mask);
 
-	return mv88e6xxx_g2_update(chip, GLOBAL2_TRUNK_MAPPING, val);
+	return mv88e6xxx_g2_update(chip, MV88E6XXX_G2_TRUNK_MAPPING, val);
 }
 
 static int mv88e6xxx_g2_clear_trunk(struct mv88e6xxx_chip *chip)
@@ -149,27 +149,36 @@ static int mv88e6xxx_g2_clear_trunk(struct mv88e6xxx_chip *chip)
  * Offset 0x0A: Ingress Rate Data register
  */
 
-static int mv88e6xxx_g2_clear_irl(struct mv88e6xxx_chip *chip)
+static int mv88e6xxx_g2_irl_wait(struct mv88e6xxx_chip *chip)
 {
-	int port, err;
+	return mv88e6xxx_g2_wait(chip, MV88E6XXX_G2_IRL_CMD,
+				 MV88E6XXX_G2_IRL_CMD_BUSY);
+}
 
-	/* Init all Ingress Rate Limit resources of all ports */
-	for (port = 0; port < mv88e6xxx_num_ports(chip); ++port) {
-		/* XXX newer chips (like 88E6390) have different 2-bit ops */
-		err = mv88e6xxx_g2_write(chip, GLOBAL2_IRL_CMD,
-					 GLOBAL2_IRL_CMD_OP_INIT_ALL |
-					 (port << 8));
-		if (err)
-			break;
+static int mv88e6xxx_g2_irl_op(struct mv88e6xxx_chip *chip, u16 op, int port,
+			       int res, int reg)
+{
+	int err;
 
-		/* Wait for the operation to complete */
-		err = mv88e6xxx_g2_wait(chip, GLOBAL2_IRL_CMD,
-					GLOBAL2_IRL_CMD_BUSY);
-		if (err)
-			break;
-	}
+	err = mv88e6xxx_g2_write(chip, MV88E6XXX_G2_IRL_CMD,
+				 MV88E6XXX_G2_IRL_CMD_BUSY | op | (port << 8) |
+				 (res << 5) | reg);
+	if (err)
+		return err;
 
-	return err;
+	return mv88e6xxx_g2_irl_wait(chip);
+}
+
+int mv88e6352_g2_irl_init_all(struct mv88e6xxx_chip *chip, int port)
+{
+	return mv88e6xxx_g2_irl_op(chip, MV88E6352_G2_IRL_CMD_OP_INIT_ALL, port,
+				   0, 0);
+}
+
+int mv88e6390_g2_irl_init_all(struct mv88e6xxx_chip *chip, int port)
+{
+	return mv88e6xxx_g2_irl_op(chip, MV88E6390_G2_IRL_CMD_OP_INIT_ALL, port,
+				   0, 0);
 }
 
 /* Offset 0x0B: Cross-chip Port VLAN (Addr) Register
@@ -178,7 +187,8 @@ static int mv88e6xxx_g2_clear_irl(struct mv88e6xxx_chip *chip)
 
 static int mv88e6xxx_g2_pvt_op_wait(struct mv88e6xxx_chip *chip)
 {
-	return mv88e6xxx_g2_wait(chip, GLOBAL2_PVT_ADDR, GLOBAL2_PVT_ADDR_BUSY);
+	return mv88e6xxx_g2_wait(chip, MV88E6XXX_G2_PVT_ADDR,
+				 MV88E6XXX_G2_PVT_ADDR_BUSY);
 }
 
 static int mv88e6xxx_g2_pvt_op(struct mv88e6xxx_chip *chip, int src_dev,
@@ -186,13 +196,14 @@ static int mv88e6xxx_g2_pvt_op(struct mv88e6xxx_chip *chip, int src_dev,
 {
 	int err;
 
-	/* 9-bit Cross-chip PVT pointer: with GLOBAL2_MISC_5_BIT_PORT cleared,
-	 * source device is 5-bit, source port is 4-bit.
+	/* 9-bit Cross-chip PVT pointer: with MV88E6XXX_G2_MISC_5_BIT_PORT
+	 * cleared, source device is 5-bit, source port is 4-bit.
 	 */
+	op |= MV88E6XXX_G2_PVT_ADDR_BUSY;
 	op |= (src_dev & 0x1f) << 4;
 	op |= (src_port & 0xf);
 
-	err = mv88e6xxx_g2_write(chip, GLOBAL2_PVT_ADDR, op);
+	err = mv88e6xxx_g2_write(chip, MV88E6XXX_G2_PVT_ADDR, op);
 	if (err)
 		return err;
 
@@ -208,12 +219,12 @@ int mv88e6xxx_g2_pvt_write(struct mv88e6xxx_chip *chip, int src_dev,
 	if (err)
 		return err;
 
-	err = mv88e6xxx_g2_write(chip, GLOBAL2_PVT_DATA, data);
+	err = mv88e6xxx_g2_write(chip, MV88E6XXX_G2_PVT_DATA, data);
 	if (err)
 		return err;
 
 	return mv88e6xxx_g2_pvt_op(chip, src_dev, src_port,
-				   GLOBAL2_PVT_ADDR_OP_WRITE_PVLAN);
+				   MV88E6XXX_G2_PVT_ADDR_OP_WRITE_PVLAN);
 }
 
 /* Offset 0x0D: Switch MAC/WoL/WoF register */
@@ -223,7 +234,7 @@ static int mv88e6xxx_g2_switch_mac_write(struct mv88e6xxx_chip *chip,
 {
 	u16 val = (pointer << 8) | data;
 
-	return mv88e6xxx_g2_update(chip, GLOBAL2_SWITCH_MAC, val);
+	return mv88e6xxx_g2_update(chip, MV88E6XXX_G2_SWITCH_MAC, val);
 }
 
 int mv88e6xxx_g2_set_switch_mac(struct mv88e6xxx_chip *chip, u8 *addr)
@@ -246,7 +257,7 @@ static int mv88e6xxx_g2_pot_write(struct mv88e6xxx_chip *chip, int pointer,
 {
 	u16 val = (pointer << 8) | (data & 0x7);
 
-	return mv88e6xxx_g2_update(chip, GLOBAL2_PRIO_OVERRIDE, val);
+	return mv88e6xxx_g2_update(chip, MV88E6XXX_G2_PRIO_OVERRIDE, val);
 }
 
 static int mv88e6xxx_g2_clear_pot(struct mv88e6xxx_chip *chip)
@@ -270,16 +281,17 @@ static int mv88e6xxx_g2_clear_pot(struct mv88e6xxx_chip *chip)
 
 static int mv88e6xxx_g2_eeprom_wait(struct mv88e6xxx_chip *chip)
 {
-	return mv88e6xxx_g2_wait(chip, GLOBAL2_EEPROM_CMD,
-				 GLOBAL2_EEPROM_CMD_BUSY |
-				 GLOBAL2_EEPROM_CMD_RUNNING);
+	return mv88e6xxx_g2_wait(chip, MV88E6XXX_G2_EEPROM_CMD,
+				 MV88E6XXX_G2_EEPROM_CMD_BUSY |
+				 MV88E6XXX_G2_EEPROM_CMD_RUNNING);
 }
 
 static int mv88e6xxx_g2_eeprom_cmd(struct mv88e6xxx_chip *chip, u16 cmd)
 {
 	int err;
 
-	err = mv88e6xxx_g2_write(chip, GLOBAL2_EEPROM_CMD, cmd);
+	err = mv88e6xxx_g2_write(chip, MV88E6XXX_G2_EEPROM_CMD,
+				 MV88E6XXX_G2_EEPROM_CMD_BUSY | cmd);
 	if (err)
 		return err;
 
@@ -289,14 +301,14 @@ static int mv88e6xxx_g2_eeprom_cmd(struct mv88e6xxx_chip *chip, u16 cmd)
 static int mv88e6xxx_g2_eeprom_read8(struct mv88e6xxx_chip *chip,
 				     u16 addr, u8 *data)
 {
-	u16 cmd = GLOBAL2_EEPROM_CMD_OP_READ;
+	u16 cmd = MV88E6XXX_G2_EEPROM_CMD_OP_READ;
 	int err;
 
 	err = mv88e6xxx_g2_eeprom_wait(chip);
 	if (err)
 		return err;
 
-	err = mv88e6xxx_g2_write(chip, GLOBAL2_EEPROM_ADDR, addr);
+	err = mv88e6xxx_g2_write(chip, MV88E6390_G2_EEPROM_ADDR, addr);
 	if (err)
 		return err;
 
@@ -304,7 +316,7 @@ static int mv88e6xxx_g2_eeprom_read8(struct mv88e6xxx_chip *chip,
 	if (err)
 		return err;
 
-	err = mv88e6xxx_g2_read(chip, GLOBAL2_EEPROM_CMD, &cmd);
+	err = mv88e6xxx_g2_read(chip, MV88E6XXX_G2_EEPROM_CMD, &cmd);
 	if (err)
 		return err;
 
@@ -316,14 +328,15 @@ static int mv88e6xxx_g2_eeprom_read8(struct mv88e6xxx_chip *chip,
 static int mv88e6xxx_g2_eeprom_write8(struct mv88e6xxx_chip *chip,
 				      u16 addr, u8 data)
 {
-	u16 cmd = GLOBAL2_EEPROM_CMD_OP_WRITE | GLOBAL2_EEPROM_CMD_WRITE_EN;
+	u16 cmd = MV88E6XXX_G2_EEPROM_CMD_OP_WRITE |
+		MV88E6XXX_G2_EEPROM_CMD_WRITE_EN;
 	int err;
 
 	err = mv88e6xxx_g2_eeprom_wait(chip);
 	if (err)
 		return err;
 
-	err = mv88e6xxx_g2_write(chip, GLOBAL2_EEPROM_ADDR, addr);
+	err = mv88e6xxx_g2_write(chip, MV88E6390_G2_EEPROM_ADDR, addr);
 	if (err)
 		return err;
 
@@ -333,7 +346,7 @@ static int mv88e6xxx_g2_eeprom_write8(struct mv88e6xxx_chip *chip,
 static int mv88e6xxx_g2_eeprom_read16(struct mv88e6xxx_chip *chip,
 				      u8 addr, u16 *data)
 {
-	u16 cmd = GLOBAL2_EEPROM_CMD_OP_READ | addr;
+	u16 cmd = MV88E6XXX_G2_EEPROM_CMD_OP_READ | addr;
 	int err;
 
 	err = mv88e6xxx_g2_eeprom_wait(chip);
@@ -344,20 +357,20 @@ static int mv88e6xxx_g2_eeprom_read16(struct mv88e6xxx_chip *chip,
 	if (err)
 		return err;
 
-	return mv88e6xxx_g2_read(chip, GLOBAL2_EEPROM_DATA, data);
+	return mv88e6xxx_g2_read(chip, MV88E6352_G2_EEPROM_DATA, data);
 }
 
 static int mv88e6xxx_g2_eeprom_write16(struct mv88e6xxx_chip *chip,
 				       u8 addr, u16 data)
 {
-	u16 cmd = GLOBAL2_EEPROM_CMD_OP_WRITE | addr;
+	u16 cmd = MV88E6XXX_G2_EEPROM_CMD_OP_WRITE | addr;
 	int err;
 
 	err = mv88e6xxx_g2_eeprom_wait(chip);
 	if (err)
 		return err;
 
-	err = mv88e6xxx_g2_write(chip, GLOBAL2_EEPROM_DATA, data);
+	err = mv88e6xxx_g2_write(chip, MV88E6352_G2_EEPROM_DATA, data);
 	if (err)
 		return err;
 
@@ -469,11 +482,11 @@ int mv88e6xxx_g2_set_eeprom16(struct mv88e6xxx_chip *chip,
 	int err;
 
 	/* Ensure the RO WriteEn bit is set */
-	err = mv88e6xxx_g2_read(chip, GLOBAL2_EEPROM_CMD, &val);
+	err = mv88e6xxx_g2_read(chip, MV88E6XXX_G2_EEPROM_CMD, &val);
 	if (err)
 		return err;
 
-	if (!(val & GLOBAL2_EEPROM_CMD_WRITE_EN))
+	if (!(val & MV88E6XXX_G2_EEPROM_CMD_WRITE_EN))
 		return -EROFS;
 
 	eeprom->len = 0;
@@ -532,178 +545,213 @@ int mv88e6xxx_g2_set_eeprom16(struct mv88e6xxx_chip *chip,
 
 static int mv88e6xxx_g2_smi_phy_wait(struct mv88e6xxx_chip *chip)
 {
-	return mv88e6xxx_g2_wait(chip, GLOBAL2_SMI_PHY_CMD,
-				 GLOBAL2_SMI_PHY_CMD_BUSY);
+	return mv88e6xxx_g2_wait(chip, MV88E6XXX_G2_SMI_PHY_CMD,
+				 MV88E6XXX_G2_SMI_PHY_CMD_BUSY);
 }
 
 static int mv88e6xxx_g2_smi_phy_cmd(struct mv88e6xxx_chip *chip, u16 cmd)
 {
 	int err;
 
-	err = mv88e6xxx_g2_write(chip, GLOBAL2_SMI_PHY_CMD, cmd);
+	err = mv88e6xxx_g2_write(chip, MV88E6XXX_G2_SMI_PHY_CMD,
+				 MV88E6XXX_G2_SMI_PHY_CMD_BUSY | cmd);
 	if (err)
 		return err;
 
 	return mv88e6xxx_g2_smi_phy_wait(chip);
 }
 
-static int mv88e6xxx_g2_smi_phy_write_addr(struct mv88e6xxx_chip *chip,
-					   int addr, int device, int reg,
-					   bool external)
+static int mv88e6xxx_g2_smi_phy_access(struct mv88e6xxx_chip *chip,
+				       bool external, bool c45, u16 op, int dev,
+				       int reg)
 {
-	int cmd = SMI_CMD_OP_45_WRITE_ADDR | (addr << 5) | device;
-	int err;
+	u16 cmd = op;
 
 	if (external)
-		cmd |= GLOBAL2_SMI_PHY_CMD_EXTERNAL;
+		cmd |= MV88E6390_G2_SMI_PHY_CMD_FUNC_EXTERNAL;
+	else
+		cmd |= MV88E6390_G2_SMI_PHY_CMD_FUNC_INTERNAL; /* empty mask */
 
-	err = mv88e6xxx_g2_smi_phy_wait(chip);
-	if (err)
-		return err;
+	if (c45)
+		cmd |= MV88E6XXX_G2_SMI_PHY_CMD_MODE_45; /* empty mask */
+	else
+		cmd |= MV88E6XXX_G2_SMI_PHY_CMD_MODE_22;
 
-	err = mv88e6xxx_g2_write(chip, GLOBAL2_SMI_PHY_DATA, reg);
-	if (err)
-		return err;
+	dev <<= __bf_shf(MV88E6XXX_G2_SMI_PHY_CMD_DEV_ADDR_MASK);
+	cmd |= dev & MV88E6XXX_G2_SMI_PHY_CMD_DEV_ADDR_MASK;
+	cmd |= reg & MV88E6XXX_G2_SMI_PHY_CMD_REG_ADDR_MASK;
 
 	return mv88e6xxx_g2_smi_phy_cmd(chip, cmd);
 }
 
-static int mv88e6xxx_g2_smi_phy_read_c45(struct mv88e6xxx_chip *chip,
-					 int addr, int reg_c45, u16 *val,
-					 bool external)
+static int mv88e6xxx_g2_smi_phy_access_c22(struct mv88e6xxx_chip *chip,
+					   bool external, u16 op, int dev,
+					   int reg)
+{
+	return mv88e6xxx_g2_smi_phy_access(chip, external, false, op, dev, reg);
+}
+
+/* IEEE 802.3 Clause 22 Read Data Register */
+static int mv88e6xxx_g2_smi_phy_read_data_c22(struct mv88e6xxx_chip *chip,
+					      bool external, int dev, int reg,
+					      u16 *data)
 {
-	int device = (reg_c45 >> 16) & 0x1f;
-	int reg = reg_c45 & 0xffff;
+	u16 op = MV88E6XXX_G2_SMI_PHY_CMD_OP_22_READ_DATA;
 	int err;
-	u16 cmd;
 
-	err = mv88e6xxx_g2_smi_phy_write_addr(chip, addr, device, reg,
-					      external);
+	err = mv88e6xxx_g2_smi_phy_wait(chip);
 	if (err)
 		return err;
 
-	cmd = GLOBAL2_SMI_PHY_CMD_OP_45_READ_DATA | (addr << 5) | device;
+	err = mv88e6xxx_g2_smi_phy_access_c22(chip, external, op, dev, reg);
+	if (err)
+		return err;
 
-	if (external)
-		cmd |= GLOBAL2_SMI_PHY_CMD_EXTERNAL;
+	return mv88e6xxx_g2_read(chip, MV88E6XXX_G2_SMI_PHY_DATA, data);
+}
 
-	err = mv88e6xxx_g2_smi_phy_cmd(chip, cmd);
+/* IEEE 802.3 Clause 22 Write Data Register */
+static int mv88e6xxx_g2_smi_phy_write_data_c22(struct mv88e6xxx_chip *chip,
+					       bool external, int dev, int reg,
+					       u16 data)
+{
+	u16 op = MV88E6XXX_G2_SMI_PHY_CMD_OP_22_WRITE_DATA;
+	int err;
+
+	err = mv88e6xxx_g2_smi_phy_wait(chip);
 	if (err)
 		return err;
 
-	err = mv88e6xxx_g2_read(chip, GLOBAL2_SMI_PHY_DATA, val);
+	err = mv88e6xxx_g2_write(chip, MV88E6XXX_G2_SMI_PHY_DATA, data);
 	if (err)
 		return err;
 
-	err = *val;
+	return mv88e6xxx_g2_smi_phy_access_c22(chip, external, op, dev, reg);
+}
 
-	return 0;
+static int mv88e6xxx_g2_smi_phy_access_c45(struct mv88e6xxx_chip *chip,
+					   bool external, u16 op, int port,
+					   int dev)
+{
+	return mv88e6xxx_g2_smi_phy_access(chip, external, true, op, port, dev);
 }
 
-static int mv88e6xxx_g2_smi_phy_read_c22(struct mv88e6xxx_chip *chip,
-					 int addr, int reg, u16 *val,
-					 bool external)
+/* IEEE 802.3 Clause 45 Write Address Register */
+static int mv88e6xxx_g2_smi_phy_write_addr_c45(struct mv88e6xxx_chip *chip,
+					       bool external, int port, int dev,
+					       int addr)
 {
-	u16 cmd = GLOBAL2_SMI_PHY_CMD_OP_22_READ_DATA | (addr << 5) | reg;
+	u16 op = MV88E6XXX_G2_SMI_PHY_CMD_OP_45_WRITE_ADDR;
 	int err;
 
-	if (external)
-		cmd |= GLOBAL2_SMI_PHY_CMD_EXTERNAL;
-
 	err = mv88e6xxx_g2_smi_phy_wait(chip);
 	if (err)
 		return err;
 
-	err = mv88e6xxx_g2_smi_phy_cmd(chip, cmd);
+	err = mv88e6xxx_g2_write(chip, MV88E6XXX_G2_SMI_PHY_DATA, addr);
 	if (err)
 		return err;
 
-	return mv88e6xxx_g2_read(chip, GLOBAL2_SMI_PHY_DATA, val);
+	return mv88e6xxx_g2_smi_phy_access_c45(chip, external, op, port, dev);
 }
 
-int mv88e6xxx_g2_smi_phy_read(struct mv88e6xxx_chip *chip,
-			      struct mii_bus *bus,
-			      int addr, int reg, u16 *val)
+/* IEEE 802.3 Clause 45 Read Data Register */
+static int mv88e6xxx_g2_smi_phy_read_data_c45(struct mv88e6xxx_chip *chip,
+					      bool external, int port, int dev,
+					      u16 *data)
 {
-	struct mv88e6xxx_mdio_bus *mdio_bus = bus->priv;
-	bool external = mdio_bus->external;
+	u16 op = MV88E6XXX_G2_SMI_PHY_CMD_OP_45_READ_DATA;
+	int err;
 
-	if (reg & MII_ADDR_C45)
-		return mv88e6xxx_g2_smi_phy_read_c45(chip, addr, reg, val,
-						     external);
-	return mv88e6xxx_g2_smi_phy_read_c22(chip, addr, reg, val, external);
+	err = mv88e6xxx_g2_smi_phy_access_c45(chip, external, op, port, dev);
+	if (err)
+		return err;
+
+	return mv88e6xxx_g2_read(chip, MV88E6XXX_G2_SMI_PHY_DATA, data);
 }
 
-static int mv88e6xxx_g2_smi_phy_write_c45(struct mv88e6xxx_chip *chip,
-					  int addr, int reg_c45, u16 val,
-					  bool external)
+static int mv88e6xxx_g2_smi_phy_read_c45(struct mv88e6xxx_chip *chip,
+					 bool external, int port, int reg,
+					 u16 *data)
 {
-	int device = (reg_c45 >> 16) & 0x1f;
-	int reg = reg_c45 & 0xffff;
+	int dev = (reg >> 16) & 0x1f;
+	int addr = reg & 0xffff;
 	int err;
-	u16 cmd;
 
-	err = mv88e6xxx_g2_smi_phy_write_addr(chip, addr, device, reg,
-					      external);
+	err = mv88e6xxx_g2_smi_phy_write_addr_c45(chip, external, port, dev,
+						  addr);
 	if (err)
 		return err;
 
-	cmd = GLOBAL2_SMI_PHY_CMD_OP_45_WRITE_DATA | (addr << 5) | device;
-
-	if (external)
-		cmd |= GLOBAL2_SMI_PHY_CMD_EXTERNAL;
+	return mv88e6xxx_g2_smi_phy_read_data_c45(chip, external, port, dev,
+						  data);
+}
 
-	err = mv88e6xxx_g2_write(chip, GLOBAL2_SMI_PHY_DATA, val);
-	if (err)
-		return err;
+/* IEEE 802.3 Clause 45 Write Data Register */
+static int mv88e6xxx_g2_smi_phy_write_data_c45(struct mv88e6xxx_chip *chip,
+					       bool external, int port, int dev,
+					       u16 data)
+{
+	u16 op = MV88E6XXX_G2_SMI_PHY_CMD_OP_45_WRITE_DATA;
+	int err;
 
-	err = mv88e6xxx_g2_smi_phy_cmd(chip, cmd);
+	err = mv88e6xxx_g2_write(chip, MV88E6XXX_G2_SMI_PHY_DATA, data);
 	if (err)
 		return err;
 
-	return 0;
+	return mv88e6xxx_g2_smi_phy_access_c45(chip, external, op, port, dev);
 }
 
-static int mv88e6xxx_g2_smi_phy_write_c22(struct mv88e6xxx_chip *chip,
-					  int addr, int reg, u16 val,
-					  bool external)
+static int mv88e6xxx_g2_smi_phy_write_c45(struct mv88e6xxx_chip *chip,
+					  bool external, int port, int reg,
+					  u16 data)
 {
-	u16 cmd = GLOBAL2_SMI_PHY_CMD_OP_22_WRITE_DATA | (addr << 5) | reg;
+	int dev = (reg >> 16) & 0x1f;
+	int addr = reg & 0xffff;
 	int err;
 
-	if (external)
-		cmd |= GLOBAL2_SMI_PHY_CMD_EXTERNAL;
-
-	err = mv88e6xxx_g2_smi_phy_wait(chip);
+	err = mv88e6xxx_g2_smi_phy_write_addr_c45(chip, external, port, dev,
+						  addr);
 	if (err)
 		return err;
 
-	err = mv88e6xxx_g2_write(chip, GLOBAL2_SMI_PHY_DATA, val);
-	if (err)
-		return err;
+	return mv88e6xxx_g2_smi_phy_write_data_c45(chip, external, port, dev,
+						   data);
+}
 
-	return mv88e6xxx_g2_smi_phy_cmd(chip, cmd);
+int mv88e6xxx_g2_smi_phy_read(struct mv88e6xxx_chip *chip, struct mii_bus *bus,
+			      int addr, int reg, u16 *val)
+{
+	struct mv88e6xxx_mdio_bus *mdio_bus = bus->priv;
+	bool external = mdio_bus->external;
+
+	if (reg & MII_ADDR_C45)
+		return mv88e6xxx_g2_smi_phy_read_c45(chip, external, addr, reg,
+						     val);
+
+	return mv88e6xxx_g2_smi_phy_read_data_c22(chip, external, addr, reg,
+						  val);
 }
 
-int mv88e6xxx_g2_smi_phy_write(struct mv88e6xxx_chip *chip,
-			       struct mii_bus *bus,
+int mv88e6xxx_g2_smi_phy_write(struct mv88e6xxx_chip *chip, struct mii_bus *bus,
 			       int addr, int reg, u16 val)
 {
 	struct mv88e6xxx_mdio_bus *mdio_bus = bus->priv;
 	bool external = mdio_bus->external;
 
 	if (reg & MII_ADDR_C45)
-		return mv88e6xxx_g2_smi_phy_write_c45(chip, addr, reg, val,
-						      external);
+		return mv88e6xxx_g2_smi_phy_write_c45(chip, external, addr, reg,
+						      val);
 
-	return mv88e6xxx_g2_smi_phy_write_c22(chip, addr, reg, val, external);
+	return mv88e6xxx_g2_smi_phy_write_data_c22(chip, external, addr, reg,
+						   val);
 }
 
 static int mv88e6097_watchdog_action(struct mv88e6xxx_chip *chip, int irq)
 {
 	u16 reg;
 
-	mv88e6xxx_g2_read(chip, GLOBAL2_WDOG_CONTROL, &reg);
+	mv88e6xxx_g2_read(chip, MV88E6352_G2_WDOG_CTL, &reg);
 
 	dev_info(chip->dev, "Watchdog event: 0x%04x", reg);
 
@@ -714,20 +762,20 @@ static void mv88e6097_watchdog_free(struct mv88e6xxx_chip *chip)
 {
 	u16 reg;
 
-	mv88e6xxx_g2_read(chip, GLOBAL2_WDOG_CONTROL, &reg);
+	mv88e6xxx_g2_read(chip, MV88E6352_G2_WDOG_CTL, &reg);
 
-	reg &= ~(GLOBAL2_WDOG_CONTROL_EGRESS_ENABLE |
-		 GLOBAL2_WDOG_CONTROL_QC_ENABLE);
+	reg &= ~(MV88E6352_G2_WDOG_CTL_EGRESS_ENABLE |
+		 MV88E6352_G2_WDOG_CTL_QC_ENABLE);
 
-	mv88e6xxx_g2_write(chip, GLOBAL2_WDOG_CONTROL, reg);
+	mv88e6xxx_g2_write(chip, MV88E6352_G2_WDOG_CTL, reg);
 }
 
 static int mv88e6097_watchdog_setup(struct mv88e6xxx_chip *chip)
 {
-	return mv88e6xxx_g2_write(chip, GLOBAL2_WDOG_CONTROL,
-				  GLOBAL2_WDOG_CONTROL_EGRESS_ENABLE |
-				  GLOBAL2_WDOG_CONTROL_QC_ENABLE |
-				  GLOBAL2_WDOG_CONTROL_SWRESET);
+	return mv88e6xxx_g2_write(chip, MV88E6352_G2_WDOG_CTL,
+				  MV88E6352_G2_WDOG_CTL_EGRESS_ENABLE |
+				  MV88E6352_G2_WDOG_CTL_QC_ENABLE |
+				  MV88E6352_G2_WDOG_CTL_SWRESET);
 }
 
 const struct mv88e6xxx_irq_ops mv88e6097_watchdog_ops = {
@@ -738,12 +786,12 @@ const struct mv88e6xxx_irq_ops mv88e6097_watchdog_ops = {
 
 static int mv88e6390_watchdog_setup(struct mv88e6xxx_chip *chip)
 {
-	return mv88e6xxx_g2_update(chip, GLOBAL2_WDOG_CONTROL,
-				   GLOBAL2_WDOG_INT_ENABLE |
-				   GLOBAL2_WDOG_CUT_THROUGH |
-				   GLOBAL2_WDOG_QUEUE_CONTROLLER |
-				   GLOBAL2_WDOG_EGRESS |
-				   GLOBAL2_WDOG_FORCE_IRQ);
+	return mv88e6xxx_g2_update(chip, MV88E6390_G2_WDOG_CTL,
+				   MV88E6390_G2_WDOG_CTL_PTR_INT_ENABLE |
+				   MV88E6390_G2_WDOG_CTL_CUT_THROUGH |
+				   MV88E6390_G2_WDOG_CTL_QUEUE_CONTROLLER |
+				   MV88E6390_G2_WDOG_CTL_EGRESS |
+				   MV88E6390_G2_WDOG_CTL_FORCE_IRQ);
 }
 
 static int mv88e6390_watchdog_action(struct mv88e6xxx_chip *chip, int irq)
@@ -751,17 +799,19 @@ static int mv88e6390_watchdog_action(struct mv88e6xxx_chip *chip, int irq)
 	int err;
 	u16 reg;
 
-	mv88e6xxx_g2_write(chip, GLOBAL2_WDOG_CONTROL, GLOBAL2_WDOG_EVENT);
-	err = mv88e6xxx_g2_read(chip, GLOBAL2_WDOG_CONTROL, &reg);
+	mv88e6xxx_g2_write(chip, MV88E6390_G2_WDOG_CTL,
+			   MV88E6390_G2_WDOG_CTL_PTR_EVENT);
+	err = mv88e6xxx_g2_read(chip, MV88E6390_G2_WDOG_CTL, &reg);
 
 	dev_info(chip->dev, "Watchdog event: 0x%04x",
-		 reg & GLOBAL2_WDOG_DATA_MASK);
+		 reg & MV88E6390_G2_WDOG_CTL_DATA_MASK);
 
-	mv88e6xxx_g2_write(chip, GLOBAL2_WDOG_CONTROL, GLOBAL2_WDOG_HISTORY);
-	err = mv88e6xxx_g2_read(chip, GLOBAL2_WDOG_CONTROL, &reg);
+	mv88e6xxx_g2_write(chip, MV88E6390_G2_WDOG_CTL,
+			   MV88E6390_G2_WDOG_CTL_PTR_HISTORY);
+	err = mv88e6xxx_g2_read(chip, MV88E6390_G2_WDOG_CTL, &reg);
 
 	dev_info(chip->dev, "Watchdog history: 0x%04x",
-		 reg & GLOBAL2_WDOG_DATA_MASK);
+		 reg & MV88E6390_G2_WDOG_CTL_DATA_MASK);
 
 	/* Trigger a software reset to try to recover the switch */
 	if (chip->info->ops->reset)
@@ -774,8 +824,8 @@ static int mv88e6390_watchdog_action(struct mv88e6xxx_chip *chip, int irq)
 
 static void mv88e6390_watchdog_free(struct mv88e6xxx_chip *chip)
 {
-	mv88e6xxx_g2_update(chip, GLOBAL2_WDOG_CONTROL,
-			    GLOBAL2_WDOG_INT_ENABLE);
+	mv88e6xxx_g2_update(chip, MV88E6390_G2_WDOG_CTL,
+			    MV88E6390_G2_WDOG_CTL_PTR_INT_ENABLE);
 }
 
 const struct mv88e6xxx_irq_ops mv88e6390_watchdog_ops = {
@@ -813,7 +863,7 @@ static int mv88e6xxx_g2_watchdog_setup(struct mv88e6xxx_chip *chip)
 	int err;
 
 	chip->watchdog_irq = irq_find_mapping(chip->g2_irq.domain,
-					      GLOBAL2_INT_SOURCE_WATCHDOG);
+					      MV88E6XXX_G2_INT_SOURCE_WATCHDOG);
 	if (chip->watchdog_irq < 0)
 		return chip->watchdog_irq;
 
@@ -840,16 +890,16 @@ static int mv88e6xxx_g2_misc_5_bit_port(struct mv88e6xxx_chip *chip,
 	u16 val;
 	int err;
 
-	err = mv88e6xxx_g2_read(chip, GLOBAL2_MISC, &val);
+	err = mv88e6xxx_g2_read(chip, MV88E6XXX_G2_MISC, &val);
 	if (err)
 		return err;
 
 	if (port_5_bit)
-		val |= GLOBAL2_MISC_5_BIT_PORT;
+		val |= MV88E6XXX_G2_MISC_5_BIT_PORT;
 	else
-		val &= ~GLOBAL2_MISC_5_BIT_PORT;
+		val &= ~MV88E6XXX_G2_MISC_5_BIT_PORT;
 
-	return mv88e6xxx_g2_write(chip, GLOBAL2_MISC, val);
+	return mv88e6xxx_g2_write(chip, MV88E6XXX_G2_MISC, val);
 }
 
 int mv88e6xxx_g2_misc_4_bit_port(struct mv88e6xxx_chip *chip)
@@ -883,7 +933,7 @@ static irqreturn_t mv88e6xxx_g2_irq_thread_fn(int irq, void *dev_id)
 	u16 reg;
 
 	mutex_lock(&chip->reg_lock);
-	err = mv88e6xxx_g2_read(chip, GLOBAL2_INT_SOURCE, &reg);
+	err = mv88e6xxx_g2_read(chip, MV88E6XXX_G2_INT_SOURCE, &reg);
 	mutex_unlock(&chip->reg_lock);
 	if (err)
 		goto out;
@@ -910,7 +960,7 @@ static void mv88e6xxx_g2_irq_bus_sync_unlock(struct irq_data *d)
 {
 	struct mv88e6xxx_chip *chip = irq_data_get_irq_chip_data(d);
 
-	mv88e6xxx_g2_write(chip, GLOBAL2_INT_MASK, ~chip->g2_irq.masked);
+	mv88e6xxx_g2_write(chip, MV88E6XXX_G2_INT_MASK, ~chip->g2_irq.masked);
 
 	mutex_unlock(&chip->reg_lock);
 }
@@ -977,7 +1027,7 @@ int mv88e6xxx_g2_irq_setup(struct mv88e6xxx_chip *chip)
 	chip->g2_irq.masked = ~0;
 
 	chip->device_irq = irq_find_mapping(chip->g1_irq.domain,
-					    GLOBAL_STATUS_IRQ_DEVICE);
+					    MV88E6XXX_G1_STS_IRQ_DEVICE);
 	if (chip->device_irq < 0) {
 		err = chip->device_irq;
 		goto out;
@@ -1012,11 +1062,11 @@ int mv88e6xxx_g2_setup(struct mv88e6xxx_chip *chip)
 	 * highest, and send all special multicast frames to the CPU
 	 * port at the highest priority.
 	 */
-	reg = GLOBAL2_SWITCH_MGMT_FORCE_FLOW_CTRL_PRI | (0x7 << 4);
+	reg = MV88E6XXX_G2_SWITCH_MGMT_FORCE_FLOW_CTL_PRI | (0x7 << 4);
 	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_MGMT_EN_0X) ||
 	    mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_MGMT_EN_2X))
-		reg |= GLOBAL2_SWITCH_MGMT_RSVD2CPU | 0x7;
-	err = mv88e6xxx_g2_write(chip, GLOBAL2_SWITCH_MGMT, reg);
+		reg |= MV88E6XXX_G2_SWITCH_MGMT_RSVD2CPU | 0x7;
+	err = mv88e6xxx_g2_write(chip, MV88E6XXX_G2_SWITCH_MGMT, reg);
 	if (err)
 		return err;
 
@@ -1030,15 +1080,6 @@ int mv88e6xxx_g2_setup(struct mv88e6xxx_chip *chip)
 	if (err)
 		return err;
 
-	if (mv88e6xxx_has(chip, MV88E6XXX_FLAGS_IRL)) {
-		/* Disable ingress rate limiting by resetting all per port
-		 * ingress rate limit resources to their initial state.
-		 */
-		err = mv88e6xxx_g2_clear_irl(chip);
-			if (err)
-				return err;
-	}
-
 	if (mv88e6xxx_has(chip, MV88E6XXX_FLAG_G2_POT)) {
 		/* Clear the priority override table. */
 		err = mv88e6xxx_g2_clear_pot(chip);
diff --git a/drivers/net/dsa/mv88e6xxx/global2.h b/drivers/net/dsa/mv88e6xxx/global2.h
index 14c0be98e0a4..317ffd8f323d 100644
--- a/drivers/net/dsa/mv88e6xxx/global2.h
+++ b/drivers/net/dsa/mv88e6xxx/global2.h
@@ -1,5 +1,5 @@
 /*
- * Marvell 88E6xxx Switch Global 2 Registers support (device address 0x1C)
+ * Marvell 88E6xxx Switch Global 2 Registers support
  *
  * Copyright (c) 2008 Marvell Semiconductor
  *
@@ -15,7 +15,200 @@
 #ifndef _MV88E6XXX_GLOBAL2_H
 #define _MV88E6XXX_GLOBAL2_H
 
-#include "mv88e6xxx.h"
+#include "chip.h"
+
+#define MV88E6XXX_G2	0x1c
+
+/* Offset 0x00: Interrupt Source Register */
+#define MV88E6XXX_G2_INT_SOURCE			0x00
+#define MV88E6XXX_G2_INT_SOURCE_WATCHDOG	15
+
+/* Offset 0x01: Interrupt Mask Register */
+#define MV88E6XXX_G2_INT_MASK	0x01
+
+/* Offset 0x02: MGMT Enable Register 2x */
+#define MV88E6XXX_G2_MGMT_EN_2X		0x02
+
+/* Offset 0x03: MGMT Enable Register 0x */
+#define MV88E6XXX_G2_MGMT_EN_0X		0x03
+
+/* Offset 0x04: Flow Control Delay Register */
+#define MV88E6XXX_G2_FLOW_CTL	0x04
+
+/* Offset 0x05: Switch Management Register */
+#define MV88E6XXX_G2_SWITCH_MGMT			0x05
+#define MV88E6XXX_G2_SWITCH_MGMT_USE_DOUBLE_TAG_DATA	0x8000
+#define MV88E6XXX_G2_SWITCH_MGMT_PREVENT_LOOPS		0x4000
+#define MV88E6XXX_G2_SWITCH_MGMT_FLOW_CTL_MSG		0x2000
+#define MV88E6XXX_G2_SWITCH_MGMT_FORCE_FLOW_CTL_PRI	0x0080
+#define MV88E6XXX_G2_SWITCH_MGMT_RSVD2CPU		0x0008
+
+/* Offset 0x06: Device Mapping Table Register */
+#define MV88E6XXX_G2_DEVICE_MAPPING		0x06
+#define MV88E6XXX_G2_DEVICE_MAPPING_UPDATE	0x8000
+#define MV88E6XXX_G2_DEVICE_MAPPING_DEV_MASK	0x1f00
+#define MV88E6XXX_G2_DEVICE_MAPPING_PORT_MASK	0x000f
+
+/* Offset 0x07: Trunk Mask Table Register */
+#define MV88E6XXX_G2_TRUNK_MASK			0x07
+#define MV88E6XXX_G2_TRUNK_MASK_UPDATE		0x8000
+#define MV88E6XXX_G2_TRUNK_MASK_NUM_MASK	0x7000
+#define MV88E6XXX_G2_TRUNK_MASK_HASH		0x0800
+
+/* Offset 0x08: Trunk Mapping Table Register */
+#define MV88E6XXX_G2_TRUNK_MAPPING		0x08
+#define MV88E6XXX_G2_TRUNK_MAPPING_UPDATE	0x8000
+#define MV88E6XXX_G2_TRUNK_MAPPING_ID_MASK	0x7800
+
+/* Offset 0x09: Ingress Rate Command Register */
+#define MV88E6XXX_G2_IRL_CMD			0x09
+#define MV88E6XXX_G2_IRL_CMD_BUSY		0x8000
+#define MV88E6352_G2_IRL_CMD_OP_MASK		0x7000
+#define MV88E6352_G2_IRL_CMD_OP_NOOP		0x0000
+#define MV88E6352_G2_IRL_CMD_OP_INIT_ALL	0x1000
+#define MV88E6352_G2_IRL_CMD_OP_INIT_RES	0x2000
+#define MV88E6352_G2_IRL_CMD_OP_WRITE_REG	0x3000
+#define MV88E6352_G2_IRL_CMD_OP_READ_REG	0x4000
+#define MV88E6390_G2_IRL_CMD_OP_MASK		0x6000
+#define MV88E6390_G2_IRL_CMD_OP_READ_REG	0x0000
+#define MV88E6390_G2_IRL_CMD_OP_INIT_ALL	0x2000
+#define MV88E6390_G2_IRL_CMD_OP_INIT_RES	0x4000
+#define MV88E6390_G2_IRL_CMD_OP_WRITE_REG	0x6000
+#define MV88E6352_G2_IRL_CMD_PORT_MASK		0x0f00
+#define MV88E6390_G2_IRL_CMD_PORT_MASK		0x1f00
+#define MV88E6XXX_G2_IRL_CMD_RES_MASK		0x00e0
+#define MV88E6XXX_G2_IRL_CMD_REG_MASK		0x000f
+
+/* Offset 0x0A: Ingress Rate Data Register */
+#define MV88E6XXX_G2_IRL_DATA		0x0a
+#define MV88E6XXX_G2_IRL_DATA_MASK	0xffff
+
+/* Offset 0x0B: Cross-chip Port VLAN Register */
+#define MV88E6XXX_G2_PVT_ADDR			0x0b
+#define MV88E6XXX_G2_PVT_ADDR_BUSY		0x8000
+#define MV88E6XXX_G2_PVT_ADDR_OP_MASK		0x7000
+#define MV88E6XXX_G2_PVT_ADDR_OP_INIT_ONES	0x1000
+#define MV88E6XXX_G2_PVT_ADDR_OP_WRITE_PVLAN	0x3000
+#define MV88E6XXX_G2_PVT_ADDR_OP_READ		0x4000
+#define MV88E6XXX_G2_PVT_ADDR_PTR_MASK		0x01ff
+
+/* Offset 0x0C: Cross-chip Port VLAN Data Register */
+#define MV88E6XXX_G2_PVT_DATA		0x0c
+#define MV88E6XXX_G2_PVT_DATA_MASK	0x7f
+
+/* Offset 0x0D: Switch MAC/WoL/WoF Register */
+#define MV88E6XXX_G2_SWITCH_MAC			0x0d
+#define MV88E6XXX_G2_SWITCH_MAC_UPDATE		0x8000
+#define MV88E6XXX_G2_SWITCH_MAC_PTR_MASK	0x1f00
+#define MV88E6XXX_G2_SWITCH_MAC_DATA_MASK	0x00ff
+
+/* Offset 0x0E: ATU Stats Register */
+#define MV88E6XXX_G2_ATU_STATS		0x0e
+
+/* Offset 0x0F: Priority Override Table */
+#define MV88E6XXX_G2_PRIO_OVERRIDE		0x0f
+#define MV88E6XXX_G2_PRIO_OVERRIDE_UPDATE	0x8000
+#define MV88E6XXX_G2_PRIO_OVERRIDE_FPRISET	0x1000
+#define MV88E6XXX_G2_PRIO_OVERRIDE_PTR_MASK	0x0f00
+#define MV88E6352_G2_PRIO_OVERRIDE_QPRIAVBEN	0x0080
+#define MV88E6352_G2_PRIO_OVERRIDE_DATAAVB_MASK	0x0030
+#define MV88E6XXX_G2_PRIO_OVERRIDE_QFPRIEN	0x0008
+#define MV88E6XXX_G2_PRIO_OVERRIDE_DATA_MASK	0x0007
+
+/* Offset 0x14: EEPROM Command */
+#define MV88E6XXX_G2_EEPROM_CMD			0x14
+#define MV88E6XXX_G2_EEPROM_CMD_BUSY		0x8000
+#define MV88E6XXX_G2_EEPROM_CMD_OP_MASK		0x7000
+#define MV88E6XXX_G2_EEPROM_CMD_OP_WRITE	0x3000
+#define MV88E6XXX_G2_EEPROM_CMD_OP_READ		0x4000
+#define MV88E6XXX_G2_EEPROM_CMD_OP_LOAD		0x6000
+#define MV88E6XXX_G2_EEPROM_CMD_RUNNING		0x0800
+#define MV88E6XXX_G2_EEPROM_CMD_WRITE_EN	0x0400
+#define MV88E6352_G2_EEPROM_CMD_ADDR_MASK	0x00ff
+#define MV88E6390_G2_EEPROM_CMD_DATA_MASK	0x00ff
+
+/* Offset 0x15: EEPROM Data */
+#define MV88E6352_G2_EEPROM_DATA	0x15
+#define MV88E6352_G2_EEPROM_DATA_MASK	0xffff
+
+/* Offset 0x15: EEPROM Addr */
+#define MV88E6390_G2_EEPROM_ADDR	0x15
+#define MV88E6390_G2_EEPROM_ADDR_MASK	0xffff
+
+/* Offset 0x16: AVB Command Register */
+#define MV88E6352_G2_AVB_CMD		0x16
+
+/* Offset 0x17: AVB Data Register */
+#define MV88E6352_G2_AVB_DATA		0x17
+
+/* Offset 0x18: SMI PHY Command Register */
+#define MV88E6XXX_G2_SMI_PHY_CMD			0x18
+#define MV88E6XXX_G2_SMI_PHY_CMD_BUSY			0x8000
+#define MV88E6390_G2_SMI_PHY_CMD_FUNC_MASK		0x6000
+#define MV88E6390_G2_SMI_PHY_CMD_FUNC_INTERNAL		0x0000
+#define MV88E6390_G2_SMI_PHY_CMD_FUNC_EXTERNAL		0x2000
+#define MV88E6390_G2_SMI_PHY_CMD_FUNC_SETUP		0x4000
+#define MV88E6XXX_G2_SMI_PHY_CMD_MODE_MASK		0x1000
+#define MV88E6XXX_G2_SMI_PHY_CMD_MODE_45		0x0000
+#define MV88E6XXX_G2_SMI_PHY_CMD_MODE_22		0x1000
+#define MV88E6XXX_G2_SMI_PHY_CMD_OP_MASK		0x0c00
+#define MV88E6XXX_G2_SMI_PHY_CMD_OP_22_WRITE_DATA	0x0400
+#define MV88E6XXX_G2_SMI_PHY_CMD_OP_22_READ_DATA	0x0800
+#define MV88E6XXX_G2_SMI_PHY_CMD_OP_45_WRITE_ADDR	0x0000
+#define MV88E6XXX_G2_SMI_PHY_CMD_OP_45_WRITE_DATA	0x0400
+#define MV88E6XXX_G2_SMI_PHY_CMD_OP_45_READ_DATA_INC	0x0800
+#define MV88E6XXX_G2_SMI_PHY_CMD_OP_45_READ_DATA	0x0c00
+#define MV88E6XXX_G2_SMI_PHY_CMD_DEV_ADDR_MASK		0x03e0
+#define MV88E6XXX_G2_SMI_PHY_CMD_REG_ADDR_MASK		0x001f
+#define MV88E6XXX_G2_SMI_PHY_CMD_SETUP_PTR_MASK		0x03ff
+
+/* Offset 0x19: SMI PHY Data Register */
+#define MV88E6XXX_G2_SMI_PHY_DATA	0x19
+
+/* Offset 0x1A: Scratch and Misc. Register */
+#define MV88E6XXX_G2_SCRATCH_MISC_MISC		0x1a
+#define MV88E6XXX_G2_SCRATCH_MISC_UPDATE	0x8000
+#define MV88E6XXX_G2_SCRATCH_MISC_PTR_MASK	0x7f00
+#define MV88E6XXX_G2_SCRATCH_MISC_DATA_MASK	0x00ff
+
+/* Offset 0x1B: Watch Dog Control Register */
+#define MV88E6352_G2_WDOG_CTL			0x1b
+#define MV88E6352_G2_WDOG_CTL_EGRESS_EVENT	0x0080
+#define MV88E6352_G2_WDOG_CTL_RMU_TIMEOUT	0x0040
+#define MV88E6352_G2_WDOG_CTL_QC_ENABLE		0x0020
+#define MV88E6352_G2_WDOG_CTL_EGRESS_HISTORY	0x0010
+#define MV88E6352_G2_WDOG_CTL_EGRESS_ENABLE	0x0008
+#define MV88E6352_G2_WDOG_CTL_FORCE_IRQ		0x0004
+#define MV88E6352_G2_WDOG_CTL_HISTORY		0x0002
+#define MV88E6352_G2_WDOG_CTL_SWRESET		0x0001
+
+/* Offset 0x1B: Watch Dog Control Register */
+#define MV88E6390_G2_WDOG_CTL				0x1b
+#define MV88E6390_G2_WDOG_CTL_UPDATE			0x8000
+#define MV88E6390_G2_WDOG_CTL_PTR_MASK			0x7f00
+#define MV88E6390_G2_WDOG_CTL_PTR_INT_SOURCE		0x0000
+#define MV88E6390_G2_WDOG_CTL_PTR_INT_STS		0x1000
+#define MV88E6390_G2_WDOG_CTL_PTR_INT_ENABLE		0x1100
+#define MV88E6390_G2_WDOG_CTL_PTR_EVENT			0x1200
+#define MV88E6390_G2_WDOG_CTL_PTR_HISTORY		0x1300
+#define MV88E6390_G2_WDOG_CTL_DATA_MASK			0x00ff
+#define MV88E6390_G2_WDOG_CTL_CUT_THROUGH		0x0008
+#define MV88E6390_G2_WDOG_CTL_QUEUE_CONTROLLER		0x0004
+#define MV88E6390_G2_WDOG_CTL_EGRESS			0x0002
+#define MV88E6390_G2_WDOG_CTL_FORCE_IRQ			0x0001
+
+/* Offset 0x1C: QoS Weights Register */
+#define MV88E6XXX_G2_QOS_WEIGHTS		0x1c
+#define MV88E6XXX_G2_QOS_WEIGHTS_UPDATE		0x8000
+#define MV88E6352_G2_QOS_WEIGHTS_PTR_MASK	0x3f00
+#define MV88E6390_G2_QOS_WEIGHTS_PTR_MASK	0x7f00
+#define MV88E6XXX_G2_QOS_WEIGHTS_DATA_MASK	0x00ff
+
+/* Offset 0x1D: Misc Register */
+#define MV88E6XXX_G2_MISC		0x1d
+#define MV88E6XXX_G2_MISC_5_BIT_PORT	0x4000
+#define MV88E6352_G2_NOEGR_POLICY	0x2000
+#define MV88E6390_G2_LAG_ID_4		0x2000
 
 #ifdef CONFIG_NET_DSA_MV88E6XXX_GLOBAL2
 
@@ -24,6 +217,9 @@ static inline int mv88e6xxx_g2_require(struct mv88e6xxx_chip *chip)
 	return 0;
 }
 
+int mv88e6352_g2_irl_init_all(struct mv88e6xxx_chip *chip, int port);
+int mv88e6390_g2_irl_init_all(struct mv88e6xxx_chip *chip, int port);
+
 int mv88e6xxx_g2_smi_phy_read(struct mv88e6xxx_chip *chip,
 			      struct mii_bus *bus,
 			      int addr, int reg, u16 *val);
@@ -66,6 +262,18 @@ static inline int mv88e6xxx_g2_require(struct mv88e6xxx_chip *chip)
 	return 0;
 }
 
+static inline int mv88e6352_g2_irl_init_all(struct mv88e6xxx_chip *chip,
+					    int port)
+{
+	return -EOPNOTSUPP;
+}
+
+static inline int mv88e6390_g2_irl_init_all(struct mv88e6xxx_chip *chip,
+					    int port)
+{
+	return -EOPNOTSUPP;
+}
+
 static inline int mv88e6xxx_g2_smi_phy_read(struct mv88e6xxx_chip *chip,
 					    struct mii_bus *bus,
 					    int addr, int reg, u16 *val)
diff --git a/drivers/net/dsa/mv88e6xxx/mv88e6xxx.h b/drivers/net/dsa/mv88e6xxx/mv88e6xxx.h
deleted file mode 100644
index 77236cd72df2..000000000000
--- a/drivers/net/dsa/mv88e6xxx/mv88e6xxx.h
+++ /dev/null
@@ -1,946 +0,0 @@
-/*
- * Marvell 88e6xxx common definitions
- *
- * Copyright (c) 2008 Marvell Semiconductor
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#ifndef __MV88E6XXX_H
-#define __MV88E6XXX_H
-
-#include <linux/if_vlan.h>
-#include <linux/irq.h>
-#include <linux/gpio/consumer.h>
-#include <linux/phy.h>
-#include <net/dsa.h>
-
-#ifndef UINT64_MAX
-#define UINT64_MAX		(u64)(~((u64)0))
-#endif
-
-#define SMI_CMD			0x00
-#define SMI_CMD_BUSY		BIT(15)
-#define SMI_CMD_CLAUSE_22	BIT(12)
-#define SMI_CMD_OP_22_WRITE	((1 << 10) | SMI_CMD_BUSY | SMI_CMD_CLAUSE_22)
-#define SMI_CMD_OP_22_READ	((2 << 10) | SMI_CMD_BUSY | SMI_CMD_CLAUSE_22)
-#define SMI_CMD_OP_45_WRITE_ADDR	((0 << 10) | SMI_CMD_BUSY)
-#define SMI_CMD_OP_45_WRITE_DATA	((1 << 10) | SMI_CMD_BUSY)
-#define SMI_CMD_OP_45_READ_DATA		((2 << 10) | SMI_CMD_BUSY)
-#define SMI_CMD_OP_45_READ_DATA_INC	((3 << 10) | SMI_CMD_BUSY)
-#define SMI_DATA		0x01
-
-/* PHY Registers */
-#define PHY_PAGE		0x16
-#define PHY_PAGE_COPPER		0x00
-
-#define ADDR_SERDES		0x0f
-#define SERDES_PAGE_FIBER	0x01
-
-#define PORT_STATUS		0x00
-#define PORT_STATUS_PAUSE_EN	BIT(15)
-#define PORT_STATUS_MY_PAUSE	BIT(14)
-#define PORT_STATUS_HD_FLOW	BIT(13)
-#define PORT_STATUS_PHY_DETECT	BIT(12)
-#define PORT_STATUS_LINK	BIT(11)
-#define PORT_STATUS_DUPLEX	BIT(10)
-#define PORT_STATUS_SPEED_MASK	0x0300
-#define PORT_STATUS_SPEED_10	0x0000
-#define PORT_STATUS_SPEED_100	0x0100
-#define PORT_STATUS_SPEED_1000	0x0200
-#define PORT_STATUS_EEE		BIT(6) /* 6352 */
-#define PORT_STATUS_AM_DIS	BIT(6) /* 6165 */
-#define PORT_STATUS_MGMII	BIT(6) /* 6185 */
-#define PORT_STATUS_TX_PAUSED	BIT(5)
-#define PORT_STATUS_FLOW_CTRL	BIT(4)
-#define PORT_STATUS_CMODE_MASK	0x0f
-#define PORT_STATUS_CMODE_100BASE_X	0x8
-#define PORT_STATUS_CMODE_1000BASE_X	0x9
-#define PORT_STATUS_CMODE_SGMII		0xa
-#define PORT_STATUS_CMODE_2500BASEX	0xb
-#define PORT_STATUS_CMODE_XAUI		0xc
-#define PORT_STATUS_CMODE_RXAUI		0xd
-#define PORT_PCS_CTRL		0x01
-#define PORT_PCS_CTRL_RGMII_DELAY_RXCLK	BIT(15)
-#define PORT_PCS_CTRL_RGMII_DELAY_TXCLK	BIT(14)
-#define PORT_PCS_CTRL_FORCE_SPEED	BIT(13) /* 6390 */
-#define PORT_PCS_CTRL_ALTSPEED		BIT(12) /* 6390 */
-#define PORT_PCS_CTRL_200BASE		BIT(12) /* 6352 */
-#define PORT_PCS_CTRL_FC		BIT(7)
-#define PORT_PCS_CTRL_FORCE_FC		BIT(6)
-#define PORT_PCS_CTRL_LINK_UP		BIT(5)
-#define PORT_PCS_CTRL_FORCE_LINK	BIT(4)
-#define PORT_PCS_CTRL_DUPLEX_FULL	BIT(3)
-#define PORT_PCS_CTRL_FORCE_DUPLEX	BIT(2)
-#define PORT_PCS_CTRL_SPEED_MASK	(0x03)
-#define PORT_PCS_CTRL_SPEED_10		(0x00)
-#define PORT_PCS_CTRL_SPEED_100		(0x01)
-#define PORT_PCS_CTRL_SPEED_200		(0x02) /* 6065 and non Gb chips */
-#define PORT_PCS_CTRL_SPEED_1000	(0x02)
-#define PORT_PCS_CTRL_SPEED_10000	(0x03) /* 6390X */
-#define PORT_PCS_CTRL_SPEED_UNFORCED	(0x03)
-#define PORT_PAUSE_CTRL		0x02
-#define PORT_FLOW_CTRL_LIMIT_IN		((0x00 << 8) | BIT(15))
-#define PORT_FLOW_CTRL_LIMIT_OUT	((0x01 << 8) | BIT(15))
-#define PORT_SWITCH_ID		0x03
-#define PORT_SWITCH_ID_PROD_NUM_6085	0x04a
-#define PORT_SWITCH_ID_PROD_NUM_6095	0x095
-#define PORT_SWITCH_ID_PROD_NUM_6097	0x099
-#define PORT_SWITCH_ID_PROD_NUM_6131	0x106
-#define PORT_SWITCH_ID_PROD_NUM_6320	0x115
-#define PORT_SWITCH_ID_PROD_NUM_6123	0x121
-#define PORT_SWITCH_ID_PROD_NUM_6141	0x340
-#define PORT_SWITCH_ID_PROD_NUM_6161	0x161
-#define PORT_SWITCH_ID_PROD_NUM_6165	0x165
-#define PORT_SWITCH_ID_PROD_NUM_6171	0x171
-#define PORT_SWITCH_ID_PROD_NUM_6172	0x172
-#define PORT_SWITCH_ID_PROD_NUM_6175	0x175
-#define PORT_SWITCH_ID_PROD_NUM_6176	0x176
-#define PORT_SWITCH_ID_PROD_NUM_6185	0x1a7
-#define PORT_SWITCH_ID_PROD_NUM_6190	0x190
-#define PORT_SWITCH_ID_PROD_NUM_6190X	0x0a0
-#define PORT_SWITCH_ID_PROD_NUM_6191	0x191
-#define PORT_SWITCH_ID_PROD_NUM_6240	0x240
-#define PORT_SWITCH_ID_PROD_NUM_6290	0x290
-#define PORT_SWITCH_ID_PROD_NUM_6321	0x310
-#define PORT_SWITCH_ID_PROD_NUM_6341	0x341
-#define PORT_SWITCH_ID_PROD_NUM_6352	0x352
-#define PORT_SWITCH_ID_PROD_NUM_6350	0x371
-#define PORT_SWITCH_ID_PROD_NUM_6351	0x375
-#define PORT_SWITCH_ID_PROD_NUM_6390	0x390
-#define PORT_SWITCH_ID_PROD_NUM_6390X	0x0a1
-#define PORT_CONTROL		0x04
-#define PORT_CONTROL_USE_CORE_TAG	BIT(15)
-#define PORT_CONTROL_DROP_ON_LOCK	BIT(14)
-#define PORT_CONTROL_EGRESS_UNMODIFIED	(0x0 << 12)
-#define PORT_CONTROL_EGRESS_UNTAGGED	(0x1 << 12)
-#define PORT_CONTROL_EGRESS_TAGGED	(0x2 << 12)
-#define PORT_CONTROL_EGRESS_ADD_TAG	(0x3 << 12)
-#define PORT_CONTROL_EGRESS_MASK	(0x3 << 12)
-#define PORT_CONTROL_HEADER		BIT(11)
-#define PORT_CONTROL_IGMP_MLD_SNOOP	BIT(10)
-#define PORT_CONTROL_DOUBLE_TAG		BIT(9)
-#define PORT_CONTROL_FRAME_MODE_NORMAL		(0x0 << 8)
-#define PORT_CONTROL_FRAME_MODE_DSA		(0x1 << 8)
-#define PORT_CONTROL_FRAME_MODE_PROVIDER	(0x2 << 8)
-#define PORT_CONTROL_FRAME_ETHER_TYPE_DSA	(0x3 << 8)
-#define PORT_CONTROL_FRAME_MASK			(0x3 << 8)
-#define PORT_CONTROL_DSA_TAG		BIT(8)
-#define PORT_CONTROL_VLAN_TUNNEL	BIT(7)
-#define PORT_CONTROL_TAG_IF_BOTH	BIT(6)
-#define PORT_CONTROL_USE_IP		BIT(5)
-#define PORT_CONTROL_USE_TAG		BIT(4)
-#define PORT_CONTROL_FORWARD_UNKNOWN	BIT(2)
-#define PORT_CONTROL_EGRESS_FLOODS_MASK			(0x3 << 2)
-#define PORT_CONTROL_EGRESS_FLOODS_NO_UNKNOWN_DA	(0x0 << 2)
-#define PORT_CONTROL_EGRESS_FLOODS_NO_UNKNOWN_MC_DA	(0x1 << 2)
-#define PORT_CONTROL_EGRESS_FLOODS_NO_UNKNOWN_UC_DA	(0x2 << 2)
-#define PORT_CONTROL_EGRESS_FLOODS_ALL_UNKNOWN_DA	(0x3 << 2)
-#define PORT_CONTROL_STATE_MASK		0x03
-#define PORT_CONTROL_STATE_DISABLED	0x00
-#define PORT_CONTROL_STATE_BLOCKING	0x01
-#define PORT_CONTROL_STATE_LEARNING	0x02
-#define PORT_CONTROL_STATE_FORWARDING	0x03
-#define PORT_CONTROL_1		0x05
-#define PORT_CONTROL_1_MESSAGE_PORT	BIT(15)
-#define PORT_CONTROL_1_FID_11_4_MASK	(0xff << 0)
-#define PORT_BASE_VLAN		0x06
-#define PORT_BASE_VLAN_FID_3_0_MASK	(0xf << 12)
-#define PORT_DEFAULT_VLAN	0x07
-#define PORT_DEFAULT_VLAN_MASK	0xfff
-#define PORT_CONTROL_2		0x08
-#define PORT_CONTROL_2_IGNORE_FCS	BIT(15)
-#define PORT_CONTROL_2_VTU_PRI_OVERRIDE	BIT(14)
-#define PORT_CONTROL_2_SA_PRIO_OVERRIDE	BIT(13)
-#define PORT_CONTROL_2_DA_PRIO_OVERRIDE	BIT(12)
-#define PORT_CONTROL_2_JUMBO_1522	(0x00 << 12)
-#define PORT_CONTROL_2_JUMBO_2048	(0x01 << 12)
-#define PORT_CONTROL_2_JUMBO_10240	(0x02 << 12)
-#define PORT_CONTROL_2_8021Q_MASK	(0x03 << 10)
-#define PORT_CONTROL_2_8021Q_DISABLED	(0x00 << 10)
-#define PORT_CONTROL_2_8021Q_FALLBACK	(0x01 << 10)
-#define PORT_CONTROL_2_8021Q_CHECK	(0x02 << 10)
-#define PORT_CONTROL_2_8021Q_SECURE	(0x03 << 10)
-#define PORT_CONTROL_2_DISCARD_TAGGED	BIT(9)
-#define PORT_CONTROL_2_DISCARD_UNTAGGED	BIT(8)
-#define PORT_CONTROL_2_MAP_DA		BIT(7)
-#define PORT_CONTROL_2_DEFAULT_FORWARD	BIT(6)
-#define PORT_CONTROL_2_EGRESS_MONITOR	BIT(5)
-#define PORT_CONTROL_2_INGRESS_MONITOR	BIT(4)
-#define PORT_CONTROL_2_UPSTREAM_MASK	0x0f
-#define PORT_RATE_CONTROL	0x09
-#define PORT_RATE_CONTROL_2	0x0a
-#define PORT_ASSOC_VECTOR	0x0b
-#define PORT_ASSOC_VECTOR_HOLD_AT_1		BIT(15)
-#define PORT_ASSOC_VECTOR_INT_AGE_OUT		BIT(14)
-#define PORT_ASSOC_VECTOR_LOCKED_PORT		BIT(13)
-#define PORT_ASSOC_VECTOR_IGNORE_WRONG		BIT(12)
-#define PORT_ASSOC_VECTOR_REFRESH_LOCKED	BIT(11)
-#define PORT_ATU_CONTROL	0x0c
-#define PORT_PRI_OVERRIDE	0x0d
-#define PORT_ETH_TYPE		0x0f
-#define PORT_ETH_TYPE_DEFAULT	0x9100
-#define PORT_IN_DISCARD_LO	0x10
-#define PORT_IN_DISCARD_HI	0x11
-#define PORT_IN_FILTERED	0x12
-#define PORT_OUT_FILTERED	0x13
-#define PORT_TAG_REGMAP_0123	0x18
-#define PORT_TAG_REGMAP_4567	0x19
-#define PORT_IEEE_PRIO_MAP_TABLE	0x18    /* 6390 */
-#define PORT_IEEE_PRIO_MAP_TABLE_UPDATE		BIT(15)
-#define PORT_IEEE_PRIO_MAP_TABLE_INGRESS_PCP		(0x0 << 12)
-#define PORT_IEEE_PRIO_MAP_TABLE_EGRESS_GREEN_PCP	(0x1 << 12)
-#define PORT_IEEE_PRIO_MAP_TABLE_EGRESS_YELLOW_PCP	(0x2 << 12)
-#define PORT_IEEE_PRIO_MAP_TABLE_EGRESS_AVB_PCP		(0x3 << 12)
-#define PORT_IEEE_PRIO_MAP_TABLE_EGRESS_GREEN_DSCP	(0x5 << 12)
-#define PORT_IEEE_PRIO_MAP_TABLE_EGRESS_YELLOW_DSCP	(0x6 << 12)
-#define PORT_IEEE_PRIO_MAP_TABLE_EGRESS_AVB_DSCP	(0x7 << 12)
-#define PORT_IEEE_PRIO_MAP_TABLE_POINTER_SHIFT		9
-
-#define GLOBAL_STATUS		0x00
-#define GLOBAL_STATUS_PPU_STATE BIT(15) /* 6351 and 6171 */
-#define GLOBAL_STATUS_PPU_STATE_MASK		(0x3 << 14) /* 6165 6185 */
-#define GLOBAL_STATUS_PPU_STATE_DISABLED_RST	(0x0 << 14)
-#define GLOBAL_STATUS_PPU_STATE_INITIALIZING	(0x1 << 14)
-#define GLOBAL_STATUS_PPU_STATE_DISABLED	(0x2 << 14)
-#define GLOBAL_STATUS_PPU_STATE_POLLING		(0x3 << 14)
-#define GLOBAL_STATUS_INIT_READY	BIT(11)
-#define GLOBAL_STATUS_IRQ_AVB		8
-#define GLOBAL_STATUS_IRQ_DEVICE	7
-#define GLOBAL_STATUS_IRQ_STATS		6
-#define GLOBAL_STATUS_IRQ_VTU_PROBLEM	5
-#define GLOBAL_STATUS_IRQ_VTU_DONE	4
-#define GLOBAL_STATUS_IRQ_ATU_PROBLEM	3
-#define GLOBAL_STATUS_IRQ_ATU_DONE	2
-#define GLOBAL_STATUS_IRQ_TCAM_DONE	1
-#define GLOBAL_STATUS_IRQ_EEPROM_DONE	0
-#define GLOBAL_MAC_01		0x01
-#define GLOBAL_MAC_23		0x02
-#define GLOBAL_MAC_45		0x03
-#define GLOBAL_ATU_FID		0x01
-#define GLOBAL_VTU_FID		0x02
-#define GLOBAL_VTU_FID_MASK	0xfff
-#define GLOBAL_VTU_SID		0x03	/* 6097 6165 6351 6352 */
-#define GLOBAL_VTU_SID_MASK	0x3f
-#define GLOBAL_CONTROL		0x04
-#define GLOBAL_CONTROL_SW_RESET		BIT(15)
-#define GLOBAL_CONTROL_PPU_ENABLE	BIT(14)
-#define GLOBAL_CONTROL_DISCARD_EXCESS	BIT(13) /* 6352 */
-#define GLOBAL_CONTROL_SCHED_PRIO	BIT(11) /* 6152 */
-#define GLOBAL_CONTROL_MAX_FRAME_1632	BIT(10) /* 6152 */
-#define GLOBAL_CONTROL_RELOAD_EEPROM	BIT(9)	/* 6152 */
-#define GLOBAL_CONTROL_DEVICE_EN	BIT(7)
-#define GLOBAL_CONTROL_STATS_DONE_EN	BIT(6)
-#define GLOBAL_CONTROL_VTU_PROBLEM_EN	BIT(5)
-#define GLOBAL_CONTROL_VTU_DONE_EN	BIT(4)
-#define GLOBAL_CONTROL_ATU_PROBLEM_EN	BIT(3)
-#define GLOBAL_CONTROL_ATU_DONE_EN	BIT(2)
-#define GLOBAL_CONTROL_TCAM_EN		BIT(1)
-#define GLOBAL_CONTROL_EEPROM_DONE_EN	BIT(0)
-#define GLOBAL_VTU_OP		0x05
-#define GLOBAL_VTU_OP_BUSY	BIT(15)
-#define GLOBAL_VTU_OP_FLUSH_ALL		((0x01 << 12) | GLOBAL_VTU_OP_BUSY)
-#define GLOBAL_VTU_OP_VTU_LOAD_PURGE	((0x03 << 12) | GLOBAL_VTU_OP_BUSY)
-#define GLOBAL_VTU_OP_VTU_GET_NEXT	((0x04 << 12) | GLOBAL_VTU_OP_BUSY)
-#define GLOBAL_VTU_OP_STU_LOAD_PURGE	((0x05 << 12) | GLOBAL_VTU_OP_BUSY)
-#define GLOBAL_VTU_OP_STU_GET_NEXT	((0x06 << 12) | GLOBAL_VTU_OP_BUSY)
-#define GLOBAL_VTU_VID		0x06
-#define GLOBAL_VTU_VID_MASK	0xfff
-#define GLOBAL_VTU_VID_PAGE	BIT(13)
-#define GLOBAL_VTU_VID_VALID	BIT(12)
-#define GLOBAL_VTU_DATA_0_3	0x07
-#define GLOBAL_VTU_DATA_4_7	0x08
-#define GLOBAL_VTU_DATA_8_11	0x09
-#define GLOBAL_VTU_STU_DATA_MASK		0x03
-#define GLOBAL_VTU_DATA_MEMBER_TAG_UNMODIFIED	0x00
-#define GLOBAL_VTU_DATA_MEMBER_TAG_UNTAGGED	0x01
-#define GLOBAL_VTU_DATA_MEMBER_TAG_TAGGED	0x02
-#define GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER	0x03
-#define GLOBAL_STU_DATA_PORT_STATE_DISABLED	0x00
-#define GLOBAL_STU_DATA_PORT_STATE_BLOCKING	0x01
-#define GLOBAL_STU_DATA_PORT_STATE_LEARNING	0x02
-#define GLOBAL_STU_DATA_PORT_STATE_FORWARDING	0x03
-#define GLOBAL_ATU_CONTROL	0x0a
-#define GLOBAL_ATU_CONTROL_LEARN2ALL	BIT(3)
-#define GLOBAL_ATU_OP		0x0b
-#define GLOBAL_ATU_OP_BUSY	BIT(15)
-#define GLOBAL_ATU_OP_NOP		(0 << 12)
-#define GLOBAL_ATU_OP_FLUSH_MOVE_ALL		((1 << 12) | GLOBAL_ATU_OP_BUSY)
-#define GLOBAL_ATU_OP_FLUSH_MOVE_NON_STATIC	((2 << 12) | GLOBAL_ATU_OP_BUSY)
-#define GLOBAL_ATU_OP_LOAD_DB		((3 << 12) | GLOBAL_ATU_OP_BUSY)
-#define GLOBAL_ATU_OP_GET_NEXT_DB	((4 << 12) | GLOBAL_ATU_OP_BUSY)
-#define GLOBAL_ATU_OP_FLUSH_MOVE_ALL_DB		((5 << 12) | GLOBAL_ATU_OP_BUSY)
-#define GLOBAL_ATU_OP_FLUSH_MOVE_NON_STATIC_DB ((6 << 12) | GLOBAL_ATU_OP_BUSY)
-#define GLOBAL_ATU_OP_GET_CLR_VIOLATION	  ((7 << 12) | GLOBAL_ATU_OP_BUSY)
-#define GLOBAL_ATU_DATA		0x0c
-#define GLOBAL_ATU_DATA_TRUNK			BIT(15)
-#define GLOBAL_ATU_DATA_TRUNK_ID_MASK		0x00f0
-#define GLOBAL_ATU_DATA_TRUNK_ID_SHIFT		4
-#define GLOBAL_ATU_DATA_PORT_VECTOR_MASK	0x3ff0
-#define GLOBAL_ATU_DATA_PORT_VECTOR_SHIFT	4
-#define GLOBAL_ATU_DATA_STATE_MASK		0x0f
-#define GLOBAL_ATU_DATA_STATE_UNUSED		0x00
-#define GLOBAL_ATU_DATA_STATE_UC_MGMT		0x0d
-#define GLOBAL_ATU_DATA_STATE_UC_STATIC		0x0e
-#define GLOBAL_ATU_DATA_STATE_UC_PRIO_OVER	0x0f
-#define GLOBAL_ATU_DATA_STATE_MC_NONE_RATE	0x05
-#define GLOBAL_ATU_DATA_STATE_MC_STATIC		0x07
-#define GLOBAL_ATU_DATA_STATE_MC_MGMT		0x0e
-#define GLOBAL_ATU_DATA_STATE_MC_PRIO_OVER	0x0f
-#define GLOBAL_ATU_MAC_01	0x0d
-#define GLOBAL_ATU_MAC_23	0x0e
-#define GLOBAL_ATU_MAC_45	0x0f
-#define GLOBAL_IP_PRI_0		0x10
-#define GLOBAL_IP_PRI_1		0x11
-#define GLOBAL_IP_PRI_2		0x12
-#define GLOBAL_IP_PRI_3		0x13
-#define GLOBAL_IP_PRI_4		0x14
-#define GLOBAL_IP_PRI_5		0x15
-#define GLOBAL_IP_PRI_6		0x16
-#define GLOBAL_IP_PRI_7		0x17
-#define GLOBAL_IEEE_PRI		0x18
-#define GLOBAL_CORE_TAG_TYPE	0x19
-#define GLOBAL_MONITOR_CONTROL	0x1a
-#define GLOBAL_MONITOR_CONTROL_INGRESS_SHIFT	12
-#define GLOBAL_MONITOR_CONTROL_INGRESS_MASK	(0xf << 12)
-#define GLOBAL_MONITOR_CONTROL_EGRESS_SHIFT	8
-#define GLOBAL_MONITOR_CONTROL_EGRESS_MASK	(0xf << 8)
-#define GLOBAL_MONITOR_CONTROL_ARP_SHIFT	4
-#define GLOBAL_MONITOR_CONTROL_ARP_MASK	        (0xf << 4)
-#define GLOBAL_MONITOR_CONTROL_MIRROR_SHIFT	0
-#define GLOBAL_MONITOR_CONTROL_ARP_DISABLED	(0xf0)
-#define GLOBAL_MONITOR_CONTROL_UPDATE			BIT(15)
-#define GLOBAL_MONITOR_CONTROL_0180C280000000XLO	(0x00 << 8)
-#define GLOBAL_MONITOR_CONTROL_0180C280000000XHI	(0x01 << 8)
-#define GLOBAL_MONITOR_CONTROL_0180C280000002XLO	(0x02 << 8)
-#define GLOBAL_MONITOR_CONTROL_0180C280000002XHI	(0x03 << 8)
-#define GLOBAL_MONITOR_CONTROL_INGRESS			(0x20 << 8)
-#define GLOBAL_MONITOR_CONTROL_EGRESS			(0x21 << 8)
-#define GLOBAL_MONITOR_CONTROL_CPU_DEST			(0x30 << 8)
-#define GLOBAL_CONTROL_2	0x1c
-#define GLOBAL_CONTROL_2_NO_CASCADE		0xe000
-#define GLOBAL_CONTROL_2_MULTIPLE_CASCADE	0xf000
-#define GLOBAL_CONTROL_2_HIST_RX	       (0x1 << 6)
-#define GLOBAL_CONTROL_2_HIST_TX	       (0x2 << 6)
-#define GLOBAL_CONTROL_2_HIST_RX_TX	       (0x3 << 6)
-#define GLOBAL_STATS_OP		0x1d
-#define GLOBAL_STATS_OP_BUSY	BIT(15)
-#define GLOBAL_STATS_OP_NOP		(0 << 12)
-#define GLOBAL_STATS_OP_FLUSH_ALL	((1 << 12) | GLOBAL_STATS_OP_BUSY)
-#define GLOBAL_STATS_OP_FLUSH_PORT	((2 << 12) | GLOBAL_STATS_OP_BUSY)
-#define GLOBAL_STATS_OP_READ_CAPTURED	((4 << 12) | GLOBAL_STATS_OP_BUSY)
-#define GLOBAL_STATS_OP_CAPTURE_PORT	((5 << 12) | GLOBAL_STATS_OP_BUSY)
-#define GLOBAL_STATS_OP_HIST_RX		((1 << 10) | GLOBAL_STATS_OP_BUSY)
-#define GLOBAL_STATS_OP_HIST_TX		((2 << 10) | GLOBAL_STATS_OP_BUSY)
-#define GLOBAL_STATS_OP_HIST_RX_TX	((3 << 10) | GLOBAL_STATS_OP_BUSY)
-#define GLOBAL_STATS_OP_BANK_1_BIT_9	BIT(9)
-#define GLOBAL_STATS_OP_BANK_1_BIT_10	BIT(10)
-#define GLOBAL_STATS_COUNTER_32	0x1e
-#define GLOBAL_STATS_COUNTER_01	0x1f
-
-#define GLOBAL2_INT_SOURCE	0x00
-#define GLOBAL2_INT_SOURCE_WATCHDOG	15
-#define GLOBAL2_INT_MASK	0x01
-#define GLOBAL2_MGMT_EN_2X	0x02
-#define GLOBAL2_MGMT_EN_0X	0x03
-#define GLOBAL2_FLOW_CONTROL	0x04
-#define GLOBAL2_SWITCH_MGMT	0x05
-#define GLOBAL2_SWITCH_MGMT_USE_DOUBLE_TAG_DATA	BIT(15)
-#define GLOBAL2_SWITCH_MGMT_PREVENT_LOOPS	BIT(14)
-#define GLOBAL2_SWITCH_MGMT_FLOW_CONTROL_MSG	BIT(13)
-#define GLOBAL2_SWITCH_MGMT_FORCE_FLOW_CTRL_PRI	BIT(7)
-#define GLOBAL2_SWITCH_MGMT_RSVD2CPU		BIT(3)
-#define GLOBAL2_DEVICE_MAPPING	0x06
-#define GLOBAL2_DEVICE_MAPPING_UPDATE		BIT(15)
-#define GLOBAL2_DEVICE_MAPPING_TARGET_SHIFT	8
-#define GLOBAL2_DEVICE_MAPPING_PORT_MASK	0x0f
-#define GLOBAL2_TRUNK_MASK	0x07
-#define GLOBAL2_TRUNK_MASK_UPDATE		BIT(15)
-#define GLOBAL2_TRUNK_MASK_NUM_SHIFT		12
-#define GLOBAL2_TRUNK_MASK_HASK			BIT(11)
-#define GLOBAL2_TRUNK_MAPPING	0x08
-#define GLOBAL2_TRUNK_MAPPING_UPDATE		BIT(15)
-#define GLOBAL2_TRUNK_MAPPING_ID_SHIFT		11
-#define GLOBAL2_IRL_CMD		0x09
-#define GLOBAL2_IRL_CMD_BUSY	BIT(15)
-#define GLOBAL2_IRL_CMD_OP_INIT_ALL	((0x001 << 12) | GLOBAL2_IRL_CMD_BUSY)
-#define GLOBAL2_IRL_CMD_OP_INIT_SEL	((0x010 << 12) | GLOBAL2_IRL_CMD_BUSY)
-#define GLOBAL2_IRL_CMD_OP_WRITE_SEL	((0x011 << 12) | GLOBAL2_IRL_CMD_BUSY)
-#define GLOBAL2_IRL_CMD_OP_READ_SEL	((0x100 << 12) | GLOBAL2_IRL_CMD_BUSY)
-#define GLOBAL2_IRL_DATA	0x0a
-#define GLOBAL2_PVT_ADDR	0x0b
-#define GLOBAL2_PVT_ADDR_BUSY	BIT(15)
-#define GLOBAL2_PVT_ADDR_OP_INIT_ONES	((0x01 << 12) | GLOBAL2_PVT_ADDR_BUSY)
-#define GLOBAL2_PVT_ADDR_OP_WRITE_PVLAN	((0x03 << 12) | GLOBAL2_PVT_ADDR_BUSY)
-#define GLOBAL2_PVT_ADDR_OP_READ	((0x04 << 12) | GLOBAL2_PVT_ADDR_BUSY)
-#define GLOBAL2_PVT_DATA	0x0c
-#define GLOBAL2_SWITCH_MAC	0x0d
-#define GLOBAL2_ATU_STATS	0x0e
-#define GLOBAL2_PRIO_OVERRIDE	0x0f
-#define GLOBAL2_PRIO_OVERRIDE_FORCE_SNOOP	BIT(7)
-#define GLOBAL2_PRIO_OVERRIDE_SNOOP_SHIFT	4
-#define GLOBAL2_PRIO_OVERRIDE_FORCE_ARP		BIT(3)
-#define GLOBAL2_PRIO_OVERRIDE_ARP_SHIFT		0
-#define GLOBAL2_EEPROM_CMD		0x14
-#define GLOBAL2_EEPROM_CMD_BUSY		BIT(15)
-#define GLOBAL2_EEPROM_CMD_OP_WRITE	((0x3 << 12) | GLOBAL2_EEPROM_CMD_BUSY)
-#define GLOBAL2_EEPROM_CMD_OP_READ	((0x4 << 12) | GLOBAL2_EEPROM_CMD_BUSY)
-#define GLOBAL2_EEPROM_CMD_OP_LOAD	((0x6 << 12) | GLOBAL2_EEPROM_CMD_BUSY)
-#define GLOBAL2_EEPROM_CMD_RUNNING	BIT(11)
-#define GLOBAL2_EEPROM_CMD_WRITE_EN	BIT(10)
-#define GLOBAL2_EEPROM_CMD_ADDR_MASK	0xff
-#define GLOBAL2_EEPROM_DATA	0x15
-#define GLOBAL2_EEPROM_ADDR	0x15 /* 6390, 6341 */
-#define GLOBAL2_PTP_AVB_OP	0x16
-#define GLOBAL2_PTP_AVB_DATA	0x17
-#define GLOBAL2_SMI_PHY_CMD			0x18
-#define GLOBAL2_SMI_PHY_CMD_BUSY		BIT(15)
-#define GLOBAL2_SMI_PHY_CMD_EXTERNAL		BIT(13)
-#define GLOBAL2_SMI_PHY_CMD_MODE_22		BIT(12)
-#define GLOBAL2_SMI_PHY_CMD_OP_22_WRITE_DATA	((0x1 << 10) | \
-						 GLOBAL2_SMI_PHY_CMD_MODE_22 | \
-						 GLOBAL2_SMI_PHY_CMD_BUSY)
-#define GLOBAL2_SMI_PHY_CMD_OP_22_READ_DATA	((0x2 << 10) | \
-						 GLOBAL2_SMI_PHY_CMD_MODE_22 | \
-						 GLOBAL2_SMI_PHY_CMD_BUSY)
-#define GLOBAL2_SMI_PHY_CMD_OP_45_WRITE_ADDR	((0x0 << 10) | \
-						 GLOBAL2_SMI_PHY_CMD_BUSY)
-#define GLOBAL2_SMI_PHY_CMD_OP_45_WRITE_DATA	((0x1 << 10) | \
-						 GLOBAL2_SMI_PHY_CMD_BUSY)
-#define GLOBAL2_SMI_PHY_CMD_OP_45_READ_DATA	((0x3 << 10) | \
-						 GLOBAL2_SMI_PHY_CMD_BUSY)
-
-#define GLOBAL2_SMI_PHY_DATA			0x19
-#define GLOBAL2_SCRATCH_MISC	0x1a
-#define GLOBAL2_SCRATCH_BUSY		BIT(15)
-#define GLOBAL2_SCRATCH_REGISTER_SHIFT	8
-#define GLOBAL2_SCRATCH_VALUE_MASK	0xff
-#define GLOBAL2_WDOG_CONTROL	0x1b
-#define GLOBAL2_WDOG_CONTROL_EGRESS_EVENT	BIT(7)
-#define GLOBAL2_WDOG_CONTROL_RMU_TIMEOUT	BIT(6)
-#define GLOBAL2_WDOG_CONTROL_QC_ENABLE		BIT(5)
-#define GLOBAL2_WDOG_CONTROL_EGRESS_HISTORY	BIT(4)
-#define GLOBAL2_WDOG_CONTROL_EGRESS_ENABLE	BIT(3)
-#define GLOBAL2_WDOG_CONTROL_FORCE_IRQ		BIT(2)
-#define GLOBAL2_WDOG_CONTROL_HISTORY		BIT(1)
-#define GLOBAL2_WDOG_CONTROL_SWRESET		BIT(0)
-#define GLOBAL2_WDOG_UPDATE			BIT(15)
-#define GLOBAL2_WDOG_INT_SOURCE			(0x00 << 8)
-#define GLOBAL2_WDOG_INT_STATUS			(0x10 << 8)
-#define GLOBAL2_WDOG_INT_ENABLE			(0x11 << 8)
-#define GLOBAL2_WDOG_EVENT			(0x12 << 8)
-#define GLOBAL2_WDOG_HISTORY			(0x13 << 8)
-#define GLOBAL2_WDOG_DATA_MASK			0xff
-#define GLOBAL2_WDOG_CUT_THROUGH		BIT(3)
-#define GLOBAL2_WDOG_QUEUE_CONTROLLER		BIT(2)
-#define GLOBAL2_WDOG_EGRESS			BIT(1)
-#define GLOBAL2_WDOG_FORCE_IRQ			BIT(0)
-#define GLOBAL2_QOS_WEIGHT	0x1c
-#define GLOBAL2_MISC		0x1d
-#define GLOBAL2_MISC_5_BIT_PORT	BIT(14)
-
-#define MV88E6XXX_N_FID		4096
-
-/* PVT limits for 4-bit port and 5-bit switch */
-#define MV88E6XXX_MAX_PVT_SWITCHES	32
-#define MV88E6XXX_MAX_PVT_PORTS		16
-
-enum mv88e6xxx_frame_mode {
-	MV88E6XXX_FRAME_MODE_NORMAL,
-	MV88E6XXX_FRAME_MODE_DSA,
-	MV88E6XXX_FRAME_MODE_PROVIDER,
-	MV88E6XXX_FRAME_MODE_ETHERTYPE,
-};
-
-/* List of supported models */
-enum mv88e6xxx_model {
-	MV88E6085,
-	MV88E6095,
-	MV88E6097,
-	MV88E6123,
-	MV88E6131,
-	MV88E6141,
-	MV88E6161,
-	MV88E6165,
-	MV88E6171,
-	MV88E6172,
-	MV88E6175,
-	MV88E6176,
-	MV88E6185,
-	MV88E6190,
-	MV88E6190X,
-	MV88E6191,
-	MV88E6240,
-	MV88E6290,
-	MV88E6320,
-	MV88E6321,
-	MV88E6341,
-	MV88E6350,
-	MV88E6351,
-	MV88E6352,
-	MV88E6390,
-	MV88E6390X,
-};
-
-enum mv88e6xxx_family {
-	MV88E6XXX_FAMILY_NONE,
-	MV88E6XXX_FAMILY_6065,	/* 6031 6035 6061 6065 */
-	MV88E6XXX_FAMILY_6095,	/* 6092 6095 */
-	MV88E6XXX_FAMILY_6097,	/* 6046 6085 6096 6097 */
-	MV88E6XXX_FAMILY_6165,	/* 6123 6161 6165 */
-	MV88E6XXX_FAMILY_6185,	/* 6108 6121 6122 6131 6152 6155 6182 6185 */
-	MV88E6XXX_FAMILY_6320,	/* 6320 6321 */
-	MV88E6XXX_FAMILY_6341,	/* 6141 6341 */
-	MV88E6XXX_FAMILY_6351,	/* 6171 6175 6350 6351 */
-	MV88E6XXX_FAMILY_6352,	/* 6172 6176 6240 6352 */
-	MV88E6XXX_FAMILY_6390,  /* 6190 6190X 6191 6290 6390 6390X */
-};
-
-enum mv88e6xxx_cap {
-	/* Energy Efficient Ethernet.
-	 */
-	MV88E6XXX_CAP_EEE,
-
-	/* Multi-chip Addressing Mode.
-	 * Some chips respond to only 2 registers of its own SMI device address
-	 * when it is non-zero, and use indirect access to internal registers.
-	 */
-	MV88E6XXX_CAP_SMI_CMD,		/* (0x00) SMI Command */
-	MV88E6XXX_CAP_SMI_DATA,		/* (0x01) SMI Data */
-
-	/* PHY Registers.
-	 */
-	MV88E6XXX_CAP_PHY_PAGE,		/* (0x16) Page Register */
-
-	/* Fiber/SERDES Registers (SMI address F).
-	 */
-	MV88E6XXX_CAP_SERDES,
-
-	/* Switch Global (1) Registers.
-	 */
-	MV88E6XXX_CAP_G1_ATU_FID,	/* (0x01) ATU FID Register */
-	MV88E6XXX_CAP_G1_VTU_FID,	/* (0x02) VTU FID Register */
-
-	/* Switch Global 2 Registers.
-	 * The device contains a second set of global 16-bit registers.
-	 */
-	MV88E6XXX_CAP_GLOBAL2,
-	MV88E6XXX_CAP_G2_INT,		/* (0x00) Interrupt Status */
-	MV88E6XXX_CAP_G2_MGMT_EN_2X,	/* (0x02) MGMT Enable Register 2x */
-	MV88E6XXX_CAP_G2_MGMT_EN_0X,	/* (0x03) MGMT Enable Register 0x */
-	MV88E6XXX_CAP_G2_IRL_CMD,	/* (0x09) Ingress Rate Command */
-	MV88E6XXX_CAP_G2_IRL_DATA,	/* (0x0a) Ingress Rate Data */
-	MV88E6XXX_CAP_G2_POT,		/* (0x0f) Priority Override Table */
-
-	/* Per VLAN Spanning Tree Unit (STU).
-	 * The Port State database, if present, is accessed through VTU
-	 * operations and dedicated SID registers. See GLOBAL_VTU_SID.
-	 */
-	MV88E6XXX_CAP_STU,
-
-	/* VLAN Table Unit.
-	 * The VTU is used to program 802.1Q VLANs. See GLOBAL_VTU_OP.
-	 */
-	MV88E6XXX_CAP_VTU,
-};
-
-/* Bitmask of capabilities */
-#define MV88E6XXX_FLAG_EEE		BIT_ULL(MV88E6XXX_CAP_EEE)
-
-#define MV88E6XXX_FLAG_SMI_CMD		BIT_ULL(MV88E6XXX_CAP_SMI_CMD)
-#define MV88E6XXX_FLAG_SMI_DATA		BIT_ULL(MV88E6XXX_CAP_SMI_DATA)
-
-#define MV88E6XXX_FLAG_PHY_PAGE		BIT_ULL(MV88E6XXX_CAP_PHY_PAGE)
-
-#define MV88E6XXX_FLAG_SERDES		BIT_ULL(MV88E6XXX_CAP_SERDES)
-
-#define MV88E6XXX_FLAG_G1_VTU_FID	BIT_ULL(MV88E6XXX_CAP_G1_VTU_FID)
-
-#define MV88E6XXX_FLAG_GLOBAL2		BIT_ULL(MV88E6XXX_CAP_GLOBAL2)
-#define MV88E6XXX_FLAG_G2_INT		BIT_ULL(MV88E6XXX_CAP_G2_INT)
-#define MV88E6XXX_FLAG_G2_MGMT_EN_2X	BIT_ULL(MV88E6XXX_CAP_G2_MGMT_EN_2X)
-#define MV88E6XXX_FLAG_G2_MGMT_EN_0X	BIT_ULL(MV88E6XXX_CAP_G2_MGMT_EN_0X)
-#define MV88E6XXX_FLAG_G2_IRL_CMD	BIT_ULL(MV88E6XXX_CAP_G2_IRL_CMD)
-#define MV88E6XXX_FLAG_G2_IRL_DATA	BIT_ULL(MV88E6XXX_CAP_G2_IRL_DATA)
-#define MV88E6XXX_FLAG_G2_POT		BIT_ULL(MV88E6XXX_CAP_G2_POT)
-
-/* Ingress Rate Limit unit */
-#define MV88E6XXX_FLAGS_IRL		\
-	(MV88E6XXX_FLAG_G2_IRL_CMD |	\
-	 MV88E6XXX_FLAG_G2_IRL_DATA)
-
-/* Multi-chip Addressing Mode */
-#define MV88E6XXX_FLAGS_MULTI_CHIP	\
-	(MV88E6XXX_FLAG_SMI_CMD |	\
-	 MV88E6XXX_FLAG_SMI_DATA)
-
-/* Fiber/SERDES Registers at SMI address F, page 1 */
-#define MV88E6XXX_FLAGS_SERDES		\
-	(MV88E6XXX_FLAG_PHY_PAGE |	\
-	 MV88E6XXX_FLAG_SERDES)
-
-#define MV88E6XXX_FLAGS_FAMILY_6095	\
-	(MV88E6XXX_FLAG_GLOBAL2 |	\
-	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
-	 MV88E6XXX_FLAGS_MULTI_CHIP)
-
-#define MV88E6XXX_FLAGS_FAMILY_6097	\
-	(MV88E6XXX_FLAG_G1_VTU_FID |	\
-	 MV88E6XXX_FLAG_GLOBAL2 |	\
-	 MV88E6XXX_FLAG_G2_INT |        \
-	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
-	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
-	 MV88E6XXX_FLAG_G2_POT |	\
-	 MV88E6XXX_FLAGS_IRL |		\
-	 MV88E6XXX_FLAGS_MULTI_CHIP)
-
-#define MV88E6XXX_FLAGS_FAMILY_6165	\
-	(MV88E6XXX_FLAG_G1_VTU_FID |	\
-	 MV88E6XXX_FLAG_GLOBAL2 |	\
-	 MV88E6XXX_FLAG_G2_INT |	\
-	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
-	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
-	 MV88E6XXX_FLAG_G2_POT |	\
-	 MV88E6XXX_FLAGS_IRL |		\
-	 MV88E6XXX_FLAGS_MULTI_CHIP)
-
-#define MV88E6XXX_FLAGS_FAMILY_6185	\
-	(MV88E6XXX_FLAG_GLOBAL2 |	\
-	 MV88E6XXX_FLAG_G2_INT |	\
-	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
-	 MV88E6XXX_FLAGS_MULTI_CHIP)
-
-#define MV88E6XXX_FLAGS_FAMILY_6320	\
-	(MV88E6XXX_FLAG_EEE |		\
-	 MV88E6XXX_FLAG_GLOBAL2 |	\
-	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
-	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
-	 MV88E6XXX_FLAG_G2_POT |	\
-	 MV88E6XXX_FLAGS_IRL |		\
-	 MV88E6XXX_FLAGS_MULTI_CHIP)
-
-#define MV88E6XXX_FLAGS_FAMILY_6341	\
-	(MV88E6XXX_FLAG_EEE |		\
-	 MV88E6XXX_FLAG_G1_VTU_FID |	\
-	 MV88E6XXX_FLAG_GLOBAL2 |	\
-	 MV88E6XXX_FLAG_G2_INT |	\
-	 MV88E6XXX_FLAG_G2_POT |	\
-	 MV88E6XXX_FLAGS_IRL |		\
-	 MV88E6XXX_FLAGS_MULTI_CHIP |	\
-	 MV88E6XXX_FLAGS_SERDES)
-
-#define MV88E6XXX_FLAGS_FAMILY_6351	\
-	(MV88E6XXX_FLAG_G1_VTU_FID |	\
-	 MV88E6XXX_FLAG_GLOBAL2 |	\
-	 MV88E6XXX_FLAG_G2_INT |	\
-	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
-	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
-	 MV88E6XXX_FLAG_G2_POT |	\
-	 MV88E6XXX_FLAGS_IRL |		\
-	 MV88E6XXX_FLAGS_MULTI_CHIP)
-
-#define MV88E6XXX_FLAGS_FAMILY_6352	\
-	(MV88E6XXX_FLAG_EEE |		\
-	 MV88E6XXX_FLAG_G1_VTU_FID |	\
-	 MV88E6XXX_FLAG_GLOBAL2 |	\
-	 MV88E6XXX_FLAG_G2_INT |	\
-	 MV88E6XXX_FLAG_G2_MGMT_EN_2X |	\
-	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
-	 MV88E6XXX_FLAG_G2_POT |	\
-	 MV88E6XXX_FLAGS_IRL |		\
-	 MV88E6XXX_FLAGS_MULTI_CHIP |	\
-	 MV88E6XXX_FLAGS_SERDES)
-
-#define MV88E6XXX_FLAGS_FAMILY_6390	\
-	(MV88E6XXX_FLAG_EEE |		\
-	 MV88E6XXX_FLAG_GLOBAL2 |	\
-	 MV88E6XXX_FLAG_G2_INT |        \
-	 MV88E6XXX_FLAGS_IRL |		\
-	 MV88E6XXX_FLAGS_MULTI_CHIP)
-
-struct mv88e6xxx_ops;
-
-struct mv88e6xxx_info {
-	enum mv88e6xxx_family family;
-	u16 prod_num;
-	const char *name;
-	unsigned int num_databases;
-	unsigned int num_ports;
-	unsigned int max_vid;
-	unsigned int port_base_addr;
-	unsigned int global1_addr;
-	unsigned int age_time_coeff;
-	unsigned int g1_irqs;
-	bool pvt;
-	enum dsa_tag_protocol tag_protocol;
-	unsigned long long flags;
-
-	/* Mask for FromPort and ToPort value of PortVec used in ATU Move
-	 * operation. 0 means that the ATU Move operation is not supported.
-	 */
-	u8 atu_move_port_mask;
-	const struct mv88e6xxx_ops *ops;
-};
-
-struct mv88e6xxx_atu_entry {
-	u8	state;
-	bool	trunk;
-	u16	portvec;
-	u8	mac[ETH_ALEN];
-};
-
-struct mv88e6xxx_vtu_entry {
-	u16	vid;
-	u16	fid;
-	u8	sid;
-	bool	valid;
-	u8	member[DSA_MAX_PORTS];
-	u8	state[DSA_MAX_PORTS];
-};
-
-struct mv88e6xxx_bus_ops;
-struct mv88e6xxx_irq_ops;
-
-struct mv88e6xxx_irq {
-	u16 masked;
-	struct irq_chip chip;
-	struct irq_domain *domain;
-	unsigned int nirqs;
-};
-
-struct mv88e6xxx_chip {
-	const struct mv88e6xxx_info *info;
-
-	/* The dsa_switch this private structure is related to */
-	struct dsa_switch *ds;
-
-	/* The device this structure is associated to */
-	struct device *dev;
-
-	/* This mutex protects the access to the switch registers */
-	struct mutex reg_lock;
-
-	/* The MII bus and the address on the bus that is used to
-	 * communication with the switch
-	 */
-	const struct mv88e6xxx_bus_ops *smi_ops;
-	struct mii_bus *bus;
-	int sw_addr;
-
-	/* Handles automatic disabling and re-enabling of the PHY
-	 * polling unit.
-	 */
-	const struct mv88e6xxx_bus_ops *phy_ops;
-	struct mutex		ppu_mutex;
-	int			ppu_disabled;
-	struct work_struct	ppu_work;
-	struct timer_list	ppu_timer;
-
-	/* This mutex serialises access to the statistics unit.
-	 * Hold this mutex over snapshot + dump sequences.
-	 */
-	struct mutex	stats_mutex;
-
-	/* A switch may have a GPIO line tied to its reset pin. Parse
-	 * this from the device tree, and use it before performing
-	 * switch soft reset.
-	 */
-	struct gpio_desc *reset;
-
-	/* set to size of eeprom if supported by the switch */
-	int		eeprom_len;
-
-	/* List of mdio busses */
-	struct list_head mdios;
-
-	/* There can be two interrupt controllers, which are chained
-	 * off a GPIO as interrupt source
-	 */
-	struct mv88e6xxx_irq g1_irq;
-	struct mv88e6xxx_irq g2_irq;
-	int irq;
-	int device_irq;
-	int watchdog_irq;
-};
-
-struct mv88e6xxx_bus_ops {
-	int (*read)(struct mv88e6xxx_chip *chip, int addr, int reg, u16 *val);
-	int (*write)(struct mv88e6xxx_chip *chip, int addr, int reg, u16 val);
-};
-
-struct mv88e6xxx_mdio_bus {
-	struct mii_bus *bus;
-	struct mv88e6xxx_chip *chip;
-	struct list_head list;
-	bool external;
-};
-
-struct mv88e6xxx_ops {
-	int (*get_eeprom)(struct mv88e6xxx_chip *chip,
-			  struct ethtool_eeprom *eeprom, u8 *data);
-	int (*set_eeprom)(struct mv88e6xxx_chip *chip,
-			  struct ethtool_eeprom *eeprom, u8 *data);
-
-	int (*set_switch_mac)(struct mv88e6xxx_chip *chip, u8 *addr);
-
-	int (*phy_read)(struct mv88e6xxx_chip *chip,
-			struct mii_bus *bus,
-			int addr, int reg, u16 *val);
-	int (*phy_write)(struct mv88e6xxx_chip *chip,
-			 struct mii_bus *bus,
-			 int addr, int reg, u16 val);
-
-	/* PHY Polling Unit (PPU) operations */
-	int (*ppu_enable)(struct mv88e6xxx_chip *chip);
-	int (*ppu_disable)(struct mv88e6xxx_chip *chip);
-
-	/* Switch Software Reset */
-	int (*reset)(struct mv88e6xxx_chip *chip);
-
-	/* RGMII Receive/Transmit Timing Control
-	 * Add delay on PHY_INTERFACE_MODE_RGMII_*ID, no delay otherwise.
-	 */
-	int (*port_set_rgmii_delay)(struct mv88e6xxx_chip *chip, int port,
-				    phy_interface_t mode);
-
-#define LINK_FORCED_DOWN	0
-#define LINK_FORCED_UP		1
-#define LINK_UNFORCED		-2
-
-	/* Port's MAC link state
-	 * Use LINK_FORCED_UP or LINK_FORCED_DOWN to force link up or down,
-	 * or LINK_UNFORCED for normal link detection.
-	 */
-	int (*port_set_link)(struct mv88e6xxx_chip *chip, int port, int link);
-
-#define DUPLEX_UNFORCED		-2
-
-	/* Port's MAC duplex mode
-	 *
-	 * Use DUPLEX_HALF or DUPLEX_FULL to force half or full duplex,
-	 * or DUPLEX_UNFORCED for normal duplex detection.
-	 */
-	int (*port_set_duplex)(struct mv88e6xxx_chip *chip, int port, int dup);
-
-#define SPEED_MAX		INT_MAX
-#define SPEED_UNFORCED		-2
-
-	/* Port's MAC speed (in Mbps)
-	 *
-	 * Depending on the chip, 10, 100, 200, 1000, 2500, 10000 are valid.
-	 * Use SPEED_UNFORCED for normal detection, SPEED_MAX for max value.
-	 */
-	int (*port_set_speed)(struct mv88e6xxx_chip *chip, int port, int speed);
-
-	int (*port_tag_remap)(struct mv88e6xxx_chip *chip, int port);
-
-	int (*port_set_frame_mode)(struct mv88e6xxx_chip *chip, int port,
-				   enum mv88e6xxx_frame_mode mode);
-	int (*port_set_egress_floods)(struct mv88e6xxx_chip *chip, int port,
-				      bool unicast, bool multicast);
-	int (*port_set_ether_type)(struct mv88e6xxx_chip *chip, int port,
-				   u16 etype);
-	int (*port_jumbo_config)(struct mv88e6xxx_chip *chip, int port);
-
-	int (*port_egress_rate_limiting)(struct mv88e6xxx_chip *chip, int port);
-	int (*port_pause_config)(struct mv88e6xxx_chip *chip, int port);
-	int (*port_disable_learn_limit)(struct mv88e6xxx_chip *chip, int port);
-	int (*port_disable_pri_override)(struct mv88e6xxx_chip *chip, int port);
-
-	/* CMODE control what PHY mode the MAC will use, eg. SGMII, RGMII, etc.
-	 * Some chips allow this to be configured on specific ports.
-	 */
-	int (*port_set_cmode)(struct mv88e6xxx_chip *chip, int port,
-			      phy_interface_t mode);
-
-	/* Some devices have a per port register indicating what is
-	 * the upstream port this port should forward to.
-	 */
-	int (*port_set_upstream_port)(struct mv88e6xxx_chip *chip, int port,
-				      int upstream_port);
-
-	/* Snapshot the statistics for a port. The statistics can then
-	 * be read back a leisure but still with a consistent view.
-	 */
-	int (*stats_snapshot)(struct mv88e6xxx_chip *chip, int port);
-
-	/* Set the histogram mode for statistics, when the control registers
-	 * are separated out of the STATS_OP register.
-	 */
-	int (*stats_set_histogram)(struct mv88e6xxx_chip *chip);
-
-	/* Return the number of strings describing statistics */
-	int (*stats_get_sset_count)(struct mv88e6xxx_chip *chip);
-	void (*stats_get_strings)(struct mv88e6xxx_chip *chip,  uint8_t *data);
-	void (*stats_get_stats)(struct mv88e6xxx_chip *chip,  int port,
-				uint64_t *data);
-	int (*g1_set_cpu_port)(struct mv88e6xxx_chip *chip, int port);
-	int (*g1_set_egress_port)(struct mv88e6xxx_chip *chip, int port);
-	const struct mv88e6xxx_irq_ops *watchdog_ops;
-
-	/* Can be either in g1 or g2, so don't use a prefix */
-	int (*mgmt_rsvd2cpu)(struct mv88e6xxx_chip *chip);
-
-	/* VLAN Translation Unit operations */
-	int (*vtu_getnext)(struct mv88e6xxx_chip *chip,
-			   struct mv88e6xxx_vtu_entry *entry);
-	int (*vtu_loadpurge)(struct mv88e6xxx_chip *chip,
-			     struct mv88e6xxx_vtu_entry *entry);
-};
-
-struct mv88e6xxx_irq_ops {
-	/* Action to be performed when the interrupt happens */
-	int (*irq_action)(struct mv88e6xxx_chip *chip, int irq);
-	/* Setup the hardware to generate the interrupt */
-	int (*irq_setup)(struct mv88e6xxx_chip *chip);
-	/* Reset the hardware to stop generating the interrupt */
-	void (*irq_free)(struct mv88e6xxx_chip *chip);
-};
-
-#define STATS_TYPE_PORT		BIT(0)
-#define STATS_TYPE_BANK0	BIT(1)
-#define STATS_TYPE_BANK1	BIT(2)
-
-struct mv88e6xxx_hw_stat {
-	char string[ETH_GSTRING_LEN];
-	int sizeof_stat;
-	int reg;
-	int type;
-};
-
-static inline bool mv88e6xxx_has(struct mv88e6xxx_chip *chip,
-				 unsigned long flags)
-{
-	return (chip->info->flags & flags) == flags;
-}
-
-static inline bool mv88e6xxx_has_pvt(struct mv88e6xxx_chip *chip)
-{
-	return chip->info->pvt;
-}
-
-static inline unsigned int mv88e6xxx_num_databases(struct mv88e6xxx_chip *chip)
-{
-	return chip->info->num_databases;
-}
-
-static inline unsigned int mv88e6xxx_num_ports(struct mv88e6xxx_chip *chip)
-{
-	return chip->info->num_ports;
-}
-
-static inline u16 mv88e6xxx_port_mask(struct mv88e6xxx_chip *chip)
-{
-	return GENMASK(mv88e6xxx_num_ports(chip) - 1, 0);
-}
-
-int mv88e6xxx_read(struct mv88e6xxx_chip *chip, int addr, int reg, u16 *val);
-int mv88e6xxx_write(struct mv88e6xxx_chip *chip, int addr, int reg, u16 val);
-int mv88e6xxx_update(struct mv88e6xxx_chip *chip, int addr, int reg,
-		     u16 update);
-int mv88e6xxx_wait(struct mv88e6xxx_chip *chip, int addr, int reg, u16 mask);
-
-#endif
diff --git a/drivers/net/dsa/mv88e6xxx/phy.c b/drivers/net/dsa/mv88e6xxx/phy.c
new file mode 100644
index 000000000000..3500ac0ea848
--- /dev/null
+++ b/drivers/net/dsa/mv88e6xxx/phy.c
@@ -0,0 +1,249 @@
+/*
+ * Marvell 88e6xxx Ethernet switch PHY and PPU support
+ *
+ * Copyright (c) 2008 Marvell Semiconductor
+ *
+ * Copyright (c) 2017 Andrew Lunn <andrew@lunn.ch>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/mdio.h>
+#include <linux/module.h>
+#include <net/dsa.h>
+
+#include "chip.h"
+#include "phy.h"
+
+int mv88e6165_phy_read(struct mv88e6xxx_chip *chip, struct mii_bus *bus,
+		       int addr, int reg, u16 *val)
+{
+	return mv88e6xxx_read(chip, addr, reg, val);
+}
+
+int mv88e6165_phy_write(struct mv88e6xxx_chip *chip, struct mii_bus *bus,
+			int addr, int reg, u16 val)
+{
+	return mv88e6xxx_write(chip, addr, reg, val);
+}
+
+int mv88e6xxx_phy_read(struct mv88e6xxx_chip *chip, int phy, int reg, u16 *val)
+{
+	int addr = phy; /* PHY devices addresses start at 0x0 */
+	struct mii_bus *bus;
+
+	bus = mv88e6xxx_default_mdio_bus(chip);
+	if (!bus)
+		return -EOPNOTSUPP;
+
+	if (!chip->info->ops->phy_read)
+		return -EOPNOTSUPP;
+
+	return chip->info->ops->phy_read(chip, bus, addr, reg, val);
+}
+
+int mv88e6xxx_phy_write(struct mv88e6xxx_chip *chip, int phy, int reg, u16 val)
+{
+	int addr = phy; /* PHY devices addresses start at 0x0 */
+	struct mii_bus *bus;
+
+	bus = mv88e6xxx_default_mdio_bus(chip);
+	if (!bus)
+		return -EOPNOTSUPP;
+
+	if (!chip->info->ops->phy_write)
+		return -EOPNOTSUPP;
+
+	return chip->info->ops->phy_write(chip, bus, addr, reg, val);
+}
+
+static int mv88e6xxx_phy_page_get(struct mv88e6xxx_chip *chip, int phy, u8 page)
+{
+	return mv88e6xxx_phy_write(chip, phy, MV88E6XXX_PHY_PAGE, page);
+}
+
+static void mv88e6xxx_phy_page_put(struct mv88e6xxx_chip *chip, int phy)
+{
+	int err;
+
+	/* Restore PHY page Copper 0x0 for access via the registered
+	 * MDIO bus
+	 */
+	err = mv88e6xxx_phy_write(chip, phy, MV88E6XXX_PHY_PAGE,
+				  MV88E6XXX_PHY_PAGE_COPPER);
+	if (unlikely(err)) {
+		dev_err(chip->dev,
+			"failed to restore PHY %d page Copper (%d)\n",
+			phy, err);
+	}
+}
+
+int mv88e6xxx_phy_page_read(struct mv88e6xxx_chip *chip, int phy,
+			    u8 page, int reg, u16 *val)
+{
+	int err;
+
+	/* There is no paging for registers 22 */
+	if (reg == MV88E6XXX_PHY_PAGE)
+		return -EINVAL;
+
+	err = mv88e6xxx_phy_page_get(chip, phy, page);
+	if (!err) {
+		err = mv88e6xxx_phy_read(chip, phy, reg, val);
+		mv88e6xxx_phy_page_put(chip, phy);
+	}
+
+	return err;
+}
+
+int mv88e6xxx_phy_page_write(struct mv88e6xxx_chip *chip, int phy,
+			     u8 page, int reg, u16 val)
+{
+	int err;
+
+	/* There is no paging for registers 22 */
+	if (reg == MV88E6XXX_PHY_PAGE)
+		return -EINVAL;
+
+	err = mv88e6xxx_phy_page_get(chip, phy, page);
+	if (!err) {
+		err = mv88e6xxx_phy_write(chip, phy, MV88E6XXX_PHY_PAGE, page);
+		mv88e6xxx_phy_page_put(chip, phy);
+	}
+
+	return err;
+}
+
+static int mv88e6xxx_phy_ppu_disable(struct mv88e6xxx_chip *chip)
+{
+	if (!chip->info->ops->ppu_disable)
+		return 0;
+
+	return chip->info->ops->ppu_disable(chip);
+}
+
+static int mv88e6xxx_phy_ppu_enable(struct mv88e6xxx_chip *chip)
+{
+	if (!chip->info->ops->ppu_enable)
+		return 0;
+
+	return chip->info->ops->ppu_enable(chip);
+}
+
+static void mv88e6xxx_phy_ppu_reenable_work(struct work_struct *ugly)
+{
+	struct mv88e6xxx_chip *chip;
+
+	chip = container_of(ugly, struct mv88e6xxx_chip, ppu_work);
+
+	mutex_lock(&chip->reg_lock);
+
+	if (mutex_trylock(&chip->ppu_mutex)) {
+		if (mv88e6xxx_phy_ppu_enable(chip) == 0)
+			chip->ppu_disabled = 0;
+		mutex_unlock(&chip->ppu_mutex);
+	}
+
+	mutex_unlock(&chip->reg_lock);
+}
+
+static void mv88e6xxx_phy_ppu_reenable_timer(unsigned long _ps)
+{
+	struct mv88e6xxx_chip *chip = (void *)_ps;
+
+	schedule_work(&chip->ppu_work);
+}
+
+static int mv88e6xxx_phy_ppu_access_get(struct mv88e6xxx_chip *chip)
+{
+	int ret;
+
+	mutex_lock(&chip->ppu_mutex);
+
+	/* If the PHY polling unit is enabled, disable it so that
+	 * we can access the PHY registers.  If it was already
+	 * disabled, cancel the timer that is going to re-enable
+	 * it.
+	 */
+	if (!chip->ppu_disabled) {
+		ret = mv88e6xxx_phy_ppu_disable(chip);
+		if (ret < 0) {
+			mutex_unlock(&chip->ppu_mutex);
+			return ret;
+		}
+		chip->ppu_disabled = 1;
+	} else {
+		del_timer(&chip->ppu_timer);
+		ret = 0;
+	}
+
+	return ret;
+}
+
+static void mv88e6xxx_phy_ppu_access_put(struct mv88e6xxx_chip *chip)
+{
+	/* Schedule a timer to re-enable the PHY polling unit. */
+	mod_timer(&chip->ppu_timer, jiffies + msecs_to_jiffies(10));
+	mutex_unlock(&chip->ppu_mutex);
+}
+
+static void mv88e6xxx_phy_ppu_state_init(struct mv88e6xxx_chip *chip)
+{
+	mutex_init(&chip->ppu_mutex);
+	INIT_WORK(&chip->ppu_work, mv88e6xxx_phy_ppu_reenable_work);
+	setup_timer(&chip->ppu_timer, mv88e6xxx_phy_ppu_reenable_timer,
+		    (unsigned long)chip);
+}
+
+static void mv88e6xxx_phy_ppu_state_destroy(struct mv88e6xxx_chip *chip)
+{
+	del_timer_sync(&chip->ppu_timer);
+}
+
+int mv88e6185_phy_ppu_read(struct mv88e6xxx_chip *chip, struct mii_bus *bus,
+			   int addr, int reg, u16 *val)
+{
+	int err;
+
+	err = mv88e6xxx_phy_ppu_access_get(chip);
+	if (!err) {
+		err = mv88e6xxx_read(chip, addr, reg, val);
+		mv88e6xxx_phy_ppu_access_put(chip);
+	}
+
+	return err;
+}
+
+int mv88e6185_phy_ppu_write(struct mv88e6xxx_chip *chip, struct mii_bus *bus,
+			    int addr, int reg, u16 val)
+{
+	int err;
+
+	err = mv88e6xxx_phy_ppu_access_get(chip);
+	if (!err) {
+		err = mv88e6xxx_write(chip, addr, reg, val);
+		mv88e6xxx_phy_ppu_access_put(chip);
+	}
+
+	return err;
+}
+
+void mv88e6xxx_phy_init(struct mv88e6xxx_chip *chip)
+{
+	if (chip->info->ops->ppu_enable && chip->info->ops->ppu_disable)
+		mv88e6xxx_phy_ppu_state_init(chip);
+}
+
+void mv88e6xxx_phy_destroy(struct mv88e6xxx_chip *chip)
+{
+	if (chip->info->ops->ppu_enable && chip->info->ops->ppu_disable)
+		mv88e6xxx_phy_ppu_state_destroy(chip);
+}
+
+int mv88e6xxx_phy_setup(struct mv88e6xxx_chip *chip)
+{
+	return mv88e6xxx_phy_ppu_enable(chip);
+}
diff --git a/drivers/net/dsa/mv88e6xxx/phy.h b/drivers/net/dsa/mv88e6xxx/phy.h
new file mode 100644
index 000000000000..556b74a0502a
--- /dev/null
+++ b/drivers/net/dsa/mv88e6xxx/phy.h
@@ -0,0 +1,43 @@
+/*
+ * Marvell 88E6xxx PHY access
+ *
+ * Copyright (c) 2008 Marvell Semiconductor
+ *
+ * Copyright (c) 2017 Andrew Lunn <andrew@lunn.ch>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef _MV88E6XXX_PHY_H
+#define _MV88E6XXX_PHY_H
+
+#define MV88E6XXX_PHY_PAGE		0x16
+#define MV88E6XXX_PHY_PAGE_COPPER	0x00
+
+/* PHY Registers accesses implementations */
+int mv88e6165_phy_read(struct mv88e6xxx_chip *chip, struct mii_bus *bus,
+		       int addr, int reg, u16 *val);
+int mv88e6165_phy_write(struct mv88e6xxx_chip *chip, struct mii_bus *bus,
+			int addr, int reg, u16 val);
+int mv88e6185_phy_ppu_read(struct mv88e6xxx_chip *chip, struct mii_bus *bus,
+			   int addr, int reg, u16 *val);
+int mv88e6185_phy_ppu_write(struct mv88e6xxx_chip *chip, struct mii_bus *bus,
+			    int addr, int reg, u16 val);
+
+/* Generic PHY operations */
+int mv88e6xxx_phy_read(struct mv88e6xxx_chip *chip, int phy,
+		       int reg, u16 *val);
+int mv88e6xxx_phy_write(struct mv88e6xxx_chip *chip, int phy,
+			int reg, u16 val);
+int mv88e6xxx_phy_page_read(struct mv88e6xxx_chip *chip, int phy,
+			    u8 page, int reg, u16 *val);
+int mv88e6xxx_phy_page_write(struct mv88e6xxx_chip *chip, int phy,
+			     u8 page, int reg, u16 val);
+void mv88e6xxx_phy_init(struct mv88e6xxx_chip *chip);
+void mv88e6xxx_phy_destroy(struct mv88e6xxx_chip *chip);
+int mv88e6xxx_phy_setup(struct mv88e6xxx_chip *chip);
+
+#endif /*_MV88E6XXX_PHY_H */
diff --git a/drivers/net/dsa/mv88e6xxx/port.c b/drivers/net/dsa/mv88e6xxx/port.c
index 548a956637ee..a7801f6668a5 100644
--- a/drivers/net/dsa/mv88e6xxx/port.c
+++ b/drivers/net/dsa/mv88e6xxx/port.c
@@ -12,8 +12,11 @@
  * (at your option) any later version.
  */
 
+#include <linux/bitfield.h>
+#include <linux/if_bridge.h>
 #include <linux/phy.h>
-#include "mv88e6xxx.h"
+
+#include "chip.h"
 #include "port.h"
 
 int mv88e6xxx_port_read(struct mv88e6xxx_chip *chip, int port, int reg,
@@ -47,23 +50,23 @@ static int mv88e6xxx_port_set_rgmii_delay(struct mv88e6xxx_chip *chip, int port,
 	u16 reg;
 	int err;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_PCS_CTRL, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_MAC_CTL, &reg);
 	if (err)
 		return err;
 
-	reg &= ~(PORT_PCS_CTRL_RGMII_DELAY_RXCLK |
-		 PORT_PCS_CTRL_RGMII_DELAY_TXCLK);
+	reg &= ~(MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_RXCLK |
+		 MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_TXCLK);
 
 	switch (mode) {
 	case PHY_INTERFACE_MODE_RGMII_RXID:
-		reg |= PORT_PCS_CTRL_RGMII_DELAY_RXCLK;
+		reg |= MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_RXCLK;
 		break;
 	case PHY_INTERFACE_MODE_RGMII_TXID:
-		reg |= PORT_PCS_CTRL_RGMII_DELAY_TXCLK;
+		reg |= MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_TXCLK;
 		break;
 	case PHY_INTERFACE_MODE_RGMII_ID:
-		reg |= PORT_PCS_CTRL_RGMII_DELAY_RXCLK |
-			PORT_PCS_CTRL_RGMII_DELAY_TXCLK;
+		reg |= MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_RXCLK |
+			MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_TXCLK;
 		break;
 	case PHY_INTERFACE_MODE_RGMII:
 		break;
@@ -71,13 +74,13 @@ static int mv88e6xxx_port_set_rgmii_delay(struct mv88e6xxx_chip *chip, int port,
 		return 0;
 	}
 
-	err = mv88e6xxx_port_write(chip, port, PORT_PCS_CTRL, reg);
+	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_MAC_CTL, reg);
 	if (err)
 		return err;
 
-	netdev_dbg(chip->ds->ports[port].netdev, "delay RXCLK %s, TXCLK %s\n",
-		   reg & PORT_PCS_CTRL_RGMII_DELAY_RXCLK ? "yes" : "no",
-		   reg & PORT_PCS_CTRL_RGMII_DELAY_TXCLK ? "yes" : "no");
+	dev_dbg(chip->dev, "p%d: delay RXCLK %s, TXCLK %s\n", port,
+		reg & MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_RXCLK ? "yes" : "no",
+		reg & MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_TXCLK ? "yes" : "no");
 
 	return 0;
 }
@@ -105,18 +108,20 @@ int mv88e6xxx_port_set_link(struct mv88e6xxx_chip *chip, int port, int link)
 	u16 reg;
 	int err;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_PCS_CTRL, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_MAC_CTL, &reg);
 	if (err)
 		return err;
 
-	reg &= ~(PORT_PCS_CTRL_FORCE_LINK | PORT_PCS_CTRL_LINK_UP);
+	reg &= ~(MV88E6XXX_PORT_MAC_CTL_FORCE_LINK |
+		 MV88E6XXX_PORT_MAC_CTL_LINK_UP);
 
 	switch (link) {
 	case LINK_FORCED_DOWN:
-		reg |= PORT_PCS_CTRL_FORCE_LINK;
+		reg |= MV88E6XXX_PORT_MAC_CTL_FORCE_LINK;
 		break;
 	case LINK_FORCED_UP:
-		reg |= PORT_PCS_CTRL_FORCE_LINK | PORT_PCS_CTRL_LINK_UP;
+		reg |= MV88E6XXX_PORT_MAC_CTL_FORCE_LINK |
+			MV88E6XXX_PORT_MAC_CTL_LINK_UP;
 		break;
 	case LINK_UNFORCED:
 		/* normal link detection */
@@ -125,13 +130,13 @@ int mv88e6xxx_port_set_link(struct mv88e6xxx_chip *chip, int port, int link)
 		return -EINVAL;
 	}
 
-	err = mv88e6xxx_port_write(chip, port, PORT_PCS_CTRL, reg);
+	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_MAC_CTL, reg);
 	if (err)
 		return err;
 
-	netdev_dbg(chip->ds->ports[port].netdev, "%s link %s\n",
-		   reg & PORT_PCS_CTRL_FORCE_LINK ? "Force" : "Unforce",
-		   reg & PORT_PCS_CTRL_LINK_UP ? "up" : "down");
+	dev_dbg(chip->dev, "p%d: %s link %s\n", port,
+		reg & MV88E6XXX_PORT_MAC_CTL_FORCE_LINK ? "Force" : "Unforce",
+		reg & MV88E6XXX_PORT_MAC_CTL_LINK_UP ? "up" : "down");
 
 	return 0;
 }
@@ -141,18 +146,20 @@ int mv88e6xxx_port_set_duplex(struct mv88e6xxx_chip *chip, int port, int dup)
 	u16 reg;
 	int err;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_PCS_CTRL, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_MAC_CTL, &reg);
 	if (err)
 		return err;
 
-	reg &= ~(PORT_PCS_CTRL_FORCE_DUPLEX | PORT_PCS_CTRL_DUPLEX_FULL);
+	reg &= ~(MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX |
+		 MV88E6XXX_PORT_MAC_CTL_DUPLEX_FULL);
 
 	switch (dup) {
 	case DUPLEX_HALF:
-		reg |= PORT_PCS_CTRL_FORCE_DUPLEX;
+		reg |= MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX;
 		break;
 	case DUPLEX_FULL:
-		reg |= PORT_PCS_CTRL_FORCE_DUPLEX | PORT_PCS_CTRL_DUPLEX_FULL;
+		reg |= MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX |
+			MV88E6XXX_PORT_MAC_CTL_DUPLEX_FULL;
 		break;
 	case DUPLEX_UNFORCED:
 		/* normal duplex detection */
@@ -161,13 +168,13 @@ int mv88e6xxx_port_set_duplex(struct mv88e6xxx_chip *chip, int port, int dup)
 		return -EINVAL;
 	}
 
-	err = mv88e6xxx_port_write(chip, port, PORT_PCS_CTRL, reg);
+	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_MAC_CTL, reg);
 	if (err)
 		return err;
 
-	netdev_dbg(chip->ds->ports[port].netdev, "%s %s duplex\n",
-		   reg & PORT_PCS_CTRL_FORCE_DUPLEX ? "Force" : "Unforce",
-		   reg & PORT_PCS_CTRL_DUPLEX_FULL ? "full" : "half");
+	dev_dbg(chip->dev, "p%d: %s %s duplex\n", port,
+		reg & MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX ? "Force" : "Unforce",
+		reg & MV88E6XXX_PORT_MAC_CTL_DUPLEX_FULL ? "full" : "half");
 
 	return 0;
 }
@@ -180,55 +187,56 @@ static int mv88e6xxx_port_set_speed(struct mv88e6xxx_chip *chip, int port,
 
 	switch (speed) {
 	case 10:
-		ctrl = PORT_PCS_CTRL_SPEED_10;
+		ctrl = MV88E6XXX_PORT_MAC_CTL_SPEED_10;
 		break;
 	case 100:
-		ctrl = PORT_PCS_CTRL_SPEED_100;
+		ctrl = MV88E6XXX_PORT_MAC_CTL_SPEED_100;
 		break;
 	case 200:
 		if (alt_bit)
-			ctrl = PORT_PCS_CTRL_SPEED_100 | PORT_PCS_CTRL_ALTSPEED;
+			ctrl = MV88E6XXX_PORT_MAC_CTL_SPEED_100 |
+				MV88E6390_PORT_MAC_CTL_ALTSPEED;
 		else
-			ctrl = PORT_PCS_CTRL_SPEED_200;
+			ctrl = MV88E6065_PORT_MAC_CTL_SPEED_200;
 		break;
 	case 1000:
-		ctrl = PORT_PCS_CTRL_SPEED_1000;
+		ctrl = MV88E6XXX_PORT_MAC_CTL_SPEED_1000;
 		break;
 	case 2500:
-		ctrl = PORT_PCS_CTRL_SPEED_10000 | PORT_PCS_CTRL_ALTSPEED;
+		ctrl = MV88E6390_PORT_MAC_CTL_SPEED_10000 |
+			MV88E6390_PORT_MAC_CTL_ALTSPEED;
 		break;
 	case 10000:
 		/* all bits set, fall through... */
 	case SPEED_UNFORCED:
-		ctrl = PORT_PCS_CTRL_SPEED_UNFORCED;
+		ctrl = MV88E6XXX_PORT_MAC_CTL_SPEED_UNFORCED;
 		break;
 	default:
 		return -EOPNOTSUPP;
 	}
 
-	err = mv88e6xxx_port_read(chip, port, PORT_PCS_CTRL, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_MAC_CTL, &reg);
 	if (err)
 		return err;
 
-	reg &= ~PORT_PCS_CTRL_SPEED_MASK;
+	reg &= ~MV88E6XXX_PORT_MAC_CTL_SPEED_MASK;
 	if (alt_bit)
-		reg &= ~PORT_PCS_CTRL_ALTSPEED;
+		reg &= ~MV88E6390_PORT_MAC_CTL_ALTSPEED;
 	if (force_bit) {
-		reg &= ~PORT_PCS_CTRL_FORCE_SPEED;
+		reg &= ~MV88E6390_PORT_MAC_CTL_FORCE_SPEED;
 		if (speed != SPEED_UNFORCED)
-			ctrl |= PORT_PCS_CTRL_FORCE_SPEED;
+			ctrl |= MV88E6390_PORT_MAC_CTL_FORCE_SPEED;
 	}
 	reg |= ctrl;
 
-	err = mv88e6xxx_port_write(chip, port, PORT_PCS_CTRL, reg);
+	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_MAC_CTL, reg);
 	if (err)
 		return err;
 
 	if (speed)
-		netdev_dbg(chip->ds->ports[port].netdev,
-			   "Speed set to %d Mbps\n", speed);
+		dev_dbg(chip->dev, "p%d: Speed set to %d Mbps\n", port, speed);
 	else
-		netdev_dbg(chip->ds->ports[port].netdev, "Speed unforced\n");
+		dev_dbg(chip->dev, "p%d: Speed unforced\n", port);
 
 	return 0;
 }
@@ -321,33 +329,33 @@ int mv88e6390x_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
 
 	switch (mode) {
 	case PHY_INTERFACE_MODE_1000BASEX:
-		cmode = PORT_STATUS_CMODE_1000BASE_X;
+		cmode = MV88E6XXX_PORT_STS_CMODE_1000BASE_X;
 		break;
 	case PHY_INTERFACE_MODE_SGMII:
-		cmode = PORT_STATUS_CMODE_SGMII;
+		cmode = MV88E6XXX_PORT_STS_CMODE_SGMII;
 		break;
 	case PHY_INTERFACE_MODE_2500BASEX:
-		cmode = PORT_STATUS_CMODE_2500BASEX;
+		cmode = MV88E6XXX_PORT_STS_CMODE_2500BASEX;
 		break;
 	case PHY_INTERFACE_MODE_XGMII:
-		cmode = PORT_STATUS_CMODE_XAUI;
+		cmode = MV88E6XXX_PORT_STS_CMODE_XAUI;
 		break;
 	case PHY_INTERFACE_MODE_RXAUI:
-		cmode = PORT_STATUS_CMODE_RXAUI;
+		cmode = MV88E6XXX_PORT_STS_CMODE_RXAUI;
 		break;
 	default:
 		cmode = 0;
 	}
 
 	if (cmode) {
-		err = mv88e6xxx_port_read(chip, port, PORT_STATUS, &reg);
+		err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_STS, &reg);
 		if (err)
 			return err;
 
-		reg &= ~PORT_STATUS_CMODE_MASK;
+		reg &= ~MV88E6XXX_PORT_STS_CMODE_MASK;
 		reg |= cmode;
 
-		err = mv88e6xxx_port_write(chip, port, PORT_STATUS, reg);
+		err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_STS, reg);
 		if (err)
 			return err;
 	}
@@ -360,46 +368,51 @@ int mv88e6xxx_port_get_cmode(struct mv88e6xxx_chip *chip, int port, u8 *cmode)
 	int err;
 	u16 reg;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_STATUS, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_STS, &reg);
 	if (err)
 		return err;
 
-	*cmode = reg & PORT_STATUS_CMODE_MASK;
+	*cmode = reg & MV88E6XXX_PORT_STS_CMODE_MASK;
 
 	return 0;
 }
 
-/* Offset 0x02: Pause Control
+/* Offset 0x02: Jamming Control
  *
  * Do not limit the period of time that this port can be paused for by
  * the remote end or the period of time that this port can pause the
  * remote end.
  */
-int mv88e6097_port_pause_config(struct mv88e6xxx_chip *chip, int port)
+int mv88e6097_port_pause_limit(struct mv88e6xxx_chip *chip, int port, u8 in,
+			       u8 out)
 {
-	return mv88e6xxx_port_write(chip, port, PORT_PAUSE_CTRL, 0x0000);
+	return mv88e6xxx_port_write(chip, port, MV88E6097_PORT_JAM_CTL,
+				    out << 8 | in);
 }
 
-int mv88e6390_port_pause_config(struct mv88e6xxx_chip *chip, int port)
+int mv88e6390_port_pause_limit(struct mv88e6xxx_chip *chip, int port, u8 in,
+			       u8 out)
 {
 	int err;
 
-	err = mv88e6xxx_port_write(chip, port, PORT_PAUSE_CTRL,
-				   PORT_FLOW_CTRL_LIMIT_IN | 0);
+	err = mv88e6xxx_port_write(chip, port, MV88E6390_PORT_FLOW_CTL,
+				   MV88E6390_PORT_FLOW_CTL_UPDATE |
+				   MV88E6390_PORT_FLOW_CTL_LIMIT_IN | in);
 	if (err)
 		return err;
 
-	return mv88e6xxx_port_write(chip, port, PORT_PAUSE_CTRL,
-				    PORT_FLOW_CTRL_LIMIT_OUT | 0);
+	return mv88e6xxx_port_write(chip, port, MV88E6390_PORT_FLOW_CTL,
+				    MV88E6390_PORT_FLOW_CTL_UPDATE |
+				    MV88E6390_PORT_FLOW_CTL_LIMIT_OUT | out);
 }
 
 /* Offset 0x04: Port Control Register */
 
 static const char * const mv88e6xxx_port_state_names[] = {
-	[PORT_CONTROL_STATE_DISABLED] = "Disabled",
-	[PORT_CONTROL_STATE_BLOCKING] = "Blocking/Listening",
-	[PORT_CONTROL_STATE_LEARNING] = "Learning",
-	[PORT_CONTROL_STATE_FORWARDING] = "Forwarding",
+	[MV88E6XXX_PORT_CTL0_STATE_DISABLED] = "Disabled",
+	[MV88E6XXX_PORT_CTL0_STATE_BLOCKING] = "Blocking/Listening",
+	[MV88E6XXX_PORT_CTL0_STATE_LEARNING] = "Learning",
+	[MV88E6XXX_PORT_CTL0_STATE_FORWARDING] = "Forwarding",
 };
 
 int mv88e6xxx_port_set_state(struct mv88e6xxx_chip *chip, int port, u8 state)
@@ -407,37 +420,72 @@ int mv88e6xxx_port_set_state(struct mv88e6xxx_chip *chip, int port, u8 state)
 	u16 reg;
 	int err;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL0, &reg);
 	if (err)
 		return err;
 
-	reg &= ~PORT_CONTROL_STATE_MASK;
+	reg &= ~MV88E6XXX_PORT_CTL0_STATE_MASK;
+
+	switch (state) {
+	case BR_STATE_DISABLED:
+		state = MV88E6XXX_PORT_CTL0_STATE_DISABLED;
+		break;
+	case BR_STATE_BLOCKING:
+	case BR_STATE_LISTENING:
+		state = MV88E6XXX_PORT_CTL0_STATE_BLOCKING;
+		break;
+	case BR_STATE_LEARNING:
+		state = MV88E6XXX_PORT_CTL0_STATE_LEARNING;
+		break;
+	case BR_STATE_FORWARDING:
+		state = MV88E6XXX_PORT_CTL0_STATE_FORWARDING;
+		break;
+	default:
+		return -EINVAL;
+	}
+
 	reg |= state;
 
-	err = mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
+	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 	if (err)
 		return err;
 
-	netdev_dbg(chip->ds->ports[port].netdev, "PortState set to %s\n",
-		   mv88e6xxx_port_state_names[state]);
+	dev_dbg(chip->dev, "p%d: PortState set to %s\n", port,
+		mv88e6xxx_port_state_names[state]);
 
 	return 0;
 }
 
 int mv88e6xxx_port_set_egress_mode(struct mv88e6xxx_chip *chip, int port,
-				   u16 mode)
+				   enum mv88e6xxx_egress_mode mode)
 {
 	int err;
 	u16 reg;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL0, &reg);
 	if (err)
 		return err;
 
-	reg &= ~PORT_CONTROL_EGRESS_MASK;
-	reg |= mode;
+	reg &= ~MV88E6XXX_PORT_CTL0_EGRESS_MODE_MASK;
 
-	return mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
+	switch (mode) {
+	case MV88E6XXX_EGRESS_MODE_UNMODIFIED:
+		reg |= MV88E6XXX_PORT_CTL0_EGRESS_MODE_UNMODIFIED;
+		break;
+	case MV88E6XXX_EGRESS_MODE_UNTAGGED:
+		reg |= MV88E6XXX_PORT_CTL0_EGRESS_MODE_UNTAGGED;
+		break;
+	case MV88E6XXX_EGRESS_MODE_TAGGED:
+		reg |= MV88E6XXX_PORT_CTL0_EGRESS_MODE_TAGGED;
+		break;
+	case MV88E6XXX_EGRESS_MODE_ETHERTYPE:
+		reg |= MV88E6XXX_PORT_CTL0_EGRESS_MODE_ETHER_TYPE_DSA;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 }
 
 int mv88e6085_port_set_frame_mode(struct mv88e6xxx_chip *chip, int port,
@@ -446,24 +494,24 @@ int mv88e6085_port_set_frame_mode(struct mv88e6xxx_chip *chip, int port,
 	int err;
 	u16 reg;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL0, &reg);
 	if (err)
 		return err;
 
-	reg &= ~PORT_CONTROL_FRAME_MODE_DSA;
+	reg &= ~MV88E6XXX_PORT_CTL0_FRAME_MODE_MASK;
 
 	switch (mode) {
 	case MV88E6XXX_FRAME_MODE_NORMAL:
-		reg |= PORT_CONTROL_FRAME_MODE_NORMAL;
+		reg |= MV88E6XXX_PORT_CTL0_FRAME_MODE_NORMAL;
 		break;
 	case MV88E6XXX_FRAME_MODE_DSA:
-		reg |= PORT_CONTROL_FRAME_MODE_DSA;
+		reg |= MV88E6XXX_PORT_CTL0_FRAME_MODE_DSA;
 		break;
 	default:
 		return -EINVAL;
 	}
 
-	return mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 }
 
 int mv88e6351_port_set_frame_mode(struct mv88e6xxx_chip *chip, int port,
@@ -472,30 +520,30 @@ int mv88e6351_port_set_frame_mode(struct mv88e6xxx_chip *chip, int port,
 	int err;
 	u16 reg;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL0, &reg);
 	if (err)
 		return err;
 
-	reg &= ~PORT_CONTROL_FRAME_MASK;
+	reg &= ~MV88E6XXX_PORT_CTL0_FRAME_MODE_MASK;
 
 	switch (mode) {
 	case MV88E6XXX_FRAME_MODE_NORMAL:
-		reg |= PORT_CONTROL_FRAME_MODE_NORMAL;
+		reg |= MV88E6XXX_PORT_CTL0_FRAME_MODE_NORMAL;
 		break;
 	case MV88E6XXX_FRAME_MODE_DSA:
-		reg |= PORT_CONTROL_FRAME_MODE_DSA;
+		reg |= MV88E6XXX_PORT_CTL0_FRAME_MODE_DSA;
 		break;
 	case MV88E6XXX_FRAME_MODE_PROVIDER:
-		reg |= PORT_CONTROL_FRAME_MODE_PROVIDER;
+		reg |= MV88E6XXX_PORT_CTL0_FRAME_MODE_PROVIDER;
 		break;
 	case MV88E6XXX_FRAME_MODE_ETHERTYPE:
-		reg |= PORT_CONTROL_FRAME_ETHER_TYPE_DSA;
+		reg |= MV88E6XXX_PORT_CTL0_FRAME_MODE_ETHER_TYPE_DSA;
 		break;
 	default:
 		return -EINVAL;
 	}
 
-	return mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 }
 
 static int mv88e6185_port_set_forward_unknown(struct mv88e6xxx_chip *chip,
@@ -504,16 +552,16 @@ static int mv88e6185_port_set_forward_unknown(struct mv88e6xxx_chip *chip,
 	int err;
 	u16 reg;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL0, &reg);
 	if (err)
 		return err;
 
 	if (unicast)
-		reg |= PORT_CONTROL_FORWARD_UNKNOWN;
+		reg |= MV88E6185_PORT_CTL0_FORWARD_UNKNOWN;
 	else
-		reg &= ~PORT_CONTROL_FORWARD_UNKNOWN;
+		reg &= ~MV88E6185_PORT_CTL0_FORWARD_UNKNOWN;
 
-	return mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 }
 
 int mv88e6352_port_set_egress_floods(struct mv88e6xxx_chip *chip, int port,
@@ -522,22 +570,22 @@ int mv88e6352_port_set_egress_floods(struct mv88e6xxx_chip *chip, int port,
 	int err;
 	u16 reg;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL0, &reg);
 	if (err)
 		return err;
 
-	reg &= ~PORT_CONTROL_EGRESS_FLOODS_MASK;
+	reg &= ~MV88E6352_PORT_CTL0_EGRESS_FLOODS_MASK;
 
 	if (unicast && multicast)
-		reg |= PORT_CONTROL_EGRESS_FLOODS_ALL_UNKNOWN_DA;
+		reg |= MV88E6352_PORT_CTL0_EGRESS_FLOODS_ALL_UNKNOWN_DA;
 	else if (unicast)
-		reg |= PORT_CONTROL_EGRESS_FLOODS_NO_UNKNOWN_MC_DA;
+		reg |= MV88E6352_PORT_CTL0_EGRESS_FLOODS_NO_UNKNOWN_MC_DA;
 	else if (multicast)
-		reg |= PORT_CONTROL_EGRESS_FLOODS_NO_UNKNOWN_UC_DA;
+		reg |= MV88E6352_PORT_CTL0_EGRESS_FLOODS_NO_UNKNOWN_UC_DA;
 	else
-		reg |= PORT_CONTROL_EGRESS_FLOODS_NO_UNKNOWN_DA;
+		reg |= MV88E6352_PORT_CTL0_EGRESS_FLOODS_NO_UNKNOWN_DA;
 
-	return mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 }
 
 /* Offset 0x05: Port Control 1 */
@@ -548,16 +596,16 @@ int mv88e6xxx_port_set_message_port(struct mv88e6xxx_chip *chip, int port,
 	u16 val;
 	int err;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_1, &val);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL1, &val);
 	if (err)
 		return err;
 
 	if (message_port)
-		val |= PORT_CONTROL_1_MESSAGE_PORT;
+		val |= MV88E6XXX_PORT_CTL1_MESSAGE_PORT;
 	else
-		val &= ~PORT_CONTROL_1_MESSAGE_PORT;
+		val &= ~MV88E6XXX_PORT_CTL1_MESSAGE_PORT;
 
-	return mv88e6xxx_port_write(chip, port, PORT_CONTROL_1, val);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL1, val);
 }
 
 /* Offset 0x06: Port Based VLAN Map */
@@ -568,19 +616,18 @@ int mv88e6xxx_port_set_vlan_map(struct mv88e6xxx_chip *chip, int port, u16 map)
 	u16 reg;
 	int err;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_BASE_VLAN, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_BASE_VLAN, &reg);
 	if (err)
 		return err;
 
 	reg &= ~mask;
 	reg |= map & mask;
 
-	err = mv88e6xxx_port_write(chip, port, PORT_BASE_VLAN, reg);
+	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_BASE_VLAN, reg);
 	if (err)
 		return err;
 
-	netdev_dbg(chip->ds->ports[port].netdev, "VLANTable set to %.3x\n",
-		   map);
+	dev_dbg(chip->dev, "p%d: VLANTable set to %.3x\n", port, map);
 
 	return 0;
 }
@@ -592,7 +639,7 @@ int mv88e6xxx_port_get_fid(struct mv88e6xxx_chip *chip, int port, u16 *fid)
 	int err;
 
 	/* Port's default FID lower 4 bits are located in reg 0x06, offset 12 */
-	err = mv88e6xxx_port_read(chip, port, PORT_BASE_VLAN, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_BASE_VLAN, &reg);
 	if (err)
 		return err;
 
@@ -600,7 +647,8 @@ int mv88e6xxx_port_get_fid(struct mv88e6xxx_chip *chip, int port, u16 *fid)
 
 	/* Port's default FID upper bits are located in reg 0x05, offset 0 */
 	if (upper_mask) {
-		err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_1, &reg);
+		err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL1,
+					  &reg);
 		if (err)
 			return err;
 
@@ -620,32 +668,34 @@ int mv88e6xxx_port_set_fid(struct mv88e6xxx_chip *chip, int port, u16 fid)
 		return -EINVAL;
 
 	/* Port's default FID lower 4 bits are located in reg 0x06, offset 12 */
-	err = mv88e6xxx_port_read(chip, port, PORT_BASE_VLAN, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_BASE_VLAN, &reg);
 	if (err)
 		return err;
 
 	reg &= 0x0fff;
 	reg |= (fid & 0x000f) << 12;
 
-	err = mv88e6xxx_port_write(chip, port, PORT_BASE_VLAN, reg);
+	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_BASE_VLAN, reg);
 	if (err)
 		return err;
 
 	/* Port's default FID upper bits are located in reg 0x05, offset 0 */
 	if (upper_mask) {
-		err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_1, &reg);
+		err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL1,
+					  &reg);
 		if (err)
 			return err;
 
 		reg &= ~upper_mask;
 		reg |= (fid >> 4) & upper_mask;
 
-		err = mv88e6xxx_port_write(chip, port, PORT_CONTROL_1, reg);
+		err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL1,
+					   reg);
 		if (err)
 			return err;
 	}
 
-	netdev_dbg(chip->ds->ports[port].netdev, "FID set to %u\n", fid);
+	dev_dbg(chip->dev, "p%d: FID set to %u\n", port, fid);
 
 	return 0;
 }
@@ -657,11 +707,12 @@ int mv88e6xxx_port_get_pvid(struct mv88e6xxx_chip *chip, int port, u16 *pvid)
 	u16 reg;
 	int err;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_DEFAULT_VLAN, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_DEFAULT_VLAN,
+				  &reg);
 	if (err)
 		return err;
 
-	*pvid = reg & PORT_DEFAULT_VLAN_MASK;
+	*pvid = reg & MV88E6XXX_PORT_DEFAULT_VLAN_MASK;
 
 	return 0;
 }
@@ -671,19 +722,20 @@ int mv88e6xxx_port_set_pvid(struct mv88e6xxx_chip *chip, int port, u16 pvid)
 	u16 reg;
 	int err;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_DEFAULT_VLAN, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_DEFAULT_VLAN,
+				  &reg);
 	if (err)
 		return err;
 
-	reg &= ~PORT_DEFAULT_VLAN_MASK;
-	reg |= pvid & PORT_DEFAULT_VLAN_MASK;
+	reg &= ~MV88E6XXX_PORT_DEFAULT_VLAN_MASK;
+	reg |= pvid & MV88E6XXX_PORT_DEFAULT_VLAN_MASK;
 
-	err = mv88e6xxx_port_write(chip, port, PORT_DEFAULT_VLAN, reg);
+	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_DEFAULT_VLAN,
+				   reg);
 	if (err)
 		return err;
 
-	netdev_dbg(chip->ds->ports[port].netdev, "DefaultVID set to %u\n",
-		   pvid);
+	dev_dbg(chip->dev, "p%d: DefaultVID set to %u\n", port, pvid);
 
 	return 0;
 }
@@ -691,10 +743,10 @@ int mv88e6xxx_port_set_pvid(struct mv88e6xxx_chip *chip, int port, u16 pvid)
 /* Offset 0x08: Port Control 2 Register */
 
 static const char * const mv88e6xxx_port_8021q_mode_names[] = {
-	[PORT_CONTROL_2_8021Q_DISABLED] = "Disabled",
-	[PORT_CONTROL_2_8021Q_FALLBACK] = "Fallback",
-	[PORT_CONTROL_2_8021Q_CHECK] = "Check",
-	[PORT_CONTROL_2_8021Q_SECURE] = "Secure",
+	[MV88E6XXX_PORT_CTL2_8021Q_MODE_DISABLED] = "Disabled",
+	[MV88E6XXX_PORT_CTL2_8021Q_MODE_FALLBACK] = "Fallback",
+	[MV88E6XXX_PORT_CTL2_8021Q_MODE_CHECK] = "Check",
+	[MV88E6XXX_PORT_CTL2_8021Q_MODE_SECURE] = "Secure",
 };
 
 static int mv88e6185_port_set_default_forward(struct mv88e6xxx_chip *chip,
@@ -703,16 +755,16 @@ static int mv88e6185_port_set_default_forward(struct mv88e6xxx_chip *chip,
 	int err;
 	u16 reg;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_2, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
 	if (err)
 		return err;
 
 	if (multicast)
-		reg |= PORT_CONTROL_2_DEFAULT_FORWARD;
+		reg |= MV88E6XXX_PORT_CTL2_DEFAULT_FORWARD;
 	else
-		reg &= ~PORT_CONTROL_2_DEFAULT_FORWARD;
+		reg &= ~MV88E6XXX_PORT_CTL2_DEFAULT_FORWARD;
 
-	return mv88e6xxx_port_write(chip, port, PORT_CONTROL_2, reg);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
 }
 
 int mv88e6185_port_set_egress_floods(struct mv88e6xxx_chip *chip, int port,
@@ -733,14 +785,14 @@ int mv88e6095_port_set_upstream_port(struct mv88e6xxx_chip *chip, int port,
 	int err;
 	u16 reg;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_2, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
 	if (err)
 		return err;
 
-	reg &= ~PORT_CONTROL_2_UPSTREAM_MASK;
+	reg &= ~MV88E6095_PORT_CTL2_CPU_PORT_MASK;
 	reg |= upstream_port;
 
-	return mv88e6xxx_port_write(chip, port, PORT_CONTROL_2, reg);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
 }
 
 int mv88e6xxx_port_set_8021q_mode(struct mv88e6xxx_chip *chip, int port,
@@ -749,19 +801,19 @@ int mv88e6xxx_port_set_8021q_mode(struct mv88e6xxx_chip *chip, int port,
 	u16 reg;
 	int err;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_2, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
 	if (err)
 		return err;
 
-	reg &= ~PORT_CONTROL_2_8021Q_MASK;
-	reg |= mode & PORT_CONTROL_2_8021Q_MASK;
+	reg &= ~MV88E6XXX_PORT_CTL2_8021Q_MODE_MASK;
+	reg |= mode & MV88E6XXX_PORT_CTL2_8021Q_MODE_MASK;
 
-	err = mv88e6xxx_port_write(chip, port, PORT_CONTROL_2, reg);
+	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
 	if (err)
 		return err;
 
-	netdev_dbg(chip->ds->ports[port].netdev, "802.1QMode set to %s\n",
-		   mv88e6xxx_port_8021q_mode_names[mode]);
+	dev_dbg(chip->dev, "p%d: 802.1QMode set to %s\n", port,
+		mv88e6xxx_port_8021q_mode_names[mode]);
 
 	return 0;
 }
@@ -771,53 +823,65 @@ int mv88e6xxx_port_set_map_da(struct mv88e6xxx_chip *chip, int port)
 	u16 reg;
 	int err;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_2, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
 	if (err)
 		return err;
 
-	reg |= PORT_CONTROL_2_MAP_DA;
+	reg |= MV88E6XXX_PORT_CTL2_MAP_DA;
 
-	return mv88e6xxx_port_write(chip, port, PORT_CONTROL_2, reg);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
 }
 
-int mv88e6165_port_jumbo_config(struct mv88e6xxx_chip *chip, int port)
+int mv88e6165_port_set_jumbo_size(struct mv88e6xxx_chip *chip, int port,
+				  size_t size)
 {
 	u16 reg;
 	int err;
 
-	err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_2, &reg);
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
 	if (err)
 		return err;
 
-	reg |= PORT_CONTROL_2_JUMBO_10240;
+	reg &= ~MV88E6XXX_PORT_CTL2_JUMBO_MODE_MASK;
+
+	if (size <= 1522)
+		reg |= MV88E6XXX_PORT_CTL2_JUMBO_MODE_1522;
+	else if (size <= 2048)
+		reg |= MV88E6XXX_PORT_CTL2_JUMBO_MODE_2048;
+	else if (size <= 10240)
+		reg |= MV88E6XXX_PORT_CTL2_JUMBO_MODE_10240;
+	else
+		return -ERANGE;
 
-	return mv88e6xxx_port_write(chip, port, PORT_CONTROL_2, reg);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
 }
 
 /* Offset 0x09: Port Rate Control */
 
 int mv88e6095_port_egress_rate_limiting(struct mv88e6xxx_chip *chip, int port)
 {
-	return mv88e6xxx_port_write(chip, port, PORT_RATE_CONTROL, 0x0000);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_EGRESS_RATE_CTL1,
+				    0x0000);
 }
 
 int mv88e6097_port_egress_rate_limiting(struct mv88e6xxx_chip *chip, int port)
 {
-	return mv88e6xxx_port_write(chip, port, PORT_RATE_CONTROL, 0x0001);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_EGRESS_RATE_CTL1,
+				    0x0001);
 }
 
 /* Offset 0x0C: Port ATU Control */
 
 int mv88e6xxx_port_disable_learn_limit(struct mv88e6xxx_chip *chip, int port)
 {
-	return mv88e6xxx_port_write(chip, port, PORT_ATU_CONTROL, 0);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_ATU_CTL, 0);
 }
 
 /* Offset 0x0D: (Priority) Override Register */
 
 int mv88e6xxx_port_disable_pri_override(struct mv88e6xxx_chip *chip, int port)
 {
-	return mv88e6xxx_port_write(chip, port, PORT_PRI_OVERRIDE, 0);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_PRI_OVERRIDE, 0);
 }
 
 /* Offset 0x0f: Port Ether type */
@@ -825,7 +889,7 @@ int mv88e6xxx_port_disable_pri_override(struct mv88e6xxx_chip *chip, int port)
 int mv88e6351_port_set_ether_type(struct mv88e6xxx_chip *chip, int port,
 				  u16 etype)
 {
-	return mv88e6xxx_port_write(chip, port, PORT_ETH_TYPE, etype);
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_ETH_TYPE, etype);
 }
 
 /* Offset 0x18: Port IEEE Priority Remapping Registers [0-3]
@@ -837,53 +901,54 @@ int mv88e6095_port_tag_remap(struct mv88e6xxx_chip *chip, int port)
 	int err;
 
 	/* Use a direct priority mapping for all IEEE tagged frames */
-	err = mv88e6xxx_port_write(chip, port, PORT_TAG_REGMAP_0123, 0x3210);
+	err = mv88e6xxx_port_write(chip, port,
+				   MV88E6095_PORT_IEEE_PRIO_REMAP_0123,
+				   0x3210);
 	if (err)
 		return err;
 
-	return mv88e6xxx_port_write(chip, port, PORT_TAG_REGMAP_4567, 0x7654);
+	return mv88e6xxx_port_write(chip, port,
+				    MV88E6095_PORT_IEEE_PRIO_REMAP_4567,
+				    0x7654);
 }
 
 static int mv88e6xxx_port_ieeepmt_write(struct mv88e6xxx_chip *chip,
-					int port, u16 table,
-					u8 pointer, u16 data)
+					int port, u16 table, u8 ptr, u16 data)
 {
 	u16 reg;
 
-	reg = PORT_IEEE_PRIO_MAP_TABLE_UPDATE |
-		table |
-		(pointer << PORT_IEEE_PRIO_MAP_TABLE_POINTER_SHIFT) |
-		data;
+	reg = MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_UPDATE | table |
+		(ptr << __bf_shf(MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_PTR_MASK)) |
+		(data & MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_DATA_MASK);
 
-	return mv88e6xxx_port_write(chip, port, PORT_IEEE_PRIO_MAP_TABLE, reg);
+	return mv88e6xxx_port_write(chip, port,
+				    MV88E6390_PORT_IEEE_PRIO_MAP_TABLE, reg);
 }
 
 int mv88e6390_port_tag_remap(struct mv88e6xxx_chip *chip, int port)
 {
 	int err, i;
+	u16 table;
 
 	for (i = 0; i <= 7; i++) {
-		err = mv88e6xxx_port_ieeepmt_write(
-			chip, port, PORT_IEEE_PRIO_MAP_TABLE_INGRESS_PCP,
-			i, (i | i << 4));
+		table = MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_INGRESS_PCP;
+		err = mv88e6xxx_port_ieeepmt_write(chip, port, table, i,
+						   (i | i << 4));
 		if (err)
 			return err;
 
-		err = mv88e6xxx_port_ieeepmt_write(
-			chip, port, PORT_IEEE_PRIO_MAP_TABLE_EGRESS_GREEN_PCP,
-			i, i);
+		table = MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_EGRESS_GREEN_PCP;
+		err = mv88e6xxx_port_ieeepmt_write(chip, port, table, i, i);
 		if (err)
 			return err;
 
-		err = mv88e6xxx_port_ieeepmt_write(
-			chip, port, PORT_IEEE_PRIO_MAP_TABLE_EGRESS_YELLOW_PCP,
-			i, i);
+		table = MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_EGRESS_YELLOW_PCP;
+		err = mv88e6xxx_port_ieeepmt_write(chip, port, table, i, i);
 		if (err)
 			return err;
 
-		err = mv88e6xxx_port_ieeepmt_write(
-			chip, port, PORT_IEEE_PRIO_MAP_TABLE_EGRESS_AVB_PCP,
-			i, i);
+		table = MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_EGRESS_AVB_PCP;
+		err = mv88e6xxx_port_ieeepmt_write(chip, port, table, i, i);
 		if (err)
 			return err;
 	}
diff --git a/drivers/net/dsa/mv88e6xxx/port.h b/drivers/net/dsa/mv88e6xxx/port.h
index 86f40887b6d2..8f3991bf1851 100644
--- a/drivers/net/dsa/mv88e6xxx/port.h
+++ b/drivers/net/dsa/mv88e6xxx/port.h
@@ -15,7 +15,229 @@
 #ifndef _MV88E6XXX_PORT_H
 #define _MV88E6XXX_PORT_H
 
-#include "mv88e6xxx.h"
+#include "chip.h"
+
+/* Offset 0x00: Port Status Register */
+#define MV88E6XXX_PORT_STS			0x00
+#define MV88E6XXX_PORT_STS_PAUSE_EN		0x8000
+#define MV88E6XXX_PORT_STS_MY_PAUSE		0x4000
+#define MV88E6XXX_PORT_STS_HD_FLOW		0x2000
+#define MV88E6XXX_PORT_STS_PHY_DETECT		0x1000
+#define MV88E6XXX_PORT_STS_LINK			0x0800
+#define MV88E6XXX_PORT_STS_DUPLEX		0x0400
+#define MV88E6XXX_PORT_STS_SPEED_MASK		0x0300
+#define MV88E6XXX_PORT_STS_SPEED_10		0x0000
+#define MV88E6XXX_PORT_STS_SPEED_100		0x0100
+#define MV88E6XXX_PORT_STS_SPEED_1000		0x0200
+#define MV88E6352_PORT_STS_EEE			0x0040
+#define MV88E6165_PORT_STS_AM_DIS		0x0040
+#define MV88E6185_PORT_STS_MGMII		0x0040
+#define MV88E6XXX_PORT_STS_TX_PAUSED		0x0020
+#define MV88E6XXX_PORT_STS_FLOW_CTL		0x0010
+#define MV88E6XXX_PORT_STS_CMODE_MASK		0x000f
+#define MV88E6XXX_PORT_STS_CMODE_100BASE_X	0x0008
+#define MV88E6XXX_PORT_STS_CMODE_1000BASE_X	0x0009
+#define MV88E6XXX_PORT_STS_CMODE_SGMII		0x000a
+#define MV88E6XXX_PORT_STS_CMODE_2500BASEX	0x000b
+#define MV88E6XXX_PORT_STS_CMODE_XAUI		0x000c
+#define MV88E6XXX_PORT_STS_CMODE_RXAUI		0x000d
+
+/* Offset 0x01: MAC (or PCS or Physical) Control Register */
+#define MV88E6XXX_PORT_MAC_CTL				0x01
+#define MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_RXCLK	0x8000
+#define MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_TXCLK	0x4000
+#define MV88E6390_PORT_MAC_CTL_FORCE_SPEED		0x2000
+#define MV88E6390_PORT_MAC_CTL_ALTSPEED			0x1000
+#define MV88E6352_PORT_MAC_CTL_200BASE			0x1000
+#define MV88E6XXX_PORT_MAC_CTL_FC			0x0080
+#define MV88E6XXX_PORT_MAC_CTL_FORCE_FC			0x0040
+#define MV88E6XXX_PORT_MAC_CTL_LINK_UP			0x0020
+#define MV88E6XXX_PORT_MAC_CTL_FORCE_LINK		0x0010
+#define MV88E6XXX_PORT_MAC_CTL_DUPLEX_FULL		0x0008
+#define MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX		0x0004
+#define MV88E6XXX_PORT_MAC_CTL_SPEED_MASK		0x0003
+#define MV88E6XXX_PORT_MAC_CTL_SPEED_10			0x0000
+#define MV88E6XXX_PORT_MAC_CTL_SPEED_100		0x0001
+#define MV88E6065_PORT_MAC_CTL_SPEED_200		0x0002
+#define MV88E6XXX_PORT_MAC_CTL_SPEED_1000		0x0002
+#define MV88E6390_PORT_MAC_CTL_SPEED_10000		0x0003
+#define MV88E6XXX_PORT_MAC_CTL_SPEED_UNFORCED		0x0003
+
+/* Offset 0x02: Jamming Control Register */
+#define MV88E6097_PORT_JAM_CTL			0x02
+#define MV88E6097_PORT_JAM_CTL_LIMIT_OUT_MASK	0xff00
+#define MV88E6097_PORT_JAM_CTL_LIMIT_IN_MASK	0x00ff
+
+/* Offset 0x02: Flow Control Register */
+#define MV88E6390_PORT_FLOW_CTL			0x02
+#define MV88E6390_PORT_FLOW_CTL_UPDATE		0x8000
+#define MV88E6390_PORT_FLOW_CTL_PTR_MASK	0x7f00
+#define MV88E6390_PORT_FLOW_CTL_LIMIT_IN	0x0000
+#define MV88E6390_PORT_FLOW_CTL_LIMIT_OUT	0x0100
+#define MV88E6390_PORT_FLOW_CTL_DATA_MASK	0x00ff
+
+/* Offset 0x03: Switch Identifier Register */
+#define MV88E6XXX_PORT_SWITCH_ID		0x03
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_MASK	0xfff0
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6085	0x04a0
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6095	0x0950
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6097	0x0990
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6190X	0x0a00
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6390X	0x0a10
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6131	0x1060
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6320	0x1150
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6123	0x1210
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6161	0x1610
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6165	0x1650
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6171	0x1710
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6172	0x1720
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6175	0x1750
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6176	0x1760
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6190	0x1900
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6191	0x1910
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6185	0x1a70
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6240	0x2400
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6290	0x2900
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6321	0x3100
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6141	0x3400
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6341	0x3410
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6352	0x3520
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6350	0x3710
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6351	0x3750
+#define MV88E6XXX_PORT_SWITCH_ID_PROD_6390	0x3900
+#define MV88E6XXX_PORT_SWITCH_ID_REV_MASK	0x000f
+
+/* Offset 0x04: Port Control Register */
+#define MV88E6XXX_PORT_CTL0					0x04
+#define MV88E6XXX_PORT_CTL0_USE_CORE_TAG			0x8000
+#define MV88E6XXX_PORT_CTL0_DROP_ON_LOCK			0x4000
+#define MV88E6XXX_PORT_CTL0_EGRESS_MODE_MASK			0x3000
+#define MV88E6XXX_PORT_CTL0_EGRESS_MODE_UNMODIFIED		0x0000
+#define MV88E6XXX_PORT_CTL0_EGRESS_MODE_UNTAGGED		0x1000
+#define MV88E6XXX_PORT_CTL0_EGRESS_MODE_TAGGED			0x2000
+#define MV88E6XXX_PORT_CTL0_EGRESS_MODE_ETHER_TYPE_DSA		0x3000
+#define MV88E6XXX_PORT_CTL0_HEADER				0x0800
+#define MV88E6XXX_PORT_CTL0_IGMP_MLD_SNOOP			0x0400
+#define MV88E6XXX_PORT_CTL0_DOUBLE_TAG				0x0200
+#define MV88E6XXX_PORT_CTL0_FRAME_MODE_MASK			0x0300
+#define MV88E6XXX_PORT_CTL0_FRAME_MODE_NORMAL			0x0000
+#define MV88E6XXX_PORT_CTL0_FRAME_MODE_DSA			0x0100
+#define MV88E6XXX_PORT_CTL0_FRAME_MODE_PROVIDER			0x0200
+#define MV88E6XXX_PORT_CTL0_FRAME_MODE_ETHER_TYPE_DSA		0x0300
+#define MV88E6XXX_PORT_CTL0_DSA_TAG				0x0100
+#define MV88E6XXX_PORT_CTL0_VLAN_TUNNEL				0x0080
+#define MV88E6XXX_PORT_CTL0_TAG_IF_BOTH				0x0040
+#define MV88E6185_PORT_CTL0_USE_IP				0x0020
+#define MV88E6185_PORT_CTL0_USE_TAG				0x0010
+#define MV88E6185_PORT_CTL0_FORWARD_UNKNOWN			0x0004
+#define MV88E6352_PORT_CTL0_EGRESS_FLOODS_MASK			0x000c
+#define MV88E6352_PORT_CTL0_EGRESS_FLOODS_NO_UNKNOWN_DA		0x0000
+#define MV88E6352_PORT_CTL0_EGRESS_FLOODS_NO_UNKNOWN_MC_DA	0x0004
+#define MV88E6352_PORT_CTL0_EGRESS_FLOODS_NO_UNKNOWN_UC_DA	0x0008
+#define MV88E6352_PORT_CTL0_EGRESS_FLOODS_ALL_UNKNOWN_DA	0x000c
+#define MV88E6XXX_PORT_CTL0_STATE_MASK				0x0003
+#define MV88E6XXX_PORT_CTL0_STATE_DISABLED			0x0000
+#define MV88E6XXX_PORT_CTL0_STATE_BLOCKING			0x0001
+#define MV88E6XXX_PORT_CTL0_STATE_LEARNING			0x0002
+#define MV88E6XXX_PORT_CTL0_STATE_FORWARDING			0x0003
+
+/* Offset 0x05: Port Control 1 */
+#define MV88E6XXX_PORT_CTL1			0x05
+#define MV88E6XXX_PORT_CTL1_MESSAGE_PORT	0x8000
+#define MV88E6XXX_PORT_CTL1_FID_11_4_MASK	0x00ff
+
+/* Offset 0x06: Port Based VLAN Map */
+#define MV88E6XXX_PORT_BASE_VLAN		0x06
+#define MV88E6XXX_PORT_BASE_VLAN_FID_3_0_MASK	0xf000
+
+/* Offset 0x07: Default Port VLAN ID & Priority */
+#define MV88E6XXX_PORT_DEFAULT_VLAN		0x07
+#define MV88E6XXX_PORT_DEFAULT_VLAN_MASK	0x0fff
+
+/* Offset 0x08: Port Control 2 Register */
+#define MV88E6XXX_PORT_CTL2				0x08
+#define MV88E6XXX_PORT_CTL2_IGNORE_FCS			0x8000
+#define MV88E6XXX_PORT_CTL2_VTU_PRI_OVERRIDE		0x4000
+#define MV88E6XXX_PORT_CTL2_SA_PRIO_OVERRIDE		0x2000
+#define MV88E6XXX_PORT_CTL2_DA_PRIO_OVERRIDE		0x1000
+#define MV88E6XXX_PORT_CTL2_JUMBO_MODE_MASK		0x3000
+#define MV88E6XXX_PORT_CTL2_JUMBO_MODE_1522		0x0000
+#define MV88E6XXX_PORT_CTL2_JUMBO_MODE_2048		0x1000
+#define MV88E6XXX_PORT_CTL2_JUMBO_MODE_10240		0x2000
+#define MV88E6XXX_PORT_CTL2_8021Q_MODE_MASK		0x0c00
+#define MV88E6XXX_PORT_CTL2_8021Q_MODE_DISABLED		0x0000
+#define MV88E6XXX_PORT_CTL2_8021Q_MODE_FALLBACK		0x0400
+#define MV88E6XXX_PORT_CTL2_8021Q_MODE_CHECK		0x0800
+#define MV88E6XXX_PORT_CTL2_8021Q_MODE_SECURE		0x0c00
+#define MV88E6XXX_PORT_CTL2_DISCARD_TAGGED		0x0200
+#define MV88E6XXX_PORT_CTL2_DISCARD_UNTAGGED		0x0100
+#define MV88E6XXX_PORT_CTL2_MAP_DA			0x0080
+#define MV88E6XXX_PORT_CTL2_DEFAULT_FORWARD		0x0040
+#define MV88E6XXX_PORT_CTL2_EGRESS_MONITOR		0x0020
+#define MV88E6XXX_PORT_CTL2_INGRESS_MONITOR		0x0010
+#define MV88E6095_PORT_CTL2_CPU_PORT_MASK		0x000f
+
+/* Offset 0x09: Egress Rate Control */
+#define MV88E6XXX_PORT_EGRESS_RATE_CTL1		0x09
+
+/* Offset 0x0A: Egress Rate Control 2 */
+#define MV88E6XXX_PORT_EGRESS_RATE_CTL2		0x0a
+
+/* Offset 0x0B: Port Association Vector */
+#define MV88E6XXX_PORT_ASSOC_VECTOR			0x0b
+#define MV88E6XXX_PORT_ASSOC_VECTOR_HOLD_AT_1		0x8000
+#define MV88E6XXX_PORT_ASSOC_VECTOR_INT_AGE_OUT		0x4000
+#define MV88E6XXX_PORT_ASSOC_VECTOR_LOCKED_PORT		0x2000
+#define MV88E6XXX_PORT_ASSOC_VECTOR_IGNORE_WRONG	0x1000
+#define MV88E6XXX_PORT_ASSOC_VECTOR_REFRESH_LOCKED	0x0800
+
+/* Offset 0x0C: Port ATU Control */
+#define MV88E6XXX_PORT_ATU_CTL		0x0c
+
+/* Offset 0x0D: Priority Override Register */
+#define MV88E6XXX_PORT_PRI_OVERRIDE	0x0d
+
+/* Offset 0x0E: Policy Control Register */
+#define MV88E6XXX_PORT_POLICY_CTL	0x0e
+
+/* Offset 0x0F: Port Special Ether Type */
+#define MV88E6XXX_PORT_ETH_TYPE		0x0f
+#define MV88E6XXX_PORT_ETH_TYPE_DEFAULT	0x9100
+
+/* Offset 0x10: InDiscards Low Counter */
+#define MV88E6XXX_PORT_IN_DISCARD_LO	0x10
+
+/* Offset 0x11: InDiscards High Counter */
+#define MV88E6XXX_PORT_IN_DISCARD_HI	0x11
+
+/* Offset 0x12: InFiltered Counter */
+#define MV88E6XXX_PORT_IN_FILTERED	0x12
+
+/* Offset 0x13: OutFiltered Counter */
+#define MV88E6XXX_PORT_OUT_FILTERED	0x13
+
+/* Offset 0x16: LED Control */
+#define MV88E6XXX_PORT_LED_CONTROL	0x16
+
+/* Offset 0x18: IEEE Priority Mapping Table */
+#define MV88E6390_PORT_IEEE_PRIO_MAP_TABLE			0x18
+#define MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_UPDATE		0x8000
+#define MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_MASK			0x7000
+#define MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_INGRESS_PCP		0x0000
+#define MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_EGRESS_GREEN_PCP	0x1000
+#define MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_EGRESS_YELLOW_PCP	0x2000
+#define MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_EGRESS_AVB_PCP	0x3000
+#define MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_EGRESS_GREEN_DSCP	0x5000
+#define MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_EGRESS_YELLOW_DSCP	0x6000
+#define MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_EGRESS_AVB_DSCP	0x7000
+#define MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_PTR_MASK		0x0e00
+#define MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_DATA_MASK		0x01ff
+
+/* Offset 0x18: Port IEEE Priority Remapping Registers (0-3) */
+#define MV88E6095_PORT_IEEE_PRIO_REMAP_0123	0x18
+
+/* Offset 0x19: Port IEEE Priority Remapping Registers (4-7) */
+#define MV88E6095_PORT_IEEE_PRIO_REMAP_4567	0x19
 
 int mv88e6xxx_port_read(struct mv88e6xxx_chip *chip, int port, int reg,
 			u16 *val);
@@ -52,7 +274,7 @@ int mv88e6xxx_port_set_8021q_mode(struct mv88e6xxx_chip *chip, int port,
 int mv88e6095_port_tag_remap(struct mv88e6xxx_chip *chip, int port);
 int mv88e6390_port_tag_remap(struct mv88e6xxx_chip *chip, int port);
 int mv88e6xxx_port_set_egress_mode(struct mv88e6xxx_chip *chip, int port,
-				   u16 mode);
+				   enum mv88e6xxx_egress_mode mode);
 int mv88e6085_port_set_frame_mode(struct mv88e6xxx_chip *chip, int port,
 				  enum mv88e6xxx_frame_mode mode);
 int mv88e6351_port_set_frame_mode(struct mv88e6xxx_chip *chip, int port,
@@ -65,11 +287,14 @@ int mv88e6351_port_set_ether_type(struct mv88e6xxx_chip *chip, int port,
 				  u16 etype);
 int mv88e6xxx_port_set_message_port(struct mv88e6xxx_chip *chip, int port,
 				    bool message_port);
-int mv88e6165_port_jumbo_config(struct mv88e6xxx_chip *chip, int port);
+int mv88e6165_port_set_jumbo_size(struct mv88e6xxx_chip *chip, int port,
+				  size_t size);
 int mv88e6095_port_egress_rate_limiting(struct mv88e6xxx_chip *chip, int port);
 int mv88e6097_port_egress_rate_limiting(struct mv88e6xxx_chip *chip, int port);
-int mv88e6097_port_pause_config(struct mv88e6xxx_chip *chip, int port);
-int mv88e6390_port_pause_config(struct mv88e6xxx_chip *chip, int port);
+int mv88e6097_port_pause_limit(struct mv88e6xxx_chip *chip, int port, u8 in,
+			       u8 out);
+int mv88e6390_port_pause_limit(struct mv88e6xxx_chip *chip, int port, u8 in,
+			       u8 out);
 int mv88e6390x_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
 			      phy_interface_t mode);
 int mv88e6xxx_port_get_cmode(struct mv88e6xxx_chip *chip, int port, u8 *cmode);
diff --git a/drivers/net/dsa/mv88e6xxx/serdes.c b/drivers/net/dsa/mv88e6xxx/serdes.c
new file mode 100644
index 000000000000..f3c01119b3d1
--- /dev/null
+++ b/drivers/net/dsa/mv88e6xxx/serdes.c
@@ -0,0 +1,229 @@
+/*
+ * Marvell 88E6xxx SERDES manipulation, via SMI bus
+ *
+ * Copyright (c) 2008 Marvell Semiconductor
+ *
+ * Copyright (c) 2017 Andrew Lunn <andrew@lunn.ch>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/mii.h>
+
+#include "chip.h"
+#include "global2.h"
+#include "phy.h"
+#include "port.h"
+#include "serdes.h"
+
+static int mv88e6352_serdes_read(struct mv88e6xxx_chip *chip, int reg,
+				 u16 *val)
+{
+	return mv88e6xxx_phy_page_read(chip, MV88E6352_ADDR_SERDES,
+				       MV88E6352_SERDES_PAGE_FIBER,
+				       reg, val);
+}
+
+static int mv88e6352_serdes_write(struct mv88e6xxx_chip *chip, int reg,
+				  u16 val)
+{
+	return mv88e6xxx_phy_page_write(chip, MV88E6352_ADDR_SERDES,
+					MV88E6352_SERDES_PAGE_FIBER,
+					reg, val);
+}
+
+static int mv88e6352_serdes_power_set(struct mv88e6xxx_chip *chip, bool on)
+{
+	u16 val, new_val;
+	int err;
+
+	err = mv88e6352_serdes_read(chip, MII_BMCR, &val);
+	if (err)
+		return err;
+
+	if (on)
+		new_val = val & ~BMCR_PDOWN;
+	else
+		new_val = val | BMCR_PDOWN;
+
+	if (val != new_val)
+		err = mv88e6352_serdes_write(chip, MII_BMCR, new_val);
+
+	return err;
+}
+
+int mv88e6352_serdes_power(struct mv88e6xxx_chip *chip, int port, bool on)
+{
+	int err;
+	u8 cmode;
+
+	err = mv88e6xxx_port_get_cmode(chip, port, &cmode);
+	if (err)
+		return err;
+
+	if ((cmode == MV88E6XXX_PORT_STS_CMODE_100BASE_X) ||
+	    (cmode == MV88E6XXX_PORT_STS_CMODE_1000BASE_X) ||
+	    (cmode == MV88E6XXX_PORT_STS_CMODE_SGMII)) {
+		err = mv88e6352_serdes_power_set(chip, on);
+		if (err < 0)
+			return err;
+	}
+
+	return 0;
+}
+
+/* Set the power on/off for 10GBASE-R and 10GBASE-X4/X2 */
+static int mv88e6390_serdes_10g(struct mv88e6xxx_chip *chip, int addr, bool on)
+{
+	u16 val, new_val;
+	int reg_c45;
+	int err;
+
+	reg_c45 = MII_ADDR_C45 | MV88E6390_SERDES_DEVICE |
+		MV88E6390_PCS_CONTROL_1;
+	err = mv88e6xxx_phy_read(chip, addr, reg_c45, &val);
+	if (err)
+		return err;
+
+	if (on)
+		new_val = val & ~(MV88E6390_PCS_CONTROL_1_RESET |
+				  MV88E6390_PCS_CONTROL_1_LOOPBACK |
+				  MV88E6390_PCS_CONTROL_1_PDOWN);
+	else
+		new_val = val | MV88E6390_PCS_CONTROL_1_PDOWN;
+
+	if (val != new_val)
+		err = mv88e6xxx_phy_write(chip, addr, reg_c45, new_val);
+
+	return err;
+}
+
+/* Set the power on/off for 10GBASE-R and 10GBASE-X4/X2 */
+static int mv88e6390_serdes_sgmii(struct mv88e6xxx_chip *chip, int addr,
+				  bool on)
+{
+	u16 val, new_val;
+	int reg_c45;
+	int err;
+
+	reg_c45 = MII_ADDR_C45 | MV88E6390_SERDES_DEVICE |
+		MV88E6390_SGMII_CONTROL;
+	err = mv88e6xxx_phy_read(chip, addr, reg_c45, &val);
+	if (err)
+		return err;
+
+	if (on)
+		new_val = val & ~(MV88E6390_SGMII_CONTROL_RESET |
+				  MV88E6390_SGMII_CONTROL_LOOPBACK |
+				  MV88E6390_SGMII_CONTROL_PDOWN);
+	else
+		new_val = val | MV88E6390_SGMII_CONTROL_PDOWN;
+
+	if (val != new_val)
+		err = mv88e6xxx_phy_write(chip, addr, reg_c45, new_val);
+
+	return err;
+}
+
+static int mv88e6390_serdes_lower(struct mv88e6xxx_chip *chip, u8 cmode,
+				  int port_donor, int lane, bool rxaui, bool on)
+{
+	int err;
+	u8 cmode_donor;
+
+	err = mv88e6xxx_port_get_cmode(chip, port_donor, &cmode_donor);
+	if (err)
+		return err;
+
+	switch (cmode_donor) {
+	case MV88E6XXX_PORT_STS_CMODE_RXAUI:
+		if (!rxaui)
+			break;
+		/* Fall through */
+	case MV88E6XXX_PORT_STS_CMODE_1000BASE_X:
+	case MV88E6XXX_PORT_STS_CMODE_SGMII:
+	case MV88E6XXX_PORT_STS_CMODE_2500BASEX:
+		if (cmode == MV88E6XXX_PORT_STS_CMODE_1000BASE_X ||
+		    cmode == MV88E6XXX_PORT_STS_CMODE_SGMII)
+			return	mv88e6390_serdes_sgmii(chip, lane, on);
+	}
+	return 0;
+}
+
+static int mv88e6390_serdes_port9(struct mv88e6xxx_chip *chip, u8 cmode,
+				  bool on)
+{
+	switch (cmode) {
+	case MV88E6XXX_PORT_STS_CMODE_1000BASE_X:
+	case MV88E6XXX_PORT_STS_CMODE_SGMII:
+		return mv88e6390_serdes_sgmii(chip, MV88E6390_PORT9_LANE0, on);
+	case MV88E6XXX_PORT_STS_CMODE_XAUI:
+	case MV88E6XXX_PORT_STS_CMODE_RXAUI:
+	case MV88E6XXX_PORT_STS_CMODE_2500BASEX:
+		return mv88e6390_serdes_10g(chip, MV88E6390_PORT9_LANE0, on);
+	}
+
+	return 0;
+}
+
+static int mv88e6390_serdes_port10(struct mv88e6xxx_chip *chip, u8 cmode,
+				   bool on)
+{
+	switch (cmode) {
+	case MV88E6XXX_PORT_STS_CMODE_SGMII:
+		return mv88e6390_serdes_sgmii(chip, MV88E6390_PORT10_LANE0, on);
+	case MV88E6XXX_PORT_STS_CMODE_XAUI:
+	case MV88E6XXX_PORT_STS_CMODE_RXAUI:
+	case MV88E6XXX_PORT_STS_CMODE_1000BASE_X:
+	case MV88E6XXX_PORT_STS_CMODE_2500BASEX:
+		return mv88e6390_serdes_10g(chip, MV88E6390_PORT10_LANE0, on);
+	}
+
+	return 0;
+}
+
+int mv88e6390_serdes_power(struct mv88e6xxx_chip *chip, int port, bool on)
+{
+	u8 cmode;
+	int err;
+
+	err = mv88e6xxx_port_get_cmode(chip, port, &cmode);
+	if (err)
+		return err;
+
+	switch (port) {
+	case 2:
+		return mv88e6390_serdes_lower(chip, cmode, 9,
+					      MV88E6390_PORT9_LANE1,
+					      false, on);
+	case 3:
+		return mv88e6390_serdes_lower(chip, cmode, 9,
+					      MV88E6390_PORT9_LANE2,
+					      true, on);
+	case 4:
+		return mv88e6390_serdes_lower(chip, cmode, 9,
+					      MV88E6390_PORT9_LANE3,
+					      true, on);
+	case 5:
+		return mv88e6390_serdes_lower(chip, cmode, 10,
+					      MV88E6390_PORT10_LANE1,
+					      false, on);
+	case 6:
+		return mv88e6390_serdes_lower(chip, cmode, 10,
+					      MV88E6390_PORT10_LANE2,
+					      true, on);
+	case 7:
+		return mv88e6390_serdes_lower(chip, cmode, 10,
+					      MV88E6390_PORT10_LANE3,
+					      true, on);
+	case 9:
+		return mv88e6390_serdes_port9(chip, cmode, on);
+	case 10:
+		return mv88e6390_serdes_port10(chip, cmode, on);
+	}
+
+	return 0;
+}
diff --git a/drivers/net/dsa/mv88e6xxx/serdes.h b/drivers/net/dsa/mv88e6xxx/serdes.h
new file mode 100644
index 000000000000..5c1cd6d8e9a5
--- /dev/null
+++ b/drivers/net/dsa/mv88e6xxx/serdes.h
@@ -0,0 +1,48 @@
+/*
+ * Marvell 88E6xxx SERDES manipulation, via SMI bus
+ *
+ * Copyright (c) 2008 Marvell Semiconductor
+ *
+ * Copyright (c) 2016 Andrew Lunn <andrew@lunn.ch>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef _MV88E6XXX_SERDES_H
+#define _MV88E6XXX_SERDES_H
+
+#include "chip.h"
+
+#define MV88E6352_ADDR_SERDES		0x0f
+#define MV88E6352_SERDES_PAGE_FIBER	0x01
+
+#define MV88E6390_PORT9_LANE0		0x09
+#define MV88E6390_PORT9_LANE1		0x12
+#define MV88E6390_PORT9_LANE2		0x13
+#define MV88E6390_PORT9_LANE3		0x14
+#define MV88E6390_PORT10_LANE0		0x0a
+#define MV88E6390_PORT10_LANE1		0x15
+#define MV88E6390_PORT10_LANE2		0x16
+#define MV88E6390_PORT10_LANE3		0x17
+#define MV88E6390_SERDES_DEVICE		(4 << 16)
+
+/* 10GBASE-R and 10GBASE-X4/X2 */
+#define MV88E6390_PCS_CONTROL_1		0x1000
+#define MV88E6390_PCS_CONTROL_1_RESET		BIT(15)
+#define MV88E6390_PCS_CONTROL_1_LOOPBACK	BIT(14)
+#define MV88E6390_PCS_CONTROL_1_SPEED		BIT(13)
+#define MV88E6390_PCS_CONTROL_1_PDOWN		BIT(11)
+
+/* 1000BASE-X and SGMII */
+#define MV88E6390_SGMII_CONTROL		0x2000
+#define MV88E6390_SGMII_CONTROL_RESET		BIT(15)
+#define MV88E6390_SGMII_CONTROL_LOOPBACK	BIT(14)
+#define MV88E6390_SGMII_CONTROL_PDOWN		BIT(11)
+
+int mv88e6352_serdes_power(struct mv88e6xxx_chip *chip, int port, bool on);
+int mv88e6390_serdes_power(struct mv88e6xxx_chip *chip, int port, bool on);
+
+#endif
diff --git a/drivers/net/dsa/qca8k.c b/drivers/net/dsa/qca8k.c
index a4fd4ccf7b67..b3bee7eab45f 100644
--- a/drivers/net/dsa/qca8k.c
+++ b/drivers/net/dsa/qca8k.c
@@ -18,7 +18,6 @@
 #include <linux/phy.h>
 #include <linux/netdevice.h>
 #include <net/dsa.h>
-#include <net/switchdev.h>
 #include <linux/of_net.h>
 #include <linux/of_platform.h>
 #include <linux/if_bridge.h>
@@ -507,7 +506,7 @@ qca8k_setup(struct dsa_switch *ds)
 		pr_warn("regmap initialization failed");
 
 	/* Initialize CPU port pad mode (xMII type, delays...) */
-	phy_mode = of_get_phy_mode(ds->ports[ds->dst->cpu_port].dn);
+	phy_mode = of_get_phy_mode(ds->dst->cpu_dp->dn);
 	if (phy_mode < 0) {
 		pr_err("Can't find phy-mode for master device\n");
 		return phy_mode;
@@ -873,7 +872,7 @@ qca8k_port_fdb_del(struct dsa_switch *ds, int port,
 static int
 qca8k_port_fdb_dump(struct dsa_switch *ds, int port,
 		    struct switchdev_obj_port_fdb *fdb,
-		    int (*cb)(struct switchdev_obj *obj))
+		    switchdev_obj_dump_cb_t *cb)
 {
 	struct qca8k_priv *priv = (struct qca8k_priv *)ds->priv;
 	struct qca8k_fdb _fdb = { 0 };
@@ -959,7 +958,7 @@ qca8k_sw_probe(struct mdio_device *mdiodev)
 	mutex_init(&priv->reg_mutex);
 	dev_set_drvdata(&mdiodev->dev, priv);
 
-	return dsa_register_switch(priv->ds, &mdiodev->dev);
+	return dsa_register_switch(priv->ds);
 }
 
 static void
diff --git a/drivers/net/dummy.c b/drivers/net/dummy.c
index 149244aac20a..d0c165d2086e 100644
--- a/drivers/net/dummy.c
+++ b/drivers/net/dummy.c
@@ -328,7 +328,6 @@ static void dummy_free_netdev(struct net_device *dev)
 	struct dummy_priv *priv = netdev_priv(dev);
 
 	kfree(priv->vfinfo);
-	free_netdev(dev);
 }
 
 static void dummy_setup(struct net_device *dev)
@@ -338,7 +337,8 @@ static void dummy_setup(struct net_device *dev)
 	/* Initialize the device structure. */
 	dev->netdev_ops = &dummy_netdev_ops;
 	dev->ethtool_ops = &dummy_ethtool_ops;
-	dev->destructor = dummy_free_netdev;
+	dev->needs_free_netdev = true;
+	dev->priv_destructor = dummy_free_netdev;
 
 	/* Fill in device structure with ethernet-generic values. */
 	dev->flags |= IFF_NOARP;
@@ -356,7 +356,8 @@ static void dummy_setup(struct net_device *dev)
 	dev->max_mtu = ETH_MAX_MTU;
 }
 
-static int dummy_validate(struct nlattr *tb[], struct nlattr *data[])
+static int dummy_validate(struct nlattr *tb[], struct nlattr *data[],
+			  struct netlink_ext_ack *extack)
 {
 	if (tb[IFLA_ADDRESS]) {
 		if (nla_len(tb[IFLA_ADDRESS]) != ETH_ALEN)
diff --git a/drivers/net/ethernet/3com/3c509.c b/drivers/net/ethernet/3com/3c509.c
index db8592d412ab..f66c9710cb81 100644
--- a/drivers/net/ethernet/3com/3c509.c
+++ b/drivers/net/ethernet/3com/3c509.c
@@ -1039,7 +1039,7 @@ el3_link_ok(struct net_device *dev)
 	return tmp & (1<<11);
 }
 
-static int
+static void
 el3_netdev_get_ecmd(struct net_device *dev, struct ethtool_link_ksettings *cmd)
 {
 	u16 tmp;
@@ -1082,7 +1082,6 @@ el3_netdev_get_ecmd(struct net_device *dev, struct ethtool_link_ksettings *cmd)
 						supported);
 	cmd->base.speed = SPEED_10;
 	EL3WINDOW(1);
-	return 0;
 }
 
 static int
@@ -1151,12 +1150,11 @@ static int el3_get_link_ksettings(struct net_device *dev,
 				  struct ethtool_link_ksettings *cmd)
 {
 	struct el3_private *lp = netdev_priv(dev);
-	int ret;
 
 	spin_lock_irq(&lp->lock);
-	ret = el3_netdev_get_ecmd(dev, cmd);
+	el3_netdev_get_ecmd(dev, cmd);
 	spin_unlock_irq(&lp->lock);
-	return ret;
+	return 0;
 }
 
 static int el3_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/3com/3c515.c b/drivers/net/ethernet/3com/3c515.c
index e7b1fa56b290..c5987f518cb2 100644
--- a/drivers/net/ethernet/3com/3c515.c
+++ b/drivers/net/ethernet/3com/3c515.c
@@ -1370,9 +1370,9 @@ static int boomerang_rx(struct net_device *dev)
 			    (skb = netdev_alloc_skb(dev, pkt_len + 4)) != NULL) {
 				skb_reserve(skb, 2);	/* Align IP on 16 byte boundaries */
 				/* 'skb_put()' points to the start of sk_buff data area. */
-				memcpy(skb_put(skb, pkt_len),
-				       isa_bus_to_virt(vp->rx_ring[entry].
-						   addr), pkt_len);
+				skb_put_data(skb,
+					     isa_bus_to_virt(vp->rx_ring[entry].addr),
+					     pkt_len);
 				rx_copy++;
 			} else {
 				void *temp;
diff --git a/drivers/net/ethernet/3com/3c59x.c b/drivers/net/ethernet/3com/3c59x.c
index e41245a54f8b..3b516ebeeddb 100644
--- a/drivers/net/ethernet/3com/3c59x.c
+++ b/drivers/net/ethernet/3com/3c59x.c
@@ -2628,9 +2628,8 @@ boomerang_rx(struct net_device *dev)
 				skb_reserve(skb, 2);	/* Align IP on 16 byte boundaries */
 				pci_dma_sync_single_for_cpu(VORTEX_PCI(vp), dma, PKT_BUF_SZ, PCI_DMA_FROMDEVICE);
 				/* 'skb_put()' points to the start of sk_buff data area. */
-				memcpy(skb_put(skb, pkt_len),
-					   vp->rx_skbuff[entry]->data,
-					   pkt_len);
+				skb_put_data(skb, vp->rx_skbuff[entry]->data,
+					     pkt_len);
 				pci_dma_sync_single_for_device(VORTEX_PCI(vp), dma, PKT_BUF_SZ, PCI_DMA_FROMDEVICE);
 				vp->rx_copy++;
 			} else {
@@ -2912,7 +2911,9 @@ static int vortex_get_link_ksettings(struct net_device *dev,
 {
 	struct vortex_private *vp = netdev_priv(dev);
 
-	return mii_ethtool_get_link_ksettings(&vp->mii, cmd);
+	mii_ethtool_get_link_ksettings(&vp->mii, cmd);
+
+	return 0;
 }
 
 static int vortex_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/8390/ax88796.c b/drivers/net/ethernet/8390/ax88796.c
index db02bc2fb4b2..05d9d3e2e92e 100644
--- a/drivers/net/ethernet/8390/ax88796.c
+++ b/drivers/net/ethernet/8390/ax88796.c
@@ -723,6 +723,12 @@ static int ax_init_dev(struct net_device *dev)
 	    ax->plat->mac_addr)
 		memcpy(dev->dev_addr, ax->plat->mac_addr, ETH_ALEN);
 
+	if (!is_valid_ether_addr(dev->dev_addr)) {
+		eth_hw_addr_random(dev);
+		dev_info(&dev->dev, "Using random MAC address: %pM\n",
+			 dev->dev_addr);
+	}
+
 	ax_reset_8390(dev);
 
 	ei_local->name = "AX88796";
diff --git a/drivers/net/ethernet/aeroflex/greth.c b/drivers/net/ethernet/aeroflex/greth.c
index d8e133ced7b8..4309be3724ad 100644
--- a/drivers/net/ethernet/aeroflex/greth.c
+++ b/drivers/net/ethernet/aeroflex/greth.c
@@ -807,7 +807,8 @@ static int greth_rx(struct net_device *dev, int limit)
 				if (netif_msg_pktdata(greth))
 					greth_print_rx_packet(phys_to_virt(dma_addr), pkt_len);
 
-				memcpy(skb_put(skb, pkt_len), phys_to_virt(dma_addr), pkt_len);
+				skb_put_data(skb, phys_to_virt(dma_addr),
+					     pkt_len);
 
 				skb->protocol = eth_type_trans(skb, dev);
 				dev->stats.rx_bytes += pkt_len;
diff --git a/drivers/net/ethernet/agere/et131x.c b/drivers/net/ethernet/agere/et131x.c
index 87a11b9f0ea5..54eff90e2f02 100644
--- a/drivers/net/ethernet/agere/et131x.c
+++ b/drivers/net/ethernet/agere/et131x.c
@@ -2282,7 +2282,7 @@ static struct rfd *nic_rx_pkts(struct et131x_adapter *adapter)
 
 	adapter->netdev->stats.rx_bytes += rfd->len;
 
-	memcpy(skb_put(skb, rfd->len), fbr->virt[buff_index], rfd->len);
+	skb_put_data(skb, fbr->virt[buff_index], rfd->len);
 
 	skb->protocol = eth_type_trans(skb, adapter->netdev);
 	skb->ip_summed = CHECKSUM_NONE;
diff --git a/drivers/net/ethernet/allwinner/sun4i-emac.c b/drivers/net/ethernet/allwinner/sun4i-emac.c
index c8f4d26fc9d4..3143de45baaa 100644
--- a/drivers/net/ethernet/allwinner/sun4i-emac.c
+++ b/drivers/net/ethernet/allwinner/sun4i-emac.c
@@ -633,7 +633,7 @@ static void emac_rx(struct net_device *dev)
 			if (!skb)
 				continue;
 			skb_reserve(skb, 2);
-			rdptr = (u8 *) skb_put(skb, rxlen - 4);
+			rdptr = skb_put(skb, rxlen - 4);
 
 			/* Read received packet from RX SRAM */
 			if (netif_msg_rx_status(db))
diff --git a/drivers/net/ethernet/amazon/ena/ena_admin_defs.h b/drivers/net/ethernet/amazon/ena/ena_admin_defs.h
index 5b6509d59716..305dc1996b4e 100644
--- a/drivers/net/ethernet/amazon/ena/ena_admin_defs.h
+++ b/drivers/net/ethernet/amazon/ena/ena_admin_defs.h
@@ -70,6 +70,8 @@ enum ena_admin_aq_feature_id {
 
 	ENA_ADMIN_MAX_QUEUES_NUM		= 2,
 
+	ENA_ADMIN_HW_HINTS			= 3,
+
 	ENA_ADMIN_RSS_HASH_FUNCTION		= 10,
 
 	ENA_ADMIN_STATELESS_OFFLOAD_CONFIG	= 11,
@@ -749,6 +751,31 @@ struct ena_admin_feature_rss_ind_table {
 	struct ena_admin_rss_ind_table_entry inline_entry;
 };
 
+/* When hint value is 0, driver should use it's own predefined value */
+struct ena_admin_ena_hw_hints {
+	/* value in ms */
+	u16 mmio_read_timeout;
+
+	/* value in ms */
+	u16 driver_watchdog_timeout;
+
+	/* Per packet tx completion timeout. value in ms */
+	u16 missing_tx_completion_timeout;
+
+	u16 missed_tx_completion_count_threshold_to_reset;
+
+	/* value in ms */
+	u16 admin_completion_tx_timeout;
+
+	u16 netdev_wd_timeout;
+
+	u16 max_tx_sgl_size;
+
+	u16 max_rx_sgl_size;
+
+	u16 reserved[8];
+};
+
 struct ena_admin_get_feat_cmd {
 	struct ena_admin_aq_common_desc aq_common_descriptor;
 
@@ -782,6 +809,8 @@ struct ena_admin_get_feat_resp {
 		struct ena_admin_feature_rss_ind_table ind_table;
 
 		struct ena_admin_feature_intr_moder_desc intr_moderation;
+
+		struct ena_admin_ena_hw_hints hw_hints;
 	} u;
 };
 
@@ -857,6 +886,8 @@ enum ena_admin_aenq_notification_syndrom {
 	ENA_ADMIN_SUSPEND	= 0,
 
 	ENA_ADMIN_RESUME	= 1,
+
+	ENA_ADMIN_UPDATE_HINTS	= 2,
 };
 
 struct ena_admin_aenq_entry {
diff --git a/drivers/net/ethernet/amazon/ena/ena_com.c b/drivers/net/ethernet/amazon/ena/ena_com.c
index 08d11cede9c9..52beba8c7a39 100644
--- a/drivers/net/ethernet/amazon/ena/ena_com.c
+++ b/drivers/net/ethernet/amazon/ena/ena_com.c
@@ -61,6 +61,8 @@
 
 #define ENA_MMIO_READ_TIMEOUT 0xFFFFFFFF
 
+#define ENA_REGS_ADMIN_INTR_MASK 1
+
 /*****************************************************************************/
 /*****************************************************************************/
 /*****************************************************************************/
@@ -97,8 +99,8 @@ static inline int ena_com_mem_addr_set(struct ena_com_dev *ena_dev,
 		return -EINVAL;
 	}
 
-	ena_addr->mem_addr_low = (u32)addr;
-	ena_addr->mem_addr_high = (u64)addr >> 32;
+	ena_addr->mem_addr_low = lower_32_bits(addr);
+	ena_addr->mem_addr_high = (u16)upper_32_bits(addr);
 
 	return 0;
 }
@@ -232,11 +234,9 @@ static struct ena_comp_ctx *__ena_com_submit_admin_cmd(struct ena_com_admin_queu
 	tail_masked = admin_queue->sq.tail & queue_size_mask;
 
 	/* In case of queue FULL */
-	cnt = admin_queue->sq.tail - admin_queue->sq.head;
+	cnt = atomic_read(&admin_queue->outstanding_cmds);
 	if (cnt >= admin_queue->q_depth) {
-		pr_debug("admin queue is FULL (tail %d head %d depth: %d)\n",
-			 admin_queue->sq.tail, admin_queue->sq.head,
-			 admin_queue->q_depth);
+		pr_debug("admin queue is full.\n");
 		admin_queue->stats.out_of_space++;
 		return ERR_PTR(-ENOSPC);
 	}
@@ -329,7 +329,7 @@ static int ena_com_init_io_sq(struct ena_com_dev *ena_dev,
 	size_t size;
 	int dev_node = 0;
 
-	memset(&io_sq->desc_addr, 0x0, sizeof(struct ena_com_io_desc_addr));
+	memset(&io_sq->desc_addr, 0x0, sizeof(io_sq->desc_addr));
 
 	io_sq->desc_entry_size =
 		(io_sq->direction == ENA_COM_IO_QUEUE_DIRECTION_TX) ?
@@ -383,7 +383,7 @@ static int ena_com_init_io_cq(struct ena_com_dev *ena_dev,
 	size_t size;
 	int prev_node = 0;
 
-	memset(&io_cq->cdesc_addr, 0x0, sizeof(struct ena_com_io_desc_addr));
+	memset(&io_cq->cdesc_addr, 0x0, sizeof(io_cq->cdesc_addr));
 
 	/* Use the basic completion descriptor for Rx */
 	io_cq->cdesc_entry_size_in_bytes =
@@ -494,7 +494,7 @@ static int ena_com_comp_status_to_errno(u8 comp_status)
 	case ENA_ADMIN_RESOURCE_ALLOCATION_FAILURE:
 		return -ENOMEM;
 	case ENA_ADMIN_UNSUPPORTED_OPCODE:
-		return -EPERM;
+		return -EOPNOTSUPP;
 	case ENA_ADMIN_BAD_OPCODE:
 	case ENA_ADMIN_MALFORMED_REQUEST:
 	case ENA_ADMIN_ILLEGAL_PARAMETER:
@@ -508,15 +508,20 @@ static int ena_com_comp_status_to_errno(u8 comp_status)
 static int ena_com_wait_and_process_admin_cq_polling(struct ena_comp_ctx *comp_ctx,
 						     struct ena_com_admin_queue *admin_queue)
 {
-	unsigned long flags;
-	u32 start_time;
+	unsigned long flags, timeout;
 	int ret;
 
-	start_time = ((u32)jiffies_to_usecs(jiffies));
+	timeout = jiffies + usecs_to_jiffies(admin_queue->completion_timeout);
+
+	while (1) {
+		spin_lock_irqsave(&admin_queue->q_lock, flags);
+		ena_com_handle_admin_completion(admin_queue);
+		spin_unlock_irqrestore(&admin_queue->q_lock, flags);
+
+		if (comp_ctx->status != ENA_CMD_SUBMITTED)
+			break;
 
-	while (comp_ctx->status == ENA_CMD_SUBMITTED) {
-		if ((((u32)jiffies_to_usecs(jiffies)) - start_time) >
-		    ADMIN_CMD_TIMEOUT_US) {
+		if (time_is_before_jiffies(timeout)) {
 			pr_err("Wait for completion (polling) timeout\n");
 			/* ENA didn't have any completion */
 			spin_lock_irqsave(&admin_queue->q_lock, flags);
@@ -528,10 +533,6 @@ static int ena_com_wait_and_process_admin_cq_polling(struct ena_comp_ctx *comp_c
 			goto err;
 		}
 
-		spin_lock_irqsave(&admin_queue->q_lock, flags);
-		ena_com_handle_admin_completion(admin_queue);
-		spin_unlock_irqrestore(&admin_queue->q_lock, flags);
-
 		msleep(100);
 	}
 
@@ -560,7 +561,8 @@ static int ena_com_wait_and_process_admin_cq_interrupts(struct ena_comp_ctx *com
 	int ret;
 
 	wait_for_completion_timeout(&comp_ctx->wait_event,
-				    usecs_to_jiffies(ADMIN_CMD_TIMEOUT_US));
+				    usecs_to_jiffies(
+					    admin_queue->completion_timeout));
 
 	/* In case the command wasn't completed find out the root cause.
 	 * There might be 2 kinds of errors
@@ -600,12 +602,15 @@ static u32 ena_com_reg_bar_read32(struct ena_com_dev *ena_dev, u16 offset)
 	struct ena_com_mmio_read *mmio_read = &ena_dev->mmio_read;
 	volatile struct ena_admin_ena_mmio_req_read_less_resp *read_resp =
 		mmio_read->read_resp;
-	u32 mmio_read_reg, ret;
+	u32 mmio_read_reg, ret, i;
 	unsigned long flags;
-	int i;
+	u32 timeout = mmio_read->reg_read_to;
 
 	might_sleep();
 
+	if (timeout == 0)
+		timeout = ENA_REG_READ_TIMEOUT;
+
 	/* If readless is disabled, perform regular read */
 	if (!mmio_read->readless_supported)
 		return readl(ena_dev->reg_bar + offset);
@@ -626,14 +631,14 @@ static u32 ena_com_reg_bar_read32(struct ena_com_dev *ena_dev, u16 offset)
 
 	writel(mmio_read_reg, ena_dev->reg_bar + ENA_REGS_MMIO_REG_READ_OFF);
 
-	for (i = 0; i < ENA_REG_READ_TIMEOUT; i++) {
+	for (i = 0; i < timeout; i++) {
 		if (read_resp->req_id == mmio_read->seq_num)
 			break;
 
 		udelay(1);
 	}
 
-	if (unlikely(i == ENA_REG_READ_TIMEOUT)) {
+	if (unlikely(i == timeout)) {
 		pr_err("reading reg failed for timeout. expected: req id[%hu] offset[%hu] actual: req id[%hu] offset[%hu]\n",
 		       mmio_read->seq_num, offset, read_resp->req_id,
 		       read_resp->reg_off);
@@ -680,7 +685,7 @@ static int ena_com_destroy_io_sq(struct ena_com_dev *ena_dev,
 	u8 direction;
 	int ret;
 
-	memset(&destroy_cmd, 0x0, sizeof(struct ena_admin_aq_destroy_sq_cmd));
+	memset(&destroy_cmd, 0x0, sizeof(destroy_cmd));
 
 	if (io_sq->direction == ENA_COM_IO_QUEUE_DIRECTION_TX)
 		direction = ENA_ADMIN_SQ_DIRECTION_TX;
@@ -785,7 +790,7 @@ static int ena_com_get_feature_ex(struct ena_com_dev *ena_dev,
 
 	if (!ena_com_check_supported_feature_id(ena_dev, feature_id)) {
 		pr_debug("Feature %d isn't supported\n", feature_id);
-		return -EPERM;
+		return -EOPNOTSUPP;
 	}
 
 	memset(&get_cmd, 0x0, sizeof(get_cmd));
@@ -962,7 +967,7 @@ static int ena_com_create_io_sq(struct ena_com_dev *ena_dev,
 	u8 direction;
 	int ret;
 
-	memset(&create_cmd, 0x0, sizeof(struct ena_admin_aq_create_sq_cmd));
+	memset(&create_cmd, 0x0, sizeof(create_cmd));
 
 	create_cmd.aq_common_descriptor.opcode = ENA_ADMIN_CREATE_SQ;
 
@@ -1154,7 +1159,7 @@ int ena_com_create_io_cq(struct ena_com_dev *ena_dev,
 	struct ena_admin_acq_create_cq_resp_desc cmd_completion;
 	int ret;
 
-	memset(&create_cmd, 0x0, sizeof(struct ena_admin_aq_create_cq_cmd));
+	memset(&create_cmd, 0x0, sizeof(create_cmd));
 
 	create_cmd.aq_common_descriptor.opcode = ENA_ADMIN_CREATE_CQ;
 
@@ -1262,7 +1267,7 @@ int ena_com_destroy_io_cq(struct ena_com_dev *ena_dev,
 	struct ena_admin_acq_destroy_cq_resp_desc destroy_resp;
 	int ret;
 
-	memset(&destroy_cmd, 0x0, sizeof(struct ena_admin_aq_destroy_sq_cmd));
+	memset(&destroy_cmd, 0x0, sizeof(destroy_cmd));
 
 	destroy_cmd.cq_idx = io_cq->idx;
 	destroy_cmd.aq_common_descriptor.opcode = ENA_ADMIN_DESTROY_CQ;
@@ -1323,7 +1328,7 @@ int ena_com_set_aenq_config(struct ena_com_dev *ena_dev, u32 groups_flag)
 	if ((get_resp.u.aenq.supported_groups & groups_flag) != groups_flag) {
 		pr_warn("Trying to set unsupported aenq events. supported flag: %x asked flag: %x\n",
 			get_resp.u.aenq.supported_groups, groups_flag);
-		return -EPERM;
+		return -EOPNOTSUPP;
 	}
 
 	memset(&cmd, 0x0, sizeof(cmd));
@@ -1455,6 +1460,12 @@ void ena_com_admin_destroy(struct ena_com_dev *ena_dev)
 
 void ena_com_set_admin_polling_mode(struct ena_com_dev *ena_dev, bool polling)
 {
+	u32 mask_value = 0;
+
+	if (polling)
+		mask_value = ENA_REGS_ADMIN_INTR_MASK;
+
+	writel(mask_value, ena_dev->reg_bar + ENA_REGS_INTR_MASK_OFF);
 	ena_dev->admin_queue.polling = polling;
 }
 
@@ -1612,8 +1623,8 @@ int ena_com_create_io_queue(struct ena_com_dev *ena_dev,
 	io_sq = &ena_dev->io_sq_queues[ctx->qid];
 	io_cq = &ena_dev->io_cq_queues[ctx->qid];
 
-	memset(io_sq, 0x0, sizeof(struct ena_com_io_sq));
-	memset(io_cq, 0x0, sizeof(struct ena_com_io_cq));
+	memset(io_sq, 0x0, sizeof(*io_sq));
+	memset(io_cq, 0x0, sizeof(*io_cq));
 
 	/* Init CQ */
 	io_cq->q_depth = ctx->queue_size;
@@ -1723,6 +1734,20 @@ int ena_com_get_dev_attr_feat(struct ena_com_dev *ena_dev,
 	memcpy(&get_feat_ctx->offload, &get_resp.u.offload,
 	       sizeof(get_resp.u.offload));
 
+	/* Driver hints isn't mandatory admin command. So in case the
+	 * command isn't supported set driver hints to 0
+	 */
+	rc = ena_com_get_feature(ena_dev, &get_resp, ENA_ADMIN_HW_HINTS);
+
+	if (!rc)
+		memcpy(&get_feat_ctx->hw_hints, &get_resp.u.hw_hints,
+		       sizeof(get_resp.u.hw_hints));
+	else if (rc == -EOPNOTSUPP)
+		memset(&get_feat_ctx->hw_hints, 0x0,
+		       sizeof(get_feat_ctx->hw_hints));
+	else
+		return rc;
+
 	return 0;
 }
 
@@ -1800,7 +1825,8 @@ void ena_com_aenq_intr_handler(struct ena_com_dev *dev, void *data)
 	writel((u32)aenq->head, dev->reg_bar + ENA_REGS_AENQ_HEAD_DB_OFF);
 }
 
-int ena_com_dev_reset(struct ena_com_dev *ena_dev)
+int ena_com_dev_reset(struct ena_com_dev *ena_dev,
+		      enum ena_regs_reset_reason_types reset_reason)
 {
 	u32 stat, timeout, cap, reset_val;
 	int rc;
@@ -1828,6 +1854,8 @@ int ena_com_dev_reset(struct ena_com_dev *ena_dev)
 
 	/* start reset */
 	reset_val = ENA_REGS_DEV_CTL_DEV_RESET_MASK;
+	reset_val |= (reset_reason << ENA_REGS_DEV_CTL_RESET_REASON_SHIFT) &
+		     ENA_REGS_DEV_CTL_RESET_REASON_MASK;
 	writel(reset_val, ena_dev->reg_bar + ENA_REGS_DEV_CTL_OFF);
 
 	/* Write again the MMIO read request address */
@@ -1848,6 +1876,14 @@ int ena_com_dev_reset(struct ena_com_dev *ena_dev)
 		return rc;
 	}
 
+	timeout = (cap & ENA_REGS_CAPS_ADMIN_CMD_TO_MASK) >>
+		ENA_REGS_CAPS_ADMIN_CMD_TO_SHIFT;
+	if (timeout)
+		/* the resolution of timeout reg is 100ms */
+		ena_dev->admin_queue.completion_timeout = timeout * 100000;
+	else
+		ena_dev->admin_queue.completion_timeout = ADMIN_CMD_TIMEOUT_US;
+
 	return 0;
 }
 
@@ -1902,7 +1938,7 @@ int ena_com_set_dev_mtu(struct ena_com_dev *ena_dev, int mtu)
 
 	if (!ena_com_check_supported_feature_id(ena_dev, ENA_ADMIN_MTU)) {
 		pr_debug("Feature %d isn't supported\n", ENA_ADMIN_MTU);
-		return -EPERM;
+		return -EOPNOTSUPP;
 	}
 
 	memset(&cmd, 0x0, sizeof(cmd));
@@ -1956,7 +1992,7 @@ int ena_com_set_hash_function(struct ena_com_dev *ena_dev)
 						ENA_ADMIN_RSS_HASH_FUNCTION)) {
 		pr_debug("Feature %d isn't supported\n",
 			 ENA_ADMIN_RSS_HASH_FUNCTION);
-		return -EPERM;
+		return -EOPNOTSUPP;
 	}
 
 	/* Validate hash function is supported */
@@ -1968,7 +2004,7 @@ int ena_com_set_hash_function(struct ena_com_dev *ena_dev)
 	if (get_resp.u.flow_hash_func.supported_func & (1 << rss->hash_func)) {
 		pr_err("Func hash %d isn't supported by device, abort\n",
 		       rss->hash_func);
-		return -EPERM;
+		return -EOPNOTSUPP;
 	}
 
 	memset(&cmd, 0x0, sizeof(cmd));
@@ -2027,7 +2063,7 @@ int ena_com_fill_hash_function(struct ena_com_dev *ena_dev,
 
 	if (!((1 << func) & get_resp.u.flow_hash_func.supported_func)) {
 		pr_err("Flow hash function %d isn't supported\n", func);
-		return -EPERM;
+		return -EOPNOTSUPP;
 	}
 
 	switch (func) {
@@ -2120,7 +2156,7 @@ int ena_com_set_hash_ctrl(struct ena_com_dev *ena_dev)
 						ENA_ADMIN_RSS_HASH_INPUT)) {
 		pr_debug("Feature %d isn't supported\n",
 			 ENA_ADMIN_RSS_HASH_INPUT);
-		return -EPERM;
+		return -EOPNOTSUPP;
 	}
 
 	memset(&cmd, 0x0, sizeof(cmd));
@@ -2201,7 +2237,7 @@ int ena_com_set_default_hash_ctrl(struct ena_com_dev *ena_dev)
 			pr_err("hash control doesn't support all the desire configuration. proto %x supported %x selected %x\n",
 			       i, hash_ctrl->supported_fields[i].fields,
 			       hash_ctrl->selected_fields[i].fields);
-			return -EPERM;
+			return -EOPNOTSUPP;
 		}
 	}
 
@@ -2279,7 +2315,7 @@ int ena_com_indirect_table_set(struct ena_com_dev *ena_dev)
 		    ena_dev, ENA_ADMIN_RSS_REDIRECTION_TABLE_CONFIG)) {
 		pr_debug("Feature %d isn't supported\n",
 			 ENA_ADMIN_RSS_REDIRECTION_TABLE_CONFIG);
-		return -EPERM;
+		return -EOPNOTSUPP;
 	}
 
 	ret = ena_com_ind_tbl_convert_to_device(ena_dev);
@@ -2546,7 +2582,7 @@ int ena_com_init_interrupt_moderation(struct ena_com_dev *ena_dev)
 				 ENA_ADMIN_INTERRUPT_MODERATION);
 
 	if (rc) {
-		if (rc == -EPERM) {
+		if (rc == -EOPNOTSUPP) {
 			pr_debug("Feature %d isn't supported\n",
 				 ENA_ADMIN_INTERRUPT_MODERATION);
 			rc = 0;
diff --git a/drivers/net/ethernet/amazon/ena/ena_com.h b/drivers/net/ethernet/amazon/ena/ena_com.h
index c9b33ee5f258..7b784f8a06a6 100644
--- a/drivers/net/ethernet/amazon/ena/ena_com.h
+++ b/drivers/net/ethernet/amazon/ena/ena_com.h
@@ -97,6 +97,8 @@
 #define ENA_INTR_MODER_LEVEL_STRIDE			2
 #define ENA_INTR_BYTE_COUNT_NOT_SUPPORTED		0xFFFFFF
 
+#define ENA_HW_HINTS_NO_TIMEOUT				0xFFFF
+
 enum ena_intr_moder_level {
 	ENA_INTR_MODER_LOWEST = 0,
 	ENA_INTR_MODER_LOW,
@@ -232,7 +234,9 @@ struct ena_com_stats_admin {
 struct ena_com_admin_queue {
 	void *q_dmadev;
 	spinlock_t q_lock; /* spinlock for the admin queue */
+
 	struct ena_comp_ctx *comp_ctx;
+	u32 completion_timeout;
 	u16 q_depth;
 	struct ena_com_admin_cq cq;
 	struct ena_com_admin_sq sq;
@@ -267,6 +271,7 @@ struct ena_com_aenq {
 struct ena_com_mmio_read {
 	struct ena_admin_ena_mmio_req_read_less_resp *read_resp;
 	dma_addr_t read_resp_dma_addr;
+	u32 reg_read_to; /* in us */
 	u16 seq_num;
 	bool readless_supported;
 	/* spin lock to ensure a single outstanding read */
@@ -336,6 +341,7 @@ struct ena_com_dev_get_features_ctx {
 	struct ena_admin_device_attr_feature_desc dev_attr;
 	struct ena_admin_feature_aenq_desc aenq;
 	struct ena_admin_feature_offload_desc offload;
+	struct ena_admin_ena_hw_hints hw_hints;
 };
 
 struct ena_com_create_io_ctx {
@@ -414,10 +420,12 @@ void ena_com_admin_destroy(struct ena_com_dev *ena_dev);
 
 /* ena_com_dev_reset - Perform device FLR to the device.
  * @ena_dev: ENA communication layer struct
+ * @reset_reason: Specify what is the trigger for the reset in case of an error.
  *
  * @return - 0 on success, negative value on failure.
  */
-int ena_com_dev_reset(struct ena_com_dev *ena_dev);
+int ena_com_dev_reset(struct ena_com_dev *ena_dev,
+		      enum ena_regs_reset_reason_types reset_reason);
 
 /* ena_com_create_io_queue - Create io queue.
  * @ena_dev: ENA communication layer struct
diff --git a/drivers/net/ethernet/amazon/ena/ena_eth_com.c b/drivers/net/ethernet/amazon/ena/ena_eth_com.c
index f999305e1363..b11e573ad57a 100644
--- a/drivers/net/ethernet/amazon/ena/ena_eth_com.c
+++ b/drivers/net/ethernet/amazon/ena/ena_eth_com.c
@@ -493,6 +493,11 @@ int ena_com_tx_comp_req_id_get(struct ena_com_io_cq *io_cq, u16 *req_id)
 	if (cdesc_phase != expected_phase)
 		return -EAGAIN;
 
+	if (unlikely(cdesc->req_id >= io_cq->q_depth)) {
+		pr_err("Invalid req id %d\n", cdesc->req_id);
+		return -EINVAL;
+	}
+
 	ena_com_cq_inc_head(io_cq);
 
 	*req_id = READ_ONCE(cdesc->req_id);
diff --git a/drivers/net/ethernet/amazon/ena/ena_ethtool.c b/drivers/net/ethernet/amazon/ena/ena_ethtool.c
index 67b2338f8fb3..b1212debc2e1 100644
--- a/drivers/net/ethernet/amazon/ena/ena_ethtool.c
+++ b/drivers/net/ethernet/amazon/ena/ena_ethtool.c
@@ -80,7 +80,6 @@ static const struct ena_stats ena_stats_tx_strings[] = {
 	ENA_STAT_TX_ENTRY(tx_poll),
 	ENA_STAT_TX_ENTRY(doorbells),
 	ENA_STAT_TX_ENTRY(prepare_ctx_err),
-	ENA_STAT_TX_ENTRY(missing_tx_comp),
 	ENA_STAT_TX_ENTRY(bad_req_id),
 };
 
@@ -94,6 +93,8 @@ static const struct ena_stats ena_stats_rx_strings[] = {
 	ENA_STAT_RX_ENTRY(dma_mapping_err),
 	ENA_STAT_RX_ENTRY(bad_desc_num),
 	ENA_STAT_RX_ENTRY(rx_copybreak_pkt),
+	ENA_STAT_RX_ENTRY(bad_req_id),
+	ENA_STAT_RX_ENTRY(empty_rx_ring),
 };
 
 static const struct ena_stats ena_stats_ena_com_strings[] = {
@@ -539,12 +540,8 @@ static int ena_get_rss_hash(struct ena_com_dev *ena_dev,
 	}
 
 	rc = ena_com_get_hash_ctrl(ena_dev, proto, &hash_fields);
-	if (rc) {
-		/* If device don't have permission, return unsupported */
-		if (rc == -EPERM)
-			rc = -EOPNOTSUPP;
+	if (rc)
 		return rc;
-	}
 
 	cmd->data = ena_flow_hash_to_flow_type(hash_fields);
 
@@ -612,7 +609,7 @@ static int ena_set_rxnfc(struct net_device *netdev, struct ethtool_rxnfc *info)
 		rc = -EOPNOTSUPP;
 	}
 
-	return (rc == -EPERM) ? -EOPNOTSUPP : rc;
+	return rc;
 }
 
 static int ena_get_rxnfc(struct net_device *netdev, struct ethtool_rxnfc *info,
@@ -638,7 +635,7 @@ static int ena_get_rxnfc(struct net_device *netdev, struct ethtool_rxnfc *info,
 		rc = -EOPNOTSUPP;
 	}
 
-	return (rc == -EPERM) ? -EOPNOTSUPP : rc;
+	return rc;
 }
 
 static u32 ena_get_rxfh_indir_size(struct net_device *netdev)
diff --git a/drivers/net/ethernet/amazon/ena/ena_netdev.c b/drivers/net/ethernet/amazon/ena/ena_netdev.c
index 7c1214d78855..f7dc22f65d9f 100644
--- a/drivers/net/ethernet/amazon/ena/ena_netdev.c
+++ b/drivers/net/ethernet/amazon/ena/ena_netdev.c
@@ -87,6 +87,7 @@ static void ena_tx_timeout(struct net_device *dev)
 	if (test_and_set_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags))
 		return;
 
+	adapter->reset_reason = ENA_REGS_RESET_OS_NETDEV_WD;
 	u64_stats_update_begin(&adapter->syncp);
 	adapter->dev_stats.tx_timeout++;
 	u64_stats_update_end(&adapter->syncp);
@@ -190,6 +191,7 @@ static void ena_init_io_rings(struct ena_adapter *adapter)
 		rxr->sgl_size = adapter->max_rx_sgl_size;
 		rxr->smoothed_interval =
 			ena_com_get_nonadaptive_moderation_interval_rx(ena_dev);
+		rxr->empty_rx_queue = 0;
 	}
 }
 
@@ -302,6 +304,24 @@ static void ena_free_all_io_tx_resources(struct ena_adapter *adapter)
 		ena_free_tx_resources(adapter, i);
 }
 
+static inline int validate_rx_req_id(struct ena_ring *rx_ring, u16 req_id)
+{
+	if (likely(req_id < rx_ring->ring_size))
+		return 0;
+
+	netif_err(rx_ring->adapter, rx_err, rx_ring->netdev,
+		  "Invalid rx req_id: %hu\n", req_id);
+
+	u64_stats_update_begin(&rx_ring->syncp);
+	rx_ring->rx_stats.bad_req_id++;
+	u64_stats_update_end(&rx_ring->syncp);
+
+	/* Trigger device reset */
+	rx_ring->adapter->reset_reason = ENA_REGS_RESET_INV_RX_REQ_ID;
+	set_bit(ENA_FLAG_TRIGGER_RESET, &rx_ring->adapter->flags);
+	return -EFAULT;
+}
+
 /* ena_setup_rx_resources - allocate I/O Rx resources (Descriptors)
  * @adapter: network interface device structure
  * @qid: queue index
@@ -313,7 +333,7 @@ static int ena_setup_rx_resources(struct ena_adapter *adapter,
 {
 	struct ena_ring *rx_ring = &adapter->rx_ring[qid];
 	struct ena_irq *ena_irq = &adapter->irq_tbl[ENA_IO_IRQ_IDX(qid)];
-	int size, node;
+	int size, node, i;
 
 	if (rx_ring->rx_buffer_info) {
 		netif_err(adapter, ifup, adapter->netdev,
@@ -334,6 +354,20 @@ static int ena_setup_rx_resources(struct ena_adapter *adapter,
 			return -ENOMEM;
 	}
 
+	size = sizeof(u16) * rx_ring->ring_size;
+	rx_ring->free_rx_ids = vzalloc_node(size, node);
+	if (!rx_ring->free_rx_ids) {
+		rx_ring->free_rx_ids = vzalloc(size);
+		if (!rx_ring->free_rx_ids) {
+			vfree(rx_ring->rx_buffer_info);
+			return -ENOMEM;
+		}
+	}
+
+	/* Req id ring for receiving RX pkts out of order */
+	for (i = 0; i < rx_ring->ring_size; i++)
+		rx_ring->free_rx_ids[i] = i;
+
 	/* Reset rx statistics */
 	memset(&rx_ring->rx_stats, 0x0, sizeof(rx_ring->rx_stats));
 
@@ -357,6 +391,9 @@ static void ena_free_rx_resources(struct ena_adapter *adapter,
 
 	vfree(rx_ring->rx_buffer_info);
 	rx_ring->rx_buffer_info = NULL;
+
+	vfree(rx_ring->free_rx_ids);
+	rx_ring->free_rx_ids = NULL;
 }
 
 /* ena_setup_all_rx_resources - allocate I/O Rx queues resources for all queues
@@ -462,15 +499,22 @@ static void ena_free_rx_page(struct ena_ring *rx_ring,
 
 static int ena_refill_rx_bufs(struct ena_ring *rx_ring, u32 num)
 {
-	u16 next_to_use;
+	u16 next_to_use, req_id;
 	u32 i;
 	int rc;
 
 	next_to_use = rx_ring->next_to_use;
 
 	for (i = 0; i < num; i++) {
-		struct ena_rx_buffer *rx_info =
-			&rx_ring->rx_buffer_info[next_to_use];
+		struct ena_rx_buffer *rx_info;
+
+		req_id = rx_ring->free_rx_ids[next_to_use];
+		rc = validate_rx_req_id(rx_ring, req_id);
+		if (unlikely(rc < 0))
+			break;
+
+		rx_info = &rx_ring->rx_buffer_info[req_id];
+
 
 		rc = ena_alloc_rx_page(rx_ring, rx_info,
 				       __GFP_COLD | GFP_ATOMIC | __GFP_COMP);
@@ -482,7 +526,7 @@ static int ena_refill_rx_bufs(struct ena_ring *rx_ring, u32 num)
 		}
 		rc = ena_com_add_single_rx_desc(rx_ring->ena_com_io_sq,
 						&rx_info->ena_buf,
-						next_to_use);
+						req_id);
 		if (unlikely(rc)) {
 			netif_warn(rx_ring->adapter, rx_status, rx_ring->netdev,
 				   "failed to add buffer for rx queue %d\n",
@@ -669,6 +713,7 @@ static int validate_tx_req_id(struct ena_ring *tx_ring, u16 req_id)
 	u64_stats_update_end(&tx_ring->syncp);
 
 	/* Trigger device reset */
+	tx_ring->adapter->reset_reason = ENA_REGS_RESET_INV_TX_REQ_ID;
 	set_bit(ENA_FLAG_TRIGGER_RESET, &tx_ring->adapter->flags);
 	return -EFAULT;
 }
@@ -780,19 +825,42 @@ static int ena_clean_tx_irq(struct ena_ring *tx_ring, u32 budget)
 	return tx_pkts;
 }
 
+static struct sk_buff *ena_alloc_skb(struct ena_ring *rx_ring, bool frags)
+{
+	struct sk_buff *skb;
+
+	if (frags)
+		skb = napi_get_frags(rx_ring->napi);
+	else
+		skb = netdev_alloc_skb_ip_align(rx_ring->netdev,
+						rx_ring->rx_copybreak);
+
+	if (unlikely(!skb)) {
+		u64_stats_update_begin(&rx_ring->syncp);
+		rx_ring->rx_stats.skb_alloc_fail++;
+		u64_stats_update_end(&rx_ring->syncp);
+		netif_dbg(rx_ring->adapter, rx_err, rx_ring->netdev,
+			  "Failed to allocate skb. frags: %d\n", frags);
+		return NULL;
+	}
+
+	return skb;
+}
+
 static struct sk_buff *ena_rx_skb(struct ena_ring *rx_ring,
 				  struct ena_com_rx_buf_info *ena_bufs,
 				  u32 descs,
 				  u16 *next_to_clean)
 {
 	struct sk_buff *skb;
-	struct ena_rx_buffer *rx_info =
-		&rx_ring->rx_buffer_info[*next_to_clean];
-	u32 len;
-	u32 buf = 0;
+	struct ena_rx_buffer *rx_info;
+	u16 len, req_id, buf = 0;
 	void *va;
 
-	len = ena_bufs[0].len;
+	len = ena_bufs[buf].len;
+	req_id = ena_bufs[buf].req_id;
+	rx_info = &rx_ring->rx_buffer_info[req_id];
+
 	if (unlikely(!rx_info->page)) {
 		netif_err(rx_ring->adapter, rx_err, rx_ring->netdev,
 			  "Page is NULL\n");
@@ -808,16 +876,9 @@ static struct sk_buff *ena_rx_skb(struct ena_ring *rx_ring,
 	prefetch(va + NET_IP_ALIGN);
 
 	if (len <= rx_ring->rx_copybreak) {
-		skb = netdev_alloc_skb_ip_align(rx_ring->netdev,
-						rx_ring->rx_copybreak);
-		if (unlikely(!skb)) {
-			u64_stats_update_begin(&rx_ring->syncp);
-			rx_ring->rx_stats.skb_alloc_fail++;
-			u64_stats_update_end(&rx_ring->syncp);
-			netif_err(rx_ring->adapter, rx_err, rx_ring->netdev,
-				  "Failed to allocate skb\n");
+		skb = ena_alloc_skb(rx_ring, false);
+		if (unlikely(!skb))
 			return NULL;
-		}
 
 		netif_dbg(rx_ring->adapter, rx_status, rx_ring->netdev,
 			  "rx allocated small packet. len %d. data_len %d\n",
@@ -836,20 +897,15 @@ static struct sk_buff *ena_rx_skb(struct ena_ring *rx_ring,
 
 		skb_put(skb, len);
 		skb->protocol = eth_type_trans(skb, rx_ring->netdev);
+		rx_ring->free_rx_ids[*next_to_clean] = req_id;
 		*next_to_clean = ENA_RX_RING_IDX_ADD(*next_to_clean, descs,
 						     rx_ring->ring_size);
 		return skb;
 	}
 
-	skb = napi_get_frags(rx_ring->napi);
-	if (unlikely(!skb)) {
-		netif_dbg(rx_ring->adapter, rx_status, rx_ring->netdev,
-			  "Failed allocating skb\n");
-		u64_stats_update_begin(&rx_ring->syncp);
-		rx_ring->rx_stats.skb_alloc_fail++;
-		u64_stats_update_end(&rx_ring->syncp);
+	skb = ena_alloc_skb(rx_ring, true);
+	if (unlikely(!skb))
 		return NULL;
-	}
 
 	do {
 		dma_unmap_page(rx_ring->dev,
@@ -864,13 +920,18 @@ static struct sk_buff *ena_rx_skb(struct ena_ring *rx_ring,
 			  skb->len, skb->data_len);
 
 		rx_info->page = NULL;
+
+		rx_ring->free_rx_ids[*next_to_clean] = req_id;
 		*next_to_clean =
 			ENA_RX_RING_IDX_NEXT(*next_to_clean,
 					     rx_ring->ring_size);
 		if (likely(--descs == 0))
 			break;
-		rx_info = &rx_ring->rx_buffer_info[*next_to_clean];
-		len = ena_bufs[++buf].len;
+
+		buf++;
+		len = ena_bufs[buf].len;
+		req_id = ena_bufs[buf].req_id;
+		rx_info = &rx_ring->rx_buffer_info[req_id];
 	} while (1);
 
 	return skb;
@@ -971,6 +1032,7 @@ static int ena_clean_rx_irq(struct ena_ring *rx_ring, struct napi_struct *napi,
 	int rc = 0;
 	int total_len = 0;
 	int rx_copybreak_pkt = 0;
+	int i;
 
 	netif_dbg(rx_ring->adapter, rx_status, rx_ring->netdev,
 		  "%s qid %d\n", __func__, rx_ring->qid);
@@ -1000,9 +1062,13 @@ static int ena_clean_rx_irq(struct ena_ring *rx_ring, struct napi_struct *napi,
 
 		/* exit if we failed to retrieve a buffer */
 		if (unlikely(!skb)) {
-			next_to_clean = ENA_RX_RING_IDX_ADD(next_to_clean,
-							    ena_rx_ctx.descs,
-							    rx_ring->ring_size);
+			for (i = 0; i < ena_rx_ctx.descs; i++) {
+				rx_ring->free_tx_ids[next_to_clean] =
+					rx_ring->ena_bufs[i].req_id;
+				next_to_clean =
+					ENA_RX_RING_IDX_NEXT(next_to_clean,
+							     rx_ring->ring_size);
+			}
 			break;
 		}
 
@@ -1054,6 +1120,7 @@ error:
 	u64_stats_update_end(&rx_ring->syncp);
 
 	/* Too many desc from the device. Trigger reset */
+	adapter->reset_reason = ENA_REGS_RESET_TOO_MANY_RX_DESCS;
 	set_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags);
 
 	return 0;
@@ -1078,6 +1145,26 @@ inline void ena_adjust_intr_moderation(struct ena_ring *rx_ring,
 	rx_ring->per_napi_bytes = 0;
 }
 
+static inline void ena_unmask_interrupt(struct ena_ring *tx_ring,
+					struct ena_ring *rx_ring)
+{
+	struct ena_eth_io_intr_reg intr_reg;
+
+	/* Update intr register: rx intr delay,
+	 * tx intr delay and interrupt unmask
+	 */
+	ena_com_update_intr_reg(&intr_reg,
+				rx_ring->smoothed_interval,
+				tx_ring->smoothed_interval,
+				true);
+
+	/* It is a shared MSI-X.
+	 * Tx and Rx CQ have pointer to it.
+	 * So we use one of them to reach the intr reg
+	 */
+	ena_com_unmask_intr(rx_ring->ena_com_io_cq, &intr_reg);
+}
+
 static inline void ena_update_ring_numa_node(struct ena_ring *tx_ring,
 					     struct ena_ring *rx_ring)
 {
@@ -1108,7 +1195,6 @@ static int ena_io_poll(struct napi_struct *napi, int budget)
 {
 	struct ena_napi *ena_napi = container_of(napi, struct ena_napi, napi);
 	struct ena_ring *tx_ring, *rx_ring;
-	struct ena_eth_io_intr_reg intr_reg;
 
 	u32 tx_work_done;
 	u32 rx_work_done;
@@ -1149,22 +1235,9 @@ static int ena_io_poll(struct napi_struct *napi, int budget)
 			if (ena_com_get_adaptive_moderation_enabled(rx_ring->ena_dev))
 				ena_adjust_intr_moderation(rx_ring, tx_ring);
 
-			/* Update intr register: rx intr delay,
-			 * tx intr delay and interrupt unmask
-			 */
-			ena_com_update_intr_reg(&intr_reg,
-						rx_ring->smoothed_interval,
-						tx_ring->smoothed_interval,
-						true);
-
-			/* It is a shared MSI-X.
-			 * Tx and Rx CQ have pointer to it.
-			 * So we use one of them to reach the intr reg
-			 */
-			ena_com_unmask_intr(rx_ring->ena_com_io_cq, &intr_reg);
+			ena_unmask_interrupt(tx_ring, rx_ring);
 		}
 
-
 		ena_update_ring_numa_node(tx_ring, rx_ring);
 
 		ret = rx_work_done;
@@ -1201,14 +1274,25 @@ static irqreturn_t ena_intr_msix_io(int irq, void *data)
 {
 	struct ena_napi *ena_napi = data;
 
-	napi_schedule(&ena_napi->napi);
+	napi_schedule_irqoff(&ena_napi->napi);
 
 	return IRQ_HANDLED;
 }
 
+/* Reserve a single MSI-X vector for management (admin + aenq).
+ * plus reserve one vector for each potential io queue.
+ * the number of potential io queues is the minimum of what the device
+ * supports and the number of vCPUs.
+ */
 static int ena_enable_msix(struct ena_adapter *adapter, int num_queues)
 {
-	int msix_vecs, rc;
+	int msix_vecs, irq_cnt;
+
+	if (test_bit(ENA_FLAG_MSIX_ENABLED, &adapter->flags)) {
+		netif_err(adapter, probe, adapter->netdev,
+			  "Error, MSI-X is already enabled\n");
+		return -EPERM;
+	}
 
 	/* Reserved the max msix vectors we might need */
 	msix_vecs = ENA_MAX_MSIX_VEC(num_queues);
@@ -1216,25 +1300,28 @@ static int ena_enable_msix(struct ena_adapter *adapter, int num_queues)
 	netif_dbg(adapter, probe, adapter->netdev,
 		  "trying to enable MSI-X, vectors %d\n", msix_vecs);
 
-	rc = pci_alloc_irq_vectors(adapter->pdev, msix_vecs, msix_vecs,
-			PCI_IRQ_MSIX);
-	if (rc < 0) {
+	irq_cnt = pci_alloc_irq_vectors(adapter->pdev, ENA_MIN_MSIX_VEC,
+					msix_vecs, PCI_IRQ_MSIX);
+
+	if (irq_cnt < 0) {
 		netif_err(adapter, probe, adapter->netdev,
-			  "Failed to enable MSI-X, vectors %d rc %d\n",
-			  msix_vecs, rc);
+			  "Failed to enable MSI-X. irq_cnt %d\n", irq_cnt);
 		return -ENOSPC;
 	}
 
-	netif_dbg(adapter, probe, adapter->netdev, "enable MSI-X, vectors %d\n",
-		  msix_vecs);
-
-	if (msix_vecs >= 1) {
-		if (ena_init_rx_cpu_rmap(adapter))
-			netif_warn(adapter, probe, adapter->netdev,
-				   "Failed to map IRQs to CPUs\n");
+	if (irq_cnt != msix_vecs) {
+		netif_notice(adapter, probe, adapter->netdev,
+			     "enable only %d MSI-X (out of %d), reduce the number of queues\n",
+			     irq_cnt, msix_vecs);
+		adapter->num_queues = irq_cnt - ENA_ADMIN_MSIX_VEC;
 	}
 
-	adapter->msix_vecs = msix_vecs;
+	if (ena_init_rx_cpu_rmap(adapter))
+		netif_warn(adapter, probe, adapter->netdev,
+			   "Failed to map IRQs to CPUs\n");
+
+	adapter->msix_vecs = irq_cnt;
+	set_bit(ENA_FLAG_MSIX_ENABLED, &adapter->flags);
 
 	return 0;
 }
@@ -1311,6 +1398,12 @@ static int ena_request_io_irq(struct ena_adapter *adapter)
 	struct ena_irq *irq;
 	int rc = 0, i, k;
 
+	if (!test_bit(ENA_FLAG_MSIX_ENABLED, &adapter->flags)) {
+		netif_err(adapter, ifup, adapter->netdev,
+			  "Failed to request I/O IRQ: MSI-X is not enabled\n");
+		return -EINVAL;
+	}
+
 	for (i = ENA_IO_IRQ_FIRST_IDX; i < adapter->msix_vecs; i++) {
 		irq = &adapter->irq_tbl[i];
 		rc = request_irq(irq->vector, irq->handler, flags, irq->name,
@@ -1369,6 +1462,12 @@ static void ena_free_io_irq(struct ena_adapter *adapter)
 	}
 }
 
+static void ena_disable_msix(struct ena_adapter *adapter)
+{
+	if (test_and_clear_bit(ENA_FLAG_MSIX_ENABLED, &adapter->flags))
+		pci_free_irq_vectors(adapter->pdev);
+}
+
 static void ena_disable_io_intr_sync(struct ena_adapter *adapter)
 {
 	int i;
@@ -1439,7 +1538,7 @@ static int ena_rss_configure(struct ena_adapter *adapter)
 	/* In case the RSS table wasn't initialized by probe */
 	if (!ena_dev->rss.tbl_log_size) {
 		rc = ena_rss_init_default(adapter);
-		if (rc && (rc != -EPERM)) {
+		if (rc && (rc != -EOPNOTSUPP)) {
 			netif_err(adapter, ifup, adapter->netdev,
 				  "Failed to init RSS rc: %d\n", rc);
 			return rc;
@@ -1448,17 +1547,17 @@ static int ena_rss_configure(struct ena_adapter *adapter)
 
 	/* Set indirect table */
 	rc = ena_com_indirect_table_set(ena_dev);
-	if (unlikely(rc && rc != -EPERM))
+	if (unlikely(rc && rc != -EOPNOTSUPP))
 		return rc;
 
 	/* Configure hash function (if supported) */
 	rc = ena_com_set_hash_function(ena_dev);
-	if (unlikely(rc && (rc != -EPERM)))
+	if (unlikely(rc && (rc != -EOPNOTSUPP)))
 		return rc;
 
 	/* Configure hash inputs (if supported) */
 	rc = ena_com_set_hash_ctrl(ena_dev);
-	if (unlikely(rc && (rc != -EPERM)))
+	if (unlikely(rc && (rc != -EOPNOTSUPP)))
 		return rc;
 
 	return 0;
@@ -1485,6 +1584,11 @@ static int ena_up_complete(struct ena_adapter *adapter)
 
 	ena_napi_enable_all(adapter);
 
+	/* Enable completion queues interrupt */
+	for (i = 0; i < adapter->num_queues; i++)
+		ena_unmask_interrupt(&adapter->tx_ring[i],
+				     &adapter->rx_ring[i]);
+
 	/* schedule napi in case we had pending packets
 	 * from the last time we disable napi
 	 */
@@ -1532,6 +1636,7 @@ static int ena_create_io_tx_queue(struct ena_adapter *adapter, int qid)
 			  "Failed to get TX queue handlers. TX queue num %d rc: %d\n",
 			  qid, rc);
 		ena_com_destroy_io_queue(ena_dev, ena_qid);
+		return rc;
 	}
 
 	ena_com_update_numa_node(tx_ring->ena_com_io_cq, ctx.numa_node);
@@ -1596,6 +1701,7 @@ static int ena_create_io_rx_queue(struct ena_adapter *adapter, int qid)
 			  "Failed to get RX queue handlers. RX queue num %d rc: %d\n",
 			  qid, rc);
 		ena_com_destroy_io_queue(ena_dev, ena_qid);
+		return rc;
 	}
 
 	ena_com_update_numa_node(rx_ring->ena_com_io_cq, ctx.numa_node);
@@ -1706,7 +1812,7 @@ static void ena_down(struct ena_adapter *adapter)
 	if (test_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags)) {
 		int rc;
 
-		rc = ena_com_dev_reset(adapter->ena_dev);
+		rc = ena_com_dev_reset(adapter->ena_dev, adapter->reset_reason);
 		if (rc)
 			dev_err(&adapter->pdev->dev, "Device reset failed\n");
 	}
@@ -1981,6 +2087,7 @@ static netdev_tx_t ena_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	tx_info->tx_descs = nb_hw_desc;
 	tx_info->last_jiffies = jiffies;
+	tx_info->print_once = 0;
 
 	tx_ring->next_to_use = ENA_TX_RING_IDX_NEXT(next_to_use,
 		tx_ring->ring_size);
@@ -2129,7 +2236,7 @@ static void ena_config_host_info(struct ena_com_dev *ena_dev)
 
 	rc = ena_com_set_host_attributes(ena_dev);
 	if (rc) {
-		if (rc == -EPERM)
+		if (rc == -EOPNOTSUPP)
 			pr_warn("Cannot set host attributes\n");
 		else
 			pr_err("Cannot set host attributes\n");
@@ -2166,7 +2273,7 @@ static void ena_config_debug_area(struct ena_adapter *adapter)
 
 	rc = ena_com_set_host_attributes(adapter->ena_dev);
 	if (rc) {
-		if (rc == -EPERM)
+		if (rc == -EOPNOTSUPP)
 			netif_warn(adapter, drv, adapter->netdev,
 				   "Cannot set host attributes\n");
 		else
@@ -2338,7 +2445,7 @@ static int ena_device_init(struct ena_com_dev *ena_dev, struct pci_dev *pdev,
 	readless_supported = !(pdev->revision & ENA_MMIO_DISABLE_REG_READ);
 	ena_com_set_mmio_read_mode(ena_dev, readless_supported);
 
-	rc = ena_com_dev_reset(ena_dev);
+	rc = ena_com_dev_reset(ena_dev, ENA_REGS_RESET_NORMAL);
 	if (rc) {
 		dev_err(dev, "Can not reset device\n");
 		goto err_mmio_read_less;
@@ -2449,7 +2556,8 @@ static int ena_enable_msix_and_set_admin_interrupts(struct ena_adapter *adapter,
 	return 0;
 
 err_disable_msix:
-	pci_free_irq_vectors(adapter->pdev);
+	ena_disable_msix(adapter);
+
 	return rc;
 }
 
@@ -2487,7 +2595,7 @@ static void ena_fw_reset_device(struct work_struct *work)
 
 	ena_free_mgmnt_irq(adapter);
 
-	pci_free_irq_vectors(adapter->pdev);
+	ena_disable_msix(adapter);
 
 	ena_com_abort_admin_commands(ena_dev);
 
@@ -2497,6 +2605,7 @@ static void ena_fw_reset_device(struct work_struct *work)
 
 	ena_com_mmio_reg_read_request_destroy(ena_dev);
 
+	adapter->reset_reason = ENA_REGS_RESET_NORMAL;
 	clear_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags);
 
 	/* Finish with the destroy part. Start the init part */
@@ -2538,7 +2647,7 @@ static void ena_fw_reset_device(struct work_struct *work)
 	return;
 err_disable_msix:
 	ena_free_mgmnt_irq(adapter);
-	pci_free_irq_vectors(adapter->pdev);
+	ena_disable_msix(adapter);
 err_device_destroy:
 	ena_com_admin_destroy(ena_dev);
 err:
@@ -2550,13 +2659,47 @@ err:
 		"Reset attempt failed. Can not reset the device\n");
 }
 
-static void check_for_missing_tx_completions(struct ena_adapter *adapter)
+static int check_missing_comp_in_queue(struct ena_adapter *adapter,
+				       struct ena_ring *tx_ring)
 {
 	struct ena_tx_buffer *tx_buf;
 	unsigned long last_jiffies;
+	u32 missed_tx = 0;
+	int i;
+
+	for (i = 0; i < tx_ring->ring_size; i++) {
+		tx_buf = &tx_ring->tx_buffer_info[i];
+		last_jiffies = tx_buf->last_jiffies;
+		if (unlikely(last_jiffies &&
+			     time_is_before_jiffies(last_jiffies + adapter->missing_tx_completion_to))) {
+			if (!tx_buf->print_once)
+				netif_notice(adapter, tx_err, adapter->netdev,
+					     "Found a Tx that wasn't completed on time, qid %d, index %d.\n",
+					     tx_ring->qid, i);
+
+			tx_buf->print_once = 1;
+			missed_tx++;
+
+			if (unlikely(missed_tx > adapter->missing_tx_completion_threshold)) {
+				netif_err(adapter, tx_err, adapter->netdev,
+					  "The number of lost tx completions is above the threshold (%d > %d). Reset the device\n",
+					  missed_tx,
+					  adapter->missing_tx_completion_threshold);
+				adapter->reset_reason =
+					ENA_REGS_RESET_MISS_TX_CMPL;
+				set_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags);
+				return -EIO;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static void check_for_missing_tx_completions(struct ena_adapter *adapter)
+{
 	struct ena_ring *tx_ring;
-	int i, j, budget;
-	u32 missed_tx;
+	int i, budget, rc;
 
 	/* Make sure the driver doesn't turn the device in other process */
 	smp_rmb();
@@ -2567,36 +2710,17 @@ static void check_for_missing_tx_completions(struct ena_adapter *adapter)
 	if (test_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags))
 		return;
 
+	if (adapter->missing_tx_completion_to == ENA_HW_HINTS_NO_TIMEOUT)
+		return;
+
 	budget = ENA_MONITORED_TX_QUEUES;
 
 	for (i = adapter->last_monitored_tx_qid; i < adapter->num_queues; i++) {
 		tx_ring = &adapter->tx_ring[i];
 
-		for (j = 0; j < tx_ring->ring_size; j++) {
-			tx_buf = &tx_ring->tx_buffer_info[j];
-			last_jiffies = tx_buf->last_jiffies;
-			if (unlikely(last_jiffies && time_is_before_jiffies(last_jiffies + TX_TIMEOUT))) {
-				netif_notice(adapter, tx_err, adapter->netdev,
-					     "Found a Tx that wasn't completed on time, qid %d, index %d.\n",
-					     tx_ring->qid, j);
-
-				u64_stats_update_begin(&tx_ring->syncp);
-				missed_tx = tx_ring->tx_stats.missing_tx_comp++;
-				u64_stats_update_end(&tx_ring->syncp);
-
-				/* Clear last jiffies so the lost buffer won't
-				 * be counted twice.
-				 */
-				tx_buf->last_jiffies = 0;
-
-				if (unlikely(missed_tx > MAX_NUM_OF_TIMEOUTED_PACKETS)) {
-					netif_err(adapter, tx_err, adapter->netdev,
-						  "The number of lost tx completion is above the threshold (%d > %d). Reset the device\n",
-						  missed_tx, MAX_NUM_OF_TIMEOUTED_PACKETS);
-					set_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags);
-				}
-			}
-		}
+		rc = check_missing_comp_in_queue(adapter, tx_ring);
+		if (unlikely(rc))
+			return;
 
 		budget--;
 		if (!budget)
@@ -2606,6 +2730,58 @@ static void check_for_missing_tx_completions(struct ena_adapter *adapter)
 	adapter->last_monitored_tx_qid = i % adapter->num_queues;
 }
 
+/* trigger napi schedule after 2 consecutive detections */
+#define EMPTY_RX_REFILL 2
+/* For the rare case where the device runs out of Rx descriptors and the
+ * napi handler failed to refill new Rx descriptors (due to a lack of memory
+ * for example).
+ * This case will lead to a deadlock:
+ * The device won't send interrupts since all the new Rx packets will be dropped
+ * The napi handler won't allocate new Rx descriptors so the device will be
+ * able to send new packets.
+ *
+ * This scenario can happen when the kernel's vm.min_free_kbytes is too small.
+ * It is recommended to have at least 512MB, with a minimum of 128MB for
+ * constrained environment).
+ *
+ * When such a situation is detected - Reschedule napi
+ */
+static void check_for_empty_rx_ring(struct ena_adapter *adapter)
+{
+	struct ena_ring *rx_ring;
+	int i, refill_required;
+
+	if (!test_bit(ENA_FLAG_DEV_UP, &adapter->flags))
+		return;
+
+	if (test_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags))
+		return;
+
+	for (i = 0; i < adapter->num_queues; i++) {
+		rx_ring = &adapter->rx_ring[i];
+
+		refill_required =
+			ena_com_sq_empty_space(rx_ring->ena_com_io_sq);
+		if (unlikely(refill_required == (rx_ring->ring_size - 1))) {
+			rx_ring->empty_rx_queue++;
+
+			if (rx_ring->empty_rx_queue >= EMPTY_RX_REFILL) {
+				u64_stats_update_begin(&rx_ring->syncp);
+				rx_ring->rx_stats.empty_rx_ring++;
+				u64_stats_update_end(&rx_ring->syncp);
+
+				netif_err(adapter, drv, adapter->netdev,
+					  "trigger refill for ring %d\n", i);
+
+				napi_schedule(rx_ring->napi);
+				rx_ring->empty_rx_queue = 0;
+			}
+		} else {
+			rx_ring->empty_rx_queue = 0;
+		}
+	}
+}
+
 /* Check for keep alive expiration */
 static void check_for_missing_keep_alive(struct ena_adapter *adapter)
 {
@@ -2614,14 +2790,18 @@ static void check_for_missing_keep_alive(struct ena_adapter *adapter)
 	if (!adapter->wd_state)
 		return;
 
-	keep_alive_expired = round_jiffies(adapter->last_keep_alive_jiffies
-					   + ENA_DEVICE_KALIVE_TIMEOUT);
+	if (adapter->keep_alive_timeout == ENA_HW_HINTS_NO_TIMEOUT)
+		return;
+
+	keep_alive_expired = round_jiffies(adapter->last_keep_alive_jiffies +
+					   adapter->keep_alive_timeout);
 	if (unlikely(time_is_before_jiffies(keep_alive_expired))) {
 		netif_err(adapter, drv, adapter->netdev,
 			  "Keep alive watchdog timeout.\n");
 		u64_stats_update_begin(&adapter->syncp);
 		adapter->dev_stats.wd_expired++;
 		u64_stats_update_end(&adapter->syncp);
+		adapter->reset_reason = ENA_REGS_RESET_KEEP_ALIVE_TO;
 		set_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags);
 	}
 }
@@ -2634,10 +2814,49 @@ static void check_for_admin_com_state(struct ena_adapter *adapter)
 		u64_stats_update_begin(&adapter->syncp);
 		adapter->dev_stats.admin_q_pause++;
 		u64_stats_update_end(&adapter->syncp);
+		adapter->reset_reason = ENA_REGS_RESET_ADMIN_TO;
 		set_bit(ENA_FLAG_TRIGGER_RESET, &adapter->flags);
 	}
 }
 
+static void ena_update_hints(struct ena_adapter *adapter,
+			     struct ena_admin_ena_hw_hints *hints)
+{
+	struct net_device *netdev = adapter->netdev;
+
+	if (hints->admin_completion_tx_timeout)
+		adapter->ena_dev->admin_queue.completion_timeout =
+			hints->admin_completion_tx_timeout * 1000;
+
+	if (hints->mmio_read_timeout)
+		/* convert to usec */
+		adapter->ena_dev->mmio_read.reg_read_to =
+			hints->mmio_read_timeout * 1000;
+
+	if (hints->missed_tx_completion_count_threshold_to_reset)
+		adapter->missing_tx_completion_threshold =
+			hints->missed_tx_completion_count_threshold_to_reset;
+
+	if (hints->missing_tx_completion_timeout) {
+		if (hints->missing_tx_completion_timeout == ENA_HW_HINTS_NO_TIMEOUT)
+			adapter->missing_tx_completion_to = ENA_HW_HINTS_NO_TIMEOUT;
+		else
+			adapter->missing_tx_completion_to =
+				msecs_to_jiffies(hints->missing_tx_completion_timeout);
+	}
+
+	if (hints->netdev_wd_timeout)
+		netdev->watchdog_timeo = msecs_to_jiffies(hints->netdev_wd_timeout);
+
+	if (hints->driver_watchdog_timeout) {
+		if (hints->driver_watchdog_timeout == ENA_HW_HINTS_NO_TIMEOUT)
+			adapter->keep_alive_timeout = ENA_HW_HINTS_NO_TIMEOUT;
+		else
+			adapter->keep_alive_timeout =
+				msecs_to_jiffies(hints->driver_watchdog_timeout);
+	}
+}
+
 static void ena_update_host_info(struct ena_admin_host_info *host_info,
 				 struct net_device *netdev)
 {
@@ -2660,6 +2879,8 @@ static void ena_timer_service(unsigned long data)
 
 	check_for_missing_tx_completions(adapter);
 
+	check_for_empty_rx_ring(adapter);
+
 	if (debug_area)
 		ena_dump_stats_to_buf(adapter, debug_area);
 
@@ -2808,7 +3029,7 @@ static int ena_rss_init_default(struct ena_adapter *adapter)
 		val = ethtool_rxfh_indir_default(i, adapter->num_queues);
 		rc = ena_com_indirect_table_fill_entry(ena_dev, i,
 						       ENA_IO_RXQ_IDX(val));
-		if (unlikely(rc && (rc != -EPERM))) {
+		if (unlikely(rc && (rc != -EOPNOTSUPP))) {
 			dev_err(dev, "Cannot fill indirect table\n");
 			goto err_fill_indir;
 		}
@@ -2816,13 +3037,13 @@ static int ena_rss_init_default(struct ena_adapter *adapter)
 
 	rc = ena_com_fill_hash_function(ena_dev, ENA_ADMIN_CRC32, NULL,
 					ENA_HASH_KEY_SIZE, 0xFFFFFFFF);
-	if (unlikely(rc && (rc != -EPERM))) {
+	if (unlikely(rc && (rc != -EOPNOTSUPP))) {
 		dev_err(dev, "Cannot fill hash function\n");
 		goto err_fill_indir;
 	}
 
 	rc = ena_com_set_default_hash_ctrl(ena_dev);
-	if (unlikely(rc && (rc != -EPERM))) {
+	if (unlikely(rc && (rc != -EOPNOTSUPP))) {
 		dev_err(dev, "Cannot fill hash control\n");
 		goto err_fill_indir;
 	}
@@ -2840,6 +3061,11 @@ static void ena_release_bars(struct ena_com_dev *ena_dev, struct pci_dev *pdev)
 {
 	int release_bars;
 
+	if (ena_dev->mem_bar)
+		devm_iounmap(&pdev->dev, ena_dev->mem_bar);
+
+	devm_iounmap(&pdev->dev, ena_dev->reg_bar);
+
 	release_bars = pci_select_bars(pdev, IORESOURCE_MEM) & ENA_BAR_MASK;
 	pci_release_selected_regions(pdev, release_bars);
 }
@@ -2927,8 +3153,9 @@ static int ena_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto err_free_ena_dev;
 	}
 
-	ena_dev->reg_bar = ioremap(pci_resource_start(pdev, ENA_REG_BAR),
-				   pci_resource_len(pdev, ENA_REG_BAR));
+	ena_dev->reg_bar = devm_ioremap(&pdev->dev,
+					pci_resource_start(pdev, ENA_REG_BAR),
+					pci_resource_len(pdev, ENA_REG_BAR));
 	if (!ena_dev->reg_bar) {
 		dev_err(&pdev->dev, "failed to remap regs bar\n");
 		rc = -EFAULT;
@@ -2948,8 +3175,9 @@ static int ena_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	ena_set_push_mode(pdev, ena_dev, &get_feat_ctx);
 
 	if (ena_dev->tx_mem_queue_type == ENA_ADMIN_PLACEMENT_POLICY_DEV) {
-		ena_dev->mem_bar = ioremap_wc(pci_resource_start(pdev, ENA_MEM_BAR),
-					      pci_resource_len(pdev, ENA_MEM_BAR));
+		ena_dev->mem_bar = devm_ioremap_wc(&pdev->dev,
+						   pci_resource_start(pdev, ENA_MEM_BAR),
+						   pci_resource_len(pdev, ENA_MEM_BAR));
 		if (!ena_dev->mem_bar) {
 			rc = -EFAULT;
 			goto err_device_destroy;
@@ -2991,6 +3219,7 @@ static int ena_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	ena_set_conf_feat_params(adapter, &get_feat_ctx);
 
 	adapter->msg_enable = netif_msg_init(debug, DEFAULT_MSG_ENABLE);
+	adapter->reset_reason = ENA_REGS_RESET_NORMAL;
 
 	adapter->tx_ring_size = queue_size;
 	adapter->rx_ring_size = queue_size;
@@ -3029,7 +3258,7 @@ static int ena_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto err_worker_destroy;
 	}
 	rc = ena_rss_init_default(adapter);
-	if (rc && (rc != -EPERM)) {
+	if (rc && (rc != -EOPNOTSUPP)) {
 		dev_err(&pdev->dev, "Cannot init RSS rc: %d\n", rc);
 		goto err_free_msix;
 	}
@@ -3051,6 +3280,11 @@ static int ena_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	INIT_WORK(&adapter->reset_task, ena_fw_reset_device);
 
 	adapter->last_keep_alive_jiffies = jiffies;
+	adapter->keep_alive_timeout = ENA_DEVICE_KALIVE_TIMEOUT;
+	adapter->missing_tx_completion_to = TX_TIMEOUT;
+	adapter->missing_tx_completion_threshold = MAX_NUM_OF_TIMEOUTED_PACKETS;
+
+	ena_update_hints(adapter, &get_feat_ctx.hw_hints);
 
 	setup_timer(&adapter->timer_service, ena_timer_service,
 		    (unsigned long)adapter);
@@ -3070,9 +3304,9 @@ err_rss:
 	ena_com_delete_debug_area(ena_dev);
 	ena_com_rss_destroy(ena_dev);
 err_free_msix:
-	ena_com_dev_reset(ena_dev);
+	ena_com_dev_reset(ena_dev, ENA_REGS_RESET_INIT_ERR);
 	ena_free_mgmnt_irq(adapter);
-	pci_free_irq_vectors(adapter->pdev);
+	ena_disable_msix(adapter);
 err_worker_destroy:
 	ena_com_destroy_interrupt_moderation(ena_dev);
 	del_timer(&adapter->timer_service);
@@ -3153,11 +3387,11 @@ static void ena_remove(struct pci_dev *pdev)
 
 	/* Reset the device only if the device is running. */
 	if (test_bit(ENA_FLAG_DEVICE_RUNNING, &adapter->flags))
-		ena_com_dev_reset(ena_dev);
+		ena_com_dev_reset(ena_dev, adapter->reset_reason);
 
 	ena_free_mgmnt_irq(adapter);
 
-	pci_free_irq_vectors(adapter->pdev);
+	ena_disable_msix(adapter);
 
 	free_netdev(netdev);
 
@@ -3244,14 +3478,24 @@ static void ena_keep_alive_wd(void *adapter_data,
 			      struct ena_admin_aenq_entry *aenq_e)
 {
 	struct ena_adapter *adapter = (struct ena_adapter *)adapter_data;
+	struct ena_admin_aenq_keep_alive_desc *desc;
+	u64 rx_drops;
 
+	desc = (struct ena_admin_aenq_keep_alive_desc *)aenq_e;
 	adapter->last_keep_alive_jiffies = jiffies;
+
+	rx_drops = ((u64)desc->rx_drops_high << 32) | desc->rx_drops_low;
+
+	u64_stats_update_begin(&adapter->syncp);
+	adapter->dev_stats.rx_drops = rx_drops;
+	u64_stats_update_end(&adapter->syncp);
 }
 
 static void ena_notification(void *adapter_data,
 			     struct ena_admin_aenq_entry *aenq_e)
 {
 	struct ena_adapter *adapter = (struct ena_adapter *)adapter_data;
+	struct ena_admin_ena_hw_hints *hints;
 
 	WARN(aenq_e->aenq_common_desc.group != ENA_ADMIN_NOTIFICATION,
 	     "Invalid group(%x) expected %x\n",
@@ -3269,6 +3513,11 @@ static void ena_notification(void *adapter_data,
 	case ENA_ADMIN_RESUME:
 		queue_work(ena_wq, &adapter->resume_io_task);
 		break;
+	case ENA_ADMIN_UPDATE_HINTS:
+		hints = (struct ena_admin_ena_hw_hints *)
+			(&aenq_e->inline_data_w4);
+		ena_update_hints(adapter, hints);
+		break;
 	default:
 		netif_err(adapter, drv, adapter->netdev,
 			  "Invalid aenq notification link state %d\n",
diff --git a/drivers/net/ethernet/amazon/ena/ena_netdev.h b/drivers/net/ethernet/amazon/ena/ena_netdev.h
index 0e22bce6239d..29bb5704260b 100644
--- a/drivers/net/ethernet/amazon/ena/ena_netdev.h
+++ b/drivers/net/ethernet/amazon/ena/ena_netdev.h
@@ -44,21 +44,24 @@
 #include "ena_eth_com.h"
 
 #define DRV_MODULE_VER_MAJOR	1
-#define DRV_MODULE_VER_MINOR	1
-#define DRV_MODULE_VER_SUBMINOR 2
+#define DRV_MODULE_VER_MINOR	2
+#define DRV_MODULE_VER_SUBMINOR 0
 
 #define DRV_MODULE_NAME		"ena"
 #ifndef DRV_MODULE_VERSION
 #define DRV_MODULE_VERSION \
 	__stringify(DRV_MODULE_VER_MAJOR) "."	\
 	__stringify(DRV_MODULE_VER_MINOR) "."	\
-	__stringify(DRV_MODULE_VER_SUBMINOR)
+	__stringify(DRV_MODULE_VER_SUBMINOR) "k"
 #endif
 
 #define DEVICE_NAME	"Elastic Network Adapter (ENA)"
 
 /* 1 for AENQ + ADMIN */
-#define ENA_MAX_MSIX_VEC(io_queues)	(1 + (io_queues))
+#define ENA_ADMIN_MSIX_VEC		1
+#define ENA_MAX_MSIX_VEC(io_queues)	(ENA_ADMIN_MSIX_VEC + (io_queues))
+
+#define ENA_MIN_MSIX_VEC		2
 
 #define ENA_REG_BAR			0
 #define ENA_MEM_BAR			2
@@ -146,7 +149,18 @@ struct ena_tx_buffer {
 	u32 tx_descs;
 	/* num of buffers used by this skb */
 	u32 num_of_bufs;
-	/* Save the last jiffies to detect missing tx packets */
+
+	/* Used for detect missing tx packets to limit the number of prints */
+	u32 print_once;
+	/* Save the last jiffies to detect missing tx packets
+	 *
+	 * sets to non zero value on ena_start_xmit and set to zero on
+	 * napi and timer_Service_routine.
+	 *
+	 * while this value is not protected by lock,
+	 * a given packet is not expected to be handled by ena_start_xmit
+	 * and by napi/timer_service at the same time.
+	 */
 	unsigned long last_jiffies;
 	struct ena_com_buf bufs[ENA_PKT_MAX_BUFS];
 } ____cacheline_aligned;
@@ -170,7 +184,6 @@ struct ena_stats_tx {
 	u64 napi_comp;
 	u64 tx_poll;
 	u64 doorbells;
-	u64 missing_tx_comp;
 	u64 bad_req_id;
 };
 
@@ -184,11 +197,19 @@ struct ena_stats_rx {
 	u64 dma_mapping_err;
 	u64 bad_desc_num;
 	u64 rx_copybreak_pkt;
+	u64 bad_req_id;
+	u64 empty_rx_ring;
 };
 
 struct ena_ring {
-	/* Holds the empty requests for TX out of order completions */
-	u16 *free_tx_ids;
+	union {
+		/* Holds the empty requests for TX/RX
+		 * out of order completions
+		 */
+		u16 *free_tx_ids;
+		u16 *free_rx_ids;
+	};
+
 	union {
 		struct ena_tx_buffer *tx_buffer_info;
 		struct ena_rx_buffer *rx_buffer_info;
@@ -231,6 +252,7 @@ struct ena_ring {
 		struct ena_stats_tx tx_stats;
 		struct ena_stats_rx rx_stats;
 	};
+	int empty_rx_queue;
 } ____cacheline_aligned;
 
 struct ena_stats_dev {
@@ -248,6 +270,7 @@ enum ena_flags_t {
 	ENA_FLAG_DEVICE_RUNNING,
 	ENA_FLAG_DEV_UP,
 	ENA_FLAG_LINK_UP,
+	ENA_FLAG_MSIX_ENABLED,
 	ENA_FLAG_TRIGGER_RESET
 };
 
@@ -268,6 +291,8 @@ struct ena_adapter {
 
 	int msix_vecs;
 
+	u32 missing_tx_completion_threshold;
+
 	u32 tx_usecs, rx_usecs; /* interrupt moderation */
 	u32 tx_frames, rx_frames; /* interrupt moderation */
 
@@ -281,6 +306,9 @@ struct ena_adapter {
 
 	u8 mac_addr[ETH_ALEN];
 
+	unsigned long keep_alive_timeout;
+	unsigned long missing_tx_completion_to;
+
 	char name[ENA_NAME_MAX_LEN];
 
 	unsigned long flags;
@@ -310,6 +338,8 @@ struct ena_adapter {
 
 	/* last queue index that was checked for uncompleted tx packets */
 	u32 last_monitored_tx_qid;
+
+	enum ena_regs_reset_reason_types reset_reason;
 };
 
 void ena_set_ethtool_ops(struct net_device *netdev);
diff --git a/drivers/net/ethernet/amazon/ena/ena_regs_defs.h b/drivers/net/ethernet/amazon/ena/ena_regs_defs.h
index 26097a2b6030..9aec43c5bba8 100644
--- a/drivers/net/ethernet/amazon/ena/ena_regs_defs.h
+++ b/drivers/net/ethernet/amazon/ena/ena_regs_defs.h
@@ -32,6 +32,36 @@
 #ifndef _ENA_REGS_H_
 #define _ENA_REGS_H_
 
+enum ena_regs_reset_reason_types {
+	ENA_REGS_RESET_NORMAL			= 0,
+
+	ENA_REGS_RESET_KEEP_ALIVE_TO		= 1,
+
+	ENA_REGS_RESET_ADMIN_TO			= 2,
+
+	ENA_REGS_RESET_MISS_TX_CMPL		= 3,
+
+	ENA_REGS_RESET_INV_RX_REQ_ID		= 4,
+
+	ENA_REGS_RESET_INV_TX_REQ_ID		= 5,
+
+	ENA_REGS_RESET_TOO_MANY_RX_DESCS	= 6,
+
+	ENA_REGS_RESET_INIT_ERR			= 7,
+
+	ENA_REGS_RESET_DRIVER_INVALID_STATE	= 8,
+
+	ENA_REGS_RESET_OS_TRIGGER		= 9,
+
+	ENA_REGS_RESET_OS_NETDEV_WD		= 10,
+
+	ENA_REGS_RESET_SHUTDOWN			= 11,
+
+	ENA_REGS_RESET_USER_TRIGGER		= 12,
+
+	ENA_REGS_RESET_GENERIC			= 13,
+};
+
 /* ena_registers offsets */
 #define ENA_REGS_VERSION_OFF		0x0
 #define ENA_REGS_CONTROLLER_VERSION_OFF		0x4
@@ -78,6 +108,8 @@
 #define ENA_REGS_CAPS_RESET_TIMEOUT_MASK		0x3e
 #define ENA_REGS_CAPS_DMA_ADDR_WIDTH_SHIFT		8
 #define ENA_REGS_CAPS_DMA_ADDR_WIDTH_MASK		0xff00
+#define ENA_REGS_CAPS_ADMIN_CMD_TO_SHIFT		16
+#define ENA_REGS_CAPS_ADMIN_CMD_TO_MASK		0xf0000
 
 /* aq_caps register */
 #define ENA_REGS_AQ_CAPS_AQ_DEPTH_MASK		0xffff
@@ -102,6 +134,8 @@
 #define ENA_REGS_DEV_CTL_QUIESCENT_MASK		0x4
 #define ENA_REGS_DEV_CTL_IO_RESUME_SHIFT		3
 #define ENA_REGS_DEV_CTL_IO_RESUME_MASK		0x8
+#define ENA_REGS_DEV_CTL_RESET_REASON_SHIFT		28
+#define ENA_REGS_DEV_CTL_RESET_REASON_MASK		0xf0000000
 
 /* dev_sts register */
 #define ENA_REGS_DEV_STS_READY_MASK		0x1
diff --git a/drivers/net/ethernet/amd/pcnet32.c b/drivers/net/ethernet/amd/pcnet32.c
index 86369d7c9a0f..7f60d17819ce 100644
--- a/drivers/net/ethernet/amd/pcnet32.c
+++ b/drivers/net/ethernet/amd/pcnet32.c
@@ -731,12 +731,10 @@ static int pcnet32_get_link_ksettings(struct net_device *dev,
 {
 	struct pcnet32_private *lp = netdev_priv(dev);
 	unsigned long flags;
-	int r = -EOPNOTSUPP;
 
 	spin_lock_irqsave(&lp->lock, flags);
 	if (lp->mii) {
 		mii_ethtool_get_link_ksettings(&lp->mii_if, cmd);
-		r = 0;
 	} else if (lp->chip_version == PCNET32_79C970A) {
 		if (lp->autoneg) {
 			cmd->base.autoneg = AUTONEG_ENABLE;
@@ -753,10 +751,9 @@ static int pcnet32_get_link_ksettings(struct net_device *dev,
 		ethtool_convert_legacy_u32_to_link_mode(
 						cmd->link_modes.supported,
 						SUPPORTED_TP | SUPPORTED_AUI);
-		r = 0;
 	}
 	spin_unlock_irqrestore(&lp->lock, flags);
-	return r;
+	return 0;
 }
 
 static int pcnet32_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-common.h b/drivers/net/ethernet/amd/xgbe/xgbe-common.h
index 127adbeefb10..9795419aac2d 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-common.h
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-common.h
@@ -123,38 +123,13 @@
 #define DMA_ISR				0x3008
 #define DMA_AXIARCR			0x3010
 #define DMA_AXIAWCR			0x3018
+#define DMA_AXIAWARCR			0x301c
 #define DMA_DSR0			0x3020
 #define DMA_DSR1			0x3024
+#define DMA_TXEDMACR			0x3040
+#define DMA_RXEDMACR			0x3044
 
 /* DMA register entry bit positions and sizes */
-#define DMA_AXIARCR_DRC_INDEX		0
-#define DMA_AXIARCR_DRC_WIDTH		4
-#define DMA_AXIARCR_DRD_INDEX		4
-#define DMA_AXIARCR_DRD_WIDTH		2
-#define DMA_AXIARCR_TEC_INDEX		8
-#define DMA_AXIARCR_TEC_WIDTH		4
-#define DMA_AXIARCR_TED_INDEX		12
-#define DMA_AXIARCR_TED_WIDTH		2
-#define DMA_AXIARCR_THC_INDEX		16
-#define DMA_AXIARCR_THC_WIDTH		4
-#define DMA_AXIARCR_THD_INDEX		20
-#define DMA_AXIARCR_THD_WIDTH		2
-#define DMA_AXIAWCR_DWC_INDEX		0
-#define DMA_AXIAWCR_DWC_WIDTH		4
-#define DMA_AXIAWCR_DWD_INDEX		4
-#define DMA_AXIAWCR_DWD_WIDTH		2
-#define DMA_AXIAWCR_RPC_INDEX		8
-#define DMA_AXIAWCR_RPC_WIDTH		4
-#define DMA_AXIAWCR_RPD_INDEX		12
-#define DMA_AXIAWCR_RPD_WIDTH		2
-#define DMA_AXIAWCR_RHC_INDEX		16
-#define DMA_AXIAWCR_RHC_WIDTH		4
-#define DMA_AXIAWCR_RHD_INDEX		20
-#define DMA_AXIAWCR_RHD_WIDTH		2
-#define DMA_AXIAWCR_TDC_INDEX		24
-#define DMA_AXIAWCR_TDC_WIDTH		4
-#define DMA_AXIAWCR_TDD_INDEX		28
-#define DMA_AXIAWCR_TDD_WIDTH		2
 #define DMA_ISR_MACIS_INDEX		17
 #define DMA_ISR_MACIS_WIDTH		1
 #define DMA_ISR_MTLIS_INDEX		16
@@ -163,14 +138,31 @@
 #define DMA_MR_INTM_WIDTH		2
 #define DMA_MR_SWR_INDEX		0
 #define DMA_MR_SWR_WIDTH		1
+#define DMA_RXEDMACR_RDPS_INDEX		0
+#define DMA_RXEDMACR_RDPS_WIDTH		3
+#define DMA_SBMR_AAL_INDEX		12
+#define DMA_SBMR_AAL_WIDTH		1
 #define DMA_SBMR_EAME_INDEX		11
 #define DMA_SBMR_EAME_WIDTH		1
-#define DMA_SBMR_BLEN_256_INDEX		7
-#define DMA_SBMR_BLEN_256_WIDTH		1
+#define DMA_SBMR_BLEN_INDEX		1
+#define DMA_SBMR_BLEN_WIDTH		7
+#define DMA_SBMR_RD_OSR_LMT_INDEX	16
+#define DMA_SBMR_RD_OSR_LMT_WIDTH	6
 #define DMA_SBMR_UNDEF_INDEX		0
 #define DMA_SBMR_UNDEF_WIDTH		1
+#define DMA_SBMR_WR_OSR_LMT_INDEX	24
+#define DMA_SBMR_WR_OSR_LMT_WIDTH	6
+#define DMA_TXEDMACR_TDPS_INDEX		0
+#define DMA_TXEDMACR_TDPS_WIDTH		3
 
 /* DMA register values */
+#define DMA_SBMR_BLEN_256		256
+#define DMA_SBMR_BLEN_128		128
+#define DMA_SBMR_BLEN_64		64
+#define DMA_SBMR_BLEN_32		32
+#define DMA_SBMR_BLEN_16		16
+#define DMA_SBMR_BLEN_8			8
+#define DMA_SBMR_BLEN_4			4
 #define DMA_DSR_RPS_WIDTH		4
 #define DMA_DSR_TPS_WIDTH		4
 #define DMA_DSR_Q_WIDTH			(DMA_DSR_RPS_WIDTH + DMA_DSR_TPS_WIDTH)
@@ -959,6 +951,7 @@
 #define XP_DRIVER_INT_RO		0x0064
 #define XP_DRIVER_SCRATCH_0		0x0068
 #define XP_DRIVER_SCRATCH_1		0x006c
+#define XP_INT_REISSUE_EN		0x0074
 #define XP_INT_EN			0x0078
 #define XP_I2C_MUTEX			0x0080
 #define XP_MDIO_MUTEX			0x0084
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-desc.c b/drivers/net/ethernet/amd/xgbe/xgbe-desc.c
index 0a98c369df20..45d92304068e 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-desc.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-desc.c
@@ -176,8 +176,8 @@ static void xgbe_free_ring_resources(struct xgbe_prv_data *pdata)
 
 	DBGPR("-->xgbe_free_ring_resources\n");
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
+	for (i = 0; i < pdata->channel_count; i++) {
+		channel = pdata->channel[i];
 		xgbe_free_ring(pdata, channel->tx_ring);
 		xgbe_free_ring(pdata, channel->rx_ring);
 	}
@@ -185,34 +185,60 @@ static void xgbe_free_ring_resources(struct xgbe_prv_data *pdata)
 	DBGPR("<--xgbe_free_ring_resources\n");
 }
 
+static void *xgbe_alloc_node(size_t size, int node)
+{
+	void *mem;
+
+	mem = kzalloc_node(size, GFP_KERNEL, node);
+	if (!mem)
+		mem = kzalloc(size, GFP_KERNEL);
+
+	return mem;
+}
+
+static void *xgbe_dma_alloc_node(struct device *dev, size_t size,
+				 dma_addr_t *dma, int node)
+{
+	void *mem;
+	int cur_node = dev_to_node(dev);
+
+	set_dev_node(dev, node);
+	mem = dma_alloc_coherent(dev, size, dma, GFP_KERNEL);
+	set_dev_node(dev, cur_node);
+
+	if (!mem)
+		mem = dma_alloc_coherent(dev, size, dma, GFP_KERNEL);
+
+	return mem;
+}
+
 static int xgbe_init_ring(struct xgbe_prv_data *pdata,
 			  struct xgbe_ring *ring, unsigned int rdesc_count)
 {
-	DBGPR("-->xgbe_init_ring\n");
+	size_t size;
 
 	if (!ring)
 		return 0;
 
 	/* Descriptors */
+	size = rdesc_count * sizeof(struct xgbe_ring_desc);
+
 	ring->rdesc_count = rdesc_count;
-	ring->rdesc = dma_alloc_coherent(pdata->dev,
-					 (sizeof(struct xgbe_ring_desc) *
-					  rdesc_count), &ring->rdesc_dma,
-					 GFP_KERNEL);
+	ring->rdesc = xgbe_dma_alloc_node(pdata->dev, size, &ring->rdesc_dma,
+					  ring->node);
 	if (!ring->rdesc)
 		return -ENOMEM;
 
 	/* Descriptor information */
-	ring->rdata = kcalloc(rdesc_count, sizeof(struct xgbe_ring_data),
-			      GFP_KERNEL);
+	size = rdesc_count * sizeof(struct xgbe_ring_data);
+
+	ring->rdata = xgbe_alloc_node(size, ring->node);
 	if (!ring->rdata)
 		return -ENOMEM;
 
 	netif_dbg(pdata, drv, pdata->netdev,
-		  "rdesc=%p, rdesc_dma=%pad, rdata=%p\n",
-		  ring->rdesc, &ring->rdesc_dma, ring->rdata);
-
-	DBGPR("<--xgbe_init_ring\n");
+		  "rdesc=%p, rdesc_dma=%pad, rdata=%p, node=%d\n",
+		  ring->rdesc, &ring->rdesc_dma, ring->rdata, ring->node);
 
 	return 0;
 }
@@ -223,10 +249,8 @@ static int xgbe_alloc_ring_resources(struct xgbe_prv_data *pdata)
 	unsigned int i;
 	int ret;
 
-	DBGPR("-->xgbe_alloc_ring_resources\n");
-
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
+	for (i = 0; i < pdata->channel_count; i++) {
+		channel = pdata->channel[i];
 		netif_dbg(pdata, drv, pdata->netdev, "%s - Tx ring:\n",
 			  channel->name);
 
@@ -250,8 +274,6 @@ static int xgbe_alloc_ring_resources(struct xgbe_prv_data *pdata)
 		}
 	}
 
-	DBGPR("<--xgbe_alloc_ring_resources\n");
-
 	return 0;
 
 err_ring:
@@ -261,21 +283,33 @@ err_ring:
 }
 
 static int xgbe_alloc_pages(struct xgbe_prv_data *pdata,
-			    struct xgbe_page_alloc *pa, gfp_t gfp, int order)
+			    struct xgbe_page_alloc *pa, int alloc_order,
+			    int node)
 {
 	struct page *pages = NULL;
 	dma_addr_t pages_dma;
-	int ret;
+	gfp_t gfp;
+	int order, ret;
+
+again:
+	order = alloc_order;
 
 	/* Try to obtain pages, decreasing order if necessary */
-	gfp |= __GFP_COLD | __GFP_COMP | __GFP_NOWARN;
+	gfp = GFP_ATOMIC | __GFP_COLD | __GFP_COMP | __GFP_NOWARN;
 	while (order >= 0) {
-		pages = alloc_pages(gfp, order);
+		pages = alloc_pages_node(node, gfp, order);
 		if (pages)
 			break;
 
 		order--;
 	}
+
+	/* If we couldn't get local pages, try getting from anywhere */
+	if (!pages && (node != NUMA_NO_NODE)) {
+		node = NUMA_NO_NODE;
+		goto again;
+	}
+
 	if (!pages)
 		return -ENOMEM;
 
@@ -327,14 +361,14 @@ static int xgbe_map_rx_buffer(struct xgbe_prv_data *pdata,
 	int ret;
 
 	if (!ring->rx_hdr_pa.pages) {
-		ret = xgbe_alloc_pages(pdata, &ring->rx_hdr_pa, GFP_ATOMIC, 0);
+		ret = xgbe_alloc_pages(pdata, &ring->rx_hdr_pa, 0, ring->node);
 		if (ret)
 			return ret;
 	}
 
 	if (!ring->rx_buf_pa.pages) {
-		ret = xgbe_alloc_pages(pdata, &ring->rx_buf_pa, GFP_ATOMIC,
-				       PAGE_ALLOC_COSTLY_ORDER);
+		ret = xgbe_alloc_pages(pdata, &ring->rx_buf_pa,
+				       PAGE_ALLOC_COSTLY_ORDER, ring->node);
 		if (ret)
 			return ret;
 	}
@@ -362,8 +396,8 @@ static void xgbe_wrapper_tx_descriptor_init(struct xgbe_prv_data *pdata)
 
 	DBGPR("-->xgbe_wrapper_tx_descriptor_init\n");
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
+	for (i = 0; i < pdata->channel_count; i++) {
+		channel = pdata->channel[i];
 		ring = channel->tx_ring;
 		if (!ring)
 			break;
@@ -403,8 +437,8 @@ static void xgbe_wrapper_rx_descriptor_init(struct xgbe_prv_data *pdata)
 
 	DBGPR("-->xgbe_wrapper_rx_descriptor_init\n");
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
+	for (i = 0; i < pdata->channel_count; i++) {
+		channel = pdata->channel[i];
 		ring = channel->rx_ring;
 		if (!ring)
 			break;
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-dev.c b/drivers/net/ethernet/amd/xgbe/xgbe-dev.c
index 24a687ce4388..06f953e1e9b2 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-dev.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-dev.c
@@ -174,58 +174,30 @@ static unsigned int xgbe_riwt_to_usec(struct xgbe_prv_data *pdata,
 	return ret;
 }
 
-static int xgbe_config_pblx8(struct xgbe_prv_data *pdata)
+static int xgbe_config_pbl_val(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
+	unsigned int pblx8, pbl;
 	unsigned int i;
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++)
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_CR, PBLX8,
-				       pdata->pblx8);
-
-	return 0;
-}
-
-static int xgbe_get_tx_pbl_val(struct xgbe_prv_data *pdata)
-{
-	return XGMAC_DMA_IOREAD_BITS(pdata->channel, DMA_CH_TCR, PBL);
-}
-
-static int xgbe_config_tx_pbl_val(struct xgbe_prv_data *pdata)
-{
-	struct xgbe_channel *channel;
-	unsigned int i;
-
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->tx_ring)
-			break;
+	pblx8 = DMA_PBL_X8_DISABLE;
+	pbl = pdata->pbl;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_TCR, PBL,
-				       pdata->tx_pbl);
+	if (pdata->pbl > 32) {
+		pblx8 = DMA_PBL_X8_ENABLE;
+		pbl >>= 3;
 	}
 
-	return 0;
-}
-
-static int xgbe_get_rx_pbl_val(struct xgbe_prv_data *pdata)
-{
-	return XGMAC_DMA_IOREAD_BITS(pdata->channel, DMA_CH_RCR, PBL);
-}
-
-static int xgbe_config_rx_pbl_val(struct xgbe_prv_data *pdata)
-{
-	struct xgbe_channel *channel;
-	unsigned int i;
+	for (i = 0; i < pdata->channel_count; i++) {
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_CR, PBLX8,
+				       pblx8);
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->rx_ring)
-			break;
+		if (pdata->channel[i]->tx_ring)
+			XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_TCR,
+					       PBL, pbl);
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_RCR, PBL,
-				       pdata->rx_pbl);
+		if (pdata->channel[i]->rx_ring)
+			XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_RCR,
+					       PBL, pbl);
 	}
 
 	return 0;
@@ -233,15 +205,13 @@ static int xgbe_config_rx_pbl_val(struct xgbe_prv_data *pdata)
 
 static int xgbe_config_osp_mode(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->tx_ring)
+	for (i = 0; i < pdata->channel_count; i++) {
+		if (!pdata->channel[i]->tx_ring)
 			break;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_TCR, OSP,
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_TCR, OSP,
 				       pdata->tx_osp_mode);
 	}
 
@@ -292,15 +262,13 @@ static int xgbe_config_tx_threshold(struct xgbe_prv_data *pdata,
 
 static int xgbe_config_rx_coalesce(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->rx_ring)
+	for (i = 0; i < pdata->channel_count; i++) {
+		if (!pdata->channel[i]->rx_ring)
 			break;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_RIWT, RWT,
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_RIWT, RWT,
 				       pdata->rx_riwt);
 	}
 
@@ -314,44 +282,38 @@ static int xgbe_config_tx_coalesce(struct xgbe_prv_data *pdata)
 
 static void xgbe_config_rx_buffer_size(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->rx_ring)
+	for (i = 0; i < pdata->channel_count; i++) {
+		if (!pdata->channel[i]->rx_ring)
 			break;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_RCR, RBSZ,
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_RCR, RBSZ,
 				       pdata->rx_buf_size);
 	}
 }
 
 static void xgbe_config_tso_mode(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->tx_ring)
+	for (i = 0; i < pdata->channel_count; i++) {
+		if (!pdata->channel[i]->tx_ring)
 			break;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_TCR, TSE, 1);
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_TCR, TSE, 1);
 	}
 }
 
 static void xgbe_config_sph_mode(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->rx_ring)
+	for (i = 0; i < pdata->channel_count; i++) {
+		if (!pdata->channel[i]->rx_ring)
 			break;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_CR, SPH, 1);
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_CR, SPH, 1);
 	}
 
 	XGMAC_IOWRITE_BITS(pdata, MAC_RCR, HDSMS, XGBE_SPH_HDSMS_SIZE);
@@ -651,8 +613,9 @@ static void xgbe_enable_dma_interrupts(struct xgbe_prv_data *pdata)
 		XGMAC_IOWRITE_BITS(pdata, DMA_MR, INTM,
 				   pdata->channel_irq_mode);
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
+	for (i = 0; i < pdata->channel_count; i++) {
+		channel = pdata->channel[i];
+
 		/* Clear all the interrupts which are set */
 		dma_ch_isr = XGMAC_DMA_IOREAD(channel, DMA_CH_SR);
 		XGMAC_DMA_IOWRITE(channel, DMA_CH_SR, dma_ch_isr);
@@ -1497,26 +1460,37 @@ static void xgbe_rx_desc_init(struct xgbe_channel *channel)
 static void xgbe_update_tstamp_addend(struct xgbe_prv_data *pdata,
 				      unsigned int addend)
 {
+	unsigned int count = 10000;
+
 	/* Set the addend register value and tell the device */
 	XGMAC_IOWRITE(pdata, MAC_TSAR, addend);
 	XGMAC_IOWRITE_BITS(pdata, MAC_TSCR, TSADDREG, 1);
 
 	/* Wait for addend update to complete */
-	while (XGMAC_IOREAD_BITS(pdata, MAC_TSCR, TSADDREG))
+	while (--count && XGMAC_IOREAD_BITS(pdata, MAC_TSCR, TSADDREG))
 		udelay(5);
+
+	if (!count)
+		netdev_err(pdata->netdev,
+			   "timed out updating timestamp addend register\n");
 }
 
 static void xgbe_set_tstamp_time(struct xgbe_prv_data *pdata, unsigned int sec,
 				 unsigned int nsec)
 {
+	unsigned int count = 10000;
+
 	/* Set the time values and tell the device */
 	XGMAC_IOWRITE(pdata, MAC_STSUR, sec);
 	XGMAC_IOWRITE(pdata, MAC_STNUR, nsec);
 	XGMAC_IOWRITE_BITS(pdata, MAC_TSCR, TSINIT, 1);
 
 	/* Wait for time update to complete */
-	while (XGMAC_IOREAD_BITS(pdata, MAC_TSCR, TSINIT))
+	while (--count && XGMAC_IOREAD_BITS(pdata, MAC_TSCR, TSINIT))
 		udelay(5);
+
+	if (!count)
+		netdev_err(pdata->netdev, "timed out initializing timestamp\n");
 }
 
 static u64 xgbe_get_tstamp_time(struct xgbe_prv_data *pdata)
@@ -2140,37 +2114,38 @@ static int xgbe_flush_tx_queues(struct xgbe_prv_data *pdata)
 
 static void xgbe_config_dma_bus(struct xgbe_prv_data *pdata)
 {
+	unsigned int sbmr;
+
+	sbmr = XGMAC_IOREAD(pdata, DMA_SBMR);
+
 	/* Set enhanced addressing mode */
-	XGMAC_IOWRITE_BITS(pdata, DMA_SBMR, EAME, 1);
+	XGMAC_SET_BITS(sbmr, DMA_SBMR, EAME, 1);
 
 	/* Set the System Bus mode */
-	XGMAC_IOWRITE_BITS(pdata, DMA_SBMR, UNDEF, 1);
-	XGMAC_IOWRITE_BITS(pdata, DMA_SBMR, BLEN_256, 1);
+	XGMAC_SET_BITS(sbmr, DMA_SBMR, UNDEF, 1);
+	XGMAC_SET_BITS(sbmr, DMA_SBMR, BLEN, pdata->blen >> 2);
+	XGMAC_SET_BITS(sbmr, DMA_SBMR, AAL, pdata->aal);
+	XGMAC_SET_BITS(sbmr, DMA_SBMR, RD_OSR_LMT, pdata->rd_osr_limit - 1);
+	XGMAC_SET_BITS(sbmr, DMA_SBMR, WR_OSR_LMT, pdata->wr_osr_limit - 1);
+
+	XGMAC_IOWRITE(pdata, DMA_SBMR, sbmr);
+
+	/* Set descriptor fetching threshold */
+	if (pdata->vdata->tx_desc_prefetch)
+		XGMAC_IOWRITE_BITS(pdata, DMA_TXEDMACR, TDPS,
+				   pdata->vdata->tx_desc_prefetch);
+
+	if (pdata->vdata->rx_desc_prefetch)
+		XGMAC_IOWRITE_BITS(pdata, DMA_RXEDMACR, RDPS,
+				   pdata->vdata->rx_desc_prefetch);
 }
 
 static void xgbe_config_dma_cache(struct xgbe_prv_data *pdata)
 {
-	unsigned int arcache, awcache;
-
-	arcache = 0;
-	XGMAC_SET_BITS(arcache, DMA_AXIARCR, DRC, pdata->arcache);
-	XGMAC_SET_BITS(arcache, DMA_AXIARCR, DRD, pdata->axdomain);
-	XGMAC_SET_BITS(arcache, DMA_AXIARCR, TEC, pdata->arcache);
-	XGMAC_SET_BITS(arcache, DMA_AXIARCR, TED, pdata->axdomain);
-	XGMAC_SET_BITS(arcache, DMA_AXIARCR, THC, pdata->arcache);
-	XGMAC_SET_BITS(arcache, DMA_AXIARCR, THD, pdata->axdomain);
-	XGMAC_IOWRITE(pdata, DMA_AXIARCR, arcache);
-
-	awcache = 0;
-	XGMAC_SET_BITS(awcache, DMA_AXIAWCR, DWC, pdata->awcache);
-	XGMAC_SET_BITS(awcache, DMA_AXIAWCR, DWD, pdata->axdomain);
-	XGMAC_SET_BITS(awcache, DMA_AXIAWCR, RPC, pdata->awcache);
-	XGMAC_SET_BITS(awcache, DMA_AXIAWCR, RPD, pdata->axdomain);
-	XGMAC_SET_BITS(awcache, DMA_AXIAWCR, RHC, pdata->awcache);
-	XGMAC_SET_BITS(awcache, DMA_AXIAWCR, RHD, pdata->axdomain);
-	XGMAC_SET_BITS(awcache, DMA_AXIAWCR, TDC, pdata->awcache);
-	XGMAC_SET_BITS(awcache, DMA_AXIAWCR, TDD, pdata->axdomain);
-	XGMAC_IOWRITE(pdata, DMA_AXIAWCR, awcache);
+	XGMAC_IOWRITE(pdata, DMA_AXIARCR, pdata->arcr);
+	XGMAC_IOWRITE(pdata, DMA_AXIAWCR, pdata->awcr);
+	if (pdata->awarcr)
+		XGMAC_IOWRITE(pdata, DMA_AXIAWARCR, pdata->awarcr);
 }
 
 static void xgbe_config_mtl_mode(struct xgbe_prv_data *pdata)
@@ -3202,16 +3177,14 @@ static void xgbe_prepare_tx_stop(struct xgbe_prv_data *pdata,
 
 static void xgbe_enable_tx(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
 	/* Enable each Tx DMA channel */
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->tx_ring)
+	for (i = 0; i < pdata->channel_count; i++) {
+		if (!pdata->channel[i]->tx_ring)
 			break;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_TCR, ST, 1);
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_TCR, ST, 1);
 	}
 
 	/* Enable each Tx queue */
@@ -3225,7 +3198,6 @@ static void xgbe_enable_tx(struct xgbe_prv_data *pdata)
 
 static void xgbe_disable_tx(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
 	/* Prepare for Tx DMA channel stop */
@@ -3240,12 +3212,11 @@ static void xgbe_disable_tx(struct xgbe_prv_data *pdata)
 		XGMAC_MTL_IOWRITE_BITS(pdata, i, MTL_Q_TQOMR, TXQEN, 0);
 
 	/* Disable each Tx DMA channel */
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->tx_ring)
+	for (i = 0; i < pdata->channel_count; i++) {
+		if (!pdata->channel[i]->tx_ring)
 			break;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_TCR, ST, 0);
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_TCR, ST, 0);
 	}
 }
 
@@ -3277,16 +3248,14 @@ static void xgbe_prepare_rx_stop(struct xgbe_prv_data *pdata,
 
 static void xgbe_enable_rx(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int reg_val, i;
 
 	/* Enable each Rx DMA channel */
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->rx_ring)
+	for (i = 0; i < pdata->channel_count; i++) {
+		if (!pdata->channel[i]->rx_ring)
 			break;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_RCR, SR, 1);
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_RCR, SR, 1);
 	}
 
 	/* Enable each Rx queue */
@@ -3304,7 +3273,6 @@ static void xgbe_enable_rx(struct xgbe_prv_data *pdata)
 
 static void xgbe_disable_rx(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
 	/* Disable MAC Rx */
@@ -3321,27 +3289,24 @@ static void xgbe_disable_rx(struct xgbe_prv_data *pdata)
 	XGMAC_IOWRITE(pdata, MAC_RQC0R, 0);
 
 	/* Disable each Rx DMA channel */
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->rx_ring)
+	for (i = 0; i < pdata->channel_count; i++) {
+		if (!pdata->channel[i]->rx_ring)
 			break;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_RCR, SR, 0);
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_RCR, SR, 0);
 	}
 }
 
 static void xgbe_powerup_tx(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
 	/* Enable each Tx DMA channel */
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->tx_ring)
+	for (i = 0; i < pdata->channel_count; i++) {
+		if (!pdata->channel[i]->tx_ring)
 			break;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_TCR, ST, 1);
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_TCR, ST, 1);
 	}
 
 	/* Enable MAC Tx */
@@ -3350,7 +3315,6 @@ static void xgbe_powerup_tx(struct xgbe_prv_data *pdata)
 
 static void xgbe_powerdown_tx(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
 	/* Prepare for Tx DMA channel stop */
@@ -3361,42 +3325,37 @@ static void xgbe_powerdown_tx(struct xgbe_prv_data *pdata)
 	XGMAC_IOWRITE_BITS(pdata, MAC_TCR, TE, 0);
 
 	/* Disable each Tx DMA channel */
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->tx_ring)
+	for (i = 0; i < pdata->channel_count; i++) {
+		if (!pdata->channel[i]->tx_ring)
 			break;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_TCR, ST, 0);
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_TCR, ST, 0);
 	}
 }
 
 static void xgbe_powerup_rx(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
 	/* Enable each Rx DMA channel */
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->rx_ring)
+	for (i = 0; i < pdata->channel_count; i++) {
+		if (!pdata->channel[i]->rx_ring)
 			break;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_RCR, SR, 1);
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_RCR, SR, 1);
 	}
 }
 
 static void xgbe_powerdown_rx(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
 	/* Disable each Rx DMA channel */
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		if (!channel->rx_ring)
+	for (i = 0; i < pdata->channel_count; i++) {
+		if (!pdata->channel[i]->rx_ring)
 			break;
 
-		XGMAC_DMA_IOWRITE_BITS(channel, DMA_CH_RCR, SR, 0);
+		XGMAC_DMA_IOWRITE_BITS(pdata->channel[i], DMA_CH_RCR, SR, 0);
 	}
 }
 
@@ -3420,9 +3379,7 @@ static int xgbe_init(struct xgbe_prv_data *pdata)
 	xgbe_config_dma_bus(pdata);
 	xgbe_config_dma_cache(pdata);
 	xgbe_config_osp_mode(pdata);
-	xgbe_config_pblx8(pdata);
-	xgbe_config_tx_pbl_val(pdata);
-	xgbe_config_rx_pbl_val(pdata);
+	xgbe_config_pbl_val(pdata);
 	xgbe_config_rx_coalesce(pdata);
 	xgbe_config_tx_coalesce(pdata);
 	xgbe_config_rx_buffer_size(pdata);
@@ -3550,13 +3507,6 @@ void xgbe_init_function_ptrs_dev(struct xgbe_hw_if *hw_if)
 	/* For TX DMA Operating on Second Frame config */
 	hw_if->config_osp_mode = xgbe_config_osp_mode;
 
-	/* For RX and TX PBL config */
-	hw_if->config_rx_pbl_val = xgbe_config_rx_pbl_val;
-	hw_if->get_rx_pbl_val = xgbe_get_rx_pbl_val;
-	hw_if->config_tx_pbl_val = xgbe_config_tx_pbl_val;
-	hw_if->get_tx_pbl_val = xgbe_get_tx_pbl_val;
-	hw_if->config_pblx8 = xgbe_config_pblx8;
-
 	/* For MMC statistics support */
 	hw_if->tx_mmc_int = xgbe_tx_mmc_int;
 	hw_if->rx_mmc_int = xgbe_rx_mmc_int;
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
index c772420fa41c..ecef3ee87b17 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
@@ -158,81 +158,106 @@ static int xgbe_one_poll(struct napi_struct *, int);
 static int xgbe_all_poll(struct napi_struct *, int);
 static void xgbe_stop(struct xgbe_prv_data *);
 
+static void *xgbe_alloc_node(size_t size, int node)
+{
+	void *mem;
+
+	mem = kzalloc_node(size, GFP_KERNEL, node);
+	if (!mem)
+		mem = kzalloc(size, GFP_KERNEL);
+
+	return mem;
+}
+
+static void xgbe_free_channels(struct xgbe_prv_data *pdata)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(pdata->channel); i++) {
+		if (!pdata->channel[i])
+			continue;
+
+		kfree(pdata->channel[i]->rx_ring);
+		kfree(pdata->channel[i]->tx_ring);
+		kfree(pdata->channel[i]);
+
+		pdata->channel[i] = NULL;
+	}
+
+	pdata->channel_count = 0;
+}
+
 static int xgbe_alloc_channels(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel_mem, *channel;
-	struct xgbe_ring *tx_ring, *rx_ring;
+	struct xgbe_channel *channel;
+	struct xgbe_ring *ring;
 	unsigned int count, i;
-	int ret = -ENOMEM;
+	unsigned int cpu;
+	int node;
 
 	count = max_t(unsigned int, pdata->tx_ring_count, pdata->rx_ring_count);
+	for (i = 0; i < count; i++) {
+		/* Attempt to use a CPU on the node the device is on */
+		cpu = cpumask_local_spread(i, dev_to_node(pdata->dev));
 
-	channel_mem = kcalloc(count, sizeof(struct xgbe_channel), GFP_KERNEL);
-	if (!channel_mem)
-		goto err_channel;
-
-	tx_ring = kcalloc(pdata->tx_ring_count, sizeof(struct xgbe_ring),
-			  GFP_KERNEL);
-	if (!tx_ring)
-		goto err_tx_ring;
+		/* Set the allocation node based on the returned CPU */
+		node = cpu_to_node(cpu);
 
-	rx_ring = kcalloc(pdata->rx_ring_count, sizeof(struct xgbe_ring),
-			  GFP_KERNEL);
-	if (!rx_ring)
-		goto err_rx_ring;
+		channel = xgbe_alloc_node(sizeof(*channel), node);
+		if (!channel)
+			goto err_mem;
+		pdata->channel[i] = channel;
 
-	for (i = 0, channel = channel_mem; i < count; i++, channel++) {
 		snprintf(channel->name, sizeof(channel->name), "channel-%u", i);
 		channel->pdata = pdata;
 		channel->queue_index = i;
 		channel->dma_regs = pdata->xgmac_regs + DMA_CH_BASE +
 				    (DMA_CH_INC * i);
+		channel->node = node;
+		cpumask_set_cpu(cpu, &channel->affinity_mask);
 
 		if (pdata->per_channel_irq)
 			channel->dma_irq = pdata->channel_irq[i];
 
 		if (i < pdata->tx_ring_count) {
-			spin_lock_init(&tx_ring->lock);
-			channel->tx_ring = tx_ring++;
+			ring = xgbe_alloc_node(sizeof(*ring), node);
+			if (!ring)
+				goto err_mem;
+
+			spin_lock_init(&ring->lock);
+			ring->node = node;
+
+			channel->tx_ring = ring;
 		}
 
 		if (i < pdata->rx_ring_count) {
-			spin_lock_init(&rx_ring->lock);
-			channel->rx_ring = rx_ring++;
+			ring = xgbe_alloc_node(sizeof(*ring), node);
+			if (!ring)
+				goto err_mem;
+
+			spin_lock_init(&ring->lock);
+			ring->node = node;
+
+			channel->rx_ring = ring;
 		}
 
 		netif_dbg(pdata, drv, pdata->netdev,
+			  "%s: cpu=%u, node=%d\n", channel->name, cpu, node);
+
+		netif_dbg(pdata, drv, pdata->netdev,
 			  "%s: dma_regs=%p, dma_irq=%d, tx=%p, rx=%p\n",
 			  channel->name, channel->dma_regs, channel->dma_irq,
 			  channel->tx_ring, channel->rx_ring);
 	}
 
-	pdata->channel = channel_mem;
 	pdata->channel_count = count;
 
 	return 0;
 
-err_rx_ring:
-	kfree(tx_ring);
-
-err_tx_ring:
-	kfree(channel_mem);
-
-err_channel:
-	return ret;
-}
-
-static void xgbe_free_channels(struct xgbe_prv_data *pdata)
-{
-	if (!pdata->channel)
-		return;
-
-	kfree(pdata->channel->rx_ring);
-	kfree(pdata->channel->tx_ring);
-	kfree(pdata->channel);
+err_mem:
+	xgbe_free_channels(pdata);
 
-	pdata->channel = NULL;
-	pdata->channel_count = 0;
+	return -ENOMEM;
 }
 
 static inline unsigned int xgbe_tx_avail_desc(struct xgbe_ring *ring)
@@ -301,12 +326,10 @@ static void xgbe_enable_rx_tx_int(struct xgbe_prv_data *pdata,
 
 static void xgbe_enable_rx_tx_ints(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++)
-		xgbe_enable_rx_tx_int(pdata, channel);
+	for (i = 0; i < pdata->channel_count; i++)
+		xgbe_enable_rx_tx_int(pdata, pdata->channel[i]);
 }
 
 static void xgbe_disable_rx_tx_int(struct xgbe_prv_data *pdata,
@@ -329,12 +352,10 @@ static void xgbe_disable_rx_tx_int(struct xgbe_prv_data *pdata,
 
 static void xgbe_disable_rx_tx_ints(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_channel *channel;
 	unsigned int i;
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++)
-		xgbe_disable_rx_tx_int(pdata, channel);
+	for (i = 0; i < pdata->channel_count; i++)
+		xgbe_disable_rx_tx_int(pdata, pdata->channel[i]);
 }
 
 static bool xgbe_ecc_sec(struct xgbe_prv_data *pdata, unsigned long *period,
@@ -382,9 +403,9 @@ static bool xgbe_ecc_ded(struct xgbe_prv_data *pdata, unsigned long *period,
 	return false;
 }
 
-static irqreturn_t xgbe_ecc_isr(int irq, void *data)
+static void xgbe_ecc_isr_task(unsigned long data)
 {
-	struct xgbe_prv_data *pdata = data;
+	struct xgbe_prv_data *pdata = (struct xgbe_prv_data *)data;
 	unsigned int ecc_isr;
 	bool stop = false;
 
@@ -435,12 +456,26 @@ out:
 	/* Clear all ECC interrupts */
 	XP_IOWRITE(pdata, XP_ECC_ISR, ecc_isr);
 
-	return IRQ_HANDLED;
+	/* Reissue interrupt if status is not clear */
+	if (pdata->vdata->irq_reissue_support)
+		XP_IOWRITE(pdata, XP_INT_REISSUE_EN, 1 << 1);
 }
 
-static irqreturn_t xgbe_isr(int irq, void *data)
+static irqreturn_t xgbe_ecc_isr(int irq, void *data)
 {
 	struct xgbe_prv_data *pdata = data;
+
+	if (pdata->isr_as_tasklet)
+		tasklet_schedule(&pdata->tasklet_ecc);
+	else
+		xgbe_ecc_isr_task((unsigned long)pdata);
+
+	return IRQ_HANDLED;
+}
+
+static void xgbe_isr_task(unsigned long data)
+{
+	struct xgbe_prv_data *pdata = (struct xgbe_prv_data *)data;
 	struct xgbe_hw_if *hw_if = &pdata->hw_if;
 	struct xgbe_channel *channel;
 	unsigned int dma_isr, dma_ch_isr;
@@ -461,7 +496,7 @@ static irqreturn_t xgbe_isr(int irq, void *data)
 		if (!(dma_isr & (1 << i)))
 			continue;
 
-		channel = pdata->channel + i;
+		channel = pdata->channel[i];
 
 		dma_ch_isr = XGMAC_DMA_IOREAD(channel, DMA_CH_SR);
 		netif_dbg(pdata, intr, pdata->netdev, "DMA_CH%u_ISR=%#010x\n",
@@ -543,15 +578,36 @@ static irqreturn_t xgbe_isr(int irq, void *data)
 isr_done:
 	/* If there is not a separate AN irq, handle it here */
 	if (pdata->dev_irq == pdata->an_irq)
-		pdata->phy_if.an_isr(irq, pdata);
+		pdata->phy_if.an_isr(pdata);
 
 	/* If there is not a separate ECC irq, handle it here */
 	if (pdata->vdata->ecc_support && (pdata->dev_irq == pdata->ecc_irq))
-		xgbe_ecc_isr(irq, pdata);
+		xgbe_ecc_isr_task((unsigned long)pdata);
 
 	/* If there is not a separate I2C irq, handle it here */
 	if (pdata->vdata->i2c_support && (pdata->dev_irq == pdata->i2c_irq))
-		pdata->i2c_if.i2c_isr(irq, pdata);
+		pdata->i2c_if.i2c_isr(pdata);
+
+	/* Reissue interrupt if status is not clear */
+	if (pdata->vdata->irq_reissue_support) {
+		unsigned int reissue_mask;
+
+		reissue_mask = 1 << 0;
+		if (!pdata->per_channel_irq)
+			reissue_mask |= 0xffff < 4;
+
+		XP_IOWRITE(pdata, XP_INT_REISSUE_EN, reissue_mask);
+	}
+}
+
+static irqreturn_t xgbe_isr(int irq, void *data)
+{
+	struct xgbe_prv_data *pdata = data;
+
+	if (pdata->isr_as_tasklet)
+		tasklet_schedule(&pdata->tasklet_dev);
+	else
+		xgbe_isr_task((unsigned long)pdata);
 
 	return IRQ_HANDLED;
 }
@@ -640,8 +696,8 @@ static void xgbe_init_timers(struct xgbe_prv_data *pdata)
 	setup_timer(&pdata->service_timer, xgbe_service_timer,
 		    (unsigned long)pdata);
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
+	for (i = 0; i < pdata->channel_count; i++) {
+		channel = pdata->channel[i];
 		if (!channel->tx_ring)
 			break;
 
@@ -662,8 +718,8 @@ static void xgbe_stop_timers(struct xgbe_prv_data *pdata)
 
 	del_timer_sync(&pdata->service_timer);
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
+	for (i = 0; i < pdata->channel_count; i++) {
+		channel = pdata->channel[i];
 		if (!channel->tx_ring)
 			break;
 
@@ -781,8 +837,8 @@ static void xgbe_napi_enable(struct xgbe_prv_data *pdata, unsigned int add)
 	unsigned int i;
 
 	if (pdata->per_channel_irq) {
-		channel = pdata->channel;
-		for (i = 0; i < pdata->channel_count; i++, channel++) {
+		for (i = 0; i < pdata->channel_count; i++) {
+			channel = pdata->channel[i];
 			if (add)
 				netif_napi_add(pdata->netdev, &channel->napi,
 					       xgbe_one_poll, NAPI_POLL_WEIGHT);
@@ -804,8 +860,8 @@ static void xgbe_napi_disable(struct xgbe_prv_data *pdata, unsigned int del)
 	unsigned int i;
 
 	if (pdata->per_channel_irq) {
-		channel = pdata->channel;
-		for (i = 0; i < pdata->channel_count; i++, channel++) {
+		for (i = 0; i < pdata->channel_count; i++) {
+			channel = pdata->channel[i];
 			napi_disable(&channel->napi);
 
 			if (del)
@@ -826,6 +882,10 @@ static int xgbe_request_irqs(struct xgbe_prv_data *pdata)
 	unsigned int i;
 	int ret;
 
+	tasklet_init(&pdata->tasklet_dev, xgbe_isr_task, (unsigned long)pdata);
+	tasklet_init(&pdata->tasklet_ecc, xgbe_ecc_isr_task,
+		     (unsigned long)pdata);
+
 	ret = devm_request_irq(pdata->dev, pdata->dev_irq, xgbe_isr, 0,
 			       netdev->name, pdata);
 	if (ret) {
@@ -847,8 +907,8 @@ static int xgbe_request_irqs(struct xgbe_prv_data *pdata)
 	if (!pdata->per_channel_irq)
 		return 0;
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
+	for (i = 0; i < pdata->channel_count; i++) {
+		channel = pdata->channel[i];
 		snprintf(channel->dma_irq_name,
 			 sizeof(channel->dma_irq_name) - 1,
 			 "%s-TxRx-%u", netdev_name(netdev),
@@ -862,14 +922,21 @@ static int xgbe_request_irqs(struct xgbe_prv_data *pdata)
 				     channel->dma_irq);
 			goto err_dma_irq;
 		}
+
+		irq_set_affinity_hint(channel->dma_irq,
+				      &channel->affinity_mask);
 	}
 
 	return 0;
 
 err_dma_irq:
 	/* Using an unsigned int, 'i' will go to UINT_MAX and exit */
-	for (i--, channel--; i < pdata->channel_count; i--, channel--)
+	for (i--; i < pdata->channel_count; i--) {
+		channel = pdata->channel[i];
+
+		irq_set_affinity_hint(channel->dma_irq, NULL);
 		devm_free_irq(pdata->dev, channel->dma_irq, channel);
+	}
 
 	if (pdata->vdata->ecc_support && (pdata->dev_irq != pdata->ecc_irq))
 		devm_free_irq(pdata->dev, pdata->ecc_irq, pdata);
@@ -893,9 +960,12 @@ static void xgbe_free_irqs(struct xgbe_prv_data *pdata)
 	if (!pdata->per_channel_irq)
 		return;
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++)
+	for (i = 0; i < pdata->channel_count; i++) {
+		channel = pdata->channel[i];
+
+		irq_set_affinity_hint(channel->dma_irq, NULL);
 		devm_free_irq(pdata->dev, channel->dma_irq, channel);
+	}
 }
 
 void xgbe_init_tx_coalesce(struct xgbe_prv_data *pdata)
@@ -930,16 +1000,14 @@ void xgbe_init_rx_coalesce(struct xgbe_prv_data *pdata)
 static void xgbe_free_tx_data(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_desc_if *desc_if = &pdata->desc_if;
-	struct xgbe_channel *channel;
 	struct xgbe_ring *ring;
 	struct xgbe_ring_data *rdata;
 	unsigned int i, j;
 
 	DBGPR("-->xgbe_free_tx_data\n");
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		ring = channel->tx_ring;
+	for (i = 0; i < pdata->channel_count; i++) {
+		ring = pdata->channel[i]->tx_ring;
 		if (!ring)
 			break;
 
@@ -955,16 +1023,14 @@ static void xgbe_free_tx_data(struct xgbe_prv_data *pdata)
 static void xgbe_free_rx_data(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_desc_if *desc_if = &pdata->desc_if;
-	struct xgbe_channel *channel;
 	struct xgbe_ring *ring;
 	struct xgbe_ring_data *rdata;
 	unsigned int i, j;
 
 	DBGPR("-->xgbe_free_rx_data\n");
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		ring = channel->rx_ring;
+	for (i = 0; i < pdata->channel_count; i++) {
+		ring = pdata->channel[i]->rx_ring;
 		if (!ring)
 			break;
 
@@ -1140,8 +1206,8 @@ static void xgbe_stop(struct xgbe_prv_data *pdata)
 
 	hw_if->exit(pdata);
 
-	channel = pdata->channel;
-	for (i = 0; i < pdata->channel_count; i++, channel++) {
+	for (i = 0; i < pdata->channel_count; i++) {
+		channel = pdata->channel[i];
 		if (!channel->tx_ring)
 			continue;
 
@@ -1212,6 +1278,10 @@ static void xgbe_tx_tstamp(struct work_struct *work)
 	u64 nsec;
 	unsigned long flags;
 
+	spin_lock_irqsave(&pdata->tstamp_lock, flags);
+	if (!pdata->tx_tstamp_skb)
+		goto unlock;
+
 	if (pdata->tx_tstamp) {
 		nsec = timecounter_cyc2time(&pdata->tstamp_tc,
 					    pdata->tx_tstamp);
@@ -1223,8 +1293,9 @@ static void xgbe_tx_tstamp(struct work_struct *work)
 
 	dev_kfree_skb_any(pdata->tx_tstamp_skb);
 
-	spin_lock_irqsave(&pdata->tstamp_lock, flags);
 	pdata->tx_tstamp_skb = NULL;
+
+unlock:
 	spin_unlock_irqrestore(&pdata->tstamp_lock, flags);
 }
 
@@ -1268,6 +1339,7 @@ static int xgbe_set_hwtstamp_settings(struct xgbe_prv_data *pdata,
 	case HWTSTAMP_FILTER_NONE:
 		break;
 
+	case HWTSTAMP_FILTER_NTP_ALL:
 	case HWTSTAMP_FILTER_ALL:
 		XGMAC_SET_BITS(mac_tscr, MAC_TSCR, TSENALL, 1);
 		XGMAC_SET_BITS(mac_tscr, MAC_TSCR, TSENA, 1);
@@ -1390,8 +1462,7 @@ static void xgbe_prep_tx_tstamp(struct xgbe_prv_data *pdata,
 		spin_unlock_irqrestore(&pdata->tstamp_lock, flags);
 	}
 
-	if (!XGMAC_GET_BITS(packet->attributes, TX_PACKET_ATTRIBUTES, PTP))
-		skb_tx_timestamp(skb);
+	skb_tx_timestamp(skb);
 }
 
 static void xgbe_prep_vlan(struct sk_buff *skb, struct xgbe_packet_data *packet)
@@ -1623,7 +1694,7 @@ static int xgbe_xmit(struct sk_buff *skb, struct net_device *netdev)
 
 	DBGPR("-->xgbe_xmit: skb->len = %d\n", skb->len);
 
-	channel = pdata->channel + skb->queue_mapping;
+	channel = pdata->channel[skb->queue_mapping];
 	txq = netdev_get_tx_queue(netdev, channel->queue_index);
 	ring = channel->tx_ring;
 	packet = &ring->packet_data;
@@ -1833,9 +1904,10 @@ static void xgbe_poll_controller(struct net_device *netdev)
 	DBGPR("-->xgbe_poll_controller\n");
 
 	if (pdata->per_channel_irq) {
-		channel = pdata->channel;
-		for (i = 0; i < pdata->channel_count; i++, channel++)
+		for (i = 0; i < pdata->channel_count; i++) {
+			channel = pdata->channel[i];
 			xgbe_dma_isr(channel->dma_irq, channel);
+		}
 	} else {
 		disable_irq(pdata->dev_irq);
 		xgbe_isr(pdata->dev_irq, pdata);
@@ -1846,7 +1918,8 @@ static void xgbe_poll_controller(struct net_device *netdev)
 }
 #endif /* End CONFIG_NET_POLL_CONTROLLER */
 
-static int xgbe_setup_tc(struct net_device *netdev, u32 handle, __be16 proto,
+static int xgbe_setup_tc(struct net_device *netdev, u32 handle, u32 chain_index,
+			 __be16 proto,
 			 struct tc_to_netdev *tc_to_netdev)
 {
 	struct xgbe_prv_data *pdata = netdev_priv(netdev);
@@ -2327,8 +2400,9 @@ static int xgbe_all_poll(struct napi_struct *napi, int budget)
 	do {
 		last_processed = processed;
 
-		channel = pdata->channel;
-		for (i = 0; i < pdata->channel_count; i++, channel++) {
+		for (i = 0; i < pdata->channel_count; i++) {
+			channel = pdata->channel[i];
+
 			/* Cleanup Tx ring first */
 			xgbe_tx_poll(channel);
 
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c b/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
index 920566a3a599..67a2e52ad25d 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
@@ -247,7 +247,7 @@ static int xgbe_set_pauseparam(struct net_device *netdev,
 
 	if (pause->autoneg && (pdata->phy.autoneg != AUTONEG_ENABLE)) {
 		netdev_err(netdev,
-			   "autoneg disabled, pause autoneg not avialable\n");
+			   "autoneg disabled, pause autoneg not available\n");
 		return -EINVAL;
 	}
 
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-i2c.c b/drivers/net/ethernet/amd/xgbe/xgbe-i2c.c
index 417bdb5982a9..4d9062d35930 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-i2c.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-i2c.c
@@ -274,13 +274,16 @@ static void xgbe_i2c_clear_isr_interrupts(struct xgbe_prv_data *pdata,
 		XI2C_IOREAD(pdata, IC_CLR_STOP_DET);
 }
 
-static irqreturn_t xgbe_i2c_isr(int irq, void *data)
+static void xgbe_i2c_isr_task(unsigned long data)
 {
 	struct xgbe_prv_data *pdata = (struct xgbe_prv_data *)data;
 	struct xgbe_i2c_op_state *state = &pdata->i2c.op_state;
 	unsigned int isr;
 
 	isr = XI2C_IOREAD(pdata, IC_RAW_INTR_STAT);
+	if (!isr)
+		goto reissue_check;
+
 	netif_dbg(pdata, intr, pdata->netdev,
 		  "I2C interrupt received: status=%#010x\n", isr);
 
@@ -308,6 +311,21 @@ out:
 	if (state->ret || XI2C_GET_BITS(isr, IC_RAW_INTR_STAT, STOP_DET))
 		complete(&pdata->i2c_complete);
 
+reissue_check:
+	/* Reissue interrupt if status is not clear */
+	if (pdata->vdata->irq_reissue_support)
+		XP_IOWRITE(pdata, XP_INT_REISSUE_EN, 1 << 2);
+}
+
+static irqreturn_t xgbe_i2c_isr(int irq, void *data)
+{
+	struct xgbe_prv_data *pdata = (struct xgbe_prv_data *)data;
+
+	if (pdata->isr_as_tasklet)
+		tasklet_schedule(&pdata->tasklet_i2c);
+	else
+		xgbe_i2c_isr_task((unsigned long)pdata);
+
 	return IRQ_HANDLED;
 }
 
@@ -349,12 +367,11 @@ static void xgbe_i2c_set_target(struct xgbe_prv_data *pdata, unsigned int addr)
 	XI2C_IOWRITE(pdata, IC_TAR, addr);
 }
 
-static irqreturn_t xgbe_i2c_combined_isr(int irq, struct xgbe_prv_data *pdata)
+static irqreturn_t xgbe_i2c_combined_isr(struct xgbe_prv_data *pdata)
 {
-	if (!XI2C_IOREAD(pdata, IC_RAW_INTR_STAT))
-		return IRQ_HANDLED;
+	xgbe_i2c_isr_task((unsigned long)pdata);
 
-	return xgbe_i2c_isr(irq, pdata);
+	return IRQ_HANDLED;
 }
 
 static int xgbe_i2c_xfer(struct xgbe_prv_data *pdata, struct xgbe_i2c_op *op)
@@ -445,6 +462,9 @@ static int xgbe_i2c_start(struct xgbe_prv_data *pdata)
 
 	/* If we have a separate I2C irq, enable it */
 	if (pdata->dev_irq != pdata->i2c_irq) {
+		tasklet_init(&pdata->tasklet_i2c, xgbe_i2c_isr_task,
+			     (unsigned long)pdata);
+
 		ret = devm_request_irq(pdata->dev, pdata->i2c_irq,
 				       xgbe_i2c_isr, 0, pdata->i2c_name,
 				       pdata);
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-main.c b/drivers/net/ethernet/amd/xgbe/xgbe-main.c
index 17ac8f9a51a0..500147d9e3c8 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-main.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-main.c
@@ -140,14 +140,16 @@ static void xgbe_default_config(struct xgbe_prv_data *pdata)
 {
 	DBGPR("-->xgbe_default_config\n");
 
-	pdata->pblx8 = DMA_PBL_X8_ENABLE;
+	pdata->blen = DMA_SBMR_BLEN_64;
+	pdata->pbl = DMA_PBL_128;
+	pdata->aal = 1;
+	pdata->rd_osr_limit = 8;
+	pdata->wr_osr_limit = 8;
 	pdata->tx_sf_mode = MTL_TSF_ENABLE;
 	pdata->tx_threshold = MTL_TX_THRESHOLD_64;
-	pdata->tx_pbl = DMA_PBL_16;
 	pdata->tx_osp_mode = DMA_OSP_ENABLE;
 	pdata->rx_sf_mode = MTL_RSF_DISABLE;
 	pdata->rx_threshold = MTL_RX_THRESHOLD_64;
-	pdata->rx_pbl = DMA_PBL_16;
 	pdata->pause_autoneg = 1;
 	pdata->tx_pause = 1;
 	pdata->rx_pause = 1;
@@ -277,7 +279,11 @@ int xgbe_config_netdev(struct xgbe_prv_data *pdata)
 	pdata->desc_ded_period = jiffies;
 
 	/* Issue software reset to device */
-	pdata->hw_if.exit(pdata);
+	ret = pdata->hw_if.exit(pdata);
+	if (ret) {
+		dev_err(dev, "software reset failed\n");
+		return ret;
+	}
 
 	/* Set default configuration data */
 	xgbe_default_config(pdata);
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-mdio.c b/drivers/net/ethernet/amd/xgbe/xgbe-mdio.c
index b672d9249539..80684914dd8a 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-mdio.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-mdio.c
@@ -665,6 +665,10 @@ static void xgbe_an37_isr(struct xgbe_prv_data *pdata)
 	} else {
 		/* Enable AN interrupts */
 		xgbe_an37_enable_interrupts(pdata);
+
+		/* Reissue interrupt if status is not clear */
+		if (pdata->vdata->irq_reissue_support)
+			XP_IOWRITE(pdata, XP_INT_REISSUE_EN, 1 << 3);
 	}
 }
 
@@ -684,10 +688,14 @@ static void xgbe_an73_isr(struct xgbe_prv_data *pdata)
 	} else {
 		/* Enable AN interrupts */
 		xgbe_an73_enable_interrupts(pdata);
+
+		/* Reissue interrupt if status is not clear */
+		if (pdata->vdata->irq_reissue_support)
+			XP_IOWRITE(pdata, XP_INT_REISSUE_EN, 1 << 3);
 	}
 }
 
-static irqreturn_t xgbe_an_isr(int irq, void *data)
+static void xgbe_an_isr_task(unsigned long data)
 {
 	struct xgbe_prv_data *pdata = (struct xgbe_prv_data *)data;
 
@@ -705,13 +713,25 @@ static irqreturn_t xgbe_an_isr(int irq, void *data)
 	default:
 		break;
 	}
+}
+
+static irqreturn_t xgbe_an_isr(int irq, void *data)
+{
+	struct xgbe_prv_data *pdata = (struct xgbe_prv_data *)data;
+
+	if (pdata->isr_as_tasklet)
+		tasklet_schedule(&pdata->tasklet_an);
+	else
+		xgbe_an_isr_task((unsigned long)pdata);
 
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t xgbe_an_combined_isr(int irq, struct xgbe_prv_data *pdata)
+static irqreturn_t xgbe_an_combined_isr(struct xgbe_prv_data *pdata)
 {
-	return xgbe_an_isr(irq, pdata);
+	xgbe_an_isr_task((unsigned long)pdata);
+
+	return IRQ_HANDLED;
 }
 
 static void xgbe_an_irq_work(struct work_struct *work)
@@ -915,6 +935,10 @@ static void xgbe_an_state_machine(struct work_struct *work)
 		break;
 	}
 
+	/* Reissue interrupt if status is not clear */
+	if (pdata->vdata->irq_reissue_support)
+		XP_IOWRITE(pdata, XP_INT_REISSUE_EN, 1 << 3);
+
 	mutex_unlock(&pdata->an_mutex);
 }
 
@@ -1379,6 +1403,9 @@ static int xgbe_phy_start(struct xgbe_prv_data *pdata)
 
 	/* If we have a separate AN irq, enable it */
 	if (pdata->dev_irq != pdata->an_irq) {
+		tasklet_init(&pdata->tasklet_an, xgbe_an_isr_task,
+			     (unsigned long)pdata);
+
 		ret = devm_request_irq(pdata->dev, pdata->an_irq,
 				       xgbe_an_isr, 0, pdata->an_name,
 				       pdata);
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-pci.c b/drivers/net/ethernet/amd/xgbe/xgbe-pci.c
index 38392a520725..1e56ad7bd9a5 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-pci.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-pci.c
@@ -139,6 +139,7 @@ static int xgbe_config_multi_msi(struct xgbe_prv_data *pdata)
 		return ret;
 	}
 
+	pdata->isr_as_tasklet = 1;
 	pdata->irq_count = ret;
 
 	pdata->dev_irq = pci_irq_vector(pdata->pcidev, 0);
@@ -175,6 +176,7 @@ static int xgbe_config_irqs(struct xgbe_prv_data *pdata)
 		return ret;
 	}
 
+	pdata->isr_as_tasklet = pdata->pcidev->msi_enabled ? 1 : 0;
 	pdata->irq_count = 1;
 	pdata->channel_irq_count = 1;
 
@@ -325,9 +327,9 @@ static int xgbe_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	/* Set the DMA coherency values */
 	pdata->coherent = 1;
-	pdata->axdomain = XGBE_DMA_OS_AXDOMAIN;
-	pdata->arcache = XGBE_DMA_OS_ARCACHE;
-	pdata->awcache = XGBE_DMA_OS_AWCACHE;
+	pdata->arcr = XGBE_DMA_PCI_ARCR;
+	pdata->awcr = XGBE_DMA_PCI_AWCR;
+	pdata->awarcr = XGBE_DMA_PCI_AWARCR;
 
 	/* Set the maximum channels and queues */
 	reg = XP_IOREAD(pdata, XP_PROP_1);
@@ -445,6 +447,9 @@ static const struct xgbe_version_data xgbe_v2a = {
 	.tx_tstamp_workaround		= 1,
 	.ecc_support			= 1,
 	.i2c_support			= 1,
+	.irq_reissue_support		= 1,
+	.tx_desc_prefetch		= 5,
+	.rx_desc_prefetch		= 5,
 };
 
 static const struct xgbe_version_data xgbe_v2b = {
@@ -456,6 +461,9 @@ static const struct xgbe_version_data xgbe_v2b = {
 	.tx_tstamp_workaround		= 1,
 	.ecc_support			= 1,
 	.i2c_support			= 1,
+	.irq_reissue_support		= 1,
+	.tx_desc_prefetch		= 5,
+	.rx_desc_prefetch		= 5,
 };
 
 static const struct pci_device_id xgbe_pci_table[] = {
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-phy-v2.c b/drivers/net/ethernet/amd/xgbe/xgbe-phy-v2.c
index e707c49cc55a..04b5c149caca 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-phy-v2.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-phy-v2.c
@@ -711,23 +711,39 @@ static void xgbe_phy_sfp_phy_settings(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_phy_data *phy_data = pdata->phy_data;
 
+	if (!phy_data->sfp_mod_absent && !phy_data->sfp_changed)
+		return;
+
+	pdata->phy.supported &= ~SUPPORTED_Autoneg;
+	pdata->phy.supported &= ~(SUPPORTED_Pause | SUPPORTED_Asym_Pause);
+	pdata->phy.supported &= ~SUPPORTED_TP;
+	pdata->phy.supported &= ~SUPPORTED_FIBRE;
+	pdata->phy.supported &= ~SUPPORTED_100baseT_Full;
+	pdata->phy.supported &= ~SUPPORTED_1000baseT_Full;
+	pdata->phy.supported &= ~SUPPORTED_10000baseT_Full;
+
 	if (phy_data->sfp_mod_absent) {
 		pdata->phy.speed = SPEED_UNKNOWN;
 		pdata->phy.duplex = DUPLEX_UNKNOWN;
 		pdata->phy.autoneg = AUTONEG_ENABLE;
+		pdata->phy.pause_autoneg = AUTONEG_ENABLE;
+
+		pdata->phy.supported |= SUPPORTED_Autoneg;
+		pdata->phy.supported |= SUPPORTED_Pause | SUPPORTED_Asym_Pause;
+		pdata->phy.supported |= SUPPORTED_TP;
+		pdata->phy.supported |= SUPPORTED_FIBRE;
+		if (phy_data->port_speeds & XGBE_PHY_PORT_SPEED_100)
+			pdata->phy.supported |= SUPPORTED_100baseT_Full;
+		if (phy_data->port_speeds & XGBE_PHY_PORT_SPEED_1000)
+			pdata->phy.supported |= SUPPORTED_1000baseT_Full;
+		if (phy_data->port_speeds & XGBE_PHY_PORT_SPEED_10000)
+			pdata->phy.supported |= SUPPORTED_10000baseT_Full;
+
 		pdata->phy.advertising = pdata->phy.supported;
 
 		return;
 	}
 
-	pdata->phy.advertising &= ~ADVERTISED_Autoneg;
-	pdata->phy.advertising &= ~ADVERTISED_TP;
-	pdata->phy.advertising &= ~ADVERTISED_FIBRE;
-	pdata->phy.advertising &= ~ADVERTISED_100baseT_Full;
-	pdata->phy.advertising &= ~ADVERTISED_1000baseT_Full;
-	pdata->phy.advertising &= ~ADVERTISED_10000baseT_Full;
-	pdata->phy.advertising &= ~ADVERTISED_10000baseR_FEC;
-
 	switch (phy_data->sfp_base) {
 	case XGBE_SFP_BASE_1000_T:
 	case XGBE_SFP_BASE_1000_SX:
@@ -736,17 +752,25 @@ static void xgbe_phy_sfp_phy_settings(struct xgbe_prv_data *pdata)
 		pdata->phy.speed = SPEED_UNKNOWN;
 		pdata->phy.duplex = DUPLEX_UNKNOWN;
 		pdata->phy.autoneg = AUTONEG_ENABLE;
-		pdata->phy.advertising |= ADVERTISED_Autoneg;
+		pdata->phy.pause_autoneg = AUTONEG_ENABLE;
+		pdata->phy.supported |= SUPPORTED_Autoneg;
+		pdata->phy.supported |= SUPPORTED_Pause | SUPPORTED_Asym_Pause;
 		break;
 	case XGBE_SFP_BASE_10000_SR:
 	case XGBE_SFP_BASE_10000_LR:
 	case XGBE_SFP_BASE_10000_LRM:
 	case XGBE_SFP_BASE_10000_ER:
 	case XGBE_SFP_BASE_10000_CR:
-	default:
 		pdata->phy.speed = SPEED_10000;
 		pdata->phy.duplex = DUPLEX_FULL;
 		pdata->phy.autoneg = AUTONEG_DISABLE;
+		pdata->phy.pause_autoneg = AUTONEG_DISABLE;
+		break;
+	default:
+		pdata->phy.speed = SPEED_UNKNOWN;
+		pdata->phy.duplex = DUPLEX_UNKNOWN;
+		pdata->phy.autoneg = AUTONEG_DISABLE;
+		pdata->phy.pause_autoneg = AUTONEG_DISABLE;
 		break;
 	}
 
@@ -754,36 +778,38 @@ static void xgbe_phy_sfp_phy_settings(struct xgbe_prv_data *pdata)
 	case XGBE_SFP_BASE_1000_T:
 	case XGBE_SFP_BASE_1000_CX:
 	case XGBE_SFP_BASE_10000_CR:
-		pdata->phy.advertising |= ADVERTISED_TP;
+		pdata->phy.supported |= SUPPORTED_TP;
 		break;
 	default:
-		pdata->phy.advertising |= ADVERTISED_FIBRE;
+		pdata->phy.supported |= SUPPORTED_FIBRE;
 	}
 
 	switch (phy_data->sfp_speed) {
 	case XGBE_SFP_SPEED_100_1000:
 		if (phy_data->port_speeds & XGBE_PHY_PORT_SPEED_100)
-			pdata->phy.advertising |= ADVERTISED_100baseT_Full;
+			pdata->phy.supported |= SUPPORTED_100baseT_Full;
 		if (phy_data->port_speeds & XGBE_PHY_PORT_SPEED_1000)
-			pdata->phy.advertising |= ADVERTISED_1000baseT_Full;
+			pdata->phy.supported |= SUPPORTED_1000baseT_Full;
 		break;
 	case XGBE_SFP_SPEED_1000:
 		if (phy_data->port_speeds & XGBE_PHY_PORT_SPEED_1000)
-			pdata->phy.advertising |= ADVERTISED_1000baseT_Full;
+			pdata->phy.supported |= SUPPORTED_1000baseT_Full;
 		break;
 	case XGBE_SFP_SPEED_10000:
 		if (phy_data->port_speeds & XGBE_PHY_PORT_SPEED_10000)
-			pdata->phy.advertising |= ADVERTISED_10000baseT_Full;
+			pdata->phy.supported |= SUPPORTED_10000baseT_Full;
 		break;
 	default:
 		/* Choose the fastest supported speed */
 		if (phy_data->port_speeds & XGBE_PHY_PORT_SPEED_10000)
-			pdata->phy.advertising |= ADVERTISED_10000baseT_Full;
+			pdata->phy.supported |= SUPPORTED_10000baseT_Full;
 		else if (phy_data->port_speeds & XGBE_PHY_PORT_SPEED_1000)
-			pdata->phy.advertising |= ADVERTISED_1000baseT_Full;
+			pdata->phy.supported |= SUPPORTED_1000baseT_Full;
 		else if (phy_data->port_speeds & XGBE_PHY_PORT_SPEED_100)
-			pdata->phy.advertising |= ADVERTISED_100baseT_Full;
+			pdata->phy.supported |= SUPPORTED_100baseT_Full;
 	}
+
+	pdata->phy.advertising = pdata->phy.supported;
 }
 
 static bool xgbe_phy_sfp_bit_rate(struct xgbe_sfp_eeprom *sfp_eeprom,
@@ -1095,7 +1121,8 @@ static int xgbe_phy_sfp_read_eeprom(struct xgbe_prv_data *pdata)
 
 	ret = xgbe_phy_sfp_get_mux(pdata);
 	if (ret) {
-		netdev_err(pdata->netdev, "I2C error setting SFP MUX\n");
+		dev_err_once(pdata->dev, "%s: I2C error setting SFP MUX\n",
+			     netdev_name(pdata->netdev));
 		return ret;
 	}
 
@@ -1105,7 +1132,8 @@ static int xgbe_phy_sfp_read_eeprom(struct xgbe_prv_data *pdata)
 				&eeprom_addr, sizeof(eeprom_addr),
 				&sfp_eeprom, sizeof(sfp_eeprom));
 	if (ret) {
-		netdev_err(pdata->netdev, "I2C error reading SFP EEPROM\n");
+		dev_err_once(pdata->dev, "%s: I2C error reading SFP EEPROM\n",
+			     netdev_name(pdata->netdev));
 		goto put;
 	}
 
@@ -1164,7 +1192,8 @@ static void xgbe_phy_sfp_signals(struct xgbe_prv_data *pdata)
 				&gpio_reg, sizeof(gpio_reg),
 				gpio_ports, sizeof(gpio_ports));
 	if (ret) {
-		netdev_err(pdata->netdev, "I2C error reading SFP GPIOs\n");
+		dev_err_once(pdata->dev, "%s: I2C error reading SFP GPIOs\n",
+			     netdev_name(pdata->netdev));
 		return;
 	}
 
@@ -1694,19 +1723,25 @@ static void xgbe_phy_set_redrv_mode(struct xgbe_prv_data *pdata)
 	xgbe_phy_put_comm_ownership(pdata);
 }
 
-static void xgbe_phy_start_ratechange(struct xgbe_prv_data *pdata)
+static void xgbe_phy_perform_ratechange(struct xgbe_prv_data *pdata,
+					unsigned int cmd, unsigned int sub_cmd)
 {
-	if (!XP_IOREAD_BITS(pdata, XP_DRIVER_INT_RO, STATUS))
-		return;
+	unsigned int s0 = 0;
+	unsigned int wait;
 
 	/* Log if a previous command did not complete */
-	netif_dbg(pdata, link, pdata->netdev,
-		  "firmware mailbox not ready for command\n");
-}
+	if (XP_IOREAD_BITS(pdata, XP_DRIVER_INT_RO, STATUS))
+		netif_dbg(pdata, link, pdata->netdev,
+			  "firmware mailbox not ready for command\n");
 
-static void xgbe_phy_complete_ratechange(struct xgbe_prv_data *pdata)
-{
-	unsigned int wait;
+	/* Construct the command */
+	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, COMMAND, cmd);
+	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, SUB_COMMAND, sub_cmd);
+
+	/* Issue the command */
+	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_0, s0);
+	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_1, 0);
+	XP_IOWRITE_BITS(pdata, XP_DRIVER_INT_REQ, REQUEST, 1);
 
 	/* Wait for command to complete */
 	wait = XGBE_RATECHANGE_COUNT;
@@ -1723,21 +1758,8 @@ static void xgbe_phy_complete_ratechange(struct xgbe_prv_data *pdata)
 
 static void xgbe_phy_rrc(struct xgbe_prv_data *pdata)
 {
-	unsigned int s0;
-
-	xgbe_phy_start_ratechange(pdata);
-
 	/* Receiver Reset Cycle */
-	s0 = 0;
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, COMMAND, 5);
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, SUB_COMMAND, 0);
-
-	/* Call FW to make the change */
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_0, s0);
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_1, 0);
-	XP_IOWRITE_BITS(pdata, XP_DRIVER_INT_REQ, REQUEST, 1);
-
-	xgbe_phy_complete_ratechange(pdata);
+	xgbe_phy_perform_ratechange(pdata, 5, 0);
 
 	netif_dbg(pdata, link, pdata->netdev, "receiver reset complete\n");
 }
@@ -1746,14 +1768,8 @@ static void xgbe_phy_power_off(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_phy_data *phy_data = pdata->phy_data;
 
-	xgbe_phy_start_ratechange(pdata);
-
-	/* Call FW to make the change */
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_0, 0);
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_1, 0);
-	XP_IOWRITE_BITS(pdata, XP_DRIVER_INT_REQ, REQUEST, 1);
-
-	xgbe_phy_complete_ratechange(pdata);
+	/* Power off */
+	xgbe_phy_perform_ratechange(pdata, 0, 0);
 
 	phy_data->cur_mode = XGBE_MODE_UNKNOWN;
 
@@ -1763,33 +1779,21 @@ static void xgbe_phy_power_off(struct xgbe_prv_data *pdata)
 static void xgbe_phy_sfi_mode(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_phy_data *phy_data = pdata->phy_data;
-	unsigned int s0;
 
 	xgbe_phy_set_redrv_mode(pdata);
 
-	xgbe_phy_start_ratechange(pdata);
-
 	/* 10G/SFI */
-	s0 = 0;
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, COMMAND, 3);
 	if (phy_data->sfp_cable != XGBE_SFP_CABLE_PASSIVE) {
-		XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, SUB_COMMAND, 0);
+		xgbe_phy_perform_ratechange(pdata, 3, 0);
 	} else {
 		if (phy_data->sfp_cable_len <= 1)
-			XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, SUB_COMMAND, 1);
+			xgbe_phy_perform_ratechange(pdata, 3, 1);
 		else if (phy_data->sfp_cable_len <= 3)
-			XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, SUB_COMMAND, 2);
+			xgbe_phy_perform_ratechange(pdata, 3, 2);
 		else
-			XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, SUB_COMMAND, 3);
+			xgbe_phy_perform_ratechange(pdata, 3, 3);
 	}
 
-	/* Call FW to make the change */
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_0, s0);
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_1, 0);
-	XP_IOWRITE_BITS(pdata, XP_DRIVER_INT_REQ, REQUEST, 1);
-
-	xgbe_phy_complete_ratechange(pdata);
-
 	phy_data->cur_mode = XGBE_MODE_SFI;
 
 	netif_dbg(pdata, link, pdata->netdev, "10GbE SFI mode set\n");
@@ -1798,23 +1802,11 @@ static void xgbe_phy_sfi_mode(struct xgbe_prv_data *pdata)
 static void xgbe_phy_x_mode(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_phy_data *phy_data = pdata->phy_data;
-	unsigned int s0;
 
 	xgbe_phy_set_redrv_mode(pdata);
 
-	xgbe_phy_start_ratechange(pdata);
-
 	/* 1G/X */
-	s0 = 0;
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, COMMAND, 1);
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, SUB_COMMAND, 3);
-
-	/* Call FW to make the change */
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_0, s0);
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_1, 0);
-	XP_IOWRITE_BITS(pdata, XP_DRIVER_INT_REQ, REQUEST, 1);
-
-	xgbe_phy_complete_ratechange(pdata);
+	xgbe_phy_perform_ratechange(pdata, 1, 3);
 
 	phy_data->cur_mode = XGBE_MODE_X;
 
@@ -1824,23 +1816,11 @@ static void xgbe_phy_x_mode(struct xgbe_prv_data *pdata)
 static void xgbe_phy_sgmii_1000_mode(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_phy_data *phy_data = pdata->phy_data;
-	unsigned int s0;
 
 	xgbe_phy_set_redrv_mode(pdata);
 
-	xgbe_phy_start_ratechange(pdata);
-
 	/* 1G/SGMII */
-	s0 = 0;
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, COMMAND, 1);
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, SUB_COMMAND, 2);
-
-	/* Call FW to make the change */
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_0, s0);
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_1, 0);
-	XP_IOWRITE_BITS(pdata, XP_DRIVER_INT_REQ, REQUEST, 1);
-
-	xgbe_phy_complete_ratechange(pdata);
+	xgbe_phy_perform_ratechange(pdata, 1, 2);
 
 	phy_data->cur_mode = XGBE_MODE_SGMII_1000;
 
@@ -1850,23 +1830,11 @@ static void xgbe_phy_sgmii_1000_mode(struct xgbe_prv_data *pdata)
 static void xgbe_phy_sgmii_100_mode(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_phy_data *phy_data = pdata->phy_data;
-	unsigned int s0;
 
 	xgbe_phy_set_redrv_mode(pdata);
 
-	xgbe_phy_start_ratechange(pdata);
-
-	/* 1G/SGMII */
-	s0 = 0;
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, COMMAND, 1);
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, SUB_COMMAND, 1);
-
-	/* Call FW to make the change */
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_0, s0);
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_1, 0);
-	XP_IOWRITE_BITS(pdata, XP_DRIVER_INT_REQ, REQUEST, 1);
-
-	xgbe_phy_complete_ratechange(pdata);
+	/* 100M/SGMII */
+	xgbe_phy_perform_ratechange(pdata, 1, 1);
 
 	phy_data->cur_mode = XGBE_MODE_SGMII_100;
 
@@ -1876,23 +1844,11 @@ static void xgbe_phy_sgmii_100_mode(struct xgbe_prv_data *pdata)
 static void xgbe_phy_kr_mode(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_phy_data *phy_data = pdata->phy_data;
-	unsigned int s0;
 
 	xgbe_phy_set_redrv_mode(pdata);
 
-	xgbe_phy_start_ratechange(pdata);
-
 	/* 10G/KR */
-	s0 = 0;
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, COMMAND, 4);
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, SUB_COMMAND, 0);
-
-	/* Call FW to make the change */
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_0, s0);
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_1, 0);
-	XP_IOWRITE_BITS(pdata, XP_DRIVER_INT_REQ, REQUEST, 1);
-
-	xgbe_phy_complete_ratechange(pdata);
+	xgbe_phy_perform_ratechange(pdata, 4, 0);
 
 	phy_data->cur_mode = XGBE_MODE_KR;
 
@@ -1902,23 +1858,11 @@ static void xgbe_phy_kr_mode(struct xgbe_prv_data *pdata)
 static void xgbe_phy_kx_2500_mode(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_phy_data *phy_data = pdata->phy_data;
-	unsigned int s0;
 
 	xgbe_phy_set_redrv_mode(pdata);
 
-	xgbe_phy_start_ratechange(pdata);
-
 	/* 2.5G/KX */
-	s0 = 0;
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, COMMAND, 2);
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, SUB_COMMAND, 0);
-
-	/* Call FW to make the change */
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_0, s0);
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_1, 0);
-	XP_IOWRITE_BITS(pdata, XP_DRIVER_INT_REQ, REQUEST, 1);
-
-	xgbe_phy_complete_ratechange(pdata);
+	xgbe_phy_perform_ratechange(pdata, 2, 0);
 
 	phy_data->cur_mode = XGBE_MODE_KX_2500;
 
@@ -1928,23 +1872,11 @@ static void xgbe_phy_kx_2500_mode(struct xgbe_prv_data *pdata)
 static void xgbe_phy_kx_1000_mode(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_phy_data *phy_data = pdata->phy_data;
-	unsigned int s0;
 
 	xgbe_phy_set_redrv_mode(pdata);
 
-	xgbe_phy_start_ratechange(pdata);
-
 	/* 1G/KX */
-	s0 = 0;
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, COMMAND, 1);
-	XP_SET_BITS(s0, XP_DRIVER_SCRATCH_0, SUB_COMMAND, 3);
-
-	/* Call FW to make the change */
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_0, s0);
-	XP_IOWRITE(pdata, XP_DRIVER_SCRATCH_1, 0);
-	XP_IOWRITE_BITS(pdata, XP_DRIVER_INT_REQ, REQUEST, 1);
-
-	xgbe_phy_complete_ratechange(pdata);
+	xgbe_phy_perform_ratechange(pdata, 1, 3);
 
 	phy_data->cur_mode = XGBE_MODE_KX_1000;
 
@@ -2037,6 +1969,8 @@ static enum xgbe_mode xgbe_phy_get_baset_mode(struct xgbe_phy_data *phy_data,
 		return XGBE_MODE_SGMII_100;
 	case SPEED_1000:
 		return XGBE_MODE_SGMII_1000;
+	case SPEED_2500:
+		return XGBE_MODE_KX_2500;
 	case SPEED_10000:
 		return XGBE_MODE_KR;
 	default:
@@ -2180,6 +2114,9 @@ static bool xgbe_phy_use_baset_mode(struct xgbe_prv_data *pdata,
 	case XGBE_MODE_SGMII_1000:
 		return xgbe_phy_check_mode(pdata, mode,
 					   ADVERTISED_1000baseT_Full);
+	case XGBE_MODE_KX_2500:
+		return xgbe_phy_check_mode(pdata, mode,
+					   ADVERTISED_2500baseX_Full);
 	case XGBE_MODE_KR:
 		return xgbe_phy_check_mode(pdata, mode,
 					   ADVERTISED_10000baseT_Full);
@@ -2210,6 +2147,8 @@ static bool xgbe_phy_use_sfp_mode(struct xgbe_prv_data *pdata,
 		return xgbe_phy_check_mode(pdata, mode,
 					   ADVERTISED_1000baseT_Full);
 	case XGBE_MODE_SFI:
+		if (phy_data->sfp_mod_absent)
+			return true;
 		return xgbe_phy_check_mode(pdata, mode,
 					   ADVERTISED_10000baseT_Full);
 	default:
@@ -2287,6 +2226,8 @@ static bool xgbe_phy_valid_speed_baset_mode(struct xgbe_phy_data *phy_data,
 	case SPEED_100:
 	case SPEED_1000:
 		return true;
+	case SPEED_2500:
+		return (phy_data->port_mode == XGBE_PORT_MODE_NBASE_T);
 	case SPEED_10000:
 		return (phy_data->port_mode == XGBE_PORT_MODE_10GBASE_T);
 	default:
@@ -3013,9 +2954,6 @@ static int xgbe_phy_init(struct xgbe_prv_data *pdata)
 		if (phy_data->port_speeds & XGBE_PHY_PORT_SPEED_10000) {
 			pdata->phy.supported |= SUPPORTED_10000baseT_Full;
 			phy_data->start_mode = XGBE_MODE_SFI;
-			if (pdata->fec_ability & MDIO_PMA_10GBR_FECABLE_ABLE)
-				pdata->phy.supported |=
-					SUPPORTED_10000baseR_FEC;
 		}
 
 		phy_data->phydev_mode = XGBE_MDIO_MODE_CL22;
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-platform.c b/drivers/net/ethernet/amd/xgbe/xgbe-platform.c
index 84d4c51cab8c..d0f3dfb88202 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-platform.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-platform.c
@@ -448,13 +448,11 @@ static int xgbe_platform_probe(struct platform_device *pdev)
 	}
 	pdata->coherent = (attr == DEV_DMA_COHERENT);
 	if (pdata->coherent) {
-		pdata->axdomain = XGBE_DMA_OS_AXDOMAIN;
-		pdata->arcache = XGBE_DMA_OS_ARCACHE;
-		pdata->awcache = XGBE_DMA_OS_AWCACHE;
+		pdata->arcr = XGBE_DMA_OS_ARCR;
+		pdata->awcr = XGBE_DMA_OS_AWCR;
 	} else {
-		pdata->axdomain = XGBE_DMA_SYS_AXDOMAIN;
-		pdata->arcache = XGBE_DMA_SYS_ARCACHE;
-		pdata->awcache = XGBE_DMA_SYS_AWCACHE;
+		pdata->arcr = XGBE_DMA_SYS_ARCR;
+		pdata->awcr = XGBE_DMA_SYS_AWCR;
 	}
 
 	/* Set the maximum fifo amounts */
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c b/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c
index a533a6cc2d53..d06d260cf1e2 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c
@@ -267,7 +267,7 @@ void xgbe_ptp_register(struct xgbe_prv_data *pdata)
 			 ktime_to_ns(ktime_get_real()));
 
 	/* Disable all timestamping to start */
-	XGMAC_IOWRITE(pdata, MAC_TCR, 0);
+	XGMAC_IOWRITE(pdata, MAC_TSCR, 0);
 	pdata->tstamp_config.tx_type = HWTSTAMP_TX_OFF;
 	pdata->tstamp_config.rx_filter = HWTSTAMP_FILTER_NONE;
 }
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe.h b/drivers/net/ethernet/amd/xgbe/xgbe.h
index f9a24639f574..0938294f640a 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe.h
+++ b/drivers/net/ethernet/amd/xgbe/xgbe.h
@@ -128,6 +128,7 @@
 #include <linux/net_tstamp.h>
 #include <net/dcbnl.h>
 #include <linux/completion.h>
+#include <linux/cpumask.h>
 
 #define XGBE_DRV_NAME		"amd-xgbe"
 #define XGBE_DRV_VERSION	"1.0.3"
@@ -163,14 +164,17 @@
 #define XGBE_DMA_STOP_TIMEOUT	1
 
 /* DMA cache settings - Outer sharable, write-back, write-allocate */
-#define XGBE_DMA_OS_AXDOMAIN	0x2
-#define XGBE_DMA_OS_ARCACHE	0xb
-#define XGBE_DMA_OS_AWCACHE	0xf
+#define XGBE_DMA_OS_ARCR	0x002b2b2b
+#define XGBE_DMA_OS_AWCR	0x2f2f2f2f
 
 /* DMA cache settings - System, no caches used */
-#define XGBE_DMA_SYS_AXDOMAIN	0x3
-#define XGBE_DMA_SYS_ARCACHE	0x0
-#define XGBE_DMA_SYS_AWCACHE	0x0
+#define XGBE_DMA_SYS_ARCR	0x00303030
+#define XGBE_DMA_SYS_AWCR	0x30303030
+
+/* DMA cache settings - PCI device */
+#define XGBE_DMA_PCI_ARCR	0x00000003
+#define XGBE_DMA_PCI_AWCR	0x13131313
+#define XGBE_DMA_PCI_AWARCR	0x00000313
 
 /* DMA channel interrupt modes */
 #define XGBE_IRQ_MODE_EDGE	0
@@ -412,6 +416,7 @@ struct xgbe_ring {
 	/* Page allocation for RX buffers */
 	struct xgbe_page_alloc rx_hdr_pa;
 	struct xgbe_page_alloc rx_buf_pa;
+	int node;
 
 	/* Ring index values
 	 *  cur   - Tx: index of descriptor to be used for current transfer
@@ -462,6 +467,9 @@ struct xgbe_channel {
 
 	struct xgbe_ring *tx_ring;
 	struct xgbe_ring *rx_ring;
+
+	int node;
+	cpumask_t affinity_mask;
 } ____cacheline_aligned;
 
 enum xgbe_state {
@@ -734,13 +742,6 @@ struct xgbe_hw_if {
 	/* For TX DMA Operate on Second Frame config */
 	int (*config_osp_mode)(struct xgbe_prv_data *);
 
-	/* For RX and TX PBL config */
-	int (*config_rx_pbl_val)(struct xgbe_prv_data *);
-	int (*get_rx_pbl_val)(struct xgbe_prv_data *);
-	int (*config_tx_pbl_val)(struct xgbe_prv_data *);
-	int (*get_tx_pbl_val)(struct xgbe_prv_data *);
-	int (*config_pblx8)(struct xgbe_prv_data *);
-
 	/* For MMC statistics */
 	void (*rx_mmc_int)(struct xgbe_prv_data *);
 	void (*tx_mmc_int)(struct xgbe_prv_data *);
@@ -837,7 +838,7 @@ struct xgbe_phy_if {
 	bool (*phy_valid_speed)(struct xgbe_prv_data *, int);
 
 	/* For single interrupt support */
-	irqreturn_t (*an_isr)(int, struct xgbe_prv_data *);
+	irqreturn_t (*an_isr)(struct xgbe_prv_data *);
 
 	/* PHY implementation specific services */
 	struct xgbe_phy_impl_if phy_impl;
@@ -855,7 +856,7 @@ struct xgbe_i2c_if {
 	int (*i2c_xfer)(struct xgbe_prv_data *, struct xgbe_i2c_op *);
 
 	/* For single interrupt support */
-	irqreturn_t (*i2c_isr)(int, struct xgbe_prv_data *);
+	irqreturn_t (*i2c_isr)(struct xgbe_prv_data *);
 };
 
 struct xgbe_desc_if {
@@ -924,6 +925,9 @@ struct xgbe_version_data {
 	unsigned int tx_tstamp_workaround;
 	unsigned int ecc_support;
 	unsigned int i2c_support;
+	unsigned int irq_reissue_support;
+	unsigned int tx_desc_prefetch;
+	unsigned int rx_desc_prefetch;
 };
 
 struct xgbe_prv_data {
@@ -1001,9 +1005,9 @@ struct xgbe_prv_data {
 
 	/* AXI DMA settings */
 	unsigned int coherent;
-	unsigned int axdomain;
-	unsigned int arcache;
-	unsigned int awcache;
+	unsigned int arcr;
+	unsigned int awcr;
+	unsigned int awarcr;
 
 	/* Service routine support */
 	struct workqueue_struct *dev_workqueue;
@@ -1011,7 +1015,7 @@ struct xgbe_prv_data {
 	struct timer_list service_timer;
 
 	/* Rings for Tx/Rx on a DMA channel */
-	struct xgbe_channel *channel;
+	struct xgbe_channel *channel[XGBE_MAX_DMA_CHANNELS];
 	unsigned int tx_max_channel_count;
 	unsigned int rx_max_channel_count;
 	unsigned int channel_count;
@@ -1026,19 +1030,21 @@ struct xgbe_prv_data {
 	unsigned int rx_q_count;
 
 	/* Tx/Rx common settings */
-	unsigned int pblx8;
+	unsigned int blen;
+	unsigned int pbl;
+	unsigned int aal;
+	unsigned int rd_osr_limit;
+	unsigned int wr_osr_limit;
 
 	/* Tx settings */
 	unsigned int tx_sf_mode;
 	unsigned int tx_threshold;
-	unsigned int tx_pbl;
 	unsigned int tx_osp_mode;
 	unsigned int tx_max_fifo_size;
 
 	/* Rx settings */
 	unsigned int rx_sf_mode;
 	unsigned int rx_threshold;
-	unsigned int rx_pbl;
 	unsigned int rx_max_fifo_size;
 
 	/* Tx coalescing settings */
@@ -1159,6 +1165,12 @@ struct xgbe_prv_data {
 
 	unsigned int lpm_ctrl;		/* CTRL1 for resume */
 
+	unsigned int isr_as_tasklet;
+	struct tasklet_struct tasklet_dev;
+	struct tasklet_struct tasklet_ecc;
+	struct tasklet_struct tasklet_i2c;
+	struct tasklet_struct tasklet_an;
+
 #ifdef CONFIG_DEBUG_FS
 	struct dentry *xgbe_debugfs;
 
diff --git a/drivers/net/ethernet/apm/xgene-v2/ethtool.c b/drivers/net/ethernet/apm/xgene-v2/ethtool.c
index b6666e418e79..d31ad8270d93 100644
--- a/drivers/net/ethernet/apm/xgene-v2/ethtool.c
+++ b/drivers/net/ethernet/apm/xgene-v2/ethtool.c
@@ -157,7 +157,9 @@ static int xge_get_link_ksettings(struct net_device *ndev,
 	if (!phydev)
 		return -ENODEV;
 
-	return phy_ethtool_ksettings_get(phydev, cmd);
+	phy_ethtool_ksettings_get(phydev, cmd);
+
+	return 0;
 }
 
 static int xge_set_link_ksettings(struct net_device *ndev,
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c b/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c
index 28fdedc30b74..4f50f11718f4 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c
@@ -23,9 +23,17 @@
 struct xgene_gstrings_stats {
 	char name[ETH_GSTRING_LEN];
 	int offset;
+	u32 addr;
+	u32 mask;
 };
 
-#define XGENE_STAT(m) { #m, offsetof(struct xgene_enet_pdata, stats.m) }
+#define XGENE_STAT(m) { #m, offsetof(struct rtnl_link_stats64, m) }
+#define XGENE_EXTD_STAT(s, a, m)		\
+		{			\
+		.name = #s,		\
+		.addr = a ## _ADDR,	\
+		.mask = m		\
+		}
 
 static const struct xgene_gstrings_stats gstrings_stats[] = {
 	XGENE_STAT(rx_packets),
@@ -40,7 +48,65 @@ static const struct xgene_gstrings_stats gstrings_stats[] = {
 	XGENE_STAT(rx_fifo_errors)
 };
 
+static const struct xgene_gstrings_stats gstrings_extd_stats[] = {
+	XGENE_EXTD_STAT(tx_rx_64b_frame_cntr, TR64, 31),
+	XGENE_EXTD_STAT(tx_rx_127b_frame_cntr, TR127, 31),
+	XGENE_EXTD_STAT(tx_rx_255b_frame_cntr, TR255, 31),
+	XGENE_EXTD_STAT(tx_rx_511b_frame_cntr, TR511, 31),
+	XGENE_EXTD_STAT(tx_rx_1023b_frame_cntr, TR1K, 31),
+	XGENE_EXTD_STAT(tx_rx_1518b_frame_cntr, TRMAX, 31),
+	XGENE_EXTD_STAT(tx_rx_1522b_frame_cntr, TRMGV, 31),
+	XGENE_EXTD_STAT(rx_fcs_error_cntr, RFCS, 16),
+	XGENE_EXTD_STAT(rx_multicast_pkt_cntr, RMCA, 31),
+	XGENE_EXTD_STAT(rx_broadcast_pkt_cntr, RBCA, 31),
+	XGENE_EXTD_STAT(rx_ctrl_frame_pkt_cntr, RXCF, 16),
+	XGENE_EXTD_STAT(rx_pause_frame_pkt_cntr, RXPF, 16),
+	XGENE_EXTD_STAT(rx_unk_opcode_cntr, RXUO, 16),
+	XGENE_EXTD_STAT(rx_align_err_cntr, RALN, 16),
+	XGENE_EXTD_STAT(rx_frame_len_err_cntr, RFLR, 16),
+	XGENE_EXTD_STAT(rx_frame_len_err_recov_cntr, DUMP, 0),
+	XGENE_EXTD_STAT(rx_code_err_cntr, RCDE, 16),
+	XGENE_EXTD_STAT(rx_carrier_sense_err_cntr, RCSE, 16),
+	XGENE_EXTD_STAT(rx_undersize_pkt_cntr, RUND, 16),
+	XGENE_EXTD_STAT(rx_oversize_pkt_cntr, ROVR, 16),
+	XGENE_EXTD_STAT(rx_fragments_cntr, RFRG, 16),
+	XGENE_EXTD_STAT(rx_jabber_cntr, RJBR, 16),
+	XGENE_EXTD_STAT(rx_jabber_recov_cntr, DUMP, 0),
+	XGENE_EXTD_STAT(rx_dropped_pkt_cntr, RDRP, 16),
+	XGENE_EXTD_STAT(rx_overrun_cntr, DUMP, 0),
+	XGENE_EXTD_STAT(tx_multicast_pkt_cntr, TMCA, 31),
+	XGENE_EXTD_STAT(tx_broadcast_pkt_cntr, TBCA, 31),
+	XGENE_EXTD_STAT(tx_pause_ctrl_frame_cntr, TXPF, 16),
+	XGENE_EXTD_STAT(tx_defer_pkt_cntr, TDFR, 31),
+	XGENE_EXTD_STAT(tx_excv_defer_pkt_cntr, TEDF, 31),
+	XGENE_EXTD_STAT(tx_single_col_pkt_cntr, TSCL, 31),
+	XGENE_EXTD_STAT(tx_multi_col_pkt_cntr, TMCL, 31),
+	XGENE_EXTD_STAT(tx_late_col_pkt_cntr, TLCL, 31),
+	XGENE_EXTD_STAT(tx_excv_col_pkt_cntr, TXCL, 31),
+	XGENE_EXTD_STAT(tx_total_col_cntr, TNCL, 31),
+	XGENE_EXTD_STAT(tx_pause_frames_hnrd_cntr, TPFH, 16),
+	XGENE_EXTD_STAT(tx_drop_frame_cntr, TDRP, 16),
+	XGENE_EXTD_STAT(tx_jabber_frame_cntr, TJBR, 12),
+	XGENE_EXTD_STAT(tx_fcs_error_cntr, TFCS, 12),
+	XGENE_EXTD_STAT(tx_ctrl_frame_cntr, TXCF, 12),
+	XGENE_EXTD_STAT(tx_oversize_frame_cntr, TOVR, 12),
+	XGENE_EXTD_STAT(tx_undersize_frame_cntr, TUND, 12),
+	XGENE_EXTD_STAT(tx_fragments_cntr, TFRG, 12),
+	XGENE_EXTD_STAT(tx_underrun_cntr, DUMP, 0)
+};
+
 #define XGENE_STATS_LEN		ARRAY_SIZE(gstrings_stats)
+#define XGENE_EXTD_STATS_LEN	ARRAY_SIZE(gstrings_extd_stats)
+#define RFCS_IDX		7
+#define RALN_IDX		13
+#define RFLR_IDX		14
+#define FALSE_RFLR_IDX		15
+#define RUND_IDX		18
+#define FALSE_RJBR_IDX		22
+#define RX_OVERRUN_IDX		24
+#define TFCS_IDX		38
+#define TFRG_IDX		42
+#define TX_UNDERRUN_IDX		43
 
 static void xgene_get_drvinfo(struct net_device *ndev,
 			      struct ethtool_drvinfo *info)
@@ -61,17 +127,21 @@ static int xgene_get_link_ksettings(struct net_device *ndev,
 	struct phy_device *phydev = ndev->phydev;
 	u32 supported;
 
-	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII) {
+	if (phy_interface_mode_is_rgmii(pdata->phy_mode)) {
 		if (phydev == NULL)
 			return -ENODEV;
 
-		return phy_ethtool_ksettings_get(phydev, cmd);
+		phy_ethtool_ksettings_get(phydev, cmd);
+
+		return 0;
 	} else if (pdata->phy_mode == PHY_INTERFACE_MODE_SGMII) {
 		if (pdata->mdio_driver) {
 			if (!phydev)
 				return -ENODEV;
 
-			return phy_ethtool_ksettings_get(phydev, cmd);
+			phy_ethtool_ksettings_get(phydev, cmd);
+
+			return 0;
 		}
 
 		supported = SUPPORTED_1000baseT_Full | SUPPORTED_Autoneg |
@@ -111,7 +181,7 @@ static int xgene_set_link_ksettings(struct net_device *ndev,
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
 	struct phy_device *phydev = ndev->phydev;
 
-	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII) {
+	if (phy_interface_mode_is_rgmii(pdata->phy_mode)) {
 		if (!phydev)
 			return -ENODEV;
 
@@ -142,6 +212,11 @@ static void xgene_get_strings(struct net_device *ndev, u32 stringset, u8 *data)
 		memcpy(p, gstrings_stats[i].name, ETH_GSTRING_LEN);
 		p += ETH_GSTRING_LEN;
 	}
+
+	for (i = 0; i < XGENE_EXTD_STATS_LEN; i++) {
+		memcpy(p, gstrings_extd_stats[i].name, ETH_GSTRING_LEN);
+		p += ETH_GSTRING_LEN;
+	}
 }
 
 static int xgene_get_sset_count(struct net_device *ndev, int sset)
@@ -149,18 +224,71 @@ static int xgene_get_sset_count(struct net_device *ndev, int sset)
 	if (sset != ETH_SS_STATS)
 		return -EINVAL;
 
-	return XGENE_STATS_LEN;
+	return XGENE_STATS_LEN + XGENE_EXTD_STATS_LEN;
+}
+
+static void xgene_get_extd_stats(struct xgene_enet_pdata *pdata)
+{
+	u32 rx_drop, tx_drop;
+	u32 mask, tmp;
+	int i;
+
+	for (i = 0; i < XGENE_EXTD_STATS_LEN; i++) {
+		tmp = xgene_enet_rd_stat(pdata, gstrings_extd_stats[i].addr);
+		if (gstrings_extd_stats[i].mask) {
+			mask = GENMASK(gstrings_extd_stats[i].mask - 1, 0);
+			pdata->extd_stats[i] += (tmp & mask);
+		}
+	}
+
+	if (pdata->phy_mode == PHY_INTERFACE_MODE_XGMII) {
+		/* Errata 10GE_10 - SW should intepret RALN as 0 */
+		pdata->extd_stats[RALN_IDX] = 0;
+	} else {
+		/* Errata ENET_15 - Fixes RFCS, RFLR, TFCS counter */
+		pdata->extd_stats[RFCS_IDX] -= pdata->extd_stats[RALN_IDX];
+		pdata->extd_stats[RFLR_IDX] -= pdata->extd_stats[RUND_IDX];
+		pdata->extd_stats[TFCS_IDX] -= pdata->extd_stats[TFRG_IDX];
+	}
+
+	pdata->mac_ops->get_drop_cnt(pdata, &rx_drop, &tx_drop);
+	pdata->extd_stats[RX_OVERRUN_IDX] += rx_drop;
+	pdata->extd_stats[TX_UNDERRUN_IDX] += tx_drop;
+
+	/* Errata 10GE_8 -  Update Frame recovered from Errata 10GE_8/ENET_11 */
+	pdata->extd_stats[FALSE_RFLR_IDX] = pdata->false_rflr;
+	/* Errata ENET_15 - Jabber Frame recov'ed from Errata 10GE_10/ENET_15 */
+	pdata->extd_stats[FALSE_RJBR_IDX] = pdata->vlan_rjbr;
+}
+
+int xgene_extd_stats_init(struct xgene_enet_pdata *pdata)
+{
+	pdata->extd_stats = devm_kmalloc_array(&pdata->pdev->dev,
+			XGENE_EXTD_STATS_LEN, sizeof(u64), GFP_KERNEL);
+	if (!pdata->extd_stats)
+		return -ENOMEM;
+
+	xgene_get_extd_stats(pdata);
+	memset(pdata->extd_stats, 0, XGENE_EXTD_STATS_LEN * sizeof(u64));
+
+	return 0;
 }
 
 static void xgene_get_ethtool_stats(struct net_device *ndev,
 				    struct ethtool_stats *dummy,
 				    u64 *data)
 {
-	void *pdata = netdev_priv(ndev);
+	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
+	struct rtnl_link_stats64 stats;
 	int i;
 
+	dev_get_stats(ndev, &stats);
 	for (i = 0; i < XGENE_STATS_LEN; i++)
-		*data++ = *(u64 *)(pdata + gstrings_stats[i].offset);
+		data[i] = *(u64 *)((char *)&stats + gstrings_stats[i].offset);
+
+	xgene_get_extd_stats(pdata);
+	for (i = 0; i < XGENE_EXTD_STATS_LEN; i++)
+		data[i + XGENE_STATS_LEN] = pdata->extd_stats[i];
 }
 
 static void xgene_get_pauseparam(struct net_device *ndev,
@@ -180,7 +308,7 @@ static int xgene_set_pauseparam(struct net_device *ndev,
 	struct phy_device *phydev = ndev->phydev;
 	u32 oldadv, newadv;
 
-	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII ||
+	if (phy_interface_mode_is_rgmii(pdata->phy_mode) ||
 	    pdata->phy_mode == PHY_INTERFACE_MODE_SGMII) {
 		if (!phydev)
 			return -EINVAL;
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
index 2a835e07adfb..e45b587c2994 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
@@ -205,30 +205,24 @@ static u32 xgene_enet_ring_len(struct xgene_enet_desc_ring *ring)
 }
 
 void xgene_enet_parse_error(struct xgene_enet_desc_ring *ring,
-			    struct xgene_enet_pdata *pdata,
 			    enum xgene_enet_err_code status)
 {
 	switch (status) {
 	case INGRESS_CRC:
 		ring->rx_crc_errors++;
-		ring->rx_dropped++;
 		break;
 	case INGRESS_CHECKSUM:
 	case INGRESS_CHECKSUM_COMPUTE:
 		ring->rx_errors++;
-		ring->rx_dropped++;
 		break;
 	case INGRESS_TRUNC_FRAME:
 		ring->rx_frame_errors++;
-		ring->rx_dropped++;
 		break;
 	case INGRESS_PKT_LEN:
 		ring->rx_length_errors++;
-		ring->rx_dropped++;
 		break;
 	case INGRESS_PKT_UNDER:
 		ring->rx_frame_errors++;
-		ring->rx_dropped++;
 		break;
 	case INGRESS_FIFO_OVERRUN:
 		ring->rx_fifo_errors++;
@@ -270,42 +264,39 @@ static void xgene_enet_wr_mcx_csr(struct xgene_enet_pdata *pdata,
 	iowrite32(val, addr);
 }
 
-static bool xgene_enet_wr_indirect(void __iomem *addr, void __iomem *wr,
-				   void __iomem *cmd, void __iomem *cmd_done,
-				   u32 wr_addr, u32 wr_data)
+void xgene_enet_wr_mac(struct xgene_enet_pdata *pdata, u32 wr_addr, u32 wr_data)
 {
-	u32 done;
+	void __iomem *addr, *wr, *cmd, *cmd_done;
+	struct net_device *ndev = pdata->ndev;
 	u8 wait = 10;
+	u32 done;
+
+	if (pdata->mdio_driver && ndev->phydev &&
+	    phy_interface_mode_is_rgmii(pdata->phy_mode)) {
+		struct mii_bus *bus = ndev->phydev->mdio.bus;
+
+		return xgene_mdio_wr_mac(bus->priv, wr_addr, wr_data);
+	}
+
+	addr = pdata->mcx_mac_addr + MAC_ADDR_REG_OFFSET;
+	wr = pdata->mcx_mac_addr + MAC_WRITE_REG_OFFSET;
+	cmd = pdata->mcx_mac_addr + MAC_COMMAND_REG_OFFSET;
+	cmd_done = pdata->mcx_mac_addr + MAC_COMMAND_DONE_REG_OFFSET;
 
+	spin_lock(&pdata->mac_lock);
 	iowrite32(wr_addr, addr);
 	iowrite32(wr_data, wr);
 	iowrite32(XGENE_ENET_WR_CMD, cmd);
 
-	/* wait for write command to complete */
 	while (!(done = ioread32(cmd_done)) && wait--)
 		udelay(1);
 
 	if (!done)
-		return false;
+		netdev_err(ndev, "mac write failed, addr: %04x data: %08x\n",
+			   wr_addr, wr_data);
 
 	iowrite32(0, cmd);
-
-	return true;
-}
-
-static void xgene_enet_wr_mcx_mac(struct xgene_enet_pdata *pdata,
-				  u32 wr_addr, u32 wr_data)
-{
-	void __iomem *addr, *wr, *cmd, *cmd_done;
-
-	addr = pdata->mcx_mac_addr + MAC_ADDR_REG_OFFSET;
-	wr = pdata->mcx_mac_addr + MAC_WRITE_REG_OFFSET;
-	cmd = pdata->mcx_mac_addr + MAC_COMMAND_REG_OFFSET;
-	cmd_done = pdata->mcx_mac_addr + MAC_COMMAND_DONE_REG_OFFSET;
-
-	if (!xgene_enet_wr_indirect(addr, wr, cmd, cmd_done, wr_addr, wr_data))
-		netdev_err(pdata->ndev, "MCX mac write failed, addr: %04x\n",
-			   wr_addr);
+	spin_unlock(&pdata->mac_lock);
 }
 
 static void xgene_enet_rd_csr(struct xgene_enet_pdata *pdata,
@@ -332,42 +323,69 @@ static void xgene_enet_rd_mcx_csr(struct xgene_enet_pdata *pdata,
 	*val = ioread32(addr);
 }
 
-static bool xgene_enet_rd_indirect(void __iomem *addr, void __iomem *rd,
-				   void __iomem *cmd, void __iomem *cmd_done,
-				   u32 rd_addr, u32 *rd_data)
+u32 xgene_enet_rd_mac(struct xgene_enet_pdata *pdata, u32 rd_addr)
 {
-	u32 done;
+	void __iomem *addr, *rd, *cmd, *cmd_done;
+	struct net_device *ndev = pdata->ndev;
+	u32 done, rd_data;
 	u8 wait = 10;
 
+	if (pdata->mdio_driver && ndev->phydev &&
+	    phy_interface_mode_is_rgmii(pdata->phy_mode)) {
+		struct mii_bus *bus = ndev->phydev->mdio.bus;
+
+		return xgene_mdio_rd_mac(bus->priv, rd_addr);
+	}
+
+	addr = pdata->mcx_mac_addr + MAC_ADDR_REG_OFFSET;
+	rd = pdata->mcx_mac_addr + MAC_READ_REG_OFFSET;
+	cmd = pdata->mcx_mac_addr + MAC_COMMAND_REG_OFFSET;
+	cmd_done = pdata->mcx_mac_addr + MAC_COMMAND_DONE_REG_OFFSET;
+
+	spin_lock(&pdata->mac_lock);
 	iowrite32(rd_addr, addr);
 	iowrite32(XGENE_ENET_RD_CMD, cmd);
 
-	/* wait for read command to complete */
 	while (!(done = ioread32(cmd_done)) && wait--)
 		udelay(1);
 
 	if (!done)
-		return false;
+		netdev_err(ndev, "mac read failed, addr: %04x\n", rd_addr);
 
-	*rd_data = ioread32(rd);
+	rd_data = ioread32(rd);
 	iowrite32(0, cmd);
+	spin_unlock(&pdata->mac_lock);
 
-	return true;
+	return rd_data;
 }
 
-static void xgene_enet_rd_mcx_mac(struct xgene_enet_pdata *pdata,
-				  u32 rd_addr, u32 *rd_data)
+u32 xgene_enet_rd_stat(struct xgene_enet_pdata *pdata, u32 rd_addr)
 {
 	void __iomem *addr, *rd, *cmd, *cmd_done;
+	u32 done, rd_data;
+	u8 wait = 10;
 
-	addr = pdata->mcx_mac_addr + MAC_ADDR_REG_OFFSET;
-	rd = pdata->mcx_mac_addr + MAC_READ_REG_OFFSET;
-	cmd = pdata->mcx_mac_addr + MAC_COMMAND_REG_OFFSET;
-	cmd_done = pdata->mcx_mac_addr + MAC_COMMAND_DONE_REG_OFFSET;
+	addr = pdata->mcx_stats_addr + STAT_ADDR_REG_OFFSET;
+	rd = pdata->mcx_stats_addr + STAT_READ_REG_OFFSET;
+	cmd = pdata->mcx_stats_addr + STAT_COMMAND_REG_OFFSET;
+	cmd_done = pdata->mcx_stats_addr + STAT_COMMAND_DONE_REG_OFFSET;
+
+	spin_lock(&pdata->stats_lock);
+	iowrite32(rd_addr, addr);
+	iowrite32(XGENE_ENET_RD_CMD, cmd);
 
-	if (!xgene_enet_rd_indirect(addr, rd, cmd, cmd_done, rd_addr, rd_data))
-		netdev_err(pdata->ndev, "MCX mac read failed, addr: %04x\n",
+	while (!(done = ioread32(cmd_done)) && wait--)
+		udelay(1);
+
+	if (!done)
+		netdev_err(pdata->ndev, "mac stats read failed, addr: %04x\n",
 			   rd_addr);
+
+	rd_data = ioread32(rd);
+	iowrite32(0, cmd);
+	spin_unlock(&pdata->stats_lock);
+
+	return rd_data;
 }
 
 static void xgene_gmac_set_mac_addr(struct xgene_enet_pdata *pdata)
@@ -379,8 +397,8 @@ static void xgene_gmac_set_mac_addr(struct xgene_enet_pdata *pdata)
 		(dev_addr[1] << 8) | dev_addr[0];
 	addr1 = (dev_addr[5] << 24) | (dev_addr[4] << 16);
 
-	xgene_enet_wr_mcx_mac(pdata, STATION_ADDR0_ADDR, addr0);
-	xgene_enet_wr_mcx_mac(pdata, STATION_ADDR1_ADDR, addr1);
+	xgene_enet_wr_mac(pdata, STATION_ADDR0_ADDR, addr0);
+	xgene_enet_wr_mac(pdata, STATION_ADDR1_ADDR, addr1);
 }
 
 static int xgene_enet_ecc_init(struct xgene_enet_pdata *pdata)
@@ -405,8 +423,8 @@ static int xgene_enet_ecc_init(struct xgene_enet_pdata *pdata)
 
 static void xgene_gmac_reset(struct xgene_enet_pdata *pdata)
 {
-	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, SOFT_RESET1);
-	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, 0);
+	xgene_enet_wr_mac(pdata, MAC_CONFIG_1_ADDR, SOFT_RESET1);
+	xgene_enet_wr_mac(pdata, MAC_CONFIG_1_ADDR, 0);
 }
 
 static void xgene_enet_configure_clock(struct xgene_enet_pdata *pdata)
@@ -456,8 +474,8 @@ static void xgene_gmac_set_speed(struct xgene_enet_pdata *pdata)
 
 	xgene_enet_rd_mcx_csr(pdata, ICM_CONFIG0_REG_0_ADDR, &icm0);
 	xgene_enet_rd_mcx_csr(pdata, ICM_CONFIG2_REG_0_ADDR, &icm2);
-	xgene_enet_rd_mcx_mac(pdata, MAC_CONFIG_2_ADDR, &mc2);
-	xgene_enet_rd_mcx_mac(pdata, INTERFACE_CONTROL_ADDR, &intf_ctl);
+	mc2 = xgene_enet_rd_mac(pdata, MAC_CONFIG_2_ADDR);
+	intf_ctl = xgene_enet_rd_mac(pdata, INTERFACE_CONTROL_ADDR);
 	xgene_enet_rd_csr(pdata, RGMII_REG_0_ADDR, &rgmii);
 
 	switch (pdata->phy_speed) {
@@ -495,8 +513,8 @@ static void xgene_gmac_set_speed(struct xgene_enet_pdata *pdata)
 	}
 
 	mc2 |= FULL_DUPLEX2 | PAD_CRC | LENGTH_CHK;
-	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_2_ADDR, mc2);
-	xgene_enet_wr_mcx_mac(pdata, INTERFACE_CONTROL_ADDR, intf_ctl);
+	xgene_enet_wr_mac(pdata, MAC_CONFIG_2_ADDR, mc2);
+	xgene_enet_wr_mac(pdata, INTERFACE_CONTROL_ADDR, intf_ctl);
 	xgene_enet_wr_csr(pdata, RGMII_REG_0_ADDR, rgmii);
 	xgene_enet_configure_clock(pdata);
 
@@ -506,7 +524,7 @@ static void xgene_gmac_set_speed(struct xgene_enet_pdata *pdata)
 
 static void xgene_enet_set_frame_size(struct xgene_enet_pdata *pdata, int size)
 {
-	xgene_enet_wr_mcx_mac(pdata, MAX_FRAME_LEN_ADDR, size);
+	xgene_enet_wr_mac(pdata, MAX_FRAME_LEN_ADDR, size);
 }
 
 static void xgene_gmac_enable_tx_pause(struct xgene_enet_pdata *pdata,
@@ -528,14 +546,14 @@ static void xgene_gmac_flowctl_tx(struct xgene_enet_pdata *pdata, bool enable)
 {
 	u32 data;
 
-	xgene_enet_rd_mcx_mac(pdata, MAC_CONFIG_1_ADDR, &data);
+	data = xgene_enet_rd_mac(pdata, MAC_CONFIG_1_ADDR);
 
 	if (enable)
 		data |= TX_FLOW_EN;
 	else
 		data &= ~TX_FLOW_EN;
 
-	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, data);
+	xgene_enet_wr_mac(pdata, MAC_CONFIG_1_ADDR, data);
 
 	pdata->mac_ops->enable_tx_pause(pdata, enable);
 }
@@ -544,14 +562,14 @@ static void xgene_gmac_flowctl_rx(struct xgene_enet_pdata *pdata, bool enable)
 {
 	u32 data;
 
-	xgene_enet_rd_mcx_mac(pdata, MAC_CONFIG_1_ADDR, &data);
+	data = xgene_enet_rd_mac(pdata, MAC_CONFIG_1_ADDR);
 
 	if (enable)
 		data |= RX_FLOW_EN;
 	else
 		data &= ~RX_FLOW_EN;
 
-	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, data);
+	xgene_enet_wr_mac(pdata, MAC_CONFIG_1_ADDR, data);
 }
 
 static void xgene_gmac_init(struct xgene_enet_pdata *pdata)
@@ -565,9 +583,9 @@ static void xgene_gmac_init(struct xgene_enet_pdata *pdata)
 	xgene_gmac_set_mac_addr(pdata);
 
 	/* Adjust MDC clock frequency */
-	xgene_enet_rd_mcx_mac(pdata, MII_MGMT_CONFIG_ADDR, &value);
+	value = xgene_enet_rd_mac(pdata, MII_MGMT_CONFIG_ADDR);
 	MGMT_CLOCK_SEL_SET(&value, 7);
-	xgene_enet_wr_mcx_mac(pdata, MII_MGMT_CONFIG_ADDR, value);
+	xgene_enet_wr_mac(pdata, MII_MGMT_CONFIG_ADDR, value);
 
 	/* Enable drop if bufpool not available */
 	xgene_enet_rd_csr(pdata, RSIF_CONFIG_REG_ADDR, &value);
@@ -600,6 +618,18 @@ static void xgene_gmac_init(struct xgene_enet_pdata *pdata)
 	xgene_enet_wr_csr(pdata, CFG_BYPASS_ADDR, RESUME_TX);
 }
 
+static void xgene_gmac_get_drop_cnt(struct xgene_enet_pdata *pdata,
+				    u32 *rx, u32 *tx)
+{
+	u32 count;
+
+	xgene_enet_rd_mcx_csr(pdata, ICM_ECM_DROP_COUNT_REG0_ADDR, &count);
+	*rx = ICM_DROP_COUNT(count);
+	*tx = ECM_DROP_COUNT(count);
+	/* Errata: 10GE_4 - Fix ICM_ECM_DROP_COUNT not clear-on-read */
+	xgene_enet_rd_mcx_csr(pdata, ECM_CONFIG0_REG_0_ADDR, &count);
+}
+
 static void xgene_enet_config_ring_if_assoc(struct xgene_enet_pdata *pdata)
 {
 	u32 val = 0xffffffff;
@@ -637,32 +667,32 @@ static void xgene_gmac_rx_enable(struct xgene_enet_pdata *pdata)
 {
 	u32 data;
 
-	xgene_enet_rd_mcx_mac(pdata, MAC_CONFIG_1_ADDR, &data);
-	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, data | RX_EN);
+	data = xgene_enet_rd_mac(pdata, MAC_CONFIG_1_ADDR);
+	xgene_enet_wr_mac(pdata, MAC_CONFIG_1_ADDR, data | RX_EN);
 }
 
 static void xgene_gmac_tx_enable(struct xgene_enet_pdata *pdata)
 {
 	u32 data;
 
-	xgene_enet_rd_mcx_mac(pdata, MAC_CONFIG_1_ADDR, &data);
-	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, data | TX_EN);
+	data = xgene_enet_rd_mac(pdata, MAC_CONFIG_1_ADDR);
+	xgene_enet_wr_mac(pdata, MAC_CONFIG_1_ADDR, data | TX_EN);
 }
 
 static void xgene_gmac_rx_disable(struct xgene_enet_pdata *pdata)
 {
 	u32 data;
 
-	xgene_enet_rd_mcx_mac(pdata, MAC_CONFIG_1_ADDR, &data);
-	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, data & ~RX_EN);
+	data = xgene_enet_rd_mac(pdata, MAC_CONFIG_1_ADDR);
+	xgene_enet_wr_mac(pdata, MAC_CONFIG_1_ADDR, data & ~RX_EN);
 }
 
 static void xgene_gmac_tx_disable(struct xgene_enet_pdata *pdata)
 {
 	u32 data;
 
-	xgene_enet_rd_mcx_mac(pdata, MAC_CONFIG_1_ADDR, &data);
-	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, data & ~TX_EN);
+	data = xgene_enet_rd_mac(pdata, MAC_CONFIG_1_ADDR);
+	xgene_enet_wr_mac(pdata, MAC_CONFIG_1_ADDR, data & ~TX_EN);
 }
 
 bool xgene_ring_mgr_init(struct xgene_enet_pdata *p)
@@ -733,27 +763,6 @@ static void xgene_enet_clear(struct xgene_enet_pdata *pdata,
 static void xgene_gport_shutdown(struct xgene_enet_pdata *pdata)
 {
 	struct device *dev = &pdata->pdev->dev;
-	struct xgene_enet_desc_ring *ring;
-	u32 pb;
-	int i;
-
-	pb = 0;
-	for (i = 0; i < pdata->rxq_cnt; i++) {
-		ring = pdata->rx_ring[i]->buf_pool;
-		pb |= BIT(xgene_enet_get_fpsel(ring->id));
-		ring = pdata->rx_ring[i]->page_pool;
-		if (ring)
-			pb |= BIT(xgene_enet_get_fpsel(ring->id));
-
-	}
-	xgene_enet_wr_ring_if(pdata, ENET_CFGSSQMIFPRESET_ADDR, pb);
-
-	pb = 0;
-	for (i = 0; i < pdata->txq_cnt; i++) {
-		ring = pdata->tx_ring[i];
-		pb |= BIT(xgene_enet_ring_bufnum(ring->id));
-	}
-	xgene_enet_wr_ring_if(pdata, ENET_CFGSSQMIWQRESET_ADDR, pb);
 
 	if (dev->of_node) {
 		if (!IS_ERR(pdata->clk))
@@ -1009,6 +1018,7 @@ const struct xgene_mac_ops xgene_gmac_ops = {
 	.tx_enable = xgene_gmac_tx_enable,
 	.rx_disable = xgene_gmac_rx_disable,
 	.tx_disable = xgene_gmac_tx_disable,
+	.get_drop_cnt = xgene_gmac_get_drop_cnt,
 	.set_speed = xgene_gmac_set_speed,
 	.set_mac_addr = xgene_gmac_set_mac_addr,
 	.set_framesize = xgene_enet_set_frame_size,
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
index d250bfe94d24..5d3e18d3c94c 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
@@ -115,6 +115,7 @@ enum xgene_enet_rm {
 #define BLOCK_ETH_CLKRST_CSR_OFFSET	0xc000
 #define BLOCK_ETH_DIAG_CSR_OFFSET	0xD000
 #define BLOCK_ETH_MAC_OFFSET		0x0000
+#define BLOCK_ETH_STATS_OFFSET		0x0000
 #define BLOCK_ETH_MAC_CSR_OFFSET	0x2800
 
 #define CLKEN_ADDR			0xc208
@@ -126,6 +127,12 @@ enum xgene_enet_rm {
 #define MAC_READ_REG_OFFSET		0x0c
 #define MAC_COMMAND_DONE_REG_OFFSET	0x10
 
+#define STAT_ADDR_REG_OFFSET            0x14
+#define STAT_COMMAND_REG_OFFSET         0x18
+#define STAT_WRITE_REG_OFFSET           0x1c
+#define STAT_READ_REG_OFFSET            0x20
+#define STAT_COMMAND_DONE_REG_OFFSET    0x24
+
 #define PCS_ADDR_REG_OFFSET		0x00
 #define PCS_COMMAND_REG_OFFSET		0x04
 #define PCS_WRITE_REG_OFFSET		0x08
@@ -185,6 +192,10 @@ enum xgene_enet_rm {
 #define CFG_CLE_NXTFPSEL0(val)		(((val) << 20) & GENMASK(23, 20))
 #define ICM_CONFIG0_REG_0_ADDR		0x0400
 #define ICM_CONFIG2_REG_0_ADDR		0x0410
+#define ECM_CONFIG0_REG_0_ADDR		0x0500
+#define ECM_CONFIG0_REG_1_ADDR		0x0504
+#define ICM_ECM_DROP_COUNT_REG0_ADDR	0x0508
+#define ICM_ECM_DROP_COUNT_REG1_ADDR	0x050c
 #define RX_DV_GATE_REG_0_ADDR		0x05fc
 #define TX_DV_GATE_EN0			BIT(2)
 #define RX_DV_GATE_EN0			BIT(1)
@@ -217,12 +228,53 @@ enum xgene_enet_rm {
 #define FULL_DUPLEX2			BIT(0)
 #define PAD_CRC				BIT(2)
 #define LENGTH_CHK			BIT(4)
-#define SCAN_AUTO_INCR			BIT(5)
-#define TBYT_ADDR			0x38
-#define TPKT_ADDR			0x39
-#define TDRP_ADDR			0x45
-#define TFCS_ADDR			0x47
-#define TUND_ADDR			0x4a
+
+#define TR64_ADDR	0x20
+#define TR127_ADDR	0x21
+#define TR255_ADDR	0x22
+#define TR511_ADDR	0x23
+#define TR1K_ADDR	0x24
+#define TRMAX_ADDR	0x25
+#define TRMGV_ADDR	0x26
+
+#define RFCS_ADDR	0x29
+#define RMCA_ADDR	0x2a
+#define RBCA_ADDR	0x2b
+#define RXCF_ADDR	0x2c
+#define RXPF_ADDR	0x2d
+#define RXUO_ADDR	0x2e
+#define RALN_ADDR	0x2f
+#define RFLR_ADDR	0x30
+#define RCDE_ADDR	0x31
+#define RCSE_ADDR	0x32
+#define RUND_ADDR	0x33
+#define ROVR_ADDR	0x34
+#define RFRG_ADDR	0x35
+#define RJBR_ADDR	0x36
+#define RDRP_ADDR	0x37
+
+#define TMCA_ADDR	0x3a
+#define TBCA_ADDR	0x3b
+#define TXPF_ADDR	0x3c
+#define TDFR_ADDR	0x3d
+#define TEDF_ADDR	0x3e
+#define TSCL_ADDR	0x3f
+#define TMCL_ADDR	0x40
+#define TLCL_ADDR	0x41
+#define TXCL_ADDR	0x42
+#define TNCL_ADDR	0x43
+#define TPFH_ADDR	0x44
+#define TDRP_ADDR	0x45
+#define TJBR_ADDR	0x46
+#define TFCS_ADDR	0x47
+#define TXCF_ADDR	0x48
+#define TOVR_ADDR	0x49
+#define TUND_ADDR	0x4a
+#define TFRG_ADDR	0x4b
+#define DUMP_ADDR	0x27
+
+#define ECM_DROP_COUNT(src)	xgene_get_bits(src, 0, 15)
+#define ICM_DROP_COUNT(src)	xgene_get_bits(src, 16, 31)
 
 #define TSO_IPPROTO_TCP			1
 
@@ -380,14 +432,16 @@ static inline u16 xgene_enet_get_numslots(u16 id, u32 size)
 }
 
 void xgene_enet_parse_error(struct xgene_enet_desc_ring *ring,
-			    struct xgene_enet_pdata *pdata,
 			    enum xgene_enet_err_code status);
-
 int xgene_enet_mdio_config(struct xgene_enet_pdata *pdata);
 void xgene_enet_mdio_remove(struct xgene_enet_pdata *pdata);
 bool xgene_ring_mgr_init(struct xgene_enet_pdata *p);
 int xgene_enet_phy_connect(struct net_device *ndev);
 void xgene_enet_phy_disconnect(struct xgene_enet_pdata *pdata);
+u32 xgene_enet_rd_mac(struct xgene_enet_pdata *pdata, u32 rd_addr);
+void xgene_enet_wr_mac(struct xgene_enet_pdata *pdata, u32 wr_addr,
+		       u32 wr_data);
+u32 xgene_enet_rd_stat(struct xgene_enet_pdata *pdata, u32 rd_addr);
 
 extern const struct xgene_mac_ops xgene_gmac_ops;
 extern const struct xgene_port_ops xgene_gport_ops;
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
index 5f37ed3506d5..d3906f6b01bd 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
@@ -246,9 +246,9 @@ static int xgene_enet_tx_completion(struct xgene_enet_desc_ring *cp_ring,
 	skb_frag_t *frag;
 	dma_addr_t *frag_dma_addr;
 	u16 skb_index;
-	u8 status;
-	int i, ret = 0;
 	u8 mss_index;
+	u8 status;
+	int i;
 
 	skb_index = GET_VAL(USERINFO, le64_to_cpu(raw_desc->m0));
 	skb = cp_ring->cp_skb[skb_index];
@@ -275,19 +275,17 @@ static int xgene_enet_tx_completion(struct xgene_enet_desc_ring *cp_ring,
 	/* Checking for error */
 	status = GET_VAL(LERR, le64_to_cpu(raw_desc->m0));
 	if (unlikely(status > 2)) {
-		xgene_enet_parse_error(cp_ring, netdev_priv(cp_ring->ndev),
-				       status);
-		ret = -EIO;
+		cp_ring->tx_dropped++;
+		cp_ring->tx_errors++;
 	}
 
 	if (likely(skb)) {
 		dev_kfree_skb_any(skb);
 	} else {
 		netdev_err(cp_ring->ndev, "completion skb is NULL\n");
-		ret = -EIO;
 	}
 
-	return ret;
+	return 0;
 }
 
 static int xgene_enet_setup_mss(struct net_device *ndev, u32 mss)
@@ -658,6 +656,18 @@ static void xgene_enet_free_pagepool(struct xgene_enet_desc_ring *buf_pool,
 	buf_pool->head = head;
 }
 
+/* Errata 10GE_10 and ENET_15 - Fix duplicated HW statistic counters */
+static bool xgene_enet_errata_10GE_10(struct sk_buff *skb, u32 len, u8 status)
+{
+	if (status == INGRESS_CRC &&
+	    len >= (ETHER_STD_PACKET + 1) &&
+	    len <= (ETHER_STD_PACKET + 4) &&
+	    skb->protocol == htons(ETH_P_8021Q))
+		return true;
+
+	return false;
+}
+
 /* Errata 10GE_8 and ENET_11 - allow packet with length <=64B */
 static bool xgene_enet_errata_10GE_8(struct sk_buff *skb, u32 len, u8 status)
 {
@@ -708,10 +718,15 @@ static int xgene_enet_rx_frame(struct xgene_enet_desc_ring *rx_ring,
 	status = (GET_VAL(ELERR, le64_to_cpu(raw_desc->m0)) << LERR_LEN) |
 		  GET_VAL(LERR, le64_to_cpu(raw_desc->m0));
 	if (unlikely(status)) {
-		if (!xgene_enet_errata_10GE_8(skb, datalen, status)) {
+		if (xgene_enet_errata_10GE_8(skb, datalen, status)) {
+			pdata->false_rflr++;
+		} else if (xgene_enet_errata_10GE_10(skb, datalen, status)) {
+			pdata->vlan_rjbr++;
+		} else {
 			dev_kfree_skb_any(skb);
 			xgene_enet_free_pagepool(page_pool, raw_desc, exp_desc);
-			xgene_enet_parse_error(rx_ring, pdata, status);
+			xgene_enet_parse_error(rx_ring, status);
+			rx_ring->rx_dropped++;
 			goto out;
 		}
 	}
@@ -1466,10 +1481,9 @@ err:
 
 static void xgene_enet_get_stats64(
 			struct net_device *ndev,
-			struct rtnl_link_stats64 *storage)
+			struct rtnl_link_stats64 *stats)
 {
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
-	struct rtnl_link_stats64 *stats = &pdata->stats;
 	struct xgene_enet_desc_ring *ring;
 	int i;
 
@@ -1478,6 +1492,8 @@ static void xgene_enet_get_stats64(
 		if (ring) {
 			stats->tx_packets += ring->tx_packets;
 			stats->tx_bytes += ring->tx_bytes;
+			stats->tx_dropped += ring->tx_dropped;
+			stats->tx_errors += ring->tx_errors;
 		}
 	}
 
@@ -1486,14 +1502,18 @@ static void xgene_enet_get_stats64(
 		if (ring) {
 			stats->rx_packets += ring->rx_packets;
 			stats->rx_bytes += ring->rx_bytes;
-			stats->rx_errors += ring->rx_length_errors +
+			stats->rx_dropped += ring->rx_dropped;
+			stats->rx_errors += ring->rx_errors +
+				ring->rx_length_errors +
 				ring->rx_crc_errors +
 				ring->rx_frame_errors +
 				ring->rx_fifo_errors;
-			stats->rx_dropped += ring->rx_dropped;
+			stats->rx_length_errors += ring->rx_length_errors;
+			stats->rx_crc_errors += ring->rx_crc_errors;
+			stats->rx_frame_errors += ring->rx_frame_errors;
+			stats->rx_fifo_errors += ring->rx_fifo_errors;
 		}
 	}
-	memcpy(storage, stats, sizeof(struct rtnl_link_stats64));
 }
 
 static int xgene_enet_set_mac_address(struct net_device *ndev, void *addr)
@@ -1614,7 +1634,7 @@ static int xgene_enet_get_irqs(struct xgene_enet_pdata *pdata)
 	struct device *dev = &pdev->dev;
 	int i, ret, max_irqs;
 
-	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII)
+	if (phy_interface_mode_is_rgmii(pdata->phy_mode))
 		max_irqs = 1;
 	else if (pdata->phy_mode == PHY_INTERFACE_MODE_SGMII)
 		max_irqs = 2;
@@ -1740,7 +1760,7 @@ static int xgene_enet_get_resources(struct xgene_enet_pdata *pdata)
 		dev_err(dev, "Unable to get phy-connection-type\n");
 		return pdata->phy_mode;
 	}
-	if (pdata->phy_mode != PHY_INTERFACE_MODE_RGMII &&
+	if (!phy_interface_mode_is_rgmii(pdata->phy_mode) &&
 	    pdata->phy_mode != PHY_INTERFACE_MODE_SGMII &&
 	    pdata->phy_mode != PHY_INTERFACE_MODE_XGMII) {
 		dev_err(dev, "Incorrect phy-connection-type specified\n");
@@ -1785,15 +1805,18 @@ static int xgene_enet_get_resources(struct xgene_enet_pdata *pdata)
 	pdata->cle.base = base_addr + BLOCK_ETH_CLE_CSR_OFFSET;
 	pdata->eth_ring_if_addr = base_addr + BLOCK_ETH_RING_IF_OFFSET;
 	pdata->eth_diag_csr_addr = base_addr + BLOCK_ETH_DIAG_CSR_OFFSET;
-	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII ||
+	if (phy_interface_mode_is_rgmii(pdata->phy_mode) ||
 	    pdata->phy_mode == PHY_INTERFACE_MODE_SGMII) {
 		pdata->mcx_mac_addr = pdata->base_addr + BLOCK_ETH_MAC_OFFSET;
+		pdata->mcx_stats_addr =
+			pdata->base_addr + BLOCK_ETH_STATS_OFFSET;
 		offset = (pdata->enet_id == XGENE_ENET1) ?
 			  BLOCK_ETH_MAC_CSR_OFFSET :
 			  X2_BLOCK_ETH_MAC_CSR_OFFSET;
 		pdata->mcx_mac_csr_addr = base_addr + offset;
 	} else {
 		pdata->mcx_mac_addr = base_addr + BLOCK_AXG_MAC_OFFSET;
+		pdata->mcx_stats_addr = base_addr + BLOCK_AXG_STATS_OFFSET;
 		pdata->mcx_mac_csr_addr = base_addr + BLOCK_AXG_MAC_CSR_OFFSET;
 		pdata->pcs_addr = base_addr + BLOCK_PCS_OFFSET;
 	}
@@ -1881,6 +1904,9 @@ static void xgene_enet_setup_ops(struct xgene_enet_pdata *pdata)
 {
 	switch (pdata->phy_mode) {
 	case PHY_INTERFACE_MODE_RGMII:
+	case PHY_INTERFACE_MODE_RGMII_ID:
+	case PHY_INTERFACE_MODE_RGMII_RXID:
+	case PHY_INTERFACE_MODE_RGMII_TXID:
 		pdata->mac_ops = &xgene_gmac_ops;
 		pdata->port_ops = &xgene_gport_ops;
 		pdata->rm = RM3;
@@ -2055,6 +2081,7 @@ static int xgene_enet_probe(struct platform_device *pdev)
 		goto err;
 
 	xgene_enet_setup_ops(pdata);
+	spin_lock_init(&pdata->mac_lock);
 
 	if (pdata->phy_mode == PHY_INTERFACE_MODE_XGMII) {
 		ndev->features |= NETIF_F_TSO | NETIF_F_RXCSUM;
@@ -2076,7 +2103,7 @@ static int xgene_enet_probe(struct platform_device *pdev)
 	if (pdata->phy_mode == PHY_INTERFACE_MODE_XGMII) {
 		INIT_DELAYED_WORK(&pdata->link_work, link_state);
 	} else if (!pdata->mdio_driver) {
-		if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII)
+		if (phy_interface_mode_is_rgmii(pdata->phy_mode))
 			ret = xgene_enet_mdio_config(pdata);
 		else
 			INIT_DELAYED_WORK(&pdata->link_work, link_state);
@@ -2085,6 +2112,11 @@ static int xgene_enet_probe(struct platform_device *pdev)
 			goto err1;
 	}
 
+	spin_lock_init(&pdata->stats_lock);
+	ret = xgene_extd_stats_init(pdata);
+	if (ret)
+		goto err2;
+
 	xgene_enet_napi_add(pdata);
 	ret = register_netdev(ndev);
 	if (ret) {
@@ -2102,7 +2134,7 @@ err2:
 
 	if (pdata->mdio_driver)
 		xgene_enet_phy_disconnect(pdata);
-	else if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII)
+	else if (phy_interface_mode_is_rgmii(pdata->phy_mode))
 		xgene_enet_mdio_remove(pdata);
 err1:
 	xgene_enet_delete_desc_rings(pdata);
@@ -2126,12 +2158,12 @@ static int xgene_enet_remove(struct platform_device *pdev)
 
 	if (pdata->mdio_driver)
 		xgene_enet_phy_disconnect(pdata);
-	else if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII)
+	else if (phy_interface_mode_is_rgmii(pdata->phy_mode))
 		xgene_enet_mdio_remove(pdata);
 
 	unregister_netdev(ndev);
-	pdata->port_ops->shutdown(pdata);
 	xgene_enet_delete_desc_rings(pdata);
+	pdata->port_ops->shutdown(pdata);
 	free_netdev(ndev);
 
 	return 0;
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_main.h b/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
index 0d4be2425ebc..985768596900 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
@@ -42,6 +42,7 @@
 
 #define XGENE_DRV_VERSION	"v1.0"
 #define ETHER_MIN_PACKET	64
+#define ETHER_STD_PACKET	1518
 #define XGENE_ENET_STD_MTU	1536
 #define XGENE_ENET_MAX_MTU	9600
 #define SKB_BUFFER_SIZE		(XGENE_ENET_STD_MTU - NET_IP_ALIGN)
@@ -138,6 +139,8 @@ struct xgene_enet_desc_ring {
 	__le64 *exp_bufs;
 	u64 tx_packets;
 	u64 tx_bytes;
+	u64 tx_dropped;
+	u64 tx_errors;
 	u64 rx_packets;
 	u64 rx_bytes;
 	u64 rx_dropped;
@@ -155,6 +158,7 @@ struct xgene_mac_ops {
 	void (*rx_enable)(struct xgene_enet_pdata *pdata);
 	void (*tx_disable)(struct xgene_enet_pdata *pdata);
 	void (*rx_disable)(struct xgene_enet_pdata *pdata);
+	void (*get_drop_cnt)(struct xgene_enet_pdata *pdata, u32 *rx, u32 *tx);
 	void (*set_speed)(struct xgene_enet_pdata *pdata);
 	void (*set_mac_addr)(struct xgene_enet_pdata *pdata);
 	void (*set_framesize)(struct xgene_enet_pdata *pdata, int framesize);
@@ -212,6 +216,7 @@ struct xgene_enet_pdata {
 	void __iomem *eth_diag_csr_addr;
 	void __iomem *mcx_mac_addr;
 	void __iomem *mcx_mac_csr_addr;
+	void __iomem *mcx_stats_addr;
 	void __iomem *base_addr;
 	void __iomem *pcs_addr;
 	void __iomem *ring_csr_addr;
@@ -219,8 +224,12 @@ struct xgene_enet_pdata {
 	int phy_mode;
 	enum xgene_enet_rm rm;
 	struct xgene_enet_cle cle;
-	struct rtnl_link_stats64 stats;
+	u64 *extd_stats;
+	u64 false_rflr;
+	u64 vlan_rjbr;
+	spinlock_t stats_lock; /* statistics lock */
 	const struct xgene_mac_ops *mac_ops;
+	spinlock_t mac_lock; /* mac lock */
 	const struct xgene_port_ops *port_ops;
 	struct xgene_ring_ops *ring_ops;
 	const struct xgene_cle_ops *cle_ops;
@@ -263,5 +272,6 @@ static inline u16 xgene_enet_dst_ring_num(struct xgene_enet_desc_ring *ring)
 }
 
 void xgene_enet_set_ethtool_ops(struct net_device *netdev);
+int xgene_extd_stats_init(struct xgene_enet_pdata *pdata);
 
 #endif /* __XGENE_ENET_MAIN_H__ */
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
index a8e063bdee3b..b1a83fdbefb8 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
@@ -54,41 +54,6 @@ static void xgene_enet_wr_mcx_csr(struct xgene_enet_pdata *pdata,
 	iowrite32(val, addr);
 }
 
-static bool xgene_enet_wr_indirect(struct xgene_indirect_ctl *ctl,
-				   u32 wr_addr, u32 wr_data)
-{
-	int i;
-
-	iowrite32(wr_addr, ctl->addr);
-	iowrite32(wr_data, ctl->ctl);
-	iowrite32(XGENE_ENET_WR_CMD, ctl->cmd);
-
-	/* wait for write command to complete */
-	for (i = 0; i < 10; i++) {
-		if (ioread32(ctl->cmd_done)) {
-			iowrite32(0, ctl->cmd);
-			return true;
-		}
-		udelay(1);
-	}
-
-	return false;
-}
-
-static void xgene_enet_wr_mac(struct xgene_enet_pdata *p,
-			      u32 wr_addr, u32 wr_data)
-{
-	struct xgene_indirect_ctl ctl = {
-		.addr = p->mcx_mac_addr + MAC_ADDR_REG_OFFSET,
-		.ctl = p->mcx_mac_addr + MAC_WRITE_REG_OFFSET,
-		.cmd = p->mcx_mac_addr + MAC_COMMAND_REG_OFFSET,
-		.cmd_done = p->mcx_mac_addr + MAC_COMMAND_DONE_REG_OFFSET
-	};
-
-	if (!xgene_enet_wr_indirect(&ctl, wr_addr, wr_data))
-		netdev_err(p->ndev, "mac write failed, addr: %04x\n", wr_addr);
-}
-
 static u32 xgene_enet_rd_csr(struct xgene_enet_pdata *p, u32 offset)
 {
 	return ioread32(p->eth_csr_addr + offset);
@@ -104,42 +69,6 @@ static u32 xgene_enet_rd_mcx_csr(struct xgene_enet_pdata *p, u32 offset)
 	return ioread32(p->mcx_mac_csr_addr + offset);
 }
 
-static u32 xgene_enet_rd_indirect(struct xgene_indirect_ctl *ctl, u32 rd_addr)
-{
-	u32 rd_data;
-	int i;
-
-	iowrite32(rd_addr, ctl->addr);
-	iowrite32(XGENE_ENET_RD_CMD, ctl->cmd);
-
-	/* wait for read command to complete */
-	for (i = 0; i < 10; i++) {
-		if (ioread32(ctl->cmd_done)) {
-			rd_data = ioread32(ctl->ctl);
-			iowrite32(0, ctl->cmd);
-
-			return rd_data;
-		}
-		udelay(1);
-	}
-
-	pr_err("%s: mac read failed, addr: %04x\n", __func__, rd_addr);
-
-	return 0;
-}
-
-static u32 xgene_enet_rd_mac(struct xgene_enet_pdata *p, u32 rd_addr)
-{
-	struct xgene_indirect_ctl ctl = {
-		.addr = p->mcx_mac_addr + MAC_ADDR_REG_OFFSET,
-		.ctl = p->mcx_mac_addr + MAC_READ_REG_OFFSET,
-		.cmd = p->mcx_mac_addr + MAC_COMMAND_REG_OFFSET,
-		.cmd_done = p->mcx_mac_addr + MAC_COMMAND_DONE_REG_OFFSET
-	};
-
-	return xgene_enet_rd_indirect(&ctl, rd_addr);
-}
-
 static int xgene_enet_ecc_init(struct xgene_enet_pdata *p)
 {
 	struct net_device *ndev = p->ndev;
@@ -166,6 +95,24 @@ static int xgene_enet_ecc_init(struct xgene_enet_pdata *p)
 	return -ENODEV;
 }
 
+static void xgene_sgmac_get_drop_cnt(struct xgene_enet_pdata *pdata,
+				     u32 *rx, u32 *tx)
+{
+	u32 addr, count;
+
+	addr = (pdata->enet_id != XGENE_ENET1) ?
+		XG_MCX_ICM_ECM_DROP_COUNT_REG0_ADDR :
+		ICM_ECM_DROP_COUNT_REG0_ADDR + pdata->port_id * OFFSET_4;
+	count = xgene_enet_rd_mcx_csr(pdata, addr);
+	*rx = ICM_DROP_COUNT(count);
+	*tx = ECM_DROP_COUNT(count);
+	/* Errata: 10GE_4 - ICM_ECM_DROP_COUNT not clear-on-read */
+	addr = (pdata->enet_id != XGENE_ENET1) ?
+		XG_MCX_ECM_CONFIG0_REG_0_ADDR :
+		ECM_CONFIG0_REG_0_ADDR + pdata->port_id * OFFSET_4;
+	xgene_enet_rd_mcx_csr(pdata, addr);
+}
+
 static void xgene_enet_config_ring_if_assoc(struct xgene_enet_pdata *p)
 {
 	u32 val;
@@ -587,26 +534,6 @@ static void xgene_enet_clear(struct xgene_enet_pdata *pdata,
 static void xgene_enet_shutdown(struct xgene_enet_pdata *p)
 {
 	struct device *dev = &p->pdev->dev;
-	struct xgene_enet_desc_ring *ring;
-	u32 pb;
-	int i;
-
-	pb = 0;
-	for (i = 0; i < p->rxq_cnt; i++) {
-		ring = p->rx_ring[i]->buf_pool;
-		pb |= BIT(xgene_enet_get_fpsel(ring->id));
-		ring = p->rx_ring[i]->page_pool;
-		if (ring)
-			pb |= BIT(xgene_enet_get_fpsel(ring->id));
-	}
-	xgene_enet_wr_ring_if(p, ENET_CFGSSQMIFPRESET_ADDR, pb);
-
-	pb = 0;
-	for (i = 0; i < p->txq_cnt; i++) {
-		ring = p->tx_ring[i];
-		pb |= BIT(xgene_enet_ring_bufnum(ring->id));
-	}
-	xgene_enet_wr_ring_if(p, ENET_CFGSSQMIWQRESET_ADDR, pb);
 
 	if (dev->of_node) {
 		if (!IS_ERR(p->clk))
@@ -671,6 +598,7 @@ const struct xgene_mac_ops xgene_sgmac_ops = {
 	.tx_enable	= xgene_sgmac_tx_enable,
 	.rx_disable	= xgene_sgmac_rx_disable,
 	.tx_disable	= xgene_sgmac_tx_disable,
+	.get_drop_cnt   = xgene_sgmac_get_drop_cnt,
 	.set_speed	= xgene_sgmac_set_speed,
 	.set_mac_addr	= xgene_sgmac_set_mac_addr,
 	.set_framesize  = xgene_sgmac_set_frame_size,
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
index 423240c97d39..b7d75d067c7a 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
@@ -71,21 +71,6 @@ static bool xgene_enet_wr_indirect(void __iomem *addr, void __iomem *wr,
 	return true;
 }
 
-static void xgene_enet_wr_mac(struct xgene_enet_pdata *pdata,
-			      u32 wr_addr, u32 wr_data)
-{
-	void __iomem *addr, *wr, *cmd, *cmd_done;
-
-	addr = pdata->mcx_mac_addr + MAC_ADDR_REG_OFFSET;
-	wr = pdata->mcx_mac_addr + MAC_WRITE_REG_OFFSET;
-	cmd = pdata->mcx_mac_addr + MAC_COMMAND_REG_OFFSET;
-	cmd_done = pdata->mcx_mac_addr + MAC_COMMAND_DONE_REG_OFFSET;
-
-	if (!xgene_enet_wr_indirect(addr, wr, cmd, cmd_done, wr_addr, wr_data))
-		netdev_err(pdata->ndev, "MCX mac write failed, addr: %04x\n",
-			   wr_addr);
-}
-
 static void xgene_enet_wr_pcs(struct xgene_enet_pdata *pdata,
 			      u32 wr_addr, u32 wr_data)
 {
@@ -148,21 +133,6 @@ static bool xgene_enet_rd_indirect(void __iomem *addr, void __iomem *rd,
 	return true;
 }
 
-static void xgene_enet_rd_mac(struct xgene_enet_pdata *pdata,
-			      u32 rd_addr, u32 *rd_data)
-{
-	void __iomem *addr, *rd, *cmd, *cmd_done;
-
-	addr = pdata->mcx_mac_addr + MAC_ADDR_REG_OFFSET;
-	rd = pdata->mcx_mac_addr + MAC_READ_REG_OFFSET;
-	cmd = pdata->mcx_mac_addr + MAC_COMMAND_REG_OFFSET;
-	cmd_done = pdata->mcx_mac_addr + MAC_COMMAND_DONE_REG_OFFSET;
-
-	if (!xgene_enet_rd_indirect(addr, rd, cmd, cmd_done, rd_addr, rd_data))
-		netdev_err(pdata->ndev, "MCX mac read failed, addr: %04x\n",
-			   rd_addr);
-}
-
 static bool xgene_enet_rd_pcs(struct xgene_enet_pdata *pdata,
 			      u32 rd_addr, u32 *rd_data)
 {
@@ -210,6 +180,18 @@ static int xgene_enet_ecc_init(struct xgene_enet_pdata *pdata)
 	return 0;
 }
 
+static void xgene_xgmac_get_drop_cnt(struct xgene_enet_pdata *pdata,
+				     u32 *rx, u32 *tx)
+{
+	u32 count;
+
+	xgene_enet_rd_axg_csr(pdata, XGENET_ICM_ECM_DROP_COUNT_REG0, &count);
+	*rx = ICM_DROP_COUNT(count);
+	*tx = ECM_DROP_COUNT(count);
+	/* Errata: 10GE_4 - ICM_ECM_DROP_COUNT not clear-on-read */
+	xgene_enet_rd_axg_csr(pdata, XGENET_ECM_CONFIG0_REG_0, &count);
+}
+
 static void xgene_enet_config_ring_if_assoc(struct xgene_enet_pdata *pdata)
 {
 	xgene_enet_wr_ring_if(pdata, ENET_CFGSSQMIWQASSOC_ADDR, 0);
@@ -300,7 +282,7 @@ static void xgene_xgmac_flowctl_tx(struct xgene_enet_pdata *pdata, bool enable)
 {
 	u32 data;
 
-	xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1, &data);
+	data = xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1);
 
 	if (enable)
 		data |= HSTTCTLEN;
@@ -316,7 +298,7 @@ static void xgene_xgmac_flowctl_rx(struct xgene_enet_pdata *pdata, bool enable)
 {
 	u32 data;
 
-	xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1, &data);
+	data = xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1);
 
 	if (enable)
 		data |= HSTRCTLEN;
@@ -332,7 +314,7 @@ static void xgene_xgmac_init(struct xgene_enet_pdata *pdata)
 
 	xgene_xgmac_reset(pdata);
 
-	xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1, &data);
+	data = xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1);
 	data |= HSTPPEN;
 	data &= ~HSTLENCHK;
 	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_1, data);
@@ -379,7 +361,7 @@ static void xgene_xgmac_rx_enable(struct xgene_enet_pdata *pdata)
 {
 	u32 data;
 
-	xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1, &data);
+	data = xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1);
 	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_1, data | HSTRFEN);
 }
 
@@ -387,7 +369,7 @@ static void xgene_xgmac_tx_enable(struct xgene_enet_pdata *pdata)
 {
 	u32 data;
 
-	xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1, &data);
+	data = xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1);
 	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_1, data | HSTTFEN);
 }
 
@@ -395,7 +377,7 @@ static void xgene_xgmac_rx_disable(struct xgene_enet_pdata *pdata)
 {
 	u32 data;
 
-	xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1, &data);
+	data = xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1);
 	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_1, data & ~HSTRFEN);
 }
 
@@ -403,7 +385,7 @@ static void xgene_xgmac_tx_disable(struct xgene_enet_pdata *pdata)
 {
 	u32 data;
 
-	xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1, &data);
+	data = xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1);
 	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_1, data & ~HSTTFEN);
 }
 
@@ -464,26 +446,6 @@ static void xgene_enet_xgcle_bypass(struct xgene_enet_pdata *pdata,
 static void xgene_enet_shutdown(struct xgene_enet_pdata *pdata)
 {
 	struct device *dev = &pdata->pdev->dev;
-	struct xgene_enet_desc_ring *ring;
-	u32 pb;
-	int i;
-
-	pb = 0;
-	for (i = 0; i < pdata->rxq_cnt; i++) {
-		ring = pdata->rx_ring[i]->buf_pool;
-		pb |= BIT(xgene_enet_get_fpsel(ring->id));
-		ring = pdata->rx_ring[i]->page_pool;
-		if (ring)
-			pb |= BIT(xgene_enet_get_fpsel(ring->id));
-	}
-	xgene_enet_wr_ring_if(pdata, ENET_CFGSSQMIFPRESET_ADDR, pb);
-
-	pb = 0;
-	for (i = 0; i < pdata->txq_cnt; i++) {
-		ring = pdata->tx_ring[i];
-		pb |= BIT(xgene_enet_ring_bufnum(ring->id));
-	}
-	xgene_enet_wr_ring_if(pdata, ENET_CFGSSQMIWQRESET_ADDR, pb);
 
 	if (dev->of_node) {
 		if (!IS_ERR(pdata->clk))
@@ -567,6 +529,7 @@ const struct xgene_mac_ops xgene_xgmac_ops = {
 	.set_mac_addr = xgene_xgmac_set_mac_addr,
 	.set_framesize = xgene_xgmac_set_frame_size,
 	.set_mss = xgene_xgmac_set_mss,
+	.get_drop_cnt = xgene_xgmac_get_drop_cnt,
 	.link_state = xgene_enet_link_state,
 	.enable_tx_pause = xgene_xgmac_enable_tx_pause,
 	.flowctl_rx = xgene_xgmac_flowctl_rx,
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
index e644a429ebf4..a3b45517df45 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
@@ -23,6 +23,7 @@
 
 #define X2_BLOCK_ETH_MAC_CSR_OFFSET	0x3000
 #define BLOCK_AXG_MAC_OFFSET		0x0800
+#define BLOCK_AXG_STATS_OFFSET		0x0800
 #define BLOCK_AXG_MAC_CSR_OFFSET	0x2000
 #define BLOCK_PCS_OFFSET		0x3800
 
@@ -70,6 +71,8 @@
 #define XG_RSIF_CONFIG1_REG_ADDR       0x00b8
 #define XG_RSIF_PLC_CLE_BUFF_THRESH    0x1
 #define RSIF_PLC_CLE_BUFF_THRESH_SET(dst, val) xgene_set_bits(dst, val, 0, 2)
+#define XG_MCX_ECM_CONFIG0_REG_0_ADDR          0x0070
+#define XG_MCX_ICM_ECM_DROP_COUNT_REG0_ADDR    0x0124
 #define XCLE_BYPASS_REG0_ADDR           0x0160
 #define XCLE_BYPASS_REG1_ADDR           0x0164
 #define XG_CFG_BYPASS_ADDR		0x0204
@@ -80,6 +83,8 @@
 #define XG_ENET_SPARE_CFG_REG_ADDR	0x040c
 #define XG_ENET_SPARE_CFG_REG_1_ADDR	0x0410
 #define XGENET_RX_DV_GATE_REG_0_ADDR	0x0804
+#define XGENET_ECM_CONFIG0_REG_0	0x0870
+#define XGENET_ICM_ECM_DROP_COUNT_REG0	0x0924
 #define XGENET_CSR_ECM_CFG_0_ADDR	0x0880
 #define XGENET_CSR_MULTI_DPF0_ADDR	0x0888
 #define XGENET_CSR_MULTI_DPF1_ADDR	0x088c
diff --git a/drivers/net/ethernet/apple/bmac.c b/drivers/net/ethernet/apple/bmac.c
index 2b2d87089987..eac740c476ce 100644
--- a/drivers/net/ethernet/apple/bmac.c
+++ b/drivers/net/ethernet/apple/bmac.c
@@ -1218,8 +1218,7 @@ static void bmac_reset_and_enable(struct net_device *dev)
 	 */
 	skb = netdev_alloc_skb(dev, ETHERMINPACKET);
 	if (skb != NULL) {
-		data = skb_put(skb, ETHERMINPACKET);
-		memset(data, 0, ETHERMINPACKET);
+		data = skb_put_zero(skb, ETHERMINPACKET);
 		memcpy(data, dev->dev_addr, ETH_ALEN);
 		memcpy(data + ETH_ALEN, dev->dev_addr, ETH_ALEN);
 		bmac_transmit_packet(skb, dev);
diff --git a/drivers/net/ethernet/apple/macmace.c b/drivers/net/ethernet/apple/macmace.c
index 857df9c45f04..f17a160dbff2 100644
--- a/drivers/net/ethernet/apple/macmace.c
+++ b/drivers/net/ethernet/apple/macmace.c
@@ -663,7 +663,7 @@ static void mace_dma_rx_frame(struct net_device *dev, struct mace_frame *mf)
 			return;
 		}
 		skb_reserve(skb, 2);
-		memcpy(skb_put(skb, frame_length), mf->data, frame_length);
+		skb_put_data(skb, mf->data, frame_length);
 
 		skb->protocol = eth_type_trans(skb, dev);
 		netif_rx(skb);
diff --git a/drivers/net/ethernet/aquantia/atlantic/aq_ring.c b/drivers/net/ethernet/aquantia/atlantic/aq_ring.c
index 3a8a4aa13687..9a0817938eca 100644
--- a/drivers/net/ethernet/aquantia/atlantic/aq_ring.c
+++ b/drivers/net/ethernet/aquantia/atlantic/aq_ring.c
@@ -223,7 +223,7 @@ int aq_ring_rx_clean(struct aq_ring_s *self, int *work_done, int budget)
 		skb->protocol = eth_type_trans(skb, ndev);
 		if (unlikely(buff->is_cso_err)) {
 			++self->stats.rx.errors;
-			__skb_mark_checksum_bad(skb);
+			skb->ip_summed = CHECKSUM_NONE;
 		} else {
 			if (buff->is_ip_cso) {
 				__skb_incr_checksum_unnecessary(skb);
diff --git a/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_utils.h b/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_utils.h
index b8e3d88f0879..a66aee51ab5b 100644
--- a/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_utils.h
+++ b/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_utils.h
@@ -193,9 +193,6 @@ int hw_atl_utils_hw_get_regs(struct aq_hw_s *self,
 			     struct aq_hw_caps_s *aq_hw_caps,
 			     u32 *regs_buff);
 
-int hw_atl_utils_hw_get_settings(struct aq_hw_s *self,
-				 struct ethtool_cmd *cmd);
-
 int hw_atl_utils_hw_set_power(struct aq_hw_s *self,
 			      unsigned int power_state);
 
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
index 7e913d8331c3..8c9986f3fc01 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
@@ -2252,7 +2252,7 @@ static netdev_tx_t atl1c_xmit_frame(struct sk_buff *skb,
 
 	if (atl1c_tx_map(adapter, skb, tpd, type) < 0) {
 		netif_info(adapter, tx_done, adapter->netdev,
-			   "tx-skb droppted due to dma error\n");
+			   "tx-skb dropped due to dma error\n");
 		/* roll back tpd/buffer */
 		atl1c_tx_rollback(adapter, tpd, type);
 		dev_kfree_skb_any(skb);
diff --git a/drivers/net/ethernet/aurora/nb8800.c b/drivers/net/ethernet/aurora/nb8800.c
index 5711fbbd6ae3..041cfb7952f8 100644
--- a/drivers/net/ethernet/aurora/nb8800.c
+++ b/drivers/net/ethernet/aurora/nb8800.c
@@ -251,7 +251,7 @@ static void nb8800_receive(struct net_device *dev, unsigned int i,
 
 	if (len <= RX_COPYBREAK) {
 		dma_sync_single_for_cpu(&dev->dev, dma, len, DMA_FROM_DEVICE);
-		memcpy(skb_put(skb, len), data, len);
+		skb_put_data(skb, data, len);
 		dma_sync_single_for_device(&dev->dev, dma, len,
 					   DMA_FROM_DEVICE);
 	} else {
@@ -264,7 +264,7 @@ static void nb8800_receive(struct net_device *dev, unsigned int i,
 		}
 
 		dma_unmap_page(&dev->dev, dma, RX_BUF_SIZE, DMA_FROM_DEVICE);
-		memcpy(skb_put(skb, RX_COPYHDR), data, RX_COPYHDR);
+		skb_put_data(skb, data, RX_COPYHDR);
 		skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page,
 				offset + RX_COPYHDR, len - RX_COPYHDR,
 				RX_BUF_SIZE);
diff --git a/drivers/net/ethernet/broadcom/b44.c b/drivers/net/ethernet/broadcom/b44.c
index 5b95bb48ce97..f411936b744c 100644
--- a/drivers/net/ethernet/broadcom/b44.c
+++ b/drivers/net/ethernet/broadcom/b44.c
@@ -1836,7 +1836,9 @@ static int b44_get_link_ksettings(struct net_device *dev,
 
 	if (bp->flags & B44_FLAG_EXTERNAL_PHY) {
 		BUG_ON(!dev->phydev);
-		return phy_ethtool_ksettings_get(dev->phydev, cmd);
+		phy_ethtool_ksettings_get(dev->phydev, cmd);
+
+		return 0;
 	}
 
 	supported = (SUPPORTED_Autoneg);
diff --git a/drivers/net/ethernet/broadcom/bcm63xx_enet.c b/drivers/net/ethernet/broadcom/bcm63xx_enet.c
index 50d88d3e03b6..61a88b64bd39 100644
--- a/drivers/net/ethernet/broadcom/bcm63xx_enet.c
+++ b/drivers/net/ethernet/broadcom/bcm63xx_enet.c
@@ -609,8 +609,7 @@ static int bcm_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			dev_kfree_skb(skb);
 			skb = nskb;
 		}
-		data = skb_put(skb, needed);
-		memset(data, 0, needed);
+		data = skb_put_zero(skb, needed);
 	}
 
 	/* point to the next available desc */
@@ -1453,7 +1452,10 @@ static int bcm_enet_get_link_ksettings(struct net_device *dev,
 	if (priv->has_phy) {
 		if (!dev->phydev)
 			return -ENODEV;
-		return phy_ethtool_ksettings_get(dev->phydev, cmd);
+
+		phy_ethtool_ksettings_get(dev->phydev, cmd);
+
+		return 0;
 	} else {
 		cmd->base.autoneg = 0;
 		cmd->base.speed = (priv->force_speed_100) ?
diff --git a/drivers/net/ethernet/broadcom/bcmsysport.c b/drivers/net/ethernet/broadcom/bcmsysport.c
index 5274501428e4..5333601f855f 100644
--- a/drivers/net/ethernet/broadcom/bcmsysport.c
+++ b/drivers/net/ethernet/broadcom/bcmsysport.c
@@ -1099,7 +1099,7 @@ static struct sk_buff *bcm_sysport_insert_tsb(struct sk_buff *skb,
 		skb = nskb;
 	}
 
-	tsb = (struct bcm_tsb *)skb_push(skb, sizeof(*tsb));
+	tsb = skb_push(skb, sizeof(*tsb));
 	/* Zero-out TSB by default */
 	memset(tsb, 0, sizeof(*tsb));
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
index 5f49334dcad5..67fe3d826566 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
@@ -3883,15 +3883,26 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		/* when transmitting in a vf, start bd must hold the ethertype
 		 * for fw to enforce it
 		 */
+		u16 vlan_tci = 0;
 #ifndef BNX2X_STOP_ON_ERROR
-		if (IS_VF(bp))
+		if (IS_VF(bp)) {
 #endif
-			tx_start_bd->vlan_or_ethertype =
-				cpu_to_le16(ntohs(eth->h_proto));
+			/* Still need to consider inband vlan for enforced */
+			if (__vlan_get_tag(skb, &vlan_tci)) {
+				tx_start_bd->vlan_or_ethertype =
+					cpu_to_le16(ntohs(eth->h_proto));
+			} else {
+				tx_start_bd->bd_flags.as_bitfield |=
+					(X_ETH_INBAND_VLAN <<
+					 ETH_TX_BD_FLAGS_VLAN_MODE_SHIFT);
+				tx_start_bd->vlan_or_ethertype =
+					cpu_to_le16(vlan_tci);
+			}
 #ifndef BNX2X_STOP_ON_ERROR
-		else
+		} else {
 			/* used by FW for packet accounting */
 			tx_start_bd->vlan_or_ethertype = cpu_to_le16(pkt_prod);
+		}
 #endif
 	}
 
@@ -4273,8 +4284,8 @@ int bnx2x_setup_tc(struct net_device *dev, u8 num_tc)
 	return 0;
 }
 
-int __bnx2x_setup_tc(struct net_device *dev, u32 handle, __be16 proto,
-		     struct tc_to_netdev *tc)
+int __bnx2x_setup_tc(struct net_device *dev, u32 handle, u32 chain_index,
+		     __be16 proto, struct tc_to_netdev *tc)
 {
 	if (tc->type != TC_SETUP_MQPRIO)
 		return -EINVAL;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
index 243cb9748d35..c26688d2f326 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
@@ -486,8 +486,8 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev);
 
 /* setup_tc callback */
 int bnx2x_setup_tc(struct net_device *dev, u8 num_tc);
-int __bnx2x_setup_tc(struct net_device *dev, u32 handle, __be16 proto,
-		     struct tc_to_netdev *tc);
+int __bnx2x_setup_tc(struct net_device *dev, u32 handle, u32 chain_index,
+		     __be16 proto, struct tc_to_netdev *tc);
 
 int bnx2x_get_vf_config(struct net_device *dev, int vf,
 			struct ifla_vf_info *ivi);
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index a851f95c307a..c12b4d3e946e 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -10303,7 +10303,7 @@ sp_rtnl_not_reset:
 	}
 	if (test_and_clear_bit(BNX2X_SP_RTNL_VFPF_CHANNEL_DOWN,
 			       &bp->sp_rtnl_state)){
-		if (!test_bit(__LINK_STATE_NOCARRIER, &bp->dev->state)) {
+		if (netif_carrier_ok(bp->dev)) {
 			bnx2x_tx_disable(bp);
 			BNX2X_ERR("PF indicated channel is not servicable anymore. This means this VF device is no longer operational\n");
 		}
@@ -12729,7 +12729,7 @@ static int bnx2x_set_mc_list(struct bnx2x *bp)
 	} else {
 		/* If no mc addresses are required, flush the configuration */
 		rc = bnx2x_config_mcast(bp, &rparam, BNX2X_MCAST_CMD_DEL);
-		if (rc)
+		if (rc < 0)
 			BNX2X_ERR("Failed to clear multicast configuration %d\n",
 				  rc);
 	}
@@ -15351,6 +15351,7 @@ int bnx2x_configure_ptp_filters(struct bnx2x *bp)
 		break;
 	case HWTSTAMP_FILTER_ALL:
 	case HWTSTAMP_FILTER_SOME:
+	case HWTSTAMP_FILTER_NTP_ALL:
 		bp->rx_filter = HWTSTAMP_FILTER_NONE;
 		break;
 	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
index bdfd53b46bc5..9ca994d0bab6 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
@@ -901,6 +901,8 @@ static void bnx2x_vf_flr(struct bnx2x *bp, struct bnx2x_virtf *vf)
 	/* release VF resources */
 	bnx2x_vf_free_resc(bp, vf);
 
+	vf->malicious = false;
+
 	/* re-open the mailbox */
 	bnx2x_vf_enable_mbx(bp, vf->abs_vfid);
 	return;
@@ -1822,9 +1824,11 @@ get_vf:
 		   vf->abs_vfid, qidx);
 		bnx2x_vf_handle_rss_update_eqe(bp, vf);
 	case EVENT_RING_OPCODE_VF_FLR:
-	case EVENT_RING_OPCODE_MALICIOUS_VF:
 		/* Do nothing for now */
 		return 0;
+	case EVENT_RING_OPCODE_MALICIOUS_VF:
+		vf->malicious = true;
+		return 0;
 	}
 
 	return 0;
@@ -1905,6 +1909,13 @@ void bnx2x_iov_adjust_stats_req(struct bnx2x *bp)
 			continue;
 		}
 
+		if (vf->malicious) {
+			DP_AND((BNX2X_MSG_IOV | BNX2X_MSG_STATS),
+			       "vf %d malicious so no stats for it\n",
+			       vf->abs_vfid);
+			continue;
+		}
+
 		DP_AND((BNX2X_MSG_IOV | BNX2X_MSG_STATS),
 		       "add addresses for vf %d\n", vf->abs_vfid);
 		for_each_vfq(vf, j) {
@@ -3042,7 +3053,7 @@ void bnx2x_vf_pci_dealloc(struct bnx2x *bp)
 {
 	BNX2X_PCI_FREE(bp->vf2pf_mbox, bp->vf2pf_mbox_mapping,
 		       sizeof(struct bnx2x_vf_mbx_msg));
-	BNX2X_PCI_FREE(bp->vf2pf_mbox, bp->pf2vf_bulletin_mapping,
+	BNX2X_PCI_FREE(bp->pf2vf_bulletin, bp->pf2vf_bulletin_mapping,
 		       sizeof(union pf_vf_bulletin));
 }
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
index 888d0b6632e8..53466f6cebab 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
@@ -141,6 +141,7 @@ struct bnx2x_virtf {
 #define VF_RESET	3	/* VF FLR'd, pending cleanup */
 
 	bool flr_clnup_stage;	/* true during flr cleanup */
+	bool malicious;		/* true if FW indicated so, until FLR */
 
 	/* dma */
 	dma_addr_t fw_stat_map;
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt.c b/drivers/net/ethernet/broadcom/bnxt/bnxt.c
index 03f55daecb20..a19f68f5862d 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt.c
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt.c
@@ -175,6 +175,8 @@ static const struct pci_device_id bnxt_pci_tbl[] = {
 	{ PCI_VDEVICE(BROADCOM, 0x16f1), .driver_data = BCM57452 },
 	{ PCI_VDEVICE(BROADCOM, 0x1614), .driver_data = BCM57454 },
 #ifdef CONFIG_BNXT_SRIOV
+	{ PCI_VDEVICE(BROADCOM, 0x1606), .driver_data = NETXTREME_E_VF },
+	{ PCI_VDEVICE(BROADCOM, 0x1609), .driver_data = NETXTREME_E_VF },
 	{ PCI_VDEVICE(BROADCOM, 0x16c1), .driver_data = NETXTREME_E_VF },
 	{ PCI_VDEVICE(BROADCOM, 0x16cb), .driver_data = NETXTREME_C_VF },
 	{ PCI_VDEVICE(BROADCOM, 0x16d3), .driver_data = NETXTREME_E_VF },
@@ -461,14 +463,17 @@ normal_tx:
 	prod = NEXT_TX(prod);
 	txr->tx_prod = prod;
 
-	writel(DB_KEY_TX | prod, txr->tx_doorbell);
-	writel(DB_KEY_TX | prod, txr->tx_doorbell);
+	if (!skb->xmit_more || netif_xmit_stopped(txq))
+		bnxt_db_write(bp, txr->tx_doorbell, DB_KEY_TX | prod);
 
 tx_done:
 
 	mmiowb();
 
 	if (unlikely(bnxt_tx_avail(bp, txr) <= MAX_SKB_FRAGS + 1)) {
+		if (skb->xmit_more && !tx_buf->is_push)
+			bnxt_db_write(bp, txr->tx_doorbell, DB_KEY_TX | prod);
+
 		netif_tx_stop_queue(txq);
 
 		/* netif_tx_stop_queue() must be done before checking
@@ -582,7 +587,8 @@ static struct page *__bnxt_alloc_rx_page(struct bnxt *bp, dma_addr_t *mapping,
 	if (!page)
 		return NULL;
 
-	*mapping = dma_map_page(dev, page, 0, PAGE_SIZE, bp->rx_dir);
+	*mapping = dma_map_page_attrs(dev, page, 0, PAGE_SIZE, bp->rx_dir,
+				      DMA_ATTR_WEAK_ORDERING);
 	if (dma_mapping_error(dev, *mapping)) {
 		__free_page(page);
 		return NULL;
@@ -601,8 +607,9 @@ static inline u8 *__bnxt_alloc_rx_data(struct bnxt *bp, dma_addr_t *mapping,
 	if (!data)
 		return NULL;
 
-	*mapping = dma_map_single(&pdev->dev, data + bp->rx_dma_offset,
-				  bp->rx_buf_use_size, bp->rx_dir);
+	*mapping = dma_map_single_attrs(&pdev->dev, data + bp->rx_dma_offset,
+					bp->rx_buf_use_size, bp->rx_dir,
+					DMA_ATTR_WEAK_ORDERING);
 
 	if (dma_mapping_error(&pdev->dev, *mapping)) {
 		kfree(data);
@@ -705,8 +712,9 @@ static inline int bnxt_alloc_rx_page(struct bnxt *bp,
 			return -ENOMEM;
 	}
 
-	mapping = dma_map_page(&pdev->dev, page, offset, BNXT_RX_PAGE_SIZE,
-			       PCI_DMA_FROMDEVICE);
+	mapping = dma_map_page_attrs(&pdev->dev, page, offset,
+				     BNXT_RX_PAGE_SIZE, PCI_DMA_FROMDEVICE,
+				     DMA_ATTR_WEAK_ORDERING);
 	if (dma_mapping_error(&pdev->dev, mapping)) {
 		__free_page(page);
 		return -EIO;
@@ -799,7 +807,8 @@ static struct sk_buff *bnxt_rx_page_skb(struct bnxt *bp,
 		return NULL;
 	}
 	dma_addr -= bp->rx_dma_offset;
-	dma_unmap_page(&bp->pdev->dev, dma_addr, PAGE_SIZE, bp->rx_dir);
+	dma_unmap_page_attrs(&bp->pdev->dev, dma_addr, PAGE_SIZE, bp->rx_dir,
+			     DMA_ATTR_WEAK_ORDERING);
 
 	if (unlikely(!payload))
 		payload = eth_get_headlen(data_ptr, len);
@@ -841,8 +850,8 @@ static struct sk_buff *bnxt_rx_skb(struct bnxt *bp,
 	}
 
 	skb = build_skb(data, 0);
-	dma_unmap_single(&bp->pdev->dev, dma_addr, bp->rx_buf_use_size,
-			 bp->rx_dir);
+	dma_unmap_single_attrs(&bp->pdev->dev, dma_addr, bp->rx_buf_use_size,
+			       bp->rx_dir, DMA_ATTR_WEAK_ORDERING);
 	if (!skb) {
 		kfree(data);
 		return NULL;
@@ -909,8 +918,9 @@ static struct sk_buff *bnxt_rx_pages(struct bnxt *bp, struct bnxt_napi *bnapi,
 			return NULL;
 		}
 
-		dma_unmap_page(&pdev->dev, mapping, BNXT_RX_PAGE_SIZE,
-			       PCI_DMA_FROMDEVICE);
+		dma_unmap_page_attrs(&pdev->dev, mapping, BNXT_RX_PAGE_SIZE,
+				     PCI_DMA_FROMDEVICE,
+				     DMA_ATTR_WEAK_ORDERING);
 
 		skb->data_len += frag_len;
 		skb->len += frag_len;
@@ -1301,10 +1311,11 @@ static inline struct sk_buff *bnxt_tpa_end(struct bnxt *bp,
 		cp_cons = NEXT_CMP(cp_cons);
 	}
 
-	if (unlikely(agg_bufs > MAX_SKB_FRAGS)) {
+	if (unlikely(agg_bufs > MAX_SKB_FRAGS || TPA_END_ERRORS(tpa_end1))) {
 		bnxt_abort_tpa(bp, bnapi, cp_cons, agg_bufs);
-		netdev_warn(bp->dev, "TPA frags %d exceeded MAX_SKB_FRAGS %d\n",
-			    agg_bufs, (int)MAX_SKB_FRAGS);
+		if (agg_bufs > MAX_SKB_FRAGS)
+			netdev_warn(bp->dev, "TPA frags %d exceeded MAX_SKB_FRAGS %d\n",
+				    agg_bufs, (int)MAX_SKB_FRAGS);
 		return NULL;
 	}
 
@@ -1329,8 +1340,9 @@ static inline struct sk_buff *bnxt_tpa_end(struct bnxt *bp,
 		tpa_info->mapping = new_mapping;
 
 		skb = build_skb(data, 0);
-		dma_unmap_single(&bp->pdev->dev, mapping, bp->rx_buf_use_size,
-				 bp->rx_dir);
+		dma_unmap_single_attrs(&bp->pdev->dev, mapping,
+				       bp->rx_buf_use_size, bp->rx_dir,
+				       DMA_ATTR_WEAK_ORDERING);
 
 		if (!skb) {
 			kfree(data);
@@ -1562,6 +1574,45 @@ next_rx_no_prod:
 	return rc;
 }
 
+/* In netpoll mode, if we are using a combined completion ring, we need to
+ * discard the rx packets and recycle the buffers.
+ */
+static int bnxt_force_rx_discard(struct bnxt *bp, struct bnxt_napi *bnapi,
+				 u32 *raw_cons, u8 *event)
+{
+	struct bnxt_cp_ring_info *cpr = &bnapi->cp_ring;
+	u32 tmp_raw_cons = *raw_cons;
+	struct rx_cmp_ext *rxcmp1;
+	struct rx_cmp *rxcmp;
+	u16 cp_cons;
+	u8 cmp_type;
+
+	cp_cons = RING_CMP(tmp_raw_cons);
+	rxcmp = (struct rx_cmp *)
+			&cpr->cp_desc_ring[CP_RING(cp_cons)][CP_IDX(cp_cons)];
+
+	tmp_raw_cons = NEXT_RAW_CMP(tmp_raw_cons);
+	cp_cons = RING_CMP(tmp_raw_cons);
+	rxcmp1 = (struct rx_cmp_ext *)
+			&cpr->cp_desc_ring[CP_RING(cp_cons)][CP_IDX(cp_cons)];
+
+	if (!RX_CMP_VALID(rxcmp1, tmp_raw_cons))
+		return -EBUSY;
+
+	cmp_type = RX_CMP_TYPE(rxcmp);
+	if (cmp_type == CMP_TYPE_RX_L2_CMP) {
+		rxcmp1->rx_cmp_cfa_code_errors_v2 |=
+			cpu_to_le32(RX_CMPL_ERRORS_CRC_ERROR);
+	} else if (cmp_type == CMP_TYPE_RX_L2_TPA_END_CMP) {
+		struct rx_tpa_end_cmp_ext *tpa_end1;
+
+		tpa_end1 = (struct rx_tpa_end_cmp_ext *)rxcmp1;
+		tpa_end1->rx_tpa_end_cmp_errors_v2 |=
+			cpu_to_le32(RX_TPA_END_CMP_ERRORS);
+	}
+	return bnxt_rx_pkt(bp, bnapi, raw_cons, event);
+}
+
 #define BNXT_GET_EVENT_PORT(data)	\
 	((data) &			\
 	 ASYNC_EVENT_CMPL_PORT_CONN_NOT_ALLOWED_EVENT_DATA1_PORT_ID_MASK)
@@ -1744,7 +1795,11 @@ static int bnxt_poll_work(struct bnxt *bp, struct bnxt_napi *bnapi, int budget)
 			if (unlikely(tx_pkts > bp->tx_wake_thresh))
 				rx_pkts = budget;
 		} else if ((TX_CMP_TYPE(txcmp) & 0x30) == 0x10) {
-			rc = bnxt_rx_pkt(bp, bnapi, &raw_cons, &event);
+			if (likely(budget))
+				rc = bnxt_rx_pkt(bp, bnapi, &raw_cons, &event);
+			else
+				rc = bnxt_force_rx_discard(bp, bnapi, &raw_cons,
+							   &event);
 			if (likely(rc >= 0))
 				rx_pkts += rc;
 			else if (rc == -EBUSY)	/* partial completion */
@@ -1771,8 +1826,7 @@ static int bnxt_poll_work(struct bnxt *bp, struct bnxt_napi *bnapi, int budget)
 		/* Sync BD data before updating doorbell */
 		wmb();
 
-		writel(DB_KEY_TX | prod, db);
-		writel(DB_KEY_TX | prod, db);
+		bnxt_db_write(bp, db, DB_KEY_TX | prod);
 	}
 
 	cpr->cp_raw_cons = raw_cons;
@@ -1788,14 +1842,10 @@ static int bnxt_poll_work(struct bnxt *bp, struct bnxt_napi *bnapi, int budget)
 	if (event & BNXT_RX_EVENT) {
 		struct bnxt_rx_ring_info *rxr = bnapi->rx_ring;
 
-		writel(DB_KEY_RX | rxr->rx_prod, rxr->rx_doorbell);
-		writel(DB_KEY_RX | rxr->rx_prod, rxr->rx_doorbell);
-		if (event & BNXT_AGG_EVENT) {
-			writel(DB_KEY_RX | rxr->rx_agg_prod,
-			       rxr->rx_agg_doorbell);
-			writel(DB_KEY_RX | rxr->rx_agg_prod,
-			       rxr->rx_agg_doorbell);
-		}
+		bnxt_db_write(bp, rxr->rx_doorbell, DB_KEY_RX | rxr->rx_prod);
+		if (event & BNXT_AGG_EVENT)
+			bnxt_db_write(bp, rxr->rx_agg_doorbell,
+				      DB_KEY_RX | rxr->rx_agg_prod);
 	}
 	return rx_pkts;
 }
@@ -1855,13 +1905,11 @@ static int bnxt_poll_nitroa0(struct napi_struct *napi, int budget)
 
 	cpr->cp_raw_cons = raw_cons;
 	BNXT_CP_DB(cpr->cp_doorbell, cpr->cp_raw_cons);
-	writel(DB_KEY_RX | rxr->rx_prod, rxr->rx_doorbell);
-	writel(DB_KEY_RX | rxr->rx_prod, rxr->rx_doorbell);
+	bnxt_db_write(bp, rxr->rx_doorbell, DB_KEY_RX | rxr->rx_prod);
 
-	if (event & BNXT_AGG_EVENT) {
-		writel(DB_KEY_RX | rxr->rx_agg_prod, rxr->rx_agg_doorbell);
-		writel(DB_KEY_RX | rxr->rx_agg_prod, rxr->rx_agg_doorbell);
-	}
+	if (event & BNXT_AGG_EVENT)
+		bnxt_db_write(bp, rxr->rx_agg_doorbell,
+			      DB_KEY_RX | rxr->rx_agg_prod);
 
 	if (!bnxt_has_work(bp, cpr) && rx_pkts < budget) {
 		napi_complete_done(napi, rx_pkts);
@@ -1971,9 +2019,11 @@ static void bnxt_free_rx_skbs(struct bnxt *bp)
 				if (!data)
 					continue;
 
-				dma_unmap_single(&pdev->dev, tpa_info->mapping,
-						 bp->rx_buf_use_size,
-						 bp->rx_dir);
+				dma_unmap_single_attrs(&pdev->dev,
+						       tpa_info->mapping,
+						       bp->rx_buf_use_size,
+						       bp->rx_dir,
+						       DMA_ATTR_WEAK_ORDERING);
 
 				tpa_info->data = NULL;
 
@@ -1993,13 +2043,15 @@ static void bnxt_free_rx_skbs(struct bnxt *bp)
 
 			if (BNXT_RX_PAGE_MODE(bp)) {
 				mapping -= bp->rx_dma_offset;
-				dma_unmap_page(&pdev->dev, mapping,
-					       PAGE_SIZE, bp->rx_dir);
+				dma_unmap_page_attrs(&pdev->dev, mapping,
+						     PAGE_SIZE, bp->rx_dir,
+						     DMA_ATTR_WEAK_ORDERING);
 				__free_page(data);
 			} else {
-				dma_unmap_single(&pdev->dev, mapping,
-						 bp->rx_buf_use_size,
-						 bp->rx_dir);
+				dma_unmap_single_attrs(&pdev->dev, mapping,
+						       bp->rx_buf_use_size,
+						       bp->rx_dir,
+						       DMA_ATTR_WEAK_ORDERING);
 				kfree(data);
 			}
 		}
@@ -2012,8 +2064,10 @@ static void bnxt_free_rx_skbs(struct bnxt *bp)
 			if (!page)
 				continue;
 
-			dma_unmap_page(&pdev->dev, rx_agg_buf->mapping,
-				       BNXT_RX_PAGE_SIZE, PCI_DMA_FROMDEVICE);
+			dma_unmap_page_attrs(&pdev->dev, rx_agg_buf->mapping,
+					     BNXT_RX_PAGE_SIZE,
+					     PCI_DMA_FROMDEVICE,
+					     DMA_ATTR_WEAK_ORDERING);
 
 			rx_agg_buf->page = NULL;
 			__clear_bit(j, rxr->rx_agg_bmap);
@@ -2856,6 +2910,32 @@ static int bnxt_alloc_hwrm_resources(struct bnxt *bp)
 	return 0;
 }
 
+static void bnxt_free_hwrm_short_cmd_req(struct bnxt *bp)
+{
+	if (bp->hwrm_short_cmd_req_addr) {
+		struct pci_dev *pdev = bp->pdev;
+
+		dma_free_coherent(&pdev->dev, BNXT_HWRM_MAX_REQ_LEN,
+				  bp->hwrm_short_cmd_req_addr,
+				  bp->hwrm_short_cmd_req_dma_addr);
+		bp->hwrm_short_cmd_req_addr = NULL;
+	}
+}
+
+static int bnxt_alloc_hwrm_short_cmd_req(struct bnxt *bp)
+{
+	struct pci_dev *pdev = bp->pdev;
+
+	bp->hwrm_short_cmd_req_addr =
+		dma_alloc_coherent(&pdev->dev, BNXT_HWRM_MAX_REQ_LEN,
+				   &bp->hwrm_short_cmd_req_dma_addr,
+				   GFP_KERNEL);
+	if (!bp->hwrm_short_cmd_req_addr)
+		return -ENOMEM;
+
+	return 0;
+}
+
 static void bnxt_free_stats(struct bnxt *bp)
 {
 	u32 size, i;
@@ -3203,16 +3283,41 @@ static int bnxt_hwrm_do_send_msg(struct bnxt *bp, void *msg, u32 msg_len,
 	__le32 *resp_len, *valid;
 	u16 cp_ring_id, len = 0;
 	struct hwrm_err_output *resp = bp->hwrm_cmd_resp_addr;
+	u16 max_req_len = BNXT_HWRM_MAX_REQ_LEN;
 
 	req->seq_id = cpu_to_le16(bp->hwrm_cmd_seq++);
 	memset(resp, 0, PAGE_SIZE);
 	cp_ring_id = le16_to_cpu(req->cmpl_ring);
 	intr_process = (cp_ring_id == INVALID_HW_RING_ID) ? 0 : 1;
 
+	if (bp->flags & BNXT_FLAG_SHORT_CMD) {
+		void *short_cmd_req = bp->hwrm_short_cmd_req_addr;
+		struct hwrm_short_input short_input = {0};
+
+		memcpy(short_cmd_req, req, msg_len);
+		memset(short_cmd_req + msg_len, 0, BNXT_HWRM_MAX_REQ_LEN -
+						   msg_len);
+
+		short_input.req_type = req->req_type;
+		short_input.signature =
+				cpu_to_le16(SHORT_REQ_SIGNATURE_SHORT_CMD);
+		short_input.size = cpu_to_le16(msg_len);
+		short_input.req_addr =
+			cpu_to_le64(bp->hwrm_short_cmd_req_dma_addr);
+
+		data = (u32 *)&short_input;
+		msg_len = sizeof(short_input);
+
+		/* Sync memory write before updating doorbell */
+		wmb();
+
+		max_req_len = BNXT_HWRM_SHORT_REQ_LEN;
+	}
+
 	/* Write request msg to hwrm channel */
 	__iowrite32_copy(bp->bar0, data, msg_len / 4);
 
-	for (i = msg_len; i < BNXT_HWRM_MAX_REQ_LEN; i += 4)
+	for (i = msg_len; i < max_req_len; i += 4)
 		writel(0, bp->bar0 + i);
 
 	/* currently supports only one outstanding message */
@@ -4650,6 +4755,7 @@ static int bnxt_hwrm_ver_get(struct bnxt *bp)
 	int rc;
 	struct hwrm_ver_get_input req = {0};
 	struct hwrm_ver_get_output *resp = bp->hwrm_cmd_resp_addr;
+	u32 dev_caps_cfg;
 
 	bp->hwrm_max_req_len = HWRM_MAX_REQ_LEN;
 	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_VER_GET, -1, -1);
@@ -4687,6 +4793,11 @@ static int bnxt_hwrm_ver_get(struct bnxt *bp)
 	    !resp->chip_metal)
 		bp->flags |= BNXT_FLAG_CHIP_NITRO_A0;
 
+	dev_caps_cfg = le32_to_cpu(resp->dev_caps_cfg);
+	if ((dev_caps_cfg & VER_GET_RESP_DEV_CAPS_CFG_SHORT_CMD_SUPPORTED) &&
+	    (dev_caps_cfg & VER_GET_RESP_DEV_CAPS_CFG_SHORT_CMD_REQUIRED))
+		bp->flags |= BNXT_FLAG_SHORT_CMD;
+
 hwrm_ver_get_exit:
 	mutex_unlock(&bp->hwrm_cmd_lock);
 	return rc;
@@ -6663,12 +6774,11 @@ static void bnxt_poll_controller(struct net_device *dev)
 	struct bnxt *bp = netdev_priv(dev);
 	int i;
 
-	for (i = 0; i < bp->cp_nr_rings; i++) {
-		struct bnxt_irq *irq = &bp->irq_tbl[i];
+	/* Only process tx rings/combined rings in netpoll mode. */
+	for (i = 0; i < bp->tx_nr_rings; i++) {
+		struct bnxt_tx_ring_info *txr = &bp->tx_ring[i];
 
-		disable_irq(irq->vector);
-		irq->handler(irq->vector, bp->bnapi[i]);
-		enable_irq(irq->vector);
+		napi_schedule(&txr->bnapi->napi);
 	}
 }
 #endif
@@ -7036,8 +7146,8 @@ int bnxt_setup_mq_tc(struct net_device *dev, u8 tc)
 	return 0;
 }
 
-static int bnxt_setup_tc(struct net_device *dev, u32 handle, __be16 proto,
-			 struct tc_to_netdev *ntc)
+static int bnxt_setup_tc(struct net_device *dev, u32 handle, u32 chain_index,
+			 __be16 proto, struct tc_to_netdev *ntc)
 {
 	if (ntc->type != TC_SETUP_MQPRIO)
 		return -EINVAL;
@@ -7345,6 +7455,7 @@ static void bnxt_remove_one(struct pci_dev *pdev)
 	bnxt_clear_int_mode(bp);
 	bnxt_hwrm_func_drv_unrgtr(bp);
 	bnxt_free_hwrm_resources(bp);
+	bnxt_free_hwrm_short_cmd_req(bp);
 	bnxt_ethtool_free(bp);
 	bnxt_dcb_free(bp);
 	kfree(bp->edev);
@@ -7499,10 +7610,9 @@ static int bnxt_get_dflt_rings(struct bnxt *bp, int *max_rx, int *max_tx,
 	return rc;
 }
 
-static int bnxt_set_dflt_rings(struct bnxt *bp)
+static int bnxt_set_dflt_rings(struct bnxt *bp, bool sh)
 {
 	int dflt_rings, max_rx_rings, max_tx_rings, rc;
-	bool sh = true;
 
 	if (sh)
 		bp->flags |= BNXT_FLAG_SHARED_RINGS;
@@ -7595,6 +7705,12 @@ static int bnxt_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (rc)
 		goto init_err_pci_clean;
 
+	if (bp->flags & BNXT_FLAG_SHORT_CMD) {
+		rc = bnxt_alloc_hwrm_short_cmd_req(bp);
+		if (rc)
+			goto init_err_pci_clean;
+	}
+
 	rc = bnxt_hwrm_func_reset(bp);
 	if (rc)
 		goto init_err_pci_clean;
@@ -7634,8 +7750,10 @@ static int bnxt_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	init_waitqueue_head(&bp->sriov_cfg_wait);
 #endif
 	bp->gro_func = bnxt_gro_func_5730x;
-	if (BNXT_CHIP_NUM_57X1X(bp->chip_num))
+	if (BNXT_CHIP_P4_PLUS(bp))
 		bp->gro_func = bnxt_gro_func_5731x;
+	else
+		bp->flags |= BNXT_FLAG_DOUBLE_DB;
 
 	rc = bnxt_hwrm_func_drv_rgtr(bp);
 	if (rc)
@@ -7673,7 +7791,7 @@ static int bnxt_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	bnxt_set_tpa_flags(bp);
 	bnxt_set_ring_params(bp);
 	bnxt_set_max_func_irqs(bp, max_irqs);
-	rc = bnxt_set_dflt_rings(bp);
+	rc = bnxt_set_dflt_rings(bp, true);
 	if (rc) {
 		netdev_err(bp->dev, "Not enough rings available.\n");
 		rc = -ENOMEM;
@@ -7685,9 +7803,7 @@ static int bnxt_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 			   VNIC_RSS_CFG_REQ_HASH_TYPE_TCP_IPV4 |
 			   VNIC_RSS_CFG_REQ_HASH_TYPE_IPV6 |
 			   VNIC_RSS_CFG_REQ_HASH_TYPE_TCP_IPV6;
-	if (!BNXT_CHIP_NUM_57X0X(bp->chip_num) &&
-	    !BNXT_CHIP_TYPE_NITRO_A0(bp) &&
-	    bp->hwrm_spec_code >= 0x10501) {
+	if (BNXT_CHIP_P4_PLUS(bp) && bp->hwrm_spec_code >= 0x10501) {
 		bp->flags |= BNXT_FLAG_UDP_RSS_CAP;
 		bp->rss_hash_cfg |= VNIC_RSS_CFG_REQ_HASH_TYPE_UDP_IPV4 |
 				    VNIC_RSS_CFG_REQ_HASH_TYPE_UDP_IPV6;
@@ -7759,6 +7875,7 @@ static void bnxt_shutdown(struct pci_dev *pdev)
 		dev_close(dev);
 
 	if (system_state == SYSTEM_POWER_OFF) {
+		bnxt_ulp_shutdown(bp);
 		bnxt_clear_int_mode(bp);
 		pci_wake_from_d3(pdev, bp->wol);
 		pci_set_power_state(pdev, PCI_D3hot);
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt.h b/drivers/net/ethernet/broadcom/bnxt/bnxt.h
index 3ef42dbc6327..f872a7db2ca8 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt.h
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt.h
@@ -374,12 +374,16 @@ struct rx_tpa_end_cmp_ext {
 
 	__le32 rx_tpa_end_cmp_errors_v2;
 	#define RX_TPA_END_CMP_V2				(0x1 << 0)
-	#define RX_TPA_END_CMP_ERRORS				(0x7fff << 1)
+	#define RX_TPA_END_CMP_ERRORS				(0x3 << 1)
 	#define RX_TPA_END_CMPL_ERRORS_SHIFT			 1
 
 	u32 rx_tpa_end_cmp_start_opaque;
 };
 
+#define TPA_END_ERRORS(rx_tpa_end_ext)					\
+	((rx_tpa_end_ext)->rx_tpa_end_cmp_errors_v2 &			\
+	 cpu_to_le32(RX_TPA_END_CMP_ERRORS))
+
 #define DB_IDX_MASK						0xffffff
 #define DB_IDX_VALID						(0x1 << 26)
 #define DB_IRQ_DIS						(0x1 << 27)
@@ -500,6 +504,7 @@ struct rx_tpa_end_cmp_ext {
 #define NEXT_CMP(idx)		RING_CMP(ADV_RAW_CMP(idx, 1))
 
 #define BNXT_HWRM_MAX_REQ_LEN		(bp->hwrm_max_req_len)
+#define BNXT_HWRM_SHORT_REQ_LEN		sizeof(struct hwrm_short_input)
 #define DFLT_HWRM_CMD_TIMEOUT		500
 #define HWRM_CMD_TIMEOUT		(bp->hwrm_cmd_timeout)
 #define HWRM_RESET_TIMEOUT		((HWRM_CMD_TIMEOUT) * 4)
@@ -937,31 +942,45 @@ struct bnxt {
 #define CHIP_NUM_57402		0x16d0
 #define CHIP_NUM_57404		0x16d1
 #define CHIP_NUM_57406		0x16d2
+#define CHIP_NUM_57407		0x16d5
 
 #define CHIP_NUM_57311		0x16ce
 #define CHIP_NUM_57312		0x16cf
 #define CHIP_NUM_57314		0x16df
+#define CHIP_NUM_57317		0x16e0
 #define CHIP_NUM_57412		0x16d6
 #define CHIP_NUM_57414		0x16d7
 #define CHIP_NUM_57416		0x16d8
 #define CHIP_NUM_57417		0x16d9
+#define CHIP_NUM_57412L		0x16da
+#define CHIP_NUM_57414L		0x16db
+
+#define CHIP_NUM_5745X		0xd730
 
 #define BNXT_CHIP_NUM_5730X(chip_num)		\
 	((chip_num) >= CHIP_NUM_57301 &&	\
 	 (chip_num) <= CHIP_NUM_57304)
 
 #define BNXT_CHIP_NUM_5740X(chip_num)		\
-	((chip_num) >= CHIP_NUM_57402 &&	\
-	 (chip_num) <= CHIP_NUM_57406)
+	(((chip_num) >= CHIP_NUM_57402 &&	\
+	  (chip_num) <= CHIP_NUM_57406) ||	\
+	 (chip_num) == CHIP_NUM_57407)
 
 #define BNXT_CHIP_NUM_5731X(chip_num)		\
 	((chip_num) == CHIP_NUM_57311 ||	\
 	 (chip_num) == CHIP_NUM_57312 ||	\
-	 (chip_num) == CHIP_NUM_57314)
+	 (chip_num) == CHIP_NUM_57314 ||	\
+	 (chip_num) == CHIP_NUM_57317)
 
 #define BNXT_CHIP_NUM_5741X(chip_num)		\
 	((chip_num) >= CHIP_NUM_57412 &&	\
-	 (chip_num) <= CHIP_NUM_57417)
+	 (chip_num) <= CHIP_NUM_57414L)
+
+#define BNXT_CHIP_NUM_58700(chip_num)		\
+	 ((chip_num) == CHIP_NUM_58700)
+
+#define BNXT_CHIP_NUM_5745X(chip_num)		\
+	 ((chip_num) == CHIP_NUM_5745X)
 
 #define BNXT_CHIP_NUM_57X0X(chip_num)		\
 	(BNXT_CHIP_NUM_5730X(chip_num) || BNXT_CHIP_NUM_5740X(chip_num))
@@ -1006,6 +1025,8 @@ struct bnxt {
 	#define BNXT_FLAG_RX_PAGE_MODE	0x40000
 	#define BNXT_FLAG_FW_LLDP_AGENT	0x80000
 	#define BNXT_FLAG_MULTI_HOST	0x100000
+	#define BNXT_FLAG_SHORT_CMD	0x200000
+	#define BNXT_FLAG_DOUBLE_DB	0x400000
 	#define BNXT_FLAG_CHIP_NITRO_A0	0x1000000
 
 	#define BNXT_FLAG_ALL_CONFIG_FEATS (BNXT_FLAG_TPA |		\
@@ -1020,6 +1041,13 @@ struct bnxt {
 #define BNXT_CHIP_TYPE_NITRO_A0(bp) ((bp)->flags & BNXT_FLAG_CHIP_NITRO_A0)
 #define BNXT_RX_PAGE_MODE(bp)	((bp)->flags & BNXT_FLAG_RX_PAGE_MODE)
 
+/* Chip class phase 4 and later */
+#define BNXT_CHIP_P4_PLUS(bp)			\
+	(BNXT_CHIP_NUM_57X1X((bp)->chip_num) ||	\
+	 BNXT_CHIP_NUM_5745X((bp)->chip_num) ||	\
+	 (BNXT_CHIP_NUM_58700((bp)->chip_num) &&	\
+	  !BNXT_CHIP_TYPE_NITRO_A0(bp)))
+
 	struct bnxt_en_dev	*edev;
 	struct bnxt_en_dev *	(*ulp_probe)(struct net_device *);
 
@@ -1106,6 +1134,8 @@ struct bnxt {
 	u32			hwrm_spec_code;
 	u16			hwrm_cmd_seq;
 	u32			hwrm_intr_seq_id;
+	void			*hwrm_short_cmd_req_addr;
+	dma_addr_t		hwrm_short_cmd_req_dma_addr;
 	void			*hwrm_cmd_resp_addr;
 	dma_addr_t		hwrm_cmd_resp_dma_addr;
 	void			*hwrm_dbg_resp_addr;
@@ -1229,6 +1259,14 @@ static inline u32 bnxt_tx_avail(struct bnxt *bp, struct bnxt_tx_ring_info *txr)
 		((txr->tx_prod - txr->tx_cons) & bp->tx_ring_mask);
 }
 
+/* For TX and RX ring doorbells */
+static inline void bnxt_db_write(struct bnxt *bp, void __iomem *db, u32 val)
+{
+	writel(val, db);
+	if (bp->flags & BNXT_FLAG_DOUBLE_DB)
+		writel(val, db);
+}
+
 extern const u16 bnxt_lhint_arr[];
 
 int bnxt_alloc_rx_data(struct bnxt *bp, struct bnxt_rx_ring_info *rxr,
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt_ethtool.c b/drivers/net/ethernet/broadcom/bnxt/bnxt_ethtool.c
index 11ddf0adc6e1..fd1181510b65 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt_ethtool.c
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt_ethtool.c
@@ -2376,8 +2376,7 @@ static int bnxt_run_loopback(struct bnxt *bp)
 	/* Sync BD data before updating doorbell */
 	wmb();
 
-	writel(DB_KEY_TX | txr->tx_prod, txr->tx_doorbell);
-	writel(DB_KEY_TX | txr->tx_prod, txr->tx_doorbell);
+	bnxt_db_write(bp, txr->tx_doorbell, DB_KEY_TX | txr->tx_prod);
 	rc = bnxt_poll_loopback(bp, pkt_size);
 
 	dma_unmap_single(&bp->pdev->dev, map, pkt_size, PCI_DMA_TODEVICE);
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt_ulp.c b/drivers/net/ethernet/broadcom/bnxt/bnxt_ulp.c
index 8b7464b76501..77da75a55c02 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt_ulp.c
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt_ulp.c
@@ -266,6 +266,25 @@ void bnxt_ulp_sriov_cfg(struct bnxt *bp, int num_vfs)
 	}
 }
 
+void bnxt_ulp_shutdown(struct bnxt *bp)
+{
+	struct bnxt_en_dev *edev = bp->edev;
+	struct bnxt_ulp_ops *ops;
+	int i;
+
+	if (!edev)
+		return;
+
+	for (i = 0; i < BNXT_MAX_ULP; i++) {
+		struct bnxt_ulp *ulp = &edev->ulp_tbl[i];
+
+		ops = rtnl_dereference(ulp->ulp_ops);
+		if (!ops || !ops->ulp_shutdown)
+			continue;
+		ops->ulp_shutdown(ulp->handle);
+	}
+}
+
 void bnxt_ulp_async_events(struct bnxt *bp, struct hwrm_async_event_cmpl *cmpl)
 {
 	u16 event_id = le16_to_cpu(cmpl->event_id);
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt_ulp.h b/drivers/net/ethernet/broadcom/bnxt/bnxt_ulp.h
index 74f816e46a33..d2471067dc37 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt_ulp.h
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt_ulp.h
@@ -26,6 +26,7 @@ struct bnxt_ulp_ops {
 	void (*ulp_stop)(void *);
 	void (*ulp_start)(void *);
 	void (*ulp_sriov_config)(void *, int);
+	void (*ulp_shutdown)(void *);
 };
 
 struct bnxt_msix_entry {
@@ -87,6 +88,7 @@ void bnxt_subtract_ulp_resources(struct bnxt *bp, int ulp_id);
 void bnxt_ulp_stop(struct bnxt *bp);
 void bnxt_ulp_start(struct bnxt *bp);
 void bnxt_ulp_sriov_cfg(struct bnxt *bp, int num_vfs);
+void bnxt_ulp_shutdown(struct bnxt *bp);
 void bnxt_ulp_async_events(struct bnxt *bp, struct hwrm_async_event_cmpl *cmpl);
 struct bnxt_en_dev *bnxt_ulp_probe(struct net_device *dev);
 
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt_xdp.c b/drivers/net/ethernet/broadcom/bnxt/bnxt_xdp.c
index 9dae32756767..7d67552e70d7 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt_xdp.c
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt_xdp.c
@@ -63,7 +63,7 @@ void bnxt_tx_int_xdp(struct bnxt *bp, struct bnxt_napi *bnapi, int nr_pkts)
 		tx_buf = &txr->tx_buf_ring[last_tx_cons];
 		rx_prod = tx_buf->rx_prod;
 	}
-	writel(DB_KEY_RX | rx_prod, rxr->rx_doorbell);
+	bnxt_db_write(bp, rxr->rx_doorbell, DB_KEY_RX | rx_prod);
 }
 
 /* returns the following:
@@ -218,6 +218,7 @@ int bnxt_xdp(struct net_device *dev, struct netdev_xdp *xdp)
 		break;
 	case XDP_QUERY_PROG:
 		xdp->prog_attached = !!bp->xdp_prog;
+		xdp->prog_id = bp->xdp_prog ? bp->xdp_prog->aux->id : 0;
 		rc = 0;
 		break;
 	default:
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet.c b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
index a205a9ff9e17..daca1c9d254b 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
@@ -477,7 +477,9 @@ static int bcmgenet_get_link_ksettings(struct net_device *dev,
 	if (!priv->phydev)
 		return -ENODEV;
 
-	return phy_ethtool_ksettings_get(priv->phydev, cmd);
+	phy_ethtool_ksettings_get(priv->phydev, cmd);
+
+	return 0;
 }
 
 static int bcmgenet_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/broadcom/genet/bcmmii.c b/drivers/net/ethernet/broadcom/genet/bcmmii.c
index 285676f8da6b..071fcbd14e6a 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmmii.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmmii.c
@@ -251,11 +251,8 @@ int bcmgenet_mii_config(struct net_device *dev)
 	priv->ext_phy = !priv->internal_phy &&
 			(priv->phy_interface != PHY_INTERFACE_MODE_MOCA);
 
-	if (priv->internal_phy)
-		priv->phy_interface = PHY_INTERFACE_MODE_NA;
-
 	switch (priv->phy_interface) {
-	case PHY_INTERFACE_MODE_NA:
+	case PHY_INTERFACE_MODE_INTERNAL:
 	case PHY_INTERFACE_MODE_MOCA:
 		/* Irrespective of the actually configured PHY speed (100 or
 		 * 1000) GENETv4 only has an internal GPHY so we will just end
@@ -471,7 +468,6 @@ static int bcmgenet_mii_of_init(struct bcmgenet_priv *priv)
 {
 	struct device_node *dn = priv->pdev->dev.of_node;
 	struct device *kdev = &priv->pdev->dev;
-	const char *phy_mode_str = NULL;
 	struct phy_device *phydev = NULL;
 	char *compat;
 	int phy_mode;
@@ -510,23 +506,19 @@ static int bcmgenet_mii_of_init(struct bcmgenet_priv *priv)
 
 	/* Get the link mode */
 	phy_mode = of_get_phy_mode(dn);
+	if (phy_mode < 0) {
+		dev_err(kdev, "invalid PHY mode property\n");
+		return phy_mode;
+	}
+
 	priv->phy_interface = phy_mode;
 
 	/* We need to specifically look up whether this PHY interface is internal
 	 * or not *before* we even try to probe the PHY driver over MDIO as we
 	 * may have shut down the internal PHY for power saving purposes.
 	 */
-	if (phy_mode < 0) {
-		ret = of_property_read_string(dn, "phy-mode", &phy_mode_str);
-		if (ret < 0) {
-			dev_err(kdev, "invalid PHY mode property\n");
-			return ret;
-		}
-
-		priv->phy_interface = PHY_INTERFACE_MODE_NA;
-		if (!strcasecmp(phy_mode_str, "internal"))
-			priv->internal_phy = true;
-	}
+	if (priv->phy_interface == PHY_INTERFACE_MODE_INTERNAL)
+		priv->internal_phy = true;
 
 	/* Make sure we initialize MoCA PHYs with a link down */
 	if (phy_mode == PHY_INTERFACE_MODE_MOCA) {
diff --git a/drivers/net/ethernet/broadcom/tg3.c b/drivers/net/ethernet/broadcom/tg3.c
index 537d571ee601..d600c41fb1dc 100644
--- a/drivers/net/ethernet/broadcom/tg3.c
+++ b/drivers/net/ethernet/broadcom/tg3.c
@@ -12097,7 +12097,9 @@ static int tg3_get_link_ksettings(struct net_device *dev,
 		if (!(tp->phy_flags & TG3_PHYFLG_IS_CONNECTED))
 			return -EAGAIN;
 		phydev = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr);
-		return phy_ethtool_ksettings_get(phydev, cmd);
+		phy_ethtool_ksettings_get(phydev, cmd);
+
+		return 0;
 	}
 
 	supported = (SUPPORTED_Autoneg);
diff --git a/drivers/net/ethernet/cadence/Kconfig b/drivers/net/ethernet/cadence/Kconfig
index 608bea171956..427d65a1a126 100644
--- a/drivers/net/ethernet/cadence/Kconfig
+++ b/drivers/net/ethernet/cadence/Kconfig
@@ -29,7 +29,15 @@ config MACB
 	  support for the MACB/GEM chip.
 
 	  To compile this driver as a module, choose M here: the module
-	  will be called macb.
+	  will be macb.
+
+config MACB_USE_HWSTAMP
+	bool "Use IEEE 1588 hwstamp"
+	depends on MACB
+	default y
+	imply PTP_1588_CLOCK
+	---help---
+	  Enable IEEE 1588 Precision Time Protocol (PTP) support for MACB.
 
 config MACB_PCI
 	tristate "Cadence PCI MACB/GEM support"
diff --git a/drivers/net/ethernet/cadence/Makefile b/drivers/net/ethernet/cadence/Makefile
index 4ba75594d5c5..1d66ddb68969 100644
--- a/drivers/net/ethernet/cadence/Makefile
+++ b/drivers/net/ethernet/cadence/Makefile
@@ -1,6 +1,11 @@
 #
 # Makefile for the Atmel network device drivers.
 #
+macb-y	:= macb_main.o
+
+ifeq ($(CONFIG_MACB_USE_HWSTAMP),y)
+macb-y	+= macb_ptp.o
+endif
 
 obj-$(CONFIG_MACB) += macb.o
 obj-$(CONFIG_MACB_PCI) += macb_pci.o
diff --git a/drivers/net/ethernet/cadence/macb.h b/drivers/net/ethernet/cadence/macb.h
index ec037b0fa2a4..c93f3a2dc6c1 100644
--- a/drivers/net/ethernet/cadence/macb.h
+++ b/drivers/net/ethernet/cadence/macb.h
@@ -11,6 +11,12 @@
 #define _MACB_H
 
 #include <linux/phy.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/net_tstamp.h>
+
+#if defined(CONFIG_ARCH_DMA_ADDR_T_64BIT) || defined(CONFIG_MACB_USE_HWSTAMP)
+#define MACB_EXT_DESC
+#endif
 
 #define MACB_GREGS_NBR 16
 #define MACB_GREGS_VERSION 2
@@ -86,6 +92,10 @@
 #define GEM_SA3T		0x009C /* Specific3 Top */
 #define GEM_SA4B		0x00A0 /* Specific4 Bottom */
 #define GEM_SA4T		0x00A4 /* Specific4 Top */
+#define GEM_EFTSH		0x00e8 /* PTP Event Frame Transmitted Seconds Register 47:32 */
+#define GEM_EFRSH		0x00ec /* PTP Event Frame Received Seconds Register 47:32 */
+#define GEM_PEFTSH		0x00f0 /* PTP Peer Event Frame Transmitted Seconds Register 47:32 */
+#define GEM_PEFRSH		0x00f4 /* PTP Peer Event Frame Received Seconds Register 47:32 */
 #define GEM_OTX			0x0100 /* Octets transmitted */
 #define GEM_OCTTXL		0x0100 /* Octets transmitted [31:0] */
 #define GEM_OCTTXH		0x0104 /* Octets transmitted [47:32] */
@@ -155,6 +165,9 @@
 #define GEM_DCFG6		0x0294 /* Design Config 6 */
 #define GEM_DCFG7		0x0298 /* Design Config 7 */
 
+#define GEM_TXBDCTRL	0x04cc /* TX Buffer Descriptor control register */
+#define GEM_RXBDCTRL	0x04d0 /* RX Buffer Descriptor control register */
+
 #define GEM_ISR(hw_q)		(0x0400 + ((hw_q) << 2))
 #define GEM_TBQP(hw_q)		(0x0440 + ((hw_q) << 2))
 #define GEM_TBQPH(hw_q)		(0x04C8)
@@ -191,6 +204,8 @@
 #define MACB_TZQ_OFFSET		12 /* Transmit zero quantum pause frame */
 #define MACB_TZQ_SIZE		1
 #define MACB_SRTSM_OFFSET	15
+#define MACB_OSSMODE_OFFSET 24 /* Enable One Step Synchro Mode */
+#define MACB_OSSMODE_SIZE	1
 
 /* Bitfields in NCFGR */
 #define MACB_SPD_OFFSET		0 /* Speed */
@@ -269,6 +284,10 @@
 #define GEM_RXBS_SIZE		8
 #define GEM_DDRP_OFFSET		24 /* disc_when_no_ahb */
 #define GEM_DDRP_SIZE		1
+#define GEM_RXEXT_OFFSET	28 /* RX extended Buffer Descriptor mode */
+#define GEM_RXEXT_SIZE		1
+#define GEM_TXEXT_OFFSET	29 /* TX extended Buffer Descriptor mode */
+#define GEM_TXEXT_SIZE		1
 #define GEM_ADDR64_OFFSET	30 /* Address bus width - 64b or 32b */
 #define GEM_ADDR64_SIZE		1
 
@@ -425,6 +444,11 @@
 #define GEM_TX_PKT_BUFF_OFFSET			21
 #define GEM_TX_PKT_BUFF_SIZE			1
 
+
+/* Bitfields in DCFG5. */
+#define GEM_TSU_OFFSET				8
+#define GEM_TSU_SIZE				1
+
 /* Bitfields in DCFG6. */
 #define GEM_PBUF_LSO_OFFSET			27
 #define GEM_PBUF_LSO_SIZE			1
@@ -439,6 +463,52 @@
 #define GEM_NSINCR_OFFSET			0
 #define GEM_NSINCR_SIZE				8
 
+/* Bitfields in TSH */
+#define GEM_TSH_OFFSET				0 /* TSU timer value (s). MSB [47:32] of seconds timer count */
+#define GEM_TSH_SIZE				16
+
+/* Bitfields in TSL */
+#define GEM_TSL_OFFSET				0 /* TSU timer value (s). LSB [31:0] of seconds timer count */
+#define GEM_TSL_SIZE				32
+
+/* Bitfields in TN */
+#define GEM_TN_OFFSET				0 /* TSU timer value (ns) */
+#define GEM_TN_SIZE					30
+
+/* Bitfields in TXBDCTRL */
+#define GEM_TXTSMODE_OFFSET			4 /* TX Descriptor Timestamp Insertion mode */
+#define GEM_TXTSMODE_SIZE			2
+
+/* Bitfields in RXBDCTRL */
+#define GEM_RXTSMODE_OFFSET			4 /* RX Descriptor Timestamp Insertion mode */
+#define GEM_RXTSMODE_SIZE			2
+
+/* Transmit DMA buffer descriptor Word 1 */
+#define GEM_DMA_TXVALID_OFFSET		23 /* timestamp has been captured in the Buffer Descriptor */
+#define GEM_DMA_TXVALID_SIZE		1
+
+/* Receive DMA buffer descriptor Word 0 */
+#define GEM_DMA_RXVALID_OFFSET		2 /* indicates a valid timestamp in the Buffer Descriptor */
+#define GEM_DMA_RXVALID_SIZE		1
+
+/* DMA buffer descriptor Word 2 (32 bit addressing) or Word 4 (64 bit addressing) */
+#define GEM_DMA_SECL_OFFSET			30 /* Timestamp seconds[1:0]  */
+#define GEM_DMA_SECL_SIZE			2
+#define GEM_DMA_NSEC_OFFSET			0 /* Timestamp nanosecs [29:0] */
+#define GEM_DMA_NSEC_SIZE			30
+
+/* DMA buffer descriptor Word 3 (32 bit addressing) or Word 5 (64 bit addressing) */
+
+/* New hardware supports 12 bit precision of timestamp in DMA buffer descriptor.
+ * Old hardware supports only 6 bit precision but it is enough for PTP.
+ * Less accuracy is used always instead of checking hardware version.
+ */
+#define GEM_DMA_SECH_OFFSET			0 /* Timestamp seconds[5:2] */
+#define GEM_DMA_SECH_SIZE			4
+#define GEM_DMA_SEC_WIDTH			(GEM_DMA_SECH_SIZE + GEM_DMA_SECL_SIZE)
+#define GEM_DMA_SEC_TOP				(1 << GEM_DMA_SEC_WIDTH)
+#define GEM_DMA_SEC_MASK			(GEM_DMA_SEC_TOP - 1)
+
 /* Bitfields in ADJ */
 #define GEM_ADDSUB_OFFSET			31
 #define GEM_ADDSUB_SIZE				1
@@ -514,6 +584,8 @@
 #define queue_readl(queue, reg)		(queue)->bp->macb_reg_readl((queue)->bp, (queue)->reg)
 #define queue_writel(queue, reg, value)	(queue)->bp->macb_reg_writel((queue)->bp, (queue)->reg, (value))
 
+#define PTP_TS_BUFFER_SIZE		128 /* must be power of 2 */
+
 /* Conditional GEM/MACB macros.  These perform the operation to the correct
  * register dependent on whether the device is a GEM or a MACB.  For registers
  * and bitfields that are common across both devices, use macb_{read,write}l
@@ -546,16 +618,26 @@ struct macb_dma_desc {
 	u32	ctrl;
 };
 
-#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
-enum macb_hw_dma_cap {
-	HW_DMA_CAP_32B,
-	HW_DMA_CAP_64B,
-};
+#ifdef MACB_EXT_DESC
+#define HW_DMA_CAP_32B		0
+#define HW_DMA_CAP_64B		(1 << 0)
+#define HW_DMA_CAP_PTP		(1 << 1)
+#define HW_DMA_CAP_64B_PTP	(HW_DMA_CAP_64B | HW_DMA_CAP_PTP)
 
 struct macb_dma_desc_64 {
 	u32 addrh;
 	u32 resvd;
 };
+
+struct macb_dma_desc_ptp {
+	u32	ts_1;
+	u32	ts_2;
+};
+
+struct gem_tx_ts {
+	struct sk_buff *skb;
+	struct macb_dma_desc_ptp desc_ptp;
+};
 #endif
 
 /* DMA descriptor bitfields */
@@ -871,6 +953,11 @@ struct macb_config {
 	int	jumbo_max_len;
 };
 
+struct tsu_incr {
+	u32 sub_ns;
+	u32 ns;
+};
+
 struct macb_queue {
 	struct macb		*bp;
 	int			irq;
@@ -887,6 +974,12 @@ struct macb_queue {
 	struct macb_tx_skb	*tx_skb;
 	dma_addr_t		tx_ring_dma;
 	struct work_struct	tx_error_task;
+
+#ifdef CONFIG_MACB_USE_HWSTAMP
+	struct work_struct	tx_ts_task;
+	unsigned int		tx_ts_head, tx_ts_tail;
+	struct gem_tx_ts	tx_timestamps[PTP_TS_BUFFER_SIZE];
+#endif
 };
 
 struct macb {
@@ -930,6 +1023,7 @@ struct macb {
 	struct macb_or_gem_ops	macbgem_ops;
 
 	struct mii_bus		*mii_bus;
+	struct device_node	*phy_node;
 	int 			link;
 	int 			speed;
 	int 			duplex;
@@ -954,11 +1048,62 @@ struct macb {
 	u32			wol;
 
 	struct macb_ptp_info	*ptp_info;	/* macb-ptp interface */
-#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
-	enum macb_hw_dma_cap hw_dma_cap;
+#ifdef MACB_EXT_DESC
+	uint8_t hw_dma_cap;
 #endif
+	spinlock_t tsu_clk_lock; /* gem tsu clock locking */
+	unsigned int tsu_rate;
+	struct ptp_clock *ptp_clock;
+	struct ptp_clock_info ptp_clock_info;
+	struct tsu_incr tsu_incr;
+	struct hwtstamp_config tstamp_config;
 };
 
+#ifdef CONFIG_MACB_USE_HWSTAMP
+#define GEM_TSEC_SIZE  (GEM_TSH_SIZE + GEM_TSL_SIZE)
+#define TSU_SEC_MAX_VAL (((u64)1 << GEM_TSEC_SIZE) - 1)
+#define TSU_NSEC_MAX_VAL ((1 << GEM_TN_SIZE) - 1)
+
+enum macb_bd_control {
+	TSTAMP_DISABLED,
+	TSTAMP_FRAME_PTP_EVENT_ONLY,
+	TSTAMP_ALL_PTP_FRAMES,
+	TSTAMP_ALL_FRAMES,
+};
+
+void gem_ptp_init(struct net_device *ndev);
+void gem_ptp_remove(struct net_device *ndev);
+int gem_ptp_txstamp(struct macb_queue *queue, struct sk_buff *skb, struct macb_dma_desc *des);
+void gem_ptp_rxstamp(struct macb *bp, struct sk_buff *skb, struct macb_dma_desc *desc);
+static inline int gem_ptp_do_txstamp(struct macb_queue *queue, struct sk_buff *skb, struct macb_dma_desc *desc)
+{
+	if (queue->bp->tstamp_config.tx_type == TSTAMP_DISABLED)
+		return -ENOTSUPP;
+
+	return gem_ptp_txstamp(queue, skb, desc);
+}
+
+static inline void gem_ptp_do_rxstamp(struct macb *bp, struct sk_buff *skb, struct macb_dma_desc *desc)
+{
+	if (bp->tstamp_config.rx_filter == TSTAMP_DISABLED)
+		return;
+
+	gem_ptp_rxstamp(bp, skb, desc);
+}
+int gem_get_hwtst(struct net_device *dev, struct ifreq *rq);
+int gem_set_hwtst(struct net_device *dev, struct ifreq *ifr, int cmd);
+#else
+static inline void gem_ptp_init(struct net_device *ndev) { }
+static inline void gem_ptp_remove(struct net_device *ndev) { }
+
+static inline int gem_ptp_do_txstamp(struct macb_queue *queue, struct sk_buff *skb, struct macb_dma_desc *desc)
+{
+	return -1;
+}
+
+static inline void gem_ptp_do_rxstamp(struct macb *bp, struct sk_buff *skb, struct macb_dma_desc *desc) { }
+#endif
+
 static inline bool macb_is_gem(struct macb *bp)
 {
 	return !!(bp->caps & MACB_CAPS_MACB_IS_GEM);
diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb_main.c
index 91f7492623d3..e69ebdd65658 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb_main.c
@@ -79,33 +79,83 @@
 #define MACB_HALT_TIMEOUT	1230
 
 /* DMA buffer descriptor might be different size
- * depends on hardware configuration.
+ * depends on hardware configuration:
+ *
+ * 1. dma address width 32 bits:
+ *    word 1: 32 bit address of Data Buffer
+ *    word 2: control
+ *
+ * 2. dma address width 64 bits:
+ *    word 1: 32 bit address of Data Buffer
+ *    word 2: control
+ *    word 3: upper 32 bit address of Data Buffer
+ *    word 4: unused
+ *
+ * 3. dma address width 32 bits with hardware timestamping:
+ *    word 1: 32 bit address of Data Buffer
+ *    word 2: control
+ *    word 3: timestamp word 1
+ *    word 4: timestamp word 2
+ *
+ * 4. dma address width 64 bits with hardware timestamping:
+ *    word 1: 32 bit address of Data Buffer
+ *    word 2: control
+ *    word 3: upper 32 bit address of Data Buffer
+ *    word 4: unused
+ *    word 5: timestamp word 1
+ *    word 6: timestamp word 2
  */
 static unsigned int macb_dma_desc_get_size(struct macb *bp)
 {
-#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
-	if (bp->hw_dma_cap == HW_DMA_CAP_64B)
-		return sizeof(struct macb_dma_desc) + sizeof(struct macb_dma_desc_64);
+#ifdef MACB_EXT_DESC
+	unsigned int desc_size;
+
+	switch (bp->hw_dma_cap) {
+	case HW_DMA_CAP_64B:
+		desc_size = sizeof(struct macb_dma_desc)
+			+ sizeof(struct macb_dma_desc_64);
+		break;
+	case HW_DMA_CAP_PTP:
+		desc_size = sizeof(struct macb_dma_desc)
+			+ sizeof(struct macb_dma_desc_ptp);
+		break;
+	case HW_DMA_CAP_64B_PTP:
+		desc_size = sizeof(struct macb_dma_desc)
+			+ sizeof(struct macb_dma_desc_64)
+			+ sizeof(struct macb_dma_desc_ptp);
+		break;
+	default:
+		desc_size = sizeof(struct macb_dma_desc);
+	}
+	return desc_size;
 #endif
 	return sizeof(struct macb_dma_desc);
 }
 
-static unsigned int macb_adj_dma_desc_idx(struct macb *bp, unsigned int idx)
+static unsigned int macb_adj_dma_desc_idx(struct macb *bp, unsigned int desc_idx)
 {
-#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
-	/* Dma buffer descriptor is 4 words length (instead of 2 words)
-	 * for 64b GEM.
-	 */
-	if (bp->hw_dma_cap == HW_DMA_CAP_64B)
-		idx <<= 1;
+#ifdef MACB_EXT_DESC
+	switch (bp->hw_dma_cap) {
+	case HW_DMA_CAP_64B:
+	case HW_DMA_CAP_PTP:
+		desc_idx <<= 1;
+		break;
+	case HW_DMA_CAP_64B_PTP:
+		desc_idx *= 3;
+		break;
+	default:
+		break;
+	}
 #endif
-	return idx;
+	return desc_idx;
 }
 
 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
 static struct macb_dma_desc_64 *macb_64b_desc(struct macb *bp, struct macb_dma_desc *desc)
 {
-	return (struct macb_dma_desc_64 *)((void *)desc + sizeof(struct macb_dma_desc));
+	if (bp->hw_dma_cap & HW_DMA_CAP_64B)
+		return (struct macb_dma_desc_64 *)((void *)desc + sizeof(struct macb_dma_desc));
+	return NULL;
 }
 #endif
 
@@ -425,32 +475,40 @@ static int macb_mii_probe(struct net_device *dev)
 	int phy_irq;
 	int ret;
 
-	phydev = phy_find_first(bp->mii_bus);
-	if (!phydev) {
-		netdev_err(dev, "no PHY found\n");
-		return -ENXIO;
-	}
+	if (bp->phy_node) {
+		phydev = of_phy_connect(dev, bp->phy_node,
+					&macb_handle_link_change, 0,
+					bp->phy_interface);
+		if (!phydev)
+			return -ENODEV;
+	} else {
+		phydev = phy_find_first(bp->mii_bus);
+		if (!phydev) {
+			netdev_err(dev, "no PHY found\n");
+			return -ENXIO;
+		}
 
-	pdata = dev_get_platdata(&bp->pdev->dev);
-	if (pdata) {
-		if (gpio_is_valid(pdata->phy_irq_pin)) {
-			ret = devm_gpio_request(&bp->pdev->dev,
-						pdata->phy_irq_pin, "phy int");
-			if (!ret) {
-				phy_irq = gpio_to_irq(pdata->phy_irq_pin);
-				phydev->irq = (phy_irq < 0) ? PHY_POLL : phy_irq;
+		pdata = dev_get_platdata(&bp->pdev->dev);
+		if (pdata) {
+			if (gpio_is_valid(pdata->phy_irq_pin)) {
+				ret = devm_gpio_request(&bp->pdev->dev,
+							pdata->phy_irq_pin, "phy int");
+				if (!ret) {
+					phy_irq = gpio_to_irq(pdata->phy_irq_pin);
+					phydev->irq = (phy_irq < 0) ? PHY_POLL : phy_irq;
+				}
+			} else {
+				phydev->irq = PHY_POLL;
 			}
-		} else {
-			phydev->irq = PHY_POLL;
 		}
-	}
 
-	/* attach the mac to the phy */
-	ret = phy_connect_direct(dev, phydev, &macb_handle_link_change,
-				 bp->phy_interface);
-	if (ret) {
-		netdev_err(dev, "Could not attach to PHY\n");
-		return ret;
+		/* attach the mac to the phy */
+		ret = phy_connect_direct(dev, phydev, &macb_handle_link_change,
+					 bp->phy_interface);
+		if (ret) {
+			netdev_err(dev, "Could not attach to PHY\n");
+			return ret;
+		}
 	}
 
 	/* mask with MAC supported features */
@@ -499,26 +557,37 @@ static int macb_mii_init(struct macb *bp)
 
 	np = bp->pdev->dev.of_node;
 	if (np) {
-		/* try dt phy registration */
-		err = of_mdiobus_register(bp->mii_bus, np);
+		if (of_phy_is_fixed_link(np)) {
+			if (of_phy_register_fixed_link(np) < 0) {
+				dev_err(&bp->pdev->dev,
+					"broken fixed-link specification\n");
+				goto err_out_unregister_bus;
+			}
+			bp->phy_node = of_node_get(np);
 
-		/* fallback to standard phy registration if no phy were
-		 * found during dt phy registration
-		 */
-		if (!err && !phy_find_first(bp->mii_bus)) {
-			for (i = 0; i < PHY_MAX_ADDR; i++) {
-				struct phy_device *phydev;
-
-				phydev = mdiobus_scan(bp->mii_bus, i);
-				if (IS_ERR(phydev) &&
-				    PTR_ERR(phydev) != -ENODEV) {
-					err = PTR_ERR(phydev);
-					break;
+			err = mdiobus_register(bp->mii_bus);
+		} else {
+			/* try dt phy registration */
+			err = of_mdiobus_register(bp->mii_bus, np);
+
+			/* fallback to standard phy registration if no phy were
+			 * found during dt phy registration
+			 */
+			if (!err && !phy_find_first(bp->mii_bus)) {
+				for (i = 0; i < PHY_MAX_ADDR; i++) {
+					struct phy_device *phydev;
+
+					phydev = mdiobus_scan(bp->mii_bus, i);
+					if (IS_ERR(phydev) &&
+					    PTR_ERR(phydev) != -ENODEV) {
+						err = PTR_ERR(phydev);
+						break;
+					}
 				}
-			}
 
-			if (err)
-				goto err_out_unregister_bus;
+				if (err)
+					goto err_out_unregister_bus;
+			}
 		}
 	} else {
 		for (i = 0; i < PHY_MAX_ADDR; i++)
@@ -602,7 +671,7 @@ static void macb_set_addr(struct macb *bp, struct macb_dma_desc *desc, dma_addr_
 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
 	struct macb_dma_desc_64 *desc_64;
 
-	if (bp->hw_dma_cap == HW_DMA_CAP_64B) {
+	if (bp->hw_dma_cap & HW_DMA_CAP_64B) {
 		desc_64 = macb_64b_desc(bp, desc);
 		desc_64->addrh = upper_32_bits(addr);
 	}
@@ -616,7 +685,7 @@ static dma_addr_t macb_get_addr(struct macb *bp, struct macb_dma_desc *desc)
 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
 	struct macb_dma_desc_64 *desc_64;
 
-	if (bp->hw_dma_cap == HW_DMA_CAP_64B) {
+	if (bp->hw_dma_cap & HW_DMA_CAP_64B) {
 		desc_64 = macb_64b_desc(bp, desc);
 		addr = ((u64)(desc_64->addrh) << 32);
 	}
@@ -715,7 +784,7 @@ static void macb_tx_error_task(struct work_struct *work)
 	/* Reinitialize the TX desc queue */
 	queue_writel(queue, TBQP, lower_32_bits(queue->tx_ring_dma));
 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
-	if (bp->hw_dma_cap == HW_DMA_CAP_64B)
+	if (bp->hw_dma_cap & HW_DMA_CAP_64B)
 		queue_writel(queue, TBQPH, upper_32_bits(queue->tx_ring_dma));
 #endif
 	/* Make TX ring reflect state of hardware */
@@ -777,6 +846,12 @@ static void macb_tx_interrupt(struct macb_queue *queue)
 
 			/* First, update TX stats if needed */
 			if (skb) {
+				if (gem_ptp_do_txstamp(queue, skb, desc) == 0) {
+					/* skb now belongs to timestamp buffer
+					 * and will be removed later
+					 */
+					tx_skb->skb = NULL;
+				}
 				netdev_vdbg(bp->dev, "skb %u (data %p) TX complete\n",
 					    macb_tx_ring_wrap(bp, tail),
 					    skb->data);
@@ -943,6 +1018,8 @@ static int gem_rx(struct macb *bp, int budget)
 		bp->dev->stats.rx_packets++;
 		bp->dev->stats.rx_bytes += skb->len;
 
+		gem_ptp_do_rxstamp(bp, skb, desc);
+
 #if defined(DEBUG) && defined(VERBOSE_DEBUG)
 		netdev_vdbg(bp->dev, "received skb of length %u, csum: %08x\n",
 			    skb->len, skb->csum);
@@ -1264,7 +1341,6 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
 			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
 				queue_writel(queue, ISR, MACB_BIT(HRESP));
 		}
-
 		status = queue_readl(queue, ISR);
 	}
 
@@ -1594,7 +1670,6 @@ static int macb_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* Make newly initialized descriptor visible to hardware */
 	wmb();
-
 	skb_tx_timestamp(skb);
 
 	macb_writel(bp, NCR, macb_readl(bp, NCR) | MACB_BIT(TSTART));
@@ -1923,9 +1998,13 @@ static void macb_configure_dma(struct macb *bp)
 			dmacfg &= ~GEM_BIT(TXCOEN);
 
 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
-		if (bp->hw_dma_cap == HW_DMA_CAP_64B)
+		if (bp->hw_dma_cap & HW_DMA_CAP_64B)
 			dmacfg |= GEM_BIT(ADDR64);
 #endif
+#ifdef CONFIG_MACB_USE_HWSTAMP
+		if (bp->hw_dma_cap & HW_DMA_CAP_PTP)
+			dmacfg |= GEM_BIT(RXEXT) | GEM_BIT(TXEXT);
+#endif
 		netdev_dbg(bp->dev, "Cadence configure DMA with 0x%08x\n",
 			   dmacfg);
 		gem_writel(bp, DMACFG, dmacfg);
@@ -1973,13 +2052,13 @@ static void macb_init_hw(struct macb *bp)
 	/* Initialize TX and RX buffers */
 	macb_writel(bp, RBQP, lower_32_bits(bp->rx_ring_dma));
 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
-	if (bp->hw_dma_cap == HW_DMA_CAP_64B)
+	if (bp->hw_dma_cap & HW_DMA_CAP_64B)
 		macb_writel(bp, RBQPH, upper_32_bits(bp->rx_ring_dma));
 #endif
 	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
 		queue_writel(queue, TBQP, lower_32_bits(queue->tx_ring_dma));
 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
-		if (bp->hw_dma_cap == HW_DMA_CAP_64B)
+		if (bp->hw_dma_cap & HW_DMA_CAP_64B)
 			queue_writel(queue, TBQPH, upper_32_bits(queue->tx_ring_dma));
 #endif
 
@@ -2448,6 +2527,70 @@ static int macb_set_ringparam(struct net_device *netdev,
 	return 0;
 }
 
+#ifdef CONFIG_MACB_USE_HWSTAMP
+static unsigned int gem_get_tsu_rate(struct macb *bp)
+{
+	struct clk *tsu_clk;
+	unsigned int tsu_rate;
+
+	tsu_clk = devm_clk_get(&bp->pdev->dev, "tsu_clk");
+	if (!IS_ERR(tsu_clk))
+		tsu_rate = clk_get_rate(tsu_clk);
+	/* try pclk instead */
+	else if (!IS_ERR(bp->pclk)) {
+		tsu_clk = bp->pclk;
+		tsu_rate = clk_get_rate(tsu_clk);
+	} else
+		return -ENOTSUPP;
+	return tsu_rate;
+}
+
+static s32 gem_get_ptp_max_adj(void)
+{
+	return 64000000;
+}
+
+static int gem_get_ts_info(struct net_device *dev,
+			   struct ethtool_ts_info *info)
+{
+	struct macb *bp = netdev_priv(dev);
+
+	if ((bp->hw_dma_cap & HW_DMA_CAP_PTP) == 0) {
+		ethtool_op_get_ts_info(dev, info);
+		return 0;
+	}
+
+	info->so_timestamping =
+		SOF_TIMESTAMPING_TX_SOFTWARE |
+		SOF_TIMESTAMPING_RX_SOFTWARE |
+		SOF_TIMESTAMPING_SOFTWARE |
+		SOF_TIMESTAMPING_TX_HARDWARE |
+		SOF_TIMESTAMPING_RX_HARDWARE |
+		SOF_TIMESTAMPING_RAW_HARDWARE;
+	info->tx_types =
+		(1 << HWTSTAMP_TX_ONESTEP_SYNC) |
+		(1 << HWTSTAMP_TX_OFF) |
+		(1 << HWTSTAMP_TX_ON);
+	info->rx_filters =
+		(1 << HWTSTAMP_FILTER_NONE) |
+		(1 << HWTSTAMP_FILTER_ALL);
+
+	info->phc_index = bp->ptp_clock ? ptp_clock_index(bp->ptp_clock) : -1;
+
+	return 0;
+}
+
+static struct macb_ptp_info gem_ptp_info = {
+	.ptp_init	 = gem_ptp_init,
+	.ptp_remove	 = gem_ptp_remove,
+	.get_ptp_max_adj = gem_get_ptp_max_adj,
+	.get_tsu_rate	 = gem_get_tsu_rate,
+	.get_ts_info	 = gem_get_ts_info,
+	.get_hwtst	 = gem_get_hwtst,
+	.set_hwtst	 = gem_set_hwtst,
+};
+#endif
+
 static int macb_get_ts_info(struct net_device *netdev,
 			    struct ethtool_ts_info *info)
 {
@@ -2581,6 +2724,16 @@ static void macb_configure_caps(struct macb *bp,
 		dcfg = gem_readl(bp, DCFG2);
 		if ((dcfg & (GEM_BIT(RX_PKT_BUFF) | GEM_BIT(TX_PKT_BUFF))) == 0)
 			bp->caps |= MACB_CAPS_FIFO_MODE;
+#ifdef CONFIG_MACB_USE_HWSTAMP
+		if (gem_has_ptp(bp)) {
+			if (!GEM_BFEXT(TSU, gem_readl(bp, DCFG5)))
+				pr_err("GEM doesn't support hardware ptp.\n");
+			else {
+				bp->hw_dma_cap |= HW_DMA_CAP_PTP;
+				bp->ptp_info = &gem_ptp_info;
+			}
+		}
+#endif
 	}
 
 	dev_dbg(&bp->pdev->dev, "Cadence caps 0x%08x\n", bp->caps);
@@ -2718,7 +2871,7 @@ static int macb_init(struct platform_device *pdev)
 			queue->IMR  = GEM_IMR(hw_q - 1);
 			queue->TBQP = GEM_TBQP(hw_q - 1);
 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
-			if (bp->hw_dma_cap == HW_DMA_CAP_64B)
+			if (bp->hw_dma_cap & HW_DMA_CAP_64B)
 				queue->TBQPH = GEM_TBQPH(hw_q - 1);
 #endif
 		} else {
@@ -2729,7 +2882,7 @@ static int macb_init(struct platform_device *pdev)
 			queue->IMR  = MACB_IMR;
 			queue->TBQP = MACB_TBQP;
 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
-			if (bp->hw_dma_cap == HW_DMA_CAP_64B)
+			if (bp->hw_dma_cap & HW_DMA_CAP_64B)
 				queue->TBQPH = MACB_TBQPH;
 #endif
 		}
@@ -2992,7 +3145,7 @@ static void at91ether_rx(struct net_device *dev)
 		skb = netdev_alloc_skb(dev, pktlen + 2);
 		if (skb) {
 			skb_reserve(skb, 2);
-			memcpy(skb_put(skb, pktlen), p_recv, pktlen);
+			skb_put_data(skb, p_recv, pktlen);
 
 			skb->protocol = eth_type_trans(skb, dev);
 			dev->stats.rx_packets++;
@@ -3186,7 +3339,9 @@ static const struct macb_config np4_config = {
 };
 
 static const struct macb_config zynqmp_config = {
-	.caps = MACB_CAPS_GIGABIT_MODE_AVAILABLE | MACB_CAPS_JUMBO,
+	.caps = MACB_CAPS_GIGABIT_MODE_AVAILABLE |
+			MACB_CAPS_JUMBO |
+			MACB_CAPS_GEM_HAS_PTP,
 	.dma_burst_length = 16,
 	.clk_init = macb_clk_init,
 	.init = macb_init,
@@ -3220,7 +3375,9 @@ MODULE_DEVICE_TABLE(of, macb_dt_ids);
 #endif /* CONFIG_OF */
 
 static const struct macb_config default_gem_config = {
-	.caps = MACB_CAPS_GIGABIT_MODE_AVAILABLE | MACB_CAPS_JUMBO,
+	.caps = MACB_CAPS_GIGABIT_MODE_AVAILABLE |
+			MACB_CAPS_JUMBO |
+			MACB_CAPS_GEM_HAS_PTP,
 	.dma_burst_length = 16,
 	.clk_init = macb_clk_init,
 	.init = macb_init,
@@ -3309,19 +3466,17 @@ static int macb_probe(struct platform_device *pdev)
 		bp->wol |= MACB_WOL_HAS_MAGIC_PACKET;
 	device_init_wakeup(&pdev->dev, bp->wol & MACB_WOL_HAS_MAGIC_PACKET);
 
-#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
-	if (GEM_BFEXT(DAW64, gem_readl(bp, DCFG6))) {
-		dma_set_mask(&pdev->dev, DMA_BIT_MASK(44));
-		bp->hw_dma_cap = HW_DMA_CAP_64B;
-	} else
-		bp->hw_dma_cap = HW_DMA_CAP_32B;
-#endif
-
 	spin_lock_init(&bp->lock);
 
 	/* setup capabilities */
 	macb_configure_caps(bp, macb_config);
 
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+	if (GEM_BFEXT(DAW64, gem_readl(bp, DCFG6))) {
+		dma_set_mask(&pdev->dev, DMA_BIT_MASK(44));
+		bp->hw_dma_cap |= HW_DMA_CAP_64B;
+	}
+#endif
 	platform_set_drvdata(pdev, dev);
 
 	dev->irq = platform_get_irq(pdev, 0);
@@ -3438,6 +3593,7 @@ static int macb_remove(struct platform_device *pdev)
 		clk_disable_unprepare(bp->hclk);
 		clk_disable_unprepare(bp->pclk);
 		clk_disable_unprepare(bp->rx_clk);
+		of_node_put(bp->phy_node);
 		free_netdev(dev);
 	}
 
diff --git a/drivers/net/ethernet/cadence/macb_ptp.c b/drivers/net/ethernet/cadence/macb_ptp.c
new file mode 100755
index 000000000000..67cca08472b7
--- /dev/null
+++ b/drivers/net/ethernet/cadence/macb_ptp.c
@@ -0,0 +1,518 @@
+/**
+ * 1588 PTP support for Cadence GEM device.
+ *
+ * Copyright (C) 2017 Cadence Design Systems - http://www.cadence.com
+ *
+ * Authors: Rafal Ozieblo <rafalo@cadence.com>
+ *          Bartosz Folta <bfolta@cadence.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2  of
+ * the License as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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/types.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/etherdevice.h>
+#include <linux/platform_device.h>
+#include <linux/time64.h>
+#include <linux/ptp_classify.h>
+#include <linux/if_ether.h>
+#include <linux/if_vlan.h>
+#include <linux/net_tstamp.h>
+#include <linux/circ_buf.h>
+#include <linux/spinlock.h>
+
+#include "macb.h"
+
+#define  GEM_PTP_TIMER_NAME "gem-ptp-timer"
+
+static struct macb_dma_desc_ptp *macb_ptp_desc(struct macb *bp,
+					       struct macb_dma_desc *desc)
+{
+	if (bp->hw_dma_cap == HW_DMA_CAP_PTP)
+		return (struct macb_dma_desc_ptp *)
+				((u8 *)desc + sizeof(struct macb_dma_desc));
+	if (bp->hw_dma_cap == HW_DMA_CAP_64B_PTP)
+		return (struct macb_dma_desc_ptp *)
+				((u8 *)desc + sizeof(struct macb_dma_desc)
+				+ sizeof(struct macb_dma_desc_64));
+	return NULL;
+}
+
+static int gem_tsu_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+	struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
+	unsigned long flags;
+	long first, second;
+	u32 secl, sech;
+
+	spin_lock_irqsave(&bp->tsu_clk_lock, flags);
+	first = gem_readl(bp, TN);
+	secl = gem_readl(bp, TSL);
+	sech = gem_readl(bp, TSH);
+	second = gem_readl(bp, TN);
+
+	/* test for nsec rollover */
+	if (first > second) {
+		/* if so, use later read & re-read seconds
+		 * (assume all done within 1s)
+		 */
+		ts->tv_nsec = gem_readl(bp, TN);
+		secl = gem_readl(bp, TSL);
+		sech = gem_readl(bp, TSH);
+	} else {
+		ts->tv_nsec = first;
+	}
+
+	spin_unlock_irqrestore(&bp->tsu_clk_lock, flags);
+	ts->tv_sec = (((u64)sech << GEM_TSL_SIZE) | secl)
+			& TSU_SEC_MAX_VAL;
+	return 0;
+}
+
+static int gem_tsu_set_time(struct ptp_clock_info *ptp,
+			    const struct timespec64 *ts)
+{
+	struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
+	unsigned long flags;
+	u32 ns, sech, secl;
+
+	secl = (u32)ts->tv_sec;
+	sech = (ts->tv_sec >> GEM_TSL_SIZE) & ((1 << GEM_TSH_SIZE) - 1);
+	ns = ts->tv_nsec;
+
+	spin_lock_irqsave(&bp->tsu_clk_lock, flags);
+
+	/* TSH doesn't latch the time and no atomicity! */
+	gem_writel(bp, TN, 0); /* clear to avoid overflow */
+	gem_writel(bp, TSH, sech);
+	/* write lower bits 2nd, for synchronized secs update */
+	gem_writel(bp, TSL, secl);
+	gem_writel(bp, TN, ns);
+
+	spin_unlock_irqrestore(&bp->tsu_clk_lock, flags);
+
+	return 0;
+}
+
+static int gem_tsu_incr_set(struct macb *bp, struct tsu_incr *incr_spec)
+{
+	unsigned long flags;
+
+	/* tsu_timer_incr register must be written after
+	 * the tsu_timer_incr_sub_ns register and the write operation
+	 * will cause the value written to the tsu_timer_incr_sub_ns register
+	 * to take effect.
+	 */
+	spin_lock_irqsave(&bp->tsu_clk_lock, flags);
+	gem_writel(bp, TISUBN, GEM_BF(SUBNSINCR, incr_spec->sub_ns));
+	gem_writel(bp, TI, GEM_BF(NSINCR, incr_spec->ns));
+	spin_unlock_irqrestore(&bp->tsu_clk_lock, flags);
+
+	return 0;
+}
+
+static int gem_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
+{
+	struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
+	struct tsu_incr incr_spec;
+	bool neg_adj = false;
+	u32 word;
+	u64 adj;
+
+	if (scaled_ppm < 0) {
+		neg_adj = true;
+		scaled_ppm = -scaled_ppm;
+	}
+
+	/* Adjustment is relative to base frequency */
+	incr_spec.sub_ns = bp->tsu_incr.sub_ns;
+	incr_spec.ns = bp->tsu_incr.ns;
+
+	/* scaling: unused(8bit) | ns(8bit) | fractions(16bit) */
+	word = ((u64)incr_spec.ns << GEM_SUBNSINCR_SIZE) + incr_spec.sub_ns;
+	adj = (u64)scaled_ppm * word;
+	/* Divide with rounding, equivalent to floating dividing:
+	 * (temp / USEC_PER_SEC) + 0.5
+	 */
+	adj += (USEC_PER_SEC >> 1);
+	adj >>= GEM_SUBNSINCR_SIZE; /* remove fractions */
+	adj = div_u64(adj, USEC_PER_SEC);
+	adj = neg_adj ? (word - adj) : (word + adj);
+
+	incr_spec.ns = (adj >> GEM_SUBNSINCR_SIZE)
+			& ((1 << GEM_NSINCR_SIZE) - 1);
+	incr_spec.sub_ns = adj & ((1 << GEM_SUBNSINCR_SIZE) - 1);
+	gem_tsu_incr_set(bp, &incr_spec);
+	return 0;
+}
+
+static int gem_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
+	struct timespec64 now, then = ns_to_timespec64(delta);
+	u32 adj, sign = 0;
+
+	if (delta < 0) {
+		sign = 1;
+		delta = -delta;
+	}
+
+	if (delta > TSU_NSEC_MAX_VAL) {
+		gem_tsu_get_time(&bp->ptp_clock_info, &now);
+		if (sign)
+			now = timespec64_sub(now, then);
+		else
+			now = timespec64_add(now, then);
+
+		gem_tsu_set_time(&bp->ptp_clock_info,
+				 (const struct timespec64 *)&now);
+	} else {
+		adj = (sign << GEM_ADDSUB_OFFSET) | delta;
+
+		gem_writel(bp, TA, adj);
+	}
+
+	return 0;
+}
+
+static int gem_ptp_enable(struct ptp_clock_info *ptp,
+			  struct ptp_clock_request *rq, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+static struct ptp_clock_info gem_ptp_caps_template = {
+	.owner		= THIS_MODULE,
+	.name		= GEM_PTP_TIMER_NAME,
+	.max_adj	= 0,
+	.n_alarm	= 0,
+	.n_ext_ts	= 0,
+	.n_per_out	= 0,
+	.n_pins		= 0,
+	.pps		= 1,
+	.adjfine	= gem_ptp_adjfine,
+	.adjtime	= gem_ptp_adjtime,
+	.gettime64	= gem_tsu_get_time,
+	.settime64	= gem_tsu_set_time,
+	.enable		= gem_ptp_enable,
+};
+
+static void gem_ptp_init_timer(struct macb *bp)
+{
+	u32 rem = 0;
+	u64 adj;
+
+	bp->tsu_incr.ns = div_u64_rem(NSEC_PER_SEC, bp->tsu_rate, &rem);
+	if (rem) {
+		adj = rem;
+		adj <<= GEM_SUBNSINCR_SIZE;
+		bp->tsu_incr.sub_ns = div_u64(adj, bp->tsu_rate);
+	} else {
+		bp->tsu_incr.sub_ns = 0;
+	}
+}
+
+static void gem_ptp_init_tsu(struct macb *bp)
+{
+	struct timespec64 ts;
+
+	/* 1. get current system time */
+	ts = ns_to_timespec64(ktime_to_ns(ktime_get_real()));
+
+	/* 2. set ptp timer */
+	gem_tsu_set_time(&bp->ptp_clock_info, &ts);
+
+	/* 3. set PTP timer increment value to BASE_INCREMENT */
+	gem_tsu_incr_set(bp, &bp->tsu_incr);
+
+	gem_writel(bp, TA, 0);
+}
+
+static void gem_ptp_clear_timer(struct macb *bp)
+{
+	bp->tsu_incr.sub_ns = 0;
+	bp->tsu_incr.ns = 0;
+
+	gem_writel(bp, TISUBN, GEM_BF(SUBNSINCR, 0));
+	gem_writel(bp, TI, GEM_BF(NSINCR, 0));
+	gem_writel(bp, TA, 0);
+}
+
+static int gem_hw_timestamp(struct macb *bp, u32 dma_desc_ts_1,
+			    u32 dma_desc_ts_2, struct timespec64 *ts)
+{
+	struct timespec64 tsu;
+
+	ts->tv_sec = (GEM_BFEXT(DMA_SECH, dma_desc_ts_2) << GEM_DMA_SECL_SIZE) |
+			GEM_BFEXT(DMA_SECL, dma_desc_ts_1);
+	ts->tv_nsec = GEM_BFEXT(DMA_NSEC, dma_desc_ts_1);
+
+	/* TSU overlapping workaround
+	 * The timestamp only contains lower few bits of seconds,
+	 * so add value from 1588 timer
+	 */
+	gem_tsu_get_time(&bp->ptp_clock_info, &tsu);
+
+	/* If the top bit is set in the timestamp,
+	 * but not in 1588 timer, it has rolled over,
+	 * so subtract max size
+	 */
+	if ((ts->tv_sec & (GEM_DMA_SEC_TOP >> 1)) &&
+	    !(tsu.tv_sec & (GEM_DMA_SEC_TOP >> 1)))
+		ts->tv_sec -= GEM_DMA_SEC_TOP;
+
+	ts->tv_sec += ((~GEM_DMA_SEC_MASK) & tsu.tv_sec);
+
+	return 0;
+}
+
+void gem_ptp_rxstamp(struct macb *bp, struct sk_buff *skb,
+		     struct macb_dma_desc *desc)
+{
+	struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb);
+	struct macb_dma_desc_ptp *desc_ptp;
+	struct timespec64 ts;
+
+	if (GEM_BFEXT(DMA_RXVALID, desc->addr)) {
+		desc_ptp = macb_ptp_desc(bp, desc);
+		gem_hw_timestamp(bp, desc_ptp->ts_1, desc_ptp->ts_2, &ts);
+		memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
+		shhwtstamps->hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
+	}
+}
+
+static void gem_tstamp_tx(struct macb *bp, struct sk_buff *skb,
+			  struct macb_dma_desc_ptp *desc_ptp)
+{
+	struct skb_shared_hwtstamps shhwtstamps;
+	struct timespec64 ts;
+
+	gem_hw_timestamp(bp, desc_ptp->ts_1, desc_ptp->ts_2, &ts);
+	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
+	shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
+	skb_tstamp_tx(skb, &shhwtstamps);
+}
+
+int gem_ptp_txstamp(struct macb_queue *queue, struct sk_buff *skb,
+		    struct macb_dma_desc *desc)
+{
+	unsigned long tail = READ_ONCE(queue->tx_ts_tail);
+	unsigned long head = queue->tx_ts_head;
+	struct macb_dma_desc_ptp *desc_ptp;
+	struct gem_tx_ts *tx_timestamp;
+
+	if (!GEM_BFEXT(DMA_TXVALID, desc->ctrl))
+		return -EINVAL;
+
+	if (CIRC_SPACE(head, tail, PTP_TS_BUFFER_SIZE) == 0)
+		return -ENOMEM;
+
+	skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+	desc_ptp = macb_ptp_desc(queue->bp, desc);
+	tx_timestamp = &queue->tx_timestamps[head];
+	tx_timestamp->skb = skb;
+	tx_timestamp->desc_ptp.ts_1 = desc_ptp->ts_1;
+	tx_timestamp->desc_ptp.ts_2 = desc_ptp->ts_2;
+	/* move head */
+	smp_store_release(&queue->tx_ts_head,
+			  (head + 1) & (PTP_TS_BUFFER_SIZE - 1));
+
+	schedule_work(&queue->tx_ts_task);
+	return 0;
+}
+
+static void gem_tx_timestamp_flush(struct work_struct *work)
+{
+	struct macb_queue *queue =
+			container_of(work, struct macb_queue, tx_ts_task);
+	unsigned long head, tail;
+	struct gem_tx_ts *tx_ts;
+
+	/* take current head */
+	head = smp_load_acquire(&queue->tx_ts_head);
+	tail = queue->tx_ts_tail;
+
+	while (CIRC_CNT(head, tail, PTP_TS_BUFFER_SIZE)) {
+		tx_ts = &queue->tx_timestamps[tail];
+		gem_tstamp_tx(queue->bp, tx_ts->skb, &tx_ts->desc_ptp);
+		/* cleanup */
+		dev_kfree_skb_any(tx_ts->skb);
+		/* remove old tail */
+		smp_store_release(&queue->tx_ts_tail,
+				  (tail + 1) & (PTP_TS_BUFFER_SIZE - 1));
+		tail = queue->tx_ts_tail;
+	}
+}
+
+void gem_ptp_init(struct net_device *dev)
+{
+	struct macb *bp = netdev_priv(dev);
+	struct macb_queue *queue;
+	unsigned int q;
+
+	bp->ptp_clock_info = gem_ptp_caps_template;
+
+	/* nominal frequency and maximum adjustment in ppb */
+	bp->tsu_rate = bp->ptp_info->get_tsu_rate(bp);
+	bp->ptp_clock_info.max_adj = bp->ptp_info->get_ptp_max_adj();
+	gem_ptp_init_timer(bp);
+	bp->ptp_clock = ptp_clock_register(&bp->ptp_clock_info, &dev->dev);
+	if (IS_ERR(bp->ptp_clock)) {
+		pr_err("ptp clock register failed: %ld\n",
+			PTR_ERR(bp->ptp_clock));
+		bp->ptp_clock = NULL;
+		return;
+	} else if (bp->ptp_clock == NULL) {
+		pr_err("ptp clock register failed\n");
+		return;
+	}
+
+	spin_lock_init(&bp->tsu_clk_lock);
+	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
+		queue->tx_ts_head = 0;
+		queue->tx_ts_tail = 0;
+		INIT_WORK(&queue->tx_ts_task, gem_tx_timestamp_flush);
+	}
+
+	gem_ptp_init_tsu(bp);
+
+	dev_info(&bp->pdev->dev, "%s ptp clock registered.\n",
+		 GEM_PTP_TIMER_NAME);
+}
+
+void gem_ptp_remove(struct net_device *ndev)
+{
+	struct macb *bp = netdev_priv(ndev);
+
+	if (bp->ptp_clock)
+		ptp_clock_unregister(bp->ptp_clock);
+
+	gem_ptp_clear_timer(bp);
+
+	dev_info(&bp->pdev->dev, "%s ptp clock unregistered.\n",
+		 GEM_PTP_TIMER_NAME);
+}
+
+static int gem_ptp_set_ts_mode(struct macb *bp,
+			       enum macb_bd_control tx_bd_control,
+			       enum macb_bd_control rx_bd_control)
+{
+	gem_writel(bp, TXBDCTRL, GEM_BF(TXTSMODE, tx_bd_control));
+	gem_writel(bp, RXBDCTRL, GEM_BF(RXTSMODE, rx_bd_control));
+
+	return 0;
+}
+
+int gem_get_hwtst(struct net_device *dev, struct ifreq *rq)
+{
+	struct hwtstamp_config *tstamp_config;
+	struct macb *bp = netdev_priv(dev);
+
+	tstamp_config = &bp->tstamp_config;
+	if ((bp->hw_dma_cap & HW_DMA_CAP_PTP) == 0)
+		return -EOPNOTSUPP;
+
+	if (copy_to_user(rq->ifr_data, tstamp_config, sizeof(*tstamp_config)))
+		return -EFAULT;
+	else
+		return 0;
+}
+
+static int gem_ptp_set_one_step_sync(struct macb *bp, u8 enable)
+{
+	u32 reg_val;
+
+	reg_val = macb_readl(bp, NCR);
+
+	if (enable)
+		macb_writel(bp, NCR, reg_val | MACB_BIT(OSSMODE));
+	else
+		macb_writel(bp, NCR, reg_val & ~MACB_BIT(OSSMODE));
+
+	return 0;
+}
+
+int gem_set_hwtst(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	enum macb_bd_control tx_bd_control = TSTAMP_DISABLED;
+	enum macb_bd_control rx_bd_control = TSTAMP_DISABLED;
+	struct hwtstamp_config *tstamp_config;
+	struct macb *bp = netdev_priv(dev);
+	u32 regval;
+
+	tstamp_config = &bp->tstamp_config;
+	if ((bp->hw_dma_cap & HW_DMA_CAP_PTP) == 0)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(tstamp_config, ifr->ifr_data,
+			   sizeof(*tstamp_config)))
+		return -EFAULT;
+
+	/* reserved for future extensions */
+	if (tstamp_config->flags)
+		return -EINVAL;
+
+	switch (tstamp_config->tx_type) {
+	case HWTSTAMP_TX_OFF:
+		break;
+	case HWTSTAMP_TX_ONESTEP_SYNC:
+		if (gem_ptp_set_one_step_sync(bp, 1) != 0)
+			return -ERANGE;
+	case HWTSTAMP_TX_ON:
+		tx_bd_control = TSTAMP_ALL_FRAMES;
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	switch (tstamp_config->rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		break;
+	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+		break;
+	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+		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:
+	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+		rx_bd_control =  TSTAMP_ALL_PTP_FRAMES;
+		tstamp_config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+		regval = macb_readl(bp, NCR);
+		macb_writel(bp, NCR, (regval | MACB_BIT(SRTSM)));
+		break;
+	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+	case HWTSTAMP_FILTER_ALL:
+		rx_bd_control = TSTAMP_ALL_FRAMES;
+		tstamp_config->rx_filter = HWTSTAMP_FILTER_ALL;
+		break;
+	default:
+		tstamp_config->rx_filter = HWTSTAMP_FILTER_NONE;
+		return -ERANGE;
+	}
+
+	if (gem_ptp_set_ts_mode(bp, tx_bd_control, rx_bd_control) != 0)
+		return -ERANGE;
+
+	if (copy_to_user(ifr->ifr_data, tstamp_config, sizeof(*tstamp_config)))
+		return -EFAULT;
+	else
+		return 0;
+}
+
diff --git a/drivers/net/ethernet/cavium/liquidio/cn23xx_pf_device.c b/drivers/net/ethernet/cavium/liquidio/cn23xx_pf_device.c
index 962dcbcef8b5..6081c3132135 100644
--- a/drivers/net/ethernet/cavium/liquidio/cn23xx_pf_device.c
+++ b/drivers/net/ethernet/cavium/liquidio/cn23xx_pf_device.c
@@ -493,9 +493,8 @@ static void cn23xx_pf_setup_global_output_regs(struct octeon_device *oct)
 	for (q_no = srn; q_no < ern; q_no++) {
 		reg_val = octeon_read_csr(oct, CN23XX_SLI_OQ_PKT_CONTROL(q_no));
 
-		/* set IPTR & DPTR */
-		reg_val |=
-		    (CN23XX_PKT_OUTPUT_CTL_IPTR | CN23XX_PKT_OUTPUT_CTL_DPTR);
+		/* set DPTR */
+		reg_val |= CN23XX_PKT_OUTPUT_CTL_DPTR;
 
 		/* reset BMODE */
 		reg_val &= ~(CN23XX_PKT_OUTPUT_CTL_BMODE);
@@ -638,7 +637,7 @@ static void cn23xx_setup_oq_regs(struct octeon_device *oct, u32 oq_no)
 	octeon_write_csr(oct, CN23XX_SLI_OQ_SIZE(oq_no), droq->max_count);
 
 	octeon_write_csr(oct, CN23XX_SLI_OQ_BUFF_INFO_SIZE(oq_no),
-			 (droq->buffer_size | (OCT_RH_SIZE << 16)));
+			 droq->buffer_size);
 
 	/* Get the mapped address of the pkt_sent and pkts_credit regs */
 	droq->pkts_sent_reg =
@@ -1343,8 +1342,7 @@ int validate_cn23xx_pf_config_info(struct octeon_device *oct,
 		return 1;
 	}
 
-	if (!(CFG_GET_OQ_INFO_PTR(conf23xx)) ||
-	    !(CFG_GET_OQ_REFILL_THRESHOLD(conf23xx))) {
+	if (!CFG_GET_OQ_REFILL_THRESHOLD(conf23xx)) {
 		dev_err(&oct->pci_dev->dev, "%s: Invalid parameter for OQ\n",
 			__func__);
 		return 1;
diff --git a/drivers/net/ethernet/cavium/liquidio/cn23xx_vf_device.c b/drivers/net/ethernet/cavium/liquidio/cn23xx_vf_device.c
index b6117b6a1de2..9338a0008378 100644
--- a/drivers/net/ethernet/cavium/liquidio/cn23xx_vf_device.c
+++ b/drivers/net/ethernet/cavium/liquidio/cn23xx_vf_device.c
@@ -165,9 +165,8 @@ static void cn23xx_vf_setup_global_output_regs(struct octeon_device *oct)
 		reg_val =
 		    octeon_read_csr(oct, CN23XX_VF_SLI_OQ_PKT_CONTROL(q_no));
 
-		/* set IPTR & DPTR */
-		reg_val |=
-		    (CN23XX_PKT_OUTPUT_CTL_IPTR | CN23XX_PKT_OUTPUT_CTL_DPTR);
+		/* set DPTR */
+		reg_val |= CN23XX_PKT_OUTPUT_CTL_DPTR;
 
 		/* reset BMODE */
 		reg_val &= ~(CN23XX_PKT_OUTPUT_CTL_BMODE);
@@ -249,7 +248,7 @@ static void cn23xx_setup_vf_oq_regs(struct octeon_device *oct, u32 oq_no)
 	octeon_write_csr(oct, CN23XX_VF_SLI_OQ_SIZE(oq_no), droq->max_count);
 
 	octeon_write_csr(oct, CN23XX_VF_SLI_OQ_BUFF_INFO_SIZE(oq_no),
-			 (droq->buffer_size | (OCT_RH_SIZE << 16)));
+			 droq->buffer_size);
 
 	/* Get the mapped address of the pkt_sent and pkts_credit regs */
 	droq->pkts_sent_reg =
@@ -431,11 +430,6 @@ int cn23xx_octeon_pfvf_handshake(struct octeon_device *oct)
 	mbox_cmd.fn = (octeon_mbox_callback_t)octeon_pfvf_hs_callback;
 	mbox_cmd.fn_arg = &status;
 
-	/* Interrupts are not enabled at this point.
-	 * Enable them with default oq ticks
-	 */
-	oct->fn_list.enable_interrupt(oct, OCTEON_ALL_INTR);
-
 	octeon_mbox_write(oct, &mbox_cmd);
 
 	atomic_set(&status, 0);
@@ -444,11 +438,6 @@ int cn23xx_octeon_pfvf_handshake(struct octeon_device *oct)
 		schedule_timeout_uninterruptible(1);
 	} while ((!atomic_read(&status)) && (count++ < 100000));
 
-	/* Disable the interrupt so that the interrupsts will be reenabled
-	 * with the oq ticks received from the PF
-	 */
-	oct->fn_list.disable_interrupt(oct, OCTEON_ALL_INTR);
-
 	ret = atomic_read(&status);
 	if (!ret) {
 		dev_err(&oct->pci_dev->dev, "octeon_pfvf_handshake timeout\n");
diff --git a/drivers/net/ethernet/cavium/liquidio/cn66xx_device.c b/drivers/net/ethernet/cavium/liquidio/cn66xx_device.c
index bdec051107a6..b28253c96d97 100644
--- a/drivers/net/ethernet/cavium/liquidio/cn66xx_device.c
+++ b/drivers/net/ethernet/cavium/liquidio/cn66xx_device.c
@@ -209,9 +209,6 @@ void lio_cn6xxx_setup_global_output_regs(struct octeon_device *oct)
 		octeon_write_csr64(oct, CN6XXX_SLI_OQ_WMARK, 0);
 	}
 
-	/* / Select Info Ptr for length & data */
-	octeon_write_csr(oct, CN6XXX_SLI_PKT_IPTR, 0xFFFFFFFF);
-
 	/* / Select Packet count instead of bytes for SLI_PKTi_CNTS[CNT] */
 	octeon_write_csr(oct, CN6XXX_SLI_PKT_OUT_BMODE, 0);
 
@@ -314,7 +311,7 @@ void lio_cn6xxx_setup_oq_regs(struct octeon_device *oct, u32 oq_no)
 	octeon_write_csr(oct, CN6XXX_SLI_OQ_SIZE(oq_no), droq->max_count);
 
 	octeon_write_csr(oct, CN6XXX_SLI_OQ_BUFF_INFO_SIZE(oq_no),
-			 (droq->buffer_size | (OCT_RH_SIZE << 16)));
+			 droq->buffer_size);
 
 	/* Get the mapped address of the pkt_sent and pkts_credit regs */
 	droq->pkts_sent_reg =
@@ -734,8 +731,7 @@ int lio_validate_cn6xxx_config_info(struct octeon_device *oct,
 			__func__);
 		return 1;
 	}
-	if (!(CFG_GET_OQ_INFO_PTR(conf6xxx)) ||
-	    !(CFG_GET_OQ_REFILL_THRESHOLD(conf6xxx))) {
+	if (!CFG_GET_OQ_REFILL_THRESHOLD(conf6xxx)) {
 		dev_err(&oct->pci_dev->dev, "%s: Invalid parameter for OQ\n",
 			__func__);
 		return 1;
diff --git a/drivers/net/ethernet/cavium/liquidio/lio_core.c b/drivers/net/ethernet/cavium/liquidio/lio_core.c
index 796c2cbc11f6..adde7745d069 100644
--- a/drivers/net/ethernet/cavium/liquidio/lio_core.c
+++ b/drivers/net/ethernet/cavium/liquidio/lio_core.c
@@ -202,9 +202,13 @@ void liquidio_link_ctrl_cmd_completion(void *nctrl_ptr)
 			 netdev->name);
 		break;
 
-	case OCTNET_CMD_ENABLE_VLAN_FILTER:
-		dev_info(&oct->pci_dev->dev, "%s VLAN filter enabled\n",
-			 netdev->name);
+	case OCTNET_CMD_VLAN_FILTER_CTL:
+		if (nctrl->ncmd.s.param1)
+			dev_info(&oct->pci_dev->dev,
+				 "%s VLAN filter enabled\n", netdev->name);
+		else
+			dev_info(&oct->pci_dev->dev,
+				 "%s VLAN filter disabled\n", netdev->name);
 		break;
 
 	case OCTNET_CMD_ADD_VLAN_FILTER:
diff --git a/drivers/net/ethernet/cavium/liquidio/lio_ethtool.c b/drivers/net/ethernet/cavium/liquidio/lio_ethtool.c
index 579dc7336f58..28ecda3d3404 100644
--- a/drivers/net/ethernet/cavium/liquidio/lio_ethtool.c
+++ b/drivers/net/ethernet/cavium/liquidio/lio_ethtool.c
@@ -700,6 +700,13 @@ static void lio_set_msglevel(struct net_device *netdev, u32 msglvl)
 	lio->msg_enable = msglvl;
 }
 
+static void lio_vf_set_msglevel(struct net_device *netdev, u32 msglvl)
+{
+	struct lio *lio = GET_LIO(netdev);
+
+	lio->msg_enable = msglvl;
+}
+
 static void
 lio_get_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *pause)
 {
@@ -984,11 +991,11 @@ lio_get_ethtool_stats(struct net_device *netdev,
 		data[i++] =
 			CVM_CAST64(oct_dev->instr_queue[j]->stats.instr_posted);
 		/*# of instructions processed */
-		data[i++] = CVM_CAST64(oct_dev->instr_queue[j]->
-				       stats.instr_processed);
+		data[i++] = CVM_CAST64(
+				oct_dev->instr_queue[j]->stats.instr_processed);
 		/*# of instructions could not be processed */
-		data[i++] = CVM_CAST64(oct_dev->instr_queue[j]->
-				       stats.instr_dropped);
+		data[i++] = CVM_CAST64(
+				oct_dev->instr_queue[j]->stats.instr_dropped);
 		/*bytes sent through the queue */
 		data[i++] =
 			CVM_CAST64(oct_dev->instr_queue[j]->stats.bytes_sent);
@@ -1801,7 +1808,7 @@ oct_cfg_rx_intrcnt(struct lio *lio,
 			    (octeon_read_csr64(
 				 oct, CN23XX_VF_SLI_OQ_PKT_INT_LEVELS(q_no)) &
 			     (0x3fffff00000000UL)) |
-				rx_max_coalesced_frames);
+				(rx_max_coalesced_frames - 1));
 			/*consider writing to resend bit here*/
 		}
 		intrmod->rx_frames = rx_max_coalesced_frames;
@@ -2611,7 +2618,7 @@ static const struct ethtool_ops lio_vf_ethtool_ops = {
 	.get_regs_len		= lio_get_regs_len,
 	.get_regs		= lio_get_regs,
 	.get_msglevel		= lio_get_msglevel,
-	.set_msglevel		= lio_set_msglevel,
+	.set_msglevel		= lio_vf_set_msglevel,
 	.get_sset_count		= lio_vf_get_sset_count,
 	.get_coalesce		= lio_get_intr_coalesce,
 	.set_coalesce		= lio_set_intr_coalesce,
diff --git a/drivers/net/ethernet/cavium/liquidio/lio_main.c b/drivers/net/ethernet/cavium/liquidio/lio_main.c
index 927617cbf6a9..51583ae4b1eb 100644
--- a/drivers/net/ethernet/cavium/liquidio/lio_main.c
+++ b/drivers/net/ethernet/cavium/liquidio/lio_main.c
@@ -1421,7 +1421,7 @@ static bool fw_type_is_none(void)
  */
 static void octeon_destroy_resources(struct octeon_device *oct)
 {
-	int i;
+	int i, refcount;
 	struct msix_entry *msix_entries;
 	struct octeon_device_priv *oct_priv =
 		(struct octeon_device_priv *)oct->priv;
@@ -1556,10 +1556,14 @@ static void octeon_destroy_resources(struct octeon_device *oct)
 
 		/* fallthrough */
 	case OCT_DEV_PCI_MAP_DONE:
+		refcount = octeon_deregister_device(oct);
+
 		if (!fw_type_is_none()) {
-			/* Soft reset the octeon device before exiting */
-			if (!OCTEON_CN23XX_PF(oct) ||
-			    (OCTEON_CN23XX_PF(oct) && !oct->octeon_id))
+			/* Soft reset the octeon device before exiting.
+			 * Implementation note: here, we reset the device
+			 * if it is a CN6XXX OR the last CN23XX device.
+			 */
+			if (OCTEON_CN6XXX(oct) || !refcount)
 				oct->fn_list.soft_reset(oct);
 		}
 
@@ -3020,6 +3024,7 @@ static int hwtstamp_ioctl(struct net_device *netdev, struct ifreq *ifr)
 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+	case HWTSTAMP_FILTER_NTP_ALL:
 		conf.rx_filter = HWTSTAMP_FILTER_ALL;
 		break;
 	default:
@@ -3586,6 +3591,10 @@ static netdev_features_t liquidio_fix_features(struct net_device *netdev,
 	    (lio->dev_capability & NETIF_F_LRO))
 		request &= ~NETIF_F_LRO;
 
+	if ((request & NETIF_F_HW_VLAN_CTAG_FILTER) &&
+	    !(lio->dev_capability & NETIF_F_HW_VLAN_CTAG_FILTER))
+		request &= ~NETIF_F_HW_VLAN_CTAG_FILTER;
+
 	return request;
 }
 
@@ -3598,14 +3607,14 @@ static int liquidio_set_features(struct net_device *netdev,
 {
 	struct lio *lio = netdev_priv(netdev);
 
-	if (!((netdev->features ^ features) & NETIF_F_LRO))
-		return 0;
-
-	if ((features & NETIF_F_LRO) && (lio->dev_capability & NETIF_F_LRO))
+	if ((features & NETIF_F_LRO) &&
+	    (lio->dev_capability & NETIF_F_LRO) &&
+	    !(netdev->features & NETIF_F_LRO))
 		liquidio_set_feature(netdev, OCTNET_CMD_LRO_ENABLE,
 				     OCTNIC_LROIPV4 | OCTNIC_LROIPV6);
 	else if (!(features & NETIF_F_LRO) &&
-		 (lio->dev_capability & NETIF_F_LRO))
+		 (lio->dev_capability & NETIF_F_LRO) &&
+		 (netdev->features & NETIF_F_LRO))
 		liquidio_set_feature(netdev, OCTNET_CMD_LRO_DISABLE,
 				     OCTNIC_LROIPV4 | OCTNIC_LROIPV6);
 
@@ -3624,6 +3633,17 @@ static int liquidio_set_features(struct net_device *netdev,
 		liquidio_set_rxcsum_command(netdev, OCTNET_CMD_TNL_RX_CSUM_CTL,
 					    OCTNET_CMD_RXCSUM_DISABLE);
 
+	if ((features & NETIF_F_HW_VLAN_CTAG_FILTER) &&
+	    (lio->dev_capability & NETIF_F_HW_VLAN_CTAG_FILTER) &&
+	    !(netdev->features & NETIF_F_HW_VLAN_CTAG_FILTER))
+		liquidio_set_feature(netdev, OCTNET_CMD_VLAN_FILTER_CTL,
+				     OCTNET_CMD_VLAN_FILTER_ENABLE);
+	else if (!(features & NETIF_F_HW_VLAN_CTAG_FILTER) &&
+		 (lio->dev_capability & NETIF_F_HW_VLAN_CTAG_FILTER) &&
+		 (netdev->features & NETIF_F_HW_VLAN_CTAG_FILTER))
+		liquidio_set_feature(netdev, OCTNET_CMD_VLAN_FILTER_CTL,
+				     OCTNET_CMD_VLAN_FILTER_DISABLE);
+
 	return 0;
 }
 
@@ -3694,6 +3714,9 @@ static int liquidio_set_vf_mac(struct net_device *netdev, int vfidx, u8 *mac)
 	struct octeon_device *oct = lio->oct_dev;
 	int retval;
 
+	if (vfidx < 0 || vfidx >= oct->sriov_info.num_vfs_alloced)
+		return -EINVAL;
+
 	retval = __liquidio_set_vf_mac(netdev, vfidx, mac, true);
 	if (!retval)
 		cn23xx_tell_vf_its_macaddr_changed(oct, vfidx, mac);
@@ -3876,7 +3899,7 @@ static int lio_nic_info(struct octeon_recv_info *recv_info, void *buf)
 	union oct_link_status *ls;
 	int i;
 
-	if (recv_pkt->buffer_size[0] != sizeof(*ls)) {
+	if (recv_pkt->buffer_size[0] != (sizeof(*ls) + OCT_DROQ_INFO_SIZE)) {
 		dev_err(&oct->pci_dev->dev, "Malformed NIC_INFO, len=%d, ifidx=%d\n",
 			recv_pkt->buffer_size[0],
 			recv_pkt->rh.r_nic_info.gmxport);
@@ -3884,7 +3907,8 @@ static int lio_nic_info(struct octeon_recv_info *recv_info, void *buf)
 	}
 
 	gmxport = recv_pkt->rh.r_nic_info.gmxport;
-	ls = (union oct_link_status *)get_rbd(recv_pkt->buffer_ptr[0]);
+	ls = (union oct_link_status *)(get_rbd(recv_pkt->buffer_ptr[0]) +
+		OCT_DROQ_INFO_SIZE);
 
 	octeon_swap_8B_data((u64 *)ls, (sizeof(union oct_link_status)) >> 3);
 	for (i = 0; i < oct->ifcount; i++) {
@@ -4191,7 +4215,8 @@ static int setup_nic_devices(struct octeon_device *octeon_dev)
 			liquidio_set_feature(netdev, OCTNET_CMD_LRO_ENABLE,
 					     OCTNIC_LROIPV4 | OCTNIC_LROIPV6);
 
-		liquidio_set_feature(netdev, OCTNET_CMD_ENABLE_VLAN_FILTER, 0);
+		liquidio_set_feature(netdev, OCTNET_CMD_VLAN_FILTER_CTL,
+				     OCTNET_CMD_VLAN_FILTER_ENABLE);
 
 		if ((debug != -1) && (debug & NETIF_MSG_HW))
 			liquidio_set_feature(netdev,
@@ -4441,7 +4466,7 @@ octeon_recv_vf_drv_notice(struct octeon_recv_info *recv_info, void *buf)
 	u64 *data, vf_num;
 
 	notice = recv_pkt->rh.r.ossp;
-	data = (u64 *)get_rbd(recv_pkt->buffer_ptr[0]);
+	data = (u64 *)(get_rbd(recv_pkt->buffer_ptr[0]) + OCT_DROQ_INFO_SIZE);
 
 	/* the first 64-bit word of data is the vf_num */
 	vf_num = data[0];
@@ -4511,6 +4536,15 @@ static int octeon_device_init(struct octeon_device *octeon_dev)
 
 	atomic_set(&octeon_dev->status, OCT_DEV_PCI_MAP_DONE);
 
+	/* Only add a reference after setting status 'OCT_DEV_PCI_MAP_DONE',
+	 * since that is what is required for the reference to be removed
+	 * during de-initialization (see 'octeon_destroy_resources').
+	 */
+	octeon_register_device(octeon_dev, octeon_dev->pci_dev->bus->number,
+			       PCI_SLOT(octeon_dev->pci_dev->devfn),
+			       PCI_FUNC(octeon_dev->pci_dev->devfn),
+			       true);
+
 	octeon_dev->app_mode = CVM_DRV_INVALID_APP;
 
 	if (OCTEON_CN23XX_PF(octeon_dev)) {
diff --git a/drivers/net/ethernet/cavium/liquidio/lio_vf_main.c b/drivers/net/ethernet/cavium/liquidio/lio_vf_main.c
index 34c77821fad9..9b247102eb92 100644
--- a/drivers/net/ethernet/cavium/liquidio/lio_vf_main.c
+++ b/drivers/net/ethernet/cavium/liquidio/lio_vf_main.c
@@ -879,8 +879,6 @@ liquidio_vf_probe(struct pci_dev *pdev,
  */
 static void octeon_pci_flr(struct octeon_device *oct)
 {
-	u16 status;
-
 	pci_save_state(oct->pci_dev);
 
 	pci_cfg_access_lock(oct->pci_dev);
@@ -889,20 +887,7 @@ static void octeon_pci_flr(struct octeon_device *oct)
 	pci_write_config_word(oct->pci_dev, PCI_COMMAND,
 			      PCI_COMMAND_INTX_DISABLE);
 
-	/* Wait for Transaction Pending bit clean */
-	msleep(100);
-	pcie_capability_read_word(oct->pci_dev, PCI_EXP_DEVSTA, &status);
-	if (status & PCI_EXP_DEVSTA_TRPND) {
-		dev_info(&oct->pci_dev->dev, "Function reset incomplete after 100ms, sleeping for 5 seconds\n");
-		ssleep(5);
-		pcie_capability_read_word(oct->pci_dev, PCI_EXP_DEVSTA,
-					  &status);
-		if (status & PCI_EXP_DEVSTA_TRPND)
-			dev_info(&oct->pci_dev->dev, "Function reset still incomplete after 5s, reset anyway\n");
-	}
-	pcie_capability_set_word(oct->pci_dev, PCI_EXP_DEVCTL,
-				 PCI_EXP_DEVCTL_BCR_FLR);
-	mdelay(100);
+	pcie_flr(oct->pci_dev);
 
 	pci_cfg_access_unlock(oct->pci_dev);
 
@@ -2100,6 +2085,7 @@ static int hwtstamp_ioctl(struct net_device *netdev, struct ifreq *ifr)
 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+	case HWTSTAMP_FILTER_NTP_ALL:
 		conf.rx_filter = HWTSTAMP_FILTER_ALL;
 		break;
 	default:
@@ -2732,7 +2718,7 @@ static int lio_nic_info(struct octeon_recv_info *recv_info, void *buf)
 	int gmxport = 0;
 	int i;
 
-	if (recv_pkt->buffer_size[0] != sizeof(*ls)) {
+	if (recv_pkt->buffer_size[0] != (sizeof(*ls) + OCT_DROQ_INFO_SIZE)) {
 		dev_err(&oct->pci_dev->dev, "Malformed NIC_INFO, len=%d, ifidx=%d\n",
 			recv_pkt->buffer_size[0],
 			recv_pkt->rh.r_nic_info.gmxport);
@@ -2740,7 +2726,8 @@ static int lio_nic_info(struct octeon_recv_info *recv_info, void *buf)
 	}
 
 	gmxport = recv_pkt->rh.r_nic_info.gmxport;
-	ls = (union oct_link_status *)get_rbd(recv_pkt->buffer_ptr[0]);
+	ls = (union oct_link_status *)(get_rbd(recv_pkt->buffer_ptr[0]) +
+		OCT_DROQ_INFO_SIZE);
 
 	octeon_swap_8B_data((u64 *)ls, (sizeof(union oct_link_status)) >> 3);
 
@@ -3011,10 +2998,6 @@ static int setup_nic_devices(struct octeon_device *octeon_dev)
 			liquidio_set_feature(netdev, OCTNET_CMD_LRO_ENABLE,
 					     OCTNIC_LROIPV4 | OCTNIC_LROIPV6);
 
-		if ((debug != -1) && (debug & NETIF_MSG_HW))
-			liquidio_set_feature(netdev, OCTNET_CMD_VERBOSE_ENABLE,
-					     0);
-
 		if (setup_link_status_change_wq(netdev))
 			goto setup_nic_dev_fail;
 
@@ -3202,13 +3185,28 @@ static int octeon_device_init(struct octeon_device *oct)
 	if (octeon_setup_interrupt(oct))
 		return 1;
 
+	atomic_set(&oct->status, OCT_DEV_INTR_SET_DONE);
+
+	/* ***************************************************************
+	 * The interrupts need to be enabled for the PF<-->VF handshake.
+	 * They are [re]-enabled after the PF<-->VF handshake so that the
+	 * correct OQ tick value is used (i.e. the value retrieved from
+	 * the PF as part of the handshake).
+	 */
+
+	/* Enable Octeon device interrupts */
+	oct->fn_list.enable_interrupt(oct, OCTEON_ALL_INTR);
+
 	if (cn23xx_octeon_pfvf_handshake(oct))
 		return 1;
 
+	/* Here we [re]-enable the interrupts so that the correct OQ tick value
+	 * is used (i.e. the value that was retrieved during the handshake)
+	 */
+
 	/* Enable Octeon device interrupts */
 	oct->fn_list.enable_interrupt(oct, OCTEON_ALL_INTR);
-
-	atomic_set(&oct->status, OCT_DEV_INTR_SET_DONE);
+	/* *************************************************************** */
 
 	/* Enable the input and output queues for this Octeon device */
 	if (oct->fn_list.enable_io_queues(oct)) {
diff --git a/drivers/net/ethernet/cavium/liquidio/liquidio_common.h b/drivers/net/ethernet/cavium/liquidio/liquidio_common.h
index 8ea2323d8d67..231dd7fbfb80 100644
--- a/drivers/net/ethernet/cavium/liquidio/liquidio_common.h
+++ b/drivers/net/ethernet/cavium/liquidio/liquidio_common.h
@@ -173,6 +173,8 @@ static inline void add_sg_size(struct octeon_sg_entry *sg_entry,
 
 /*------------------------- End Scatter/Gather ---------------------------*/
 
+#define   OCTNET_FRM_LENGTH_SIZE      8
+
 #define   OCTNET_FRM_PTP_HEADER_SIZE  8
 
 #define   OCTNET_FRM_HEADER_SIZE     22 /* VLAN + Ethernet */
@@ -215,7 +217,7 @@ static inline void add_sg_size(struct octeon_sg_entry *sg_entry,
 #define   OCTNET_CMD_VERBOSE_ENABLE   0x14
 #define   OCTNET_CMD_VERBOSE_DISABLE  0x15
 
-#define   OCTNET_CMD_ENABLE_VLAN_FILTER 0x16
+#define   OCTNET_CMD_VLAN_FILTER_CTL 0x16
 #define   OCTNET_CMD_ADD_VLAN_FILTER  0x17
 #define   OCTNET_CMD_DEL_VLAN_FILTER  0x18
 #define   OCTNET_CMD_VXLAN_PORT_CONFIG 0x19
@@ -230,6 +232,8 @@ static inline void add_sg_size(struct octeon_sg_entry *sg_entry,
 #define   OCTNET_CMD_RXCSUM_DISABLE    0x1
 #define   OCTNET_CMD_TXCSUM_ENABLE     0x0
 #define   OCTNET_CMD_TXCSUM_DISABLE    0x1
+#define   OCTNET_CMD_VLAN_FILTER_ENABLE 0x1
+#define   OCTNET_CMD_VLAN_FILTER_DISABLE 0x0
 
 /* RX(packets coming from wire) Checksum verification flags */
 /* TCP/UDP csum */
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_config.h b/drivers/net/ethernet/cavium/liquidio/octeon_config.h
index d29ebc531151..f229d792c2b3 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_config.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_config.h
@@ -47,7 +47,7 @@
 /* CN6xxx OQ configuration macros */
 #define   CN6XXX_MAX_OUTPUT_QUEUES     32
 #define   CN6XXX_MAX_OQ_DESCRIPTORS    2048
-#define   CN6XXX_OQ_BUF_SIZE           1536
+#define   CN6XXX_OQ_BUF_SIZE           1664
 #define   CN6XXX_OQ_PKTSPER_INTR       ((CN6XXX_MAX_OQ_DESCRIPTORS < 512) ? \
 					(CN6XXX_MAX_OQ_DESCRIPTORS / 4) : 128)
 #define   CN6XXX_OQ_REFIL_THRESHOLD    ((CN6XXX_MAX_OQ_DESCRIPTORS < 512) ? \
@@ -78,7 +78,7 @@
 
 #define   CN23XX_MAX_OUTPUT_QUEUES	CN23XX_MAX_RINGS_PER_PF
 #define   CN23XX_MAX_OQ_DESCRIPTORS	512
-#define   CN23XX_OQ_BUF_SIZE		1536
+#define   CN23XX_OQ_BUF_SIZE		1664
 #define   CN23XX_OQ_PKTSPER_INTR	128
 /*#define CAVIUM_ONLY_CN23XX_RX_PERF*/
 #define   CN23XX_OQ_REFIL_THRESHOLD	16
@@ -98,8 +98,6 @@
 #define   OCTEON_32BYTE_INSTR          32
 #define   OCTEON_64BYTE_INSTR          64
 #define   OCTEON_MAX_BASE_IOQ          4
-#define   OCTEON_OQ_BUFPTR_MODE        0
-#define   OCTEON_OQ_INFOPTR_MODE       1
 
 #define   OCTEON_DMA_INTR_PKT          64
 #define   OCTEON_DMA_INTR_TIME         1000
@@ -125,7 +123,6 @@
 #define CFG_SET_IQ_INTR_PKT(cfg, val)            (cfg)->iq.iq_intr_pkt = val
 
 #define CFG_GET_OQ_MAX_Q(cfg)                    ((cfg)->oq.max_oqs)
-#define CFG_GET_OQ_INFO_PTR(cfg)                 ((cfg)->oq.info_ptr)
 #define CFG_GET_OQ_PKTS_PER_INTR(cfg)            ((cfg)->oq.pkts_per_intr)
 #define CFG_GET_OQ_REFILL_THRESHOLD(cfg)         ((cfg)->oq.refill_threshold)
 #define CFG_GET_OQ_INTR_PKT(cfg)                 ((cfg)->oq.oq_intr_pkt)
@@ -266,9 +263,6 @@ struct octeon_oq_config {
 	 */
 	u64 refill_threshold:16;
 
-	/** If set, the Output queue uses info-pointer mode. (Default: 1) */
-	u64 info_ptr:32;
-
 	/* Max number of OQs available */
 	u64 max_oqs:8;
 
@@ -276,9 +270,6 @@ struct octeon_oq_config {
 	/* Max number of OQs available */
 	u64 max_oqs:8;
 
-	/** If set, the Output queue uses info-pointer mode. (Default: 1) */
-	u64 info_ptr:32;
-
 	/** The number of buffers that were consumed during packet processing by
 	 *   the driver on this Output queue before the driver attempts to
 	 *   replenish
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_console.c b/drivers/net/ethernet/cavium/liquidio/octeon_console.c
index 53f38d05f7c2..e08f7600f986 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_console.c
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_console.c
@@ -724,13 +724,11 @@ static int octeon_console_read(struct octeon_device *oct, u32 console_num,
 }
 
 #define FBUF_SIZE	(4 * 1024 * 1024)
-u8 fbuf[FBUF_SIZE];
 
 int octeon_download_firmware(struct octeon_device *oct, const u8 *data,
 			     size_t size)
 {
 	int ret = 0;
-	u8 *p = fbuf;
 	u32 crc32_result;
 	u64 load_addr;
 	u32 image_len;
@@ -805,10 +803,8 @@ int octeon_download_firmware(struct octeon_device *oct, const u8 *data,
 			else
 				size = FBUF_SIZE;
 
-			memcpy(p, data, size);
-
 			/* download the image */
-			octeon_pci_write_core_mem(oct, load_addr, p, (u32)size);
+			octeon_pci_write_core_mem(oct, load_addr, data, (u32)size);
 
 			data += size;
 			rem -= (u32)size;
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_device.c b/drivers/net/ethernet/cavium/liquidio/octeon_device.c
index e21b477d0159..623e28ca736e 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_device.c
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_device.c
@@ -51,7 +51,6 @@ static struct octeon_config default_cn66xx_conf = {
 	/** OQ attributes */
 	.oq					= {
 		.max_oqs			= CN6XXX_CFG_IO_QUEUES,
-		.info_ptr			= OCTEON_OQ_INFOPTR_MODE,
 		.refill_threshold		= CN6XXX_OQ_REFIL_THRESHOLD,
 		.oq_intr_pkt			= CN6XXX_OQ_INTR_PKT,
 		.oq_intr_time			= CN6XXX_OQ_INTR_TIME,
@@ -161,7 +160,6 @@ static struct octeon_config default_cn68xx_conf = {
 	/** OQ attributes */
 	.oq					= {
 		.max_oqs			= CN6XXX_CFG_IO_QUEUES,
-		.info_ptr			= OCTEON_OQ_INFOPTR_MODE,
 		.refill_threshold		= CN6XXX_OQ_REFIL_THRESHOLD,
 		.oq_intr_pkt			= CN6XXX_OQ_INTR_PKT,
 		.oq_intr_time			= CN6XXX_OQ_INTR_TIME,
@@ -328,7 +326,6 @@ static struct octeon_config default_cn68xx_210nv_conf = {
 	/** OQ attributes */
 	.oq					= {
 		.max_oqs			= CN6XXX_CFG_IO_QUEUES,
-		.info_ptr			= OCTEON_OQ_INFOPTR_MODE,
 		.refill_threshold		= CN6XXX_OQ_REFIL_THRESHOLD,
 		.oq_intr_pkt			= CN6XXX_OQ_INTR_PKT,
 		.oq_intr_time			= CN6XXX_OQ_INTR_TIME,
@@ -432,7 +429,6 @@ static struct octeon_config default_cn23xx_conf = {
 	/** OQ attributes */
 	.oq = {
 		.max_oqs		= CN23XX_CFG_IO_QUEUES,
-		.info_ptr		= OCTEON_OQ_INFOPTR_MODE,
 		.pkts_per_intr	= CN23XX_OQ_PKTSPER_INTR,
 		.refill_threshold	= CN23XX_OQ_REFIL_THRESHOLD,
 		.oq_intr_pkt	= CN23XX_OQ_INTR_PKT,
@@ -543,7 +539,11 @@ static char oct_dev_app_str[CVM_DRV_APP_COUNT + 1][32] = {
 	"BASE", "NIC", "UNKNOWN"};
 
 static struct octeon_device *octeon_device[MAX_OCTEON_DEVICES];
+static atomic_t adapter_refcounts[MAX_OCTEON_DEVICES];
+
 static u32 octeon_device_count;
+/* locks device array (i.e. octeon_device[]) */
+static spinlock_t octeon_devices_lock;
 
 static struct octeon_core_setup core_setup[MAX_OCTEON_DEVICES];
 
@@ -561,6 +561,7 @@ void octeon_init_device_list(int conf_type)
 	memset(octeon_device, 0, (sizeof(void *) * MAX_OCTEON_DEVICES));
 	for (i = 0; i <  MAX_OCTEON_DEVICES; i++)
 		oct_set_config_info(i, conf_type);
+	spin_lock_init(&octeon_devices_lock);
 }
 
 static void *__retrieve_octeon_config_info(struct octeon_device *oct,
@@ -720,23 +721,27 @@ struct octeon_device *octeon_allocate_device(u32 pci_id,
 	u32 oct_idx = 0;
 	struct octeon_device *oct = NULL;
 
+	spin_lock(&octeon_devices_lock);
+
 	for (oct_idx = 0; oct_idx < MAX_OCTEON_DEVICES; oct_idx++)
 		if (!octeon_device[oct_idx])
 			break;
 
-	if (oct_idx == MAX_OCTEON_DEVICES)
-		return NULL;
+	if (oct_idx < MAX_OCTEON_DEVICES) {
+		oct = octeon_allocate_device_mem(pci_id, priv_size);
+		if (oct) {
+			octeon_device_count++;
+			octeon_device[oct_idx] = oct;
+		}
+	}
 
-	oct = octeon_allocate_device_mem(pci_id, priv_size);
+	spin_unlock(&octeon_devices_lock);
 	if (!oct)
 		return NULL;
 
 	spin_lock_init(&oct->pci_win_lock);
 	spin_lock_init(&oct->mem_access_lock);
 
-	octeon_device_count++;
-	octeon_device[oct_idx] = oct;
-
 	oct->octeon_id = oct_idx;
 	snprintf(oct->device_name, sizeof(oct->device_name),
 		 "LiquidIO%d", (oct->octeon_id));
@@ -744,6 +749,72 @@ struct octeon_device *octeon_allocate_device(u32 pci_id,
 	return oct;
 }
 
+/** Register a device's bus location at initialization time.
+ *  @param octeon_dev - pointer to the octeon device structure.
+ *  @param bus        - PCIe bus #
+ *  @param dev        - PCIe device #
+ *  @param func       - PCIe function #
+ *  @param is_pf      - TRUE for PF, FALSE for VF
+ *  @return reference count of device's adapter
+ */
+int octeon_register_device(struct octeon_device *oct,
+			   int bus, int dev, int func, int is_pf)
+{
+	int idx, refcount;
+
+	oct->loc.bus = bus;
+	oct->loc.dev = dev;
+	oct->loc.func = func;
+
+	oct->adapter_refcount = &adapter_refcounts[oct->octeon_id];
+	atomic_set(oct->adapter_refcount, 0);
+
+	spin_lock(&octeon_devices_lock);
+	for (idx = (int)oct->octeon_id - 1; idx >= 0; idx--) {
+		if (!octeon_device[idx]) {
+			dev_err(&oct->pci_dev->dev,
+				"%s: Internal driver error, missing dev",
+				__func__);
+			spin_unlock(&octeon_devices_lock);
+			atomic_inc(oct->adapter_refcount);
+			return 1; /* here, refcount is guaranteed to be 1 */
+		}
+		/* if another device is at same bus/dev, use its refcounter */
+		if ((octeon_device[idx]->loc.bus == bus) &&
+		    (octeon_device[idx]->loc.dev == dev)) {
+			oct->adapter_refcount =
+				octeon_device[idx]->adapter_refcount;
+			break;
+		}
+	}
+	spin_unlock(&octeon_devices_lock);
+
+	atomic_inc(oct->adapter_refcount);
+	refcount = atomic_read(oct->adapter_refcount);
+
+	dev_dbg(&oct->pci_dev->dev, "%s: %02x:%02x:%d refcount %u", __func__,
+		oct->loc.bus, oct->loc.dev, oct->loc.func, refcount);
+
+	return refcount;
+}
+
+/** Deregister a device at de-initialization time.
+ *  @param octeon_dev - pointer to the octeon device structure.
+ *  @return reference count of device's adapter
+ */
+int octeon_deregister_device(struct octeon_device *oct)
+{
+	int refcount;
+
+	atomic_dec(oct->adapter_refcount);
+	refcount = atomic_read(oct->adapter_refcount);
+
+	dev_dbg(&oct->pci_dev->dev, "%s: %04d:%02d:%d refcount %u", __func__,
+		oct->loc.bus, oct->loc.dev, oct->loc.func, refcount);
+
+	return refcount;
+}
+
 int
 octeon_allocate_ioq_vector(struct octeon_device  *oct)
 {
@@ -1161,13 +1232,15 @@ int octeon_core_drv_init(struct octeon_recv_info *recv_info, void *buf)
 
 	cs = &core_setup[oct->octeon_id];
 
-	if (recv_pkt->buffer_size[0] != sizeof(*cs)) {
+	if (recv_pkt->buffer_size[0] != (sizeof(*cs) + OCT_DROQ_INFO_SIZE)) {
 		dev_dbg(&oct->pci_dev->dev, "Core setup bytes expected %u found %d\n",
 			(u32)sizeof(*cs),
 			recv_pkt->buffer_size[0]);
 	}
 
-	memcpy(cs, get_rbd(recv_pkt->buffer_ptr[0]), sizeof(*cs));
+	memcpy(cs, get_rbd(
+	       recv_pkt->buffer_ptr[0]) + OCT_DROQ_INFO_SIZE, sizeof(*cs));
+
 	strncpy(oct->boardinfo.name, cs->boardname, OCT_BOARD_NAME);
 	strncpy(oct->boardinfo.serial_number, cs->board_serial_number,
 		OCT_SERIAL_LEN);
@@ -1354,13 +1427,15 @@ int lio_get_device_id(void *dev)
 void lio_enable_irq(struct octeon_droq *droq, struct octeon_instr_queue *iq)
 {
 	u64 instr_cnt;
+	u32 pkts_pend;
 	struct octeon_device *oct = NULL;
 
 	/* the whole thing needs to be atomic, ideally */
 	if (droq) {
+		pkts_pend = (u32)atomic_read(&droq->pkts_pending);
 		spin_lock_bh(&droq->lock);
-		writel(droq->pkt_count, droq->pkts_sent_reg);
-		droq->pkt_count = 0;
+		writel(droq->pkt_count - pkts_pend, droq->pkts_sent_reg);
+		droq->pkt_count = pkts_pend;
 		/* this write needs to be flushed before we release the lock */
 		mmiowb();
 		spin_unlock_bh(&droq->lock);
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_device.h b/drivers/net/ethernet/cavium/liquidio/octeon_device.h
index 92f67de111aa..c90ed48ae8ab 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_device.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_device.h
@@ -544,6 +544,14 @@ struct octeon_device {
 	u32 tx_max_coalesced_frames;
 
 	bool cores_crashed;
+
+	struct {
+		int bus;
+		int dev;
+		int func;
+	} loc;
+
+	atomic_t *adapter_refcount; /* reference count of adapter */
 };
 
 #define  OCT_DRV_ONLINE 1
@@ -572,6 +580,23 @@ void octeon_free_device_mem(struct octeon_device *oct);
 struct octeon_device *octeon_allocate_device(u32 pci_id,
 					     u32 priv_size);
 
+/** Register a device's bus location at initialization time.
+ *  @param octeon_dev - pointer to the octeon device structure.
+ *  @param bus        - PCIe bus #
+ *  @param dev        - PCIe device #
+ *  @param func       - PCIe function #
+ *  @param is_pf      - TRUE for PF, FALSE for VF
+ *  @return reference count of device's adapter
+ */
+int octeon_register_device(struct octeon_device *oct,
+			   int bus, int dev, int func, int is_pf);
+
+/** Deregister a device at de-initialization time.
+ *  @param octeon_dev - pointer to the octeon device structure.
+ *  @return reference count of device's adapter
+ */
+int octeon_deregister_device(struct octeon_device *oct);
+
 /**  Initialize the driver's dispatch list which is a mix of a hash table
  *  and a linked list. This is done at driver load time.
  *  @param octeon_dev - pointer to the octeon device structure.
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_droq.c b/drivers/net/ethernet/cavium/liquidio/octeon_droq.c
index 286be5539cef..2e190deb2233 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_droq.c
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_droq.c
@@ -181,10 +181,7 @@ octeon_droq_setup_ring_buffers(struct octeon_device *oct,
 
 		droq->recv_buf_list[i].buffer = buf;
 		droq->recv_buf_list[i].data = get_rbd(buf);
-		droq->info_list[i].length = 0;
-
-		/* map ring buffers into memory */
-		desc_ring[i].info_ptr = lio_map_ring_info(droq, i);
+		desc_ring[i].info_ptr = 0;
 		desc_ring[i].buffer_ptr =
 			lio_map_ring(droq->recv_buf_list[i].buffer);
 	}
@@ -205,9 +202,6 @@ int octeon_delete_droq(struct octeon_device *oct, u32 q_no)
 	octeon_droq_destroy_ring_buffers(oct, droq);
 	vfree(droq->recv_buf_list);
 
-	if (droq->info_base_addr)
-		lio_free_info_buffer(oct, droq);
-
 	if (droq->desc_ring)
 		lio_dma_free(oct, (droq->max_count * OCT_DROQ_DESC_SIZE),
 			     droq->desc_ring, droq->desc_ring_dma);
@@ -280,14 +274,6 @@ int octeon_init_droq(struct octeon_device *oct,
 	dev_dbg(&oct->pci_dev->dev, "droq[%d]: num_desc: %d\n", q_no,
 		droq->max_count);
 
-	droq->info_list = lio_alloc_info_buffer(oct, droq);
-	if (!droq->info_list) {
-		dev_err(&oct->pci_dev->dev, "Cannot allocate memory for info list.\n");
-		lio_dma_free(oct, (droq->max_count * OCT_DROQ_DESC_SIZE),
-			     droq->desc_ring, droq->desc_ring_dma);
-		return 1;
-	}
-
 	droq->recv_buf_list = (struct octeon_recv_buffer *)
 			      vmalloc_node(droq->max_count *
 						OCT_DROQ_RECVBUF_SIZE,
@@ -357,7 +343,7 @@ static inline struct octeon_recv_info *octeon_create_recv_info(
 	u32 i, bytes_left;
 	struct octeon_skb_page_info *pg_info;
 
-	info = &droq->info_list[idx];
+	info = (struct octeon_droq_info *)droq->recv_buf_list[idx].data;
 
 	recv_info = octeon_alloc_recv_info(sizeof(struct __dispatch));
 	if (!recv_info)
@@ -425,8 +411,7 @@ octeon_droq_refill_pullup_descs(struct octeon_droq *droq,
 							      droq->max_count);
 				desc_refilled++;
 				droq->refill_count--;
-			} while (droq->recv_buf_list[droq->refill_idx].
-				 buffer);
+			} while (droq->recv_buf_list[droq->refill_idx].buffer);
 		}
 		refill_index = incr_index(refill_index, 1, droq->max_count);
 	}                       /* while */
@@ -490,10 +475,8 @@ octeon_droq_refill(struct octeon_device *octeon_dev, struct octeon_droq *droq)
 		droq->recv_buf_list[droq->refill_idx].data = data;
 
 		desc_ring[droq->refill_idx].buffer_ptr =
-			lio_map_ring(droq->recv_buf_list[droq->
-				     refill_idx].buffer);
-		/* Reset any previous values in the length field. */
-		droq->info_list[droq->refill_idx].length = 0;
+			lio_map_ring(droq->recv_buf_list[
+				     droq->refill_idx].buffer);
 
 		droq->refill_idx = incr_index(droq->refill_idx, 1,
 					      droq->max_count);
@@ -542,11 +525,7 @@ void octeon_droq_check_oom(struct octeon_droq *droq)
 static inline u32
 octeon_droq_get_bufcount(u32 buf_size, u32 total_len)
 {
-	u32 buf_cnt = 0;
-
-	while (total_len > (buf_size * buf_cnt))
-		buf_cnt++;
-	return buf_cnt;
+	return ((total_len + buf_size - 1) / buf_size);
 }
 
 static int
@@ -594,11 +573,12 @@ static inline void octeon_droq_drop_packets(struct octeon_device *oct,
 	struct octeon_droq_info *info;
 
 	for (i = 0; i < cnt; i++) {
-		info = &droq->info_list[droq->read_idx];
+		info = (struct octeon_droq_info *)
+			droq->recv_buf_list[droq->read_idx].data;
 		octeon_swap_8B_data((u64 *)info, 2);
 
 		if (info->length) {
-			info->length -= OCT_RH_SIZE;
+			info->length += OCTNET_FRM_LENGTH_SIZE;
 			droq->stats.bytes_received += info->length;
 			buf_cnt = octeon_droq_get_bufcount(droq->buffer_size,
 							   (u32)info->length);
@@ -630,7 +610,8 @@ octeon_droq_fast_process_packets(struct octeon_device *oct,
 		struct octeon_skb_page_info *pg_info;
 		void *buf;
 
-		info = &droq->info_list[droq->read_idx];
+		info = (struct octeon_droq_info *)
+			droq->recv_buf_list[droq->read_idx].data;
 		octeon_swap_8B_data((u64 *)info, 2);
 
 		if (!info->length) {
@@ -644,9 +625,10 @@ octeon_droq_fast_process_packets(struct octeon_device *oct,
 		}
 
 		/* Len of resp hdr in included in the received data len. */
-		info->length -= OCT_RH_SIZE;
 		rh = &info->rh;
 
+		info->length += OCTNET_FRM_LENGTH_SIZE;
+		rh->r_dh.len += (ROUNDUP8(OCT_DROQ_INFO_SIZE) / sizeof(u64));
 		total_len += (u32)info->length;
 		if (opcode_slow_path(rh)) {
 			u32 buf_cnt;
@@ -690,8 +672,8 @@ octeon_droq_fast_process_packets(struct octeon_device *oct,
 									nicbuf,
 									cpy_len,
 									idx);
-						buf = droq->recv_buf_list[idx].
-							buffer;
+						buf = droq->recv_buf_list[
+							idx].buffer;
 						recv_buffer_fast_free(buf);
 						droq->recv_buf_list[idx].buffer
 							= NULL;
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_droq.h b/drivers/net/ethernet/cavium/liquidio/octeon_droq.h
index 9781577115e7..6efd139b894d 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_droq.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_droq.h
@@ -51,11 +51,11 @@ struct octeon_droq_desc {
  *  about the packet.
  */
 struct octeon_droq_info {
-	/** The Output Receive Header. */
-	union octeon_rh rh;
-
 	/** The Length of the packet. */
 	u64 length;
+
+	/** The Output Receive Header. */
+	union octeon_rh rh;
 };
 
 #define OCT_DROQ_INFO_SIZE   (sizeof(struct octeon_droq_info))
@@ -294,9 +294,6 @@ struct octeon_droq {
 	 */
 	u32 max_empty_descs;
 
-	/** The 8B aligned info ptrs begin from this address. */
-	struct octeon_droq_info *info_list;
-
 	/** The receive buffer list. This list has the virtual addresses of the
 	 * buffers.
 	 */
@@ -324,15 +321,6 @@ struct octeon_droq {
 	/** DMA mapped address of the DROQ descriptor ring. */
 	size_t desc_ring_dma;
 
-	/** Info ptr list are allocated at this virtual address. */
-	void *info_base_addr;
-
-	/** DMA mapped address of the info list */
-	dma_addr_t info_list_dma;
-
-	/** Allocated size of info list. */
-	u32 info_alloc_size;
-
 	/** application context */
 	void *app_ctx;
 
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_iq.h b/drivers/net/ethernet/cavium/liquidio/octeon_iq.h
index 5063a12613e5..5c3c8da976f7 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_iq.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_iq.h
@@ -251,7 +251,7 @@ union octeon_instr_64B {
 
 /** The size of each buffer in soft command buffer pool
  */
-#define  SOFT_COMMAND_BUFFER_SIZE	1536
+#define  SOFT_COMMAND_BUFFER_SIZE	2048
 
 struct octeon_soft_command {
 	/** Soft command buffer info. */
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_mailbox.c b/drivers/net/ethernet/cavium/liquidio/octeon_mailbox.c
index 5cca73b8880b..57af7df74ced 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_mailbox.c
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_mailbox.c
@@ -178,7 +178,10 @@ int octeon_mbox_write(struct octeon_device *oct,
 					break;
 				}
 			}
-			writeq(mbox_cmd->data[i], mbox->mbox_write_reg);
+			if (ret == OCTEON_MBOX_STATUS_SUCCESS)
+				writeq(mbox_cmd->data[i], mbox->mbox_write_reg);
+			else
+				break;
 		}
 	}
 
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_mailbox.h b/drivers/net/ethernet/cavium/liquidio/octeon_mailbox.h
index c9376fe075bc..1def22afeff1 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_mailbox.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_mailbox.h
@@ -20,16 +20,16 @@
 
 /* Macros for Mail Box Communication */
 
-#define OCTEON_MBOX_DATA_MAX	32
+#define OCTEON_MBOX_DATA_MAX		32
 
 #define OCTEON_VF_ACTIVE		0x1
 #define OCTEON_VF_FLR_REQUEST		0x2
 #define OCTEON_PF_CHANGED_VF_MACADDR	0x4
 
 /*Macro for Read acknowldgement*/
-#define OCTEON_PFVFACK			0xffffffffffffffff
-#define OCTEON_PFVFSIG			0x1122334455667788
-#define OCTEON_PFVFERR			0xDEADDEADDEADDEAD
+#define OCTEON_PFVFACK			0xffffffffffffffffULL
+#define OCTEON_PFVFSIG			0x1122334455667788ULL
+#define OCTEON_PFVFERR			0xDEADDEADDEADDEADULL
 
 #define LIO_MBOX_WRITE_WAIT_CNT         1000
 #define LIO_MBOX_WRITE_WAIT_TIME        msecs_to_jiffies(1)
@@ -74,8 +74,8 @@ enum octeon_mbox_state {
 	OCTEON_MBOX_STATE_REQUEST_RECEIVED = 4,
 	OCTEON_MBOX_STATE_RESPONSE_PENDING = 8,
 	OCTEON_MBOX_STATE_RESPONSE_RECEIVING = 16,
-	OCTEON_MBOX_STATE_RESPONSE_RECEIVED = 16,
-	OCTEON_MBOX_STATE_ERROR = 32
+	OCTEON_MBOX_STATE_RESPONSE_RECEIVED = 32,
+	OCTEON_MBOX_STATE_ERROR = 64
 };
 
 struct octeon_mbox {
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_main.h b/drivers/net/ethernet/cavium/liquidio/octeon_main.h
index bed9ef17bc26..7ccffbb0019e 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_main.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_main.h
@@ -144,7 +144,7 @@ static inline int
 sleep_cond(wait_queue_head_t *wait_queue, int *condition)
 {
 	int errno = 0;
-	wait_queue_t we;
+	wait_queue_entry_t we;
 
 	init_waitqueue_entry(&we, current);
 	add_wait_queue(wait_queue, &we);
@@ -171,7 +171,7 @@ sleep_timeout_cond(wait_queue_head_t *wait_queue,
 		   int *condition,
 		   int timeout)
 {
-	wait_queue_t we;
+	wait_queue_entry_t we;
 
 	init_waitqueue_entry(&we, current);
 	add_wait_queue(wait_queue, &we);
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.c b/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.c
index 5cd96e7d426c..4c85ae643b7b 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.c
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.c
@@ -167,10 +167,10 @@ octeon_pci_read_core_mem(struct octeon_device *oct,
 void
 octeon_pci_write_core_mem(struct octeon_device *oct,
 			  u64 coreaddr,
-			  u8 *buf,
+			  const u8 *buf,
 			  u32 len)
 {
-	__octeon_pci_rw_core_mem(oct, coreaddr, buf, len, 0);
+	__octeon_pci_rw_core_mem(oct, coreaddr, (u8 *)buf, len, 0);
 }
 
 u64 octeon_read_device_mem64(struct octeon_device *oct, u64 coreaddr)
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.h b/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.h
index bae2fdd89503..47a3ff5f9b1e 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.h
@@ -66,7 +66,7 @@ octeon_pci_read_core_mem(struct octeon_device *oct,
 void
 octeon_pci_write_core_mem(struct octeon_device *oct,
 			  u64 coreaddr,
-			  u8 *buf,
+			  const u8 *buf,
 			  u32 len);
 
 #endif
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_network.h b/drivers/net/ethernet/cavium/liquidio/octeon_network.h
index bf483932ff25..ec8504b2942d 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_network.h
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_network.h
@@ -356,29 +356,6 @@ static inline void tx_buffer_free(void *buffer)
 #define lio_dma_free(oct, size, virt_addr, dma_addr) \
 	dma_free_coherent(&(oct)->pci_dev->dev, size, virt_addr, dma_addr)
 
-static inline void *
-lio_alloc_info_buffer(struct octeon_device *oct,
-		      struct octeon_droq *droq)
-{
-	void *virt_ptr;
-
-	virt_ptr = lio_dma_alloc(oct, (droq->max_count * OCT_DROQ_INFO_SIZE),
-				 &droq->info_list_dma);
-	if (virt_ptr) {
-		droq->info_alloc_size = droq->max_count * OCT_DROQ_INFO_SIZE;
-		droq->info_base_addr = virt_ptr;
-	}
-
-	return virt_ptr;
-}
-
-static inline void lio_free_info_buffer(struct octeon_device *oct,
-					struct octeon_droq *droq)
-{
-	lio_dma_free(oct, droq->info_alloc_size, droq->info_base_addr,
-		     droq->info_list_dma);
-}
-
 static inline
 void *get_rbd(struct sk_buff *skb)
 {
@@ -392,12 +369,6 @@ void *get_rbd(struct sk_buff *skb)
 }
 
 static inline u64
-lio_map_ring_info(struct octeon_droq *droq, u32 i)
-{
-	return droq->info_list_dma + (i * sizeof(struct octeon_droq_info));
-}
-
-static inline u64
 lio_map_ring(void *buf)
 {
 	dma_addr_t dma_addr;
@@ -443,8 +414,8 @@ static inline void octeon_fast_packet_next(struct octeon_droq *droq,
 					   int copy_len,
 					   int idx)
 {
-	memcpy(skb_put(nicbuf, copy_len),
-	       get_rbd(droq->recv_buf_list[idx].buffer), copy_len);
+	skb_put_data(nicbuf, get_rbd(droq->recv_buf_list[idx].buffer),
+		     copy_len);
 }
 
 /**
diff --git a/drivers/net/ethernet/cavium/liquidio/request_manager.c b/drivers/net/ethernet/cavium/liquidio/request_manager.c
index 261f448f9de2..7b297f1f6dbe 100644
--- a/drivers/net/ethernet/cavium/liquidio/request_manager.c
+++ b/drivers/net/ethernet/cavium/liquidio/request_manager.c
@@ -252,8 +252,7 @@ int lio_wait_for_instr_fetch(struct octeon_device *oct)
 			if (!(oct->io_qmask.iq & BIT_ULL(i)))
 				continue;
 			pending =
-			    atomic_read(&oct->
-					       instr_queue[i]->instr_pending);
+			    atomic_read(&oct->instr_queue[i]->instr_pending);
 			if (pending)
 				__check_db_timeout(oct, i);
 			instr_cnt += pending;
diff --git a/drivers/net/ethernet/cavium/octeon/octeon_mgmt.c b/drivers/net/ethernet/cavium/octeon/octeon_mgmt.c
index a2138686c605..2887bcaf6af5 100644
--- a/drivers/net/ethernet/cavium/octeon/octeon_mgmt.c
+++ b/drivers/net/ethernet/cavium/octeon/octeon_mgmt.c
@@ -755,6 +755,7 @@ static int octeon_mgmt_ioctl_hwtstamp(struct net_device *netdev,
 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+	case HWTSTAMP_FILTER_NTP_ALL:
 		p->has_rx_tstamp = have_hw_timestamps;
 		config.rx_filter = HWTSTAMP_FILTER_ALL;
 		if (p->has_rx_tstamp) {
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_main.c b/drivers/net/ethernet/cavium/thunder/nicvf_main.c
index d6477af88085..49b80da51ba7 100644
--- a/drivers/net/ethernet/cavium/thunder/nicvf_main.c
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_main.c
@@ -227,15 +227,14 @@ static void  nicvf_handle_mbx_intr(struct nicvf *nic)
 		nic->speed = mbx.link_status.speed;
 		nic->mac_type = mbx.link_status.mac_type;
 		if (nic->link_up) {
-			netdev_info(nic->netdev, "%s: Link is Up %d Mbps %s\n",
-				    nic->netdev->name, nic->speed,
+			netdev_info(nic->netdev, "Link is Up %d Mbps %s duplex\n",
+				    nic->speed,
 				    nic->duplex == DUPLEX_FULL ?
-				"Full duplex" : "Half duplex");
+				    "Full" : "Half");
 			netif_carrier_on(nic->netdev);
 			netif_tx_start_all_queues(nic->netdev);
 		} else {
-			netdev_info(nic->netdev, "%s: Link is Down\n",
-				    nic->netdev->name);
+			netdev_info(nic->netdev, "Link is Down\n");
 			netif_carrier_off(nic->netdev);
 			netif_tx_stop_all_queues(nic->netdev);
 		}
@@ -721,8 +720,7 @@ static void nicvf_rcv_pkt_handler(struct net_device *netdev,
 		return;
 
 	if (netif_msg_pktdata(nic)) {
-		netdev_info(nic->netdev, "%s: skb 0x%p, len=%d\n", netdev->name,
-			    skb, skb->len);
+		netdev_info(nic->netdev, "skb 0x%p, len=%d\n", skb, skb->len);
 		print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET, 16, 1,
 			       skb->data, skb->len, true);
 	}
@@ -854,10 +852,8 @@ done:
 			netif_tx_wake_queue(txq);
 			nic = nic->pnicvf;
 			this_cpu_inc(nic->drv_stats->txq_wake);
-			if (netif_msg_tx_err(nic))
-				netdev_warn(netdev,
-					    "%s: Transmit queue wakeup SQ%d\n",
-					    netdev->name, txq_idx);
+			netif_warn(nic, tx_err, netdev,
+				   "Transmit queue wakeup SQ%d\n", txq_idx);
 		}
 	}
 
@@ -928,9 +924,8 @@ static void nicvf_handle_qs_err(unsigned long data)
 
 static void nicvf_dump_intr_status(struct nicvf *nic)
 {
-	if (netif_msg_intr(nic))
-		netdev_info(nic->netdev, "%s: interrupt status 0x%llx\n",
-			    nic->netdev->name, nicvf_reg_read(nic, NIC_VF_INT));
+	netif_info(nic, intr, nic->netdev, "interrupt status 0x%llx\n",
+		   nicvf_reg_read(nic, NIC_VF_INT));
 }
 
 static irqreturn_t nicvf_misc_intr_handler(int irq, void *nicvf_irq)
@@ -1212,10 +1207,8 @@ static netdev_tx_t nicvf_xmit(struct sk_buff *skb, struct net_device *netdev)
 			netif_tx_wake_queue(txq);
 		} else {
 			this_cpu_inc(nic->drv_stats->txq_stop);
-			if (netif_msg_tx_err(nic))
-				netdev_warn(netdev,
-					    "%s: Transmit ring full, stopping SQ%d\n",
-					    netdev->name, qid);
+			netif_warn(nic, tx_err, netdev,
+				   "Transmit ring full, stopping SQ%d\n", qid);
 		}
 		return NETDEV_TX_BUSY;
 	}
@@ -1600,9 +1593,7 @@ static void nicvf_tx_timeout(struct net_device *dev)
 {
 	struct nicvf *nic = netdev_priv(dev);
 
-	if (netif_msg_tx_err(nic))
-		netdev_warn(dev, "%s: Transmit timed out, resetting\n",
-			    dev->name);
+	netif_warn(nic, tx_err, dev, "Transmit timed out, resetting\n");
 
 	this_cpu_inc(nic->drv_stats->tx_timeout);
 	schedule_work(&nic->reset_task);
@@ -1763,6 +1754,7 @@ static int nicvf_xdp(struct net_device *netdev, struct netdev_xdp *xdp)
 		return nicvf_xdp_setup(nic, xdp->prog);
 	case XDP_QUERY_PROG:
 		xdp->prog_attached = !!nic->xdp_prog;
+		xdp->prog_id = nic->xdp_prog ? nic->xdp_prog->aux->id : 0;
 		return 0;
 	default:
 		return -EINVAL;
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_queues.c b/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
index 2b181762ad49..d4496e9afcdf 100644
--- a/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
@@ -1811,11 +1811,9 @@ void nicvf_update_sq_stats(struct nicvf *nic, int sq_idx)
 /* Check for errors in the receive cmp.queue entry */
 int nicvf_check_cqe_rx_errs(struct nicvf *nic, struct cqe_rx_t *cqe_rx)
 {
-	if (netif_msg_rx_err(nic))
-		netdev_err(nic->netdev,
-			   "%s: RX error CQE err_level 0x%x err_opcode 0x%x\n",
-			   nic->netdev->name,
-			   cqe_rx->err_level, cqe_rx->err_opcode);
+	netif_err(nic, rx_err, nic->netdev,
+		  "RX error CQE err_level 0x%x err_opcode 0x%x\n",
+		  cqe_rx->err_level, cqe_rx->err_opcode);
 
 	switch (cqe_rx->err_opcode) {
 	case CQ_RX_ERROP_RE_PARTIAL:
diff --git a/drivers/net/ethernet/chelsio/cxgb/sge.c b/drivers/net/ethernet/chelsio/cxgb/sge.c
index d56142b98534..0f13a7f7c1d3 100644
--- a/drivers/net/ethernet/chelsio/cxgb/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb/sge.c
@@ -1801,7 +1801,7 @@ netdev_tx_t t1_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		eth_type = skb_network_offset(skb) == ETH_HLEN ?
 			CPL_ETH_II : CPL_ETH_II_VLAN;
 
-		hdr = (struct cpl_tx_pkt_lso *)skb_push(skb, sizeof(*hdr));
+		hdr = skb_push(skb, sizeof(*hdr));
 		hdr->opcode = CPL_TX_PKT_LSO;
 		hdr->ip_csum_dis = hdr->l4_csum_dis = 0;
 		hdr->ip_hdr_words = ip_hdr(skb)->ihl;
@@ -1849,7 +1849,7 @@ netdev_tx_t t1_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			}
 		}
 
-		cpl = (struct cpl_tx_pkt *)__skb_push(skb, sizeof(*cpl));
+		cpl = __skb_push(skb, sizeof(*cpl));
 		cpl->opcode = CPL_TX_PKT;
 		cpl->ip_csum_dis = 1;    /* SW calculates IP csum */
 		cpl->l4_csum_dis = skb->ip_summed == CHECKSUM_PARTIAL ? 0 : 1;
diff --git a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
index 2ff6bd139c96..0bc6a4ffce30 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
@@ -471,8 +471,7 @@ static int init_tp_parity(struct adapter *adap)
 		if (!skb)
 			goto alloc_skb_fail;
 
-		req = (struct cpl_smt_write_req *)__skb_put(skb, sizeof(*req));
-		memset(req, 0, sizeof(*req));
+		req = __skb_put_zero(skb, sizeof(*req));
 		req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 		OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_SMT_WRITE_REQ, i));
 		req->mtu_idx = NMTUS - 1;
@@ -495,8 +494,7 @@ static int init_tp_parity(struct adapter *adap)
 		if (!skb)
 			goto alloc_skb_fail;
 
-		req = (struct cpl_l2t_write_req *)__skb_put(skb, sizeof(*req));
-		memset(req, 0, sizeof(*req));
+		req = __skb_put_zero(skb, sizeof(*req));
 		req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 		OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_L2T_WRITE_REQ, i));
 		req->params = htonl(V_L2T_W_IDX(i));
@@ -518,8 +516,7 @@ static int init_tp_parity(struct adapter *adap)
 		if (!skb)
 			goto alloc_skb_fail;
 
-		req = (struct cpl_rte_write_req *)__skb_put(skb, sizeof(*req));
-		memset(req, 0, sizeof(*req));
+		req = __skb_put_zero(skb, sizeof(*req));
 		req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 		OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_RTE_WRITE_REQ, i));
 		req->l2t_idx = htonl(V_L2T_W_IDX(i));
@@ -538,8 +535,7 @@ static int init_tp_parity(struct adapter *adap)
 	if (!skb)
 		goto alloc_skb_fail;
 
-	greq = (struct cpl_set_tcb_field *)__skb_put(skb, sizeof(*greq));
-	memset(greq, 0, sizeof(*greq));
+	greq = __skb_put_zero(skb, sizeof(*greq));
 	greq->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 	OPCODE_TID(greq) = htonl(MK_OPCODE_TID(CPL_SET_TCB_FIELD, 0));
 	greq->mask = cpu_to_be64(1);
@@ -909,7 +905,7 @@ static int write_smt_entry(struct adapter *adapter, int idx)
 	if (!skb)
 		return -ENOMEM;
 
-	req = (struct cpl_smt_write_req *)__skb_put(skb, sizeof(*req));
+	req = __skb_put(skb, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_SMT_WRITE_REQ, idx));
 	req->mtu_idx = NMTUS - 1;	/* should be 0 but there's a T3 bug */
@@ -952,7 +948,7 @@ static int send_pktsched_cmd(struct adapter *adap, int sched, int qidx, int lo,
 	if (!skb)
 		return -ENOMEM;
 
-	req = (struct mngt_pktsched_wr *)skb_put(skb, sizeof(*req));
+	req = skb_put(skb, sizeof(*req));
 	req->wr_hi = htonl(V_WR_OP(FW_WROPCODE_MNGT));
 	req->mngt_opcode = FW_MNGTOPCODE_PKTSCHED_SET;
 	req->sched = sched;
diff --git a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c
index fa81445e334c..50cd660732c5 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c
@@ -552,7 +552,7 @@ static inline void mk_tid_release(struct sk_buff *skb, unsigned int tid)
 	struct cpl_tid_release *req;
 
 	skb->priority = CPL_PRIORITY_SETUP;
-	req = (struct cpl_tid_release *)__skb_put(skb, sizeof(*req));
+	req = __skb_put(skb, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_TID_RELEASE, tid));
 }
@@ -1096,7 +1096,7 @@ static void set_l2t_ix(struct t3cdev *tdev, u32 tid, struct l2t_entry *e)
 		return;
 	}
 	skb->priority = CPL_PRIORITY_CONTROL;
-	req = (struct cpl_set_tcb_field *)skb_put(skb, sizeof(*req));
+	req = skb_put(skb, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_SET_TCB_FIELD, tid));
 	req->reply = 0;
diff --git a/drivers/net/ethernet/chelsio/cxgb3/l2t.c b/drivers/net/ethernet/chelsio/cxgb3/l2t.c
index 26264125865f..248e40c6966c 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/l2t.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/l2t.c
@@ -96,7 +96,7 @@ static int setup_l2e_send_pending(struct t3cdev *dev, struct sk_buff *skb,
 			return -ENOMEM;
 	}
 
-	req = (struct cpl_l2t_write_req *)__skb_put(skb, sizeof(*req));
+	req = __skb_put(skb, sizeof(*req));
 	req->wr.wr_hi = htonl(V_WR_OP(FW_WROPCODE_FORWARD));
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_L2T_WRITE_REQ, e->idx));
 	req->params = htonl(V_L2T_W_IDX(e->idx) | V_L2T_W_IFF(e->smt_idx) |
diff --git a/drivers/net/ethernet/chelsio/cxgb3/sge.c b/drivers/net/ethernet/chelsio/cxgb3/sge.c
index 1b9d154f1149..e2d342647b19 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/sge.c
@@ -2282,7 +2282,7 @@ static int process_responses(struct adapter *adap, struct sge_qset *qs,
 			if (!skb)
 				goto no_mem;
 
-			memcpy(__skb_put(skb, AN_PKT_SIZE), r, AN_PKT_SIZE);
+			__skb_put_data(skb, r, AN_PKT_SIZE);
 			skb->data[0] = CPL_ASYNC_NOTIF;
 			rss_hi = htonl(CPL_ASYNC_NOTIF << 24);
 			q->async_notif++;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/Makefile b/drivers/net/ethernet/chelsio/cxgb4/Makefile
index c6b71f656992..817212702f0a 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/Makefile
+++ b/drivers/net/ethernet/chelsio/cxgb4/Makefile
@@ -4,7 +4,7 @@
 
 obj-$(CONFIG_CHELSIO_T4) += cxgb4.o
 
-cxgb4-objs := cxgb4_main.o l2t.o t4_hw.o sge.o clip_tbl.o cxgb4_ethtool.o cxgb4_uld.o sched.o cxgb4_filter.o cxgb4_tc_u32.o
+cxgb4-objs := cxgb4_main.o l2t.o t4_hw.o sge.o clip_tbl.o cxgb4_ethtool.o cxgb4_uld.o sched.o cxgb4_filter.o cxgb4_tc_u32.o cxgb4_ptp.o
 cxgb4-$(CONFIG_CHELSIO_T4_DCB) +=  cxgb4_dcb.o
 cxgb4-$(CONFIG_CHELSIO_T4_FCOE) +=  cxgb4_fcoe.o
 cxgb4-$(CONFIG_DEBUG_FS) += cxgb4_debugfs.o
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index e88c1808e46f..ef4be781fd05 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -48,6 +48,8 @@
 #include <linux/vmalloc.h>
 #include <linux/etherdevice.h>
 #include <linux/net_tstamp.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/ptp_classify.h>
 #include <asm/io.h>
 #include "t4_chip_type.h"
 #include "cxgb4_uld.h"
@@ -362,6 +364,11 @@ struct adapter_params {
 	unsigned int max_ordird_qp;       /* Max read depth per RDMA QP */
 	unsigned int max_ird_adapter;     /* Max read depth per adapter */
 	bool fr_nsmr_tpte_wr_support;	  /* FW support for FR_NSMR_TPTE_WR */
+
+	/* MPS Buffer Group Map[per Port].  Bit i is set if buffer group i is
+	 * used by the Port
+	 */
+	u8 mps_bg_map[MAX_NPORTS];	/* MPS Buffer Group Map */
 };
 
 /* State needed to monitor the forward progress of SGE Ingress DMA activities
@@ -505,6 +512,7 @@ struct port_info {
 #endif /* CONFIG_CHELSIO_T4_FCOE */
 	bool rxtstamp;  /* Enable TS */
 	struct hwtstamp_config tstamp_config;
+	bool ptp_enable;
 	struct sched_table *sched_tbl;
 };
 
@@ -700,6 +708,7 @@ struct sge_uld_txq_info {
 
 struct sge {
 	struct sge_eth_txq ethtxq[MAX_ETH_QSETS];
+	struct sge_eth_txq ptptxq;
 	struct sge_ctrl_txq ctrlq[MAX_CTRL_QUEUES];
 
 	struct sge_eth_rxq ethrxq[MAX_ETH_QSETS];
@@ -777,6 +786,7 @@ struct uld_msix_info {
 
 struct vf_info {
 	unsigned char vf_mac_addr[ETH_ALEN];
+	unsigned int tx_rate;
 	bool pf_set_mac;
 };
 
@@ -863,11 +873,17 @@ struct adapter {
 			 * used for all 4 filters.
 			 */
 
+	struct ptp_clock *ptp_clock;
+	struct ptp_clock_info ptp_clock_info;
+	struct sk_buff *ptp_tx_skb;
+	/* ptp lock */
+	spinlock_t ptp_lock;
 	spinlock_t stats_lock;
 	spinlock_t win0_lock ____cacheline_aligned_in_smp;
 
 	/* TC u32 offload */
 	struct cxgb4_tc_u32_table *tc_u32;
+	struct chcr_stats_debug chcr_stats;
 };
 
 /* Support for "sched-class" command to allow a TX Scheduling Class to be
@@ -1433,7 +1449,8 @@ void t4_read_rss_vf_config(struct adapter *adapter, unsigned int index,
 u32 t4_read_rss_pf_map(struct adapter *adapter);
 u32 t4_read_rss_pf_mask(struct adapter *adapter);
 
-unsigned int t4_get_mps_bg_map(struct adapter *adapter, int idx);
+unsigned int t4_get_mps_bg_map(struct adapter *adapter, int pidx);
+unsigned int t4_get_tp_ch_map(struct adapter *adapter, int pidx);
 void t4_pmtx_get_stats(struct adapter *adap, u32 cnt[], u64 cycles[]);
 void t4_pmrx_get_stats(struct adapter *adap, u32 cnt[], u64 cycles[]);
 int t4_read_cim_ibq(struct adapter *adap, unsigned int qid, u32 *data,
@@ -1493,9 +1510,12 @@ int t4_fw_initialize(struct adapter *adap, unsigned int mbox);
 int t4_query_params(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		    unsigned int vf, unsigned int nparams, const u32 *params,
 		    u32 *val);
+int t4_query_params_ns(struct adapter *adap, unsigned int mbox, unsigned int pf,
+		       unsigned int vf, unsigned int nparams, const u32 *params,
+		       u32 *val);
 int t4_query_params_rw(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		       unsigned int vf, unsigned int nparams, const u32 *params,
-		       u32 *val, int rw);
+		       u32 *val, int rw, bool sleep_ok);
 int t4_set_params_timeout(struct adapter *adap, unsigned int mbox,
 			  unsigned int pf, unsigned int vf,
 			  unsigned int nparams, const u32 *params,
@@ -1551,6 +1571,7 @@ int t4_ofld_eq_free(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		    unsigned int vf, unsigned int eqid);
 int t4_sge_ctxt_flush(struct adapter *adap, unsigned int mbox);
 void t4_handle_get_port_info(struct port_info *pi, const __be64 *rpl);
+int t4_update_port_info(struct port_info *pi);
 int t4_handle_fw_rpl(struct adapter *adap, const __be64 *rpl);
 void t4_db_full(struct adapter *adapter);
 void t4_db_dropped(struct adapter *adapter);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
index 1fa34b009891..76540b0e082d 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
@@ -2669,6 +2669,8 @@ static int tid_info_show(struct seq_file *seq, void *v)
 
 	if (t4_read_reg(adap, LE_DB_CONFIG_A) & HASHEN_F) {
 		unsigned int sb;
+		seq_printf(seq, "Connections in use: %u\n",
+			   atomic_read(&t->conns_in_use));
 
 		if (chip <= CHELSIO_T5)
 			sb = t4_read_reg(adap, LE_DB_SERVER_INDEX_A) / 4;
@@ -2699,17 +2701,23 @@ static int tid_info_show(struct seq_file *seq, void *v)
 				   atomic_read(&t->hash_tids_in_use));
 		}
 	} else if (t->ntids) {
+		seq_printf(seq, "Connections in use: %u\n",
+			   atomic_read(&t->conns_in_use));
+
 		seq_printf(seq, "TID range: 0..%u", t->ntids - 1);
 		seq_printf(seq, ", in use: %u\n",
 			   atomic_read(&t->tids_in_use));
 	}
 
 	if (t->nstids)
-		seq_printf(seq, "STID range: %u..%u, in use: %u\n",
+		seq_printf(seq, "STID range: %u..%u, in use-IPv4/IPv6: %u/%u\n",
 			   (!t->stid_base &&
 			   (chip <= CHELSIO_T5)) ?
 			   t->stid_base + 1 : t->stid_base,
-			   t->stid_base + t->nstids - 1, t->stids_in_use);
+			   t->stid_base + t->nstids - 1,
+			   t->stids_in_use - t->v6_stids_in_use,
+			   t->v6_stids_in_use);
+
 	if (t->natids)
 		seq_printf(seq, "ATID range: 0..%u, in use: %u\n",
 			   t->natids - 1, t->atids_in_use);
@@ -3069,6 +3077,40 @@ static const struct file_operations meminfo_fops = {
 	.llseek  = seq_lseek,
 	.release = single_release,
 };
+
+static int chcr_show(struct seq_file *seq, void *v)
+{
+	struct adapter *adap = seq->private;
+
+	seq_puts(seq, "Chelsio Crypto Accelerator Stats \n");
+	seq_printf(seq, "Cipher Ops: %10u \n",
+		   atomic_read(&adap->chcr_stats.cipher_rqst));
+	seq_printf(seq, "Digest Ops: %10u \n",
+		   atomic_read(&adap->chcr_stats.digest_rqst));
+	seq_printf(seq, "Aead Ops: %10u \n",
+		   atomic_read(&adap->chcr_stats.aead_rqst));
+	seq_printf(seq, "Completion: %10u \n",
+		   atomic_read(&adap->chcr_stats.complete));
+	seq_printf(seq, "Error: %10u \n",
+		   atomic_read(&adap->chcr_stats.error));
+	seq_printf(seq, "Fallback: %10u \n",
+		   atomic_read(&adap->chcr_stats.fallback));
+	return 0;
+}
+
+
+static int chcr_stats_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, chcr_show, inode->i_private);
+}
+
+static const struct file_operations chcr_stats_debugfs_fops = {
+        .owner   = THIS_MODULE,
+        .open    = chcr_stats_open,
+        .read    = seq_read,
+        .llseek  = seq_lseek,
+        .release = single_release,
+};
 /* Add an array of Debug FS files.
  */
 void add_debugfs_files(struct adapter *adap,
@@ -3143,6 +3185,7 @@ int t4_setup_debugfs(struct adapter *adap)
 		{ "tids", &tid_info_debugfs_fops, S_IRUSR, 0},
 		{ "blocked_fl", &blocked_fl_fops, S_IRUSR | S_IWUSR, 0 },
 		{ "meminfo", &meminfo_fops, S_IRUSR, 0 },
+		{ "crypto", &chcr_stats_debugfs_fops, S_IRUSR, 0 },
 	};
 
 	/* Debug FS nodes common to all T5 and later adapters.
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c
index 0ba7866c8259..26eb00a45db1 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c
@@ -500,7 +500,11 @@ static int from_fw_port_mod_type(enum fw_port_type port_type,
 	} else if (port_type == FW_PORT_TYPE_SFP ||
 		   port_type == FW_PORT_TYPE_QSFP_10G ||
 		   port_type == FW_PORT_TYPE_QSA ||
-		   port_type == FW_PORT_TYPE_QSFP) {
+		   port_type == FW_PORT_TYPE_QSFP ||
+		   port_type == FW_PORT_TYPE_CR4_QSFP ||
+		   port_type == FW_PORT_TYPE_CR_QSFP ||
+		   port_type == FW_PORT_TYPE_CR2_QSFP ||
+		   port_type == FW_PORT_TYPE_SFP28) {
 		if (mod_type == FW_PORT_MOD_TYPE_LR ||
 		    mod_type == FW_PORT_MOD_TYPE_SR ||
 		    mod_type == FW_PORT_MOD_TYPE_ER ||
@@ -511,6 +515,9 @@ static int from_fw_port_mod_type(enum fw_port_type port_type,
 			return PORT_DA;
 		else
 			return PORT_OTHER;
+	} else if (port_type == FW_PORT_TYPE_KR4_100G ||
+		   port_type == FW_PORT_TYPE_KR_SFP28) {
+		return PORT_NONE;
 	}
 
 	return PORT_OTHER;
@@ -618,7 +625,21 @@ static void fw_caps_to_lmm(enum fw_port_type port_type,
 	case FW_PORT_TYPE_CR_QSFP:
 	case FW_PORT_TYPE_SFP28:
 		SET_LMM(FIBRE);
-		SET_LMM(25000baseCR_Full);
+		FW_CAPS_TO_LMM(SPEED_1G, 1000baseT_Full);
+		FW_CAPS_TO_LMM(SPEED_10G, 10000baseT_Full);
+		FW_CAPS_TO_LMM(SPEED_25G, 25000baseCR_Full);
+		break;
+
+	case FW_PORT_TYPE_KR_SFP28:
+		SET_LMM(Backplane);
+		FW_CAPS_TO_LMM(SPEED_1G, 1000baseT_Full);
+		FW_CAPS_TO_LMM(SPEED_10G, 10000baseKR_Full);
+		FW_CAPS_TO_LMM(SPEED_25G, 25000baseKR_Full);
+		break;
+
+	case FW_PORT_TYPE_CR2_QSFP:
+		SET_LMM(FIBRE);
+		SET_LMM(50000baseSR2_Full);
 		break;
 
 	case FW_PORT_TYPE_KR4_100G:
@@ -674,13 +695,20 @@ static unsigned int lmm_to_fw_caps(const unsigned long *link_mode_mask)
 static int get_link_ksettings(struct net_device *dev,
 			      struct ethtool_link_ksettings *link_ksettings)
 {
-	const struct port_info *pi = netdev_priv(dev);
+	struct port_info *pi = netdev_priv(dev);
 	struct ethtool_link_settings *base = &link_ksettings->base;
 
 	ethtool_link_ksettings_zero_link_mode(link_ksettings, supported);
 	ethtool_link_ksettings_zero_link_mode(link_ksettings, advertising);
 	ethtool_link_ksettings_zero_link_mode(link_ksettings, lp_advertising);
 
+	/* For the nonce, the Firmware doesn't send up Port State changes
+	 * when the Virtual Interface attached to the Port is down.  So
+	 * if it's down, let's grab any changes.
+	 */
+	if (!netif_running(dev))
+		(void)t4_update_port_info(pi);
+
 	base->port = from_fw_port_mod_type(pi->port_type, pi->mod_type);
 
 	if (pi->mdio_addr >= 0) {
@@ -1085,14 +1113,31 @@ static int set_flash(struct net_device *netdev, struct ethtool_flash *ef)
 
 static int get_ts_info(struct net_device *dev, struct ethtool_ts_info *ts_info)
 {
+	struct port_info *pi = netdev_priv(dev);
+	struct  adapter *adapter = pi->adapter;
+
 	ts_info->so_timestamping = SOF_TIMESTAMPING_TX_SOFTWARE |
 				   SOF_TIMESTAMPING_RX_SOFTWARE |
 				   SOF_TIMESTAMPING_SOFTWARE;
 
 	ts_info->so_timestamping |= SOF_TIMESTAMPING_RX_HARDWARE |
+				    SOF_TIMESTAMPING_TX_HARDWARE |
 				    SOF_TIMESTAMPING_RAW_HARDWARE;
 
-	ts_info->phc_index = -1;
+	ts_info->tx_types = (1 << HWTSTAMP_TX_OFF) |
+			    (1 << HWTSTAMP_TX_ON);
+
+	ts_info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
+			      (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) |
+			      (1 << HWTSTAMP_FILTER_PTP_V1_L4_SYNC) |
+			      (1 << HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ) |
+			      (1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC) |
+			      (1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ);
+
+	if (adapter->ptp_clock)
+		ts_info->phc_index = ptp_clock_index(adapter->ptp_clock);
+	else
+		ts_info->phc_index = -1;
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_filter.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_filter.c
index 10736738ff30..45b5853ca2f1 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_filter.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_filter.c
@@ -190,7 +190,7 @@ static int del_filter_wr(struct adapter *adapter, int fidx)
 	if (!skb)
 		return -ENOMEM;
 
-	fwr = (struct fw_filter_wr *)__skb_put(skb, len);
+	fwr = __skb_put(skb, len);
 	t4_mk_filtdelwr(f->tid, fwr, adapter->sge.fw_evtq.abs_id);
 
 	/* Mark the filter as "pending" and ship off the Filter Work Request.
@@ -231,8 +231,7 @@ int set_filter_wr(struct adapter *adapter, int fidx)
 		}
 	}
 
-	fwr = (struct fw_filter_wr *)__skb_put(skb, sizeof(*fwr));
-	memset(fwr, 0, sizeof(*fwr));
+	fwr = __skb_put_zero(skb, sizeof(*fwr));
 
 	/* It would be nice to put most of the following in t4_hw.c but most
 	 * of the work is translating the cxgbtool ch_filter_specification
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index 77ed2f628f9c..86f92e31e8aa 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -79,6 +79,7 @@
 #include "l2t.h"
 #include "sched.h"
 #include "cxgb4_tc_u32.h"
+#include "cxgb4_ptp.h"
 
 char cxgb4_driver_name[] = KBUILD_MODNAME;
 
@@ -824,9 +825,12 @@ static int setup_sge_queues(struct adapter *adap)
 {
 	int err, i, j;
 	struct sge *s = &adap->sge;
-	struct sge_uld_rxq_info *rxq_info = s->uld_rxq_info[CXGB4_ULD_RDMA];
+	struct sge_uld_rxq_info *rxq_info = NULL;
 	unsigned int cmplqid = 0;
 
+	if (is_uld(adap))
+		rxq_info = s->uld_rxq_info[CXGB4_ULD_RDMA];
+
 	for_each_port(adap, i) {
 		struct net_device *dev = adap->port[i];
 		struct port_info *pi = netdev_priv(dev);
@@ -840,8 +844,8 @@ static int setup_sge_queues(struct adapter *adap)
 					       adap->msi_idx, &q->fl,
 					       t4_ethrx_handler,
 					       NULL,
-					       t4_get_mps_bg_map(adap,
-								 pi->tx_chan));
+					       t4_get_tp_ch_map(adap,
+								pi->tx_chan));
 			if (err)
 				goto freeout;
 			q->rspq.idx = j;
@@ -869,6 +873,14 @@ static int setup_sge_queues(struct adapter *adap)
 			goto freeout;
 	}
 
+	if (!is_t4(adap->params.chip)) {
+		err = t4_sge_alloc_eth_txq(adap, &s->ptptxq, adap->port[0],
+					   netdev_get_tx_queue(adap->port[0], 0)
+					   , s->fw_evtq.cntxt_id);
+		if (err)
+			goto freeout;
+	}
+
 	t4_write_reg(adap, is_t4(adap->params.chip) ?
 				MPS_TRC_RSS_CONTROL_A :
 				MPS_T5_TRC_RSS_CONTROL_A,
@@ -891,7 +903,7 @@ static u16 cxgb_select_queue(struct net_device *dev, struct sk_buff *skb,
 	 * The skb's priority is determined via the VLAN Tag Priority Code
 	 * Point field.
 	 */
-	if (cxgb4_dcb_enabled(dev)) {
+	if (cxgb4_dcb_enabled(dev) && !is_kdump_kernel()) {
 		u16 vlan_tci;
 		int err;
 
@@ -1093,10 +1105,12 @@ int cxgb4_alloc_stid(struct tid_info *t, int family, void *data)
 		 * This is equivalent to 4 TIDs. With CLIP enabled it
 		 * needs 2 TIDs.
 		 */
-		if (family == PF_INET)
-			t->stids_in_use++;
-		else
+		if (family == PF_INET6) {
 			t->stids_in_use += 2;
+			t->v6_stids_in_use += 2;
+		} else {
+			t->stids_in_use++;
+		}
 	}
 	spin_unlock_bh(&t->stid_lock);
 	return stid;
@@ -1150,13 +1164,16 @@ void cxgb4_free_stid(struct tid_info *t, unsigned int stid, int family)
 		bitmap_release_region(t->stid_bmap, stid, 1);
 	t->stid_tab[stid].data = NULL;
 	if (stid < t->nstids) {
-		if (family == PF_INET)
-			t->stids_in_use--;
-		else
+		if (family == PF_INET6) {
 			t->stids_in_use -= 2;
+			t->v6_stids_in_use -= 2;
+		} else {
+			t->stids_in_use--;
+		}
 	} else {
 		t->sftids_in_use--;
 	}
+
 	spin_unlock_bh(&t->stid_lock);
 }
 EXPORT_SYMBOL(cxgb4_free_stid);
@@ -1170,7 +1187,7 @@ static void mk_tid_release(struct sk_buff *skb, unsigned int chan,
 	struct cpl_tid_release *req;
 
 	set_wr_txq(skb, CPL_PRIORITY_SETUP, chan);
-	req = (struct cpl_tid_release *)__skb_put(skb, sizeof(*req));
+	req = __skb_put(skb, sizeof(*req));
 	INIT_TP_WR(req, tid);
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_TID_RELEASE, tid));
 }
@@ -1232,7 +1249,8 @@ static void process_tid_release_list(struct work_struct *work)
  * Release a TID and inform HW.  If we are unable to allocate the release
  * message we defer to a work queue.
  */
-void cxgb4_remove_tid(struct tid_info *t, unsigned int chan, unsigned int tid)
+void cxgb4_remove_tid(struct tid_info *t, unsigned int chan, unsigned int tid,
+		      unsigned short family)
 {
 	struct sk_buff *skb;
 	struct adapter *adap = container_of(t, struct adapter, tids);
@@ -1241,10 +1259,18 @@ void cxgb4_remove_tid(struct tid_info *t, unsigned int chan, unsigned int tid)
 
 	if (t->tid_tab[tid]) {
 		t->tid_tab[tid] = NULL;
-		if (t->hash_base && (tid >= t->hash_base))
-			atomic_dec(&t->hash_tids_in_use);
-		else
-			atomic_dec(&t->tids_in_use);
+		atomic_dec(&t->conns_in_use);
+		if (t->hash_base && (tid >= t->hash_base)) {
+			if (family == AF_INET6)
+				atomic_sub(2, &t->hash_tids_in_use);
+			else
+				atomic_dec(&t->hash_tids_in_use);
+		} else {
+			if (family == AF_INET6)
+				atomic_sub(2, &t->tids_in_use);
+			else
+				atomic_dec(&t->tids_in_use);
+		}
 	}
 
 	skb = alloc_skb(sizeof(struct cpl_tid_release), GFP_ATOMIC);
@@ -1292,10 +1318,12 @@ static int tid_init(struct tid_info *t)
 	spin_lock_init(&t->ftid_lock);
 
 	t->stids_in_use = 0;
+	t->v6_stids_in_use = 0;
 	t->sftids_in_use = 0;
 	t->afree = NULL;
 	t->atids_in_use = 0;
 	atomic_set(&t->tids_in_use, 0);
+	atomic_set(&t->conns_in_use, 0);
 	atomic_set(&t->hash_tids_in_use, 0);
 
 	/* Setup the free list for atid_tab and clear the stid bitmap. */
@@ -1343,7 +1371,7 @@ int cxgb4_create_server(const struct net_device *dev, unsigned int stid,
 		return -ENOMEM;
 
 	adap = netdev2adap(dev);
-	req = (struct cpl_pass_open_req *)__skb_put(skb, sizeof(*req));
+	req = __skb_put(skb, sizeof(*req));
 	INIT_TP_WR(req, 0);
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_PASS_OPEN_REQ, stid));
 	req->local_port = sport;
@@ -1384,7 +1412,7 @@ int cxgb4_create_server6(const struct net_device *dev, unsigned int stid,
 		return -ENOMEM;
 
 	adap = netdev2adap(dev);
-	req = (struct cpl_pass_open_req6 *)__skb_put(skb, sizeof(*req));
+	req = __skb_put(skb, sizeof(*req));
 	INIT_TP_WR(req, 0);
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_PASS_OPEN_REQ6, stid));
 	req->local_port = sport;
@@ -1416,7 +1444,7 @@ int cxgb4_remove_server(const struct net_device *dev, unsigned int stid,
 	if (!skb)
 		return -ENOMEM;
 
-	req = (struct cpl_close_listsvr_req *)__skb_put(skb, sizeof(*req));
+	req = __skb_put(skb, sizeof(*req));
 	INIT_TP_WR(req, 0);
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_CLOSE_LISTSRV_REQ, stid));
 	req->reply_ctrl = htons(NO_REPLY_V(0) | (ipv6 ? LISTSVR_IPV6_V(1) :
@@ -2171,9 +2199,10 @@ static int cxgb_up(struct adapter *adap)
 {
 	int err;
 
+	mutex_lock(&uld_mutex);
 	err = setup_sge_queues(adap);
 	if (err)
-		goto out;
+		goto rel_lock;
 	err = setup_rss(adap);
 	if (err)
 		goto freeq;
@@ -2197,7 +2226,6 @@ static int cxgb_up(struct adapter *adap)
 			goto irq_err;
 	}
 
-	mutex_lock(&uld_mutex);
 	enable_rx(adap);
 	t4_sge_start(adap);
 	t4_intr_enable(adap);
@@ -2210,13 +2238,15 @@ static int cxgb_up(struct adapter *adap)
 #endif
 	/* Initialize hash mac addr list*/
 	INIT_LIST_HEAD(&adap->mac_hlist);
- out:
 	return err;
+
  irq_err:
 	dev_err(adap->pdev_dev, "request_irq failed, err %d\n", err);
  freeq:
 	t4_free_sge_resources(adap);
-	goto out;
+ rel_lock:
+	mutex_unlock(&uld_mutex);
+	return err;
 }
 
 static void cxgb_down(struct adapter *adapter)
@@ -2249,6 +2279,13 @@ static int cxgb_open(struct net_device *dev)
 			return err;
 	}
 
+	/* It's possible that the basic port information could have
+	 * changed since we first read it.
+	 */
+	err = t4_update_port_info(pi);
+	if (err < 0)
+		return err;
+
 	err = link_start(dev);
 	if (!err)
 		netif_tx_start_all_queues(dev);
@@ -2410,6 +2447,7 @@ static int cxgb_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
 	unsigned int mbox;
 	int ret = 0, prtad, devad;
 	struct port_info *pi = netdev_priv(dev);
+	struct adapter *adapter = pi->adapter;
 	struct mii_ioctl_data *data = (struct mii_ioctl_data *)&req->ifr_data;
 
 	switch (cmd) {
@@ -2447,18 +2485,69 @@ static int cxgb_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
 				   sizeof(pi->tstamp_config)))
 			return -EFAULT;
 
-		switch (pi->tstamp_config.rx_filter) {
-		case HWTSTAMP_FILTER_NONE:
+		if (!is_t4(adapter->params.chip)) {
+			switch (pi->tstamp_config.tx_type) {
+			case HWTSTAMP_TX_OFF:
+			case HWTSTAMP_TX_ON:
+				break;
+			default:
+				return -ERANGE;
+			}
+
+			switch (pi->tstamp_config.rx_filter) {
+			case HWTSTAMP_FILTER_NONE:
+				pi->rxtstamp = false;
+				break;
+			case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+			case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+				cxgb4_ptprx_timestamping(pi, pi->port_id,
+							 PTP_TS_L4);
+				break;
+			case HWTSTAMP_FILTER_PTP_V2_EVENT:
+				cxgb4_ptprx_timestamping(pi, pi->port_id,
+							 PTP_TS_L2_L4);
+				break;
+			case HWTSTAMP_FILTER_ALL:
+			case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+			case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+			case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+			case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+				pi->rxtstamp = true;
+				break;
+			default:
+				pi->tstamp_config.rx_filter =
+					HWTSTAMP_FILTER_NONE;
+				return -ERANGE;
+			}
+
+			if ((pi->tstamp_config.tx_type == HWTSTAMP_TX_OFF) &&
+			    (pi->tstamp_config.rx_filter ==
+				HWTSTAMP_FILTER_NONE)) {
+				if (cxgb4_ptp_txtype(adapter, pi->port_id) >= 0)
+					pi->ptp_enable = false;
+			}
+
+			if (pi->tstamp_config.rx_filter !=
+				HWTSTAMP_FILTER_NONE) {
+				if (cxgb4_ptp_redirect_rx_packet(adapter,
+								 pi) >= 0)
+					pi->ptp_enable = true;
+			}
+		} else {
+			/* For T4 Adapters */
+			switch (pi->tstamp_config.rx_filter) {
+			case HWTSTAMP_FILTER_NONE:
 			pi->rxtstamp = false;
 			break;
-		case HWTSTAMP_FILTER_ALL:
+			case HWTSTAMP_FILTER_ALL:
 			pi->rxtstamp = true;
 			break;
-		default:
-			pi->tstamp_config.rx_filter = HWTSTAMP_FILTER_NONE;
+			default:
+			pi->tstamp_config.rx_filter =
+			HWTSTAMP_FILTER_NONE;
 			return -ERANGE;
+			}
 		}
-
 		return copy_to_user(req->ifr_data, &pi->tstamp_config,
 				    sizeof(pi->tstamp_config)) ?
 			-EFAULT : 0;
@@ -2560,6 +2649,8 @@ static int cxgb_get_vf_config(struct net_device *dev,
 	if (vf >= adap->num_vfs)
 		return -EINVAL;
 	ivi->vf = vf;
+	ivi->max_tx_rate = adap->vfinfo[vf].tx_rate;
+	ivi->min_tx_rate = 0;
 	ether_addr_copy(ivi->mac, adap->vfinfo[vf].vf_mac_addr);
 	return 0;
 }
@@ -2576,6 +2667,109 @@ static int cxgb_get_phys_port_id(struct net_device *dev,
 	return 0;
 }
 
+static int cxgb_set_vf_rate(struct net_device *dev, int vf, int min_tx_rate,
+			    int max_tx_rate)
+{
+	struct port_info *pi = netdev_priv(dev);
+	struct adapter *adap = pi->adapter;
+	struct fw_port_cmd port_cmd, port_rpl;
+	u32 link_status, speed = 0;
+	u32 fw_pfvf, fw_class;
+	int class_id = vf;
+	int link_ok, ret;
+	u16 pktsize;
+
+	if (vf >= adap->num_vfs)
+		return -EINVAL;
+
+	if (min_tx_rate) {
+		dev_err(adap->pdev_dev,
+			"Min tx rate (%d) (> 0) for VF %d is Invalid.\n",
+			min_tx_rate, vf);
+		return -EINVAL;
+	}
+	/* Retrieve link details for VF port */
+	memset(&port_cmd, 0, sizeof(port_cmd));
+	port_cmd.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PORT_CMD) |
+					    FW_CMD_REQUEST_F |
+					    FW_CMD_READ_F |
+					    FW_PORT_CMD_PORTID_V(pi->port_id));
+	port_cmd.action_to_len16 =
+		cpu_to_be32(FW_PORT_CMD_ACTION_V(FW_PORT_ACTION_GET_PORT_INFO) |
+			    FW_LEN16(port_cmd));
+	ret = t4_wr_mbox(adap, adap->mbox, &port_cmd, sizeof(port_cmd),
+			 &port_rpl);
+	if (ret != FW_SUCCESS) {
+		dev_err(adap->pdev_dev,
+			"Failed to get link status for VF %d\n", vf);
+		return -EINVAL;
+	}
+	link_status = be32_to_cpu(port_rpl.u.info.lstatus_to_modtype);
+	link_ok = (link_status & FW_PORT_CMD_LSTATUS_F) != 0;
+	if (!link_ok) {
+		dev_err(adap->pdev_dev, "Link down for VF %d\n", vf);
+		return -EINVAL;
+	}
+	/* Determine link speed */
+	if (link_status & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_100M))
+		speed = 100;
+	else if (link_status & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_1G))
+		speed = 1000;
+	else if (link_status & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_10G))
+		speed = 10000;
+	else if (link_status & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_25G))
+		speed = 25000;
+	else if (link_status & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_40G))
+		speed = 40000;
+	else if (link_status & FW_PORT_CMD_LSPEED_V(FW_PORT_CAP_SPEED_100G))
+		speed = 100000;
+
+	if (max_tx_rate > speed) {
+		dev_err(adap->pdev_dev,
+			"Max tx rate %d for VF %d can't be > link-speed %u",
+			max_tx_rate, vf, speed);
+		return -EINVAL;
+	}
+	pktsize = be16_to_cpu(port_rpl.u.info.mtu);
+	/* subtract ethhdr size and 4 bytes crc since, f/w appends it */
+	pktsize = pktsize - sizeof(struct ethhdr) - 4;
+	/* subtract ipv4 hdr size, tcp hdr size to get typical IPv4 MSS size */
+	pktsize = pktsize - sizeof(struct iphdr) - sizeof(struct tcphdr);
+	/* configure Traffic Class for rate-limiting */
+	ret = t4_sched_params(adap, SCHED_CLASS_TYPE_PACKET,
+			      SCHED_CLASS_LEVEL_CL_RL,
+			      SCHED_CLASS_MODE_CLASS,
+			      SCHED_CLASS_RATEUNIT_BITS,
+			      SCHED_CLASS_RATEMODE_ABS,
+			      pi->port_id, class_id, 0,
+			      max_tx_rate * 1000, 0, pktsize);
+	if (ret) {
+		dev_err(adap->pdev_dev, "Err %d for Traffic Class config\n",
+			ret);
+		return -EINVAL;
+	}
+	dev_info(adap->pdev_dev,
+		 "Class %d with MSS %u configured with rate %u\n",
+		 class_id, pktsize, max_tx_rate);
+
+	/* bind VF to configured Traffic Class */
+	fw_pfvf = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_PFVF) |
+		   FW_PARAMS_PARAM_X_V(FW_PARAMS_PARAM_PFVF_SCHEDCLASS_ETH));
+	fw_class = class_id;
+	ret = t4_set_params(adap, adap->mbox, adap->pf, vf + 1, 1, &fw_pfvf,
+			    &fw_class);
+	if (ret) {
+		dev_err(adap->pdev_dev,
+			"Err %d in binding VF %d to Traffic Class %d\n",
+			ret, vf, class_id);
+		return -EINVAL;
+	}
+	dev_info(adap->pdev_dev, "PF %d VF %d is bound to Class %d\n",
+		 adap->pf, vf, class_id);
+	adap->vfinfo[vf].tx_rate = max_tx_rate;
+	return 0;
+}
+
 #endif
 
 static int cxgb_set_mac_addr(struct net_device *dev, void *p)
@@ -2695,12 +2889,15 @@ static int cxgb_set_tx_maxrate(struct net_device *dev, int index, u32 rate)
 	return err;
 }
 
-static int cxgb_setup_tc(struct net_device *dev, u32 handle, __be16 proto,
-			 struct tc_to_netdev *tc)
+static int cxgb_setup_tc(struct net_device *dev, u32 handle, u32 chain_index,
+			 __be16 proto, struct tc_to_netdev *tc)
 {
 	struct port_info *pi = netdev2pinfo(dev);
 	struct adapter *adap = netdev2adap(dev);
 
+	if (chain_index)
+		return -EOPNOTSUPP;
+
 	if (!(adap->flags & FULL_INIT_DONE)) {
 		dev_err(adap->pdev_dev,
 			"Failed to setup tc on port %d. Link Down?\n",
@@ -2724,6 +2921,16 @@ static int cxgb_setup_tc(struct net_device *dev, u32 handle, __be16 proto,
 	return -EOPNOTSUPP;
 }
 
+static netdev_features_t cxgb_fix_features(struct net_device *dev,
+					   netdev_features_t features)
+{
+	/* Disable GRO, if RX_CSUM is disabled */
+	if (!(features & NETIF_F_RXCSUM))
+		features &= ~NETIF_F_GRO;
+
+	return features;
+}
+
 static const struct net_device_ops cxgb4_netdev_ops = {
 	.ndo_open             = cxgb_open,
 	.ndo_stop             = cxgb_close,
@@ -2745,6 +2952,7 @@ static const struct net_device_ops cxgb4_netdev_ops = {
 #endif /* CONFIG_CHELSIO_T4_FCOE */
 	.ndo_set_tx_maxrate   = cxgb_set_tx_maxrate,
 	.ndo_setup_tc         = cxgb_setup_tc,
+	.ndo_fix_features     = cxgb_fix_features,
 };
 
 #ifdef CONFIG_PCI_IOV
@@ -2752,6 +2960,7 @@ static const struct net_device_ops cxgb4_mgmt_netdev_ops = {
 	.ndo_open             = dummy_open,
 	.ndo_set_vf_mac       = cxgb_set_vf_mac,
 	.ndo_get_vf_config    = cxgb_get_vf_config,
+	.ndo_set_vf_rate      = cxgb_set_vf_rate,
 	.ndo_get_phys_port_id = cxgb_get_phys_port_id,
 };
 #endif
@@ -4016,10 +4225,7 @@ static void cfg_queues(struct adapter *adap)
 
 	/* Reduce memory usage in kdump environment, disable all offload.
 	 */
-	if (is_kdump_kernel()) {
-		adap->params.offload = 0;
-		adap->params.crypto = 0;
-	} else if (is_uld(adap) && t4_uld_mem_alloc(adap)) {
+	if (is_kdump_kernel() || (is_uld(adap) && t4_uld_mem_alloc(adap))) {
 		adap->params.offload = 0;
 		adap->params.crypto = 0;
 	}
@@ -4040,7 +4246,7 @@ static void cfg_queues(struct adapter *adap)
 		struct port_info *pi = adap2pinfo(adap, i);
 
 		pi->first_qset = qidx;
-		pi->nqsets = 8;
+		pi->nqsets = is_kdump_kernel() ? 1 : 8;
 		qidx += pi->nqsets;
 	}
 #else /* !CONFIG_CHELSIO_T4_DCB */
@@ -4053,6 +4259,9 @@ static void cfg_queues(struct adapter *adap)
 	if (q10g > netif_get_num_default_rss_queues())
 		q10g = netif_get_num_default_rss_queues();
 
+	if (is_kdump_kernel())
+		q10g = 1;
+
 	for_each_port(adap, i) {
 		struct port_info *pi = adap2pinfo(adap, i);
 
@@ -4092,6 +4301,9 @@ static void cfg_queues(struct adapter *adap)
 	for (i = 0; i < ARRAY_SIZE(s->ctrlq); i++)
 		s->ctrlq[i].q.size = 512;
 
+	if (!is_t4(adap->params.chip))
+		s->ptptxq.q.size = 8;
+
 	init_rspq(adap, &s->fw_evtq, 0, 1, 1024, 64);
 	init_rspq(adap, &s->intrq, 0, 1, 512, 64);
 }
@@ -4525,7 +4737,7 @@ static void dummy_setup(struct net_device *dev)
 	/* Initialize the device structure. */
 	dev->netdev_ops = &cxgb4_mgmt_netdev_ops;
 	dev->ethtool_ops = &cxgb4_mgmt_ethtool_ops;
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 }
 
 static int config_mgmt_dev(struct pci_dev *pdev)
@@ -4958,6 +5170,8 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		netif_set_real_num_tx_queues(adapter->port[i], pi->nqsets);
 		netif_set_real_num_rx_queues(adapter->port[i], pi->nqsets);
 
+		netif_carrier_off(adapter->port[i]);
+
 		err = register_netdev(adapter->port[i]);
 		if (err)
 			break;
@@ -4988,6 +5202,9 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		mutex_unlock(&uld_mutex);
 	}
 
+	if (!is_t4(adapter->params.chip))
+		cxgb4_ptp_init(adapter);
+
 	print_adapter_info(adapter);
 	setup_fw_sge_queues(adapter);
 	return 0;
@@ -5024,13 +5241,15 @@ sriov:
 			      &v, &port_vec);
 	if (err < 0) {
 		dev_err(adapter->pdev_dev, "Could not fetch port params\n");
-		goto free_adapter;
+		goto free_mbox_log;
 	}
 
 	adapter->params.nports = hweight32(port_vec);
 	pci_set_drvdata(pdev, adapter);
 	return 0;
 
+free_mbox_log:
+	kfree(adapter->mbox_log);
  free_adapter:
 	kfree(adapter);
  free_pci_region:
@@ -5095,6 +5314,9 @@ static void remove_one(struct pci_dev *pdev)
 
 		debugfs_remove_recursive(adapter->debugfs_root);
 
+		if (!is_t4(adapter->params.chip))
+			cxgb4_ptp_stop(adapter);
+
 		/* If we allocated filters, free up state associated with any
 		 * valid filters ...
 		 */
@@ -5130,6 +5352,7 @@ static void remove_one(struct pci_dev *pdev)
 			unregister_netdev(adapter->port[0]);
 		iounmap(adapter->regs);
 		kfree(adapter->vfinfo);
+		kfree(adapter->mbox_log);
 		kfree(adapter);
 		pci_disable_sriov(pdev);
 		pci_release_regions(pdev);
@@ -5176,6 +5399,7 @@ static void shutdown_one(struct pci_dev *pdev)
 			unregister_netdev(adapter->port[0]);
 		iounmap(adapter->regs);
 		kfree(adapter->vfinfo);
+		kfree(adapter->mbox_log);
 		kfree(adapter);
 		pci_disable_sriov(pdev);
 		pci_release_regions(pdev);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ptp.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ptp.c
new file mode 100644
index 000000000000..50517cfd9671
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ptp.c
@@ -0,0 +1,475 @@
+/*
+ * cxgb4_ptp.c:Chelsio PTP support for T5/T6
+ *
+ * Copyright (c) 2003-2017 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Written by: Atul Gupta (atul.gupta@chelsio.com)
+ */
+
+#include <linux/module.h>
+#include <linux/net_tstamp.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/pps_kernel.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/ptp_classify.h>
+#include <linux/udp.h>
+
+#include "cxgb4.h"
+#include "t4_hw.h"
+#include "t4_regs.h"
+#include "t4_msg.h"
+#include "t4fw_api.h"
+#include "cxgb4_ptp.h"
+
+/**
+ * cxgb4_ptp_is_ptp_tx - determine whether TX packet is PTP or not
+ * @skb: skb of outgoing ptp request
+ *
+ */
+bool cxgb4_ptp_is_ptp_tx(struct sk_buff *skb)
+{
+	struct udphdr *uh;
+
+	uh = udp_hdr(skb);
+	return skb->len >= PTP_MIN_LENGTH &&
+		skb->len <= PTP_IN_TRANSMIT_PACKET_MAXNUM &&
+		likely(skb->protocol == htons(ETH_P_IP)) &&
+		ip_hdr(skb)->protocol == IPPROTO_UDP &&
+		uh->dest == htons(PTP_EVENT_PORT);
+}
+
+bool is_ptp_enabled(struct sk_buff *skb, struct net_device *dev)
+{
+	struct port_info *pi;
+
+	pi = netdev_priv(dev);
+	return (pi->ptp_enable && cxgb4_xmit_with_hwtstamp(skb) &&
+		cxgb4_ptp_is_ptp_tx(skb));
+}
+
+/**
+ * cxgb4_ptp_is_ptp_rx - determine whether RX packet is PTP or not
+ * @skb: skb of incoming ptp request
+ *
+ */
+bool cxgb4_ptp_is_ptp_rx(struct sk_buff *skb)
+{
+	struct udphdr *uh = (struct udphdr *)(skb->data + ETH_HLEN +
+					      IPV4_HLEN(skb->data));
+
+	return  uh->dest == htons(PTP_EVENT_PORT) &&
+		uh->source == htons(PTP_EVENT_PORT);
+}
+
+/**
+ * cxgb4_ptp_read_hwstamp - read timestamp for TX event PTP message
+ * @adapter: board private structure
+ * @pi: port private structure
+ *
+ */
+void cxgb4_ptp_read_hwstamp(struct adapter *adapter, struct port_info *pi)
+{
+	struct skb_shared_hwtstamps *skb_ts = NULL;
+	u64 tx_ts;
+
+	skb_ts = skb_hwtstamps(adapter->ptp_tx_skb);
+
+	tx_ts = t4_read_reg(adapter,
+			    T5_PORT_REG(pi->port_id, MAC_PORT_TX_TS_VAL_LO));
+
+	tx_ts |= (u64)t4_read_reg(adapter,
+				  T5_PORT_REG(pi->port_id,
+					      MAC_PORT_TX_TS_VAL_HI)) << 32;
+	skb_ts->hwtstamp = ns_to_ktime(tx_ts);
+	skb_tstamp_tx(adapter->ptp_tx_skb, skb_ts);
+	dev_kfree_skb_any(adapter->ptp_tx_skb);
+	spin_lock(&adapter->ptp_lock);
+	adapter->ptp_tx_skb = NULL;
+	spin_unlock(&adapter->ptp_lock);
+}
+
+/**
+ * cxgb4_ptprx_timestamping - Enable Timestamp for RX PTP event message
+ * @pi: port private structure
+ * @port: pot number
+ * @mode: RX mode
+ *
+ */
+int cxgb4_ptprx_timestamping(struct port_info *pi, u8 port, u16 mode)
+{
+	struct adapter *adapter = pi->adapter;
+	struct fw_ptp_cmd c;
+	int err;
+
+	memset(&c, 0, sizeof(c));
+	c.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PTP_CMD) |
+				     FW_CMD_REQUEST_F |
+				     FW_CMD_WRITE_F |
+				     FW_PTP_CMD_PORTID_V(port));
+	c.retval_len16 = cpu_to_be32(FW_CMD_LEN16_V(sizeof(c) / 16));
+	c.u.init.sc = FW_PTP_SC_RXTIME_STAMP;
+	c.u.init.mode = cpu_to_be16(mode);
+
+	err = t4_wr_mbox(adapter, adapter->mbox, &c, sizeof(c), NULL);
+	if (err < 0)
+		dev_err(adapter->pdev_dev,
+			"PTP: %s error %d\n", __func__, -err);
+	return err;
+}
+
+int cxgb4_ptp_txtype(struct adapter *adapter, u8 port)
+{
+	struct fw_ptp_cmd c;
+	int err;
+
+	memset(&c, 0, sizeof(c));
+	c.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PTP_CMD) |
+				     FW_CMD_REQUEST_F |
+				     FW_CMD_WRITE_F |
+				     FW_PTP_CMD_PORTID_V(port));
+	c.retval_len16 = cpu_to_be32(FW_CMD_LEN16_V(sizeof(c) / 16));
+	c.u.init.sc = FW_PTP_SC_TX_TYPE;
+	c.u.init.mode = cpu_to_be16(PTP_TS_NONE);
+
+	err = t4_wr_mbox(adapter, adapter->mbox, &c, sizeof(c), NULL);
+	if (err < 0)
+		dev_err(adapter->pdev_dev,
+			"PTP: %s error %d\n", __func__, -err);
+
+	return err;
+}
+
+int cxgb4_ptp_redirect_rx_packet(struct adapter *adapter, struct port_info *pi)
+{
+	struct sge *s = &adapter->sge;
+	struct sge_eth_rxq *receive_q =  &s->ethrxq[pi->first_qset];
+	struct fw_ptp_cmd c;
+	int err;
+
+	memset(&c, 0, sizeof(c));
+	c.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PTP_CMD) |
+				     FW_CMD_REQUEST_F |
+				     FW_CMD_WRITE_F |
+				     FW_PTP_CMD_PORTID_V(pi->port_id));
+
+	c.retval_len16 = cpu_to_be32(FW_CMD_LEN16_V(sizeof(c) / 16));
+	c.u.init.sc = FW_PTP_SC_RDRX_TYPE;
+	c.u.init.txchan = pi->tx_chan;
+	c.u.init.absid = cpu_to_be16(receive_q->rspq.abs_id);
+
+	err = t4_wr_mbox(adapter, adapter->mbox, &c, sizeof(c), NULL);
+	if (err < 0)
+		dev_err(adapter->pdev_dev,
+			"PTP: %s error %d\n", __func__, -err);
+	return err;
+}
+
+/**
+ * @ptp: ptp clock structure
+ * @ppb: Desired frequency change in parts per billion
+ *
+ * Adjust the frequency of the PHC cycle counter by the indicated ppb from
+ * the base frequency.
+ */
+static int cxgb4_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+{
+	struct adapter *adapter = (struct adapter *)container_of(ptp,
+				   struct adapter, ptp_clock_info);
+	struct fw_ptp_cmd c;
+	int err;
+
+	memset(&c, 0, sizeof(c));
+	c.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PTP_CMD) |
+				     FW_CMD_REQUEST_F |
+				     FW_CMD_WRITE_F |
+				     FW_PTP_CMD_PORTID_V(0));
+	c.retval_len16 = cpu_to_be32(FW_CMD_LEN16_V(sizeof(c) / 16));
+	c.u.ts.sc = FW_PTP_SC_ADJ_FREQ;
+	c.u.ts.sign = (ppb < 0) ? 1 : 0;
+	if (ppb < 0)
+		ppb = -ppb;
+	c.u.ts.ppb = cpu_to_be32(ppb);
+
+	err = t4_wr_mbox(adapter, adapter->mbox, &c, sizeof(c), NULL);
+	if (err < 0)
+		dev_err(adapter->pdev_dev,
+			"PTP: %s error %d\n", __func__, -err);
+
+	return err;
+}
+
+/**
+ * cxgb4_ptp_fineadjtime - Shift the time of the hardware clock
+ * @ptp: ptp clock structure
+ * @delta: Desired change in nanoseconds
+ *
+ * Adjust the timer by resetting the timecounter structure.
+ */
+static int  cxgb4_ptp_fineadjtime(struct adapter *adapter, s64 delta)
+{
+	struct fw_ptp_cmd c;
+	int err;
+
+	memset(&c, 0, sizeof(c));
+	c.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PTP_CMD) |
+			     FW_CMD_REQUEST_F |
+			     FW_CMD_WRITE_F |
+			     FW_PTP_CMD_PORTID_V(0));
+	c.retval_len16 = cpu_to_be32(FW_CMD_LEN16_V(sizeof(c) / 16));
+	c.u.ts.sc = FW_PTP_SC_ADJ_FTIME;
+	c.u.ts.tm = cpu_to_be64(delta);
+
+	err = t4_wr_mbox(adapter, adapter->mbox, &c, sizeof(c), NULL);
+	if (err < 0)
+		dev_err(adapter->pdev_dev,
+			"PTP: %s error %d\n", __func__, -err);
+	return err;
+}
+
+/**
+ * cxgb4_ptp_adjtime - Shift the time of the hardware clock
+ * @ptp: ptp clock structure
+ * @delta: Desired change in nanoseconds
+ *
+ * Adjust the timer by resetting the timecounter structure.
+ */
+static int cxgb4_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct adapter *adapter =
+		(struct adapter *)container_of(ptp, struct adapter,
+					       ptp_clock_info);
+	struct fw_ptp_cmd c;
+	s64 sign = 1;
+	int err;
+
+	if (delta < 0)
+		sign = -1;
+
+	if (delta * sign > PTP_CLOCK_MAX_ADJTIME) {
+		memset(&c, 0, sizeof(c));
+		c.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PTP_CMD) |
+					     FW_CMD_REQUEST_F |
+					     FW_CMD_WRITE_F |
+					     FW_PTP_CMD_PORTID_V(0));
+		c.retval_len16 = cpu_to_be32(FW_CMD_LEN16_V(sizeof(c) / 16));
+		c.u.ts.sc = FW_PTP_SC_ADJ_TIME;
+		c.u.ts.sign = (delta < 0) ? 1 : 0;
+		if (delta < 0)
+			delta = -delta;
+		c.u.ts.tm = cpu_to_be64(delta);
+
+		err = t4_wr_mbox(adapter, adapter->mbox, &c, sizeof(c), NULL);
+		if (err < 0)
+			dev_err(adapter->pdev_dev,
+				"PTP: %s error %d\n", __func__, -err);
+	} else {
+		err = cxgb4_ptp_fineadjtime(adapter, delta);
+	}
+
+	return err;
+}
+
+/**
+ * cxgb4_ptp_gettime - Reads the current time from the hardware clock
+ * @ptp: ptp clock structure
+ * @ts: timespec structure to hold the current time value
+ *
+ * Read the timecounter and return the correct value in ns after converting
+ * it into a struct timespec.
+ */
+static int cxgb4_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+	struct adapter *adapter = (struct adapter *)container_of(ptp,
+				   struct adapter, ptp_clock_info);
+	struct fw_ptp_cmd c;
+	u64 ns;
+	int err;
+
+	memset(&c, 0, sizeof(c));
+	c.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PTP_CMD) |
+				     FW_CMD_REQUEST_F |
+				     FW_CMD_READ_F |
+				     FW_PTP_CMD_PORTID_V(0));
+	c.retval_len16 = cpu_to_be32(FW_CMD_LEN16_V(sizeof(c) / 16));
+	c.u.ts.sc = FW_PTP_SC_GET_TIME;
+
+	err = t4_wr_mbox(adapter, adapter->mbox, &c, sizeof(c), &c);
+	if (err < 0) {
+		dev_err(adapter->pdev_dev,
+			"PTP: %s error %d\n", __func__, -err);
+		return err;
+	}
+
+	/* convert to timespec*/
+	ns = be64_to_cpu(c.u.ts.tm);
+	*ts = ns_to_timespec64(ns);
+
+	return err;
+}
+
+/**
+ *  cxgb4_ptp_settime - Set the current time on the hardware clock
+ *  @ptp: ptp clock structure
+ *  @ts: timespec containing the new time for the cycle counter
+ *
+ *  Reset value to new base value instead of the kernel
+ *  wall timer value.
+ */
+static int cxgb4_ptp_settime(struct ptp_clock_info *ptp,
+			     const struct timespec64 *ts)
+{
+	struct adapter *adapter = (struct adapter *)container_of(ptp,
+				   struct adapter, ptp_clock_info);
+	struct fw_ptp_cmd c;
+	u64 ns;
+	int err;
+
+	memset(&c, 0, sizeof(c));
+	c.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PTP_CMD) |
+				     FW_CMD_REQUEST_F |
+				     FW_CMD_WRITE_F |
+				     FW_PTP_CMD_PORTID_V(0));
+	c.retval_len16 = cpu_to_be32(FW_CMD_LEN16_V(sizeof(c) / 16));
+	c.u.ts.sc = FW_PTP_SC_SET_TIME;
+
+	ns = timespec64_to_ns(ts);
+	c.u.ts.tm = cpu_to_be64(ns);
+
+	err =  t4_wr_mbox(adapter, adapter->mbox, &c, sizeof(c), NULL);
+	if (err < 0)
+		dev_err(adapter->pdev_dev,
+			"PTP: %s error %d\n", __func__, -err);
+
+	return err;
+}
+
+static void cxgb4_init_ptp_timer(struct adapter *adapter)
+{
+	struct fw_ptp_cmd c;
+	int err;
+
+	memset(&c, 0, sizeof(c));
+		c.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PTP_CMD) |
+					     FW_CMD_REQUEST_F |
+					     FW_CMD_WRITE_F |
+					     FW_PTP_CMD_PORTID_V(0));
+	c.retval_len16 = cpu_to_be32(FW_CMD_LEN16_V(sizeof(c) / 16));
+	c.u.scmd.sc = FW_PTP_SC_INIT_TIMER;
+
+	err = t4_wr_mbox(adapter, adapter->mbox, &c, sizeof(c), NULL);
+	if (err < 0)
+		dev_err(adapter->pdev_dev,
+			"PTP: %s error %d\n", __func__, -err);
+}
+
+/**
+ * cxgb4_ptp_enable - enable or disable an ancillary feature
+ * @ptp: ptp clock structure
+ * @request: Desired resource to enable or disable
+ * @on: Caller passes one to enable or zero to disable
+ *
+ * Enable (or disable) ancillary features of the PHC subsystem.
+ * Currently, no ancillary features are supported.
+ */
+static int cxgb4_ptp_enable(struct ptp_clock_info __always_unused *ptp,
+			    struct ptp_clock_request __always_unused *request,
+			    int __always_unused on)
+{
+	return -ENOTSUPP;
+}
+
+static const struct ptp_clock_info cxgb4_ptp_clock_info = {
+	.owner          = THIS_MODULE,
+	.name           = "cxgb4_clock",
+	.max_adj        = MAX_PTP_FREQ_ADJ,
+	.n_alarm        = 0,
+	.n_ext_ts       = 0,
+	.n_per_out      = 0,
+	.pps            = 0,
+	.adjfreq        = cxgb4_ptp_adjfreq,
+	.adjtime        = cxgb4_ptp_adjtime,
+	.gettime64      = cxgb4_ptp_gettime,
+	.settime64      = cxgb4_ptp_settime,
+	.enable         = cxgb4_ptp_enable,
+};
+
+/**
+ * cxgb4_ptp_init - initialize PTP for devices which support it
+ * @adapter: board private structure
+ *
+ * This function performs the required steps for enabling PTP support.
+ */
+void cxgb4_ptp_init(struct adapter *adapter)
+{
+	struct timespec64 now;
+	 /* no need to create a clock device if we already have one */
+	if (!IS_ERR_OR_NULL(adapter->ptp_clock))
+		return;
+
+	adapter->ptp_tx_skb = NULL;
+	adapter->ptp_clock_info = cxgb4_ptp_clock_info;
+	spin_lock_init(&adapter->ptp_lock);
+
+	adapter->ptp_clock = ptp_clock_register(&adapter->ptp_clock_info,
+						&adapter->pdev->dev);
+	if (!adapter->ptp_clock) {
+		dev_err(adapter->pdev_dev,
+			"PTP %s Clock registration has failed\n", __func__);
+		return;
+	}
+
+	now = ktime_to_timespec64(ktime_get_real());
+	cxgb4_init_ptp_timer(adapter);
+	if (cxgb4_ptp_settime(&adapter->ptp_clock_info, &now) < 0) {
+		ptp_clock_unregister(adapter->ptp_clock);
+		adapter->ptp_clock = NULL;
+	}
+}
+
+/**
+ * cxgb4_ptp_remove - disable PTP device and stop the overflow check
+ * @adapter: board private structure
+ *
+ * Stop the PTP support.
+ */
+void cxgb4_ptp_stop(struct adapter *adapter)
+{
+	if (adapter->ptp_tx_skb) {
+		dev_kfree_skb_any(adapter->ptp_tx_skb);
+		adapter->ptp_tx_skb = NULL;
+	}
+
+	if (adapter->ptp_clock) {
+		ptp_clock_unregister(adapter->ptp_clock);
+		adapter->ptp_clock = NULL;
+	}
+}
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ptp.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ptp.h
new file mode 100644
index 000000000000..cccfae84bb84
--- /dev/null
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ptp.h
@@ -0,0 +1,74 @@
+/*
+ * This file is part of the Chelsio T4 Ethernet driver for Linux.
+ *
+ * Copyright (c) 2003-2017 Chelsio Communications, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef __CXGB4_PTP_H__
+#define __CXGB4_PTP_H__
+
+/* Maximum parts-per-billion adjustment that is acceptable */
+#define MAX_PTP_FREQ_ADJ		1000000
+#define PTP_CLOCK_MAX_ADJTIME		10000000 /* 10 ms */
+
+#define PTP_MIN_LENGTH			63
+#define PTP_IN_TRANSMIT_PACKET_MAXNUM	240
+#define PTP_EVENT_PORT			319
+
+enum ptp_rx_filter_mode {
+	PTP_TS_NONE = 0,
+	PTP_TS_L2,
+	PTP_TS_L4,
+	PTP_TS_L2_L4
+};
+
+struct port_info;
+
+static inline bool cxgb4_xmit_with_hwtstamp(struct sk_buff *skb)
+{
+	return skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP;
+}
+
+static inline void cxgb4_xmit_hwtstamp_pending(struct sk_buff *skb)
+{
+	skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+}
+
+void cxgb4_ptp_init(struct adapter *adap);
+void cxgb4_ptp_stop(struct adapter *adap);
+bool cxgb4_ptp_is_ptp_tx(struct sk_buff *skb);
+bool cxgb4_ptp_is_ptp_rx(struct sk_buff *skb);
+int cxgb4_ptprx_timestamping(struct port_info *pi, u8 port, u16 mode);
+int cxgb4_ptp_redirect_rx_packet(struct adapter *adap, struct port_info *pi);
+int cxgb4_ptp_txtype(struct adapter *adap, u8 port_id);
+void cxgb4_ptp_read_hwstamp(struct adapter *adap, struct port_info *pi);
+bool is_ptp_enabled(struct sk_buff *skb, struct net_device *dev);
+#endif /* __CXGB4_PTP_H__ */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.c
index d0868c2320da..ec53fe9dec68 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.c
@@ -642,6 +642,7 @@ static void uld_init(struct adapter *adap, struct cxgb4_lld_info *lld)
 	lld->sge_ingpadboundary = adap->sge.fl_align;
 	lld->sge_egrstatuspagesize = adap->sge.stat_len;
 	lld->sge_pktshift = adap->sge.pktshift;
+	lld->ulp_crypto = adap->params.crypto;
 	lld->enable_fw_ofld_conn = adap->flags & FW_OFLD_CONN;
 	lld->max_ordird_qp = adap->params.max_ordird_qp;
 	lld->max_ird_adapter = adap->params.max_ird_adapter;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
index 6e74040af49a..84541fce94c5 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
@@ -123,12 +123,14 @@ struct tid_info {
 
 	spinlock_t stid_lock;
 	unsigned int stids_in_use;
+	unsigned int v6_stids_in_use;
 	unsigned int sftids_in_use;
 
 	/* TIDs in the TCAM */
 	atomic_t tids_in_use;
 	/* TIDs in the HASH */
 	atomic_t hash_tids_in_use;
+	atomic_t conns_in_use;
 	/* lock for setting/clearing filter bitmap */
 	spinlock_t ftid_lock;
 };
@@ -157,13 +159,21 @@ static inline void *lookup_stid(const struct tid_info *t, unsigned int stid)
 }
 
 static inline void cxgb4_insert_tid(struct tid_info *t, void *data,
-				    unsigned int tid)
+				    unsigned int tid, unsigned short family)
 {
 	t->tid_tab[tid] = data;
-	if (t->hash_base && (tid >= t->hash_base))
-		atomic_inc(&t->hash_tids_in_use);
-	else
-		atomic_inc(&t->tids_in_use);
+	if (t->hash_base && (tid >= t->hash_base)) {
+		if (family == AF_INET6)
+			atomic_add(2, &t->hash_tids_in_use);
+		else
+			atomic_inc(&t->hash_tids_in_use);
+	} else {
+		if (family == AF_INET6)
+			atomic_add(2, &t->tids_in_use);
+		else
+			atomic_inc(&t->tids_in_use);
+	}
+	atomic_inc(&t->conns_in_use);
 }
 
 int cxgb4_alloc_atid(struct tid_info *t, void *data);
@@ -171,8 +181,8 @@ int cxgb4_alloc_stid(struct tid_info *t, int family, void *data);
 int cxgb4_alloc_sftid(struct tid_info *t, int family, void *data);
 void cxgb4_free_atid(struct tid_info *t, unsigned int atid);
 void cxgb4_free_stid(struct tid_info *t, unsigned int stid, int family);
-void cxgb4_remove_tid(struct tid_info *t, unsigned int qid, unsigned int tid);
-
+void cxgb4_remove_tid(struct tid_info *t, unsigned int qid, unsigned int tid,
+		      unsigned short family);
 struct in6_addr;
 
 int cxgb4_create_server(const struct net_device *dev, unsigned int stid,
@@ -275,6 +285,15 @@ struct cxgb4_virt_res {                      /* virtualized HW resources */
 	unsigned int ncrypto_fc;
 };
 
+struct chcr_stats_debug {
+	atomic_t cipher_rqst;
+	atomic_t digest_rqst;
+	atomic_t aead_rqst;
+	atomic_t complete;
+	atomic_t error;
+	atomic_t fallback;
+};
+
 #define OCQ_WIN_OFFSET(pdev, vres) \
 	(pci_resource_len((pdev), 2) - roundup_pow_of_two((vres)->ocq.size))
 
@@ -322,6 +341,7 @@ struct cxgb4_lld_info {
 	unsigned int iscsi_tagmask;	     /* iscsi ddp tag mask */
 	unsigned int iscsi_pgsz_order;	     /* iscsi ddp page size orders */
 	unsigned int iscsi_llimit;	     /* chip's iscsi region llimit */
+	unsigned int ulp_crypto;             /* crypto lookaside support */
 	void **iscsi_ppm;		     /* iscsi page pod manager */
 	int nodeid;			     /* device numa node id */
 	bool fr_nsmr_tpte_wr_support;	     /* FW supports FR_NSMR_TPTE_WR */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/l2t.c b/drivers/net/ethernet/chelsio/cxgb4/l2t.c
index 6f3692db29af..f7ef8871dd0b 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/l2t.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/l2t.c
@@ -146,7 +146,7 @@ static int write_l2e(struct adapter *adap, struct l2t_entry *e, int sync)
 	if (!skb)
 		return -ENOMEM;
 
-	req = (struct cpl_l2t_write_req *)__skb_put(skb, sizeof(*req));
+	req = __skb_put(skb, sizeof(*req));
 	INIT_TP_WR(req, 0);
 
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_L2T_WRITE_REQ,
diff --git a/drivers/net/ethernet/chelsio/cxgb4/sge.c b/drivers/net/ethernet/chelsio/cxgb4/sge.c
index f05f0d400324..ede12209f20b 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/sge.c
@@ -52,6 +52,7 @@
 #include "t4_values.h"
 #include "t4_msg.h"
 #include "t4fw_api.h"
+#include "cxgb4_ptp.h"
 
 /*
  * Rx buffer size.  We use largish buffers if possible but settle for single
@@ -1162,7 +1163,7 @@ cxgb_fcoe_offload(struct sk_buff *skb, struct adapter *adap,
  */
 netdev_tx_t t4_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 {
-	u32 wr_mid, ctrl0;
+	u32 wr_mid, ctrl0, op;
 	u64 cntrl, *end;
 	int qidx, credits;
 	unsigned int flits, ndesc;
@@ -1175,6 +1176,7 @@ netdev_tx_t t4_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 	dma_addr_t addr[MAX_SKB_FRAGS + 1];
 	bool immediate = false;
 	int len, max_pkt_len;
+	bool ptp_enabled = is_ptp_enabled(skb, dev);
 #ifdef CONFIG_CHELSIO_T4_FCOE
 	int err;
 #endif /* CONFIG_CHELSIO_T4_FCOE */
@@ -1198,15 +1200,31 @@ out_free:	dev_kfree_skb_any(skb);
 	pi = netdev_priv(dev);
 	adap = pi->adapter;
 	qidx = skb_get_queue_mapping(skb);
-	q = &adap->sge.ethtxq[qidx + pi->first_qset];
+	if (ptp_enabled) {
+		spin_lock(&adap->ptp_lock);
+		if (!(adap->ptp_tx_skb)) {
+			skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+			adap->ptp_tx_skb = skb_get(skb);
+		} else {
+			spin_unlock(&adap->ptp_lock);
+			goto out_free;
+		}
+		q = &adap->sge.ptptxq;
+	} else {
+		q = &adap->sge.ethtxq[qidx + pi->first_qset];
+	}
+	skb_tx_timestamp(skb);
 
 	reclaim_completed_tx(adap, &q->q, true);
 	cntrl = TXPKT_L4CSUM_DIS_F | TXPKT_IPCSUM_DIS_F;
 
 #ifdef CONFIG_CHELSIO_T4_FCOE
 	err = cxgb_fcoe_offload(skb, adap, pi, &cntrl);
-	if (unlikely(err == -ENOTSUPP))
+	if (unlikely(err == -ENOTSUPP)) {
+		if (ptp_enabled)
+			spin_unlock(&adap->ptp_lock);
 		goto out_free;
+	}
 #endif /* CONFIG_CHELSIO_T4_FCOE */
 
 	flits = calc_tx_flits(skb);
@@ -1218,6 +1236,8 @@ out_free:	dev_kfree_skb_any(skb);
 		dev_err(adap->pdev_dev,
 			"%s: Tx ring %u full while queue awake!\n",
 			dev->name, qidx);
+		if (ptp_enabled)
+			spin_unlock(&adap->ptp_lock);
 		return NETDEV_TX_BUSY;
 	}
 
@@ -1227,6 +1247,8 @@ out_free:	dev_kfree_skb_any(skb);
 	if (!immediate &&
 	    unlikely(map_skb(adap->pdev_dev, skb, addr) < 0)) {
 		q->mapping_err++;
+		if (ptp_enabled)
+			spin_unlock(&adap->ptp_lock);
 		goto out_free;
 	}
 
@@ -1279,7 +1301,11 @@ out_free:	dev_kfree_skb_any(skb);
 		q->tx_cso += ssi->gso_segs;
 	} else {
 		len += sizeof(*cpl);
-		wr->op_immdlen = htonl(FW_WR_OP_V(FW_ETH_TX_PKT_WR) |
+		if (ptp_enabled)
+			op = FW_PTP_TX_PKT_WR;
+		else
+			op = FW_ETH_TX_PKT_WR;
+		wr->op_immdlen = htonl(FW_WR_OP_V(op) |
 				       FW_WR_IMMDLEN_V(len));
 		cpl = (void *)(wr + 1);
 		if (skb->ip_summed == CHECKSUM_PARTIAL) {
@@ -1301,6 +1327,8 @@ out_free:	dev_kfree_skb_any(skb);
 
 	ctrl0 = TXPKT_OPCODE_V(CPL_TX_PKT_XT) | TXPKT_INTF_V(pi->tx_chan) |
 		TXPKT_PF_V(adap->pf);
+	if (ptp_enabled)
+		ctrl0 |= TXPKT_TSTAMP_F;
 #ifdef CONFIG_CHELSIO_T4_DCB
 	if (is_t4(adap->params.chip))
 		ctrl0 |= TXPKT_OVLAN_IDX_V(q->dcb_prio);
@@ -1332,6 +1360,8 @@ out_free:	dev_kfree_skb_any(skb);
 	txq_advance(&q->q, ndesc);
 
 	ring_tx_db(adap, &q->q, ndesc);
+	if (ptp_enabled)
+		spin_unlock(&adap->ptp_lock);
 	return NETDEV_TX_OK;
 }
 
@@ -2023,6 +2053,92 @@ static void do_gro(struct sge_eth_rxq *rxq, const struct pkt_gl *gl,
 	rxq->stats.rx_cso++;
 }
 
+enum {
+	RX_NON_PTP_PKT = 0,
+	RX_PTP_PKT_SUC = 1,
+	RX_PTP_PKT_ERR = 2
+};
+
+/**
+ *     t4_systim_to_hwstamp - read hardware time stamp
+ *     @adap: the adapter
+ *     @skb: the packet
+ *
+ *     Read Time Stamp from MPS packet and insert in skb which
+ *     is forwarded to PTP application
+ */
+static noinline int t4_systim_to_hwstamp(struct adapter *adapter,
+					 struct sk_buff *skb)
+{
+	struct skb_shared_hwtstamps *hwtstamps;
+	struct cpl_rx_mps_pkt *cpl = NULL;
+	unsigned char *data;
+	int offset;
+
+	cpl = (struct cpl_rx_mps_pkt *)skb->data;
+	if (!(CPL_RX_MPS_PKT_TYPE_G(ntohl(cpl->op_to_r1_hi)) &
+	     X_CPL_RX_MPS_PKT_TYPE_PTP))
+		return RX_PTP_PKT_ERR;
+
+	data = skb->data + sizeof(*cpl);
+	skb_pull(skb, 2 * sizeof(u64) + sizeof(struct cpl_rx_mps_pkt));
+	offset = ETH_HLEN + IPV4_HLEN(skb->data) + UDP_HLEN;
+	if (skb->len < offset + OFF_PTP_SEQUENCE_ID + sizeof(short))
+		return RX_PTP_PKT_ERR;
+
+	hwtstamps = skb_hwtstamps(skb);
+	memset(hwtstamps, 0, sizeof(*hwtstamps));
+	hwtstamps->hwtstamp = ns_to_ktime(be64_to_cpu(*((u64 *)data)));
+
+	return RX_PTP_PKT_SUC;
+}
+
+/**
+ *     t4_rx_hststamp - Recv PTP Event Message
+ *     @adap: the adapter
+ *     @rsp: the response queue descriptor holding the RX_PKT message
+ *     @skb: the packet
+ *
+ *     PTP enabled and MPS packet, read HW timestamp
+ */
+static int t4_rx_hststamp(struct adapter *adapter, const __be64 *rsp,
+			  struct sge_eth_rxq *rxq, struct sk_buff *skb)
+{
+	int ret;
+
+	if (unlikely((*(u8 *)rsp == CPL_RX_MPS_PKT) &&
+		     !is_t4(adapter->params.chip))) {
+		ret = t4_systim_to_hwstamp(adapter, skb);
+		if (ret == RX_PTP_PKT_ERR) {
+			kfree_skb(skb);
+			rxq->stats.rx_drops++;
+		}
+		return ret;
+	}
+	return RX_NON_PTP_PKT;
+}
+
+/**
+ *      t4_tx_hststamp - Loopback PTP Transmit Event Message
+ *      @adap: the adapter
+ *      @skb: the packet
+ *      @dev: the ingress net device
+ *
+ *      Read hardware timestamp for the loopback PTP Tx event message
+ */
+static int t4_tx_hststamp(struct adapter *adapter, struct sk_buff *skb,
+			  struct net_device *dev)
+{
+	struct port_info *pi = netdev_priv(dev);
+
+	if (!is_t4(adapter->params.chip) && adapter->ptp_tx_skb) {
+		cxgb4_ptp_read_hwstamp(adapter, pi);
+		kfree_skb(skb);
+		return 0;
+	}
+	return 1;
+}
+
 /**
  *	t4_ethrx_handler - process an ingress ethernet packet
  *	@q: the response queue that received the packet
@@ -2038,11 +2154,13 @@ int t4_ethrx_handler(struct sge_rspq *q, const __be64 *rsp,
 	struct sk_buff *skb;
 	const struct cpl_rx_pkt *pkt;
 	struct sge_eth_rxq *rxq = container_of(q, struct sge_eth_rxq, rspq);
+	struct adapter *adapter = q->adap;
 	struct sge *s = &q->adap->sge;
 	int cpl_trace_pkt = is_t4(q->adap->params.chip) ?
 			    CPL_TRACE_PKT : CPL_TRACE_PKT_T5;
 	u16 err_vec;
 	struct port_info *pi;
+	int ret = 0;
 
 	if (unlikely(*(u8 *)rsp == cpl_trace_pkt))
 		return handle_trace_pkt(q->adap, si);
@@ -2068,8 +2186,25 @@ int t4_ethrx_handler(struct sge_rspq *q, const __be64 *rsp,
 		rxq->stats.rx_drops++;
 		return 0;
 	}
+	pi = netdev_priv(q->netdev);
+
+	/* Handle PTP Event Rx packet */
+	if (unlikely(pi->ptp_enable)) {
+		ret = t4_rx_hststamp(adapter, rsp, rxq, skb);
+		if (ret == RX_PTP_PKT_ERR)
+			return 0;
+	}
+	if (likely(!ret))
+		__skb_pull(skb, s->pktshift); /* remove ethernet header pad */
+
+	/* Handle the PTP Event Tx Loopback packet */
+	if (unlikely(pi->ptp_enable && !ret &&
+		     (pkt->l2info & htonl(RXF_UDP_F)) &&
+		     cxgb4_ptp_is_ptp_rx(skb))) {
+		if (!t4_tx_hststamp(adapter, skb, q->netdev))
+			return 0;
+	}
 
-	__skb_pull(skb, s->pktshift);      /* remove ethernet header padding */
 	skb->protocol = eth_type_trans(skb, q->netdev);
 	skb_record_rx_queue(skb, q->idx);
 	if (skb->dev->features & NETIF_F_RXHASH)
@@ -2078,7 +2213,6 @@ int t4_ethrx_handler(struct sge_rspq *q, const __be64 *rsp,
 
 	rxq->stats.pkts++;
 
-	pi = netdev_priv(skb->dev);
 	if (pi->rxtstamp)
 		cxgb4_sgetim_to_hwtstamp(q->adap, skb_hwtstamps(skb),
 					 si->sgetstamp);
@@ -2502,6 +2636,20 @@ static void sge_tx_timer_cb(unsigned long data)
 			tasklet_schedule(&txq->qresume_tsk);
 		}
 
+	if (!is_t4(adap->params.chip)) {
+		struct sge_eth_txq *q = &s->ptptxq;
+		int avail;
+
+		spin_lock(&adap->ptp_lock);
+		avail = reclaimable(&q->q);
+
+		if (avail) {
+			free_tx_desc(adap, &q->q, avail, false);
+			q->q.in_use -= avail;
+		}
+		spin_unlock(&adap->ptp_lock);
+	}
+
 	budget = MAX_TIMER_TX_RECLAIM;
 	i = s->ethtxq_rover;
 	do {
@@ -3068,6 +3216,19 @@ void t4_free_sge_resources(struct adapter *adap)
 	if (adap->sge.intrq.desc)
 		free_rspq_fl(adap, &adap->sge.intrq, NULL);
 
+	if (!is_t4(adap->params.chip)) {
+		etq = &adap->sge.ptptxq;
+		if (etq->q.desc) {
+			t4_eth_eq_free(adap, adap->mbox, adap->pf, 0,
+				       etq->q.cntxt_id);
+			spin_lock_bh(&adap->ptp_lock);
+			free_tx_desc(adap, &etq->q, etq->q.in_use, true);
+			spin_unlock_bh(&adap->ptp_lock);
+			kfree(etq->q.sdesc);
+			free_txq(adap, &etq->q);
+		}
+	}
+
 	/* clear the reverse egress queue map */
 	memset(adap->sge.egr_map, 0,
 	       adap->sge.egr_sz * sizeof(*adap->sge.egr_map));
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index 3a34aa629f7d..82bf7aac6cdb 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -3540,7 +3540,7 @@ int t4_load_phy_fw(struct adapter *adap,
 		 FW_PARAMS_PARAM_Z_V(FW_PARAMS_PARAM_DEV_PHYFW_DOWNLOAD));
 	val = phy_fw_size;
 	ret = t4_query_params_rw(adap, adap->mbox, adap->pf, 0, 1,
-				 &param, &val, 1);
+				 &param, &val, 1, true);
 	if (ret < 0)
 		return ret;
 	mtype = val >> 8;
@@ -4040,6 +4040,7 @@ static void cim_intr_handler(struct adapter *adapter)
 		{ MBHOSTPARERR_F, "CIM mailbox host parity error", -1, 1 },
 		{ TIEQINPARERRINT_F, "CIM TIEQ outgoing parity error", -1, 1 },
 		{ TIEQOUTPARERRINT_F, "CIM TIEQ incoming parity error", -1, 1 },
+		{ TIMER0INT_F, "CIM TIMER0 interrupt", -1, 1 },
 		{ 0 }
 	};
 	static const struct intr_info cim_upintr_info[] = {
@@ -4074,11 +4075,27 @@ static void cim_intr_handler(struct adapter *adapter)
 		{ 0 }
 	};
 
+	u32 val, fw_err;
 	int fat;
 
-	if (t4_read_reg(adapter, PCIE_FW_A) & PCIE_FW_ERR_F)
+	fw_err = t4_read_reg(adapter, PCIE_FW_A);
+	if (fw_err & PCIE_FW_ERR_F)
 		t4_report_fw_error(adapter);
 
+	/* When the Firmware detects an internal error which normally
+	 * wouldn't raise a Host Interrupt, it forces a CIM Timer0 interrupt
+	 * in order to make sure the Host sees the Firmware Crash.  So
+	 * if we have a Timer0 interrupt and don't see a Firmware Crash,
+	 * ignore the Timer0 interrupt.
+	 */
+
+	val = t4_read_reg(adapter, CIM_HOST_INT_CAUSE_A);
+	if (val & TIMER0INT_F)
+		if (!(fw_err & PCIE_FW_ERR_F) ||
+		    (PCIE_FW_EVAL_G(fw_err) != PCIE_FW_EVAL_CRASH))
+			t4_write_reg(adapter, CIM_HOST_INT_CAUSE_A,
+				     TIMER0INT_F);
+
 	fat = t4_handle_intr_status(adapter, CIM_HOST_INT_CAUSE_A,
 				    cim_intr_info) +
 	      t4_handle_intr_status(adapter, CIM_HOST_UPACC_INT_CAUSE_A,
@@ -4445,7 +4462,7 @@ static void pl_intr_handler(struct adapter *adap)
 #define PF_INTR_MASK (PFSW_F)
 #define GLBL_INTR_MASK (CIM_F | MPS_F | PL_F | PCIE_F | MC_F | EDC0_F | \
 		EDC1_F | LE_F | TP_F | MA_F | PM_TX_F | PM_RX_F | ULP_RX_F | \
-		CPL_SWITCH_F | SGE_F | ULP_TX_F)
+		CPL_SWITCH_F | SGE_F | ULP_TX_F | SF_F)
 
 /**
  *	t4_slow_intr_handler - control path interrupt handler
@@ -5423,30 +5440,155 @@ void t4_pmrx_get_stats(struct adapter *adap, u32 cnt[], u64 cycles[])
 }
 
 /**
- *	t4_get_mps_bg_map - return the buffer groups associated with a port
+ *	compute_mps_bg_map - compute the MPS Buffer Group Map for a Port
  *	@adap: the adapter
- *	@idx: the port index
+ *	@pidx: the port index
+ *
+ *	Computes and returns a bitmap indicating which MPS buffer groups are
+ *	associated with the given Port.  Bit i is set if buffer group i is
+ *	used by the Port.
+ */
+static inline unsigned int compute_mps_bg_map(struct adapter *adapter,
+					      int pidx)
+{
+	unsigned int chip_version, nports;
+
+	chip_version = CHELSIO_CHIP_VERSION(adapter->params.chip);
+	nports = 1 << NUMPORTS_G(t4_read_reg(adapter, MPS_CMN_CTL_A));
+
+	switch (chip_version) {
+	case CHELSIO_T4:
+	case CHELSIO_T5:
+		switch (nports) {
+		case 1: return 0xf;
+		case 2: return 3 << (2 * pidx);
+		case 4: return 1 << pidx;
+		}
+		break;
+
+	case CHELSIO_T6:
+		switch (nports) {
+		case 2: return 1 << (2 * pidx);
+		}
+		break;
+	}
+
+	dev_err(adapter->pdev_dev, "Need MPS Buffer Group Map for Chip %0x, Nports %d\n",
+		chip_version, nports);
+
+	return 0;
+}
+
+/**
+ *	t4_get_mps_bg_map - return the buffer groups associated with a port
+ *	@adapter: the adapter
+ *	@pidx: the port index
  *
  *	Returns a bitmap indicating which MPS buffer groups are associated
- *	with the given port.  Bit i is set if buffer group i is used by the
- *	port.
+ *	with the given Port.  Bit i is set if buffer group i is used by the
+ *	Port.
  */
-unsigned int t4_get_mps_bg_map(struct adapter *adap, int idx)
+unsigned int t4_get_mps_bg_map(struct adapter *adapter, int pidx)
 {
-	u32 n = NUMPORTS_G(t4_read_reg(adap, MPS_CMN_CTL_A));
+	u8 *mps_bg_map;
+	unsigned int nports;
+
+	nports = 1 << NUMPORTS_G(t4_read_reg(adapter, MPS_CMN_CTL_A));
+	if (pidx >= nports) {
+		CH_WARN(adapter, "MPS Port Index %d >= Nports %d\n",
+			pidx, nports);
+		return 0;
+	}
+
+	/* If we've already retrieved/computed this, just return the result.
+	 */
+	mps_bg_map = adapter->params.mps_bg_map;
+	if (mps_bg_map[pidx])
+		return mps_bg_map[pidx];
+
+	/* Newer Firmware can tell us what the MPS Buffer Group Map is.
+	 * If we're talking to such Firmware, let it tell us.  If the new
+	 * API isn't supported, revert back to old hardcoded way.  The value
+	 * obtained from Firmware is encoded in below format:
+	 *
+	 * val = (( MPSBGMAP[Port 3] << 24 ) |
+	 *        ( MPSBGMAP[Port 2] << 16 ) |
+	 *        ( MPSBGMAP[Port 1] <<  8 ) |
+	 *        ( MPSBGMAP[Port 0] <<  0 ))
+	 */
+	if (adapter->flags & FW_OK) {
+		u32 param, val;
+		int ret;
+
+		param = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DEV) |
+			 FW_PARAMS_PARAM_X_V(FW_PARAMS_PARAM_DEV_MPSBGMAP));
+		ret = t4_query_params_ns(adapter, adapter->mbox, adapter->pf,
+					 0, 1, &param, &val);
+		if (!ret) {
+			int p;
+
+			/* Store the BG Map for all of the Ports in order to
+			 * avoid more calls to the Firmware in the future.
+			 */
+			for (p = 0; p < MAX_NPORTS; p++, val >>= 8)
+				mps_bg_map[p] = val & 0xff;
+
+			return mps_bg_map[pidx];
+		}
+	}
 
-	if (n == 0)
-		return idx == 0 ? 0xf : 0;
-	/* In T6 (which is a 2 port card),
-	 * port 0 is mapped to channel 0 and port 1 is mapped to channel 1.
-	 * For 2 port T4/T5 adapter,
-	 * port 0 is mapped to channel 0 and 1,
-	 * port 1 is mapped to channel 2 and 3.
+	/* Either we're not talking to the Firmware or we're dealing with
+	 * older Firmware which doesn't support the new API to get the MPS
+	 * Buffer Group Map.  Fall back to computing it ourselves.
 	 */
-	if ((n == 1) &&
-	    (CHELSIO_CHIP_VERSION(adap->params.chip) <= CHELSIO_T5))
-		return idx < 2 ? (3 << (2 * idx)) : 0;
-	return 1 << idx;
+	mps_bg_map[pidx] = compute_mps_bg_map(adapter, pidx);
+	return mps_bg_map[pidx];
+}
+
+/**
+ *	t4_get_tp_ch_map - return TP ingress channels associated with a port
+ *	@adapter: the adapter
+ *	@pidx: the port index
+ *
+ *	Returns a bitmap indicating which TP Ingress Channels are associated
+ *	with a given Port.  Bit i is set if TP Ingress Channel i is used by
+ *	the Port.
+ */
+unsigned int t4_get_tp_ch_map(struct adapter *adap, int pidx)
+{
+	unsigned int chip_version = CHELSIO_CHIP_VERSION(adap->params.chip);
+	unsigned int nports = 1 << NUMPORTS_G(t4_read_reg(adap, MPS_CMN_CTL_A));
+
+	if (pidx >= nports) {
+		dev_warn(adap->pdev_dev, "TP Port Index %d >= Nports %d\n",
+			 pidx, nports);
+		return 0;
+	}
+
+	switch (chip_version) {
+	case CHELSIO_T4:
+	case CHELSIO_T5:
+		/* Note that this happens to be the same values as the MPS
+		 * Buffer Group Map for these Chips.  But we replicate the code
+		 * here because they're really separate concepts.
+		 */
+		switch (nports) {
+		case 1: return 0xf;
+		case 2: return 3 << (2 * pidx);
+		case 4: return 1 << pidx;
+		}
+		break;
+
+	case CHELSIO_T6:
+		switch (nports) {
+		case 2: return 1 << pidx;
+		}
+		break;
+	}
+
+	dev_err(adap->pdev_dev, "Need TP Channel Map for Chip %0x, Nports %d\n",
+		chip_version, nports);
+	return 0;
 }
 
 /**
@@ -6293,13 +6435,18 @@ int t4_fw_upgrade(struct adapter *adap, unsigned int mbox,
 	if (!t4_fw_matches_chip(adap, fw_hdr))
 		return -EINVAL;
 
+	/* Disable FW_OK flag so that mbox commands with FW_OK flag set
+	 * wont be sent when we are flashing FW.
+	 */
+	adap->flags &= ~FW_OK;
+
 	ret = t4_fw_halt(adap, mbox, force);
 	if (ret < 0 && !force)
-		return ret;
+		goto out;
 
 	ret = t4_load_fw(adap, fw_data, size);
 	if (ret < 0)
-		return ret;
+		goto out;
 
 	/*
 	 * Older versions of the firmware don't understand the new
@@ -6310,7 +6457,17 @@ int t4_fw_upgrade(struct adapter *adap, unsigned int mbox,
 	 * its header flags to see if it advertises the capability.
 	 */
 	reset = ((be32_to_cpu(fw_hdr->flags) & FW_HDR_FLAGS_RESET_HALT) == 0);
-	return t4_fw_restart(adap, mbox, reset);
+	ret = t4_fw_restart(adap, mbox, reset);
+
+	/* Grab potentially new Firmware Device Log parameters so we can see
+	 * how healthy the new Firmware is.  It's okay to contact the new
+	 * Firmware for these parameters even though, as far as it's
+	 * concerned, we've never said "HELLO" to it ...
+	 */
+	(void)t4_init_devlog_params(adap);
+out:
+	adap->flags |= FW_OK;
+	return ret;
 }
 
 /**
@@ -6546,13 +6703,14 @@ int t4_fw_initialize(struct adapter *adap, unsigned int mbox)
  *	@params: the parameter names
  *	@val: the parameter values
  *	@rw: Write and read flag
+ *	@sleep_ok: if true, we may sleep awaiting mbox cmd completion
  *
  *	Reads the value of FW or device parameters.  Up to 7 parameters can be
  *	queried at once.
  */
 int t4_query_params_rw(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		       unsigned int vf, unsigned int nparams, const u32 *params,
-		       u32 *val, int rw)
+		       u32 *val, int rw, bool sleep_ok)
 {
 	int i, ret;
 	struct fw_params_cmd c;
@@ -6575,7 +6733,7 @@ int t4_query_params_rw(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		p++;
 	}
 
-	ret = t4_wr_mbox(adap, mbox, &c, sizeof(c), &c);
+	ret = t4_wr_mbox_meat(adap, mbox, &c, sizeof(c), &c, sleep_ok);
 	if (ret == 0)
 		for (i = 0, p = &c.param[0].val; i < nparams; i++, p += 2)
 			*val++ = be32_to_cpu(*p);
@@ -6586,7 +6744,16 @@ int t4_query_params(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		    unsigned int vf, unsigned int nparams, const u32 *params,
 		    u32 *val)
 {
-	return t4_query_params_rw(adap, mbox, pf, vf, nparams, params, val, 0);
+	return t4_query_params_rw(adap, mbox, pf, vf, nparams, params, val, 0,
+				  true);
+}
+
+int t4_query_params_ns(struct adapter *adap, unsigned int mbox, unsigned int pf,
+		       unsigned int vf, unsigned int nparams, const u32 *params,
+		       u32 *val)
+{
+	return t4_query_params_rw(adap, mbox, pf, vf, nparams, params, val, 0,
+				  false);
 }
 
 /**
@@ -7360,11 +7527,41 @@ void t4_handle_get_port_info(struct port_info *pi, const __be64 *rpl)
 		lc->fc = fc;
 		lc->supported = be16_to_cpu(p->u.info.pcap);
 		lc->lp_advertising = be16_to_cpu(p->u.info.lpacap);
+
 		t4_os_link_changed(adap, pi->port_id, link_ok);
 	}
 }
 
 /**
+ *	t4_update_port_info - retrieve and update port information if changed
+ *	@pi: the port_info
+ *
+ *	We issue a Get Port Information Command to the Firmware and, if
+ *	successful, we check to see if anything is different from what we
+ *	last recorded and update things accordingly.
+ */
+int t4_update_port_info(struct port_info *pi)
+{
+	struct fw_port_cmd port_cmd;
+	int ret;
+
+	memset(&port_cmd, 0, sizeof(port_cmd));
+	port_cmd.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PORT_CMD) |
+					    FW_CMD_REQUEST_F | FW_CMD_READ_F |
+					    FW_PORT_CMD_PORTID_V(pi->port_id));
+	port_cmd.action_to_len16 = cpu_to_be32(
+		FW_PORT_CMD_ACTION_V(FW_PORT_ACTION_GET_PORT_INFO) |
+		FW_LEN16(port_cmd));
+	ret = t4_wr_mbox(pi->adapter, pi->adapter->mbox,
+			 &port_cmd, sizeof(port_cmd), &port_cmd);
+	if (ret)
+		return ret;
+
+	t4_handle_get_port_info(pi, (__be64 *)&port_cmd);
+	return 0;
+}
+
+/**
  *      t4_handle_fw_rpl - process a FW reply message
  *      @adap: the adapter
  *      @rpl: start of the FW message
@@ -7643,10 +7840,9 @@ int t4_shutdown_adapter(struct adapter *adapter)
 	t4_intr_disable(adapter);
 	t4_write_reg(adapter, DBG_GPIO_EN_A, 0);
 	for_each_port(adapter, port) {
-		u32 a_port_cfg = PORT_REG(port,
-					  is_t4(adapter->params.chip)
-					  ? XGMAC_PORT_CFG_A
-					  : MAC_PORT_CFG_A);
+		u32 a_port_cfg = is_t4(adapter->params.chip) ?
+				       PORT_REG(port, XGMAC_PORT_CFG_A) :
+				       T5_PORT_REG(port, MAC_PORT_CFG_A);
 
 		t4_write_reg(adapter, a_port_cfg,
 			     t4_read_reg(adapter, a_port_cfg)
@@ -8272,7 +8468,16 @@ int t4_cim_read_la(struct adapter *adap, u32 *la_buf, unsigned int *wrptr)
 		ret = t4_cim_read(adap, UP_UP_DBG_LA_DATA_A, 1, &la_buf[i]);
 		if (ret)
 			break;
-		idx = (idx + 1) & UPDBGLARDPTR_M;
+
+		/* Bits 0-3 of UpDbgLaRdPtr can be between 0000 to 1001 to
+		 * identify the 32-bit portion of the full 312-bit data
+		 */
+		if (is_t6(adap->params.chip) && (idx & 0xf) >= 9)
+			idx = (idx & 0xff0) + 0x10;
+		else
+			idx++;
+		/* address can't exceed 0xfff */
+		idx &= UPDBGLARDPTR_M;
 	}
 restart:
 	if (cfg & UPDBGLAEN_F) {
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h b/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
index 8098c93cd16e..b0ff78da8aa2 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
@@ -92,6 +92,7 @@ enum {
 	CPL_RDMA_TERMINATE    = 0xA2,
 	CPL_RDMA_WRITE        = 0xA4,
 	CPL_SGE_EGR_UPDATE    = 0xA5,
+	CPL_RX_MPS_PKT        = 0xAF,
 
 	CPL_TRACE_PKT         = 0xB0,
 	CPL_ISCSI_DATA	      = 0xB2,
@@ -807,6 +808,10 @@ struct cpl_tx_pkt {
 #define TXPKT_INS_OVLAN_V(x) ((x) << TXPKT_INS_OVLAN_S)
 #define TXPKT_INS_OVLAN_F    TXPKT_INS_OVLAN_V(1U)
 
+#define TXPKT_TSTAMP_S    23
+#define TXPKT_TSTAMP_V(x) ((x) << TXPKT_TSTAMP_S)
+#define TXPKT_TSTAMP_F    TXPKT_TSTAMP_V(1ULL)
+
 #define TXPKT_OPCODE_S    24
 #define TXPKT_OPCODE_V(x) ((x) << TXPKT_OPCODE_S)
 
@@ -1875,4 +1880,27 @@ struct cpl_rx_phys_dsgl {
 	(((x) >> CPL_RX_PHYS_DSGL_NOOFSGENTR_S) & \
 	 CPL_RX_PHYS_DSGL_NOOFSGENTR_M)
 
+struct cpl_rx_mps_pkt {
+	__be32 op_to_r1_hi;
+	__be32 r1_lo_length;
+};
+
+#define CPL_RX_MPS_PKT_OP_S     24
+#define CPL_RX_MPS_PKT_OP_M     0xff
+#define CPL_RX_MPS_PKT_OP_V(x)  ((x) << CPL_RX_MPS_PKT_OP_S)
+#define CPL_RX_MPS_PKT_OP_G(x)  \
+	(((x) >> CPL_RX_MPS_PKT_OP_S) & CPL_RX_MPS_PKT_OP_M)
+
+#define CPL_RX_MPS_PKT_TYPE_S           20
+#define CPL_RX_MPS_PKT_TYPE_M           0xf
+#define CPL_RX_MPS_PKT_TYPE_V(x)        ((x) << CPL_RX_MPS_PKT_TYPE_S)
+#define CPL_RX_MPS_PKT_TYPE_G(x)        \
+	(((x) >> CPL_RX_MPS_PKT_TYPE_S) & CPL_RX_MPS_PKT_TYPE_M)
+
+enum {
+	X_CPL_RX_MPS_PKT_TYPE_PAUSE = 1 << 0,
+	X_CPL_RX_MPS_PKT_TYPE_PPP   = 1 << 1,
+	X_CPL_RX_MPS_PKT_TYPE_QFC   = 1 << 2,
+	X_CPL_RX_MPS_PKT_TYPE_PTP   = 1 << 3
+};
 #endif  /* __T4_MSG_H */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h b/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h
index a323185507ec..99987d8e437e 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h
@@ -172,6 +172,8 @@ CH_PCI_DEVICE_ID_TABLE_DEFINE_BEGIN
 	CH_PCI_ID_TABLE_FENTRY(0x509e), /* Custom T520-CR */
 	CH_PCI_ID_TABLE_FENTRY(0x509f), /* Custom T540-CR */
 	CH_PCI_ID_TABLE_FENTRY(0x50a0), /* Custom T540-CR */
+	CH_PCI_ID_TABLE_FENTRY(0x50a1), /* Custom T540-CR */
+	CH_PCI_ID_TABLE_FENTRY(0x50a2), /* Custom T540-KR4 */
 
 	/* T6 adapters:
 	 */
@@ -190,6 +192,9 @@ CH_PCI_DEVICE_ID_TABLE_DEFINE_BEGIN
 	CH_PCI_ID_TABLE_FENTRY(0x6015),
 	CH_PCI_ID_TABLE_FENTRY(0x6080),
 	CH_PCI_ID_TABLE_FENTRY(0x6081),
+	CH_PCI_ID_TABLE_FENTRY(0x6082), /* Custom T6225-CR SFP28 */
+	CH_PCI_ID_TABLE_FENTRY(0x6083), /* Custom T62100-CR QSFP28 */
+	CH_PCI_ID_TABLE_FENTRY(0x6084), /* Custom T64100-CR QSFP28 */
 CH_PCI_DEVICE_ID_TABLE_DEFINE_END;
 
 #endif /* __T4_PCI_ID_TBL_H__ */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
index 3348d33c36fa..dac90837842b 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
@@ -1077,6 +1077,10 @@
 #define TIEQINPARERRINT_V(x) ((x) << TIEQINPARERRINT_S)
 #define TIEQINPARERRINT_F    TIEQINPARERRINT_V(1U)
 
+#define TIMER0INT_S    2
+#define TIMER0INT_V(x) ((x) << TIMER0INT_S)
+#define TIMER0INT_F    TIMER0INT_V(1U)
+
 #define PREFDROPINT_S    1
 #define PREFDROPINT_V(x) ((x) << PREFDROPINT_S)
 #define PREFDROPINT_F    PREFDROPINT_V(1U)
@@ -1795,6 +1799,8 @@
 #define MPS_PORT_STAT_RX_PORT_LESS_64B_H 0x614
 #define MAC_PORT_MAGIC_MACID_LO 0x824
 #define MAC_PORT_MAGIC_MACID_HI 0x828
+#define MAC_PORT_TX_TS_VAL_LO   0x928
+#define MAC_PORT_TX_TS_VAL_HI   0x92c
 
 #define MAC_PORT_EPIO_DATA0_A 0x8c0
 #define MAC_PORT_EPIO_DATA1_A 0x8c4
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
index 251a35e9795c..0ebed64d62d3 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
@@ -103,6 +103,7 @@ enum fw_wr_opcodes {
 	FW_RI_FR_NSMR_TPTE_WR	       = 0x20,
 	FW_RI_INV_LSTAG_WR             = 0x1a,
 	FW_ISCSI_TX_DATA_WR	       = 0x45,
+	FW_PTP_TX_PKT_WR               = 0x46,
 	FW_CRYPTO_LOOKASIDE_WR         = 0X6d,
 	FW_LASTC2E_WR                  = 0x70
 };
@@ -685,6 +686,7 @@ enum fw_cmd_opcodes {
 	FW_SCHED_CMD                   = 0x24,
 	FW_DEVLOG_CMD                  = 0x25,
 	FW_CLIP_CMD                    = 0x28,
+	FW_PTP_CMD                     = 0x3e,
 	FW_LASTC2E_CMD                 = 0x40,
 	FW_ERROR_CMD                   = 0x80,
 	FW_DEBUG_CMD                   = 0x81,
@@ -1123,6 +1125,7 @@ enum fw_params_param_dev {
 	FW_PARAMS_PARAM_DEV_ULPTX_MEMWRITE_DSGL = 0x17,
 	FW_PARAMS_PARAM_DEV_FWCACHE = 0x18,
 	FW_PARAMS_PARAM_DEV_RI_FR_NSMR_TPTE_WR	= 0x1C,
+	FW_PARAMS_PARAM_DEV_MPSBGMAP	= 0x1E,
 };
 
 /*
@@ -2572,6 +2575,7 @@ enum fw_port_type {
 	FW_PORT_TYPE_CR_QSFP,
 	FW_PORT_TYPE_CR2_QSFP,
 	FW_PORT_TYPE_SFP28,
+	FW_PORT_TYPE_KR_SFP28,
 
 	FW_PORT_TYPE_NONE = FW_PORT_CMD_PTYPE_M
 };
@@ -2800,6 +2804,54 @@ struct fw_port_lb_stats_cmd {
 	} u;
 };
 
+enum fw_ptp_subop {
+	/* none */
+	FW_PTP_SC_INIT_TIMER            = 0x00,
+	FW_PTP_SC_TX_TYPE               = 0x01,
+	/* init */
+	FW_PTP_SC_RXTIME_STAMP          = 0x08,
+	FW_PTP_SC_RDRX_TYPE             = 0x09,
+	/* ts */
+	FW_PTP_SC_ADJ_FREQ              = 0x10,
+	FW_PTP_SC_ADJ_TIME              = 0x11,
+	FW_PTP_SC_ADJ_FTIME             = 0x12,
+	FW_PTP_SC_WALL_CLOCK            = 0x13,
+	FW_PTP_SC_GET_TIME              = 0x14,
+	FW_PTP_SC_SET_TIME              = 0x15,
+};
+
+struct fw_ptp_cmd {
+	__be32 op_to_portid;
+	__be32 retval_len16;
+	union fw_ptp {
+		struct fw_ptp_sc {
+			__u8   sc;
+			__u8   r3[7];
+		} scmd;
+		struct fw_ptp_init {
+			__u8   sc;
+			__u8   txchan;
+			__be16 absid;
+			__be16 mode;
+			__be16 r3;
+		} init;
+		struct fw_ptp_ts {
+			__u8   sc;
+			__u8   sign;
+			__be16 r3;
+			__be32 ppb;
+			__be64 tm;
+		} ts;
+	} u;
+	__be64 r3;
+};
+
+#define FW_PTP_CMD_PORTID_S             0
+#define FW_PTP_CMD_PORTID_M             0xf
+#define FW_PTP_CMD_PORTID_V(x)          ((x) << FW_PTP_CMD_PORTID_S)
+#define FW_PTP_CMD_PORTID_G(x)          \
+	(((x) >> FW_PTP_CMD_PORTID_S) & FW_PTP_CMD_PORTID_M)
+
 struct fw_rss_ind_tbl_cmd {
 	__be32 op_to_viid;
 	__be32 retval_len16;
@@ -3087,6 +3139,10 @@ struct fw_debug_cmd {
 #define FW_DEBUG_CMD_TYPE_G(x)	\
 	(((x) >> FW_DEBUG_CMD_TYPE_S) & FW_DEBUG_CMD_TYPE_M)
 
+enum pcie_fw_eval {
+	PCIE_FW_EVAL_CRASH = 0,
+};
+
 #define PCIE_FW_ERR_S		31
 #define PCIE_FW_ERR_V(x)	((x) << PCIE_FW_ERR_S)
 #define PCIE_FW_ERR_F		PCIE_FW_ERR_V(1U)
diff --git a/drivers/net/ethernet/chelsio/libcxgb/libcxgb_cm.h b/drivers/net/ethernet/chelsio/libcxgb/libcxgb_cm.h
index 515b94ff9080..4b5aacc09cab 100644
--- a/drivers/net/ethernet/chelsio/libcxgb/libcxgb_cm.h
+++ b/drivers/net/ethernet/chelsio/libcxgb/libcxgb_cm.h
@@ -90,7 +90,7 @@ cxgb_mk_tid_release(struct sk_buff *skb, u32 len, u32 tid, u16 chan)
 {
 	struct cpl_tid_release *req;
 
-	req = (struct cpl_tid_release *)__skb_put(skb, len);
+	req = __skb_put(skb, len);
 	memset(req, 0, len);
 
 	INIT_TP_WR(req, tid);
@@ -104,7 +104,7 @@ cxgb_mk_close_con_req(struct sk_buff *skb, u32 len, u32 tid, u16 chan,
 {
 	struct cpl_close_con_req *req;
 
-	req = (struct cpl_close_con_req *)__skb_put(skb, len);
+	req = __skb_put(skb, len);
 	memset(req, 0, len);
 
 	INIT_TP_WR(req, tid);
@@ -119,7 +119,7 @@ cxgb_mk_abort_req(struct sk_buff *skb, u32 len, u32 tid, u16 chan,
 {
 	struct cpl_abort_req *req;
 
-	req = (struct cpl_abort_req *)__skb_put(skb, len);
+	req = __skb_put(skb, len);
 	memset(req, 0, len);
 
 	INIT_TP_WR(req, tid);
@@ -134,7 +134,7 @@ cxgb_mk_abort_rpl(struct sk_buff *skb, u32 len, u32 tid, u16 chan)
 {
 	struct cpl_abort_rpl *rpl;
 
-	rpl = (struct cpl_abort_rpl *)__skb_put(skb, len);
+	rpl = __skb_put(skb, len);
 	memset(rpl, 0, len);
 
 	INIT_TP_WR(rpl, tid);
@@ -149,7 +149,7 @@ cxgb_mk_rx_data_ack(struct sk_buff *skb, u32 len, u32 tid, u16 chan,
 {
 	struct cpl_rx_data_ack *req;
 
-	req = (struct cpl_rx_data_ack *)__skb_put(skb, len);
+	req = __skb_put(skb, len);
 	memset(req, 0, len);
 
 	INIT_TP_WR(req, tid);
diff --git a/drivers/net/ethernet/cirrus/cs89x0.c b/drivers/net/ethernet/cirrus/cs89x0.c
index da5b58b853e2..410a0a95130b 100644
--- a/drivers/net/ethernet/cirrus/cs89x0.c
+++ b/drivers/net/ethernet/cirrus/cs89x0.c
@@ -450,11 +450,10 @@ skip_this_frame:
 
 	if (bp + length > lp->end_dma_buff) {
 		int semi_cnt = lp->end_dma_buff - bp;
-		memcpy(skb_put(skb, semi_cnt), bp, semi_cnt);
-		memcpy(skb_put(skb, length - semi_cnt), lp->dma_buff,
-		       length - semi_cnt);
+		skb_put_data(skb, bp, semi_cnt);
+		skb_put_data(skb, lp->dma_buff, length - semi_cnt);
 	} else {
-		memcpy(skb_put(skb, length), bp, length);
+		skb_put_data(skb, bp, length);
 	}
 	bp += (length + 3) & ~3;
 	if (bp >= lp->end_dma_buff)
diff --git a/drivers/net/ethernet/cirrus/ep93xx_eth.c b/drivers/net/ethernet/cirrus/ep93xx_eth.c
index 7a7c02f1f8b9..e2a702996db4 100644
--- a/drivers/net/ethernet/cirrus/ep93xx_eth.c
+++ b/drivers/net/ethernet/cirrus/ep93xx_eth.c
@@ -702,7 +702,10 @@ static int ep93xx_get_link_ksettings(struct net_device *dev,
 				     struct ethtool_link_ksettings *cmd)
 {
 	struct ep93xx_priv *ep = netdev_priv(dev);
-	return mii_ethtool_get_link_ksettings(&ep->mii, cmd);
+
+	mii_ethtool_get_link_ksettings(&ep->mii, cmd);
+
+	return 0;
 }
 
 static int ep93xx_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/cisco/enic/enic.h b/drivers/net/ethernet/cisco/enic/enic.h
index 2b23f46b34d3..ba032ac9ae86 100644
--- a/drivers/net/ethernet/cisco/enic/enic.h
+++ b/drivers/net/ethernet/cisco/enic/enic.h
@@ -33,7 +33,7 @@
 
 #define DRV_NAME		"enic"
 #define DRV_DESCRIPTION		"Cisco VIC Ethernet NIC Driver"
-#define DRV_VERSION		"2.3.0.31"
+#define DRV_VERSION		"2.3.0.42"
 #define DRV_COPYRIGHT		"Copyright 2008-2013 Cisco Systems, Inc"
 
 #define ENIC_BARS_MAX		6
@@ -47,7 +47,7 @@
 
 struct enic_msix_entry {
 	int requested;
-	char devname[IFNAMSIZ];
+	char devname[IFNAMSIZ + 8];
 	irqreturn_t (*isr)(int, void *);
 	void *devid;
 	cpumask_var_t affinity_mask;
diff --git a/drivers/net/ethernet/cisco/enic/enic_main.c b/drivers/net/ethernet/cisco/enic/enic_main.c
index 4b87beeabce1..d24ee1ad3be1 100644
--- a/drivers/net/ethernet/cisco/enic/enic_main.c
+++ b/drivers/net/ethernet/cisco/enic/enic_main.c
@@ -1537,13 +1537,12 @@ static int enic_poll(struct napi_struct *napi, int budget)
 		 */
 		enic_calc_int_moderation(enic, &enic->rq[0]);
 
-	if (rq_work_done < rq_work_to_do) {
+	if ((rq_work_done < budget) && napi_complete_done(napi, rq_work_done)) {
 
 		/* Some work done, but not enough to stay in polling,
 		 * exit polling
 		 */
 
-		napi_complete_done(napi, rq_work_done);
 		if (enic->rx_coalesce_setting.use_adaptive_rx_coalesce)
 			enic_set_int_moderation(enic, &enic->rq[0]);
 		vnic_intr_unmask(&enic->intr[intr]);
@@ -1663,13 +1662,12 @@ static int enic_poll_msix_rq(struct napi_struct *napi, int budget)
 		 */
 		enic_calc_int_moderation(enic, &enic->rq[rq]);
 
-	if (work_done < work_to_do) {
+	if ((work_done < budget) && napi_complete_done(napi, work_done)) {
 
 		/* Some work done, but not enough to stay in polling,
 		 * exit polling
 		 */
 
-		napi_complete_done(napi, work_done);
 		if (enic->rx_coalesce_setting.use_adaptive_rx_coalesce)
 			enic_set_int_moderation(enic, &enic->rq[rq]);
 		vnic_intr_unmask(&enic->intr[intr]);
@@ -1739,7 +1737,7 @@ static int enic_request_intr(struct enic *enic)
 			intr = enic_msix_rq_intr(enic, i);
 			snprintf(enic->msix[intr].devname,
 				sizeof(enic->msix[intr].devname),
-				"%.11s-rx-%u", netdev->name, i);
+				"%s-rx-%u", netdev->name, i);
 			enic->msix[intr].isr = enic_isr_msix;
 			enic->msix[intr].devid = &enic->napi[i];
 		}
@@ -1750,7 +1748,7 @@ static int enic_request_intr(struct enic *enic)
 			intr = enic_msix_wq_intr(enic, i);
 			snprintf(enic->msix[intr].devname,
 				sizeof(enic->msix[intr].devname),
-				"%.11s-tx-%u", netdev->name, i);
+				"%s-tx-%u", netdev->name, i);
 			enic->msix[intr].isr = enic_isr_msix;
 			enic->msix[intr].devid = &enic->napi[wq];
 		}
@@ -1758,14 +1756,14 @@ static int enic_request_intr(struct enic *enic)
 		intr = enic_msix_err_intr(enic);
 		snprintf(enic->msix[intr].devname,
 			sizeof(enic->msix[intr].devname),
-			"%.11s-err", netdev->name);
+			"%s-err", netdev->name);
 		enic->msix[intr].isr = enic_isr_msix_err;
 		enic->msix[intr].devid = enic;
 
 		intr = enic_msix_notify_intr(enic);
 		snprintf(enic->msix[intr].devname,
 			sizeof(enic->msix[intr].devname),
-			"%.11s-notify", netdev->name);
+			"%s-notify", netdev->name);
 		enic->msix[intr].isr = enic_isr_msix_notify;
 		enic->msix[intr].devid = enic;
 
diff --git a/drivers/net/ethernet/davicom/dm9000.c b/drivers/net/ethernet/davicom/dm9000.c
index 008dc8161775..16fe776ddbe5 100644
--- a/drivers/net/ethernet/davicom/dm9000.c
+++ b/drivers/net/ethernet/davicom/dm9000.c
@@ -1171,7 +1171,7 @@ dm9000_rx(struct net_device *dev)
 		if (GoodPacket &&
 		    ((skb = netdev_alloc_skb(dev, RxLen + 4)) != NULL)) {
 			skb_reserve(skb, 2);
-			rdptr = (u8 *) skb_put(skb, RxLen - 4);
+			rdptr = skb_put(skb, RxLen - 4);
 
 			/* Read received packet from RX SRAM */
 
diff --git a/drivers/net/ethernet/dec/tulip/de2104x.c b/drivers/net/ethernet/dec/tulip/de2104x.c
index 91b8f6f5a765..c87b8cc42963 100644
--- a/drivers/net/ethernet/dec/tulip/de2104x.c
+++ b/drivers/net/ethernet/dec/tulip/de2104x.c
@@ -1483,8 +1483,8 @@ static void __de_get_regs(struct de_private *de, u8 *buf)
 	de_rx_missed(de, rbuf[8]);
 }
 
-static int __de_get_link_ksettings(struct de_private *de,
-				   struct ethtool_link_ksettings *cmd)
+static void __de_get_link_ksettings(struct de_private *de,
+				    struct ethtool_link_ksettings *cmd)
 {
 	ethtool_convert_legacy_u32_to_link_mode(cmd->link_modes.supported,
 						de->media_supported);
@@ -1517,8 +1517,6 @@ static int __de_get_link_ksettings(struct de_private *de,
 		cmd->base.autoneg = AUTONEG_ENABLE;
 
 	/* ignore maxtxpkt, maxrxpkt for now */
-
-	return 0;
 }
 
 static int __de_set_link_ksettings(struct de_private *de,
@@ -1615,13 +1613,12 @@ static int de_get_link_ksettings(struct net_device *dev,
 				 struct ethtool_link_ksettings *cmd)
 {
 	struct de_private *de = netdev_priv(dev);
-	int rc;
 
 	spin_lock_irq(&de->lock);
-	rc = __de_get_link_ksettings(de, cmd);
+	__de_get_link_ksettings(de, cmd);
 	spin_unlock_irq(&de->lock);
 
-	return rc;
+	return 0;
 }
 
 static int de_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/dec/tulip/de4x5.c b/drivers/net/ethernet/dec/tulip/de4x5.c
index fd6bcf024729..47be5018d35d 100644
--- a/drivers/net/ethernet/dec/tulip/de4x5.c
+++ b/drivers/net/ethernet/dec/tulip/de4x5.c
@@ -3624,10 +3624,10 @@ de4x5_alloc_rx_buff(struct net_device *dev, int index, int len)
     skb_reserve(p, 2);	                               /* Align */
     if (index < lp->rx_old) {                          /* Wrapped buffer */
 	short tlen = (lp->rxRingSize - lp->rx_old) * RX_BUFF_SZ;
-	memcpy(skb_put(p,tlen),lp->rx_bufs + lp->rx_old * RX_BUFF_SZ,tlen);
-	memcpy(skb_put(p,len-tlen),lp->rx_bufs,len-tlen);
+	skb_put_data(p, lp->rx_bufs + lp->rx_old * RX_BUFF_SZ, tlen);
+	skb_put_data(p, lp->rx_bufs, len - tlen);
     } else {                                           /* Linear buffer */
-	memcpy(skb_put(p,len),lp->rx_bufs + lp->rx_old * RX_BUFF_SZ,len);
+	skb_put_data(p, lp->rx_bufs + lp->rx_old * RX_BUFF_SZ, len);
     }
 
     return p;
diff --git a/drivers/net/ethernet/dec/tulip/interrupt.c b/drivers/net/ethernet/dec/tulip/interrupt.c
index ba6ae24acf62..8df80880ecaa 100644
--- a/drivers/net/ethernet/dec/tulip/interrupt.c
+++ b/drivers/net/ethernet/dec/tulip/interrupt.c
@@ -218,9 +218,9 @@ int tulip_poll(struct napi_struct *napi, int budget)
                                                         pkt_len);
                                        skb_put(skb, pkt_len);
 #else
-                                       memcpy(skb_put(skb, pkt_len),
-                                              tp->rx_buffers[entry].skb->data,
-                                              pkt_len);
+                                       skb_put_data(skb,
+                                                    tp->rx_buffers[entry].skb->data,
+                                                    pkt_len);
 #endif
                                        pci_dma_sync_single_for_device(tp->pdev,
 								      tp->rx_buffers[entry].mapping,
@@ -444,9 +444,9 @@ static int tulip_rx(struct net_device *dev)
 						 pkt_len);
 				skb_put(skb, pkt_len);
 #else
-				memcpy(skb_put(skb, pkt_len),
-				       tp->rx_buffers[entry].skb->data,
-				       pkt_len);
+				skb_put_data(skb,
+					     tp->rx_buffers[entry].skb->data,
+					     pkt_len);
 #endif
 				pci_dma_sync_single_for_device(tp->pdev,
 							       tp->rx_buffers[entry].mapping,
diff --git a/drivers/net/ethernet/dec/tulip/uli526x.c b/drivers/net/ethernet/dec/tulip/uli526x.c
index 8d98b259d1ba..7fc248efc4ba 100644
--- a/drivers/net/ethernet/dec/tulip/uli526x.c
+++ b/drivers/net/ethernet/dec/tulip/uli526x.c
@@ -864,9 +864,9 @@ static void uli526x_rx_packet(struct net_device *dev, struct uli526x_board_info
 					skb = new_skb;
 					/* size less than COPY_SIZE, allocate a rxlen SKB */
 					skb_reserve(skb, 2); /* 16byte align */
-					memcpy(skb_put(skb, rxlen),
-					       skb_tail_pointer(rxptr->rx_skb_ptr),
-					       rxlen);
+					skb_put_data(skb,
+						     skb_tail_pointer(rxptr->rx_skb_ptr),
+						     rxlen);
 					uli526x_reuse_skb(db, rxptr->rx_skb_ptr);
 				} else
 					skb_put(skb, rxlen);
diff --git a/drivers/net/ethernet/dec/tulip/winbond-840.c b/drivers/net/ethernet/dec/tulip/winbond-840.c
index d1f2f3cc7cfa..32d7229544fa 100644
--- a/drivers/net/ethernet/dec/tulip/winbond-840.c
+++ b/drivers/net/ethernet/dec/tulip/winbond-840.c
@@ -1395,13 +1395,12 @@ static int netdev_get_link_ksettings(struct net_device *dev,
 				     struct ethtool_link_ksettings *cmd)
 {
 	struct netdev_private *np = netdev_priv(dev);
-	int rc;
 
 	spin_lock_irq(&np->lock);
-	rc = mii_ethtool_get_link_ksettings(&np->mii_if, cmd);
+	mii_ethtool_get_link_ksettings(&np->mii_if, cmd);
 	spin_unlock_irq(&np->lock);
 
-	return rc;
+	return 0;
 }
 
 static int netdev_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/dnet.c b/drivers/net/ethernet/dnet.c
index 3e77dd863175..5a847941c46b 100644
--- a/drivers/net/ethernet/dnet.c
+++ b/drivers/net/ethernet/dnet.c
@@ -399,7 +399,7 @@ static int dnet_poll(struct napi_struct *napi, int budget)
 			 * 'skb_put()' points to the start of sk_buff
 			 * data area.
 			 */
-			data_ptr = (unsigned int *)skb_put(skb, pkt_len);
+			data_ptr = skb_put(skb, pkt_len);
 			for (i = 0; i < (pkt_len + 3) >> 2; i++)
 				*data_ptr++ = dnet_readl(bp, RX_DATA_FIFO);
 			skb->protocol = eth_type_trans(skb, dev);
diff --git a/drivers/net/ethernet/ec_bhf.c b/drivers/net/ethernet/ec_bhf.c
index 278f139f2a22..4ee042c034a1 100644
--- a/drivers/net/ethernet/ec_bhf.c
+++ b/drivers/net/ethernet/ec_bhf.c
@@ -223,7 +223,7 @@ static void ec_bhf_process_rx(struct ec_bhf_priv *priv)
 
 		skb = netdev_alloc_skb_ip_align(priv->net_dev, pkt_size);
 		if (skb) {
-			memcpy(skb_put(skb, pkt_size), data, pkt_size);
+			skb_put_data(skb, data, pkt_size);
 			skb->protocol = eth_type_trans(skb, priv->net_dev);
 			priv->stat_rx_bytes += pkt_size;
 
diff --git a/drivers/net/ethernet/emulex/benet/be.h b/drivers/net/ethernet/emulex/benet/be.h
index 50566243e6fa..674cf9d13b98 100644
--- a/drivers/net/ethernet/emulex/benet/be.h
+++ b/drivers/net/ethernet/emulex/benet/be.h
@@ -37,7 +37,7 @@
 #include "be_hw.h"
 #include "be_roce.h"
 
-#define DRV_VER			"11.1.0.0"
+#define DRV_VER			"11.4.0.0"
 #define DRV_NAME		"be2net"
 #define BE_NAME			"Emulex BladeEngine2"
 #define BE3_NAME		"Emulex BladeEngine3"
diff --git a/drivers/net/ethernet/emulex/benet/be_hw.h b/drivers/net/ethernet/emulex/benet/be_hw.h
index 36e4232ed6b8..c967f45705d9 100644
--- a/drivers/net/ethernet/emulex/benet/be_hw.h
+++ b/drivers/net/ethernet/emulex/benet/be_hw.h
@@ -49,6 +49,9 @@
 #define POST_STAGE_BE_RESET		0x3 /* Host wants to reset chip */
 #define POST_STAGE_ARMFW_RDY		0xc000	/* FW is done with POST */
 #define POST_STAGE_RECOVERABLE_ERR	0xE000	/* Recoverable err detected */
+/* FW has detected a UE and is dumping FAT log data */
+#define POST_STAGE_FAT_LOG_START       0x0D00
+#define POST_STAGE_ARMFW_UE            0xF000  /*FW has asserted an UE*/
 
 /* Lancer SLIPORT registers */
 #define SLIPORT_STATUS_OFFSET		0x404
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 4eee18ce9be4..319eee36649b 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -3241,8 +3241,9 @@ 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;
 	struct device *dev = &adapter->pdev->dev;
+	u16 val;
+	u32 i;
 
 	if (be_check_error(adapter, BE_ERROR_HW))
 		return;
@@ -3280,15 +3281,25 @@ void be_detect_error(struct be_adapter *adapter)
 		ue_lo = (ue_lo & ~ue_lo_mask);
 		ue_hi = (ue_hi & ~ue_hi_mask);
 
-		/* On certain platforms BE hardware can indicate spurious UEs.
-		 * Allow HW to stop working completely in case of a real UE.
-		 * Hence not setting the hw_error for UE detection.
-		 */
-
 		if (ue_lo || ue_hi) {
+			/* On certain platforms BE3 hardware can indicate
+			 * spurious UEs. In case of a UE in the chip,
+			 * the POST register correctly reports either a
+			 * FAT_LOG_START state (FW is currently dumping
+			 * FAT log data) or a ARMFW_UE state. Check for the
+			 * above states to ascertain if the UE is valid or not.
+			 */
+			if (BE3_chip(adapter)) {
+				val = be_POST_stage_get(adapter);
+				if ((val & POST_STAGE_FAT_LOG_START)
+				     != POST_STAGE_FAT_LOG_START &&
+				    (val & POST_STAGE_ARMFW_UE)
+				     != POST_STAGE_ARMFW_UE)
+					return;
+			}
+
 			dev_err(dev, "Error detected in the adapter");
-			if (skyhawk_chip(adapter))
-				be_set_error(adapter, BE_ERROR_UE);
+			be_set_error(adapter, BE_ERROR_UE);
 
 			for (i = 0; ue_lo; ue_lo >>= 1, i++) {
 				if (ue_lo & 1)
diff --git a/drivers/net/ethernet/faraday/ftmac100.c b/drivers/net/ethernet/faraday/ftmac100.c
index 1536356e2ea8..66928a922824 100644
--- a/drivers/net/ethernet/faraday/ftmac100.c
+++ b/drivers/net/ethernet/faraday/ftmac100.c
@@ -829,7 +829,10 @@ static int ftmac100_get_link_ksettings(struct net_device *netdev,
 				       struct ethtool_link_ksettings *cmd)
 {
 	struct ftmac100 *priv = netdev_priv(netdev);
-	return mii_ethtool_get_link_ksettings(&priv->mii, cmd);
+
+	mii_ethtool_get_link_ksettings(&priv->mii, cmd);
+
+	return 0;
 }
 
 static int ftmac100_set_link_ksettings(struct net_device *netdev,
diff --git a/drivers/net/ethernet/fealnx.c b/drivers/net/ethernet/fealnx.c
index 766636a7c25e..e92859dab7ae 100644
--- a/drivers/net/ethernet/fealnx.c
+++ b/drivers/net/ethernet/fealnx.c
@@ -1711,8 +1711,8 @@ static int netdev_rx(struct net_device *dev)
 					np->cur_rx->skbuff->data, pkt_len);
 				skb_put(skb, pkt_len);
 #else
-				memcpy(skb_put(skb, pkt_len),
-					np->cur_rx->skbuff->data, pkt_len);
+				skb_put_data(skb, np->cur_rx->skbuff->data,
+					     pkt_len);
 #endif
 				pci_dma_sync_single_for_device(np->pci_dev,
 							       np->cur_rx->buffer,
@@ -1821,13 +1821,12 @@ static int netdev_get_link_ksettings(struct net_device *dev,
 				     struct ethtool_link_ksettings *cmd)
 {
 	struct netdev_private *np = netdev_priv(dev);
-	int rc;
 
 	spin_lock_irq(&np->lock);
-	rc = mii_ethtool_get_link_ksettings(&np->mii, cmd);
+	mii_ethtool_get_link_ksettings(&np->mii, cmd);
 	spin_unlock_irq(&np->lock);
 
-	return rc;
+	return 0;
 }
 
 static int netdev_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/freescale/dpaa/dpaa_eth.c b/drivers/net/ethernet/freescale/dpaa/dpaa_eth.c
index 9a520e4f0df9..757b873735a5 100644
--- a/drivers/net/ethernet/freescale/dpaa/dpaa_eth.c
+++ b/drivers/net/ethernet/freescale/dpaa/dpaa_eth.c
@@ -342,8 +342,8 @@ static void dpaa_get_stats64(struct net_device *net_dev,
 	}
 }
 
-static int dpaa_setup_tc(struct net_device *net_dev, u32 handle, __be16 proto,
-			 struct tc_to_netdev *tc)
+static int dpaa_setup_tc(struct net_device *net_dev, u32 handle,
+			 u32 chain_index, __be16 proto, struct tc_to_netdev *tc)
 {
 	struct dpaa_priv *priv = netdev_priv(net_dev);
 	u8 num_tc;
@@ -2647,7 +2647,7 @@ static int dpaa_eth_probe(struct platform_device *pdev)
 	priv->buf_layout[TX].priv_data_size = DPAA_TX_PRIV_DATA_SIZE; /* Tx */
 
 	/* device used for DMA mapping */
-	arch_setup_dma_ops(dev, 0, 0, NULL, false);
+	set_dma_ops(dev, get_dma_ops(&pdev->dev));
 	err = dma_coerce_mask_and_coherent(dev, DMA_BIT_MASK(40));
 	if (err) {
 		dev_err(dev, "dma_coerce_mask_and_coherent() failed\n");
diff --git a/drivers/net/ethernet/freescale/dpaa/dpaa_ethtool.c b/drivers/net/ethernet/freescale/dpaa/dpaa_ethtool.c
index 15571e251fb9..aad825088357 100644
--- a/drivers/net/ethernet/freescale/dpaa/dpaa_ethtool.c
+++ b/drivers/net/ethernet/freescale/dpaa/dpaa_ethtool.c
@@ -75,16 +75,14 @@ static char dpaa_stats_global[][ETH_GSTRING_LEN] = {
 static int dpaa_get_link_ksettings(struct net_device *net_dev,
 				   struct ethtool_link_ksettings *cmd)
 {
-	int err;
-
 	if (!net_dev->phydev) {
 		netdev_dbg(net_dev, "phy device not initialized\n");
 		return 0;
 	}
 
-	err = phy_ethtool_ksettings_get(net_dev->phydev, cmd);
+	phy_ethtool_ksettings_get(net_dev->phydev, cmd);
 
-	return err;
+	return 0;
 }
 
 static int dpaa_set_link_ksettings(struct net_device *net_dev,
diff --git a/drivers/net/ethernet/freescale/fec.h b/drivers/net/ethernet/freescale/fec.h
index 5ea740b4cf14..38c7b21e5d63 100644
--- a/drivers/net/ethernet/freescale/fec.h
+++ b/drivers/net/ethernet/freescale/fec.h
@@ -446,6 +446,10 @@ struct bufdesc_ex {
 #define FEC_QUIRK_HAS_COALESCE		(1 << 13)
 /* Interrupt doesn't wake CPU from deep idle */
 #define FEC_QUIRK_ERR006687		(1 << 14)
+/* The MIB counters should be cleared and enabled during
+ * initialisation.
+ */
+#define FEC_QUIRK_MIB_CLEAR		(1 << 15)
 
 struct bufdesc_prop {
 	int qid;
diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c
index f7c8649fd28f..a6e323f15637 100644
--- a/drivers/net/ethernet/freescale/fec_main.c
+++ b/drivers/net/ethernet/freescale/fec_main.c
@@ -89,10 +89,10 @@ static struct platform_device_id fec_devtype[] = {
 		.driver_data = 0,
 	}, {
 		.name = "imx25-fec",
-		.driver_data = FEC_QUIRK_USE_GASKET,
+		.driver_data = FEC_QUIRK_USE_GASKET | FEC_QUIRK_MIB_CLEAR,
 	}, {
 		.name = "imx27-fec",
-		.driver_data = 0,
+		.driver_data = FEC_QUIRK_MIB_CLEAR,
 	}, {
 		.name = "imx28-fec",
 		.driver_data = FEC_QUIRK_ENET_MAC | FEC_QUIRK_SWAP_FRAME |
@@ -184,6 +184,9 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 #define FEC_RACC_SHIFT16	BIT(7)
 #define FEC_RACC_OPTIONS	(FEC_RACC_IPDIS | FEC_RACC_PRODIS)
 
+/* MIB Control Register */
+#define FEC_MIB_CTRLSTAT_DISABLE	BIT(31)
+
 /*
  * The 5270/5271/5280/5282/532x RX control register also contains maximum frame
  * size bits. Other FEC hardware does not, so we need to take that into
@@ -2356,11 +2359,30 @@ static int fec_enet_get_sset_count(struct net_device *dev, int sset)
 	}
 }
 
+static void fec_enet_clear_ethtool_stats(struct net_device *dev)
+{
+	struct fec_enet_private *fep = netdev_priv(dev);
+	int i;
+
+	/* Disable MIB statistics counters */
+	writel(FEC_MIB_CTRLSTAT_DISABLE, fep->hwp + FEC_MIB_CTRLSTAT);
+
+	for (i = 0; i < ARRAY_SIZE(fec_stats); i++)
+		writel(0, fep->hwp + fec_stats[i].offset);
+
+	/* Don't disable MIB statistics counters */
+	writel(0, fep->hwp + FEC_MIB_CTRLSTAT);
+}
+
 #else	/* !defined(CONFIG_M5272) */
 #define FEC_STATS_SIZE	0
 static inline void fec_enet_update_ethtool_stats(struct net_device *dev)
 {
 }
+
+static inline void fec_enet_clear_ethtool_stats(struct net_device *dev)
+{
+}
 #endif /* !defined(CONFIG_M5272) */
 
 /* ITR clock source is enet system clock (clk_ahb).
@@ -3182,7 +3204,10 @@ static int fec_enet_init(struct net_device *ndev)
 
 	fec_restart(ndev);
 
-	fec_enet_update_ethtool_stats(ndev);
+	if (fep->quirks & FEC_QUIRK_MIB_CLEAR)
+		fec_enet_clear_ethtool_stats(ndev);
+	else
+		fec_enet_update_ethtool_stats(ndev);
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/freescale/fman/Kconfig b/drivers/net/ethernet/freescale/fman/Kconfig
index dc0850b3b517..8870a9a798ca 100644
--- a/drivers/net/ethernet/freescale/fman/Kconfig
+++ b/drivers/net/ethernet/freescale/fman/Kconfig
@@ -2,6 +2,7 @@ config FSL_FMAN
 	tristate "FMan support"
 	depends on FSL_SOC || ARCH_LAYERSCAPE || COMPILE_TEST
 	select GENERIC_ALLOCATOR
+	depends on HAS_DMA
 	select PHYLIB
 	default n
 	help
diff --git a/drivers/net/ethernet/freescale/fman/mac.c b/drivers/net/ethernet/freescale/fman/mac.c
index 0b31f8502ada..6e67d22fd0d5 100644
--- a/drivers/net/ethernet/freescale/fman/mac.c
+++ b/drivers/net/ethernet/freescale/fman/mac.c
@@ -623,6 +623,8 @@ static struct platform_device *dpaa_eth_add_device(int fman_id,
 		goto no_mem;
 	}
 
+	set_dma_ops(&pdev->dev, get_dma_ops(priv->dev));
+
 	ret = platform_device_add_data(pdev, &data, sizeof(data));
 	if (ret)
 		goto err;
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index 0ff166ec3e7e..c4b4b0a1bbf0 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -1718,7 +1718,7 @@ static int gfar_restore(struct device *dev)
 	return 0;
 }
 
-static struct dev_pm_ops gfar_pm_ops = {
+static const struct dev_pm_ops gfar_pm_ops = {
 	.suspend = gfar_suspend,
 	.resume = gfar_resume,
 	.freeze = gfar_suspend,
@@ -2250,7 +2250,7 @@ static int gfar_enet_open(struct net_device *dev)
 
 static inline struct txfcb *gfar_add_fcb(struct sk_buff *skb)
 {
-	struct txfcb *fcb = (struct txfcb *)skb_push(skb, GMAC_FCB_LEN);
+	struct txfcb *fcb = skb_push(skb, GMAC_FCB_LEN);
 
 	memset(fcb, 0, GMAC_FCB_LEN);
 
diff --git a/drivers/net/ethernet/freescale/ucc_geth_ethtool.c b/drivers/net/ethernet/freescale/ucc_geth_ethtool.c
index b642990b549c..4df282ed22c7 100644
--- a/drivers/net/ethernet/freescale/ucc_geth_ethtool.c
+++ b/drivers/net/ethernet/freescale/ucc_geth_ethtool.c
@@ -113,7 +113,9 @@ uec_get_ksettings(struct net_device *netdev, struct ethtool_link_ksettings *cmd)
 	if (!phydev)
 		return -ENODEV;
 
-	return phy_ethtool_ksettings_get(phydev, cmd);
+	phy_ethtool_ksettings_get(phydev, cmd);
+
+	return 0;
 }
 
 static int
diff --git a/drivers/net/ethernet/hisilicon/hns/hnae.h b/drivers/net/ethernet/hisilicon/hns/hnae.h
index 04211ac73b36..7ba653af19cb 100644
--- a/drivers/net/ethernet/hisilicon/hns/hnae.h
+++ b/drivers/net/ethernet/hisilicon/hns/hnae.h
@@ -360,6 +360,7 @@ enum hnae_loop {
 	MAC_INTERNALLOOP_MAC = 0,
 	MAC_INTERNALLOOP_SERDES,
 	MAC_INTERNALLOOP_PHY,
+	MAC_LOOP_PHY_NONE,
 	MAC_LOOP_NONE,
 };
 
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.c b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.c
index e13aa064a8e9..7a8addda726e 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.c
@@ -29,10 +29,9 @@ enum _dsm_rst_type {
 	HNS_ROCE_RESET_FUNC     = 0x7,
 };
 
-const u8 hns_dsaf_acpi_dsm_uuid[] = {
-	0x1A, 0xAA, 0x85, 0x1A, 0x93, 0xE2, 0x5E, 0x41,
-	0x8E, 0x28, 0x8D, 0x69, 0x0A, 0x0F, 0x82, 0x0A
-};
+static const guid_t hns_dsaf_acpi_dsm_guid =
+	GUID_INIT(0x1A85AA1A, 0xE293, 0x415E,
+		  0x8E, 0x28, 0x8D, 0x69, 0x0A, 0x0F, 0x82, 0x0A);
 
 static void dsaf_write_sub(struct dsaf_device *dsaf_dev, u32 reg, u32 val)
 {
@@ -151,7 +150,7 @@ static void hns_dsaf_acpi_srst_by_port(struct dsaf_device *dsaf_dev, u8 op_type,
 	argv4.package.elements = obj_args;
 
 	obj = acpi_evaluate_dsm(ACPI_HANDLE(dsaf_dev->dev),
-				hns_dsaf_acpi_dsm_uuid, 0, op_type, &argv4);
+				&hns_dsaf_acpi_dsm_guid, 0, op_type, &argv4);
 	if (!obj) {
 		dev_warn(dsaf_dev->dev, "reset port_type%d port%d fail!",
 			 port_type, port);
@@ -434,7 +433,7 @@ static phy_interface_t hns_mac_get_phy_if_acpi(struct hns_mac_cb *mac_cb)
 	argv4.package.elements = &obj_args,
 
 	obj = acpi_evaluate_dsm(ACPI_HANDLE(mac_cb->dev),
-				hns_dsaf_acpi_dsm_uuid, 0,
+				&hns_dsaf_acpi_dsm_guid, 0,
 				HNS_OP_GET_PORT_TYPE_FUNC, &argv4);
 
 	if (!obj || obj->type != ACPI_TYPE_INTEGER)
@@ -474,7 +473,7 @@ int hns_mac_get_sfp_prsnt_acpi(struct hns_mac_cb *mac_cb, int *sfp_prsnt)
 	argv4.package.elements = &obj_args,
 
 	obj = acpi_evaluate_dsm(ACPI_HANDLE(mac_cb->dev),
-				hns_dsaf_acpi_dsm_uuid, 0,
+				&hns_dsaf_acpi_dsm_guid, 0,
 				HNS_OP_GET_SFP_STAT_FUNC, &argv4);
 
 	if (!obj || obj->type != ACPI_TYPE_INTEGER)
@@ -565,7 +564,7 @@ hns_mac_config_sds_loopback_acpi(struct hns_mac_cb *mac_cb, bool en)
 	argv4.package.elements = obj_args;
 
 	obj = acpi_evaluate_dsm(ACPI_HANDLE(mac_cb->dsaf_dev->dev),
-				hns_dsaf_acpi_dsm_uuid, 0,
+				&hns_dsaf_acpi_dsm_guid, 0,
 				HNS_OP_SERDES_LP_FUNC, &argv4);
 	if (!obj) {
 		dev_warn(mac_cb->dsaf_dev->dev, "set port%d serdes lp fail!",
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_ethtool.c b/drivers/net/ethernet/hisilicon/hns/hns_ethtool.c
index b8fab149690f..a8db27e86a11 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_ethtool.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_ethtool.c
@@ -150,7 +150,7 @@ static int hns_nic_get_link_ksettings(struct net_device *net_dev,
 	cmd->base.duplex = duplex;
 
 	if (net_dev->phydev)
-		(void)phy_ethtool_ksettings_get(net_dev->phydev, cmd);
+		phy_ethtool_ksettings_get(net_dev->phydev, cmd);
 
 	link_stat = hns_nic_get_link(net_dev);
 	if (!link_stat) {
@@ -259,55 +259,27 @@ static const char hns_nic_test_strs[][ETH_GSTRING_LEN] = {
 
 static int hns_nic_config_phy_loopback(struct phy_device *phy_dev, u8 en)
 {
-#define COPPER_CONTROL_REG 0
-#define PHY_POWER_DOWN BIT(11)
-#define PHY_LOOP_BACK BIT(14)
-	u16 val = 0;
-
-	if (phy_dev->is_c45) /* c45 branch adding for XGE PHY */
-		return -ENOTSUPP;
+	int err;
 
 	if (en) {
-		/* speed : 1000M */
-		phy_write(phy_dev, HNS_PHY_PAGE_REG, 2);
-		phy_write(phy_dev, 21, 0x1046);
-
-		phy_write(phy_dev, HNS_PHY_PAGE_REG, 0);
-		/* Force Master */
-		phy_write(phy_dev, 9, 0x1F00);
-
-		/* Soft-reset */
-		phy_write(phy_dev, 0, 0x9140);
-		/* If autoneg disabled,two soft-reset operations */
-		phy_write(phy_dev, 0, 0x9140);
-
-		phy_write(phy_dev, HNS_PHY_PAGE_REG, 0xFA);
-
-		/* Default is 0x0400 */
-		phy_write(phy_dev, 1, 0x418);
-
-		/* Force 1000M Link, Default is 0x0200 */
-		phy_write(phy_dev, 7, 0x20C);
-		phy_write(phy_dev, HNS_PHY_PAGE_REG, 0);
-
-		/* Enable PHY loop-back */
-		val = phy_read(phy_dev, COPPER_CONTROL_REG);
-		val |= PHY_LOOP_BACK;
-		val &= ~PHY_POWER_DOWN;
-		phy_write(phy_dev, COPPER_CONTROL_REG, val);
+		/* Doing phy loopback in offline state, phy resuming is
+		 * needed to power up the device.
+		 */
+		err = phy_resume(phy_dev);
+		if (err)
+			goto out;
+
+		err = phy_loopback(phy_dev, true);
 	} else {
-		phy_write(phy_dev, HNS_PHY_PAGE_REG, 0xFA);
-		phy_write(phy_dev, 1, 0x400);
-		phy_write(phy_dev, 7, 0x200);
-		phy_write(phy_dev, HNS_PHY_PAGE_REG, 0);
-		phy_write(phy_dev, 9, 0xF00);
-
-		val = phy_read(phy_dev, COPPER_CONTROL_REG);
-		val &= ~PHY_LOOP_BACK;
-		val |= PHY_POWER_DOWN;
-		phy_write(phy_dev, COPPER_CONTROL_REG, val);
+		err = phy_loopback(phy_dev, false);
+		if (err)
+			goto out;
+
+		err = phy_suspend(phy_dev);
 	}
-	return 0;
+
+out:
+	return err;
 }
 
 static int __lb_setup(struct net_device *ndev,
@@ -320,10 +292,9 @@ static int __lb_setup(struct net_device *ndev,
 
 	switch (loop) {
 	case MAC_INTERNALLOOP_PHY:
-		if ((phy_dev) && (!phy_dev->is_c45)) {
-			ret = hns_nic_config_phy_loopback(phy_dev, 0x1);
-			ret |= h->dev->ops->set_loopback(h, loop, 0x1);
-		}
+		ret = hns_nic_config_phy_loopback(phy_dev, 0x1);
+		if (!ret)
+			ret = h->dev->ops->set_loopback(h, loop, 0x1);
 		break;
 	case MAC_INTERNALLOOP_MAC:
 		if ((h->dev->ops->set_loopback) &&
@@ -334,17 +305,17 @@ static int __lb_setup(struct net_device *ndev,
 		if (h->dev->ops->set_loopback)
 			ret = h->dev->ops->set_loopback(h, loop, 0x1);
 		break;
+	case MAC_LOOP_PHY_NONE:
+		ret = hns_nic_config_phy_loopback(phy_dev, 0x0);
 	case MAC_LOOP_NONE:
-		if ((phy_dev) && (!phy_dev->is_c45))
-			ret |= hns_nic_config_phy_loopback(phy_dev, 0x0);
-
-		if (h->dev->ops->set_loopback) {
+		if (!ret && h->dev->ops->set_loopback) {
 			if (priv->ae_handle->phy_if != PHY_INTERFACE_MODE_XGMII)
-				ret |= h->dev->ops->set_loopback(h,
+				ret = h->dev->ops->set_loopback(h,
 					MAC_INTERNALLOOP_MAC, 0x0);
 
-			ret |= h->dev->ops->set_loopback(h,
-				MAC_INTERNALLOOP_SERDES, 0x0);
+			if (!ret)
+				ret = h->dev->ops->set_loopback(h,
+					MAC_INTERNALLOOP_SERDES, 0x0);
 		}
 		break;
 	default:
@@ -570,13 +541,16 @@ static int __lb_run_test(struct net_device *ndev,
 	return ret_val;
 }
 
-static int __lb_down(struct net_device *ndev)
+static int __lb_down(struct net_device *ndev, enum hnae_loop loop)
 {
 	struct hns_nic_priv *priv = netdev_priv(ndev);
 	struct hnae_handle *h = priv->ae_handle;
 	int ret;
 
-	ret = __lb_setup(ndev, MAC_LOOP_NONE);
+	if (loop == MAC_INTERNALLOOP_PHY)
+		ret = __lb_setup(ndev, MAC_LOOP_PHY_NONE);
+	else
+		ret = __lb_setup(ndev, MAC_LOOP_NONE);
 	if (ret)
 		netdev_err(ndev, "%s: __lb_setup return error(%d)!\n",
 			   __func__,
@@ -632,7 +606,8 @@ static void hns_nic_self_test(struct net_device *ndev,
 			if (!data[test_index]) {
 				data[test_index] = __lb_run_test(
 					ndev, (enum hnae_loop)st_param[i][0]);
-				(void)__lb_down(ndev);
+				(void)__lb_down(ndev,
+						(enum hnae_loop)st_param[i][0]);
 			}
 
 			if (data[test_index])
diff --git a/drivers/net/ethernet/hp/hp100.c b/drivers/net/ethernet/hp/hp100.c
index 5673b071e39d..c6164a98f257 100644
--- a/drivers/net/ethernet/hp/hp100.c
+++ b/drivers/net/ethernet/hp/hp100.c
@@ -1281,7 +1281,7 @@ static int hp100_build_rx_pdl(hp100_ring_t * ringptr,
 		 */
 		skb_reserve(ringptr->skb, 2);
 
-		ringptr->skb->data = (u_char *) skb_put(ringptr->skb, MAX_ETHER_SIZE);
+		ringptr->skb->data = skb_put(ringptr->skb, MAX_ETHER_SIZE);
 
 		/* ringptr->pdl points to the beginning of the PDL, i.e. the PDH */
 		/* Note: 1st Fragment is used for the 4 byte packet status
diff --git a/drivers/net/ethernet/i825xx/82596.c b/drivers/net/ethernet/i825xx/82596.c
index 945883842533..d719668a6684 100644
--- a/drivers/net/ethernet/i825xx/82596.c
+++ b/drivers/net/ethernet/i825xx/82596.c
@@ -809,7 +809,8 @@ memory_squeeze:
 				if (!rx_in_place) {
 					/* 16 byte align the data fields */
 					skb_reserve(skb, 2);
-					memcpy(skb_put(skb,pkt_len), rbd->v_data, pkt_len);
+					skb_put_data(skb, rbd->v_data,
+						     pkt_len);
 				}
 				skb->protocol=eth_type_trans(skb,dev);
 				skb->len = pkt_len;
diff --git a/drivers/net/ethernet/i825xx/lib82596.c b/drivers/net/ethernet/i825xx/lib82596.c
index e86773325cbe..8449c58f01fd 100644
--- a/drivers/net/ethernet/i825xx/lib82596.c
+++ b/drivers/net/ethernet/i825xx/lib82596.c
@@ -727,7 +727,8 @@ memory_squeeze:
 					dma_sync_single_for_cpu(dev->dev.parent,
 								(dma_addr_t)SWAP32(rbd->b_data),
 								PKT_BUF_SZ, DMA_FROM_DEVICE);
-					memcpy(skb_put(skb, pkt_len), rbd->v_data, pkt_len);
+					skb_put_data(skb, rbd->v_data,
+						     pkt_len);
 					dma_sync_single_for_device(dev->dev.parent,
 								   (dma_addr_t)SWAP32(rbd->b_data),
 								   PKT_BUF_SZ, DMA_FROM_DEVICE);
diff --git a/drivers/net/ethernet/ibm/ehea/ehea_main.c b/drivers/net/ethernet/ibm/ehea/ehea_main.c
index 1e53d7a82675..b9d310f20bcc 100644
--- a/drivers/net/ethernet/ibm/ehea/ehea_main.c
+++ b/drivers/net/ethernet/ibm/ehea/ehea_main.c
@@ -3553,14 +3553,12 @@ static int check_module_parm(void)
 	return ret;
 }
 
-static ssize_t ehea_show_capabilities(struct device_driver *drv,
-				      char *buf)
+static ssize_t capabilities_show(struct device_driver *drv, char *buf)
 {
 	return sprintf(buf, "%d", EHEA_CAPABILITIES);
 }
 
-static DRIVER_ATTR(capabilities, S_IRUSR | S_IRGRP | S_IROTH,
-		   ehea_show_capabilities, NULL);
+static DRIVER_ATTR_RO(capabilities);
 
 static int __init ehea_module_init(void)
 {
diff --git a/drivers/net/ethernet/ibm/emac/core.c b/drivers/net/ethernet/ibm/emac/core.c
index 508923f39ccf..259e69a52ec5 100644
--- a/drivers/net/ethernet/ibm/emac/core.c
+++ b/drivers/net/ethernet/ibm/emac/core.c
@@ -343,6 +343,7 @@ static int emac_reset(struct emac_instance *dev)
 {
 	struct emac_regs __iomem *p = dev->emacp;
 	int n = 20;
+	bool __maybe_unused try_internal_clock = false;
 
 	DBG(dev, "reset" NL);
 
@@ -355,6 +356,7 @@ static int emac_reset(struct emac_instance *dev)
 	}
 
 #ifdef CONFIG_PPC_DCR_NATIVE
+do_retry:
 	/*
 	 * PPC460EX/GT Embedded Processor Advanced User's Manual
 	 * section 28.10.1 Mode Register 0 (EMACx_MR0) states:
@@ -362,10 +364,19 @@ static int emac_reset(struct emac_instance *dev)
 	 * of the EMAC. If none is present, select the internal clock
 	 * (SDR0_ETH_CFG[EMACx_PHY_CLK] = 1).
 	 * After a soft reset, select the external clock.
+	 *
+	 * The AR8035-A PHY Meraki MR24 does not provide a TX Clk if the
+	 * ethernet cable is not attached. This causes the reset to timeout
+	 * and the PHY detection code in emac_init_phy() is unable to
+	 * communicate and detect the AR8035-A PHY. As a result, the emac
+	 * driver bails out early and the user has no ethernet.
+	 * In order to stay compatible with existing configurations, the
+	 * driver will temporarily switch to the internal clock, after
+	 * the first reset fails.
 	 */
 	if (emac_has_feature(dev, EMAC_FTR_460EX_PHY_CLK_FIX)) {
-		if (dev->phy_address == 0xffffffff &&
-		    dev->phy_map == 0xffffffff) {
+		if (try_internal_clock || (dev->phy_address == 0xffffffff &&
+					   dev->phy_map == 0xffffffff)) {
 			/* No PHY: select internal loop clock before reset */
 			dcri_clrset(SDR0, SDR0_ETH_CFG,
 				    0, SDR0_ETH_CFG_ECS << dev->cell_index);
@@ -383,8 +394,15 @@ static int emac_reset(struct emac_instance *dev)
 
 #ifdef CONFIG_PPC_DCR_NATIVE
 	if (emac_has_feature(dev, EMAC_FTR_460EX_PHY_CLK_FIX)) {
-		if (dev->phy_address == 0xffffffff &&
-		    dev->phy_map == 0xffffffff) {
+		if (!n && !try_internal_clock) {
+			/* first attempt has timed out. */
+			n = 20;
+			try_internal_clock = true;
+			goto do_retry;
+		}
+
+		if (try_internal_clock || (dev->phy_address == 0xffffffff &&
+					   dev->phy_map == 0xffffffff)) {
 			/* No PHY: restore external clock source after reset */
 			dcri_clrset(SDR0, SDR0_ETH_CFG,
 				    SDR0_ETH_CFG_ECS << dev->cell_index, 0);
@@ -2460,20 +2478,24 @@ static int emac_mii_bus_reset(struct mii_bus *bus)
 	return emac_reset(dev);
 }
 
+static int emac_mdio_phy_start_aneg(struct mii_phy *phy,
+				    struct phy_device *phy_dev)
+{
+	phy_dev->autoneg = phy->autoneg;
+	phy_dev->speed = phy->speed;
+	phy_dev->duplex = phy->duplex;
+	phy_dev->advertising = phy->advertising;
+	return phy_start_aneg(phy_dev);
+}
+
 static int emac_mdio_setup_aneg(struct mii_phy *phy, u32 advertise)
 {
 	struct net_device *ndev = phy->dev;
 	struct emac_instance *dev = netdev_priv(ndev);
 
-	dev->phy.autoneg = AUTONEG_ENABLE;
-	dev->phy.speed = SPEED_1000;
-	dev->phy.duplex = DUPLEX_FULL;
-	dev->phy.advertising = advertise;
 	phy->autoneg = AUTONEG_ENABLE;
-	phy->speed = dev->phy.speed;
-	phy->duplex = dev->phy.duplex;
 	phy->advertising = advertise;
-	return phy_start_aneg(dev->phy_dev);
+	return emac_mdio_phy_start_aneg(phy, dev->phy_dev);
 }
 
 static int emac_mdio_setup_forced(struct mii_phy *phy, int speed, int fd)
@@ -2481,13 +2503,10 @@ static int emac_mdio_setup_forced(struct mii_phy *phy, int speed, int fd)
 	struct net_device *ndev = phy->dev;
 	struct emac_instance *dev = netdev_priv(ndev);
 
-	dev->phy.autoneg =  AUTONEG_DISABLE;
-	dev->phy.speed = speed;
-	dev->phy.duplex = fd;
 	phy->autoneg = AUTONEG_DISABLE;
 	phy->speed = speed;
 	phy->duplex = fd;
-	return phy_start_aneg(dev->phy_dev);
+	return emac_mdio_phy_start_aneg(phy, dev->phy_dev);
 }
 
 static int emac_mdio_poll_link(struct mii_phy *phy)
@@ -2509,16 +2528,17 @@ static int emac_mdio_read_link(struct mii_phy *phy)
 {
 	struct net_device *ndev = phy->dev;
 	struct emac_instance *dev = netdev_priv(ndev);
+	struct phy_device *phy_dev = dev->phy_dev;
 	int res;
 
-	res = phy_read_status(dev->phy_dev);
+	res = phy_read_status(phy_dev);
 	if (res)
 		return res;
 
-	dev->phy.speed = phy->speed;
-	dev->phy.duplex = phy->duplex;
-	dev->phy.pause = phy->pause;
-	dev->phy.asym_pause = phy->asym_pause;
+	phy->speed = phy_dev->speed;
+	phy->duplex = phy_dev->duplex;
+	phy->pause = phy_dev->pause;
+	phy->asym_pause = phy_dev->asym_pause;
 	return 0;
 }
 
@@ -2528,13 +2548,6 @@ static int emac_mdio_init_phy(struct mii_phy *phy)
 	struct emac_instance *dev = netdev_priv(ndev);
 
 	phy_start(dev->phy_dev);
-	dev->phy.autoneg = phy->autoneg;
-	dev->phy.speed = phy->speed;
-	dev->phy.duplex = phy->duplex;
-	dev->phy.advertising = phy->advertising;
-	dev->phy.pause = phy->pause;
-	dev->phy.asym_pause = phy->asym_pause;
-
 	return phy_init_hw(dev->phy_dev);
 }
 
diff --git a/drivers/net/ethernet/ibm/emac/phy.c b/drivers/net/ethernet/ibm/emac/phy.c
index 5b88cc690c22..35865d05fccd 100644
--- a/drivers/net/ethernet/ibm/emac/phy.c
+++ b/drivers/net/ethernet/ibm/emac/phy.c
@@ -276,7 +276,7 @@ static int genmii_read_link(struct mii_phy *phy)
 }
 
 /* Generic implementation for most 10/100/1000 PHYs */
-static struct mii_phy_ops generic_phy_ops = {
+static const struct mii_phy_ops generic_phy_ops = {
 	.setup_aneg	= genmii_setup_aneg,
 	.setup_forced	= genmii_setup_forced,
 	.poll_link	= genmii_poll_link,
@@ -340,7 +340,7 @@ static int cis8201_init(struct mii_phy *phy)
 	return 0;
 }
 
-static struct mii_phy_ops cis8201_phy_ops = {
+static const struct mii_phy_ops cis8201_phy_ops = {
 	.init		= cis8201_init,
 	.setup_aneg	= genmii_setup_aneg,
 	.setup_forced	= genmii_setup_forced,
@@ -420,7 +420,7 @@ static int et1011c_init(struct mii_phy *phy)
 	return 0;
 }
 
-static struct mii_phy_ops et1011c_phy_ops = {
+static const struct mii_phy_ops et1011c_phy_ops = {
 	.init		= et1011c_init,
 	.setup_aneg	= genmii_setup_aneg,
 	.setup_forced	= genmii_setup_forced,
@@ -439,7 +439,7 @@ static struct mii_phy_def et1011c_phy_def = {
 
 
 
-static struct mii_phy_ops m88e1111_phy_ops = {
+static const struct mii_phy_ops m88e1111_phy_ops = {
 	.init		= m88e1111_init,
 	.setup_aneg	= genmii_setup_aneg,
 	.setup_forced	= genmii_setup_forced,
@@ -455,7 +455,7 @@ static struct mii_phy_def m88e1111_phy_def = {
 	.ops		= &m88e1111_phy_ops,
 };
 
-static struct mii_phy_ops m88e1112_phy_ops = {
+static const struct mii_phy_ops m88e1112_phy_ops = {
 	.init		= m88e1112_init,
 	.setup_aneg	= genmii_setup_aneg,
 	.setup_forced	= genmii_setup_forced,
@@ -480,7 +480,7 @@ static int ar8035_init(struct mii_phy *phy)
 	return 0;
 }
 
-static struct mii_phy_ops ar8035_phy_ops = {
+static const struct mii_phy_ops ar8035_phy_ops = {
 	.init		= ar8035_init,
 	.setup_aneg	= genmii_setup_aneg,
 	.setup_forced	= genmii_setup_forced,
diff --git a/drivers/net/ethernet/ibm/ibmveth.c b/drivers/net/ethernet/ibm/ibmveth.c
index 72ab7b6bf20b..3e0a695537e2 100644
--- a/drivers/net/ethernet/ibm/ibmveth.c
+++ b/drivers/net/ethernet/ibm/ibmveth.c
@@ -46,6 +46,8 @@
 #include <asm/vio.h>
 #include <asm/iommu.h>
 #include <asm/firmware.h>
+#include <net/tcp.h>
+#include <net/ip6_checksum.h>
 
 #include "ibmveth.h"
 
@@ -808,8 +810,7 @@ static int ibmveth_set_csum_offload(struct net_device *dev, u32 data)
 
 	ret = h_illan_attributes(adapter->vdev->unit_address, 0, 0, &ret_attr);
 
-	if (ret == H_SUCCESS && !(ret_attr & IBMVETH_ILLAN_ACTIVE_TRUNK) &&
-	    !(ret_attr & IBMVETH_ILLAN_TRUNK_PRI_MASK) &&
+	if (ret == H_SUCCESS &&
 	    (ret_attr & IBMVETH_ILLAN_PADDED_PKT_CSUM)) {
 		ret4 = h_illan_attributes(adapter->vdev->unit_address, clr_attr,
 					 set_attr, &ret_attr);
@@ -1040,6 +1041,15 @@ static netdev_tx_t ibmveth_start_xmit(struct sk_buff *skb,
 	dma_addr_t dma_addr;
 	unsigned long mss = 0;
 
+	/* veth doesn't handle frag_list, so linearize the skb.
+	 * When GRO is enabled SKB's can have frag_list.
+	 */
+	if (adapter->is_active_trunk &&
+	    skb_has_frag_list(skb) && __skb_linearize(skb)) {
+		netdev->stats.tx_dropped++;
+		goto out;
+	}
+
 	/*
 	 * veth handles a maximum of 6 segments including the header, so
 	 * we have to linearize the skb if there are more than this.
@@ -1064,9 +1074,6 @@ static netdev_tx_t ibmveth_start_xmit(struct sk_buff *skb,
 
 	desc_flags = IBMVETH_BUF_VALID;
 
-	if (skb_is_gso(skb) && adapter->fw_large_send_support)
-		desc_flags |= IBMVETH_BUF_LRG_SND;
-
 	if (skb->ip_summed == CHECKSUM_PARTIAL) {
 		unsigned char *buf = skb_transport_header(skb) +
 						skb->csum_offset;
@@ -1076,6 +1083,9 @@ static netdev_tx_t ibmveth_start_xmit(struct sk_buff *skb,
 		/* Need to zero out the checksum */
 		buf[0] = 0;
 		buf[1] = 0;
+
+		if (skb_is_gso(skb) && adapter->fw_large_send_support)
+			desc_flags |= IBMVETH_BUF_LRG_SND;
 	}
 
 retry_bounce:
@@ -1128,7 +1138,7 @@ retry_bounce:
 		descs[i+1].fields.address = dma_addr;
 	}
 
-	if (skb_is_gso(skb)) {
+	if (skb->ip_summed == CHECKSUM_PARTIAL && skb_is_gso(skb)) {
 		if (adapter->fw_large_send_support) {
 			mss = (unsigned long)skb_shinfo(skb)->gso_size;
 			adapter->tx_large_packets++;
@@ -1232,6 +1242,71 @@ static void ibmveth_rx_mss_helper(struct sk_buff *skb, u16 mss, int lrg_pkt)
 	}
 }
 
+static void ibmveth_rx_csum_helper(struct sk_buff *skb,
+				   struct ibmveth_adapter *adapter)
+{
+	struct iphdr *iph = NULL;
+	struct ipv6hdr *iph6 = NULL;
+	__be16 skb_proto = 0;
+	u16 iphlen = 0;
+	u16 iph_proto = 0;
+	u16 tcphdrlen = 0;
+
+	skb_proto = be16_to_cpu(skb->protocol);
+
+	if (skb_proto == ETH_P_IP) {
+		iph = (struct iphdr *)skb->data;
+
+		/* If the IP checksum is not offloaded and if the packet
+		 *  is large send, the checksum must be rebuilt.
+		 */
+		if (iph->check == 0xffff) {
+			iph->check = 0;
+			iph->check = ip_fast_csum((unsigned char *)iph,
+						  iph->ihl);
+		}
+
+		iphlen = iph->ihl * 4;
+		iph_proto = iph->protocol;
+	} else if (skb_proto == ETH_P_IPV6) {
+		iph6 = (struct ipv6hdr *)skb->data;
+		iphlen = sizeof(struct ipv6hdr);
+		iph_proto = iph6->nexthdr;
+	}
+
+	/* In OVS environment, when a flow is not cached, specifically for a
+	 * new TCP connection, the first packet information is passed up
+	 * the user space for finding a flow. During this process, OVS computes
+	 * checksum on the first packet when CHECKSUM_PARTIAL flag is set.
+	 *
+	 * Given that we zeroed out TCP checksum field in transmit path
+	 * (refer ibmveth_start_xmit routine) as we set "no checksum bit",
+	 * OVS computed checksum will be incorrect w/o TCP pseudo checksum
+	 * in the packet. This leads to OVS dropping the packet and hence
+	 * TCP retransmissions are seen.
+	 *
+	 * So, re-compute TCP pseudo header checksum.
+	 */
+	if (iph_proto == IPPROTO_TCP && adapter->is_active_trunk) {
+		struct tcphdr *tcph = (struct tcphdr *)(skb->data + iphlen);
+
+		tcphdrlen = skb->len - iphlen;
+
+		/* Recompute TCP pseudo header checksum */
+		if (skb_proto == ETH_P_IP)
+			tcph->check = ~csum_tcpudp_magic(iph->saddr,
+					iph->daddr, tcphdrlen, iph_proto, 0);
+		else if (skb_proto == ETH_P_IPV6)
+			tcph->check = ~csum_ipv6_magic(&iph6->saddr,
+					&iph6->daddr, tcphdrlen, iph_proto, 0);
+
+		/* Setup SKB fields for checksum offload */
+		skb_partial_csum_set(skb, iphlen,
+				     offsetof(struct tcphdr, check));
+		skb_reset_network_header(skb);
+	}
+}
+
 static int ibmveth_poll(struct napi_struct *napi, int budget)
 {
 	struct ibmveth_adapter *adapter =
@@ -1239,7 +1314,6 @@ static int ibmveth_poll(struct napi_struct *napi, int budget)
 	struct net_device *netdev = adapter->netdev;
 	int frames_processed = 0;
 	unsigned long lpar_rc;
-	struct iphdr *iph;
 	u16 mss = 0;
 
 restart_poll:
@@ -1297,17 +1371,7 @@ restart_poll:
 
 			if (csum_good) {
 				skb->ip_summed = CHECKSUM_UNNECESSARY;
-				if (be16_to_cpu(skb->protocol) == ETH_P_IP) {
-					iph = (struct iphdr *)skb->data;
-
-					/* If the IP checksum is not offloaded and if the packet
-					 *  is large send, the checksum must be rebuilt.
-					 */
-					if (iph->check == 0xffff) {
-						iph->check = 0;
-						iph->check = ip_fast_csum((unsigned char *)iph, iph->ihl);
-					}
-				}
+				ibmveth_rx_csum_helper(skb, adapter);
 			}
 
 			if (length > netdev->mtu + ETH_HLEN) {
@@ -1626,6 +1690,13 @@ static int ibmveth_probe(struct vio_dev *dev, const struct vio_device_id *id)
 		netdev->hw_features |= NETIF_F_TSO;
 	}
 
+	adapter->is_active_trunk = false;
+	if (ret == H_SUCCESS && (ret_attr & IBMVETH_ILLAN_ACTIVE_TRUNK)) {
+		adapter->is_active_trunk = true;
+		netdev->hw_features |= NETIF_F_FRAGLIST;
+		netdev->features |= NETIF_F_FRAGLIST;
+	}
+
 	netdev->min_mtu = IBMVETH_MIN_MTU;
 	netdev->max_mtu = ETH_MAX_MTU;
 
@@ -1843,7 +1914,7 @@ static struct vio_device_id ibmveth_device_table[] = {
 };
 MODULE_DEVICE_TABLE(vio, ibmveth_device_table);
 
-static struct dev_pm_ops ibmveth_pm_ops = {
+static const struct dev_pm_ops ibmveth_pm_ops = {
 	.resume = ibmveth_resume
 };
 
diff --git a/drivers/net/ethernet/ibm/ibmveth.h b/drivers/net/ethernet/ibm/ibmveth.h
index ed8780cca982..01c587fc02c7 100644
--- a/drivers/net/ethernet/ibm/ibmveth.h
+++ b/drivers/net/ethernet/ibm/ibmveth.h
@@ -156,6 +156,7 @@ struct ibmveth_adapter {
     int pool_config;
     int rx_csum;
     int large_send;
+    bool is_active_trunk;
     void *bounce_buffer;
     dma_addr_t bounce_buffer_dma;
 
diff --git a/drivers/net/ethernet/ibm/ibmvnic.c b/drivers/net/ethernet/ibm/ibmvnic.c
index a93757c255f7..a3e694679635 100644
--- a/drivers/net/ethernet/ibm/ibmvnic.c
+++ b/drivers/net/ethernet/ibm/ibmvnic.c
@@ -183,6 +183,12 @@ static int alloc_long_term_buff(struct ibmvnic_adapter *adapter,
 	send_request_map(adapter, ltb->addr,
 			 ltb->size, ltb->map_id);
 	wait_for_completion(&adapter->fw_done);
+
+	if (adapter->fw_done_rc) {
+		dev_err(dev, "Couldn't map long term buffer,rc = %d\n",
+			adapter->fw_done_rc);
+		return -1;
+	}
 	return 0;
 }
 
@@ -200,6 +206,33 @@ static void free_long_term_buff(struct ibmvnic_adapter *adapter,
 	dma_free_coherent(dev, ltb->size, ltb->buff, ltb->addr);
 }
 
+static int reset_long_term_buff(struct ibmvnic_adapter *adapter,
+				struct ibmvnic_long_term_buff *ltb)
+{
+	memset(ltb->buff, 0, ltb->size);
+
+	init_completion(&adapter->fw_done);
+	send_request_map(adapter, ltb->addr, ltb->size, ltb->map_id);
+	wait_for_completion(&adapter->fw_done);
+
+	if (adapter->fw_done_rc) {
+		dev_info(&adapter->vdev->dev,
+			 "Reset failed, attempting to free and reallocate buffer\n");
+		free_long_term_buff(adapter, ltb);
+		return alloc_long_term_buff(adapter, ltb, ltb->size);
+	}
+	return 0;
+}
+
+static void deactivate_rx_pools(struct ibmvnic_adapter *adapter)
+{
+	int i;
+
+	for (i = 0; i < be32_to_cpu(adapter->login_rsp_buf->num_rxadd_subcrqs);
+	     i++)
+		adapter->rx_pool[i].active = 0;
+}
+
 static void replenish_rx_pool(struct ibmvnic_adapter *adapter,
 			      struct ibmvnic_rx_pool *pool)
 {
@@ -217,6 +250,9 @@ static void replenish_rx_pool(struct ibmvnic_adapter *adapter,
 	int index;
 	int i;
 
+	if (!pool->active)
+		return;
+
 	handle_array = (u64 *)((u8 *)(adapter->login_rsp_buf) +
 				      be32_to_cpu(adapter->login_rsp_buf->
 				      off_rxadd_subcrqs));
@@ -287,6 +323,15 @@ failure:
 	dev_kfree_skb_any(skb);
 	adapter->replenish_add_buff_failure++;
 	atomic_add(buffers_added, &pool->available);
+
+	if (lpar_rc == H_CLOSED) {
+		/* Disable buffer pool replenishment and report carrier off if
+		 * queue is closed. Firmware guarantees that a signal will
+		 * be sent to the driver, triggering a reset.
+		 */
+		deactivate_rx_pools(adapter);
+		netif_carrier_off(adapter->netdev);
+	}
 }
 
 static void replenish_pools(struct ibmvnic_adapter *adapter)
@@ -331,6 +376,35 @@ static int init_stats_token(struct ibmvnic_adapter *adapter)
 	return 0;
 }
 
+static int reset_rx_pools(struct ibmvnic_adapter *adapter)
+{
+	struct ibmvnic_rx_pool *rx_pool;
+	int rx_scrqs;
+	int i, j, rc;
+
+	rx_scrqs = be32_to_cpu(adapter->login_rsp_buf->num_rxadd_subcrqs);
+	for (i = 0; i < rx_scrqs; i++) {
+		rx_pool = &adapter->rx_pool[i];
+
+		rc = reset_long_term_buff(adapter, &rx_pool->long_term_buff);
+		if (rc)
+			return rc;
+
+		for (j = 0; j < rx_pool->size; j++)
+			rx_pool->free_map[j] = j;
+
+		memset(rx_pool->rx_buff, 0,
+		       rx_pool->size * sizeof(struct ibmvnic_rx_buff));
+
+		atomic_set(&rx_pool->available, 0);
+		rx_pool->next_alloc = 0;
+		rx_pool->next_free = 0;
+		rx_pool->active = 1;
+	}
+
+	return 0;
+}
+
 static void release_rx_pools(struct ibmvnic_adapter *adapter)
 {
 	struct ibmvnic_rx_pool *rx_pool;
@@ -432,6 +506,34 @@ static int init_rx_pools(struct net_device *netdev)
 	return 0;
 }
 
+static int reset_tx_pools(struct ibmvnic_adapter *adapter)
+{
+	struct ibmvnic_tx_pool *tx_pool;
+	int tx_scrqs;
+	int i, j, rc;
+
+	tx_scrqs = be32_to_cpu(adapter->login_rsp_buf->num_txsubm_subcrqs);
+	for (i = 0; i < tx_scrqs; i++) {
+		tx_pool = &adapter->tx_pool[i];
+
+		rc = reset_long_term_buff(adapter, &tx_pool->long_term_buff);
+		if (rc)
+			return rc;
+
+		memset(tx_pool->tx_buff, 0,
+		       adapter->req_tx_entries_per_subcrq *
+		       sizeof(struct ibmvnic_tx_buff));
+
+		for (j = 0; j < adapter->req_tx_entries_per_subcrq; j++)
+			tx_pool->free_map[j] = j;
+
+		tx_pool->consumer_index = 0;
+		tx_pool->producer_index = 0;
+	}
+
+	return 0;
+}
+
 static void release_tx_pools(struct ibmvnic_adapter *adapter)
 {
 	struct ibmvnic_tx_pool *tx_pool;
@@ -518,6 +620,32 @@ static void release_error_buffers(struct ibmvnic_adapter *adapter)
 	spin_unlock_irqrestore(&adapter->error_list_lock, flags);
 }
 
+static void ibmvnic_napi_enable(struct ibmvnic_adapter *adapter)
+{
+	int i;
+
+	if (adapter->napi_enabled)
+		return;
+
+	for (i = 0; i < adapter->req_rx_queues; i++)
+		napi_enable(&adapter->napi[i]);
+
+	adapter->napi_enabled = true;
+}
+
+static void ibmvnic_napi_disable(struct ibmvnic_adapter *adapter)
+{
+	int i;
+
+	if (!adapter->napi_enabled)
+		return;
+
+	for (i = 0; i < adapter->req_rx_queues; i++)
+		napi_disable(&adapter->napi[i]);
+
+	adapter->napi_enabled = false;
+}
+
 static int ibmvnic_login(struct net_device *netdev)
 {
 	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
@@ -635,12 +763,6 @@ static int init_resources(struct ibmvnic_adapter *adapter)
 	if (rc)
 		return rc;
 
-	rc = init_sub_crq_irqs(adapter);
-	if (rc) {
-		netdev_err(netdev, "failed to initialize sub crq irqs\n");
-		return -1;
-	}
-
 	rc = init_stats_token(adapter);
 	if (rc)
 		return rc;
@@ -674,9 +796,7 @@ static int __ibmvnic_open(struct net_device *netdev)
 
 	adapter->state = VNIC_OPENING;
 	replenish_pools(adapter);
-
-	for (i = 0; i < adapter->req_rx_queues; i++)
-		napi_enable(&adapter->napi[i]);
+	ibmvnic_napi_enable(adapter);
 
 	/* We're ready to receive frames, enable the sub-crq interrupts and
 	 * set the logical link state to up
@@ -778,14 +898,14 @@ static int __ibmvnic_close(struct net_device *netdev)
 	int i;
 
 	adapter->state = VNIC_CLOSING;
-	netif_tx_stop_all_queues(netdev);
 
-	if (adapter->napi) {
-		for (i = 0; i < adapter->req_rx_queues; i++)
-			napi_disable(&adapter->napi[i]);
-	}
+	/* ensure that transmissions are stopped if called by do_reset */
+	if (adapter->resetting)
+		netif_tx_disable(netdev);
+	else
+		netif_tx_stop_all_queues(netdev);
 
-	clean_tx_pools(adapter);
+	ibmvnic_napi_disable(adapter);
 
 	if (adapter->tx_scrq) {
 		for (i = 0; i < adapter->req_tx_queues; i++)
@@ -814,6 +934,7 @@ static int __ibmvnic_close(struct net_device *netdev)
 		}
 	}
 
+	clean_tx_pools(adapter);
 	adapter->state = VNIC_CLOSED;
 	return rc;
 }
@@ -1092,8 +1213,14 @@ static int ibmvnic_xmit(struct sk_buff *skb, struct net_device *netdev)
 		dev_kfree_skb_any(skb);
 		tx_buff->skb = NULL;
 
-		if (lpar_rc == H_CLOSED)
-			netif_stop_subqueue(netdev, queue_num);
+		if (lpar_rc == H_CLOSED) {
+			/* Disable TX and report carrier off if queue is closed.
+			 * Firmware guarantees that a signal will be sent to the
+			 * driver, triggering a reset or some other action.
+			 */
+			netif_tx_stop_all_queues(netdev);
+			netif_carrier_off(netdev);
+		}
 
 		tx_send_failed++;
 		tx_dropped++;
@@ -1206,37 +1333,39 @@ static int do_reset(struct ibmvnic_adapter *adapter,
 	if (rc)
 		return rc;
 
-	/* remove the closed state so when we call open it appears
-	 * we are coming from the probed state.
-	 */
-	adapter->state = VNIC_PROBED;
+	if (adapter->reset_reason != VNIC_RESET_NON_FATAL) {
+		/* remove the closed state so when we call open it appears
+		 * we are coming from the probed state.
+		 */
+		adapter->state = VNIC_PROBED;
 
-	release_resources(adapter);
-	release_sub_crqs(adapter);
-	release_crq_queue(adapter);
+		rc = ibmvnic_init(adapter);
+		if (rc)
+			return 0;
 
-	rc = ibmvnic_init(adapter);
-	if (rc)
-		return 0;
+		/* If the adapter was in PROBE state prior to the reset,
+		 * exit here.
+		 */
+		if (reset_state == VNIC_PROBED)
+			return 0;
 
-	/* If the adapter was in PROBE state prior to the reset, exit here. */
-	if (reset_state == VNIC_PROBED)
-		return 0;
+		rc = ibmvnic_login(netdev);
+		if (rc) {
+			adapter->state = VNIC_PROBED;
+			return 0;
+		}
 
-	rc = ibmvnic_login(netdev);
-	if (rc) {
-		adapter->state = VNIC_PROBED;
-		return 0;
-	}
+		rc = reset_tx_pools(adapter);
+		if (rc)
+			return rc;
 
-	rtnl_lock();
-	rc = init_resources(adapter);
-	rtnl_unlock();
-	if (rc)
-		return rc;
+		rc = reset_rx_pools(adapter);
+		if (rc)
+			return rc;
 
-	if (reset_state == VNIC_CLOSED)
-		return 0;
+		if (reset_state == VNIC_CLOSED)
+			return 0;
+	}
 
 	rc = __ibmvnic_open(netdev);
 	if (rc) {
@@ -1254,6 +1383,9 @@ static int do_reset(struct ibmvnic_adapter *adapter,
 	for (i = 0; i < adapter->req_rx_queues; i++)
 		napi_schedule(&adapter->napi[i]);
 
+	if (adapter->reset_reason != VNIC_RESET_FAILOVER)
+		netdev_notify_peers(netdev);
+
 	return 0;
 }
 
@@ -1313,6 +1445,7 @@ static void __ibmvnic_reset(struct work_struct *work)
 
 	if (rc) {
 		free_all_rwi(adapter);
+		mutex_unlock(&adapter->reset_lock);
 		return;
 	}
 
@@ -1333,6 +1466,12 @@ static void ibmvnic_reset(struct ibmvnic_adapter *adapter,
 		return;
 	}
 
+	if (adapter->state == VNIC_PROBING) {
+		netdev_warn(netdev, "Adapter reset during probe\n");
+		adapter->init_done_rc = EAGAIN;
+		return;
+	}
+
 	mutex_lock(&adapter->rwi_lock);
 
 	list_for_each(entry, &adapter->rwi_list) {
@@ -1383,6 +1522,7 @@ static int ibmvnic_poll(struct napi_struct *napi, int budget)
 	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
 	int scrq_num = (int)(napi - adapter->napi);
 	int frames_processed = 0;
+
 restart_poll:
 	while (frames_processed < budget) {
 		struct sk_buff *skb;
@@ -1392,6 +1532,12 @@ restart_poll:
 		u16 offset;
 		u8 flags = 0;
 
+		if (unlikely(adapter->resetting)) {
+			enable_scrq_irq(adapter, adapter->rx_scrq[scrq_num]);
+			napi_complete_done(napi, frames_processed);
+			return frames_processed;
+		}
+
 		if (!pending_scrq(adapter, adapter->rx_scrq[scrq_num]))
 			break;
 		next = ibmvnic_next_scrq(adapter, adapter->rx_scrq[scrq_num]);
@@ -1441,7 +1587,9 @@ restart_poll:
 		netdev->stats.rx_bytes += length;
 		frames_processed++;
 	}
-	replenish_rx_pool(adapter, &adapter->rx_pool[scrq_num]);
+
+	if (adapter->state != VNIC_CLOSING)
+		replenish_rx_pool(adapter, &adapter->rx_pool[scrq_num]);
 
 	if (frames_processed < budget) {
 		enable_scrq_irq(adapter, adapter->rx_scrq[scrq_num]);
@@ -1468,6 +1616,11 @@ static void ibmvnic_netpoll_controller(struct net_device *dev)
 }
 #endif
 
+static int ibmvnic_change_mtu(struct net_device *netdev, int new_mtu)
+{
+	return -EOPNOTSUPP;
+}
+
 static const struct net_device_ops ibmvnic_netdev_ops = {
 	.ndo_open		= ibmvnic_open,
 	.ndo_stop		= ibmvnic_close,
@@ -1479,6 +1632,7 @@ static const struct net_device_ops ibmvnic_netdev_ops = {
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= ibmvnic_netpoll_controller,
 #endif
+	.ndo_change_mtu		= ibmvnic_change_mtu,
 };
 
 /* ethtool functions */
@@ -1608,6 +1762,44 @@ static const struct ethtool_ops ibmvnic_ethtool_ops = {
 
 /* Routines for managing CRQs/sCRQs  */
 
+static int reset_one_sub_crq_queue(struct ibmvnic_adapter *adapter,
+				   struct ibmvnic_sub_crq_queue *scrq)
+{
+	int rc;
+
+	if (scrq->irq) {
+		free_irq(scrq->irq, scrq);
+		irq_dispose_mapping(scrq->irq);
+		scrq->irq = 0;
+	}
+
+	memset(scrq->msgs, 0, 4 * PAGE_SIZE);
+	scrq->cur = 0;
+
+	rc = h_reg_sub_crq(adapter->vdev->unit_address, scrq->msg_token,
+			   4 * PAGE_SIZE, &scrq->crq_num, &scrq->hw_irq);
+	return rc;
+}
+
+static int reset_sub_crq_queues(struct ibmvnic_adapter *adapter)
+{
+	int i, rc;
+
+	for (i = 0; i < adapter->req_tx_queues; i++) {
+		rc = reset_one_sub_crq_queue(adapter, adapter->tx_scrq[i]);
+		if (rc)
+			return rc;
+	}
+
+	for (i = 0; i < adapter->req_rx_queues; i++) {
+		rc = reset_one_sub_crq_queue(adapter, adapter->rx_scrq[i]);
+		if (rc)
+			return rc;
+	}
+
+	return rc;
+}
+
 static void release_sub_crq_queue(struct ibmvnic_adapter *adapter,
 				  struct ibmvnic_sub_crq_queue *scrq)
 {
@@ -2103,8 +2295,7 @@ static int pending_scrq(struct ibmvnic_adapter *adapter,
 {
 	union sub_crq *entry = &scrq->msgs[scrq->cur];
 
-	if (entry->generic.first & IBMVNIC_CRQ_CMD_RSP ||
-	    adapter->state == VNIC_CLOSING)
+	if (entry->generic.first & IBMVNIC_CRQ_CMD_RSP)
 		return 1;
 	else
 		return 0;
@@ -2742,6 +2933,8 @@ static void handle_error_indication(union ibmvnic_crq *crq,
 
 	if (crq->error_indication.flags & IBMVNIC_FATAL_ERROR)
 		ibmvnic_reset(adapter, VNIC_RESET_FATAL);
+	else
+		ibmvnic_reset(adapter, VNIC_RESET_NON_FATAL);
 }
 
 static void handle_change_mac_rsp(union ibmvnic_crq *crq,
@@ -2893,36 +3086,6 @@ static int handle_login_rsp(union ibmvnic_crq *login_rsp_crq,
 	return 0;
 }
 
-static void handle_request_map_rsp(union ibmvnic_crq *crq,
-				   struct ibmvnic_adapter *adapter)
-{
-	struct device *dev = &adapter->vdev->dev;
-	u8 map_id = crq->request_map_rsp.map_id;
-	int tx_subcrqs;
-	int rx_subcrqs;
-	long rc;
-	int i;
-
-	tx_subcrqs = be32_to_cpu(adapter->login_rsp_buf->num_txsubm_subcrqs);
-	rx_subcrqs = be32_to_cpu(adapter->login_rsp_buf->num_rxadd_subcrqs);
-
-	rc = crq->request_map_rsp.rc.code;
-	if (rc) {
-		dev_err(dev, "Error %ld in REQUEST_MAP_RSP\n", rc);
-		adapter->map_id--;
-		/* need to find and zero tx/rx_pool map_id */
-		for (i = 0; i < tx_subcrqs; i++) {
-			if (adapter->tx_pool[i].long_term_buff.map_id == map_id)
-				adapter->tx_pool[i].long_term_buff.map_id = 0;
-		}
-		for (i = 0; i < rx_subcrqs; i++) {
-			if (adapter->rx_pool[i].long_term_buff.map_id == map_id)
-				adapter->rx_pool[i].long_term_buff.map_id = 0;
-		}
-	}
-	complete(&adapter->fw_done);
-}
-
 static void handle_request_unmap_rsp(union ibmvnic_crq *crq,
 				     struct ibmvnic_adapter *adapter)
 {
@@ -3147,6 +3310,8 @@ static void ibmvnic_handle_crq(union ibmvnic_crq *crq,
 		switch (gen_crq->cmd) {
 		case IBMVNIC_CRQ_INIT:
 			dev_info(dev, "Partner initialized\n");
+			adapter->from_passive_init = true;
+			complete(&adapter->init_done);
 			break;
 		case IBMVNIC_CRQ_INIT_COMPLETE:
 			dev_info(dev, "Partner initialization complete\n");
@@ -3201,7 +3366,8 @@ static void ibmvnic_handle_crq(union ibmvnic_crq *crq,
 		handle_query_map_rsp(crq, adapter);
 		break;
 	case REQUEST_MAP_RSP:
-		handle_request_map_rsp(crq, adapter);
+		adapter->fw_done_rc = crq->request_map_rsp.rc.code;
+		complete(&adapter->fw_done);
 		break;
 	case REQUEST_UNMAP_RSP:
 		handle_request_unmap_rsp(crq, adapter);
@@ -3455,29 +3621,61 @@ static int ibmvnic_init(struct ibmvnic_adapter *adapter)
 	unsigned long timeout = msecs_to_jiffies(30000);
 	int rc;
 
-	rc = init_crq_queue(adapter);
+	if (adapter->resetting) {
+		rc = ibmvnic_reset_crq(adapter);
+		if (!rc)
+			rc = vio_enable_interrupts(adapter->vdev);
+	} else {
+		rc = init_crq_queue(adapter);
+	}
+
 	if (rc) {
 		dev_err(dev, "Couldn't initialize crq. rc=%d\n", rc);
 		return rc;
 	}
 
+	adapter->from_passive_init = false;
+
 	init_completion(&adapter->init_done);
+	adapter->init_done_rc = 0;
 	ibmvnic_send_crq_init(adapter);
 	if (!wait_for_completion_timeout(&adapter->init_done, timeout)) {
 		dev_err(dev, "Initialization sequence timed out\n");
+		return -1;
+	}
+
+	if (adapter->init_done_rc) {
 		release_crq_queue(adapter);
+		return adapter->init_done_rc;
+	}
+
+	if (adapter->from_passive_init) {
+		adapter->state = VNIC_OPEN;
+		adapter->from_passive_init = false;
 		return -1;
 	}
 
-	rc = init_sub_crqs(adapter);
+	if (adapter->resetting)
+		rc = reset_sub_crq_queues(adapter);
+	else
+		rc = init_sub_crqs(adapter);
 	if (rc) {
 		dev_err(dev, "Initialization of sub crqs failed\n");
 		release_crq_queue(adapter);
+		return rc;
+	}
+
+	rc = init_sub_crq_irqs(adapter);
+	if (rc) {
+		dev_err(dev, "Failed to initialize sub crq irqs\n");
+		release_crq_queue(adapter);
 	}
 
 	return rc;
 }
 
+static struct device_attribute dev_attr_failover;
+
 static int ibmvnic_probe(struct vio_dev *dev, const struct vio_device_id *id)
 {
 	struct ibmvnic_adapter *adapter;
@@ -3526,17 +3724,26 @@ static int ibmvnic_probe(struct vio_dev *dev, const struct vio_device_id *id)
 	mutex_init(&adapter->rwi_lock);
 	adapter->resetting = false;
 
-	rc = ibmvnic_init(adapter);
+	do {
+		rc = ibmvnic_init(adapter);
+		if (rc && rc != EAGAIN) {
+			free_netdev(netdev);
+			return rc;
+		}
+	} while (rc == EAGAIN);
+
+	netdev->mtu = adapter->req_mtu - ETH_HLEN;
+
+	rc = device_create_file(&dev->dev, &dev_attr_failover);
 	if (rc) {
 		free_netdev(netdev);
 		return rc;
 	}
 
-	netdev->mtu = adapter->req_mtu - ETH_HLEN;
-
 	rc = register_netdev(netdev);
 	if (rc) {
 		dev_err(&dev->dev, "failed to register netdev rc=%d\n", rc);
+		device_remove_file(&dev->dev, &dev_attr_failover);
 		free_netdev(netdev);
 		return rc;
 	}
@@ -3562,12 +3769,49 @@ static int ibmvnic_remove(struct vio_dev *dev)
 	adapter->state = VNIC_REMOVED;
 
 	mutex_unlock(&adapter->reset_lock);
+	device_remove_file(&dev->dev, &dev_attr_failover);
 	free_netdev(netdev);
 	dev_set_drvdata(&dev->dev, NULL);
 
 	return 0;
 }
 
+static ssize_t failover_store(struct device *dev, struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	struct net_device *netdev = dev_get_drvdata(dev);
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+	unsigned long retbuf[PLPAR_HCALL_BUFSIZE];
+	__be64 session_token;
+	long rc;
+
+	if (!sysfs_streq(buf, "1"))
+		return -EINVAL;
+
+	rc = plpar_hcall(H_VIOCTL, retbuf, adapter->vdev->unit_address,
+			 H_GET_SESSION_TOKEN, 0, 0, 0);
+	if (rc) {
+		netdev_err(netdev, "Couldn't retrieve session token, rc %ld\n",
+			   rc);
+		return -EINVAL;
+	}
+
+	session_token = (__be64)retbuf[0];
+	netdev_dbg(netdev, "Initiating client failover, session id %llx\n",
+		   be64_to_cpu(session_token));
+	rc = plpar_hcall_norets(H_VIOCTL, adapter->vdev->unit_address,
+				H_SESSION_ERR_DETECTED, session_token, 0, 0);
+	if (rc) {
+		netdev_err(netdev, "Client initiated failover failed, rc %ld\n",
+			   rc);
+		return -EINVAL;
+	}
+
+	return count;
+}
+
+static DEVICE_ATTR(failover, 0200, NULL, failover_store);
+
 static unsigned long ibmvnic_get_desired_dma(struct vio_dev *vdev)
 {
 	struct net_device *netdev = dev_get_drvdata(&vdev->dev);
@@ -3604,6 +3848,9 @@ static int ibmvnic_resume(struct device *dev)
 	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
 	int i;
 
+	if (adapter->state != VNIC_OPEN)
+		return 0;
+
 	/* kick the interrupt handlers just in case we lost an interrupt */
 	for (i = 0; i < adapter->req_rx_queues; i++)
 		ibmvnic_interrupt_rx(adapter->rx_scrq[i]->irq,
diff --git a/drivers/net/ethernet/ibm/ibmvnic.h b/drivers/net/ethernet/ibm/ibmvnic.h
index 4702b48cfa44..8eff6e15f4bb 100644
--- a/drivers/net/ethernet/ibm/ibmvnic.h
+++ b/drivers/net/ethernet/ibm/ibmvnic.h
@@ -595,7 +595,7 @@ struct ibmvnic_request_map_rsp {
 	u8 cmd;
 	u8 reserved1;
 	u8 map_id;
-	u8 reserved2[4];
+	u8 reserved2[8];
 	struct ibmvnic_rc rc;
 } __packed __aligned(8);
 
@@ -925,6 +925,7 @@ enum vnic_state {VNIC_PROBING = 1,
 enum ibmvnic_reset_reason {VNIC_RESET_FAILOVER = 1,
 			   VNIC_RESET_MOBILITY,
 			   VNIC_RESET_FATAL,
+			   VNIC_RESET_NON_FATAL,
 			   VNIC_RESET_TIMEOUT};
 
 struct ibmvnic_rwi {
@@ -987,6 +988,7 @@ struct ibmvnic_adapter {
 	spinlock_t error_list_lock;
 
 	struct completion fw_done;
+	int fw_done_rc;
 
 	/* partner capabilities */
 	u64 min_tx_queues;
@@ -1031,4 +1033,5 @@ struct ibmvnic_adapter {
 	struct list_head rwi_list;
 	struct work_struct ibmvnic_reset;
 	bool resetting;
+	bool napi_enabled, from_passive_init;
 };
diff --git a/drivers/net/ethernet/intel/Kconfig b/drivers/net/ethernet/intel/Kconfig
index 1542a2158e96..1feb54b6d92e 100644
--- a/drivers/net/ethernet/intel/Kconfig
+++ b/drivers/net/ethernet/intel/Kconfig
@@ -236,12 +236,14 @@ config I40E_DCB
 	  If unsure, say N.
 
 config I40EVF
-	tristate "Intel(R) XL710 X710 Virtual Function Ethernet support"
+	tristate "Intel(R) Ethernet Adaptive Virtual Function support"
 	depends on PCI_MSI
 	---help---
-	  This driver supports Intel(R) XL710 and X710 virtual functions.
-	  For more information on how to identify your adapter, go to the
-	  Adapter & Driver ID Guide that can be located at:
+	  This driver supports virtual functions for Intel XL710,
+	  X710, X722, and all devices advertising support for Intel
+	  Ethernet Adaptive Virtual Function devices. For more
+	  information on how to identify your adapter, go to the Adapter
+	  & Driver ID Guide that can be located at:
 
 	  <http://support.intel.com>
 
diff --git a/drivers/net/ethernet/intel/e100.c b/drivers/net/ethernet/intel/e100.c
index 2b7323d392dc..4d10270ddf8f 100644
--- a/drivers/net/ethernet/intel/e100.c
+++ b/drivers/net/ethernet/intel/e100.c
@@ -2430,7 +2430,10 @@ static int e100_get_link_ksettings(struct net_device *netdev,
 				   struct ethtool_link_ksettings *cmd)
 {
 	struct nic *nic = netdev_priv(netdev);
-	return mii_ethtool_get_link_ksettings(&nic->mii, cmd);
+
+	mii_ethtool_get_link_ksettings(&nic->mii, cmd);
+
+	return 0;
 }
 
 static int e100_set_link_ksettings(struct net_device *netdev,
diff --git a/drivers/net/ethernet/intel/e1000/e1000_main.c b/drivers/net/ethernet/intel/e1000/e1000_main.c
index bd8b05fe8258..98375e1e1185 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_main.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_main.c
@@ -4345,7 +4345,7 @@ static struct sk_buff *e1000_copybreak(struct e1000_adapter *adapter,
 	dma_sync_single_for_cpu(&adapter->pdev->dev, buffer_info->dma,
 				length, DMA_FROM_DEVICE);
 
-	memcpy(skb_put(skb, length), data, length);
+	skb_put_data(skb, data, length);
 
 	return skb;
 }
diff --git a/drivers/net/ethernet/intel/e1000e/e1000.h b/drivers/net/ethernet/intel/e1000e/e1000.h
index c7c994eb410e..98e68888abb1 100644
--- a/drivers/net/ethernet/intel/e1000e/e1000.h
+++ b/drivers/net/ethernet/intel/e1000e/e1000.h
@@ -268,6 +268,7 @@ struct e1000_adapter {
 	u32 tx_fifo_size;
 	u32 tx_dma_failed;
 	u32 tx_hwtstamp_timeouts;
+	u32 tx_hwtstamp_skipped;
 
 	/* Rx */
 	bool (*clean_rx)(struct e1000_ring *ring, int *work_done,
diff --git a/drivers/net/ethernet/intel/e1000e/ethtool.c b/drivers/net/ethernet/intel/e1000e/ethtool.c
index e23dbd9190d6..003cbd605799 100644
--- a/drivers/net/ethernet/intel/e1000e/ethtool.c
+++ b/drivers/net/ethernet/intel/e1000e/ethtool.c
@@ -105,6 +105,7 @@ static const struct e1000_stats e1000_gstrings_stats[] = {
 	E1000_STAT("uncorr_ecc_errors", uncorr_errors),
 	E1000_STAT("corr_ecc_errors", corr_errors),
 	E1000_STAT("tx_hwtstamp_timeouts", tx_hwtstamp_timeouts),
+	E1000_STAT("tx_hwtstamp_skipped", tx_hwtstamp_skipped),
 };
 
 #define E1000_GLOBAL_STATS_LEN	ARRAY_SIZE(e1000_gstrings_stats)
@@ -2072,7 +2073,7 @@ static void e1000_get_ethtool_stats(struct net_device *netdev,
 
 	pm_runtime_get_sync(netdev->dev.parent);
 
-	e1000e_get_stats64(netdev, &net_stats);
+	dev_get_stats(netdev, &net_stats);
 
 	pm_runtime_put_sync(netdev->dev.parent);
 
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index b3679728caac..2dcb5463d9b8 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -1183,6 +1183,7 @@ static void e1000e_tx_hwtstamp_work(struct work_struct *work)
 	struct e1000_hw *hw = &adapter->hw;
 
 	if (er32(TSYNCTXCTL) & E1000_TSYNCTXCTL_VALID) {
+		struct sk_buff *skb = adapter->tx_hwtstamp_skb;
 		struct skb_shared_hwtstamps shhwtstamps;
 		u64 txstmp;
 
@@ -1191,9 +1192,14 @@ static void e1000e_tx_hwtstamp_work(struct work_struct *work)
 
 		e1000e_systim_to_hwtstamp(adapter, &shhwtstamps, txstmp);
 
-		skb_tstamp_tx(adapter->tx_hwtstamp_skb, &shhwtstamps);
-		dev_kfree_skb_any(adapter->tx_hwtstamp_skb);
+		/* Clear the global tx_hwtstamp_skb pointer and force writes
+		 * prior to notifying the stack of a Tx timestamp.
+		 */
 		adapter->tx_hwtstamp_skb = NULL;
+		wmb(); /* force write prior to skb_tstamp_tx */
+
+		skb_tstamp_tx(skb, &shhwtstamps);
+		dev_kfree_skb_any(skb);
 	} else if (time_after(jiffies, adapter->tx_hwtstamp_start
 			      + adapter->tx_timeout_factor * HZ)) {
 		dev_kfree_skb_any(adapter->tx_hwtstamp_skb);
@@ -3680,6 +3686,7 @@ static int e1000e_config_hwtstamp(struct e1000_adapter *adapter,
 		 * Delay Request messages but not both so fall-through to
 		 * time stamp all packets.
 		 */
+	case HWTSTAMP_FILTER_NTP_ALL:
 	case HWTSTAMP_FILTER_ALL:
 		is_l2 = true;
 		is_l4 = true;
@@ -5860,17 +5867,20 @@ static netdev_tx_t e1000_xmit_frame(struct sk_buff *skb,
 			     nr_frags);
 	if (count) {
 		if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
-		    (adapter->flags & FLAG_HAS_HW_TIMESTAMP) &&
-		    !adapter->tx_hwtstamp_skb) {
-			skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
-			tx_flags |= E1000_TX_FLAGS_HWTSTAMP;
-			adapter->tx_hwtstamp_skb = skb_get(skb);
-			adapter->tx_hwtstamp_start = jiffies;
-			schedule_work(&adapter->tx_hwtstamp_work);
-		} else {
-			skb_tx_timestamp(skb);
+		    (adapter->flags & FLAG_HAS_HW_TIMESTAMP)) {
+			if (!adapter->tx_hwtstamp_skb) {
+				skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+				tx_flags |= E1000_TX_FLAGS_HWTSTAMP;
+				adapter->tx_hwtstamp_skb = skb_get(skb);
+				adapter->tx_hwtstamp_start = jiffies;
+				schedule_work(&adapter->tx_hwtstamp_work);
+			} else {
+				adapter->tx_hwtstamp_skipped++;
+			}
 		}
 
+		skb_tx_timestamp(skb);
+
 		netdev_sent_queue(netdev, skb->len);
 		e1000_tx_queue(tx_ring, tx_flags, count);
 		/* Make sure there is space in the ring for the next send. */
@@ -6630,12 +6640,17 @@ static int e1000e_pm_thaw(struct device *dev)
 static int e1000e_pm_suspend(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
+	int rc;
 
 	e1000e_flush_lpic(pdev);
 
 	e1000e_pm_freeze(dev);
 
-	return __e1000_shutdown(pdev, false);
+	rc = __e1000_shutdown(pdev, false);
+	if (rc)
+		e1000e_pm_thaw(dev);
+
+	return rc;
 }
 
 static int e1000e_pm_resume(struct device *dev)
@@ -6733,20 +6748,20 @@ static irqreturn_t e1000_intr_msix(int __always_unused irq, void *data)
 
 		vector = 0;
 		msix_irq = adapter->msix_entries[vector].vector;
-		disable_irq(msix_irq);
-		e1000_intr_msix_rx(msix_irq, netdev);
+		if (disable_hardirq(msix_irq))
+			e1000_intr_msix_rx(msix_irq, netdev);
 		enable_irq(msix_irq);
 
 		vector++;
 		msix_irq = adapter->msix_entries[vector].vector;
-		disable_irq(msix_irq);
-		e1000_intr_msix_tx(msix_irq, netdev);
+		if (disable_hardirq(msix_irq))
+			e1000_intr_msix_tx(msix_irq, netdev);
 		enable_irq(msix_irq);
 
 		vector++;
 		msix_irq = adapter->msix_entries[vector].vector;
-		disable_irq(msix_irq);
-		e1000_msix_other(msix_irq, netdev);
+		if (disable_hardirq(msix_irq))
+			e1000_msix_other(msix_irq, netdev);
 		enable_irq(msix_irq);
 	}
 
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c b/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
index 24f2f6f86f5a..5e37387c7082 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
@@ -1265,8 +1265,8 @@ err_queueing_scheme:
 	return err;
 }
 
-static int __fm10k_setup_tc(struct net_device *dev, u32 handle, __be16 proto,
-			    struct tc_to_netdev *tc)
+static int __fm10k_setup_tc(struct net_device *dev, u32 handle, u32 chain_index,
+			    __be16 proto, struct tc_to_netdev *tc)
 {
 	if (tc->type != TC_SETUP_MQPRIO)
 		return -EINVAL;
diff --git a/drivers/net/ethernet/intel/i40e/i40e.h b/drivers/net/ethernet/intel/i40e/i40e.h
index cdde3cc28fb5..d616f698e155 100644
--- a/drivers/net/ethernet/intel/i40e/i40e.h
+++ b/drivers/net/ethernet/intel/i40e/i40e.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2016 Intel Corporation.
+ * Copyright(c) 2013 - 2017 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -57,7 +57,7 @@
 #include "i40e_type.h"
 #include "i40e_prototype.h"
 #include "i40e_client.h"
-#include "i40e_virtchnl.h"
+#include <linux/avf/virtchnl.h>
 #include "i40e_virtchnl_pf.h"
 #include "i40e_txrx.h"
 #include "i40e_dcb.h"
@@ -103,6 +103,12 @@
 	(I40E_AQ_PHY_DEBUG_DISABLE_LINK_FW | \
 	I40E_AQ_PHY_DEBUG_DISABLE_ALL_LINK_FW)
 
+#define I40E_OEM_EETRACK_ID		0xffffffff
+#define I40E_OEM_GEN_SHIFT		24
+#define I40E_OEM_SNAP_MASK		0x00ff0000
+#define I40E_OEM_SNAP_SHIFT		16
+#define I40E_OEM_RELEASE_MASK		0x0000ffff
+
 /* The values in here are decimal coded as hex as is the case in the NVM map*/
 #define I40E_CURRENT_NVM_VERSION_HI	0x2
 #define I40E_CURRENT_NVM_VERSION_LO	0x40
@@ -399,6 +405,7 @@ struct i40e_pf {
 #define I40E_FLAG_RX_CSUM_ENABLED		BIT_ULL(1)
 #define I40E_FLAG_MSI_ENABLED			BIT_ULL(2)
 #define I40E_FLAG_MSIX_ENABLED			BIT_ULL(3)
+#define I40E_FLAG_HW_ATR_EVICT_ENABLED		BIT_ULL(4)
 #define I40E_FLAG_RSS_ENABLED			BIT_ULL(6)
 #define I40E_FLAG_VMDQ_ENABLED			BIT_ULL(7)
 #define I40E_FLAG_IWARP_ENABLED			BIT_ULL(10)
@@ -502,10 +509,12 @@ struct i40e_pf {
 	struct ptp_clock *ptp_clock;
 	struct ptp_clock_info ptp_caps;
 	struct sk_buff *ptp_tx_skb;
+	unsigned long ptp_tx_start;
 	struct hwtstamp_config tstamp_config;
 	struct mutex tmreg_lock; /* Used to protect the SYSTIME registers. */
 	u64 ptp_base_adj;
 	u32 tx_hwtstamp_timeouts;
+	u32 tx_hwtstamp_skipped;
 	u32 rx_hwtstamp_cleared;
 	u32 latch_event_flags;
 	spinlock_t ptp_rx_lock; /* Used to protect Rx timestamp registers. */
@@ -513,9 +522,8 @@ struct i40e_pf {
 	bool ptp_tx;
 	bool ptp_rx;
 	u16 rss_table_size; /* HW RSS table size */
-	/* These are only valid in NPAR modes */
-	u32 npar_max_bw;
-	u32 npar_min_bw;
+	u32 max_bw;
+	u32 min_bw;
 
 	u32 ioremap_len;
 	u32 fd_inv;
@@ -626,6 +634,7 @@ struct i40e_vsi {
 	/* These are containers of ring pointers, allocated at run-time */
 	struct i40e_ring **rx_rings;
 	struct i40e_ring **tx_rings;
+	struct i40e_ring **xdp_rings; /* XDP Tx rings */
 
 	u32  active_filters;
 	u32  promisc_threshold;
@@ -642,6 +651,8 @@ struct i40e_vsi {
 	u16 max_frame;
 	u16 rx_buf_len;
 
+	struct bpf_prog *xdp_prog;
+
 	/* List of q_vectors allocated to this VSI */
 	struct i40e_q_vector **q_vectors;
 	int num_q_vectors;
@@ -729,22 +740,36 @@ static inline char *i40e_nvm_version_str(struct i40e_hw *hw)
 {
 	static char buf[32];
 	u32 full_ver;
-	u8 ver, patch;
-	u16 build;
 
 	full_ver = hw->nvm.oem_ver;
-	ver = (u8)(full_ver >> I40E_OEM_VER_SHIFT);
-	build = (u16)((full_ver >> I40E_OEM_VER_BUILD_SHIFT) &
-		 I40E_OEM_VER_BUILD_MASK);
-	patch = (u8)(full_ver & I40E_OEM_VER_PATCH_MASK);
-
-	snprintf(buf, sizeof(buf),
-		 "%x.%02x 0x%x %d.%d.%d",
-		 (hw->nvm.version & I40E_NVM_VERSION_HI_MASK) >>
-			I40E_NVM_VERSION_HI_SHIFT,
-		 (hw->nvm.version & I40E_NVM_VERSION_LO_MASK) >>
-			I40E_NVM_VERSION_LO_SHIFT,
-		 hw->nvm.eetrack, ver, build, patch);
+
+	if (hw->nvm.eetrack == I40E_OEM_EETRACK_ID) {
+		u8 gen, snap;
+		u16 release;
+
+		gen = (u8)(full_ver >> I40E_OEM_GEN_SHIFT);
+		snap = (u8)((full_ver & I40E_OEM_SNAP_MASK) >>
+			I40E_OEM_SNAP_SHIFT);
+		release = (u16)(full_ver & I40E_OEM_RELEASE_MASK);
+
+		snprintf(buf, sizeof(buf), "%x.%x.%x", gen, snap, release);
+	} else {
+		u8 ver, patch;
+		u16 build;
+
+		ver = (u8)(full_ver >> I40E_OEM_VER_SHIFT);
+		build = (u16)((full_ver >> I40E_OEM_VER_BUILD_SHIFT) &
+			 I40E_OEM_VER_BUILD_MASK);
+		patch = (u8)(full_ver & I40E_OEM_VER_PATCH_MASK);
+
+		snprintf(buf, sizeof(buf),
+			 "%x.%02x 0x%x %d.%d.%d",
+			 (hw->nvm.version & I40E_NVM_VERSION_HI_MASK) >>
+				I40E_NVM_VERSION_HI_SHIFT,
+			 (hw->nvm.version & I40E_NVM_VERSION_LO_MASK) >>
+				I40E_NVM_VERSION_LO_SHIFT,
+			 hw->nvm.eetrack, ver, build, patch);
+	}
 
 	return buf;
 }
@@ -955,7 +980,8 @@ bool i40e_dcb_need_reconfig(struct i40e_pf *pf,
 			    struct i40e_dcbx_config *old_cfg,
 			    struct i40e_dcbx_config *new_cfg);
 #endif /* CONFIG_I40E_DCB */
-void i40e_ptp_rx_hang(struct i40e_vsi *vsi);
+void i40e_ptp_rx_hang(struct i40e_pf *pf);
+void i40e_ptp_tx_hang(struct i40e_pf *pf);
 void i40e_ptp_tx_hwtstamp(struct i40e_pf *pf);
 void i40e_ptp_rx_hwtstamp(struct i40e_pf *pf, struct sk_buff *skb, u8 index);
 void i40e_ptp_set_increment(struct i40e_pf *pf);
@@ -964,8 +990,13 @@ int i40e_ptp_get_ts_config(struct i40e_pf *pf, struct ifreq *ifr);
 void i40e_ptp_init(struct i40e_pf *pf);
 void i40e_ptp_stop(struct i40e_pf *pf);
 int i40e_is_vsi_uplink_mode_veb(struct i40e_vsi *vsi);
-i40e_status i40e_get_npar_bw_setting(struct i40e_pf *pf);
-i40e_status i40e_set_npar_bw_setting(struct i40e_pf *pf);
-i40e_status i40e_commit_npar_bw_setting(struct i40e_pf *pf);
+i40e_status i40e_get_partition_bw_setting(struct i40e_pf *pf);
+i40e_status i40e_set_partition_bw_setting(struct i40e_pf *pf);
+i40e_status i40e_commit_partition_bw_setting(struct i40e_pf *pf);
 void i40e_print_link_message(struct i40e_vsi *vsi, bool isup);
+
+static inline bool i40e_enabled_xdp_vsi(struct i40e_vsi *vsi)
+{
+	return !!vsi->xdp_prog;
+}
 #endif /* _I40E_H_ */
diff --git a/drivers/net/ethernet/intel/i40e/i40e_adminq_cmd.h b/drivers/net/ethernet/intel/i40e/i40e_adminq_cmd.h
index 5eb04114e13f..5d5f422cbae5 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_adminq_cmd.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_adminq_cmd.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2016 Intel Corporation.
+ * Copyright(c) 2013 - 2017 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -531,7 +531,7 @@ struct i40e_aqc_mac_address_read {
 #define I40E_AQC_PORT_ADDR_VALID	0x40
 #define I40E_AQC_WOL_ADDR_VALID		0x80
 #define I40E_AQC_MC_MAG_EN_VALID	0x100
-#define I40E_AQC_ADDR_VALID_MASK	0x1F0
+#define I40E_AQC_ADDR_VALID_MASK	0x3F0
 	u8	reserved[6];
 	__le32	addr_high;
 	__le32	addr_low;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_client.c b/drivers/net/ethernet/intel/i40e/i40e_client.c
index c3b81a97558e..1b1e2acbd07f 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_client.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_client.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2015 Intel Corporation.
+ * Copyright(c) 2013 - 2017 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -273,8 +273,8 @@ int i40e_vf_client_capable(struct i40e_pf *pf, u32 vf_id)
 	if (!cdev || !cdev->client)
 		goto out;
 	if (!cdev->client->ops || !cdev->client->ops->vf_capable) {
-		dev_info(&pf->pdev->dev,
-			 "Cannot locate client instance VF capability routine\n");
+		dev_dbg(&pf->pdev->dev,
+			"Cannot locate client instance VF capability routine\n");
 		goto out;
 	}
 	if (!test_bit(__I40E_CLIENT_INSTANCE_OPENED, &cdev->state))
@@ -565,7 +565,7 @@ static int i40e_client_virtchnl_send(struct i40e_info *ldev,
 	struct i40e_hw *hw = &pf->hw;
 	i40e_status err;
 
-	err = i40e_aq_send_msg_to_vf(hw, vf_id, I40E_VIRTCHNL_OP_IWARP,
+	err = i40e_aq_send_msg_to_vf(hw, vf_id, VIRTCHNL_OP_IWARP,
 				     0, msg, len, NULL);
 	if (err)
 		dev_err(&pf->pdev->dev, "Unable to send iWarp message to VF, error %d, aq status %d\n",
@@ -595,6 +595,8 @@ static int i40e_client_setup_qvlist(struct i40e_info *ldev,
 	size = sizeof(struct i40e_qvlist_info) +
 	       (sizeof(struct i40e_qv_info) * (qvlist_info->num_vectors - 1));
 	ldev->qvlist_info = kzalloc(size, GFP_KERNEL);
+	if (!ldev->qvlist_info)
+		return -ENOMEM;
 	ldev->qvlist_info->num_vectors = qvlist_info->num_vectors;
 
 	for (i = 0; i < qvlist_info->num_vectors; i++) {
diff --git a/drivers/net/ethernet/intel/i40e/i40e_common.c b/drivers/net/ethernet/intel/i40e/i40e_common.c
index 24f020655291..8e082a946411 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_common.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_common.c
@@ -27,7 +27,7 @@
 #include "i40e_type.h"
 #include "i40e_adminq.h"
 #include "i40e_prototype.h"
-#include "i40e_virtchnl.h"
+#include <linux/avf/virtchnl.h>
 
 /**
  * i40e_set_mac_type - Sets MAC type
@@ -3614,11 +3614,15 @@ i40e_status i40e_aq_get_cee_dcb_config(struct i40e_hw *hw,
 /**
  * i40e_aq_add_udp_tunnel
  * @hw: pointer to the hw struct
- * @udp_port: the UDP port to add
+ * @udp_port: the UDP port to add in Host byte order
  * @header_len: length of the tunneling header length in DWords
  * @protocol_index: protocol index type
  * @filter_index: pointer to filter index
  * @cmd_details: pointer to command details structure or NULL
+ *
+ * Note: Firmware expects the udp_port value to be in Little Endian format,
+ * and this function will call cpu_to_le16 to convert from Host byte order to
+ * Little Endian order.
  **/
 i40e_status i40e_aq_add_udp_tunnel(struct i40e_hw *hw,
 				u16 udp_port, u8 protocol_index,
diff --git a/drivers/net/ethernet/intel/i40e/i40e_dcb.c b/drivers/net/ethernet/intel/i40e/i40e_dcb.c
index 0fab3a9b51d9..55079fe3ed63 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_dcb.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_dcb.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 2017 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -390,6 +390,8 @@ static void i40e_parse_cee_app_tlv(struct i40e_cee_feat_tlv *tlv,
 
 	if (!dcbcfg->numapps)
 		return;
+	if (dcbcfg->numapps > I40E_DCBX_MAX_APPS)
+		dcbcfg->numapps = I40E_DCBX_MAX_APPS;
 
 	for (i = 0; i < dcbcfg->numapps; i++) {
 		u8 up, selector;
@@ -618,14 +620,17 @@ static void i40e_cee_to_dcb_v1_config(
 	/* CEE PG data to ETS config */
 	dcbcfg->etscfg.maxtcs = cee_cfg->oper_num_tc;
 
+	/* Note that the FW creates the oper_prio_tc nibbles reversed
+	 * from those in the CEE Priority Group sub-TLV.
+	 */
 	for (i = 0; i < 4; i++) {
 		tc = (u8)((cee_cfg->oper_prio_tc[i] &
-			 I40E_CEE_PGID_PRIO_1_MASK) >>
-			 I40E_CEE_PGID_PRIO_1_SHIFT);
-		dcbcfg->etscfg.prioritytable[i*2] =  tc;
-		tc = (u8)((cee_cfg->oper_prio_tc[i] &
 			 I40E_CEE_PGID_PRIO_0_MASK) >>
 			 I40E_CEE_PGID_PRIO_0_SHIFT);
+		dcbcfg->etscfg.prioritytable[i * 2] =  tc;
+		tc = (u8)((cee_cfg->oper_prio_tc[i] &
+			 I40E_CEE_PGID_PRIO_1_MASK) >>
+			 I40E_CEE_PGID_PRIO_1_SHIFT);
 		dcbcfg->etscfg.prioritytable[i*2 + 1] = tc;
 	}
 
diff --git a/drivers/net/ethernet/intel/i40e/i40e_ethtool.c b/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
index 7a8eb486b9ea..9692a5294fa3 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
@@ -147,6 +147,7 @@ static const struct i40e_stats i40e_gstrings_stats[] = {
 	I40E_PF_STAT("VF_admin_queue_requests", vf_aq_requests),
 	I40E_PF_STAT("arq_overflows", arq_overflows),
 	I40E_PF_STAT("rx_hwtstamp_cleared", rx_hwtstamp_cleared),
+	I40E_PF_STAT("tx_hwtstamp_skipped", tx_hwtstamp_skipped),
 	I40E_PF_STAT("fdir_flush_cnt", fd_flush_cnt),
 	I40E_PF_STAT("fdir_atr_match", stats.fd_atr_match),
 	I40E_PF_STAT("fdir_atr_tunnel_match", stats.fd_atr_tunnel_match),
@@ -224,7 +225,7 @@ static const struct i40e_priv_flags i40e_gstrings_priv_flags[] = {
 	I40E_PRIV_FLAG("LinkPolling", I40E_FLAG_LINK_POLLING_ENABLED, 0),
 	I40E_PRIV_FLAG("flow-director-atr", I40E_FLAG_FD_ATR_ENABLED, 0),
 	I40E_PRIV_FLAG("veb-stats", I40E_FLAG_VEB_STATS_ENABLED, 0),
-	I40E_PRIV_FLAG("hw-atr-eviction", I40E_FLAG_HW_ATR_EVICT_CAPABLE, 0),
+	I40E_PRIV_FLAG("hw-atr-eviction", I40E_FLAG_HW_ATR_EVICT_ENABLED, 0),
 	I40E_PRIV_FLAG("legacy-rx", I40E_FLAG_LEGACY_RX, 0),
 };
 
@@ -1298,6 +1299,17 @@ static void i40e_get_ringparam(struct net_device *netdev,
 	ring->rx_jumbo_pending = 0;
 }
 
+static bool i40e_active_tx_ring_index(struct i40e_vsi *vsi, u16 index)
+{
+	if (i40e_enabled_xdp_vsi(vsi)) {
+		return index < vsi->num_queue_pairs ||
+			(index >= vsi->alloc_queue_pairs &&
+			 index < vsi->alloc_queue_pairs + vsi->num_queue_pairs);
+	}
+
+	return index < vsi->num_queue_pairs;
+}
+
 static int i40e_set_ringparam(struct net_device *netdev,
 			      struct ethtool_ringparam *ring)
 {
@@ -1307,6 +1319,7 @@ static int i40e_set_ringparam(struct net_device *netdev,
 	struct i40e_vsi *vsi = np->vsi;
 	struct i40e_pf *pf = vsi->back;
 	u32 new_rx_count, new_tx_count;
+	u16 tx_alloc_queue_pairs;
 	int timeout = 50;
 	int i, err = 0;
 
@@ -1344,6 +1357,8 @@ static int i40e_set_ringparam(struct net_device *netdev,
 		for (i = 0; i < vsi->num_queue_pairs; i++) {
 			vsi->tx_rings[i]->count = new_tx_count;
 			vsi->rx_rings[i]->count = new_rx_count;
+			if (i40e_enabled_xdp_vsi(vsi))
+				vsi->xdp_rings[i]->count = new_tx_count;
 		}
 		goto done;
 	}
@@ -1353,20 +1368,24 @@ static int i40e_set_ringparam(struct net_device *netdev,
 	 * to the Tx and Rx ring structs.
 	 */
 
-	/* alloc updated Tx resources */
+	/* alloc updated Tx and XDP Tx resources */
+	tx_alloc_queue_pairs = vsi->alloc_queue_pairs *
+			       (i40e_enabled_xdp_vsi(vsi) ? 2 : 1);
 	if (new_tx_count != vsi->tx_rings[0]->count) {
 		netdev_info(netdev,
 			    "Changing Tx descriptor count from %d to %d.\n",
 			    vsi->tx_rings[0]->count, new_tx_count);
-		tx_rings = kcalloc(vsi->alloc_queue_pairs,
+		tx_rings = kcalloc(tx_alloc_queue_pairs,
 				   sizeof(struct i40e_ring), GFP_KERNEL);
 		if (!tx_rings) {
 			err = -ENOMEM;
 			goto done;
 		}
 
-		for (i = 0; i < vsi->num_queue_pairs; i++) {
-			/* clone ring and setup updated count */
+		for (i = 0; i < tx_alloc_queue_pairs; i++) {
+			if (!i40e_active_tx_ring_index(vsi, i))
+				continue;
+
 			tx_rings[i] = *vsi->tx_rings[i];
 			tx_rings[i].count = new_tx_count;
 			/* the desc and bi pointers will be reallocated in the
@@ -1378,6 +1397,8 @@ static int i40e_set_ringparam(struct net_device *netdev,
 			if (err) {
 				while (i) {
 					i--;
+					if (!i40e_active_tx_ring_index(vsi, i))
+						continue;
 					i40e_free_tx_resources(&tx_rings[i]);
 				}
 				kfree(tx_rings);
@@ -1445,9 +1466,11 @@ rx_unwind:
 	i40e_down(vsi);
 
 	if (tx_rings) {
-		for (i = 0; i < vsi->num_queue_pairs; i++) {
-			i40e_free_tx_resources(vsi->tx_rings[i]);
-			*vsi->tx_rings[i] = tx_rings[i];
+		for (i = 0; i < tx_alloc_queue_pairs; i++) {
+			if (i40e_active_tx_ring_index(vsi, i)) {
+				i40e_free_tx_resources(vsi->tx_rings[i]);
+				*vsi->tx_rings[i] = tx_rings[i];
+			}
 		}
 		kfree(tx_rings);
 		tx_rings = NULL;
@@ -1478,8 +1501,10 @@ rx_unwind:
 free_tx:
 	/* error cleanup if the Rx allocations failed after getting Tx */
 	if (tx_rings) {
-		for (i = 0; i < vsi->num_queue_pairs; i++)
-			i40e_free_tx_resources(&tx_rings[i]);
+		for (i = 0; i < tx_alloc_queue_pairs; i++) {
+			if (i40e_active_tx_ring_index(vsi, i))
+				i40e_free_tx_resources(vsi->tx_rings[i]);
+		}
 		kfree(tx_rings);
 		tx_rings = NULL;
 	}
@@ -2686,6 +2711,12 @@ static int i40e_set_rss_hash_opt(struct i40e_pf *pf, struct ethtool_rxnfc *nfc)
 	u8 flow_pctype = 0;
 	u64 i_set, i_setc;
 
+	if (pf->flags & I40E_FLAG_MFP_ENABLED) {
+		dev_err(&pf->pdev->dev,
+			"Change of RSS hash input set is not supported when MFP mode is enabled\n");
+		return -EOPNOTSUPP;
+	}
+
 	/* RSS does not support anything other than hashing
 	 * to queues on src and dst IPs and ports
 	 */
@@ -4092,7 +4123,7 @@ flags_complete:
 
 	/* Only allow ATR evict on hardware that is capable of handling it */
 	if (pf->flags & I40E_FLAG_HW_ATR_EVICT_CAPABLE)
-		pf->flags &= ~I40E_FLAG_HW_ATR_EVICT_CAPABLE;
+		pf->flags &= ~I40E_FLAG_HW_ATR_EVICT_ENABLED;
 
 	if (changed_flags & I40E_FLAG_TRUE_PROMISC_SUPPORT) {
 		u16 sw_flags = 0, valid_flags = 0;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_fcoe.c b/drivers/net/ethernet/intel/i40e/i40e_fcoe.c
index b077ef8b00fa..2d1253c5b7a1 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_fcoe.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_fcoe.c
@@ -762,7 +762,7 @@ int i40e_fcoe_handle_offload(struct i40e_ring *rx_ring,
 		    (fh->fh_r_ctl == FC_RCTL_DD_SOL_DATA)) {
 			struct fcoe_crc_eof *crc = NULL;
 
-			crc = (struct fcoe_crc_eof *)skb_put(skb, sizeof(*crc));
+			crc = skb_put(skb, sizeof(*crc));
 			crc->fcoe_eof = FC_EOF_T;
 		} else {
 			/* otherwise, drop the header only frame */
diff --git a/drivers/net/ethernet/intel/i40e/i40e_main.c b/drivers/net/ethernet/intel/i40e/i40e_main.c
index 150caf6ca2b4..2db93d3f6d23 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_main.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_main.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2016 Intel Corporation.
+ * Copyright(c) 2013 - 2017 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -27,6 +27,7 @@
 #include <linux/etherdevice.h>
 #include <linux/of_net.h>
 #include <linux/pci.h>
+#include <linux/bpf.h>
 
 /* Local includes */
 #include "i40e.h"
@@ -407,6 +408,27 @@ struct rtnl_link_stats64 *i40e_get_vsi_stats_struct(struct i40e_vsi *vsi)
 }
 
 /**
+ * i40e_get_netdev_stats_struct_tx - populate stats from a Tx ring
+ * @ring: Tx ring to get statistics from
+ * @stats: statistics entry to be updated
+ **/
+static void i40e_get_netdev_stats_struct_tx(struct i40e_ring *ring,
+					    struct rtnl_link_stats64 *stats)
+{
+	u64 bytes, packets;
+	unsigned int start;
+
+	do {
+		start = u64_stats_fetch_begin_irq(&ring->syncp);
+		packets = ring->stats.packets;
+		bytes   = ring->stats.bytes;
+	} while (u64_stats_fetch_retry_irq(&ring->syncp, start));
+
+	stats->tx_packets += packets;
+	stats->tx_bytes   += bytes;
+}
+
+/**
  * i40e_get_netdev_stats_struct - Get statistics for netdev interface
  * @netdev: network interface device structure
  *
@@ -436,15 +458,8 @@ static void i40e_get_netdev_stats_struct(struct net_device *netdev,
 		tx_ring = ACCESS_ONCE(vsi->tx_rings[i]);
 		if (!tx_ring)
 			continue;
+		i40e_get_netdev_stats_struct_tx(tx_ring, stats);
 
-		do {
-			start = u64_stats_fetch_begin_irq(&tx_ring->syncp);
-			packets = tx_ring->stats.packets;
-			bytes   = tx_ring->stats.bytes;
-		} while (u64_stats_fetch_retry_irq(&tx_ring->syncp, start));
-
-		stats->tx_packets += packets;
-		stats->tx_bytes   += bytes;
 		rx_ring = &tx_ring[1];
 
 		do {
@@ -455,6 +470,9 @@ static void i40e_get_netdev_stats_struct(struct net_device *netdev,
 
 		stats->rx_packets += packets;
 		stats->rx_bytes   += bytes;
+
+		if (i40e_enabled_xdp_vsi(vsi))
+			i40e_get_netdev_stats_struct_tx(&rx_ring[1], stats);
 	}
 	rcu_read_unlock();
 
@@ -2263,9 +2281,8 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi)
 				 i40e_aq_str(hw, hw->aq.asq_last_status));
 		}
 	}
-	if ((changed_flags & IFF_PROMISC) ||
-	    (promisc_changed &&
-	     test_bit(__I40E_VSI_OVERFLOW_PROMISC, vsi->state))) {
+
+	if ((changed_flags & IFF_PROMISC) || promisc_changed) {
 		bool cur_promisc;
 
 		cur_promisc = (!!(vsi->current_netdev_flags & IFF_PROMISC) ||
@@ -2396,6 +2413,18 @@ static void i40e_sync_filters_subtask(struct i40e_pf *pf)
 }
 
 /**
+ * i40e_max_xdp_frame_size - returns the maximum allowed frame size for XDP
+ * @vsi: the vsi
+ **/
+static int i40e_max_xdp_frame_size(struct i40e_vsi *vsi)
+{
+	if (PAGE_SIZE >= 8192 || (vsi->back->flags & I40E_FLAG_LEGACY_RX))
+		return I40E_RXBUFFER_2048;
+	else
+		return I40E_RXBUFFER_3072;
+}
+
+/**
  * i40e_change_mtu - NDO callback to change the Maximum Transfer Unit
  * @netdev: network interface device structure
  * @new_mtu: new value for maximum frame size
@@ -2408,6 +2437,13 @@ static int i40e_change_mtu(struct net_device *netdev, int new_mtu)
 	struct i40e_vsi *vsi = np->vsi;
 	struct i40e_pf *pf = vsi->back;
 
+	if (i40e_enabled_xdp_vsi(vsi)) {
+		int frame_size = new_mtu + ETH_HLEN + ETH_FCS_LEN + VLAN_HLEN;
+
+		if (frame_size > i40e_max_xdp_frame_size(vsi))
+			return -EINVAL;
+	}
+
 	netdev_info(netdev, "changing MTU from %d to %d\n",
 		    netdev->mtu, new_mtu);
 	netdev->mtu = new_mtu;
@@ -2794,6 +2830,12 @@ static int i40e_vsi_setup_tx_resources(struct i40e_vsi *vsi)
 	for (i = 0; i < vsi->num_queue_pairs && !err; i++)
 		err = i40e_setup_tx_descriptors(vsi->tx_rings[i]);
 
+	if (!i40e_enabled_xdp_vsi(vsi))
+		return err;
+
+	for (i = 0; i < vsi->num_queue_pairs && !err; i++)
+		err = i40e_setup_tx_descriptors(vsi->xdp_rings[i]);
+
 	return err;
 }
 
@@ -2807,12 +2849,17 @@ static void i40e_vsi_free_tx_resources(struct i40e_vsi *vsi)
 {
 	int i;
 
-	if (!vsi->tx_rings)
-		return;
+	if (vsi->tx_rings) {
+		for (i = 0; i < vsi->num_queue_pairs; i++)
+			if (vsi->tx_rings[i] && vsi->tx_rings[i]->desc)
+				i40e_free_tx_resources(vsi->tx_rings[i]);
+	}
 
-	for (i = 0; i < vsi->num_queue_pairs; i++)
-		if (vsi->tx_rings[i] && vsi->tx_rings[i]->desc)
-			i40e_free_tx_resources(vsi->tx_rings[i]);
+	if (vsi->xdp_rings) {
+		for (i = 0; i < vsi->num_queue_pairs; i++)
+			if (vsi->xdp_rings[i] && vsi->xdp_rings[i]->desc)
+				i40e_free_tx_resources(vsi->xdp_rings[i]);
+	}
 }
 
 /**
@@ -3073,6 +3120,12 @@ static int i40e_vsi_configure_tx(struct i40e_vsi *vsi)
 	for (i = 0; (i < vsi->num_queue_pairs) && !err; i++)
 		err = i40e_configure_tx_ring(vsi->tx_rings[i]);
 
+	if (!i40e_enabled_xdp_vsi(vsi))
+		return err;
+
+	for (i = 0; (i < vsi->num_queue_pairs) && !err; i++)
+		err = i40e_configure_tx_ring(vsi->xdp_rings[i]);
+
 	return err;
 }
 
@@ -3217,6 +3270,7 @@ static int i40e_vsi_configure(struct i40e_vsi *vsi)
  **/
 static void i40e_vsi_configure_msix(struct i40e_vsi *vsi)
 {
+	bool has_xdp = i40e_enabled_xdp_vsi(vsi);
 	struct i40e_pf *pf = vsi->back;
 	struct i40e_hw *hw = &pf->hw;
 	u16 vector;
@@ -3247,28 +3301,40 @@ static void i40e_vsi_configure_msix(struct i40e_vsi *vsi)
 		/* Linked list for the queuepairs assigned to this vector */
 		wr32(hw, I40E_PFINT_LNKLSTN(vector - 1), qp);
 		for (q = 0; q < q_vector->num_ringpairs; q++) {
+			u32 nextqp = has_xdp ? qp + vsi->alloc_queue_pairs : qp;
 			u32 val;
 
 			val = I40E_QINT_RQCTL_CAUSE_ENA_MASK |
-			      (I40E_RX_ITR << I40E_QINT_RQCTL_ITR_INDX_SHIFT)  |
-			      (vector      << I40E_QINT_RQCTL_MSIX_INDX_SHIFT) |
-			      (qp          << I40E_QINT_RQCTL_NEXTQ_INDX_SHIFT)|
-			      (I40E_QUEUE_TYPE_TX
-				      << I40E_QINT_RQCTL_NEXTQ_TYPE_SHIFT);
+			      (I40E_RX_ITR << I40E_QINT_RQCTL_ITR_INDX_SHIFT) |
+			      (vector << I40E_QINT_RQCTL_MSIX_INDX_SHIFT) |
+			      (nextqp << I40E_QINT_RQCTL_NEXTQ_INDX_SHIFT) |
+			      (I40E_QUEUE_TYPE_TX <<
+			       I40E_QINT_RQCTL_NEXTQ_TYPE_SHIFT);
 
 			wr32(hw, I40E_QINT_RQCTL(qp), val);
 
+			if (has_xdp) {
+				val = I40E_QINT_TQCTL_CAUSE_ENA_MASK |
+				      (I40E_TX_ITR << I40E_QINT_TQCTL_ITR_INDX_SHIFT) |
+				      (vector << I40E_QINT_TQCTL_MSIX_INDX_SHIFT) |
+				      (qp << I40E_QINT_TQCTL_NEXTQ_INDX_SHIFT) |
+				      (I40E_QUEUE_TYPE_TX <<
+				       I40E_QINT_TQCTL_NEXTQ_TYPE_SHIFT);
+
+				wr32(hw, I40E_QINT_TQCTL(nextqp), val);
+			}
+
 			val = I40E_QINT_TQCTL_CAUSE_ENA_MASK |
-			      (I40E_TX_ITR << I40E_QINT_TQCTL_ITR_INDX_SHIFT)  |
-			      (vector      << I40E_QINT_TQCTL_MSIX_INDX_SHIFT) |
-			      ((qp+1)      << I40E_QINT_TQCTL_NEXTQ_INDX_SHIFT)|
-			      (I40E_QUEUE_TYPE_RX
-				      << I40E_QINT_TQCTL_NEXTQ_TYPE_SHIFT);
+			      (I40E_TX_ITR << I40E_QINT_TQCTL_ITR_INDX_SHIFT) |
+			      (vector << I40E_QINT_TQCTL_MSIX_INDX_SHIFT) |
+			      ((qp + 1) << I40E_QINT_TQCTL_NEXTQ_INDX_SHIFT) |
+			      (I40E_QUEUE_TYPE_RX <<
+			       I40E_QINT_TQCTL_NEXTQ_TYPE_SHIFT);
 
 			/* Terminate the linked list */
 			if (q == (q_vector->num_ringpairs - 1))
-				val |= (I40E_QUEUE_END_OF_LIST
-					   << I40E_QINT_TQCTL_NEXTQ_INDX_SHIFT);
+				val |= (I40E_QUEUE_END_OF_LIST <<
+					I40E_QINT_TQCTL_NEXTQ_INDX_SHIFT);
 
 			wr32(hw, I40E_QINT_TQCTL(qp), val);
 			qp++;
@@ -3322,6 +3388,7 @@ static void i40e_enable_misc_int_causes(struct i40e_pf *pf)
  **/
 static void i40e_configure_msi_and_legacy(struct i40e_vsi *vsi)
 {
+	u32 nextqp = i40e_enabled_xdp_vsi(vsi) ? vsi->alloc_queue_pairs : 0;
 	struct i40e_q_vector *q_vector = vsi->q_vectors[0];
 	struct i40e_pf *pf = vsi->back;
 	struct i40e_hw *hw = &pf->hw;
@@ -3342,12 +3409,22 @@ static void i40e_configure_msi_and_legacy(struct i40e_vsi *vsi)
 	wr32(hw, I40E_PFINT_LNKLST0, 0);
 
 	/* Associate the queue pair to the vector and enable the queue int */
-	val = I40E_QINT_RQCTL_CAUSE_ENA_MASK		      |
-	      (I40E_RX_ITR << I40E_QINT_RQCTL_ITR_INDX_SHIFT) |
+	val = I40E_QINT_RQCTL_CAUSE_ENA_MASK		       |
+	      (I40E_RX_ITR << I40E_QINT_RQCTL_ITR_INDX_SHIFT)  |
+	      (nextqp	   << I40E_QINT_RQCTL_NEXTQ_INDX_SHIFT)|
 	      (I40E_QUEUE_TYPE_TX << I40E_QINT_TQCTL_NEXTQ_TYPE_SHIFT);
 
 	wr32(hw, I40E_QINT_RQCTL(0), val);
 
+	if (i40e_enabled_xdp_vsi(vsi)) {
+		val = I40E_QINT_TQCTL_CAUSE_ENA_MASK		     |
+		      (I40E_TX_ITR << I40E_QINT_TQCTL_ITR_INDX_SHIFT)|
+		      (I40E_QUEUE_TYPE_TX
+		       << I40E_QINT_TQCTL_NEXTQ_TYPE_SHIFT);
+
+	       wr32(hw, I40E_QINT_TQCTL(nextqp), val);
+	}
+
 	val = I40E_QINT_TQCTL_CAUSE_ENA_MASK		      |
 	      (I40E_TX_ITR << I40E_QINT_TQCTL_ITR_INDX_SHIFT) |
 	      (I40E_QUEUE_END_OF_LIST << I40E_QINT_TQCTL_NEXTQ_INDX_SHIFT);
@@ -3511,11 +3588,24 @@ static void i40e_vsi_disable_irq(struct i40e_vsi *vsi)
 	int base = vsi->base_vector;
 	int i;
 
+	/* disable interrupt causation from each queue */
 	for (i = 0; i < vsi->num_queue_pairs; i++) {
-		wr32(hw, I40E_QINT_TQCTL(vsi->tx_rings[i]->reg_idx), 0);
-		wr32(hw, I40E_QINT_RQCTL(vsi->rx_rings[i]->reg_idx), 0);
+		u32 val;
+
+		val = rd32(hw, I40E_QINT_TQCTL(vsi->tx_rings[i]->reg_idx));
+		val &= ~I40E_QINT_TQCTL_CAUSE_ENA_MASK;
+		wr32(hw, I40E_QINT_TQCTL(vsi->tx_rings[i]->reg_idx), val);
+
+		val = rd32(hw, I40E_QINT_RQCTL(vsi->rx_rings[i]->reg_idx));
+		val &= ~I40E_QINT_RQCTL_CAUSE_ENA_MASK;
+		wr32(hw, I40E_QINT_RQCTL(vsi->rx_rings[i]->reg_idx), val);
+
+		if (!i40e_enabled_xdp_vsi(vsi))
+			continue;
+		wr32(hw, I40E_QINT_TQCTL(vsi->xdp_rings[i]->reg_idx), 0);
 	}
 
+	/* disable each interrupt */
 	if (pf->flags & I40E_FLAG_MSIX_ENABLED) {
 		for (i = vsi->base_vector;
 		     i < (vsi->num_q_vectors + vsi->base_vector); i++)
@@ -3594,10 +3684,10 @@ static irqreturn_t i40e_intr(int irq, void *data)
 		pf->sw_int_count++;
 
 	if ((pf->flags & I40E_FLAG_IWARP_ENABLED) &&
-	    (ena_mask & I40E_PFINT_ICR0_ENA_PE_CRITERR_MASK)) {
+	    (icr0 & I40E_PFINT_ICR0_ENA_PE_CRITERR_MASK)) {
 		ena_mask &= ~I40E_PFINT_ICR0_ENA_PE_CRITERR_MASK;
-		icr0 &= ~I40E_PFINT_ICR0_ENA_PE_CRITERR_MASK;
 		dev_dbg(&pf->pdev->dev, "cleared PE_CRITERR\n");
+		set_bit(__I40E_CORE_RESET_REQUESTED, pf->state);
 	}
 
 	/* only q0 is used in MSI/Legacy mode, and none are used in MSIX */
@@ -3816,6 +3906,16 @@ static void i40e_map_vector_to_qp(struct i40e_vsi *vsi, int v_idx, int qp_idx)
 	q_vector->tx.ring = tx_ring;
 	q_vector->tx.count++;
 
+	/* Place XDP Tx ring in the same q_vector ring list as regular Tx */
+	if (i40e_enabled_xdp_vsi(vsi)) {
+		struct i40e_ring *xdp_ring = vsi->xdp_rings[qp_idx];
+
+		xdp_ring->q_vector = q_vector;
+		xdp_ring->next = q_vector->tx.ring;
+		q_vector->tx.ring = xdp_ring;
+		q_vector->tx.count++;
+	}
+
 	rx_ring->q_vector = q_vector;
 	rx_ring->next = q_vector->rx.ring;
 	q_vector->rx.ring = rx_ring;
@@ -3995,6 +4095,33 @@ static void i40e_control_tx_q(struct i40e_pf *pf, int pf_q, bool enable)
 }
 
 /**
+ * i40e_control_wait_tx_q - Start/stop Tx queue and wait for completion
+ * @seid: VSI SEID
+ * @pf: the PF structure
+ * @pf_q: the PF queue to configure
+ * @is_xdp: true if the queue is used for XDP
+ * @enable: start or stop the queue
+ **/
+static int i40e_control_wait_tx_q(int seid, struct i40e_pf *pf, int pf_q,
+				  bool is_xdp, bool enable)
+{
+	int ret;
+
+	i40e_control_tx_q(pf, pf_q, enable);
+
+	/* wait for the change to finish */
+	ret = i40e_pf_txq_wait(pf, pf_q, enable);
+	if (ret) {
+		dev_info(&pf->pdev->dev,
+			 "VSI seid %d %sTx ring %d %sable timeout\n",
+			 seid, (is_xdp ? "XDP " : ""), pf_q,
+			 (enable ? "en" : "dis"));
+	}
+
+	return ret;
+}
+
+/**
  * i40e_vsi_control_tx - Start or stop a VSI's rings
  * @vsi: the VSI being configured
  * @enable: start or stop the rings
@@ -4006,16 +4133,20 @@ static int i40e_vsi_control_tx(struct i40e_vsi *vsi, bool enable)
 
 	pf_q = vsi->base_queue;
 	for (i = 0; i < vsi->num_queue_pairs; i++, pf_q++) {
-		i40e_control_tx_q(pf, pf_q, enable);
+		ret = i40e_control_wait_tx_q(vsi->seid, pf,
+					     pf_q,
+					     false /*is xdp*/, enable);
+		if (ret)
+			break;
 
-		/* wait for the change to finish */
-		ret = i40e_pf_txq_wait(pf, pf_q, enable);
-		if (ret) {
-			dev_info(&pf->pdev->dev,
-				 "VSI seid %d Tx ring %d %sable timeout\n",
-				 vsi->seid, pf_q, (enable ? "en" : "dis"));
+		if (!i40e_enabled_xdp_vsi(vsi))
+			continue;
+
+		ret = i40e_control_wait_tx_q(vsi->seid, pf,
+					     pf_q + vsi->alloc_queue_pairs,
+					     true /*is xdp*/, enable);
+		if (ret)
 			break;
-		}
 	}
 
 	return ret;
@@ -4527,7 +4658,21 @@ int i40e_vsi_wait_queues_disabled(struct i40e_vsi *vsi)
 				 vsi->seid, pf_q);
 			return ret;
 		}
-		/* Check and wait for the Tx queue */
+
+		if (!i40e_enabled_xdp_vsi(vsi))
+			goto wait_rx;
+
+		/* Check and wait for the XDP Tx queue */
+		ret = i40e_pf_txq_wait(pf, pf_q + vsi->alloc_queue_pairs,
+				       false);
+		if (ret) {
+			dev_info(&pf->pdev->dev,
+				 "VSI seid %d XDP Tx ring %d disable timeout\n",
+				 vsi->seid, pf_q);
+			return ret;
+		}
+wait_rx:
+		/* Check and wait for the Rx queue */
 		ret = i40e_pf_rxq_wait(pf, pf_q, false);
 		if (ret) {
 			dev_info(&pf->pdev->dev,
@@ -5446,6 +5591,8 @@ void i40e_down(struct i40e_vsi *vsi)
 
 	for (i = 0; i < vsi->num_queue_pairs; i++) {
 		i40e_clean_tx_ring(vsi->tx_rings[i]);
+		if (i40e_enabled_xdp_vsi(vsi))
+			i40e_clean_tx_ring(vsi->xdp_rings[i]);
 		i40e_clean_rx_ring(vsi->rx_rings[i]);
 	}
 
@@ -5509,7 +5656,8 @@ exit:
 	return ret;
 }
 
-static int __i40e_setup_tc(struct net_device *netdev, u32 handle, __be16 proto,
+static int __i40e_setup_tc(struct net_device *netdev, u32 handle,
+			   u32 chain_index, __be16 proto,
 			   struct tc_to_netdev *tc)
 {
 	if (tc->type != TC_SETUP_MQPRIO)
@@ -6372,7 +6520,8 @@ static void i40e_watchdog_subtask(struct i40e_pf *pf)
 				i40e_update_veb_stats(pf->veb[i]);
 	}
 
-	i40e_ptp_rx_hang(pf->vsi[pf->lan_vsi]);
+	i40e_ptp_rx_hang(pf);
+	i40e_ptp_tx_hang(pf);
 }
 
 /**
@@ -6417,9 +6566,7 @@ static void i40e_reset_subtask(struct i40e_pf *pf)
 	if (reset_flags &&
 	    !test_bit(__I40E_DOWN, pf->state) &&
 	    !test_bit(__I40E_CONFIG_BUSY, pf->state)) {
-		rtnl_lock();
-		i40e_do_reset(pf, reset_flags, true);
-		rtnl_unlock();
+		i40e_do_reset(pf, reset_flags, false);
 	}
 }
 
@@ -6968,6 +7115,51 @@ static void i40e_send_version(struct i40e_pf *pf)
 }
 
 /**
+ * i40e_get_oem_version - get OEM specific version information
+ * @hw: pointer to the hardware structure
+ **/
+static void i40e_get_oem_version(struct i40e_hw *hw)
+{
+	u16 block_offset = 0xffff;
+	u16 block_length = 0;
+	u16 capabilities = 0;
+	u16 gen_snap = 0;
+	u16 release = 0;
+
+#define I40E_SR_NVM_OEM_VERSION_PTR		0x1B
+#define I40E_NVM_OEM_LENGTH_OFFSET		0x00
+#define I40E_NVM_OEM_CAPABILITIES_OFFSET	0x01
+#define I40E_NVM_OEM_GEN_OFFSET			0x02
+#define I40E_NVM_OEM_RELEASE_OFFSET		0x03
+#define I40E_NVM_OEM_CAPABILITIES_MASK		0x000F
+#define I40E_NVM_OEM_LENGTH			3
+
+	/* Check if pointer to OEM version block is valid. */
+	i40e_read_nvm_word(hw, I40E_SR_NVM_OEM_VERSION_PTR, &block_offset);
+	if (block_offset == 0xffff)
+		return;
+
+	/* Check if OEM version block has correct length. */
+	i40e_read_nvm_word(hw, block_offset + I40E_NVM_OEM_LENGTH_OFFSET,
+			   &block_length);
+	if (block_length < I40E_NVM_OEM_LENGTH)
+		return;
+
+	/* Check if OEM version format is as expected. */
+	i40e_read_nvm_word(hw, block_offset + I40E_NVM_OEM_CAPABILITIES_OFFSET,
+			   &capabilities);
+	if ((capabilities & I40E_NVM_OEM_CAPABILITIES_MASK) != 0)
+		return;
+
+	i40e_read_nvm_word(hw, block_offset + I40E_NVM_OEM_GEN_OFFSET,
+			   &gen_snap);
+	i40e_read_nvm_word(hw, block_offset + I40E_NVM_OEM_RELEASE_OFFSET,
+			   &release);
+	hw->nvm.oem_ver = (gen_snap << I40E_OEM_SNAP_SHIFT) | release;
+	hw->nvm.eetrack = I40E_OEM_EETRACK_ID;
+}
+
+/**
  * i40e_reset - wait for core reset to finish reset, reset pf if corer not seen
  * @pf: board private structure
  **/
@@ -7014,6 +7206,7 @@ static void i40e_rebuild(struct i40e_pf *pf, bool reinit, bool lock_acquired)
 			 i40e_aq_str(&pf->hw, pf->hw.aq.asq_last_status));
 		goto clear_recovery;
 	}
+	i40e_get_oem_version(&pf->hw);
 
 	/* re-verify the eeprom if we just had an EMP reset */
 	if (test_and_clear_bit(__I40E_EMP_RESET_INTR_RECEIVED, pf->state))
@@ -7513,15 +7706,22 @@ static int i40e_set_num_rings_in_vsi(struct i40e_vsi *vsi)
  **/
 static int i40e_vsi_alloc_arrays(struct i40e_vsi *vsi, bool alloc_qvectors)
 {
+	struct i40e_ring **next_rings;
 	int size;
 	int ret = 0;
 
-	/* allocate memory for both Tx and Rx ring pointers */
-	size = sizeof(struct i40e_ring *) * vsi->alloc_queue_pairs * 2;
+	/* allocate memory for both Tx, XDP Tx and Rx ring pointers */
+	size = sizeof(struct i40e_ring *) * vsi->alloc_queue_pairs *
+	       (i40e_enabled_xdp_vsi(vsi) ? 3 : 2);
 	vsi->tx_rings = kzalloc(size, GFP_KERNEL);
 	if (!vsi->tx_rings)
 		return -ENOMEM;
-	vsi->rx_rings = &vsi->tx_rings[vsi->alloc_queue_pairs];
+	next_rings = vsi->tx_rings + vsi->alloc_queue_pairs;
+	if (i40e_enabled_xdp_vsi(vsi)) {
+		vsi->xdp_rings = next_rings;
+		next_rings += vsi->alloc_queue_pairs;
+	}
+	vsi->rx_rings = next_rings;
 
 	if (alloc_qvectors) {
 		/* allocate memory for q_vector pointers */
@@ -7641,6 +7841,7 @@ static void i40e_vsi_free_arrays(struct i40e_vsi *vsi, bool free_qvectors)
 	kfree(vsi->tx_rings);
 	vsi->tx_rings = NULL;
 	vsi->rx_rings = NULL;
+	vsi->xdp_rings = NULL;
 }
 
 /**
@@ -7724,6 +7925,8 @@ static void i40e_vsi_clear_rings(struct i40e_vsi *vsi)
 			kfree_rcu(vsi->tx_rings[i], rcu);
 			vsi->tx_rings[i] = NULL;
 			vsi->rx_rings[i] = NULL;
+			if (vsi->xdp_rings)
+				vsi->xdp_rings[i] = NULL;
 		}
 	}
 }
@@ -7734,43 +7937,61 @@ static void i40e_vsi_clear_rings(struct i40e_vsi *vsi)
  **/
 static int i40e_alloc_rings(struct i40e_vsi *vsi)
 {
-	struct i40e_ring *tx_ring, *rx_ring;
+	int i, qpv = i40e_enabled_xdp_vsi(vsi) ? 3 : 2;
 	struct i40e_pf *pf = vsi->back;
-	int i;
+	struct i40e_ring *ring;
 
 	/* Set basic values in the rings to be used later during open() */
 	for (i = 0; i < vsi->alloc_queue_pairs; i++) {
 		/* allocate space for both Tx and Rx in one shot */
-		tx_ring = kzalloc(sizeof(struct i40e_ring) * 2, GFP_KERNEL);
-		if (!tx_ring)
+		ring = kcalloc(qpv, sizeof(struct i40e_ring), GFP_KERNEL);
+		if (!ring)
 			goto err_out;
 
-		tx_ring->queue_index = i;
-		tx_ring->reg_idx = vsi->base_queue + i;
-		tx_ring->ring_active = false;
-		tx_ring->vsi = vsi;
-		tx_ring->netdev = vsi->netdev;
-		tx_ring->dev = &pf->pdev->dev;
-		tx_ring->count = vsi->num_desc;
-		tx_ring->size = 0;
-		tx_ring->dcb_tc = 0;
+		ring->queue_index = i;
+		ring->reg_idx = vsi->base_queue + i;
+		ring->ring_active = false;
+		ring->vsi = vsi;
+		ring->netdev = vsi->netdev;
+		ring->dev = &pf->pdev->dev;
+		ring->count = vsi->num_desc;
+		ring->size = 0;
+		ring->dcb_tc = 0;
 		if (vsi->back->flags & I40E_FLAG_WB_ON_ITR_CAPABLE)
-			tx_ring->flags = I40E_TXR_FLAGS_WB_ON_ITR;
-		tx_ring->tx_itr_setting = pf->tx_itr_default;
-		vsi->tx_rings[i] = tx_ring;
-
-		rx_ring = &tx_ring[1];
-		rx_ring->queue_index = i;
-		rx_ring->reg_idx = vsi->base_queue + i;
-		rx_ring->ring_active = false;
-		rx_ring->vsi = vsi;
-		rx_ring->netdev = vsi->netdev;
-		rx_ring->dev = &pf->pdev->dev;
-		rx_ring->count = vsi->num_desc;
-		rx_ring->size = 0;
-		rx_ring->dcb_tc = 0;
-		rx_ring->rx_itr_setting = pf->rx_itr_default;
-		vsi->rx_rings[i] = rx_ring;
+			ring->flags = I40E_TXR_FLAGS_WB_ON_ITR;
+		ring->tx_itr_setting = pf->tx_itr_default;
+		vsi->tx_rings[i] = ring++;
+
+		if (!i40e_enabled_xdp_vsi(vsi))
+			goto setup_rx;
+
+		ring->queue_index = vsi->alloc_queue_pairs + i;
+		ring->reg_idx = vsi->base_queue + ring->queue_index;
+		ring->ring_active = false;
+		ring->vsi = vsi;
+		ring->netdev = NULL;
+		ring->dev = &pf->pdev->dev;
+		ring->count = vsi->num_desc;
+		ring->size = 0;
+		ring->dcb_tc = 0;
+		if (vsi->back->flags & I40E_FLAG_WB_ON_ITR_CAPABLE)
+			ring->flags = I40E_TXR_FLAGS_WB_ON_ITR;
+		set_ring_xdp(ring);
+		ring->tx_itr_setting = pf->tx_itr_default;
+		vsi->xdp_rings[i] = ring++;
+
+setup_rx:
+		ring->queue_index = i;
+		ring->reg_idx = vsi->base_queue + i;
+		ring->ring_active = false;
+		ring->vsi = vsi;
+		ring->netdev = vsi->netdev;
+		ring->dev = &pf->pdev->dev;
+		ring->count = vsi->num_desc;
+		ring->size = 0;
+		ring->dcb_tc = 0;
+		ring->rx_itr_setting = pf->rx_itr_default;
+		vsi->rx_rings[i] = ring;
 	}
 
 	return 0;
@@ -8572,10 +8793,10 @@ int i40e_reconfig_rss_queues(struct i40e_pf *pf, int queue_count)
 }
 
 /**
- * i40e_get_npar_bw_setting - Retrieve BW settings for this PF partition
+ * i40e_get_partition_bw_setting - Retrieve BW settings for this PF partition
  * @pf: board private structure
  **/
-i40e_status i40e_get_npar_bw_setting(struct i40e_pf *pf)
+i40e_status i40e_get_partition_bw_setting(struct i40e_pf *pf)
 {
 	i40e_status status;
 	bool min_valid, max_valid;
@@ -8586,27 +8807,27 @@ i40e_status i40e_get_npar_bw_setting(struct i40e_pf *pf)
 
 	if (!status) {
 		if (min_valid)
-			pf->npar_min_bw = min_bw;
+			pf->min_bw = min_bw;
 		if (max_valid)
-			pf->npar_max_bw = max_bw;
+			pf->max_bw = max_bw;
 	}
 
 	return status;
 }
 
 /**
- * i40e_set_npar_bw_setting - Set BW settings for this PF partition
+ * i40e_set_partition_bw_setting - Set BW settings for this PF partition
  * @pf: board private structure
  **/
-i40e_status i40e_set_npar_bw_setting(struct i40e_pf *pf)
+i40e_status i40e_set_partition_bw_setting(struct i40e_pf *pf)
 {
 	struct i40e_aqc_configure_partition_bw_data bw_data;
 	i40e_status status;
 
 	/* Set the valid bit for this PF */
 	bw_data.pf_valid_bits = cpu_to_le16(BIT(pf->hw.pf_id));
-	bw_data.max_bw[pf->hw.pf_id] = pf->npar_max_bw & I40E_ALT_BW_VALUE_MASK;
-	bw_data.min_bw[pf->hw.pf_id] = pf->npar_min_bw & I40E_ALT_BW_VALUE_MASK;
+	bw_data.max_bw[pf->hw.pf_id] = pf->max_bw & I40E_ALT_BW_VALUE_MASK;
+	bw_data.min_bw[pf->hw.pf_id] = pf->min_bw & I40E_ALT_BW_VALUE_MASK;
 
 	/* Set the new bandwidths */
 	status = i40e_aq_configure_partition_bw(&pf->hw, &bw_data, NULL);
@@ -8615,10 +8836,10 @@ i40e_status i40e_set_npar_bw_setting(struct i40e_pf *pf)
 }
 
 /**
- * i40e_commit_npar_bw_setting - Commit BW settings for this PF partition
+ * i40e_commit_partition_bw_setting - Commit BW settings for this PF partition
  * @pf: board private structure
  **/
-i40e_status i40e_commit_npar_bw_setting(struct i40e_pf *pf)
+i40e_status i40e_commit_partition_bw_setting(struct i40e_pf *pf)
 {
 	/* Commit temporary BW setting to permanent NVM image */
 	enum i40e_admin_queue_err last_aq_status;
@@ -8737,16 +8958,19 @@ static int i40e_sw_init(struct i40e_pf *pf)
 	if (pf->hw.func_caps.npar_enable || pf->hw.func_caps.flex10_enable) {
 		pf->flags |= I40E_FLAG_MFP_ENABLED;
 		dev_info(&pf->pdev->dev, "MFP mode Enabled\n");
-		if (i40e_get_npar_bw_setting(pf))
+		if (i40e_get_partition_bw_setting(pf)) {
 			dev_warn(&pf->pdev->dev,
-				 "Could not get NPAR bw settings\n");
-		else
+				 "Could not get partition bw settings\n");
+		} else {
 			dev_info(&pf->pdev->dev,
-				 "Min BW = %8.8x, Max BW = %8.8x\n",
-				 pf->npar_min_bw, pf->npar_max_bw);
+				 "Partition BW Min = %8.8x, Max = %8.8x\n",
+				 pf->min_bw, pf->max_bw);
+
+			/* nudge the Tx scheduler */
+			i40e_set_partition_bw_setting(pf);
+		}
 	}
 
-	/* FW/NVM is not yet fixed in this regard */
 	if ((pf->hw.func_caps.fd_filters_guaranteed > 0) ||
 	    (pf->hw.func_caps.fd_filters_best_effort > 0)) {
 		pf->flags |= I40E_FLAG_FD_ATR_ENABLED;
@@ -8821,11 +9045,12 @@ static int i40e_sw_init(struct i40e_pf *pf)
 		    (pf->hw.aq.api_min_ver > 4))) {
 		/* Supported in FW API version higher than 1.4 */
 		pf->flags |= I40E_FLAG_GENEVE_OFFLOAD_CAPABLE;
-		pf->flags = I40E_FLAG_HW_ATR_EVICT_CAPABLE;
-	} else {
-		pf->flags = I40E_FLAG_HW_ATR_EVICT_CAPABLE;
 	}
 
+	/* Enable HW ATR eviction if possible */
+	if (pf->flags & I40E_FLAG_HW_ATR_EVICT_CAPABLE)
+		pf->flags |= I40E_FLAG_HW_ATR_EVICT_ENABLED;
+
 	pf->eeprom_version = 0xDEAD;
 	pf->lan_veb = I40E_NO_VEB;
 	pf->lan_vsi = I40E_NO_VSI;
@@ -8848,10 +9073,6 @@ static int i40e_sw_init(struct i40e_pf *pf)
 
 	mutex_init(&pf->switch_mutex);
 
-	/* If NPAR is enabled nudge the Tx scheduler */
-	if (pf->hw.func_caps.npar_enable && (!i40e_get_npar_bw_setting(pf)))
-		i40e_set_npar_bw_setting(pf);
-
 sw_init_done:
 	return err;
 }
@@ -9308,6 +9529,72 @@ out_err:
 	return features & ~(NETIF_F_CSUM_MASK | NETIF_F_GSO_MASK);
 }
 
+/**
+ * i40e_xdp_setup - add/remove an XDP program
+ * @vsi: VSI to changed
+ * @prog: XDP program
+ **/
+static int i40e_xdp_setup(struct i40e_vsi *vsi,
+			  struct bpf_prog *prog)
+{
+	int frame_size = vsi->netdev->mtu + ETH_HLEN + ETH_FCS_LEN + VLAN_HLEN;
+	struct i40e_pf *pf = vsi->back;
+	struct bpf_prog *old_prog;
+	bool need_reset;
+	int i;
+
+	/* Don't allow frames that span over multiple buffers */
+	if (frame_size > vsi->rx_buf_len)
+		return -EINVAL;
+
+	if (!i40e_enabled_xdp_vsi(vsi) && !prog)
+		return 0;
+
+	/* When turning XDP on->off/off->on we reset and rebuild the rings. */
+	need_reset = (i40e_enabled_xdp_vsi(vsi) != !!prog);
+
+	if (need_reset)
+		i40e_prep_for_reset(pf, true);
+
+	old_prog = xchg(&vsi->xdp_prog, prog);
+
+	if (need_reset)
+		i40e_reset_and_rebuild(pf, true, true);
+
+	for (i = 0; i < vsi->num_queue_pairs; i++)
+		WRITE_ONCE(vsi->rx_rings[i]->xdp_prog, vsi->xdp_prog);
+
+	if (old_prog)
+		bpf_prog_put(old_prog);
+
+	return 0;
+}
+
+/**
+ * i40e_xdp - implements ndo_xdp for i40e
+ * @dev: netdevice
+ * @xdp: XDP command
+ **/
+static int i40e_xdp(struct net_device *dev,
+		    struct netdev_xdp *xdp)
+{
+	struct i40e_netdev_priv *np = netdev_priv(dev);
+	struct i40e_vsi *vsi = np->vsi;
+
+	if (vsi->type != I40E_VSI_MAIN)
+		return -EINVAL;
+
+	switch (xdp->command) {
+	case XDP_SETUP_PROG:
+		return i40e_xdp_setup(vsi, xdp->prog);
+	case XDP_QUERY_PROG:
+		xdp->prog_attached = i40e_enabled_xdp_vsi(vsi);
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
 static const struct net_device_ops i40e_netdev_ops = {
 	.ndo_open		= i40e_open,
 	.ndo_stop		= i40e_close,
@@ -9340,6 +9627,7 @@ static const struct net_device_ops i40e_netdev_ops = {
 	.ndo_features_check	= i40e_features_check,
 	.ndo_bridge_getlink	= i40e_ndo_bridge_getlink,
 	.ndo_bridge_setlink	= i40e_ndo_bridge_setlink,
+	.ndo_xdp		= i40e_xdp,
 };
 
 /**
@@ -9908,6 +10196,7 @@ vector_setup_out:
  **/
 static struct i40e_vsi *i40e_vsi_reinit_setup(struct i40e_vsi *vsi)
 {
+	u16 alloc_queue_pairs;
 	struct i40e_pf *pf;
 	u8 enabled_tc;
 	int ret;
@@ -9926,11 +10215,14 @@ static struct i40e_vsi *i40e_vsi_reinit_setup(struct i40e_vsi *vsi)
 	if (ret)
 		goto err_vsi;
 
-	ret = i40e_get_lump(pf, pf->qp_pile, vsi->alloc_queue_pairs, vsi->idx);
+	alloc_queue_pairs = vsi->alloc_queue_pairs *
+			    (i40e_enabled_xdp_vsi(vsi) ? 2 : 1);
+
+	ret = i40e_get_lump(pf, pf->qp_pile, alloc_queue_pairs, vsi->idx);
 	if (ret < 0) {
 		dev_info(&pf->pdev->dev,
 			 "failed to get tracking for %d queues for VSI %d err %d\n",
-			 vsi->alloc_queue_pairs, vsi->seid, ret);
+			 alloc_queue_pairs, vsi->seid, ret);
 		goto err_vsi;
 	}
 	vsi->base_queue = ret;
@@ -9986,6 +10278,7 @@ struct i40e_vsi *i40e_vsi_setup(struct i40e_pf *pf, u8 type,
 {
 	struct i40e_vsi *vsi = NULL;
 	struct i40e_veb *veb = NULL;
+	u16 alloc_queue_pairs;
 	int ret, i;
 	int v_idx;
 
@@ -10073,12 +10366,14 @@ struct i40e_vsi *i40e_vsi_setup(struct i40e_pf *pf, u8 type,
 	else if (type == I40E_VSI_SRIOV)
 		vsi->vf_id = param1;
 	/* assign it some queues */
-	ret = i40e_get_lump(pf, pf->qp_pile, vsi->alloc_queue_pairs,
-				vsi->idx);
+	alloc_queue_pairs = vsi->alloc_queue_pairs *
+			    (i40e_enabled_xdp_vsi(vsi) ? 2 : 1);
+
+	ret = i40e_get_lump(pf, pf->qp_pile, alloc_queue_pairs, vsi->idx);
 	if (ret < 0) {
 		dev_info(&pf->pdev->dev,
 			 "failed to get tracking for %d queues for VSI %d err=%d\n",
-			 vsi->alloc_queue_pairs, vsi->seid, ret);
+			 alloc_queue_pairs, vsi->seid, ret);
 		goto err_vsi;
 	}
 	vsi->base_queue = ret;
@@ -11096,6 +11391,7 @@ static int i40e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 		goto err_pf_reset;
 	}
+	i40e_get_oem_version(hw);
 
 	/* provide nvm, fw, api versions */
 	dev_info(&pdev->dev, "fw %d.%d.%05d api %d.%d nvm %s\n",
@@ -11608,11 +11904,8 @@ static pci_ers_result_t i40e_pci_error_detected(struct pci_dev *pdev,
 	}
 
 	/* shutdown all operations */
-	if (!test_bit(__I40E_SUSPENDED, pf->state)) {
-		rtnl_lock();
-		i40e_prep_for_reset(pf, true);
-		rtnl_unlock();
-	}
+	if (!test_bit(__I40E_SUSPENDED, pf->state))
+		i40e_prep_for_reset(pf, false);
 
 	/* Request a slot reset */
 	return PCI_ERS_RESULT_NEED_RESET;
@@ -11678,9 +11971,7 @@ static void i40e_pci_error_resume(struct pci_dev *pdev)
 	if (test_bit(__I40E_SUSPENDED, pf->state))
 		return;
 
-	rtnl_lock();
-	i40e_handle_reset_warning(pf, true);
-	rtnl_unlock();
+	i40e_handle_reset_warning(pf, false);
 }
 
 /**
@@ -11760,9 +12051,7 @@ static void i40e_shutdown(struct pci_dev *pdev)
 	if (pf->wol_en && (pf->flags & I40E_FLAG_WOL_MC_MAGIC_PKT_WAKE))
 		i40e_enable_mc_magic_wake(pf);
 
-	rtnl_lock();
-	i40e_prep_for_reset(pf, true);
-	rtnl_unlock();
+	i40e_prep_for_reset(pf, false);
 
 	wr32(hw, I40E_PFPM_APM,
 	     (pf->wol_en ? I40E_PFPM_APM_APME_MASK : 0));
@@ -11794,9 +12083,7 @@ static int i40e_suspend(struct pci_dev *pdev, pm_message_t state)
 	if (pf->wol_en && (pf->flags & I40E_FLAG_WOL_MC_MAGIC_PKT_WAKE))
 		i40e_enable_mc_magic_wake(pf);
 
-	rtnl_lock();
-	i40e_prep_for_reset(pf, true);
-	rtnl_unlock();
+	i40e_prep_for_reset(pf, false);
 
 	wr32(hw, I40E_PFPM_APM, (pf->wol_en ? I40E_PFPM_APM_APME_MASK : 0));
 	wr32(hw, I40E_PFPM_WUFC, (pf->wol_en ? I40E_PFPM_WUFC_MAG_MASK : 0));
@@ -11842,9 +12129,7 @@ static int i40e_resume(struct pci_dev *pdev)
 	/* handling the reset will rebuild the device state */
 	if (test_and_clear_bit(__I40E_SUSPENDED, pf->state)) {
 		clear_bit(__I40E_DOWN, pf->state);
-		rtnl_lock();
-		i40e_reset_and_rebuild(pf, false, true);
-		rtnl_unlock();
+		i40e_reset_and_rebuild(pf, false, false);
 	}
 
 	return 0;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_prototype.h b/drivers/net/ethernet/intel/i40e/i40e_prototype.h
index c56d976cf85a..df613ea40313 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_prototype.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_prototype.h
@@ -29,7 +29,7 @@
 
 #include "i40e_type.h"
 #include "i40e_alloc.h"
-#include "i40e_virtchnl.h"
+#include <linux/avf/virtchnl.h>
 
 /* Prototypes for shared code functions that are not in
  * the standard function pointer structures.  These are
@@ -333,10 +333,10 @@ static inline struct i40e_rx_ptype_decoded decode_rx_desc_ptype(u8 ptype)
 
 /* i40e_common for VF drivers*/
 void i40e_vf_parse_hw_config(struct i40e_hw *hw,
-			     struct i40e_virtchnl_vf_resource *msg);
+			     struct virtchnl_vf_resource *msg);
 i40e_status i40e_vf_reset(struct i40e_hw *hw);
 i40e_status i40e_aq_send_msg_to_pf(struct i40e_hw *hw,
-				enum i40e_virtchnl_ops v_opcode,
+				enum virtchnl_ops v_opcode,
 				i40e_status v_retval,
 				u8 *msg, u16 msglen,
 				struct i40e_asq_cmd_details *cmd_details);
diff --git a/drivers/net/ethernet/intel/i40e/i40e_ptp.c b/drivers/net/ethernet/intel/i40e/i40e_ptp.c
index 18c1cc08da97..1a0be835fa06 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_ptp.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_ptp.c
@@ -269,6 +269,7 @@ static u32 i40e_ptp_get_rx_events(struct i40e_pf *pf)
 
 /**
  * i40e_ptp_rx_hang - Detect error case when Rx timestamp registers are hung
+ * @pf: The PF private data structure
  * @vsi: The VSI with the rings relevant to 1588
  *
  * This watchdog task is scheduled to detect error case where hardware has
@@ -276,9 +277,8 @@ static u32 i40e_ptp_get_rx_events(struct i40e_pf *pf)
  * particular error is rare but leaves the device in a state unable to timestamp
  * any future packets.
  **/
-void i40e_ptp_rx_hang(struct i40e_vsi *vsi)
+void i40e_ptp_rx_hang(struct i40e_pf *pf)
 {
-	struct i40e_pf *pf = vsi->back;
 	struct i40e_hw *hw = &pf->hw;
 	unsigned int i, cleared = 0;
 
@@ -328,6 +328,36 @@ void i40e_ptp_rx_hang(struct i40e_vsi *vsi)
 }
 
 /**
+ * i40e_ptp_tx_hang - Detect error case when Tx timestamp register is hung
+ * @pf: The PF private data structure
+ *
+ * This watchdog task is run periodically to make sure that we clear the Tx
+ * timestamp logic if we don't obtain a timestamp in a reasonable amount of
+ * time. It is unexpected in the normal case but if it occurs it results in
+ * permanently prevent timestamps of future packets
+ **/
+void i40e_ptp_tx_hang(struct i40e_pf *pf)
+{
+	if (!(pf->flags & I40E_FLAG_PTP) || !pf->ptp_tx)
+		return;
+
+	/* Nothing to do if we're not already waiting for a timestamp */
+	if (!test_bit(__I40E_PTP_TX_IN_PROGRESS, pf->state))
+		return;
+
+	/* We already have a handler routine which is run when we are notified
+	 * of a Tx timestamp in the hardware. If we don't get an interrupt
+	 * within a second it is reasonable to assume that we never will.
+	 */
+	if (time_is_before_jiffies(pf->ptp_tx_start + HZ)) {
+		dev_kfree_skb_any(pf->ptp_tx_skb);
+		pf->ptp_tx_skb = NULL;
+		clear_bit_unlock(__I40E_PTP_TX_IN_PROGRESS, pf->state);
+		pf->tx_hwtstamp_timeouts++;
+	}
+}
+
+/**
  * i40e_ptp_tx_hwtstamp - Utility function which returns the Tx timestamp
  * @pf: Board private structure
  *
@@ -338,6 +368,7 @@ void i40e_ptp_rx_hang(struct i40e_vsi *vsi)
 void i40e_ptp_tx_hwtstamp(struct i40e_pf *pf)
 {
 	struct skb_shared_hwtstamps shhwtstamps;
+	struct sk_buff *skb = pf->ptp_tx_skb;
 	struct i40e_hw *hw = &pf->hw;
 	u32 hi, lo;
 	u64 ns;
@@ -353,12 +384,19 @@ void i40e_ptp_tx_hwtstamp(struct i40e_pf *pf)
 	hi = rd32(hw, I40E_PRTTSYN_TXTIME_H);
 
 	ns = (((u64)hi) << 32) | lo;
-
 	i40e_ptp_convert_to_hwtstamp(&shhwtstamps, ns);
-	skb_tstamp_tx(pf->ptp_tx_skb, &shhwtstamps);
-	dev_kfree_skb_any(pf->ptp_tx_skb);
+
+	/* Clear the bit lock as soon as possible after reading the register,
+	 * and prior to notifying the stack via skb_tstamp_tx(). Otherwise
+	 * applications might wake up and attempt to request another transmit
+	 * timestamp prior to the bit lock being cleared.
+	 */
 	pf->ptp_tx_skb = NULL;
 	clear_bit_unlock(__I40E_PTP_TX_IN_PROGRESS, pf->state);
+
+	/* Notify the stack and free the skb after we've unlocked */
+	skb_tstamp_tx(skb, &shhwtstamps);
+	dev_kfree_skb_any(skb);
 }
 
 /**
@@ -562,6 +600,7 @@ static int i40e_ptp_set_timestamp_mode(struct i40e_pf *pf,
 			config->rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
 		}
 		break;
+	case HWTSTAMP_FILTER_NTP_ALL:
 	case HWTSTAMP_FILTER_ALL:
 	default:
 		return -ERANGE;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.c b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
index cd894f4023b1..b936febc315a 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
@@ -26,6 +26,7 @@
 
 #include <linux/prefetch.h>
 #include <net/busy_poll.h>
+#include <linux/bpf_trace.h>
 #include "i40e.h"
 #include "i40e_trace.h"
 #include "i40e_prototype.h"
@@ -629,6 +630,8 @@ static void i40e_unmap_and_free_tx_resource(struct i40e_ring *ring,
 	if (tx_buffer->skb) {
 		if (tx_buffer->tx_flags & I40E_TX_FLAGS_FD_SB)
 			kfree(tx_buffer->raw_buf);
+		else if (ring_is_xdp(ring))
+			page_frag_free(tx_buffer->raw_buf);
 		else
 			dev_kfree_skb_any(tx_buffer->skb);
 		if (dma_unmap_len(tx_buffer, len))
@@ -770,8 +773,11 @@ static bool i40e_clean_tx_irq(struct i40e_vsi *vsi,
 		total_bytes += tx_buf->bytecount;
 		total_packets += tx_buf->gso_segs;
 
-		/* free the skb */
-		napi_consume_skb(tx_buf->skb, napi_budget);
+		/* free the skb/XDP data */
+		if (ring_is_xdp(tx_ring))
+			page_frag_free(tx_buf->raw_buf);
+		else
+			napi_consume_skb(tx_buf->skb, napi_budget);
 
 		/* unmap skb header data */
 		dma_unmap_single(tx_ring->dev,
@@ -847,6 +853,9 @@ static bool i40e_clean_tx_irq(struct i40e_vsi *vsi,
 			tx_ring->arm_wb = true;
 	}
 
+	if (ring_is_xdp(tx_ring))
+		return !!budget;
+
 	/* notify netdev of completed buffers */
 	netdev_tx_completed_queue(txring_txq(tx_ring),
 				  total_packets, total_bytes);
@@ -1195,6 +1204,7 @@ void i40e_clean_rx_ring(struct i40e_ring *rx_ring)
 void i40e_free_rx_resources(struct i40e_ring *rx_ring)
 {
 	i40e_clean_rx_ring(rx_ring);
+	rx_ring->xdp_prog = NULL;
 	kfree(rx_ring->rx_bi);
 	rx_ring->rx_bi = NULL;
 
@@ -1241,6 +1251,8 @@ int i40e_setup_rx_descriptors(struct i40e_ring *rx_ring)
 	rx_ring->next_to_clean = 0;
 	rx_ring->next_to_use = 0;
 
+	rx_ring->xdp_prog = rx_ring->vsi->xdp_prog;
+
 	return 0;
 err:
 	kfree(rx_ring->rx_bi);
@@ -1593,6 +1605,7 @@ void i40e_process_skb_fields(struct i40e_ring *rx_ring,
  * i40e_cleanup_headers - Correct empty headers
  * @rx_ring: rx descriptor ring packet is being transacted on
  * @skb: pointer to current skb being fixed
+ * @rx_desc: pointer to the EOP Rx descriptor
  *
  * Also address the case where we are pulling data in on pages only
  * and as such no data is present in the skb header.
@@ -1602,8 +1615,25 @@ void i40e_process_skb_fields(struct i40e_ring *rx_ring,
  *
  * Returns true if an error was encountered and skb was freed.
  **/
-static bool i40e_cleanup_headers(struct i40e_ring *rx_ring, struct sk_buff *skb)
+static bool i40e_cleanup_headers(struct i40e_ring *rx_ring, struct sk_buff *skb,
+				 union i40e_rx_desc *rx_desc)
+
 {
+	/* XDP packets use error pointer so abort at this point */
+	if (IS_ERR(skb))
+		return true;
+
+	/* ERR_MASK will only have valid bits if EOP set, and
+	 * what we are doing here is actually checking
+	 * I40E_RX_DESC_ERROR_RXE_SHIFT, since it is the zeroth bit in
+	 * the error field
+	 */
+	if (unlikely(i40e_test_staterr(rx_desc,
+				       BIT(I40E_RXD_QW1_ERROR_SHIFT)))) {
+		dev_kfree_skb_any(skb);
+		return true;
+	}
+
 	/* if eth_skb_pad returns an error the skb was freed */
 	if (eth_skb_pad(skb))
 		return true;
@@ -1776,7 +1806,7 @@ static struct i40e_rx_buffer *i40e_get_rx_buffer(struct i40e_ring *rx_ring,
  * i40e_construct_skb - Allocate skb and populate it
  * @rx_ring: rx descriptor ring to transact packets on
  * @rx_buffer: rx buffer to pull data from
- * @size: size of buffer to add to skb
+ * @xdp: xdp_buff pointing to the data
  *
  * This function allocates an skb.  It then populates it with the page
  * data from the current receive descriptor, taking care to set up the
@@ -1784,9 +1814,9 @@ static struct i40e_rx_buffer *i40e_get_rx_buffer(struct i40e_ring *rx_ring,
  */
 static struct sk_buff *i40e_construct_skb(struct i40e_ring *rx_ring,
 					  struct i40e_rx_buffer *rx_buffer,
-					  unsigned int size)
+					  struct xdp_buff *xdp)
 {
-	void *va = page_address(rx_buffer->page) + rx_buffer->page_offset;
+	unsigned int size = xdp->data_end - xdp->data;
 #if (PAGE_SIZE < 8192)
 	unsigned int truesize = i40e_rx_pg_size(rx_ring) / 2;
 #else
@@ -1796,9 +1826,9 @@ static struct sk_buff *i40e_construct_skb(struct i40e_ring *rx_ring,
 	struct sk_buff *skb;
 
 	/* prefetch first cache line of first page */
-	prefetch(va);
+	prefetch(xdp->data);
 #if L1_CACHE_BYTES < 128
-	prefetch(va + L1_CACHE_BYTES);
+	prefetch(xdp->data + L1_CACHE_BYTES);
 #endif
 
 	/* allocate a skb to store the frags */
@@ -1811,10 +1841,11 @@ static struct sk_buff *i40e_construct_skb(struct i40e_ring *rx_ring,
 	/* Determine available headroom for copy */
 	headlen = size;
 	if (headlen > I40E_RX_HDR_SIZE)
-		headlen = eth_get_headlen(va, I40E_RX_HDR_SIZE);
+		headlen = eth_get_headlen(xdp->data, I40E_RX_HDR_SIZE);
 
 	/* align pull length to size of long to optimize memcpy performance */
-	memcpy(__skb_put(skb, headlen), va, ALIGN(headlen, sizeof(long)));
+	memcpy(__skb_put(skb, headlen), xdp->data,
+	       ALIGN(headlen, sizeof(long)));
 
 	/* update all of the pointers */
 	size -= headlen;
@@ -1841,16 +1872,16 @@ static struct sk_buff *i40e_construct_skb(struct i40e_ring *rx_ring,
  * i40e_build_skb - Build skb around an existing buffer
  * @rx_ring: Rx descriptor ring to transact packets on
  * @rx_buffer: Rx buffer to pull data from
- * @size: size of buffer to add to skb
+ * @xdp: xdp_buff pointing to the data
  *
  * This function builds an skb around an existing Rx buffer, taking care
  * to set up the skb correctly and avoid any memcpy overhead.
  */
 static struct sk_buff *i40e_build_skb(struct i40e_ring *rx_ring,
 				      struct i40e_rx_buffer *rx_buffer,
-				      unsigned int size)
+				      struct xdp_buff *xdp)
 {
-	void *va = page_address(rx_buffer->page) + rx_buffer->page_offset;
+	unsigned int size = xdp->data_end - xdp->data;
 #if (PAGE_SIZE < 8192)
 	unsigned int truesize = i40e_rx_pg_size(rx_ring) / 2;
 #else
@@ -1860,12 +1891,12 @@ static struct sk_buff *i40e_build_skb(struct i40e_ring *rx_ring,
 	struct sk_buff *skb;
 
 	/* prefetch first cache line of first page */
-	prefetch(va);
+	prefetch(xdp->data);
 #if L1_CACHE_BYTES < 128
-	prefetch(va + L1_CACHE_BYTES);
+	prefetch(xdp->data + L1_CACHE_BYTES);
 #endif
 	/* build an skb around the page buffer */
-	skb = build_skb(va - I40E_SKB_PAD, truesize);
+	skb = build_skb(xdp->data_hard_start, truesize);
 	if (unlikely(!skb))
 		return NULL;
 
@@ -1944,6 +1975,75 @@ static bool i40e_is_non_eop(struct i40e_ring *rx_ring,
 	return true;
 }
 
+#define I40E_XDP_PASS 0
+#define I40E_XDP_CONSUMED 1
+#define I40E_XDP_TX 2
+
+static int i40e_xmit_xdp_ring(struct xdp_buff *xdp,
+			      struct i40e_ring *xdp_ring);
+
+/**
+ * i40e_run_xdp - run an XDP program
+ * @rx_ring: Rx ring being processed
+ * @xdp: XDP buffer containing the frame
+ **/
+static struct sk_buff *i40e_run_xdp(struct i40e_ring *rx_ring,
+				    struct xdp_buff *xdp)
+{
+	int result = I40E_XDP_PASS;
+	struct i40e_ring *xdp_ring;
+	struct bpf_prog *xdp_prog;
+	u32 act;
+
+	rcu_read_lock();
+	xdp_prog = READ_ONCE(rx_ring->xdp_prog);
+
+	if (!xdp_prog)
+		goto xdp_out;
+
+	act = bpf_prog_run_xdp(xdp_prog, xdp);
+	switch (act) {
+	case XDP_PASS:
+		break;
+	case XDP_TX:
+		xdp_ring = rx_ring->vsi->xdp_rings[rx_ring->queue_index];
+		result = i40e_xmit_xdp_ring(xdp, xdp_ring);
+		break;
+	default:
+		bpf_warn_invalid_xdp_action(act);
+	case XDP_ABORTED:
+		trace_xdp_exception(rx_ring->netdev, xdp_prog, act);
+		/* fallthrough -- handle aborts by dropping packet */
+	case XDP_DROP:
+		result = I40E_XDP_CONSUMED;
+		break;
+	}
+xdp_out:
+	rcu_read_unlock();
+	return ERR_PTR(-result);
+}
+
+/**
+ * i40e_rx_buffer_flip - adjusted rx_buffer to point to an unused region
+ * @rx_ring: Rx ring
+ * @rx_buffer: Rx buffer to adjust
+ * @size: Size of adjustment
+ **/
+static void i40e_rx_buffer_flip(struct i40e_ring *rx_ring,
+				struct i40e_rx_buffer *rx_buffer,
+				unsigned int size)
+{
+#if (PAGE_SIZE < 8192)
+	unsigned int truesize = i40e_rx_pg_size(rx_ring) / 2;
+
+	rx_buffer->page_offset ^= truesize;
+#else
+	unsigned int truesize = SKB_DATA_ALIGN(i40e_rx_offset(rx_ring) + size);
+
+	rx_buffer->page_offset += truesize;
+#endif
+}
+
 /**
  * i40e_clean_rx_irq - Clean completed descriptors from Rx ring - bounce buf
  * @rx_ring: rx descriptor ring to transact packets on
@@ -1961,11 +2061,12 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 	unsigned int total_rx_bytes = 0, total_rx_packets = 0;
 	struct sk_buff *skb = rx_ring->skb;
 	u16 cleaned_count = I40E_DESC_UNUSED(rx_ring);
-	bool failure = false;
+	bool failure = false, xdp_xmit = false;
 
 	while (likely(total_rx_packets < budget)) {
 		struct i40e_rx_buffer *rx_buffer;
 		union i40e_rx_desc *rx_desc;
+		struct xdp_buff xdp;
 		unsigned int size;
 		u16 vlan_tag;
 		u8 rx_ptype;
@@ -2006,12 +2107,32 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 		rx_buffer = i40e_get_rx_buffer(rx_ring, size);
 
 		/* retrieve a buffer from the ring */
-		if (skb)
+		if (!skb) {
+			xdp.data = page_address(rx_buffer->page) +
+				   rx_buffer->page_offset;
+			xdp.data_hard_start = xdp.data -
+					      i40e_rx_offset(rx_ring);
+			xdp.data_end = xdp.data + size;
+
+			skb = i40e_run_xdp(rx_ring, &xdp);
+		}
+
+		if (IS_ERR(skb)) {
+			if (PTR_ERR(skb) == -I40E_XDP_TX) {
+				xdp_xmit = true;
+				i40e_rx_buffer_flip(rx_ring, rx_buffer, size);
+			} else {
+				rx_buffer->pagecnt_bias++;
+			}
+			total_rx_bytes += size;
+			total_rx_packets++;
+		} else if (skb) {
 			i40e_add_rx_frag(rx_ring, rx_buffer, skb, size);
-		else if (ring_uses_build_skb(rx_ring))
-			skb = i40e_build_skb(rx_ring, rx_buffer, size);
-		else
-			skb = i40e_construct_skb(rx_ring, rx_buffer, size);
+		} else if (ring_uses_build_skb(rx_ring)) {
+			skb = i40e_build_skb(rx_ring, rx_buffer, &xdp);
+		} else {
+			skb = i40e_construct_skb(rx_ring, rx_buffer, &xdp);
+		}
 
 		/* exit if we failed to retrieve a buffer */
 		if (!skb) {
@@ -2026,18 +2147,7 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 		if (i40e_is_non_eop(rx_ring, rx_desc, skb))
 			continue;
 
-		/* ERR_MASK will only have valid bits if EOP set, and
-		 * what we are doing here is actually checking
-		 * I40E_RX_DESC_ERROR_RXE_SHIFT, since it is the zeroth bit in
-		 * the error field
-		 */
-		if (unlikely(i40e_test_staterr(rx_desc, BIT(I40E_RXD_QW1_ERROR_SHIFT)))) {
-			dev_kfree_skb_any(skb);
-			skb = NULL;
-			continue;
-		}
-
-		if (i40e_cleanup_headers(rx_ring, skb)) {
+		if (i40e_cleanup_headers(rx_ring, skb, rx_desc)) {
 			skb = NULL;
 			continue;
 		}
@@ -2063,6 +2173,19 @@ static int i40e_clean_rx_irq(struct i40e_ring *rx_ring, int budget)
 		total_rx_packets++;
 	}
 
+	if (xdp_xmit) {
+		struct i40e_ring *xdp_ring;
+
+		xdp_ring = rx_ring->vsi->xdp_rings[rx_ring->queue_index];
+
+		/* Force memory writes to complete before letting h/w
+		 * know there are new descriptors to fetch.
+		 */
+		wmb();
+
+		writel(xdp_ring->next_to_use, xdp_ring->tail);
+	}
+
 	rx_ring->skb = skb;
 
 	u64_stats_update_begin(&rx_ring->syncp);
@@ -2341,7 +2464,7 @@ static void i40e_atr(struct i40e_ring *tx_ring, struct sk_buff *skb,
 	/* Due to lack of space, no more new filters can be programmed */
 	if (th->syn && (pf->flags & I40E_FLAG_FD_ATR_AUTO_DISABLED))
 		return;
-	if (pf->flags & I40E_FLAG_HW_ATR_EVICT_CAPABLE) {
+	if (pf->flags & I40E_FLAG_HW_ATR_EVICT_ENABLED) {
 		/* HW ATR eviction will take care of removing filters on FIN
 		 * and RST packets.
 		 */
@@ -2403,7 +2526,7 @@ static void i40e_atr(struct i40e_ring *tx_ring, struct sk_buff *skb,
 			I40E_TXD_FLTR_QW1_CNTINDEX_SHIFT) &
 			I40E_TXD_FLTR_QW1_CNTINDEX_MASK;
 
-	if (pf->flags & I40E_FLAG_HW_ATR_EVICT_CAPABLE)
+	if (pf->flags & I40E_FLAG_HW_ATR_EVICT_ENABLED)
 		dtype_cmd |= I40E_TXD_FLTR_QW1_ATR_MASK;
 
 	fdir_desc->qindex_flex_ptype_vsi = cpu_to_le32(flex_ptype);
@@ -2629,8 +2752,10 @@ static int i40e_tsyn(struct i40e_ring *tx_ring, struct sk_buff *skb,
 	if (pf->ptp_tx &&
 	    !test_and_set_bit_lock(__I40E_PTP_TX_IN_PROGRESS, pf->state)) {
 		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+		pf->ptp_tx_start = jiffies;
 		pf->ptp_tx_skb = skb_get(skb);
 	} else {
+		pf->tx_hwtstamp_skipped++;
 		return 0;
 	}
 
@@ -2933,10 +3058,12 @@ bool __i40e_chk_linearize(struct sk_buff *skb)
  * @hdr_len:  size of the packet header
  * @td_cmd:   the command field in the descriptor
  * @td_offset: offset for checksum or crc
+ *
+ * Returns 0 on success, -1 on failure to DMA
  **/
-static inline void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
-			       struct i40e_tx_buffer *first, u32 tx_flags,
-			       const u8 hdr_len, u32 td_cmd, u32 td_offset)
+static inline int i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
+			      struct i40e_tx_buffer *first, u32 tx_flags,
+			      const u8 hdr_len, u32 td_cmd, u32 td_offset)
 {
 	unsigned int data_len = skb->data_len;
 	unsigned int size = skb_headlen(skb);
@@ -3094,7 +3221,7 @@ do_rs:
 		mmiowb();
 	}
 
-	return;
+	return 0;
 
 dma_error:
 	dev_info(tx_ring->dev, "TX DMA map failed\n");
@@ -3111,6 +3238,61 @@ dma_error:
 	}
 
 	tx_ring->next_to_use = i;
+
+	return -1;
+}
+
+/**
+ * i40e_xmit_xdp_ring - transmits an XDP buffer to an XDP Tx ring
+ * @xdp: data to transmit
+ * @xdp_ring: XDP Tx ring
+ **/
+static int i40e_xmit_xdp_ring(struct xdp_buff *xdp,
+			      struct i40e_ring *xdp_ring)
+{
+	u32 size = xdp->data_end - xdp->data;
+	u16 i = xdp_ring->next_to_use;
+	struct i40e_tx_buffer *tx_bi;
+	struct i40e_tx_desc *tx_desc;
+	dma_addr_t dma;
+
+	if (!unlikely(I40E_DESC_UNUSED(xdp_ring))) {
+		xdp_ring->tx_stats.tx_busy++;
+		return I40E_XDP_CONSUMED;
+	}
+
+	dma = dma_map_single(xdp_ring->dev, xdp->data, size, DMA_TO_DEVICE);
+	if (dma_mapping_error(xdp_ring->dev, dma))
+		return I40E_XDP_CONSUMED;
+
+	tx_bi = &xdp_ring->tx_bi[i];
+	tx_bi->bytecount = size;
+	tx_bi->gso_segs = 1;
+	tx_bi->raw_buf = xdp->data;
+
+	/* record length, and DMA address */
+	dma_unmap_len_set(tx_bi, len, size);
+	dma_unmap_addr_set(tx_bi, dma, dma);
+
+	tx_desc = I40E_TX_DESC(xdp_ring, i);
+	tx_desc->buffer_addr = cpu_to_le64(dma);
+	tx_desc->cmd_type_offset_bsz = build_ctob(I40E_TX_DESC_CMD_ICRC
+						  | I40E_TXD_CMD,
+						  0, size, 0);
+
+	/* Make certain all of the status bits have been updated
+	 * before next_to_watch is written.
+	 */
+	smp_wmb();
+
+	i++;
+	if (i == xdp_ring->count)
+		i = 0;
+
+	tx_bi->next_to_watch = tx_desc;
+	xdp_ring->next_to_use = i;
+
+	return I40E_XDP_TX;
 }
 
 /**
@@ -3211,8 +3393,9 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	 */
 	i40e_atr(tx_ring, skb, tx_flags);
 
-	i40e_tx_map(tx_ring, skb, first, tx_flags, hdr_len,
-		    td_cmd, td_offset);
+	if (i40e_tx_map(tx_ring, skb, first, tx_flags, hdr_len,
+			td_cmd, td_offset))
+		goto cleanup_tx_tstamp;
 
 	return NETDEV_TX_OK;
 
@@ -3220,6 +3403,15 @@ out_drop:
 	i40e_trace(xmit_frame_ring_drop, first->skb, tx_ring);
 	dev_kfree_skb_any(first->skb);
 	first->skb = NULL;
+cleanup_tx_tstamp:
+	if (unlikely(tx_flags & I40E_TX_FLAGS_TSYN)) {
+		struct i40e_pf *pf = i40e_netdev_to_pf(tx_ring->netdev);
+
+		dev_kfree_skb_any(pf->ptp_tx_skb);
+		pf->ptp_tx_skb = NULL;
+		clear_bit_unlock(__I40E_PTP_TX_IN_PROGRESS, pf->state);
+	}
+
 	return NETDEV_TX_OK;
 }
 
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.h b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
index f5de51124cae..b288d58313a6 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
@@ -360,6 +360,7 @@ struct i40e_ring {
 	void *desc;			/* Descriptor ring memory */
 	struct device *dev;		/* Used for DMA mapping */
 	struct net_device *netdev;	/* netdev ring maps to */
+	struct bpf_prog *xdp_prog;
 	union {
 		struct i40e_tx_buffer *tx_bi;
 		struct i40e_rx_buffer *rx_bi;
@@ -395,6 +396,7 @@ struct i40e_ring {
 	u16 flags;
 #define I40E_TXR_FLAGS_WB_ON_ITR		BIT(0)
 #define I40E_RXR_FLAGS_BUILD_SKB_ENABLED	BIT(1)
+#define I40E_TXR_FLAGS_XDP			BIT(2)
 
 	/* stats structs */
 	struct i40e_queue_stats	stats;
@@ -437,6 +439,16 @@ static inline void clear_ring_build_skb_enabled(struct i40e_ring *ring)
 	ring->flags &= ~I40E_RXR_FLAGS_BUILD_SKB_ENABLED;
 }
 
+static inline bool ring_is_xdp(struct i40e_ring *ring)
+{
+	return !!(ring->flags & I40E_TXR_FLAGS_XDP);
+}
+
+static inline void set_ring_xdp(struct i40e_ring *ring)
+{
+	ring->flags |= I40E_TXR_FLAGS_XDP;
+}
+
 enum i40e_latency_range {
 	I40E_LOWEST_LATENCY = 0,
 	I40E_LOW_LATENCY = 1,
diff --git a/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h b/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h
deleted file mode 100644
index 8552192a5bde..000000000000
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h
+++ /dev/null
@@ -1,449 +0,0 @@
-/*******************************************************************************
- *
- * Intel Ethernet Controller XL710 Family Linux Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- * The full GNU General Public License is included in this distribution in
- * the file called "COPYING".
- *
- * Contact Information:
- * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
- * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
- *
- ******************************************************************************/
-
-#ifndef _I40E_VIRTCHNL_H_
-#define _I40E_VIRTCHNL_H_
-
-#include "i40e_type.h"
-
-/* Description:
- * This header file describes the VF-PF communication protocol used
- * by the various i40e drivers.
- *
- * Admin queue buffer usage:
- * desc->opcode is always i40e_aqc_opc_send_msg_to_pf
- * flags, retval, datalen, and data addr are all used normally.
- * Firmware copies the cookie fields when sending messages between the PF and
- * VF, but uses all other fields internally. Due to this limitation, we
- * must send all messages as "indirect", i.e. using an external buffer.
- *
- * All the vsi indexes are relative to the VF. Each VF can have maximum of
- * three VSIs. All the queue indexes are relative to the VSI.  Each VF can
- * have a maximum of sixteen queues for all of its VSIs.
- *
- * The PF is required to return a status code in v_retval for all messages
- * except RESET_VF, which does not require any response. The return value is of
- * i40e_status_code type, defined in the i40e_type.h.
- *
- * In general, VF driver initialization should roughly follow the order of these
- * opcodes. The VF driver must first validate the API version of the PF driver,
- * then request a reset, then get resources, then configure queues and
- * interrupts. After these operations are complete, the VF driver may start
- * its queues, optionally add MAC and VLAN filters, and process traffic.
- */
-
-/* Opcodes for VF-PF communication. These are placed in the v_opcode field
- * of the virtchnl_msg structure.
- */
-enum i40e_virtchnl_ops {
-/* The PF sends status change events to VFs using
- * the I40E_VIRTCHNL_OP_EVENT opcode.
- * VFs send requests to the PF using the other ops.
- */
-	I40E_VIRTCHNL_OP_UNKNOWN = 0,
-	I40E_VIRTCHNL_OP_VERSION = 1, /* must ALWAYS be 1 */
-	I40E_VIRTCHNL_OP_RESET_VF = 2,
-	I40E_VIRTCHNL_OP_GET_VF_RESOURCES = 3,
-	I40E_VIRTCHNL_OP_CONFIG_TX_QUEUE = 4,
-	I40E_VIRTCHNL_OP_CONFIG_RX_QUEUE = 5,
-	I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES = 6,
-	I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP = 7,
-	I40E_VIRTCHNL_OP_ENABLE_QUEUES = 8,
-	I40E_VIRTCHNL_OP_DISABLE_QUEUES = 9,
-	I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS = 10,
-	I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS = 11,
-	I40E_VIRTCHNL_OP_ADD_VLAN = 12,
-	I40E_VIRTCHNL_OP_DEL_VLAN = 13,
-	I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE = 14,
-	I40E_VIRTCHNL_OP_GET_STATS = 15,
-	I40E_VIRTCHNL_OP_FCOE = 16,
-	I40E_VIRTCHNL_OP_EVENT = 17, /* must ALWAYS be 17 */
-	I40E_VIRTCHNL_OP_IWARP = 20,
-	I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP = 21,
-	I40E_VIRTCHNL_OP_RELEASE_IWARP_IRQ_MAP = 22,
-	I40E_VIRTCHNL_OP_CONFIG_RSS_KEY = 23,
-	I40E_VIRTCHNL_OP_CONFIG_RSS_LUT = 24,
-	I40E_VIRTCHNL_OP_GET_RSS_HENA_CAPS = 25,
-	I40E_VIRTCHNL_OP_SET_RSS_HENA = 26,
-
-};
-
-/* Virtual channel message descriptor. This overlays the admin queue
- * descriptor. All other data is passed in external buffers.
- */
-
-struct i40e_virtchnl_msg {
-	u8 pad[8];			 /* AQ flags/opcode/len/retval fields */
-	enum i40e_virtchnl_ops v_opcode; /* avoid confusion with desc->opcode */
-	i40e_status v_retval;  /* ditto for desc->retval */
-	u32 vfid;			 /* used by PF when sending to VF */
-};
-
-/* Message descriptions and data structures.*/
-
-/* I40E_VIRTCHNL_OP_VERSION
- * VF posts its version number to the PF. PF responds with its version number
- * in the same format, along with a return code.
- * Reply from PF has its major/minor versions also in param0 and param1.
- * If there is a major version mismatch, then the VF cannot operate.
- * If there is a minor version mismatch, then the VF can operate but should
- * add a warning to the system log.
- *
- * This enum element MUST always be specified as == 1, regardless of other
- * changes in the API. The PF must always respond to this message without
- * error regardless of version mismatch.
- */
-#define I40E_VIRTCHNL_VERSION_MAJOR		1
-#define I40E_VIRTCHNL_VERSION_MINOR		1
-#define I40E_VIRTCHNL_VERSION_MINOR_NO_VF_CAPS	0
-
-struct i40e_virtchnl_version_info {
-	u32 major;
-	u32 minor;
-};
-
-/* I40E_VIRTCHNL_OP_RESET_VF
- * VF sends this request to PF with no parameters
- * PF does NOT respond! VF driver must delay then poll VFGEN_RSTAT register
- * until reset completion is indicated. The admin queue must be reinitialized
- * after this operation.
- *
- * When reset is complete, PF must ensure that all queues in all VSIs associated
- * with the VF are stopped, all queue configurations in the HMC are set to 0,
- * and all MAC and VLAN filters (except the default MAC address) on all VSIs
- * are cleared.
- */
-
-/* I40E_VIRTCHNL_OP_GET_VF_RESOURCES
- * Version 1.0 VF sends this request to PF with no parameters
- * Version 1.1 VF sends this request to PF with u32 bitmap of its capabilities
- * PF responds with an indirect message containing
- * i40e_virtchnl_vf_resource and one or more
- * i40e_virtchnl_vsi_resource structures.
- */
-
-struct i40e_virtchnl_vsi_resource {
-	u16 vsi_id;
-	u16 num_queue_pairs;
-	enum i40e_vsi_type vsi_type;
-	u16 qset_handle;
-	u8 default_mac_addr[ETH_ALEN];
-};
-/* VF offload flags */
-#define I40E_VIRTCHNL_VF_OFFLOAD_L2		0x00000001
-#define I40E_VIRTCHNL_VF_OFFLOAD_IWARP		0x00000002
-#define I40E_VIRTCHNL_VF_OFFLOAD_FCOE		0x00000004
-#define I40E_VIRTCHNL_VF_OFFLOAD_RSS_AQ		0x00000008
-#define I40E_VIRTCHNL_VF_OFFLOAD_RSS_REG	0x00000010
-#define I40E_VIRTCHNL_VF_OFFLOAD_WB_ON_ITR	0x00000020
-#define I40E_VIRTCHNL_VF_OFFLOAD_VLAN		0x00010000
-#define I40E_VIRTCHNL_VF_OFFLOAD_RX_POLLING	0x00020000
-#define I40E_VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2	0x00040000
-#define I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF		0X00080000
-#define I40E_VIRTCHNL_VF_OFFLOAD_ENCAP		0X00100000
-#define I40E_VIRTCHNL_VF_OFFLOAD_ENCAP_CSUM	0X00200000
-
-#define I40E_VF_BASE_MODE_OFFLOADS (I40E_VIRTCHNL_VF_OFFLOAD_L2 | \
-				    I40E_VIRTCHNL_VF_OFFLOAD_VLAN | \
-				    I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF)
-
-struct i40e_virtchnl_vf_resource {
-	u16 num_vsis;
-	u16 num_queue_pairs;
-	u16 max_vectors;
-	u16 max_mtu;
-
-	u32 vf_offload_flags;
-	u32 rss_key_size;
-	u32 rss_lut_size;
-
-	struct i40e_virtchnl_vsi_resource vsi_res[1];
-};
-
-/* I40E_VIRTCHNL_OP_CONFIG_TX_QUEUE
- * VF sends this message to set up parameters for one TX queue.
- * External data buffer contains one instance of i40e_virtchnl_txq_info.
- * PF configures requested queue and returns a status code.
- */
-
-/* Tx queue config info */
-struct i40e_virtchnl_txq_info {
-	u16 vsi_id;
-	u16 queue_id;
-	u16 ring_len;		/* number of descriptors, multiple of 8 */
-	u16 headwb_enabled;
-	u64 dma_ring_addr;
-	u64 dma_headwb_addr;
-};
-
-/* I40E_VIRTCHNL_OP_CONFIG_RX_QUEUE
- * VF sends this message to set up parameters for one RX queue.
- * External data buffer contains one instance of i40e_virtchnl_rxq_info.
- * PF configures requested queue and returns a status code.
- */
-
-/* Rx queue config info */
-struct i40e_virtchnl_rxq_info {
-	u16 vsi_id;
-	u16 queue_id;
-	u32 ring_len;		/* number of descriptors, multiple of 32 */
-	u16 hdr_size;
-	u16 splithdr_enabled;
-	u32 databuffer_size;
-	u32 max_pkt_size;
-	u64 dma_ring_addr;
-	enum i40e_hmc_obj_rx_hsplit_0 rx_split_pos;
-};
-
-/* I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES
- * VF sends this message to set parameters for all active TX and RX queues
- * associated with the specified VSI.
- * PF configures queues and returns status.
- * If the number of queues specified is greater than the number of queues
- * associated with the VSI, an error is returned and no queues are configured.
- */
-struct i40e_virtchnl_queue_pair_info {
-	/* NOTE: vsi_id and queue_id should be identical for both queues. */
-	struct i40e_virtchnl_txq_info txq;
-	struct i40e_virtchnl_rxq_info rxq;
-};
-
-struct i40e_virtchnl_vsi_queue_config_info {
-	u16 vsi_id;
-	u16 num_queue_pairs;
-	struct i40e_virtchnl_queue_pair_info qpair[1];
-};
-
-/* I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP
- * VF uses this message to map vectors to queues.
- * The rxq_map and txq_map fields are bitmaps used to indicate which queues
- * are to be associated with the specified vector.
- * The "other" causes are always mapped to vector 0.
- * PF configures interrupt mapping and returns status.
- */
-struct i40e_virtchnl_vector_map {
-	u16 vsi_id;
-	u16 vector_id;
-	u16 rxq_map;
-	u16 txq_map;
-	u16 rxitr_idx;
-	u16 txitr_idx;
-};
-
-struct i40e_virtchnl_irq_map_info {
-	u16 num_vectors;
-	struct i40e_virtchnl_vector_map vecmap[1];
-};
-
-/* I40E_VIRTCHNL_OP_ENABLE_QUEUES
- * I40E_VIRTCHNL_OP_DISABLE_QUEUES
- * VF sends these message to enable or disable TX/RX queue pairs.
- * The queues fields are bitmaps indicating which queues to act upon.
- * (Currently, we only support 16 queues per VF, but we make the field
- * u32 to allow for expansion.)
- * PF performs requested action and returns status.
- */
-struct i40e_virtchnl_queue_select {
-	u16 vsi_id;
-	u16 pad;
-	u32 rx_queues;
-	u32 tx_queues;
-};
-
-/* I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS
- * VF sends this message in order to add one or more unicast or multicast
- * address filters for the specified VSI.
- * PF adds the filters and returns status.
- */
-
-/* I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS
- * VF sends this message in order to remove one or more unicast or multicast
- * filters for the specified VSI.
- * PF removes the filters and returns status.
- */
-
-struct i40e_virtchnl_ether_addr {
-	u8 addr[ETH_ALEN];
-	u8 pad[2];
-};
-
-struct i40e_virtchnl_ether_addr_list {
-	u16 vsi_id;
-	u16 num_elements;
-	struct i40e_virtchnl_ether_addr list[1];
-};
-
-/* I40E_VIRTCHNL_OP_ADD_VLAN
- * VF sends this message to add one or more VLAN tag filters for receives.
- * PF adds the filters and returns status.
- * If a port VLAN is configured by the PF, this operation will return an
- * error to the VF.
- */
-
-/* I40E_VIRTCHNL_OP_DEL_VLAN
- * VF sends this message to remove one or more VLAN tag filters for receives.
- * PF removes the filters and returns status.
- * If a port VLAN is configured by the PF, this operation will return an
- * error to the VF.
- */
-
-struct i40e_virtchnl_vlan_filter_list {
-	u16 vsi_id;
-	u16 num_elements;
-	u16 vlan_id[1];
-};
-
-/* I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE
- * VF sends VSI id and flags.
- * PF returns status code in retval.
- * Note: we assume that broadcast accept mode is always enabled.
- */
-struct i40e_virtchnl_promisc_info {
-	u16 vsi_id;
-	u16 flags;
-};
-
-#define I40E_FLAG_VF_UNICAST_PROMISC	0x00000001
-#define I40E_FLAG_VF_MULTICAST_PROMISC	0x00000002
-
-/* I40E_VIRTCHNL_OP_GET_STATS
- * VF sends this message to request stats for the selected VSI. VF uses
- * the i40e_virtchnl_queue_select struct to specify the VSI. The queue_id
- * field is ignored by the PF.
- *
- * PF replies with struct i40e_eth_stats in an external buffer.
- */
-
-/* I40E_VIRTCHNL_OP_CONFIG_RSS_KEY
- * I40E_VIRTCHNL_OP_CONFIG_RSS_LUT
- * VF sends these messages to configure RSS. Only supported if both PF
- * and VF drivers set the I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF bit during
- * configuration negotiation. If this is the case, then the RSS fields in
- * the VF resource struct are valid.
- * Both the key and LUT are initialized to 0 by the PF, meaning that
- * RSS is effectively disabled until set up by the VF.
- */
-struct i40e_virtchnl_rss_key {
-	u16 vsi_id;
-	u16 key_len;
-	u8 key[1];         /* RSS hash key, packed bytes */
-};
-
-struct i40e_virtchnl_rss_lut {
-	u16 vsi_id;
-	u16 lut_entries;
-	u8 lut[1];        /* RSS lookup table*/
-};
-
-/* I40E_VIRTCHNL_OP_GET_RSS_HENA_CAPS
- * I40E_VIRTCHNL_OP_SET_RSS_HENA
- * VF sends these messages to get and set the hash filter enable bits for RSS.
- * By default, the PF sets these to all possible traffic types that the
- * hardware supports. The VF can query this value if it wants to change the
- * traffic types that are hashed by the hardware.
- * Traffic types are defined in the i40e_filter_pctype enum in i40e_type.h
- */
-struct i40e_virtchnl_rss_hena {
-	u64 hena;
-};
-
-/* I40E_VIRTCHNL_OP_EVENT
- * PF sends this message to inform the VF driver of events that may affect it.
- * No direct response is expected from the VF, though it may generate other
- * messages in response to this one.
- */
-enum i40e_virtchnl_event_codes {
-	I40E_VIRTCHNL_EVENT_UNKNOWN = 0,
-	I40E_VIRTCHNL_EVENT_LINK_CHANGE,
-	I40E_VIRTCHNL_EVENT_RESET_IMPENDING,
-	I40E_VIRTCHNL_EVENT_PF_DRIVER_CLOSE,
-};
-#define I40E_PF_EVENT_SEVERITY_INFO		0
-#define I40E_PF_EVENT_SEVERITY_CERTAIN_DOOM	255
-
-struct i40e_virtchnl_pf_event {
-	enum i40e_virtchnl_event_codes event;
-	union {
-		struct {
-			enum i40e_aq_link_speed link_speed;
-			bool link_status;
-		} link_event;
-	} event_data;
-
-	int severity;
-};
-
-/* I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP
- * VF uses this message to request PF to map IWARP vectors to IWARP queues.
- * The request for this originates from the VF IWARP driver through
- * a client interface between VF LAN and VF IWARP driver.
- * A vector could have an AEQ and CEQ attached to it although
- * there is a single AEQ per VF IWARP instance in which case
- * most vectors will have an INVALID_IDX for aeq and valid idx for ceq.
- * There will never be a case where there will be multiple CEQs attached
- * to a single vector.
- * PF configures interrupt mapping and returns status.
- */
-
-/* HW does not define a type value for AEQ; only for RX/TX and CEQ.
- * In order for us to keep the interface simple, SW will define a
- * unique type value for AEQ.
-*/
-#define I40E_QUEUE_TYPE_PE_AEQ  0x80
-#define I40E_QUEUE_INVALID_IDX  0xFFFF
-
-struct i40e_virtchnl_iwarp_qv_info {
-	u32 v_idx; /* msix_vector */
-	u16 ceq_idx;
-	u16 aeq_idx;
-	u8 itr_idx;
-};
-
-struct i40e_virtchnl_iwarp_qvlist_info {
-	u32 num_vectors;
-	struct i40e_virtchnl_iwarp_qv_info qv_info[1];
-};
-
-/* VF reset states - these are written into the RSTAT register:
- * I40E_VFGEN_RSTAT1 on the PF
- * I40E_VFGEN_RSTAT on the VF
- * When the PF initiates a reset, it writes 0
- * When the reset is complete, it writes 1
- * When the PF detects that the VF has recovered, it writes 2
- * VF checks this register periodically to determine if a reset has occurred,
- * then polls it to know when the reset is complete.
- * If either the PF or VF reads the register while the hardware
- * is in a reset state, it will return DEADBEEF, which, when masked
- * will result in 3.
- */
-enum i40e_vfr_states {
-	I40E_VFR_INPROGRESS = 0,
-	I40E_VFR_COMPLETED,
-	I40E_VFR_VFACTIVE,
-	I40E_VFR_UNKNOWN,
-};
-
-#endif /* _I40E_VIRTCHNL_H_ */
diff --git a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
index 95c23fbaa211..ecbe40ea8ffe 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
@@ -39,7 +39,7 @@
  * send a message to all VFs on a given PF
  **/
 static void i40e_vc_vf_broadcast(struct i40e_pf *pf,
-				 enum i40e_virtchnl_ops v_opcode,
+				 enum virtchnl_ops v_opcode,
 				 i40e_status v_retval, u8 *msg,
 				 u16 msglen)
 {
@@ -70,14 +70,14 @@ static void i40e_vc_vf_broadcast(struct i40e_pf *pf,
  **/
 static void i40e_vc_notify_vf_link_state(struct i40e_vf *vf)
 {
-	struct i40e_virtchnl_pf_event pfe;
+	struct virtchnl_pf_event pfe;
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_hw *hw = &pf->hw;
 	struct i40e_link_status *ls = &pf->hw.phy.link_info;
 	int abs_vf_id = vf->vf_id + (int)hw->func_caps.vf_base_id;
 
-	pfe.event = I40E_VIRTCHNL_EVENT_LINK_CHANGE;
-	pfe.severity = I40E_PF_EVENT_SEVERITY_INFO;
+	pfe.event = VIRTCHNL_EVENT_LINK_CHANGE;
+	pfe.severity = PF_EVENT_SEVERITY_INFO;
 	if (vf->link_forced) {
 		pfe.event_data.link_event.link_status = vf->link_up;
 		pfe.event_data.link_event.link_speed =
@@ -85,9 +85,10 @@ static void i40e_vc_notify_vf_link_state(struct i40e_vf *vf)
 	} else {
 		pfe.event_data.link_event.link_status =
 			ls->link_info & I40E_AQ_LINK_UP;
-		pfe.event_data.link_event.link_speed = ls->link_speed;
+		pfe.event_data.link_event.link_speed =
+			(enum virtchnl_link_speed)ls->link_speed;
 	}
-	i40e_aq_send_msg_to_vf(hw, abs_vf_id, I40E_VIRTCHNL_OP_EVENT,
+	i40e_aq_send_msg_to_vf(hw, abs_vf_id, VIRTCHNL_OP_EVENT,
 			       0, (u8 *)&pfe, sizeof(pfe), NULL);
 }
 
@@ -113,12 +114,12 @@ void i40e_vc_notify_link_state(struct i40e_pf *pf)
  **/
 void i40e_vc_notify_reset(struct i40e_pf *pf)
 {
-	struct i40e_virtchnl_pf_event pfe;
+	struct virtchnl_pf_event pfe;
 
-	pfe.event = I40E_VIRTCHNL_EVENT_RESET_IMPENDING;
-	pfe.severity = I40E_PF_EVENT_SEVERITY_CERTAIN_DOOM;
-	i40e_vc_vf_broadcast(pf, I40E_VIRTCHNL_OP_EVENT, 0,
-			     (u8 *)&pfe, sizeof(struct i40e_virtchnl_pf_event));
+	pfe.event = VIRTCHNL_EVENT_RESET_IMPENDING;
+	pfe.severity = PF_EVENT_SEVERITY_CERTAIN_DOOM;
+	i40e_vc_vf_broadcast(pf, VIRTCHNL_OP_EVENT, 0,
+			     (u8 *)&pfe, sizeof(struct virtchnl_pf_event));
 }
 
 /**
@@ -129,7 +130,7 @@ void i40e_vc_notify_reset(struct i40e_pf *pf)
  **/
 void i40e_vc_notify_vf_reset(struct i40e_vf *vf)
 {
-	struct i40e_virtchnl_pf_event pfe;
+	struct virtchnl_pf_event pfe;
 	int abs_vf_id;
 
 	/* validate the request */
@@ -143,11 +144,11 @@ void i40e_vc_notify_vf_reset(struct i40e_vf *vf)
 
 	abs_vf_id = vf->vf_id + (int)vf->pf->hw.func_caps.vf_base_id;
 
-	pfe.event = I40E_VIRTCHNL_EVENT_RESET_IMPENDING;
-	pfe.severity = I40E_PF_EVENT_SEVERITY_CERTAIN_DOOM;
-	i40e_aq_send_msg_to_vf(&vf->pf->hw, abs_vf_id, I40E_VIRTCHNL_OP_EVENT,
+	pfe.event = VIRTCHNL_EVENT_RESET_IMPENDING;
+	pfe.severity = PF_EVENT_SEVERITY_CERTAIN_DOOM;
+	i40e_aq_send_msg_to_vf(&vf->pf->hw, abs_vf_id, VIRTCHNL_OP_EVENT,
 			       0, (u8 *)&pfe,
-			       sizeof(struct i40e_virtchnl_pf_event), NULL);
+			       sizeof(struct virtchnl_pf_event), NULL);
 }
 /***********************misc routines*****************************/
 
@@ -250,7 +251,7 @@ static u16 i40e_vc_get_pf_queue_id(struct i40e_vf *vf, u16 vsi_id,
  * configure irq link list from the map
  **/
 static void i40e_config_irq_link_list(struct i40e_vf *vf, u16 vsi_id,
-				      struct i40e_virtchnl_vector_map *vecmap)
+				      struct virtchnl_vector_map *vecmap)
 {
 	unsigned long linklistmap = 0, tempmap;
 	struct i40e_pf *pf = vf->pf;
@@ -338,7 +339,7 @@ static void i40e_config_irq_link_list(struct i40e_vf *vf, u16 vsi_id,
 	/* if the vf is running in polling mode and using interrupt zero,
 	 * need to disable auto-mask on enabling zero interrupt for VFs.
 	 */
-	if ((vf->driver_caps & I40E_VIRTCHNL_VF_OFFLOAD_RX_POLLING) &&
+	if ((vf->driver_caps & VIRTCHNL_VF_OFFLOAD_RX_POLLING) &&
 	    (vector_id == 0)) {
 		reg = rd32(hw, I40E_GLINT_CTL);
 		if (!(reg & I40E_GLINT_CTL_DIS_AUTOMASK_VF0_MASK)) {
@@ -359,7 +360,7 @@ irq_list_done:
 static void i40e_release_iwarp_qvlist(struct i40e_vf *vf)
 {
 	struct i40e_pf *pf = vf->pf;
-	struct i40e_virtchnl_iwarp_qvlist_info *qvlist_info = vf->qvlist_info;
+	struct virtchnl_iwarp_qvlist_info *qvlist_info = vf->qvlist_info;
 	u32 msix_vf;
 	u32 i;
 
@@ -368,7 +369,7 @@ static void i40e_release_iwarp_qvlist(struct i40e_vf *vf)
 
 	msix_vf = pf->hw.func_caps.num_msix_vectors_vf;
 	for (i = 0; i < qvlist_info->num_vectors; i++) {
-		struct i40e_virtchnl_iwarp_qv_info *qv_info;
+		struct virtchnl_iwarp_qv_info *qv_info;
 		u32 next_q_index, next_q_type;
 		struct i40e_hw *hw = &pf->hw;
 		u32 v_idx, reg_idx, reg;
@@ -409,17 +410,17 @@ static void i40e_release_iwarp_qvlist(struct i40e_vf *vf)
  * Return 0 on success or < 0 on error
  **/
 static int i40e_config_iwarp_qvlist(struct i40e_vf *vf,
-				    struct i40e_virtchnl_iwarp_qvlist_info *qvlist_info)
+				    struct virtchnl_iwarp_qvlist_info *qvlist_info)
 {
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_hw *hw = &pf->hw;
-	struct i40e_virtchnl_iwarp_qv_info *qv_info;
+	struct virtchnl_iwarp_qv_info *qv_info;
 	u32 v_idx, i, reg_idx, reg;
 	u32 next_q_idx, next_q_type;
 	u32 msix_vf, size;
 
-	size = sizeof(struct i40e_virtchnl_iwarp_qvlist_info) +
-	       (sizeof(struct i40e_virtchnl_iwarp_qv_info) *
+	size = sizeof(struct virtchnl_iwarp_qvlist_info) +
+	       (sizeof(struct virtchnl_iwarp_qv_info) *
 						(qvlist_info->num_vectors - 1));
 	vf->qvlist_info = kzalloc(size, GFP_KERNEL);
 	vf->qvlist_info->num_vectors = qvlist_info->num_vectors;
@@ -492,7 +493,7 @@ err:
  **/
 static int i40e_config_vsi_tx_queue(struct i40e_vf *vf, u16 vsi_id,
 				    u16 vsi_queue_id,
-				    struct i40e_virtchnl_txq_info *info)
+				    struct virtchnl_txq_info *info)
 {
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_hw *hw = &pf->hw;
@@ -569,7 +570,7 @@ error_context:
  **/
 static int i40e_config_vsi_rx_queue(struct i40e_vf *vf, u16 vsi_id,
 				    u16 vsi_queue_id,
-				    struct i40e_virtchnl_rxq_info *info)
+				    struct virtchnl_rxq_info *info)
 {
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_hw *hw = &pf->hw;
@@ -1017,7 +1018,7 @@ static void i40e_cleanup_reset_vf(struct i40e_vf *vf)
 	 * after VF has been fully initialized, because the VF driver may
 	 * request resources immediately after setting this flag.
 	 */
-	wr32(hw, I40E_VFGEN_RSTAT1(vf->vf_id), I40E_VFR_VFACTIVE);
+	wr32(hw, I40E_VFGEN_RSTAT1(vf->vf_id), VIRTCHNL_VFR_VFACTIVE);
 }
 
 /**
@@ -1461,7 +1462,7 @@ static int i40e_vc_send_msg_to_vf(struct i40e_vf *vf, u32 v_opcode,
  * send resp msg to VF
  **/
 static int i40e_vc_send_resp_to_vf(struct i40e_vf *vf,
-				   enum i40e_virtchnl_ops opcode,
+				   enum virtchnl_ops opcode,
 				   i40e_status retval)
 {
 	return i40e_vc_send_msg_to_vf(vf, opcode, retval, NULL, 0);
@@ -1475,18 +1476,17 @@ static int i40e_vc_send_resp_to_vf(struct i40e_vf *vf,
  **/
 static int i40e_vc_get_version_msg(struct i40e_vf *vf, u8 *msg)
 {
-	struct i40e_virtchnl_version_info info = {
-		I40E_VIRTCHNL_VERSION_MAJOR, I40E_VIRTCHNL_VERSION_MINOR
+	struct virtchnl_version_info info = {
+		VIRTCHNL_VERSION_MAJOR, VIRTCHNL_VERSION_MINOR
 	};
 
-	vf->vf_ver = *(struct i40e_virtchnl_version_info *)msg;
+	vf->vf_ver = *(struct virtchnl_version_info *)msg;
 	/* VFs running the 1.0 API expect to get 1.0 back or they will cry. */
-	if (VF_IS_V10(vf))
-		info.minor = I40E_VIRTCHNL_VERSION_MINOR_NO_VF_CAPS;
-	return i40e_vc_send_msg_to_vf(vf, I40E_VIRTCHNL_OP_VERSION,
+	if (VF_IS_V10(&vf->vf_ver))
+		info.minor = VIRTCHNL_VERSION_MINOR_NO_VF_CAPS;
+	return i40e_vc_send_msg_to_vf(vf, VIRTCHNL_OP_VERSION,
 				      I40E_SUCCESS, (u8 *)&info,
-				      sizeof(struct
-					     i40e_virtchnl_version_info));
+				      sizeof(struct virtchnl_version_info));
 }
 
 /**
@@ -1499,7 +1499,7 @@ static int i40e_vc_get_version_msg(struct i40e_vf *vf, u8 *msg)
  **/
 static int i40e_vc_get_vf_resources_msg(struct i40e_vf *vf, u8 *msg)
 {
-	struct i40e_virtchnl_vf_resource *vfres = NULL;
+	struct virtchnl_vf_resource *vfres = NULL;
 	struct i40e_pf *pf = vf->pf;
 	i40e_status aq_ret = 0;
 	struct i40e_vsi *vsi;
@@ -1512,8 +1512,8 @@ static int i40e_vc_get_vf_resources_msg(struct i40e_vf *vf, u8 *msg)
 		goto err;
 	}
 
-	len = (sizeof(struct i40e_virtchnl_vf_resource) +
-	       sizeof(struct i40e_virtchnl_vsi_resource) * num_vsis);
+	len = (sizeof(struct virtchnl_vf_resource) +
+	       sizeof(struct virtchnl_vsi_resource) * num_vsis);
 
 	vfres = kzalloc(len, GFP_KERNEL);
 	if (!vfres) {
@@ -1521,50 +1521,48 @@ static int i40e_vc_get_vf_resources_msg(struct i40e_vf *vf, u8 *msg)
 		len = 0;
 		goto err;
 	}
-	if (VF_IS_V11(vf))
+	if (VF_IS_V11(&vf->vf_ver))
 		vf->driver_caps = *(u32 *)msg;
 	else
-		vf->driver_caps = I40E_VIRTCHNL_VF_OFFLOAD_L2 |
-				  I40E_VIRTCHNL_VF_OFFLOAD_RSS_REG |
-				  I40E_VIRTCHNL_VF_OFFLOAD_VLAN;
+		vf->driver_caps = VIRTCHNL_VF_OFFLOAD_L2 |
+				  VIRTCHNL_VF_OFFLOAD_RSS_REG |
+				  VIRTCHNL_VF_OFFLOAD_VLAN;
 
-	vfres->vf_offload_flags = I40E_VIRTCHNL_VF_OFFLOAD_L2;
+	vfres->vf_offload_flags = VIRTCHNL_VF_OFFLOAD_L2;
 	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (!vsi->info.pvid)
-		vfres->vf_offload_flags |= I40E_VIRTCHNL_VF_OFFLOAD_VLAN;
+		vfres->vf_offload_flags |= VIRTCHNL_VF_OFFLOAD_VLAN;
 
 	if (i40e_vf_client_capable(pf, vf->vf_id) &&
-	    (vf->driver_caps & I40E_VIRTCHNL_VF_OFFLOAD_IWARP)) {
-		vfres->vf_offload_flags |= I40E_VIRTCHNL_VF_OFFLOAD_IWARP;
+	    (vf->driver_caps & VIRTCHNL_VF_OFFLOAD_IWARP)) {
+		vfres->vf_offload_flags |= VIRTCHNL_VF_OFFLOAD_IWARP;
 		set_bit(I40E_VF_STATE_IWARPENA, &vf->vf_states);
 	}
 
-	if (vf->driver_caps & I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF) {
-		vfres->vf_offload_flags |= I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF;
+	if (vf->driver_caps & VIRTCHNL_VF_OFFLOAD_RSS_PF) {
+		vfres->vf_offload_flags |= VIRTCHNL_VF_OFFLOAD_RSS_PF;
 	} else {
 		if ((pf->flags & I40E_FLAG_RSS_AQ_CAPABLE) &&
-		    (vf->driver_caps & I40E_VIRTCHNL_VF_OFFLOAD_RSS_AQ))
-			vfres->vf_offload_flags |=
-					I40E_VIRTCHNL_VF_OFFLOAD_RSS_AQ;
+		    (vf->driver_caps & VIRTCHNL_VF_OFFLOAD_RSS_AQ))
+			vfres->vf_offload_flags |= VIRTCHNL_VF_OFFLOAD_RSS_AQ;
 		else
-			vfres->vf_offload_flags |=
-					I40E_VIRTCHNL_VF_OFFLOAD_RSS_REG;
+			vfres->vf_offload_flags |= VIRTCHNL_VF_OFFLOAD_RSS_REG;
 	}
 
 	if (pf->flags & I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE) {
-		if (vf->driver_caps & I40E_VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2)
+		if (vf->driver_caps & VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2)
 			vfres->vf_offload_flags |=
-				I40E_VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2;
+				VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2;
 	}
 
-	if (vf->driver_caps & I40E_VIRTCHNL_VF_OFFLOAD_ENCAP)
-		vfres->vf_offload_flags |= I40E_VIRTCHNL_VF_OFFLOAD_ENCAP;
+	if (vf->driver_caps & VIRTCHNL_VF_OFFLOAD_ENCAP)
+		vfres->vf_offload_flags |= VIRTCHNL_VF_OFFLOAD_ENCAP;
 
 	if ((pf->flags & I40E_FLAG_OUTER_UDP_CSUM_CAPABLE) &&
-	    (vf->driver_caps & I40E_VIRTCHNL_VF_OFFLOAD_ENCAP_CSUM))
-		vfres->vf_offload_flags |= I40E_VIRTCHNL_VF_OFFLOAD_ENCAP_CSUM;
+	    (vf->driver_caps & VIRTCHNL_VF_OFFLOAD_ENCAP_CSUM))
+		vfres->vf_offload_flags |= VIRTCHNL_VF_OFFLOAD_ENCAP_CSUM;
 
-	if (vf->driver_caps & I40E_VIRTCHNL_VF_OFFLOAD_RX_POLLING) {
+	if (vf->driver_caps & VIRTCHNL_VF_OFFLOAD_RX_POLLING) {
 		if (pf->flags & I40E_FLAG_MFP_ENABLED) {
 			dev_err(&pf->pdev->dev,
 				"VF %d requested polling mode: this feature is supported only when the device is running in single function per port (SFP) mode\n",
@@ -1572,13 +1570,13 @@ static int i40e_vc_get_vf_resources_msg(struct i40e_vf *vf, u8 *msg)
 			ret = I40E_ERR_PARAM;
 			goto err;
 		}
-		vfres->vf_offload_flags |= I40E_VIRTCHNL_VF_OFFLOAD_RX_POLLING;
+		vfres->vf_offload_flags |= VIRTCHNL_VF_OFFLOAD_RX_POLLING;
 	}
 
 	if (pf->flags & I40E_FLAG_WB_ON_ITR_CAPABLE) {
-		if (vf->driver_caps & I40E_VIRTCHNL_VF_OFFLOAD_WB_ON_ITR)
+		if (vf->driver_caps & VIRTCHNL_VF_OFFLOAD_WB_ON_ITR)
 			vfres->vf_offload_flags |=
-					I40E_VIRTCHNL_VF_OFFLOAD_WB_ON_ITR;
+					VIRTCHNL_VF_OFFLOAD_WB_ON_ITR;
 	}
 
 	vfres->num_vsis = num_vsis;
@@ -1589,7 +1587,7 @@ static int i40e_vc_get_vf_resources_msg(struct i40e_vf *vf, u8 *msg)
 
 	if (vf->lan_vsi_idx) {
 		vfres->vsi_res[0].vsi_id = vf->lan_vsi_id;
-		vfres->vsi_res[0].vsi_type = I40E_VSI_SRIOV;
+		vfres->vsi_res[0].vsi_type = VIRTCHNL_VSI_SRIOV;
 		vfres->vsi_res[0].num_queue_pairs = vsi->alloc_queue_pairs;
 		/* VFs only use TC 0 */
 		vfres->vsi_res[0].qset_handle
@@ -1601,7 +1599,7 @@ static int i40e_vc_get_vf_resources_msg(struct i40e_vf *vf, u8 *msg)
 
 err:
 	/* send the response back to the VF */
-	ret = i40e_vc_send_msg_to_vf(vf, I40E_VIRTCHNL_OP_GET_VF_RESOURCES,
+	ret = i40e_vc_send_msg_to_vf(vf, VIRTCHNL_OP_GET_VF_RESOURCES,
 				     aq_ret, (u8 *)vfres, len);
 
 	kfree(vfres);
@@ -1655,8 +1653,8 @@ static inline int i40e_getnum_vf_vsi_vlan_filters(struct i40e_vsi *vsi)
 static int i40e_vc_config_promiscuous_mode_msg(struct i40e_vf *vf,
 					       u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_promisc_info *info =
-	    (struct i40e_virtchnl_promisc_info *)msg;
+	struct virtchnl_promisc_info *info =
+	    (struct virtchnl_promisc_info *)msg;
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_hw *hw = &pf->hw;
 	struct i40e_mac_filter *f;
@@ -1683,7 +1681,7 @@ static int i40e_vc_config_promiscuous_mode_msg(struct i40e_vf *vf,
 		goto error_param;
 	}
 	/* Multicast promiscuous handling*/
-	if (info->flags & I40E_FLAG_VF_MULTICAST_PROMISC)
+	if (info->flags & FLAG_VF_MULTICAST_PROMISC)
 		allmulti = true;
 
 	if (vf->port_vlan_id) {
@@ -1734,7 +1732,7 @@ static int i40e_vc_config_promiscuous_mode_msg(struct i40e_vf *vf,
 			clear_bit(I40E_VF_STATE_MC_PROMISC, &vf->vf_states);
 	}
 
-	if (info->flags & I40E_FLAG_VF_UNICAST_PROMISC)
+	if (info->flags & FLAG_VF_UNICAST_PROMISC)
 		alluni = true;
 	if (vf->port_vlan_id) {
 		aq_ret = i40e_aq_set_vsi_uc_promisc_on_vlan(hw, vsi->seid,
@@ -1788,7 +1786,7 @@ static int i40e_vc_config_promiscuous_mode_msg(struct i40e_vf *vf,
 error_param:
 	/* send the response to the VF */
 	return i40e_vc_send_resp_to_vf(vf,
-				       I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE,
+				       VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE,
 				       aq_ret);
 }
 
@@ -1803,9 +1801,9 @@ error_param:
  **/
 static int i40e_vc_config_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_vsi_queue_config_info *qci =
-	    (struct i40e_virtchnl_vsi_queue_config_info *)msg;
-	struct i40e_virtchnl_queue_pair_info *qpi;
+	struct virtchnl_vsi_queue_config_info *qci =
+	    (struct virtchnl_vsi_queue_config_info *)msg;
+	struct virtchnl_queue_pair_info *qpi;
 	struct i40e_pf *pf = vf->pf;
 	u16 vsi_id, vsi_queue_id;
 	i40e_status aq_ret = 0;
@@ -1845,7 +1843,7 @@ static int i40e_vc_config_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 
 error_param:
 	/* send the response to the VF */
-	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES,
+	return i40e_vc_send_resp_to_vf(vf, VIRTCHNL_OP_CONFIG_VSI_QUEUES,
 				       aq_ret);
 }
 
@@ -1860,9 +1858,9 @@ error_param:
  **/
 static int i40e_vc_config_irq_map_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_irq_map_info *irqmap_info =
-	    (struct i40e_virtchnl_irq_map_info *)msg;
-	struct i40e_virtchnl_vector_map *map;
+	struct virtchnl_irq_map_info *irqmap_info =
+	    (struct virtchnl_irq_map_info *)msg;
+	struct virtchnl_vector_map *map;
 	u16 vsi_id, vsi_queue_id, vector_id;
 	i40e_status aq_ret = 0;
 	unsigned long tempmap;
@@ -1908,7 +1906,7 @@ static int i40e_vc_config_irq_map_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 	}
 error_param:
 	/* send the response to the VF */
-	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP,
+	return i40e_vc_send_resp_to_vf(vf, VIRTCHNL_OP_CONFIG_IRQ_MAP,
 				       aq_ret);
 }
 
@@ -1922,8 +1920,8 @@ error_param:
  **/
 static int i40e_vc_enable_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_queue_select *vqs =
-	    (struct i40e_virtchnl_queue_select *)msg;
+	struct virtchnl_queue_select *vqs =
+	    (struct virtchnl_queue_select *)msg;
 	struct i40e_pf *pf = vf->pf;
 	u16 vsi_id = vqs->vsi_id;
 	i40e_status aq_ret = 0;
@@ -1947,7 +1945,7 @@ static int i40e_vc_enable_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 		aq_ret = I40E_ERR_TIMEOUT;
 error_param:
 	/* send the response to the VF */
-	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_ENABLE_QUEUES,
+	return i40e_vc_send_resp_to_vf(vf, VIRTCHNL_OP_ENABLE_QUEUES,
 				       aq_ret);
 }
 
@@ -1962,8 +1960,8 @@ error_param:
  **/
 static int i40e_vc_disable_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_queue_select *vqs =
-	    (struct i40e_virtchnl_queue_select *)msg;
+	struct virtchnl_queue_select *vqs =
+	    (struct virtchnl_queue_select *)msg;
 	struct i40e_pf *pf = vf->pf;
 	i40e_status aq_ret = 0;
 
@@ -1986,7 +1984,7 @@ static int i40e_vc_disable_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 
 error_param:
 	/* send the response to the VF */
-	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_DISABLE_QUEUES,
+	return i40e_vc_send_resp_to_vf(vf, VIRTCHNL_OP_DISABLE_QUEUES,
 				       aq_ret);
 }
 
@@ -2000,8 +1998,8 @@ error_param:
  **/
 static int i40e_vc_get_stats_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_queue_select *vqs =
-	    (struct i40e_virtchnl_queue_select *)msg;
+	struct virtchnl_queue_select *vqs =
+	    (struct virtchnl_queue_select *)msg;
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_eth_stats stats;
 	i40e_status aq_ret = 0;
@@ -2029,7 +2027,7 @@ static int i40e_vc_get_stats_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 
 error_param:
 	/* send the response back to the VF */
-	return i40e_vc_send_msg_to_vf(vf, I40E_VIRTCHNL_OP_GET_STATS, aq_ret,
+	return i40e_vc_send_msg_to_vf(vf, VIRTCHNL_OP_GET_STATS, aq_ret,
 				      (u8 *)&stats, sizeof(stats));
 }
 
@@ -2088,8 +2086,8 @@ static inline int i40e_check_vf_permission(struct i40e_vf *vf, u8 *macaddr)
  **/
 static int i40e_vc_add_mac_addr_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_ether_addr_list *al =
-	    (struct i40e_virtchnl_ether_addr_list *)msg;
+	struct virtchnl_ether_addr_list *al =
+	    (struct virtchnl_ether_addr_list *)msg;
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_vsi *vsi = NULL;
 	u16 vsi_id = al->vsi_id;
@@ -2143,7 +2141,7 @@ static int i40e_vc_add_mac_addr_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 
 error_param:
 	/* send the response to the VF */
-	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS,
+	return i40e_vc_send_resp_to_vf(vf, VIRTCHNL_OP_ADD_ETH_ADDR,
 				       ret);
 }
 
@@ -2157,8 +2155,8 @@ error_param:
  **/
 static int i40e_vc_del_mac_addr_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_ether_addr_list *al =
-	    (struct i40e_virtchnl_ether_addr_list *)msg;
+	struct virtchnl_ether_addr_list *al =
+	    (struct virtchnl_ether_addr_list *)msg;
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_vsi *vsi = NULL;
 	u16 vsi_id = al->vsi_id;
@@ -2203,7 +2201,7 @@ static int i40e_vc_del_mac_addr_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 
 error_param:
 	/* send the response to the VF */
-	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS,
+	return i40e_vc_send_resp_to_vf(vf, VIRTCHNL_OP_DEL_ETH_ADDR,
 				       ret);
 }
 
@@ -2217,8 +2215,8 @@ error_param:
  **/
 static int i40e_vc_add_vlan_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_vlan_filter_list *vfl =
-	    (struct i40e_virtchnl_vlan_filter_list *)msg;
+	struct virtchnl_vlan_filter_list *vfl =
+	    (struct virtchnl_vlan_filter_list *)msg;
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_vsi *vsi = NULL;
 	u16 vsi_id = vfl->vsi_id;
@@ -2277,7 +2275,7 @@ static int i40e_vc_add_vlan_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 
 error_param:
 	/* send the response to the VF */
-	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_ADD_VLAN, aq_ret);
+	return i40e_vc_send_resp_to_vf(vf, VIRTCHNL_OP_ADD_VLAN, aq_ret);
 }
 
 /**
@@ -2290,8 +2288,8 @@ error_param:
  **/
 static int i40e_vc_remove_vlan_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_vlan_filter_list *vfl =
-	    (struct i40e_virtchnl_vlan_filter_list *)msg;
+	struct virtchnl_vlan_filter_list *vfl =
+	    (struct virtchnl_vlan_filter_list *)msg;
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_vsi *vsi = NULL;
 	u16 vsi_id = vfl->vsi_id;
@@ -2335,7 +2333,7 @@ static int i40e_vc_remove_vlan_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 
 error_param:
 	/* send the response to the VF */
-	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_DEL_VLAN, aq_ret);
+	return i40e_vc_send_resp_to_vf(vf, VIRTCHNL_OP_DEL_VLAN, aq_ret);
 }
 
 /**
@@ -2363,7 +2361,7 @@ static int i40e_vc_iwarp_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 
 error_param:
 	/* send the response to the VF */
-	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_IWARP,
+	return i40e_vc_send_resp_to_vf(vf, VIRTCHNL_OP_IWARP,
 				       aq_ret);
 }
 
@@ -2379,8 +2377,8 @@ error_param:
 static int i40e_vc_iwarp_qvmap_msg(struct i40e_vf *vf, u8 *msg, u16 msglen,
 				   bool config)
 {
-	struct i40e_virtchnl_iwarp_qvlist_info *qvlist_info =
-				(struct i40e_virtchnl_iwarp_qvlist_info *)msg;
+	struct virtchnl_iwarp_qvlist_info *qvlist_info =
+				(struct virtchnl_iwarp_qvlist_info *)msg;
 	i40e_status aq_ret = 0;
 
 	if (!test_bit(I40E_VF_STATE_ACTIVE, &vf->vf_states) ||
@@ -2399,8 +2397,8 @@ static int i40e_vc_iwarp_qvmap_msg(struct i40e_vf *vf, u8 *msg, u16 msglen,
 error_param:
 	/* send the response to the VF */
 	return i40e_vc_send_resp_to_vf(vf,
-			       config ? I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP :
-			       I40E_VIRTCHNL_OP_RELEASE_IWARP_IRQ_MAP,
+			       config ? VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP :
+			       VIRTCHNL_OP_RELEASE_IWARP_IRQ_MAP,
 			       aq_ret);
 }
 
@@ -2414,8 +2412,8 @@ error_param:
  **/
 static int i40e_vc_config_rss_key(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_rss_key *vrk =
-		(struct i40e_virtchnl_rss_key *)msg;
+	struct virtchnl_rss_key *vrk =
+		(struct virtchnl_rss_key *)msg;
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_vsi *vsi = NULL;
 	u16 vsi_id = vrk->vsi_id;
@@ -2432,7 +2430,7 @@ static int i40e_vc_config_rss_key(struct i40e_vf *vf, u8 *msg, u16 msglen)
 	aq_ret = i40e_config_rss(vsi, vrk->key, NULL, 0);
 err:
 	/* send the response to the VF */
-	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_CONFIG_RSS_KEY,
+	return i40e_vc_send_resp_to_vf(vf, VIRTCHNL_OP_CONFIG_RSS_KEY,
 				       aq_ret);
 }
 
@@ -2446,8 +2444,8 @@ err:
  **/
 static int i40e_vc_config_rss_lut(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_rss_lut *vrl =
-		(struct i40e_virtchnl_rss_lut *)msg;
+	struct virtchnl_rss_lut *vrl =
+		(struct virtchnl_rss_lut *)msg;
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_vsi *vsi = NULL;
 	u16 vsi_id = vrl->vsi_id;
@@ -2464,7 +2462,7 @@ static int i40e_vc_config_rss_lut(struct i40e_vf *vf, u8 *msg, u16 msglen)
 	aq_ret = i40e_config_rss(vsi, NULL, vrl->lut, I40E_VF_HLUT_ARRAY_SIZE);
 	/* send the response to the VF */
 err:
-	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_CONFIG_RSS_LUT,
+	return i40e_vc_send_resp_to_vf(vf, VIRTCHNL_OP_CONFIG_RSS_LUT,
 				       aq_ret);
 }
 
@@ -2478,7 +2476,7 @@ err:
  **/
 static int i40e_vc_get_rss_hena(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_rss_hena *vrh = NULL;
+	struct virtchnl_rss_hena *vrh = NULL;
 	struct i40e_pf *pf = vf->pf;
 	i40e_status aq_ret = 0;
 	int len = 0;
@@ -2487,7 +2485,7 @@ static int i40e_vc_get_rss_hena(struct i40e_vf *vf, u8 *msg, u16 msglen)
 		aq_ret = I40E_ERR_PARAM;
 		goto err;
 	}
-	len = sizeof(struct i40e_virtchnl_rss_hena);
+	len = sizeof(struct virtchnl_rss_hena);
 
 	vrh = kzalloc(len, GFP_KERNEL);
 	if (!vrh) {
@@ -2498,7 +2496,7 @@ static int i40e_vc_get_rss_hena(struct i40e_vf *vf, u8 *msg, u16 msglen)
 	vrh->hena = i40e_pf_get_default_rss_hena(pf);
 err:
 	/* send the response back to the VF */
-	aq_ret = i40e_vc_send_msg_to_vf(vf, I40E_VIRTCHNL_OP_GET_RSS_HENA_CAPS,
+	aq_ret = i40e_vc_send_msg_to_vf(vf, VIRTCHNL_OP_GET_RSS_HENA_CAPS,
 					aq_ret, (u8 *)vrh, len);
 	kfree(vrh);
 	return aq_ret;
@@ -2514,8 +2512,8 @@ err:
  **/
 static int i40e_vc_set_rss_hena(struct i40e_vf *vf, u8 *msg, u16 msglen)
 {
-	struct i40e_virtchnl_rss_hena *vrh =
-		(struct i40e_virtchnl_rss_hena *)msg;
+	struct virtchnl_rss_hena *vrh =
+		(struct virtchnl_rss_hena *)msg;
 	struct i40e_pf *pf = vf->pf;
 	struct i40e_hw *hw = &pf->hw;
 	i40e_status aq_ret = 0;
@@ -2530,170 +2528,7 @@ static int i40e_vc_set_rss_hena(struct i40e_vf *vf, u8 *msg, u16 msglen)
 
 	/* send the response to the VF */
 err:
-	return i40e_vc_send_resp_to_vf(vf, I40E_VIRTCHNL_OP_SET_RSS_HENA,
-				       aq_ret);
-}
-
-/**
- * i40e_vc_validate_vf_msg
- * @vf: pointer to the VF info
- * @msg: pointer to the msg buffer
- * @msglen: msg length
- * @msghndl: msg handle
- *
- * validate msg
- **/
-static int i40e_vc_validate_vf_msg(struct i40e_vf *vf, u32 v_opcode,
-				   u32 v_retval, u8 *msg, u16 msglen)
-{
-	bool err_msg_format = false;
-	int valid_len = 0;
-
-	/* Check if VF is disabled. */
-	if (test_bit(I40E_VF_STATE_DISABLED, &vf->vf_states))
-		return I40E_ERR_PARAM;
-
-	/* Validate message length. */
-	switch (v_opcode) {
-	case I40E_VIRTCHNL_OP_VERSION:
-		valid_len = sizeof(struct i40e_virtchnl_version_info);
-		break;
-	case I40E_VIRTCHNL_OP_RESET_VF:
-		break;
-	case I40E_VIRTCHNL_OP_GET_VF_RESOURCES:
-		if (VF_IS_V11(vf))
-			valid_len = sizeof(u32);
-		break;
-	case I40E_VIRTCHNL_OP_CONFIG_TX_QUEUE:
-		valid_len = sizeof(struct i40e_virtchnl_txq_info);
-		break;
-	case I40E_VIRTCHNL_OP_CONFIG_RX_QUEUE:
-		valid_len = sizeof(struct i40e_virtchnl_rxq_info);
-		break;
-	case I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES:
-		valid_len = sizeof(struct i40e_virtchnl_vsi_queue_config_info);
-		if (msglen >= valid_len) {
-			struct i40e_virtchnl_vsi_queue_config_info *vqc =
-			    (struct i40e_virtchnl_vsi_queue_config_info *)msg;
-			valid_len += (vqc->num_queue_pairs *
-				      sizeof(struct
-					     i40e_virtchnl_queue_pair_info));
-			if (vqc->num_queue_pairs == 0)
-				err_msg_format = true;
-		}
-		break;
-	case I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP:
-		valid_len = sizeof(struct i40e_virtchnl_irq_map_info);
-		if (msglen >= valid_len) {
-			struct i40e_virtchnl_irq_map_info *vimi =
-			    (struct i40e_virtchnl_irq_map_info *)msg;
-			valid_len += (vimi->num_vectors *
-				      sizeof(struct i40e_virtchnl_vector_map));
-			if (vimi->num_vectors == 0)
-				err_msg_format = true;
-		}
-		break;
-	case I40E_VIRTCHNL_OP_ENABLE_QUEUES:
-	case I40E_VIRTCHNL_OP_DISABLE_QUEUES:
-		valid_len = sizeof(struct i40e_virtchnl_queue_select);
-		break;
-	case I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS:
-	case I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS:
-		valid_len = sizeof(struct i40e_virtchnl_ether_addr_list);
-		if (msglen >= valid_len) {
-			struct i40e_virtchnl_ether_addr_list *veal =
-			    (struct i40e_virtchnl_ether_addr_list *)msg;
-			valid_len += veal->num_elements *
-			    sizeof(struct i40e_virtchnl_ether_addr);
-			if (veal->num_elements == 0)
-				err_msg_format = true;
-		}
-		break;
-	case I40E_VIRTCHNL_OP_ADD_VLAN:
-	case I40E_VIRTCHNL_OP_DEL_VLAN:
-		valid_len = sizeof(struct i40e_virtchnl_vlan_filter_list);
-		if (msglen >= valid_len) {
-			struct i40e_virtchnl_vlan_filter_list *vfl =
-			    (struct i40e_virtchnl_vlan_filter_list *)msg;
-			valid_len += vfl->num_elements * sizeof(u16);
-			if (vfl->num_elements == 0)
-				err_msg_format = true;
-		}
-		break;
-	case I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE:
-		valid_len = sizeof(struct i40e_virtchnl_promisc_info);
-		break;
-	case I40E_VIRTCHNL_OP_GET_STATS:
-		valid_len = sizeof(struct i40e_virtchnl_queue_select);
-		break;
-	case I40E_VIRTCHNL_OP_IWARP:
-		/* These messages are opaque to us and will be validated in
-		 * the RDMA client code. We just need to check for nonzero
-		 * length. The firmware will enforce max length restrictions.
-		 */
-		if (msglen)
-			valid_len = msglen;
-		else
-			err_msg_format = true;
-		break;
-	case I40E_VIRTCHNL_OP_RELEASE_IWARP_IRQ_MAP:
-		valid_len = 0;
-		break;
-	case I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP:
-		valid_len = sizeof(struct i40e_virtchnl_iwarp_qvlist_info);
-		if (msglen >= valid_len) {
-			struct i40e_virtchnl_iwarp_qvlist_info *qv =
-				(struct i40e_virtchnl_iwarp_qvlist_info *)msg;
-			if (qv->num_vectors == 0) {
-				err_msg_format = true;
-				break;
-			}
-			valid_len += ((qv->num_vectors - 1) *
-				sizeof(struct i40e_virtchnl_iwarp_qv_info));
-		}
-		break;
-	case I40E_VIRTCHNL_OP_CONFIG_RSS_KEY:
-		valid_len = sizeof(struct i40e_virtchnl_rss_key);
-		if (msglen >= valid_len) {
-			struct i40e_virtchnl_rss_key *vrk =
-				(struct i40e_virtchnl_rss_key *)msg;
-			if (vrk->key_len != I40E_HKEY_ARRAY_SIZE) {
-				err_msg_format = true;
-				break;
-			}
-			valid_len += vrk->key_len - 1;
-		}
-		break;
-	case I40E_VIRTCHNL_OP_CONFIG_RSS_LUT:
-		valid_len = sizeof(struct i40e_virtchnl_rss_lut);
-		if (msglen >= valid_len) {
-			struct i40e_virtchnl_rss_lut *vrl =
-				(struct i40e_virtchnl_rss_lut *)msg;
-			if (vrl->lut_entries != I40E_VF_HLUT_ARRAY_SIZE) {
-				err_msg_format = true;
-				break;
-			}
-			valid_len += vrl->lut_entries - 1;
-		}
-		break;
-	case I40E_VIRTCHNL_OP_GET_RSS_HENA_CAPS:
-		break;
-	case I40E_VIRTCHNL_OP_SET_RSS_HENA:
-		valid_len = sizeof(struct i40e_virtchnl_rss_hena);
-		break;
-	/* These are always errors coming from the VF. */
-	case I40E_VIRTCHNL_OP_EVENT:
-	case I40E_VIRTCHNL_OP_UNKNOWN:
-	default:
-		return -EPERM;
-	}
-	/* few more checks */
-	if ((valid_len != msglen) || (err_msg_format)) {
-		i40e_vc_send_resp_to_vf(vf, v_opcode, I40E_ERR_PARAM);
-		return -EINVAL;
-	} else {
-		return 0;
-	}
+	return i40e_vc_send_resp_to_vf(vf, VIRTCHNL_OP_SET_RSS_HENA, aq_ret);
 }
 
 /**
@@ -2719,80 +2554,104 @@ int i40e_vc_process_vf_msg(struct i40e_pf *pf, s16 vf_id, u32 v_opcode,
 	if (local_vf_id >= pf->num_alloc_vfs)
 		return -EINVAL;
 	vf = &(pf->vf[local_vf_id]);
+
+	/* Check if VF is disabled. */
+	if (test_bit(I40E_VF_STATE_DISABLED, &vf->vf_states))
+		return I40E_ERR_PARAM;
+
 	/* perform basic checks on the msg */
-	ret = i40e_vc_validate_vf_msg(vf, v_opcode, v_retval, msg, msglen);
+	ret = virtchnl_vc_validate_vf_msg(&vf->vf_ver, v_opcode, msg, msglen);
+
+	/* perform additional checks specific to this driver */
+	if (v_opcode == VIRTCHNL_OP_CONFIG_RSS_KEY) {
+		struct virtchnl_rss_key *vrk = (struct virtchnl_rss_key *)msg;
+
+		if (vrk->key_len != I40E_HKEY_ARRAY_SIZE)
+			ret = -EINVAL;
+	} else if (v_opcode == VIRTCHNL_OP_CONFIG_RSS_LUT) {
+		struct virtchnl_rss_lut *vrl = (struct virtchnl_rss_lut *)msg;
+
+		if (vrl->lut_entries != I40E_VF_HLUT_ARRAY_SIZE)
+			ret = -EINVAL;
+	}
 
 	if (ret) {
+		i40e_vc_send_resp_to_vf(vf, v_opcode, I40E_ERR_PARAM);
 		dev_err(&pf->pdev->dev, "Invalid message from VF %d, opcode %d, len %d\n",
 			local_vf_id, v_opcode, msglen);
-		return ret;
+		switch (ret) {
+		case VIRTCHNL_ERR_PARAM:
+			return -EPERM;
+		default:
+			return -EINVAL;
+		}
 	}
 
 	switch (v_opcode) {
-	case I40E_VIRTCHNL_OP_VERSION:
+	case VIRTCHNL_OP_VERSION:
 		ret = i40e_vc_get_version_msg(vf, msg);
 		break;
-	case I40E_VIRTCHNL_OP_GET_VF_RESOURCES:
+	case VIRTCHNL_OP_GET_VF_RESOURCES:
 		ret = i40e_vc_get_vf_resources_msg(vf, msg);
 		break;
-	case I40E_VIRTCHNL_OP_RESET_VF:
+	case VIRTCHNL_OP_RESET_VF:
 		i40e_vc_reset_vf_msg(vf);
 		ret = 0;
 		break;
-	case I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE:
+	case VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE:
 		ret = i40e_vc_config_promiscuous_mode_msg(vf, msg, msglen);
 		break;
-	case I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES:
+	case VIRTCHNL_OP_CONFIG_VSI_QUEUES:
 		ret = i40e_vc_config_queues_msg(vf, msg, msglen);
 		break;
-	case I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP:
+	case VIRTCHNL_OP_CONFIG_IRQ_MAP:
 		ret = i40e_vc_config_irq_map_msg(vf, msg, msglen);
 		break;
-	case I40E_VIRTCHNL_OP_ENABLE_QUEUES:
+	case VIRTCHNL_OP_ENABLE_QUEUES:
 		ret = i40e_vc_enable_queues_msg(vf, msg, msglen);
 		i40e_vc_notify_vf_link_state(vf);
 		break;
-	case I40E_VIRTCHNL_OP_DISABLE_QUEUES:
+	case VIRTCHNL_OP_DISABLE_QUEUES:
 		ret = i40e_vc_disable_queues_msg(vf, msg, msglen);
 		break;
-	case I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS:
+	case VIRTCHNL_OP_ADD_ETH_ADDR:
 		ret = i40e_vc_add_mac_addr_msg(vf, msg, msglen);
 		break;
-	case I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS:
+	case VIRTCHNL_OP_DEL_ETH_ADDR:
 		ret = i40e_vc_del_mac_addr_msg(vf, msg, msglen);
 		break;
-	case I40E_VIRTCHNL_OP_ADD_VLAN:
+	case VIRTCHNL_OP_ADD_VLAN:
 		ret = i40e_vc_add_vlan_msg(vf, msg, msglen);
 		break;
-	case I40E_VIRTCHNL_OP_DEL_VLAN:
+	case VIRTCHNL_OP_DEL_VLAN:
 		ret = i40e_vc_remove_vlan_msg(vf, msg, msglen);
 		break;
-	case I40E_VIRTCHNL_OP_GET_STATS:
+	case VIRTCHNL_OP_GET_STATS:
 		ret = i40e_vc_get_stats_msg(vf, msg, msglen);
 		break;
-	case I40E_VIRTCHNL_OP_IWARP:
+	case VIRTCHNL_OP_IWARP:
 		ret = i40e_vc_iwarp_msg(vf, msg, msglen);
 		break;
-	case I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP:
+	case VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP:
 		ret = i40e_vc_iwarp_qvmap_msg(vf, msg, msglen, true);
 		break;
-	case I40E_VIRTCHNL_OP_RELEASE_IWARP_IRQ_MAP:
+	case VIRTCHNL_OP_RELEASE_IWARP_IRQ_MAP:
 		ret = i40e_vc_iwarp_qvmap_msg(vf, msg, msglen, false);
 		break;
-	case I40E_VIRTCHNL_OP_CONFIG_RSS_KEY:
+	case VIRTCHNL_OP_CONFIG_RSS_KEY:
 		ret = i40e_vc_config_rss_key(vf, msg, msglen);
 		break;
-	case I40E_VIRTCHNL_OP_CONFIG_RSS_LUT:
+	case VIRTCHNL_OP_CONFIG_RSS_LUT:
 		ret = i40e_vc_config_rss_lut(vf, msg, msglen);
 		break;
-	case I40E_VIRTCHNL_OP_GET_RSS_HENA_CAPS:
+	case VIRTCHNL_OP_GET_RSS_HENA_CAPS:
 		ret = i40e_vc_get_rss_hena(vf, msg, msglen);
 		break;
-	case I40E_VIRTCHNL_OP_SET_RSS_HENA:
+	case VIRTCHNL_OP_SET_RSS_HENA:
 		ret = i40e_vc_set_rss_hena(vf, msg, msglen);
 		break;
 
-	case I40E_VIRTCHNL_OP_UNKNOWN:
+	case VIRTCHNL_OP_UNKNOWN:
 	default:
 		dev_err(&pf->pdev->dev, "Unsupported opcode %d from VF %d\n",
 			v_opcode, local_vf_id);
@@ -3017,10 +2876,12 @@ int i40e_ndo_set_vf_port_vlan(struct net_device *netdev, int vf_id,
 					   VLAN_VID_MASK));
 	}
 
+	spin_unlock_bh(&vsi->mac_filter_hash_lock);
 	if (vlan_id || qos)
 		ret = i40e_vsi_add_pvid(vsi, vlanprio);
 	else
 		i40e_vsi_remove_pvid(vsi);
+	spin_lock_bh(&vsi->mac_filter_hash_lock);
 
 	if (vlan_id) {
 		dev_info(&pf->pdev->dev, "Setting VLAN %d, QOS 0x%x on VF %d\n",
@@ -3218,7 +3079,7 @@ int i40e_ndo_set_vf_link_state(struct net_device *netdev, int vf_id, int link)
 {
 	struct i40e_netdev_priv *np = netdev_priv(netdev);
 	struct i40e_pf *pf = np->vsi->back;
-	struct i40e_virtchnl_pf_event pfe;
+	struct virtchnl_pf_event pfe;
 	struct i40e_hw *hw = &pf->hw;
 	struct i40e_vf *vf;
 	int abs_vf_id;
@@ -3234,8 +3095,8 @@ int i40e_ndo_set_vf_link_state(struct net_device *netdev, int vf_id, int link)
 	vf = &pf->vf[vf_id];
 	abs_vf_id = vf->vf_id + hw->func_caps.vf_base_id;
 
-	pfe.event = I40E_VIRTCHNL_EVENT_LINK_CHANGE;
-	pfe.severity = I40E_PF_EVENT_SEVERITY_INFO;
+	pfe.event = VIRTCHNL_EVENT_LINK_CHANGE;
+	pfe.severity = PF_EVENT_SEVERITY_INFO;
 
 	switch (link) {
 	case IFLA_VF_LINK_STATE_AUTO:
@@ -3243,6 +3104,7 @@ int i40e_ndo_set_vf_link_state(struct net_device *netdev, int vf_id, int link)
 		pfe.event_data.link_event.link_status =
 			pf->hw.phy.link_info.link_info & I40E_AQ_LINK_UP;
 		pfe.event_data.link_event.link_speed =
+			(enum virtchnl_link_speed)
 			pf->hw.phy.link_info.link_speed;
 		break;
 	case IFLA_VF_LINK_STATE_ENABLE:
@@ -3262,7 +3124,7 @@ int i40e_ndo_set_vf_link_state(struct net_device *netdev, int vf_id, int link)
 		goto error_out;
 	}
 	/* Notify the VF of its new link state */
-	i40e_aq_send_msg_to_vf(hw, abs_vf_id, I40E_VIRTCHNL_OP_EVENT,
+	i40e_aq_send_msg_to_vf(hw, abs_vf_id, VIRTCHNL_OP_EVENT,
 			       0, (u8 *)&pfe, sizeof(pfe), NULL);
 
 error_out:
diff --git a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h
index 20d7c8160e9e..1f4b0c504368 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h
@@ -40,9 +40,6 @@
 #define I40E_VLAN_MASK			0xFFF
 #define I40E_PRIORITY_MASK		0x7000
 
-#define VF_IS_V10(_v) (((_v)->vf_ver.major == 1) && ((_v)->vf_ver.minor == 0))
-#define VF_IS_V11(_v) (((_v)->vf_ver.major == 1) && ((_v)->vf_ver.minor == 1))
-
 /* Various queue ctrls */
 enum i40e_queue_ctrl {
 	I40E_QUEUE_CTRL_UNKNOWN = 0,
@@ -81,13 +78,13 @@ struct i40e_vf {
 	s16 vf_id;
 	/* all VF vsis connect to the same parent */
 	enum i40e_switch_element_types parent_type;
-	struct i40e_virtchnl_version_info vf_ver;
+	struct virtchnl_version_info vf_ver;
 	u32 driver_caps; /* reported by VF driver */
 
 	/* VF Port Extender (PE) stag if used */
 	u16 stag;
 
-	struct i40e_virtchnl_ether_addr default_lan_addr;
+	struct virtchnl_ether_addr default_lan_addr;
 	u16 port_vlan_id;
 	bool pf_set_mac;	/* The VMM admin set the VF MAC address */
 	bool trusted;
@@ -115,7 +112,7 @@ struct i40e_vf {
 	u16 num_vlan;
 
 	/* RDMA Client */
-	struct i40e_virtchnl_iwarp_qvlist_info *qvlist_info;
+	struct virtchnl_iwarp_qvlist_info *qvlist_info;
 };
 
 void i40e_free_vfs(struct i40e_pf *pf);
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_adminq_cmd.h b/drivers/net/ethernet/intel/i40evf/i40e_adminq_cmd.h
index 91d8786d386d..83e63e55c4b4 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_adminq_cmd.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_adminq_cmd.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  * Intel Ethernet Controller XL710 Family Linux Virtual Function Driver
- * Copyright(c) 2013 - 2016 Intel Corporation.
+ * Copyright(c) 2013 - 2017 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -528,7 +528,7 @@ struct i40e_aqc_mac_address_read {
 #define I40E_AQC_PORT_ADDR_VALID	0x40
 #define I40E_AQC_WOL_ADDR_VALID		0x80
 #define I40E_AQC_MC_MAG_EN_VALID	0x100
-#define I40E_AQC_ADDR_VALID_MASK	0x1F0
+#define I40E_AQC_ADDR_VALID_MASK	0x3F0
 	u8	reserved[6];
 	__le32	addr_high;
 	__le32	addr_low;
@@ -586,6 +586,7 @@ struct i40e_aqc_set_wol_filter {
 	__le16 cmd_flags;
 #define I40E_AQC_SET_WOL_FILTER				0x8000
 #define I40E_AQC_SET_WOL_FILTER_NO_TCO_WOL		0x4000
+#define I40E_AQC_SET_WOL_FILTER_WOL_PRESERVE_ON_PFR	0x2000
 #define I40E_AQC_SET_WOL_FILTER_ACTION_CLEAR		0
 #define I40E_AQC_SET_WOL_FILTER_ACTION_SET		1
 	__le16 valid_flags;
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_common.c b/drivers/net/ethernet/intel/i40evf/i40e_common.c
index 43f10761f4ba..1dd1938f594f 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_common.c
+++ b/drivers/net/ethernet/intel/i40evf/i40e_common.c
@@ -27,7 +27,7 @@
 #include "i40e_type.h"
 #include "i40e_adminq.h"
 #include "i40e_prototype.h"
-#include "i40e_virtchnl.h"
+#include <linux/avf/virtchnl.h>
 
 /**
  * i40e_set_mac_type - Sets MAC type
@@ -68,6 +68,7 @@ i40e_status i40e_set_mac_type(struct i40e_hw *hw)
 			break;
 		case I40E_DEV_ID_VF:
 		case I40E_DEV_ID_VF_HV:
+		case I40E_DEV_ID_ADAPTIVE_VF:
 			hw->mac.type = I40E_MAC_VF;
 			break;
 		default:
@@ -1054,7 +1055,7 @@ do_retry:
  * completion before returning.
  **/
 i40e_status i40e_aq_send_msg_to_pf(struct i40e_hw *hw,
-				enum i40e_virtchnl_ops v_opcode,
+				enum virtchnl_ops v_opcode,
 				i40e_status v_retval,
 				u8 *msg, u16 msglen,
 				struct i40e_asq_cmd_details *cmd_details)
@@ -1092,9 +1093,9 @@ i40e_status i40e_aq_send_msg_to_pf(struct i40e_hw *hw,
  * with appropriate information.
  **/
 void i40e_vf_parse_hw_config(struct i40e_hw *hw,
-			     struct i40e_virtchnl_vf_resource *msg)
+			     struct virtchnl_vf_resource *msg)
 {
-	struct i40e_virtchnl_vsi_resource *vsi_res;
+	struct virtchnl_vsi_resource *vsi_res;
 	int i;
 
 	vsi_res = &msg->vsi_res[0];
@@ -1104,11 +1105,10 @@ void i40e_vf_parse_hw_config(struct i40e_hw *hw,
 	hw->dev_caps.num_tx_qp = msg->num_queue_pairs;
 	hw->dev_caps.num_msix_vectors_vf = msg->max_vectors;
 	hw->dev_caps.dcb = msg->vf_offload_flags &
-			   I40E_VIRTCHNL_VF_OFFLOAD_L2;
-	hw->dev_caps.fcoe = (msg->vf_offload_flags &
-			     I40E_VIRTCHNL_VF_OFFLOAD_FCOE) ? 1 : 0;
+			   VIRTCHNL_VF_OFFLOAD_L2;
+	hw->dev_caps.fcoe = 0;
 	for (i = 0; i < msg->num_vsis; i++) {
-		if (vsi_res->vsi_type == I40E_VSI_SRIOV) {
+		if (vsi_res->vsi_type == VIRTCHNL_VSI_SRIOV) {
 			ether_addr_copy(hw->mac.perm_addr,
 					vsi_res->default_mac_addr);
 			ether_addr_copy(hw->mac.addr,
@@ -1128,7 +1128,7 @@ void i40e_vf_parse_hw_config(struct i40e_hw *hw,
  **/
 i40e_status i40e_vf_reset(struct i40e_hw *hw)
 {
-	return i40e_aq_send_msg_to_pf(hw, I40E_VIRTCHNL_OP_RESET_VF,
+	return i40e_aq_send_msg_to_pf(hw, VIRTCHNL_OP_RESET_VF,
 				      0, NULL, 0, NULL);
 }
 
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_devids.h b/drivers/net/ethernet/intel/i40evf/i40e_devids.h
index d76393c95056..0469e4bfd3ec 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_devids.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_devids.h
@@ -43,6 +43,7 @@
 #define I40E_DEV_ID_25G_SFP28		0x158B
 #define I40E_DEV_ID_VF			0x154C
 #define I40E_DEV_ID_VF_HV		0x1571
+#define I40E_DEV_ID_ADAPTIVE_VF		0x1889
 #define I40E_DEV_ID_SFP_X722		0x37D0
 #define I40E_DEV_ID_1G_BASE_T_X722	0x37D1
 #define I40E_DEV_ID_10G_BASE_T_X722	0x37D2
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_prototype.h b/drivers/net/ethernet/intel/i40evf/i40e_prototype.h
index 741223d5d809..c9836bba487d 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_prototype.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_prototype.h
@@ -29,7 +29,7 @@
 
 #include "i40e_type.h"
 #include "i40e_alloc.h"
-#include "i40e_virtchnl.h"
+#include <linux/avf/virtchnl.h>
 
 /* Prototypes for shared code functions that are not in
  * the standard function pointer structures.  These are
@@ -87,10 +87,10 @@ static inline struct i40e_rx_ptype_decoded decode_rx_desc_ptype(u8 ptype)
 
 /* i40e_common for VF drivers*/
 void i40e_vf_parse_hw_config(struct i40e_hw *hw,
-			     struct i40e_virtchnl_vf_resource *msg);
+			     struct virtchnl_vf_resource *msg);
 i40e_status i40e_vf_reset(struct i40e_hw *hw);
 i40e_status i40e_aq_send_msg_to_pf(struct i40e_hw *hw,
-				enum i40e_virtchnl_ops v_opcode,
+				enum virtchnl_ops v_opcode,
 				i40e_status v_retval,
 				u8 *msg, u16 msglen,
 				struct i40e_asq_cmd_details *cmd_details);
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h b/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h
deleted file mode 100644
index c5ad0388c3d5..000000000000
--- a/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h
+++ /dev/null
@@ -1,449 +0,0 @@
-/*******************************************************************************
- *
- * Intel Ethernet Controller XL710 Family Linux Virtual Function Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- * The full GNU General Public License is included in this distribution in
- * the file called "COPYING".
- *
- * Contact Information:
- * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
- * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
- *
- ******************************************************************************/
-
-#ifndef _I40E_VIRTCHNL_H_
-#define _I40E_VIRTCHNL_H_
-
-#include "i40e_type.h"
-
-/* Description:
- * This header file describes the VF-PF communication protocol used
- * by the various i40e drivers.
- *
- * Admin queue buffer usage:
- * desc->opcode is always i40e_aqc_opc_send_msg_to_pf
- * flags, retval, datalen, and data addr are all used normally.
- * Firmware copies the cookie fields when sending messages between the PF and
- * VF, but uses all other fields internally. Due to this limitation, we
- * must send all messages as "indirect", i.e. using an external buffer.
- *
- * All the vsi indexes are relative to the VF. Each VF can have maximum of
- * three VSIs. All the queue indexes are relative to the VSI.  Each VF can
- * have a maximum of sixteen queues for all of its VSIs.
- *
- * The PF is required to return a status code in v_retval for all messages
- * except RESET_VF, which does not require any response. The return value is of
- * i40e_status_code type, defined in the i40e_type.h.
- *
- * In general, VF driver initialization should roughly follow the order of these
- * opcodes. The VF driver must first validate the API version of the PF driver,
- * then request a reset, then get resources, then configure queues and
- * interrupts. After these operations are complete, the VF driver may start
- * its queues, optionally add MAC and VLAN filters, and process traffic.
- */
-
-/* Opcodes for VF-PF communication. These are placed in the v_opcode field
- * of the virtchnl_msg structure.
- */
-enum i40e_virtchnl_ops {
-/* The PF sends status change events to VFs using
- * the I40E_VIRTCHNL_OP_EVENT opcode.
- * VFs send requests to the PF using the other ops.
- */
-	I40E_VIRTCHNL_OP_UNKNOWN = 0,
-	I40E_VIRTCHNL_OP_VERSION = 1, /* must ALWAYS be 1 */
-	I40E_VIRTCHNL_OP_RESET_VF = 2,
-	I40E_VIRTCHNL_OP_GET_VF_RESOURCES = 3,
-	I40E_VIRTCHNL_OP_CONFIG_TX_QUEUE = 4,
-	I40E_VIRTCHNL_OP_CONFIG_RX_QUEUE = 5,
-	I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES = 6,
-	I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP = 7,
-	I40E_VIRTCHNL_OP_ENABLE_QUEUES = 8,
-	I40E_VIRTCHNL_OP_DISABLE_QUEUES = 9,
-	I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS = 10,
-	I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS = 11,
-	I40E_VIRTCHNL_OP_ADD_VLAN = 12,
-	I40E_VIRTCHNL_OP_DEL_VLAN = 13,
-	I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE = 14,
-	I40E_VIRTCHNL_OP_GET_STATS = 15,
-	I40E_VIRTCHNL_OP_FCOE = 16,
-	I40E_VIRTCHNL_OP_EVENT = 17, /* must ALWAYS be 17 */
-	I40E_VIRTCHNL_OP_IWARP = 20,
-	I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP = 21,
-	I40E_VIRTCHNL_OP_RELEASE_IWARP_IRQ_MAP = 22,
-	I40E_VIRTCHNL_OP_CONFIG_RSS_KEY = 23,
-	I40E_VIRTCHNL_OP_CONFIG_RSS_LUT = 24,
-	I40E_VIRTCHNL_OP_GET_RSS_HENA_CAPS = 25,
-	I40E_VIRTCHNL_OP_SET_RSS_HENA = 26,
-
-};
-
-/* Virtual channel message descriptor. This overlays the admin queue
- * descriptor. All other data is passed in external buffers.
- */
-
-struct i40e_virtchnl_msg {
-	u8 pad[8];			 /* AQ flags/opcode/len/retval fields */
-	enum i40e_virtchnl_ops v_opcode; /* avoid confusion with desc->opcode */
-	i40e_status v_retval;  /* ditto for desc->retval */
-	u32 vfid;			 /* used by PF when sending to VF */
-};
-
-/* Message descriptions and data structures.*/
-
-/* I40E_VIRTCHNL_OP_VERSION
- * VF posts its version number to the PF. PF responds with its version number
- * in the same format, along with a return code.
- * Reply from PF has its major/minor versions also in param0 and param1.
- * If there is a major version mismatch, then the VF cannot operate.
- * If there is a minor version mismatch, then the VF can operate but should
- * add a warning to the system log.
- *
- * This enum element MUST always be specified as == 1, regardless of other
- * changes in the API. The PF must always respond to this message without
- * error regardless of version mismatch.
- */
-#define I40E_VIRTCHNL_VERSION_MAJOR		1
-#define I40E_VIRTCHNL_VERSION_MINOR		1
-#define I40E_VIRTCHNL_VERSION_MINOR_NO_VF_CAPS	0
-
-struct i40e_virtchnl_version_info {
-	u32 major;
-	u32 minor;
-};
-
-/* I40E_VIRTCHNL_OP_RESET_VF
- * VF sends this request to PF with no parameters
- * PF does NOT respond! VF driver must delay then poll VFGEN_RSTAT register
- * until reset completion is indicated. The admin queue must be reinitialized
- * after this operation.
- *
- * When reset is complete, PF must ensure that all queues in all VSIs associated
- * with the VF are stopped, all queue configurations in the HMC are set to 0,
- * and all MAC and VLAN filters (except the default MAC address) on all VSIs
- * are cleared.
- */
-
-/* I40E_VIRTCHNL_OP_GET_VF_RESOURCES
- * Version 1.0 VF sends this request to PF with no parameters
- * Version 1.1 VF sends this request to PF with u32 bitmap of its capabilities
- * PF responds with an indirect message containing
- * i40e_virtchnl_vf_resource and one or more
- * i40e_virtchnl_vsi_resource structures.
- */
-
-struct i40e_virtchnl_vsi_resource {
-	u16 vsi_id;
-	u16 num_queue_pairs;
-	enum i40e_vsi_type vsi_type;
-	u16 qset_handle;
-	u8 default_mac_addr[ETH_ALEN];
-};
-/* VF offload flags */
-#define I40E_VIRTCHNL_VF_OFFLOAD_L2		0x00000001
-#define I40E_VIRTCHNL_VF_OFFLOAD_IWARP		0x00000002
-#define I40E_VIRTCHNL_VF_OFFLOAD_FCOE		0x00000004
-#define I40E_VIRTCHNL_VF_OFFLOAD_RSS_AQ		0x00000008
-#define I40E_VIRTCHNL_VF_OFFLOAD_RSS_REG	0x00000010
-#define I40E_VIRTCHNL_VF_OFFLOAD_WB_ON_ITR	0x00000020
-#define I40E_VIRTCHNL_VF_OFFLOAD_VLAN		0x00010000
-#define I40E_VIRTCHNL_VF_OFFLOAD_RX_POLLING	0x00020000
-#define I40E_VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2	0x00040000
-#define I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF		0X00080000
-#define I40E_VIRTCHNL_VF_OFFLOAD_ENCAP		0X00100000
-#define I40E_VIRTCHNL_VF_OFFLOAD_ENCAP_CSUM	0X00200000
-
-#define I40E_VF_BASE_MODE_OFFLOADS (I40E_VIRTCHNL_VF_OFFLOAD_L2 | \
-				    I40E_VIRTCHNL_VF_OFFLOAD_VLAN | \
-				    I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF)
-
-struct i40e_virtchnl_vf_resource {
-	u16 num_vsis;
-	u16 num_queue_pairs;
-	u16 max_vectors;
-	u16 max_mtu;
-
-	u32 vf_offload_flags;
-	u32 rss_key_size;
-	u32 rss_lut_size;
-
-	struct i40e_virtchnl_vsi_resource vsi_res[1];
-};
-
-/* I40E_VIRTCHNL_OP_CONFIG_TX_QUEUE
- * VF sends this message to set up parameters for one TX queue.
- * External data buffer contains one instance of i40e_virtchnl_txq_info.
- * PF configures requested queue and returns a status code.
- */
-
-/* Tx queue config info */
-struct i40e_virtchnl_txq_info {
-	u16 vsi_id;
-	u16 queue_id;
-	u16 ring_len;		/* number of descriptors, multiple of 8 */
-	u16 headwb_enabled;
-	u64 dma_ring_addr;
-	u64 dma_headwb_addr;
-};
-
-/* I40E_VIRTCHNL_OP_CONFIG_RX_QUEUE
- * VF sends this message to set up parameters for one RX queue.
- * External data buffer contains one instance of i40e_virtchnl_rxq_info.
- * PF configures requested queue and returns a status code.
- */
-
-/* Rx queue config info */
-struct i40e_virtchnl_rxq_info {
-	u16 vsi_id;
-	u16 queue_id;
-	u32 ring_len;		/* number of descriptors, multiple of 32 */
-	u16 hdr_size;
-	u16 splithdr_enabled;
-	u32 databuffer_size;
-	u32 max_pkt_size;
-	u64 dma_ring_addr;
-	enum i40e_hmc_obj_rx_hsplit_0 rx_split_pos;
-};
-
-/* I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES
- * VF sends this message to set parameters for all active TX and RX queues
- * associated with the specified VSI.
- * PF configures queues and returns status.
- * If the number of queues specified is greater than the number of queues
- * associated with the VSI, an error is returned and no queues are configured.
- */
-struct i40e_virtchnl_queue_pair_info {
-	/* NOTE: vsi_id and queue_id should be identical for both queues. */
-	struct i40e_virtchnl_txq_info txq;
-	struct i40e_virtchnl_rxq_info rxq;
-};
-
-struct i40e_virtchnl_vsi_queue_config_info {
-	u16 vsi_id;
-	u16 num_queue_pairs;
-	struct i40e_virtchnl_queue_pair_info qpair[1];
-};
-
-/* I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP
- * VF uses this message to map vectors to queues.
- * The rxq_map and txq_map fields are bitmaps used to indicate which queues
- * are to be associated with the specified vector.
- * The "other" causes are always mapped to vector 0.
- * PF configures interrupt mapping and returns status.
- */
-struct i40e_virtchnl_vector_map {
-	u16 vsi_id;
-	u16 vector_id;
-	u16 rxq_map;
-	u16 txq_map;
-	u16 rxitr_idx;
-	u16 txitr_idx;
-};
-
-struct i40e_virtchnl_irq_map_info {
-	u16 num_vectors;
-	struct i40e_virtchnl_vector_map vecmap[1];
-};
-
-/* I40E_VIRTCHNL_OP_ENABLE_QUEUES
- * I40E_VIRTCHNL_OP_DISABLE_QUEUES
- * VF sends these message to enable or disable TX/RX queue pairs.
- * The queues fields are bitmaps indicating which queues to act upon.
- * (Currently, we only support 16 queues per VF, but we make the field
- * u32 to allow for expansion.)
- * PF performs requested action and returns status.
- */
-struct i40e_virtchnl_queue_select {
-	u16 vsi_id;
-	u16 pad;
-	u32 rx_queues;
-	u32 tx_queues;
-};
-
-/* I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS
- * VF sends this message in order to add one or more unicast or multicast
- * address filters for the specified VSI.
- * PF adds the filters and returns status.
- */
-
-/* I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS
- * VF sends this message in order to remove one or more unicast or multicast
- * filters for the specified VSI.
- * PF removes the filters and returns status.
- */
-
-struct i40e_virtchnl_ether_addr {
-	u8 addr[ETH_ALEN];
-	u8 pad[2];
-};
-
-struct i40e_virtchnl_ether_addr_list {
-	u16 vsi_id;
-	u16 num_elements;
-	struct i40e_virtchnl_ether_addr list[1];
-};
-
-/* I40E_VIRTCHNL_OP_ADD_VLAN
- * VF sends this message to add one or more VLAN tag filters for receives.
- * PF adds the filters and returns status.
- * If a port VLAN is configured by the PF, this operation will return an
- * error to the VF.
- */
-
-/* I40E_VIRTCHNL_OP_DEL_VLAN
- * VF sends this message to remove one or more VLAN tag filters for receives.
- * PF removes the filters and returns status.
- * If a port VLAN is configured by the PF, this operation will return an
- * error to the VF.
- */
-
-struct i40e_virtchnl_vlan_filter_list {
-	u16 vsi_id;
-	u16 num_elements;
-	u16 vlan_id[1];
-};
-
-/* I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE
- * VF sends VSI id and flags.
- * PF returns status code in retval.
- * Note: we assume that broadcast accept mode is always enabled.
- */
-struct i40e_virtchnl_promisc_info {
-	u16 vsi_id;
-	u16 flags;
-};
-
-#define I40E_FLAG_VF_UNICAST_PROMISC	0x00000001
-#define I40E_FLAG_VF_MULTICAST_PROMISC	0x00000002
-
-/* I40E_VIRTCHNL_OP_GET_STATS
- * VF sends this message to request stats for the selected VSI. VF uses
- * the i40e_virtchnl_queue_select struct to specify the VSI. The queue_id
- * field is ignored by the PF.
- *
- * PF replies with struct i40e_eth_stats in an external buffer.
- */
-
-/* I40E_VIRTCHNL_OP_CONFIG_RSS_KEY
- * I40E_VIRTCHNL_OP_CONFIG_RSS_LUT
- * VF sends these messages to configure RSS. Only supported if both PF
- * and VF drivers set the I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF bit during
- * configuration negotiation. If this is the case, then the RSS fields in
- * the VF resource struct are valid.
- * Both the key and LUT are initialized to 0 by the PF, meaning that
- * RSS is effectively disabled until set up by the VF.
- */
-struct i40e_virtchnl_rss_key {
-	u16 vsi_id;
-	u16 key_len;
-	u8 key[1];         /* RSS hash key, packed bytes */
-};
-
-struct i40e_virtchnl_rss_lut {
-	u16 vsi_id;
-	u16 lut_entries;
-	u8 lut[1];        /* RSS lookup table*/
-};
-
-/* I40E_VIRTCHNL_OP_GET_RSS_HENA_CAPS
- * I40E_VIRTCHNL_OP_SET_RSS_HENA
- * VF sends these messages to get and set the hash filter enable bits for RSS.
- * By default, the PF sets these to all possible traffic types that the
- * hardware supports. The VF can query this value if it wants to change the
- * traffic types that are hashed by the hardware.
- * Traffic types are defined in the i40e_filter_pctype enum in i40e_type.h
- */
-struct i40e_virtchnl_rss_hena {
-	u64 hena;
-};
-
-/* I40E_VIRTCHNL_OP_EVENT
- * PF sends this message to inform the VF driver of events that may affect it.
- * No direct response is expected from the VF, though it may generate other
- * messages in response to this one.
- */
-enum i40e_virtchnl_event_codes {
-	I40E_VIRTCHNL_EVENT_UNKNOWN = 0,
-	I40E_VIRTCHNL_EVENT_LINK_CHANGE,
-	I40E_VIRTCHNL_EVENT_RESET_IMPENDING,
-	I40E_VIRTCHNL_EVENT_PF_DRIVER_CLOSE,
-};
-#define I40E_PF_EVENT_SEVERITY_INFO		0
-#define I40E_PF_EVENT_SEVERITY_CERTAIN_DOOM	255
-
-struct i40e_virtchnl_pf_event {
-	enum i40e_virtchnl_event_codes event;
-	union {
-		struct {
-			enum i40e_aq_link_speed link_speed;
-			bool link_status;
-		} link_event;
-	} event_data;
-
-	int severity;
-};
-
-/* I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP
- * VF uses this message to request PF to map IWARP vectors to IWARP queues.
- * The request for this originates from the VF IWARP driver through
- * a client interface between VF LAN and VF IWARP driver.
- * A vector could have an AEQ and CEQ attached to it although
- * there is a single AEQ per VF IWARP instance in which case
- * most vectors will have an INVALID_IDX for aeq and valid idx for ceq.
- * There will never be a case where there will be multiple CEQs attached
- * to a single vector.
- * PF configures interrupt mapping and returns status.
- */
-
-/* HW does not define a type value for AEQ; only for RX/TX and CEQ.
- * In order for us to keep the interface simple, SW will define a
- * unique type value for AEQ.
- */
-#define I40E_QUEUE_TYPE_PE_AEQ  0x80
-#define I40E_QUEUE_INVALID_IDX  0xFFFF
-
-struct i40e_virtchnl_iwarp_qv_info {
-	u32 v_idx; /* msix_vector */
-	u16 ceq_idx;
-	u16 aeq_idx;
-	u8 itr_idx;
-};
-
-struct i40e_virtchnl_iwarp_qvlist_info {
-	u32 num_vectors;
-	struct i40e_virtchnl_iwarp_qv_info qv_info[1];
-};
-
-/* VF reset states - these are written into the RSTAT register:
- * I40E_VFGEN_RSTAT1 on the PF
- * I40E_VFGEN_RSTAT on the VF
- * When the PF initiates a reset, it writes 0
- * When the reset is complete, it writes 1
- * When the PF detects that the VF has recovered, it writes 2
- * VF checks this register periodically to determine if a reset has occurred,
- * then polls it to know when the reset is complete.
- * If either the PF or VF reads the register while the hardware
- * is in a reset state, it will return DEADBEEF, which, when masked
- * will result in 3.
- */
-enum i40e_vfr_states {
-	I40E_VFR_INPROGRESS = 0,
-	I40E_VFR_COMPLETED,
-	I40E_VFR_VFACTIVE,
-	I40E_VFR_UNKNOWN,
-};
-
-#endif /* _I40E_VIRTCHNL_H_ */
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf.h b/drivers/net/ethernet/intel/i40evf/i40evf.h
index b8ada6d8d890..6cc92089fecb 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf.h
+++ b/drivers/net/ethernet/intel/i40evf/i40evf.h
@@ -43,7 +43,7 @@
 #include <net/udp.h>
 
 #include "i40e_type.h"
-#include "i40e_virtchnl.h"
+#include <linux/avf/virtchnl.h>
 #include "i40e_txrx.h"
 
 #define DEFAULT_DEBUG_LEVEL_SHIFT 3
@@ -263,26 +263,26 @@ struct i40evf_adapter {
 	struct work_struct watchdog_task;
 	bool netdev_registered;
 	bool link_up;
-	enum i40e_aq_link_speed link_speed;
-	enum i40e_virtchnl_ops current_op;
+	enum virtchnl_link_speed link_speed;
+	enum virtchnl_ops current_op;
 #define CLIENT_ALLOWED(_a) ((_a)->vf_res ? \
 			    (_a)->vf_res->vf_offload_flags & \
-				I40E_VIRTCHNL_VF_OFFLOAD_IWARP : \
+				VIRTCHNL_VF_OFFLOAD_IWARP : \
 			    0)
 #define CLIENT_ENABLED(_a) ((_a)->cinst)
 /* RSS by the PF should be preferred over RSS via other methods. */
 #define RSS_PF(_a) ((_a)->vf_res->vf_offload_flags & \
-		    I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF)
+		    VIRTCHNL_VF_OFFLOAD_RSS_PF)
 #define RSS_AQ(_a) ((_a)->vf_res->vf_offload_flags & \
-		    I40E_VIRTCHNL_VF_OFFLOAD_RSS_AQ)
+		    VIRTCHNL_VF_OFFLOAD_RSS_AQ)
 #define RSS_REG(_a) (!((_a)->vf_res->vf_offload_flags & \
-		       (I40E_VIRTCHNL_VF_OFFLOAD_RSS_AQ | \
-			I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF)))
+		       (VIRTCHNL_VF_OFFLOAD_RSS_AQ | \
+			VIRTCHNL_VF_OFFLOAD_RSS_PF)))
 #define VLAN_ALLOWED(_a) ((_a)->vf_res->vf_offload_flags & \
-			  I40E_VIRTCHNL_VF_OFFLOAD_VLAN)
-	struct i40e_virtchnl_vf_resource *vf_res; /* incl. all VSIs */
-	struct i40e_virtchnl_vsi_resource *vsi_res; /* our LAN VSI */
-	struct i40e_virtchnl_version_info pf_version;
+			  VIRTCHNL_VF_OFFLOAD_VLAN)
+	struct virtchnl_vf_resource *vf_res; /* incl. all VSIs */
+	struct virtchnl_vsi_resource *vsi_res; /* our LAN VSI */
+	struct virtchnl_version_info pf_version;
 #define PF_IS_V11(_a) (((_a)->pf_version.major == 1) && \
 		       ((_a)->pf_version.minor == 1))
 	u16 msg_enable;
@@ -348,7 +348,7 @@ void i40evf_set_hena(struct i40evf_adapter *adapter);
 void i40evf_set_rss_key(struct i40evf_adapter *adapter);
 void i40evf_set_rss_lut(struct i40evf_adapter *adapter);
 void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
-				enum i40e_virtchnl_ops v_opcode,
+				enum virtchnl_ops v_opcode,
 				i40e_status v_retval, u8 *msg, u16 msglen);
 int i40evf_config_rss(struct i40evf_adapter *adapter);
 int i40evf_lan_add_device(struct i40evf_adapter *adapter);
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_client.c b/drivers/net/ethernet/intel/i40evf/i40evf_client.c
index ee737680a0e9..93cf5fd17d91 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_client.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_client.c
@@ -120,7 +120,7 @@ static int i40evf_client_release_qvlist(struct i40e_info *ldev)
 		return -EAGAIN;
 
 	err = i40e_aq_send_msg_to_pf(&adapter->hw,
-			I40E_VIRTCHNL_OP_RELEASE_IWARP_IRQ_MAP,
+			VIRTCHNL_OP_RELEASE_IWARP_IRQ_MAP,
 			I40E_SUCCESS, NULL, 0, NULL);
 
 	if (err)
@@ -410,7 +410,7 @@ static u32 i40evf_client_virtchnl_send(struct i40e_info *ldev,
 	if (adapter->aq_required)
 		return -EAGAIN;
 
-	err = i40e_aq_send_msg_to_pf(&adapter->hw, I40E_VIRTCHNL_OP_IWARP,
+	err = i40e_aq_send_msg_to_pf(&adapter->hw, VIRTCHNL_OP_IWARP,
 				     I40E_SUCCESS, msg, len, NULL);
 	if (err)
 		dev_err(&adapter->pdev->dev, "Unable to send iWarp message to PF, error %d, aq status %d\n",
@@ -431,7 +431,7 @@ static int i40evf_client_setup_qvlist(struct i40e_info *ldev,
 				      struct i40e_client *client,
 				      struct i40e_qvlist_info *qvlist_info)
 {
-	struct i40e_virtchnl_iwarp_qvlist_info *v_qvlist_info;
+	struct virtchnl_iwarp_qvlist_info *v_qvlist_info;
 	struct i40evf_adapter *adapter = ldev->vf;
 	struct i40e_qv_info *qv_info;
 	i40e_status err;
@@ -453,14 +453,14 @@ static int i40evf_client_setup_qvlist(struct i40e_info *ldev,
 			return -EINVAL;
 	}
 
-	v_qvlist_info = (struct i40e_virtchnl_iwarp_qvlist_info *)qvlist_info;
-	msg_size = sizeof(struct i40e_virtchnl_iwarp_qvlist_info) +
-			(sizeof(struct i40e_virtchnl_iwarp_qv_info) *
+	v_qvlist_info = (struct virtchnl_iwarp_qvlist_info *)qvlist_info;
+	msg_size = sizeof(struct virtchnl_iwarp_qvlist_info) +
+			(sizeof(struct virtchnl_iwarp_qv_info) *
 			(v_qvlist_info->num_vectors - 1));
 
-	adapter->client_pending |= BIT(I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP);
+	adapter->client_pending |= BIT(VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP);
 	err = i40e_aq_send_msg_to_pf(&adapter->hw,
-			I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP,
+			VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP,
 			I40E_SUCCESS, (u8 *)v_qvlist_info, msg_size, NULL);
 
 	if (err) {
@@ -474,7 +474,7 @@ static int i40evf_client_setup_qvlist(struct i40e_info *ldev,
 	for (i = 0; i < 5; i++) {
 		msleep(100);
 		if (!(adapter->client_pending &
-		      BIT(I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP))) {
+		      BIT(VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP))) {
 			err = 0;
 			break;
 		}
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_main.c b/drivers/net/ethernet/intel/i40evf/i40evf_main.c
index ea110a730e16..7c213a347909 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_main.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_main.c
@@ -44,9 +44,9 @@ static const char i40evf_driver_string[] =
 
 #define DRV_KERN "-k"
 
-#define DRV_VERSION_MAJOR 2
-#define DRV_VERSION_MINOR 1
-#define DRV_VERSION_BUILD 14
+#define DRV_VERSION_MAJOR 3
+#define DRV_VERSION_MINOR 0
+#define DRV_VERSION_BUILD 0
 #define DRV_VERSION __stringify(DRV_VERSION_MAJOR) "." \
 	     __stringify(DRV_VERSION_MINOR) "." \
 	     __stringify(DRV_VERSION_BUILD) \
@@ -67,6 +67,7 @@ static const struct pci_device_id i40evf_pci_tbl[] = {
 	{PCI_VDEVICE(INTEL, I40E_DEV_ID_VF), 0},
 	{PCI_VDEVICE(INTEL, I40E_DEV_ID_VF_HV), 0},
 	{PCI_VDEVICE(INTEL, I40E_DEV_ID_X722_VF), 0},
+	{PCI_VDEVICE(INTEL, I40E_DEV_ID_ADAPTIVE_VF), 0},
 	/* required last entry */
 	{0, }
 };
@@ -1131,7 +1132,7 @@ void i40evf_down(struct i40evf_adapter *adapter)
 	if (!(adapter->flags & I40EVF_FLAG_PF_COMMS_FAILED) &&
 	    adapter->state != __I40EVF_RESETTING) {
 		/* cancel any current operation */
-		adapter->current_op = I40E_VIRTCHNL_OP_UNKNOWN;
+		adapter->current_op = VIRTCHNL_OP_UNKNOWN;
 		/* Schedule operations to close down the HW. Don't wait
 		 * here for this to complete. The watchdog is still running
 		 * and it will take care of this.
@@ -1197,6 +1198,7 @@ static void i40evf_free_queues(struct i40evf_adapter *adapter)
 {
 	if (!adapter->vsi_res)
 		return;
+	adapter->num_active_queues = 0;
 	kfree(adapter->tx_rings);
 	adapter->tx_rings = NULL;
 	kfree(adapter->rx_rings);
@@ -1213,18 +1215,22 @@ static void i40evf_free_queues(struct i40evf_adapter *adapter)
  **/
 static int i40evf_alloc_queues(struct i40evf_adapter *adapter)
 {
-	int i;
+	int i, num_active_queues;
+
+	num_active_queues = min_t(int,
+				  adapter->vsi_res->num_queue_pairs,
+				  (int)(num_online_cpus()));
 
-	adapter->tx_rings = kcalloc(adapter->num_active_queues,
+	adapter->tx_rings = kcalloc(num_active_queues,
 				    sizeof(struct i40e_ring), GFP_KERNEL);
 	if (!adapter->tx_rings)
 		goto err_out;
-	adapter->rx_rings = kcalloc(adapter->num_active_queues,
+	adapter->rx_rings = kcalloc(num_active_queues,
 				    sizeof(struct i40e_ring), GFP_KERNEL);
 	if (!adapter->rx_rings)
 		goto err_out;
 
-	for (i = 0; i < adapter->num_active_queues; i++) {
+	for (i = 0; i < num_active_queues; i++) {
 		struct i40e_ring *tx_ring;
 		struct i40e_ring *rx_ring;
 
@@ -1246,6 +1252,8 @@ static int i40evf_alloc_queues(struct i40evf_adapter *adapter)
 		rx_ring->rx_itr_setting = (I40E_ITR_DYNAMIC | I40E_ITR_RX_DEF);
 	}
 
+	adapter->num_active_queues = num_active_queues;
+
 	return 0;
 
 err_out:
@@ -1311,7 +1319,7 @@ static int i40evf_config_rss_aq(struct i40evf_adapter *adapter)
 	struct i40e_hw *hw = &adapter->hw;
 	int ret = 0;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot configure RSS, command %d pending\n",
 			adapter->current_op);
@@ -1410,7 +1418,7 @@ static int i40evf_init_rss(struct i40evf_adapter *adapter)
 	if (!RSS_PF(adapter)) {
 		/* Enable PCTYPES for RSS, TCP/UDP with IPv4/IPv6 */
 		if (adapter->vf_res->vf_offload_flags &
-		    I40E_VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2)
+		    VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2)
 			adapter->hena = I40E_DEFAULT_RSS_HENA_EXPANDED;
 		else
 			adapter->hena = I40E_DEFAULT_RSS_HENA;
@@ -1588,8 +1596,8 @@ static void i40evf_watchdog_task(struct work_struct *work)
 	if (adapter->flags & I40EVF_FLAG_PF_COMMS_FAILED) {
 		reg_val = rd32(hw, I40E_VFGEN_RSTAT) &
 			  I40E_VFGEN_RSTAT_VFR_STATE_MASK;
-		if ((reg_val == I40E_VFR_VFACTIVE) ||
-		    (reg_val == I40E_VFR_COMPLETED)) {
+		if ((reg_val == VIRTCHNL_VFR_VFACTIVE) ||
+		    (reg_val == VIRTCHNL_VFR_COMPLETED)) {
 			/* A chance for redemption! */
 			dev_err(&adapter->pdev->dev, "Hardware came out of reset. Attempting reinit.\n");
 			adapter->state = __I40EVF_STARTUP;
@@ -1605,7 +1613,7 @@ static void i40evf_watchdog_task(struct work_struct *work)
 			return;
 		}
 		adapter->aq_required = 0;
-		adapter->current_op = I40E_VIRTCHNL_OP_UNKNOWN;
+		adapter->current_op = VIRTCHNL_OP_UNKNOWN;
 		goto watchdog_done;
 	}
 
@@ -1621,7 +1629,7 @@ static void i40evf_watchdog_task(struct work_struct *work)
 		dev_err(&adapter->pdev->dev, "Hardware reset detected\n");
 		schedule_work(&adapter->reset_task);
 		adapter->aq_required = 0;
-		adapter->current_op = I40E_VIRTCHNL_OP_UNKNOWN;
+		adapter->current_op = VIRTCHNL_OP_UNKNOWN;
 		goto watchdog_done;
 	}
 
@@ -1707,13 +1715,13 @@ static void i40evf_watchdog_task(struct work_struct *work)
 	}
 
 	if (adapter->aq_required & I40EVF_FLAG_AQ_REQUEST_PROMISC) {
-		i40evf_set_promiscuous(adapter, I40E_FLAG_VF_UNICAST_PROMISC |
-				       I40E_FLAG_VF_MULTICAST_PROMISC);
+		i40evf_set_promiscuous(adapter, FLAG_VF_UNICAST_PROMISC |
+				       FLAG_VF_MULTICAST_PROMISC);
 		goto watchdog_done;
 	}
 
 	if (adapter->aq_required & I40EVF_FLAG_AQ_REQUEST_ALLMULTI) {
-		i40evf_set_promiscuous(adapter, I40E_FLAG_VF_MULTICAST_PROMISC);
+		i40evf_set_promiscuous(adapter, FLAG_VF_MULTICAST_PROMISC);
 		goto watchdog_done;
 	}
 
@@ -1854,7 +1862,7 @@ static void i40evf_reset_task(struct work_struct *work)
 
 		reg_val = rd32(hw, I40E_VFGEN_RSTAT) &
 			  I40E_VFGEN_RSTAT_VFR_STATE_MASK;
-		if (reg_val == I40E_VFR_VFACTIVE)
+		if (reg_val == VIRTCHNL_VFR_VFACTIVE)
 			break;
 	}
 
@@ -1888,7 +1896,7 @@ continue_reset:
 
 	/* kill and reinit the admin queue */
 	i40evf_shutdown_adminq(hw);
-	adapter->current_op = I40E_VIRTCHNL_OP_UNKNOWN;
+	adapter->current_op = VIRTCHNL_OP_UNKNOWN;
 	err = i40evf_init_adminq(hw);
 	if (err)
 		dev_info(&adapter->pdev->dev, "Failed to init adminq: %d\n",
@@ -1949,7 +1957,7 @@ static void i40evf_adminq_task(struct work_struct *work)
 		container_of(work, struct i40evf_adapter, adminq_task);
 	struct i40e_hw *hw = &adapter->hw;
 	struct i40e_arq_event_info event;
-	struct i40e_virtchnl_msg *v_msg;
+	struct virtchnl_msg *v_msg;
 	i40e_status ret;
 	u32 val, oldval;
 	u16 pending;
@@ -1962,14 +1970,15 @@ static void i40evf_adminq_task(struct work_struct *work)
 	if (!event.msg_buf)
 		goto out;
 
-	v_msg = (struct i40e_virtchnl_msg *)&event.desc;
+	v_msg = (struct virtchnl_msg *)&event.desc;
 	do {
 		ret = i40evf_clean_arq_element(hw, &event, &pending);
 		if (ret || !v_msg->v_opcode)
 			break; /* No event to process or error cleaning ARQ */
 
 		i40evf_virtchnl_completion(adapter, v_msg->v_opcode,
-					   v_msg->v_retval, event.msg_buf,
+					   (i40e_status)v_msg->v_retval,
+					   event.msg_buf,
 					   event.msg_len);
 		if (pending != 0)
 			memset(event.msg_buf, 0, I40EVF_MAX_AQ_BUF_SIZE);
@@ -2347,7 +2356,7 @@ static netdev_features_t i40evf_fix_features(struct net_device *netdev,
 	struct i40evf_adapter *adapter = netdev_priv(netdev);
 
 	features &= ~I40EVF_VLAN_FEATURES;
-	if (adapter->vf_res->vf_offload_flags & I40E_VIRTCHNL_VF_OFFLOAD_VLAN)
+	if (adapter->vf_res->vf_offload_flags & VIRTCHNL_VF_OFFLOAD_VLAN)
 		features |= I40EVF_VLAN_FEATURES;
 	return features;
 }
@@ -2384,8 +2393,8 @@ static int i40evf_check_reset_complete(struct i40e_hw *hw)
 	for (i = 0; i < 100; i++) {
 		rstat = rd32(hw, I40E_VFGEN_RSTAT) &
 			    I40E_VFGEN_RSTAT_VFR_STATE_MASK;
-		if ((rstat == I40E_VFR_VFACTIVE) ||
-		    (rstat == I40E_VFR_COMPLETED))
+		if ((rstat == VIRTCHNL_VFR_VFACTIVE) ||
+		    (rstat == VIRTCHNL_VFR_COMPLETED))
 			return 0;
 		usleep_range(10, 20);
 	}
@@ -2401,7 +2410,7 @@ static int i40evf_check_reset_complete(struct i40e_hw *hw)
  **/
 int i40evf_process_config(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_vf_resource *vfres = adapter->vf_res;
+	struct virtchnl_vf_resource *vfres = adapter->vf_res;
 	struct net_device *netdev = adapter->netdev;
 	struct i40e_vsi *vsi = &adapter->vsi;
 	int i;
@@ -2410,7 +2419,7 @@ int i40evf_process_config(struct i40evf_adapter *adapter)
 
 	/* got VF config message back from PF, now we can parse it */
 	for (i = 0; i < vfres->num_vsis; i++) {
-		if (vfres->vsi_res[i].vsi_type == I40E_VSI_SRIOV)
+		if (vfres->vsi_res[i].vsi_type == VIRTCHNL_VSI_SRIOV)
 			adapter->vsi_res = &vfres->vsi_res[i];
 	}
 	if (!adapter->vsi_res) {
@@ -2434,7 +2443,7 @@ int i40evf_process_config(struct i40evf_adapter *adapter)
 	/* advertise to stack only if offloads for encapsulated packets is
 	 * supported
 	 */
-	if (vfres->vf_offload_flags & I40E_VIRTCHNL_VF_OFFLOAD_ENCAP) {
+	if (vfres->vf_offload_flags & VIRTCHNL_VF_OFFLOAD_ENCAP) {
 		hw_enc_features |= NETIF_F_GSO_UDP_TUNNEL	|
 				   NETIF_F_GSO_GRE		|
 				   NETIF_F_GSO_GRE_CSUM		|
@@ -2445,7 +2454,7 @@ int i40evf_process_config(struct i40evf_adapter *adapter)
 				   0;
 
 		if (!(vfres->vf_offload_flags &
-		      I40E_VIRTCHNL_VF_OFFLOAD_ENCAP_CSUM))
+		      VIRTCHNL_VF_OFFLOAD_ENCAP_CSUM))
 			netdev->gso_partial_features |=
 				NETIF_F_GSO_UDP_TUNNEL_CSUM;
 
@@ -2472,7 +2481,7 @@ int i40evf_process_config(struct i40evf_adapter *adapter)
 	adapter->vsi.work_limit = I40E_DEFAULT_IRQ_WORK;
 	vsi->netdev = adapter->netdev;
 	vsi->qs_handle = adapter->vsi_res->qset_handle;
-	if (vfres->vf_offload_flags & I40E_VIRTCHNL_VF_OFFLOAD_RSS_PF) {
+	if (vfres->vf_offload_flags & VIRTCHNL_VF_OFFLOAD_RSS_PF) {
 		adapter->rss_key_size = vfres->rss_key_size;
 		adapter->rss_lut_size = vfres->rss_lut_size;
 	} else {
@@ -2558,8 +2567,8 @@ static void i40evf_init_task(struct work_struct *work)
 				dev_err(&pdev->dev, "Unsupported PF API version %d.%d, expected %d.%d\n",
 					adapter->pf_version.major,
 					adapter->pf_version.minor,
-					I40E_VIRTCHNL_VERSION_MAJOR,
-					I40E_VIRTCHNL_VERSION_MINOR);
+					VIRTCHNL_VERSION_MAJOR,
+					VIRTCHNL_VERSION_MINOR);
 			goto err;
 		}
 		err = i40evf_send_vf_config_msg(adapter);
@@ -2573,9 +2582,9 @@ static void i40evf_init_task(struct work_struct *work)
 	case __I40EVF_INIT_GET_RESOURCES:
 		/* aq msg sent, awaiting reply */
 		if (!adapter->vf_res) {
-			bufsz = sizeof(struct i40e_virtchnl_vf_resource) +
+			bufsz = sizeof(struct virtchnl_vf_resource) +
 				(I40E_MAX_VF_VSI *
-				 sizeof(struct i40e_virtchnl_vsi_resource));
+				 sizeof(struct virtchnl_vsi_resource));
 			adapter->vf_res = kzalloc(bufsz, GFP_KERNEL);
 			if (!adapter->vf_res)
 				goto err;
@@ -2606,7 +2615,7 @@ static void i40evf_init_task(struct work_struct *work)
 
 	if (i40evf_process_config(adapter))
 		goto err_alloc;
-	adapter->current_op = I40E_VIRTCHNL_OP_UNKNOWN;
+	adapter->current_op = VIRTCHNL_OP_UNKNOWN;
 
 	adapter->flags |= I40EVF_FLAG_RX_CSUM_ENABLED;
 
@@ -2634,9 +2643,6 @@ static void i40evf_init_task(struct work_struct *work)
 	adapter->watchdog_timer.data = (unsigned long)adapter;
 	mod_timer(&adapter->watchdog_timer, jiffies + 1);
 
-	adapter->num_active_queues = min_t(int,
-					   adapter->vsi_res->num_queue_pairs,
-					   (int)(num_online_cpus()));
 	adapter->tx_desc_count = I40EVF_DEFAULT_TXD;
 	adapter->rx_desc_count = I40EVF_DEFAULT_RXD;
 	err = i40evf_init_interrupt_scheme(adapter);
@@ -2644,7 +2650,7 @@ static void i40evf_init_task(struct work_struct *work)
 		goto err_sw_init;
 	i40evf_map_rings_to_vectors(adapter);
 	if (adapter->vf_res->vf_offload_flags &
-	    I40E_VIRTCHNL_VF_OFFLOAD_WB_ON_ITR)
+	    VIRTCHNL_VF_OFFLOAD_WB_ON_ITR)
 		adapter->flags |= I40EVF_FLAG_WB_ON_ITR_CAPABLE;
 
 	err = i40evf_request_misc_irq(adapter);
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c b/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
index deb2cb8dac6b..d2bb250a71af 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
@@ -42,7 +42,7 @@
  * Send message to PF and print status if failure.
  **/
 static int i40evf_send_pf_msg(struct i40evf_adapter *adapter,
-			      enum i40e_virtchnl_ops op, u8 *msg, u16 len)
+			      enum virtchnl_ops op, u8 *msg, u16 len)
 {
 	struct i40e_hw *hw = &adapter->hw;
 	i40e_status err;
@@ -68,12 +68,12 @@ static int i40evf_send_pf_msg(struct i40evf_adapter *adapter,
  **/
 int i40evf_send_api_ver(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_version_info vvi;
+	struct virtchnl_version_info vvi;
 
-	vvi.major = I40E_VIRTCHNL_VERSION_MAJOR;
-	vvi.minor = I40E_VIRTCHNL_VERSION_MINOR;
+	vvi.major = VIRTCHNL_VERSION_MAJOR;
+	vvi.minor = VIRTCHNL_VERSION_MINOR;
 
-	return i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_VERSION, (u8 *)&vvi,
+	return i40evf_send_pf_msg(adapter, VIRTCHNL_OP_VERSION, (u8 *)&vvi,
 				  sizeof(vvi));
 }
 
@@ -88,10 +88,10 @@ int i40evf_send_api_ver(struct i40evf_adapter *adapter)
  **/
 int i40evf_verify_api_ver(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_version_info *pf_vvi;
+	struct virtchnl_version_info *pf_vvi;
 	struct i40e_hw *hw = &adapter->hw;
 	struct i40e_arq_event_info event;
-	enum i40e_virtchnl_ops op;
+	enum virtchnl_ops op;
 	i40e_status err;
 
 	event.buf_len = I40EVF_MAX_AQ_BUF_SIZE;
@@ -109,8 +109,8 @@ int i40evf_verify_api_ver(struct i40evf_adapter *adapter)
 		if (err)
 			goto out_alloc;
 		op =
-		    (enum i40e_virtchnl_ops)le32_to_cpu(event.desc.cookie_high);
-		if (op == I40E_VIRTCHNL_OP_VERSION)
+		    (enum virtchnl_ops)le32_to_cpu(event.desc.cookie_high);
+		if (op == VIRTCHNL_OP_VERSION)
 			break;
 	}
 
@@ -119,19 +119,19 @@ int i40evf_verify_api_ver(struct i40evf_adapter *adapter)
 	if (err)
 		goto out_alloc;
 
-	if (op != I40E_VIRTCHNL_OP_VERSION) {
+	if (op != VIRTCHNL_OP_VERSION) {
 		dev_info(&adapter->pdev->dev, "Invalid reply type %d from PF\n",
 			op);
 		err = -EIO;
 		goto out_alloc;
 	}
 
-	pf_vvi = (struct i40e_virtchnl_version_info *)event.msg_buf;
+	pf_vvi = (struct virtchnl_version_info *)event.msg_buf;
 	adapter->pf_version = *pf_vvi;
 
-	if ((pf_vvi->major > I40E_VIRTCHNL_VERSION_MAJOR) ||
-	    ((pf_vvi->major == I40E_VIRTCHNL_VERSION_MAJOR) &&
-	     (pf_vvi->minor > I40E_VIRTCHNL_VERSION_MINOR)))
+	if ((pf_vvi->major > VIRTCHNL_VERSION_MAJOR) ||
+	    ((pf_vvi->major == VIRTCHNL_VERSION_MAJOR) &&
+	     (pf_vvi->minor > VIRTCHNL_VERSION_MINOR)))
 		err = -EIO;
 
 out_alloc:
@@ -152,26 +152,25 @@ int i40evf_send_vf_config_msg(struct i40evf_adapter *adapter)
 {
 	u32 caps;
 
-	adapter->current_op = I40E_VIRTCHNL_OP_GET_VF_RESOURCES;
-	adapter->aq_required &= ~I40EVF_FLAG_AQ_GET_CONFIG;
-	caps = I40E_VIRTCHNL_VF_OFFLOAD_L2 |
-	       I40E_VIRTCHNL_VF_OFFLOAD_RSS_AQ |
-	       I40E_VIRTCHNL_VF_OFFLOAD_RSS_REG |
-	       I40E_VIRTCHNL_VF_OFFLOAD_VLAN |
-	       I40E_VIRTCHNL_VF_OFFLOAD_WB_ON_ITR |
-	       I40E_VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2 |
-	       I40E_VIRTCHNL_VF_OFFLOAD_ENCAP |
-	       I40E_VIRTCHNL_VF_OFFLOAD_ENCAP_CSUM;
-
-	adapter->current_op = I40E_VIRTCHNL_OP_GET_VF_RESOURCES;
+	caps = VIRTCHNL_VF_OFFLOAD_L2 |
+	       VIRTCHNL_VF_OFFLOAD_RSS_PF |
+	       VIRTCHNL_VF_OFFLOAD_RSS_AQ |
+	       VIRTCHNL_VF_OFFLOAD_RSS_REG |
+	       VIRTCHNL_VF_OFFLOAD_VLAN |
+	       VIRTCHNL_VF_OFFLOAD_WB_ON_ITR |
+	       VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2 |
+	       VIRTCHNL_VF_OFFLOAD_ENCAP |
+	       VIRTCHNL_VF_OFFLOAD_ENCAP_CSUM;
+
+	adapter->current_op = VIRTCHNL_OP_GET_VF_RESOURCES;
 	adapter->aq_required &= ~I40EVF_FLAG_AQ_GET_CONFIG;
 	if (PF_IS_V11(adapter))
 		return i40evf_send_pf_msg(adapter,
-					  I40E_VIRTCHNL_OP_GET_VF_RESOURCES,
+					  VIRTCHNL_OP_GET_VF_RESOURCES,
 					  (u8 *)&caps, sizeof(caps));
 	else
 		return i40evf_send_pf_msg(adapter,
-					  I40E_VIRTCHNL_OP_GET_VF_RESOURCES,
+					  VIRTCHNL_OP_GET_VF_RESOURCES,
 					  NULL, 0);
 }
 
@@ -189,12 +188,12 @@ int i40evf_get_vf_config(struct i40evf_adapter *adapter)
 {
 	struct i40e_hw *hw = &adapter->hw;
 	struct i40e_arq_event_info event;
-	enum i40e_virtchnl_ops op;
+	enum virtchnl_ops op;
 	i40e_status err;
 	u16 len;
 
-	len =  sizeof(struct i40e_virtchnl_vf_resource) +
-		I40E_MAX_VF_VSI * sizeof(struct i40e_virtchnl_vsi_resource);
+	len =  sizeof(struct virtchnl_vf_resource) +
+		I40E_MAX_VF_VSI * sizeof(struct virtchnl_vsi_resource);
 	event.buf_len = len;
 	event.msg_buf = kzalloc(event.buf_len, GFP_KERNEL);
 	if (!event.msg_buf) {
@@ -210,8 +209,8 @@ int i40evf_get_vf_config(struct i40evf_adapter *adapter)
 		if (err)
 			goto out_alloc;
 		op =
-		    (enum i40e_virtchnl_ops)le32_to_cpu(event.desc.cookie_high);
-		if (op == I40E_VIRTCHNL_OP_GET_VF_RESOURCES)
+		    (enum virtchnl_ops)le32_to_cpu(event.desc.cookie_high);
+		if (op == VIRTCHNL_OP_GET_VF_RESOURCES)
 			break;
 	}
 
@@ -233,20 +232,20 @@ out:
  **/
 void i40evf_configure_queues(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_vsi_queue_config_info *vqci;
-	struct i40e_virtchnl_queue_pair_info *vqpi;
+	struct virtchnl_vsi_queue_config_info *vqci;
+	struct virtchnl_queue_pair_info *vqpi;
 	int pairs = adapter->num_active_queues;
 	int i, len, max_frame = I40E_MAX_RXBUFFER;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot configure queues, command %d pending\n",
 			adapter->current_op);
 		return;
 	}
-	adapter->current_op = I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES;
-	len = sizeof(struct i40e_virtchnl_vsi_queue_config_info) +
-		       (sizeof(struct i40e_virtchnl_queue_pair_info) * pairs);
+	adapter->current_op = VIRTCHNL_OP_CONFIG_VSI_QUEUES;
+	len = sizeof(struct virtchnl_vsi_queue_config_info) +
+		       (sizeof(struct virtchnl_queue_pair_info) * pairs);
 	vqci = kzalloc(len, GFP_KERNEL);
 	if (!vqci)
 		return;
@@ -279,7 +278,7 @@ void i40evf_configure_queues(struct i40evf_adapter *adapter)
 	}
 
 	adapter->aq_required &= ~I40EVF_FLAG_AQ_CONFIGURE_QUEUES;
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES,
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_CONFIG_VSI_QUEUES,
 			   (u8 *)vqci, len);
 	kfree(vqci);
 }
@@ -292,20 +291,20 @@ void i40evf_configure_queues(struct i40evf_adapter *adapter)
  **/
 void i40evf_enable_queues(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_queue_select vqs;
+	struct virtchnl_queue_select vqs;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot enable queues, command %d pending\n",
 			adapter->current_op);
 		return;
 	}
-	adapter->current_op = I40E_VIRTCHNL_OP_ENABLE_QUEUES;
+	adapter->current_op = VIRTCHNL_OP_ENABLE_QUEUES;
 	vqs.vsi_id = adapter->vsi_res->vsi_id;
 	vqs.tx_queues = BIT(adapter->num_active_queues) - 1;
 	vqs.rx_queues = vqs.tx_queues;
 	adapter->aq_required &= ~I40EVF_FLAG_AQ_ENABLE_QUEUES;
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_ENABLE_QUEUES,
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_ENABLE_QUEUES,
 			   (u8 *)&vqs, sizeof(vqs));
 }
 
@@ -317,20 +316,20 @@ void i40evf_enable_queues(struct i40evf_adapter *adapter)
  **/
 void i40evf_disable_queues(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_queue_select vqs;
+	struct virtchnl_queue_select vqs;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot disable queues, command %d pending\n",
 			adapter->current_op);
 		return;
 	}
-	adapter->current_op = I40E_VIRTCHNL_OP_DISABLE_QUEUES;
+	adapter->current_op = VIRTCHNL_OP_DISABLE_QUEUES;
 	vqs.vsi_id = adapter->vsi_res->vsi_id;
 	vqs.tx_queues = BIT(adapter->num_active_queues) - 1;
 	vqs.rx_queues = vqs.tx_queues;
 	adapter->aq_required &= ~I40EVF_FLAG_AQ_DISABLE_QUEUES;
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_DISABLE_QUEUES,
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_DISABLE_QUEUES,
 			   (u8 *)&vqs, sizeof(vqs));
 }
 
@@ -343,23 +342,23 @@ void i40evf_disable_queues(struct i40evf_adapter *adapter)
  **/
 void i40evf_map_queues(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_irq_map_info *vimi;
+	struct virtchnl_irq_map_info *vimi;
 	int v_idx, q_vectors, len;
 	struct i40e_q_vector *q_vector;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot map queues to vectors, command %d pending\n",
 			adapter->current_op);
 		return;
 	}
-	adapter->current_op = I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP;
+	adapter->current_op = VIRTCHNL_OP_CONFIG_IRQ_MAP;
 
 	q_vectors = adapter->num_msix_vectors - NONQ_VECS;
 
-	len = sizeof(struct i40e_virtchnl_irq_map_info) +
+	len = sizeof(struct virtchnl_irq_map_info) +
 	      (adapter->num_msix_vectors *
-		sizeof(struct i40e_virtchnl_vector_map));
+		sizeof(struct virtchnl_vector_map));
 	vimi = kzalloc(len, GFP_KERNEL);
 	if (!vimi)
 		return;
@@ -380,7 +379,7 @@ void i40evf_map_queues(struct i40evf_adapter *adapter)
 	vimi->vecmap[v_idx].rxq_map = 0;
 
 	adapter->aq_required &= ~I40EVF_FLAG_AQ_MAP_VECTORS;
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP,
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_CONFIG_IRQ_MAP,
 			   (u8 *)vimi, len);
 	kfree(vimi);
 }
@@ -395,12 +394,12 @@ void i40evf_map_queues(struct i40evf_adapter *adapter)
  **/
 void i40evf_add_ether_addrs(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_ether_addr_list *veal;
+	struct virtchnl_ether_addr_list *veal;
 	int len, i = 0, count = 0;
 	struct i40evf_mac_filter *f;
 	bool more = false;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot add filters, command %d pending\n",
 			adapter->current_op);
@@ -414,17 +413,17 @@ void i40evf_add_ether_addrs(struct i40evf_adapter *adapter)
 		adapter->aq_required &= ~I40EVF_FLAG_AQ_ADD_MAC_FILTER;
 		return;
 	}
-	adapter->current_op = I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS;
+	adapter->current_op = VIRTCHNL_OP_ADD_ETH_ADDR;
 
-	len = sizeof(struct i40e_virtchnl_ether_addr_list) +
-	      (count * sizeof(struct i40e_virtchnl_ether_addr));
+	len = sizeof(struct virtchnl_ether_addr_list) +
+	      (count * sizeof(struct virtchnl_ether_addr));
 	if (len > I40EVF_MAX_AQ_BUF_SIZE) {
 		dev_warn(&adapter->pdev->dev, "Too many add MAC changes in one request\n");
 		count = (I40EVF_MAX_AQ_BUF_SIZE -
-			 sizeof(struct i40e_virtchnl_ether_addr_list)) /
-			sizeof(struct i40e_virtchnl_ether_addr);
-		len = sizeof(struct i40e_virtchnl_ether_addr_list) +
-		      (count * sizeof(struct i40e_virtchnl_ether_addr));
+			 sizeof(struct virtchnl_ether_addr_list)) /
+			sizeof(struct virtchnl_ether_addr);
+		len = sizeof(struct virtchnl_ether_addr_list) +
+		      (count * sizeof(struct virtchnl_ether_addr));
 		more = true;
 	}
 
@@ -445,7 +444,7 @@ void i40evf_add_ether_addrs(struct i40evf_adapter *adapter)
 	}
 	if (!more)
 		adapter->aq_required &= ~I40EVF_FLAG_AQ_ADD_MAC_FILTER;
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS,
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_ADD_ETH_ADDR,
 			   (u8 *)veal, len);
 	kfree(veal);
 }
@@ -460,12 +459,12 @@ void i40evf_add_ether_addrs(struct i40evf_adapter *adapter)
  **/
 void i40evf_del_ether_addrs(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_ether_addr_list *veal;
+	struct virtchnl_ether_addr_list *veal;
 	struct i40evf_mac_filter *f, *ftmp;
 	int len, i = 0, count = 0;
 	bool more = false;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot remove filters, command %d pending\n",
 			adapter->current_op);
@@ -479,17 +478,17 @@ void i40evf_del_ether_addrs(struct i40evf_adapter *adapter)
 		adapter->aq_required &= ~I40EVF_FLAG_AQ_DEL_MAC_FILTER;
 		return;
 	}
-	adapter->current_op = I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS;
+	adapter->current_op = VIRTCHNL_OP_DEL_ETH_ADDR;
 
-	len = sizeof(struct i40e_virtchnl_ether_addr_list) +
-	      (count * sizeof(struct i40e_virtchnl_ether_addr));
+	len = sizeof(struct virtchnl_ether_addr_list) +
+	      (count * sizeof(struct virtchnl_ether_addr));
 	if (len > I40EVF_MAX_AQ_BUF_SIZE) {
 		dev_warn(&adapter->pdev->dev, "Too many delete MAC changes in one request\n");
 		count = (I40EVF_MAX_AQ_BUF_SIZE -
-			 sizeof(struct i40e_virtchnl_ether_addr_list)) /
-			sizeof(struct i40e_virtchnl_ether_addr);
-		len = sizeof(struct i40e_virtchnl_ether_addr_list) +
-		      (count * sizeof(struct i40e_virtchnl_ether_addr));
+			 sizeof(struct virtchnl_ether_addr_list)) /
+			sizeof(struct virtchnl_ether_addr);
+		len = sizeof(struct virtchnl_ether_addr_list) +
+		      (count * sizeof(struct virtchnl_ether_addr));
 		more = true;
 	}
 	veal = kzalloc(len, GFP_KERNEL);
@@ -510,7 +509,7 @@ void i40evf_del_ether_addrs(struct i40evf_adapter *adapter)
 	}
 	if (!more)
 		adapter->aq_required &= ~I40EVF_FLAG_AQ_DEL_MAC_FILTER;
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS,
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_DEL_ETH_ADDR,
 			   (u8 *)veal, len);
 	kfree(veal);
 }
@@ -525,12 +524,12 @@ void i40evf_del_ether_addrs(struct i40evf_adapter *adapter)
  **/
 void i40evf_add_vlans(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_vlan_filter_list *vvfl;
+	struct virtchnl_vlan_filter_list *vvfl;
 	int len, i = 0, count = 0;
 	struct i40evf_vlan_filter *f;
 	bool more = false;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot add VLANs, command %d pending\n",
 			adapter->current_op);
@@ -545,16 +544,16 @@ void i40evf_add_vlans(struct i40evf_adapter *adapter)
 		adapter->aq_required &= ~I40EVF_FLAG_AQ_ADD_VLAN_FILTER;
 		return;
 	}
-	adapter->current_op = I40E_VIRTCHNL_OP_ADD_VLAN;
+	adapter->current_op = VIRTCHNL_OP_ADD_VLAN;
 
-	len = sizeof(struct i40e_virtchnl_vlan_filter_list) +
+	len = sizeof(struct virtchnl_vlan_filter_list) +
 	      (count * sizeof(u16));
 	if (len > I40EVF_MAX_AQ_BUF_SIZE) {
 		dev_warn(&adapter->pdev->dev, "Too many add VLAN changes in one request\n");
 		count = (I40EVF_MAX_AQ_BUF_SIZE -
-			 sizeof(struct i40e_virtchnl_vlan_filter_list)) /
+			 sizeof(struct virtchnl_vlan_filter_list)) /
 			sizeof(u16);
-		len = sizeof(struct i40e_virtchnl_vlan_filter_list) +
+		len = sizeof(struct virtchnl_vlan_filter_list) +
 		      (count * sizeof(u16));
 		more = true;
 	}
@@ -575,7 +574,7 @@ void i40evf_add_vlans(struct i40evf_adapter *adapter)
 	}
 	if (!more)
 		adapter->aq_required &= ~I40EVF_FLAG_AQ_ADD_VLAN_FILTER;
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_ADD_VLAN, (u8 *)vvfl, len);
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_ADD_VLAN, (u8 *)vvfl, len);
 	kfree(vvfl);
 }
 
@@ -589,12 +588,12 @@ void i40evf_add_vlans(struct i40evf_adapter *adapter)
  **/
 void i40evf_del_vlans(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_vlan_filter_list *vvfl;
+	struct virtchnl_vlan_filter_list *vvfl;
 	struct i40evf_vlan_filter *f, *ftmp;
 	int len, i = 0, count = 0;
 	bool more = false;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot remove VLANs, command %d pending\n",
 			adapter->current_op);
@@ -609,16 +608,16 @@ void i40evf_del_vlans(struct i40evf_adapter *adapter)
 		adapter->aq_required &= ~I40EVF_FLAG_AQ_DEL_VLAN_FILTER;
 		return;
 	}
-	adapter->current_op = I40E_VIRTCHNL_OP_DEL_VLAN;
+	adapter->current_op = VIRTCHNL_OP_DEL_VLAN;
 
-	len = sizeof(struct i40e_virtchnl_vlan_filter_list) +
+	len = sizeof(struct virtchnl_vlan_filter_list) +
 	      (count * sizeof(u16));
 	if (len > I40EVF_MAX_AQ_BUF_SIZE) {
 		dev_warn(&adapter->pdev->dev, "Too many delete VLAN changes in one request\n");
 		count = (I40EVF_MAX_AQ_BUF_SIZE -
-			 sizeof(struct i40e_virtchnl_vlan_filter_list)) /
+			 sizeof(struct virtchnl_vlan_filter_list)) /
 			sizeof(u16);
-		len = sizeof(struct i40e_virtchnl_vlan_filter_list) +
+		len = sizeof(struct virtchnl_vlan_filter_list) +
 		      (count * sizeof(u16));
 		more = true;
 	}
@@ -640,7 +639,7 @@ void i40evf_del_vlans(struct i40evf_adapter *adapter)
 	}
 	if (!more)
 		adapter->aq_required &= ~I40EVF_FLAG_AQ_DEL_VLAN_FILTER;
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_DEL_VLAN, (u8 *)vvfl, len);
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_DEL_VLAN, (u8 *)vvfl, len);
 	kfree(vvfl);
 }
 
@@ -653,25 +652,25 @@ void i40evf_del_vlans(struct i40evf_adapter *adapter)
  **/
 void i40evf_set_promiscuous(struct i40evf_adapter *adapter, int flags)
 {
-	struct i40e_virtchnl_promisc_info vpi;
+	struct virtchnl_promisc_info vpi;
 	int promisc_all;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot set promiscuous mode, command %d pending\n",
 			adapter->current_op);
 		return;
 	}
 
-	promisc_all = I40E_FLAG_VF_UNICAST_PROMISC |
-		      I40E_FLAG_VF_MULTICAST_PROMISC;
+	promisc_all = FLAG_VF_UNICAST_PROMISC |
+		      FLAG_VF_MULTICAST_PROMISC;
 	if ((flags & promisc_all) == promisc_all) {
 		adapter->flags |= I40EVF_FLAG_PROMISC_ON;
 		adapter->aq_required &= ~I40EVF_FLAG_AQ_REQUEST_PROMISC;
 		dev_info(&adapter->pdev->dev, "Entering promiscuous mode\n");
 	}
 
-	if (flags & I40E_FLAG_VF_MULTICAST_PROMISC) {
+	if (flags & FLAG_VF_MULTICAST_PROMISC) {
 		adapter->flags |= I40EVF_FLAG_ALLMULTI_ON;
 		adapter->aq_required &= ~I40EVF_FLAG_AQ_REQUEST_ALLMULTI;
 		dev_info(&adapter->pdev->dev, "Entering multicast promiscuous mode\n");
@@ -683,10 +682,10 @@ void i40evf_set_promiscuous(struct i40evf_adapter *adapter, int flags)
 		dev_info(&adapter->pdev->dev, "Leaving promiscuous mode\n");
 	}
 
-	adapter->current_op = I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE;
+	adapter->current_op = VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE;
 	vpi.vsi_id = adapter->vsi_res->vsi_id;
 	vpi.flags = flags;
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE,
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_CONFIG_PROMISCUOUS_MODE,
 			   (u8 *)&vpi, sizeof(vpi));
 }
 
@@ -698,19 +697,19 @@ void i40evf_set_promiscuous(struct i40evf_adapter *adapter, int flags)
  **/
 void i40evf_request_stats(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_queue_select vqs;
+	struct virtchnl_queue_select vqs;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* no error message, this isn't crucial */
 		return;
 	}
-	adapter->current_op = I40E_VIRTCHNL_OP_GET_STATS;
+	adapter->current_op = VIRTCHNL_OP_GET_STATS;
 	vqs.vsi_id = adapter->vsi_res->vsi_id;
 	/* queue maps are ignored for this message - only the vsi is used */
-	if (i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_GET_STATS,
+	if (i40evf_send_pf_msg(adapter, VIRTCHNL_OP_GET_STATS,
 			       (u8 *)&vqs, sizeof(vqs)))
 		/* if the request failed, don't lock out others */
-		adapter->current_op = I40E_VIRTCHNL_OP_UNKNOWN;
+		adapter->current_op = VIRTCHNL_OP_UNKNOWN;
 }
 
 /**
@@ -721,15 +720,15 @@ void i40evf_request_stats(struct i40evf_adapter *adapter)
  **/
 void i40evf_get_hena(struct i40evf_adapter *adapter)
 {
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot get RSS hash capabilities, command %d pending\n",
 			adapter->current_op);
 		return;
 	}
-	adapter->current_op = I40E_VIRTCHNL_OP_GET_RSS_HENA_CAPS;
+	adapter->current_op = VIRTCHNL_OP_GET_RSS_HENA_CAPS;
 	adapter->aq_required &= ~I40EVF_FLAG_AQ_GET_HENA;
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_GET_RSS_HENA_CAPS,
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_GET_RSS_HENA_CAPS,
 			   NULL, 0);
 }
 
@@ -741,18 +740,18 @@ void i40evf_get_hena(struct i40evf_adapter *adapter)
  **/
 void i40evf_set_hena(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_rss_hena vrh;
+	struct virtchnl_rss_hena vrh;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot set RSS hash enable, command %d pending\n",
 			adapter->current_op);
 		return;
 	}
 	vrh.hena = adapter->hena;
-	adapter->current_op = I40E_VIRTCHNL_OP_SET_RSS_HENA;
+	adapter->current_op = VIRTCHNL_OP_SET_RSS_HENA;
 	adapter->aq_required &= ~I40EVF_FLAG_AQ_SET_HENA;
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_SET_RSS_HENA,
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_SET_RSS_HENA,
 			   (u8 *)&vrh, sizeof(vrh));
 }
 
@@ -764,16 +763,16 @@ void i40evf_set_hena(struct i40evf_adapter *adapter)
  **/
 void i40evf_set_rss_key(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_rss_key *vrk;
+	struct virtchnl_rss_key *vrk;
 	int len;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot set RSS key, command %d pending\n",
 			adapter->current_op);
 		return;
 	}
-	len = sizeof(struct i40e_virtchnl_rss_key) +
+	len = sizeof(struct virtchnl_rss_key) +
 	      (adapter->rss_key_size * sizeof(u8)) - 1;
 	vrk = kzalloc(len, GFP_KERNEL);
 	if (!vrk)
@@ -782,9 +781,9 @@ void i40evf_set_rss_key(struct i40evf_adapter *adapter)
 	vrk->key_len = adapter->rss_key_size;
 	memcpy(vrk->key, adapter->rss_key, adapter->rss_key_size);
 
-	adapter->current_op = I40E_VIRTCHNL_OP_CONFIG_RSS_KEY;
+	adapter->current_op = VIRTCHNL_OP_CONFIG_RSS_KEY;
 	adapter->aq_required &= ~I40EVF_FLAG_AQ_SET_RSS_KEY;
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_CONFIG_RSS_KEY,
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_CONFIG_RSS_KEY,
 			   (u8 *)vrk, len);
 	kfree(vrk);
 }
@@ -797,16 +796,16 @@ void i40evf_set_rss_key(struct i40evf_adapter *adapter)
  **/
 void i40evf_set_rss_lut(struct i40evf_adapter *adapter)
 {
-	struct i40e_virtchnl_rss_lut *vrl;
+	struct virtchnl_rss_lut *vrl;
 	int len;
 
-	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
+	if (adapter->current_op != VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
 		dev_err(&adapter->pdev->dev, "Cannot set RSS LUT, command %d pending\n",
 			adapter->current_op);
 		return;
 	}
-	len = sizeof(struct i40e_virtchnl_rss_lut) +
+	len = sizeof(struct virtchnl_rss_lut) +
 	      (adapter->rss_lut_size * sizeof(u8)) - 1;
 	vrl = kzalloc(len, GFP_KERNEL);
 	if (!vrl)
@@ -814,9 +813,9 @@ void i40evf_set_rss_lut(struct i40evf_adapter *adapter)
 	vrl->vsi_id = adapter->vsi.id;
 	vrl->lut_entries = adapter->rss_lut_size;
 	memcpy(vrl->lut, adapter->rss_lut, adapter->rss_lut_size);
-	adapter->current_op = I40E_VIRTCHNL_OP_CONFIG_RSS_LUT;
+	adapter->current_op = VIRTCHNL_OP_CONFIG_RSS_LUT;
 	adapter->aq_required &= ~I40EVF_FLAG_AQ_SET_RSS_LUT;
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_CONFIG_RSS_LUT,
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_CONFIG_RSS_LUT,
 			   (u8 *)vrl, len);
 	kfree(vrl);
 }
@@ -872,8 +871,8 @@ static void i40evf_print_link_message(struct i40evf_adapter *adapter)
 void i40evf_request_reset(struct i40evf_adapter *adapter)
 {
 	/* Don't check CURRENT_OP - this is always higher priority */
-	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_RESET_VF, NULL, 0);
-	adapter->current_op = I40E_VIRTCHNL_OP_UNKNOWN;
+	i40evf_send_pf_msg(adapter, VIRTCHNL_OP_RESET_VF, NULL, 0);
+	adapter->current_op = VIRTCHNL_OP_UNKNOWN;
 }
 
 /**
@@ -889,17 +888,17 @@ void i40evf_request_reset(struct i40evf_adapter *adapter)
  * This function handles the reply messages.
  **/
 void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
-				enum i40e_virtchnl_ops v_opcode,
+				enum virtchnl_ops v_opcode,
 				i40e_status v_retval,
 				u8 *msg, u16 msglen)
 {
 	struct net_device *netdev = adapter->netdev;
 
-	if (v_opcode == I40E_VIRTCHNL_OP_EVENT) {
-		struct i40e_virtchnl_pf_event *vpe =
-			(struct i40e_virtchnl_pf_event *)msg;
+	if (v_opcode == VIRTCHNL_OP_EVENT) {
+		struct virtchnl_pf_event *vpe =
+			(struct virtchnl_pf_event *)msg;
 		switch (vpe->event) {
-		case I40E_VIRTCHNL_EVENT_LINK_CHANGE:
+		case VIRTCHNL_EVENT_LINK_CHANGE:
 			adapter->link_speed =
 				vpe->event_data.link_event.link_speed;
 			if (adapter->link_up !=
@@ -916,7 +915,7 @@ void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
 				i40evf_print_link_message(adapter);
 			}
 			break;
-		case I40E_VIRTCHNL_EVENT_RESET_IMPENDING:
+		case VIRTCHNL_EVENT_RESET_IMPENDING:
 			dev_info(&adapter->pdev->dev, "PF reset warning received\n");
 			if (!(adapter->flags & I40EVF_FLAG_RESET_PENDING)) {
 				adapter->flags |= I40EVF_FLAG_RESET_PENDING;
@@ -933,19 +932,19 @@ void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
 	}
 	if (v_retval) {
 		switch (v_opcode) {
-		case I40E_VIRTCHNL_OP_ADD_VLAN:
+		case VIRTCHNL_OP_ADD_VLAN:
 			dev_err(&adapter->pdev->dev, "Failed to add VLAN filter, error %s\n",
 				i40evf_stat_str(&adapter->hw, v_retval));
 			break;
-		case I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS:
+		case VIRTCHNL_OP_ADD_ETH_ADDR:
 			dev_err(&adapter->pdev->dev, "Failed to add MAC filter, error %s\n",
 				i40evf_stat_str(&adapter->hw, v_retval));
 			break;
-		case I40E_VIRTCHNL_OP_DEL_VLAN:
+		case VIRTCHNL_OP_DEL_VLAN:
 			dev_err(&adapter->pdev->dev, "Failed to delete VLAN filter, error %s\n",
 				i40evf_stat_str(&adapter->hw, v_retval));
 			break;
-		case I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS:
+		case VIRTCHNL_OP_DEL_ETH_ADDR:
 			dev_err(&adapter->pdev->dev, "Failed to delete MAC filter, error %s\n",
 				i40evf_stat_str(&adapter->hw, v_retval));
 			break;
@@ -957,7 +956,7 @@ void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
 		}
 	}
 	switch (v_opcode) {
-	case I40E_VIRTCHNL_OP_GET_STATS: {
+	case VIRTCHNL_OP_GET_STATS: {
 		struct i40e_eth_stats *stats =
 			(struct i40e_eth_stats *)msg;
 		netdev->stats.rx_packets = stats->rx_unicast +
@@ -974,10 +973,10 @@ void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
 		adapter->current_stats = *stats;
 		}
 		break;
-	case I40E_VIRTCHNL_OP_GET_VF_RESOURCES: {
-		u16 len = sizeof(struct i40e_virtchnl_vf_resource) +
+	case VIRTCHNL_OP_GET_VF_RESOURCES: {
+		u16 len = sizeof(struct virtchnl_vf_resource) +
 			  I40E_MAX_VF_VSI *
-			  sizeof(struct i40e_virtchnl_vsi_resource);
+			  sizeof(struct virtchnl_vsi_resource);
 		memcpy(adapter->vf_res, msg, min(msglen, len));
 		i40e_vf_parse_hw_config(&adapter->hw, adapter->vf_res);
 		/* restore current mac address */
@@ -985,18 +984,18 @@ void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
 		i40evf_process_config(adapter);
 		}
 		break;
-	case I40E_VIRTCHNL_OP_ENABLE_QUEUES:
+	case VIRTCHNL_OP_ENABLE_QUEUES:
 		/* enable transmits */
 		i40evf_irq_enable(adapter, true);
 		break;
-	case I40E_VIRTCHNL_OP_DISABLE_QUEUES:
+	case VIRTCHNL_OP_DISABLE_QUEUES:
 		i40evf_free_all_tx_resources(adapter);
 		i40evf_free_all_rx_resources(adapter);
 		if (adapter->state == __I40EVF_DOWN_PENDING)
 			adapter->state = __I40EVF_DOWN;
 		break;
-	case I40E_VIRTCHNL_OP_VERSION:
-	case I40E_VIRTCHNL_OP_CONFIG_IRQ_MAP:
+	case VIRTCHNL_OP_VERSION:
+	case VIRTCHNL_OP_CONFIG_IRQ_MAP:
 		/* Don't display an error if we get these out of sequence.
 		 * If the firmware needed to get kicked, we'll get these and
 		 * it's no problem.
@@ -1004,7 +1003,7 @@ void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
 		if (v_opcode != adapter->current_op)
 			return;
 		break;
-	case I40E_VIRTCHNL_OP_IWARP:
+	case VIRTCHNL_OP_IWARP:
 		/* Gobble zero-length replies from the PF. They indicate that
 		 * a previous message was received OK, and the client doesn't
 		 * care about that.
@@ -1014,13 +1013,12 @@ void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
 						     msg, msglen);
 		break;
 
-	case I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP:
+	case VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP:
 		adapter->client_pending &=
-				~(BIT(I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP));
+				~(BIT(VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP));
 		break;
-	case I40E_VIRTCHNL_OP_GET_RSS_HENA_CAPS: {
-		struct i40e_virtchnl_rss_hena *vrh =
-			(struct i40e_virtchnl_rss_hena *)msg;
+	case VIRTCHNL_OP_GET_RSS_HENA_CAPS: {
+		struct virtchnl_rss_hena *vrh = (struct virtchnl_rss_hena *)msg;
 		if (msglen == sizeof(*vrh))
 			adapter->hena = vrh->hena;
 		else
@@ -1034,5 +1032,5 @@ void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
 				 adapter->current_op, v_opcode);
 		break;
 	} /* switch v_opcode */
-	adapter->current_op = I40E_VIRTCHNL_OP_UNKNOWN;
+	adapter->current_op = VIRTCHNL_OP_UNKNOWN;
 }
diff --git a/drivers/net/ethernet/intel/igb/e1000_82575.c b/drivers/net/ethernet/intel/igb/e1000_82575.c
index ee443985581f..4a50870e0fa7 100644
--- a/drivers/net/ethernet/intel/igb/e1000_82575.c
+++ b/drivers/net/ethernet/intel/igb/e1000_82575.c
@@ -257,6 +257,7 @@ static s32 igb_init_phy_params_82575(struct e1000_hw *hw)
 	}
 
 	/* Set phy->phy_addr and phy->id. */
+	igb_write_phy_reg_82580(hw, I347AT4_PAGE_SELECT, 0);
 	ret_val = igb_get_phy_id_82575(hw);
 	if (ret_val)
 		return ret_val;
diff --git a/drivers/net/ethernet/intel/igb/igb.h b/drivers/net/ethernet/intel/igb/igb.h
index bf9bf9056d0c..06ffb2bc713e 100644
--- a/drivers/net/ethernet/intel/igb/igb.h
+++ b/drivers/net/ethernet/intel/igb/igb.h
@@ -563,6 +563,7 @@ struct igb_adapter {
 	struct cyclecounter cc;
 	struct timecounter tc;
 	u32 tx_hwtstamp_timeouts;
+	u32 tx_hwtstamp_skipped;
 	u32 rx_hwtstamp_cleared;
 	bool pps_sys_wrap_on;
 
@@ -666,7 +667,7 @@ void igb_setup_tctl(struct igb_adapter *);
 void igb_setup_rctl(struct igb_adapter *);
 netdev_tx_t igb_xmit_frame_ring(struct sk_buff *, struct igb_ring *);
 void igb_alloc_rx_buffers(struct igb_ring *, u16);
-void igb_update_stats(struct igb_adapter *, struct rtnl_link_stats64 *);
+void igb_update_stats(struct igb_adapter *);
 bool igb_has_link(struct igb_adapter *adapter);
 void igb_set_ethtool_ops(struct net_device *);
 void igb_power_up_link(struct igb_adapter *);
@@ -676,6 +677,7 @@ void igb_ptp_stop(struct igb_adapter *adapter);
 void igb_ptp_reset(struct igb_adapter *adapter);
 void igb_ptp_suspend(struct igb_adapter *adapter);
 void igb_ptp_rx_hang(struct igb_adapter *adapter);
+void igb_ptp_tx_hang(struct igb_adapter *adapter);
 void igb_ptp_rx_rgtstamp(struct igb_q_vector *q_vector, struct sk_buff *skb);
 void igb_ptp_rx_pktstamp(struct igb_q_vector *q_vector, void *va,
 			 struct sk_buff *skb);
diff --git a/drivers/net/ethernet/intel/igb/igb_ethtool.c b/drivers/net/ethernet/intel/igb/igb_ethtool.c
index 0efb62db6efd..d06a8db514d4 100644
--- a/drivers/net/ethernet/intel/igb/igb_ethtool.c
+++ b/drivers/net/ethernet/intel/igb/igb_ethtool.c
@@ -90,6 +90,7 @@ static const struct igb_stats igb_gstrings_stats[] = {
 	IGB_STAT("os2bmc_tx_by_host", stats.o2bspc),
 	IGB_STAT("os2bmc_rx_by_host", stats.b2ogprc),
 	IGB_STAT("tx_hwtstamp_timeouts", tx_hwtstamp_timeouts),
+	IGB_STAT("tx_hwtstamp_skipped", tx_hwtstamp_skipped),
 	IGB_STAT("rx_hwtstamp_cleared", rx_hwtstamp_cleared),
 };
 
@@ -2315,7 +2316,7 @@ static void igb_get_ethtool_stats(struct net_device *netdev,
 	char *p;
 
 	spin_lock(&adapter->stats64_lock);
-	igb_update_stats(adapter, net_stats);
+	igb_update_stats(adapter);
 
 	for (i = 0; i < IGB_GLOBAL_STATS_LEN; i++) {
 		p = (char *)adapter + igb_gstrings_stats[i].stat_offset;
diff --git a/drivers/net/ethernet/intel/igb/igb_main.c b/drivers/net/ethernet/intel/igb/igb_main.c
index 1cf74aa4ebd9..ec62410b035a 100644
--- a/drivers/net/ethernet/intel/igb/igb_main.c
+++ b/drivers/net/ethernet/intel/igb/igb_main.c
@@ -191,10 +191,7 @@ static int igb_disable_sriov(struct pci_dev *dev);
 static int igb_pci_disable_sriov(struct pci_dev *dev);
 #endif
 
-#ifdef CONFIG_PM
-#ifdef CONFIG_PM_SLEEP
 static int igb_suspend(struct device *);
-#endif
 static int igb_resume(struct device *);
 static int igb_runtime_suspend(struct device *dev);
 static int igb_runtime_resume(struct device *dev);
@@ -204,7 +201,6 @@ static const struct dev_pm_ops igb_pm_ops = {
 	SET_RUNTIME_PM_OPS(igb_runtime_suspend, igb_runtime_resume,
 			igb_runtime_idle)
 };
-#endif
 static void igb_shutdown(struct pci_dev *);
 static int igb_pci_sriov_configure(struct pci_dev *dev, int num_vfs);
 #ifdef CONFIG_IGB_DCA
@@ -1822,7 +1818,7 @@ void igb_down(struct igb_adapter *adapter)
 
 	/* record the stats before reset*/
 	spin_lock(&adapter->stats64_lock);
-	igb_update_stats(adapter, &adapter->stats64);
+	igb_update_stats(adapter);
 	spin_unlock(&adapter->stats64_lock);
 
 	adapter->link_speed = 0;
@@ -4690,7 +4686,7 @@ no_wait:
 	}
 
 	spin_lock(&adapter->stats64_lock);
-	igb_update_stats(adapter, &adapter->stats64);
+	igb_update_stats(adapter);
 	spin_unlock(&adapter->stats64_lock);
 
 	for (i = 0; i < adapter->num_tx_queues; i++) {
@@ -4726,6 +4722,7 @@ no_wait:
 
 	igb_spoof_check(adapter);
 	igb_ptp_rx_hang(adapter);
+	igb_ptp_tx_hang(adapter);
 
 	/* Check LVMMC register on i350/i354 only */
 	if ((adapter->hw.mac.type == e1000_i350) ||
@@ -5201,9 +5198,9 @@ static inline int igb_maybe_stop_tx(struct igb_ring *tx_ring, const u16 size)
 	return __igb_maybe_stop_tx(tx_ring, size);
 }
 
-static void igb_tx_map(struct igb_ring *tx_ring,
-		       struct igb_tx_buffer *first,
-		       const u8 hdr_len)
+static int igb_tx_map(struct igb_ring *tx_ring,
+		      struct igb_tx_buffer *first,
+		      const u8 hdr_len)
 {
 	struct sk_buff *skb = first->skb;
 	struct igb_tx_buffer *tx_buffer;
@@ -5314,7 +5311,7 @@ static void igb_tx_map(struct igb_ring *tx_ring,
 		 */
 		mmiowb();
 	}
-	return;
+	return 0;
 
 dma_error:
 	dev_err(tx_ring->dev, "TX DMA map failed\n");
@@ -5345,6 +5342,8 @@ dma_error:
 	tx_buffer->skb = NULL;
 
 	tx_ring->next_to_use = i;
+
+	return -1;
 }
 
 netdev_tx_t igb_xmit_frame_ring(struct sk_buff *skb,
@@ -5390,6 +5389,8 @@ netdev_tx_t igb_xmit_frame_ring(struct sk_buff *skb,
 			adapter->ptp_tx_start = jiffies;
 			if (adapter->hw.mac.type == e1000_82576)
 				schedule_work(&adapter->ptp_tx_work);
+		} else {
+			adapter->tx_hwtstamp_skipped++;
 		}
 	}
 
@@ -5410,13 +5411,24 @@ netdev_tx_t igb_xmit_frame_ring(struct sk_buff *skb,
 	else if (!tso)
 		igb_tx_csum(tx_ring, first);
 
-	igb_tx_map(tx_ring, first, hdr_len);
+	if (igb_tx_map(tx_ring, first, hdr_len))
+		goto cleanup_tx_tstamp;
 
 	return NETDEV_TX_OK;
 
 out_drop:
 	dev_kfree_skb_any(first->skb);
 	first->skb = NULL;
+cleanup_tx_tstamp:
+	if (unlikely(tx_flags & IGB_TX_FLAGS_TSTAMP)) {
+		struct igb_adapter *adapter = netdev_priv(tx_ring->netdev);
+
+		dev_kfree_skb_any(adapter->ptp_tx_skb);
+		adapter->ptp_tx_skb = NULL;
+		if (adapter->hw.mac.type == e1000_82576)
+			cancel_work_sync(&adapter->ptp_tx_work);
+		clear_bit_unlock(__IGB_PTP_TX_IN_PROGRESS, &adapter->state);
+	}
 
 	return NETDEV_TX_OK;
 }
@@ -5487,7 +5499,7 @@ static void igb_get_stats64(struct net_device *netdev,
 	struct igb_adapter *adapter = netdev_priv(netdev);
 
 	spin_lock(&adapter->stats64_lock);
-	igb_update_stats(adapter, &adapter->stats64);
+	igb_update_stats(adapter);
 	memcpy(stats, &adapter->stats64, sizeof(*stats));
 	spin_unlock(&adapter->stats64_lock);
 }
@@ -5536,9 +5548,9 @@ static int igb_change_mtu(struct net_device *netdev, int new_mtu)
  *  igb_update_stats - Update the board statistics counters
  *  @adapter: board private structure
  **/
-void igb_update_stats(struct igb_adapter *adapter,
-		      struct rtnl_link_stats64 *net_stats)
+void igb_update_stats(struct igb_adapter *adapter)
 {
+	struct rtnl_link_stats64 *net_stats = &adapter->stats64;
 	struct e1000_hw *hw = &adapter->hw;
 	struct pci_dev *pdev = adapter->pdev;
 	u32 reg, mpc;
@@ -6457,8 +6469,8 @@ static void igb_set_default_mac_filter(struct igb_adapter *adapter)
 	igb_rar_set_index(adapter, 0);
 }
 
-int igb_add_mac_filter(struct igb_adapter *adapter, const u8 *addr,
-		       const u8 queue)
+static int igb_add_mac_filter(struct igb_adapter *adapter, const u8 *addr,
+			      const u8 queue)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	int rar_entries = hw->mac.rar_entry_count -
@@ -6487,8 +6499,8 @@ int igb_add_mac_filter(struct igb_adapter *adapter, const u8 *addr,
 	return -ENOSPC;
 }
 
-int igb_del_mac_filter(struct igb_adapter *adapter, const u8 *addr,
-		       const u8 queue)
+static int igb_del_mac_filter(struct igb_adapter *adapter, const u8 *addr,
+			      const u8 queue)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	int rar_entries = hw->mac.rar_entry_count -
@@ -6540,8 +6552,8 @@ static int igb_uc_unsync(struct net_device *netdev, const unsigned char *addr)
 	return 0;
 }
 
-int igb_set_vf_mac_filter(struct igb_adapter *adapter, const int vf,
-			  const u32 info, const u8 *addr)
+static int igb_set_vf_mac_filter(struct igb_adapter *adapter, const int vf,
+				 const u32 info, const u8 *addr)
 {
 	struct pci_dev *pdev = adapter->pdev;
 	struct vf_data_storage *vf_data = &adapter->vf_data[vf];
@@ -8015,9 +8027,7 @@ static void igb_deliver_wake_packet(struct net_device *netdev)
 	netif_rx(skb);
 }
 
-#ifdef CONFIG_PM
-#ifdef CONFIG_PM_SLEEP
-static int igb_suspend(struct device *dev)
+static int __maybe_unused igb_suspend(struct device *dev)
 {
 	int retval;
 	bool wake;
@@ -8036,9 +8046,8 @@ static int igb_suspend(struct device *dev)
 
 	return 0;
 }
-#endif /* CONFIG_PM_SLEEP */
 
-static int igb_resume(struct device *dev)
+static int __maybe_unused igb_resume(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct net_device *netdev = pci_get_drvdata(pdev);
@@ -8092,7 +8101,7 @@ static int igb_resume(struct device *dev)
 	return err;
 }
 
-static int igb_runtime_idle(struct device *dev)
+static int __maybe_unused igb_runtime_idle(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct net_device *netdev = pci_get_drvdata(pdev);
@@ -8104,7 +8113,7 @@ static int igb_runtime_idle(struct device *dev)
 	return -EBUSY;
 }
 
-static int igb_runtime_suspend(struct device *dev)
+static int __maybe_unused igb_runtime_suspend(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	int retval;
@@ -8124,11 +8133,10 @@ static int igb_runtime_suspend(struct device *dev)
 	return 0;
 }
 
-static int igb_runtime_resume(struct device *dev)
+static int __maybe_unused igb_runtime_resume(struct device *dev)
 {
 	return igb_resume(dev);
 }
-#endif /* CONFIG_PM */
 
 static void igb_shutdown(struct pci_dev *pdev)
 {
diff --git a/drivers/net/ethernet/intel/igb/igb_ptp.c b/drivers/net/ethernet/intel/igb/igb_ptp.c
index 7a3fd4d74592..841c2a083349 100644
--- a/drivers/net/ethernet/intel/igb/igb_ptp.c
+++ b/drivers/net/ethernet/intel/igb/igb_ptp.c
@@ -712,6 +712,35 @@ void igb_ptp_rx_hang(struct igb_adapter *adapter)
 }
 
 /**
+ * igb_ptp_tx_hang - detect error case where Tx timestamp never finishes
+ * @adapter: private network adapter structure
+ */
+void igb_ptp_tx_hang(struct igb_adapter *adapter)
+{
+	bool timeout = time_is_before_jiffies(adapter->ptp_tx_start +
+					      IGB_PTP_TX_TIMEOUT);
+
+	if (!adapter->ptp_tx_skb)
+		return;
+
+	if (!test_bit(__IGB_PTP_TX_IN_PROGRESS, &adapter->state))
+		return;
+
+	/* If we haven't received a timestamp within the timeout, it is
+	 * reasonable to assume that it will never occur, so we can unlock the
+	 * timestamp bit when this occurs.
+	 */
+	if (timeout) {
+		cancel_work_sync(&adapter->ptp_tx_work);
+		dev_kfree_skb_any(adapter->ptp_tx_skb);
+		adapter->ptp_tx_skb = NULL;
+		clear_bit_unlock(__IGB_PTP_TX_IN_PROGRESS, &adapter->state);
+		adapter->tx_hwtstamp_timeouts++;
+		dev_warn(&adapter->pdev->dev, "clearing Tx timestamp hang\n");
+	}
+}
+
+/**
  * igb_ptp_tx_hwtstamp - utility function which checks for TX time stamp
  * @adapter: Board private structure.
  *
@@ -721,6 +750,7 @@ void igb_ptp_rx_hang(struct igb_adapter *adapter)
  **/
 static void igb_ptp_tx_hwtstamp(struct igb_adapter *adapter)
 {
+	struct sk_buff *skb = adapter->ptp_tx_skb;
 	struct e1000_hw *hw = &adapter->hw;
 	struct skb_shared_hwtstamps shhwtstamps;
 	u64 regval;
@@ -748,10 +778,17 @@ static void igb_ptp_tx_hwtstamp(struct igb_adapter *adapter)
 	shhwtstamps.hwtstamp =
 		ktime_add_ns(shhwtstamps.hwtstamp, adjust);
 
-	skb_tstamp_tx(adapter->ptp_tx_skb, &shhwtstamps);
-	dev_kfree_skb_any(adapter->ptp_tx_skb);
+	/* Clear the lock early before calling skb_tstamp_tx so that
+	 * applications are not woken up before the lock bit is clear. We use
+	 * a copy of the skb pointer to ensure other threads can't change it
+	 * while we're notifying the stack.
+	 */
 	adapter->ptp_tx_skb = NULL;
 	clear_bit_unlock(__IGB_PTP_TX_IN_PROGRESS, &adapter->state);
+
+	/* Notify the stack and free the skb after we've unlocked */
+	skb_tstamp_tx(skb, &shhwtstamps);
+	dev_kfree_skb_any(skb);
 }
 
 /**
@@ -941,6 +978,7 @@ static int igb_ptp_set_timestamp_mode(struct igb_adapter *adapter,
 		is_l4 = true;
 		break;
 	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+	case HWTSTAMP_FILTER_NTP_ALL:
 	case HWTSTAMP_FILTER_ALL:
 		/* 82576 cannot timestamp all packets, which it needs to do to
 		 * support both V1 Sync and Delay_Req messages
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
index 76263762bea1..dd5578756ae0 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
@@ -733,6 +733,7 @@ struct ixgbe_adapter {
 	struct timecounter hw_tc;
 	u32 base_incval;
 	u32 tx_hwtstamp_timeouts;
+	u32 tx_hwtstamp_skipped;
 	u32 rx_hwtstamp_cleared;
 	void (*ptp_setup_sdp)(struct ixgbe_adapter *);
 
@@ -960,6 +961,7 @@ void ixgbe_ptp_suspend(struct ixgbe_adapter *adapter);
 void ixgbe_ptp_stop(struct ixgbe_adapter *adapter);
 void ixgbe_ptp_overflow_check(struct ixgbe_adapter *adapter);
 void ixgbe_ptp_rx_hang(struct ixgbe_adapter *adapter);
+void ixgbe_ptp_tx_hang(struct ixgbe_adapter *adapter);
 void ixgbe_ptp_rx_pktstamp(struct ixgbe_q_vector *, struct sk_buff *);
 void ixgbe_ptp_rx_rgtstamp(struct ixgbe_q_vector *, struct sk_buff *skb);
 static inline void ixgbe_ptp_rx_hwtstamp(struct ixgbe_ring *rx_ring,
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
index c8ac46049f34..d602637ccc40 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
@@ -1589,15 +1589,17 @@ s32 ixgbe_fdir_set_input_mask_82599(struct ixgbe_hw *hw,
 
 	switch (ntohs(input_mask->formatted.vlan_id) & 0xEFFF) {
 	case 0x0000:
-		/* mask VLAN ID, fall through to mask VLAN priority */
+		/* mask VLAN ID */
 		fdirm |= IXGBE_FDIRM_VLANID;
+		/* fall through */
 	case 0x0FFF:
 		/* mask VLAN priority */
 		fdirm |= IXGBE_FDIRM_VLANP;
 		break;
 	case 0xE000:
-		/* mask VLAN ID only, fall through */
+		/* mask VLAN ID only */
 		fdirm |= IXGBE_FDIRM_VLANID;
+		/* fall through */
 	case 0xEFFF:
 		/* no VLAN fields masked */
 		break;
@@ -1608,8 +1610,9 @@ s32 ixgbe_fdir_set_input_mask_82599(struct ixgbe_hw *hw,
 
 	switch (input_mask->formatted.flex_bytes & 0xFFFF) {
 	case 0x0000:
-		/* Mask Flex Bytes, fall through */
+		/* Mask Flex Bytes */
 		fdirm |= IXGBE_FDIRM_FLEX;
+		/* fall through */
 	case 0xFFFF:
 		break;
 	default:
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
index c38d50c1fcf7..4e35e7017f3d 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
@@ -155,7 +155,7 @@ s32 ixgbe_setup_fc_generic(struct ixgbe_hw *hw)
 		if (ret_val)
 			return ret_val;
 
-		/* only backplane uses autoc so fall though */
+		/* fall through - only backplane uses autoc */
 	case ixgbe_media_type_fiber:
 		reg = IXGBE_READ_REG(hw, IXGBE_PCS1GANA);
 
@@ -395,7 +395,8 @@ s32 ixgbe_init_hw_generic(struct ixgbe_hw *hw)
 	}
 
 	/* Initialize the LED link active for LED blink support */
-	hw->mac.ops.init_led_link_act(hw);
+	if (hw->mac.ops.init_led_link_act)
+		hw->mac.ops.init_led_link_act(hw);
 
 	return status;
 }
@@ -3548,7 +3549,7 @@ void ixgbe_set_rxpba_generic(struct ixgbe_hw *hw,
 		rxpktsize <<= IXGBE_RXPBSIZE_SHIFT;
 		for (; i < (num_pb / 2); i++)
 			IXGBE_WRITE_REG(hw, IXGBE_RXPBSIZE(i), rxpktsize);
-		/* Fall through to configure remaining packet buffers */
+		/* fall through - configure remaining packet buffers */
 	case (PBA_STRATEGY_EQUAL):
 		/* Divide the remaining Rx packet buffer evenly among the TCs */
 		rxpktsize = (pbsize / (num_pb - i)) << IXGBE_RXPBSIZE_SHIFT;
@@ -4120,15 +4121,6 @@ s32 ixgbe_setup_mac_link_multispeed_fiber(struct ixgbe_hw *hw,
 		speedcnt++;
 		highest_link_speed = IXGBE_LINK_SPEED_10GB_FULL;
 
-		/* If we already have link at this speed, just jump out */
-		status = hw->mac.ops.check_link(hw, &link_speed, &link_up,
-						false);
-		if (status)
-			return status;
-
-		if (link_speed == IXGBE_LINK_SPEED_10GB_FULL && link_up)
-			goto out;
-
 		/* Set the module link speed */
 		switch (hw->phy.media_type) {
 		case ixgbe_media_type_fiber:
@@ -4180,15 +4172,6 @@ s32 ixgbe_setup_mac_link_multispeed_fiber(struct ixgbe_hw *hw,
 		if (highest_link_speed == IXGBE_LINK_SPEED_UNKNOWN)
 			highest_link_speed = IXGBE_LINK_SPEED_1GB_FULL;
 
-		/* If we already have link at this speed, just jump out */
-		status = hw->mac.ops.check_link(hw, &link_speed, &link_up,
-						false);
-		if (status)
-			return status;
-
-		if (link_speed == IXGBE_LINK_SPEED_1GB_FULL && link_up)
-			goto out;
-
 		/* Set the module link speed */
 		switch (hw->phy.media_type) {
 		case ixgbe_media_type_fiber:
@@ -4295,4 +4278,23 @@ void ixgbe_set_soft_rate_select_speed(struct ixgbe_hw *hw,
 		hw_dbg(hw, "Failed to write Rx Rate Select RS0\n");
 		return;
 	}
+
+	/* Set RS1 */
+	status = hw->phy.ops.read_i2c_byte(hw, IXGBE_SFF_SFF_8472_ESCB,
+					   IXGBE_I2C_EEPROM_DEV_ADDR2,
+					   &eeprom_data);
+	if (status) {
+		hw_dbg(hw, "Failed to read Rx Rate Select RS1\n");
+		return;
+	}
+
+	eeprom_data = (eeprom_data & ~IXGBE_SFF_SOFT_RS_SELECT_MASK) | rs;
+
+	status = hw->phy.ops.write_i2c_byte(hw, IXGBE_SFF_SFF_8472_ESCB,
+					    IXGBE_I2C_EEPROM_DEV_ADDR2,
+					    eeprom_data);
+	if (status) {
+		hw_dbg(hw, "Failed to write Rx Rate Select RS1\n");
+		return;
+	}
 }
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
index 7e5e336d7dcc..72c565712a5f 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
@@ -111,6 +111,9 @@ static const struct ixgbe_stats ixgbe_gstrings_stats[] = {
 	{"os2bmc_tx_by_bmc", IXGBE_STAT(stats.b2ospc)},
 	{"os2bmc_tx_by_host", IXGBE_STAT(stats.o2bspc)},
 	{"os2bmc_rx_by_host", IXGBE_STAT(stats.b2ogprc)},
+	{"tx_hwtstamp_timeouts", IXGBE_STAT(tx_hwtstamp_timeouts)},
+	{"tx_hwtstamp_skipped", IXGBE_STAT(tx_hwtstamp_skipped)},
+	{"rx_hwtstamp_cleared", IXGBE_STAT(rx_hwtstamp_cleared)},
 #ifdef IXGBE_FCOE
 	{"fcoe_bad_fccrc", IXGBE_STAT(stats.fccrc)},
 	{"rx_fcoe_dropped", IXGBE_STAT(stats.fcoerpdc)},
@@ -1274,7 +1277,7 @@ static void ixgbe_get_strings(struct net_device *netdev, u32 stringset,
 			      u8 *data)
 {
 	char *p = (char *)data;
-	int i;
+	unsigned int i;
 
 	switch (stringset) {
 	case ETH_SS_TEST:
@@ -2254,6 +2257,9 @@ static int ixgbe_set_phys_id(struct net_device *netdev,
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
 
+	if (!hw->mac.ops.led_on || !hw->mac.ops.led_off)
+		return -EOPNOTSUPP;
+
 	switch (state) {
 	case ETHTOOL_ID_ACTIVE:
 		adapter->led_reg = IXGBE_READ_REG(hw, IXGBE_LEDCTL);
@@ -2665,6 +2671,7 @@ static int ixgbe_flowspec_to_flow_type(struct ethtool_rx_flow_spec *fsp,
 				*flow_type = IXGBE_ATR_FLOW_TYPE_IPV4;
 				break;
 			}
+			/* fall through */
 		default:
 			return 0;
 		}
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c
index 2a653ec954f5..a23c2b5411a0 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c
@@ -491,7 +491,7 @@ int ixgbe_fcoe_ddp(struct ixgbe_adapter *adapter,
 	if ((fh->fh_r_ctl == FC_RCTL_DD_SOL_DATA) &&
 	    (fctl & FC_FC_END_SEQ)) {
 		skb_linearize(skb);
-		crc = (struct fcoe_crc_eof *)skb_put(skb, sizeof(*crc));
+		crc = skb_put(skb, sizeof(*crc));
 		crc->fcoe_eof = FC_EOF_T;
 	}
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index d39cba214320..f1dbdf26d8e1 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
@@ -58,6 +58,7 @@
 #include <net/tc_act/tc_gact.h>
 #include <net/tc_act/tc_mirred.h>
 #include <net/vxlan.h>
+#include <net/mpls.h>
 
 #include "ixgbe.h"
 #include "ixgbe_common.h"
@@ -75,7 +76,7 @@ char ixgbe_default_device_descr[] =
 static char ixgbe_default_device_descr[] =
 			      "Intel(R) 10 Gigabit Network Connection";
 #endif
-#define DRV_VERSION "5.0.0-k"
+#define DRV_VERSION "5.1.0-k"
 const char ixgbe_driver_version[] = DRV_VERSION;
 static const char ixgbe_copyright[] =
 				"Copyright (c) 1999-2016 Intel Corporation.";
@@ -1451,7 +1452,7 @@ static int __ixgbe_notify_dca(struct device *dev, void *data)
 					IXGBE_DCA_CTRL_DCA_MODE_CB2);
 			break;
 		}
-		/* Fall Through since DCA is disabled. */
+		/* fall through - DCA is disabled. */
 	case DCA_PROVIDER_REMOVE:
 		if (adapter->flags & IXGBE_FLAG_DCA_ENABLED) {
 			dca_remove_requester(dev);
@@ -2232,6 +2233,7 @@ static struct sk_buff *ixgbe_run_xdp(struct ixgbe_adapter *adapter,
 		break;
 	default:
 		bpf_warn_invalid_xdp_action(act);
+		/* fallthrough */
 	case XDP_ABORTED:
 		trace_xdp_exception(rx_ring->netdev, xdp_prog, act);
 		/* fallthrough -- handle aborts by dropping packet */
@@ -2638,8 +2640,7 @@ static void ixgbe_check_overtemp_subtask(struct ixgbe_adapter *adapter)
 	if (test_bit(__IXGBE_DOWN, &adapter->state))
 		return;
 
-	if (!(adapter->flags2 & IXGBE_FLAG2_TEMP_SENSOR_CAPABLE) &&
-	    !(adapter->flags2 & IXGBE_FLAG2_TEMP_SENSOR_EVENT))
+	if (!(adapter->flags2 & IXGBE_FLAG2_TEMP_SENSOR_EVENT))
 		return;
 
 	adapter->flags2 &= ~IXGBE_FLAG2_TEMP_SENSOR_EVENT;
@@ -3105,23 +3106,23 @@ 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;
+	unsigned int ri = 0, ti = 0;
 	int vector, err;
-	int ri = 0, ti = 0;
 
 	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];
 
 		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++);
+			snprintf(q_vector->name, sizeof(q_vector->name),
+				 "%s-TxRx-%u", netdev->name, ri++);
 			ti++;
 		} else if (q_vector->rx.ring) {
-			snprintf(q_vector->name, sizeof(q_vector->name) - 1,
-				 "%s-%s-%d", netdev->name, "rx", ri++);
+			snprintf(q_vector->name, sizeof(q_vector->name),
+				 "%s-rx-%u", netdev->name, ri++);
 		} else if (q_vector->tx.ring) {
-			snprintf(q_vector->name, sizeof(q_vector->name) - 1,
-				 "%s-%s-%d", netdev->name, "tx", ti++);
+			snprintf(q_vector->name, sizeof(q_vector->name),
+				 "%s-tx-%u", netdev->name, ti++);
 		} else {
 			/* skip this unused q_vector */
 			continue;
@@ -3802,6 +3803,9 @@ static void ixgbe_setup_mrqc(struct ixgbe_adapter *adapter)
 				mrqc = IXGBE_MRQC_VMDQRSS32EN;
 			else
 				mrqc = IXGBE_MRQC_VMDQRSS64EN;
+
+			/* Enable L3/L4 for Tx Switched packets */
+			mrqc |= IXGBE_MRQC_L3L4TXSWEN;
 		} else {
 			if (tcs > 4)
 				mrqc = IXGBE_MRQC_RTRSS8TCEN;
@@ -4174,7 +4178,7 @@ static void ixgbe_setup_rdrxctl(struct ixgbe_adapter *adapter)
 	case ixgbe_mac_x550em_a:
 		if (adapter->num_vfs)
 			rdrxctl |= IXGBE_RDRXCTL_PSP;
-		/* fall through for older HW */
+		/* fall through */
 	case ixgbe_mac_82599EB:
 	case ixgbe_mac_X540:
 		/* Disable RSC for ACK packets */
@@ -6183,8 +6187,6 @@ int ixgbe_setup_tx_resources(struct ixgbe_ring *tx_ring)
 	if (!tx_ring->tx_buffer_info)
 		goto err;
 
-	u64_stats_init(&tx_ring->syncp);
-
 	/* round up to nearest 4K */
 	tx_ring->size = tx_ring->count * sizeof(union ixgbe_adv_tx_desc);
 	tx_ring->size = ALIGN(tx_ring->size, 4096);
@@ -6278,8 +6280,6 @@ int ixgbe_setup_rx_resources(struct ixgbe_adapter *adapter,
 	if (!rx_ring->rx_buffer_info)
 		goto err;
 
-	u64_stats_init(&rx_ring->syncp);
-
 	/* Round up to nearest 4K */
 	rx_ring->size = rx_ring->count * sizeof(union ixgbe_adv_rx_desc);
 	rx_ring->size = ALIGN(rx_ring->size, 4096);
@@ -6886,6 +6886,7 @@ void ixgbe_update_stats(struct ixgbe_adapter *adapter)
 		hwstats->o2bspc += IXGBE_READ_REG(hw, IXGBE_O2BSPC);
 		hwstats->b2ospc += IXGBE_READ_REG(hw, IXGBE_B2OSPC);
 		hwstats->b2ogprc += IXGBE_READ_REG(hw, IXGBE_B2OGPRC);
+		/* fall through */
 	case ixgbe_mac_82599EB:
 		for (i = 0; i < 16; i++)
 			adapter->hw_rx_no_dma_resources +=
@@ -7634,6 +7635,7 @@ static void ixgbe_service_task(struct work_struct *work)
 	if (test_bit(__IXGBE_PTP_RUNNING, &adapter->state)) {
 		ixgbe_ptp_overflow_check(adapter);
 		ixgbe_ptp_rx_hang(adapter);
+		ixgbe_ptp_tx_hang(adapter);
 	}
 
 	ixgbe_service_event_complete(adapter);
@@ -7667,7 +7669,10 @@ static int ixgbe_tso(struct ixgbe_ring *tx_ring,
 	if (err < 0)
 		return err;
 
-	ip.hdr = skb_network_header(skb);
+	if (eth_p_mpls(first->protocol))
+		ip.hdr = skb_inner_network_header(skb);
+	else
+		ip.hdr = skb_network_header(skb);
 	l4.hdr = skb_checksum_start(skb);
 
 	/* ADV DTYP TUCMD MKRLOC/ISCSIHEDLEN */
@@ -7871,9 +7876,9 @@ static inline int ixgbe_maybe_stop_tx(struct ixgbe_ring *tx_ring, u16 size)
 #define IXGBE_TXD_CMD (IXGBE_TXD_CMD_EOP | \
 		       IXGBE_TXD_CMD_RS)
 
-static void ixgbe_tx_map(struct ixgbe_ring *tx_ring,
-			 struct ixgbe_tx_buffer *first,
-			 const u8 hdr_len)
+static int ixgbe_tx_map(struct ixgbe_ring *tx_ring,
+			struct ixgbe_tx_buffer *first,
+			const u8 hdr_len)
 {
 	struct sk_buff *skb = first->skb;
 	struct ixgbe_tx_buffer *tx_buffer;
@@ -8000,7 +8005,7 @@ static void ixgbe_tx_map(struct ixgbe_ring *tx_ring,
 		mmiowb();
 	}
 
-	return;
+	return 0;
 dma_error:
 	dev_err(tx_ring->dev, "TX DMA map failed\n");
 	tx_buffer = &tx_ring->tx_buffer_info[i];
@@ -8030,6 +8035,8 @@ dma_error:
 	first->skb = NULL;
 
 	tx_ring->next_to_use = i;
+
+	return -1;
 }
 
 static void ixgbe_atr(struct ixgbe_ring *ring,
@@ -8205,6 +8212,7 @@ static u16 ixgbe_select_queue(struct net_device *dev, struct sk_buff *skb,
 
 		if (adapter->flags & IXGBE_FLAG_FCOE_ENABLED)
 			break;
+		/* fall through */
 	default:
 		return fallback(dev, skb);
 	}
@@ -8330,16 +8338,19 @@ netdev_tx_t ixgbe_xmit_frame_ring(struct sk_buff *skb,
 	protocol = vlan_get_protocol(skb);
 
 	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
-	    adapter->ptp_clock &&
-	    !test_and_set_bit_lock(__IXGBE_PTP_TX_IN_PROGRESS,
-				   &adapter->state)) {
-		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
-		tx_flags |= IXGBE_TX_FLAGS_TSTAMP;
-
-		/* schedule check for Tx timestamp */
-		adapter->ptp_tx_skb = skb_get(skb);
-		adapter->ptp_tx_start = jiffies;
-		schedule_work(&adapter->ptp_tx_work);
+	    adapter->ptp_clock) {
+		if (!test_and_set_bit_lock(__IXGBE_PTP_TX_IN_PROGRESS,
+					   &adapter->state)) {
+			skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+			tx_flags |= IXGBE_TX_FLAGS_TSTAMP;
+
+			/* schedule check for Tx timestamp */
+			adapter->ptp_tx_skb = skb_get(skb);
+			adapter->ptp_tx_start = jiffies;
+			schedule_work(&adapter->ptp_tx_work);
+		} else {
+			adapter->tx_hwtstamp_skipped++;
+		}
 	}
 
 	skb_tx_timestamp(skb);
@@ -8402,13 +8413,21 @@ netdev_tx_t ixgbe_xmit_frame_ring(struct sk_buff *skb,
 #ifdef IXGBE_FCOE
 xmit_fcoe:
 #endif /* IXGBE_FCOE */
-	ixgbe_tx_map(tx_ring, first, hdr_len);
+	if (ixgbe_tx_map(tx_ring, first, hdr_len))
+		goto cleanup_tx_timestamp;
 
 	return NETDEV_TX_OK;
 
 out_drop:
 	dev_kfree_skb_any(first->skb);
 	first->skb = NULL;
+cleanup_tx_timestamp:
+	if (unlikely(tx_flags & IXGBE_TX_FLAGS_TSTAMP)) {
+		dev_kfree_skb_any(adapter->ptp_tx_skb);
+		adapter->ptp_tx_skb = NULL;
+		cancel_work_sync(&adapter->ptp_tx_work);
+		clear_bit_unlock(__IXGBE_PTP_TX_IN_PROGRESS, &adapter->state);
+	}
 
 	return NETDEV_TX_OK;
 }
@@ -9195,11 +9214,14 @@ free_jump:
 	return err;
 }
 
-static int __ixgbe_setup_tc(struct net_device *dev, u32 handle, __be16 proto,
-			    struct tc_to_netdev *tc)
+static int __ixgbe_setup_tc(struct net_device *dev, u32 handle, u32 chain_index,
+			    __be16 proto, struct tc_to_netdev *tc)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(dev);
 
+	if (chain_index)
+		return -EOPNOTSUPP;
+
 	if (TC_H_MAJ(handle) == TC_H_MAJ(TC_H_INGRESS) &&
 	    tc->type == TC_SETUP_CLSU32) {
 		switch (tc->cls_u32->command) {
@@ -9793,6 +9815,8 @@ static int ixgbe_xdp(struct net_device *dev, struct netdev_xdp *xdp)
 		return ixgbe_xdp_setup(dev, xdp->prog);
 	case XDP_QUERY_PROG:
 		xdp->prog_attached = !!(adapter->xdp_prog);
+		xdp->prog_id = adapter->xdp_prog ?
+			adapter->xdp_prog->aux->id : 0;
 		return 0;
 	default:
 		return -EINVAL;
@@ -9929,6 +9953,7 @@ bool ixgbe_wol_supported(struct ixgbe_adapter *adapter, u16 device_id,
 			/* only support first port */
 			if (hw->bus.func != 0)
 				break;
+			/* fall through */
 		case IXGBE_SUBDEV_ID_82599_SP_560FLR:
 		case IXGBE_SUBDEV_ID_82599_SFP:
 		case IXGBE_SUBDEV_ID_82599_RNDC:
@@ -10191,7 +10216,11 @@ skip_sriov:
 
 	netdev->vlan_features |= netdev->features | NETIF_F_TSO_MANGLEID;
 	netdev->hw_enc_features |= netdev->vlan_features;
-	netdev->mpls_features |= NETIF_F_HW_CSUM;
+	netdev->mpls_features |= NETIF_F_SG |
+				 NETIF_F_TSO |
+				 NETIF_F_TSO6 |
+				 NETIF_F_HW_CSUM;
+	netdev->mpls_features |= IXGBE_GSO_PARTIAL_FEATURES;
 
 	/* set this bit last since it cannot be part of vlan_features */
 	netdev->features |= NETIF_F_HW_VLAN_CTAG_FILTER |
@@ -10275,6 +10304,10 @@ skip_sriov:
 	if (err)
 		goto err_sw_init;
 
+	for (i = 0; i < adapter->num_rx_queues; i++)
+		u64_stats_init(&adapter->rx_ring[i]->syncp);
+	for (i = 0; i < adapter->num_tx_queues; i++)
+		u64_stats_init(&adapter->tx_ring[i]->syncp);
 	for (i = 0; i < adapter->num_xdp_queues; i++)
 		u64_stats_init(&adapter->xdp_ring[i]->syncp);
 
@@ -10341,11 +10374,11 @@ skip_sriov:
 			   "hardware.\n");
 	}
 	strcpy(netdev->name, "eth%d");
+	pci_set_drvdata(pdev, adapter);
 	err = register_netdev(netdev);
 	if (err)
 		goto err_register;
 
-	pci_set_drvdata(pdev, adapter);
 
 	/* power down the optics for 82599 SFP+ fiber */
 	if (hw->mac.ops.disable_tx_laser)
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.h
index 5aa2c3cf7aec..b0cac961df3b 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.h
@@ -84,8 +84,9 @@
 #define IXGBE_CS4227_GLOBAL_ID_LSB		0
 #define IXGBE_CS4227_GLOBAL_ID_MSB		1
 #define IXGBE_CS4227_SCRATCH			2
-#define IXGBE_CS4223_PHY_ID			0x7003	/* Quad port */
-#define IXGBE_CS4227_PHY_ID			0x3003	/* Dual port */
+#define IXGBE_CS4227_EFUSE_PDF_SKU		0x19F
+#define IXGBE_CS4223_SKU_ID			0x0010  /* Quad port */
+#define IXGBE_CS4227_SKU_ID			0x0014  /* Dual port */
 #define IXGBE_CS4227_RESET_PENDING		0x1357
 #define IXGBE_CS4227_RESET_COMPLETE		0x5AA5
 #define IXGBE_CS4227_RETRIES			15
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
index ef0635e0918c..86d6924a2b71 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
@@ -663,6 +663,33 @@ static void ixgbe_ptp_clear_tx_timestamp(struct ixgbe_adapter *adapter)
 }
 
 /**
+ * ixgbe_ptp_tx_hang - detect error case where Tx timestamp never finishes
+ * @adapter: private network adapter structure
+ */
+void ixgbe_ptp_tx_hang(struct ixgbe_adapter *adapter)
+{
+	bool timeout = time_is_before_jiffies(adapter->ptp_tx_start +
+					      IXGBE_PTP_TX_TIMEOUT);
+
+	if (!adapter->ptp_tx_skb)
+		return;
+
+	if (!test_bit(__IXGBE_PTP_TX_IN_PROGRESS, &adapter->state))
+		return;
+
+	/* If we haven't received a timestamp within the timeout, it is
+	 * reasonable to assume that it will never occur, so we can unlock the
+	 * timestamp bit when this occurs.
+	 */
+	if (timeout) {
+		cancel_work_sync(&adapter->ptp_tx_work);
+		ixgbe_ptp_clear_tx_timestamp(adapter);
+		adapter->tx_hwtstamp_timeouts++;
+		e_warn(drv, "clearing Tx timestamp hang\n");
+	}
+}
+
+/**
  * ixgbe_ptp_tx_hwtstamp - utility function which checks for TX time stamp
  * @adapter: the private adapter struct
  *
@@ -672,17 +699,26 @@ static void ixgbe_ptp_clear_tx_timestamp(struct ixgbe_adapter *adapter)
  */
 static void ixgbe_ptp_tx_hwtstamp(struct ixgbe_adapter *adapter)
 {
+	struct sk_buff *skb = adapter->ptp_tx_skb;
 	struct ixgbe_hw *hw = &adapter->hw;
 	struct skb_shared_hwtstamps shhwtstamps;
 	u64 regval = 0;
 
 	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_TXSTMPL);
 	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_TXSTMPH) << 32;
-
 	ixgbe_ptp_convert_to_hwtstamp(adapter, &shhwtstamps, regval);
-	skb_tstamp_tx(adapter->ptp_tx_skb, &shhwtstamps);
 
-	ixgbe_ptp_clear_tx_timestamp(adapter);
+	/* Handle cleanup of the ptp_tx_skb ourselves, and unlock the state
+	 * bit prior to notifying the stack via skb_tstamp_tx(). This prevents
+	 * well behaved applications from attempting to timestamp again prior
+	 * to the lock bit being clear.
+	 */
+	adapter->ptp_tx_skb = NULL;
+	clear_bit_unlock(__IXGBE_PTP_TX_IN_PROGRESS, &adapter->state);
+
+	/* Notify the stack and then free the skb after we've unlocked */
+	skb_tstamp_tx(skb, &shhwtstamps);
+	dev_kfree_skb_any(skb);
 }
 
 /**
@@ -883,6 +919,7 @@ static int ixgbe_ptp_set_timestamp_mode(struct ixgbe_adapter *adapter,
 				   IXGBE_FLAG_RX_HWTSTAMP_IN_REGISTER);
 		break;
 	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+	case HWTSTAMP_FILTER_NTP_ALL:
 	case HWTSTAMP_FILTER_ALL:
 		/* The X550 controller is capable of timestamping all packets,
 		 * which allows it to accept any filter.
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
index 8baf298a8516..0760bd7eeb01 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
@@ -540,16 +540,15 @@ static s32 ixgbe_set_vf_lpe(struct ixgbe_adapter *adapter, u32 *msgbuf, u32 vf)
 		case ixgbe_mbox_api_11:
 		case ixgbe_mbox_api_12:
 		case ixgbe_mbox_api_13:
-			/*
-			 * Version 1.1 supports jumbo frames on VFs if PF has
+			/* Version 1.1 supports jumbo frames on VFs if PF has
 			 * jumbo frames enabled which means legacy VFs are
 			 * disabled
 			 */
 			if (pf_max_frame > ETH_FRAME_LEN)
 				break;
+			/* fall through */
 		default:
-			/*
-			 * If the PF or VF are running w/ jumbo frames enabled
+			/* If the PF or VF are running w/ jumbo frames enabled
 			 * we need to shut down the VF Rx path as we cannot
 			 * support jumbo frames on legacy VFs
 			 */
@@ -778,11 +777,17 @@ static inline void ixgbe_vf_reset_event(struct ixgbe_adapter *adapter, u32 vf)
 static int ixgbe_set_vf_mac(struct ixgbe_adapter *adapter,
 			    int vf, unsigned char *mac_addr)
 {
+	s32 retval;
+
 	ixgbe_del_mac_filter(adapter, adapter->vfinfo[vf].vf_mac_addresses, vf);
-	memcpy(adapter->vfinfo[vf].vf_mac_addresses, mac_addr, ETH_ALEN);
-	ixgbe_add_mac_filter(adapter, adapter->vfinfo[vf].vf_mac_addresses, vf);
+	retval = ixgbe_add_mac_filter(adapter, mac_addr, vf);
+	if (retval >= 0)
+		memcpy(adapter->vfinfo[vf].vf_mac_addresses, mac_addr,
+		       ETH_ALEN);
+	else
+		memset(adapter->vfinfo[vf].vf_mac_addresses, 0, ETH_ALEN);
 
-	return 0;
+	return retval;
 }
 
 int ixgbe_vf_configuration(struct pci_dev *pdev, unsigned int event_mask)
@@ -813,7 +818,7 @@ static inline void ixgbe_write_qde(struct ixgbe_adapter *adapter, u32 vf,
 		IXGBE_WRITE_FLUSH(hw);
 
 		/* indicate to hardware that we want to set drop enable */
-		reg = IXGBE_QDE_WRITE | IXGBE_QDE_ENABLE;
+		reg = IXGBE_QDE_WRITE | qde;
 		reg |= i <<  IXGBE_QDE_IDX_SHIFT;
 		IXGBE_WRITE_REG(hw, IXGBE_QDE, reg);
 	}
@@ -1347,27 +1352,49 @@ void ixgbe_ping_all_vfs(struct ixgbe_adapter *adapter)
 int ixgbe_ndo_set_vf_mac(struct net_device *netdev, int vf, u8 *mac)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
+	s32 retval;
 
 	if (vf >= adapter->num_vfs)
 		return -EINVAL;
 
-	if (is_zero_ether_addr(mac)) {
-		adapter->vfinfo[vf].pf_set_mac = false;
-		dev_info(&adapter->pdev->dev, "removing MAC on VF %d\n", vf);
-	} else if (is_valid_ether_addr(mac)) {
-		adapter->vfinfo[vf].pf_set_mac = true;
+	if (is_valid_ether_addr(mac)) {
 		dev_info(&adapter->pdev->dev, "setting MAC %pM on VF %d\n",
 			 mac, vf);
 		dev_info(&adapter->pdev->dev, "Reload the VF driver to make this change effective.");
-		if (test_bit(__IXGBE_DOWN, &adapter->state)) {
-			dev_warn(&adapter->pdev->dev, "The VF MAC address has been set, but the PF device is not up.\n");
-			dev_warn(&adapter->pdev->dev, "Bring the PF device up before attempting to use the VF device.\n");
+
+		retval = ixgbe_set_vf_mac(adapter, vf, mac);
+		if (retval >= 0) {
+			adapter->vfinfo[vf].pf_set_mac = true;
+
+			if (test_bit(__IXGBE_DOWN, &adapter->state)) {
+				dev_warn(&adapter->pdev->dev, "The VF MAC address has been set, but the PF device is not up.\n");
+				dev_warn(&adapter->pdev->dev, "Bring the PF device up before attempting to use the VF device.\n");
+			}
+		} else {
+			dev_warn(&adapter->pdev->dev, "The VF MAC address was NOT set due to invalid or duplicate MAC address.\n");
+		}
+	} else if (is_zero_ether_addr(mac)) {
+		unsigned char *vf_mac_addr =
+					   adapter->vfinfo[vf].vf_mac_addresses;
+
+		/* nothing to do */
+		if (is_zero_ether_addr(vf_mac_addr))
+			return 0;
+
+		dev_info(&adapter->pdev->dev, "removing MAC on VF %d\n", vf);
+
+		retval = ixgbe_del_mac_filter(adapter, vf_mac_addr, vf);
+		if (retval >= 0) {
+			adapter->vfinfo[vf].pf_set_mac = false;
+			memcpy(vf_mac_addr, mac, ETH_ALEN);
+		} else {
+			dev_warn(&adapter->pdev->dev, "Could NOT remove the VF MAC address.\n");
 		}
 	} else {
-		return -EINVAL;
+		retval = -EINVAL;
 	}
 
-	return ixgbe_set_vf_mac(adapter, vf, mac);
+	return retval;
 }
 
 static int ixgbe_enable_port_vlan(struct ixgbe_adapter *adapter, int vf,
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
index 2ba024b575ea..72d84a065e34 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
@@ -1750,14 +1750,14 @@ ixgbe_setup_mac_link_sfp_n(struct ixgbe_hw *hw, ixgbe_link_speed speed,
 	if (ret_val == IXGBE_ERR_SFP_NOT_PRESENT)
 		return 0;
 
-	if (!ret_val)
+	if (ret_val)
 		return ret_val;
 
 	/* Configure internal PHY for native SFI based on module type */
 	ret_val = hw->mac.ops.read_iosf_sb_reg(hw,
 				IXGBE_KRM_PMD_FLX_MASK_ST20(hw->bus.lan_id),
 				IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_phy_int);
-	if (!ret_val)
+	if (ret_val)
 		return ret_val;
 
 	reg_phy_int &= IXGBE_KRM_PMD_FLX_MASK_ST20_SFI_10G_DA;
@@ -1767,7 +1767,7 @@ ixgbe_setup_mac_link_sfp_n(struct ixgbe_hw *hw, ixgbe_link_speed speed,
 	ret_val = hw->mac.ops.write_iosf_sb_reg(hw,
 				IXGBE_KRM_PMD_FLX_MASK_ST20(hw->bus.lan_id),
 				IXGBE_SB_IOSF_TARGET_KR_PHY, reg_phy_int);
-	if (!ret_val)
+	if (ret_val)
 		return ret_val;
 
 	/* Setup SFI internal link. */
@@ -1798,7 +1798,7 @@ ixgbe_setup_mac_link_sfp_x550a(struct ixgbe_hw *hw, ixgbe_link_speed speed,
 	if (ret_val == IXGBE_ERR_SFP_NOT_PRESENT)
 		return 0;
 
-	if (!ret_val)
+	if (ret_val)
 		return ret_val;
 
 	/* Configure internal PHY for KR/KX. */
@@ -1807,16 +1807,16 @@ ixgbe_setup_mac_link_sfp_x550a(struct ixgbe_hw *hw, ixgbe_link_speed speed,
 	if (hw->phy.mdio.prtad == MDIO_PRTAD_NONE)
 		return IXGBE_ERR_PHY_ADDR_INVALID;
 
-	/* Get external PHY device id */
-	ret_val = hw->phy.ops.read_reg(hw, IXGBE_CS4227_GLOBAL_ID_MSB,
-				  IXGBE_MDIO_ZERO_DEV_TYPE, &reg_phy_ext);
+	/* Get external PHY SKU id */
+	ret_val = hw->phy.ops.read_reg(hw, IXGBE_CS4227_EFUSE_PDF_SKU,
+				       IXGBE_MDIO_ZERO_DEV_TYPE, &reg_phy_ext);
 	if (ret_val)
 		return ret_val;
 
 	/* When configuring quad port CS4223, the MAC instance is part
 	 * of the slice offset.
 	 */
-	if (reg_phy_ext == IXGBE_CS4223_PHY_ID)
+	if (reg_phy_ext == IXGBE_CS4223_SKU_ID)
 		slice_offset = (hw->bus.lan_id +
 				(hw->bus.instance_id << 1)) << 12;
 	else
@@ -1824,12 +1824,28 @@ ixgbe_setup_mac_link_sfp_x550a(struct ixgbe_hw *hw, ixgbe_link_speed speed,
 
 	/* Configure CS4227/CS4223 LINE side to proper mode. */
 	reg_slice = IXGBE_CS4227_LINE_SPARE24_LSB + slice_offset;
+
+	ret_val = hw->phy.ops.read_reg(hw, reg_slice,
+				       IXGBE_MDIO_ZERO_DEV_TYPE, &reg_phy_ext);
+	if (ret_val)
+		return ret_val;
+
+	reg_phy_ext &= ~((IXGBE_CS4227_EDC_MODE_CX1 << 1) |
+			 (IXGBE_CS4227_EDC_MODE_SR << 1));
+
 	if (setup_linear)
 		reg_phy_ext = (IXGBE_CS4227_EDC_MODE_CX1 << 1) | 1;
 	else
 		reg_phy_ext = (IXGBE_CS4227_EDC_MODE_SR << 1) | 1;
-	return hw->phy.ops.write_reg(hw, reg_slice, IXGBE_MDIO_ZERO_DEV_TYPE,
-				     reg_phy_ext);
+
+	ret_val = hw->phy.ops.write_reg(hw, reg_slice,
+					IXGBE_MDIO_ZERO_DEV_TYPE, reg_phy_ext);
+	if (ret_val)
+		return ret_val;
+
+	/* Flush previous write with a read */
+	return hw->phy.ops.read_reg(hw, reg_slice,
+				    IXGBE_MDIO_ZERO_DEV_TYPE, &reg_phy_ext);
 }
 
 /**
@@ -3206,6 +3222,7 @@ static s32 ixgbe_init_phy_ops_X550em(struct ixgbe_hw *hw)
 		phy->ops.setup_link = NULL;
 		phy->ops.read_reg = NULL;
 		phy->ops.write_reg = NULL;
+		phy->ops.reset = NULL;
 		break;
 	default:
 		break;
@@ -3819,6 +3836,28 @@ static const struct ixgbe_mac_operations mac_ops_X550EM_x = {
 	.write_iosf_sb_reg	= ixgbe_write_iosf_sb_reg_x550,
 };
 
+static const struct ixgbe_mac_operations mac_ops_X550EM_x_fw = {
+	X550_COMMON_MAC
+	.led_on			= NULL,
+	.led_off		= NULL,
+	.init_led_link_act	= NULL,
+	.reset_hw		= &ixgbe_reset_hw_X550em,
+	.get_media_type		= &ixgbe_get_media_type_X550em,
+	.get_san_mac_addr	= NULL,
+	.get_wwn_prefix		= NULL,
+	.setup_link		= &ixgbe_setup_mac_link_X540,
+	.get_link_capabilities	= &ixgbe_get_link_capabilities_X550em,
+	.get_bus_info		= &ixgbe_get_bus_info_X550em,
+	.setup_sfp		= ixgbe_setup_sfp_modules_X550em,
+	.acquire_swfw_sync	= &ixgbe_acquire_swfw_sync_X550em,
+	.release_swfw_sync	= &ixgbe_release_swfw_sync_X550em,
+	.init_swfw_sync		= &ixgbe_init_swfw_sync_X540,
+	.setup_fc		= NULL,
+	.fc_autoneg		= ixgbe_fc_autoneg,
+	.read_iosf_sb_reg	= ixgbe_read_iosf_sb_reg_x550,
+	.write_iosf_sb_reg	= ixgbe_write_iosf_sb_reg_x550,
+};
+
 static struct ixgbe_mac_operations mac_ops_x550em_a = {
 	X550_COMMON_MAC
 	.led_on			= ixgbe_led_on_t_x550em,
@@ -3986,7 +4025,7 @@ const struct ixgbe_info ixgbe_X550EM_x_info = {
 const struct ixgbe_info ixgbe_x550em_x_fw_info = {
 	.mac			= ixgbe_mac_X550EM_x,
 	.get_invariants		= ixgbe_get_invariants_X550_x_fw,
-	.mac_ops		= &mac_ops_X550EM_x,
+	.mac_ops		= &mac_ops_X550EM_x_fw,
 	.eeprom_ops		= &eeprom_ops_X550EM_x,
 	.phy_ops		= &phy_ops_x550em_x_fw,
 	.mbx_ops		= &mbx_ops_generic,
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
index eee29bddddc1..084c53582793 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
@@ -49,6 +49,7 @@
 #include <linux/if.h>
 #include <linux/if_vlan.h>
 #include <linux/prefetch.h>
+#include <net/mpls.h>
 
 #include "ixgbevf.h"
 
@@ -56,7 +57,7 @@ const char ixgbevf_driver_name[] = "ixgbevf";
 static const char ixgbevf_driver_string[] =
 	"Intel(R) 10 Gigabit PCI Express Virtual Function Network Driver";
 
-#define DRV_VERSION "3.2.2-k"
+#define DRV_VERSION "4.1.0-k"
 const char ixgbevf_driver_version[] = DRV_VERSION;
 static char ixgbevf_copyright[] =
 	"Copyright (c) 2009 - 2015 Intel Corporation.";
@@ -1350,23 +1351,23 @@ static int ixgbevf_request_msix_irqs(struct ixgbevf_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
 	int q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
+	unsigned int ri = 0, ti = 0;
 	int vector, err;
-	int ri = 0, ti = 0;
 
 	for (vector = 0; vector < q_vectors; 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++);
+			snprintf(q_vector->name, sizeof(q_vector->name),
+				 "%s-TxRx-%u", netdev->name, ri++);
 			ti++;
 		} else if (q_vector->rx.ring) {
-			snprintf(q_vector->name, sizeof(q_vector->name) - 1,
-				 "%s-%s-%d", netdev->name, "rx", ri++);
+			snprintf(q_vector->name, sizeof(q_vector->name),
+				 "%s-rx-%u", netdev->name, ri++);
 		} else if (q_vector->tx.ring) {
-			snprintf(q_vector->name, sizeof(q_vector->name) - 1,
-				 "%s-%s-%d", netdev->name, "tx", ti++);
+			snprintf(q_vector->name, sizeof(q_vector->name),
+				 "%s-tx-%u", netdev->name, ti++);
 		} else {
 			/* skip this unused q_vector */
 			continue;
@@ -3321,7 +3322,10 @@ static int ixgbevf_tso(struct ixgbevf_ring *tx_ring,
 	if (err < 0)
 		return err;
 
-	ip.hdr = skb_network_header(skb);
+	if (eth_p_mpls(first->protocol))
+		ip.hdr = skb_inner_network_header(skb);
+	else
+		ip.hdr = skb_network_header(skb);
 	l4.hdr = skb_checksum_start(skb);
 
 	/* ADV DTYP TUCMD MKRLOC/ISCSIHEDLEN */
@@ -4075,7 +4079,11 @@ static int ixgbevf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		netdev->features |= NETIF_F_HIGHDMA;
 
 	netdev->vlan_features |= netdev->features | NETIF_F_TSO_MANGLEID;
-	netdev->mpls_features |= NETIF_F_HW_CSUM;
+	netdev->mpls_features |= NETIF_F_SG |
+				 NETIF_F_TSO |
+				 NETIF_F_TSO6 |
+				 NETIF_F_HW_CSUM;
+	netdev->mpls_features |= IXGBEVF_GSO_PARTIAL_FEATURES;
 	netdev->hw_enc_features |= netdev->vlan_features;
 
 	/* set this bit last since it cannot be part of vlan_features */
diff --git a/drivers/net/ethernet/intel/ixgbevf/vf.c b/drivers/net/ethernet/intel/ixgbevf/vf.c
index b6d0c01eab10..0c25006ce9af 100644
--- a/drivers/net/ethernet/intel/ixgbevf/vf.c
+++ b/drivers/net/ethernet/intel/ixgbevf/vf.c
@@ -335,6 +335,7 @@ int ixgbevf_get_reta_locked(struct ixgbe_hw *hw, u32 *reta, int num_rx_queues)
 	case ixgbe_mbox_api_12:
 		if (hw->mac.type < ixgbe_mac_X550_vf)
 			break;
+		/* fall through */
 	default:
 		return -EOPNOTSUPP;
 	}
@@ -401,6 +402,7 @@ int ixgbevf_get_rss_key_locked(struct ixgbe_hw *hw, u8 *rss_key)
 	case ixgbe_mbox_api_12:
 		if (hw->mac.type < ixgbe_mac_X550_vf)
 			break;
+		/* fall through */
 	default:
 		return -EOPNOTSUPP;
 	}
diff --git a/drivers/net/ethernet/jme.c b/drivers/net/ethernet/jme.c
index f580b49e6b67..62d848df26ef 100644
--- a/drivers/net/ethernet/jme.c
+++ b/drivers/net/ethernet/jme.c
@@ -691,17 +691,6 @@ jme_enable_tx_engine(struct jme_adapter *jme)
 }
 
 static inline void
-jme_restart_tx_engine(struct jme_adapter *jme)
-{
-	/*
-	 * Restart TX Engine
-	 */
-	jwrite32(jme, JME_TXCS, jme->reg_txcs |
-				TXCS_SELECT_QUEUE0 |
-				TXCS_ENABLE);
-}
-
-static inline void
 jme_disable_tx_engine(struct jme_adapter *jme)
 {
 	int i;
@@ -2382,37 +2371,6 @@ jme_tx_timeout(struct net_device *netdev)
 	jme_reset_link(jme);
 }
 
-static inline void jme_pause_rx(struct jme_adapter *jme)
-{
-	atomic_dec(&jme->link_changing);
-
-	jme_set_rx_pcc(jme, PCC_OFF);
-	if (test_bit(JME_FLAG_POLL, &jme->flags)) {
-		JME_NAPI_DISABLE(jme);
-	} else {
-		tasklet_disable(&jme->rxclean_task);
-		tasklet_disable(&jme->rxempty_task);
-	}
-}
-
-static inline void jme_resume_rx(struct jme_adapter *jme)
-{
-	struct dynpcc_info *dpi = &(jme->dpi);
-
-	if (test_bit(JME_FLAG_POLL, &jme->flags)) {
-		JME_NAPI_ENABLE(jme);
-	} else {
-		tasklet_enable(&jme->rxclean_task);
-		tasklet_enable(&jme->rxempty_task);
-	}
-	dpi->cur		= PCC_P1;
-	dpi->attempt		= PCC_P1;
-	dpi->cnt		= 0;
-	jme_set_rx_pcc(jme, PCC_P1);
-
-	atomic_inc(&jme->link_changing);
-}
-
 static void
 jme_get_drvinfo(struct net_device *netdev,
 		     struct ethtool_drvinfo *info)
@@ -2652,12 +2610,11 @@ jme_get_link_ksettings(struct net_device *netdev,
 		       struct ethtool_link_ksettings *cmd)
 {
 	struct jme_adapter *jme = netdev_priv(netdev);
-	int rc;
 
 	spin_lock_bh(&jme->phy_lock);
-	rc = mii_ethtool_get_link_ksettings(&jme->mii_if, cmd);
+	mii_ethtool_get_link_ksettings(&jme->mii_if, cmd);
 	spin_unlock_bh(&jme->phy_lock);
-	return rc;
+	return 0;
 }
 
 static int
diff --git a/drivers/net/ethernet/korina.c b/drivers/net/ethernet/korina.c
index 9fae98caf83a..3c0a6451273d 100644
--- a/drivers/net/ethernet/korina.c
+++ b/drivers/net/ethernet/korina.c
@@ -699,13 +699,12 @@ static int netdev_get_link_ksettings(struct net_device *dev,
 				     struct ethtool_link_ksettings *cmd)
 {
 	struct korina_private *lp = netdev_priv(dev);
-	int rc;
 
 	spin_lock_irq(&lp->lock);
-	rc = mii_ethtool_get_link_ksettings(&lp->mii_if, cmd);
+	mii_ethtool_get_link_ksettings(&lp->mii_if, cmd);
 	spin_unlock_irq(&lp->lock);
 
-	return rc;
+	return 0;
 }
 
 static int netdev_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/marvell/mv643xx_eth.c b/drivers/net/ethernet/marvell/mv643xx_eth.c
index 25642dee49d3..5794d98d946f 100644
--- a/drivers/net/ethernet/marvell/mv643xx_eth.c
+++ b/drivers/net/ethernet/marvell/mv643xx_eth.c
@@ -1501,10 +1501,9 @@ mv643xx_eth_get_link_ksettings_phy(struct mv643xx_eth_private *mp,
 				   struct ethtool_link_ksettings *cmd)
 {
 	struct net_device *dev = mp->dev;
-	int err;
 	u32 supported, advertising;
 
-	err = phy_ethtool_ksettings_get(dev->phydev, cmd);
+	phy_ethtool_ksettings_get(dev->phydev, cmd);
 
 	/*
 	 * The MAC does not support 1000baseT_Half.
@@ -1520,7 +1519,7 @@ mv643xx_eth_get_link_ksettings_phy(struct mv643xx_eth_private *mp,
 	ethtool_convert_legacy_u32_to_link_mode(cmd->link_modes.advertising,
 						advertising);
 
-	return err;
+	return 0;
 }
 
 static int
diff --git a/drivers/net/ethernet/marvell/mvmdio.c b/drivers/net/ethernet/marvell/mvmdio.c
index 90a60b98c28e..c9798210fa0f 100644
--- a/drivers/net/ethernet/marvell/mvmdio.c
+++ b/drivers/net/ethernet/marvell/mvmdio.c
@@ -17,41 +17,52 @@
  * warranty of any kind, whether express or implied.
  */
 
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/mutex.h>
+#include <linux/of_device.h>
+#include <linux/of_mdio.h>
 #include <linux/phy.h>
-#include <linux/interrupt.h>
 #include <linux/platform_device.h>
-#include <linux/delay.h>
-#include <linux/io.h>
-#include <linux/clk.h>
-#include <linux/of_mdio.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
 
-#define MVMDIO_SMI_DATA_SHIFT              0
-#define MVMDIO_SMI_PHY_ADDR_SHIFT          16
-#define MVMDIO_SMI_PHY_REG_SHIFT           21
-#define MVMDIO_SMI_READ_OPERATION          BIT(26)
-#define MVMDIO_SMI_WRITE_OPERATION         0
-#define MVMDIO_SMI_READ_VALID              BIT(27)
-#define MVMDIO_SMI_BUSY                    BIT(28)
-#define MVMDIO_ERR_INT_CAUSE		   0x007C
-#define  MVMDIO_ERR_INT_SMI_DONE	   0x00000010
-#define MVMDIO_ERR_INT_MASK		   0x0080
+#define MVMDIO_SMI_DATA_SHIFT		0
+#define MVMDIO_SMI_PHY_ADDR_SHIFT	16
+#define MVMDIO_SMI_PHY_REG_SHIFT	21
+#define MVMDIO_SMI_READ_OPERATION	BIT(26)
+#define MVMDIO_SMI_WRITE_OPERATION	0
+#define MVMDIO_SMI_READ_VALID		BIT(27)
+#define MVMDIO_SMI_BUSY			BIT(28)
+#define MVMDIO_ERR_INT_CAUSE		0x007C
+#define  MVMDIO_ERR_INT_SMI_DONE	0x00000010
+#define MVMDIO_ERR_INT_MASK		0x0080
+
+#define MVMDIO_XSMI_MGNT_REG		0x0
+#define  MVMDIO_XSMI_PHYADDR_SHIFT	16
+#define  MVMDIO_XSMI_DEVADDR_SHIFT	21
+#define  MVMDIO_XSMI_WRITE_OPERATION	(0x5 << 26)
+#define  MVMDIO_XSMI_READ_OPERATION	(0x7 << 26)
+#define  MVMDIO_XSMI_READ_VALID		BIT(29)
+#define  MVMDIO_XSMI_BUSY		BIT(30)
+#define MVMDIO_XSMI_ADDR_REG		0x8
 
 /*
  * SMI Timeout measurements:
  * - Kirkwood 88F6281 (Globalscale Dreamplug): 45us to 95us (Interrupt)
  * - Armada 370       (Globalscale Mirabox):   41us to 43us (Polled)
  */
-#define MVMDIO_SMI_TIMEOUT		   1000 /* 1000us = 1ms */
-#define MVMDIO_SMI_POLL_INTERVAL_MIN	   45
-#define MVMDIO_SMI_POLL_INTERVAL_MAX	   55
+#define MVMDIO_SMI_TIMEOUT		1000 /* 1000us = 1ms */
+#define MVMDIO_SMI_POLL_INTERVAL_MIN	45
+#define MVMDIO_SMI_POLL_INTERVAL_MAX	55
+
+#define MVMDIO_XSMI_POLL_INTERVAL_MIN	150
+#define MVMDIO_XSMI_POLL_INTERVAL_MAX	160
 
 struct orion_mdio_dev {
-	struct mutex lock;
 	void __iomem *regs;
 	struct clk *clk[3];
 	/*
@@ -64,14 +75,21 @@ struct orion_mdio_dev {
 	wait_queue_head_t smi_busy_wait;
 };
 
-static int orion_mdio_smi_is_done(struct orion_mdio_dev *dev)
-{
-	return !(readl(dev->regs) & MVMDIO_SMI_BUSY);
-}
+enum orion_mdio_bus_type {
+	BUS_TYPE_SMI,
+	BUS_TYPE_XSMI
+};
+
+struct orion_mdio_ops {
+	int (*is_done)(struct orion_mdio_dev *);
+	unsigned int poll_interval_min;
+	unsigned int poll_interval_max;
+};
 
 /* Wait for the SMI unit to be ready for another operation
  */
-static int orion_mdio_wait_ready(struct mii_bus *bus)
+static int orion_mdio_wait_ready(const struct orion_mdio_ops *ops,
+				 struct mii_bus *bus)
 {
 	struct orion_mdio_dev *dev = bus->priv;
 	unsigned long timeout = usecs_to_jiffies(MVMDIO_SMI_TIMEOUT);
@@ -79,14 +97,14 @@ static int orion_mdio_wait_ready(struct mii_bus *bus)
 	int timedout = 0;
 
 	while (1) {
-	        if (orion_mdio_smi_is_done(dev))
+	        if (ops->is_done(dev))
 			return 0;
 	        else if (timedout)
 			break;
 
 	        if (dev->err_interrupt <= 0) {
-			usleep_range(MVMDIO_SMI_POLL_INTERVAL_MIN,
-				     MVMDIO_SMI_POLL_INTERVAL_MAX);
+			usleep_range(ops->poll_interval_min,
+				     ops->poll_interval_max);
 
 			if (time_is_before_jiffies(end))
 				++timedout;
@@ -98,8 +116,7 @@ static int orion_mdio_wait_ready(struct mii_bus *bus)
 			if (timeout < 2)
 				timeout = 2;
 			wait_event_timeout(dev->smi_busy_wait,
-				           orion_mdio_smi_is_done(dev),
-				           timeout);
+				           ops->is_done(dev), timeout);
 
 			++timedout;
 	        }
@@ -109,52 +126,61 @@ static int orion_mdio_wait_ready(struct mii_bus *bus)
 	return  -ETIMEDOUT;
 }
 
-static int orion_mdio_read(struct mii_bus *bus, int mii_id,
-			   int regnum)
+static int orion_mdio_smi_is_done(struct orion_mdio_dev *dev)
+{
+	return !(readl(dev->regs) & MVMDIO_SMI_BUSY);
+}
+
+static const struct orion_mdio_ops orion_mdio_smi_ops = {
+	.is_done = orion_mdio_smi_is_done,
+	.poll_interval_min = MVMDIO_SMI_POLL_INTERVAL_MIN,
+	.poll_interval_max = MVMDIO_SMI_POLL_INTERVAL_MAX,
+};
+
+static int orion_mdio_smi_read(struct mii_bus *bus, int mii_id,
+			       int regnum)
 {
 	struct orion_mdio_dev *dev = bus->priv;
 	u32 val;
 	int ret;
 
-	mutex_lock(&dev->lock);
+	if (regnum & MII_ADDR_C45)
+		return -EOPNOTSUPP;
 
-	ret = orion_mdio_wait_ready(bus);
+	ret = orion_mdio_wait_ready(&orion_mdio_smi_ops, bus);
 	if (ret < 0)
-		goto out;
+		return ret;
 
 	writel(((mii_id << MVMDIO_SMI_PHY_ADDR_SHIFT) |
 		(regnum << MVMDIO_SMI_PHY_REG_SHIFT)  |
 		MVMDIO_SMI_READ_OPERATION),
 	       dev->regs);
 
-	ret = orion_mdio_wait_ready(bus);
+	ret = orion_mdio_wait_ready(&orion_mdio_smi_ops, bus);
 	if (ret < 0)
-		goto out;
+		return ret;
 
 	val = readl(dev->regs);
 	if (!(val & MVMDIO_SMI_READ_VALID)) {
 		dev_err(bus->parent, "SMI bus read not valid\n");
-		ret = -ENODEV;
-		goto out;
+		return -ENODEV;
 	}
 
-	ret = val & 0xFFFF;
-out:
-	mutex_unlock(&dev->lock);
-	return ret;
+	return val & GENMASK(15, 0);
 }
 
-static int orion_mdio_write(struct mii_bus *bus, int mii_id,
-			    int regnum, u16 value)
+static int orion_mdio_smi_write(struct mii_bus *bus, int mii_id,
+				int regnum, u16 value)
 {
 	struct orion_mdio_dev *dev = bus->priv;
 	int ret;
 
-	mutex_lock(&dev->lock);
+	if (regnum & MII_ADDR_C45)
+		return -EOPNOTSUPP;
 
-	ret = orion_mdio_wait_ready(bus);
+	ret = orion_mdio_wait_ready(&orion_mdio_smi_ops, bus);
 	if (ret < 0)
-		goto out;
+		return ret;
 
 	writel(((mii_id << MVMDIO_SMI_PHY_ADDR_SHIFT) |
 		(regnum << MVMDIO_SMI_PHY_REG_SHIFT)  |
@@ -162,9 +188,74 @@ static int orion_mdio_write(struct mii_bus *bus, int mii_id,
 		(value << MVMDIO_SMI_DATA_SHIFT)),
 	       dev->regs);
 
-out:
-	mutex_unlock(&dev->lock);
-	return ret;
+	return 0;
+}
+
+static int orion_mdio_xsmi_is_done(struct orion_mdio_dev *dev)
+{
+	return !(readl(dev->regs + MVMDIO_XSMI_MGNT_REG) & MVMDIO_XSMI_BUSY);
+}
+
+static const struct orion_mdio_ops orion_mdio_xsmi_ops = {
+	.is_done = orion_mdio_xsmi_is_done,
+	.poll_interval_min = MVMDIO_XSMI_POLL_INTERVAL_MIN,
+	.poll_interval_max = MVMDIO_XSMI_POLL_INTERVAL_MAX,
+};
+
+static int orion_mdio_xsmi_read(struct mii_bus *bus, int mii_id,
+				int regnum)
+{
+	struct orion_mdio_dev *dev = bus->priv;
+	u16 dev_addr = (regnum >> 16) & GENMASK(4, 0);
+	int ret;
+
+	if (!(regnum & MII_ADDR_C45))
+		return -EOPNOTSUPP;
+
+	ret = orion_mdio_wait_ready(&orion_mdio_xsmi_ops, bus);
+	if (ret < 0)
+		return ret;
+
+	writel(regnum & GENMASK(15, 0), dev->regs + MVMDIO_XSMI_ADDR_REG);
+	writel((mii_id << MVMDIO_XSMI_PHYADDR_SHIFT) |
+	       (dev_addr << MVMDIO_XSMI_DEVADDR_SHIFT) |
+	       MVMDIO_XSMI_READ_OPERATION,
+	       dev->regs + MVMDIO_XSMI_MGNT_REG);
+
+	ret = orion_mdio_wait_ready(&orion_mdio_xsmi_ops, bus);
+	if (ret < 0)
+		return ret;
+
+	if (!(readl(dev->regs + MVMDIO_XSMI_MGNT_REG) &
+	      MVMDIO_XSMI_READ_VALID)) {
+		dev_err(bus->parent, "XSMI bus read not valid\n");
+		return -ENODEV;
+	}
+
+	return readl(dev->regs + MVMDIO_XSMI_MGNT_REG) & GENMASK(15, 0);
+}
+
+static int orion_mdio_xsmi_write(struct mii_bus *bus, int mii_id,
+				int regnum, u16 value)
+{
+	struct orion_mdio_dev *dev = bus->priv;
+	u16 dev_addr = (regnum >> 16) & GENMASK(4, 0);
+	int ret;
+
+	if (!(regnum & MII_ADDR_C45))
+		return -EOPNOTSUPP;
+
+	ret = orion_mdio_wait_ready(&orion_mdio_xsmi_ops, bus);
+	if (ret < 0)
+		return ret;
+
+	writel(regnum & GENMASK(15, 0), dev->regs + MVMDIO_XSMI_ADDR_REG);
+	writel((mii_id << MVMDIO_XSMI_PHYADDR_SHIFT) |
+	       (dev_addr << MVMDIO_XSMI_DEVADDR_SHIFT) |
+	       MVMDIO_XSMI_WRITE_OPERATION | value,
+	       dev->regs + MVMDIO_XSMI_MGNT_REG);
+
+	return 0;
 }
 
 static irqreturn_t orion_mdio_err_irq(int irq, void *dev_id)
@@ -184,11 +275,14 @@ static irqreturn_t orion_mdio_err_irq(int irq, void *dev_id)
 
 static int orion_mdio_probe(struct platform_device *pdev)
 {
+	enum orion_mdio_bus_type type;
 	struct resource *r;
 	struct mii_bus *bus;
 	struct orion_mdio_dev *dev;
 	int i, ret;
 
+	type = (enum orion_mdio_bus_type)of_device_get_match_data(&pdev->dev);
+
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!r) {
 		dev_err(&pdev->dev, "No SMI register address given\n");
@@ -200,9 +294,18 @@ static int orion_mdio_probe(struct platform_device *pdev)
 	if (!bus)
 		return -ENOMEM;
 
+	switch (type) {
+	case BUS_TYPE_SMI:
+		bus->read = orion_mdio_smi_read;
+		bus->write = orion_mdio_smi_write;
+		break;
+	case BUS_TYPE_XSMI:
+		bus->read = orion_mdio_xsmi_read;
+		bus->write = orion_mdio_xsmi_write;
+		break;
+	}
+
 	bus->name = "orion_mdio_bus";
-	bus->read = orion_mdio_read;
-	bus->write = orion_mdio_write;
 	snprintf(bus->id, MII_BUS_ID_SIZE, "%s-mii",
 		 dev_name(&pdev->dev));
 	bus->parent = &pdev->dev;
@@ -244,8 +347,6 @@ static int orion_mdio_probe(struct platform_device *pdev)
 		return -EPROBE_DEFER;
 	}
 
-	mutex_init(&dev->lock);
-
 	if (pdev->dev.of_node)
 		ret = of_mdiobus_register(bus, pdev->dev.of_node);
 	else
@@ -294,7 +395,8 @@ static int orion_mdio_remove(struct platform_device *pdev)
 }
 
 static const struct of_device_id orion_mdio_match[] = {
-	{ .compatible = "marvell,orion-mdio" },
+	{ .compatible = "marvell,orion-mdio", .data = (void *)BUS_TYPE_SMI },
+	{ .compatible = "marvell,xmdio", .data = (void *)BUS_TYPE_XSMI },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, orion_mdio_match);
diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index d297011b535d..0aab74c2a209 100644
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -1976,9 +1976,8 @@ err_drop_frame:
 						      MVNETA_MH_SIZE + NET_SKB_PAD,
 						      rx_bytes,
 						      DMA_FROM_DEVICE);
-			memcpy(skb_put(skb, rx_bytes),
-			       data + MVNETA_MH_SIZE + NET_SKB_PAD,
-			       rx_bytes);
+			skb_put_data(skb, data + MVNETA_MH_SIZE + NET_SKB_PAD,
+				     rx_bytes);
 
 			skb->protocol = eth_type_trans(skb, dev);
 			mvneta_rx_csum(pp, rx_status, skb);
@@ -2103,9 +2102,8 @@ err_drop_frame:
 			                              MVNETA_MH_SIZE + NET_SKB_PAD,
 			                              rx_bytes,
 			                              DMA_FROM_DEVICE);
-			memcpy(skb_put(skb, rx_bytes),
-			       data + MVNETA_MH_SIZE + NET_SKB_PAD,
-			       rx_bytes);
+			skb_put_data(skb, data + MVNETA_MH_SIZE + NET_SKB_PAD,
+				     rx_bytes);
 
 			skb->protocol = eth_type_trans(skb, dev);
 			mvneta_rx_csum(pp, rx_status, skb);
diff --git a/drivers/net/ethernet/marvell/mvpp2.c b/drivers/net/ethernet/marvell/mvpp2.c
index 9b875d776b29..48d21c1e09f2 100644
--- a/drivers/net/ethernet/marvell/mvpp2.c
+++ b/drivers/net/ethernet/marvell/mvpp2.c
@@ -345,9 +345,15 @@
 /* Per-port XGMAC registers. PPv2.2 only, only for GOP port 0,
  * relative to port->base.
  */
+#define MVPP22_XLG_CTRL0_REG			0x100
+#define      MVPP22_XLG_CTRL0_PORT_EN		BIT(0)
+#define      MVPP22_XLG_CTRL0_MAC_RESET_DIS	BIT(1)
+#define      MVPP22_XLG_CTRL0_MIB_CNT_DIS	BIT(14)
+
 #define MVPP22_XLG_CTRL3_REG			0x11c
 #define      MVPP22_XLG_CTRL3_MACMODESELECT_MASK	(7 << 13)
 #define      MVPP22_XLG_CTRL3_MACMODESELECT_GMAC	(0 << 13)
+#define      MVPP22_XLG_CTRL3_MACMODESELECT_10G		(1 << 13)
 
 /* SMI registers. PPv2.2 only, relative to priv->iface_base. */
 #define MVPP22_SMI_MISC_CFG_REG			0x1204
@@ -3719,7 +3725,7 @@ static void mvpp2_bm_bufs_get_addrs(struct device *dev, struct mvpp2 *priv,
 				    dma_addr_t *dma_addr,
 				    phys_addr_t *phys_addr)
 {
-	int cpu = smp_processor_id();
+	int cpu = get_cpu();
 
 	*dma_addr = mvpp2_percpu_read(priv, cpu,
 				      MVPP2_BM_PHY_ALLOC_REG(bm_pool->id));
@@ -3740,6 +3746,8 @@ static void mvpp2_bm_bufs_get_addrs(struct device *dev, struct mvpp2 *priv,
 		if (sizeof(phys_addr_t) == 8)
 			*phys_addr |= (u64)phys_addr_highbits << 32;
 	}
+
+	put_cpu();
 }
 
 /* Free all buffers from the pool */
@@ -3909,29 +3917,12 @@ static void *mvpp2_buf_alloc(struct mvpp2_port *port,
 	return data;
 }
 
-/* Set pool number in a BM cookie */
-static inline u32 mvpp2_bm_cookie_pool_set(u32 cookie, int pool)
-{
-	u32 bm;
-
-	bm = cookie & ~(0xFF << MVPP2_BM_COOKIE_POOL_OFFS);
-	bm |= ((pool & 0xFF) << MVPP2_BM_COOKIE_POOL_OFFS);
-
-	return bm;
-}
-
-/* Get pool number from a BM cookie */
-static inline int mvpp2_bm_cookie_pool_get(unsigned long cookie)
-{
-	return (cookie >> MVPP2_BM_COOKIE_POOL_OFFS) & 0xFF;
-}
-
 /* Release buffer to BM */
 static inline void mvpp2_bm_pool_put(struct mvpp2_port *port, int pool,
 				     dma_addr_t buf_dma_addr,
 				     phys_addr_t buf_phys_addr)
 {
-	int cpu = smp_processor_id();
+	int cpu = get_cpu();
 
 	if (port->priv->hw_version == MVPP22) {
 		u32 val = 0;
@@ -3958,16 +3949,8 @@ static inline void mvpp2_bm_pool_put(struct mvpp2_port *port, int pool,
 			   MVPP2_BM_VIRT_RLS_REG, buf_phys_addr);
 	mvpp2_percpu_write(port->priv, cpu,
 			   MVPP2_BM_PHY_RLS_REG(pool), buf_dma_addr);
-}
 
-/* Refill BM pool */
-static void mvpp2_pool_refill(struct mvpp2_port *port, u32 bm,
-			      dma_addr_t dma_addr,
-			      phys_addr_t phys_addr)
-{
-	int pool = mvpp2_bm_cookie_pool_get(bm);
-
-	mvpp2_bm_pool_put(port, pool, dma_addr, phys_addr);
+	put_cpu();
 }
 
 /* Allocate buffers for the pool */
@@ -4160,7 +4143,10 @@ static inline void mvpp2_interrupts_disable(struct mvpp2_port *port)
 		    MVPP2_ISR_DISABLE_INTERRUPT(cpu_mask));
 }
 
-/* Mask the current CPU's Rx/Tx interrupts */
+/* Mask the current CPU's Rx/Tx interrupts
+ * Called by on_each_cpu(), guaranteed to run with migration disabled,
+ * using smp_processor_id() is OK.
+ */
 static void mvpp2_interrupts_mask(void *arg)
 {
 	struct mvpp2_port *port = arg;
@@ -4169,7 +4155,10 @@ static void mvpp2_interrupts_mask(void *arg)
 			   MVPP2_ISR_RX_TX_MASK_REG(port->id), 0);
 }
 
-/* Unmask the current CPU's Rx/Tx interrupts */
+/* Unmask the current CPU's Rx/Tx interrupts.
+ * Called by on_each_cpu(), guaranteed to run with migration disabled,
+ * using smp_processor_id() is OK.
+ */
 static void mvpp2_interrupts_unmask(void *arg)
 {
 	struct mvpp2_port *port = arg;
@@ -4186,13 +4175,17 @@ static void mvpp22_port_mii_set(struct mvpp2_port *port)
 {
 	u32 val;
 
-	return;
-
 	/* Only GOP port 0 has an XLG MAC */
 	if (port->gop_id == 0) {
 		val = readl(port->base + MVPP22_XLG_CTRL3_REG);
 		val &= ~MVPP22_XLG_CTRL3_MACMODESELECT_MASK;
-		val |= MVPP22_XLG_CTRL3_MACMODESELECT_GMAC;
+
+		if (port->phy_interface == PHY_INTERFACE_MODE_XAUI ||
+		    port->phy_interface == PHY_INTERFACE_MODE_10GKR)
+			val |= MVPP22_XLG_CTRL3_MACMODESELECT_10G;
+		else
+			val |= MVPP22_XLG_CTRL3_MACMODESELECT_GMAC;
+
 		writel(val, port->base + MVPP22_XLG_CTRL3_REG);
 	}
 
@@ -4242,19 +4235,40 @@ static void mvpp2_port_enable(struct mvpp2_port *port)
 {
 	u32 val;
 
-	val = readl(port->base + MVPP2_GMAC_CTRL_0_REG);
-	val |= MVPP2_GMAC_PORT_EN_MASK;
-	val |= MVPP2_GMAC_MIB_CNTR_EN_MASK;
-	writel(val, port->base + MVPP2_GMAC_CTRL_0_REG);
+	/* Only GOP port 0 has an XLG MAC */
+	if (port->gop_id == 0 &&
+	    (port->phy_interface == PHY_INTERFACE_MODE_XAUI ||
+	     port->phy_interface == PHY_INTERFACE_MODE_10GKR)) {
+		val = readl(port->base + MVPP22_XLG_CTRL0_REG);
+		val |= MVPP22_XLG_CTRL0_PORT_EN |
+		       MVPP22_XLG_CTRL0_MAC_RESET_DIS;
+		val &= ~MVPP22_XLG_CTRL0_MIB_CNT_DIS;
+		writel(val, port->base + MVPP22_XLG_CTRL0_REG);
+	} else {
+		val = readl(port->base + MVPP2_GMAC_CTRL_0_REG);
+		val |= MVPP2_GMAC_PORT_EN_MASK;
+		val |= MVPP2_GMAC_MIB_CNTR_EN_MASK;
+		writel(val, port->base + MVPP2_GMAC_CTRL_0_REG);
+	}
 }
 
 static void mvpp2_port_disable(struct mvpp2_port *port)
 {
 	u32 val;
 
-	val = readl(port->base + MVPP2_GMAC_CTRL_0_REG);
-	val &= ~(MVPP2_GMAC_PORT_EN_MASK);
-	writel(val, port->base + MVPP2_GMAC_CTRL_0_REG);
+	/* Only GOP port 0 has an XLG MAC */
+	if (port->gop_id == 0 &&
+	    (port->phy_interface == PHY_INTERFACE_MODE_XAUI ||
+	     port->phy_interface == PHY_INTERFACE_MODE_10GKR)) {
+		val = readl(port->base + MVPP22_XLG_CTRL0_REG);
+		val &= ~(MVPP22_XLG_CTRL0_PORT_EN |
+			 MVPP22_XLG_CTRL0_MAC_RESET_DIS);
+		writel(val, port->base + MVPP22_XLG_CTRL0_REG);
+	} else {
+		val = readl(port->base + MVPP2_GMAC_CTRL_0_REG);
+		val &= ~(MVPP2_GMAC_PORT_EN_MASK);
+		writel(val, port->base + MVPP2_GMAC_CTRL_0_REG);
+	}
 }
 
 /* Set IEEE 802.3x Flow Control Xon Packet Transmission Mode */
@@ -4515,21 +4529,6 @@ static void mvpp2_rxq_offset_set(struct mvpp2_port *port,
 	mvpp2_write(port->priv, MVPP2_RXQ_CONFIG_REG(prxq), val);
 }
 
-/* Obtain BM cookie information from descriptor */
-static u32 mvpp2_bm_cookie_build(struct mvpp2_port *port,
-				 struct mvpp2_rx_desc *rx_desc)
-{
-	int cpu = smp_processor_id();
-	int pool;
-
-	pool = (mvpp2_rxdesc_status_get(port, rx_desc) &
-		MVPP2_RXD_BM_POOL_ID_MASK) >>
-		MVPP2_RXD_BM_POOL_ID_OFFS;
-
-	return ((pool & 0xFF) << MVPP2_BM_COOKIE_POOL_OFFS) |
-	       ((cpu & 0xFF) << MVPP2_BM_COOKIE_CPU_OFFS);
-}
-
 /* Tx descriptors helper methods */
 
 /* Get pointer to next Tx descriptor to be processed (send) by HW */
@@ -4542,7 +4541,11 @@ mvpp2_txq_next_desc_get(struct mvpp2_tx_queue *txq)
 	return txq->descs + tx_desc;
 }
 
-/* Update HW with number of aggregated Tx descriptors to be sent */
+/* Update HW with number of aggregated Tx descriptors to be sent
+ *
+ * Called only from mvpp2_tx(), so migration is disabled, using
+ * smp_processor_id() is OK.
+ */
 static void mvpp2_aggr_txq_pend_desc_add(struct mvpp2_port *port, int pending)
 {
 	/* aggregated access - relevant TXQ number is written in TX desc */
@@ -4553,6 +4556,9 @@ static void mvpp2_aggr_txq_pend_desc_add(struct mvpp2_port *port, int pending)
 
 /* Check if there are enough free descriptors in aggregated txq.
  * If not, update the number of occupied descriptors and repeat the check.
+ *
+ * Called only from mvpp2_tx(), so migration is disabled, using
+ * smp_processor_id() is OK.
  */
 static int mvpp2_aggr_desc_num_check(struct mvpp2 *priv,
 				     struct mvpp2_tx_queue *aggr_txq, int num)
@@ -4571,7 +4577,12 @@ static int mvpp2_aggr_desc_num_check(struct mvpp2 *priv,
 	return 0;
 }
 
-/* Reserved Tx descriptors allocation request */
+/* Reserved Tx descriptors allocation request
+ *
+ * Called only from mvpp2_txq_reserved_desc_num_proc(), itself called
+ * only by mvpp2_tx(), so migration is disabled, using
+ * smp_processor_id() is OK.
+ */
 static int mvpp2_txq_alloc_reserved_desc(struct mvpp2 *priv,
 					 struct mvpp2_tx_queue *txq, int num)
 {
@@ -4675,6 +4686,10 @@ static u32 mvpp2_txq_desc_csum(int l3_offs, int l3_proto,
 /* Get number of sent descriptors and decrement counter.
  * The number of sent descriptors is returned.
  * Per-CPU access
+ *
+ * Called only from mvpp2_txq_done(), called from mvpp2_tx()
+ * (migration disabled) and from the TX completion tasklet (migration
+ * disabled) so using smp_processor_id() is OK.
  */
 static inline int mvpp2_txq_sent_desc_proc(struct mvpp2_port *port,
 					   struct mvpp2_tx_queue *txq)
@@ -4689,6 +4704,9 @@ static inline int mvpp2_txq_sent_desc_proc(struct mvpp2_port *port,
 		MVPP2_TRANSMITTED_COUNT_OFFSET;
 }
 
+/* Called through on_each_cpu(), so runs on all CPUs, with migration
+ * disabled, therefore using smp_processor_id() is OK.
+ */
 static void mvpp2_txq_sent_counter_clear(void *arg)
 {
 	struct mvpp2_port *port = arg;
@@ -4757,7 +4775,7 @@ static void mvpp2_txp_max_tx_size_set(struct mvpp2_port *port)
 static void mvpp2_rx_pkts_coal_set(struct mvpp2_port *port,
 				   struct mvpp2_rx_queue *rxq)
 {
-	int cpu = smp_processor_id();
+	int cpu = get_cpu();
 
 	if (rxq->pkts_coal > MVPP2_OCCUPIED_THRESH_MASK)
 		rxq->pkts_coal = MVPP2_OCCUPIED_THRESH_MASK;
@@ -4765,6 +4783,8 @@ static void mvpp2_rx_pkts_coal_set(struct mvpp2_port *port,
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_RXQ_NUM_REG, rxq->id);
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_RXQ_THRESH_REG,
 			   rxq->pkts_coal);
+
+	put_cpu();
 }
 
 static u32 mvpp2_usec_to_cycles(u32 usec, unsigned long clk_hz)
@@ -4945,7 +4965,7 @@ static int mvpp2_rxq_init(struct mvpp2_port *port,
 	mvpp2_write(port->priv, MVPP2_RXQ_STATUS_REG(rxq->id), 0);
 
 	/* Set Rx descriptors queue starting address - indirect access */
-	cpu = smp_processor_id();
+	cpu = get_cpu();
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_RXQ_NUM_REG, rxq->id);
 	if (port->priv->hw_version == MVPP21)
 		rxq_dma = rxq->descs_dma;
@@ -4954,6 +4974,7 @@ static int mvpp2_rxq_init(struct mvpp2_port *port,
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_RXQ_DESC_ADDR_REG, rxq_dma);
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_RXQ_DESC_SIZE_REG, rxq->size);
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_RXQ_INDEX_REG, 0);
+	put_cpu();
 
 	/* Set Offset */
 	mvpp2_rxq_offset_set(port, rxq->id, NET_SKB_PAD);
@@ -4980,9 +5001,13 @@ static void mvpp2_rxq_drop_pkts(struct mvpp2_port *port,
 
 	for (i = 0; i < rx_received; i++) {
 		struct mvpp2_rx_desc *rx_desc = mvpp2_rxq_next_desc_get(rxq);
-		u32 bm = mvpp2_bm_cookie_build(port, rx_desc);
+		u32 status = mvpp2_rxdesc_status_get(port, rx_desc);
+		int pool;
+
+		pool = (status & MVPP2_RXD_BM_POOL_ID_MASK) >>
+			MVPP2_RXD_BM_POOL_ID_OFFS;
 
-		mvpp2_pool_refill(port, bm,
+		mvpp2_bm_pool_put(port, pool,
 				  mvpp2_rxdesc_dma_addr_get(port, rx_desc),
 				  mvpp2_rxdesc_cookie_get(port, rx_desc));
 	}
@@ -5012,10 +5037,11 @@ static void mvpp2_rxq_deinit(struct mvpp2_port *port,
 	 * free descriptor number
 	 */
 	mvpp2_write(port->priv, MVPP2_RXQ_STATUS_REG(rxq->id), 0);
-	cpu = smp_processor_id();
+	cpu = get_cpu();
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_RXQ_NUM_REG, rxq->id);
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_RXQ_DESC_ADDR_REG, 0);
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_RXQ_DESC_SIZE_REG, 0);
+	put_cpu();
 }
 
 /* Create and initialize a Tx queue */
@@ -5038,7 +5064,7 @@ static int mvpp2_txq_init(struct mvpp2_port *port,
 	txq->last_desc = txq->size - 1;
 
 	/* Set Tx descriptors queue starting address - indirect access */
-	cpu = smp_processor_id();
+	cpu = get_cpu();
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_TXQ_NUM_REG, txq->id);
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_TXQ_DESC_ADDR_REG,
 			   txq->descs_dma);
@@ -5063,6 +5089,7 @@ static int mvpp2_txq_init(struct mvpp2_port *port,
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_TXQ_PREF_BUF_REG,
 			   MVPP2_PREF_BUF_PTR(desc) | MVPP2_PREF_BUF_SIZE_16 |
 			   MVPP2_PREF_BUF_THRESH(desc_per_txq / 2));
+	put_cpu();
 
 	/* WRR / EJP configuration - indirect access */
 	tx_port_num = mvpp2_egress_port(port);
@@ -5133,10 +5160,11 @@ static void mvpp2_txq_deinit(struct mvpp2_port *port,
 	mvpp2_write(port->priv, MVPP2_TXQ_SCHED_TOKEN_CNTR_REG(txq->id), 0);
 
 	/* Set Tx descriptors queue starting address and size */
-	cpu = smp_processor_id();
+	cpu = get_cpu();
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_TXQ_NUM_REG, txq->id);
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_TXQ_DESC_ADDR_REG, 0);
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_TXQ_DESC_SIZE_REG, 0);
+	put_cpu();
 }
 
 /* Cleanup Tx ports */
@@ -5146,7 +5174,7 @@ static void mvpp2_txq_clean(struct mvpp2_port *port, struct mvpp2_tx_queue *txq)
 	int delay, pending, cpu;
 	u32 val;
 
-	cpu = smp_processor_id();
+	cpu = get_cpu();
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_TXQ_NUM_REG, txq->id);
 	val = mvpp2_percpu_read(port->priv, cpu, MVPP2_TXQ_PREF_BUF_REG);
 	val |= MVPP2_TXQ_DRAIN_EN_MASK;
@@ -5173,6 +5201,7 @@ static void mvpp2_txq_clean(struct mvpp2_port *port, struct mvpp2_tx_queue *txq)
 
 	val &= ~MVPP2_TXQ_DRAIN_EN_MASK;
 	mvpp2_percpu_write(port->priv, cpu, MVPP2_TXQ_PREF_BUF_REG, val);
+	put_cpu();
 
 	for_each_present_cpu(cpu) {
 		txq_pcpu = per_cpu_ptr(txq->pcpu, cpu);
@@ -5420,7 +5449,7 @@ static void mvpp2_rx_csum(struct mvpp2_port *port, u32 status,
 
 /* Reuse skb if possible, or allocate a new skb and add it to BM pool */
 static int mvpp2_rx_refill(struct mvpp2_port *port,
-			   struct mvpp2_bm_pool *bm_pool, u32 bm)
+			   struct mvpp2_bm_pool *bm_pool, int pool)
 {
 	dma_addr_t dma_addr;
 	phys_addr_t phys_addr;
@@ -5432,7 +5461,7 @@ static int mvpp2_rx_refill(struct mvpp2_port *port,
 	if (!buf)
 		return -ENOMEM;
 
-	mvpp2_pool_refill(port, bm, dma_addr, phys_addr);
+	mvpp2_bm_pool_put(port, pool, dma_addr, phys_addr);
 
 	return 0;
 }
@@ -5490,7 +5519,7 @@ static int mvpp2_rx(struct mvpp2_port *port, int rx_todo,
 		unsigned int frag_size;
 		dma_addr_t dma_addr;
 		phys_addr_t phys_addr;
-		u32 bm, rx_status;
+		u32 rx_status;
 		int pool, rx_bytes, err;
 		void *data;
 
@@ -5502,8 +5531,8 @@ static int mvpp2_rx(struct mvpp2_port *port, int rx_todo,
 		phys_addr = mvpp2_rxdesc_cookie_get(port, rx_desc);
 		data = (void *)phys_to_virt(phys_addr);
 
-		bm = mvpp2_bm_cookie_build(port, rx_desc);
-		pool = mvpp2_bm_cookie_pool_get(bm);
+		pool = (rx_status & MVPP2_RXD_BM_POOL_ID_MASK) >>
+			MVPP2_RXD_BM_POOL_ID_OFFS;
 		bm_pool = &port->priv->bm_pools[pool];
 
 		/* In case of an error, release the requested buffer pointer
@@ -5516,7 +5545,7 @@ err_drop_frame:
 			dev->stats.rx_errors++;
 			mvpp2_rx_error(port, rx_desc);
 			/* Return the buffer to the pool */
-			mvpp2_pool_refill(port, bm, dma_addr, phys_addr);
+			mvpp2_bm_pool_put(port, pool, dma_addr, phys_addr);
 			continue;
 		}
 
@@ -5531,7 +5560,7 @@ err_drop_frame:
 			goto err_drop_frame;
 		}
 
-		err = mvpp2_rx_refill(port, bm_pool, bm);
+		err = mvpp2_rx_refill(port, bm_pool, pool);
 		if (err) {
 			netdev_err(port->dev, "failed to refill BM pools\n");
 			goto err_drop_frame;
diff --git a/drivers/net/ethernet/mediatek/mtk_eth_soc.c b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
index 16f97552ae98..41a5c5d2ac89 100644
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.c
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
@@ -221,6 +221,9 @@ static void mtk_phy_link_adjust(struct net_device *dev)
 		netif_carrier_on(dev);
 	else
 		netif_carrier_off(dev);
+
+	if (!of_phy_is_fixed_link(mac->of_node))
+		phy_print_status(dev->phydev);
 }
 
 static int mtk_phy_connect_node(struct mtk_eth *eth, struct mtk_mac *mac,
@@ -369,28 +372,48 @@ static void mtk_mdio_cleanup(struct mtk_eth *eth)
 	mdiobus_unregister(eth->mii_bus);
 }
 
-static inline void mtk_irq_disable(struct mtk_eth *eth,
-				   unsigned reg, u32 mask)
+static inline void mtk_tx_irq_disable(struct mtk_eth *eth, u32 mask)
+{
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&eth->tx_irq_lock, flags);
+	val = mtk_r32(eth, MTK_QDMA_INT_MASK);
+	mtk_w32(eth, val & ~mask, MTK_QDMA_INT_MASK);
+	spin_unlock_irqrestore(&eth->tx_irq_lock, flags);
+}
+
+static inline void mtk_tx_irq_enable(struct mtk_eth *eth, u32 mask)
+{
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&eth->tx_irq_lock, flags);
+	val = mtk_r32(eth, MTK_QDMA_INT_MASK);
+	mtk_w32(eth, val | mask, MTK_QDMA_INT_MASK);
+	spin_unlock_irqrestore(&eth->tx_irq_lock, flags);
+}
+
+static inline void mtk_rx_irq_disable(struct mtk_eth *eth, u32 mask)
 {
 	unsigned long flags;
 	u32 val;
 
-	spin_lock_irqsave(&eth->irq_lock, flags);
-	val = mtk_r32(eth, reg);
-	mtk_w32(eth, val & ~mask, reg);
-	spin_unlock_irqrestore(&eth->irq_lock, flags);
+	spin_lock_irqsave(&eth->rx_irq_lock, flags);
+	val = mtk_r32(eth, MTK_PDMA_INT_MASK);
+	mtk_w32(eth, val & ~mask, MTK_PDMA_INT_MASK);
+	spin_unlock_irqrestore(&eth->rx_irq_lock, flags);
 }
 
-static inline void mtk_irq_enable(struct mtk_eth *eth,
-				  unsigned reg, u32 mask)
+static inline void mtk_rx_irq_enable(struct mtk_eth *eth, u32 mask)
 {
 	unsigned long flags;
 	u32 val;
 
-	spin_lock_irqsave(&eth->irq_lock, flags);
-	val = mtk_r32(eth, reg);
-	mtk_w32(eth, val | mask, reg);
-	spin_unlock_irqrestore(&eth->irq_lock, flags);
+	spin_lock_irqsave(&eth->rx_irq_lock, flags);
+	val = mtk_r32(eth, MTK_PDMA_INT_MASK);
+	mtk_w32(eth, val | mask, MTK_PDMA_INT_MASK);
+	spin_unlock_irqrestore(&eth->rx_irq_lock, flags);
 }
 
 static int mtk_set_mac_address(struct net_device *dev, void *p)
@@ -470,9 +493,9 @@ static void mtk_get_stats64(struct net_device *dev,
 	unsigned int start;
 
 	if (netif_running(dev) && netif_device_present(dev)) {
-		if (spin_trylock(&hw_stats->stats_lock)) {
+		if (spin_trylock_bh(&hw_stats->stats_lock)) {
 			mtk_stats_update_mac(mac);
-			spin_unlock(&hw_stats->stats_lock);
+			spin_unlock_bh(&hw_stats->stats_lock);
 		}
 	}
 
@@ -969,6 +992,7 @@ static int mtk_poll_rx(struct napi_struct *napi, int budget,
 		    RX_DMA_VID(trxd.rxd3))
 			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
 					       RX_DMA_VID(trxd.rxd3));
+		skb_record_rx_queue(skb, 0);
 		napi_gro_receive(napi, skb);
 
 		ring->data[idx] = new_data;
@@ -1098,7 +1122,7 @@ static int mtk_napi_tx(struct napi_struct *napi, int budget)
 		return budget;
 
 	napi_complete(napi);
-	mtk_irq_enable(eth, MTK_QDMA_INT_MASK, MTK_TX_DONE_INT);
+	mtk_tx_irq_enable(eth, MTK_TX_DONE_INT);
 
 	return tx_done;
 }
@@ -1132,7 +1156,7 @@ poll_again:
 		goto poll_again;
 	}
 	napi_complete(napi);
-	mtk_irq_enable(eth, MTK_PDMA_INT_MASK, MTK_RX_DONE_INT);
+	mtk_rx_irq_enable(eth, MTK_RX_DONE_INT);
 
 	return rx_done + budget - remain_budget;
 }
@@ -1667,7 +1691,7 @@ static irqreturn_t mtk_handle_irq_rx(int irq, void *_eth)
 
 	if (likely(napi_schedule_prep(&eth->rx_napi))) {
 		__napi_schedule(&eth->rx_napi);
-		mtk_irq_disable(eth, MTK_PDMA_INT_MASK, MTK_RX_DONE_INT);
+		mtk_rx_irq_disable(eth, MTK_RX_DONE_INT);
 	}
 
 	return IRQ_HANDLED;
@@ -1679,7 +1703,7 @@ static irqreturn_t mtk_handle_irq_tx(int irq, void *_eth)
 
 	if (likely(napi_schedule_prep(&eth->tx_napi))) {
 		__napi_schedule(&eth->tx_napi);
-		mtk_irq_disable(eth, MTK_QDMA_INT_MASK, MTK_TX_DONE_INT);
+		mtk_tx_irq_disable(eth, MTK_TX_DONE_INT);
 	}
 
 	return IRQ_HANDLED;
@@ -1691,11 +1715,11 @@ static void mtk_poll_controller(struct net_device *dev)
 	struct mtk_mac *mac = netdev_priv(dev);
 	struct mtk_eth *eth = mac->hw;
 
-	mtk_irq_disable(eth, MTK_QDMA_INT_MASK, MTK_TX_DONE_INT);
-	mtk_irq_disable(eth, MTK_PDMA_INT_MASK, MTK_RX_DONE_INT);
+	mtk_tx_irq_disable(eth, MTK_TX_DONE_INT);
+	mtk_rx_irq_disable(eth, MTK_RX_DONE_INT);
 	mtk_handle_irq_rx(eth->irq[2], dev);
-	mtk_irq_enable(eth, MTK_QDMA_INT_MASK, MTK_TX_DONE_INT);
-	mtk_irq_enable(eth, MTK_PDMA_INT_MASK, MTK_RX_DONE_INT);
+	mtk_tx_irq_enable(eth, MTK_TX_DONE_INT);
+	mtk_rx_irq_enable(eth, MTK_RX_DONE_INT);
 }
 #endif
 
@@ -1736,8 +1760,8 @@ static int mtk_open(struct net_device *dev)
 
 		napi_enable(&eth->tx_napi);
 		napi_enable(&eth->rx_napi);
-		mtk_irq_enable(eth, MTK_QDMA_INT_MASK, MTK_TX_DONE_INT);
-		mtk_irq_enable(eth, MTK_PDMA_INT_MASK, MTK_RX_DONE_INT);
+		mtk_tx_irq_enable(eth, MTK_TX_DONE_INT);
+		mtk_rx_irq_enable(eth, MTK_RX_DONE_INT);
 	}
 	atomic_inc(&eth->dma_refcnt);
 
@@ -1782,8 +1806,8 @@ static int mtk_stop(struct net_device *dev)
 	if (!atomic_dec_and_test(&eth->dma_refcnt))
 		return 0;
 
-	mtk_irq_disable(eth, MTK_QDMA_INT_MASK, MTK_TX_DONE_INT);
-	mtk_irq_disable(eth, MTK_PDMA_INT_MASK, MTK_RX_DONE_INT);
+	mtk_tx_irq_disable(eth, MTK_TX_DONE_INT);
+	mtk_rx_irq_disable(eth, MTK_RX_DONE_INT);
 	napi_disable(&eth->tx_napi);
 	napi_disable(&eth->rx_napi);
 
@@ -1858,11 +1882,13 @@ static int mtk_hw_init(struct mtk_eth *eth)
 	/* Enable RX VLan Offloading */
 	mtk_w32(eth, 1, MTK_CDMP_EG_CTRL);
 
+	/* enable interrupt delay for RX */
+	mtk_w32(eth, MTK_PDMA_DELAY_RX_DELAY, MTK_PDMA_DELAY_INT);
+
 	/* disable delay and normal interrupt */
 	mtk_w32(eth, 0, MTK_QDMA_DELAY_INT);
-	mtk_w32(eth, 0, MTK_PDMA_DELAY_INT);
-	mtk_irq_disable(eth, MTK_QDMA_INT_MASK, ~0);
-	mtk_irq_disable(eth, MTK_PDMA_INT_MASK, ~0);
+	mtk_tx_irq_disable(eth, ~0);
+	mtk_rx_irq_disable(eth, ~0);
 	mtk_w32(eth, RST_GL_PSE, MTK_RST_GL);
 	mtk_w32(eth, 0, MTK_RST_GL);
 
@@ -1933,8 +1959,8 @@ static void mtk_uninit(struct net_device *dev)
 	phy_disconnect(dev->phydev);
 	if (of_phy_is_fixed_link(mac->of_node))
 		of_phy_deregister_fixed_link(mac->of_node);
-	mtk_irq_disable(eth, MTK_QDMA_INT_MASK, ~0);
-	mtk_irq_disable(eth, MTK_PDMA_INT_MASK, ~0);
+	mtk_tx_irq_disable(eth, ~0);
+	mtk_rx_irq_disable(eth, ~0);
 }
 
 static int mtk_do_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
@@ -2056,7 +2082,9 @@ static int mtk_get_link_ksettings(struct net_device *ndev,
 	if (unlikely(test_bit(MTK_RESETTING, &mac->hw->state)))
 		return -EBUSY;
 
-	return phy_ethtool_ksettings_get(ndev->phydev, cmd);
+	phy_ethtool_ksettings_get(ndev->phydev, cmd);
+
+	return 0;
 }
 
 static int mtk_set_link_ksettings(struct net_device *ndev,
@@ -2156,9 +2184,9 @@ static void mtk_get_ethtool_stats(struct net_device *dev,
 		return;
 
 	if (netif_running(dev) && netif_device_present(dev)) {
-		if (spin_trylock(&hwstats->stats_lock)) {
+		if (spin_trylock_bh(&hwstats->stats_lock)) {
 			mtk_stats_update_mac(mac);
-			spin_unlock(&hwstats->stats_lock);
+			spin_unlock_bh(&hwstats->stats_lock);
 		}
 	}
 
@@ -2392,7 +2420,8 @@ static int mtk_probe(struct platform_device *pdev)
 		return PTR_ERR(eth->base);
 
 	spin_lock_init(&eth->page_lock);
-	spin_lock_init(&eth->irq_lock);
+	spin_lock_init(&eth->tx_irq_lock);
+	spin_lock_init(&eth->rx_irq_lock);
 
 	eth->ethsys = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
 						      "mediatek,ethsys");
diff --git a/drivers/net/ethernet/mediatek/mtk_eth_soc.h b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
index 3c46a3b613b9..5868a09f623a 100644
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.h
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
@@ -125,7 +125,14 @@
 #define MTK_PST_DRX_IDX_CFG(x)	(MTK_PST_DRX_IDX0 << (x))
 
 /* PDMA Delay Interrupt Register */
-#define MTK_PDMA_DELAY_INT	0xa0c
+#define MTK_PDMA_DELAY_INT		0xa0c
+#define MTK_PDMA_DELAY_RX_EN		BIT(15)
+#define MTK_PDMA_DELAY_RX_PINT		4
+#define MTK_PDMA_DELAY_RX_PINT_SHIFT	8
+#define MTK_PDMA_DELAY_RX_PTIME		4
+#define MTK_PDMA_DELAY_RX_DELAY		\
+	(MTK_PDMA_DELAY_RX_EN | MTK_PDMA_DELAY_RX_PTIME | \
+	(MTK_PDMA_DELAY_RX_PINT << MTK_PDMA_DELAY_RX_PINT_SHIFT))
 
 /* PDMA Interrupt Status Register */
 #define MTK_PDMA_INT_STATUS	0xa20
@@ -206,6 +213,7 @@
 
 /* QDMA Interrupt Status Register */
 #define MTK_QMTK_INT_STATUS	0x1A18
+#define MTK_RX_DONE_DLY		BIT(30)
 #define MTK_RX_DONE_INT3	BIT(19)
 #define MTK_RX_DONE_INT2	BIT(18)
 #define MTK_RX_DONE_INT1	BIT(17)
@@ -214,8 +222,7 @@
 #define MTK_TX_DONE_INT2	BIT(2)
 #define MTK_TX_DONE_INT1	BIT(1)
 #define MTK_TX_DONE_INT0	BIT(0)
-#define MTK_RX_DONE_INT		(MTK_RX_DONE_INT0 | MTK_RX_DONE_INT1 | \
-				 MTK_RX_DONE_INT2 | MTK_RX_DONE_INT3)
+#define MTK_RX_DONE_INT		MTK_RX_DONE_DLY
 #define MTK_TX_DONE_INT		(MTK_TX_DONE_INT0 | MTK_TX_DONE_INT1 | \
 				 MTK_TX_DONE_INT2 | MTK_TX_DONE_INT3)
 
@@ -512,6 +519,8 @@ struct mtk_rx_ring {
  * @dev:		The device pointer
  * @base:		The mapped register i/o base
  * @page_lock:		Make sure that register operations are atomic
+ * @tx_irq__lock:	Make sure that IRQ register operations are atomic
+ * @rx_irq__lock:	Make sure that IRQ register operations are atomic
  * @dummy_dev:		we run 2 netdevs on 1 physical DMA ring and need a
  *			dummy for NAPI to work
  * @netdev:		The netdev instances
@@ -540,7 +549,8 @@ struct mtk_eth {
 	struct device			*dev;
 	void __iomem			*base;
 	spinlock_t			page_lock;
-	spinlock_t			irq_lock;
+	spinlock_t			tx_irq_lock;
+	spinlock_t			rx_irq_lock;
 	struct net_device		dummy_dev;
 	struct net_device		*netdev[MTK_MAX_DEVS];
 	struct mtk_mac			*mac[MTK_MAX_DEVS];
diff --git a/drivers/net/ethernet/mellanox/Kconfig b/drivers/net/ethernet/mellanox/Kconfig
index d54701047401..84a200764111 100644
--- a/drivers/net/ethernet/mellanox/Kconfig
+++ b/drivers/net/ethernet/mellanox/Kconfig
@@ -19,5 +19,6 @@ if NET_VENDOR_MELLANOX
 source "drivers/net/ethernet/mellanox/mlx4/Kconfig"
 source "drivers/net/ethernet/mellanox/mlx5/core/Kconfig"
 source "drivers/net/ethernet/mellanox/mlxsw/Kconfig"
+source "drivers/net/ethernet/mellanox/mlxfw/Kconfig"
 
 endif # NET_VENDOR_MELLANOX
diff --git a/drivers/net/ethernet/mellanox/Makefile b/drivers/net/ethernet/mellanox/Makefile
index 2e2a5ec509ac..016aa263bc04 100644
--- a/drivers/net/ethernet/mellanox/Makefile
+++ b/drivers/net/ethernet/mellanox/Makefile
@@ -5,3 +5,4 @@
 obj-$(CONFIG_MLX4_CORE) += mlx4/
 obj-$(CONFIG_MLX5_CORE) += mlx5/core/
 obj-$(CONFIG_MLXSW_CORE) += mlxsw/
+obj-$(CONFIG_MLXFW) += mlxfw/
diff --git a/drivers/net/ethernet/mellanox/mlx4/cmd.c b/drivers/net/ethernet/mellanox/mlx4/cmd.c
index c1af47e45d3f..674773b28b2e 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cmd.c
@@ -3280,7 +3280,7 @@ int mlx4_set_vf_link_state(struct mlx4_dev *dev, int port, int vf, int link_stat
 
 	if (mlx4_master_immediate_activate_vlan_qos(priv, slave, port))
 		mlx4_dbg(dev,
-			 "updating vf %d port %d no link state HW enforcment\n",
+			 "updating vf %d port %d no link state HW enforcement\n",
 			 vf, port);
 	return 0;
 }
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_cq.c b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
index 09dd3776db76..85fe17e4dcfb 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_cq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
@@ -146,16 +146,25 @@ int mlx4_en_activate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq,
 	if (err)
 		goto free_eq;
 
-	cq->mcq.comp  = cq->type != RX ? mlx4_en_tx_irq : mlx4_en_rx_irq;
 	cq->mcq.event = mlx4_en_cq_event;
 
-	if (cq->type != RX)
+	switch (cq->type) {
+	case TX:
+		cq->mcq.comp = mlx4_en_tx_irq;
 		netif_tx_napi_add(cq->dev, &cq->napi, mlx4_en_poll_tx_cq,
 				  NAPI_POLL_WEIGHT);
-	else
+		napi_enable(&cq->napi);
+		break;
+	case RX:
+		cq->mcq.comp = mlx4_en_rx_irq;
 		netif_napi_add(cq->dev, &cq->napi, mlx4_en_poll_rx_cq, 64);
-
-	napi_enable(&cq->napi);
+		napi_enable(&cq->napi);
+		break;
+	case TX_XDP:
+		/* nothing regarding napi, it's shared with rx ring */
+		cq->xdp_busy = false;
+		break;
+	}
 
 	return 0;
 
@@ -184,8 +193,10 @@ void mlx4_en_destroy_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq **pcq)
 
 void mlx4_en_deactivate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq)
 {
-	napi_disable(&cq->napi);
-	netif_napi_del(&cq->napi);
+	if (cq->type != TX_XDP) {
+		napi_disable(&cq->napi);
+		netif_napi_del(&cq->napi);
+	}
 
 	mlx4_cq_free(priv->mdev->dev, &cq->mcq);
 }
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c b/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c
index 1dae8e40fb25..5f41dc92aa68 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_dcb_nl.c
@@ -238,7 +238,7 @@ static u8 mlx4_en_dcbnl_set_state(struct net_device *dev, u8 state)
 		priv->flags &= ~MLX4_EN_FLAG_DCB_ENABLED;
 	}
 
-	if (mlx4_en_setup_tc(dev, num_tcs))
+	if (mlx4_en_alloc_tx_queue_per_tc(dev, num_tcs))
 		return 1;
 
 	return 0;
@@ -303,7 +303,7 @@ static int mlx4_en_ets_validate(struct mlx4_en_priv *priv, struct ieee_ets *ets)
 	int has_ets_tc = 0;
 
 	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
-		if (ets->prio_tc[i] >= MLX4_EN_NUM_UP) {
+		if (ets->prio_tc[i] >= MLX4_EN_NUM_UP_HIGH) {
 			en_err(priv, "Bad priority in UP <=> TC mapping. TC: %d, UP: %d\n",
 					i, ets->prio_tc[i]);
 			return -EINVAL;
@@ -472,7 +472,7 @@ static u8 mlx4_en_dcbnl_setdcbx(struct net_device *dev, u8 mode)
 			goto err;
 		if (mlx4_en_dcbnl_ieee_setpfc(dev, &pfc))
 			goto err;
-		if (mlx4_en_setup_tc(dev, 0))
+		if (mlx4_en_alloc_tx_queue_per_tc(dev, 0))
 			goto err;
 	}
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
index ffbcb27c05e5..c751a1d434ad 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
@@ -89,7 +89,7 @@ mlx4_en_get_drvinfo(struct net_device *dev, struct ethtool_drvinfo *drvinfo)
 	struct mlx4_en_dev *mdev = priv->mdev;
 
 	strlcpy(drvinfo->driver, DRV_NAME, sizeof(drvinfo->driver));
-	strlcpy(drvinfo->version, DRV_VERSION " (" DRV_RELDATE ")",
+	strlcpy(drvinfo->version, DRV_VERSION,
 		sizeof(drvinfo->version));
 	snprintf(drvinfo->fw_version, sizeof(drvinfo->fw_version),
 		"%d.%d.%d",
@@ -1750,7 +1750,8 @@ static void mlx4_en_get_channels(struct net_device *dev,
 	channel->max_tx = MLX4_EN_MAX_TX_RING_P_UP;
 
 	channel->rx_count = priv->rx_ring_num;
-	channel->tx_count = priv->tx_ring_num[TX] / MLX4_EN_NUM_UP;
+	channel->tx_count = priv->tx_ring_num[TX] /
+			    priv->prof->num_up;
 }
 
 static int mlx4_en_set_channels(struct net_device *dev,
@@ -1763,6 +1764,7 @@ static int mlx4_en_set_channels(struct net_device *dev,
 	int port_up = 0;
 	int xdp_count;
 	int err = 0;
+	u8 up;
 
 	if (!channel->tx_count || !channel->rx_count)
 		return -EINVAL;
@@ -1773,18 +1775,19 @@ static int mlx4_en_set_channels(struct net_device *dev,
 
 	mutex_lock(&mdev->state_lock);
 	xdp_count = priv->tx_ring_num[TX_XDP] ? channel->rx_count : 0;
-	if (channel->tx_count * MLX4_EN_NUM_UP + xdp_count > MAX_TX_RINGS) {
+	if (channel->tx_count * priv->prof->num_up + xdp_count >
+	    MAX_TX_RINGS) {
 		err = -EINVAL;
 		en_err(priv,
 		       "Total number of TX and XDP rings (%d) exceeds the maximum supported (%d)\n",
-		       channel->tx_count * MLX4_EN_NUM_UP + xdp_count,
+		       channel->tx_count * priv->prof->num_up  + xdp_count,
 		       MAX_TX_RINGS);
 		goto out;
 	}
 
 	memcpy(&new_prof, priv->prof, sizeof(struct mlx4_en_port_profile));
 	new_prof.num_tx_rings_p_up = channel->tx_count;
-	new_prof.tx_ring_num[TX] = channel->tx_count * MLX4_EN_NUM_UP;
+	new_prof.tx_ring_num[TX] = channel->tx_count * priv->prof->num_up;
 	new_prof.tx_ring_num[TX_XDP] = xdp_count;
 	new_prof.rx_ring_num = channel->rx_count;
 
@@ -1799,11 +1802,11 @@ static int mlx4_en_set_channels(struct net_device *dev,
 
 	mlx4_en_safe_replace_resources(priv, tmp);
 
-	netif_set_real_num_tx_queues(dev, priv->tx_ring_num[TX]);
 	netif_set_real_num_rx_queues(dev, priv->rx_ring_num);
 
-	if (netdev_get_num_tc(dev))
-		mlx4_en_setup_tc(dev, MLX4_EN_NUM_UP);
+	up = (priv->prof->num_up == MLX4_EN_NUM_UP_LOW) ?
+				    0 : priv->prof->num_up;
+	mlx4_en_setup_tc(dev, up);
 
 	en_warn(priv, "Using %d TX rings\n", priv->tx_ring_num[TX]);
 	en_warn(priv, "Using %d RX rings\n", priv->rx_ring_num);
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_main.c b/drivers/net/ethernet/mellanox/mlx4/en_main.c
index 36a7a54bbb82..2b0cbca4beb5 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_main.c
@@ -46,11 +46,11 @@
 MODULE_AUTHOR("Liran Liss, Yevgeny Petrilin");
 MODULE_DESCRIPTION("Mellanox ConnectX HCA Ethernet driver");
 MODULE_LICENSE("Dual BSD/GPL");
-MODULE_VERSION(DRV_VERSION " ("DRV_RELDATE")");
+MODULE_VERSION(DRV_VERSION);
 
 static const char mlx4_en_version[] =
 	DRV_NAME ": Mellanox ConnectX HCA Ethernet driver v"
-	DRV_VERSION " (" DRV_RELDATE ")\n";
+	DRV_VERSION "\n";
 
 #define MLX4_EN_PARM_INT(X, def_val, desc) \
 	static unsigned int X = def_val;\
@@ -125,9 +125,9 @@ void mlx4_en_update_loopback_state(struct net_device *dev,
 		priv->flags |= MLX4_EN_FLAG_ENABLE_HW_LOOPBACK;
 
 	mutex_lock(&priv->mdev->state_lock);
-	if (priv->mdev->dev->caps.flags2 &
-	    MLX4_DEV_CAP_FLAG2_UPDATE_QP_SRC_CHECK_LB &&
-	    priv->rss_map.indir_qp.qpn) {
+	if ((priv->mdev->dev->caps.flags2 &
+	     MLX4_DEV_CAP_FLAG2_UPDATE_QP_SRC_CHECK_LB) &&
+	    priv->rss_map.indir_qp && priv->rss_map.indir_qp->qpn) {
 		int i;
 		int err = 0;
 		int loopback = !!(features & NETIF_F_LOOPBACK);
@@ -169,8 +169,10 @@ static int mlx4_en_get_profile(struct mlx4_en_dev *mdev)
 		params->prof[i].tx_ppp = pfctx;
 		params->prof[i].tx_ring_size = MLX4_EN_DEF_TX_RING_SIZE;
 		params->prof[i].rx_ring_size = MLX4_EN_DEF_RX_RING_SIZE;
+		params->prof[i].num_up = MLX4_EN_NUM_UP_LOW;
+		params->prof[i].num_tx_rings_p_up = params->num_tx_rings_p_up;
 		params->prof[i].tx_ring_num[TX] = params->num_tx_rings_p_up *
-			MLX4_EN_NUM_UP;
+			params->prof[i].num_up;
 		params->prof[i].rss_rings = 0;
 		params->prof[i].inline_thold = inline_thold;
 	}
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index 94fab20ef146..3a291fc1780a 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -60,11 +60,11 @@ int mlx4_en_setup_tc(struct net_device *dev, u8 up)
 	int i;
 	unsigned int offset = 0;
 
-	if (up && up != MLX4_EN_NUM_UP)
+	if (up && up != MLX4_EN_NUM_UP_HIGH)
 		return -EINVAL;
 
 	netdev_set_num_tc(dev, up);
-
+	netif_set_real_num_tx_queues(dev, priv->tx_ring_num[TX]);
 	/* Partition Tx queues evenly amongst UP's */
 	for (i = 0; i < up; i++) {
 		netdev_set_tc_queue(dev, i, priv->num_tx_rings_p_up, offset);
@@ -86,15 +86,63 @@ int mlx4_en_setup_tc(struct net_device *dev, u8 up)
 	return 0;
 }
 
-static int __mlx4_en_setup_tc(struct net_device *dev, u32 handle, __be16 proto,
+int mlx4_en_alloc_tx_queue_per_tc(struct net_device *dev, u8 tc)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = priv->mdev;
+	struct mlx4_en_port_profile new_prof;
+	struct mlx4_en_priv *tmp;
+	int port_up = 0;
+	int err = 0;
+
+	tmp = kzalloc(sizeof(*tmp), GFP_KERNEL);
+	if (!tmp)
+		return -ENOMEM;
+
+	mutex_lock(&mdev->state_lock);
+	memcpy(&new_prof, priv->prof, sizeof(struct mlx4_en_port_profile));
+	new_prof.num_up = (tc == 0) ? MLX4_EN_NUM_UP_LOW :
+				      MLX4_EN_NUM_UP_HIGH;
+	new_prof.tx_ring_num[TX] = new_prof.num_tx_rings_p_up *
+				   new_prof.num_up;
+	err = mlx4_en_try_alloc_resources(priv, tmp, &new_prof, true);
+	if (err)
+		goto out;
+
+	if (priv->port_up) {
+		port_up = 1;
+		mlx4_en_stop_port(dev, 1);
+	}
+
+	mlx4_en_safe_replace_resources(priv, tmp);
+	if (port_up) {
+		err = mlx4_en_start_port(dev);
+		if (err) {
+			en_err(priv, "Failed starting port for setup TC\n");
+			goto out;
+		}
+	}
+
+	err = mlx4_en_setup_tc(dev, tc);
+out:
+	mutex_unlock(&mdev->state_lock);
+	kfree(tmp);
+	return err;
+}
+
+static int __mlx4_en_setup_tc(struct net_device *dev, u32 handle,
+			      u32 chain_index, __be16 proto,
 			      struct tc_to_netdev *tc)
 {
 	if (tc->type != TC_SETUP_MQPRIO)
 		return -EINVAL;
 
+	if (tc->mqprio->num_tc && tc->mqprio->num_tc != MLX4_EN_NUM_UP_HIGH)
+		return -EINVAL;
+
 	tc->mqprio->hw = TC_MQPRIO_HW_OFFLOAD_TCS;
 
-	return mlx4_en_setup_tc(dev, tc->mqprio->num_tc);
+	return mlx4_en_alloc_tx_queue_per_tc(dev, tc->mqprio->num_tc);
 }
 
 #ifdef CONFIG_RFS_ACCEL
@@ -595,6 +643,8 @@ static int mlx4_en_get_qp(struct mlx4_en_priv *priv)
 		return err;
 	}
 
+	en_info(priv, "Steering Mode %d\n", dev->caps.steering_mode);
+
 	if (dev->caps.steering_mode == MLX4_STEERING_MODE_A0) {
 		int base_qpn = mlx4_get_base_qpn(dev, priv->port);
 		*qpn = base_qpn + index;
@@ -1009,7 +1059,7 @@ static void mlx4_en_do_multicast(struct mlx4_en_priv *priv,
 				memcpy(&mc_list[10], mclist->addr, ETH_ALEN);
 				mc_list[5] = priv->port;
 				err = mlx4_multicast_detach(mdev->dev,
-							    &priv->rss_map.indir_qp,
+							    priv->rss_map.indir_qp,
 							    mc_list,
 							    MLX4_PROT_ETH,
 							    mclist->reg_id);
@@ -1031,7 +1081,7 @@ static void mlx4_en_do_multicast(struct mlx4_en_priv *priv,
 				/* needed for B0 steering support */
 				mc_list[5] = priv->port;
 				err = mlx4_multicast_attach(mdev->dev,
-							    &priv->rss_map.indir_qp,
+							    priv->rss_map.indir_qp,
 							    mc_list,
 							    priv->port, 0,
 							    MLX4_PROT_ETH,
@@ -1349,7 +1399,7 @@ static void mlx4_en_set_default_moderation(struct mlx4_en_priv *priv)
 	priv->rx_usecs = MLX4_EN_RX_COAL_TIME;
 	priv->tx_frames = MLX4_EN_TX_COAL_PKTS;
 	priv->tx_usecs = MLX4_EN_TX_COAL_TIME;
-	en_dbg(INTR, priv, "Default coalesing params for mtu:%d - rx_frames:%d rx_usecs:%d\n",
+	en_dbg(INTR, priv, "Default coalescing params for mtu:%d - rx_frames:%d rx_usecs:%d\n",
 	       priv->dev->mtu, priv->rx_frames, priv->rx_usecs);
 
 	/* Setup cq moderation params */
@@ -1676,13 +1726,15 @@ int mlx4_en_start_port(struct net_device *dev)
 			if (t != TX_XDP) {
 				tx_ring->tx_queue = netdev_get_tx_queue(dev, i);
 				tx_ring->recycle_ring = NULL;
+
+				/* Arm CQ for TX completions */
+				mlx4_en_arm_cq(priv, cq);
+
 			} else {
 				mlx4_en_init_recycle_ring(priv, i);
+				/* XDP TX CQ should never be armed */
 			}
 
-			/* Arm CQ for TX completions */
-			mlx4_en_arm_cq(priv, cq);
-
 			/* Set initial ownership of all Tx TXBBs to SW (1) */
 			for (j = 0; j < tx_ring->buf_size; j += STAMP_STRIDE)
 				*((u32 *)(tx_ring->buf + j)) = 0xffffffff;
@@ -1741,7 +1793,7 @@ int mlx4_en_start_port(struct net_device *dev)
 	/* Attach rx QP to bradcast address */
 	eth_broadcast_addr(&mc_list[10]);
 	mc_list[5] = priv->port; /* needed for B0 steering support */
-	if (mlx4_multicast_attach(mdev->dev, &priv->rss_map.indir_qp, mc_list,
+	if (mlx4_multicast_attach(mdev->dev, priv->rss_map.indir_qp, mc_list,
 				  priv->port, 0, MLX4_PROT_ETH,
 				  &priv->broadcast_id))
 		mlx4_warn(mdev, "Failed Attaching Broadcast\n");
@@ -1865,12 +1917,12 @@ void mlx4_en_stop_port(struct net_device *dev, int detach)
 	/* Detach All multicasts */
 	eth_broadcast_addr(&mc_list[10]);
 	mc_list[5] = priv->port; /* needed for B0 steering support */
-	mlx4_multicast_detach(mdev->dev, &priv->rss_map.indir_qp, mc_list,
+	mlx4_multicast_detach(mdev->dev, priv->rss_map.indir_qp, mc_list,
 			      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,
+		mlx4_multicast_detach(mdev->dev, priv->rss_map.indir_qp,
 				      mc_list, MLX4_PROT_ETH, mclist->reg_id);
 		if (mclist->tunnel_reg_id)
 			mlx4_flow_detach(mdev->dev, mclist->tunnel_reg_id);
@@ -2139,7 +2191,7 @@ static int mlx4_en_copy_priv(struct mlx4_en_priv *dst,
 
 	memcpy(&dst->hwtstamp_config, &prof->hwtstamp_config,
 	       sizeof(dst->hwtstamp_config));
-	dst->num_tx_rings_p_up = src->mdev->profile.num_tx_rings_p_up;
+	dst->num_tx_rings_p_up = prof->num_tx_rings_p_up;
 	dst->rx_ring_num = prof->rx_ring_num;
 	dst->flags = prof->flags;
 	dst->mdev = src->mdev;
@@ -2192,6 +2244,7 @@ static void mlx4_en_update_priv(struct mlx4_en_priv *dst,
 		dst->tx_ring[t] = src->tx_ring[t];
 		dst->tx_cq[t] = src->tx_cq[t];
 	}
+	dst->num_tx_rings_p_up = src->num_tx_rings_p_up;
 	dst->rx_ring_num = src->rx_ring_num;
 	memcpy(dst->prof, src->prof, sizeof(struct mlx4_en_port_profile));
 }
@@ -2375,6 +2428,7 @@ static int mlx4_en_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+	case HWTSTAMP_FILTER_NTP_ALL:
 		config.rx_filter = HWTSTAMP_FILTER_ALL;
 		break;
 	default:
@@ -2774,7 +2828,7 @@ static int mlx4_xdp_set(struct net_device *dev, struct bpf_prog *prog)
 	if (priv->tx_ring_num[TX] + xdp_ring_num > MAX_TX_RINGS) {
 		tx_changed = 1;
 		new_prof.tx_ring_num[TX] =
-			MAX_TX_RINGS - ALIGN(xdp_ring_num, MLX4_EN_NUM_UP);
+			MAX_TX_RINGS - ALIGN(xdp_ring_num, priv->prof->num_up);
 		en_warn(priv, "Reducing the number of TX rings, to not exceed the max total rings number.\n");
 	}
 
@@ -2819,11 +2873,25 @@ out:
 	return err;
 }
 
-static bool mlx4_xdp_attached(struct net_device *dev)
+static u32 mlx4_xdp_query(struct net_device *dev)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = priv->mdev;
+	const struct bpf_prog *xdp_prog;
+	u32 prog_id = 0;
+
+	if (!priv->tx_ring_num[TX_XDP])
+		return prog_id;
+
+	mutex_lock(&mdev->state_lock);
+	xdp_prog = rcu_dereference_protected(
+		priv->rx_ring[0]->xdp_prog,
+		lockdep_is_held(&mdev->state_lock));
+	if (xdp_prog)
+		prog_id = xdp_prog->aux->id;
+	mutex_unlock(&mdev->state_lock);
 
-	return !!priv->tx_ring_num[TX_XDP];
+	return prog_id;
 }
 
 static int mlx4_xdp(struct net_device *dev, struct netdev_xdp *xdp)
@@ -2832,7 +2900,8 @@ static int mlx4_xdp(struct net_device *dev, struct netdev_xdp *xdp)
 	case XDP_SETUP_PROG:
 		return mlx4_xdp_set(dev, xdp->prog);
 	case XDP_QUERY_PROG:
-		xdp->prog_attached = mlx4_xdp_attached(dev);
+		xdp->prog_id = mlx4_xdp_query(dev);
+		xdp->prog_attached = !!xdp->prog_id;
 		return 0;
 	default:
 		return -EINVAL;
@@ -3250,7 +3319,7 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 		priv->flags |= MLX4_EN_DCB_ENABLED;
 		priv->cee_config.pfc_state = false;
 
-		for (i = 0; i < MLX4_EN_NUM_UP; i++)
+		for (i = 0; i < MLX4_EN_NUM_UP_HIGH; i++)
 			priv->cee_config.dcb_pfc[i] = pfc_disabled;
 
 		if (mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_ETS_CFG) {
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_resources.c b/drivers/net/ethernet/mellanox/mlx4/en_resources.c
index a6b0db0e0383..86d2d42d658d 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_resources.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_resources.c
@@ -63,7 +63,8 @@ void mlx4_en_fill_qp_context(struct mlx4_en_priv *priv, int size, int stride,
 	context->local_qpn = cpu_to_be32(qpn);
 	context->pri_path.ackto = 1 & 0x07;
 	context->pri_path.sched_queue = 0x83 | (priv->port - 1) << 6;
-	if (user_prio >= 0) {
+	/* force user priority per tx ring */
+	if (user_prio >= 0 && priv->prof->num_up == MLX4_EN_NUM_UP_HIGH) {
 		context->pri_path.sched_queue |= user_prio << 3;
 		context->pri_path.feup = MLX4_FEUP_FORCE_ETH_UP;
 	}
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_rx.c b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
index 77abd1813047..e5fb89505a13 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
@@ -134,10 +134,11 @@ static int mlx4_en_prepare_rx_desc(struct mlx4_en_priv *priv,
 				   struct mlx4_en_rx_ring *ring, int index,
 				   gfp_t gfp)
 {
-	struct mlx4_en_rx_desc *rx_desc = ring->buf + (index * ring->stride);
+	struct mlx4_en_rx_desc *rx_desc = ring->buf +
+		(index << ring->log_stride);
 	struct mlx4_en_rx_alloc *frags = ring->rx_info +
 					(index << priv->log_rx_info);
-	if (ring->page_cache.index > 0) {
+	if (likely(ring->page_cache.index > 0)) {
 		/* XDP uses a single page per frame */
 		if (!frags->page) {
 			ring->page_cache.index--;
@@ -178,6 +179,7 @@ static void mlx4_en_free_rx_desc(const struct mlx4_en_priv *priv,
 	}
 }
 
+/* Function not in fast-path */
 static int mlx4_en_fill_rx_buffers(struct mlx4_en_priv *priv)
 {
 	struct mlx4_en_rx_ring *ring;
@@ -539,14 +541,14 @@ static void validate_loopback(struct mlx4_en_priv *priv, void *va)
 	priv->loopback_ok = 1;
 }
 
-static bool mlx4_en_refill_rx_buffers(struct mlx4_en_priv *priv,
+static void mlx4_en_refill_rx_buffers(struct mlx4_en_priv *priv,
 				      struct mlx4_en_rx_ring *ring)
 {
 	u32 missing = ring->actual_size - (ring->prod - ring->cons);
 
 	/* Try to batch allocations, but not too much. */
 	if (missing < 8)
-		return false;
+		return;
 	do {
 		if (mlx4_en_prepare_rx_desc(priv, ring,
 					    ring->prod & ring->size_mask,
@@ -554,9 +556,9 @@ static bool mlx4_en_refill_rx_buffers(struct mlx4_en_priv *priv,
 					    __GFP_MEMALLOC))
 			break;
 		ring->prod++;
-	} while (--missing);
+	} while (likely(--missing));
 
-	return true;
+	mlx4_en_update_rx_prod_db(ring);
 }
 
 /* When hardware doesn't strip the vlan, we need to calculate the checksum
@@ -637,21 +639,14 @@ static int check_csum(struct mlx4_cqe *cqe, struct sk_buff *skb, void *va,
 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_en_dev *mdev = priv->mdev;
-	struct mlx4_cqe *cqe;
-	struct mlx4_en_rx_ring *ring = priv->rx_ring[cq->ring];
-	struct mlx4_en_rx_alloc *frags;
+	int factor = priv->cqe_factor;
+	struct mlx4_en_rx_ring *ring;
 	struct bpf_prog *xdp_prog;
-	int doorbell_pending;
-	struct sk_buff *skb;
-	int index;
-	int nr;
-	unsigned int length;
+	int cq_ring = cq->ring;
+	bool doorbell_pending;
+	struct mlx4_cqe *cqe;
 	int polled = 0;
-	int ip_summed;
-	int factor = priv->cqe_factor;
-	u64 timestamp;
-	bool l2_tunnel;
+	int index;
 
 	if (unlikely(!priv->port_up))
 		return 0;
@@ -659,6 +654,8 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 	if (unlikely(budget <= 0))
 		return polled;
 
+	ring = priv->rx_ring[cq_ring];
+
 	/* Protect accesses to: ring->xdp_prog, priv->mac_hash list */
 	rcu_read_lock();
 	xdp_prog = rcu_dereference(ring->xdp_prog);
@@ -673,10 +670,17 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 	/* Process all completed CQEs */
 	while (XNOR(cqe->owner_sr_opcode & MLX4_CQE_OWNER_MASK,
 		    cq->mcq.cons_index & cq->size)) {
+		struct mlx4_en_rx_alloc *frags;
+		enum pkt_hash_types hash_type;
+		struct sk_buff *skb;
+		unsigned int length;
+		int ip_summed;
 		void *va;
+		int nr;
 
 		frags = ring->rx_info + (index << priv->log_rx_info);
 		va = page_address(frags[0].page) + frags[0].page_offset;
+		prefetchw(va);
 		/*
 		 * make sure we read the CQE after we read the ownership bit
 		 */
@@ -768,7 +772,7 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 				break;
 			case XDP_TX:
 				if (likely(!mlx4_en_xmit_frame(ring, frags, dev,
-							length, cq->ring,
+							length, cq_ring,
 							&doorbell_pending))) {
 					frags[0].page = NULL;
 					goto next;
@@ -790,24 +794,27 @@ xdp_drop_no_cnt:
 		ring->packets++;
 
 		skb = napi_get_frags(&cq->napi);
-		if (!skb)
+		if (unlikely(!skb))
 			goto next;
 
 		if (unlikely(ring->hwtstamp_rx_filter == HWTSTAMP_FILTER_ALL)) {
-			timestamp = mlx4_en_get_cqe_ts(cqe);
-			mlx4_en_fill_hwtstamps(mdev, skb_hwtstamps(skb),
+			u64 timestamp = mlx4_en_get_cqe_ts(cqe);
+
+			mlx4_en_fill_hwtstamps(priv->mdev, skb_hwtstamps(skb),
 					       timestamp);
 		}
-		skb_record_rx_queue(skb, cq->ring);
+		skb_record_rx_queue(skb, cq_ring);
 
 		if (likely(dev->features & NETIF_F_RXCSUM)) {
 			if (cqe->status & cpu_to_be16(MLX4_CQE_STATUS_TCP |
 						      MLX4_CQE_STATUS_UDP)) {
 				if ((cqe->status & cpu_to_be16(MLX4_CQE_STATUS_IPOK)) &&
 				    cqe->checksum == cpu_to_be16(0xffff)) {
-					ip_summed = CHECKSUM_UNNECESSARY;
-					l2_tunnel = (dev->hw_enc_features & NETIF_F_RXCSUM) &&
+					bool l2_tunnel = (dev->hw_enc_features & NETIF_F_RXCSUM) &&
 						(cqe->vlan_my_qpn & cpu_to_be32(MLX4_CQE_L2_TUNNEL));
+
+					ip_summed = CHECKSUM_UNNECESSARY;
+					hash_type = PKT_HASH_TYPE_L4;
 					if (l2_tunnel)
 						skb->csum_level = 1;
 					ring->csum_ok++;
@@ -822,6 +829,7 @@ xdp_drop_no_cnt:
 						goto csum_none;
 					} else {
 						ip_summed = CHECKSUM_COMPLETE;
+						hash_type = PKT_HASH_TYPE_L3;
 						ring->csum_complete++;
 					}
 				} else {
@@ -831,16 +839,14 @@ xdp_drop_no_cnt:
 		} else {
 csum_none:
 			ip_summed = CHECKSUM_NONE;
+			hash_type = PKT_HASH_TYPE_L3;
 			ring->csum_none++;
 		}
 		skb->ip_summed = ip_summed;
 		if (dev->features & NETIF_F_RXHASH)
 			skb_set_hash(skb,
 				     be32_to_cpu(cqe->immed_rss_invalid),
-				     (ip_summed == CHECKSUM_UNNECESSARY) ?
-					PKT_HASH_TYPE_L4 :
-					PKT_HASH_TYPE_L3);
-
+				     hash_type);
 
 		if ((cqe->vlan_my_qpn &
 		     cpu_to_be32(MLX4_CQE_CVLAN_PRESENT_MASK)) &&
@@ -867,15 +873,17 @@ next:
 		++cq->mcq.cons_index;
 		index = (cq->mcq.cons_index) & ring->size_mask;
 		cqe = mlx4_en_get_cqe(cq->buf, index, priv->cqe_size) + factor;
-		if (++polled == budget)
+		if (unlikely(++polled == budget))
 			break;
 	}
 
 	rcu_read_unlock();
 
-	if (polled) {
-		if (doorbell_pending)
-			mlx4_en_xmit_doorbell(priv->tx_ring[TX_XDP][cq->ring]);
+	if (likely(polled)) {
+		if (doorbell_pending) {
+			priv->tx_cq[TX_XDP][cq_ring]->xdp_busy = true;
+			mlx4_en_xmit_doorbell(priv->tx_ring[TX_XDP][cq_ring]);
+		}
 
 		mlx4_cq_set_ci(&cq->mcq);
 		wmb(); /* ensure HW sees CQ consumer before we post new buffers */
@@ -883,8 +891,7 @@ next:
 	}
 	AVG_PERF_COUNTER(priv->pstats.rx_coal_avg, polled);
 
-	if (mlx4_en_refill_rx_buffers(priv, ring))
-		mlx4_en_update_rx_prod_db(ring);
+	mlx4_en_refill_rx_buffers(priv, ring);
 
 	return polled;
 }
@@ -907,16 +914,30 @@ int mlx4_en_poll_rx_cq(struct napi_struct *napi, int budget)
 	struct mlx4_en_cq *cq = container_of(napi, struct mlx4_en_cq, napi);
 	struct net_device *dev = cq->dev;
 	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_cq *xdp_tx_cq = NULL;
+	bool clean_complete = true;
 	int done;
 
+	if (priv->tx_ring_num[TX_XDP]) {
+		xdp_tx_cq = priv->tx_cq[TX_XDP][cq->ring];
+		if (xdp_tx_cq->xdp_busy) {
+			clean_complete = mlx4_en_process_tx_cq(dev, xdp_tx_cq,
+							       budget);
+			xdp_tx_cq->xdp_busy = !clean_complete;
+		}
+	}
+
 	done = mlx4_en_process_rx_cq(dev, cq, budget);
 
 	/* If we used up all the quota - we're probably not done yet... */
-	if (done == budget) {
+	if (done == budget || !clean_complete) {
 		const struct cpumask *aff;
 		struct irq_data *idata;
 		int cpu_curr;
 
+		/* in case we got here because of !clean_complete */
+		done = budget;
+
 		INC_PERF_COUNTER(priv->pstats.napi_quota);
 
 		cpu_curr = smp_processor_id();
@@ -936,7 +957,7 @@ int mlx4_en_poll_rx_cq(struct napi_struct *napi, int budget)
 			done--;
 	}
 	/* Done for now */
-	if (napi_complete_done(napi, done))
+	if (likely(napi_complete_done(napi, done)))
 		mlx4_en_arm_cq(priv, cq);
 	return done;
 }
@@ -1099,11 +1120,14 @@ int mlx4_en_config_rss_steer(struct mlx4_en_priv *priv)
 	int i, qpn;
 	int err = 0;
 	int good_qps = 0;
+	u8 flags;
 
 	en_dbg(DRV, priv, "Configuring rss steering\n");
+
+	flags = priv->rx_ring_num == 1 ? MLX4_RESERVE_A0_QP : 0;
 	err = mlx4_qp_reserve_range(mdev->dev, priv->rx_ring_num,
 				    priv->rx_ring_num,
-				    &rss_map->base_qpn, 0);
+				    &rss_map->base_qpn, flags);
 	if (err) {
 		en_err(priv, "Failed reserving %d qps\n", priv->rx_ring_num);
 		return err;
@@ -1120,13 +1144,28 @@ int mlx4_en_config_rss_steer(struct mlx4_en_priv *priv)
 		++good_qps;
 	}
 
+	if (priv->rx_ring_num == 1) {
+		rss_map->indir_qp = &rss_map->qps[0];
+		priv->base_qpn = rss_map->indir_qp->qpn;
+		en_info(priv, "Optimized Non-RSS steering\n");
+		return 0;
+	}
+
+	rss_map->indir_qp = kzalloc(sizeof(*rss_map->indir_qp), GFP_KERNEL);
+	if (!rss_map->indir_qp) {
+		err = -ENOMEM;
+		goto rss_err;
+	}
+
 	/* Configure RSS indirection qp */
-	err = mlx4_qp_alloc(mdev->dev, priv->base_qpn, &rss_map->indir_qp, GFP_KERNEL);
+	err = mlx4_qp_alloc(mdev->dev, priv->base_qpn, rss_map->indir_qp,
+			    GFP_KERNEL);
 	if (err) {
 		en_err(priv, "Failed to allocate RSS indirection QP\n");
 		goto rss_err;
 	}
-	rss_map->indir_qp.event = mlx4_en_sqp_event;
+
+	rss_map->indir_qp->event = mlx4_en_sqp_event;
 	mlx4_en_fill_qp_context(priv, 0, 0, 0, 1, priv->base_qpn,
 				priv->rx_ring[0]->cqn, -1, &context);
 
@@ -1164,8 +1203,9 @@ int mlx4_en_config_rss_steer(struct mlx4_en_priv *priv)
 		err = -EINVAL;
 		goto indir_err;
 	}
+
 	err = mlx4_qp_to_ready(mdev->dev, &priv->res.mtt, &context,
-			       &rss_map->indir_qp, &rss_map->indir_state);
+			       rss_map->indir_qp, &rss_map->indir_state);
 	if (err)
 		goto indir_err;
 
@@ -1173,9 +1213,11 @@ int mlx4_en_config_rss_steer(struct mlx4_en_priv *priv)
 
 indir_err:
 	mlx4_qp_modify(mdev->dev, NULL, rss_map->indir_state,
-		       MLX4_QP_STATE_RST, NULL, 0, 0, &rss_map->indir_qp);
-	mlx4_qp_remove(mdev->dev, &rss_map->indir_qp);
-	mlx4_qp_free(mdev->dev, &rss_map->indir_qp);
+		       MLX4_QP_STATE_RST, NULL, 0, 0, rss_map->indir_qp);
+	mlx4_qp_remove(mdev->dev, rss_map->indir_qp);
+	mlx4_qp_free(mdev->dev, rss_map->indir_qp);
+	kfree(rss_map->indir_qp);
+	rss_map->indir_qp = NULL;
 rss_err:
 	for (i = 0; i < good_qps; i++) {
 		mlx4_qp_modify(mdev->dev, NULL, rss_map->state[i],
@@ -1193,10 +1235,15 @@ void mlx4_en_release_rss_steer(struct mlx4_en_priv *priv)
 	struct mlx4_en_rss_map *rss_map = &priv->rss_map;
 	int i;
 
-	mlx4_qp_modify(mdev->dev, NULL, rss_map->indir_state,
-		       MLX4_QP_STATE_RST, NULL, 0, 0, &rss_map->indir_qp);
-	mlx4_qp_remove(mdev->dev, &rss_map->indir_qp);
-	mlx4_qp_free(mdev->dev, &rss_map->indir_qp);
+	if (priv->rx_ring_num > 1) {
+		mlx4_qp_modify(mdev->dev, NULL, rss_map->indir_state,
+			       MLX4_QP_STATE_RST, NULL, 0, 0,
+			       rss_map->indir_qp);
+		mlx4_qp_remove(mdev->dev, rss_map->indir_qp);
+		mlx4_qp_free(mdev->dev, rss_map->indir_qp);
+		kfree(rss_map->indir_qp);
+		rss_map->indir_qp = NULL;
+	}
 
 	for (i = 0; i < priv->rx_ring_num; i++) {
 		mlx4_qp_modify(mdev->dev, NULL, rss_map->state[i],
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_selftest.c b/drivers/net/ethernet/mellanox/mlx4/en_selftest.c
index 17112faafbcc..88699b181946 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_selftest.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_selftest.c
@@ -63,8 +63,8 @@ static int mlx4_en_test_loopback_xmit(struct mlx4_en_priv *priv)
 
 	skb_reserve(skb, NET_IP_ALIGN);
 
-	ethh = (struct ethhdr *)skb_put(skb, sizeof(struct ethhdr));
-	packet	= (unsigned char *)skb_put(skb, packet_size);
+	ethh = skb_put(skb, sizeof(struct ethhdr));
+	packet = skb_put(skb, packet_size);
 	memcpy(ethh->h_dest, priv->dev->dev_addr, ETH_ALEN);
 	eth_zero_addr(ethh->h_source);
 	ethh->h_proto = htons(ETH_P_ARP);
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_tx.c b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
index 6ffd1849a604..4f3a9b27ce4a 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
@@ -234,23 +234,24 @@ static void mlx4_en_stamp_wqe(struct mlx4_en_priv *priv,
 			      u8 owner)
 {
 	__be32 stamp = cpu_to_be32(STAMP_VAL | (!!owner << STAMP_SHIFT));
-	struct mlx4_en_tx_desc *tx_desc = ring->buf + index * TXBB_SIZE;
+	struct mlx4_en_tx_desc *tx_desc = ring->buf + (index << LOG_TXBB_SIZE);
 	struct mlx4_en_tx_info *tx_info = &ring->tx_info[index];
 	void *end = ring->buf + ring->buf_size;
 	__be32 *ptr = (__be32 *)tx_desc;
 	int i;
 
 	/* Optimize the common case when there are no wraparounds */
-	if (likely((void *)tx_desc + tx_info->nr_txbb * TXBB_SIZE <= end)) {
+	if (likely((void *)tx_desc +
+		   (tx_info->nr_txbb << LOG_TXBB_SIZE) <= end)) {
 		/* Stamp the freed descriptor */
-		for (i = 0; i < tx_info->nr_txbb * TXBB_SIZE;
+		for (i = 0; i < tx_info->nr_txbb << LOG_TXBB_SIZE;
 		     i += STAMP_STRIDE) {
 			*ptr = stamp;
 			ptr += STAMP_DWORDS;
 		}
 	} else {
 		/* Stamp the freed descriptor */
-		for (i = 0; i < tx_info->nr_txbb * TXBB_SIZE;
+		for (i = 0; i < tx_info->nr_txbb << LOG_TXBB_SIZE;
 		     i += STAMP_STRIDE) {
 			*ptr = stamp;
 			ptr += STAMP_DWORDS;
@@ -265,11 +266,11 @@ static void mlx4_en_stamp_wqe(struct mlx4_en_priv *priv,
 
 u32 mlx4_en_free_tx_desc(struct mlx4_en_priv *priv,
 			 struct mlx4_en_tx_ring *ring,
-			 int index, u8 owner, u64 timestamp,
+			 int index, u64 timestamp,
 			 int napi_mode)
 {
 	struct mlx4_en_tx_info *tx_info = &ring->tx_info[index];
-	struct mlx4_en_tx_desc *tx_desc = ring->buf + index * TXBB_SIZE;
+	struct mlx4_en_tx_desc *tx_desc = ring->buf + (index << LOG_TXBB_SIZE);
 	struct mlx4_wqe_data_seg *data = (void *) tx_desc + tx_info->data_offset;
 	void *end = ring->buf + ring->buf_size;
 	struct sk_buff *skb = tx_info->skb;
@@ -288,19 +289,20 @@ u32 mlx4_en_free_tx_desc(struct mlx4_en_priv *priv,
 		skb_tstamp_tx(skb, &hwts);
 	}
 
-	/* Optimize the common case when there are no wraparounds */
-	if (likely((void *) tx_desc + tx_info->nr_txbb * TXBB_SIZE <= end)) {
-		if (!tx_info->inl) {
-			if (tx_info->linear)
-				dma_unmap_single(priv->ddev,
-						tx_info->map0_dma,
-						tx_info->map0_byte_count,
-						PCI_DMA_TODEVICE);
-			else
-				dma_unmap_page(priv->ddev,
-					       tx_info->map0_dma,
-					       tx_info->map0_byte_count,
-					       PCI_DMA_TODEVICE);
+	if (!tx_info->inl) {
+		if (tx_info->linear)
+			dma_unmap_single(priv->ddev,
+					 tx_info->map0_dma,
+					 tx_info->map0_byte_count,
+					 PCI_DMA_TODEVICE);
+		else
+			dma_unmap_page(priv->ddev,
+				       tx_info->map0_dma,
+				       tx_info->map0_byte_count,
+				       PCI_DMA_TODEVICE);
+		/* Optimize the common case when there are no wraparounds */
+		if (likely((void *)tx_desc +
+			   (tx_info->nr_txbb << LOG_TXBB_SIZE) <= end)) {
 			for (i = 1; i < nr_maps; i++) {
 				data++;
 				dma_unmap_page(priv->ddev,
@@ -308,23 +310,10 @@ u32 mlx4_en_free_tx_desc(struct mlx4_en_priv *priv,
 					be32_to_cpu(data->byte_count),
 					PCI_DMA_TODEVICE);
 			}
-		}
-	} else {
-		if (!tx_info->inl) {
-			if ((void *) data >= end) {
+		} else {
+			if ((void *)data >= end)
 				data = ring->buf + ((void *)data - end);
-			}
 
-			if (tx_info->linear)
-				dma_unmap_single(priv->ddev,
-						tx_info->map0_dma,
-						tx_info->map0_byte_count,
-						PCI_DMA_TODEVICE);
-			else
-				dma_unmap_page(priv->ddev,
-					       tx_info->map0_dma,
-					       tx_info->map0_byte_count,
-					       PCI_DMA_TODEVICE);
 			for (i = 1; i < nr_maps; i++) {
 				data++;
 				/* Check for wraparound before unmapping */
@@ -344,7 +333,7 @@ u32 mlx4_en_free_tx_desc(struct mlx4_en_priv *priv,
 
 u32 mlx4_en_recycle_tx_desc(struct mlx4_en_priv *priv,
 			    struct mlx4_en_tx_ring *ring,
-			    int index, u8 owner, u64 timestamp,
+			    int index, u64 timestamp,
 			    int napi_mode)
 {
 	struct mlx4_en_tx_info *tx_info = &ring->tx_info[index];
@@ -381,8 +370,7 @@ int mlx4_en_free_tx_buf(struct net_device *dev, struct mlx4_en_tx_ring *ring)
 	while (ring->cons != ring->prod) {
 		ring->last_nr_txbb = ring->free_tx_desc(priv, ring,
 						ring->cons & ring->size_mask,
-						!!(ring->cons & ring->size), 0,
-						0 /* Non-NAPI caller */);
+						0, 0 /* Non-NAPI caller */);
 		ring->cons += ring->last_nr_txbb;
 		cnt++;
 	}
@@ -396,15 +384,14 @@ int mlx4_en_free_tx_buf(struct net_device *dev, struct mlx4_en_tx_ring *ring)
 	return cnt;
 }
 
-static bool mlx4_en_process_tx_cq(struct net_device *dev,
-				  struct mlx4_en_cq *cq, int napi_budget)
+bool mlx4_en_process_tx_cq(struct net_device *dev,
+			   struct mlx4_en_cq *cq, int napi_budget)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	struct mlx4_cq *mcq = &cq->mcq;
 	struct mlx4_en_tx_ring *ring = priv->tx_ring[cq->type][cq->ring];
 	struct mlx4_cqe *cqe;
-	u16 index;
-	u16 new_index, ring_index, stamp_index;
+	u16 index, ring_index, stamp_index;
 	u32 txbbs_skipped = 0;
 	u32 txbbs_stamp = 0;
 	u32 cons_index = mcq->cons_index;
@@ -419,7 +406,7 @@ static bool mlx4_en_process_tx_cq(struct net_device *dev,
 	u32 last_nr_txbb;
 	u32 ring_cons;
 
-	if (!priv->port_up)
+	if (unlikely(!priv->port_up))
 		return true;
 
 	netdev_txq_bql_complete_prefetchw(ring->tx_queue);
@@ -434,6 +421,8 @@ static bool mlx4_en_process_tx_cq(struct net_device *dev,
 	/* Process all completed CQEs */
 	while (XNOR(cqe->owner_sr_opcode & MLX4_CQE_OWNER_MASK,
 			cons_index & size) && (done < budget)) {
+		u16 new_index;
+
 		/*
 		 * make sure we read the CQE after we read the
 		 * ownership bit
@@ -464,8 +453,7 @@ static bool mlx4_en_process_tx_cq(struct net_device *dev,
 			/* free next descriptor */
 			last_nr_txbb = ring->free_tx_desc(
 					priv, ring, ring_index,
-					!!((ring_cons + txbbs_skipped) &
-					ring->size), timestamp, napi_budget);
+					timestamp, napi_budget);
 
 			mlx4_en_stamp_wqe(priv, ring, stamp_index,
 					  !!((ring_cons + txbbs_stamp) &
@@ -481,7 +469,6 @@ static bool mlx4_en_process_tx_cq(struct net_device *dev,
 		cqe = mlx4_en_get_cqe(buf, index, priv->cqe_size) + factor;
 	}
 
-
 	/*
 	 * To prevent CQ overflow we first update CQ consumer and only then
 	 * the ring consumer.
@@ -494,7 +481,7 @@ static bool mlx4_en_process_tx_cq(struct net_device *dev,
 	ACCESS_ONCE(ring->last_nr_txbb) = last_nr_txbb;
 	ACCESS_ONCE(ring->cons) = ring_cons + txbbs_skipped;
 
-	if (ring->free_tx_desc == mlx4_en_recycle_tx_desc)
+	if (cq->type == TX_XDP)
 		return done < budget;
 
 	netdev_tx_completed_queue(ring->tx_queue, packets, bytes);
@@ -506,6 +493,7 @@ static bool mlx4_en_process_tx_cq(struct net_device *dev,
 		netif_tx_wake_queue(ring->tx_queue);
 		ring->wake_queue++;
 	}
+
 	return done < budget;
 }
 
@@ -526,7 +514,7 @@ int mlx4_en_poll_tx_cq(struct napi_struct *napi, int budget)
 	struct mlx4_en_cq *cq = container_of(napi, struct mlx4_en_cq, napi);
 	struct net_device *dev = cq->dev;
 	struct mlx4_en_priv *priv = netdev_priv(dev);
-	int clean_complete;
+	bool clean_complete;
 
 	clean_complete = mlx4_en_process_tx_cq(dev, cq, budget);
 	if (!clean_complete)
@@ -543,7 +531,7 @@ static struct mlx4_en_tx_desc *mlx4_en_bounce_to_desc(struct mlx4_en_priv *priv,
 						      u32 index,
 						      unsigned int desc_size)
 {
-	u32 copy = (ring->size - index) * TXBB_SIZE;
+	u32 copy = (ring->size - index) << LOG_TXBB_SIZE;
 	int i;
 
 	for (i = desc_size - copy - 4; i >= 0; i -= 4) {
@@ -558,12 +546,12 @@ static struct mlx4_en_tx_desc *mlx4_en_bounce_to_desc(struct mlx4_en_priv *priv,
 		if ((i & (TXBB_SIZE - 1)) == 0)
 			wmb();
 
-		*((u32 *) (ring->buf + index * TXBB_SIZE + i)) =
+		*((u32 *)(ring->buf + (index << LOG_TXBB_SIZE) + i)) =
 			*((u32 *) (ring->bounce_buf + i));
 	}
 
 	/* Return real descriptor location */
-	return ring->buf + index * TXBB_SIZE;
+	return ring->buf + (index << LOG_TXBB_SIZE);
 }
 
 /* Decide if skb can be inlined in tx descriptor to avoid dma mapping
@@ -703,15 +691,11 @@ u16 mlx4_en_select_queue(struct net_device *dev, struct sk_buff *skb,
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	u16 rings_p_up = priv->num_tx_rings_p_up;
-	u8 up = 0;
 
 	if (netdev_get_num_tc(dev))
 		return skb_tx_hash(dev, skb);
 
-	if (skb_vlan_tag_present(skb))
-		up = skb_vlan_tag_get(skb) >> VLAN_PRIO_SHIFT;
-
-	return fallback(dev, skb) % rings_p_up + up * rings_p_up;
+	return fallback(dev, skb) % rings_p_up;
 }
 
 static void mlx4_bf_copy(void __iomem *dst, const void *src,
@@ -775,37 +759,101 @@ static void mlx4_en_tx_write_desc(struct mlx4_en_tx_ring *ring,
 	}
 }
 
+static bool mlx4_en_build_dma_wqe(struct mlx4_en_priv *priv,
+				  struct skb_shared_info *shinfo,
+				  struct mlx4_wqe_data_seg *data,
+				  struct sk_buff *skb,
+				  int lso_header_size,
+				  __be32 mr_key,
+				  struct mlx4_en_tx_info *tx_info)
+{
+	struct device *ddev = priv->ddev;
+	dma_addr_t dma = 0;
+	u32 byte_count = 0;
+	int i_frag;
+
+	/* Map fragments if any */
+	for (i_frag = shinfo->nr_frags - 1; i_frag >= 0; i_frag--) {
+		const struct skb_frag_struct *frag;
+
+		frag = &shinfo->frags[i_frag];
+		byte_count = skb_frag_size(frag);
+		dma = skb_frag_dma_map(ddev, frag,
+				       0, byte_count,
+				       DMA_TO_DEVICE);
+		if (dma_mapping_error(ddev, dma))
+			goto tx_drop_unmap;
+
+		data->addr = cpu_to_be64(dma);
+		data->lkey = mr_key;
+		dma_wmb();
+		data->byte_count = cpu_to_be32(byte_count);
+		--data;
+	}
+
+	/* Map linear part if needed */
+	if (tx_info->linear) {
+		byte_count = skb_headlen(skb) - lso_header_size;
+
+		dma = dma_map_single(ddev, skb->data +
+				     lso_header_size, byte_count,
+				     PCI_DMA_TODEVICE);
+		if (dma_mapping_error(ddev, dma))
+			goto tx_drop_unmap;
+
+		data->addr = cpu_to_be64(dma);
+		data->lkey = mr_key;
+		dma_wmb();
+		data->byte_count = cpu_to_be32(byte_count);
+	}
+	/* tx completion can avoid cache line miss for common cases */
+	tx_info->map0_dma = dma;
+	tx_info->map0_byte_count = byte_count;
+
+	return true;
+
+tx_drop_unmap:
+	en_err(priv, "DMA mapping error\n");
+
+	while (++i_frag < shinfo->nr_frags) {
+		++data;
+		dma_unmap_page(ddev, (dma_addr_t)be64_to_cpu(data->addr),
+			       be32_to_cpu(data->byte_count),
+			       PCI_DMA_TODEVICE);
+	}
+
+	return false;
+}
+
 netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct skb_shared_info *shinfo = skb_shinfo(skb);
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	union mlx4_wqe_qpn_vlan	qpn_vlan = {};
-	struct device *ddev = priv->ddev;
 	struct mlx4_en_tx_ring *ring;
 	struct mlx4_en_tx_desc *tx_desc;
 	struct mlx4_wqe_data_seg *data;
 	struct mlx4_en_tx_info *tx_info;
-	int tx_ind = 0;
+	int tx_ind;
 	int nr_txbb;
 	int desc_size;
 	int real_size;
 	u32 index, bf_index;
 	__be32 op_own;
-	u16 vlan_proto = 0;
-	int i_frag;
 	int lso_header_size;
 	void *fragptr = NULL;
 	bool bounce = false;
 	bool send_doorbell;
 	bool stop_queue;
 	bool inline_ok;
+	u8 data_offset;
 	u32 ring_cons;
 	bool bf_ok;
 
 	tx_ind = skb_get_queue_mapping(skb);
 	ring = priv->tx_ring[TX][tx_ind];
 
-	if (!priv->port_up)
+	if (unlikely(!priv->port_up))
 		goto tx_drop;
 
 	/* fetch ring->cons far ahead before needing it to avoid stall */
@@ -818,7 +866,7 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* Align descriptor to TXBB size */
 	desc_size = ALIGN(real_size, TXBB_SIZE);
-	nr_txbb = desc_size / TXBB_SIZE;
+	nr_txbb = desc_size >> LOG_TXBB_SIZE;
 	if (unlikely(nr_txbb > MAX_DESC_TXBBS)) {
 		if (netif_msg_tx_err(priv))
 			en_warn(priv, "Oversized header or SG list\n");
@@ -827,6 +875,8 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	bf_ok = ring->bf_enabled;
 	if (skb_vlan_tag_present(skb)) {
+		u16 vlan_proto;
+
 		qpn_vlan.vlan_tag = cpu_to_be16(skb_vlan_tag_get(skb));
 		vlan_proto = be16_to_cpu(skb->vlan_proto);
 		if (vlan_proto == ETH_P_8021AD)
@@ -851,7 +901,7 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 	/* See if we have enough space for whole descriptor TXBB for setting
 	 * SW ownership on next descriptor; if not, use a bounce buffer. */
 	if (likely(index + nr_txbb <= ring->size))
-		tx_desc = ring->buf + index * TXBB_SIZE;
+		tx_desc = ring->buf + (index << LOG_TXBB_SIZE);
 	else {
 		tx_desc = (struct mlx4_en_tx_desc *) ring->bounce_buf;
 		bounce = true;
@@ -863,64 +913,31 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 	tx_info->skb = skb;
 	tx_info->nr_txbb = nr_txbb;
 
-	data = &tx_desc->data;
-	if (lso_header_size)
-		data = ((void *)&tx_desc->lso + ALIGN(lso_header_size + 4,
-						      DS_SIZE));
+	if (!lso_header_size) {
+		data = &tx_desc->data;
+		data_offset = offsetof(struct mlx4_en_tx_desc, data);
+	} else {
+		int lso_align = ALIGN(lso_header_size + 4, DS_SIZE);
+
+		data = (void *)&tx_desc->lso + lso_align;
+		data_offset = offsetof(struct mlx4_en_tx_desc, lso) + lso_align;
+	}
 
 	/* valid only for none inline segments */
-	tx_info->data_offset = (void *)data - (void *)tx_desc;
+	tx_info->data_offset = data_offset;
 
 	tx_info->inl = inline_ok;
 
-	tx_info->linear = (lso_header_size < skb_headlen(skb) &&
-			   !inline_ok) ? 1 : 0;
+	tx_info->linear = lso_header_size < skb_headlen(skb) && !inline_ok;
 
 	tx_info->nr_maps = shinfo->nr_frags + tx_info->linear;
 	data += tx_info->nr_maps - 1;
 
-	if (!tx_info->inl) {
-		dma_addr_t dma = 0;
-		u32 byte_count = 0;
-
-		/* Map fragments if any */
-		for (i_frag = shinfo->nr_frags - 1; i_frag >= 0; i_frag--) {
-			const struct skb_frag_struct *frag;
-
-			frag = &shinfo->frags[i_frag];
-			byte_count = skb_frag_size(frag);
-			dma = skb_frag_dma_map(ddev, frag,
-					       0, byte_count,
-					       DMA_TO_DEVICE);
-			if (dma_mapping_error(ddev, dma))
-				goto tx_drop_unmap;
-
-			data->addr = cpu_to_be64(dma);
-			data->lkey = ring->mr_key;
-			dma_wmb();
-			data->byte_count = cpu_to_be32(byte_count);
-			--data;
-		}
-
-		/* Map linear part if needed */
-		if (tx_info->linear) {
-			byte_count = skb_headlen(skb) - lso_header_size;
-
-			dma = dma_map_single(ddev, skb->data +
-					     lso_header_size, byte_count,
-					     PCI_DMA_TODEVICE);
-			if (dma_mapping_error(ddev, dma))
-				goto tx_drop_unmap;
-
-			data->addr = cpu_to_be64(dma);
-			data->lkey = ring->mr_key;
-			dma_wmb();
-			data->byte_count = cpu_to_be32(byte_count);
-		}
-		/* tx completion can avoid cache line miss for common cases */
-		tx_info->map0_dma = dma;
-		tx_info->map0_byte_count = byte_count;
-	}
+	if (!tx_info->inl)
+		if (!mlx4_en_build_dma_wqe(priv, shinfo, data, skb,
+					   lso_header_size, ring->mr_key,
+					   tx_info))
+			goto tx_drop_count;
 
 	/*
 	 * For timestamping add flag to skb_shinfo and
@@ -1056,16 +1073,6 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 	}
 	return NETDEV_TX_OK;
 
-tx_drop_unmap:
-	en_err(priv, "DMA mapping error\n");
-
-	while (++i_frag < shinfo->nr_frags) {
-		++data;
-		dma_unmap_page(ddev, (dma_addr_t) be64_to_cpu(data->addr),
-			       be32_to_cpu(data->byte_count),
-			       PCI_DMA_TODEVICE);
-	}
-
 tx_drop_count:
 	ring->tx_dropped++;
 tx_drop:
@@ -1073,52 +1080,41 @@ tx_drop:
 	return NETDEV_TX_OK;
 }
 
+#define MLX4_EN_XDP_TX_NRTXBB  1
+#define MLX4_EN_XDP_TX_REAL_SZ (((CTRL_SIZE + MLX4_EN_XDP_TX_NRTXBB * DS_SIZE) \
+				 / 16) & 0x3f)
+
 netdev_tx_t mlx4_en_xmit_frame(struct mlx4_en_rx_ring *rx_ring,
 			       struct mlx4_en_rx_alloc *frame,
 			       struct net_device *dev, unsigned int length,
-			       int tx_ind, int *doorbell_pending)
+			       int tx_ind, bool *doorbell_pending)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	union mlx4_wqe_qpn_vlan	qpn_vlan = {};
-	struct mlx4_en_tx_ring *ring;
 	struct mlx4_en_tx_desc *tx_desc;
-	struct mlx4_wqe_data_seg *data;
 	struct mlx4_en_tx_info *tx_info;
-	int index, bf_index;
-	bool send_doorbell;
-	int nr_txbb = 1;
-	bool stop_queue;
+	struct mlx4_wqe_data_seg *data;
+	struct mlx4_en_tx_ring *ring;
 	dma_addr_t dma;
-	int real_size;
 	__be32 op_own;
-	u32 ring_cons;
-	bool bf_ok;
+	int index;
 
-	BUILD_BUG_ON_MSG(ALIGN(CTRL_SIZE + DS_SIZE, TXBB_SIZE) != TXBB_SIZE,
-			 "mlx4_en_xmit_frame requires minimum size tx desc");
+	if (unlikely(!priv->port_up))
+		goto tx_drop;
 
 	ring = priv->tx_ring[TX_XDP][tx_ind];
 
-	if (!priv->port_up)
-		goto tx_drop;
-
-	if (mlx4_en_is_tx_ring_full(ring))
+	if (unlikely(mlx4_en_is_tx_ring_full(ring)))
 		goto tx_drop_count;
 
-	/* fetch ring->cons far ahead before needing it to avoid stall */
-	ring_cons = READ_ONCE(ring->cons);
-
 	index = ring->prod & ring->size_mask;
 	tx_info = &ring->tx_info[index];
 
-	bf_ok = ring->bf_enabled;
-
 	/* Track current inflight packets for performance analysis */
 	AVG_PERF_COUNTER(priv->pstats.inflight_avg,
-			 (u32)(ring->prod - ring_cons - 1));
+			 (u32)(ring->prod - READ_ONCE(ring->cons) - 1));
 
-	bf_index = ring->prod;
-	tx_desc = ring->buf + index * TXBB_SIZE;
+	tx_desc = ring->buf + (index << LOG_TXBB_SIZE);
 	data = &tx_desc->data;
 
 	dma = frame->dma;
@@ -1127,9 +1123,9 @@ netdev_tx_t mlx4_en_xmit_frame(struct mlx4_en_rx_ring *rx_ring,
 	frame->page = NULL;
 	tx_info->map0_dma = dma;
 	tx_info->map0_byte_count = PAGE_SIZE;
-	tx_info->nr_txbb = nr_txbb;
+	tx_info->nr_txbb = MLX4_EN_XDP_TX_NRTXBB;
 	tx_info->nr_bytes = max_t(unsigned int, length, ETH_ZLEN);
-	tx_info->data_offset = (void *)data - (void *)tx_desc;
+	tx_info->data_offset = offsetof(struct mlx4_en_tx_desc, data);
 	tx_info->ts_requested = 0;
 	tx_info->nr_maps = 1;
 	tx_info->linear = 1;
@@ -1153,28 +1149,19 @@ netdev_tx_t mlx4_en_xmit_frame(struct mlx4_en_rx_ring *rx_ring,
 	rx_ring->xdp_tx++;
 	AVG_PERF_COUNTER(priv->pstats.tx_pktsz_avg, length);
 
-	ring->prod += nr_txbb;
-
-	stop_queue = mlx4_en_is_tx_ring_full(ring);
-	send_doorbell = stop_queue ||
-				*doorbell_pending > MLX4_EN_DOORBELL_BUDGET;
-	bf_ok &= send_doorbell;
+	ring->prod += MLX4_EN_XDP_TX_NRTXBB;
 
-	real_size = ((CTRL_SIZE + nr_txbb * DS_SIZE) / 16) & 0x3f;
+	qpn_vlan.fence_size = MLX4_EN_XDP_TX_REAL_SZ;
 
-	if (bf_ok)
-		qpn_vlan.bf_qpn = ring->doorbell_qpn | cpu_to_be32(real_size);
-	else
-		qpn_vlan.fence_size = real_size;
-
-	mlx4_en_tx_write_desc(ring, tx_desc, qpn_vlan, TXBB_SIZE, bf_index,
-			      op_own, bf_ok, send_doorbell);
-	*doorbell_pending = send_doorbell ? 0 : *doorbell_pending + 1;
+	mlx4_en_tx_write_desc(ring, tx_desc, qpn_vlan, TXBB_SIZE, 0,
+			      op_own, false, false);
+	*doorbell_pending = true;
 
 	return NETDEV_TX_OK;
 
 tx_drop_count:
 	rx_ring->xdp_tx_full++;
+	*doorbell_pending = true;
 tx_drop:
 	return NETDEV_TX_BUSY;
 }
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index 83aab1e4c8c8..a27c9c13a36e 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -91,7 +91,7 @@ module_param_array(probe_vf, byte, &probe_vfs_argc, 0444);
 MODULE_PARM_DESC(probe_vf, "number of vfs to probe by pf driver (num_vfs > 0)\n"
 			   "probe_vf=port1,port2,port1+2");
 
-int mlx4_log_num_mgm_entry_size = MLX4_DEFAULT_MGM_LOG_ENTRY_SIZE;
+static int mlx4_log_num_mgm_entry_size = MLX4_DEFAULT_MGM_LOG_ENTRY_SIZE;
 module_param_named(log_num_mgm_entry_size,
 			mlx4_log_num_mgm_entry_size, int, 0444);
 MODULE_PARM_DESC(log_num_mgm_entry_size, "log mgm size, that defines the num"
@@ -119,7 +119,7 @@ MODULE_PARM_DESC(enable_4k_uar,
 
 static char mlx4_version[] =
 	DRV_NAME ": Mellanox ConnectX core driver v"
-	DRV_VERSION " (" DRV_RELDATE ")\n";
+	DRV_VERSION "\n";
 
 static struct mlx4_profile default_profile = {
 	.num_qp		= 1 << 18,
@@ -2356,8 +2356,8 @@ static int mlx4_init_hca(struct mlx4_dev *dev)
 					MLX4_A0_STEERING_TABLE_SIZE;
 			}
 
-			mlx4_dbg(dev, "DMFS high rate steer mode is: %s\n",
-				 dmfs_high_rate_steering_mode_str(
+			mlx4_info(dev, "DMFS high rate steer mode is: %s\n",
+				  dmfs_high_rate_steering_mode_str(
 					dev->caps.dmfs_high_steer_mode));
 		}
 	} else {
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4.h b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
index b4f1bc56cc68..30616cd0140d 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
@@ -56,8 +56,7 @@
 
 #define DRV_NAME	"mlx4_core"
 #define PFX		DRV_NAME ": "
-#define DRV_VERSION	"2.2-1"
-#define DRV_RELDATE	"Feb, 2014"
+#define DRV_VERSION	"4.0-0"
 
 #define MLX4_FS_UDP_UC_EN		(1 << 1)
 #define MLX4_FS_TCP_UC_EN		(1 << 2)
@@ -231,7 +230,6 @@ do {									\
 #define mlx4_warn(mdev, format, ...)					\
 	dev_warn(&(mdev)->persist->pdev->dev, format, ##__VA_ARGS__)
 
-extern int mlx4_log_num_mgm_entry_size;
 extern int log_mtts_per_seg;
 extern int mlx4_internal_err_reset;
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
index 39f401aa3047..d350b2158104 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
@@ -58,8 +58,7 @@
 #include "mlx4_stats.h"
 
 #define DRV_NAME	"mlx4_en"
-#define DRV_VERSION	"2.2-1"
-#define DRV_RELDATE	"Feb 2014"
+#define DRV_VERSION	"4.0-0"
 
 #define MLX4_EN_MSG_LEVEL	(NETIF_MSG_LINK | NETIF_MSG_IFDOWN)
 
@@ -73,7 +72,8 @@
 #define DEF_RX_RINGS		16
 #define MAX_RX_RINGS		128
 #define MIN_RX_RINGS		4
-#define TXBB_SIZE		64
+#define LOG_TXBB_SIZE		6
+#define TXBB_SIZE		BIT(LOG_TXBB_SIZE)
 #define HEADROOM		(2048 / TXBB_SIZE + 1)
 #define STAMP_STRIDE		64
 #define STAMP_DWORDS		(STAMP_STRIDE / 4)
@@ -115,14 +115,14 @@
 #define MLX4_EN_SMALL_PKT_SIZE		64
 #define MLX4_EN_MIN_TX_RING_P_UP	1
 #define MLX4_EN_MAX_TX_RING_P_UP	32
-#define MLX4_EN_NUM_UP			8
-#define MLX4_EN_DEF_TX_RING_SIZE	512
+#define MLX4_EN_NUM_UP_LOW		1
+#define MLX4_EN_NUM_UP_HIGH		8
 #define MLX4_EN_DEF_RX_RING_SIZE  	1024
+#define MLX4_EN_DEF_TX_RING_SIZE	MLX4_EN_DEF_RX_RING_SIZE
 #define MAX_TX_RINGS			(MLX4_EN_MAX_TX_RING_P_UP * \
-					 MLX4_EN_NUM_UP)
+					 MLX4_EN_NUM_UP_HIGH)
 
 #define MLX4_EN_DEFAULT_TX_WORK		256
-#define MLX4_EN_DOORBELL_BUDGET		8
 
 /* Target number of packets to coalesce with interrupt moderation */
 #define MLX4_EN_RX_COAL_TARGET	44
@@ -277,7 +277,7 @@ struct mlx4_en_tx_ring {
 	struct netdev_queue	*tx_queue;
 	u32			(*free_tx_desc)(struct mlx4_en_priv *priv,
 						struct mlx4_en_tx_ring *ring,
-						int index, u8 owner,
+						int index,
 						u64 timestamp, int napi_mode);
 	struct mlx4_en_rx_ring	*recycle_ring;
 
@@ -360,7 +360,10 @@ struct mlx4_en_cq {
 	struct mlx4_hwq_resources wqres;
 	int                     ring;
 	struct net_device      *dev;
-	struct napi_struct	napi;
+	union {
+		struct napi_struct napi;
+		bool               xdp_busy;
+	};
 	int size;
 	int buf_size;
 	int vector;
@@ -384,6 +387,7 @@ struct mlx4_en_port_profile {
 	u8 rx_ppp;
 	u8 tx_pause;
 	u8 tx_ppp;
+	u8 num_up;
 	int rss_rings;
 	int inline_thold;
 	struct hwtstamp_config hwtstamp_config;
@@ -432,7 +436,7 @@ struct mlx4_en_rss_map {
 	int base_qpn;
 	struct mlx4_qp qps[MAX_RX_RINGS];
 	enum mlx4_qp_state state[MAX_RX_RINGS];
-	struct mlx4_qp indir_qp;
+	struct mlx4_qp *indir_qp;
 	enum mlx4_qp_state indir_state;
 };
 
@@ -483,7 +487,7 @@ enum dcb_pfc_type {
 
 struct mlx4_en_cee_config {
 	bool	pfc_state;
-	enum	dcb_pfc_type dcb_pfc[MLX4_EN_NUM_UP];
+	enum	dcb_pfc_type dcb_pfc[MLX4_EN_NUM_UP_HIGH];
 };
 #endif
 
@@ -690,7 +694,7 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev);
 netdev_tx_t mlx4_en_xmit_frame(struct mlx4_en_rx_ring *rx_ring,
 			       struct mlx4_en_rx_alloc *frame,
 			       struct net_device *dev, unsigned int length,
-			       int tx_ind, int *doorbell_pending);
+			       int tx_ind, bool *doorbell_pending);
 void mlx4_en_xmit_doorbell(struct mlx4_en_tx_ring *ring);
 bool mlx4_en_rx_recycle(struct mlx4_en_rx_ring *ring,
 			struct mlx4_en_rx_alloc *frame);
@@ -722,13 +726,15 @@ int mlx4_en_process_rx_cq(struct net_device *dev,
 			  int budget);
 int mlx4_en_poll_rx_cq(struct napi_struct *napi, int budget);
 int mlx4_en_poll_tx_cq(struct napi_struct *napi, int budget);
+bool mlx4_en_process_tx_cq(struct net_device *dev,
+			   struct mlx4_en_cq *cq, int napi_budget);
 u32 mlx4_en_free_tx_desc(struct mlx4_en_priv *priv,
 			 struct mlx4_en_tx_ring *ring,
-			 int index, u8 owner, u64 timestamp,
+			 int index, u64 timestamp,
 			 int napi_mode);
 u32 mlx4_en_recycle_tx_desc(struct mlx4_en_priv *priv,
 			    struct mlx4_en_tx_ring *ring,
-			    int index, u8 owner, u64 timestamp,
+			    int index, u64 timestamp,
 			    int napi_mode);
 void mlx4_en_fill_qp_context(struct mlx4_en_priv *priv, int size, int stride,
 		int is_tx, int rss, int qpn, int cqn, int user_prio,
@@ -757,6 +763,7 @@ extern const struct dcbnl_rtnl_ops mlx4_en_dcbnl_pfc_ops;
 #endif
 
 int mlx4_en_setup_tc(struct net_device *dev, u8 up);
+int mlx4_en_alloc_tx_queue_per_tc(struct net_device *dev, u8 tc);
 
 #ifdef CONFIG_RFS_ACCEL
 void mlx4_en_cleanup_filters(struct mlx4_en_priv *priv);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/Kconfig b/drivers/net/ethernet/mellanox/mlx5/core/Kconfig
index 27251a78075c..5aee05992f27 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/Kconfig
+++ b/drivers/net/ethernet/mellanox/mlx5/core/Kconfig
@@ -11,6 +11,20 @@ config MLX5_CORE
 	  Core driver for low level functionality of the ConnectX-4 and
 	  Connect-IB cards by Mellanox Technologies.
 
+config MLX5_ACCEL
+	bool
+
+config MLX5_FPGA
+        bool "Mellanox Technologies Innova support"
+        depends on MLX5_CORE
+	select MLX5_ACCEL
+        ---help---
+          Build support for the Innova family of network cards by Mellanox
+          Technologies. Innova network cards are comprised of a ConnectX chip
+          and an FPGA chip on one board. If you select this option, the
+          mlx5_core driver will include the Innova FPGA core and allow building
+          sandbox-specific client drivers.
+
 config MLX5_CORE_EN
 	bool "Mellanox Technologies ConnectX-4 Ethernet support"
 	depends on NETDEVICES && ETHERNET && INET && PCI && MLX5_CORE
@@ -38,3 +52,15 @@ config MLX5_CORE_IPOIB
 	default n
 	---help---
 	  MLX5 IPoIB offloads & acceleration support.
+
+config MLX5_EN_IPSEC
+	bool "IPSec XFRM cryptography-offload accelaration"
+	depends on MLX5_ACCEL
+	depends on MLX5_CORE_EN
+	depends on XFRM_OFFLOAD
+	depends on INET_ESP_OFFLOAD || INET6_ESP_OFFLOAD
+	default n
+	---help---
+	  Build support for IPsec cryptography-offload accelaration in the NIC.
+	  Note: Support for hardware with this capability needs to be selected
+	  for this option to become available.
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/Makefile b/drivers/net/ethernet/mellanox/mlx5/core/Makefile
index 9e644615f07a..ca367445f864 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/Makefile
+++ b/drivers/net/ethernet/mellanox/mlx5/core/Makefile
@@ -1,9 +1,15 @@
 obj-$(CONFIG_MLX5_CORE)		+= mlx5_core.o
+subdir-ccflags-y += -I$(src)
 
 mlx5_core-y :=	main.o cmd.o debugfs.o fw.o eq.o uar.o pagealloc.o \
 		health.o mcg.o cq.o srq.o alloc.o qp.o port.o mr.o pd.o \
 		mad.o transobj.o vport.o sriov.o fs_cmd.o fs_core.o \
-		fs_counters.o rl.o lag.o dev.o
+		fs_counters.o rl.o lag.o dev.o lib/gid.o
+
+mlx5_core-$(CONFIG_MLX5_ACCEL) += accel/ipsec.o
+
+mlx5_core-$(CONFIG_MLX5_FPGA) += fpga/cmd.o fpga/core.o fpga/conn.o fpga/sdk.o \
+		fpga/ipsec.o
 
 mlx5_core-$(CONFIG_MLX5_CORE_EN) += wq.o eswitch.o eswitch_offloads.o \
 		en_main.o en_common.o en_fs.o en_ethtool.o en_tx.o \
@@ -12,4 +18,7 @@ mlx5_core-$(CONFIG_MLX5_CORE_EN) += wq.o eswitch.o eswitch_offloads.o \
 
 mlx5_core-$(CONFIG_MLX5_CORE_EN_DCB) +=  en_dcbnl.o
 
-mlx5_core-$(CONFIG_MLX5_CORE_IPOIB) += ipoib.o
+mlx5_core-$(CONFIG_MLX5_CORE_IPOIB) += ipoib/ipoib.o ipoib/ethtool.o
+
+mlx5_core-$(CONFIG_MLX5_EN_IPSEC) += en_accel/ipsec.o en_accel/ipsec_rxtx.o \
+		en_accel/ipsec_stats.o
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/accel/ipsec.c b/drivers/net/ethernet/mellanox/mlx5/core/accel/ipsec.c
new file mode 100644
index 000000000000..53e69edaedde
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/accel/ipsec.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#include <linux/mlx5/device.h>
+
+#include "accel/ipsec.h"
+#include "mlx5_core.h"
+#include "fpga/ipsec.h"
+
+void *mlx5_accel_ipsec_sa_cmd_exec(struct mlx5_core_dev *mdev,
+				   struct mlx5_accel_ipsec_sa *cmd)
+{
+	if (!MLX5_IPSEC_DEV(mdev))
+		return ERR_PTR(-EOPNOTSUPP);
+
+	return mlx5_fpga_ipsec_sa_cmd_exec(mdev, cmd);
+}
+
+int mlx5_accel_ipsec_sa_cmd_wait(void *ctx)
+{
+	return mlx5_fpga_ipsec_sa_cmd_wait(ctx);
+}
+
+u32 mlx5_accel_ipsec_device_caps(struct mlx5_core_dev *mdev)
+{
+	return mlx5_fpga_ipsec_device_caps(mdev);
+}
+
+unsigned int mlx5_accel_ipsec_counters_count(struct mlx5_core_dev *mdev)
+{
+	return mlx5_fpga_ipsec_counters_count(mdev);
+}
+
+int mlx5_accel_ipsec_counters_read(struct mlx5_core_dev *mdev, u64 *counters,
+				   unsigned int count)
+{
+	return mlx5_fpga_ipsec_counters_read(mdev, counters, count);
+}
+
+int mlx5_accel_ipsec_init(struct mlx5_core_dev *mdev)
+{
+	return mlx5_fpga_ipsec_init(mdev);
+}
+
+void mlx5_accel_ipsec_cleanup(struct mlx5_core_dev *mdev)
+{
+	mlx5_fpga_ipsec_cleanup(mdev);
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/accel/ipsec.h b/drivers/net/ethernet/mellanox/mlx5/core/accel/ipsec.h
new file mode 100644
index 000000000000..d6e20fea9554
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/accel/ipsec.h
@@ -0,0 +1,138 @@
+/*
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#ifndef __MLX5_ACCEL_IPSEC_H__
+#define __MLX5_ACCEL_IPSEC_H__
+
+#ifdef CONFIG_MLX5_ACCEL
+
+#include <linux/mlx5/driver.h>
+
+enum {
+	MLX5_ACCEL_IPSEC_DEVICE = BIT(1),
+	MLX5_ACCEL_IPSEC_IPV6 = BIT(2),
+	MLX5_ACCEL_IPSEC_ESP = BIT(3),
+	MLX5_ACCEL_IPSEC_LSO = BIT(4),
+};
+
+#define MLX5_IPSEC_SADB_IP_AH       BIT(7)
+#define MLX5_IPSEC_SADB_IP_ESP      BIT(6)
+#define MLX5_IPSEC_SADB_SA_VALID    BIT(5)
+#define MLX5_IPSEC_SADB_SPI_EN      BIT(4)
+#define MLX5_IPSEC_SADB_DIR_SX      BIT(3)
+#define MLX5_IPSEC_SADB_IPV6        BIT(2)
+
+enum {
+	MLX5_IPSEC_CMD_ADD_SA = 0,
+	MLX5_IPSEC_CMD_DEL_SA = 1,
+};
+
+enum mlx5_accel_ipsec_enc_mode {
+	MLX5_IPSEC_SADB_MODE_NONE = 0,
+	MLX5_IPSEC_SADB_MODE_AES_GCM_128_AUTH_128 = 1,
+	MLX5_IPSEC_SADB_MODE_AES_GCM_256_AUTH_128 = 3,
+};
+
+#define MLX5_IPSEC_DEV(mdev) (mlx5_accel_ipsec_device_caps(mdev) & \
+			      MLX5_ACCEL_IPSEC_DEVICE)
+
+struct mlx5_accel_ipsec_sa {
+	__be32 cmd;
+	u8 key_enc[32];
+	u8 key_auth[32];
+	__be32 sip[4];
+	__be32 dip[4];
+	union {
+		struct {
+			__be32 reserved;
+			u8 salt_iv[8];
+			__be32 salt;
+		} __packed gcm;
+		struct {
+			u8 salt[16];
+		} __packed cbc;
+	};
+	__be32 spi;
+	__be32 sw_sa_handle;
+	__be16 tfclen;
+	u8 enc_mode;
+	u8 sip_masklen;
+	u8 dip_masklen;
+	u8 flags;
+	u8 reserved[2];
+} __packed;
+
+/**
+ * mlx5_accel_ipsec_sa_cmd_exec - Execute an IPSec SADB command
+ * @mdev: mlx5 device
+ * @cmd: command to execute
+ * May be called from atomic context. Returns context pointer, or error
+ * Caller must eventually call mlx5_accel_ipsec_sa_cmd_wait from non-atomic
+ * context, to cleanup the context pointer
+ */
+void *mlx5_accel_ipsec_sa_cmd_exec(struct mlx5_core_dev *mdev,
+				   struct mlx5_accel_ipsec_sa *cmd);
+
+/**
+ * mlx5_accel_ipsec_sa_cmd_wait - Wait for command execution completion
+ * @context: Context pointer returned from call to mlx5_accel_ipsec_sa_cmd_exec
+ * Sleeps (killable) until command execution is complete.
+ * Returns the command result, or -EINTR if killed
+ */
+int mlx5_accel_ipsec_sa_cmd_wait(void *context);
+
+u32 mlx5_accel_ipsec_device_caps(struct mlx5_core_dev *mdev);
+
+unsigned int mlx5_accel_ipsec_counters_count(struct mlx5_core_dev *mdev);
+int mlx5_accel_ipsec_counters_read(struct mlx5_core_dev *mdev, u64 *counters,
+				   unsigned int count);
+
+int mlx5_accel_ipsec_init(struct mlx5_core_dev *mdev);
+void mlx5_accel_ipsec_cleanup(struct mlx5_core_dev *mdev);
+
+#else
+
+#define MLX5_IPSEC_DEV(mdev) false
+
+static inline int mlx5_accel_ipsec_init(struct mlx5_core_dev *mdev)
+{
+	return 0;
+}
+
+static inline void mlx5_accel_ipsec_cleanup(struct mlx5_core_dev *mdev)
+{
+}
+
+#endif
+
+#endif	/* __MLX5_ACCEL_IPSEC_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/alloc.c b/drivers/net/ethernet/mellanox/mlx5/core/alloc.c
index 66bd213f35ce..3c95f7f53802 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/alloc.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/alloc.c
@@ -274,7 +274,6 @@ void mlx5_db_free(struct mlx5_core_dev *dev, struct mlx5_db *db)
 }
 EXPORT_SYMBOL_GPL(mlx5_db_free);
 
-
 void mlx5_fill_page_array(struct mlx5_buf *buf, __be64 *pas)
 {
 	u64 addr;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/cmd.c b/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
index 10d282841f5b..f5a2c605749f 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
@@ -217,7 +217,6 @@ static void free_cmd(struct mlx5_cmd_work_ent *ent)
 	kfree(ent);
 }
 
-
 static int verify_signature(struct mlx5_cmd_work_ent *ent)
 {
 	struct mlx5_cmd_mailbox *next = ent->out->next;
@@ -308,6 +307,7 @@ static int mlx5_internal_err_ret_value(struct mlx5_core_dev *dev, u16 op,
 	case MLX5_CMD_OP_SET_FLOW_TABLE_ROOT:
 	case MLX5_CMD_OP_DEALLOC_ENCAP_HEADER:
 	case MLX5_CMD_OP_DEALLOC_MODIFY_HEADER_CONTEXT:
+	case MLX5_CMD_OP_FPGA_DESTROY_QP:
 		return MLX5_CMD_STAT_OK;
 
 	case MLX5_CMD_OP_QUERY_HCA_CAP:
@@ -420,6 +420,10 @@ static int mlx5_internal_err_ret_value(struct mlx5_core_dev *dev, u16 op,
 	case MLX5_CMD_OP_QUERY_FLOW_COUNTER:
 	case MLX5_CMD_OP_ALLOC_ENCAP_HEADER:
 	case MLX5_CMD_OP_ALLOC_MODIFY_HEADER_CONTEXT:
+	case MLX5_CMD_OP_FPGA_CREATE_QP:
+	case MLX5_CMD_OP_FPGA_MODIFY_QP:
+	case MLX5_CMD_OP_FPGA_QUERY_QP:
+	case MLX5_CMD_OP_FPGA_QUERY_QP_COUNTERS:
 		*status = MLX5_DRIVER_STATUS_ABORTED;
 		*synd = MLX5_DRIVER_SYND;
 		return -EIO;
@@ -586,6 +590,11 @@ const char *mlx5_command_str(int command)
 	MLX5_COMMAND_STR_CASE(DEALLOC_ENCAP_HEADER);
 	MLX5_COMMAND_STR_CASE(ALLOC_MODIFY_HEADER_CONTEXT);
 	MLX5_COMMAND_STR_CASE(DEALLOC_MODIFY_HEADER_CONTEXT);
+	MLX5_COMMAND_STR_CASE(FPGA_CREATE_QP);
+	MLX5_COMMAND_STR_CASE(FPGA_MODIFY_QP);
+	MLX5_COMMAND_STR_CASE(FPGA_QUERY_QP);
+	MLX5_COMMAND_STR_CASE(FPGA_QUERY_QP_COUNTERS);
+	MLX5_COMMAND_STR_CASE(FPGA_DESTROY_QP);
 	default: return "unknown command opcode";
 	}
 }
@@ -786,6 +795,8 @@ static void cmd_work_handler(struct work_struct *work)
 	struct mlx5_cmd_layout *lay;
 	struct semaphore *sem;
 	unsigned long flags;
+	bool poll_cmd = ent->polling;
+
 
 	sem = ent->page_queue ? &cmd->pages_sem : &cmd->sem;
 	down(sem);
@@ -846,7 +857,7 @@ static void cmd_work_handler(struct work_struct *work)
 	iowrite32be(1 << ent->idx, &dev->iseg->cmd_dbell);
 	mmiowb();
 	/* if not in polling don't use ent after this point */
-	if (cmd->mode == CMD_MODE_POLLING) {
+	if (cmd->mode == CMD_MODE_POLLING || poll_cmd) {
 		poll_timeout(ent);
 		/* make sure we read the descriptor after ownership is SW */
 		rmb();
@@ -874,7 +885,7 @@ static const char *deliv_status_to_str(u8 status)
 	case MLX5_CMD_DELIVERY_STAT_IN_LENGTH_ERR:
 		return "command input length error";
 	case MLX5_CMD_DELIVERY_STAT_OUT_LENGTH_ERR:
-		return "command ouput length error";
+		return "command output length error";
 	case MLX5_CMD_DELIVERY_STAT_RES_FLD_NOT_CLR_ERR:
 		return "reserved fields not cleared";
 	case MLX5_CMD_DELIVERY_STAT_CMD_DESCR_ERR:
@@ -890,7 +901,7 @@ static int wait_func(struct mlx5_core_dev *dev, struct mlx5_cmd_work_ent *ent)
 	struct mlx5_cmd *cmd = &dev->cmd;
 	int err;
 
-	if (cmd->mode == CMD_MODE_POLLING) {
+	if (cmd->mode == CMD_MODE_POLLING || ent->polling) {
 		wait_for_completion(&ent->done);
 	} else if (!wait_for_completion_timeout(&ent->done, timeout)) {
 		ent->ret = -ETIMEDOUT;
@@ -918,7 +929,7 @@ static int mlx5_cmd_invoke(struct mlx5_core_dev *dev, struct mlx5_cmd_msg *in,
 			   struct mlx5_cmd_msg *out, void *uout, int uout_size,
 			   mlx5_cmd_cbk_t callback,
 			   void *context, int page_queue, u8 *status,
-			   u8 token)
+			   u8 token, bool force_polling)
 {
 	struct mlx5_cmd *cmd = &dev->cmd;
 	struct mlx5_cmd_work_ent *ent;
@@ -936,6 +947,7 @@ static int mlx5_cmd_invoke(struct mlx5_core_dev *dev, struct mlx5_cmd_msg *in,
 		return PTR_ERR(ent);
 
 	ent->token = token;
+	ent->polling = force_polling;
 
 	if (!callback)
 		init_completion(&ent->done);
@@ -1001,7 +1013,6 @@ static ssize_t dbg_write(struct file *filp, const char __user *buf,
 	return err ? err : count;
 }
 
-
 static const struct file_operations fops = {
 	.owner	= THIS_MODULE,
 	.open	= simple_open,
@@ -1153,7 +1164,7 @@ err_alloc:
 }
 
 static void mlx5_free_cmd_msg(struct mlx5_core_dev *dev,
-				  struct mlx5_cmd_msg *msg)
+			      struct mlx5_cmd_msg *msg)
 {
 	struct mlx5_cmd_mailbox *head = msg->next;
 	struct mlx5_cmd_mailbox *next;
@@ -1537,7 +1548,8 @@ static int is_manage_pages(void *in)
 }
 
 static int cmd_exec(struct mlx5_core_dev *dev, void *in, int in_size, void *out,
-		    int out_size, mlx5_cmd_cbk_t callback, void *context)
+		    int out_size, mlx5_cmd_cbk_t callback, void *context,
+		    bool force_polling)
 {
 	struct mlx5_cmd_msg *inb;
 	struct mlx5_cmd_msg *outb;
@@ -1582,7 +1594,7 @@ static int cmd_exec(struct mlx5_core_dev *dev, void *in, int in_size, void *out,
 	}
 
 	err = mlx5_cmd_invoke(dev, inb, outb, out, out_size, callback, context,
-			      pages_queue, &status, token);
+			      pages_queue, &status, token, force_polling);
 	if (err)
 		goto out_out;
 
@@ -1610,7 +1622,7 @@ int mlx5_cmd_exec(struct mlx5_core_dev *dev, void *in, int in_size, void *out,
 {
 	int err;
 
-	err = cmd_exec(dev, in, in_size, out, out_size, NULL, NULL);
+	err = cmd_exec(dev, in, in_size, out, out_size, NULL, NULL, false);
 	return err ? : mlx5_cmd_check(dev, in, out);
 }
 EXPORT_SYMBOL(mlx5_cmd_exec);
@@ -1619,10 +1631,22 @@ int mlx5_cmd_exec_cb(struct mlx5_core_dev *dev, void *in, int in_size,
 		     void *out, int out_size, mlx5_cmd_cbk_t callback,
 		     void *context)
 {
-	return cmd_exec(dev, in, in_size, out, out_size, callback, context);
+	return cmd_exec(dev, in, in_size, out, out_size, callback, context,
+			false);
 }
 EXPORT_SYMBOL(mlx5_cmd_exec_cb);
 
+int mlx5_cmd_exec_polling(struct mlx5_core_dev *dev, void *in, int in_size,
+			  void *out, int out_size)
+{
+	int err;
+
+	err = cmd_exec(dev, in, in_size, out, out_size, NULL, NULL, true);
+
+	return err ? : mlx5_cmd_check(dev, in, out);
+}
+EXPORT_SYMBOL(mlx5_cmd_exec_polling);
+
 static void destroy_msg_cache(struct mlx5_core_dev *dev)
 {
 	struct cmd_msg_cache *ch;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c b/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c
index e94a9532e218..7ecadb501743 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c
@@ -168,7 +168,6 @@ static ssize_t average_read(struct file *filp, char __user *buf, size_t count,
 	return ret;
 }
 
-
 static ssize_t average_write(struct file *filp, const char __user *buf,
 			     size_t count, loff_t *pos)
 {
@@ -405,7 +404,7 @@ static u64 cq_read_field(struct mlx5_core_dev *dev, struct mlx5_core_cq *cq,
 	u32 *out;
 	int err;
 
-	out = mlx5_vzalloc(outlen);
+	out = kvzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return param;
 
@@ -466,7 +465,6 @@ static ssize_t dbg_read(struct file *filp, char __user *buf, size_t count,
 		return -EINVAL;
 	}
 
-
 	if (is_str)
 		ret = snprintf(tbuf, sizeof(tbuf), "%s\n", (const char *)(unsigned long)field);
 	else
@@ -562,7 +560,6 @@ void mlx5_debug_qp_remove(struct mlx5_core_dev *dev, struct mlx5_core_qp *qp)
 		rem_res_tree(qp->dbg);
 }
 
-
 int mlx5_debug_eq_add(struct mlx5_core_dev *dev, struct mlx5_eq *eq)
 {
 	int err;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en.h b/drivers/net/ethernet/mellanox/mlx5/core/en.h
index 2fd044b23875..e1b7ddfecd01 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en.h
@@ -52,8 +52,10 @@
 
 #define MLX5_SET_CFG(p, f, v) MLX5_SET(create_flow_group_in, p, f, v)
 
-#define MLX5E_HW2SW_MTU(hwmtu) ((hwmtu) - (ETH_HLEN + VLAN_HLEN + ETH_FCS_LEN))
-#define MLX5E_SW2HW_MTU(swmtu) ((swmtu) + (ETH_HLEN + VLAN_HLEN + ETH_FCS_LEN))
+#define MLX5E_ETH_HARD_MTU (ETH_HLEN + VLAN_HLEN + ETH_FCS_LEN)
+
+#define MLX5E_HW2SW_MTU(priv, hwmtu) ((hwmtu) - ((priv)->hard_mtu))
+#define MLX5E_SW2HW_MTU(priv, swmtu) ((swmtu) + ((priv)->hard_mtu))
 
 #define MLX5E_MAX_NUM_TC	8
 
@@ -70,6 +72,8 @@
 #define MLX5E_PARAMS_MAXIMUM_LOG_RQ_SIZE_MPW            0x6
 
 #define MLX5_RX_HEADROOM NET_SKB_PAD
+#define MLX5_SKB_FRAG_SZ(len)	(SKB_DATA_ALIGN(len) +	\
+				 SKB_DATA_ALIGN(sizeof(struct skb_shared_info)))
 
 #define MLX5_MPWRQ_MIN_LOG_STRIDE_SZ(mdev) \
 	(6 + MLX5_CAP_GEN(mdev, cache_line_128byte)) /* HW restriction */
@@ -213,6 +217,7 @@ struct mlx5e_cq_moder {
 struct mlx5e_params {
 	u8  log_sq_size;
 	u8  rq_wq_type;
+	u16 rq_headroom;
 	u8  mpwqe_log_stride_sz;
 	u8  mpwqe_log_num_strides;
 	u8  log_rq_size;
@@ -323,6 +328,7 @@ struct mlx5e_sq_dma {
 
 enum {
 	MLX5E_SQ_STATE_ENABLED,
+	MLX5E_SQ_STATE_IPSEC,
 };
 
 struct mlx5e_sq_wqe_info {
@@ -443,6 +449,11 @@ struct mlx5e_dma_info {
 	dma_addr_t	addr;
 };
 
+struct mlx5e_wqe_frag_info {
+	struct mlx5e_dma_info di;
+	u32 offset;
+};
+
 struct mlx5e_umr_dma_info {
 	__be64                *mtt;
 	dma_addr_t             mtt_addr;
@@ -458,13 +469,15 @@ struct mlx5e_mpw_info {
 
 struct mlx5e_rx_am_stats {
 	int ppms; /* packets per msec */
+	int bpms; /* bytes per msec */
 	int epms; /* events per msec */
 };
 
 struct mlx5e_rx_am_sample {
-	ktime_t		time;
-	unsigned int	pkt_ctr;
-	u16		event_ctr;
+	ktime_t	time;
+	u32	pkt_ctr;
+	u32	byte_ctr;
+	u16	event_ctr;
 };
 
 struct mlx5e_rx_am { /* Adaptive Moderation */
@@ -502,7 +515,12 @@ struct mlx5e_rq {
 	struct mlx5_wq_ll      wq;
 
 	union {
-		struct mlx5e_dma_info *dma_info;
+		struct {
+			struct mlx5e_wqe_frag_info *frag_info;
+			u32 frag_sz;	/* max possible skb frag_sz */
+			bool page_reuse;
+			bool xdp_xmit;
+		} wqe;
 		struct {
 			struct mlx5e_mpw_info *info;
 			void                  *mtt_no_align;
@@ -623,6 +641,8 @@ struct mlx5e_tc_table {
 
 	struct rhashtable_params        ht_params;
 	struct rhashtable               ht;
+
+	DECLARE_HASHTABLE(mod_hdr_tbl, 8);
 };
 
 struct mlx5e_vlan_table {
@@ -743,6 +763,7 @@ struct mlx5e_priv {
 	struct mlx5e_tir           indir_tir[MLX5E_NUM_INDIR_TIRS];
 	struct mlx5e_tir           direct_tir[MLX5E_MAX_NUM_CHANNELS];
 	u32                        tx_rates[MLX5E_MAX_NUM_SQS];
+	int                        hard_mtu;
 
 	struct mlx5e_flow_steering fs;
 	struct mlx5e_vxlan_db      vxlan;
@@ -764,6 +785,9 @@ struct mlx5e_priv {
 
 	const struct mlx5e_profile *profile;
 	void                      *ppriv;
+#ifdef CONFIG_MLX5_EN_IPSEC
+	struct mlx5e_ipsec        *ipsec;
+#endif
 };
 
 struct mlx5e_profile {
@@ -778,6 +802,7 @@ struct mlx5e_profile {
 	void	(*enable)(struct mlx5e_priv *priv);
 	void	(*disable)(struct mlx5e_priv *priv);
 	void	(*update_stats)(struct mlx5e_priv *priv);
+	void	(*update_carrier)(struct mlx5e_priv *priv);
 	int	(*max_nch)(struct mlx5_core_dev *mdev);
 	struct {
 		mlx5e_fp_handle_rx_cqe handle_rx_cqe;
@@ -812,13 +837,12 @@ void mlx5e_dealloc_rx_wqe(struct mlx5e_rq *rq, u16 ix);
 void mlx5e_dealloc_rx_mpwqe(struct mlx5e_rq *rq, u16 ix);
 void mlx5e_post_rx_mpwqe(struct mlx5e_rq *rq);
 void mlx5e_free_rx_mpwqe(struct mlx5e_rq *rq, struct mlx5e_mpw_info *wi);
-struct mlx5_cqe64 *mlx5e_get_cqe(struct mlx5e_cq *cq);
 
 void mlx5e_rx_am(struct mlx5e_rq *rq);
 void mlx5e_rx_am_work(struct work_struct *work);
 struct mlx5e_cq_moder mlx5e_am_get_def_profile(u8 rx_cq_period_mode);
 
-void mlx5e_update_stats(struct mlx5e_priv *priv);
+void mlx5e_update_stats(struct mlx5e_priv *priv, bool full);
 
 int mlx5e_create_flow_steering(struct mlx5e_priv *priv);
 void mlx5e_destroy_flow_steering(struct mlx5e_priv *priv);
@@ -845,8 +869,8 @@ void mlx5e_timestamp_init(struct mlx5e_priv *priv);
 void mlx5e_timestamp_cleanup(struct mlx5e_priv *priv);
 void mlx5e_pps_event_handler(struct mlx5e_priv *priv,
 			     struct ptp_clock_event *event);
-int mlx5e_hwstamp_set(struct net_device *dev, struct ifreq *ifr);
-int mlx5e_hwstamp_get(struct net_device *dev, struct ifreq *ifr);
+int mlx5e_hwstamp_set(struct mlx5e_priv *priv, struct ifreq *ifr);
+int mlx5e_hwstamp_get(struct mlx5e_priv *priv, struct ifreq *ifr);
 int mlx5e_modify_rx_cqe_compression_locked(struct mlx5e_priv *priv, bool val);
 
 int mlx5e_vlan_rx_add_vid(struct net_device *dev, __always_unused __be16 proto,
@@ -1017,6 +1041,31 @@ int mlx5e_open(struct net_device *netdev);
 void mlx5e_update_stats_work(struct work_struct *work);
 u32 mlx5e_choose_lro_timeout(struct mlx5_core_dev *mdev, u32 wanted_timeout);
 
+/* ethtool helpers */
+void mlx5e_ethtool_get_drvinfo(struct mlx5e_priv *priv,
+			       struct ethtool_drvinfo *drvinfo);
+void mlx5e_ethtool_get_strings(struct mlx5e_priv *priv,
+			       uint32_t stringset, uint8_t *data);
+int mlx5e_ethtool_get_sset_count(struct mlx5e_priv *priv, int sset);
+void mlx5e_ethtool_get_ethtool_stats(struct mlx5e_priv *priv,
+				     struct ethtool_stats *stats, u64 *data);
+void mlx5e_ethtool_get_ringparam(struct mlx5e_priv *priv,
+				 struct ethtool_ringparam *param);
+int mlx5e_ethtool_set_ringparam(struct mlx5e_priv *priv,
+				struct ethtool_ringparam *param);
+void mlx5e_ethtool_get_channels(struct mlx5e_priv *priv,
+				struct ethtool_channels *ch);
+int mlx5e_ethtool_set_channels(struct mlx5e_priv *priv,
+			       struct ethtool_channels *ch);
+int mlx5e_ethtool_get_coalesce(struct mlx5e_priv *priv,
+			       struct ethtool_coalesce *coal);
+int mlx5e_ethtool_set_coalesce(struct mlx5e_priv *priv,
+			       struct ethtool_coalesce *coal);
+int mlx5e_ethtool_get_ts_info(struct mlx5e_priv *priv,
+			      struct ethtool_ts_info *info);
+int mlx5e_ethtool_flash_device(struct mlx5e_priv *priv,
+			       struct ethtool_flash *flash);
+
 /* mlx5e generic netdev management API */
 struct net_device*
 mlx5e_create_netdev(struct mlx5_core_dev *mdev, const struct mlx5e_profile *profile,
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec.c b/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec.c
new file mode 100644
index 000000000000..bac5103efad3
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec.c
@@ -0,0 +1,461 @@
+/*
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#include <crypto/internal/geniv.h>
+#include <crypto/aead.h>
+#include <linux/inetdevice.h>
+#include <linux/netdevice.h>
+#include <linux/module.h>
+
+#include "en.h"
+#include "accel/ipsec.h"
+#include "en_accel/ipsec.h"
+#include "en_accel/ipsec_rxtx.h"
+
+struct mlx5e_ipsec_sa_entry {
+	struct hlist_node hlist; /* Item in SADB_RX hashtable */
+	unsigned int handle; /* Handle in SADB_RX */
+	struct xfrm_state *x;
+	struct mlx5e_ipsec *ipsec;
+	void *context;
+};
+
+struct xfrm_state *mlx5e_ipsec_sadb_rx_lookup(struct mlx5e_ipsec *ipsec,
+					      unsigned int handle)
+{
+	struct mlx5e_ipsec_sa_entry *sa_entry;
+	struct xfrm_state *ret = NULL;
+
+	rcu_read_lock();
+	hash_for_each_possible_rcu(ipsec->sadb_rx, sa_entry, hlist, handle)
+		if (sa_entry->handle == handle) {
+			ret = sa_entry->x;
+			xfrm_state_hold(ret);
+			break;
+		}
+	rcu_read_unlock();
+
+	return ret;
+}
+
+static int mlx5e_ipsec_sadb_rx_add(struct mlx5e_ipsec_sa_entry *sa_entry)
+{
+	struct mlx5e_ipsec *ipsec = sa_entry->ipsec;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&ipsec->sadb_rx_lock, flags);
+	ret = ida_simple_get(&ipsec->halloc, 1, 0, GFP_KERNEL);
+	if (ret < 0)
+		goto out;
+
+	sa_entry->handle = ret;
+	hash_add_rcu(ipsec->sadb_rx, &sa_entry->hlist, sa_entry->handle);
+	ret = 0;
+
+out:
+	spin_unlock_irqrestore(&ipsec->sadb_rx_lock, flags);
+	return ret;
+}
+
+static void mlx5e_ipsec_sadb_rx_del(struct mlx5e_ipsec_sa_entry *sa_entry)
+{
+	struct mlx5e_ipsec *ipsec = sa_entry->ipsec;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ipsec->sadb_rx_lock, flags);
+	hash_del_rcu(&sa_entry->hlist);
+	spin_unlock_irqrestore(&ipsec->sadb_rx_lock, flags);
+}
+
+static void mlx5e_ipsec_sadb_rx_free(struct mlx5e_ipsec_sa_entry *sa_entry)
+{
+	struct mlx5e_ipsec *ipsec = sa_entry->ipsec;
+	unsigned long flags;
+
+	/* Wait for the hash_del_rcu call in sadb_rx_del to affect data path */
+	synchronize_rcu();
+	spin_lock_irqsave(&ipsec->sadb_rx_lock, flags);
+	ida_simple_remove(&ipsec->halloc, sa_entry->handle);
+	spin_unlock_irqrestore(&ipsec->sadb_rx_lock, flags);
+}
+
+static enum mlx5_accel_ipsec_enc_mode mlx5e_ipsec_enc_mode(struct xfrm_state *x)
+{
+	unsigned int key_len = (x->aead->alg_key_len + 7) / 8 - 4;
+
+	switch (key_len) {
+	case 16:
+		return MLX5_IPSEC_SADB_MODE_AES_GCM_128_AUTH_128;
+	case 32:
+		return MLX5_IPSEC_SADB_MODE_AES_GCM_256_AUTH_128;
+	default:
+		netdev_warn(x->xso.dev, "Bad key len: %d for alg %s\n",
+			    key_len, x->aead->alg_name);
+		return -1;
+	}
+}
+
+static void mlx5e_ipsec_build_hw_sa(u32 op, struct mlx5e_ipsec_sa_entry *sa_entry,
+				    struct mlx5_accel_ipsec_sa *hw_sa)
+{
+	struct xfrm_state *x = sa_entry->x;
+	struct aead_geniv_ctx *geniv_ctx;
+	unsigned int crypto_data_len;
+	struct crypto_aead *aead;
+	unsigned int key_len;
+	int ivsize;
+
+	memset(hw_sa, 0, sizeof(*hw_sa));
+
+	if (op == MLX5_IPSEC_CMD_ADD_SA) {
+		crypto_data_len = (x->aead->alg_key_len + 7) / 8;
+		key_len = crypto_data_len - 4; /* 4 bytes salt at end */
+		aead = x->data;
+		geniv_ctx = crypto_aead_ctx(aead);
+		ivsize = crypto_aead_ivsize(aead);
+
+		memcpy(&hw_sa->key_enc, x->aead->alg_key, key_len);
+		/* Duplicate 128 bit key twice according to HW layout */
+		if (key_len == 16)
+			memcpy(&hw_sa->key_enc[16], x->aead->alg_key, key_len);
+		memcpy(&hw_sa->gcm.salt_iv, geniv_ctx->salt, ivsize);
+		hw_sa->gcm.salt = *((__be32 *)(x->aead->alg_key + key_len));
+	}
+
+	hw_sa->cmd = htonl(op);
+	hw_sa->flags |= MLX5_IPSEC_SADB_SA_VALID | MLX5_IPSEC_SADB_SPI_EN;
+	if (x->props.family == AF_INET) {
+		hw_sa->sip[3] = x->props.saddr.a4;
+		hw_sa->dip[3] = x->id.daddr.a4;
+		hw_sa->sip_masklen = 32;
+		hw_sa->dip_masklen = 32;
+	} else {
+		memcpy(hw_sa->sip, x->props.saddr.a6, sizeof(hw_sa->sip));
+		memcpy(hw_sa->dip, x->id.daddr.a6, sizeof(hw_sa->dip));
+		hw_sa->sip_masklen = 128;
+		hw_sa->dip_masklen = 128;
+		hw_sa->flags |= MLX5_IPSEC_SADB_IPV6;
+	}
+	hw_sa->spi = x->id.spi;
+	hw_sa->sw_sa_handle = htonl(sa_entry->handle);
+	switch (x->id.proto) {
+	case IPPROTO_ESP:
+		hw_sa->flags |= MLX5_IPSEC_SADB_IP_ESP;
+		break;
+	case IPPROTO_AH:
+		hw_sa->flags |= MLX5_IPSEC_SADB_IP_AH;
+		break;
+	default:
+		break;
+	}
+	hw_sa->enc_mode = mlx5e_ipsec_enc_mode(x);
+	if (!(x->xso.flags & XFRM_OFFLOAD_INBOUND))
+		hw_sa->flags |= MLX5_IPSEC_SADB_DIR_SX;
+}
+
+static inline int mlx5e_xfrm_validate_state(struct xfrm_state *x)
+{
+	struct net_device *netdev = x->xso.dev;
+	struct mlx5e_priv *priv;
+
+	priv = netdev_priv(netdev);
+
+	if (x->props.aalgo != SADB_AALG_NONE) {
+		netdev_info(netdev, "Cannot offload authenticated xfrm states\n");
+		return -EINVAL;
+	}
+	if (x->props.ealgo != SADB_X_EALG_AES_GCM_ICV16) {
+		netdev_info(netdev, "Only AES-GCM-ICV16 xfrm state may be offloaded\n");
+		return -EINVAL;
+	}
+	if (x->props.calgo != SADB_X_CALG_NONE) {
+		netdev_info(netdev, "Cannot offload compressed xfrm states\n");
+		return -EINVAL;
+	}
+	if (x->props.flags & XFRM_STATE_ESN) {
+		netdev_info(netdev, "Cannot offload ESN xfrm states\n");
+		return -EINVAL;
+	}
+	if (x->props.family != AF_INET &&
+	    x->props.family != AF_INET6) {
+		netdev_info(netdev, "Only IPv4/6 xfrm states may be offloaded\n");
+		return -EINVAL;
+	}
+	if (x->props.mode != XFRM_MODE_TRANSPORT &&
+	    x->props.mode != XFRM_MODE_TUNNEL) {
+		dev_info(&netdev->dev, "Only transport and tunnel xfrm states may be offloaded\n");
+		return -EINVAL;
+	}
+	if (x->id.proto != IPPROTO_ESP) {
+		netdev_info(netdev, "Only ESP xfrm state may be offloaded\n");
+		return -EINVAL;
+	}
+	if (x->encap) {
+		netdev_info(netdev, "Encapsulated xfrm state may not be offloaded\n");
+		return -EINVAL;
+	}
+	if (!x->aead) {
+		netdev_info(netdev, "Cannot offload xfrm states without aead\n");
+		return -EINVAL;
+	}
+	if (x->aead->alg_icv_len != 128) {
+		netdev_info(netdev, "Cannot offload xfrm states with AEAD ICV length other than 128bit\n");
+		return -EINVAL;
+	}
+	if ((x->aead->alg_key_len != 128 + 32) &&
+	    (x->aead->alg_key_len != 256 + 32)) {
+		netdev_info(netdev, "Cannot offload xfrm states with AEAD key length other than 128/256 bit\n");
+		return -EINVAL;
+	}
+	if (x->tfcpad) {
+		netdev_info(netdev, "Cannot offload xfrm states with tfc padding\n");
+		return -EINVAL;
+	}
+	if (!x->geniv) {
+		netdev_info(netdev, "Cannot offload xfrm states without geniv\n");
+		return -EINVAL;
+	}
+	if (strcmp(x->geniv, "seqiv")) {
+		netdev_info(netdev, "Cannot offload xfrm states with geniv other than seqiv\n");
+		return -EINVAL;
+	}
+	if (x->props.family == AF_INET6 &&
+	    !(mlx5_accel_ipsec_device_caps(priv->mdev) & MLX5_ACCEL_IPSEC_IPV6)) {
+		netdev_info(netdev, "IPv6 xfrm state offload is not supported by this device\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int mlx5e_xfrm_add_state(struct xfrm_state *x)
+{
+	struct mlx5e_ipsec_sa_entry *sa_entry = NULL;
+	struct net_device *netdev = x->xso.dev;
+	struct mlx5_accel_ipsec_sa hw_sa;
+	struct mlx5e_priv *priv;
+	void *context;
+	int err;
+
+	priv = netdev_priv(netdev);
+
+	err = mlx5e_xfrm_validate_state(x);
+	if (err)
+		return err;
+
+	sa_entry = kzalloc(sizeof(*sa_entry), GFP_KERNEL);
+	if (!sa_entry) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	sa_entry->x = x;
+	sa_entry->ipsec = priv->ipsec;
+
+	/* Add the SA to handle processed incoming packets before the add SA
+	 * completion was received
+	 */
+	if (x->xso.flags & XFRM_OFFLOAD_INBOUND) {
+		err = mlx5e_ipsec_sadb_rx_add(sa_entry);
+		if (err) {
+			netdev_info(netdev, "Failed adding to SADB_RX: %d\n", err);
+			goto err_entry;
+		}
+	}
+
+	mlx5e_ipsec_build_hw_sa(MLX5_IPSEC_CMD_ADD_SA, sa_entry, &hw_sa);
+	context = mlx5_accel_ipsec_sa_cmd_exec(sa_entry->ipsec->en_priv->mdev, &hw_sa);
+	if (IS_ERR(context)) {
+		err = PTR_ERR(context);
+		goto err_sadb_rx;
+	}
+
+	err = mlx5_accel_ipsec_sa_cmd_wait(context);
+	if (err)
+		goto err_sadb_rx;
+
+	x->xso.offload_handle = (unsigned long)sa_entry;
+	goto out;
+
+err_sadb_rx:
+	if (x->xso.flags & XFRM_OFFLOAD_INBOUND) {
+		mlx5e_ipsec_sadb_rx_del(sa_entry);
+		mlx5e_ipsec_sadb_rx_free(sa_entry);
+	}
+err_entry:
+	kfree(sa_entry);
+out:
+	return err;
+}
+
+static void mlx5e_xfrm_del_state(struct xfrm_state *x)
+{
+	struct mlx5e_ipsec_sa_entry *sa_entry;
+	struct mlx5_accel_ipsec_sa hw_sa;
+	void *context;
+
+	if (!x->xso.offload_handle)
+		return;
+
+	sa_entry = (struct mlx5e_ipsec_sa_entry *)x->xso.offload_handle;
+	WARN_ON(sa_entry->x != x);
+
+	if (x->xso.flags & XFRM_OFFLOAD_INBOUND)
+		mlx5e_ipsec_sadb_rx_del(sa_entry);
+
+	mlx5e_ipsec_build_hw_sa(MLX5_IPSEC_CMD_DEL_SA, sa_entry, &hw_sa);
+	context = mlx5_accel_ipsec_sa_cmd_exec(sa_entry->ipsec->en_priv->mdev, &hw_sa);
+	if (IS_ERR(context))
+		return;
+
+	sa_entry->context = context;
+}
+
+static void mlx5e_xfrm_free_state(struct xfrm_state *x)
+{
+	struct mlx5e_ipsec_sa_entry *sa_entry;
+	int res;
+
+	if (!x->xso.offload_handle)
+		return;
+
+	sa_entry = (struct mlx5e_ipsec_sa_entry *)x->xso.offload_handle;
+	WARN_ON(sa_entry->x != x);
+
+	res = mlx5_accel_ipsec_sa_cmd_wait(sa_entry->context);
+	sa_entry->context = NULL;
+	if (res) {
+		/* Leftover object will leak */
+		return;
+	}
+
+	if (x->xso.flags & XFRM_OFFLOAD_INBOUND)
+		mlx5e_ipsec_sadb_rx_free(sa_entry);
+
+	kfree(sa_entry);
+}
+
+int mlx5e_ipsec_init(struct mlx5e_priv *priv)
+{
+	struct mlx5e_ipsec *ipsec = NULL;
+
+	if (!MLX5_IPSEC_DEV(priv->mdev)) {
+		netdev_dbg(priv->netdev, "Not an IPSec offload device\n");
+		return 0;
+	}
+
+	ipsec = kzalloc(sizeof(*ipsec), GFP_KERNEL);
+	if (!ipsec)
+		return -ENOMEM;
+
+	hash_init(ipsec->sadb_rx);
+	spin_lock_init(&ipsec->sadb_rx_lock);
+	ida_init(&ipsec->halloc);
+	ipsec->en_priv = priv;
+	ipsec->en_priv->ipsec = ipsec;
+	netdev_dbg(priv->netdev, "IPSec attached to netdevice\n");
+	return 0;
+}
+
+void mlx5e_ipsec_cleanup(struct mlx5e_priv *priv)
+{
+	struct mlx5e_ipsec *ipsec = priv->ipsec;
+
+	if (!ipsec)
+		return;
+
+	ida_destroy(&ipsec->halloc);
+	kfree(ipsec);
+	priv->ipsec = NULL;
+}
+
+static bool mlx5e_ipsec_offload_ok(struct sk_buff *skb, struct xfrm_state *x)
+{
+	if (x->props.family == AF_INET) {
+		/* Offload with IPv4 options is not supported yet */
+		if (ip_hdr(skb)->ihl > 5)
+			return false;
+	} else {
+		/* Offload with IPv6 extension headers is not support yet */
+		if (ipv6_ext_hdr(ipv6_hdr(skb)->nexthdr))
+			return false;
+	}
+
+	return true;
+}
+
+static const struct xfrmdev_ops mlx5e_ipsec_xfrmdev_ops = {
+	.xdo_dev_state_add	= mlx5e_xfrm_add_state,
+	.xdo_dev_state_delete	= mlx5e_xfrm_del_state,
+	.xdo_dev_state_free	= mlx5e_xfrm_free_state,
+	.xdo_dev_offload_ok	= mlx5e_ipsec_offload_ok,
+};
+
+void mlx5e_ipsec_build_netdev(struct mlx5e_priv *priv)
+{
+	struct mlx5_core_dev *mdev = priv->mdev;
+	struct net_device *netdev = priv->netdev;
+
+	if (!priv->ipsec)
+		return;
+
+	if (!(mlx5_accel_ipsec_device_caps(mdev) & MLX5_ACCEL_IPSEC_ESP) ||
+	    !MLX5_CAP_ETH(mdev, swp)) {
+		mlx5_core_dbg(mdev, "mlx5e: ESP and SWP offload not supported\n");
+		return;
+	}
+
+	mlx5_core_info(mdev, "mlx5e: IPSec ESP acceleration enabled\n");
+	netdev->xfrmdev_ops = &mlx5e_ipsec_xfrmdev_ops;
+	netdev->features |= NETIF_F_HW_ESP;
+	netdev->hw_enc_features |= NETIF_F_HW_ESP;
+
+	if (!MLX5_CAP_ETH(mdev, swp_csum)) {
+		mlx5_core_dbg(mdev, "mlx5e: SWP checksum not supported\n");
+		return;
+	}
+
+	netdev->features |= NETIF_F_HW_ESP_TX_CSUM;
+	netdev->hw_enc_features |= NETIF_F_HW_ESP_TX_CSUM;
+
+	if (!(mlx5_accel_ipsec_device_caps(mdev) & MLX5_ACCEL_IPSEC_LSO) ||
+	    !MLX5_CAP_ETH(mdev, swp_lso)) {
+		mlx5_core_dbg(mdev, "mlx5e: ESP LSO not supported\n");
+		return;
+	}
+
+	mlx5_core_dbg(mdev, "mlx5e: ESP GSO capability turned on\n");
+	netdev->features |= NETIF_F_GSO_ESP;
+	netdev->hw_features |= NETIF_F_GSO_ESP;
+	netdev->hw_enc_features |= NETIF_F_GSO_ESP;
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec.h b/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec.h
new file mode 100644
index 000000000000..56e00baf16cc
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec.h
@@ -0,0 +1,140 @@
+/*
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#ifndef __MLX5E_IPSEC_H__
+#define __MLX5E_IPSEC_H__
+
+#ifdef CONFIG_MLX5_EN_IPSEC
+
+#include <linux/mlx5/device.h>
+#include <net/xfrm.h>
+#include <linux/idr.h>
+
+#define MLX5E_IPSEC_SADB_RX_BITS 10
+#define MLX5E_METADATA_ETHER_TYPE (0x8CE4)
+#define MLX5E_METADATA_ETHER_LEN 8
+
+struct mlx5e_priv;
+
+struct mlx5e_ipsec_sw_stats {
+	atomic64_t ipsec_rx_drop_sp_alloc;
+	atomic64_t ipsec_rx_drop_sadb_miss;
+	atomic64_t ipsec_rx_drop_syndrome;
+	atomic64_t ipsec_tx_drop_bundle;
+	atomic64_t ipsec_tx_drop_no_state;
+	atomic64_t ipsec_tx_drop_not_ip;
+	atomic64_t ipsec_tx_drop_trailer;
+	atomic64_t ipsec_tx_drop_metadata;
+};
+
+struct mlx5e_ipsec_stats {
+	u64 ipsec_dec_in_packets;
+	u64 ipsec_dec_out_packets;
+	u64 ipsec_dec_bypass_packets;
+	u64 ipsec_enc_in_packets;
+	u64 ipsec_enc_out_packets;
+	u64 ipsec_enc_bypass_packets;
+	u64 ipsec_dec_drop_packets;
+	u64 ipsec_dec_auth_fail_packets;
+	u64 ipsec_enc_drop_packets;
+	u64 ipsec_add_sa_success;
+	u64 ipsec_add_sa_fail;
+	u64 ipsec_del_sa_success;
+	u64 ipsec_del_sa_fail;
+	u64 ipsec_cmd_drop;
+};
+
+struct mlx5e_ipsec {
+	struct mlx5e_priv *en_priv;
+	DECLARE_HASHTABLE(sadb_rx, MLX5E_IPSEC_SADB_RX_BITS);
+	spinlock_t sadb_rx_lock; /* Protects sadb_rx and halloc */
+	struct ida halloc;
+	struct mlx5e_ipsec_sw_stats sw_stats;
+	struct mlx5e_ipsec_stats stats;
+};
+
+void mlx5e_ipsec_build_inverse_table(void);
+int mlx5e_ipsec_init(struct mlx5e_priv *priv);
+void mlx5e_ipsec_cleanup(struct mlx5e_priv *priv);
+void mlx5e_ipsec_build_netdev(struct mlx5e_priv *priv);
+
+int mlx5e_ipsec_get_count(struct mlx5e_priv *priv);
+int mlx5e_ipsec_get_strings(struct mlx5e_priv *priv, uint8_t *data);
+void mlx5e_ipsec_update_stats(struct mlx5e_priv *priv);
+int mlx5e_ipsec_get_stats(struct mlx5e_priv *priv, u64 *data);
+
+struct xfrm_state *mlx5e_ipsec_sadb_rx_lookup(struct mlx5e_ipsec *dev,
+					      unsigned int handle);
+
+#else
+
+static inline void mlx5e_ipsec_build_inverse_table(void)
+{
+}
+
+static inline int mlx5e_ipsec_init(struct mlx5e_priv *priv)
+{
+	return 0;
+}
+
+static inline void mlx5e_ipsec_cleanup(struct mlx5e_priv *priv)
+{
+}
+
+static inline void mlx5e_ipsec_build_netdev(struct mlx5e_priv *priv)
+{
+}
+
+static inline int mlx5e_ipsec_get_count(struct mlx5e_priv *priv)
+{
+	return 0;
+}
+
+static inline int mlx5e_ipsec_get_strings(struct mlx5e_priv *priv,
+					  uint8_t *data)
+{
+	return 0;
+}
+
+static inline void mlx5e_ipsec_update_stats(struct mlx5e_priv *priv)
+{
+}
+
+static inline int mlx5e_ipsec_get_stats(struct mlx5e_priv *priv, u64 *data)
+{
+	return 0;
+}
+
+#endif
+
+#endif	/* __MLX5E_IPSEC_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec_rxtx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec_rxtx.c
new file mode 100644
index 000000000000..4a78aefdf157
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec_rxtx.c
@@ -0,0 +1,378 @@
+/*
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#include <crypto/aead.h>
+#include <net/xfrm.h>
+#include <net/esp.h>
+
+#include "en_accel/ipsec_rxtx.h"
+#include "en_accel/ipsec.h"
+#include "en.h"
+
+enum {
+	MLX5E_IPSEC_RX_SYNDROME_DECRYPTED = 0x11,
+	MLX5E_IPSEC_RX_SYNDROME_AUTH_FAILED = 0x12,
+};
+
+struct mlx5e_ipsec_rx_metadata {
+	unsigned char   reserved;
+	__be32		sa_handle;
+} __packed;
+
+enum {
+	MLX5E_IPSEC_TX_SYNDROME_OFFLOAD = 0x8,
+	MLX5E_IPSEC_TX_SYNDROME_OFFLOAD_WITH_LSO_TCP = 0x9,
+};
+
+struct mlx5e_ipsec_tx_metadata {
+	__be16 mss_inv;         /* 1/MSS in 16bit fixed point, only for LSO */
+	__be16 seq;             /* LSBs of the first TCP seq, only for LSO */
+	u8     esp_next_proto;  /* Next protocol of ESP */
+} __packed;
+
+struct mlx5e_ipsec_metadata {
+	unsigned char syndrome;
+	union {
+		unsigned char raw[5];
+		/* from FPGA to host, on successful decrypt */
+		struct mlx5e_ipsec_rx_metadata rx;
+		/* from host to FPGA */
+		struct mlx5e_ipsec_tx_metadata tx;
+	} __packed content;
+	/* packet type ID field	*/
+	__be16 ethertype;
+} __packed;
+
+#define MAX_LSO_MSS 2048
+
+/* Pre-calculated (Q0.16) fixed-point inverse 1/x function */
+static __be16 mlx5e_ipsec_inverse_table[MAX_LSO_MSS];
+
+static inline __be16 mlx5e_ipsec_mss_inv(struct sk_buff *skb)
+{
+	return mlx5e_ipsec_inverse_table[skb_shinfo(skb)->gso_size];
+}
+
+static struct mlx5e_ipsec_metadata *mlx5e_ipsec_add_metadata(struct sk_buff *skb)
+{
+	struct mlx5e_ipsec_metadata *mdata;
+	struct ethhdr *eth;
+
+	if (unlikely(skb_cow_head(skb, sizeof(*mdata))))
+		return ERR_PTR(-ENOMEM);
+
+	eth = (struct ethhdr *)skb_push(skb, sizeof(*mdata));
+	skb->mac_header -= sizeof(*mdata);
+	mdata = (struct mlx5e_ipsec_metadata *)(eth + 1);
+
+	memmove(skb->data, skb->data + sizeof(*mdata),
+		2 * ETH_ALEN);
+
+	eth->h_proto = cpu_to_be16(MLX5E_METADATA_ETHER_TYPE);
+
+	memset(mdata->content.raw, 0, sizeof(mdata->content.raw));
+	return mdata;
+}
+
+static int mlx5e_ipsec_remove_trailer(struct sk_buff *skb, struct xfrm_state *x)
+{
+	unsigned int alen = crypto_aead_authsize(x->data);
+	struct ipv6hdr *ipv6hdr = ipv6_hdr(skb);
+	struct iphdr *ipv4hdr = ip_hdr(skb);
+	unsigned int trailer_len;
+	u8 plen;
+	int ret;
+
+	ret = skb_copy_bits(skb, skb->len - alen - 2, &plen, 1);
+	if (unlikely(ret))
+		return ret;
+
+	trailer_len = alen + plen + 2;
+
+	pskb_trim(skb, skb->len - trailer_len);
+	if (skb->protocol == htons(ETH_P_IP)) {
+		ipv4hdr->tot_len = htons(ntohs(ipv4hdr->tot_len) - trailer_len);
+		ip_send_check(ipv4hdr);
+	} else {
+		ipv6hdr->payload_len = htons(ntohs(ipv6hdr->payload_len) -
+					     trailer_len);
+	}
+	return 0;
+}
+
+static void mlx5e_ipsec_set_swp(struct sk_buff *skb,
+				struct mlx5_wqe_eth_seg *eseg, u8 mode,
+				struct xfrm_offload *xo)
+{
+	u8 proto;
+
+	/* Tunnel Mode:
+	 * SWP:      OutL3       InL3  InL4
+	 * Pkt: MAC  IP     ESP  IP    L4
+	 *
+	 * Transport Mode:
+	 * SWP:      OutL3       InL4
+	 *           InL3
+	 * Pkt: MAC  IP     ESP  L4
+	 *
+	 * Offsets are in 2-byte words, counting from start of frame
+	 */
+	eseg->swp_outer_l3_offset = skb_network_offset(skb) / 2;
+	if (skb->protocol == htons(ETH_P_IPV6))
+		eseg->swp_flags |= MLX5_ETH_WQE_SWP_OUTER_L3_IPV6;
+
+	if (mode == XFRM_MODE_TUNNEL) {
+		eseg->swp_inner_l3_offset = skb_inner_network_offset(skb) / 2;
+		if (xo->proto == IPPROTO_IPV6) {
+			eseg->swp_flags |= MLX5_ETH_WQE_SWP_INNER_L3_IPV6;
+			proto = inner_ipv6_hdr(skb)->nexthdr;
+		} else {
+			proto = inner_ip_hdr(skb)->protocol;
+		}
+	} else {
+		eseg->swp_inner_l3_offset = skb_network_offset(skb) / 2;
+		if (skb->protocol == htons(ETH_P_IPV6))
+			eseg->swp_flags |= MLX5_ETH_WQE_SWP_INNER_L3_IPV6;
+		proto = xo->proto;
+	}
+	switch (proto) {
+	case IPPROTO_UDP:
+		eseg->swp_flags |= MLX5_ETH_WQE_SWP_INNER_L4_UDP;
+		/* Fall through */
+	case IPPROTO_TCP:
+		eseg->swp_inner_l4_offset = skb_inner_transport_offset(skb) / 2;
+		break;
+	}
+}
+
+static void mlx5e_ipsec_set_iv(struct sk_buff *skb, struct xfrm_offload *xo)
+{
+	int iv_offset;
+	__be64 seqno;
+
+	/* Place the SN in the IV field */
+	seqno = cpu_to_be64(xo->seq.low + ((u64)xo->seq.hi << 32));
+	iv_offset = skb_transport_offset(skb) + sizeof(struct ip_esp_hdr);
+	skb_store_bits(skb, iv_offset, &seqno, 8);
+}
+
+static void mlx5e_ipsec_set_metadata(struct sk_buff *skb,
+				     struct mlx5e_ipsec_metadata *mdata,
+				     struct xfrm_offload *xo)
+{
+	struct ip_esp_hdr *esph;
+	struct tcphdr *tcph;
+
+	if (skb_is_gso(skb)) {
+		/* Add LSO metadata indication */
+		esph = ip_esp_hdr(skb);
+		tcph = inner_tcp_hdr(skb);
+		netdev_dbg(skb->dev, "   Offloading GSO packet outer L3 %u; L4 %u; Inner L3 %u; L4 %u\n",
+			   skb->network_header,
+			   skb->transport_header,
+			   skb->inner_network_header,
+			   skb->inner_transport_header);
+		netdev_dbg(skb->dev, "   Offloading GSO packet of len %u; mss %u; TCP sp %u dp %u seq 0x%x ESP seq 0x%x\n",
+			   skb->len, skb_shinfo(skb)->gso_size,
+			   ntohs(tcph->source), ntohs(tcph->dest),
+			   ntohl(tcph->seq), ntohl(esph->seq_no));
+		mdata->syndrome = MLX5E_IPSEC_TX_SYNDROME_OFFLOAD_WITH_LSO_TCP;
+		mdata->content.tx.mss_inv = mlx5e_ipsec_mss_inv(skb);
+		mdata->content.tx.seq = htons(ntohl(tcph->seq) & 0xFFFF);
+	} else {
+		mdata->syndrome = MLX5E_IPSEC_TX_SYNDROME_OFFLOAD;
+	}
+	mdata->content.tx.esp_next_proto = xo->proto;
+
+	netdev_dbg(skb->dev, "   TX metadata syndrome %u proto %u mss_inv %04x seq %04x\n",
+		   mdata->syndrome, mdata->content.tx.esp_next_proto,
+		   ntohs(mdata->content.tx.mss_inv),
+		   ntohs(mdata->content.tx.seq));
+}
+
+struct sk_buff *mlx5e_ipsec_handle_tx_skb(struct net_device *netdev,
+					  struct mlx5e_tx_wqe *wqe,
+					  struct sk_buff *skb)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+	struct xfrm_offload *xo = xfrm_offload(skb);
+	struct mlx5e_ipsec_metadata *mdata;
+	struct xfrm_state *x;
+
+	if (!xo)
+		return skb;
+
+	if (unlikely(skb->sp->len != 1)) {
+		atomic64_inc(&priv->ipsec->sw_stats.ipsec_tx_drop_bundle);
+		goto drop;
+	}
+
+	x = xfrm_input_state(skb);
+	if (unlikely(!x)) {
+		atomic64_inc(&priv->ipsec->sw_stats.ipsec_tx_drop_no_state);
+		goto drop;
+	}
+
+	if (unlikely(!x->xso.offload_handle ||
+		     (skb->protocol != htons(ETH_P_IP) &&
+		      skb->protocol != htons(ETH_P_IPV6)))) {
+		atomic64_inc(&priv->ipsec->sw_stats.ipsec_tx_drop_not_ip);
+		goto drop;
+	}
+
+	if (!skb_is_gso(skb))
+		if (unlikely(mlx5e_ipsec_remove_trailer(skb, x))) {
+			atomic64_inc(&priv->ipsec->sw_stats.ipsec_tx_drop_trailer);
+			goto drop;
+		}
+	mdata = mlx5e_ipsec_add_metadata(skb);
+	if (unlikely(IS_ERR(mdata))) {
+		atomic64_inc(&priv->ipsec->sw_stats.ipsec_tx_drop_metadata);
+		goto drop;
+	}
+	mlx5e_ipsec_set_swp(skb, &wqe->eth, x->props.mode, xo);
+	mlx5e_ipsec_set_iv(skb, xo);
+	mlx5e_ipsec_set_metadata(skb, mdata, xo);
+
+	return skb;
+
+drop:
+	kfree_skb(skb);
+	return NULL;
+}
+
+static inline struct xfrm_state *
+mlx5e_ipsec_build_sp(struct net_device *netdev, struct sk_buff *skb,
+		     struct mlx5e_ipsec_metadata *mdata)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+	struct xfrm_offload *xo;
+	struct xfrm_state *xs;
+	u32 sa_handle;
+
+	skb->sp = secpath_dup(skb->sp);
+	if (unlikely(!skb->sp)) {
+		atomic64_inc(&priv->ipsec->sw_stats.ipsec_rx_drop_sp_alloc);
+		return NULL;
+	}
+
+	sa_handle = be32_to_cpu(mdata->content.rx.sa_handle);
+	xs = mlx5e_ipsec_sadb_rx_lookup(priv->ipsec, sa_handle);
+	if (unlikely(!xs)) {
+		atomic64_inc(&priv->ipsec->sw_stats.ipsec_rx_drop_sadb_miss);
+		return NULL;
+	}
+
+	skb->sp->xvec[skb->sp->len++] = xs;
+	skb->sp->olen++;
+
+	xo = xfrm_offload(skb);
+	xo->flags = CRYPTO_DONE;
+	switch (mdata->syndrome) {
+	case MLX5E_IPSEC_RX_SYNDROME_DECRYPTED:
+		xo->status = CRYPTO_SUCCESS;
+		break;
+	case MLX5E_IPSEC_RX_SYNDROME_AUTH_FAILED:
+		xo->status = CRYPTO_TUNNEL_ESP_AUTH_FAILED;
+		break;
+	default:
+		atomic64_inc(&priv->ipsec->sw_stats.ipsec_rx_drop_syndrome);
+		return NULL;
+	}
+	return xs;
+}
+
+struct sk_buff *mlx5e_ipsec_handle_rx_skb(struct net_device *netdev,
+					  struct sk_buff *skb)
+{
+	struct mlx5e_ipsec_metadata *mdata;
+	struct ethhdr *old_eth;
+	struct ethhdr *new_eth;
+	struct xfrm_state *xs;
+	__be16 *ethtype;
+
+	/* Detect inline metadata */
+	if (skb->len < ETH_HLEN + MLX5E_METADATA_ETHER_LEN)
+		return skb;
+	ethtype = (__be16 *)(skb->data + ETH_ALEN * 2);
+	if (*ethtype != cpu_to_be16(MLX5E_METADATA_ETHER_TYPE))
+		return skb;
+
+	/* Use the metadata */
+	mdata = (struct mlx5e_ipsec_metadata *)(skb->data + ETH_HLEN);
+	xs = mlx5e_ipsec_build_sp(netdev, skb, mdata);
+	if (unlikely(!xs)) {
+		kfree_skb(skb);
+		return NULL;
+	}
+
+	/* Remove the metadata from the buffer */
+	old_eth = (struct ethhdr *)skb->data;
+	new_eth = (struct ethhdr *)(skb->data + MLX5E_METADATA_ETHER_LEN);
+	memmove(new_eth, old_eth, 2 * ETH_ALEN);
+	/* Ethertype is already in its new place */
+	skb_pull_inline(skb, MLX5E_METADATA_ETHER_LEN);
+
+	return skb;
+}
+
+bool mlx5e_ipsec_feature_check(struct sk_buff *skb, struct net_device *netdev,
+			       netdev_features_t features)
+{
+	struct xfrm_state *x;
+
+	if (skb->sp && skb->sp->len) {
+		x = skb->sp->xvec[0];
+		if (x && x->xso.offload_handle)
+			return true;
+	}
+	return false;
+}
+
+void mlx5e_ipsec_build_inverse_table(void)
+{
+	u16 mss_inv;
+	u32 mss;
+
+	/* Calculate 1/x inverse table for use in GSO data path.
+	 * Using this table, we provide the IPSec accelerator with the value of
+	 * 1/gso_size so that it can infer the position of each segment inside
+	 * the GSO, and increment the ESP sequence number, and generate the IV.
+	 * The HW needs this value in Q0.16 fixed-point number format
+	 */
+	mlx5e_ipsec_inverse_table[1] = htons(0xFFFF);
+	for (mss = 2; mss < MAX_LSO_MSS; mss++) {
+		mss_inv = ((1ULL << 32) / mss) >> 16;
+		mlx5e_ipsec_inverse_table[mss] = htons(mss_inv);
+	}
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec_rxtx.h b/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec_rxtx.h
new file mode 100644
index 000000000000..e37ae2598dbb
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec_rxtx.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#ifndef __MLX5E_IPSEC_RXTX_H__
+#define __MLX5E_IPSEC_RXTX_H__
+
+#ifdef CONFIG_MLX5_EN_IPSEC
+
+#include <linux/skbuff.h>
+#include "en.h"
+
+struct sk_buff *mlx5e_ipsec_handle_rx_skb(struct net_device *netdev,
+					  struct sk_buff *skb);
+void mlx5e_ipsec_handle_rx_cqe(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe);
+
+void mlx5e_ipsec_inverse_table_init(void);
+bool mlx5e_ipsec_feature_check(struct sk_buff *skb, struct net_device *netdev,
+			       netdev_features_t features);
+struct sk_buff *mlx5e_ipsec_handle_tx_skb(struct net_device *netdev,
+					  struct mlx5e_tx_wqe *wqe,
+					  struct sk_buff *skb);
+
+#endif /* CONFIG_MLX5_EN_IPSEC */
+
+#endif /* __MLX5E_IPSEC_RXTX_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec_stats.c b/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec_stats.c
new file mode 100644
index 000000000000..6fea59223dc4
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_accel/ipsec_stats.c
@@ -0,0 +1,133 @@
+/*
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#include <linux/ethtool.h>
+#include <net/sock.h>
+
+#include "en.h"
+#include "accel/ipsec.h"
+#include "fpga/sdk.h"
+#include "en_accel/ipsec.h"
+
+static const struct counter_desc mlx5e_ipsec_hw_stats_desc[] = {
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_dec_in_packets) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_dec_out_packets) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_dec_bypass_packets) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_enc_in_packets) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_enc_out_packets) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_enc_bypass_packets) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_dec_drop_packets) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_dec_auth_fail_packets) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_enc_drop_packets) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_add_sa_success) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_add_sa_fail) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_del_sa_success) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_del_sa_fail) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_stats, ipsec_cmd_drop) },
+};
+
+static const struct counter_desc mlx5e_ipsec_sw_stats_desc[] = {
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_sw_stats, ipsec_rx_drop_sp_alloc) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_sw_stats, ipsec_rx_drop_sadb_miss) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_sw_stats, ipsec_rx_drop_syndrome) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_sw_stats, ipsec_tx_drop_bundle) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_sw_stats, ipsec_tx_drop_no_state) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_sw_stats, ipsec_tx_drop_not_ip) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_sw_stats, ipsec_tx_drop_trailer) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_ipsec_sw_stats, ipsec_tx_drop_metadata) },
+};
+
+#define MLX5E_READ_CTR_ATOMIC64(ptr, dsc, i) \
+	atomic64_read((atomic64_t *)((char *)(ptr) + (dsc)[i].offset))
+
+#define NUM_IPSEC_HW_COUNTERS ARRAY_SIZE(mlx5e_ipsec_hw_stats_desc)
+#define NUM_IPSEC_SW_COUNTERS ARRAY_SIZE(mlx5e_ipsec_sw_stats_desc)
+
+#define NUM_IPSEC_COUNTERS (NUM_IPSEC_HW_COUNTERS + NUM_IPSEC_SW_COUNTERS)
+
+int mlx5e_ipsec_get_count(struct mlx5e_priv *priv)
+{
+	if (!priv->ipsec)
+		return 0;
+
+	return NUM_IPSEC_COUNTERS;
+}
+
+int mlx5e_ipsec_get_strings(struct mlx5e_priv *priv, uint8_t *data)
+{
+	unsigned int i, idx = 0;
+
+	if (!priv->ipsec)
+		return 0;
+
+	for (i = 0; i < NUM_IPSEC_HW_COUNTERS; i++)
+		strcpy(data + (idx++) * ETH_GSTRING_LEN,
+		       mlx5e_ipsec_hw_stats_desc[i].format);
+
+	for (i = 0; i < NUM_IPSEC_SW_COUNTERS; i++)
+		strcpy(data + (idx++) * ETH_GSTRING_LEN,
+		       mlx5e_ipsec_sw_stats_desc[i].format);
+
+	return NUM_IPSEC_COUNTERS;
+}
+
+void mlx5e_ipsec_update_stats(struct mlx5e_priv *priv)
+{
+	int ret;
+
+	if (!priv->ipsec)
+		return;
+
+	ret = mlx5_accel_ipsec_counters_read(priv->mdev, (u64 *)&priv->ipsec->stats,
+					     NUM_IPSEC_HW_COUNTERS);
+	if (ret)
+		memset(&priv->ipsec->stats, 0, sizeof(priv->ipsec->stats));
+}
+
+int mlx5e_ipsec_get_stats(struct mlx5e_priv *priv, u64 *data)
+{
+	int i, idx = 0;
+
+	if (!priv->ipsec)
+		return 0;
+
+	for (i = 0; i < NUM_IPSEC_HW_COUNTERS; i++)
+		data[idx++] = MLX5E_READ_CTR64_CPU(&priv->ipsec->stats,
+						   mlx5e_ipsec_hw_stats_desc, i);
+
+	for (i = 0; i < NUM_IPSEC_SW_COUNTERS; i++)
+		data[idx++] = MLX5E_READ_CTR_ATOMIC64(&priv->ipsec->sw_stats,
+						      mlx5e_ipsec_sw_stats_desc, i);
+
+	return NUM_IPSEC_COUNTERS;
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_arfs.c b/drivers/net/ethernet/mellanox/mlx5/core/en_arfs.c
index c8a005326e30..12d3ced61114 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_arfs.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_arfs.c
@@ -178,34 +178,26 @@ static int arfs_add_default_rule(struct mlx5e_priv *priv,
 	struct mlx5_flow_destination dest;
 	MLX5_DECLARE_FLOW_ACT(flow_act);
 	struct mlx5_flow_spec *spec;
+	enum mlx5e_traffic_types tt;
 	int err = 0;
 
-	spec = mlx5_vzalloc(sizeof(*spec));
+	spec = kvzalloc(sizeof(*spec), GFP_KERNEL);
 	if (!spec) {
-		netdev_err(priv->netdev, "%s: alloc failed\n", __func__);
 		err = -ENOMEM;
 		goto out;
 	}
 
 	dest.type = MLX5_FLOW_DESTINATION_TYPE_TIR;
-	switch (type) {
-	case ARFS_IPV4_TCP:
-		dest.tir_num = tir[MLX5E_TT_IPV4_TCP].tirn;
-		break;
-	case ARFS_IPV4_UDP:
-		dest.tir_num = tir[MLX5E_TT_IPV4_UDP].tirn;
-		break;
-	case ARFS_IPV6_TCP:
-		dest.tir_num = tir[MLX5E_TT_IPV6_TCP].tirn;
-		break;
-	case ARFS_IPV6_UDP:
-		dest.tir_num = tir[MLX5E_TT_IPV6_UDP].tirn;
-		break;
-	default:
+	tt = arfs_get_tt(type);
+	if (tt == -EINVAL) {
+		netdev_err(priv->netdev, "%s: bad arfs_type: %d\n",
+			   __func__, type);
 		err = -EINVAL;
 		goto out;
 	}
 
+	dest.tir_num = tir[tt].tirn;
+
 	arfs_t->default_rule = mlx5_add_flow_rules(arfs_t->ft.t, spec,
 						   &flow_act,
 						   &dest, 1);
@@ -237,7 +229,7 @@ static int arfs_create_groups(struct mlx5e_flow_table *ft,
 
 	ft->g = kcalloc(MLX5E_ARFS_NUM_GROUPS,
 			sizeof(*ft->g), GFP_KERNEL);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if  (!in || !ft->g) {
 		kvfree(ft->g);
 		kvfree(in);
@@ -481,9 +473,8 @@ static struct mlx5_flow_handle *arfs_add_rule(struct mlx5e_priv *priv,
 	struct mlx5_flow_table *ft;
 	int err = 0;
 
-	spec = mlx5_vzalloc(sizeof(*spec));
+	spec = kvzalloc(sizeof(*spec), GFP_KERNEL);
 	if (!spec) {
-		netdev_err(priv->netdev, "%s: alloc failed\n", __func__);
 		err = -ENOMEM;
 		goto out;
 	}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_clock.c b/drivers/net/ethernet/mellanox/mlx5/core/en_clock.c
index e706a87fc8b2..66f432385dbb 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_clock.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_clock.c
@@ -86,9 +86,8 @@ static void mlx5e_timestamp_overflow(struct work_struct *work)
 	schedule_delayed_work(&tstamp->overflow_work, tstamp->overflow_period);
 }
 
-int mlx5e_hwstamp_set(struct net_device *dev, struct ifreq *ifr)
+int mlx5e_hwstamp_set(struct mlx5e_priv *priv, struct ifreq *ifr)
 {
-	struct mlx5e_priv *priv = netdev_priv(dev);
 	struct hwtstamp_config config;
 	int err;
 
@@ -128,11 +127,12 @@ int mlx5e_hwstamp_set(struct net_device *dev, struct ifreq *ifr)
 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+	case HWTSTAMP_FILTER_NTP_ALL:
 		/* Disable CQE compression */
-		netdev_warn(dev, "Disabling cqe compression");
+		netdev_warn(priv->netdev, "Disabling cqe compression");
 		err = mlx5e_modify_rx_cqe_compression_locked(priv, false);
 		if (err) {
-			netdev_err(dev, "Failed disabling cqe compression err=%d\n", err);
+			netdev_err(priv->netdev, "Failed disabling cqe compression err=%d\n", err);
 			mutex_unlock(&priv->state_lock);
 			return err;
 		}
@@ -150,9 +150,8 @@ int mlx5e_hwstamp_set(struct net_device *dev, struct ifreq *ifr)
 			    sizeof(config)) ? -EFAULT : 0;
 }
 
-int mlx5e_hwstamp_get(struct net_device *dev, struct ifreq *ifr)
+int mlx5e_hwstamp_get(struct mlx5e_priv *priv, struct ifreq *ifr)
 {
-	struct mlx5e_priv *priv = netdev_priv(dev);
 	struct hwtstamp_config *cfg = &priv->tstamp.hwtstamp_config;
 
 	if (!MLX5_CAP_GEN(priv->mdev, device_frequency_khz))
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_common.c b/drivers/net/ethernet/mellanox/mlx5/core/en_common.c
index f1f17f7a3cd0..ece3fb147e3e 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_common.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_common.c
@@ -65,7 +65,7 @@ static int mlx5e_create_mkey(struct mlx5_core_dev *mdev, u32 pdn,
 	u32 *in;
 	int err;
 
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -145,9 +145,8 @@ int mlx5e_refresh_tirs(struct mlx5e_priv *priv, bool enable_uc_lb)
 	int inlen;
 	void *in;
 
-
 	inlen = MLX5_ST_SZ_BYTES(modify_tir_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		goto out;
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
index 8209affa75c3..917fade5f5d5 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
@@ -31,15 +31,15 @@
  */
 
 #include "en.h"
+#include "en_accel/ipsec.h"
 
-static void mlx5e_get_drvinfo(struct net_device *dev,
-			      struct ethtool_drvinfo *drvinfo)
+void mlx5e_ethtool_get_drvinfo(struct mlx5e_priv *priv,
+			       struct ethtool_drvinfo *drvinfo)
 {
-	struct mlx5e_priv *priv = netdev_priv(dev);
 	struct mlx5_core_dev *mdev = priv->mdev;
 
 	strlcpy(drvinfo->driver, DRIVER_NAME, sizeof(drvinfo->driver));
-	strlcpy(drvinfo->version, DRIVER_VERSION " (" DRIVER_RELDATE ")",
+	strlcpy(drvinfo->version, DRIVER_VERSION,
 		sizeof(drvinfo->version));
 	snprintf(drvinfo->fw_version, sizeof(drvinfo->fw_version),
 		 "%d.%d.%04d (%.16s)",
@@ -49,6 +49,14 @@ static void mlx5e_get_drvinfo(struct net_device *dev,
 		sizeof(drvinfo->bus_info));
 }
 
+static void mlx5e_get_drvinfo(struct net_device *dev,
+			      struct ethtool_drvinfo *drvinfo)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	mlx5e_ethtool_get_drvinfo(priv, drvinfo);
+}
+
 struct ptys2ethtool_config {
 	__ETHTOOL_DECLARE_LINK_MODE_MASK(supported);
 	__ETHTOOL_DECLARE_LINK_MODE_MASK(advertised);
@@ -135,6 +143,9 @@ static unsigned long mlx5e_query_pfc_combined(struct mlx5e_priv *priv)
 	u8 pfc_en_rx;
 	int err;
 
+	if (MLX5_CAP_GEN(mdev, port_type) != MLX5_CAP_PORT_TYPE_ETH)
+		return 0;
+
 	err = mlx5_query_port_pfc(mdev, &pfc_en_tx, &pfc_en_rx);
 
 	return err ? 0 : pfc_en_tx | pfc_en_rx;
@@ -147,6 +158,9 @@ static bool mlx5e_query_global_pause_combined(struct mlx5e_priv *priv)
 	u32 tx_pause;
 	int err;
 
+	if (MLX5_CAP_GEN(mdev, port_type) != MLX5_CAP_PORT_TYPE_ETH)
+		return false;
+
 	err = mlx5_query_port_pause(mdev, &rx_pause, &tx_pause);
 
 	return err ? false : rx_pause | tx_pause;
@@ -160,9 +174,8 @@ static bool mlx5e_query_global_pause_combined(struct mlx5e_priv *priv)
 	((mlx5e_query_global_pause_combined(priv) + hweight8(mlx5e_query_pfc_combined(priv))) * \
 	  NUM_PPORT_PER_PRIO_PFC_COUNTERS)
 
-static int mlx5e_get_sset_count(struct net_device *dev, int sset)
+int mlx5e_ethtool_get_sset_count(struct mlx5e_priv *priv, int sset)
 {
-	struct mlx5e_priv *priv = netdev_priv(dev);
 
 	switch (sset) {
 	case ETH_SS_STATS:
@@ -174,7 +187,8 @@ static int mlx5e_get_sset_count(struct net_device *dev, int sset)
 		       MLX5E_NUM_SQ_STATS(priv) +
 		       MLX5E_NUM_PFC_COUNTERS(priv) +
 		       ARRAY_SIZE(mlx5e_pme_status_desc) +
-		       ARRAY_SIZE(mlx5e_pme_error_desc);
+		       ARRAY_SIZE(mlx5e_pme_error_desc) +
+		       mlx5e_ipsec_get_count(priv);
 
 	case ETH_SS_PRIV_FLAGS:
 		return ARRAY_SIZE(mlx5e_priv_flags);
@@ -186,6 +200,13 @@ static int mlx5e_get_sset_count(struct net_device *dev, int sset)
 	}
 }
 
+static int mlx5e_get_sset_count(struct net_device *dev, int sset)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	return mlx5e_ethtool_get_sset_count(priv, sset);
+}
+
 static void mlx5e_fill_stats_strings(struct mlx5e_priv *priv, uint8_t *data)
 {
 	int i, j, tc, prio, idx = 0;
@@ -256,6 +277,9 @@ static void mlx5e_fill_stats_strings(struct mlx5e_priv *priv, uint8_t *data)
 	for (i = 0; i < ARRAY_SIZE(mlx5e_pme_error_desc); i++)
 		strcpy(data + (idx++) * ETH_GSTRING_LEN, mlx5e_pme_error_desc[i].format);
 
+	/* IPSec counters */
+	idx += mlx5e_ipsec_get_strings(priv, data + idx * ETH_GSTRING_LEN);
+
 	if (!test_bit(MLX5E_STATE_OPENED, &priv->state))
 		return;
 
@@ -273,10 +297,9 @@ static void mlx5e_fill_stats_strings(struct mlx5e_priv *priv, uint8_t *data)
 					priv->channel_tc2txq[i][tc]);
 }
 
-static void mlx5e_get_strings(struct net_device *dev,
-			      uint32_t stringset, uint8_t *data)
+void mlx5e_ethtool_get_strings(struct mlx5e_priv *priv,
+			       uint32_t stringset, uint8_t *data)
 {
-	struct mlx5e_priv *priv = netdev_priv(dev);
 	int i;
 
 	switch (stringset) {
@@ -297,10 +320,17 @@ static void mlx5e_get_strings(struct net_device *dev,
 	}
 }
 
-static void mlx5e_get_ethtool_stats(struct net_device *dev,
-				    struct ethtool_stats *stats, u64 *data)
+static void mlx5e_get_strings(struct net_device *dev,
+			      uint32_t stringset, uint8_t *data)
 {
 	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	mlx5e_ethtool_get_strings(priv, stringset, data);
+}
+
+void mlx5e_ethtool_get_ethtool_stats(struct mlx5e_priv *priv,
+				     struct ethtool_stats *stats, u64 *data)
+{
 	struct mlx5e_channels *channels;
 	struct mlx5_priv *mlx5_priv;
 	int i, j, tc, prio, idx = 0;
@@ -311,7 +341,7 @@ static void mlx5e_get_ethtool_stats(struct net_device *dev,
 
 	mutex_lock(&priv->state_lock);
 	if (test_bit(MLX5E_STATE_OPENED, &priv->state))
-		mlx5e_update_stats(priv);
+		mlx5e_update_stats(priv, true);
 	channels = &priv->channels;
 	mutex_unlock(&priv->state_lock);
 
@@ -378,6 +408,9 @@ static void mlx5e_get_ethtool_stats(struct net_device *dev,
 		data[idx++] = MLX5E_READ_CTR64_CPU(mlx5_priv->pme_stats.error_counters,
 						   mlx5e_pme_error_desc, i);
 
+	/* IPSec counters */
+	idx += mlx5e_ipsec_get_stats(priv, data + idx);
+
 	if (!test_bit(MLX5E_STATE_OPENED, &priv->state))
 		return;
 
@@ -395,6 +428,15 @@ static void mlx5e_get_ethtool_stats(struct net_device *dev,
 								   sq_stats_desc, j);
 }
 
+static void mlx5e_get_ethtool_stats(struct net_device *dev,
+				    struct ethtool_stats *stats,
+				    u64 *data)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	mlx5e_ethtool_get_ethtool_stats(priv, stats, data);
+}
+
 static u32 mlx5e_rx_wqes_to_packets(struct mlx5e_priv *priv, int rq_wq_type,
 				    int num_wqe)
 {
@@ -439,10 +481,9 @@ static u32 mlx5e_packets_to_rx_wqes(struct mlx5e_priv *priv, int rq_wq_type,
 	return 1 << (order_base_2(num_wqes));
 }
 
-static void mlx5e_get_ringparam(struct net_device *dev,
-				struct ethtool_ringparam *param)
+void mlx5e_ethtool_get_ringparam(struct mlx5e_priv *priv,
+				 struct ethtool_ringparam *param)
 {
-	struct mlx5e_priv *priv = netdev_priv(dev);
 	int rq_wq_type = priv->channels.params.rq_wq_type;
 
 	param->rx_max_pending = mlx5e_rx_wqes_to_packets(priv, rq_wq_type,
@@ -453,10 +494,17 @@ static void mlx5e_get_ringparam(struct net_device *dev,
 	param->tx_pending     = 1 << priv->channels.params.log_sq_size;
 }
 
-static int mlx5e_set_ringparam(struct net_device *dev,
-			       struct ethtool_ringparam *param)
+static void mlx5e_get_ringparam(struct net_device *dev,
+				struct ethtool_ringparam *param)
 {
 	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	mlx5e_ethtool_get_ringparam(priv, param);
+}
+
+int mlx5e_ethtool_set_ringparam(struct mlx5e_priv *priv,
+				struct ethtool_ringparam *param)
+{
 	int rq_wq_type = priv->channels.params.rq_wq_type;
 	struct mlx5e_channels new_channels = {};
 	u32 rx_pending_wqes;
@@ -468,12 +516,12 @@ static int mlx5e_set_ringparam(struct net_device *dev,
 	int err = 0;
 
 	if (param->rx_jumbo_pending) {
-		netdev_info(dev, "%s: rx_jumbo_pending not supported\n",
+		netdev_info(priv->netdev, "%s: rx_jumbo_pending not supported\n",
 			    __func__);
 		return -EINVAL;
 	}
 	if (param->rx_mini_pending) {
-		netdev_info(dev, "%s: rx_mini_pending not supported\n",
+		netdev_info(priv->netdev, "%s: rx_mini_pending not supported\n",
 			    __func__);
 		return -EINVAL;
 	}
@@ -486,13 +534,13 @@ static int mlx5e_set_ringparam(struct net_device *dev,
 						   param->rx_pending);
 
 	if (param->rx_pending < min_rq_size) {
-		netdev_info(dev, "%s: rx_pending (%d) < min (%d)\n",
+		netdev_info(priv->netdev, "%s: rx_pending (%d) < min (%d)\n",
 			    __func__, param->rx_pending,
 			    min_rq_size);
 		return -EINVAL;
 	}
 	if (param->rx_pending > max_rq_size) {
-		netdev_info(dev, "%s: rx_pending (%d) > max (%d)\n",
+		netdev_info(priv->netdev, "%s: rx_pending (%d) > max (%d)\n",
 			    __func__, param->rx_pending,
 			    max_rq_size);
 		return -EINVAL;
@@ -501,19 +549,19 @@ static int mlx5e_set_ringparam(struct net_device *dev,
 	num_mtts = MLX5E_REQUIRED_MTTS(rx_pending_wqes);
 	if (priv->channels.params.rq_wq_type == MLX5_WQ_TYPE_LINKED_LIST_STRIDING_RQ &&
 	    !MLX5E_VALID_NUM_MTTS(num_mtts)) {
-		netdev_info(dev, "%s: rx_pending (%d) request can't be satisfied, try to reduce.\n",
+		netdev_info(priv->netdev, "%s: rx_pending (%d) request can't be satisfied, try to reduce.\n",
 			    __func__, param->rx_pending);
 		return -EINVAL;
 	}
 
 	if (param->tx_pending < (1 << MLX5E_PARAMS_MINIMUM_LOG_SQ_SIZE)) {
-		netdev_info(dev, "%s: tx_pending (%d) < min (%d)\n",
+		netdev_info(priv->netdev, "%s: tx_pending (%d) < min (%d)\n",
 			    __func__, param->tx_pending,
 			    1 << MLX5E_PARAMS_MINIMUM_LOG_SQ_SIZE);
 		return -EINVAL;
 	}
 	if (param->tx_pending > (1 << MLX5E_PARAMS_MAXIMUM_LOG_SQ_SIZE)) {
-		netdev_info(dev, "%s: tx_pending (%d) > max (%d)\n",
+		netdev_info(priv->netdev, "%s: tx_pending (%d) > max (%d)\n",
 			    __func__, param->tx_pending,
 			    1 << MLX5E_PARAMS_MAXIMUM_LOG_SQ_SIZE);
 		return -EINVAL;
@@ -549,26 +597,39 @@ unlock:
 	return err;
 }
 
-static void mlx5e_get_channels(struct net_device *dev,
-			       struct ethtool_channels *ch)
+static int mlx5e_set_ringparam(struct net_device *dev,
+			       struct ethtool_ringparam *param)
 {
 	struct mlx5e_priv *priv = netdev_priv(dev);
 
+	return mlx5e_ethtool_set_ringparam(priv, param);
+}
+
+void mlx5e_ethtool_get_channels(struct mlx5e_priv *priv,
+				struct ethtool_channels *ch)
+{
 	ch->max_combined   = priv->profile->max_nch(priv->mdev);
 	ch->combined_count = priv->channels.params.num_channels;
 }
 
-static int mlx5e_set_channels(struct net_device *dev,
-			      struct ethtool_channels *ch)
+static void mlx5e_get_channels(struct net_device *dev,
+			       struct ethtool_channels *ch)
 {
 	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	mlx5e_ethtool_get_channels(priv, ch);
+}
+
+int mlx5e_ethtool_set_channels(struct mlx5e_priv *priv,
+			       struct ethtool_channels *ch)
+{
 	unsigned int count = ch->combined_count;
 	struct mlx5e_channels new_channels = {};
 	bool arfs_enabled;
 	int err = 0;
 
 	if (!count) {
-		netdev_info(dev, "%s: combined_count=0 not supported\n",
+		netdev_info(priv->netdev, "%s: combined_count=0 not supported\n",
 			    __func__);
 		return -EINVAL;
 	}
@@ -593,7 +654,7 @@ static int mlx5e_set_channels(struct net_device *dev,
 	if (err)
 		goto out;
 
-	arfs_enabled = dev->features & NETIF_F_NTUPLE;
+	arfs_enabled = priv->netdev->features & NETIF_F_NTUPLE;
 	if (arfs_enabled)
 		mlx5e_arfs_disable(priv);
 
@@ -603,7 +664,7 @@ static int mlx5e_set_channels(struct net_device *dev,
 	if (arfs_enabled) {
 		err = mlx5e_arfs_enable(priv);
 		if (err)
-			netdev_err(dev, "%s: mlx5e_arfs_enable failed: %d\n",
+			netdev_err(priv->netdev, "%s: mlx5e_arfs_enable failed: %d\n",
 				   __func__, err);
 	}
 
@@ -613,11 +674,17 @@ out:
 	return err;
 }
 
-static int mlx5e_get_coalesce(struct net_device *netdev,
-			      struct ethtool_coalesce *coal)
+static int mlx5e_set_channels(struct net_device *dev,
+			      struct ethtool_channels *ch)
 {
-	struct mlx5e_priv *priv = netdev_priv(netdev);
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	return mlx5e_ethtool_set_channels(priv, ch);
+}
 
+int mlx5e_ethtool_get_coalesce(struct mlx5e_priv *priv,
+			       struct ethtool_coalesce *coal)
+{
 	if (!MLX5_CAP_GEN(priv->mdev, cq_moderation))
 		return -EOPNOTSUPP;
 
@@ -630,6 +697,14 @@ static int mlx5e_get_coalesce(struct net_device *netdev,
 	return 0;
 }
 
+static int mlx5e_get_coalesce(struct net_device *netdev,
+			      struct ethtool_coalesce *coal)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+
+	return mlx5e_ethtool_get_coalesce(priv, coal);
+}
+
 static void
 mlx5e_set_priv_channels_coalesce(struct mlx5e_priv *priv, struct ethtool_coalesce *coal)
 {
@@ -653,10 +728,9 @@ mlx5e_set_priv_channels_coalesce(struct mlx5e_priv *priv, struct ethtool_coalesc
 	}
 }
 
-static int mlx5e_set_coalesce(struct net_device *netdev,
-			      struct ethtool_coalesce *coal)
+int mlx5e_ethtool_set_coalesce(struct mlx5e_priv *priv,
+			       struct ethtool_coalesce *coal)
 {
-	struct mlx5e_priv *priv    = netdev_priv(netdev);
 	struct mlx5_core_dev *mdev = priv->mdev;
 	struct mlx5e_channels new_channels = {};
 	int err = 0;
@@ -699,6 +773,14 @@ out:
 	return err;
 }
 
+static int mlx5e_set_coalesce(struct net_device *netdev,
+			      struct ethtool_coalesce *coal)
+{
+	struct mlx5e_priv *priv    = netdev_priv(netdev);
+
+	return mlx5e_ethtool_set_coalesce(priv, coal);
+}
+
 static void ptys2ethtool_supported_link(unsigned long *supported_modes,
 					u32 eth_proto_cap)
 {
@@ -723,24 +805,81 @@ static void ptys2ethtool_adver_link(unsigned long *advertising_modes,
 			  __ETHTOOL_LINK_MODE_MASK_NBITS);
 }
 
-static void ptys2ethtool_supported_port(struct ethtool_link_ksettings *link_ksettings,
-					u32 eth_proto_cap)
+static void ptys2ethtool_supported_advertised_port(struct ethtool_link_ksettings *link_ksettings,
+						   u32 eth_proto_cap,
+						   u8 connector_type)
 {
-	if (eth_proto_cap & (MLX5E_PROT_MASK(MLX5E_10GBASE_CR)
-			   | MLX5E_PROT_MASK(MLX5E_10GBASE_SR)
-			   | MLX5E_PROT_MASK(MLX5E_40GBASE_CR4)
-			   | MLX5E_PROT_MASK(MLX5E_40GBASE_SR4)
-			   | MLX5E_PROT_MASK(MLX5E_100GBASE_SR4)
-			   | MLX5E_PROT_MASK(MLX5E_1000BASE_CX_SGMII))) {
-		ethtool_link_ksettings_add_link_mode(link_ksettings, supported, FIBRE);
+	if (!connector_type || connector_type >= MLX5E_CONNECTOR_TYPE_NUMBER) {
+		if (eth_proto_cap & (MLX5E_PROT_MASK(MLX5E_10GBASE_CR)
+				   | MLX5E_PROT_MASK(MLX5E_10GBASE_SR)
+				   | MLX5E_PROT_MASK(MLX5E_40GBASE_CR4)
+				   | MLX5E_PROT_MASK(MLX5E_40GBASE_SR4)
+				   | MLX5E_PROT_MASK(MLX5E_100GBASE_SR4)
+				   | MLX5E_PROT_MASK(MLX5E_1000BASE_CX_SGMII))) {
+			ethtool_link_ksettings_add_link_mode(link_ksettings,
+							     supported,
+							     FIBRE);
+			ethtool_link_ksettings_add_link_mode(link_ksettings,
+							     advertising,
+							     FIBRE);
+		}
+
+		if (eth_proto_cap & (MLX5E_PROT_MASK(MLX5E_100GBASE_KR4)
+				   | MLX5E_PROT_MASK(MLX5E_40GBASE_KR4)
+				   | MLX5E_PROT_MASK(MLX5E_10GBASE_KR)
+				   | MLX5E_PROT_MASK(MLX5E_10GBASE_KX4)
+				   | MLX5E_PROT_MASK(MLX5E_1000BASE_KX))) {
+			ethtool_link_ksettings_add_link_mode(link_ksettings,
+							     supported,
+							     Backplane);
+			ethtool_link_ksettings_add_link_mode(link_ksettings,
+							     advertising,
+							     Backplane);
+		}
+		return;
 	}
 
-	if (eth_proto_cap & (MLX5E_PROT_MASK(MLX5E_100GBASE_KR4)
-			   | MLX5E_PROT_MASK(MLX5E_40GBASE_KR4)
-			   | MLX5E_PROT_MASK(MLX5E_10GBASE_KR)
-			   | MLX5E_PROT_MASK(MLX5E_10GBASE_KX4)
-			   | MLX5E_PROT_MASK(MLX5E_1000BASE_KX))) {
-		ethtool_link_ksettings_add_link_mode(link_ksettings, supported, Backplane);
+	switch (connector_type) {
+	case MLX5E_PORT_TP:
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     supported, TP);
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     advertising, TP);
+		break;
+	case MLX5E_PORT_AUI:
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     supported, AUI);
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     advertising, AUI);
+		break;
+	case MLX5E_PORT_BNC:
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     supported, BNC);
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     advertising, BNC);
+		break;
+	case MLX5E_PORT_MII:
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     supported, MII);
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     advertising, MII);
+		break;
+	case MLX5E_PORT_FIBRE:
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     supported, FIBRE);
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     advertising, FIBRE);
+		break;
+	case MLX5E_PORT_DA:
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     supported, Backplane);
+		ethtool_link_ksettings_add_link_mode(link_ksettings,
+						     advertising, Backplane);
+		break;
+	case MLX5E_PORT_NONE:
+	case MLX5E_PORT_OTHER:
+	default:
+		break;
 	}
 }
 
@@ -791,7 +930,6 @@ static void get_supported(u32 eth_proto_cap,
 {
 	unsigned long *supported = link_ksettings->link_modes.supported;
 
-	ptys2ethtool_supported_port(link_ksettings, eth_proto_cap);
 	ptys2ethtool_supported_link(supported, eth_proto_cap);
 	ethtool_link_ksettings_add_link_mode(link_ksettings, supported, Pause);
 }
@@ -809,8 +947,23 @@ static void get_advertising(u32 eth_proto_cap, u8 tx_pause,
 		ethtool_link_ksettings_add_link_mode(link_ksettings, advertising, Asym_Pause);
 }
 
-static u8 get_connector_port(u32 eth_proto)
+static int ptys2connector_type[MLX5E_CONNECTOR_TYPE_NUMBER] = {
+		[MLX5E_PORT_UNKNOWN]            = PORT_OTHER,
+		[MLX5E_PORT_NONE]               = PORT_NONE,
+		[MLX5E_PORT_TP]                 = PORT_TP,
+		[MLX5E_PORT_AUI]                = PORT_AUI,
+		[MLX5E_PORT_BNC]                = PORT_BNC,
+		[MLX5E_PORT_MII]                = PORT_MII,
+		[MLX5E_PORT_FIBRE]              = PORT_FIBRE,
+		[MLX5E_PORT_DA]                 = PORT_DA,
+		[MLX5E_PORT_OTHER]              = PORT_OTHER,
+	};
+
+static u8 get_connector_port(u32 eth_proto, u8 connector_type)
 {
+	if (connector_type && connector_type < MLX5E_CONNECTOR_TYPE_NUMBER)
+		return ptys2connector_type[connector_type];
+
 	if (eth_proto & (MLX5E_PROT_MASK(MLX5E_10GBASE_SR)
 			 | MLX5E_PROT_MASK(MLX5E_40GBASE_SR4)
 			 | MLX5E_PROT_MASK(MLX5E_100GBASE_SR4)
@@ -856,6 +1009,7 @@ static int mlx5e_get_link_ksettings(struct net_device *netdev,
 	u32 eth_proto_oper;
 	u8 an_disable_admin;
 	u8 an_status;
+	u8 connector_type;
 	int err;
 
 	err = mlx5_query_port_ptys(mdev, out, sizeof(out), MLX5_PTYS_EN, 1);
@@ -871,6 +1025,7 @@ static int mlx5e_get_link_ksettings(struct net_device *netdev,
 	eth_proto_lp     = MLX5_GET(ptys_reg, out, eth_proto_lp_advertise);
 	an_disable_admin = MLX5_GET(ptys_reg, out, an_disable_admin);
 	an_status        = MLX5_GET(ptys_reg, out, an_status);
+	connector_type   = MLX5_GET(ptys_reg, out, connector_type);
 
 	mlx5_query_port_pause(mdev, &rx_pause, &tx_pause);
 
@@ -883,7 +1038,10 @@ static int mlx5e_get_link_ksettings(struct net_device *netdev,
 
 	eth_proto_oper = eth_proto_oper ? eth_proto_oper : eth_proto_cap;
 
-	link_ksettings->base.port = get_connector_port(eth_proto_oper);
+	link_ksettings->base.port = get_connector_port(eth_proto_oper,
+						       connector_type);
+	ptys2ethtool_supported_advertised_port(link_ksettings, eth_proto_admin,
+					       connector_type);
 	get_lp_advertising(eth_proto_lp, link_ksettings);
 
 	if (an_status == MLX5_AN_COMPLETE)
@@ -1048,7 +1206,7 @@ static int mlx5e_set_rxfh(struct net_device *dev, const u32 *indir,
 	    (hfunc != ETH_RSS_HASH_TOP))
 		return -EINVAL;
 
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -1222,13 +1380,12 @@ static int mlx5e_set_pauseparam(struct net_device *netdev,
 	return err;
 }
 
-static int mlx5e_get_ts_info(struct net_device *dev,
-			     struct ethtool_ts_info *info)
+int mlx5e_ethtool_get_ts_info(struct mlx5e_priv *priv,
+			      struct ethtool_ts_info *info)
 {
-	struct mlx5e_priv *priv = netdev_priv(dev);
 	int ret;
 
-	ret = ethtool_op_get_ts_info(dev, info);
+	ret = ethtool_op_get_ts_info(priv->netdev, info);
 	if (ret)
 		return ret;
 
@@ -1242,15 +1399,23 @@ static int mlx5e_get_ts_info(struct net_device *dev,
 				 SOF_TIMESTAMPING_RX_HARDWARE |
 				 SOF_TIMESTAMPING_RAW_HARDWARE;
 
-	info->tx_types = (BIT(1) << HWTSTAMP_TX_OFF) |
-			 (BIT(1) << HWTSTAMP_TX_ON);
+	info->tx_types = BIT(HWTSTAMP_TX_OFF) |
+			 BIT(HWTSTAMP_TX_ON);
 
-	info->rx_filters = (BIT(1) << HWTSTAMP_FILTER_NONE) |
-			   (BIT(1) << HWTSTAMP_FILTER_ALL);
+	info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
+			   BIT(HWTSTAMP_FILTER_ALL);
 
 	return 0;
 }
 
+static int mlx5e_get_ts_info(struct net_device *dev,
+			     struct ethtool_ts_info *info)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	return mlx5e_ethtool_get_ts_info(priv, info);
+}
+
 static __u32 mlx5e_get_wol_supported(struct mlx5_core_dev *mdev)
 {
 	__u32 ret = 0;
@@ -1638,6 +1803,40 @@ static int mlx5e_set_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd)
 	return err;
 }
 
+int mlx5e_ethtool_flash_device(struct mlx5e_priv *priv,
+			       struct ethtool_flash *flash)
+{
+	struct mlx5_core_dev *mdev = priv->mdev;
+	struct net_device *dev = priv->netdev;
+	const struct firmware *fw;
+	int err;
+
+	if (flash->region != ETHTOOL_FLASH_ALL_REGIONS)
+		return -EOPNOTSUPP;
+
+	err = request_firmware_direct(&fw, flash->data, &dev->dev);
+	if (err)
+		return err;
+
+	dev_hold(dev);
+	rtnl_unlock();
+
+	err = mlx5_firmware_flash(mdev, fw);
+	release_firmware(fw);
+
+	rtnl_lock();
+	dev_put(dev);
+	return err;
+}
+
+static int mlx5e_flash_device(struct net_device *dev,
+			      struct ethtool_flash *flash)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	return mlx5e_ethtool_flash_device(priv, flash);
+}
+
 const struct ethtool_ops mlx5e_ethtool_ops = {
 	.get_drvinfo       = mlx5e_get_drvinfo,
 	.get_link          = ethtool_op_get_link,
@@ -1658,6 +1857,7 @@ const struct ethtool_ops mlx5e_ethtool_ops = {
 	.set_rxfh          = mlx5e_set_rxfh,
 	.get_rxnfc         = mlx5e_get_rxnfc,
 	.set_rxnfc         = mlx5e_set_rxnfc,
+	.flash_device      = mlx5e_flash_device,
 	.get_tunable       = mlx5e_get_tunable,
 	.set_tunable       = mlx5e_set_tunable,
 	.get_pauseparam    = mlx5e_get_pauseparam,
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c b/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c
index 53ed58320a24..dfccb5305e9c 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c
@@ -170,7 +170,6 @@ static int __mlx5e_add_vlan_rule(struct mlx5e_priv *priv,
 
 	spec->match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
 
-
 	switch (rule_type) {
 	case MLX5E_VLAN_RULE_TYPE_UNTAGGED:
 		rule_p = &priv->fs.vlan.untagged_rule;
@@ -218,11 +217,9 @@ static int mlx5e_add_vlan_rule(struct mlx5e_priv *priv,
 	struct mlx5_flow_spec *spec;
 	int err = 0;
 
-	spec = mlx5_vzalloc(sizeof(*spec));
-	if (!spec) {
-		netdev_err(priv->netdev, "%s: alloc failed\n", __func__);
+	spec = kvzalloc(sizeof(*spec), GFP_KERNEL);
+	if (!spec)
 		return -ENOMEM;
-	}
 
 	if (rule_type == MLX5E_VLAN_RULE_TYPE_MATCH_VID)
 		mlx5e_vport_context_update_vlans(priv);
@@ -660,11 +657,9 @@ mlx5e_generate_ttc_rule(struct mlx5e_priv *priv,
 	struct mlx5_flow_spec *spec;
 	int err = 0;
 
-	spec = mlx5_vzalloc(sizeof(*spec));
-	if (!spec) {
-		netdev_err(priv->netdev, "%s: alloc failed\n", __func__);
+	spec = kvzalloc(sizeof(*spec), GFP_KERNEL);
+	if (!spec)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	if (proto) {
 		spec->match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
@@ -742,7 +737,7 @@ static int mlx5e_create_ttc_table_groups(struct mlx5e_ttc_table *ttc)
 			sizeof(*ft->g), GFP_KERNEL);
 	if (!ft->g)
 		return -ENOMEM;
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in) {
 		kfree(ft->g);
 		return -ENOMEM;
@@ -852,11 +847,9 @@ static int mlx5e_add_l2_flow_rule(struct mlx5e_priv *priv,
 	u8 *mc_dmac;
 	u8 *mv_dmac;
 
-	spec = mlx5_vzalloc(sizeof(*spec));
-	if (!spec) {
-		netdev_err(priv->netdev, "%s: alloc failed\n", __func__);
+	spec = kvzalloc(sizeof(*spec), GFP_KERNEL);
+	if (!spec)
 		return -ENOMEM;
-	}
 
 	mc_dmac = MLX5_ADDR_OF(fte_match_param, spec->match_criteria,
 			       outer_headers.dmac_47_16);
@@ -916,7 +909,7 @@ static int mlx5e_create_l2_table_groups(struct mlx5e_l2_table *l2_table)
 	ft->g = kcalloc(MLX5E_NUM_L2_GROUPS, sizeof(*ft->g), GFP_KERNEL);
 	if (!ft->g)
 		return -ENOMEM;
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in) {
 		kfree(ft->g);
 		return -ENOMEM;
@@ -1071,7 +1064,7 @@ static int mlx5e_create_vlan_table_groups(struct mlx5e_flow_table *ft)
 	int inlen = MLX5_ST_SZ_BYTES(create_flow_group_in);
 	int err;
 
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_fs_ethtool.c b/drivers/net/ethernet/mellanox/mlx5/core/en_fs_ethtool.c
index 85bf4a389295..bdd82c9b3992 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_fs_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_fs_ethtool.c
@@ -296,7 +296,7 @@ add_ethtool_flow_rule(struct mlx5e_priv *priv,
 	struct mlx5_flow_handle *rule;
 	int err = 0;
 
-	spec = mlx5_vzalloc(sizeof(*spec));
+	spec = kvzalloc(sizeof(*spec), GFP_KERNEL);
 	if (!spec)
 		return ERR_PTR(-ENOMEM);
 	err = set_flow_attrs(spec->match_criteria, spec->match_value,
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
index 41cd22a223dc..1eac5003084f 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
@@ -39,6 +39,9 @@
 #include "en.h"
 #include "en_tc.h"
 #include "en_rep.h"
+#include "en_accel/ipsec.h"
+#include "en_accel/ipsec_rxtx.h"
+#include "accel/ipsec.h"
 #include "vxlan.h"
 
 struct mlx5e_rq_param {
@@ -96,9 +99,12 @@ void mlx5e_set_rq_type_params(struct mlx5_core_dev *mdev,
 		params->log_rq_size = is_kdump_kernel() ?
 			MLX5E_PARAMS_MINIMUM_LOG_RQ_SIZE :
 			MLX5E_PARAMS_DEFAULT_LOG_RQ_SIZE;
+		params->rq_headroom = params->xdp_prog ?
+			XDP_PACKET_HEADROOM : MLX5_RX_HEADROOM;
+		params->rq_headroom += NET_IP_ALIGN;
 
 		/* Extra room needed for build_skb */
-		params->lro_wqe_sz -= MLX5_RX_HEADROOM +
+		params->lro_wqe_sz -= params->rq_headroom +
 			SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
 	}
 
@@ -112,7 +118,7 @@ void mlx5e_set_rq_type_params(struct mlx5_core_dev *mdev,
 static void mlx5e_set_rq_params(struct mlx5_core_dev *mdev, struct mlx5e_params *params)
 {
 	u8 rq_type = mlx5e_check_fragmented_striding_rq_cap(mdev) &&
-		    !params->xdp_prog ?
+		    !params->xdp_prog && !MLX5_IPSEC_DEV(mdev) ?
 		    MLX5_WQ_TYPE_LINKED_LIST_STRIDING_RQ :
 		    MLX5_WQ_TYPE_LINKED_LIST;
 	mlx5e_set_rq_type_params(mdev, params, rq_type);
@@ -124,7 +130,8 @@ static void mlx5e_update_carrier(struct mlx5e_priv *priv)
 	u8 port_state;
 
 	port_state = mlx5_query_vport_state(mdev,
-		MLX5_QUERY_VPORT_STATE_IN_OP_MOD_VNIC_VPORT, 0);
+					    MLX5_QUERY_VPORT_STATE_IN_OP_MOD_VNIC_VPORT,
+					    0);
 
 	if (port_state == VPORT_STATE_UP) {
 		netdev_info(priv->netdev, "Link up\n");
@@ -142,7 +149,8 @@ static void mlx5e_update_carrier_work(struct work_struct *work)
 
 	mutex_lock(&priv->state_lock);
 	if (test_bit(MLX5E_STATE_OPENED, &priv->state))
-		mlx5e_update_carrier(priv);
+		if (priv->profile->update_carrier)
+			priv->profile->update_carrier(priv);
 	mutex_unlock(&priv->state_lock);
 }
 
@@ -195,6 +203,7 @@ static void mlx5e_update_sw_counters(struct mlx5e_priv *priv)
 		s->rx_buff_alloc_err += rq_stats->buff_alloc_err;
 		s->rx_cqe_compress_blks += rq_stats->cqe_compress_blks;
 		s->rx_cqe_compress_pkts += rq_stats->cqe_compress_pkts;
+		s->rx_page_reuse  += rq_stats->page_reuse;
 		s->rx_cache_reuse += rq_stats->cache_reuse;
 		s->rx_cache_full  += rq_stats->cache_full;
 		s->rx_cache_empty += rq_stats->cache_empty;
@@ -243,18 +252,14 @@ static void mlx5e_update_vport_counters(struct mlx5e_priv *priv)
 	mlx5_cmd_exec(mdev, in, sizeof(in), out, outlen);
 }
 
-static void mlx5e_update_pport_counters(struct mlx5e_priv *priv)
+static void mlx5e_update_pport_counters(struct mlx5e_priv *priv, bool full)
 {
 	struct mlx5e_pport_stats *pstats = &priv->stats.pport;
 	struct mlx5_core_dev *mdev = priv->mdev;
+	u32 in[MLX5_ST_SZ_DW(ppcnt_reg)] = {0};
 	int sz = MLX5_ST_SZ_BYTES(ppcnt_reg);
 	int prio;
 	void *out;
-	u32 *in;
-
-	in = mlx5_vzalloc(sz);
-	if (!in)
-		goto free_out;
 
 	MLX5_SET(ppcnt_reg, in, local_port, 1);
 
@@ -262,6 +267,9 @@ static void mlx5e_update_pport_counters(struct mlx5e_priv *priv)
 	MLX5_SET(ppcnt_reg, in, grp, MLX5_IEEE_802_3_COUNTERS_GROUP);
 	mlx5_core_access_reg(mdev, in, sz, out, sz, MLX5_REG_PPCNT, 0, 0);
 
+	if (!full)
+		return;
+
 	out = pstats->RFC_2863_counters;
 	MLX5_SET(ppcnt_reg, in, grp, MLX5_RFC_2863_COUNTERS_GROUP);
 	mlx5_core_access_reg(mdev, in, sz, out, sz, MLX5_REG_PPCNT, 0, 0);
@@ -287,53 +295,57 @@ static void mlx5e_update_pport_counters(struct mlx5e_priv *priv)
 		mlx5_core_access_reg(mdev, in, sz, out, sz,
 				     MLX5_REG_PPCNT, 0, 0);
 	}
-
-free_out:
-	kvfree(in);
 }
 
 static void mlx5e_update_q_counter(struct mlx5e_priv *priv)
 {
 	struct mlx5e_qcounter_stats *qcnt = &priv->stats.qcnt;
+	u32 out[MLX5_ST_SZ_DW(query_q_counter_out)];
+	int err;
 
 	if (!priv->q_counter)
 		return;
 
-	mlx5_core_query_out_of_buffer(priv->mdev, priv->q_counter,
-				      &qcnt->rx_out_of_buffer);
+	err = mlx5_core_query_q_counter(priv->mdev, priv->q_counter, 0, out, sizeof(out));
+	if (err)
+		return;
+
+	qcnt->rx_out_of_buffer = MLX5_GET(query_q_counter_out, out, out_of_buffer);
 }
 
 static void mlx5e_update_pcie_counters(struct mlx5e_priv *priv)
 {
 	struct mlx5e_pcie_stats *pcie_stats = &priv->stats.pcie;
 	struct mlx5_core_dev *mdev = priv->mdev;
+	u32 in[MLX5_ST_SZ_DW(mpcnt_reg)] = {0};
 	int sz = MLX5_ST_SZ_BYTES(mpcnt_reg);
 	void *out;
-	u32 *in;
 
 	if (!MLX5_CAP_MCAM_FEATURE(mdev, pcie_performance_group))
 		return;
 
-	in = mlx5_vzalloc(sz);
-	if (!in)
-		return;
-
 	out = pcie_stats->pcie_perf_counters;
 	MLX5_SET(mpcnt_reg, in, grp, MLX5_PCIE_PERFORMANCE_COUNTERS_GROUP);
 	mlx5_core_access_reg(mdev, in, sz, out, sz, MLX5_REG_MPCNT, 0, 0);
-
-	kvfree(in);
 }
 
-void mlx5e_update_stats(struct mlx5e_priv *priv)
+void mlx5e_update_stats(struct mlx5e_priv *priv, bool full)
 {
-	mlx5e_update_pcie_counters(priv);
-	mlx5e_update_pport_counters(priv);
+	if (full) {
+		mlx5e_update_pcie_counters(priv);
+		mlx5e_ipsec_update_stats(priv);
+	}
+	mlx5e_update_pport_counters(priv, full);
 	mlx5e_update_vport_counters(priv);
 	mlx5e_update_q_counter(priv);
 	mlx5e_update_sw_counters(priv);
 }
 
+static void mlx5e_update_ndo_stats(struct mlx5e_priv *priv)
+{
+	mlx5e_update_stats(priv, false);
+}
+
 void mlx5e_update_stats_work(struct work_struct *work)
 {
 	struct delayed_work *dwork = to_delayed_work(work);
@@ -503,7 +515,7 @@ static int mlx5e_create_umr_mkey(struct mlx5_core_dev *mdev,
 	if (!MLX5E_VALID_NUM_MTTS(npages))
 		return -EINVAL;
 
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -544,7 +556,6 @@ static int mlx5e_alloc_rq(struct mlx5e_channel *c,
 	void *rqc = rqp->rqc;
 	void *rqc_wq = MLX5_ADDR_OF(rqc, rqc, wq);
 	u32 byte_count;
-	u32 frag_sz;
 	int npages;
 	int wq_sz;
 	int err;
@@ -576,13 +587,8 @@ static int mlx5e_alloc_rq(struct mlx5e_channel *c,
 		goto err_rq_wq_destroy;
 	}
 
-	if (rq->xdp_prog) {
-		rq->buff.map_dir = DMA_BIDIRECTIONAL;
-		rq->rx_headroom = XDP_PACKET_HEADROOM;
-	} else {
-		rq->buff.map_dir = DMA_FROM_DEVICE;
-		rq->rx_headroom = MLX5_RX_HEADROOM;
-	}
+	rq->buff.map_dir = rq->xdp_prog ? DMA_BIDIRECTIONAL : DMA_FROM_DEVICE;
+	rq->rx_headroom = params->rq_headroom;
 
 	switch (rq->wq_type) {
 	case MLX5_WQ_TYPE_LINKED_LIST_STRIDING_RQ:
@@ -591,6 +597,13 @@ static int mlx5e_alloc_rq(struct mlx5e_channel *c,
 		rq->dealloc_wqe = mlx5e_dealloc_rx_mpwqe;
 
 		rq->handle_rx_cqe = c->priv->profile->rx_handlers.handle_rx_cqe_mpwqe;
+#ifdef CONFIG_MLX5_EN_IPSEC
+		if (MLX5_IPSEC_DEV(mdev)) {
+			err = -EINVAL;
+			netdev_err(c->netdev, "MPWQE RQ with IPSec offload not supported\n");
+			goto err_rq_wq_destroy;
+		}
+#endif
 		if (!rq->handle_rx_cqe) {
 			err = -EINVAL;
 			netdev_err(c->netdev, "RX handler of MPWQE RQ is not set, err %d\n", err);
@@ -613,18 +626,24 @@ static int mlx5e_alloc_rq(struct mlx5e_channel *c,
 			goto err_destroy_umr_mkey;
 		break;
 	default: /* MLX5_WQ_TYPE_LINKED_LIST */
-		rq->dma_info = kzalloc_node(wq_sz * sizeof(*rq->dma_info),
-					    GFP_KERNEL, cpu_to_node(c->cpu));
-		if (!rq->dma_info) {
+		rq->wqe.frag_info =
+			kzalloc_node(wq_sz * sizeof(*rq->wqe.frag_info),
+				     GFP_KERNEL, cpu_to_node(c->cpu));
+		if (!rq->wqe.frag_info) {
 			err = -ENOMEM;
 			goto err_rq_wq_destroy;
 		}
 		rq->alloc_wqe = mlx5e_alloc_rx_wqe;
 		rq->dealloc_wqe = mlx5e_dealloc_rx_wqe;
 
-		rq->handle_rx_cqe = c->priv->profile->rx_handlers.handle_rx_cqe;
+#ifdef CONFIG_MLX5_EN_IPSEC
+		if (c->priv->ipsec)
+			rq->handle_rx_cqe = mlx5e_ipsec_handle_rx_cqe;
+		else
+#endif
+			rq->handle_rx_cqe = c->priv->profile->rx_handlers.handle_rx_cqe;
 		if (!rq->handle_rx_cqe) {
-			kfree(rq->dma_info);
+			kfree(rq->wqe.frag_info);
 			err = -EINVAL;
 			netdev_err(c->netdev, "RX handler of RQ is not set, err %d\n", err);
 			goto err_rq_wq_destroy;
@@ -632,16 +651,17 @@ static int mlx5e_alloc_rq(struct mlx5e_channel *c,
 
 		rq->buff.wqe_sz = params->lro_en  ?
 				params->lro_wqe_sz :
-				MLX5E_SW2HW_MTU(c->netdev->mtu);
+				MLX5E_SW2HW_MTU(c->priv, c->netdev->mtu);
+#ifdef CONFIG_MLX5_EN_IPSEC
+		if (MLX5_IPSEC_DEV(mdev))
+			rq->buff.wqe_sz += MLX5E_METADATA_ETHER_LEN;
+#endif
+		rq->wqe.page_reuse = !params->xdp_prog && !params->lro_en;
 		byte_count = rq->buff.wqe_sz;
 
 		/* calc the required page order */
-		frag_sz = rq->rx_headroom +
-			  byte_count /* packet data */ +
-			  SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
-		frag_sz = SKB_DATA_ALIGN(frag_sz);
-
-		npages = DIV_ROUND_UP(frag_sz, PAGE_SIZE);
+		rq->wqe.frag_sz = MLX5_SKB_FRAG_SZ(rq->rx_headroom + byte_count);
+		npages = DIV_ROUND_UP(rq->wqe.frag_sz, PAGE_SIZE);
 		rq->buff.page_order = order_base_2(npages);
 
 		byte_count |= MLX5_HW_START_PADDING;
@@ -686,7 +706,7 @@ static void mlx5e_free_rq(struct mlx5e_rq *rq)
 		mlx5_core_destroy_mkey(rq->mdev, &rq->umr_mkey);
 		break;
 	default: /* MLX5_WQ_TYPE_LINKED_LIST */
-		kfree(rq->dma_info);
+		kfree(rq->wqe.frag_info);
 	}
 
 	for (i = rq->page_cache.head; i != rq->page_cache.tail;
@@ -711,7 +731,7 @@ static int mlx5e_create_rq(struct mlx5e_rq *rq,
 
 	inlen = MLX5_ST_SZ_BYTES(create_rq_in) +
 		sizeof(u64) * rq->wq_ctrl.buf.npages;
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -748,7 +768,7 @@ static int mlx5e_modify_rq_state(struct mlx5e_rq *rq, int curr_state,
 	int err;
 
 	inlen = MLX5_ST_SZ_BYTES(modify_rq_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -776,7 +796,7 @@ static int mlx5e_modify_rq_scatter_fcs(struct mlx5e_rq *rq, bool enable)
 	int err;
 
 	inlen = MLX5_ST_SZ_BYTES(modify_rq_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -805,7 +825,7 @@ static int mlx5e_modify_rq_vsd(struct mlx5e_rq *rq, bool vsd)
 	int err;
 
 	inlen = MLX5_ST_SZ_BYTES(modify_rq_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -868,6 +888,16 @@ static void mlx5e_free_rx_descs(struct mlx5e_rq *rq)
 		mlx5_wq_ll_pop(&rq->wq, wqe_ix_be,
 			       &wqe->next.next_wqe_index);
 	}
+
+	if (rq->wq_type == MLX5_WQ_TYPE_LINKED_LIST && rq->wqe.page_reuse) {
+		/* Clean outstanding pages on handled WQEs that decided to do page-reuse,
+		 * but yet to be re-posted.
+		 */
+		int wq_sz = mlx5_wq_ll_get_size(&rq->wq);
+
+		for (wqe_ix = 0; wqe_ix < wq_sz; wqe_ix++)
+			rq->dealloc_wqe(rq, wqe_ix);
+	}
 }
 
 static int mlx5e_open_rq(struct mlx5e_channel *c,
@@ -1086,6 +1116,8 @@ static int mlx5e_alloc_txqsq(struct mlx5e_channel *c,
 	sq->uar_map   = mdev->mlx5e_res.bfreg.map;
 	sq->max_inline      = params->tx_max_inline;
 	sq->min_inline_mode = params->tx_min_inline_mode;
+	if (MLX5_IPSEC_DEV(c->priv->mdev))
+		set_bit(MLX5E_SQ_STATE_IPSEC, &sq->state);
 
 	param->wq.db_numa_node = cpu_to_node(c->cpu);
 	err = mlx5_wq_cyc_create(mdev, &param->wq, sqc_wq, &sq->wq, &sq->wq_ctrl);
@@ -1134,7 +1166,7 @@ static int mlx5e_create_sq(struct mlx5_core_dev *mdev,
 
 	inlen = MLX5_ST_SZ_BYTES(create_sq_in) +
 		sizeof(u64) * csp->wq_ctrl->buf.npages;
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -1182,7 +1214,7 @@ static int mlx5e_modify_sq(struct mlx5_core_dev *mdev, u32 sqn,
 	int err;
 
 	inlen = MLX5_ST_SZ_BYTES(modify_sq_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -1496,7 +1528,7 @@ static int mlx5e_create_cq(struct mlx5e_cq *cq, struct mlx5e_cq_param *param)
 
 	inlen = MLX5_ST_SZ_BYTES(create_cq_in) +
 		sizeof(u64) * cq->wq_ctrl.frag_buf.npages;
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -1905,6 +1937,7 @@ static void mlx5e_build_sq_param(struct mlx5e_priv *priv,
 
 	mlx5e_build_sq_param_common(priv, param);
 	MLX5_SET(wq, wq, log_wq_sz, params->log_sq_size);
+	MLX5_SET(sqc, sqc, allow_swp, !!MLX5_IPSEC_DEV(priv->mdev));
 }
 
 static void mlx5e_build_common_cq_param(struct mlx5e_priv *priv,
@@ -2091,7 +2124,7 @@ mlx5e_create_rqt(struct mlx5e_priv *priv, int sz, struct mlx5e_rqt *rqt)
 	int i;
 
 	inlen = MLX5_ST_SZ_BYTES(create_rqt_in) + sizeof(u32) * sz;
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -2210,7 +2243,7 @@ int mlx5e_redirect_rqt(struct mlx5e_priv *priv, u32 rqtn, int sz,
 	int err;
 
 	inlen = MLX5_ST_SZ_BYTES(modify_rqt_in) + sizeof(u32) * sz;
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -2433,7 +2466,7 @@ static int mlx5e_modify_tirs_lro(struct mlx5e_priv *priv)
 	int ix;
 
 	inlen = MLX5_ST_SZ_BYTES(modify_tir_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -2465,7 +2498,7 @@ free_in:
 static int mlx5e_set_mtu(struct mlx5e_priv *priv, u16 mtu)
 {
 	struct mlx5_core_dev *mdev = priv->mdev;
-	u16 hw_mtu = MLX5E_SW2HW_MTU(mtu);
+	u16 hw_mtu = MLX5E_SW2HW_MTU(priv, mtu);
 	int err;
 
 	err = mlx5_set_port_mtu(mdev, hw_mtu, 1);
@@ -2487,7 +2520,7 @@ static void mlx5e_query_mtu(struct mlx5e_priv *priv, u16 *mtu)
 	if (err || !hw_mtu) /* fallback to port oper mtu */
 		mlx5_query_port_oper_mtu(mdev, &hw_mtu, 1);
 
-	*mtu = MLX5E_HW2SW_MTU(hw_mtu);
+	*mtu = MLX5E_HW2SW_MTU(priv, hw_mtu);
 }
 
 static int mlx5e_set_dev_port_mtu(struct mlx5e_priv *priv)
@@ -2596,9 +2629,10 @@ void mlx5e_switch_priv_channels(struct mlx5e_priv *priv,
 {
 	struct net_device *netdev = priv->netdev;
 	int new_num_txqs;
-
+	int carrier_ok;
 	new_num_txqs = new_chs->num * new_chs->params.num_tc;
 
+	carrier_ok = netif_carrier_ok(netdev);
 	netif_carrier_off(netdev);
 
 	if (new_num_txqs < netdev->real_num_tx_queues)
@@ -2616,7 +2650,9 @@ void mlx5e_switch_priv_channels(struct mlx5e_priv *priv,
 	mlx5e_refresh_tirs(priv, false);
 	mlx5e_activate_priv_channels(priv);
 
-	mlx5e_update_carrier(priv);
+	/* return carrier back if needed */
+	if (carrier_ok)
+		netif_carrier_on(netdev);
 }
 
 int mlx5e_open_locked(struct net_device *netdev)
@@ -2632,7 +2668,8 @@ int mlx5e_open_locked(struct net_device *netdev)
 
 	mlx5e_refresh_tirs(priv, false);
 	mlx5e_activate_priv_channels(priv);
-	mlx5e_update_carrier(priv);
+	if (priv->profile->update_carrier)
+		priv->profile->update_carrier(priv);
 	mlx5e_timestamp_init(priv);
 
 	if (priv->profile->update_stats)
@@ -2850,7 +2887,7 @@ int mlx5e_create_indirect_tirs(struct mlx5e_priv *priv)
 	int tt;
 
 	inlen = MLX5_ST_SZ_BYTES(create_tir_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -2889,7 +2926,7 @@ int mlx5e_create_direct_tirs(struct mlx5e_priv *priv)
 	int ix;
 
 	inlen = MLX5_ST_SZ_BYTES(create_tir_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -2992,13 +3029,17 @@ out:
 }
 
 static int mlx5e_ndo_setup_tc(struct net_device *dev, u32 handle,
-			      __be16 proto, struct tc_to_netdev *tc)
+			      u32 chain_index, __be16 proto,
+			      struct tc_to_netdev *tc)
 {
 	struct mlx5e_priv *priv = netdev_priv(dev);
 
 	if (TC_H_MAJ(handle) != TC_H_MAJ(TC_H_INGRESS))
 		goto mqprio;
 
+	if (chain_index)
+		return -EOPNOTSUPP;
+
 	switch (tc->type) {
 	case TC_SETUP_CLSFLOWER:
 		switch (tc->cls_flower->command) {
@@ -3053,8 +3094,6 @@ mlx5e_get_stats(struct net_device *dev, struct rtnl_link_stats64 *stats)
 		PPORT_802_3_GET(pstats, a_frame_check_sequence_errors);
 	stats->rx_frame_errors = PPORT_802_3_GET(pstats, a_alignment_errors);
 	stats->tx_aborted_errors = PPORT_2863_GET(pstats, if_out_discards);
-	stats->tx_carrier_errors =
-		PPORT_802_3_GET(pstats, a_symbol_error_during_carrier);
 	stats->rx_errors = stats->rx_length_errors + stats->rx_crc_errors +
 			   stats->rx_frame_errors;
 	stats->tx_errors = stats->tx_aborted_errors + stats->tx_carrier_errors;
@@ -3064,7 +3103,6 @@ mlx5e_get_stats(struct net_device *dev, struct rtnl_link_stats64 *stats)
 	 */
 	stats->multicast =
 		VPORT_COUNTER_GET(vstats, received_eth_multicast.packets);
-
 }
 
 static void mlx5e_set_rx_mode(struct net_device *dev)
@@ -3307,11 +3345,13 @@ out:
 
 static int mlx5e_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 {
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
 	switch (cmd) {
 	case SIOCSHWTSTAMP:
-		return mlx5e_hwstamp_set(dev, ifr);
+		return mlx5e_hwstamp_set(priv, ifr);
 	case SIOCGHWTSTAMP:
-		return mlx5e_hwstamp_get(dev, ifr);
+		return mlx5e_hwstamp_get(priv, ifr);
 	default:
 		return -EOPNOTSUPP;
 	}
@@ -3490,6 +3530,11 @@ static netdev_features_t mlx5e_features_check(struct sk_buff *skb,
 	features = vlan_features_check(skb, features);
 	features = vxlan_features_check(skb, features);
 
+#ifdef CONFIG_MLX5_EN_IPSEC
+	if (mlx5e_ipsec_feature_check(skb, netdev, features))
+		return features;
+#endif
+
 	/* Validate if the tunneled packet is being offloaded by HW */
 	if (skb->encapsulation &&
 	    (features & NETIF_F_CSUM_MASK || features & NETIF_F_GSO_MASK))
@@ -3537,6 +3582,12 @@ static int mlx5e_xdp_set(struct net_device *netdev, struct bpf_prog *prog)
 		goto unlock;
 	}
 
+	if ((netdev->features & NETIF_F_HW_ESP) && prog) {
+		netdev_warn(netdev, "can't set XDP with IPSec offload\n");
+		err = -EINVAL;
+		goto unlock;
+	}
+
 	was_opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
 	/* no need for full reset when exchanging programs */
 	reset = (!priv->channels.params.xdp_prog || !prog);
@@ -3596,11 +3647,19 @@ unlock:
 	return err;
 }
 
-static bool mlx5e_xdp_attached(struct net_device *dev)
+static u32 mlx5e_xdp_query(struct net_device *dev)
 {
 	struct mlx5e_priv *priv = netdev_priv(dev);
+	const struct bpf_prog *xdp_prog;
+	u32 prog_id = 0;
+
+	mutex_lock(&priv->state_lock);
+	xdp_prog = priv->channels.params.xdp_prog;
+	if (xdp_prog)
+		prog_id = xdp_prog->aux->id;
+	mutex_unlock(&priv->state_lock);
 
-	return !!priv->channels.params.xdp_prog;
+	return prog_id;
 }
 
 static int mlx5e_xdp(struct net_device *dev, struct netdev_xdp *xdp)
@@ -3609,7 +3668,8 @@ static int mlx5e_xdp(struct net_device *dev, struct netdev_xdp *xdp)
 	case XDP_SETUP_PROG:
 		return mlx5e_xdp_set(dev, xdp->prog);
 	case XDP_QUERY_PROG:
-		xdp->prog_attached = mlx5e_xdp_attached(dev);
+		xdp->prog_id = mlx5e_xdp_query(dev);
+		xdp->prog_attached = !!xdp->prog_id;
 		return 0;
 	default:
 		return -EINVAL;
@@ -3715,7 +3775,7 @@ static int mlx5e_check_required_hca_cap(struct mlx5_core_dev *mdev)
 	if (!MLX5_CAP_ETH(mdev, self_lb_en_modifiable))
 		mlx5_core_warn(mdev, "Self loop back prevention is not supported\n");
 	if (!MLX5_CAP_GEN(mdev, cq_moderation))
-		mlx5_core_warn(mdev, "CQ modiration is not supported\n");
+		mlx5_core_warn(mdev, "CQ moderation is not supported\n");
 
 	return 0;
 }
@@ -3848,7 +3908,7 @@ void mlx5e_build_nic_params(struct mlx5_core_dev *mdev,
 	/* set CQE compression */
 	params->rx_cqe_compress_def = false;
 	if (MLX5_CAP_GEN(mdev, cqe_compression) &&
-	     MLX5_CAP_GEN(mdev, vport_group_manager))
+	    MLX5_CAP_GEN(mdev, vport_group_manager))
 		params->rx_cqe_compress_def = cqe_compress_heuristic(link_speed, pci_bw);
 
 	MLX5E_SET_PFLAG(params, MLX5E_PFLAG_RX_CQE_COMPRESS, params->rx_cqe_compress_def);
@@ -3857,6 +3917,7 @@ void mlx5e_build_nic_params(struct mlx5_core_dev *mdev,
 	mlx5e_set_rq_params(mdev, params);
 
 	/* HW LRO */
+
 	/* TODO: && MLX5_CAP_ETH(mdev, lro_cap) */
 	if (params->rq_wq_type == MLX5_WQ_TYPE_LINKED_LIST_STRIDING_RQ)
 		params->lro_en = hw_lro_heuristic(link_speed, pci_bw);
@@ -3897,6 +3958,7 @@ static void mlx5e_build_nic_netdev_priv(struct mlx5_core_dev *mdev,
 	priv->netdev      = netdev;
 	priv->profile     = profile;
 	priv->ppriv       = ppriv;
+	priv->hard_mtu = MLX5E_ETH_HARD_MTU;
 
 	mlx5e_build_nic_params(mdev, &priv->channels.params, profile->max_nch(mdev));
 
@@ -4017,6 +4079,8 @@ static void mlx5e_build_nic_netdev(struct net_device *netdev)
 	if (MLX5_CAP_GEN(mdev, vport_group_manager))
 		netdev->switchdev_ops = &mlx5e_switchdev_ops;
 #endif
+
+	mlx5e_ipsec_build_netdev(priv);
 }
 
 static void mlx5e_create_q_counter(struct mlx5e_priv *priv)
@@ -4045,14 +4109,19 @@ static void mlx5e_nic_init(struct mlx5_core_dev *mdev,
 			   void *ppriv)
 {
 	struct mlx5e_priv *priv = netdev_priv(netdev);
+	int err;
 
 	mlx5e_build_nic_netdev_priv(mdev, netdev, profile, ppriv);
+	err = mlx5e_ipsec_init(priv);
+	if (err)
+		mlx5_core_err(mdev, "IPSec initialization failed, %d\n", err);
 	mlx5e_build_nic_netdev(netdev);
 	mlx5e_vxlan_init(priv);
 }
 
 static void mlx5e_nic_cleanup(struct mlx5e_priv *priv)
 {
+	mlx5e_ipsec_cleanup(priv);
 	mlx5e_vxlan_cleanup(priv);
 
 	if (priv->channels.params.xdp_prog)
@@ -4142,7 +4211,7 @@ static void mlx5e_nic_enable(struct mlx5e_priv *priv)
 	/* MTU range: 68 - hw-specific max */
 	netdev->min_mtu = ETH_MIN_MTU;
 	mlx5_query_port_max_mtu(priv->mdev, &max_mtu, 1);
-	netdev->max_mtu = MLX5E_HW2SW_MTU(max_mtu);
+	netdev->max_mtu = MLX5E_HW2SW_MTU(priv, max_mtu);
 	mlx5e_set_dev_port_mtu(priv);
 
 	mlx5_lag_add(mdev, netdev);
@@ -4199,8 +4268,9 @@ static const struct mlx5e_profile mlx5e_nic_profile = {
 	.cleanup_tx	   = mlx5e_cleanup_nic_tx,
 	.enable		   = mlx5e_nic_enable,
 	.disable	   = mlx5e_nic_disable,
-	.update_stats	   = mlx5e_update_stats,
+	.update_stats	   = mlx5e_update_ndo_stats,
 	.max_nch	   = mlx5e_get_max_num_channels,
+	.update_carrier	   = mlx5e_update_carrier,
 	.rx_handlers.handle_rx_cqe       = mlx5e_handle_rx_cqe,
 	.rx_handlers.handle_rx_cqe_mpwqe = mlx5e_handle_rx_cqe_mpwrq,
 	.max_tc		   = MLX5E_MAX_NUM_TC,
@@ -4241,7 +4311,8 @@ struct net_device *mlx5e_create_netdev(struct mlx5_core_dev *mdev,
 	return netdev;
 
 err_cleanup_nic:
-	profile->cleanup(priv);
+	if (profile->cleanup)
+		profile->cleanup(priv);
 	free_netdev(netdev);
 
 	return NULL;
@@ -4442,6 +4513,7 @@ static struct mlx5_interface mlx5e_interface = {
 
 void mlx5e_init(void)
 {
+	mlx5e_ipsec_build_inverse_table();
 	mlx5e_build_ptys2ethtool_map();
 	mlx5_register_interface(&mlx5e_interface);
 }
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_rep.c b/drivers/net/ethernet/mellanox/mlx5/core/en_rep.c
index 79462c0368a0..45e60be9c277 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_rep.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_rep.c
@@ -652,7 +652,8 @@ static int mlx5e_rep_get_phys_port_name(struct net_device *dev,
 }
 
 static int mlx5e_rep_ndo_setup_tc(struct net_device *dev, u32 handle,
-				  __be16 proto, struct tc_to_netdev *tc)
+				  u32 chain_index, __be16 proto,
+				  struct tc_to_netdev *tc)
 {
 	struct mlx5e_priv *priv = netdev_priv(dev);
 
@@ -664,9 +665,13 @@ static int mlx5e_rep_ndo_setup_tc(struct net_device *dev, u32 handle,
 		struct net_device *uplink_dev = mlx5_eswitch_get_uplink_netdev(esw);
 
 		return uplink_dev->netdev_ops->ndo_setup_tc(uplink_dev, handle,
+							    chain_index,
 							    proto, tc);
 	}
 
+	if (chain_index)
+		return -EOPNOTSUPP;
+
 	switch (tc->type) {
 	case TC_SETUP_CLSFLOWER:
 		switch (tc->cls_flower->command) {
@@ -791,6 +796,8 @@ static void mlx5e_build_rep_params(struct mlx5_core_dev *mdev,
 	params->tx_max_inline         = mlx5e_get_max_inline_cap(mdev);
 	params->num_tc                = 1;
 	params->lro_wqe_sz            = MLX5E_PARAMS_DEFAULT_LRO_WQE_SZ;
+
+	mlx5_query_min_inline(mdev, &params->tx_min_inline_mode);
 }
 
 static void mlx5e_build_rep_netdev(struct net_device *netdev)
@@ -828,6 +835,9 @@ static void mlx5e_init_rep(struct mlx5_core_dev *mdev,
 	INIT_DELAYED_WORK(&priv->update_stats_work, mlx5e_update_stats_work);
 
 	priv->channels.params.num_channels = profile->max_nch(mdev);
+
+	priv->hard_mtu = MLX5E_ETH_HARD_MTU;
+
 	mlx5e_build_rep_params(mdev, &priv->channels.params);
 	mlx5e_build_rep_netdev(netdev);
 }
@@ -911,6 +921,7 @@ static struct mlx5e_profile mlx5e_rep_profile = {
 	.cleanup_tx		= mlx5e_cleanup_nic_tx,
 	.update_stats           = mlx5e_rep_update_stats,
 	.max_nch		= mlx5e_get_rep_max_num_channels,
+	.update_carrier		= NULL,
 	.rx_handlers.handle_rx_cqe       = mlx5e_handle_rx_cqe_rep,
 	.rx_handlers.handle_rx_cqe_mpwqe = NULL /* Not supported */,
 	.max_tc			= 1,
@@ -1014,7 +1025,6 @@ err_destroy_netdev:
 	mlx5e_destroy_netdev(netdev_priv(netdev));
 	kfree(rpriv);
 	return err;
-
 }
 
 static void
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
index 66b5fec15313..325b2c8c1c6d 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
@@ -40,7 +40,8 @@
 #include "en_tc.h"
 #include "eswitch.h"
 #include "en_rep.h"
-#include "ipoib.h"
+#include "ipoib/ipoib.h"
+#include "en_accel/ipsec_rxtx.h"
 
 static inline bool mlx5e_rx_hw_stamp(struct mlx5e_tstamp *tstamp)
 {
@@ -160,6 +161,11 @@ static inline u32 mlx5e_decompress_cqes_start(struct mlx5e_rq *rq,
 
 #define RQ_PAGE_SIZE(rq) ((1 << rq->buff.page_order) << PAGE_SHIFT)
 
+static inline bool mlx5e_page_is_reserved(struct page *page)
+{
+	return page_is_pfmemalloc(page) || page_to_nid(page) != numa_node_id();
+}
+
 static inline bool mlx5e_rx_cache_put(struct mlx5e_rq *rq,
 				      struct mlx5e_dma_info *dma_info)
 {
@@ -238,22 +244,54 @@ void mlx5e_page_release(struct mlx5e_rq *rq, struct mlx5e_dma_info *dma_info,
 	put_page(dma_info->page);
 }
 
+static inline bool mlx5e_page_reuse(struct mlx5e_rq *rq,
+				    struct mlx5e_wqe_frag_info *wi)
+{
+	return rq->wqe.page_reuse && wi->di.page &&
+		(wi->offset + rq->wqe.frag_sz <= RQ_PAGE_SIZE(rq)) &&
+		!mlx5e_page_is_reserved(wi->di.page);
+}
+
 int mlx5e_alloc_rx_wqe(struct mlx5e_rq *rq, struct mlx5e_rx_wqe *wqe, u16 ix)
 {
-	struct mlx5e_dma_info *di = &rq->dma_info[ix];
+	struct mlx5e_wqe_frag_info *wi = &rq->wqe.frag_info[ix];
 
-	if (unlikely(mlx5e_page_alloc_mapped(rq, di)))
-		return -ENOMEM;
+	/* check if page exists, hence can be reused */
+	if (!wi->di.page) {
+		if (unlikely(mlx5e_page_alloc_mapped(rq, &wi->di)))
+			return -ENOMEM;
+		wi->offset = 0;
+	}
 
-	wqe->data.addr = cpu_to_be64(di->addr + rq->rx_headroom);
+	wqe->data.addr = cpu_to_be64(wi->di.addr + wi->offset +
+				     rq->rx_headroom);
 	return 0;
 }
 
+static inline void mlx5e_free_rx_wqe(struct mlx5e_rq *rq,
+				     struct mlx5e_wqe_frag_info *wi)
+{
+	mlx5e_page_release(rq, &wi->di, true);
+	wi->di.page = NULL;
+}
+
+static inline void mlx5e_free_rx_wqe_reuse(struct mlx5e_rq *rq,
+					   struct mlx5e_wqe_frag_info *wi)
+{
+	if (mlx5e_page_reuse(rq, wi)) {
+		rq->stats.page_reuse++;
+		return;
+	}
+
+	mlx5e_free_rx_wqe(rq, wi);
+}
+
 void mlx5e_dealloc_rx_wqe(struct mlx5e_rq *rq, u16 ix)
 {
-	struct mlx5e_dma_info *di = &rq->dma_info[ix];
+	struct mlx5e_wqe_frag_info *wi = &rq->wqe.frag_info[ix];
 
-	mlx5e_page_release(rq, di, true);
+	if (wi->di.page)
+		mlx5e_free_rx_wqe(rq, wi);
 }
 
 static inline int mlx5e_mpwqe_strides_per_page(struct mlx5e_rq *rq)
@@ -648,9 +686,8 @@ static inline bool mlx5e_xmit_xdp_frame(struct mlx5e_rq *rq,
 	prefetchw(wqe);
 
 	if (unlikely(dma_len < MLX5E_XDP_MIN_INLINE ||
-		     MLX5E_SW2HW_MTU(rq->netdev->mtu) < dma_len)) {
+		     MLX5E_SW2HW_MTU(rq->channel->priv, rq->netdev->mtu) < dma_len)) {
 		rq->stats.xdp_drop++;
-		mlx5e_page_release(rq, di, true);
 		return false;
 	}
 
@@ -661,7 +698,6 @@ static inline bool mlx5e_xmit_xdp_frame(struct mlx5e_rq *rq,
 			sq->db.doorbell = false;
 		}
 		rq->stats.xdp_tx_full++;
-		mlx5e_page_release(rq, di, true);
 		return false;
 	}
 
@@ -686,10 +722,15 @@ static inline bool mlx5e_xmit_xdp_frame(struct mlx5e_rq *rq,
 
 	cseg->opmod_idx_opcode = cpu_to_be32((sq->pc << 8) | MLX5_OPCODE_SEND);
 
+	/* move page to reference to sq responsibility,
+	 * and mark so it's not put back in page-cache.
+	 */
+	rq->wqe.xdp_xmit = true;
 	sq->db.di[pi] = *di;
 	sq->pc++;
 
 	sq->db.doorbell = true;
+
 	rq->stats.xdp_tx++;
 	return true;
 }
@@ -726,35 +767,34 @@ static inline int mlx5e_xdp_handle(struct mlx5e_rq *rq,
 		trace_xdp_exception(rq->netdev, prog, act);
 	case XDP_DROP:
 		rq->stats.xdp_drop++;
-		mlx5e_page_release(rq, di, true);
 		return true;
 	}
 }
 
 static inline
 struct sk_buff *skb_from_cqe(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe,
-			     u16 wqe_counter, u32 cqe_bcnt)
+			     struct mlx5e_wqe_frag_info *wi, u32 cqe_bcnt)
 {
-	struct mlx5e_dma_info *di;
+	struct mlx5e_dma_info *di = &wi->di;
 	struct sk_buff *skb;
 	void *va, *data;
 	u16 rx_headroom = rq->rx_headroom;
 	bool consumed;
+	u32 frag_size;
 
-	di             = &rq->dma_info[wqe_counter];
-	va             = page_address(di->page);
+	va             = page_address(di->page) + wi->offset;
 	data           = va + rx_headroom;
+	frag_size      = MLX5_SKB_FRAG_SZ(rx_headroom + cqe_bcnt);
 
 	dma_sync_single_range_for_cpu(rq->pdev,
-				      di->addr,
-				      rx_headroom,
-				      rq->buff.wqe_sz,
+				      di->addr + wi->offset,
+				      0, frag_size,
 				      DMA_FROM_DEVICE);
 	prefetch(data);
+	wi->offset += frag_size;
 
 	if (unlikely((cqe->op_own >> 4) != MLX5_CQE_RESP_SEND)) {
 		rq->stats.wqe_err++;
-		mlx5e_page_release(rq, di, true);
 		return NULL;
 	}
 
@@ -764,16 +804,14 @@ struct sk_buff *skb_from_cqe(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe,
 	if (consumed)
 		return NULL; /* page/packet was consumed by XDP */
 
-	skb = build_skb(va, RQ_PAGE_SIZE(rq));
+	skb = build_skb(va, frag_size);
 	if (unlikely(!skb)) {
 		rq->stats.buff_alloc_err++;
-		mlx5e_page_release(rq, di, true);
 		return NULL;
 	}
 
-	/* queue up for recycling ..*/
+	/* queue up for recycling/reuse */
 	page_ref_inc(di->page);
-	mlx5e_page_release(rq, di, true);
 
 	skb_reserve(skb, rx_headroom);
 	skb_put(skb, cqe_bcnt);
@@ -783,6 +821,7 @@ struct sk_buff *skb_from_cqe(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe,
 
 void mlx5e_handle_rx_cqe(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe)
 {
+	struct mlx5e_wqe_frag_info *wi;
 	struct mlx5e_rx_wqe *wqe;
 	__be16 wqe_counter_be;
 	struct sk_buff *skb;
@@ -792,15 +831,27 @@ void mlx5e_handle_rx_cqe(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe)
 	wqe_counter_be = cqe->wqe_counter;
 	wqe_counter    = be16_to_cpu(wqe_counter_be);
 	wqe            = mlx5_wq_ll_get_wqe(&rq->wq, wqe_counter);
+	wi             = &rq->wqe.frag_info[wqe_counter];
 	cqe_bcnt       = be32_to_cpu(cqe->byte_cnt);
 
-	skb = skb_from_cqe(rq, cqe, wqe_counter, cqe_bcnt);
-	if (!skb)
+	skb = skb_from_cqe(rq, cqe, wi, cqe_bcnt);
+	if (!skb) {
+		/* probably for XDP */
+		if (rq->wqe.xdp_xmit) {
+			wi->di.page = NULL;
+			rq->wqe.xdp_xmit = false;
+			/* do not return page to cache, it will be returned on XDP_TX completion */
+			goto wq_ll_pop;
+		}
+		/* probably an XDP_DROP, save the page-reuse checks */
+		mlx5e_free_rx_wqe(rq, wi);
 		goto wq_ll_pop;
+	}
 
 	mlx5e_complete_rx_cqe(rq, cqe, cqe_bcnt, skb);
 	napi_gro_receive(rq->cq.napi, skb);
 
+	mlx5e_free_rx_wqe_reuse(rq, wi);
 wq_ll_pop:
 	mlx5_wq_ll_pop(&rq->wq, wqe_counter_be,
 		       &wqe->next.next_wqe_index);
@@ -812,6 +863,7 @@ void mlx5e_handle_rx_cqe_rep(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe)
 	struct mlx5e_priv *priv = netdev_priv(netdev);
 	struct mlx5e_rep_priv *rpriv  = priv->ppriv;
 	struct mlx5_eswitch_rep *rep = rpriv->rep;
+	struct mlx5e_wqe_frag_info *wi;
 	struct mlx5e_rx_wqe *wqe;
 	struct sk_buff *skb;
 	__be16 wqe_counter_be;
@@ -821,11 +873,21 @@ void mlx5e_handle_rx_cqe_rep(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe)
 	wqe_counter_be = cqe->wqe_counter;
 	wqe_counter    = be16_to_cpu(wqe_counter_be);
 	wqe            = mlx5_wq_ll_get_wqe(&rq->wq, wqe_counter);
+	wi             = &rq->wqe.frag_info[wqe_counter];
 	cqe_bcnt       = be32_to_cpu(cqe->byte_cnt);
 
-	skb = skb_from_cqe(rq, cqe, wqe_counter, cqe_bcnt);
-	if (!skb)
+	skb = skb_from_cqe(rq, cqe, wi, cqe_bcnt);
+	if (!skb) {
+		if (rq->wqe.xdp_xmit) {
+			wi->di.page = NULL;
+			rq->wqe.xdp_xmit = false;
+			/* do not return page to cache, it will be returned on XDP_TX completion */
+			goto wq_ll_pop;
+		}
+		/* probably an XDP_DROP, save the page-reuse checks */
+		mlx5e_free_rx_wqe(rq, wi);
 		goto wq_ll_pop;
+	}
 
 	mlx5e_complete_rx_cqe(rq, cqe, cqe_bcnt, skb);
 
@@ -834,6 +896,7 @@ void mlx5e_handle_rx_cqe_rep(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe)
 
 	napi_gro_receive(rq->cq.napi, skb);
 
+	mlx5e_free_rx_wqe_reuse(rq, wi);
 wq_ll_pop:
 	mlx5_wq_ll_pop(&rq->wq, wqe_counter_be,
 		       &wqe->next.next_wqe_index);
@@ -934,7 +997,7 @@ int mlx5e_poll_rx_cq(struct mlx5e_cq *cq, int budget)
 		work_done += mlx5e_decompress_cqes_cont(rq, cq, 0, budget);
 
 	for (; work_done < budget; work_done++) {
-		struct mlx5_cqe64 *cqe = mlx5e_get_cqe(cq);
+		struct mlx5_cqe64 *cqe = mlx5_cqwq_get_cqe(&cq->wq);
 
 		if (!cqe)
 			break;
@@ -988,7 +1051,7 @@ bool mlx5e_poll_xdpsq_cq(struct mlx5e_cq *cq)
 		u16 wqe_counter;
 		bool last_wqe;
 
-		cqe = mlx5e_get_cqe(cq);
+		cqe = mlx5_cqwq_get_cqe(&cq->wq);
 		if (!cqe)
 			break;
 
@@ -1038,11 +1101,7 @@ void mlx5e_free_xdpsq_descs(struct mlx5e_xdpsq *sq)
 #ifdef CONFIG_MLX5_CORE_IPOIB
 
 #define MLX5_IB_GRH_DGID_OFFSET 24
-#define MLX5_IB_GRH_BYTES       40
-#define MLX5_IPOIB_ENCAP_LEN    4
 #define MLX5_GID_SIZE           16
-#define MLX5_IPOIB_PSEUDO_LEN   20
-#define MLX5_IPOIB_HARD_LEN     (MLX5_IPOIB_PSEUDO_LEN + MLX5_IPOIB_ENCAP_LEN)
 
 static inline void mlx5i_complete_rx_cqe(struct mlx5e_rq *rq,
 					 struct mlx5_cqe64 *cqe,
@@ -1050,6 +1109,7 @@ static inline void mlx5i_complete_rx_cqe(struct mlx5e_rq *rq,
 					 struct sk_buff *skb)
 {
 	struct net_device *netdev = rq->netdev;
+	struct mlx5e_tstamp *tstamp = rq->tstamp;
 	char *pseudo_header;
 	u8 *dgid;
 	u8 g;
@@ -1074,6 +1134,9 @@ static inline void mlx5i_complete_rx_cqe(struct mlx5e_rq *rq,
 	skb->ip_summed = CHECKSUM_COMPLETE;
 	skb->csum = csum_unfold((__force __sum16)cqe->check_sum);
 
+	if (unlikely(mlx5e_rx_hw_stamp(tstamp)))
+		mlx5e_fill_hwstamp(tstamp, get_cqe_ts(cqe), skb_hwtstamps(skb));
+
 	skb_record_rx_queue(skb, rq->ix);
 
 	if (likely(netdev->features & NETIF_F_RXHASH))
@@ -1094,6 +1157,7 @@ static inline void mlx5i_complete_rx_cqe(struct mlx5e_rq *rq,
 
 void mlx5i_handle_rx_cqe(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe)
 {
+	struct mlx5e_wqe_frag_info *wi;
 	struct mlx5e_rx_wqe *wqe;
 	__be16 wqe_counter_be;
 	struct sk_buff *skb;
@@ -1103,18 +1167,60 @@ void mlx5i_handle_rx_cqe(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe)
 	wqe_counter_be = cqe->wqe_counter;
 	wqe_counter    = be16_to_cpu(wqe_counter_be);
 	wqe            = mlx5_wq_ll_get_wqe(&rq->wq, wqe_counter);
+	wi             = &rq->wqe.frag_info[wqe_counter];
 	cqe_bcnt       = be32_to_cpu(cqe->byte_cnt);
 
-	skb = skb_from_cqe(rq, cqe, wqe_counter, cqe_bcnt);
+	skb = skb_from_cqe(rq, cqe, wi, cqe_bcnt);
 	if (!skb)
-		goto wq_ll_pop;
+		goto wq_free_wqe;
 
 	mlx5i_complete_rx_cqe(rq, cqe, cqe_bcnt, skb);
 	napi_gro_receive(rq->cq.napi, skb);
 
-wq_ll_pop:
+wq_free_wqe:
+	mlx5e_free_rx_wqe_reuse(rq, wi);
 	mlx5_wq_ll_pop(&rq->wq, wqe_counter_be,
 		       &wqe->next.next_wqe_index);
 }
 
 #endif /* CONFIG_MLX5_CORE_IPOIB */
+
+#ifdef CONFIG_MLX5_EN_IPSEC
+
+void mlx5e_ipsec_handle_rx_cqe(struct mlx5e_rq *rq, struct mlx5_cqe64 *cqe)
+{
+	struct mlx5e_wqe_frag_info *wi;
+	struct mlx5e_rx_wqe *wqe;
+	__be16 wqe_counter_be;
+	struct sk_buff *skb;
+	u16 wqe_counter;
+	u32 cqe_bcnt;
+
+	wqe_counter_be = cqe->wqe_counter;
+	wqe_counter    = be16_to_cpu(wqe_counter_be);
+	wqe            = mlx5_wq_ll_get_wqe(&rq->wq, wqe_counter);
+	wi             = &rq->wqe.frag_info[wqe_counter];
+	cqe_bcnt       = be32_to_cpu(cqe->byte_cnt);
+
+	skb = skb_from_cqe(rq, cqe, wi, cqe_bcnt);
+	if (unlikely(!skb)) {
+		/* a DROP, save the page-reuse checks */
+		mlx5e_free_rx_wqe(rq, wi);
+		goto wq_ll_pop;
+	}
+	skb = mlx5e_ipsec_handle_rx_skb(rq->netdev, skb);
+	if (unlikely(!skb)) {
+		mlx5e_free_rx_wqe(rq, wi);
+		goto wq_ll_pop;
+	}
+
+	mlx5e_complete_rx_cqe(rq, cqe, cqe_bcnt, skb);
+	napi_gro_receive(rq->cq.napi, skb);
+
+	mlx5e_free_rx_wqe_reuse(rq, wi);
+wq_ll_pop:
+	mlx5_wq_ll_pop(&rq->wq, wqe_counter_be,
+		       &wqe->next.next_wqe_index);
+}
+
+#endif /* CONFIG_MLX5_EN_IPSEC */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_rx_am.c b/drivers/net/ethernet/mellanox/mlx5/core/en_rx_am.c
index 02dd3a95ed8f..acf32fe952cd 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_rx_am.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_rx_am.c
@@ -183,28 +183,27 @@ static void mlx5e_am_exit_parking(struct mlx5e_rx_am *am)
 	mlx5e_am_step(am);
 }
 
+#define IS_SIGNIFICANT_DIFF(val, ref) \
+	(((100 * abs((val) - (ref))) / (ref)) > 10) /* more than 10% difference */
+
 static int mlx5e_am_stats_compare(struct mlx5e_rx_am_stats *curr,
 				  struct mlx5e_rx_am_stats *prev)
 {
-	int diff;
-
-	if (!prev->ppms)
-		return curr->ppms ? MLX5E_AM_STATS_BETTER :
+	if (!prev->bpms)
+		return curr->bpms ? MLX5E_AM_STATS_BETTER :
 				    MLX5E_AM_STATS_SAME;
 
-	diff = curr->ppms - prev->ppms;
-	if (((100 * abs(diff)) / prev->ppms) > 10) /* more than 10% diff */
-		return (diff > 0) ? MLX5E_AM_STATS_BETTER :
-				    MLX5E_AM_STATS_WORSE;
+	if (IS_SIGNIFICANT_DIFF(curr->bpms, prev->bpms))
+		return (curr->bpms > prev->bpms) ? MLX5E_AM_STATS_BETTER :
+						   MLX5E_AM_STATS_WORSE;
 
-	if (!prev->epms)
-		return curr->epms ? MLX5E_AM_STATS_WORSE :
-				    MLX5E_AM_STATS_SAME;
+	if (IS_SIGNIFICANT_DIFF(curr->ppms, prev->ppms))
+		return (curr->ppms > prev->ppms) ? MLX5E_AM_STATS_BETTER :
+						   MLX5E_AM_STATS_WORSE;
 
-	diff = curr->epms - prev->epms;
-	if (((100 * abs(diff)) / prev->epms) > 10) /* more than 10% diff */
-		return (diff < 0) ? MLX5E_AM_STATS_BETTER :
-				    MLX5E_AM_STATS_WORSE;
+	if (IS_SIGNIFICANT_DIFF(curr->epms, prev->epms))
+		return (curr->epms < prev->epms) ? MLX5E_AM_STATS_BETTER :
+						   MLX5E_AM_STATS_WORSE;
 
 	return MLX5E_AM_STATS_SAME;
 }
@@ -266,10 +265,13 @@ static void mlx5e_am_sample(struct mlx5e_rq *rq,
 {
 	s->time	     = ktime_get();
 	s->pkt_ctr   = rq->stats.packets;
+	s->byte_ctr  = rq->stats.bytes;
 	s->event_ctr = rq->cq.event_ctr;
 }
 
 #define MLX5E_AM_NEVENTS 64
+#define BITS_PER_TYPE(type) (sizeof(type) * BITS_PER_BYTE)
+#define BIT_GAP(bits, end, start) ((((end) - (start)) + BIT_ULL(bits)) & (BIT_ULL(bits) - 1))
 
 static void mlx5e_am_calc_stats(struct mlx5e_rx_am_sample *start,
 				struct mlx5e_rx_am_sample *end,
@@ -277,13 +279,17 @@ static void mlx5e_am_calc_stats(struct mlx5e_rx_am_sample *start,
 {
 	/* u32 holds up to 71 minutes, should be enough */
 	u32 delta_us = ktime_us_delta(end->time, start->time);
-	unsigned int npkts = end->pkt_ctr - start->pkt_ctr;
+	u32 npkts = BIT_GAP(BITS_PER_TYPE(u32), end->pkt_ctr, start->pkt_ctr);
+	u32 nbytes = BIT_GAP(BITS_PER_TYPE(u32), end->byte_ctr,
+			     start->byte_ctr);
 
 	if (!delta_us)
 		return;
 
-	curr_stats->ppms =            (npkts * USEC_PER_MSEC) / delta_us;
-	curr_stats->epms = (MLX5E_AM_NEVENTS * USEC_PER_MSEC) / delta_us;
+	curr_stats->ppms = DIV_ROUND_UP(npkts * USEC_PER_MSEC, delta_us);
+	curr_stats->bpms = DIV_ROUND_UP(nbytes * USEC_PER_MSEC, delta_us);
+	curr_stats->epms = DIV_ROUND_UP(MLX5E_AM_NEVENTS * USEC_PER_MSEC,
+					delta_us);
 }
 
 void mlx5e_rx_am_work(struct work_struct *work)
@@ -308,7 +314,8 @@ void mlx5e_rx_am(struct mlx5e_rq *rq)
 
 	switch (am->state) {
 	case MLX5E_AM_MEASURE_IN_PROGRESS:
-		nevents = rq->cq.event_ctr - am->start_sample.event_ctr;
+		nevents = BIT_GAP(BITS_PER_TYPE(u16), rq->cq.event_ctr,
+				  am->start_sample.event_ctr);
 		if (nevents < MLX5E_AM_NEVENTS)
 			break;
 		mlx5e_am_sample(rq, &end_sample);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_selftest.c b/drivers/net/ethernet/mellanox/mlx5/core/en_selftest.c
index 5225f2226a67..898759fcf9ec 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_selftest.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_selftest.c
@@ -132,14 +132,14 @@ static struct sk_buff *mlx5e_test_get_udp_skb(struct mlx5e_priv *priv)
 	skb_reserve(skb, NET_IP_ALIGN);
 
 	/*  Reserve for ethernet and IP header  */
-	ethh = (struct ethhdr *)skb_push(skb, ETH_HLEN);
+	ethh = skb_push(skb, ETH_HLEN);
 	skb_reset_mac_header(skb);
 
 	skb_set_network_header(skb, skb->len);
-	iph = (struct iphdr *)skb_put(skb, sizeof(struct iphdr));
+	iph = skb_put(skb, sizeof(struct iphdr));
 
 	skb_set_transport_header(skb, skb->len);
-	udph = (struct udphdr *)skb_put(skb, sizeof(struct udphdr));
+	udph = skb_put(skb, sizeof(struct udphdr));
 
 	/* Fill ETH header */
 	ether_addr_copy(ethh->h_dest, priv->netdev->dev_addr);
@@ -167,12 +167,12 @@ static struct sk_buff *mlx5e_test_get_udp_skb(struct mlx5e_priv *priv)
 	ip_send_check(iph);
 
 	/* Fill test header and data */
-	mlxh = (struct mlx5ehdr *)skb_put(skb, sizeof(*mlxh));
+	mlxh = skb_put(skb, sizeof(*mlxh));
 	mlxh->version = 0;
 	mlxh->magic = cpu_to_be64(MLX5E_TEST_MAGIC);
 	strlcpy(mlxh->text, mlx5e_test_text, sizeof(mlxh->text));
 	datalen -= sizeof(*mlxh);
-	memset(skb_put(skb, datalen), 0, datalen);
+	skb_put_zero(skb, datalen);
 
 	skb->csum = 0;
 	skb->ip_summed = CHECKSUM_PARTIAL;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_stats.h b/drivers/net/ethernet/mellanox/mlx5/core/en_stats.h
index 53e4992d6511..e65517eafc58 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_stats.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_stats.h
@@ -79,6 +79,7 @@ struct mlx5e_sw_stats {
 	u64 rx_buff_alloc_err;
 	u64 rx_cqe_compress_blks;
 	u64 rx_cqe_compress_pkts;
+	u64 rx_page_reuse;
 	u64 rx_cache_reuse;
 	u64 rx_cache_full;
 	u64 rx_cache_empty;
@@ -117,6 +118,7 @@ static const struct counter_desc sw_stats_desc[] = {
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_buff_alloc_err) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_cqe_compress_blks) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_cqe_compress_pkts) },
+	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_page_reuse) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_cache_reuse) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_cache_full) },
 	{ MLX5E_DECLARE_STAT(struct mlx5e_sw_stats, rx_cache_empty) },
@@ -268,7 +270,7 @@ static const struct counter_desc pport_2819_stats_desc[] = {
 };
 
 static const struct counter_desc pport_phy_statistical_stats_desc[] = {
-	{ "rx_symbol_errors_phy", PPORT_PHY_STATISTICAL_OFF(phy_symbol_errors) },
+	{ "rx_pcs_symbol_err_phy", PPORT_PHY_STATISTICAL_OFF(phy_symbol_errors) },
 	{ "rx_corrected_bits_phy", PPORT_PHY_STATISTICAL_OFF(phy_corrected_bits) },
 };
 
@@ -319,6 +321,7 @@ struct mlx5e_rq_stats {
 	u64 buff_alloc_err;
 	u64 cqe_compress_blks;
 	u64 cqe_compress_pkts;
+	u64 page_reuse;
 	u64 cache_reuse;
 	u64 cache_full;
 	u64 cache_empty;
@@ -341,6 +344,7 @@ static const struct counter_desc rq_stats_desc[] = {
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, buff_alloc_err) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, cqe_compress_blks) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, cqe_compress_pkts) },
+	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, page_reuse) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, cache_reuse) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, cache_full) },
 	{ MLX5E_DECLARE_RX_STAT(struct mlx5e_rq_stats, cache_empty) },
@@ -417,20 +421,13 @@ struct mlx5e_stats {
 };
 
 static const struct counter_desc mlx5e_pme_status_desc[] = {
-	{ "module_plug", 0 },
 	{ "module_unplug", 8 },
 };
 
 static const struct counter_desc mlx5e_pme_error_desc[] = {
-	{ "module_pwr_budget_exd", 0 },  /* power budget exceed */
-	{ "module_long_range", 8 },      /* long range for non MLNX cable */
-	{ "module_bus_stuck", 16 },      /* bus stuck (I2C or data shorted) */
-	{ "module_no_eeprom", 24 },      /* no eeprom/retry time out */
-	{ "module_enforce_part", 32 },   /* enforce part number list */
-	{ "module_unknown_id", 40 },     /* unknown identifier */
-	{ "module_high_temp", 48 },      /* high temperature */
+	{ "module_bus_stuck", 16 },       /* bus stuck (I2C or data shorted) */
+	{ "module_high_temp", 48 },       /* high temperature */
 	{ "module_bad_shorted", 56 },    /* bad or shorted cable/module */
-	{ "module_unknown_status", 64 },
 };
 
 #endif /* __MLX5_EN_STATS_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_tc.c b/drivers/net/ethernet/mellanox/mlx5/core/en_tc.c
index ec63158ab643..3c536f560dd2 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_tc.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_tc.c
@@ -69,7 +69,8 @@ struct mlx5e_tc_flow {
 	u64			cookie;
 	u8			flags;
 	struct mlx5_flow_handle *rule;
-	struct list_head	encap; /* flows sharing the same encap */
+	struct list_head	encap;   /* flows sharing the same encap ID */
+	struct list_head	mod_hdr; /* flows sharing the same mod hdr ID */
 	union {
 		struct mlx5_esw_flow_attr esw_attr[0];
 		struct mlx5_nic_flow_attr nic_attr[0];
@@ -90,6 +91,135 @@ enum {
 #define MLX5E_TC_TABLE_NUM_ENTRIES 1024
 #define MLX5E_TC_TABLE_NUM_GROUPS 4
 
+struct mod_hdr_key {
+	int num_actions;
+	void *actions;
+};
+
+struct mlx5e_mod_hdr_entry {
+	/* a node of a hash table which keeps all the mod_hdr entries */
+	struct hlist_node mod_hdr_hlist;
+
+	/* flows sharing the same mod_hdr entry */
+	struct list_head flows;
+
+	struct mod_hdr_key key;
+
+	u32 mod_hdr_id;
+};
+
+#define MLX5_MH_ACT_SZ MLX5_UN_SZ_BYTES(set_action_in_add_action_in_auto)
+
+static inline u32 hash_mod_hdr_info(struct mod_hdr_key *key)
+{
+	return jhash(key->actions,
+		     key->num_actions * MLX5_MH_ACT_SZ, 0);
+}
+
+static inline int cmp_mod_hdr_info(struct mod_hdr_key *a,
+				   struct mod_hdr_key *b)
+{
+	if (a->num_actions != b->num_actions)
+		return 1;
+
+	return memcmp(a->actions, b->actions, a->num_actions * MLX5_MH_ACT_SZ);
+}
+
+static int mlx5e_attach_mod_hdr(struct mlx5e_priv *priv,
+				struct mlx5e_tc_flow *flow,
+				struct mlx5e_tc_flow_parse_attr *parse_attr)
+{
+	struct mlx5_eswitch *esw = priv->mdev->priv.eswitch;
+	int num_actions, actions_size, namespace, err;
+	struct mlx5e_mod_hdr_entry *mh;
+	struct mod_hdr_key key;
+	bool found = false;
+	u32 hash_key;
+
+	num_actions  = parse_attr->num_mod_hdr_actions;
+	actions_size = MLX5_MH_ACT_SZ * num_actions;
+
+	key.actions = parse_attr->mod_hdr_actions;
+	key.num_actions = num_actions;
+
+	hash_key = hash_mod_hdr_info(&key);
+
+	if (flow->flags & MLX5E_TC_FLOW_ESWITCH) {
+		namespace = MLX5_FLOW_NAMESPACE_FDB;
+		hash_for_each_possible(esw->offloads.mod_hdr_tbl, mh,
+				       mod_hdr_hlist, hash_key) {
+			if (!cmp_mod_hdr_info(&mh->key, &key)) {
+				found = true;
+				break;
+			}
+		}
+	} else {
+		namespace = MLX5_FLOW_NAMESPACE_KERNEL;
+		hash_for_each_possible(priv->fs.tc.mod_hdr_tbl, mh,
+				       mod_hdr_hlist, hash_key) {
+			if (!cmp_mod_hdr_info(&mh->key, &key)) {
+				found = true;
+				break;
+			}
+		}
+	}
+
+	if (found)
+		goto attach_flow;
+
+	mh = kzalloc(sizeof(*mh) + actions_size, GFP_KERNEL);
+	if (!mh)
+		return -ENOMEM;
+
+	mh->key.actions = (void *)mh + sizeof(*mh);
+	memcpy(mh->key.actions, key.actions, actions_size);
+	mh->key.num_actions = num_actions;
+	INIT_LIST_HEAD(&mh->flows);
+
+	err = mlx5_modify_header_alloc(priv->mdev, namespace,
+				       mh->key.num_actions,
+				       mh->key.actions,
+				       &mh->mod_hdr_id);
+	if (err)
+		goto out_err;
+
+	if (flow->flags & MLX5E_TC_FLOW_ESWITCH)
+		hash_add(esw->offloads.mod_hdr_tbl, &mh->mod_hdr_hlist, hash_key);
+	else
+		hash_add(priv->fs.tc.mod_hdr_tbl, &mh->mod_hdr_hlist, hash_key);
+
+attach_flow:
+	list_add(&flow->mod_hdr, &mh->flows);
+	if (flow->flags & MLX5E_TC_FLOW_ESWITCH)
+		flow->esw_attr->mod_hdr_id = mh->mod_hdr_id;
+	else
+		flow->nic_attr->mod_hdr_id = mh->mod_hdr_id;
+
+	return 0;
+
+out_err:
+	kfree(mh);
+	return err;
+}
+
+static void mlx5e_detach_mod_hdr(struct mlx5e_priv *priv,
+				 struct mlx5e_tc_flow *flow)
+{
+	struct list_head *next = flow->mod_hdr.next;
+
+	list_del(&flow->mod_hdr);
+
+	if (list_empty(next)) {
+		struct mlx5e_mod_hdr_entry *mh;
+
+		mh = list_entry(next, struct mlx5e_mod_hdr_entry, flows);
+
+		mlx5_modify_header_dealloc(priv->mdev, mh->mod_hdr_id);
+		hash_del(&mh->mod_hdr_hlist);
+		kfree(mh);
+	}
+}
+
 static struct mlx5_flow_handle *
 mlx5e_tc_add_nic_flow(struct mlx5e_priv *priv,
 		      struct mlx5e_tc_flow_parse_attr *parse_attr,
@@ -121,10 +251,7 @@ mlx5e_tc_add_nic_flow(struct mlx5e_priv *priv,
 	}
 
 	if (attr->action & MLX5_FLOW_CONTEXT_ACTION_MOD_HDR) {
-		err = mlx5_modify_header_alloc(dev, MLX5_FLOW_NAMESPACE_KERNEL,
-					       parse_attr->num_mod_hdr_actions,
-					       parse_attr->mod_hdr_actions,
-					       &attr->mod_hdr_id);
+		err = mlx5e_attach_mod_hdr(priv, flow, parse_attr);
 		flow_act.modify_id = attr->mod_hdr_id;
 		kfree(parse_attr->mod_hdr_actions);
 		if (err) {
@@ -166,8 +293,7 @@ err_add_rule:
 	}
 err_create_ft:
 	if (attr->action & MLX5_FLOW_CONTEXT_ACTION_MOD_HDR)
-		mlx5_modify_header_dealloc(priv->mdev,
-					   attr->mod_hdr_id);
+		mlx5e_detach_mod_hdr(priv, flow);
 err_create_mod_hdr_id:
 	mlx5_fc_destroy(dev, counter);
 
@@ -177,6 +303,7 @@ err_create_mod_hdr_id:
 static void mlx5e_tc_del_nic_flow(struct mlx5e_priv *priv,
 				  struct mlx5e_tc_flow *flow)
 {
+	struct mlx5_nic_flow_attr *attr = flow->nic_attr;
 	struct mlx5_fc *counter = NULL;
 
 	counter = mlx5_flow_rule_counter(flow->rule);
@@ -188,9 +315,8 @@ static void mlx5e_tc_del_nic_flow(struct mlx5e_priv *priv,
 		priv->fs.tc.t = NULL;
 	}
 
-	if (flow->nic_attr->action & MLX5_FLOW_CONTEXT_ACTION_MOD_HDR)
-		mlx5_modify_header_dealloc(priv->mdev,
-					   flow->nic_attr->mod_hdr_id);
+	if (attr->action & MLX5_FLOW_CONTEXT_ACTION_MOD_HDR)
+		mlx5e_detach_mod_hdr(priv, flow);
 }
 
 static void mlx5e_detach_encap(struct mlx5e_priv *priv,
@@ -213,10 +339,7 @@ mlx5e_tc_add_fdb_flow(struct mlx5e_priv *priv,
 	}
 
 	if (attr->action & MLX5_FLOW_CONTEXT_ACTION_MOD_HDR) {
-		err = mlx5_modify_header_alloc(priv->mdev, MLX5_FLOW_NAMESPACE_FDB,
-					       parse_attr->num_mod_hdr_actions,
-					       parse_attr->mod_hdr_actions,
-					       &attr->mod_hdr_id);
+		err = mlx5e_attach_mod_hdr(priv, flow, parse_attr);
 		kfree(parse_attr->mod_hdr_actions);
 		if (err) {
 			rule = ERR_PTR(err);
@@ -231,9 +354,8 @@ mlx5e_tc_add_fdb_flow(struct mlx5e_priv *priv,
 	return rule;
 
 err_add_rule:
-	if (flow->esw_attr->action & MLX5_FLOW_CONTEXT_ACTION_MOD_HDR)
-		mlx5_modify_header_dealloc(priv->mdev,
-					   attr->mod_hdr_id);
+	if (attr->action & MLX5_FLOW_CONTEXT_ACTION_MOD_HDR)
+		mlx5e_detach_mod_hdr(priv, flow);
 err_mod_hdr:
 	mlx5_eswitch_del_vlan_action(esw, attr);
 err_add_vlan:
@@ -250,19 +372,18 @@ static void mlx5e_tc_del_fdb_flow(struct mlx5e_priv *priv,
 
 	if (flow->flags & MLX5E_TC_FLOW_OFFLOADED) {
 		flow->flags &= ~MLX5E_TC_FLOW_OFFLOADED;
-		mlx5_eswitch_del_offloaded_rule(esw, flow->rule, flow->esw_attr);
+		mlx5_eswitch_del_offloaded_rule(esw, flow->rule, attr);
 	}
 
-	mlx5_eswitch_del_vlan_action(esw, flow->esw_attr);
+	mlx5_eswitch_del_vlan_action(esw, attr);
 
-	if (flow->esw_attr->action & MLX5_FLOW_CONTEXT_ACTION_ENCAP) {
+	if (attr->action & MLX5_FLOW_CONTEXT_ACTION_ENCAP) {
 		mlx5e_detach_encap(priv, flow);
-		kvfree(flow->esw_attr->parse_attr);
+		kvfree(attr->parse_attr);
 	}
 
-	if (flow->esw_attr->action & MLX5_FLOW_CONTEXT_ACTION_MOD_HDR)
-		mlx5_modify_header_dealloc(priv->mdev,
-					   attr->mod_hdr_id);
+	if (attr->action & MLX5_FLOW_CONTEXT_ACTION_MOD_HDR)
+		mlx5e_detach_mod_hdr(priv, flow);
 }
 
 void mlx5e_tc_encap_flows_add(struct mlx5e_priv *priv,
@@ -581,7 +702,9 @@ static int __parse_cls_flower(struct mlx5e_priv *priv,
 	      BIT(FLOW_DISSECTOR_KEY_ENC_IPV4_ADDRS) |
 	      BIT(FLOW_DISSECTOR_KEY_ENC_IPV6_ADDRS) |
 	      BIT(FLOW_DISSECTOR_KEY_ENC_PORTS)	|
-	      BIT(FLOW_DISSECTOR_KEY_ENC_CONTROL))) {
+	      BIT(FLOW_DISSECTOR_KEY_ENC_CONTROL) |
+	      BIT(FLOW_DISSECTOR_KEY_TCP) |
+	      BIT(FLOW_DISSECTOR_KEY_IP))) {
 		netdev_warn(priv->netdev, "Unsupported key used: 0x%x\n",
 			    f->dissector->used_keys);
 		return -EOPNOTSUPP;
@@ -765,6 +888,34 @@ static int __parse_cls_flower(struct mlx5e_priv *priv,
 			*min_inline = MLX5_INLINE_MODE_IP;
 	}
 
+	if (dissector_uses_key(f->dissector, FLOW_DISSECTOR_KEY_IP)) {
+		struct flow_dissector_key_ip *key =
+			skb_flow_dissector_target(f->dissector,
+						  FLOW_DISSECTOR_KEY_IP,
+						  f->key);
+		struct flow_dissector_key_ip *mask =
+			skb_flow_dissector_target(f->dissector,
+						  FLOW_DISSECTOR_KEY_IP,
+						  f->mask);
+
+		MLX5_SET(fte_match_set_lyr_2_4, headers_c, ip_ecn, mask->tos & 0x3);
+		MLX5_SET(fte_match_set_lyr_2_4, headers_v, ip_ecn, key->tos & 0x3);
+
+		MLX5_SET(fte_match_set_lyr_2_4, headers_c, ip_dscp, mask->tos >> 2);
+		MLX5_SET(fte_match_set_lyr_2_4, headers_v, ip_dscp, key->tos  >> 2);
+
+		MLX5_SET(fte_match_set_lyr_2_4, headers_c, ttl_hoplimit, mask->ttl);
+		MLX5_SET(fte_match_set_lyr_2_4, headers_v, ttl_hoplimit, key->ttl);
+
+		if (mask->ttl &&
+		    !MLX5_CAP_ESW_FLOWTABLE_FDB(priv->mdev,
+						ft_field_support.outer_ipv4_ttl))
+			return -EOPNOTSUPP;
+
+		if (mask->tos || mask->ttl)
+			*min_inline = MLX5_INLINE_MODE_IP;
+	}
+
 	if (dissector_uses_key(f->dissector, FLOW_DISSECTOR_KEY_PORTS)) {
 		struct flow_dissector_key_ports *key =
 			skb_flow_dissector_target(f->dissector,
@@ -808,6 +959,25 @@ static int __parse_cls_flower(struct mlx5e_priv *priv,
 			*min_inline = MLX5_INLINE_MODE_TCP_UDP;
 	}
 
+	if (dissector_uses_key(f->dissector, FLOW_DISSECTOR_KEY_TCP)) {
+		struct flow_dissector_key_tcp *key =
+			skb_flow_dissector_target(f->dissector,
+						  FLOW_DISSECTOR_KEY_TCP,
+						  f->key);
+		struct flow_dissector_key_tcp *mask =
+			skb_flow_dissector_target(f->dissector,
+						  FLOW_DISSECTOR_KEY_TCP,
+						  f->mask);
+
+		MLX5_SET(fte_match_set_lyr_2_4, headers_c, tcp_flags,
+			 ntohs(mask->flags));
+		MLX5_SET(fte_match_set_lyr_2_4, headers_v, tcp_flags,
+			 ntohs(key->flags));
+
+		if (mask->flags)
+			*min_inline = MLX5_INLINE_MODE_TCP_UDP;
+	}
+
 	return 0;
 }
 
@@ -888,33 +1058,37 @@ struct mlx5_fields {
 	u32 offset;
 };
 
+#define OFFLOAD(fw_field, size, field, off) \
+		{MLX5_ACTION_IN_FIELD_OUT_ ## fw_field, size, offsetof(struct pedit_headers, field) + (off)}
+
 static struct mlx5_fields fields[] = {
-	{MLX5_ACTION_IN_FIELD_OUT_DMAC_47_16, 4, offsetof(struct pedit_headers, eth.h_dest[0])},
-	{MLX5_ACTION_IN_FIELD_OUT_DMAC_15_0,  2, offsetof(struct pedit_headers, eth.h_dest[4])},
-	{MLX5_ACTION_IN_FIELD_OUT_SMAC_47_16, 4, offsetof(struct pedit_headers, eth.h_source[0])},
-	{MLX5_ACTION_IN_FIELD_OUT_SMAC_15_0,  2, offsetof(struct pedit_headers, eth.h_source[4])},
-	{MLX5_ACTION_IN_FIELD_OUT_ETHERTYPE,  2, offsetof(struct pedit_headers, eth.h_proto)},
-
-	{MLX5_ACTION_IN_FIELD_OUT_IP_DSCP, 1, offsetof(struct pedit_headers, ip4.tos)},
-	{MLX5_ACTION_IN_FIELD_OUT_IP_TTL,  1, offsetof(struct pedit_headers, ip4.ttl)},
-	{MLX5_ACTION_IN_FIELD_OUT_SIPV4,   4, offsetof(struct pedit_headers, ip4.saddr)},
-	{MLX5_ACTION_IN_FIELD_OUT_DIPV4,   4, offsetof(struct pedit_headers, ip4.daddr)},
-
-	{MLX5_ACTION_IN_FIELD_OUT_SIPV6_127_96, 4, offsetof(struct pedit_headers, ip6.saddr.s6_addr32[0])},
-	{MLX5_ACTION_IN_FIELD_OUT_SIPV6_95_64,  4, offsetof(struct pedit_headers, ip6.saddr.s6_addr32[1])},
-	{MLX5_ACTION_IN_FIELD_OUT_SIPV6_63_32,  4, offsetof(struct pedit_headers, ip6.saddr.s6_addr32[2])},
-	{MLX5_ACTION_IN_FIELD_OUT_SIPV6_31_0,   4, offsetof(struct pedit_headers, ip6.saddr.s6_addr32[3])},
-	{MLX5_ACTION_IN_FIELD_OUT_DIPV6_127_96, 4, offsetof(struct pedit_headers, ip6.daddr.s6_addr32[0])},
-	{MLX5_ACTION_IN_FIELD_OUT_DIPV6_95_64,  4, offsetof(struct pedit_headers, ip6.daddr.s6_addr32[1])},
-	{MLX5_ACTION_IN_FIELD_OUT_DIPV6_63_32,  4, offsetof(struct pedit_headers, ip6.daddr.s6_addr32[2])},
-	{MLX5_ACTION_IN_FIELD_OUT_DIPV6_31_0,   4, offsetof(struct pedit_headers, ip6.daddr.s6_addr32[3])},
-
-	{MLX5_ACTION_IN_FIELD_OUT_TCP_SPORT, 2, offsetof(struct pedit_headers, tcp.source)},
-	{MLX5_ACTION_IN_FIELD_OUT_TCP_DPORT, 2, offsetof(struct pedit_headers, tcp.dest)},
-	{MLX5_ACTION_IN_FIELD_OUT_TCP_FLAGS, 1, offsetof(struct pedit_headers, tcp.ack_seq) + 5},
-
-	{MLX5_ACTION_IN_FIELD_OUT_UDP_SPORT, 2, offsetof(struct pedit_headers, udp.source)},
-	{MLX5_ACTION_IN_FIELD_OUT_UDP_DPORT, 2, offsetof(struct pedit_headers, udp.dest)},
+	OFFLOAD(DMAC_47_16, 4, eth.h_dest[0], 0),
+	OFFLOAD(DMAC_47_16, 4, eth.h_dest[0], 0),
+	OFFLOAD(DMAC_15_0,  2, eth.h_dest[4], 0),
+	OFFLOAD(SMAC_47_16, 4, eth.h_source[0], 0),
+	OFFLOAD(SMAC_15_0,  2, eth.h_source[4], 0),
+	OFFLOAD(ETHERTYPE,  2, eth.h_proto, 0),
+
+	OFFLOAD(IP_TTL, 1, ip4.ttl,   0),
+	OFFLOAD(SIPV4,  4, ip4.saddr, 0),
+	OFFLOAD(DIPV4,  4, ip4.daddr, 0),
+
+	OFFLOAD(SIPV6_127_96, 4, ip6.saddr.s6_addr32[0], 0),
+	OFFLOAD(SIPV6_95_64,  4, ip6.saddr.s6_addr32[1], 0),
+	OFFLOAD(SIPV6_63_32,  4, ip6.saddr.s6_addr32[2], 0),
+	OFFLOAD(SIPV6_31_0,   4, ip6.saddr.s6_addr32[3], 0),
+	OFFLOAD(DIPV6_127_96, 4, ip6.daddr.s6_addr32[0], 0),
+	OFFLOAD(DIPV6_95_64,  4, ip6.daddr.s6_addr32[1], 0),
+	OFFLOAD(DIPV6_63_32,  4, ip6.daddr.s6_addr32[2], 0),
+	OFFLOAD(DIPV6_31_0,   4, ip6.daddr.s6_addr32[3], 0),
+	OFFLOAD(IPV6_HOPLIMIT, 1, ip6.hop_limit, 0),
+
+	OFFLOAD(TCP_SPORT, 2, tcp.source,  0),
+	OFFLOAD(TCP_DPORT, 2, tcp.dest,    0),
+	OFFLOAD(TCP_FLAGS, 1, tcp.ack_seq, 5),
+
+	OFFLOAD(UDP_SPORT, 2, udp.source, 0),
+	OFFLOAD(UDP_DPORT, 2, udp.dest,   0),
 };
 
 /* On input attr->num_mod_hdr_actions tells how many HW actions can be parsed at
@@ -926,12 +1100,14 @@ static int offload_pedit_fields(struct pedit_headers *masks,
 				struct mlx5e_tc_flow_parse_attr *parse_attr)
 {
 	struct pedit_headers *set_masks, *add_masks, *set_vals, *add_vals;
-	int i, action_size, nactions, max_actions, first, last, first_z;
+	int i, action_size, nactions, max_actions, first, last, next_z;
 	void *s_masks_p, *a_masks_p, *vals_p;
 	struct mlx5_fields *f;
 	u8 cmd, field_bsize;
 	u32 s_mask, a_mask;
 	unsigned long mask;
+	__be32 mask_be32;
+	__be16 mask_be16;
 	void *action;
 
 	set_masks = &masks[TCA_PEDIT_KEY_EX_CMD_SET];
@@ -985,11 +1161,19 @@ static int offload_pedit_fields(struct pedit_headers *masks,
 
 		field_bsize = f->size * BITS_PER_BYTE;
 
-		first_z = find_first_zero_bit(&mask, field_bsize);
+		if (field_bsize == 32) {
+			mask_be32 = *(__be32 *)&mask;
+			mask = (__force unsigned long)cpu_to_le32(be32_to_cpu(mask_be32));
+		} else if (field_bsize == 16) {
+			mask_be16 = *(__be16 *)&mask;
+			mask = (__force unsigned long)cpu_to_le16(be16_to_cpu(mask_be16));
+		}
+
 		first = find_first_bit(&mask, field_bsize);
+		next_z = find_next_zero_bit(&mask, field_bsize, first);
 		last  = find_last_bit(&mask, field_bsize);
-		if (first > 0 || last != (field_bsize - 1) || first_z < last) {
-			printk(KERN_WARNING "mlx5: partial rewrite (mask %lx) is currently not offloaded\n",
+		if (first < next_z && next_z < last) {
+			printk(KERN_WARNING "mlx5: rewrite of few sub-fields (mask %lx) isn't offloaded\n",
 			       mask);
 			return -EOPNOTSUPP;
 		}
@@ -998,17 +1182,17 @@ static int offload_pedit_fields(struct pedit_headers *masks,
 		MLX5_SET(set_action_in, action, field, f->field);
 
 		if (cmd == MLX5_ACTION_TYPE_SET) {
-			MLX5_SET(set_action_in, action, offset, 0);
+			MLX5_SET(set_action_in, action, offset, first);
 			/* length is num of bits to be written, zero means length of 32 */
-			MLX5_SET(set_action_in, action, length, field_bsize);
+			MLX5_SET(set_action_in, action, length, (last - first + 1));
 		}
 
 		if (field_bsize == 32)
-			MLX5_SET(set_action_in, action, data, ntohl(*(__be32 *)vals_p));
+			MLX5_SET(set_action_in, action, data, ntohl(*(__be32 *)vals_p) >> first);
 		else if (field_bsize == 16)
-			MLX5_SET(set_action_in, action, data, ntohs(*(__be16 *)vals_p));
+			MLX5_SET(set_action_in, action, data, ntohs(*(__be16 *)vals_p) >> first);
 		else if (field_bsize == 8)
-			MLX5_SET(set_action_in, action, data, *(u8 *)vals_p);
+			MLX5_SET(set_action_in, action, data, *(u8 *)vals_p >> first);
 
 		action += action_size;
 		nactions++;
@@ -1150,10 +1334,6 @@ static int parse_tc_nic_actions(struct mlx5e_priv *priv, struct tcf_exts *exts,
 
 	tcf_exts_to_list(exts, &actions);
 	list_for_each_entry(a, &actions, list) {
-		/* Only support a single action per rule */
-		if (attr->action)
-			return -EINVAL;
-
 		if (is_tcf_gact_shot(a)) {
 			attr->action |= MLX5_FLOW_CONTEXT_ACTION_DROP;
 			if (MLX5_CAP_FLOWTABLE(priv->mdev,
@@ -1436,8 +1616,8 @@ static int mlx5e_create_encap_header_ipv4(struct mlx5e_priv *priv,
 
 	if (!(nud_state & NUD_VALID)) {
 		neigh_event_send(n, NULL);
-		neigh_release(n);
-		return -EAGAIN;
+		err = -EAGAIN;
+		goto out;
 	}
 
 	err = mlx5_encap_alloc(priv->mdev, e->tunnel_type,
@@ -1542,8 +1722,8 @@ static int mlx5e_create_encap_header_ipv6(struct mlx5e_priv *priv,
 
 	if (!(nud_state & NUD_VALID)) {
 		neigh_event_send(n, NULL);
-		neigh_release(n);
-		return -EAGAIN;
+		err = -EAGAIN;
+		goto out;
 	}
 
 	err = mlx5_encap_alloc(priv->mdev, e->tunnel_type,
@@ -1778,7 +1958,7 @@ int mlx5e_configure_flower(struct mlx5e_priv *priv, __be16 protocol,
 	}
 
 	flow = kzalloc(sizeof(*flow) + attr_size, GFP_KERNEL);
-	parse_attr = mlx5_vzalloc(sizeof(*parse_attr));
+	parse_attr = kvzalloc(sizeof(*parse_attr), GFP_KERNEL);
 	if (!parse_attr || !flow) {
 		err = -ENOMEM;
 		goto err_free;
@@ -1863,9 +2043,7 @@ int mlx5e_stats_flower(struct mlx5e_priv *priv,
 {
 	struct mlx5e_tc_table *tc = &priv->fs.tc;
 	struct mlx5e_tc_flow *flow;
-	struct tc_action *a;
 	struct mlx5_fc *counter;
-	LIST_HEAD(actions);
 	u64 bytes;
 	u64 packets;
 	u64 lastuse;
@@ -1884,13 +2062,7 @@ int mlx5e_stats_flower(struct mlx5e_priv *priv,
 
 	mlx5_fc_query_cached(counter, &bytes, &packets, &lastuse);
 
-	preempt_disable();
-
-	tcf_exts_to_list(f->exts, &actions);
-	list_for_each_entry(a, &actions, list)
-		tcf_action_stats_update(a, bytes, packets, lastuse);
-
-	preempt_enable();
+	tcf_exts_stats_update(f->exts, bytes, packets, lastuse);
 
 	return 0;
 }
@@ -1906,6 +2078,8 @@ int mlx5e_tc_init(struct mlx5e_priv *priv)
 {
 	struct mlx5e_tc_table *tc = &priv->fs.tc;
 
+	hash_init(tc->mod_hdr_tbl);
+
 	tc->ht_params = mlx5e_tc_flow_ht_params;
 	return rhashtable_init(&tc->ht, &tc->ht_params);
 }
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
index ab3bb026ff9e..aaa0f4ebba9a 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
@@ -33,7 +33,8 @@
 #include <linux/tcp.h>
 #include <linux/if_vlan.h>
 #include "en.h"
-#include "ipoib.h"
+#include "ipoib/ipoib.h"
+#include "en_accel/ipsec_rxtx.h"
 
 #define MLX5E_SQ_NOPS_ROOM  MLX5_SEND_WQE_MAX_WQEBBS
 #define MLX5E_SQ_STOP_ROOM (MLX5_SEND_WQE_MAX_WQEBBS +\
@@ -245,7 +246,7 @@ mlx5e_txwqe_build_dsegs(struct mlx5e_txqsq *sq, struct sk_buff *skb,
 		int fsz = skb_frag_size(frag);
 
 		dma_addr = skb_frag_dma_map(sq->pdev, frag, 0, fsz,
-				     DMA_TO_DEVICE);
+					    DMA_TO_DEVICE);
 		if (unlikely(dma_mapping_error(sq->pdev, dma_addr)))
 			return -ENOMEM;
 
@@ -299,12 +300,9 @@ mlx5e_txwqe_complete(struct mlx5e_txqsq *sq, struct sk_buff *skb,
 	}
 }
 
-static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_txqsq *sq, struct sk_buff *skb)
+static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_txqsq *sq, struct sk_buff *skb,
+				 struct mlx5e_tx_wqe *wqe, u16 pi)
 {
-	struct mlx5_wq_cyc       *wq   = &sq->wq;
-
-	u16 pi = sq->pc & wq->sz_m1;
-	struct mlx5e_tx_wqe      *wqe  = mlx5_wq_cyc_get_wqe(wq, pi);
 	struct mlx5e_tx_wqe_info *wi   = &sq->db.wqe_info[pi];
 
 	struct mlx5_wqe_ctrl_seg *cseg = &wqe->ctrl;
@@ -319,8 +317,6 @@ static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_txqsq *sq, struct sk_buff *skb)
 	u16 ds_cnt;
 	u16 ihs;
 
-	memset(wqe, 0, sizeof(*wqe));
-
 	mlx5e_txwqe_build_eseg_csum(sq, skb, eseg);
 
 	if (skb_is_gso(skb)) {
@@ -375,8 +371,21 @@ netdev_tx_t mlx5e_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct mlx5e_priv *priv = netdev_priv(dev);
 	struct mlx5e_txqsq *sq = priv->txq2sq[skb_get_queue_mapping(skb)];
+	struct mlx5_wq_cyc *wq = &sq->wq;
+	u16 pi = sq->pc & wq->sz_m1;
+	struct mlx5e_tx_wqe *wqe = mlx5_wq_cyc_get_wqe(wq, pi);
+
+	memset(wqe, 0, sizeof(*wqe));
 
-	return mlx5e_sq_xmit(sq, skb);
+#ifdef CONFIG_MLX5_EN_IPSEC
+	if (sq->state & BIT(MLX5E_SQ_STATE_IPSEC)) {
+		skb = mlx5e_ipsec_handle_tx_skb(dev, wqe, skb);
+		if (unlikely(!skb))
+			return NETDEV_TX_OK;
+	}
+#endif
+
+	return mlx5e_sq_xmit(sq, skb, wqe, pi);
 }
 
 bool mlx5e_poll_tx_cq(struct mlx5e_cq *cq, int napi_budget)
@@ -409,7 +418,7 @@ bool mlx5e_poll_tx_cq(struct mlx5e_cq *cq, int napi_budget)
 		u16 wqe_counter;
 		bool last_wqe;
 
-		cqe = mlx5e_get_cqe(cq);
+		cqe = mlx5_cqwq_get_cqe(&cq->wq);
 		if (!cqe)
 			break;
 
@@ -557,11 +566,16 @@ netdev_tx_t mlx5i_sq_xmit(struct mlx5e_txqsq *sq, struct sk_buff *skb,
 	if (skb_is_gso(skb)) {
 		opcode = MLX5_OPCODE_LSO;
 		ihs = mlx5e_txwqe_build_eseg_gso(sq, skb, eseg, &num_bytes);
+		sq->stats.packets += skb_shinfo(skb)->gso_segs;
 	} else {
 		ihs = mlx5e_calc_min_inline(sq->min_inline_mode, skb);
 		num_bytes = max_t(unsigned int, skb->len, ETH_ZLEN);
+		sq->stats.packets++;
 	}
 
+	sq->stats.bytes += num_bytes;
+	sq->stats.xmit_more += skb->xmit_more;
+
 	ds_cnt = sizeof(*wqe) / MLX5_SEND_WQE_DS;
 	if (ihs) {
 		memcpy(eseg->inline_hdr.start, skb_data, ihs);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c
index 5ca6714e3e02..92db28a9ed43 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c
@@ -32,23 +32,6 @@
 
 #include "en.h"
 
-struct mlx5_cqe64 *mlx5e_get_cqe(struct mlx5e_cq *cq)
-{
-	struct mlx5_cqwq *wq = &cq->wq;
-	u32 ci = mlx5_cqwq_get_ci(wq);
-	struct mlx5_cqe64 *cqe = mlx5_cqwq_get_wqe(wq, ci);
-	u8 cqe_ownership_bit = cqe->op_own & MLX5_CQE_OWNER_MASK;
-	u8 sw_ownership_val = mlx5_cqwq_get_wrap_cnt(wq) & 1;
-
-	if (cqe_ownership_bit != sw_ownership_val)
-		return NULL;
-
-	/* ensure cqe content is read after cqe ownership bit */
-	dma_rmb();
-
-	return cqe;
-}
-
 static inline void mlx5e_poll_ico_single_cqe(struct mlx5e_cq *cq,
 					     struct mlx5e_icosq *sq,
 					     struct mlx5_cqe64 *cqe,
@@ -89,7 +72,7 @@ static void mlx5e_poll_ico_cq(struct mlx5e_cq *cq)
 	if (unlikely(!test_bit(MLX5E_SQ_STATE_ENABLED, &sq->state)))
 		return;
 
-	cqe = mlx5e_get_cqe(cq);
+	cqe = mlx5_cqwq_get_cqe(&cq->wq);
 	if (likely(!cqe))
 		return;
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eq.c b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
index 33eae5ad2fb0..af51a5d2b912 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
@@ -35,6 +35,7 @@
 #include <linux/mlx5/driver.h>
 #include <linux/mlx5/cmd.h>
 #include "mlx5_core.h"
+#include "fpga/core.h"
 #ifdef CONFIG_MLX5_CORE_EN
 #include "eswitch.h"
 #endif
@@ -156,6 +157,10 @@ static const char *eqe_type_str(u8 type)
 		return "MLX5_EVENT_TYPE_PAGE_FAULT";
 	case MLX5_EVENT_TYPE_PPS_EVENT:
 		return "MLX5_EVENT_TYPE_PPS_EVENT";
+	case MLX5_EVENT_TYPE_NIC_VPORT_CHANGE:
+		return "MLX5_EVENT_TYPE_NIC_VPORT_CHANGE";
+	case MLX5_EVENT_TYPE_FPGA_ERROR:
+		return "MLX5_EVENT_TYPE_FPGA_ERROR";
 	default:
 		return "Unrecognized event";
 	}
@@ -186,7 +191,7 @@ static void eq_update_ci(struct mlx5_eq *eq, int arm)
 {
 	__be32 __iomem *addr = eq->doorbell + (arm ? 0 : 2);
 	u32 val = (eq->cons_index & 0xffffff) | (eq->eqn << 24);
-	__raw_writel((__force u32) cpu_to_be32(val), addr);
+	__raw_writel((__force u32)cpu_to_be32(val), addr);
 	/* We still want ordering, just not swabbing, so add a barrier */
 	mb();
 }
@@ -476,6 +481,11 @@ static irqreturn_t mlx5_eq_int(int irq, void *eq_ptr)
 			if (dev->event)
 				dev->event(dev, MLX5_DEV_EVENT_PPS, (unsigned long)eqe);
 			break;
+
+		case MLX5_EVENT_TYPE_FPGA_ERROR:
+			mlx5_fpga_event(dev, eqe->type, &eqe->data.raw);
+			break;
+
 		default:
 			mlx5_core_warn(dev, "Unhandled event 0x%x on EQ 0x%x\n",
 				       eqe->type, eq->eqn);
@@ -548,7 +558,7 @@ int mlx5_create_map_eq(struct mlx5_core_dev *dev, struct mlx5_eq *eq, u8 vecidx,
 	inlen = MLX5_ST_SZ_BYTES(create_eq_in) +
 		MLX5_FLD_SZ_BYTES(create_eq_in, pas[0]) * eq->buf.npages;
 
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in) {
 		err = -ENOMEM;
 		goto err_buf;
@@ -667,7 +677,6 @@ int mlx5_eq_init(struct mlx5_core_dev *dev)
 	return err;
 }
 
-
 void mlx5_eq_cleanup(struct mlx5_core_dev *dev)
 {
 	mlx5_eq_debugfs_cleanup(dev);
@@ -679,7 +688,6 @@ int mlx5_start_eqs(struct mlx5_core_dev *dev)
 	u64 async_event_mask = MLX5_ASYNC_EVENT_MASK;
 	int err;
 
-
 	if (MLX5_CAP_GEN(dev, port_type) == MLX5_CAP_PORT_TYPE_ETH &&
 	    MLX5_CAP_GEN(dev, vport_group_manager) &&
 	    mlx5_core_is_pf(dev))
@@ -693,6 +701,9 @@ int mlx5_start_eqs(struct mlx5_core_dev *dev)
 	if (MLX5_CAP_GEN(dev, pps))
 		async_event_mask |= (1ull << MLX5_EVENT_TYPE_PPS_EVENT);
 
+	if (MLX5_CAP_GEN(dev, fpga))
+		async_event_mask |= (1ull << MLX5_EVENT_TYPE_FPGA_ERROR);
+
 	err = mlx5_create_map_eq(dev, &table->cmd_eq, MLX5_EQ_VEC_CMD,
 				 MLX5_NUM_CMD_EQE, 1ull << MLX5_EVENT_TYPE_CMD,
 				 "mlx5_cmd_eq", MLX5_EQ_TYPE_ASYNC);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eswitch.c b/drivers/net/ethernet/mellanox/mlx5/core/eswitch.c
index 2e34d95ea776..89bfda419efe 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/eswitch.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eswitch.c
@@ -248,11 +248,10 @@ __esw_fdb_set_vport_rule(struct mlx5_eswitch *esw, u32 vport, bool rx_rule,
 	if (rx_rule)
 		match_header |= MLX5_MATCH_MISC_PARAMETERS;
 
-	spec = mlx5_vzalloc(sizeof(*spec));
-	if (!spec) {
-		esw_warn(esw->dev, "FDB: Failed to alloc match parameters\n");
+	spec = kvzalloc(sizeof(*spec), GFP_KERNEL);
+	if (!spec)
 		return NULL;
-	}
+
 	dmac_v = MLX5_ADDR_OF(fte_match_param, spec->match_value,
 			      outer_headers.dmac_47_16);
 	dmac_c = MLX5_ADDR_OF(fte_match_param, spec->match_criteria,
@@ -350,10 +349,9 @@ static int esw_create_legacy_fdb_table(struct mlx5_eswitch *esw, int nvports)
 		return -EOPNOTSUPP;
 	}
 
-	flow_group_in = mlx5_vzalloc(inlen);
+	flow_group_in = kvzalloc(inlen, GFP_KERNEL);
 	if (!flow_group_in)
 		return -ENOMEM;
-	memset(flow_group_in, 0, inlen);
 
 	table_size = BIT(MLX5_CAP_ESW_FLOWTABLE_FDB(dev, log_max_ft_size));
 
@@ -961,7 +959,7 @@ static int esw_vport_enable_egress_acl(struct mlx5_eswitch *esw,
 		return -EOPNOTSUPP;
 	}
 
-	flow_group_in = mlx5_vzalloc(inlen);
+	flow_group_in = kvzalloc(inlen, GFP_KERNEL);
 	if (!flow_group_in)
 		return -ENOMEM;
 
@@ -1078,7 +1076,7 @@ static int esw_vport_enable_ingress_acl(struct mlx5_eswitch *esw,
 		return -EOPNOTSUPP;
 	}
 
-	flow_group_in = mlx5_vzalloc(inlen);
+	flow_group_in = kvzalloc(inlen, GFP_KERNEL);
 	if (!flow_group_in)
 		return -ENOMEM;
 
@@ -1219,7 +1217,6 @@ static int esw_vport_ingress_config(struct mlx5_eswitch *esw,
 			       "vport[%d] configure ingress rules failed, illegal mac with spoofchk\n",
 			       vport->vport);
 		return -EPERM;
-
 	}
 
 	esw_vport_cleanup_ingress_rules(esw, vport);
@@ -1241,11 +1238,9 @@ static int esw_vport_ingress_config(struct mlx5_eswitch *esw,
 		  "vport[%d] configure ingress rules, vlan(%d) qos(%d)\n",
 		  vport->vport, vport->info.vlan, vport->info.qos);
 
-	spec = mlx5_vzalloc(sizeof(*spec));
+	spec = kvzalloc(sizeof(*spec), GFP_KERNEL);
 	if (!spec) {
 		err = -ENOMEM;
-		esw_warn(esw->dev, "vport[%d] configure ingress rules failed, err(%d)\n",
-			 vport->vport, err);
 		goto out;
 	}
 
@@ -1322,11 +1317,9 @@ static int esw_vport_egress_config(struct mlx5_eswitch *esw,
 		  "vport[%d] configure egress rules, vlan(%d) qos(%d)\n",
 		  vport->vport, vport->info.vlan, vport->info.qos);
 
-	spec = mlx5_vzalloc(sizeof(*spec));
+	spec = kvzalloc(sizeof(*spec), GFP_KERNEL);
 	if (!spec) {
 		err = -ENOMEM;
-		esw_warn(esw->dev, "vport[%d] configure egress rules failed, err(%d)\n",
-			 vport->vport, err);
 		goto out;
 	}
 
@@ -1775,6 +1768,7 @@ int mlx5_eswitch_init(struct mlx5_core_dev *dev)
 	}
 
 	hash_init(esw->offloads.encap_tbl);
+	hash_init(esw->offloads.mod_hdr_tbl);
 	mutex_init(&esw->state_lock);
 
 	for (vport_num = 0; vport_num < total_vports; vport_num++) {
@@ -2158,7 +2152,7 @@ int mlx5_eswitch_get_vport_stats(struct mlx5_eswitch *esw,
 	if (!LEGAL_VPORT(esw, vport))
 		return -EINVAL;
 
-	out = mlx5_vzalloc(outlen);
+	out = kvzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eswitch.h b/drivers/net/ethernet/mellanox/mlx5/core/eswitch.h
index b746f62c8c79..834a33050969 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/eswitch.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eswitch.h
@@ -207,6 +207,7 @@ struct mlx5_esw_offload {
 	struct mlx5_flow_group *vport_rx_group;
 	struct mlx5_eswitch_rep *vport_reps;
 	DECLARE_HASHTABLE(encap_tbl, 8);
+	DECLARE_HASHTABLE(mod_hdr_tbl, 8);
 	u8 inline_mode;
 	u64 num_flows;
 	u8 encap;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eswitch_offloads.c b/drivers/net/ethernet/mellanox/mlx5/core/eswitch_offloads.c
index f991f669047e..95b64025ce36 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/eswitch_offloads.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eswitch_offloads.c
@@ -311,9 +311,8 @@ mlx5_eswitch_add_send_to_vport_rule(struct mlx5_eswitch *esw, int vport, u32 sqn
 	struct mlx5_flow_spec *spec;
 	void *misc;
 
-	spec = mlx5_vzalloc(sizeof(*spec));
+	spec = kvzalloc(sizeof(*spec), GFP_KERNEL);
 	if (!spec) {
-		esw_warn(esw->dev, "FDB: Failed to alloc match parameters\n");
 		flow_rule = ERR_PTR(-ENOMEM);
 		goto out;
 	}
@@ -401,9 +400,8 @@ static int esw_add_fdb_miss_rule(struct mlx5_eswitch *esw)
 	struct mlx5_flow_spec *spec;
 	int err = 0;
 
-	spec = mlx5_vzalloc(sizeof(*spec));
+	spec = kvzalloc(sizeof(*spec), GFP_KERNEL);
 	if (!spec) {
-		esw_warn(esw->dev, "FDB: Failed to alloc match parameters\n");
 		err = -ENOMEM;
 		goto out;
 	}
@@ -488,7 +486,7 @@ static int esw_create_offloads_fdb_tables(struct mlx5_eswitch *esw, int nvports)
 	u32 *flow_group_in;
 
 	esw_debug(esw->dev, "Create offloads FDB Tables\n");
-	flow_group_in = mlx5_vzalloc(inlen);
+	flow_group_in = kvzalloc(inlen, GFP_KERNEL);
 	if (!flow_group_in)
 		return -ENOMEM;
 
@@ -631,7 +629,7 @@ static int esw_create_vport_rx_group(struct mlx5_eswitch *esw)
 	int err = 0;
 	int nvports = priv->sriov.num_vfs + 2;
 
-	flow_group_in = mlx5_vzalloc(inlen);
+	flow_group_in = kvzalloc(inlen, GFP_KERNEL);
 	if (!flow_group_in)
 		return -ENOMEM;
 
@@ -675,9 +673,8 @@ mlx5_eswitch_create_vport_rx_rule(struct mlx5_eswitch *esw, int vport, u32 tirn)
 	struct mlx5_flow_spec *spec;
 	void *misc;
 
-	spec = mlx5_vzalloc(sizeof(*spec));
+	spec = kvzalloc(sizeof(*spec), GFP_KERNEL);
 	if (!spec) {
-		esw_warn(esw->dev, "Failed to alloc match parameters\n");
 		flow_rule = ERR_PTR(-ENOMEM);
 		goto out;
 	}
@@ -694,7 +691,7 @@ mlx5_eswitch_create_vport_rx_rule(struct mlx5_eswitch *esw, int vport, u32 tirn)
 
 	flow_act.action = MLX5_FLOW_CONTEXT_ACTION_FWD_DEST;
 	flow_rule = mlx5_add_flow_rules(esw->offloads.ft_offloads, spec,
-				       &flow_act, &dest, 1);
+					&flow_act, &dest, 1);
 	if (IS_ERR(flow_rule)) {
 		esw_warn(esw->dev, "fs offloads: Failed to add vport rx rule err %ld\n", PTR_ERR(flow_rule));
 		goto out;
@@ -906,21 +903,34 @@ static int esw_inline_mode_to_devlink(u8 mlx5_mode, u8 *mode)
 	return 0;
 }
 
-int mlx5_devlink_eswitch_mode_set(struct devlink *devlink, u16 mode)
+static int mlx5_devlink_eswitch_check(struct devlink *devlink)
 {
-	struct mlx5_core_dev *dev;
-	u16 cur_mlx5_mode, mlx5_mode = 0;
+	struct mlx5_core_dev *dev = devlink_priv(devlink);
 
-	dev = devlink_priv(devlink);
+	if (MLX5_CAP_GEN(dev, port_type) != MLX5_CAP_PORT_TYPE_ETH)
+		return -EOPNOTSUPP;
 
 	if (!MLX5_CAP_GEN(dev, vport_group_manager))
 		return -EOPNOTSUPP;
 
-	cur_mlx5_mode = dev->priv.eswitch->mode;
-
-	if (cur_mlx5_mode == SRIOV_NONE)
+	if (dev->priv.eswitch->mode == SRIOV_NONE)
 		return -EOPNOTSUPP;
 
+	return 0;
+}
+
+int mlx5_devlink_eswitch_mode_set(struct devlink *devlink, u16 mode)
+{
+	struct mlx5_core_dev *dev = devlink_priv(devlink);
+	u16 cur_mlx5_mode, mlx5_mode = 0;
+	int err;
+
+	err = mlx5_devlink_eswitch_check(devlink);
+	if (err)
+		return err;
+
+	cur_mlx5_mode = dev->priv.eswitch->mode;
+
 	if (esw_mode_from_devlink(mode, &mlx5_mode))
 		return -EINVAL;
 
@@ -937,15 +947,12 @@ int mlx5_devlink_eswitch_mode_set(struct devlink *devlink, u16 mode)
 
 int mlx5_devlink_eswitch_mode_get(struct devlink *devlink, u16 *mode)
 {
-	struct mlx5_core_dev *dev;
-
-	dev = devlink_priv(devlink);
-
-	if (!MLX5_CAP_GEN(dev, vport_group_manager))
-		return -EOPNOTSUPP;
+	struct mlx5_core_dev *dev = devlink_priv(devlink);
+	int err;
 
-	if (dev->priv.eswitch->mode == SRIOV_NONE)
-		return -EOPNOTSUPP;
+	err = mlx5_devlink_eswitch_check(devlink);
+	if (err)
+		return err;
 
 	return esw_mode_to_devlink(dev->priv.eswitch->mode, mode);
 }
@@ -954,15 +961,12 @@ int mlx5_devlink_eswitch_inline_mode_set(struct devlink *devlink, u8 mode)
 {
 	struct mlx5_core_dev *dev = devlink_priv(devlink);
 	struct mlx5_eswitch *esw = dev->priv.eswitch;
-	int num_vports = esw->enabled_vports;
 	int err, vport;
 	u8 mlx5_mode;
 
-	if (!MLX5_CAP_GEN(dev, vport_group_manager))
-		return -EOPNOTSUPP;
-
-	if (esw->mode == SRIOV_NONE)
-		return -EOPNOTSUPP;
+	err = mlx5_devlink_eswitch_check(devlink);
+	if (err)
+		return err;
 
 	switch (MLX5_CAP_ETH(dev, wqe_inline_mode)) {
 	case MLX5_CAP_INLINE_MODE_NOT_REQUIRED:
@@ -985,7 +989,7 @@ int mlx5_devlink_eswitch_inline_mode_set(struct devlink *devlink, u8 mode)
 	if (err)
 		goto out;
 
-	for (vport = 1; vport < num_vports; vport++) {
+	for (vport = 1; vport < esw->enabled_vports; vport++) {
 		err = mlx5_modify_nic_vport_min_inline(dev, vport, mlx5_mode);
 		if (err) {
 			esw_warn(dev, "Failed to set min inline on vport %d\n",
@@ -1010,12 +1014,11 @@ int mlx5_devlink_eswitch_inline_mode_get(struct devlink *devlink, u8 *mode)
 {
 	struct mlx5_core_dev *dev = devlink_priv(devlink);
 	struct mlx5_eswitch *esw = dev->priv.eswitch;
+	int err;
 
-	if (!MLX5_CAP_GEN(dev, vport_group_manager))
-		return -EOPNOTSUPP;
-
-	if (esw->mode == SRIOV_NONE)
-		return -EOPNOTSUPP;
+	err = mlx5_devlink_eswitch_check(devlink);
+	if (err)
+		return err;
 
 	return esw_inline_mode_to_devlink(esw->offloads.inline_mode, mode);
 }
@@ -1062,11 +1065,9 @@ int mlx5_devlink_eswitch_encap_mode_set(struct devlink *devlink, u8 encap)
 	struct mlx5_eswitch *esw = dev->priv.eswitch;
 	int err;
 
-	if (!MLX5_CAP_GEN(dev, vport_group_manager))
-		return -EOPNOTSUPP;
-
-	if (esw->mode == SRIOV_NONE)
-		return -EOPNOTSUPP;
+	err = mlx5_devlink_eswitch_check(devlink);
+	if (err)
+		return err;
 
 	if (encap != DEVLINK_ESWITCH_ENCAP_MODE_NONE &&
 	    (!MLX5_CAP_ESW_FLOWTABLE_FDB(dev, encap) ||
@@ -1096,7 +1097,7 @@ int mlx5_devlink_eswitch_encap_mode_set(struct devlink *devlink, u8 encap)
 	if (err) {
 		esw_warn(esw->dev, "Failed re-creating fast FDB table, err %d\n", err);
 		esw->offloads.encap = !encap;
-		(void) esw_create_offloads_fast_fdb_table(esw);
+		(void)esw_create_offloads_fast_fdb_table(esw);
 	}
 	return err;
 }
@@ -1105,12 +1106,11 @@ int mlx5_devlink_eswitch_encap_mode_get(struct devlink *devlink, u8 *encap)
 {
 	struct mlx5_core_dev *dev = devlink_priv(devlink);
 	struct mlx5_eswitch *esw = dev->priv.eswitch;
+	int err;
 
-	if (!MLX5_CAP_GEN(dev, vport_group_manager))
-		return -EOPNOTSUPP;
-
-	if (esw->mode == SRIOV_NONE)
-		return -EOPNOTSUPP;
+	err = mlx5_devlink_eswitch_check(devlink);
+	if (err)
+		return err;
 
 	*encap = esw->offloads.encap;
 	return 0;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fpga/cmd.c b/drivers/net/ethernet/mellanox/mlx5/core/fpga/cmd.c
new file mode 100644
index 000000000000..e37453d838db
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/cmd.c
@@ -0,0 +1,238 @@
+/*
+ * Copyright (c) 2017, Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/etherdevice.h>
+#include <linux/mlx5/cmd.h>
+#include <linux/mlx5/driver.h>
+#include <linux/mlx5/device.h>
+
+#include "mlx5_core.h"
+#include "fpga/cmd.h"
+
+#define MLX5_FPGA_ACCESS_REG_SZ (MLX5_ST_SZ_DW(fpga_access_reg) + \
+				 MLX5_FPGA_ACCESS_REG_SIZE_MAX)
+
+int mlx5_fpga_access_reg(struct mlx5_core_dev *dev, u8 size, u64 addr,
+			 void *buf, bool write)
+{
+	u32 in[MLX5_FPGA_ACCESS_REG_SZ] = {0};
+	u32 out[MLX5_FPGA_ACCESS_REG_SZ];
+	int err;
+
+	if (size & 3)
+		return -EINVAL;
+	if (addr & 3)
+		return -EINVAL;
+	if (size > MLX5_FPGA_ACCESS_REG_SIZE_MAX)
+		return -EINVAL;
+
+	MLX5_SET(fpga_access_reg, in, size, size);
+	MLX5_SET64(fpga_access_reg, in, address, addr);
+	if (write)
+		memcpy(MLX5_ADDR_OF(fpga_access_reg, in, data), buf, size);
+
+	err = mlx5_core_access_reg(dev, in, sizeof(in), out, sizeof(out),
+				   MLX5_REG_FPGA_ACCESS_REG, 0, write);
+	if (err)
+		return err;
+
+	if (!write)
+		memcpy(buf, MLX5_ADDR_OF(fpga_access_reg, out, data), size);
+
+	return 0;
+}
+
+int mlx5_fpga_caps(struct mlx5_core_dev *dev, u32 *caps)
+{
+	u32 in[MLX5_ST_SZ_DW(fpga_cap)] = {0};
+
+	return mlx5_core_access_reg(dev, in, sizeof(in), caps,
+				    MLX5_ST_SZ_BYTES(fpga_cap),
+				    MLX5_REG_FPGA_CAP, 0, 0);
+}
+
+int mlx5_fpga_ctrl_op(struct mlx5_core_dev *dev, u8 op)
+{
+	u32 in[MLX5_ST_SZ_DW(fpga_ctrl)] = {0};
+	u32 out[MLX5_ST_SZ_DW(fpga_ctrl)];
+
+	MLX5_SET(fpga_ctrl, in, operation, op);
+
+	return mlx5_core_access_reg(dev, in, sizeof(in), out, sizeof(out),
+				    MLX5_REG_FPGA_CTRL, 0, true);
+}
+
+int mlx5_fpga_sbu_caps(struct mlx5_core_dev *dev, void *caps, int size)
+{
+	unsigned int cap_size = MLX5_CAP_FPGA(dev, sandbox_extended_caps_len);
+	u64 addr = MLX5_CAP64_FPGA(dev, sandbox_extended_caps_addr);
+	unsigned int read;
+	int ret = 0;
+
+	if (cap_size > size) {
+		mlx5_core_warn(dev, "Not enough buffer %u for FPGA SBU caps %u",
+			       size, cap_size);
+		return -EINVAL;
+	}
+
+	while (cap_size > 0) {
+		read = min_t(unsigned int, cap_size,
+			     MLX5_FPGA_ACCESS_REG_SIZE_MAX);
+
+		ret = mlx5_fpga_access_reg(dev, read, addr, caps, false);
+		if (ret) {
+			mlx5_core_warn(dev, "Error reading FPGA SBU caps %u bytes at address 0x%llx: %d",
+				       read, addr, ret);
+			return ret;
+		}
+
+		cap_size -= read;
+		addr += read;
+		caps += read;
+	}
+
+	return ret;
+}
+
+int mlx5_fpga_query(struct mlx5_core_dev *dev, struct mlx5_fpga_query *query)
+{
+	u32 in[MLX5_ST_SZ_DW(fpga_ctrl)] = {0};
+	u32 out[MLX5_ST_SZ_DW(fpga_ctrl)];
+	int err;
+
+	err = mlx5_core_access_reg(dev, in, sizeof(in), out, sizeof(out),
+				   MLX5_REG_FPGA_CTRL, 0, false);
+	if (err)
+		return err;
+
+	query->status = MLX5_GET(fpga_ctrl, out, status);
+	query->admin_image = MLX5_GET(fpga_ctrl, out, flash_select_admin);
+	query->oper_image = MLX5_GET(fpga_ctrl, out, flash_select_oper);
+	return 0;
+}
+
+int mlx5_fpga_create_qp(struct mlx5_core_dev *dev, void *fpga_qpc,
+			u32 *fpga_qpn)
+{
+	u32 in[MLX5_ST_SZ_DW(fpga_create_qp_in)] = {0};
+	u32 out[MLX5_ST_SZ_DW(fpga_create_qp_out)];
+	int ret;
+
+	MLX5_SET(fpga_create_qp_in, in, opcode, MLX5_CMD_OP_FPGA_CREATE_QP);
+	memcpy(MLX5_ADDR_OF(fpga_create_qp_in, in, fpga_qpc), fpga_qpc,
+	       MLX5_FLD_SZ_BYTES(fpga_create_qp_in, fpga_qpc));
+
+	ret = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+	if (ret)
+		return ret;
+
+	memcpy(fpga_qpc, MLX5_ADDR_OF(fpga_create_qp_out, out, fpga_qpc),
+	       MLX5_FLD_SZ_BYTES(fpga_create_qp_out, fpga_qpc));
+	*fpga_qpn = MLX5_GET(fpga_create_qp_out, out, fpga_qpn);
+	return ret;
+}
+
+int mlx5_fpga_modify_qp(struct mlx5_core_dev *dev, u32 fpga_qpn,
+			enum mlx5_fpga_qpc_field_select fields,
+			void *fpga_qpc)
+{
+	u32 in[MLX5_ST_SZ_DW(fpga_modify_qp_in)] = {0};
+	u32 out[MLX5_ST_SZ_DW(fpga_modify_qp_out)];
+
+	MLX5_SET(fpga_modify_qp_in, in, opcode, MLX5_CMD_OP_FPGA_MODIFY_QP);
+	MLX5_SET(fpga_modify_qp_in, in, field_select, fields);
+	MLX5_SET(fpga_modify_qp_in, in, fpga_qpn, fpga_qpn);
+	memcpy(MLX5_ADDR_OF(fpga_modify_qp_in, in, fpga_qpc), fpga_qpc,
+	       MLX5_FLD_SZ_BYTES(fpga_modify_qp_in, fpga_qpc));
+
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+}
+
+int mlx5_fpga_query_qp(struct mlx5_core_dev *dev,
+		       u32 fpga_qpn, void *fpga_qpc)
+{
+	u32 in[MLX5_ST_SZ_DW(fpga_query_qp_in)] = {0};
+	u32 out[MLX5_ST_SZ_DW(fpga_query_qp_out)];
+	int ret;
+
+	MLX5_SET(fpga_query_qp_in, in, opcode, MLX5_CMD_OP_FPGA_QUERY_QP);
+	MLX5_SET(fpga_query_qp_in, in, fpga_qpn, fpga_qpn);
+
+	ret = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+	if (ret)
+		return ret;
+
+	memcpy(fpga_qpc, MLX5_ADDR_OF(fpga_query_qp_out, out, fpga_qpc),
+	       MLX5_FLD_SZ_BYTES(fpga_query_qp_out, fpga_qpc));
+	return ret;
+}
+
+int mlx5_fpga_destroy_qp(struct mlx5_core_dev *dev, u32 fpga_qpn)
+{
+	u32 in[MLX5_ST_SZ_DW(fpga_destroy_qp_in)] = {0};
+	u32 out[MLX5_ST_SZ_DW(fpga_destroy_qp_out)];
+
+	MLX5_SET(fpga_destroy_qp_in, in, opcode, MLX5_CMD_OP_FPGA_DESTROY_QP);
+	MLX5_SET(fpga_destroy_qp_in, in, fpga_qpn, fpga_qpn);
+
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+}
+
+int mlx5_fpga_query_qp_counters(struct mlx5_core_dev *dev, u32 fpga_qpn,
+				bool clear, struct mlx5_fpga_qp_counters *data)
+{
+	u32 in[MLX5_ST_SZ_DW(fpga_query_qp_counters_in)] = {0};
+	u32 out[MLX5_ST_SZ_DW(fpga_query_qp_counters_out)];
+	int ret;
+
+	MLX5_SET(fpga_query_qp_counters_in, in, opcode,
+		 MLX5_CMD_OP_FPGA_QUERY_QP_COUNTERS);
+	MLX5_SET(fpga_query_qp_counters_in, in, clear, clear);
+	MLX5_SET(fpga_query_qp_counters_in, in, fpga_qpn, fpga_qpn);
+
+	ret = mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+	if (ret)
+		return ret;
+
+	data->rx_ack_packets = MLX5_GET64(fpga_query_qp_counters_out, out,
+					  rx_ack_packets);
+	data->rx_send_packets = MLX5_GET64(fpga_query_qp_counters_out, out,
+					   rx_send_packets);
+	data->tx_ack_packets = MLX5_GET64(fpga_query_qp_counters_out, out,
+					  tx_ack_packets);
+	data->tx_send_packets = MLX5_GET64(fpga_query_qp_counters_out, out,
+					   tx_send_packets);
+	data->rx_total_drop = MLX5_GET64(fpga_query_qp_counters_out, out,
+					 rx_total_drop);
+
+	return ret;
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fpga/cmd.h b/drivers/net/ethernet/mellanox/mlx5/core/fpga/cmd.h
new file mode 100644
index 000000000000..94bdfd47c3f0
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/cmd.h
@@ -0,0 +1,84 @@
+/*
+ * Copyright (c) 2017, Mellanox Technologies, Ltd.  All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef __MLX5_FPGA_H__
+#define __MLX5_FPGA_H__
+
+#include <linux/mlx5/driver.h>
+
+enum mlx5_fpga_image {
+	MLX5_FPGA_IMAGE_USER = 0,
+	MLX5_FPGA_IMAGE_FACTORY,
+};
+
+enum mlx5_fpga_status {
+	MLX5_FPGA_STATUS_SUCCESS = 0,
+	MLX5_FPGA_STATUS_FAILURE = 1,
+	MLX5_FPGA_STATUS_IN_PROGRESS = 2,
+	MLX5_FPGA_STATUS_NONE = 0xFFFF,
+};
+
+struct mlx5_fpga_query {
+	enum mlx5_fpga_image admin_image;
+	enum mlx5_fpga_image oper_image;
+	enum mlx5_fpga_status status;
+};
+
+enum mlx5_fpga_qpc_field_select {
+	MLX5_FPGA_QPC_STATE = BIT(0),
+};
+
+struct mlx5_fpga_qp_counters {
+	u64 rx_ack_packets;
+	u64 rx_send_packets;
+	u64 tx_ack_packets;
+	u64 tx_send_packets;
+	u64 rx_total_drop;
+};
+
+int mlx5_fpga_caps(struct mlx5_core_dev *dev, u32 *caps);
+int mlx5_fpga_query(struct mlx5_core_dev *dev, struct mlx5_fpga_query *query);
+int mlx5_fpga_ctrl_op(struct mlx5_core_dev *dev, u8 op);
+int mlx5_fpga_access_reg(struct mlx5_core_dev *dev, u8 size, u64 addr,
+			 void *buf, bool write);
+int mlx5_fpga_sbu_caps(struct mlx5_core_dev *dev, void *caps, int size);
+
+int mlx5_fpga_create_qp(struct mlx5_core_dev *dev, void *fpga_qpc,
+			u32 *fpga_qpn);
+int mlx5_fpga_modify_qp(struct mlx5_core_dev *dev, u32 fpga_qpn,
+			enum mlx5_fpga_qpc_field_select fields, void *fpga_qpc);
+int mlx5_fpga_query_qp(struct mlx5_core_dev *dev, u32 fpga_qpn, void *fpga_qpc);
+int mlx5_fpga_query_qp_counters(struct mlx5_core_dev *dev, u32 fpga_qpn,
+				bool clear, struct mlx5_fpga_qp_counters *data);
+int mlx5_fpga_destroy_qp(struct mlx5_core_dev *dev, u32 fpga_qpn);
+
+#endif /* __MLX5_FPGA_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fpga/conn.c b/drivers/net/ethernet/mellanox/mlx5/core/fpga/conn.c
new file mode 100644
index 000000000000..c4392f741c5f
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/conn.c
@@ -0,0 +1,1042 @@
+/*
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#include <net/addrconf.h>
+#include <linux/etherdevice.h>
+#include <linux/mlx5/vport.h>
+
+#include "mlx5_core.h"
+#include "lib/mlx5.h"
+#include "fpga/conn.h"
+
+#define MLX5_FPGA_PKEY 0xFFFF
+#define MLX5_FPGA_PKEY_INDEX 0 /* RoCE PKEY 0xFFFF is always at index 0 */
+#define MLX5_FPGA_RECV_SIZE 2048
+#define MLX5_FPGA_PORT_NUM 1
+#define MLX5_FPGA_CQ_BUDGET 64
+
+static int mlx5_fpga_conn_map_buf(struct mlx5_fpga_conn *conn,
+				  struct mlx5_fpga_dma_buf *buf)
+{
+	struct device *dma_device;
+	int err = 0;
+
+	if (unlikely(!buf->sg[0].data))
+		goto out;
+
+	dma_device = &conn->fdev->mdev->pdev->dev;
+	buf->sg[0].dma_addr = dma_map_single(dma_device, buf->sg[0].data,
+					     buf->sg[0].size, buf->dma_dir);
+	err = dma_mapping_error(dma_device, buf->sg[0].dma_addr);
+	if (unlikely(err)) {
+		mlx5_fpga_warn(conn->fdev, "DMA error on sg 0: %d\n", err);
+		err = -ENOMEM;
+		goto out;
+	}
+
+	if (!buf->sg[1].data)
+		goto out;
+
+	buf->sg[1].dma_addr = dma_map_single(dma_device, buf->sg[1].data,
+					     buf->sg[1].size, buf->dma_dir);
+	err = dma_mapping_error(dma_device, buf->sg[1].dma_addr);
+	if (unlikely(err)) {
+		mlx5_fpga_warn(conn->fdev, "DMA error on sg 1: %d\n", err);
+		dma_unmap_single(dma_device, buf->sg[0].dma_addr,
+				 buf->sg[0].size, buf->dma_dir);
+		err = -ENOMEM;
+	}
+
+out:
+	return err;
+}
+
+static void mlx5_fpga_conn_unmap_buf(struct mlx5_fpga_conn *conn,
+				     struct mlx5_fpga_dma_buf *buf)
+{
+	struct device *dma_device;
+
+	dma_device = &conn->fdev->mdev->pdev->dev;
+	if (buf->sg[1].data)
+		dma_unmap_single(dma_device, buf->sg[1].dma_addr,
+				 buf->sg[1].size, buf->dma_dir);
+
+	if (likely(buf->sg[0].data))
+		dma_unmap_single(dma_device, buf->sg[0].dma_addr,
+				 buf->sg[0].size, buf->dma_dir);
+}
+
+static int mlx5_fpga_conn_post_recv(struct mlx5_fpga_conn *conn,
+				    struct mlx5_fpga_dma_buf *buf)
+{
+	struct mlx5_wqe_data_seg *data;
+	unsigned int ix;
+	int err = 0;
+
+	err = mlx5_fpga_conn_map_buf(conn, buf);
+	if (unlikely(err))
+		goto out;
+
+	if (unlikely(conn->qp.rq.pc - conn->qp.rq.cc >= conn->qp.rq.size)) {
+		mlx5_fpga_conn_unmap_buf(conn, buf);
+		return -EBUSY;
+	}
+
+	ix = conn->qp.rq.pc & (conn->qp.rq.size - 1);
+	data = mlx5_wq_cyc_get_wqe(&conn->qp.wq.rq, ix);
+	data->byte_count = cpu_to_be32(buf->sg[0].size);
+	data->lkey = cpu_to_be32(conn->fdev->conn_res.mkey.key);
+	data->addr = cpu_to_be64(buf->sg[0].dma_addr);
+
+	conn->qp.rq.pc++;
+	conn->qp.rq.bufs[ix] = buf;
+
+	/* Make sure that descriptors are written before doorbell record. */
+	dma_wmb();
+	*conn->qp.wq.rq.db = cpu_to_be32(conn->qp.rq.pc & 0xffff);
+out:
+	return err;
+}
+
+static void mlx5_fpga_conn_notify_hw(struct mlx5_fpga_conn *conn, void *wqe)
+{
+	/* ensure wqe is visible to device before updating doorbell record */
+	dma_wmb();
+	*conn->qp.wq.sq.db = cpu_to_be32(conn->qp.sq.pc);
+	/* Make sure that doorbell record is visible before ringing */
+	wmb();
+	mlx5_write64(wqe, conn->fdev->conn_res.uar->map + MLX5_BF_OFFSET, NULL);
+}
+
+static void mlx5_fpga_conn_post_send(struct mlx5_fpga_conn *conn,
+				     struct mlx5_fpga_dma_buf *buf)
+{
+	struct mlx5_wqe_ctrl_seg *ctrl;
+	struct mlx5_wqe_data_seg *data;
+	unsigned int ix, sgi;
+	int size = 1;
+
+	ix = conn->qp.sq.pc & (conn->qp.sq.size - 1);
+
+	ctrl = mlx5_wq_cyc_get_wqe(&conn->qp.wq.sq, ix);
+	data = (void *)(ctrl + 1);
+
+	for (sgi = 0; sgi < ARRAY_SIZE(buf->sg); sgi++) {
+		if (!buf->sg[sgi].data)
+			break;
+		data->byte_count = cpu_to_be32(buf->sg[sgi].size);
+		data->lkey = cpu_to_be32(conn->fdev->conn_res.mkey.key);
+		data->addr = cpu_to_be64(buf->sg[sgi].dma_addr);
+		data++;
+		size++;
+	}
+
+	ctrl->imm = 0;
+	ctrl->fm_ce_se = MLX5_WQE_CTRL_CQ_UPDATE;
+	ctrl->opmod_idx_opcode = cpu_to_be32(((conn->qp.sq.pc & 0xffff) << 8) |
+					     MLX5_OPCODE_SEND);
+	ctrl->qpn_ds = cpu_to_be32(size | (conn->qp.mqp.qpn << 8));
+
+	conn->qp.sq.pc++;
+	conn->qp.sq.bufs[ix] = buf;
+	mlx5_fpga_conn_notify_hw(conn, ctrl);
+}
+
+int mlx5_fpga_conn_send(struct mlx5_fpga_conn *conn,
+			struct mlx5_fpga_dma_buf *buf)
+{
+	unsigned long flags;
+	int err;
+
+	if (!conn->qp.active)
+		return -ENOTCONN;
+
+	err = mlx5_fpga_conn_map_buf(conn, buf);
+	if (err)
+		return err;
+
+	spin_lock_irqsave(&conn->qp.sq.lock, flags);
+
+	if (conn->qp.sq.pc - conn->qp.sq.cc >= conn->qp.sq.size) {
+		list_add_tail(&buf->list, &conn->qp.sq.backlog);
+		goto out_unlock;
+	}
+
+	mlx5_fpga_conn_post_send(conn, buf);
+
+out_unlock:
+	spin_unlock_irqrestore(&conn->qp.sq.lock, flags);
+	return err;
+}
+
+static int mlx5_fpga_conn_post_recv_buf(struct mlx5_fpga_conn *conn)
+{
+	struct mlx5_fpga_dma_buf *buf;
+	int err;
+
+	buf = kzalloc(sizeof(*buf) + MLX5_FPGA_RECV_SIZE, 0);
+	if (!buf)
+		return -ENOMEM;
+
+	buf->sg[0].data = (void *)(buf + 1);
+	buf->sg[0].size = MLX5_FPGA_RECV_SIZE;
+	buf->dma_dir = DMA_FROM_DEVICE;
+
+	err = mlx5_fpga_conn_post_recv(conn, buf);
+	if (err)
+		kfree(buf);
+
+	return err;
+}
+
+static int mlx5_fpga_conn_create_mkey(struct mlx5_core_dev *mdev, u32 pdn,
+				      struct mlx5_core_mkey *mkey)
+{
+	int inlen = MLX5_ST_SZ_BYTES(create_mkey_in);
+	void *mkc;
+	u32 *in;
+	int err;
+
+	in = kvzalloc(inlen, GFP_KERNEL);
+	if (!in)
+		return -ENOMEM;
+
+	mkc = MLX5_ADDR_OF(create_mkey_in, in, memory_key_mkey_entry);
+	MLX5_SET(mkc, mkc, access_mode, MLX5_MKC_ACCESS_MODE_PA);
+	MLX5_SET(mkc, mkc, lw, 1);
+	MLX5_SET(mkc, mkc, lr, 1);
+
+	MLX5_SET(mkc, mkc, pd, pdn);
+	MLX5_SET(mkc, mkc, length64, 1);
+	MLX5_SET(mkc, mkc, qpn, 0xffffff);
+
+	err = mlx5_core_create_mkey(mdev, mkey, in, inlen);
+
+	kvfree(in);
+	return err;
+}
+
+static void mlx5_fpga_conn_rq_cqe(struct mlx5_fpga_conn *conn,
+				  struct mlx5_cqe64 *cqe, u8 status)
+{
+	struct mlx5_fpga_dma_buf *buf;
+	int ix, err;
+
+	ix = be16_to_cpu(cqe->wqe_counter) & (conn->qp.rq.size - 1);
+	buf = conn->qp.rq.bufs[ix];
+	conn->qp.rq.bufs[ix] = NULL;
+	if (!status)
+		buf->sg[0].size = be32_to_cpu(cqe->byte_cnt);
+	conn->qp.rq.cc++;
+
+	if (unlikely(status && (status != MLX5_CQE_SYNDROME_WR_FLUSH_ERR)))
+		mlx5_fpga_warn(conn->fdev, "RQ buf %p on FPGA QP %u completion status %d\n",
+			       buf, conn->fpga_qpn, status);
+	else
+		mlx5_fpga_dbg(conn->fdev, "RQ buf %p on FPGA QP %u completion status %d\n",
+			      buf, conn->fpga_qpn, status);
+
+	mlx5_fpga_conn_unmap_buf(conn, buf);
+
+	if (unlikely(status || !conn->qp.active)) {
+		conn->qp.active = false;
+		kfree(buf);
+		return;
+	}
+
+	mlx5_fpga_dbg(conn->fdev, "Message with %u bytes received successfully\n",
+		      buf->sg[0].size);
+	conn->recv_cb(conn->cb_arg, buf);
+
+	buf->sg[0].size = MLX5_FPGA_RECV_SIZE;
+	err = mlx5_fpga_conn_post_recv(conn, buf);
+	if (unlikely(err)) {
+		mlx5_fpga_warn(conn->fdev,
+			       "Failed to re-post recv buf: %d\n", err);
+		kfree(buf);
+	}
+}
+
+static void mlx5_fpga_conn_sq_cqe(struct mlx5_fpga_conn *conn,
+				  struct mlx5_cqe64 *cqe, u8 status)
+{
+	struct mlx5_fpga_dma_buf *buf, *nextbuf;
+	unsigned long flags;
+	int ix;
+
+	spin_lock_irqsave(&conn->qp.sq.lock, flags);
+
+	ix = be16_to_cpu(cqe->wqe_counter) & (conn->qp.sq.size - 1);
+	buf = conn->qp.sq.bufs[ix];
+	conn->qp.sq.bufs[ix] = NULL;
+	conn->qp.sq.cc++;
+
+	/* Handle backlog still under the spinlock to ensure message post order */
+	if (unlikely(!list_empty(&conn->qp.sq.backlog))) {
+		if (likely(conn->qp.active)) {
+			nextbuf = list_first_entry(&conn->qp.sq.backlog,
+						   struct mlx5_fpga_dma_buf, list);
+			list_del(&nextbuf->list);
+			mlx5_fpga_conn_post_send(conn, nextbuf);
+		}
+	}
+
+	spin_unlock_irqrestore(&conn->qp.sq.lock, flags);
+
+	if (unlikely(status && (status != MLX5_CQE_SYNDROME_WR_FLUSH_ERR)))
+		mlx5_fpga_warn(conn->fdev, "SQ buf %p on FPGA QP %u completion status %d\n",
+			       buf, conn->fpga_qpn, status);
+	else
+		mlx5_fpga_dbg(conn->fdev, "SQ buf %p on FPGA QP %u completion status %d\n",
+			      buf, conn->fpga_qpn, status);
+
+	mlx5_fpga_conn_unmap_buf(conn, buf);
+
+	if (likely(buf->complete))
+		buf->complete(conn, conn->fdev, buf, status);
+
+	if (unlikely(status))
+		conn->qp.active = false;
+}
+
+static void mlx5_fpga_conn_handle_cqe(struct mlx5_fpga_conn *conn,
+				      struct mlx5_cqe64 *cqe)
+{
+	u8 opcode, status = 0;
+
+	opcode = cqe->op_own >> 4;
+
+	switch (opcode) {
+	case MLX5_CQE_REQ_ERR:
+		status = ((struct mlx5_err_cqe *)cqe)->syndrome;
+		/* Fall through */
+	case MLX5_CQE_REQ:
+		mlx5_fpga_conn_sq_cqe(conn, cqe, status);
+		break;
+
+	case MLX5_CQE_RESP_ERR:
+		status = ((struct mlx5_err_cqe *)cqe)->syndrome;
+		/* Fall through */
+	case MLX5_CQE_RESP_SEND:
+		mlx5_fpga_conn_rq_cqe(conn, cqe, status);
+		break;
+	default:
+		mlx5_fpga_warn(conn->fdev, "Unexpected cqe opcode %u\n",
+			       opcode);
+	}
+}
+
+static void mlx5_fpga_conn_arm_cq(struct mlx5_fpga_conn *conn)
+{
+	mlx5_cq_arm(&conn->cq.mcq, MLX5_CQ_DB_REQ_NOT,
+		    conn->fdev->conn_res.uar->map, conn->cq.wq.cc);
+}
+
+static void mlx5_fpga_conn_cq_event(struct mlx5_core_cq *mcq,
+				    enum mlx5_event event)
+{
+	struct mlx5_fpga_conn *conn;
+
+	conn = container_of(mcq, struct mlx5_fpga_conn, cq.mcq);
+	mlx5_fpga_warn(conn->fdev, "CQ event %u on CQ #%u\n", event, mcq->cqn);
+}
+
+static void mlx5_fpga_conn_event(struct mlx5_core_qp *mqp, int event)
+{
+	struct mlx5_fpga_conn *conn;
+
+	conn = container_of(mqp, struct mlx5_fpga_conn, qp.mqp);
+	mlx5_fpga_warn(conn->fdev, "QP event %u on QP #%u\n", event, mqp->qpn);
+}
+
+static inline void mlx5_fpga_conn_cqes(struct mlx5_fpga_conn *conn,
+				       unsigned int budget)
+{
+	struct mlx5_cqe64 *cqe;
+
+	while (budget) {
+		cqe = mlx5_cqwq_get_cqe(&conn->cq.wq);
+		if (!cqe)
+			break;
+
+		budget--;
+		mlx5_cqwq_pop(&conn->cq.wq);
+		mlx5_fpga_conn_handle_cqe(conn, cqe);
+		mlx5_cqwq_update_db_record(&conn->cq.wq);
+	}
+	if (!budget) {
+		tasklet_schedule(&conn->cq.tasklet);
+		return;
+	}
+
+	mlx5_fpga_dbg(conn->fdev, "Re-arming CQ with cc# %u\n", conn->cq.wq.cc);
+	/* ensure cq space is freed before enabling more cqes */
+	wmb();
+	mlx5_fpga_conn_arm_cq(conn);
+}
+
+static void mlx5_fpga_conn_cq_tasklet(unsigned long data)
+{
+	struct mlx5_fpga_conn *conn = (void *)data;
+
+	if (unlikely(!conn->qp.active))
+		return;
+	mlx5_fpga_conn_cqes(conn, MLX5_FPGA_CQ_BUDGET);
+}
+
+static void mlx5_fpga_conn_cq_complete(struct mlx5_core_cq *mcq)
+{
+	struct mlx5_fpga_conn *conn;
+
+	conn = container_of(mcq, struct mlx5_fpga_conn, cq.mcq);
+	if (unlikely(!conn->qp.active))
+		return;
+	mlx5_fpga_conn_cqes(conn, MLX5_FPGA_CQ_BUDGET);
+}
+
+static int mlx5_fpga_conn_create_cq(struct mlx5_fpga_conn *conn, int cq_size)
+{
+	struct mlx5_fpga_device *fdev = conn->fdev;
+	struct mlx5_core_dev *mdev = fdev->mdev;
+	u32 temp_cqc[MLX5_ST_SZ_DW(cqc)] = {0};
+	struct mlx5_wq_param wqp;
+	struct mlx5_cqe64 *cqe;
+	int inlen, err, eqn;
+	unsigned int irqn;
+	void *cqc, *in;
+	__be64 *pas;
+	u32 i;
+
+	cq_size = roundup_pow_of_two(cq_size);
+	MLX5_SET(cqc, temp_cqc, log_cq_size, ilog2(cq_size));
+
+	wqp.buf_numa_node = mdev->priv.numa_node;
+	wqp.db_numa_node  = mdev->priv.numa_node;
+
+	err = mlx5_cqwq_create(mdev, &wqp, temp_cqc, &conn->cq.wq,
+			       &conn->cq.wq_ctrl);
+	if (err)
+		return err;
+
+	for (i = 0; i < mlx5_cqwq_get_size(&conn->cq.wq); i++) {
+		cqe = mlx5_cqwq_get_wqe(&conn->cq.wq, i);
+		cqe->op_own = MLX5_CQE_INVALID << 4 | MLX5_CQE_OWNER_MASK;
+	}
+
+	inlen = MLX5_ST_SZ_BYTES(create_cq_in) +
+		sizeof(u64) * conn->cq.wq_ctrl.frag_buf.npages;
+	in = kvzalloc(inlen, GFP_KERNEL);
+	if (!in) {
+		err = -ENOMEM;
+		goto err_cqwq;
+	}
+
+	err = mlx5_vector2eqn(mdev, smp_processor_id(), &eqn, &irqn);
+	if (err)
+		goto err_cqwq;
+
+	cqc = MLX5_ADDR_OF(create_cq_in, in, cq_context);
+	MLX5_SET(cqc, cqc, log_cq_size, ilog2(cq_size));
+	MLX5_SET(cqc, cqc, c_eqn, eqn);
+	MLX5_SET(cqc, cqc, uar_page, fdev->conn_res.uar->index);
+	MLX5_SET(cqc, cqc, log_page_size, conn->cq.wq_ctrl.frag_buf.page_shift -
+			   MLX5_ADAPTER_PAGE_SHIFT);
+	MLX5_SET64(cqc, cqc, dbr_addr, conn->cq.wq_ctrl.db.dma);
+
+	pas = (__be64 *)MLX5_ADDR_OF(create_cq_in, in, pas);
+	mlx5_fill_page_frag_array(&conn->cq.wq_ctrl.frag_buf, pas);
+
+	err = mlx5_core_create_cq(mdev, &conn->cq.mcq, in, inlen);
+	kvfree(in);
+
+	if (err)
+		goto err_cqwq;
+
+	conn->cq.mcq.cqe_sz     = 64;
+	conn->cq.mcq.set_ci_db  = conn->cq.wq_ctrl.db.db;
+	conn->cq.mcq.arm_db     = conn->cq.wq_ctrl.db.db + 1;
+	*conn->cq.mcq.set_ci_db = 0;
+	*conn->cq.mcq.arm_db    = 0;
+	conn->cq.mcq.vector     = 0;
+	conn->cq.mcq.comp       = mlx5_fpga_conn_cq_complete;
+	conn->cq.mcq.event      = mlx5_fpga_conn_cq_event;
+	conn->cq.mcq.irqn       = irqn;
+	conn->cq.mcq.uar        = fdev->conn_res.uar;
+	tasklet_init(&conn->cq.tasklet, mlx5_fpga_conn_cq_tasklet,
+		     (unsigned long)conn);
+
+	mlx5_fpga_dbg(fdev, "Created CQ #0x%x\n", conn->cq.mcq.cqn);
+
+	goto out;
+
+err_cqwq:
+	mlx5_cqwq_destroy(&conn->cq.wq_ctrl);
+out:
+	return err;
+}
+
+static void mlx5_fpga_conn_destroy_cq(struct mlx5_fpga_conn *conn)
+{
+	tasklet_disable(&conn->cq.tasklet);
+	tasklet_kill(&conn->cq.tasklet);
+	mlx5_core_destroy_cq(conn->fdev->mdev, &conn->cq.mcq);
+	mlx5_cqwq_destroy(&conn->cq.wq_ctrl);
+}
+
+static int mlx5_fpga_conn_create_wq(struct mlx5_fpga_conn *conn, void *qpc)
+{
+	struct mlx5_fpga_device *fdev = conn->fdev;
+	struct mlx5_core_dev *mdev = fdev->mdev;
+	struct mlx5_wq_param wqp;
+
+	wqp.buf_numa_node = mdev->priv.numa_node;
+	wqp.db_numa_node  = mdev->priv.numa_node;
+
+	return mlx5_wq_qp_create(mdev, &wqp, qpc, &conn->qp.wq,
+				 &conn->qp.wq_ctrl);
+}
+
+static int mlx5_fpga_conn_create_qp(struct mlx5_fpga_conn *conn,
+				    unsigned int tx_size, unsigned int rx_size)
+{
+	struct mlx5_fpga_device *fdev = conn->fdev;
+	struct mlx5_core_dev *mdev = fdev->mdev;
+	u32 temp_qpc[MLX5_ST_SZ_DW(qpc)] = {0};
+	void *in = NULL, *qpc;
+	int err, inlen;
+
+	conn->qp.rq.pc = 0;
+	conn->qp.rq.cc = 0;
+	conn->qp.rq.size = roundup_pow_of_two(rx_size);
+	conn->qp.sq.pc = 0;
+	conn->qp.sq.cc = 0;
+	conn->qp.sq.size = roundup_pow_of_two(tx_size);
+
+	MLX5_SET(qpc, temp_qpc, log_rq_stride, ilog2(MLX5_SEND_WQE_DS) - 4);
+	MLX5_SET(qpc, temp_qpc, log_rq_size, ilog2(conn->qp.rq.size));
+	MLX5_SET(qpc, temp_qpc, log_sq_size, ilog2(conn->qp.sq.size));
+	err = mlx5_fpga_conn_create_wq(conn, temp_qpc);
+	if (err)
+		goto out;
+
+	conn->qp.rq.bufs = kvzalloc(sizeof(conn->qp.rq.bufs[0]) *
+				    conn->qp.rq.size, GFP_KERNEL);
+	if (!conn->qp.rq.bufs) {
+		err = -ENOMEM;
+		goto err_wq;
+	}
+
+	conn->qp.sq.bufs = kvzalloc(sizeof(conn->qp.sq.bufs[0]) *
+				    conn->qp.sq.size, GFP_KERNEL);
+	if (!conn->qp.sq.bufs) {
+		err = -ENOMEM;
+		goto err_rq_bufs;
+	}
+
+	inlen = MLX5_ST_SZ_BYTES(create_qp_in) +
+		MLX5_FLD_SZ_BYTES(create_qp_in, pas[0]) *
+		conn->qp.wq_ctrl.buf.npages;
+	in = kvzalloc(inlen, GFP_KERNEL);
+	if (!in) {
+		err = -ENOMEM;
+		goto err_sq_bufs;
+	}
+
+	qpc = MLX5_ADDR_OF(create_qp_in, in, qpc);
+	MLX5_SET(qpc, qpc, uar_page, fdev->conn_res.uar->index);
+	MLX5_SET(qpc, qpc, log_page_size,
+		 conn->qp.wq_ctrl.buf.page_shift - MLX5_ADAPTER_PAGE_SHIFT);
+	MLX5_SET(qpc, qpc, fre, 1);
+	MLX5_SET(qpc, qpc, rlky, 1);
+	MLX5_SET(qpc, qpc, st, MLX5_QP_ST_RC);
+	MLX5_SET(qpc, qpc, pm_state, MLX5_QP_PM_MIGRATED);
+	MLX5_SET(qpc, qpc, pd, fdev->conn_res.pdn);
+	MLX5_SET(qpc, qpc, log_rq_stride, ilog2(MLX5_SEND_WQE_DS) - 4);
+	MLX5_SET(qpc, qpc, log_rq_size, ilog2(conn->qp.rq.size));
+	MLX5_SET(qpc, qpc, rq_type, MLX5_NON_ZERO_RQ);
+	MLX5_SET(qpc, qpc, log_sq_size, ilog2(conn->qp.sq.size));
+	MLX5_SET(qpc, qpc, cqn_snd, conn->cq.mcq.cqn);
+	MLX5_SET(qpc, qpc, cqn_rcv, conn->cq.mcq.cqn);
+	MLX5_SET64(qpc, qpc, dbr_addr, conn->qp.wq_ctrl.db.dma);
+	if (MLX5_CAP_GEN(mdev, cqe_version) == 1)
+		MLX5_SET(qpc, qpc, user_index, 0xFFFFFF);
+
+	mlx5_fill_page_array(&conn->qp.wq_ctrl.buf,
+			     (__be64 *)MLX5_ADDR_OF(create_qp_in, in, pas));
+
+	err = mlx5_core_create_qp(mdev, &conn->qp.mqp, in, inlen);
+	if (err)
+		goto err_sq_bufs;
+
+	conn->qp.mqp.event = mlx5_fpga_conn_event;
+	mlx5_fpga_dbg(fdev, "Created QP #0x%x\n", conn->qp.mqp.qpn);
+
+	goto out;
+
+err_sq_bufs:
+	kvfree(conn->qp.sq.bufs);
+err_rq_bufs:
+	kvfree(conn->qp.rq.bufs);
+err_wq:
+	mlx5_wq_destroy(&conn->qp.wq_ctrl);
+out:
+	kvfree(in);
+	return err;
+}
+
+static void mlx5_fpga_conn_free_recv_bufs(struct mlx5_fpga_conn *conn)
+{
+	int ix;
+
+	for (ix = 0; ix < conn->qp.rq.size; ix++) {
+		if (!conn->qp.rq.bufs[ix])
+			continue;
+		mlx5_fpga_conn_unmap_buf(conn, conn->qp.rq.bufs[ix]);
+		kfree(conn->qp.rq.bufs[ix]);
+		conn->qp.rq.bufs[ix] = NULL;
+	}
+}
+
+static void mlx5_fpga_conn_flush_send_bufs(struct mlx5_fpga_conn *conn)
+{
+	struct mlx5_fpga_dma_buf *buf, *temp;
+	int ix;
+
+	for (ix = 0; ix < conn->qp.sq.size; ix++) {
+		buf = conn->qp.sq.bufs[ix];
+		if (!buf)
+			continue;
+		conn->qp.sq.bufs[ix] = NULL;
+		mlx5_fpga_conn_unmap_buf(conn, buf);
+		if (!buf->complete)
+			continue;
+		buf->complete(conn, conn->fdev, buf, MLX5_CQE_SYNDROME_WR_FLUSH_ERR);
+	}
+	list_for_each_entry_safe(buf, temp, &conn->qp.sq.backlog, list) {
+		mlx5_fpga_conn_unmap_buf(conn, buf);
+		if (!buf->complete)
+			continue;
+		buf->complete(conn, conn->fdev, buf, MLX5_CQE_SYNDROME_WR_FLUSH_ERR);
+	}
+}
+
+static void mlx5_fpga_conn_destroy_qp(struct mlx5_fpga_conn *conn)
+{
+	mlx5_core_destroy_qp(conn->fdev->mdev, &conn->qp.mqp);
+	mlx5_fpga_conn_free_recv_bufs(conn);
+	mlx5_fpga_conn_flush_send_bufs(conn);
+	kvfree(conn->qp.sq.bufs);
+	kvfree(conn->qp.rq.bufs);
+	mlx5_wq_destroy(&conn->qp.wq_ctrl);
+}
+
+static inline int mlx5_fpga_conn_reset_qp(struct mlx5_fpga_conn *conn)
+{
+	struct mlx5_core_dev *mdev = conn->fdev->mdev;
+
+	mlx5_fpga_dbg(conn->fdev, "Modifying QP %u to RST\n", conn->qp.mqp.qpn);
+
+	return mlx5_core_qp_modify(mdev, MLX5_CMD_OP_2RST_QP, 0, NULL,
+				   &conn->qp.mqp);
+}
+
+static inline int mlx5_fpga_conn_init_qp(struct mlx5_fpga_conn *conn)
+{
+	struct mlx5_fpga_device *fdev = conn->fdev;
+	struct mlx5_core_dev *mdev = fdev->mdev;
+	u32 *qpc = NULL;
+	int err;
+
+	mlx5_fpga_dbg(conn->fdev, "Modifying QP %u to INIT\n", conn->qp.mqp.qpn);
+
+	qpc = kzalloc(MLX5_ST_SZ_BYTES(qpc), GFP_KERNEL);
+	if (!qpc) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	MLX5_SET(qpc, qpc, st, MLX5_QP_ST_RC);
+	MLX5_SET(qpc, qpc, pm_state, MLX5_QP_PM_MIGRATED);
+	MLX5_SET(qpc, qpc, primary_address_path.pkey_index, MLX5_FPGA_PKEY_INDEX);
+	MLX5_SET(qpc, qpc, primary_address_path.port, MLX5_FPGA_PORT_NUM);
+	MLX5_SET(qpc, qpc, pd, conn->fdev->conn_res.pdn);
+	MLX5_SET(qpc, qpc, cqn_snd, conn->cq.mcq.cqn);
+	MLX5_SET(qpc, qpc, cqn_rcv, conn->cq.mcq.cqn);
+	MLX5_SET64(qpc, qpc, dbr_addr, conn->qp.wq_ctrl.db.dma);
+
+	err = mlx5_core_qp_modify(mdev, MLX5_CMD_OP_RST2INIT_QP, 0, qpc,
+				  &conn->qp.mqp);
+	if (err) {
+		mlx5_fpga_warn(fdev, "qp_modify RST2INIT failed: %d\n", err);
+		goto out;
+	}
+
+out:
+	kfree(qpc);
+	return err;
+}
+
+static inline int mlx5_fpga_conn_rtr_qp(struct mlx5_fpga_conn *conn)
+{
+	struct mlx5_fpga_device *fdev = conn->fdev;
+	struct mlx5_core_dev *mdev = fdev->mdev;
+	u32 *qpc = NULL;
+	int err;
+
+	mlx5_fpga_dbg(conn->fdev, "QP RTR\n");
+
+	qpc = kzalloc(MLX5_ST_SZ_BYTES(qpc), GFP_KERNEL);
+	if (!qpc) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	MLX5_SET(qpc, qpc, mtu, MLX5_QPC_MTU_1K_BYTES);
+	MLX5_SET(qpc, qpc, log_msg_max, (u8)MLX5_CAP_GEN(mdev, log_max_msg));
+	MLX5_SET(qpc, qpc, remote_qpn, conn->fpga_qpn);
+	MLX5_SET(qpc, qpc, next_rcv_psn,
+		 MLX5_GET(fpga_qpc, conn->fpga_qpc, next_send_psn));
+	MLX5_SET(qpc, qpc, primary_address_path.pkey_index, MLX5_FPGA_PKEY_INDEX);
+	MLX5_SET(qpc, qpc, primary_address_path.port, MLX5_FPGA_PORT_NUM);
+	ether_addr_copy(MLX5_ADDR_OF(qpc, qpc, primary_address_path.rmac_47_32),
+			MLX5_ADDR_OF(fpga_qpc, conn->fpga_qpc, fpga_mac_47_32));
+	MLX5_SET(qpc, qpc, primary_address_path.udp_sport,
+		 MLX5_CAP_ROCE(mdev, r_roce_min_src_udp_port));
+	MLX5_SET(qpc, qpc, primary_address_path.src_addr_index,
+		 conn->qp.sgid_index);
+	MLX5_SET(qpc, qpc, primary_address_path.hop_limit, 0);
+	memcpy(MLX5_ADDR_OF(qpc, qpc, primary_address_path.rgid_rip),
+	       MLX5_ADDR_OF(fpga_qpc, conn->fpga_qpc, fpga_ip),
+	       MLX5_FLD_SZ_BYTES(qpc, primary_address_path.rgid_rip));
+
+	err = mlx5_core_qp_modify(mdev, MLX5_CMD_OP_INIT2RTR_QP, 0, qpc,
+				  &conn->qp.mqp);
+	if (err) {
+		mlx5_fpga_warn(fdev, "qp_modify RST2INIT failed: %d\n", err);
+		goto out;
+	}
+
+out:
+	kfree(qpc);
+	return err;
+}
+
+static inline int mlx5_fpga_conn_rts_qp(struct mlx5_fpga_conn *conn)
+{
+	struct mlx5_fpga_device *fdev = conn->fdev;
+	struct mlx5_core_dev *mdev = fdev->mdev;
+	u32 *qpc = NULL;
+	u32 opt_mask;
+	int err;
+
+	mlx5_fpga_dbg(conn->fdev, "QP RTS\n");
+
+	qpc = kzalloc(MLX5_ST_SZ_BYTES(qpc), GFP_KERNEL);
+	if (!qpc) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	MLX5_SET(qpc, qpc, log_ack_req_freq, 8);
+	MLX5_SET(qpc, qpc, min_rnr_nak, 0x12);
+	MLX5_SET(qpc, qpc, primary_address_path.ack_timeout, 0x12); /* ~1.07s */
+	MLX5_SET(qpc, qpc, next_send_psn,
+		 MLX5_GET(fpga_qpc, conn->fpga_qpc, next_rcv_psn));
+	MLX5_SET(qpc, qpc, retry_count, 7);
+	MLX5_SET(qpc, qpc, rnr_retry, 7); /* Infinite retry if RNR NACK */
+
+	opt_mask = MLX5_QP_OPTPAR_RNR_TIMEOUT;
+	err = mlx5_core_qp_modify(mdev, MLX5_CMD_OP_RTR2RTS_QP, opt_mask, qpc,
+				  &conn->qp.mqp);
+	if (err) {
+		mlx5_fpga_warn(fdev, "qp_modify RST2INIT failed: %d\n", err);
+		goto out;
+	}
+
+out:
+	kfree(qpc);
+	return err;
+}
+
+static int mlx5_fpga_conn_connect(struct mlx5_fpga_conn *conn)
+{
+	struct mlx5_fpga_device *fdev = conn->fdev;
+	int err;
+
+	MLX5_SET(fpga_qpc, conn->fpga_qpc, state, MLX5_FPGA_QPC_STATE_ACTIVE);
+	err = mlx5_fpga_modify_qp(conn->fdev->mdev, conn->fpga_qpn,
+				  MLX5_FPGA_QPC_STATE, &conn->fpga_qpc);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to activate FPGA RC QP: %d\n", err);
+		goto out;
+	}
+
+	err = mlx5_fpga_conn_reset_qp(conn);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to change QP state to reset\n");
+		goto err_fpga_qp;
+	}
+
+	err = mlx5_fpga_conn_init_qp(conn);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to modify QP from RESET to INIT\n");
+		goto err_fpga_qp;
+	}
+	conn->qp.active = true;
+
+	while (!mlx5_fpga_conn_post_recv_buf(conn))
+		;
+
+	err = mlx5_fpga_conn_rtr_qp(conn);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to change QP state from INIT to RTR\n");
+		goto err_recv_bufs;
+	}
+
+	err = mlx5_fpga_conn_rts_qp(conn);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to change QP state from RTR to RTS\n");
+		goto err_recv_bufs;
+	}
+	goto out;
+
+err_recv_bufs:
+	mlx5_fpga_conn_free_recv_bufs(conn);
+err_fpga_qp:
+	MLX5_SET(fpga_qpc, conn->fpga_qpc, state, MLX5_FPGA_QPC_STATE_INIT);
+	if (mlx5_fpga_modify_qp(conn->fdev->mdev, conn->fpga_qpn,
+				MLX5_FPGA_QPC_STATE, &conn->fpga_qpc))
+		mlx5_fpga_err(fdev, "Failed to revert FPGA QP to INIT\n");
+out:
+	return err;
+}
+
+struct mlx5_fpga_conn *mlx5_fpga_conn_create(struct mlx5_fpga_device *fdev,
+					     struct mlx5_fpga_conn_attr *attr,
+					     enum mlx5_ifc_fpga_qp_type qp_type)
+{
+	struct mlx5_fpga_conn *ret, *conn;
+	u8 *remote_mac, *remote_ip;
+	int err;
+
+	if (!attr->recv_cb)
+		return ERR_PTR(-EINVAL);
+
+	conn = kzalloc(sizeof(*conn), GFP_KERNEL);
+	if (!conn)
+		return ERR_PTR(-ENOMEM);
+
+	conn->fdev = fdev;
+	INIT_LIST_HEAD(&conn->qp.sq.backlog);
+
+	spin_lock_init(&conn->qp.sq.lock);
+
+	conn->recv_cb = attr->recv_cb;
+	conn->cb_arg = attr->cb_arg;
+
+	remote_mac = MLX5_ADDR_OF(fpga_qpc, conn->fpga_qpc, remote_mac_47_32);
+	err = mlx5_query_nic_vport_mac_address(fdev->mdev, 0, remote_mac);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to query local MAC: %d\n", err);
+		ret = ERR_PTR(err);
+		goto err;
+	}
+
+	/* Build Modified EUI-64 IPv6 address from the MAC address */
+	remote_ip = MLX5_ADDR_OF(fpga_qpc, conn->fpga_qpc, remote_ip);
+	remote_ip[0] = 0xfe;
+	remote_ip[1] = 0x80;
+	addrconf_addr_eui48(&remote_ip[8], remote_mac);
+
+	err = mlx5_core_reserved_gid_alloc(fdev->mdev, &conn->qp.sgid_index);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to allocate SGID: %d\n", err);
+		ret = ERR_PTR(err);
+		goto err;
+	}
+
+	err = mlx5_core_roce_gid_set(fdev->mdev, conn->qp.sgid_index,
+				     MLX5_ROCE_VERSION_2,
+				     MLX5_ROCE_L3_TYPE_IPV6,
+				     remote_ip, remote_mac, true, 0);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to set SGID: %d\n", err);
+		ret = ERR_PTR(err);
+		goto err_rsvd_gid;
+	}
+	mlx5_fpga_dbg(fdev, "Reserved SGID index %u\n", conn->qp.sgid_index);
+
+	/* Allow for one cqe per rx/tx wqe, plus one cqe for the next wqe,
+	 * created during processing of the cqe
+	 */
+	err = mlx5_fpga_conn_create_cq(conn,
+				       (attr->tx_size + attr->rx_size) * 2);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to create CQ: %d\n", err);
+		ret = ERR_PTR(err);
+		goto err_gid;
+	}
+
+	mlx5_fpga_conn_arm_cq(conn);
+
+	err = mlx5_fpga_conn_create_qp(conn, attr->tx_size, attr->rx_size);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to create QP: %d\n", err);
+		ret = ERR_PTR(err);
+		goto err_cq;
+	}
+
+	MLX5_SET(fpga_qpc, conn->fpga_qpc, state, MLX5_FPGA_QPC_STATE_INIT);
+	MLX5_SET(fpga_qpc, conn->fpga_qpc, qp_type, qp_type);
+	MLX5_SET(fpga_qpc, conn->fpga_qpc, st, MLX5_FPGA_QPC_ST_RC);
+	MLX5_SET(fpga_qpc, conn->fpga_qpc, ether_type, ETH_P_8021Q);
+	MLX5_SET(fpga_qpc, conn->fpga_qpc, vid, 0);
+	MLX5_SET(fpga_qpc, conn->fpga_qpc, next_rcv_psn, 1);
+	MLX5_SET(fpga_qpc, conn->fpga_qpc, next_send_psn, 0);
+	MLX5_SET(fpga_qpc, conn->fpga_qpc, pkey, MLX5_FPGA_PKEY);
+	MLX5_SET(fpga_qpc, conn->fpga_qpc, remote_qpn, conn->qp.mqp.qpn);
+	MLX5_SET(fpga_qpc, conn->fpga_qpc, rnr_retry, 7);
+	MLX5_SET(fpga_qpc, conn->fpga_qpc, retry_count, 7);
+
+	err = mlx5_fpga_create_qp(fdev->mdev, &conn->fpga_qpc,
+				  &conn->fpga_qpn);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to create FPGA RC QP: %d\n", err);
+		ret = ERR_PTR(err);
+		goto err_qp;
+	}
+
+	err = mlx5_fpga_conn_connect(conn);
+	if (err) {
+		ret = ERR_PTR(err);
+		goto err_conn;
+	}
+
+	mlx5_fpga_dbg(fdev, "FPGA QPN is %u\n", conn->fpga_qpn);
+	ret = conn;
+	goto out;
+
+err_conn:
+	mlx5_fpga_destroy_qp(conn->fdev->mdev, conn->fpga_qpn);
+err_qp:
+	mlx5_fpga_conn_destroy_qp(conn);
+err_cq:
+	mlx5_fpga_conn_destroy_cq(conn);
+err_gid:
+	mlx5_core_roce_gid_set(fdev->mdev, conn->qp.sgid_index, 0, 0, NULL,
+			       NULL, false, 0);
+err_rsvd_gid:
+	mlx5_core_reserved_gid_free(fdev->mdev, conn->qp.sgid_index);
+err:
+	kfree(conn);
+out:
+	return ret;
+}
+
+void mlx5_fpga_conn_destroy(struct mlx5_fpga_conn *conn)
+{
+	struct mlx5_fpga_device *fdev = conn->fdev;
+	struct mlx5_core_dev *mdev = fdev->mdev;
+	int err = 0;
+
+	conn->qp.active = false;
+	tasklet_disable(&conn->cq.tasklet);
+	synchronize_irq(conn->cq.mcq.irqn);
+
+	mlx5_fpga_destroy_qp(conn->fdev->mdev, conn->fpga_qpn);
+	err = mlx5_core_qp_modify(mdev, MLX5_CMD_OP_2ERR_QP, 0, NULL,
+				  &conn->qp.mqp);
+	if (err)
+		mlx5_fpga_warn(fdev, "qp_modify 2ERR failed: %d\n", err);
+	mlx5_fpga_conn_destroy_qp(conn);
+	mlx5_fpga_conn_destroy_cq(conn);
+
+	mlx5_core_roce_gid_set(conn->fdev->mdev, conn->qp.sgid_index, 0, 0,
+			       NULL, NULL, false, 0);
+	mlx5_core_reserved_gid_free(conn->fdev->mdev, conn->qp.sgid_index);
+	kfree(conn);
+}
+
+int mlx5_fpga_conn_device_init(struct mlx5_fpga_device *fdev)
+{
+	int err;
+
+	err = mlx5_nic_vport_enable_roce(fdev->mdev);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to enable RoCE: %d\n", err);
+		goto out;
+	}
+
+	fdev->conn_res.uar = mlx5_get_uars_page(fdev->mdev);
+	if (IS_ERR(fdev->conn_res.uar)) {
+		err = PTR_ERR(fdev->conn_res.uar);
+		mlx5_fpga_err(fdev, "get_uars_page failed, %d\n", err);
+		goto err_roce;
+	}
+	mlx5_fpga_dbg(fdev, "Allocated UAR index %u\n",
+		      fdev->conn_res.uar->index);
+
+	err = mlx5_core_alloc_pd(fdev->mdev, &fdev->conn_res.pdn);
+	if (err) {
+		mlx5_fpga_err(fdev, "alloc pd failed, %d\n", err);
+		goto err_uar;
+	}
+	mlx5_fpga_dbg(fdev, "Allocated PD %u\n", fdev->conn_res.pdn);
+
+	err = mlx5_fpga_conn_create_mkey(fdev->mdev, fdev->conn_res.pdn,
+					 &fdev->conn_res.mkey);
+	if (err) {
+		mlx5_fpga_err(fdev, "create mkey failed, %d\n", err);
+		goto err_dealloc_pd;
+	}
+	mlx5_fpga_dbg(fdev, "Created mkey 0x%x\n", fdev->conn_res.mkey.key);
+
+	return 0;
+
+err_dealloc_pd:
+	mlx5_core_dealloc_pd(fdev->mdev, fdev->conn_res.pdn);
+err_uar:
+	mlx5_put_uars_page(fdev->mdev, fdev->conn_res.uar);
+err_roce:
+	mlx5_nic_vport_disable_roce(fdev->mdev);
+out:
+	return err;
+}
+
+void mlx5_fpga_conn_device_cleanup(struct mlx5_fpga_device *fdev)
+{
+	mlx5_core_destroy_mkey(fdev->mdev, &fdev->conn_res.mkey);
+	mlx5_core_dealloc_pd(fdev->mdev, fdev->conn_res.pdn);
+	mlx5_put_uars_page(fdev->mdev, fdev->conn_res.uar);
+	mlx5_nic_vport_disable_roce(fdev->mdev);
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fpga/conn.h b/drivers/net/ethernet/mellanox/mlx5/core/fpga/conn.h
new file mode 100644
index 000000000000..44bd9eccc711
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/conn.h
@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#ifndef __MLX5_FPGA_CONN_H__
+#define __MLX5_FPGA_CONN_H__
+
+#include <linux/mlx5/cq.h>
+#include <linux/mlx5/qp.h>
+
+#include "fpga/core.h"
+#include "fpga/sdk.h"
+#include "wq.h"
+
+struct mlx5_fpga_conn {
+	struct mlx5_fpga_device *fdev;
+
+	void (*recv_cb)(void *cb_arg, struct mlx5_fpga_dma_buf *buf);
+	void *cb_arg;
+
+	/* FPGA QP */
+	u32 fpga_qpc[MLX5_ST_SZ_DW(fpga_qpc)];
+	u32 fpga_qpn;
+
+	/* CQ */
+	struct {
+		struct mlx5_cqwq wq;
+		struct mlx5_frag_wq_ctrl wq_ctrl;
+		struct mlx5_core_cq mcq;
+		struct tasklet_struct tasklet;
+	} cq;
+
+	/* QP */
+	struct {
+		bool active;
+		int sgid_index;
+		struct mlx5_wq_qp wq;
+		struct mlx5_wq_ctrl wq_ctrl;
+		struct mlx5_core_qp mqp;
+		struct {
+			spinlock_t lock; /* Protects all SQ state */
+			unsigned int pc;
+			unsigned int cc;
+			unsigned int size;
+			struct mlx5_fpga_dma_buf **bufs;
+			struct list_head backlog;
+		} sq;
+		struct {
+			unsigned int pc;
+			unsigned int cc;
+			unsigned int size;
+			struct mlx5_fpga_dma_buf **bufs;
+		} rq;
+	} qp;
+};
+
+int mlx5_fpga_conn_device_init(struct mlx5_fpga_device *fdev);
+void mlx5_fpga_conn_device_cleanup(struct mlx5_fpga_device *fdev);
+struct mlx5_fpga_conn *
+mlx5_fpga_conn_create(struct mlx5_fpga_device *fdev,
+		      struct mlx5_fpga_conn_attr *attr,
+		      enum mlx5_ifc_fpga_qp_type qp_type);
+void mlx5_fpga_conn_destroy(struct mlx5_fpga_conn *conn);
+int mlx5_fpga_conn_send(struct mlx5_fpga_conn *conn,
+			struct mlx5_fpga_dma_buf *buf);
+
+#endif /* __MLX5_FPGA_CONN_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fpga/core.c b/drivers/net/ethernet/mellanox/mlx5/core/fpga/core.c
new file mode 100644
index 000000000000..31e5a2627eb8
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/core.c
@@ -0,0 +1,283 @@
+/*
+ * Copyright (c) 2017, Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/module.h>
+#include <linux/etherdevice.h>
+#include <linux/mlx5/driver.h>
+
+#include "mlx5_core.h"
+#include "lib/mlx5.h"
+#include "fpga/core.h"
+#include "fpga/conn.h"
+
+static const char *const mlx5_fpga_error_strings[] = {
+	"Null Syndrome",
+	"Corrupted DDR",
+	"Flash Timeout",
+	"Internal Link Error",
+	"Watchdog HW Failure",
+	"I2C Failure",
+	"Image Changed",
+	"Temperature Critical",
+};
+
+static struct mlx5_fpga_device *mlx5_fpga_device_alloc(void)
+{
+	struct mlx5_fpga_device *fdev = NULL;
+
+	fdev = kzalloc(sizeof(*fdev), GFP_KERNEL);
+	if (!fdev)
+		return NULL;
+
+	spin_lock_init(&fdev->state_lock);
+	fdev->state = MLX5_FPGA_STATUS_NONE;
+	return fdev;
+}
+
+static const char *mlx5_fpga_image_name(enum mlx5_fpga_image image)
+{
+	switch (image) {
+	case MLX5_FPGA_IMAGE_USER:
+		return "user";
+	case MLX5_FPGA_IMAGE_FACTORY:
+		return "factory";
+	default:
+		return "unknown";
+	}
+}
+
+static int mlx5_fpga_device_load_check(struct mlx5_fpga_device *fdev)
+{
+	struct mlx5_fpga_query query;
+	int err;
+
+	err = mlx5_fpga_query(fdev->mdev, &query);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to query status: %d\n", err);
+		return err;
+	}
+
+	fdev->last_admin_image = query.admin_image;
+	fdev->last_oper_image = query.oper_image;
+
+	mlx5_fpga_dbg(fdev, "Status %u; Admin image %u; Oper image %u\n",
+		      query.status, query.admin_image, query.oper_image);
+
+	if (query.status != MLX5_FPGA_STATUS_SUCCESS) {
+		mlx5_fpga_err(fdev, "%s image failed to load; status %u\n",
+			      mlx5_fpga_image_name(fdev->last_oper_image),
+			      query.status);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+int mlx5_fpga_device_brb(struct mlx5_fpga_device *fdev)
+{
+	int err;
+	struct mlx5_core_dev *mdev = fdev->mdev;
+
+	err = mlx5_fpga_ctrl_op(mdev, MLX5_FPGA_CTRL_OPERATION_SANDBOX_BYPASS_ON);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to set bypass on: %d\n", err);
+		return err;
+	}
+	err = mlx5_fpga_ctrl_op(mdev, MLX5_FPGA_CTRL_OPERATION_RESET_SANDBOX);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to reset SBU: %d\n", err);
+		return err;
+	}
+	err = mlx5_fpga_ctrl_op(mdev, MLX5_FPGA_CTRL_OPERATION_SANDBOX_BYPASS_OFF);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to set bypass off: %d\n", err);
+		return err;
+	}
+	return 0;
+}
+
+int mlx5_fpga_device_start(struct mlx5_core_dev *mdev)
+{
+	struct mlx5_fpga_device *fdev = mdev->fpga;
+	unsigned long flags;
+	unsigned int max_num_qps;
+	int err;
+
+	if (!fdev)
+		return 0;
+
+	err = mlx5_fpga_device_load_check(fdev);
+	if (err)
+		goto out;
+
+	err = mlx5_fpga_caps(fdev->mdev,
+			     fdev->mdev->caps.hca_cur[MLX5_CAP_FPGA]);
+	if (err)
+		goto out;
+
+	mlx5_fpga_info(fdev, "device %u; %s image, version %u\n",
+		       MLX5_CAP_FPGA(fdev->mdev, fpga_device),
+		       mlx5_fpga_image_name(fdev->last_oper_image),
+		       MLX5_CAP_FPGA(fdev->mdev, image_version));
+
+	max_num_qps = MLX5_CAP_FPGA(mdev, shell_caps.max_num_qps);
+	err = mlx5_core_reserve_gids(mdev, max_num_qps);
+	if (err)
+		goto out;
+
+	err = mlx5_fpga_conn_device_init(fdev);
+	if (err)
+		goto err_rsvd_gid;
+
+	if (fdev->last_oper_image == MLX5_FPGA_IMAGE_USER) {
+		err = mlx5_fpga_device_brb(fdev);
+		if (err)
+			goto err_conn_init;
+	}
+
+	goto out;
+
+err_conn_init:
+	mlx5_fpga_conn_device_cleanup(fdev);
+
+err_rsvd_gid:
+	mlx5_core_unreserve_gids(mdev, max_num_qps);
+out:
+	spin_lock_irqsave(&fdev->state_lock, flags);
+	fdev->state = err ? MLX5_FPGA_STATUS_FAILURE : MLX5_FPGA_STATUS_SUCCESS;
+	spin_unlock_irqrestore(&fdev->state_lock, flags);
+	return err;
+}
+
+int mlx5_fpga_init(struct mlx5_core_dev *mdev)
+{
+	struct mlx5_fpga_device *fdev = NULL;
+
+	if (!MLX5_CAP_GEN(mdev, fpga)) {
+		mlx5_core_dbg(mdev, "FPGA capability not present\n");
+		return 0;
+	}
+
+	mlx5_core_dbg(mdev, "Initializing FPGA\n");
+
+	fdev = mlx5_fpga_device_alloc();
+	if (!fdev)
+		return -ENOMEM;
+
+	fdev->mdev = mdev;
+	mdev->fpga = fdev;
+
+	return 0;
+}
+
+void mlx5_fpga_device_stop(struct mlx5_core_dev *mdev)
+{
+	struct mlx5_fpga_device *fdev = mdev->fpga;
+	unsigned int max_num_qps;
+	unsigned long flags;
+	int err;
+
+	if (!fdev)
+		return;
+
+	spin_lock_irqsave(&fdev->state_lock, flags);
+	if (fdev->state != MLX5_FPGA_STATUS_SUCCESS) {
+		spin_unlock_irqrestore(&fdev->state_lock, flags);
+		return;
+	}
+	fdev->state = MLX5_FPGA_STATUS_NONE;
+	spin_unlock_irqrestore(&fdev->state_lock, flags);
+
+	if (fdev->last_oper_image == MLX5_FPGA_IMAGE_USER) {
+		err = mlx5_fpga_ctrl_op(mdev, MLX5_FPGA_CTRL_OPERATION_SANDBOX_BYPASS_ON);
+		if (err)
+			mlx5_fpga_err(fdev, "Failed to re-set SBU bypass on: %d\n",
+				      err);
+	}
+
+	mlx5_fpga_conn_device_cleanup(fdev);
+	max_num_qps = MLX5_CAP_FPGA(mdev, shell_caps.max_num_qps);
+	mlx5_core_unreserve_gids(mdev, max_num_qps);
+}
+
+void mlx5_fpga_cleanup(struct mlx5_core_dev *mdev)
+{
+	struct mlx5_fpga_device *fdev = mdev->fpga;
+
+	mlx5_fpga_device_stop(mdev);
+	kfree(fdev);
+	mdev->fpga = NULL;
+}
+
+static const char *mlx5_fpga_syndrome_to_string(u8 syndrome)
+{
+	if (syndrome < ARRAY_SIZE(mlx5_fpga_error_strings))
+		return mlx5_fpga_error_strings[syndrome];
+	return "Unknown";
+}
+
+void mlx5_fpga_event(struct mlx5_core_dev *mdev, u8 event, void *data)
+{
+	struct mlx5_fpga_device *fdev = mdev->fpga;
+	const char *event_name;
+	bool teardown = false;
+	unsigned long flags;
+	u8 syndrome;
+
+	if (event != MLX5_EVENT_TYPE_FPGA_ERROR) {
+		mlx5_fpga_warn_ratelimited(fdev, "Unexpected event %u\n",
+					   event);
+		return;
+	}
+
+	syndrome = MLX5_GET(fpga_error_event, data, syndrome);
+	event_name = mlx5_fpga_syndrome_to_string(syndrome);
+
+	spin_lock_irqsave(&fdev->state_lock, flags);
+	switch (fdev->state) {
+	case MLX5_FPGA_STATUS_SUCCESS:
+		mlx5_fpga_warn(fdev, "Error %u: %s\n", syndrome, event_name);
+		teardown = true;
+		break;
+	default:
+		mlx5_fpga_warn_ratelimited(fdev, "Unexpected error event %u: %s\n",
+					   syndrome, event_name);
+	}
+	spin_unlock_irqrestore(&fdev->state_lock, flags);
+	/* We tear-down the card's interfaces and functionality because
+	 * the FPGA bump-on-the-wire is misbehaving and we lose ability
+	 * to communicate with the network. User may still be able to
+	 * recover by re-programming or debugging the FPGA
+	 */
+	if (teardown)
+		mlx5_trigger_health_work(fdev->mdev);
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fpga/core.h b/drivers/net/ethernet/mellanox/mlx5/core/fpga/core.h
new file mode 100644
index 000000000000..82405ed84725
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/core.h
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2017, Mellanox Technologies, Ltd.  All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef __MLX5_FPGA_CORE_H__
+#define __MLX5_FPGA_CORE_H__
+
+#ifdef CONFIG_MLX5_FPGA
+
+#include "fpga/cmd.h"
+
+/* Represents an Innova device */
+struct mlx5_fpga_device {
+	struct mlx5_core_dev *mdev;
+	spinlock_t state_lock; /* Protects state transitions */
+	enum mlx5_fpga_status state;
+	enum mlx5_fpga_image last_admin_image;
+	enum mlx5_fpga_image last_oper_image;
+
+	/* QP Connection resources */
+	struct {
+		u32 pdn;
+		struct mlx5_core_mkey mkey;
+		struct mlx5_uars_page *uar;
+	} conn_res;
+
+	struct mlx5_fpga_ipsec *ipsec;
+};
+
+#define mlx5_fpga_dbg(__adev, format, ...) \
+	dev_dbg(&(__adev)->mdev->pdev->dev, "FPGA: %s:%d:(pid %d): " format, \
+		 __func__, __LINE__, current->pid, ##__VA_ARGS__)
+
+#define mlx5_fpga_err(__adev, format, ...) \
+	dev_err(&(__adev)->mdev->pdev->dev, "FPGA: %s:%d:(pid %d): " format, \
+		__func__, __LINE__, current->pid, ##__VA_ARGS__)
+
+#define mlx5_fpga_warn(__adev, format, ...) \
+	dev_warn(&(__adev)->mdev->pdev->dev, "FPGA: %s:%d:(pid %d): " format, \
+		__func__, __LINE__, current->pid, ##__VA_ARGS__)
+
+#define mlx5_fpga_warn_ratelimited(__adev, format, ...) \
+	dev_warn_ratelimited(&(__adev)->mdev->pdev->dev, "FPGA: %s:%d: " \
+		format, __func__, __LINE__, ##__VA_ARGS__)
+
+#define mlx5_fpga_notice(__adev, format, ...) \
+	dev_notice(&(__adev)->mdev->pdev->dev, "FPGA: " format, ##__VA_ARGS__)
+
+#define mlx5_fpga_info(__adev, format, ...) \
+	dev_info(&(__adev)->mdev->pdev->dev, "FPGA: " format, ##__VA_ARGS__)
+
+int mlx5_fpga_init(struct mlx5_core_dev *mdev);
+void mlx5_fpga_cleanup(struct mlx5_core_dev *mdev);
+int mlx5_fpga_device_start(struct mlx5_core_dev *mdev);
+void mlx5_fpga_device_stop(struct mlx5_core_dev *mdev);
+void mlx5_fpga_event(struct mlx5_core_dev *mdev, u8 event, void *data);
+
+#else
+
+static inline int mlx5_fpga_init(struct mlx5_core_dev *mdev)
+{
+	return 0;
+}
+
+static inline void mlx5_fpga_cleanup(struct mlx5_core_dev *mdev)
+{
+}
+
+static inline int mlx5_fpga_device_start(struct mlx5_core_dev *mdev)
+{
+	return 0;
+}
+
+static inline void mlx5_fpga_device_stop(struct mlx5_core_dev *mdev)
+{
+}
+
+static inline void mlx5_fpga_event(struct mlx5_core_dev *mdev, u8 event,
+				   void *data)
+{
+}
+
+#endif
+
+#endif /* __MLX5_FPGA_CORE_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fpga/ipsec.c b/drivers/net/ethernet/mellanox/mlx5/core/fpga/ipsec.c
new file mode 100644
index 000000000000..42970e2a05ff
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/ipsec.c
@@ -0,0 +1,376 @@
+/*
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#include <linux/mlx5/driver.h>
+
+#include "mlx5_core.h"
+#include "fpga/ipsec.h"
+#include "fpga/sdk.h"
+#include "fpga/core.h"
+
+#define SBU_QP_QUEUE_SIZE 8
+
+enum mlx5_ipsec_response_syndrome {
+	MLX5_IPSEC_RESPONSE_SUCCESS = 0,
+	MLX5_IPSEC_RESPONSE_ILLEGAL_REQUEST = 1,
+	MLX5_IPSEC_RESPONSE_SADB_ISSUE = 2,
+	MLX5_IPSEC_RESPONSE_WRITE_RESPONSE_ISSUE = 3,
+};
+
+enum mlx5_fpga_ipsec_sacmd_status {
+	MLX5_FPGA_IPSEC_SACMD_PENDING,
+	MLX5_FPGA_IPSEC_SACMD_SEND_FAIL,
+	MLX5_FPGA_IPSEC_SACMD_COMPLETE,
+};
+
+struct mlx5_ipsec_command_context {
+	struct mlx5_fpga_dma_buf buf;
+	struct mlx5_accel_ipsec_sa sa;
+	enum mlx5_fpga_ipsec_sacmd_status status;
+	int status_code;
+	struct completion complete;
+	struct mlx5_fpga_device *dev;
+	struct list_head list; /* Item in pending_cmds */
+};
+
+struct mlx5_ipsec_sadb_resp {
+	__be32 syndrome;
+	__be32 sw_sa_handle;
+	u8 reserved[24];
+} __packed;
+
+struct mlx5_fpga_ipsec {
+	struct list_head pending_cmds;
+	spinlock_t pending_cmds_lock; /* Protects pending_cmds */
+	u32 caps[MLX5_ST_SZ_DW(ipsec_extended_cap)];
+	struct mlx5_fpga_conn *conn;
+};
+
+static bool mlx5_fpga_is_ipsec_device(struct mlx5_core_dev *mdev)
+{
+	if (!mdev->fpga || !MLX5_CAP_GEN(mdev, fpga))
+		return false;
+
+	if (MLX5_CAP_FPGA(mdev, ieee_vendor_id) !=
+	    MLX5_FPGA_CAP_SANDBOX_VENDOR_ID_MLNX)
+		return false;
+
+	if (MLX5_CAP_FPGA(mdev, sandbox_product_id) !=
+	    MLX5_FPGA_CAP_SANDBOX_PRODUCT_ID_IPSEC)
+		return false;
+
+	return true;
+}
+
+static void mlx5_fpga_ipsec_send_complete(struct mlx5_fpga_conn *conn,
+					  struct mlx5_fpga_device *fdev,
+					  struct mlx5_fpga_dma_buf *buf,
+					  u8 status)
+{
+	struct mlx5_ipsec_command_context *context;
+
+	if (status) {
+		context = container_of(buf, struct mlx5_ipsec_command_context,
+				       buf);
+		mlx5_fpga_warn(fdev, "IPSec command send failed with status %u\n",
+			       status);
+		context->status = MLX5_FPGA_IPSEC_SACMD_SEND_FAIL;
+		complete(&context->complete);
+	}
+}
+
+static inline int syndrome_to_errno(enum mlx5_ipsec_response_syndrome syndrome)
+{
+	switch (syndrome) {
+	case MLX5_IPSEC_RESPONSE_SUCCESS:
+		return 0;
+	case MLX5_IPSEC_RESPONSE_SADB_ISSUE:
+		return -EEXIST;
+	case MLX5_IPSEC_RESPONSE_ILLEGAL_REQUEST:
+		return -EINVAL;
+	case MLX5_IPSEC_RESPONSE_WRITE_RESPONSE_ISSUE:
+		return -EIO;
+	}
+	return -EIO;
+}
+
+static void mlx5_fpga_ipsec_recv(void *cb_arg, struct mlx5_fpga_dma_buf *buf)
+{
+	struct mlx5_ipsec_sadb_resp *resp = buf->sg[0].data;
+	struct mlx5_ipsec_command_context *context;
+	enum mlx5_ipsec_response_syndrome syndrome;
+	struct mlx5_fpga_device *fdev = cb_arg;
+	unsigned long flags;
+
+	if (buf->sg[0].size < sizeof(*resp)) {
+		mlx5_fpga_warn(fdev, "Short receive from FPGA IPSec: %u < %zu bytes\n",
+			       buf->sg[0].size, sizeof(*resp));
+		return;
+	}
+
+	mlx5_fpga_dbg(fdev, "mlx5_ipsec recv_cb syndrome %08x sa_id %x\n",
+		      ntohl(resp->syndrome), ntohl(resp->sw_sa_handle));
+
+	spin_lock_irqsave(&fdev->ipsec->pending_cmds_lock, flags);
+	context = list_first_entry_or_null(&fdev->ipsec->pending_cmds,
+					   struct mlx5_ipsec_command_context,
+					   list);
+	if (context)
+		list_del(&context->list);
+	spin_unlock_irqrestore(&fdev->ipsec->pending_cmds_lock, flags);
+
+	if (!context) {
+		mlx5_fpga_warn(fdev, "Received IPSec offload response without pending command request\n");
+		return;
+	}
+	mlx5_fpga_dbg(fdev, "Handling response for %p\n", context);
+
+	if (context->sa.sw_sa_handle != resp->sw_sa_handle) {
+		mlx5_fpga_err(fdev, "mismatch SA handle. cmd 0x%08x vs resp 0x%08x\n",
+			      ntohl(context->sa.sw_sa_handle),
+			      ntohl(resp->sw_sa_handle));
+		return;
+	}
+
+	syndrome = ntohl(resp->syndrome);
+	context->status_code = syndrome_to_errno(syndrome);
+	context->status = MLX5_FPGA_IPSEC_SACMD_COMPLETE;
+
+	if (context->status_code)
+		mlx5_fpga_warn(fdev, "IPSec SADB command failed with syndrome %08x\n",
+			       syndrome);
+	complete(&context->complete);
+}
+
+void *mlx5_fpga_ipsec_sa_cmd_exec(struct mlx5_core_dev *mdev,
+				  struct mlx5_accel_ipsec_sa *cmd)
+{
+	struct mlx5_ipsec_command_context *context;
+	struct mlx5_fpga_device *fdev = mdev->fpga;
+	unsigned long flags;
+	int res = 0;
+
+	BUILD_BUG_ON((sizeof(struct mlx5_accel_ipsec_sa) & 3) != 0);
+	if (!fdev || !fdev->ipsec)
+		return ERR_PTR(-EOPNOTSUPP);
+
+	context = kzalloc(sizeof(*context), GFP_ATOMIC);
+	if (!context)
+		return ERR_PTR(-ENOMEM);
+
+	memcpy(&context->sa, cmd, sizeof(*cmd));
+	context->buf.complete = mlx5_fpga_ipsec_send_complete;
+	context->buf.sg[0].size = sizeof(context->sa);
+	context->buf.sg[0].data = &context->sa;
+	init_completion(&context->complete);
+	context->dev = fdev;
+	spin_lock_irqsave(&fdev->ipsec->pending_cmds_lock, flags);
+	list_add_tail(&context->list, &fdev->ipsec->pending_cmds);
+	spin_unlock_irqrestore(&fdev->ipsec->pending_cmds_lock, flags);
+
+	context->status = MLX5_FPGA_IPSEC_SACMD_PENDING;
+
+	res = mlx5_fpga_sbu_conn_sendmsg(fdev->ipsec->conn, &context->buf);
+	if (res) {
+		mlx5_fpga_warn(fdev, "Failure sending IPSec command: %d\n",
+			       res);
+		spin_lock_irqsave(&fdev->ipsec->pending_cmds_lock, flags);
+		list_del(&context->list);
+		spin_unlock_irqrestore(&fdev->ipsec->pending_cmds_lock, flags);
+		kfree(context);
+		return ERR_PTR(res);
+	}
+	/* Context will be freed by wait func after completion */
+	return context;
+}
+
+int mlx5_fpga_ipsec_sa_cmd_wait(void *ctx)
+{
+	struct mlx5_ipsec_command_context *context = ctx;
+	int res;
+
+	res = wait_for_completion_killable(&context->complete);
+	if (res) {
+		mlx5_fpga_warn(context->dev, "Failure waiting for IPSec command response\n");
+		return -EINTR;
+	}
+
+	if (context->status == MLX5_FPGA_IPSEC_SACMD_COMPLETE)
+		res = context->status_code;
+	else
+		res = -EIO;
+
+	kfree(context);
+	return res;
+}
+
+u32 mlx5_fpga_ipsec_device_caps(struct mlx5_core_dev *mdev)
+{
+	struct mlx5_fpga_device *fdev = mdev->fpga;
+	u32 ret = 0;
+
+	if (mlx5_fpga_is_ipsec_device(mdev))
+		ret |= MLX5_ACCEL_IPSEC_DEVICE;
+	else
+		return ret;
+
+	if (!fdev->ipsec)
+		return ret;
+
+	if (MLX5_GET(ipsec_extended_cap, fdev->ipsec->caps, esp))
+		ret |= MLX5_ACCEL_IPSEC_ESP;
+
+	if (MLX5_GET(ipsec_extended_cap, fdev->ipsec->caps, ipv6))
+		ret |= MLX5_ACCEL_IPSEC_IPV6;
+
+	if (MLX5_GET(ipsec_extended_cap, fdev->ipsec->caps, lso))
+		ret |= MLX5_ACCEL_IPSEC_LSO;
+
+	return ret;
+}
+
+unsigned int mlx5_fpga_ipsec_counters_count(struct mlx5_core_dev *mdev)
+{
+	struct mlx5_fpga_device *fdev = mdev->fpga;
+
+	if (!fdev || !fdev->ipsec)
+		return 0;
+
+	return MLX5_GET(ipsec_extended_cap, fdev->ipsec->caps,
+			number_of_ipsec_counters);
+}
+
+int mlx5_fpga_ipsec_counters_read(struct mlx5_core_dev *mdev, u64 *counters,
+				  unsigned int counters_count)
+{
+	struct mlx5_fpga_device *fdev = mdev->fpga;
+	unsigned int i;
+	u32 *data;
+	u32 count;
+	u64 addr;
+	int ret;
+
+	if (!fdev || !fdev->ipsec)
+		return 0;
+
+	addr = (u64)MLX5_GET(ipsec_extended_cap, fdev->ipsec->caps,
+			     ipsec_counters_addr_low) +
+	       ((u64)MLX5_GET(ipsec_extended_cap, fdev->ipsec->caps,
+			     ipsec_counters_addr_high) << 32);
+
+	count = mlx5_fpga_ipsec_counters_count(mdev);
+
+	data = kzalloc(sizeof(u32) * count * 2, GFP_KERNEL);
+	if (!data) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = mlx5_fpga_mem_read(fdev, count * sizeof(u64), addr, data,
+				 MLX5_FPGA_ACCESS_TYPE_DONTCARE);
+	if (ret < 0) {
+		mlx5_fpga_err(fdev, "Failed to read IPSec counters from HW: %d\n",
+			      ret);
+		goto out;
+	}
+	ret = 0;
+
+	if (count > counters_count)
+		count = counters_count;
+
+	/* Each counter is low word, then high. But each word is big-endian */
+	for (i = 0; i < count; i++)
+		counters[i] = (u64)ntohl(data[i * 2]) |
+			      ((u64)ntohl(data[i * 2 + 1]) << 32);
+
+out:
+	kfree(data);
+	return ret;
+}
+
+int mlx5_fpga_ipsec_init(struct mlx5_core_dev *mdev)
+{
+	struct mlx5_fpga_conn_attr init_attr = {0};
+	struct mlx5_fpga_device *fdev = mdev->fpga;
+	struct mlx5_fpga_conn *conn;
+	int err;
+
+	if (!mlx5_fpga_is_ipsec_device(mdev))
+		return 0;
+
+	fdev->ipsec = kzalloc(sizeof(*fdev->ipsec), GFP_KERNEL);
+	if (!fdev->ipsec)
+		return -ENOMEM;
+
+	err = mlx5_fpga_get_sbu_caps(fdev, sizeof(fdev->ipsec->caps),
+				     fdev->ipsec->caps);
+	if (err) {
+		mlx5_fpga_err(fdev, "Failed to retrieve IPSec extended capabilities: %d\n",
+			      err);
+		goto error;
+	}
+
+	INIT_LIST_HEAD(&fdev->ipsec->pending_cmds);
+	spin_lock_init(&fdev->ipsec->pending_cmds_lock);
+
+	init_attr.rx_size = SBU_QP_QUEUE_SIZE;
+	init_attr.tx_size = SBU_QP_QUEUE_SIZE;
+	init_attr.recv_cb = mlx5_fpga_ipsec_recv;
+	init_attr.cb_arg = fdev;
+	conn = mlx5_fpga_sbu_conn_create(fdev, &init_attr);
+	if (IS_ERR(conn)) {
+		err = PTR_ERR(conn);
+		mlx5_fpga_err(fdev, "Error creating IPSec command connection %d\n",
+			      err);
+		goto error;
+	}
+	fdev->ipsec->conn = conn;
+	return 0;
+
+error:
+	kfree(fdev->ipsec);
+	fdev->ipsec = NULL;
+	return err;
+}
+
+void mlx5_fpga_ipsec_cleanup(struct mlx5_core_dev *mdev)
+{
+	struct mlx5_fpga_device *fdev = mdev->fpga;
+
+	if (!mlx5_fpga_is_ipsec_device(mdev))
+		return;
+
+	mlx5_fpga_sbu_conn_destroy(fdev->ipsec->conn);
+	kfree(fdev->ipsec);
+	fdev->ipsec = NULL;
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fpga/ipsec.h b/drivers/net/ethernet/mellanox/mlx5/core/fpga/ipsec.h
new file mode 100644
index 000000000000..26a3e4b56972
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/ipsec.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#ifndef __MLX5_FPGA_IPSEC_H__
+#define __MLX5_FPGA_IPSEC_H__
+
+#include "accel/ipsec.h"
+
+#ifdef CONFIG_MLX5_FPGA
+
+void *mlx5_fpga_ipsec_sa_cmd_exec(struct mlx5_core_dev *mdev,
+				  struct mlx5_accel_ipsec_sa *cmd);
+int mlx5_fpga_ipsec_sa_cmd_wait(void *context);
+
+u32 mlx5_fpga_ipsec_device_caps(struct mlx5_core_dev *mdev);
+unsigned int mlx5_fpga_ipsec_counters_count(struct mlx5_core_dev *mdev);
+int mlx5_fpga_ipsec_counters_read(struct mlx5_core_dev *mdev, u64 *counters,
+				  unsigned int counters_count);
+
+int mlx5_fpga_ipsec_init(struct mlx5_core_dev *mdev);
+void mlx5_fpga_ipsec_cleanup(struct mlx5_core_dev *mdev);
+
+#else
+
+static inline void *mlx5_fpga_ipsec_sa_cmd_exec(struct mlx5_core_dev *mdev,
+						struct mlx5_accel_ipsec_sa *cmd)
+{
+	return ERR_PTR(-EOPNOTSUPP);
+}
+
+static inline int mlx5_fpga_ipsec_sa_cmd_wait(void *context)
+{
+	return -EOPNOTSUPP;
+}
+
+static inline u32 mlx5_fpga_ipsec_device_caps(struct mlx5_core_dev *mdev)
+{
+	return 0;
+}
+
+static inline unsigned int
+mlx5_fpga_ipsec_counters_count(struct mlx5_core_dev *mdev)
+{
+	return 0;
+}
+
+static inline int mlx5_fpga_ipsec_counters_read(struct mlx5_core_dev *mdev,
+						u64 *counters)
+{
+	return 0;
+}
+
+static inline int mlx5_fpga_ipsec_init(struct mlx5_core_dev *mdev)
+{
+	return 0;
+}
+
+static inline void mlx5_fpga_ipsec_cleanup(struct mlx5_core_dev *mdev)
+{
+}
+
+#endif /* CONFIG_MLX5_FPGA */
+
+#endif	/* __MLX5_FPGA_SADB_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fpga/sdk.c b/drivers/net/ethernet/mellanox/mlx5/core/fpga/sdk.c
new file mode 100644
index 000000000000..3c11d6e2160a
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/sdk.c
@@ -0,0 +1,164 @@
+/*
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#include <linux/mlx5/device.h>
+
+#include "fpga/core.h"
+#include "fpga/conn.h"
+#include "fpga/sdk.h"
+
+struct mlx5_fpga_conn *
+mlx5_fpga_sbu_conn_create(struct mlx5_fpga_device *fdev,
+			  struct mlx5_fpga_conn_attr *attr)
+{
+	return mlx5_fpga_conn_create(fdev, attr, MLX5_FPGA_QPC_QP_TYPE_SANDBOX_QP);
+}
+EXPORT_SYMBOL(mlx5_fpga_sbu_conn_create);
+
+void mlx5_fpga_sbu_conn_destroy(struct mlx5_fpga_conn *conn)
+{
+	mlx5_fpga_conn_destroy(conn);
+}
+EXPORT_SYMBOL(mlx5_fpga_sbu_conn_destroy);
+
+int mlx5_fpga_sbu_conn_sendmsg(struct mlx5_fpga_conn *conn,
+			       struct mlx5_fpga_dma_buf *buf)
+{
+	return mlx5_fpga_conn_send(conn, buf);
+}
+EXPORT_SYMBOL(mlx5_fpga_sbu_conn_sendmsg);
+
+static int mlx5_fpga_mem_read_i2c(struct mlx5_fpga_device *fdev, size_t size,
+				  u64 addr, u8 *buf)
+{
+	size_t max_size = MLX5_FPGA_ACCESS_REG_SIZE_MAX;
+	size_t bytes_done = 0;
+	u8 actual_size;
+	int err;
+
+	if (!fdev->mdev)
+		return -ENOTCONN;
+
+	while (bytes_done < size) {
+		actual_size = min(max_size, (size - bytes_done));
+
+		err = mlx5_fpga_access_reg(fdev->mdev, actual_size,
+					   addr + bytes_done,
+					   buf + bytes_done, false);
+		if (err) {
+			mlx5_fpga_err(fdev, "Failed to read over I2C: %d\n",
+				      err);
+			break;
+		}
+
+		bytes_done += actual_size;
+	}
+
+	return err;
+}
+
+static int mlx5_fpga_mem_write_i2c(struct mlx5_fpga_device *fdev, size_t size,
+				   u64 addr, u8 *buf)
+{
+	size_t max_size = MLX5_FPGA_ACCESS_REG_SIZE_MAX;
+	size_t bytes_done = 0;
+	u8 actual_size;
+	int err;
+
+	if (!fdev->mdev)
+		return -ENOTCONN;
+
+	while (bytes_done < size) {
+		actual_size = min(max_size, (size - bytes_done));
+
+		err = mlx5_fpga_access_reg(fdev->mdev, actual_size,
+					   addr + bytes_done,
+					   buf + bytes_done, true);
+		if (err) {
+			mlx5_fpga_err(fdev, "Failed to write FPGA crspace\n");
+			break;
+		}
+
+		bytes_done += actual_size;
+	}
+
+	return err;
+}
+
+int mlx5_fpga_mem_read(struct mlx5_fpga_device *fdev, size_t size, u64 addr,
+		       void *buf, enum mlx5_fpga_access_type access_type)
+{
+	int ret;
+
+	switch (access_type) {
+	case MLX5_FPGA_ACCESS_TYPE_I2C:
+		ret = mlx5_fpga_mem_read_i2c(fdev, size, addr, buf);
+		if (ret)
+			return ret;
+		break;
+	default:
+		mlx5_fpga_warn(fdev, "Unexpected read access_type %u\n",
+			       access_type);
+		return -EACCES;
+	}
+
+	return size;
+}
+EXPORT_SYMBOL(mlx5_fpga_mem_read);
+
+int mlx5_fpga_mem_write(struct mlx5_fpga_device *fdev, size_t size, u64 addr,
+			void *buf, enum mlx5_fpga_access_type access_type)
+{
+	int ret;
+
+	switch (access_type) {
+	case MLX5_FPGA_ACCESS_TYPE_I2C:
+		ret = mlx5_fpga_mem_write_i2c(fdev, size, addr, buf);
+		if (ret)
+			return ret;
+		break;
+	default:
+		mlx5_fpga_warn(fdev, "Unexpected write access_type %u\n",
+			       access_type);
+		return -EACCES;
+	}
+
+	return size;
+}
+EXPORT_SYMBOL(mlx5_fpga_mem_write);
+
+int mlx5_fpga_get_sbu_caps(struct mlx5_fpga_device *fdev, int size, void *buf)
+{
+	return mlx5_fpga_sbu_caps(fdev->mdev, buf, size);
+}
+EXPORT_SYMBOL(mlx5_fpga_get_sbu_caps);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fpga/sdk.h b/drivers/net/ethernet/mellanox/mlx5/core/fpga/sdk.h
new file mode 100644
index 000000000000..baa537e54a49
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/sdk.h
@@ -0,0 +1,204 @@
+/*
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#ifndef MLX5_FPGA_SDK_H
+#define MLX5_FPGA_SDK_H
+
+#include <linux/types.h>
+#include <linux/dma-direction.h>
+
+/**
+ * DOC: Innova SDK
+ * This header defines the in-kernel API for Innova FPGA client drivers.
+ */
+
+enum mlx5_fpga_access_type {
+	MLX5_FPGA_ACCESS_TYPE_I2C = 0x0,
+	MLX5_FPGA_ACCESS_TYPE_DONTCARE = 0x0,
+};
+
+struct mlx5_fpga_conn;
+struct mlx5_fpga_device;
+
+/**
+ * struct mlx5_fpga_dma_entry - A scatter-gather DMA entry
+ */
+struct mlx5_fpga_dma_entry {
+	/** @data: Virtual address pointer to the data */
+	void *data;
+	/** @size: Size in bytes of the data */
+	unsigned int size;
+	/** @dma_addr: Private member. Physical DMA-mapped address of the data */
+	dma_addr_t dma_addr;
+};
+
+/**
+ * struct mlx5_fpga_dma_buf - A packet buffer
+ * May contain up to 2 scatter-gather data entries
+ */
+struct mlx5_fpga_dma_buf {
+	/** @dma_dir: DMA direction */
+	enum dma_data_direction dma_dir;
+	/** @sg: Scatter-gather entries pointing to the data in memory */
+	struct mlx5_fpga_dma_entry sg[2];
+	/** @list: Item in SQ backlog, for TX packets */
+	struct list_head list;
+	/**
+	 * @complete: Completion routine, for TX packets
+	 * @conn: FPGA Connection this packet was sent to
+	 * @fdev: FPGA device this packet was sent to
+	 * @buf: The packet buffer
+	 * @status: 0 if successful, or an error code otherwise
+	 */
+	void (*complete)(struct mlx5_fpga_conn *conn,
+			 struct mlx5_fpga_device *fdev,
+			 struct mlx5_fpga_dma_buf *buf, u8 status);
+};
+
+/**
+ * struct mlx5_fpga_conn_attr - FPGA connection attributes
+ * Describes the attributes of a connection
+ */
+struct mlx5_fpga_conn_attr {
+	/** @tx_size: Size of connection TX queue, in packets */
+	unsigned int tx_size;
+	/** @rx_size: Size of connection RX queue, in packets */
+	unsigned int rx_size;
+	/**
+	 * @recv_cb: Callback function which is called for received packets
+	 * @cb_arg: The value provided in mlx5_fpga_conn_attr.cb_arg
+	 * @buf: A buffer containing a received packet
+	 *
+	 * buf is guaranteed to only contain a single scatter-gather entry.
+	 * The size of the actual packet received is specified in buf.sg[0].size
+	 * When this callback returns, the packet buffer may be re-used for
+	 * subsequent receives.
+	 */
+	void (*recv_cb)(void *cb_arg, struct mlx5_fpga_dma_buf *buf);
+	void *cb_arg;
+};
+
+/**
+ * mlx5_fpga_sbu_conn_create() - Initialize a new FPGA SBU connection
+ * @fdev: The FPGA device
+ * @attr: Attributes of the new connection
+ *
+ * Sets up a new FPGA SBU connection with the specified attributes.
+ * The receive callback function may be called for incoming messages even
+ * before this function returns.
+ *
+ * The caller must eventually destroy the connection by calling
+ * mlx5_fpga_sbu_conn_destroy.
+ *
+ * Return: A new connection, or ERR_PTR() error value otherwise.
+ */
+struct mlx5_fpga_conn *
+mlx5_fpga_sbu_conn_create(struct mlx5_fpga_device *fdev,
+			  struct mlx5_fpga_conn_attr *attr);
+
+/**
+ * mlx5_fpga_sbu_conn_destroy() - Destroy an FPGA SBU connection
+ * @conn: The FPGA SBU connection to destroy
+ *
+ * Cleans up an FPGA SBU connection which was previously created with
+ * mlx5_fpga_sbu_conn_create.
+ */
+void mlx5_fpga_sbu_conn_destroy(struct mlx5_fpga_conn *conn);
+
+/**
+ * mlx5_fpga_sbu_conn_sendmsg() - Queue the transmission of a packet
+ * @fdev: An FPGA SBU connection
+ * @buf: The packet buffer
+ *
+ * Queues a packet for transmission over an FPGA SBU connection.
+ * The buffer should not be modified or freed until completion.
+ * Upon completion, the buf's complete() callback is invoked, indicating the
+ * success or error status of the transmission.
+ *
+ * Return: 0 if successful, or an error value otherwise.
+ */
+int mlx5_fpga_sbu_conn_sendmsg(struct mlx5_fpga_conn *conn,
+			       struct mlx5_fpga_dma_buf *buf);
+
+/**
+ * mlx5_fpga_mem_read() - Read from FPGA memory address space
+ * @fdev: The FPGA device
+ * @size: Size of chunk to read, in bytes
+ * @addr: Starting address to read from, in FPGA address space
+ * @buf: Buffer to read into
+ * @access_type: Method for reading
+ *
+ * Reads from the specified address into the specified buffer.
+ * The address may point to configuration space or to DDR.
+ * Large reads may be performed internally as several non-atomic operations.
+ * This function may sleep, so should not be called from atomic contexts.
+ *
+ * Return: 0 if successful, or an error value otherwise.
+ */
+int mlx5_fpga_mem_read(struct mlx5_fpga_device *fdev, size_t size, u64 addr,
+		       void *buf, enum mlx5_fpga_access_type access_type);
+
+/**
+ * mlx5_fpga_mem_write() - Write to FPGA memory address space
+ * @fdev: The FPGA device
+ * @size: Size of chunk to write, in bytes
+ * @addr: Starting address to write to, in FPGA address space
+ * @buf: Buffer which contains data to write
+ * @access_type: Method for writing
+ *
+ * Writes the specified buffer data to FPGA memory at the specified address.
+ * The address may point to configuration space or to DDR.
+ * Large writes may be performed internally as several non-atomic operations.
+ * This function may sleep, so should not be called from atomic contexts.
+ *
+ * Return: 0 if successful, or an error value otherwise.
+ */
+int mlx5_fpga_mem_write(struct mlx5_fpga_device *fdev, size_t size, u64 addr,
+			void *buf, enum mlx5_fpga_access_type access_type);
+
+/**
+ * mlx5_fpga_get_sbu_caps() - Read the SBU capabilities
+ * @fdev: The FPGA device
+ * @size: Size of the buffer to read into
+ * @buf: Buffer to read the capabilities into
+ *
+ * Reads the FPGA SBU capabilities into the specified buffer.
+ * The format of the capabilities buffer is SBU-dependent.
+ *
+ * Return: 0 if successful
+ *         -EINVAL if the buffer is not large enough to contain SBU caps
+ *         or any other error value otherwise.
+ */
+int mlx5_fpga_get_sbu_caps(struct mlx5_fpga_device *fdev, int size, void *buf);
+
+#endif /* MLX5_FPGA_SDK_H */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c
index fcec7bedd3cd..e750f07793b8 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c
@@ -78,28 +78,33 @@ int mlx5_cmd_create_flow_table(struct mlx5_core_dev *dev,
 		 MLX5_CMD_OP_CREATE_FLOW_TABLE);
 
 	MLX5_SET(create_flow_table_in, in, table_type, type);
-	MLX5_SET(create_flow_table_in, in, level, level);
-	MLX5_SET(create_flow_table_in, in, log_size, log_size);
+	MLX5_SET(create_flow_table_in, in, flow_table_context.level, level);
+	MLX5_SET(create_flow_table_in, in, flow_table_context.log_size, log_size);
 	if (vport) {
 		MLX5_SET(create_flow_table_in, in, vport_number, vport);
 		MLX5_SET(create_flow_table_in, in, other_vport, 1);
 	}
 
-	MLX5_SET(create_flow_table_in, in, decap_en, en_encap_decap);
-	MLX5_SET(create_flow_table_in, in, encap_en, en_encap_decap);
+	MLX5_SET(create_flow_table_in, in, flow_table_context.decap_en,
+		 en_encap_decap);
+	MLX5_SET(create_flow_table_in, in, flow_table_context.encap_en,
+		 en_encap_decap);
 
 	switch (op_mod) {
 	case FS_FT_OP_MOD_NORMAL:
 		if (next_ft) {
-			MLX5_SET(create_flow_table_in, in, table_miss_mode, 1);
-			MLX5_SET(create_flow_table_in, in, table_miss_id, next_ft->id);
+			MLX5_SET(create_flow_table_in, in,
+				 flow_table_context.table_miss_action, 1);
+			MLX5_SET(create_flow_table_in, in,
+				 flow_table_context.table_miss_id, next_ft->id);
 		}
 		break;
 
 	case FS_FT_OP_MOD_LAG_DEMUX:
 		MLX5_SET(create_flow_table_in, in, op_mod, 0x1);
 		if (next_ft)
-			MLX5_SET(create_flow_table_in, in, lag_master_next_table_id,
+			MLX5_SET(create_flow_table_in, in,
+				 flow_table_context.lag_master_next_table_id,
 				 next_ft->id);
 		break;
 	}
@@ -146,10 +151,10 @@ int mlx5_cmd_modify_flow_table(struct mlx5_core_dev *dev,
 			 MLX5_MODIFY_FLOW_TABLE_LAG_NEXT_TABLE_ID);
 		if (next_ft) {
 			MLX5_SET(modify_flow_table_in, in,
-				 lag_master_next_table_id, next_ft->id);
+				 flow_table_context.lag_master_next_table_id, next_ft->id);
 		} else {
 			MLX5_SET(modify_flow_table_in, in,
-				 lag_master_next_table_id, 0);
+				 flow_table_context.lag_master_next_table_id, 0);
 		}
 	} else {
 		if (ft->vport) {
@@ -160,11 +165,14 @@ int mlx5_cmd_modify_flow_table(struct mlx5_core_dev *dev,
 		MLX5_SET(modify_flow_table_in, in, modify_field_select,
 			 MLX5_MODIFY_FLOW_TABLE_MISS_TABLE_ID);
 		if (next_ft) {
-			MLX5_SET(modify_flow_table_in, in, table_miss_mode, 1);
-			MLX5_SET(modify_flow_table_in, in, table_miss_id,
+			MLX5_SET(modify_flow_table_in, in,
+				 flow_table_context.table_miss_action, 1);
+			MLX5_SET(modify_flow_table_in, in,
+				 flow_table_context.table_miss_id,
 				 next_ft->id);
 		} else {
-			MLX5_SET(modify_flow_table_in, in, table_miss_mode, 0);
+			MLX5_SET(modify_flow_table_in, in,
+				 flow_table_context.table_miss_action, 0);
 		}
 	}
 
@@ -232,11 +240,9 @@ static int mlx5_cmd_set_fte(struct mlx5_core_dev *dev,
 	u32 *in;
 	int err;
 
-	in = mlx5_vzalloc(inlen);
-	if (!in) {
-		mlx5_core_warn(dev, "failed to allocate inbox\n");
+	in = kvzalloc(inlen, GFP_KERNEL);
+	if (!in)
 		return -ENOMEM;
-	}
 
 	MLX5_SET(set_fte_in, in, opcode, MLX5_CMD_OP_SET_FLOW_TABLE_ENTRY);
 	MLX5_SET(set_fte_in, in, op_mod, opmod);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c
index 0e487e8ca634..e8690fe46bf2 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c
@@ -104,6 +104,7 @@ struct node_caps {
 	size_t	arr_sz;
 	long	*caps;
 };
+
 static struct init_tree_node {
 	enum fs_node_type	type;
 	struct init_tree_node *children;
@@ -376,11 +377,9 @@ static void del_rule(struct fs_node *node)
 	int err;
 	bool update_fte = false;
 
-	match_value = mlx5_vzalloc(match_len);
-	if (!match_value) {
-		mlx5_core_warn(dev, "failed to allocate inbox\n");
+	match_value = kvzalloc(match_len, GFP_KERNEL);
+	if (!match_value)
 		return;
-	}
 
 	fs_get_obj(rule, node);
 	fs_get_obj(fte, rule->node.parent);
@@ -862,7 +861,7 @@ struct mlx5_flow_table *mlx5_create_vport_flow_table(struct mlx5_flow_namespace
 	ft_attr.level   = level;
 	ft_attr.prio    = prio;
 
-	return __mlx5_create_flow_table(ns, &ft_attr, FS_FT_OP_MOD_NORMAL, 0);
+	return __mlx5_create_flow_table(ns, &ft_attr, FS_FT_OP_MOD_NORMAL, vport);
 }
 
 struct mlx5_flow_table*
@@ -1157,7 +1156,7 @@ static struct mlx5_flow_group *create_autogroup(struct mlx5_flow_table *ft,
 	if (!ft->autogroup.active)
 		return ERR_PTR(-ENOENT);
 
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return ERR_PTR(-ENOMEM);
 
@@ -1777,7 +1776,7 @@ static struct mlx5_flow_root_namespace *create_root_ns(struct mlx5_flow_steering
 	struct mlx5_flow_namespace *ns;
 
 	/* Create the root namespace */
-	root_ns = mlx5_vzalloc(sizeof(*root_ns));
+	root_ns = kvzalloc(sizeof(*root_ns), GFP_KERNEL);
 	if (!root_ns)
 		return NULL;
 
@@ -1860,7 +1859,6 @@ static int create_anchor_flow_table(struct mlx5_flow_steering *steering)
 
 static int init_root_ns(struct mlx5_flow_steering *steering)
 {
-
 	steering->root_ns = create_root_ns(steering, FS_FT_NIC_RX);
 	if (!steering->root_ns)
 		goto cleanup;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fw.c b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
index 1bc14d0fded8..fa33d59ab485 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
@@ -34,6 +34,7 @@
 #include <linux/mlx5/cmd.h>
 #include <linux/module.h>
 #include "mlx5_core.h"
+#include "../../mlxfw/mlxfw.h"
 
 static int mlx5_cmd_query_adapter(struct mlx5_core_dev *dev, u32 *out,
 				  int outlen)
@@ -195,3 +196,298 @@ int mlx5_cmd_teardown_hca(struct mlx5_core_dev *dev)
 	MLX5_SET(teardown_hca_in, in, opcode, MLX5_CMD_OP_TEARDOWN_HCA);
 	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
+
+int mlx5_cmd_force_teardown_hca(struct mlx5_core_dev *dev)
+{
+	u32 out[MLX5_ST_SZ_DW(teardown_hca_out)] = {0};
+	u32 in[MLX5_ST_SZ_DW(teardown_hca_in)] = {0};
+	int force_state;
+	int ret;
+
+	if (!MLX5_CAP_GEN(dev, force_teardown)) {
+		mlx5_core_dbg(dev, "force teardown is not supported in the firmware\n");
+		return -EOPNOTSUPP;
+	}
+
+	MLX5_SET(teardown_hca_in, in, opcode, MLX5_CMD_OP_TEARDOWN_HCA);
+	MLX5_SET(teardown_hca_in, in, profile, MLX5_TEARDOWN_HCA_IN_PROFILE_FORCE_CLOSE);
+
+	ret = mlx5_cmd_exec_polling(dev, in, sizeof(in), out, sizeof(out));
+	if (ret)
+		return ret;
+
+	force_state = MLX5_GET(teardown_hca_out, out, force_state);
+	if (force_state == MLX5_TEARDOWN_HCA_OUT_FORCE_STATE_FAIL) {
+		mlx5_core_err(dev, "teardown with force mode failed\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+enum mlxsw_reg_mcc_instruction {
+	MLX5_REG_MCC_INSTRUCTION_LOCK_UPDATE_HANDLE = 0x01,
+	MLX5_REG_MCC_INSTRUCTION_RELEASE_UPDATE_HANDLE = 0x02,
+	MLX5_REG_MCC_INSTRUCTION_UPDATE_COMPONENT = 0x03,
+	MLX5_REG_MCC_INSTRUCTION_VERIFY_COMPONENT = 0x04,
+	MLX5_REG_MCC_INSTRUCTION_ACTIVATE = 0x06,
+	MLX5_REG_MCC_INSTRUCTION_CANCEL = 0x08,
+};
+
+static int mlx5_reg_mcc_set(struct mlx5_core_dev *dev,
+			    enum mlxsw_reg_mcc_instruction instr,
+			    u16 component_index, u32 update_handle,
+			    u32 component_size)
+{
+	u32 out[MLX5_ST_SZ_DW(mcc_reg)];
+	u32 in[MLX5_ST_SZ_DW(mcc_reg)];
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(mcc_reg, in, instruction, instr);
+	MLX5_SET(mcc_reg, in, component_index, component_index);
+	MLX5_SET(mcc_reg, in, update_handle, update_handle);
+	MLX5_SET(mcc_reg, in, component_size, component_size);
+
+	return mlx5_core_access_reg(dev, in, sizeof(in), out,
+				    sizeof(out), MLX5_REG_MCC, 0, 1);
+}
+
+static int mlx5_reg_mcc_query(struct mlx5_core_dev *dev,
+			      u32 *update_handle, u8 *error_code,
+			      u8 *control_state)
+{
+	u32 out[MLX5_ST_SZ_DW(mcc_reg)];
+	u32 in[MLX5_ST_SZ_DW(mcc_reg)];
+	int err;
+
+	memset(in, 0, sizeof(in));
+	memset(out, 0, sizeof(out));
+	MLX5_SET(mcc_reg, in, update_handle, *update_handle);
+
+	err = mlx5_core_access_reg(dev, in, sizeof(in), out,
+				   sizeof(out), MLX5_REG_MCC, 0, 0);
+	if (err)
+		goto out;
+
+	*update_handle = MLX5_GET(mcc_reg, out, update_handle);
+	*error_code = MLX5_GET(mcc_reg, out, error_code);
+	*control_state = MLX5_GET(mcc_reg, out, control_state);
+
+out:
+	return err;
+}
+
+static int mlx5_reg_mcda_set(struct mlx5_core_dev *dev,
+			     u32 update_handle,
+			     u32 offset, u16 size,
+			     u8 *data)
+{
+	int err, in_size = MLX5_ST_SZ_BYTES(mcda_reg) + size;
+	u32 out[MLX5_ST_SZ_DW(mcda_reg)];
+	int i, j, dw_size = size >> 2;
+	__be32 data_element;
+	u32 *in;
+
+	in = kzalloc(in_size, GFP_KERNEL);
+	if (!in)
+		return -ENOMEM;
+
+	MLX5_SET(mcda_reg, in, update_handle, update_handle);
+	MLX5_SET(mcda_reg, in, offset, offset);
+	MLX5_SET(mcda_reg, in, size, size);
+
+	for (i = 0; i < dw_size; i++) {
+		j = i * 4;
+		data_element = htonl(*(u32 *)&data[j]);
+		memcpy(MLX5_ADDR_OF(mcda_reg, in, data) + j, &data_element, 4);
+	}
+
+	err = mlx5_core_access_reg(dev, in, in_size, out,
+				   sizeof(out), MLX5_REG_MCDA, 0, 1);
+	kfree(in);
+	return err;
+}
+
+static int mlx5_reg_mcqi_query(struct mlx5_core_dev *dev,
+			       u16 component_index,
+			       u32 *max_component_size,
+			       u8 *log_mcda_word_size,
+			       u16 *mcda_max_write_size)
+{
+	u32 out[MLX5_ST_SZ_DW(mcqi_reg) + MLX5_ST_SZ_DW(mcqi_cap)];
+	int offset = MLX5_ST_SZ_DW(mcqi_reg);
+	u32 in[MLX5_ST_SZ_DW(mcqi_reg)];
+	int err;
+
+	memset(in, 0, sizeof(in));
+	memset(out, 0, sizeof(out));
+
+	MLX5_SET(mcqi_reg, in, component_index, component_index);
+	MLX5_SET(mcqi_reg, in, data_size, MLX5_ST_SZ_BYTES(mcqi_cap));
+
+	err = mlx5_core_access_reg(dev, in, sizeof(in), out,
+				   sizeof(out), MLX5_REG_MCQI, 0, 0);
+	if (err)
+		goto out;
+
+	*max_component_size = MLX5_GET(mcqi_cap, out + offset, max_component_size);
+	*log_mcda_word_size = MLX5_GET(mcqi_cap, out + offset, log_mcda_word_size);
+	*mcda_max_write_size = MLX5_GET(mcqi_cap, out + offset, mcda_max_write_size);
+
+out:
+	return err;
+}
+
+struct mlx5_mlxfw_dev {
+	struct mlxfw_dev mlxfw_dev;
+	struct mlx5_core_dev *mlx5_core_dev;
+};
+
+static int mlx5_component_query(struct mlxfw_dev *mlxfw_dev,
+				u16 component_index, u32 *p_max_size,
+				u8 *p_align_bits, u16 *p_max_write_size)
+{
+	struct mlx5_mlxfw_dev *mlx5_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlx5_mlxfw_dev, mlxfw_dev);
+	struct mlx5_core_dev *dev = mlx5_mlxfw_dev->mlx5_core_dev;
+
+	return mlx5_reg_mcqi_query(dev, component_index, p_max_size,
+				   p_align_bits, p_max_write_size);
+}
+
+static int mlx5_fsm_lock(struct mlxfw_dev *mlxfw_dev, u32 *fwhandle)
+{
+	struct mlx5_mlxfw_dev *mlx5_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlx5_mlxfw_dev, mlxfw_dev);
+	struct mlx5_core_dev *dev = mlx5_mlxfw_dev->mlx5_core_dev;
+	u8 control_state, error_code;
+	int err;
+
+	*fwhandle = 0;
+	err = mlx5_reg_mcc_query(dev, fwhandle, &error_code, &control_state);
+	if (err)
+		return err;
+
+	if (control_state != MLXFW_FSM_STATE_IDLE)
+		return -EBUSY;
+
+	return mlx5_reg_mcc_set(dev, MLX5_REG_MCC_INSTRUCTION_LOCK_UPDATE_HANDLE,
+				0, *fwhandle, 0);
+}
+
+static int mlx5_fsm_component_update(struct mlxfw_dev *mlxfw_dev, u32 fwhandle,
+				     u16 component_index, u32 component_size)
+{
+	struct mlx5_mlxfw_dev *mlx5_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlx5_mlxfw_dev, mlxfw_dev);
+	struct mlx5_core_dev *dev = mlx5_mlxfw_dev->mlx5_core_dev;
+
+	return mlx5_reg_mcc_set(dev, MLX5_REG_MCC_INSTRUCTION_UPDATE_COMPONENT,
+				component_index, fwhandle, component_size);
+}
+
+static int mlx5_fsm_block_download(struct mlxfw_dev *mlxfw_dev, u32 fwhandle,
+				   u8 *data, u16 size, u32 offset)
+{
+	struct mlx5_mlxfw_dev *mlx5_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlx5_mlxfw_dev, mlxfw_dev);
+	struct mlx5_core_dev *dev = mlx5_mlxfw_dev->mlx5_core_dev;
+
+	return mlx5_reg_mcda_set(dev, fwhandle, offset, size, data);
+}
+
+static int mlx5_fsm_component_verify(struct mlxfw_dev *mlxfw_dev, u32 fwhandle,
+				     u16 component_index)
+{
+	struct mlx5_mlxfw_dev *mlx5_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlx5_mlxfw_dev, mlxfw_dev);
+	struct mlx5_core_dev *dev = mlx5_mlxfw_dev->mlx5_core_dev;
+
+	return mlx5_reg_mcc_set(dev, MLX5_REG_MCC_INSTRUCTION_VERIFY_COMPONENT,
+				component_index, fwhandle, 0);
+}
+
+static int mlx5_fsm_activate(struct mlxfw_dev *mlxfw_dev, u32 fwhandle)
+{
+	struct mlx5_mlxfw_dev *mlx5_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlx5_mlxfw_dev, mlxfw_dev);
+	struct mlx5_core_dev *dev = mlx5_mlxfw_dev->mlx5_core_dev;
+
+	return mlx5_reg_mcc_set(dev, MLX5_REG_MCC_INSTRUCTION_ACTIVATE,	0,
+				fwhandle, 0);
+}
+
+static int mlx5_fsm_query_state(struct mlxfw_dev *mlxfw_dev, u32 fwhandle,
+				enum mlxfw_fsm_state *fsm_state,
+				enum mlxfw_fsm_state_err *fsm_state_err)
+{
+	struct mlx5_mlxfw_dev *mlx5_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlx5_mlxfw_dev, mlxfw_dev);
+	struct mlx5_core_dev *dev = mlx5_mlxfw_dev->mlx5_core_dev;
+	u8 control_state, error_code;
+	int err;
+
+	err = mlx5_reg_mcc_query(dev, &fwhandle, &error_code, &control_state);
+	if (err)
+		return err;
+
+	*fsm_state = control_state;
+	*fsm_state_err = min_t(enum mlxfw_fsm_state_err, error_code,
+			       MLXFW_FSM_STATE_ERR_MAX);
+	return 0;
+}
+
+static void mlx5_fsm_cancel(struct mlxfw_dev *mlxfw_dev, u32 fwhandle)
+{
+	struct mlx5_mlxfw_dev *mlx5_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlx5_mlxfw_dev, mlxfw_dev);
+	struct mlx5_core_dev *dev = mlx5_mlxfw_dev->mlx5_core_dev;
+
+	mlx5_reg_mcc_set(dev, MLX5_REG_MCC_INSTRUCTION_CANCEL, 0, fwhandle, 0);
+}
+
+static void mlx5_fsm_release(struct mlxfw_dev *mlxfw_dev, u32 fwhandle)
+{
+	struct mlx5_mlxfw_dev *mlx5_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlx5_mlxfw_dev, mlxfw_dev);
+	struct mlx5_core_dev *dev = mlx5_mlxfw_dev->mlx5_core_dev;
+
+	mlx5_reg_mcc_set(dev, MLX5_REG_MCC_INSTRUCTION_RELEASE_UPDATE_HANDLE, 0,
+			 fwhandle, 0);
+}
+
+static const struct mlxfw_dev_ops mlx5_mlxfw_dev_ops = {
+	.component_query	= mlx5_component_query,
+	.fsm_lock		= mlx5_fsm_lock,
+	.fsm_component_update	= mlx5_fsm_component_update,
+	.fsm_block_download	= mlx5_fsm_block_download,
+	.fsm_component_verify	= mlx5_fsm_component_verify,
+	.fsm_activate		= mlx5_fsm_activate,
+	.fsm_query_state	= mlx5_fsm_query_state,
+	.fsm_cancel		= mlx5_fsm_cancel,
+	.fsm_release		= mlx5_fsm_release
+};
+
+int mlx5_firmware_flash(struct mlx5_core_dev *dev,
+			const struct firmware *firmware)
+{
+	struct mlx5_mlxfw_dev mlx5_mlxfw_dev = {
+		.mlxfw_dev = {
+			.ops = &mlx5_mlxfw_dev_ops,
+			.psid = dev->board_id,
+			.psid_size = strlen(dev->board_id),
+		},
+		.mlx5_core_dev = dev
+	};
+
+	if (!MLX5_CAP_GEN(dev, mcam_reg)  ||
+	    !MLX5_CAP_MCAM_REG(dev, mcqi) ||
+	    !MLX5_CAP_MCAM_REG(dev, mcc)  ||
+	    !MLX5_CAP_MCAM_REG(dev, mcda)) {
+		pr_info("%s flashing isn't supported by the running FW\n", __func__);
+		return -EOPNOTSUPP;
+	}
+
+	return mlxfw_firmware_flash(&mlx5_mlxfw_dev.mlxfw_dev, firmware);
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/health.c b/drivers/net/ethernet/mellanox/mlx5/core/health.c
index 44f59b1d6f0f..4b6b03d6297f 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/health.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/health.c
@@ -67,6 +67,7 @@ enum {
 
 enum {
 	MLX5_DROP_NEW_HEALTH_WORK,
+	MLX5_DROP_NEW_RECOVERY_WORK,
 };
 
 static u8 get_nic_state(struct mlx5_core_dev *dev)
@@ -111,14 +112,14 @@ static int in_fatal(struct mlx5_core_dev *dev)
 	return 0;
 }
 
-void mlx5_enter_error_state(struct mlx5_core_dev *dev)
+void mlx5_enter_error_state(struct mlx5_core_dev *dev, bool force)
 {
 	mutex_lock(&dev->intf_state_mutex);
 	if (dev->state == MLX5_DEVICE_STATE_INTERNAL_ERROR)
 		goto unlock;
 
 	mlx5_core_err(dev, "start\n");
-	if (pci_channel_offline(dev->pdev) || in_fatal(dev)) {
+	if (pci_channel_offline(dev->pdev) || in_fatal(dev) || force) {
 		dev->state = MLX5_DEVICE_STATE_INTERNAL_ERROR;
 		trigger_cmd_completions(dev);
 	}
@@ -185,6 +186,7 @@ static void health_care(struct work_struct *work)
 	struct mlx5_core_health *health;
 	struct mlx5_core_dev *dev;
 	struct mlx5_priv *priv;
+	unsigned long flags;
 
 	health = container_of(work, struct mlx5_core_health, work);
 	priv = container_of(health, struct mlx5_priv, health);
@@ -192,13 +194,13 @@ static void health_care(struct work_struct *work)
 	mlx5_core_warn(dev, "handling bad device here\n");
 	mlx5_handle_bad_state(dev);
 
-	spin_lock(&health->wq_lock);
-	if (!test_bit(MLX5_DROP_NEW_HEALTH_WORK, &health->flags))
+	spin_lock_irqsave(&health->wq_lock, flags);
+	if (!test_bit(MLX5_DROP_NEW_RECOVERY_WORK, &health->flags))
 		schedule_delayed_work(&health->recover_work, recover_delay);
 	else
 		dev_err(&dev->pdev->dev,
 			"new health works are not permitted at this stage\n");
-	spin_unlock(&health->wq_lock);
+	spin_unlock_irqrestore(&health->wq_lock, flags);
 }
 
 static const char *hsynd_str(u8 synd)
@@ -269,16 +271,28 @@ static unsigned long get_next_poll_jiffies(void)
 	return next;
 }
 
+void mlx5_trigger_health_work(struct mlx5_core_dev *dev)
+{
+	struct mlx5_core_health *health = &dev->priv.health;
+	unsigned long flags;
+
+	spin_lock_irqsave(&health->wq_lock, flags);
+	if (!test_bit(MLX5_DROP_NEW_HEALTH_WORK, &health->flags))
+		queue_work(health->wq, &health->work);
+	else
+		dev_err(&dev->pdev->dev,
+			"new health works are not permitted at this stage\n");
+	spin_unlock_irqrestore(&health->wq_lock, flags);
+}
+
 static void poll_health(unsigned long data)
 {
 	struct mlx5_core_dev *dev = (struct mlx5_core_dev *)data;
 	struct mlx5_core_health *health = &dev->priv.health;
 	u32 count;
 
-	if (dev->state == MLX5_DEVICE_STATE_INTERNAL_ERROR) {
-		mod_timer(&health->timer, get_next_poll_jiffies());
-		return;
-	}
+	if (dev->state == MLX5_DEVICE_STATE_INTERNAL_ERROR)
+		goto out;
 
 	count = ioread32be(health->health_counter);
 	if (count == health->prev)
@@ -290,21 +304,16 @@ static void poll_health(unsigned long data)
 	if (health->miss_counter == MAX_MISSES) {
 		dev_err(&dev->pdev->dev, "device's health compromised - reached miss count\n");
 		print_health_info(dev);
-	} else {
-		mod_timer(&health->timer, get_next_poll_jiffies());
 	}
 
 	if (in_fatal(dev) && !health->sick) {
 		health->sick = true;
 		print_health_info(dev);
-		spin_lock(&health->wq_lock);
-		if (!test_bit(MLX5_DROP_NEW_HEALTH_WORK, &health->flags))
-			queue_work(health->wq, &health->work);
-		else
-			dev_err(&dev->pdev->dev,
-				"new health works are not permitted at this stage\n");
-		spin_unlock(&health->wq_lock);
+		mlx5_trigger_health_work(dev);
 	}
+
+out:
+	mod_timer(&health->timer, get_next_poll_jiffies());
 }
 
 void mlx5_start_health_poll(struct mlx5_core_dev *dev)
@@ -314,6 +323,7 @@ void mlx5_start_health_poll(struct mlx5_core_dev *dev)
 	init_timer(&health->timer);
 	health->sick = 0;
 	clear_bit(MLX5_DROP_NEW_HEALTH_WORK, &health->flags);
+	clear_bit(MLX5_DROP_NEW_RECOVERY_WORK, &health->flags);
 	health->health = &dev->iseg->health;
 	health->health_counter = &dev->iseg->health_counter;
 
@@ -333,14 +343,26 @@ void mlx5_stop_health_poll(struct mlx5_core_dev *dev)
 void mlx5_drain_health_wq(struct mlx5_core_dev *dev)
 {
 	struct mlx5_core_health *health = &dev->priv.health;
+	unsigned long flags;
 
-	spin_lock(&health->wq_lock);
+	spin_lock_irqsave(&health->wq_lock, flags);
 	set_bit(MLX5_DROP_NEW_HEALTH_WORK, &health->flags);
-	spin_unlock(&health->wq_lock);
+	set_bit(MLX5_DROP_NEW_RECOVERY_WORK, &health->flags);
+	spin_unlock_irqrestore(&health->wq_lock, flags);
 	cancel_delayed_work_sync(&health->recover_work);
 	cancel_work_sync(&health->work);
 }
 
+void mlx5_drain_health_recovery(struct mlx5_core_dev *dev)
+{
+	struct mlx5_core_health *health = &dev->priv.health;
+
+	spin_lock(&health->wq_lock);
+	set_bit(MLX5_DROP_NEW_RECOVERY_WORK, &health->flags);
+	spin_unlock(&health->wq_lock);
+	cancel_delayed_work_sync(&dev->priv.health.recover_work);
+}
+
 void mlx5_health_cleanup(struct mlx5_core_dev *dev)
 {
 	struct mlx5_core_health *health = &dev->priv.health;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/ipoib/ethtool.c b/drivers/net/ethernet/mellanox/mlx5/core/ipoib/ethtool.c
new file mode 100644
index 000000000000..eb04e97d8765
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/ipoib/ethtool.c
@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2017, Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "en.h"
+#include "ipoib.h"
+
+static void mlx5i_get_drvinfo(struct net_device *dev,
+			      struct ethtool_drvinfo *drvinfo)
+{
+	struct mlx5e_priv *priv = mlx5i_epriv(dev);
+
+	mlx5e_ethtool_get_drvinfo(priv, drvinfo);
+}
+
+static void mlx5i_get_strings(struct net_device *dev,
+			      uint32_t stringset, uint8_t *data)
+{
+	struct mlx5e_priv *priv  = mlx5i_epriv(dev);
+
+	mlx5e_ethtool_get_strings(priv, stringset, data);
+}
+
+static int mlx5i_get_sset_count(struct net_device *dev, int sset)
+{
+	struct mlx5e_priv *priv = mlx5i_epriv(dev);
+
+	return mlx5e_ethtool_get_sset_count(priv, sset);
+}
+
+static void mlx5i_get_ethtool_stats(struct net_device *dev,
+				    struct ethtool_stats *stats,
+				    u64 *data)
+{
+	struct mlx5e_priv *priv = mlx5i_epriv(dev);
+
+	mlx5e_ethtool_get_ethtool_stats(priv, stats, data);
+}
+
+static int mlx5i_set_ringparam(struct net_device *dev,
+			       struct ethtool_ringparam *param)
+{
+	struct mlx5e_priv *priv = mlx5i_epriv(dev);
+
+	return mlx5e_ethtool_set_ringparam(priv, param);
+}
+
+static void mlx5i_get_ringparam(struct net_device *dev,
+				struct ethtool_ringparam *param)
+{
+	struct mlx5e_priv *priv = mlx5i_epriv(dev);
+
+	mlx5e_ethtool_get_ringparam(priv, param);
+}
+
+static int mlx5i_set_channels(struct net_device *dev,
+			      struct ethtool_channels *ch)
+{
+	struct mlx5e_priv *priv = mlx5i_epriv(dev);
+
+	return mlx5e_ethtool_set_channels(priv, ch);
+}
+
+static void mlx5i_get_channels(struct net_device *dev,
+			       struct ethtool_channels *ch)
+{
+	struct mlx5e_priv *priv = mlx5i_epriv(dev);
+
+	mlx5e_ethtool_get_channels(priv, ch);
+}
+
+static int mlx5i_set_coalesce(struct net_device *netdev,
+			      struct ethtool_coalesce *coal)
+{
+	struct mlx5e_priv *priv = mlx5i_epriv(netdev);
+
+	return mlx5e_ethtool_set_coalesce(priv, coal);
+}
+
+static int mlx5i_get_coalesce(struct net_device *netdev,
+			      struct ethtool_coalesce *coal)
+{
+	struct mlx5e_priv *priv = mlx5i_epriv(netdev);
+
+	return mlx5e_ethtool_get_coalesce(priv, coal);
+}
+
+static int mlx5i_get_ts_info(struct net_device *netdev,
+			     struct ethtool_ts_info *info)
+{
+	struct mlx5e_priv *priv = mlx5i_epriv(netdev);
+
+	return mlx5e_ethtool_get_ts_info(priv, info);
+}
+
+static int mlx5i_flash_device(struct net_device *netdev,
+			      struct ethtool_flash *flash)
+{
+	struct mlx5e_priv *priv = mlx5i_epriv(netdev);
+
+	return mlx5e_ethtool_flash_device(priv, flash);
+}
+
+const struct ethtool_ops mlx5i_ethtool_ops = {
+	.get_drvinfo       = mlx5i_get_drvinfo,
+	.get_strings       = mlx5i_get_strings,
+	.get_sset_count    = mlx5i_get_sset_count,
+	.get_ethtool_stats = mlx5i_get_ethtool_stats,
+	.get_ringparam     = mlx5i_get_ringparam,
+	.set_ringparam     = mlx5i_set_ringparam,
+	.flash_device      = mlx5i_flash_device,
+	.get_channels      = mlx5i_get_channels,
+	.set_channels      = mlx5i_set_channels,
+	.get_coalesce      = mlx5i_get_coalesce,
+	.set_coalesce      = mlx5i_set_coalesce,
+	.get_ts_info       = mlx5i_get_ts_info,
+};
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/ipoib.c b/drivers/net/ethernet/mellanox/mlx5/core/ipoib/ipoib.c
index cc1858752e70..1ee5bce85901 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/ipoib.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/ipoib/ipoib.c
@@ -36,20 +36,38 @@
 #include "ipoib.h"
 
 #define IB_DEFAULT_Q_KEY   0xb1b
+#define MLX5I_PARAMS_DEFAULT_LOG_RQ_SIZE 9
 
 static int mlx5i_open(struct net_device *netdev);
 static int mlx5i_close(struct net_device *netdev);
 static int  mlx5i_dev_init(struct net_device *dev);
 static void mlx5i_dev_cleanup(struct net_device *dev);
+static int mlx5i_change_mtu(struct net_device *netdev, int new_mtu);
+static int mlx5i_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd);
 
 static const struct net_device_ops mlx5i_netdev_ops = {
 	.ndo_open                = mlx5i_open,
 	.ndo_stop                = mlx5i_close,
 	.ndo_init                = mlx5i_dev_init,
 	.ndo_uninit              = mlx5i_dev_cleanup,
+	.ndo_change_mtu          = mlx5i_change_mtu,
+	.ndo_do_ioctl            = mlx5i_ioctl,
 };
 
 /* IPoIB mlx5 netdev profile */
+static void mlx5i_build_nic_params(struct mlx5_core_dev *mdev,
+				   struct mlx5e_params *params)
+{
+	/* Override RQ params as IPoIB supports only LINKED LIST RQ for now */
+	mlx5e_set_rq_type_params(mdev, params, MLX5_WQ_TYPE_LINKED_LIST);
+
+	/* RQ size in ipoib by default is 512 */
+	params->log_rq_size = is_kdump_kernel() ?
+		MLX5E_PARAMS_MINIMUM_LOG_RQ_SIZE :
+		MLX5I_PARAMS_DEFAULT_LOG_RQ_SIZE;
+
+	params->lro_en = false;
+}
 
 /* Called directly after IPoIB netdevice was created to initialize SW structs */
 static void mlx5i_init(struct mlx5_core_dev *mdev,
@@ -59,19 +77,18 @@ static void mlx5i_init(struct mlx5_core_dev *mdev,
 {
 	struct mlx5e_priv *priv  = mlx5i_epriv(netdev);
 
+	/* priv init */
 	priv->mdev        = mdev;
 	priv->netdev      = netdev;
 	priv->profile     = profile;
 	priv->ppriv       = ppriv;
+	priv->hard_mtu = MLX5_IB_GRH_BYTES + MLX5_IPOIB_HARD_LEN;
+	mutex_init(&priv->state_lock);
 
 	mlx5e_build_nic_params(mdev, &priv->channels.params, profile->max_nch(mdev));
+	mlx5i_build_nic_params(mdev, &priv->channels.params);
 
-	/* Override RQ params as IPoIB supports only LINKED LIST RQ for now */
-	mlx5e_set_rq_type_params(mdev, &priv->channels.params, MLX5_WQ_TYPE_LINKED_LIST);
-	priv->channels.params.lro_en = false;
-
-	mutex_init(&priv->state_lock);
-
+	/* netdev init */
 	netdev->hw_features    |= NETIF_F_SG;
 	netdev->hw_features    |= NETIF_F_IP_CSUM;
 	netdev->hw_features    |= NETIF_F_IPV6_CSUM;
@@ -82,6 +99,7 @@ static void mlx5i_init(struct mlx5_core_dev *mdev,
 	netdev->hw_features    |= NETIF_F_RXHASH;
 
 	netdev->netdev_ops = &mlx5i_netdev_ops;
+	netdev->ethtool_ops = &mlx5i_ethtool_ops;
 }
 
 /* Called directly before IPoIB netdevice is destroyed to cleanup SW structs */
@@ -102,7 +120,7 @@ static int mlx5i_create_underlay_qp(struct mlx5_core_dev *mdev, struct mlx5_core
 	void *qpc;
 
 	inlen = MLX5_ST_SZ_BYTES(create_qp_in);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -290,6 +308,7 @@ static const struct mlx5e_profile mlx5i_nic_profile = {
 	.disable	   = NULL, /* mlx5i_disable */
 	.update_stats	   = NULL, /* mlx5i_update_stats */
 	.max_nch	   = mlx5e_get_max_num_channels,
+	.update_carrier    = NULL, /* no HW update in IB link */
 	.rx_handlers.handle_rx_cqe       = mlx5i_handle_rx_cqe,
 	.rx_handlers.handle_rx_cqe_mpwqe = NULL, /* Not supported */
 	.max_tc		   = MLX5I_MAX_NUM_TC,
@@ -297,6 +316,35 @@ static const struct mlx5e_profile mlx5i_nic_profile = {
 
 /* mlx5i netdev NDos */
 
+static int mlx5i_change_mtu(struct net_device *netdev, int new_mtu)
+{
+	struct mlx5e_priv *priv = mlx5i_epriv(netdev);
+	struct mlx5e_channels new_channels = {};
+	int curr_mtu;
+	int err = 0;
+
+	mutex_lock(&priv->state_lock);
+
+	curr_mtu    = netdev->mtu;
+	netdev->mtu = new_mtu;
+
+	if (!test_bit(MLX5E_STATE_OPENED, &priv->state))
+		goto out;
+
+	new_channels.params = priv->channels.params;
+	err = mlx5e_open_channels(priv, &new_channels);
+	if (err) {
+		netdev->mtu = curr_mtu;
+		goto out;
+	}
+
+	mlx5e_switch_priv_channels(priv, &new_channels, NULL);
+
+out:
+	mutex_unlock(&priv->state_lock);
+	return err;
+}
+
 static int mlx5i_dev_init(struct net_device *dev)
 {
 	struct mlx5e_priv    *priv   = mlx5i_epriv(dev);
@@ -310,6 +358,20 @@ static int mlx5i_dev_init(struct net_device *dev)
 	return 0;
 }
 
+static int mlx5i_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	struct mlx5e_priv *priv = mlx5i_epriv(dev);
+
+	switch (cmd) {
+	case SIOCSHWTSTAMP:
+		return mlx5e_hwstamp_set(priv, ifr);
+	case SIOCGHWTSTAMP:
+		return mlx5e_hwstamp_get(priv, ifr);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
 static void mlx5i_dev_cleanup(struct net_device *dev)
 {
 	struct mlx5e_priv    *priv   = mlx5i_epriv(dev);
@@ -336,6 +398,8 @@ static int mlx5i_open(struct net_device *netdev)
 
 	mlx5e_refresh_tirs(priv, false);
 	mlx5e_activate_priv_channels(priv);
+	mlx5e_timestamp_init(priv);
+
 	mutex_unlock(&priv->state_lock);
 	return 0;
 
@@ -359,6 +423,7 @@ static int mlx5i_close(struct net_device *netdev)
 
 	clear_bit(MLX5E_STATE_OPENED, &priv->state);
 
+	mlx5e_timestamp_cleanup(priv);
 	netif_carrier_off(priv->netdev);
 	mlx5e_deactivate_priv_channels(priv);
 	mlx5e_close_channels(&priv->channels);
@@ -510,4 +575,3 @@ void mlx5_rdma_netdev_free(struct net_device *netdev)
 	mlx5e_destroy_mdev_resources(priv->mdev);
 }
 EXPORT_SYMBOL(mlx5_rdma_netdev_free);
-
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/ipoib.h b/drivers/net/ethernet/mellanox/mlx5/core/ipoib/ipoib.h
index 213191a78464..a0f405f520f7 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/ipoib.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/ipoib/ipoib.h
@@ -38,6 +38,13 @@
 
 #define MLX5I_MAX_NUM_TC 1
 
+extern const struct ethtool_ops mlx5i_ethtool_ops;
+
+#define MLX5_IB_GRH_BYTES       40
+#define MLX5_IPOIB_ENCAP_LEN    4
+#define MLX5_IPOIB_PSEUDO_LEN   20
+#define MLX5_IPOIB_HARD_LEN     (MLX5_IPOIB_PSEUDO_LEN + MLX5_IPOIB_ENCAP_LEN)
+
 /* ipoib rdma netdev's private data structure */
 struct mlx5i_priv {
 	struct rdma_netdev rn; /* keep this first */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/lag.c b/drivers/net/ethernet/mellanox/mlx5/core/lag.c
index b5d5519542e8..a3a836bdcfd2 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/lag.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/lag.c
@@ -61,6 +61,11 @@ struct mlx5_lag {
 	struct lag_tracker        tracker;
 	struct delayed_work       bond_work;
 	struct notifier_block     nb;
+
+	/* Admin state. Allow lag only if allowed is true
+	 * even if network conditions for lag were met
+	 */
+	bool                      allowed;
 };
 
 /* General purpose, use for short periods of time.
@@ -214,6 +219,7 @@ static void mlx5_do_bond(struct mlx5_lag *ldev)
 	struct lag_tracker tracker;
 	u8 v2p_port1, v2p_port2;
 	int i, err;
+	bool do_bond;
 
 	if (!dev0 || !dev1)
 		return;
@@ -222,13 +228,9 @@ static void mlx5_do_bond(struct mlx5_lag *ldev)
 	tracker = ldev->tracker;
 	mutex_unlock(&lag_mutex);
 
-	if (tracker.is_bonded && !mlx5_lag_is_bonded(ldev)) {
-		if (mlx5_sriov_is_enabled(dev0) ||
-		    mlx5_sriov_is_enabled(dev1)) {
-			mlx5_core_warn(dev0, "LAG is not supported with SRIOV");
-			return;
-		}
+	do_bond = tracker.is_bonded && ldev->allowed;
 
+	if (do_bond && !mlx5_lag_is_bonded(ldev)) {
 		for (i = 0; i < MLX5_MAX_PORTS; i++)
 			mlx5_remove_dev_by_protocol(ldev->pf[i].dev,
 						    MLX5_INTERFACE_PROTOCOL_IB);
@@ -237,7 +239,7 @@ static void mlx5_do_bond(struct mlx5_lag *ldev)
 
 		mlx5_add_dev_by_protocol(dev0, MLX5_INTERFACE_PROTOCOL_IB);
 		mlx5_nic_vport_enable_roce(dev1);
-	} else if (tracker.is_bonded && mlx5_lag_is_bonded(ldev)) {
+	} else if (do_bond && mlx5_lag_is_bonded(ldev)) {
 		mlx5_infer_tx_affinity_mapping(&tracker, &v2p_port1,
 					       &v2p_port2);
 
@@ -252,7 +254,7 @@ static void mlx5_do_bond(struct mlx5_lag *ldev)
 					      "Failed to modify LAG (%d)\n",
 					      err);
 		}
-	} else if (!tracker.is_bonded && mlx5_lag_is_bonded(ldev)) {
+	} else if (!do_bond && mlx5_lag_is_bonded(ldev)) {
 		mlx5_remove_dev_by_protocol(dev0, MLX5_INTERFACE_PROTOCOL_IB);
 		mlx5_nic_vport_disable_roce(dev1);
 
@@ -411,6 +413,15 @@ static int mlx5_lag_netdev_event(struct notifier_block *this,
 	return NOTIFY_DONE;
 }
 
+static bool mlx5_lag_check_prereq(struct mlx5_lag *ldev)
+{
+	if ((ldev->pf[0].dev && mlx5_sriov_is_enabled(ldev->pf[0].dev)) ||
+	    (ldev->pf[1].dev && mlx5_sriov_is_enabled(ldev->pf[1].dev)))
+		return false;
+	else
+		return true;
+}
+
 static struct mlx5_lag *mlx5_lag_dev_alloc(void)
 {
 	struct mlx5_lag *ldev;
@@ -420,6 +431,7 @@ static struct mlx5_lag *mlx5_lag_dev_alloc(void)
 		return NULL;
 
 	INIT_DELAYED_WORK(&ldev->bond_work, mlx5_do_bond_work);
+	ldev->allowed = mlx5_lag_check_prereq(ldev);
 
 	return ldev;
 }
@@ -444,7 +456,9 @@ static void mlx5_lag_dev_add_pf(struct mlx5_lag *ldev,
 	ldev->tracker.netdev_state[fn].link_up = 0;
 	ldev->tracker.netdev_state[fn].tx_enabled = 0;
 
+	ldev->allowed = mlx5_lag_check_prereq(ldev);
 	dev->priv.lag = ldev;
+
 	mutex_unlock(&lag_mutex);
 }
 
@@ -464,10 +478,10 @@ static void mlx5_lag_dev_remove_pf(struct mlx5_lag *ldev,
 	memset(&ldev->pf[i], 0, sizeof(*ldev->pf));
 
 	dev->priv.lag = NULL;
+	ldev->allowed = mlx5_lag_check_prereq(ldev);
 	mutex_unlock(&lag_mutex);
 }
 
-
 /* Must be called with intf_mutex held */
 void mlx5_lag_add(struct mlx5_core_dev *dev, struct net_device *netdev)
 {
@@ -543,6 +557,44 @@ bool mlx5_lag_is_active(struct mlx5_core_dev *dev)
 }
 EXPORT_SYMBOL(mlx5_lag_is_active);
 
+static int mlx5_lag_set_state(struct mlx5_core_dev *dev, bool allow)
+{
+	struct mlx5_lag *ldev;
+	int ret = 0;
+	bool lag_active;
+
+	mlx5_dev_list_lock();
+
+	ldev = mlx5_lag_dev_get(dev);
+	if (!ldev) {
+		ret = -ENODEV;
+		goto unlock;
+	}
+	lag_active = mlx5_lag_is_bonded(ldev);
+	if (!mlx5_lag_check_prereq(ldev) && allow) {
+		ret = -EINVAL;
+		goto unlock;
+	}
+	if (ldev->allowed == allow)
+		goto unlock;
+	ldev->allowed = allow;
+	if ((lag_active && !allow) || allow)
+		mlx5_do_bond(ldev);
+unlock:
+	mlx5_dev_list_unlock();
+	return ret;
+}
+
+int mlx5_lag_forbid(struct mlx5_core_dev *dev)
+{
+	return mlx5_lag_set_state(dev, false);
+}
+
+int mlx5_lag_allow(struct mlx5_core_dev *dev)
+{
+	return mlx5_lag_set_state(dev, true);
+}
+
 struct net_device *mlx5_lag_get_roce_netdev(struct mlx5_core_dev *dev)
 {
 	struct net_device *ndev = NULL;
@@ -586,4 +638,3 @@ bool mlx5_lag_intf_add(struct mlx5_interface *intf, struct mlx5_priv *priv)
 	/* If bonded, we do not add an IB device for PF1. */
 	return false;
 }
-
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/lib/gid.c b/drivers/net/ethernet/mellanox/mlx5/core/lib/gid.c
new file mode 100644
index 000000000000..de2aed44ab85
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/lib/gid.c
@@ -0,0 +1,154 @@
+/*
+ * Copyright (c) 2017, Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/mlx5/driver.h>
+#include <linux/etherdevice.h>
+#include <linux/idr.h>
+#include "mlx5_core.h"
+
+void mlx5_init_reserved_gids(struct mlx5_core_dev *dev)
+{
+	unsigned int tblsz = MLX5_CAP_ROCE(dev, roce_address_table_size);
+
+	ida_init(&dev->roce.reserved_gids.ida);
+	dev->roce.reserved_gids.start = tblsz;
+	dev->roce.reserved_gids.count = 0;
+}
+
+void mlx5_cleanup_reserved_gids(struct mlx5_core_dev *dev)
+{
+	WARN_ON(!ida_is_empty(&dev->roce.reserved_gids.ida));
+	dev->roce.reserved_gids.start = 0;
+	dev->roce.reserved_gids.count = 0;
+	ida_destroy(&dev->roce.reserved_gids.ida);
+}
+
+int mlx5_core_reserve_gids(struct mlx5_core_dev *dev, unsigned int count)
+{
+	if (test_bit(MLX5_INTERFACE_STATE_UP, &dev->intf_state)) {
+		mlx5_core_err(dev, "Cannot reserve GIDs when interfaces are up\n");
+		return -EPERM;
+	}
+	if (dev->roce.reserved_gids.start < count) {
+		mlx5_core_warn(dev, "GID table exhausted attempting to reserve %d more GIDs\n",
+			       count);
+		return -ENOMEM;
+	}
+	if (dev->roce.reserved_gids.count + count > MLX5_MAX_RESERVED_GIDS) {
+		mlx5_core_warn(dev, "Unable to reserve %d more GIDs\n", count);
+		return -ENOMEM;
+	}
+
+	dev->roce.reserved_gids.start -= count;
+	dev->roce.reserved_gids.count += count;
+	mlx5_core_dbg(dev, "Reserved %u GIDs starting at %u\n",
+		      dev->roce.reserved_gids.count,
+		      dev->roce.reserved_gids.start);
+	return 0;
+}
+
+void mlx5_core_unreserve_gids(struct mlx5_core_dev *dev, unsigned int count)
+{
+	WARN(test_bit(MLX5_INTERFACE_STATE_UP, &dev->intf_state), "Unreserving GIDs when interfaces are up");
+	WARN(count > dev->roce.reserved_gids.count, "Unreserving %u GIDs when only %u reserved",
+	     count, dev->roce.reserved_gids.count);
+
+	dev->roce.reserved_gids.start += count;
+	dev->roce.reserved_gids.count -= count;
+	mlx5_core_dbg(dev, "%u GIDs starting at %u left reserved\n",
+		      dev->roce.reserved_gids.count,
+		      dev->roce.reserved_gids.start);
+}
+
+int mlx5_core_reserved_gid_alloc(struct mlx5_core_dev *dev, int *gid_index)
+{
+	int end = dev->roce.reserved_gids.start +
+		  dev->roce.reserved_gids.count;
+	int index = 0;
+
+	index = ida_simple_get(&dev->roce.reserved_gids.ida,
+			       dev->roce.reserved_gids.start, end,
+			       GFP_KERNEL);
+	if (index < 0)
+		return index;
+
+	mlx5_core_dbg(dev, "Allocating reserved GID %u\n", index);
+	*gid_index = index;
+	return 0;
+}
+
+void mlx5_core_reserved_gid_free(struct mlx5_core_dev *dev, int gid_index)
+{
+	mlx5_core_dbg(dev, "Freeing reserved GID %u\n", gid_index);
+	ida_simple_remove(&dev->roce.reserved_gids.ida, gid_index);
+}
+
+unsigned int mlx5_core_reserved_gids_count(struct mlx5_core_dev *dev)
+{
+	return dev->roce.reserved_gids.count;
+}
+EXPORT_SYMBOL_GPL(mlx5_core_reserved_gids_count);
+
+int mlx5_core_roce_gid_set(struct mlx5_core_dev *dev, unsigned int index,
+			   u8 roce_version, u8 roce_l3_type, const u8 *gid,
+			   const u8 *mac, bool vlan, u16 vlan_id)
+{
+#define MLX5_SET_RA(p, f, v) MLX5_SET(roce_addr_layout, p, f, v)
+	u32  in[MLX5_ST_SZ_DW(set_roce_address_in)] = {0};
+	u32 out[MLX5_ST_SZ_DW(set_roce_address_out)] = {0};
+	void *in_addr = MLX5_ADDR_OF(set_roce_address_in, in, roce_address);
+	char *addr_l3_addr = MLX5_ADDR_OF(roce_addr_layout, in_addr,
+					  source_l3_address);
+	void *addr_mac = MLX5_ADDR_OF(roce_addr_layout, in_addr,
+				      source_mac_47_32);
+	int gidsz = MLX5_FLD_SZ_BYTES(roce_addr_layout, source_l3_address);
+
+	if (MLX5_CAP_GEN(dev, port_type) != MLX5_CAP_PORT_TYPE_ETH)
+		return -EINVAL;
+
+	if (gid) {
+		if (vlan) {
+			MLX5_SET_RA(in_addr, vlan_valid, 1);
+			MLX5_SET_RA(in_addr, vlan_id, vlan_id);
+		}
+
+		ether_addr_copy(addr_mac, mac);
+		MLX5_SET_RA(in_addr, roce_version, roce_version);
+		MLX5_SET_RA(in_addr, roce_l3_type, roce_l3_type);
+		memcpy(addr_l3_addr, gid, gidsz);
+	}
+
+	MLX5_SET(set_roce_address_in, in, roce_address_index, index);
+	MLX5_SET(set_roce_address_in, in, opcode, MLX5_CMD_OP_SET_ROCE_ADDRESS);
+	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
+}
+EXPORT_SYMBOL(mlx5_core_roce_gid_set);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/lib/mlx5.h b/drivers/net/ethernet/mellanox/mlx5/core/lib/mlx5.h
new file mode 100644
index 000000000000..7550b1cc8c6a
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/lib/mlx5.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2017, Mellanox Technologies, Ltd.  All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef __LIB_MLX5_H__
+#define __LIB_MLX5_H__
+
+void mlx5_init_reserved_gids(struct mlx5_core_dev *dev);
+void mlx5_cleanup_reserved_gids(struct mlx5_core_dev *dev);
+int  mlx5_core_reserve_gids(struct mlx5_core_dev *dev, unsigned int count);
+void mlx5_core_unreserve_gids(struct mlx5_core_dev *dev, unsigned int count);
+int  mlx5_core_reserved_gid_alloc(struct mlx5_core_dev *dev, int *gid_index);
+void mlx5_core_reserved_gid_free(struct mlx5_core_dev *dev, int gid_index);
+
+#endif
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/main.c b/drivers/net/ethernet/mellanox/mlx5/core/main.c
index af945edfee19..c065132b956d 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/main.c
@@ -56,6 +56,9 @@
 #ifdef CONFIG_MLX5_CORE_EN
 #include "eswitch.h"
 #endif
+#include "lib/mlx5.h"
+#include "fpga/core.h"
+#include "accel/ipsec.h"
 
 MODULE_AUTHOR("Eli Cohen <eli@mellanox.com>");
 MODULE_DESCRIPTION("Mellanox Connect-IB, ConnectX-4 core driver");
@@ -175,8 +178,9 @@ static struct mlx5_profile profile[] = {
 	},
 };
 
-#define FW_INIT_TIMEOUT_MILI	2000
-#define FW_INIT_WAIT_MS		2
+#define FW_INIT_TIMEOUT_MILI		2000
+#define FW_INIT_WAIT_MS			2
+#define FW_PRE_INIT_TIMEOUT_MILI	10000
 
 static int wait_fw_init(struct mlx5_core_dev *dev, u32 max_wait_mili)
 {
@@ -355,12 +359,11 @@ static void mlx5_disable_msix(struct mlx5_core_dev *dev)
 	kfree(priv->msix_arr);
 }
 
-struct mlx5_reg_host_endianess {
+struct mlx5_reg_host_endianness {
 	u8	he;
 	u8      rsvd[15];
 };
 
-
 #define CAP_MASK(pos, size) ((u64)((1 << (size)) - 1) << (pos))
 
 enum {
@@ -474,7 +477,7 @@ static int handle_hca_cap_atomic(struct mlx5_core_dev *dev)
 
 	req_endianness =
 		MLX5_CAP_ATOMIC(dev,
-				supported_atomic_req_8B_endianess_mode_1);
+				supported_atomic_req_8B_endianness_mode_1);
 
 	if (req_endianness != MLX5_ATOMIC_REQ_MODE_HOST_ENDIANNESS)
 		return 0;
@@ -486,7 +489,7 @@ static int handle_hca_cap_atomic(struct mlx5_core_dev *dev)
 	set_hca_cap = MLX5_ADDR_OF(set_hca_cap_in, set_ctx, capability);
 
 	/* Set requestor to host endianness */
-	MLX5_SET(atomic_caps, set_hca_cap, atomic_req_8B_endianess_mode,
+	MLX5_SET(atomic_caps, set_hca_cap, atomic_req_8B_endianness_mode,
 		 MLX5_ATOMIC_REQ_MODE_HOST_ENDIANNESS);
 
 	err = set_caps(dev, set_ctx, set_sz, MLX5_SET_HCA_CAP_OP_MOD_ATOMIC);
@@ -537,8 +540,10 @@ static int handle_hca_cap(struct mlx5_core_dev *dev)
 	/* disable cmdif checksum */
 	MLX5_SET(cmd_hca_cap, set_hca_cap, cmdif_checksum, 0);
 
-	/* If the HCA supports 4K UARs use it */
-	if (MLX5_CAP_GEN_MAX(dev, uar_4k))
+	/* Enable 4K UAR only when HCA supports it and page size is bigger
+	 * than 4K.
+	 */
+	if (MLX5_CAP_GEN_MAX(dev, uar_4k) && PAGE_SIZE > 4096)
 		MLX5_SET(cmd_hca_cap, set_hca_cap, uar_4k, 1);
 
 	MLX5_SET(cmd_hca_cap, set_hca_cap, log_uar_page_sz, PAGE_SHIFT - 12);
@@ -559,8 +564,8 @@ query_ex:
 
 static int set_hca_ctrl(struct mlx5_core_dev *dev)
 {
-	struct mlx5_reg_host_endianess he_in;
-	struct mlx5_reg_host_endianess he_out;
+	struct mlx5_reg_host_endianness he_in;
+	struct mlx5_reg_host_endianness he_out;
 	int err;
 
 	if (!mlx5_core_is_pf(dev))
@@ -933,6 +938,8 @@ static int mlx5_init_once(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 
 	mlx5_init_mkey_table(dev);
 
+	mlx5_init_reserved_gids(dev);
+
 	err = mlx5_init_rl_table(dev);
 	if (err) {
 		dev_err(&pdev->dev, "Failed to init rate limiting\n");
@@ -953,8 +960,16 @@ static int mlx5_init_once(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 		goto err_eswitch_cleanup;
 	}
 
+	err = mlx5_fpga_init(dev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to init fpga device %d\n", err);
+		goto err_sriov_cleanup;
+	}
+
 	return 0;
 
+err_sriov_cleanup:
+	mlx5_sriov_cleanup(dev);
 err_eswitch_cleanup:
 #ifdef CONFIG_MLX5_CORE_EN
 	mlx5_eswitch_cleanup(dev->priv.eswitch);
@@ -978,11 +993,13 @@ out:
 
 static void mlx5_cleanup_once(struct mlx5_core_dev *dev)
 {
+	mlx5_fpga_cleanup(dev);
 	mlx5_sriov_cleanup(dev);
 #ifdef CONFIG_MLX5_CORE_EN
 	mlx5_eswitch_cleanup(dev->priv.eswitch);
 #endif
 	mlx5_cleanup_rl_table(dev);
+	mlx5_cleanup_reserved_gids(dev);
 	mlx5_cleanup_mkey_table(dev);
 	mlx5_cleanup_srq_table(dev);
 	mlx5_cleanup_qp_table(dev);
@@ -1011,6 +1028,15 @@ static int mlx5_load_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv,
 	 */
 	dev->state = MLX5_DEVICE_STATE_UP;
 
+	/* wait for firmware to accept initialization segments configurations
+	 */
+	err = wait_fw_init(dev, FW_PRE_INIT_TIMEOUT_MILI);
+	if (err) {
+		dev_err(&dev->pdev->dev, "Firmware over %d MS in pre-initializing state, aborting\n",
+			FW_PRE_INIT_TIMEOUT_MILI);
+		goto out_err;
+	}
+
 	err = mlx5_cmd_init(dev);
 	if (err) {
 		dev_err(&pdev->dev, "Failed initializing command interface, aborting\n");
@@ -1139,6 +1165,17 @@ static int mlx5_load_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv,
 		goto err_sriov;
 	}
 
+	err = mlx5_fpga_device_start(dev);
+	if (err) {
+		dev_err(&pdev->dev, "fpga device start failed %d\n", err);
+		goto err_fpga_start;
+	}
+	err = mlx5_accel_ipsec_init(dev);
+	if (err) {
+		dev_err(&pdev->dev, "IPSec device start failed %d\n", err);
+		goto err_ipsec_start;
+	}
+
 	if (mlx5_device_registered(dev)) {
 		mlx5_attach_device(dev);
 	} else {
@@ -1157,6 +1194,11 @@ out:
 	return 0;
 
 err_reg_dev:
+	mlx5_accel_ipsec_cleanup(dev);
+err_ipsec_start:
+	mlx5_fpga_device_stop(dev);
+
+err_fpga_start:
 	mlx5_sriov_detach(dev);
 
 err_sriov:
@@ -1216,7 +1258,7 @@ static int mlx5_unload_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv,
 	int err = 0;
 
 	if (cleanup)
-		mlx5_drain_health_wq(dev);
+		mlx5_drain_health_recovery(dev);
 
 	mutex_lock(&dev->intf_state_mutex);
 	if (test_bit(MLX5_INTERFACE_STATE_DOWN, &dev->intf_state)) {
@@ -1227,9 +1269,15 @@ static int mlx5_unload_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv,
 		goto out;
 	}
 
+	clear_bit(MLX5_INTERFACE_STATE_UP, &dev->intf_state);
+	set_bit(MLX5_INTERFACE_STATE_DOWN, &dev->intf_state);
+
 	if (mlx5_device_registered(dev))
 		mlx5_detach_device(dev);
 
+	mlx5_accel_ipsec_cleanup(dev);
+	mlx5_fpga_device_stop(dev);
+
 	mlx5_sriov_detach(dev);
 #ifdef CONFIG_MLX5_CORE_EN
 	mlx5_eswitch_detach(dev->priv.eswitch);
@@ -1254,8 +1302,6 @@ static int mlx5_unload_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv,
 	mlx5_cmd_cleanup(dev);
 
 out:
-	clear_bit(MLX5_INTERFACE_STATE_UP, &dev->intf_state);
-	set_bit(MLX5_INTERFACE_STATE_DOWN, &dev->intf_state);
 	mutex_unlock(&dev->intf_state_mutex);
 	return err;
 }
@@ -1400,7 +1446,7 @@ static pci_ers_result_t mlx5_pci_err_detected(struct pci_dev *pdev,
 
 	dev_info(&pdev->dev, "%s was called\n", __func__);
 
-	mlx5_enter_error_state(dev);
+	mlx5_enter_error_state(dev, false);
 	mlx5_unload_one(dev, priv, false);
 	/* In case of kernel call drain the health wq */
 	if (state) {
@@ -1487,24 +1533,52 @@ static const struct pci_error_handlers mlx5_err_handler = {
 	.resume		= mlx5_pci_resume
 };
 
+static int mlx5_try_fast_unload(struct mlx5_core_dev *dev)
+{
+	int ret;
+
+	if (!MLX5_CAP_GEN(dev, force_teardown)) {
+		mlx5_core_dbg(dev, "force teardown is not supported in the firmware\n");
+		return -EOPNOTSUPP;
+	}
+
+	if (dev->state == MLX5_DEVICE_STATE_INTERNAL_ERROR) {
+		mlx5_core_dbg(dev, "Device in internal error state, giving up\n");
+		return -EAGAIN;
+	}
+
+	ret = mlx5_cmd_force_teardown_hca(dev);
+	if (ret) {
+		mlx5_core_dbg(dev, "Firmware couldn't do fast unload error: %d\n", ret);
+		return ret;
+	}
+
+	mlx5_enter_error_state(dev, true);
+
+	return 0;
+}
+
 static void shutdown(struct pci_dev *pdev)
 {
 	struct mlx5_core_dev *dev  = pci_get_drvdata(pdev);
 	struct mlx5_priv *priv = &dev->priv;
+	int err;
 
 	dev_info(&pdev->dev, "Shutdown was called\n");
 	/* Notify mlx5 clients that the kernel is being shut down */
 	set_bit(MLX5_INTERFACE_STATE_SHUTDOWN, &dev->intf_state);
-	mlx5_unload_one(dev, priv, false);
+	err = mlx5_try_fast_unload(dev);
+	if (err)
+		mlx5_unload_one(dev, priv, false);
 	mlx5_pci_disable_device(dev);
 }
 
 static const struct pci_device_id mlx5_core_pci_table[] = {
-	{ PCI_VDEVICE(MELLANOX, 0x1011) },			/* Connect-IB */
+	{ PCI_VDEVICE(MELLANOX, PCI_DEVICE_ID_MELLANOX_CONNECTIB) },
 	{ PCI_VDEVICE(MELLANOX, 0x1012), MLX5_PCI_DEV_IS_VF},	/* Connect-IB VF */
-	{ PCI_VDEVICE(MELLANOX, 0x1013) },			/* ConnectX-4 */
+	{ PCI_VDEVICE(MELLANOX, PCI_DEVICE_ID_MELLANOX_CONNECTX4) },
 	{ PCI_VDEVICE(MELLANOX, 0x1014), MLX5_PCI_DEV_IS_VF},	/* ConnectX-4 VF */
-	{ PCI_VDEVICE(MELLANOX, 0x1015) },			/* ConnectX-4LX */
+	{ PCI_VDEVICE(MELLANOX, PCI_DEVICE_ID_MELLANOX_CONNECTX4_LX) },
 	{ PCI_VDEVICE(MELLANOX, 0x1016), MLX5_PCI_DEV_IS_VF},	/* ConnectX-4LX VF */
 	{ PCI_VDEVICE(MELLANOX, 0x1017) },			/* ConnectX-5, PCIe 3.0 */
 	{ PCI_VDEVICE(MELLANOX, 0x1018), MLX5_PCI_DEV_IS_VF},	/* ConnectX-5 VF */
@@ -1512,6 +1586,8 @@ static const struct pci_device_id mlx5_core_pci_table[] = {
 	{ PCI_VDEVICE(MELLANOX, 0x101a), MLX5_PCI_DEV_IS_VF},	/* ConnectX-5 Ex VF */
 	{ PCI_VDEVICE(MELLANOX, 0x101b) },			/* ConnectX-6 */
 	{ PCI_VDEVICE(MELLANOX, 0x101c), MLX5_PCI_DEV_IS_VF},	/* ConnectX-6 VF */
+	{ PCI_VDEVICE(MELLANOX, 0xa2d2) },			/* BlueField integrated ConnectX-5 network controller */
+	{ PCI_VDEVICE(MELLANOX, 0xa2d3), MLX5_PCI_DEV_IS_VF},	/* BlueField integrated ConnectX-5 network controller VF */
 	{ 0, }
 };
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h b/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
index fbc6e9e9e305..6a3d6bef7dd4 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
@@ -37,10 +37,10 @@
 #include <linux/kernel.h>
 #include <linux/sched.h>
 #include <linux/if_link.h>
+#include <linux/firmware.h>
 
 #define DRIVER_NAME "mlx5_core"
-#define DRIVER_VERSION "3.0-1"
-#define DRIVER_RELDATE  "January 2015"
+#define DRIVER_VERSION "5.0-0"
 
 #define MLX5_TOTAL_VPORTS(mdev) (1 + pci_sriov_get_totalvfs(mdev->pdev))
 
@@ -84,12 +84,13 @@ int mlx5_query_hca_caps(struct mlx5_core_dev *dev);
 int mlx5_query_board_id(struct mlx5_core_dev *dev);
 int mlx5_cmd_init_hca(struct mlx5_core_dev *dev);
 int mlx5_cmd_teardown_hca(struct mlx5_core_dev *dev);
+int mlx5_cmd_force_teardown_hca(struct mlx5_core_dev *dev);
 void mlx5_core_event(struct mlx5_core_dev *dev, enum mlx5_dev_event event,
 		     unsigned long param);
 void mlx5_core_page_fault(struct mlx5_core_dev *dev,
 			  struct mlx5_pagefault *pfault);
 void mlx5_port_module_event(struct mlx5_core_dev *dev, struct mlx5_eqe *eqe);
-void mlx5_enter_error_state(struct mlx5_core_dev *dev);
+void mlx5_enter_error_state(struct mlx5_core_dev *dev, bool force);
 void mlx5_disable_device(struct mlx5_core_dev *dev);
 void mlx5_recover_device(struct mlx5_core_dev *dev);
 int mlx5_sriov_init(struct mlx5_core_dev *dev);
@@ -153,6 +154,8 @@ int mlx5_set_mtpps(struct mlx5_core_dev *mdev, u32 *mtpps, u32 mtpps_size);
 int mlx5_query_mtppse(struct mlx5_core_dev *mdev, u8 pin, u8 *arm, u8 *mode);
 int mlx5_set_mtppse(struct mlx5_core_dev *mdev, u8 pin, u8 arm, u8 mode);
 
+int mlx5_firmware_flash(struct mlx5_core_dev *dev, const struct firmware *fw);
+
 void mlx5e_init(void);
 void mlx5e_cleanup(void);
 
@@ -168,4 +171,7 @@ static inline int mlx5_lag_is_lacp_owner(struct mlx5_core_dev *dev)
 		    MLX5_CAP_GEN(dev, lag_master);
 }
 
+int mlx5_lag_allow(struct mlx5_core_dev *dev);
+int mlx5_lag_forbid(struct mlx5_core_dev *dev);
+
 #endif /* __MLX5_CORE_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c b/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c
index a57d5a81eb05..e36d3e3675f9 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c
@@ -279,7 +279,7 @@ static int give_pages(struct mlx5_core_dev *dev, u16 func_id, int npages,
 	int i;
 
 	inlen += npages * MLX5_FLD_SZ_BYTES(manage_pages_in, pas[0]);
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in) {
 		err = -ENOMEM;
 		mlx5_core_warn(dev, "vzalloc failed %d\n", inlen);
@@ -376,7 +376,7 @@ static int reclaim_pages(struct mlx5_core_dev *dev, u32 func_id, int npages,
 		*nclaimed = 0;
 
 	outlen += npages * MLX5_FLD_SZ_BYTES(manage_pages_out, pas[0]);
-	out = mlx5_vzalloc(outlen);
+	out = kvzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
 
@@ -403,7 +403,6 @@ static int reclaim_pages(struct mlx5_core_dev *dev, u32 func_id, int npages,
 	for (i = 0; i < num_claimed; i++)
 		free_4k(dev, MLX5_GET64(manage_pages_out, out, pas[i]));
 
-
 	if (nclaimed)
 		*nclaimed = num_claimed;
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/port.c b/drivers/net/ethernet/mellanox/mlx5/core/port.c
index 141583daf5a2..1975d4388d4f 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/port.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/port.c
@@ -47,8 +47,8 @@ int mlx5_core_access_reg(struct mlx5_core_dev *dev, void *data_in,
 	u32 *in = NULL;
 	void *data;
 
-	in = mlx5_vzalloc(inlen);
-	out = mlx5_vzalloc(outlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
+	out = kvzalloc(outlen, GFP_KERNEL);
 	if (!in || !out)
 		goto out;
 
@@ -454,7 +454,7 @@ int mlx5_core_query_ib_ppcnt(struct mlx5_core_dev *dev,
 	u32 *in;
 	int err;
 
-	in  = mlx5_vzalloc(sz);
+	in  = kvzalloc(sz, GFP_KERNEL);
 	if (!in) {
 		err = -ENOMEM;
 		return err;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/qp.c b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
index cbbcef2884be..340f281c9801 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
@@ -30,7 +30,6 @@
  * SOFTWARE.
  */
 
-
 #include <linux/gfp.h>
 #include <linux/export.h>
 #include <linux/mlx5/cmd.h>
@@ -519,23 +518,3 @@ int mlx5_core_query_q_counter(struct mlx5_core_dev *dev, u16 counter_id,
 	return mlx5_cmd_exec(dev, in, sizeof(in), out, out_size);
 }
 EXPORT_SYMBOL_GPL(mlx5_core_query_q_counter);
-
-int mlx5_core_query_out_of_buffer(struct mlx5_core_dev *dev, u16 counter_id,
-				  u32 *out_of_buffer)
-{
-	int outlen = MLX5_ST_SZ_BYTES(query_q_counter_out);
-	void *out;
-	int err;
-
-	out = mlx5_vzalloc(outlen);
-	if (!out)
-		return -ENOMEM;
-
-	err = mlx5_core_query_q_counter(dev, counter_id, 0, out, outlen);
-	if (!err)
-		*out_of_buffer = MLX5_GET(query_q_counter_out, out,
-					  out_of_buffer);
-
-	kfree(out);
-	return err;
-}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/sriov.c b/drivers/net/ethernet/mellanox/mlx5/core/sriov.c
index e08627785590..bcdf7779c48d 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/sriov.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/sriov.c
@@ -175,15 +175,20 @@ int mlx5_core_sriov_configure(struct pci_dev *pdev, int num_vfs)
 	if (!mlx5_core_is_pf(dev))
 		return -EPERM;
 
-	if (num_vfs && mlx5_lag_is_active(dev)) {
-		mlx5_core_warn(dev, "can't turn sriov on while LAG is active");
-		return -EINVAL;
+	if (num_vfs) {
+		int ret;
+
+		ret = mlx5_lag_forbid(dev);
+		if (ret && (ret != -ENODEV))
+			return ret;
 	}
 
-	if (num_vfs)
+	if (num_vfs) {
 		err = mlx5_sriov_enable(pdev, num_vfs);
-	else
+	} else {
 		mlx5_sriov_disable(pdev);
+		mlx5_lag_allow(dev);
+	}
 
 	return err ? err : num_vfs;
 }
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/srq.c b/drivers/net/ethernet/mellanox/mlx5/core/srq.c
index 3099630015d7..f774de6f5fcb 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/srq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/srq.c
@@ -162,7 +162,7 @@ static int create_srq_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
 
 	pas_size  = get_pas_size(in);
 	inlen	  = MLX5_ST_SZ_BYTES(create_srq_in) + pas_size;
-	create_in = mlx5_vzalloc(inlen);
+	create_in = kvzalloc(inlen, GFP_KERNEL);
 	if (!create_in)
 		return -ENOMEM;
 
@@ -221,7 +221,7 @@ static int query_srq_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
 	void *srqc;
 	int err;
 
-	srq_out = mlx5_vzalloc(MLX5_ST_SZ_BYTES(query_srq_out));
+	srq_out = kvzalloc(MLX5_ST_SZ_BYTES(query_srq_out), GFP_KERNEL);
 	if (!srq_out)
 		return -ENOMEM;
 
@@ -256,7 +256,7 @@ static int create_xrc_srq_cmd(struct mlx5_core_dev *dev,
 
 	pas_size  = get_pas_size(in);
 	inlen	  = MLX5_ST_SZ_BYTES(create_xrc_srq_in) + pas_size;
-	create_in = mlx5_vzalloc(inlen);
+	create_in = kvzalloc(inlen, GFP_KERNEL);
 	if (!create_in)
 		return -ENOMEM;
 
@@ -320,7 +320,7 @@ static int query_xrc_srq_cmd(struct mlx5_core_dev *dev,
 	void *xrc_srqc;
 	int err;
 
-	xrcsrq_out = mlx5_vzalloc(MLX5_ST_SZ_BYTES(query_xrc_srq_out));
+	xrcsrq_out = kvzalloc(MLX5_ST_SZ_BYTES(query_xrc_srq_out), GFP_KERNEL);
 	if (!xrcsrq_out)
 		return -ENOMEM;
 	memset(xrcsrq_in, 0, sizeof(xrcsrq_in));
@@ -357,7 +357,7 @@ static int create_rmp_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
 
 	pas_size = get_pas_size(in);
 	inlen = MLX5_ST_SZ_BYTES(create_rmp_in) + pas_size;
-	create_in = mlx5_vzalloc(inlen);
+	create_in = kvzalloc(inlen, GFP_KERNEL);
 	if (!create_in)
 		return -ENOMEM;
 
@@ -390,7 +390,7 @@ static int arm_rmp_cmd(struct mlx5_core_dev *dev,
 	void *bitmask;
 	int err;
 
-	in = mlx5_vzalloc(MLX5_ST_SZ_BYTES(modify_rmp_in));
+	in = kvzalloc(MLX5_ST_SZ_BYTES(modify_rmp_in), GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -417,7 +417,7 @@ static int query_rmp_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
 	void *rmpc;
 	int err;
 
-	rmp_out =  mlx5_vzalloc(MLX5_ST_SZ_BYTES(query_rmp_out));
+	rmp_out =  kvzalloc(MLX5_ST_SZ_BYTES(query_rmp_out), GFP_KERNEL);
 	if (!rmp_out)
 		return -ENOMEM;
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/transobj.c b/drivers/net/ethernet/mellanox/mlx5/core/transobj.c
index a00ff49eec18..5e128d7a9ffd 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/transobj.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/transobj.c
@@ -284,7 +284,7 @@ int mlx5_core_arm_rmp(struct mlx5_core_dev *dev, u32 rmpn, u16 lwm)
 	void *bitmask;
 	int  err;
 
-	in = mlx5_vzalloc(MLX5_ST_SZ_BYTES(modify_rmp_in));
+	in = kvzalloc(MLX5_ST_SZ_BYTES(modify_rmp_in), GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/vport.c b/drivers/net/ethernet/mellanox/mlx5/core/vport.c
index 15c2294dd2b4..5abfec1c3399 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/vport.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/vport.c
@@ -172,7 +172,7 @@ int mlx5_query_nic_vport_mac_address(struct mlx5_core_dev *mdev,
 	u8 *out_addr;
 	int err;
 
-	out = mlx5_vzalloc(outlen);
+	out = kvzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
 
@@ -197,11 +197,9 @@ int mlx5_modify_nic_vport_mac_address(struct mlx5_core_dev *mdev,
 	void *nic_vport_ctx;
 	u8 *perm_mac;
 
-	in = mlx5_vzalloc(inlen);
-	if (!in) {
-		mlx5_core_warn(mdev, "failed to allocate inbox\n");
+	in = kvzalloc(inlen, GFP_KERNEL);
+	if (!in)
 		return -ENOMEM;
-	}
 
 	MLX5_SET(modify_nic_vport_context_in, in,
 		 field_select.permanent_address, 1);
@@ -231,7 +229,7 @@ int mlx5_query_nic_vport_mtu(struct mlx5_core_dev *mdev, u16 *mtu)
 	u32 *out;
 	int err;
 
-	out = mlx5_vzalloc(outlen);
+	out = kvzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
 
@@ -251,7 +249,7 @@ int mlx5_modify_nic_vport_mtu(struct mlx5_core_dev *mdev, u16 mtu)
 	void *in;
 	int err;
 
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -501,7 +499,7 @@ int mlx5_query_nic_vport_system_image_guid(struct mlx5_core_dev *mdev,
 	u32 *out;
 	int outlen = MLX5_ST_SZ_BYTES(query_nic_vport_context_out);
 
-	out = mlx5_vzalloc(outlen);
+	out = kvzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
 
@@ -521,7 +519,7 @@ int mlx5_query_nic_vport_node_guid(struct mlx5_core_dev *mdev, u64 *node_guid)
 	u32 *out;
 	int outlen = MLX5_ST_SZ_BYTES(query_nic_vport_context_out);
 
-	out = mlx5_vzalloc(outlen);
+	out = kvzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
 
@@ -551,7 +549,7 @@ int mlx5_modify_nic_vport_node_guid(struct mlx5_core_dev *mdev,
 	if (!MLX5_CAP_ESW(mdev, nic_vport_node_guid_modify))
 		return -EOPNOTSUPP;
 
-	in = mlx5_vzalloc(inlen);
+	in = kvzalloc(inlen, GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
 
@@ -577,7 +575,7 @@ int mlx5_query_nic_vport_qkey_viol_cntr(struct mlx5_core_dev *mdev,
 	u32 *out;
 	int outlen = MLX5_ST_SZ_BYTES(query_nic_vport_context_out);
 
-	out = mlx5_vzalloc(outlen);
+	out = kvzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
 
@@ -879,11 +877,9 @@ int mlx5_modify_nic_vport_promisc(struct mlx5_core_dev *mdev,
 	int inlen = MLX5_ST_SZ_BYTES(modify_nic_vport_context_in);
 	int err;
 
-	in = mlx5_vzalloc(inlen);
-	if (!in) {
-		mlx5_core_err(mdev, "failed to allocate inbox\n");
+	in = kvzalloc(inlen, GFP_KERNEL);
+	if (!in)
 		return -ENOMEM;
-	}
 
 	MLX5_SET(modify_nic_vport_context_in, in, field_select.promisc, 1);
 	MLX5_SET(modify_nic_vport_context_in, in,
@@ -913,11 +909,9 @@ static int mlx5_nic_vport_update_roce_state(struct mlx5_core_dev *mdev,
 	int inlen = MLX5_ST_SZ_BYTES(modify_nic_vport_context_in);
 	int err;
 
-	in = mlx5_vzalloc(inlen);
-	if (!in) {
-		mlx5_core_warn(mdev, "failed to allocate inbox\n");
+	in = kvzalloc(inlen, GFP_KERNEL);
+	if (!in)
 		return -ENOMEM;
-	}
 
 	MLX5_SET(modify_nic_vport_context_in, in, field_select.roce_en, 1);
 	MLX5_SET(modify_nic_vport_context_in, in, nic_vport_context.roce_en,
@@ -932,12 +926,16 @@ static int mlx5_nic_vport_update_roce_state(struct mlx5_core_dev *mdev,
 
 int mlx5_nic_vport_enable_roce(struct mlx5_core_dev *mdev)
 {
+	if (atomic_inc_return(&mdev->roce.roce_en) != 1)
+		return 0;
 	return mlx5_nic_vport_update_roce_state(mdev, MLX5_VPORT_ROCE_ENABLED);
 }
 EXPORT_SYMBOL_GPL(mlx5_nic_vport_enable_roce);
 
 int mlx5_nic_vport_disable_roce(struct mlx5_core_dev *mdev)
 {
+	if (atomic_dec_return(&mdev->roce.roce_en) != 0)
+		return 0;
 	return mlx5_nic_vport_update_roce_state(mdev, MLX5_VPORT_ROCE_DISABLED);
 }
 EXPORT_SYMBOL_GPL(mlx5_nic_vport_disable_roce);
@@ -952,7 +950,7 @@ int mlx5_core_query_vport_counter(struct mlx5_core_dev *dev, u8 other_vport,
 	int	err;
 
 	is_group_manager = MLX5_CAP_GEN(dev, vport_group_manager);
-	in = mlx5_vzalloc(in_sz);
+	in = kvzalloc(in_sz, GFP_KERNEL);
 	if (!in) {
 		err = -ENOMEM;
 		return err;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/wq.c b/drivers/net/ethernet/mellanox/mlx5/core/wq.c
index 921673c42bc9..6bcfc25350f5 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/wq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/wq.c
@@ -54,6 +54,12 @@ static u32 mlx5_wq_cyc_get_byte_size(struct mlx5_wq_cyc *wq)
 	return mlx5_wq_cyc_get_size(wq) << wq->log_stride;
 }
 
+static u32 mlx5_wq_qp_get_byte_size(struct mlx5_wq_qp *wq)
+{
+	return mlx5_wq_cyc_get_byte_size(&wq->rq) +
+	       mlx5_wq_cyc_get_byte_size(&wq->sq);
+}
+
 static u32 mlx5_cqwq_get_byte_size(struct mlx5_cqwq *wq)
 {
 	return mlx5_cqwq_get_size(wq) << wq->log_stride;
@@ -99,6 +105,46 @@ err_db_free:
 	return err;
 }
 
+int mlx5_wq_qp_create(struct mlx5_core_dev *mdev, struct mlx5_wq_param *param,
+		      void *qpc, struct mlx5_wq_qp *wq,
+		      struct mlx5_wq_ctrl *wq_ctrl)
+{
+	int err;
+
+	wq->rq.log_stride = MLX5_GET(qpc, qpc, log_rq_stride) + 4;
+	wq->rq.sz_m1 = (1 << MLX5_GET(qpc, qpc, log_rq_size)) - 1;
+
+	wq->sq.log_stride = ilog2(MLX5_SEND_WQE_BB);
+	wq->sq.sz_m1 = (1 << MLX5_GET(qpc, qpc, log_sq_size)) - 1;
+
+	err = mlx5_db_alloc_node(mdev, &wq_ctrl->db, param->db_numa_node);
+	if (err) {
+		mlx5_core_warn(mdev, "mlx5_db_alloc_node() failed, %d\n", err);
+		return err;
+	}
+
+	err = mlx5_buf_alloc_node(mdev, mlx5_wq_qp_get_byte_size(wq),
+				  &wq_ctrl->buf, param->buf_numa_node);
+	if (err) {
+		mlx5_core_warn(mdev, "mlx5_buf_alloc_node() failed, %d\n", err);
+		goto err_db_free;
+	}
+
+	wq->rq.buf = wq_ctrl->buf.direct.buf;
+	wq->sq.buf = wq->rq.buf + mlx5_wq_cyc_get_byte_size(&wq->rq);
+	wq->rq.db  = &wq_ctrl->db.db[MLX5_RCV_DBR];
+	wq->sq.db  = &wq_ctrl->db.db[MLX5_SND_DBR];
+
+	wq_ctrl->mdev = mdev;
+
+	return 0;
+
+err_db_free:
+	mlx5_db_free(mdev, &wq_ctrl->db);
+
+	return err;
+}
+
 int mlx5_cqwq_create(struct mlx5_core_dev *mdev, struct mlx5_wq_param *param,
 		     void *cqc, struct mlx5_cqwq *wq,
 		     struct mlx5_frag_wq_ctrl *wq_ctrl)
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/wq.h b/drivers/net/ethernet/mellanox/mlx5/core/wq.h
index d8afed898c31..718589d0cec2 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/wq.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/wq.h
@@ -34,6 +34,8 @@
 #define __MLX5_WQ_H__
 
 #include <linux/mlx5/mlx5_ifc.h>
+#include <linux/mlx5/cq.h>
+#include <linux/mlx5/qp.h>
 
 struct mlx5_wq_param {
 	int		linear;
@@ -60,6 +62,11 @@ struct mlx5_wq_cyc {
 	u8			log_stride;
 };
 
+struct mlx5_wq_qp {
+	struct mlx5_wq_cyc	rq;
+	struct mlx5_wq_cyc	sq;
+};
+
 struct mlx5_cqwq {
 	struct mlx5_frag_buf	frag_buf;
 	__be32			*db;
@@ -87,6 +94,10 @@ int mlx5_wq_cyc_create(struct mlx5_core_dev *mdev, struct mlx5_wq_param *param,
 		       struct mlx5_wq_ctrl *wq_ctrl);
 u32 mlx5_wq_cyc_get_size(struct mlx5_wq_cyc *wq);
 
+int mlx5_wq_qp_create(struct mlx5_core_dev *mdev, struct mlx5_wq_param *param,
+		      void *qpc, struct mlx5_wq_qp *wq,
+		      struct mlx5_wq_ctrl *wq_ctrl);
+
 int mlx5_cqwq_create(struct mlx5_core_dev *mdev, struct mlx5_wq_param *param,
 		     void *cqc, struct mlx5_cqwq *wq,
 		     struct mlx5_frag_wq_ctrl *wq_ctrl);
@@ -146,6 +157,22 @@ static inline void mlx5_cqwq_update_db_record(struct mlx5_cqwq *wq)
 	*wq->db = cpu_to_be32(wq->cc & 0xffffff);
 }
 
+static inline struct mlx5_cqe64 *mlx5_cqwq_get_cqe(struct mlx5_cqwq *wq)
+{
+	u32 ci = mlx5_cqwq_get_ci(wq);
+	struct mlx5_cqe64 *cqe = mlx5_cqwq_get_wqe(wq, ci);
+	u8 cqe_ownership_bit = cqe->op_own & MLX5_CQE_OWNER_MASK;
+	u8 sw_ownership_val = mlx5_cqwq_get_wrap_cnt(wq) & 1;
+
+	if (cqe_ownership_bit != sw_ownership_val)
+		return NULL;
+
+	/* ensure cqe content is read after cqe ownership bit */
+	dma_rmb();
+
+	return cqe;
+}
+
 static inline int mlx5_wq_ll_is_full(struct mlx5_wq_ll *wq)
 {
 	return wq->cur_sz == wq->sz_m1;
diff --git a/drivers/net/ethernet/mellanox/mlxfw/Kconfig b/drivers/net/ethernet/mellanox/mlxfw/Kconfig
new file mode 100644
index 000000000000..186ebe783f97
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/Kconfig
@@ -0,0 +1,13 @@
+#
+# Mellanox firmware flash library configuration
+#
+
+config MLXFW
+	tristate "Mellanox Technologies firmware flash module"
+	---help---
+	  This driver supports Mellanox Technologies Firmware
+	  flashing common logic.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mlxfw.
+	select XZ_DEC
diff --git a/drivers/net/ethernet/mellanox/mlxfw/Makefile b/drivers/net/ethernet/mellanox/mlxfw/Makefile
new file mode 100644
index 000000000000..7448b301104c
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_MLXFW)	+= mlxfw.o
+mlxfw-objs		:= mlxfw_fsm.o mlxfw_mfa2_tlv_multi.o mlxfw_mfa2.o
diff --git a/drivers/net/ethernet/mellanox/mlxfw/mlxfw.h b/drivers/net/ethernet/mellanox/mlxfw/mlxfw.h
new file mode 100644
index 000000000000..7a712b6b09ec
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/mlxfw.h
@@ -0,0 +1,111 @@
+/*
+ * drivers/net/ethernet/mellanox/mlxfw/mlxfw.h
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2017 Yotam Gigi <yotamg@mellanox.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the names of the copyright holders nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _MLXFW_H
+#define _MLXFW_H
+
+#include <linux/firmware.h>
+
+enum mlxfw_fsm_state {
+	MLXFW_FSM_STATE_IDLE,
+	MLXFW_FSM_STATE_LOCKED,
+	MLXFW_FSM_STATE_INITIALIZE,
+	MLXFW_FSM_STATE_DOWNLOAD,
+	MLXFW_FSM_STATE_VERIFY,
+	MLXFW_FSM_STATE_APPLY,
+	MLXFW_FSM_STATE_ACTIVATE,
+};
+
+enum mlxfw_fsm_state_err {
+	MLXFW_FSM_STATE_ERR_OK,
+	MLXFW_FSM_STATE_ERR_ERROR,
+	MLXFW_FSM_STATE_ERR_REJECTED_DIGEST_ERR,
+	MLXFW_FSM_STATE_ERR_REJECTED_NOT_APPLICABLE,
+	MLXFW_FSM_STATE_ERR_REJECTED_UNKNOWN_KEY,
+	MLXFW_FSM_STATE_ERR_REJECTED_AUTH_FAILED,
+	MLXFW_FSM_STATE_ERR_REJECTED_UNSIGNED,
+	MLXFW_FSM_STATE_ERR_REJECTED_KEY_NOT_APPLICABLE,
+	MLXFW_FSM_STATE_ERR_REJECTED_BAD_FORMAT,
+	MLXFW_FSM_STATE_ERR_BLOCKED_PENDING_RESET,
+	MLXFW_FSM_STATE_ERR_MAX,
+};
+
+struct mlxfw_dev;
+
+struct mlxfw_dev_ops {
+	int (*component_query)(struct mlxfw_dev *mlxfw_dev, u16 component_index,
+			       u32 *p_max_size, u8 *p_align_bits,
+			       u16 *p_max_write_size);
+
+	int (*fsm_lock)(struct mlxfw_dev *mlxfw_dev, u32 *fwhandle);
+
+	int (*fsm_component_update)(struct mlxfw_dev *mlxfw_dev, u32 fwhandle,
+				    u16 component_index, u32 component_size);
+
+	int (*fsm_block_download)(struct mlxfw_dev *mlxfw_dev, u32 fwhandle,
+				  u8 *data, u16 size, u32 offset);
+
+	int (*fsm_component_verify)(struct mlxfw_dev *mlxfw_dev, u32 fwhandle,
+				    u16 component_index);
+
+	int (*fsm_activate)(struct mlxfw_dev *mlxfw_dev, u32 fwhandle);
+
+	int (*fsm_query_state)(struct mlxfw_dev *mlxfw_dev, u32 fwhandle,
+			       enum mlxfw_fsm_state *fsm_state,
+			       enum mlxfw_fsm_state_err *fsm_state_err);
+
+	void (*fsm_cancel)(struct mlxfw_dev *mlxfw_dev, u32 fwhandle);
+
+	void (*fsm_release)(struct mlxfw_dev *mlxfw_dev, u32 fwhandle);
+};
+
+struct mlxfw_dev {
+	const struct mlxfw_dev_ops *ops;
+	const char *psid;
+	u16 psid_size;
+};
+
+#if IS_REACHABLE(CONFIG_MLXFW)
+int mlxfw_firmware_flash(struct mlxfw_dev *mlxfw_dev,
+			 const struct firmware *firmware);
+#else
+static inline
+int mlxfw_firmware_flash(struct mlxfw_dev *mlxfw_dev,
+			 const struct firmware *firmware)
+{
+	return -EOPNOTSUPP;
+}
+#endif
+
+#endif
diff --git a/drivers/net/ethernet/mellanox/mlxfw/mlxfw_fsm.c b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_fsm.c
new file mode 100644
index 000000000000..2cf89126fb23
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_fsm.c
@@ -0,0 +1,273 @@
+/*
+ * drivers/net/ethernet/mellanox/mlxfw/mlxfw.c
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2017 Yotam Gigi <yotamg@mellanox.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the names of the copyright holders nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#define pr_fmt(fmt) "mlxfw: " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+
+#include "mlxfw.h"
+#include "mlxfw_mfa2.h"
+
+#define MLXFW_FSM_STATE_WAIT_CYCLE_MS 200
+#define MLXFW_FSM_STATE_WAIT_TIMEOUT_MS 30000
+#define MLXFW_FSM_STATE_WAIT_ROUNDS \
+	(MLXFW_FSM_STATE_WAIT_TIMEOUT_MS / MLXFW_FSM_STATE_WAIT_CYCLE_MS)
+#define MLXFW_FSM_MAX_COMPONENT_SIZE (10 * (1 << 20))
+
+static const char * const mlxfw_fsm_state_err_str[] = {
+	[MLXFW_FSM_STATE_ERR_ERROR] =
+		"general error",
+	[MLXFW_FSM_STATE_ERR_REJECTED_DIGEST_ERR] =
+		"component hash mismatch",
+	[MLXFW_FSM_STATE_ERR_REJECTED_NOT_APPLICABLE] =
+		"component not applicable",
+	[MLXFW_FSM_STATE_ERR_REJECTED_UNKNOWN_KEY] =
+		"unknown key",
+	[MLXFW_FSM_STATE_ERR_REJECTED_AUTH_FAILED] =
+		"authentication failed",
+	[MLXFW_FSM_STATE_ERR_REJECTED_UNSIGNED] =
+		"component was not signed",
+	[MLXFW_FSM_STATE_ERR_REJECTED_KEY_NOT_APPLICABLE] =
+		"key not applicable",
+	[MLXFW_FSM_STATE_ERR_REJECTED_BAD_FORMAT] =
+		"bad format",
+	[MLXFW_FSM_STATE_ERR_BLOCKED_PENDING_RESET] =
+		"pending reset",
+	[MLXFW_FSM_STATE_ERR_MAX] =
+		"unknown error"
+};
+
+static int mlxfw_fsm_state_wait(struct mlxfw_dev *mlxfw_dev, u32 fwhandle,
+				enum mlxfw_fsm_state fsm_state)
+{
+	enum mlxfw_fsm_state_err fsm_state_err;
+	enum mlxfw_fsm_state curr_fsm_state;
+	int times;
+	int err;
+
+	times = MLXFW_FSM_STATE_WAIT_ROUNDS;
+retry:
+	err = mlxfw_dev->ops->fsm_query_state(mlxfw_dev, fwhandle,
+					      &curr_fsm_state, &fsm_state_err);
+	if (err)
+		return err;
+
+	if (fsm_state_err != MLXFW_FSM_STATE_ERR_OK) {
+		pr_err("Firmware flash failed: %s\n",
+		       mlxfw_fsm_state_err_str[fsm_state_err]);
+		return -EINVAL;
+	}
+	if (curr_fsm_state != fsm_state) {
+		if (--times == 0) {
+			pr_err("Timeout reached on FSM state change");
+			return -ETIMEDOUT;
+		}
+		msleep(MLXFW_FSM_STATE_WAIT_CYCLE_MS);
+		goto retry;
+	}
+	return 0;
+}
+
+#define MLXFW_ALIGN_DOWN(x, align_bits) ((x) & ~((1 << (align_bits)) - 1))
+#define MLXFW_ALIGN_UP(x, align_bits) \
+		MLXFW_ALIGN_DOWN((x) + ((1 << (align_bits)) - 1), (align_bits))
+
+static int mlxfw_flash_component(struct mlxfw_dev *mlxfw_dev,
+				 u32 fwhandle,
+				 struct mlxfw_mfa2_component *comp)
+{
+	u16 comp_max_write_size;
+	u8 comp_align_bits;
+	u32 comp_max_size;
+	u16 block_size;
+	u8 *block_ptr;
+	u32 offset;
+	int err;
+
+	err = mlxfw_dev->ops->component_query(mlxfw_dev, comp->index,
+					      &comp_max_size, &comp_align_bits,
+					      &comp_max_write_size);
+	if (err)
+		return err;
+
+	comp_max_size = min_t(u32, comp_max_size, MLXFW_FSM_MAX_COMPONENT_SIZE);
+	if (comp->data_size > comp_max_size) {
+		pr_err("Component %d is of size %d which is bigger than limit %d\n",
+		       comp->index, comp->data_size, comp_max_size);
+		return -EINVAL;
+	}
+
+	comp_max_write_size = MLXFW_ALIGN_DOWN(comp_max_write_size,
+					       comp_align_bits);
+
+	pr_debug("Component update\n");
+	err = mlxfw_dev->ops->fsm_component_update(mlxfw_dev, fwhandle,
+						   comp->index,
+						   comp->data_size);
+	if (err)
+		return err;
+
+	err = mlxfw_fsm_state_wait(mlxfw_dev, fwhandle,
+				   MLXFW_FSM_STATE_DOWNLOAD);
+	if (err)
+		goto err_out;
+
+	pr_debug("Component download\n");
+	for (offset = 0;
+	     offset < MLXFW_ALIGN_UP(comp->data_size, comp_align_bits);
+	     offset += comp_max_write_size) {
+		block_ptr = comp->data + offset;
+		block_size = (u16) min_t(u32, comp->data_size - offset,
+					 comp_max_write_size);
+		err = mlxfw_dev->ops->fsm_block_download(mlxfw_dev, fwhandle,
+							 block_ptr, block_size,
+							 offset);
+		if (err)
+			goto err_out;
+	}
+
+	pr_debug("Component verify\n");
+	err = mlxfw_dev->ops->fsm_component_verify(mlxfw_dev, fwhandle,
+						   comp->index);
+	if (err)
+		goto err_out;
+
+	err = mlxfw_fsm_state_wait(mlxfw_dev, fwhandle, MLXFW_FSM_STATE_LOCKED);
+	if (err)
+		goto err_out;
+	return 0;
+
+err_out:
+	mlxfw_dev->ops->fsm_cancel(mlxfw_dev, fwhandle);
+	return err;
+}
+
+static int mlxfw_flash_components(struct mlxfw_dev *mlxfw_dev, u32 fwhandle,
+				  struct mlxfw_mfa2_file *mfa2_file)
+{
+	u32 component_count;
+	int err;
+	int i;
+
+	err = mlxfw_mfa2_file_component_count(mfa2_file, mlxfw_dev->psid,
+					      mlxfw_dev->psid_size,
+					      &component_count);
+	if (err) {
+		pr_err("Could not find device PSID in MFA2 file\n");
+		return err;
+	}
+
+	for (i = 0; i < component_count; i++) {
+		struct mlxfw_mfa2_component *comp;
+
+		comp = mlxfw_mfa2_file_component_get(mfa2_file, mlxfw_dev->psid,
+						     mlxfw_dev->psid_size, i);
+		if (IS_ERR(comp))
+			return PTR_ERR(comp);
+
+		pr_info("Flashing component type %d\n", comp->index);
+		err = mlxfw_flash_component(mlxfw_dev, fwhandle, comp);
+		mlxfw_mfa2_file_component_put(comp);
+		if (err)
+			return err;
+	}
+	return 0;
+}
+
+int mlxfw_firmware_flash(struct mlxfw_dev *mlxfw_dev,
+			 const struct firmware *firmware)
+{
+	struct mlxfw_mfa2_file *mfa2_file;
+	u32 fwhandle;
+	int err;
+
+	if (!mlxfw_mfa2_check(firmware)) {
+		pr_err("Firmware file is not MFA2\n");
+		return -EINVAL;
+	}
+
+	mfa2_file = mlxfw_mfa2_file_init(firmware);
+	if (IS_ERR(mfa2_file))
+		return PTR_ERR(mfa2_file);
+
+	pr_info("Initialize firmware flash process\n");
+	err = mlxfw_dev->ops->fsm_lock(mlxfw_dev, &fwhandle);
+	if (err) {
+		pr_err("Could not lock the firmware FSM\n");
+		goto err_fsm_lock;
+	}
+
+	err = mlxfw_fsm_state_wait(mlxfw_dev, fwhandle,
+				   MLXFW_FSM_STATE_LOCKED);
+	if (err)
+		goto err_state_wait_idle_to_locked;
+
+	err = mlxfw_flash_components(mlxfw_dev, fwhandle, mfa2_file);
+	if (err)
+		goto err_flash_components;
+
+	pr_debug("Activate image\n");
+	err = mlxfw_dev->ops->fsm_activate(mlxfw_dev, fwhandle);
+	if (err) {
+		pr_err("Could not activate the downloaded image\n");
+		goto err_fsm_activate;
+	}
+
+	err = mlxfw_fsm_state_wait(mlxfw_dev, fwhandle, MLXFW_FSM_STATE_LOCKED);
+	if (err)
+		goto err_state_wait_activate_to_locked;
+
+	pr_debug("Handle release\n");
+	mlxfw_dev->ops->fsm_release(mlxfw_dev, fwhandle);
+
+	pr_info("Firmware flash done.\n");
+	mlxfw_mfa2_file_fini(mfa2_file);
+	return 0;
+
+err_state_wait_activate_to_locked:
+err_fsm_activate:
+err_flash_components:
+err_state_wait_idle_to_locked:
+	mlxfw_dev->ops->fsm_release(mlxfw_dev, fwhandle);
+err_fsm_lock:
+	mlxfw_mfa2_file_fini(mfa2_file);
+	return err;
+}
+EXPORT_SYMBOL(mlxfw_firmware_flash);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("Yotam Gigi <yotamg@mellanox.com>");
+MODULE_DESCRIPTION("Mellanox firmware flash lib");
diff --git a/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2.c b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2.c
new file mode 100644
index 000000000000..993cb5ba934e
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2.c
@@ -0,0 +1,619 @@
+/*
+ * drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2.c
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2017 Yotam Gigi <yotamg@mellanox.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the names of the copyright holders nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#define pr_fmt(fmt) "mlxfw_mfa2: " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netlink.h>
+#include <linux/xz.h>
+#include "mlxfw_mfa2.h"
+#include "mlxfw_mfa2_file.h"
+#include "mlxfw_mfa2_tlv.h"
+#include "mlxfw_mfa2_format.h"
+#include "mlxfw_mfa2_tlv_multi.h"
+
+/*               MFA2 FILE
+ *  +----------------------------------+
+ *  |        MFA2 finger print         |
+ *  +----------------------------------+
+ *  |   package descriptor multi_tlv   |
+ *  | +------------------------------+ |     +-----------------+
+ *  | |    package descriptor tlv    +-----> |num_devices=n    |
+ *  | +------------------------------+ |     |num_components=m |
+ *  +----------------------------------+     |CB offset        |
+ *  |    device descriptor multi_tlv   |     |...              |
+ *  | +------------------------------+ |     |                 |
+ *  | |           PSID tlv           | |     +-----------------+
+ *  | +------------------------------+ |
+ *  | |     component index tlv      | |
+ *  | +------------------------------+ |
+ *  +----------------------------------+
+ *  |  component descriptor multi_tlv  |
+ *  | +------------------------------+ |     +-----------------+
+ *  | |  component descriptor tlv    +-----> |Among others:    |
+ *  | +------------------------------+ |     |CB offset=o      |
+ *  +----------------------------------+     |comp index=i     |
+ *  |                                  |     |...              |
+ *  |                                  |     |                 |
+ *  |                                  |     +-----------------+
+ *  |        COMPONENT BLOCK (CB)      |
+ *  |                                  |
+ *  |                                  |
+ *  |                                  |
+ *  +----------------------------------+
+ *
+ * On the top level, an MFA2 file contains:
+ *  - Fingerprint
+ *  - Several multi_tlvs (TLVs of type MLXFW_MFA2_TLV_MULTI, as defined in
+ *    mlxfw_mfa2_format.h)
+ *  - Compresses content block
+ *
+ * The first multi_tlv
+ * -------------------
+ * The first multi TLV is treated as package descriptor, and expected to have a
+ * first TLV child of type MLXFW_MFA2_TLV_PACKAGE_DESCRIPTOR which contains all
+ * the global information needed to parse the file. Among others, it contains
+ * the number of device descriptors and component descriptor following this
+ * multi TLV.
+ *
+ * The device descriptor multi_tlv
+ * -------------------------------
+ * The multi TLVs following the package descriptor are treated as device
+ * descriptor, and are expected to have the following children:
+ *  - PSID TLV child of type MLXFW_MFA2_TLV_PSID containing that device PSID.
+ *  - Component index of type MLXFW_MFA2_TLV_COMPONENT_PTR that contains that
+ *    device component index.
+ *
+ * The component descriptor multi_tlv
+ * ----------------------------------
+ * The multi TLVs following the device descriptor multi TLVs are treated as
+ * component descriptor, and are expected to have a first child of type
+ * MLXFW_MFA2_TLV_COMPONENT_DESCRIPTOR that contains mostly the component index,
+ * needed for the flash process and the offset to the binary within the
+ * component block.
+ */
+
+static const u8 mlxfw_mfa2_fingerprint[] = "MLNX.MFA2.XZ.00!";
+static const int mlxfw_mfa2_fingerprint_len =
+			sizeof(mlxfw_mfa2_fingerprint) - 1;
+
+static const u8 mlxfw_mfa2_comp_magic[] = "#BIN.COMPONENT!#";
+static const int mlxfw_mfa2_comp_magic_len = sizeof(mlxfw_mfa2_comp_magic) - 1;
+
+bool mlxfw_mfa2_check(const struct firmware *fw)
+{
+	if (fw->size < sizeof(mlxfw_mfa2_fingerprint))
+		return false;
+
+	return memcmp(fw->data, mlxfw_mfa2_fingerprint,
+		      mlxfw_mfa2_fingerprint_len) == 0;
+}
+
+static bool
+mlxfw_mfa2_tlv_multi_validate(const struct mlxfw_mfa2_file *mfa2_file,
+			      const struct mlxfw_mfa2_tlv_multi *multi)
+{
+	const struct mlxfw_mfa2_tlv *tlv;
+	u16 idx;
+
+	/* Check that all children are valid */
+	mlxfw_mfa2_tlv_multi_foreach(mfa2_file, tlv, idx, multi) {
+		if (!tlv) {
+			pr_err("Multi has invalid child");
+			return false;
+		}
+	}
+	return true;
+}
+
+static bool
+mlxfw_mfa2_file_dev_validate(const struct mlxfw_mfa2_file *mfa2_file,
+			     const struct mlxfw_mfa2_tlv *dev_tlv,
+			     u16 dev_idx)
+{
+	const struct mlxfw_mfa2_tlv_component_ptr *cptr;
+	const struct mlxfw_mfa2_tlv_multi *multi;
+	const struct mlxfw_mfa2_tlv_psid *psid;
+	const struct mlxfw_mfa2_tlv *tlv;
+	u16 cptr_count;
+	u16 cptr_idx;
+	int err;
+
+	pr_debug("Device %d\n", dev_idx);
+
+	multi = mlxfw_mfa2_tlv_multi_get(mfa2_file, dev_tlv);
+	if (!multi) {
+		pr_err("Device %d is not a valid TLV error\n", dev_idx);
+		return false;
+	}
+
+	if (!mlxfw_mfa2_tlv_multi_validate(mfa2_file, multi))
+		return false;
+
+	/* Validate the device has PSID tlv */
+	tlv = mlxfw_mfa2_tlv_multi_child_find(mfa2_file, multi,
+					      MLXFW_MFA2_TLV_PSID, 0);
+	if (!tlv) {
+		pr_err("Device %d does not have PSID\n", dev_idx);
+		return false;
+	}
+
+	psid = mlxfw_mfa2_tlv_psid_get(mfa2_file, tlv);
+	if (!psid) {
+		pr_err("Device %d PSID TLV is not valid\n", dev_idx);
+		return false;
+	}
+
+	print_hex_dump_debug("  -- Device PSID ", DUMP_PREFIX_NONE, 16, 16,
+			     psid->psid, be16_to_cpu(tlv->len), true);
+
+	/* Validate the device has COMPONENT_PTR */
+	err = mlxfw_mfa2_tlv_multi_child_count(mfa2_file, multi,
+					       MLXFW_MFA2_TLV_COMPONENT_PTR,
+					       &cptr_count);
+	if (err)
+		return false;
+
+	if (cptr_count == 0) {
+		pr_err("Device %d has no components\n", dev_idx);
+		return false;
+	}
+
+	for (cptr_idx = 0; cptr_idx < cptr_count; cptr_idx++) {
+		tlv = mlxfw_mfa2_tlv_multi_child_find(mfa2_file, multi,
+						      MLXFW_MFA2_TLV_COMPONENT_PTR,
+						      cptr_idx);
+		if (!tlv)
+			return false;
+
+		cptr = mlxfw_mfa2_tlv_component_ptr_get(mfa2_file, tlv);
+		if (!cptr) {
+			pr_err("Device %d COMPONENT_PTR TLV is not valid\n",
+			       dev_idx);
+			return false;
+		}
+
+		pr_debug("  -- Component index %d\n",
+			 be16_to_cpu(cptr->component_index));
+	}
+	return true;
+}
+
+static bool
+mlxfw_mfa2_file_comp_validate(const struct mlxfw_mfa2_file *mfa2_file,
+			      const struct mlxfw_mfa2_tlv *comp_tlv,
+			      u16 comp_idx)
+{
+	const struct mlxfw_mfa2_tlv_component_descriptor *cdesc;
+	const struct mlxfw_mfa2_tlv_multi *multi;
+	const struct mlxfw_mfa2_tlv *tlv;
+
+	pr_debug("Component %d\n", comp_idx);
+
+	multi = mlxfw_mfa2_tlv_multi_get(mfa2_file, comp_tlv);
+	if (!multi) {
+		pr_err("Component %d is not a valid TLV error\n", comp_idx);
+		return false;
+	}
+
+	if (!mlxfw_mfa2_tlv_multi_validate(mfa2_file, multi))
+		return false;
+
+	/* Check that component have COMPONENT_DESCRIPTOR as first child */
+	tlv = mlxfw_mfa2_tlv_multi_child(mfa2_file, multi);
+	if (!tlv) {
+		pr_err("Component descriptor %d multi TLV error\n", comp_idx);
+		return false;
+	}
+
+	cdesc = mlxfw_mfa2_tlv_component_descriptor_get(mfa2_file, tlv);
+	if (!cdesc) {
+		pr_err("Component %d does not have a valid descriptor\n",
+		       comp_idx);
+		return false;
+	}
+	pr_debug("  -- Component type %d\n", be16_to_cpu(cdesc->identifier));
+	pr_debug("  -- Offset 0x%llx and size %d\n",
+		 ((u64) be32_to_cpu(cdesc->cb_offset_h) << 32)
+		 | be32_to_cpu(cdesc->cb_offset_l), be32_to_cpu(cdesc->size));
+
+	return true;
+}
+
+static bool mlxfw_mfa2_file_validate(const struct mlxfw_mfa2_file *mfa2_file)
+{
+	const struct mlxfw_mfa2_tlv *tlv;
+	u16 idx;
+
+	pr_debug("Validating file\n");
+
+	/* check that all the devices exist */
+	mlxfw_mfa2_tlv_foreach(mfa2_file, tlv, idx, mfa2_file->first_dev,
+			       mfa2_file->dev_count) {
+		if (!tlv) {
+			pr_err("Device TLV error\n");
+			return false;
+		}
+
+		/* Check each device */
+		if (!mlxfw_mfa2_file_dev_validate(mfa2_file, tlv, idx))
+			return false;
+	}
+
+	/* check that all the components exist */
+	mlxfw_mfa2_tlv_foreach(mfa2_file, tlv, idx, mfa2_file->first_component,
+			       mfa2_file->component_count) {
+		if (!tlv) {
+			pr_err("Device TLV error\n");
+			return false;
+		}
+
+		/* Check each component */
+		if (!mlxfw_mfa2_file_comp_validate(mfa2_file, tlv, idx))
+			return false;
+	}
+	return true;
+}
+
+struct mlxfw_mfa2_file *mlxfw_mfa2_file_init(const struct firmware *fw)
+{
+	const struct mlxfw_mfa2_tlv_package_descriptor *pd;
+	const struct mlxfw_mfa2_tlv_multi *multi;
+	const struct mlxfw_mfa2_tlv *multi_child;
+	const struct mlxfw_mfa2_tlv *first_tlv;
+	struct mlxfw_mfa2_file *mfa2_file;
+	const void *first_tlv_ptr;
+	const void *cb_top_ptr;
+
+	mfa2_file = kcalloc(1, sizeof(*mfa2_file), GFP_KERNEL);
+	if (!mfa2_file)
+		return ERR_PTR(-ENOMEM);
+
+	mfa2_file->fw = fw;
+	first_tlv_ptr = fw->data + NLA_ALIGN(mlxfw_mfa2_fingerprint_len);
+	first_tlv = mlxfw_mfa2_tlv_get(mfa2_file, first_tlv_ptr);
+	if (!first_tlv) {
+		pr_err("Could not parse package descriptor TLV\n");
+		goto err_out;
+	}
+
+	multi = mlxfw_mfa2_tlv_multi_get(mfa2_file, first_tlv);
+	if (!multi) {
+		pr_err("First TLV is not of valid multi type\n");
+		goto err_out;
+	}
+
+	multi_child = mlxfw_mfa2_tlv_multi_child(mfa2_file, multi);
+	if (!multi_child)
+		goto err_out;
+
+	pd = mlxfw_mfa2_tlv_package_descriptor_get(mfa2_file, multi_child);
+	if (!pd) {
+		pr_err("Could not parse package descriptor TLV\n");
+		goto err_out;
+	}
+
+	mfa2_file->first_dev = mlxfw_mfa2_tlv_next(mfa2_file, first_tlv);
+	if (!mfa2_file->first_dev) {
+		pr_err("First device TLV is not valid\n");
+		goto err_out;
+	}
+
+	mfa2_file->dev_count = be16_to_cpu(pd->num_devices);
+	mfa2_file->first_component = mlxfw_mfa2_tlv_advance(mfa2_file,
+							    mfa2_file->first_dev,
+							    mfa2_file->dev_count);
+	mfa2_file->component_count = be16_to_cpu(pd->num_components);
+	mfa2_file->cb = fw->data + NLA_ALIGN(be32_to_cpu(pd->cb_offset));
+	if (!mlxfw_mfa2_valid_ptr(mfa2_file, mfa2_file->cb)) {
+		pr_err("Component block is out side the file\n");
+		goto err_out;
+	}
+	mfa2_file->cb_archive_size = be32_to_cpu(pd->cb_archive_size);
+	cb_top_ptr = mfa2_file->cb + mfa2_file->cb_archive_size - 1;
+	if (!mlxfw_mfa2_valid_ptr(mfa2_file, cb_top_ptr)) {
+		pr_err("Component block size is too big\n");
+		goto err_out;
+	}
+
+	if (!mlxfw_mfa2_file_validate(mfa2_file))
+		goto err_out;
+	return mfa2_file;
+err_out:
+	kfree(mfa2_file);
+	return ERR_PTR(-EINVAL);
+}
+
+static const struct mlxfw_mfa2_tlv_multi *
+mlxfw_mfa2_tlv_dev_get(const struct mlxfw_mfa2_file *mfa2_file,
+		       const char *psid, u16 psid_size)
+{
+	const struct mlxfw_mfa2_tlv_psid *tlv_psid;
+	const struct mlxfw_mfa2_tlv_multi *dev_multi;
+	const struct mlxfw_mfa2_tlv *dev_tlv;
+	const struct mlxfw_mfa2_tlv *tlv;
+	u32 idx;
+
+	/* for each device tlv */
+	mlxfw_mfa2_tlv_foreach(mfa2_file, dev_tlv, idx, mfa2_file->first_dev,
+			       mfa2_file->dev_count) {
+		if (!dev_tlv)
+			return NULL;
+
+		dev_multi = mlxfw_mfa2_tlv_multi_get(mfa2_file, dev_tlv);
+		if (!dev_multi)
+			return NULL;
+
+		/* find psid child and compare */
+		tlv = mlxfw_mfa2_tlv_multi_child_find(mfa2_file, dev_multi,
+						      MLXFW_MFA2_TLV_PSID, 0);
+		if (!tlv)
+			return NULL;
+		if (be16_to_cpu(tlv->len) != psid_size)
+			continue;
+
+		tlv_psid = mlxfw_mfa2_tlv_psid_get(mfa2_file, tlv);
+		if (!tlv_psid)
+			return NULL;
+
+		if (memcmp(psid, tlv_psid->psid, psid_size) == 0)
+			return dev_multi;
+	}
+
+	return NULL;
+}
+
+int mlxfw_mfa2_file_component_count(const struct mlxfw_mfa2_file *mfa2_file,
+				    const char *psid, u32 psid_size,
+				    u32 *p_count)
+{
+	const struct mlxfw_mfa2_tlv_multi *dev_multi;
+	u16 count;
+	int err;
+
+	dev_multi = mlxfw_mfa2_tlv_dev_get(mfa2_file, psid, psid_size);
+	if (!dev_multi)
+		return -EINVAL;
+
+	err = mlxfw_mfa2_tlv_multi_child_count(mfa2_file, dev_multi,
+					       MLXFW_MFA2_TLV_COMPONENT_PTR,
+					       &count);
+	if (err)
+		return err;
+
+	*p_count = count;
+	return 0;
+}
+
+static int mlxfw_mfa2_xz_dec_run(struct xz_dec *xz_dec, struct xz_buf *xz_buf,
+				 bool *finished)
+{
+	enum xz_ret xz_ret;
+
+	xz_ret = xz_dec_run(xz_dec, xz_buf);
+
+	switch (xz_ret) {
+	case XZ_STREAM_END:
+		*finished = true;
+		return 0;
+	case XZ_OK:
+		*finished = false;
+		return 0;
+	case XZ_MEM_ERROR:
+		pr_err("xz no memory\n");
+		return -ENOMEM;
+	case XZ_DATA_ERROR:
+		pr_err("xz file corrupted\n");
+		return -EINVAL;
+	case XZ_FORMAT_ERROR:
+		pr_err("xz format not found\n");
+		return -EINVAL;
+	case XZ_OPTIONS_ERROR:
+		pr_err("unsupported xz option\n");
+		return -EINVAL;
+	case XZ_MEMLIMIT_ERROR:
+		pr_err("xz dictionary too small\n");
+		return -EINVAL;
+	default:
+		pr_err("xz error %d\n", xz_ret);
+		return -EINVAL;
+	}
+}
+
+static int mlxfw_mfa2_file_cb_offset_xz(const struct mlxfw_mfa2_file *mfa2_file,
+					off_t off, size_t size, u8 *buf)
+{
+	struct xz_dec *xz_dec;
+	struct xz_buf dec_buf;
+	off_t curr_off = 0;
+	bool finished;
+	int err;
+
+	xz_dec = xz_dec_init(XZ_DYNALLOC, (u32) -1);
+	if (!xz_dec)
+		return -EINVAL;
+
+	dec_buf.in_size = mfa2_file->cb_archive_size;
+	dec_buf.in = mfa2_file->cb;
+	dec_buf.in_pos = 0;
+	dec_buf.out = buf;
+
+	/* decode up to the offset */
+	do {
+		dec_buf.out_pos = 0;
+		dec_buf.out_size = min_t(size_t, size, off - curr_off);
+		if (dec_buf.out_size == 0)
+			break;
+
+		err = mlxfw_mfa2_xz_dec_run(xz_dec, &dec_buf, &finished);
+		if (err)
+			goto out;
+		if (finished) {
+			pr_err("xz section too short\n");
+			err = -EINVAL;
+			goto out;
+		}
+		curr_off += dec_buf.out_pos;
+	} while (curr_off != off);
+
+	/* decode the needed section */
+	dec_buf.out_pos = 0;
+	dec_buf.out_size = size;
+	err = mlxfw_mfa2_xz_dec_run(xz_dec, &dec_buf, &finished);
+out:
+	xz_dec_end(xz_dec);
+	return err;
+}
+
+static const struct mlxfw_mfa2_tlv_component_descriptor *
+mlxfw_mfa2_file_component_tlv_get(const struct mlxfw_mfa2_file *mfa2_file,
+				  u16 comp_index)
+{
+	const struct mlxfw_mfa2_tlv_multi *multi;
+	const struct mlxfw_mfa2_tlv *multi_child;
+	const struct mlxfw_mfa2_tlv *comp_tlv;
+
+	if (comp_index > mfa2_file->component_count)
+		return NULL;
+
+	comp_tlv = mlxfw_mfa2_tlv_advance(mfa2_file, mfa2_file->first_component,
+					  comp_index);
+	if (!comp_tlv)
+		return NULL;
+
+	multi = mlxfw_mfa2_tlv_multi_get(mfa2_file, comp_tlv);
+	if (!multi)
+		return NULL;
+
+	multi_child = mlxfw_mfa2_tlv_multi_child(mfa2_file, multi);
+	if (!multi_child)
+		return NULL;
+
+	return mlxfw_mfa2_tlv_component_descriptor_get(mfa2_file, multi_child);
+}
+
+struct mlxfw_mfa2_comp_data {
+	struct mlxfw_mfa2_component comp;
+	u8 buff[0];
+};
+
+static const struct mlxfw_mfa2_tlv_component_descriptor *
+mlxfw_mfa2_file_component_find(const struct mlxfw_mfa2_file *mfa2_file,
+			       const char *psid, int psid_size,
+			       int component_index)
+{
+	const struct mlxfw_mfa2_tlv_component_ptr *cptr;
+	const struct mlxfw_mfa2_tlv_multi *dev_multi;
+	const struct mlxfw_mfa2_tlv *cptr_tlv;
+	u16 comp_idx;
+
+	dev_multi = mlxfw_mfa2_tlv_dev_get(mfa2_file, psid, psid_size);
+	if (!dev_multi)
+		return NULL;
+
+	cptr_tlv = mlxfw_mfa2_tlv_multi_child_find(mfa2_file, dev_multi,
+						   MLXFW_MFA2_TLV_COMPONENT_PTR,
+						   component_index);
+	if (!cptr_tlv)
+		return NULL;
+
+	cptr = mlxfw_mfa2_tlv_component_ptr_get(mfa2_file, cptr_tlv);
+	if (!cptr)
+		return NULL;
+
+	comp_idx = be16_to_cpu(cptr->component_index);
+	return mlxfw_mfa2_file_component_tlv_get(mfa2_file, comp_idx);
+}
+
+struct mlxfw_mfa2_component *
+mlxfw_mfa2_file_component_get(const struct mlxfw_mfa2_file *mfa2_file,
+			      const char *psid, int psid_size,
+			      int component_index)
+{
+	const struct mlxfw_mfa2_tlv_component_descriptor *comp;
+	struct mlxfw_mfa2_comp_data *comp_data;
+	u32 comp_buf_size;
+	off_t cb_offset;
+	u32 comp_size;
+	int err;
+
+	comp = mlxfw_mfa2_file_component_find(mfa2_file, psid, psid_size,
+					      component_index);
+	if (!comp)
+		return ERR_PTR(-EINVAL);
+
+	cb_offset = (u64) be32_to_cpu(comp->cb_offset_h) << 32 |
+		    be32_to_cpu(comp->cb_offset_l);
+	comp_size = be32_to_cpu(comp->size);
+	comp_buf_size = comp_size + mlxfw_mfa2_comp_magic_len;
+
+	comp_data = kmalloc(sizeof(*comp_data) + comp_buf_size, GFP_KERNEL);
+	if (!comp_data)
+		return ERR_PTR(-ENOMEM);
+	comp_data->comp.data_size = comp_size;
+	comp_data->comp.index = be16_to_cpu(comp->identifier);
+	err = mlxfw_mfa2_file_cb_offset_xz(mfa2_file, cb_offset, comp_buf_size,
+					   comp_data->buff);
+	if (err) {
+		pr_err("Component could not be reached in CB\n");
+		goto err_out;
+	}
+
+	if (memcmp(comp_data->buff, mlxfw_mfa2_comp_magic,
+		   mlxfw_mfa2_comp_magic_len) != 0) {
+		pr_err("Component has wrong magic\n");
+		err = -EINVAL;
+		goto err_out;
+	}
+
+	comp_data->comp.data = comp_data->buff + mlxfw_mfa2_comp_magic_len;
+	return &comp_data->comp;
+err_out:
+	kfree(comp_data);
+	return ERR_PTR(err);
+}
+
+void mlxfw_mfa2_file_component_put(struct mlxfw_mfa2_component *comp)
+{
+	const struct mlxfw_mfa2_comp_data *comp_data;
+
+	comp_data = container_of(comp, struct mlxfw_mfa2_comp_data, comp);
+	kfree(comp_data);
+}
+
+void mlxfw_mfa2_file_fini(struct mlxfw_mfa2_file *mfa2_file)
+{
+	kfree(mfa2_file);
+}
diff --git a/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2.h b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2.h
new file mode 100644
index 000000000000..20472aa139cd
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2.h
@@ -0,0 +1,66 @@
+/*
+ * drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2.h
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2017 Yotam Gigi <yotamg@mellanox.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the names of the copyright holders nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _MLXFW_MFA2_H
+#define _MLXFW_MFA2_H
+
+#include <linux/firmware.h>
+#include "mlxfw.h"
+
+struct mlxfw_mfa2_component {
+	u16 index;
+	u32 data_size;
+	u8 *data;
+};
+
+struct mlxfw_mfa2_file;
+
+bool mlxfw_mfa2_check(const struct firmware *fw);
+
+struct mlxfw_mfa2_file *mlxfw_mfa2_file_init(const struct firmware *fw);
+
+int mlxfw_mfa2_file_component_count(const struct mlxfw_mfa2_file *mfa2_file,
+				    const char *psid, u32 psid_size,
+				    u32 *p_count);
+
+struct mlxfw_mfa2_component *
+mlxfw_mfa2_file_component_get(const struct mlxfw_mfa2_file *mfa2_file,
+			      const char *psid, int psid_size,
+			      int component_index);
+
+void mlxfw_mfa2_file_component_put(struct mlxfw_mfa2_component *component);
+
+void mlxfw_mfa2_file_fini(struct mlxfw_mfa2_file *mfa2_file);
+
+#endif
diff --git a/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_file.h b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_file.h
new file mode 100644
index 000000000000..f667942b1ea3
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_file.h
@@ -0,0 +1,60 @@
+/*
+ * drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_file.h
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2017 Yotam Gigi <yotamg@mellanox.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the names of the copyright holders nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _MLXFW_MFA2_FILE_H
+#define _MLXFW_MFA2_FILE_H
+
+#include <linux/firmware.h>
+#include <linux/kernel.h>
+
+struct mlxfw_mfa2_file {
+	const struct firmware *fw;
+	const struct mlxfw_mfa2_tlv *first_dev;
+	u16 dev_count;
+	const struct mlxfw_mfa2_tlv *first_component;
+	u16 component_count;
+	const void *cb; /* components block */
+	u32 cb_archive_size; /* size of compressed components block */
+};
+
+static inline bool mlxfw_mfa2_valid_ptr(const struct mlxfw_mfa2_file *mfa2_file,
+					const void *ptr)
+{
+	const void *valid_to = mfa2_file->fw->data + mfa2_file->fw->size;
+	const void *valid_from = mfa2_file->fw->data;
+
+	return ptr > valid_from && ptr < valid_to;
+}
+
+#endif
diff --git a/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_format.h b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_format.h
new file mode 100644
index 000000000000..dd66737c033d
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_format.h
@@ -0,0 +1,103 @@
+/*
+ * drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_format.h
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2017 Yotam Gigi <yotamg@mellanox.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the names of the copyright holders nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef _MLXFW_MFA2_FORMAT_H
+#define _MLXFW_MFA2_FORMAT_H
+
+#include "mlxfw_mfa2_file.h"
+#include "mlxfw_mfa2_tlv.h"
+
+enum mlxfw_mfa2_tlv_type {
+	MLXFW_MFA2_TLV_MULTI_PART = 0x01,
+	MLXFW_MFA2_TLV_PACKAGE_DESCRIPTOR = 0x02,
+	MLXFW_MFA2_TLV_COMPONENT_DESCRIPTOR = 0x04,
+	MLXFW_MFA2_TLV_COMPONENT_PTR = 0x22,
+	MLXFW_MFA2_TLV_PSID = 0x2A,
+};
+
+enum mlxfw_mfa2_compression_type {
+	MLXFW_MFA2_COMPRESSION_TYPE_NONE,
+	MLXFW_MFA2_COMPRESSION_TYPE_XZ,
+};
+
+struct mlxfw_mfa2_tlv_package_descriptor {
+	__be16 num_components;
+	__be16 num_devices;
+	__be32 cb_offset;
+	__be32 cb_archive_size;
+	__be32 cb_size_h;
+	__be32 cb_size_l;
+	u8 padding[3];
+	u8 cv_compression;
+	__be32 user_data_offset;
+} __packed;
+
+MLXFW_MFA2_TLV(package_descriptor, struct mlxfw_mfa2_tlv_package_descriptor,
+	       MLXFW_MFA2_TLV_PACKAGE_DESCRIPTOR);
+
+struct mlxfw_mfa2_tlv_multi {
+	__be16 num_extensions;
+	__be16 total_len;
+} __packed;
+
+MLXFW_MFA2_TLV(multi, struct mlxfw_mfa2_tlv_multi,
+	       MLXFW_MFA2_TLV_MULTI_PART);
+
+struct mlxfw_mfa2_tlv_psid {
+	u8 psid[0];
+} __packed;
+
+MLXFW_MFA2_TLV_VARSIZE(psid, struct mlxfw_mfa2_tlv_psid,
+		       MLXFW_MFA2_TLV_PSID);
+
+struct mlxfw_mfa2_tlv_component_ptr {
+	__be16 storage_id;
+	__be16 component_index;
+	__be32 storage_address;
+} __packed;
+
+MLXFW_MFA2_TLV(component_ptr, struct mlxfw_mfa2_tlv_component_ptr,
+	       MLXFW_MFA2_TLV_COMPONENT_PTR);
+
+struct mlxfw_mfa2_tlv_component_descriptor {
+	__be16 pldm_classification;
+	__be16 identifier;
+	__be32 cb_offset_h;
+	__be32 cb_offset_l;
+	__be32 size;
+} __packed;
+
+MLXFW_MFA2_TLV(component_descriptor, struct mlxfw_mfa2_tlv_component_descriptor,
+	       MLXFW_MFA2_TLV_COMPONENT_DESCRIPTOR);
+
+#endif
diff --git a/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_tlv.h b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_tlv.h
new file mode 100644
index 000000000000..cc013e77b326
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_tlv.h
@@ -0,0 +1,98 @@
+/*
+ * drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_tlv.h
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2017 Yotam Gigi <yotamg@mellanox.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the names of the copyright holders nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _MLXFW_MFA2_TLV_H
+#define _MLXFW_MFA2_TLV_H
+
+#include <linux/kernel.h>
+#include "mlxfw_mfa2_file.h"
+
+struct mlxfw_mfa2_tlv {
+	u8 version;
+	u8 type;
+	__be16 len;
+	u8 data[0];
+} __packed;
+
+static inline const struct mlxfw_mfa2_tlv *
+mlxfw_mfa2_tlv_get(const struct mlxfw_mfa2_file *mfa2_file, const void *ptr)
+{
+	if (!mlxfw_mfa2_valid_ptr(mfa2_file, ptr) ||
+	    !mlxfw_mfa2_valid_ptr(mfa2_file, ptr + sizeof(struct mlxfw_mfa2_tlv)))
+		return NULL;
+	return ptr;
+}
+
+static inline const void *
+mlxfw_mfa2_tlv_payload_get(const struct mlxfw_mfa2_file *mfa2_file,
+			   const struct mlxfw_mfa2_tlv *tlv, u8 payload_type,
+			   size_t payload_size, bool varsize)
+{
+	void *tlv_top;
+
+	tlv_top = (void *) tlv + be16_to_cpu(tlv->len) - 1;
+	if (!mlxfw_mfa2_valid_ptr(mfa2_file, tlv) ||
+	    !mlxfw_mfa2_valid_ptr(mfa2_file, tlv_top))
+		return NULL;
+	if (tlv->type != payload_type)
+		return NULL;
+	if (varsize && (be16_to_cpu(tlv->len) < payload_size))
+		return NULL;
+	if (!varsize && (be16_to_cpu(tlv->len) != payload_size))
+		return NULL;
+
+	return tlv->data;
+}
+
+#define MLXFW_MFA2_TLV(name, payload_type, tlv_type)			       \
+static inline const payload_type *					       \
+mlxfw_mfa2_tlv_ ## name ## _get(const struct mlxfw_mfa2_file *mfa2_file,       \
+				const struct mlxfw_mfa2_tlv *tlv)	       \
+{									       \
+	return mlxfw_mfa2_tlv_payload_get(mfa2_file, tlv,		       \
+					  tlv_type, sizeof(payload_type),      \
+					  false);			       \
+}
+
+#define MLXFW_MFA2_TLV_VARSIZE(name, payload_type, tlv_type)		       \
+static inline const payload_type *					       \
+mlxfw_mfa2_tlv_ ## name ## _get(const struct mlxfw_mfa2_file *mfa2_file,       \
+				const struct mlxfw_mfa2_tlv *tlv)	       \
+{									       \
+	return mlxfw_mfa2_tlv_payload_get(mfa2_file, tlv,		       \
+					  tlv_type, sizeof(payload_type),      \
+					  true);			       \
+}
+
+#endif
diff --git a/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_tlv_multi.c b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_tlv_multi.c
new file mode 100644
index 000000000000..0094b92a233b
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_tlv_multi.c
@@ -0,0 +1,126 @@
+/*
+ * drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_tlv_multi.c
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2017 Yotam Gigi <yotamg@mellanox.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the names of the copyright holders nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#define pr_fmt(fmt) "MFA2: " fmt
+
+#include "mlxfw_mfa2_tlv_multi.h"
+#include <uapi/linux/netlink.h>
+
+#define MLXFW_MFA2_TLV_TOTAL_SIZE(tlv) \
+	NLA_ALIGN(sizeof(*(tlv)) + be16_to_cpu((tlv)->len))
+
+const struct mlxfw_mfa2_tlv *
+mlxfw_mfa2_tlv_multi_child(const struct mlxfw_mfa2_file *mfa2_file,
+			   const struct mlxfw_mfa2_tlv_multi *multi)
+{
+	size_t multi_len;
+
+	multi_len = NLA_ALIGN(sizeof(struct mlxfw_mfa2_tlv_multi));
+	return mlxfw_mfa2_tlv_get(mfa2_file, (void *) multi + multi_len);
+}
+
+const struct mlxfw_mfa2_tlv *
+mlxfw_mfa2_tlv_next(const struct mlxfw_mfa2_file *mfa2_file,
+		    const struct mlxfw_mfa2_tlv *tlv)
+{
+	const struct mlxfw_mfa2_tlv_multi *multi;
+	u16 tlv_len;
+	void *next;
+
+	tlv_len = MLXFW_MFA2_TLV_TOTAL_SIZE(tlv);
+
+	if (tlv->type == MLXFW_MFA2_TLV_MULTI_PART) {
+		multi = mlxfw_mfa2_tlv_multi_get(mfa2_file, tlv);
+		tlv_len = NLA_ALIGN(tlv_len + be16_to_cpu(multi->total_len));
+	}
+
+	next = (void *) tlv + tlv_len;
+	return mlxfw_mfa2_tlv_get(mfa2_file, next);
+}
+
+const struct mlxfw_mfa2_tlv *
+mlxfw_mfa2_tlv_advance(const struct mlxfw_mfa2_file *mfa2_file,
+		       const struct mlxfw_mfa2_tlv *from_tlv, u16 count)
+{
+	const struct mlxfw_mfa2_tlv *tlv;
+	u16 idx;
+
+	mlxfw_mfa2_tlv_foreach(mfa2_file, tlv, idx, from_tlv, count)
+		if (!tlv)
+			return NULL;
+	return tlv;
+}
+
+const struct mlxfw_mfa2_tlv *
+mlxfw_mfa2_tlv_multi_child_find(const struct mlxfw_mfa2_file *mfa2_file,
+				const struct mlxfw_mfa2_tlv_multi *multi,
+				enum mlxfw_mfa2_tlv_type type, u16 index)
+{
+	const struct mlxfw_mfa2_tlv *tlv;
+	u16 skip = 0;
+	u16 idx;
+
+	mlxfw_mfa2_tlv_multi_foreach(mfa2_file, tlv, idx, multi) {
+		if (!tlv) {
+			pr_err("TLV parsing error\n");
+			return NULL;
+		}
+		if (tlv->type == type)
+			if (skip++ == index)
+				return tlv;
+	}
+	return NULL;
+}
+
+int mlxfw_mfa2_tlv_multi_child_count(const struct mlxfw_mfa2_file *mfa2_file,
+				     const struct mlxfw_mfa2_tlv_multi *multi,
+				     enum mlxfw_mfa2_tlv_type type,
+				     u16 *p_count)
+{
+	const struct mlxfw_mfa2_tlv *tlv;
+	u16 count = 0;
+	u16 idx;
+
+	mlxfw_mfa2_tlv_multi_foreach(mfa2_file, tlv, idx, multi) {
+		if (!tlv) {
+			pr_err("TLV parsing error\n");
+			return -EINVAL;
+		}
+
+		if (tlv->type == type)
+			count++;
+	}
+	*p_count = count;
+	return 0;
+}
diff --git a/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_tlv_multi.h b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_tlv_multi.h
new file mode 100644
index 000000000000..2c667894f3a2
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_tlv_multi.h
@@ -0,0 +1,71 @@
+/*
+ * drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2_tlv_multi.h
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2017 Yotam Gigi <yotamg@mellanox.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the names of the copyright holders nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef _MLXFW_MFA2_TLV_MULTI_H
+#define _MLXFW_MFA2_TLV_MULTI_H
+
+#include "mlxfw_mfa2_tlv.h"
+#include "mlxfw_mfa2_format.h"
+#include "mlxfw_mfa2_file.h"
+
+const struct mlxfw_mfa2_tlv *
+mlxfw_mfa2_tlv_multi_child(const struct mlxfw_mfa2_file *mfa2_file,
+			   const struct mlxfw_mfa2_tlv_multi *multi);
+
+const struct mlxfw_mfa2_tlv *
+mlxfw_mfa2_tlv_next(const struct mlxfw_mfa2_file *mfa2_file,
+		    const struct mlxfw_mfa2_tlv *tlv);
+
+const struct mlxfw_mfa2_tlv *
+mlxfw_mfa2_tlv_advance(const struct mlxfw_mfa2_file *mfa2_file,
+		       const struct mlxfw_mfa2_tlv *from_tlv, u16 count);
+
+const struct mlxfw_mfa2_tlv *
+mlxfw_mfa2_tlv_multi_child_find(const struct mlxfw_mfa2_file *mfa2_file,
+				const struct mlxfw_mfa2_tlv_multi *multi,
+				enum mlxfw_mfa2_tlv_type type, u16 index);
+
+int mlxfw_mfa2_tlv_multi_child_count(const struct mlxfw_mfa2_file *mfa2_file,
+				     const struct mlxfw_mfa2_tlv_multi *multi,
+				     enum mlxfw_mfa2_tlv_type type,
+				     u16 *p_count);
+
+#define mlxfw_mfa2_tlv_foreach(mfa2_file, tlv, idx, from_tlv, count) \
+	for (idx = 0, tlv = from_tlv; idx < (count); \
+	     idx++, tlv = mlxfw_mfa2_tlv_next(mfa2_file, tlv))
+
+#define mlxfw_mfa2_tlv_multi_foreach(mfa2_file, tlv, idx, multi) \
+	mlxfw_mfa2_tlv_foreach(mfa2_file, tlv, idx, \
+			       mlxfw_mfa2_tlv_multi_child(mfa2_file, multi), \
+			       be16_to_cpu(multi->num_extensions) + 1)
+#endif
diff --git a/drivers/net/ethernet/mellanox/mlxsw/Kconfig b/drivers/net/ethernet/mellanox/mlxsw/Kconfig
index ef23eaedc2ff..695adff89d71 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/Kconfig
+++ b/drivers/net/ethernet/mellanox/mlxsw/Kconfig
@@ -74,7 +74,9 @@ config MLXSW_SPECTRUM
 	tristate "Mellanox Technologies Spectrum support"
 	depends on MLXSW_CORE && MLXSW_PCI && NET_SWITCHDEV && VLAN_8021Q
 	depends on PSAMPLE || PSAMPLE=n
+	depends on BRIDGE || BRIDGE=n
 	select PARMAN
+	select MLXFW
 	default m
 	---help---
 	  This driver supports Mellanox Technologies Spectrum Ethernet
diff --git a/drivers/net/ethernet/mellanox/mlxsw/Makefile b/drivers/net/ethernet/mellanox/mlxsw/Makefile
index 2fb8c6585ac7..62fc42f396bb 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/Makefile
+++ b/drivers/net/ethernet/mellanox/mlxsw/Makefile
@@ -16,7 +16,8 @@ mlxsw_spectrum-objs		:= spectrum.o spectrum_buffers.o \
 				   spectrum_switchdev.o spectrum_router.o \
 				   spectrum_kvdl.o spectrum_acl_tcam.o \
 				   spectrum_acl.o spectrum_flower.o \
-				   spectrum_cnt.o spectrum_dpipe.o
+				   spectrum_cnt.o spectrum_dpipe.o \
+				   spectrum_fid.o
 mlxsw_spectrum-$(CONFIG_MLXSW_SPECTRUM_DCB)	+= spectrum_dcb.o
 obj-$(CONFIG_MLXSW_MINIMAL)	+= mlxsw_minimal.o
 mlxsw_minimal-objs		:= minimal.o
diff --git a/drivers/net/ethernet/mellanox/mlxsw/core.h b/drivers/net/ethernet/mellanox/mlxsw/core.h
index 7fb35395adf5..6e966af72fc4 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/core.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/core.h
@@ -344,15 +344,17 @@ struct mlxsw_bus {
 	u8 features;
 };
 
+struct mlxsw_fw_rev {
+	u16 major;
+	u16 minor;
+	u16 subminor;
+};
+
 struct mlxsw_bus_info {
 	const char *device_kind;
 	const char *device_name;
 	struct device *dev;
-	struct {
-		u16 major;
-		u16 minor;
-		u16 subminor;
-	} fw_rev;
+	struct mlxsw_fw_rev fw_rev;
 	u8 vsd[MLXSW_CMD_BOARDINFO_VSD_LEN];
 	u8 psid[MLXSW_CMD_BOARDINFO_PSID_LEN];
 };
diff --git a/drivers/net/ethernet/mellanox/mlxsw/core_acl_flex_actions.c b/drivers/net/ethernet/mellanox/mlxsw/core_acl_flex_actions.c
index 46304ffb9449..5ae110172c22 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/core_acl_flex_actions.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/core_acl_flex_actions.c
@@ -40,6 +40,7 @@
 #include <linux/list.h>
 
 #include "item.h"
+#include "trap.h"
 #include "core_acl_flex_actions.h"
 
 enum mlxsw_afa_set_type {
@@ -662,6 +663,16 @@ EXPORT_SYMBOL(mlxsw_afa_block_append_vlan_modify);
 #define MLXSW_AFA_TRAPDISC_CODE 0x03
 #define MLXSW_AFA_TRAPDISC_SIZE 1
 
+enum mlxsw_afa_trapdisc_trap_action {
+	MLXSW_AFA_TRAPDISC_TRAP_ACTION_NOP = 0,
+	MLXSW_AFA_TRAPDISC_TRAP_ACTION_TRAP = 2,
+};
+
+/* afa_trapdisc_trap_action
+ * Trap Action.
+ */
+MLXSW_ITEM32(afa, trapdisc, trap_action, 0x00, 24, 4);
+
 enum mlxsw_afa_trapdisc_forward_action {
 	MLXSW_AFA_TRAPDISC_FORWARD_ACTION_DISCARD = 3,
 };
@@ -671,11 +682,20 @@ enum mlxsw_afa_trapdisc_forward_action {
  */
 MLXSW_ITEM32(afa, trapdisc, forward_action, 0x00, 0, 4);
 
+/* afa_trapdisc_trap_id
+ * Trap ID to configure.
+ */
+MLXSW_ITEM32(afa, trapdisc, trap_id, 0x04, 0, 9);
+
 static inline void
 mlxsw_afa_trapdisc_pack(char *payload,
-			enum mlxsw_afa_trapdisc_forward_action forward_action)
+			enum mlxsw_afa_trapdisc_trap_action trap_action,
+			enum mlxsw_afa_trapdisc_forward_action forward_action,
+			u16 trap_id)
 {
+	mlxsw_afa_trapdisc_trap_action_set(payload, trap_action);
 	mlxsw_afa_trapdisc_forward_action_set(payload, forward_action);
+	mlxsw_afa_trapdisc_trap_id_set(payload, trap_id);
 }
 
 int mlxsw_afa_block_append_drop(struct mlxsw_afa_block *block)
@@ -686,11 +706,27 @@ int mlxsw_afa_block_append_drop(struct mlxsw_afa_block *block)
 
 	if (!act)
 		return -ENOBUFS;
-	mlxsw_afa_trapdisc_pack(act, MLXSW_AFA_TRAPDISC_FORWARD_ACTION_DISCARD);
+	mlxsw_afa_trapdisc_pack(act, MLXSW_AFA_TRAPDISC_TRAP_ACTION_NOP,
+				MLXSW_AFA_TRAPDISC_FORWARD_ACTION_DISCARD, 0);
 	return 0;
 }
 EXPORT_SYMBOL(mlxsw_afa_block_append_drop);
 
+int mlxsw_afa_block_append_trap(struct mlxsw_afa_block *block)
+{
+	char *act = mlxsw_afa_block_append_action(block,
+						  MLXSW_AFA_TRAPDISC_CODE,
+						  MLXSW_AFA_TRAPDISC_SIZE);
+
+	if (!act)
+		return -ENOBUFS;
+	mlxsw_afa_trapdisc_pack(act, MLXSW_AFA_TRAPDISC_TRAP_ACTION_TRAP,
+				MLXSW_AFA_TRAPDISC_FORWARD_ACTION_DISCARD,
+				MLXSW_TRAP_ID_ACL0);
+	return 0;
+}
+EXPORT_SYMBOL(mlxsw_afa_block_append_trap);
+
 /* Forwarding Action
  * -----------------
  * Forwarding Action can be used to implement Policy Based Switching (PBS)
diff --git a/drivers/net/ethernet/mellanox/mlxsw/core_acl_flex_actions.h b/drivers/net/ethernet/mellanox/mlxsw/core_acl_flex_actions.h
index bd8b91d02880..f99c341b2497 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/core_acl_flex_actions.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/core_acl_flex_actions.h
@@ -60,6 +60,7 @@ u32 mlxsw_afa_block_first_set_kvdl_index(struct mlxsw_afa_block *block);
 void mlxsw_afa_block_continue(struct mlxsw_afa_block *block);
 void mlxsw_afa_block_jump(struct mlxsw_afa_block *block, u16 group_id);
 int mlxsw_afa_block_append_drop(struct mlxsw_afa_block *block);
+int mlxsw_afa_block_append_trap(struct mlxsw_afa_block *block);
 int mlxsw_afa_block_append_fwd(struct mlxsw_afa_block *block,
 			       u8 local_port, bool in_port);
 int mlxsw_afa_block_append_vlan_modify(struct mlxsw_afa_block *block,
diff --git a/drivers/net/ethernet/mellanox/mlxsw/core_acl_flex_keys.h b/drivers/net/ethernet/mellanox/mlxsw/core_acl_flex_keys.h
index c75e9141e3ec..9807ef814e42 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/core_acl_flex_keys.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/core_acl_flex_keys.h
@@ -56,6 +56,7 @@ enum mlxsw_afk_element {
 	MLXSW_AFK_ELEMENT_SRC_L4_PORT,
 	MLXSW_AFK_ELEMENT_VID,
 	MLXSW_AFK_ELEMENT_PCP,
+	MLXSW_AFK_ELEMENT_TCP_FLAGS,
 	MLXSW_AFK_ELEMENT_MAX,
 };
 
@@ -102,6 +103,7 @@ static const struct mlxsw_afk_element_info mlxsw_afk_element_infos[] = {
 	MLXSW_AFK_ELEMENT_INFO_U32(IP_PROTO, 0x10, 0, 8),
 	MLXSW_AFK_ELEMENT_INFO_U32(VID, 0x10, 8, 12),
 	MLXSW_AFK_ELEMENT_INFO_U32(PCP, 0x10, 20, 3),
+	MLXSW_AFK_ELEMENT_INFO_U32(TCP_FLAGS, 0x10, 23, 9),
 	MLXSW_AFK_ELEMENT_INFO_U32(SRC_IP4, 0x18, 0, 32),
 	MLXSW_AFK_ELEMENT_INFO_U32(DST_IP4, 0x1C, 0, 32),
 	MLXSW_AFK_ELEMENT_INFO_BUF(SRC_IP6_HI, 0x18, 8),
diff --git a/drivers/net/ethernet/mellanox/mlxsw/pci_hw.h b/drivers/net/ethernet/mellanox/mlxsw/pci_hw.h
index 0af3338bfcb4..a6441208e9d9 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/pci_hw.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/pci_hw.h
@@ -155,7 +155,7 @@ MLXSW_ITEM32(pci, cqe, byte_count, 0x04, 0, 14);
 /* pci_cqe_trap_id
  * Trap ID that captured the packet.
  */
-MLXSW_ITEM32(pci, cqe, trap_id, 0x08, 0, 8);
+MLXSW_ITEM32(pci, cqe, trap_id, 0x08, 0, 9);
 
 /* pci_cqe_crc
  * Length include CRC. Indicates the length field includes
diff --git a/drivers/net/ethernet/mellanox/mlxsw/reg.h b/drivers/net/ethernet/mellanox/mlxsw/reg.h
index 83b277c8090e..1bd34d9a7b9e 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/reg.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/reg.h
@@ -958,7 +958,7 @@ enum mlxsw_flood_table_type {
 	MLXSW_REG_SFGC_TABLE_TYPE_VID = 1,
 	MLXSW_REG_SFGC_TABLE_TYPE_SINGLE = 2,
 	MLXSW_REG_SFGC_TABLE_TYPE_ANY = 0,
-	MLXSW_REG_SFGC_TABLE_TYPE_FID_OFFEST = 3,
+	MLXSW_REG_SFGC_TABLE_TYPE_FID_OFFSET = 3,
 	MLXSW_REG_SFGC_TABLE_TYPE_FID = 4,
 };
 
@@ -5491,6 +5491,81 @@ static inline void mlxsw_reg_mtmp_unpack(char *payload, unsigned int *p_temp,
 		mlxsw_reg_mtmp_sensor_name_memcpy_from(payload, sensor_name);
 }
 
+/* MCIA - Management Cable Info Access
+ * -----------------------------------
+ * MCIA register is used to access the SFP+ and QSFP connector's EPROM.
+ */
+
+#define MLXSW_REG_MCIA_ID 0x9014
+#define MLXSW_REG_MCIA_LEN 0x40
+
+MLXSW_REG_DEFINE(mcia, MLXSW_REG_MCIA_ID, MLXSW_REG_MCIA_LEN);
+
+/* reg_mcia_l
+ * Lock bit. Setting this bit will lock the access to the specific
+ * cable. Used for updating a full page in a cable EPROM. Any access
+ * other then subsequence writes will fail while the port is locked.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mcia, l, 0x00, 31, 1);
+
+/* reg_mcia_module
+ * Module number.
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, mcia, module, 0x00, 16, 8);
+
+/* reg_mcia_status
+ * Module status.
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mcia, status, 0x00, 0, 8);
+
+/* reg_mcia_i2c_device_address
+ * I2C device address.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mcia, i2c_device_address, 0x04, 24, 8);
+
+/* reg_mcia_page_number
+ * Page number.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mcia, page_number, 0x04, 16, 8);
+
+/* reg_mcia_device_address
+ * Device address.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mcia, device_address, 0x04, 0, 16);
+
+/* reg_mcia_size
+ * Number of bytes to read/write (up to 48 bytes).
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mcia, size, 0x08, 0, 16);
+
+#define MLXSW_SP_REG_MCIA_EEPROM_SIZE 48
+
+/* reg_mcia_eeprom
+ * Bytes to read/write.
+ * Access: RW
+ */
+MLXSW_ITEM_BUF(reg, mcia, eeprom, 0x10, MLXSW_SP_REG_MCIA_EEPROM_SIZE);
+
+static inline void mlxsw_reg_mcia_pack(char *payload, u8 module, u8 lock,
+				       u8 page_number, u16 device_addr,
+				       u8 size, u8 i2c_device_addr)
+{
+	MLXSW_REG_ZERO(mcia, payload);
+	mlxsw_reg_mcia_module_set(payload, module);
+	mlxsw_reg_mcia_l_set(payload, lock);
+	mlxsw_reg_mcia_page_number_set(payload, page_number);
+	mlxsw_reg_mcia_device_address_set(payload, device_addr);
+	mlxsw_reg_mcia_size_set(payload, size);
+	mlxsw_reg_mcia_i2c_device_address_set(payload, i2c_device_addr);
+}
+
 /* MPAT - Monitoring Port Analyzer Table
  * -------------------------------------
  * MPAT Register is used to query and configure the Switch PortAnalyzer Table.
@@ -5643,6 +5718,222 @@ static inline void mlxsw_reg_mlcr_pack(char *payload, u8 local_port,
 					   MLXSW_REG_MLCR_DURATION_MAX : 0);
 }
 
+/* MCQI - Management Component Query Information
+ * ---------------------------------------------
+ * This register allows querying information about firmware components.
+ */
+#define MLXSW_REG_MCQI_ID 0x9061
+#define MLXSW_REG_MCQI_BASE_LEN 0x18
+#define MLXSW_REG_MCQI_CAP_LEN 0x14
+#define MLXSW_REG_MCQI_LEN (MLXSW_REG_MCQI_BASE_LEN + MLXSW_REG_MCQI_CAP_LEN)
+
+MLXSW_REG_DEFINE(mcqi, MLXSW_REG_MCQI_ID, MLXSW_REG_MCQI_LEN);
+
+/* reg_mcqi_component_index
+ * Index of the accessed component.
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, mcqi, component_index, 0x00, 0, 16);
+
+enum mlxfw_reg_mcqi_info_type {
+	MLXSW_REG_MCQI_INFO_TYPE_CAPABILITIES,
+};
+
+/* reg_mcqi_info_type
+ * Component properties set.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mcqi, info_type, 0x08, 0, 5);
+
+/* reg_mcqi_offset
+ * The requested/returned data offset from the section start, given in bytes.
+ * Must be DWORD aligned.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mcqi, offset, 0x10, 0, 32);
+
+/* reg_mcqi_data_size
+ * The requested/returned data size, given in bytes. If data_size is not DWORD
+ * aligned, the last bytes are zero padded.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mcqi, data_size, 0x14, 0, 16);
+
+/* reg_mcqi_cap_max_component_size
+ * Maximum size for this component, given in bytes.
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mcqi, cap_max_component_size, 0x20, 0, 32);
+
+/* reg_mcqi_cap_log_mcda_word_size
+ * Log 2 of the access word size in bytes. Read and write access must be aligned
+ * to the word size. Write access must be done for an integer number of words.
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mcqi, cap_log_mcda_word_size, 0x24, 28, 4);
+
+/* reg_mcqi_cap_mcda_max_write_size
+ * Maximal write size for MCDA register
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mcqi, cap_mcda_max_write_size, 0x24, 0, 16);
+
+static inline void mlxsw_reg_mcqi_pack(char *payload, u16 component_index)
+{
+	MLXSW_REG_ZERO(mcqi, payload);
+	mlxsw_reg_mcqi_component_index_set(payload, component_index);
+	mlxsw_reg_mcqi_info_type_set(payload,
+				     MLXSW_REG_MCQI_INFO_TYPE_CAPABILITIES);
+	mlxsw_reg_mcqi_offset_set(payload, 0);
+	mlxsw_reg_mcqi_data_size_set(payload, MLXSW_REG_MCQI_CAP_LEN);
+}
+
+static inline void mlxsw_reg_mcqi_unpack(char *payload,
+					 u32 *p_cap_max_component_size,
+					 u8 *p_cap_log_mcda_word_size,
+					 u16 *p_cap_mcda_max_write_size)
+{
+	*p_cap_max_component_size =
+		mlxsw_reg_mcqi_cap_max_component_size_get(payload);
+	*p_cap_log_mcda_word_size =
+		mlxsw_reg_mcqi_cap_log_mcda_word_size_get(payload);
+	*p_cap_mcda_max_write_size =
+		mlxsw_reg_mcqi_cap_mcda_max_write_size_get(payload);
+}
+
+/* MCC - Management Component Control
+ * ----------------------------------
+ * Controls the firmware component and updates the FSM.
+ */
+#define MLXSW_REG_MCC_ID 0x9062
+#define MLXSW_REG_MCC_LEN 0x1C
+
+MLXSW_REG_DEFINE(mcc, MLXSW_REG_MCC_ID, MLXSW_REG_MCC_LEN);
+
+enum mlxsw_reg_mcc_instruction {
+	MLXSW_REG_MCC_INSTRUCTION_LOCK_UPDATE_HANDLE = 0x01,
+	MLXSW_REG_MCC_INSTRUCTION_RELEASE_UPDATE_HANDLE = 0x02,
+	MLXSW_REG_MCC_INSTRUCTION_UPDATE_COMPONENT = 0x03,
+	MLXSW_REG_MCC_INSTRUCTION_VERIFY_COMPONENT = 0x04,
+	MLXSW_REG_MCC_INSTRUCTION_ACTIVATE = 0x06,
+	MLXSW_REG_MCC_INSTRUCTION_CANCEL = 0x08,
+};
+
+/* reg_mcc_instruction
+ * Command to be executed by the FSM.
+ * Applicable for write operation only.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mcc, instruction, 0x00, 0, 8);
+
+/* reg_mcc_component_index
+ * Index of the accessed component. Applicable only for commands that
+ * refer to components. Otherwise, this field is reserved.
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, mcc, component_index, 0x04, 0, 16);
+
+/* reg_mcc_update_handle
+ * Token representing the current flow executed by the FSM.
+ * Access: WO
+ */
+MLXSW_ITEM32(reg, mcc, update_handle, 0x08, 0, 24);
+
+/* reg_mcc_error_code
+ * Indicates the successful completion of the instruction, or the reason it
+ * failed
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mcc, error_code, 0x0C, 8, 8);
+
+/* reg_mcc_control_state
+ * Current FSM state
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mcc, control_state, 0x0C, 0, 4);
+
+/* reg_mcc_component_size
+ * Component size in bytes. Valid for UPDATE_COMPONENT instruction. Specifying
+ * the size may shorten the update time. Value 0x0 means that size is
+ * unspecified.
+ * Access: WO
+ */
+MLXSW_ITEM32(reg, mcc, component_size, 0x10, 0, 32);
+
+static inline void mlxsw_reg_mcc_pack(char *payload,
+				      enum mlxsw_reg_mcc_instruction instr,
+				      u16 component_index, u32 update_handle,
+				      u32 component_size)
+{
+	MLXSW_REG_ZERO(mcc, payload);
+	mlxsw_reg_mcc_instruction_set(payload, instr);
+	mlxsw_reg_mcc_component_index_set(payload, component_index);
+	mlxsw_reg_mcc_update_handle_set(payload, update_handle);
+	mlxsw_reg_mcc_component_size_set(payload, component_size);
+}
+
+static inline void mlxsw_reg_mcc_unpack(char *payload, u32 *p_update_handle,
+					u8 *p_error_code, u8 *p_control_state)
+{
+	if (p_update_handle)
+		*p_update_handle = mlxsw_reg_mcc_update_handle_get(payload);
+	if (p_error_code)
+		*p_error_code = mlxsw_reg_mcc_error_code_get(payload);
+	if (p_control_state)
+		*p_control_state = mlxsw_reg_mcc_control_state_get(payload);
+}
+
+/* MCDA - Management Component Data Access
+ * ---------------------------------------
+ * This register allows reading and writing a firmware component.
+ */
+#define MLXSW_REG_MCDA_ID 0x9063
+#define MLXSW_REG_MCDA_BASE_LEN 0x10
+#define MLXSW_REG_MCDA_MAX_DATA_LEN 0x80
+#define MLXSW_REG_MCDA_LEN \
+		(MLXSW_REG_MCDA_BASE_LEN + MLXSW_REG_MCDA_MAX_DATA_LEN)
+
+MLXSW_REG_DEFINE(mcda, MLXSW_REG_MCDA_ID, MLXSW_REG_MCDA_LEN);
+
+/* reg_mcda_update_handle
+ * Token representing the current flow executed by the FSM.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mcda, update_handle, 0x00, 0, 24);
+
+/* reg_mcda_offset
+ * Offset of accessed address relative to component start. Accesses must be in
+ * accordance to log_mcda_word_size in MCQI reg.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mcda, offset, 0x04, 0, 32);
+
+/* reg_mcda_size
+ * Size of the data accessed, given in bytes.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mcda, size, 0x08, 0, 16);
+
+/* reg_mcda_data
+ * Data block accessed.
+ * Access: RW
+ */
+MLXSW_ITEM32_INDEXED(reg, mcda, data, 0x10, 0, 32, 4, 0, false);
+
+static inline void mlxsw_reg_mcda_pack(char *payload, u32 update_handle,
+				       u32 offset, u16 size, u8 *data)
+{
+	int i;
+
+	MLXSW_REG_ZERO(mcda, payload);
+	mlxsw_reg_mcda_update_handle_set(payload, update_handle);
+	mlxsw_reg_mcda_offset_set(payload, offset);
+	mlxsw_reg_mcda_size_set(payload, size);
+
+	for (i = 0; i < size / 4; i++)
+		mlxsw_reg_mcda_data_set(payload, i, *(u32 *) &data[i * 4]);
+}
+
 /* MPSC - Monitoring Packet Sampling Configuration Register
  * --------------------------------------------------------
  * MPSC Register is used to configure the Packet Sampling mechanism.
@@ -6217,10 +6508,14 @@ static const struct mlxsw_reg_info *mlxsw_reg_infos[] = {
 	MLXSW_REG(mfsl),
 	MLXSW_REG(mtcap),
 	MLXSW_REG(mtmp),
+	MLXSW_REG(mcia),
 	MLXSW_REG(mpat),
 	MLXSW_REG(mpar),
 	MLXSW_REG(mlcr),
 	MLXSW_REG(mpsc),
+	MLXSW_REG(mcqi),
+	MLXSW_REG(mcc),
+	MLXSW_REG(mcda),
 	MLXSW_REG(mgpc),
 	MLXSW_REG(sbpr),
 	MLXSW_REG(sbcm),
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum.c
index 88357cee7679..60bf8f27cc00 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum.c
@@ -68,6 +68,22 @@
 #include "txheader.h"
 #include "spectrum_cnt.h"
 #include "spectrum_dpipe.h"
+#include "../mlxfw/mlxfw.h"
+
+#define MLXSW_FWREV_MAJOR 13
+#define MLXSW_FWREV_MINOR 1420
+#define MLXSW_FWREV_SUBMINOR 122
+
+static const struct mlxsw_fw_rev mlxsw_sp_supported_fw_rev = {
+	.major = MLXSW_FWREV_MAJOR,
+	.minor = MLXSW_FWREV_MINOR,
+	.subminor = MLXSW_FWREV_SUBMINOR
+};
+
+#define MLXSW_SP_FW_FILENAME \
+	"mellanox/mlxsw_spectrum-" __stringify(MLXSW_FWREV_MAJOR) \
+	"." __stringify(MLXSW_FWREV_MINOR) \
+	"." __stringify(MLXSW_FWREV_SUBMINOR) ".mfa2"
 
 static const char mlxsw_sp_driver_name[] = "mlxsw_spectrum";
 static const char mlxsw_sp_driver_version[] = "1.0";
@@ -140,6 +156,223 @@ MLXSW_ITEM32(tx, hdr, fid, 0x08, 0, 16);
  */
 MLXSW_ITEM32(tx, hdr, type, 0x0C, 0, 4);
 
+struct mlxsw_sp_mlxfw_dev {
+	struct mlxfw_dev mlxfw_dev;
+	struct mlxsw_sp *mlxsw_sp;
+};
+
+static int mlxsw_sp_component_query(struct mlxfw_dev *mlxfw_dev,
+				    u16 component_index, u32 *p_max_size,
+				    u8 *p_align_bits, u16 *p_max_write_size)
+{
+	struct mlxsw_sp_mlxfw_dev *mlxsw_sp_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlxsw_sp_mlxfw_dev, mlxfw_dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_mlxfw_dev->mlxsw_sp;
+	char mcqi_pl[MLXSW_REG_MCQI_LEN];
+	int err;
+
+	mlxsw_reg_mcqi_pack(mcqi_pl, component_index);
+	err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(mcqi), mcqi_pl);
+	if (err)
+		return err;
+	mlxsw_reg_mcqi_unpack(mcqi_pl, p_max_size, p_align_bits,
+			      p_max_write_size);
+
+	*p_align_bits = max_t(u8, *p_align_bits, 2);
+	*p_max_write_size = min_t(u16, *p_max_write_size,
+				  MLXSW_REG_MCDA_MAX_DATA_LEN);
+	return 0;
+}
+
+static int mlxsw_sp_fsm_lock(struct mlxfw_dev *mlxfw_dev, u32 *fwhandle)
+{
+	struct mlxsw_sp_mlxfw_dev *mlxsw_sp_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlxsw_sp_mlxfw_dev, mlxfw_dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_mlxfw_dev->mlxsw_sp;
+	char mcc_pl[MLXSW_REG_MCC_LEN];
+	u8 control_state;
+	int err;
+
+	mlxsw_reg_mcc_pack(mcc_pl, 0, 0, 0, 0);
+	err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(mcc), mcc_pl);
+	if (err)
+		return err;
+
+	mlxsw_reg_mcc_unpack(mcc_pl, fwhandle, NULL, &control_state);
+	if (control_state != MLXFW_FSM_STATE_IDLE)
+		return -EBUSY;
+
+	mlxsw_reg_mcc_pack(mcc_pl,
+			   MLXSW_REG_MCC_INSTRUCTION_LOCK_UPDATE_HANDLE,
+			   0, *fwhandle, 0);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mcc), mcc_pl);
+}
+
+static int mlxsw_sp_fsm_component_update(struct mlxfw_dev *mlxfw_dev,
+					 u32 fwhandle, u16 component_index,
+					 u32 component_size)
+{
+	struct mlxsw_sp_mlxfw_dev *mlxsw_sp_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlxsw_sp_mlxfw_dev, mlxfw_dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_mlxfw_dev->mlxsw_sp;
+	char mcc_pl[MLXSW_REG_MCC_LEN];
+
+	mlxsw_reg_mcc_pack(mcc_pl, MLXSW_REG_MCC_INSTRUCTION_UPDATE_COMPONENT,
+			   component_index, fwhandle, component_size);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mcc), mcc_pl);
+}
+
+static int mlxsw_sp_fsm_block_download(struct mlxfw_dev *mlxfw_dev,
+				       u32 fwhandle, u8 *data, u16 size,
+				       u32 offset)
+{
+	struct mlxsw_sp_mlxfw_dev *mlxsw_sp_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlxsw_sp_mlxfw_dev, mlxfw_dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_mlxfw_dev->mlxsw_sp;
+	char mcda_pl[MLXSW_REG_MCDA_LEN];
+
+	mlxsw_reg_mcda_pack(mcda_pl, fwhandle, offset, size, data);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mcda), mcda_pl);
+}
+
+static int mlxsw_sp_fsm_component_verify(struct mlxfw_dev *mlxfw_dev,
+					 u32 fwhandle, u16 component_index)
+{
+	struct mlxsw_sp_mlxfw_dev *mlxsw_sp_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlxsw_sp_mlxfw_dev, mlxfw_dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_mlxfw_dev->mlxsw_sp;
+	char mcc_pl[MLXSW_REG_MCC_LEN];
+
+	mlxsw_reg_mcc_pack(mcc_pl, MLXSW_REG_MCC_INSTRUCTION_VERIFY_COMPONENT,
+			   component_index, fwhandle, 0);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mcc), mcc_pl);
+}
+
+static int mlxsw_sp_fsm_activate(struct mlxfw_dev *mlxfw_dev, u32 fwhandle)
+{
+	struct mlxsw_sp_mlxfw_dev *mlxsw_sp_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlxsw_sp_mlxfw_dev, mlxfw_dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_mlxfw_dev->mlxsw_sp;
+	char mcc_pl[MLXSW_REG_MCC_LEN];
+
+	mlxsw_reg_mcc_pack(mcc_pl, MLXSW_REG_MCC_INSTRUCTION_ACTIVATE, 0,
+			   fwhandle, 0);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mcc), mcc_pl);
+}
+
+static int mlxsw_sp_fsm_query_state(struct mlxfw_dev *mlxfw_dev, u32 fwhandle,
+				    enum mlxfw_fsm_state *fsm_state,
+				    enum mlxfw_fsm_state_err *fsm_state_err)
+{
+	struct mlxsw_sp_mlxfw_dev *mlxsw_sp_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlxsw_sp_mlxfw_dev, mlxfw_dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_mlxfw_dev->mlxsw_sp;
+	char mcc_pl[MLXSW_REG_MCC_LEN];
+	u8 control_state;
+	u8 error_code;
+	int err;
+
+	mlxsw_reg_mcc_pack(mcc_pl, 0, 0, fwhandle, 0);
+	err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(mcc), mcc_pl);
+	if (err)
+		return err;
+
+	mlxsw_reg_mcc_unpack(mcc_pl, NULL, &error_code, &control_state);
+	*fsm_state = control_state;
+	*fsm_state_err = min_t(enum mlxfw_fsm_state_err, error_code,
+			       MLXFW_FSM_STATE_ERR_MAX);
+	return 0;
+}
+
+static void mlxsw_sp_fsm_cancel(struct mlxfw_dev *mlxfw_dev, u32 fwhandle)
+{
+	struct mlxsw_sp_mlxfw_dev *mlxsw_sp_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlxsw_sp_mlxfw_dev, mlxfw_dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_mlxfw_dev->mlxsw_sp;
+	char mcc_pl[MLXSW_REG_MCC_LEN];
+
+	mlxsw_reg_mcc_pack(mcc_pl, MLXSW_REG_MCC_INSTRUCTION_CANCEL, 0,
+			   fwhandle, 0);
+	mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mcc), mcc_pl);
+}
+
+static void mlxsw_sp_fsm_release(struct mlxfw_dev *mlxfw_dev, u32 fwhandle)
+{
+	struct mlxsw_sp_mlxfw_dev *mlxsw_sp_mlxfw_dev =
+		container_of(mlxfw_dev, struct mlxsw_sp_mlxfw_dev, mlxfw_dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_mlxfw_dev->mlxsw_sp;
+	char mcc_pl[MLXSW_REG_MCC_LEN];
+
+	mlxsw_reg_mcc_pack(mcc_pl,
+			   MLXSW_REG_MCC_INSTRUCTION_RELEASE_UPDATE_HANDLE, 0,
+			   fwhandle, 0);
+	mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mcc), mcc_pl);
+}
+
+static const struct mlxfw_dev_ops mlxsw_sp_mlxfw_dev_ops = {
+	.component_query	= mlxsw_sp_component_query,
+	.fsm_lock		= mlxsw_sp_fsm_lock,
+	.fsm_component_update	= mlxsw_sp_fsm_component_update,
+	.fsm_block_download	= mlxsw_sp_fsm_block_download,
+	.fsm_component_verify	= mlxsw_sp_fsm_component_verify,
+	.fsm_activate		= mlxsw_sp_fsm_activate,
+	.fsm_query_state	= mlxsw_sp_fsm_query_state,
+	.fsm_cancel		= mlxsw_sp_fsm_cancel,
+	.fsm_release		= mlxsw_sp_fsm_release
+};
+
+static int mlxsw_sp_firmware_flash(struct mlxsw_sp *mlxsw_sp,
+				   const struct firmware *firmware)
+{
+	struct mlxsw_sp_mlxfw_dev mlxsw_sp_mlxfw_dev = {
+		.mlxfw_dev = {
+			.ops = &mlxsw_sp_mlxfw_dev_ops,
+			.psid = mlxsw_sp->bus_info->psid,
+			.psid_size = strlen(mlxsw_sp->bus_info->psid),
+		},
+		.mlxsw_sp = mlxsw_sp
+	};
+
+	return mlxfw_firmware_flash(&mlxsw_sp_mlxfw_dev.mlxfw_dev, firmware);
+}
+
+static bool mlxsw_sp_fw_rev_ge(const struct mlxsw_fw_rev *a,
+			       const struct mlxsw_fw_rev *b)
+{
+	if (a->major != b->major)
+		return a->major > b->major;
+	if (a->minor != b->minor)
+		return a->minor > b->minor;
+	return a->subminor >= b->subminor;
+}
+
+static int mlxsw_sp_fw_rev_validate(struct mlxsw_sp *mlxsw_sp)
+{
+	const struct mlxsw_fw_rev *rev = &mlxsw_sp->bus_info->fw_rev;
+	const struct firmware *firmware;
+	int err;
+
+	if (mlxsw_sp_fw_rev_ge(rev, &mlxsw_sp_supported_fw_rev))
+		return 0;
+
+	dev_info(mlxsw_sp->bus_info->dev, "The firmware version %d.%d.%d out of data\n",
+		 rev->major, rev->minor, rev->subminor);
+	dev_info(mlxsw_sp->bus_info->dev, "Upgrading firmware using file %s\n",
+		 MLXSW_SP_FW_FILENAME);
+
+	err = request_firmware_direct(&firmware, MLXSW_SP_FW_FILENAME,
+				      mlxsw_sp->bus_info->dev);
+	if (err) {
+		dev_err(mlxsw_sp->bus_info->dev, "Could not request firmware file %s\n",
+			MLXSW_SP_FW_FILENAME);
+		return err;
+	}
+
+	err = mlxsw_sp_firmware_flash(mlxsw_sp, firmware);
+	release_firmware(firmware);
+	return err;
+}
+
 int mlxsw_sp_flow_counter_get(struct mlxsw_sp *mlxsw_sp,
 			      unsigned int counter_index, u64 *packets,
 			      u64 *bytes)
@@ -210,6 +443,41 @@ static void mlxsw_sp_txhdr_construct(struct sk_buff *skb,
 	mlxsw_tx_hdr_type_set(txhdr, MLXSW_TXHDR_TYPE_CONTROL);
 }
 
+int mlxsw_sp_port_vid_stp_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid,
+			      u8 state)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	enum mlxsw_reg_spms_state spms_state;
+	char *spms_pl;
+	int err;
+
+	switch (state) {
+	case BR_STATE_FORWARDING:
+		spms_state = MLXSW_REG_SPMS_STATE_FORWARDING;
+		break;
+	case BR_STATE_LEARNING:
+		spms_state = MLXSW_REG_SPMS_STATE_LEARNING;
+		break;
+	case BR_STATE_LISTENING: /* fall-through */
+	case BR_STATE_DISABLED: /* fall-through */
+	case BR_STATE_BLOCKING:
+		spms_state = MLXSW_REG_SPMS_STATE_DISCARDING;
+		break;
+	default:
+		BUG();
+	}
+
+	spms_pl = kmalloc(MLXSW_REG_SPMS_LEN, GFP_KERNEL);
+	if (!spms_pl)
+		return -ENOMEM;
+	mlxsw_reg_spms_pack(spms_pl, mlxsw_sp_port->local_port);
+	mlxsw_reg_spms_vid_pack(spms_pl, vid, spms_state);
+
+	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(spms), spms_pl);
+	kfree(spms_pl);
+	return err;
+}
+
 static int mlxsw_sp_base_mac_get(struct mlxsw_sp *mlxsw_sp)
 {
 	char spad_pl[MLXSW_REG_SPAD_LEN] = {0};
@@ -592,25 +860,16 @@ static int mlxsw_sp_port_mtu_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 mtu)
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(pmtu), pmtu_pl);
 }
 
-static int __mlxsw_sp_port_swid_set(struct mlxsw_sp *mlxsw_sp, u8 local_port,
-				    u8 swid)
-{
-	char pspa_pl[MLXSW_REG_PSPA_LEN];
-
-	mlxsw_reg_pspa_pack(pspa_pl, swid, local_port);
-	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(pspa), pspa_pl);
-}
-
 static int mlxsw_sp_port_swid_set(struct mlxsw_sp_port *mlxsw_sp_port, u8 swid)
 {
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char pspa_pl[MLXSW_REG_PSPA_LEN];
 
-	return __mlxsw_sp_port_swid_set(mlxsw_sp, mlxsw_sp_port->local_port,
-					swid);
+	mlxsw_reg_pspa_pack(pspa_pl, swid, mlxsw_sp_port->local_port);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(pspa), pspa_pl);
 }
 
-static int mlxsw_sp_port_vp_mode_set(struct mlxsw_sp_port *mlxsw_sp_port,
-				     bool enable)
+int mlxsw_sp_port_vp_mode_set(struct mlxsw_sp_port *mlxsw_sp_port, bool enable)
 {
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	char svpe_pl[MLXSW_REG_SVPE_LEN];
@@ -619,21 +878,8 @@ static int mlxsw_sp_port_vp_mode_set(struct mlxsw_sp_port *mlxsw_sp_port,
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(svpe), svpe_pl);
 }
 
-int mlxsw_sp_port_vid_to_fid_set(struct mlxsw_sp_port *mlxsw_sp_port,
-				 enum mlxsw_reg_svfa_mt mt, bool valid, u16 fid,
-				 u16 vid)
-{
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	char svfa_pl[MLXSW_REG_SVFA_LEN];
-
-	mlxsw_reg_svfa_pack(svfa_pl, mlxsw_sp_port->local_port, mt, valid,
-			    fid, vid);
-	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(svfa), svfa_pl);
-}
-
-int __mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
-				     u16 vid_begin, u16 vid_end,
-				     bool learn_enable)
+int mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid,
+				   bool learn_enable)
 {
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	char *spvmlr_pl;
@@ -642,18 +888,56 @@ int __mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
 	spvmlr_pl = kmalloc(MLXSW_REG_SPVMLR_LEN, GFP_KERNEL);
 	if (!spvmlr_pl)
 		return -ENOMEM;
-	mlxsw_reg_spvmlr_pack(spvmlr_pl, mlxsw_sp_port->local_port, vid_begin,
-			      vid_end, learn_enable);
+	mlxsw_reg_spvmlr_pack(spvmlr_pl, mlxsw_sp_port->local_port, vid, vid,
+			      learn_enable);
 	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(spvmlr), spvmlr_pl);
 	kfree(spvmlr_pl);
 	return err;
 }
 
-static int mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
-					  u16 vid, bool learn_enable)
+static int __mlxsw_sp_port_pvid_set(struct mlxsw_sp_port *mlxsw_sp_port,
+				    u16 vid)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char spvid_pl[MLXSW_REG_SPVID_LEN];
+
+	mlxsw_reg_spvid_pack(spvid_pl, mlxsw_sp_port->local_port, vid);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(spvid), spvid_pl);
+}
+
+static int mlxsw_sp_port_allow_untagged_set(struct mlxsw_sp_port *mlxsw_sp_port,
+					    bool allow)
 {
-	return __mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, vid,
-						learn_enable);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char spaft_pl[MLXSW_REG_SPAFT_LEN];
+
+	mlxsw_reg_spaft_pack(spaft_pl, mlxsw_sp_port->local_port, allow);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(spaft), spaft_pl);
+}
+
+int mlxsw_sp_port_pvid_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
+{
+	int err;
+
+	if (!vid) {
+		err = mlxsw_sp_port_allow_untagged_set(mlxsw_sp_port, false);
+		if (err)
+			return err;
+	} else {
+		err = __mlxsw_sp_port_pvid_set(mlxsw_sp_port, vid);
+		if (err)
+			return err;
+		err = mlxsw_sp_port_allow_untagged_set(mlxsw_sp_port, true);
+		if (err)
+			goto err_port_allow_untagged_set;
+	}
+
+	mlxsw_sp_port->pvid = vid;
+	return 0;
+
+err_port_allow_untagged_set:
+	__mlxsw_sp_port_pvid_set(mlxsw_sp_port, mlxsw_sp_port->pvid);
+	return err;
 }
 
 static int
@@ -683,13 +967,14 @@ static int mlxsw_sp_port_module_info_get(struct mlxsw_sp *mlxsw_sp,
 	return 0;
 }
 
-static int mlxsw_sp_port_module_map(struct mlxsw_sp *mlxsw_sp, u8 local_port,
+static int mlxsw_sp_port_module_map(struct mlxsw_sp_port *mlxsw_sp_port,
 				    u8 module, u8 width, u8 lane)
 {
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	char pmlp_pl[MLXSW_REG_PMLP_LEN];
 	int i;
 
-	mlxsw_reg_pmlp_pack(pmlp_pl, local_port);
+	mlxsw_reg_pmlp_pack(pmlp_pl, mlxsw_sp_port->local_port);
 	mlxsw_reg_pmlp_width_set(pmlp_pl, width);
 	for (i = 0; i < width; i++) {
 		mlxsw_reg_pmlp_module_set(pmlp_pl, i, module);
@@ -699,11 +984,12 @@ static int mlxsw_sp_port_module_map(struct mlxsw_sp *mlxsw_sp, u8 local_port,
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(pmlp), pmlp_pl);
 }
 
-static int mlxsw_sp_port_module_unmap(struct mlxsw_sp *mlxsw_sp, u8 local_port)
+static int mlxsw_sp_port_module_unmap(struct mlxsw_sp_port *mlxsw_sp_port)
 {
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	char pmlp_pl[MLXSW_REG_PMLP_LEN];
 
-	mlxsw_reg_pmlp_pack(pmlp_pl, local_port);
+	mlxsw_reg_pmlp_pack(pmlp_pl, mlxsw_sp_port->local_port);
 	mlxsw_reg_pmlp_width_set(pmlp_pl, 0);
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(pmlp), pmlp_pl);
 }
@@ -1100,95 +1386,82 @@ int mlxsw_sp_port_vlan_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid_begin,
 	return 0;
 }
 
-static int mlxsw_sp_port_vp_mode_trans(struct mlxsw_sp_port *mlxsw_sp_port)
+static void mlxsw_sp_port_vlan_flush(struct mlxsw_sp_port *mlxsw_sp_port)
 {
-	enum mlxsw_reg_svfa_mt mt = MLXSW_REG_SVFA_MT_PORT_VID_TO_FID;
-	u16 vid, last_visited_vid;
-	int err;
-
-	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID) {
-		err = mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_port, mt, true, vid,
-						   vid);
-		if (err) {
-			last_visited_vid = vid;
-			goto err_port_vid_to_fid_set;
-		}
-	}
-
-	err = mlxsw_sp_port_vp_mode_set(mlxsw_sp_port, true);
-	if (err) {
-		last_visited_vid = VLAN_N_VID;
-		goto err_port_vid_to_fid_set;
-	}
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan, *tmp;
 
-	return 0;
-
-err_port_vid_to_fid_set:
-	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, last_visited_vid)
-		mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_port, mt, false, vid,
-					     vid);
-	return err;
+	list_for_each_entry_safe(mlxsw_sp_port_vlan, tmp,
+				 &mlxsw_sp_port->vlans_list, list)
+		mlxsw_sp_port_vlan_put(mlxsw_sp_port_vlan);
 }
 
-static int mlxsw_sp_port_vlan_mode_trans(struct mlxsw_sp_port *mlxsw_sp_port)
+static struct mlxsw_sp_port_vlan *
+mlxsw_sp_port_vlan_create(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
 {
-	enum mlxsw_reg_svfa_mt mt = MLXSW_REG_SVFA_MT_PORT_VID_TO_FID;
-	u16 vid;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
+	bool untagged = vid == 1;
 	int err;
 
-	err = mlxsw_sp_port_vp_mode_set(mlxsw_sp_port, false);
+	err = mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid, vid, true, untagged);
 	if (err)
-		return err;
+		return ERR_PTR(err);
 
-	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID) {
-		err = mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_port, mt, false,
-						   vid, vid);
-		if (err)
-			return err;
+	mlxsw_sp_port_vlan = kzalloc(sizeof(*mlxsw_sp_port_vlan), GFP_KERNEL);
+	if (!mlxsw_sp_port_vlan) {
+		err = -ENOMEM;
+		goto err_port_vlan_alloc;
 	}
 
-	return 0;
+	mlxsw_sp_port_vlan->mlxsw_sp_port = mlxsw_sp_port;
+	mlxsw_sp_port_vlan->vid = vid;
+	list_add(&mlxsw_sp_port_vlan->list, &mlxsw_sp_port->vlans_list);
+
+	return mlxsw_sp_port_vlan;
+
+err_port_vlan_alloc:
+	mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid, vid, false, false);
+	return ERR_PTR(err);
 }
 
-static struct mlxsw_sp_port *
-mlxsw_sp_port_vport_create(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
+static void
+mlxsw_sp_port_vlan_destroy(struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan)
 {
-	struct mlxsw_sp_port *mlxsw_sp_vport;
+	struct mlxsw_sp_port *mlxsw_sp_port = mlxsw_sp_port_vlan->mlxsw_sp_port;
+	u16 vid = mlxsw_sp_port_vlan->vid;
 
-	mlxsw_sp_vport = kzalloc(sizeof(*mlxsw_sp_vport), GFP_KERNEL);
-	if (!mlxsw_sp_vport)
-		return NULL;
+	list_del(&mlxsw_sp_port_vlan->list);
+	kfree(mlxsw_sp_port_vlan);
+	mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid, vid, false, false);
+}
 
-	/* dev will be set correctly after the VLAN device is linked
-	 * with the real device. In case of bridge SELF invocation, dev
-	 * will remain as is.
-	 */
-	mlxsw_sp_vport->dev = mlxsw_sp_port->dev;
-	mlxsw_sp_vport->mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	mlxsw_sp_vport->local_port = mlxsw_sp_port->local_port;
-	mlxsw_sp_vport->stp_state = BR_STATE_FORWARDING;
-	mlxsw_sp_vport->lagged = mlxsw_sp_port->lagged;
-	mlxsw_sp_vport->lag_id = mlxsw_sp_port->lag_id;
-	mlxsw_sp_vport->vport.vid = vid;
+struct mlxsw_sp_port_vlan *
+mlxsw_sp_port_vlan_get(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
+{
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 
-	list_add(&mlxsw_sp_vport->vport.list, &mlxsw_sp_port->vports_list);
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_vid(mlxsw_sp_port, vid);
+	if (mlxsw_sp_port_vlan)
+		return mlxsw_sp_port_vlan;
 
-	return mlxsw_sp_vport;
+	return mlxsw_sp_port_vlan_create(mlxsw_sp_port, vid);
 }
 
-static void mlxsw_sp_port_vport_destroy(struct mlxsw_sp_port *mlxsw_sp_vport)
+void mlxsw_sp_port_vlan_put(struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan)
 {
-	list_del(&mlxsw_sp_vport->vport.list);
-	kfree(mlxsw_sp_vport);
+	struct mlxsw_sp_fid *fid = mlxsw_sp_port_vlan->fid;
+
+	if (mlxsw_sp_port_vlan->bridge_port)
+		mlxsw_sp_port_vlan_bridge_leave(mlxsw_sp_port_vlan);
+	else if (fid)
+		mlxsw_sp_port_vlan_router_leave(mlxsw_sp_port_vlan);
+
+	mlxsw_sp_port_vlan_destroy(mlxsw_sp_port_vlan);
 }
 
 static int mlxsw_sp_port_add_vid(struct net_device *dev,
 				 __be16 __always_unused proto, u16 vid)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
-	struct mlxsw_sp_port *mlxsw_sp_vport;
-	bool untagged = vid == 1;
-	int err;
 
 	/* VLAN 0 is added to HW filter when device goes up, but it is
 	 * reserved in our case, so simply return.
@@ -1196,43 +1469,14 @@ static int mlxsw_sp_port_add_vid(struct net_device *dev,
 	if (!vid)
 		return 0;
 
-	if (mlxsw_sp_port_vport_find(mlxsw_sp_port, vid))
-		return 0;
-
-	mlxsw_sp_vport = mlxsw_sp_port_vport_create(mlxsw_sp_port, vid);
-	if (!mlxsw_sp_vport)
-		return -ENOMEM;
-
-	/* When adding the first VLAN interface on a bridged port we need to
-	 * transition all the active 802.1Q bridge VLANs to use explicit
-	 * {Port, VID} to FID mappings and set the port's mode to Virtual mode.
-	 */
-	if (list_is_singular(&mlxsw_sp_port->vports_list)) {
-		err = mlxsw_sp_port_vp_mode_trans(mlxsw_sp_port);
-		if (err)
-			goto err_port_vp_mode_trans;
-	}
-
-	err = mlxsw_sp_port_vlan_set(mlxsw_sp_vport, vid, vid, true, untagged);
-	if (err)
-		goto err_port_add_vid;
-
-	return 0;
-
-err_port_add_vid:
-	if (list_is_singular(&mlxsw_sp_port->vports_list))
-		mlxsw_sp_port_vlan_mode_trans(mlxsw_sp_port);
-err_port_vp_mode_trans:
-	mlxsw_sp_port_vport_destroy(mlxsw_sp_vport);
-	return err;
+	return PTR_ERR_OR_ZERO(mlxsw_sp_port_vlan_get(mlxsw_sp_port, vid));
 }
 
 static int mlxsw_sp_port_kill_vid(struct net_device *dev,
 				  __be16 __always_unused proto, u16 vid)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
-	struct mlxsw_sp_port *mlxsw_sp_vport;
-	struct mlxsw_sp_fid *f;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 
 	/* VLAN 0 is removed from HW filter when device goes down, but
 	 * it is reserved in our case, so simply return.
@@ -1240,27 +1484,10 @@ static int mlxsw_sp_port_kill_vid(struct net_device *dev,
 	if (!vid)
 		return 0;
 
-	mlxsw_sp_vport = mlxsw_sp_port_vport_find(mlxsw_sp_port, vid);
-	if (WARN_ON(!mlxsw_sp_vport))
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_vid(mlxsw_sp_port, vid);
+	if (!mlxsw_sp_port_vlan)
 		return 0;
-
-	mlxsw_sp_port_vlan_set(mlxsw_sp_vport, vid, vid, false, false);
-
-	/* Drop FID reference. If this was the last reference the
-	 * resources will be freed.
-	 */
-	f = mlxsw_sp_vport_fid_get(mlxsw_sp_vport);
-	if (f && !WARN_ON(!f->leave))
-		f->leave(mlxsw_sp_vport);
-
-	/* When removing the last VLAN interface on a bridged port we need to
-	 * transition all active 802.1Q bridge VLANs to use VID to FID
-	 * mappings and set port's mode to VLAN mode.
-	 */
-	if (list_is_singular(&mlxsw_sp_port->vports_list))
-		mlxsw_sp_port_vlan_mode_trans(mlxsw_sp_port);
-
-	mlxsw_sp_port_vport_destroy(mlxsw_sp_vport);
+	mlxsw_sp_port_vlan_put(mlxsw_sp_port_vlan);
 
 	return 0;
 }
@@ -1466,11 +1693,15 @@ static void mlxsw_sp_port_del_cls_matchall(struct mlxsw_sp_port *mlxsw_sp_port,
 }
 
 static int mlxsw_sp_setup_tc(struct net_device *dev, u32 handle,
-			     __be16 proto, struct tc_to_netdev *tc)
+			     u32 chain_index, __be16 proto,
+			     struct tc_to_netdev *tc)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	bool ingress = TC_H_MAJ(handle) == TC_H_MAJ(TC_H_INGRESS);
 
+	if (chain_index)
+		return -EOPNOTSUPP;
+
 	switch (tc->type) {
 	case TC_SETUP_MATCHALL:
 		switch (tc->cls_mall->command) {
@@ -1519,12 +1750,6 @@ static const struct net_device_ops mlxsw_sp_port_netdev_ops = {
 	.ndo_get_offload_stats	= mlxsw_sp_port_get_offload_stats,
 	.ndo_vlan_rx_add_vid	= mlxsw_sp_port_add_vid,
 	.ndo_vlan_rx_kill_vid	= mlxsw_sp_port_kill_vid,
-	.ndo_fdb_add		= switchdev_port_fdb_add,
-	.ndo_fdb_del		= switchdev_port_fdb_del,
-	.ndo_fdb_dump		= switchdev_port_fdb_dump,
-	.ndo_bridge_setlink	= switchdev_port_bridge_setlink,
-	.ndo_bridge_getlink	= switchdev_port_bridge_getlink,
-	.ndo_bridge_dellink	= switchdev_port_bridge_dellink,
 	.ndo_get_phys_port_name	= mlxsw_sp_port_get_phys_port_name,
 };
 
@@ -2269,6 +2494,160 @@ mlxsw_sp_port_set_link_ksettings(struct net_device *dev,
 	return 0;
 }
 
+static int mlxsw_sp_flash_device(struct net_device *dev,
+				 struct ethtool_flash *flash)
+{
+	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	const struct firmware *firmware;
+	int err;
+
+	if (flash->region != ETHTOOL_FLASH_ALL_REGIONS)
+		return -EOPNOTSUPP;
+
+	dev_hold(dev);
+	rtnl_unlock();
+
+	err = request_firmware_direct(&firmware, flash->data, &dev->dev);
+	if (err)
+		goto out;
+	err = mlxsw_sp_firmware_flash(mlxsw_sp, firmware);
+	release_firmware(firmware);
+out:
+	rtnl_lock();
+	dev_put(dev);
+	return err;
+}
+
+#define MLXSW_SP_QSFP_I2C_ADDR 0x50
+
+static int mlxsw_sp_query_module_eeprom(struct mlxsw_sp_port *mlxsw_sp_port,
+					u16 offset, u16 size, void *data,
+					unsigned int *p_read_size)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char eeprom_tmp[MLXSW_SP_REG_MCIA_EEPROM_SIZE];
+	char mcia_pl[MLXSW_REG_MCIA_LEN];
+	int status;
+	int err;
+
+	size = min_t(u16, size, MLXSW_SP_REG_MCIA_EEPROM_SIZE);
+	mlxsw_reg_mcia_pack(mcia_pl, mlxsw_sp_port->mapping.module,
+			    0, 0, offset, size, MLXSW_SP_QSFP_I2C_ADDR);
+
+	err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(mcia), mcia_pl);
+	if (err)
+		return err;
+
+	status = mlxsw_reg_mcia_status_get(mcia_pl);
+	if (status)
+		return -EIO;
+
+	mlxsw_reg_mcia_eeprom_memcpy_from(mcia_pl, eeprom_tmp);
+	memcpy(data, eeprom_tmp, size);
+	*p_read_size = size;
+
+	return 0;
+}
+
+enum mlxsw_sp_eeprom_module_info_rev_id {
+	MLXSW_SP_EEPROM_MODULE_INFO_REV_ID_UNSPC      = 0x00,
+	MLXSW_SP_EEPROM_MODULE_INFO_REV_ID_8436       = 0x01,
+	MLXSW_SP_EEPROM_MODULE_INFO_REV_ID_8636       = 0x03,
+};
+
+enum mlxsw_sp_eeprom_module_info_id {
+	MLXSW_SP_EEPROM_MODULE_INFO_ID_SFP              = 0x03,
+	MLXSW_SP_EEPROM_MODULE_INFO_ID_QSFP             = 0x0C,
+	MLXSW_SP_EEPROM_MODULE_INFO_ID_QSFP_PLUS        = 0x0D,
+	MLXSW_SP_EEPROM_MODULE_INFO_ID_QSFP28           = 0x11,
+};
+
+enum mlxsw_sp_eeprom_module_info {
+	MLXSW_SP_EEPROM_MODULE_INFO_ID,
+	MLXSW_SP_EEPROM_MODULE_INFO_REV_ID,
+	MLXSW_SP_EEPROM_MODULE_INFO_SIZE,
+};
+
+static int mlxsw_sp_get_module_info(struct net_device *netdev,
+				    struct ethtool_modinfo *modinfo)
+{
+	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(netdev);
+	u8 module_info[MLXSW_SP_EEPROM_MODULE_INFO_SIZE];
+	u8 module_rev_id, module_id;
+	unsigned int read_size;
+	int err;
+
+	err = mlxsw_sp_query_module_eeprom(mlxsw_sp_port, 0,
+					   MLXSW_SP_EEPROM_MODULE_INFO_SIZE,
+					   module_info, &read_size);
+	if (err)
+		return err;
+
+	if (read_size < MLXSW_SP_EEPROM_MODULE_INFO_SIZE)
+		return -EIO;
+
+	module_rev_id = module_info[MLXSW_SP_EEPROM_MODULE_INFO_REV_ID];
+	module_id = module_info[MLXSW_SP_EEPROM_MODULE_INFO_ID];
+
+	switch (module_id) {
+	case MLXSW_SP_EEPROM_MODULE_INFO_ID_QSFP:
+		modinfo->type       = ETH_MODULE_SFF_8436;
+		modinfo->eeprom_len = ETH_MODULE_SFF_8436_LEN;
+		break;
+	case MLXSW_SP_EEPROM_MODULE_INFO_ID_QSFP_PLUS:
+	case MLXSW_SP_EEPROM_MODULE_INFO_ID_QSFP28:
+		if (module_id  == MLXSW_SP_EEPROM_MODULE_INFO_ID_QSFP28 ||
+		    module_rev_id >= MLXSW_SP_EEPROM_MODULE_INFO_REV_ID_8636) {
+			modinfo->type       = ETH_MODULE_SFF_8636;
+			modinfo->eeprom_len = ETH_MODULE_SFF_8636_LEN;
+		} else {
+			modinfo->type       = ETH_MODULE_SFF_8436;
+			modinfo->eeprom_len = ETH_MODULE_SFF_8436_LEN;
+		}
+		break;
+	case MLXSW_SP_EEPROM_MODULE_INFO_ID_SFP:
+		modinfo->type       = ETH_MODULE_SFF_8472;
+		modinfo->eeprom_len = ETH_MODULE_SFF_8472_LEN;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int mlxsw_sp_get_module_eeprom(struct net_device *netdev,
+				      struct ethtool_eeprom *ee,
+				      u8 *data)
+{
+	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(netdev);
+	int offset = ee->offset;
+	unsigned int read_size;
+	int i = 0;
+	int err;
+
+	if (!ee->len)
+		return -EINVAL;
+
+	memset(data, 0, ee->len);
+
+	while (i < ee->len) {
+		err = mlxsw_sp_query_module_eeprom(mlxsw_sp_port, offset,
+						   ee->len - i, data + i,
+						   &read_size);
+		if (err) {
+			netdev_err(mlxsw_sp_port->dev, "Eeprom query failed\n");
+			return err;
+		}
+
+		i += read_size;
+		offset += read_size;
+	}
+
+	return 0;
+}
+
 static const struct ethtool_ops mlxsw_sp_port_ethtool_ops = {
 	.get_drvinfo		= mlxsw_sp_port_get_drvinfo,
 	.get_link		= ethtool_op_get_link,
@@ -2280,6 +2659,9 @@ static const struct ethtool_ops mlxsw_sp_port_ethtool_ops = {
 	.get_sset_count		= mlxsw_sp_port_get_sset_count,
 	.get_link_ksettings	= mlxsw_sp_port_get_link_ksettings,
 	.set_link_ksettings	= mlxsw_sp_port_set_link_ksettings,
+	.flash_device		= mlxsw_sp_flash_device,
+	.get_module_info	= mlxsw_sp_get_module_info,
+	.get_module_eeprom	= mlxsw_sp_get_module_eeprom,
 };
 
 static int
@@ -2398,51 +2780,38 @@ static int mlxsw_sp_port_ets_init(struct mlxsw_sp_port *mlxsw_sp_port)
 	return 0;
 }
 
-static int mlxsw_sp_port_pvid_vport_create(struct mlxsw_sp_port *mlxsw_sp_port)
-{
-	mlxsw_sp_port->pvid = 1;
-
-	return mlxsw_sp_port_add_vid(mlxsw_sp_port->dev, 0, 1);
-}
-
-static int mlxsw_sp_port_pvid_vport_destroy(struct mlxsw_sp_port *mlxsw_sp_port)
-{
-	return mlxsw_sp_port_kill_vid(mlxsw_sp_port->dev, 0, 1);
-}
-
-static int __mlxsw_sp_port_create(struct mlxsw_sp *mlxsw_sp, u8 local_port,
-				  bool split, u8 module, u8 width, u8 lane)
+static int mlxsw_sp_port_create(struct mlxsw_sp *mlxsw_sp, u8 local_port,
+				bool split, u8 module, u8 width, u8 lane)
 {
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 	struct mlxsw_sp_port *mlxsw_sp_port;
 	struct net_device *dev;
-	size_t bytes;
 	int err;
 
+	err = mlxsw_core_port_init(mlxsw_sp->core, local_port);
+	if (err) {
+		dev_err(mlxsw_sp->bus_info->dev, "Port %d: Failed to init core port\n",
+			local_port);
+		return err;
+	}
+
 	dev = alloc_etherdev(sizeof(struct mlxsw_sp_port));
-	if (!dev)
-		return -ENOMEM;
+	if (!dev) {
+		err = -ENOMEM;
+		goto err_alloc_etherdev;
+	}
 	SET_NETDEV_DEV(dev, mlxsw_sp->bus_info->dev);
 	mlxsw_sp_port = netdev_priv(dev);
 	mlxsw_sp_port->dev = dev;
 	mlxsw_sp_port->mlxsw_sp = mlxsw_sp;
 	mlxsw_sp_port->local_port = local_port;
+	mlxsw_sp_port->pvid = 1;
 	mlxsw_sp_port->split = split;
 	mlxsw_sp_port->mapping.module = module;
 	mlxsw_sp_port->mapping.width = width;
 	mlxsw_sp_port->mapping.lane = lane;
 	mlxsw_sp_port->link.autoneg = 1;
-	bytes = DIV_ROUND_UP(VLAN_N_VID, BITS_PER_BYTE);
-	mlxsw_sp_port->active_vlans = kzalloc(bytes, GFP_KERNEL);
-	if (!mlxsw_sp_port->active_vlans) {
-		err = -ENOMEM;
-		goto err_port_active_vlans_alloc;
-	}
-	mlxsw_sp_port->untagged_vlans = kzalloc(bytes, GFP_KERNEL);
-	if (!mlxsw_sp_port->untagged_vlans) {
-		err = -ENOMEM;
-		goto err_port_untagged_vlans_alloc;
-	}
-	INIT_LIST_HEAD(&mlxsw_sp_port->vports_list);
+	INIT_LIST_HEAD(&mlxsw_sp_port->vlans_list);
 	INIT_LIST_HEAD(&mlxsw_sp_port->mall_tc_list);
 
 	mlxsw_sp_port->pcpu_stats =
@@ -2472,6 +2841,13 @@ static int __mlxsw_sp_port_create(struct mlxsw_sp *mlxsw_sp, u8 local_port,
 	dev->netdev_ops = &mlxsw_sp_port_netdev_ops;
 	dev->ethtool_ops = &mlxsw_sp_port_ethtool_ops;
 
+	err = mlxsw_sp_port_module_map(mlxsw_sp_port, module, width, lane);
+	if (err) {
+		dev_err(mlxsw_sp->bus_info->dev, "Port %d: Failed to map module\n",
+			mlxsw_sp_port->local_port);
+		goto err_port_module_map;
+	}
+
 	err = mlxsw_sp_port_swid_set(mlxsw_sp_port, 0);
 	if (err) {
 		dev_err(mlxsw_sp->bus_info->dev, "Port %d: Failed to set SWID\n",
@@ -2547,11 +2923,18 @@ static int __mlxsw_sp_port_create(struct mlxsw_sp *mlxsw_sp, u8 local_port,
 		goto err_port_dcb_init;
 	}
 
-	err = mlxsw_sp_port_pvid_vport_create(mlxsw_sp_port);
+	err = mlxsw_sp_port_fids_init(mlxsw_sp_port);
 	if (err) {
-		dev_err(mlxsw_sp->bus_info->dev, "Port %d: Failed to create PVID vPort\n",
+		dev_err(mlxsw_sp->bus_info->dev, "Port %d: Failed to initialize FIDs\n",
 			mlxsw_sp_port->local_port);
-		goto err_port_pvid_vport_create;
+		goto err_port_fids_init;
+	}
+
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_get(mlxsw_sp_port, 1);
+	if (IS_ERR(mlxsw_sp_port_vlan)) {
+		dev_err(mlxsw_sp->bus_info->dev, "Port %d: Failed to create VID 1\n",
+			mlxsw_sp_port->local_port);
+		goto err_port_vlan_get;
 	}
 
 	mlxsw_sp_port_switchdev_init(mlxsw_sp_port);
@@ -2572,8 +2955,10 @@ static int __mlxsw_sp_port_create(struct mlxsw_sp *mlxsw_sp, u8 local_port,
 err_register_netdev:
 	mlxsw_sp->ports[local_port] = NULL;
 	mlxsw_sp_port_switchdev_fini(mlxsw_sp_port);
-	mlxsw_sp_port_pvid_vport_destroy(mlxsw_sp_port);
-err_port_pvid_vport_create:
+	mlxsw_sp_port_vlan_put(mlxsw_sp_port_vlan);
+err_port_vlan_get:
+	mlxsw_sp_port_fids_fini(mlxsw_sp_port);
+err_port_fids_init:
 	mlxsw_sp_port_dcb_fini(mlxsw_sp_port);
 err_port_dcb_init:
 err_port_ets_init:
@@ -2585,43 +2970,21 @@ err_port_system_port_mapping_set:
 err_dev_addr_init:
 	mlxsw_sp_port_swid_set(mlxsw_sp_port, MLXSW_PORT_SWID_DISABLED_PORT);
 err_port_swid_set:
+	mlxsw_sp_port_module_unmap(mlxsw_sp_port);
+err_port_module_map:
 	kfree(mlxsw_sp_port->hw_stats.cache);
 err_alloc_hw_stats:
 	kfree(mlxsw_sp_port->sample);
 err_alloc_sample:
 	free_percpu(mlxsw_sp_port->pcpu_stats);
 err_alloc_stats:
-	kfree(mlxsw_sp_port->untagged_vlans);
-err_port_untagged_vlans_alloc:
-	kfree(mlxsw_sp_port->active_vlans);
-err_port_active_vlans_alloc:
 	free_netdev(dev);
-	return err;
-}
-
-static int mlxsw_sp_port_create(struct mlxsw_sp *mlxsw_sp, u8 local_port,
-				bool split, u8 module, u8 width, u8 lane)
-{
-	int err;
-
-	err = mlxsw_core_port_init(mlxsw_sp->core, local_port);
-	if (err) {
-		dev_err(mlxsw_sp->bus_info->dev, "Port %d: Failed to init core port\n",
-			local_port);
-		return err;
-	}
-	err = __mlxsw_sp_port_create(mlxsw_sp, local_port, split,
-				     module, width, lane);
-	if (err)
-		goto err_port_create;
-	return 0;
-
-err_port_create:
+err_alloc_etherdev:
 	mlxsw_core_port_fini(mlxsw_sp->core, local_port);
 	return err;
 }
 
-static void __mlxsw_sp_port_remove(struct mlxsw_sp *mlxsw_sp, u8 local_port)
+static void mlxsw_sp_port_remove(struct mlxsw_sp *mlxsw_sp, u8 local_port)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port = mlxsw_sp->ports[local_port];
 
@@ -2630,22 +2993,16 @@ static void __mlxsw_sp_port_remove(struct mlxsw_sp *mlxsw_sp, u8 local_port)
 	unregister_netdev(mlxsw_sp_port->dev); /* This calls ndo_stop */
 	mlxsw_sp->ports[local_port] = NULL;
 	mlxsw_sp_port_switchdev_fini(mlxsw_sp_port);
-	mlxsw_sp_port_pvid_vport_destroy(mlxsw_sp_port);
+	mlxsw_sp_port_vlan_flush(mlxsw_sp_port);
+	mlxsw_sp_port_fids_fini(mlxsw_sp_port);
 	mlxsw_sp_port_dcb_fini(mlxsw_sp_port);
 	mlxsw_sp_port_swid_set(mlxsw_sp_port, MLXSW_PORT_SWID_DISABLED_PORT);
-	mlxsw_sp_port_module_unmap(mlxsw_sp, mlxsw_sp_port->local_port);
+	mlxsw_sp_port_module_unmap(mlxsw_sp_port);
 	kfree(mlxsw_sp_port->hw_stats.cache);
 	kfree(mlxsw_sp_port->sample);
 	free_percpu(mlxsw_sp_port->pcpu_stats);
-	kfree(mlxsw_sp_port->untagged_vlans);
-	kfree(mlxsw_sp_port->active_vlans);
-	WARN_ON_ONCE(!list_empty(&mlxsw_sp_port->vports_list));
+	WARN_ON_ONCE(!list_empty(&mlxsw_sp_port->vlans_list));
 	free_netdev(mlxsw_sp_port->dev);
-}
-
-static void mlxsw_sp_port_remove(struct mlxsw_sp *mlxsw_sp, u8 local_port)
-{
-	__mlxsw_sp_port_remove(mlxsw_sp, local_port);
 	mlxsw_core_port_fini(mlxsw_sp->core, local_port);
 }
 
@@ -2724,19 +3081,6 @@ static int mlxsw_sp_port_split_create(struct mlxsw_sp *mlxsw_sp, u8 base_port,
 	int err, i;
 
 	for (i = 0; i < count; i++) {
-		err = mlxsw_sp_port_module_map(mlxsw_sp, base_port + i, module,
-					       width, i * width);
-		if (err)
-			goto err_port_module_map;
-	}
-
-	for (i = 0; i < count; i++) {
-		err = __mlxsw_sp_port_swid_set(mlxsw_sp, base_port + i, 0);
-		if (err)
-			goto err_port_swid_set;
-	}
-
-	for (i = 0; i < count; i++) {
 		err = mlxsw_sp_port_create(mlxsw_sp, base_port + i, true,
 					   module, width, i * width);
 		if (err)
@@ -2749,15 +3093,6 @@ err_port_create:
 	for (i--; i >= 0; i--)
 		if (mlxsw_sp_port_created(mlxsw_sp, base_port + i))
 			mlxsw_sp_port_remove(mlxsw_sp, base_port + i);
-	i = count;
-err_port_swid_set:
-	for (i--; i >= 0; i--)
-		__mlxsw_sp_port_swid_set(mlxsw_sp, base_port + i,
-					 MLXSW_PORT_SWID_DISABLED_PORT);
-	i = count;
-err_port_module_map:
-	for (i--; i >= 0; i--)
-		mlxsw_sp_port_module_unmap(mlxsw_sp, base_port + i);
 	return err;
 }
 
@@ -2776,17 +3111,6 @@ static void mlxsw_sp_port_unsplit_create(struct mlxsw_sp *mlxsw_sp,
 		local_port = base_port + i * 2;
 		module = mlxsw_sp->port_to_module[local_port];
 
-		mlxsw_sp_port_module_map(mlxsw_sp, local_port, module, width,
-					 0);
-	}
-
-	for (i = 0; i < count; i++)
-		__mlxsw_sp_port_swid_set(mlxsw_sp, base_port + i * 2, 0);
-
-	for (i = 0; i < count; i++) {
-		local_port = base_port + i * 2;
-		module = mlxsw_sp->port_to_module[local_port];
-
 		mlxsw_sp_port_create(mlxsw_sp, local_port, false, module,
 				     width, 0);
 	}
@@ -3020,7 +3344,9 @@ static const struct mlxsw_listener mlxsw_sp_listener[] = {
 	MLXSW_SP_RXL_NO_MARK(BGP_IPV4, TRAP_TO_CPU, BGP_IPV4, false),
 	/* PKT Sample trap */
 	MLXSW_RXL(mlxsw_sp_rx_listener_sample_func, PKT_SAMPLE, MIRROR_TO_CPU,
-		  false, SP_IP2ME, DISCARD)
+		  false, SP_IP2ME, DISCARD),
+	/* ACL trap */
+	MLXSW_SP_RXL_NO_MARK(ACL0, TRAP_TO_CPU, IP2ME, false),
 };
 
 static int mlxsw_sp_cpu_policers_set(struct mlxsw_core *mlxsw_core)
@@ -3192,57 +3518,6 @@ static void mlxsw_sp_traps_fini(struct mlxsw_sp *mlxsw_sp)
 	}
 }
 
-static int __mlxsw_sp_flood_init(struct mlxsw_core *mlxsw_core,
-				 enum mlxsw_reg_sfgc_type type,
-				 enum mlxsw_reg_sfgc_bridge_type bridge_type)
-{
-	enum mlxsw_flood_table_type table_type;
-	enum mlxsw_sp_flood_table flood_table;
-	char sfgc_pl[MLXSW_REG_SFGC_LEN];
-
-	if (bridge_type == MLXSW_REG_SFGC_BRIDGE_TYPE_VFID)
-		table_type = MLXSW_REG_SFGC_TABLE_TYPE_FID;
-	else
-		table_type = MLXSW_REG_SFGC_TABLE_TYPE_FID_OFFEST;
-
-	switch (type) {
-	case MLXSW_REG_SFGC_TYPE_UNKNOWN_UNICAST:
-		flood_table = MLXSW_SP_FLOOD_TABLE_UC;
-		break;
-	case MLXSW_REG_SFGC_TYPE_UNREGISTERED_MULTICAST_IPV4:
-		flood_table = MLXSW_SP_FLOOD_TABLE_MC;
-		break;
-	default:
-		flood_table = MLXSW_SP_FLOOD_TABLE_BC;
-	}
-
-	mlxsw_reg_sfgc_pack(sfgc_pl, type, bridge_type, table_type,
-			    flood_table);
-	return mlxsw_reg_write(mlxsw_core, MLXSW_REG(sfgc), sfgc_pl);
-}
-
-static int mlxsw_sp_flood_init(struct mlxsw_sp *mlxsw_sp)
-{
-	int type, err;
-
-	for (type = 0; type < MLXSW_REG_SFGC_TYPE_MAX; type++) {
-		if (type == MLXSW_REG_SFGC_TYPE_RESERVED)
-			continue;
-
-		err = __mlxsw_sp_flood_init(mlxsw_sp->core, type,
-					    MLXSW_REG_SFGC_BRIDGE_TYPE_VFID);
-		if (err)
-			return err;
-
-		err = __mlxsw_sp_flood_init(mlxsw_sp->core, type,
-					    MLXSW_REG_SFGC_BRIDGE_TYPE_1Q_FID);
-		if (err)
-			return err;
-	}
-
-	return 0;
-}
-
 static int mlxsw_sp_lag_init(struct mlxsw_sp *mlxsw_sp)
 {
 	char slcr_pl[MLXSW_REG_SLCR_LEN];
@@ -3290,18 +3565,6 @@ static int mlxsw_sp_basic_trap_groups_set(struct mlxsw_core *mlxsw_core)
 	return mlxsw_reg_write(mlxsw_core, MLXSW_REG(htgt), htgt_pl);
 }
 
-static int mlxsw_sp_vfid_op(struct mlxsw_sp *mlxsw_sp, u16 fid, bool create);
-
-static int mlxsw_sp_dummy_fid_init(struct mlxsw_sp *mlxsw_sp)
-{
-	return mlxsw_sp_vfid_op(mlxsw_sp, MLXSW_SP_DUMMY_FID, true);
-}
-
-static void mlxsw_sp_dummy_fid_fini(struct mlxsw_sp *mlxsw_sp)
-{
-	mlxsw_sp_vfid_op(mlxsw_sp, MLXSW_SP_DUMMY_FID, false);
-}
-
 static int mlxsw_sp_init(struct mlxsw_core *mlxsw_core,
 			 const struct mlxsw_bus_info *mlxsw_bus_info)
 {
@@ -3310,9 +3573,12 @@ static int mlxsw_sp_init(struct mlxsw_core *mlxsw_core,
 
 	mlxsw_sp->core = mlxsw_core;
 	mlxsw_sp->bus_info = mlxsw_bus_info;
-	INIT_LIST_HEAD(&mlxsw_sp->fids);
-	INIT_LIST_HEAD(&mlxsw_sp->vfids.list);
-	INIT_LIST_HEAD(&mlxsw_sp->br_mids.list);
+
+	err = mlxsw_sp_fw_rev_validate(mlxsw_sp);
+	if (err) {
+		dev_err(mlxsw_sp->bus_info->dev, "Could not upgrade firmware\n");
+		return err;
+	}
 
 	err = mlxsw_sp_base_mac_get(mlxsw_sp);
 	if (err) {
@@ -3320,16 +3586,16 @@ static int mlxsw_sp_init(struct mlxsw_core *mlxsw_core,
 		return err;
 	}
 
-	err = mlxsw_sp_traps_init(mlxsw_sp);
+	err = mlxsw_sp_fids_init(mlxsw_sp);
 	if (err) {
-		dev_err(mlxsw_sp->bus_info->dev, "Failed to set traps\n");
+		dev_err(mlxsw_sp->bus_info->dev, "Failed to initialize FIDs\n");
 		return err;
 	}
 
-	err = mlxsw_sp_flood_init(mlxsw_sp);
+	err = mlxsw_sp_traps_init(mlxsw_sp);
 	if (err) {
-		dev_err(mlxsw_sp->bus_info->dev, "Failed to initialize flood tables\n");
-		goto err_flood_init;
+		dev_err(mlxsw_sp->bus_info->dev, "Failed to set traps\n");
+		goto err_traps_init;
 	}
 
 	err = mlxsw_sp_buffers_init(mlxsw_sp);
@@ -3380,12 +3646,6 @@ static int mlxsw_sp_init(struct mlxsw_core *mlxsw_core,
 		goto err_dpipe_init;
 	}
 
-	err = mlxsw_sp_dummy_fid_init(mlxsw_sp);
-	if (err) {
-		dev_err(mlxsw_sp->bus_info->dev, "Failed to init dummy FID\n");
-		goto err_dummy_fid_init;
-	}
-
 	err = mlxsw_sp_ports_create(mlxsw_sp);
 	if (err) {
 		dev_err(mlxsw_sp->bus_info->dev, "Failed to create ports\n");
@@ -3395,8 +3655,6 @@ static int mlxsw_sp_init(struct mlxsw_core *mlxsw_core,
 	return 0;
 
 err_ports_create:
-	mlxsw_sp_dummy_fid_fini(mlxsw_sp);
-err_dummy_fid_init:
 	mlxsw_sp_dpipe_fini(mlxsw_sp);
 err_dpipe_init:
 	mlxsw_sp_counter_pool_fini(mlxsw_sp);
@@ -3413,8 +3671,9 @@ err_switchdev_init:
 err_lag_init:
 	mlxsw_sp_buffers_fini(mlxsw_sp);
 err_buffers_init:
-err_flood_init:
 	mlxsw_sp_traps_fini(mlxsw_sp);
+err_traps_init:
+	mlxsw_sp_fids_fini(mlxsw_sp);
 	return err;
 }
 
@@ -3423,7 +3682,6 @@ static void mlxsw_sp_fini(struct mlxsw_core *mlxsw_core)
 	struct mlxsw_sp *mlxsw_sp = mlxsw_core_driver_priv(mlxsw_core);
 
 	mlxsw_sp_ports_remove(mlxsw_sp);
-	mlxsw_sp_dummy_fid_fini(mlxsw_sp);
 	mlxsw_sp_dpipe_fini(mlxsw_sp);
 	mlxsw_sp_counter_pool_fini(mlxsw_sp);
 	mlxsw_sp_acl_fini(mlxsw_sp);
@@ -3433,8 +3691,7 @@ static void mlxsw_sp_fini(struct mlxsw_core *mlxsw_core)
 	mlxsw_sp_lag_fini(mlxsw_sp);
 	mlxsw_sp_buffers_fini(mlxsw_sp);
 	mlxsw_sp_traps_fini(mlxsw_sp);
-	WARN_ON(!list_empty(&mlxsw_sp->vfids.list));
-	WARN_ON(!list_empty(&mlxsw_sp->fids));
+	mlxsw_sp_fids_fini(mlxsw_sp);
 }
 
 static struct mlxsw_config_profile mlxsw_sp_config_profile = {
@@ -3450,7 +3707,7 @@ static struct mlxsw_config_profile mlxsw_sp_config_profile = {
 	.max_fid_offset_flood_tables	= 3,
 	.fid_offset_flood_table_size	= VLAN_N_VID - 1,
 	.max_fid_flood_tables		= 3,
-	.fid_flood_table_size		= MLXSW_SP_VFID_MAX,
+	.fid_flood_table_size		= MLXSW_SP_FID_8021D_MAX,
 	.used_max_ib_mc			= 1,
 	.max_ib_mc			= 0,
 	.used_max_pkey			= 1,
@@ -3510,7 +3767,7 @@ static int mlxsw_sp_lower_dev_walk(struct net_device *lower_dev, void *data)
 	return ret;
 }
 
-static struct mlxsw_sp_port *mlxsw_sp_port_dev_lower_find(struct net_device *dev)
+struct mlxsw_sp_port *mlxsw_sp_port_dev_lower_find(struct net_device *dev)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port;
 
@@ -3531,7 +3788,7 @@ struct mlxsw_sp *mlxsw_sp_lower_get(struct net_device *dev)
 	return mlxsw_sp_port ? mlxsw_sp_port->mlxsw_sp : NULL;
 }
 
-static struct mlxsw_sp_port *mlxsw_sp_port_dev_lower_find_rcu(struct net_device *dev)
+struct mlxsw_sp_port *mlxsw_sp_port_dev_lower_find_rcu(struct net_device *dev)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port;
 
@@ -3562,176 +3819,6 @@ void mlxsw_sp_port_dev_put(struct mlxsw_sp_port *mlxsw_sp_port)
 	dev_put(mlxsw_sp_port->dev);
 }
 
-static bool mlxsw_sp_lag_port_fid_member(struct mlxsw_sp_port *lag_port,
-					 u16 fid)
-{
-	if (mlxsw_sp_fid_is_vfid(fid))
-		return mlxsw_sp_port_vport_find_by_fid(lag_port, fid);
-	else
-		return test_bit(fid, lag_port->active_vlans);
-}
-
-static bool mlxsw_sp_port_fdb_should_flush(struct mlxsw_sp_port *mlxsw_sp_port,
-					   u16 fid)
-{
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	u8 local_port = mlxsw_sp_port->local_port;
-	u16 lag_id = mlxsw_sp_port->lag_id;
-	u64 max_lag_members;
-	int i, count = 0;
-
-	if (!mlxsw_sp_port->lagged)
-		return true;
-
-	max_lag_members = MLXSW_CORE_RES_GET(mlxsw_sp->core,
-					     MAX_LAG_MEMBERS);
-	for (i = 0; i < max_lag_members; i++) {
-		struct mlxsw_sp_port *lag_port;
-
-		lag_port = mlxsw_sp_port_lagged_get(mlxsw_sp, lag_id, i);
-		if (!lag_port || lag_port->local_port == local_port)
-			continue;
-		if (mlxsw_sp_lag_port_fid_member(lag_port, fid))
-			count++;
-	}
-
-	return !count;
-}
-
-static int
-mlxsw_sp_port_fdb_flush_by_port_fid(const struct mlxsw_sp_port *mlxsw_sp_port,
-				    u16 fid)
-{
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	char sfdf_pl[MLXSW_REG_SFDF_LEN];
-
-	mlxsw_reg_sfdf_pack(sfdf_pl, MLXSW_REG_SFDF_FLUSH_PER_PORT_AND_FID);
-	mlxsw_reg_sfdf_fid_set(sfdf_pl, fid);
-	mlxsw_reg_sfdf_port_fid_system_port_set(sfdf_pl,
-						mlxsw_sp_port->local_port);
-
-	netdev_dbg(mlxsw_sp_port->dev, "FDB flushed using Port=%d, FID=%d\n",
-		   mlxsw_sp_port->local_port, fid);
-
-	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfdf), sfdf_pl);
-}
-
-static int
-mlxsw_sp_port_fdb_flush_by_lag_id_fid(const struct mlxsw_sp_port *mlxsw_sp_port,
-				      u16 fid)
-{
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	char sfdf_pl[MLXSW_REG_SFDF_LEN];
-
-	mlxsw_reg_sfdf_pack(sfdf_pl, MLXSW_REG_SFDF_FLUSH_PER_LAG_AND_FID);
-	mlxsw_reg_sfdf_fid_set(sfdf_pl, fid);
-	mlxsw_reg_sfdf_lag_fid_lag_id_set(sfdf_pl, mlxsw_sp_port->lag_id);
-
-	netdev_dbg(mlxsw_sp_port->dev, "FDB flushed using LAG ID=%d, FID=%d\n",
-		   mlxsw_sp_port->lag_id, fid);
-
-	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfdf), sfdf_pl);
-}
-
-int mlxsw_sp_port_fdb_flush(struct mlxsw_sp_port *mlxsw_sp_port, u16 fid)
-{
-	if (!mlxsw_sp_port_fdb_should_flush(mlxsw_sp_port, fid))
-		return 0;
-
-	if (mlxsw_sp_port->lagged)
-		return mlxsw_sp_port_fdb_flush_by_lag_id_fid(mlxsw_sp_port,
-							     fid);
-	else
-		return mlxsw_sp_port_fdb_flush_by_port_fid(mlxsw_sp_port, fid);
-}
-
-static void mlxsw_sp_master_bridge_gone_sync(struct mlxsw_sp *mlxsw_sp)
-{
-	struct mlxsw_sp_fid *f, *tmp;
-
-	list_for_each_entry_safe(f, tmp, &mlxsw_sp->fids, list)
-		if (--f->ref_count == 0)
-			mlxsw_sp_fid_destroy(mlxsw_sp, f);
-		else
-			WARN_ON_ONCE(1);
-}
-
-static bool mlxsw_sp_master_bridge_check(struct mlxsw_sp *mlxsw_sp,
-					 struct net_device *br_dev)
-{
-	return !mlxsw_sp->master_bridge.dev ||
-	       mlxsw_sp->master_bridge.dev == br_dev;
-}
-
-static void mlxsw_sp_master_bridge_inc(struct mlxsw_sp *mlxsw_sp,
-				       struct net_device *br_dev)
-{
-	mlxsw_sp->master_bridge.dev = br_dev;
-	mlxsw_sp->master_bridge.ref_count++;
-}
-
-static void mlxsw_sp_master_bridge_dec(struct mlxsw_sp *mlxsw_sp)
-{
-	if (--mlxsw_sp->master_bridge.ref_count == 0) {
-		mlxsw_sp->master_bridge.dev = NULL;
-		/* It's possible upper VLAN devices are still holding
-		 * references to underlying FIDs. Drop the reference
-		 * and release the resources if it was the last one.
-		 * If it wasn't, then something bad happened.
-		 */
-		mlxsw_sp_master_bridge_gone_sync(mlxsw_sp);
-	}
-}
-
-static int mlxsw_sp_port_bridge_join(struct mlxsw_sp_port *mlxsw_sp_port,
-				     struct net_device *br_dev)
-{
-	struct net_device *dev = mlxsw_sp_port->dev;
-	int err;
-
-	/* When port is not bridged untagged packets are tagged with
-	 * PVID=VID=1, thereby creating an implicit VLAN interface in
-	 * the device. Remove it and let bridge code take care of its
-	 * own VLANs.
-	 */
-	err = mlxsw_sp_port_kill_vid(dev, 0, 1);
-	if (err)
-		return err;
-
-	mlxsw_sp_master_bridge_inc(mlxsw_sp_port->mlxsw_sp, br_dev);
-
-	mlxsw_sp_port->learning = 1;
-	mlxsw_sp_port->learning_sync = 1;
-	mlxsw_sp_port->uc_flood = 1;
-	mlxsw_sp_port->mc_flood = 1;
-	mlxsw_sp_port->mc_router = 0;
-	mlxsw_sp_port->mc_disabled = 1;
-	mlxsw_sp_port->bridged = 1;
-
-	return 0;
-}
-
-static void mlxsw_sp_port_bridge_leave(struct mlxsw_sp_port *mlxsw_sp_port)
-{
-	struct net_device *dev = mlxsw_sp_port->dev;
-
-	mlxsw_sp_port_pvid_set(mlxsw_sp_port, 1);
-
-	mlxsw_sp_master_bridge_dec(mlxsw_sp_port->mlxsw_sp);
-
-	mlxsw_sp_port->learning = 0;
-	mlxsw_sp_port->learning_sync = 0;
-	mlxsw_sp_port->uc_flood = 0;
-	mlxsw_sp_port->mc_flood = 0;
-	mlxsw_sp_port->mc_router = 0;
-	mlxsw_sp_port->bridged = 0;
-
-	/* Add implicit VLAN interface in the device, so that untagged
-	 * packets will be classified to the default vFID.
-	 */
-	mlxsw_sp_port_add_vid(dev, 0, 1);
-}
-
 static int mlxsw_sp_lag_create(struct mlxsw_sp *mlxsw_sp, u16 lag_id)
 {
 	char sldr_pl[MLXSW_REG_SLDR_LEN];
@@ -3850,51 +3937,11 @@ static int mlxsw_sp_port_lag_index_get(struct mlxsw_sp *mlxsw_sp,
 	return -EBUSY;
 }
 
-static void
-mlxsw_sp_port_pvid_vport_lag_join(struct mlxsw_sp_port *mlxsw_sp_port,
-				  struct net_device *lag_dev, u16 lag_id)
-{
-	struct mlxsw_sp_port *mlxsw_sp_vport;
-	struct mlxsw_sp_fid *f;
-
-	mlxsw_sp_vport = mlxsw_sp_port_vport_find(mlxsw_sp_port, 1);
-	if (WARN_ON(!mlxsw_sp_vport))
-		return;
-
-	/* If vPort is assigned a RIF, then leave it since it's no
-	 * longer valid.
-	 */
-	f = mlxsw_sp_vport_fid_get(mlxsw_sp_vport);
-	if (f)
-		f->leave(mlxsw_sp_vport);
-
-	mlxsw_sp_vport->lag_id = lag_id;
-	mlxsw_sp_vport->lagged = 1;
-	mlxsw_sp_vport->dev = lag_dev;
-}
-
-static void
-mlxsw_sp_port_pvid_vport_lag_leave(struct mlxsw_sp_port *mlxsw_sp_port)
-{
-	struct mlxsw_sp_port *mlxsw_sp_vport;
-	struct mlxsw_sp_fid *f;
-
-	mlxsw_sp_vport = mlxsw_sp_port_vport_find(mlxsw_sp_port, 1);
-	if (WARN_ON(!mlxsw_sp_vport))
-		return;
-
-	f = mlxsw_sp_vport_fid_get(mlxsw_sp_vport);
-	if (f)
-		f->leave(mlxsw_sp_vport);
-
-	mlxsw_sp_vport->dev = mlxsw_sp_port->dev;
-	mlxsw_sp_vport->lagged = 0;
-}
-
 static int mlxsw_sp_port_lag_join(struct mlxsw_sp_port *mlxsw_sp_port,
 				  struct net_device *lag_dev)
 {
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 	struct mlxsw_sp_upper *lag;
 	u16 lag_id;
 	u8 port_index;
@@ -3927,7 +3974,10 @@ static int mlxsw_sp_port_lag_join(struct mlxsw_sp_port *mlxsw_sp_port,
 	mlxsw_sp_port->lagged = 1;
 	lag->ref_count++;
 
-	mlxsw_sp_port_pvid_vport_lag_join(mlxsw_sp_port, lag_dev, lag_id);
+	/* Port is no longer usable as a router interface */
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_vid(mlxsw_sp_port, 1);
+	if (mlxsw_sp_port_vlan->fid)
+		mlxsw_sp_port_vlan_router_leave(mlxsw_sp_port_vlan);
 
 	return 0;
 
@@ -3954,10 +4004,8 @@ static void mlxsw_sp_port_lag_leave(struct mlxsw_sp_port *mlxsw_sp_port,
 	mlxsw_sp_lag_col_port_disable(mlxsw_sp_port, lag_id);
 	mlxsw_sp_lag_col_port_remove(mlxsw_sp_port, lag_id);
 
-	if (mlxsw_sp_port->bridged) {
-		mlxsw_sp_port_active_vlans_del(mlxsw_sp_port);
-		mlxsw_sp_port_bridge_leave(mlxsw_sp_port);
-	}
+	/* Any VLANs configured on the port are no longer valid */
+	mlxsw_sp_port_vlan_flush(mlxsw_sp_port);
 
 	if (lag->ref_count == 1)
 		mlxsw_sp_lag_destroy(mlxsw_sp, lag_id);
@@ -3967,7 +4015,9 @@ static void mlxsw_sp_port_lag_leave(struct mlxsw_sp_port *mlxsw_sp_port,
 	mlxsw_sp_port->lagged = 0;
 	lag->ref_count--;
 
-	mlxsw_sp_port_pvid_vport_lag_leave(mlxsw_sp_port);
+	mlxsw_sp_port_vlan_get(mlxsw_sp_port, 1);
+	/* Make sure untagged frames are allowed to ingress */
+	mlxsw_sp_port_pvid_set(mlxsw_sp_port, 1);
 }
 
 static int mlxsw_sp_lag_dist_port_add(struct mlxsw_sp_port *mlxsw_sp_port,
@@ -4009,34 +4059,6 @@ static int mlxsw_sp_port_lag_changed(struct mlxsw_sp_port *mlxsw_sp_port,
 	return mlxsw_sp_port_lag_tx_en_set(mlxsw_sp_port, info->tx_enabled);
 }
 
-static int mlxsw_sp_port_vlan_link(struct mlxsw_sp_port *mlxsw_sp_port,
-				   struct net_device *vlan_dev)
-{
-	struct mlxsw_sp_port *mlxsw_sp_vport;
-	u16 vid = vlan_dev_vlan_id(vlan_dev);
-
-	mlxsw_sp_vport = mlxsw_sp_port_vport_find(mlxsw_sp_port, vid);
-	if (WARN_ON(!mlxsw_sp_vport))
-		return -EINVAL;
-
-	mlxsw_sp_vport->dev = vlan_dev;
-
-	return 0;
-}
-
-static void mlxsw_sp_port_vlan_unlink(struct mlxsw_sp_port *mlxsw_sp_port,
-				      struct net_device *vlan_dev)
-{
-	struct mlxsw_sp_port *mlxsw_sp_vport;
-	u16 vid = vlan_dev_vlan_id(vlan_dev);
-
-	mlxsw_sp_vport = mlxsw_sp_port_vport_find(mlxsw_sp_port, vid);
-	if (WARN_ON(!mlxsw_sp_vport))
-		return;
-
-	mlxsw_sp_vport->dev = mlxsw_sp_port->dev;
-}
-
 static int mlxsw_sp_port_stp_set(struct mlxsw_sp_port *mlxsw_sp_port,
 				 bool enable)
 {
@@ -4066,9 +4088,12 @@ static int mlxsw_sp_port_ovs_join(struct mlxsw_sp_port *mlxsw_sp_port)
 {
 	int err;
 
-	err = mlxsw_sp_port_stp_set(mlxsw_sp_port, true);
+	err = mlxsw_sp_port_vp_mode_set(mlxsw_sp_port, true);
 	if (err)
 		return err;
+	err = mlxsw_sp_port_stp_set(mlxsw_sp_port, true);
+	if (err)
+		goto err_port_stp_set;
 	err = mlxsw_sp_port_vlan_set(mlxsw_sp_port, 2, VLAN_N_VID - 1,
 				     true, false);
 	if (err)
@@ -4077,6 +4102,8 @@ static int mlxsw_sp_port_ovs_join(struct mlxsw_sp_port *mlxsw_sp_port)
 
 err_port_vlan_set:
 	mlxsw_sp_port_stp_set(mlxsw_sp_port, false);
+err_port_stp_set:
+	mlxsw_sp_port_vp_mode_set(mlxsw_sp_port, false);
 	return err;
 }
 
@@ -4085,9 +4112,11 @@ static void mlxsw_sp_port_ovs_leave(struct mlxsw_sp_port *mlxsw_sp_port)
 	mlxsw_sp_port_vlan_set(mlxsw_sp_port, 2, VLAN_N_VID - 1,
 			       false, false);
 	mlxsw_sp_port_stp_set(mlxsw_sp_port, false);
+	mlxsw_sp_port_vp_mode_set(mlxsw_sp_port, false);
 }
 
-static int mlxsw_sp_netdevice_port_upper_event(struct net_device *dev,
+static int mlxsw_sp_netdevice_port_upper_event(struct net_device *lower_dev,
+					       struct net_device *dev,
 					       unsigned long event, void *ptr)
 {
 	struct netdev_notifier_changeupper_info *info;
@@ -4110,10 +4139,6 @@ static int mlxsw_sp_netdevice_port_upper_event(struct net_device *dev,
 			return -EINVAL;
 		if (!info->linking)
 			break;
-		/* HW limitation forbids to put ports to multiple bridges. */
-		if (netif_is_bridge_master(upper_dev) &&
-		    !mlxsw_sp_master_bridge_check(mlxsw_sp, upper_dev))
-			return -EINVAL;
 		if (netif_is_lag_master(upper_dev) &&
 		    !mlxsw_sp_master_lag_check(mlxsw_sp, upper_dev,
 					       info->upper_info))
@@ -4130,19 +4155,15 @@ static int mlxsw_sp_netdevice_port_upper_event(struct net_device *dev,
 		break;
 	case NETDEV_CHANGEUPPER:
 		upper_dev = info->upper_dev;
-		if (is_vlan_dev(upper_dev)) {
-			if (info->linking)
-				err = mlxsw_sp_port_vlan_link(mlxsw_sp_port,
-							      upper_dev);
-			else
-				mlxsw_sp_port_vlan_unlink(mlxsw_sp_port,
-							  upper_dev);
-		} else if (netif_is_bridge_master(upper_dev)) {
+		if (netif_is_bridge_master(upper_dev)) {
 			if (info->linking)
 				err = mlxsw_sp_port_bridge_join(mlxsw_sp_port,
+								lower_dev,
 								upper_dev);
 			else
-				mlxsw_sp_port_bridge_leave(mlxsw_sp_port);
+				mlxsw_sp_port_bridge_leave(mlxsw_sp_port,
+							   lower_dev,
+							   upper_dev);
 		} else if (netif_is_lag_master(upper_dev)) {
 			if (info->linking)
 				err = mlxsw_sp_port_lag_join(mlxsw_sp_port,
@@ -4155,9 +4176,6 @@ static int mlxsw_sp_netdevice_port_upper_event(struct net_device *dev,
 				err = mlxsw_sp_port_ovs_join(mlxsw_sp_port);
 			else
 				mlxsw_sp_port_ovs_leave(mlxsw_sp_port);
-		} else {
-			err = -EINVAL;
-			WARN_ON(1);
 		}
 		break;
 	}
@@ -4189,15 +4207,18 @@ static int mlxsw_sp_netdevice_port_lower_event(struct net_device *dev,
 	return 0;
 }
 
-static int mlxsw_sp_netdevice_port_event(struct net_device *dev,
+static int mlxsw_sp_netdevice_port_event(struct net_device *lower_dev,
+					 struct net_device *port_dev,
 					 unsigned long event, void *ptr)
 {
 	switch (event) {
 	case NETDEV_PRECHANGEUPPER:
 	case NETDEV_CHANGEUPPER:
-		return mlxsw_sp_netdevice_port_upper_event(dev, event, ptr);
+		return mlxsw_sp_netdevice_port_upper_event(lower_dev, port_dev,
+							   event, ptr);
 	case NETDEV_CHANGELOWERSTATE:
-		return mlxsw_sp_netdevice_port_lower_event(dev, event, ptr);
+		return mlxsw_sp_netdevice_port_lower_event(port_dev, event,
+							   ptr);
 	}
 
 	return 0;
@@ -4212,7 +4233,8 @@ static int mlxsw_sp_netdevice_lag_event(struct net_device *lag_dev,
 
 	netdev_for_each_lower_dev(lag_dev, dev, iter) {
 		if (mlxsw_sp_port_dev_check(dev)) {
-			ret = mlxsw_sp_netdevice_port_event(dev, event, ptr);
+			ret = mlxsw_sp_netdevice_port_event(lag_dev, dev, event,
+							    ptr);
 			if (ret)
 				return ret;
 		}
@@ -4221,322 +4243,33 @@ static int mlxsw_sp_netdevice_lag_event(struct net_device *lag_dev,
 	return 0;
 }
 
-static int mlxsw_sp_master_bridge_vlan_link(struct mlxsw_sp *mlxsw_sp,
-					    struct net_device *vlan_dev)
-{
-	u16 fid = vlan_dev_vlan_id(vlan_dev);
-	struct mlxsw_sp_fid *f;
-
-	f = mlxsw_sp_fid_find(mlxsw_sp, fid);
-	if (!f) {
-		f = mlxsw_sp_fid_create(mlxsw_sp, fid);
-		if (IS_ERR(f))
-			return PTR_ERR(f);
-	}
-
-	f->ref_count++;
-
-	return 0;
-}
-
-static void mlxsw_sp_master_bridge_vlan_unlink(struct mlxsw_sp *mlxsw_sp,
-					       struct net_device *vlan_dev)
-{
-	u16 fid = vlan_dev_vlan_id(vlan_dev);
-	struct mlxsw_sp_fid *f;
-
-	f = mlxsw_sp_fid_find(mlxsw_sp, fid);
-	if (f && f->rif)
-		mlxsw_sp_rif_bridge_destroy(mlxsw_sp, f->rif);
-	if (f && --f->ref_count == 0)
-		mlxsw_sp_fid_destroy(mlxsw_sp, f);
-}
-
-static int mlxsw_sp_netdevice_bridge_event(struct net_device *br_dev,
-					   unsigned long event, void *ptr)
-{
-	struct netdev_notifier_changeupper_info *info;
-	struct net_device *upper_dev;
-	struct mlxsw_sp *mlxsw_sp;
-	int err = 0;
-
-	mlxsw_sp = mlxsw_sp_lower_get(br_dev);
-	if (!mlxsw_sp)
-		return 0;
-
-	info = ptr;
-
-	switch (event) {
-	case NETDEV_PRECHANGEUPPER:
-		upper_dev = info->upper_dev;
-		if (!is_vlan_dev(upper_dev))
-			return -EINVAL;
-		if (is_vlan_dev(upper_dev) &&
-		    br_dev != mlxsw_sp->master_bridge.dev)
-			return -EINVAL;
-		break;
-	case NETDEV_CHANGEUPPER:
-		upper_dev = info->upper_dev;
-		if (is_vlan_dev(upper_dev)) {
-			if (info->linking)
-				err = mlxsw_sp_master_bridge_vlan_link(mlxsw_sp,
-								       upper_dev);
-			else
-				mlxsw_sp_master_bridge_vlan_unlink(mlxsw_sp,
-								   upper_dev);
-		} else {
-			err = -EINVAL;
-			WARN_ON(1);
-		}
-		break;
-	}
-
-	return err;
-}
-
-static u16 mlxsw_sp_avail_vfid_get(const struct mlxsw_sp *mlxsw_sp)
-{
-	return find_first_zero_bit(mlxsw_sp->vfids.mapped,
-				   MLXSW_SP_VFID_MAX);
-}
-
-static int mlxsw_sp_vfid_op(struct mlxsw_sp *mlxsw_sp, u16 fid, bool create)
-{
-	char sfmr_pl[MLXSW_REG_SFMR_LEN];
-
-	mlxsw_reg_sfmr_pack(sfmr_pl, !create, fid, 0);
-	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfmr), sfmr_pl);
-}
-
-static void mlxsw_sp_vport_vfid_leave(struct mlxsw_sp_port *mlxsw_sp_vport);
-
-static struct mlxsw_sp_fid *mlxsw_sp_vfid_create(struct mlxsw_sp *mlxsw_sp,
-						 struct net_device *br_dev)
-{
-	struct device *dev = mlxsw_sp->bus_info->dev;
-	struct mlxsw_sp_fid *f;
-	u16 vfid, fid;
-	int err;
-
-	vfid = mlxsw_sp_avail_vfid_get(mlxsw_sp);
-	if (vfid == MLXSW_SP_VFID_MAX) {
-		dev_err(dev, "No available vFIDs\n");
-		return ERR_PTR(-ERANGE);
-	}
-
-	fid = mlxsw_sp_vfid_to_fid(vfid);
-	err = mlxsw_sp_vfid_op(mlxsw_sp, fid, true);
-	if (err) {
-		dev_err(dev, "Failed to create FID=%d\n", fid);
-		return ERR_PTR(err);
-	}
-
-	f = kzalloc(sizeof(*f), GFP_KERNEL);
-	if (!f)
-		goto err_allocate_vfid;
-
-	f->leave = mlxsw_sp_vport_vfid_leave;
-	f->fid = fid;
-	f->dev = br_dev;
-
-	list_add(&f->list, &mlxsw_sp->vfids.list);
-	set_bit(vfid, mlxsw_sp->vfids.mapped);
-
-	return f;
-
-err_allocate_vfid:
-	mlxsw_sp_vfid_op(mlxsw_sp, fid, false);
-	return ERR_PTR(-ENOMEM);
-}
-
-static void mlxsw_sp_vfid_destroy(struct mlxsw_sp *mlxsw_sp,
-				  struct mlxsw_sp_fid *f)
-{
-	u16 vfid = mlxsw_sp_fid_to_vfid(f->fid);
-	u16 fid = f->fid;
-
-	clear_bit(vfid, mlxsw_sp->vfids.mapped);
-	list_del(&f->list);
-
-	if (f->rif)
-		mlxsw_sp_rif_bridge_destroy(mlxsw_sp, f->rif);
-
-	kfree(f);
-
-	mlxsw_sp_vfid_op(mlxsw_sp, fid, false);
-}
-
-static int mlxsw_sp_vport_fid_map(struct mlxsw_sp_port *mlxsw_sp_vport, u16 fid,
-				  bool valid)
-{
-	enum mlxsw_reg_svfa_mt mt = MLXSW_REG_SVFA_MT_PORT_VID_TO_FID;
-	u16 vid = mlxsw_sp_vport_vid_get(mlxsw_sp_vport);
-
-	return mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_vport, mt, valid, fid,
-					    vid);
-}
-
-static int mlxsw_sp_vport_vfid_join(struct mlxsw_sp_port *mlxsw_sp_vport,
-				    struct net_device *br_dev)
-{
-	struct mlxsw_sp_fid *f;
-	int err;
-
-	f = mlxsw_sp_vfid_find(mlxsw_sp_vport->mlxsw_sp, br_dev);
-	if (!f) {
-		f = mlxsw_sp_vfid_create(mlxsw_sp_vport->mlxsw_sp, br_dev);
-		if (IS_ERR(f))
-			return PTR_ERR(f);
-	}
-
-	err = mlxsw_sp_vport_flood_set(mlxsw_sp_vport, f->fid, true);
-	if (err)
-		goto err_vport_flood_set;
-
-	err = mlxsw_sp_vport_fid_map(mlxsw_sp_vport, f->fid, true);
-	if (err)
-		goto err_vport_fid_map;
-
-	mlxsw_sp_vport_fid_set(mlxsw_sp_vport, f);
-	f->ref_count++;
-
-	netdev_dbg(mlxsw_sp_vport->dev, "Joined FID=%d\n", f->fid);
-
-	return 0;
-
-err_vport_fid_map:
-	mlxsw_sp_vport_flood_set(mlxsw_sp_vport, f->fid, false);
-err_vport_flood_set:
-	if (!f->ref_count)
-		mlxsw_sp_vfid_destroy(mlxsw_sp_vport->mlxsw_sp, f);
-	return err;
-}
-
-static void mlxsw_sp_vport_vfid_leave(struct mlxsw_sp_port *mlxsw_sp_vport)
-{
-	struct mlxsw_sp_fid *f = mlxsw_sp_vport_fid_get(mlxsw_sp_vport);
-
-	netdev_dbg(mlxsw_sp_vport->dev, "Left FID=%d\n", f->fid);
-
-	mlxsw_sp_vport_fid_map(mlxsw_sp_vport, f->fid, false);
-
-	mlxsw_sp_vport_flood_set(mlxsw_sp_vport, f->fid, false);
-
-	mlxsw_sp_port_fdb_flush(mlxsw_sp_vport, f->fid);
-
-	mlxsw_sp_vport_fid_set(mlxsw_sp_vport, NULL);
-	if (--f->ref_count == 0)
-		mlxsw_sp_vfid_destroy(mlxsw_sp_vport->mlxsw_sp, f);
-}
-
-static int mlxsw_sp_vport_bridge_join(struct mlxsw_sp_port *mlxsw_sp_vport,
-				      struct net_device *br_dev)
-{
-	struct mlxsw_sp_fid *f = mlxsw_sp_vport_fid_get(mlxsw_sp_vport);
-	u16 vid = mlxsw_sp_vport_vid_get(mlxsw_sp_vport);
-	struct net_device *dev = mlxsw_sp_vport->dev;
-	int err;
-
-	if (f && !WARN_ON(!f->leave))
-		f->leave(mlxsw_sp_vport);
-
-	err = mlxsw_sp_vport_vfid_join(mlxsw_sp_vport, br_dev);
-	if (err) {
-		netdev_err(dev, "Failed to join vFID\n");
-		return err;
-	}
-
-	err = mlxsw_sp_port_vid_learning_set(mlxsw_sp_vport, vid, true);
-	if (err) {
-		netdev_err(dev, "Failed to enable learning\n");
-		goto err_port_vid_learning_set;
-	}
-
-	mlxsw_sp_vport->learning = 1;
-	mlxsw_sp_vport->learning_sync = 1;
-	mlxsw_sp_vport->uc_flood = 1;
-	mlxsw_sp_vport->mc_flood = 1;
-	mlxsw_sp_vport->mc_router = 0;
-	mlxsw_sp_vport->mc_disabled = 1;
-	mlxsw_sp_vport->bridged = 1;
-
-	return 0;
-
-err_port_vid_learning_set:
-	mlxsw_sp_vport_vfid_leave(mlxsw_sp_vport);
-	return err;
-}
-
-static void mlxsw_sp_vport_bridge_leave(struct mlxsw_sp_port *mlxsw_sp_vport)
-{
-	u16 vid = mlxsw_sp_vport_vid_get(mlxsw_sp_vport);
-
-	mlxsw_sp_port_vid_learning_set(mlxsw_sp_vport, vid, false);
-
-	mlxsw_sp_vport_vfid_leave(mlxsw_sp_vport);
-
-	mlxsw_sp_vport->learning = 0;
-	mlxsw_sp_vport->learning_sync = 0;
-	mlxsw_sp_vport->uc_flood = 0;
-	mlxsw_sp_vport->mc_flood = 0;
-	mlxsw_sp_vport->mc_router = 0;
-	mlxsw_sp_vport->bridged = 0;
-}
-
-static bool
-mlxsw_sp_port_master_bridge_check(const struct mlxsw_sp_port *mlxsw_sp_port,
-				  const struct net_device *br_dev)
-{
-	struct mlxsw_sp_port *mlxsw_sp_vport;
-
-	list_for_each_entry(mlxsw_sp_vport, &mlxsw_sp_port->vports_list,
-			    vport.list) {
-		struct net_device *dev = mlxsw_sp_vport_dev_get(mlxsw_sp_vport);
-
-		if (dev && dev == br_dev)
-			return false;
-	}
-
-	return true;
-}
-
-static int mlxsw_sp_netdevice_vport_event(struct net_device *dev,
-					  unsigned long event, void *ptr,
-					  u16 vid)
+static int mlxsw_sp_netdevice_port_vlan_event(struct net_device *vlan_dev,
+					      struct net_device *dev,
+					      unsigned long event, void *ptr,
+					      u16 vid)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	struct netdev_notifier_changeupper_info *info = ptr;
-	struct mlxsw_sp_port *mlxsw_sp_vport;
 	struct net_device *upper_dev;
 	int err = 0;
 
-	mlxsw_sp_vport = mlxsw_sp_port_vport_find(mlxsw_sp_port, vid);
-	if (!mlxsw_sp_vport)
-		return 0;
-
 	switch (event) {
 	case NETDEV_PRECHANGEUPPER:
 		upper_dev = info->upper_dev;
 		if (!netif_is_bridge_master(upper_dev))
 			return -EINVAL;
-		if (!info->linking)
-			break;
-		/* We can't have multiple VLAN interfaces configured on
-		 * the same port and being members in the same bridge.
-		 */
-		if (netif_is_bridge_master(upper_dev) &&
-		    !mlxsw_sp_port_master_bridge_check(mlxsw_sp_port,
-						       upper_dev))
-			return -EINVAL;
 		break;
 	case NETDEV_CHANGEUPPER:
 		upper_dev = info->upper_dev;
 		if (netif_is_bridge_master(upper_dev)) {
 			if (info->linking)
-				err = mlxsw_sp_vport_bridge_join(mlxsw_sp_vport,
-								 upper_dev);
+				err = mlxsw_sp_port_bridge_join(mlxsw_sp_port,
+								vlan_dev,
+								upper_dev);
 			else
-				mlxsw_sp_vport_bridge_leave(mlxsw_sp_vport);
+				mlxsw_sp_port_bridge_leave(mlxsw_sp_port,
+							   vlan_dev,
+							   upper_dev);
 		} else {
 			err = -EINVAL;
 			WARN_ON(1);
@@ -4547,9 +4280,10 @@ static int mlxsw_sp_netdevice_vport_event(struct net_device *dev,
 	return err;
 }
 
-static int mlxsw_sp_netdevice_lag_vport_event(struct net_device *lag_dev,
-					      unsigned long event, void *ptr,
-					      u16 vid)
+static int mlxsw_sp_netdevice_lag_port_vlan_event(struct net_device *vlan_dev,
+						  struct net_device *lag_dev,
+						  unsigned long event,
+						  void *ptr, u16 vid)
 {
 	struct net_device *dev;
 	struct list_head *iter;
@@ -4557,8 +4291,9 @@ static int mlxsw_sp_netdevice_lag_vport_event(struct net_device *lag_dev,
 
 	netdev_for_each_lower_dev(lag_dev, dev, iter) {
 		if (mlxsw_sp_port_dev_check(dev)) {
-			ret = mlxsw_sp_netdevice_vport_event(dev, event, ptr,
-							     vid);
+			ret = mlxsw_sp_netdevice_port_vlan_event(vlan_dev, dev,
+								 event, ptr,
+								 vid);
 			if (ret)
 				return ret;
 		}
@@ -4574,11 +4309,12 @@ static int mlxsw_sp_netdevice_vlan_event(struct net_device *vlan_dev,
 	u16 vid = vlan_dev_vlan_id(vlan_dev);
 
 	if (mlxsw_sp_port_dev_check(real_dev))
-		return mlxsw_sp_netdevice_vport_event(real_dev, event, ptr,
-						      vid);
+		return mlxsw_sp_netdevice_port_vlan_event(vlan_dev, real_dev,
+							  event, ptr, vid);
 	else if (netif_is_lag_master(real_dev))
-		return mlxsw_sp_netdevice_lag_vport_event(real_dev, event, ptr,
-							  vid);
+		return mlxsw_sp_netdevice_lag_port_vlan_event(vlan_dev,
+							      real_dev, event,
+							      ptr, vid);
 
 	return 0;
 }
@@ -4603,11 +4339,9 @@ static int mlxsw_sp_netdevice_event(struct notifier_block *unused,
 	else if (mlxsw_sp_is_vrf_event(event, ptr))
 		err = mlxsw_sp_netdevice_vrf_event(dev, event, ptr);
 	else if (mlxsw_sp_port_dev_check(dev))
-		err = mlxsw_sp_netdevice_port_event(dev, event, ptr);
+		err = mlxsw_sp_netdevice_port_event(dev, dev, event, ptr);
 	else if (netif_is_lag_master(dev))
 		err = mlxsw_sp_netdevice_lag_event(dev, event, ptr);
-	else if (netif_is_bridge_master(dev))
-		err = mlxsw_sp_netdevice_bridge_event(dev, event, ptr);
 	else if (is_vlan_dev(dev))
 		err = mlxsw_sp_netdevice_vlan_event(dev, event, ptr);
 
@@ -4680,3 +4414,4 @@ MODULE_LICENSE("Dual BSD/GPL");
 MODULE_AUTHOR("Jiri Pirko <jiri@mellanox.com>");
 MODULE_DESCRIPTION("Mellanox Spectrum driver");
 MODULE_DEVICE_TABLE(pci, mlxsw_sp_pci_id_table);
+MODULE_FIRMWARE(MLXSW_SP_FW_FILENAME);
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum.h b/drivers/net/ethernet/mellanox/mlxsw/spectrum.h
index 0c23bc1e946d..5ef98d4d0ab6 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum.h
@@ -54,12 +54,7 @@
 #include "core_acl_flex_keys.h"
 #include "core_acl_flex_actions.h"
 
-#define MLXSW_SP_VFID_BASE VLAN_N_VID
-#define MLXSW_SP_VFID_MAX 1024	/* Bridged VLAN interfaces */
-
-#define MLXSW_SP_DUMMY_FID 15359
-
-#define MLXSW_SP_RFID_BASE 15360
+#define MLXSW_SP_FID_8021D_MAX 1024
 
 #define MLXSW_SP_MID_MAX 7000
 
@@ -78,13 +73,19 @@ struct mlxsw_sp_upper {
 	unsigned int ref_count;
 };
 
-struct mlxsw_sp_fid {
-	void (*leave)(struct mlxsw_sp_port *mlxsw_sp_vport);
-	struct list_head list;
-	unsigned int ref_count;
-	struct net_device *dev;
-	struct mlxsw_sp_rif *rif;
-	u16 fid;
+enum mlxsw_sp_rif_type {
+	MLXSW_SP_RIF_TYPE_SUBPORT,
+	MLXSW_SP_RIF_TYPE_VLAN,
+	MLXSW_SP_RIF_TYPE_FID,
+	MLXSW_SP_RIF_TYPE_MAX,
+};
+
+enum mlxsw_sp_fid_type {
+	MLXSW_SP_FID_TYPE_8021Q,
+	MLXSW_SP_FID_TYPE_8021D,
+	MLXSW_SP_FID_TYPE_RFID,
+	MLXSW_SP_FID_TYPE_DUMMY,
+	MLXSW_SP_FID_TYPE_MAX,
 };
 
 struct mlxsw_sp_mid {
@@ -95,85 +96,6 @@ struct mlxsw_sp_mid {
 	unsigned int ref_count;
 };
 
-static inline u16 mlxsw_sp_vfid_to_fid(u16 vfid)
-{
-	return MLXSW_SP_VFID_BASE + vfid;
-}
-
-static inline u16 mlxsw_sp_fid_to_vfid(u16 fid)
-{
-	return fid - MLXSW_SP_VFID_BASE;
-}
-
-static inline bool mlxsw_sp_fid_is_vfid(u16 fid)
-{
-	return fid >= MLXSW_SP_VFID_BASE && fid < MLXSW_SP_DUMMY_FID;
-}
-
-struct mlxsw_sp_sb_pr {
-	enum mlxsw_reg_sbpr_mode mode;
-	u32 size;
-};
-
-struct mlxsw_cp_sb_occ {
-	u32 cur;
-	u32 max;
-};
-
-struct mlxsw_sp_sb_cm {
-	u32 min_buff;
-	u32 max_buff;
-	u8 pool;
-	struct mlxsw_cp_sb_occ occ;
-};
-
-struct mlxsw_sp_sb_pm {
-	u32 min_buff;
-	u32 max_buff;
-	struct mlxsw_cp_sb_occ occ;
-};
-
-#define MLXSW_SP_SB_POOL_COUNT	4
-#define MLXSW_SP_SB_TC_COUNT	8
-
-struct mlxsw_sp_sb_port {
-	struct mlxsw_sp_sb_cm cms[2][MLXSW_SP_SB_TC_COUNT];
-	struct mlxsw_sp_sb_pm pms[2][MLXSW_SP_SB_POOL_COUNT];
-};
-
-struct mlxsw_sp_sb {
-	struct mlxsw_sp_sb_pr prs[2][MLXSW_SP_SB_POOL_COUNT];
-	struct mlxsw_sp_sb_port *ports;
-	u32 cell_size;
-};
-
-#define MLXSW_SP_PREFIX_COUNT (sizeof(struct in6_addr) * BITS_PER_BYTE)
-
-struct mlxsw_sp_prefix_usage {
-	DECLARE_BITMAP(b, MLXSW_SP_PREFIX_COUNT);
-};
-
-enum mlxsw_sp_l3proto {
-	MLXSW_SP_L3_PROTO_IPV4,
-	MLXSW_SP_L3_PROTO_IPV6,
-};
-
-struct mlxsw_sp_lpm_tree {
-	u8 id; /* tree ID */
-	unsigned int ref_count;
-	enum mlxsw_sp_l3proto proto;
-	struct mlxsw_sp_prefix_usage prefix_usage;
-};
-
-struct mlxsw_sp_fib;
-
-struct mlxsw_sp_vr {
-	u16 id; /* virtual router ID */
-	u32 tb_id; /* kernel fib table id */
-	unsigned int rif_count;
-	struct mlxsw_sp_fib *fib4;
-};
-
 enum mlxsw_sp_span_type {
 	MLXSW_SP_SPAN_EGRESS,
 	MLXSW_SP_SPAN_INGRESS
@@ -212,58 +134,25 @@ struct mlxsw_sp_port_mall_tc_entry {
 	};
 };
 
-struct mlxsw_sp_router {
-	struct mlxsw_sp_vr *vrs;
-	struct rhashtable neigh_ht;
-	struct rhashtable nexthop_group_ht;
-	struct rhashtable nexthop_ht;
-	struct {
-		struct mlxsw_sp_lpm_tree *trees;
-		unsigned int tree_count;
-	} lpm;
-	struct {
-		struct delayed_work dw;
-		unsigned long interval;	/* ms */
-	} neighs_update;
-	struct delayed_work nexthop_probe_dw;
-#define MLXSW_SP_UNRESOLVED_NH_PROBE_INTERVAL 5000 /* ms */
-	struct list_head nexthop_neighs_list;
-	bool aborted;
-};
-
+struct mlxsw_sp_sb;
+struct mlxsw_sp_bridge;
+struct mlxsw_sp_router;
 struct mlxsw_sp_acl;
 struct mlxsw_sp_counter_pool;
+struct mlxsw_sp_fid_core;
 
 struct mlxsw_sp {
-	struct {
-		struct list_head list;
-		DECLARE_BITMAP(mapped, MLXSW_SP_VFID_MAX);
-	} vfids;
-	struct {
-		struct list_head list;
-		DECLARE_BITMAP(mapped, MLXSW_SP_MID_MAX);
-	} br_mids;
-	struct list_head fids;	/* VLAN-aware bridge FIDs */
-	struct mlxsw_sp_rif **rifs;
 	struct mlxsw_sp_port **ports;
 	struct mlxsw_core *core;
 	const struct mlxsw_bus_info *bus_info;
 	unsigned char base_mac[ETH_ALEN];
-	struct {
-		struct delayed_work dw;
-#define MLXSW_SP_DEFAULT_LEARNING_INTERVAL 100
-		unsigned int interval; /* ms */
-	} fdb_notify;
-#define MLXSW_SP_MIN_AGEING_TIME 10
-#define MLXSW_SP_MAX_AGEING_TIME 1000000
-#define MLXSW_SP_DEFAULT_AGEING_TIME 300
-	u32 ageing_time;
-	struct mlxsw_sp_upper master_bridge;
 	struct mlxsw_sp_upper *lags;
 	u8 *port_to_module;
-	struct mlxsw_sp_sb sb;
-	struct mlxsw_sp_router router;
+	struct mlxsw_sp_sb *sb;
+	struct mlxsw_sp_bridge *bridge;
+	struct mlxsw_sp_router *router;
 	struct mlxsw_sp_acl *acl;
+	struct mlxsw_sp_fid_core *fid_core;
 	struct {
 		DECLARE_BITMAP(usage, MLXSW_SP_KVD_LINEAR_SIZE);
 	} kvdl;
@@ -273,7 +162,6 @@ struct mlxsw_sp {
 		struct mlxsw_sp_span_entry *entries;
 		int entries_count;
 	} span;
-	struct notifier_block fib_nb;
 };
 
 static inline struct mlxsw_sp_upper *
@@ -282,18 +170,6 @@ mlxsw_sp_lag_get(struct mlxsw_sp *mlxsw_sp, u16 lag_id)
 	return &mlxsw_sp->lags[lag_id];
 }
 
-static inline u32 mlxsw_sp_cells_bytes(const struct mlxsw_sp *mlxsw_sp,
-				       u32 cells)
-{
-	return mlxsw_sp->sb.cell_size * cells;
-}
-
-static inline u32 mlxsw_sp_bytes_cells(const struct mlxsw_sp *mlxsw_sp,
-				       u32 bytes)
-{
-	return DIV_ROUND_UP(bytes, mlxsw_sp->sb.cell_size);
-}
-
 struct mlxsw_sp_port_pcpu_stats {
 	u64			rx_packets;
 	u64			rx_bytes;
@@ -310,29 +186,28 @@ struct mlxsw_sp_port_sample {
 	bool truncate;
 };
 
+struct mlxsw_sp_bridge_port;
+struct mlxsw_sp_fid;
+
+struct mlxsw_sp_port_vlan {
+	struct list_head list;
+	struct mlxsw_sp_port *mlxsw_sp_port;
+	struct mlxsw_sp_fid *fid;
+	u16 vid;
+	struct mlxsw_sp_bridge_port *bridge_port;
+	struct list_head bridge_vlan_node;
+};
+
 struct mlxsw_sp_port {
 	struct net_device *dev;
 	struct mlxsw_sp_port_pcpu_stats __percpu *pcpu_stats;
 	struct mlxsw_sp *mlxsw_sp;
 	u8 local_port;
-	u8 stp_state;
-	u16 learning:1,
-	   learning_sync:1,
-	   uc_flood:1,
-	   mc_flood:1,
-	   mc_router:1,
-	   mc_disabled:1,
-	   bridged:1,
-	   lagged:1,
+	u8 lagged:1,
 	   split:1;
 	u16 pvid;
 	u16 lag_id;
 	struct {
-		struct list_head list;
-		struct mlxsw_sp_fid *f;
-		u16 vid;
-	} vport;
-	struct {
 		u8 tx_pause:1,
 		   rx_pause:1,
 		   autoneg:1;
@@ -347,11 +222,6 @@ struct mlxsw_sp_port {
 		u8 width;
 		u8 lane;
 	} mapping;
-	/* 802.1Q bridge VLANs */
-	unsigned long *active_vlans;
-	unsigned long *untagged_vlans;
-	/* VLAN interfaces */
-	struct list_head vports_list;
 	/* TC handles */
 	struct list_head mall_tc_list;
 	struct {
@@ -360,13 +230,9 @@ struct mlxsw_sp_port {
 		struct delayed_work update_dw;
 	} hw_stats;
 	struct mlxsw_sp_port_sample *sample;
+	struct list_head vlans_list;
 };
 
-bool mlxsw_sp_port_dev_check(const struct net_device *dev);
-struct mlxsw_sp *mlxsw_sp_lower_get(struct net_device *dev);
-struct mlxsw_sp_port *mlxsw_sp_port_lower_dev_hold(struct net_device *dev);
-void mlxsw_sp_port_dev_put(struct mlxsw_sp_port *mlxsw_sp_port);
-
 static inline bool
 mlxsw_sp_port_is_pause_en(const struct mlxsw_sp_port *mlxsw_sp_port)
 {
@@ -385,102 +251,28 @@ mlxsw_sp_port_lagged_get(struct mlxsw_sp *mlxsw_sp, u16 lag_id, u8 port_index)
 	return mlxsw_sp_port && mlxsw_sp_port->lagged ? mlxsw_sp_port : NULL;
 }
 
-static inline u16
-mlxsw_sp_vport_vid_get(const struct mlxsw_sp_port *mlxsw_sp_vport)
-{
-	return mlxsw_sp_vport->vport.vid;
-}
-
-static inline bool
-mlxsw_sp_port_is_vport(const struct mlxsw_sp_port *mlxsw_sp_port)
+static inline struct mlxsw_sp_port_vlan *
+mlxsw_sp_port_vlan_find_by_vid(const struct mlxsw_sp_port *mlxsw_sp_port,
+			       u16 vid)
 {
-	u16 vid = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 
-	return vid != 0;
-}
-
-static inline void mlxsw_sp_vport_fid_set(struct mlxsw_sp_port *mlxsw_sp_vport,
-					  struct mlxsw_sp_fid *f)
-{
-	mlxsw_sp_vport->vport.f = f;
-}
-
-static inline struct mlxsw_sp_fid *
-mlxsw_sp_vport_fid_get(const struct mlxsw_sp_port *mlxsw_sp_vport)
-{
-	return mlxsw_sp_vport->vport.f;
-}
-
-static inline struct net_device *
-mlxsw_sp_vport_dev_get(const struct mlxsw_sp_port *mlxsw_sp_vport)
-{
-	struct mlxsw_sp_fid *f = mlxsw_sp_vport_fid_get(mlxsw_sp_vport);
-
-	return f ? f->dev : NULL;
-}
-
-static inline struct mlxsw_sp_port *
-mlxsw_sp_port_vport_find(const struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
-{
-	struct mlxsw_sp_port *mlxsw_sp_vport;
-
-	list_for_each_entry(mlxsw_sp_vport, &mlxsw_sp_port->vports_list,
-			    vport.list) {
-		if (mlxsw_sp_vport_vid_get(mlxsw_sp_vport) == vid)
-			return mlxsw_sp_vport;
+	list_for_each_entry(mlxsw_sp_port_vlan, &mlxsw_sp_port->vlans_list,
+			    list) {
+		if (mlxsw_sp_port_vlan->vid == vid)
+			return mlxsw_sp_port_vlan;
 	}
 
 	return NULL;
 }
 
-static inline struct mlxsw_sp_port *
-mlxsw_sp_port_vport_find_by_fid(const struct mlxsw_sp_port *mlxsw_sp_port,
-				u16 fid)
-{
-	struct mlxsw_sp_port *mlxsw_sp_vport;
-
-	list_for_each_entry(mlxsw_sp_vport, &mlxsw_sp_port->vports_list,
-			    vport.list) {
-		struct mlxsw_sp_fid *f = mlxsw_sp_vport_fid_get(mlxsw_sp_vport);
-
-		if (f && f->fid == fid)
-			return mlxsw_sp_vport;
-	}
-
-	return NULL;
-}
-
-static inline struct mlxsw_sp_fid *mlxsw_sp_fid_find(struct mlxsw_sp *mlxsw_sp,
-						     u16 fid)
-{
-	struct mlxsw_sp_fid *f;
-
-	list_for_each_entry(f, &mlxsw_sp->fids, list)
-		if (f->fid == fid)
-			return f;
-
-	return NULL;
-}
-
-static inline struct mlxsw_sp_fid *
-mlxsw_sp_vfid_find(const struct mlxsw_sp *mlxsw_sp,
-		   const struct net_device *br_dev)
-{
-	struct mlxsw_sp_fid *f;
-
-	list_for_each_entry(f, &mlxsw_sp->vfids.list, list)
-		if (f->dev == br_dev)
-			return f;
-
-	return NULL;
-}
-
-enum mlxsw_sp_flood_table {
-	MLXSW_SP_FLOOD_TABLE_UC,
-	MLXSW_SP_FLOOD_TABLE_BC,
-	MLXSW_SP_FLOOD_TABLE_MC,
+enum mlxsw_sp_flood_type {
+	MLXSW_SP_FLOOD_TYPE_UC,
+	MLXSW_SP_FLOOD_TYPE_BC,
+	MLXSW_SP_FLOOD_TYPE_MC,
 };
 
+/* spectrum_buffers.c */
 int mlxsw_sp_buffers_init(struct mlxsw_sp *mlxsw_sp);
 void mlxsw_sp_buffers_fini(struct mlxsw_sp *mlxsw_sp);
 int mlxsw_sp_port_buffers_init(struct mlxsw_sp_port *mlxsw_sp_port);
@@ -515,26 +307,26 @@ int mlxsw_sp_sb_occ_tc_port_bind_get(struct mlxsw_core_port *mlxsw_core_port,
 				     unsigned int sb_index, u16 tc_index,
 				     enum devlink_sb_pool_type pool_type,
 				     u32 *p_cur, u32 *p_max);
+u32 mlxsw_sp_cells_bytes(const struct mlxsw_sp *mlxsw_sp, u32 cells);
+u32 mlxsw_sp_bytes_cells(const struct mlxsw_sp *mlxsw_sp, u32 bytes);
 
+/* spectrum_switchdev.c */
 int mlxsw_sp_switchdev_init(struct mlxsw_sp *mlxsw_sp);
 void mlxsw_sp_switchdev_fini(struct mlxsw_sp *mlxsw_sp);
-int mlxsw_sp_port_vlan_init(struct mlxsw_sp_port *mlxsw_sp_port);
 void mlxsw_sp_port_switchdev_init(struct mlxsw_sp_port *mlxsw_sp_port);
 void mlxsw_sp_port_switchdev_fini(struct mlxsw_sp_port *mlxsw_sp_port);
-int mlxsw_sp_port_vid_to_fid_set(struct mlxsw_sp_port *mlxsw_sp_port,
-				 enum mlxsw_reg_svfa_mt mt, bool valid, u16 fid,
-				 u16 vid);
-int mlxsw_sp_port_vlan_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid_begin,
-			   u16 vid_end, bool is_member, bool untagged);
-int mlxsw_sp_vport_flood_set(struct mlxsw_sp_port *mlxsw_sp_vport, u16 fid,
-			     bool set);
-void mlxsw_sp_port_active_vlans_del(struct mlxsw_sp_port *mlxsw_sp_port);
-int mlxsw_sp_port_pvid_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid);
-int mlxsw_sp_port_fdb_flush(struct mlxsw_sp_port *mlxsw_sp_port, u16 fid);
 int mlxsw_sp_rif_fdb_op(struct mlxsw_sp *mlxsw_sp, const char *mac, u16 fid,
 			bool adding);
-struct mlxsw_sp_fid *mlxsw_sp_fid_create(struct mlxsw_sp *mlxsw_sp, u16 fid);
-void mlxsw_sp_fid_destroy(struct mlxsw_sp *mlxsw_sp, struct mlxsw_sp_fid *f);
+void
+mlxsw_sp_port_vlan_bridge_leave(struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan);
+int mlxsw_sp_port_bridge_join(struct mlxsw_sp_port *mlxsw_sp_port,
+			      struct net_device *brport_dev,
+			      struct net_device *br_dev);
+void mlxsw_sp_port_bridge_leave(struct mlxsw_sp_port *mlxsw_sp_port,
+				struct net_device *brport_dev,
+				struct net_device *br_dev);
+
+/* spectrum.c */
 int mlxsw_sp_port_ets_set(struct mlxsw_sp_port *mlxsw_sp_port,
 			  enum mlxsw_reg_qeec_hr hr, u8 index, u8 next_index,
 			  bool dwrr, u8 dwrr_weight);
@@ -546,27 +338,45 @@ int __mlxsw_sp_port_headroom_set(struct mlxsw_sp_port *mlxsw_sp_port, int mtu,
 int mlxsw_sp_port_ets_maxrate_set(struct mlxsw_sp_port *mlxsw_sp_port,
 				  enum mlxsw_reg_qeec_hr hr, u8 index,
 				  u8 next_index, u32 maxrate);
-int __mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
-				     u16 vid_begin, u16 vid_end,
-				     bool learn_enable);
+int mlxsw_sp_port_vid_stp_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid,
+			      u8 state);
+int mlxsw_sp_port_vp_mode_set(struct mlxsw_sp_port *mlxsw_sp_port, bool enable);
+int mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid,
+				   bool learn_enable);
+int mlxsw_sp_port_pvid_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid);
+struct mlxsw_sp_port_vlan *
+mlxsw_sp_port_vlan_get(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid);
+void mlxsw_sp_port_vlan_put(struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan);
+int mlxsw_sp_port_vlan_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid_begin,
+			   u16 vid_end, bool is_member, bool untagged);
+int mlxsw_sp_flow_counter_get(struct mlxsw_sp *mlxsw_sp,
+			      unsigned int counter_index, u64 *packets,
+			      u64 *bytes);
+int mlxsw_sp_flow_counter_alloc(struct mlxsw_sp *mlxsw_sp,
+				unsigned int *p_counter_index);
+void mlxsw_sp_flow_counter_free(struct mlxsw_sp *mlxsw_sp,
+				unsigned int counter_index);
+bool mlxsw_sp_port_dev_check(const struct net_device *dev);
+struct mlxsw_sp *mlxsw_sp_lower_get(struct net_device *dev);
+struct mlxsw_sp_port *mlxsw_sp_port_dev_lower_find(struct net_device *dev);
+struct mlxsw_sp_port *mlxsw_sp_port_lower_dev_hold(struct net_device *dev);
+void mlxsw_sp_port_dev_put(struct mlxsw_sp_port *mlxsw_sp_port);
+struct mlxsw_sp_port *mlxsw_sp_port_dev_lower_find_rcu(struct net_device *dev);
 
+/* spectrum_dcb.c */
 #ifdef CONFIG_MLXSW_SPECTRUM_DCB
-
 int mlxsw_sp_port_dcb_init(struct mlxsw_sp_port *mlxsw_sp_port);
 void mlxsw_sp_port_dcb_fini(struct mlxsw_sp_port *mlxsw_sp_port);
-
 #else
-
 static inline int mlxsw_sp_port_dcb_init(struct mlxsw_sp_port *mlxsw_sp_port)
 {
 	return 0;
 }
-
 static inline void mlxsw_sp_port_dcb_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 {}
-
 #endif
 
+/* spectrum_router.c */
 int mlxsw_sp_router_init(struct mlxsw_sp *mlxsw_sp);
 void mlxsw_sp_router_fini(struct mlxsw_sp *mlxsw_sp);
 int mlxsw_sp_router_netevent_event(struct notifier_block *unused,
@@ -574,17 +384,17 @@ int mlxsw_sp_router_netevent_event(struct notifier_block *unused,
 int mlxsw_sp_netdevice_router_port_event(struct net_device *dev);
 int mlxsw_sp_inetaddr_event(struct notifier_block *unused,
 			    unsigned long event, void *ptr);
-void mlxsw_sp_rif_bridge_destroy(struct mlxsw_sp *mlxsw_sp,
-				 struct mlxsw_sp_rif *rif);
 int mlxsw_sp_netdevice_vrf_event(struct net_device *l3_dev, unsigned long event,
 				 struct netdev_notifier_changeupper_info *info);
+void
+mlxsw_sp_port_vlan_router_leave(struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan);
+void mlxsw_sp_rif_destroy(struct mlxsw_sp_rif *rif);
 
+/* spectrum_kvdl.c */
 int mlxsw_sp_kvdl_alloc(struct mlxsw_sp *mlxsw_sp, unsigned int entry_count,
 			u32 *p_entry_index);
 void mlxsw_sp_kvdl_free(struct mlxsw_sp *mlxsw_sp, int entry_index);
 
-struct mlxsw_afk *mlxsw_sp_acl_afk(struct mlxsw_sp_acl *acl);
-
 struct mlxsw_sp_acl_rule_info {
 	unsigned int priority;
 	struct mlxsw_afk_element_values values;
@@ -625,6 +435,8 @@ struct mlxsw_sp_acl_ops {
 
 struct mlxsw_sp_acl_ruleset;
 
+/* spectrum_acl.c */
+struct mlxsw_afk *mlxsw_sp_acl_afk(struct mlxsw_sp_acl *acl);
 struct mlxsw_sp_acl_ruleset *
 mlxsw_sp_acl_ruleset_get(struct mlxsw_sp *mlxsw_sp,
 			 struct net_device *dev, bool ingress,
@@ -649,6 +461,7 @@ void mlxsw_sp_acl_rulei_act_continue(struct mlxsw_sp_acl_rule_info *rulei);
 void mlxsw_sp_acl_rulei_act_jump(struct mlxsw_sp_acl_rule_info *rulei,
 				 u16 group_id);
 int mlxsw_sp_acl_rulei_act_drop(struct mlxsw_sp_acl_rule_info *rulei);
+int mlxsw_sp_acl_rulei_act_trap(struct mlxsw_sp_acl_rule_info *rulei);
 int mlxsw_sp_acl_rulei_act_fwd(struct mlxsw_sp *mlxsw_sp,
 			       struct mlxsw_sp_acl_rule_info *rulei,
 			       struct net_device *out_dev);
@@ -683,23 +496,48 @@ int mlxsw_sp_acl_rule_get_stats(struct mlxsw_sp *mlxsw_sp,
 				struct mlxsw_sp_acl_rule *rule,
 				u64 *packets, u64 *bytes, u64 *last_use);
 
+struct mlxsw_sp_fid *mlxsw_sp_acl_dummy_fid(struct mlxsw_sp *mlxsw_sp);
+
 int mlxsw_sp_acl_init(struct mlxsw_sp *mlxsw_sp);
 void mlxsw_sp_acl_fini(struct mlxsw_sp *mlxsw_sp);
 
+/* spectrum_acl_tcam.c */
 extern const struct mlxsw_sp_acl_ops mlxsw_sp_acl_tcam_ops;
 
+/* spectrum_flower.c */
 int mlxsw_sp_flower_replace(struct mlxsw_sp_port *mlxsw_sp_port, bool ingress,
 			    __be16 protocol, struct tc_cls_flower_offload *f);
 void mlxsw_sp_flower_destroy(struct mlxsw_sp_port *mlxsw_sp_port, bool ingress,
 			     struct tc_cls_flower_offload *f);
 int mlxsw_sp_flower_stats(struct mlxsw_sp_port *mlxsw_sp_port, bool ingress,
 			  struct tc_cls_flower_offload *f);
-int mlxsw_sp_flow_counter_get(struct mlxsw_sp *mlxsw_sp,
-			      unsigned int counter_index, u64 *packets,
-			      u64 *bytes);
-int mlxsw_sp_flow_counter_alloc(struct mlxsw_sp *mlxsw_sp,
-				unsigned int *p_counter_index);
-void mlxsw_sp_flow_counter_free(struct mlxsw_sp *mlxsw_sp,
-				unsigned int counter_index);
+
+/* spectrum_fid.c */
+int mlxsw_sp_fid_flood_set(struct mlxsw_sp_fid *fid,
+			   enum mlxsw_sp_flood_type packet_type, u8 local_port,
+			   bool member);
+int mlxsw_sp_fid_port_vid_map(struct mlxsw_sp_fid *fid,
+			      struct mlxsw_sp_port *mlxsw_sp_port, u16 vid);
+void mlxsw_sp_fid_port_vid_unmap(struct mlxsw_sp_fid *fid,
+				 struct mlxsw_sp_port *mlxsw_sp_port, u16 vid);
+enum mlxsw_sp_rif_type mlxsw_sp_fid_rif_type(const struct mlxsw_sp_fid *fid);
+u16 mlxsw_sp_fid_index(const struct mlxsw_sp_fid *fid);
+enum mlxsw_sp_fid_type mlxsw_sp_fid_type(const struct mlxsw_sp_fid *fid);
+void mlxsw_sp_fid_rif_set(struct mlxsw_sp_fid *fid, struct mlxsw_sp_rif *rif);
+enum mlxsw_sp_rif_type
+mlxsw_sp_fid_type_rif_type(const struct mlxsw_sp *mlxsw_sp,
+			   enum mlxsw_sp_fid_type type);
+u16 mlxsw_sp_fid_8021q_vid(const struct mlxsw_sp_fid *fid);
+struct mlxsw_sp_fid *mlxsw_sp_fid_8021q_get(struct mlxsw_sp *mlxsw_sp, u16 vid);
+struct mlxsw_sp_fid *mlxsw_sp_fid_8021d_get(struct mlxsw_sp *mlxsw_sp,
+					    int br_ifindex);
+struct mlxsw_sp_fid *mlxsw_sp_fid_rfid_get(struct mlxsw_sp *mlxsw_sp,
+					   u16 rif_index);
+struct mlxsw_sp_fid *mlxsw_sp_fid_dummy_get(struct mlxsw_sp *mlxsw_sp);
+void mlxsw_sp_fid_put(struct mlxsw_sp_fid *fid);
+int mlxsw_sp_port_fids_init(struct mlxsw_sp_port *mlxsw_sp_port);
+void mlxsw_sp_port_fids_fini(struct mlxsw_sp_port *mlxsw_sp_port);
+int mlxsw_sp_fids_init(struct mlxsw_sp *mlxsw_sp);
+void mlxsw_sp_fids_fini(struct mlxsw_sp *mlxsw_sp);
 
 #endif
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_acl.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_acl.c
index 317f7b14627f..01a1501b56ca 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_acl.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_acl.c
@@ -53,6 +53,7 @@ struct mlxsw_sp_acl {
 	struct mlxsw_sp *mlxsw_sp;
 	struct mlxsw_afk *afk;
 	struct mlxsw_afa *afa;
+	struct mlxsw_sp_fid *dummy_fid;
 	const struct mlxsw_sp_acl_ops *ops;
 	struct rhashtable ruleset_ht;
 	struct list_head rules;
@@ -112,6 +113,11 @@ static const struct rhashtable_params mlxsw_sp_acl_rule_ht_params = {
 	.automatic_shrinking = true,
 };
 
+struct mlxsw_sp_fid *mlxsw_sp_acl_dummy_fid(struct mlxsw_sp *mlxsw_sp)
+{
+	return mlxsw_sp->acl->dummy_fid;
+}
+
 static struct mlxsw_sp_acl_ruleset *
 mlxsw_sp_acl_ruleset_create(struct mlxsw_sp *mlxsw_sp,
 			    const struct mlxsw_sp_acl_profile_ops *ops)
@@ -341,6 +347,11 @@ int mlxsw_sp_acl_rulei_act_drop(struct mlxsw_sp_acl_rule_info *rulei)
 	return mlxsw_afa_block_append_drop(rulei->act_block);
 }
 
+int mlxsw_sp_acl_rulei_act_trap(struct mlxsw_sp_acl_rule_info *rulei)
+{
+	return mlxsw_afa_block_append_trap(rulei->act_block);
+}
+
 int mlxsw_sp_acl_rulei_act_fwd(struct mlxsw_sp *mlxsw_sp,
 			       struct mlxsw_sp_acl_rule_info *rulei,
 			       struct net_device *out_dev)
@@ -676,6 +687,7 @@ static const struct mlxsw_afa_ops mlxsw_sp_act_afa_ops = {
 int mlxsw_sp_acl_init(struct mlxsw_sp *mlxsw_sp)
 {
 	const struct mlxsw_sp_acl_ops *acl_ops = &mlxsw_sp_acl_tcam_ops;
+	struct mlxsw_sp_fid *fid;
 	struct mlxsw_sp_acl *acl;
 	int err;
 
@@ -706,6 +718,13 @@ int mlxsw_sp_acl_init(struct mlxsw_sp *mlxsw_sp)
 	if (err)
 		goto err_rhashtable_init;
 
+	fid = mlxsw_sp_fid_dummy_get(mlxsw_sp);
+	if (IS_ERR(fid)) {
+		err = PTR_ERR(fid);
+		goto err_fid_get;
+	}
+	acl->dummy_fid = fid;
+
 	INIT_LIST_HEAD(&acl->rules);
 	err = acl_ops->init(mlxsw_sp, acl->priv);
 	if (err)
@@ -721,6 +740,8 @@ int mlxsw_sp_acl_init(struct mlxsw_sp *mlxsw_sp)
 	return 0;
 
 err_acl_ops_init:
+	mlxsw_sp_fid_put(fid);
+err_fid_get:
 	rhashtable_destroy(&acl->ruleset_ht);
 err_rhashtable_init:
 	mlxsw_afa_destroy(acl->afa);
@@ -739,6 +760,7 @@ void mlxsw_sp_acl_fini(struct mlxsw_sp *mlxsw_sp)
 	cancel_delayed_work_sync(&mlxsw_sp->acl->rule_activity_update.dw);
 	acl_ops->fini(mlxsw_sp, acl->priv);
 	WARN_ON(!list_empty(&acl->rules));
+	mlxsw_sp_fid_put(acl->dummy_fid);
 	rhashtable_destroy(&acl->ruleset_ht);
 	mlxsw_afa_destroy(acl->afa);
 	mlxsw_afk_destroy(acl->afk);
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_acl_flex_keys.h b/drivers/net/ethernet/mellanox/mlxsw/spectrum_acl_flex_keys.h
index af7b7bad48df..85d5001a5818 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_acl_flex_keys.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_acl_flex_keys.h
@@ -68,6 +68,11 @@ static struct mlxsw_afk_element_inst mlxsw_sp_afk_element_info_ipv4_dip[] = {
 	MLXSW_AFK_ELEMENT_INST_U32(SRC_SYS_PORT, 0x0C, 0, 16),
 };
 
+static struct mlxsw_afk_element_inst mlxsw_sp_afk_element_info_ipv4[] = {
+	MLXSW_AFK_ELEMENT_INST_U32(SRC_IP4, 0x00, 0, 32),
+	MLXSW_AFK_ELEMENT_INST_U32(TCP_FLAGS, 0x08, 8, 9), /* TCP_CONTROL+TCP_ECN */
+};
+
 static struct mlxsw_afk_element_inst mlxsw_sp_afk_element_info_ipv4_ex[] = {
 	MLXSW_AFK_ELEMENT_INST_U32(VID, 0x00, 0, 12),
 	MLXSW_AFK_ELEMENT_INST_U32(PCP, 0x08, 29, 3),
@@ -102,6 +107,7 @@ static const struct mlxsw_afk_block mlxsw_sp_afk_blocks[] = {
 	MLXSW_AFK_BLOCK(0x12, mlxsw_sp_afk_element_info_l2_smac_ex),
 	MLXSW_AFK_BLOCK(0x30, mlxsw_sp_afk_element_info_ipv4_sip),
 	MLXSW_AFK_BLOCK(0x31, mlxsw_sp_afk_element_info_ipv4_dip),
+	MLXSW_AFK_BLOCK(0x32, mlxsw_sp_afk_element_info_ipv4),
 	MLXSW_AFK_BLOCK(0x33, mlxsw_sp_afk_element_info_ipv4_ex),
 	MLXSW_AFK_BLOCK(0x60, mlxsw_sp_afk_element_info_ipv6_dip),
 	MLXSW_AFK_BLOCK(0x65, mlxsw_sp_afk_element_info_ipv6_ex1),
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_acl_tcam.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_acl_tcam.c
index 3a24289979d9..61a10f166f97 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_acl_tcam.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_acl_tcam.c
@@ -983,6 +983,7 @@ static const enum mlxsw_afk_element mlxsw_sp_acl_tcam_pattern_ipv4[] = {
 	MLXSW_AFK_ELEMENT_SRC_L4_PORT,
 	MLXSW_AFK_ELEMENT_VID,
 	MLXSW_AFK_ELEMENT_PCP,
+	MLXSW_AFK_ELEMENT_TCP_FLAGS,
 };
 
 static const enum mlxsw_afk_element mlxsw_sp_acl_tcam_pattern_ipv6[] = {
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_buffers.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_buffers.c
index 997189cfe7fd..93728c694e6d 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_buffers.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_buffers.c
@@ -43,25 +43,72 @@
 #include "port.h"
 #include "reg.h"
 
+struct mlxsw_sp_sb_pr {
+	enum mlxsw_reg_sbpr_mode mode;
+	u32 size;
+};
+
+struct mlxsw_cp_sb_occ {
+	u32 cur;
+	u32 max;
+};
+
+struct mlxsw_sp_sb_cm {
+	u32 min_buff;
+	u32 max_buff;
+	u8 pool;
+	struct mlxsw_cp_sb_occ occ;
+};
+
+struct mlxsw_sp_sb_pm {
+	u32 min_buff;
+	u32 max_buff;
+	struct mlxsw_cp_sb_occ occ;
+};
+
+#define MLXSW_SP_SB_POOL_COUNT	4
+#define MLXSW_SP_SB_TC_COUNT	8
+
+struct mlxsw_sp_sb_port {
+	struct mlxsw_sp_sb_cm cms[2][MLXSW_SP_SB_TC_COUNT];
+	struct mlxsw_sp_sb_pm pms[2][MLXSW_SP_SB_POOL_COUNT];
+};
+
+struct mlxsw_sp_sb {
+	struct mlxsw_sp_sb_pr prs[2][MLXSW_SP_SB_POOL_COUNT];
+	struct mlxsw_sp_sb_port *ports;
+	u32 cell_size;
+};
+
+u32 mlxsw_sp_cells_bytes(const struct mlxsw_sp *mlxsw_sp, u32 cells)
+{
+	return mlxsw_sp->sb->cell_size * cells;
+}
+
+u32 mlxsw_sp_bytes_cells(const struct mlxsw_sp *mlxsw_sp, u32 bytes)
+{
+	return DIV_ROUND_UP(bytes, mlxsw_sp->sb->cell_size);
+}
+
 static struct mlxsw_sp_sb_pr *mlxsw_sp_sb_pr_get(struct mlxsw_sp *mlxsw_sp,
 						 u8 pool,
 						 enum mlxsw_reg_sbxx_dir dir)
 {
-	return &mlxsw_sp->sb.prs[dir][pool];
+	return &mlxsw_sp->sb->prs[dir][pool];
 }
 
 static struct mlxsw_sp_sb_cm *mlxsw_sp_sb_cm_get(struct mlxsw_sp *mlxsw_sp,
 						 u8 local_port, u8 pg_buff,
 						 enum mlxsw_reg_sbxx_dir dir)
 {
-	return &mlxsw_sp->sb.ports[local_port].cms[dir][pg_buff];
+	return &mlxsw_sp->sb->ports[local_port].cms[dir][pg_buff];
 }
 
 static struct mlxsw_sp_sb_pm *mlxsw_sp_sb_pm_get(struct mlxsw_sp *mlxsw_sp,
 						 u8 local_port, u8 pool,
 						 enum mlxsw_reg_sbxx_dir dir)
 {
-	return &mlxsw_sp->sb.ports[local_port].pms[dir][pool];
+	return &mlxsw_sp->sb->ports[local_port].pms[dir][pool];
 }
 
 static int mlxsw_sp_sb_pr_write(struct mlxsw_sp *mlxsw_sp, u8 pool,
@@ -215,16 +262,17 @@ static int mlxsw_sp_sb_ports_init(struct mlxsw_sp *mlxsw_sp)
 {
 	unsigned int max_ports = mlxsw_core_max_ports(mlxsw_sp->core);
 
-	mlxsw_sp->sb.ports = kcalloc(max_ports, sizeof(struct mlxsw_sp_sb_port),
-				     GFP_KERNEL);
-	if (!mlxsw_sp->sb.ports)
+	mlxsw_sp->sb->ports = kcalloc(max_ports,
+				      sizeof(struct mlxsw_sp_sb_port),
+				      GFP_KERNEL);
+	if (!mlxsw_sp->sb->ports)
 		return -ENOMEM;
 	return 0;
 }
 
 static void mlxsw_sp_sb_ports_fini(struct mlxsw_sp *mlxsw_sp)
 {
-	kfree(mlxsw_sp->sb.ports);
+	kfree(mlxsw_sp->sb->ports);
 }
 
 #define MLXSW_SP_SB_PR_INGRESS_SIZE	12440000
@@ -551,15 +599,19 @@ int mlxsw_sp_buffers_init(struct mlxsw_sp *mlxsw_sp)
 
 	if (!MLXSW_CORE_RES_VALID(mlxsw_sp->core, CELL_SIZE))
 		return -EIO;
-	mlxsw_sp->sb.cell_size = MLXSW_CORE_RES_GET(mlxsw_sp->core, CELL_SIZE);
 
 	if (!MLXSW_CORE_RES_VALID(mlxsw_sp->core, MAX_BUFFER_SIZE))
 		return -EIO;
 	sb_size = MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_BUFFER_SIZE);
 
+	mlxsw_sp->sb = kzalloc(sizeof(*mlxsw_sp->sb), GFP_KERNEL);
+	if (!mlxsw_sp->sb)
+		return -ENOMEM;
+	mlxsw_sp->sb->cell_size = MLXSW_CORE_RES_GET(mlxsw_sp->core, CELL_SIZE);
+
 	err = mlxsw_sp_sb_ports_init(mlxsw_sp);
 	if (err)
-		return err;
+		goto err_sb_ports_init;
 	err = mlxsw_sp_sb_prs_init(mlxsw_sp);
 	if (err)
 		goto err_sb_prs_init;
@@ -584,6 +636,8 @@ err_sb_mms_init:
 err_sb_cpu_port_sb_cms_init:
 err_sb_prs_init:
 	mlxsw_sp_sb_ports_fini(mlxsw_sp);
+err_sb_ports_init:
+	kfree(mlxsw_sp->sb);
 	return err;
 }
 
@@ -591,6 +645,7 @@ void mlxsw_sp_buffers_fini(struct mlxsw_sp *mlxsw_sp)
 {
 	devlink_sb_unregister(priv_to_devlink(mlxsw_sp->core), 0);
 	mlxsw_sp_sb_ports_fini(mlxsw_sp);
+	kfree(mlxsw_sp->sb);
 }
 
 int mlxsw_sp_port_buffers_init(struct mlxsw_sp_port *mlxsw_sp_port)
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_dpipe.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_dpipe.c
index 5f0a7bc692a4..af2c65a3fd9f 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_dpipe.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_dpipe.c
@@ -218,7 +218,7 @@ static int
 mlxsw_sp_table_erif_entries_dump(void *priv, bool counters_enabled,
 				 struct devlink_dpipe_dump_ctx *dump_ctx)
 {
-	struct devlink_dpipe_value match_value = {{0}}, action_value = {{0}};
+	struct devlink_dpipe_value match_value, action_value;
 	struct devlink_dpipe_action action = {0};
 	struct devlink_dpipe_match match = {0};
 	struct devlink_dpipe_entry entry = {0};
@@ -227,6 +227,9 @@ mlxsw_sp_table_erif_entries_dump(void *priv, bool counters_enabled,
 	int i, j;
 	int err;
 
+	memset(&match_value, 0, sizeof(match_value));
+	memset(&action_value, 0, sizeof(action_value));
+
 	mlxsw_sp_erif_match_action_prepare(&match, &action);
 	err = mlxsw_sp_erif_entry_prepare(&entry, &match_value, &match,
 					  &action_value, &action);
@@ -242,10 +245,11 @@ start_again:
 		return err;
 	j = 0;
 	for (; i < rif_count; i++) {
-		if (!mlxsw_sp->rifs[i])
+		struct mlxsw_sp_rif *rif = mlxsw_sp_rif_by_index(mlxsw_sp, i);
+
+		if (!rif)
 			continue;
-		err = mlxsw_sp_erif_entry_get(mlxsw_sp, &entry,
-					      mlxsw_sp->rifs[i],
+		err = mlxsw_sp_erif_entry_get(mlxsw_sp, &entry, rif,
 					      counters_enabled);
 		if (err)
 			goto err_entry_get;
@@ -282,15 +286,15 @@ static int mlxsw_sp_table_erif_counters_update(void *priv, bool enable)
 
 	rtnl_lock();
 	for (i = 0; i < MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_RIFS); i++) {
-		if (!mlxsw_sp->rifs[i])
+		struct mlxsw_sp_rif *rif = mlxsw_sp_rif_by_index(mlxsw_sp, i);
+
+		if (!rif)
 			continue;
 		if (enable)
-			mlxsw_sp_rif_counter_alloc(mlxsw_sp,
-						   mlxsw_sp->rifs[i],
+			mlxsw_sp_rif_counter_alloc(mlxsw_sp, rif,
 						   MLXSW_SP_RIF_COUNTER_EGRESS);
 		else
-			mlxsw_sp_rif_counter_free(mlxsw_sp,
-						  mlxsw_sp->rifs[i],
+			mlxsw_sp_rif_counter_free(mlxsw_sp, rif,
 						  MLXSW_SP_RIF_COUNTER_EGRESS);
 	}
 	rtnl_unlock();
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_fid.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_fid.c
new file mode 100644
index 000000000000..6afbe9ec64e2
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_fid.c
@@ -0,0 +1,992 @@
+/*
+ * drivers/net/ethernet/mellanox/mlxsw/spectrum_fid.c
+ * Copyright (c) 2017 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2017 Ido Schimmel <idosch@mellanox.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the names of the copyright holders nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <linux/kernel.h>
+#include <linux/bitops.h>
+#include <linux/if_vlan.h>
+#include <linux/if_bridge.h>
+#include <linux/netdevice.h>
+#include <linux/rtnetlink.h>
+
+#include "spectrum.h"
+#include "reg.h"
+
+struct mlxsw_sp_fid_family;
+
+struct mlxsw_sp_fid_core {
+	struct mlxsw_sp_fid_family *fid_family_arr[MLXSW_SP_FID_TYPE_MAX];
+	unsigned int *port_fid_mappings;
+};
+
+struct mlxsw_sp_fid {
+	struct list_head list;
+	struct mlxsw_sp_rif *rif;
+	unsigned int ref_count;
+	u16 fid_index;
+	struct mlxsw_sp_fid_family *fid_family;
+};
+
+struct mlxsw_sp_fid_8021q {
+	struct mlxsw_sp_fid common;
+	u16 vid;
+};
+
+struct mlxsw_sp_fid_8021d {
+	struct mlxsw_sp_fid common;
+	int br_ifindex;
+};
+
+struct mlxsw_sp_flood_table {
+	enum mlxsw_sp_flood_type packet_type;
+	enum mlxsw_reg_sfgc_bridge_type bridge_type;
+	enum mlxsw_flood_table_type table_type;
+	int table_index;
+};
+
+struct mlxsw_sp_fid_ops {
+	void (*setup)(struct mlxsw_sp_fid *fid, const void *arg);
+	int (*configure)(struct mlxsw_sp_fid *fid);
+	void (*deconfigure)(struct mlxsw_sp_fid *fid);
+	int (*index_alloc)(struct mlxsw_sp_fid *fid, const void *arg,
+			   u16 *p_fid_index);
+	bool (*compare)(const struct mlxsw_sp_fid *fid,
+			const void *arg);
+	u16 (*flood_index)(const struct mlxsw_sp_fid *fid);
+	int (*port_vid_map)(struct mlxsw_sp_fid *fid,
+			    struct mlxsw_sp_port *port, u16 vid);
+	void (*port_vid_unmap)(struct mlxsw_sp_fid *fid,
+			       struct mlxsw_sp_port *port, u16 vid);
+};
+
+struct mlxsw_sp_fid_family {
+	enum mlxsw_sp_fid_type type;
+	size_t fid_size;
+	u16 start_index;
+	u16 end_index;
+	struct list_head fids_list;
+	unsigned long *fids_bitmap;
+	const struct mlxsw_sp_flood_table *flood_tables;
+	int nr_flood_tables;
+	enum mlxsw_sp_rif_type rif_type;
+	const struct mlxsw_sp_fid_ops *ops;
+	struct mlxsw_sp *mlxsw_sp;
+};
+
+static const int mlxsw_sp_sfgc_uc_packet_types[MLXSW_REG_SFGC_TYPE_MAX] = {
+	[MLXSW_REG_SFGC_TYPE_UNKNOWN_UNICAST]			= 1,
+};
+
+static const int mlxsw_sp_sfgc_bc_packet_types[MLXSW_REG_SFGC_TYPE_MAX] = {
+	[MLXSW_REG_SFGC_TYPE_BROADCAST]				= 1,
+	[MLXSW_REG_SFGC_TYPE_UNREGISTERED_MULTICAST_IPV6]	= 1,
+	[MLXSW_REG_SFGC_TYPE_UNREGISTERED_MULTICAST_NON_IP]	= 1,
+	[MLXSW_REG_SFGC_TYPE_IPV4_LINK_LOCAL]			= 1,
+	[MLXSW_REG_SFGC_TYPE_IPV6_ALL_HOST]			= 1,
+};
+
+static const int mlxsw_sp_sfgc_mc_packet_types[MLXSW_REG_SFGC_TYPE_MAX] = {
+	[MLXSW_REG_SFGC_TYPE_UNREGISTERED_MULTICAST_IPV4]	= 1,
+};
+
+static const int *mlxsw_sp_packet_type_sfgc_types[] = {
+	[MLXSW_SP_FLOOD_TYPE_UC]	= mlxsw_sp_sfgc_uc_packet_types,
+	[MLXSW_SP_FLOOD_TYPE_BC]	= mlxsw_sp_sfgc_bc_packet_types,
+	[MLXSW_SP_FLOOD_TYPE_MC]	= mlxsw_sp_sfgc_mc_packet_types,
+};
+
+static const struct mlxsw_sp_flood_table *
+mlxsw_sp_fid_flood_table_lookup(const struct mlxsw_sp_fid *fid,
+				enum mlxsw_sp_flood_type packet_type)
+{
+	struct mlxsw_sp_fid_family *fid_family = fid->fid_family;
+	int i;
+
+	for (i = 0; i < fid_family->nr_flood_tables; i++) {
+		if (fid_family->flood_tables[i].packet_type != packet_type)
+			continue;
+		return &fid_family->flood_tables[i];
+	}
+
+	return NULL;
+}
+
+int mlxsw_sp_fid_flood_set(struct mlxsw_sp_fid *fid,
+			   enum mlxsw_sp_flood_type packet_type, u8 local_port,
+			   bool member)
+{
+	struct mlxsw_sp_fid_family *fid_family = fid->fid_family;
+	const struct mlxsw_sp_fid_ops *ops = fid_family->ops;
+	const struct mlxsw_sp_flood_table *flood_table;
+	char *sftr_pl;
+	int err;
+
+	if (WARN_ON(!fid_family->flood_tables || !ops->flood_index))
+		return -EINVAL;
+
+	flood_table = mlxsw_sp_fid_flood_table_lookup(fid, packet_type);
+	if (!flood_table)
+		return -ESRCH;
+
+	sftr_pl = kmalloc(MLXSW_REG_SFTR_LEN, GFP_KERNEL);
+	if (!sftr_pl)
+		return -ENOMEM;
+
+	mlxsw_reg_sftr_pack(sftr_pl, flood_table->table_index,
+			    ops->flood_index(fid), flood_table->table_type, 1,
+			    local_port, member);
+	err = mlxsw_reg_write(fid_family->mlxsw_sp->core, MLXSW_REG(sftr),
+			      sftr_pl);
+	kfree(sftr_pl);
+	return err;
+}
+
+int mlxsw_sp_fid_port_vid_map(struct mlxsw_sp_fid *fid,
+			      struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
+{
+	if (WARN_ON(!fid->fid_family->ops->port_vid_map))
+		return -EINVAL;
+	return fid->fid_family->ops->port_vid_map(fid, mlxsw_sp_port, vid);
+}
+
+void mlxsw_sp_fid_port_vid_unmap(struct mlxsw_sp_fid *fid,
+				 struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
+{
+	fid->fid_family->ops->port_vid_unmap(fid, mlxsw_sp_port, vid);
+}
+
+enum mlxsw_sp_rif_type mlxsw_sp_fid_rif_type(const struct mlxsw_sp_fid *fid)
+{
+	return fid->fid_family->rif_type;
+}
+
+u16 mlxsw_sp_fid_index(const struct mlxsw_sp_fid *fid)
+{
+	return fid->fid_index;
+}
+
+enum mlxsw_sp_fid_type mlxsw_sp_fid_type(const struct mlxsw_sp_fid *fid)
+{
+	return fid->fid_family->type;
+}
+
+void mlxsw_sp_fid_rif_set(struct mlxsw_sp_fid *fid, struct mlxsw_sp_rif *rif)
+{
+	fid->rif = rif;
+}
+
+enum mlxsw_sp_rif_type
+mlxsw_sp_fid_type_rif_type(const struct mlxsw_sp *mlxsw_sp,
+			   enum mlxsw_sp_fid_type type)
+{
+	struct mlxsw_sp_fid_core *fid_core = mlxsw_sp->fid_core;
+
+	return fid_core->fid_family_arr[type]->rif_type;
+}
+
+static struct mlxsw_sp_fid_8021q *
+mlxsw_sp_fid_8021q_fid(const struct mlxsw_sp_fid *fid)
+{
+	return container_of(fid, struct mlxsw_sp_fid_8021q, common);
+}
+
+u16 mlxsw_sp_fid_8021q_vid(const struct mlxsw_sp_fid *fid)
+{
+	return mlxsw_sp_fid_8021q_fid(fid)->vid;
+}
+
+static void mlxsw_sp_fid_8021q_setup(struct mlxsw_sp_fid *fid, const void *arg)
+{
+	u16 vid = *(u16 *) arg;
+
+	mlxsw_sp_fid_8021q_fid(fid)->vid = vid;
+}
+
+static enum mlxsw_reg_sfmr_op mlxsw_sp_sfmr_op(bool valid)
+{
+	return valid ? MLXSW_REG_SFMR_OP_CREATE_FID :
+		       MLXSW_REG_SFMR_OP_DESTROY_FID;
+}
+
+static int mlxsw_sp_fid_op(struct mlxsw_sp *mlxsw_sp, u16 fid_index,
+			   u16 fid_offset, bool valid)
+{
+	char sfmr_pl[MLXSW_REG_SFMR_LEN];
+
+	mlxsw_reg_sfmr_pack(sfmr_pl, mlxsw_sp_sfmr_op(valid), fid_index,
+			    fid_offset);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfmr), sfmr_pl);
+}
+
+static int mlxsw_sp_fid_vid_map(struct mlxsw_sp *mlxsw_sp, u16 fid_index,
+				u16 vid, bool valid)
+{
+	enum mlxsw_reg_svfa_mt mt = MLXSW_REG_SVFA_MT_VID_TO_FID;
+	char svfa_pl[MLXSW_REG_SVFA_LEN];
+
+	mlxsw_reg_svfa_pack(svfa_pl, 0, mt, valid, fid_index, vid);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(svfa), svfa_pl);
+}
+
+static int __mlxsw_sp_fid_port_vid_map(struct mlxsw_sp *mlxsw_sp, u16 fid_index,
+				       u8 local_port, u16 vid, bool valid)
+{
+	enum mlxsw_reg_svfa_mt mt = MLXSW_REG_SVFA_MT_PORT_VID_TO_FID;
+	char svfa_pl[MLXSW_REG_SVFA_LEN];
+
+	mlxsw_reg_svfa_pack(svfa_pl, local_port, mt, valid, fid_index, vid);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(svfa), svfa_pl);
+}
+
+static int mlxsw_sp_fid_8021q_configure(struct mlxsw_sp_fid *fid)
+{
+	struct mlxsw_sp *mlxsw_sp = fid->fid_family->mlxsw_sp;
+	struct mlxsw_sp_fid_8021q *fid_8021q;
+	int err;
+
+	err = mlxsw_sp_fid_op(mlxsw_sp, fid->fid_index, fid->fid_index, true);
+	if (err)
+		return err;
+
+	fid_8021q = mlxsw_sp_fid_8021q_fid(fid);
+	err = mlxsw_sp_fid_vid_map(mlxsw_sp, fid->fid_index, fid_8021q->vid,
+				   true);
+	if (err)
+		goto err_fid_map;
+
+	return 0;
+
+err_fid_map:
+	mlxsw_sp_fid_op(mlxsw_sp, fid->fid_index, 0, false);
+	return err;
+}
+
+static void mlxsw_sp_fid_8021q_deconfigure(struct mlxsw_sp_fid *fid)
+{
+	struct mlxsw_sp *mlxsw_sp = fid->fid_family->mlxsw_sp;
+	struct mlxsw_sp_fid_8021q *fid_8021q;
+
+	fid_8021q = mlxsw_sp_fid_8021q_fid(fid);
+	mlxsw_sp_fid_vid_map(mlxsw_sp, fid->fid_index, fid_8021q->vid, false);
+	mlxsw_sp_fid_op(mlxsw_sp, fid->fid_index, 0, false);
+}
+
+static int mlxsw_sp_fid_8021q_index_alloc(struct mlxsw_sp_fid *fid,
+					  const void *arg, u16 *p_fid_index)
+{
+	struct mlxsw_sp_fid_family *fid_family = fid->fid_family;
+	u16 vid = *(u16 *) arg;
+
+	/* Use 1:1 mapping for simplicity although not a must */
+	if (vid < fid_family->start_index || vid > fid_family->end_index)
+		return -EINVAL;
+	*p_fid_index = vid;
+
+	return 0;
+}
+
+static bool
+mlxsw_sp_fid_8021q_compare(const struct mlxsw_sp_fid *fid, const void *arg)
+{
+	u16 vid = *(u16 *) arg;
+
+	return mlxsw_sp_fid_8021q_fid(fid)->vid == vid;
+}
+
+static u16 mlxsw_sp_fid_8021q_flood_index(const struct mlxsw_sp_fid *fid)
+{
+	return fid->fid_index;
+}
+
+static int mlxsw_sp_fid_8021q_port_vid_map(struct mlxsw_sp_fid *fid,
+					   struct mlxsw_sp_port *mlxsw_sp_port,
+					   u16 vid)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	u8 local_port = mlxsw_sp_port->local_port;
+
+	/* In case there are no {Port, VID} => FID mappings on the port,
+	 * we can use the global VID => FID mapping we created when the
+	 * FID was configured.
+	 */
+	if (mlxsw_sp->fid_core->port_fid_mappings[local_port] == 0)
+		return 0;
+	return __mlxsw_sp_fid_port_vid_map(mlxsw_sp, fid->fid_index, local_port,
+					   vid, true);
+}
+
+static void
+mlxsw_sp_fid_8021q_port_vid_unmap(struct mlxsw_sp_fid *fid,
+				  struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	u8 local_port = mlxsw_sp_port->local_port;
+
+	if (mlxsw_sp->fid_core->port_fid_mappings[local_port] == 0)
+		return;
+	__mlxsw_sp_fid_port_vid_map(mlxsw_sp, fid->fid_index, local_port, vid,
+				    false);
+}
+
+static const struct mlxsw_sp_fid_ops mlxsw_sp_fid_8021q_ops = {
+	.setup			= mlxsw_sp_fid_8021q_setup,
+	.configure		= mlxsw_sp_fid_8021q_configure,
+	.deconfigure		= mlxsw_sp_fid_8021q_deconfigure,
+	.index_alloc		= mlxsw_sp_fid_8021q_index_alloc,
+	.compare		= mlxsw_sp_fid_8021q_compare,
+	.flood_index		= mlxsw_sp_fid_8021q_flood_index,
+	.port_vid_map		= mlxsw_sp_fid_8021q_port_vid_map,
+	.port_vid_unmap		= mlxsw_sp_fid_8021q_port_vid_unmap,
+};
+
+static const struct mlxsw_sp_flood_table mlxsw_sp_fid_8021q_flood_tables[] = {
+	{
+		.packet_type	= MLXSW_SP_FLOOD_TYPE_UC,
+		.bridge_type	= MLXSW_REG_SFGC_BRIDGE_TYPE_1Q_FID,
+		.table_type	= MLXSW_REG_SFGC_TABLE_TYPE_FID_OFFSET,
+		.table_index	= 0,
+	},
+	{
+		.packet_type	= MLXSW_SP_FLOOD_TYPE_MC,
+		.bridge_type	= MLXSW_REG_SFGC_BRIDGE_TYPE_1Q_FID,
+		.table_type	= MLXSW_REG_SFGC_TABLE_TYPE_FID_OFFSET,
+		.table_index	= 1,
+	},
+	{
+		.packet_type	= MLXSW_SP_FLOOD_TYPE_BC,
+		.bridge_type	= MLXSW_REG_SFGC_BRIDGE_TYPE_1Q_FID,
+		.table_type	= MLXSW_REG_SFGC_TABLE_TYPE_FID_OFFSET,
+		.table_index	= 2,
+	},
+};
+
+/* Range and flood configuration must match mlxsw_config_profile */
+static const struct mlxsw_sp_fid_family mlxsw_sp_fid_8021q_family = {
+	.type			= MLXSW_SP_FID_TYPE_8021Q,
+	.fid_size		= sizeof(struct mlxsw_sp_fid_8021q),
+	.start_index		= 1,
+	.end_index		= VLAN_VID_MASK,
+	.flood_tables		= mlxsw_sp_fid_8021q_flood_tables,
+	.nr_flood_tables	= ARRAY_SIZE(mlxsw_sp_fid_8021q_flood_tables),
+	.rif_type		= MLXSW_SP_RIF_TYPE_VLAN,
+	.ops			= &mlxsw_sp_fid_8021q_ops,
+};
+
+static struct mlxsw_sp_fid_8021d *
+mlxsw_sp_fid_8021d_fid(const struct mlxsw_sp_fid *fid)
+{
+	return container_of(fid, struct mlxsw_sp_fid_8021d, common);
+}
+
+static void mlxsw_sp_fid_8021d_setup(struct mlxsw_sp_fid *fid, const void *arg)
+{
+	int br_ifindex = *(int *) arg;
+
+	mlxsw_sp_fid_8021d_fid(fid)->br_ifindex = br_ifindex;
+}
+
+static int mlxsw_sp_fid_8021d_configure(struct mlxsw_sp_fid *fid)
+{
+	struct mlxsw_sp_fid_family *fid_family = fid->fid_family;
+
+	return mlxsw_sp_fid_op(fid_family->mlxsw_sp, fid->fid_index, 0, true);
+}
+
+static void mlxsw_sp_fid_8021d_deconfigure(struct mlxsw_sp_fid *fid)
+{
+	mlxsw_sp_fid_op(fid->fid_family->mlxsw_sp, fid->fid_index, 0, false);
+}
+
+static int mlxsw_sp_fid_8021d_index_alloc(struct mlxsw_sp_fid *fid,
+					  const void *arg, u16 *p_fid_index)
+{
+	struct mlxsw_sp_fid_family *fid_family = fid->fid_family;
+	u16 nr_fids, fid_index;
+
+	nr_fids = fid_family->end_index - fid_family->start_index + 1;
+	fid_index = find_first_zero_bit(fid_family->fids_bitmap, nr_fids);
+	if (fid_index == nr_fids)
+		return -ENOBUFS;
+	*p_fid_index = fid_family->start_index + fid_index;
+
+	return 0;
+}
+
+static bool
+mlxsw_sp_fid_8021d_compare(const struct mlxsw_sp_fid *fid, const void *arg)
+{
+	int br_ifindex = *(int *) arg;
+
+	return mlxsw_sp_fid_8021d_fid(fid)->br_ifindex == br_ifindex;
+}
+
+static u16 mlxsw_sp_fid_8021d_flood_index(const struct mlxsw_sp_fid *fid)
+{
+	return fid->fid_index - fid->fid_family->start_index;
+}
+
+static int mlxsw_sp_port_vp_mode_trans(struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
+	int err;
+
+	list_for_each_entry(mlxsw_sp_port_vlan, &mlxsw_sp_port->vlans_list,
+			    list) {
+		struct mlxsw_sp_fid *fid = mlxsw_sp_port_vlan->fid;
+		u16 vid = mlxsw_sp_port_vlan->vid;
+
+		if (!fid)
+			continue;
+
+		err = __mlxsw_sp_fid_port_vid_map(mlxsw_sp, fid->fid_index,
+						  mlxsw_sp_port->local_port,
+						  vid, true);
+		if (err)
+			goto err_fid_port_vid_map;
+	}
+
+	err = mlxsw_sp_port_vp_mode_set(mlxsw_sp_port, true);
+	if (err)
+		goto err_port_vp_mode_set;
+
+	return 0;
+
+err_port_vp_mode_set:
+err_fid_port_vid_map:
+	list_for_each_entry_continue_reverse(mlxsw_sp_port_vlan,
+					     &mlxsw_sp_port->vlans_list, list) {
+		struct mlxsw_sp_fid *fid = mlxsw_sp_port_vlan->fid;
+		u16 vid = mlxsw_sp_port_vlan->vid;
+
+		if (!fid)
+			continue;
+
+		__mlxsw_sp_fid_port_vid_map(mlxsw_sp, fid->fid_index,
+					    mlxsw_sp_port->local_port, vid,
+					    false);
+	}
+	return err;
+}
+
+static void mlxsw_sp_port_vlan_mode_trans(struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
+
+	mlxsw_sp_port_vp_mode_set(mlxsw_sp_port, false);
+
+	list_for_each_entry_reverse(mlxsw_sp_port_vlan,
+				    &mlxsw_sp_port->vlans_list, list) {
+		struct mlxsw_sp_fid *fid = mlxsw_sp_port_vlan->fid;
+		u16 vid = mlxsw_sp_port_vlan->vid;
+
+		if (!fid)
+			continue;
+
+		__mlxsw_sp_fid_port_vid_map(mlxsw_sp, fid->fid_index,
+					    mlxsw_sp_port->local_port, vid,
+					    false);
+	}
+}
+
+static int mlxsw_sp_fid_8021d_port_vid_map(struct mlxsw_sp_fid *fid,
+					   struct mlxsw_sp_port *mlxsw_sp_port,
+					   u16 vid)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	u8 local_port = mlxsw_sp_port->local_port;
+	int err;
+
+	err = __mlxsw_sp_fid_port_vid_map(mlxsw_sp, fid->fid_index,
+					  mlxsw_sp_port->local_port, vid, true);
+	if (err)
+		return err;
+
+	if (mlxsw_sp->fid_core->port_fid_mappings[local_port]++ == 0) {
+		err = mlxsw_sp_port_vp_mode_trans(mlxsw_sp_port);
+		if (err)
+			goto err_port_vp_mode_trans;
+	}
+
+	return 0;
+
+err_port_vp_mode_trans:
+	mlxsw_sp->fid_core->port_fid_mappings[local_port]--;
+	__mlxsw_sp_fid_port_vid_map(mlxsw_sp, fid->fid_index,
+				    mlxsw_sp_port->local_port, vid, false);
+	return err;
+}
+
+static void
+mlxsw_sp_fid_8021d_port_vid_unmap(struct mlxsw_sp_fid *fid,
+				  struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	u8 local_port = mlxsw_sp_port->local_port;
+
+	if (mlxsw_sp->fid_core->port_fid_mappings[local_port] == 1)
+		mlxsw_sp_port_vlan_mode_trans(mlxsw_sp_port);
+	mlxsw_sp->fid_core->port_fid_mappings[local_port]--;
+	__mlxsw_sp_fid_port_vid_map(mlxsw_sp, fid->fid_index,
+				    mlxsw_sp_port->local_port, vid, false);
+}
+
+static const struct mlxsw_sp_fid_ops mlxsw_sp_fid_8021d_ops = {
+	.setup			= mlxsw_sp_fid_8021d_setup,
+	.configure		= mlxsw_sp_fid_8021d_configure,
+	.deconfigure		= mlxsw_sp_fid_8021d_deconfigure,
+	.index_alloc		= mlxsw_sp_fid_8021d_index_alloc,
+	.compare		= mlxsw_sp_fid_8021d_compare,
+	.flood_index		= mlxsw_sp_fid_8021d_flood_index,
+	.port_vid_map		= mlxsw_sp_fid_8021d_port_vid_map,
+	.port_vid_unmap		= mlxsw_sp_fid_8021d_port_vid_unmap,
+};
+
+static const struct mlxsw_sp_flood_table mlxsw_sp_fid_8021d_flood_tables[] = {
+	{
+		.packet_type	= MLXSW_SP_FLOOD_TYPE_UC,
+		.bridge_type	= MLXSW_REG_SFGC_BRIDGE_TYPE_VFID,
+		.table_type	= MLXSW_REG_SFGC_TABLE_TYPE_FID,
+		.table_index	= 0,
+	},
+	{
+		.packet_type	= MLXSW_SP_FLOOD_TYPE_MC,
+		.bridge_type	= MLXSW_REG_SFGC_BRIDGE_TYPE_VFID,
+		.table_type	= MLXSW_REG_SFGC_TABLE_TYPE_FID,
+		.table_index	= 1,
+	},
+	{
+		.packet_type	= MLXSW_SP_FLOOD_TYPE_BC,
+		.bridge_type	= MLXSW_REG_SFGC_BRIDGE_TYPE_VFID,
+		.table_type	= MLXSW_REG_SFGC_TABLE_TYPE_FID,
+		.table_index	= 2,
+	},
+};
+
+/* Range and flood configuration must match mlxsw_config_profile */
+static const struct mlxsw_sp_fid_family mlxsw_sp_fid_8021d_family = {
+	.type			= MLXSW_SP_FID_TYPE_8021D,
+	.fid_size		= sizeof(struct mlxsw_sp_fid_8021d),
+	.start_index		= VLAN_N_VID,
+	.end_index		= VLAN_N_VID + MLXSW_SP_FID_8021D_MAX - 1,
+	.flood_tables		= mlxsw_sp_fid_8021d_flood_tables,
+	.nr_flood_tables	= ARRAY_SIZE(mlxsw_sp_fid_8021d_flood_tables),
+	.rif_type		= MLXSW_SP_RIF_TYPE_FID,
+	.ops			= &mlxsw_sp_fid_8021d_ops,
+};
+
+static int mlxsw_sp_fid_rfid_configure(struct mlxsw_sp_fid *fid)
+{
+	/* rFIDs are allocated by the device during init */
+	return 0;
+}
+
+static void mlxsw_sp_fid_rfid_deconfigure(struct mlxsw_sp_fid *fid)
+{
+}
+
+static int mlxsw_sp_fid_rfid_index_alloc(struct mlxsw_sp_fid *fid,
+					 const void *arg, u16 *p_fid_index)
+{
+	u16 rif_index = *(u16 *) arg;
+
+	*p_fid_index = fid->fid_family->start_index + rif_index;
+
+	return 0;
+}
+
+static bool mlxsw_sp_fid_rfid_compare(const struct mlxsw_sp_fid *fid,
+				      const void *arg)
+{
+	u16 rif_index = *(u16 *) arg;
+
+	return fid->fid_index == rif_index + fid->fid_family->start_index;
+}
+
+static int mlxsw_sp_fid_rfid_port_vid_map(struct mlxsw_sp_fid *fid,
+					  struct mlxsw_sp_port *mlxsw_sp_port,
+					  u16 vid)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	u8 local_port = mlxsw_sp_port->local_port;
+	int err;
+
+	/* We only need to transition the port to virtual mode since
+	 * {Port, VID} => FID is done by the firmware upon RIF creation.
+	 */
+	if (mlxsw_sp->fid_core->port_fid_mappings[local_port]++ == 0) {
+		err = mlxsw_sp_port_vp_mode_trans(mlxsw_sp_port);
+		if (err)
+			goto err_port_vp_mode_trans;
+	}
+
+	return 0;
+
+err_port_vp_mode_trans:
+	mlxsw_sp->fid_core->port_fid_mappings[local_port]--;
+	return err;
+}
+
+static void
+mlxsw_sp_fid_rfid_port_vid_unmap(struct mlxsw_sp_fid *fid,
+				 struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	u8 local_port = mlxsw_sp_port->local_port;
+
+	if (mlxsw_sp->fid_core->port_fid_mappings[local_port] == 1)
+		mlxsw_sp_port_vlan_mode_trans(mlxsw_sp_port);
+	mlxsw_sp->fid_core->port_fid_mappings[local_port]--;
+}
+
+static const struct mlxsw_sp_fid_ops mlxsw_sp_fid_rfid_ops = {
+	.configure		= mlxsw_sp_fid_rfid_configure,
+	.deconfigure		= mlxsw_sp_fid_rfid_deconfigure,
+	.index_alloc		= mlxsw_sp_fid_rfid_index_alloc,
+	.compare		= mlxsw_sp_fid_rfid_compare,
+	.port_vid_map		= mlxsw_sp_fid_rfid_port_vid_map,
+	.port_vid_unmap		= mlxsw_sp_fid_rfid_port_vid_unmap,
+};
+
+#define MLXSW_SP_RFID_BASE	(15 * 1024)
+#define MLXSW_SP_RFID_MAX	1024
+
+static const struct mlxsw_sp_fid_family mlxsw_sp_fid_rfid_family = {
+	.type			= MLXSW_SP_FID_TYPE_RFID,
+	.fid_size		= sizeof(struct mlxsw_sp_fid),
+	.start_index		= MLXSW_SP_RFID_BASE,
+	.end_index		= MLXSW_SP_RFID_BASE + MLXSW_SP_RFID_MAX - 1,
+	.rif_type		= MLXSW_SP_RIF_TYPE_SUBPORT,
+	.ops			= &mlxsw_sp_fid_rfid_ops,
+};
+
+static int mlxsw_sp_fid_dummy_configure(struct mlxsw_sp_fid *fid)
+{
+	struct mlxsw_sp *mlxsw_sp = fid->fid_family->mlxsw_sp;
+
+	return mlxsw_sp_fid_op(mlxsw_sp, fid->fid_index, 0, true);
+}
+
+static void mlxsw_sp_fid_dummy_deconfigure(struct mlxsw_sp_fid *fid)
+{
+	mlxsw_sp_fid_op(fid->fid_family->mlxsw_sp, fid->fid_index, 0, false);
+}
+
+static int mlxsw_sp_fid_dummy_index_alloc(struct mlxsw_sp_fid *fid,
+					  const void *arg, u16 *p_fid_index)
+{
+	*p_fid_index = fid->fid_family->start_index;
+
+	return 0;
+}
+
+static bool mlxsw_sp_fid_dummy_compare(const struct mlxsw_sp_fid *fid,
+				       const void *arg)
+{
+	return true;
+}
+
+static const struct mlxsw_sp_fid_ops mlxsw_sp_fid_dummy_ops = {
+	.configure		= mlxsw_sp_fid_dummy_configure,
+	.deconfigure		= mlxsw_sp_fid_dummy_deconfigure,
+	.index_alloc		= mlxsw_sp_fid_dummy_index_alloc,
+	.compare		= mlxsw_sp_fid_dummy_compare,
+};
+
+static const struct mlxsw_sp_fid_family mlxsw_sp_fid_dummy_family = {
+	.type			= MLXSW_SP_FID_TYPE_DUMMY,
+	.fid_size		= sizeof(struct mlxsw_sp_fid),
+	.start_index		= MLXSW_SP_RFID_BASE - 1,
+	.end_index		= MLXSW_SP_RFID_BASE - 1,
+	.ops			= &mlxsw_sp_fid_dummy_ops,
+};
+
+static const struct mlxsw_sp_fid_family *mlxsw_sp_fid_family_arr[] = {
+	[MLXSW_SP_FID_TYPE_8021Q]	= &mlxsw_sp_fid_8021q_family,
+	[MLXSW_SP_FID_TYPE_8021D]	= &mlxsw_sp_fid_8021d_family,
+	[MLXSW_SP_FID_TYPE_RFID]	= &mlxsw_sp_fid_rfid_family,
+	[MLXSW_SP_FID_TYPE_DUMMY]	= &mlxsw_sp_fid_dummy_family,
+};
+
+static struct mlxsw_sp_fid *mlxsw_sp_fid_get(struct mlxsw_sp *mlxsw_sp,
+					     enum mlxsw_sp_fid_type type,
+					     const void *arg)
+{
+	struct mlxsw_sp_fid_family *fid_family;
+	struct mlxsw_sp_fid *fid;
+	u16 fid_index;
+	int err;
+
+	fid_family = mlxsw_sp->fid_core->fid_family_arr[type];
+	list_for_each_entry(fid, &fid_family->fids_list, list) {
+		if (!fid->fid_family->ops->compare(fid, arg))
+			continue;
+		fid->ref_count++;
+		return fid;
+	}
+
+	fid = kzalloc(fid_family->fid_size, GFP_KERNEL);
+	if (!fid)
+		return ERR_PTR(-ENOMEM);
+	fid->fid_family = fid_family;
+
+	err = fid->fid_family->ops->index_alloc(fid, arg, &fid_index);
+	if (err)
+		goto err_index_alloc;
+	fid->fid_index = fid_index;
+	__set_bit(fid_index - fid_family->start_index, fid_family->fids_bitmap);
+
+	if (fid->fid_family->ops->setup)
+		fid->fid_family->ops->setup(fid, arg);
+
+	err = fid->fid_family->ops->configure(fid);
+	if (err)
+		goto err_configure;
+
+	list_add(&fid->list, &fid_family->fids_list);
+	fid->ref_count++;
+	return fid;
+
+err_configure:
+	__clear_bit(fid_index - fid_family->start_index,
+		    fid_family->fids_bitmap);
+err_index_alloc:
+	kfree(fid);
+	return ERR_PTR(err);
+}
+
+void mlxsw_sp_fid_put(struct mlxsw_sp_fid *fid)
+{
+	struct mlxsw_sp_fid_family *fid_family = fid->fid_family;
+
+	if (--fid->ref_count == 1 && fid->rif) {
+		/* Destroy the associated RIF and let it drop the last
+		 * reference on the FID.
+		 */
+		return mlxsw_sp_rif_destroy(fid->rif);
+	} else if (fid->ref_count == 0) {
+		list_del(&fid->list);
+		fid->fid_family->ops->deconfigure(fid);
+		__clear_bit(fid->fid_index - fid_family->start_index,
+			    fid_family->fids_bitmap);
+		kfree(fid);
+	}
+}
+
+struct mlxsw_sp_fid *mlxsw_sp_fid_8021q_get(struct mlxsw_sp *mlxsw_sp, u16 vid)
+{
+	return mlxsw_sp_fid_get(mlxsw_sp, MLXSW_SP_FID_TYPE_8021Q, &vid);
+}
+
+struct mlxsw_sp_fid *mlxsw_sp_fid_8021d_get(struct mlxsw_sp *mlxsw_sp,
+					    int br_ifindex)
+{
+	return mlxsw_sp_fid_get(mlxsw_sp, MLXSW_SP_FID_TYPE_8021D, &br_ifindex);
+}
+
+struct mlxsw_sp_fid *mlxsw_sp_fid_rfid_get(struct mlxsw_sp *mlxsw_sp,
+					   u16 rif_index)
+{
+	return mlxsw_sp_fid_get(mlxsw_sp, MLXSW_SP_FID_TYPE_RFID, &rif_index);
+}
+
+struct mlxsw_sp_fid *mlxsw_sp_fid_dummy_get(struct mlxsw_sp *mlxsw_sp)
+{
+	return mlxsw_sp_fid_get(mlxsw_sp, MLXSW_SP_FID_TYPE_DUMMY, NULL);
+}
+
+static int
+mlxsw_sp_fid_flood_table_init(struct mlxsw_sp_fid_family *fid_family,
+			      const struct mlxsw_sp_flood_table *flood_table)
+{
+	enum mlxsw_sp_flood_type packet_type = flood_table->packet_type;
+	const int *sfgc_packet_types;
+	int i;
+
+	sfgc_packet_types = mlxsw_sp_packet_type_sfgc_types[packet_type];
+	for (i = 0; i < MLXSW_REG_SFGC_TYPE_MAX; i++) {
+		struct mlxsw_sp *mlxsw_sp = fid_family->mlxsw_sp;
+		char sfgc_pl[MLXSW_REG_SFGC_LEN];
+		int err;
+
+		if (!sfgc_packet_types[i])
+			continue;
+		mlxsw_reg_sfgc_pack(sfgc_pl, i, flood_table->bridge_type,
+				    flood_table->table_type,
+				    flood_table->table_index);
+		err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfgc), sfgc_pl);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static int
+mlxsw_sp_fid_flood_tables_init(struct mlxsw_sp_fid_family *fid_family)
+{
+	int i;
+
+	for (i = 0; i < fid_family->nr_flood_tables; i++) {
+		const struct mlxsw_sp_flood_table *flood_table;
+		int err;
+
+		flood_table = &fid_family->flood_tables[i];
+		err = mlxsw_sp_fid_flood_table_init(fid_family, flood_table);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static int mlxsw_sp_fid_family_register(struct mlxsw_sp *mlxsw_sp,
+					const struct mlxsw_sp_fid_family *tmpl)
+{
+	u16 nr_fids = tmpl->end_index - tmpl->start_index + 1;
+	struct mlxsw_sp_fid_family *fid_family;
+	int err;
+
+	fid_family = kmemdup(tmpl, sizeof(*fid_family), GFP_KERNEL);
+	if (!fid_family)
+		return -ENOMEM;
+
+	fid_family->mlxsw_sp = mlxsw_sp;
+	INIT_LIST_HEAD(&fid_family->fids_list);
+	fid_family->fids_bitmap = kcalloc(BITS_TO_LONGS(nr_fids),
+					  sizeof(unsigned long), GFP_KERNEL);
+	if (!fid_family->fids_bitmap) {
+		err = -ENOMEM;
+		goto err_alloc_fids_bitmap;
+	}
+
+	if (fid_family->flood_tables) {
+		err = mlxsw_sp_fid_flood_tables_init(fid_family);
+		if (err)
+			goto err_fid_flood_tables_init;
+	}
+
+	mlxsw_sp->fid_core->fid_family_arr[tmpl->type] = fid_family;
+
+	return 0;
+
+err_fid_flood_tables_init:
+	kfree(fid_family->fids_bitmap);
+err_alloc_fids_bitmap:
+	kfree(fid_family);
+	return err;
+}
+
+static void
+mlxsw_sp_fid_family_unregister(struct mlxsw_sp *mlxsw_sp,
+			       struct mlxsw_sp_fid_family *fid_family)
+{
+	mlxsw_sp->fid_core->fid_family_arr[fid_family->type] = NULL;
+	kfree(fid_family->fids_bitmap);
+	WARN_ON_ONCE(!list_empty(&fid_family->fids_list));
+	kfree(fid_family);
+}
+
+int mlxsw_sp_port_fids_init(struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+
+	/* Track number of FIDs configured on the port with mapping type
+	 * PORT_VID_TO_FID, so that we know when to transition the port
+	 * back to non-virtual (VLAN) mode.
+	 */
+	mlxsw_sp->fid_core->port_fid_mappings[mlxsw_sp_port->local_port] = 0;
+
+	return mlxsw_sp_port_vp_mode_set(mlxsw_sp_port, false);
+}
+
+void mlxsw_sp_port_fids_fini(struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+
+	mlxsw_sp->fid_core->port_fid_mappings[mlxsw_sp_port->local_port] = 0;
+}
+
+int mlxsw_sp_fids_init(struct mlxsw_sp *mlxsw_sp)
+{
+	unsigned int max_ports = mlxsw_core_max_ports(mlxsw_sp->core);
+	struct mlxsw_sp_fid_core *fid_core;
+	int err, i;
+
+	fid_core = kzalloc(sizeof(*mlxsw_sp->fid_core), GFP_KERNEL);
+	if (!fid_core)
+		return -ENOMEM;
+	mlxsw_sp->fid_core = fid_core;
+
+	fid_core->port_fid_mappings = kcalloc(max_ports, sizeof(unsigned int),
+					      GFP_KERNEL);
+	if (!fid_core->port_fid_mappings) {
+		err = -ENOMEM;
+		goto err_alloc_port_fid_mappings;
+	}
+
+	for (i = 0; i < MLXSW_SP_FID_TYPE_MAX; i++) {
+		err = mlxsw_sp_fid_family_register(mlxsw_sp,
+						   mlxsw_sp_fid_family_arr[i]);
+
+		if (err)
+			goto err_fid_ops_register;
+	}
+
+	return 0;
+
+err_fid_ops_register:
+	for (i--; i >= 0; i--) {
+		struct mlxsw_sp_fid_family *fid_family;
+
+		fid_family = fid_core->fid_family_arr[i];
+		mlxsw_sp_fid_family_unregister(mlxsw_sp, fid_family);
+	}
+	kfree(fid_core->port_fid_mappings);
+err_alloc_port_fid_mappings:
+	kfree(fid_core);
+	return err;
+}
+
+void mlxsw_sp_fids_fini(struct mlxsw_sp *mlxsw_sp)
+{
+	struct mlxsw_sp_fid_core *fid_core = mlxsw_sp->fid_core;
+	int i;
+
+	for (i = 0; i < MLXSW_SP_FID_TYPE_MAX; i++)
+		mlxsw_sp_fid_family_unregister(mlxsw_sp,
+					       fid_core->fid_family_arr[i]);
+	kfree(fid_core->port_fid_mappings);
+	kfree(fid_core);
+}
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_flower.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_flower.c
index 7d87e23578a3..21bb2bf62d3e 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_flower.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_flower.c
@@ -67,12 +67,20 @@ static int mlxsw_sp_flower_parse_actions(struct mlxsw_sp *mlxsw_sp,
 			err = mlxsw_sp_acl_rulei_act_drop(rulei);
 			if (err)
 				return err;
+		} else if (is_tcf_gact_trap(a)) {
+			err = mlxsw_sp_acl_rulei_act_trap(rulei);
+			if (err)
+				return err;
 		} else if (is_tcf_mirred_egress_redirect(a)) {
 			int ifindex = tcf_mirred_ifindex(a);
 			struct net_device *out_dev;
+			struct mlxsw_sp_fid *fid;
+			u16 fid_index;
 
+			fid = mlxsw_sp_acl_dummy_fid(mlxsw_sp);
+			fid_index = mlxsw_sp_fid_index(fid);
 			err = mlxsw_sp_acl_rulei_act_fid_set(mlxsw_sp, rulei,
-							     MLXSW_SP_DUMMY_FID);
+							     fid_index);
 			if (err)
 				return err;
 
@@ -178,6 +186,32 @@ static int mlxsw_sp_flower_parse_ports(struct mlxsw_sp *mlxsw_sp,
 	return 0;
 }
 
+static int mlxsw_sp_flower_parse_tcp(struct mlxsw_sp *mlxsw_sp,
+				     struct mlxsw_sp_acl_rule_info *rulei,
+				     struct tc_cls_flower_offload *f,
+				     u8 ip_proto)
+{
+	struct flow_dissector_key_tcp *key, *mask;
+
+	if (!dissector_uses_key(f->dissector, FLOW_DISSECTOR_KEY_TCP))
+		return 0;
+
+	if (ip_proto != IPPROTO_TCP) {
+		dev_err(mlxsw_sp->bus_info->dev, "TCP keys supported only for TCP\n");
+		return -EINVAL;
+	}
+
+	key = skb_flow_dissector_target(f->dissector,
+					FLOW_DISSECTOR_KEY_TCP,
+					f->key);
+	mask = skb_flow_dissector_target(f->dissector,
+					 FLOW_DISSECTOR_KEY_TCP,
+					 f->mask);
+	mlxsw_sp_acl_rulei_keymask_u32(rulei, MLXSW_AFK_ELEMENT_TCP_FLAGS,
+				       ntohs(key->flags), ntohs(mask->flags));
+	return 0;
+}
+
 static int mlxsw_sp_flower_parse(struct mlxsw_sp *mlxsw_sp,
 				 struct net_device *dev,
 				 struct mlxsw_sp_acl_rule_info *rulei,
@@ -194,6 +228,7 @@ static int mlxsw_sp_flower_parse(struct mlxsw_sp *mlxsw_sp,
 	      BIT(FLOW_DISSECTOR_KEY_IPV4_ADDRS) |
 	      BIT(FLOW_DISSECTOR_KEY_IPV6_ADDRS) |
 	      BIT(FLOW_DISSECTOR_KEY_PORTS) |
+	      BIT(FLOW_DISSECTOR_KEY_TCP) |
 	      BIT(FLOW_DISSECTOR_KEY_VLAN))) {
 		dev_err(mlxsw_sp->bus_info->dev, "Unsupported key\n");
 		return -EOPNOTSUPP;
@@ -285,6 +320,9 @@ static int mlxsw_sp_flower_parse(struct mlxsw_sp *mlxsw_sp,
 	err = mlxsw_sp_flower_parse_ports(mlxsw_sp, rulei, f, ip_proto);
 	if (err)
 		return err;
+	err = mlxsw_sp_flower_parse_tcp(mlxsw_sp, rulei, f, ip_proto);
+	if (err)
+		return err;
 
 	return mlxsw_sp_flower_parse_actions(mlxsw_sp, dev, rulei, f->exts);
 }
@@ -363,8 +401,6 @@ int mlxsw_sp_flower_stats(struct mlxsw_sp_port *mlxsw_sp_port, bool ingress,
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	struct mlxsw_sp_acl_ruleset *ruleset;
 	struct mlxsw_sp_acl_rule *rule;
-	struct tc_action *a;
-	LIST_HEAD(actions);
 	u64 packets;
 	u64 lastuse;
 	u64 bytes;
@@ -385,13 +421,7 @@ int mlxsw_sp_flower_stats(struct mlxsw_sp_port *mlxsw_sp_port, bool ingress,
 	if (err)
 		goto err_rule_get_stats;
 
-	preempt_disable();
-
-	tcf_exts_to_list(f->exts, &actions);
-	list_for_each_entry(a, &actions, list)
-		tcf_action_stats_update(a, bytes, packets, lastuse);
-
-	preempt_enable();
+	tcf_exts_stats_update(f->exts, bytes, packets, lastuse);
 
 	mlxsw_sp_acl_ruleset_put(mlxsw_sp, ruleset);
 	return 0;
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_router.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_router.c
index 9f89c4137d21..192cb93e7669 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_router.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_router.c
@@ -42,6 +42,7 @@
 #include <linux/notifier.h>
 #include <linux/inetdevice.h>
 #include <linux/netdevice.h>
+#include <linux/if_bridge.h>
 #include <net/netevent.h>
 #include <net/neighbour.h>
 #include <net/arp.h>
@@ -56,21 +57,82 @@
 #include "spectrum_dpipe.h"
 #include "spectrum_router.h"
 
+struct mlxsw_sp_vr;
+struct mlxsw_sp_lpm_tree;
+struct mlxsw_sp_rif_ops;
+
+struct mlxsw_sp_router {
+	struct mlxsw_sp *mlxsw_sp;
+	struct mlxsw_sp_rif **rifs;
+	struct mlxsw_sp_vr *vrs;
+	struct rhashtable neigh_ht;
+	struct rhashtable nexthop_group_ht;
+	struct rhashtable nexthop_ht;
+	struct {
+		struct mlxsw_sp_lpm_tree *trees;
+		unsigned int tree_count;
+	} lpm;
+	struct {
+		struct delayed_work dw;
+		unsigned long interval;	/* ms */
+	} neighs_update;
+	struct delayed_work nexthop_probe_dw;
+#define MLXSW_SP_UNRESOLVED_NH_PROBE_INTERVAL 5000 /* ms */
+	struct list_head nexthop_neighs_list;
+	bool aborted;
+	struct notifier_block fib_nb;
+	const struct mlxsw_sp_rif_ops **rif_ops_arr;
+};
+
 struct mlxsw_sp_rif {
 	struct list_head nexthop_list;
 	struct list_head neigh_list;
 	struct net_device *dev;
-	struct mlxsw_sp_fid *f;
+	struct mlxsw_sp_fid *fid;
 	unsigned char addr[ETH_ALEN];
 	int mtu;
 	u16 rif_index;
 	u16 vr_id;
+	const struct mlxsw_sp_rif_ops *ops;
+	struct mlxsw_sp *mlxsw_sp;
+
 	unsigned int counter_ingress;
 	bool counter_ingress_valid;
 	unsigned int counter_egress;
 	bool counter_egress_valid;
 };
 
+struct mlxsw_sp_rif_params {
+	struct net_device *dev;
+	union {
+		u16 system_port;
+		u16 lag_id;
+	};
+	u16 vid;
+	bool lag;
+};
+
+struct mlxsw_sp_rif_subport {
+	struct mlxsw_sp_rif common;
+	union {
+		u16 system_port;
+		u16 lag_id;
+	};
+	u16 vid;
+	bool lag;
+};
+
+struct mlxsw_sp_rif_ops {
+	enum mlxsw_sp_rif_type type;
+	size_t rif_size;
+
+	void (*setup)(struct mlxsw_sp_rif *rif,
+		      const struct mlxsw_sp_rif_params *params);
+	int (*configure)(struct mlxsw_sp_rif *rif);
+	void (*deconfigure)(struct mlxsw_sp_rif *rif);
+	struct mlxsw_sp_fid * (*fid_get)(struct mlxsw_sp_rif *rif);
+};
+
 static unsigned int *
 mlxsw_sp_rif_p_counter_get(struct mlxsw_sp_rif *rif,
 			   enum mlxsw_sp_rif_counter_dir dir)
@@ -219,10 +281,35 @@ void mlxsw_sp_rif_counter_free(struct mlxsw_sp *mlxsw_sp,
 	mlxsw_sp_rif_counter_valid_set(rif, dir, false);
 }
 
+static void mlxsw_sp_rif_counters_alloc(struct mlxsw_sp_rif *rif)
+{
+	struct mlxsw_sp *mlxsw_sp = rif->mlxsw_sp;
+	struct devlink *devlink;
+
+	devlink = priv_to_devlink(mlxsw_sp->core);
+	if (!devlink_dpipe_table_counter_enabled(devlink,
+						 MLXSW_SP_DPIPE_TABLE_NAME_ERIF))
+		return;
+	mlxsw_sp_rif_counter_alloc(mlxsw_sp, rif, MLXSW_SP_RIF_COUNTER_EGRESS);
+}
+
+static void mlxsw_sp_rif_counters_free(struct mlxsw_sp_rif *rif)
+{
+	struct mlxsw_sp *mlxsw_sp = rif->mlxsw_sp;
+
+	mlxsw_sp_rif_counter_free(mlxsw_sp, rif, MLXSW_SP_RIF_COUNTER_EGRESS);
+}
+
 static struct mlxsw_sp_rif *
 mlxsw_sp_rif_find_by_dev(const struct mlxsw_sp *mlxsw_sp,
 			 const struct net_device *dev);
 
+#define MLXSW_SP_PREFIX_COUNT (sizeof(struct in6_addr) * BITS_PER_BYTE)
+
+struct mlxsw_sp_prefix_usage {
+	DECLARE_BITMAP(b, MLXSW_SP_PREFIX_COUNT);
+};
+
 #define mlxsw_sp_prefix_usage_for_each(prefix, prefix_usage) \
 	for_each_set_bit(prefix, (prefix_usage)->b, MLXSW_SP_PREFIX_COUNT)
 
@@ -287,6 +374,7 @@ enum mlxsw_sp_fib_entry_type {
 };
 
 struct mlxsw_sp_nexthop_group;
+struct mlxsw_sp_fib;
 
 struct mlxsw_sp_fib_node {
 	struct list_head entry_list;
@@ -313,6 +401,18 @@ struct mlxsw_sp_fib_entry {
 	bool offloaded;
 };
 
+enum mlxsw_sp_l3proto {
+	MLXSW_SP_L3_PROTO_IPV4,
+	MLXSW_SP_L3_PROTO_IPV6,
+};
+
+struct mlxsw_sp_lpm_tree {
+	u8 id; /* tree ID */
+	unsigned int ref_count;
+	enum mlxsw_sp_l3proto proto;
+	struct mlxsw_sp_prefix_usage prefix_usage;
+};
+
 struct mlxsw_sp_fib {
 	struct rhashtable ht;
 	struct list_head node_list;
@@ -323,6 +423,13 @@ struct mlxsw_sp_fib {
 	enum mlxsw_sp_l3proto proto;
 };
 
+struct mlxsw_sp_vr {
+	u16 id; /* virtual router ID */
+	u32 tb_id; /* kernel fib table id */
+	unsigned int rif_count;
+	struct mlxsw_sp_fib *fib4;
+};
+
 static const struct rhashtable_params mlxsw_sp_fib_ht_params;
 
 static struct mlxsw_sp_fib *mlxsw_sp_fib_create(struct mlxsw_sp_vr *vr,
@@ -361,8 +468,8 @@ mlxsw_sp_lpm_tree_find_unused(struct mlxsw_sp *mlxsw_sp)
 	static struct mlxsw_sp_lpm_tree *lpm_tree;
 	int i;
 
-	for (i = 0; i < mlxsw_sp->router.lpm.tree_count; i++) {
-		lpm_tree = &mlxsw_sp->router.lpm.trees[i];
+	for (i = 0; i < mlxsw_sp->router->lpm.tree_count; i++) {
+		lpm_tree = &mlxsw_sp->router->lpm.trees[i];
 		if (lpm_tree->ref_count == 0)
 			return lpm_tree;
 	}
@@ -458,8 +565,8 @@ mlxsw_sp_lpm_tree_get(struct mlxsw_sp *mlxsw_sp,
 	struct mlxsw_sp_lpm_tree *lpm_tree;
 	int i;
 
-	for (i = 0; i < mlxsw_sp->router.lpm.tree_count; i++) {
-		lpm_tree = &mlxsw_sp->router.lpm.trees[i];
+	for (i = 0; i < mlxsw_sp->router->lpm.tree_count; i++) {
+		lpm_tree = &mlxsw_sp->router->lpm.trees[i];
 		if (lpm_tree->ref_count != 0 &&
 		    lpm_tree->proto == proto &&
 		    mlxsw_sp_prefix_usage_eq(&lpm_tree->prefix_usage,
@@ -484,7 +591,7 @@ static int mlxsw_sp_lpm_tree_put(struct mlxsw_sp *mlxsw_sp,
 	return 0;
 }
 
-#define MLXSW_SP_LPM_TREE_MIN 2 /* trees 0 and 1 are reserved */
+#define MLXSW_SP_LPM_TREE_MIN 1 /* tree 0 is reserved */
 
 static int mlxsw_sp_lpm_init(struct mlxsw_sp *mlxsw_sp)
 {
@@ -496,15 +603,15 @@ static int mlxsw_sp_lpm_init(struct mlxsw_sp *mlxsw_sp)
 		return -EIO;
 
 	max_trees = MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_LPM_TREES);
-	mlxsw_sp->router.lpm.tree_count = max_trees - MLXSW_SP_LPM_TREE_MIN;
-	mlxsw_sp->router.lpm.trees = kcalloc(mlxsw_sp->router.lpm.tree_count,
+	mlxsw_sp->router->lpm.tree_count = max_trees - MLXSW_SP_LPM_TREE_MIN;
+	mlxsw_sp->router->lpm.trees = kcalloc(mlxsw_sp->router->lpm.tree_count,
 					     sizeof(struct mlxsw_sp_lpm_tree),
 					     GFP_KERNEL);
-	if (!mlxsw_sp->router.lpm.trees)
+	if (!mlxsw_sp->router->lpm.trees)
 		return -ENOMEM;
 
-	for (i = 0; i < mlxsw_sp->router.lpm.tree_count; i++) {
-		lpm_tree = &mlxsw_sp->router.lpm.trees[i];
+	for (i = 0; i < mlxsw_sp->router->lpm.tree_count; i++) {
+		lpm_tree = &mlxsw_sp->router->lpm.trees[i];
 		lpm_tree->id = i + MLXSW_SP_LPM_TREE_MIN;
 	}
 
@@ -513,7 +620,7 @@ static int mlxsw_sp_lpm_init(struct mlxsw_sp *mlxsw_sp)
 
 static void mlxsw_sp_lpm_fini(struct mlxsw_sp *mlxsw_sp)
 {
-	kfree(mlxsw_sp->router.lpm.trees);
+	kfree(mlxsw_sp->router->lpm.trees);
 }
 
 static bool mlxsw_sp_vr_is_used(const struct mlxsw_sp_vr *vr)
@@ -527,7 +634,7 @@ static struct mlxsw_sp_vr *mlxsw_sp_vr_find_unused(struct mlxsw_sp *mlxsw_sp)
 	int i;
 
 	for (i = 0; i < MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_VRS); i++) {
-		vr = &mlxsw_sp->router.vrs[i];
+		vr = &mlxsw_sp->router->vrs[i];
 		if (!mlxsw_sp_vr_is_used(vr))
 			return vr;
 	}
@@ -573,7 +680,7 @@ static struct mlxsw_sp_vr *mlxsw_sp_vr_find(struct mlxsw_sp *mlxsw_sp,
 	tb_id = mlxsw_sp_fix_tb_id(tb_id);
 
 	for (i = 0; i < MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_VRS); i++) {
-		vr = &mlxsw_sp->router.vrs[i];
+		vr = &mlxsw_sp->router->vrs[i];
 		if (mlxsw_sp_vr_is_used(vr) && vr->tb_id == tb_id)
 			return vr;
 	}
@@ -680,13 +787,13 @@ static int mlxsw_sp_vrs_init(struct mlxsw_sp *mlxsw_sp)
 		return -EIO;
 
 	max_vrs = MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_VRS);
-	mlxsw_sp->router.vrs = kcalloc(max_vrs, sizeof(struct mlxsw_sp_vr),
-				       GFP_KERNEL);
-	if (!mlxsw_sp->router.vrs)
+	mlxsw_sp->router->vrs = kcalloc(max_vrs, sizeof(struct mlxsw_sp_vr),
+					GFP_KERNEL);
+	if (!mlxsw_sp->router->vrs)
 		return -ENOMEM;
 
 	for (i = 0; i < max_vrs; i++) {
-		vr = &mlxsw_sp->router.vrs[i];
+		vr = &mlxsw_sp->router->vrs[i];
 		vr->id = i;
 	}
 
@@ -706,7 +813,7 @@ static void mlxsw_sp_vrs_fini(struct mlxsw_sp *mlxsw_sp)
 	 */
 	mlxsw_core_flush_owq();
 	mlxsw_sp_router_fib_flush(mlxsw_sp);
-	kfree(mlxsw_sp->router.vrs);
+	kfree(mlxsw_sp->router->vrs);
 }
 
 struct mlxsw_sp_neigh_key {
@@ -758,7 +865,7 @@ static int
 mlxsw_sp_neigh_entry_insert(struct mlxsw_sp *mlxsw_sp,
 			    struct mlxsw_sp_neigh_entry *neigh_entry)
 {
-	return rhashtable_insert_fast(&mlxsw_sp->router.neigh_ht,
+	return rhashtable_insert_fast(&mlxsw_sp->router->neigh_ht,
 				      &neigh_entry->ht_node,
 				      mlxsw_sp_neigh_ht_params);
 }
@@ -767,7 +874,7 @@ static void
 mlxsw_sp_neigh_entry_remove(struct mlxsw_sp *mlxsw_sp,
 			    struct mlxsw_sp_neigh_entry *neigh_entry)
 {
-	rhashtable_remove_fast(&mlxsw_sp->router.neigh_ht,
+	rhashtable_remove_fast(&mlxsw_sp->router->neigh_ht,
 			       &neigh_entry->ht_node,
 			       mlxsw_sp_neigh_ht_params);
 }
@@ -815,7 +922,7 @@ mlxsw_sp_neigh_entry_lookup(struct mlxsw_sp *mlxsw_sp, struct neighbour *n)
 	struct mlxsw_sp_neigh_key key;
 
 	key.n = n;
-	return rhashtable_lookup_fast(&mlxsw_sp->router.neigh_ht,
+	return rhashtable_lookup_fast(&mlxsw_sp->router->neigh_ht,
 				      &key, mlxsw_sp_neigh_ht_params);
 }
 
@@ -824,7 +931,7 @@ mlxsw_sp_router_neighs_update_interval_init(struct mlxsw_sp *mlxsw_sp)
 {
 	unsigned long interval = NEIGH_VAR(&arp_tbl.parms, DELAY_PROBE_TIME);
 
-	mlxsw_sp->router.neighs_update.interval = jiffies_to_msecs(interval);
+	mlxsw_sp->router->neighs_update.interval = jiffies_to_msecs(interval);
 }
 
 static void mlxsw_sp_router_neigh_ent_ipv4_process(struct mlxsw_sp *mlxsw_sp,
@@ -839,13 +946,13 @@ static void mlxsw_sp_router_neigh_ent_ipv4_process(struct mlxsw_sp *mlxsw_sp,
 
 	mlxsw_reg_rauhtd_ent_ipv4_unpack(rauhtd_pl, ent_index, &rif, &dip);
 
-	if (!mlxsw_sp->rifs[rif]) {
+	if (!mlxsw_sp->router->rifs[rif]) {
 		dev_err_ratelimited(mlxsw_sp->bus_info->dev, "Incorrect RIF in neighbour entry\n");
 		return;
 	}
 
 	dipn = htonl(dip);
-	dev = mlxsw_sp->rifs[rif]->dev;
+	dev = mlxsw_sp->router->rifs[rif]->dev;
 	n = neigh_lookup(&arp_tbl, &dipn, dev);
 	if (!n) {
 		netdev_err(dev, "Failed to find matching neighbour for IP=%pI4h\n",
@@ -954,7 +1061,7 @@ static void mlxsw_sp_router_neighs_update_nh(struct mlxsw_sp *mlxsw_sp)
 
 	/* Take RTNL mutex here to prevent lists from changes */
 	rtnl_lock();
-	list_for_each_entry(neigh_entry, &mlxsw_sp->router.nexthop_neighs_list,
+	list_for_each_entry(neigh_entry, &mlxsw_sp->router->nexthop_neighs_list,
 			    nexthop_neighs_list_node)
 		/* If this neigh have nexthops, make the kernel think this neigh
 		 * is active regardless of the traffic.
@@ -966,33 +1073,35 @@ static void mlxsw_sp_router_neighs_update_nh(struct mlxsw_sp *mlxsw_sp)
 static void
 mlxsw_sp_router_neighs_update_work_schedule(struct mlxsw_sp *mlxsw_sp)
 {
-	unsigned long interval = mlxsw_sp->router.neighs_update.interval;
+	unsigned long interval = mlxsw_sp->router->neighs_update.interval;
 
-	mlxsw_core_schedule_dw(&mlxsw_sp->router.neighs_update.dw,
+	mlxsw_core_schedule_dw(&mlxsw_sp->router->neighs_update.dw,
 			       msecs_to_jiffies(interval));
 }
 
 static void mlxsw_sp_router_neighs_update_work(struct work_struct *work)
 {
-	struct mlxsw_sp *mlxsw_sp = container_of(work, struct mlxsw_sp,
-						 router.neighs_update.dw.work);
+	struct mlxsw_sp_router *router;
 	int err;
 
-	err = mlxsw_sp_router_neighs_update_rauhtd(mlxsw_sp);
+	router = container_of(work, struct mlxsw_sp_router,
+			      neighs_update.dw.work);
+	err = mlxsw_sp_router_neighs_update_rauhtd(router->mlxsw_sp);
 	if (err)
-		dev_err(mlxsw_sp->bus_info->dev, "Could not update kernel for neigh activity");
+		dev_err(router->mlxsw_sp->bus_info->dev, "Could not update kernel for neigh activity");
 
-	mlxsw_sp_router_neighs_update_nh(mlxsw_sp);
+	mlxsw_sp_router_neighs_update_nh(router->mlxsw_sp);
 
-	mlxsw_sp_router_neighs_update_work_schedule(mlxsw_sp);
+	mlxsw_sp_router_neighs_update_work_schedule(router->mlxsw_sp);
 }
 
 static void mlxsw_sp_router_probe_unresolved_nexthops(struct work_struct *work)
 {
 	struct mlxsw_sp_neigh_entry *neigh_entry;
-	struct mlxsw_sp *mlxsw_sp = container_of(work, struct mlxsw_sp,
-						 router.nexthop_probe_dw.work);
+	struct mlxsw_sp_router *router;
 
+	router = container_of(work, struct mlxsw_sp_router,
+			      nexthop_probe_dw.work);
 	/* Iterate over nexthop neighbours, find those who are unresolved and
 	 * send arp on them. This solves the chicken-egg problem when
 	 * the nexthop wouldn't get offloaded until the neighbor is resolved
@@ -1002,13 +1111,13 @@ static void mlxsw_sp_router_probe_unresolved_nexthops(struct work_struct *work)
 	 * Take RTNL mutex here to prevent lists from changes.
 	 */
 	rtnl_lock();
-	list_for_each_entry(neigh_entry, &mlxsw_sp->router.nexthop_neighs_list,
+	list_for_each_entry(neigh_entry, &router->nexthop_neighs_list,
 			    nexthop_neighs_list_node)
 		if (!neigh_entry->connected)
 			neigh_event_send(neigh_entry->key.n, NULL);
 	rtnl_unlock();
 
-	mlxsw_core_schedule_dw(&mlxsw_sp->router.nexthop_probe_dw,
+	mlxsw_core_schedule_dw(&router->nexthop_probe_dw,
 			       MLXSW_SP_UNRESOLVED_NH_PROBE_INTERVAL);
 }
 
@@ -1130,7 +1239,7 @@ int mlxsw_sp_router_netevent_event(struct notifier_block *unused,
 
 		mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 		interval = jiffies_to_msecs(NEIGH_VAR(p, DELAY_PROBE_TIME));
-		mlxsw_sp->router.neighs_update.interval = interval;
+		mlxsw_sp->router->neighs_update.interval = interval;
 
 		mlxsw_sp_port_dev_put(mlxsw_sp_port);
 		break;
@@ -1171,7 +1280,7 @@ static int mlxsw_sp_neigh_init(struct mlxsw_sp *mlxsw_sp)
 {
 	int err;
 
-	err = rhashtable_init(&mlxsw_sp->router.neigh_ht,
+	err = rhashtable_init(&mlxsw_sp->router->neigh_ht,
 			      &mlxsw_sp_neigh_ht_params);
 	if (err)
 		return err;
@@ -1182,20 +1291,20 @@ static int mlxsw_sp_neigh_init(struct mlxsw_sp *mlxsw_sp)
 	mlxsw_sp_router_neighs_update_interval_init(mlxsw_sp);
 
 	/* Create the delayed works for the activity_update */
-	INIT_DELAYED_WORK(&mlxsw_sp->router.neighs_update.dw,
+	INIT_DELAYED_WORK(&mlxsw_sp->router->neighs_update.dw,
 			  mlxsw_sp_router_neighs_update_work);
-	INIT_DELAYED_WORK(&mlxsw_sp->router.nexthop_probe_dw,
+	INIT_DELAYED_WORK(&mlxsw_sp->router->nexthop_probe_dw,
 			  mlxsw_sp_router_probe_unresolved_nexthops);
-	mlxsw_core_schedule_dw(&mlxsw_sp->router.neighs_update.dw, 0);
-	mlxsw_core_schedule_dw(&mlxsw_sp->router.nexthop_probe_dw, 0);
+	mlxsw_core_schedule_dw(&mlxsw_sp->router->neighs_update.dw, 0);
+	mlxsw_core_schedule_dw(&mlxsw_sp->router->nexthop_probe_dw, 0);
 	return 0;
 }
 
 static void mlxsw_sp_neigh_fini(struct mlxsw_sp *mlxsw_sp)
 {
-	cancel_delayed_work_sync(&mlxsw_sp->router.neighs_update.dw);
-	cancel_delayed_work_sync(&mlxsw_sp->router.nexthop_probe_dw);
-	rhashtable_destroy(&mlxsw_sp->router.neigh_ht);
+	cancel_delayed_work_sync(&mlxsw_sp->router->neighs_update.dw);
+	cancel_delayed_work_sync(&mlxsw_sp->router->nexthop_probe_dw);
+	rhashtable_destroy(&mlxsw_sp->router->neigh_ht);
 }
 
 static int mlxsw_sp_neigh_rif_flush(struct mlxsw_sp *mlxsw_sp,
@@ -1270,7 +1379,7 @@ static const struct rhashtable_params mlxsw_sp_nexthop_group_ht_params = {
 static int mlxsw_sp_nexthop_group_insert(struct mlxsw_sp *mlxsw_sp,
 					 struct mlxsw_sp_nexthop_group *nh_grp)
 {
-	return rhashtable_insert_fast(&mlxsw_sp->router.nexthop_group_ht,
+	return rhashtable_insert_fast(&mlxsw_sp->router->nexthop_group_ht,
 				      &nh_grp->ht_node,
 				      mlxsw_sp_nexthop_group_ht_params);
 }
@@ -1278,7 +1387,7 @@ static int mlxsw_sp_nexthop_group_insert(struct mlxsw_sp *mlxsw_sp,
 static void mlxsw_sp_nexthop_group_remove(struct mlxsw_sp *mlxsw_sp,
 					  struct mlxsw_sp_nexthop_group *nh_grp)
 {
-	rhashtable_remove_fast(&mlxsw_sp->router.nexthop_group_ht,
+	rhashtable_remove_fast(&mlxsw_sp->router->nexthop_group_ht,
 			       &nh_grp->ht_node,
 			       mlxsw_sp_nexthop_group_ht_params);
 }
@@ -1287,7 +1396,7 @@ static struct mlxsw_sp_nexthop_group *
 mlxsw_sp_nexthop_group_lookup(struct mlxsw_sp *mlxsw_sp,
 			      struct mlxsw_sp_nexthop_group_key key)
 {
-	return rhashtable_lookup_fast(&mlxsw_sp->router.nexthop_group_ht, &key,
+	return rhashtable_lookup_fast(&mlxsw_sp->router->nexthop_group_ht, &key,
 				      mlxsw_sp_nexthop_group_ht_params);
 }
 
@@ -1300,14 +1409,14 @@ static const struct rhashtable_params mlxsw_sp_nexthop_ht_params = {
 static int mlxsw_sp_nexthop_insert(struct mlxsw_sp *mlxsw_sp,
 				   struct mlxsw_sp_nexthop *nh)
 {
-	return rhashtable_insert_fast(&mlxsw_sp->router.nexthop_ht,
+	return rhashtable_insert_fast(&mlxsw_sp->router->nexthop_ht,
 				      &nh->ht_node, mlxsw_sp_nexthop_ht_params);
 }
 
 static void mlxsw_sp_nexthop_remove(struct mlxsw_sp *mlxsw_sp,
 				    struct mlxsw_sp_nexthop *nh)
 {
-	rhashtable_remove_fast(&mlxsw_sp->router.nexthop_ht, &nh->ht_node,
+	rhashtable_remove_fast(&mlxsw_sp->router->nexthop_ht, &nh->ht_node,
 			       mlxsw_sp_nexthop_ht_params);
 }
 
@@ -1315,7 +1424,7 @@ static struct mlxsw_sp_nexthop *
 mlxsw_sp_nexthop_lookup(struct mlxsw_sp *mlxsw_sp,
 			struct mlxsw_sp_nexthop_key key)
 {
-	return rhashtable_lookup_fast(&mlxsw_sp->router.nexthop_ht, &key,
+	return rhashtable_lookup_fast(&mlxsw_sp->router->nexthop_ht, &key,
 				      mlxsw_sp_nexthop_ht_params);
 }
 
@@ -1602,7 +1711,7 @@ static int mlxsw_sp_nexthop_neigh_init(struct mlxsw_sp *mlxsw_sp,
 	 */
 	if (list_empty(&neigh_entry->nexthop_list))
 		list_add_tail(&neigh_entry->nexthop_neighs_list_node,
-			      &mlxsw_sp->router.nexthop_neighs_list);
+			      &mlxsw_sp->router->nexthop_neighs_list);
 
 	nh->neigh_entry = neigh_entry;
 	list_add_tail(&nh->neigh_list_node, &neigh_entry->nexthop_list);
@@ -1700,7 +1809,7 @@ static void mlxsw_sp_nexthop_event(struct mlxsw_sp *mlxsw_sp,
 	struct mlxsw_sp_nexthop *nh;
 	struct mlxsw_sp_rif *rif;
 
-	if (mlxsw_sp->router.aborted)
+	if (mlxsw_sp->router->aborted)
 		return;
 
 	key.fib_nh = fib_nh;
@@ -2513,7 +2622,7 @@ mlxsw_sp_router_fib4_add(struct mlxsw_sp *mlxsw_sp,
 	struct mlxsw_sp_fib_node *fib_node;
 	int err;
 
-	if (mlxsw_sp->router.aborted)
+	if (mlxsw_sp->router->aborted)
 		return 0;
 
 	fib_node = mlxsw_sp_fib4_node_get(mlxsw_sp, fen_info);
@@ -2553,7 +2662,7 @@ static void mlxsw_sp_router_fib4_del(struct mlxsw_sp *mlxsw_sp,
 	struct mlxsw_sp_fib_entry *fib_entry;
 	struct mlxsw_sp_fib_node *fib_node;
 
-	if (mlxsw_sp->router.aborted)
+	if (mlxsw_sp->router->aborted)
 		return;
 
 	fib_entry = mlxsw_sp_fib4_entry_lookup(mlxsw_sp, fen_info);
@@ -2584,7 +2693,7 @@ static int mlxsw_sp_router_set_abort_trap(struct mlxsw_sp *mlxsw_sp)
 		return err;
 
 	for (i = 0; i < MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_VRS); i++) {
-		struct mlxsw_sp_vr *vr = &mlxsw_sp->router.vrs[i];
+		struct mlxsw_sp_vr *vr = &mlxsw_sp->router->vrs[i];
 		char raltb_pl[MLXSW_REG_RALTB_LEN];
 		char ralue_pl[MLXSW_REG_RALUE_LEN];
 
@@ -2666,7 +2775,7 @@ static void mlxsw_sp_router_fib_flush(struct mlxsw_sp *mlxsw_sp)
 	int i;
 
 	for (i = 0; i < MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_VRS); i++) {
-		struct mlxsw_sp_vr *vr = &mlxsw_sp->router.vrs[i];
+		struct mlxsw_sp_vr *vr = &mlxsw_sp->router->vrs[i];
 
 		if (!mlxsw_sp_vr_is_used(vr))
 			continue;
@@ -2678,11 +2787,11 @@ static void mlxsw_sp_router_fib4_abort(struct mlxsw_sp *mlxsw_sp)
 {
 	int err;
 
-	if (mlxsw_sp->router.aborted)
+	if (mlxsw_sp->router->aborted)
 		return;
 	dev_warn(mlxsw_sp->bus_info->dev, "FIB abort triggered. Note that FIB entries are no longer being offloaded to this device.\n");
 	mlxsw_sp_router_fib_flush(mlxsw_sp);
-	mlxsw_sp->router.aborted = true;
+	mlxsw_sp->router->aborted = true;
 	err = mlxsw_sp_router_set_abort_trap(mlxsw_sp);
 	if (err)
 		dev_warn(mlxsw_sp->bus_info->dev, "Failed to set abort trap.\n");
@@ -2748,9 +2857,9 @@ static void mlxsw_sp_router_fib_event_work(struct work_struct *work)
 static int mlxsw_sp_router_fib_event(struct notifier_block *nb,
 				     unsigned long event, void *ptr)
 {
-	struct mlxsw_sp *mlxsw_sp = container_of(nb, struct mlxsw_sp, fib_nb);
 	struct mlxsw_sp_fib_event_work *fib_work;
 	struct fib_notifier_info *info = ptr;
+	struct mlxsw_sp_router *router;
 
 	if (!net_eq(info->net, &init_net))
 		return NOTIFY_DONE;
@@ -2760,7 +2869,8 @@ static int mlxsw_sp_router_fib_event(struct notifier_block *nb,
 		return NOTIFY_BAD;
 
 	INIT_WORK(&fib_work->work, mlxsw_sp_router_fib_event_work);
-	fib_work->mlxsw_sp = mlxsw_sp;
+	router = container_of(nb, struct mlxsw_sp_router, fib_nb);
+	fib_work->mlxsw_sp = router->mlxsw_sp;
 	fib_work->event = event;
 
 	switch (event) {
@@ -2798,8 +2908,9 @@ mlxsw_sp_rif_find_by_dev(const struct mlxsw_sp *mlxsw_sp,
 	int i;
 
 	for (i = 0; i < MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_RIFS); i++)
-		if (mlxsw_sp->rifs[i] && mlxsw_sp->rifs[i]->dev == dev)
-			return mlxsw_sp->rifs[i];
+		if (mlxsw_sp->router->rifs[i] &&
+		    mlxsw_sp->router->rifs[i]->dev == dev)
+			return mlxsw_sp->router->rifs[i];
 
 	return NULL;
 }
@@ -2849,77 +2960,46 @@ static bool mlxsw_sp_rif_should_config(struct mlxsw_sp_rif *rif,
 	return false;
 }
 
-#define MLXSW_SP_INVALID_INDEX_RIF 0xffff
-static int mlxsw_sp_avail_rif_get(struct mlxsw_sp *mlxsw_sp)
+static enum mlxsw_sp_rif_type
+mlxsw_sp_dev_rif_type(const struct mlxsw_sp *mlxsw_sp,
+		      const struct net_device *dev)
 {
-	int i;
-
-	for (i = 0; i < MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_RIFS); i++)
-		if (!mlxsw_sp->rifs[i])
-			return i;
+	enum mlxsw_sp_fid_type type;
 
-	return MLXSW_SP_INVALID_INDEX_RIF;
-}
-
-static void mlxsw_sp_vport_rif_sp_attr_get(struct mlxsw_sp_port *mlxsw_sp_vport,
-					   bool *p_lagged, u16 *p_system_port)
-{
-	u8 local_port = mlxsw_sp_vport->local_port;
-
-	*p_lagged = mlxsw_sp_vport->lagged;
-	*p_system_port = *p_lagged ? mlxsw_sp_vport->lag_id : local_port;
-}
-
-static int mlxsw_sp_vport_rif_sp_op(struct mlxsw_sp_port *mlxsw_sp_vport,
-				    u16 vr_id, struct net_device *l3_dev,
-				    u16 rif_index, bool create)
-{
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_vport->mlxsw_sp;
-	bool lagged = mlxsw_sp_vport->lagged;
-	char ritr_pl[MLXSW_REG_RITR_LEN];
-	u16 system_port;
-
-	mlxsw_reg_ritr_pack(ritr_pl, create, MLXSW_REG_RITR_SP_IF, rif_index,
-			    vr_id, l3_dev->mtu, l3_dev->dev_addr);
-
-	mlxsw_sp_vport_rif_sp_attr_get(mlxsw_sp_vport, &lagged, &system_port);
-	mlxsw_reg_ritr_sp_if_pack(ritr_pl, lagged, system_port,
-				  mlxsw_sp_vport_vid_get(mlxsw_sp_vport));
-
-	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(ritr), ritr_pl);
-}
-
-static void mlxsw_sp_vport_rif_sp_leave(struct mlxsw_sp_port *mlxsw_sp_vport);
+	/* RIF type is derived from the type of the underlying FID */
+	if (is_vlan_dev(dev) && netif_is_bridge_master(vlan_dev_real_dev(dev)))
+		type = MLXSW_SP_FID_TYPE_8021Q;
+	else if (netif_is_bridge_master(dev) && br_vlan_enabled(dev))
+		type = MLXSW_SP_FID_TYPE_8021Q;
+	else if (netif_is_bridge_master(dev))
+		type = MLXSW_SP_FID_TYPE_8021D;
+	else
+		type = MLXSW_SP_FID_TYPE_RFID;
 
-static u16 mlxsw_sp_rif_sp_to_fid(u16 rif_index)
-{
-	return MLXSW_SP_RFID_BASE + rif_index;
+	return mlxsw_sp_fid_type_rif_type(mlxsw_sp, type);
 }
 
-static struct mlxsw_sp_fid *
-mlxsw_sp_rfid_alloc(u16 fid, struct net_device *l3_dev)
+static int mlxsw_sp_rif_index_alloc(struct mlxsw_sp *mlxsw_sp, u16 *p_rif_index)
 {
-	struct mlxsw_sp_fid *f;
-
-	f = kzalloc(sizeof(*f), GFP_KERNEL);
-	if (!f)
-		return NULL;
+	int i;
 
-	f->leave = mlxsw_sp_vport_rif_sp_leave;
-	f->ref_count = 0;
-	f->dev = l3_dev;
-	f->fid = fid;
+	for (i = 0; i < MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_RIFS); i++) {
+		if (!mlxsw_sp->router->rifs[i]) {
+			*p_rif_index = i;
+			return 0;
+		}
+	}
 
-	return f;
+	return -ENOBUFS;
 }
 
-static struct mlxsw_sp_rif *
-mlxsw_sp_rif_alloc(u16 rif_index, u16 vr_id, struct net_device *l3_dev,
-		   struct mlxsw_sp_fid *f)
+static struct mlxsw_sp_rif *mlxsw_sp_rif_alloc(size_t rif_size, u16 rif_index,
+					       u16 vr_id,
+					       struct net_device *l3_dev)
 {
 	struct mlxsw_sp_rif *rif;
 
-	rif = kzalloc(sizeof(*rif), GFP_KERNEL);
+	rif = kzalloc(rif_size, GFP_KERNEL);
 	if (!rif)
 		return NULL;
 
@@ -2930,11 +3010,16 @@ mlxsw_sp_rif_alloc(u16 rif_index, u16 vr_id, struct net_device *l3_dev,
 	rif->vr_id = vr_id;
 	rif->dev = l3_dev;
 	rif->rif_index = rif_index;
-	rif->f = f;
 
 	return rif;
 }
 
+struct mlxsw_sp_rif *mlxsw_sp_rif_by_index(const struct mlxsw_sp *mlxsw_sp,
+					   u16 rif_index)
+{
+	return mlxsw_sp->router->rifs[rif_index];
+}
+
 u16 mlxsw_sp_rif_index(const struct mlxsw_sp_rif *rif)
 {
 	return rif->rif_index;
@@ -2946,152 +3031,199 @@ int mlxsw_sp_rif_dev_ifindex(const struct mlxsw_sp_rif *rif)
 }
 
 static struct mlxsw_sp_rif *
-mlxsw_sp_vport_rif_sp_create(struct mlxsw_sp_port *mlxsw_sp_vport,
-			     struct net_device *l3_dev)
+mlxsw_sp_rif_create(struct mlxsw_sp *mlxsw_sp,
+		    const struct mlxsw_sp_rif_params *params)
 {
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_vport->mlxsw_sp;
-	u32 tb_id = l3mdev_fib_table(l3_dev);
-	struct mlxsw_sp_vr *vr;
-	struct mlxsw_sp_fid *f;
+	u32 tb_id = l3mdev_fib_table(params->dev);
+	const struct mlxsw_sp_rif_ops *ops;
+	enum mlxsw_sp_rif_type type;
 	struct mlxsw_sp_rif *rif;
-	u16 fid, rif_index;
+	struct mlxsw_sp_fid *fid;
+	struct mlxsw_sp_vr *vr;
+	u16 rif_index;
 	int err;
 
-	rif_index = mlxsw_sp_avail_rif_get(mlxsw_sp);
-	if (rif_index == MLXSW_SP_INVALID_INDEX_RIF)
-		return ERR_PTR(-ERANGE);
+	type = mlxsw_sp_dev_rif_type(mlxsw_sp, params->dev);
+	ops = mlxsw_sp->router->rif_ops_arr[type];
 
 	vr = mlxsw_sp_vr_get(mlxsw_sp, tb_id ? : RT_TABLE_MAIN);
 	if (IS_ERR(vr))
 		return ERR_CAST(vr);
 
-	err = mlxsw_sp_vport_rif_sp_op(mlxsw_sp_vport, vr->id, l3_dev,
-				       rif_index, true);
+	err = mlxsw_sp_rif_index_alloc(mlxsw_sp, &rif_index);
 	if (err)
-		goto err_vport_rif_sp_op;
+		goto err_rif_index_alloc;
 
-	fid = mlxsw_sp_rif_sp_to_fid(rif_index);
-	err = mlxsw_sp_rif_fdb_op(mlxsw_sp, l3_dev->dev_addr, fid, true);
-	if (err)
-		goto err_rif_fdb_op;
-
-	f = mlxsw_sp_rfid_alloc(fid, l3_dev);
-	if (!f) {
-		err = -ENOMEM;
-		goto err_rfid_alloc;
-	}
-
-	rif = mlxsw_sp_rif_alloc(rif_index, vr->id, l3_dev, f);
+	rif = mlxsw_sp_rif_alloc(ops->rif_size, rif_index, vr->id, params->dev);
 	if (!rif) {
 		err = -ENOMEM;
 		goto err_rif_alloc;
 	}
+	rif->mlxsw_sp = mlxsw_sp;
+	rif->ops = ops;
 
-	if (devlink_dpipe_table_counter_enabled(priv_to_devlink(mlxsw_sp->core),
-						MLXSW_SP_DPIPE_TABLE_NAME_ERIF)) {
-		err = mlxsw_sp_rif_counter_alloc(mlxsw_sp, rif,
-						 MLXSW_SP_RIF_COUNTER_EGRESS);
-		if (err)
-			netdev_dbg(mlxsw_sp_vport->dev,
-				   "Counter alloc Failed err=%d\n", err);
+	fid = ops->fid_get(rif);
+	if (IS_ERR(fid)) {
+		err = PTR_ERR(fid);
+		goto err_fid_get;
 	}
+	rif->fid = fid;
+
+	if (ops->setup)
+		ops->setup(rif, params);
+
+	err = ops->configure(rif);
+	if (err)
+		goto err_configure;
 
-	f->rif = rif;
-	mlxsw_sp->rifs[rif_index] = rif;
+	err = mlxsw_sp_rif_fdb_op(mlxsw_sp, params->dev->dev_addr,
+				  mlxsw_sp_fid_index(fid), true);
+	if (err)
+		goto err_rif_fdb_op;
+
+	mlxsw_sp_rif_counters_alloc(rif);
+	mlxsw_sp_fid_rif_set(fid, rif);
+	mlxsw_sp->router->rifs[rif_index] = rif;
 	vr->rif_count++;
 
 	return rif;
 
-err_rif_alloc:
-	kfree(f);
-err_rfid_alloc:
-	mlxsw_sp_rif_fdb_op(mlxsw_sp, l3_dev->dev_addr, fid, false);
 err_rif_fdb_op:
-	mlxsw_sp_vport_rif_sp_op(mlxsw_sp_vport, vr->id, l3_dev, rif_index,
-				 false);
-err_vport_rif_sp_op:
+	ops->deconfigure(rif);
+err_configure:
+	mlxsw_sp_fid_put(fid);
+err_fid_get:
+	kfree(rif);
+err_rif_alloc:
+err_rif_index_alloc:
 	mlxsw_sp_vr_put(vr);
 	return ERR_PTR(err);
 }
 
-static void mlxsw_sp_vport_rif_sp_destroy(struct mlxsw_sp_port *mlxsw_sp_vport,
-					  struct mlxsw_sp_rif *rif)
+void mlxsw_sp_rif_destroy(struct mlxsw_sp_rif *rif)
 {
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_vport->mlxsw_sp;
-	struct mlxsw_sp_vr *vr = &mlxsw_sp->router.vrs[rif->vr_id];
-	struct net_device *l3_dev = rif->dev;
-	struct mlxsw_sp_fid *f = rif->f;
-	u16 rif_index = rif->rif_index;
-	u16 fid = f->fid;
+	const struct mlxsw_sp_rif_ops *ops = rif->ops;
+	struct mlxsw_sp *mlxsw_sp = rif->mlxsw_sp;
+	struct mlxsw_sp_fid *fid = rif->fid;
+	struct mlxsw_sp_vr *vr;
 
 	mlxsw_sp_router_rif_gone_sync(mlxsw_sp, rif);
-
-	mlxsw_sp_rif_counter_free(mlxsw_sp, rif, MLXSW_SP_RIF_COUNTER_EGRESS);
-	mlxsw_sp_rif_counter_free(mlxsw_sp, rif, MLXSW_SP_RIF_COUNTER_INGRESS);
+	vr = &mlxsw_sp->router->vrs[rif->vr_id];
 
 	vr->rif_count--;
-	mlxsw_sp->rifs[rif_index] = NULL;
-	f->rif = NULL;
-
+	mlxsw_sp->router->rifs[rif->rif_index] = NULL;
+	mlxsw_sp_fid_rif_set(fid, NULL);
+	mlxsw_sp_rif_counters_free(rif);
+	mlxsw_sp_rif_fdb_op(mlxsw_sp, rif->dev->dev_addr,
+			    mlxsw_sp_fid_index(fid), false);
+	ops->deconfigure(rif);
+	mlxsw_sp_fid_put(fid);
 	kfree(rif);
+	mlxsw_sp_vr_put(vr);
+}
 
-	kfree(f);
-
-	mlxsw_sp_rif_fdb_op(mlxsw_sp, l3_dev->dev_addr, fid, false);
+static void
+mlxsw_sp_rif_subport_params_init(struct mlxsw_sp_rif_params *params,
+				 struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan)
+{
+	struct mlxsw_sp_port *mlxsw_sp_port = mlxsw_sp_port_vlan->mlxsw_sp_port;
 
-	mlxsw_sp_vport_rif_sp_op(mlxsw_sp_vport, vr->id, l3_dev, rif_index,
-				 false);
-	mlxsw_sp_vr_put(vr);
+	params->vid = mlxsw_sp_port_vlan->vid;
+	params->lag = mlxsw_sp_port->lagged;
+	if (params->lag)
+		params->lag_id = mlxsw_sp_port->lag_id;
+	else
+		params->system_port = mlxsw_sp_port->local_port;
 }
 
-static int mlxsw_sp_vport_rif_sp_join(struct mlxsw_sp_port *mlxsw_sp_vport,
-				      struct net_device *l3_dev)
+static int
+mlxsw_sp_port_vlan_router_join(struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan,
+			       struct net_device *l3_dev)
 {
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_vport->mlxsw_sp;
+	struct mlxsw_sp_port *mlxsw_sp_port = mlxsw_sp_port_vlan->mlxsw_sp_port;
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	u16 vid = mlxsw_sp_port_vlan->vid;
 	struct mlxsw_sp_rif *rif;
+	struct mlxsw_sp_fid *fid;
+	int err;
 
 	rif = mlxsw_sp_rif_find_by_dev(mlxsw_sp, l3_dev);
 	if (!rif) {
-		rif = mlxsw_sp_vport_rif_sp_create(mlxsw_sp_vport, l3_dev);
+		struct mlxsw_sp_rif_params params = {
+			.dev = l3_dev,
+		};
+
+		mlxsw_sp_rif_subport_params_init(&params, mlxsw_sp_port_vlan);
+		rif = mlxsw_sp_rif_create(mlxsw_sp, &params);
 		if (IS_ERR(rif))
 			return PTR_ERR(rif);
 	}
 
-	mlxsw_sp_vport_fid_set(mlxsw_sp_vport, rif->f);
-	rif->f->ref_count++;
+	/* FID was already created, just take a reference */
+	fid = rif->ops->fid_get(rif);
+	err = mlxsw_sp_fid_port_vid_map(fid, mlxsw_sp_port, vid);
+	if (err)
+		goto err_fid_port_vid_map;
+
+	err = mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, false);
+	if (err)
+		goto err_port_vid_learning_set;
+
+	err = mlxsw_sp_port_vid_stp_set(mlxsw_sp_port, vid,
+					BR_STATE_FORWARDING);
+	if (err)
+		goto err_port_vid_stp_set;
 
-	netdev_dbg(mlxsw_sp_vport->dev, "Joined FID=%d\n", rif->f->fid);
+	mlxsw_sp_port_vlan->fid = fid;
 
 	return 0;
+
+err_port_vid_stp_set:
+	mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, true);
+err_port_vid_learning_set:
+	mlxsw_sp_fid_port_vid_unmap(fid, mlxsw_sp_port, vid);
+err_fid_port_vid_map:
+	mlxsw_sp_fid_put(fid);
+	return err;
 }
 
-static void mlxsw_sp_vport_rif_sp_leave(struct mlxsw_sp_port *mlxsw_sp_vport)
+void
+mlxsw_sp_port_vlan_router_leave(struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan)
 {
-	struct mlxsw_sp_fid *f = mlxsw_sp_vport_fid_get(mlxsw_sp_vport);
+	struct mlxsw_sp_port *mlxsw_sp_port = mlxsw_sp_port_vlan->mlxsw_sp_port;
+	struct mlxsw_sp_fid *fid = mlxsw_sp_port_vlan->fid;
+	u16 vid = mlxsw_sp_port_vlan->vid;
 
-	netdev_dbg(mlxsw_sp_vport->dev, "Left FID=%d\n", f->fid);
+	if (WARN_ON(mlxsw_sp_fid_type(fid) != MLXSW_SP_FID_TYPE_RFID))
+		return;
 
-	mlxsw_sp_vport_fid_set(mlxsw_sp_vport, NULL);
-	if (--f->ref_count == 0)
-		mlxsw_sp_vport_rif_sp_destroy(mlxsw_sp_vport, f->rif);
+	mlxsw_sp_port_vlan->fid = NULL;
+	mlxsw_sp_port_vid_stp_set(mlxsw_sp_port, vid, BR_STATE_BLOCKING);
+	mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, true);
+	mlxsw_sp_fid_port_vid_unmap(fid, mlxsw_sp_port, vid);
+	/* If router port holds the last reference on the rFID, then the
+	 * associated Sub-port RIF will be destroyed.
+	 */
+	mlxsw_sp_fid_put(fid);
 }
 
-static int mlxsw_sp_inetaddr_vport_event(struct net_device *l3_dev,
-					 struct net_device *port_dev,
-					 unsigned long event, u16 vid)
+static int mlxsw_sp_inetaddr_port_vlan_event(struct net_device *l3_dev,
+					     struct net_device *port_dev,
+					     unsigned long event, u16 vid)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(port_dev);
-	struct mlxsw_sp_port *mlxsw_sp_vport;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 
-	mlxsw_sp_vport = mlxsw_sp_port_vport_find(mlxsw_sp_port, vid);
-	if (WARN_ON(!mlxsw_sp_vport))
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_vid(mlxsw_sp_port, vid);
+	if (WARN_ON(!mlxsw_sp_port_vlan))
 		return -EINVAL;
 
 	switch (event) {
 	case NETDEV_UP:
-		return mlxsw_sp_vport_rif_sp_join(mlxsw_sp_vport, l3_dev);
+		return mlxsw_sp_port_vlan_router_join(mlxsw_sp_port_vlan,
+						      l3_dev);
 	case NETDEV_DOWN:
-		mlxsw_sp_vport_rif_sp_leave(mlxsw_sp_vport);
+		mlxsw_sp_port_vlan_router_leave(mlxsw_sp_port_vlan);
 		break;
 	}
 
@@ -3106,7 +3238,7 @@ static int mlxsw_sp_inetaddr_port_event(struct net_device *port_dev,
 	    netif_is_ovs_port(port_dev))
 		return 0;
 
-	return mlxsw_sp_inetaddr_vport_event(port_dev, port_dev, event, 1);
+	return mlxsw_sp_inetaddr_port_vlan_event(port_dev, port_dev, event, 1);
 }
 
 static int __mlxsw_sp_inetaddr_lag_event(struct net_device *l3_dev,
@@ -3119,8 +3251,9 @@ static int __mlxsw_sp_inetaddr_lag_event(struct net_device *l3_dev,
 
 	netdev_for_each_lower_dev(lag_dev, port_dev, iter) {
 		if (mlxsw_sp_port_dev_check(port_dev)) {
-			err = mlxsw_sp_inetaddr_vport_event(l3_dev, port_dev,
-							    event, vid);
+			err = mlxsw_sp_inetaddr_port_vlan_event(l3_dev,
+								port_dev,
+								event, vid);
 			if (err)
 				return err;
 		}
@@ -3138,189 +3271,24 @@ static int mlxsw_sp_inetaddr_lag_event(struct net_device *lag_dev,
 	return __mlxsw_sp_inetaddr_lag_event(lag_dev, lag_dev, event, 1);
 }
 
-static struct mlxsw_sp_fid *mlxsw_sp_bridge_fid_get(struct mlxsw_sp *mlxsw_sp,
-						    struct net_device *l3_dev)
-{
-	u16 fid;
-
-	if (is_vlan_dev(l3_dev))
-		fid = vlan_dev_vlan_id(l3_dev);
-	else if (mlxsw_sp->master_bridge.dev == l3_dev)
-		fid = 1;
-	else
-		return mlxsw_sp_vfid_find(mlxsw_sp, l3_dev);
-
-	return mlxsw_sp_fid_find(mlxsw_sp, fid);
-}
-
-static u8 mlxsw_sp_router_port(const struct mlxsw_sp *mlxsw_sp)
-{
-	return mlxsw_core_max_ports(mlxsw_sp->core) + 1;
-}
-
-static enum mlxsw_flood_table_type mlxsw_sp_flood_table_type_get(u16 fid)
-{
-	return mlxsw_sp_fid_is_vfid(fid) ? MLXSW_REG_SFGC_TABLE_TYPE_FID :
-	       MLXSW_REG_SFGC_TABLE_TYPE_FID_OFFEST;
-}
-
-static u16 mlxsw_sp_flood_table_index_get(u16 fid)
-{
-	return mlxsw_sp_fid_is_vfid(fid) ? mlxsw_sp_fid_to_vfid(fid) : fid;
-}
-
-static int mlxsw_sp_router_port_flood_set(struct mlxsw_sp *mlxsw_sp, u16 fid,
-					  bool set)
-{
-	u8 router_port = mlxsw_sp_router_port(mlxsw_sp);
-	enum mlxsw_flood_table_type table_type;
-	char *sftr_pl;
-	u16 index;
-	int err;
-
-	sftr_pl = kmalloc(MLXSW_REG_SFTR_LEN, GFP_KERNEL);
-	if (!sftr_pl)
-		return -ENOMEM;
-
-	table_type = mlxsw_sp_flood_table_type_get(fid);
-	index = mlxsw_sp_flood_table_index_get(fid);
-	mlxsw_reg_sftr_pack(sftr_pl, MLXSW_SP_FLOOD_TABLE_BC, index, table_type,
-			    1, router_port, set);
-	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sftr), sftr_pl);
-
-	kfree(sftr_pl);
-	return err;
-}
-
-static enum mlxsw_reg_ritr_if_type mlxsw_sp_rif_type_get(u16 fid)
-{
-	if (mlxsw_sp_fid_is_vfid(fid))
-		return MLXSW_REG_RITR_FID_IF;
-	else
-		return MLXSW_REG_RITR_VLAN_IF;
-}
-
-static int mlxsw_sp_rif_bridge_op(struct mlxsw_sp *mlxsw_sp, u16 vr_id,
-				  struct net_device *l3_dev,
-				  u16 fid, u16 rif,
-				  bool create)
-{
-	enum mlxsw_reg_ritr_if_type rif_type;
-	char ritr_pl[MLXSW_REG_RITR_LEN];
-
-	rif_type = mlxsw_sp_rif_type_get(fid);
-	mlxsw_reg_ritr_pack(ritr_pl, create, rif_type, rif, vr_id, l3_dev->mtu,
-			    l3_dev->dev_addr);
-	mlxsw_reg_ritr_fid_set(ritr_pl, rif_type, fid);
-
-	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(ritr), ritr_pl);
-}
-
-static int mlxsw_sp_rif_bridge_create(struct mlxsw_sp *mlxsw_sp,
-				      struct net_device *l3_dev,
-				      struct mlxsw_sp_fid *f)
-{
-	u32 tb_id = l3mdev_fib_table(l3_dev);
-	struct mlxsw_sp_rif *rif;
-	struct mlxsw_sp_vr *vr;
-	u16 rif_index;
-	int err;
-
-	rif_index = mlxsw_sp_avail_rif_get(mlxsw_sp);
-	if (rif_index == MLXSW_SP_INVALID_INDEX_RIF)
-		return -ERANGE;
-
-	vr = mlxsw_sp_vr_get(mlxsw_sp, tb_id ? : RT_TABLE_MAIN);
-	if (IS_ERR(vr))
-		return PTR_ERR(vr);
-
-	err = mlxsw_sp_router_port_flood_set(mlxsw_sp, f->fid, true);
-	if (err)
-		goto err_port_flood_set;
-
-	err = mlxsw_sp_rif_bridge_op(mlxsw_sp, vr->id, l3_dev, f->fid,
-				     rif_index, true);
-	if (err)
-		goto err_rif_bridge_op;
-
-	err = mlxsw_sp_rif_fdb_op(mlxsw_sp, l3_dev->dev_addr, f->fid, true);
-	if (err)
-		goto err_rif_fdb_op;
-
-	rif = mlxsw_sp_rif_alloc(rif_index, vr->id, l3_dev, f);
-	if (!rif) {
-		err = -ENOMEM;
-		goto err_rif_alloc;
-	}
-
-	f->rif = rif;
-	mlxsw_sp->rifs[rif_index] = rif;
-	vr->rif_count++;
-
-	netdev_dbg(l3_dev, "RIF=%d created\n", rif_index);
-
-	return 0;
-
-err_rif_alloc:
-	mlxsw_sp_rif_fdb_op(mlxsw_sp, l3_dev->dev_addr, f->fid, false);
-err_rif_fdb_op:
-	mlxsw_sp_rif_bridge_op(mlxsw_sp, vr->id, l3_dev, f->fid, rif_index,
-			       false);
-err_rif_bridge_op:
-	mlxsw_sp_router_port_flood_set(mlxsw_sp, f->fid, false);
-err_port_flood_set:
-	mlxsw_sp_vr_put(vr);
-	return err;
-}
-
-void mlxsw_sp_rif_bridge_destroy(struct mlxsw_sp *mlxsw_sp,
-				 struct mlxsw_sp_rif *rif)
-{
-	struct mlxsw_sp_vr *vr = &mlxsw_sp->router.vrs[rif->vr_id];
-	struct net_device *l3_dev = rif->dev;
-	struct mlxsw_sp_fid *f = rif->f;
-	u16 rif_index = rif->rif_index;
-
-	mlxsw_sp_router_rif_gone_sync(mlxsw_sp, rif);
-
-	vr->rif_count--;
-	mlxsw_sp->rifs[rif_index] = NULL;
-	f->rif = NULL;
-
-	kfree(rif);
-
-	mlxsw_sp_rif_fdb_op(mlxsw_sp, l3_dev->dev_addr, f->fid, false);
-
-	mlxsw_sp_rif_bridge_op(mlxsw_sp, vr->id, l3_dev, f->fid, rif_index,
-			       false);
-
-	mlxsw_sp_router_port_flood_set(mlxsw_sp, f->fid, false);
-
-	mlxsw_sp_vr_put(vr);
-
-	netdev_dbg(l3_dev, "RIF=%d destroyed\n", rif_index);
-}
-
 static int mlxsw_sp_inetaddr_bridge_event(struct net_device *l3_dev,
-					  struct net_device *br_dev,
 					  unsigned long event)
 {
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_lower_get(l3_dev);
-	struct mlxsw_sp_fid *f;
-
-	/* FID can either be an actual FID if the L3 device is the
-	 * VLAN-aware bridge or a VLAN device on top. Otherwise, the
-	 * L3 device is a VLAN-unaware bridge and we get a vFID.
-	 */
-	f = mlxsw_sp_bridge_fid_get(mlxsw_sp, l3_dev);
-	if (WARN_ON(!f))
-		return -EINVAL;
+	struct mlxsw_sp_rif_params params = {
+		.dev = l3_dev,
+	};
+	struct mlxsw_sp_rif *rif;
 
 	switch (event) {
 	case NETDEV_UP:
-		return mlxsw_sp_rif_bridge_create(mlxsw_sp, l3_dev, f);
+		rif = mlxsw_sp_rif_create(mlxsw_sp, &params);
+		if (IS_ERR(rif))
+			return PTR_ERR(rif);
+		break;
 	case NETDEV_DOWN:
-		mlxsw_sp_rif_bridge_destroy(mlxsw_sp, f->rif);
+		rif = mlxsw_sp_rif_find_by_dev(mlxsw_sp, l3_dev);
+		mlxsw_sp_rif_destroy(rif);
 		break;
 	}
 
@@ -3331,19 +3299,19 @@ static int mlxsw_sp_inetaddr_vlan_event(struct net_device *vlan_dev,
 					unsigned long event)
 {
 	struct net_device *real_dev = vlan_dev_real_dev(vlan_dev);
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_lower_get(vlan_dev);
 	u16 vid = vlan_dev_vlan_id(vlan_dev);
 
+	if (netif_is_bridge_port(vlan_dev))
+		return 0;
+
 	if (mlxsw_sp_port_dev_check(real_dev))
-		return mlxsw_sp_inetaddr_vport_event(vlan_dev, real_dev, event,
-						     vid);
+		return mlxsw_sp_inetaddr_port_vlan_event(vlan_dev, real_dev,
+							 event, vid);
 	else if (netif_is_lag_master(real_dev))
 		return __mlxsw_sp_inetaddr_lag_event(vlan_dev, real_dev, event,
 						     vid);
-	else if (netif_is_bridge_master(real_dev) &&
-		 mlxsw_sp->master_bridge.dev == real_dev)
-		return mlxsw_sp_inetaddr_bridge_event(vlan_dev, real_dev,
-						      event);
+	else if (netif_is_bridge_master(real_dev) && br_vlan_enabled(real_dev))
+		return mlxsw_sp_inetaddr_bridge_event(vlan_dev, event);
 
 	return 0;
 }
@@ -3356,7 +3324,7 @@ static int __mlxsw_sp_inetaddr_event(struct net_device *dev,
 	else if (netif_is_lag_master(dev))
 		return mlxsw_sp_inetaddr_lag_event(dev, event);
 	else if (netif_is_bridge_master(dev))
-		return mlxsw_sp_inetaddr_bridge_event(dev, dev, event);
+		return mlxsw_sp_inetaddr_bridge_event(dev, event);
 	else if (is_vlan_dev(dev))
 		return mlxsw_sp_inetaddr_vlan_event(dev, event);
 	else
@@ -3406,6 +3374,7 @@ int mlxsw_sp_netdevice_router_port_event(struct net_device *dev)
 {
 	struct mlxsw_sp *mlxsw_sp;
 	struct mlxsw_sp_rif *rif;
+	u16 fid_index;
 	int err;
 
 	mlxsw_sp = mlxsw_sp_lower_get(dev);
@@ -3415,8 +3384,9 @@ int mlxsw_sp_netdevice_router_port_event(struct net_device *dev)
 	rif = mlxsw_sp_rif_find_by_dev(mlxsw_sp, dev);
 	if (!rif)
 		return 0;
+	fid_index = mlxsw_sp_fid_index(rif->fid);
 
-	err = mlxsw_sp_rif_fdb_op(mlxsw_sp, rif->addr, rif->f->fid, false);
+	err = mlxsw_sp_rif_fdb_op(mlxsw_sp, rif->addr, fid_index, false);
 	if (err)
 		return err;
 
@@ -3425,7 +3395,7 @@ int mlxsw_sp_netdevice_router_port_event(struct net_device *dev)
 	if (err)
 		goto err_rif_edit;
 
-	err = mlxsw_sp_rif_fdb_op(mlxsw_sp, dev->dev_addr, rif->f->fid, true);
+	err = mlxsw_sp_rif_fdb_op(mlxsw_sp, dev->dev_addr, fid_index, true);
 	if (err)
 		goto err_rif_fdb_op;
 
@@ -3439,7 +3409,7 @@ int mlxsw_sp_netdevice_router_port_event(struct net_device *dev)
 err_rif_fdb_op:
 	mlxsw_sp_rif_edit(mlxsw_sp, rif->rif_index, rif->addr, rif->mtu);
 err_rif_edit:
-	mlxsw_sp_rif_fdb_op(mlxsw_sp, rif->addr, rif->f->fid, true);
+	mlxsw_sp_rif_fdb_op(mlxsw_sp, rif->addr, fid_index, true);
 	return err;
 }
 
@@ -3492,16 +3462,225 @@ int mlxsw_sp_netdevice_vrf_event(struct net_device *l3_dev, unsigned long event,
 	return err;
 }
 
+static struct mlxsw_sp_rif_subport *
+mlxsw_sp_rif_subport_rif(const struct mlxsw_sp_rif *rif)
+{
+	return container_of(rif, struct mlxsw_sp_rif_subport, common);
+}
+
+static void mlxsw_sp_rif_subport_setup(struct mlxsw_sp_rif *rif,
+				       const struct mlxsw_sp_rif_params *params)
+{
+	struct mlxsw_sp_rif_subport *rif_subport;
+
+	rif_subport = mlxsw_sp_rif_subport_rif(rif);
+	rif_subport->vid = params->vid;
+	rif_subport->lag = params->lag;
+	if (params->lag)
+		rif_subport->lag_id = params->lag_id;
+	else
+		rif_subport->system_port = params->system_port;
+}
+
+static int mlxsw_sp_rif_subport_op(struct mlxsw_sp_rif *rif, bool enable)
+{
+	struct mlxsw_sp *mlxsw_sp = rif->mlxsw_sp;
+	struct mlxsw_sp_rif_subport *rif_subport;
+	char ritr_pl[MLXSW_REG_RITR_LEN];
+
+	rif_subport = mlxsw_sp_rif_subport_rif(rif);
+	mlxsw_reg_ritr_pack(ritr_pl, enable, MLXSW_REG_RITR_SP_IF,
+			    rif->rif_index, rif->vr_id, rif->dev->mtu,
+			    rif->dev->dev_addr);
+	mlxsw_reg_ritr_sp_if_pack(ritr_pl, rif_subport->lag,
+				  rif_subport->lag ? rif_subport->lag_id :
+						     rif_subport->system_port,
+				  rif_subport->vid);
+
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(ritr), ritr_pl);
+}
+
+static int mlxsw_sp_rif_subport_configure(struct mlxsw_sp_rif *rif)
+{
+	return mlxsw_sp_rif_subport_op(rif, true);
+}
+
+static void mlxsw_sp_rif_subport_deconfigure(struct mlxsw_sp_rif *rif)
+{
+	mlxsw_sp_rif_subport_op(rif, false);
+}
+
+static struct mlxsw_sp_fid *
+mlxsw_sp_rif_subport_fid_get(struct mlxsw_sp_rif *rif)
+{
+	return mlxsw_sp_fid_rfid_get(rif->mlxsw_sp, rif->rif_index);
+}
+
+static const struct mlxsw_sp_rif_ops mlxsw_sp_rif_subport_ops = {
+	.type			= MLXSW_SP_RIF_TYPE_SUBPORT,
+	.rif_size		= sizeof(struct mlxsw_sp_rif_subport),
+	.setup			= mlxsw_sp_rif_subport_setup,
+	.configure		= mlxsw_sp_rif_subport_configure,
+	.deconfigure		= mlxsw_sp_rif_subport_deconfigure,
+	.fid_get		= mlxsw_sp_rif_subport_fid_get,
+};
+
+static int mlxsw_sp_rif_vlan_fid_op(struct mlxsw_sp_rif *rif,
+				    enum mlxsw_reg_ritr_if_type type,
+				    u16 vid_fid, bool enable)
+{
+	struct mlxsw_sp *mlxsw_sp = rif->mlxsw_sp;
+	char ritr_pl[MLXSW_REG_RITR_LEN];
+
+	mlxsw_reg_ritr_pack(ritr_pl, enable, type, rif->rif_index, rif->vr_id,
+			    rif->dev->mtu, rif->dev->dev_addr);
+	mlxsw_reg_ritr_fid_set(ritr_pl, type, vid_fid);
+
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(ritr), ritr_pl);
+}
+
+static u8 mlxsw_sp_router_port(const struct mlxsw_sp *mlxsw_sp)
+{
+	return mlxsw_core_max_ports(mlxsw_sp->core) + 1;
+}
+
+static int mlxsw_sp_rif_vlan_configure(struct mlxsw_sp_rif *rif)
+{
+	struct mlxsw_sp *mlxsw_sp = rif->mlxsw_sp;
+	u16 vid = mlxsw_sp_fid_8021q_vid(rif->fid);
+	int err;
+
+	err = mlxsw_sp_rif_vlan_fid_op(rif, MLXSW_REG_RITR_VLAN_IF, vid, true);
+	if (err)
+		return err;
+
+	err = mlxsw_sp_fid_flood_set(rif->fid, MLXSW_SP_FLOOD_TYPE_BC,
+				     mlxsw_sp_router_port(mlxsw_sp), true);
+	if (err)
+		goto err_fid_bc_flood_set;
+
+	return 0;
+
+err_fid_bc_flood_set:
+	mlxsw_sp_rif_vlan_fid_op(rif, MLXSW_REG_RITR_VLAN_IF, vid, false);
+	return err;
+}
+
+static void mlxsw_sp_rif_vlan_deconfigure(struct mlxsw_sp_rif *rif)
+{
+	struct mlxsw_sp *mlxsw_sp = rif->mlxsw_sp;
+	u16 vid = mlxsw_sp_fid_8021q_vid(rif->fid);
+
+	mlxsw_sp_fid_flood_set(rif->fid, MLXSW_SP_FLOOD_TYPE_BC,
+			       mlxsw_sp_router_port(mlxsw_sp), false);
+	mlxsw_sp_rif_vlan_fid_op(rif, MLXSW_REG_RITR_VLAN_IF, vid, false);
+}
+
+static struct mlxsw_sp_fid *
+mlxsw_sp_rif_vlan_fid_get(struct mlxsw_sp_rif *rif)
+{
+	u16 vid = is_vlan_dev(rif->dev) ? vlan_dev_vlan_id(rif->dev) : 1;
+
+	return mlxsw_sp_fid_8021q_get(rif->mlxsw_sp, vid);
+}
+
+static const struct mlxsw_sp_rif_ops mlxsw_sp_rif_vlan_ops = {
+	.type			= MLXSW_SP_RIF_TYPE_VLAN,
+	.rif_size		= sizeof(struct mlxsw_sp_rif),
+	.configure		= mlxsw_sp_rif_vlan_configure,
+	.deconfigure		= mlxsw_sp_rif_vlan_deconfigure,
+	.fid_get		= mlxsw_sp_rif_vlan_fid_get,
+};
+
+static int mlxsw_sp_rif_fid_configure(struct mlxsw_sp_rif *rif)
+{
+	struct mlxsw_sp *mlxsw_sp = rif->mlxsw_sp;
+	u16 fid_index = mlxsw_sp_fid_index(rif->fid);
+	int err;
+
+	err = mlxsw_sp_rif_vlan_fid_op(rif, MLXSW_REG_RITR_FID_IF, fid_index,
+				       true);
+	if (err)
+		return err;
+
+	err = mlxsw_sp_fid_flood_set(rif->fid, MLXSW_SP_FLOOD_TYPE_BC,
+				     mlxsw_sp_router_port(mlxsw_sp), true);
+	if (err)
+		goto err_fid_bc_flood_set;
+
+	return 0;
+
+err_fid_bc_flood_set:
+	mlxsw_sp_rif_vlan_fid_op(rif, MLXSW_REG_RITR_FID_IF, fid_index, false);
+	return err;
+}
+
+static void mlxsw_sp_rif_fid_deconfigure(struct mlxsw_sp_rif *rif)
+{
+	struct mlxsw_sp *mlxsw_sp = rif->mlxsw_sp;
+	u16 fid_index = mlxsw_sp_fid_index(rif->fid);
+
+	mlxsw_sp_fid_flood_set(rif->fid, MLXSW_SP_FLOOD_TYPE_BC,
+			       mlxsw_sp_router_port(mlxsw_sp), false);
+	mlxsw_sp_rif_vlan_fid_op(rif, MLXSW_REG_RITR_FID_IF, fid_index, false);
+}
+
+static struct mlxsw_sp_fid *
+mlxsw_sp_rif_fid_fid_get(struct mlxsw_sp_rif *rif)
+{
+	return mlxsw_sp_fid_8021d_get(rif->mlxsw_sp, rif->dev->ifindex);
+}
+
+static const struct mlxsw_sp_rif_ops mlxsw_sp_rif_fid_ops = {
+	.type			= MLXSW_SP_RIF_TYPE_FID,
+	.rif_size		= sizeof(struct mlxsw_sp_rif),
+	.configure		= mlxsw_sp_rif_fid_configure,
+	.deconfigure		= mlxsw_sp_rif_fid_deconfigure,
+	.fid_get		= mlxsw_sp_rif_fid_fid_get,
+};
+
+static const struct mlxsw_sp_rif_ops *mlxsw_sp_rif_ops_arr[] = {
+	[MLXSW_SP_RIF_TYPE_SUBPORT]	= &mlxsw_sp_rif_subport_ops,
+	[MLXSW_SP_RIF_TYPE_VLAN]	= &mlxsw_sp_rif_vlan_ops,
+	[MLXSW_SP_RIF_TYPE_FID]		= &mlxsw_sp_rif_fid_ops,
+};
+
+static int mlxsw_sp_rifs_init(struct mlxsw_sp *mlxsw_sp)
+{
+	u64 max_rifs = MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_RIFS);
+
+	mlxsw_sp->router->rifs = kcalloc(max_rifs,
+					 sizeof(struct mlxsw_sp_rif *),
+					 GFP_KERNEL);
+	if (!mlxsw_sp->router->rifs)
+		return -ENOMEM;
+
+	mlxsw_sp->router->rif_ops_arr = mlxsw_sp_rif_ops_arr;
+
+	return 0;
+}
+
+static void mlxsw_sp_rifs_fini(struct mlxsw_sp *mlxsw_sp)
+{
+	int i;
+
+	for (i = 0; i < MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_RIFS); i++)
+		WARN_ON_ONCE(mlxsw_sp->router->rifs[i]);
+
+	kfree(mlxsw_sp->router->rifs);
+}
+
 static void mlxsw_sp_router_fib_dump_flush(struct notifier_block *nb)
 {
-	struct mlxsw_sp *mlxsw_sp = container_of(nb, struct mlxsw_sp, fib_nb);
+	struct mlxsw_sp_router *router;
 
 	/* Flush pending FIB notifications and then flush the device's
 	 * table before requesting another dump. The FIB notification
 	 * block is unregistered, so no need to take RTNL.
 	 */
 	mlxsw_core_flush_owq();
-	mlxsw_sp_router_fib_flush(mlxsw_sp);
+	router = container_of(nb, struct mlxsw_sp_router, fib_nb);
+	mlxsw_sp_router_fib_flush(router->mlxsw_sp);
 }
 
 static int __mlxsw_sp_router_init(struct mlxsw_sp *mlxsw_sp)
@@ -3512,55 +3691,50 @@ static int __mlxsw_sp_router_init(struct mlxsw_sp *mlxsw_sp)
 
 	if (!MLXSW_CORE_RES_VALID(mlxsw_sp->core, MAX_RIFS))
 		return -EIO;
-
 	max_rifs = MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_RIFS);
-	mlxsw_sp->rifs = kcalloc(max_rifs, sizeof(struct mlxsw_sp_rif *),
-				 GFP_KERNEL);
-	if (!mlxsw_sp->rifs)
-		return -ENOMEM;
 
 	mlxsw_reg_rgcr_pack(rgcr_pl, true);
 	mlxsw_reg_rgcr_max_router_interfaces_set(rgcr_pl, max_rifs);
 	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(rgcr), rgcr_pl);
 	if (err)
-		goto err_rgcr_fail;
-
+		return err;
 	return 0;
-
-err_rgcr_fail:
-	kfree(mlxsw_sp->rifs);
-	return err;
 }
 
 static void __mlxsw_sp_router_fini(struct mlxsw_sp *mlxsw_sp)
 {
 	char rgcr_pl[MLXSW_REG_RGCR_LEN];
-	int i;
 
 	mlxsw_reg_rgcr_pack(rgcr_pl, false);
 	mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(rgcr), rgcr_pl);
-
-	for (i = 0; i < MLXSW_CORE_RES_GET(mlxsw_sp->core, MAX_RIFS); i++)
-		WARN_ON_ONCE(mlxsw_sp->rifs[i]);
-
-	kfree(mlxsw_sp->rifs);
 }
 
 int mlxsw_sp_router_init(struct mlxsw_sp *mlxsw_sp)
 {
+	struct mlxsw_sp_router *router;
 	int err;
 
-	INIT_LIST_HEAD(&mlxsw_sp->router.nexthop_neighs_list);
+	router = kzalloc(sizeof(*mlxsw_sp->router), GFP_KERNEL);
+	if (!router)
+		return -ENOMEM;
+	mlxsw_sp->router = router;
+	router->mlxsw_sp = mlxsw_sp;
+
+	INIT_LIST_HEAD(&mlxsw_sp->router->nexthop_neighs_list);
 	err = __mlxsw_sp_router_init(mlxsw_sp);
 	if (err)
-		return err;
+		goto err_router_init;
+
+	err = mlxsw_sp_rifs_init(mlxsw_sp);
+	if (err)
+		goto err_rifs_init;
 
-	err = rhashtable_init(&mlxsw_sp->router.nexthop_ht,
+	err = rhashtable_init(&mlxsw_sp->router->nexthop_ht,
 			      &mlxsw_sp_nexthop_ht_params);
 	if (err)
 		goto err_nexthop_ht_init;
 
-	err = rhashtable_init(&mlxsw_sp->router.nexthop_group_ht,
+	err = rhashtable_init(&mlxsw_sp->router->nexthop_group_ht,
 			      &mlxsw_sp_nexthop_group_ht_params);
 	if (err)
 		goto err_nexthop_group_ht_init;
@@ -3577,8 +3751,8 @@ int mlxsw_sp_router_init(struct mlxsw_sp *mlxsw_sp)
 	if (err)
 		goto err_neigh_init;
 
-	mlxsw_sp->fib_nb.notifier_call = mlxsw_sp_router_fib_event;
-	err = register_fib_notifier(&mlxsw_sp->fib_nb,
+	mlxsw_sp->router->fib_nb.notifier_call = mlxsw_sp_router_fib_event;
+	err = register_fib_notifier(&mlxsw_sp->router->fib_nb,
 				    mlxsw_sp_router_fib_dump_flush);
 	if (err)
 		goto err_register_fib_notifier;
@@ -3592,21 +3766,27 @@ err_neigh_init:
 err_vrs_init:
 	mlxsw_sp_lpm_fini(mlxsw_sp);
 err_lpm_init:
-	rhashtable_destroy(&mlxsw_sp->router.nexthop_group_ht);
+	rhashtable_destroy(&mlxsw_sp->router->nexthop_group_ht);
 err_nexthop_group_ht_init:
-	rhashtable_destroy(&mlxsw_sp->router.nexthop_ht);
+	rhashtable_destroy(&mlxsw_sp->router->nexthop_ht);
 err_nexthop_ht_init:
+	mlxsw_sp_rifs_fini(mlxsw_sp);
+err_rifs_init:
 	__mlxsw_sp_router_fini(mlxsw_sp);
+err_router_init:
+	kfree(mlxsw_sp->router);
 	return err;
 }
 
 void mlxsw_sp_router_fini(struct mlxsw_sp *mlxsw_sp)
 {
-	unregister_fib_notifier(&mlxsw_sp->fib_nb);
+	unregister_fib_notifier(&mlxsw_sp->router->fib_nb);
 	mlxsw_sp_neigh_fini(mlxsw_sp);
 	mlxsw_sp_vrs_fini(mlxsw_sp);
 	mlxsw_sp_lpm_fini(mlxsw_sp);
-	rhashtable_destroy(&mlxsw_sp->router.nexthop_group_ht);
-	rhashtable_destroy(&mlxsw_sp->router.nexthop_ht);
+	rhashtable_destroy(&mlxsw_sp->router->nexthop_group_ht);
+	rhashtable_destroy(&mlxsw_sp->router->nexthop_ht);
+	mlxsw_sp_rifs_fini(mlxsw_sp);
 	__mlxsw_sp_router_fini(mlxsw_sp);
+	kfree(mlxsw_sp->router);
 }
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_router.h b/drivers/net/ethernet/mellanox/mlxsw/spectrum_router.h
index c3095fef6697..a3e8d2b25148 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_router.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_router.h
@@ -42,6 +42,8 @@ enum mlxsw_sp_rif_counter_dir {
 	MLXSW_SP_RIF_COUNTER_EGRESS,
 };
 
+struct mlxsw_sp_rif *mlxsw_sp_rif_by_index(const struct mlxsw_sp *mlxsw_sp,
+					   u16 rif_index);
 u16 mlxsw_sp_rif_index(const struct mlxsw_sp_rif *rif);
 int mlxsw_sp_rif_dev_ifindex(const struct mlxsw_sp_rif *rif);
 int mlxsw_sp_rif_counter_value_get(struct mlxsw_sp *mlxsw_sp,
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c
index f4bb0c0b7c1d..cd89a3e6cd81 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c
@@ -52,340 +52,597 @@
 #include "core.h"
 #include "reg.h"
 
-static u16 mlxsw_sp_port_vid_to_fid_get(struct mlxsw_sp_port *mlxsw_sp_port,
-					u16 vid)
-{
-	struct mlxsw_sp_fid *f = mlxsw_sp_vport_fid_get(mlxsw_sp_port);
-	u16 fid = vid;
+struct mlxsw_sp_bridge_ops;
 
-	fid = f ? f->fid : fid;
+struct mlxsw_sp_bridge {
+	struct mlxsw_sp *mlxsw_sp;
+	struct {
+		struct delayed_work dw;
+#define MLXSW_SP_DEFAULT_LEARNING_INTERVAL 100
+		unsigned int interval; /* ms */
+	} fdb_notify;
+#define MLXSW_SP_MIN_AGEING_TIME 10
+#define MLXSW_SP_MAX_AGEING_TIME 1000000
+#define MLXSW_SP_DEFAULT_AGEING_TIME 300
+	u32 ageing_time;
+	bool vlan_enabled_exists;
+	struct list_head bridges_list;
+	struct list_head mids_list;
+	DECLARE_BITMAP(mids_bitmap, MLXSW_SP_MID_MAX);
+	const struct mlxsw_sp_bridge_ops *bridge_8021q_ops;
+	const struct mlxsw_sp_bridge_ops *bridge_8021d_ops;
+};
+
+struct mlxsw_sp_bridge_device {
+	struct net_device *dev;
+	struct list_head list;
+	struct list_head ports_list;
+	u8 vlan_enabled:1,
+	   multicast_enabled:1;
+	const struct mlxsw_sp_bridge_ops *ops;
+};
 
-	if (!fid)
-		fid = mlxsw_sp_port->pvid;
+struct mlxsw_sp_bridge_port {
+	struct net_device *dev;
+	struct mlxsw_sp_bridge_device *bridge_device;
+	struct list_head list;
+	struct list_head vlans_list;
+	unsigned int ref_count;
+	u8 stp_state;
+	unsigned long flags;
+	bool mrouter;
+	bool lagged;
+	union {
+		u16 lag_id;
+		u16 system_port;
+	};
+};
 
-	return fid;
+struct mlxsw_sp_bridge_vlan {
+	struct list_head list;
+	struct list_head port_vlan_list;
+	u16 vid;
+};
+
+struct mlxsw_sp_bridge_ops {
+	int (*port_join)(struct mlxsw_sp_bridge_device *bridge_device,
+			 struct mlxsw_sp_bridge_port *bridge_port,
+			 struct mlxsw_sp_port *mlxsw_sp_port);
+	void (*port_leave)(struct mlxsw_sp_bridge_device *bridge_device,
+			   struct mlxsw_sp_bridge_port *bridge_port,
+			   struct mlxsw_sp_port *mlxsw_sp_port);
+	struct mlxsw_sp_fid *
+		(*fid_get)(struct mlxsw_sp_bridge_device *bridge_device,
+			   u16 vid);
+};
+
+static int
+mlxsw_sp_bridge_port_fdb_flush(struct mlxsw_sp *mlxsw_sp,
+			       struct mlxsw_sp_bridge_port *bridge_port,
+			       u16 fid_index);
+
+static struct mlxsw_sp_bridge_device *
+mlxsw_sp_bridge_device_find(const struct mlxsw_sp_bridge *bridge,
+			    const struct net_device *br_dev)
+{
+	struct mlxsw_sp_bridge_device *bridge_device;
+
+	list_for_each_entry(bridge_device, &bridge->bridges_list, list)
+		if (bridge_device->dev == br_dev)
+			return bridge_device;
+
+	return NULL;
 }
 
-static struct mlxsw_sp_port *
-mlxsw_sp_port_orig_get(struct net_device *dev,
-		       struct mlxsw_sp_port *mlxsw_sp_port)
+static struct mlxsw_sp_bridge_device *
+mlxsw_sp_bridge_device_create(struct mlxsw_sp_bridge *bridge,
+			      struct net_device *br_dev)
 {
-	struct mlxsw_sp_port *mlxsw_sp_vport;
-	struct mlxsw_sp_fid *fid;
-	u16 vid;
+	struct device *dev = bridge->mlxsw_sp->bus_info->dev;
+	struct mlxsw_sp_bridge_device *bridge_device;
+	bool vlan_enabled = br_vlan_enabled(br_dev);
 
-	if (netif_is_bridge_master(dev)) {
-		fid = mlxsw_sp_vfid_find(mlxsw_sp_port->mlxsw_sp,
-					 dev);
-		if (fid) {
-			mlxsw_sp_vport =
-				mlxsw_sp_port_vport_find_by_fid(mlxsw_sp_port,
-								fid->fid);
-			WARN_ON(!mlxsw_sp_vport);
-			return mlxsw_sp_vport;
-		}
+	if (vlan_enabled && bridge->vlan_enabled_exists) {
+		dev_err(dev, "Only one VLAN-aware bridge is supported\n");
+		return ERR_PTR(-EINVAL);
 	}
 
-	if (!is_vlan_dev(dev))
-		return mlxsw_sp_port;
+	bridge_device = kzalloc(sizeof(*bridge_device), GFP_KERNEL);
+	if (!bridge_device)
+		return ERR_PTR(-ENOMEM);
+
+	bridge_device->dev = br_dev;
+	bridge_device->vlan_enabled = vlan_enabled;
+	bridge_device->multicast_enabled = br_multicast_enabled(br_dev);
+	INIT_LIST_HEAD(&bridge_device->ports_list);
+	if (vlan_enabled) {
+		bridge->vlan_enabled_exists = true;
+		bridge_device->ops = bridge->bridge_8021q_ops;
+	} else {
+		bridge_device->ops = bridge->bridge_8021d_ops;
+	}
+	list_add(&bridge_device->list, &bridge->bridges_list);
 
-	vid = vlan_dev_vlan_id(dev);
-	mlxsw_sp_vport = mlxsw_sp_port_vport_find(mlxsw_sp_port, vid);
-	WARN_ON(!mlxsw_sp_vport);
+	return bridge_device;
+}
 
-	return mlxsw_sp_vport;
+static void
+mlxsw_sp_bridge_device_destroy(struct mlxsw_sp_bridge *bridge,
+			       struct mlxsw_sp_bridge_device *bridge_device)
+{
+	list_del(&bridge_device->list);
+	if (bridge_device->vlan_enabled)
+		bridge->vlan_enabled_exists = false;
+	WARN_ON(!list_empty(&bridge_device->ports_list));
+	kfree(bridge_device);
 }
 
-static int mlxsw_sp_port_attr_get(struct net_device *dev,
-				  struct switchdev_attr *attr)
+static struct mlxsw_sp_bridge_device *
+mlxsw_sp_bridge_device_get(struct mlxsw_sp_bridge *bridge,
+			   struct net_device *br_dev)
 {
-	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct mlxsw_sp_bridge_device *bridge_device;
 
-	mlxsw_sp_port = mlxsw_sp_port_orig_get(attr->orig_dev, mlxsw_sp_port);
-	if (!mlxsw_sp_port)
-		return -EINVAL;
+	bridge_device = mlxsw_sp_bridge_device_find(bridge, br_dev);
+	if (bridge_device)
+		return bridge_device;
 
-	switch (attr->id) {
-	case SWITCHDEV_ATTR_ID_PORT_PARENT_ID:
-		attr->u.ppid.id_len = sizeof(mlxsw_sp->base_mac);
-		memcpy(&attr->u.ppid.id, &mlxsw_sp->base_mac,
-		       attr->u.ppid.id_len);
-		break;
-	case SWITCHDEV_ATTR_ID_PORT_BRIDGE_FLAGS:
-		attr->u.brport_flags =
-			(mlxsw_sp_port->learning ? BR_LEARNING : 0) |
-			(mlxsw_sp_port->learning_sync ? BR_LEARNING_SYNC : 0) |
-			(mlxsw_sp_port->uc_flood ? BR_FLOOD : 0);
-		break;
-	default:
-		return -EOPNOTSUPP;
+	return mlxsw_sp_bridge_device_create(bridge, br_dev);
+}
+
+static void
+mlxsw_sp_bridge_device_put(struct mlxsw_sp_bridge *bridge,
+			   struct mlxsw_sp_bridge_device *bridge_device)
+{
+	if (list_empty(&bridge_device->ports_list))
+		mlxsw_sp_bridge_device_destroy(bridge, bridge_device);
+}
+
+static struct mlxsw_sp_bridge_port *
+__mlxsw_sp_bridge_port_find(const struct mlxsw_sp_bridge_device *bridge_device,
+			    const struct net_device *brport_dev)
+{
+	struct mlxsw_sp_bridge_port *bridge_port;
+
+	list_for_each_entry(bridge_port, &bridge_device->ports_list, list) {
+		if (bridge_port->dev == brport_dev)
+			return bridge_port;
 	}
 
-	return 0;
+	return NULL;
 }
 
-static int mlxsw_sp_port_stp_state_set(struct mlxsw_sp_port *mlxsw_sp_port,
-				       u8 state)
+static struct mlxsw_sp_bridge_port *
+mlxsw_sp_bridge_port_find(struct mlxsw_sp_bridge *bridge,
+			  struct net_device *brport_dev)
 {
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	enum mlxsw_reg_spms_state spms_state;
-	char *spms_pl;
-	u16 vid;
+	struct net_device *br_dev = netdev_master_upper_dev_get(brport_dev);
+	struct mlxsw_sp_bridge_device *bridge_device;
+
+	if (!br_dev)
+		return NULL;
+
+	bridge_device = mlxsw_sp_bridge_device_find(bridge, br_dev);
+	if (!bridge_device)
+		return NULL;
+
+	return __mlxsw_sp_bridge_port_find(bridge_device, brport_dev);
+}
+
+static struct mlxsw_sp_bridge_port *
+mlxsw_sp_bridge_port_create(struct mlxsw_sp_bridge_device *bridge_device,
+			    struct net_device *brport_dev)
+{
+	struct mlxsw_sp_bridge_port *bridge_port;
+	struct mlxsw_sp_port *mlxsw_sp_port;
+
+	bridge_port = kzalloc(sizeof(*bridge_port), GFP_KERNEL);
+	if (!bridge_port)
+		return NULL;
+
+	mlxsw_sp_port = mlxsw_sp_port_dev_lower_find(brport_dev);
+	bridge_port->lagged = mlxsw_sp_port->lagged;
+	if (bridge_port->lagged)
+		bridge_port->lag_id = mlxsw_sp_port->lag_id;
+	else
+		bridge_port->system_port = mlxsw_sp_port->local_port;
+	bridge_port->dev = brport_dev;
+	bridge_port->bridge_device = bridge_device;
+	bridge_port->stp_state = BR_STATE_DISABLED;
+	bridge_port->flags = BR_LEARNING | BR_FLOOD | BR_LEARNING_SYNC;
+	INIT_LIST_HEAD(&bridge_port->vlans_list);
+	list_add(&bridge_port->list, &bridge_device->ports_list);
+	bridge_port->ref_count = 1;
+
+	return bridge_port;
+}
+
+static void
+mlxsw_sp_bridge_port_destroy(struct mlxsw_sp_bridge_port *bridge_port)
+{
+	list_del(&bridge_port->list);
+	WARN_ON(!list_empty(&bridge_port->vlans_list));
+	kfree(bridge_port);
+}
+
+static bool
+mlxsw_sp_bridge_port_should_destroy(const struct mlxsw_sp_bridge_port *
+				    bridge_port)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_lower_get(bridge_port->dev);
+
+	/* In case ports were pulled from out of a bridged LAG, then
+	 * it's possible the reference count isn't zero, yet the bridge
+	 * port should be destroyed, as it's no longer an upper of ours.
+	 */
+	if (!mlxsw_sp && list_empty(&bridge_port->vlans_list))
+		return true;
+	else if (bridge_port->ref_count == 0)
+		return true;
+	else
+		return false;
+}
+
+static struct mlxsw_sp_bridge_port *
+mlxsw_sp_bridge_port_get(struct mlxsw_sp_bridge *bridge,
+			 struct net_device *brport_dev)
+{
+	struct net_device *br_dev = netdev_master_upper_dev_get(brport_dev);
+	struct mlxsw_sp_bridge_device *bridge_device;
+	struct mlxsw_sp_bridge_port *bridge_port;
 	int err;
 
-	switch (state) {
-	case BR_STATE_FORWARDING:
-		spms_state = MLXSW_REG_SPMS_STATE_FORWARDING;
-		break;
-	case BR_STATE_LEARNING:
-		spms_state = MLXSW_REG_SPMS_STATE_LEARNING;
-		break;
-	case BR_STATE_LISTENING: /* fall-through */
-	case BR_STATE_DISABLED: /* fall-through */
-	case BR_STATE_BLOCKING:
-		spms_state = MLXSW_REG_SPMS_STATE_DISCARDING;
-		break;
-	default:
-		BUG();
+	bridge_port = mlxsw_sp_bridge_port_find(bridge, brport_dev);
+	if (bridge_port) {
+		bridge_port->ref_count++;
+		return bridge_port;
 	}
 
-	spms_pl = kmalloc(MLXSW_REG_SPMS_LEN, GFP_KERNEL);
-	if (!spms_pl)
-		return -ENOMEM;
-	mlxsw_reg_spms_pack(spms_pl, mlxsw_sp_port->local_port);
+	bridge_device = mlxsw_sp_bridge_device_get(bridge, br_dev);
+	if (IS_ERR(bridge_device))
+		return ERR_CAST(bridge_device);
 
-	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
-		vid = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
-		mlxsw_reg_spms_vid_pack(spms_pl, vid, spms_state);
-	} else {
-		for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID)
-			mlxsw_reg_spms_vid_pack(spms_pl, vid, spms_state);
+	bridge_port = mlxsw_sp_bridge_port_create(bridge_device, brport_dev);
+	if (!bridge_port) {
+		err = -ENOMEM;
+		goto err_bridge_port_create;
 	}
 
-	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(spms), spms_pl);
-	kfree(spms_pl);
-	return err;
+	return bridge_port;
+
+err_bridge_port_create:
+	mlxsw_sp_bridge_device_put(bridge, bridge_device);
+	return ERR_PTR(err);
 }
 
-static int mlxsw_sp_port_attr_stp_state_set(struct mlxsw_sp_port *mlxsw_sp_port,
-					    struct switchdev_trans *trans,
-					    u8 state)
+static void mlxsw_sp_bridge_port_put(struct mlxsw_sp_bridge *bridge,
+				     struct mlxsw_sp_bridge_port *bridge_port)
 {
-	if (switchdev_trans_ph_prepare(trans))
-		return 0;
+	struct mlxsw_sp_bridge_device *bridge_device;
 
-	mlxsw_sp_port->stp_state = state;
-	return mlxsw_sp_port_stp_state_set(mlxsw_sp_port, state);
+	bridge_port->ref_count--;
+	if (!mlxsw_sp_bridge_port_should_destroy(bridge_port))
+		return;
+	bridge_device = bridge_port->bridge_device;
+	mlxsw_sp_bridge_port_destroy(bridge_port);
+	mlxsw_sp_bridge_device_put(bridge, bridge_device);
 }
 
-static int __mlxsw_sp_port_flood_table_set(struct mlxsw_sp_port *mlxsw_sp_port,
-					   u16 idx_begin, u16 idx_end,
-					   enum mlxsw_sp_flood_table table,
-					   bool set)
+static struct mlxsw_sp_port_vlan *
+mlxsw_sp_port_vlan_find_by_bridge(struct mlxsw_sp_port *mlxsw_sp_port,
+				  const struct mlxsw_sp_bridge_device *
+				  bridge_device,
+				  u16 vid)
 {
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	u16 local_port = mlxsw_sp_port->local_port;
-	enum mlxsw_flood_table_type table_type;
-	u16 range = idx_end - idx_begin + 1;
-	char *sftr_pl;
-	int err;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 
-	if (mlxsw_sp_port_is_vport(mlxsw_sp_port))
-		table_type = MLXSW_REG_SFGC_TABLE_TYPE_FID;
-	else
-		table_type = MLXSW_REG_SFGC_TABLE_TYPE_FID_OFFEST;
+	list_for_each_entry(mlxsw_sp_port_vlan, &mlxsw_sp_port->vlans_list,
+			    list) {
+		if (!mlxsw_sp_port_vlan->bridge_port)
+			continue;
+		if (mlxsw_sp_port_vlan->bridge_port->bridge_device !=
+		    bridge_device)
+			continue;
+		if (bridge_device->vlan_enabled &&
+		    mlxsw_sp_port_vlan->vid != vid)
+			continue;
+		return mlxsw_sp_port_vlan;
+	}
 
-	sftr_pl = kmalloc(MLXSW_REG_SFTR_LEN, GFP_KERNEL);
-	if (!sftr_pl)
-		return -ENOMEM;
+	return NULL;
+}
 
-	mlxsw_reg_sftr_pack(sftr_pl, table, idx_begin,
-			    table_type, range, local_port, set);
-	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sftr), sftr_pl);
+static struct mlxsw_sp_port_vlan*
+mlxsw_sp_port_vlan_find_by_fid(struct mlxsw_sp_port *mlxsw_sp_port,
+			       u16 fid_index)
+{
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 
-	kfree(sftr_pl);
-	return err;
+	list_for_each_entry(mlxsw_sp_port_vlan, &mlxsw_sp_port->vlans_list,
+			    list) {
+		struct mlxsw_sp_fid *fid = mlxsw_sp_port_vlan->fid;
+
+		if (fid && mlxsw_sp_fid_index(fid) == fid_index)
+			return mlxsw_sp_port_vlan;
+	}
+
+	return NULL;
 }
 
-static int __mlxsw_sp_port_flood_set(struct mlxsw_sp_port *mlxsw_sp_port,
-				     u16 idx_begin, u16 idx_end, bool uc_set,
-				     bool bc_set, bool mc_set)
+static struct mlxsw_sp_bridge_vlan *
+mlxsw_sp_bridge_vlan_find(const struct mlxsw_sp_bridge_port *bridge_port,
+			  u16 vid)
 {
-	int err;
+	struct mlxsw_sp_bridge_vlan *bridge_vlan;
 
-	err = __mlxsw_sp_port_flood_table_set(mlxsw_sp_port, idx_begin, idx_end,
-					      MLXSW_SP_FLOOD_TABLE_UC, uc_set);
-	if (err)
-		return err;
+	list_for_each_entry(bridge_vlan, &bridge_port->vlans_list, list) {
+		if (bridge_vlan->vid == vid)
+			return bridge_vlan;
+	}
 
-	err = __mlxsw_sp_port_flood_table_set(mlxsw_sp_port, idx_begin, idx_end,
-					      MLXSW_SP_FLOOD_TABLE_BC, bc_set);
-	if (err)
-		goto err_flood_bm_set;
+	return NULL;
+}
 
-	err = __mlxsw_sp_port_flood_table_set(mlxsw_sp_port, idx_begin, idx_end,
-					      MLXSW_SP_FLOOD_TABLE_MC, mc_set);
-	if (err)
-		goto err_flood_mc_set;
-	return 0;
+static struct mlxsw_sp_bridge_vlan *
+mlxsw_sp_bridge_vlan_create(struct mlxsw_sp_bridge_port *bridge_port, u16 vid)
+{
+	struct mlxsw_sp_bridge_vlan *bridge_vlan;
 
-err_flood_mc_set:
-	__mlxsw_sp_port_flood_table_set(mlxsw_sp_port, idx_begin, idx_end,
-					MLXSW_SP_FLOOD_TABLE_BC, !bc_set);
-err_flood_bm_set:
-	__mlxsw_sp_port_flood_table_set(mlxsw_sp_port, idx_begin, idx_end,
-					MLXSW_SP_FLOOD_TABLE_UC, !uc_set);
-	return err;
+	bridge_vlan = kzalloc(sizeof(*bridge_vlan), GFP_KERNEL);
+	if (!bridge_vlan)
+		return NULL;
+
+	INIT_LIST_HEAD(&bridge_vlan->port_vlan_list);
+	bridge_vlan->vid = vid;
+	list_add(&bridge_vlan->list, &bridge_port->vlans_list);
+
+	return bridge_vlan;
 }
 
-static int mlxsw_sp_port_flood_table_set(struct mlxsw_sp_port *mlxsw_sp_port,
-					 enum mlxsw_sp_flood_table table,
-					 bool set)
+static void
+mlxsw_sp_bridge_vlan_destroy(struct mlxsw_sp_bridge_vlan *bridge_vlan)
 {
-	struct net_device *dev = mlxsw_sp_port->dev;
-	u16 vid, last_visited_vid;
-	int err;
+	list_del(&bridge_vlan->list);
+	WARN_ON(!list_empty(&bridge_vlan->port_vlan_list));
+	kfree(bridge_vlan);
+}
 
-	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
-		u16 fid = mlxsw_sp_vport_fid_get(mlxsw_sp_port)->fid;
-		u16 vfid = mlxsw_sp_fid_to_vfid(fid);
+static struct mlxsw_sp_bridge_vlan *
+mlxsw_sp_bridge_vlan_get(struct mlxsw_sp_bridge_port *bridge_port, u16 vid)
+{
+	struct mlxsw_sp_bridge_vlan *bridge_vlan;
 
-		return __mlxsw_sp_port_flood_table_set(mlxsw_sp_port, vfid,
-						       vfid, table, set);
-	}
+	bridge_vlan = mlxsw_sp_bridge_vlan_find(bridge_port, vid);
+	if (bridge_vlan)
+		return bridge_vlan;
 
-	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID) {
-		err = __mlxsw_sp_port_flood_table_set(mlxsw_sp_port, vid, vid,
-						      table, set);
-		if (err) {
-			last_visited_vid = vid;
-			goto err_port_flood_set;
-		}
+	return mlxsw_sp_bridge_vlan_create(bridge_port, vid);
+}
+
+static void mlxsw_sp_bridge_vlan_put(struct mlxsw_sp_bridge_vlan *bridge_vlan)
+{
+	if (list_empty(&bridge_vlan->port_vlan_list))
+		mlxsw_sp_bridge_vlan_destroy(bridge_vlan);
+}
+
+static void mlxsw_sp_port_bridge_flags_get(struct mlxsw_sp_bridge *bridge,
+					   struct net_device *dev,
+					   unsigned long *brport_flags)
+{
+	struct mlxsw_sp_bridge_port *bridge_port;
+
+	bridge_port = mlxsw_sp_bridge_port_find(bridge, dev);
+	if (WARN_ON(!bridge_port))
+		return;
+
+	memcpy(brport_flags, &bridge_port->flags, sizeof(*brport_flags));
+}
+
+static int mlxsw_sp_port_attr_get(struct net_device *dev,
+				  struct switchdev_attr *attr)
+{
+	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+
+	switch (attr->id) {
+	case SWITCHDEV_ATTR_ID_PORT_PARENT_ID:
+		attr->u.ppid.id_len = sizeof(mlxsw_sp->base_mac);
+		memcpy(&attr->u.ppid.id, &mlxsw_sp->base_mac,
+		       attr->u.ppid.id_len);
+		break;
+	case SWITCHDEV_ATTR_ID_PORT_BRIDGE_FLAGS:
+		mlxsw_sp_port_bridge_flags_get(mlxsw_sp->bridge, attr->orig_dev,
+					       &attr->u.brport_flags);
+		break;
+	case SWITCHDEV_ATTR_ID_PORT_BRIDGE_FLAGS_SUPPORT:
+		attr->u.brport_flags_support = BR_LEARNING | BR_FLOOD;
+		break;
+	default:
+		return -EOPNOTSUPP;
 	}
 
 	return 0;
+}
 
-err_port_flood_set:
-	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, last_visited_vid)
-		__mlxsw_sp_port_flood_table_set(mlxsw_sp_port, vid, vid, table,
-						!set);
-	netdev_err(dev, "Failed to configure unicast flooding\n");
-	return err;
+static int
+mlxsw_sp_port_bridge_vlan_stp_set(struct mlxsw_sp_port *mlxsw_sp_port,
+				  struct mlxsw_sp_bridge_vlan *bridge_vlan,
+				  u8 state)
+{
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
+
+	list_for_each_entry(mlxsw_sp_port_vlan, &bridge_vlan->port_vlan_list,
+			    bridge_vlan_node) {
+		if (mlxsw_sp_port_vlan->mlxsw_sp_port != mlxsw_sp_port)
+			continue;
+		return mlxsw_sp_port_vid_stp_set(mlxsw_sp_port,
+						 bridge_vlan->vid, state);
+	}
+
+	return 0;
 }
 
-static int mlxsw_sp_port_mc_disabled_set(struct mlxsw_sp_port *mlxsw_sp_port,
-					 struct switchdev_trans *trans,
-					 bool mc_disabled)
+static int mlxsw_sp_port_attr_stp_state_set(struct mlxsw_sp_port *mlxsw_sp_port,
+					    struct switchdev_trans *trans,
+					    struct net_device *orig_dev,
+					    u8 state)
 {
-	int set;
-	int err = 0;
+	struct mlxsw_sp_bridge_port *bridge_port;
+	struct mlxsw_sp_bridge_vlan *bridge_vlan;
+	int err;
 
 	if (switchdev_trans_ph_prepare(trans))
 		return 0;
 
-	if (mlxsw_sp_port->mc_router != mlxsw_sp_port->mc_flood) {
-		set = mc_disabled ?
-			mlxsw_sp_port->mc_flood : mlxsw_sp_port->mc_router;
-		err = mlxsw_sp_port_flood_table_set(mlxsw_sp_port,
-						    MLXSW_SP_FLOOD_TABLE_MC,
-						    set);
+	/* It's possible we failed to enslave the port, yet this
+	 * operation is executed due to it being deferred.
+	 */
+	bridge_port = mlxsw_sp_bridge_port_find(mlxsw_sp_port->mlxsw_sp->bridge,
+						orig_dev);
+	if (!bridge_port)
+		return 0;
+
+	list_for_each_entry(bridge_vlan, &bridge_port->vlans_list, list) {
+		err = mlxsw_sp_port_bridge_vlan_stp_set(mlxsw_sp_port,
+							bridge_vlan, state);
+		if (err)
+			goto err_port_bridge_vlan_stp_set;
 	}
 
-	if (!err)
-		mlxsw_sp_port->mc_disabled = mc_disabled;
+	bridge_port->stp_state = state;
 
+	return 0;
+
+err_port_bridge_vlan_stp_set:
+	list_for_each_entry_continue_reverse(bridge_vlan,
+					     &bridge_port->vlans_list, list)
+		mlxsw_sp_port_bridge_vlan_stp_set(mlxsw_sp_port, bridge_vlan,
+						  bridge_port->stp_state);
 	return err;
 }
 
-int mlxsw_sp_vport_flood_set(struct mlxsw_sp_port *mlxsw_sp_vport, u16 fid,
-			     bool set)
+static int
+mlxsw_sp_port_bridge_vlan_flood_set(struct mlxsw_sp_port *mlxsw_sp_port,
+				    struct mlxsw_sp_bridge_vlan *bridge_vlan,
+				    enum mlxsw_sp_flood_type packet_type,
+				    bool member)
 {
-	bool mc_set = set;
-	u16 vfid;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 
-	/* In case of vFIDs, index into the flooding table is relative to
-	 * the start of the vFIDs range.
-	 */
-	vfid = mlxsw_sp_fid_to_vfid(fid);
-
-	if (set)
-		mc_set = mlxsw_sp_vport->mc_disabled ?
-			 mlxsw_sp_vport->mc_flood : mlxsw_sp_vport->mc_router;
+	list_for_each_entry(mlxsw_sp_port_vlan, &bridge_vlan->port_vlan_list,
+			    bridge_vlan_node) {
+		if (mlxsw_sp_port_vlan->mlxsw_sp_port != mlxsw_sp_port)
+			continue;
+		return mlxsw_sp_fid_flood_set(mlxsw_sp_port_vlan->fid,
+					      packet_type,
+					      mlxsw_sp_port->local_port,
+					      member);
+	}
 
-	return __mlxsw_sp_port_flood_set(mlxsw_sp_vport, vfid, vfid, set, set,
-					 mc_set);
+	return 0;
 }
 
-static int mlxsw_sp_port_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
-				      bool set)
+static int
+mlxsw_sp_bridge_port_flood_table_set(struct mlxsw_sp_port *mlxsw_sp_port,
+				     struct mlxsw_sp_bridge_port *bridge_port,
+				     enum mlxsw_sp_flood_type packet_type,
+				     bool member)
 {
-	u16 vid;
+	struct mlxsw_sp_bridge_vlan *bridge_vlan;
 	int err;
 
-	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
-		vid = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
+	list_for_each_entry(bridge_vlan, &bridge_port->vlans_list, list) {
+		err = mlxsw_sp_port_bridge_vlan_flood_set(mlxsw_sp_port,
+							  bridge_vlan,
+							  packet_type,
+							  member);
+		if (err)
+			goto err_port_bridge_vlan_flood_set;
+	}
+
+	return 0;
+
+err_port_bridge_vlan_flood_set:
+	list_for_each_entry_continue_reverse(bridge_vlan,
+					     &bridge_port->vlans_list, list)
+		mlxsw_sp_port_bridge_vlan_flood_set(mlxsw_sp_port, bridge_vlan,
+						    packet_type, !member);
+	return err;
+}
 
-		return __mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, vid,
-							set);
+static int
+mlxsw_sp_port_bridge_vlan_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
+				       struct mlxsw_sp_bridge_vlan *bridge_vlan,
+				       bool set)
+{
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
+	u16 vid = bridge_vlan->vid;
+
+	list_for_each_entry(mlxsw_sp_port_vlan, &bridge_vlan->port_vlan_list,
+			    bridge_vlan_node) {
+		if (mlxsw_sp_port_vlan->mlxsw_sp_port != mlxsw_sp_port)
+			continue;
+		return mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, set);
 	}
 
-	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID) {
-		err = __mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, vid,
-						       set);
+	return 0;
+}
+
+static int
+mlxsw_sp_bridge_port_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
+				  struct mlxsw_sp_bridge_port *bridge_port,
+				  bool set)
+{
+	struct mlxsw_sp_bridge_vlan *bridge_vlan;
+	int err;
+
+	list_for_each_entry(bridge_vlan, &bridge_port->vlans_list, list) {
+		err = mlxsw_sp_port_bridge_vlan_learning_set(mlxsw_sp_port,
+							     bridge_vlan, set);
 		if (err)
-			goto err_port_vid_learning_set;
+			goto err_port_bridge_vlan_learning_set;
 	}
 
 	return 0;
 
-err_port_vid_learning_set:
-	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID)
-		__mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, vid, !set);
+err_port_bridge_vlan_learning_set:
+	list_for_each_entry_continue_reverse(bridge_vlan,
+					     &bridge_port->vlans_list, list)
+		mlxsw_sp_port_bridge_vlan_learning_set(mlxsw_sp_port,
+						       bridge_vlan, !set);
 	return err;
 }
 
 static int mlxsw_sp_port_attr_br_flags_set(struct mlxsw_sp_port *mlxsw_sp_port,
 					   struct switchdev_trans *trans,
+					   struct net_device *orig_dev,
 					   unsigned long brport_flags)
 {
-	unsigned long learning = mlxsw_sp_port->learning ? BR_LEARNING : 0;
-	unsigned long uc_flood = mlxsw_sp_port->uc_flood ? BR_FLOOD : 0;
+	struct mlxsw_sp_bridge_port *bridge_port;
 	int err;
 
-	if (!mlxsw_sp_port->bridged)
-		return -EINVAL;
-
 	if (switchdev_trans_ph_prepare(trans))
 		return 0;
 
-	if ((uc_flood ^ brport_flags) & BR_FLOOD) {
-		err = mlxsw_sp_port_flood_table_set(mlxsw_sp_port,
-						    MLXSW_SP_FLOOD_TABLE_UC,
-						    !mlxsw_sp_port->uc_flood);
-		if (err)
-			return err;
-	}
+	bridge_port = mlxsw_sp_bridge_port_find(mlxsw_sp_port->mlxsw_sp->bridge,
+						orig_dev);
+	if (WARN_ON(!bridge_port))
+		return -EINVAL;
 
-	if ((learning ^ brport_flags) & BR_LEARNING) {
-		err = mlxsw_sp_port_learning_set(mlxsw_sp_port,
-						 !mlxsw_sp_port->learning);
-		if (err)
-			goto err_port_learning_set;
-	}
+	err = mlxsw_sp_bridge_port_flood_table_set(mlxsw_sp_port, bridge_port,
+						   MLXSW_SP_FLOOD_TYPE_UC,
+						   brport_flags & BR_FLOOD);
+	if (err)
+		return err;
 
-	mlxsw_sp_port->uc_flood = brport_flags & BR_FLOOD ? 1 : 0;
-	mlxsw_sp_port->learning = brport_flags & BR_LEARNING ? 1 : 0;
-	mlxsw_sp_port->learning_sync = brport_flags & BR_LEARNING_SYNC ? 1 : 0;
+	err = mlxsw_sp_bridge_port_learning_set(mlxsw_sp_port, bridge_port,
+						brport_flags & BR_LEARNING);
+	if (err)
+		return err;
 
-	return 0;
+	memcpy(&bridge_port->flags, &brport_flags, sizeof(brport_flags));
 
-err_port_learning_set:
-	if ((uc_flood ^ brport_flags) & BR_FLOOD)
-		mlxsw_sp_port_flood_table_set(mlxsw_sp_port,
-					      MLXSW_SP_FLOOD_TABLE_UC,
-					      mlxsw_sp_port->uc_flood);
-	return err;
+	return 0;
 }
 
 static int mlxsw_sp_ageing_set(struct mlxsw_sp *mlxsw_sp, u32 ageing_time)
@@ -397,7 +654,7 @@ static int mlxsw_sp_ageing_set(struct mlxsw_sp *mlxsw_sp, u32 ageing_time)
 	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfdat), sfdat_pl);
 	if (err)
 		return err;
-	mlxsw_sp->ageing_time = ageing_time;
+	mlxsw_sp->bridge->ageing_time = ageing_time;
 	return 0;
 }
 
@@ -426,28 +683,77 @@ static int mlxsw_sp_port_attr_br_vlan_set(struct mlxsw_sp_port *mlxsw_sp_port,
 					  bool vlan_enabled)
 {
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct mlxsw_sp_bridge_device *bridge_device;
 
-	/* SWITCHDEV_TRANS_PREPARE phase */
-	if ((!vlan_enabled) && (mlxsw_sp->master_bridge.dev == orig_dev)) {
-		netdev_err(mlxsw_sp_port->dev, "Bridge must be vlan-aware\n");
+	if (!switchdev_trans_ph_prepare(trans))
+		return 0;
+
+	bridge_device = mlxsw_sp_bridge_device_find(mlxsw_sp->bridge, orig_dev);
+	if (WARN_ON(!bridge_device))
 		return -EINVAL;
-	}
 
-	return 0;
+	if (bridge_device->vlan_enabled == vlan_enabled)
+		return 0;
+
+	netdev_err(bridge_device->dev, "VLAN filtering can't be changed for existing bridge\n");
+	return -EINVAL;
 }
 
 static int mlxsw_sp_port_attr_mc_router_set(struct mlxsw_sp_port *mlxsw_sp_port,
 					    struct switchdev_trans *trans,
+					    struct net_device *orig_dev,
 					    bool is_port_mc_router)
 {
+	struct mlxsw_sp_bridge_port *bridge_port;
+
+	if (switchdev_trans_ph_prepare(trans))
+		return 0;
+
+	bridge_port = mlxsw_sp_bridge_port_find(mlxsw_sp_port->mlxsw_sp->bridge,
+						orig_dev);
+	if (WARN_ON(!bridge_port))
+		return -EINVAL;
+
+	if (!bridge_port->bridge_device->multicast_enabled)
+		return 0;
+
+	return mlxsw_sp_bridge_port_flood_table_set(mlxsw_sp_port, bridge_port,
+						    MLXSW_SP_FLOOD_TYPE_MC,
+						    is_port_mc_router);
+}
+
+static int mlxsw_sp_port_mc_disabled_set(struct mlxsw_sp_port *mlxsw_sp_port,
+					 struct switchdev_trans *trans,
+					 struct net_device *orig_dev,
+					 bool mc_disabled)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct mlxsw_sp_bridge_device *bridge_device;
+	struct mlxsw_sp_bridge_port *bridge_port;
+	int err;
+
 	if (switchdev_trans_ph_prepare(trans))
 		return 0;
 
-	mlxsw_sp_port->mc_router = is_port_mc_router;
-	if (!mlxsw_sp_port->mc_disabled)
-		return mlxsw_sp_port_flood_table_set(mlxsw_sp_port,
-						     MLXSW_SP_FLOOD_TABLE_MC,
-						     is_port_mc_router);
+	/* It's possible we failed to enslave the port, yet this
+	 * operation is executed due to it being deferred.
+	 */
+	bridge_device = mlxsw_sp_bridge_device_find(mlxsw_sp->bridge, orig_dev);
+	if (!bridge_device)
+		return 0;
+
+	list_for_each_entry(bridge_port, &bridge_device->ports_list, list) {
+		enum mlxsw_sp_flood_type packet_type = MLXSW_SP_FLOOD_TYPE_MC;
+		bool member = mc_disabled ? true : bridge_port->mrouter;
+
+		err = mlxsw_sp_bridge_port_flood_table_set(mlxsw_sp_port,
+							   bridge_port,
+							   packet_type, member);
+		if (err)
+			return err;
+	}
+
+	bridge_device->multicast_enabled = !mc_disabled;
 
 	return 0;
 }
@@ -457,19 +763,17 @@ static int mlxsw_sp_port_attr_set(struct net_device *dev,
 				  struct switchdev_trans *trans)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
-	int err = 0;
-
-	mlxsw_sp_port = mlxsw_sp_port_orig_get(attr->orig_dev, mlxsw_sp_port);
-	if (!mlxsw_sp_port)
-		return -EINVAL;
+	int err;
 
 	switch (attr->id) {
 	case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
 		err = mlxsw_sp_port_attr_stp_state_set(mlxsw_sp_port, trans,
+						       attr->orig_dev,
 						       attr->u.stp_state);
 		break;
 	case SWITCHDEV_ATTR_ID_PORT_BRIDGE_FLAGS:
 		err = mlxsw_sp_port_attr_br_flags_set(mlxsw_sp_port, trans,
+						      attr->orig_dev,
 						      attr->u.brport_flags);
 		break;
 	case SWITCHDEV_ATTR_ID_BRIDGE_AGEING_TIME:
@@ -483,10 +787,12 @@ static int mlxsw_sp_port_attr_set(struct net_device *dev,
 		break;
 	case SWITCHDEV_ATTR_ID_PORT_MROUTER:
 		err = mlxsw_sp_port_attr_mc_router_set(mlxsw_sp_port, trans,
+						       attr->orig_dev,
 						       attr->u.mrouter);
 		break;
 	case SWITCHDEV_ATTR_ID_BRIDGE_MC_DISABLED:
 		err = mlxsw_sp_port_mc_disabled_set(mlxsw_sp_port, trans,
+						    attr->orig_dev,
 						    attr->u.mc_disabled);
 		break;
 	default:
@@ -497,268 +803,243 @@ static int mlxsw_sp_port_attr_set(struct net_device *dev,
 	return err;
 }
 
-static int mlxsw_sp_fid_op(struct mlxsw_sp *mlxsw_sp, u16 fid, bool create)
+static bool mlxsw_sp_mc_flood(const struct mlxsw_sp_bridge_port *bridge_port)
 {
-	char sfmr_pl[MLXSW_REG_SFMR_LEN];
+	const struct mlxsw_sp_bridge_device *bridge_device;
 
-	mlxsw_reg_sfmr_pack(sfmr_pl, !create, fid, fid);
-	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfmr), sfmr_pl);
+	bridge_device = bridge_port->bridge_device;
+	return !bridge_device->multicast_enabled ? true : bridge_port->mrouter;
 }
 
-static int mlxsw_sp_fid_map(struct mlxsw_sp *mlxsw_sp, u16 fid, bool valid)
-{
-	enum mlxsw_reg_svfa_mt mt = MLXSW_REG_SVFA_MT_VID_TO_FID;
-	char svfa_pl[MLXSW_REG_SVFA_LEN];
-
-	mlxsw_reg_svfa_pack(svfa_pl, 0, mt, valid, fid, fid);
-	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(svfa), svfa_pl);
-}
-
-static struct mlxsw_sp_fid *mlxsw_sp_fid_alloc(u16 fid)
+static int
+mlxsw_sp_port_vlan_fid_join(struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan,
+			    struct mlxsw_sp_bridge_port *bridge_port)
 {
-	struct mlxsw_sp_fid *f;
-
-	f = kzalloc(sizeof(*f), GFP_KERNEL);
-	if (!f)
-		return NULL;
+	struct mlxsw_sp_port *mlxsw_sp_port = mlxsw_sp_port_vlan->mlxsw_sp_port;
+	struct mlxsw_sp_bridge_device *bridge_device;
+	u8 local_port = mlxsw_sp_port->local_port;
+	u16 vid = mlxsw_sp_port_vlan->vid;
+	struct mlxsw_sp_fid *fid;
+	int err;
 
-	f->fid = fid;
+	bridge_device = bridge_port->bridge_device;
+	fid = bridge_device->ops->fid_get(bridge_device, vid);
+	if (IS_ERR(fid))
+		return PTR_ERR(fid);
 
-	return f;
-}
-
-struct mlxsw_sp_fid *mlxsw_sp_fid_create(struct mlxsw_sp *mlxsw_sp, u16 fid)
-{
-	struct mlxsw_sp_fid *f;
-	int err;
+	err = mlxsw_sp_fid_flood_set(fid, MLXSW_SP_FLOOD_TYPE_UC, local_port,
+				     bridge_port->flags & BR_FLOOD);
+	if (err)
+		goto err_fid_uc_flood_set;
 
-	err = mlxsw_sp_fid_op(mlxsw_sp, fid, true);
+	err = mlxsw_sp_fid_flood_set(fid, MLXSW_SP_FLOOD_TYPE_MC, local_port,
+				     mlxsw_sp_mc_flood(bridge_port));
 	if (err)
-		return ERR_PTR(err);
+		goto err_fid_mc_flood_set;
 
-	/* Although all the ports member in the FID might be using a
-	 * {Port, VID} to FID mapping, we create a global VID-to-FID
-	 * mapping. This allows a port to transition to VLAN mode,
-	 * knowing the global mapping exists.
-	 */
-	err = mlxsw_sp_fid_map(mlxsw_sp, fid, true);
+	err = mlxsw_sp_fid_flood_set(fid, MLXSW_SP_FLOOD_TYPE_BC, local_port,
+				     true);
 	if (err)
-		goto err_fid_map;
+		goto err_fid_bc_flood_set;
 
-	f = mlxsw_sp_fid_alloc(fid);
-	if (!f) {
-		err = -ENOMEM;
-		goto err_allocate_fid;
-	}
+	err = mlxsw_sp_fid_port_vid_map(fid, mlxsw_sp_port, vid);
+	if (err)
+		goto err_fid_port_vid_map;
 
-	list_add(&f->list, &mlxsw_sp->fids);
+	mlxsw_sp_port_vlan->fid = fid;
 
-	return f;
+	return 0;
 
-err_allocate_fid:
-	mlxsw_sp_fid_map(mlxsw_sp, fid, false);
-err_fid_map:
-	mlxsw_sp_fid_op(mlxsw_sp, fid, false);
-	return ERR_PTR(err);
+err_fid_port_vid_map:
+	mlxsw_sp_fid_flood_set(fid, MLXSW_SP_FLOOD_TYPE_BC, local_port, false);
+err_fid_bc_flood_set:
+	mlxsw_sp_fid_flood_set(fid, MLXSW_SP_FLOOD_TYPE_MC, local_port, false);
+err_fid_mc_flood_set:
+	mlxsw_sp_fid_flood_set(fid, MLXSW_SP_FLOOD_TYPE_UC, local_port, false);
+err_fid_uc_flood_set:
+	mlxsw_sp_fid_put(fid);
+	return err;
 }
 
-void mlxsw_sp_fid_destroy(struct mlxsw_sp *mlxsw_sp, struct mlxsw_sp_fid *f)
+static void
+mlxsw_sp_port_vlan_fid_leave(struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan)
 {
-	u16 fid = f->fid;
+	struct mlxsw_sp_port *mlxsw_sp_port = mlxsw_sp_port_vlan->mlxsw_sp_port;
+	struct mlxsw_sp_fid *fid = mlxsw_sp_port_vlan->fid;
+	u8 local_port = mlxsw_sp_port->local_port;
+	u16 vid = mlxsw_sp_port_vlan->vid;
 
-	list_del(&f->list);
-
-	if (f->rif)
-		mlxsw_sp_rif_bridge_destroy(mlxsw_sp, f->rif);
-
-	kfree(f);
-
-	mlxsw_sp_fid_map(mlxsw_sp, fid, false);
+	mlxsw_sp_port_vlan->fid = NULL;
+	mlxsw_sp_fid_port_vid_unmap(fid, mlxsw_sp_port, vid);
+	mlxsw_sp_fid_flood_set(fid, MLXSW_SP_FLOOD_TYPE_BC, local_port, false);
+	mlxsw_sp_fid_flood_set(fid, MLXSW_SP_FLOOD_TYPE_MC, local_port, false);
+	mlxsw_sp_fid_flood_set(fid, MLXSW_SP_FLOOD_TYPE_UC, local_port, false);
+	mlxsw_sp_fid_put(fid);
+}
 
-	mlxsw_sp_fid_op(mlxsw_sp, fid, false);
+static u16
+mlxsw_sp_port_pvid_determine(const struct mlxsw_sp_port *mlxsw_sp_port,
+			     u16 vid, bool is_pvid)
+{
+	if (is_pvid)
+		return vid;
+	else if (mlxsw_sp_port->pvid == vid)
+		return 0;	/* Dis-allow untagged packets */
+	else
+		return mlxsw_sp_port->pvid;
 }
 
-static int __mlxsw_sp_port_fid_join(struct mlxsw_sp_port *mlxsw_sp_port,
-				    u16 fid)
+static int
+mlxsw_sp_port_vlan_bridge_join(struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan,
+			       struct mlxsw_sp_bridge_port *bridge_port)
 {
-	struct mlxsw_sp_fid *f;
+	struct mlxsw_sp_port *mlxsw_sp_port = mlxsw_sp_port_vlan->mlxsw_sp_port;
+	struct mlxsw_sp_bridge_vlan *bridge_vlan;
+	u16 vid = mlxsw_sp_port_vlan->vid;
+	int err;
 
-	if (test_bit(fid, mlxsw_sp_port->active_vlans))
+	/* No need to continue if only VLAN flags were changed */
+	if (mlxsw_sp_port_vlan->bridge_port)
 		return 0;
 
-	f = mlxsw_sp_fid_find(mlxsw_sp_port->mlxsw_sp, fid);
-	if (!f) {
-		f = mlxsw_sp_fid_create(mlxsw_sp_port->mlxsw_sp, fid);
-		if (IS_ERR(f))
-			return PTR_ERR(f);
+	err = mlxsw_sp_port_vlan_fid_join(mlxsw_sp_port_vlan, bridge_port);
+	if (err)
+		return err;
+
+	err = mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid,
+					     bridge_port->flags & BR_LEARNING);
+	if (err)
+		goto err_port_vid_learning_set;
+
+	err = mlxsw_sp_port_vid_stp_set(mlxsw_sp_port, vid,
+					bridge_port->stp_state);
+	if (err)
+		goto err_port_vid_stp_set;
+
+	bridge_vlan = mlxsw_sp_bridge_vlan_get(bridge_port, vid);
+	if (!bridge_vlan) {
+		err = -ENOMEM;
+		goto err_bridge_vlan_get;
 	}
 
-	f->ref_count++;
+	list_add(&mlxsw_sp_port_vlan->bridge_vlan_node,
+		 &bridge_vlan->port_vlan_list);
 
-	netdev_dbg(mlxsw_sp_port->dev, "Joined FID=%d\n", fid);
+	mlxsw_sp_bridge_port_get(mlxsw_sp_port->mlxsw_sp->bridge,
+				 bridge_port->dev);
+	mlxsw_sp_port_vlan->bridge_port = bridge_port;
 
 	return 0;
+
+err_bridge_vlan_get:
+	mlxsw_sp_port_vid_stp_set(mlxsw_sp_port, vid, BR_STATE_DISABLED);
+err_port_vid_stp_set:
+	mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, false);
+err_port_vid_learning_set:
+	mlxsw_sp_port_vlan_fid_leave(mlxsw_sp_port_vlan);
+	return err;
 }
 
-static void __mlxsw_sp_port_fid_leave(struct mlxsw_sp_port *mlxsw_sp_port,
-				      u16 fid)
+void
+mlxsw_sp_port_vlan_bridge_leave(struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan)
 {
-	struct mlxsw_sp_fid *f;
+	struct mlxsw_sp_port *mlxsw_sp_port = mlxsw_sp_port_vlan->mlxsw_sp_port;
+	struct mlxsw_sp_fid *fid = mlxsw_sp_port_vlan->fid;
+	struct mlxsw_sp_bridge_vlan *bridge_vlan;
+	struct mlxsw_sp_bridge_port *bridge_port;
+	u16 vid = mlxsw_sp_port_vlan->vid;
+	bool last;
 
-	f = mlxsw_sp_fid_find(mlxsw_sp_port->mlxsw_sp, fid);
-	if (WARN_ON(!f))
+	if (WARN_ON(mlxsw_sp_fid_type(fid) != MLXSW_SP_FID_TYPE_8021Q &&
+		    mlxsw_sp_fid_type(fid) != MLXSW_SP_FID_TYPE_8021D))
 		return;
 
-	netdev_dbg(mlxsw_sp_port->dev, "Left FID=%d\n", fid);
+	bridge_port = mlxsw_sp_port_vlan->bridge_port;
+	bridge_vlan = mlxsw_sp_bridge_vlan_find(bridge_port, vid);
+	last = list_is_singular(&bridge_vlan->port_vlan_list);
 
-	mlxsw_sp_port_fdb_flush(mlxsw_sp_port, fid);
+	list_del(&mlxsw_sp_port_vlan->bridge_vlan_node);
+	mlxsw_sp_bridge_vlan_put(bridge_vlan);
+	mlxsw_sp_port_vid_stp_set(mlxsw_sp_port, vid, BR_STATE_DISABLED);
+	mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, false);
+	if (last)
+		mlxsw_sp_bridge_port_fdb_flush(mlxsw_sp_port->mlxsw_sp,
+					       bridge_port,
+					       mlxsw_sp_fid_index(fid));
+	mlxsw_sp_port_vlan_fid_leave(mlxsw_sp_port_vlan);
 
-	if (--f->ref_count == 0)
-		mlxsw_sp_fid_destroy(mlxsw_sp_port->mlxsw_sp, f);
+	mlxsw_sp_bridge_port_put(mlxsw_sp_port->mlxsw_sp->bridge, bridge_port);
+	mlxsw_sp_port_vlan->bridge_port = NULL;
 }
 
-static int mlxsw_sp_port_fid_map(struct mlxsw_sp_port *mlxsw_sp_port, u16 fid,
-				 bool valid)
-{
-	enum mlxsw_reg_svfa_mt mt = MLXSW_REG_SVFA_MT_PORT_VID_TO_FID;
-
-	/* If port doesn't have vPorts, then it can use the global
-	 * VID-to-FID mapping.
-	 */
-	if (list_empty(&mlxsw_sp_port->vports_list))
-		return 0;
+static int
+mlxsw_sp_bridge_port_vlan_add(struct mlxsw_sp_port *mlxsw_sp_port,
+			      struct mlxsw_sp_bridge_port *bridge_port,
+			      u16 vid, bool is_untagged, bool is_pvid)
+{
+	u16 pvid = mlxsw_sp_port_pvid_determine(mlxsw_sp_port, vid, is_pvid);
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
+	struct mlxsw_sp_bridge_vlan *bridge_vlan;
+	u16 old_pvid = mlxsw_sp_port->pvid;
+	int err;
 
-	return mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_port, mt, valid, fid, fid);
-}
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_get(mlxsw_sp_port, vid);
+	if (IS_ERR(mlxsw_sp_port_vlan))
+		return PTR_ERR(mlxsw_sp_port_vlan);
 
-static int mlxsw_sp_port_fid_join(struct mlxsw_sp_port *mlxsw_sp_port,
-				  u16 fid_begin, u16 fid_end)
-{
-	bool mc_flood;
-	int fid, err;
-
-	for (fid = fid_begin; fid <= fid_end; fid++) {
-		err = __mlxsw_sp_port_fid_join(mlxsw_sp_port, fid);
-		if (err)
-			goto err_port_fid_join;
-	}
+	err = mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid, vid, true,
+				     is_untagged);
+	if (err)
+		goto err_port_vlan_set;
 
-	mc_flood = mlxsw_sp_port->mc_disabled ?
-			mlxsw_sp_port->mc_flood : mlxsw_sp_port->mc_router;
+	err = mlxsw_sp_port_pvid_set(mlxsw_sp_port, pvid);
+	if (err)
+		goto err_port_pvid_set;
 
-	err = __mlxsw_sp_port_flood_set(mlxsw_sp_port, fid_begin, fid_end,
-					mlxsw_sp_port->uc_flood, true,
-					mc_flood);
+	err = mlxsw_sp_port_vlan_bridge_join(mlxsw_sp_port_vlan, bridge_port);
 	if (err)
-		goto err_port_flood_set;
+		goto err_port_vlan_bridge_join;
 
-	for (fid = fid_begin; fid <= fid_end; fid++) {
-		err = mlxsw_sp_port_fid_map(mlxsw_sp_port, fid, true);
-		if (err)
-			goto err_port_fid_map;
-	}
+	bridge_vlan = mlxsw_sp_bridge_vlan_find(bridge_port, vid);
 
 	return 0;
 
-err_port_fid_map:
-	for (fid--; fid >= fid_begin; fid--)
-		mlxsw_sp_port_fid_map(mlxsw_sp_port, fid, false);
-	__mlxsw_sp_port_flood_set(mlxsw_sp_port, fid_begin, fid_end, false,
-				  false, false);
-err_port_flood_set:
-	fid = fid_end;
-err_port_fid_join:
-	for (fid--; fid >= fid_begin; fid--)
-		__mlxsw_sp_port_fid_leave(mlxsw_sp_port, fid);
+err_port_vlan_bridge_join:
+	mlxsw_sp_port_pvid_set(mlxsw_sp_port, old_pvid);
+err_port_pvid_set:
+	mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid, vid, false, false);
+err_port_vlan_set:
+	mlxsw_sp_port_vlan_put(mlxsw_sp_port_vlan);
 	return err;
 }
 
-static void mlxsw_sp_port_fid_leave(struct mlxsw_sp_port *mlxsw_sp_port,
-				    u16 fid_begin, u16 fid_end)
-{
-	int fid;
-
-	for (fid = fid_begin; fid <= fid_end; fid++)
-		mlxsw_sp_port_fid_map(mlxsw_sp_port, fid, false);
-
-	__mlxsw_sp_port_flood_set(mlxsw_sp_port, fid_begin, fid_end, false,
-				  false, false);
-
-	for (fid = fid_begin; fid <= fid_end; fid++)
-		__mlxsw_sp_port_fid_leave(mlxsw_sp_port, fid);
-}
-
-static int __mlxsw_sp_port_pvid_set(struct mlxsw_sp_port *mlxsw_sp_port,
-				    u16 vid)
-{
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	char spvid_pl[MLXSW_REG_SPVID_LEN];
-
-	mlxsw_reg_spvid_pack(spvid_pl, mlxsw_sp_port->local_port, vid);
-	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(spvid), spvid_pl);
-}
-
-static int mlxsw_sp_port_allow_untagged_set(struct mlxsw_sp_port *mlxsw_sp_port,
-					    bool allow)
+static int mlxsw_sp_port_vlans_add(struct mlxsw_sp_port *mlxsw_sp_port,
+				   const struct switchdev_obj_port_vlan *vlan,
+				   struct switchdev_trans *trans)
 {
+	bool flag_untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
+	bool flag_pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	char spaft_pl[MLXSW_REG_SPAFT_LEN];
-
-	mlxsw_reg_spaft_pack(spaft_pl, mlxsw_sp_port->local_port, allow);
-	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(spaft), spaft_pl);
-}
-
-int mlxsw_sp_port_pvid_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
-{
-	struct net_device *dev = mlxsw_sp_port->dev;
-	int err;
-
-	if (!vid) {
-		err = mlxsw_sp_port_allow_untagged_set(mlxsw_sp_port, false);
-		if (err) {
-			netdev_err(dev, "Failed to disallow untagged traffic\n");
-			return err;
-		}
-	} else {
-		err = __mlxsw_sp_port_pvid_set(mlxsw_sp_port, vid);
-		if (err) {
-			netdev_err(dev, "Failed to set PVID\n");
-			return err;
-		}
-
-		/* Only allow if not already allowed. */
-		if (!mlxsw_sp_port->pvid) {
-			err = mlxsw_sp_port_allow_untagged_set(mlxsw_sp_port,
-							       true);
-			if (err) {
-				netdev_err(dev, "Failed to allow untagged traffic\n");
-				goto err_port_allow_untagged_set;
-			}
-		}
-	}
+	struct net_device *orig_dev = vlan->obj.orig_dev;
+	struct mlxsw_sp_bridge_port *bridge_port;
+	u16 vid;
 
-	mlxsw_sp_port->pvid = vid;
-	return 0;
+	if (switchdev_trans_ph_prepare(trans))
+		return 0;
 
-err_port_allow_untagged_set:
-	__mlxsw_sp_port_pvid_set(mlxsw_sp_port, mlxsw_sp_port->pvid);
-	return err;
-}
+	bridge_port = mlxsw_sp_bridge_port_find(mlxsw_sp->bridge, orig_dev);
+	if (WARN_ON(!bridge_port))
+		return -EINVAL;
 
-static int mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
-					  u16 vid_begin, u16 vid_end,
-					  bool learn_enable)
-{
-	u16 vid, vid_e;
-	int err;
+	if (!bridge_port->bridge_device->vlan_enabled)
+		return 0;
 
-	for (vid = vid_begin; vid <= vid_end;
-	     vid += MLXSW_REG_SPVMLR_REC_MAX_COUNT) {
-		vid_e = min((u16) (vid + MLXSW_REG_SPVMLR_REC_MAX_COUNT - 1),
-			    vid_end);
+	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
+		int err;
 
-		err = __mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid,
-						       vid_e, learn_enable);
+		err = mlxsw_sp_bridge_port_vlan_add(mlxsw_sp_port, bridge_port,
+						    vid, flag_untagged,
+						    flag_pvid);
 		if (err)
 			return err;
 	}
@@ -766,102 +1047,27 @@ static int mlxsw_sp_port_vid_learning_set(struct mlxsw_sp_port *mlxsw_sp_port,
 	return 0;
 }
 
-static int __mlxsw_sp_port_vlans_add(struct mlxsw_sp_port *mlxsw_sp_port,
-				     u16 vid_begin, u16 vid_end,
-				     bool flag_untagged, bool flag_pvid)
+static enum mlxsw_reg_sfdf_flush_type mlxsw_sp_fdb_flush_type(bool lagged)
 {
-	struct net_device *dev = mlxsw_sp_port->dev;
-	u16 vid, old_pvid;
-	int err;
-
-	if (!mlxsw_sp_port->bridged)
-		return -EINVAL;
-
-	err = mlxsw_sp_port_fid_join(mlxsw_sp_port, vid_begin, vid_end);
-	if (err) {
-		netdev_err(dev, "Failed to join FIDs\n");
-		return err;
-	}
-
-	err = mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid_begin, vid_end,
-				     true, flag_untagged);
-	if (err) {
-		netdev_err(dev, "Unable to add VIDs %d-%d\n", vid_begin,
-			   vid_end);
-		goto err_port_vlans_set;
-	}
-
-	old_pvid = mlxsw_sp_port->pvid;
-	if (flag_pvid && old_pvid != vid_begin) {
-		err = mlxsw_sp_port_pvid_set(mlxsw_sp_port, vid_begin);
-		if (err) {
-			netdev_err(dev, "Unable to add PVID %d\n", vid_begin);
-			goto err_port_pvid_set;
-		}
-	} else if (!flag_pvid && old_pvid >= vid_begin && old_pvid <= vid_end) {
-		err = mlxsw_sp_port_pvid_set(mlxsw_sp_port, 0);
-		if (err) {
-			netdev_err(dev, "Unable to del PVID\n");
-			goto err_port_pvid_set;
-		}
-	}
-
-	err = mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid_begin, vid_end,
-					     mlxsw_sp_port->learning);
-	if (err) {
-		netdev_err(dev, "Failed to set learning for VIDs %d-%d\n",
-			   vid_begin, vid_end);
-		goto err_port_vid_learning_set;
-	}
-
-	/* Changing activity bits only if HW operation succeded */
-	for (vid = vid_begin; vid <= vid_end; vid++) {
-		set_bit(vid, mlxsw_sp_port->active_vlans);
-		if (flag_untagged)
-			set_bit(vid, mlxsw_sp_port->untagged_vlans);
-		else
-			clear_bit(vid, mlxsw_sp_port->untagged_vlans);
-	}
-
-	/* STP state change must be done after we set active VLANs */
-	err = mlxsw_sp_port_stp_state_set(mlxsw_sp_port,
-					  mlxsw_sp_port->stp_state);
-	if (err) {
-		netdev_err(dev, "Failed to set STP state\n");
-		goto err_port_stp_state_set;
-	}
-
-	return 0;
-
-err_port_stp_state_set:
-	for (vid = vid_begin; vid <= vid_end; vid++)
-		clear_bit(vid, mlxsw_sp_port->active_vlans);
-	mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid_begin, vid_end,
-				       false);
-err_port_vid_learning_set:
-	if (old_pvid != mlxsw_sp_port->pvid)
-		mlxsw_sp_port_pvid_set(mlxsw_sp_port, old_pvid);
-err_port_pvid_set:
-	mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid_begin, vid_end,
-			       false, false);
-err_port_vlans_set:
-	mlxsw_sp_port_fid_leave(mlxsw_sp_port, vid_begin, vid_end);
-	return err;
+	return lagged ? MLXSW_REG_SFDF_FLUSH_PER_LAG_AND_FID :
+			MLXSW_REG_SFDF_FLUSH_PER_PORT_AND_FID;
 }
 
-static int mlxsw_sp_port_vlans_add(struct mlxsw_sp_port *mlxsw_sp_port,
-				   const struct switchdev_obj_port_vlan *vlan,
-				   struct switchdev_trans *trans)
+static int
+mlxsw_sp_bridge_port_fdb_flush(struct mlxsw_sp *mlxsw_sp,
+			       struct mlxsw_sp_bridge_port *bridge_port,
+			       u16 fid_index)
 {
-	bool flag_untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
-	bool flag_pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
+	bool lagged = bridge_port->lagged;
+	char sfdf_pl[MLXSW_REG_SFDF_LEN];
+	u16 system_port;
 
-	if (switchdev_trans_ph_prepare(trans))
-		return 0;
+	system_port = lagged ? bridge_port->lag_id : bridge_port->system_port;
+	mlxsw_reg_sfdf_pack(sfdf_pl, mlxsw_sp_fdb_flush_type(lagged));
+	mlxsw_reg_sfdf_fid_set(sfdf_pl, fid_index);
+	mlxsw_reg_sfdf_port_fid_system_port_set(sfdf_pl, system_port);
 
-	return __mlxsw_sp_port_vlans_add(mlxsw_sp_port,
-					 vlan->vid_begin, vlan->vid_end,
-					 flag_untagged, flag_pvid);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfdf), sfdf_pl);
 }
 
 static enum mlxsw_reg_sfd_rec_policy mlxsw_sp_sfd_rec_policy(bool dynamic)
@@ -935,29 +1141,40 @@ static int mlxsw_sp_port_fdb_uc_lag_op(struct mlxsw_sp *mlxsw_sp, u16 lag_id,
 }
 
 static int
-mlxsw_sp_port_fdb_static_add(struct mlxsw_sp_port *mlxsw_sp_port,
-			     const struct switchdev_obj_port_fdb *fdb,
-			     struct switchdev_trans *trans)
+mlxsw_sp_port_fdb_set(struct mlxsw_sp_port *mlxsw_sp_port,
+		      struct switchdev_notifier_fdb_info *fdb_info, bool adding)
 {
-	u16 fid = mlxsw_sp_port_vid_to_fid_get(mlxsw_sp_port, fdb->vid);
-	u16 lag_vid = 0;
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct net_device *orig_dev = fdb_info->info.dev;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
+	struct mlxsw_sp_bridge_device *bridge_device;
+	struct mlxsw_sp_bridge_port *bridge_port;
+	u16 fid_index, vid;
+
+	bridge_port = mlxsw_sp_bridge_port_find(mlxsw_sp->bridge, orig_dev);
+	if (!bridge_port)
+		return -EINVAL;
 
-	if (switchdev_trans_ph_prepare(trans))
+	bridge_device = bridge_port->bridge_device;
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_bridge(mlxsw_sp_port,
+							       bridge_device,
+							       fdb_info->vid);
+	if (!mlxsw_sp_port_vlan)
 		return 0;
 
-	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
-		lag_vid = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
-	}
+	fid_index = mlxsw_sp_fid_index(mlxsw_sp_port_vlan->fid);
+	vid = mlxsw_sp_port_vlan->vid;
 
-	if (!mlxsw_sp_port->lagged)
-		return mlxsw_sp_port_fdb_uc_op(mlxsw_sp_port->mlxsw_sp,
-					       mlxsw_sp_port->local_port,
-					       fdb->addr, fid, true, false);
+	if (!bridge_port->lagged)
+		return mlxsw_sp_port_fdb_uc_op(mlxsw_sp,
+					       bridge_port->system_port,
+					       fdb_info->addr, fid_index,
+					       adding, false);
 	else
-		return mlxsw_sp_port_fdb_uc_lag_op(mlxsw_sp_port->mlxsw_sp,
-						   mlxsw_sp_port->lag_id,
-						   fdb->addr, fid, lag_vid,
-						   true, false);
+		return mlxsw_sp_port_fdb_uc_lag_op(mlxsw_sp,
+						   bridge_port->lag_id,
+						   fdb_info->addr, fid_index,
+						   vid, adding, false);
 }
 
 static int mlxsw_sp_port_mdb_op(struct mlxsw_sp *mlxsw_sp, const char *addr,
@@ -1006,7 +1223,7 @@ static struct mlxsw_sp_mid *__mlxsw_sp_mc_get(struct mlxsw_sp *mlxsw_sp,
 {
 	struct mlxsw_sp_mid *mid;
 
-	list_for_each_entry(mid, &mlxsw_sp->br_mids.list, list) {
+	list_for_each_entry(mid, &mlxsw_sp->bridge->mids_list, list) {
 		if (ether_addr_equal(mid->addr, addr) && mid->fid == fid)
 			return mid;
 	}
@@ -1020,7 +1237,7 @@ static struct mlxsw_sp_mid *__mlxsw_sp_mc_alloc(struct mlxsw_sp *mlxsw_sp,
 	struct mlxsw_sp_mid *mid;
 	u16 mid_idx;
 
-	mid_idx = find_first_zero_bit(mlxsw_sp->br_mids.mapped,
+	mid_idx = find_first_zero_bit(mlxsw_sp->bridge->mids_bitmap,
 				      MLXSW_SP_MID_MAX);
 	if (mid_idx == MLXSW_SP_MID_MAX)
 		return NULL;
@@ -1029,12 +1246,12 @@ static struct mlxsw_sp_mid *__mlxsw_sp_mc_alloc(struct mlxsw_sp *mlxsw_sp,
 	if (!mid)
 		return NULL;
 
-	set_bit(mid_idx, mlxsw_sp->br_mids.mapped);
+	set_bit(mid_idx, mlxsw_sp->bridge->mids_bitmap);
 	ether_addr_copy(mid->addr, addr);
 	mid->fid = fid;
 	mid->mid = mid_idx;
 	mid->ref_count = 0;
-	list_add_tail(&mid->list, &mlxsw_sp->br_mids.list);
+	list_add_tail(&mid->list, &mlxsw_sp->bridge->mids_list);
 
 	return mid;
 }
@@ -1044,7 +1261,7 @@ static int __mlxsw_sp_mc_dec_ref(struct mlxsw_sp *mlxsw_sp,
 {
 	if (--mid->ref_count == 0) {
 		list_del(&mid->list);
-		clear_bit(mid->mid, mlxsw_sp->br_mids.mapped);
+		clear_bit(mid->mid, mlxsw_sp->bridge->mids_bitmap);
 		kfree(mid);
 		return 1;
 	}
@@ -1056,17 +1273,34 @@ static int mlxsw_sp_port_mdb_add(struct mlxsw_sp_port *mlxsw_sp_port,
 				 struct switchdev_trans *trans)
 {
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct net_device *orig_dev = mdb->obj.orig_dev;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 	struct net_device *dev = mlxsw_sp_port->dev;
+	struct mlxsw_sp_bridge_device *bridge_device;
+	struct mlxsw_sp_bridge_port *bridge_port;
 	struct mlxsw_sp_mid *mid;
-	u16 fid = mlxsw_sp_port_vid_to_fid_get(mlxsw_sp_port, mdb->vid);
+	u16 fid_index;
 	int err = 0;
 
 	if (switchdev_trans_ph_prepare(trans))
 		return 0;
 
-	mid = __mlxsw_sp_mc_get(mlxsw_sp, mdb->addr, fid);
+	bridge_port = mlxsw_sp_bridge_port_find(mlxsw_sp->bridge, orig_dev);
+	if (WARN_ON(!bridge_port))
+		return -EINVAL;
+
+	bridge_device = bridge_port->bridge_device;
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_bridge(mlxsw_sp_port,
+							       bridge_device,
+							       mdb->vid);
+	if (WARN_ON(!mlxsw_sp_port_vlan))
+		return -EINVAL;
+
+	fid_index = mlxsw_sp_fid_index(mlxsw_sp_port_vlan->fid);
+
+	mid = __mlxsw_sp_mc_get(mlxsw_sp, mdb->addr, fid_index);
 	if (!mid) {
-		mid = __mlxsw_sp_mc_alloc(mlxsw_sp, mdb->addr, fid);
+		mid = __mlxsw_sp_mc_alloc(mlxsw_sp, mdb->addr, fid_index);
 		if (!mid) {
 			netdev_err(dev, "Unable to allocate MC group\n");
 			return -ENOMEM;
@@ -1082,8 +1316,8 @@ static int mlxsw_sp_port_mdb_add(struct mlxsw_sp_port *mlxsw_sp_port,
 	}
 
 	if (mid->ref_count == 1) {
-		err = mlxsw_sp_port_mdb_op(mlxsw_sp, mdb->addr, fid, mid->mid,
-					   true);
+		err = mlxsw_sp_port_mdb_op(mlxsw_sp, mdb->addr, fid_index,
+					   mid->mid, true);
 		if (err) {
 			netdev_err(dev, "Unable to set MC SFD\n");
 			goto err_out;
@@ -1104,24 +1338,12 @@ static int mlxsw_sp_port_obj_add(struct net_device *dev,
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	int err = 0;
 
-	mlxsw_sp_port = mlxsw_sp_port_orig_get(obj->orig_dev, mlxsw_sp_port);
-	if (!mlxsw_sp_port)
-		return -EINVAL;
-
 	switch (obj->id) {
 	case SWITCHDEV_OBJ_ID_PORT_VLAN:
-		if (mlxsw_sp_port_is_vport(mlxsw_sp_port))
-			return 0;
-
 		err = mlxsw_sp_port_vlans_add(mlxsw_sp_port,
 					      SWITCHDEV_OBJ_PORT_VLAN(obj),
 					      trans);
 		break;
-	case SWITCHDEV_OBJ_ID_PORT_FDB:
-		err = mlxsw_sp_port_fdb_static_add(mlxsw_sp_port,
-						   SWITCHDEV_OBJ_PORT_FDB(obj),
-						   trans);
-		break;
 	case SWITCHDEV_OBJ_ID_PORT_MDB:
 		err = mlxsw_sp_port_mdb_add(mlxsw_sp_port,
 					    SWITCHDEV_OBJ_PORT_MDB(obj),
@@ -1135,82 +1357,72 @@ static int mlxsw_sp_port_obj_add(struct net_device *dev,
 	return err;
 }
 
-static int __mlxsw_sp_port_vlans_del(struct mlxsw_sp_port *mlxsw_sp_port,
-				     u16 vid_begin, u16 vid_end)
+static void
+mlxsw_sp_bridge_port_vlan_del(struct mlxsw_sp_port *mlxsw_sp_port,
+			      struct mlxsw_sp_bridge_port *bridge_port, u16 vid)
 {
-	u16 vid, pvid;
-
-	if (!mlxsw_sp_port->bridged)
-		return -EINVAL;
-
-	mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid_begin, vid_end,
-				       false);
-
-	pvid = mlxsw_sp_port->pvid;
-	if (pvid >= vid_begin && pvid <= vid_end)
-		mlxsw_sp_port_pvid_set(mlxsw_sp_port, 0);
-
-	mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid_begin, vid_end,
-			       false, false);
+	u16 pvid = mlxsw_sp_port->pvid == vid ? 0 : vid;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 
-	mlxsw_sp_port_fid_leave(mlxsw_sp_port, vid_begin, vid_end);
-
-	/* Changing activity bits only if HW operation succeded */
-	for (vid = vid_begin; vid <= vid_end; vid++)
-		clear_bit(vid, mlxsw_sp_port->active_vlans);
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_vid(mlxsw_sp_port, vid);
+	if (WARN_ON(!mlxsw_sp_port_vlan))
+		return;
 
-	return 0;
+	mlxsw_sp_port_vlan_bridge_leave(mlxsw_sp_port_vlan);
+	mlxsw_sp_port_pvid_set(mlxsw_sp_port, pvid);
+	mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid, vid, false, false);
+	mlxsw_sp_port_vlan_put(mlxsw_sp_port_vlan);
 }
 
 static int mlxsw_sp_port_vlans_del(struct mlxsw_sp_port *mlxsw_sp_port,
 				   const struct switchdev_obj_port_vlan *vlan)
 {
-	return __mlxsw_sp_port_vlans_del(mlxsw_sp_port, vlan->vid_begin,
-					 vlan->vid_end);
-}
-
-void mlxsw_sp_port_active_vlans_del(struct mlxsw_sp_port *mlxsw_sp_port)
-{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct net_device *orig_dev = vlan->obj.orig_dev;
+	struct mlxsw_sp_bridge_port *bridge_port;
 	u16 vid;
 
-	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID)
-		__mlxsw_sp_port_vlans_del(mlxsw_sp_port, vid, vid);
-}
+	bridge_port = mlxsw_sp_bridge_port_find(mlxsw_sp->bridge, orig_dev);
+	if (WARN_ON(!bridge_port))
+		return -EINVAL;
 
-static int
-mlxsw_sp_port_fdb_static_del(struct mlxsw_sp_port *mlxsw_sp_port,
-			     const struct switchdev_obj_port_fdb *fdb)
-{
-	u16 fid = mlxsw_sp_port_vid_to_fid_get(mlxsw_sp_port, fdb->vid);
-	u16 lag_vid = 0;
+	if (!bridge_port->bridge_device->vlan_enabled)
+		return 0;
 
-	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
-		lag_vid = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
-	}
+	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++)
+		mlxsw_sp_bridge_port_vlan_del(mlxsw_sp_port, bridge_port, vid);
 
-	if (!mlxsw_sp_port->lagged)
-		return mlxsw_sp_port_fdb_uc_op(mlxsw_sp_port->mlxsw_sp,
-					       mlxsw_sp_port->local_port,
-					       fdb->addr, fid,
-					       false, false);
-	else
-		return mlxsw_sp_port_fdb_uc_lag_op(mlxsw_sp_port->mlxsw_sp,
-						   mlxsw_sp_port->lag_id,
-						   fdb->addr, fid, lag_vid,
-						   false, false);
+	return 0;
 }
 
 static int mlxsw_sp_port_mdb_del(struct mlxsw_sp_port *mlxsw_sp_port,
 				 const struct switchdev_obj_port_mdb *mdb)
 {
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct net_device *orig_dev = mdb->obj.orig_dev;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
+	struct mlxsw_sp_bridge_device *bridge_device;
 	struct net_device *dev = mlxsw_sp_port->dev;
+	struct mlxsw_sp_bridge_port *bridge_port;
 	struct mlxsw_sp_mid *mid;
-	u16 fid = mlxsw_sp_port_vid_to_fid_get(mlxsw_sp_port, mdb->vid);
+	u16 fid_index;
 	u16 mid_idx;
 	int err = 0;
 
-	mid = __mlxsw_sp_mc_get(mlxsw_sp, mdb->addr, fid);
+	bridge_port = mlxsw_sp_bridge_port_find(mlxsw_sp->bridge, orig_dev);
+	if (WARN_ON(!bridge_port))
+		return -EINVAL;
+
+	bridge_device = bridge_port->bridge_device;
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_bridge(mlxsw_sp_port,
+							       bridge_device,
+							       mdb->vid);
+	if (WARN_ON(!mlxsw_sp_port_vlan))
+		return -EINVAL;
+
+	fid_index = mlxsw_sp_fid_index(mlxsw_sp_port_vlan->fid);
+
+	mid = __mlxsw_sp_mc_get(mlxsw_sp, mdb->addr, fid_index);
 	if (!mid) {
 		netdev_err(dev, "Unable to remove port from MC DB\n");
 		return -EINVAL;
@@ -1222,8 +1434,8 @@ static int mlxsw_sp_port_mdb_del(struct mlxsw_sp_port *mlxsw_sp_port,
 
 	mid_idx = mid->mid;
 	if (__mlxsw_sp_mc_dec_ref(mlxsw_sp, mid)) {
-		err = mlxsw_sp_port_mdb_op(mlxsw_sp, mdb->addr, fid, mid_idx,
-					   false);
+		err = mlxsw_sp_port_mdb_op(mlxsw_sp, mdb->addr, fid_index,
+					   mid_idx, false);
 		if (err)
 			netdev_err(dev, "Unable to remove MC SFD\n");
 	}
@@ -1237,22 +1449,11 @@ static int mlxsw_sp_port_obj_del(struct net_device *dev,
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	int err = 0;
 
-	mlxsw_sp_port = mlxsw_sp_port_orig_get(obj->orig_dev, mlxsw_sp_port);
-	if (!mlxsw_sp_port)
-		return -EINVAL;
-
 	switch (obj->id) {
 	case SWITCHDEV_OBJ_ID_PORT_VLAN:
-		if (mlxsw_sp_port_is_vport(mlxsw_sp_port))
-			return 0;
-
 		err = mlxsw_sp_port_vlans_del(mlxsw_sp_port,
 					      SWITCHDEV_OBJ_PORT_VLAN(obj));
 		break;
-	case SWITCHDEV_OBJ_ID_PORT_FDB:
-		err = mlxsw_sp_port_fdb_static_del(mlxsw_sp_port,
-						   SWITCHDEV_OBJ_PORT_FDB(obj));
-		break;
 	case SWITCHDEV_OBJ_ID_PORT_MDB:
 		err = mlxsw_sp_port_mdb_del(mlxsw_sp_port,
 					    SWITCHDEV_OBJ_PORT_MDB(obj));
@@ -1282,188 +1483,200 @@ static struct mlxsw_sp_port *mlxsw_sp_lag_rep_port(struct mlxsw_sp *mlxsw_sp,
 	return NULL;
 }
 
-static int mlxsw_sp_port_fdb_dump(struct mlxsw_sp_port *mlxsw_sp_port,
-				  struct switchdev_obj_port_fdb *fdb,
-				  switchdev_obj_dump_cb_t *cb,
-				  struct net_device *orig_dev)
+static const struct switchdev_ops mlxsw_sp_port_switchdev_ops = {
+	.switchdev_port_attr_get	= mlxsw_sp_port_attr_get,
+	.switchdev_port_attr_set	= mlxsw_sp_port_attr_set,
+	.switchdev_port_obj_add		= mlxsw_sp_port_obj_add,
+	.switchdev_port_obj_del		= mlxsw_sp_port_obj_del,
+};
+
+static int
+mlxsw_sp_bridge_8021q_port_join(struct mlxsw_sp_bridge_device *bridge_device,
+				struct mlxsw_sp_bridge_port *bridge_port,
+				struct mlxsw_sp_port *mlxsw_sp_port)
 {
-	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	struct mlxsw_sp_port *tmp;
-	struct mlxsw_sp_fid *f;
-	u16 vport_fid;
-	char *sfd_pl;
-	char mac[ETH_ALEN];
-	u16 fid;
-	u8 local_port;
-	u16 lag_id;
-	u8 num_rec;
-	int stored_err = 0;
-	int i;
-	int err;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 
-	sfd_pl = kmalloc(MLXSW_REG_SFD_LEN, GFP_KERNEL);
-	if (!sfd_pl)
-		return -ENOMEM;
+	if (is_vlan_dev(bridge_port->dev))
+		return -EINVAL;
 
-	f = mlxsw_sp_vport_fid_get(mlxsw_sp_port);
-	vport_fid = f ? f->fid : 0;
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_vid(mlxsw_sp_port, 1);
+	if (WARN_ON(!mlxsw_sp_port_vlan))
+		return -EINVAL;
 
-	mlxsw_reg_sfd_pack(sfd_pl, MLXSW_REG_SFD_OP_QUERY_DUMP, 0);
-	do {
-		mlxsw_reg_sfd_num_rec_set(sfd_pl, MLXSW_REG_SFD_REC_MAX_COUNT);
-		err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(sfd), sfd_pl);
-		if (err)
-			goto out;
+	/* Let VLAN-aware bridge take care of its own VLANs */
+	mlxsw_sp_port_vlan_put(mlxsw_sp_port_vlan);
 
-		num_rec = mlxsw_reg_sfd_num_rec_get(sfd_pl);
+	return 0;
+}
 
-		/* Even in case of error, we have to run the dump to the end
-		 * so the session in firmware is finished.
-		 */
-		if (stored_err)
-			continue;
+static void
+mlxsw_sp_bridge_8021q_port_leave(struct mlxsw_sp_bridge_device *bridge_device,
+				 struct mlxsw_sp_bridge_port *bridge_port,
+				 struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	mlxsw_sp_port_vlan_get(mlxsw_sp_port, 1);
+	/* Make sure untagged frames are allowed to ingress */
+	mlxsw_sp_port_pvid_set(mlxsw_sp_port, 1);
+}
 
-		for (i = 0; i < num_rec; i++) {
-			switch (mlxsw_reg_sfd_rec_type_get(sfd_pl, i)) {
-			case MLXSW_REG_SFD_REC_TYPE_UNICAST:
-				mlxsw_reg_sfd_uc_unpack(sfd_pl, i, mac, &fid,
-							&local_port);
-				if (local_port == mlxsw_sp_port->local_port) {
-					if (vport_fid && vport_fid == fid)
-						fdb->vid = 0;
-					else if (!vport_fid &&
-						 !mlxsw_sp_fid_is_vfid(fid))
-						fdb->vid = fid;
-					else
-						continue;
-					ether_addr_copy(fdb->addr, mac);
-					fdb->ndm_state = NUD_REACHABLE;
-					err = cb(&fdb->obj);
-					if (err)
-						stored_err = err;
-				}
-				break;
-			case MLXSW_REG_SFD_REC_TYPE_UNICAST_LAG:
-				mlxsw_reg_sfd_uc_lag_unpack(sfd_pl, i,
-							    mac, &fid, &lag_id);
-				tmp = mlxsw_sp_lag_rep_port(mlxsw_sp, lag_id);
-				if (tmp && tmp->local_port ==
-				    mlxsw_sp_port->local_port) {
-					/* LAG records can only point to LAG
-					 * devices or VLAN devices on top.
-					 */
-					if (!netif_is_lag_master(orig_dev) &&
-					    !is_vlan_dev(orig_dev))
-						continue;
-					if (vport_fid && vport_fid == fid)
-						fdb->vid = 0;
-					else if (!vport_fid &&
-						 !mlxsw_sp_fid_is_vfid(fid))
-						fdb->vid = fid;
-					else
-						continue;
-					ether_addr_copy(fdb->addr, mac);
-					fdb->ndm_state = NUD_REACHABLE;
-					err = cb(&fdb->obj);
-					if (err)
-						stored_err = err;
-				}
-				break;
-			}
-		}
-	} while (num_rec == MLXSW_REG_SFD_REC_MAX_COUNT);
+static struct mlxsw_sp_fid *
+mlxsw_sp_bridge_8021q_fid_get(struct mlxsw_sp_bridge_device *bridge_device,
+			      u16 vid)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_lower_get(bridge_device->dev);
 
-out:
-	kfree(sfd_pl);
-	return stored_err ? stored_err : err;
+	return mlxsw_sp_fid_8021q_get(mlxsw_sp, vid);
 }
 
-static int mlxsw_sp_port_vlan_dump(struct mlxsw_sp_port *mlxsw_sp_port,
-				   struct switchdev_obj_port_vlan *vlan,
-				   switchdev_obj_dump_cb_t *cb)
+static const struct mlxsw_sp_bridge_ops mlxsw_sp_bridge_8021q_ops = {
+	.port_join	= mlxsw_sp_bridge_8021q_port_join,
+	.port_leave	= mlxsw_sp_bridge_8021q_port_leave,
+	.fid_get	= mlxsw_sp_bridge_8021q_fid_get,
+};
+
+static bool
+mlxsw_sp_port_is_br_member(const struct mlxsw_sp_port *mlxsw_sp_port,
+			   const struct net_device *br_dev)
 {
-	u16 vid;
-	int err = 0;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 
-	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
-		vlan->flags = 0;
-		vlan->vid_begin = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
-		vlan->vid_end = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
-		return cb(&vlan->obj);
+	list_for_each_entry(mlxsw_sp_port_vlan, &mlxsw_sp_port->vlans_list,
+			    list) {
+		if (mlxsw_sp_port_vlan->bridge_port &&
+		    mlxsw_sp_port_vlan->bridge_port->bridge_device->dev ==
+		    br_dev)
+			return true;
 	}
 
-	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID) {
-		vlan->flags = 0;
-		if (vid == mlxsw_sp_port->pvid)
-			vlan->flags |= BRIDGE_VLAN_INFO_PVID;
-		if (test_bit(vid, mlxsw_sp_port->untagged_vlans))
-			vlan->flags |= BRIDGE_VLAN_INFO_UNTAGGED;
-		vlan->vid_begin = vid;
-		vlan->vid_end = vid;
-		err = cb(&vlan->obj);
-		if (err)
-			break;
-	}
-	return err;
+	return false;
 }
 
-static int mlxsw_sp_port_obj_dump(struct net_device *dev,
-				  struct switchdev_obj *obj,
-				  switchdev_obj_dump_cb_t *cb)
+static int
+mlxsw_sp_bridge_8021d_port_join(struct mlxsw_sp_bridge_device *bridge_device,
+				struct mlxsw_sp_bridge_port *bridge_port,
+				struct mlxsw_sp_port *mlxsw_sp_port)
 {
-	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
-	int err = 0;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
+	u16 vid;
 
-	mlxsw_sp_port = mlxsw_sp_port_orig_get(obj->orig_dev, mlxsw_sp_port);
-	if (!mlxsw_sp_port)
+	if (!is_vlan_dev(bridge_port->dev))
 		return -EINVAL;
+	vid = vlan_dev_vlan_id(bridge_port->dev);
 
-	switch (obj->id) {
-	case SWITCHDEV_OBJ_ID_PORT_VLAN:
-		err = mlxsw_sp_port_vlan_dump(mlxsw_sp_port,
-					      SWITCHDEV_OBJ_PORT_VLAN(obj), cb);
-		break;
-	case SWITCHDEV_OBJ_ID_PORT_FDB:
-		err = mlxsw_sp_port_fdb_dump(mlxsw_sp_port,
-					     SWITCHDEV_OBJ_PORT_FDB(obj), cb,
-					     obj->orig_dev);
-		break;
-	default:
-		err = -EOPNOTSUPP;
-		break;
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_vid(mlxsw_sp_port, vid);
+	if (WARN_ON(!mlxsw_sp_port_vlan))
+		return -EINVAL;
+
+	if (mlxsw_sp_port_is_br_member(mlxsw_sp_port, bridge_device->dev)) {
+		netdev_err(mlxsw_sp_port->dev, "Can't bridge VLAN uppers of the same port\n");
+		return -EINVAL;
 	}
 
-	return err;
+	/* Port is no longer usable as a router interface */
+	if (mlxsw_sp_port_vlan->fid)
+		mlxsw_sp_port_vlan_router_leave(mlxsw_sp_port_vlan);
+
+	return mlxsw_sp_port_vlan_bridge_join(mlxsw_sp_port_vlan, bridge_port);
 }
 
-static const struct switchdev_ops mlxsw_sp_port_switchdev_ops = {
-	.switchdev_port_attr_get	= mlxsw_sp_port_attr_get,
-	.switchdev_port_attr_set	= mlxsw_sp_port_attr_set,
-	.switchdev_port_obj_add		= mlxsw_sp_port_obj_add,
-	.switchdev_port_obj_del		= mlxsw_sp_port_obj_del,
-	.switchdev_port_obj_dump	= mlxsw_sp_port_obj_dump,
+static void
+mlxsw_sp_bridge_8021d_port_leave(struct mlxsw_sp_bridge_device *bridge_device,
+				 struct mlxsw_sp_bridge_port *bridge_port,
+				 struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
+	u16 vid = vlan_dev_vlan_id(bridge_port->dev);
+
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_vid(mlxsw_sp_port, vid);
+	if (WARN_ON(!mlxsw_sp_port_vlan))
+		return;
+
+	mlxsw_sp_port_vlan_bridge_leave(mlxsw_sp_port_vlan);
+}
+
+static struct mlxsw_sp_fid *
+mlxsw_sp_bridge_8021d_fid_get(struct mlxsw_sp_bridge_device *bridge_device,
+			      u16 vid)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_lower_get(bridge_device->dev);
+
+	return mlxsw_sp_fid_8021d_get(mlxsw_sp, bridge_device->dev->ifindex);
+}
+
+static const struct mlxsw_sp_bridge_ops mlxsw_sp_bridge_8021d_ops = {
+	.port_join	= mlxsw_sp_bridge_8021d_port_join,
+	.port_leave	= mlxsw_sp_bridge_8021d_port_leave,
+	.fid_get	= mlxsw_sp_bridge_8021d_fid_get,
 };
 
-static void mlxsw_sp_fdb_call_notifiers(bool learning_sync, bool adding,
-					char *mac, u16 vid,
-					struct net_device *dev)
+int mlxsw_sp_port_bridge_join(struct mlxsw_sp_port *mlxsw_sp_port,
+			      struct net_device *brport_dev,
+			      struct net_device *br_dev)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct mlxsw_sp_bridge_device *bridge_device;
+	struct mlxsw_sp_bridge_port *bridge_port;
+	int err;
+
+	bridge_port = mlxsw_sp_bridge_port_get(mlxsw_sp->bridge, brport_dev);
+	if (IS_ERR(bridge_port))
+		return PTR_ERR(bridge_port);
+	bridge_device = bridge_port->bridge_device;
+
+	err = bridge_device->ops->port_join(bridge_device, bridge_port,
+					    mlxsw_sp_port);
+	if (err)
+		goto err_port_join;
+
+	return 0;
+
+err_port_join:
+	mlxsw_sp_bridge_port_put(mlxsw_sp->bridge, bridge_port);
+	return err;
+}
+
+void mlxsw_sp_port_bridge_leave(struct mlxsw_sp_port *mlxsw_sp_port,
+				struct net_device *brport_dev,
+				struct net_device *br_dev)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct mlxsw_sp_bridge_device *bridge_device;
+	struct mlxsw_sp_bridge_port *bridge_port;
+
+	bridge_device = mlxsw_sp_bridge_device_find(mlxsw_sp->bridge, br_dev);
+	if (!bridge_device)
+		return;
+	bridge_port = __mlxsw_sp_bridge_port_find(bridge_device, brport_dev);
+	if (!bridge_port)
+		return;
+
+	bridge_device->ops->port_leave(bridge_device, bridge_port,
+				       mlxsw_sp_port);
+	mlxsw_sp_bridge_port_put(mlxsw_sp->bridge, bridge_port);
+}
+
+static void
+mlxsw_sp_fdb_call_notifiers(enum switchdev_notifier_type type,
+			    const char *mac, u16 vid,
+			    struct net_device *dev)
 {
 	struct switchdev_notifier_fdb_info info;
-	unsigned long notifier_type;
 
-	if (learning_sync) {
-		info.addr = mac;
-		info.vid = vid;
-		notifier_type = adding ? SWITCHDEV_FDB_ADD : SWITCHDEV_FDB_DEL;
-		call_switchdev_notifiers(notifier_type, dev, &info.info);
-	}
+	info.addr = mac;
+	info.vid = vid;
+	call_switchdev_notifiers(type, dev, &info.info);
 }
 
 static void mlxsw_sp_fdb_notify_mac_process(struct mlxsw_sp *mlxsw_sp,
 					    char *sfn_pl, int rec_index,
 					    bool adding)
 {
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
+	struct mlxsw_sp_bridge_device *bridge_device;
+	struct mlxsw_sp_bridge_port *bridge_port;
 	struct mlxsw_sp_port *mlxsw_sp_port;
+	enum switchdev_notifier_type type;
 	char mac[ETH_ALEN];
 	u8 local_port;
 	u16 vid, fid;
@@ -1477,22 +1690,21 @@ static void mlxsw_sp_fdb_notify_mac_process(struct mlxsw_sp *mlxsw_sp,
 		goto just_remove;
 	}
 
-	if (mlxsw_sp_fid_is_vfid(fid)) {
-		struct mlxsw_sp_port *mlxsw_sp_vport;
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_fid(mlxsw_sp_port, fid);
+	if (!mlxsw_sp_port_vlan) {
+		netdev_err(mlxsw_sp_port->dev, "Failed to find a matching {Port, VID} following FDB notification\n");
+		goto just_remove;
+	}
 
-		mlxsw_sp_vport = mlxsw_sp_port_vport_find_by_fid(mlxsw_sp_port,
-								 fid);
-		if (!mlxsw_sp_vport) {
-			netdev_err(mlxsw_sp_port->dev, "Failed to find a matching vPort following FDB notification\n");
-			goto just_remove;
-		}
-		vid = 0;
-		/* Override the physical port with the vPort. */
-		mlxsw_sp_port = mlxsw_sp_vport;
-	} else {
-		vid = fid;
+	bridge_port = mlxsw_sp_port_vlan->bridge_port;
+	if (!bridge_port) {
+		netdev_err(mlxsw_sp_port->dev, "{Port, VID} not associated with a bridge\n");
+		goto just_remove;
 	}
 
+	bridge_device = bridge_port->bridge_device;
+	vid = bridge_device->vlan_enabled ? mlxsw_sp_port_vlan->vid : 0;
+
 do_fdb_op:
 	err = mlxsw_sp_port_fdb_uc_op(mlxsw_sp, local_port, mac, fid,
 				      adding, true);
@@ -1503,8 +1715,9 @@ do_fdb_op:
 
 	if (!do_notification)
 		return;
-	mlxsw_sp_fdb_call_notifiers(mlxsw_sp_port->learning_sync,
-				    adding, mac, vid, mlxsw_sp_port->dev);
+	type = adding ? SWITCHDEV_FDB_ADD_TO_BRIDGE : SWITCHDEV_FDB_DEL_TO_BRIDGE;
+	mlxsw_sp_fdb_call_notifiers(type, mac, vid, bridge_port->dev);
+
 	return;
 
 just_remove:
@@ -1517,8 +1730,11 @@ static void mlxsw_sp_fdb_notify_mac_lag_process(struct mlxsw_sp *mlxsw_sp,
 						char *sfn_pl, int rec_index,
 						bool adding)
 {
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
+	struct mlxsw_sp_bridge_device *bridge_device;
+	struct mlxsw_sp_bridge_port *bridge_port;
 	struct mlxsw_sp_port *mlxsw_sp_port;
-	struct net_device *dev;
+	enum switchdev_notifier_type type;
 	char mac[ETH_ALEN];
 	u16 lag_vid = 0;
 	u16 lag_id;
@@ -1533,26 +1749,22 @@ static void mlxsw_sp_fdb_notify_mac_lag_process(struct mlxsw_sp *mlxsw_sp,
 		goto just_remove;
 	}
 
-	if (mlxsw_sp_fid_is_vfid(fid)) {
-		struct mlxsw_sp_port *mlxsw_sp_vport;
-
-		mlxsw_sp_vport = mlxsw_sp_port_vport_find_by_fid(mlxsw_sp_port,
-								 fid);
-		if (!mlxsw_sp_vport) {
-			netdev_err(mlxsw_sp_port->dev, "Failed to find a matching vPort following FDB notification\n");
-			goto just_remove;
-		}
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_fid(mlxsw_sp_port, fid);
+	if (!mlxsw_sp_port_vlan) {
+		netdev_err(mlxsw_sp_port->dev, "Failed to find a matching {Port, VID} following FDB notification\n");
+		goto just_remove;
+	}
 
-		lag_vid = mlxsw_sp_vport_vid_get(mlxsw_sp_vport);
-		dev = mlxsw_sp_vport->dev;
-		vid = 0;
-		/* Override the physical port with the vPort. */
-		mlxsw_sp_port = mlxsw_sp_vport;
-	} else {
-		dev = mlxsw_sp_lag_get(mlxsw_sp, lag_id)->dev;
-		vid = fid;
+	bridge_port = mlxsw_sp_port_vlan->bridge_port;
+	if (!bridge_port) {
+		netdev_err(mlxsw_sp_port->dev, "{Port, VID} not associated with a bridge\n");
+		goto just_remove;
 	}
 
+	bridge_device = bridge_port->bridge_device;
+	vid = bridge_device->vlan_enabled ? mlxsw_sp_port_vlan->vid : 0;
+	lag_vid = mlxsw_sp_port_vlan->vid;
+
 do_fdb_op:
 	err = mlxsw_sp_port_fdb_uc_lag_op(mlxsw_sp, lag_id, mac, fid, lag_vid,
 					  adding, true);
@@ -1563,8 +1775,9 @@ do_fdb_op:
 
 	if (!do_notification)
 		return;
-	mlxsw_sp_fdb_call_notifiers(mlxsw_sp_port->learning_sync, adding, mac,
-				    vid, dev);
+	type = adding ? SWITCHDEV_FDB_ADD_TO_BRIDGE : SWITCHDEV_FDB_DEL_TO_BRIDGE;
+	mlxsw_sp_fdb_call_notifiers(type, mac, vid, bridge_port->dev);
+
 	return;
 
 just_remove:
@@ -1598,12 +1811,15 @@ static void mlxsw_sp_fdb_notify_rec_process(struct mlxsw_sp *mlxsw_sp,
 
 static void mlxsw_sp_fdb_notify_work_schedule(struct mlxsw_sp *mlxsw_sp)
 {
-	mlxsw_core_schedule_dw(&mlxsw_sp->fdb_notify.dw,
-			       msecs_to_jiffies(mlxsw_sp->fdb_notify.interval));
+	struct mlxsw_sp_bridge *bridge = mlxsw_sp->bridge;
+
+	mlxsw_core_schedule_dw(&bridge->fdb_notify.dw,
+			       msecs_to_jiffies(bridge->fdb_notify.interval));
 }
 
 static void mlxsw_sp_fdb_notify_work(struct work_struct *work)
 {
+	struct mlxsw_sp_bridge *bridge;
 	struct mlxsw_sp *mlxsw_sp;
 	char *sfn_pl;
 	u8 num_rec;
@@ -1614,7 +1830,8 @@ static void mlxsw_sp_fdb_notify_work(struct work_struct *work)
 	if (!sfn_pl)
 		return;
 
-	mlxsw_sp = container_of(work, struct mlxsw_sp, fdb_notify.dw.work);
+	bridge = container_of(work, struct mlxsw_sp_bridge, fdb_notify.dw.work);
+	mlxsw_sp = bridge->mlxsw_sp;
 
 	rtnl_lock();
 	mlxsw_reg_sfn_pack(sfn_pl);
@@ -1633,8 +1850,100 @@ out:
 	mlxsw_sp_fdb_notify_work_schedule(mlxsw_sp);
 }
 
+struct mlxsw_sp_switchdev_event_work {
+	struct work_struct work;
+	struct switchdev_notifier_fdb_info fdb_info;
+	struct net_device *dev;
+	unsigned long event;
+};
+
+static void mlxsw_sp_switchdev_event_work(struct work_struct *work)
+{
+	struct mlxsw_sp_switchdev_event_work *switchdev_work =
+		container_of(work, struct mlxsw_sp_switchdev_event_work, work);
+	struct net_device *dev = switchdev_work->dev;
+	struct switchdev_notifier_fdb_info *fdb_info;
+	struct mlxsw_sp_port *mlxsw_sp_port;
+	int err;
+
+	rtnl_lock();
+	mlxsw_sp_port = mlxsw_sp_port_dev_lower_find(dev);
+	if (!mlxsw_sp_port)
+		goto out;
+
+	switch (switchdev_work->event) {
+	case SWITCHDEV_FDB_ADD_TO_DEVICE:
+		fdb_info = &switchdev_work->fdb_info;
+		err = mlxsw_sp_port_fdb_set(mlxsw_sp_port, fdb_info, true);
+		if (err)
+			break;
+		mlxsw_sp_fdb_call_notifiers(SWITCHDEV_FDB_OFFLOADED,
+					    fdb_info->addr,
+					    fdb_info->vid, dev);
+		break;
+	case SWITCHDEV_FDB_DEL_TO_DEVICE:
+		fdb_info = &switchdev_work->fdb_info;
+		mlxsw_sp_port_fdb_set(mlxsw_sp_port, fdb_info, false);
+		break;
+	}
+
+out:
+	rtnl_unlock();
+	kfree(switchdev_work->fdb_info.addr);
+	kfree(switchdev_work);
+	dev_put(dev);
+}
+
+/* Called under rcu_read_lock() */
+static int mlxsw_sp_switchdev_event(struct notifier_block *unused,
+				    unsigned long event, void *ptr)
+{
+	struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
+	struct mlxsw_sp_switchdev_event_work *switchdev_work;
+	struct switchdev_notifier_fdb_info *fdb_info = ptr;
+
+	if (!mlxsw_sp_port_dev_lower_find_rcu(dev))
+		return NOTIFY_DONE;
+
+	switchdev_work = kzalloc(sizeof(*switchdev_work), GFP_ATOMIC);
+	if (!switchdev_work)
+		return NOTIFY_BAD;
+
+	INIT_WORK(&switchdev_work->work, mlxsw_sp_switchdev_event_work);
+	switchdev_work->dev = dev;
+	switchdev_work->event = event;
+
+	switch (event) {
+	case SWITCHDEV_FDB_ADD_TO_DEVICE: /* fall through */
+	case SWITCHDEV_FDB_DEL_TO_DEVICE:
+		memcpy(&switchdev_work->fdb_info, ptr,
+		       sizeof(switchdev_work->fdb_info));
+		switchdev_work->fdb_info.addr = kzalloc(ETH_ALEN, GFP_ATOMIC);
+		ether_addr_copy((u8 *)switchdev_work->fdb_info.addr,
+				fdb_info->addr);
+		/* Take a reference on the device. This can be either
+		 * upper device containig mlxsw_sp_port or just a
+		 * mlxsw_sp_port
+		 */
+		dev_hold(dev);
+		break;
+	default:
+		kfree(switchdev_work);
+		return NOTIFY_DONE;
+	}
+
+	mlxsw_core_schedule_work(&switchdev_work->work);
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block mlxsw_sp_switchdev_notifier = {
+	.notifier_call = mlxsw_sp_switchdev_event,
+};
+
 static int mlxsw_sp_fdb_init(struct mlxsw_sp *mlxsw_sp)
 {
+	struct mlxsw_sp_bridge *bridge = mlxsw_sp->bridge;
 	int err;
 
 	err = mlxsw_sp_ageing_set(mlxsw_sp, MLXSW_SP_DEFAULT_AGEING_TIME);
@@ -1642,25 +1951,51 @@ static int mlxsw_sp_fdb_init(struct mlxsw_sp *mlxsw_sp)
 		dev_err(mlxsw_sp->bus_info->dev, "Failed to set default ageing time\n");
 		return err;
 	}
-	INIT_DELAYED_WORK(&mlxsw_sp->fdb_notify.dw, mlxsw_sp_fdb_notify_work);
-	mlxsw_sp->fdb_notify.interval = MLXSW_SP_DEFAULT_LEARNING_INTERVAL;
+
+	err = register_switchdev_notifier(&mlxsw_sp_switchdev_notifier);
+	if (err) {
+		dev_err(mlxsw_sp->bus_info->dev, "Failed to register switchdev notifier\n");
+		return err;
+	}
+
+	INIT_DELAYED_WORK(&bridge->fdb_notify.dw, mlxsw_sp_fdb_notify_work);
+	bridge->fdb_notify.interval = MLXSW_SP_DEFAULT_LEARNING_INTERVAL;
 	mlxsw_sp_fdb_notify_work_schedule(mlxsw_sp);
 	return 0;
 }
 
 static void mlxsw_sp_fdb_fini(struct mlxsw_sp *mlxsw_sp)
 {
-	cancel_delayed_work_sync(&mlxsw_sp->fdb_notify.dw);
+	cancel_delayed_work_sync(&mlxsw_sp->bridge->fdb_notify.dw);
+	unregister_switchdev_notifier(&mlxsw_sp_switchdev_notifier);
+
 }
 
 int mlxsw_sp_switchdev_init(struct mlxsw_sp *mlxsw_sp)
 {
+	struct mlxsw_sp_bridge *bridge;
+
+	bridge = kzalloc(sizeof(*mlxsw_sp->bridge), GFP_KERNEL);
+	if (!bridge)
+		return -ENOMEM;
+	mlxsw_sp->bridge = bridge;
+	bridge->mlxsw_sp = mlxsw_sp;
+
+	INIT_LIST_HEAD(&mlxsw_sp->bridge->bridges_list);
+	INIT_LIST_HEAD(&mlxsw_sp->bridge->mids_list);
+
+	bridge->bridge_8021q_ops = &mlxsw_sp_bridge_8021q_ops;
+	bridge->bridge_8021d_ops = &mlxsw_sp_bridge_8021d_ops;
+
 	return mlxsw_sp_fdb_init(mlxsw_sp);
 }
 
 void mlxsw_sp_switchdev_fini(struct mlxsw_sp *mlxsw_sp)
 {
 	mlxsw_sp_fdb_fini(mlxsw_sp);
+	WARN_ON(!list_empty(&mlxsw_sp->bridge->mids_list));
+	WARN_ON(!list_empty(&mlxsw_sp->bridge->bridges_list));
+	kfree(mlxsw_sp->bridge);
 }
 
 void mlxsw_sp_port_switchdev_init(struct mlxsw_sp_port *mlxsw_sp_port)
diff --git a/drivers/net/ethernet/mellanox/mlxsw/trap.h b/drivers/net/ethernet/mellanox/mlxsw/trap.h
index e008fdbed20f..12b5ed58f3eb 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/trap.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/trap.h
@@ -66,6 +66,7 @@ enum {
 	MLXSW_TRAP_ID_RTR_INGRESS0 = 0x70,
 	MLXSW_TRAP_ID_BGP_IPV4 = 0x88,
 	MLXSW_TRAP_ID_HOST_MISS_IPV4 = 0x90,
+	MLXSW_TRAP_ID_ACL0 = 0x1C0,
 
 	MLXSW_TRAP_ID_MAX = 0x1FF
 };
diff --git a/drivers/net/ethernet/micrel/ks8842.c b/drivers/net/ethernet/micrel/ks8842.c
index cb0102dd7f70..e3d7c74d47bb 100644
--- a/drivers/net/ethernet/micrel/ks8842.c
+++ b/drivers/net/ethernet/micrel/ks8842.c
@@ -669,7 +669,7 @@ static void ks8842_rx_frame(struct net_device *netdev,
 			ks8842_update_rx_counters(netdev, status, len);
 
 			if (adapter->conf_flags & KS884X_16BIT) {
-				u16 *data16 = (u16 *)skb_put(skb, len);
+				u16 *data16 = skb_put(skb, len);
 				ks8842_select_bank(adapter, 17);
 				while (len > 0) {
 					*data16++ = ioread16(adapter->hw_addr +
@@ -679,7 +679,7 @@ static void ks8842_rx_frame(struct net_device *netdev,
 					len -= sizeof(u32);
 				}
 			} else {
-				u32 *data = (u32 *)skb_put(skb, len);
+				u32 *data = skb_put(skb, len);
 
 				ks8842_select_bank(adapter, 17);
 				while (len > 0) {
diff --git a/drivers/net/ethernet/micrel/ks8851.c b/drivers/net/ethernet/micrel/ks8851.c
index 20358f87de57..2fe96f1f3fe5 100644
--- a/drivers/net/ethernet/micrel/ks8851.c
+++ b/drivers/net/ethernet/micrel/ks8851.c
@@ -1071,7 +1071,10 @@ static int ks8851_get_link_ksettings(struct net_device *dev,
 				     struct ethtool_link_ksettings *cmd)
 {
 	struct ks8851_net *ks = netdev_priv(dev);
-	return mii_ethtool_get_link_ksettings(&ks->mii, cmd);
+
+	mii_ethtool_get_link_ksettings(&ks->mii, cmd);
+
+	return 0;
 }
 
 static int ks8851_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/micrel/ks8851_mll.c b/drivers/net/ethernet/micrel/ks8851_mll.c
index 7647f7bdbcb8..f3e9dd47b56f 100644
--- a/drivers/net/ethernet/micrel/ks8851_mll.c
+++ b/drivers/net/ethernet/micrel/ks8851_mll.c
@@ -1315,7 +1315,10 @@ static int ks_get_link_ksettings(struct net_device *netdev,
 				 struct ethtool_link_ksettings *cmd)
 {
 	struct ks_net *ks = netdev_priv(netdev);
-	return mii_ethtool_get_link_ksettings(&ks->mii, cmd);
+
+	mii_ethtool_get_link_ksettings(&ks->mii, cmd);
+
+	return 0;
 }
 
 static int ks_set_link_ksettings(struct net_device *netdev,
diff --git a/drivers/net/ethernet/micrel/ksz884x.c b/drivers/net/ethernet/micrel/ksz884x.c
index ee1c78abab0b..e798fbe08600 100644
--- a/drivers/net/ethernet/micrel/ksz884x.c
+++ b/drivers/net/ethernet/micrel/ksz884x.c
@@ -5020,8 +5020,7 @@ static inline int rx_proc(struct net_device *dev, struct ksz_hw* hw,
 		 */
 		skb_reserve(skb, 2);
 
-		memcpy(skb_put(skb, packet_len),
-			dma_buf->skb->data, packet_len);
+		skb_put_data(skb, dma_buf->skb->data, packet_len);
 	} while (0);
 
 	skb->protocol = eth_type_trans(skb, dev);
diff --git a/drivers/net/ethernet/neterion/s2io.c b/drivers/net/ethernet/neterion/s2io.c
index 118723ea681a..fd2ec36c6fa1 100644
--- a/drivers/net/ethernet/neterion/s2io.c
+++ b/drivers/net/ethernet/neterion/s2io.c
@@ -2459,7 +2459,6 @@ static int fill_rx_buffers(struct s2io_nic *nic, struct ring_info *ring,
 	struct buffAdd *ba;
 	struct RxD_t *first_rxdp = NULL;
 	u64 Buffer0_ptr = 0, Buffer1_ptr = 0;
-	int rxd_index = 0;
 	struct RxD1 *rxdp1;
 	struct RxD3 *rxdp3;
 	struct swStat *swstats = &ring->nic->mac_control.stats_info->sw_stat;
@@ -2474,10 +2473,6 @@ static int fill_rx_buffers(struct s2io_nic *nic, struct ring_info *ring,
 
 		rxdp = ring->rx_blocks[block_no].rxds[off].virt_addr;
 
-		rxd_index = off + 1;
-		if (block_no)
-			rxd_index += (block_no * ring->rxd_count);
-
 		if ((block_no == block_no1) &&
 		    (off == ring->rx_curr_get_info.offset) &&
 		    (rxdp->Host_Control)) {
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-main.c b/drivers/net/ethernet/neterion/vxge/vxge-main.c
index 6a4310af5d97..50ea69d88480 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-main.c
+++ b/drivers/net/ethernet/neterion/vxge/vxge-main.c
@@ -3218,6 +3218,7 @@ static int vxge_hwtstamp_set(struct vxgedev *vdev, void __user *data)
 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+	case HWTSTAMP_FILTER_NTP_ALL:
 		if (vdev->devh->config.hwts_en != VXGE_HW_HWTS_ENABLE)
 			return -EFAULT;
 
diff --git a/drivers/net/ethernet/netronome/Kconfig b/drivers/net/ethernet/netronome/Kconfig
index 967d7ca8c28c..0e331e2f685a 100644
--- a/drivers/net/ethernet/netronome/Kconfig
+++ b/drivers/net/ethernet/netronome/Kconfig
@@ -19,11 +19,22 @@ config NFP
 	tristate "Netronome(R) NFP4000/NFP6000 NIC driver"
 	depends on PCI && PCI_MSI
 	depends on VXLAN || VXLAN=n
+	depends on MAY_USE_DEVLINK
 	---help---
 	  This driver supports the Netronome(R) NFP4000/NFP6000 based
 	  cards working as a advanced Ethernet NIC.  It works with both
 	  SR-IOV physical and virtual functions.
 
+config NFP_APP_FLOWER
+	bool "NFP4000/NFP6000 TC Flower offload support"
+	depends on NFP
+	depends on NET_SWITCHDEV
+	---help---
+	  Enable driver support for TC Flower offload on NFP4000 and NFP6000.
+	  Say Y, if you are planning to make use of TC Flower offload
+	  either directly, with Open vSwitch, or any other way.  Note that
+	  TC Flower offload requires specific FW to work.
+
 config NFP_DEBUG
 	bool "Debug support for Netronome(R) NFP4000/NFP6000 NIC drivers"
 	depends on NFP
diff --git a/drivers/net/ethernet/netronome/nfp/Makefile b/drivers/net/ethernet/netronome/nfp/Makefile
index 4b15f0f496aa..b8e1358868bd 100644
--- a/drivers/net/ethernet/netronome/nfp/Makefile
+++ b/drivers/net/ethernet/netronome/nfp/Makefile
@@ -14,17 +14,35 @@ nfp-objs := \
 	    nfpcore/nfp_resource.o \
 	    nfpcore/nfp_rtsym.o \
 	    nfpcore/nfp_target.o \
+	    nfp_app.o \
+	    nfp_app_nic.o \
+	    nfp_devlink.o \
+	    nfp_hwmon.o \
 	    nfp_main.o \
 	    nfp_net_common.o \
 	    nfp_net_ethtool.o \
-	    nfp_net_offload.o \
 	    nfp_net_main.o \
-	    nfp_netvf_main.o
+	    nfp_net_repr.o \
+	    nfp_netvf_main.o \
+	    nfp_port.o \
+	    bpf/main.o \
+	    bpf/offload.o \
+	    nic/main.o
+
+ifeq ($(CONFIG_NFP_APP_FLOWER),y)
+nfp-objs += \
+	    flower/action.o \
+	    flower/cmsg.o \
+	    flower/main.o \
+	    flower/match.o \
+	    flower/metadata.o \
+	    flower/offload.o
+endif
 
 ifeq ($(CONFIG_BPF_SYSCALL),y)
 nfp-objs += \
-	    nfp_bpf_verifier.o \
-	    nfp_bpf_jit.o
+	    bpf/verifier.o \
+	    bpf/jit.o
 endif
 
 nfp-$(CONFIG_NFP_DEBUG) += nfp_net_debugfs.o
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_bpf_jit.c b/drivers/net/ethernet/netronome/nfp/bpf/jit.c
index 97a8f00674d0..8e57fda6b8b5 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_bpf_jit.c
+++ b/drivers/net/ethernet/netronome/nfp/bpf/jit.c
@@ -39,8 +39,8 @@
 #include <linux/pkt_cls.h>
 #include <linux/unistd.h>
 
-#include "nfp_asm.h"
-#include "nfp_bpf.h"
+#include "main.h"
+#include "../nfp_asm.h"
 
 /* --- NFP prog --- */
 /* Foreach "multiple" entries macros provide pos and next<n> pointers.
diff --git a/drivers/net/ethernet/netronome/nfp/bpf/main.c b/drivers/net/ethernet/netronome/nfp/bpf/main.c
new file mode 100644
index 000000000000..afbdf5fd4e4f
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/bpf/main.c
@@ -0,0 +1,160 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <net/pkt_cls.h>
+
+#include "../nfpcore/nfp_cpp.h"
+#include "../nfp_app.h"
+#include "../nfp_main.h"
+#include "../nfp_net.h"
+#include "../nfp_port.h"
+#include "main.h"
+
+static bool nfp_net_ebpf_capable(struct nfp_net *nn)
+{
+	if (nn->cap & NFP_NET_CFG_CTRL_BPF &&
+	    nn_readb(nn, NFP_NET_CFG_BPF_ABI) == NFP_NET_BPF_ABI)
+		return true;
+	return false;
+}
+
+static int
+nfp_bpf_xdp_offload(struct nfp_app *app, struct nfp_net *nn,
+		    struct bpf_prog *prog)
+{
+	struct tc_cls_bpf_offload cmd = {
+		.prog = prog,
+	};
+	int ret;
+
+	if (!nfp_net_ebpf_capable(nn))
+		return -EINVAL;
+
+	if (nn->dp.ctrl & NFP_NET_CFG_CTRL_BPF) {
+		if (!nn->dp.bpf_offload_xdp)
+			return prog ? -EBUSY : 0;
+		cmd.command = prog ? TC_CLSBPF_REPLACE : TC_CLSBPF_DESTROY;
+	} else {
+		if (!prog)
+			return 0;
+		cmd.command = TC_CLSBPF_ADD;
+	}
+
+	ret = nfp_net_bpf_offload(nn, &cmd);
+	/* Stop offload if replace not possible */
+	if (ret && cmd.command == TC_CLSBPF_REPLACE)
+		nfp_bpf_xdp_offload(app, nn, NULL);
+	nn->dp.bpf_offload_xdp = prog && !ret;
+	return ret;
+}
+
+static const char *nfp_bpf_extra_cap(struct nfp_app *app, struct nfp_net *nn)
+{
+	return nfp_net_ebpf_capable(nn) ? "BPF" : "";
+}
+
+static int
+nfp_bpf_vnic_init(struct nfp_app *app, struct nfp_net *nn, unsigned int id)
+{
+	struct nfp_net_bpf_priv *priv;
+	int ret;
+
+	/* Limit to single port, otherwise it's just a NIC */
+	if (id > 0) {
+		nfp_warn(app->cpp,
+			 "BPF NIC doesn't support more than one port right now\n");
+		nn->port = nfp_port_alloc(app, NFP_PORT_INVALID, nn->dp.netdev);
+		return PTR_ERR_OR_ZERO(nn->port);
+	}
+
+	priv = kmalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	nn->app_priv = priv;
+	spin_lock_init(&priv->rx_filter_lock);
+	setup_timer(&priv->rx_filter_stats_timer,
+		    nfp_net_filter_stats_timer, (unsigned long)nn);
+
+	ret = nfp_app_nic_vnic_init(app, nn, id);
+	if (ret)
+		kfree(priv);
+
+	return ret;
+}
+
+static void nfp_bpf_vnic_clean(struct nfp_app *app, struct nfp_net *nn)
+{
+	if (nn->dp.bpf_offload_xdp)
+		nfp_bpf_xdp_offload(app, nn, NULL);
+	kfree(nn->app_priv);
+}
+
+static int nfp_bpf_setup_tc(struct nfp_app *app, struct net_device *netdev,
+			    u32 handle, __be16 proto, struct tc_to_netdev *tc)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+
+	if (TC_H_MAJ(handle) != TC_H_MAJ(TC_H_INGRESS))
+		return -EOPNOTSUPP;
+	if (proto != htons(ETH_P_ALL))
+		return -EOPNOTSUPP;
+
+	if (tc->type == TC_SETUP_CLSBPF && nfp_net_ebpf_capable(nn)) {
+		if (!nn->dp.bpf_offload_xdp)
+			return nfp_net_bpf_offload(nn, tc->cls_bpf);
+		else
+			return -EBUSY;
+	}
+
+	return -EINVAL;
+}
+
+static bool nfp_bpf_tc_busy(struct nfp_app *app, struct nfp_net *nn)
+{
+	return nn->dp.ctrl & NFP_NET_CFG_CTRL_BPF;
+}
+
+const struct nfp_app_type app_bpf = {
+	.id		= NFP_APP_BPF_NIC,
+	.name		= "ebpf",
+
+	.extra_cap	= nfp_bpf_extra_cap,
+
+	.vnic_init	= nfp_bpf_vnic_init,
+	.vnic_clean	= nfp_bpf_vnic_clean,
+
+	.setup_tc	= nfp_bpf_setup_tc,
+	.tc_busy	= nfp_bpf_tc_busy,
+	.xdp_offload	= nfp_bpf_xdp_offload,
+};
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_bpf.h b/drivers/net/ethernet/netronome/nfp/bpf/main.h
index 9513c80f7be5..4051e943f363 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_bpf.h
+++ b/drivers/net/ethernet/netronome/nfp/bpf/main.h
@@ -39,6 +39,8 @@
 #include <linux/list.h>
 #include <linux/types.h>
 
+#include "../nfp_net.h"
+
 /* For branch fixup logic use up-most byte of branch instruction as scratch
  * area.  Remember to clear this before sending instructions to HW!
  */
@@ -198,4 +200,25 @@ nfp_bpf_jit(struct bpf_prog *filter, void *prog, enum nfp_bpf_action_type act,
 
 int nfp_prog_verify(struct nfp_prog *nfp_prog, struct bpf_prog *prog);
 
+struct nfp_net;
+struct tc_cls_bpf_offload;
+
+/**
+ * struct nfp_net_bpf_priv - per-vNIC BPF private data
+ * @rx_filter:		Filter offload statistics - dropped packets/bytes
+ * @rx_filter_prev:	Filter offload statistics - values from previous update
+ * @rx_filter_change:	Jiffies when statistics last changed
+ * @rx_filter_stats_timer:  Timer for polling filter offload statistics
+ * @rx_filter_lock:	Lock protecting timer state changes (teardown)
+ */
+struct nfp_net_bpf_priv {
+	struct nfp_stat_pair rx_filter, rx_filter_prev;
+	unsigned long rx_filter_change;
+	struct timer_list rx_filter_stats_timer;
+	spinlock_t rx_filter_lock;
+};
+
+int nfp_net_bpf_offload(struct nfp_net *nn, struct tc_cls_bpf_offload *cls_bpf);
+void nfp_net_filter_stats_timer(unsigned long data);
+
 #endif
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_offload.c b/drivers/net/ethernet/netronome/nfp/bpf/offload.c
index cc823df12c8a..78d80a364edb 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net_offload.c
+++ b/drivers/net/ethernet/netronome/nfp/bpf/offload.c
@@ -47,60 +47,59 @@
 #include <net/tc_act/tc_gact.h>
 #include <net/tc_act/tc_mirred.h>
 
-#include "nfp_bpf.h"
-#include "nfp_net_ctrl.h"
-#include "nfp_net.h"
+#include "main.h"
+#include "../nfp_net_ctrl.h"
+#include "../nfp_net.h"
 
 void nfp_net_filter_stats_timer(unsigned long data)
 {
 	struct nfp_net *nn = (void *)data;
+	struct nfp_net_bpf_priv *priv;
 	struct nfp_stat_pair latest;
 
-	spin_lock_bh(&nn->rx_filter_lock);
+	priv = nn->app_priv;
+
+	spin_lock_bh(&priv->rx_filter_lock);
 
 	if (nn->dp.ctrl & NFP_NET_CFG_CTRL_BPF)
-		mod_timer(&nn->rx_filter_stats_timer,
+		mod_timer(&priv->rx_filter_stats_timer,
 			  jiffies + NFP_NET_STAT_POLL_IVL);
 
-	spin_unlock_bh(&nn->rx_filter_lock);
+	spin_unlock_bh(&priv->rx_filter_lock);
 
 	latest.pkts = nn_readq(nn, NFP_NET_CFG_STATS_APP1_FRAMES);
 	latest.bytes = nn_readq(nn, NFP_NET_CFG_STATS_APP1_BYTES);
 
-	if (latest.pkts != nn->rx_filter.pkts)
-		nn->rx_filter_change = jiffies;
+	if (latest.pkts != priv->rx_filter.pkts)
+		priv->rx_filter_change = jiffies;
 
-	nn->rx_filter = latest;
+	priv->rx_filter = latest;
 }
 
 static void nfp_net_bpf_stats_reset(struct nfp_net *nn)
 {
-	nn->rx_filter.pkts = nn_readq(nn, NFP_NET_CFG_STATS_APP1_FRAMES);
-	nn->rx_filter.bytes = nn_readq(nn, NFP_NET_CFG_STATS_APP1_BYTES);
-	nn->rx_filter_prev = nn->rx_filter;
-	nn->rx_filter_change = jiffies;
+	struct nfp_net_bpf_priv *priv = nn->app_priv;
+
+	priv->rx_filter.pkts = nn_readq(nn, NFP_NET_CFG_STATS_APP1_FRAMES);
+	priv->rx_filter.bytes = nn_readq(nn, NFP_NET_CFG_STATS_APP1_BYTES);
+	priv->rx_filter_prev = priv->rx_filter;
+	priv->rx_filter_change = jiffies;
 }
 
 static int
 nfp_net_bpf_stats_update(struct nfp_net *nn, struct tc_cls_bpf_offload *cls_bpf)
 {
-	struct tc_action *a;
-	LIST_HEAD(actions);
+	struct nfp_net_bpf_priv *priv = nn->app_priv;
 	u64 bytes, pkts;
 
-	pkts = nn->rx_filter.pkts - nn->rx_filter_prev.pkts;
-	bytes = nn->rx_filter.bytes - nn->rx_filter_prev.bytes;
+	pkts = priv->rx_filter.pkts - priv->rx_filter_prev.pkts;
+	bytes = priv->rx_filter.bytes - priv->rx_filter_prev.bytes;
 	bytes -= pkts * ETH_HLEN;
 
-	nn->rx_filter_prev = nn->rx_filter;
+	priv->rx_filter_prev = priv->rx_filter;
 
-	preempt_disable();
-
-	tcf_exts_to_list(cls_bpf->exts, &actions);
-	list_for_each_entry(a, &actions, list)
-		tcf_action_stats_update(a, bytes, pkts, nn->rx_filter_change);
-
-	preempt_enable();
+	tcf_exts_stats_update(cls_bpf->exts,
+			      bytes, pkts, priv->rx_filter_change);
 
 	return 0;
 }
@@ -190,6 +189,7 @@ nfp_net_bpf_load_and_start(struct nfp_net *nn, u32 tc_flags,
 			   unsigned int code_sz, unsigned int n_instr,
 			   bool dense_mode)
 {
+	struct nfp_net_bpf_priv *priv = nn->app_priv;
 	u64 bpf_addr = dma_addr;
 	int err;
 
@@ -216,20 +216,23 @@ nfp_net_bpf_load_and_start(struct nfp_net *nn, u32 tc_flags,
 	dma_free_coherent(nn->dp.dev, code_sz, code, dma_addr);
 
 	nfp_net_bpf_stats_reset(nn);
-	mod_timer(&nn->rx_filter_stats_timer, jiffies + NFP_NET_STAT_POLL_IVL);
+	mod_timer(&priv->rx_filter_stats_timer,
+		  jiffies + NFP_NET_STAT_POLL_IVL);
 }
 
 static int nfp_net_bpf_stop(struct nfp_net *nn)
 {
+	struct nfp_net_bpf_priv *priv = nn->app_priv;
+
 	if (!(nn->dp.ctrl & NFP_NET_CFG_CTRL_BPF))
 		return 0;
 
-	spin_lock_bh(&nn->rx_filter_lock);
+	spin_lock_bh(&priv->rx_filter_lock);
 	nn->dp.ctrl &= ~NFP_NET_CFG_CTRL_BPF;
-	spin_unlock_bh(&nn->rx_filter_lock);
+	spin_unlock_bh(&priv->rx_filter_lock);
 	nn_writel(nn, NFP_NET_CFG_CTRL, nn->dp.ctrl);
 
-	del_timer_sync(&nn->rx_filter_stats_timer);
+	del_timer_sync(&priv->rx_filter_stats_timer);
 	nn->dp.bpf_offload_skip_sw = 0;
 
 	return nfp_net_reconfig(nn, NFP_NET_CFG_UPDATE_GEN);
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_bpf_verifier.c b/drivers/net/ethernet/netronome/nfp/bpf/verifier.c
index b3361f9b8e5c..d696ba46f70a 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_bpf_verifier.c
+++ b/drivers/net/ethernet/netronome/nfp/bpf/verifier.c
@@ -38,7 +38,7 @@
 #include <linux/kernel.h>
 #include <linux/pkt_cls.h>
 
-#include "nfp_bpf.h"
+#include "main.h"
 
 /* Analyzer/verifier definitions */
 struct nfp_bpf_analyzer_priv {
diff --git a/drivers/net/ethernet/netronome/nfp/flower/action.c b/drivers/net/ethernet/netronome/nfp/flower/action.c
new file mode 100644
index 000000000000..db9750695dc7
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/flower/action.c
@@ -0,0 +1,211 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/bitfield.h>
+#include <net/pkt_cls.h>
+#include <net/switchdev.h>
+#include <net/tc_act/tc_gact.h>
+#include <net/tc_act/tc_mirred.h>
+#include <net/tc_act/tc_vlan.h>
+
+#include "cmsg.h"
+#include "main.h"
+#include "../nfp_net_repr.h"
+
+static void nfp_fl_pop_vlan(struct nfp_fl_pop_vlan *pop_vlan)
+{
+	size_t act_size = sizeof(struct nfp_fl_pop_vlan);
+	u16 tmp_pop_vlan_op;
+
+	tmp_pop_vlan_op =
+		FIELD_PREP(NFP_FL_ACT_LEN_LW, act_size >> NFP_FL_LW_SIZ) |
+		FIELD_PREP(NFP_FL_ACT_JMP_ID, NFP_FL_ACTION_OPCODE_POP_VLAN);
+
+	pop_vlan->a_op = cpu_to_be16(tmp_pop_vlan_op);
+	pop_vlan->reserved = 0;
+}
+
+static void
+nfp_fl_push_vlan(struct nfp_fl_push_vlan *push_vlan,
+		 const struct tc_action *action)
+{
+	size_t act_size = sizeof(struct nfp_fl_push_vlan);
+	struct tcf_vlan *vlan = to_vlan(action);
+	u16 tmp_push_vlan_tci;
+	u16 tmp_push_vlan_op;
+
+	tmp_push_vlan_op =
+		FIELD_PREP(NFP_FL_ACT_LEN_LW, act_size >> NFP_FL_LW_SIZ) |
+		FIELD_PREP(NFP_FL_ACT_JMP_ID, NFP_FL_ACTION_OPCODE_PUSH_VLAN);
+
+	push_vlan->a_op = cpu_to_be16(tmp_push_vlan_op);
+	/* Set action push vlan parameters. */
+	push_vlan->reserved = 0;
+	push_vlan->vlan_tpid = tcf_vlan_push_proto(action);
+
+	tmp_push_vlan_tci =
+		FIELD_PREP(NFP_FL_PUSH_VLAN_PRIO, vlan->tcfv_push_prio) |
+		FIELD_PREP(NFP_FL_PUSH_VLAN_VID, vlan->tcfv_push_vid) |
+		NFP_FL_PUSH_VLAN_CFI;
+	push_vlan->vlan_tci = cpu_to_be16(tmp_push_vlan_tci);
+}
+
+static int
+nfp_fl_output(struct nfp_fl_output *output, const struct tc_action *action,
+	      struct nfp_fl_payload *nfp_flow, bool last,
+	      struct net_device *in_dev)
+{
+	size_t act_size = sizeof(struct nfp_fl_output);
+	struct net_device *out_dev;
+	u16 tmp_output_op;
+	int ifindex;
+
+	/* Set action opcode to output action. */
+	tmp_output_op =
+		FIELD_PREP(NFP_FL_ACT_LEN_LW, act_size >> NFP_FL_LW_SIZ) |
+		FIELD_PREP(NFP_FL_ACT_JMP_ID, NFP_FL_ACTION_OPCODE_OUTPUT);
+
+	output->a_op = cpu_to_be16(tmp_output_op);
+
+	/* Set action output parameters. */
+	output->flags = cpu_to_be16(last ? NFP_FL_OUT_FLAGS_LAST : 0);
+
+	ifindex = tcf_mirred_ifindex(action);
+	out_dev = __dev_get_by_index(dev_net(in_dev), ifindex);
+	if (!out_dev)
+		return -EOPNOTSUPP;
+
+	/* Only offload egress ports are on the same device as the ingress
+	 * port.
+	 */
+	if (!switchdev_port_same_parent_id(in_dev, out_dev))
+		return -EOPNOTSUPP;
+
+	output->port = cpu_to_be32(nfp_repr_get_port_id(out_dev));
+	if (!output->port)
+		return -EOPNOTSUPP;
+
+	nfp_flow->meta.shortcut = output->port;
+
+	return 0;
+}
+
+static int
+nfp_flower_loop_action(const struct tc_action *a,
+		       struct nfp_fl_payload *nfp_fl, int *a_len,
+		       struct net_device *netdev)
+{
+	struct nfp_fl_push_vlan *psh_v;
+	struct nfp_fl_pop_vlan *pop_v;
+	struct nfp_fl_output *output;
+	int err;
+
+	if (is_tcf_gact_shot(a)) {
+		nfp_fl->meta.shortcut = cpu_to_be32(NFP_FL_SC_ACT_DROP);
+	} else if (is_tcf_mirred_egress_redirect(a)) {
+		if (*a_len + sizeof(struct nfp_fl_output) > NFP_FL_MAX_A_SIZ)
+			return -EOPNOTSUPP;
+
+		output = (struct nfp_fl_output *)&nfp_fl->action_data[*a_len];
+		err = nfp_fl_output(output, a, nfp_fl, true, netdev);
+		if (err)
+			return err;
+
+		*a_len += sizeof(struct nfp_fl_output);
+	} else if (is_tcf_mirred_egress_mirror(a)) {
+		if (*a_len + sizeof(struct nfp_fl_output) > NFP_FL_MAX_A_SIZ)
+			return -EOPNOTSUPP;
+
+		output = (struct nfp_fl_output *)&nfp_fl->action_data[*a_len];
+		err = nfp_fl_output(output, a, nfp_fl, false, netdev);
+		if (err)
+			return err;
+
+		*a_len += sizeof(struct nfp_fl_output);
+	} else if (is_tcf_vlan(a) && tcf_vlan_action(a) == TCA_VLAN_ACT_POP) {
+		if (*a_len + sizeof(struct nfp_fl_pop_vlan) > NFP_FL_MAX_A_SIZ)
+			return -EOPNOTSUPP;
+
+		pop_v = (struct nfp_fl_pop_vlan *)&nfp_fl->action_data[*a_len];
+		nfp_fl->meta.shortcut = cpu_to_be32(NFP_FL_SC_ACT_POPV);
+
+		nfp_fl_pop_vlan(pop_v);
+		*a_len += sizeof(struct nfp_fl_pop_vlan);
+	} else if (is_tcf_vlan(a) && tcf_vlan_action(a) == TCA_VLAN_ACT_PUSH) {
+		if (*a_len + sizeof(struct nfp_fl_push_vlan) > NFP_FL_MAX_A_SIZ)
+			return -EOPNOTSUPP;
+
+		psh_v = (struct nfp_fl_push_vlan *)&nfp_fl->action_data[*a_len];
+		nfp_fl->meta.shortcut = cpu_to_be32(NFP_FL_SC_ACT_NULL);
+
+		nfp_fl_push_vlan(psh_v, a);
+		*a_len += sizeof(struct nfp_fl_push_vlan);
+	} else {
+		/* Currently we do not handle any other actions. */
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+int nfp_flower_compile_action(struct tc_cls_flower_offload *flow,
+			      struct net_device *netdev,
+			      struct nfp_fl_payload *nfp_flow)
+{
+	int act_len, act_cnt, err;
+	const struct tc_action *a;
+	LIST_HEAD(actions);
+
+	memset(nfp_flow->action_data, 0, NFP_FL_MAX_A_SIZ);
+	nfp_flow->meta.act_len = 0;
+	act_len = 0;
+	act_cnt = 0;
+
+	tcf_exts_to_list(flow->exts, &actions);
+	list_for_each_entry(a, &actions, list) {
+		err = nfp_flower_loop_action(a, nfp_flow, &act_len, netdev);
+		if (err)
+			return err;
+		act_cnt++;
+	}
+
+	/* We optimise when the action list is small, this can unfortunately
+	 * not happen once we have more than one action in the action list.
+	 */
+	if (act_cnt > 1)
+		nfp_flow->meta.shortcut = cpu_to_be32(NFP_FL_SC_ACT_NULL);
+
+	nfp_flow->meta.act_len = act_len;
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/netronome/nfp/flower/cmsg.c b/drivers/net/ethernet/netronome/nfp/flower/cmsg.c
new file mode 100644
index 000000000000..dd7fa9cf225f
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/flower/cmsg.c
@@ -0,0 +1,157 @@
+/*
+ * Copyright (C) 2015-2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/bitfield.h>
+#include <linux/netdevice.h>
+#include <linux/skbuff.h>
+#include <net/dst_metadata.h>
+
+#include "main.h"
+#include "../nfpcore/nfp_cpp.h"
+#include "../nfp_net_repr.h"
+#include "./cmsg.h"
+
+#define nfp_flower_cmsg_warn(app, fmt, args...)				\
+	do {								\
+		if (net_ratelimit())					\
+			nfp_warn((app)->cpp, fmt, ## args);		\
+	} while (0)
+
+static struct nfp_flower_cmsg_hdr *
+nfp_flower_cmsg_get_hdr(struct sk_buff *skb)
+{
+	return (struct nfp_flower_cmsg_hdr *)skb->data;
+}
+
+struct sk_buff *
+nfp_flower_cmsg_alloc(struct nfp_app *app, unsigned int size,
+		      enum nfp_flower_cmsg_type_port type)
+{
+	struct nfp_flower_cmsg_hdr *ch;
+	struct sk_buff *skb;
+
+	size += NFP_FLOWER_CMSG_HLEN;
+
+	skb = nfp_app_ctrl_msg_alloc(app, size, GFP_KERNEL);
+	if (!skb)
+		return NULL;
+
+	ch = nfp_flower_cmsg_get_hdr(skb);
+	ch->pad = 0;
+	ch->version = NFP_FLOWER_CMSG_VER1;
+	ch->type = type;
+	skb_put(skb, size);
+
+	return skb;
+}
+
+int nfp_flower_cmsg_portmod(struct nfp_repr *repr, bool carrier_ok)
+{
+	struct nfp_flower_cmsg_portmod *msg;
+	struct sk_buff *skb;
+
+	skb = nfp_flower_cmsg_alloc(repr->app, sizeof(*msg),
+				    NFP_FLOWER_CMSG_TYPE_PORT_MOD);
+	if (!skb)
+		return -ENOMEM;
+
+	msg = nfp_flower_cmsg_get_data(skb);
+	msg->portnum = cpu_to_be32(repr->dst->u.port_info.port_id);
+	msg->reserved = 0;
+	msg->info = carrier_ok;
+	msg->mtu = cpu_to_be16(repr->netdev->mtu);
+
+	nfp_ctrl_tx(repr->app->ctrl, skb);
+
+	return 0;
+}
+
+static void
+nfp_flower_cmsg_portmod_rx(struct nfp_app *app, struct sk_buff *skb)
+{
+	struct nfp_flower_cmsg_portmod *msg;
+	struct net_device *netdev;
+	bool link;
+
+	msg = nfp_flower_cmsg_get_data(skb);
+	link = msg->info & NFP_FLOWER_CMSG_PORTMOD_INFO_LINK;
+
+	rcu_read_lock();
+	netdev = nfp_app_repr_get(app, be32_to_cpu(msg->portnum));
+	if (!netdev) {
+		nfp_flower_cmsg_warn(app, "ctrl msg for unknown port 0x%08x\n",
+				     be32_to_cpu(msg->portnum));
+		rcu_read_unlock();
+		return;
+	}
+
+	if (link) {
+		netif_carrier_on(netdev);
+		rtnl_lock();
+		dev_set_mtu(netdev, be16_to_cpu(msg->mtu));
+		rtnl_unlock();
+	} else {
+		netif_carrier_off(netdev);
+	}
+	rcu_read_unlock();
+}
+
+void nfp_flower_cmsg_rx(struct nfp_app *app, struct sk_buff *skb)
+{
+	struct nfp_flower_cmsg_hdr *cmsg_hdr;
+	enum nfp_flower_cmsg_type_port type;
+
+	cmsg_hdr = nfp_flower_cmsg_get_hdr(skb);
+
+	if (unlikely(cmsg_hdr->version != NFP_FLOWER_CMSG_VER1)) {
+		nfp_flower_cmsg_warn(app, "Cannot handle repr control version %u\n",
+				     cmsg_hdr->version);
+		goto out;
+	}
+
+	type = cmsg_hdr->type;
+	switch (type) {
+	case NFP_FLOWER_CMSG_TYPE_PORT_MOD:
+		nfp_flower_cmsg_portmod_rx(app, skb);
+		break;
+	case NFP_FLOWER_CMSG_TYPE_FLOW_STATS:
+		nfp_flower_rx_flow_stats(app, skb);
+		break;
+	default:
+		nfp_flower_cmsg_warn(app, "Cannot handle invalid repr control type %u\n",
+				     type);
+	}
+
+out:
+	dev_kfree_skb_any(skb);
+}
diff --git a/drivers/net/ethernet/netronome/nfp/flower/cmsg.h b/drivers/net/ethernet/netronome/nfp/flower/cmsg.h
new file mode 100644
index 000000000000..cf738de170ab
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/flower/cmsg.h
@@ -0,0 +1,317 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef NFP_FLOWER_CMSG_H
+#define NFP_FLOWER_CMSG_H
+
+#include <linux/bitfield.h>
+#include <linux/skbuff.h>
+#include <linux/types.h>
+
+#include "../nfp_app.h"
+
+#define NFP_FLOWER_LAYER_META		BIT(0)
+#define NFP_FLOWER_LAYER_PORT		BIT(1)
+#define NFP_FLOWER_LAYER_MAC		BIT(2)
+#define NFP_FLOWER_LAYER_TP		BIT(3)
+#define NFP_FLOWER_LAYER_IPV4		BIT(4)
+#define NFP_FLOWER_LAYER_IPV6		BIT(5)
+#define NFP_FLOWER_LAYER_CT		BIT(6)
+#define NFP_FLOWER_LAYER_VXLAN		BIT(7)
+
+#define NFP_FLOWER_LAYER_ETHER		BIT(3)
+#define NFP_FLOWER_LAYER_ARP		BIT(4)
+
+#define NFP_FLOWER_MASK_VLAN_PRIO	GENMASK(15, 13)
+#define NFP_FLOWER_MASK_VLAN_CFI	BIT(12)
+#define NFP_FLOWER_MASK_VLAN_VID	GENMASK(11, 0)
+
+#define NFP_FL_SC_ACT_DROP		0x80000000
+#define NFP_FL_SC_ACT_USER		0x7D000000
+#define NFP_FL_SC_ACT_POPV		0x6A000000
+#define NFP_FL_SC_ACT_NULL		0x00000000
+
+/* The maximum action list size (in bytes) supported by the NFP.
+ */
+#define NFP_FL_MAX_A_SIZ		1216
+#define NFP_FL_LW_SIZ			2
+
+/* Action opcodes */
+#define NFP_FL_ACTION_OPCODE_OUTPUT	0
+#define NFP_FL_ACTION_OPCODE_PUSH_VLAN	1
+#define NFP_FL_ACTION_OPCODE_POP_VLAN	2
+#define NFP_FL_ACTION_OPCODE_NUM	32
+
+#define NFP_FL_ACT_JMP_ID		GENMASK(15, 8)
+#define NFP_FL_ACT_LEN_LW		GENMASK(7, 0)
+
+#define NFP_FL_OUT_FLAGS_LAST		BIT(15)
+#define NFP_FL_OUT_FLAGS_USE_TUN	BIT(4)
+#define NFP_FL_OUT_FLAGS_TYPE_IDX	GENMASK(2, 0)
+
+#define NFP_FL_PUSH_VLAN_PRIO		GENMASK(15, 13)
+#define NFP_FL_PUSH_VLAN_CFI		BIT(12)
+#define NFP_FL_PUSH_VLAN_VID		GENMASK(11, 0)
+
+struct nfp_fl_output {
+	__be16 a_op;
+	__be16 flags;
+	__be32 port;
+};
+
+struct nfp_fl_push_vlan {
+	__be16 a_op;
+	__be16 reserved;
+	__be16 vlan_tpid;
+	__be16 vlan_tci;
+};
+
+struct nfp_fl_pop_vlan {
+	__be16 a_op;
+	__be16 reserved;
+};
+
+/* Metadata without L2 (1W/4B)
+ * ----------------------------------------------------------------
+ *    3                   2                   1
+ *  1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |  key_layers   |    mask_id    |           reserved            |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+struct nfp_flower_meta_one {
+	u8 nfp_flow_key_layer;
+	u8 mask_id;
+	u16 reserved;
+};
+
+/* Metadata with L2 (1W/4B)
+ * ----------------------------------------------------------------
+ *    3                   2                   1
+ *  1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |    key_type   |    mask_id    | PCP |p|   vlan outermost VID  |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *                                 ^                               ^
+ *                           NOTE: |             TCI               |
+ *                                 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+struct nfp_flower_meta_two {
+	u8 nfp_flow_key_layer;
+	u8 mask_id;
+	__be16 tci;
+};
+
+/* Port details (1W/4B)
+ * ----------------------------------------------------------------
+ *    3                   2                   1
+ *  1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                         port_ingress                          |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+struct nfp_flower_in_port {
+	__be32 in_port;
+};
+
+/* L2 details (4W/16B)
+ *    3                   2                   1
+ *  1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                     mac_addr_dst, 31 - 0                      |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |      mac_addr_dst, 47 - 32    |     mac_addr_src, 15 - 0      |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                     mac_addr_src, 47 - 16                     |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |       mpls outermost label            |  TC |B|   reserved  |q|
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+struct nfp_flower_mac_mpls {
+	u8 mac_dst[6];
+	u8 mac_src[6];
+	__be32 mpls_lse;
+};
+
+/* L4 ports (for UDP, TCP, SCTP) (1W/4B)
+ *    3                   2                   1
+ *  1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |            port_src           |           port_dst            |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+struct nfp_flower_tp_ports {
+	__be16 port_src;
+	__be16 port_dst;
+};
+
+/* L3 IPv4 details (3W/12B)
+ *    3                   2                   1
+ *  1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |    DSCP   |ECN|   protocol    |           reserved            |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                        ipv4_addr_src                          |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                        ipv4_addr_dst                          |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+struct nfp_flower_ipv4 {
+	u8 tos;
+	u8 proto;
+	u8 ttl;
+	u8 reserved;
+	__be32 ipv4_src;
+	__be32 ipv4_dst;
+};
+
+/* L3 IPv6 details (10W/40B)
+ *    3                   2                   1
+ *  1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |    DSCP   |ECN|   protocol    |          reserved             |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |   ipv6_exthdr   | res |            ipv6_flow_label            |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                  ipv6_addr_src,   31 - 0                      |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                  ipv6_addr_src,  63 - 32                      |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                  ipv6_addr_src,  95 - 64                      |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                  ipv6_addr_src, 127 - 96                      |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                  ipv6_addr_dst,   31 - 0                      |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                  ipv6_addr_dst,  63 - 32                      |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                  ipv6_addr_dst,  95 - 64                      |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                  ipv6_addr_dst, 127 - 96                      |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+struct nfp_flower_ipv6 {
+	u8 tos;
+	u8 proto;
+	u8 ttl;
+	u8 reserved;
+	__be32 ipv6_flow_label_exthdr;
+	struct in6_addr ipv6_src;
+	struct in6_addr ipv6_dst;
+};
+
+/* The base header for a control message packet.
+ * Defines an 8-bit version, and an 8-bit type, padded
+ * to a 32-bit word. Rest of the packet is type-specific.
+ */
+struct nfp_flower_cmsg_hdr {
+	__be16 pad;
+	u8 type;
+	u8 version;
+};
+
+#define NFP_FLOWER_CMSG_HLEN		sizeof(struct nfp_flower_cmsg_hdr)
+#define NFP_FLOWER_CMSG_VER1		1
+
+/* Types defined for port related control messages  */
+enum nfp_flower_cmsg_type_port {
+	NFP_FLOWER_CMSG_TYPE_FLOW_ADD =		0,
+	NFP_FLOWER_CMSG_TYPE_FLOW_DEL =		2,
+	NFP_FLOWER_CMSG_TYPE_PORT_MOD =		8,
+	NFP_FLOWER_CMSG_TYPE_FLOW_STATS =	15,
+	NFP_FLOWER_CMSG_TYPE_PORT_ECHO =	16,
+	NFP_FLOWER_CMSG_TYPE_MAX =		32,
+};
+
+/* NFP_FLOWER_CMSG_TYPE_PORT_MOD */
+struct nfp_flower_cmsg_portmod {
+	__be32 portnum;
+	u8 reserved;
+	u8 info;
+	__be16 mtu;
+};
+
+#define NFP_FLOWER_CMSG_PORTMOD_INFO_LINK	BIT(0)
+
+enum nfp_flower_cmsg_port_type {
+	NFP_FLOWER_CMSG_PORT_TYPE_UNSPEC =	0x0,
+	NFP_FLOWER_CMSG_PORT_TYPE_PHYS_PORT =	0x1,
+	NFP_FLOWER_CMSG_PORT_TYPE_PCIE_PORT =	0x2,
+};
+
+enum nfp_flower_cmsg_port_vnic_type {
+	NFP_FLOWER_CMSG_PORT_VNIC_TYPE_VF =	0x0,
+	NFP_FLOWER_CMSG_PORT_VNIC_TYPE_PF =	0x1,
+	NFP_FLOWER_CMSG_PORT_VNIC_TYPE_CTRL =	0x2,
+};
+
+#define NFP_FLOWER_CMSG_PORT_TYPE		GENMASK(31, 28)
+#define NFP_FLOWER_CMSG_PORT_SYS_ID		GENMASK(27, 24)
+#define NFP_FLOWER_CMSG_PORT_NFP_ID		GENMASK(23, 22)
+#define NFP_FLOWER_CMSG_PORT_PCI		GENMASK(15, 14)
+#define NFP_FLOWER_CMSG_PORT_VNIC_TYPE		GENMASK(13, 12)
+#define NFP_FLOWER_CMSG_PORT_VNIC		GENMASK(11, 6)
+#define NFP_FLOWER_CMSG_PORT_PCIE_Q		GENMASK(5, 0)
+#define NFP_FLOWER_CMSG_PORT_PHYS_PORT_NUM	GENMASK(7, 0)
+
+static inline u32 nfp_flower_cmsg_phys_port(u8 phys_port)
+{
+	return FIELD_PREP(NFP_FLOWER_CMSG_PORT_PHYS_PORT_NUM, phys_port) |
+		FIELD_PREP(NFP_FLOWER_CMSG_PORT_TYPE,
+			   NFP_FLOWER_CMSG_PORT_TYPE_PHYS_PORT);
+}
+
+static inline u32
+nfp_flower_cmsg_pcie_port(u8 nfp_pcie, enum nfp_flower_cmsg_port_vnic_type type,
+			  u8 vnic, u8 q)
+{
+	return FIELD_PREP(NFP_FLOWER_CMSG_PORT_PCI, nfp_pcie) |
+		FIELD_PREP(NFP_FLOWER_CMSG_PORT_VNIC_TYPE, type) |
+		FIELD_PREP(NFP_FLOWER_CMSG_PORT_VNIC, vnic) |
+		FIELD_PREP(NFP_FLOWER_CMSG_PORT_PCIE_Q, q) |
+		FIELD_PREP(NFP_FLOWER_CMSG_PORT_TYPE,
+			   NFP_FLOWER_CMSG_PORT_TYPE_PCIE_PORT);
+}
+
+static inline void *nfp_flower_cmsg_get_data(struct sk_buff *skb)
+{
+	return (unsigned char *)skb->data + NFP_FLOWER_CMSG_HLEN;
+}
+
+int nfp_flower_cmsg_portmod(struct nfp_repr *repr, bool carrier_ok);
+void nfp_flower_cmsg_rx(struct nfp_app *app, struct sk_buff *skb);
+struct sk_buff *
+nfp_flower_cmsg_alloc(struct nfp_app *app, unsigned int size,
+		      enum nfp_flower_cmsg_type_port type);
+
+#endif
diff --git a/drivers/net/ethernet/netronome/nfp/flower/main.c b/drivers/net/ethernet/netronome/nfp/flower/main.c
new file mode 100644
index 000000000000..fc10f27e0a0c
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/flower/main.c
@@ -0,0 +1,390 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/etherdevice.h>
+#include <linux/pci.h>
+#include <linux/skbuff.h>
+#include <linux/vmalloc.h>
+#include <net/devlink.h>
+#include <net/dst_metadata.h>
+
+#include "main.h"
+#include "../nfpcore/nfp_cpp.h"
+#include "../nfpcore/nfp_nffw.h"
+#include "../nfpcore/nfp_nsp.h"
+#include "../nfp_app.h"
+#include "../nfp_main.h"
+#include "../nfp_net.h"
+#include "../nfp_net_repr.h"
+#include "../nfp_port.h"
+#include "./cmsg.h"
+
+#define NFP_FLOWER_ALLOWED_VER 0x0001000000010000UL
+
+static const char *nfp_flower_extra_cap(struct nfp_app *app, struct nfp_net *nn)
+{
+	return "FLOWER";
+}
+
+static enum devlink_eswitch_mode eswitch_mode_get(struct nfp_app *app)
+{
+	return DEVLINK_ESWITCH_MODE_SWITCHDEV;
+}
+
+static enum nfp_repr_type
+nfp_flower_repr_get_type_and_port(struct nfp_app *app, u32 port_id, u8 *port)
+{
+	switch (FIELD_GET(NFP_FLOWER_CMSG_PORT_TYPE, port_id)) {
+	case NFP_FLOWER_CMSG_PORT_TYPE_PHYS_PORT:
+		*port = FIELD_GET(NFP_FLOWER_CMSG_PORT_PHYS_PORT_NUM,
+				  port_id);
+		return NFP_REPR_TYPE_PHYS_PORT;
+
+	case NFP_FLOWER_CMSG_PORT_TYPE_PCIE_PORT:
+		*port = FIELD_GET(NFP_FLOWER_CMSG_PORT_VNIC, port_id);
+		if (FIELD_GET(NFP_FLOWER_CMSG_PORT_VNIC_TYPE, port_id) ==
+		    NFP_FLOWER_CMSG_PORT_VNIC_TYPE_PF)
+			return NFP_REPR_TYPE_PF;
+		else
+			return NFP_REPR_TYPE_VF;
+	}
+
+	return NFP_FLOWER_CMSG_PORT_TYPE_UNSPEC;
+}
+
+static struct net_device *
+nfp_flower_repr_get(struct nfp_app *app, u32 port_id)
+{
+	enum nfp_repr_type repr_type;
+	struct nfp_reprs *reprs;
+	u8 port = 0;
+
+	repr_type = nfp_flower_repr_get_type_and_port(app, port_id, &port);
+
+	reprs = rcu_dereference(app->reprs[repr_type]);
+	if (!reprs)
+		return NULL;
+
+	if (port >= reprs->num_reprs)
+		return NULL;
+
+	return reprs->reprs[port];
+}
+
+static int
+nfp_flower_repr_netdev_open(struct nfp_app *app, struct nfp_repr *repr)
+{
+	int err;
+
+	err = nfp_flower_cmsg_portmod(repr, true);
+	if (err)
+		return err;
+
+	netif_carrier_on(repr->netdev);
+	netif_tx_wake_all_queues(repr->netdev);
+
+	return 0;
+}
+
+static int
+nfp_flower_repr_netdev_stop(struct nfp_app *app, struct nfp_repr *repr)
+{
+	netif_carrier_off(repr->netdev);
+	netif_tx_disable(repr->netdev);
+
+	return nfp_flower_cmsg_portmod(repr, false);
+}
+
+static void nfp_flower_sriov_disable(struct nfp_app *app)
+{
+	nfp_reprs_clean_and_free_by_type(app, NFP_REPR_TYPE_VF);
+}
+
+static int
+nfp_flower_spawn_vnic_reprs(struct nfp_app *app,
+			    enum nfp_flower_cmsg_port_vnic_type vnic_type,
+			    enum nfp_repr_type repr_type, unsigned int cnt)
+{
+	u8 nfp_pcie = nfp_cppcore_pcie_unit(app->pf->cpp);
+	struct nfp_flower_priv *priv = app->priv;
+	struct nfp_reprs *reprs, *old_reprs;
+	enum nfp_port_type port_type;
+	const u8 queue = 0;
+	int i, err;
+
+	port_type = repr_type == NFP_REPR_TYPE_PF ? NFP_PORT_PF_PORT :
+						    NFP_PORT_VF_PORT;
+
+	reprs = nfp_reprs_alloc(cnt);
+	if (!reprs)
+		return -ENOMEM;
+
+	for (i = 0; i < cnt; i++) {
+		struct nfp_port *port;
+		u32 port_id;
+
+		reprs->reprs[i] = nfp_repr_alloc(app);
+		if (!reprs->reprs[i]) {
+			err = -ENOMEM;
+			goto err_reprs_clean;
+		}
+
+		port = nfp_port_alloc(app, port_type, reprs->reprs[i]);
+		if (repr_type == NFP_REPR_TYPE_PF) {
+			port->pf_id = i;
+		} else {
+			port->pf_id = 0; /* For now we only support 1 PF */
+			port->vf_id = i;
+		}
+
+		eth_hw_addr_random(reprs->reprs[i]);
+
+		port_id = nfp_flower_cmsg_pcie_port(nfp_pcie, vnic_type,
+						    i, queue);
+		err = nfp_repr_init(app, reprs->reprs[i],
+				    port_id, port, priv->nn->dp.netdev);
+		if (err) {
+			nfp_port_free(port);
+			goto err_reprs_clean;
+		}
+
+		nfp_info(app->cpp, "%s%d Representor(%s) created\n",
+			 repr_type == NFP_REPR_TYPE_PF ? "PF" : "VF", i,
+			 reprs->reprs[i]->name);
+	}
+
+	old_reprs = nfp_app_reprs_set(app, repr_type, reprs);
+	if (IS_ERR(old_reprs)) {
+		err = PTR_ERR(old_reprs);
+		goto err_reprs_clean;
+	}
+
+	return 0;
+err_reprs_clean:
+	nfp_reprs_clean_and_free(reprs);
+	return err;
+}
+
+static int nfp_flower_sriov_enable(struct nfp_app *app, int num_vfs)
+{
+	return nfp_flower_spawn_vnic_reprs(app,
+					   NFP_FLOWER_CMSG_PORT_VNIC_TYPE_VF,
+					   NFP_REPR_TYPE_VF, num_vfs);
+}
+
+static void nfp_flower_stop(struct nfp_app *app)
+{
+	nfp_reprs_clean_and_free_by_type(app, NFP_REPR_TYPE_PF);
+	nfp_reprs_clean_and_free_by_type(app, NFP_REPR_TYPE_PHYS_PORT);
+
+}
+
+static int
+nfp_flower_spawn_phy_reprs(struct nfp_app *app, struct nfp_flower_priv *priv)
+{
+	struct nfp_eth_table *eth_tbl = app->pf->eth_tbl;
+	struct nfp_reprs *reprs, *old_reprs;
+	unsigned int i;
+	int err;
+
+	reprs = nfp_reprs_alloc(eth_tbl->max_index + 1);
+	if (!reprs)
+		return -ENOMEM;
+
+	for (i = 0; i < eth_tbl->count; i++) {
+		int phys_port = eth_tbl->ports[i].index;
+		struct nfp_port *port;
+		u32 cmsg_port_id;
+
+		reprs->reprs[phys_port] = nfp_repr_alloc(app);
+		if (!reprs->reprs[phys_port]) {
+			err = -ENOMEM;
+			goto err_reprs_clean;
+		}
+
+		port = nfp_port_alloc(app, NFP_PORT_PHYS_PORT,
+				      reprs->reprs[phys_port]);
+		if (IS_ERR(port)) {
+			err = PTR_ERR(port);
+			goto err_reprs_clean;
+		}
+		err = nfp_port_init_phy_port(app->pf, app, port, i);
+		if (err) {
+			nfp_port_free(port);
+			goto err_reprs_clean;
+		}
+
+		SET_NETDEV_DEV(reprs->reprs[phys_port], &priv->nn->pdev->dev);
+		nfp_net_get_mac_addr(app->pf, port);
+
+		cmsg_port_id = nfp_flower_cmsg_phys_port(phys_port);
+		err = nfp_repr_init(app, reprs->reprs[phys_port],
+				    cmsg_port_id, port, priv->nn->dp.netdev);
+		if (err) {
+			nfp_port_free(port);
+			goto err_reprs_clean;
+		}
+
+		nfp_info(app->cpp, "Phys Port %d Representor(%s) created\n",
+			 phys_port, reprs->reprs[phys_port]->name);
+	}
+
+	old_reprs = nfp_app_reprs_set(app, NFP_REPR_TYPE_PHYS_PORT, reprs);
+	if (IS_ERR(old_reprs)) {
+		err = PTR_ERR(old_reprs);
+		goto err_reprs_clean;
+	}
+
+	return 0;
+err_reprs_clean:
+	nfp_reprs_clean_and_free(reprs);
+	return err;
+}
+
+static int nfp_flower_start(struct nfp_app *app)
+{
+	int err;
+
+	err = nfp_flower_spawn_phy_reprs(app, app->priv);
+	if (err)
+		return err;
+
+	return nfp_flower_spawn_vnic_reprs(app,
+					   NFP_FLOWER_CMSG_PORT_VNIC_TYPE_PF,
+					   NFP_REPR_TYPE_PF, 1);
+}
+
+static int nfp_flower_vnic_init(struct nfp_app *app, struct nfp_net *nn,
+				unsigned int id)
+{
+	struct nfp_flower_priv *priv = app->priv;
+
+	if (id > 0) {
+		nfp_warn(app->cpp, "FlowerNIC doesn't support more than one data vNIC\n");
+		goto err_invalid_port;
+	}
+
+	priv->nn = nn;
+
+	eth_hw_addr_random(nn->dp.netdev);
+	netif_keep_dst(nn->dp.netdev);
+
+	return 0;
+
+err_invalid_port:
+	nn->port = nfp_port_alloc(app, NFP_PORT_INVALID, nn->dp.netdev);
+	return PTR_ERR_OR_ZERO(nn->port);
+}
+
+static int nfp_flower_init(struct nfp_app *app)
+{
+	const struct nfp_pf *pf = app->pf;
+	u64 version;
+	int err;
+
+	if (!pf->eth_tbl) {
+		nfp_warn(app->cpp, "FlowerNIC requires eth table\n");
+		return -EINVAL;
+	}
+
+	if (!pf->mac_stats_bar) {
+		nfp_warn(app->cpp, "FlowerNIC requires mac_stats BAR\n");
+		return -EINVAL;
+	}
+
+	if (!pf->vf_cfg_bar) {
+		nfp_warn(app->cpp, "FlowerNIC requires vf_cfg BAR\n");
+		return -EINVAL;
+	}
+
+	version = nfp_rtsym_read_le(app->pf->rtbl, "hw_flower_version", &err);
+	if (err) {
+		nfp_warn(app->cpp, "FlowerNIC requires hw_flower_version memory symbol\n");
+		return err;
+	}
+
+	/* We need to ensure hardware has enough flower capabilities. */
+	if (version != NFP_FLOWER_ALLOWED_VER) {
+		nfp_warn(app->cpp, "FlowerNIC: unsupported firmware version\n");
+		return -EINVAL;
+	}
+
+	app->priv = vzalloc(sizeof(struct nfp_flower_priv));
+	if (!app->priv)
+		return -ENOMEM;
+
+	err = nfp_flower_metadata_init(app);
+	if (err)
+		goto err_free_app_priv;
+
+	return 0;
+
+err_free_app_priv:
+	vfree(app->priv);
+	return err;
+}
+
+static void nfp_flower_clean(struct nfp_app *app)
+{
+	vfree(app->priv);
+	app->priv = NULL;
+}
+
+const struct nfp_app_type app_flower = {
+	.id		= NFP_APP_FLOWER_NIC,
+	.name		= "flower",
+	.ctrl_has_meta	= true,
+
+	.extra_cap	= nfp_flower_extra_cap,
+
+	.init		= nfp_flower_init,
+	.clean		= nfp_flower_clean,
+
+	.vnic_init	= nfp_flower_vnic_init,
+
+	.repr_open	= nfp_flower_repr_netdev_open,
+	.repr_stop	= nfp_flower_repr_netdev_stop,
+
+	.start		= nfp_flower_start,
+	.stop		= nfp_flower_stop,
+
+	.ctrl_msg_rx	= nfp_flower_cmsg_rx,
+
+	.sriov_enable	= nfp_flower_sriov_enable,
+	.sriov_disable	= nfp_flower_sriov_disable,
+
+	.eswitch_mode_get  = eswitch_mode_get,
+	.repr_get	= nfp_flower_repr_get,
+
+	.setup_tc	= nfp_flower_setup_tc,
+};
diff --git a/drivers/net/ethernet/netronome/nfp/flower/main.h b/drivers/net/ethernet/netronome/nfp/flower/main.h
new file mode 100644
index 000000000000..9e64c048e83f
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/flower/main.h
@@ -0,0 +1,159 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef __NFP_FLOWER_H__
+#define __NFP_FLOWER_H__ 1
+
+#include <linux/circ_buf.h>
+#include <linux/hashtable.h>
+#include <linux/time64.h>
+#include <linux/types.h>
+
+struct tc_to_netdev;
+struct net_device;
+struct nfp_app;
+
+#define NFP_FL_STATS_ENTRY_RS		BIT(20)
+#define NFP_FL_STATS_ELEM_RS		4
+#define NFP_FL_REPEATED_HASH_MAX	BIT(17)
+#define NFP_FLOWER_HASH_BITS		19
+#define NFP_FLOWER_MASK_ENTRY_RS	256
+#define NFP_FLOWER_MASK_ELEMENT_RS	1
+#define NFP_FLOWER_MASK_HASH_BITS	10
+
+#define NFP_FL_META_FLAG_NEW_MASK	128
+#define NFP_FL_META_FLAG_LAST_MASK	1
+
+#define NFP_FL_MASK_REUSE_TIME_NS	40000
+#define NFP_FL_MASK_ID_LOCATION		1
+
+struct nfp_fl_mask_id {
+	struct circ_buf mask_id_free_list;
+	struct timespec64 *last_used;
+	u8 init_unallocated;
+};
+
+struct nfp_fl_stats_id {
+	struct circ_buf free_list;
+	u32 init_unalloc;
+	u8 repeated_em_count;
+};
+
+/**
+ * struct nfp_flower_priv - Flower APP per-vNIC priv data
+ * @nn:			Pointer to vNIC
+ * @mask_id_seed:	Seed used for mask hash table
+ * @flower_version:	HW version of flower
+ * @stats_ids:		List of free stats ids
+ * @mask_ids:		List of free mask ids
+ * @mask_table:		Hash table used to store masks
+ * @flow_table:		Hash table used to store flower rules
+ */
+struct nfp_flower_priv {
+	struct nfp_net *nn;
+	u32 mask_id_seed;
+	u64 flower_version;
+	struct nfp_fl_stats_id stats_ids;
+	struct nfp_fl_mask_id mask_ids;
+	DECLARE_HASHTABLE(mask_table, NFP_FLOWER_MASK_HASH_BITS);
+	DECLARE_HASHTABLE(flow_table, NFP_FLOWER_HASH_BITS);
+};
+
+struct nfp_fl_key_ls {
+	u32 key_layer_two;
+	u8 key_layer;
+	int key_size;
+};
+
+struct nfp_fl_rule_metadata {
+	u8 key_len;
+	u8 mask_len;
+	u8 act_len;
+	u8 flags;
+	__be32 host_ctx_id;
+	__be64 host_cookie __packed;
+	__be64 flow_version __packed;
+	__be32 shortcut;
+};
+
+struct nfp_fl_stats {
+	u64 pkts;
+	u64 bytes;
+	u64 used;
+};
+
+struct nfp_fl_payload {
+	struct nfp_fl_rule_metadata meta;
+	unsigned long tc_flower_cookie;
+	struct hlist_node link;
+	struct rcu_head rcu;
+	spinlock_t lock; /* lock stats */
+	struct nfp_fl_stats stats;
+	char *unmasked_data;
+	char *mask_data;
+	char *action_data;
+};
+
+struct nfp_fl_stats_frame {
+	__be32 stats_con_id;
+	__be32 pkt_count;
+	__be64 byte_count;
+	__be64 stats_cookie;
+};
+
+int nfp_flower_metadata_init(struct nfp_app *app);
+void nfp_flower_metadata_cleanup(struct nfp_app *app);
+
+int nfp_flower_setup_tc(struct nfp_app *app, struct net_device *netdev,
+			u32 handle, __be16 proto, struct tc_to_netdev *tc);
+int nfp_flower_compile_flow_match(struct tc_cls_flower_offload *flow,
+				  struct nfp_fl_key_ls *key_ls,
+				  struct net_device *netdev,
+				  struct nfp_fl_payload *nfp_flow);
+int nfp_flower_compile_action(struct tc_cls_flower_offload *flow,
+			      struct net_device *netdev,
+			      struct nfp_fl_payload *nfp_flow);
+int nfp_compile_flow_metadata(struct nfp_app *app,
+			      struct tc_cls_flower_offload *flow,
+			      struct nfp_fl_payload *nfp_flow);
+int nfp_modify_flow_metadata(struct nfp_app *app,
+			     struct nfp_fl_payload *nfp_flow);
+
+struct nfp_fl_payload *
+nfp_flower_search_fl_table(struct nfp_app *app, unsigned long tc_flower_cookie);
+struct nfp_fl_payload *
+nfp_flower_remove_fl_table(struct nfp_app *app, unsigned long tc_flower_cookie);
+
+void nfp_flower_rx_flow_stats(struct nfp_app *app, struct sk_buff *skb);
+
+#endif
diff --git a/drivers/net/ethernet/netronome/nfp/flower/match.c b/drivers/net/ethernet/netronome/nfp/flower/match.c
new file mode 100644
index 000000000000..0e08404480ef
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/flower/match.c
@@ -0,0 +1,292 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/bitfield.h>
+#include <net/pkt_cls.h>
+
+#include "cmsg.h"
+#include "main.h"
+
+static void
+nfp_flower_compile_meta_tci(struct nfp_flower_meta_two *frame,
+			    struct tc_cls_flower_offload *flow, u8 key_type,
+			    bool mask_version)
+{
+	struct flow_dissector_key_vlan *flow_vlan;
+	u16 tmp_tci;
+
+	/* Populate the metadata frame. */
+	frame->nfp_flow_key_layer = key_type;
+	frame->mask_id = ~0;
+
+	if (mask_version) {
+		frame->tci = cpu_to_be16(~0);
+		return;
+	}
+
+	flow_vlan = skb_flow_dissector_target(flow->dissector,
+					      FLOW_DISSECTOR_KEY_VLAN,
+					      flow->key);
+
+	/* Populate the tci field. */
+	if (!flow_vlan->vlan_id) {
+		tmp_tci = 0;
+	} else {
+		tmp_tci = FIELD_PREP(NFP_FLOWER_MASK_VLAN_PRIO,
+				     flow_vlan->vlan_priority) |
+			  FIELD_PREP(NFP_FLOWER_MASK_VLAN_VID,
+				     flow_vlan->vlan_id) |
+			  NFP_FLOWER_MASK_VLAN_CFI;
+	}
+	frame->tci = cpu_to_be16(tmp_tci);
+}
+
+static void
+nfp_flower_compile_meta(struct nfp_flower_meta_one *frame, u8 key_type)
+{
+	frame->nfp_flow_key_layer = key_type;
+	frame->mask_id = 0;
+	frame->reserved = 0;
+}
+
+static int
+nfp_flower_compile_port(struct nfp_flower_in_port *frame, u32 cmsg_port,
+			bool mask_version)
+{
+	if (mask_version) {
+		frame->in_port = cpu_to_be32(~0);
+		return 0;
+	}
+
+	frame->in_port = cpu_to_be32(cmsg_port);
+
+	return 0;
+}
+
+static void
+nfp_flower_compile_mac(struct nfp_flower_mac_mpls *frame,
+		       struct tc_cls_flower_offload *flow,
+		       bool mask_version)
+{
+	struct fl_flow_key *target = mask_version ? flow->mask : flow->key;
+	struct flow_dissector_key_eth_addrs *flow_mac;
+
+	flow_mac = skb_flow_dissector_target(flow->dissector,
+					     FLOW_DISSECTOR_KEY_ETH_ADDRS,
+					     target);
+
+	memset(frame, 0, sizeof(struct nfp_flower_mac_mpls));
+
+	/* Populate mac frame. */
+	ether_addr_copy(frame->mac_dst, &flow_mac->dst[0]);
+	ether_addr_copy(frame->mac_src, &flow_mac->src[0]);
+
+	if (mask_version)
+		frame->mpls_lse = cpu_to_be32(~0);
+}
+
+static void
+nfp_flower_compile_tport(struct nfp_flower_tp_ports *frame,
+			 struct tc_cls_flower_offload *flow,
+			 bool mask_version)
+{
+	struct fl_flow_key *target = mask_version ? flow->mask : flow->key;
+	struct flow_dissector_key_ports *flow_tp;
+
+	flow_tp = skb_flow_dissector_target(flow->dissector,
+					    FLOW_DISSECTOR_KEY_PORTS,
+					    target);
+
+	frame->port_src = flow_tp->src;
+	frame->port_dst = flow_tp->dst;
+}
+
+static void
+nfp_flower_compile_ipv4(struct nfp_flower_ipv4 *frame,
+			struct tc_cls_flower_offload *flow,
+			bool mask_version)
+{
+	struct fl_flow_key *target = mask_version ? flow->mask : flow->key;
+	struct flow_dissector_key_ipv4_addrs *flow_ipv4;
+	struct flow_dissector_key_basic *flow_basic;
+
+	flow_ipv4 = skb_flow_dissector_target(flow->dissector,
+					      FLOW_DISSECTOR_KEY_IPV4_ADDRS,
+					      target);
+
+	flow_basic = skb_flow_dissector_target(flow->dissector,
+					       FLOW_DISSECTOR_KEY_BASIC,
+					       target);
+
+	/* Populate IPv4 frame. */
+	frame->reserved = 0;
+	frame->ipv4_src = flow_ipv4->src;
+	frame->ipv4_dst = flow_ipv4->dst;
+	frame->proto = flow_basic->ip_proto;
+	/* Wildcard TOS/TTL for now. */
+	frame->tos = 0;
+	frame->ttl = 0;
+}
+
+static void
+nfp_flower_compile_ipv6(struct nfp_flower_ipv6 *frame,
+			struct tc_cls_flower_offload *flow,
+			bool mask_version)
+{
+	struct fl_flow_key *target = mask_version ? flow->mask : flow->key;
+	struct flow_dissector_key_ipv6_addrs *flow_ipv6;
+	struct flow_dissector_key_basic *flow_basic;
+
+	flow_ipv6 = skb_flow_dissector_target(flow->dissector,
+					      FLOW_DISSECTOR_KEY_IPV6_ADDRS,
+					      target);
+
+	flow_basic = skb_flow_dissector_target(flow->dissector,
+					       FLOW_DISSECTOR_KEY_BASIC,
+					       target);
+
+	/* Populate IPv6 frame. */
+	frame->reserved = 0;
+	frame->ipv6_src = flow_ipv6->src;
+	frame->ipv6_dst = flow_ipv6->dst;
+	frame->proto = flow_basic->ip_proto;
+	/* Wildcard LABEL/TOS/TTL for now. */
+	frame->ipv6_flow_label_exthdr = 0;
+	frame->tos = 0;
+	frame->ttl = 0;
+}
+
+int nfp_flower_compile_flow_match(struct tc_cls_flower_offload *flow,
+				  struct nfp_fl_key_ls *key_ls,
+				  struct net_device *netdev,
+				  struct nfp_fl_payload *nfp_flow)
+{
+	int err;
+	u8 *ext;
+	u8 *msk;
+
+	memset(nfp_flow->unmasked_data, 0, key_ls->key_size);
+	memset(nfp_flow->mask_data, 0, key_ls->key_size);
+
+	ext = nfp_flow->unmasked_data;
+	msk = nfp_flow->mask_data;
+	if (NFP_FLOWER_LAYER_PORT & key_ls->key_layer) {
+		/* Populate Exact Metadata. */
+		nfp_flower_compile_meta_tci((struct nfp_flower_meta_two *)ext,
+					    flow, key_ls->key_layer, false);
+		/* Populate Mask Metadata. */
+		nfp_flower_compile_meta_tci((struct nfp_flower_meta_two *)msk,
+					    flow, key_ls->key_layer, true);
+		ext += sizeof(struct nfp_flower_meta_two);
+		msk += sizeof(struct nfp_flower_meta_two);
+
+		/* Populate Exact Port data. */
+		err = nfp_flower_compile_port((struct nfp_flower_in_port *)ext,
+					      nfp_repr_get_port_id(netdev),
+					      false);
+		if (err)
+			return err;
+
+		/* Populate Mask Port Data. */
+		err = nfp_flower_compile_port((struct nfp_flower_in_port *)msk,
+					      nfp_repr_get_port_id(netdev),
+					      true);
+		if (err)
+			return err;
+
+		ext += sizeof(struct nfp_flower_in_port);
+		msk += sizeof(struct nfp_flower_in_port);
+	} else {
+		/* Populate Exact Metadata. */
+		nfp_flower_compile_meta((struct nfp_flower_meta_one *)ext,
+					key_ls->key_layer);
+		/* Populate Mask Metadata. */
+		nfp_flower_compile_meta((struct nfp_flower_meta_one *)msk,
+					key_ls->key_layer);
+		ext += sizeof(struct nfp_flower_meta_one);
+		msk += sizeof(struct nfp_flower_meta_one);
+	}
+
+	if (NFP_FLOWER_LAYER_META & key_ls->key_layer) {
+		/* Additional Metadata Fields.
+		 * Currently unsupported.
+		 */
+		return -EOPNOTSUPP;
+	}
+
+	if (NFP_FLOWER_LAYER_MAC & key_ls->key_layer) {
+		/* Populate Exact MAC Data. */
+		nfp_flower_compile_mac((struct nfp_flower_mac_mpls *)ext,
+				       flow, false);
+		/* Populate Mask MAC Data. */
+		nfp_flower_compile_mac((struct nfp_flower_mac_mpls *)msk,
+				       flow, true);
+		ext += sizeof(struct nfp_flower_mac_mpls);
+		msk += sizeof(struct nfp_flower_mac_mpls);
+	}
+
+	if (NFP_FLOWER_LAYER_TP & key_ls->key_layer) {
+		/* Populate Exact TP Data. */
+		nfp_flower_compile_tport((struct nfp_flower_tp_ports *)ext,
+					 flow, false);
+		/* Populate Mask TP Data. */
+		nfp_flower_compile_tport((struct nfp_flower_tp_ports *)msk,
+					 flow, true);
+		ext += sizeof(struct nfp_flower_tp_ports);
+		msk += sizeof(struct nfp_flower_tp_ports);
+	}
+
+	if (NFP_FLOWER_LAYER_IPV4 & key_ls->key_layer) {
+		/* Populate Exact IPv4 Data. */
+		nfp_flower_compile_ipv4((struct nfp_flower_ipv4 *)ext,
+					flow, false);
+		/* Populate Mask IPv4 Data. */
+		nfp_flower_compile_ipv4((struct nfp_flower_ipv4 *)msk,
+					flow, true);
+		ext += sizeof(struct nfp_flower_ipv4);
+		msk += sizeof(struct nfp_flower_ipv4);
+	}
+
+	if (NFP_FLOWER_LAYER_IPV6 & key_ls->key_layer) {
+		/* Populate Exact IPv4 Data. */
+		nfp_flower_compile_ipv6((struct nfp_flower_ipv6 *)ext,
+					flow, false);
+		/* Populate Mask IPv4 Data. */
+		nfp_flower_compile_ipv6((struct nfp_flower_ipv6 *)msk,
+					flow, true);
+		ext += sizeof(struct nfp_flower_ipv6);
+		msk += sizeof(struct nfp_flower_ipv6);
+	}
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/netronome/nfp/flower/metadata.c b/drivers/net/ethernet/netronome/nfp/flower/metadata.c
new file mode 100644
index 000000000000..fec0ff2ca94f
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/flower/metadata.c
@@ -0,0 +1,438 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/hash.h>
+#include <linux/hashtable.h>
+#include <linux/jhash.h>
+#include <linux/vmalloc.h>
+#include <net/pkt_cls.h>
+
+#include "cmsg.h"
+#include "main.h"
+#include "../nfp_app.h"
+
+struct nfp_mask_id_table {
+	struct hlist_node link;
+	u32 hash_key;
+	u32 ref_cnt;
+	u8 mask_id;
+};
+
+static int nfp_release_stats_entry(struct nfp_app *app, u32 stats_context_id)
+{
+	struct nfp_flower_priv *priv = app->priv;
+	struct circ_buf *ring;
+
+	ring = &priv->stats_ids.free_list;
+	/* Check if buffer is full. */
+	if (!CIRC_SPACE(ring->head, ring->tail, NFP_FL_STATS_ENTRY_RS *
+			NFP_FL_STATS_ELEM_RS -
+			NFP_FL_STATS_ELEM_RS + 1))
+		return -ENOBUFS;
+
+	memcpy(&ring->buf[ring->head], &stats_context_id, NFP_FL_STATS_ELEM_RS);
+	ring->head = (ring->head + NFP_FL_STATS_ELEM_RS) %
+		     (NFP_FL_STATS_ENTRY_RS * NFP_FL_STATS_ELEM_RS);
+
+	return 0;
+}
+
+static int nfp_get_stats_entry(struct nfp_app *app, u32 *stats_context_id)
+{
+	struct nfp_flower_priv *priv = app->priv;
+	u32 freed_stats_id, temp_stats_id;
+	struct circ_buf *ring;
+
+	ring = &priv->stats_ids.free_list;
+	freed_stats_id = NFP_FL_STATS_ENTRY_RS;
+	/* Check for unallocated entries first. */
+	if (priv->stats_ids.init_unalloc > 0) {
+		*stats_context_id = priv->stats_ids.init_unalloc - 1;
+		priv->stats_ids.init_unalloc--;
+		return 0;
+	}
+
+	/* Check if buffer is empty. */
+	if (ring->head == ring->tail) {
+		*stats_context_id = freed_stats_id;
+		return -ENOENT;
+	}
+
+	memcpy(&temp_stats_id, &ring->buf[ring->tail], NFP_FL_STATS_ELEM_RS);
+	*stats_context_id = temp_stats_id;
+	memcpy(&ring->buf[ring->tail], &freed_stats_id, NFP_FL_STATS_ELEM_RS);
+	ring->tail = (ring->tail + NFP_FL_STATS_ELEM_RS) %
+		     (NFP_FL_STATS_ENTRY_RS * NFP_FL_STATS_ELEM_RS);
+
+	return 0;
+}
+
+/* Must be called with either RTNL or rcu_read_lock */
+struct nfp_fl_payload *
+nfp_flower_search_fl_table(struct nfp_app *app, unsigned long tc_flower_cookie)
+{
+	struct nfp_flower_priv *priv = app->priv;
+	struct nfp_fl_payload *flower_entry;
+
+	hash_for_each_possible_rcu(priv->flow_table, flower_entry, link,
+				   tc_flower_cookie)
+		if (flower_entry->tc_flower_cookie == tc_flower_cookie)
+			return flower_entry;
+
+	return NULL;
+}
+
+static void
+nfp_flower_update_stats(struct nfp_app *app, struct nfp_fl_stats_frame *stats)
+{
+	struct nfp_fl_payload *nfp_flow;
+	unsigned long flower_cookie;
+
+	flower_cookie = be64_to_cpu(stats->stats_cookie);
+
+	rcu_read_lock();
+	nfp_flow = nfp_flower_search_fl_table(app, flower_cookie);
+	if (!nfp_flow)
+		goto exit_rcu_unlock;
+
+	if (nfp_flow->meta.host_ctx_id != stats->stats_con_id)
+		goto exit_rcu_unlock;
+
+	spin_lock(&nfp_flow->lock);
+	nfp_flow->stats.pkts += be32_to_cpu(stats->pkt_count);
+	nfp_flow->stats.bytes += be64_to_cpu(stats->byte_count);
+	nfp_flow->stats.used = jiffies;
+	spin_unlock(&nfp_flow->lock);
+
+exit_rcu_unlock:
+	rcu_read_unlock();
+}
+
+void nfp_flower_rx_flow_stats(struct nfp_app *app, struct sk_buff *skb)
+{
+	unsigned int msg_len = skb->len - NFP_FLOWER_CMSG_HLEN;
+	struct nfp_fl_stats_frame *stats_frame;
+	unsigned char *msg;
+	int i;
+
+	msg = nfp_flower_cmsg_get_data(skb);
+
+	stats_frame = (struct nfp_fl_stats_frame *)msg;
+	for (i = 0; i < msg_len / sizeof(*stats_frame); i++)
+		nfp_flower_update_stats(app, stats_frame + i);
+}
+
+static int nfp_release_mask_id(struct nfp_app *app, u8 mask_id)
+{
+	struct nfp_flower_priv *priv = app->priv;
+	struct circ_buf *ring;
+	struct timespec64 now;
+
+	ring = &priv->mask_ids.mask_id_free_list;
+	/* Checking if buffer is full. */
+	if (CIRC_SPACE(ring->head, ring->tail, NFP_FLOWER_MASK_ENTRY_RS) == 0)
+		return -ENOBUFS;
+
+	memcpy(&ring->buf[ring->head], &mask_id, NFP_FLOWER_MASK_ELEMENT_RS);
+	ring->head = (ring->head + NFP_FLOWER_MASK_ELEMENT_RS) %
+		     (NFP_FLOWER_MASK_ENTRY_RS * NFP_FLOWER_MASK_ELEMENT_RS);
+
+	getnstimeofday64(&now);
+	priv->mask_ids.last_used[mask_id] = now;
+
+	return 0;
+}
+
+static int nfp_mask_alloc(struct nfp_app *app, u8 *mask_id)
+{
+	struct nfp_flower_priv *priv = app->priv;
+	struct timespec64 delta, now;
+	struct circ_buf *ring;
+	u8 temp_id, freed_id;
+
+	ring = &priv->mask_ids.mask_id_free_list;
+	freed_id = NFP_FLOWER_MASK_ENTRY_RS - 1;
+	/* Checking for unallocated entries first. */
+	if (priv->mask_ids.init_unallocated > 0) {
+		*mask_id = priv->mask_ids.init_unallocated;
+		priv->mask_ids.init_unallocated--;
+		return 0;
+	}
+
+	/* Checking if buffer is empty. */
+	if (ring->head == ring->tail)
+		goto err_not_found;
+
+	memcpy(&temp_id, &ring->buf[ring->tail], NFP_FLOWER_MASK_ELEMENT_RS);
+	*mask_id = temp_id;
+
+	getnstimeofday64(&now);
+	delta = timespec64_sub(now, priv->mask_ids.last_used[*mask_id]);
+
+	if (timespec64_to_ns(&delta) < NFP_FL_MASK_REUSE_TIME_NS)
+		goto err_not_found;
+
+	memcpy(&ring->buf[ring->tail], &freed_id, NFP_FLOWER_MASK_ELEMENT_RS);
+	ring->tail = (ring->tail + NFP_FLOWER_MASK_ELEMENT_RS) %
+		     (NFP_FLOWER_MASK_ENTRY_RS * NFP_FLOWER_MASK_ELEMENT_RS);
+
+	return 0;
+
+err_not_found:
+	*mask_id = freed_id;
+	return -ENOENT;
+}
+
+static int
+nfp_add_mask_table(struct nfp_app *app, char *mask_data, u32 mask_len)
+{
+	struct nfp_flower_priv *priv = app->priv;
+	struct nfp_mask_id_table *mask_entry;
+	unsigned long hash_key;
+	u8 mask_id;
+
+	if (nfp_mask_alloc(app, &mask_id))
+		return -ENOENT;
+
+	mask_entry = kmalloc(sizeof(*mask_entry), GFP_KERNEL);
+	if (!mask_entry) {
+		nfp_release_mask_id(app, mask_id);
+		return -ENOMEM;
+	}
+
+	INIT_HLIST_NODE(&mask_entry->link);
+	mask_entry->mask_id = mask_id;
+	hash_key = jhash(mask_data, mask_len, priv->mask_id_seed);
+	mask_entry->hash_key = hash_key;
+	mask_entry->ref_cnt = 1;
+	hash_add(priv->mask_table, &mask_entry->link, hash_key);
+
+	return mask_id;
+}
+
+static struct nfp_mask_id_table *
+nfp_search_mask_table(struct nfp_app *app, char *mask_data, u32 mask_len)
+{
+	struct nfp_flower_priv *priv = app->priv;
+	struct nfp_mask_id_table *mask_entry;
+	unsigned long hash_key;
+
+	hash_key = jhash(mask_data, mask_len, priv->mask_id_seed);
+
+	hash_for_each_possible(priv->mask_table, mask_entry, link, hash_key)
+		if (mask_entry->hash_key == hash_key)
+			return mask_entry;
+
+	return NULL;
+}
+
+static int
+nfp_find_in_mask_table(struct nfp_app *app, char *mask_data, u32 mask_len)
+{
+	struct nfp_mask_id_table *mask_entry;
+
+	mask_entry = nfp_search_mask_table(app, mask_data, mask_len);
+	if (!mask_entry)
+		return -ENOENT;
+
+	mask_entry->ref_cnt++;
+
+	/* Casting u8 to int for later use. */
+	return mask_entry->mask_id;
+}
+
+static bool
+nfp_check_mask_add(struct nfp_app *app, char *mask_data, u32 mask_len,
+		   u8 *meta_flags, u8 *mask_id)
+{
+	int id;
+
+	id = nfp_find_in_mask_table(app, mask_data, mask_len);
+	if (id < 0) {
+		id = nfp_add_mask_table(app, mask_data, mask_len);
+		if (id < 0)
+			return false;
+		*meta_flags |= NFP_FL_META_FLAG_NEW_MASK;
+	}
+	*mask_id = id;
+
+	return true;
+}
+
+static bool
+nfp_check_mask_remove(struct nfp_app *app, char *mask_data, u32 mask_len,
+		      u8 *meta_flags, u8 *mask_id)
+{
+	struct nfp_mask_id_table *mask_entry;
+
+	mask_entry = nfp_search_mask_table(app, mask_data, mask_len);
+	if (!mask_entry)
+		return false;
+
+	*mask_id = mask_entry->mask_id;
+	mask_entry->ref_cnt--;
+	if (!mask_entry->ref_cnt) {
+		hash_del(&mask_entry->link);
+		nfp_release_mask_id(app, *mask_id);
+		kfree(mask_entry);
+		if (meta_flags)
+			*meta_flags |= NFP_FL_META_FLAG_LAST_MASK;
+	}
+
+	return true;
+}
+
+int nfp_compile_flow_metadata(struct nfp_app *app,
+			      struct tc_cls_flower_offload *flow,
+			      struct nfp_fl_payload *nfp_flow)
+{
+	struct nfp_flower_priv *priv = app->priv;
+	struct nfp_fl_payload *check_entry;
+	u8 new_mask_id;
+	u32 stats_cxt;
+
+	if (nfp_get_stats_entry(app, &stats_cxt))
+		return -ENOENT;
+
+	nfp_flow->meta.host_ctx_id = cpu_to_be32(stats_cxt);
+	nfp_flow->meta.host_cookie = cpu_to_be64(flow->cookie);
+
+	new_mask_id = 0;
+	if (!nfp_check_mask_add(app, nfp_flow->mask_data,
+				nfp_flow->meta.mask_len,
+				&nfp_flow->meta.flags, &new_mask_id)) {
+		if (nfp_release_stats_entry(app, stats_cxt))
+			return -EINVAL;
+		return -ENOENT;
+	}
+
+	nfp_flow->meta.flow_version = cpu_to_be64(priv->flower_version);
+	priv->flower_version++;
+
+	/* Update flow payload with mask ids. */
+	nfp_flow->unmasked_data[NFP_FL_MASK_ID_LOCATION] = new_mask_id;
+	nfp_flow->stats.pkts = 0;
+	nfp_flow->stats.bytes = 0;
+	nfp_flow->stats.used = jiffies;
+
+	check_entry = nfp_flower_search_fl_table(app, flow->cookie);
+	if (check_entry) {
+		if (nfp_release_stats_entry(app, stats_cxt))
+			return -EINVAL;
+
+		if (!nfp_check_mask_remove(app, nfp_flow->mask_data,
+					   nfp_flow->meta.mask_len,
+					   NULL, &new_mask_id))
+			return -EINVAL;
+
+		return -EEXIST;
+	}
+
+	return 0;
+}
+
+int nfp_modify_flow_metadata(struct nfp_app *app,
+			     struct nfp_fl_payload *nfp_flow)
+{
+	struct nfp_flower_priv *priv = app->priv;
+	u8 new_mask_id = 0;
+	u32 temp_ctx_id;
+
+	nfp_check_mask_remove(app, nfp_flow->mask_data,
+			      nfp_flow->meta.mask_len, &nfp_flow->meta.flags,
+			      &new_mask_id);
+
+	nfp_flow->meta.flow_version = cpu_to_be64(priv->flower_version);
+	priv->flower_version++;
+
+	/* Update flow payload with mask ids. */
+	nfp_flow->unmasked_data[NFP_FL_MASK_ID_LOCATION] = new_mask_id;
+
+	/* Release the stats ctx id. */
+	temp_ctx_id = be32_to_cpu(nfp_flow->meta.host_ctx_id);
+
+	return nfp_release_stats_entry(app, temp_ctx_id);
+}
+
+int nfp_flower_metadata_init(struct nfp_app *app)
+{
+	struct nfp_flower_priv *priv = app->priv;
+
+	hash_init(priv->mask_table);
+	hash_init(priv->flow_table);
+	get_random_bytes(&priv->mask_id_seed, sizeof(priv->mask_id_seed));
+
+	/* Init ring buffer and unallocated mask_ids. */
+	priv->mask_ids.mask_id_free_list.buf =
+		kmalloc_array(NFP_FLOWER_MASK_ENTRY_RS,
+			      NFP_FLOWER_MASK_ELEMENT_RS, GFP_KERNEL);
+	if (!priv->mask_ids.mask_id_free_list.buf)
+		return -ENOMEM;
+
+	priv->mask_ids.init_unallocated = NFP_FLOWER_MASK_ENTRY_RS - 1;
+
+	/* Init timestamps for mask id*/
+	priv->mask_ids.last_used =
+		kmalloc_array(NFP_FLOWER_MASK_ENTRY_RS,
+			      sizeof(*priv->mask_ids.last_used), GFP_KERNEL);
+	if (!priv->mask_ids.last_used)
+		goto err_free_mask_id;
+
+	/* Init ring buffer and unallocated stats_ids. */
+	priv->stats_ids.free_list.buf =
+		vmalloc(NFP_FL_STATS_ENTRY_RS * NFP_FL_STATS_ELEM_RS);
+	if (!priv->stats_ids.free_list.buf)
+		goto err_free_last_used;
+
+	priv->stats_ids.init_unalloc = NFP_FL_REPEATED_HASH_MAX;
+
+	return 0;
+
+err_free_last_used:
+	kfree(priv->stats_ids.free_list.buf);
+err_free_mask_id:
+	kfree(priv->mask_ids.mask_id_free_list.buf);
+	return -ENOMEM;
+}
+
+void nfp_flower_metadata_cleanup(struct nfp_app *app)
+{
+	struct nfp_flower_priv *priv = app->priv;
+
+	if (!priv)
+		return;
+
+	kfree(priv->mask_ids.mask_id_free_list.buf);
+	kfree(priv->mask_ids.last_used);
+	vfree(priv->stats_ids.free_list.buf);
+}
diff --git a/drivers/net/ethernet/netronome/nfp/flower/offload.c b/drivers/net/ethernet/netronome/nfp/flower/offload.c
new file mode 100644
index 000000000000..4ad10bd5e139
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/flower/offload.c
@@ -0,0 +1,400 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/skbuff.h>
+#include <net/devlink.h>
+#include <net/pkt_cls.h>
+
+#include "cmsg.h"
+#include "main.h"
+#include "../nfpcore/nfp_cpp.h"
+#include "../nfpcore/nfp_nsp.h"
+#include "../nfp_app.h"
+#include "../nfp_main.h"
+#include "../nfp_net.h"
+#include "../nfp_port.h"
+
+static int
+nfp_flower_xmit_flow(struct net_device *netdev,
+		     struct nfp_fl_payload *nfp_flow, u8 mtype)
+{
+	u32 meta_len, key_len, mask_len, act_len, tot_len;
+	struct nfp_repr *priv = netdev_priv(netdev);
+	struct sk_buff *skb;
+	unsigned char *msg;
+
+	meta_len =  sizeof(struct nfp_fl_rule_metadata);
+	key_len = nfp_flow->meta.key_len;
+	mask_len = nfp_flow->meta.mask_len;
+	act_len = nfp_flow->meta.act_len;
+
+	tot_len = meta_len + key_len + mask_len + act_len;
+
+	/* Convert to long words as firmware expects
+	 * lengths in units of NFP_FL_LW_SIZ.
+	 */
+	nfp_flow->meta.key_len >>= NFP_FL_LW_SIZ;
+	nfp_flow->meta.mask_len >>= NFP_FL_LW_SIZ;
+	nfp_flow->meta.act_len >>= NFP_FL_LW_SIZ;
+
+	skb = nfp_flower_cmsg_alloc(priv->app, tot_len, mtype);
+	if (!skb)
+		return -ENOMEM;
+
+	msg = nfp_flower_cmsg_get_data(skb);
+	memcpy(msg, &nfp_flow->meta, meta_len);
+	memcpy(&msg[meta_len], nfp_flow->unmasked_data, key_len);
+	memcpy(&msg[meta_len + key_len], nfp_flow->mask_data, mask_len);
+	memcpy(&msg[meta_len + key_len + mask_len],
+	       nfp_flow->action_data, act_len);
+
+	/* Convert back to bytes as software expects
+	 * lengths in units of bytes.
+	 */
+	nfp_flow->meta.key_len <<= NFP_FL_LW_SIZ;
+	nfp_flow->meta.mask_len <<= NFP_FL_LW_SIZ;
+	nfp_flow->meta.act_len <<= NFP_FL_LW_SIZ;
+
+	nfp_ctrl_tx(priv->app->ctrl, skb);
+
+	return 0;
+}
+
+static bool nfp_flower_check_higher_than_mac(struct tc_cls_flower_offload *f)
+{
+	return dissector_uses_key(f->dissector,
+				  FLOW_DISSECTOR_KEY_IPV4_ADDRS) ||
+		dissector_uses_key(f->dissector,
+				   FLOW_DISSECTOR_KEY_IPV6_ADDRS) ||
+		dissector_uses_key(f->dissector,
+				   FLOW_DISSECTOR_KEY_PORTS) ||
+		dissector_uses_key(f->dissector, FLOW_DISSECTOR_KEY_ICMP);
+}
+
+static int
+nfp_flower_calculate_key_layers(struct nfp_fl_key_ls *ret_key_ls,
+				struct tc_cls_flower_offload *flow)
+{
+	struct flow_dissector_key_control *mask_enc_ctl;
+	struct flow_dissector_key_basic *mask_basic;
+	struct flow_dissector_key_basic *key_basic;
+	u32 key_layer_two;
+	u8 key_layer;
+	int key_size;
+
+	mask_enc_ctl = skb_flow_dissector_target(flow->dissector,
+						 FLOW_DISSECTOR_KEY_ENC_CONTROL,
+						 flow->mask);
+
+	mask_basic = skb_flow_dissector_target(flow->dissector,
+					       FLOW_DISSECTOR_KEY_BASIC,
+					       flow->mask);
+
+	key_basic = skb_flow_dissector_target(flow->dissector,
+					      FLOW_DISSECTOR_KEY_BASIC,
+					      flow->key);
+	key_layer_two = 0;
+	key_layer = NFP_FLOWER_LAYER_PORT | NFP_FLOWER_LAYER_MAC;
+	key_size = sizeof(struct nfp_flower_meta_one) +
+		   sizeof(struct nfp_flower_in_port) +
+		   sizeof(struct nfp_flower_mac_mpls);
+
+	/* We are expecting a tunnel. For now we ignore offloading. */
+	if (mask_enc_ctl->addr_type)
+		return -EOPNOTSUPP;
+
+	if (mask_basic->n_proto) {
+		/* Ethernet type is present in the key. */
+		switch (key_basic->n_proto) {
+		case cpu_to_be16(ETH_P_IP):
+			key_layer |= NFP_FLOWER_LAYER_IPV4;
+			key_size += sizeof(struct nfp_flower_ipv4);
+			break;
+
+		case cpu_to_be16(ETH_P_IPV6):
+			key_layer |= NFP_FLOWER_LAYER_IPV6;
+			key_size += sizeof(struct nfp_flower_ipv6);
+			break;
+
+		/* Currently we do not offload ARP
+		 * because we rely on it to get to the host.
+		 */
+		case cpu_to_be16(ETH_P_ARP):
+			return -EOPNOTSUPP;
+
+		/* Will be included in layer 2. */
+		case cpu_to_be16(ETH_P_8021Q):
+			break;
+
+		default:
+			/* Other ethtype - we need check the masks for the
+			 * remainder of the key to ensure we can offload.
+			 */
+			if (nfp_flower_check_higher_than_mac(flow))
+				return -EOPNOTSUPP;
+			break;
+		}
+	}
+
+	if (mask_basic->ip_proto) {
+		/* Ethernet type is present in the key. */
+		switch (key_basic->ip_proto) {
+		case IPPROTO_TCP:
+		case IPPROTO_UDP:
+		case IPPROTO_SCTP:
+		case IPPROTO_ICMP:
+		case IPPROTO_ICMPV6:
+			key_layer |= NFP_FLOWER_LAYER_TP;
+			key_size += sizeof(struct nfp_flower_tp_ports);
+			break;
+		default:
+			/* Other ip proto - we need check the masks for the
+			 * remainder of the key to ensure we can offload.
+			 */
+			return -EOPNOTSUPP;
+		}
+	}
+
+	ret_key_ls->key_layer = key_layer;
+	ret_key_ls->key_layer_two = key_layer_two;
+	ret_key_ls->key_size = key_size;
+
+	return 0;
+}
+
+static struct nfp_fl_payload *
+nfp_flower_allocate_new(struct nfp_fl_key_ls *key_layer)
+{
+	struct nfp_fl_payload *flow_pay;
+
+	flow_pay = kmalloc(sizeof(*flow_pay), GFP_KERNEL);
+	if (!flow_pay)
+		return NULL;
+
+	flow_pay->meta.key_len = key_layer->key_size;
+	flow_pay->unmasked_data = kmalloc(key_layer->key_size, GFP_KERNEL);
+	if (!flow_pay->unmasked_data)
+		goto err_free_flow;
+
+	flow_pay->meta.mask_len = key_layer->key_size;
+	flow_pay->mask_data = kmalloc(key_layer->key_size, GFP_KERNEL);
+	if (!flow_pay->mask_data)
+		goto err_free_unmasked;
+
+	flow_pay->action_data = kmalloc(NFP_FL_MAX_A_SIZ, GFP_KERNEL);
+	if (!flow_pay->action_data)
+		goto err_free_mask;
+
+	flow_pay->meta.flags = 0;
+	spin_lock_init(&flow_pay->lock);
+
+	return flow_pay;
+
+err_free_mask:
+	kfree(flow_pay->mask_data);
+err_free_unmasked:
+	kfree(flow_pay->unmasked_data);
+err_free_flow:
+	kfree(flow_pay);
+	return NULL;
+}
+
+/**
+ * nfp_flower_add_offload() - Adds a new flow to hardware.
+ * @app:	Pointer to the APP handle
+ * @netdev:	netdev structure.
+ * @flow:	TC flower classifier offload structure.
+ *
+ * Adds a new flow to the repeated hash structure and action payload.
+ *
+ * Return: negative value on error, 0 if configured successfully.
+ */
+static int
+nfp_flower_add_offload(struct nfp_app *app, struct net_device *netdev,
+		       struct tc_cls_flower_offload *flow)
+{
+	struct nfp_flower_priv *priv = app->priv;
+	struct nfp_fl_payload *flow_pay;
+	struct nfp_fl_key_ls *key_layer;
+	int err;
+
+	key_layer = kmalloc(sizeof(*key_layer), GFP_KERNEL);
+	if (!key_layer)
+		return -ENOMEM;
+
+	err = nfp_flower_calculate_key_layers(key_layer, flow);
+	if (err)
+		goto err_free_key_ls;
+
+	flow_pay = nfp_flower_allocate_new(key_layer);
+	if (!flow_pay) {
+		err = -ENOMEM;
+		goto err_free_key_ls;
+	}
+
+	err = nfp_flower_compile_flow_match(flow, key_layer, netdev, flow_pay);
+	if (err)
+		goto err_destroy_flow;
+
+	err = nfp_flower_compile_action(flow, netdev, flow_pay);
+	if (err)
+		goto err_destroy_flow;
+
+	err = nfp_compile_flow_metadata(app, flow, flow_pay);
+	if (err)
+		goto err_destroy_flow;
+
+	err = nfp_flower_xmit_flow(netdev, flow_pay,
+				   NFP_FLOWER_CMSG_TYPE_FLOW_ADD);
+	if (err)
+		goto err_destroy_flow;
+
+	INIT_HLIST_NODE(&flow_pay->link);
+	flow_pay->tc_flower_cookie = flow->cookie;
+	hash_add_rcu(priv->flow_table, &flow_pay->link, flow->cookie);
+
+	/* Deallocate flow payload when flower rule has been destroyed. */
+	kfree(key_layer);
+
+	return 0;
+
+err_destroy_flow:
+	kfree(flow_pay->action_data);
+	kfree(flow_pay->mask_data);
+	kfree(flow_pay->unmasked_data);
+	kfree(flow_pay);
+err_free_key_ls:
+	kfree(key_layer);
+	return err;
+}
+
+/**
+ * nfp_flower_del_offload() - Removes a flow from hardware.
+ * @app:	Pointer to the APP handle
+ * @netdev:	netdev structure.
+ * @flow:	TC flower classifier offload structure
+ *
+ * Removes a flow from the repeated hash structure and clears the
+ * action payload.
+ *
+ * Return: negative value on error, 0 if removed successfully.
+ */
+static int
+nfp_flower_del_offload(struct nfp_app *app, struct net_device *netdev,
+		       struct tc_cls_flower_offload *flow)
+{
+	struct nfp_fl_payload *nfp_flow;
+	int err;
+
+	nfp_flow = nfp_flower_search_fl_table(app, flow->cookie);
+	if (!nfp_flow)
+		return -ENOENT;
+
+	err = nfp_modify_flow_metadata(app, nfp_flow);
+	if (err)
+		goto err_free_flow;
+
+	err = nfp_flower_xmit_flow(netdev, nfp_flow,
+				   NFP_FLOWER_CMSG_TYPE_FLOW_DEL);
+	if (err)
+		goto err_free_flow;
+
+err_free_flow:
+	hash_del_rcu(&nfp_flow->link);
+	kfree(nfp_flow->action_data);
+	kfree(nfp_flow->mask_data);
+	kfree(nfp_flow->unmasked_data);
+	kfree_rcu(nfp_flow, rcu);
+	return err;
+}
+
+/**
+ * nfp_flower_get_stats() - Populates flow stats obtained from hardware.
+ * @app:	Pointer to the APP handle
+ * @flow:	TC flower classifier offload structure
+ *
+ * Populates a flow statistics structure which which corresponds to a
+ * specific flow.
+ *
+ * Return: negative value on error, 0 if stats populated successfully.
+ */
+static int
+nfp_flower_get_stats(struct nfp_app *app, struct tc_cls_flower_offload *flow)
+{
+	struct nfp_fl_payload *nfp_flow;
+
+	nfp_flow = nfp_flower_search_fl_table(app, flow->cookie);
+	if (!nfp_flow)
+		return -EINVAL;
+
+	spin_lock_bh(&nfp_flow->lock);
+	tcf_exts_stats_update(flow->exts, nfp_flow->stats.bytes,
+			      nfp_flow->stats.pkts, nfp_flow->stats.used);
+
+	nfp_flow->stats.pkts = 0;
+	nfp_flow->stats.bytes = 0;
+	spin_unlock_bh(&nfp_flow->lock);
+
+	return 0;
+}
+
+static int
+nfp_flower_repr_offload(struct nfp_app *app, struct net_device *netdev,
+			struct tc_cls_flower_offload *flower)
+{
+	switch (flower->command) {
+	case TC_CLSFLOWER_REPLACE:
+		return nfp_flower_add_offload(app, netdev, flower);
+	case TC_CLSFLOWER_DESTROY:
+		return nfp_flower_del_offload(app, netdev, flower);
+	case TC_CLSFLOWER_STATS:
+		return nfp_flower_get_stats(app, flower);
+	}
+
+	return -EOPNOTSUPP;
+}
+
+int nfp_flower_setup_tc(struct nfp_app *app, struct net_device *netdev,
+			u32 handle, __be16 proto, struct tc_to_netdev *tc)
+{
+	if (TC_H_MAJ(handle) != TC_H_MAJ(TC_H_INGRESS))
+		return -EOPNOTSUPP;
+
+	if (!eth_proto_is_802_3(proto))
+		return -EOPNOTSUPP;
+
+	if (tc->type != TC_SETUP_CLSFLOWER)
+		return -EINVAL;
+
+	return nfp_flower_repr_offload(app, netdev, tc->cls_flower);
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_app.c b/drivers/net/ethernet/netronome/nfp/nfp_app.c
new file mode 100644
index 000000000000..c704c022574f
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_app.c
@@ -0,0 +1,126 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+
+#include "nfpcore/nfp_cpp.h"
+#include "nfpcore/nfp_nffw.h"
+#include "nfp_app.h"
+#include "nfp_main.h"
+#include "nfp_net_repr.h"
+
+static const struct nfp_app_type *apps[] = {
+	&app_nic,
+	&app_bpf,
+#ifdef CONFIG_NFP_APP_FLOWER
+	&app_flower,
+#endif
+};
+
+const char *nfp_app_mip_name(struct nfp_app *app)
+{
+	if (!app || !app->pf->mip)
+		return "";
+	return nfp_mip_name(app->pf->mip);
+}
+
+struct sk_buff *
+nfp_app_ctrl_msg_alloc(struct nfp_app *app, unsigned int size, gfp_t priority)
+{
+	struct sk_buff *skb;
+
+	if (nfp_app_ctrl_has_meta(app))
+		size += 8;
+
+	skb = alloc_skb(size, priority);
+	if (!skb)
+		return NULL;
+
+	if (nfp_app_ctrl_has_meta(app))
+		skb_reserve(skb, 8);
+
+	return skb;
+}
+
+struct nfp_reprs *
+nfp_app_reprs_set(struct nfp_app *app, enum nfp_repr_type type,
+		  struct nfp_reprs *reprs)
+{
+	struct nfp_reprs *old;
+
+	old = rcu_dereference_protected(app->reprs[type],
+					lockdep_is_held(&app->pf->lock));
+	if (reprs && old) {
+		old = ERR_PTR(-EBUSY);
+		goto exit_unlock;
+	}
+
+	rcu_assign_pointer(app->reprs[type], reprs);
+
+exit_unlock:
+	return old;
+}
+
+struct nfp_app *nfp_app_alloc(struct nfp_pf *pf, enum nfp_app_id id)
+{
+	struct nfp_app *app;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(apps); i++)
+		if (apps[i]->id == id)
+			break;
+	if (i == ARRAY_SIZE(apps)) {
+		nfp_err(pf->cpp, "failed to find app with ID 0x%02hhx\n", id);
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (WARN_ON(!apps[i]->name || !apps[i]->vnic_init))
+		return ERR_PTR(-EINVAL);
+
+	app = kzalloc(sizeof(*app), GFP_KERNEL);
+	if (!app)
+		return ERR_PTR(-ENOMEM);
+
+	app->pf = pf;
+	app->cpp = pf->cpp;
+	app->pdev = pf->pdev;
+	app->type = apps[i];
+
+	return app;
+}
+
+void nfp_app_free(struct nfp_app *app)
+{
+	kfree(app);
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_app.h b/drivers/net/ethernet/netronome/nfp/nfp_app.h
new file mode 100644
index 000000000000..5d714e10d9a9
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_app.h
@@ -0,0 +1,314 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _NFP_APP_H
+#define _NFP_APP_H 1
+
+#include <net/devlink.h>
+
+#include "nfp_net_repr.h"
+
+struct bpf_prog;
+struct net_device;
+struct pci_dev;
+struct sk_buff;
+struct tc_to_netdev;
+struct sk_buff;
+struct nfp_app;
+struct nfp_cpp;
+struct nfp_pf;
+struct nfp_repr;
+struct nfp_net;
+
+enum nfp_app_id {
+	NFP_APP_CORE_NIC	= 0x1,
+	NFP_APP_BPF_NIC		= 0x2,
+	NFP_APP_FLOWER_NIC	= 0x3,
+};
+
+extern const struct nfp_app_type app_nic;
+extern const struct nfp_app_type app_bpf;
+extern const struct nfp_app_type app_flower;
+
+/**
+ * struct nfp_app_type - application definition
+ * @id:		application ID
+ * @name:	application name
+ * @ctrl_has_meta:  control messages have prepend of type:5/port:CTRL
+ *
+ * Callbacks
+ * @init:	perform basic app checks and init
+ * @clean:	clean app state
+ * @extra_cap:	extra capabilities string
+ * @vnic_init:	init vNICs (assign port types, etc.)
+ * @vnic_clean:	clean up app's vNIC state
+ * @repr_open:	representor netdev open callback
+ * @repr_stop:	representor netdev stop callback
+ * @start:	start application logic
+ * @stop:	stop application logic
+ * @ctrl_msg_rx:    control message handler
+ * @setup_tc:	setup TC ndo
+ * @tc_busy:	TC HW offload busy (rules loaded)
+ * @xdp_offload:    offload an XDP program
+ * @eswitch_mode_get:    get SR-IOV eswitch mode
+ * @sriov_enable: app-specific sriov initialisation
+ * @sriov_disable: app-specific sriov clean-up
+ * @repr_get:	get representor netdev
+ */
+struct nfp_app_type {
+	enum nfp_app_id id;
+	const char *name;
+
+	bool ctrl_has_meta;
+
+	int (*init)(struct nfp_app *app);
+	void (*clean)(struct nfp_app *app);
+
+	const char *(*extra_cap)(struct nfp_app *app, struct nfp_net *nn);
+
+	int (*vnic_init)(struct nfp_app *app, struct nfp_net *nn,
+			 unsigned int id);
+	void (*vnic_clean)(struct nfp_app *app, struct nfp_net *nn);
+
+	int (*repr_open)(struct nfp_app *app, struct nfp_repr *repr);
+	int (*repr_stop)(struct nfp_app *app, struct nfp_repr *repr);
+
+	int (*start)(struct nfp_app *app);
+	void (*stop)(struct nfp_app *app);
+
+	void (*ctrl_msg_rx)(struct nfp_app *app, struct sk_buff *skb);
+
+	int (*setup_tc)(struct nfp_app *app, struct net_device *netdev,
+			u32 handle, __be16 proto, struct tc_to_netdev *tc);
+	bool (*tc_busy)(struct nfp_app *app, struct nfp_net *nn);
+	int (*xdp_offload)(struct nfp_app *app, struct nfp_net *nn,
+			   struct bpf_prog *prog);
+
+	int (*sriov_enable)(struct nfp_app *app, int num_vfs);
+	void (*sriov_disable)(struct nfp_app *app);
+
+	enum devlink_eswitch_mode (*eswitch_mode_get)(struct nfp_app *app);
+	struct net_device *(*repr_get)(struct nfp_app *app, u32 id);
+};
+
+/**
+ * struct nfp_app - NFP application container
+ * @pdev:	backpointer to PCI device
+ * @pf:		backpointer to NFP PF structure
+ * @cpp:	pointer to the CPP handle
+ * @ctrl:	pointer to ctrl vNIC struct
+ * @reprs:	array of pointers to representors
+ * @type:	pointer to const application ops and info
+ * @priv:	app-specific priv data
+ */
+struct nfp_app {
+	struct pci_dev *pdev;
+	struct nfp_pf *pf;
+	struct nfp_cpp *cpp;
+
+	struct nfp_net *ctrl;
+	struct nfp_reprs __rcu *reprs[NFP_REPR_TYPE_MAX + 1];
+
+	const struct nfp_app_type *type;
+	void *priv;
+};
+
+bool nfp_ctrl_tx(struct nfp_net *nn, struct sk_buff *skb);
+
+static inline int nfp_app_init(struct nfp_app *app)
+{
+	if (!app->type->init)
+		return 0;
+	return app->type->init(app);
+}
+
+static inline void nfp_app_clean(struct nfp_app *app)
+{
+	if (app->type->clean)
+		app->type->clean(app);
+}
+
+static inline int nfp_app_vnic_init(struct nfp_app *app, struct nfp_net *nn,
+				    unsigned int id)
+{
+	return app->type->vnic_init(app, nn, id);
+}
+
+static inline void nfp_app_vnic_clean(struct nfp_app *app, struct nfp_net *nn)
+{
+	if (app->type->vnic_clean)
+		app->type->vnic_clean(app, nn);
+}
+
+static inline int nfp_app_repr_open(struct nfp_app *app, struct nfp_repr *repr)
+{
+	if (!app->type->repr_open)
+		return -EINVAL;
+	return app->type->repr_open(app, repr);
+}
+
+static inline int nfp_app_repr_stop(struct nfp_app *app, struct nfp_repr *repr)
+{
+	if (!app->type->repr_stop)
+		return -EINVAL;
+	return app->type->repr_stop(app, repr);
+}
+
+static inline int nfp_app_start(struct nfp_app *app, struct nfp_net *ctrl)
+{
+	app->ctrl = ctrl;
+	if (!app->type->start)
+		return 0;
+	return app->type->start(app);
+}
+
+static inline void nfp_app_stop(struct nfp_app *app)
+{
+	if (!app->type->stop)
+		return;
+	app->type->stop(app);
+}
+
+static inline const char *nfp_app_name(struct nfp_app *app)
+{
+	if (!app)
+		return "";
+	return app->type->name;
+}
+
+static inline bool nfp_app_needs_ctrl_vnic(struct nfp_app *app)
+{
+	return app && app->type->ctrl_msg_rx;
+}
+
+static inline bool nfp_app_ctrl_has_meta(struct nfp_app *app)
+{
+	return app->type->ctrl_has_meta;
+}
+
+static inline const char *nfp_app_extra_cap(struct nfp_app *app,
+					    struct nfp_net *nn)
+{
+	if (!app || !app->type->extra_cap)
+		return "";
+	return app->type->extra_cap(app, nn);
+}
+
+static inline bool nfp_app_has_tc(struct nfp_app *app)
+{
+	return app && app->type->setup_tc;
+}
+
+static inline bool nfp_app_tc_busy(struct nfp_app *app, struct nfp_net *nn)
+{
+	if (!app || !app->type->tc_busy)
+		return false;
+	return app->type->tc_busy(app, nn);
+}
+
+static inline int nfp_app_setup_tc(struct nfp_app *app,
+				   struct net_device *netdev,
+				   u32 handle, __be16 proto,
+				   struct tc_to_netdev *tc)
+{
+	if (!app || !app->type->setup_tc)
+		return -EOPNOTSUPP;
+	return app->type->setup_tc(app, netdev, handle, proto, tc);
+}
+
+static inline int nfp_app_xdp_offload(struct nfp_app *app, struct nfp_net *nn,
+				      struct bpf_prog *prog)
+{
+	if (!app || !app->type->xdp_offload)
+		return -EOPNOTSUPP;
+	return app->type->xdp_offload(app, nn, prog);
+}
+
+static inline bool nfp_app_ctrl_tx(struct nfp_app *app, struct sk_buff *skb)
+{
+	return nfp_ctrl_tx(app->ctrl, skb);
+}
+
+static inline void nfp_app_ctrl_rx(struct nfp_app *app, struct sk_buff *skb)
+{
+	app->type->ctrl_msg_rx(app, skb);
+}
+
+static inline int nfp_app_eswitch_mode_get(struct nfp_app *app, u16 *mode)
+{
+	if (!app->type->eswitch_mode_get)
+		return -EOPNOTSUPP;
+
+	*mode = app->type->eswitch_mode_get(app);
+
+	return 0;
+}
+
+static inline int nfp_app_sriov_enable(struct nfp_app *app, int num_vfs)
+{
+	if (!app || !app->type->sriov_enable)
+		return -EOPNOTSUPP;
+	return app->type->sriov_enable(app, num_vfs);
+}
+
+static inline void nfp_app_sriov_disable(struct nfp_app *app)
+{
+	if (app && app->type->sriov_disable)
+		app->type->sriov_disable(app);
+}
+
+static inline struct net_device *nfp_app_repr_get(struct nfp_app *app, u32 id)
+{
+	if (unlikely(!app || !app->type->repr_get))
+		return NULL;
+
+	return app->type->repr_get(app, id);
+}
+
+struct nfp_reprs *
+nfp_app_reprs_set(struct nfp_app *app, enum nfp_repr_type type,
+		  struct nfp_reprs *reprs);
+
+const char *nfp_app_mip_name(struct nfp_app *app);
+struct sk_buff *
+nfp_app_ctrl_msg_alloc(struct nfp_app *app, unsigned int size, gfp_t priority);
+
+struct nfp_app *nfp_app_alloc(struct nfp_pf *pf, enum nfp_app_id id);
+void nfp_app_free(struct nfp_app *app);
+
+/* Callbacks shared between apps */
+
+int nfp_app_nic_vnic_init(struct nfp_app *app, struct nfp_net *nn,
+			  unsigned int id);
+
+#endif
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_app_nic.c b/drivers/net/ethernet/netronome/nfp/nfp_app_nic.c
new file mode 100644
index 000000000000..4e37c81f9eaf
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_app_nic.c
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "nfpcore/nfp_cpp.h"
+#include "nfpcore/nfp_nsp.h"
+#include "nfp_app.h"
+#include "nfp_main.h"
+#include "nfp_net.h"
+#include "nfp_port.h"
+
+static int
+nfp_app_nic_vnic_init_phy_port(struct nfp_pf *pf, struct nfp_app *app,
+			       struct nfp_net *nn, unsigned int id)
+{
+	int err;
+
+	if (!pf->eth_tbl)
+		return 0;
+
+	nn->port = nfp_port_alloc(app, NFP_PORT_PHYS_PORT, nn->dp.netdev);
+	if (IS_ERR(nn->port))
+		return PTR_ERR(nn->port);
+
+	err = nfp_port_init_phy_port(pf, app, nn->port, id);
+	if (err) {
+		nfp_port_free(nn->port);
+		return err;
+	}
+
+	return nn->port->type == NFP_PORT_INVALID;
+}
+
+int nfp_app_nic_vnic_init(struct nfp_app *app, struct nfp_net *nn,
+			  unsigned int id)
+{
+	int err;
+
+	err = nfp_app_nic_vnic_init_phy_port(app->pf, app, nn, id);
+	if (err)
+		return err < 0 ? err : 0;
+
+	nfp_net_get_mac_addr(app->pf, nn->port);
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_asm.h b/drivers/net/ethernet/netronome/nfp/nfp_asm.h
index 22484b6fd3e8..d2b535739d2b 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_asm.h
+++ b/drivers/net/ethernet/netronome/nfp/nfp_asm.h
@@ -34,7 +34,7 @@
 #ifndef __NFP_ASM_H__
 #define __NFP_ASM_H__ 1
 
-#include "nfp_bpf.h"
+#include <linux/types.h>
 
 #define REG_NONE	0
 
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_devlink.c b/drivers/net/ethernet/netronome/nfp/nfp_devlink.c
new file mode 100644
index 000000000000..6c9f29c2e975
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_devlink.c
@@ -0,0 +1,199 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/rtnetlink.h>
+#include <net/devlink.h>
+
+#include "nfpcore/nfp_nsp.h"
+#include "nfp_app.h"
+#include "nfp_main.h"
+#include "nfp_port.h"
+
+static int
+nfp_devlink_fill_eth_port(struct nfp_port *port,
+			  struct nfp_eth_table_port *copy)
+{
+	struct nfp_eth_table_port *eth_port;
+
+	eth_port = __nfp_port_get_eth_port(port);
+	if (!eth_port)
+		return -EINVAL;
+
+	memcpy(copy, eth_port, sizeof(*eth_port));
+
+	return 0;
+}
+
+static int
+nfp_devlink_fill_eth_port_from_id(struct nfp_pf *pf, unsigned int port_index,
+				  struct nfp_eth_table_port *copy)
+{
+	struct nfp_port *port;
+
+	port = nfp_port_from_id(pf, NFP_PORT_PHYS_PORT, port_index);
+
+	return nfp_devlink_fill_eth_port(port, copy);
+}
+
+static int
+nfp_devlink_set_lanes(struct nfp_pf *pf, unsigned int idx, unsigned int lanes)
+{
+	struct nfp_nsp *nsp;
+	int ret;
+
+	nsp = nfp_eth_config_start(pf->cpp, idx);
+	if (IS_ERR(nsp))
+		return PTR_ERR(nsp);
+
+	ret = __nfp_eth_set_split(nsp, lanes);
+	if (ret) {
+		nfp_eth_config_cleanup_end(nsp);
+		return ret;
+	}
+
+	ret = nfp_eth_config_commit_end(nsp);
+	if (ret < 0)
+		return ret;
+	if (ret) /* no change */
+		return 0;
+
+	return nfp_net_refresh_port_table_sync(pf);
+}
+
+static int
+nfp_devlink_port_split(struct devlink *devlink, unsigned int port_index,
+		       unsigned int count)
+{
+	struct nfp_pf *pf = devlink_priv(devlink);
+	struct nfp_eth_table_port eth_port;
+	int ret;
+
+	if (count < 2)
+		return -EINVAL;
+
+	mutex_lock(&pf->lock);
+
+	rtnl_lock();
+	ret = nfp_devlink_fill_eth_port_from_id(pf, port_index, &eth_port);
+	rtnl_unlock();
+	if (ret)
+		goto out;
+
+	if (eth_port.is_split || eth_port.port_lanes % count) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = nfp_devlink_set_lanes(pf, eth_port.index,
+				    eth_port.port_lanes / count);
+out:
+	mutex_unlock(&pf->lock);
+
+	return ret;
+}
+
+static int
+nfp_devlink_port_unsplit(struct devlink *devlink, unsigned int port_index)
+{
+	struct nfp_pf *pf = devlink_priv(devlink);
+	struct nfp_eth_table_port eth_port;
+	int ret;
+
+	mutex_lock(&pf->lock);
+
+	rtnl_lock();
+	ret = nfp_devlink_fill_eth_port_from_id(pf, port_index, &eth_port);
+	rtnl_unlock();
+	if (ret)
+		goto out;
+
+	if (!eth_port.is_split) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = nfp_devlink_set_lanes(pf, eth_port.index, eth_port.port_lanes);
+out:
+	mutex_unlock(&pf->lock);
+
+	return ret;
+}
+
+static int nfp_devlink_eswitch_mode_get(struct devlink *devlink, u16 *mode)
+{
+	struct nfp_pf *pf = devlink_priv(devlink);
+	int ret;
+
+	mutex_lock(&pf->lock);
+	if (!pf->app) {
+		ret = -EBUSY;
+		goto out;
+	}
+	ret = nfp_app_eswitch_mode_get(pf->app, mode);
+out:
+	mutex_unlock(&pf->lock);
+
+	return ret;
+}
+
+const struct devlink_ops nfp_devlink_ops = {
+	.port_split		= nfp_devlink_port_split,
+	.port_unsplit		= nfp_devlink_port_unsplit,
+	.eswitch_mode_get	= nfp_devlink_eswitch_mode_get,
+};
+
+int nfp_devlink_port_register(struct nfp_app *app, struct nfp_port *port)
+{
+	struct nfp_eth_table_port eth_port;
+	struct devlink *devlink;
+	int ret;
+
+	rtnl_lock();
+	ret = nfp_devlink_fill_eth_port(port, &eth_port);
+	rtnl_unlock();
+	if (ret)
+		return ret;
+
+	devlink_port_type_eth_set(&port->dl_port, port->netdev);
+	if (eth_port.is_split)
+		devlink_port_split_set(&port->dl_port, eth_port.label_port);
+
+	devlink = priv_to_devlink(app->pf);
+
+	return devlink_port_register(devlink, &port->dl_port, port->eth_id);
+}
+
+void nfp_devlink_port_unregister(struct nfp_port *port)
+{
+	devlink_port_unregister(&port->dl_port);
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_hwmon.c b/drivers/net/ethernet/netronome/nfp/nfp_hwmon.c
new file mode 100644
index 000000000000..f0dcf45aeec1
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_hwmon.c
@@ -0,0 +1,192 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/kernel.h>
+#include <linux/bitops.h>
+#include <linux/hwmon.h>
+
+#include "nfpcore/nfp_cpp.h"
+#include "nfpcore/nfp_nsp.h"
+#include "nfp_main.h"
+
+#define NFP_TEMP_MAX		(95 * 1000)
+#define NFP_TEMP_CRIT		(105 * 1000)
+
+#define NFP_POWER_MAX		(25 * 1000 * 1000)
+
+static int nfp_hwmon_sensor_id(enum hwmon_sensor_types type, int channel)
+{
+	if (type == hwmon_temp)
+		return NFP_SENSOR_CHIP_TEMPERATURE;
+	if (type == hwmon_power)
+		return NFP_SENSOR_ASSEMBLY_POWER + channel;
+	return -EINVAL;
+}
+
+static int
+nfp_hwmon_read(struct device *dev, enum hwmon_sensor_types type, u32 attr,
+	       int channel, long *val)
+{
+	static const struct {
+		enum hwmon_sensor_types type;
+		u32 attr;
+		long val;
+	} const_vals[] = {
+		{ hwmon_temp,	hwmon_temp_max,		NFP_TEMP_MAX },
+		{ hwmon_temp,	hwmon_temp_crit,	NFP_TEMP_CRIT },
+		{ hwmon_power,	hwmon_power_max,	NFP_POWER_MAX },
+	};
+	struct nfp_pf *pf = dev_get_drvdata(dev);
+	enum nfp_nsp_sensor_id id;
+	int err, i;
+
+	for (i = 0; i < ARRAY_SIZE(const_vals); i++)
+		if (const_vals[i].type == type && const_vals[i].attr == attr) {
+			*val = const_vals[i].val;
+			return 0;
+		}
+
+	err = nfp_hwmon_sensor_id(type, channel);
+	if (err < 0)
+		return err;
+	id = err;
+
+	if (!(pf->nspi->sensor_mask & BIT(id)))
+		return -EOPNOTSUPP;
+
+	if (type == hwmon_temp && attr == hwmon_temp_input)
+		return nfp_hwmon_read_sensor(pf->cpp, id, val);
+	if (type == hwmon_power && attr == hwmon_power_input)
+		return nfp_hwmon_read_sensor(pf->cpp, id, val);
+
+	return -EINVAL;
+}
+
+static umode_t
+nfp_hwmon_is_visible(const void *data, enum hwmon_sensor_types type, u32 attr,
+		     int channel)
+{
+	if (type == hwmon_temp) {
+		switch (attr) {
+		case hwmon_temp_input:
+		case hwmon_temp_crit:
+		case hwmon_temp_max:
+			return 0444;
+		}
+	} else if (type == hwmon_power) {
+		switch (attr) {
+		case hwmon_power_input:
+		case hwmon_power_max:
+			return 0444;
+		}
+	}
+	return 0;
+}
+
+static u32 nfp_chip_config[] = {
+	HWMON_C_REGISTER_TZ,
+	0
+};
+
+static const struct hwmon_channel_info nfp_chip = {
+	.type = hwmon_chip,
+	.config = nfp_chip_config,
+};
+
+static u32 nfp_temp_config[] = {
+	HWMON_T_INPUT | HWMON_T_MAX | HWMON_T_CRIT,
+	0
+};
+
+static const struct hwmon_channel_info nfp_temp = {
+	.type = hwmon_temp,
+	.config = nfp_temp_config,
+};
+
+static u32 nfp_power_config[] = {
+	HWMON_P_INPUT | HWMON_P_MAX,
+	HWMON_P_INPUT,
+	HWMON_P_INPUT,
+	0
+};
+
+static const struct hwmon_channel_info nfp_power = {
+	.type = hwmon_power,
+	.config = nfp_power_config,
+};
+
+static const struct hwmon_channel_info *nfp_hwmon_info[] = {
+	&nfp_chip,
+	&nfp_temp,
+	&nfp_power,
+	NULL
+};
+
+static const struct hwmon_ops nfp_hwmon_ops = {
+	.is_visible = nfp_hwmon_is_visible,
+	.read = nfp_hwmon_read,
+};
+
+static const struct hwmon_chip_info nfp_chip_info = {
+	.ops = &nfp_hwmon_ops,
+	.info = nfp_hwmon_info,
+};
+
+int nfp_hwmon_register(struct nfp_pf *pf)
+{
+	if (!IS_REACHABLE(CONFIG_HWMON))
+		return 0;
+
+	if (!pf->nspi) {
+		nfp_warn(pf->cpp, "not registering HWMON (no NSP info)\n");
+		return 0;
+	}
+	if (!pf->nspi->sensor_mask) {
+		nfp_info(pf->cpp,
+			 "not registering HWMON (NSP doesn't report sensors)\n");
+		return 0;
+	}
+
+	pf->hwmon_dev = hwmon_device_register_with_info(&pf->pdev->dev, "nfp",
+							pf, &nfp_chip_info,
+							NULL);
+	return PTR_ERR_OR_ZERO(pf->hwmon_dev);
+}
+
+void nfp_hwmon_unregister(struct nfp_pf *pf)
+{
+	if (!IS_REACHABLE(CONFIG_HWMON) || !pf->hwmon_dev)
+		return;
+
+	hwmon_device_unregister(pf->hwmon_dev);
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_main.c b/drivers/net/ethernet/netronome/nfp/nfp_main.c
index dde35dae35c5..d67969d3e484 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_main.c
+++ b/drivers/net/ethernet/netronome/nfp/nfp_main.c
@@ -41,9 +41,11 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/mutex.h>
 #include <linux/pci.h>
 #include <linux/firmware.h>
 #include <linux/vermagic.h>
+#include <net/devlink.h>
 
 #include "nfpcore/nfp.h"
 #include "nfpcore/nfp_cpp.h"
@@ -52,6 +54,7 @@
 
 #include "nfpcore/nfp6000_pcie.h"
 
+#include "nfp_app.h"
 #include "nfp_main.h"
 #include "nfp_net.h"
 
@@ -71,20 +74,22 @@ static const struct pci_device_id nfp_pci_device_ids[] = {
 };
 MODULE_DEVICE_TABLE(pci, nfp_pci_device_ids);
 
-static void nfp_pcie_sriov_read_nfd_limit(struct nfp_pf *pf)
+static int nfp_pcie_sriov_read_nfd_limit(struct nfp_pf *pf)
 {
-#ifdef CONFIG_PCI_IOV
 	int err;
 
-	pf->limit_vfs = nfp_rtsym_read_le(pf->cpp, "nfd_vf_cfg_max_vfs", &err);
+	pf->limit_vfs = nfp_rtsym_read_le(pf->rtbl, "nfd_vf_cfg_max_vfs", &err);
 	if (!err)
-		return;
+		return pci_sriov_set_totalvfs(pf->pdev, pf->limit_vfs);
 
 	pf->limit_vfs = ~0;
+	pci_sriov_set_totalvfs(pf->pdev, 0); /* 0 is unset */
 	/* Allow any setting for backwards compatibility if symbol not found */
-	if (err != -ENOENT)
-		nfp_warn(pf->cpp, "Warning: VF limit read failed: %d\n", err);
-#endif
+	if (err == -ENOENT)
+		return 0;
+
+	nfp_warn(pf->cpp, "Warning: VF limit read failed: %d\n", err);
+	return err;
 }
 
 static int nfp_pcie_sriov_enable(struct pci_dev *pdev, int num_vfs)
@@ -93,23 +98,41 @@ static int nfp_pcie_sriov_enable(struct pci_dev *pdev, int num_vfs)
 	struct nfp_pf *pf = pci_get_drvdata(pdev);
 	int err;
 
+	mutex_lock(&pf->lock);
+
 	if (num_vfs > pf->limit_vfs) {
 		nfp_info(pf->cpp, "Firmware limits number of VFs to %u\n",
 			 pf->limit_vfs);
-		return -EINVAL;
+		err = -EINVAL;
+		goto err_unlock;
 	}
 
 	err = pci_enable_sriov(pdev, num_vfs);
 	if (err) {
-		dev_warn(&pdev->dev, "Failed to enable PCI sriov: %d\n", err);
-		return err;
+		dev_warn(&pdev->dev, "Failed to enable PCI SR-IOV: %d\n", err);
+		goto err_unlock;
+	}
+
+	err = nfp_app_sriov_enable(pf->app, num_vfs);
+	if (err) {
+		dev_warn(&pdev->dev,
+			 "App specific PCI SR-IOV configuration failed: %d\n",
+			 err);
+		goto err_sriov_disable;
 	}
 
 	pf->num_vfs = num_vfs;
 
 	dev_dbg(&pdev->dev, "Created %d VFs.\n", pf->num_vfs);
 
+	mutex_unlock(&pf->lock);
 	return num_vfs;
+
+err_sriov_disable:
+	pci_disable_sriov(pdev);
+err_unlock:
+	mutex_unlock(&pf->lock);
+	return err;
 #endif
 	return 0;
 }
@@ -119,19 +142,26 @@ static int nfp_pcie_sriov_disable(struct pci_dev *pdev)
 #ifdef CONFIG_PCI_IOV
 	struct nfp_pf *pf = pci_get_drvdata(pdev);
 
+	mutex_lock(&pf->lock);
+
 	/* If the VFs are assigned we cannot shut down SR-IOV without
 	 * causing issues, so just leave the hardware available but
 	 * disabled
 	 */
 	if (pci_vfs_assigned(pdev)) {
 		dev_warn(&pdev->dev, "Disabling while VFs assigned - VFs will not be deallocated\n");
+		mutex_unlock(&pf->lock);
 		return -EPERM;
 	}
 
+	nfp_app_sriov_disable(pf->app);
+
 	pf->num_vfs = 0;
 
 	pci_disable_sriov(pdev);
 	dev_dbg(&pdev->dev, "Removed VFs.\n");
+
+	mutex_unlock(&pf->lock);
 #endif
 	return 0;
 }
@@ -166,7 +196,7 @@ nfp_net_fw_find(struct pci_dev *pdev, struct nfp_pf *pf)
 		return NULL;
 	}
 
-	fw_model = nfp_hwinfo_lookup(pf->cpp, "assembly.partno");
+	fw_model = nfp_hwinfo_lookup(pf->hwinfo, "assembly.partno");
 	if (!fw_model) {
 		dev_err(&pdev->dev, "Error: can't read part number\n");
 		return NULL;
@@ -253,7 +283,6 @@ exit_release_fw:
 
 static int nfp_nsp_init(struct pci_dev *pdev, struct nfp_pf *pf)
 {
-	struct nfp_nsp_identify *nspi;
 	struct nfp_nsp *nsp;
 	int err;
 
@@ -270,14 +299,13 @@ static int nfp_nsp_init(struct pci_dev *pdev, struct nfp_pf *pf)
 
 	pf->eth_tbl = __nfp_eth_read_ports(pf->cpp, nsp);
 
-	nspi = __nfp_nsp_identify(nsp);
-	if (nspi) {
-		dev_info(&pdev->dev, "BSP: %s\n", nspi->version);
-		kfree(nspi);
-	}
+	pf->nspi = __nfp_nsp_identify(nsp);
+	if (pf->nspi)
+		dev_info(&pdev->dev, "BSP: %s\n", pf->nspi->version);
 
 	err = nfp_fw_load(pdev, pf, nsp);
 	if (err < 0) {
+		kfree(pf->nspi);
 		kfree(pf->eth_tbl);
 		dev_err(&pdev->dev, "Failed to load FW\n");
 		goto exit_close_nsp;
@@ -315,6 +343,7 @@ static void nfp_fw_unload(struct nfp_pf *pf)
 static int nfp_pci_probe(struct pci_dev *pdev,
 			 const struct pci_device_id *pci_id)
 {
+	struct devlink *devlink;
 	struct nfp_pf *pf;
 	int err;
 
@@ -335,15 +364,24 @@ static int nfp_pci_probe(struct pci_dev *pdev,
 		goto err_pci_disable;
 	}
 
-	pf = kzalloc(sizeof(*pf), GFP_KERNEL);
-	if (!pf) {
+	devlink = devlink_alloc(&nfp_devlink_ops, sizeof(*pf));
+	if (!devlink) {
 		err = -ENOMEM;
 		goto err_rel_regions;
 	}
+	pf = devlink_priv(devlink);
+	INIT_LIST_HEAD(&pf->vnics);
 	INIT_LIST_HEAD(&pf->ports);
+	mutex_init(&pf->lock);
 	pci_set_drvdata(pdev, pf);
 	pf->pdev = pdev;
 
+	pf->wq = alloc_workqueue("nfp-%s", 0, 2, pci_name(pdev));
+	if (!pf->wq) {
+		err = -ENOMEM;
+		goto err_pci_priv_unset;
+	}
+
 	pf->cpp = nfp_cpp_from_nfp6000_pcie(pdev);
 	if (IS_ERR_OR_NULL(pf->cpp)) {
 		err = PTR_ERR(pf->cpp);
@@ -352,34 +390,72 @@ static int nfp_pci_probe(struct pci_dev *pdev,
 		goto err_disable_msix;
 	}
 
+	pf->hwinfo = nfp_hwinfo_read(pf->cpp);
+
 	dev_info(&pdev->dev, "Assembly: %s%s%s-%s CPLD: %s\n",
-		 nfp_hwinfo_lookup(pf->cpp, "assembly.vendor"),
-		 nfp_hwinfo_lookup(pf->cpp, "assembly.partno"),
-		 nfp_hwinfo_lookup(pf->cpp, "assembly.serial"),
-		 nfp_hwinfo_lookup(pf->cpp, "assembly.revision"),
-		 nfp_hwinfo_lookup(pf->cpp, "cpld.version"));
+		 nfp_hwinfo_lookup(pf->hwinfo, "assembly.vendor"),
+		 nfp_hwinfo_lookup(pf->hwinfo, "assembly.partno"),
+		 nfp_hwinfo_lookup(pf->hwinfo, "assembly.serial"),
+		 nfp_hwinfo_lookup(pf->hwinfo, "assembly.revision"),
+		 nfp_hwinfo_lookup(pf->hwinfo, "cpld.version"));
+
+	err = devlink_register(devlink, &pdev->dev);
+	if (err)
+		goto err_hwinfo_free;
 
 	err = nfp_nsp_init(pdev, pf);
 	if (err)
-		goto err_cpp_free;
+		goto err_devlink_unreg;
 
-	nfp_pcie_sriov_read_nfd_limit(pf);
+	pf->mip = nfp_mip_open(pf->cpp);
+	pf->rtbl = __nfp_rtsym_table_read(pf->cpp, pf->mip);
 
-	err = nfp_net_pci_probe(pf);
+	err = nfp_pcie_sriov_read_nfd_limit(pf);
 	if (err)
 		goto err_fw_unload;
 
+	pf->num_vfs = pci_num_vf(pdev);
+	if (pf->num_vfs > pf->limit_vfs) {
+		dev_err(&pdev->dev,
+			"Error: %d VFs already enabled, but loaded FW can only support %d\n",
+			pf->num_vfs, pf->limit_vfs);
+		goto err_fw_unload;
+	}
+
+	err = nfp_net_pci_probe(pf);
+	if (err)
+		goto err_sriov_unlimit;
+
+	err = nfp_hwmon_register(pf);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to register hwmon info\n");
+		goto err_net_remove;
+	}
+
 	return 0;
 
+err_net_remove:
+	nfp_net_pci_remove(pf);
+err_sriov_unlimit:
+	pci_sriov_set_totalvfs(pf->pdev, 0);
 err_fw_unload:
+	kfree(pf->rtbl);
+	nfp_mip_close(pf->mip);
 	if (pf->fw_loaded)
 		nfp_fw_unload(pf);
 	kfree(pf->eth_tbl);
-err_cpp_free:
+	kfree(pf->nspi);
+err_devlink_unreg:
+	devlink_unregister(devlink);
+err_hwinfo_free:
+	kfree(pf->hwinfo);
 	nfp_cpp_free(pf->cpp);
 err_disable_msix:
+	destroy_workqueue(pf->wq);
+err_pci_priv_unset:
 	pci_set_drvdata(pdev, NULL);
-	kfree(pf);
+	mutex_destroy(&pf->lock);
+	devlink_free(devlink);
 err_rel_regions:
 	pci_release_regions(pdev);
 err_pci_disable:
@@ -391,19 +467,33 @@ err_pci_disable:
 static void nfp_pci_remove(struct pci_dev *pdev)
 {
 	struct nfp_pf *pf = pci_get_drvdata(pdev);
+	struct devlink *devlink;
+
+	nfp_hwmon_unregister(pf);
+
+	devlink = priv_to_devlink(pf);
 
 	nfp_net_pci_remove(pf);
 
 	nfp_pcie_sriov_disable(pdev);
+	pci_sriov_set_totalvfs(pf->pdev, 0);
+
+	devlink_unregister(devlink);
 
+	kfree(pf->rtbl);
+	nfp_mip_close(pf->mip);
 	if (pf->fw_loaded)
 		nfp_fw_unload(pf);
 
+	destroy_workqueue(pf->wq);
 	pci_set_drvdata(pdev, NULL);
+	kfree(pf->hwinfo);
 	nfp_cpp_free(pf->cpp);
 
 	kfree(pf->eth_tbl);
-	kfree(pf);
+	kfree(pf->nspi);
+	mutex_destroy(&pf->lock);
+	devlink_free(devlink);
 	pci_release_regions(pdev);
 	pci_disable_device(pdev);
 }
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_main.h b/drivers/net/ethernet/netronome/nfp/nfp_main.h
index b57de047b002..6922410806db 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_main.h
+++ b/drivers/net/ethernet/netronome/nfp/nfp_main.h
@@ -47,39 +47,66 @@
 #include <linux/workqueue.h>
 
 struct dentry;
+struct device;
+struct devlink_ops;
 struct pci_dev;
 
 struct nfp_cpp;
 struct nfp_cpp_area;
 struct nfp_eth_table;
+struct nfp_hwinfo;
+struct nfp_mip;
+struct nfp_net;
+struct nfp_nsp_identify;
+struct nfp_port;
+struct nfp_rtsym_table;
 
 /**
  * struct nfp_pf - NFP PF-specific device structure
  * @pdev:		Backpointer to PCI device
  * @cpp:		Pointer to the CPP handle
- * @ctrl_area:		Pointer to the CPP area for the control BAR
- * @tx_area:		Pointer to the CPP area for the TX queues
- * @rx_area:		Pointer to the CPP area for the FL/RX queues
- * @irq_entries:	Array of MSI-X entries for all ports
+ * @app:		Pointer to the APP handle
+ * @data_vnic_bar:	Pointer to the CPP area for the data vNICs' BARs
+ * @ctrl_vnic_bar:	Pointer to the CPP area for the ctrl vNIC's BAR
+ * @qc_area:		Pointer to the CPP area for the queues
+ * @mac_stats_bar:	Pointer to the CPP area for the MAC stats
+ * @mac_stats_mem:	Pointer to mapped MAC stats area
+ * @vf_cfg_bar:		Pointer to the CPP area for the VF configuration BAR
+ * @vf_cfg_mem:		Pointer to mapped VF configuration area
+ * @irq_entries:	Array of MSI-X entries for all vNICs
  * @limit_vfs:		Number of VFs supported by firmware (~0 for PCI limit)
  * @num_vfs:		Number of SR-IOV VFs enabled
  * @fw_loaded:		Is the firmware loaded?
+ * @ctrl_vnic:		Pointer to the control vNIC if available
+ * @mip:		MIP handle
+ * @rtbl:		RTsym table
+ * @hwinfo:		HWInfo table
  * @eth_tbl:		NSP ETH table
+ * @nspi:		NSP identification info
+ * @hwmon_dev:		pointer to hwmon device
  * @ddir:		Per-device debugfs directory
- * @num_ports:		Number of adapter ports app firmware supports
- * @num_netdevs:	Number of netdevs spawned
- * @ports:		Linked list of port structures (struct nfp_net)
- * @port_lock:		Protects @ports, @num_ports, @num_netdevs
+ * @max_data_vnics:	Number of data vNICs app firmware supports
+ * @num_vnics:		Number of vNICs spawned
+ * @vnics:		Linked list of vNIC structures (struct nfp_net)
+ * @ports:		Linked list of port structures (struct nfp_port)
+ * @wq:			Workqueue for running works which need to grab @lock
  * @port_refresh_work:	Work entry for taking netdevs out
+ * @lock:		Protects all fields which may change after probe
  */
 struct nfp_pf {
 	struct pci_dev *pdev;
 
 	struct nfp_cpp *cpp;
 
-	struct nfp_cpp_area *ctrl_area;
-	struct nfp_cpp_area *tx_area;
-	struct nfp_cpp_area *rx_area;
+	struct nfp_app *app;
+
+	struct nfp_cpp_area *data_vnic_bar;
+	struct nfp_cpp_area *ctrl_vnic_bar;
+	struct nfp_cpp_area *qc_area;
+	struct nfp_cpp_area *mac_stats_bar;
+	u8 __iomem *mac_stats_mem;
+	struct nfp_cpp_area *vf_cfg_bar;
+	u8 __iomem *vf_cfg_mem;
 
 	struct msix_entry *irq_entries;
 
@@ -88,21 +115,42 @@ struct nfp_pf {
 
 	bool fw_loaded;
 
+	struct nfp_net *ctrl_vnic;
+
+	const struct nfp_mip *mip;
+	struct nfp_rtsym_table *rtbl;
+	struct nfp_hwinfo *hwinfo;
 	struct nfp_eth_table *eth_tbl;
+	struct nfp_nsp_identify *nspi;
+
+	struct device *hwmon_dev;
 
 	struct dentry *ddir;
 
-	unsigned int num_ports;
-	unsigned int num_netdevs;
+	unsigned int max_data_vnics;
+	unsigned int num_vnics;
 
+	struct list_head vnics;
 	struct list_head ports;
+
+	struct workqueue_struct *wq;
 	struct work_struct port_refresh_work;
-	struct mutex port_lock;
+
+	struct mutex lock;
 };
 
 extern struct pci_driver nfp_netvf_pci_driver;
 
+extern const struct devlink_ops nfp_devlink_ops;
+
 int nfp_net_pci_probe(struct nfp_pf *pf);
 void nfp_net_pci_remove(struct nfp_pf *pf);
 
+int nfp_hwmon_register(struct nfp_pf *pf);
+void nfp_hwmon_unregister(struct nfp_pf *pf);
+
+void nfp_net_get_mac_addr(struct nfp_pf *pf, struct nfp_port *port);
+
+bool nfp_ctrl_tx(struct nfp_net *nn, struct sk_buff *skb);
+
 #endif /* NFP_MAIN_H */
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net.h b/drivers/net/ethernet/netronome/nfp/nfp_net.h
index fcf81b3be830..b1fa77bd708b 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net.h
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net.h
@@ -50,15 +50,32 @@
 
 #include "nfp_net_ctrl.h"
 
-#define nn_err(nn, fmt, args...)  netdev_err((nn)->dp.netdev, fmt, ## args)
-#define nn_warn(nn, fmt, args...) netdev_warn((nn)->dp.netdev, fmt, ## args)
-#define nn_info(nn, fmt, args...) netdev_info((nn)->dp.netdev, fmt, ## args)
-#define nn_dbg(nn, fmt, args...)  netdev_dbg((nn)->dp.netdev, fmt, ## args)
+#define nn_pr(nn, lvl, fmt, args...)					\
+	({								\
+		struct nfp_net *__nn = (nn);				\
+									\
+		if (__nn->dp.netdev)					\
+			netdev_printk(lvl, __nn->dp.netdev, fmt, ## args); \
+		else							\
+			dev_printk(lvl, __nn->dp.dev, "ctrl: " fmt, ## args); \
+	})
+
+#define nn_err(nn, fmt, args...)	nn_pr(nn, KERN_ERR, fmt, ## args)
+#define nn_warn(nn, fmt, args...)	nn_pr(nn, KERN_WARNING, fmt, ## args)
+#define nn_info(nn, fmt, args...)	nn_pr(nn, KERN_INFO, fmt, ## args)
+#define nn_dbg(nn, fmt, args...)	nn_pr(nn, KERN_DEBUG, fmt, ## args)
+
 #define nn_dp_warn(dp, fmt, args...)					\
-	do {								\
-		if (unlikely(net_ratelimit()))				\
-			netdev_warn((dp)->netdev, fmt, ## args);	\
-	} while (0)
+	({								\
+		struct nfp_net_dp *__dp = (dp);				\
+									\
+		if (unlikely(net_ratelimit())) {			\
+			if (__dp->netdev)				\
+				netdev_warn(__dp->netdev, fmt, ## args); \
+			else						\
+				dev_warn(__dp->dev, fmt, ## args);	\
+		}							\
+	})
 
 /* Max time to wait for NFP to respond on updates (in seconds) */
 #define NFP_NET_POLL_TIMEOUT	5
@@ -84,7 +101,7 @@
 #define NFP_NET_NON_Q_VECTORS		2
 #define NFP_NET_IRQ_LSC_IDX		0
 #define NFP_NET_IRQ_EXN_IDX		1
-#define NFP_NET_MIN_PORT_IRQS		(NFP_NET_NON_Q_VECTORS + 1)
+#define NFP_NET_MIN_VNIC_IRQS		(NFP_NET_NON_Q_VECTORS + 1)
 
 /* Queue/Ring definitions */
 #define NFP_NET_MAX_TX_RINGS	64	/* Max. # of Tx rings per device */
@@ -102,6 +119,7 @@
 #define NFP_NET_RX_DESCS_DEFAULT 4096	/* Default # of Rx descs per ring */
 
 #define NFP_NET_FL_BATCH	16	/* Add freelist in this Batch size */
+#define NFP_NET_XDP_MAX_COMPLETE 2048	/* XDP bufs to reclaim in NAPI poll */
 
 /* Offload definitions */
 #define NFP_NET_N_VXLAN_PORTS	(NFP_NET_CFG_VXLAN_SZ / sizeof(__be16))
@@ -115,6 +133,10 @@ struct nfp_cpp;
 struct nfp_eth_table_port;
 struct nfp_net;
 struct nfp_net_r_vector;
+struct nfp_port;
+
+/* Convenience macro for wrapping descriptor index on ring size */
+#define D_IDX(ring, idx)	((idx) & ((ring)->cnt - 1))
 
 /* Convenience macro for writing dma address into RX/TX descriptors */
 #define nfp_desc_set_dma_addr(desc, dma_addr)				\
@@ -153,10 +175,15 @@ struct nfp_net_tx_desc {
 			__le32 dma_addr_lo; /* Low 32bit of host buf addr */
 
 			__le16 mss;	/* MSS to be used for LSO */
-			u8 l4_offset;	/* LSO, where the L4 data starts */
+			u8 lso_hdrlen;	/* LSO, TCP payload offset */
 			u8 flags;	/* TX Flags, see @PCIE_DESC_TX_* */
-
-			__le16 vlan;	/* VLAN tag to add if indicated */
+			union {
+				struct {
+					u8 l3_offset; /* L3 header offset */
+					u8 l4_offset; /* L4 header offset */
+				};
+				__le16 vlan; /* VLAN tag to add if indicated */
+			};
 			__le16 data_len; /* Length of frame + meta data */
 		} __packed;
 		__le32 vals[4];
@@ -287,9 +314,12 @@ struct nfp_net_rx_desc {
 #define NFP_NET_META_FIELD_MASK GENMASK(NFP_NET_META_FIELD_SIZE - 1, 0)
 
 struct nfp_meta_parsed {
-	u32 hash_type;
+	u8 hash_type;
+	u8 csum_type;
 	u32 hash;
 	u32 mark;
+	u32 portid;
+	__wsum csum;
 };
 
 struct nfp_net_rx_hash {
@@ -316,8 +346,6 @@ struct nfp_net_rx_buf {
  * @idx:        Ring index from Linux's perspective
  * @fl_qcidx:   Queue Controller Peripheral (QCP) queue index for the freelist
  * @qcp_fl:     Pointer to base of the QCP freelist queue
- * @wr_ptr_add: Accumulated number of buffers to add to QCP write pointer
- *              (used for free list batching)
  * @rxbufs:     Array of transmitted FL/RX buffers
  * @rxds:       Virtual address of FL/RX ring in host memory
  * @dma:        DMA address of the FL/RX ring
@@ -331,7 +359,6 @@ struct nfp_net_rx_ring {
 	u32 rd_p;
 
 	u32 idx;
-	u32 wr_ptr_add;
 
 	int fl_qcidx;
 	u8 __iomem *qcp_fl;
@@ -379,7 +406,14 @@ struct nfp_net_rx_ring {
  */
 struct nfp_net_r_vector {
 	struct nfp_net *nfp_net;
-	struct napi_struct napi;
+	union {
+		struct napi_struct napi;
+		struct {
+			struct tasklet_struct tasklet;
+			struct sk_buff_head queue;
+			struct spinlock lock;
+		};
+	};
 
 	struct nfp_net_tx_ring *tx_ring;
 	struct nfp_net_rx_ring *rx_ring;
@@ -508,11 +542,8 @@ struct nfp_net_dp {
  * @rss_cfg:            RSS configuration
  * @rss_key:            RSS secret key
  * @rss_itbl:           RSS indirection table
- * @rx_filter:		Filter offload statistics - dropped packets/bytes
- * @rx_filter_prev:	Filter offload statistics - values from previous update
- * @rx_filter_change:	Jiffies when statistics last changed
- * @rx_filter_stats_timer:  Timer for polling filter offload statistics
- * @rx_filter_lock:	Lock protecting timer state changes (teardown)
+ * @xdp_flags:		Flags with which XDP prog was loaded
+ * @xdp_prog:		XDP prog (for ctrl path, both DRV and HW modes)
  * @max_r_vecs:		Number of allocated interrupt vectors for RX/TX
  * @max_tx_rings:       Maximum number of TX rings supported by the Firmware
  * @max_rx_rings:       Maximum number of RX rings supported by the Firmware
@@ -531,7 +562,6 @@ struct nfp_net_dp {
  * @reconfig_sync_present:  Some thread is performing synchronous reconfig
  * @reconfig_timer:	Timer for async reading of reconfig results
  * @link_up:            Is the link up?
- * @link_changed:	Has link state changes since last port refresh?
  * @link_status_lock:	Protects @link_* and ensures atomicity with BAR reading
  * @rx_coalesce_usecs:      RX interrupt moderation usecs delay parameter
  * @rx_coalesce_max_frames: RX interrupt moderation frame count parameter
@@ -544,10 +574,11 @@ struct nfp_net_dp {
  * @rx_bar:             Pointer to mapped FL/RX queues
  * @debugfs_dir:	Device directory in debugfs
  * @ethtool_dump_flag:	Ethtool dump flag
- * @port_list:		Entry on device port list
+ * @vnic_list:		Entry on device vNIC list
  * @pdev:		Backpointer to PCI device
- * @cpp:		CPP device handle if available
- * @eth_port:		Translated ETH Table port entry
+ * @app:		APP handle if available
+ * @port:		Pointer to nfp_port structure if vNIC is a port
+ * @app_priv:		APP private data for this vNIC
  */
 struct nfp_net {
 	struct nfp_net_dp dp;
@@ -562,10 +593,8 @@ struct nfp_net {
 	u8 rss_key[NFP_NET_CFG_RSS_KEY_SZ];
 	u8 rss_itbl[NFP_NET_CFG_RSS_ITBL_SZ];
 
-	struct nfp_stat_pair rx_filter, rx_filter_prev;
-	unsigned long rx_filter_change;
-	struct timer_list rx_filter_stats_timer;
-	spinlock_t rx_filter_lock;
+	u32 xdp_flags;
+	struct bpf_prog *xdp_prog;
 
 	unsigned int max_tx_rings;
 	unsigned int max_rx_rings;
@@ -589,7 +618,6 @@ struct nfp_net {
 	u32 me_freq_mhz;
 
 	bool link_up;
-	bool link_changed;
 	spinlock_t link_status_lock;
 
 	spinlock_t reconfig_lock;
@@ -614,12 +642,14 @@ struct nfp_net {
 	struct dentry *debugfs_dir;
 	u32 ethtool_dump_flag;
 
-	struct list_head port_list;
+	struct list_head vnic_list;
 
 	struct pci_dev *pdev;
-	struct nfp_cpp *cpp;
+	struct nfp_app *app;
+
+	struct nfp_port *port;
 
-	struct nfp_eth_table_port *eth_port;
+	void *app_priv;
 };
 
 /* Functions to read/write from/to a BAR
@@ -681,6 +711,7 @@ static inline void nn_pci_flush(struct nfp_net *nn)
  * either add to a pointer or to read the pointer value.
  */
 #define NFP_QCP_QUEUE_ADDR_SZ			0x800
+#define NFP_QCP_QUEUE_AREA_SZ			0x80000
 #define NFP_QCP_QUEUE_OFF(_x)			((_x) * NFP_QCP_QUEUE_ADDR_SZ)
 #define NFP_QCP_QUEUE_ADD_RPTR			0x0000
 #define NFP_QCP_QUEUE_ADD_WPTR			0x0004
@@ -788,19 +819,47 @@ static inline u32 nfp_qcp_wr_ptr_read(u8 __iomem *q)
 	return _nfp_qcp_read(q, NFP_QCP_WRITE_PTR);
 }
 
+static inline bool nfp_net_is_data_vnic(struct nfp_net *nn)
+{
+	WARN_ON_ONCE(!nn->dp.netdev && nn->port);
+	return !!nn->dp.netdev;
+}
+
+static inline bool nfp_net_running(struct nfp_net *nn)
+{
+	return nn->dp.ctrl & NFP_NET_CFG_CTRL_ENABLE;
+}
+
+static inline const char *nfp_net_name(struct nfp_net *nn)
+{
+	return nn->dp.netdev ? nn->dp.netdev->name : "ctrl";
+}
+
 /* Globals */
 extern const char nfp_driver_version[];
 
+extern const struct net_device_ops nfp_net_netdev_ops;
+
+static inline bool nfp_netdev_is_nfp_net(struct net_device *netdev)
+{
+	return netdev->netdev_ops == &nfp_net_netdev_ops;
+}
+
 /* Prototypes */
 void nfp_net_get_fw_version(struct nfp_net_fw_version *fw_ver,
 			    void __iomem *ctrl_bar);
 
 struct nfp_net *
-nfp_net_netdev_alloc(struct pci_dev *pdev,
-		     unsigned int max_tx_rings, unsigned int max_rx_rings);
-void nfp_net_netdev_free(struct nfp_net *nn);
-int nfp_net_netdev_init(struct net_device *netdev);
-void nfp_net_netdev_clean(struct net_device *netdev);
+nfp_net_alloc(struct pci_dev *pdev, bool needs_netdev,
+	      unsigned int max_tx_rings, unsigned int max_rx_rings);
+void nfp_net_free(struct nfp_net *nn);
+
+int nfp_net_init(struct nfp_net *nn);
+void nfp_net_clean(struct nfp_net *nn);
+
+int nfp_ctrl_open(struct nfp_net *nn);
+void nfp_ctrl_close(struct nfp_net *nn);
+
 void nfp_net_set_ethtool_ops(struct net_device *netdev);
 void nfp_net_info(struct nfp_net *nn);
 int nfp_net_reconfig(struct nfp_net *nn, u32 update);
@@ -821,15 +880,11 @@ struct nfp_net_dp *nfp_net_clone_dp(struct nfp_net *nn);
 int nfp_net_ring_reconfig(struct nfp_net *nn, struct nfp_net_dp *new,
 			  struct netlink_ext_ack *extack);
 
-bool nfp_net_link_changed_read_clear(struct nfp_net *nn);
-int nfp_net_refresh_eth_port(struct nfp_net *nn);
-void nfp_net_refresh_port_table(struct nfp_net *nn);
-
 #ifdef CONFIG_NFP_DEBUG
 void nfp_net_debugfs_create(void);
 void nfp_net_debugfs_destroy(void);
 struct dentry *nfp_net_debugfs_device_add(struct pci_dev *pdev);
-void nfp_net_debugfs_port_add(struct nfp_net *nn, struct dentry *ddir, int id);
+void nfp_net_debugfs_vnic_add(struct nfp_net *nn, struct dentry *ddir, int id);
 void nfp_net_debugfs_dir_clean(struct dentry **dir);
 #else
 static inline void nfp_net_debugfs_create(void)
@@ -846,7 +901,7 @@ static inline struct dentry *nfp_net_debugfs_device_add(struct pci_dev *pdev)
 }
 
 static inline void
-nfp_net_debugfs_port_add(struct nfp_net *nn, struct dentry *ddir, int id)
+nfp_net_debugfs_vnic_add(struct nfp_net *nn, struct dentry *ddir, int id)
 {
 }
 
@@ -855,7 +910,4 @@ static inline void nfp_net_debugfs_dir_clean(struct dentry **dir)
 }
 #endif /* CONFIG_NFP_DEBUG */
 
-void nfp_net_filter_stats_timer(unsigned long data);
-int nfp_net_bpf_offload(struct nfp_net *nn, struct tc_cls_bpf_offload *cls_bpf);
-
 #endif /* _NFP_NET_H_ */
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_common.c b/drivers/net/ethernet/netronome/nfp/nfp_net_common.c
index 82bd6b0935f1..18750ff0ede6 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net_common.c
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_common.c
@@ -61,15 +61,17 @@
 #include <linux/log2.h>
 #include <linux/if_vlan.h>
 #include <linux/random.h>
-
+#include <linux/vmalloc.h>
 #include <linux/ktime.h>
 
-#include <net/pkt_cls.h>
+#include <net/switchdev.h>
 #include <net/vxlan.h>
 
 #include "nfpcore/nfp_nsp.h"
+#include "nfp_app.h"
 #include "nfp_net_ctrl.h"
 #include "nfp_net.h"
+#include "nfp_port.h"
 
 /**
  * nfp_net_get_fw_version() - Read and parse the FW version
@@ -279,6 +281,30 @@ int nfp_net_reconfig(struct nfp_net *nn, u32 update)
 	return ret;
 }
 
+/**
+ * nfp_net_reconfig_mbox() - Reconfigure the firmware via the mailbox
+ * @nn:        NFP Net device to reconfigure
+ * @mbox_cmd:  The value for the mailbox command
+ *
+ * Helper function for mailbox updates
+ *
+ * Return: Negative errno on error, 0 on success
+ */
+static int nfp_net_reconfig_mbox(struct nfp_net *nn, u32 mbox_cmd)
+{
+	int ret;
+
+	nn_writeq(nn, NFP_NET_CFG_MBOX_CMD, mbox_cmd);
+
+	ret = nfp_net_reconfig(nn, NFP_NET_CFG_UPDATE_MBOX);
+	if (ret) {
+		nn_err(nn, "Mailbox update error\n");
+		return ret;
+	}
+
+	return -nn_readl(nn, NFP_NET_CFG_MBOX_RET);
+}
+
 /* Interrupt configuration and handling
  */
 
@@ -391,17 +417,13 @@ static irqreturn_t nfp_net_irq_rxtx(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-bool nfp_net_link_changed_read_clear(struct nfp_net *nn)
+static irqreturn_t nfp_ctrl_irq_rxtx(int irq, void *data)
 {
-	unsigned long flags;
-	bool ret;
+	struct nfp_net_r_vector *r_vec = data;
 
-	spin_lock_irqsave(&nn->link_status_lock, flags);
-	ret = nn->link_changed;
-	nn->link_changed = false;
-	spin_unlock_irqrestore(&nn->link_status_lock, flags);
+	tasklet_schedule(&r_vec->tasklet);
 
-	return ret;
+	return IRQ_HANDLED;
 }
 
 /**
@@ -423,7 +445,8 @@ static void nfp_net_read_link_status(struct nfp_net *nn)
 		goto out;
 
 	nn->link_up = link_up;
-	nn->link_changed = true;
+	if (nn->port)
+		set_bit(NFP_PORT_CHANGED, &nn->port->flags);
 
 	if (nn->link_up) {
 		netif_carrier_on(nn->dp.netdev);
@@ -515,34 +538,6 @@ nfp_net_rx_ring_init(struct nfp_net_rx_ring *rx_ring,
 }
 
 /**
- * nfp_net_vecs_init() - Assign IRQs and setup rvecs.
- * @netdev:   netdev structure
- */
-static void nfp_net_vecs_init(struct net_device *netdev)
-{
-	struct nfp_net *nn = netdev_priv(netdev);
-	struct nfp_net_r_vector *r_vec;
-	int r;
-
-	nn->lsc_handler = nfp_net_irq_lsc;
-	nn->exn_handler = nfp_net_irq_exn;
-
-	for (r = 0; r < nn->max_r_vecs; r++) {
-		struct msix_entry *entry;
-
-		entry = &nn->irq_entries[NFP_NET_NON_Q_VECTORS + r];
-
-		r_vec = &nn->r_vecs[r];
-		r_vec->nfp_net = nn;
-		r_vec->handler = nfp_net_irq_rxtx;
-		r_vec->irq_entry = entry->entry;
-		r_vec->irq_vector = entry->vector;
-
-		cpumask_set_cpu(r, &r_vec->affinity_mask);
-	}
-}
-
-/**
  * nfp_net_aux_irq_request() - Request an auxiliary interrupt (LSC or EXN)
  * @nn:		NFP Network structure
  * @ctrl_offset: Control BAR offset where IRQ configuration should be written
@@ -562,7 +557,7 @@ nfp_net_aux_irq_request(struct nfp_net *nn, u32 ctrl_offset,
 
 	entry = &nn->irq_entries[vector_idx];
 
-	snprintf(name, name_sz, format, netdev_name(nn->dp.netdev));
+	snprintf(name, name_sz, format, nfp_net_name(nn));
 	err = request_irq(entry->vector, handler, 0, name, nn);
 	if (err) {
 		nn_err(nn, "Failed to request IRQ %d (err=%d).\n",
@@ -667,17 +662,22 @@ static void nfp_net_tx_tso(struct nfp_net_r_vector *r_vec,
 	if (!skb_is_gso(skb))
 		return;
 
-	if (!skb->encapsulation)
+	if (!skb->encapsulation) {
+		txd->l3_offset = skb_network_offset(skb);
+		txd->l4_offset = skb_transport_offset(skb);
 		hdrlen = skb_transport_offset(skb) + tcp_hdrlen(skb);
-	else
+	} else {
+		txd->l3_offset = skb_inner_network_offset(skb);
+		txd->l4_offset = skb_inner_transport_offset(skb);
 		hdrlen = skb_inner_transport_header(skb) - skb->data +
 			inner_tcp_hdrlen(skb);
+	}
 
 	txbuf->pkt_cnt = skb_shinfo(skb)->gso_segs;
 	txbuf->real_len += hdrlen * (txbuf->pkt_cnt - 1);
 
 	mss = skb_shinfo(skb)->gso_size & PCIE_DESC_TX_MSS_MASK;
-	txd->l4_offset = hdrlen;
+	txd->lso_hdrlen = hdrlen;
 	txd->mss = cpu_to_le16(mss);
 	txd->flags |= PCIE_DESC_TX_LSO;
 
@@ -756,6 +756,26 @@ static void nfp_net_tx_xmit_more_flush(struct nfp_net_tx_ring *tx_ring)
 	tx_ring->wr_ptr_add = 0;
 }
 
+static int nfp_net_prep_port_id(struct sk_buff *skb)
+{
+	struct metadata_dst *md_dst = skb_metadata_dst(skb);
+	unsigned char *data;
+
+	if (likely(!md_dst))
+		return 0;
+	if (unlikely(md_dst->type != METADATA_HW_PORT_MUX))
+		return 0;
+
+	if (unlikely(skb_cow_head(skb, 8)))
+		return -ENOMEM;
+
+	data = skb_push(skb, 8);
+	put_unaligned_be32(NFP_NET_META_PORTID, data);
+	put_unaligned_be32(md_dst->u.port_info.port_id, data + 4);
+
+	return 8;
+}
+
 /**
  * nfp_net_tx() - Main transmit entry point
  * @skb:    SKB to transmit
@@ -768,6 +788,7 @@ static int nfp_net_tx(struct sk_buff *skb, struct net_device *netdev)
 	struct nfp_net *nn = netdev_priv(netdev);
 	const struct skb_frag_struct *frag;
 	struct nfp_net_tx_desc *txd, txdg;
+	int f, nr_frags, wr_idx, md_bytes;
 	struct nfp_net_tx_ring *tx_ring;
 	struct nfp_net_r_vector *r_vec;
 	struct nfp_net_tx_buf *txbuf;
@@ -775,8 +796,6 @@ static int nfp_net_tx(struct sk_buff *skb, struct net_device *netdev)
 	struct nfp_net_dp *dp;
 	dma_addr_t dma_addr;
 	unsigned int fsize;
-	int f, nr_frags;
-	int wr_idx;
 	u16 qidx;
 
 	dp = &nn->dp;
@@ -798,13 +817,20 @@ static int nfp_net_tx(struct sk_buff *skb, struct net_device *netdev)
 		return NETDEV_TX_BUSY;
 	}
 
+	md_bytes = nfp_net_prep_port_id(skb);
+	if (unlikely(md_bytes < 0)) {
+		nfp_net_tx_xmit_more_flush(tx_ring);
+		dev_kfree_skb_any(skb);
+		return NETDEV_TX_OK;
+	}
+
 	/* Start with the head skbuf */
 	dma_addr = dma_map_single(dp->dev, skb->data, skb_headlen(skb),
 				  DMA_TO_DEVICE);
 	if (dma_mapping_error(dp->dev, dma_addr))
 		goto err_free;
 
-	wr_idx = tx_ring->wr_p & (tx_ring->cnt - 1);
+	wr_idx = D_IDX(tx_ring, tx_ring->wr_p);
 
 	/* Stash the soft descriptor of the head then initialize it */
 	txbuf = &tx_ring->txbufs[wr_idx];
@@ -816,19 +842,18 @@ static int nfp_net_tx(struct sk_buff *skb, struct net_device *netdev)
 
 	/* Build TX descriptor */
 	txd = &tx_ring->txds[wr_idx];
-	txd->offset_eop = (nr_frags == 0) ? PCIE_DESC_TX_EOP : 0;
+	txd->offset_eop = (nr_frags ? 0 : PCIE_DESC_TX_EOP) | md_bytes;
 	txd->dma_len = cpu_to_le16(skb_headlen(skb));
 	nfp_desc_set_dma_addr(txd, dma_addr);
 	txd->data_len = cpu_to_le16(skb->len);
 
 	txd->flags = 0;
 	txd->mss = 0;
-	txd->l4_offset = 0;
+	txd->lso_hdrlen = 0;
 
+	/* Do not reorder - tso may adjust pkt cnt, vlan may override fields */
 	nfp_net_tx_tso(r_vec, txbuf, txd, skb);
-
 	nfp_net_tx_csum(dp, r_vec, txbuf, txd, skb);
-
 	if (skb_vlan_tag_present(skb) && dp->ctrl & NFP_NET_CFG_CTRL_TXVLAN) {
 		txd->flags |= PCIE_DESC_TX_VLAN;
 		txd->vlan = cpu_to_le16(skb_vlan_tag_get(skb));
@@ -848,7 +873,7 @@ static int nfp_net_tx(struct sk_buff *skb, struct net_device *netdev)
 			if (dma_mapping_error(dp->dev, dma_addr))
 				goto err_unmap;
 
-			wr_idx = (wr_idx + 1) & (tx_ring->cnt - 1);
+			wr_idx = D_IDX(tx_ring, wr_idx + 1);
 			tx_ring->txbufs[wr_idx].skb = skb;
 			tx_ring->txbufs[wr_idx].dma_addr = dma_addr;
 			tx_ring->txbufs[wr_idx].fidx = f;
@@ -857,7 +882,7 @@ static int nfp_net_tx(struct sk_buff *skb, struct net_device *netdev)
 			*txd = txdg;
 			txd->dma_len = cpu_to_le16(fsize);
 			nfp_desc_set_dma_addr(txd, dma_addr);
-			txd->offset_eop =
+			txd->offset_eop |=
 				(f == nr_frags - 1) ? PCIE_DESC_TX_EOP : 0;
 		}
 
@@ -936,14 +961,10 @@ static void nfp_net_tx_complete(struct nfp_net_tx_ring *tx_ring)
 	if (qcp_rd_p == tx_ring->qcp_rd_p)
 		return;
 
-	if (qcp_rd_p > tx_ring->qcp_rd_p)
-		todo = qcp_rd_p - tx_ring->qcp_rd_p;
-	else
-		todo = qcp_rd_p + tx_ring->cnt - tx_ring->qcp_rd_p;
+	todo = D_IDX(tx_ring, qcp_rd_p - tx_ring->qcp_rd_p);
 
 	while (todo--) {
-		idx = tx_ring->rd_p & (tx_ring->cnt - 1);
-		tx_ring->rd_p++;
+		idx = D_IDX(tx_ring, tx_ring->rd_p++);
 
 		skb = tx_ring->txbufs[idx].skb;
 		if (!skb)
@@ -982,6 +1003,9 @@ static void nfp_net_tx_complete(struct nfp_net_tx_ring *tx_ring)
 	r_vec->tx_pkts += done_pkts;
 	u64_stats_update_end(&r_vec->tx_sync);
 
+	if (!dp->netdev)
+		return;
+
 	nd_q = netdev_get_tx_queue(dp->netdev, tx_ring->idx);
 	netdev_tx_completed_queue(nd_q, done_pkts, done_bytes);
 	if (nfp_net_tx_ring_should_wake(tx_ring)) {
@@ -997,45 +1021,45 @@ static void nfp_net_tx_complete(struct nfp_net_tx_ring *tx_ring)
 		  tx_ring->rd_p, tx_ring->wr_p, tx_ring->cnt);
 }
 
-static void nfp_net_xdp_complete(struct nfp_net_tx_ring *tx_ring)
+static bool nfp_net_xdp_complete(struct nfp_net_tx_ring *tx_ring)
 {
 	struct nfp_net_r_vector *r_vec = tx_ring->r_vec;
 	u32 done_pkts = 0, done_bytes = 0;
+	bool done_all;
 	int idx, todo;
 	u32 qcp_rd_p;
 
-	if (tx_ring->wr_p == tx_ring->rd_p)
-		return;
-
 	/* Work out how many descriptors have been transmitted */
 	qcp_rd_p = nfp_qcp_rd_ptr_read(tx_ring->qcp_q);
 
 	if (qcp_rd_p == tx_ring->qcp_rd_p)
-		return;
+		return true;
 
-	if (qcp_rd_p > tx_ring->qcp_rd_p)
-		todo = qcp_rd_p - tx_ring->qcp_rd_p;
-	else
-		todo = qcp_rd_p + tx_ring->cnt - tx_ring->qcp_rd_p;
+	todo = D_IDX(tx_ring, qcp_rd_p - tx_ring->qcp_rd_p);
+
+	done_all = todo <= NFP_NET_XDP_MAX_COMPLETE;
+	todo = min(todo, NFP_NET_XDP_MAX_COMPLETE);
+
+	tx_ring->qcp_rd_p = D_IDX(tx_ring, tx_ring->qcp_rd_p + todo);
 
 	done_pkts = todo;
 	while (todo--) {
-		idx = tx_ring->rd_p & (tx_ring->cnt - 1);
+		idx = D_IDX(tx_ring, tx_ring->rd_p);
 		tx_ring->rd_p++;
 
 		done_bytes += tx_ring->txbufs[idx].real_len;
 	}
 
-	tx_ring->qcp_rd_p = qcp_rd_p;
-
 	u64_stats_update_begin(&r_vec->tx_sync);
 	r_vec->tx_bytes += done_bytes;
 	r_vec->tx_pkts += done_pkts;
 	u64_stats_update_end(&r_vec->tx_sync);
 
 	WARN_ONCE(tx_ring->wr_p - tx_ring->rd_p > tx_ring->cnt,
-		  "TX ring corruption rd_p=%u wr_p=%u cnt=%u\n",
+		  "XDP TX ring corruption rd_p=%u wr_p=%u cnt=%u\n",
 		  tx_ring->rd_p, tx_ring->wr_p, tx_ring->cnt);
+
+	return done_all;
 }
 
 /**
@@ -1056,7 +1080,7 @@ nfp_net_tx_ring_reset(struct nfp_net_dp *dp, struct nfp_net_tx_ring *tx_ring)
 		struct sk_buff *skb;
 		int idx, nr_frags;
 
-		idx = tx_ring->rd_p & (tx_ring->cnt - 1);
+		idx = D_IDX(tx_ring, tx_ring->rd_p);
 		tx_buf = &tx_ring->txbufs[idx];
 
 		skb = tx_ring->txbufs[idx].skb;
@@ -1091,7 +1115,7 @@ nfp_net_tx_ring_reset(struct nfp_net_dp *dp, struct nfp_net_tx_ring *tx_ring)
 	tx_ring->qcp_rd_p = 0;
 	tx_ring->wr_ptr_add = 0;
 
-	if (tx_ring->is_xdp)
+	if (tx_ring->is_xdp || !dp->netdev)
 		return;
 
 	nd_q = netdev_get_tx_queue(dp->netdev, tx_ring->idx);
@@ -1209,7 +1233,7 @@ static void nfp_net_rx_give_one(const struct nfp_net_dp *dp,
 {
 	unsigned int wr_idx;
 
-	wr_idx = rx_ring->wr_p & (rx_ring->cnt - 1);
+	wr_idx = D_IDX(rx_ring, rx_ring->wr_p);
 
 	nfp_net_dma_sync_dev_rx(dp, dma_addr);
 
@@ -1224,14 +1248,12 @@ static void nfp_net_rx_give_one(const struct nfp_net_dp *dp,
 			      dma_addr + dp->rx_dma_off);
 
 	rx_ring->wr_p++;
-	rx_ring->wr_ptr_add++;
-	if (rx_ring->wr_ptr_add >= NFP_NET_FL_BATCH) {
+	if (!(rx_ring->wr_p % NFP_NET_FL_BATCH)) {
 		/* Update write pointer of the freelist queue. Make
 		 * sure all writes are flushed before telling the hardware.
 		 */
 		wmb();
-		nfp_qcp_wr_ptr_add(rx_ring->qcp_fl, rx_ring->wr_ptr_add);
-		rx_ring->wr_ptr_add = 0;
+		nfp_qcp_wr_ptr_add(rx_ring->qcp_fl, NFP_NET_FL_BATCH);
 	}
 }
 
@@ -1247,7 +1269,7 @@ static void nfp_net_rx_ring_reset(struct nfp_net_rx_ring *rx_ring)
 	unsigned int wr_idx, last_idx;
 
 	/* Move the empty entry to the end of the list */
-	wr_idx = rx_ring->wr_p & (rx_ring->cnt - 1);
+	wr_idx = D_IDX(rx_ring, rx_ring->wr_p);
 	last_idx = rx_ring->cnt - 1;
 	rx_ring->rxbufs[wr_idx].dma_addr = rx_ring->rxbufs[last_idx].dma_addr;
 	rx_ring->rxbufs[wr_idx].frag = rx_ring->rxbufs[last_idx].frag;
@@ -1257,7 +1279,6 @@ static void nfp_net_rx_ring_reset(struct nfp_net_rx_ring *rx_ring)
 	memset(rx_ring->rxds, 0, sizeof(*rx_ring->rxds) * rx_ring->cnt);
 	rx_ring->wr_p = 0;
 	rx_ring->rd_p = 0;
-	rx_ring->wr_ptr_add = 0;
 }
 
 /**
@@ -1350,17 +1371,28 @@ static int nfp_net_rx_csum_has_errors(u16 flags)
  * @dp:  NFP Net data path struct
  * @r_vec: per-ring structure
  * @rxd: Pointer to RX descriptor
+ * @meta: Parsed metadata prepend
  * @skb: Pointer to SKB
  */
 static void nfp_net_rx_csum(struct nfp_net_dp *dp,
 			    struct nfp_net_r_vector *r_vec,
-			    struct nfp_net_rx_desc *rxd, struct sk_buff *skb)
+			    struct nfp_net_rx_desc *rxd,
+			    struct nfp_meta_parsed *meta, struct sk_buff *skb)
 {
 	skb_checksum_none_assert(skb);
 
 	if (!(dp->netdev->features & NETIF_F_RXCSUM))
 		return;
 
+	if (meta->csum_type) {
+		skb->ip_summed = meta->csum_type;
+		skb->csum = meta->csum;
+		u64_stats_update_begin(&r_vec->rx_sync);
+		r_vec->hw_csum_rx_ok++;
+		u64_stats_update_end(&r_vec->rx_sync);
+		return;
+	}
+
 	if (nfp_net_rx_csum_has_errors(le16_to_cpu(rxd->rxd.flags))) {
 		u64_stats_update_begin(&r_vec->rx_sync);
 		r_vec->hw_csum_rx_error++;
@@ -1445,6 +1477,16 @@ nfp_net_parse_meta(struct net_device *netdev, struct nfp_meta_parsed *meta,
 			meta->mark = get_unaligned_be32(data);
 			data += 4;
 			break;
+		case NFP_NET_META_PORTID:
+			meta->portid = get_unaligned_be32(data);
+			data += 4;
+			break;
+		case NFP_NET_META_CSUM:
+			meta->csum_type = CHECKSUM_COMPLETE;
+			meta->csum =
+				(__force __wsum)__get_unaligned_cpu32(data);
+			data += 4;
+			break;
 		default:
 			return NULL;
 		}
@@ -1479,18 +1521,26 @@ static bool
 nfp_net_tx_xdp_buf(struct nfp_net_dp *dp, struct nfp_net_rx_ring *rx_ring,
 		   struct nfp_net_tx_ring *tx_ring,
 		   struct nfp_net_rx_buf *rxbuf, unsigned int dma_off,
-		   unsigned int pkt_len)
+		   unsigned int pkt_len, bool *completed)
 {
 	struct nfp_net_tx_buf *txbuf;
 	struct nfp_net_tx_desc *txd;
 	int wr_idx;
 
 	if (unlikely(nfp_net_tx_full(tx_ring, 1))) {
-		nfp_net_rx_drop(dp, rx_ring->r_vec, rx_ring, rxbuf, NULL);
-		return false;
+		if (!*completed) {
+			nfp_net_xdp_complete(tx_ring);
+			*completed = true;
+		}
+
+		if (unlikely(nfp_net_tx_full(tx_ring, 1))) {
+			nfp_net_rx_drop(dp, rx_ring->r_vec, rx_ring, rxbuf,
+					NULL);
+			return false;
+		}
 	}
 
-	wr_idx = tx_ring->wr_p & (tx_ring->cnt - 1);
+	wr_idx = D_IDX(tx_ring, tx_ring->wr_p);
 
 	/* Stash the soft descriptor of the head then initialize it */
 	txbuf = &tx_ring->txbufs[wr_idx];
@@ -1515,7 +1565,7 @@ nfp_net_tx_xdp_buf(struct nfp_net_dp *dp, struct nfp_net_rx_ring *rx_ring,
 
 	txd->flags = 0;
 	txd->mss = 0;
-	txd->l4_offset = 0;
+	txd->lso_hdrlen = 0;
 
 	tx_ring->wr_p++;
 	tx_ring->wr_ptr_add++;
@@ -1559,6 +1609,7 @@ static int nfp_net_rx(struct nfp_net_rx_ring *rx_ring, int budget)
 	struct nfp_net_dp *dp = &r_vec->nfp_net->dp;
 	struct nfp_net_tx_ring *tx_ring;
 	struct bpf_prog *xdp_prog;
+	bool xdp_tx_cmpl = false;
 	unsigned int true_bufsz;
 	struct sk_buff *skb;
 	int pkts_polled = 0;
@@ -1574,10 +1625,11 @@ static int nfp_net_rx(struct nfp_net_rx_ring *rx_ring, int budget)
 		struct nfp_net_rx_buf *rxbuf;
 		struct nfp_net_rx_desc *rxd;
 		struct nfp_meta_parsed meta;
+		struct net_device *netdev;
 		dma_addr_t new_dma_addr;
 		void *new_frag;
 
-		idx = rx_ring->rd_p & (rx_ring->cnt - 1);
+		idx = D_IDX(rx_ring, rx_ring->rd_p);
 
 		rxd = &rx_ring->rxds[idx];
 		if (!(rxd->rxd.meta_len_dd & PCIE_DESC_RX_DD))
@@ -1652,7 +1704,7 @@ static int nfp_net_rx(struct nfp_net_rx_ring *rx_ring, int budget)
 		}
 
 		if (xdp_prog && !(rxd->rxd.flags & PCIE_DESC_RX_BPF &&
-				  dp->bpf_offload_xdp)) {
+				  dp->bpf_offload_xdp) && !meta.portid) {
 			unsigned int dma_off;
 			void *hard_start;
 			int act;
@@ -1669,14 +1721,17 @@ static int nfp_net_rx(struct nfp_net_rx_ring *rx_ring, int budget)
 				if (unlikely(!nfp_net_tx_xdp_buf(dp, rx_ring,
 								 tx_ring, rxbuf,
 								 dma_off,
-								 pkt_len)))
+								 pkt_len,
+								 &xdp_tx_cmpl)))
 					trace_xdp_exception(dp->netdev,
 							    xdp_prog, act);
 				continue;
 			default:
 				bpf_warn_invalid_xdp_action(act);
+				/* fall through */
 			case XDP_ABORTED:
 				trace_xdp_exception(dp->netdev, xdp_prog, act);
+				/* fall through */
 			case XDP_DROP:
 				nfp_net_rx_give_one(dp, rx_ring, rxbuf->frag,
 						    rxbuf->dma_addr);
@@ -1695,6 +1750,20 @@ static int nfp_net_rx(struct nfp_net_rx_ring *rx_ring, int budget)
 			continue;
 		}
 
+		if (likely(!meta.portid)) {
+			netdev = dp->netdev;
+		} else {
+			struct nfp_net *nn;
+
+			nn = netdev_priv(dp->netdev);
+			netdev = nfp_app_repr_get(nn->app, meta.portid);
+			if (unlikely(!netdev)) {
+				nfp_net_rx_drop(dp, r_vec, rx_ring, rxbuf, skb);
+				continue;
+			}
+			nfp_repr_inc_rx_stats(netdev, pkt_len);
+		}
+
 		nfp_net_dma_unmap_rx(dp, rxbuf->dma_addr);
 
 		nfp_net_rx_give_one(dp, rx_ring, new_frag, new_dma_addr);
@@ -1706,9 +1775,9 @@ static int nfp_net_rx(struct nfp_net_rx_ring *rx_ring, int budget)
 		skb_set_hash(skb, meta.hash, meta.hash_type);
 
 		skb_record_rx_queue(skb, rx_ring->idx);
-		skb->protocol = eth_type_trans(skb, dp->netdev);
+		skb->protocol = eth_type_trans(skb, netdev);
 
-		nfp_net_rx_csum(dp, r_vec, rxd, skb);
+		nfp_net_rx_csum(dp, r_vec, rxd, &meta, skb);
 
 		if (rxd->rxd.flags & PCIE_DESC_RX_VLAN)
 			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
@@ -1717,8 +1786,14 @@ static int nfp_net_rx(struct nfp_net_rx_ring *rx_ring, int budget)
 		napi_gro_receive(&rx_ring->r_vec->napi, skb);
 	}
 
-	if (xdp_prog && tx_ring->wr_ptr_add)
-		nfp_net_tx_xmit_more_flush(tx_ring);
+	if (xdp_prog) {
+		if (tx_ring->wr_ptr_add)
+			nfp_net_tx_xmit_more_flush(tx_ring);
+		else if (unlikely(tx_ring->wr_p != tx_ring->rd_p) &&
+			 !xdp_tx_cmpl)
+			if (!nfp_net_xdp_complete(tx_ring))
+				pkts_polled = budget;
+	}
 	rcu_read_unlock();
 
 	return pkts_polled;
@@ -1739,11 +1814,8 @@ static int nfp_net_poll(struct napi_struct *napi, int budget)
 
 	if (r_vec->tx_ring)
 		nfp_net_tx_complete(r_vec->tx_ring);
-	if (r_vec->rx_ring) {
+	if (r_vec->rx_ring)
 		pkts_polled = nfp_net_rx(r_vec->rx_ring, budget);
-		if (r_vec->xdp_ring)
-			nfp_net_xdp_complete(r_vec->xdp_ring);
-	}
 
 	if (pkts_polled < budget)
 		if (napi_complete_done(napi, pkts_polled))
@@ -1752,10 +1824,273 @@ static int nfp_net_poll(struct napi_struct *napi, int budget)
 	return pkts_polled;
 }
 
+/* Control device data path
+ */
+
+static bool
+nfp_ctrl_tx_one(struct nfp_net *nn, struct nfp_net_r_vector *r_vec,
+		struct sk_buff *skb, bool old)
+{
+	unsigned int real_len = skb->len, meta_len = 0;
+	struct nfp_net_tx_ring *tx_ring;
+	struct nfp_net_tx_buf *txbuf;
+	struct nfp_net_tx_desc *txd;
+	struct nfp_net_dp *dp;
+	dma_addr_t dma_addr;
+	int wr_idx;
+
+	dp = &r_vec->nfp_net->dp;
+	tx_ring = r_vec->tx_ring;
+
+	if (WARN_ON_ONCE(skb_shinfo(skb)->nr_frags)) {
+		nn_dp_warn(dp, "Driver's CTRL TX does not implement gather\n");
+		goto err_free;
+	}
+
+	if (unlikely(nfp_net_tx_full(tx_ring, 1))) {
+		u64_stats_update_begin(&r_vec->tx_sync);
+		r_vec->tx_busy++;
+		u64_stats_update_end(&r_vec->tx_sync);
+		if (!old)
+			__skb_queue_tail(&r_vec->queue, skb);
+		else
+			__skb_queue_head(&r_vec->queue, skb);
+		return true;
+	}
+
+	if (nfp_app_ctrl_has_meta(nn->app)) {
+		if (unlikely(skb_headroom(skb) < 8)) {
+			nn_dp_warn(dp, "CTRL TX on skb without headroom\n");
+			goto err_free;
+		}
+		meta_len = 8;
+		put_unaligned_be32(NFP_META_PORT_ID_CTRL, skb_push(skb, 4));
+		put_unaligned_be32(NFP_NET_META_PORTID, skb_push(skb, 4));
+	}
+
+	/* Start with the head skbuf */
+	dma_addr = dma_map_single(dp->dev, skb->data, skb_headlen(skb),
+				  DMA_TO_DEVICE);
+	if (dma_mapping_error(dp->dev, dma_addr))
+		goto err_dma_warn;
+
+	wr_idx = D_IDX(tx_ring, tx_ring->wr_p);
+
+	/* Stash the soft descriptor of the head then initialize it */
+	txbuf = &tx_ring->txbufs[wr_idx];
+	txbuf->skb = skb;
+	txbuf->dma_addr = dma_addr;
+	txbuf->fidx = -1;
+	txbuf->pkt_cnt = 1;
+	txbuf->real_len = real_len;
+
+	/* Build TX descriptor */
+	txd = &tx_ring->txds[wr_idx];
+	txd->offset_eop = meta_len | PCIE_DESC_TX_EOP;
+	txd->dma_len = cpu_to_le16(skb_headlen(skb));
+	nfp_desc_set_dma_addr(txd, dma_addr);
+	txd->data_len = cpu_to_le16(skb->len);
+
+	txd->flags = 0;
+	txd->mss = 0;
+	txd->lso_hdrlen = 0;
+
+	tx_ring->wr_p++;
+	tx_ring->wr_ptr_add++;
+	nfp_net_tx_xmit_more_flush(tx_ring);
+
+	return false;
+
+err_dma_warn:
+	nn_dp_warn(dp, "Failed to DMA map TX CTRL buffer\n");
+err_free:
+	u64_stats_update_begin(&r_vec->tx_sync);
+	r_vec->tx_errors++;
+	u64_stats_update_end(&r_vec->tx_sync);
+	dev_kfree_skb_any(skb);
+	return false;
+}
+
+bool nfp_ctrl_tx(struct nfp_net *nn, struct sk_buff *skb)
+{
+	struct nfp_net_r_vector *r_vec = &nn->r_vecs[0];
+	bool ret;
+
+	spin_lock_bh(&r_vec->lock);
+	ret = nfp_ctrl_tx_one(nn, r_vec, skb, false);
+	spin_unlock_bh(&r_vec->lock);
+
+	return ret;
+}
+
+static void __nfp_ctrl_tx_queued(struct nfp_net_r_vector *r_vec)
+{
+	struct sk_buff *skb;
+
+	while ((skb = __skb_dequeue(&r_vec->queue)))
+		if (nfp_ctrl_tx_one(r_vec->nfp_net, r_vec, skb, true))
+			return;
+}
+
+static bool
+nfp_ctrl_meta_ok(struct nfp_net *nn, void *data, unsigned int meta_len)
+{
+	u32 meta_type, meta_tag;
+
+	if (!nfp_app_ctrl_has_meta(nn->app))
+		return !meta_len;
+
+	if (meta_len != 8)
+		return false;
+
+	meta_type = get_unaligned_be32(data);
+	meta_tag = get_unaligned_be32(data + 4);
+
+	return (meta_type == NFP_NET_META_PORTID &&
+		meta_tag == NFP_META_PORT_ID_CTRL);
+}
+
+static bool
+nfp_ctrl_rx_one(struct nfp_net *nn, struct nfp_net_dp *dp,
+		struct nfp_net_r_vector *r_vec, struct nfp_net_rx_ring *rx_ring)
+{
+	unsigned int meta_len, data_len, meta_off, pkt_len, pkt_off;
+	struct nfp_net_rx_buf *rxbuf;
+	struct nfp_net_rx_desc *rxd;
+	dma_addr_t new_dma_addr;
+	struct sk_buff *skb;
+	void *new_frag;
+	int idx;
+
+	idx = D_IDX(rx_ring, rx_ring->rd_p);
+
+	rxd = &rx_ring->rxds[idx];
+	if (!(rxd->rxd.meta_len_dd & PCIE_DESC_RX_DD))
+		return false;
+
+	/* Memory barrier to ensure that we won't do other reads
+	 * before the DD bit.
+	 */
+	dma_rmb();
+
+	rx_ring->rd_p++;
+
+	rxbuf =	&rx_ring->rxbufs[idx];
+	meta_len = rxd->rxd.meta_len_dd & PCIE_DESC_RX_META_LEN_MASK;
+	data_len = le16_to_cpu(rxd->rxd.data_len);
+	pkt_len = data_len - meta_len;
+
+	pkt_off = NFP_NET_RX_BUF_HEADROOM + dp->rx_dma_off;
+	if (dp->rx_offset == NFP_NET_CFG_RX_OFFSET_DYNAMIC)
+		pkt_off += meta_len;
+	else
+		pkt_off += dp->rx_offset;
+	meta_off = pkt_off - meta_len;
+
+	/* Stats update */
+	u64_stats_update_begin(&r_vec->rx_sync);
+	r_vec->rx_pkts++;
+	r_vec->rx_bytes += pkt_len;
+	u64_stats_update_end(&r_vec->rx_sync);
+
+	nfp_net_dma_sync_cpu_rx(dp, rxbuf->dma_addr + meta_off,	data_len);
+
+	if (unlikely(!nfp_ctrl_meta_ok(nn, rxbuf->frag + meta_off, meta_len))) {
+		nn_dp_warn(dp, "incorrect metadata for ctrl packet (%d)\n",
+			   meta_len);
+		nfp_net_rx_drop(dp, r_vec, rx_ring, rxbuf, NULL);
+		return true;
+	}
+
+	skb = build_skb(rxbuf->frag, dp->fl_bufsz);
+	if (unlikely(!skb)) {
+		nfp_net_rx_drop(dp, r_vec, rx_ring, rxbuf, NULL);
+		return true;
+	}
+	new_frag = nfp_net_napi_alloc_one(dp, &new_dma_addr);
+	if (unlikely(!new_frag)) {
+		nfp_net_rx_drop(dp, r_vec, rx_ring, rxbuf, skb);
+		return true;
+	}
+
+	nfp_net_dma_unmap_rx(dp, rxbuf->dma_addr);
+
+	nfp_net_rx_give_one(dp, rx_ring, new_frag, new_dma_addr);
+
+	skb_reserve(skb, pkt_off);
+	skb_put(skb, pkt_len);
+
+	nfp_app_ctrl_rx(nn->app, skb);
+
+	return true;
+}
+
+static void nfp_ctrl_rx(struct nfp_net_r_vector *r_vec)
+{
+	struct nfp_net_rx_ring *rx_ring = r_vec->rx_ring;
+	struct nfp_net *nn = r_vec->nfp_net;
+	struct nfp_net_dp *dp = &nn->dp;
+
+	while (nfp_ctrl_rx_one(nn, dp, r_vec, rx_ring))
+		continue;
+}
+
+static void nfp_ctrl_poll(unsigned long arg)
+{
+	struct nfp_net_r_vector *r_vec = (void *)arg;
+
+	spin_lock_bh(&r_vec->lock);
+	nfp_net_tx_complete(r_vec->tx_ring);
+	__nfp_ctrl_tx_queued(r_vec);
+	spin_unlock_bh(&r_vec->lock);
+
+	nfp_ctrl_rx(r_vec);
+
+	nfp_net_irq_unmask(r_vec->nfp_net, r_vec->irq_entry);
+}
+
 /* Setup and Configuration
  */
 
 /**
+ * nfp_net_vecs_init() - Assign IRQs and setup rvecs.
+ * @nn:		NFP Network structure
+ */
+static void nfp_net_vecs_init(struct nfp_net *nn)
+{
+	struct nfp_net_r_vector *r_vec;
+	int r;
+
+	nn->lsc_handler = nfp_net_irq_lsc;
+	nn->exn_handler = nfp_net_irq_exn;
+
+	for (r = 0; r < nn->max_r_vecs; r++) {
+		struct msix_entry *entry;
+
+		entry = &nn->irq_entries[NFP_NET_NON_Q_VECTORS + r];
+
+		r_vec = &nn->r_vecs[r];
+		r_vec->nfp_net = nn;
+		r_vec->irq_entry = entry->entry;
+		r_vec->irq_vector = entry->vector;
+
+		if (nn->dp.netdev) {
+			r_vec->handler = nfp_net_irq_rxtx;
+		} else {
+			r_vec->handler = nfp_ctrl_irq_rxtx;
+
+			__skb_queue_head_init(&r_vec->queue);
+			spin_lock_init(&r_vec->lock);
+			tasklet_init(&r_vec->tasklet, nfp_ctrl_poll,
+				     (unsigned long)r_vec);
+			tasklet_disable(&r_vec->tasklet);
+		}
+
+		cpumask_set_cpu(r, &r_vec->affinity_mask);
+	}
+}
+
+/**
  * nfp_net_tx_ring_free() - Free resources allocated to a TX ring
  * @tx_ring:   TX ring to free
  */
@@ -1803,7 +2138,7 @@ nfp_net_tx_ring_alloc(struct nfp_net_dp *dp, struct nfp_net_tx_ring *tx_ring)
 	if (!tx_ring->txbufs)
 		goto err_alloc;
 
-	if (!tx_ring->is_xdp)
+	if (!tx_ring->is_xdp && dp->netdev)
 		netif_set_xps_queue(dp->netdev, &r_vec->affinity_mask,
 				    tx_ring->idx);
 
@@ -2017,15 +2352,22 @@ nfp_net_prepare_vector(struct nfp_net *nn, struct nfp_net_r_vector *r_vec,
 	int err;
 
 	/* Setup NAPI */
-	netif_napi_add(nn->dp.netdev, &r_vec->napi,
-		       nfp_net_poll, NAPI_POLL_WEIGHT);
+	if (nn->dp.netdev)
+		netif_napi_add(nn->dp.netdev, &r_vec->napi,
+			       nfp_net_poll, NAPI_POLL_WEIGHT);
+	else
+		tasklet_enable(&r_vec->tasklet);
 
 	snprintf(r_vec->name, sizeof(r_vec->name),
-		 "%s-rxtx-%d", nn->dp.netdev->name, idx);
+		 "%s-rxtx-%d", nfp_net_name(nn), idx);
 	err = request_irq(r_vec->irq_vector, r_vec->handler, 0, r_vec->name,
 			  r_vec);
 	if (err) {
-		netif_napi_del(&r_vec->napi);
+		if (nn->dp.netdev)
+			netif_napi_del(&r_vec->napi);
+		else
+			tasklet_disable(&r_vec->tasklet);
+
 		nn_err(nn, "Error requesting IRQ %d\n", r_vec->irq_vector);
 		return err;
 	}
@@ -2043,7 +2385,11 @@ static void
 nfp_net_cleanup_vector(struct nfp_net *nn, struct nfp_net_r_vector *r_vec)
 {
 	irq_set_affinity_hint(r_vec->irq_vector, NULL);
-	netif_napi_del(&r_vec->napi);
+	if (nn->dp.netdev)
+		netif_napi_del(&r_vec->napi);
+	else
+		tasklet_disable(&r_vec->tasklet);
+
 	free_irq(r_vec->irq_vector, r_vec);
 }
 
@@ -2105,17 +2451,16 @@ void nfp_net_coalesce_write_cfg(struct nfp_net *nn)
 /**
  * nfp_net_write_mac_addr() - Write mac address to the device control BAR
  * @nn:      NFP Net device to reconfigure
+ * @addr:    MAC address to write
  *
  * Writes the MAC address from the netdev to the device control BAR.  Does not
  * perform the required reconfig.  We do a bit of byte swapping dance because
  * firmware is LE.
  */
-static void nfp_net_write_mac_addr(struct nfp_net *nn)
+static void nfp_net_write_mac_addr(struct nfp_net *nn, const u8 *addr)
 {
-	nn_writel(nn, NFP_NET_CFG_MACADDR + 0,
-		  get_unaligned_be32(nn->dp.netdev->dev_addr));
-	nn_writew(nn, NFP_NET_CFG_MACADDR + 6,
-		  get_unaligned_be16(nn->dp.netdev->dev_addr + 4));
+	nn_writel(nn, NFP_NET_CFG_MACADDR + 0, get_unaligned_be32(addr));
+	nn_writew(nn, NFP_NET_CFG_MACADDR + 6, get_unaligned_be16(addr + 4));
 }
 
 static void nfp_net_vec_clear_ring_data(struct nfp_net *nn, unsigned int idx)
@@ -2197,17 +2542,15 @@ static int nfp_net_set_config_and_enable(struct nfp_net *nn)
 
 	new_ctrl = nn->dp.ctrl;
 
-	if (nn->cap & NFP_NET_CFG_CTRL_RSS) {
+	if (nn->dp.ctrl & NFP_NET_CFG_CTRL_RSS_ANY) {
 		nfp_net_rss_write_key(nn);
 		nfp_net_rss_write_itbl(nn);
 		nn_writel(nn, NFP_NET_CFG_RSS_CTRL, nn->rss_cfg);
 		update |= NFP_NET_CFG_UPDATE_RSS;
 	}
 
-	if (nn->cap & NFP_NET_CFG_CTRL_IRQMOD) {
+	if (nn->dp.ctrl & NFP_NET_CFG_CTRL_IRQMOD) {
 		nfp_net_coalesce_write_cfg(nn);
-
-		new_ctrl |= NFP_NET_CFG_CTRL_IRQMOD;
 		update |= NFP_NET_CFG_UPDATE_IRQMOD;
 	}
 
@@ -2222,9 +2565,10 @@ static int nfp_net_set_config_and_enable(struct nfp_net *nn)
 	nn_writeq(nn, NFP_NET_CFG_RXRS_ENABLE, nn->dp.num_rx_rings == 64 ?
 		  0xffffffffffffffffULL : ((u64)1 << nn->dp.num_rx_rings) - 1);
 
-	nfp_net_write_mac_addr(nn);
+	if (nn->dp.netdev)
+		nfp_net_write_mac_addr(nn, nn->dp.netdev->dev_addr);
 
-	nn_writel(nn, NFP_NET_CFG_MTU, nn->dp.netdev->mtu);
+	nn_writel(nn, NFP_NET_CFG_MTU, nn->dp.mtu);
 
 	bufsz = nn->dp.fl_bufsz - nn->dp.rx_dma_off - NFP_NET_RX_BUF_NON_DATA;
 	nn_writel(nn, NFP_NET_CFG_FLBUFSZ, bufsz);
@@ -2262,6 +2606,86 @@ static int nfp_net_set_config_and_enable(struct nfp_net *nn)
 }
 
 /**
+ * nfp_net_close_stack() - Quiesce the stack (part of close)
+ * @nn:	     NFP Net device to reconfigure
+ */
+static void nfp_net_close_stack(struct nfp_net *nn)
+{
+	unsigned int r;
+
+	disable_irq(nn->irq_entries[NFP_NET_IRQ_LSC_IDX].vector);
+	netif_carrier_off(nn->dp.netdev);
+	nn->link_up = false;
+
+	for (r = 0; r < nn->dp.num_r_vecs; r++) {
+		disable_irq(nn->r_vecs[r].irq_vector);
+		napi_disable(&nn->r_vecs[r].napi);
+	}
+
+	netif_tx_disable(nn->dp.netdev);
+}
+
+/**
+ * nfp_net_close_free_all() - Free all runtime resources
+ * @nn:      NFP Net device to reconfigure
+ */
+static void nfp_net_close_free_all(struct nfp_net *nn)
+{
+	unsigned int r;
+
+	nfp_net_tx_rings_free(&nn->dp);
+	nfp_net_rx_rings_free(&nn->dp);
+
+	for (r = 0; r < nn->dp.num_r_vecs; r++)
+		nfp_net_cleanup_vector(nn, &nn->r_vecs[r]);
+
+	nfp_net_aux_irq_free(nn, NFP_NET_CFG_LSC, NFP_NET_IRQ_LSC_IDX);
+	nfp_net_aux_irq_free(nn, NFP_NET_CFG_EXN, NFP_NET_IRQ_EXN_IDX);
+}
+
+/**
+ * nfp_net_netdev_close() - Called when the device is downed
+ * @netdev:      netdev structure
+ */
+static int nfp_net_netdev_close(struct net_device *netdev)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+
+	/* Step 1: Disable RX and TX rings from the Linux kernel perspective
+	 */
+	nfp_net_close_stack(nn);
+
+	/* Step 2: Tell NFP
+	 */
+	nfp_net_clear_config_and_disable(nn);
+
+	/* Step 3: Free resources
+	 */
+	nfp_net_close_free_all(nn);
+
+	nn_dbg(nn, "%s down", netdev->name);
+	return 0;
+}
+
+void nfp_ctrl_close(struct nfp_net *nn)
+{
+	int r;
+
+	rtnl_lock();
+
+	for (r = 0; r < nn->dp.num_r_vecs; r++) {
+		disable_irq(nn->r_vecs[r].irq_vector);
+		tasklet_disable(&nn->r_vecs[r].tasklet);
+	}
+
+	nfp_net_clear_config_and_disable(nn);
+
+	nfp_net_close_free_all(nn);
+
+	rtnl_unlock();
+}
+
+/**
  * nfp_net_open_stack() - Start the device from stack's perspective
  * @nn:      NFP Net device to reconfigure
  */
@@ -2280,16 +2704,10 @@ static void nfp_net_open_stack(struct nfp_net *nn)
 	nfp_net_read_link_status(nn);
 }
 
-static int nfp_net_netdev_open(struct net_device *netdev)
+static int nfp_net_open_alloc_all(struct nfp_net *nn)
 {
-	struct nfp_net *nn = netdev_priv(netdev);
 	int err, r;
 
-	/* Step 1: Allocate resources for rings and the like
-	 * - Request interrupts
-	 * - Allocate RX and TX ring resources
-	 * - Setup initial RSS table
-	 */
 	err = nfp_net_aux_irq_request(nn, NFP_NET_CFG_EXN, "%s-exn",
 				      nn->exn_name, sizeof(nn->exn_name),
 				      NFP_NET_IRQ_EXN_IDX, nn->exn_handler);
@@ -2319,13 +2737,42 @@ static int nfp_net_netdev_open(struct net_device *netdev)
 	for (r = 0; r < nn->max_r_vecs; r++)
 		nfp_net_vector_assign_rings(&nn->dp, &nn->r_vecs[r], r);
 
+	return 0;
+
+err_free_rx_rings:
+	nfp_net_rx_rings_free(&nn->dp);
+err_cleanup_vec:
+	r = nn->dp.num_r_vecs;
+err_cleanup_vec_p:
+	while (r--)
+		nfp_net_cleanup_vector(nn, &nn->r_vecs[r]);
+	nfp_net_aux_irq_free(nn, NFP_NET_CFG_LSC, NFP_NET_IRQ_LSC_IDX);
+err_free_exn:
+	nfp_net_aux_irq_free(nn, NFP_NET_CFG_EXN, NFP_NET_IRQ_EXN_IDX);
+	return err;
+}
+
+static int nfp_net_netdev_open(struct net_device *netdev)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	int err;
+
+	/* Step 1: Allocate resources for rings and the like
+	 * - Request interrupts
+	 * - Allocate RX and TX ring resources
+	 * - Setup initial RSS table
+	 */
+	err = nfp_net_open_alloc_all(nn);
+	if (err)
+		return err;
+
 	err = netif_set_real_num_tx_queues(netdev, nn->dp.num_stack_tx_rings);
 	if (err)
-		goto err_free_rings;
+		goto err_free_all;
 
 	err = netif_set_real_num_rx_queues(netdev, nn->dp.num_rx_rings);
 	if (err)
-		goto err_free_rings;
+		goto err_free_all;
 
 	/* Step 2: Configure the NFP
 	 * - Enable rings from 0 to tx_rings/rx_rings - 1.
@@ -2336,7 +2783,7 @@ static int nfp_net_netdev_open(struct net_device *netdev)
 	 */
 	err = nfp_net_set_config_and_enable(nn);
 	if (err)
-		goto err_free_rings;
+		goto err_free_all;
 
 	/* Step 3: Enable for kernel
 	 * - put some freelist descriptors on each RX ring
@@ -2348,89 +2795,38 @@ static int nfp_net_netdev_open(struct net_device *netdev)
 
 	return 0;
 
-err_free_rings:
-	nfp_net_tx_rings_free(&nn->dp);
-err_free_rx_rings:
-	nfp_net_rx_rings_free(&nn->dp);
-err_cleanup_vec:
-	r = nn->dp.num_r_vecs;
-err_cleanup_vec_p:
-	while (r--)
-		nfp_net_cleanup_vector(nn, &nn->r_vecs[r]);
-	nfp_net_aux_irq_free(nn, NFP_NET_CFG_LSC, NFP_NET_IRQ_LSC_IDX);
-err_free_exn:
-	nfp_net_aux_irq_free(nn, NFP_NET_CFG_EXN, NFP_NET_IRQ_EXN_IDX);
+err_free_all:
+	nfp_net_close_free_all(nn);
 	return err;
 }
 
-/**
- * nfp_net_close_stack() - Quiescent the stack (part of close)
- * @nn:	     NFP Net device to reconfigure
- */
-static void nfp_net_close_stack(struct nfp_net *nn)
+int nfp_ctrl_open(struct nfp_net *nn)
 {
-	unsigned int r;
-
-	disable_irq(nn->irq_entries[NFP_NET_IRQ_LSC_IDX].vector);
-	netif_carrier_off(nn->dp.netdev);
-	nn->link_up = false;
+	int err, r;
 
-	for (r = 0; r < nn->dp.num_r_vecs; r++) {
-		disable_irq(nn->r_vecs[r].irq_vector);
-		napi_disable(&nn->r_vecs[r].napi);
-	}
+	/* ring dumping depends on vNICs being opened/closed under rtnl */
+	rtnl_lock();
 
-	netif_tx_disable(nn->dp.netdev);
-}
+	err = nfp_net_open_alloc_all(nn);
+	if (err)
+		goto err_unlock;
 
-/**
- * nfp_net_close_free_all() - Free all runtime resources
- * @nn:      NFP Net device to reconfigure
- */
-static void nfp_net_close_free_all(struct nfp_net *nn)
-{
-	unsigned int r;
+	err = nfp_net_set_config_and_enable(nn);
+	if (err)
+		goto err_free_all;
 
-	for (r = 0; r < nn->dp.num_rx_rings; r++) {
-		nfp_net_rx_ring_bufs_free(&nn->dp, &nn->dp.rx_rings[r]);
-		nfp_net_rx_ring_free(&nn->dp.rx_rings[r]);
-	}
-	for (r = 0; r < nn->dp.num_tx_rings; r++) {
-		nfp_net_tx_ring_bufs_free(&nn->dp, &nn->dp.tx_rings[r]);
-		nfp_net_tx_ring_free(&nn->dp.tx_rings[r]);
-	}
 	for (r = 0; r < nn->dp.num_r_vecs; r++)
-		nfp_net_cleanup_vector(nn, &nn->r_vecs[r]);
-
-	kfree(nn->dp.rx_rings);
-	kfree(nn->dp.tx_rings);
-
-	nfp_net_aux_irq_free(nn, NFP_NET_CFG_LSC, NFP_NET_IRQ_LSC_IDX);
-	nfp_net_aux_irq_free(nn, NFP_NET_CFG_EXN, NFP_NET_IRQ_EXN_IDX);
-}
-
-/**
- * nfp_net_netdev_close() - Called when the device is downed
- * @netdev:      netdev structure
- */
-static int nfp_net_netdev_close(struct net_device *netdev)
-{
-	struct nfp_net *nn = netdev_priv(netdev);
+		enable_irq(nn->r_vecs[r].irq_vector);
 
-	/* Step 1: Disable RX and TX rings from the Linux kernel perspective
-	 */
-	nfp_net_close_stack(nn);
+	rtnl_unlock();
 
-	/* Step 2: Tell NFP
-	 */
-	nfp_net_clear_config_and_disable(nn);
+	return 0;
 
-	/* Step 3: Free resources
-	 */
+err_free_all:
 	nfp_net_close_free_all(nn);
-
-	nn_dbg(nn, "%s down", netdev->name);
-	return 0;
+err_unlock:
+	rtnl_unlock();
+	return err;
 }
 
 static void nfp_net_set_rx_mode(struct net_device *netdev)
@@ -2634,6 +3030,40 @@ static int nfp_net_change_mtu(struct net_device *netdev, int new_mtu)
 	return nfp_net_ring_reconfig(nn, dp, NULL);
 }
 
+static int
+nfp_net_vlan_rx_add_vid(struct net_device *netdev, __be16 proto, u16 vid)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+
+	/* Priority tagged packets with vlan id 0 are processed by the
+	 * NFP as untagged packets
+	 */
+	if (!vid)
+		return 0;
+
+	nn_writew(nn, NFP_NET_CFG_VLAN_FILTER_VID, vid);
+	nn_writew(nn, NFP_NET_CFG_VLAN_FILTER_PROTO, ETH_P_8021Q);
+
+	return nfp_net_reconfig_mbox(nn, NFP_NET_CFG_MBOX_CMD_CTAG_FILTER_ADD);
+}
+
+static int
+nfp_net_vlan_rx_kill_vid(struct net_device *netdev, __be16 proto, u16 vid)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+
+	/* Priority tagged packets with vlan id 0 are processed by the
+	 * NFP as untagged packets
+	 */
+	if (!vid)
+		return 0;
+
+	nn_writew(nn, NFP_NET_CFG_VLAN_FILTER_VID, vid);
+	nn_writew(nn, NFP_NET_CFG_VLAN_FILTER_PROTO, ETH_P_8021Q);
+
+	return nfp_net_reconfig_mbox(nn, NFP_NET_CFG_MBOX_CMD_CTAG_FILTER_KILL);
+}
+
 static void nfp_net_stat64(struct net_device *netdev,
 			   struct rtnl_link_stats64 *stats)
 {
@@ -2667,35 +3097,6 @@ static void nfp_net_stat64(struct net_device *netdev,
 	}
 }
 
-static bool nfp_net_ebpf_capable(struct nfp_net *nn)
-{
-	if (nn->cap & NFP_NET_CFG_CTRL_BPF &&
-	    nn_readb(nn, NFP_NET_CFG_BPF_ABI) == NFP_NET_BPF_ABI)
-		return true;
-	return false;
-}
-
-static int
-nfp_net_setup_tc(struct net_device *netdev, u32 handle, __be16 proto,
-		 struct tc_to_netdev *tc)
-{
-	struct nfp_net *nn = netdev_priv(netdev);
-
-	if (TC_H_MAJ(handle) != TC_H_MAJ(TC_H_INGRESS))
-		return -EOPNOTSUPP;
-	if (proto != htons(ETH_P_ALL))
-		return -EOPNOTSUPP;
-
-	if (tc->type == TC_SETUP_CLSBPF && nfp_net_ebpf_capable(nn)) {
-		if (!nn->dp.bpf_offload_xdp)
-			return nfp_net_bpf_offload(nn, tc->cls_bpf);
-		else
-			return -EBUSY;
-	}
-
-	return -EINVAL;
-}
-
 static int nfp_net_set_features(struct net_device *netdev,
 				netdev_features_t features)
 {
@@ -2710,9 +3111,9 @@ static int nfp_net_set_features(struct net_device *netdev,
 
 	if (changed & NETIF_F_RXCSUM) {
 		if (features & NETIF_F_RXCSUM)
-			new_ctrl |= NFP_NET_CFG_CTRL_RXCSUM;
+			new_ctrl |= nn->cap & NFP_NET_CFG_CTRL_RXCSUM_ANY;
 		else
-			new_ctrl &= ~NFP_NET_CFG_CTRL_RXCSUM;
+			new_ctrl &= ~NFP_NET_CFG_CTRL_RXCSUM_ANY;
 	}
 
 	if (changed & (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM)) {
@@ -2724,9 +3125,10 @@ static int nfp_net_set_features(struct net_device *netdev,
 
 	if (changed & (NETIF_F_TSO | NETIF_F_TSO6)) {
 		if (features & (NETIF_F_TSO | NETIF_F_TSO6))
-			new_ctrl |= NFP_NET_CFG_CTRL_LSO;
+			new_ctrl |= nn->cap & NFP_NET_CFG_CTRL_LSO2 ?:
+					      NFP_NET_CFG_CTRL_LSO;
 		else
-			new_ctrl &= ~NFP_NET_CFG_CTRL_LSO;
+			new_ctrl &= ~NFP_NET_CFG_CTRL_LSO_ANY;
 	}
 
 	if (changed & NETIF_F_HW_VLAN_CTAG_RX) {
@@ -2743,6 +3145,13 @@ static int nfp_net_set_features(struct net_device *netdev,
 			new_ctrl &= ~NFP_NET_CFG_CTRL_TXVLAN;
 	}
 
+	if (changed & NETIF_F_HW_VLAN_CTAG_FILTER) {
+		if (features & NETIF_F_HW_VLAN_CTAG_FILTER)
+			new_ctrl |= NFP_NET_CFG_CTRL_CTAG_FILTER;
+		else
+			new_ctrl &= ~NFP_NET_CFG_CTRL_CTAG_FILTER;
+	}
+
 	if (changed & NETIF_F_SG) {
 		if (features & NETIF_F_SG)
 			new_ctrl |= NFP_NET_CFG_CTRL_GATHER;
@@ -2750,7 +3159,7 @@ static int nfp_net_set_features(struct net_device *netdev,
 			new_ctrl &= ~NFP_NET_CFG_CTRL_GATHER;
 	}
 
-	if (changed & NETIF_F_HW_TC && nn->dp.ctrl & NFP_NET_CFG_CTRL_BPF) {
+	if (changed & NETIF_F_HW_TC && nfp_app_tc_busy(nn->app, nn)) {
 		nn_err(nn, "Cannot disable HW TC offload while in use\n");
 		return -EBUSY;
 	}
@@ -2818,26 +3227,6 @@ nfp_net_features_check(struct sk_buff *skb, struct net_device *dev,
 	return features;
 }
 
-static int
-nfp_net_get_phys_port_name(struct net_device *netdev, char *name, size_t len)
-{
-	struct nfp_net *nn = netdev_priv(netdev);
-	int err;
-
-	if (!nn->eth_port)
-		return -EOPNOTSUPP;
-
-	if (!nn->eth_port->is_split)
-		err = snprintf(name, len, "p%d", nn->eth_port->label_port);
-	else
-		err = snprintf(name, len, "p%ds%d", nn->eth_port->label_port,
-			       nn->eth_port->label_subport);
-	if (err >= len)
-		return -EINVAL;
-
-	return 0;
-}
-
 /**
  * nfp_net_set_vxlan_port() - set vxlan port in SW and reconfigure HW
  * @nn:   NFP Net device to reconfigure
@@ -2919,47 +3308,14 @@ static void nfp_net_del_vxlan_port(struct net_device *netdev,
 		nfp_net_set_vxlan_port(nn, idx, 0);
 }
 
-static int nfp_net_xdp_offload(struct nfp_net *nn, struct bpf_prog *prog)
-{
-	struct tc_cls_bpf_offload cmd = {
-		.prog = prog,
-	};
-	int ret;
-
-	if (!nfp_net_ebpf_capable(nn))
-		return -EINVAL;
-
-	if (nn->dp.ctrl & NFP_NET_CFG_CTRL_BPF) {
-		if (!nn->dp.bpf_offload_xdp)
-			return prog ? -EBUSY : 0;
-		cmd.command = prog ? TC_CLSBPF_REPLACE : TC_CLSBPF_DESTROY;
-	} else {
-		if (!prog)
-			return 0;
-		cmd.command = TC_CLSBPF_ADD;
-	}
-
-	ret = nfp_net_bpf_offload(nn, &cmd);
-	/* Stop offload if replace not possible */
-	if (ret && cmd.command == TC_CLSBPF_REPLACE)
-		nfp_net_xdp_offload(nn, NULL);
-	nn->dp.bpf_offload_xdp = prog && !ret;
-	return ret;
-}
-
-static int nfp_net_xdp_setup(struct nfp_net *nn, struct netdev_xdp *xdp)
+static int
+nfp_net_xdp_setup_drv(struct nfp_net *nn, struct bpf_prog *prog,
+		      struct netlink_ext_ack *extack)
 {
-	struct bpf_prog *old_prog = nn->dp.xdp_prog;
-	struct bpf_prog *prog = xdp->prog;
 	struct nfp_net_dp *dp;
-	int err;
 
-	if (!prog && !nn->dp.xdp_prog)
-		return 0;
-	if (prog && nn->dp.xdp_prog) {
-		prog = xchg(&nn->dp.xdp_prog, prog);
-		bpf_prog_put(prog);
-		nfp_net_xdp_offload(nn, nn->dp.xdp_prog);
+	if (!prog == !nn->dp.xdp_prog) {
+		WRITE_ONCE(nn->dp.xdp_prog, prog);
 		return 0;
 	}
 
@@ -2973,14 +3329,37 @@ static int nfp_net_xdp_setup(struct nfp_net *nn, struct netdev_xdp *xdp)
 	dp->rx_dma_off = prog ? XDP_PACKET_HEADROOM - nn->dp.rx_offset : 0;
 
 	/* We need RX reconfig to remap the buffers (BIDIR vs FROM_DEV) */
-	err = nfp_net_ring_reconfig(nn, dp, xdp->extack);
+	return nfp_net_ring_reconfig(nn, dp, extack);
+}
+
+static int
+nfp_net_xdp_setup(struct nfp_net *nn, struct bpf_prog *prog, u32 flags,
+		  struct netlink_ext_ack *extack)
+{
+	struct bpf_prog *drv_prog, *offload_prog;
+	int err;
+
+	if (nn->xdp_prog && (flags ^ nn->xdp_flags) & XDP_FLAGS_MODES)
+		return -EBUSY;
+
+	/* Load both when no flags set to allow easy activation of driver path
+	 * when program is replaced by one which can't be offloaded.
+	 */
+	drv_prog     = flags & XDP_FLAGS_HW_MODE  ? NULL : prog;
+	offload_prog = flags & XDP_FLAGS_DRV_MODE ? NULL : prog;
+
+	err = nfp_net_xdp_setup_drv(nn, drv_prog, extack);
 	if (err)
 		return err;
 
-	if (old_prog)
-		bpf_prog_put(old_prog);
+	err = nfp_app_xdp_offload(nn->app, nn, offload_prog);
+	if (err && flags & XDP_FLAGS_HW_MODE)
+		return err;
 
-	nfp_net_xdp_offload(nn, nn->dp.xdp_prog);
+	if (nn->xdp_prog)
+		bpf_prog_put(nn->xdp_prog);
+	nn->xdp_prog = prog;
+	nn->xdp_flags = flags;
 
 	return 0;
 }
@@ -2991,28 +3370,56 @@ static int nfp_net_xdp(struct net_device *netdev, struct netdev_xdp *xdp)
 
 	switch (xdp->command) {
 	case XDP_SETUP_PROG:
-		return nfp_net_xdp_setup(nn, xdp);
+	case XDP_SETUP_PROG_HW:
+		return nfp_net_xdp_setup(nn, xdp->prog, xdp->flags,
+					 xdp->extack);
 	case XDP_QUERY_PROG:
-		xdp->prog_attached = !!nn->dp.xdp_prog;
+		xdp->prog_attached = !!nn->xdp_prog;
+		if (nn->dp.bpf_offload_xdp)
+			xdp->prog_attached = XDP_ATTACHED_HW;
+		xdp->prog_id = nn->xdp_prog ? nn->xdp_prog->aux->id : 0;
 		return 0;
 	default:
 		return -EINVAL;
 	}
 }
 
-static const struct net_device_ops nfp_net_netdev_ops = {
+static int nfp_net_set_mac_address(struct net_device *netdev, void *addr)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	struct sockaddr *saddr = addr;
+	int err;
+
+	err = eth_prepare_mac_addr_change(netdev, addr);
+	if (err)
+		return err;
+
+	nfp_net_write_mac_addr(nn, saddr->sa_data);
+
+	err = nfp_net_reconfig(nn, NFP_NET_CFG_UPDATE_MACADDR);
+	if (err)
+		return err;
+
+	eth_commit_mac_addr_change(netdev, addr);
+
+	return 0;
+}
+
+const struct net_device_ops nfp_net_netdev_ops = {
 	.ndo_open		= nfp_net_netdev_open,
 	.ndo_stop		= nfp_net_netdev_close,
 	.ndo_start_xmit		= nfp_net_tx,
 	.ndo_get_stats64	= nfp_net_stat64,
-	.ndo_setup_tc		= nfp_net_setup_tc,
+	.ndo_vlan_rx_add_vid	= nfp_net_vlan_rx_add_vid,
+	.ndo_vlan_rx_kill_vid	= nfp_net_vlan_rx_kill_vid,
+	.ndo_setup_tc		= nfp_port_setup_tc,
 	.ndo_tx_timeout		= nfp_net_tx_timeout,
 	.ndo_set_rx_mode	= nfp_net_set_rx_mode,
 	.ndo_change_mtu		= nfp_net_change_mtu,
-	.ndo_set_mac_address	= eth_mac_addr,
+	.ndo_set_mac_address	= nfp_net_set_mac_address,
 	.ndo_set_features	= nfp_net_set_features,
 	.ndo_features_check	= nfp_net_features_check,
-	.ndo_get_phys_port_name	= nfp_net_get_phys_port_name,
+	.ndo_get_phys_port_name	= nfp_port_get_phys_port_name,
 	.ndo_udp_tunnel_add	= nfp_net_add_vxlan_port,
 	.ndo_udp_tunnel_del	= nfp_net_del_vxlan_port,
 	.ndo_xdp		= nfp_net_xdp,
@@ -3032,7 +3439,7 @@ void nfp_net_info(struct nfp_net *nn)
 		nn->fw_ver.resv, nn->fw_ver.class,
 		nn->fw_ver.major, nn->fw_ver.minor,
 		nn->max_mtu);
-	nn_info(nn, "CAP: %#x %s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
+	nn_info(nn, "CAP: %#x %s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
 		nn->cap,
 		nn->cap & NFP_NET_CFG_CTRL_PROMISC  ? "PROMISC "  : "",
 		nn->cap & NFP_NET_CFG_CTRL_L2BC     ? "L2BCFILT " : "",
@@ -3043,43 +3450,58 @@ void nfp_net_info(struct nfp_net *nn)
 		nn->cap & NFP_NET_CFG_CTRL_TXVLAN   ? "TXVLAN "   : "",
 		nn->cap & NFP_NET_CFG_CTRL_SCATTER  ? "SCATTER "  : "",
 		nn->cap & NFP_NET_CFG_CTRL_GATHER   ? "GATHER "   : "",
-		nn->cap & NFP_NET_CFG_CTRL_LSO      ? "TSO "      : "",
-		nn->cap & NFP_NET_CFG_CTRL_RSS      ? "RSS "      : "",
+		nn->cap & NFP_NET_CFG_CTRL_LSO      ? "TSO1 "     : "",
+		nn->cap & NFP_NET_CFG_CTRL_LSO2     ? "TSO2 "     : "",
+		nn->cap & NFP_NET_CFG_CTRL_RSS      ? "RSS1 "     : "",
+		nn->cap & NFP_NET_CFG_CTRL_RSS2     ? "RSS2 "     : "",
+		nn->cap & NFP_NET_CFG_CTRL_CTAG_FILTER ? "CTAG_FILTER " : "",
 		nn->cap & NFP_NET_CFG_CTRL_L2SWITCH ? "L2SWITCH " : "",
 		nn->cap & NFP_NET_CFG_CTRL_MSIXAUTO ? "AUTOMASK " : "",
 		nn->cap & NFP_NET_CFG_CTRL_IRQMOD   ? "IRQMOD "   : "",
 		nn->cap & NFP_NET_CFG_CTRL_VXLAN    ? "VXLAN "    : "",
 		nn->cap & NFP_NET_CFG_CTRL_NVGRE    ? "NVGRE "	  : "",
-		nfp_net_ebpf_capable(nn)            ? "BPF "	  : "");
+		nn->cap & NFP_NET_CFG_CTRL_CSUM_COMPLETE ?
+						      "RXCSUM_COMPLETE " : "",
+		nn->cap & NFP_NET_CFG_CTRL_LIVE_ADDR ? "LIVE_ADDR " : "",
+		nfp_app_extra_cap(nn->app, nn));
 }
 
 /**
- * nfp_net_netdev_alloc() - Allocate netdev and related structure
+ * nfp_net_alloc() - Allocate netdev and related structure
  * @pdev:         PCI device
+ * @needs_netdev: Whether to allocate a netdev for this vNIC
  * @max_tx_rings: Maximum number of TX rings supported by device
  * @max_rx_rings: Maximum number of RX rings supported by device
  *
  * This function allocates a netdev device and fills in the initial
- * part of the @struct nfp_net structure.
+ * part of the @struct nfp_net structure.  In case of control device
+ * nfp_net structure is allocated without the netdev.
  *
  * Return: NFP Net device structure, or ERR_PTR on error.
  */
-struct nfp_net *nfp_net_netdev_alloc(struct pci_dev *pdev,
-				     unsigned int max_tx_rings,
-				     unsigned int max_rx_rings)
+struct nfp_net *nfp_net_alloc(struct pci_dev *pdev, bool needs_netdev,
+			      unsigned int max_tx_rings,
+			      unsigned int max_rx_rings)
 {
-	struct net_device *netdev;
 	struct nfp_net *nn;
 
-	netdev = alloc_etherdev_mqs(sizeof(struct nfp_net),
-				    max_tx_rings, max_rx_rings);
-	if (!netdev)
-		return ERR_PTR(-ENOMEM);
+	if (needs_netdev) {
+		struct net_device *netdev;
 
-	SET_NETDEV_DEV(netdev, &pdev->dev);
-	nn = netdev_priv(netdev);
+		netdev = alloc_etherdev_mqs(sizeof(struct nfp_net),
+					    max_tx_rings, max_rx_rings);
+		if (!netdev)
+			return ERR_PTR(-ENOMEM);
+
+		SET_NETDEV_DEV(netdev, &pdev->dev);
+		nn = netdev_priv(netdev);
+		nn->dp.netdev = netdev;
+	} else {
+		nn = vzalloc(sizeof(*nn));
+		if (!nn)
+			return ERR_PTR(-ENOMEM);
+	}
 
-	nn->dp.netdev = netdev;
 	nn->dp.dev = &pdev->dev;
 	nn->pdev = pdev;
 
@@ -3099,24 +3521,27 @@ struct nfp_net *nfp_net_netdev_alloc(struct pci_dev *pdev,
 	nn->dp.rxd_cnt = NFP_NET_RX_DESCS_DEFAULT;
 
 	spin_lock_init(&nn->reconfig_lock);
-	spin_lock_init(&nn->rx_filter_lock);
 	spin_lock_init(&nn->link_status_lock);
 
 	setup_timer(&nn->reconfig_timer,
 		    nfp_net_reconfig_timer, (unsigned long)nn);
-	setup_timer(&nn->rx_filter_stats_timer,
-		    nfp_net_filter_stats_timer, (unsigned long)nn);
 
 	return nn;
 }
 
 /**
- * nfp_net_netdev_free() - Undo what @nfp_net_netdev_alloc() did
+ * nfp_net_free() - Undo what @nfp_net_alloc() did
  * @nn:      NFP Net device to reconfigure
  */
-void nfp_net_netdev_free(struct nfp_net *nn)
+void nfp_net_free(struct nfp_net *nn)
 {
-	free_netdev(nn->dp.netdev);
+	if (nn->xdp_prog)
+		bpf_prog_put(nn->xdp_prog);
+
+	if (nn->dp.netdev)
+		free_netdev(nn->dp.netdev);
+	else
+		vfree(nn);
 }
 
 /**
@@ -3187,48 +3612,13 @@ static void nfp_net_irqmod_init(struct nfp_net *nn)
 	nn->tx_coalesce_max_frames = 64;
 }
 
-/**
- * nfp_net_netdev_init() - Initialise/finalise the netdev structure
- * @netdev:      netdev structure
- *
- * Return: 0 on success or negative errno on error.
- */
-int nfp_net_netdev_init(struct net_device *netdev)
+static void nfp_net_netdev_init(struct nfp_net *nn)
 {
-	struct nfp_net *nn = netdev_priv(netdev);
-	int err;
-
-	nn->dp.chained_metadata_format = nn->fw_ver.major > 3;
-
-	nn->dp.rx_dma_dir = DMA_FROM_DEVICE;
-
-	/* Get some of the read-only fields from the BAR */
-	nn->cap = nn_readl(nn, NFP_NET_CFG_CAP);
-	nn->max_mtu = nn_readl(nn, NFP_NET_CFG_MAX_MTU);
+	struct net_device *netdev = nn->dp.netdev;
 
-	nfp_net_write_mac_addr(nn);
+	nfp_net_write_mac_addr(nn, nn->dp.netdev->dev_addr);
 
-	/* Determine RX packet/metadata boundary offset */
-	if (nn->fw_ver.major >= 2) {
-		u32 reg;
-
-		reg = nn_readl(nn, NFP_NET_CFG_RX_OFFSET);
-		if (reg > NFP_NET_MAX_PREPEND) {
-			nn_err(nn, "Invalid rx offset: %d\n", reg);
-			return -EINVAL;
-		}
-		nn->dp.rx_offset = reg;
-	} else {
-		nn->dp.rx_offset = NFP_NET_RX_OFFSET;
-	}
-
-	/* Set default MTU and Freelist buffer size */
-	if (nn->max_mtu < NFP_NET_DEFAULT_MTU)
-		netdev->mtu = nn->max_mtu;
-	else
-		netdev->mtu = NFP_NET_DEFAULT_MTU;
-	nn->dp.mtu = netdev->mtu;
-	nn->dp.fl_bufsz = nfp_net_calc_fl_bufsz(&nn->dp);
+	netdev->mtu = nn->dp.mtu;
 
 	/* Advertise/enable offloads based on capabilities
 	 *
@@ -3236,10 +3626,13 @@ int nfp_net_netdev_init(struct net_device *netdev)
 	 * and netdev->hw_features advertises which features are
 	 * supported.  By default we enable most features.
 	 */
+	if (nn->cap & NFP_NET_CFG_CTRL_LIVE_ADDR)
+		netdev->priv_flags |= IFF_LIVE_ADDR_CHANGE;
+
 	netdev->hw_features = NETIF_F_HIGHDMA;
-	if (nn->cap & NFP_NET_CFG_CTRL_RXCSUM) {
+	if (nn->cap & NFP_NET_CFG_CTRL_RXCSUM_ANY) {
 		netdev->hw_features |= NETIF_F_RXCSUM;
-		nn->dp.ctrl |= NFP_NET_CFG_CTRL_RXCSUM;
+		nn->dp.ctrl |= nn->cap & NFP_NET_CFG_CTRL_RXCSUM_ANY;
 	}
 	if (nn->cap & NFP_NET_CFG_CTRL_TXCSUM) {
 		netdev->hw_features |= NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM;
@@ -3249,15 +3642,14 @@ int nfp_net_netdev_init(struct net_device *netdev)
 		netdev->hw_features |= NETIF_F_SG;
 		nn->dp.ctrl |= NFP_NET_CFG_CTRL_GATHER;
 	}
-	if ((nn->cap & NFP_NET_CFG_CTRL_LSO) && nn->fw_ver.major > 2) {
+	if ((nn->cap & NFP_NET_CFG_CTRL_LSO && nn->fw_ver.major > 2) ||
+	    nn->cap & NFP_NET_CFG_CTRL_LSO2) {
 		netdev->hw_features |= NETIF_F_TSO | NETIF_F_TSO6;
-		nn->dp.ctrl |= NFP_NET_CFG_CTRL_LSO;
+		nn->dp.ctrl |= nn->cap & NFP_NET_CFG_CTRL_LSO2 ?:
+					 NFP_NET_CFG_CTRL_LSO;
 	}
-	if (nn->cap & NFP_NET_CFG_CTRL_RSS) {
+	if (nn->cap & NFP_NET_CFG_CTRL_RSS_ANY)
 		netdev->hw_features |= NETIF_F_RXHASH;
-		nfp_net_rss_init(nn);
-		nn->dp.ctrl |= NFP_NET_CFG_CTRL_RSS;
-	}
 	if (nn->cap & NFP_NET_CFG_CTRL_VXLAN &&
 	    nn->cap & NFP_NET_CFG_CTRL_NVGRE) {
 		if (nn->cap & NFP_NET_CFG_CTRL_LSO)
@@ -3275,17 +3667,98 @@ int nfp_net_netdev_init(struct net_device *netdev)
 		nn->dp.ctrl |= NFP_NET_CFG_CTRL_RXVLAN;
 	}
 	if (nn->cap & NFP_NET_CFG_CTRL_TXVLAN) {
-		netdev->hw_features |= NETIF_F_HW_VLAN_CTAG_TX;
-		nn->dp.ctrl |= NFP_NET_CFG_CTRL_TXVLAN;
+		if (nn->cap & NFP_NET_CFG_CTRL_LSO2) {
+			nn_warn(nn, "Device advertises both TSO2 and TXVLAN. Refusing to enable TXVLAN.\n");
+		} else {
+			netdev->hw_features |= NETIF_F_HW_VLAN_CTAG_TX;
+			nn->dp.ctrl |= NFP_NET_CFG_CTRL_TXVLAN;
+		}
+	}
+	if (nn->cap & NFP_NET_CFG_CTRL_CTAG_FILTER) {
+		netdev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER;
+		nn->dp.ctrl |= NFP_NET_CFG_CTRL_CTAG_FILTER;
 	}
 
 	netdev->features = netdev->hw_features;
 
-	if (nfp_net_ebpf_capable(nn))
+	if (nfp_app_has_tc(nn->app))
 		netdev->hw_features |= NETIF_F_HW_TC;
 
 	/* Advertise but disable TSO by default. */
 	netdev->features &= ~(NETIF_F_TSO | NETIF_F_TSO6);
+	nn->dp.ctrl &= ~NFP_NET_CFG_CTRL_LSO_ANY;
+
+	/* Finalise the netdev setup */
+	netdev->netdev_ops = &nfp_net_netdev_ops;
+	netdev->watchdog_timeo = msecs_to_jiffies(5 * 1000);
+
+	SWITCHDEV_SET_OPS(netdev, &nfp_port_switchdev_ops);
+
+	/* MTU range: 68 - hw-specific max */
+	netdev->min_mtu = ETH_MIN_MTU;
+	netdev->max_mtu = nn->max_mtu;
+
+	netif_carrier_off(netdev);
+
+	nfp_net_set_ethtool_ops(netdev);
+}
+
+/**
+ * nfp_net_init() - Initialise/finalise the nfp_net structure
+ * @nn:		NFP Net device structure
+ *
+ * Return: 0 on success or negative errno on error.
+ */
+int nfp_net_init(struct nfp_net *nn)
+{
+	int err;
+
+	nn->dp.rx_dma_dir = DMA_FROM_DEVICE;
+
+	/* Get some of the read-only fields from the BAR */
+	nn->cap = nn_readl(nn, NFP_NET_CFG_CAP);
+	nn->max_mtu = nn_readl(nn, NFP_NET_CFG_MAX_MTU);
+
+	/* ABI 4.x and ctrl vNIC always use chained metadata, in other cases
+	 * we allow use of non-chained metadata if RSS(v1) is the only
+	 * advertised capability requiring metadata.
+	 */
+	nn->dp.chained_metadata_format = nn->fw_ver.major == 4 ||
+					 !nn->dp.netdev ||
+					 !(nn->cap & NFP_NET_CFG_CTRL_RSS) ||
+					 nn->cap & NFP_NET_CFG_CTRL_CHAIN_META;
+	/* RSS(v1) uses non-chained metadata format, except in ABI 4.x where
+	 * it has the same meaning as RSSv2.
+	 */
+	if (nn->dp.chained_metadata_format && nn->fw_ver.major != 4)
+		nn->cap &= ~NFP_NET_CFG_CTRL_RSS;
+
+	/* Determine RX packet/metadata boundary offset */
+	if (nn->fw_ver.major >= 2) {
+		u32 reg;
+
+		reg = nn_readl(nn, NFP_NET_CFG_RX_OFFSET);
+		if (reg > NFP_NET_MAX_PREPEND) {
+			nn_err(nn, "Invalid rx offset: %d\n", reg);
+			return -EINVAL;
+		}
+		nn->dp.rx_offset = reg;
+	} else {
+		nn->dp.rx_offset = NFP_NET_RX_OFFSET;
+	}
+
+	/* Set default MTU and Freelist buffer size */
+	if (nn->max_mtu < NFP_NET_DEFAULT_MTU)
+		nn->dp.mtu = nn->max_mtu;
+	else
+		nn->dp.mtu = NFP_NET_DEFAULT_MTU;
+	nn->dp.fl_bufsz = nfp_net_calc_fl_bufsz(&nn->dp);
+
+	if (nn->cap & NFP_NET_CFG_CTRL_RSS_ANY) {
+		nfp_net_rss_init(nn);
+		nn->dp.ctrl |= nn->cap & NFP_NET_CFG_CTRL_RSS2 ?:
+					 NFP_NET_CFG_CTRL_RSS;
+	}
 
 	/* Allow L2 Broadcast and Multicast through by default, if supported */
 	if (nn->cap & NFP_NET_CFG_CTRL_L2BC)
@@ -3299,6 +3772,9 @@ int nfp_net_netdev_init(struct net_device *netdev)
 		nn->dp.ctrl |= NFP_NET_CFG_CTRL_IRQMOD;
 	}
 
+	if (nn->dp.netdev)
+		nfp_net_netdev_init(nn);
+
 	/* Stash the re-configuration queue away.  First odd queue in TX Bar */
 	nn->qcp_cfg = nn->tx_bar + NFP_QCP_QUEUE_ADDR_SZ;
 
@@ -3311,34 +3787,21 @@ int nfp_net_netdev_init(struct net_device *netdev)
 	if (err)
 		return err;
 
-	/* Finalise the netdev setup */
-	netdev->netdev_ops = &nfp_net_netdev_ops;
-	netdev->watchdog_timeo = msecs_to_jiffies(5 * 1000);
-
-	/* MTU range: 68 - hw-specific max */
-	netdev->min_mtu = ETH_MIN_MTU;
-	netdev->max_mtu = nn->max_mtu;
-
-	netif_carrier_off(netdev);
-
-	nfp_net_set_ethtool_ops(netdev);
-	nfp_net_vecs_init(netdev);
+	nfp_net_vecs_init(nn);
 
-	return register_netdev(netdev);
+	if (!nn->dp.netdev)
+		return 0;
+	return register_netdev(nn->dp.netdev);
 }
 
 /**
- * nfp_net_netdev_clean() - Undo what nfp_net_netdev_init() did.
- * @netdev:      netdev structure
+ * nfp_net_clean() - Undo what nfp_net_init() did.
+ * @nn:		NFP Net device structure
  */
-void nfp_net_netdev_clean(struct net_device *netdev)
+void nfp_net_clean(struct nfp_net *nn)
 {
-	struct nfp_net *nn = netdev_priv(netdev);
+	if (!nn->dp.netdev)
+		return;
 
 	unregister_netdev(nn->dp.netdev);
-
-	if (nn->dp.xdp_prog)
-		bpf_prog_put(nn->dp.xdp_prog);
-	if (nn->dp.bpf_offload_xdp)
-		nfp_net_xdp_offload(nn, NULL);
 }
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_ctrl.h b/drivers/net/ethernet/netronome/nfp/nfp_net_ctrl.h
index d04ccc9f6116..e5e94e0746ec 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net_ctrl.h
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_ctrl.h
@@ -71,6 +71,10 @@
 #define NFP_NET_META_FIELD_SIZE		4
 #define NFP_NET_META_HASH		1 /* next field carries hash type */
 #define NFP_NET_META_MARK		2
+#define NFP_NET_META_PORTID		5
+#define NFP_NET_META_CSUM		6 /* checksum complete type */
+
+#define	NFP_META_PORT_ID_CTRL		~0U
 
 /**
  * Hash type pre-pended when a RSS hash was computed
@@ -119,9 +123,10 @@
 #define   NFP_NET_CFG_CTRL_TXVLAN         (0x1 <<  7) /* Enable VLAN insert */
 #define   NFP_NET_CFG_CTRL_SCATTER        (0x1 <<  8) /* Scatter DMA */
 #define   NFP_NET_CFG_CTRL_GATHER         (0x1 <<  9) /* Gather DMA */
-#define   NFP_NET_CFG_CTRL_LSO            (0x1 << 10) /* LSO/TSO */
+#define   NFP_NET_CFG_CTRL_LSO            (0x1 << 10) /* LSO/TSO (version 1) */
+#define   NFP_NET_CFG_CTRL_CTAG_FILTER	  (0x1 << 11) /* VLAN CTAG filtering */
 #define   NFP_NET_CFG_CTRL_RINGCFG        (0x1 << 16) /* Ring runtime changes */
-#define   NFP_NET_CFG_CTRL_RSS            (0x1 << 17) /* RSS */
+#define   NFP_NET_CFG_CTRL_RSS		  (0x1 << 17) /* RSS (version 1) */
 #define   NFP_NET_CFG_CTRL_IRQMOD         (0x1 << 18) /* Interrupt moderation */
 #define   NFP_NET_CFG_CTRL_RINGPRIO       (0x1 << 19) /* Ring priorities */
 #define   NFP_NET_CFG_CTRL_MSIXAUTO       (0x1 << 20) /* MSI-X auto-masking */
@@ -131,6 +136,20 @@
 #define   NFP_NET_CFG_CTRL_VXLAN	  (0x1 << 24) /* VXLAN tunnel support */
 #define   NFP_NET_CFG_CTRL_NVGRE	  (0x1 << 25) /* NVGRE tunnel support */
 #define   NFP_NET_CFG_CTRL_BPF		  (0x1 << 27) /* BPF offload capable */
+#define   NFP_NET_CFG_CTRL_LSO2		  (0x1 << 28) /* LSO/TSO (version 2) */
+#define   NFP_NET_CFG_CTRL_RSS2		  (0x1 << 29) /* RSS (version 2) */
+#define   NFP_NET_CFG_CTRL_CSUM_COMPLETE  (0x1 << 30) /* Checksum complete */
+#define   NFP_NET_CFG_CTRL_LIVE_ADDR	  (0x1 << 31) /* live MAC addr change */
+
+#define NFP_NET_CFG_CTRL_LSO_ANY	(NFP_NET_CFG_CTRL_LSO | \
+					 NFP_NET_CFG_CTRL_LSO2)
+#define NFP_NET_CFG_CTRL_RSS_ANY	(NFP_NET_CFG_CTRL_RSS | \
+					 NFP_NET_CFG_CTRL_RSS2)
+#define NFP_NET_CFG_CTRL_RXCSUM_ANY	(NFP_NET_CFG_CTRL_RXCSUM | \
+					 NFP_NET_CFG_CTRL_CSUM_COMPLETE)
+#define NFP_NET_CFG_CTRL_CHAIN_META	(NFP_NET_CFG_CTRL_RSS2 | \
+					 NFP_NET_CFG_CTRL_CSUM_COMPLETE)
+
 #define NFP_NET_CFG_UPDATE              0x0004
 #define   NFP_NET_CFG_UPDATE_GEN          (0x1 <<  0) /* General update */
 #define   NFP_NET_CFG_UPDATE_RING         (0x1 <<  1) /* Ring config change */
@@ -143,6 +162,8 @@
 #define   NFP_NET_CFG_UPDATE_IRQMOD       (0x1 <<  8) /* IRQ mod change */
 #define   NFP_NET_CFG_UPDATE_VXLAN	  (0x1 <<  9) /* VXLAN port change */
 #define   NFP_NET_CFG_UPDATE_BPF	  (0x1 << 10) /* BPF program load */
+#define   NFP_NET_CFG_UPDATE_MACADDR	  (0x1 << 11) /* MAC address change */
+#define   NFP_NET_CFG_UPDATE_MBOX	  (0x1 << 12) /* Mailbox update */
 #define   NFP_NET_CFG_UPDATE_ERR          (0x1 << 31) /* A error occurred */
 #define NFP_NET_CFG_TXRS_ENABLE         0x0008
 #define NFP_NET_CFG_RXRS_ENABLE         0x0010
@@ -381,4 +402,29 @@
 #define NFP_NET_CFG_RXR_STATS(_x)       (NFP_NET_CFG_RXR_STATS_BASE + \
 					 ((_x) * 0x10))
 
+/**
+ * General use mailbox area (0x1800 - 0x19ff)
+ * 4B used for update command and 4B return code
+ * followed by a max of 504B of variable length value
+ */
+#define NFP_NET_CFG_MBOX_CMD		0x1800
+#define NFP_NET_CFG_MBOX_RET		0x1804
+#define NFP_NET_CFG_MBOX_VAL		0x1808
+#define NFP_NET_CFG_MBOX_VAL_MAX_SZ	0x1F8
+
+#define NFP_NET_CFG_MBOX_CMD_CTAG_FILTER_ADD 1
+#define NFP_NET_CFG_MBOX_CMD_CTAG_FILTER_KILL 2
+
+/**
+ * VLAN filtering using general use mailbox
+ * @NFP_NET_CFG_VLAN_FILTER:		Base address of VLAN filter mailbox
+ * @NFP_NET_CFG_VLAN_FILTER_VID:	VLAN ID to filter
+ * @NFP_NET_CFG_VLAN_FILTER_PROTO:	VLAN proto to filter
+ * @NFP_NET_CFG_VXLAN_SZ:		Size of the VLAN filter mailbox in bytes
+ */
+#define NFP_NET_CFG_VLAN_FILTER		NFP_NET_CFG_MBOX_VAL
+#define  NFP_NET_CFG_VLAN_FILTER_VID	NFP_NET_CFG_VLAN_FILTER
+#define  NFP_NET_CFG_VLAN_FILTER_PROTO	 (NFP_NET_CFG_VLAN_FILTER + 2)
+#define NFP_NET_CFG_VLAN_FILTER_SZ	 0x0004
+
 #endif /* _NFP_NET_CTRL_H_ */
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_debugfs.c b/drivers/net/ethernet/netronome/nfp/nfp_net_debugfs.c
index 4077c59bf782..40217ece5fcb 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net_debugfs.c
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_debugfs.c
@@ -54,7 +54,7 @@ static int nfp_net_debugfs_rx_q_read(struct seq_file *file, void *data)
 		goto out;
 	nn = r_vec->nfp_net;
 	rx_ring = r_vec->rx_ring;
-	if (!netif_running(nn->dp.netdev))
+	if (!nfp_net_running(nn))
 		goto out;
 
 	rxd_cnt = rx_ring->cnt;
@@ -62,7 +62,7 @@ static int nfp_net_debugfs_rx_q_read(struct seq_file *file, void *data)
 	fl_rd_p = nfp_qcp_rd_ptr_read(rx_ring->qcp_fl);
 	fl_wr_p = nfp_qcp_wr_ptr_read(rx_ring->qcp_fl);
 
-	seq_printf(file, "RX[%02d,%02d]: cnt=%d dma=%pad host=%p   H_RD=%d H_WR=%d FL_RD=%d FL_WR=%d\n",
+	seq_printf(file, "RX[%02d,%02d]: cnt=%u dma=%pad host=%p   H_RD=%u H_WR=%u FL_RD=%u FL_WR=%u\n",
 		   rx_ring->idx, rx_ring->fl_qcidx,
 		   rx_ring->cnt, &rx_ring->dma, rx_ring->rxds,
 		   rx_ring->rd_p, rx_ring->wr_p, fl_rd_p, fl_wr_p);
@@ -138,7 +138,7 @@ static int nfp_net_debugfs_tx_q_read(struct seq_file *file, void *data)
 	if (!r_vec->nfp_net || !tx_ring)
 		goto out;
 	nn = r_vec->nfp_net;
-	if (!netif_running(nn->dp.netdev))
+	if (!nfp_net_running(nn))
 		goto out;
 
 	txd_cnt = tx_ring->cnt;
@@ -146,7 +146,7 @@ static int nfp_net_debugfs_tx_q_read(struct seq_file *file, void *data)
 	d_rd_p = nfp_qcp_rd_ptr_read(tx_ring->qcp_q);
 	d_wr_p = nfp_qcp_wr_ptr_read(tx_ring->qcp_q);
 
-	seq_printf(file, "TX[%02d,%02d%s]: cnt=%d dma=%pad host=%p   H_RD=%d H_WR=%d D_RD=%d D_WR=%d\n",
+	seq_printf(file, "TX[%02d,%02d%s]: cnt=%u dma=%pad host=%p   H_RD=%u H_WR=%u D_RD=%u D_WR=%u\n",
 		   tx_ring->idx, tx_ring->qcidx,
 		   tx_ring == r_vec->tx_ring ? "" : "xdp",
 		   tx_ring->cnt, &tx_ring->dma, tx_ring->txds,
@@ -200,7 +200,7 @@ static const struct file_operations nfp_xdp_q_fops = {
 	.llseek = seq_lseek
 };
 
-void nfp_net_debugfs_port_add(struct nfp_net *nn, struct dentry *ddir, int id)
+void nfp_net_debugfs_vnic_add(struct nfp_net *nn, struct dentry *ddir, int id)
 {
 	struct dentry *queues, *tx, *rx, *xdp;
 	char name[20];
@@ -209,7 +209,10 @@ void nfp_net_debugfs_port_add(struct nfp_net *nn, struct dentry *ddir, int id)
 	if (IS_ERR_OR_NULL(nfp_dir))
 		return;
 
-	sprintf(name, "port%d", id);
+	if (nfp_net_is_data_vnic(nn))
+		sprintf(name, "vnic%d", id);
+	else
+		strcpy(name, "ctrl-vnic");
 	nn->debugfs_dir = debugfs_create_dir(name, ddir);
 	if (IS_ERR_OR_NULL(nn->debugfs_dir))
 		return;
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_ethtool.c b/drivers/net/ethernet/netronome/nfp/nfp_net_ethtool.c
index abbb47e60cc3..6e31355c3567 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net_ethtool.c
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_ethtool.c
@@ -50,8 +50,10 @@
 
 #include "nfpcore/nfp.h"
 #include "nfpcore/nfp_nsp.h"
+#include "nfp_app.h"
 #include "nfp_net_ctrl.h"
 #include "nfp_net.h"
+#include "nfp_port.h"
 
 enum nfp_dump_diag {
 	NFP_DUMP_NSP_DIAG = 0,
@@ -134,14 +136,14 @@ static const struct _nfp_net_et_stats nfp_net_et_stats[] = {
 #define NN_ET_STATS_LEN (NN_ET_GLOBAL_STATS_LEN + NN_ET_RVEC_GATHER_STATS + \
 			 NN_ET_RVEC_STATS_LEN + NN_ET_QUEUE_STATS_LEN)
 
-static void nfp_net_get_nspinfo(struct nfp_net *nn, char *version)
+static void nfp_net_get_nspinfo(struct nfp_app *app, char *version)
 {
 	struct nfp_nsp *nsp;
 
-	if (!nn->cpp)
+	if (!app)
 		return;
 
-	nsp = nfp_nsp_open(nn->cpp);
+	nsp = nfp_nsp_open(app->cpp);
 	if (IS_ERR(nsp))
 		return;
 
@@ -162,11 +164,12 @@ static void nfp_net_get_drvinfo(struct net_device *netdev,
 		sizeof(drvinfo->driver));
 	strlcpy(drvinfo->version, nfp_driver_version, sizeof(drvinfo->version));
 
-	nfp_net_get_nspinfo(nn, nsp_version);
+	nfp_net_get_nspinfo(nn->app, nsp_version);
 	snprintf(drvinfo->fw_version, sizeof(drvinfo->fw_version),
-		 "%d.%d.%d.%d %s",
+		 "%d.%d.%d.%d %s %s %s",
 		 nn->fw_ver.resv, nn->fw_ver.class,
-		 nn->fw_ver.major, nn->fw_ver.minor, nsp_version);
+		 nn->fw_ver.major, nn->fw_ver.minor, nsp_version,
+		 nfp_app_mip_name(nn->app), nfp_app_name(nn->app));
 	strlcpy(drvinfo->bus_info, pci_name(nn->pdev),
 		sizeof(drvinfo->bus_info));
 
@@ -195,37 +198,38 @@ nfp_net_get_link_ksettings(struct net_device *netdev,
 		[NFP_NET_CFG_STS_LINK_RATE_50G]		= SPEED_50000,
 		[NFP_NET_CFG_STS_LINK_RATE_100G]	= SPEED_100000,
 	};
-	struct nfp_net *nn = netdev_priv(netdev);
+	struct nfp_eth_table_port *eth_port;
+	struct nfp_port *port;
+	struct nfp_net *nn;
 	u32 sts, ls;
 
+	/* Init to unknowns */
 	ethtool_link_ksettings_add_link_mode(cmd, supported, FIBRE);
 	cmd->base.port = PORT_OTHER;
 	cmd->base.speed = SPEED_UNKNOWN;
 	cmd->base.duplex = DUPLEX_UNKNOWN;
 
-	if (nn->eth_port)
-		cmd->base.autoneg = nn->eth_port->aneg != NFP_ANEG_DISABLED ?
+	port = nfp_port_from_netdev(netdev);
+	eth_port = nfp_port_get_eth_port(port);
+	if (eth_port)
+		cmd->base.autoneg = eth_port->aneg != NFP_ANEG_DISABLED ?
 			AUTONEG_ENABLE : AUTONEG_DISABLE;
 
 	if (!netif_carrier_ok(netdev))
 		return 0;
 
 	/* Use link speed from ETH table if available, otherwise try the BAR */
-	if (nn->eth_port) {
-		int err;
-
-		if (nfp_net_link_changed_read_clear(nn)) {
-			err = nfp_net_refresh_eth_port(nn);
-			if (err)
-				return err;
-		}
-
-		cmd->base.port = nn->eth_port->port_type;
-		cmd->base.speed = nn->eth_port->speed;
+	if (eth_port) {
+		cmd->base.port = eth_port->port_type;
+		cmd->base.speed = eth_port->speed;
 		cmd->base.duplex = DUPLEX_FULL;
 		return 0;
 	}
 
+	if (!nfp_netdev_is_nfp_net(netdev))
+		return -EOPNOTSUPP;
+	nn = netdev_priv(netdev);
+
 	sts = nn_readl(nn, NFP_NET_CFG_STS);
 
 	ls = FIELD_GET(NFP_NET_CFG_STS_LINK_RATE, sts);
@@ -246,19 +250,22 @@ static int
 nfp_net_set_link_ksettings(struct net_device *netdev,
 			   const struct ethtool_link_ksettings *cmd)
 {
-	struct nfp_net *nn = netdev_priv(netdev);
+	struct nfp_eth_table_port *eth_port;
+	struct nfp_port *port;
 	struct nfp_nsp *nsp;
 	int err;
 
-	if (!nn->eth_port)
+	port = nfp_port_from_netdev(netdev);
+	eth_port = __nfp_port_get_eth_port(port);
+	if (!eth_port)
 		return -EOPNOTSUPP;
 
 	if (netif_running(netdev)) {
-		nn_warn(nn, "Changing settings not allowed on an active interface. It may cause the port to be disabled until reboot.\n");
+		netdev_warn(netdev, "Changing settings not allowed on an active interface. It may cause the port to be disabled until reboot.\n");
 		return -EBUSY;
 	}
 
-	nsp = nfp_eth_config_start(nn->cpp, nn->eth_port->index);
+	nsp = nfp_eth_config_start(port->app->cpp, eth_port->index);
 	if (IS_ERR(nsp))
 		return PTR_ERR(nsp);
 
@@ -267,7 +274,7 @@ nfp_net_set_link_ksettings(struct net_device *netdev,
 	if (err)
 		goto err_bad_set;
 	if (cmd->base.speed != SPEED_UNKNOWN) {
-		u32 speed = cmd->base.speed / nn->eth_port->lanes;
+		u32 speed = cmd->base.speed / eth_port->lanes;
 
 		err = __nfp_eth_set_speed(nsp, speed);
 		if (err)
@@ -278,7 +285,7 @@ nfp_net_set_link_ksettings(struct net_device *netdev,
 	if (err > 0)
 		return 0; /* no change */
 
-	nfp_net_refresh_port_table(nn);
+	nfp_net_refresh_port_table(port);
 
 	return err;
 
@@ -496,7 +503,7 @@ static int nfp_net_get_rss_hash_opts(struct nfp_net *nn,
 
 	cmd->data = 0;
 
-	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS))
+	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS_ANY))
 		return -EOPNOTSUPP;
 
 	nfp_rss_flag = ethtool_flow_to_nfp_flag(cmd->flow_type);
@@ -533,7 +540,7 @@ static int nfp_net_set_rss_hash_opt(struct nfp_net *nn,
 	u32 nfp_rss_flag;
 	int err;
 
-	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS))
+	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS_ANY))
 		return -EOPNOTSUPP;
 
 	/* RSS only supports IP SA/DA and L4 src/dst ports  */
@@ -595,7 +602,7 @@ static u32 nfp_net_get_rxfh_indir_size(struct net_device *netdev)
 {
 	struct nfp_net *nn = netdev_priv(netdev);
 
-	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS))
+	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS_ANY))
 		return 0;
 
 	return ARRAY_SIZE(nn->rss_itbl);
@@ -605,7 +612,7 @@ static u32 nfp_net_get_rxfh_key_size(struct net_device *netdev)
 {
 	struct nfp_net *nn = netdev_priv(netdev);
 
-	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS))
+	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS_ANY))
 		return -EOPNOTSUPP;
 
 	return nfp_net_rss_key_sz(nn);
@@ -617,7 +624,7 @@ static int nfp_net_get_rxfh(struct net_device *netdev, u32 *indir, u8 *key,
 	struct nfp_net *nn = netdev_priv(netdev);
 	int i;
 
-	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS))
+	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS_ANY))
 		return -EOPNOTSUPP;
 
 	if (indir)
@@ -641,7 +648,7 @@ static int nfp_net_set_rxfh(struct net_device *netdev,
 	struct nfp_net *nn = netdev_priv(netdev);
 	int i;
 
-	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS) ||
+	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS_ANY) ||
 	    !(hfunc == ETH_RSS_HASH_NO_CHANGE || hfunc == nn->rss_hfunc))
 		return -EOPNOTSUPP;
 
@@ -706,13 +713,13 @@ nfp_dump_nsp_diag(struct nfp_net *nn, struct ethtool_dump *dump, void *buffer)
 	struct nfp_resource *res;
 	int ret;
 
-	if (!nn->cpp)
+	if (!nn->app)
 		return -EOPNOTSUPP;
 
 	dump->version = 1;
 	dump->flag = NFP_DUMP_NSP_DIAG;
 
-	res = nfp_resource_acquire(nn->cpp, NFP_RESOURCE_NSP_DIAG);
+	res = nfp_resource_acquire(nn->app->cpp, NFP_RESOURCE_NSP_DIAG);
 	if (IS_ERR(res))
 		return PTR_ERR(res);
 
@@ -722,7 +729,7 @@ nfp_dump_nsp_diag(struct nfp_net *nn, struct ethtool_dump *dump, void *buffer)
 			goto exit_release;
 		}
 
-		ret = nfp_cpp_read(nn->cpp, nfp_resource_cpp_id(res),
+		ret = nfp_cpp_read(nn->app->cpp, nfp_resource_cpp_id(res),
 				   nfp_resource_address(res),
 				   buffer, dump->len);
 		if (ret != dump->len)
@@ -743,7 +750,7 @@ static int nfp_net_set_dump(struct net_device *netdev, struct ethtool_dump *val)
 {
 	struct nfp_net *nn = netdev_priv(netdev);
 
-	if (!nn->cpp)
+	if (!nn->app)
 		return -EOPNOTSUPP;
 
 	if (val->flag != NFP_DUMP_NSP_DIAG)
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_main.c b/drivers/net/ethernet/netronome/nfp/nfp_net_main.c
index 8cb87cbe1120..5797dbf2b507 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net_main.c
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_main.c
@@ -43,6 +43,7 @@
 #include <linux/etherdevice.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
+#include <linux/lockdep.h>
 #include <linux/pci.h>
 #include <linux/pci_regs.h>
 #include <linux/msi.h>
@@ -54,20 +55,21 @@
 #include "nfpcore/nfp_nffw.h"
 #include "nfpcore/nfp_nsp.h"
 #include "nfpcore/nfp6000_pcie.h"
-
+#include "nfp_app.h"
 #include "nfp_net_ctrl.h"
 #include "nfp_net.h"
 #include "nfp_main.h"
+#include "nfp_port.h"
 
 #define NFP_PF_CSR_SLICE_SIZE	(32 * 1024)
 
-static int nfp_is_ready(struct nfp_cpp *cpp)
+static int nfp_is_ready(struct nfp_pf *pf)
 {
 	const char *cp;
 	long state;
 	int err;
 
-	cp = nfp_hwinfo_lookup(cpp, "board.state");
+	cp = nfp_hwinfo_lookup(pf->hwinfo, "board.state");
 	if (!cp)
 		return 0;
 
@@ -79,375 +81,277 @@ static int nfp_is_ready(struct nfp_cpp *cpp)
 }
 
 /**
- * nfp_net_map_area() - Help function to map an area
- * @cpp:    NFP CPP handler
- * @name:   Name for the area
- * @target: CPP target
- * @addr:   CPP address
- * @size:   Size of the area
- * @area:   Area handle (returned).
- *
- * This function is primarily to simplify the code in the main probe
- * function. To undo the effect of this functions call
- * @nfp_cpp_area_release_free(*area);
- *
- * Return: Pointer to memory mapped area or ERR_PTR
- */
-static u8 __iomem *nfp_net_map_area(struct nfp_cpp *cpp,
-				    const char *name, int isl, int target,
-				    unsigned long long addr, unsigned long size,
-				    struct nfp_cpp_area **area)
-{
-	u8 __iomem *res;
-	u32 dest;
-	int err;
-
-	dest = NFP_CPP_ISLAND_ID(target, NFP_CPP_ACTION_RW, 0, isl);
-
-	*area = nfp_cpp_area_alloc_with_name(cpp, dest, name, addr, size);
-	if (!*area) {
-		err = -EIO;
-		goto err_area;
-	}
-
-	err = nfp_cpp_area_acquire(*area);
-	if (err < 0)
-		goto err_acquire;
-
-	res = nfp_cpp_area_iomem(*area);
-	if (!res) {
-		err = -EIO;
-		goto err_map;
-	}
-
-	return res;
-
-err_map:
-	nfp_cpp_area_release(*area);
-err_acquire:
-	nfp_cpp_area_free(*area);
-err_area:
-	return (u8 __iomem *)ERR_PTR(err);
-}
-
-/**
  * nfp_net_get_mac_addr() - Get the MAC address.
- * @nn:       NFP Network structure
- * @cpp:      NFP CPP handle
- * @id:	      NFP port id
+ * @pf:       NFP PF handle
+ * @port:     NFP port structure
  *
  * First try to get the MAC address from NSP ETH table. If that
- * fails try HWInfo.  As a last resort generate a random address.
+ * fails generate a random address.
  */
-static void
-nfp_net_get_mac_addr(struct nfp_net *nn, struct nfp_cpp *cpp, unsigned int id)
+void nfp_net_get_mac_addr(struct nfp_pf *pf, struct nfp_port *port)
 {
-	struct nfp_net_dp *dp = &nn->dp;
-	u8 mac_addr[ETH_ALEN];
-	const char *mac_str;
-	char name[32];
-
-	if (nn->eth_port) {
-		ether_addr_copy(dp->netdev->dev_addr, nn->eth_port->mac_addr);
-		ether_addr_copy(dp->netdev->perm_addr, nn->eth_port->mac_addr);
-		return;
-	}
-
-	snprintf(name, sizeof(name), "eth%d.mac", id);
-
-	mac_str = nfp_hwinfo_lookup(cpp, name);
-	if (!mac_str) {
-		dev_warn(dp->dev, "Can't lookup MAC address. Generate\n");
-		eth_hw_addr_random(dp->netdev);
-		return;
-	}
+	struct nfp_eth_table_port *eth_port;
 
-	if (sscanf(mac_str, "%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx",
-		   &mac_addr[0], &mac_addr[1], &mac_addr[2],
-		   &mac_addr[3], &mac_addr[4], &mac_addr[5]) != 6) {
-		dev_warn(dp->dev,
-			 "Can't parse MAC address (%s). Generate.\n", mac_str);
-		eth_hw_addr_random(dp->netdev);
+	eth_port = __nfp_port_get_eth_port(port);
+	if (!eth_port) {
+		eth_hw_addr_random(port->netdev);
 		return;
 	}
 
-	ether_addr_copy(dp->netdev->dev_addr, mac_addr);
-	ether_addr_copy(dp->netdev->perm_addr, mac_addr);
+	ether_addr_copy(port->netdev->dev_addr, eth_port->mac_addr);
+	ether_addr_copy(port->netdev->perm_addr, eth_port->mac_addr);
 }
 
 static struct nfp_eth_table_port *
-nfp_net_find_port(struct nfp_eth_table *eth_tbl, unsigned int id)
+nfp_net_find_port(struct nfp_eth_table *eth_tbl, unsigned int index)
 {
 	int i;
 
 	for (i = 0; eth_tbl && i < eth_tbl->count; i++)
-		if (eth_tbl->ports[i].eth_index == id)
+		if (eth_tbl->ports[i].index == index)
 			return &eth_tbl->ports[i];
 
 	return NULL;
 }
 
-static unsigned int nfp_net_pf_get_num_ports(struct nfp_pf *pf)
+static int
+nfp_net_pf_rtsym_read_optional(struct nfp_pf *pf, const char *format,
+			       unsigned int default_val)
 {
 	char name[256];
-	u16 interface;
-	int pcie_pf;
 	int err = 0;
 	u64 val;
 
-	interface = nfp_cpp_interface(pf->cpp);
-	pcie_pf = NFP_CPP_INTERFACE_UNIT_of(interface);
-
-	snprintf(name, sizeof(name), "nfd_cfg_pf%d_num_ports", pcie_pf);
+	snprintf(name, sizeof(name), format, nfp_cppcore_pcie_unit(pf->cpp));
 
-	val = nfp_rtsym_read_le(pf->cpp, name, &err);
-	/* Default to one port */
+	val = nfp_rtsym_read_le(pf->rtbl, name, &err);
 	if (err) {
-		if (err != -ENOENT)
-			nfp_err(pf->cpp, "Unable to read adapter port count\n");
-		val = 1;
+		if (err == -ENOENT)
+			return default_val;
+		nfp_err(pf->cpp, "Unable to read symbol %s\n", name);
+		return err;
 	}
 
 	return val;
 }
 
-static unsigned int
-nfp_net_pf_total_qcs(struct nfp_pf *pf, void __iomem *ctrl_bar,
-		     unsigned int stride, u32 start_off, u32 num_off)
+static int nfp_net_pf_get_num_ports(struct nfp_pf *pf)
 {
-	unsigned int i, min_qc, max_qc;
-
-	min_qc = readl(ctrl_bar + start_off);
-	max_qc = min_qc;
-
-	for (i = 0; i < pf->num_ports; i++) {
-		/* To make our lives simpler only accept configuration where
-		 * queues are allocated to PFs in order (queues of PFn all have
-		 * indexes lower than PFn+1).
-		 */
-		if (max_qc > readl(ctrl_bar + start_off))
-			return 0;
-
-		max_qc = readl(ctrl_bar + start_off);
-		max_qc += readl(ctrl_bar + num_off) * stride;
-		ctrl_bar += NFP_PF_CSR_SLICE_SIZE;
-	}
+	return nfp_net_pf_rtsym_read_optional(pf, "nfd_cfg_pf%u_num_ports", 1);
+}
 
-	return max_qc - min_qc;
+static int nfp_net_pf_get_app_id(struct nfp_pf *pf)
+{
+	return nfp_net_pf_rtsym_read_optional(pf, "_pf%u_net_app_id",
+					      NFP_APP_CORE_NIC);
 }
 
-static u8 __iomem *nfp_net_pf_map_ctrl_bar(struct nfp_pf *pf)
+static u8 __iomem *
+nfp_net_pf_map_rtsym(struct nfp_pf *pf, const char *name, const char *sym_fmt,
+		     unsigned int min_size, struct nfp_cpp_area **area)
 {
-	const struct nfp_rtsym *ctrl_sym;
-	u8 __iomem *ctrl_bar;
 	char pf_symbol[256];
-	u16 interface;
-	int pcie_pf;
-
-	interface = nfp_cpp_interface(pf->cpp);
-	pcie_pf = NFP_CPP_INTERFACE_UNIT_of(interface);
-
-	snprintf(pf_symbol, sizeof(pf_symbol), "_pf%d_net_bar0", pcie_pf);
-
-	ctrl_sym = nfp_rtsym_lookup(pf->cpp, pf_symbol);
-	if (!ctrl_sym) {
-		dev_err(&pf->pdev->dev,
-			"Failed to find PF BAR0 symbol %s\n", pf_symbol);
-		return NULL;
-	}
-
-	if (ctrl_sym->size < pf->num_ports * NFP_PF_CSR_SLICE_SIZE) {
-		dev_err(&pf->pdev->dev,
-			"PF BAR0 too small to contain %d ports\n",
-			pf->num_ports);
-		return NULL;
-	}
 
-	ctrl_bar = nfp_net_map_area(pf->cpp, "net.ctrl",
-				    ctrl_sym->domain, ctrl_sym->target,
-				    ctrl_sym->addr, ctrl_sym->size,
-				    &pf->ctrl_area);
-	if (IS_ERR(ctrl_bar)) {
-		dev_err(&pf->pdev->dev, "Failed to map PF BAR0: %ld\n",
-			PTR_ERR(ctrl_bar));
-		return NULL;
-	}
+	snprintf(pf_symbol, sizeof(pf_symbol), sym_fmt,
+		 nfp_cppcore_pcie_unit(pf->cpp));
 
-	return ctrl_bar;
+	return nfp_rtsym_map(pf->rtbl, pf_symbol, name, min_size, area);
 }
 
-static void nfp_net_pf_free_netdevs(struct nfp_pf *pf)
+static void nfp_net_pf_free_vnic(struct nfp_pf *pf, struct nfp_net *nn)
 {
-	struct nfp_net *nn;
+	nfp_port_free(nn->port);
+	list_del(&nn->vnic_list);
+	pf->num_vnics--;
+	nfp_net_free(nn);
+}
 
-	while (!list_empty(&pf->ports)) {
-		nn = list_first_entry(&pf->ports, struct nfp_net, port_list);
-		list_del(&nn->port_list);
-		pf->num_netdevs--;
+static void nfp_net_pf_free_vnics(struct nfp_pf *pf)
+{
+	struct nfp_net *nn, *next;
 
-		nfp_net_netdev_free(nn);
-	}
+	list_for_each_entry_safe(nn, next, &pf->vnics, vnic_list)
+		if (nfp_net_is_data_vnic(nn))
+			nfp_net_pf_free_vnic(pf, nn);
 }
 
 static struct nfp_net *
-nfp_net_pf_alloc_port_netdev(struct nfp_pf *pf, void __iomem *ctrl_bar,
-			     void __iomem *tx_bar, void __iomem *rx_bar,
-			     int stride, struct nfp_net_fw_version *fw_ver,
-			     struct nfp_eth_table_port *eth_port)
+nfp_net_pf_alloc_vnic(struct nfp_pf *pf, bool needs_netdev,
+		      void __iomem *ctrl_bar, void __iomem *qc_bar,
+		      int stride, unsigned int id)
 {
-	u32 n_tx_rings, n_rx_rings;
+	u32 tx_base, rx_base, n_tx_rings, n_rx_rings;
 	struct nfp_net *nn;
+	int err;
 
+	tx_base = readl(ctrl_bar + NFP_NET_CFG_START_TXQ);
+	rx_base = readl(ctrl_bar + NFP_NET_CFG_START_RXQ);
 	n_tx_rings = readl(ctrl_bar + NFP_NET_CFG_MAX_TXRINGS);
 	n_rx_rings = readl(ctrl_bar + NFP_NET_CFG_MAX_RXRINGS);
 
-	/* Allocate and initialise the netdev */
-	nn = nfp_net_netdev_alloc(pf->pdev, n_tx_rings, n_rx_rings);
+	/* Allocate and initialise the vNIC */
+	nn = nfp_net_alloc(pf->pdev, needs_netdev, n_tx_rings, n_rx_rings);
 	if (IS_ERR(nn))
 		return nn;
 
-	nn->cpp = pf->cpp;
-	nn->fw_ver = *fw_ver;
+	nn->app = pf->app;
+	nfp_net_get_fw_version(&nn->fw_ver, ctrl_bar);
 	nn->dp.ctrl_bar = ctrl_bar;
-	nn->tx_bar = tx_bar;
-	nn->rx_bar = rx_bar;
+	nn->tx_bar = qc_bar + tx_base * NFP_QCP_QUEUE_ADDR_SZ;
+	nn->rx_bar = qc_bar + rx_base * NFP_QCP_QUEUE_ADDR_SZ;
 	nn->dp.is_vf = 0;
 	nn->stride_rx = stride;
 	nn->stride_tx = stride;
-	nn->eth_port = eth_port;
+
+	if (needs_netdev) {
+		err = nfp_app_vnic_init(pf->app, nn, id);
+		if (err) {
+			nfp_net_free(nn);
+			return ERR_PTR(err);
+		}
+	}
+
+	pf->num_vnics++;
+	list_add_tail(&nn->vnic_list, &pf->vnics);
 
 	return nn;
 }
 
 static int
-nfp_net_pf_init_port_netdev(struct nfp_pf *pf, struct nfp_net *nn,
-			    unsigned int id)
+nfp_net_pf_init_vnic(struct nfp_pf *pf, struct nfp_net *nn, unsigned int id)
 {
 	int err;
 
-	/* Get MAC address */
-	nfp_net_get_mac_addr(nn, pf->cpp, id);
-
 	/* Get ME clock frequency from ctrl BAR
 	 * XXX for now frequency is hardcoded until we figure out how
 	 * to get the value from nfp-hwinfo into ctrl bar
 	 */
 	nn->me_freq_mhz = 1200;
 
-	err = nfp_net_netdev_init(nn->dp.netdev);
+	err = nfp_net_init(nn);
 	if (err)
 		return err;
 
-	nfp_net_debugfs_port_add(nn, pf->ddir, id);
+	nfp_net_debugfs_vnic_add(nn, pf->ddir, id);
+
+	if (nn->port) {
+		err = nfp_devlink_port_register(pf->app, nn->port);
+		if (err)
+			goto err_dfs_clean;
+	}
 
 	nfp_net_info(nn);
 
 	return 0;
+
+err_dfs_clean:
+	nfp_net_debugfs_dir_clean(&nn->debugfs_dir);
+	nfp_net_clean(nn);
+	return err;
 }
 
 static int
-nfp_net_pf_alloc_netdevs(struct nfp_pf *pf, void __iomem *ctrl_bar,
-			 void __iomem *tx_bar, void __iomem *rx_bar,
-			 int stride, struct nfp_net_fw_version *fw_ver)
+nfp_net_pf_alloc_vnics(struct nfp_pf *pf, void __iomem *ctrl_bar,
+		       void __iomem *qc_bar, int stride)
 {
-	u32 prev_tx_base, prev_rx_base, tgt_tx_base, tgt_rx_base;
-	struct nfp_eth_table_port *eth_port;
 	struct nfp_net *nn;
 	unsigned int i;
 	int err;
 
-	prev_tx_base = readl(ctrl_bar + NFP_NET_CFG_START_TXQ);
-	prev_rx_base = readl(ctrl_bar + NFP_NET_CFG_START_RXQ);
-
-	for (i = 0; i < pf->num_ports; i++) {
-		tgt_tx_base = readl(ctrl_bar + NFP_NET_CFG_START_TXQ);
-		tgt_rx_base = readl(ctrl_bar + NFP_NET_CFG_START_RXQ);
-		tx_bar += (tgt_tx_base - prev_tx_base) * NFP_QCP_QUEUE_ADDR_SZ;
-		rx_bar += (tgt_rx_base - prev_rx_base) * NFP_QCP_QUEUE_ADDR_SZ;
-		prev_tx_base = tgt_tx_base;
-		prev_rx_base = tgt_rx_base;
-
-		eth_port = nfp_net_find_port(pf->eth_tbl, i);
-		if (eth_port && eth_port->override_changed) {
-			nfp_warn(pf->cpp, "Config changed for port #%d, reboot required before port will be operational\n", i);
-		} else {
-			nn = nfp_net_pf_alloc_port_netdev(pf, ctrl_bar, tx_bar,
-							  rx_bar, stride,
-							  fw_ver, eth_port);
-			if (IS_ERR(nn)) {
-				err = PTR_ERR(nn);
-				goto err_free_prev;
-			}
-			list_add_tail(&nn->port_list, &pf->ports);
-			pf->num_netdevs++;
+	for (i = 0; i < pf->max_data_vnics; i++) {
+		nn = nfp_net_pf_alloc_vnic(pf, true, ctrl_bar, qc_bar,
+					   stride, i);
+		if (IS_ERR(nn)) {
+			err = PTR_ERR(nn);
+			goto err_free_prev;
 		}
 
 		ctrl_bar += NFP_PF_CSR_SLICE_SIZE;
+
+		/* Kill the vNIC if app init marked it as invalid */
+		if (nn->port && nn->port->type == NFP_PORT_INVALID) {
+			nfp_net_pf_free_vnic(pf, nn);
+			continue;
+		}
 	}
 
-	if (list_empty(&pf->ports))
+	if (list_empty(&pf->vnics))
 		return -ENODEV;
 
 	return 0;
 
 err_free_prev:
-	nfp_net_pf_free_netdevs(pf);
+	nfp_net_pf_free_vnics(pf);
 	return err;
 }
 
-static int
-nfp_net_pf_spawn_netdevs(struct nfp_pf *pf,
-			 void __iomem *ctrl_bar, void __iomem *tx_bar,
-			 void __iomem *rx_bar, int stride,
-			 struct nfp_net_fw_version *fw_ver)
+static void nfp_net_pf_clean_vnic(struct nfp_pf *pf, struct nfp_net *nn)
 {
-	unsigned int id, wanted_irqs, num_irqs, ports_left, irqs_left;
-	struct nfp_net *nn;
-	int err;
+	if (nn->port)
+		nfp_devlink_port_unregister(nn->port);
+	nfp_net_debugfs_dir_clean(&nn->debugfs_dir);
+	nfp_net_clean(nn);
+	nfp_app_vnic_clean(pf->app, nn);
+}
 
-	/* Allocate the netdevs and do basic init */
-	err = nfp_net_pf_alloc_netdevs(pf, ctrl_bar, tx_bar, rx_bar,
-				       stride, fw_ver);
-	if (err)
-		return err;
+static int nfp_net_pf_alloc_irqs(struct nfp_pf *pf)
+{
+	unsigned int wanted_irqs, num_irqs, vnics_left, irqs_left;
+	struct nfp_net *nn;
 
 	/* Get MSI-X vectors */
 	wanted_irqs = 0;
-	list_for_each_entry(nn, &pf->ports, port_list)
+	list_for_each_entry(nn, &pf->vnics, vnic_list)
 		wanted_irqs += NFP_NET_NON_Q_VECTORS + nn->dp.num_r_vecs;
 	pf->irq_entries = kcalloc(wanted_irqs, sizeof(*pf->irq_entries),
 				  GFP_KERNEL);
-	if (!pf->irq_entries) {
-		err = -ENOMEM;
-		goto err_nn_free;
-	}
+	if (!pf->irq_entries)
+		return -ENOMEM;
 
 	num_irqs = nfp_net_irqs_alloc(pf->pdev, pf->irq_entries,
-				      NFP_NET_MIN_PORT_IRQS * pf->num_netdevs,
+				      NFP_NET_MIN_VNIC_IRQS * pf->num_vnics,
 				      wanted_irqs);
 	if (!num_irqs) {
-		nn_warn(nn, "Unable to allocate MSI-X Vectors. Exiting\n");
-		err = -ENOMEM;
-		goto err_vec_free;
+		nfp_warn(pf->cpp, "Unable to allocate MSI-X vectors\n");
+		kfree(pf->irq_entries);
+		return -ENOMEM;
 	}
 
-	/* Distribute IRQs to ports */
+	/* Distribute IRQs to vNICs */
 	irqs_left = num_irqs;
-	ports_left = pf->num_netdevs;
-	list_for_each_entry(nn, &pf->ports, port_list) {
+	vnics_left = pf->num_vnics;
+	list_for_each_entry(nn, &pf->vnics, vnic_list) {
 		unsigned int n;
 
-		n = DIV_ROUND_UP(irqs_left, ports_left);
+		n = min(NFP_NET_NON_Q_VECTORS + nn->dp.num_r_vecs,
+			DIV_ROUND_UP(irqs_left, vnics_left));
 		nfp_net_irqs_assign(nn, &pf->irq_entries[num_irqs - irqs_left],
 				    n);
 		irqs_left -= n;
-		ports_left--;
+		vnics_left--;
 	}
 
-	/* Finish netdev init and register */
+	return 0;
+}
+
+static void nfp_net_pf_free_irqs(struct nfp_pf *pf)
+{
+	nfp_net_irqs_disable(pf->pdev);
+	kfree(pf->irq_entries);
+}
+
+static int nfp_net_pf_init_vnics(struct nfp_pf *pf)
+{
+	struct nfp_net *nn;
+	unsigned int id;
+	int err;
+
+	/* Finish vNIC init and register */
 	id = 0;
-	list_for_each_entry(nn, &pf->ports, port_list) {
-		err = nfp_net_pf_init_port_netdev(pf, nn, id);
+	list_for_each_entry(nn, &pf->vnics, vnic_list) {
+		if (!nfp_net_is_data_vnic(nn))
+			continue;
+		err = nfp_net_pf_init_vnic(pf, nn, id);
 		if (err)
 			goto err_prev_deinit;
 
@@ -457,118 +361,326 @@ nfp_net_pf_spawn_netdevs(struct nfp_pf *pf,
 	return 0;
 
 err_prev_deinit:
-	list_for_each_entry_continue_reverse(nn, &pf->ports, port_list) {
-		nfp_net_debugfs_dir_clean(&nn->debugfs_dir);
-		nfp_net_netdev_clean(nn->dp.netdev);
+	list_for_each_entry_continue_reverse(nn, &pf->vnics, vnic_list)
+		if (nfp_net_is_data_vnic(nn))
+			nfp_net_pf_clean_vnic(pf, nn);
+	return err;
+}
+
+static int
+nfp_net_pf_app_init(struct nfp_pf *pf, u8 __iomem *qc_bar, unsigned int stride)
+{
+	u8 __iomem *ctrl_bar;
+	int err;
+
+	pf->app = nfp_app_alloc(pf, nfp_net_pf_get_app_id(pf));
+	if (IS_ERR(pf->app))
+		return PTR_ERR(pf->app);
+
+	err = nfp_app_init(pf->app);
+	if (err)
+		goto err_free;
+
+	if (!nfp_app_needs_ctrl_vnic(pf->app))
+		return 0;
+
+	ctrl_bar = nfp_net_pf_map_rtsym(pf, "net.ctrl", "_pf%u_net_ctrl_bar",
+					NFP_PF_CSR_SLICE_SIZE,
+					&pf->ctrl_vnic_bar);
+	if (IS_ERR(ctrl_bar)) {
+		nfp_err(pf->cpp, "Failed to find data vNIC memory symbol\n");
+		err = PTR_ERR(ctrl_bar);
+		goto err_app_clean;
 	}
-	nfp_net_irqs_disable(pf->pdev);
-err_vec_free:
-	kfree(pf->irq_entries);
-err_nn_free:
-	nfp_net_pf_free_netdevs(pf);
+
+	pf->ctrl_vnic =	nfp_net_pf_alloc_vnic(pf, false, ctrl_bar, qc_bar,
+					      stride, 0);
+	if (IS_ERR(pf->ctrl_vnic)) {
+		err = PTR_ERR(pf->ctrl_vnic);
+		goto err_unmap;
+	}
+
+	return 0;
+
+err_unmap:
+	nfp_cpp_area_release_free(pf->ctrl_vnic_bar);
+err_app_clean:
+	nfp_app_clean(pf->app);
+err_free:
+	nfp_app_free(pf->app);
+	pf->app = NULL;
+	return err;
+}
+
+static void nfp_net_pf_app_clean(struct nfp_pf *pf)
+{
+	if (pf->ctrl_vnic) {
+		nfp_net_pf_free_vnic(pf, pf->ctrl_vnic);
+		nfp_cpp_area_release_free(pf->ctrl_vnic_bar);
+	}
+	nfp_app_clean(pf->app);
+	nfp_app_free(pf->app);
+	pf->app = NULL;
+}
+
+static int nfp_net_pf_app_start_ctrl(struct nfp_pf *pf)
+{
+	int err;
+
+	if (!pf->ctrl_vnic)
+		return 0;
+	err = nfp_net_pf_init_vnic(pf, pf->ctrl_vnic, 0);
+	if (err)
+		return err;
+
+	err = nfp_ctrl_open(pf->ctrl_vnic);
+	if (err)
+		goto err_clean_ctrl;
+
+	return 0;
+
+err_clean_ctrl:
+	nfp_net_pf_clean_vnic(pf, pf->ctrl_vnic);
+	return err;
+}
+
+static void nfp_net_pf_app_stop_ctrl(struct nfp_pf *pf)
+{
+	if (!pf->ctrl_vnic)
+		return;
+	nfp_ctrl_close(pf->ctrl_vnic);
+	nfp_net_pf_clean_vnic(pf, pf->ctrl_vnic);
+}
+
+static int nfp_net_pf_app_start(struct nfp_pf *pf)
+{
+	int err;
+
+	err = nfp_net_pf_app_start_ctrl(pf);
+	if (err)
+		return err;
+
+	err = nfp_app_start(pf->app, pf->ctrl_vnic);
+	if (err)
+		goto err_ctrl_stop;
+
+	if (pf->num_vfs) {
+		err = nfp_app_sriov_enable(pf->app, pf->num_vfs);
+		if (err)
+			goto err_app_stop;
+	}
+
+	return 0;
+
+err_app_stop:
+	nfp_app_stop(pf->app);
+err_ctrl_stop:
+	nfp_net_pf_app_stop_ctrl(pf);
+	return err;
+}
+
+static void nfp_net_pf_app_stop(struct nfp_pf *pf)
+{
+	if (pf->num_vfs)
+		nfp_app_sriov_disable(pf->app);
+	nfp_app_stop(pf->app);
+	nfp_net_pf_app_stop_ctrl(pf);
+}
+
+static void nfp_net_pci_unmap_mem(struct nfp_pf *pf)
+{
+	if (pf->vf_cfg_bar)
+		nfp_cpp_area_release_free(pf->vf_cfg_bar);
+	if (pf->mac_stats_bar)
+		nfp_cpp_area_release_free(pf->mac_stats_bar);
+	nfp_cpp_area_release_free(pf->qc_area);
+	nfp_cpp_area_release_free(pf->data_vnic_bar);
+}
+
+static int nfp_net_pci_map_mem(struct nfp_pf *pf)
+{
+	u8 __iomem *mem;
+	u32 min_size;
+	int err;
+
+	min_size = pf->max_data_vnics * NFP_PF_CSR_SLICE_SIZE;
+	mem = nfp_net_pf_map_rtsym(pf, "net.ctrl", "_pf%d_net_bar0",
+				   min_size, &pf->data_vnic_bar);
+	if (IS_ERR(mem)) {
+		nfp_err(pf->cpp, "Failed to find data vNIC memory symbol\n");
+		return PTR_ERR(mem);
+	}
+
+	min_size =  NFP_MAC_STATS_SIZE * (pf->eth_tbl->max_index + 1);
+	pf->mac_stats_mem = nfp_rtsym_map(pf->rtbl, "_mac_stats",
+					  "net.macstats", min_size,
+					  &pf->mac_stats_bar);
+	if (IS_ERR(pf->mac_stats_mem)) {
+		if (PTR_ERR(pf->mac_stats_mem) != -ENOENT) {
+			err = PTR_ERR(pf->mac_stats_mem);
+			goto err_unmap_ctrl;
+		}
+		pf->mac_stats_mem = NULL;
+	}
+
+	pf->vf_cfg_mem = nfp_net_pf_map_rtsym(pf, "net.vfcfg",
+					      "_pf%d_net_vf_bar",
+					      NFP_NET_CFG_BAR_SZ *
+					      pf->limit_vfs, &pf->vf_cfg_bar);
+	if (IS_ERR(pf->vf_cfg_mem)) {
+		if (PTR_ERR(pf->vf_cfg_mem) != -ENOENT) {
+			err = PTR_ERR(pf->vf_cfg_mem);
+			goto err_unmap_mac_stats;
+		}
+		pf->vf_cfg_mem = NULL;
+	}
+
+	mem = nfp_cpp_map_area(pf->cpp, "net.qc", 0, 0,
+			       NFP_PCIE_QUEUE(0), NFP_QCP_QUEUE_AREA_SZ,
+			       &pf->qc_area);
+	if (IS_ERR(mem)) {
+		nfp_err(pf->cpp, "Failed to map Queue Controller area.\n");
+		err = PTR_ERR(mem);
+		goto err_unmap_vf_cfg;
+	}
+
+	return 0;
+
+err_unmap_vf_cfg:
+	if (pf->vf_cfg_bar)
+		nfp_cpp_area_release_free(pf->vf_cfg_bar);
+err_unmap_mac_stats:
+	if (pf->mac_stats_bar)
+		nfp_cpp_area_release_free(pf->mac_stats_bar);
+err_unmap_ctrl:
+	nfp_cpp_area_release_free(pf->data_vnic_bar);
 	return err;
 }
 
 static void nfp_net_pci_remove_finish(struct nfp_pf *pf)
 {
+	nfp_net_pf_app_stop(pf);
+	/* stop app first, to avoid double free of ctrl vNIC's ddir */
 	nfp_net_debugfs_dir_clean(&pf->ddir);
 
-	nfp_net_irqs_disable(pf->pdev);
-	kfree(pf->irq_entries);
+	nfp_net_pf_free_irqs(pf);
+	nfp_net_pf_app_clean(pf);
+	nfp_net_pci_unmap_mem(pf);
+}
 
-	nfp_cpp_area_release_free(pf->rx_area);
-	nfp_cpp_area_release_free(pf->tx_area);
-	nfp_cpp_area_release_free(pf->ctrl_area);
+static int
+nfp_net_eth_port_update(struct nfp_cpp *cpp, struct nfp_port *port,
+			struct nfp_eth_table *eth_table)
+{
+	struct nfp_eth_table_port *eth_port;
+
+	ASSERT_RTNL();
+
+	eth_port = nfp_net_find_port(eth_table, port->eth_id);
+	if (!eth_port) {
+		set_bit(NFP_PORT_CHANGED, &port->flags);
+		nfp_warn(cpp, "Warning: port #%d not present after reconfig\n",
+			 port->eth_id);
+		return -EIO;
+	}
+	if (eth_port->override_changed) {
+		nfp_warn(cpp, "Port #%d config changed, unregistering. Reboot required before port will be operational again.\n", port->eth_id);
+		port->type = NFP_PORT_INVALID;
+	}
+
+	memcpy(port->eth_port, eth_port, sizeof(*eth_port));
+
+	return 0;
 }
 
-static void nfp_net_refresh_netdevs(struct work_struct *work)
+int nfp_net_refresh_port_table_sync(struct nfp_pf *pf)
 {
-	struct nfp_pf *pf = container_of(work, struct nfp_pf,
-					 port_refresh_work);
 	struct nfp_eth_table *eth_table;
 	struct nfp_net *nn, *next;
+	struct nfp_port *port;
 
-	mutex_lock(&pf->port_lock);
+	lockdep_assert_held(&pf->lock);
 
 	/* Check for nfp_net_pci_remove() racing against us */
-	if (list_empty(&pf->ports))
-		goto out;
+	if (list_empty(&pf->vnics))
+		return 0;
 
-	list_for_each_entry(nn, &pf->ports, port_list)
-		nfp_net_link_changed_read_clear(nn);
+	/* Update state of all ports */
+	rtnl_lock();
+	list_for_each_entry(port, &pf->ports, port_list)
+		clear_bit(NFP_PORT_CHANGED, &port->flags);
 
 	eth_table = nfp_eth_read_ports(pf->cpp);
 	if (!eth_table) {
+		list_for_each_entry(port, &pf->ports, port_list)
+			if (__nfp_port_get_eth_port(port))
+				set_bit(NFP_PORT_CHANGED, &port->flags);
+		rtnl_unlock();
 		nfp_err(pf->cpp, "Error refreshing port config!\n");
-		goto out;
+		return -EIO;
 	}
 
-	rtnl_lock();
-	list_for_each_entry(nn, &pf->ports, port_list) {
-		if (!nn->eth_port)
-			continue;
-		nn->eth_port = nfp_net_find_port(eth_table,
-						 nn->eth_port->eth_index);
-	}
+	list_for_each_entry(port, &pf->ports, port_list)
+		if (__nfp_port_get_eth_port(port))
+			nfp_net_eth_port_update(pf->cpp, port, eth_table);
 	rtnl_unlock();
 
-	kfree(pf->eth_tbl);
-	pf->eth_tbl = eth_table;
+	kfree(eth_table);
 
-	list_for_each_entry_safe(nn, next, &pf->ports, port_list) {
-		if (!nn->eth_port) {
-			nfp_warn(pf->cpp, "Warning: port not present after reconfig\n");
-			continue;
-		}
-		if (!nn->eth_port->override_changed)
+	/* Shoot off the ports which became invalid */
+	list_for_each_entry_safe(nn, next, &pf->vnics, vnic_list) {
+		if (!nn->port || nn->port->type != NFP_PORT_INVALID)
 			continue;
 
-		nn_warn(nn, "Port config changed, unregistering. Reboot required before port will be operational again.\n");
-
-		nfp_net_debugfs_dir_clean(&nn->debugfs_dir);
-		nfp_net_netdev_clean(nn->dp.netdev);
-
-		list_del(&nn->port_list);
-		pf->num_netdevs--;
-		nfp_net_netdev_free(nn);
+		nfp_net_pf_clean_vnic(pf, nn);
+		nfp_net_pf_free_vnic(pf, nn);
 	}
 
-	if (list_empty(&pf->ports))
+	if (list_empty(&pf->vnics))
 		nfp_net_pci_remove_finish(pf);
-out:
-	mutex_unlock(&pf->port_lock);
+
+	return 0;
 }
 
-void nfp_net_refresh_port_table(struct nfp_net *nn)
+static void nfp_net_refresh_vnics(struct work_struct *work)
 {
-	struct nfp_pf *pf = pci_get_drvdata(nn->pdev);
+	struct nfp_pf *pf = container_of(work, struct nfp_pf,
+					 port_refresh_work);
 
-	schedule_work(&pf->port_refresh_work);
+	mutex_lock(&pf->lock);
+	nfp_net_refresh_port_table_sync(pf);
+	mutex_unlock(&pf->lock);
 }
 
-int nfp_net_refresh_eth_port(struct nfp_net *nn)
+void nfp_net_refresh_port_table(struct nfp_port *port)
 {
-	struct nfp_eth_table_port *eth_port;
+	struct nfp_pf *pf = port->app->pf;
+
+	set_bit(NFP_PORT_CHANGED, &port->flags);
+
+	queue_work(pf->wq, &pf->port_refresh_work);
+}
+
+int nfp_net_refresh_eth_port(struct nfp_port *port)
+{
+	struct nfp_cpp *cpp = port->app->cpp;
 	struct nfp_eth_table *eth_table;
+	int ret;
 
-	eth_table = nfp_eth_read_ports(nn->cpp);
-	if (!eth_table) {
-		nn_err(nn, "Error refreshing port state table!\n");
-		return -EIO;
-	}
+	clear_bit(NFP_PORT_CHANGED, &port->flags);
 
-	eth_port = nfp_net_find_port(eth_table, nn->eth_port->eth_index);
-	if (!eth_port) {
-		nn_err(nn, "Error finding state of the port!\n");
-		kfree(eth_table);
+	eth_table = nfp_eth_read_ports(cpp);
+	if (!eth_table) {
+		set_bit(NFP_PORT_CHANGED, &port->flags);
+		nfp_err(cpp, "Error refreshing port state table!\n");
 		return -EIO;
 	}
 
-	memcpy(nn->eth_port, eth_port, sizeof(*eth_port));
+	ret = nfp_net_eth_port_update(cpp, port, eth_table);
 
 	kfree(eth_table);
 
-	return 0;
+	return ret;
 }
 
 /*
@@ -576,38 +688,49 @@ int nfp_net_refresh_eth_port(struct nfp_net *nn)
  */
 int nfp_net_pci_probe(struct nfp_pf *pf)
 {
-	u8 __iomem *ctrl_bar, *tx_bar, *rx_bar;
-	u32 total_tx_qcs, total_rx_qcs;
 	struct nfp_net_fw_version fw_ver;
-	u32 tx_area_sz, rx_area_sz;
-	u32 start_q;
+	u8 __iomem *ctrl_bar, *qc_bar;
 	int stride;
 	int err;
 
-	INIT_WORK(&pf->port_refresh_work, nfp_net_refresh_netdevs);
-	mutex_init(&pf->port_lock);
+	INIT_WORK(&pf->port_refresh_work, nfp_net_refresh_vnics);
 
 	/* Verify that the board has completed initialization */
-	if (!nfp_is_ready(pf->cpp)) {
+	if (!nfp_is_ready(pf)) {
 		nfp_err(pf->cpp, "NFP is not ready for NIC operation.\n");
 		return -EINVAL;
 	}
 
-	mutex_lock(&pf->port_lock);
-	pf->num_ports = nfp_net_pf_get_num_ports(pf);
+	if (!pf->rtbl) {
+		nfp_err(pf->cpp, "No %s, giving up.\n",
+			pf->fw_loaded ? "symbol table" : "firmware found");
+		return -EPROBE_DEFER;
+	}
 
-	ctrl_bar = nfp_net_pf_map_ctrl_bar(pf);
-	if (!ctrl_bar) {
-		err = pf->fw_loaded ? -EINVAL : -EPROBE_DEFER;
+	mutex_lock(&pf->lock);
+	pf->max_data_vnics = nfp_net_pf_get_num_ports(pf);
+	if ((int)pf->max_data_vnics < 0) {
+		err = pf->max_data_vnics;
 		goto err_unlock;
 	}
 
+	err = nfp_net_pci_map_mem(pf);
+	if (err)
+		goto err_unlock;
+
+	ctrl_bar = nfp_cpp_area_iomem(pf->data_vnic_bar);
+	qc_bar = nfp_cpp_area_iomem(pf->qc_area);
+	if (!ctrl_bar || !qc_bar) {
+		err = -EIO;
+		goto err_unmap;
+	}
+
 	nfp_net_get_fw_version(&fw_ver, ctrl_bar);
 	if (fw_ver.resv || fw_ver.class != NFP_NET_CFG_VERSION_CLASS_GENERIC) {
 		nfp_err(pf->cpp, "Unknown Firmware ABI %d.%d.%d.%d\n",
 			fw_ver.resv, fw_ver.class, fw_ver.major, fw_ver.minor);
 		err = -EINVAL;
-		goto err_ctrl_unmap;
+		goto err_unmap;
 	}
 
 	/* Determine stride */
@@ -616,7 +739,7 @@ int nfp_net_pci_probe(struct nfp_pf *pf)
 		nfp_warn(pf->cpp, "OBSOLETE Firmware detected - VF isolation not available\n");
 	} else {
 		switch (fw_ver.major) {
-		case 1 ... 4:
+		case 1 ... 5:
 			stride = 4;
 			break;
 		default:
@@ -624,69 +747,51 @@ int nfp_net_pci_probe(struct nfp_pf *pf)
 				fw_ver.resv, fw_ver.class,
 				fw_ver.major, fw_ver.minor);
 			err = -EINVAL;
-			goto err_ctrl_unmap;
+			goto err_unmap;
 		}
 	}
 
-	/* Find how many QC structs need to be mapped */
-	total_tx_qcs = nfp_net_pf_total_qcs(pf, ctrl_bar, stride,
-					    NFP_NET_CFG_START_TXQ,
-					    NFP_NET_CFG_MAX_TXRINGS);
-	total_rx_qcs = nfp_net_pf_total_qcs(pf, ctrl_bar, stride,
-					    NFP_NET_CFG_START_RXQ,
-					    NFP_NET_CFG_MAX_RXRINGS);
-	if (!total_tx_qcs || !total_rx_qcs) {
-		nfp_err(pf->cpp, "Invalid PF QC configuration [%d,%d]\n",
-			total_tx_qcs, total_rx_qcs);
-		err = -EINVAL;
-		goto err_ctrl_unmap;
-	}
-
-	tx_area_sz = NFP_QCP_QUEUE_ADDR_SZ * total_tx_qcs;
-	rx_area_sz = NFP_QCP_QUEUE_ADDR_SZ * total_rx_qcs;
-
-	/* Map TX queues */
-	start_q = readl(ctrl_bar + NFP_NET_CFG_START_TXQ);
-	tx_bar = nfp_net_map_area(pf->cpp, "net.tx", 0, 0,
-				  NFP_PCIE_QUEUE(start_q),
-				  tx_area_sz, &pf->tx_area);
-	if (IS_ERR(tx_bar)) {
-		nfp_err(pf->cpp, "Failed to map TX area.\n");
-		err = PTR_ERR(tx_bar);
-		goto err_ctrl_unmap;
-	}
-
-	/* Map RX queues */
-	start_q = readl(ctrl_bar + NFP_NET_CFG_START_RXQ);
-	rx_bar = nfp_net_map_area(pf->cpp, "net.rx", 0, 0,
-				  NFP_PCIE_QUEUE(start_q),
-				  rx_area_sz, &pf->rx_area);
-	if (IS_ERR(rx_bar)) {
-		nfp_err(pf->cpp, "Failed to map RX area.\n");
-		err = PTR_ERR(rx_bar);
-		goto err_unmap_tx;
-	}
+	err = nfp_net_pf_app_init(pf, qc_bar, stride);
+	if (err)
+		goto err_unmap;
 
 	pf->ddir = nfp_net_debugfs_device_add(pf->pdev);
 
-	err = nfp_net_pf_spawn_netdevs(pf, ctrl_bar, tx_bar, rx_bar,
-				       stride, &fw_ver);
+	/* Allocate the vnics and do basic init */
+	err = nfp_net_pf_alloc_vnics(pf, ctrl_bar, qc_bar, stride);
 	if (err)
 		goto err_clean_ddir;
 
-	mutex_unlock(&pf->port_lock);
+	err = nfp_net_pf_alloc_irqs(pf);
+	if (err)
+		goto err_free_vnics;
+
+	err = nfp_net_pf_app_start(pf);
+	if (err)
+		goto err_free_irqs;
+
+	err = nfp_net_pf_init_vnics(pf);
+	if (err)
+		goto err_stop_app;
+
+	mutex_unlock(&pf->lock);
 
 	return 0;
 
+err_stop_app:
+	nfp_net_pf_app_stop(pf);
+err_free_irqs:
+	nfp_net_pf_free_irqs(pf);
+err_free_vnics:
+	nfp_net_pf_free_vnics(pf);
 err_clean_ddir:
 	nfp_net_debugfs_dir_clean(&pf->ddir);
-	nfp_cpp_area_release_free(pf->rx_area);
-err_unmap_tx:
-	nfp_cpp_area_release_free(pf->tx_area);
-err_ctrl_unmap:
-	nfp_cpp_area_release_free(pf->ctrl_area);
+	nfp_net_pf_app_clean(pf);
+err_unmap:
+	nfp_net_pci_unmap_mem(pf);
 err_unlock:
-	mutex_unlock(&pf->port_lock);
+	mutex_unlock(&pf->lock);
+	cancel_work_sync(&pf->port_refresh_work);
 	return err;
 }
 
@@ -694,21 +799,19 @@ void nfp_net_pci_remove(struct nfp_pf *pf)
 {
 	struct nfp_net *nn;
 
-	mutex_lock(&pf->port_lock);
-	if (list_empty(&pf->ports))
+	mutex_lock(&pf->lock);
+	if (list_empty(&pf->vnics))
 		goto out;
 
-	list_for_each_entry(nn, &pf->ports, port_list) {
-		nfp_net_debugfs_dir_clean(&nn->debugfs_dir);
-
-		nfp_net_netdev_clean(nn->dp.netdev);
-	}
+	list_for_each_entry(nn, &pf->vnics, vnic_list)
+		if (nfp_net_is_data_vnic(nn))
+			nfp_net_pf_clean_vnic(pf, nn);
 
-	nfp_net_pf_free_netdevs(pf);
+	nfp_net_pf_free_vnics(pf);
 
 	nfp_net_pci_remove_finish(pf);
 out:
-	mutex_unlock(&pf->port_lock);
+	mutex_unlock(&pf->lock);
 
 	cancel_work_sync(&pf->port_refresh_work);
 }
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_repr.c b/drivers/net/ethernet/netronome/nfp/nfp_net_repr.c
new file mode 100644
index 000000000000..8ec5474f4b18
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_repr.c
@@ -0,0 +1,396 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/etherdevice.h>
+#include <linux/io-64-nonatomic-hi-lo.h>
+#include <linux/lockdep.h>
+#include <net/dst_metadata.h>
+#include <net/switchdev.h>
+
+#include "nfpcore/nfp_cpp.h"
+#include "nfpcore/nfp_nsp.h"
+#include "nfp_app.h"
+#include "nfp_main.h"
+#include "nfp_net_ctrl.h"
+#include "nfp_net_repr.h"
+#include "nfp_port.h"
+
+static void
+nfp_repr_inc_tx_stats(struct net_device *netdev, unsigned int len,
+		      int tx_status)
+{
+	struct nfp_repr *repr = netdev_priv(netdev);
+	struct nfp_repr_pcpu_stats *stats;
+
+	if (unlikely(tx_status != NET_XMIT_SUCCESS &&
+		     tx_status != NET_XMIT_CN)) {
+		this_cpu_inc(repr->stats->tx_drops);
+		return;
+	}
+
+	stats = this_cpu_ptr(repr->stats);
+	u64_stats_update_begin(&stats->syncp);
+	stats->tx_packets++;
+	stats->tx_bytes += len;
+	u64_stats_update_end(&stats->syncp);
+}
+
+void nfp_repr_inc_rx_stats(struct net_device *netdev, unsigned int len)
+{
+	struct nfp_repr *repr = netdev_priv(netdev);
+	struct nfp_repr_pcpu_stats *stats;
+
+	stats = this_cpu_ptr(repr->stats);
+	u64_stats_update_begin(&stats->syncp);
+	stats->rx_packets++;
+	stats->rx_bytes += len;
+	u64_stats_update_end(&stats->syncp);
+}
+
+static void
+nfp_repr_phy_port_get_stats64(const struct nfp_app *app, u8 phy_port,
+			      struct rtnl_link_stats64 *stats)
+{
+	u8 __iomem *mem;
+
+	mem = app->pf->mac_stats_mem + phy_port * NFP_MAC_STATS_SIZE;
+
+	/* TX and RX stats are flipped as we are returning the stats as seen
+	 * at the switch port corresponding to the phys port.
+	 */
+	stats->tx_packets = readq(mem + NFP_MAC_STATS_RX_FRAMES_RECEIVED_OK);
+	stats->tx_bytes = readq(mem + NFP_MAC_STATS_RX_IN_OCTETS);
+	stats->tx_dropped = readq(mem + NFP_MAC_STATS_RX_IN_ERRORS);
+
+	stats->rx_packets = readq(mem + NFP_MAC_STATS_TX_FRAMES_TRANSMITTED_OK);
+	stats->rx_bytes = readq(mem + NFP_MAC_STATS_TX_OUT_OCTETS);
+	stats->rx_dropped = readq(mem + NFP_MAC_STATS_TX_OUT_ERRORS);
+}
+
+static void
+nfp_repr_vf_get_stats64(const struct nfp_app *app, u8 vf,
+			struct rtnl_link_stats64 *stats)
+{
+	u8 __iomem *mem;
+
+	mem = app->pf->vf_cfg_mem + vf * NFP_NET_CFG_BAR_SZ;
+
+	/* TX and RX stats are flipped as we are returning the stats as seen
+	 * at the switch port corresponding to the VF.
+	 */
+	stats->tx_packets = readq(mem + NFP_NET_CFG_STATS_RX_FRAMES);
+	stats->tx_bytes = readq(mem + NFP_NET_CFG_STATS_RX_OCTETS);
+	stats->tx_dropped = readq(mem + NFP_NET_CFG_STATS_RX_DISCARDS);
+
+	stats->rx_packets = readq(mem + NFP_NET_CFG_STATS_TX_FRAMES);
+	stats->rx_bytes = readq(mem + NFP_NET_CFG_STATS_TX_OCTETS);
+	stats->rx_dropped = readq(mem + NFP_NET_CFG_STATS_TX_DISCARDS);
+}
+
+static void
+nfp_repr_pf_get_stats64(const struct nfp_app *app, u8 pf,
+			struct rtnl_link_stats64 *stats)
+{
+	u8 __iomem *mem;
+
+	if (pf)
+		return;
+
+	mem = nfp_cpp_area_iomem(app->pf->data_vnic_bar);
+
+	stats->tx_packets = readq(mem + NFP_NET_CFG_STATS_RX_FRAMES);
+	stats->tx_bytes = readq(mem + NFP_NET_CFG_STATS_RX_OCTETS);
+	stats->tx_dropped = readq(mem + NFP_NET_CFG_STATS_RX_DISCARDS);
+
+	stats->rx_packets = readq(mem + NFP_NET_CFG_STATS_TX_FRAMES);
+	stats->rx_bytes = readq(mem + NFP_NET_CFG_STATS_TX_OCTETS);
+	stats->rx_dropped = readq(mem + NFP_NET_CFG_STATS_TX_DISCARDS);
+}
+
+static void
+nfp_repr_get_stats64(struct net_device *netdev, struct rtnl_link_stats64 *stats)
+{
+	struct nfp_repr *repr = netdev_priv(netdev);
+	struct nfp_eth_table_port *eth_port;
+	struct nfp_app *app = repr->app;
+
+	if (WARN_ON(!repr->port))
+		return;
+
+	switch (repr->port->type) {
+	case NFP_PORT_PHYS_PORT:
+		eth_port = __nfp_port_get_eth_port(repr->port);
+		if (!eth_port)
+			break;
+		nfp_repr_phy_port_get_stats64(app, eth_port->index, stats);
+		break;
+	case NFP_PORT_PF_PORT:
+		nfp_repr_pf_get_stats64(app, repr->port->pf_id, stats);
+		break;
+	case NFP_PORT_VF_PORT:
+		nfp_repr_vf_get_stats64(app, repr->port->vf_id, stats);
+	default:
+		break;
+	}
+}
+
+static bool
+nfp_repr_has_offload_stats(const struct net_device *dev, int attr_id)
+{
+	switch (attr_id) {
+	case IFLA_OFFLOAD_XSTATS_CPU_HIT:
+		return true;
+	}
+
+	return false;
+}
+
+static int
+nfp_repr_get_host_stats64(const struct net_device *netdev,
+			  struct rtnl_link_stats64 *stats)
+{
+	struct nfp_repr *repr = netdev_priv(netdev);
+	int i;
+
+	for_each_possible_cpu(i) {
+		u64 tbytes, tpkts, tdrops, rbytes, rpkts;
+		struct nfp_repr_pcpu_stats *repr_stats;
+		unsigned int start;
+
+		repr_stats = per_cpu_ptr(repr->stats, i);
+		do {
+			start = u64_stats_fetch_begin_irq(&repr_stats->syncp);
+			tbytes = repr_stats->tx_bytes;
+			tpkts = repr_stats->tx_packets;
+			tdrops = repr_stats->tx_drops;
+			rbytes = repr_stats->rx_bytes;
+			rpkts = repr_stats->rx_packets;
+		} while (u64_stats_fetch_retry_irq(&repr_stats->syncp, start));
+
+		stats->tx_bytes += tbytes;
+		stats->tx_packets += tpkts;
+		stats->tx_dropped += tdrops;
+		stats->rx_bytes += rbytes;
+		stats->rx_packets += rpkts;
+	}
+
+	return 0;
+}
+
+static int
+nfp_repr_get_offload_stats(int attr_id, const struct net_device *dev,
+			   void *stats)
+{
+	switch (attr_id) {
+	case IFLA_OFFLOAD_XSTATS_CPU_HIT:
+		return nfp_repr_get_host_stats64(dev, stats);
+	}
+
+	return -EINVAL;
+}
+
+static netdev_tx_t nfp_repr_xmit(struct sk_buff *skb, struct net_device *netdev)
+{
+	struct nfp_repr *repr = netdev_priv(netdev);
+	unsigned int len = skb->len;
+	int ret;
+
+	skb_dst_drop(skb);
+	dst_hold((struct dst_entry *)repr->dst);
+	skb_dst_set(skb, (struct dst_entry *)repr->dst);
+	skb->dev = repr->dst->u.port_info.lower_dev;
+
+	ret = dev_queue_xmit(skb);
+	nfp_repr_inc_tx_stats(netdev, len, ret);
+
+	return ret;
+}
+
+static int nfp_repr_stop(struct net_device *netdev)
+{
+	struct nfp_repr *repr = netdev_priv(netdev);
+
+	return nfp_app_repr_stop(repr->app, repr);
+}
+
+static int nfp_repr_open(struct net_device *netdev)
+{
+	struct nfp_repr *repr = netdev_priv(netdev);
+
+	return nfp_app_repr_open(repr->app, repr);
+}
+
+const struct net_device_ops nfp_repr_netdev_ops = {
+	.ndo_open		= nfp_repr_open,
+	.ndo_stop		= nfp_repr_stop,
+	.ndo_start_xmit		= nfp_repr_xmit,
+	.ndo_get_stats64	= nfp_repr_get_stats64,
+	.ndo_has_offload_stats	= nfp_repr_has_offload_stats,
+	.ndo_get_offload_stats	= nfp_repr_get_offload_stats,
+	.ndo_get_phys_port_name	= nfp_port_get_phys_port_name,
+	.ndo_setup_tc		= nfp_port_setup_tc,
+};
+
+static void nfp_repr_clean(struct nfp_repr *repr)
+{
+	unregister_netdev(repr->netdev);
+	dst_release((struct dst_entry *)repr->dst);
+	nfp_port_free(repr->port);
+}
+
+static struct lock_class_key nfp_repr_netdev_xmit_lock_key;
+static struct lock_class_key nfp_repr_netdev_addr_lock_key;
+
+static void nfp_repr_set_lockdep_class_one(struct net_device *dev,
+					   struct netdev_queue *txq,
+					   void *_unused)
+{
+	lockdep_set_class(&txq->_xmit_lock, &nfp_repr_netdev_xmit_lock_key);
+}
+
+static void nfp_repr_set_lockdep_class(struct net_device *dev)
+{
+	lockdep_set_class(&dev->addr_list_lock, &nfp_repr_netdev_addr_lock_key);
+	netdev_for_each_tx_queue(dev, nfp_repr_set_lockdep_class_one, NULL);
+}
+
+int nfp_repr_init(struct nfp_app *app, struct net_device *netdev,
+		  u32 cmsg_port_id, struct nfp_port *port,
+		  struct net_device *pf_netdev)
+{
+	struct nfp_repr *repr = netdev_priv(netdev);
+	int err;
+
+	nfp_repr_set_lockdep_class(netdev);
+
+	repr->port = port;
+	repr->dst = metadata_dst_alloc(0, METADATA_HW_PORT_MUX, GFP_KERNEL);
+	if (!repr->dst)
+		return -ENOMEM;
+	repr->dst->u.port_info.port_id = cmsg_port_id;
+	repr->dst->u.port_info.lower_dev = pf_netdev;
+
+	netdev->netdev_ops = &nfp_repr_netdev_ops;
+	SWITCHDEV_SET_OPS(netdev, &nfp_port_switchdev_ops);
+
+	if (nfp_app_has_tc(app)) {
+		netdev->features |= NETIF_F_HW_TC;
+		netdev->hw_features |= NETIF_F_HW_TC;
+	}
+
+	err = register_netdev(netdev);
+	if (err)
+		goto err_clean;
+
+	return 0;
+
+err_clean:
+	dst_release((struct dst_entry *)repr->dst);
+	return err;
+}
+
+static void nfp_repr_free(struct nfp_repr *repr)
+{
+	free_percpu(repr->stats);
+	free_netdev(repr->netdev);
+}
+
+struct net_device *nfp_repr_alloc(struct nfp_app *app)
+{
+	struct net_device *netdev;
+	struct nfp_repr *repr;
+
+	netdev = alloc_etherdev(sizeof(*repr));
+	if (!netdev)
+		return NULL;
+
+	repr = netdev_priv(netdev);
+	repr->netdev = netdev;
+	repr->app = app;
+
+	repr->stats = netdev_alloc_pcpu_stats(struct nfp_repr_pcpu_stats);
+	if (!repr->stats)
+		goto err_free_netdev;
+
+	return netdev;
+
+err_free_netdev:
+	free_netdev(netdev);
+	return NULL;
+}
+
+static void nfp_repr_clean_and_free(struct nfp_repr *repr)
+{
+	nfp_info(repr->app->cpp, "Destroying Representor(%s)\n",
+		 repr->netdev->name);
+	nfp_repr_clean(repr);
+	nfp_repr_free(repr);
+}
+
+void nfp_reprs_clean_and_free(struct nfp_reprs *reprs)
+{
+	unsigned int i;
+
+	for (i = 0; i < reprs->num_reprs; i++)
+		if (reprs->reprs[i])
+			nfp_repr_clean_and_free(netdev_priv(reprs->reprs[i]));
+
+	kfree(reprs);
+}
+
+void
+nfp_reprs_clean_and_free_by_type(struct nfp_app *app,
+				 enum nfp_repr_type type)
+{
+	struct nfp_reprs *reprs;
+
+	reprs = nfp_app_reprs_set(app, type, NULL);
+	if (!reprs)
+		return;
+
+	synchronize_rcu();
+	nfp_reprs_clean_and_free(reprs);
+}
+
+struct nfp_reprs *nfp_reprs_alloc(unsigned int num_reprs)
+{
+	struct nfp_reprs *reprs;
+
+	reprs = kzalloc(sizeof(*reprs) +
+			num_reprs * sizeof(struct net_device *), GFP_KERNEL);
+	if (!reprs)
+		return NULL;
+	reprs->num_reprs = num_reprs;
+
+	return reprs;
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_repr.h b/drivers/net/ethernet/netronome/nfp/nfp_net_repr.h
new file mode 100644
index 000000000000..32179cad062a
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_repr.h
@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef NFP_NET_REPR_H
+#define NFP_NET_REPR_H
+
+struct metadata_dst;
+struct nfp_net;
+struct nfp_port;
+
+#include <net/dst_metadata.h>
+
+/**
+ * struct nfp_reprs - container for representor netdevs
+ * @num_reprs:	Number of elements in reprs array
+ * @reprs:	Array of representor netdevs
+ */
+struct nfp_reprs {
+	unsigned int num_reprs;
+	struct net_device *reprs[0];
+};
+
+/**
+ * struct nfp_repr_pcpu_stats
+ * @rx_packets:	Received packets
+ * @rx_bytes:	Received bytes
+ * @tx_packets:	Transmitted packets
+ * @tx_bytes:	Transmitted dropped
+ * @tx_drops:	Packets dropped on transmit
+ * @syncp:	Reference count
+ */
+struct nfp_repr_pcpu_stats {
+	u64 rx_packets;
+	u64 rx_bytes;
+	u64 tx_packets;
+	u64 tx_bytes;
+	u64 tx_drops;
+	struct u64_stats_sync syncp;
+};
+
+/**
+ * struct nfp_repr - priv data for representor netdevs
+ * @netdev:	Back pointer to netdev
+ * @dst:	Destination for packet TX
+ * @port:	Port of representor
+ * @app:	APP handle
+ * @stats:	Statistic of packets hitting CPU
+ */
+struct nfp_repr {
+	struct net_device *netdev;
+	struct metadata_dst *dst;
+	struct nfp_port *port;
+	struct nfp_app *app;
+	struct nfp_repr_pcpu_stats __percpu *stats;
+};
+
+/**
+ * enum nfp_repr_type - type of representor
+ * @NFP_REPR_TYPE_PHYS_PORT:	external NIC port
+ * @NFP_REPR_TYPE_PF:		physical function
+ * @NFP_REPR_TYPE_VF:		virtual function
+ */
+enum nfp_repr_type {
+	NFP_REPR_TYPE_PHYS_PORT,
+	NFP_REPR_TYPE_PF,
+	NFP_REPR_TYPE_VF,
+
+	__NFP_REPR_TYPE_MAX,
+};
+#define NFP_REPR_TYPE_MAX (__NFP_REPR_TYPE_MAX - 1)
+
+extern const struct net_device_ops nfp_repr_netdev_ops;
+
+static inline bool nfp_netdev_is_nfp_repr(struct net_device *netdev)
+{
+	return netdev->netdev_ops == &nfp_repr_netdev_ops;
+}
+
+static inline int nfp_repr_get_port_id(struct net_device *netdev)
+{
+	struct nfp_repr *priv = netdev_priv(netdev);
+
+	return priv->dst->u.port_info.port_id;
+}
+
+void nfp_repr_inc_rx_stats(struct net_device *netdev, unsigned int len);
+int nfp_repr_init(struct nfp_app *app, struct net_device *netdev,
+		  u32 cmsg_port_id, struct nfp_port *port,
+		  struct net_device *pf_netdev);
+struct net_device *nfp_repr_alloc(struct nfp_app *app);
+void
+nfp_reprs_clean_and_free(struct nfp_reprs *reprs);
+void
+nfp_reprs_clean_and_free_by_type(struct nfp_app *app,
+				 enum nfp_repr_type type);
+struct nfp_reprs *nfp_reprs_alloc(unsigned int num_reprs);
+
+#endif /* NFP_NET_REPR_H */
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c b/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c
index 86e61be6f35c..c879626e035b 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c
+++ b/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c
@@ -161,7 +161,7 @@ static int nfp_netvf_pci_probe(struct pci_dev *pdev,
 		dev_warn(&pdev->dev, "OBSOLETE Firmware detected - VF isolation not available\n");
 	} else {
 		switch (fw_ver.major) {
-		case 1 ... 4:
+		case 1 ... 5:
 			stride = 4;
 			tx_bar_no = NFP_NET_Q0_BAR;
 			rx_bar_no = tx_bar_no;
@@ -202,7 +202,7 @@ static int nfp_netvf_pci_probe(struct pci_dev *pdev,
 	rx_bar_off = NFP_PCIE_QUEUE(startq);
 
 	/* Allocate and initialise the netdev */
-	nn = nfp_net_netdev_alloc(pdev, max_tx_rings, max_rx_rings);
+	nn = nfp_net_alloc(pdev, true, max_tx_rings, max_rx_rings);
 	if (IS_ERR(nn)) {
 		err = PTR_ERR(nn);
 		goto err_ctrl_unmap;
@@ -267,7 +267,7 @@ static int nfp_netvf_pci_probe(struct pci_dev *pdev,
 	nfp_netvf_get_mac_addr(nn);
 
 	num_irqs = nfp_net_irqs_alloc(pdev, vf->irq_entries,
-				      NFP_NET_MIN_PORT_IRQS,
+				      NFP_NET_MIN_VNIC_IRQS,
 				      NFP_NET_NON_Q_VECTORS +
 				      nn->dp.num_r_vecs);
 	if (!num_irqs) {
@@ -283,13 +283,13 @@ static int nfp_netvf_pci_probe(struct pci_dev *pdev,
 	 */
 	nn->me_freq_mhz = 1200;
 
-	err = nfp_net_netdev_init(nn->dp.netdev);
+	err = nfp_net_init(nn);
 	if (err)
 		goto err_irqs_disable;
 
 	nfp_net_info(nn);
 	vf->ddir = nfp_net_debugfs_device_add(pdev);
-	nfp_net_debugfs_port_add(nn, vf->ddir, 0);
+	nfp_net_debugfs_vnic_add(nn, vf->ddir, 0);
 
 	return 0;
 
@@ -304,7 +304,7 @@ err_unmap_tx:
 	else
 		iounmap(vf->q_bar);
 err_netdev_free:
-	nfp_net_netdev_free(nn);
+	nfp_net_free(nn);
 err_ctrl_unmap:
 	iounmap(ctrl_bar);
 err_pci_regions:
@@ -328,7 +328,7 @@ static void nfp_netvf_pci_remove(struct pci_dev *pdev)
 	nfp_net_debugfs_dir_clean(&nn->debugfs_dir);
 	nfp_net_debugfs_dir_clean(&vf->ddir);
 
-	nfp_net_netdev_clean(nn->dp.netdev);
+	nfp_net_clean(nn);
 
 	nfp_net_irqs_disable(pdev);
 
@@ -340,7 +340,7 @@ static void nfp_netvf_pci_remove(struct pci_dev *pdev)
 	}
 	iounmap(nn->dp.ctrl_bar);
 
-	nfp_net_netdev_free(nn);
+	nfp_net_free(nn);
 
 	pci_release_regions(pdev);
 	pci_disable_device(pdev);
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_port.c b/drivers/net/ethernet/netronome/nfp/nfp_port.c
new file mode 100644
index 000000000000..e42644dbb865
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_port.c
@@ -0,0 +1,233 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/lockdep.h>
+#include <net/switchdev.h>
+
+#include "nfpcore/nfp_cpp.h"
+#include "nfpcore/nfp_nsp.h"
+#include "nfp_app.h"
+#include "nfp_main.h"
+#include "nfp_net.h"
+#include "nfp_port.h"
+
+struct nfp_port *nfp_port_from_netdev(struct net_device *netdev)
+{
+	if (nfp_netdev_is_nfp_net(netdev)) {
+		struct nfp_net *nn = netdev_priv(netdev);
+
+		return nn->port;
+	}
+
+	if (nfp_netdev_is_nfp_repr(netdev)) {
+		struct nfp_repr *repr = netdev_priv(netdev);
+
+		return repr->port;
+	}
+
+	WARN(1, "Unknown netdev type for nfp_port\n");
+
+	return NULL;
+}
+
+static int
+nfp_port_attr_get(struct net_device *netdev, struct switchdev_attr *attr)
+{
+	struct nfp_port *port;
+
+	port = nfp_port_from_netdev(netdev);
+	if (!port)
+		return -EOPNOTSUPP;
+
+	switch (attr->id) {
+	case SWITCHDEV_ATTR_ID_PORT_PARENT_ID: {
+		const u8 *serial;
+		/* N.B: attr->u.ppid.id is binary data */
+		attr->u.ppid.id_len = nfp_cpp_serial(port->app->cpp, &serial);
+		memcpy(&attr->u.ppid.id, serial, attr->u.ppid.id_len);
+		break;
+	}
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+const struct switchdev_ops nfp_port_switchdev_ops = {
+	.switchdev_port_attr_get	= nfp_port_attr_get,
+};
+
+int nfp_port_setup_tc(struct net_device *netdev, u32 handle, u32 chain_index,
+		      __be16 proto, struct tc_to_netdev *tc)
+{
+	struct nfp_port *port;
+
+	if (chain_index)
+		return -EOPNOTSUPP;
+
+	port = nfp_port_from_netdev(netdev);
+	if (!port)
+		return -EOPNOTSUPP;
+
+	return nfp_app_setup_tc(port->app, netdev, handle, proto, tc);
+}
+
+struct nfp_port *
+nfp_port_from_id(struct nfp_pf *pf, enum nfp_port_type type, unsigned int id)
+{
+	struct nfp_port *port;
+
+	lockdep_assert_held(&pf->lock);
+
+	if (type != NFP_PORT_PHYS_PORT)
+		return NULL;
+
+	list_for_each_entry(port, &pf->ports, port_list)
+		if (port->eth_id == id)
+			return port;
+
+	return NULL;
+}
+
+struct nfp_eth_table_port *__nfp_port_get_eth_port(struct nfp_port *port)
+{
+	if (!port)
+		return NULL;
+	if (port->type != NFP_PORT_PHYS_PORT)
+		return NULL;
+
+	return port->eth_port;
+}
+
+struct nfp_eth_table_port *nfp_port_get_eth_port(struct nfp_port *port)
+{
+	if (!__nfp_port_get_eth_port(port))
+		return NULL;
+
+	if (test_bit(NFP_PORT_CHANGED, &port->flags))
+		if (nfp_net_refresh_eth_port(port))
+			return NULL;
+
+	return __nfp_port_get_eth_port(port);
+}
+
+int
+nfp_port_get_phys_port_name(struct net_device *netdev, char *name, size_t len)
+{
+	struct nfp_eth_table_port *eth_port;
+	struct nfp_port *port;
+	int n;
+
+	port = nfp_port_from_netdev(netdev);
+	if (!port)
+		return -EOPNOTSUPP;
+
+	switch (port->type) {
+	case NFP_PORT_PHYS_PORT:
+		eth_port = __nfp_port_get_eth_port(port);
+		if (!eth_port)
+			return -EOPNOTSUPP;
+
+		if (!eth_port->is_split)
+			n = snprintf(name, len, "p%d", eth_port->label_port);
+		else
+			n = snprintf(name, len, "p%ds%d", eth_port->label_port,
+				     eth_port->label_subport);
+		break;
+	case NFP_PORT_PF_PORT:
+		n = snprintf(name, len, "pf%d", port->pf_id);
+		break;
+	case NFP_PORT_VF_PORT:
+		n = snprintf(name, len, "pf%dvf%d", port->pf_id, port->vf_id);
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	if (n >= len)
+		return -EINVAL;
+
+	return 0;
+}
+
+int nfp_port_init_phy_port(struct nfp_pf *pf, struct nfp_app *app,
+			   struct nfp_port *port, unsigned int id)
+{
+	/* Check if vNIC has external port associated and cfg is OK */
+	if (!pf->eth_tbl || id >= pf->eth_tbl->count) {
+		nfp_err(app->cpp,
+			"NSP port entries don't match vNICs (no entry %d)\n",
+			id);
+		return -EINVAL;
+	}
+	if (pf->eth_tbl->ports[id].override_changed) {
+		nfp_warn(app->cpp,
+			 "Config changed for port #%d, reboot required before port will be operational\n",
+			 pf->eth_tbl->ports[id].index);
+		port->type = NFP_PORT_INVALID;
+		return 0;
+	}
+
+	port->eth_port = &pf->eth_tbl->ports[id];
+	port->eth_id = pf->eth_tbl->ports[id].index;
+
+	return 0;
+}
+
+struct nfp_port *
+nfp_port_alloc(struct nfp_app *app, enum nfp_port_type type,
+	       struct net_device *netdev)
+{
+	struct nfp_port *port;
+
+	port = kzalloc(sizeof(*port), GFP_KERNEL);
+	if (!port)
+		return ERR_PTR(-ENOMEM);
+
+	port->netdev = netdev;
+	port->type = type;
+	port->app = app;
+
+	list_add_tail(&port->port_list, &app->pf->ports);
+
+	return port;
+}
+
+void nfp_port_free(struct nfp_port *port)
+{
+	if (!port)
+		return;
+	list_del(&port->port_list);
+	kfree(port);
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_port.h b/drivers/net/ethernet/netronome/nfp/nfp_port.h
new file mode 100644
index 000000000000..a33d22e18f94
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_port.h
@@ -0,0 +1,199 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _NFP_PORT_H_
+#define _NFP_PORT_H_
+
+#include <net/devlink.h>
+
+struct tc_to_netdev;
+struct net_device;
+struct nfp_app;
+struct nfp_pf;
+struct nfp_port;
+
+/**
+ * enum nfp_port_type - type of port NFP can switch traffic to
+ * @NFP_PORT_INVALID:	port is invalid, %NFP_PORT_PHYS_PORT transitions to this
+ *			state when port disappears because of FW fault or config
+ *			change
+ * @NFP_PORT_PHYS_PORT:	external NIC port
+ * @NFP_PORT_PF_PORT:	logical port of PCI PF
+ * @NFP_PORT_VF_PORT:	logical port of PCI VF
+ */
+enum nfp_port_type {
+	NFP_PORT_INVALID,
+	NFP_PORT_PHYS_PORT,
+	NFP_PORT_PF_PORT,
+	NFP_PORT_VF_PORT,
+};
+
+/**
+ * enum nfp_port_flags - port flags (can be type-specific)
+ * @NFP_PORT_CHANGED:	port state has changed since last eth table refresh;
+ *			for NFP_PORT_PHYS_PORT, never set otherwise; must hold
+ *			rtnl_lock to clear
+ */
+enum nfp_port_flags {
+	NFP_PORT_CHANGED = 0,
+};
+
+/**
+ * struct nfp_port - structure representing NFP port
+ * @netdev:	backpointer to associated netdev
+ * @type:	what port type does the entity represent
+ * @flags:	port flags
+ * @app:	backpointer to the app structure
+ * @dl_port:	devlink port structure
+ * @eth_id:	for %NFP_PORT_PHYS_PORT port ID in NFP enumeration scheme
+ * @eth_port:	for %NFP_PORT_PHYS_PORT translated ETH Table port entry
+ * @pf_id:	for %NFP_PORT_PF_PORT, %NFP_PORT_VF_PORT ID of the PCI PF (0-3)
+ * @vf_id:	for %NFP_PORT_VF_PORT ID of the PCI VF within @pf_id
+ * @port_list:	entry on pf's list of ports
+ */
+struct nfp_port {
+	struct net_device *netdev;
+	enum nfp_port_type type;
+
+	unsigned long flags;
+
+	struct nfp_app *app;
+
+	struct devlink_port dl_port;
+
+	union {
+		/* NFP_PORT_PHYS_PORT */
+		struct {
+			unsigned int eth_id;
+			struct nfp_eth_table_port *eth_port;
+		};
+		/* NFP_PORT_PF_PORT, NFP_PORT_VF_PORT */
+		struct {
+			unsigned int pf_id;
+			unsigned int vf_id;
+		};
+	};
+
+	struct list_head port_list;
+};
+
+extern const struct switchdev_ops nfp_port_switchdev_ops;
+
+int nfp_port_setup_tc(struct net_device *netdev, u32 handle, u32 chain_index,
+		      __be16 proto, struct tc_to_netdev *tc);
+
+struct nfp_port *nfp_port_from_netdev(struct net_device *netdev);
+struct nfp_port *
+nfp_port_from_id(struct nfp_pf *pf, enum nfp_port_type type, unsigned int id);
+struct nfp_eth_table_port *__nfp_port_get_eth_port(struct nfp_port *port);
+struct nfp_eth_table_port *nfp_port_get_eth_port(struct nfp_port *port);
+
+int
+nfp_port_get_phys_port_name(struct net_device *netdev, char *name, size_t len);
+
+struct nfp_port *
+nfp_port_alloc(struct nfp_app *app, enum nfp_port_type type,
+	       struct net_device *netdev);
+void nfp_port_free(struct nfp_port *port);
+
+int nfp_port_init_phy_port(struct nfp_pf *pf, struct nfp_app *app,
+			   struct nfp_port *port, unsigned int id);
+
+int nfp_net_refresh_eth_port(struct nfp_port *port);
+void nfp_net_refresh_port_table(struct nfp_port *port);
+int nfp_net_refresh_port_table_sync(struct nfp_pf *pf);
+
+int nfp_devlink_port_register(struct nfp_app *app, struct nfp_port *port);
+void nfp_devlink_port_unregister(struct nfp_port *port);
+
+/**
+ * Mac stats (0x0000 - 0x0200)
+ * all counters are 64bit.
+ */
+#define NFP_MAC_STATS_BASE                0x0000
+#define NFP_MAC_STATS_SIZE                0x0200
+
+#define NFP_MAC_STATS_RX_IN_OCTETS			(NFP_MAC_STATS_BASE + 0x000)
+#define NFP_MAC_STATS_RX_FRAME_TOO_LONG_ERRORS		(NFP_MAC_STATS_BASE + 0x010)
+#define NFP_MAC_STATS_RX_RANGE_LENGTH_ERRORS		(NFP_MAC_STATS_BASE + 0x018)
+#define NFP_MAC_STATS_RX_VLAN_REVEIVE_OK		(NFP_MAC_STATS_BASE + 0x020)
+#define NFP_MAC_STATS_RX_IN_ERRORS			(NFP_MAC_STATS_BASE + 0x028)
+#define NFP_MAC_STATS_RX_IN_BROADCAST_PKTS		(NFP_MAC_STATS_BASE + 0x030)
+#define NFP_MAC_STATS_RX_STATS_DROP_EVENTS		(NFP_MAC_STATS_BASE + 0x038)
+#define NFP_MAC_STATS_RX_ALIGNMENT_ERRORS		(NFP_MAC_STATS_BASE + 0x040)
+#define NFP_MAC_STATS_RX_PAUSE_MAC_CTRL_FRAMES		(NFP_MAC_STATS_BASE + 0x048)
+#define NFP_MAC_STATS_RX_FRAMES_RECEIVED_OK		(NFP_MAC_STATS_BASE + 0x050)
+#define NFP_MAC_STATS_RX_FRAME_CHECK_SEQUENCE_ERRORS	(NFP_MAC_STATS_BASE + 0x058)
+#define NFP_MAC_STATS_RX_UNICAST_PKTS			(NFP_MAC_STATS_BASE + 0x060)
+#define NFP_MAC_STATS_RX_MULTICAST_PKTS			(NFP_MAC_STATS_BASE + 0x068)
+#define NFP_MAC_STATS_RX_STATS_PKTS			(NFP_MAC_STATS_BASE + 0x070)
+#define NFP_MAC_STATS_RX_STATS_UNDERSIZE_PKTS		(NFP_MAC_STATS_BASE + 0x078)
+#define NFP_MAC_STATS_RX_STATS_PKTS_64_OCTETS		(NFP_MAC_STATS_BASE + 0x080)
+#define NFP_MAC_STATS_RX_STATS_PKTS_65_TO_127_OCTETS	(NFP_MAC_STATS_BASE + 0x088)
+#define NFP_MAC_STATS_RX_STATS_PKTS_512_TO_1023_OCTETS	(NFP_MAC_STATS_BASE + 0x090)
+#define NFP_MAC_STATS_RX_STATS_PKTS_1024_TO_1518_OCTETS	(NFP_MAC_STATS_BASE + 0x098)
+#define NFP_MAC_STATS_RX_STATS_JABBERS			(NFP_MAC_STATS_BASE + 0x0a0)
+#define NFP_MAC_STATS_RX_STATS_FRAGMENTS		(NFP_MAC_STATS_BASE + 0x0a8)
+#define NFP_MAC_STATS_RX_PAUSE_FRAMES_CLASS2		(NFP_MAC_STATS_BASE + 0x0b0)
+#define NFP_MAC_STATS_RX_PAUSE_FRAMES_CLASS3		(NFP_MAC_STATS_BASE + 0x0b8)
+#define NFP_MAC_STATS_RX_STATS_PKTS_128_TO_255_OCTETS	(NFP_MAC_STATS_BASE + 0x0c0)
+#define NFP_MAC_STATS_RX_STATS_PKTS_256_TO_511_OCTETS	(NFP_MAC_STATS_BASE + 0x0c8)
+#define NFP_MAC_STATS_RX_STATS_PKTS_1519_TO_MAX_OCTETS	(NFP_MAC_STATS_BASE + 0x0d0)
+#define NFP_MAC_STATS_RX_OVERSIZE_PKTS			(NFP_MAC_STATS_BASE + 0x0d8)
+#define NFP_MAC_STATS_RX_PAUSE_FRAMES_CLASS0		(NFP_MAC_STATS_BASE + 0x0e0)
+#define NFP_MAC_STATS_RX_PAUSE_FRAMES_CLASS1		(NFP_MAC_STATS_BASE + 0x0e8)
+#define NFP_MAC_STATS_RX_PAUSE_FRAMES_CLASS4		(NFP_MAC_STATS_BASE + 0x0f0)
+#define NFP_MAC_STATS_RX_PAUSE_FRAMES_CLASS5		(NFP_MAC_STATS_BASE + 0x0f8)
+#define NFP_MAC_STATS_RX_PAUSE_FRAMES_CLASS6		(NFP_MAC_STATS_BASE + 0x100)
+#define NFP_MAC_STATS_RX_PAUSE_FRAMES_CLASS7		(NFP_MAC_STATS_BASE + 0x108)
+#define NFP_MAC_STATS_RX_MAC_CTRL_FRAMES_RECEIVED	(NFP_MAC_STATS_BASE + 0x110)
+#define NFP_MAC_STATS_RX_MAC_HEAD_DROP			(NFP_MAC_STATS_BASE + 0x118)
+
+#define NFP_MAC_STATS_TX_QUEUE_DROP			(NFP_MAC_STATS_BASE + 0x138)
+#define NFP_MAC_STATS_TX_OUT_OCTETS			(NFP_MAC_STATS_BASE + 0x140)
+#define NFP_MAC_STATS_TX_VLAN_TRANSMITTED_OK		(NFP_MAC_STATS_BASE + 0x150)
+#define NFP_MAC_STATS_TX_OUT_ERRORS			(NFP_MAC_STATS_BASE + 0x158)
+#define NFP_MAC_STATS_TX_BROADCAST_PKTS			(NFP_MAC_STATS_BASE + 0x160)
+#define NFP_MAC_STATS_TX_PKTS_64_OCTETS			(NFP_MAC_STATS_BASE + 0x168)
+#define NFP_MAC_STATS_TX_PKTS_256_TO_511_OCTETS		(NFP_MAC_STATS_BASE + 0x170)
+#define NFP_MAC_STATS_TX_PKTS_512_TO_1023_OCTETS	(NFP_MAC_STATS_BASE + 0x178)
+#define NFP_MAC_STATS_TX_PAUSE_MAC_CTRL_FRAMES		(NFP_MAC_STATS_BASE + 0x180)
+#define NFP_MAC_STATS_TX_FRAMES_TRANSMITTED_OK		(NFP_MAC_STATS_BASE + 0x188)
+#define NFP_MAC_STATS_TX_UNICAST_PKTS			(NFP_MAC_STATS_BASE + 0x190)
+#define NFP_MAC_STATS_TX_MULTICAST_PKTS			(NFP_MAC_STATS_BASE + 0x198)
+#define NFP_MAC_STATS_TX_PKTS_65_TO_127_OCTETS		(NFP_MAC_STATS_BASE + 0x1a0)
+#define NFP_MAC_STATS_TX_PKTS_127_TO_512_OCTETS		(NFP_MAC_STATS_BASE + 0x1a8)
+#define NFP_MAC_STATS_TX_PKTS_128_TO_1518_OCTETS	(NFP_MAC_STATS_BASE + 0x1b0)
+#define NFP_MAC_STATS_TX_PKTS_1518_TO_MAX_OCTETS	(NFP_MAC_STATS_BASE + 0x1b8)
+
+#endif
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp.h b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp.h
index 4df2ce261b3f..1a8d04a1e113 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp.h
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp.h
@@ -46,7 +46,9 @@
 
 /* Implemented in nfp_hwinfo.c */
 
-const char *nfp_hwinfo_lookup(struct nfp_cpp *cpp, const char *lookup);
+struct nfp_hwinfo;
+struct nfp_hwinfo *nfp_hwinfo_read(struct nfp_cpp *cpp);
+const char *nfp_hwinfo_lookup(struct nfp_hwinfo *hwinfo, const char *lookup);
 
 /* Implemented in nfp_nsp.c, low level functions */
 
@@ -64,6 +66,8 @@ int nfp_nsp_read_eth_table(struct nfp_nsp *state, void *buf, unsigned int size);
 int nfp_nsp_write_eth_table(struct nfp_nsp *state,
 			    const void *buf, unsigned int size);
 int nfp_nsp_read_identify(struct nfp_nsp *state, void *buf, unsigned int size);
+int nfp_nsp_read_sensors(struct nfp_nsp *state, unsigned int sensor_mask,
+			 void *buf, unsigned int size);
 
 /* Implemented in nfp_resource.c */
 
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp6000_pcie.c b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp6000_pcie.c
index 43dc68e01274..cd678323bacb 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp6000_pcie.c
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp6000_pcie.c
@@ -119,6 +119,11 @@
 #define NFP_PCIE_EM                                     0x020000
 #define NFP_PCIE_SRAM                                   0x000000
 
+/* Minimal size of the PCIe cfg memory we depend on being mapped,
+ * queue controller and DMA controller don't have to be covered.
+ */
+#define NFP_PCI_MIN_MAP_SIZE				0x080000
+
 #define NFP_PCIE_P2C_FIXED_SIZE(bar)               (1 << (bar)->bitsize)
 #define NFP_PCIE_P2C_BULK_SIZE(bar)                (1 << (bar)->bitsize)
 #define NFP_PCIE_P2C_GENERAL_TARGET_OFFSET(bar, x) ((x) << ((bar)->bitsize - 2))
@@ -583,9 +588,15 @@ static int enable_bars(struct nfp6000_pcie *nfp, u16 interface)
 		NFP_PCIE_BAR_PCIE2CPP_MapType(
 			NFP_PCIE_BAR_PCIE2CPP_MapType_EXPLICIT3),
 	};
+	char status_msg[196] = {};
 	struct nfp_bar *bar;
 	int i, bars_free;
 	int expl_groups;
+	char *msg, *end;
+
+	msg = status_msg +
+		snprintf(status_msg, sizeof(status_msg) - 1, "RESERVED BARs: ");
+	end = status_msg + sizeof(status_msg) - 1;
 
 	bar = &nfp->bar[0];
 	for (i = 0; i < ARRAY_SIZE(nfp->bar); i++, bar++) {
@@ -628,34 +639,38 @@ static int enable_bars(struct nfp6000_pcie *nfp, u16 interface)
 
 	/* Configure, and lock, BAR0.0 for General Target use (MSI-X SRAM) */
 	bar = &nfp->bar[0];
-	bar->iomem = ioremap_nocache(nfp_bar_resource_start(bar),
-				     nfp_bar_resource_len(bar));
+	if (nfp_bar_resource_len(bar) >= NFP_PCI_MIN_MAP_SIZE)
+		bar->iomem = ioremap_nocache(nfp_bar_resource_start(bar),
+					     nfp_bar_resource_len(bar));
 	if (bar->iomem) {
-		dev_info(nfp->dev,
-			 "BAR0.0 RESERVED: General Mapping/MSI-X SRAM\n");
+		msg += snprintf(msg, end - msg,	"0.0: General/MSI-X SRAM, ");
 		atomic_inc(&bar->refcnt);
 		bars_free--;
 
 		nfp6000_bar_write(nfp, bar, barcfg_msix_general);
 
 		nfp->expl.data = bar->iomem + NFP_PCIE_SRAM + 0x1000;
+
+		if (nfp->pdev->device == PCI_DEVICE_ID_NETRONOME_NFP4000 ||
+		    nfp->pdev->device == PCI_DEVICE_ID_NETRONOME_NFP6000) {
+			nfp->iomem.csr = bar->iomem + NFP_PCIE_BAR(0);
+		} else {
+			int pf = nfp->pdev->devfn & 7;
+
+			nfp->iomem.csr = bar->iomem + NFP_PCIE_BAR(pf);
+		}
+		nfp->iomem.em = bar->iomem + NFP_PCIE_EM;
 	}
 
 	if (nfp->pdev->device == PCI_DEVICE_ID_NETRONOME_NFP4000 ||
-	    nfp->pdev->device == PCI_DEVICE_ID_NETRONOME_NFP6000) {
-		nfp->iomem.csr = bar->iomem + NFP_PCIE_BAR(0);
+	    nfp->pdev->device == PCI_DEVICE_ID_NETRONOME_NFP6000)
 		expl_groups = 4;
-	} else {
-		int pf = nfp->pdev->devfn & 7;
-
-		nfp->iomem.csr = bar->iomem + NFP_PCIE_BAR(pf);
+	else
 		expl_groups = 1;
-	}
-	nfp->iomem.em = bar->iomem + NFP_PCIE_EM;
 
 	/* Configure, and lock, BAR0.1 for PCIe XPB (MSI-X PBA) */
 	bar = &nfp->bar[1];
-	dev_info(nfp->dev, "BAR0.1 RESERVED: PCIe XPB/MSI-X PBA\n");
+	msg += snprintf(msg, end - msg, "0.1: PCIe XPB/MSI-X PBA, ");
 	atomic_inc(&bar->refcnt);
 	bars_free--;
 
@@ -674,9 +689,8 @@ static int enable_bars(struct nfp6000_pcie *nfp, u16 interface)
 		bar->iomem = ioremap_nocache(nfp_bar_resource_start(bar),
 					     nfp_bar_resource_len(bar));
 		if (bar->iomem) {
-			dev_info(nfp->dev,
-				 "BAR0.%d RESERVED: Explicit%d Mapping\n",
-				 4 + i, i);
+			msg += snprintf(msg, end - msg,
+					"0.%d: Explicit%d, ", 4 + i, i);
 			atomic_inc(&bar->refcnt);
 			bars_free--;
 
@@ -694,8 +708,7 @@ static int enable_bars(struct nfp6000_pcie *nfp, u16 interface)
 	sort(&nfp->bar[0], nfp->bars, sizeof(nfp->bar[0]),
 	     bar_cmp, NULL);
 
-	dev_info(nfp->dev, "%d NFP PCI2CPP BARs, %d free\n",
-		 nfp->bars, bars_free);
+	dev_info(nfp->dev, "%sfree: %d/%d\n", status_msg, bars_free, nfp->bars);
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cpp.h b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cpp.h
index edecc0a27485..5798adc57cbc 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cpp.h
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cpp.h
@@ -42,6 +42,7 @@
 
 #include <linux/ctype.h>
 #include <linux/types.h>
+#include <linux/sizes.h>
 
 #ifndef NFP_SUBSYS
 #define NFP_SUBSYS "nfp"
@@ -59,6 +60,13 @@
 #define PCI_64BIT_BAR_COUNT             3
 
 #define NFP_CPP_NUM_TARGETS             16
+/* Max size of area it should be safe to request */
+#define NFP_CPP_SAFE_AREA_SIZE		SZ_2M
+
+/* NFP_MUTEX_WAIT_* are timeouts in seconds when waiting for a mutex */
+#define NFP_MUTEX_WAIT_FIRST_WARN	15
+#define NFP_MUTEX_WAIT_NEXT_WARN	5
+#define NFP_MUTEX_WAIT_ERROR		60
 
 struct device;
 
@@ -214,13 +222,6 @@ u32 nfp_cpp_model(struct nfp_cpp *cpp);
 u16 nfp_cpp_interface(struct nfp_cpp *cpp);
 int nfp_cpp_serial(struct nfp_cpp *cpp, const u8 **serial);
 
-void *nfp_hwinfo_cache(struct nfp_cpp *cpp);
-void nfp_hwinfo_cache_set(struct nfp_cpp *cpp, void *val);
-void *nfp_rtsym_cache(struct nfp_cpp *cpp);
-void nfp_rtsym_cache_set(struct nfp_cpp *cpp, void *val);
-
-void nfp_nffw_cache_flush(struct nfp_cpp *cpp);
-
 struct nfp_cpp_area *nfp_cpp_area_alloc_with_name(struct nfp_cpp *cpp,
 						  u32 cpp_id,
 						  const char *name,
@@ -229,6 +230,9 @@ struct nfp_cpp_area *nfp_cpp_area_alloc_with_name(struct nfp_cpp *cpp,
 struct nfp_cpp_area *nfp_cpp_area_alloc(struct nfp_cpp *cpp, u32 cpp_id,
 					unsigned long long address,
 					unsigned long size);
+struct nfp_cpp_area *
+nfp_cpp_area_alloc_acquire(struct nfp_cpp *cpp, const char *name, u32 cpp_id,
+			   unsigned long long address, unsigned long size);
 void nfp_cpp_area_free(struct nfp_cpp_area *area);
 int nfp_cpp_area_acquire(struct nfp_cpp_area *area);
 int nfp_cpp_area_acquire_nonblocking(struct nfp_cpp_area *area);
@@ -238,8 +242,6 @@ int nfp_cpp_area_read(struct nfp_cpp_area *area, unsigned long offset,
 		      void *buffer, size_t length);
 int nfp_cpp_area_write(struct nfp_cpp_area *area, unsigned long offset,
 		       const void *buffer, size_t length);
-int nfp_cpp_area_check_range(struct nfp_cpp_area *area,
-			     unsigned long long offset, unsigned long size);
 const char *nfp_cpp_area_name(struct nfp_cpp_area *cpp_area);
 void *nfp_cpp_area_priv(struct nfp_cpp_area *cpp_area);
 struct nfp_cpp *nfp_cpp_area_cpp(struct nfp_cpp_area *cpp_area);
@@ -277,6 +279,10 @@ int nfp_cpp_readq(struct nfp_cpp *cpp, u32 cpp_id,
 int nfp_cpp_writeq(struct nfp_cpp *cpp, u32 cpp_id,
 		   unsigned long long address, u64 value);
 
+u8 __iomem *
+nfp_cpp_map_area(struct nfp_cpp *cpp, const char *name, int domain, int target,
+		 u64 addr, unsigned long size, struct nfp_cpp_area **area);
+
 struct nfp_cpp_mutex;
 
 int nfp_cpp_mutex_init(struct nfp_cpp *cpp, int target,
@@ -289,6 +295,17 @@ int nfp_cpp_mutex_lock(struct nfp_cpp_mutex *mutex);
 int nfp_cpp_mutex_unlock(struct nfp_cpp_mutex *mutex);
 int nfp_cpp_mutex_trylock(struct nfp_cpp_mutex *mutex);
 
+/**
+ * nfp_cppcore_pcie_unit() - Get PCI Unit of a CPP handle
+ * @cpp:	CPP handle
+ *
+ * Return: PCI unit for the NFP CPP handle
+ */
+static inline u8 nfp_cppcore_pcie_unit(struct nfp_cpp *cpp)
+{
+	return NFP_CPP_INTERFACE_UNIT_of(nfp_cpp_interface(cpp));
+}
+
 struct nfp_cpp_explicit;
 
 struct nfp_cpp_explicit_command {
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cppcore.c b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cppcore.c
index e2abba4c3a3f..04dd5758ecf5 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cppcore.c
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cppcore.c
@@ -76,10 +76,6 @@ struct nfp_cpp_resource {
  * @serial:		chip serial number
  * @imb_cat_table:	CPP Mapping Table
  *
- * Following fields can be used only in probe() or with rtnl held:
- * @hwinfo:		HWInfo database fetched from the device
- * @rtsym:		firmware run time symbols
- *
  * Following fields use explicit locking:
  * @resource_list:	NFP CPP resource list
  * @resource_lock:	protects @resource_list
@@ -107,9 +103,6 @@ struct nfp_cpp {
 
 	struct mutex area_cache_mutex;
 	struct list_head area_cache_list;
-
-	void *hwinfo;
-	void *rtsym;
 };
 
 /* Element of the area_cache_list */
@@ -233,9 +226,6 @@ void nfp_cpp_free(struct nfp_cpp *cpp)
 	if (cpp->op->free)
 		cpp->op->free(cpp);
 
-	kfree(cpp->hwinfo);
-	kfree(cpp->rtsym);
-
 	device_unregister(&cpp->dev);
 
 	kfree(cpp);
@@ -276,39 +266,6 @@ int nfp_cpp_serial(struct nfp_cpp *cpp, const u8 **serial)
 	return sizeof(cpp->serial);
 }
 
-void *nfp_hwinfo_cache(struct nfp_cpp *cpp)
-{
-	return cpp->hwinfo;
-}
-
-void nfp_hwinfo_cache_set(struct nfp_cpp *cpp, void *val)
-{
-	cpp->hwinfo = val;
-}
-
-void *nfp_rtsym_cache(struct nfp_cpp *cpp)
-{
-	return cpp->rtsym;
-}
-
-void nfp_rtsym_cache_set(struct nfp_cpp *cpp, void *val)
-{
-	cpp->rtsym = val;
-}
-
-/**
- * nfp_nffw_cache_flush() - Flush cached firmware information
- * @cpp:	NFP CPP handle
- *
- * Flush cached firmware information.  This function should be called
- * every time firmware is loaded on unloaded.
- */
-void nfp_nffw_cache_flush(struct nfp_cpp *cpp)
-{
-	kfree(nfp_rtsym_cache(cpp));
-	nfp_rtsym_cache_set(cpp, NULL);
-}
-
 /**
  * nfp_cpp_area_alloc_with_name() - allocate a new CPP area
  * @cpp:	CPP device handle
@@ -404,6 +361,41 @@ nfp_cpp_area_alloc(struct nfp_cpp *cpp, u32 dest,
 }
 
 /**
+ * nfp_cpp_area_alloc_acquire() - allocate a new CPP area and lock it down
+ * @cpp:	CPP handle
+ * @name:	Name of region
+ * @dest:	CPP id
+ * @address:	Start address on CPP target
+ * @size:	Size of area
+ *
+ * Allocate and initialize a CPP area structure, and lock it down so
+ * that it can be accessed directly.
+ *
+ * NOTE: @address and @size must be 32-bit aligned values.
+ *
+ * NOTE: The area must also be 'released' when the structure is freed.
+ *
+ * Return: NFP CPP Area handle, or NULL
+ */
+struct nfp_cpp_area *
+nfp_cpp_area_alloc_acquire(struct nfp_cpp *cpp, const char *name, u32 dest,
+			   unsigned long long address, unsigned long size)
+{
+	struct nfp_cpp_area *area;
+
+	area = nfp_cpp_area_alloc_with_name(cpp, dest, name, address, size);
+	if (!area)
+		return NULL;
+
+	if (nfp_cpp_area_acquire(area)) {
+		nfp_cpp_area_free(area);
+		return NULL;
+	}
+
+	return area;
+}
+
+/**
  * nfp_cpp_area_free() - free up the CPP area
  * @area:	CPP area handle
  *
@@ -579,27 +571,6 @@ int nfp_cpp_area_write(struct nfp_cpp_area *area,
 }
 
 /**
- * nfp_cpp_area_check_range() - check if address range fits in CPP area
- * @area:	CPP area handle
- * @offset:	offset into CPP target
- * @length:	size of address range in bytes
- *
- * Check if address range fits within CPP area.  Return 0 if area
- * fits or -EFAULT on error.
- *
- * Return: 0, or -ERRNO
- */
-int nfp_cpp_area_check_range(struct nfp_cpp_area *area,
-			     unsigned long long offset, unsigned long length)
-{
-	if (offset < area->offset ||
-	    offset + length > area->offset + area->size)
-		return -EFAULT;
-
-	return 0;
-}
-
-/**
  * nfp_cpp_area_name() - return name of a CPP area
  * @cpp_area:	CPP area handle
  *
@@ -924,18 +895,9 @@ area_cache_put(struct nfp_cpp *cpp, struct nfp_cpp_area_cache *cache)
 	mutex_unlock(&cpp->area_cache_mutex);
 }
 
-/**
- * nfp_cpp_read() - read from CPP target
- * @cpp:		CPP handle
- * @destination:	CPP id
- * @address:		offset into CPP target
- * @kernel_vaddr:	kernel buffer for result
- * @length:		number of bytes to read
- *
- * Return: length of io, or -ERRNO
- */
-int nfp_cpp_read(struct nfp_cpp *cpp, u32 destination,
-		 unsigned long long address, void *kernel_vaddr, size_t length)
+static int __nfp_cpp_read(struct nfp_cpp *cpp, u32 destination,
+			  unsigned long long address, void *kernel_vaddr,
+			  size_t length)
 {
 	struct nfp_cpp_area_cache *cache;
 	struct nfp_cpp_area *area;
@@ -968,18 +930,43 @@ int nfp_cpp_read(struct nfp_cpp *cpp, u32 destination,
 }
 
 /**
- * nfp_cpp_write() - write to CPP target
+ * nfp_cpp_read() - read from CPP target
  * @cpp:		CPP handle
  * @destination:	CPP id
  * @address:		offset into CPP target
- * @kernel_vaddr:	kernel buffer to read from
- * @length:		number of bytes to write
+ * @kernel_vaddr:	kernel buffer for result
+ * @length:		number of bytes to read
  *
  * Return: length of io, or -ERRNO
  */
-int nfp_cpp_write(struct nfp_cpp *cpp, u32 destination,
-		  unsigned long long address,
-		  const void *kernel_vaddr, size_t length)
+int nfp_cpp_read(struct nfp_cpp *cpp, u32 destination,
+		 unsigned long long address, void *kernel_vaddr,
+		 size_t length)
+{
+	size_t n, offset;
+	int ret;
+
+	for (offset = 0; offset < length; offset += n) {
+		unsigned long long r_addr = address + offset;
+
+		/* make first read smaller to align to safe window */
+		n = min_t(size_t, length - offset,
+			  ALIGN(r_addr + 1, NFP_CPP_SAFE_AREA_SIZE) - r_addr);
+
+		ret = __nfp_cpp_read(cpp, destination, address + offset,
+				     kernel_vaddr + offset, n);
+		if (ret < 0)
+			return ret;
+		if (ret != n)
+			return offset + n;
+	}
+
+	return length;
+}
+
+static int __nfp_cpp_write(struct nfp_cpp *cpp, u32 destination,
+			   unsigned long long address,
+			   const void *kernel_vaddr, size_t length)
 {
 	struct nfp_cpp_area_cache *cache;
 	struct nfp_cpp_area *area;
@@ -1011,6 +998,41 @@ int nfp_cpp_write(struct nfp_cpp *cpp, u32 destination,
 	return err;
 }
 
+/**
+ * nfp_cpp_write() - write to CPP target
+ * @cpp:		CPP handle
+ * @destination:	CPP id
+ * @address:		offset into CPP target
+ * @kernel_vaddr:	kernel buffer to read from
+ * @length:		number of bytes to write
+ *
+ * Return: length of io, or -ERRNO
+ */
+int nfp_cpp_write(struct nfp_cpp *cpp, u32 destination,
+		  unsigned long long address,
+		  const void *kernel_vaddr, size_t length)
+{
+	size_t n, offset;
+	int ret;
+
+	for (offset = 0; offset < length; offset += n) {
+		unsigned long long w_addr = address + offset;
+
+		/* make first write smaller to align to safe window */
+		n = min_t(size_t, length - offset,
+			  ALIGN(w_addr + 1, NFP_CPP_SAFE_AREA_SIZE) - w_addr);
+
+		ret = __nfp_cpp_write(cpp, destination, address + offset,
+				      kernel_vaddr + offset, n);
+		if (ret < 0)
+			return ret;
+		if (ret != n)
+			return offset + n;
+	}
+
+	return length;
+}
+
 /* Return the correct CPP address, and fixup xpb_addr as needed. */
 static u32 nfp_xpb_to_cpp(struct nfp_cpp *cpp, u32 *xpb_addr)
 {
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cpplib.c b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cpplib.c
index 0ba0379b8f75..ab86bceb93f2 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cpplib.c
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cpplib.c
@@ -279,3 +279,43 @@ exit_release:
 
 	return err;
 }
+
+/**
+ * nfp_cpp_map_area() - Helper function to map an area
+ * @cpp:    NFP CPP handler
+ * @name:   Name for the area
+ * @domain: CPP domain
+ * @target: CPP target
+ * @addr:   CPP address
+ * @size:   Size of the area
+ * @area:   Area handle (output)
+ *
+ * Map an area of IOMEM access.  To undo the effect of this function call
+ * @nfp_cpp_area_release_free(*area).
+ *
+ * Return: Pointer to memory mapped area or ERR_PTR
+ */
+u8 __iomem *
+nfp_cpp_map_area(struct nfp_cpp *cpp, const char *name, int domain, int target,
+		 u64 addr, unsigned long size, struct nfp_cpp_area **area)
+{
+	u8 __iomem *res;
+	u32 dest;
+
+	dest = NFP_CPP_ISLAND_ID(target, NFP_CPP_ACTION_RW, 0, domain);
+
+	*area = nfp_cpp_area_alloc_acquire(cpp, name, dest, addr, size);
+	if (!*area)
+		goto err_eio;
+
+	res = nfp_cpp_area_iomem(*area);
+	if (!res)
+		goto err_release_free;
+
+	return res;
+
+err_release_free:
+	nfp_cpp_area_release_free(*area);
+err_eio:
+	return (u8 __iomem *)ERR_PTR(-EIO);
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_hwinfo.c b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_hwinfo.c
index 8d8f311ffa6e..4f24aff1e772 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_hwinfo.c
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_hwinfo.c
@@ -178,7 +178,8 @@ hwinfo_db_validate(struct nfp_cpp *cpp, struct nfp_hwinfo *db, u32 len)
 	return hwinfo_db_walk(cpp, db, size);
 }
 
-static int hwinfo_try_fetch(struct nfp_cpp *cpp, size_t *cpp_size)
+static struct nfp_hwinfo *
+hwinfo_try_fetch(struct nfp_cpp *cpp, size_t *cpp_size)
 {
 	struct nfp_hwinfo *header;
 	struct nfp_resource *res;
@@ -196,7 +197,7 @@ static int hwinfo_try_fetch(struct nfp_cpp *cpp, size_t *cpp_size)
 		nfp_resource_release(res);
 
 		if (*cpp_size < HWINFO_SIZE_MIN)
-			return -ENOENT;
+			return NULL;
 	} else if (PTR_ERR(res) == -ENOENT) {
 		/* Try getting the HWInfo table from the 'classic' location */
 		cpp_id = NFP_CPP_ISLAND_ID(NFP_CPP_TARGET_MU,
@@ -204,101 +205,86 @@ static int hwinfo_try_fetch(struct nfp_cpp *cpp, size_t *cpp_size)
 		cpp_addr = 0x30000;
 		*cpp_size = 0x0e000;
 	} else {
-		return PTR_ERR(res);
+		return NULL;
 	}
 
 	db = kmalloc(*cpp_size + 1, GFP_KERNEL);
 	if (!db)
-		return -ENOMEM;
+		return NULL;
 
 	err = nfp_cpp_read(cpp, cpp_id, cpp_addr, db, *cpp_size);
-	if (err != *cpp_size) {
-		kfree(db);
-		return err < 0 ? err : -EIO;
-	}
+	if (err != *cpp_size)
+		goto exit_free;
 
 	header = (void *)db;
-	if (nfp_hwinfo_is_updating(header)) {
-		kfree(db);
-		return -EBUSY;
-	}
+	if (nfp_hwinfo_is_updating(header))
+		goto exit_free;
 
 	if (le32_to_cpu(header->version) != NFP_HWINFO_VERSION_2) {
 		nfp_err(cpp, "Unknown HWInfo version: 0x%08x\n",
 			le32_to_cpu(header->version));
-		kfree(db);
-		return -EINVAL;
+		goto exit_free;
 	}
 
 	/* NULL-terminate for safety */
 	db[*cpp_size] = '\0';
 
-	nfp_hwinfo_cache_set(cpp, db);
-
-	return 0;
+	return (void *)db;
+exit_free:
+	kfree(db);
+	return NULL;
 }
 
-static int hwinfo_fetch(struct nfp_cpp *cpp, size_t *hwdb_size)
+static struct nfp_hwinfo *hwinfo_fetch(struct nfp_cpp *cpp, size_t *hwdb_size)
 {
 	const unsigned long wait_until = jiffies + HWINFO_WAIT * HZ;
+	struct nfp_hwinfo *db;
 	int err;
 
 	for (;;) {
 		const unsigned long start_time = jiffies;
 
-		err = hwinfo_try_fetch(cpp, hwdb_size);
-		if (!err)
-			return 0;
+		db = hwinfo_try_fetch(cpp, hwdb_size);
+		if (db)
+			return db;
 
 		err = msleep_interruptible(100);
 		if (err || time_after(start_time, wait_until)) {
 			nfp_err(cpp, "NFP access error\n");
-			return -EIO;
+			return NULL;
 		}
 	}
 }
 
-static int nfp_hwinfo_load(struct nfp_cpp *cpp)
+struct nfp_hwinfo *nfp_hwinfo_read(struct nfp_cpp *cpp)
 {
 	struct nfp_hwinfo *db;
 	size_t hwdb_size = 0;
 	int err;
 
-	err = hwinfo_fetch(cpp, &hwdb_size);
-	if (err)
-		return err;
+	db = hwinfo_fetch(cpp, &hwdb_size);
+	if (!db)
+		return NULL;
 
-	db = nfp_hwinfo_cache(cpp);
 	err = hwinfo_db_validate(cpp, db, hwdb_size);
 	if (err) {
 		kfree(db);
-		nfp_hwinfo_cache_set(cpp, NULL);
-		return err;
+		return NULL;
 	}
 
-	return 0;
+	return db;
 }
 
 /**
  * nfp_hwinfo_lookup() - Find a value in the HWInfo table by name
- * @cpp:	NFP CPP handle
+ * @hwinfo:	NFP HWinfo table
  * @lookup:	HWInfo name to search for
  *
  * Return: Value of the HWInfo name, or NULL
  */
-const char *nfp_hwinfo_lookup(struct nfp_cpp *cpp, const char *lookup)
+const char *nfp_hwinfo_lookup(struct nfp_hwinfo *hwinfo, const char *lookup)
 {
 	const char *key, *val, *end;
-	struct nfp_hwinfo *hwinfo;
-	int err;
-
-	hwinfo = nfp_hwinfo_cache(cpp);
-	if (!hwinfo) {
-		err = nfp_hwinfo_load(cpp);
-		if (err)
-			return NULL;
-		hwinfo = nfp_hwinfo_cache(cpp);
-	}
 
 	if (!hwinfo || !lookup)
 		return NULL;
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_mip.c b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_mip.c
index 3d15dd03647e..5f193fe2d69e 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_mip.c
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_mip.c
@@ -141,6 +141,8 @@ const struct nfp_mip *nfp_mip_open(struct nfp_cpp *cpp)
 		return NULL;
 	}
 
+	mip->name[sizeof(mip->name) - 1] = 0;
+
 	return mip;
 }
 
@@ -149,6 +151,11 @@ void nfp_mip_close(const struct nfp_mip *mip)
 	kfree(mip);
 }
 
+const char *nfp_mip_name(const struct nfp_mip *mip)
+{
+	return mip->name;
+}
+
 /**
  * nfp_mip_symtab() - Get the address and size of the MIP symbol table
  * @mip:	MIP handle
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_mutex.c b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_mutex.c
index 8a99c189efa8..f7b958181126 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_mutex.c
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_mutex.c
@@ -195,7 +195,8 @@ void nfp_cpp_mutex_free(struct nfp_cpp_mutex *mutex)
  */
 int nfp_cpp_mutex_lock(struct nfp_cpp_mutex *mutex)
 {
-	unsigned long warn_at = jiffies + 15 * HZ;
+	unsigned long warn_at = jiffies + NFP_MUTEX_WAIT_FIRST_WARN * HZ;
+	unsigned long err_at = jiffies + NFP_MUTEX_WAIT_ERROR * HZ;
 	unsigned int timeout_ms = 1;
 	int err;
 
@@ -214,12 +215,16 @@ int nfp_cpp_mutex_lock(struct nfp_cpp_mutex *mutex)
 			return -ERESTARTSYS;
 
 		if (time_is_before_eq_jiffies(warn_at)) {
-			warn_at = jiffies + 60 * HZ;
+			warn_at = jiffies + NFP_MUTEX_WAIT_NEXT_WARN * HZ;
 			nfp_warn(mutex->cpp,
 				 "Warning: waiting for NFP mutex [depth:%hd target:%d addr:%llx key:%08x]\n",
 				 mutex->depth,
 				 mutex->target, mutex->address, mutex->key);
 		}
+		if (time_is_before_eq_jiffies(err_at)) {
+			nfp_err(mutex->cpp, "Error: mutex wait timed out\n");
+			return -EBUSY;
+		}
 	}
 
 	return err;
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nffw.h b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nffw.h
index 988badd230d1..c9724fb7ea4b 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nffw.h
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nffw.h
@@ -55,6 +55,7 @@ struct nfp_mip;
 const struct nfp_mip *nfp_mip_open(struct nfp_cpp *cpp);
 void nfp_mip_close(const struct nfp_mip *mip);
 
+const char *nfp_mip_name(const struct nfp_mip *mip);
 void nfp_mip_symtab(const struct nfp_mip *mip, u32 *addr, u32 *size);
 void nfp_mip_strtab(const struct nfp_mip *mip, u32 *addr, u32 *size);
 
@@ -87,9 +88,20 @@ struct nfp_rtsym {
 	int domain;
 };
 
-int nfp_rtsym_count(struct nfp_cpp *cpp);
-const struct nfp_rtsym *nfp_rtsym_get(struct nfp_cpp *cpp, int idx);
-const struct nfp_rtsym *nfp_rtsym_lookup(struct nfp_cpp *cpp, const char *name);
-u64 nfp_rtsym_read_le(struct nfp_cpp *cpp, const char *name, int *error);
+struct nfp_rtsym_table;
+
+struct nfp_rtsym_table *nfp_rtsym_table_read(struct nfp_cpp *cpp);
+struct nfp_rtsym_table *
+__nfp_rtsym_table_read(struct nfp_cpp *cpp, const struct nfp_mip *mip);
+int nfp_rtsym_count(struct nfp_rtsym_table *rtbl);
+const struct nfp_rtsym *nfp_rtsym_get(struct nfp_rtsym_table *rtbl, int idx);
+const struct nfp_rtsym *
+nfp_rtsym_lookup(struct nfp_rtsym_table *rtbl, const char *name);
+
+u64 nfp_rtsym_read_le(struct nfp_rtsym_table *rtbl, const char *name,
+		      int *error);
+u8 __iomem *
+nfp_rtsym_map(struct nfp_rtsym_table *rtbl, const char *name, const char *id,
+	      unsigned int min_size, struct nfp_cpp_area **area);
 
 #endif /* NFP_NFFW_H */
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp.c b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp.c
index 2fa9247bb23d..37364555c42b 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp.c
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp.c
@@ -93,6 +93,7 @@ enum nfp_nsp_cmd {
 	SPCODE_FW_LOAD		= 6, /* Load fw from buffer, len in option */
 	SPCODE_ETH_RESCAN	= 7, /* Rescan ETHs, write ETH_TABLE to buf */
 	SPCODE_ETH_CONTROL	= 8, /* Update media config from buffer */
+	SPCODE_NSP_SENSORS	= 12, /* Read NSP sensor(s) */
 	SPCODE_NSP_IDENTIFY	= 13, /* Read NSP version */
 };
 
@@ -419,6 +420,14 @@ static int nfp_nsp_command_buf(struct nfp_nsp *nsp, u16 code, u32 option,
 		if (err < 0)
 			return err;
 	}
+	/* Zero out remaining part of the buffer */
+	if (out_buf && out_size && out_size > in_size) {
+		memset(out_buf, 0, out_size - in_size);
+		err = nfp_cpp_write(cpp, cpp_id, cpp_buf + in_size,
+				    out_buf, out_size - in_size);
+		if (err < 0)
+			return err;
+	}
 
 	ret = nfp_nsp_command(nsp, code, option, cpp_id, cpp_buf);
 	if (ret < 0)
@@ -465,13 +474,7 @@ int nfp_nsp_wait(struct nfp_nsp *state)
 
 int nfp_nsp_device_soft_reset(struct nfp_nsp *state)
 {
-	int err;
-
-	err = nfp_nsp_command(state, SPCODE_SOFT_RESET, 0, 0, 0);
-
-	nfp_nffw_cache_flush(state->cpp);
-
-	return err;
+	return nfp_nsp_command(state, SPCODE_SOFT_RESET, 0, 0, 0);
 }
 
 int nfp_nsp_load_fw(struct nfp_nsp *state, const struct firmware *fw)
@@ -498,3 +501,10 @@ int nfp_nsp_read_identify(struct nfp_nsp *state, void *buf, unsigned int size)
 	return nfp_nsp_command_buf(state, SPCODE_NSP_IDENTIFY, size, NULL, 0,
 				   buf, size);
 }
+
+int nfp_nsp_read_sensors(struct nfp_nsp *state, unsigned int sensor_mask,
+			 void *buf, unsigned int size)
+{
+	return nfp_nsp_command_buf(state, SPCODE_NSP_SENSORS, sensor_mask,
+				   NULL, 0, buf, size);
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp.h b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp.h
index 36b21e4dc56d..e2f028027c6f 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp.h
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp.h
@@ -76,6 +76,7 @@ enum nfp_eth_aneg {
 /**
  * struct nfp_eth_table - ETH table information
  * @count:	number of table entries
+ * @max_index:	max of @index fields of all @ports
  * @ports:	table of ports
  *
  * @eth_index:	port index according to legacy ethX numbering
@@ -96,10 +97,12 @@ enum nfp_eth_aneg {
  * @override_changed: is media reconfig pending?
  *
  * @port_type:	one of %PORT_* defines for ethtool
+ * @port_lanes:	total number of lanes on the port (sum of lanes of all subports)
  * @is_split:	is interface part of a split port
  */
 struct nfp_eth_table {
 	unsigned int count;
+	unsigned int max_index;
 	struct nfp_eth_table_port {
 		unsigned int eth_index;
 		unsigned int index;
@@ -127,6 +130,8 @@ struct nfp_eth_table {
 		/* Computed fields */
 		u8 port_type;
 
+		unsigned int port_lanes;
+
 		bool is_split;
 	} ports[0];
 };
@@ -157,6 +162,7 @@ int __nfp_eth_set_split(struct nfp_nsp *nsp, unsigned int lanes);
  * @primary:      version of primarary bootloader
  * @secondary:    version id of secondary bootloader
  * @nsp:          version id of NSP
+ * @sensor_mask:  mask of present sensors available on NIC
  */
 struct nfp_nsp_identify {
 	char version[40];
@@ -167,8 +173,19 @@ struct nfp_nsp_identify {
 	u16 primary;
 	u16 secondary;
 	u16 nsp;
+	u64 sensor_mask;
 };
 
 struct nfp_nsp_identify *__nfp_nsp_identify(struct nfp_nsp *nsp);
 
+enum nfp_nsp_sensor_id {
+	NFP_SENSOR_CHIP_TEMPERATURE,
+	NFP_SENSOR_ASSEMBLY_POWER,
+	NFP_SENSOR_ASSEMBLY_12V_POWER,
+	NFP_SENSOR_ASSEMBLY_3V3_POWER,
+};
+
+int nfp_hwmon_read_sensor(struct nfp_cpp *cpp, enum nfp_nsp_sensor_id id,
+			  long *val);
+
 #endif
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp_cmds.c b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp_cmds.c
index e7a263de3731..5d362f87af08 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp_cmds.c
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp_cmds.c
@@ -46,7 +46,8 @@ struct nsp_identify {
 	__le16 primary;
 	__le16 secondary;
 	__le16 nsp;
-	__le16 reserved;
+	u8 reserved[6];
+	__le64 sensor_mask;
 };
 
 struct nfp_nsp_identify *__nfp_nsp_identify(struct nfp_nsp *nsp)
@@ -82,8 +83,52 @@ struct nfp_nsp_identify *__nfp_nsp_identify(struct nfp_nsp *nsp)
 	nspi->primary = le16_to_cpu(ni->primary);
 	nspi->secondary = le16_to_cpu(ni->secondary);
 	nspi->nsp = le16_to_cpu(ni->nsp);
+	nspi->sensor_mask = le64_to_cpu(ni->sensor_mask);
 
 exit_free:
 	kfree(ni);
 	return nspi;
 }
+
+struct nfp_sensors {
+	__le32 chip_temp;
+	__le32 assembly_power;
+	__le32 assembly_12v_power;
+	__le32 assembly_3v3_power;
+};
+
+int nfp_hwmon_read_sensor(struct nfp_cpp *cpp, enum nfp_nsp_sensor_id id,
+			  long *val)
+{
+	struct nfp_sensors s;
+	struct nfp_nsp *nsp;
+	int ret;
+
+	nsp = nfp_nsp_open(cpp);
+	if (IS_ERR(nsp))
+		return PTR_ERR(nsp);
+
+	ret = nfp_nsp_read_sensors(nsp, BIT(id), &s, sizeof(s));
+	nfp_nsp_close(nsp);
+
+	if (ret < 0)
+		return ret;
+
+	switch (id) {
+	case NFP_SENSOR_CHIP_TEMPERATURE:
+		*val = le32_to_cpu(s.chip_temp);
+		break;
+	case NFP_SENSOR_ASSEMBLY_POWER:
+		*val = le32_to_cpu(s.assembly_power);
+		break;
+	case NFP_SENSOR_ASSEMBLY_12V_POWER:
+		*val = le32_to_cpu(s.assembly_12v_power);
+		break;
+	case NFP_SENSOR_ASSEMBLY_3V3_POWER:
+		*val = le32_to_cpu(s.assembly_3v3_power);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp_eth.c b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp_eth.c
index 639438d8313a..c2bc36e8649f 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp_eth.c
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp_eth.c
@@ -186,17 +186,21 @@ nfp_eth_port_translate(struct nfp_nsp *nsp, const union eth_table_entry *src,
 }
 
 static void
-nfp_eth_mark_split_ports(struct nfp_cpp *cpp, struct nfp_eth_table *table)
+nfp_eth_calc_port_geometry(struct nfp_cpp *cpp, struct nfp_eth_table *table)
 {
 	unsigned int i, j;
 
-	for (i = 0; i < table->count; i++)
+	for (i = 0; i < table->count; i++) {
+		table->max_index = max(table->max_index, table->ports[i].index);
+
 		for (j = 0; j < table->count; j++) {
-			if (i == j)
-				continue;
 			if (table->ports[i].label_port !=
 			    table->ports[j].label_port)
 				continue;
+			table->ports[i].port_lanes += table->ports[j].lanes;
+
+			if (i == j)
+				continue;
 			if (table->ports[i].label_subport ==
 			    table->ports[j].label_subport)
 				nfp_warn(cpp,
@@ -205,8 +209,8 @@ nfp_eth_mark_split_ports(struct nfp_cpp *cpp, struct nfp_eth_table *table)
 					 table->ports[i].label_subport);
 
 			table->ports[i].is_split = true;
-			break;
 		}
+	}
 }
 
 static void
@@ -289,7 +293,7 @@ __nfp_eth_read_ports(struct nfp_cpp *cpp, struct nfp_nsp *nsp)
 			nfp_eth_port_translate(nsp, &entries[i], i,
 					       &table->ports[j++]);
 
-	nfp_eth_mark_split_ports(cpp, table);
+	nfp_eth_calc_port_geometry(cpp, table);
 	for (i = 0; i < table->count; i++)
 		nfp_eth_calc_port_type(cpp, &table->ports[i]);
 
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_resource.c b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_resource.c
index 2d15a7c9d0de..072612263dab 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_resource.c
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_resource.c
@@ -181,7 +181,8 @@ err_unlock_dev:
 struct nfp_resource *
 nfp_resource_acquire(struct nfp_cpp *cpp, const char *name)
 {
-	unsigned long warn_at = jiffies + 15 * HZ;
+	unsigned long warn_at = jiffies + NFP_MUTEX_WAIT_FIRST_WARN * HZ;
+	unsigned long err_at = jiffies + NFP_MUTEX_WAIT_ERROR * HZ;
 	struct nfp_cpp_mutex *dev_mutex;
 	struct nfp_resource *res;
 	int err;
@@ -214,10 +215,15 @@ nfp_resource_acquire(struct nfp_cpp *cpp, const char *name)
 		}
 
 		if (time_is_before_eq_jiffies(warn_at)) {
-			warn_at = jiffies + 60 * HZ;
+			warn_at = jiffies + NFP_MUTEX_WAIT_NEXT_WARN * HZ;
 			nfp_warn(cpp, "Warning: waiting for NFP resource %s\n",
 				 name);
 		}
+		if (time_is_before_eq_jiffies(err_at)) {
+			nfp_err(cpp, "Error: resource %s timed out\n", name);
+			err = -EBUSY;
+			goto err_free;
+		}
 	}
 
 	nfp_cpp_mutex_free(dev_mutex);
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_rtsym.c b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_rtsym.c
index 0e3870ecfb8c..ecda474ac7c3 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_rtsym.c
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_rtsym.c
@@ -65,7 +65,8 @@ struct nfp_rtsym_entry {
 	__le32	size_lo;
 };
 
-struct nfp_rtsym_cache {
+struct nfp_rtsym_table {
+	struct nfp_cpp *cpp;
 	int num;
 	char *strtab;
 	struct nfp_rtsym symtab[];
@@ -78,7 +79,7 @@ static int nfp_meid(u8 island_id, u8 menum)
 }
 
 static void
-nfp_rtsym_sw_entry_init(struct nfp_rtsym_cache *cache, u32 strtab_size,
+nfp_rtsym_sw_entry_init(struct nfp_rtsym_table *cache, u32 strtab_size,
 			struct nfp_rtsym *sw, struct nfp_rtsym_entry *fw)
 {
 	sw->type = fw->type;
@@ -106,26 +107,36 @@ nfp_rtsym_sw_entry_init(struct nfp_rtsym_cache *cache, u32 strtab_size,
 		sw->domain = -1;
 }
 
-static int nfp_rtsymtab_probe(struct nfp_cpp *cpp)
+struct nfp_rtsym_table *nfp_rtsym_table_read(struct nfp_cpp *cpp)
+{
+	struct nfp_rtsym_table *rtbl;
+	const struct nfp_mip *mip;
+
+	mip = nfp_mip_open(cpp);
+	rtbl = __nfp_rtsym_table_read(cpp, mip);
+	nfp_mip_close(mip);
+
+	return rtbl;
+}
+
+struct nfp_rtsym_table *
+__nfp_rtsym_table_read(struct nfp_cpp *cpp, const struct nfp_mip *mip)
 {
 	const u32 dram = NFP_CPP_ID(NFP_CPP_TARGET_MU, NFP_CPP_ACTION_RW, 0) |
 		NFP_ISL_EMEM0;
 	u32 strtab_addr, symtab_addr, strtab_size, symtab_size;
 	struct nfp_rtsym_entry *rtsymtab;
-	struct nfp_rtsym_cache *cache;
-	const struct nfp_mip *mip;
+	struct nfp_rtsym_table *cache;
 	int err, n, size;
 
-	mip = nfp_mip_open(cpp);
 	if (!mip)
-		return -EIO;
+		return NULL;
 
 	nfp_mip_strtab(mip, &strtab_addr, &strtab_size);
 	nfp_mip_symtab(mip, &symtab_addr, &symtab_size);
-	nfp_mip_close(mip);
 
 	if (!symtab_size || !strtab_size || symtab_size % sizeof(*rtsymtab))
-		return -ENXIO;
+		return NULL;
 
 	/* Align to 64 bits */
 	symtab_size = round_up(symtab_size, 8);
@@ -133,27 +144,26 @@ static int nfp_rtsymtab_probe(struct nfp_cpp *cpp)
 
 	rtsymtab = kmalloc(symtab_size, GFP_KERNEL);
 	if (!rtsymtab)
-		return -ENOMEM;
+		return NULL;
 
 	size = sizeof(*cache);
 	size += symtab_size / sizeof(*rtsymtab) * sizeof(struct nfp_rtsym);
 	size +=	strtab_size + 1;
 	cache = kmalloc(size, GFP_KERNEL);
-	if (!cache) {
-		err = -ENOMEM;
-		goto err_free_rtsym_raw;
-	}
+	if (!cache)
+		goto exit_free_rtsym_raw;
 
+	cache->cpp = cpp;
 	cache->num = symtab_size / sizeof(*rtsymtab);
 	cache->strtab = (void *)&cache->symtab[cache->num];
 
 	err = nfp_cpp_read(cpp, dram, symtab_addr, rtsymtab, symtab_size);
 	if (err != symtab_size)
-		goto err_free_cache;
+		goto exit_free_cache;
 
 	err = nfp_cpp_read(cpp, dram, strtab_addr, cache->strtab, strtab_size);
 	if (err != strtab_size)
-		goto err_free_cache;
+		goto exit_free_cache;
 	cache->strtab[strtab_size] = '\0';
 
 	for (n = 0; n < cache->num; n++)
@@ -161,97 +171,71 @@ static int nfp_rtsymtab_probe(struct nfp_cpp *cpp)
 					&cache->symtab[n], &rtsymtab[n]);
 
 	kfree(rtsymtab);
-	nfp_rtsym_cache_set(cpp, cache);
-	return 0;
 
-err_free_cache:
+	return cache;
+
+exit_free_cache:
 	kfree(cache);
-err_free_rtsym_raw:
+exit_free_rtsym_raw:
 	kfree(rtsymtab);
-	return err;
-}
-
-static struct nfp_rtsym_cache *nfp_rtsym(struct nfp_cpp *cpp)
-{
-	struct nfp_rtsym_cache *cache;
-	int err;
-
-	cache = nfp_rtsym_cache(cpp);
-	if (cache)
-		return cache;
-
-	err = nfp_rtsymtab_probe(cpp);
-	if (err < 0)
-		return ERR_PTR(err);
-
-	return nfp_rtsym_cache(cpp);
+	return NULL;
 }
 
 /**
  * nfp_rtsym_count() - Get the number of RTSYM descriptors
- * @cpp:	NFP CPP handle
+ * @rtbl:	NFP RTsym table
  *
- * Return: Number of RTSYM descriptors, or -ERRNO
+ * Return: Number of RTSYM descriptors
  */
-int nfp_rtsym_count(struct nfp_cpp *cpp)
+int nfp_rtsym_count(struct nfp_rtsym_table *rtbl)
 {
-	struct nfp_rtsym_cache *cache;
-
-	cache = nfp_rtsym(cpp);
-	if (IS_ERR(cache))
-		return PTR_ERR(cache);
-
-	return cache->num;
+	if (!rtbl)
+		return -EINVAL;
+	return rtbl->num;
 }
 
 /**
  * nfp_rtsym_get() - Get the Nth RTSYM descriptor
- * @cpp:	NFP CPP handle
+ * @rtbl:	NFP RTsym table
  * @idx:	Index (0-based) of the RTSYM descriptor
  *
  * Return: const pointer to a struct nfp_rtsym descriptor, or NULL
  */
-const struct nfp_rtsym *nfp_rtsym_get(struct nfp_cpp *cpp, int idx)
+const struct nfp_rtsym *nfp_rtsym_get(struct nfp_rtsym_table *rtbl, int idx)
 {
-	struct nfp_rtsym_cache *cache;
-
-	cache = nfp_rtsym(cpp);
-	if (IS_ERR(cache))
+	if (!rtbl)
 		return NULL;
-
-	if (idx >= cache->num)
+	if (idx >= rtbl->num)
 		return NULL;
 
-	return &cache->symtab[idx];
+	return &rtbl->symtab[idx];
 }
 
 /**
  * nfp_rtsym_lookup() - Return the RTSYM descriptor for a symbol name
- * @cpp:	NFP CPP handle
+ * @rtbl:	NFP RTsym table
  * @name:	Symbol name
  *
  * Return: const pointer to a struct nfp_rtsym descriptor, or NULL
  */
-const struct nfp_rtsym *nfp_rtsym_lookup(struct nfp_cpp *cpp, const char *name)
+const struct nfp_rtsym *
+nfp_rtsym_lookup(struct nfp_rtsym_table *rtbl, const char *name)
 {
-	struct nfp_rtsym_cache *cache;
 	int n;
 
-	cache = nfp_rtsym(cpp);
-	if (IS_ERR(cache))
+	if (!rtbl)
 		return NULL;
 
-	for (n = 0; n < cache->num; n++) {
-		if (strcmp(name, cache->symtab[n].name) == 0)
-			return &cache->symtab[n];
-	}
+	for (n = 0; n < rtbl->num; n++)
+		if (strcmp(name, rtbl->symtab[n].name) == 0)
+			return &rtbl->symtab[n];
 
 	return NULL;
 }
 
 /**
  * nfp_rtsym_read_le() - Read a simple unsigned scalar value from symbol
- * @cpp:	NFP CPP handle
+ * @rtbl:	NFP RTsym table
  * @name:	Symbol name
  * @error:	Poniter to error code (optional)
  *
@@ -261,14 +245,15 @@ const struct nfp_rtsym *nfp_rtsym_lookup(struct nfp_cpp *cpp, const char *name)
  *
  * Return: value read, on error sets the error and returns ~0ULL.
  */
-u64 nfp_rtsym_read_le(struct nfp_cpp *cpp, const char *name, int *error)
+u64 nfp_rtsym_read_le(struct nfp_rtsym_table *rtbl, const char *name,
+		      int *error)
 {
 	const struct nfp_rtsym *sym;
 	u32 val32, id;
 	u64 val;
 	int err;
 
-	sym = nfp_rtsym_lookup(cpp, name);
+	sym = nfp_rtsym_lookup(rtbl, name);
 	if (!sym) {
 		err = -ENOENT;
 		goto exit;
@@ -278,14 +263,14 @@ u64 nfp_rtsym_read_le(struct nfp_cpp *cpp, const char *name, int *error)
 
 	switch (sym->size) {
 	case 4:
-		err = nfp_cpp_readl(cpp, id, sym->addr, &val32);
+		err = nfp_cpp_readl(rtbl->cpp, id, sym->addr, &val32);
 		val = val32;
 		break;
 	case 8:
-		err = nfp_cpp_readq(cpp, id, sym->addr, &val);
+		err = nfp_cpp_readq(rtbl->cpp, id, sym->addr, &val);
 		break;
 	default:
-		nfp_err(cpp,
+		nfp_err(rtbl->cpp,
 			"rtsym '%s' unsupported or non-scalar size: %lld\n",
 			name, sym->size);
 		err = -EINVAL;
@@ -304,3 +289,30 @@ exit:
 		return ~0ULL;
 	return val;
 }
+
+u8 __iomem *
+nfp_rtsym_map(struct nfp_rtsym_table *rtbl, const char *name, const char *id,
+	      unsigned int min_size, struct nfp_cpp_area **area)
+{
+	const struct nfp_rtsym *sym;
+	u8 __iomem *mem;
+
+	sym = nfp_rtsym_lookup(rtbl, name);
+	if (!sym)
+		return (u8 __iomem *)ERR_PTR(-ENOENT);
+
+	if (sym->size < min_size) {
+		nfp_err(rtbl->cpp, "Symbol %s too small\n", name);
+		return (u8 __iomem *)ERR_PTR(-EINVAL);
+	}
+
+	mem = nfp_cpp_map_area(rtbl->cpp, id, sym->domain, sym->target,
+			       sym->addr, sym->size, area);
+	if (IS_ERR(mem)) {
+		nfp_err(rtbl->cpp, "Failed to map symbol %s: %ld\n",
+			name, PTR_ERR(mem));
+		return mem;
+	}
+
+	return mem;
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nic/main.c b/drivers/net/ethernet/netronome/nfp/nic/main.c
new file mode 100644
index 000000000000..520684242b7d
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nic/main.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2017 Netronome Systems, Inc.
+ *
+ * This software is dual licensed under the GNU General License Version 2,
+ * June 1991 as shown in the file COPYING in the top-level directory of this
+ * source tree or the BSD 2-Clause License provided below.  You have the
+ * option to license this software under the complete terms of either license.
+ *
+ * The BSD 2-Clause License:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      1. Redistributions of source code must retain the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer.
+ *
+ *      2. Redistributions in binary form must reproduce the above
+ *         copyright notice, this list of conditions and the following
+ *         disclaimer in the documentation and/or other materials
+ *         provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "../nfpcore/nfp_cpp.h"
+#include "../nfpcore/nfp_nsp.h"
+#include "../nfp_app.h"
+#include "../nfp_main.h"
+
+static int nfp_nic_init(struct nfp_app *app)
+{
+	struct nfp_pf *pf = app->pf;
+
+	if (pf->eth_tbl && pf->max_data_vnics != pf->eth_tbl->count) {
+		nfp_err(pf->cpp, "ETH entries don't match vNICs (%d vs %d)\n",
+			pf->max_data_vnics, pf->eth_tbl->count);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+const struct nfp_app_type app_nic = {
+	.id		= NFP_APP_CORE_NIC,
+	.name		= "nic",
+
+	.init		= nfp_nic_init,
+	.vnic_init	= nfp_app_nic_vnic_init,
+};
diff --git a/drivers/net/ethernet/nuvoton/w90p910_ether.c b/drivers/net/ethernet/nuvoton/w90p910_ether.c
index 159564d8dcdb..89ab786da25f 100644
--- a/drivers/net/ethernet/nuvoton/w90p910_ether.c
+++ b/drivers/net/ethernet/nuvoton/w90p910_ether.c
@@ -868,7 +868,10 @@ static int w90p910_get_link_ksettings(struct net_device *dev,
 				      struct ethtool_link_ksettings *cmd)
 {
 	struct w90p910_ether *ether = netdev_priv(dev);
-	return mii_ethtool_get_link_ksettings(&ether->mii, cmd);
+
+	mii_ethtool_get_link_ksettings(&ether->mii, cmd);
+
+	return 0;
 }
 
 static int w90p910_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/nxp/lpc_eth.c b/drivers/net/ethernet/nxp/lpc_eth.c
index 9c7ffd649e9a..08381ef8bdb4 100644
--- a/drivers/net/ethernet/nxp/lpc_eth.c
+++ b/drivers/net/ethernet/nxp/lpc_eth.c
@@ -919,7 +919,6 @@ static int __lpc_handle_recv(struct net_device *ndev, int budget)
 	struct sk_buff *skb;
 	u32 rxconsidx, len, ethst;
 	struct rx_status_t *prxstat;
-	u8 *prdbuf;
 	int rx_done = 0;
 
 	/* Get the current RX buffer indexes */
@@ -959,11 +958,10 @@ static int __lpc_handle_recv(struct net_device *ndev, int budget)
 			if (!skb) {
 				ndev->stats.rx_dropped++;
 			} else {
-				prdbuf = skb_put(skb, len);
-
 				/* Copy packet from buffer */
-				memcpy(prdbuf, pldat->rx_buff_v +
-					rxconsidx * ENET_MAXF_SIZE, len);
+				skb_put_data(skb,
+					     pldat->rx_buff_v + rxconsidx * ENET_MAXF_SIZE,
+					     len);
 
 				/* Pass to upper layer */
 				skb->protocol = eth_type_trans(skb, ndev);
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 21093276d2b7..731ce1e419e4 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
@@ -85,9 +85,8 @@ static int pch_gbe_get_link_ksettings(struct net_device *netdev,
 {
 	struct pch_gbe_adapter *adapter = netdev_priv(netdev);
 	u32 supported, advertising;
-	int ret;
 
-	ret = mii_ethtool_get_link_ksettings(&adapter->mii, ecmd);
+	mii_ethtool_get_link_ksettings(&adapter->mii, ecmd);
 
 	ethtool_convert_link_mode_to_legacy_u32(&supported,
 						ecmd->link_modes.supported);
@@ -104,7 +103,8 @@ static int pch_gbe_get_link_ksettings(struct net_device *netdev,
 
 	if (!netif_carrier_ok(adapter->netdev))
 		ecmd->base.speed = SPEED_UNKNOWN;
-	return ret;
+
+	return 0;
 }
 
 /**
diff --git a/drivers/net/ethernet/packetengines/hamachi.c b/drivers/net/ethernet/packetengines/hamachi.c
index 8b026dbf0d8d..482b85e4d665 100644
--- a/drivers/net/ethernet/packetengines/hamachi.c
+++ b/drivers/net/ethernet/packetengines/hamachi.c
@@ -1495,8 +1495,8 @@ static int hamachi_rx(struct net_device *dev)
 					hmp->rx_skbuff[entry]->data, pkt_len);
 				skb_put(skb, pkt_len);
 #else
-				memcpy(skb_put(skb, pkt_len), hmp->rx_ring_dma
-					+ entry*sizeof(*desc), pkt_len);
+				skb_put_data(skb, hmp->rx_ring_dma
+					     + entry*sizeof(*desc), pkt_len);
 #endif
 				pci_dma_sync_single_for_device(hmp->pci_dev,
 							       leXX_to_cpu(hmp->rx_ring[entry].addr),
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_ctx.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_ctx.c
index e30676515529..6cec2a6a3dcc 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_ctx.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_ctx.c
@@ -174,7 +174,6 @@ netxen_setup_minidump(struct netxen_adapter *adapter)
 {
 	int err = 0, i;
 	u32 *template, *tmp_buf;
-	struct netxen_minidump_template_hdr *hdr;
 	err = netxen_get_minidump_template_size(adapter);
 	if (err) {
 		adapter->mdump.fw_supports_md = 0;
@@ -218,8 +217,6 @@ netxen_setup_minidump(struct netxen_adapter *adapter)
 	template = (u32 *) adapter->mdump.md_template;
 	for (i = 0; i < adapter->mdump.md_template_size/sizeof(u32); i++)
 		*template++ = __le32_to_cpu(*tmp_buf++);
-	hdr = (struct netxen_minidump_template_hdr *)
-				adapter->mdump.md_template;
 	adapter->mdump.md_capture_buff = NULL;
 	adapter->mdump.fw_supports_md = 1;
 	adapter->mdump.md_enabled = 0;
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c
index a996801d442d..66ff15d08bad 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c
@@ -21,6 +21,7 @@
  *
  */
 
+#include <linux/io-64-nonatomic-lo-hi.h>
 #include <linux/slab.h>
 #include "netxen_nic.h"
 #include "netxen_nic_hw.h"
@@ -44,20 +45,6 @@ static void netxen_nic_io_write_128M(struct netxen_adapter *adapter,
 		void __iomem *addr, u32 data);
 static u32 netxen_nic_io_read_128M(struct netxen_adapter *adapter,
 		void __iomem *addr);
-#ifndef readq
-static inline u64 readq(void __iomem *addr)
-{
-	return readl(addr) | (((u64) readl(addr + 4)) << 32LL);
-}
-#endif
-
-#ifndef writeq
-static inline void writeq(u64 val, void __iomem *addr)
-{
-	writel(((u32) (val)), (addr));
-	writel(((u32) (val >> 32)), (addr + 4));
-}
-#endif
 
 #define PCI_OFFSET_FIRST_RANGE(adapter, off)    \
 	((adapter)->ahw.pci_base0 + (off))
diff --git a/drivers/net/ethernet/qlogic/qed/Makefile b/drivers/net/ethernet/qlogic/qed/Makefile
index 974929dcc74e..82dd47068e18 100644
--- a/drivers/net/ethernet/qlogic/qed/Makefile
+++ b/drivers/net/ethernet/qlogic/qed/Makefile
@@ -5,6 +5,6 @@ qed-y := qed_cxt.o qed_dev.o qed_hw.o qed_init_fw_funcs.o qed_init_ops.o \
 	 qed_selftest.o qed_dcbx.o qed_debug.o qed_ptp.o
 qed-$(CONFIG_QED_SRIOV) += qed_sriov.o qed_vf.o
 qed-$(CONFIG_QED_LL2) += qed_ll2.o
-qed-$(CONFIG_QED_RDMA) += qed_roce.o
+qed-$(CONFIG_QED_RDMA) += qed_roce.o qed_rdma.o qed_iwarp.o
 qed-$(CONFIG_QED_ISCSI) += qed_iscsi.o qed_ooo.o
 qed-$(CONFIG_QED_FCOE) += qed_fcoe.o
diff --git a/drivers/net/ethernet/qlogic/qed/qed.h b/drivers/net/ethernet/qlogic/qed/qed.h
index 2ab1aab7c3fe..91003bc6f00b 100644
--- a/drivers/net/ethernet/qlogic/qed/qed.h
+++ b/drivers/net/ethernet/qlogic/qed/qed.h
@@ -54,7 +54,7 @@ extern const struct qed_common_ops qed_common_ops_pass;
 
 #define QED_MAJOR_VERSION               8
 #define QED_MINOR_VERSION               10
-#define QED_REVISION_VERSION            10
+#define QED_REVISION_VERSION            11
 #define QED_ENGINEERING_VERSION 21
 
 #define QED_VERSION						 \
@@ -92,7 +92,7 @@ enum qed_mcp_protocol_type;
 
 #define QED_MFW_SET_FIELD(name, field, value)				       \
 	do {								       \
-		(name)	&= ~((field ## _MASK) << (field ## _SHIFT));	       \
+		(name)	&= ~(field ## _MASK);	       \
 		(name)	|= (((value) << (field ## _SHIFT)) & (field ## _MASK));\
 	} while (0)
 
@@ -210,14 +210,16 @@ struct qed_tunn_update_params {
 
 /* The PCI personality is not quite synonymous to protocol ID:
  * 1. All personalities need CORE connections
- * 2. The Ethernet personality may support also the RoCE protocol
+ * 2. The Ethernet personality may support also the RoCE/iWARP protocol
  */
 enum qed_pci_personality {
 	QED_PCI_ETH,
 	QED_PCI_FCOE,
 	QED_PCI_ISCSI,
 	QED_PCI_ETH_ROCE,
-	QED_PCI_DEFAULT /* default in shmem */
+	QED_PCI_ETH_IWARP,
+	QED_PCI_ETH_RDMA,
+	QED_PCI_DEFAULT, /* default in shmem */
 };
 
 /* All VFs are symmetric, all counters are PF + all VFs */
@@ -277,6 +279,7 @@ enum qed_dev_cap {
 	QED_DEV_CAP_FCOE,
 	QED_DEV_CAP_ISCSI,
 	QED_DEV_CAP_ROCE,
+	QED_DEV_CAP_IWARP,
 };
 
 enum qed_wol_support {
@@ -286,7 +289,24 @@ enum qed_wol_support {
 
 struct qed_hw_info {
 	/* PCI personality */
-	enum qed_pci_personality	personality;
+	enum qed_pci_personality personality;
+#define QED_IS_RDMA_PERSONALITY(dev)			    \
+	((dev)->hw_info.personality == QED_PCI_ETH_ROCE ||  \
+	 (dev)->hw_info.personality == QED_PCI_ETH_IWARP || \
+	 (dev)->hw_info.personality == QED_PCI_ETH_RDMA)
+#define QED_IS_ROCE_PERSONALITY(dev)			   \
+	((dev)->hw_info.personality == QED_PCI_ETH_ROCE || \
+	 (dev)->hw_info.personality == QED_PCI_ETH_RDMA)
+#define QED_IS_IWARP_PERSONALITY(dev)			    \
+	((dev)->hw_info.personality == QED_PCI_ETH_IWARP || \
+	 (dev)->hw_info.personality == QED_PCI_ETH_RDMA)
+#define QED_IS_L2_PERSONALITY(dev)		      \
+	((dev)->hw_info.personality == QED_PCI_ETH || \
+	 QED_IS_RDMA_PERSONALITY(dev))
+#define QED_IS_FCOE_PERSONALITY(dev) \
+	((dev)->hw_info.personality == QED_PCI_FCOE)
+#define QED_IS_ISCSI_PERSONALITY(dev) \
+	((dev)->hw_info.personality == QED_PCI_ISCSI)
 
 	/* Resource Allocation scheme results */
 	u32				resc_start[QED_MAX_RESC];
@@ -412,6 +432,11 @@ struct qed_fw_data {
 	u32			init_ops_size;
 };
 
+enum BAR_ID {
+	BAR_ID_0,		/* used for GRC */
+	BAR_ID_1		/* Used for doorbells */
+};
+
 #define DRV_MODULE_VERSION		      \
 	__stringify(QED_MAJOR_VERSION) "."    \
 	__stringify(QED_MINOR_VERSION) "."    \
@@ -495,10 +520,6 @@ struct qed_hwfn {
 	bool b_rdma_enabled_in_prs;
 	u32 rdma_prs_search_reg;
 
-	/* Array of sb_info of all status blocks */
-	struct qed_sb_info		*sbs_info[MAX_SB_PER_PF_MIMD];
-	u16				num_sbs;
-
 	struct qed_cxt_mngr		*p_cxt_mngr;
 
 	/* Flag indicating whether interrupts are enabled or not*/
@@ -537,6 +558,9 @@ struct qed_hwfn {
 	u8 dcbx_no_edpm;
 	u8 db_bar_no_edpm;
 
+	/* L2-related */
+	struct qed_l2_info *p_l2_info;
+
 	struct qed_ptt *p_arfs_ptt;
 
 	struct qed_simd_fp_handler	simd_proto_handler[64];
@@ -548,7 +572,6 @@ struct qed_hwfn {
 #endif
 
 	struct z_stream_s		*stream;
-	struct qed_roce_ll2_info	*ll2;
 };
 
 struct pci_params {
@@ -598,16 +621,11 @@ struct qed_dev {
 	enum	qed_dev_type type;
 /* Translate type/revision combo into the proper conditions */
 #define QED_IS_BB(dev)  ((dev)->type == QED_DEV_TYPE_BB)
-#define QED_IS_BB_A0(dev)       (QED_IS_BB(dev) && \
-				 CHIP_REV_IS_A0(dev))
 #define QED_IS_BB_B0(dev)       (QED_IS_BB(dev) && \
 				 CHIP_REV_IS_B0(dev))
 #define QED_IS_AH(dev)  ((dev)->type == QED_DEV_TYPE_AH)
 #define QED_IS_K2(dev)  QED_IS_AH(dev)
 
-#define QED_GET_TYPE(dev)       (QED_IS_BB_A0(dev) ? CHIP_BB_A0 : \
-				 QED_IS_BB_B0(dev) ? CHIP_BB_B0 : CHIP_K2)
-
 	u16	vendor_id;
 	u16	device_id;
 #define QED_DEV_ID_MASK		0xff00
@@ -621,7 +639,6 @@ struct qed_dev {
 	u16	chip_rev;
 #define CHIP_REV_MASK                   0xf
 #define CHIP_REV_SHIFT                  12
-#define CHIP_REV_IS_A0(_cdev)   (!(_cdev)->chip_rev)
 #define CHIP_REV_IS_B0(_cdev)   ((_cdev)->chip_rev == 1)
 
 	u16				chip_metal;
@@ -633,7 +650,7 @@ struct qed_dev {
 #define CHIP_BOND_ID_SHIFT              0
 
 	u8				num_engines;
-	u8				num_ports_in_engines;
+	u8				num_ports_in_engine;
 	u8				num_funcs_in_port;
 
 	u8				path_id;
@@ -644,7 +661,6 @@ struct qed_dev {
 
 	int				pcie_width;
 	int				pcie_speed;
-	u8				ver_str[VER_SIZE];
 
 	/* Add MF related configuration */
 	u8				mcp_rev;
@@ -763,7 +779,7 @@ static inline u8 qed_concrete_to_sw_fid(struct qed_dev *cdev,
 }
 
 #define PURE_LB_TC 8
-#define OOO_LB_TC 9
+#define PKT_LB_TC 9
 
 int qed_configure_vport_wfq(struct qed_dev *cdev, u16 vp_id, u32 rate);
 void qed_configure_vp_wfq_on_link_change(struct qed_dev *cdev,
@@ -773,6 +789,8 @@ void qed_configure_vp_wfq_on_link_change(struct qed_dev *cdev,
 void qed_clean_wfq_db(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
 int qed_device_num_engines(struct qed_dev *cdev);
 int qed_device_get_port_id(struct qed_dev *cdev);
+void qed_set_fw_mac_addr(__le16 *fw_msb,
+			 __le16 *fw_mid, __le16 *fw_lsb, u8 *mac);
 
 #define QED_LEADING_HWFN(dev)   (&dev->hwfns[0])
 
diff --git a/drivers/net/ethernet/qlogic/qed/qed_cxt.c b/drivers/net/ethernet/qlogic/qed/qed_cxt.c
index 694845793af2..af106be8cc08 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_cxt.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_cxt.c
@@ -135,7 +135,6 @@ struct qed_tid_seg {
 
 struct qed_conn_type_cfg {
 	u32 cid_count;
-	u32 cid_start;
 	u32 cids_per_vf;
 	struct qed_tid_seg tid_seg[TASK_SEGMENTS];
 };
@@ -222,6 +221,9 @@ struct qed_cxt_mngr {
 	/* Acquired CIDs */
 	struct qed_cid_acquired_map	acquired[MAX_CONN_TYPES];
 
+	struct qed_cid_acquired_map
+	acquired_vf[MAX_CONN_TYPES][MAX_NUM_VFS];
+
 	/* ILT  shadow table */
 	struct qed_dma_mem		*ilt_shadow;
 	u32				pf_start_line;
@@ -244,14 +246,16 @@ struct qed_cxt_mngr {
 static bool src_proto(enum protocol_type type)
 {
 	return type == PROTOCOLID_ISCSI ||
-	       type == PROTOCOLID_FCOE;
+	       type == PROTOCOLID_FCOE ||
+	       type == PROTOCOLID_IWARP;
 }
 
 static bool tm_cid_proto(enum protocol_type type)
 {
 	return type == PROTOCOLID_ISCSI ||
 	       type == PROTOCOLID_FCOE ||
-	       type == PROTOCOLID_ROCE;
+	       type == PROTOCOLID_ROCE ||
+	       type == PROTOCOLID_IWARP;
 }
 
 static bool tm_tid_proto(enum protocol_type type)
@@ -851,7 +855,7 @@ u32 qed_cxt_cfg_ilt_compute_excess(struct qed_hwfn *p_hwfn, u32 used_lines)
 	if (!excess_lines)
 		return 0;
 
-	if (p_hwfn->hw_info.personality != QED_PCI_ETH_ROCE)
+	if (!QED_IS_RDMA_PERSONALITY(p_hwfn))
 		return 0;
 
 	p_mngr = p_hwfn->p_cxt_mngr;
@@ -1031,7 +1035,7 @@ static int qed_ilt_blk_alloc(struct qed_hwfn *p_hwfn,
 	u32 lines, line, sz_left, lines_to_skip = 0;
 
 	/* Special handling for RoCE that supports dynamic allocation */
-	if ((p_hwfn->hw_info.personality == QED_PCI_ETH_ROCE) &&
+	if (QED_IS_RDMA_PERSONALITY(p_hwfn) &&
 	    ((ilt_client == ILT_CLI_CDUT) || ilt_client == ILT_CLI_TSDM))
 		return 0;
 
@@ -1121,45 +1125,76 @@ ilt_shadow_fail:
 static void qed_cid_map_free(struct qed_hwfn *p_hwfn)
 {
 	struct qed_cxt_mngr *p_mngr = p_hwfn->p_cxt_mngr;
-	u32 type;
+	u32 type, vf;
 
 	for (type = 0; type < MAX_CONN_TYPES; type++) {
 		kfree(p_mngr->acquired[type].cid_map);
 		p_mngr->acquired[type].max_count = 0;
 		p_mngr->acquired[type].start_cid = 0;
+
+		for (vf = 0; vf < MAX_NUM_VFS; vf++) {
+			kfree(p_mngr->acquired_vf[type][vf].cid_map);
+			p_mngr->acquired_vf[type][vf].max_count = 0;
+			p_mngr->acquired_vf[type][vf].start_cid = 0;
+		}
 	}
 }
 
+static int
+qed_cid_map_alloc_single(struct qed_hwfn *p_hwfn,
+			 u32 type,
+			 u32 cid_start,
+			 u32 cid_count, struct qed_cid_acquired_map *p_map)
+{
+	u32 size;
+
+	if (!cid_count)
+		return 0;
+
+	size = DIV_ROUND_UP(cid_count,
+			    sizeof(unsigned long) * BITS_PER_BYTE) *
+	       sizeof(unsigned long);
+	p_map->cid_map = kzalloc(size, GFP_KERNEL);
+	if (!p_map->cid_map)
+		return -ENOMEM;
+
+	p_map->max_count = cid_count;
+	p_map->start_cid = cid_start;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_CXT,
+		   "Type %08x start: %08x count %08x\n",
+		   type, p_map->start_cid, p_map->max_count);
+
+	return 0;
+}
+
 static int qed_cid_map_alloc(struct qed_hwfn *p_hwfn)
 {
 	struct qed_cxt_mngr *p_mngr = p_hwfn->p_cxt_mngr;
-	u32 start_cid = 0;
-	u32 type;
+	u32 start_cid = 0, vf_start_cid = 0;
+	u32 type, vf;
 
 	for (type = 0; type < MAX_CONN_TYPES; type++) {
-		u32 cid_cnt = p_hwfn->p_cxt_mngr->conn_cfg[type].cid_count;
-		u32 size;
-
-		if (cid_cnt == 0)
-			continue;
+		struct qed_conn_type_cfg *p_cfg = &p_mngr->conn_cfg[type];
+		struct qed_cid_acquired_map *p_map;
 
-		size = DIV_ROUND_UP(cid_cnt,
-				    sizeof(unsigned long) * BITS_PER_BYTE) *
-		       sizeof(unsigned long);
-		p_mngr->acquired[type].cid_map = kzalloc(size, GFP_KERNEL);
-		if (!p_mngr->acquired[type].cid_map)
+		/* Handle PF maps */
+		p_map = &p_mngr->acquired[type];
+		if (qed_cid_map_alloc_single(p_hwfn, type, start_cid,
+					     p_cfg->cid_count, p_map))
 			goto cid_map_fail;
 
-		p_mngr->acquired[type].max_count = cid_cnt;
-		p_mngr->acquired[type].start_cid = start_cid;
-
-		p_hwfn->p_cxt_mngr->conn_cfg[type].cid_start = start_cid;
+		/* Handle VF maps */
+		for (vf = 0; vf < MAX_NUM_VFS; vf++) {
+			p_map = &p_mngr->acquired_vf[type][vf];
+			if (qed_cid_map_alloc_single(p_hwfn, type,
+						     vf_start_cid,
+						     p_cfg->cids_per_vf, p_map))
+				goto cid_map_fail;
+		}
 
-		DP_VERBOSE(p_hwfn, QED_MSG_CXT,
-			   "Type %08x start: %08x count %08x\n",
-			   type, p_mngr->acquired[type].start_cid,
-			   p_mngr->acquired[type].max_count);
-		start_cid += cid_cnt;
+		start_cid += p_cfg->cid_count;
+		vf_start_cid += p_cfg->cids_per_vf;
 	}
 
 	return 0;
@@ -1265,19 +1300,36 @@ void qed_cxt_mngr_free(struct qed_hwfn *p_hwfn)
 void qed_cxt_mngr_setup(struct qed_hwfn *p_hwfn)
 {
 	struct qed_cxt_mngr *p_mngr = p_hwfn->p_cxt_mngr;
+	struct qed_cid_acquired_map *p_map;
+	struct qed_conn_type_cfg *p_cfg;
 	int type;
+	u32 len;
 
 	/* Reset acquired cids */
 	for (type = 0; type < MAX_CONN_TYPES; type++) {
-		u32 cid_cnt = p_hwfn->p_cxt_mngr->conn_cfg[type].cid_count;
+		u32 vf;
+
+		p_cfg = &p_mngr->conn_cfg[type];
+		if (p_cfg->cid_count) {
+			p_map = &p_mngr->acquired[type];
+			len = DIV_ROUND_UP(p_map->max_count,
+					   sizeof(unsigned long) *
+					   BITS_PER_BYTE) *
+			      sizeof(unsigned long);
+			memset(p_map->cid_map, 0, len);
+		}
 
-		if (cid_cnt == 0)
+		if (!p_cfg->cids_per_vf)
 			continue;
 
-		memset(p_mngr->acquired[type].cid_map, 0,
-		       DIV_ROUND_UP(cid_cnt,
-				    sizeof(unsigned long) * BITS_PER_BYTE) *
-		       sizeof(unsigned long));
+		for (vf = 0; vf < MAX_NUM_VFS; vf++) {
+			p_map = &p_mngr->acquired_vf[type][vf];
+			len = DIV_ROUND_UP(p_map->max_count,
+					   sizeof(unsigned long) *
+					   BITS_PER_BYTE) *
+			      sizeof(unsigned long);
+			memset(p_map->cid_map, 0, len);
+		}
 	}
 }
 
@@ -1783,7 +1835,7 @@ static void qed_tm_init_pf(struct qed_hwfn *p_hwfn)
 		tm_offset += tm_iids.pf_tids[i];
 	}
 
-	if (p_hwfn->hw_info.personality == QED_PCI_ETH_ROCE)
+	if (QED_IS_RDMA_PERSONALITY(p_hwfn))
 		active_seg_mask = 0;
 
 	STORE_RT_REG(p_hwfn, TM_REG_PF_ENABLE_TASK_RT_OFFSET, active_seg_mask);
@@ -1841,91 +1893,145 @@ void qed_cxt_hw_init_pf(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 	qed_prs_init_pf(p_hwfn);
 }
 
-int qed_cxt_acquire_cid(struct qed_hwfn *p_hwfn,
-			enum protocol_type type, u32 *p_cid)
+int _qed_cxt_acquire_cid(struct qed_hwfn *p_hwfn,
+			 enum protocol_type type, u32 *p_cid, u8 vfid)
 {
 	struct qed_cxt_mngr *p_mngr = p_hwfn->p_cxt_mngr;
+	struct qed_cid_acquired_map *p_map;
 	u32 rel_cid;
 
-	if (type >= MAX_CONN_TYPES || !p_mngr->acquired[type].cid_map) {
+	if (type >= MAX_CONN_TYPES) {
 		DP_NOTICE(p_hwfn, "Invalid protocol type %d", type);
 		return -EINVAL;
 	}
 
-	rel_cid = find_first_zero_bit(p_mngr->acquired[type].cid_map,
-				      p_mngr->acquired[type].max_count);
+	if (vfid >= MAX_NUM_VFS && vfid != QED_CXT_PF_CID) {
+		DP_NOTICE(p_hwfn, "VF [%02x] is out of range\n", vfid);
+		return -EINVAL;
+	}
 
-	if (rel_cid >= p_mngr->acquired[type].max_count) {
+	/* Determine the right map to take this CID from */
+	if (vfid == QED_CXT_PF_CID)
+		p_map = &p_mngr->acquired[type];
+	else
+		p_map = &p_mngr->acquired_vf[type][vfid];
+
+	if (!p_map->cid_map) {
+		DP_NOTICE(p_hwfn, "Invalid protocol type %d", type);
+		return -EINVAL;
+	}
+
+	rel_cid = find_first_zero_bit(p_map->cid_map, p_map->max_count);
+
+	if (rel_cid >= p_map->max_count) {
 		DP_NOTICE(p_hwfn, "no CID available for protocol %d\n", type);
 		return -EINVAL;
 	}
 
-	__set_bit(rel_cid, p_mngr->acquired[type].cid_map);
+	__set_bit(rel_cid, p_map->cid_map);
 
-	*p_cid = rel_cid + p_mngr->acquired[type].start_cid;
+	*p_cid = rel_cid + p_map->start_cid;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_CXT,
+		   "Acquired cid 0x%08x [rel. %08x] vfid %02x type %d\n",
+		   *p_cid, rel_cid, vfid, type);
 
 	return 0;
 }
 
+int qed_cxt_acquire_cid(struct qed_hwfn *p_hwfn,
+			enum protocol_type type, u32 *p_cid)
+{
+	return _qed_cxt_acquire_cid(p_hwfn, type, p_cid, QED_CXT_PF_CID);
+}
+
 static bool qed_cxt_test_cid_acquired(struct qed_hwfn *p_hwfn,
-				      u32 cid, enum protocol_type *p_type)
+				      u32 cid,
+				      u8 vfid,
+				      enum protocol_type *p_type,
+				      struct qed_cid_acquired_map **pp_map)
 {
 	struct qed_cxt_mngr *p_mngr = p_hwfn->p_cxt_mngr;
-	struct qed_cid_acquired_map *p_map;
-	enum protocol_type p;
 	u32 rel_cid;
 
 	/* Iterate over protocols and find matching cid range */
-	for (p = 0; p < MAX_CONN_TYPES; p++) {
-		p_map = &p_mngr->acquired[p];
+	for (*p_type = 0; *p_type < MAX_CONN_TYPES; (*p_type)++) {
+		if (vfid == QED_CXT_PF_CID)
+			*pp_map = &p_mngr->acquired[*p_type];
+		else
+			*pp_map = &p_mngr->acquired_vf[*p_type][vfid];
 
-		if (!p_map->cid_map)
+		if (!((*pp_map)->cid_map))
 			continue;
-		if (cid >= p_map->start_cid &&
-		    cid < p_map->start_cid + p_map->max_count)
+		if (cid >= (*pp_map)->start_cid &&
+		    cid < (*pp_map)->start_cid + (*pp_map)->max_count)
 			break;
 	}
-	*p_type = p;
 
-	if (p == MAX_CONN_TYPES) {
-		DP_NOTICE(p_hwfn, "Invalid CID %d", cid);
-		return false;
+	if (*p_type == MAX_CONN_TYPES) {
+		DP_NOTICE(p_hwfn, "Invalid CID %d vfid %02x", cid, vfid);
+		goto fail;
 	}
 
-	rel_cid = cid - p_map->start_cid;
-	if (!test_bit(rel_cid, p_map->cid_map)) {
-		DP_NOTICE(p_hwfn, "CID %d not acquired", cid);
-		return false;
+	rel_cid = cid - (*pp_map)->start_cid;
+	if (!test_bit(rel_cid, (*pp_map)->cid_map)) {
+		DP_NOTICE(p_hwfn, "CID %d [vifd %02x] not acquired",
+			  cid, vfid);
+		goto fail;
 	}
+
 	return true;
+fail:
+	*p_type = MAX_CONN_TYPES;
+	*pp_map = NULL;
+	return false;
 }
 
-void qed_cxt_release_cid(struct qed_hwfn *p_hwfn, u32 cid)
+void _qed_cxt_release_cid(struct qed_hwfn *p_hwfn, u32 cid, u8 vfid)
 {
-	struct qed_cxt_mngr *p_mngr = p_hwfn->p_cxt_mngr;
+	struct qed_cid_acquired_map *p_map = NULL;
 	enum protocol_type type;
 	bool b_acquired;
 	u32 rel_cid;
 
+	if (vfid != QED_CXT_PF_CID && vfid > MAX_NUM_VFS) {
+		DP_NOTICE(p_hwfn,
+			  "Trying to return incorrect CID belonging to VF %02x\n",
+			  vfid);
+		return;
+	}
+
 	/* Test acquired and find matching per-protocol map */
-	b_acquired = qed_cxt_test_cid_acquired(p_hwfn, cid, &type);
+	b_acquired = qed_cxt_test_cid_acquired(p_hwfn, cid, vfid,
+					       &type, &p_map);
 
 	if (!b_acquired)
 		return;
 
-	rel_cid = cid - p_mngr->acquired[type].start_cid;
-	__clear_bit(rel_cid, p_mngr->acquired[type].cid_map);
+	rel_cid = cid - p_map->start_cid;
+	clear_bit(rel_cid, p_map->cid_map);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_CXT,
+		   "Released CID 0x%08x [rel. %08x] vfid %02x type %d\n",
+		   cid, rel_cid, vfid, type);
+}
+
+void qed_cxt_release_cid(struct qed_hwfn *p_hwfn, u32 cid)
+{
+	_qed_cxt_release_cid(p_hwfn, cid, QED_CXT_PF_CID);
 }
 
 int qed_cxt_get_cid_info(struct qed_hwfn *p_hwfn, struct qed_cxt_info *p_info)
 {
 	struct qed_cxt_mngr *p_mngr = p_hwfn->p_cxt_mngr;
+	struct qed_cid_acquired_map *p_map = NULL;
 	u32 conn_cxt_size, hw_p_size, cxts_per_p, line;
 	enum protocol_type type;
 	bool b_acquired;
 
 	/* Test acquired and find matching per-protocol map */
-	b_acquired = qed_cxt_test_cid_acquired(p_hwfn, p_info->iid, &type);
+	b_acquired = qed_cxt_test_cid_acquired(p_hwfn, p_info->iid,
+					       QED_CXT_PF_CID, &type, &p_map);
 
 	if (!b_acquired)
 		return -EINVAL;
@@ -1964,6 +2070,11 @@ static void qed_rdma_set_pf_params(struct qed_hwfn *p_hwfn,
 	num_srqs = min_t(u32, 32 * 1024, p_params->num_srqs);
 
 	switch (p_hwfn->hw_info.personality) {
+	case QED_PCI_ETH_IWARP:
+		/* Each QP requires one connection */
+		num_cons = min_t(u32, IWARP_MAX_QPS, p_params->num_qps);
+		proto = PROTOCOLID_IWARP;
+		break;
 	case QED_PCI_ETH_ROCE:
 		num_qps = min_t(u32, ROCE_MAX_QPS, p_params->num_qps);
 		num_cons = num_qps * 2;	/* each QP requires two connections */
@@ -1999,6 +2110,8 @@ int qed_cxt_set_pf_params(struct qed_hwfn *p_hwfn, u32 rdma_tasks)
 	qed_cxt_set_proto_cid_count(p_hwfn, PROTOCOLID_CORE, core_cids, 0);
 
 	switch (p_hwfn->hw_info.personality) {
+	case QED_PCI_ETH_RDMA:
+	case QED_PCI_ETH_IWARP:
 	case QED_PCI_ETH_ROCE:
 	{
 			qed_rdma_set_pf_params(p_hwfn,
@@ -2012,8 +2125,12 @@ int qed_cxt_set_pf_params(struct qed_hwfn *p_hwfn, u32 rdma_tasks)
 		struct qed_eth_pf_params *p_params =
 		    &p_hwfn->pf_params.eth_pf_params;
 
-		qed_cxt_set_proto_cid_count(p_hwfn, PROTOCOLID_ETH,
-					    p_params->num_cons, 1);
+			if (!p_params->num_vf_cons)
+				p_params->num_vf_cons =
+				    ETH_PF_PARAMS_VF_CONS_DEFAULT;
+			qed_cxt_set_proto_cid_count(p_hwfn, PROTOCOLID_ETH,
+						    p_params->num_cons,
+						    p_params->num_vf_cons);
 		p_hwfn->p_cxt_mngr->arfs_count = p_params->num_arfs_filters;
 		break;
 	}
@@ -2236,7 +2353,7 @@ qed_cxt_dynamic_ilt_alloc(struct qed_hwfn *p_hwfn,
 		       last_cid_allocated - 1);
 
 		if (!p_hwfn->b_rdma_enabled_in_prs) {
-			/* Enable RoCE search */
+			/* Enable RDMA search */
 			qed_wr(p_hwfn, p_ptt, p_hwfn->rdma_prs_search_reg, 1);
 			p_hwfn->b_rdma_enabled_in_prs = true;
 		}
diff --git a/drivers/net/ethernet/qlogic/qed/qed_cxt.h b/drivers/net/ethernet/qlogic/qed/qed_cxt.h
index 53ad532dc212..17836349a274 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_cxt.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_cxt.h
@@ -54,19 +54,6 @@ struct qed_tid_mem {
 };
 
 /**
- * @brief qed_cxt_acquire - Acquire a new cid of a specific protocol type
- *
- * @param p_hwfn
- * @param type
- * @param p_cid
- *
- * @return int
- */
-int qed_cxt_acquire_cid(struct qed_hwfn *p_hwfn,
-			enum protocol_type type,
-			u32 *p_cid);
-
-/**
  * @brief qedo_cid_get_cxt_info - Returns the context info for a specific cid
  *
  *
@@ -195,14 +182,51 @@ void qed_qm_init_pf(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
  */
 int qed_qm_reconf(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
 
+#define QED_CXT_PF_CID (0xff)
+
 /**
  * @brief qed_cxt_release - Release a cid
  *
  * @param p_hwfn
  * @param cid
  */
-void qed_cxt_release_cid(struct qed_hwfn *p_hwfn,
-			 u32 cid);
+void qed_cxt_release_cid(struct qed_hwfn *p_hwfn, u32 cid);
+
+/**
+ * @brief qed_cxt_release - Release a cid belonging to a vf-queue
+ *
+ * @param p_hwfn
+ * @param cid
+ * @param vfid - engine relative index. QED_CXT_PF_CID if belongs to PF
+ */
+void _qed_cxt_release_cid(struct qed_hwfn *p_hwfn, u32 cid, u8 vfid);
+
+/**
+ * @brief qed_cxt_acquire - Acquire a new cid of a specific protocol type
+ *
+ * @param p_hwfn
+ * @param type
+ * @param p_cid
+ *
+ * @return int
+ */
+int qed_cxt_acquire_cid(struct qed_hwfn *p_hwfn,
+			enum protocol_type type, u32 *p_cid);
+
+/**
+ * @brief _qed_cxt_acquire - Acquire a new cid of a specific protocol type
+ *                           for a vf-queue
+ *
+ * @param p_hwfn
+ * @param type
+ * @param p_cid
+ * @param vfid - engine relative index. QED_CXT_PF_CID if belongs to PF
+ *
+ * @return int
+ */
+int _qed_cxt_acquire_cid(struct qed_hwfn *p_hwfn,
+			 enum protocol_type type, u32 *p_cid, u8 vfid);
+
 int qed_cxt_dynamic_ilt_alloc(struct qed_hwfn *p_hwfn,
 			      enum qed_cxt_elem_type elem_type, u32 iid);
 u32 qed_cxt_get_proto_tid_count(struct qed_hwfn *p_hwfn,
diff --git a/drivers/net/ethernet/qlogic/qed/qed_dcbx.c b/drivers/net/ethernet/qlogic/qed/qed_dcbx.c
index d883ad5bec6d..eaca4578435d 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_dcbx.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_dcbx.c
@@ -44,6 +44,7 @@
 #include "qed_hsi.h"
 #include "qed_sp.h"
 #include "qed_sriov.h"
+#include "qed_rdma.h"
 #ifdef CONFIG_DCB
 #include <linux/qed/qed_eth_if.h>
 #endif
@@ -191,17 +192,19 @@ static void
 qed_dcbx_set_params(struct qed_dcbx_results *p_data,
 		    struct qed_hw_info *p_info,
 		    bool enable,
-		    bool update,
 		    u8 prio,
 		    u8 tc,
 		    enum dcbx_protocol_type type,
 		    enum qed_pci_personality personality)
 {
 	/* PF update ramrod data */
-	p_data->arr[type].update = update;
 	p_data->arr[type].enable = enable;
 	p_data->arr[type].priority = prio;
 	p_data->arr[type].tc = tc;
+	if (enable)
+		p_data->arr[type].update = UPDATE_DCB;
+	else
+		p_data->arr[type].update = DONT_UPDATE_DCB_DSCP;
 
 	/* QM reconf data */
 	if (p_info->personality == personality)
@@ -213,7 +216,6 @@ static void
 qed_dcbx_update_app_info(struct qed_dcbx_results *p_data,
 			 struct qed_hwfn *p_hwfn,
 			 bool enable,
-			 bool update,
 			 u8 prio, u8 tc, enum dcbx_protocol_type type)
 {
 	struct qed_hw_info *p_info = &p_hwfn->hw_info;
@@ -231,7 +233,7 @@ qed_dcbx_update_app_info(struct qed_dcbx_results *p_data,
 		personality = qed_dcbx_app_update[i].personality;
 		name = qed_dcbx_app_update[i].name;
 
-		qed_dcbx_set_params(p_data, p_info, enable, update,
+		qed_dcbx_set_params(p_data, p_info, enable,
 				    prio, tc, type, personality);
 	}
 }
@@ -304,22 +306,11 @@ qed_dcbx_process_tlv(struct qed_hwfn *p_hwfn,
 			 */
 			enable = !(type == DCBX_PROTOCOL_ETH);
 
-			qed_dcbx_update_app_info(p_data, p_hwfn, enable, true,
+			qed_dcbx_update_app_info(p_data, p_hwfn, enable,
 						 priority, tc, type);
 		}
 	}
 
-	/* If RoCE-V2 TLV is not detected, driver need to use RoCE app
-	 * data for RoCE-v2 not the default app data.
-	 */
-	if (!p_data->arr[DCBX_PROTOCOL_ROCE_V2].update &&
-	    p_data->arr[DCBX_PROTOCOL_ROCE].update) {
-		tc = p_data->arr[DCBX_PROTOCOL_ROCE].tc;
-		priority = p_data->arr[DCBX_PROTOCOL_ROCE].priority;
-		qed_dcbx_update_app_info(p_data, p_hwfn, true, true,
-					 priority, tc, DCBX_PROTOCOL_ROCE_V2);
-	}
-
 	/* Update ramrod protocol data and hw_info fields
 	 * with default info when corresponding APP TLV's are not detected.
 	 * The enabled field has a different logic for ethernet as only for
@@ -332,8 +323,8 @@ qed_dcbx_process_tlv(struct qed_hwfn *p_hwfn,
 		if (p_data->arr[type].update)
 			continue;
 
-		enable = !(type == DCBX_PROTOCOL_ETH);
-		qed_dcbx_update_app_info(p_data, p_hwfn, enable, true,
+		enable = (type == DCBX_PROTOCOL_ETH) ? false : !!dcbx_version;
+		qed_dcbx_update_app_info(p_data, p_hwfn, enable,
 					 priority, tc, type);
 	}
 
@@ -902,10 +893,33 @@ qed_dcbx_mib_update_event(struct qed_hwfn *p_hwfn,
 
 			/* update storm FW with negotiation results */
 			qed_sp_pf_update(p_hwfn);
+
+			/* for roce PFs, we may want to enable/disable DPM
+			 * when DCBx change occurs
+			 */
+			if (p_hwfn->hw_info.personality ==
+			    QED_PCI_ETH_ROCE)
+				qed_roce_dpm_dcbx(p_hwfn, p_ptt);
 		}
 	}
 
 	qed_dcbx_get_params(p_hwfn, &p_hwfn->p_dcbx_info->get, type);
+
+	if (type == QED_DCBX_OPERATIONAL_MIB) {
+		struct qed_dcbx_results *p_data;
+		u16 val;
+
+		/* Configure in NIG which protocols support EDPM and should
+		 * honor PFC.
+		 */
+		p_data = &p_hwfn->p_dcbx_info->results;
+		val = (0x1 << p_data->arr[DCBX_PROTOCOL_ROCE].tc) |
+		      (0x1 << p_data->arr[DCBX_PROTOCOL_ROCE_V2].tc);
+		val <<= NIG_REG_TX_EDPM_CTRL_TX_EDPM_TC_EN_SHIFT;
+		val |= NIG_REG_TX_EDPM_CTRL_TX_EDPM_EN;
+		qed_wr(p_hwfn, p_ptt, NIG_REG_TX_EDPM_CTRL, val);
+	}
+
 	qed_dcbx_aen(p_hwfn, type);
 
 	return rc;
@@ -923,6 +937,7 @@ int qed_dcbx_info_alloc(struct qed_hwfn *p_hwfn)
 void qed_dcbx_info_free(struct qed_hwfn *p_hwfn)
 {
 	kfree(p_hwfn->p_dcbx_info);
+	p_hwfn->p_dcbx_info = NULL;
 }
 
 static void qed_dcbx_update_protocol_data(struct protocol_dcb_data *p_data,
@@ -944,17 +959,18 @@ void qed_dcbx_set_pf_update_params(struct qed_dcbx_results *p_src,
 	p_dest->pf_id = p_src->pf_id;
 
 	update_flag = p_src->arr[DCBX_PROTOCOL_FCOE].update;
-	p_dest->update_fcoe_dcb_data_flag = update_flag;
+	p_dest->update_fcoe_dcb_data_mode = update_flag;
 
 	update_flag = p_src->arr[DCBX_PROTOCOL_ROCE].update;
-	p_dest->update_roce_dcb_data_flag = update_flag;
+	p_dest->update_roce_dcb_data_mode = update_flag;
+
 	update_flag = p_src->arr[DCBX_PROTOCOL_ROCE_V2].update;
-	p_dest->update_roce_dcb_data_flag = update_flag;
+	p_dest->update_rroce_dcb_data_mode = update_flag;
 
 	update_flag = p_src->arr[DCBX_PROTOCOL_ISCSI].update;
-	p_dest->update_iscsi_dcb_data_flag = update_flag;
+	p_dest->update_iscsi_dcb_data_mode = update_flag;
 	update_flag = p_src->arr[DCBX_PROTOCOL_ETH].update;
-	p_dest->update_eth_dcb_data_flag = update_flag;
+	p_dest->update_eth_dcb_data_mode = update_flag;
 
 	p_dcb_data = &p_dest->fcoe_dcb_data;
 	qed_dcbx_update_protocol_data(p_dcb_data, p_src, DCBX_PROTOCOL_FCOE);
@@ -1458,7 +1474,7 @@ static u8 qed_dcbnl_getcap(struct qed_dev *cdev, int capid, u8 *cap)
 		break;
 	case DCB_CAP_ATTR_DCBX:
 		*cap = (DCB_CAP_DCBX_LLD_MANAGED | DCB_CAP_DCBX_VER_CEE |
-			DCB_CAP_DCBX_VER_IEEE);
+			DCB_CAP_DCBX_VER_IEEE | DCB_CAP_DCBX_STATIC);
 		break;
 	default:
 		*cap = false;
@@ -1532,6 +1548,8 @@ static u8 qed_dcbnl_getdcbx(struct qed_dev *cdev)
 		mode |= DCB_CAP_DCBX_VER_IEEE;
 	if (dcbx_info->operational.cee)
 		mode |= DCB_CAP_DCBX_VER_CEE;
+	if (dcbx_info->operational.local)
+		mode |= DCB_CAP_DCBX_STATIC;
 
 	DP_VERBOSE(hwfn, QED_MSG_DCB, "dcb mode = %d\n", mode);
 	kfree(dcbx_info);
diff --git a/drivers/net/ethernet/qlogic/qed/qed_dcbx.h b/drivers/net/ethernet/qlogic/qed/qed_dcbx.h
index 414e26268f3a..5feb90e049e0 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_dcbx.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_dcbx.h
@@ -52,7 +52,7 @@ enum qed_mib_read_type {
 
 struct qed_dcbx_app_data {
 	bool enable;		/* DCB enabled */
-	bool update;		/* Update indication */
+	u8 update;		/* Update indication */
 	u8 priority;		/* Priority */
 	u8 tc;			/* Traffic Class */
 };
diff --git a/drivers/net/ethernet/qlogic/qed/qed_debug.c b/drivers/net/ethernet/qlogic/qed/qed_debug.c
index 483241b4b05d..03c3cf77aaff 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_debug.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_debug.c
@@ -15,13 +15,6 @@
 #include "qed_mcp.h"
 #include "qed_reg_addr.h"
 
-/* Chip IDs enum */
-enum chip_ids {
-	CHIP_BB_B0,
-	CHIP_K2,
-	MAX_CHIP_IDS
-};
-
 /* Memory groups enum */
 enum mem_groups {
 	MEM_GROUP_PXP_MEM,
@@ -33,7 +26,6 @@ enum mem_groups {
 	MEM_GROUP_BRB_MEM,
 	MEM_GROUP_PRS_MEM,
 	MEM_GROUP_SDM_MEM,
-	MEM_GROUP_PBUF,
 	MEM_GROUP_IOR,
 	MEM_GROUP_RAM,
 	MEM_GROUP_BTB_RAM,
@@ -45,6 +37,7 @@ enum mem_groups {
 	MEM_GROUP_CAU_PI,
 	MEM_GROUP_CAU_MEM,
 	MEM_GROUP_PXP_ILT,
+	MEM_GROUP_PBUF,
 	MEM_GROUP_MULD_MEM,
 	MEM_GROUP_BTB_MEM,
 	MEM_GROUP_IGU_MEM,
@@ -66,7 +59,6 @@ static const char * const s_mem_group_names[] = {
 	"BRB_MEM",
 	"PRS_MEM",
 	"SDM_MEM",
-	"PBUF",
 	"IOR",
 	"RAM",
 	"BTB_RAM",
@@ -78,6 +70,7 @@ static const char * const s_mem_group_names[] = {
 	"CAU_PI",
 	"CAU_MEM",
 	"PXP_ILT",
+	"PBUF",
 	"MULD_MEM",
 	"BTB_MEM",
 	"IGU_MEM",
@@ -88,48 +81,59 @@ static const char * const s_mem_group_names[] = {
 };
 
 /* Idle check conditions */
-static u32 cond4(const u32 *r, const u32 *imm)
+
+static u32 cond5(const u32 *r, const u32 *imm)
 {
 	return ((r[0] & imm[0]) != imm[1]) && ((r[1] & imm[2]) != imm[3]);
 }
 
-static u32 cond6(const u32 *r, const u32 *imm)
+static u32 cond7(const u32 *r, const u32 *imm)
 {
 	return ((r[0] >> imm[0]) & imm[1]) != imm[2];
 }
 
-static u32 cond5(const u32 *r, const u32 *imm)
+static u32 cond14(const u32 *r, const u32 *imm)
+{
+	return (r[0] != imm[0]) && (((r[1] >> imm[1]) & imm[2]) == imm[3]);
+}
+
+static u32 cond6(const u32 *r, const u32 *imm)
 {
 	return (r[0] & imm[0]) != imm[1];
 }
 
-static u32 cond8(const u32 *r, const u32 *imm)
+static u32 cond9(const u32 *r, const u32 *imm)
 {
 	return ((r[0] & imm[0]) >> imm[1]) !=
 	    (((r[0] & imm[2]) >> imm[3]) | ((r[1] & imm[4]) << imm[5]));
 }
 
-static u32 cond9(const u32 *r, const u32 *imm)
+static u32 cond10(const u32 *r, const u32 *imm)
 {
 	return ((r[0] & imm[0]) >> imm[1]) != (r[0] & imm[2]);
 }
 
-static u32 cond1(const u32 *r, const u32 *imm)
+static u32 cond4(const u32 *r, const u32 *imm)
 {
 	return (r[0] & ~imm[0]) != imm[1];
 }
 
 static u32 cond0(const u32 *r, const u32 *imm)
 {
+	return (r[0] & ~r[1]) != imm[0];
+}
+
+static u32 cond1(const u32 *r, const u32 *imm)
+{
 	return r[0] != imm[0];
 }
 
-static u32 cond10(const u32 *r, const u32 *imm)
+static u32 cond11(const u32 *r, const u32 *imm)
 {
 	return r[0] != r[1] && r[2] == imm[0];
 }
 
-static u32 cond11(const u32 *r, const u32 *imm)
+static u32 cond12(const u32 *r, const u32 *imm)
 {
 	return r[0] != r[1] && r[2] > imm[0];
 }
@@ -139,12 +143,12 @@ static u32 cond3(const u32 *r, const u32 *imm)
 	return r[0] != r[1];
 }
 
-static u32 cond12(const u32 *r, const u32 *imm)
+static u32 cond13(const u32 *r, const u32 *imm)
 {
 	return r[0] & imm[0];
 }
 
-static u32 cond7(const u32 *r, const u32 *imm)
+static u32 cond8(const u32 *r, const u32 *imm)
 {
 	return r[0] < (r[1] - imm[0]);
 }
@@ -169,6 +173,8 @@ static u32(*cond_arr[]) (const u32 *r, const u32 *imm) = {
 	cond10,
 	cond11,
 	cond12,
+	cond13,
+	cond14,
 };
 
 /******************************* Data Types **********************************/
@@ -181,11 +187,6 @@ enum platform_ids {
 	MAX_PLATFORM_IDS
 };
 
-struct dbg_array {
-	const u32 *ptr;
-	u32 size_in_dwords;
-};
-
 struct chip_platform_defs {
 	u8 num_ports;
 	u8 num_pfs;
@@ -204,7 +205,9 @@ struct platform_defs {
 	u32 delay_factor;
 };
 
-/* Storm constant definitions */
+/* Storm constant definitions.
+ * Addresses are in bytes, sizes are in quad-regs.
+ */
 struct storm_defs {
 	char letter;
 	enum block_id block_id;
@@ -218,13 +221,13 @@ struct storm_defs {
 	u32 sem_sync_dbg_empty_addr;
 	u32 sem_slow_dbg_empty_addr;
 	u32 cm_ctx_wr_addr;
-	u32 cm_conn_ag_ctx_lid_size; /* In quad-regs */
+	u32 cm_conn_ag_ctx_lid_size;
 	u32 cm_conn_ag_ctx_rd_addr;
-	u32 cm_conn_st_ctx_lid_size; /* In quad-regs */
+	u32 cm_conn_st_ctx_lid_size;
 	u32 cm_conn_st_ctx_rd_addr;
-	u32 cm_task_ag_ctx_lid_size; /* In quad-regs */
+	u32 cm_task_ag_ctx_lid_size;
 	u32 cm_task_ag_ctx_rd_addr;
-	u32 cm_task_st_ctx_lid_size; /* In quad-regs */
+	u32 cm_task_st_ctx_lid_size;
 	u32 cm_task_st_ctx_rd_addr;
 };
 
@@ -233,17 +236,23 @@ struct block_defs {
 	const char *name;
 	bool has_dbg_bus[MAX_CHIP_IDS];
 	bool associated_to_storm;
-	u32 storm_id; /* Valid only if associated_to_storm is true */
+
+	/* Valid only if associated_to_storm is true */
+	u32 storm_id;
 	enum dbg_bus_clients dbg_client_id[MAX_CHIP_IDS];
 	u32 dbg_select_addr;
-	u32 dbg_cycle_enable_addr;
+	u32 dbg_enable_addr;
 	u32 dbg_shift_addr;
 	u32 dbg_force_valid_addr;
 	u32 dbg_force_frame_addr;
 	bool has_reset_bit;
-	bool unreset; /* If true, the block is taken out of reset before dump */
+
+	/* If true, block is taken out of reset before dump */
+	bool unreset;
 	enum dbg_reset_regs reset_reg;
-	u8 reset_bit_offset; /* Bit offset in reset register */
+
+	/* Bit offset in reset register */
+	u8 reset_bit_offset;
 };
 
 /* Reset register definitions */
@@ -262,12 +271,13 @@ struct grc_param_defs {
 	u32 crash_preset_val;
 };
 
+/* Address is in 128b units. Width is in bits. */
 struct rss_mem_defs {
 	const char *mem_name;
 	const char *type_name;
-	u32 addr; /* In 128b units */
+	u32 addr;
 	u32 num_entries[MAX_CHIP_IDS];
-	u32 entry_width[MAX_CHIP_IDS]; /* In bits */
+	u32 entry_width[MAX_CHIP_IDS];
 };
 
 struct vfc_ram_defs {
@@ -289,10 +299,20 @@ struct big_ram_defs {
 
 struct phy_defs {
 	const char *phy_name;
+
+	/* PHY base GRC address */
 	u32 base_addr;
+
+	/* Relative address of indirect TBUS address register (bits 0..7) */
 	u32 tbus_addr_lo_addr;
+
+	/* Relative address of indirect TBUS address register (bits 8..10) */
 	u32 tbus_addr_hi_addr;
+
+	/* Relative address of indirect TBUS data register (bits 0..7) */
 	u32 tbus_data_lo_addr;
+
+	/* Relative address of indirect TBUS data register (bits 8..11) */
 	u32 tbus_data_hi_addr;
 };
 
@@ -300,9 +320,11 @@ struct phy_defs {
 
 #define MAX_LCIDS			320
 #define MAX_LTIDS			320
+
 #define NUM_IOR_SETS			2
 #define IORS_PER_SET			176
 #define IOR_SET_OFFSET(set_id)		((set_id) * 256)
+
 #define BYTES_IN_DWORD			sizeof(u32)
 
 /* In the macros below, size and offset are specified in bits */
@@ -315,6 +337,7 @@ struct phy_defs {
 #define FIELD_BIT_MASK(type, field) \
 	(((1 << FIELD_BIT_SIZE(type, field)) - 1) << \
 	 FIELD_DWORD_SHIFT(type, field))
+
 #define SET_VAR_FIELD(var, type, field, val) \
 	do { \
 		var[FIELD_DWORD_OFFSET(type, field)] &=	\
@@ -322,31 +345,51 @@ struct phy_defs {
 		var[FIELD_DWORD_OFFSET(type, field)] |= \
 		(val) << FIELD_DWORD_SHIFT(type, field); \
 	} while (0)
+
 #define ARR_REG_WR(dev, ptt, addr, arr, arr_size) \
 	do { \
 		for (i = 0; i < (arr_size); i++) \
 			qed_wr(dev, ptt, addr,	(arr)[i]); \
 	} while (0)
+
 #define ARR_REG_RD(dev, ptt, addr, arr, arr_size) \
 	do { \
 		for (i = 0; i < (arr_size); i++) \
 			(arr)[i] = qed_rd(dev, ptt, addr); \
 	} while (0)
 
+#ifndef DWORDS_TO_BYTES
 #define DWORDS_TO_BYTES(dwords)		((dwords) * BYTES_IN_DWORD)
+#endif
+#ifndef BYTES_TO_DWORDS
 #define BYTES_TO_DWORDS(bytes)		((bytes) / BYTES_IN_DWORD)
+#endif
+
+/* extra lines include a signature line + optional latency events line */
+#ifndef NUM_DBG_LINES
+#define NUM_EXTRA_DBG_LINES(block_desc) \
+	(1 + ((block_desc)->has_latency_events ? 1 : 0))
+#define NUM_DBG_LINES(block_desc) \
+	((block_desc)->num_of_lines + NUM_EXTRA_DBG_LINES(block_desc))
+#endif
+
 #define RAM_LINES_TO_DWORDS(lines)	((lines) * 2)
 #define RAM_LINES_TO_BYTES(lines) \
 	DWORDS_TO_BYTES(RAM_LINES_TO_DWORDS(lines))
+
 #define REG_DUMP_LEN_SHIFT		24
 #define MEM_DUMP_ENTRY_SIZE_DWORDS \
 	BYTES_TO_DWORDS(sizeof(struct dbg_dump_mem))
+
 #define IDLE_CHK_RULE_SIZE_DWORDS \
 	BYTES_TO_DWORDS(sizeof(struct dbg_idle_chk_rule))
+
 #define IDLE_CHK_RESULT_HDR_DWORDS \
 	BYTES_TO_DWORDS(sizeof(struct dbg_idle_chk_result_hdr))
+
 #define IDLE_CHK_RESULT_REG_HDR_DWORDS \
 	BYTES_TO_DWORDS(sizeof(struct dbg_idle_chk_result_reg_hdr))
+
 #define IDLE_CHK_MAX_ENTRIES_SIZE	32
 
 /* The sizes and offsets below are specified in bits */
@@ -363,62 +406,92 @@ struct phy_defs {
 #define VFC_RAM_ADDR_ROW_OFFSET		2
 #define VFC_RAM_ADDR_ROW_SIZE		10
 #define VFC_RAM_RESP_STRUCT_SIZE	256
+
 #define VFC_CAM_CMD_DWORDS		CEIL_DWORDS(VFC_CAM_CMD_STRUCT_SIZE)
 #define VFC_CAM_ADDR_DWORDS		CEIL_DWORDS(VFC_CAM_ADDR_STRUCT_SIZE)
 #define VFC_CAM_RESP_DWORDS		CEIL_DWORDS(VFC_CAM_RESP_STRUCT_SIZE)
 #define VFC_RAM_CMD_DWORDS		VFC_CAM_CMD_DWORDS
 #define VFC_RAM_ADDR_DWORDS		CEIL_DWORDS(VFC_RAM_ADDR_STRUCT_SIZE)
 #define VFC_RAM_RESP_DWORDS		CEIL_DWORDS(VFC_RAM_RESP_STRUCT_SIZE)
+
 #define NUM_VFC_RAM_TYPES		4
+
 #define VFC_CAM_NUM_ROWS		512
+
 #define VFC_OPCODE_CAM_RD		14
 #define VFC_OPCODE_RAM_RD		0
+
 #define NUM_RSS_MEM_TYPES		5
+
 #define NUM_BIG_RAM_TYPES		3
 #define BIG_RAM_BLOCK_SIZE_BYTES	128
 #define BIG_RAM_BLOCK_SIZE_DWORDS \
 	BYTES_TO_DWORDS(BIG_RAM_BLOCK_SIZE_BYTES)
+
 #define NUM_PHY_TBUS_ADDRESSES		2048
 #define PHY_DUMP_SIZE_DWORDS		(NUM_PHY_TBUS_ADDRESSES / 2)
+
 #define RESET_REG_UNRESET_OFFSET	4
+
 #define STALL_DELAY_MS			500
+
 #define STATIC_DEBUG_LINE_DWORDS	9
-#define NUM_DBG_BUS_LINES		256
+
 #define NUM_COMMON_GLOBAL_PARAMS	8
+
 #define FW_IMG_MAIN			1
-#define REG_FIFO_DEPTH_ELEMENTS		32
+
+#ifndef REG_FIFO_ELEMENT_DWORDS
 #define REG_FIFO_ELEMENT_DWORDS		2
+#endif
+#define REG_FIFO_DEPTH_ELEMENTS		32
 #define REG_FIFO_DEPTH_DWORDS \
 	(REG_FIFO_ELEMENT_DWORDS * REG_FIFO_DEPTH_ELEMENTS)
-#define IGU_FIFO_DEPTH_ELEMENTS		64
+
+#ifndef IGU_FIFO_ELEMENT_DWORDS
 #define IGU_FIFO_ELEMENT_DWORDS		4
+#endif
+#define IGU_FIFO_DEPTH_ELEMENTS		64
 #define IGU_FIFO_DEPTH_DWORDS \
 	(IGU_FIFO_ELEMENT_DWORDS * IGU_FIFO_DEPTH_ELEMENTS)
-#define PROTECTION_OVERRIDE_DEPTH_ELEMENTS	20
+
+#ifndef PROTECTION_OVERRIDE_ELEMENT_DWORDS
 #define PROTECTION_OVERRIDE_ELEMENT_DWORDS	2
+#endif
+#define PROTECTION_OVERRIDE_DEPTH_ELEMENTS	20
 #define PROTECTION_OVERRIDE_DEPTH_DWORDS \
 	(PROTECTION_OVERRIDE_DEPTH_ELEMENTS * \
 	 PROTECTION_OVERRIDE_ELEMENT_DWORDS)
+
 #define MCP_SPAD_TRACE_OFFSIZE_ADDR \
 	(MCP_REG_SCRATCH + \
 	 offsetof(struct static_init, sections[SPAD_SECTION_TRACE]))
-#define MCP_TRACE_META_IMAGE_SIGNATURE  0x669955aa
+
 #define EMPTY_FW_VERSION_STR		"???_???_???_???"
 #define EMPTY_FW_IMAGE_STR		"???????????????"
 
 /***************************** Constant Arrays *******************************/
 
+struct dbg_array {
+	const u32 *ptr;
+	u32 size_in_dwords;
+};
+
 /* Debug arrays */
-static struct dbg_array s_dbg_arrays[MAX_BIN_DBG_BUFFER_TYPE] = { {0} };
+static struct dbg_array s_dbg_arrays[MAX_BIN_DBG_BUFFER_TYPE] = { {NULL} };
 
 /* Chip constant definitions array */
 static struct chip_defs s_chip_defs[MAX_CHIP_IDS] = {
-	{ "bb_b0",
-	  { {MAX_NUM_PORTS_BB, MAX_NUM_PFS_BB, MAX_NUM_VFS_BB}, {0, 0, 0},
-	    {0, 0, 0}, {0, 0, 0} } },
-	{ "k2",
-	  { {MAX_NUM_PORTS_K2, MAX_NUM_PFS_K2, MAX_NUM_VFS_K2}, {0, 0, 0},
-	    {0, 0, 0}, {0, 0, 0} } }
+	{ "bb",
+	  {{MAX_NUM_PORTS_BB, MAX_NUM_PFS_BB, MAX_NUM_VFS_BB},
+	   {0, 0, 0},
+	   {0, 0, 0},
+	   {0, 0, 0} } },
+	{ "ah",
+	  {{MAX_NUM_PORTS_K2, MAX_NUM_PFS_K2, MAX_NUM_VFS_K2},
+	   {0, 0, 0},
+	   {0, 0, 0},
+	   {0, 0, 0} } }
 };
 
 /* Storm constant definitions array */
@@ -427,69 +500,74 @@ static struct storm_defs s_storm_defs[] = {
 	{'T', BLOCK_TSEM,
 	 {DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCT}, true,
 	 TSEM_REG_FAST_MEMORY,
-	 TSEM_REG_DBG_FRAME_MODE, TSEM_REG_SLOW_DBG_ACTIVE,
-	 TSEM_REG_SLOW_DBG_MODE, TSEM_REG_DBG_MODE1_CFG,
-	 TSEM_REG_SYNC_DBG_EMPTY, TSEM_REG_SLOW_DBG_EMPTY,
+	 TSEM_REG_DBG_FRAME_MODE_BB_K2, TSEM_REG_SLOW_DBG_ACTIVE_BB_K2,
+	 TSEM_REG_SLOW_DBG_MODE_BB_K2, TSEM_REG_DBG_MODE1_CFG_BB_K2,
+	 TSEM_REG_SYNC_DBG_EMPTY, TSEM_REG_SLOW_DBG_EMPTY_BB_K2,
 	 TCM_REG_CTX_RBC_ACCS,
 	 4, TCM_REG_AGG_CON_CTX,
 	 16, TCM_REG_SM_CON_CTX,
 	 2, TCM_REG_AGG_TASK_CTX,
 	 4, TCM_REG_SM_TASK_CTX},
+
 	/* Mstorm */
 	{'M', BLOCK_MSEM,
 	 {DBG_BUS_CLIENT_RBCT, DBG_BUS_CLIENT_RBCM}, false,
 	 MSEM_REG_FAST_MEMORY,
-	 MSEM_REG_DBG_FRAME_MODE, MSEM_REG_SLOW_DBG_ACTIVE,
-	 MSEM_REG_SLOW_DBG_MODE, MSEM_REG_DBG_MODE1_CFG,
-	 MSEM_REG_SYNC_DBG_EMPTY, MSEM_REG_SLOW_DBG_EMPTY,
+	 MSEM_REG_DBG_FRAME_MODE_BB_K2, MSEM_REG_SLOW_DBG_ACTIVE_BB_K2,
+	 MSEM_REG_SLOW_DBG_MODE_BB_K2, MSEM_REG_DBG_MODE1_CFG_BB_K2,
+	 MSEM_REG_SYNC_DBG_EMPTY, MSEM_REG_SLOW_DBG_EMPTY_BB_K2,
 	 MCM_REG_CTX_RBC_ACCS,
 	 1, MCM_REG_AGG_CON_CTX,
 	 10, MCM_REG_SM_CON_CTX,
 	 2, MCM_REG_AGG_TASK_CTX,
 	 7, MCM_REG_SM_TASK_CTX},
+
 	/* Ustorm */
 	{'U', BLOCK_USEM,
 	 {DBG_BUS_CLIENT_RBCU, DBG_BUS_CLIENT_RBCU}, false,
 	 USEM_REG_FAST_MEMORY,
-	 USEM_REG_DBG_FRAME_MODE, USEM_REG_SLOW_DBG_ACTIVE,
-	 USEM_REG_SLOW_DBG_MODE, USEM_REG_DBG_MODE1_CFG,
-	 USEM_REG_SYNC_DBG_EMPTY, USEM_REG_SLOW_DBG_EMPTY,
+	 USEM_REG_DBG_FRAME_MODE_BB_K2, USEM_REG_SLOW_DBG_ACTIVE_BB_K2,
+	 USEM_REG_SLOW_DBG_MODE_BB_K2, USEM_REG_DBG_MODE1_CFG_BB_K2,
+	 USEM_REG_SYNC_DBG_EMPTY, USEM_REG_SLOW_DBG_EMPTY_BB_K2,
 	 UCM_REG_CTX_RBC_ACCS,
 	 2, UCM_REG_AGG_CON_CTX,
 	 13, UCM_REG_SM_CON_CTX,
 	 3, UCM_REG_AGG_TASK_CTX,
 	 3, UCM_REG_SM_TASK_CTX},
+
 	/* Xstorm */
 	{'X', BLOCK_XSEM,
 	 {DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCX}, false,
 	 XSEM_REG_FAST_MEMORY,
-	 XSEM_REG_DBG_FRAME_MODE, XSEM_REG_SLOW_DBG_ACTIVE,
-	 XSEM_REG_SLOW_DBG_MODE, XSEM_REG_DBG_MODE1_CFG,
-	 XSEM_REG_SYNC_DBG_EMPTY, XSEM_REG_SLOW_DBG_EMPTY,
+	 XSEM_REG_DBG_FRAME_MODE_BB_K2, XSEM_REG_SLOW_DBG_ACTIVE_BB_K2,
+	 XSEM_REG_SLOW_DBG_MODE_BB_K2, XSEM_REG_DBG_MODE1_CFG_BB_K2,
+	 XSEM_REG_SYNC_DBG_EMPTY, XSEM_REG_SLOW_DBG_EMPTY_BB_K2,
 	 XCM_REG_CTX_RBC_ACCS,
 	 9, XCM_REG_AGG_CON_CTX,
 	 15, XCM_REG_SM_CON_CTX,
 	 0, 0,
 	 0, 0},
+
 	/* Ystorm */
 	{'Y', BLOCK_YSEM,
 	 {DBG_BUS_CLIENT_RBCX, DBG_BUS_CLIENT_RBCY}, false,
 	 YSEM_REG_FAST_MEMORY,
-	 YSEM_REG_DBG_FRAME_MODE, YSEM_REG_SLOW_DBG_ACTIVE,
-	 YSEM_REG_SLOW_DBG_MODE, YSEM_REG_DBG_MODE1_CFG,
-	 YSEM_REG_SYNC_DBG_EMPTY, TSEM_REG_SLOW_DBG_EMPTY,
+	 YSEM_REG_DBG_FRAME_MODE_BB_K2, YSEM_REG_SLOW_DBG_ACTIVE_BB_K2,
+	 YSEM_REG_SLOW_DBG_MODE_BB_K2, YSEM_REG_DBG_MODE1_CFG_BB_K2,
+	 YSEM_REG_SYNC_DBG_EMPTY, TSEM_REG_SLOW_DBG_EMPTY_BB_K2,
 	 YCM_REG_CTX_RBC_ACCS,
 	 2, YCM_REG_AGG_CON_CTX,
 	 3, YCM_REG_SM_CON_CTX,
 	 2, YCM_REG_AGG_TASK_CTX,
 	 12, YCM_REG_SM_TASK_CTX},
+
 	/* Pstorm */
 	{'P', BLOCK_PSEM,
 	 {DBG_BUS_CLIENT_RBCS, DBG_BUS_CLIENT_RBCS}, true,
 	 PSEM_REG_FAST_MEMORY,
-	 PSEM_REG_DBG_FRAME_MODE, PSEM_REG_SLOW_DBG_ACTIVE,
-	 PSEM_REG_SLOW_DBG_MODE, PSEM_REG_DBG_MODE1_CFG,
-	 PSEM_REG_SYNC_DBG_EMPTY, PSEM_REG_SLOW_DBG_EMPTY,
+	 PSEM_REG_DBG_FRAME_MODE_BB_K2, PSEM_REG_SLOW_DBG_ACTIVE_BB_K2,
+	 PSEM_REG_SLOW_DBG_MODE_BB_K2, PSEM_REG_DBG_MODE1_CFG_BB_K2,
+	 PSEM_REG_SYNC_DBG_EMPTY, PSEM_REG_SLOW_DBG_EMPTY_BB_K2,
 	 PCM_REG_CTX_RBC_ACCS,
 	 0, 0,
 	 10, PCM_REG_SM_CON_CTX,
@@ -498,6 +576,7 @@ static struct storm_defs s_storm_defs[] = {
 };
 
 /* Block definitions array */
+
 static struct block_defs block_grc_defs = {
 	"grc",
 	{true, true}, false, 0,
@@ -587,9 +666,11 @@ static struct block_defs block_pcie_defs = {
 	"pcie",
 	{false, true}, false, 0,
 	{MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCH},
-	PCIE_REG_DBG_COMMON_SELECT, PCIE_REG_DBG_COMMON_DWORD_ENABLE,
-	PCIE_REG_DBG_COMMON_SHIFT, PCIE_REG_DBG_COMMON_FORCE_VALID,
-	PCIE_REG_DBG_COMMON_FORCE_FRAME,
+	PCIE_REG_DBG_COMMON_SELECT_K2,
+	PCIE_REG_DBG_COMMON_DWORD_ENABLE_K2,
+	PCIE_REG_DBG_COMMON_SHIFT_K2,
+	PCIE_REG_DBG_COMMON_FORCE_VALID_K2,
+	PCIE_REG_DBG_COMMON_FORCE_FRAME_K2,
 	false, false, MAX_DBG_RESET_REGS, 0
 };
 
@@ -691,9 +772,9 @@ static struct block_defs block_pglcs_defs = {
 	"pglcs",
 	{false, true}, false, 0,
 	{MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCH},
-	PGLCS_REG_DBG_SELECT, PGLCS_REG_DBG_DWORD_ENABLE,
-	PGLCS_REG_DBG_SHIFT, PGLCS_REG_DBG_FORCE_VALID,
-	PGLCS_REG_DBG_FORCE_FRAME,
+	PGLCS_REG_DBG_SELECT_K2, PGLCS_REG_DBG_DWORD_ENABLE_K2,
+	PGLCS_REG_DBG_SHIFT_K2, PGLCS_REG_DBG_FORCE_VALID_K2,
+	PGLCS_REG_DBG_FORCE_FRAME_K2,
 	true, false, DBG_RESET_REG_MISCS_PL_HV, 2
 };
 
@@ -991,10 +1072,11 @@ static struct block_defs block_yuld_defs = {
 	"yuld",
 	{true, true}, false, 0,
 	{DBG_BUS_CLIENT_RBCU, DBG_BUS_CLIENT_RBCU},
-	YULD_REG_DBG_SELECT, YULD_REG_DBG_DWORD_ENABLE,
-	YULD_REG_DBG_SHIFT, YULD_REG_DBG_FORCE_VALID,
-	YULD_REG_DBG_FORCE_FRAME,
-	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2, 15
+	YULD_REG_DBG_SELECT_BB_K2, YULD_REG_DBG_DWORD_ENABLE_BB_K2,
+	YULD_REG_DBG_SHIFT_BB_K2, YULD_REG_DBG_FORCE_VALID_BB_K2,
+	YULD_REG_DBG_FORCE_FRAME_BB_K2,
+	true, true, DBG_RESET_REG_MISC_PL_PDA_VMAIN_2,
+	15
 };
 
 static struct block_defs block_xyld_defs = {
@@ -1143,9 +1225,9 @@ static struct block_defs block_umac_defs = {
 	"umac",
 	{false, true}, false, 0,
 	{MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCZ},
-	UMAC_REG_DBG_SELECT, UMAC_REG_DBG_DWORD_ENABLE,
-	UMAC_REG_DBG_SHIFT, UMAC_REG_DBG_FORCE_VALID,
-	UMAC_REG_DBG_FORCE_FRAME,
+	UMAC_REG_DBG_SELECT_K2, UMAC_REG_DBG_DWORD_ENABLE_K2,
+	UMAC_REG_DBG_SHIFT_K2, UMAC_REG_DBG_FORCE_VALID_K2,
+	UMAC_REG_DBG_FORCE_FRAME_K2,
 	true, false, DBG_RESET_REG_MISCS_PL_HV, 6
 };
 
@@ -1177,9 +1259,9 @@ static struct block_defs block_wol_defs = {
 	"wol",
 	{false, true}, false, 0,
 	{MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCZ},
-	WOL_REG_DBG_SELECT, WOL_REG_DBG_DWORD_ENABLE,
-	WOL_REG_DBG_SHIFT, WOL_REG_DBG_FORCE_VALID,
-	WOL_REG_DBG_FORCE_FRAME,
+	WOL_REG_DBG_SELECT_K2, WOL_REG_DBG_DWORD_ENABLE_K2,
+	WOL_REG_DBG_SHIFT_K2, WOL_REG_DBG_FORCE_VALID_K2,
+	WOL_REG_DBG_FORCE_FRAME_K2,
 	true, true, DBG_RESET_REG_MISC_PL_PDA_VAUX, 7
 };
 
@@ -1187,9 +1269,9 @@ static struct block_defs block_bmbn_defs = {
 	"bmbn",
 	{false, true}, false, 0,
 	{MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCB},
-	BMBN_REG_DBG_SELECT, BMBN_REG_DBG_DWORD_ENABLE,
-	BMBN_REG_DBG_SHIFT, BMBN_REG_DBG_FORCE_VALID,
-	BMBN_REG_DBG_FORCE_FRAME,
+	BMBN_REG_DBG_SELECT_K2, BMBN_REG_DBG_DWORD_ENABLE_K2,
+	BMBN_REG_DBG_SHIFT_K2, BMBN_REG_DBG_FORCE_VALID_K2,
+	BMBN_REG_DBG_FORCE_FRAME_K2,
 	false, false, MAX_DBG_RESET_REGS, 0
 };
 
@@ -1204,9 +1286,9 @@ static struct block_defs block_nwm_defs = {
 	"nwm",
 	{false, true}, false, 0,
 	{MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCW},
-	NWM_REG_DBG_SELECT, NWM_REG_DBG_DWORD_ENABLE,
-	NWM_REG_DBG_SHIFT, NWM_REG_DBG_FORCE_VALID,
-	NWM_REG_DBG_FORCE_FRAME,
+	NWM_REG_DBG_SELECT_K2, NWM_REG_DBG_DWORD_ENABLE_K2,
+	NWM_REG_DBG_SHIFT_K2, NWM_REG_DBG_FORCE_VALID_K2,
+	NWM_REG_DBG_FORCE_FRAME_K2,
 	true, false, DBG_RESET_REG_MISCS_PL_HV_2, 0
 };
 
@@ -1214,9 +1296,9 @@ static struct block_defs block_nws_defs = {
 	"nws",
 	{false, true}, false, 0,
 	{MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCW},
-	NWS_REG_DBG_SELECT, NWS_REG_DBG_DWORD_ENABLE,
-	NWS_REG_DBG_SHIFT, NWS_REG_DBG_FORCE_VALID,
-	NWS_REG_DBG_FORCE_FRAME,
+	NWS_REG_DBG_SELECT_K2, NWS_REG_DBG_DWORD_ENABLE_K2,
+	NWS_REG_DBG_SHIFT_K2, NWS_REG_DBG_FORCE_VALID_K2,
+	NWS_REG_DBG_FORCE_FRAME_K2,
 	true, false, DBG_RESET_REG_MISCS_PL_HV, 12
 };
 
@@ -1224,9 +1306,9 @@ static struct block_defs block_ms_defs = {
 	"ms",
 	{false, true}, false, 0,
 	{MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCZ},
-	MS_REG_DBG_SELECT, MS_REG_DBG_DWORD_ENABLE,
-	MS_REG_DBG_SHIFT, MS_REG_DBG_FORCE_VALID,
-	MS_REG_DBG_FORCE_FRAME,
+	MS_REG_DBG_SELECT_K2, MS_REG_DBG_DWORD_ENABLE_K2,
+	MS_REG_DBG_SHIFT_K2, MS_REG_DBG_FORCE_VALID_K2,
+	MS_REG_DBG_FORCE_FRAME_K2,
 	true, false, DBG_RESET_REG_MISCS_PL_HV, 13
 };
 
@@ -1234,9 +1316,11 @@ static struct block_defs block_phy_pcie_defs = {
 	"phy_pcie",
 	{false, true}, false, 0,
 	{MAX_DBG_BUS_CLIENTS, DBG_BUS_CLIENT_RBCH},
-	PCIE_REG_DBG_COMMON_SELECT, PCIE_REG_DBG_COMMON_DWORD_ENABLE,
-	PCIE_REG_DBG_COMMON_SHIFT, PCIE_REG_DBG_COMMON_FORCE_VALID,
-	PCIE_REG_DBG_COMMON_FORCE_FRAME,
+	PCIE_REG_DBG_COMMON_SELECT_K2,
+	PCIE_REG_DBG_COMMON_DWORD_ENABLE_K2,
+	PCIE_REG_DBG_COMMON_SHIFT_K2,
+	PCIE_REG_DBG_COMMON_FORCE_VALID_K2,
+	PCIE_REG_DBG_COMMON_FORCE_FRAME_K2,
 	false, false, MAX_DBG_RESET_REGS, 0
 };
 
@@ -1261,6 +1345,13 @@ static struct block_defs block_rgfs_defs = {
 	false, false, MAX_DBG_RESET_REGS, 0
 };
 
+static struct block_defs block_rgsrc_defs = {
+	"rgsrc", {false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	false, false, MAX_DBG_RESET_REGS, 0
+};
+
 static struct block_defs block_tgfs_defs = {
 	"tgfs", {false, false}, false, 0,
 	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
@@ -1268,6 +1359,13 @@ static struct block_defs block_tgfs_defs = {
 	false, false, MAX_DBG_RESET_REGS, 0
 };
 
+static struct block_defs block_tgsrc_defs = {
+	"tgsrc", {false, false}, false, 0,
+	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
+	0, 0, 0, 0, 0,
+	false, false, MAX_DBG_RESET_REGS, 0
+};
+
 static struct block_defs block_ptld_defs = {
 	"ptld", {false, false}, false, 0,
 	{MAX_DBG_BUS_CLIENTS, MAX_DBG_BUS_CLIENTS},
@@ -1350,6 +1448,8 @@ static struct block_defs *s_block_defs[MAX_BLOCK_ID] = {
 	&block_muld_defs,
 	&block_yuld_defs,
 	&block_xyld_defs,
+	&block_ptld_defs,
+	&block_ypld_defs,
 	&block_prm_defs,
 	&block_pbf_pb1_defs,
 	&block_pbf_pb2_defs,
@@ -1363,6 +1463,10 @@ static struct block_defs *s_block_defs[MAX_BLOCK_ID] = {
 	&block_tcfc_defs,
 	&block_igu_defs,
 	&block_cau_defs,
+	&block_rgfs_defs,
+	&block_rgsrc_defs,
+	&block_tgfs_defs,
+	&block_tgsrc_defs,
 	&block_umac_defs,
 	&block_xmac_defs,
 	&block_dbg_defs,
@@ -1376,10 +1480,6 @@ static struct block_defs *s_block_defs[MAX_BLOCK_ID] = {
 	&block_phy_pcie_defs,
 	&block_led_defs,
 	&block_avs_wrap_defs,
-	&block_rgfs_defs,
-	&block_tgfs_defs,
-	&block_ptld_defs,
-	&block_ypld_defs,
 	&block_misc_aeu_defs,
 	&block_bar0_map_defs,
 };
@@ -1392,66 +1492,151 @@ static struct platform_defs s_platform_defs[] = {
 };
 
 static struct grc_param_defs s_grc_param_defs[] = {
-	{{1, 1}, 0, 1, false, 1, 1},	/* DBG_GRC_PARAM_DUMP_TSTORM */
-	{{1, 1}, 0, 1, false, 1, 1},	/* DBG_GRC_PARAM_DUMP_MSTORM */
-	{{1, 1}, 0, 1, false, 1, 1},	/* DBG_GRC_PARAM_DUMP_USTORM */
-	{{1, 1}, 0, 1, false, 1, 1},	/* DBG_GRC_PARAM_DUMP_XSTORM */
-	{{1, 1}, 0, 1, false, 1, 1},	/* DBG_GRC_PARAM_DUMP_YSTORM */
-	{{1, 1}, 0, 1, false, 1, 1},	/* DBG_GRC_PARAM_DUMP_PSTORM */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_REGS */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_RAM */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_PBUF */
-	{{0, 0}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_IOR */
-	{{0, 0}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_VFC */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_CM_CTX */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_ILT */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_RSS */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_CAU */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_QM */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_MCP */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_RESERVED */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_CFC */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_IGU */
-	{{0, 0}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_BRB */
-	{{0, 0}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_BTB */
-	{{0, 0}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_BMB */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_NIG */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_MULD */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_PRS */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_DMAE */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_TM */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_SDM */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_DIF */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_STATIC */
-	{{0, 0}, 0, 1, false, 0, 0},	/* DBG_GRC_PARAM_UNSTALL */
+	/* DBG_GRC_PARAM_DUMP_TSTORM */
+	{{1, 1}, 0, 1, false, 1, 1},
+
+	/* DBG_GRC_PARAM_DUMP_MSTORM */
+	{{1, 1}, 0, 1, false, 1, 1},
+
+	/* DBG_GRC_PARAM_DUMP_USTORM */
+	{{1, 1}, 0, 1, false, 1, 1},
+
+	/* DBG_GRC_PARAM_DUMP_XSTORM */
+	{{1, 1}, 0, 1, false, 1, 1},
+
+	/* DBG_GRC_PARAM_DUMP_YSTORM */
+	{{1, 1}, 0, 1, false, 1, 1},
+
+	/* DBG_GRC_PARAM_DUMP_PSTORM */
+	{{1, 1}, 0, 1, false, 1, 1},
+
+	/* DBG_GRC_PARAM_DUMP_REGS */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_RAM */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_PBUF */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_IOR */
+	{{0, 0}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_VFC */
+	{{0, 0}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_CM_CTX */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_ILT */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_RSS */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_CAU */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_QM */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_MCP */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_RESERVED */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_CFC */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_IGU */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_BRB */
+	{{0, 0}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_BTB */
+	{{0, 0}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_BMB */
+	{{0, 0}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_NIG */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_MULD */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_PRS */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_DMAE */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_TM */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_SDM */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_DIF */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_STATIC */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_UNSTALL */
+	{{0, 0}, 0, 1, false, 0, 0},
+
+	/* DBG_GRC_PARAM_NUM_LCIDS */
 	{{MAX_LCIDS, MAX_LCIDS}, 1, MAX_LCIDS, false, MAX_LCIDS,
-	 MAX_LCIDS},			/* DBG_GRC_PARAM_NUM_LCIDS */
+	 MAX_LCIDS},
+
+	/* DBG_GRC_PARAM_NUM_LTIDS */
 	{{MAX_LTIDS, MAX_LTIDS}, 1, MAX_LTIDS, false, MAX_LTIDS,
-	 MAX_LTIDS},			/* DBG_GRC_PARAM_NUM_LTIDS */
-	{{0, 0}, 0, 1, true, 0, 0},	/* DBG_GRC_PARAM_EXCLUDE_ALL */
-	{{0, 0}, 0, 1, true, 0, 0},	/* DBG_GRC_PARAM_CRASH */
-	{{0, 0}, 0, 1, false, 1, 0},	/* DBG_GRC_PARAM_PARITY_SAFE */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_CM */
-	{{1, 1}, 0, 1, false, 0, 1},	/* DBG_GRC_PARAM_DUMP_PHY */
-	{{0, 0}, 0, 1, false, 0, 0},	/* DBG_GRC_PARAM_NO_MCP */
-	{{0, 0}, 0, 1, false, 0, 0}	/* DBG_GRC_PARAM_NO_FW_VER */
+	 MAX_LTIDS},
+
+	/* DBG_GRC_PARAM_EXCLUDE_ALL */
+	{{0, 0}, 0, 1, true, 0, 0},
+
+	/* DBG_GRC_PARAM_CRASH */
+	{{0, 0}, 0, 1, true, 0, 0},
+
+	/* DBG_GRC_PARAM_PARITY_SAFE */
+	{{0, 0}, 0, 1, false, 1, 0},
+
+	/* DBG_GRC_PARAM_DUMP_CM */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_DUMP_PHY */
+	{{1, 1}, 0, 1, false, 0, 1},
+
+	/* DBG_GRC_PARAM_NO_MCP */
+	{{0, 0}, 0, 1, false, 0, 0},
+
+	/* DBG_GRC_PARAM_NO_FW_VER */
+	{{0, 0}, 0, 1, false, 0, 0}
 };
 
 static struct rss_mem_defs s_rss_mem_defs[] = {
 	{ "rss_mem_cid", "rss_cid", 0,
 	  {256, 320},
 	  {32, 32} },
+
 	{ "rss_mem_key_msb", "rss_key", 1024,
 	  {128, 208},
 	  {256, 256} },
+
 	{ "rss_mem_key_lsb", "rss_key", 2048,
 	  {128, 208},
 	  {64, 64} },
+
 	{ "rss_mem_info", "rss_info", 3072,
 	  {128, 208},
 	  {16, 16} },
+
 	{ "rss_mem_ind", "rss_ind", 4096,
-	  {(128 * 128), (128 * 208)},
+	  {16384, 26624},
 	  {16, 16} }
 };
 
@@ -1466,50 +1651,71 @@ static struct big_ram_defs s_big_ram_defs[] = {
 	{ "BRB", MEM_GROUP_BRB_MEM, MEM_GROUP_BRB_RAM, DBG_GRC_PARAM_DUMP_BRB,
 	  BRB_REG_BIG_RAM_ADDRESS, BRB_REG_BIG_RAM_DATA,
 	  {4800, 5632} },
+
 	{ "BTB", MEM_GROUP_BTB_MEM, MEM_GROUP_BTB_RAM, DBG_GRC_PARAM_DUMP_BTB,
 	  BTB_REG_BIG_RAM_ADDRESS, BTB_REG_BIG_RAM_DATA,
 	  {2880, 3680} },
+
 	{ "BMB", MEM_GROUP_BMB_MEM, MEM_GROUP_BMB_RAM, DBG_GRC_PARAM_DUMP_BMB,
 	  BMB_REG_BIG_RAM_ADDRESS, BMB_REG_BIG_RAM_DATA,
 	  {1152, 1152} }
 };
 
 static struct reset_reg_defs s_reset_regs_defs[] = {
+	/* DBG_RESET_REG_MISCS_PL_UA */
 	{ MISCS_REG_RESET_PL_UA, 0x0,
-	  {true, true} },		/* DBG_RESET_REG_MISCS_PL_UA */
+	  {true, true} },
+
+	/* DBG_RESET_REG_MISCS_PL_HV */
 	{ MISCS_REG_RESET_PL_HV, 0x0,
-	  {true, true} },		/* DBG_RESET_REG_MISCS_PL_HV */
-	{ MISCS_REG_RESET_PL_HV_2, 0x0,
-	  {false, true} },	/* DBG_RESET_REG_MISCS_PL_HV_2 */
+	  {true, true} },
+
+	/* DBG_RESET_REG_MISCS_PL_HV_2 */
+	{ MISCS_REG_RESET_PL_HV_2_K2, 0x0,
+	  {false, true} },
+
+	/* DBG_RESET_REG_MISC_PL_UA */
 	{ MISC_REG_RESET_PL_UA, 0x0,
-	  {true, true} },		/* DBG_RESET_REG_MISC_PL_UA */
+	  {true, true} },
+
+	/* DBG_RESET_REG_MISC_PL_HV */
 	{ MISC_REG_RESET_PL_HV, 0x0,
-	  {true, true} },		/* DBG_RESET_REG_MISC_PL_HV */
+	  {true, true} },
+
+	/* DBG_RESET_REG_MISC_PL_PDA_VMAIN_1 */
 	{ MISC_REG_RESET_PL_PDA_VMAIN_1, 0x4404040,
-	  {true, true} },		/* DBG_RESET_REG_MISC_PL_PDA_VMAIN_1 */
+	  {true, true} },
+
+	/* DBG_RESET_REG_MISC_PL_PDA_VMAIN_2 */
 	{ MISC_REG_RESET_PL_PDA_VMAIN_2, 0x7c00007,
-	  {true, true} },		/* DBG_RESET_REG_MISC_PL_PDA_VMAIN_2 */
+	  {true, true} },
+
+	/* DBG_RESET_REG_MISC_PL_PDA_VAUX */
 	{ MISC_REG_RESET_PL_PDA_VAUX, 0x2,
-	  {true, true} },		/* DBG_RESET_REG_MISC_PL_PDA_VAUX */
+	  {true, true} },
 };
 
 static struct phy_defs s_phy_defs[] = {
-	{"nw_phy", NWS_REG_NWS_CMU, PHY_NW_IP_REG_PHY0_TOP_TBUS_ADDR_7_0,
-	 PHY_NW_IP_REG_PHY0_TOP_TBUS_ADDR_15_8,
-	 PHY_NW_IP_REG_PHY0_TOP_TBUS_DATA_7_0,
-	 PHY_NW_IP_REG_PHY0_TOP_TBUS_DATA_11_8},
-	{"sgmii_phy", MS_REG_MS_CMU, PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X132,
-	 PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X133,
-	 PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X130,
-	 PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X131},
-	{"pcie_phy0", PHY_PCIE_REG_PHY0, PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X132,
-	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X133,
-	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X130,
-	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X131},
-	{"pcie_phy1", PHY_PCIE_REG_PHY1, PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X132,
-	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X133,
-	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X130,
-	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X131},
+	{"nw_phy", NWS_REG_NWS_CMU_K2,
+	 PHY_NW_IP_REG_PHY0_TOP_TBUS_ADDR_7_0_K2,
+	 PHY_NW_IP_REG_PHY0_TOP_TBUS_ADDR_15_8_K2,
+	 PHY_NW_IP_REG_PHY0_TOP_TBUS_DATA_7_0_K2,
+	 PHY_NW_IP_REG_PHY0_TOP_TBUS_DATA_11_8_K2},
+	{"sgmii_phy", MS_REG_MS_CMU_K2,
+	 PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X132_K2,
+	 PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X133_K2,
+	 PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X130_K2,
+	 PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X131_K2},
+	{"pcie_phy0", PHY_PCIE_REG_PHY0_K2,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X132_K2,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X133_K2,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X130_K2,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X131_K2},
+	{"pcie_phy1", PHY_PCIE_REG_PHY1_K2,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X132_K2,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X133_K2,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X130_K2,
+	 PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X131_K2},
 };
 
 /**************************** Private Functions ******************************/
@@ -1556,7 +1762,7 @@ static enum dbg_status qed_dbg_dev_init(struct qed_hwfn *p_hwfn,
 		dev_data->chip_id = CHIP_K2;
 		dev_data->mode_enable[MODE_K2] = 1;
 	} else if (QED_IS_BB_B0(p_hwfn->cdev)) {
-		dev_data->chip_id = CHIP_BB_B0;
+		dev_data->chip_id = CHIP_BB;
 		dev_data->mode_enable[MODE_BB] = 1;
 	} else {
 		return DBG_STATUS_UNKNOWN_CHIP;
@@ -1569,9 +1775,20 @@ static enum dbg_status qed_dbg_dev_init(struct qed_hwfn *p_hwfn,
 	qed_dbg_grc_init_params(p_hwfn);
 
 	dev_data->initialized = true;
+
 	return DBG_STATUS_OK;
 }
 
+static struct dbg_bus_block *get_dbg_bus_block_desc(struct qed_hwfn *p_hwfn,
+						    enum block_id block_id)
+{
+	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+
+	return (struct dbg_bus_block *)&dbg_bus_blocks[block_id *
+						       MAX_CHIP_IDS +
+						       dev_data->chip_id];
+}
+
 /* Reads the FW info structure for the specified Storm from the chip,
  * and writes it to the specified fw_info pointer.
  */
@@ -1579,25 +1796,28 @@ static void qed_read_fw_info(struct qed_hwfn *p_hwfn,
 			     struct qed_ptt *p_ptt,
 			     u8 storm_id, struct fw_info *fw_info)
 {
-	/* Read first the address that points to fw_info location.
-	 * The address is located in the last line of the Storm RAM.
-	 */
-	u32 addr = s_storm_defs[storm_id].sem_fast_mem_addr +
-		   SEM_FAST_REG_INT_RAM +
-		   DWORDS_TO_BYTES(SEM_FAST_REG_INT_RAM_SIZE) -
-		   sizeof(struct fw_info_location);
+	struct storm_defs *storm = &s_storm_defs[storm_id];
 	struct fw_info_location fw_info_location;
-	u32 *dest = (u32 *)&fw_info_location;
-	u32 i;
+	u32 addr, i, *dest;
 
 	memset(&fw_info_location, 0, sizeof(fw_info_location));
 	memset(fw_info, 0, sizeof(*fw_info));
+
+	/* Read first the address that points to fw_info location.
+	 * The address is located in the last line of the Storm RAM.
+	 */
+	addr = storm->sem_fast_mem_addr + SEM_FAST_REG_INT_RAM +
+	       DWORDS_TO_BYTES(SEM_FAST_REG_INT_RAM_SIZE) -
+	       sizeof(fw_info_location);
+	dest = (u32 *)&fw_info_location;
+
 	for (i = 0; i < BYTES_TO_DWORDS(sizeof(fw_info_location));
 	     i++, addr += BYTES_IN_DWORD)
 		dest[i] = qed_rd(p_hwfn, p_ptt, addr);
+
+	/* Read FW version info from Storm RAM */
 	if (fw_info_location.size > 0 && fw_info_location.size <=
 	    sizeof(*fw_info)) {
-		/* Read FW version info from Storm RAM */
 		addr = fw_info_location.grc_addr;
 		dest = (u32 *)fw_info;
 		for (i = 0; i < BYTES_TO_DWORDS(fw_info_location.size);
@@ -1606,27 +1826,30 @@ static void qed_read_fw_info(struct qed_hwfn *p_hwfn,
 	}
 }
 
-/* Dumps the specified string to the specified buffer. Returns the dumped size
- * in bytes (actual length + 1 for the null character termination).
+/* Dumps the specified string to the specified buffer.
+ * Returns the dumped size in bytes.
  */
 static u32 qed_dump_str(char *dump_buf, bool dump, const char *str)
 {
 	if (dump)
 		strcpy(dump_buf, str);
+
 	return (u32)strlen(str) + 1;
 }
 
-/* Dumps zeros to align the specified buffer to dwords. Returns the dumped size
- * in bytes.
+/* Dumps zeros to align the specified buffer to dwords.
+ * Returns the dumped size in bytes.
  */
 static u32 qed_dump_align(char *dump_buf, bool dump, u32 byte_offset)
 {
-	u8 offset_in_dword = (u8)(byte_offset & 0x3), align_size;
+	u8 offset_in_dword, align_size;
 
+	offset_in_dword = (u8)(byte_offset & 0x3);
 	align_size = offset_in_dword ? BYTES_IN_DWORD - offset_in_dword : 0;
 
 	if (dump && align_size)
 		memset(dump_buf, 0, align_size);
+
 	return align_size;
 }
 
@@ -1653,6 +1876,7 @@ static u32 qed_dump_str_param(u32 *dump_buf,
 
 	/* Align buffer to next dword */
 	offset += qed_dump_align(char_buf + offset, dump, offset);
+
 	return BYTES_TO_DWORDS(offset);
 }
 
@@ -1681,6 +1905,7 @@ static u32 qed_dump_num_param(u32 *dump_buf,
 	if (dump)
 		*(dump_buf + offset) = param_val;
 	offset++;
+
 	return offset;
 }
 
@@ -1695,7 +1920,6 @@ static u32 qed_dump_fw_ver_param(struct qed_hwfn *p_hwfn,
 	char fw_ver_str[16] = EMPTY_FW_VERSION_STR;
 	char fw_img_str[16] = EMPTY_FW_IMAGE_STR;
 	struct fw_info fw_info = { {0}, {0} };
-	int printed_chars;
 	u32 offset = 0;
 
 	if (dump && !qed_grc_get_param(p_hwfn, DBG_GRC_PARAM_NO_FW_VER)) {
@@ -1705,37 +1929,32 @@ static u32 qed_dump_fw_ver_param(struct qed_hwfn *p_hwfn,
 
 		for (storm_id = 0; storm_id < MAX_DBG_STORMS && !found;
 		     storm_id++) {
-			/* Read FW version/image  */
-			if (!dev_data->block_in_reset
-			    [s_storm_defs[storm_id].block_id]) {
-				/* read FW info for the current Storm */
-				qed_read_fw_info(p_hwfn,
-						 p_ptt, storm_id, &fw_info);
-
-				/* Create FW version/image strings */
-				printed_chars =
-				    snprintf(fw_ver_str,
-					     sizeof(fw_ver_str),
-					     "%d_%d_%d_%d",
-					     fw_info.ver.num.major,
-					     fw_info.ver.num.minor,
-					     fw_info.ver.num.rev,
-					     fw_info.ver.num.eng);
-				if (printed_chars < 0 || printed_chars >=
-				    sizeof(fw_ver_str))
-					DP_NOTICE(p_hwfn,
-						  "Unexpected debug error: invalid FW version string\n");
-				switch (fw_info.ver.image_id) {
-				case FW_IMG_MAIN:
-					strcpy(fw_img_str, "main");
-					break;
-				default:
-					strcpy(fw_img_str, "unknown");
-					break;
-				}
+			struct storm_defs *storm = &s_storm_defs[storm_id];
 
-				found = true;
+			/* Read FW version/image */
+			if (dev_data->block_in_reset[storm->block_id])
+				continue;
+
+			/* Read FW info for the current Storm */
+			qed_read_fw_info(p_hwfn, p_ptt, storm_id, &fw_info);
+
+			/* Create FW version/image strings */
+			if (snprintf(fw_ver_str, sizeof(fw_ver_str),
+				     "%d_%d_%d_%d", fw_info.ver.num.major,
+				     fw_info.ver.num.minor, fw_info.ver.num.rev,
+				     fw_info.ver.num.eng) < 0)
+				DP_NOTICE(p_hwfn,
+					  "Unexpected debug error: invalid FW version string\n");
+			switch (fw_info.ver.image_id) {
+			case FW_IMG_MAIN:
+				strcpy(fw_img_str, "main");
+				break;
+			default:
+				strcpy(fw_img_str, "unknown");
+				break;
 			}
+
+			found = true;
 		}
 	}
 
@@ -1747,6 +1966,7 @@ static u32 qed_dump_fw_ver_param(struct qed_hwfn *p_hwfn,
 	offset += qed_dump_num_param(dump_buf + offset,
 				     dump,
 				     "fw-timestamp", fw_info.ver.timestamp);
+
 	return offset;
 }
 
@@ -1759,17 +1979,18 @@ static u32 qed_dump_mfw_ver_param(struct qed_hwfn *p_hwfn,
 {
 	char mfw_ver_str[16] = EMPTY_FW_VERSION_STR;
 
-	if (dump && !qed_grc_get_param(p_hwfn, DBG_GRC_PARAM_NO_FW_VER)) {
+	if (dump &&
+	    !qed_grc_get_param(p_hwfn, DBG_GRC_PARAM_NO_FW_VER)) {
 		u32 global_section_offsize, global_section_addr, mfw_ver;
 		u32 public_data_addr, global_section_offsize_addr;
-		int printed_chars;
 
-		/* Find MCP public data GRC address.
-		 * Needs to be ORed with MCP_REG_SCRATCH due to a HW bug.
+		/* Find MCP public data GRC address. Needs to be ORed with
+		 * MCP_REG_SCRATCH due to a HW bug.
 		 */
-		public_data_addr = qed_rd(p_hwfn, p_ptt,
+		public_data_addr = qed_rd(p_hwfn,
+					  p_ptt,
 					  MISC_REG_SHARED_MEM_ADDR) |
-					  MCP_REG_SCRATCH;
+				   MCP_REG_SCRATCH;
 
 		/* Find MCP public global section offset */
 		global_section_offsize_addr = public_data_addr +
@@ -1778,9 +1999,9 @@ static u32 qed_dump_mfw_ver_param(struct qed_hwfn *p_hwfn,
 					      sizeof(offsize_t) * PUBLIC_GLOBAL;
 		global_section_offsize = qed_rd(p_hwfn, p_ptt,
 						global_section_offsize_addr);
-		global_section_addr = MCP_REG_SCRATCH +
-				      (global_section_offsize &
-				       OFFSIZE_OFFSET_MASK) * 4;
+		global_section_addr =
+			MCP_REG_SCRATCH +
+			(global_section_offsize & OFFSIZE_OFFSET_MASK) * 4;
 
 		/* Read MFW version from MCP public global section */
 		mfw_ver = qed_rd(p_hwfn, p_ptt,
@@ -1788,13 +2009,9 @@ static u32 qed_dump_mfw_ver_param(struct qed_hwfn *p_hwfn,
 				 offsetof(struct public_global, mfw_ver));
 
 		/* Dump MFW version param */
-		printed_chars = snprintf(mfw_ver_str, sizeof(mfw_ver_str),
-					 "%d_%d_%d_%d",
-					 (u8) (mfw_ver >> 24),
-					 (u8) (mfw_ver >> 16),
-					 (u8) (mfw_ver >> 8),
-					 (u8) mfw_ver);
-		if (printed_chars < 0 || printed_chars >= sizeof(mfw_ver_str))
+		if (snprintf(mfw_ver_str, sizeof(mfw_ver_str), "%d_%d_%d_%d",
+			     (u8)(mfw_ver >> 24), (u8)(mfw_ver >> 16),
+			     (u8)(mfw_ver >> 8), (u8)mfw_ver) < 0)
 			DP_NOTICE(p_hwfn,
 				  "Unexpected debug error: invalid MFW version string\n");
 	}
@@ -1820,11 +2037,12 @@ static u32 qed_dump_common_global_params(struct qed_hwfn *p_hwfn,
 					 bool dump,
 					 u8 num_specific_global_params)
 {
-	u8 num_params = NUM_COMMON_GLOBAL_PARAMS + num_specific_global_params;
 	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
 	u32 offset = 0;
+	u8 num_params;
 
-	/* Find platform string and dump global params section header */
+	/* Dump global params section header */
+	num_params = NUM_COMMON_GLOBAL_PARAMS + num_specific_global_params;
 	offset += qed_dump_section_hdr(dump_buf + offset,
 				       dump, "global_params", num_params);
 
@@ -1846,25 +2064,29 @@ static u32 qed_dump_common_global_params(struct qed_hwfn *p_hwfn,
 	offset +=
 	    qed_dump_num_param(dump_buf + offset, dump, "pci-func",
 			       p_hwfn->abs_pf_id);
+
 	return offset;
 }
 
-/* Writes the last section to the specified buffer at the given offset.
- * Returns the dumped size in dwords.
+/* Writes the "last" section (including CRC) to the specified buffer at the
+ * given offset. Returns the dumped size in dwords.
  */
-static u32 qed_dump_last_section(u32 *dump_buf, u32 offset, bool dump)
+static u32 qed_dump_last_section(struct qed_hwfn *p_hwfn,
+				 u32 *dump_buf, u32 offset, bool dump)
 {
-	u32 start_offset = offset, crc = ~0;
+	u32 start_offset = offset;
 
 	/* Dump CRC section header */
 	offset += qed_dump_section_hdr(dump_buf + offset, dump, "last", 0);
 
-	/* Calculate CRC32 and add it to the dword following the "last" section.
-	 */
+	/* Calculate CRC32 and add it to the dword after the "last" section */
 	if (dump)
-		*(dump_buf + offset) = ~crc32(crc, (u8 *)dump_buf,
+		*(dump_buf + offset) = ~crc32(0xffffffff,
+					      (u8 *)dump_buf,
 					      DWORDS_TO_BYTES(offset));
+
 	offset++;
+
 	return offset - start_offset;
 }
 
@@ -1883,11 +2105,12 @@ static void qed_update_blocks_reset_state(struct qed_hwfn *p_hwfn,
 					    p_ptt, s_reset_regs_defs[i].addr);
 
 	/* Check if blocks are in reset */
-	for (i = 0; i < MAX_BLOCK_ID; i++)
-		dev_data->block_in_reset[i] =
-		    s_block_defs[i]->has_reset_bit &&
-		    !(reg_val[s_block_defs[i]->reset_reg] &
-		      BIT(s_block_defs[i]->reset_bit_offset));
+	for (i = 0; i < MAX_BLOCK_ID; i++) {
+		struct block_defs *block = s_block_defs[i];
+
+		dev_data->block_in_reset[i] = block->has_reset_bit &&
+		    !(reg_val[block->reset_reg] & BIT(block->reset_bit_offset));
+	}
 }
 
 /* Enable / disable the Debug block */
@@ -1902,12 +2125,12 @@ static void qed_bus_reset_dbg_block(struct qed_hwfn *p_hwfn,
 				    struct qed_ptt *p_ptt)
 {
 	u32 dbg_reset_reg_addr, old_reset_reg_val, new_reset_reg_val;
+	struct block_defs *dbg_block = s_block_defs[BLOCK_DBG];
 
-	dbg_reset_reg_addr =
-		s_reset_regs_defs[s_block_defs[BLOCK_DBG]->reset_reg].addr;
+	dbg_reset_reg_addr = s_reset_regs_defs[dbg_block->reset_reg].addr;
 	old_reset_reg_val = qed_rd(p_hwfn, p_ptt, dbg_reset_reg_addr);
-	new_reset_reg_val = old_reset_reg_val &
-			    ~BIT(s_block_defs[BLOCK_DBG]->reset_bit_offset);
+	new_reset_reg_val =
+	    old_reset_reg_val & ~BIT(dbg_block->reset_bit_offset);
 
 	qed_wr(p_hwfn, p_ptt, dbg_reset_reg_addr, new_reset_reg_val);
 	qed_wr(p_hwfn, p_ptt, dbg_reset_reg_addr, old_reset_reg_val);
@@ -1920,8 +2143,8 @@ static void qed_bus_set_framing_mode(struct qed_hwfn *p_hwfn,
 	qed_wr(p_hwfn, p_ptt, DBG_REG_FRAMING_MODE, (u8)mode);
 }
 
-/* Enable / disable Debug Bus clients according to the specified mask.
- * (1 = enable, 0 = disable)
+/* Enable / disable Debug Bus clients according to the specified mask
+ * (1 = enable, 0 = disable).
  */
 static void qed_bus_enable_clients(struct qed_hwfn *p_hwfn,
 				   struct qed_ptt *p_ptt, u32 client_mask)
@@ -1931,10 +2154,14 @@ static void qed_bus_enable_clients(struct qed_hwfn *p_hwfn,
 
 static bool qed_is_mode_match(struct qed_hwfn *p_hwfn, u16 *modes_buf_offset)
 {
-	const u32 *ptr = s_dbg_arrays[BIN_BUF_DBG_MODE_TREE].ptr;
 	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
-	u8 tree_val = ((u8 *)ptr)[(*modes_buf_offset)++];
 	bool arg1, arg2;
+	const u32 *ptr;
+	u8 tree_val;
+
+	/* Get next element from modes tree buffer */
+	ptr = s_dbg_arrays[BIN_BUF_DBG_MODE_TREE].ptr;
+	tree_val = ((u8 *)ptr)[(*modes_buf_offset)++];
 
 	switch (tree_val) {
 	case INIT_MODE_OP_NOT:
@@ -1974,75 +2201,81 @@ static bool qed_grc_is_storm_included(struct qed_hwfn *p_hwfn,
 static bool qed_grc_is_mem_included(struct qed_hwfn *p_hwfn,
 				    enum block_id block_id, u8 mem_group_id)
 {
+	struct block_defs *block = s_block_defs[block_id];
 	u8 i;
 
 	/* Check Storm match */
-	if (s_block_defs[block_id]->associated_to_storm &&
+	if (block->associated_to_storm &&
 	    !qed_grc_is_storm_included(p_hwfn,
-			(enum dbg_storms)s_block_defs[block_id]->storm_id))
+				       (enum dbg_storms)block->storm_id))
 		return false;
 
-	for (i = 0; i < NUM_BIG_RAM_TYPES; i++)
-		if (mem_group_id == s_big_ram_defs[i].mem_group_id ||
-		    mem_group_id == s_big_ram_defs[i].ram_mem_group_id)
-			return qed_grc_is_included(p_hwfn,
-						   s_big_ram_defs[i].grc_param);
-	if (mem_group_id == MEM_GROUP_PXP_ILT || mem_group_id ==
-	    MEM_GROUP_PXP_MEM)
+	for (i = 0; i < NUM_BIG_RAM_TYPES; i++) {
+		struct big_ram_defs *big_ram = &s_big_ram_defs[i];
+
+		if (mem_group_id == big_ram->mem_group_id ||
+		    mem_group_id == big_ram->ram_mem_group_id)
+			return qed_grc_is_included(p_hwfn, big_ram->grc_param);
+	}
+
+	switch (mem_group_id) {
+	case MEM_GROUP_PXP_ILT:
+	case MEM_GROUP_PXP_MEM:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_PXP);
-	if (mem_group_id == MEM_GROUP_RAM)
+	case MEM_GROUP_RAM:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_RAM);
-	if (mem_group_id == MEM_GROUP_PBUF)
+	case MEM_GROUP_PBUF:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_PBUF);
-	if (mem_group_id == MEM_GROUP_CAU_MEM ||
-	    mem_group_id == MEM_GROUP_CAU_SB ||
-	    mem_group_id == MEM_GROUP_CAU_PI)
+	case MEM_GROUP_CAU_MEM:
+	case MEM_GROUP_CAU_SB:
+	case MEM_GROUP_CAU_PI:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_CAU);
-	if (mem_group_id == MEM_GROUP_QM_MEM)
+	case MEM_GROUP_QM_MEM:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_QM);
-	if (mem_group_id == MEM_GROUP_CONN_CFC_MEM ||
-	    mem_group_id == MEM_GROUP_TASK_CFC_MEM)
+	case MEM_GROUP_CFC_MEM:
+	case MEM_GROUP_CONN_CFC_MEM:
+	case MEM_GROUP_TASK_CFC_MEM:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_CFC);
-	if (mem_group_id == MEM_GROUP_IGU_MEM || mem_group_id ==
-	    MEM_GROUP_IGU_MSIX)
+	case MEM_GROUP_IGU_MEM:
+	case MEM_GROUP_IGU_MSIX:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_IGU);
-	if (mem_group_id == MEM_GROUP_MULD_MEM)
+	case MEM_GROUP_MULD_MEM:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_MULD);
-	if (mem_group_id == MEM_GROUP_PRS_MEM)
+	case MEM_GROUP_PRS_MEM:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_PRS);
-	if (mem_group_id == MEM_GROUP_DMAE_MEM)
+	case MEM_GROUP_DMAE_MEM:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_DMAE);
-	if (mem_group_id == MEM_GROUP_TM_MEM)
+	case MEM_GROUP_TM_MEM:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_TM);
-	if (mem_group_id == MEM_GROUP_SDM_MEM)
+	case MEM_GROUP_SDM_MEM:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_SDM);
-	if (mem_group_id == MEM_GROUP_TDIF_CTX || mem_group_id ==
-	    MEM_GROUP_RDIF_CTX)
+	case MEM_GROUP_TDIF_CTX:
+	case MEM_GROUP_RDIF_CTX:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_DIF);
-	if (mem_group_id == MEM_GROUP_CM_MEM)
+	case MEM_GROUP_CM_MEM:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_CM);
-	if (mem_group_id == MEM_GROUP_IOR)
+	case MEM_GROUP_IOR:
 		return qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_IOR);
-
-	return true;
+	default:
+		return true;
+	}
 }
 
 /* Stalls all Storms */
 static void qed_grc_stall_storms(struct qed_hwfn *p_hwfn,
 				 struct qed_ptt *p_ptt, bool stall)
 {
-	u8 reg_val = stall ? 1 : 0;
+	u32 reg_addr;
 	u8 storm_id;
 
 	for (storm_id = 0; storm_id < MAX_DBG_STORMS; storm_id++) {
-		if (qed_grc_is_storm_included(p_hwfn,
-					      (enum dbg_storms)storm_id)) {
-			u32 reg_addr =
-			    s_storm_defs[storm_id].sem_fast_mem_addr +
-			    SEM_FAST_REG_STALL_0;
+		if (!qed_grc_is_storm_included(p_hwfn,
+					       (enum dbg_storms)storm_id))
+			continue;
 
-			qed_wr(p_hwfn, p_ptt, reg_addr, reg_val);
-		}
+		reg_addr = s_storm_defs[storm_id].sem_fast_mem_addr +
+		    SEM_FAST_REG_STALL_0_BB_K2;
+		qed_wr(p_hwfn, p_ptt, reg_addr, stall ? 1 : 0);
 	}
 
 	msleep(STALL_DELAY_MS);
@@ -2054,24 +2287,29 @@ static void qed_grc_unreset_blocks(struct qed_hwfn *p_hwfn,
 {
 	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
 	u32 reg_val[MAX_DBG_RESET_REGS] = { 0 };
-	u32 i;
+	u32 block_id, i;
 
 	/* Fill reset regs values */
-	for (i = 0; i < MAX_BLOCK_ID; i++)
-		if (s_block_defs[i]->has_reset_bit && s_block_defs[i]->unreset)
-			reg_val[s_block_defs[i]->reset_reg] |=
-			    BIT(s_block_defs[i]->reset_bit_offset);
+	for (block_id = 0; block_id < MAX_BLOCK_ID; block_id++) {
+		struct block_defs *block = s_block_defs[block_id];
+
+		if (block->has_reset_bit && block->unreset)
+			reg_val[block->reset_reg] |=
+			    BIT(block->reset_bit_offset);
+	}
 
 	/* Write reset registers */
 	for (i = 0; i < MAX_DBG_RESET_REGS; i++) {
-		if (s_reset_regs_defs[i].exists[dev_data->chip_id]) {
-			reg_val[i] |= s_reset_regs_defs[i].unreset_val;
-			if (reg_val[i])
-				qed_wr(p_hwfn,
-				       p_ptt,
-				       s_reset_regs_defs[i].addr +
-				       RESET_REG_UNRESET_OFFSET, reg_val[i]);
-		}
+		if (!s_reset_regs_defs[i].exists[dev_data->chip_id])
+			continue;
+
+		reg_val[i] |= s_reset_regs_defs[i].unreset_val;
+
+		if (reg_val[i])
+			qed_wr(p_hwfn,
+			       p_ptt,
+			       s_reset_regs_defs[i].addr +
+			       RESET_REG_UNRESET_OFFSET, reg_val[i]);
 	}
 }
 
@@ -2095,6 +2333,7 @@ qed_get_block_attn_regs(enum block_id block_id, enum dbg_attn_type attn_type,
 		qed_get_block_attn_data(block_id, attn_type);
 
 	*num_attn_regs = block_type_data->num_regs;
+
 	return &((const struct dbg_attn_reg *)
 		 s_dbg_arrays[BIN_BUF_DBG_ATTN_REGS].ptr)[block_type_data->
 							  regs_offset];
@@ -2105,34 +2344,34 @@ static void qed_grc_clear_all_prty(struct qed_hwfn *p_hwfn,
 				   struct qed_ptt *p_ptt)
 {
 	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	const struct dbg_attn_reg *attn_reg_arr;
 	u8 reg_idx, num_attn_regs;
 	u32 block_id;
 
 	for (block_id = 0; block_id < MAX_BLOCK_ID; block_id++) {
-		const struct dbg_attn_reg *attn_reg_arr;
-
 		if (dev_data->block_in_reset[block_id])
 			continue;
 
 		attn_reg_arr = qed_get_block_attn_regs((enum block_id)block_id,
 						       ATTN_TYPE_PARITY,
 						       &num_attn_regs);
+
 		for (reg_idx = 0; reg_idx < num_attn_regs; reg_idx++) {
 			const struct dbg_attn_reg *reg_data =
 				&attn_reg_arr[reg_idx];
+			u16 modes_buf_offset;
+			bool eval_mode;
 
 			/* Check mode */
-			bool eval_mode = GET_FIELD(reg_data->mode.data,
-						   DBG_MODE_HDR_EVAL_MODE) > 0;
-			u16 modes_buf_offset =
+			eval_mode = GET_FIELD(reg_data->mode.data,
+					      DBG_MODE_HDR_EVAL_MODE) > 0;
+			modes_buf_offset =
 				GET_FIELD(reg_data->mode.data,
 					  DBG_MODE_HDR_MODES_BUF_OFFSET);
 
+			/* If Mode match: clear parity status */
 			if (!eval_mode ||
 			    qed_is_mode_match(p_hwfn, &modes_buf_offset))
-				/* Mode match - read parity status read-clear
-				 * register.
-				 */
 				qed_rd(p_hwfn, p_ptt,
 				       DWORDS_TO_BYTES(reg_data->
 						       sts_clr_address));
@@ -2142,11 +2381,11 @@ static void qed_grc_clear_all_prty(struct qed_hwfn *p_hwfn,
 
 /* Dumps GRC registers section header. Returns the dumped size in dwords.
  * The following parameters are dumped:
- * - 'count' = num_dumped_entries
- * - 'split' = split_type
- * - 'id' = split_id (dumped only if split_id >= 0)
- * - 'param_name' = param_val (user param, dumped only if param_name != NULL and
- *	param_val != NULL)
+ * - count:	 no. of dumped entries
+ * - split:	 split type
+ * - id:	 split ID (dumped only if split_id >= 0)
+ * - param_name: user parameter value (dumped only if param_name != NULL
+ *		 and param_val != NULL).
  */
 static u32 qed_grc_dump_regs_hdr(u32 *dump_buf,
 				 bool dump,
@@ -2170,84 +2409,100 @@ static u32 qed_grc_dump_regs_hdr(u32 *dump_buf,
 	if (param_name && param_val)
 		offset += qed_dump_str_param(dump_buf + offset,
 					     dump, param_name, param_val);
+
 	return offset;
 }
 
 /* Dumps the GRC registers in the specified address range.
  * Returns the dumped size in dwords.
+ * The addr and len arguments are specified in dwords.
  */
 static u32 qed_grc_dump_addr_range(struct qed_hwfn *p_hwfn,
-				   struct qed_ptt *p_ptt, u32 *dump_buf,
-				   bool dump, u32 addr, u32 len)
+				   struct qed_ptt *p_ptt,
+				   u32 *dump_buf,
+				   bool dump, u32 addr, u32 len, bool wide_bus)
 {
 	u32 byte_addr = DWORDS_TO_BYTES(addr), offset = 0, i;
 
-	if (dump)
-		for (i = 0; i < len; i++, byte_addr += BYTES_IN_DWORD, offset++)
-			*(dump_buf + offset) = qed_rd(p_hwfn, p_ptt, byte_addr);
-	else
-		offset += len;
+	if (!dump)
+		return len;
+
+	for (i = 0; i < len; i++, byte_addr += BYTES_IN_DWORD, offset++)
+		*(dump_buf + offset) = qed_rd(p_hwfn, p_ptt, byte_addr);
+
 	return offset;
 }
 
-/* Dumps GRC registers sequence header. Returns the dumped size in dwords. */
-static u32 qed_grc_dump_reg_entry_hdr(u32 *dump_buf, bool dump, u32 addr,
-				      u32 len)
+/* Dumps GRC registers sequence header. Returns the dumped size in dwords.
+ * The addr and len arguments are specified in dwords.
+ */
+static u32 qed_grc_dump_reg_entry_hdr(u32 *dump_buf,
+				      bool dump, u32 addr, u32 len)
 {
 	if (dump)
 		*dump_buf = addr | (len << REG_DUMP_LEN_SHIFT);
+
 	return 1;
 }
 
-/* Dumps GRC registers sequence. Returns the dumped size in dwords. */
+/* Dumps GRC registers sequence. Returns the dumped size in dwords.
+ * The addr and len arguments are specified in dwords.
+ */
 static u32 qed_grc_dump_reg_entry(struct qed_hwfn *p_hwfn,
-				  struct qed_ptt *p_ptt, u32 *dump_buf,
-				  bool dump, u32 addr, u32 len)
+				  struct qed_ptt *p_ptt,
+				  u32 *dump_buf,
+				  bool dump, u32 addr, u32 len, bool wide_bus)
 {
 	u32 offset = 0;
 
 	offset += qed_grc_dump_reg_entry_hdr(dump_buf, dump, addr, len);
 	offset += qed_grc_dump_addr_range(p_hwfn,
 					  p_ptt,
-					  dump_buf + offset, dump, addr, len);
+					  dump_buf + offset,
+					  dump, addr, len, wide_bus);
+
 	return offset;
 }
 
 /* Dumps GRC registers sequence with skip cycle.
  * Returns the dumped size in dwords.
+ * - addr:	start GRC address in dwords
+ * - total_len:	total no. of dwords to dump
+ * - read_len:	no. consecutive dwords to read
+ * - skip_len:	no. of dwords to skip (and fill with zeros)
  */
 static u32 qed_grc_dump_reg_entry_skip(struct qed_hwfn *p_hwfn,
-				       struct qed_ptt *p_ptt, u32 *dump_buf,
-				       bool dump, u32 addr, u32 total_len,
+				       struct qed_ptt *p_ptt,
+				       u32 *dump_buf,
+				       bool dump,
+				       u32 addr,
+				       u32 total_len,
 				       u32 read_len, u32 skip_len)
 {
 	u32 offset = 0, reg_offset = 0;
 
 	offset += qed_grc_dump_reg_entry_hdr(dump_buf, dump, addr, total_len);
-	if (dump) {
-		while (reg_offset < total_len) {
-			u32 curr_len = min_t(u32,
-					     read_len,
-					     total_len - reg_offset);
-			offset += qed_grc_dump_addr_range(p_hwfn,
-							  p_ptt,
-							  dump_buf + offset,
-							  dump, addr, curr_len);
+
+	if (!dump)
+		return offset + total_len;
+
+	while (reg_offset < total_len) {
+		u32 curr_len = min_t(u32, read_len, total_len - reg_offset);
+
+		offset += qed_grc_dump_addr_range(p_hwfn,
+						  p_ptt,
+						  dump_buf + offset,
+						  dump, addr, curr_len, false);
+		reg_offset += curr_len;
+		addr += curr_len;
+
+		if (reg_offset < total_len) {
+			curr_len = min_t(u32, skip_len, total_len - skip_len);
+			memset(dump_buf + offset, 0, DWORDS_TO_BYTES(curr_len));
+			offset += curr_len;
 			reg_offset += curr_len;
 			addr += curr_len;
-			if (reg_offset < total_len) {
-				curr_len = min_t(u32,
-						 skip_len,
-						 total_len - skip_len);
-				memset(dump_buf + offset, 0,
-				       DWORDS_TO_BYTES(curr_len));
-				offset += curr_len;
-				reg_offset += curr_len;
-				addr += curr_len;
-			}
 		}
-	} else {
-		offset += total_len;
 	}
 
 	return offset;
@@ -2266,43 +2521,48 @@ static u32 qed_grc_dump_regs_entries(struct qed_hwfn *p_hwfn,
 	bool mode_match = true;
 
 	*num_dumped_reg_entries = 0;
+
 	while (input_offset < input_regs_arr.size_in_dwords) {
 		const struct dbg_dump_cond_hdr *cond_hdr =
 		    (const struct dbg_dump_cond_hdr *)
 		    &input_regs_arr.ptr[input_offset++];
-		bool eval_mode = GET_FIELD(cond_hdr->mode.data,
-					   DBG_MODE_HDR_EVAL_MODE) > 0;
+		u16 modes_buf_offset;
+		bool eval_mode;
 
 		/* Check mode/block */
+		eval_mode = GET_FIELD(cond_hdr->mode.data,
+				      DBG_MODE_HDR_EVAL_MODE) > 0;
 		if (eval_mode) {
-			u16 modes_buf_offset =
+			modes_buf_offset =
 				GET_FIELD(cond_hdr->mode.data,
 					  DBG_MODE_HDR_MODES_BUF_OFFSET);
 			mode_match = qed_is_mode_match(p_hwfn,
 						       &modes_buf_offset);
 		}
 
-		if (mode_match && block_enable[cond_hdr->block_id]) {
-			for (i = 0; i < cond_hdr->data_size;
-			     i++, input_offset++) {
-				const struct dbg_dump_reg *reg =
-				    (const struct dbg_dump_reg *)
-				    &input_regs_arr.ptr[input_offset];
-				u32 addr, len;
-
-				addr = GET_FIELD(reg->data,
-						 DBG_DUMP_REG_ADDRESS);
-				len = GET_FIELD(reg->data, DBG_DUMP_REG_LENGTH);
-				offset +=
-				    qed_grc_dump_reg_entry(p_hwfn, p_ptt,
-							   dump_buf + offset,
-							   dump,
-							   addr,
-							   len);
-				(*num_dumped_reg_entries)++;
-			}
-		} else {
+		if (!mode_match || !block_enable[cond_hdr->block_id]) {
 			input_offset += cond_hdr->data_size;
+			continue;
+		}
+
+		for (i = 0; i < cond_hdr->data_size; i++, input_offset++) {
+			const struct dbg_dump_reg *reg =
+			    (const struct dbg_dump_reg *)
+			    &input_regs_arr.ptr[input_offset];
+			u32 addr, len;
+			bool wide_bus;
+
+			addr = GET_FIELD(reg->data, DBG_DUMP_REG_ADDRESS);
+			len = GET_FIELD(reg->data, DBG_DUMP_REG_LENGTH);
+			wide_bus = GET_FIELD(reg->data, DBG_DUMP_REG_WIDE_BUS);
+			offset += qed_grc_dump_reg_entry(p_hwfn,
+							 p_ptt,
+							 dump_buf + offset,
+							 dump,
+							 addr,
+							 len,
+							 wide_bus);
+			(*num_dumped_reg_entries)++;
 		}
 	}
 
@@ -2350,8 +2610,8 @@ static u32 qed_grc_dump_split_data(struct qed_hwfn *p_hwfn,
 	return num_dumped_reg_entries > 0 ? offset : 0;
 }
 
-/* Dumps registers according to the input registers array.
- * Returns the dumped size in dwords.
+/* Dumps registers according to the input registers array. Returns the dumped
+ * size in dwords.
  */
 static u32 qed_grc_dump_registers(struct qed_hwfn *p_hwfn,
 				  struct qed_ptt *p_ptt,
@@ -2361,29 +2621,37 @@ static u32 qed_grc_dump_registers(struct qed_hwfn *p_hwfn,
 				  const char *param_name, const char *param_val)
 {
 	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
-	struct chip_platform_defs *p_platform_defs;
+	struct chip_platform_defs *chip_platform;
 	u32 offset = 0, input_offset = 0;
-	struct chip_defs *p_chip_defs;
+	struct chip_defs *chip;
 	u8 port_id, pf_id, vf_id;
 	u16 fid;
 
-	p_chip_defs = &s_chip_defs[dev_data->chip_id];
-	p_platform_defs = &p_chip_defs->per_platform[dev_data->platform_id];
+	chip = &s_chip_defs[dev_data->chip_id];
+	chip_platform = &chip->per_platform[dev_data->platform_id];
 
 	if (dump)
 		DP_VERBOSE(p_hwfn, QED_MSG_DEBUG, "Dumping registers...\n");
+
 	while (input_offset <
 	       s_dbg_arrays[BIN_BUF_DBG_DUMP_REG].size_in_dwords) {
-		const struct dbg_dump_split_hdr *split_hdr =
+		const struct dbg_dump_split_hdr *split_hdr;
+		struct dbg_array curr_input_regs_arr;
+		u32 split_data_size;
+		u8 split_type_id;
+
+		split_hdr =
 			(const struct dbg_dump_split_hdr *)
 			&s_dbg_arrays[BIN_BUF_DBG_DUMP_REG].ptr[input_offset++];
-		u8 split_type_id = GET_FIELD(split_hdr->hdr,
-					     DBG_DUMP_SPLIT_HDR_SPLIT_TYPE_ID);
-		u32 split_data_size = GET_FIELD(split_hdr->hdr,
-						DBG_DUMP_SPLIT_HDR_DATA_SIZE);
-		struct dbg_array curr_input_regs_arr = {
-			&s_dbg_arrays[BIN_BUF_DBG_DUMP_REG].ptr[input_offset],
-			split_data_size};
+		split_type_id =
+			GET_FIELD(split_hdr->hdr,
+				  DBG_DUMP_SPLIT_HDR_SPLIT_TYPE_ID);
+		split_data_size =
+			GET_FIELD(split_hdr->hdr,
+				  DBG_DUMP_SPLIT_HDR_DATA_SIZE);
+		curr_input_regs_arr.ptr =
+			&s_dbg_arrays[BIN_BUF_DBG_DUMP_REG].ptr[input_offset];
+		curr_input_regs_arr.size_in_dwords = split_data_size;
 
 		switch (split_type_id) {
 		case SPLIT_TYPE_NONE:
@@ -2398,8 +2666,9 @@ static u32 qed_grc_dump_registers(struct qed_hwfn *p_hwfn,
 							  param_name,
 							  param_val);
 			break;
+
 		case SPLIT_TYPE_PORT:
-			for (port_id = 0; port_id < p_platform_defs->num_ports;
+			for (port_id = 0; port_id < chip_platform->num_ports;
 			     port_id++) {
 				if (dump)
 					qed_port_pretend(p_hwfn, p_ptt,
@@ -2414,9 +2683,10 @@ static u32 qed_grc_dump_registers(struct qed_hwfn *p_hwfn,
 							    param_val);
 			}
 			break;
+
 		case SPLIT_TYPE_PF:
 		case SPLIT_TYPE_PORT_PF:
-			for (pf_id = 0; pf_id < p_platform_defs->num_pfs;
+			for (pf_id = 0; pf_id < chip_platform->num_pfs;
 			     pf_id++) {
 				u8 pfid_shift =
 					PXP_PRETEND_CONCRETE_FID_PFID_SHIFT;
@@ -2427,17 +2697,21 @@ static u32 qed_grc_dump_registers(struct qed_hwfn *p_hwfn,
 				}
 
 				offset +=
-				    qed_grc_dump_split_data(p_hwfn, p_ptt,
+				    qed_grc_dump_split_data(p_hwfn,
+							    p_ptt,
 							    curr_input_regs_arr,
 							    dump_buf + offset,
-							    dump, block_enable,
-							    "pf", pf_id,
+							    dump,
+							    block_enable,
+							    "pf",
+							    pf_id,
 							    param_name,
 							    param_val);
 			}
 			break;
+
 		case SPLIT_TYPE_VF:
-			for (vf_id = 0; vf_id < p_platform_defs->num_vfs;
+			for (vf_id = 0; vf_id < chip_platform->num_vfs;
 			     vf_id++) {
 				u8 vfvalid_shift =
 					PXP_PRETEND_CONCRETE_FID_VFVALID_SHIFT;
@@ -2460,6 +2734,7 @@ static u32 qed_grc_dump_registers(struct qed_hwfn *p_hwfn,
 							    param_val);
 			}
 			break;
+
 		default:
 			break;
 		}
@@ -2490,35 +2765,37 @@ static u32 qed_grc_dump_reset_regs(struct qed_hwfn *p_hwfn,
 
 	/* Write reset registers */
 	for (i = 0; i < MAX_DBG_RESET_REGS; i++) {
-		if (s_reset_regs_defs[i].exists[dev_data->chip_id]) {
-			u32 addr = BYTES_TO_DWORDS(s_reset_regs_defs[i].addr);
+		if (!s_reset_regs_defs[i].exists[dev_data->chip_id])
+			continue;
 
-			offset += qed_grc_dump_reg_entry(p_hwfn,
-							 p_ptt,
-							 dump_buf + offset,
-							 dump,
-							 addr,
-							 1);
-			num_regs++;
-		}
+		offset += qed_grc_dump_reg_entry(p_hwfn,
+						 p_ptt,
+						 dump_buf + offset,
+						 dump,
+						 BYTES_TO_DWORDS
+						 (s_reset_regs_defs[i].addr), 1,
+						 false);
+		num_regs++;
 	}
 
 	/* Write header */
 	if (dump)
 		qed_grc_dump_regs_hdr(dump_buf,
 				      true, num_regs, "eng", -1, NULL, NULL);
+
 	return offset;
 }
 
-/* Dump registers that are modified during GRC Dump and therefore must be dumped
- * first. Returns the dumped size in dwords.
+/* Dump registers that are modified during GRC Dump and therefore must be
+ * dumped first. Returns the dumped size in dwords.
  */
 static u32 qed_grc_dump_modified_regs(struct qed_hwfn *p_hwfn,
 				      struct qed_ptt *p_ptt,
 				      u32 *dump_buf, bool dump)
 {
 	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
-	u32 offset = 0, num_reg_entries = 0, block_id;
+	u32 block_id, offset = 0, num_reg_entries = 0;
+	const struct dbg_attn_reg *attn_reg_arr;
 	u8 storm_id, reg_idx, num_attn_regs;
 
 	/* Calculate header size */
@@ -2527,14 +2804,13 @@ static u32 qed_grc_dump_modified_regs(struct qed_hwfn *p_hwfn,
 
 	/* Write parity registers */
 	for (block_id = 0; block_id < MAX_BLOCK_ID; block_id++) {
-		const struct dbg_attn_reg *attn_reg_arr;
-
 		if (dev_data->block_in_reset[block_id] && dump)
 			continue;
 
 		attn_reg_arr = qed_get_block_attn_regs((enum block_id)block_id,
 						       ATTN_TYPE_PARITY,
 						       &num_attn_regs);
+
 		for (reg_idx = 0; reg_idx < num_attn_regs; reg_idx++) {
 			const struct dbg_attn_reg *reg_data =
 				&attn_reg_arr[reg_idx];
@@ -2548,37 +2824,36 @@ static u32 qed_grc_dump_modified_regs(struct qed_hwfn *p_hwfn,
 			modes_buf_offset =
 				GET_FIELD(reg_data->mode.data,
 					  DBG_MODE_HDR_MODES_BUF_OFFSET);
-			if (!eval_mode ||
-			    qed_is_mode_match(p_hwfn, &modes_buf_offset)) {
-				/* Mode match - read and dump registers */
-				addr = reg_data->mask_address;
-				offset +=
-				    qed_grc_dump_reg_entry(p_hwfn,
-							   p_ptt,
-							   dump_buf + offset,
-							   dump,
-							   addr,
-							   1);
-				addr = GET_FIELD(reg_data->data,
-						 DBG_ATTN_REG_STS_ADDRESS);
-				offset +=
-				    qed_grc_dump_reg_entry(p_hwfn,
-							   p_ptt,
-							   dump_buf + offset,
-							   dump,
-							   addr,
-							   1);
-				num_reg_entries += 2;
-			}
+			if (eval_mode &&
+			    !qed_is_mode_match(p_hwfn, &modes_buf_offset))
+				continue;
+
+			/* Mode match: read & dump registers */
+			addr = reg_data->mask_address;
+			offset += qed_grc_dump_reg_entry(p_hwfn,
+							 p_ptt,
+							 dump_buf + offset,
+							 dump,
+							 addr,
+							 1, false);
+			addr = GET_FIELD(reg_data->data,
+					 DBG_ATTN_REG_STS_ADDRESS);
+			offset += qed_grc_dump_reg_entry(p_hwfn,
+							 p_ptt,
+							 dump_buf + offset,
+							 dump,
+							 addr,
+							 1, false);
+			num_reg_entries += 2;
 		}
 	}
 
-	/* Write storm stall status registers */
+	/* Write Storm stall status registers */
 	for (storm_id = 0; storm_id < MAX_DBG_STORMS; storm_id++) {
+		struct storm_defs *storm = &s_storm_defs[storm_id];
 		u32 addr;
 
-		if (dev_data->block_in_reset[s_storm_defs[storm_id].block_id] &&
-		    dump)
+		if (dev_data->block_in_reset[storm->block_id] && dump)
 			continue;
 
 		addr =
@@ -2589,7 +2864,8 @@ static u32 qed_grc_dump_modified_regs(struct qed_hwfn *p_hwfn,
 						 dump_buf + offset,
 						 dump,
 						 addr,
-						 1);
+						 1,
+						 false);
 		num_reg_entries++;
 	}
 
@@ -2598,6 +2874,7 @@ static u32 qed_grc_dump_modified_regs(struct qed_hwfn *p_hwfn,
 		qed_grc_dump_regs_hdr(dump_buf,
 				      true,
 				      num_reg_entries, "eng", -1, NULL, NULL);
+
 	return offset;
 }
 
@@ -2637,17 +2914,17 @@ static u32 qed_grc_dump_special_regs(struct qed_hwfn *p_hwfn,
 	return offset;
 }
 
-/* Dumps a GRC memory header (section and params).
- * The following parameters are dumped:
- * name - name is dumped only if it's not NULL.
- * addr - addr is dumped only if name is NULL.
- * len - len is always dumped.
- * width - bit_width is dumped if it's not zero.
- * packed - packed=1 is dumped if it's not false.
- * mem_group - mem_group is always dumped.
- * is_storm - true only if the memory is related to a Storm.
- * storm_letter - storm letter (valid only if is_storm is true).
- * Returns the dumped size in dwords.
+/* Dumps a GRC memory header (section and params). Returns the dumped size in
+ * dwords. The following parameters are dumped:
+ * - name:	   dumped only if it's not NULL.
+ * - addr:	   in dwords, dumped only if name is NULL.
+ * - len:	   in dwords, always dumped.
+ * - width:	   dumped if it's not zero.
+ * - packed:	   dumped only if it's not false.
+ * - mem_group:	   always dumped.
+ * - is_storm:	   true only if the memory is related to a Storm.
+ * - storm_letter: valid only if is_storm is true.
+ *
  */
 static u32 qed_grc_dump_mem_hdr(struct qed_hwfn *p_hwfn,
 				u32 *dump_buf,
@@ -2667,6 +2944,7 @@ static u32 qed_grc_dump_mem_hdr(struct qed_hwfn *p_hwfn,
 	if (!len)
 		DP_NOTICE(p_hwfn,
 			  "Unexpected GRC Dump error: dumped memory size must be non-zero\n");
+
 	if (bit_width)
 		num_params++;
 	if (packed)
@@ -2675,6 +2953,7 @@ static u32 qed_grc_dump_mem_hdr(struct qed_hwfn *p_hwfn,
 	/* Dump section header */
 	offset += qed_dump_section_hdr(dump_buf + offset,
 				       dump, "grc_mem", num_params);
+
 	if (name) {
 		/* Dump name */
 		if (is_storm) {
@@ -2694,14 +2973,15 @@ static u32 qed_grc_dump_mem_hdr(struct qed_hwfn *p_hwfn,
 				   len, buf);
 	} else {
 		/* Dump address */
+		u32 addr_in_bytes = DWORDS_TO_BYTES(addr);
+
 		offset += qed_dump_num_param(dump_buf + offset,
-					     dump, "addr",
-					     DWORDS_TO_BYTES(addr));
+					     dump, "addr", addr_in_bytes);
 		if (dump && len > 64)
 			DP_VERBOSE(p_hwfn,
 				   QED_MSG_DEBUG,
 				   "Dumping %d registers from address 0x%x...\n",
-				   len, (u32)DWORDS_TO_BYTES(addr));
+				   len, addr_in_bytes);
 	}
 
 	/* Dump len */
@@ -2727,11 +3007,13 @@ static u32 qed_grc_dump_mem_hdr(struct qed_hwfn *p_hwfn,
 	}
 
 	offset += qed_dump_str_param(dump_buf + offset, dump, "type", buf);
+
 	return offset;
 }
 
 /* Dumps a single GRC memory. If name is NULL, the memory is stored by address.
  * Returns the dumped size in dwords.
+ * The addr and len arguments are specified in dwords.
  */
 static u32 qed_grc_dump_mem(struct qed_hwfn *p_hwfn,
 			    struct qed_ptt *p_ptt,
@@ -2740,6 +3022,7 @@ static u32 qed_grc_dump_mem(struct qed_hwfn *p_hwfn,
 			    const char *name,
 			    u32 addr,
 			    u32 len,
+			    bool wide_bus,
 			    u32 bit_width,
 			    bool packed,
 			    const char *mem_group,
@@ -2758,7 +3041,9 @@ static u32 qed_grc_dump_mem(struct qed_hwfn *p_hwfn,
 				       mem_group, is_storm, storm_letter);
 	offset += qed_grc_dump_addr_range(p_hwfn,
 					  p_ptt,
-					  dump_buf + offset, dump, addr, len);
+					  dump_buf + offset,
+					  dump, addr, len, wide_bus);
+
 	return offset;
 }
 
@@ -2773,20 +3058,21 @@ static u32 qed_grc_dump_mem_entries(struct qed_hwfn *p_hwfn,
 
 	while (input_offset < input_mems_arr.size_in_dwords) {
 		const struct dbg_dump_cond_hdr *cond_hdr;
+		u16 modes_buf_offset;
 		u32 num_entries;
 		bool eval_mode;
 
 		cond_hdr = (const struct dbg_dump_cond_hdr *)
 			   &input_mems_arr.ptr[input_offset++];
-		eval_mode = GET_FIELD(cond_hdr->mode.data,
-				      DBG_MODE_HDR_EVAL_MODE) > 0;
+		num_entries = cond_hdr->data_size / MEM_DUMP_ENTRY_SIZE_DWORDS;
 
 		/* Check required mode */
+		eval_mode = GET_FIELD(cond_hdr->mode.data,
+				      DBG_MODE_HDR_EVAL_MODE) > 0;
 		if (eval_mode) {
-			u16 modes_buf_offset =
+			modes_buf_offset =
 				GET_FIELD(cond_hdr->mode.data,
 					  DBG_MODE_HDR_MODES_BUF_OFFSET);
-
 			mode_match = qed_is_mode_match(p_hwfn,
 						       &modes_buf_offset);
 		}
@@ -2796,81 +3082,87 @@ static u32 qed_grc_dump_mem_entries(struct qed_hwfn *p_hwfn,
 			continue;
 		}
 
-		num_entries = cond_hdr->data_size / MEM_DUMP_ENTRY_SIZE_DWORDS;
 		for (i = 0; i < num_entries;
 		     i++, input_offset += MEM_DUMP_ENTRY_SIZE_DWORDS) {
 			const struct dbg_dump_mem *mem =
 				(const struct dbg_dump_mem *)
 				&input_mems_arr.ptr[input_offset];
-			u8 mem_group_id;
+			u8 mem_group_id = GET_FIELD(mem->dword0,
+						    DBG_DUMP_MEM_MEM_GROUP_ID);
+			bool is_storm = false, mem_wide_bus;
+			enum dbg_grc_params grc_param;
+			char storm_letter = 'a';
+			enum block_id block_id;
+			u32 mem_addr, mem_len;
 
-			mem_group_id = GET_FIELD(mem->dword0,
-						 DBG_DUMP_MEM_MEM_GROUP_ID);
 			if (mem_group_id >= MEM_GROUPS_NUM) {
 				DP_NOTICE(p_hwfn, "Invalid mem_group_id\n");
 				return 0;
 			}
 
-			if (qed_grc_is_mem_included(p_hwfn,
-					(enum block_id)cond_hdr->block_id,
-					mem_group_id)) {
-				u32 mem_addr = GET_FIELD(mem->dword0,
-							 DBG_DUMP_MEM_ADDRESS);
-				u32 mem_len = GET_FIELD(mem->dword1,
-							DBG_DUMP_MEM_LENGTH);
-				enum dbg_grc_params grc_param;
-				char storm_letter = 'a';
-				bool is_storm = false;
-
-				/* Update memory length for CCFC/TCFC memories
-				 * according to number of LCIDs/LTIDs.
-				 */
-				if (mem_group_id == MEM_GROUP_CONN_CFC_MEM) {
-					if (mem_len % MAX_LCIDS != 0) {
-						DP_NOTICE(p_hwfn,
-							  "Invalid CCFC connection memory size\n");
-						return 0;
-					}
-
-					grc_param = DBG_GRC_PARAM_NUM_LCIDS;
-					mem_len = qed_grc_get_param(p_hwfn,
-								    grc_param) *
-						  (mem_len / MAX_LCIDS);
-				} else if (mem_group_id ==
-					   MEM_GROUP_TASK_CFC_MEM) {
-					if (mem_len % MAX_LTIDS != 0) {
-						DP_NOTICE(p_hwfn,
-							  "Invalid TCFC task memory size\n");
-						return 0;
-					}
-
-					grc_param = DBG_GRC_PARAM_NUM_LTIDS;
-					mem_len = qed_grc_get_param(p_hwfn,
-								    grc_param) *
-						  (mem_len / MAX_LTIDS);
+			block_id = (enum block_id)cond_hdr->block_id;
+			if (!qed_grc_is_mem_included(p_hwfn,
+						     block_id,
+						     mem_group_id))
+				continue;
+
+			mem_addr = GET_FIELD(mem->dword0, DBG_DUMP_MEM_ADDRESS);
+			mem_len = GET_FIELD(mem->dword1, DBG_DUMP_MEM_LENGTH);
+			mem_wide_bus = GET_FIELD(mem->dword1,
+						 DBG_DUMP_MEM_WIDE_BUS);
+
+			/* Update memory length for CCFC/TCFC memories
+			 * according to number of LCIDs/LTIDs.
+			 */
+			if (mem_group_id == MEM_GROUP_CONN_CFC_MEM) {
+				if (mem_len % MAX_LCIDS) {
+					DP_NOTICE(p_hwfn,
+						  "Invalid CCFC connection memory size\n");
+					return 0;
 				}
 
-				/* If memory is associated with Storm, update
-				 * Storm details.
-				 */
-				if (s_block_defs[cond_hdr->block_id]->
-							associated_to_storm) {
-					is_storm = true;
-					storm_letter =
-						s_storm_defs[s_block_defs[
-						cond_hdr->block_id]->
-						storm_id].letter;
+				grc_param = DBG_GRC_PARAM_NUM_LCIDS;
+				mem_len = qed_grc_get_param(p_hwfn, grc_param) *
+					  (mem_len / MAX_LCIDS);
+			} else if (mem_group_id == MEM_GROUP_TASK_CFC_MEM) {
+				if (mem_len % MAX_LTIDS) {
+					DP_NOTICE(p_hwfn,
+						  "Invalid TCFC task memory size\n");
+					return 0;
 				}
 
-				/* Dump memory */
-				offset += qed_grc_dump_mem(p_hwfn, p_ptt,
-						dump_buf + offset, dump, NULL,
-						mem_addr, mem_len, 0,
+				grc_param = DBG_GRC_PARAM_NUM_LTIDS;
+				mem_len = qed_grc_get_param(p_hwfn, grc_param) *
+					  (mem_len / MAX_LTIDS);
+			}
+
+			/* If memory is associated with Storm, update Storm
+			 * details.
+			 */
+			if (s_block_defs
+			    [cond_hdr->block_id]->associated_to_storm) {
+				is_storm = true;
+				storm_letter =
+				    s_storm_defs[s_block_defs
+						 [cond_hdr->block_id]->
+						 storm_id].letter;
+			}
+
+			/* Dump memory */
+			offset += qed_grc_dump_mem(p_hwfn,
+						p_ptt,
+						dump_buf + offset,
+						dump,
+						NULL,
+						mem_addr,
+						mem_len,
+						mem_wide_bus,
+						0,
 						false,
 						s_mem_group_names[mem_group_id],
-						is_storm, storm_letter);
-				}
-			}
+						is_storm,
+						storm_letter);
+		}
 	}
 
 	return offset;
@@ -2887,16 +3179,22 @@ static u32 qed_grc_dump_memories(struct qed_hwfn *p_hwfn,
 
 	while (input_offset <
 	       s_dbg_arrays[BIN_BUF_DBG_DUMP_MEM].size_in_dwords) {
-		const struct dbg_dump_split_hdr *split_hdr =
-			(const struct dbg_dump_split_hdr *)
+		const struct dbg_dump_split_hdr *split_hdr;
+		struct dbg_array curr_input_mems_arr;
+		u32 split_data_size;
+		u8 split_type_id;
+
+		split_hdr = (const struct dbg_dump_split_hdr *)
 			&s_dbg_arrays[BIN_BUF_DBG_DUMP_MEM].ptr[input_offset++];
-		u8 split_type_id = GET_FIELD(split_hdr->hdr,
-					     DBG_DUMP_SPLIT_HDR_SPLIT_TYPE_ID);
-		u32 split_data_size = GET_FIELD(split_hdr->hdr,
-						DBG_DUMP_SPLIT_HDR_DATA_SIZE);
-		struct dbg_array curr_input_mems_arr = {
-			&s_dbg_arrays[BIN_BUF_DBG_DUMP_MEM].ptr[input_offset],
-			split_data_size};
+		split_type_id =
+			GET_FIELD(split_hdr->hdr,
+				  DBG_DUMP_SPLIT_HDR_SPLIT_TYPE_ID);
+		split_data_size =
+			GET_FIELD(split_hdr->hdr,
+				  DBG_DUMP_SPLIT_HDR_DATA_SIZE);
+		curr_input_mems_arr.ptr =
+			&s_dbg_arrays[BIN_BUF_DBG_DUMP_MEM].ptr[input_offset];
+		curr_input_mems_arr.size_in_dwords = split_data_size;
 
 		switch (split_type_id) {
 		case SPLIT_TYPE_NONE:
@@ -2906,6 +3204,7 @@ static u32 qed_grc_dump_memories(struct qed_hwfn *p_hwfn,
 							   dump_buf + offset,
 							   dump);
 			break;
+
 		default:
 			DP_NOTICE(p_hwfn,
 				  "Dumping split memories is currently not supported\n");
@@ -2920,6 +3219,7 @@ static u32 qed_grc_dump_memories(struct qed_hwfn *p_hwfn,
 
 /* Dumps GRC context data for the specified Storm.
  * Returns the dumped size in dwords.
+ * The lid_size argument is specified in quad-regs.
  */
 static u32 qed_grc_dump_ctx_data(struct qed_hwfn *p_hwfn,
 				 struct qed_ptt *p_ptt,
@@ -2931,13 +3231,15 @@ static u32 qed_grc_dump_ctx_data(struct qed_hwfn *p_hwfn,
 				 u32 rd_reg_addr,
 				 u8 storm_id)
 {
-	u32 i, lid, total_size;
-	u32 offset = 0;
+	struct storm_defs *storm = &s_storm_defs[storm_id];
+	u32 i, lid, total_size, offset = 0;
 
 	if (!lid_size)
 		return 0;
+
 	lid_size *= BYTES_IN_DWORD;
 	total_size = num_lids * lid_size;
+
 	offset += qed_grc_dump_mem_hdr(p_hwfn,
 				       dump_buf + offset,
 				       dump,
@@ -2945,25 +3247,19 @@ static u32 qed_grc_dump_ctx_data(struct qed_hwfn *p_hwfn,
 				       0,
 				       total_size,
 				       lid_size * 32,
-				       false,
-				       name,
-				       true, s_storm_defs[storm_id].letter);
+				       false, name, true, storm->letter);
+
+	if (!dump)
+		return offset + total_size;
 
 	/* Dump context data */
-	if (dump) {
-		for (lid = 0; lid < num_lids; lid++) {
-			for (i = 0; i < lid_size; i++, offset++) {
-				qed_wr(p_hwfn,
-				       p_ptt,
-				       s_storm_defs[storm_id].cm_ctx_wr_addr,
-				       BIT(9) | lid);
-				*(dump_buf + offset) = qed_rd(p_hwfn,
-							      p_ptt,
-							      rd_reg_addr);
-			}
+	for (lid = 0; lid < num_lids; lid++) {
+		for (i = 0; i < lid_size; i++, offset++) {
+			qed_wr(p_hwfn,
+			       p_ptt, storm->cm_ctx_wr_addr, (i << 9) | lid);
+			*(dump_buf + offset) = qed_rd(p_hwfn,
+						      p_ptt, rd_reg_addr);
 		}
-	} else {
-		offset += total_size;
 	}
 
 	return offset;
@@ -2973,15 +3269,19 @@ static u32 qed_grc_dump_ctx_data(struct qed_hwfn *p_hwfn,
 static u32 qed_grc_dump_ctx(struct qed_hwfn *p_hwfn,
 			    struct qed_ptt *p_ptt, u32 *dump_buf, bool dump)
 {
+	enum dbg_grc_params grc_param;
 	u32 offset = 0;
 	u8 storm_id;
 
 	for (storm_id = 0; storm_id < MAX_DBG_STORMS; storm_id++) {
+		struct storm_defs *storm = &s_storm_defs[storm_id];
+
 		if (!qed_grc_is_storm_included(p_hwfn,
 					       (enum dbg_storms)storm_id))
 			continue;
 
 		/* Dump Conn AG context size */
+		grc_param = DBG_GRC_PARAM_NUM_LCIDS;
 		offset +=
 			qed_grc_dump_ctx_data(p_hwfn,
 					      p_ptt,
@@ -2989,14 +3289,13 @@ static u32 qed_grc_dump_ctx(struct qed_hwfn *p_hwfn,
 					      dump,
 					      "CONN_AG_CTX",
 					      qed_grc_get_param(p_hwfn,
-						    DBG_GRC_PARAM_NUM_LCIDS),
-					      s_storm_defs[storm_id].
-						    cm_conn_ag_ctx_lid_size,
-					      s_storm_defs[storm_id].
-						    cm_conn_ag_ctx_rd_addr,
+								grc_param),
+					      storm->cm_conn_ag_ctx_lid_size,
+					      storm->cm_conn_ag_ctx_rd_addr,
 					      storm_id);
 
 		/* Dump Conn ST context size */
+		grc_param = DBG_GRC_PARAM_NUM_LCIDS;
 		offset +=
 			qed_grc_dump_ctx_data(p_hwfn,
 					      p_ptt,
@@ -3004,14 +3303,13 @@ static u32 qed_grc_dump_ctx(struct qed_hwfn *p_hwfn,
 					      dump,
 					      "CONN_ST_CTX",
 					      qed_grc_get_param(p_hwfn,
-						    DBG_GRC_PARAM_NUM_LCIDS),
-					      s_storm_defs[storm_id].
-						    cm_conn_st_ctx_lid_size,
-					      s_storm_defs[storm_id].
-						    cm_conn_st_ctx_rd_addr,
+								grc_param),
+					      storm->cm_conn_st_ctx_lid_size,
+					      storm->cm_conn_st_ctx_rd_addr,
 					      storm_id);
 
 		/* Dump Task AG context size */
+		grc_param = DBG_GRC_PARAM_NUM_LTIDS;
 		offset +=
 			qed_grc_dump_ctx_data(p_hwfn,
 					      p_ptt,
@@ -3019,14 +3317,13 @@ static u32 qed_grc_dump_ctx(struct qed_hwfn *p_hwfn,
 					      dump,
 					      "TASK_AG_CTX",
 					      qed_grc_get_param(p_hwfn,
-						    DBG_GRC_PARAM_NUM_LTIDS),
-					      s_storm_defs[storm_id].
-						    cm_task_ag_ctx_lid_size,
-					      s_storm_defs[storm_id].
-						    cm_task_ag_ctx_rd_addr,
+								grc_param),
+					      storm->cm_task_ag_ctx_lid_size,
+					      storm->cm_task_ag_ctx_rd_addr,
 					      storm_id);
 
 		/* Dump Task ST context size */
+		grc_param = DBG_GRC_PARAM_NUM_LTIDS;
 		offset +=
 			qed_grc_dump_ctx_data(p_hwfn,
 					      p_ptt,
@@ -3034,11 +3331,9 @@ static u32 qed_grc_dump_ctx(struct qed_hwfn *p_hwfn,
 					      dump,
 					      "TASK_ST_CTX",
 					      qed_grc_get_param(p_hwfn,
-						    DBG_GRC_PARAM_NUM_LTIDS),
-					      s_storm_defs[storm_id].
-						    cm_task_st_ctx_lid_size,
-					      s_storm_defs[storm_id].
-						    cm_task_st_ctx_rd_addr,
+								grc_param),
+					      storm->cm_task_st_ctx_lid_size,
+					      storm->cm_task_st_ctx_rd_addr,
 					      storm_id);
 	}
 
@@ -3050,8 +3345,8 @@ static u32 qed_grc_dump_iors(struct qed_hwfn *p_hwfn,
 			     struct qed_ptt *p_ptt, u32 *dump_buf, bool dump)
 {
 	char buf[10] = "IOR_SET_?";
+	u32 addr, offset = 0;
 	u8 storm_id, set_id;
-	u32 offset = 0;
 
 	for (storm_id = 0; storm_id < MAX_DBG_STORMS; storm_id++) {
 		struct storm_defs *storm = &s_storm_defs[storm_id];
@@ -3061,11 +3356,9 @@ static u32 qed_grc_dump_iors(struct qed_hwfn *p_hwfn,
 			continue;
 
 		for (set_id = 0; set_id < NUM_IOR_SETS; set_id++) {
-			u32 dwords, addr;
-
-			dwords = storm->sem_fast_mem_addr +
-				 SEM_FAST_REG_STORM_REG_FILE;
-			addr = BYTES_TO_DWORDS(dwords) + IOR_SET_OFFSET(set_id);
+			addr = BYTES_TO_DWORDS(storm->sem_fast_mem_addr +
+					       SEM_FAST_REG_STORM_REG_FILE) +
+			       IOR_SET_OFFSET(set_id);
 			buf[strlen(buf) - 1] = '0' + set_id;
 			offset += qed_grc_dump_mem(p_hwfn,
 						   p_ptt,
@@ -3074,6 +3367,7 @@ static u32 qed_grc_dump_iors(struct qed_hwfn *p_hwfn,
 						   buf,
 						   addr,
 						   IORS_PER_SET,
+						   false,
 						   32,
 						   false,
 						   "ior",
@@ -3091,10 +3385,10 @@ static u32 qed_grc_dump_vfc_cam(struct qed_hwfn *p_hwfn,
 				u32 *dump_buf, bool dump, u8 storm_id)
 {
 	u32 total_size = VFC_CAM_NUM_ROWS * VFC_CAM_RESP_DWORDS;
+	struct storm_defs *storm = &s_storm_defs[storm_id];
 	u32 cam_addr[VFC_CAM_ADDR_DWORDS] = { 0 };
 	u32 cam_cmd[VFC_CAM_CMD_DWORDS] = { 0 };
-	u32 offset = 0;
-	u32 row, i;
+	u32 row, i, offset = 0;
 
 	offset += qed_grc_dump_mem_hdr(p_hwfn,
 				       dump_buf + offset,
@@ -3103,38 +3397,34 @@ static u32 qed_grc_dump_vfc_cam(struct qed_hwfn *p_hwfn,
 				       0,
 				       total_size,
 				       256,
-				       false,
-				       "vfc_cam",
-				       true, s_storm_defs[storm_id].letter);
-	if (dump) {
-		/* Prepare CAM address */
-		SET_VAR_FIELD(cam_addr, VFC_CAM_ADDR, OP, VFC_OPCODE_CAM_RD);
-		for (row = 0; row < VFC_CAM_NUM_ROWS;
-		     row++, offset += VFC_CAM_RESP_DWORDS) {
-			/* Write VFC CAM command */
-			SET_VAR_FIELD(cam_cmd, VFC_CAM_CMD, ROW, row);
-			ARR_REG_WR(p_hwfn,
-				   p_ptt,
-				   s_storm_defs[storm_id].sem_fast_mem_addr +
-				   SEM_FAST_REG_VFC_DATA_WR,
-				   cam_cmd, VFC_CAM_CMD_DWORDS);
+				       false, "vfc_cam", true, storm->letter);
 
-			/* Write VFC CAM address */
-			ARR_REG_WR(p_hwfn,
-				   p_ptt,
-				   s_storm_defs[storm_id].sem_fast_mem_addr +
-				   SEM_FAST_REG_VFC_ADDR,
-				   cam_addr, VFC_CAM_ADDR_DWORDS);
+	if (!dump)
+		return offset + total_size;
 
-			/* Read VFC CAM read response */
-			ARR_REG_RD(p_hwfn,
-				   p_ptt,
-				   s_storm_defs[storm_id].sem_fast_mem_addr +
-				   SEM_FAST_REG_VFC_DATA_RD,
-				   dump_buf + offset, VFC_CAM_RESP_DWORDS);
-		}
-	} else {
-		offset += total_size;
+	/* Prepare CAM address */
+	SET_VAR_FIELD(cam_addr, VFC_CAM_ADDR, OP, VFC_OPCODE_CAM_RD);
+
+	for (row = 0; row < VFC_CAM_NUM_ROWS;
+	     row++, offset += VFC_CAM_RESP_DWORDS) {
+		/* Write VFC CAM command */
+		SET_VAR_FIELD(cam_cmd, VFC_CAM_CMD, ROW, row);
+		ARR_REG_WR(p_hwfn,
+			   p_ptt,
+			   storm->sem_fast_mem_addr + SEM_FAST_REG_VFC_DATA_WR,
+			   cam_cmd, VFC_CAM_CMD_DWORDS);
+
+		/* Write VFC CAM address */
+		ARR_REG_WR(p_hwfn,
+			   p_ptt,
+			   storm->sem_fast_mem_addr + SEM_FAST_REG_VFC_ADDR,
+			   cam_addr, VFC_CAM_ADDR_DWORDS);
+
+		/* Read VFC CAM read response */
+		ARR_REG_RD(p_hwfn,
+			   p_ptt,
+			   storm->sem_fast_mem_addr + SEM_FAST_REG_VFC_DATA_RD,
+			   dump_buf + offset, VFC_CAM_RESP_DWORDS);
 	}
 
 	return offset;
@@ -3148,10 +3438,10 @@ static u32 qed_grc_dump_vfc_ram(struct qed_hwfn *p_hwfn,
 				u8 storm_id, struct vfc_ram_defs *ram_defs)
 {
 	u32 total_size = ram_defs->num_rows * VFC_RAM_RESP_DWORDS;
+	struct storm_defs *storm = &s_storm_defs[storm_id];
 	u32 ram_addr[VFC_RAM_ADDR_DWORDS] = { 0 };
 	u32 ram_cmd[VFC_RAM_CMD_DWORDS] = { 0 };
-	u32 offset = 0;
-	u32 row, i;
+	u32 row, i, offset = 0;
 
 	offset += qed_grc_dump_mem_hdr(p_hwfn,
 				       dump_buf + offset,
@@ -3162,7 +3452,7 @@ static u32 qed_grc_dump_vfc_ram(struct qed_hwfn *p_hwfn,
 				       256,
 				       false,
 				       ram_defs->type_name,
-				       true, s_storm_defs[storm_id].letter);
+				       true, storm->letter);
 
 	/* Prepare RAM address */
 	SET_VAR_FIELD(ram_addr, VFC_RAM_ADDR, OP, VFC_OPCODE_RAM_RD);
@@ -3176,23 +3466,20 @@ static u32 qed_grc_dump_vfc_ram(struct qed_hwfn *p_hwfn,
 		/* Write VFC RAM command */
 		ARR_REG_WR(p_hwfn,
 			   p_ptt,
-			   s_storm_defs[storm_id].sem_fast_mem_addr +
-			   SEM_FAST_REG_VFC_DATA_WR,
+			   storm->sem_fast_mem_addr + SEM_FAST_REG_VFC_DATA_WR,
 			   ram_cmd, VFC_RAM_CMD_DWORDS);
 
 		/* Write VFC RAM address */
 		SET_VAR_FIELD(ram_addr, VFC_RAM_ADDR, ROW, row);
 		ARR_REG_WR(p_hwfn,
 			   p_ptt,
-			   s_storm_defs[storm_id].sem_fast_mem_addr +
-			   SEM_FAST_REG_VFC_ADDR,
+			   storm->sem_fast_mem_addr + SEM_FAST_REG_VFC_ADDR,
 			   ram_addr, VFC_RAM_ADDR_DWORDS);
 
 		/* Read VFC RAM read response */
 		ARR_REG_RD(p_hwfn,
 			   p_ptt,
-			   s_storm_defs[storm_id].sem_fast_mem_addr +
-			   SEM_FAST_REG_VFC_DATA_RD,
+			   storm->sem_fast_mem_addr + SEM_FAST_REG_VFC_DATA_RD,
 			   dump_buf + offset, VFC_RAM_RESP_DWORDS);
 	}
 
@@ -3208,28 +3495,27 @@ static u32 qed_grc_dump_vfc(struct qed_hwfn *p_hwfn,
 	u32 offset = 0;
 
 	for (storm_id = 0; storm_id < MAX_DBG_STORMS; storm_id++) {
-		if (qed_grc_is_storm_included(p_hwfn,
-					      (enum dbg_storms)storm_id) &&
-		    s_storm_defs[storm_id].has_vfc &&
-		    (storm_id != DBG_PSTORM_ID ||
-		     dev_data->platform_id == PLATFORM_ASIC)) {
-			/* Read CAM */
-			offset += qed_grc_dump_vfc_cam(p_hwfn,
+		if (!qed_grc_is_storm_included(p_hwfn,
+					       (enum dbg_storms)storm_id) ||
+		    !s_storm_defs[storm_id].has_vfc ||
+		    (storm_id == DBG_PSTORM_ID && dev_data->platform_id !=
+		     PLATFORM_ASIC))
+			continue;
+
+		/* Read CAM */
+		offset += qed_grc_dump_vfc_cam(p_hwfn,
+					       p_ptt,
+					       dump_buf + offset,
+					       dump, storm_id);
+
+		/* Read RAM */
+		for (i = 0; i < NUM_VFC_RAM_TYPES; i++)
+			offset += qed_grc_dump_vfc_ram(p_hwfn,
 						       p_ptt,
 						       dump_buf + offset,
-						       dump, storm_id);
-
-			/* Read RAM */
-			for (i = 0; i < NUM_VFC_RAM_TYPES; i++)
-				offset += qed_grc_dump_vfc_ram(p_hwfn,
-							       p_ptt,
-							       dump_buf +
-							       offset,
-							       dump,
-							       storm_id,
-							       &s_vfc_ram_defs
-							       [i]);
-		}
+						       dump,
+						       storm_id,
+						       &s_vfc_ram_defs[i]);
 	}
 
 	return offset;
@@ -3244,14 +3530,17 @@ static u32 qed_grc_dump_rss(struct qed_hwfn *p_hwfn,
 	u8 rss_mem_id;
 
 	for (rss_mem_id = 0; rss_mem_id < NUM_RSS_MEM_TYPES; rss_mem_id++) {
-		struct rss_mem_defs *rss_defs = &s_rss_mem_defs[rss_mem_id];
-		u32 num_entries = rss_defs->num_entries[dev_data->chip_id];
-		u32 entry_width = rss_defs->entry_width[dev_data->chip_id];
-		u32 total_dwords = (num_entries * entry_width) / 32;
-		u32 size = RSS_REG_RSS_RAM_DATA_SIZE;
-		bool packed = (entry_width == 16);
-		u32 rss_addr = rss_defs->addr;
-		u32 i, addr;
+		u32 rss_addr, num_entries, entry_width, total_dwords, i;
+		struct rss_mem_defs *rss_defs;
+		u32 addr, size;
+		bool packed;
+
+		rss_defs = &s_rss_mem_defs[rss_mem_id];
+		rss_addr = rss_defs->addr;
+		num_entries = rss_defs->num_entries[dev_data->chip_id];
+		entry_width = rss_defs->entry_width[dev_data->chip_id];
+		total_dwords = (num_entries * entry_width) / 32;
+		packed = (entry_width == 16);
 
 		offset += qed_grc_dump_mem_hdr(p_hwfn,
 					       dump_buf + offset,
@@ -3263,23 +3552,23 @@ static u32 qed_grc_dump_rss(struct qed_hwfn *p_hwfn,
 					       packed,
 					       rss_defs->type_name, false, 0);
 
+		/* Dump RSS data */
 		if (!dump) {
 			offset += total_dwords;
 			continue;
 		}
 
-		/* Dump RSS data */
-		for (i = 0; i < total_dwords;
-		     i += RSS_REG_RSS_RAM_DATA_SIZE, rss_addr++) {
-			addr = BYTES_TO_DWORDS(RSS_REG_RSS_RAM_DATA);
+		addr = BYTES_TO_DWORDS(RSS_REG_RSS_RAM_DATA);
+		size = RSS_REG_RSS_RAM_DATA_SIZE;
+		for (i = 0; i < total_dwords; i += size, rss_addr++) {
 			qed_wr(p_hwfn, p_ptt, RSS_REG_RSS_RAM_ADDR, rss_addr);
-				offset += qed_grc_dump_addr_range(p_hwfn,
-								  p_ptt,
-								  dump_buf +
-								  offset,
-								  dump,
-								  addr,
-								  size);
+			offset += qed_grc_dump_addr_range(p_hwfn,
+							  p_ptt,
+							  dump_buf + offset,
+							  dump,
+							  addr,
+							  size,
+							  false);
 		}
 	}
 
@@ -3316,10 +3605,11 @@ static u32 qed_grc_dump_big_ram(struct qed_hwfn *p_hwfn,
 				       BIG_RAM_BLOCK_SIZE_BYTES * 8,
 				       false, type_name, false, 0);
 
+	/* Read and dump Big RAM data */
 	if (!dump)
 		return offset + ram_size;
 
-	/* Read and dump Big RAM data */
+	/* Dump Big RAM */
 	for (i = 0; i < total_blocks / 2; i++) {
 		u32 addr, len;
 
@@ -3331,7 +3621,8 @@ static u32 qed_grc_dump_big_ram(struct qed_hwfn *p_hwfn,
 						  dump_buf + offset,
 						  dump,
 						  addr,
-						  len);
+						  len,
+						  false);
 	}
 
 	return offset;
@@ -3359,7 +3650,7 @@ static u32 qed_grc_dump_mcp(struct qed_hwfn *p_hwfn,
 				   NULL,
 				   BYTES_TO_DWORDS(MCP_REG_SCRATCH),
 				   MCP_REG_SCRATCH_SIZE,
-				   0, false, "MCP", false, 0);
+				   false, 0, false, "MCP", false, 0);
 
 	/* Dump MCP cpu_reg_file */
 	offset += qed_grc_dump_mem(p_hwfn,
@@ -3369,7 +3660,7 @@ static u32 qed_grc_dump_mcp(struct qed_hwfn *p_hwfn,
 				   NULL,
 				   BYTES_TO_DWORDS(MCP_REG_CPU_REG_FILE),
 				   MCP_REG_CPU_REG_FILE_SIZE,
-				   0, false, "MCP", false, 0);
+				   false, 0, false, "MCP", false, 0);
 
 	/* Dump MCP registers */
 	block_enable[BLOCK_MCP] = true;
@@ -3387,11 +3678,13 @@ static u32 qed_grc_dump_mcp(struct qed_hwfn *p_hwfn,
 					 dump_buf + offset,
 					 dump,
 					 addr,
-					 1);
+					 1,
+					 false);
 
 	/* Release MCP */
 	if (halted && qed_mcp_resume(p_hwfn, p_ptt))
 		DP_NOTICE(p_hwfn, "Failed to resume MCP after halt!\n");
+
 	return offset;
 }
 
@@ -3404,14 +3697,26 @@ static u32 qed_grc_dump_phy(struct qed_hwfn *p_hwfn,
 	u8 phy_id;
 
 	for (phy_id = 0; phy_id < ARRAY_SIZE(s_phy_defs); phy_id++) {
-		struct phy_defs *phy_defs = &s_phy_defs[phy_id];
-		int printed_chars;
-
-		printed_chars = snprintf(mem_name, sizeof(mem_name), "tbus_%s",
-					 phy_defs->phy_name);
-		if (printed_chars < 0 || printed_chars >= sizeof(mem_name))
+		u32 addr_lo_addr, addr_hi_addr, data_lo_addr, data_hi_addr;
+		struct phy_defs *phy_defs;
+		u8 *bytes_buf;
+
+		phy_defs = &s_phy_defs[phy_id];
+		addr_lo_addr = phy_defs->base_addr +
+			       phy_defs->tbus_addr_lo_addr;
+		addr_hi_addr = phy_defs->base_addr +
+			       phy_defs->tbus_addr_hi_addr;
+		data_lo_addr = phy_defs->base_addr +
+			       phy_defs->tbus_data_lo_addr;
+		data_hi_addr = phy_defs->base_addr +
+			       phy_defs->tbus_data_hi_addr;
+		bytes_buf = (u8 *)(dump_buf + offset);
+
+		if (snprintf(mem_name, sizeof(mem_name), "tbus_%s",
+			     phy_defs->phy_name) < 0)
 			DP_NOTICE(p_hwfn,
 				  "Unexpected debug error: invalid PHY memory name\n");
+
 		offset += qed_grc_dump_mem_hdr(p_hwfn,
 					       dump_buf + offset,
 					       dump,
@@ -3419,34 +3724,26 @@ static u32 qed_grc_dump_phy(struct qed_hwfn *p_hwfn,
 					       0,
 					       PHY_DUMP_SIZE_DWORDS,
 					       16, true, mem_name, false, 0);
-		if (dump) {
-			u32 addr_lo_addr = phy_defs->base_addr +
-					   phy_defs->tbus_addr_lo_addr;
-			u32 addr_hi_addr = phy_defs->base_addr +
-					   phy_defs->tbus_addr_hi_addr;
-			u32 data_lo_addr = phy_defs->base_addr +
-					   phy_defs->tbus_data_lo_addr;
-			u32 data_hi_addr = phy_defs->base_addr +
-					   phy_defs->tbus_data_hi_addr;
-			u8 *bytes_buf = (u8 *)(dump_buf + offset);
-
-			for (tbus_hi_offset = 0;
-			     tbus_hi_offset < (NUM_PHY_TBUS_ADDRESSES >> 8);
-			     tbus_hi_offset++) {
+
+		if (!dump) {
+			offset += PHY_DUMP_SIZE_DWORDS;
+			continue;
+		}
+
+		for (tbus_hi_offset = 0;
+		     tbus_hi_offset < (NUM_PHY_TBUS_ADDRESSES >> 8);
+		     tbus_hi_offset++) {
+			qed_wr(p_hwfn, p_ptt, addr_hi_addr, tbus_hi_offset);
+			for (tbus_lo_offset = 0; tbus_lo_offset < 256;
+			     tbus_lo_offset++) {
 				qed_wr(p_hwfn,
-				       p_ptt, addr_hi_addr, tbus_hi_offset);
-				for (tbus_lo_offset = 0; tbus_lo_offset < 256;
-				     tbus_lo_offset++) {
-					qed_wr(p_hwfn,
-					       p_ptt,
-					       addr_lo_addr, tbus_lo_offset);
-					*(bytes_buf++) =
-						(u8)qed_rd(p_hwfn, p_ptt,
-							   data_lo_addr);
-					*(bytes_buf++) =
-						(u8)qed_rd(p_hwfn, p_ptt,
-							   data_hi_addr);
-				}
+				       p_ptt, addr_lo_addr, tbus_lo_offset);
+				*(bytes_buf++) = (u8)qed_rd(p_hwfn,
+							    p_ptt,
+							    data_lo_addr);
+				*(bytes_buf++) = (u8)qed_rd(p_hwfn,
+							    p_ptt,
+							    data_hi_addr);
 			}
 		}
 
@@ -3460,16 +3757,17 @@ static void qed_config_dbg_line(struct qed_hwfn *p_hwfn,
 				struct qed_ptt *p_ptt,
 				enum block_id block_id,
 				u8 line_id,
-				u8 cycle_en,
-				u8 right_shift, u8 force_valid, u8 force_frame)
+				u8 enable_mask,
+				u8 right_shift,
+				u8 force_valid_mask, u8 force_frame_mask)
 {
-	struct block_defs *p_block_defs = s_block_defs[block_id];
+	struct block_defs *block = s_block_defs[block_id];
 
-	qed_wr(p_hwfn, p_ptt, p_block_defs->dbg_select_addr, line_id);
-	qed_wr(p_hwfn, p_ptt, p_block_defs->dbg_cycle_enable_addr, cycle_en);
-	qed_wr(p_hwfn, p_ptt, p_block_defs->dbg_shift_addr, right_shift);
-	qed_wr(p_hwfn, p_ptt, p_block_defs->dbg_force_valid_addr, force_valid);
-	qed_wr(p_hwfn, p_ptt, p_block_defs->dbg_force_frame_addr, force_frame);
+	qed_wr(p_hwfn, p_ptt, block->dbg_select_addr, line_id);
+	qed_wr(p_hwfn, p_ptt, block->dbg_enable_addr, enable_mask);
+	qed_wr(p_hwfn, p_ptt, block->dbg_shift_addr, right_shift);
+	qed_wr(p_hwfn, p_ptt, block->dbg_force_valid_addr, force_valid_mask);
+	qed_wr(p_hwfn, p_ptt, block->dbg_force_frame_addr, force_frame_mask);
 }
 
 /* Dumps Static Debug data. Returns the dumped size in dwords. */
@@ -3477,10 +3775,12 @@ static u32 qed_grc_dump_static_debug(struct qed_hwfn *p_hwfn,
 				     struct qed_ptt *p_ptt,
 				     u32 *dump_buf, bool dump)
 {
-	u32 block_dwords = NUM_DBG_BUS_LINES * STATIC_DEBUG_LINE_DWORDS;
 	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
-	u32 offset = 0, block_id, line_id;
-	struct block_defs *p_block_defs;
+	u32 block_id, line_id, offset = 0;
+
+	/* Skip static debug if a debug bus recording is in progress */
+	if (qed_rd(p_hwfn, p_ptt, DBG_REG_DBG_BLOCK_ON))
+		return 0;
 
 	if (dump) {
 		DP_VERBOSE(p_hwfn,
@@ -3488,11 +3788,11 @@ static u32 qed_grc_dump_static_debug(struct qed_hwfn *p_hwfn,
 
 		/* Disable all blocks debug output */
 		for (block_id = 0; block_id < MAX_BLOCK_ID; block_id++) {
-			p_block_defs = s_block_defs[block_id];
+			struct block_defs *block = s_block_defs[block_id];
 
-			if (p_block_defs->has_dbg_bus[dev_data->chip_id])
-				qed_wr(p_hwfn, p_ptt,
-				       p_block_defs->dbg_cycle_enable_addr, 0);
+			if (block->has_dbg_bus[dev_data->chip_id])
+				qed_wr(p_hwfn, p_ptt, block->dbg_enable_addr,
+				       0);
 		}
 
 		qed_bus_reset_dbg_block(p_hwfn, p_ptt);
@@ -3506,59 +3806,71 @@ static u32 qed_grc_dump_static_debug(struct qed_hwfn *p_hwfn,
 
 	/* Dump all static debug lines for each relevant block */
 	for (block_id = 0; block_id < MAX_BLOCK_ID; block_id++) {
-		p_block_defs = s_block_defs[block_id];
+		struct block_defs *block = s_block_defs[block_id];
+		struct dbg_bus_block *block_desc;
+		u32 block_dwords, addr, len;
+		u8 dbg_client_id;
 
-		if (!p_block_defs->has_dbg_bus[dev_data->chip_id])
+		if (!block->has_dbg_bus[dev_data->chip_id])
 			continue;
 
+		block_desc =
+			get_dbg_bus_block_desc(p_hwfn,
+					       (enum block_id)block_id);
+		block_dwords = NUM_DBG_LINES(block_desc) *
+			       STATIC_DEBUG_LINE_DWORDS;
+
 		/* Dump static section params */
 		offset += qed_grc_dump_mem_hdr(p_hwfn,
 					       dump_buf + offset,
 					       dump,
-					       p_block_defs->name, 0,
-					       block_dwords, 32, false,
-					       "STATIC", false, 0);
-
-		if (dump && !dev_data->block_in_reset[block_id]) {
-			u8 dbg_client_id =
-				p_block_defs->dbg_client_id[dev_data->chip_id];
-			u32 addr = BYTES_TO_DWORDS(DBG_REG_CALENDAR_OUT_DATA);
-			u32 len = STATIC_DEBUG_LINE_DWORDS;
-
-			/* Enable block's client */
-			qed_bus_enable_clients(p_hwfn, p_ptt,
-					       BIT(dbg_client_id));
-
-			for (line_id = 0; line_id < NUM_DBG_BUS_LINES;
-			     line_id++) {
-				/* Configure debug line ID */
-				qed_config_dbg_line(p_hwfn,
-						    p_ptt,
-						    (enum block_id)block_id,
-						    (u8)line_id,
-						    0xf, 0, 0, 0);
+					       block->name,
+					       0,
+					       block_dwords,
+					       32, false, "STATIC", false, 0);
 
-				/* Read debug line info */
-				offset +=
-				    qed_grc_dump_addr_range(p_hwfn,
-							    p_ptt,
-							    dump_buf + offset,
-							    dump,
-							    addr,
-							    len);
-			}
+		if (!dump) {
+			offset += block_dwords;
+			continue;
+		}
 
-			/* Disable block's client and debug output */
-			qed_bus_enable_clients(p_hwfn, p_ptt, 0);
-			qed_wr(p_hwfn, p_ptt,
-			       p_block_defs->dbg_cycle_enable_addr, 0);
-		} else {
-			/* All lines are invalid - dump zeros */
-			if (dump)
-				memset(dump_buf + offset, 0,
-				       DWORDS_TO_BYTES(block_dwords));
+		/* If all lines are invalid - dump zeros */
+		if (dev_data->block_in_reset[block_id]) {
+			memset(dump_buf + offset, 0,
+			       DWORDS_TO_BYTES(block_dwords));
 			offset += block_dwords;
+			continue;
 		}
+
+		/* Enable block's client */
+		dbg_client_id = block->dbg_client_id[dev_data->chip_id];
+		qed_bus_enable_clients(p_hwfn,
+				       p_ptt,
+				       BIT(dbg_client_id));
+
+		addr = BYTES_TO_DWORDS(DBG_REG_CALENDAR_OUT_DATA);
+		len = STATIC_DEBUG_LINE_DWORDS;
+		for (line_id = 0; line_id < (u32)NUM_DBG_LINES(block_desc);
+		     line_id++) {
+			/* Configure debug line ID */
+			qed_config_dbg_line(p_hwfn,
+					    p_ptt,
+					    (enum block_id)block_id,
+					    (u8)line_id, 0xf, 0, 0, 0);
+
+			/* Read debug line info */
+			offset += qed_grc_dump_addr_range(p_hwfn,
+							  p_ptt,
+							  dump_buf + offset,
+							  dump,
+							  addr,
+							  len,
+							  true);
+		}
+
+		/* Disable block's client and debug output */
+		qed_bus_enable_clients(p_hwfn, p_ptt, 0);
+		qed_wr(p_hwfn, p_ptt, block->dbg_enable_addr, 0);
 	}
 
 	if (dump) {
@@ -3584,8 +3896,8 @@ static enum dbg_status qed_grc_dump(struct qed_hwfn *p_hwfn,
 
 	*num_dumped_dwords = 0;
 
-	/* Find port mode */
 	if (dump) {
+		/* Find port mode */
 		switch (qed_rd(p_hwfn, p_ptt, MISC_REG_PORT_MODE)) {
 		case 0:
 			port_mode = 1;
@@ -3597,11 +3909,10 @@ static enum dbg_status qed_grc_dump(struct qed_hwfn *p_hwfn,
 			port_mode = 4;
 			break;
 		}
-	}
 
-	/* Update reset state */
-	if (dump)
+		/* Update reset state */
 		qed_update_blocks_reset_state(p_hwfn, p_ptt);
+	}
 
 	/* Dump global params */
 	offset += qed_dump_common_global_params(p_hwfn,
@@ -3635,7 +3946,8 @@ static enum dbg_status qed_grc_dump(struct qed_hwfn *p_hwfn,
 	}
 
 	/* Disable all parities using MFW command */
-	if (dump && !qed_grc_get_param(p_hwfn, DBG_GRC_PARAM_NO_MCP)) {
+	if (dump &&
+	    !qed_grc_get_param(p_hwfn, DBG_GRC_PARAM_NO_MCP)) {
 		parities_masked = !qed_mcp_mask_parities(p_hwfn, p_ptt, 1);
 		if (!parities_masked) {
 			DP_NOTICE(p_hwfn,
@@ -3661,9 +3973,9 @@ static enum dbg_status qed_grc_dump(struct qed_hwfn *p_hwfn,
 
 	/* Dump all regs  */
 	if (qed_grc_is_included(p_hwfn, DBG_GRC_PARAM_DUMP_REGS)) {
-		/* Dump all blocks except MCP */
 		bool block_enable[MAX_BLOCK_ID];
 
+		/* Dump all blocks except MCP */
 		for (i = 0; i < MAX_BLOCK_ID; i++)
 			block_enable[i] = true;
 		block_enable[BLOCK_MCP] = false;
@@ -3732,7 +4044,8 @@ static enum dbg_status qed_grc_dump(struct qed_hwfn *p_hwfn,
 						    dump_buf + offset, dump);
 
 	/* Dump last section */
-	offset += qed_dump_last_section(dump_buf, offset, dump);
+	offset += qed_dump_last_section(p_hwfn, dump_buf, offset, dump);
+
 	if (dump) {
 		/* Unstall storms */
 		if (qed_grc_get_param(p_hwfn, DBG_GRC_PARAM_UNSTALL))
@@ -3763,19 +4076,20 @@ static u32 qed_idle_chk_dump_failure(struct qed_hwfn *p_hwfn,
 				     const struct dbg_idle_chk_rule *rule,
 				     u16 fail_entry_id, u32 *cond_reg_values)
 {
-	const union dbg_idle_chk_reg *regs = &((const union dbg_idle_chk_reg *)
-					       s_dbg_arrays
-					       [BIN_BUF_DBG_IDLE_CHK_REGS].
-					       ptr)[rule->reg_offset];
-	const struct dbg_idle_chk_cond_reg *cond_regs = &regs[0].cond_reg;
 	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
-	struct dbg_idle_chk_result_hdr *hdr =
-		(struct dbg_idle_chk_result_hdr *)dump_buf;
-	const struct dbg_idle_chk_info_reg *info_regs =
-		&regs[rule->num_cond_regs].info_reg;
-	u32 next_reg_offset = 0, i, offset = 0;
+	const struct dbg_idle_chk_cond_reg *cond_regs;
+	const struct dbg_idle_chk_info_reg *info_regs;
+	u32 i, next_reg_offset = 0, offset = 0;
+	struct dbg_idle_chk_result_hdr *hdr;
+	const union dbg_idle_chk_reg *regs;
 	u8 reg_id;
 
+	hdr = (struct dbg_idle_chk_result_hdr *)dump_buf;
+	regs = &((const union dbg_idle_chk_reg *)
+		 s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_REGS].ptr)[rule->reg_offset];
+	cond_regs = &regs[0].cond_reg;
+	info_regs = &regs[rule->num_cond_regs].info_reg;
+
 	/* Dump rule data */
 	if (dump) {
 		memset(hdr, 0, sizeof(*hdr));
@@ -3790,33 +4104,31 @@ static u32 qed_idle_chk_dump_failure(struct qed_hwfn *p_hwfn,
 	/* Dump condition register values */
 	for (reg_id = 0; reg_id < rule->num_cond_regs; reg_id++) {
 		const struct dbg_idle_chk_cond_reg *reg = &cond_regs[reg_id];
+		struct dbg_idle_chk_result_reg_hdr *reg_hdr;
 
-		/* Write register header */
-		if (dump) {
-			struct dbg_idle_chk_result_reg_hdr *reg_hdr =
-			    (struct dbg_idle_chk_result_reg_hdr *)(dump_buf
-								   + offset);
-			offset += IDLE_CHK_RESULT_REG_HDR_DWORDS;
-			memset(reg_hdr, 0,
-			       sizeof(struct dbg_idle_chk_result_reg_hdr));
-			reg_hdr->start_entry = reg->start_entry;
-			reg_hdr->size = reg->entry_size;
-			SET_FIELD(reg_hdr->data,
-				  DBG_IDLE_CHK_RESULT_REG_HDR_IS_MEM,
-				  reg->num_entries > 1 || reg->start_entry > 0
-				  ? 1 : 0);
-			SET_FIELD(reg_hdr->data,
-				  DBG_IDLE_CHK_RESULT_REG_HDR_REG_ID, reg_id);
+		reg_hdr = (struct dbg_idle_chk_result_reg_hdr *)
+			  (dump_buf + offset);
 
-			/* Write register values */
-			for (i = 0; i < reg_hdr->size;
-			     i++, next_reg_offset++, offset++)
-				dump_buf[offset] =
-				    cond_reg_values[next_reg_offset];
-		} else {
+		/* Write register header */
+		if (!dump) {
 			offset += IDLE_CHK_RESULT_REG_HDR_DWORDS +
 			    reg->entry_size;
+			continue;
 		}
+
+		offset += IDLE_CHK_RESULT_REG_HDR_DWORDS;
+		memset(reg_hdr, 0, sizeof(*reg_hdr));
+		reg_hdr->start_entry = reg->start_entry;
+		reg_hdr->size = reg->entry_size;
+		SET_FIELD(reg_hdr->data,
+			  DBG_IDLE_CHK_RESULT_REG_HDR_IS_MEM,
+			  reg->num_entries > 1 || reg->start_entry > 0 ? 1 : 0);
+		SET_FIELD(reg_hdr->data,
+			  DBG_IDLE_CHK_RESULT_REG_HDR_REG_ID, reg_id);
+
+		/* Write register values */
+		for (i = 0; i < reg_hdr->size; i++, next_reg_offset++, offset++)
+			dump_buf[offset] = cond_reg_values[next_reg_offset];
 	}
 
 	/* Dump info register values */
@@ -3824,12 +4136,12 @@ static u32 qed_idle_chk_dump_failure(struct qed_hwfn *p_hwfn,
 		const struct dbg_idle_chk_info_reg *reg = &info_regs[reg_id];
 		u32 block_id;
 
+		/* Check if register's block is in reset */
 		if (!dump) {
 			offset += IDLE_CHK_RESULT_REG_HDR_DWORDS + reg->size;
 			continue;
 		}
 
-		/* Check if register's block is in reset */
 		block_id = GET_FIELD(reg->data, DBG_IDLE_CHK_INFO_REG_BLOCK_ID);
 		if (block_id >= MAX_BLOCK_ID) {
 			DP_NOTICE(p_hwfn, "Invalid block_id\n");
@@ -3837,47 +4149,50 @@ static u32 qed_idle_chk_dump_failure(struct qed_hwfn *p_hwfn,
 		}
 
 		if (!dev_data->block_in_reset[block_id]) {
-			bool eval_mode = GET_FIELD(reg->mode.data,
-						   DBG_MODE_HDR_EVAL_MODE) > 0;
-			bool mode_match = true;
+			struct dbg_idle_chk_result_reg_hdr *reg_hdr;
+			bool wide_bus, eval_mode, mode_match = true;
+			u16 modes_buf_offset;
+			u32 addr;
+
+			reg_hdr = (struct dbg_idle_chk_result_reg_hdr *)
+				  (dump_buf + offset);
 
 			/* Check mode */
+			eval_mode = GET_FIELD(reg->mode.data,
+					      DBG_MODE_HDR_EVAL_MODE) > 0;
 			if (eval_mode) {
-				u16 modes_buf_offset =
-					GET_FIELD(reg->mode.data,
-						DBG_MODE_HDR_MODES_BUF_OFFSET);
+				modes_buf_offset =
+				    GET_FIELD(reg->mode.data,
+					      DBG_MODE_HDR_MODES_BUF_OFFSET);
 				mode_match =
 					qed_is_mode_match(p_hwfn,
 							  &modes_buf_offset);
 			}
 
-			if (mode_match) {
-				u32 addr =
-				    GET_FIELD(reg->data,
-					      DBG_IDLE_CHK_INFO_REG_ADDRESS);
-
-				/* Write register header */
-				struct dbg_idle_chk_result_reg_hdr *reg_hdr =
-					(struct dbg_idle_chk_result_reg_hdr *)
-					(dump_buf + offset);
-
-				offset += IDLE_CHK_RESULT_REG_HDR_DWORDS;
-				hdr->num_dumped_info_regs++;
-				memset(reg_hdr, 0, sizeof(*reg_hdr));
-				reg_hdr->size = reg->size;
-				SET_FIELD(reg_hdr->data,
-					  DBG_IDLE_CHK_RESULT_REG_HDR_REG_ID,
-					  rule->num_cond_regs + reg_id);
-
-				/* Write register values */
-				offset +=
-				    qed_grc_dump_addr_range(p_hwfn,
-							    p_ptt,
-							    dump_buf + offset,
-							    dump,
-							    addr,
-							    reg->size);
-			}
+			if (!mode_match)
+				continue;
+
+			addr = GET_FIELD(reg->data,
+					 DBG_IDLE_CHK_INFO_REG_ADDRESS);
+			wide_bus = GET_FIELD(reg->data,
+					     DBG_IDLE_CHK_INFO_REG_WIDE_BUS);
+
+			/* Write register header */
+			offset += IDLE_CHK_RESULT_REG_HDR_DWORDS;
+			hdr->num_dumped_info_regs++;
+			memset(reg_hdr, 0, sizeof(*reg_hdr));
+			reg_hdr->size = reg->size;
+			SET_FIELD(reg_hdr->data,
+				  DBG_IDLE_CHK_RESULT_REG_HDR_REG_ID,
+				  rule->num_cond_regs + reg_id);
+
+			/* Write register values */
+			offset += qed_grc_dump_addr_range(p_hwfn,
+							  p_ptt,
+							  dump_buf + offset,
+							  dump,
+							  addr,
+							  reg->size, wide_bus);
 		}
 	}
 
@@ -3898,6 +4213,7 @@ qed_idle_chk_dump_rule_entries(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 	u8 reg_id;
 
 	*num_failing_rules = 0;
+
 	for (i = 0; i < num_input_rules; i++) {
 		const struct dbg_idle_chk_cond_reg *cond_regs;
 		const struct dbg_idle_chk_rule *rule;
@@ -3920,8 +4236,9 @@ qed_idle_chk_dump_rule_entries(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 		 */
 		for (reg_id = 0; reg_id < rule->num_cond_regs && check_rule;
 		     reg_id++) {
-			u32 block_id = GET_FIELD(cond_regs[reg_id].data,
-						DBG_IDLE_CHK_COND_REG_BLOCK_ID);
+			u32 block_id =
+				GET_FIELD(cond_regs[reg_id].data,
+					  DBG_IDLE_CHK_COND_REG_BLOCK_ID);
 
 			if (block_id >= MAX_BLOCK_ID) {
 				DP_NOTICE(p_hwfn, "Invalid block_id\n");
@@ -3936,48 +4253,47 @@ qed_idle_chk_dump_rule_entries(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 		if (!check_rule && dump)
 			continue;
 
-		if (!dump) {
-			u32 entry_dump_size =
-				qed_idle_chk_dump_failure(p_hwfn,
-							  p_ptt,
-							  dump_buf + offset,
-							  false,
-							  rule->rule_id,
-							  rule,
-							  0,
-							  NULL);
-
-			offset += num_reg_entries * entry_dump_size;
-			(*num_failing_rules) += num_reg_entries;
-			continue;
-		}
-
 		/* Go over all register entries (number of entries is the same
 		 * for all condition registers).
 		 */
 		for (entry_id = 0; entry_id < num_reg_entries; entry_id++) {
-			/* Read current entry of all condition registers */
 			u32 next_reg_offset = 0;
 
+			if (!dump) {
+				offset += qed_idle_chk_dump_failure(p_hwfn,
+							p_ptt,
+							dump_buf + offset,
+							false,
+							rule->rule_id,
+							rule,
+							entry_id,
+							NULL);
+				(*num_failing_rules)++;
+				break;
+			}
+
+			/* Read current entry of all condition registers */
 			for (reg_id = 0; reg_id < rule->num_cond_regs;
 			     reg_id++) {
 				const struct dbg_idle_chk_cond_reg *reg =
-					&cond_regs[reg_id];
+				    &cond_regs[reg_id];
+				u32 padded_entry_size, addr;
+				bool wide_bus;
 
-				/* Find GRC address (if it's a memory,the
+				/* Find GRC address (if it's a memory, the
 				 * address of the specific entry is calculated).
 				 */
-				u32 addr =
+				addr = GET_FIELD(reg->data,
+						 DBG_IDLE_CHK_COND_REG_ADDRESS);
+				wide_bus =
 				    GET_FIELD(reg->data,
-					      DBG_IDLE_CHK_COND_REG_ADDRESS);
-
+					      DBG_IDLE_CHK_COND_REG_WIDE_BUS);
 				if (reg->num_entries > 1 ||
 				    reg->start_entry > 0) {
-					u32 padded_entry_size =
-					   reg->entry_size > 1 ?
-					   roundup_pow_of_two(reg->entry_size) :
-					   1;
-
+					padded_entry_size =
+					    reg->entry_size > 1 ?
+					    roundup_pow_of_two(reg->entry_size)
+					    : 1;
 					addr += (reg->start_entry + entry_id) *
 						padded_entry_size;
 				}
@@ -3991,28 +4307,27 @@ qed_idle_chk_dump_rule_entries(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 				}
 
 				next_reg_offset +=
-				    qed_grc_dump_addr_range(p_hwfn,
-							    p_ptt,
+				    qed_grc_dump_addr_range(p_hwfn, p_ptt,
 							    cond_reg_values +
 							    next_reg_offset,
 							    dump, addr,
-							    reg->entry_size);
+							    reg->entry_size,
+							    wide_bus);
 			}
 
-			/* Call rule's condition function - a return value of
-			 * true indicates failure.
+			/* Call rule condition function.
+			 * If returns true, it's a failure.
 			 */
-			if ((*cond_arr[rule->cond_id])(cond_reg_values,
-						       imm_values)) {
-				offset +=
-				    qed_idle_chk_dump_failure(p_hwfn,
-							      p_ptt,
-							      dump_buf + offset,
-							      dump,
-							      rule->rule_id,
-							      rule,
-							      entry_id,
-							      cond_reg_values);
+			if ((*cond_arr[rule->cond_id]) (cond_reg_values,
+							imm_values)) {
+				offset += qed_idle_chk_dump_failure(p_hwfn,
+							p_ptt,
+							dump_buf + offset,
+							dump,
+							rule->rule_id,
+							rule,
+							entry_id,
+							cond_reg_values);
 				(*num_failing_rules)++;
 				break;
 			}
@@ -4028,8 +4343,8 @@ qed_idle_chk_dump_rule_entries(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 static u32 qed_idle_chk_dump(struct qed_hwfn *p_hwfn,
 			     struct qed_ptt *p_ptt, u32 *dump_buf, bool dump)
 {
-	u32 offset = 0, input_offset = 0, num_failing_rules = 0;
-	u32 num_failing_rules_offset;
+	u32 num_failing_rules_offset, offset = 0, input_offset = 0;
+	u32 num_failing_rules = 0;
 
 	/* Dump global params */
 	offset += qed_dump_common_global_params(p_hwfn,
@@ -4042,29 +4357,29 @@ static u32 qed_idle_chk_dump(struct qed_hwfn *p_hwfn,
 	offset += qed_dump_section_hdr(dump_buf + offset, dump, "idle_chk", 1);
 	num_failing_rules_offset = offset;
 	offset += qed_dump_num_param(dump_buf + offset, dump, "num_rules", 0);
+
 	while (input_offset <
 	       s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_RULES].size_in_dwords) {
 		const struct dbg_idle_chk_cond_hdr *cond_hdr =
 			(const struct dbg_idle_chk_cond_hdr *)
 			&s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_RULES].ptr
 			[input_offset++];
-		bool eval_mode = GET_FIELD(cond_hdr->mode.data,
-					   DBG_MODE_HDR_EVAL_MODE) > 0;
-		bool mode_match = true;
+		bool eval_mode, mode_match = true;
+		u32 curr_failing_rules;
+		u16 modes_buf_offset;
 
 		/* Check mode */
+		eval_mode = GET_FIELD(cond_hdr->mode.data,
+				      DBG_MODE_HDR_EVAL_MODE) > 0;
 		if (eval_mode) {
-			u16 modes_buf_offset =
+			modes_buf_offset =
 				GET_FIELD(cond_hdr->mode.data,
 					  DBG_MODE_HDR_MODES_BUF_OFFSET);
-
 			mode_match = qed_is_mode_match(p_hwfn,
 						       &modes_buf_offset);
 		}
 
 		if (mode_match) {
-			u32 curr_failing_rules;
-
 			offset +=
 			    qed_idle_chk_dump_rule_entries(p_hwfn,
 				p_ptt,
@@ -4086,10 +4401,13 @@ static u32 qed_idle_chk_dump(struct qed_hwfn *p_hwfn,
 		qed_dump_num_param(dump_buf + num_failing_rules_offset,
 				   dump, "num_rules", num_failing_rules);
 
+	/* Dump last section */
+	offset += qed_dump_last_section(p_hwfn, dump_buf, offset, dump);
+
 	return offset;
 }
 
-/* Finds the meta data image in NVRAM. */
+/* Finds the meta data image in NVRAM */
 static enum dbg_status qed_find_nvram_image(struct qed_hwfn *p_hwfn,
 					    struct qed_ptt *p_ptt,
 					    u32 image_type,
@@ -4098,16 +4416,16 @@ static enum dbg_status qed_find_nvram_image(struct qed_hwfn *p_hwfn,
 {
 	u32 ret_mcp_resp, ret_mcp_param, ret_txn_size;
 	struct mcp_file_att file_att;
+	int nvm_result;
 
 	/* Call NVRAM get file command */
-	int nvm_result = qed_mcp_nvm_rd_cmd(p_hwfn,
-					    p_ptt,
-					    DRV_MSG_CODE_NVM_GET_FILE_ATT,
-					    image_type,
-					    &ret_mcp_resp,
-					    &ret_mcp_param,
-					    &ret_txn_size,
-					    (u32 *)&file_att);
+	nvm_result = qed_mcp_nvm_rd_cmd(p_hwfn,
+					p_ptt,
+					DRV_MSG_CODE_NVM_GET_FILE_ATT,
+					image_type,
+					&ret_mcp_resp,
+					&ret_mcp_param,
+					&ret_txn_size, (u32 *)&file_att);
 
 	/* Check response */
 	if (nvm_result ||
@@ -4117,6 +4435,7 @@ static enum dbg_status qed_find_nvram_image(struct qed_hwfn *p_hwfn,
 	/* Update return values */
 	*nvram_offset_bytes = file_att.nvm_start_addr;
 	*nvram_size_bytes = file_att.len;
+
 	DP_VERBOSE(p_hwfn,
 		   QED_MSG_DEBUG,
 		   "find_nvram_image: found NVRAM image of type %d in NVRAM offset %d bytes with size %d bytes\n",
@@ -4125,22 +4444,25 @@ static enum dbg_status qed_find_nvram_image(struct qed_hwfn *p_hwfn,
 	/* Check alignment */
 	if (*nvram_size_bytes & 0x3)
 		return DBG_STATUS_NON_ALIGNED_NVRAM_IMAGE;
+
 	return DBG_STATUS_OK;
 }
 
+/* Reads data from NVRAM */
 static enum dbg_status qed_nvram_read(struct qed_hwfn *p_hwfn,
 				      struct qed_ptt *p_ptt,
 				      u32 nvram_offset_bytes,
 				      u32 nvram_size_bytes, u32 *ret_buf)
 {
-	u32 ret_mcp_resp, ret_mcp_param, ret_read_size;
-	u32 bytes_to_copy, read_offset = 0;
+	u32 ret_mcp_resp, ret_mcp_param, ret_read_size, bytes_to_copy;
 	s32 bytes_left = nvram_size_bytes;
+	u32 read_offset = 0;
 
 	DP_VERBOSE(p_hwfn,
 		   QED_MSG_DEBUG,
 		   "nvram_read: reading image of size %d bytes from NVRAM\n",
 		   nvram_size_bytes);
+
 	do {
 		bytes_to_copy =
 		    (bytes_left >
@@ -4155,8 +4477,7 @@ static enum dbg_status qed_nvram_read(struct qed_hwfn *p_hwfn,
 					DRV_MB_PARAM_NVM_LEN_SHIFT),
 				       &ret_mcp_resp, &ret_mcp_param,
 				       &ret_read_size,
-				       (u32 *)((u8 *)ret_buf +
-					       read_offset)) != 0)
+				       (u32 *)((u8 *)ret_buf + read_offset)))
 			return DBG_STATUS_NVRAM_READ_FAILED;
 
 		/* Check response */
@@ -4172,24 +4493,20 @@ static enum dbg_status qed_nvram_read(struct qed_hwfn *p_hwfn,
 }
 
 /* Get info on the MCP Trace data in the scratchpad:
- * - trace_data_grc_addr - the GRC address of the trace data
- * - trace_data_size_bytes - the size in bytes of the MCP Trace data (without
- *	the header)
+ * - trace_data_grc_addr (OUT): trace data GRC address in bytes
+ * - trace_data_size (OUT): trace data size in bytes (without the header)
  */
 static enum dbg_status qed_mcp_trace_get_data_info(struct qed_hwfn *p_hwfn,
 						   struct qed_ptt *p_ptt,
 						   u32 *trace_data_grc_addr,
-						   u32 *trace_data_size_bytes)
+						   u32 *trace_data_size)
 {
-	/* Read MCP trace section offsize structure from MCP scratchpad */
-	u32 spad_trace_offsize = qed_rd(p_hwfn,
-					p_ptt,
-					MCP_SPAD_TRACE_OFFSIZE_ADDR);
-	u32 signature;
+	u32 spad_trace_offsize, signature;
 
-	/* Extract MCP trace section GRC address from offsize structure (within
-	 * scratchpad).
-	 */
+	/* Read trace section offsize structure from MCP scratchpad */
+	spad_trace_offsize = qed_rd(p_hwfn, p_ptt, MCP_SPAD_TRACE_OFFSIZE_ADDR);
+
+	/* Extract trace section address from offsize (in scratchpad) */
 	*trace_data_grc_addr =
 		MCP_REG_SCRATCH + SECTION_OFFSET(spad_trace_offsize);
 
@@ -4197,42 +4514,41 @@ static enum dbg_status qed_mcp_trace_get_data_info(struct qed_hwfn *p_hwfn,
 	signature = qed_rd(p_hwfn, p_ptt,
 			   *trace_data_grc_addr +
 			   offsetof(struct mcp_trace, signature));
+
 	if (signature != MFW_TRACE_SIGNATURE)
 		return DBG_STATUS_INVALID_TRACE_SIGNATURE;
 
 	/* Read trace size from MCP trace section */
-	*trace_data_size_bytes = qed_rd(p_hwfn,
-					p_ptt,
-					*trace_data_grc_addr +
-					offsetof(struct mcp_trace, size));
+	*trace_data_size = qed_rd(p_hwfn,
+				  p_ptt,
+				  *trace_data_grc_addr +
+				  offsetof(struct mcp_trace, size));
+
 	return DBG_STATUS_OK;
 }
 
-/* Reads MCP trace meta data image from NVRAM.
- * - running_bundle_id (OUT) - the running bundle ID (invalid when loaded from
- *	file)
- * - trace_meta_offset_bytes (OUT) - the NVRAM offset in bytes in which the MCP
- *	Trace meta data starts (invalid when loaded from file)
- * - trace_meta_size_bytes (OUT) - the size in bytes of the MCP Trace meta data
+/* Reads MCP trace meta data image from NVRAM
+ * - running_bundle_id (OUT): running bundle ID (invalid when loaded from file)
+ * - trace_meta_offset (OUT): trace meta offset in NVRAM in bytes (invalid when
+ *			      loaded from file).
+ * - trace_meta_size (OUT):   size in bytes of the trace meta data.
  */
 static enum dbg_status qed_mcp_trace_get_meta_info(struct qed_hwfn *p_hwfn,
 						   struct qed_ptt *p_ptt,
 						   u32 trace_data_size_bytes,
 						   u32 *running_bundle_id,
-						   u32 *trace_meta_offset_bytes,
-						   u32 *trace_meta_size_bytes)
+						   u32 *trace_meta_offset,
+						   u32 *trace_meta_size)
 {
+	u32 spad_trace_offsize, nvram_image_type, running_mfw_addr;
+
 	/* Read MCP trace section offsize structure from MCP scratchpad */
-	u32 spad_trace_offsize = qed_rd(p_hwfn,
-					p_ptt,
-					MCP_SPAD_TRACE_OFFSIZE_ADDR);
+	spad_trace_offsize = qed_rd(p_hwfn, p_ptt, MCP_SPAD_TRACE_OFFSIZE_ADDR);
 
 	/* Find running bundle ID */
-	u32 running_mfw_addr =
+	running_mfw_addr =
 		MCP_REG_SCRATCH + SECTION_OFFSET(spad_trace_offsize) +
 		QED_SECTION_SIZE(spad_trace_offsize) + trace_data_size_bytes;
-	u32 nvram_image_type;
-
 	*running_bundle_id = qed_rd(p_hwfn, p_ptt, running_mfw_addr);
 	if (*running_bundle_id > 1)
 		return DBG_STATUS_INVALID_NVRAM_BUNDLE;
@@ -4241,40 +4557,33 @@ static enum dbg_status qed_mcp_trace_get_meta_info(struct qed_hwfn *p_hwfn,
 	nvram_image_type =
 	    (*running_bundle_id ==
 	     DIR_ID_1) ? NVM_TYPE_MFW_TRACE1 : NVM_TYPE_MFW_TRACE2;
-
 	return qed_find_nvram_image(p_hwfn,
 				    p_ptt,
 				    nvram_image_type,
-				    trace_meta_offset_bytes,
-				    trace_meta_size_bytes);
+				    trace_meta_offset, trace_meta_size);
 }
 
-/* Reads the MCP Trace meta data (from NVRAM or buffer) into the specified
- * buffer.
- */
+/* Reads the MCP Trace meta data from NVRAM into the specified buffer */
 static enum dbg_status qed_mcp_trace_read_meta(struct qed_hwfn *p_hwfn,
 					       struct qed_ptt *p_ptt,
 					       u32 nvram_offset_in_bytes,
 					       u32 size_in_bytes, u32 *buf)
 {
-	u8 *byte_buf = (u8 *)buf;
-	u8 modules_num, i;
+	u8 modules_num, module_len, i, *byte_buf = (u8 *)buf;
+	enum dbg_status status;
 	u32 signature;
 
 	/* Read meta data from NVRAM */
-	enum dbg_status status = qed_nvram_read(p_hwfn,
-						p_ptt,
-						nvram_offset_in_bytes,
-						size_in_bytes,
-						buf);
-
+	status = qed_nvram_read(p_hwfn,
+				p_ptt,
+				nvram_offset_in_bytes, size_in_bytes, buf);
 	if (status != DBG_STATUS_OK)
 		return status;
 
 	/* Extract and check first signature */
 	signature = qed_read_unaligned_dword(byte_buf);
-	byte_buf += sizeof(u32);
-	if (signature != MCP_TRACE_META_IMAGE_SIGNATURE)
+	byte_buf += sizeof(signature);
+	if (signature != NVM_MAGIC_VALUE)
 		return DBG_STATUS_INVALID_TRACE_SIGNATURE;
 
 	/* Extract number of modules */
@@ -4282,16 +4591,16 @@ static enum dbg_status qed_mcp_trace_read_meta(struct qed_hwfn *p_hwfn,
 
 	/* Skip all modules */
 	for (i = 0; i < modules_num; i++) {
-		u8 module_len = *(byte_buf++);
-
+		module_len = *(byte_buf++);
 		byte_buf += module_len;
 	}
 
 	/* Extract and check second signature */
 	signature = qed_read_unaligned_dword(byte_buf);
-	byte_buf += sizeof(u32);
-	if (signature != MCP_TRACE_META_IMAGE_SIGNATURE)
+	byte_buf += sizeof(signature);
+	if (signature != NVM_MAGIC_VALUE)
 		return DBG_STATUS_INVALID_TRACE_SIGNATURE;
+
 	return DBG_STATUS_OK;
 }
 
@@ -4308,10 +4617,10 @@ static enum dbg_status qed_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 	bool mcp_access;
 	int halted = 0;
 
-	mcp_access = !qed_grc_get_param(p_hwfn, DBG_GRC_PARAM_NO_MCP);
-
 	*num_dumped_dwords = 0;
 
+	mcp_access = !qed_grc_get_param(p_hwfn, DBG_GRC_PARAM_NO_MCP);
+
 	/* Get trace data info */
 	status = qed_mcp_trace_get_data_info(p_hwfn,
 					     p_ptt,
@@ -4328,7 +4637,7 @@ static enum dbg_status qed_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 				     dump, "dump-type", "mcp-trace");
 
 	/* Halt MCP while reading from scratchpad so the read data will be
-	 * consistent if halt fails, MCP trace is taken anyway, with a small
+	 * consistent. if halt fails, MCP trace is taken anyway, with a small
 	 * risk that it may be corrupt.
 	 */
 	if (dump && mcp_access) {
@@ -4339,8 +4648,8 @@ static enum dbg_status qed_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 
 	/* Find trace data size */
 	trace_data_size_dwords =
-		DIV_ROUND_UP(trace_data_size_bytes + sizeof(struct mcp_trace),
-			     BYTES_IN_DWORD);
+	    DIV_ROUND_UP(trace_data_size_bytes + sizeof(struct mcp_trace),
+			 BYTES_IN_DWORD);
 
 	/* Dump trace data section header and param */
 	offset += qed_dump_section_hdr(dump_buf + offset,
@@ -4354,17 +4663,17 @@ static enum dbg_status qed_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 					  dump_buf + offset,
 					  dump,
 					  BYTES_TO_DWORDS(trace_data_grc_addr),
-					  trace_data_size_dwords);
+					  trace_data_size_dwords, false);
 
 	/* Resume MCP (only if halt succeeded) */
-	if (halted && qed_mcp_resume(p_hwfn, p_ptt) != 0)
+	if (halted && qed_mcp_resume(p_hwfn, p_ptt))
 		DP_NOTICE(p_hwfn, "Failed to resume MCP after halt!\n");
 
 	/* Dump trace meta section header */
 	offset += qed_dump_section_hdr(dump_buf + offset,
 				       dump, "mcp_trace_meta", 1);
 
-	/* Read trace meta info */
+	/* Read trace meta info (trace_meta_size_bytes is dword-aligned) */
 	if (mcp_access) {
 		status = qed_mcp_trace_get_meta_info(p_hwfn,
 						     p_ptt,
@@ -4391,6 +4700,9 @@ static enum dbg_status qed_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 	if (status == DBG_STATUS_OK)
 		offset += trace_meta_size_dwords;
 
+	/* Dump last section */
+	offset += qed_dump_last_section(p_hwfn, dump_buf, offset, dump);
+
 	*num_dumped_dwords = offset;
 
 	/* If no mcp access, indicate that the dump doesn't contain the meta
@@ -4405,7 +4717,7 @@ static enum dbg_status qed_reg_fifo_dump(struct qed_hwfn *p_hwfn,
 					 u32 *dump_buf,
 					 bool dump, u32 *num_dumped_dwords)
 {
-	u32 offset = 0, dwords_read, size_param_offset;
+	u32 dwords_read, size_param_offset, offset = 0;
 	bool fifo_has_data;
 
 	*num_dumped_dwords = 0;
@@ -4417,8 +4729,8 @@ static enum dbg_status qed_reg_fifo_dump(struct qed_hwfn *p_hwfn,
 	offset += qed_dump_str_param(dump_buf + offset,
 				     dump, "dump-type", "reg-fifo");
 
-	/* Dump fifo data section header and param. The size param is 0 for now,
-	 * and is overwritten after reading the FIFO.
+	/* Dump fifo data section header and param. The size param is 0 for
+	 * now, and is overwritten after reading the FIFO.
 	 */
 	offset += qed_dump_section_hdr(dump_buf + offset,
 				       dump, "reg_fifo_data", 1);
@@ -4430,8 +4742,7 @@ static enum dbg_status qed_reg_fifo_dump(struct qed_hwfn *p_hwfn,
 		 * test how much data is available, except for reading it.
 		 */
 		offset += REG_FIFO_DEPTH_DWORDS;
-		*num_dumped_dwords = offset;
-		return DBG_STATUS_OK;
+		goto out;
 	}
 
 	fifo_has_data = qed_rd(p_hwfn, p_ptt,
@@ -4456,8 +4767,12 @@ static enum dbg_status qed_reg_fifo_dump(struct qed_hwfn *p_hwfn,
 
 	qed_dump_num_param(dump_buf + size_param_offset, dump, "size",
 			   dwords_read);
+out:
+	/* Dump last section */
+	offset += qed_dump_last_section(p_hwfn, dump_buf, offset, dump);
 
 	*num_dumped_dwords = offset;
+
 	return DBG_STATUS_OK;
 }
 
@@ -4467,7 +4782,7 @@ static enum dbg_status qed_igu_fifo_dump(struct qed_hwfn *p_hwfn,
 					 u32 *dump_buf,
 					 bool dump, u32 *num_dumped_dwords)
 {
-	u32 offset = 0, dwords_read, size_param_offset;
+	u32 dwords_read, size_param_offset, offset = 0;
 	bool fifo_has_data;
 
 	*num_dumped_dwords = 0;
@@ -4479,8 +4794,8 @@ static enum dbg_status qed_igu_fifo_dump(struct qed_hwfn *p_hwfn,
 	offset += qed_dump_str_param(dump_buf + offset,
 				     dump, "dump-type", "igu-fifo");
 
-	/* Dump fifo data section header and param. The size param is 0 for now,
-	 * and is overwritten after reading the FIFO.
+	/* Dump fifo data section header and param. The size param is 0 for
+	 * now, and is overwritten after reading the FIFO.
 	 */
 	offset += qed_dump_section_hdr(dump_buf + offset,
 				       dump, "igu_fifo_data", 1);
@@ -4492,8 +4807,7 @@ static enum dbg_status qed_igu_fifo_dump(struct qed_hwfn *p_hwfn,
 		 * test how much data is available, except for reading it.
 		 */
 		offset += IGU_FIFO_DEPTH_DWORDS;
-		*num_dumped_dwords = offset;
-		return DBG_STATUS_OK;
+		goto out;
 	}
 
 	fifo_has_data = qed_rd(p_hwfn, p_ptt,
@@ -4519,8 +4833,12 @@ static enum dbg_status qed_igu_fifo_dump(struct qed_hwfn *p_hwfn,
 
 	qed_dump_num_param(dump_buf + size_param_offset, dump, "size",
 			   dwords_read);
+out:
+	/* Dump last section */
+	offset += qed_dump_last_section(p_hwfn, dump_buf, offset, dump);
 
 	*num_dumped_dwords = offset;
+
 	return DBG_STATUS_OK;
 }
 
@@ -4531,7 +4849,7 @@ static enum dbg_status qed_protection_override_dump(struct qed_hwfn *p_hwfn,
 						    bool dump,
 						    u32 *num_dumped_dwords)
 {
-	u32 offset = 0, size_param_offset, override_window_dwords;
+	u32 size_param_offset, override_window_dwords, offset = 0;
 
 	*num_dumped_dwords = 0;
 
@@ -4542,8 +4860,8 @@ static enum dbg_status qed_protection_override_dump(struct qed_hwfn *p_hwfn,
 	offset += qed_dump_str_param(dump_buf + offset,
 				     dump, "dump-type", "protection-override");
 
-	/* Dump data section header and param. The size param is 0 for now, and
-	 * is overwritten after reading the data.
+	/* Dump data section header and param. The size param is 0 for now,
+	 * and is overwritten after reading the data.
 	 */
 	offset += qed_dump_section_hdr(dump_buf + offset,
 				       dump, "protection_override_data", 1);
@@ -4552,8 +4870,7 @@ static enum dbg_status qed_protection_override_dump(struct qed_hwfn *p_hwfn,
 
 	if (!dump) {
 		offset += PROTECTION_OVERRIDE_DEPTH_DWORDS;
-		*num_dumped_dwords = offset;
-		return DBG_STATUS_OK;
+		goto out;
 	}
 
 	/* Add override window info to buffer */
@@ -4569,8 +4886,12 @@ static enum dbg_status qed_protection_override_dump(struct qed_hwfn *p_hwfn,
 	offset += override_window_dwords;
 	qed_dump_num_param(dump_buf + size_param_offset, dump, "size",
 			   override_window_dwords);
+out:
+	/* Dump last section */
+	offset += qed_dump_last_section(p_hwfn, dump_buf, offset, dump);
 
 	*num_dumped_dwords = offset;
+
 	return DBG_STATUS_OK;
 }
 
@@ -4593,11 +4914,14 @@ static u32 qed_fw_asserts_dump(struct qed_hwfn *p_hwfn,
 						dump_buf + offset, dump, 1);
 	offset += qed_dump_str_param(dump_buf + offset,
 				     dump, "dump-type", "fw-asserts");
+
+	/* Find Storm dump size */
 	for (storm_id = 0; storm_id < MAX_DBG_STORMS; storm_id++) {
 		u32 fw_asserts_section_addr, next_list_idx_addr, next_list_idx;
+		struct storm_defs *storm = &s_storm_defs[storm_id];
 		u32 last_list_idx, addr;
 
-		if (dev_data->block_in_reset[s_storm_defs[storm_id].block_id])
+		if (dev_data->block_in_reset[storm->block_id])
 			continue;
 
 		/* Read FW info for the current Storm */
@@ -4606,26 +4930,26 @@ static u32 qed_fw_asserts_dump(struct qed_hwfn *p_hwfn,
 		asserts = &fw_info.fw_asserts_section;
 
 		/* Dump FW Asserts section header and params */
-		storm_letter_str[0] = s_storm_defs[storm_id].letter;
-		offset += qed_dump_section_hdr(dump_buf + offset, dump,
-					       "fw_asserts", 2);
-		offset += qed_dump_str_param(dump_buf + offset, dump, "storm",
-					     storm_letter_str);
-		offset += qed_dump_num_param(dump_buf + offset, dump, "size",
+		storm_letter_str[0] = storm->letter;
+		offset += qed_dump_section_hdr(dump_buf + offset,
+					       dump, "fw_asserts", 2);
+		offset += qed_dump_str_param(dump_buf + offset,
+					     dump, "storm", storm_letter_str);
+		offset += qed_dump_num_param(dump_buf + offset,
+					     dump,
+					     "size",
 					     asserts->list_element_dword_size);
 
+		/* Read and dump FW Asserts data */
 		if (!dump) {
 			offset += asserts->list_element_dword_size;
 			continue;
 		}
 
-		/* Read and dump FW Asserts data */
-		fw_asserts_section_addr =
-			s_storm_defs[storm_id].sem_fast_mem_addr +
+		fw_asserts_section_addr = storm->sem_fast_mem_addr +
 			SEM_FAST_REG_INT_RAM +
 			RAM_LINES_TO_BYTES(asserts->section_ram_line_offset);
-		next_list_idx_addr =
-			fw_asserts_section_addr +
+		next_list_idx_addr = fw_asserts_section_addr +
 			DWORDS_TO_BYTES(asserts->list_next_index_dword_offset);
 		next_list_idx = qed_rd(p_hwfn, p_ptt, next_list_idx_addr);
 		last_list_idx = (next_list_idx > 0
@@ -4638,11 +4962,13 @@ static u32 qed_fw_asserts_dump(struct qed_hwfn *p_hwfn,
 		    qed_grc_dump_addr_range(p_hwfn, p_ptt,
 					    dump_buf + offset,
 					    dump, addr,
-					    asserts->list_element_dword_size);
+					    asserts->list_element_dword_size,
+					    false);
 	}
 
 	/* Dump last section */
-	offset += qed_dump_section_hdr(dump_buf + offset, dump, "last", 0);
+	offset += qed_dump_last_section(p_hwfn, dump_buf, offset, dump);
+
 	return offset;
 }
 
@@ -4650,10 +4976,10 @@ static u32 qed_fw_asserts_dump(struct qed_hwfn *p_hwfn,
 
 enum dbg_status qed_dbg_set_bin_ptr(const u8 * const bin_ptr)
 {
-	/* Convert binary data to debug arrays */
 	struct bin_buffer_hdr *buf_array = (struct bin_buffer_hdr *)bin_ptr;
 	u8 buf_id;
 
+	/* convert binary data to debug arrays */
 	for (buf_id = 0; buf_id < MAX_BIN_DBG_BUFFER_TYPE; buf_id++) {
 		s_dbg_arrays[buf_id].ptr =
 		    (u32 *)(bin_ptr + buf_array[buf_id].offset);
@@ -4682,14 +5008,17 @@ enum dbg_status qed_dbg_grc_get_dump_buf_size(struct qed_hwfn *p_hwfn,
 	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
 
 	*buf_size = 0;
+
 	if (status != DBG_STATUS_OK)
 		return status;
+
 	if (!s_dbg_arrays[BIN_BUF_DBG_MODE_TREE].ptr ||
 	    !s_dbg_arrays[BIN_BUF_DBG_DUMP_REG].ptr ||
 	    !s_dbg_arrays[BIN_BUF_DBG_DUMP_MEM].ptr ||
 	    !s_dbg_arrays[BIN_BUF_DBG_ATTN_BLOCKS].ptr ||
 	    !s_dbg_arrays[BIN_BUF_DBG_ATTN_REGS].ptr)
 		return DBG_STATUS_DBG_ARRAY_NOT_SET;
+
 	return qed_grc_dump(p_hwfn, p_ptt, NULL, false, buf_size);
 }
 
@@ -4702,12 +5031,14 @@ enum dbg_status qed_dbg_grc_dump(struct qed_hwfn *p_hwfn,
 	u32 needed_buf_size_in_dwords;
 	enum dbg_status status;
 
-	status = qed_dbg_grc_get_dump_buf_size(p_hwfn, p_ptt,
-					       &needed_buf_size_in_dwords);
-
 	*num_dumped_dwords = 0;
+
+	status = qed_dbg_grc_get_dump_buf_size(p_hwfn,
+					       p_ptt,
+					       &needed_buf_size_in_dwords);
 	if (status != DBG_STATUS_OK)
 		return status;
+
 	if (buf_size_in_dwords < needed_buf_size_in_dwords)
 		return DBG_STATUS_DUMP_BUF_TOO_SMALL;
 
@@ -4724,25 +5055,31 @@ enum dbg_status qed_dbg_idle_chk_get_dump_buf_size(struct qed_hwfn *p_hwfn,
 						   struct qed_ptt *p_ptt,
 						   u32 *buf_size)
 {
-	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
 	struct dbg_tools_data *dev_data = &p_hwfn->dbg_info;
+	struct idle_chk_data *idle_chk;
+	enum dbg_status status;
 
+	idle_chk = &dev_data->idle_chk;
 	*buf_size = 0;
+
+	status = qed_dbg_dev_init(p_hwfn, p_ptt);
 	if (status != DBG_STATUS_OK)
 		return status;
+
 	if (!s_dbg_arrays[BIN_BUF_DBG_MODE_TREE].ptr ||
 	    !s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_REGS].ptr ||
 	    !s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_IMMS].ptr ||
 	    !s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_RULES].ptr)
 		return DBG_STATUS_DBG_ARRAY_NOT_SET;
-	if (!dev_data->idle_chk.buf_size_set) {
-		dev_data->idle_chk.buf_size = qed_idle_chk_dump(p_hwfn,
-								p_ptt,
-								NULL, false);
-		dev_data->idle_chk.buf_size_set = true;
+
+	if (!idle_chk->buf_size_set) {
+		idle_chk->buf_size = qed_idle_chk_dump(p_hwfn,
+						       p_ptt, NULL, false);
+		idle_chk->buf_size_set = true;
 	}
 
-	*buf_size = dev_data->idle_chk.buf_size;
+	*buf_size = idle_chk->buf_size;
+
 	return DBG_STATUS_OK;
 }
 
@@ -4755,12 +5092,14 @@ enum dbg_status qed_dbg_idle_chk_dump(struct qed_hwfn *p_hwfn,
 	u32 needed_buf_size_in_dwords;
 	enum dbg_status status;
 
-	status = qed_dbg_idle_chk_get_dump_buf_size(p_hwfn, p_ptt,
-						    &needed_buf_size_in_dwords);
-
 	*num_dumped_dwords = 0;
+
+	status = qed_dbg_idle_chk_get_dump_buf_size(p_hwfn,
+						    p_ptt,
+						    &needed_buf_size_in_dwords);
 	if (status != DBG_STATUS_OK)
 		return status;
+
 	if (buf_size_in_dwords < needed_buf_size_in_dwords)
 		return DBG_STATUS_DUMP_BUF_TOO_SMALL;
 
@@ -4783,8 +5122,10 @@ enum dbg_status qed_dbg_mcp_trace_get_dump_buf_size(struct qed_hwfn *p_hwfn,
 	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
 
 	*buf_size = 0;
+
 	if (status != DBG_STATUS_OK)
 		return status;
+
 	return qed_mcp_trace_dump(p_hwfn, p_ptt, NULL, false, buf_size);
 }
 
@@ -4797,13 +5138,12 @@ enum dbg_status qed_dbg_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 	u32 needed_buf_size_in_dwords;
 	enum dbg_status status;
 
-	/* validate buffer size */
 	status =
-	    qed_dbg_mcp_trace_get_dump_buf_size(p_hwfn, p_ptt,
-						&needed_buf_size_in_dwords);
-
-	if (status != DBG_STATUS_OK &&
-	    status != DBG_STATUS_NVRAM_GET_IMAGE_FAILED)
+		qed_dbg_mcp_trace_get_dump_buf_size(p_hwfn,
+						    p_ptt,
+						    &needed_buf_size_in_dwords);
+	if (status != DBG_STATUS_OK && status !=
+	    DBG_STATUS_NVRAM_GET_IMAGE_FAILED)
 		return status;
 
 	if (buf_size_in_dwords < needed_buf_size_in_dwords)
@@ -4829,8 +5169,10 @@ enum dbg_status qed_dbg_reg_fifo_get_dump_buf_size(struct qed_hwfn *p_hwfn,
 	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
 
 	*buf_size = 0;
+
 	if (status != DBG_STATUS_OK)
 		return status;
+
 	return qed_reg_fifo_dump(p_hwfn, p_ptt, NULL, false, buf_size);
 }
 
@@ -4843,12 +5185,14 @@ enum dbg_status qed_dbg_reg_fifo_dump(struct qed_hwfn *p_hwfn,
 	u32 needed_buf_size_in_dwords;
 	enum dbg_status status;
 
-	status = qed_dbg_reg_fifo_get_dump_buf_size(p_hwfn, p_ptt,
-						    &needed_buf_size_in_dwords);
-
 	*num_dumped_dwords = 0;
+
+	status = qed_dbg_reg_fifo_get_dump_buf_size(p_hwfn,
+						    p_ptt,
+						    &needed_buf_size_in_dwords);
 	if (status != DBG_STATUS_OK)
 		return status;
+
 	if (buf_size_in_dwords < needed_buf_size_in_dwords)
 		return DBG_STATUS_DUMP_BUF_TOO_SMALL;
 
@@ -4871,8 +5215,10 @@ enum dbg_status qed_dbg_igu_fifo_get_dump_buf_size(struct qed_hwfn *p_hwfn,
 	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
 
 	*buf_size = 0;
+
 	if (status != DBG_STATUS_OK)
 		return status;
+
 	return qed_igu_fifo_dump(p_hwfn, p_ptt, NULL, false, buf_size);
 }
 
@@ -4885,12 +5231,14 @@ enum dbg_status qed_dbg_igu_fifo_dump(struct qed_hwfn *p_hwfn,
 	u32 needed_buf_size_in_dwords;
 	enum dbg_status status;
 
-	status = qed_dbg_igu_fifo_get_dump_buf_size(p_hwfn, p_ptt,
-						    &needed_buf_size_in_dwords);
-
 	*num_dumped_dwords = 0;
+
+	status = qed_dbg_igu_fifo_get_dump_buf_size(p_hwfn,
+						    p_ptt,
+						    &needed_buf_size_in_dwords);
 	if (status != DBG_STATUS_OK)
 		return status;
+
 	if (buf_size_in_dwords < needed_buf_size_in_dwords)
 		return DBG_STATUS_DUMP_BUF_TOO_SMALL;
 
@@ -4913,8 +5261,10 @@ qed_dbg_protection_override_get_dump_buf_size(struct qed_hwfn *p_hwfn,
 	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
 
 	*buf_size = 0;
+
 	if (status != DBG_STATUS_OK)
 		return status;
+
 	return qed_protection_override_dump(p_hwfn,
 					    p_ptt, NULL, false, buf_size);
 }
@@ -4925,15 +5275,18 @@ enum dbg_status qed_dbg_protection_override_dump(struct qed_hwfn *p_hwfn,
 						 u32 buf_size_in_dwords,
 						 u32 *num_dumped_dwords)
 {
-	u32 needed_buf_size_in_dwords;
+	u32 needed_buf_size_in_dwords, *p_size = &needed_buf_size_in_dwords;
 	enum dbg_status status;
 
-	status = qed_dbg_protection_override_get_dump_buf_size(p_hwfn, p_ptt,
-						&needed_buf_size_in_dwords);
-
 	*num_dumped_dwords = 0;
+
+	status =
+		qed_dbg_protection_override_get_dump_buf_size(p_hwfn,
+							      p_ptt,
+							      p_size);
 	if (status != DBG_STATUS_OK)
 		return status;
+
 	if (buf_size_in_dwords < needed_buf_size_in_dwords)
 		return DBG_STATUS_DUMP_BUF_TOO_SMALL;
 
@@ -4958,12 +5311,15 @@ enum dbg_status qed_dbg_fw_asserts_get_dump_buf_size(struct qed_hwfn *p_hwfn,
 	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
 
 	*buf_size = 0;
+
 	if (status != DBG_STATUS_OK)
 		return status;
 
 	/* Update reset state */
 	qed_update_blocks_reset_state(p_hwfn, p_ptt);
+
 	*buf_size = qed_fw_asserts_dump(p_hwfn, p_ptt, NULL, false);
+
 	return DBG_STATUS_OK;
 }
 
@@ -4973,24 +5329,108 @@ enum dbg_status qed_dbg_fw_asserts_dump(struct qed_hwfn *p_hwfn,
 					u32 buf_size_in_dwords,
 					u32 *num_dumped_dwords)
 {
-	u32 needed_buf_size_in_dwords;
+	u32 needed_buf_size_in_dwords, *p_size = &needed_buf_size_in_dwords;
 	enum dbg_status status;
 
-	status = qed_dbg_fw_asserts_get_dump_buf_size(p_hwfn, p_ptt,
-						&needed_buf_size_in_dwords);
-
 	*num_dumped_dwords = 0;
+
+	status =
+		qed_dbg_fw_asserts_get_dump_buf_size(p_hwfn,
+						     p_ptt,
+						     p_size);
 	if (status != DBG_STATUS_OK)
 		return status;
+
 	if (buf_size_in_dwords < needed_buf_size_in_dwords)
 		return DBG_STATUS_DUMP_BUF_TOO_SMALL;
 
 	*num_dumped_dwords = qed_fw_asserts_dump(p_hwfn, p_ptt, dump_buf, true);
+
+	/* Revert GRC params to their default */
+	qed_dbg_grc_set_params_default(p_hwfn);
+
+	return DBG_STATUS_OK;
+}
+
+enum dbg_status qed_dbg_read_attn(struct qed_hwfn *p_hwfn,
+				  struct qed_ptt *p_ptt,
+				  enum block_id block_id,
+				  enum dbg_attn_type attn_type,
+				  bool clear_status,
+				  struct dbg_attn_block_result *results)
+{
+	enum dbg_status status = qed_dbg_dev_init(p_hwfn, p_ptt);
+	u8 reg_idx, num_attn_regs, num_result_regs = 0;
+	const struct dbg_attn_reg *attn_reg_arr;
+
+	if (status != DBG_STATUS_OK)
+		return status;
+
+	if (!s_dbg_arrays[BIN_BUF_DBG_MODE_TREE].ptr ||
+	    !s_dbg_arrays[BIN_BUF_DBG_ATTN_BLOCKS].ptr ||
+	    !s_dbg_arrays[BIN_BUF_DBG_ATTN_REGS].ptr)
+		return DBG_STATUS_DBG_ARRAY_NOT_SET;
+
+	attn_reg_arr = qed_get_block_attn_regs(block_id,
+					       attn_type, &num_attn_regs);
+
+	for (reg_idx = 0; reg_idx < num_attn_regs; reg_idx++) {
+		const struct dbg_attn_reg *reg_data = &attn_reg_arr[reg_idx];
+		struct dbg_attn_reg_result *reg_result;
+		u32 sts_addr, sts_val;
+		u16 modes_buf_offset;
+		bool eval_mode;
+
+		/* Check mode */
+		eval_mode = GET_FIELD(reg_data->mode.data,
+				      DBG_MODE_HDR_EVAL_MODE) > 0;
+		modes_buf_offset = GET_FIELD(reg_data->mode.data,
+					     DBG_MODE_HDR_MODES_BUF_OFFSET);
+		if (eval_mode && !qed_is_mode_match(p_hwfn, &modes_buf_offset))
+			continue;
+
+		/* Mode match - read attention status register */
+		sts_addr = DWORDS_TO_BYTES(clear_status ?
+					   reg_data->sts_clr_address :
+					   GET_FIELD(reg_data->data,
+						     DBG_ATTN_REG_STS_ADDRESS));
+		sts_val = qed_rd(p_hwfn, p_ptt, sts_addr);
+		if (!sts_val)
+			continue;
+
+		/* Non-zero attention status - add to results */
+		reg_result = &results->reg_results[num_result_regs];
+		SET_FIELD(reg_result->data,
+			  DBG_ATTN_REG_RESULT_STS_ADDRESS, sts_addr);
+		SET_FIELD(reg_result->data,
+			  DBG_ATTN_REG_RESULT_NUM_REG_ATTN,
+			  GET_FIELD(reg_data->data, DBG_ATTN_REG_NUM_REG_ATTN));
+		reg_result->block_attn_offset = reg_data->block_attn_offset;
+		reg_result->sts_val = sts_val;
+		reg_result->mask_val = qed_rd(p_hwfn,
+					      p_ptt,
+					      DWORDS_TO_BYTES
+					      (reg_data->mask_address));
+		num_result_regs++;
+	}
+
+	results->block_id = (u8)block_id;
+	results->names_offset =
+	    qed_get_block_attn_data(block_id, attn_type)->names_offset;
+	SET_FIELD(results->data, DBG_ATTN_BLOCK_RESULT_ATTN_TYPE, attn_type);
+	SET_FIELD(results->data,
+		  DBG_ATTN_BLOCK_RESULT_NUM_REGS, num_result_regs);
+
 	return DBG_STATUS_OK;
 }
 
 /******************************* Data Types **********************************/
 
+struct block_info {
+	const char *name;
+	enum block_id id;
+};
+
 struct mcp_trace_format {
 	u32 data;
 #define MCP_TRACE_FORMAT_MODULE_MASK	0x0000ffff
@@ -5005,9 +5445,14 @@ struct mcp_trace_format {
 #define MCP_TRACE_FORMAT_P3_SIZE_SHIFT	22
 #define MCP_TRACE_FORMAT_LEN_MASK	0xff000000
 #define MCP_TRACE_FORMAT_LEN_SHIFT	24
+
 	char *format_str;
 };
 
+/* Meta data structure, generated by a perl script during MFW build. therefore,
+ * the structs mcp_trace_meta and mcp_trace_format are duplicated in the perl
+ * script.
+ */
 struct mcp_trace_meta {
 	u32 modules_num;
 	char **modules;
@@ -5015,7 +5460,7 @@ struct mcp_trace_meta {
 	struct mcp_trace_format *formats;
 };
 
-/* Reg fifo element */
+/* REG fifo element */
 struct reg_fifo_element {
 	u64 data;
 #define REG_FIFO_ELEMENT_ADDRESS_SHIFT		0
@@ -5140,12 +5585,15 @@ struct igu_fifo_addr_data {
 /******************************** Constants **********************************/
 
 #define MAX_MSG_LEN				1024
+
 #define MCP_TRACE_MAX_MODULE_LEN		8
 #define MCP_TRACE_FORMAT_MAX_PARAMS		3
 #define MCP_TRACE_FORMAT_PARAM_WIDTH \
 	(MCP_TRACE_FORMAT_P2_SIZE_SHIFT - MCP_TRACE_FORMAT_P1_SIZE_SHIFT)
+
 #define REG_FIFO_ELEMENT_ADDR_FACTOR		4
 #define REG_FIFO_ELEMENT_IS_PF_VF_VAL		127
+
 #define PROTECTION_OVERRIDE_ELEMENT_ADDR_FACTOR	4
 
 /********************************* Macros ************************************/
@@ -5154,59 +5602,269 @@ struct igu_fifo_addr_data {
 
 /***************************** Constant Arrays *******************************/
 
+struct user_dbg_array {
+	const u32 *ptr;
+	u32 size_in_dwords;
+};
+
+/* Debug arrays */
+static struct user_dbg_array
+s_user_dbg_arrays[MAX_BIN_DBG_BUFFER_TYPE] = { {NULL} };
+
+/* Block names array */
+static struct block_info s_block_info_arr[] = {
+	{"grc", BLOCK_GRC},
+	{"miscs", BLOCK_MISCS},
+	{"misc", BLOCK_MISC},
+	{"dbu", BLOCK_DBU},
+	{"pglue_b", BLOCK_PGLUE_B},
+	{"cnig", BLOCK_CNIG},
+	{"cpmu", BLOCK_CPMU},
+	{"ncsi", BLOCK_NCSI},
+	{"opte", BLOCK_OPTE},
+	{"bmb", BLOCK_BMB},
+	{"pcie", BLOCK_PCIE},
+	{"mcp", BLOCK_MCP},
+	{"mcp2", BLOCK_MCP2},
+	{"pswhst", BLOCK_PSWHST},
+	{"pswhst2", BLOCK_PSWHST2},
+	{"pswrd", BLOCK_PSWRD},
+	{"pswrd2", BLOCK_PSWRD2},
+	{"pswwr", BLOCK_PSWWR},
+	{"pswwr2", BLOCK_PSWWR2},
+	{"pswrq", BLOCK_PSWRQ},
+	{"pswrq2", BLOCK_PSWRQ2},
+	{"pglcs", BLOCK_PGLCS},
+	{"ptu", BLOCK_PTU},
+	{"dmae", BLOCK_DMAE},
+	{"tcm", BLOCK_TCM},
+	{"mcm", BLOCK_MCM},
+	{"ucm", BLOCK_UCM},
+	{"xcm", BLOCK_XCM},
+	{"ycm", BLOCK_YCM},
+	{"pcm", BLOCK_PCM},
+	{"qm", BLOCK_QM},
+	{"tm", BLOCK_TM},
+	{"dorq", BLOCK_DORQ},
+	{"brb", BLOCK_BRB},
+	{"src", BLOCK_SRC},
+	{"prs", BLOCK_PRS},
+	{"tsdm", BLOCK_TSDM},
+	{"msdm", BLOCK_MSDM},
+	{"usdm", BLOCK_USDM},
+	{"xsdm", BLOCK_XSDM},
+	{"ysdm", BLOCK_YSDM},
+	{"psdm", BLOCK_PSDM},
+	{"tsem", BLOCK_TSEM},
+	{"msem", BLOCK_MSEM},
+	{"usem", BLOCK_USEM},
+	{"xsem", BLOCK_XSEM},
+	{"ysem", BLOCK_YSEM},
+	{"psem", BLOCK_PSEM},
+	{"rss", BLOCK_RSS},
+	{"tmld", BLOCK_TMLD},
+	{"muld", BLOCK_MULD},
+	{"yuld", BLOCK_YULD},
+	{"xyld", BLOCK_XYLD},
+	{"ptld", BLOCK_PTLD},
+	{"ypld", BLOCK_YPLD},
+	{"prm", BLOCK_PRM},
+	{"pbf_pb1", BLOCK_PBF_PB1},
+	{"pbf_pb2", BLOCK_PBF_PB2},
+	{"rpb", BLOCK_RPB},
+	{"btb", BLOCK_BTB},
+	{"pbf", BLOCK_PBF},
+	{"rdif", BLOCK_RDIF},
+	{"tdif", BLOCK_TDIF},
+	{"cdu", BLOCK_CDU},
+	{"ccfc", BLOCK_CCFC},
+	{"tcfc", BLOCK_TCFC},
+	{"igu", BLOCK_IGU},
+	{"cau", BLOCK_CAU},
+	{"rgfs", BLOCK_RGFS},
+	{"rgsrc", BLOCK_RGSRC},
+	{"tgfs", BLOCK_TGFS},
+	{"tgsrc", BLOCK_TGSRC},
+	{"umac", BLOCK_UMAC},
+	{"xmac", BLOCK_XMAC},
+	{"dbg", BLOCK_DBG},
+	{"nig", BLOCK_NIG},
+	{"wol", BLOCK_WOL},
+	{"bmbn", BLOCK_BMBN},
+	{"ipc", BLOCK_IPC},
+	{"nwm", BLOCK_NWM},
+	{"nws", BLOCK_NWS},
+	{"ms", BLOCK_MS},
+	{"phy_pcie", BLOCK_PHY_PCIE},
+	{"led", BLOCK_LED},
+	{"avs_wrap", BLOCK_AVS_WRAP},
+	{"misc_aeu", BLOCK_MISC_AEU},
+	{"bar0_map", BLOCK_BAR0_MAP}
+};
+
 /* Status string array */
 static const char * const s_status_str[] = {
+	/* DBG_STATUS_OK */
 	"Operation completed successfully",
+
+	/* DBG_STATUS_APP_VERSION_NOT_SET */
 	"Debug application version wasn't set",
+
+	/* DBG_STATUS_UNSUPPORTED_APP_VERSION */
 	"Unsupported debug application version",
+
+	/* DBG_STATUS_DBG_BLOCK_NOT_RESET */
 	"The debug block wasn't reset since the last recording",
+
+	/* DBG_STATUS_INVALID_ARGS */
 	"Invalid arguments",
+
+	/* DBG_STATUS_OUTPUT_ALREADY_SET */
 	"The debug output was already set",
+
+	/* DBG_STATUS_INVALID_PCI_BUF_SIZE */
 	"Invalid PCI buffer size",
+
+	/* DBG_STATUS_PCI_BUF_ALLOC_FAILED */
 	"PCI buffer allocation failed",
+
+	/* DBG_STATUS_PCI_BUF_NOT_ALLOCATED */
 	"A PCI buffer wasn't allocated",
+
+	/* DBG_STATUS_TOO_MANY_INPUTS */
 	"Too many inputs were enabled. Enabled less inputs, or set 'unifyInputs' to true",
-	"GRC/Timestamp input overlap in cycle dword 0",
+
+	/* DBG_STATUS_INPUT_OVERLAP */
+	"Overlapping debug bus inputs",
+
+	/* DBG_STATUS_HW_ONLY_RECORDING */
 	"Cannot record Storm data since the entire recording cycle is used by HW",
+
+	/* DBG_STATUS_STORM_ALREADY_ENABLED */
 	"The Storm was already enabled",
+
+	/* DBG_STATUS_STORM_NOT_ENABLED */
 	"The specified Storm wasn't enabled",
+
+	/* DBG_STATUS_BLOCK_ALREADY_ENABLED */
 	"The block was already enabled",
+
+	/* DBG_STATUS_BLOCK_NOT_ENABLED */
 	"The specified block wasn't enabled",
+
+	/* DBG_STATUS_NO_INPUT_ENABLED */
 	"No input was enabled for recording",
+
+	/* DBG_STATUS_NO_FILTER_TRIGGER_64B */
 	"Filters and triggers are not allowed when recording in 64b units",
+
+	/* DBG_STATUS_FILTER_ALREADY_ENABLED */
 	"The filter was already enabled",
+
+	/* DBG_STATUS_TRIGGER_ALREADY_ENABLED */
 	"The trigger was already enabled",
+
+	/* DBG_STATUS_TRIGGER_NOT_ENABLED */
 	"The trigger wasn't enabled",
+
+	/* DBG_STATUS_CANT_ADD_CONSTRAINT */
 	"A constraint can be added only after a filter was enabled or a trigger state was added",
+
+	/* DBG_STATUS_TOO_MANY_TRIGGER_STATES */
 	"Cannot add more than 3 trigger states",
+
+	/* DBG_STATUS_TOO_MANY_CONSTRAINTS */
 	"Cannot add more than 4 constraints per filter or trigger state",
+
+	/* DBG_STATUS_RECORDING_NOT_STARTED */
 	"The recording wasn't started",
+
+	/* DBG_STATUS_DATA_DIDNT_TRIGGER */
 	"A trigger was configured, but it didn't trigger",
+
+	/* DBG_STATUS_NO_DATA_RECORDED */
 	"No data was recorded",
+
+	/* DBG_STATUS_DUMP_BUF_TOO_SMALL */
 	"Dump buffer is too small",
+
+	/* DBG_STATUS_DUMP_NOT_CHUNK_ALIGNED */
 	"Dumped data is not aligned to chunks",
+
+	/* DBG_STATUS_UNKNOWN_CHIP */
 	"Unknown chip",
+
+	/* DBG_STATUS_VIRT_MEM_ALLOC_FAILED */
 	"Failed allocating virtual memory",
+
+	/* DBG_STATUS_BLOCK_IN_RESET */
 	"The input block is in reset",
+
+	/* DBG_STATUS_INVALID_TRACE_SIGNATURE */
 	"Invalid MCP trace signature found in NVRAM",
+
+	/* DBG_STATUS_INVALID_NVRAM_BUNDLE */
 	"Invalid bundle ID found in NVRAM",
+
+	/* DBG_STATUS_NVRAM_GET_IMAGE_FAILED */
 	"Failed getting NVRAM image",
+
+	/* DBG_STATUS_NON_ALIGNED_NVRAM_IMAGE */
 	"NVRAM image is not dword-aligned",
+
+	/* DBG_STATUS_NVRAM_READ_FAILED */
 	"Failed reading from NVRAM",
+
+	/* DBG_STATUS_IDLE_CHK_PARSE_FAILED */
 	"Idle check parsing failed",
+
+	/* DBG_STATUS_MCP_TRACE_BAD_DATA */
 	"MCP Trace data is corrupt",
-	"Dump doesn't contain meta data - it must be provided in an image file",
+
+	/* DBG_STATUS_MCP_TRACE_NO_META */
+	"Dump doesn't contain meta data - it must be provided in image file",
+
+	/* DBG_STATUS_MCP_COULD_NOT_HALT */
 	"Failed to halt MCP",
+
+	/* DBG_STATUS_MCP_COULD_NOT_RESUME */
 	"Failed to resume MCP after halt",
+
+	/* DBG_STATUS_DMAE_FAILED */
 	"DMAE transaction failed",
+
+	/* DBG_STATUS_SEMI_FIFO_NOT_EMPTY */
 	"Failed to empty SEMI sync FIFO",
+
+	/* DBG_STATUS_IGU_FIFO_BAD_DATA */
 	"IGU FIFO data is corrupt",
+
+	/* DBG_STATUS_MCP_COULD_NOT_MASK_PRTY */
 	"MCP failed to mask parities",
+
+	/* DBG_STATUS_FW_ASSERTS_PARSE_FAILED */
 	"FW Asserts parsing failed",
+
+	/* DBG_STATUS_REG_FIFO_BAD_DATA */
 	"GRC FIFO data is corrupt",
+
+	/* DBG_STATUS_PROTECTION_OVERRIDE_BAD_DATA */
 	"Protection Override data is corrupt",
+
+	/* DBG_STATUS_DBG_ARRAY_NOT_SET */
 	"Debug arrays were not set (when using binary files, dbg_set_bin_ptr must be called)",
-	"When a block is filtered, no other blocks can be recorded unless inputs are unified (due to a HW bug)"
+
+	/* DBG_STATUS_FILTER_BUG */
+	"Debug Bus filtering requires the -unifyInputs option (due to a HW bug)",
+
+	/* DBG_STATUS_NON_MATCHING_LINES */
+	"Non-matching debug lines - all lines must be of the same type (either 128b or 256b)",
+
+	/* DBG_STATUS_INVALID_TRIGGER_DWORD_OFFSET */
+	"The selected trigger dword offset wasn't enabled in the recorded HW block",
+
+	/* DBG_STATUS_DBG_BUS_IN_USE */
+	"The debug bus is in use"
 };
 
 /* Idle check severity names array */
@@ -5223,12 +5881,13 @@ static const char * const s_mcp_trace_level_str[] = {
 	"DEBUG"
 };
 
-/* Parsing strings */
+/* Access type names array */
 static const char * const s_access_strs[] = {
 	"read",
 	"write"
 };
 
+/* Privilege type names array */
 static const char * const s_privilege_strs[] = {
 	"VF",
 	"PDA",
@@ -5236,6 +5895,7 @@ static const char * const s_privilege_strs[] = {
 	"UA"
 };
 
+/* Protection type names array */
 static const char * const s_protection_strs[] = {
 	"(default)",
 	"(default)",
@@ -5247,6 +5907,7 @@ static const char * const s_protection_strs[] = {
 	"override UA"
 };
 
+/* Master type names array */
 static const char * const s_master_strs[] = {
 	"???",
 	"pxp",
@@ -5266,6 +5927,7 @@ static const char * const s_master_strs[] = {
 	"???"
 };
 
+/* REG FIFO error messages array */
 static const char * const s_reg_fifo_error_strs[] = {
 	"grc timeout",
 	"address doesn't belong to any block",
@@ -5274,6 +5936,7 @@ static const char * const s_reg_fifo_error_strs[] = {
 	"path isolation error"
 };
 
+/* IGU FIFO sources array */
 static const char * const s_igu_fifo_source_strs[] = {
 	"TSTORM",
 	"MSTORM",
@@ -5288,6 +5951,7 @@ static const char * const s_igu_fifo_source_strs[] = {
 	"GRC",
 };
 
+/* IGU FIFO error messages */
 static const char * const s_igu_fifo_error_strs[] = {
 	"no error",
 	"length error",
@@ -5308,13 +5972,18 @@ static const char * const s_igu_fifo_error_strs[] = {
 
 /* IGU FIFO address data */
 static const struct igu_fifo_addr_data s_igu_fifo_addr_data[] = {
-	{0x0, 0x101, "MSI-X Memory", NULL, IGU_ADDR_TYPE_MSIX_MEM},
-	{0x102, 0x1ff, "reserved", NULL, IGU_ADDR_TYPE_RESERVED},
-	{0x200, 0x200, "Write PBA[0:63]", NULL, IGU_ADDR_TYPE_WRITE_PBA},
+	{0x0, 0x101, "MSI-X Memory", NULL,
+	 IGU_ADDR_TYPE_MSIX_MEM},
+	{0x102, 0x1ff, "reserved", NULL,
+	 IGU_ADDR_TYPE_RESERVED},
+	{0x200, 0x200, "Write PBA[0:63]", NULL,
+	 IGU_ADDR_TYPE_WRITE_PBA},
 	{0x201, 0x201, "Write PBA[64:127]", "reserved",
 	 IGU_ADDR_TYPE_WRITE_PBA},
-	{0x202, 0x202, "Write PBA[128]", "reserved", IGU_ADDR_TYPE_WRITE_PBA},
-	{0x203, 0x3ff, "reserved", NULL, IGU_ADDR_TYPE_RESERVED},
+	{0x202, 0x202, "Write PBA[128]", "reserved",
+	 IGU_ADDR_TYPE_WRITE_PBA},
+	{0x203, 0x3ff, "reserved", NULL,
+	 IGU_ADDR_TYPE_RESERVED},
 	{0x400, 0x5ef, "Write interrupt acknowledgment", NULL,
 	 IGU_ADDR_TYPE_WRITE_INT_ACK},
 	{0x5f0, 0x5f0, "Attention bits update", NULL,
@@ -5331,8 +6000,10 @@ static const struct igu_fifo_addr_data s_igu_fifo_addr_data[] = {
 	 IGU_ADDR_TYPE_READ_INT},
 	{0x5f6, 0x5f6, "Read interrupt 0:63 without mask", NULL,
 	 IGU_ADDR_TYPE_READ_INT},
-	{0x5f7, 0x5ff, "reserved", NULL, IGU_ADDR_TYPE_RESERVED},
-	{0x600, 0x7ff, "Producer update", NULL, IGU_ADDR_TYPE_WRITE_PROD_UPDATE}
+	{0x5f7, 0x5ff, "reserved", NULL,
+	 IGU_ADDR_TYPE_RESERVED},
+	{0x600, 0x7ff, "Producer update", NULL,
+	 IGU_ADDR_TYPE_WRITE_PROD_UPDATE}
 };
 
 /******************************** Variables **********************************/
@@ -5340,28 +6011,12 @@ static const struct igu_fifo_addr_data s_igu_fifo_addr_data[] = {
 /* MCP Trace meta data - used in case the dump doesn't contain the meta data
  * (e.g. due to no NVRAM access).
  */
-static struct dbg_array s_mcp_trace_meta = { NULL, 0 };
+static struct user_dbg_array s_mcp_trace_meta = { NULL, 0 };
 
 /* Temporary buffer, used for print size calculations */
 static char s_temp_buf[MAX_MSG_LEN];
 
-/***************************** Public Functions *******************************/
-
-enum dbg_status qed_dbg_user_set_bin_ptr(const u8 * const bin_ptr)
-{
-	/* Convert binary data to debug arrays */
-	struct bin_buffer_hdr *buf_array = (struct bin_buffer_hdr *)bin_ptr;
-	u8 buf_id;
-
-	for (buf_id = 0; buf_id < MAX_BIN_DBG_BUFFER_TYPE; buf_id++) {
-		s_dbg_arrays[buf_id].ptr =
-		    (u32 *)(bin_ptr + buf_array[buf_id].offset);
-		s_dbg_arrays[buf_id].size_in_dwords =
-		    BYTES_TO_DWORDS(buf_array[buf_id].length);
-	}
-
-	return DBG_STATUS_OK;
-}
+/**************************** Private Functions ******************************/
 
 static u32 qed_cyclic_add(u32 a, u32 b, u32 size)
 {
@@ -5381,10 +6036,8 @@ static u32 qed_read_from_cyclic_buf(void *buf,
 				    u32 *offset,
 				    u32 buf_size, u8 num_bytes_to_read)
 {
-	u8 *bytes_buf = (u8 *)buf;
-	u8 *val_ptr;
+	u8 i, *val_ptr, *bytes_buf = (u8 *)buf;
 	u32 val = 0;
-	u8 i;
 
 	val_ptr = (u8 *)&val;
 
@@ -5412,6 +6065,7 @@ static u32 qed_read_dword_from_buf(void *buf, u32 *offset)
 	u32 dword_val = *(u32 *)&((u8 *)buf)[*offset];
 
 	*offset += 4;
+
 	return dword_val;
 }
 
@@ -5445,7 +6099,7 @@ static u32 qed_read_param(u32 *dump_buf,
 			  const char **param_str_val, u32 *param_num_val)
 {
 	char *char_buf = (char *)dump_buf;
-	u32 offset = 0; /* In bytes */
+	size_t offset = 0;
 
 	/* Extract param name */
 	*param_name = char_buf;
@@ -5493,37 +6147,31 @@ static u32 qed_print_section_params(u32 *dump_buf,
 	u32 i, dump_offset = 0, results_offset = 0;
 
 	for (i = 0; i < num_section_params; i++) {
-		const char *param_name;
-		const char *param_str_val;
+		const char *param_name, *param_str_val;
 		u32 param_num_val = 0;
 
 		dump_offset += qed_read_param(dump_buf + dump_offset,
 					      &param_name,
 					      &param_str_val, &param_num_val);
+
 		if (param_str_val)
-			/* String param */
 			results_offset +=
 				sprintf(qed_get_buf_ptr(results_buf,
 							results_offset),
 					"%s: %s\n", param_name, param_str_val);
 		else if (strcmp(param_name, "fw-timestamp"))
-			/* Numeric param */
 			results_offset +=
 				sprintf(qed_get_buf_ptr(results_buf,
 							results_offset),
 					"%s: %d\n", param_name, param_num_val);
 	}
 
-	results_offset +=
-	    sprintf(qed_get_buf_ptr(results_buf, results_offset), "\n");
+	results_offset += sprintf(qed_get_buf_ptr(results_buf, results_offset),
+				  "\n");
+
 	*num_chars_printed = results_offset;
-	return dump_offset;
-}
 
-const char *qed_dbg_get_status_str(enum dbg_status status)
-{
-	return (status <
-		MAX_DBG_STATUS) ? s_status_str[status] : "Invalid debug status";
+	return dump_offset;
 }
 
 /* Parses the idle check rules and returns the number of characters printed.
@@ -5537,7 +6185,10 @@ static u32 qed_parse_idle_chk_dump_rules(struct qed_hwfn *p_hwfn,
 					 char *results_buf,
 					 u32 *num_errors, u32 *num_warnings)
 {
-	u32 rule_idx, results_offset = 0; /* Offset in results_buf in bytes */
+	/* Offset in results_buf in bytes */
+	u32 results_offset = 0;
+
+	u32 rule_idx;
 	u16 i, j;
 
 	*num_errors = 0;
@@ -5548,16 +6199,15 @@ static u32 qed_parse_idle_chk_dump_rules(struct qed_hwfn *p_hwfn,
 	     rule_idx++) {
 		const struct dbg_idle_chk_rule_parsing_data *rule_parsing_data;
 		struct dbg_idle_chk_result_hdr *hdr;
-		const char *parsing_str;
+		const char *parsing_str, *lsi_msg;
 		u32 parsing_str_offset;
-		const char *lsi_msg;
-		u8 curr_reg_id = 0;
 		bool has_fw_msg;
+		u8 curr_reg_id;
 
 		hdr = (struct dbg_idle_chk_result_hdr *)dump_buf;
 		rule_parsing_data =
 			(const struct dbg_idle_chk_rule_parsing_data *)
-			&s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_PARSING_DATA].
+			&s_user_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_PARSING_DATA].
 			ptr[hdr->rule_id];
 		parsing_str_offset =
 			GET_FIELD(rule_parsing_data->data,
@@ -5565,16 +6215,18 @@ static u32 qed_parse_idle_chk_dump_rules(struct qed_hwfn *p_hwfn,
 		has_fw_msg =
 			GET_FIELD(rule_parsing_data->data,
 				DBG_IDLE_CHK_RULE_PARSING_DATA_HAS_FW_MSG) > 0;
-		parsing_str = &((const char *)
-				s_dbg_arrays[BIN_BUF_DBG_PARSING_STRINGS].ptr)
-				[parsing_str_offset];
+		parsing_str =
+			&((const char *)
+			s_user_dbg_arrays[BIN_BUF_DBG_PARSING_STRINGS].ptr)
+			[parsing_str_offset];
 		lsi_msg = parsing_str;
+		curr_reg_id = 0;
 
 		if (hdr->severity >= MAX_DBG_IDLE_CHK_SEVERITY_TYPES)
 			return 0;
 
 		/* Skip rule header */
-		dump_buf += (sizeof(struct dbg_idle_chk_result_hdr) / 4);
+		dump_buf += BYTES_TO_DWORDS(sizeof(*hdr));
 
 		/* Update errors/warnings count */
 		if (hdr->severity == IDLE_CHK_SEVERITY_ERROR ||
@@ -5606,19 +6258,19 @@ static u32 qed_parse_idle_chk_dump_rules(struct qed_hwfn *p_hwfn,
 		for (i = 0;
 		     i < hdr->num_dumped_cond_regs + hdr->num_dumped_info_regs;
 		     i++) {
-			struct dbg_idle_chk_result_reg_hdr *reg_hdr
-			    = (struct dbg_idle_chk_result_reg_hdr *)
-			    dump_buf;
-			bool is_mem =
-				GET_FIELD(reg_hdr->data,
-					  DBG_IDLE_CHK_RESULT_REG_HDR_IS_MEM);
-			u8 reg_id =
-				GET_FIELD(reg_hdr->data,
-					  DBG_IDLE_CHK_RESULT_REG_HDR_REG_ID);
+			struct dbg_idle_chk_result_reg_hdr *reg_hdr;
+			bool is_mem;
+			u8 reg_id;
+
+			reg_hdr =
+				(struct dbg_idle_chk_result_reg_hdr *)dump_buf;
+			is_mem = GET_FIELD(reg_hdr->data,
+					   DBG_IDLE_CHK_RESULT_REG_HDR_IS_MEM);
+			reg_id = GET_FIELD(reg_hdr->data,
+					   DBG_IDLE_CHK_RESULT_REG_HDR_REG_ID);
 
 			/* Skip reg header */
-			dump_buf +=
-			    (sizeof(struct dbg_idle_chk_result_reg_hdr) / 4);
+			dump_buf += BYTES_TO_DWORDS(sizeof(*reg_hdr));
 
 			/* Skip register names until the required reg_id is
 			 * reached.
@@ -5660,6 +6312,7 @@ static u32 qed_parse_idle_chk_dump_rules(struct qed_hwfn *p_hwfn,
 	/* Check if end of dump buffer was exceeded */
 	if (dump_buf > dump_buf_end)
 		return 0;
+
 	return results_offset;
 }
 
@@ -5680,13 +6333,16 @@ static enum dbg_status qed_parse_idle_chk_dump(struct qed_hwfn *p_hwfn,
 	const char *section_name, *param_name, *param_str_val;
 	u32 *dump_buf_end = dump_buf + num_dumped_dwords;
 	u32 num_section_params = 0, num_rules;
-	u32 results_offset = 0;	/* Offset in results_buf in bytes */
+
+	/* Offset in results_buf in bytes */
+	u32 results_offset = 0;
 
 	*parsed_results_bytes = 0;
 	*num_errors = 0;
 	*num_warnings = 0;
-	if (!s_dbg_arrays[BIN_BUF_DBG_PARSING_STRINGS].ptr ||
-	    !s_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_PARSING_DATA].ptr)
+
+	if (!s_user_dbg_arrays[BIN_BUF_DBG_PARSING_STRINGS].ptr ||
+	    !s_user_dbg_arrays[BIN_BUF_DBG_IDLE_CHK_PARSING_DATA].ptr)
 		return DBG_STATUS_DBG_ARRAY_NOT_SET;
 
 	/* Read global_params section */
@@ -5705,10 +6361,9 @@ static enum dbg_status qed_parse_idle_chk_dump(struct qed_hwfn *p_hwfn,
 					 &section_name, &num_section_params);
 	if (strcmp(section_name, "idle_chk") || num_section_params != 1)
 		return DBG_STATUS_IDLE_CHK_PARSE_FAILED;
-
 	dump_buf += qed_read_param(dump_buf,
 				   &param_name, &param_str_val, &num_rules);
-	if (strcmp(param_name, "num_rules") != 0)
+	if (strcmp(param_name, "num_rules"))
 		return DBG_STATUS_IDLE_CHK_PARSE_FAILED;
 
 	if (num_rules) {
@@ -5728,7 +6383,7 @@ static enum dbg_status qed_parse_idle_chk_dump(struct qed_hwfn *p_hwfn,
 						      results_offset : NULL,
 						      num_errors, num_warnings);
 		results_offset += rules_print_size;
-		if (rules_print_size == 0)
+		if (!rules_print_size)
 			return DBG_STATUS_IDLE_CHK_PARSE_FAILED;
 
 		/* Print LSI output */
@@ -5745,64 +6400,33 @@ static enum dbg_status qed_parse_idle_chk_dump(struct qed_hwfn *p_hwfn,
 						      results_offset : NULL,
 						      num_errors, num_warnings);
 		results_offset += rules_print_size;
-		if (rules_print_size == 0)
+		if (!rules_print_size)
 			return DBG_STATUS_IDLE_CHK_PARSE_FAILED;
 	}
 
 	/* Print errors/warnings count */
-	if (*num_errors) {
+	if (*num_errors)
 		results_offset +=
 		    sprintf(qed_get_buf_ptr(results_buf,
 					    results_offset),
 			    "\nIdle Check failed!!! (with %d errors and %d warnings)\n",
 			    *num_errors, *num_warnings);
-	} else if (*num_warnings) {
+	else if (*num_warnings)
 		results_offset +=
 		    sprintf(qed_get_buf_ptr(results_buf,
 					    results_offset),
-			    "\nIdle Check completed successfuly (with %d warnings)\n",
+			    "\nIdle Check completed successfully (with %d warnings)\n",
 			    *num_warnings);
-	} else {
+	else
 		results_offset +=
 		    sprintf(qed_get_buf_ptr(results_buf,
 					    results_offset),
-			    "\nIdle Check completed successfuly\n");
-	}
+			    "\nIdle Check completed successfully\n");
 
 	/* Add 1 for string NULL termination */
 	*parsed_results_bytes = results_offset + 1;
-	return DBG_STATUS_OK;
-}
 
-enum dbg_status qed_get_idle_chk_results_buf_size(struct qed_hwfn *p_hwfn,
-						  u32 *dump_buf,
-						  u32 num_dumped_dwords,
-						  u32 *results_buf_size)
-{
-	u32 num_errors, num_warnings;
-
-	return qed_parse_idle_chk_dump(p_hwfn,
-				       dump_buf,
-				       num_dumped_dwords,
-				       NULL,
-				       results_buf_size,
-				       &num_errors, &num_warnings);
-}
-
-enum dbg_status qed_print_idle_chk_results(struct qed_hwfn *p_hwfn,
-					   u32 *dump_buf,
-					   u32 num_dumped_dwords,
-					   char *results_buf,
-					   u32 *num_errors, u32 *num_warnings)
-{
-	u32 parsed_buf_size;
-
-	return qed_parse_idle_chk_dump(p_hwfn,
-				       dump_buf,
-				       num_dumped_dwords,
-				       results_buf,
-				       &parsed_buf_size,
-				       num_errors, num_warnings);
+	return DBG_STATUS_OK;
 }
 
 /* Frees the specified MCP Trace meta data */
@@ -5841,12 +6465,10 @@ static enum dbg_status qed_mcp_trace_alloc_meta(struct qed_hwfn *p_hwfn,
 
 	/* Read first signature */
 	signature = qed_read_dword_from_buf(meta_buf_bytes, &offset);
-	if (signature != MCP_TRACE_META_IMAGE_SIGNATURE)
+	if (signature != NVM_MAGIC_VALUE)
 		return DBG_STATUS_INVALID_TRACE_SIGNATURE;
 
-	/* Read number of modules and allocate memory for all the modules
-	 * pointers.
-	 */
+	/* Read no. of modules and allocate memory for their pointers */
 	meta->modules_num = qed_read_byte_from_buf(meta_buf_bytes, &offset);
 	meta->modules = kzalloc(meta->modules_num * sizeof(char *), GFP_KERNEL);
 	if (!meta->modules)
@@ -5871,7 +6493,7 @@ static enum dbg_status qed_mcp_trace_alloc_meta(struct qed_hwfn *p_hwfn,
 
 	/* Read second signature */
 	signature = qed_read_dword_from_buf(meta_buf_bytes, &offset);
-	if (signature != MCP_TRACE_META_IMAGE_SIGNATURE)
+	if (signature != NVM_MAGIC_VALUE)
 		return DBG_STATUS_INVALID_TRACE_SIGNATURE;
 
 	/* Read number of formats and allocate memory for all formats */
@@ -5919,10 +6541,10 @@ static enum dbg_status qed_parse_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 						char *results_buf,
 						u32 *parsed_results_bytes)
 {
-	u32 results_offset = 0, param_mask, param_shift, param_num_val;
-	u32 num_section_params, offset, end_offset, bytes_left;
+	u32 end_offset, bytes_left, trace_data_dwords, trace_meta_dwords;
+	u32 param_mask, param_shift, param_num_val, num_section_params;
 	const char *section_name, *param_name, *param_str_val;
-	u32 trace_data_dwords, trace_meta_dwords;
+	u32 offset, results_offset = 0;
 	struct mcp_trace_meta meta;
 	struct mcp_trace *trace;
 	enum dbg_status status;
@@ -5955,7 +6577,7 @@ static enum dbg_status qed_parse_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 
 	/* Prepare trace info */
 	trace = (struct mcp_trace *)dump_buf;
-	trace_buf = (u8 *)dump_buf + sizeof(struct mcp_trace);
+	trace_buf = (u8 *)dump_buf + sizeof(*trace);
 	offset = trace->trace_oldest;
 	end_offset = trace->trace_prod;
 	bytes_left = qed_cyclic_sub(end_offset, offset, trace->size);
@@ -5968,7 +6590,7 @@ static enum dbg_status qed_parse_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 		return DBG_STATUS_MCP_TRACE_BAD_DATA;
 	dump_buf += qed_read_param(dump_buf,
 				   &param_name, &param_str_val, &param_num_val);
-	if (strcmp(param_name, "size") != 0)
+	if (strcmp(param_name, "size"))
 		return DBG_STATUS_MCP_TRACE_BAD_DATA;
 	trace_meta_dwords = param_num_val;
 
@@ -6028,6 +6650,7 @@ static enum dbg_status qed_parse_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 		}
 
 		format_ptr = &meta.formats[format_idx];
+
 		for (i = 0,
 		     param_mask = MCP_TRACE_FORMAT_P1_SIZE_MASK, param_shift =
 		     MCP_TRACE_FORMAT_P1_SIZE_SHIFT;
@@ -6050,6 +6673,7 @@ static enum dbg_status qed_parse_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 			 */
 			if (param_size == 3)
 				param_size = 4;
+
 			if (bytes_left < param_size) {
 				status = DBG_STATUS_MCP_TRACE_BAD_DATA;
 				goto free_mem;
@@ -6059,13 +6683,14 @@ static enum dbg_status qed_parse_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 							     &offset,
 							     trace->size,
 							     param_size);
+
 			bytes_left -= param_size;
 		}
 
 		format_level =
 		    (u8)((format_ptr->data &
 			  MCP_TRACE_FORMAT_LEVEL_MASK) >>
-			  MCP_TRACE_FORMAT_LEVEL_SHIFT);
+			 MCP_TRACE_FORMAT_LEVEL_SHIFT);
 		format_module =
 		    (u8)((format_ptr->data &
 			  MCP_TRACE_FORMAT_MODULE_MASK) >>
@@ -6094,30 +6719,6 @@ free_mem:
 	return status;
 }
 
-enum dbg_status qed_get_mcp_trace_results_buf_size(struct qed_hwfn *p_hwfn,
-						   u32 *dump_buf,
-						   u32 num_dumped_dwords,
-						   u32 *results_buf_size)
-{
-	return qed_parse_mcp_trace_dump(p_hwfn,
-					dump_buf,
-					num_dumped_dwords,
-					NULL, results_buf_size);
-}
-
-enum dbg_status qed_print_mcp_trace_results(struct qed_hwfn *p_hwfn,
-					    u32 *dump_buf,
-					    u32 num_dumped_dwords,
-					    char *results_buf)
-{
-	u32 parsed_buf_size;
-
-	return qed_parse_mcp_trace_dump(p_hwfn,
-					dump_buf,
-					num_dumped_dwords,
-					results_buf, &parsed_buf_size);
-}
-
 /* Parses a Reg FIFO dump buffer.
  * If result_buf is not NULL, the Reg FIFO results are printed to it.
  * In any case, the required results buffer size is assigned to
@@ -6130,10 +6731,11 @@ static enum dbg_status qed_parse_reg_fifo_dump(struct qed_hwfn *p_hwfn,
 					       char *results_buf,
 					       u32 *parsed_results_bytes)
 {
-	u32 results_offset = 0, param_num_val, num_section_params, num_elements;
 	const char *section_name, *param_name, *param_str_val;
+	u32 param_num_val, num_section_params, num_elements;
 	struct reg_fifo_element *elements;
 	u8 i, j, err_val, vf_val;
+	u32 results_offset = 0;
 	char vf_str[4];
 
 	/* Read global_params section */
@@ -6179,17 +6781,17 @@ static enum dbg_status qed_parse_reg_fifo_dump(struct qed_hwfn *p_hwfn,
 			    "raw: 0x%016llx, address: 0x%07x, access: %-5s, pf: %2d, vf: %s, port: %d, privilege: %-3s, protection: %-12s, master: %-4s, errors: ",
 			    elements[i].data,
 			    (u32)GET_FIELD(elements[i].data,
-				      REG_FIFO_ELEMENT_ADDRESS) *
-				      REG_FIFO_ELEMENT_ADDR_FACTOR,
-				      s_access_strs[GET_FIELD(elements[i].data,
+					   REG_FIFO_ELEMENT_ADDRESS) *
+			    REG_FIFO_ELEMENT_ADDR_FACTOR,
+			    s_access_strs[GET_FIELD(elements[i].data,
 						    REG_FIFO_ELEMENT_ACCESS)],
 			    (u32)GET_FIELD(elements[i].data,
-					   REG_FIFO_ELEMENT_PF), vf_str,
+					   REG_FIFO_ELEMENT_PF),
+			    vf_str,
 			    (u32)GET_FIELD(elements[i].data,
-				      REG_FIFO_ELEMENT_PORT),
-				      s_privilege_strs[GET_FIELD(elements[i].
-				      data,
-				      REG_FIFO_ELEMENT_PRIVILEGE)],
+					   REG_FIFO_ELEMENT_PORT),
+			    s_privilege_strs[GET_FIELD(elements[i].data,
+						REG_FIFO_ELEMENT_PRIVILEGE)],
 			    s_protection_strs[GET_FIELD(elements[i].data,
 						REG_FIFO_ELEMENT_PROTECTION)],
 			    s_master_strs[GET_FIELD(elements[i].data,
@@ -6201,18 +6803,18 @@ static enum dbg_status qed_parse_reg_fifo_dump(struct qed_hwfn *p_hwfn,
 					 REG_FIFO_ELEMENT_ERROR);
 		     j < ARRAY_SIZE(s_reg_fifo_error_strs);
 		     j++, err_val >>= 1) {
-			if (!(err_val & 0x1))
-				continue;
-			if (err_printed)
+			if (err_val & 0x1) {
+				if (err_printed)
+					results_offset +=
+					    sprintf(qed_get_buf_ptr
+						    (results_buf,
+						     results_offset), ", ");
 				results_offset +=
-					sprintf(qed_get_buf_ptr(results_buf,
-								results_offset),
-						", ");
-			results_offset +=
-				sprintf(qed_get_buf_ptr(results_buf,
-							results_offset), "%s",
-					s_reg_fifo_error_strs[j]);
-			err_printed = true;
+				    sprintf(qed_get_buf_ptr
+					    (results_buf, results_offset), "%s",
+					    s_reg_fifo_error_strs[j]);
+				err_printed = true;
+			}
 		}
 
 		results_offset +=
@@ -6225,31 +6827,140 @@ static enum dbg_status qed_parse_reg_fifo_dump(struct qed_hwfn *p_hwfn,
 
 	/* Add 1 for string NULL termination */
 	*parsed_results_bytes = results_offset + 1;
+
 	return DBG_STATUS_OK;
 }
 
-enum dbg_status qed_get_reg_fifo_results_buf_size(struct qed_hwfn *p_hwfn,
-						  u32 *dump_buf,
-						  u32 num_dumped_dwords,
-						  u32 *results_buf_size)
+static enum dbg_status qed_parse_igu_fifo_element(struct igu_fifo_element
+						  *element, char
+						  *results_buf,
+						  u32 *results_offset,
+						  u32 *parsed_results_bytes)
 {
-	return qed_parse_reg_fifo_dump(p_hwfn,
-				       dump_buf,
-				       num_dumped_dwords,
-				       NULL, results_buf_size);
-}
+	const struct igu_fifo_addr_data *found_addr = NULL;
+	u8 source, err_type, i, is_cleanup;
+	char parsed_addr_data[32];
+	char parsed_wr_data[256];
+	u32 wr_data, prod_cons;
+	bool is_wr_cmd, is_pf;
+	u16 cmd_addr;
+	u64 dword12;
 
-enum dbg_status qed_print_reg_fifo_results(struct qed_hwfn *p_hwfn,
-					   u32 *dump_buf,
-					   u32 num_dumped_dwords,
-					   char *results_buf)
-{
-	u32 parsed_buf_size;
+	/* Dword12 (dword index 1 and 2) contains bits 32..95 of the
+	 * FIFO element.
+	 */
+	dword12 = ((u64)element->dword2 << 32) | element->dword1;
+	is_wr_cmd = GET_FIELD(dword12, IGU_FIFO_ELEMENT_DWORD12_IS_WR_CMD);
+	is_pf = GET_FIELD(element->dword0, IGU_FIFO_ELEMENT_DWORD0_IS_PF);
+	cmd_addr = GET_FIELD(element->dword0, IGU_FIFO_ELEMENT_DWORD0_CMD_ADDR);
+	source = GET_FIELD(element->dword0, IGU_FIFO_ELEMENT_DWORD0_SOURCE);
+	err_type = GET_FIELD(element->dword0, IGU_FIFO_ELEMENT_DWORD0_ERR_TYPE);
+
+	if (source >= ARRAY_SIZE(s_igu_fifo_source_strs))
+		return DBG_STATUS_IGU_FIFO_BAD_DATA;
+	if (err_type >= ARRAY_SIZE(s_igu_fifo_error_strs))
+		return DBG_STATUS_IGU_FIFO_BAD_DATA;
 
-	return qed_parse_reg_fifo_dump(p_hwfn,
-				       dump_buf,
-				       num_dumped_dwords,
-				       results_buf, &parsed_buf_size);
+	/* Find address data */
+	for (i = 0; i < ARRAY_SIZE(s_igu_fifo_addr_data) && !found_addr; i++) {
+		const struct igu_fifo_addr_data *curr_addr =
+			&s_igu_fifo_addr_data[i];
+
+		if (cmd_addr >= curr_addr->start_addr && cmd_addr <=
+		    curr_addr->end_addr)
+			found_addr = curr_addr;
+	}
+
+	if (!found_addr)
+		return DBG_STATUS_IGU_FIFO_BAD_DATA;
+
+	/* Prepare parsed address data */
+	switch (found_addr->type) {
+	case IGU_ADDR_TYPE_MSIX_MEM:
+		sprintf(parsed_addr_data, " vector_num = 0x%x", cmd_addr / 2);
+		break;
+	case IGU_ADDR_TYPE_WRITE_INT_ACK:
+	case IGU_ADDR_TYPE_WRITE_PROD_UPDATE:
+		sprintf(parsed_addr_data,
+			" SB = 0x%x", cmd_addr - found_addr->start_addr);
+		break;
+	default:
+		parsed_addr_data[0] = '\0';
+	}
+
+	if (!is_wr_cmd) {
+		parsed_wr_data[0] = '\0';
+		goto out;
+	}
+
+	/* Prepare parsed write data */
+	wr_data = GET_FIELD(dword12, IGU_FIFO_ELEMENT_DWORD12_WR_DATA);
+	prod_cons = GET_FIELD(wr_data, IGU_FIFO_WR_DATA_PROD_CONS);
+	is_cleanup = GET_FIELD(wr_data, IGU_FIFO_WR_DATA_CMD_TYPE);
+
+	if (source == IGU_SRC_ATTN) {
+		sprintf(parsed_wr_data, "prod: 0x%x, ", prod_cons);
+	} else {
+		if (is_cleanup) {
+			u8 cleanup_val, cleanup_type;
+
+			cleanup_val =
+				GET_FIELD(wr_data,
+					  IGU_FIFO_CLEANUP_WR_DATA_CLEANUP_VAL);
+			cleanup_type =
+			    GET_FIELD(wr_data,
+				      IGU_FIFO_CLEANUP_WR_DATA_CLEANUP_TYPE);
+
+			sprintf(parsed_wr_data,
+				"cmd_type: cleanup, cleanup_val: %s, cleanup_type : %d, ",
+				cleanup_val ? "set" : "clear",
+				cleanup_type);
+		} else {
+			u8 update_flag, en_dis_int_for_sb, segment;
+			u8 timer_mask;
+
+			update_flag = GET_FIELD(wr_data,
+						IGU_FIFO_WR_DATA_UPDATE_FLAG);
+			en_dis_int_for_sb =
+				GET_FIELD(wr_data,
+					  IGU_FIFO_WR_DATA_EN_DIS_INT_FOR_SB);
+			segment = GET_FIELD(wr_data,
+					    IGU_FIFO_WR_DATA_SEGMENT);
+			timer_mask = GET_FIELD(wr_data,
+					       IGU_FIFO_WR_DATA_TIMER_MASK);
+
+			sprintf(parsed_wr_data,
+				"cmd_type: prod/cons update, prod/cons: 0x%x, update_flag: %s, en_dis_int_for_sb : %s, segment : %s, timer_mask = %d, ",
+				prod_cons,
+				update_flag ? "update" : "nop",
+				en_dis_int_for_sb
+				? (en_dis_int_for_sb == 1 ? "disable" : "nop")
+				: "enable",
+				segment ? "attn" : "regular",
+				timer_mask);
+		}
+	}
+out:
+	/* Add parsed element to parsed buffer */
+	*results_offset += sprintf(qed_get_buf_ptr(results_buf,
+						   *results_offset),
+				   "raw: 0x%01x%08x%08x, %s: %d, source : %s, type : %s, cmd_addr : 0x%x(%s%s), %serror: %s\n",
+				   element->dword2, element->dword1,
+				   element->dword0,
+				   is_pf ? "pf" : "vf",
+				   GET_FIELD(element->dword0,
+					     IGU_FIFO_ELEMENT_DWORD0_FID),
+				   s_igu_fifo_source_strs[source],
+				   is_wr_cmd ? "wr" : "rd",
+				   cmd_addr,
+				   (!is_pf && found_addr->vf_desc)
+				   ? found_addr->vf_desc
+				   : found_addr->desc,
+				   parsed_addr_data,
+				   parsed_wr_data,
+				   s_igu_fifo_error_strs[err_type]);
+
+	return DBG_STATUS_OK;
 }
 
 /* Parses an IGU FIFO dump buffer.
@@ -6264,12 +6975,12 @@ static enum dbg_status qed_parse_igu_fifo_dump(struct qed_hwfn *p_hwfn,
 					       char *results_buf,
 					       u32 *parsed_results_bytes)
 {
-	u32 results_offset = 0, param_num_val, num_section_params, num_elements;
 	const char *section_name, *param_name, *param_str_val;
+	u32 param_num_val, num_section_params, num_elements;
 	struct igu_fifo_element *elements;
-	char parsed_addr_data[32];
-	char parsed_wr_data[256];
-	u8 i, j;
+	enum dbg_status status;
+	u32 results_offset = 0;
+	u8 i;
 
 	/* Read global_params section */
 	dump_buf += qed_read_section_hdr(dump_buf,
@@ -6298,118 +7009,12 @@ static enum dbg_status qed_parse_igu_fifo_dump(struct qed_hwfn *p_hwfn,
 
 	/* Decode elements */
 	for (i = 0; i < num_elements; i++) {
-		/* dword12 (dword index 1 and 2) contains bits 32..95 of the
-		 * FIFO element.
-		 */
-		u64 dword12 =
-		    ((u64)elements[i].dword2 << 32) | elements[i].dword1;
-		bool is_wr_cmd = GET_FIELD(dword12,
-					   IGU_FIFO_ELEMENT_DWORD12_IS_WR_CMD);
-		bool is_pf = GET_FIELD(elements[i].dword0,
-				       IGU_FIFO_ELEMENT_DWORD0_IS_PF);
-		u16 cmd_addr = GET_FIELD(elements[i].dword0,
-					 IGU_FIFO_ELEMENT_DWORD0_CMD_ADDR);
-		u8 source = GET_FIELD(elements[i].dword0,
-				      IGU_FIFO_ELEMENT_DWORD0_SOURCE);
-		u8 err_type = GET_FIELD(elements[i].dword0,
-					IGU_FIFO_ELEMENT_DWORD0_ERR_TYPE);
-		const struct igu_fifo_addr_data *addr_data = NULL;
-
-		if (source >= ARRAY_SIZE(s_igu_fifo_source_strs))
-			return DBG_STATUS_IGU_FIFO_BAD_DATA;
-		if (err_type >= ARRAY_SIZE(s_igu_fifo_error_strs))
-			return DBG_STATUS_IGU_FIFO_BAD_DATA;
-
-		/* Find address data */
-		for (j = 0; j < ARRAY_SIZE(s_igu_fifo_addr_data) && !addr_data;
-		     j++)
-			if (cmd_addr >= s_igu_fifo_addr_data[j].start_addr &&
-			    cmd_addr <= s_igu_fifo_addr_data[j].end_addr)
-				addr_data = &s_igu_fifo_addr_data[j];
-		if (!addr_data)
-			return DBG_STATUS_IGU_FIFO_BAD_DATA;
-
-		/* Prepare parsed address data */
-		switch (addr_data->type) {
-		case IGU_ADDR_TYPE_MSIX_MEM:
-			sprintf(parsed_addr_data,
-				" vector_num=0x%x", cmd_addr / 2);
-			break;
-		case IGU_ADDR_TYPE_WRITE_INT_ACK:
-		case IGU_ADDR_TYPE_WRITE_PROD_UPDATE:
-			sprintf(parsed_addr_data,
-				" SB=0x%x", cmd_addr - addr_data->start_addr);
-			break;
-		default:
-			parsed_addr_data[0] = '\0';
-		}
-
-		/* Prepare parsed write data */
-		if (is_wr_cmd) {
-			u32 wr_data = GET_FIELD(dword12,
-					IGU_FIFO_ELEMENT_DWORD12_WR_DATA);
-			u32 prod_cons = GET_FIELD(wr_data,
-						  IGU_FIFO_WR_DATA_PROD_CONS);
-			u8 is_cleanup = GET_FIELD(wr_data,
-						  IGU_FIFO_WR_DATA_CMD_TYPE);
-
-			if (source == IGU_SRC_ATTN) {
-				sprintf(parsed_wr_data,
-					"prod: 0x%x, ", prod_cons);
-			} else {
-				if (is_cleanup) {
-					u8 cleanup_val = GET_FIELD(wr_data,
-								   IGU_FIFO_CLEANUP_WR_DATA_CLEANUP_VAL);
-					u8 cleanup_type = GET_FIELD(wr_data,
-								    IGU_FIFO_CLEANUP_WR_DATA_CLEANUP_TYPE);
-
-					sprintf(parsed_wr_data,
-						"cmd_type: cleanup, cleanup_val: %s, cleanup_type: %d, ",
-						cleanup_val ? "set" : "clear",
-						cleanup_type);
-				} else {
-					u8 update_flag = GET_FIELD(wr_data,
-								   IGU_FIFO_WR_DATA_UPDATE_FLAG);
-					u8 en_dis_int_for_sb =
-					    GET_FIELD(wr_data,
-						      IGU_FIFO_WR_DATA_EN_DIS_INT_FOR_SB);
-					u8 segment = GET_FIELD(wr_data,
-							       IGU_FIFO_WR_DATA_SEGMENT);
-					u8 timer_mask = GET_FIELD(wr_data,
-								  IGU_FIFO_WR_DATA_TIMER_MASK);
-
-					sprintf(parsed_wr_data,
-						"cmd_type: prod/cons update, prod/cons: 0x%x, update_flag: %s, en_dis_int_for_sb: %s, segment: %s, timer_mask=%d, ",
-						prod_cons,
-						update_flag ? "update" : "nop",
-						en_dis_int_for_sb
-						? (en_dis_int_for_sb ==
-						   1 ? "disable" : "nop") :
-						"enable",
-						segment ? "attn" : "regular",
-						timer_mask);
-				}
-			}
-		} else {
-			parsed_wr_data[0] = '\0';
-		}
-
-		/* Add parsed element to parsed buffer */
-		results_offset +=
-		    sprintf(qed_get_buf_ptr(results_buf,
-					    results_offset),
-			    "raw: 0x%01x%08x%08x, %s: %d, source: %s, type: %s, cmd_addr: 0x%x (%s%s), %serror: %s\n",
-			    elements[i].dword2, elements[i].dword1,
-			    elements[i].dword0,
-			    is_pf ? "pf" : "vf",
-			    GET_FIELD(elements[i].dword0,
-				      IGU_FIFO_ELEMENT_DWORD0_FID),
-			    s_igu_fifo_source_strs[source],
-			    is_wr_cmd ? "wr" : "rd", cmd_addr,
-			    (!is_pf && addr_data->vf_desc)
-			    ? addr_data->vf_desc : addr_data->desc,
-			    parsed_addr_data, parsed_wr_data,
-			    s_igu_fifo_error_strs[err_type]);
+		status = qed_parse_igu_fifo_element(&elements[i],
+						    results_buf,
+						    &results_offset,
+						    parsed_results_bytes);
+		if (status != DBG_STATUS_OK)
+			return status;
 	}
 
 	results_offset += sprintf(qed_get_buf_ptr(results_buf,
@@ -6418,31 +7023,8 @@ static enum dbg_status qed_parse_igu_fifo_dump(struct qed_hwfn *p_hwfn,
 
 	/* Add 1 for string NULL termination */
 	*parsed_results_bytes = results_offset + 1;
-	return DBG_STATUS_OK;
-}
-
-enum dbg_status qed_get_igu_fifo_results_buf_size(struct qed_hwfn *p_hwfn,
-						  u32 *dump_buf,
-						  u32 num_dumped_dwords,
-						  u32 *results_buf_size)
-{
-	return qed_parse_igu_fifo_dump(p_hwfn,
-				       dump_buf,
-				       num_dumped_dwords,
-				       NULL, results_buf_size);
-}
-
-enum dbg_status qed_print_igu_fifo_results(struct qed_hwfn *p_hwfn,
-					   u32 *dump_buf,
-					   u32 num_dumped_dwords,
-					   char *results_buf)
-{
-	u32 parsed_buf_size;
 
-	return qed_parse_igu_fifo_dump(p_hwfn,
-				       dump_buf,
-				       num_dumped_dwords,
-				       results_buf, &parsed_buf_size);
+	return DBG_STATUS_OK;
 }
 
 static enum dbg_status
@@ -6452,9 +7034,10 @@ qed_parse_protection_override_dump(struct qed_hwfn *p_hwfn,
 				   char *results_buf,
 				   u32 *parsed_results_bytes)
 {
-	u32 results_offset = 0, param_num_val, num_section_params, num_elements;
 	const char *section_name, *param_name, *param_str_val;
+	u32 param_num_val, num_section_params, num_elements;
 	struct protection_override_element *elements;
+	u32 results_offset = 0;
 	u8 i;
 
 	/* Read global_params section */
@@ -6477,7 +7060,7 @@ qed_parse_protection_override_dump(struct qed_hwfn *p_hwfn,
 				   &param_name, &param_str_val, &param_num_val);
 	if (strcmp(param_name, "size"))
 		return DBG_STATUS_PROTECTION_OVERRIDE_BAD_DATA;
-	if (param_num_val % PROTECTION_OVERRIDE_ELEMENT_DWORDS != 0)
+	if (param_num_val % PROTECTION_OVERRIDE_ELEMENT_DWORDS)
 		return DBG_STATUS_PROTECTION_OVERRIDE_BAD_DATA;
 	num_elements = param_num_val / PROTECTION_OVERRIDE_ELEMENT_DWORDS;
 	elements = (struct protection_override_element *)dump_buf;
@@ -6486,7 +7069,7 @@ qed_parse_protection_override_dump(struct qed_hwfn *p_hwfn,
 	for (i = 0; i < num_elements; i++) {
 		u32 address = GET_FIELD(elements[i].data,
 					PROTECTION_OVERRIDE_ELEMENT_ADDRESS) *
-					PROTECTION_OVERRIDE_ELEMENT_ADDR_FACTOR;
+			      PROTECTION_OVERRIDE_ELEMENT_ADDR_FACTOR;
 
 		results_offset +=
 		    sprintf(qed_get_buf_ptr(results_buf,
@@ -6512,33 +7095,8 @@ qed_parse_protection_override_dump(struct qed_hwfn *p_hwfn,
 
 	/* Add 1 for string NULL termination */
 	*parsed_results_bytes = results_offset + 1;
-	return DBG_STATUS_OK;
-}
 
-enum dbg_status
-qed_get_protection_override_results_buf_size(struct qed_hwfn *p_hwfn,
-					     u32 *dump_buf,
-					     u32 num_dumped_dwords,
-					     u32 *results_buf_size)
-{
-	return qed_parse_protection_override_dump(p_hwfn,
-						  dump_buf,
-						  num_dumped_dwords,
-						  NULL, results_buf_size);
-}
-
-enum dbg_status qed_print_protection_override_results(struct qed_hwfn *p_hwfn,
-						      u32 *dump_buf,
-						      u32 num_dumped_dwords,
-						      char *results_buf)
-{
-	u32 parsed_buf_size;
-
-	return qed_parse_protection_override_dump(p_hwfn,
-						  dump_buf,
-						  num_dumped_dwords,
-						  results_buf,
-						  &parsed_buf_size);
+	return DBG_STATUS_OK;
 }
 
 /* Parses a FW Asserts dump buffer.
@@ -6553,7 +7111,7 @@ static enum dbg_status qed_parse_fw_asserts_dump(struct qed_hwfn *p_hwfn,
 						 char *results_buf,
 						 u32 *parsed_results_bytes)
 {
-	u32 results_offset = 0, num_section_params, param_num_val, i;
+	u32 num_section_params, param_num_val, i, results_offset = 0;
 	const char *param_name, *param_str_val, *section_name;
 	bool last_section_found = false;
 
@@ -6569,54 +7127,216 @@ static enum dbg_status qed_parse_fw_asserts_dump(struct qed_hwfn *p_hwfn,
 	dump_buf += qed_print_section_params(dump_buf,
 					     num_section_params,
 					     results_buf, &results_offset);
-	while (!last_section_found) {
-		const char *storm_letter = NULL;
-		u32 storm_dump_size = 0;
 
+	while (!last_section_found) {
 		dump_buf += qed_read_section_hdr(dump_buf,
 						 &section_name,
 						 &num_section_params);
-		if (!strcmp(section_name, "last")) {
-			last_section_found = true;
-			continue;
-		} else if (strcmp(section_name, "fw_asserts")) {
-			return DBG_STATUS_FW_ASSERTS_PARSE_FAILED;
-		}
+		if (!strcmp(section_name, "fw_asserts")) {
+			/* Extract params */
+			const char *storm_letter = NULL;
+			u32 storm_dump_size = 0;
+
+			for (i = 0; i < num_section_params; i++) {
+				dump_buf += qed_read_param(dump_buf,
+							   &param_name,
+							   &param_str_val,
+							   &param_num_val);
+				if (!strcmp(param_name, "storm"))
+					storm_letter = param_str_val;
+				else if (!strcmp(param_name, "size"))
+					storm_dump_size = param_num_val;
+				else
+					return
+					    DBG_STATUS_FW_ASSERTS_PARSE_FAILED;
+			}
 
-		/* Extract params */
-		for (i = 0; i < num_section_params; i++) {
-			dump_buf += qed_read_param(dump_buf,
-						   &param_name,
-						   &param_str_val,
-						   &param_num_val);
-			if (!strcmp(param_name, "storm"))
-				storm_letter = param_str_val;
-			else if (!strcmp(param_name, "size"))
-				storm_dump_size = param_num_val;
-			else
+			if (!storm_letter || !storm_dump_size)
 				return DBG_STATUS_FW_ASSERTS_PARSE_FAILED;
-		}
 
-		if (!storm_letter || !storm_dump_size)
-			return DBG_STATUS_FW_ASSERTS_PARSE_FAILED;
-
-		/* Print data */
-		results_offset += sprintf(qed_get_buf_ptr(results_buf,
-							  results_offset),
-					  "\n%sSTORM_ASSERT: size=%d\n",
-					  storm_letter, storm_dump_size);
-		for (i = 0; i < storm_dump_size; i++, dump_buf++)
+			/* Print data */
 			results_offset +=
 			    sprintf(qed_get_buf_ptr(results_buf,
 						    results_offset),
-				    "%08x\n", *dump_buf);
+				    "\n%sSTORM_ASSERT: size=%d\n",
+				    storm_letter, storm_dump_size);
+			for (i = 0; i < storm_dump_size; i++, dump_buf++)
+				results_offset +=
+				    sprintf(qed_get_buf_ptr(results_buf,
+							    results_offset),
+					    "%08x\n", *dump_buf);
+		} else if (!strcmp(section_name, "last")) {
+			last_section_found = true;
+		} else {
+			return DBG_STATUS_FW_ASSERTS_PARSE_FAILED;
+		}
 	}
 
 	/* Add 1 for string NULL termination */
 	*parsed_results_bytes = results_offset + 1;
+
+	return DBG_STATUS_OK;
+}
+
+/***************************** Public Functions *******************************/
+
+enum dbg_status qed_dbg_user_set_bin_ptr(const u8 * const bin_ptr)
+{
+	struct bin_buffer_hdr *buf_array = (struct bin_buffer_hdr *)bin_ptr;
+	u8 buf_id;
+
+	/* Convert binary data to debug arrays */
+	for (buf_id = 0; buf_id < MAX_BIN_DBG_BUFFER_TYPE; buf_id++) {
+		s_user_dbg_arrays[buf_id].ptr =
+			(u32 *)(bin_ptr + buf_array[buf_id].offset);
+		s_user_dbg_arrays[buf_id].size_in_dwords =
+			BYTES_TO_DWORDS(buf_array[buf_id].length);
+	}
+
 	return DBG_STATUS_OK;
 }
 
+const char *qed_dbg_get_status_str(enum dbg_status status)
+{
+	return (status <
+		MAX_DBG_STATUS) ? s_status_str[status] : "Invalid debug status";
+}
+
+enum dbg_status qed_get_idle_chk_results_buf_size(struct qed_hwfn *p_hwfn,
+						  u32 *dump_buf,
+						  u32 num_dumped_dwords,
+						  u32 *results_buf_size)
+{
+	u32 num_errors, num_warnings;
+
+	return qed_parse_idle_chk_dump(p_hwfn,
+				       dump_buf,
+				       num_dumped_dwords,
+				       NULL,
+				       results_buf_size,
+				       &num_errors, &num_warnings);
+}
+
+enum dbg_status qed_print_idle_chk_results(struct qed_hwfn *p_hwfn,
+					   u32 *dump_buf,
+					   u32 num_dumped_dwords,
+					   char *results_buf,
+					   u32 *num_errors, u32 *num_warnings)
+{
+	u32 parsed_buf_size;
+
+	return qed_parse_idle_chk_dump(p_hwfn,
+				       dump_buf,
+				       num_dumped_dwords,
+				       results_buf,
+				       &parsed_buf_size,
+				       num_errors, num_warnings);
+}
+
+void qed_dbg_mcp_trace_set_meta_data(u32 *data, u32 size)
+{
+	s_mcp_trace_meta.ptr = data;
+	s_mcp_trace_meta.size_in_dwords = size;
+}
+
+enum dbg_status qed_get_mcp_trace_results_buf_size(struct qed_hwfn *p_hwfn,
+						   u32 *dump_buf,
+						   u32 num_dumped_dwords,
+						   u32 *results_buf_size)
+{
+	return qed_parse_mcp_trace_dump(p_hwfn,
+					dump_buf,
+					num_dumped_dwords,
+					NULL, results_buf_size);
+}
+
+enum dbg_status qed_print_mcp_trace_results(struct qed_hwfn *p_hwfn,
+					    u32 *dump_buf,
+					    u32 num_dumped_dwords,
+					    char *results_buf)
+{
+	u32 parsed_buf_size;
+
+	return qed_parse_mcp_trace_dump(p_hwfn,
+					dump_buf,
+					num_dumped_dwords,
+					results_buf, &parsed_buf_size);
+}
+
+enum dbg_status qed_get_reg_fifo_results_buf_size(struct qed_hwfn *p_hwfn,
+						  u32 *dump_buf,
+						  u32 num_dumped_dwords,
+						  u32 *results_buf_size)
+{
+	return qed_parse_reg_fifo_dump(p_hwfn,
+				       dump_buf,
+				       num_dumped_dwords,
+				       NULL, results_buf_size);
+}
+
+enum dbg_status qed_print_reg_fifo_results(struct qed_hwfn *p_hwfn,
+					   u32 *dump_buf,
+					   u32 num_dumped_dwords,
+					   char *results_buf)
+{
+	u32 parsed_buf_size;
+
+	return qed_parse_reg_fifo_dump(p_hwfn,
+				       dump_buf,
+				       num_dumped_dwords,
+				       results_buf, &parsed_buf_size);
+}
+
+enum dbg_status qed_get_igu_fifo_results_buf_size(struct qed_hwfn *p_hwfn,
+						  u32 *dump_buf,
+						  u32 num_dumped_dwords,
+						  u32 *results_buf_size)
+{
+	return qed_parse_igu_fifo_dump(p_hwfn,
+				       dump_buf,
+				       num_dumped_dwords,
+				       NULL, results_buf_size);
+}
+
+enum dbg_status qed_print_igu_fifo_results(struct qed_hwfn *p_hwfn,
+					   u32 *dump_buf,
+					   u32 num_dumped_dwords,
+					   char *results_buf)
+{
+	u32 parsed_buf_size;
+
+	return qed_parse_igu_fifo_dump(p_hwfn,
+				       dump_buf,
+				       num_dumped_dwords,
+				       results_buf, &parsed_buf_size);
+}
+
+enum dbg_status
+qed_get_protection_override_results_buf_size(struct qed_hwfn *p_hwfn,
+					     u32 *dump_buf,
+					     u32 num_dumped_dwords,
+					     u32 *results_buf_size)
+{
+	return qed_parse_protection_override_dump(p_hwfn,
+						  dump_buf,
+						  num_dumped_dwords,
+						  NULL, results_buf_size);
+}
+
+enum dbg_status qed_print_protection_override_results(struct qed_hwfn *p_hwfn,
+						      u32 *dump_buf,
+						      u32 num_dumped_dwords,
+						      char *results_buf)
+{
+	u32 parsed_buf_size;
+
+	return qed_parse_protection_override_dump(p_hwfn,
+						  dump_buf,
+						  num_dumped_dwords,
+						  results_buf,
+						  &parsed_buf_size);
+}
+
 enum dbg_status qed_get_fw_asserts_results_buf_size(struct qed_hwfn *p_hwfn,
 						    u32 *dump_buf,
 						    u32 num_dumped_dwords,
@@ -6641,6 +7361,88 @@ enum dbg_status qed_print_fw_asserts_results(struct qed_hwfn *p_hwfn,
 					 results_buf, &parsed_buf_size);
 }
 
+enum dbg_status qed_dbg_parse_attn(struct qed_hwfn *p_hwfn,
+				   struct dbg_attn_block_result *results)
+{
+	struct user_dbg_array *block_attn, *pstrings;
+	const u32 *block_attn_name_offsets;
+	enum dbg_attn_type attn_type;
+	const char *block_name;
+	u8 num_regs, i, j;
+
+	num_regs = GET_FIELD(results->data, DBG_ATTN_BLOCK_RESULT_NUM_REGS);
+	attn_type = (enum dbg_attn_type)
+		    GET_FIELD(results->data,
+			      DBG_ATTN_BLOCK_RESULT_ATTN_TYPE);
+	block_name = s_block_info_arr[results->block_id].name;
+
+	if (!s_user_dbg_arrays[BIN_BUF_DBG_ATTN_INDEXES].ptr ||
+	    !s_user_dbg_arrays[BIN_BUF_DBG_ATTN_NAME_OFFSETS].ptr ||
+	    !s_user_dbg_arrays[BIN_BUF_DBG_PARSING_STRINGS].ptr)
+		return DBG_STATUS_DBG_ARRAY_NOT_SET;
+
+	block_attn = &s_user_dbg_arrays[BIN_BUF_DBG_ATTN_NAME_OFFSETS];
+	block_attn_name_offsets = &block_attn->ptr[results->names_offset];
+
+	/* Go over registers with a non-zero attention status */
+	for (i = 0; i < num_regs; i++) {
+		struct dbg_attn_reg_result *reg_result;
+		struct dbg_attn_bit_mapping *mapping;
+		u8 num_reg_attn, bit_idx = 0;
+
+		reg_result = &results->reg_results[i];
+		num_reg_attn = GET_FIELD(reg_result->data,
+					 DBG_ATTN_REG_RESULT_NUM_REG_ATTN);
+		block_attn = &s_user_dbg_arrays[BIN_BUF_DBG_ATTN_INDEXES];
+		mapping = &((struct dbg_attn_bit_mapping *)
+			    block_attn->ptr)[reg_result->block_attn_offset];
+
+		pstrings = &s_user_dbg_arrays[BIN_BUF_DBG_PARSING_STRINGS];
+
+		/* Go over attention status bits */
+		for (j = 0; j < num_reg_attn; j++) {
+			u16 attn_idx_val = GET_FIELD(mapping[j].data,
+						     DBG_ATTN_BIT_MAPPING_VAL);
+			const char *attn_name, *attn_type_str, *masked_str;
+			u32 name_offset, sts_addr;
+
+			/* Check if bit mask should be advanced (due to unused
+			 * bits).
+			 */
+			if (GET_FIELD(mapping[j].data,
+				      DBG_ATTN_BIT_MAPPING_IS_UNUSED_BIT_CNT)) {
+				bit_idx += (u8)attn_idx_val;
+				continue;
+			}
+
+			/* Check current bit index */
+			if (!(reg_result->sts_val & BIT(bit_idx))) {
+				bit_idx++;
+				continue;
+			}
+
+			/* Find attention name */
+			name_offset = block_attn_name_offsets[attn_idx_val];
+			attn_name = &((const char *)
+				      pstrings->ptr)[name_offset];
+			attn_type_str = attn_type == ATTN_TYPE_INTERRUPT ?
+					"Interrupt" : "Parity";
+			masked_str = reg_result->mask_val & BIT(bit_idx) ?
+				     " [masked]" : "";
+			sts_addr = GET_FIELD(reg_result->data,
+					     DBG_ATTN_REG_RESULT_STS_ADDRESS);
+			DP_NOTICE(p_hwfn,
+				  "%s (%s) : %s [address 0x%08x, bit %d]%s\n",
+				  block_name, attn_type_str, attn_name,
+				  sts_addr, bit_idx, masked_str);
+
+			bit_idx++;
+		}
+	}
+
+	return DBG_STATUS_OK;
+}
+
 /* Wrapper for unifying the idle_chk and mcp_trace api */
 static enum dbg_status
 qed_print_idle_chk_results_wrapper(struct qed_hwfn *p_hwfn,
diff --git a/drivers/net/ethernet/qlogic/qed/qed_debug.h b/drivers/net/ethernet/qlogic/qed/qed_debug.h
index f872d7324814..ea1cc8eaa125 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_debug.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_debug.h
@@ -20,6 +20,9 @@ enum qed_dbg_features {
 	DBG_FEATURE_NUM
 };
 
+/* Forward Declaration */
+struct qed_dev;
+
 int qed_dbg_grc(struct qed_dev *cdev, void *buffer, u32 *num_dumped_bytes);
 int qed_dbg_grc_size(struct qed_dev *cdev);
 int qed_dbg_idle_chk(struct qed_dev *cdev, void *buffer,
diff --git a/drivers/net/ethernet/qlogic/qed/qed_dev.c b/drivers/net/ethernet/qlogic/qed/qed_dev.c
index 463927f17032..6c87bed13bd2 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_dev.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_dev.c
@@ -62,19 +62,13 @@
 #include "qed_sp.h"
 #include "qed_sriov.h"
 #include "qed_vf.h"
-#include "qed_roce.h"
+#include "qed_rdma.h"
 
 static DEFINE_SPINLOCK(qm_lock);
 
 #define QED_MIN_DPIS            (4)
 #define QED_MIN_PWM_REGION      (QED_WID_SIZE * QED_MIN_DPIS)
 
-/* API common to all protocols */
-enum BAR_ID {
-	BAR_ID_0,       /* used for GRC */
-	BAR_ID_1        /* Used for doorbells */
-};
-
 static u32 qed_hw_bar_size(struct qed_hwfn *p_hwfn,
 			   struct qed_ptt *p_ptt, enum BAR_ID bar_id)
 {
@@ -83,7 +77,7 @@ static u32 qed_hw_bar_size(struct qed_hwfn *p_hwfn,
 	u32 val;
 
 	if (IS_VF(p_hwfn->cdev))
-		return 1 << 17;
+		return qed_vf_hw_bar_size(p_hwfn, bar_id);
 
 	val = qed_rd(p_hwfn, p_ptt, bar_reg);
 	if (val)
@@ -154,13 +148,17 @@ void qed_resc_free(struct qed_dev *cdev)
 {
 	int i;
 
-	if (IS_VF(cdev))
+	if (IS_VF(cdev)) {
+		for_each_hwfn(cdev, i)
+			qed_l2_free(&cdev->hwfns[i]);
 		return;
+	}
 
 	kfree(cdev->fw_data);
 	cdev->fw_data = NULL;
 
 	kfree(cdev->reset_stats);
+	cdev->reset_stats = NULL;
 
 	for_each_hwfn(cdev, i) {
 		struct qed_hwfn *p_hwfn = &cdev->hwfns[i];
@@ -168,20 +166,21 @@ void qed_resc_free(struct qed_dev *cdev)
 		qed_cxt_mngr_free(p_hwfn);
 		qed_qm_info_free(p_hwfn);
 		qed_spq_free(p_hwfn);
-		qed_eq_free(p_hwfn, p_hwfn->p_eq);
-		qed_consq_free(p_hwfn, p_hwfn->p_consq);
+		qed_eq_free(p_hwfn);
+		qed_consq_free(p_hwfn);
 		qed_int_free(p_hwfn);
 #ifdef CONFIG_QED_LL2
-		qed_ll2_free(p_hwfn, p_hwfn->p_ll2_info);
+		qed_ll2_free(p_hwfn);
 #endif
 		if (p_hwfn->hw_info.personality == QED_PCI_FCOE)
-			qed_fcoe_free(p_hwfn, p_hwfn->p_fcoe_info);
+			qed_fcoe_free(p_hwfn);
 
 		if (p_hwfn->hw_info.personality == QED_PCI_ISCSI) {
-			qed_iscsi_free(p_hwfn, p_hwfn->p_iscsi_info);
-			qed_ooo_free(p_hwfn, p_hwfn->p_ooo_info);
+			qed_iscsi_free(p_hwfn);
+			qed_ooo_free(p_hwfn);
 		}
 		qed_iov_free(p_hwfn);
+		qed_l2_free(p_hwfn);
 		qed_dmae_info_free(p_hwfn);
 		qed_dcbx_info_free(p_hwfn);
 	}
@@ -217,6 +216,10 @@ static u32 qed_get_pq_flags(struct qed_hwfn *p_hwfn)
 	case QED_PCI_ETH_ROCE:
 		flags |= PQ_FLAGS_MCOS | PQ_FLAGS_OFLD | PQ_FLAGS_LLT;
 		break;
+	case QED_PCI_ETH_IWARP:
+		flags |= PQ_FLAGS_MCOS | PQ_FLAGS_ACK | PQ_FLAGS_OOO |
+		    PQ_FLAGS_OFLD;
+		break;
 	default:
 		DP_ERR(p_hwfn,
 		       "unknown personality %d\n", p_hwfn->hw_info.personality);
@@ -299,7 +302,7 @@ static void qed_init_qm_params(struct qed_hwfn *p_hwfn)
 	qm_info->vport_wfq_en = 1;
 
 	/* TC config is different for AH 4 port */
-	four_port = p_hwfn->cdev->num_ports_in_engines == MAX_NUM_PORTS_K2;
+	four_port = p_hwfn->cdev->num_ports_in_engine == MAX_NUM_PORTS_K2;
 
 	/* in AH 4 port we have fewer TCs per port */
 	qm_info->max_phys_tcs_per_port = four_port ? NUM_PHYS_TCS_4PORT_K2 :
@@ -328,7 +331,7 @@ static void qed_init_qm_vport_params(struct qed_hwfn *p_hwfn)
 static void qed_init_qm_port_params(struct qed_hwfn *p_hwfn)
 {
 	/* Initialize qm port parameters */
-	u8 i, active_phys_tcs, num_ports = p_hwfn->cdev->num_ports_in_engines;
+	u8 i, active_phys_tcs, num_ports = p_hwfn->cdev->num_ports_in_engine;
 
 	/* indicate how ooo and high pri traffic is dealt with */
 	active_phys_tcs = num_ports == MAX_NUM_PORTS_K2 ?
@@ -692,7 +695,7 @@ static void qed_dp_init_qm_params(struct qed_hwfn *p_hwfn)
 		   qm_info->num_pf_rls, qed_get_pq_flags(p_hwfn));
 
 	/* port table */
-	for (i = 0; i < p_hwfn->cdev->num_ports_in_engines; i++) {
+	for (i = 0; i < p_hwfn->cdev->num_ports_in_engine; i++) {
 		port = &(qm_info->qm_port_params[i]);
 		DP_VERBOSE(p_hwfn,
 			   NETIF_MSG_HW,
@@ -822,7 +825,7 @@ static int qed_alloc_qm_data(struct qed_hwfn *p_hwfn)
 		goto alloc_err;
 
 	qm_info->qm_port_params = kzalloc(sizeof(*qm_info->qm_port_params) *
-					  p_hwfn->cdev->num_ports_in_engines,
+					  p_hwfn->cdev->num_ports_in_engine,
 					  GFP_KERNEL);
 	if (!qm_info->qm_port_params)
 		goto alloc_err;
@@ -843,20 +846,18 @@ alloc_err:
 
 int qed_resc_alloc(struct qed_dev *cdev)
 {
-	struct qed_iscsi_info *p_iscsi_info;
-	struct qed_fcoe_info *p_fcoe_info;
-	struct qed_ooo_info *p_ooo_info;
-#ifdef CONFIG_QED_LL2
-	struct qed_ll2_info *p_ll2_info;
-#endif
 	u32 rdma_tasks, excess_tasks;
-	struct qed_consq *p_consq;
-	struct qed_eq *p_eq;
 	u32 line_count;
 	int i, rc = 0;
 
-	if (IS_VF(cdev))
+	if (IS_VF(cdev)) {
+		for_each_hwfn(cdev, i) {
+			rc = qed_l2_alloc(&cdev->hwfns[i]);
+			if (rc)
+				return rc;
+		}
 		return rc;
+	}
 
 	cdev->fw_data = kzalloc(sizeof(*cdev->fw_data), GFP_KERNEL);
 	if (!cdev->fw_data)
@@ -939,9 +940,16 @@ int qed_resc_alloc(struct qed_dev *cdev)
 
 		/* EQ */
 		n_eqes = qed_chain_get_capacity(&p_hwfn->p_spq->chain);
-		if (p_hwfn->hw_info.personality == QED_PCI_ETH_ROCE) {
+		if (QED_IS_RDMA_PERSONALITY(p_hwfn)) {
+			enum protocol_type rdma_proto;
+
+			if (QED_IS_ROCE_PERSONALITY(p_hwfn))
+				rdma_proto = PROTOCOLID_ROCE;
+			else
+				rdma_proto = PROTOCOLID_IWARP;
+
 			num_cons = qed_cxt_get_proto_cid_count(p_hwfn,
-							       PROTOCOLID_ROCE,
+							       rdma_proto,
 							       NULL) * 2;
 			n_eqes += num_cons + 2 * MAX_NUM_VFS_BB;
 		} else if (p_hwfn->hw_info.personality == QED_PCI_ISCSI) {
@@ -956,45 +964,42 @@ int qed_resc_alloc(struct qed_dev *cdev)
 			DP_ERR(p_hwfn,
 			       "Cannot allocate 0x%x EQ elements. The maximum of a u16 chain is 0x%x\n",
 			       n_eqes, 0xFFFF);
-			rc = -EINVAL;
-			goto alloc_err;
+			goto alloc_no_mem;
 		}
 
-		p_eq = qed_eq_alloc(p_hwfn, (u16) n_eqes);
-		if (!p_eq)
-			goto alloc_no_mem;
-		p_hwfn->p_eq = p_eq;
+		rc = qed_eq_alloc(p_hwfn, (u16) n_eqes);
+		if (rc)
+			goto alloc_err;
 
-		p_consq = qed_consq_alloc(p_hwfn);
-		if (!p_consq)
-			goto alloc_no_mem;
-		p_hwfn->p_consq = p_consq;
+		rc = qed_consq_alloc(p_hwfn);
+		if (rc)
+			goto alloc_err;
+
+		rc = qed_l2_alloc(p_hwfn);
+		if (rc)
+			goto alloc_err;
 
 #ifdef CONFIG_QED_LL2
 		if (p_hwfn->using_ll2) {
-			p_ll2_info = qed_ll2_alloc(p_hwfn);
-			if (!p_ll2_info)
-				goto alloc_no_mem;
-			p_hwfn->p_ll2_info = p_ll2_info;
+			rc = qed_ll2_alloc(p_hwfn);
+			if (rc)
+				goto alloc_err;
 		}
 #endif
 
 		if (p_hwfn->hw_info.personality == QED_PCI_FCOE) {
-			p_fcoe_info = qed_fcoe_alloc(p_hwfn);
-			if (!p_fcoe_info)
-				goto alloc_no_mem;
-			p_hwfn->p_fcoe_info = p_fcoe_info;
+			rc = qed_fcoe_alloc(p_hwfn);
+			if (rc)
+				goto alloc_err;
 		}
 
 		if (p_hwfn->hw_info.personality == QED_PCI_ISCSI) {
-			p_iscsi_info = qed_iscsi_alloc(p_hwfn);
-			if (!p_iscsi_info)
-				goto alloc_no_mem;
-			p_hwfn->p_iscsi_info = p_iscsi_info;
-			p_ooo_info = qed_ooo_alloc(p_hwfn);
-			if (!p_ooo_info)
-				goto alloc_no_mem;
-			p_hwfn->p_ooo_info = p_ooo_info;
+			rc = qed_iscsi_alloc(p_hwfn);
+			if (rc)
+				goto alloc_err;
+			rc = qed_ooo_alloc(p_hwfn);
+			if (rc)
+				goto alloc_err;
 		}
 
 		/* DMA info initialization */
@@ -1025,16 +1030,19 @@ void qed_resc_setup(struct qed_dev *cdev)
 {
 	int i;
 
-	if (IS_VF(cdev))
+	if (IS_VF(cdev)) {
+		for_each_hwfn(cdev, i)
+			qed_l2_setup(&cdev->hwfns[i]);
 		return;
+	}
 
 	for_each_hwfn(cdev, i) {
 		struct qed_hwfn *p_hwfn = &cdev->hwfns[i];
 
 		qed_cxt_mngr_setup(p_hwfn);
 		qed_spq_setup(p_hwfn);
-		qed_eq_setup(p_hwfn, p_hwfn->p_eq);
-		qed_consq_setup(p_hwfn, p_hwfn->p_consq);
+		qed_eq_setup(p_hwfn);
+		qed_consq_setup(p_hwfn);
 
 		/* Read shadow of current MFW mailbox */
 		qed_mcp_read_mb(p_hwfn, p_hwfn->p_main_ptt);
@@ -1044,17 +1052,18 @@ void qed_resc_setup(struct qed_dev *cdev)
 
 		qed_int_setup(p_hwfn, p_hwfn->p_main_ptt);
 
-		qed_iov_setup(p_hwfn, p_hwfn->p_main_ptt);
+		qed_l2_setup(p_hwfn);
+		qed_iov_setup(p_hwfn);
 #ifdef CONFIG_QED_LL2
 		if (p_hwfn->using_ll2)
-			qed_ll2_setup(p_hwfn, p_hwfn->p_ll2_info);
+			qed_ll2_setup(p_hwfn);
 #endif
 		if (p_hwfn->hw_info.personality == QED_PCI_FCOE)
-			qed_fcoe_setup(p_hwfn, p_hwfn->p_fcoe_info);
+			qed_fcoe_setup(p_hwfn);
 
 		if (p_hwfn->hw_info.personality == QED_PCI_ISCSI) {
-			qed_iscsi_setup(p_hwfn, p_hwfn->p_iscsi_info);
-			qed_ooo_setup(p_hwfn, p_hwfn->p_ooo_info);
+			qed_iscsi_setup(p_hwfn);
+			qed_ooo_setup(p_hwfn);
 		}
 	}
 }
@@ -1122,7 +1131,7 @@ static int qed_calc_hw_mode(struct qed_hwfn *p_hwfn)
 		return -EINVAL;
 	}
 
-	switch (p_hwfn->cdev->num_ports_in_engines) {
+	switch (p_hwfn->cdev->num_ports_in_engine) {
 	case 1:
 		hw_mode |= 1 << MODE_PORTS_PER_ENG_1;
 		break;
@@ -1134,7 +1143,7 @@ static int qed_calc_hw_mode(struct qed_hwfn *p_hwfn)
 		break;
 	default:
 		DP_NOTICE(p_hwfn, "num_ports_in_engine = %d not supported\n",
-			  p_hwfn->cdev->num_ports_in_engines);
+			  p_hwfn->cdev->num_ports_in_engine);
 		return -EINVAL;
 	}
 
@@ -1169,7 +1178,7 @@ static int qed_calc_hw_mode(struct qed_hwfn *p_hwfn)
 static void qed_init_cau_rt_data(struct qed_dev *cdev)
 {
 	u32 offset = CAU_REG_SB_VAR_MEMORY_RT_OFFSET;
-	int i, sb_id;
+	int i, igu_sb_id;
 
 	for_each_hwfn(cdev, i) {
 		struct qed_hwfn *p_hwfn = &cdev->hwfns[i];
@@ -1179,15 +1188,17 @@ static void qed_init_cau_rt_data(struct qed_dev *cdev)
 
 		p_igu_info = p_hwfn->hw_info.p_igu_info;
 
-		for (sb_id = 0; sb_id < QED_MAPPING_MEMORY_SIZE(cdev);
-		     sb_id++) {
-			p_block = &p_igu_info->igu_map.igu_blocks[sb_id];
+		for (igu_sb_id = 0;
+		     igu_sb_id < QED_MAPPING_MEMORY_SIZE(cdev); igu_sb_id++) {
+			p_block = &p_igu_info->entry[igu_sb_id];
+
 			if (!p_block->is_pf)
 				continue;
 
 			qed_init_cau_sb_entry(p_hwfn, &sb_entry,
 					      p_block->function_id, 0, 0);
-			STORE_RT_REG_AGG(p_hwfn, offset + sb_id * 2, sb_entry);
+			STORE_RT_REG_AGG(p_hwfn, offset + igu_sb_id * 2,
+					 sb_entry);
 		}
 	}
 }
@@ -1241,6 +1252,10 @@ static void qed_init_cache_line_size(struct qed_hwfn *p_hwfn,
 			L1_CACHE_BYTES, wr_mbs);
 
 	STORE_RT_REG(p_hwfn, PGLUE_REG_B_CACHE_LINE_SIZE_RT_OFFSET, val);
+	if (val > 0) {
+		STORE_RT_REG(p_hwfn, PSWRQ2_REG_DRAM_ALIGN_WR_RT_OFFSET, val);
+		STORE_RT_REG(p_hwfn, PSWRQ2_REG_DRAM_ALIGN_RD_RT_OFFSET, val);
+	}
 }
 
 static int qed_hw_init_common(struct qed_hwfn *p_hwfn,
@@ -1267,7 +1282,7 @@ static int qed_hw_init_common(struct qed_hwfn *p_hwfn,
 	}
 
 	memset(&params, 0, sizeof(params));
-	params.max_ports_per_engine = p_hwfn->cdev->num_ports_in_engines;
+	params.max_ports_per_engine = p_hwfn->cdev->num_ports_in_engine;
 	params.max_phys_tcs_per_port = qm_info->max_phys_tcs_per_port;
 	params.pf_rl_en = qm_info->pf_rl_en;
 	params.pf_wfq_en = qm_info->pf_wfq_en;
@@ -1447,8 +1462,15 @@ qed_hw_init_pf_doorbell_bar(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 static int qed_hw_init_port(struct qed_hwfn *p_hwfn,
 			    struct qed_ptt *p_ptt, int hw_mode)
 {
-	return qed_init_run(p_hwfn, p_ptt, PHASE_PORT,
-			    p_hwfn->port_id, hw_mode);
+	int rc = 0;
+
+	rc = qed_init_run(p_hwfn, p_ptt, PHASE_PORT, p_hwfn->port_id, hw_mode);
+	if (rc)
+		return rc;
+
+	qed_wr(p_hwfn, p_ptt, PGLUE_B_REG_MASTER_WRITE_PAD_ENABLE, 0);
+
+	return 0;
 }
 
 static int qed_hw_init_pf(struct qed_hwfn *p_hwfn,
@@ -1527,7 +1549,8 @@ static int qed_hw_init_pf(struct qed_hwfn *p_hwfn,
 		qed_int_igu_enable(p_hwfn, p_ptt, int_mode);
 
 		/* send function start command */
-		rc = qed_sp_pf_start(p_hwfn, p_tunn, p_hwfn->cdev->mf_mode,
+		rc = qed_sp_pf_start(p_hwfn, p_ptt, p_tunn,
+				     p_hwfn->cdev->mf_mode,
 				     allow_npar_tx_switch);
 		if (rc) {
 			DP_NOTICE(p_hwfn, "Function start ramrod failed\n");
@@ -1711,6 +1734,11 @@ int qed_hw_init(struct qed_dev *cdev, struct qed_hw_init_params *p_params)
 			return mfw_rc;
 		}
 
+		/* Check if there is a DID mismatch between nvm-cfg/efuse */
+		if (param & FW_MB_PARAM_LOAD_DONE_DID_EFUSE_ERROR)
+			DP_NOTICE(p_hwfn,
+				  "warning: device configuration is not supported on this board type. The device may not function as expected.\n");
+
 		/* send DCBX attention request command */
 		DP_VERBOSE(p_hwfn,
 			   QED_MSG_DCB,
@@ -1956,6 +1984,13 @@ int qed_hw_start_fastpath(struct qed_hwfn *p_hwfn)
 	if (!p_ptt)
 		return -EAGAIN;
 
+	/* If roce info is allocated it means roce is initialized and should
+	 * be enabled in searcher.
+	 */
+	if (p_hwfn->p_rdma_info &&
+	    p_hwfn->b_rdma_enabled_in_prs)
+		qed_wr(p_hwfn, p_ptt, p_hwfn->rdma_prs_search_reg, 0x1);
+
 	/* Re-open incoming traffic */
 	qed_wr(p_hwfn, p_ptt, NIG_REG_RX_LLH_BRB_GATE_DNTFWD_PERPF, 0x0);
 	qed_ptt_release(p_hwfn, p_ptt);
@@ -1968,6 +2003,7 @@ static void qed_hw_hwfn_free(struct qed_hwfn *p_hwfn)
 {
 	qed_ptt_pool_free(p_hwfn);
 	kfree(p_hwfn->hw_info.p_igu_info);
+	p_hwfn->hw_info.p_igu_info = NULL;
 }
 
 /* Setup bar access */
@@ -2025,51 +2061,55 @@ static void get_function_id(struct qed_hwfn *p_hwfn)
 static void qed_hw_set_feat(struct qed_hwfn *p_hwfn)
 {
 	u32 *feat_num = p_hwfn->hw_info.feat_num;
-	struct qed_sb_cnt_info sb_cnt_info;
+	struct qed_sb_cnt_info sb_cnt;
 	u32 non_l2_sbs = 0;
 
+	memset(&sb_cnt, 0, sizeof(sb_cnt));
+	qed_int_get_num_sbs(p_hwfn, &sb_cnt);
+
 	if (IS_ENABLED(CONFIG_QED_RDMA) &&
-	    p_hwfn->hw_info.personality == QED_PCI_ETH_ROCE) {
+	    QED_IS_RDMA_PERSONALITY(p_hwfn)) {
 		/* Roce CNQ each requires: 1 status block + 1 CNQ. We divide
 		 * the status blocks equally between L2 / RoCE but with
 		 * consideration as to how many l2 queues / cnqs we have.
 		 */
 		feat_num[QED_RDMA_CNQ] =
-			min_t(u32, RESC_NUM(p_hwfn, QED_SB) / 2,
+			min_t(u32, sb_cnt.cnt / 2,
 			      RESC_NUM(p_hwfn, QED_RDMA_CNQ_RAM));
 
 		non_l2_sbs = feat_num[QED_RDMA_CNQ];
 	}
-
-	if (p_hwfn->hw_info.personality == QED_PCI_ETH_ROCE ||
-	    p_hwfn->hw_info.personality == QED_PCI_ETH) {
+	if (QED_IS_L2_PERSONALITY(p_hwfn)) {
 		/* Start by allocating VF queues, then PF's */
-		memset(&sb_cnt_info, 0, sizeof(sb_cnt_info));
-		qed_int_get_num_sbs(p_hwfn, &sb_cnt_info);
 		feat_num[QED_VF_L2_QUE] = min_t(u32,
 						RESC_NUM(p_hwfn, QED_L2_QUEUE),
-						sb_cnt_info.sb_iov_cnt);
+						sb_cnt.iov_cnt);
 		feat_num[QED_PF_L2_QUE] = min_t(u32,
-						RESC_NUM(p_hwfn, QED_SB) -
-						non_l2_sbs,
+						sb_cnt.cnt - non_l2_sbs,
 						RESC_NUM(p_hwfn,
 							 QED_L2_QUEUE) -
 						FEAT_NUM(p_hwfn,
 							 QED_VF_L2_QUE));
 	}
 
-	if (p_hwfn->hw_info.personality == QED_PCI_ISCSI)
-		feat_num[QED_ISCSI_CQ] = min_t(u32, RESC_NUM(p_hwfn, QED_SB),
+	if (QED_IS_FCOE_PERSONALITY(p_hwfn))
+		feat_num[QED_FCOE_CQ] =  min_t(u32, sb_cnt.cnt,
+					       RESC_NUM(p_hwfn,
+							QED_CMDQS_CQS));
+
+	if (QED_IS_ISCSI_PERSONALITY(p_hwfn))
+		feat_num[QED_ISCSI_CQ] = min_t(u32, sb_cnt.cnt,
 					       RESC_NUM(p_hwfn,
 							QED_CMDQS_CQS));
 	DP_VERBOSE(p_hwfn,
 		   NETIF_MSG_PROBE,
-		   "#PF_L2_QUEUES=%d VF_L2_QUEUES=%d #ROCE_CNQ=%d ISCSI_CQ=%d #SBS=%d\n",
+		   "#PF_L2_QUEUES=%d VF_L2_QUEUES=%d #ROCE_CNQ=%d FCOE_CQ=%d ISCSI_CQ=%d #SBS=%d\n",
 		   (int)FEAT_NUM(p_hwfn, QED_PF_L2_QUE),
 		   (int)FEAT_NUM(p_hwfn, QED_VF_L2_QUE),
 		   (int)FEAT_NUM(p_hwfn, QED_RDMA_CNQ),
+		   (int)FEAT_NUM(p_hwfn, QED_FCOE_CQ),
 		   (int)FEAT_NUM(p_hwfn, QED_ISCSI_CQ),
-		   RESC_NUM(p_hwfn, QED_SB));
+		   (int)sb_cnt.cnt);
 }
 
 const char *qed_hw_get_resc_name(enum qed_resources res_id)
@@ -2188,7 +2228,6 @@ int qed_hw_get_dflt_resc(struct qed_hwfn *p_hwfn,
 {
 	u8 num_funcs = p_hwfn->num_funcs_on_engine;
 	bool b_ah = QED_IS_AH(p_hwfn->cdev);
-	struct qed_sb_cnt_info sb_cnt_info;
 
 	switch (res_id) {
 	case QED_L2_QUEUE:
@@ -2240,9 +2279,10 @@ int qed_hw_get_dflt_resc(struct qed_hwfn *p_hwfn,
 			*p_resc_num = 1;
 		break;
 	case QED_SB:
-		memset(&sb_cnt_info, 0, sizeof(sb_cnt_info));
-		qed_int_get_num_sbs(p_hwfn, &sb_cnt_info);
-		*p_resc_num = sb_cnt_info.sb_cnt;
+		/* Since we want its value to reflect whether MFW supports
+		 * the new scheme, have a default of 0.
+		 */
+		*p_resc_num = 0;
 		break;
 	default:
 		return -EINVAL;
@@ -2252,7 +2292,7 @@ int qed_hw_get_dflt_resc(struct qed_hwfn *p_hwfn,
 	case QED_BDQ:
 		if (!*p_resc_num)
 			*p_resc_start = 0;
-		else if (p_hwfn->cdev->num_ports_in_engines == 4)
+		else if (p_hwfn->cdev->num_ports_in_engine == 4)
 			*p_resc_start = p_hwfn->port_id;
 		else if (p_hwfn->hw_info.personality == QED_PCI_ISCSI)
 			*p_resc_start = p_hwfn->port_id;
@@ -2311,11 +2351,6 @@ static int __qed_hw_set_resc_info(struct qed_hwfn *p_hwfn,
 		goto out;
 	}
 
-	/* Special handling for status blocks; Would be revised in future */
-	if (res_id == QED_SB) {
-		*p_resc_num -= 1;
-		*p_resc_start -= p_hwfn->enabled_func_idx;
-	}
 out:
 	/* PQs have to divide by 8 [that's the HW granularity].
 	 * Reduce number so it would fit.
@@ -2413,6 +2448,10 @@ static int qed_hw_get_resc(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 		return -EINVAL;
 	}
 
+	/* This will also learn the number of SBs from MFW */
+	if (qed_int_igu_reset_cam(p_hwfn, p_ptt))
+		return -EINVAL;
+
 	qed_hw_set_feat(p_hwfn);
 
 	for (res_id = 0; res_id < QED_MAX_RESC; res_id++)
@@ -2669,15 +2708,15 @@ static void qed_hw_info_port_num_bb(struct qed_hwfn *p_hwfn,
 	port_mode = qed_rd(p_hwfn, p_ptt, CNIG_REG_NW_PORT_MODE_BB_B0);
 
 	if (port_mode < 3) {
-		p_hwfn->cdev->num_ports_in_engines = 1;
+		p_hwfn->cdev->num_ports_in_engine = 1;
 	} else if (port_mode <= 5) {
-		p_hwfn->cdev->num_ports_in_engines = 2;
+		p_hwfn->cdev->num_ports_in_engine = 2;
 	} else {
 		DP_NOTICE(p_hwfn, "PORT MODE: %d not supported\n",
-			  p_hwfn->cdev->num_ports_in_engines);
+			  p_hwfn->cdev->num_ports_in_engine);
 
-		/* Default num_ports_in_engines to something */
-		p_hwfn->cdev->num_ports_in_engines = 1;
+		/* Default num_ports_in_engine to something */
+		p_hwfn->cdev->num_ports_in_engine = 1;
 	}
 }
 
@@ -2687,20 +2726,20 @@ static void qed_hw_info_port_num_ah(struct qed_hwfn *p_hwfn,
 	u32 port;
 	int i;
 
-	p_hwfn->cdev->num_ports_in_engines = 0;
+	p_hwfn->cdev->num_ports_in_engine = 0;
 
 	for (i = 0; i < MAX_NUM_PORTS_K2; i++) {
 		port = qed_rd(p_hwfn, p_ptt,
 			      CNIG_REG_NIG_PORT0_CONF_K2 + (i * 4));
 		if (port & 1)
-			p_hwfn->cdev->num_ports_in_engines++;
+			p_hwfn->cdev->num_ports_in_engine++;
 	}
 
-	if (!p_hwfn->cdev->num_ports_in_engines) {
+	if (!p_hwfn->cdev->num_ports_in_engine) {
 		DP_NOTICE(p_hwfn, "All NIG ports are inactive\n");
 
 		/* Default num_ports_in_engine to something */
-		p_hwfn->cdev->num_ports_in_engines = 1;
+		p_hwfn->cdev->num_ports_in_engine = 1;
 	}
 }
 
@@ -2819,12 +2858,6 @@ static int qed_get_dev_info(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 		cdev->chip_num, cdev->chip_rev,
 		cdev->chip_bond_id, cdev->chip_metal);
 
-	if (QED_IS_BB(cdev) && CHIP_REV_IS_A0(cdev)) {
-		DP_NOTICE(cdev->hwfns,
-			  "The chip type/rev (BB A0) is not supported!\n");
-		return -EINVAL;
-	}
-
 	return 0;
 }
 
@@ -3051,12 +3084,15 @@ static void qed_chain_free_pbl(struct qed_dev *cdev, struct qed_chain *p_chain)
 	}
 
 	pbl_size = page_cnt * QED_CHAIN_PBL_ENTRY_SIZE;
-	dma_free_coherent(&cdev->pdev->dev,
-			  pbl_size,
-			  p_chain->pbl_sp.p_virt_table,
-			  p_chain->pbl_sp.p_phys_table);
+
+	if (!p_chain->b_external_pbl)
+		dma_free_coherent(&cdev->pdev->dev,
+				  pbl_size,
+				  p_chain->pbl_sp.p_virt_table,
+				  p_chain->pbl_sp.p_phys_table);
 out:
 	vfree(p_chain->pbl.pp_virt_addr_tbl);
+	p_chain->pbl.pp_virt_addr_tbl = NULL;
 }
 
 void qed_chain_free(struct qed_dev *cdev, struct qed_chain *p_chain)
@@ -3150,7 +3186,10 @@ qed_chain_alloc_single(struct qed_dev *cdev, struct qed_chain *p_chain)
 	return 0;
 }
 
-static int qed_chain_alloc_pbl(struct qed_dev *cdev, struct qed_chain *p_chain)
+static int
+qed_chain_alloc_pbl(struct qed_dev *cdev,
+		    struct qed_chain *p_chain,
+		    struct qed_chain_ext_pbl *ext_pbl)
 {
 	u32 page_cnt = p_chain->page_cnt, size, i;
 	dma_addr_t p_phys = 0, p_pbl_phys = 0;
@@ -3170,8 +3209,16 @@ static int qed_chain_alloc_pbl(struct qed_dev *cdev, struct qed_chain *p_chain)
 	 * should be saved to allow its freeing during the error flow.
 	 */
 	size = page_cnt * QED_CHAIN_PBL_ENTRY_SIZE;
-	p_pbl_virt = dma_alloc_coherent(&cdev->pdev->dev,
-					size, &p_pbl_phys, GFP_KERNEL);
+
+	if (!ext_pbl) {
+		p_pbl_virt = dma_alloc_coherent(&cdev->pdev->dev,
+						size, &p_pbl_phys, GFP_KERNEL);
+	} else {
+		p_pbl_virt = ext_pbl->p_pbl_virt;
+		p_pbl_phys = ext_pbl->p_pbl_phys;
+		p_chain->b_external_pbl = true;
+	}
+
 	qed_chain_init_pbl_mem(p_chain, p_pbl_virt, p_pbl_phys,
 			       pp_virt_addr_tbl);
 	if (!p_pbl_virt)
@@ -3204,7 +3251,10 @@ int qed_chain_alloc(struct qed_dev *cdev,
 		    enum qed_chain_use_mode intended_use,
 		    enum qed_chain_mode mode,
 		    enum qed_chain_cnt_type cnt_type,
-		    u32 num_elems, size_t elem_size, struct qed_chain *p_chain)
+		    u32 num_elems,
+		    size_t elem_size,
+		    struct qed_chain *p_chain,
+		    struct qed_chain_ext_pbl *ext_pbl)
 {
 	u32 page_cnt;
 	int rc = 0;
@@ -3235,7 +3285,7 @@ int qed_chain_alloc(struct qed_dev *cdev,
 		rc = qed_chain_alloc_single(cdev, p_chain);
 		break;
 	case QED_CHAIN_MODE_PBL:
-		rc = qed_chain_alloc_pbl(cdev, p_chain);
+		rc = qed_chain_alloc_pbl(cdev, p_chain, ext_pbl);
 		break;
 	}
 	if (rc)
@@ -4074,10 +4124,21 @@ static int qed_device_num_ports(struct qed_dev *cdev)
 	if (cdev->num_hwfns > 1)
 		return 1;
 
-	return cdev->num_ports_in_engines * qed_device_num_engines(cdev);
+	return cdev->num_ports_in_engine * qed_device_num_engines(cdev);
 }
 
 int qed_device_get_port_id(struct qed_dev *cdev)
 {
 	return (QED_LEADING_HWFN(cdev)->abs_pf_id) % qed_device_num_ports(cdev);
 }
+
+void qed_set_fw_mac_addr(__le16 *fw_msb,
+			 __le16 *fw_mid, __le16 *fw_lsb, u8 *mac)
+{
+	((u8 *)fw_msb)[0] = mac[1];
+	((u8 *)fw_msb)[1] = mac[0];
+	((u8 *)fw_mid)[0] = mac[3];
+	((u8 *)fw_mid)[1] = mac[2];
+	((u8 *)fw_lsb)[0] = mac[5];
+	((u8 *)fw_lsb)[1] = mac[4];
+}
diff --git a/drivers/net/ethernet/qlogic/qed/qed_dev_api.h b/drivers/net/ethernet/qlogic/qed/qed_dev_api.h
index 12d16c096e36..1f1df1bf127c 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_dev_api.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_dev_api.h
@@ -307,6 +307,7 @@ int qed_dmae_host2host(struct qed_hwfn *p_hwfn,
  * @param num_elems
  * @param elem_size
  * @param p_chain
+ * @param ext_pbl - a possible external PBL
  *
  * @return int
  */
@@ -315,7 +316,9 @@ qed_chain_alloc(struct qed_dev *cdev,
 		enum qed_chain_use_mode intended_use,
 		enum qed_chain_mode mode,
 		enum qed_chain_cnt_type cnt_type,
-		u32 num_elems, size_t elem_size, struct qed_chain *p_chain);
+		u32 num_elems,
+		size_t elem_size,
+		struct qed_chain *p_chain, struct qed_chain_ext_pbl *ext_pbl);
 
 /**
  * @brief qed_chain_free - Free chain DMA memory
diff --git a/drivers/net/ethernet/qlogic/qed/qed_fcoe.c b/drivers/net/ethernet/qlogic/qed/qed_fcoe.c
index 21a58fffd02b..df195c02b711 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_fcoe.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_fcoe.c
@@ -43,7 +43,6 @@
 #include <linux/slab.h>
 #include <linux/stddef.h>
 #include <linux/string.h>
-#include <linux/version.h>
 #include <linux/workqueue.h>
 #include <linux/errno.h>
 #include <linux/list.h>
@@ -142,6 +141,15 @@ qed_sp_fcoe_func_start(struct qed_hwfn *p_hwfn,
 	p_data = &p_ramrod->init_ramrod_data;
 	fcoe_pf_params = &p_hwfn->pf_params.fcoe_pf_params;
 
+	/* Sanity */
+	if (fcoe_pf_params->num_cqs > p_hwfn->hw_info.feat_num[QED_FCOE_CQ]) {
+		DP_ERR(p_hwfn,
+		       "Cannot satisfy CQ amount. CQs requested %d, CQs available %d. Aborting function start\n",
+		       fcoe_pf_params->num_cqs,
+		       p_hwfn->hw_info.feat_num[QED_FCOE_CQ]);
+		return -EINVAL;
+	}
+
 	p_data->mtu = cpu_to_le16(fcoe_pf_params->mtu);
 	tmp = cpu_to_le16(fcoe_pf_params->sq_num_pbl_pages);
 	p_data->sq_num_pages_in_pbl = tmp;
@@ -184,7 +192,10 @@ qed_sp_fcoe_func_start(struct qed_hwfn *p_hwfn,
 	p_data->q_params.queue_relative_offset = (u8)tmp;
 
 	for (i = 0; i < fcoe_pf_params->num_cqs; i++) {
-		tmp = cpu_to_le16(p_hwfn->sbs_info[i]->igu_sb_id);
+		u16 igu_sb_id;
+
+		igu_sb_id = qed_get_igu_sb_id(p_hwfn, i);
+		tmp = cpu_to_le16(igu_sb_id);
 		p_data->q_params.cq_cmdq_sb_num_arr[i] = tmp;
 	}
 
@@ -539,7 +550,7 @@ static void __iomem *qed_fcoe_get_secondary_bdq_prod(struct qed_hwfn *p_hwfn,
 	}
 }
 
-struct qed_fcoe_info *qed_fcoe_alloc(struct qed_hwfn *p_hwfn)
+int qed_fcoe_alloc(struct qed_hwfn *p_hwfn)
 {
 	struct qed_fcoe_info *p_fcoe_info;
 
@@ -547,19 +558,21 @@ struct qed_fcoe_info *qed_fcoe_alloc(struct qed_hwfn *p_hwfn)
 	p_fcoe_info = kzalloc(sizeof(*p_fcoe_info), GFP_KERNEL);
 	if (!p_fcoe_info) {
 		DP_NOTICE(p_hwfn, "Failed to allocate qed_fcoe_info'\n");
-		return NULL;
+		return -ENOMEM;
 	}
 	INIT_LIST_HEAD(&p_fcoe_info->free_list);
-	return p_fcoe_info;
+
+	p_hwfn->p_fcoe_info = p_fcoe_info;
+	return 0;
 }
 
-void qed_fcoe_setup(struct qed_hwfn *p_hwfn, struct qed_fcoe_info *p_fcoe_info)
+void qed_fcoe_setup(struct qed_hwfn *p_hwfn)
 {
 	struct fcoe_task_context *p_task_ctx = NULL;
 	int rc;
 	u32 i;
 
-	spin_lock_init(&p_fcoe_info->lock);
+	spin_lock_init(&p_hwfn->p_fcoe_info->lock);
 	for (i = 0; i < p_hwfn->pf_params.fcoe_pf_params.num_tasks; i++) {
 		rc = qed_cxt_get_task_ctx(p_hwfn, i,
 					  QED_CTX_WORKING_MEM,
@@ -577,15 +590,15 @@ void qed_fcoe_setup(struct qed_hwfn *p_hwfn, struct qed_fcoe_info *p_fcoe_info)
 	}
 }
 
-void qed_fcoe_free(struct qed_hwfn *p_hwfn, struct qed_fcoe_info *p_fcoe_info)
+void qed_fcoe_free(struct qed_hwfn *p_hwfn)
 {
 	struct qed_fcoe_conn *p_conn = NULL;
 
-	if (!p_fcoe_info)
+	if (!p_hwfn->p_fcoe_info)
 		return;
 
-	while (!list_empty(&p_fcoe_info->free_list)) {
-		p_conn = list_first_entry(&p_fcoe_info->free_list,
+	while (!list_empty(&p_hwfn->p_fcoe_info->free_list)) {
+		p_conn = list_first_entry(&p_hwfn->p_fcoe_info->free_list,
 					  struct qed_fcoe_conn, list_entry);
 		if (!p_conn)
 			break;
@@ -593,7 +606,8 @@ void qed_fcoe_free(struct qed_hwfn *p_hwfn, struct qed_fcoe_info *p_fcoe_info)
 		qed_fcoe_free_connection(p_hwfn, p_conn);
 	}
 
-	kfree(p_fcoe_info);
+	kfree(p_hwfn->p_fcoe_info);
+	p_hwfn->p_fcoe_info = NULL;
 }
 
 static int
@@ -734,6 +748,11 @@ static int qed_fill_fcoe_dev_info(struct qed_dev *cdev,
 	info->secondary_bdq_rq_addr =
 	    qed_fcoe_get_secondary_bdq_prod(hwfn, BDQ_ID_RQ);
 
+	info->wwpn = hwfn->mcp_info->func_info.wwn_port;
+	info->wwnn = hwfn->mcp_info->func_info.wwn_node;
+
+	info->num_cqs = FEAT_NUM(hwfn, QED_FCOE_CQ);
+
 	return rc;
 }
 
diff --git a/drivers/net/ethernet/qlogic/qed/qed_fcoe.h b/drivers/net/ethernet/qlogic/qed/qed_fcoe.h
index 472af34a171d..027a76ac839a 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_fcoe.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_fcoe.h
@@ -49,29 +49,21 @@ struct qed_fcoe_info {
 };
 
 #if IS_ENABLED(CONFIG_QED_FCOE)
-struct qed_fcoe_info *qed_fcoe_alloc(struct qed_hwfn *p_hwfn);
+int qed_fcoe_alloc(struct qed_hwfn *p_hwfn);
 
-void qed_fcoe_setup(struct qed_hwfn *p_hwfn, struct qed_fcoe_info *p_fcoe_info);
+void qed_fcoe_setup(struct qed_hwfn *p_hwfn);
 
-void qed_fcoe_free(struct qed_hwfn *p_hwfn, struct qed_fcoe_info *p_fcoe_info);
+void qed_fcoe_free(struct qed_hwfn *p_hwfn);
 void qed_get_protocol_stats_fcoe(struct qed_dev *cdev,
 				 struct qed_mcp_fcoe_stats *stats);
 #else /* CONFIG_QED_FCOE */
-static inline struct qed_fcoe_info *
-qed_fcoe_alloc(struct qed_hwfn *p_hwfn)
+static inline int qed_fcoe_alloc(struct qed_hwfn *p_hwfn)
 {
-	return NULL;
+	return -EINVAL;
 }
 
-static inline void qed_fcoe_setup(struct qed_hwfn *p_hwfn,
-				  struct qed_fcoe_info *p_fcoe_info)
-{
-}
-
-static inline void qed_fcoe_free(struct qed_hwfn *p_hwfn,
-				 struct qed_fcoe_info *p_fcoe_info)
-{
-}
+static inline void qed_fcoe_setup(struct qed_hwfn *p_hwfn) {}
+static inline void qed_fcoe_free(struct qed_hwfn *p_hwfn) {}
 
 static inline void qed_get_protocol_stats_fcoe(struct qed_dev *cdev,
 					       struct qed_mcp_fcoe_stats *stats)
diff --git a/drivers/net/ethernet/qlogic/qed/qed_hsi.h b/drivers/net/ethernet/qlogic/qed/qed_hsi.h
index 858a57a73589..31fb0bffa098 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_hsi.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_hsi.h
@@ -46,6 +46,7 @@
 #include <linux/qed/fcoe_common.h>
 #include <linux/qed/eth_common.h>
 #include <linux/qed/iscsi_common.h>
+#include <linux/qed/iwarp_common.h>
 #include <linux/qed/rdma_common.h>
 #include <linux/qed/roce_common.h>
 #include <linux/qed/qed_fcoe_if.h>
@@ -346,7 +347,7 @@ struct xstorm_core_conn_ag_ctx {
 	u8 byte13;
 	u8 byte14;
 	u8 byte15;
-	u8 byte16;
+	u8 e5_reserved;
 	__le16 word11;
 	__le32 reg10;
 	__le32 reg11;
@@ -368,85 +369,85 @@ struct tstorm_core_conn_ag_ctx {
 	u8 byte0;
 	u8 byte1;
 	u8 flags0;
-#define TSTORM_CORE_CONN_AG_CTX_BIT0_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_BIT0_SHIFT	0
-#define TSTORM_CORE_CONN_AG_CTX_BIT1_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_BIT1_SHIFT	1
-#define TSTORM_CORE_CONN_AG_CTX_BIT2_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_BIT2_SHIFT	2
-#define TSTORM_CORE_CONN_AG_CTX_BIT3_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_BIT3_SHIFT	3
-#define TSTORM_CORE_CONN_AG_CTX_BIT4_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_BIT4_SHIFT	4
-#define TSTORM_CORE_CONN_AG_CTX_BIT5_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_BIT5_SHIFT	5
-#define TSTORM_CORE_CONN_AG_CTX_CF0_MASK	0x3
-#define TSTORM_CORE_CONN_AG_CTX_CF0_SHIFT	6
+#define TSTORM_CORE_CONN_AG_CTX_BIT0_MASK     0x1	/* exist_in_qm0 */
+#define TSTORM_CORE_CONN_AG_CTX_BIT0_SHIFT    0
+#define TSTORM_CORE_CONN_AG_CTX_BIT1_MASK     0x1	/* exist_in_qm1 */
+#define TSTORM_CORE_CONN_AG_CTX_BIT1_SHIFT    1
+#define TSTORM_CORE_CONN_AG_CTX_BIT2_MASK     0x1	/* bit2 */
+#define TSTORM_CORE_CONN_AG_CTX_BIT2_SHIFT    2
+#define TSTORM_CORE_CONN_AG_CTX_BIT3_MASK     0x1	/* bit3 */
+#define TSTORM_CORE_CONN_AG_CTX_BIT3_SHIFT    3
+#define TSTORM_CORE_CONN_AG_CTX_BIT4_MASK     0x1	/* bit4 */
+#define TSTORM_CORE_CONN_AG_CTX_BIT4_SHIFT    4
+#define TSTORM_CORE_CONN_AG_CTX_BIT5_MASK     0x1	/* bit5 */
+#define TSTORM_CORE_CONN_AG_CTX_BIT5_SHIFT    5
+#define TSTORM_CORE_CONN_AG_CTX_CF0_MASK      0x3	/* timer0cf */
+#define TSTORM_CORE_CONN_AG_CTX_CF0_SHIFT     6
 	u8 flags1;
-#define TSTORM_CORE_CONN_AG_CTX_CF1_MASK	0x3
-#define TSTORM_CORE_CONN_AG_CTX_CF1_SHIFT	0
-#define TSTORM_CORE_CONN_AG_CTX_CF2_MASK	0x3
-#define TSTORM_CORE_CONN_AG_CTX_CF2_SHIFT	2
-#define TSTORM_CORE_CONN_AG_CTX_CF3_MASK	0x3
-#define TSTORM_CORE_CONN_AG_CTX_CF3_SHIFT	4
-#define TSTORM_CORE_CONN_AG_CTX_CF4_MASK	0x3
-#define TSTORM_CORE_CONN_AG_CTX_CF4_SHIFT	6
+#define TSTORM_CORE_CONN_AG_CTX_CF1_MASK      0x3	/* timer1cf */
+#define TSTORM_CORE_CONN_AG_CTX_CF1_SHIFT     0
+#define TSTORM_CORE_CONN_AG_CTX_CF2_MASK      0x3	/* timer2cf */
+#define TSTORM_CORE_CONN_AG_CTX_CF2_SHIFT     2
+#define TSTORM_CORE_CONN_AG_CTX_CF3_MASK      0x3	/* timer_stop_all */
+#define TSTORM_CORE_CONN_AG_CTX_CF3_SHIFT     4
+#define TSTORM_CORE_CONN_AG_CTX_CF4_MASK      0x3	/* cf4 */
+#define TSTORM_CORE_CONN_AG_CTX_CF4_SHIFT     6
 	u8 flags2;
-#define TSTORM_CORE_CONN_AG_CTX_CF5_MASK	0x3
-#define TSTORM_CORE_CONN_AG_CTX_CF5_SHIFT	0
-#define TSTORM_CORE_CONN_AG_CTX_CF6_MASK	0x3
-#define TSTORM_CORE_CONN_AG_CTX_CF6_SHIFT	2
-#define TSTORM_CORE_CONN_AG_CTX_CF7_MASK	0x3
-#define TSTORM_CORE_CONN_AG_CTX_CF7_SHIFT	4
-#define TSTORM_CORE_CONN_AG_CTX_CF8_MASK	0x3
-#define TSTORM_CORE_CONN_AG_CTX_CF8_SHIFT	6
+#define TSTORM_CORE_CONN_AG_CTX_CF5_MASK      0x3	/* cf5 */
+#define TSTORM_CORE_CONN_AG_CTX_CF5_SHIFT     0
+#define TSTORM_CORE_CONN_AG_CTX_CF6_MASK      0x3	/* cf6 */
+#define TSTORM_CORE_CONN_AG_CTX_CF6_SHIFT     2
+#define TSTORM_CORE_CONN_AG_CTX_CF7_MASK      0x3	/* cf7 */
+#define TSTORM_CORE_CONN_AG_CTX_CF7_SHIFT     4
+#define TSTORM_CORE_CONN_AG_CTX_CF8_MASK      0x3	/* cf8 */
+#define TSTORM_CORE_CONN_AG_CTX_CF8_SHIFT     6
 	u8 flags3;
-#define TSTORM_CORE_CONN_AG_CTX_CF9_MASK	0x3
-#define TSTORM_CORE_CONN_AG_CTX_CF9_SHIFT	0
-#define TSTORM_CORE_CONN_AG_CTX_CF10_MASK	0x3
-#define TSTORM_CORE_CONN_AG_CTX_CF10_SHIFT	2
-#define TSTORM_CORE_CONN_AG_CTX_CF0EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_CF0EN_SHIFT	4
-#define TSTORM_CORE_CONN_AG_CTX_CF1EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_CF1EN_SHIFT	5
-#define TSTORM_CORE_CONN_AG_CTX_CF2EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_CF2EN_SHIFT	6
-#define TSTORM_CORE_CONN_AG_CTX_CF3EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_CF3EN_SHIFT	7
+#define TSTORM_CORE_CONN_AG_CTX_CF9_MASK      0x3	/* cf9 */
+#define TSTORM_CORE_CONN_AG_CTX_CF9_SHIFT     0
+#define TSTORM_CORE_CONN_AG_CTX_CF10_MASK     0x3	/* cf10 */
+#define TSTORM_CORE_CONN_AG_CTX_CF10_SHIFT    2
+#define TSTORM_CORE_CONN_AG_CTX_CF0EN_MASK    0x1	/* cf0en */
+#define TSTORM_CORE_CONN_AG_CTX_CF0EN_SHIFT   4
+#define TSTORM_CORE_CONN_AG_CTX_CF1EN_MASK    0x1	/* cf1en */
+#define TSTORM_CORE_CONN_AG_CTX_CF1EN_SHIFT   5
+#define TSTORM_CORE_CONN_AG_CTX_CF2EN_MASK    0x1	/* cf2en */
+#define TSTORM_CORE_CONN_AG_CTX_CF2EN_SHIFT   6
+#define TSTORM_CORE_CONN_AG_CTX_CF3EN_MASK    0x1	/* cf3en */
+#define TSTORM_CORE_CONN_AG_CTX_CF3EN_SHIFT   7
 	u8 flags4;
-#define TSTORM_CORE_CONN_AG_CTX_CF4EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_CF4EN_SHIFT	0
-#define TSTORM_CORE_CONN_AG_CTX_CF5EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_CF5EN_SHIFT	1
-#define TSTORM_CORE_CONN_AG_CTX_CF6EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_CF6EN_SHIFT	2
-#define TSTORM_CORE_CONN_AG_CTX_CF7EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_CF7EN_SHIFT	3
-#define TSTORM_CORE_CONN_AG_CTX_CF8EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_CF8EN_SHIFT	4
-#define TSTORM_CORE_CONN_AG_CTX_CF9EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_CF9EN_SHIFT	5
-#define TSTORM_CORE_CONN_AG_CTX_CF10EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_CF10EN_SHIFT	6
-#define TSTORM_CORE_CONN_AG_CTX_RULE0EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_RULE0EN_SHIFT	7
+#define TSTORM_CORE_CONN_AG_CTX_CF4EN_MASK    0x1	/* cf4en */
+#define TSTORM_CORE_CONN_AG_CTX_CF4EN_SHIFT   0
+#define TSTORM_CORE_CONN_AG_CTX_CF5EN_MASK    0x1	/* cf5en */
+#define TSTORM_CORE_CONN_AG_CTX_CF5EN_SHIFT   1
+#define TSTORM_CORE_CONN_AG_CTX_CF6EN_MASK    0x1	/* cf6en */
+#define TSTORM_CORE_CONN_AG_CTX_CF6EN_SHIFT   2
+#define TSTORM_CORE_CONN_AG_CTX_CF7EN_MASK    0x1	/* cf7en */
+#define TSTORM_CORE_CONN_AG_CTX_CF7EN_SHIFT   3
+#define TSTORM_CORE_CONN_AG_CTX_CF8EN_MASK    0x1	/* cf8en */
+#define TSTORM_CORE_CONN_AG_CTX_CF8EN_SHIFT   4
+#define TSTORM_CORE_CONN_AG_CTX_CF9EN_MASK    0x1	/* cf9en */
+#define TSTORM_CORE_CONN_AG_CTX_CF9EN_SHIFT   5
+#define TSTORM_CORE_CONN_AG_CTX_CF10EN_MASK   0x1	/* cf10en */
+#define TSTORM_CORE_CONN_AG_CTX_CF10EN_SHIFT  6
+#define TSTORM_CORE_CONN_AG_CTX_RULE0EN_MASK  0x1	/* rule0en */
+#define TSTORM_CORE_CONN_AG_CTX_RULE0EN_SHIFT 7
 	u8 flags5;
-#define TSTORM_CORE_CONN_AG_CTX_RULE1EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_RULE1EN_SHIFT	0
-#define TSTORM_CORE_CONN_AG_CTX_RULE2EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_RULE2EN_SHIFT	1
-#define TSTORM_CORE_CONN_AG_CTX_RULE3EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_RULE3EN_SHIFT	2
-#define TSTORM_CORE_CONN_AG_CTX_RULE4EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_RULE4EN_SHIFT	3
-#define TSTORM_CORE_CONN_AG_CTX_RULE5EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_RULE5EN_SHIFT	4
-#define TSTORM_CORE_CONN_AG_CTX_RULE6EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_RULE6EN_SHIFT	5
-#define TSTORM_CORE_CONN_AG_CTX_RULE7EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_RULE7EN_SHIFT	6
-#define TSTORM_CORE_CONN_AG_CTX_RULE8EN_MASK	0x1
-#define TSTORM_CORE_CONN_AG_CTX_RULE8EN_SHIFT	7
+#define TSTORM_CORE_CONN_AG_CTX_RULE1EN_MASK  0x1	/* rule1en */
+#define TSTORM_CORE_CONN_AG_CTX_RULE1EN_SHIFT 0
+#define TSTORM_CORE_CONN_AG_CTX_RULE2EN_MASK  0x1	/* rule2en */
+#define TSTORM_CORE_CONN_AG_CTX_RULE2EN_SHIFT 1
+#define TSTORM_CORE_CONN_AG_CTX_RULE3EN_MASK  0x1	/* rule3en */
+#define TSTORM_CORE_CONN_AG_CTX_RULE3EN_SHIFT 2
+#define TSTORM_CORE_CONN_AG_CTX_RULE4EN_MASK  0x1	/* rule4en */
+#define TSTORM_CORE_CONN_AG_CTX_RULE4EN_SHIFT 3
+#define TSTORM_CORE_CONN_AG_CTX_RULE5EN_MASK  0x1	/* rule5en */
+#define TSTORM_CORE_CONN_AG_CTX_RULE5EN_SHIFT 4
+#define TSTORM_CORE_CONN_AG_CTX_RULE6EN_MASK  0x1	/* rule6en */
+#define TSTORM_CORE_CONN_AG_CTX_RULE6EN_SHIFT 5
+#define TSTORM_CORE_CONN_AG_CTX_RULE7EN_MASK  0x1	/* rule7en */
+#define TSTORM_CORE_CONN_AG_CTX_RULE7EN_SHIFT 6
+#define TSTORM_CORE_CONN_AG_CTX_RULE8EN_MASK  0x1	/* rule8en */
+#define TSTORM_CORE_CONN_AG_CTX_RULE8EN_SHIFT 7
 	__le32 reg0;
 	__le32 reg1;
 	__le32 reg2;
@@ -681,7 +682,9 @@ struct core_rx_fast_path_cqe {
 	__le16 packet_length;
 	__le16 vlan;
 	struct core_rx_cqe_opaque_data opaque_data;
-	__le32 reserved[4];
+	struct parsing_err_flags err_flags;
+	__le16 reserved0;
+	__le32 reserved1[3];
 };
 
 struct core_rx_gsi_offload_cqe {
@@ -692,7 +695,7 @@ struct core_rx_gsi_offload_cqe {
 	__le16 vlan;
 	__le32 src_mac_addrhi;
 	__le16 src_mac_addrlo;
-	u8 reserved1[2];
+	__le16 qp_id;
 	__le32 gid_dst[4];
 };
 
@@ -774,15 +777,15 @@ struct core_tx_bd {
 	__le16 bitfield1;
 #define CORE_TX_BD_L4_HDR_OFFSET_W_MASK	0x3FFF
 #define CORE_TX_BD_L4_HDR_OFFSET_W_SHIFT 0
-#define CORE_TX_BD_TX_DST_MASK	0x1
-#define CORE_TX_BD_TX_DST_SHIFT	14
-#define CORE_TX_BD_RESERVED_MASK         0x1
-#define CORE_TX_BD_RESERVED_SHIFT        15
+#define CORE_TX_BD_TX_DST_MASK		0x3
+#define CORE_TX_BD_TX_DST_SHIFT		14
 };
 
 enum core_tx_dest {
 	CORE_TX_DEST_NW,
 	CORE_TX_DEST_LB,
+	CORE_TX_DEST_RESERVED,
+	CORE_TX_DEST_DROP,
 	MAX_CORE_TX_DEST
 };
 
@@ -804,12 +807,12 @@ struct core_tx_stop_ramrod_data {
 	__le32 reserved0[2];
 };
 
-enum dcb_dhcp_update_flag {
-	DONT_UPDATE_DCB_DHCP,
+enum dcb_dscp_update_mode {
+	DONT_UPDATE_DCB_DSCP,
 	UPDATE_DCB,
 	UPDATE_DSCP,
 	UPDATE_DCB_DSCP,
-	MAX_DCB_DHCP_UPDATE_FLAG
+	MAX_DCB_DSCP_UPDATE_MODE
 };
 
 struct eth_mstorm_per_pf_stat {
@@ -917,6 +920,14 @@ struct hsi_fp_ver_struct {
 	u8 major_ver_arr[2];
 };
 
+enum iwarp_ll2_tx_queues {
+	IWARP_LL2_IN_ORDER_TX_QUEUE =			1,
+	IWARP_LL2_ALIGNED_TX_QUEUE,
+	IWARP_LL2_ALIGNED_RIGHT_TRIMMED_TX_QUEUE,
+	IWARP_LL2_ERROR,
+	MAX_IWARP_LL2_TX_QUEUES
+};
+
 /* Mstorm non-triggering VF zone */
 enum malicious_vf_error_id {
 	MALICIOUS_VF_NO_ERROR,
@@ -960,7 +971,7 @@ enum personality_type {
 	PERSONALITY_ISCSI,
 	PERSONALITY_FCOE,
 	PERSONALITY_RDMA_AND_ETH,
-	PERSONALITY_RESERVED3,
+	PERSONALITY_RDMA,
 	PERSONALITY_CORE,
 	PERSONALITY_ETH,
 	PERSONALITY_RESERVED4,
@@ -971,16 +982,12 @@ enum personality_type {
 struct pf_start_tunnel_config {
 	u8 set_vxlan_udp_port_flg;
 	u8 set_geneve_udp_port_flg;
-	u8 tx_enable_vxlan;
-	u8 tx_enable_l2geneve;
-	u8 tx_enable_ipgeneve;
-	u8 tx_enable_l2gre;
-	u8 tx_enable_ipgre;
 	u8 tunnel_clss_vxlan;
 	u8 tunnel_clss_l2geneve;
 	u8 tunnel_clss_ipgeneve;
 	u8 tunnel_clss_l2gre;
 	u8 tunnel_clss_ipgre;
+	u8 reserved;
 	__le16 vxlan_udp_port;
 	__le16 geneve_udp_port;
 };
@@ -990,6 +997,7 @@ struct pf_start_ramrod_data {
 	struct regpair event_ring_pbl_addr;
 	struct regpair consolid_q_pbl_addr;
 	struct pf_start_tunnel_config tunnel_config;
+	__le32 reserved;
 	__le16 event_ring_sb_id;
 	u8 base_vf_id;
 	u8 num_vfs;
@@ -1007,7 +1015,6 @@ struct pf_start_ramrod_data {
 	u8 pri_map_valid;
 	__le32 outer_tag;
 	struct hsi_fp_ver_struct hsi_fp_ver;
-
 };
 
 struct protocol_dcb_data {
@@ -1023,14 +1030,8 @@ struct pf_update_tunnel_config {
 	u8 update_rx_pf_clss;
 	u8 update_rx_def_ucast_clss;
 	u8 update_rx_def_non_ucast_clss;
-	u8 update_tx_pf_clss;
 	u8 set_vxlan_udp_port_flg;
 	u8 set_geneve_udp_port_flg;
-	u8 tx_enable_vxlan;
-	u8 tx_enable_l2geneve;
-	u8 tx_enable_ipgeneve;
-	u8 tx_enable_l2gre;
-	u8 tx_enable_ipgre;
 	u8 tunnel_clss_vxlan;
 	u8 tunnel_clss_l2geneve;
 	u8 tunnel_clss_ipgeneve;
@@ -1038,17 +1039,17 @@ struct pf_update_tunnel_config {
 	u8 tunnel_clss_ipgre;
 	__le16 vxlan_udp_port;
 	__le16 geneve_udp_port;
-	__le16 reserved[2];
+	__le16 reserved;
 };
 
 struct pf_update_ramrod_data {
 	u8 pf_id;
-	u8 update_eth_dcb_data_flag;
-	u8 update_fcoe_dcb_data_flag;
-	u8 update_iscsi_dcb_data_flag;
-	u8 update_roce_dcb_data_flag;
-	u8 update_rroce_dcb_data_flag;
-	u8 update_iwarp_dcb_data_flag;
+	u8 update_eth_dcb_data_mode;
+	u8 update_fcoe_dcb_data_mode;
+	u8 update_iscsi_dcb_data_mode;
+	u8 update_roce_dcb_data_mode;
+	u8 update_rroce_dcb_data_mode;
+	u8 update_iwarp_dcb_data_mode;
 	u8 update_mf_vlan_flag;
 	struct protocol_dcb_data eth_dcb_data;
 	struct protocol_dcb_data fcoe_dcb_data;
@@ -1127,7 +1128,7 @@ struct tstorm_per_port_stat {
 	struct regpair iscsi_irregular_pkt;
 	struct regpair fcoe_irregular_pkt;
 	struct regpair roce_irregular_pkt;
-	struct regpair reserved;
+	struct regpair iwarp_irregular_pkt;
 	struct regpair eth_irregular_pkt;
 	struct regpair reserved1;
 	struct regpair preroce_irregular_pkt;
@@ -1326,6 +1327,87 @@ enum dmae_cmd_src_enum {
 	MAX_DMAE_CMD_SRC_ENUM
 };
 
+struct mstorm_core_conn_ag_ctx {
+	u8 byte0;
+	u8 byte1;
+	u8 flags0;
+#define MSTORM_CORE_CONN_AG_CTX_BIT0_MASK	0x1
+#define MSTORM_CORE_CONN_AG_CTX_BIT0_SHIFT	0
+#define MSTORM_CORE_CONN_AG_CTX_BIT1_MASK	0x1
+#define MSTORM_CORE_CONN_AG_CTX_BIT1_SHIFT	1
+#define MSTORM_CORE_CONN_AG_CTX_CF0_MASK	0x3
+#define MSTORM_CORE_CONN_AG_CTX_CF0_SHIFT	2
+#define MSTORM_CORE_CONN_AG_CTX_CF1_MASK	0x3
+#define MSTORM_CORE_CONN_AG_CTX_CF1_SHIFT	4
+#define MSTORM_CORE_CONN_AG_CTX_CF2_MASK	0x3
+#define MSTORM_CORE_CONN_AG_CTX_CF2_SHIFT	6
+	u8 flags1;
+#define MSTORM_CORE_CONN_AG_CTX_CF0EN_MASK	0x1
+#define MSTORM_CORE_CONN_AG_CTX_CF0EN_SHIFT	0
+#define MSTORM_CORE_CONN_AG_CTX_CF1EN_MASK	0x1
+#define MSTORM_CORE_CONN_AG_CTX_CF1EN_SHIFT	1
+#define MSTORM_CORE_CONN_AG_CTX_CF2EN_MASK	0x1
+#define MSTORM_CORE_CONN_AG_CTX_CF2EN_SHIFT	2
+#define MSTORM_CORE_CONN_AG_CTX_RULE0EN_MASK	0x1
+#define MSTORM_CORE_CONN_AG_CTX_RULE0EN_SHIFT	3
+#define MSTORM_CORE_CONN_AG_CTX_RULE1EN_MASK	0x1
+#define MSTORM_CORE_CONN_AG_CTX_RULE1EN_SHIFT	4
+#define MSTORM_CORE_CONN_AG_CTX_RULE2EN_MASK	0x1
+#define MSTORM_CORE_CONN_AG_CTX_RULE2EN_SHIFT	5
+#define MSTORM_CORE_CONN_AG_CTX_RULE3EN_MASK	0x1
+#define MSTORM_CORE_CONN_AG_CTX_RULE3EN_SHIFT	6
+#define MSTORM_CORE_CONN_AG_CTX_RULE4EN_MASK	0x1
+#define MSTORM_CORE_CONN_AG_CTX_RULE4EN_SHIFT	7
+	__le16 word0;
+	__le16 word1;
+	__le32 reg0;
+	__le32 reg1;
+};
+
+struct ystorm_core_conn_ag_ctx {
+	u8 byte0;
+	u8 byte1;
+	u8 flags0;
+#define YSTORM_CORE_CONN_AG_CTX_BIT0_MASK	0x1
+#define YSTORM_CORE_CONN_AG_CTX_BIT0_SHIFT	0
+#define YSTORM_CORE_CONN_AG_CTX_BIT1_MASK	0x1
+#define YSTORM_CORE_CONN_AG_CTX_BIT1_SHIFT	1
+#define YSTORM_CORE_CONN_AG_CTX_CF0_MASK	0x3
+#define YSTORM_CORE_CONN_AG_CTX_CF0_SHIFT	2
+#define YSTORM_CORE_CONN_AG_CTX_CF1_MASK	0x3
+#define YSTORM_CORE_CONN_AG_CTX_CF1_SHIFT	4
+#define YSTORM_CORE_CONN_AG_CTX_CF2_MASK	0x3
+#define YSTORM_CORE_CONN_AG_CTX_CF2_SHIFT	6
+	u8 flags1;
+#define YSTORM_CORE_CONN_AG_CTX_CF0EN_MASK	0x1
+#define YSTORM_CORE_CONN_AG_CTX_CF0EN_SHIFT	0
+#define YSTORM_CORE_CONN_AG_CTX_CF1EN_MASK	0x1
+#define YSTORM_CORE_CONN_AG_CTX_CF1EN_SHIFT	1
+#define YSTORM_CORE_CONN_AG_CTX_CF2EN_MASK	0x1
+#define YSTORM_CORE_CONN_AG_CTX_CF2EN_SHIFT	2
+#define YSTORM_CORE_CONN_AG_CTX_RULE0EN_MASK	0x1
+#define YSTORM_CORE_CONN_AG_CTX_RULE0EN_SHIFT	3
+#define YSTORM_CORE_CONN_AG_CTX_RULE1EN_MASK	0x1
+#define YSTORM_CORE_CONN_AG_CTX_RULE1EN_SHIFT	4
+#define YSTORM_CORE_CONN_AG_CTX_RULE2EN_MASK	0x1
+#define YSTORM_CORE_CONN_AG_CTX_RULE2EN_SHIFT	5
+#define YSTORM_CORE_CONN_AG_CTX_RULE3EN_MASK	0x1
+#define YSTORM_CORE_CONN_AG_CTX_RULE3EN_SHIFT	6
+#define YSTORM_CORE_CONN_AG_CTX_RULE4EN_MASK	0x1
+#define YSTORM_CORE_CONN_AG_CTX_RULE4EN_SHIFT	7
+	u8 byte2;
+	u8 byte3;
+	__le16 word0;
+	__le32 reg0;
+	__le32 reg1;
+	__le16 word1;
+	__le16 word2;
+	__le16 word3;
+	__le16 word4;
+	__le32 reg2;
+	__le32 reg3;
+};
+
 /* IGU cleanup command */
 struct igu_cleanup {
 	__le32 sb_id_and_flags;
@@ -1389,44 +1471,6 @@ struct igu_msix_vector {
 #define IGU_MSIX_VECTOR_RESERVED1_MASK		0xFF
 #define IGU_MSIX_VECTOR_RESERVED1_SHIFT		24
 };
-
-struct mstorm_core_conn_ag_ctx {
-	u8 byte0;
-	u8 byte1;
-	u8 flags0;
-#define MSTORM_CORE_CONN_AG_CTX_BIT0_MASK	0x1
-#define MSTORM_CORE_CONN_AG_CTX_BIT0_SHIFT	0
-#define MSTORM_CORE_CONN_AG_CTX_BIT1_MASK	0x1
-#define MSTORM_CORE_CONN_AG_CTX_BIT1_SHIFT	1
-#define MSTORM_CORE_CONN_AG_CTX_CF0_MASK	0x3
-#define MSTORM_CORE_CONN_AG_CTX_CF0_SHIFT	2
-#define MSTORM_CORE_CONN_AG_CTX_CF1_MASK	0x3
-#define MSTORM_CORE_CONN_AG_CTX_CF1_SHIFT	4
-#define MSTORM_CORE_CONN_AG_CTX_CF2_MASK	0x3
-#define MSTORM_CORE_CONN_AG_CTX_CF2_SHIFT	6
-	u8 flags1;
-#define MSTORM_CORE_CONN_AG_CTX_CF0EN_MASK	0x1
-#define MSTORM_CORE_CONN_AG_CTX_CF0EN_SHIFT	0
-#define MSTORM_CORE_CONN_AG_CTX_CF1EN_MASK	0x1
-#define MSTORM_CORE_CONN_AG_CTX_CF1EN_SHIFT	1
-#define MSTORM_CORE_CONN_AG_CTX_CF2EN_MASK	0x1
-#define MSTORM_CORE_CONN_AG_CTX_CF2EN_SHIFT	2
-#define MSTORM_CORE_CONN_AG_CTX_RULE0EN_MASK	0x1
-#define MSTORM_CORE_CONN_AG_CTX_RULE0EN_SHIFT	3
-#define MSTORM_CORE_CONN_AG_CTX_RULE1EN_MASK	0x1
-#define MSTORM_CORE_CONN_AG_CTX_RULE1EN_SHIFT	4
-#define MSTORM_CORE_CONN_AG_CTX_RULE2EN_MASK	0x1
-#define MSTORM_CORE_CONN_AG_CTX_RULE2EN_SHIFT	5
-#define MSTORM_CORE_CONN_AG_CTX_RULE3EN_MASK	0x1
-#define MSTORM_CORE_CONN_AG_CTX_RULE3EN_SHIFT	6
-#define MSTORM_CORE_CONN_AG_CTX_RULE4EN_MASK	0x1
-#define MSTORM_CORE_CONN_AG_CTX_RULE4EN_SHIFT	7
-	__le16 word0;
-	__le16 word1;
-	__le32 reg0;
-	__le32 reg1;
-};
-
 /* per encapsulation type enabling flags */
 struct prs_reg_encapsulation_type_en {
 	u8 flags;
@@ -1541,50 +1585,6 @@ struct sdm_op_gen {
 #define SDM_OP_GEN_RESERVED_SHIFT	20
 };
 
-struct ystorm_core_conn_ag_ctx {
-	u8 byte0;
-	u8 byte1;
-	u8 flags0;
-#define YSTORM_CORE_CONN_AG_CTX_BIT0_MASK	0x1
-#define YSTORM_CORE_CONN_AG_CTX_BIT0_SHIFT	0
-#define YSTORM_CORE_CONN_AG_CTX_BIT1_MASK	0x1
-#define YSTORM_CORE_CONN_AG_CTX_BIT1_SHIFT	1
-#define YSTORM_CORE_CONN_AG_CTX_CF0_MASK	0x3
-#define YSTORM_CORE_CONN_AG_CTX_CF0_SHIFT	2
-#define YSTORM_CORE_CONN_AG_CTX_CF1_MASK	0x3
-#define YSTORM_CORE_CONN_AG_CTX_CF1_SHIFT	4
-#define YSTORM_CORE_CONN_AG_CTX_CF2_MASK	0x3
-#define YSTORM_CORE_CONN_AG_CTX_CF2_SHIFT	6
-	u8 flags1;
-#define YSTORM_CORE_CONN_AG_CTX_CF0EN_MASK	0x1
-#define YSTORM_CORE_CONN_AG_CTX_CF0EN_SHIFT	0
-#define YSTORM_CORE_CONN_AG_CTX_CF1EN_MASK	0x1
-#define YSTORM_CORE_CONN_AG_CTX_CF1EN_SHIFT	1
-#define YSTORM_CORE_CONN_AG_CTX_CF2EN_MASK	0x1
-#define YSTORM_CORE_CONN_AG_CTX_CF2EN_SHIFT	2
-#define YSTORM_CORE_CONN_AG_CTX_RULE0EN_MASK	0x1
-#define YSTORM_CORE_CONN_AG_CTX_RULE0EN_SHIFT	3
-#define YSTORM_CORE_CONN_AG_CTX_RULE1EN_MASK	0x1
-#define YSTORM_CORE_CONN_AG_CTX_RULE1EN_SHIFT	4
-#define YSTORM_CORE_CONN_AG_CTX_RULE2EN_MASK	0x1
-#define YSTORM_CORE_CONN_AG_CTX_RULE2EN_SHIFT	5
-#define YSTORM_CORE_CONN_AG_CTX_RULE3EN_MASK	0x1
-#define YSTORM_CORE_CONN_AG_CTX_RULE3EN_SHIFT	6
-#define YSTORM_CORE_CONN_AG_CTX_RULE4EN_MASK	0x1
-#define YSTORM_CORE_CONN_AG_CTX_RULE4EN_SHIFT	7
-	u8 byte2;
-	u8 byte3;
-	__le16 word0;
-	__le32 reg0;
-	__le32 reg1;
-	__le16 word1;
-	__le16 word2;
-	__le16 word3;
-	__le16 word4;
-	__le32 reg2;
-	__le32 reg3;
-};
-
 /****************************************/
 /* Debug Tools HSI constants and macros */
 /****************************************/
@@ -1643,6 +1643,8 @@ enum block_addr {
 	GRCBASE_MULD = 0x4e0000,
 	GRCBASE_YULD = 0x4c8000,
 	GRCBASE_XYLD = 0x4c0000,
+	GRCBASE_PTLD = 0x590000,
+	GRCBASE_YPLD = 0x5b0000,
 	GRCBASE_PRM = 0x230000,
 	GRCBASE_PBF_PB1 = 0xda0000,
 	GRCBASE_PBF_PB2 = 0xda4000,
@@ -1656,6 +1658,10 @@ enum block_addr {
 	GRCBASE_TCFC = 0x2d0000,
 	GRCBASE_IGU = 0x180000,
 	GRCBASE_CAU = 0x1c0000,
+	GRCBASE_RGFS = 0xf00000,
+	GRCBASE_RGSRC = 0x320000,
+	GRCBASE_TGFS = 0xd00000,
+	GRCBASE_TGSRC = 0x322000,
 	GRCBASE_UMAC = 0x51000,
 	GRCBASE_XMAC = 0x210000,
 	GRCBASE_DBG = 0x10000,
@@ -1669,10 +1675,6 @@ enum block_addr {
 	GRCBASE_PHY_PCIE = 0x620000,
 	GRCBASE_LED = 0x6b8000,
 	GRCBASE_AVS_WRAP = 0x6b0000,
-	GRCBASE_RGFS = 0x19d0000,
-	GRCBASE_TGFS = 0x19e0000,
-	GRCBASE_PTLD = 0x19f0000,
-	GRCBASE_YPLD = 0x1a10000,
 	GRCBASE_MISC_AEU = 0x8000,
 	GRCBASE_BAR0_MAP = 0x1c00000,
 	MAX_BLOCK_ADDR
@@ -1732,6 +1734,8 @@ enum block_id {
 	BLOCK_MULD,
 	BLOCK_YULD,
 	BLOCK_XYLD,
+	BLOCK_PTLD,
+	BLOCK_YPLD,
 	BLOCK_PRM,
 	BLOCK_PBF_PB1,
 	BLOCK_PBF_PB2,
@@ -1745,6 +1749,10 @@ enum block_id {
 	BLOCK_TCFC,
 	BLOCK_IGU,
 	BLOCK_CAU,
+	BLOCK_RGFS,
+	BLOCK_RGSRC,
+	BLOCK_TGFS,
+	BLOCK_TGSRC,
 	BLOCK_UMAC,
 	BLOCK_XMAC,
 	BLOCK_DBG,
@@ -1758,10 +1766,6 @@ enum block_id {
 	BLOCK_PHY_PCIE,
 	BLOCK_LED,
 	BLOCK_AVS_WRAP,
-	BLOCK_RGFS,
-	BLOCK_TGFS,
-	BLOCK_PTLD,
-	BLOCK_YPLD,
 	BLOCK_MISC_AEU,
 	BLOCK_BAR0_MAP,
 	MAX_BLOCK_ID
@@ -1780,6 +1784,10 @@ enum bin_dbg_buffer_type {
 	BIN_BUF_DBG_ATTN_REGS,
 	BIN_BUF_DBG_ATTN_INDEXES,
 	BIN_BUF_DBG_ATTN_NAME_OFFSETS,
+	BIN_BUF_DBG_BUS_BLOCKS,
+	BIN_BUF_DBG_BUS_LINES,
+	BIN_BUF_DBG_BUS_BLOCKS_USER_DATA,
+	BIN_BUF_DBG_BUS_LINE_NAME_OFFSETS,
 	BIN_BUF_DBG_PARSING_STRINGS,
 	MAX_BIN_DBG_BUFFER_TYPE
 };
@@ -1862,6 +1870,29 @@ enum dbg_attn_type {
 	MAX_DBG_ATTN_TYPE
 };
 
+struct dbg_bus_block {
+	u8 num_of_lines;
+	u8 has_latency_events;
+	__le16 lines_offset;
+};
+
+struct dbg_bus_block_user_data {
+	u8 num_of_lines;
+	u8 has_latency_events;
+	__le16 names_offset;
+};
+
+struct dbg_bus_line {
+	u8 data;
+#define DBG_BUS_LINE_NUM_OF_GROUPS_MASK  0xF
+#define DBG_BUS_LINE_NUM_OF_GROUPS_SHIFT 0
+#define DBG_BUS_LINE_IS_256B_MASK        0x1
+#define DBG_BUS_LINE_IS_256B_SHIFT       4
+#define DBG_BUS_LINE_RESERVED_MASK       0x7
+#define DBG_BUS_LINE_RESERVED_SHIFT      5
+	u8 group_sizes;
+};
+
 /* condition header for registers dump */
 struct dbg_dump_cond_hdr {
 	struct dbg_mode_hdr mode; /* Mode header */
@@ -1879,17 +1910,21 @@ struct dbg_dump_mem {
 	__le32 dword1;
 #define DBG_DUMP_MEM_LENGTH_MASK        0xFFFFFF
 #define DBG_DUMP_MEM_LENGTH_SHIFT       0
-#define DBG_DUMP_MEM_RESERVED_MASK      0xFF
-#define DBG_DUMP_MEM_RESERVED_SHIFT     24
+#define DBG_DUMP_MEM_WIDE_BUS_MASK      0x1
+#define DBG_DUMP_MEM_WIDE_BUS_SHIFT     24
+#define DBG_DUMP_MEM_RESERVED_MASK      0x7F
+#define DBG_DUMP_MEM_RESERVED_SHIFT     25
 };
 
 /* register data for registers dump */
 struct dbg_dump_reg {
 	__le32 data;
-#define DBG_DUMP_REG_ADDRESS_MASK  0xFFFFFF /* register address (in dwords) */
+#define DBG_DUMP_REG_ADDRESS_MASK 0x7FFFFF /* register address (in dwords) */
 #define DBG_DUMP_REG_ADDRESS_SHIFT 0
-#define DBG_DUMP_REG_LENGTH_MASK   0xFF /* register size (in dwords) */
-#define DBG_DUMP_REG_LENGTH_SHIFT  24
+#define DBG_DUMP_REG_WIDE_BUS_MASK 0x1 /* indicates register is wide-bus */
+#define DBG_DUMP_REG_WIDE_BUS_SHIFT 23
+#define DBG_DUMP_REG_LENGTH_MASK  0xFF /* register size (in dwords) */
+#define DBG_DUMP_REG_LENGTH_SHIFT 24
 };
 
 /* split header for registers dump */
@@ -1910,20 +1945,24 @@ struct dbg_idle_chk_cond_hdr {
 /* Idle Check condition register */
 struct dbg_idle_chk_cond_reg {
 	__le32 data;
-#define DBG_IDLE_CHK_COND_REG_ADDRESS_MASK   0xFFFFFF
+#define DBG_IDLE_CHK_COND_REG_ADDRESS_MASK   0x7FFFFF
 #define DBG_IDLE_CHK_COND_REG_ADDRESS_SHIFT  0
+#define DBG_IDLE_CHK_COND_REG_WIDE_BUS_MASK  0x1
+#define DBG_IDLE_CHK_COND_REG_WIDE_BUS_SHIFT 23
 #define DBG_IDLE_CHK_COND_REG_BLOCK_ID_MASK  0xFF
 #define DBG_IDLE_CHK_COND_REG_BLOCK_ID_SHIFT 24
-	__le16 num_entries; /* number of registers entries to check */
-	u8 entry_size; /* size of registers entry (in dwords) */
-	u8 start_entry; /* index of the first entry to check */
+	__le16 num_entries;
+	u8 entry_size;
+	u8 start_entry;
 };
 
 /* Idle Check info register */
 struct dbg_idle_chk_info_reg {
 	__le32 data;
-#define DBG_IDLE_CHK_INFO_REG_ADDRESS_MASK   0xFFFFFF
+#define DBG_IDLE_CHK_INFO_REG_ADDRESS_MASK   0x7FFFFF
 #define DBG_IDLE_CHK_INFO_REG_ADDRESS_SHIFT  0
+#define DBG_IDLE_CHK_INFO_REG_WIDE_BUS_MASK  0x1
+#define DBG_IDLE_CHK_INFO_REG_WIDE_BUS_SHIFT 23
 #define DBG_IDLE_CHK_INFO_REG_BLOCK_ID_MASK  0xFF
 #define DBG_IDLE_CHK_INFO_REG_BLOCK_ID_SHIFT 24
 	__le16 size; /* register size in dwords */
@@ -1996,15 +2035,17 @@ enum dbg_idle_chk_severity_types {
 
 /* Debug Bus block data */
 struct dbg_bus_block_data {
-	u8 enabled; /* Indicates if the block is enabled for recording (0/1) */
-	u8 hw_id; /* HW ID associated with the block */
-	u8 line_num; /* Debug line number to select */
-	u8 right_shift; /* Number of units to  right the debug data (0-3) */
-	u8 cycle_en; /* 4-bit value: bit i set -> unit i is enabled. */
-	u8 force_valid; /* 4-bit value: bit i set -> unit i is forced valid. */
-	u8 force_frame; /* 4-bit value: bit i set -> unit i frame bit is forced.
-			 */
-	u8 reserved;
+	__le16 data;
+#define DBG_BUS_BLOCK_DATA_ENABLE_MASK_MASK       0xF
+#define DBG_BUS_BLOCK_DATA_ENABLE_MASK_SHIFT      0
+#define DBG_BUS_BLOCK_DATA_RIGHT_SHIFT_MASK       0xF
+#define DBG_BUS_BLOCK_DATA_RIGHT_SHIFT_SHIFT      4
+#define DBG_BUS_BLOCK_DATA_FORCE_VALID_MASK_MASK  0xF
+#define DBG_BUS_BLOCK_DATA_FORCE_VALID_MASK_SHIFT 8
+#define DBG_BUS_BLOCK_DATA_FORCE_FRAME_MASK_MASK  0xF
+#define DBG_BUS_BLOCK_DATA_FORCE_FRAME_MASK_SHIFT 12
+	u8 line_num;
+	u8 hw_id;
 };
 
 /* Debug Bus Clients */
@@ -2045,6 +2086,14 @@ enum dbg_bus_constraint_ops {
 	MAX_DBG_BUS_CONSTRAINT_OPS
 };
 
+struct dbg_bus_trigger_state_data {
+	u8 data;
+#define DBG_BUS_TRIGGER_STATE_DATA_BLOCK_SHIFTED_ENABLE_MASK_MASK  0xF
+#define DBG_BUS_TRIGGER_STATE_DATA_BLOCK_SHIFTED_ENABLE_MASK_SHIFT 0
+#define DBG_BUS_TRIGGER_STATE_DATA_CONSTRAINT_DWORD_MASK_MASK      0xF
+#define DBG_BUS_TRIGGER_STATE_DATA_CONSTRAINT_DWORD_MASK_SHIFT     4
+};
+
 /* Debug Bus memory address */
 struct dbg_bus_mem_addr {
 	__le32 lo;
@@ -2078,66 +2127,42 @@ union dbg_bus_storm_eid_params {
 
 /* Debug Bus Storm data */
 struct dbg_bus_storm_data {
-	u8 fast_enabled;
-	u8 fast_mode;
-	u8 slow_enabled;
-	u8 slow_mode;
+	u8 enabled;
+	u8 mode;
 	u8 hw_id;
 	u8 eid_filter_en;
 	u8 eid_range_not_mask;
 	u8 cid_filter_en;
 	union dbg_bus_storm_eid_params eid_filter_params;
-	__le16 reserved;
 	__le32 cid;
 };
 
 /* Debug Bus data */
 struct dbg_bus_data {
-	__le32 app_version; /* The tools version number of the application */
-	u8 state; /* The current debug bus state */
-	u8 hw_dwords; /* HW dwords per cycle */
-	u8 next_hw_id; /* Next HW ID to be associated with an input */
-	u8 num_enabled_blocks; /* Number of blocks enabled for recording */
-	u8 num_enabled_storms; /* Number of Storms enabled for recording */
-	u8 target; /* Output target */
-	u8 next_trigger_state; /* ID of next trigger state to be added */
-	u8 next_constraint_id; /* ID of next filter/trigger constraint to be
-				* added.
-				*/
-	u8 one_shot_en; /* Indicates if one-shot mode is enabled (0/1) */
-	u8 grc_input_en; /* Indicates if GRC recording is enabled (0/1) */
-	u8 timestamp_input_en; /* Indicates if timestamp recording is enabled
-				* (0/1).
-				*/
-	u8 filter_en; /* Indicates if the recording filter is enabled (0/1) */
-	u8 trigger_en; /* Indicates if the recording trigger is enabled (0/1) */
-	u8 adding_filter; /* If true, the next added constraint belong to the
-			   * filter. Otherwise, it belongs to the last added
-			   * trigger state. Valid only if either filter or
-			   * triggers are enabled.
-			   */
-	u8 filter_pre_trigger; /* Indicates if the recording filter should be
-				* applied before the trigger. Valid only if both
-				* filter and trigger are enabled (0/1).
-				*/
-	u8 filter_post_trigger; /* Indicates if the recording filter should be
-				 * applied after the trigger. Valid only if both
-				 * filter and trigger are enabled (0/1).
-				 */
-	u8 unify_inputs; /* If true, all inputs are associated with HW ID 0.
-			  * Otherwise, each input is assigned a different HW ID
-			  * (0/1).
-			  */
-	u8 rcv_from_other_engine; /* Indicates if the other engine sends it NW
-				   * recording to this engine (0/1).
-				   */
-	struct dbg_bus_pci_buf_data pci_buf; /* Debug Bus PCI buffer data. Valid
-					      * only when the target is
-					      * DBG_BUS_TARGET_ID_PCI.
-					      */
+	__le32 app_version;
+	u8 state;
+	u8 hw_dwords;
+	__le16 hw_id_mask;
+	u8 num_enabled_blocks;
+	u8 num_enabled_storms;
+	u8 target;
+	u8 one_shot_en;
+	u8 grc_input_en;
+	u8 timestamp_input_en;
+	u8 filter_en;
+	u8 adding_filter;
+	u8 filter_pre_trigger;
+	u8 filter_post_trigger;
 	__le16 reserved;
-	struct dbg_bus_block_data blocks[88];/* Debug Bus data for each block */
-	struct dbg_bus_storm_data storms[6]; /* Debug Bus data for each block */
+	u8 trigger_en;
+	struct dbg_bus_trigger_state_data trigger_states[3];
+	u8 next_trigger_state;
+	u8 next_constraint_id;
+	u8 unify_inputs;
+	u8 rcv_from_other_engine;
+	struct dbg_bus_pci_buf_data pci_buf;
+	struct dbg_bus_block_data blocks[88];
+	struct dbg_bus_storm_data storms[6];
 };
 
 enum dbg_bus_filter_types {
@@ -2156,12 +2181,6 @@ enum dbg_bus_frame_modes {
 	MAX_DBG_BUS_FRAME_MODES
 };
 
-enum dbg_bus_input_types {
-	DBG_BUS_INPUT_TYPE_STORM,
-	DBG_BUS_INPUT_TYPE_BLOCK,
-	MAX_DBG_BUS_INPUT_TYPES
-};
-
 enum dbg_bus_other_engine_modes {
 	DBG_BUS_OTHER_ENGINE_MODE_NONE,
 	DBG_BUS_OTHER_ENGINE_MODE_DOUBLE_BW_TX,
@@ -2185,19 +2204,19 @@ enum dbg_bus_pre_trigger_types {
 };
 
 enum dbg_bus_semi_frame_modes {
-	DBG_BUS_SEMI_FRAME_MODE_0SLOW_4FAST = 0,
-	DBG_BUS_SEMI_FRAME_MODE_4SLOW_0FAST = 3,
+	DBG_BUS_SEMI_FRAME_MODE_0SLOW_4FAST =
+	    0,
+	DBG_BUS_SEMI_FRAME_MODE_4SLOW_0FAST =
+	    3,
 	MAX_DBG_BUS_SEMI_FRAME_MODES
 };
 
 /* Debug bus states */
 enum dbg_bus_states {
-	DBG_BUS_STATE_IDLE, /* debug bus idle state (not recording) */
-	DBG_BUS_STATE_READY, /* debug bus is ready for configuration and
-			      * recording.
-			      */
-	DBG_BUS_STATE_RECORDING, /* debug bus is currently recording */
-	DBG_BUS_STATE_STOPPED, /* debug bus recording has stopped */
+	DBG_BUS_STATE_IDLE,
+	DBG_BUS_STATE_READY,
+	DBG_BUS_STATE_RECORDING,
+	DBG_BUS_STATE_STOPPED,
 	MAX_DBG_BUS_STATES
 };
 
@@ -2216,11 +2235,8 @@ enum dbg_bus_storm_modes {
 
 /* Debug bus target IDs */
 enum dbg_bus_targets {
-	/* records debug bus to DBG block internal buffer */
 	DBG_BUS_TARGET_ID_INT_BUF,
-	/* records debug bus to the NW */
 	DBG_BUS_TARGET_ID_NIG,
-	/* records debug bus to a PCI buffer */
 	DBG_BUS_TARGET_ID_PCI,
 	MAX_DBG_BUS_TARGETS
 };
@@ -2235,48 +2251,45 @@ struct dbg_grc_data {
 
 /* Debug GRC params */
 enum dbg_grc_params {
-	DBG_GRC_PARAM_DUMP_TSTORM, /* dump Tstorm memories (0/1) */
-	DBG_GRC_PARAM_DUMP_MSTORM, /* dump Mstorm memories (0/1) */
-	DBG_GRC_PARAM_DUMP_USTORM, /* dump Ustorm memories (0/1) */
-	DBG_GRC_PARAM_DUMP_XSTORM, /* dump Xstorm memories (0/1) */
-	DBG_GRC_PARAM_DUMP_YSTORM, /* dump Ystorm memories (0/1) */
-	DBG_GRC_PARAM_DUMP_PSTORM, /* dump Pstorm memories (0/1) */
-	DBG_GRC_PARAM_DUMP_REGS, /* dump non-memory registers (0/1) */
-	DBG_GRC_PARAM_DUMP_RAM, /* dump Storm internal RAMs (0/1) */
-	DBG_GRC_PARAM_DUMP_PBUF, /* dump Storm passive buffer (0/1) */
-	DBG_GRC_PARAM_DUMP_IOR, /* dump Storm IORs (0/1) */
-	DBG_GRC_PARAM_DUMP_VFC, /* dump VFC memories (0/1) */
-	DBG_GRC_PARAM_DUMP_CM_CTX, /* dump CM contexts (0/1) */
-	DBG_GRC_PARAM_DUMP_PXP, /* dump PXP memories (0/1) */
-	DBG_GRC_PARAM_DUMP_RSS, /* dump RSS memories (0/1) */
-	DBG_GRC_PARAM_DUMP_CAU, /* dump CAU memories (0/1) */
-	DBG_GRC_PARAM_DUMP_QM, /* dump QM memories (0/1) */
-	DBG_GRC_PARAM_DUMP_MCP, /* dump MCP memories (0/1) */
-	DBG_GRC_PARAM_RESERVED, /* reserved */
-	DBG_GRC_PARAM_DUMP_CFC, /* dump CFC memories (0/1) */
-	DBG_GRC_PARAM_DUMP_IGU, /* dump IGU memories (0/1) */
-	DBG_GRC_PARAM_DUMP_BRB, /* dump BRB memories (0/1) */
-	DBG_GRC_PARAM_DUMP_BTB, /* dump BTB memories (0/1) */
-	DBG_GRC_PARAM_DUMP_BMB, /* dump BMB memories (0/1) */
-	DBG_GRC_PARAM_DUMP_NIG, /* dump NIG memories (0/1) */
-	DBG_GRC_PARAM_DUMP_MULD, /* dump MULD memories (0/1) */
-	DBG_GRC_PARAM_DUMP_PRS, /* dump PRS memories (0/1) */
-	DBG_GRC_PARAM_DUMP_DMAE, /* dump PRS memories (0/1) */
-	DBG_GRC_PARAM_DUMP_TM, /* dump TM (timers) memories (0/1) */
-	DBG_GRC_PARAM_DUMP_SDM, /* dump SDM memories (0/1) */
-	DBG_GRC_PARAM_DUMP_DIF, /* dump DIF memories (0/1) */
-	DBG_GRC_PARAM_DUMP_STATIC, /* dump static debug data (0/1) */
-	DBG_GRC_PARAM_UNSTALL, /* un-stall Storms after dump (0/1) */
-	DBG_GRC_PARAM_NUM_LCIDS, /* number of LCIDs (0..320) */
-	DBG_GRC_PARAM_NUM_LTIDS, /* number of LTIDs (0..320) */
-	/* preset: exclude all memories from dump (1 only) */
+	DBG_GRC_PARAM_DUMP_TSTORM,
+	DBG_GRC_PARAM_DUMP_MSTORM,
+	DBG_GRC_PARAM_DUMP_USTORM,
+	DBG_GRC_PARAM_DUMP_XSTORM,
+	DBG_GRC_PARAM_DUMP_YSTORM,
+	DBG_GRC_PARAM_DUMP_PSTORM,
+	DBG_GRC_PARAM_DUMP_REGS,
+	DBG_GRC_PARAM_DUMP_RAM,
+	DBG_GRC_PARAM_DUMP_PBUF,
+	DBG_GRC_PARAM_DUMP_IOR,
+	DBG_GRC_PARAM_DUMP_VFC,
+	DBG_GRC_PARAM_DUMP_CM_CTX,
+	DBG_GRC_PARAM_DUMP_PXP,
+	DBG_GRC_PARAM_DUMP_RSS,
+	DBG_GRC_PARAM_DUMP_CAU,
+	DBG_GRC_PARAM_DUMP_QM,
+	DBG_GRC_PARAM_DUMP_MCP,
+	DBG_GRC_PARAM_RESERVED,
+	DBG_GRC_PARAM_DUMP_CFC,
+	DBG_GRC_PARAM_DUMP_IGU,
+	DBG_GRC_PARAM_DUMP_BRB,
+	DBG_GRC_PARAM_DUMP_BTB,
+	DBG_GRC_PARAM_DUMP_BMB,
+	DBG_GRC_PARAM_DUMP_NIG,
+	DBG_GRC_PARAM_DUMP_MULD,
+	DBG_GRC_PARAM_DUMP_PRS,
+	DBG_GRC_PARAM_DUMP_DMAE,
+	DBG_GRC_PARAM_DUMP_TM,
+	DBG_GRC_PARAM_DUMP_SDM,
+	DBG_GRC_PARAM_DUMP_DIF,
+	DBG_GRC_PARAM_DUMP_STATIC,
+	DBG_GRC_PARAM_UNSTALL,
+	DBG_GRC_PARAM_NUM_LCIDS,
+	DBG_GRC_PARAM_NUM_LTIDS,
 	DBG_GRC_PARAM_EXCLUDE_ALL,
-	/* preset: include memories for crash dump (1 only) */
 	DBG_GRC_PARAM_CRASH,
-	/* perform dump only if MFW is responding (0/1) */
 	DBG_GRC_PARAM_PARITY_SAFE,
-	DBG_GRC_PARAM_DUMP_CM, /* dump CM memories (0/1) */
-	DBG_GRC_PARAM_DUMP_PHY, /* dump PHY memories (0/1) */
+	DBG_GRC_PARAM_DUMP_CM,
+	DBG_GRC_PARAM_DUMP_PHY,
 	DBG_GRC_PARAM_NO_MCP,
 	DBG_GRC_PARAM_NO_FW_VER,
 	MAX_DBG_GRC_PARAMS
@@ -2347,7 +2360,10 @@ enum dbg_status {
 	DBG_STATUS_REG_FIFO_BAD_DATA,
 	DBG_STATUS_PROTECTION_OVERRIDE_BAD_DATA,
 	DBG_STATUS_DBG_ARRAY_NOT_SET,
-	DBG_STATUS_MULTI_BLOCKS_WITH_FILTER,
+	DBG_STATUS_FILTER_BUG,
+	DBG_STATUS_NON_MATCHING_LINES,
+	DBG_STATUS_INVALID_TRIGGER_DWORD_OFFSET,
+	DBG_STATUS_DBG_BUS_IN_USE,
 	MAX_DBG_STATUS
 };
 
@@ -2364,25 +2380,22 @@ enum dbg_storms {
 
 /* Idle Check data */
 struct idle_chk_data {
-	__le32 buf_size; /* Idle check buffer size in dwords */
-	u8 buf_size_set; /* Indicates if the idle check buffer size was set
-			  * (0/1).
-			  */
+	__le32 buf_size;
+	u8 buf_size_set;
 	u8 reserved1;
 	__le16 reserved2;
 };
 
 /* Debug Tools data (per HW function) */
 struct dbg_tools_data {
-	struct dbg_grc_data grc; /* GRC Dump data */
-	struct dbg_bus_data bus; /* Debug Bus data */
-	struct idle_chk_data idle_chk; /* Idle Check data */
-	u8 mode_enable[40]; /* Indicates if a mode is enabled (0/1) */
-	u8 block_in_reset[88]; /* Indicates if a block is in reset state (0/1).
-				*/
-	u8 chip_id; /* Chip ID (from enum chip_ids) */
-	u8 platform_id; /* Platform ID (from enum platform_ids) */
-	u8 initialized; /* Indicates if the data was initialized */
+	struct dbg_grc_data grc;
+	struct dbg_bus_data bus;
+	struct idle_chk_data idle_chk;
+	u8 mode_enable[40];
+	u8 block_in_reset[88];
+	u8 chip_id;
+	u8 platform_id;
+	u8 initialized;
 	u8 reserved;
 };
 
@@ -2464,6 +2477,12 @@ struct init_qm_vport_params {
 
 /* Max size in dwords of a zipped array */
 #define MAX_ZIPPED_SIZE	8192
+enum chip_ids {
+	CHIP_BB,
+	CHIP_K2,
+	CHIP_RESERVED,
+	MAX_CHIP_IDS
+};
 
 struct fw_asserts_ram_section {
 	__le16 section_ram_line_offset;
@@ -2475,18 +2494,18 @@ struct fw_asserts_ram_section {
 };
 
 struct fw_ver_num {
-	u8 major; /* Firmware major version number */
-	u8 minor; /* Firmware minor version number */
-	u8 rev; /* Firmware revision version number */
-	u8 eng; /* Firmware engineering version number (for bootleg versions) */
+	u8 major;
+	u8 minor;
+	u8 rev;
+	u8 eng;
 };
 
 struct fw_ver_info {
-	__le16 tools_ver; /* Tools version number */
-	u8 image_id; /* FW image ID (e.g. main) */
+	__le16 tools_ver;
+	u8 image_id;
 	u8 reserved1;
-	struct fw_ver_num num; /* FW version number */
-	__le32 timestamp; /* FW Timestamp in unix time  (sec. since 1970) */
+	struct fw_ver_num num;
+	__le32 timestamp;
 	__le32 reserved2;
 };
 
@@ -2722,7 +2741,6 @@ struct init_read_op {
 #define INIT_READ_OP_ADDRESS_MASK	0x7FFFFF
 #define INIT_READ_OP_ADDRESS_SHIFT	9
 	__le32 expected_val;
-
 };
 
 /* Init operations union */
@@ -2782,6 +2800,7 @@ struct iro {
  * @param bin_ptr - a pointer to the binary data with debug arrays.
  */
 enum dbg_status qed_dbg_set_bin_ptr(const u8 * const bin_ptr);
+
 /**
  * @brief qed_dbg_grc_set_params_default - Reverts all GRC parameters to their
  *	default value.
@@ -2805,6 +2824,7 @@ void qed_dbg_grc_set_params_default(struct qed_hwfn *p_hwfn);
 enum dbg_status qed_dbg_grc_get_dump_buf_size(struct qed_hwfn *p_hwfn,
 					      struct qed_ptt *p_ptt,
 					      u32 *buf_size);
+
 /**
  * @brief qed_dbg_grc_dump - Dumps GRC data into the specified buffer.
  *
@@ -2824,6 +2844,7 @@ enum dbg_status qed_dbg_grc_dump(struct qed_hwfn *p_hwfn,
 				 u32 *dump_buf,
 				 u32 buf_size_in_dwords,
 				 u32 *num_dumped_dwords);
+
 /**
  * @brief qed_dbg_idle_chk_get_dump_buf_size - Returns the required buffer size
  *	for idle check results.
@@ -2840,6 +2861,7 @@ enum dbg_status qed_dbg_grc_dump(struct qed_hwfn *p_hwfn,
 enum dbg_status qed_dbg_idle_chk_get_dump_buf_size(struct qed_hwfn *p_hwfn,
 						   struct qed_ptt *p_ptt,
 						   u32 *buf_size);
+
 /**
  * @brief qed_dbg_idle_chk_dump - Performs idle check and writes the results
  *	into the specified buffer.
@@ -2860,6 +2882,7 @@ enum dbg_status qed_dbg_idle_chk_dump(struct qed_hwfn *p_hwfn,
 				      u32 *dump_buf,
 				      u32 buf_size_in_dwords,
 				      u32 *num_dumped_dwords);
+
 /**
  * @brief qed_dbg_mcp_trace_get_dump_buf_size - Returns the required buffer size
  *	for mcp trace results.
@@ -2878,6 +2901,7 @@ enum dbg_status qed_dbg_idle_chk_dump(struct qed_hwfn *p_hwfn,
 enum dbg_status qed_dbg_mcp_trace_get_dump_buf_size(struct qed_hwfn *p_hwfn,
 						    struct qed_ptt *p_ptt,
 						    u32 *buf_size);
+
 /**
  * @brief qed_dbg_mcp_trace_dump - Performs mcp trace and writes the results
  *	into the specified buffer.
@@ -2902,6 +2926,7 @@ enum dbg_status qed_dbg_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 				       u32 *dump_buf,
 				       u32 buf_size_in_dwords,
 				       u32 *num_dumped_dwords);
+
 /**
  * @brief qed_dbg_reg_fifo_get_dump_buf_size - Returns the required buffer size
  *	for grc trace fifo results.
@@ -2917,6 +2942,7 @@ enum dbg_status qed_dbg_mcp_trace_dump(struct qed_hwfn *p_hwfn,
 enum dbg_status qed_dbg_reg_fifo_get_dump_buf_size(struct qed_hwfn *p_hwfn,
 						   struct qed_ptt *p_ptt,
 						   u32 *buf_size);
+
 /**
  * @brief qed_dbg_reg_fifo_dump - Reads the reg fifo and writes the results into
  *	the specified buffer.
@@ -2938,6 +2964,7 @@ enum dbg_status qed_dbg_reg_fifo_dump(struct qed_hwfn *p_hwfn,
 				      u32 *dump_buf,
 				      u32 buf_size_in_dwords,
 				      u32 *num_dumped_dwords);
+
 /**
  * @brief qed_dbg_igu_fifo_get_dump_buf_size - Returns the required buffer size
  *	for the IGU fifo results.
@@ -2954,6 +2981,7 @@ enum dbg_status qed_dbg_reg_fifo_dump(struct qed_hwfn *p_hwfn,
 enum dbg_status qed_dbg_igu_fifo_get_dump_buf_size(struct qed_hwfn *p_hwfn,
 						   struct qed_ptt *p_ptt,
 						   u32 *buf_size);
+
 /**
  * @brief qed_dbg_igu_fifo_dump - Reads the IGU fifo and writes the results into
  *	the specified buffer.
@@ -2975,6 +3003,7 @@ enum dbg_status qed_dbg_igu_fifo_dump(struct qed_hwfn *p_hwfn,
 				      u32 *dump_buf,
 				      u32 buf_size_in_dwords,
 				      u32 *num_dumped_dwords);
+
 /**
  * @brief qed_dbg_protection_override_get_dump_buf_size - Returns the required
  *	buffer size for protection override window results.
@@ -3048,6 +3077,29 @@ enum dbg_status qed_dbg_fw_asserts_dump(struct qed_hwfn *p_hwfn,
 					u32 *dump_buf,
 					u32 buf_size_in_dwords,
 					u32 *num_dumped_dwords);
+
+/**
+ * @brief qed_dbg_read_attn - Reads the attention registers of the specified
+ * block and type, and writes the results into the specified buffer.
+ *
+ * @param p_hwfn -	 HW device data
+ * @param p_ptt -	 Ptt window used for writing the registers.
+ * @param block -	 Block ID.
+ * @param attn_type -	 Attention type.
+ * @param clear_status - Indicates if the attention status should be cleared.
+ * @param results -	 OUT: Pointer to write the read results into
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_read_attn(struct qed_hwfn *p_hwfn,
+				  struct qed_ptt *p_ptt,
+				  enum block_id block,
+				  enum dbg_attn_type attn_type,
+				  bool clear_status,
+				  struct dbg_attn_block_result *results);
+
 /**
  * @brief qed_dbg_print_attn - Prints attention registers values in the
  *	specified results struct.
@@ -3074,6 +3126,7 @@ enum dbg_status qed_dbg_print_attn(struct qed_hwfn *p_hwfn,
  * @param bin_ptr - a pointer to the binary data with debug arrays.
  */
 enum dbg_status qed_dbg_user_set_bin_ptr(const u8 * const bin_ptr);
+
 /**
  * @brief qed_dbg_get_status_str - Returns a string for the specified status.
  *
@@ -3082,6 +3135,7 @@ enum dbg_status qed_dbg_user_set_bin_ptr(const u8 * const bin_ptr);
  * @return a string for the specified status
  */
 const char *qed_dbg_get_status_str(enum dbg_status status);
+
 /**
  * @brief qed_get_idle_chk_results_buf_size - Returns the required buffer size
  *	for idle check results (in bytes).
@@ -3116,6 +3170,7 @@ enum dbg_status qed_print_idle_chk_results(struct qed_hwfn *p_hwfn,
 					   char *results_buf,
 					   u32 *num_errors,
 					   u32 *num_warnings);
+
 /**
  * @brief qed_get_mcp_trace_results_buf_size - Returns the required buffer size
  *	for MCP Trace results (in bytes).
@@ -3132,6 +3187,7 @@ enum dbg_status qed_get_mcp_trace_results_buf_size(struct qed_hwfn *p_hwfn,
 						   u32 *dump_buf,
 						   u32 num_dumped_dwords,
 						   u32 *results_buf_size);
+
 /**
  * @brief qed_print_mcp_trace_results - Prints MCP Trace results
  *
@@ -3146,6 +3202,7 @@ enum dbg_status qed_print_mcp_trace_results(struct qed_hwfn *p_hwfn,
 					    u32 *dump_buf,
 					    u32 num_dumped_dwords,
 					    char *results_buf);
+
 /**
  * @brief qed_get_reg_fifo_results_buf_size - Returns the required buffer size
  *	for reg_fifo results (in bytes).
@@ -3162,6 +3219,7 @@ enum dbg_status qed_get_reg_fifo_results_buf_size(struct qed_hwfn *p_hwfn,
 						  u32 *dump_buf,
 						  u32 num_dumped_dwords,
 						  u32 *results_buf_size);
+
 /**
  * @brief qed_print_reg_fifo_results - Prints reg fifo results
  *
@@ -3176,6 +3234,7 @@ enum dbg_status qed_print_reg_fifo_results(struct qed_hwfn *p_hwfn,
 					   u32 *dump_buf,
 					   u32 num_dumped_dwords,
 					   char *results_buf);
+
 /**
  * @brief qed_get_igu_fifo_results_buf_size - Returns the required buffer size
  *	for igu_fifo results (in bytes).
@@ -3192,6 +3251,7 @@ enum dbg_status qed_get_igu_fifo_results_buf_size(struct qed_hwfn *p_hwfn,
 						  u32 *dump_buf,
 						  u32 num_dumped_dwords,
 						  u32 *results_buf_size);
+
 /**
  * @brief qed_print_igu_fifo_results - Prints IGU fifo results
  *
@@ -3206,6 +3266,7 @@ enum dbg_status qed_print_igu_fifo_results(struct qed_hwfn *p_hwfn,
 					   u32 *dump_buf,
 					   u32 num_dumped_dwords,
 					   char *results_buf);
+
 /**
  * @brief qed_get_protection_override_results_buf_size - Returns the required
  *	buffer size for protection override results (in bytes).
@@ -3223,6 +3284,7 @@ qed_get_protection_override_results_buf_size(struct qed_hwfn *p_hwfn,
 					     u32 *dump_buf,
 					     u32 num_dumped_dwords,
 					     u32 *results_buf_size);
+
 /**
  * @brief qed_print_protection_override_results - Prints protection override
  *	results.
@@ -3238,6 +3300,7 @@ enum dbg_status qed_print_protection_override_results(struct qed_hwfn *p_hwfn,
 						      u32 *dump_buf,
 						      u32 num_dumped_dwords,
 						      char *results_buf);
+
 /**
  * @brief qed_get_fw_asserts_results_buf_size - Returns the required buffer size
  *	for FW Asserts results (in bytes).
@@ -3254,6 +3317,7 @@ enum dbg_status qed_get_fw_asserts_results_buf_size(struct qed_hwfn *p_hwfn,
 						    u32 *dump_buf,
 						    u32 num_dumped_dwords,
 						    u32 *results_buf_size);
+
 /**
  * @brief qed_print_fw_asserts_results - Prints FW Asserts results
  *
@@ -3268,6 +3332,283 @@ enum dbg_status qed_print_fw_asserts_results(struct qed_hwfn *p_hwfn,
 					     u32 *dump_buf,
 					     u32 num_dumped_dwords,
 					     char *results_buf);
+
+/**
+ * @brief qed_dbg_parse_attn - Parses and prints attention registers values in
+ * the specified results struct.
+ *
+ * @param p_hwfn -  HW device data
+ * @param results - Pointer to the attention read results
+ *
+ * @return error if one of the following holds:
+ *	- the version wasn't set
+ * Otherwise, returns ok.
+ */
+enum dbg_status qed_dbg_parse_attn(struct qed_hwfn *p_hwfn,
+				   struct dbg_attn_block_result *results);
+
+/* Debug Bus blocks */
+static const u32 dbg_bus_blocks[] = {
+	0x0000000f,		/* grc, bb, 15 lines */
+	0x0000000f,		/* grc, k2, 15 lines */
+	0x00000000,
+	0x00000000,		/* miscs, bb, 0 lines */
+	0x00000000,		/* miscs, k2, 0 lines */
+	0x00000000,
+	0x00000000,		/* misc, bb, 0 lines */
+	0x00000000,		/* misc, k2, 0 lines */
+	0x00000000,
+	0x00000000,		/* dbu, bb, 0 lines */
+	0x00000000,		/* dbu, k2, 0 lines */
+	0x00000000,
+	0x000f0127,		/* pglue_b, bb, 39 lines */
+	0x0036012a,		/* pglue_b, k2, 42 lines */
+	0x00000000,
+	0x00000000,		/* cnig, bb, 0 lines */
+	0x00120102,		/* cnig, k2, 2 lines */
+	0x00000000,
+	0x00000000,		/* cpmu, bb, 0 lines */
+	0x00000000,		/* cpmu, k2, 0 lines */
+	0x00000000,
+	0x00000001,		/* ncsi, bb, 1 lines */
+	0x00000001,		/* ncsi, k2, 1 lines */
+	0x00000000,
+	0x00000000,		/* opte, bb, 0 lines */
+	0x00000000,		/* opte, k2, 0 lines */
+	0x00000000,
+	0x00600085,		/* bmb, bb, 133 lines */
+	0x00600085,		/* bmb, k2, 133 lines */
+	0x00000000,
+	0x00000000,		/* pcie, bb, 0 lines */
+	0x00e50033,		/* pcie, k2, 51 lines */
+	0x00000000,
+	0x00000000,		/* mcp, bb, 0 lines */
+	0x00000000,		/* mcp, k2, 0 lines */
+	0x00000000,
+	0x01180009,		/* mcp2, bb, 9 lines */
+	0x01180009,		/* mcp2, k2, 9 lines */
+	0x00000000,
+	0x01210104,		/* pswhst, bb, 4 lines */
+	0x01210104,		/* pswhst, k2, 4 lines */
+	0x00000000,
+	0x01250103,		/* pswhst2, bb, 3 lines */
+	0x01250103,		/* pswhst2, k2, 3 lines */
+	0x00000000,
+	0x00340101,		/* pswrd, bb, 1 lines */
+	0x00340101,		/* pswrd, k2, 1 lines */
+	0x00000000,
+	0x01280119,		/* pswrd2, bb, 25 lines */
+	0x01280119,		/* pswrd2, k2, 25 lines */
+	0x00000000,
+	0x01410109,		/* pswwr, bb, 9 lines */
+	0x01410109,		/* pswwr, k2, 9 lines */
+	0x00000000,
+	0x00000000,		/* pswwr2, bb, 0 lines */
+	0x00000000,		/* pswwr2, k2, 0 lines */
+	0x00000000,
+	0x001c0001,		/* pswrq, bb, 1 lines */
+	0x001c0001,		/* pswrq, k2, 1 lines */
+	0x00000000,
+	0x014a0015,		/* pswrq2, bb, 21 lines */
+	0x014a0015,		/* pswrq2, k2, 21 lines */
+	0x00000000,
+	0x00000000,		/* pglcs, bb, 0 lines */
+	0x00120006,		/* pglcs, k2, 6 lines */
+	0x00000000,
+	0x00100001,		/* dmae, bb, 1 lines */
+	0x00100001,		/* dmae, k2, 1 lines */
+	0x00000000,
+	0x015f0105,		/* ptu, bb, 5 lines */
+	0x015f0105,		/* ptu, k2, 5 lines */
+	0x00000000,
+	0x01640120,		/* tcm, bb, 32 lines */
+	0x01640120,		/* tcm, k2, 32 lines */
+	0x00000000,
+	0x01640120,		/* mcm, bb, 32 lines */
+	0x01640120,		/* mcm, k2, 32 lines */
+	0x00000000,
+	0x01640120,		/* ucm, bb, 32 lines */
+	0x01640120,		/* ucm, k2, 32 lines */
+	0x00000000,
+	0x01640120,		/* xcm, bb, 32 lines */
+	0x01640120,		/* xcm, k2, 32 lines */
+	0x00000000,
+	0x01640120,		/* ycm, bb, 32 lines */
+	0x01640120,		/* ycm, k2, 32 lines */
+	0x00000000,
+	0x01640120,		/* pcm, bb, 32 lines */
+	0x01640120,		/* pcm, k2, 32 lines */
+	0x00000000,
+	0x01840062,		/* qm, bb, 98 lines */
+	0x01840062,		/* qm, k2, 98 lines */
+	0x00000000,
+	0x01e60021,		/* tm, bb, 33 lines */
+	0x01e60021,		/* tm, k2, 33 lines */
+	0x00000000,
+	0x02070107,		/* dorq, bb, 7 lines */
+	0x02070107,		/* dorq, k2, 7 lines */
+	0x00000000,
+	0x00600185,		/* brb, bb, 133 lines */
+	0x00600185,		/* brb, k2, 133 lines */
+	0x00000000,
+	0x020e0019,		/* src, bb, 25 lines */
+	0x020c001a,		/* src, k2, 26 lines */
+	0x00000000,
+	0x02270104,		/* prs, bb, 4 lines */
+	0x02270104,		/* prs, k2, 4 lines */
+	0x00000000,
+	0x022b0133,		/* tsdm, bb, 51 lines */
+	0x022b0133,		/* tsdm, k2, 51 lines */
+	0x00000000,
+	0x022b0133,		/* msdm, bb, 51 lines */
+	0x022b0133,		/* msdm, k2, 51 lines */
+	0x00000000,
+	0x022b0133,		/* usdm, bb, 51 lines */
+	0x022b0133,		/* usdm, k2, 51 lines */
+	0x00000000,
+	0x022b0133,		/* xsdm, bb, 51 lines */
+	0x022b0133,		/* xsdm, k2, 51 lines */
+	0x00000000,
+	0x022b0133,		/* ysdm, bb, 51 lines */
+	0x022b0133,		/* ysdm, k2, 51 lines */
+	0x00000000,
+	0x022b0133,		/* psdm, bb, 51 lines */
+	0x022b0133,		/* psdm, k2, 51 lines */
+	0x00000000,
+	0x025e010c,		/* tsem, bb, 12 lines */
+	0x025e010c,		/* tsem, k2, 12 lines */
+	0x00000000,
+	0x025e010c,		/* msem, bb, 12 lines */
+	0x025e010c,		/* msem, k2, 12 lines */
+	0x00000000,
+	0x025e010c,		/* usem, bb, 12 lines */
+	0x025e010c,		/* usem, k2, 12 lines */
+	0x00000000,
+	0x025e010c,		/* xsem, bb, 12 lines */
+	0x025e010c,		/* xsem, k2, 12 lines */
+	0x00000000,
+	0x025e010c,		/* ysem, bb, 12 lines */
+	0x025e010c,		/* ysem, k2, 12 lines */
+	0x00000000,
+	0x025e010c,		/* psem, bb, 12 lines */
+	0x025e010c,		/* psem, k2, 12 lines */
+	0x00000000,
+	0x026a000d,		/* rss, bb, 13 lines */
+	0x026a000d,		/* rss, k2, 13 lines */
+	0x00000000,
+	0x02770106,		/* tmld, bb, 6 lines */
+	0x02770106,		/* tmld, k2, 6 lines */
+	0x00000000,
+	0x027d0106,		/* muld, bb, 6 lines */
+	0x027d0106,		/* muld, k2, 6 lines */
+	0x00000000,
+	0x02770005,		/* yuld, bb, 5 lines */
+	0x02770005,		/* yuld, k2, 5 lines */
+	0x00000000,
+	0x02830107,		/* xyld, bb, 7 lines */
+	0x027d0107,		/* xyld, k2, 7 lines */
+	0x00000000,
+	0x00000000,		/* ptld, bb, 0 lines */
+	0x00000000,		/* ptld, k2, 0 lines */
+	0x00000000,
+	0x00000000,		/* ypld, bb, 0 lines */
+	0x00000000,		/* ypld, k2, 0 lines */
+	0x00000000,
+	0x028a010e,		/* prm, bb, 14 lines */
+	0x02980110,		/* prm, k2, 16 lines */
+	0x00000000,
+	0x02a8000d,		/* pbf_pb1, bb, 13 lines */
+	0x02a8000d,		/* pbf_pb1, k2, 13 lines */
+	0x00000000,
+	0x02a8000d,		/* pbf_pb2, bb, 13 lines */
+	0x02a8000d,		/* pbf_pb2, k2, 13 lines */
+	0x00000000,
+	0x02a8000d,		/* rpb, bb, 13 lines */
+	0x02a8000d,		/* rpb, k2, 13 lines */
+	0x00000000,
+	0x00600185,		/* btb, bb, 133 lines */
+	0x00600185,		/* btb, k2, 133 lines */
+	0x00000000,
+	0x02b50117,		/* pbf, bb, 23 lines */
+	0x02b50117,		/* pbf, k2, 23 lines */
+	0x00000000,
+	0x02cc0006,		/* rdif, bb, 6 lines */
+	0x02cc0006,		/* rdif, k2, 6 lines */
+	0x00000000,
+	0x02d20006,		/* tdif, bb, 6 lines */
+	0x02d20006,		/* tdif, k2, 6 lines */
+	0x00000000,
+	0x02d80003,		/* cdu, bb, 3 lines */
+	0x02db000e,		/* cdu, k2, 14 lines */
+	0x00000000,
+	0x02e9010d,		/* ccfc, bb, 13 lines */
+	0x02f60117,		/* ccfc, k2, 23 lines */
+	0x00000000,
+	0x02e9010d,		/* tcfc, bb, 13 lines */
+	0x02f60117,		/* tcfc, k2, 23 lines */
+	0x00000000,
+	0x030d0133,		/* igu, bb, 51 lines */
+	0x030d0133,		/* igu, k2, 51 lines */
+	0x00000000,
+	0x03400106,		/* cau, bb, 6 lines */
+	0x03400106,		/* cau, k2, 6 lines */
+	0x00000000,
+	0x00000000,		/* rgfs, bb, 0 lines */
+	0x00000000,		/* rgfs, k2, 0 lines */
+	0x00000000,
+	0x00000000,		/* rgsrc, bb, 0 lines */
+	0x00000000,		/* rgsrc, k2, 0 lines */
+	0x00000000,
+	0x00000000,		/* tgfs, bb, 0 lines */
+	0x00000000,		/* tgfs, k2, 0 lines */
+	0x00000000,
+	0x00000000,		/* tgsrc, bb, 0 lines */
+	0x00000000,		/* tgsrc, k2, 0 lines */
+	0x00000000,
+	0x00000000,		/* umac, bb, 0 lines */
+	0x00120006,		/* umac, k2, 6 lines */
+	0x00000000,
+	0x00000000,		/* xmac, bb, 0 lines */
+	0x00000000,		/* xmac, k2, 0 lines */
+	0x00000000,
+	0x00000000,		/* dbg, bb, 0 lines */
+	0x00000000,		/* dbg, k2, 0 lines */
+	0x00000000,
+	0x0346012b,		/* nig, bb, 43 lines */
+	0x0346011d,		/* nig, k2, 29 lines */
+	0x00000000,
+	0x00000000,		/* wol, bb, 0 lines */
+	0x001c0002,		/* wol, k2, 2 lines */
+	0x00000000,
+	0x00000000,		/* bmbn, bb, 0 lines */
+	0x00210008,		/* bmbn, k2, 8 lines */
+	0x00000000,
+	0x00000000,		/* ipc, bb, 0 lines */
+	0x00000000,		/* ipc, k2, 0 lines */
+	0x00000000,
+	0x00000000,		/* nwm, bb, 0 lines */
+	0x0371000b,		/* nwm, k2, 11 lines */
+	0x00000000,
+	0x00000000,		/* nws, bb, 0 lines */
+	0x037c0009,		/* nws, k2, 9 lines */
+	0x00000000,
+	0x00000000,		/* ms, bb, 0 lines */
+	0x00120004,		/* ms, k2, 4 lines */
+	0x00000000,
+	0x00000000,		/* phy_pcie, bb, 0 lines */
+	0x00e5001a,		/* phy_pcie, k2, 26 lines */
+	0x00000000,
+	0x00000000,		/* led, bb, 0 lines */
+	0x00000000,		/* led, k2, 0 lines */
+	0x00000000,
+	0x00000000,		/* avs_wrap, bb, 0 lines */
+	0x00000000,		/* avs_wrap, k2, 0 lines */
+	0x00000000,
+	0x00000000,		/* bar0_map, bb, 0 lines */
+	0x00000000,		/* bar0_map, k2, 0 lines */
+	0x00000000,
+};
+
 /* Win 2 */
 #define GTT_BAR0_MAP_REG_IGU_CMD	0x00f000UL
 
@@ -3589,37 +3930,37 @@ void qed_set_rfs_mode_enable(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 #define PSTORM_FCOE_TX_STATS_OFFSET(pf_id) \
 	(IRO[44].base + ((pf_id) * IRO[44].m1))
 
-static const struct iro iro_arr[47] = {
+static const struct iro iro_arr[49] = {
 	{0x0, 0x0, 0x0, 0x0, 0x8},
 	{0x4cb0, 0x80, 0x0, 0x0, 0x80},
-	{0x6318, 0x20, 0x0, 0x0, 0x20},
+	{0x6518, 0x20, 0x0, 0x0, 0x20},
 	{0xb00, 0x8, 0x0, 0x0, 0x4},
 	{0xa80, 0x8, 0x0, 0x0, 0x4},
 	{0x0, 0x8, 0x0, 0x0, 0x2},
 	{0x80, 0x8, 0x0, 0x0, 0x4},
 	{0x84, 0x8, 0x0, 0x0, 0x2},
-	{0x4bc0, 0x0, 0x0, 0x0, 0x78},
+	{0x4c40, 0x0, 0x0, 0x0, 0x78},
 	{0x3df0, 0x0, 0x0, 0x0, 0x78},
 	{0x29b0, 0x0, 0x0, 0x0, 0x78},
 	{0x4c38, 0x0, 0x0, 0x0, 0x78},
 	{0x4990, 0x0, 0x0, 0x0, 0x78},
-	{0x7e48, 0x0, 0x0, 0x0, 0x78},
+	{0x7f48, 0x0, 0x0, 0x0, 0x78},
 	{0xa28, 0x8, 0x0, 0x0, 0x8},
-	{0x60f8, 0x10, 0x0, 0x0, 0x10},
-	{0xb820, 0x30, 0x0, 0x0, 0x30},
+	{0x61f8, 0x10, 0x0, 0x0, 0x10},
+	{0xbd20, 0x30, 0x0, 0x0, 0x30},
 	{0x95b8, 0x30, 0x0, 0x0, 0x30},
 	{0x4b60, 0x80, 0x0, 0x0, 0x40},
 	{0x1f8, 0x4, 0x0, 0x0, 0x4},
 	{0x53a0, 0x80, 0x4, 0x0, 0x4},
-	{0xc8f0, 0x0, 0x0, 0x0, 0x4},
+	{0xc7c8, 0x0, 0x0, 0x0, 0x4},
 	{0x4ba0, 0x80, 0x0, 0x0, 0x20},
-	{0x8050, 0x40, 0x0, 0x0, 0x30},
-	{0xe770, 0x60, 0x0, 0x0, 0x60},
+	{0x8150, 0x40, 0x0, 0x0, 0x30},
+	{0xec70, 0x60, 0x0, 0x0, 0x60},
 	{0x2b48, 0x80, 0x0, 0x0, 0x38},
-	{0xf188, 0x78, 0x0, 0x0, 0x78},
+	{0xf1b0, 0x78, 0x0, 0x0, 0x78},
 	{0x1f8, 0x4, 0x0, 0x0, 0x4},
-	{0xacf0, 0x0, 0x0, 0x0, 0xf0},
-	{0xade0, 0x8, 0x0, 0x0, 0x8},
+	{0xaef8, 0x0, 0x0, 0x0, 0xf0},
+	{0xafe8, 0x8, 0x0, 0x0, 0x8},
 	{0x1f8, 0x8, 0x0, 0x0, 0x8},
 	{0xac0, 0x8, 0x0, 0x0, 0x8},
 	{0x2578, 0x8, 0x0, 0x0, 0x8},
@@ -3627,16 +3968,18 @@ static const struct iro iro_arr[47] = {
 	{0x0, 0x8, 0x0, 0x0, 0x8},
 	{0x200, 0x10, 0x8, 0x0, 0x8},
 	{0xb78, 0x10, 0x8, 0x0, 0x2},
-	{0xd888, 0x38, 0x0, 0x0, 0x24},
-	{0x12c38, 0x10, 0x0, 0x0, 0x8},
-	{0x11aa0, 0x38, 0x0, 0x0, 0x18},
-	{0xa8c0, 0x38, 0x0, 0x0, 0x10},
+	{0xd9a8, 0x38, 0x0, 0x0, 0x24},
+	{0x12988, 0x10, 0x0, 0x0, 0x8},
+	{0x11fa0, 0x38, 0x0, 0x0, 0x18},
+	{0xa580, 0x38, 0x0, 0x0, 0x10},
 	{0x86f8, 0x30, 0x0, 0x0, 0x18},
 	{0x101f8, 0x10, 0x0, 0x0, 0x10},
-	{0xdd08, 0x48, 0x0, 0x0, 0x38},
+	{0xde28, 0x48, 0x0, 0x0, 0x38},
 	{0x10660, 0x20, 0x0, 0x0, 0x20},
 	{0x2b80, 0x80, 0x0, 0x0, 0x10},
 	{0x5020, 0x10, 0x0, 0x0, 0x10},
+	{0xc9b0, 0x30, 0x0, 0x0, 0x10},
+	{0xeec0, 0x10, 0x0, 0x0, 0x10},
 };
 
 /* Runtime array offsets */
@@ -3724,361 +4067,359 @@ static const struct iro iro_arr[47] = {
 #define PSWRQ2_REG_CDUC_BLOCKS_FACTOR_RT_OFFSET	6697
 #define PSWRQ2_REG_VF_BASE_RT_OFFSET	6698
 #define PSWRQ2_REG_VF_LAST_ILT_RT_OFFSET	6699
-#define PSWRQ2_REG_WR_MBS0_RT_OFFSET	6700
-#define PSWRQ2_REG_RD_MBS0_RT_OFFSET	6701
-#define PSWRQ2_REG_DRAM_ALIGN_WR_RT_OFFSET	6702
-#define PSWRQ2_REG_DRAM_ALIGN_RD_RT_OFFSET	6703
-#define PSWRQ2_REG_ILT_MEMORY_RT_OFFSET	6704
+#define PSWRQ2_REG_DRAM_ALIGN_WR_RT_OFFSET	6700
+#define PSWRQ2_REG_DRAM_ALIGN_RD_RT_OFFSET	6701
+#define PSWRQ2_REG_ILT_MEMORY_RT_OFFSET	6702
 #define PSWRQ2_REG_ILT_MEMORY_RT_SIZE	22000
-#define PGLUE_REG_B_VF_BASE_RT_OFFSET	28704
-#define PGLUE_REG_B_MSDM_OFFSET_MASK_B_RT_OFFSET	28705
-#define PGLUE_REG_B_MSDM_VF_SHIFT_B_RT_OFFSET	28706
-#define PGLUE_REG_B_CACHE_LINE_SIZE_RT_OFFSET	28707
-#define PGLUE_REG_B_PF_BAR0_SIZE_RT_OFFSET	28708
-#define PGLUE_REG_B_PF_BAR1_SIZE_RT_OFFSET	28709
-#define PGLUE_REG_B_VF_BAR1_SIZE_RT_OFFSET	28710
-#define TM_REG_VF_ENABLE_CONN_RT_OFFSET	28711
-#define TM_REG_PF_ENABLE_CONN_RT_OFFSET	28712
-#define TM_REG_PF_ENABLE_TASK_RT_OFFSET	28713
-#define TM_REG_GROUP_SIZE_RESOLUTION_CONN_RT_OFFSET	28714
-#define TM_REG_GROUP_SIZE_RESOLUTION_TASK_RT_OFFSET	28715
-#define TM_REG_CONFIG_CONN_MEM_RT_OFFSET	28716
+#define PGLUE_REG_B_VF_BASE_RT_OFFSET	28702
+#define PGLUE_REG_B_MSDM_OFFSET_MASK_B_RT_OFFSET	28703
+#define PGLUE_REG_B_MSDM_VF_SHIFT_B_RT_OFFSET	28704
+#define PGLUE_REG_B_CACHE_LINE_SIZE_RT_OFFSET	28705
+#define PGLUE_REG_B_PF_BAR0_SIZE_RT_OFFSET	28706
+#define PGLUE_REG_B_PF_BAR1_SIZE_RT_OFFSET	28707
+#define PGLUE_REG_B_VF_BAR1_SIZE_RT_OFFSET	28708
+#define TM_REG_VF_ENABLE_CONN_RT_OFFSET	28709
+#define TM_REG_PF_ENABLE_CONN_RT_OFFSET	28710
+#define TM_REG_PF_ENABLE_TASK_RT_OFFSET	28711
+#define TM_REG_GROUP_SIZE_RESOLUTION_CONN_RT_OFFSET	28712
+#define TM_REG_GROUP_SIZE_RESOLUTION_TASK_RT_OFFSET	28713
+#define TM_REG_CONFIG_CONN_MEM_RT_OFFSET	28714
 #define TM_REG_CONFIG_CONN_MEM_RT_SIZE	416
-#define TM_REG_CONFIG_TASK_MEM_RT_OFFSET	29132
-#define TM_REG_CONFIG_TASK_MEM_RT_SIZE	512
-#define QM_REG_MAXPQSIZE_0_RT_OFFSET	29644
-#define QM_REG_MAXPQSIZE_1_RT_OFFSET	29645
-#define QM_REG_MAXPQSIZE_2_RT_OFFSET	29646
-#define QM_REG_MAXPQSIZETXSEL_0_RT_OFFSET	29647
-#define QM_REG_MAXPQSIZETXSEL_1_RT_OFFSET	29648
-#define QM_REG_MAXPQSIZETXSEL_2_RT_OFFSET	29649
-#define QM_REG_MAXPQSIZETXSEL_3_RT_OFFSET	29650
-#define QM_REG_MAXPQSIZETXSEL_4_RT_OFFSET	29651
-#define QM_REG_MAXPQSIZETXSEL_5_RT_OFFSET	29652
-#define QM_REG_MAXPQSIZETXSEL_6_RT_OFFSET	29653
-#define QM_REG_MAXPQSIZETXSEL_7_RT_OFFSET	29654
-#define QM_REG_MAXPQSIZETXSEL_8_RT_OFFSET	29655
-#define QM_REG_MAXPQSIZETXSEL_9_RT_OFFSET	29656
-#define QM_REG_MAXPQSIZETXSEL_10_RT_OFFSET	29657
-#define QM_REG_MAXPQSIZETXSEL_11_RT_OFFSET	29658
-#define QM_REG_MAXPQSIZETXSEL_12_RT_OFFSET	29659
-#define QM_REG_MAXPQSIZETXSEL_13_RT_OFFSET	29660
-#define QM_REG_MAXPQSIZETXSEL_14_RT_OFFSET	29661
-#define QM_REG_MAXPQSIZETXSEL_15_RT_OFFSET	29662
-#define QM_REG_MAXPQSIZETXSEL_16_RT_OFFSET	29663
-#define QM_REG_MAXPQSIZETXSEL_17_RT_OFFSET	29664
-#define QM_REG_MAXPQSIZETXSEL_18_RT_OFFSET	29665
-#define QM_REG_MAXPQSIZETXSEL_19_RT_OFFSET	29666
-#define QM_REG_MAXPQSIZETXSEL_20_RT_OFFSET	29667
-#define QM_REG_MAXPQSIZETXSEL_21_RT_OFFSET	29668
-#define QM_REG_MAXPQSIZETXSEL_22_RT_OFFSET	29669
-#define QM_REG_MAXPQSIZETXSEL_23_RT_OFFSET	29670
-#define QM_REG_MAXPQSIZETXSEL_24_RT_OFFSET	29671
-#define QM_REG_MAXPQSIZETXSEL_25_RT_OFFSET	29672
-#define QM_REG_MAXPQSIZETXSEL_26_RT_OFFSET	29673
-#define QM_REG_MAXPQSIZETXSEL_27_RT_OFFSET	29674
-#define QM_REG_MAXPQSIZETXSEL_28_RT_OFFSET	29675
-#define QM_REG_MAXPQSIZETXSEL_29_RT_OFFSET	29676
-#define QM_REG_MAXPQSIZETXSEL_30_RT_OFFSET	29677
-#define QM_REG_MAXPQSIZETXSEL_31_RT_OFFSET	29678
-#define QM_REG_MAXPQSIZETXSEL_32_RT_OFFSET	29679
-#define QM_REG_MAXPQSIZETXSEL_33_RT_OFFSET	29680
-#define QM_REG_MAXPQSIZETXSEL_34_RT_OFFSET	29681
-#define QM_REG_MAXPQSIZETXSEL_35_RT_OFFSET	29682
-#define QM_REG_MAXPQSIZETXSEL_36_RT_OFFSET	29683
-#define QM_REG_MAXPQSIZETXSEL_37_RT_OFFSET	29684
-#define QM_REG_MAXPQSIZETXSEL_38_RT_OFFSET	29685
-#define QM_REG_MAXPQSIZETXSEL_39_RT_OFFSET	29686
-#define QM_REG_MAXPQSIZETXSEL_40_RT_OFFSET	29687
-#define QM_REG_MAXPQSIZETXSEL_41_RT_OFFSET	29688
-#define QM_REG_MAXPQSIZETXSEL_42_RT_OFFSET	29689
-#define QM_REG_MAXPQSIZETXSEL_43_RT_OFFSET	29690
-#define QM_REG_MAXPQSIZETXSEL_44_RT_OFFSET	29691
-#define QM_REG_MAXPQSIZETXSEL_45_RT_OFFSET	29692
-#define QM_REG_MAXPQSIZETXSEL_46_RT_OFFSET	29693
-#define QM_REG_MAXPQSIZETXSEL_47_RT_OFFSET	29694
-#define QM_REG_MAXPQSIZETXSEL_48_RT_OFFSET	29695
-#define QM_REG_MAXPQSIZETXSEL_49_RT_OFFSET	29696
-#define QM_REG_MAXPQSIZETXSEL_50_RT_OFFSET	29697
-#define QM_REG_MAXPQSIZETXSEL_51_RT_OFFSET	29698
-#define QM_REG_MAXPQSIZETXSEL_52_RT_OFFSET	29699
-#define QM_REG_MAXPQSIZETXSEL_53_RT_OFFSET	29700
-#define QM_REG_MAXPQSIZETXSEL_54_RT_OFFSET	29701
-#define QM_REG_MAXPQSIZETXSEL_55_RT_OFFSET	29702
-#define QM_REG_MAXPQSIZETXSEL_56_RT_OFFSET	29703
-#define QM_REG_MAXPQSIZETXSEL_57_RT_OFFSET	29704
-#define QM_REG_MAXPQSIZETXSEL_58_RT_OFFSET	29705
-#define QM_REG_MAXPQSIZETXSEL_59_RT_OFFSET	29706
-#define QM_REG_MAXPQSIZETXSEL_60_RT_OFFSET	29707
-#define QM_REG_MAXPQSIZETXSEL_61_RT_OFFSET	29708
-#define QM_REG_MAXPQSIZETXSEL_62_RT_OFFSET	29709
-#define QM_REG_MAXPQSIZETXSEL_63_RT_OFFSET	29710
-#define QM_REG_BASEADDROTHERPQ_RT_OFFSET	29711
+#define TM_REG_CONFIG_TASK_MEM_RT_OFFSET	29130
+#define TM_REG_CONFIG_TASK_MEM_RT_SIZE	608
+#define QM_REG_MAXPQSIZE_0_RT_OFFSET	29738
+#define QM_REG_MAXPQSIZE_1_RT_OFFSET	29739
+#define QM_REG_MAXPQSIZE_2_RT_OFFSET	29740
+#define QM_REG_MAXPQSIZETXSEL_0_RT_OFFSET	29741
+#define QM_REG_MAXPQSIZETXSEL_1_RT_OFFSET	29742
+#define QM_REG_MAXPQSIZETXSEL_2_RT_OFFSET	29743
+#define QM_REG_MAXPQSIZETXSEL_3_RT_OFFSET	29744
+#define QM_REG_MAXPQSIZETXSEL_4_RT_OFFSET	29745
+#define QM_REG_MAXPQSIZETXSEL_5_RT_OFFSET	29746
+#define QM_REG_MAXPQSIZETXSEL_6_RT_OFFSET	29747
+#define QM_REG_MAXPQSIZETXSEL_7_RT_OFFSET	29748
+#define QM_REG_MAXPQSIZETXSEL_8_RT_OFFSET	29749
+#define QM_REG_MAXPQSIZETXSEL_9_RT_OFFSET	29750
+#define QM_REG_MAXPQSIZETXSEL_10_RT_OFFSET	29751
+#define QM_REG_MAXPQSIZETXSEL_11_RT_OFFSET	29752
+#define QM_REG_MAXPQSIZETXSEL_12_RT_OFFSET	29753
+#define QM_REG_MAXPQSIZETXSEL_13_RT_OFFSET	29754
+#define QM_REG_MAXPQSIZETXSEL_14_RT_OFFSET	29755
+#define QM_REG_MAXPQSIZETXSEL_15_RT_OFFSET	29756
+#define QM_REG_MAXPQSIZETXSEL_16_RT_OFFSET	29757
+#define QM_REG_MAXPQSIZETXSEL_17_RT_OFFSET	29758
+#define QM_REG_MAXPQSIZETXSEL_18_RT_OFFSET	29759
+#define QM_REG_MAXPQSIZETXSEL_19_RT_OFFSET	29760
+#define QM_REG_MAXPQSIZETXSEL_20_RT_OFFSET	29761
+#define QM_REG_MAXPQSIZETXSEL_21_RT_OFFSET	29762
+#define QM_REG_MAXPQSIZETXSEL_22_RT_OFFSET	29763
+#define QM_REG_MAXPQSIZETXSEL_23_RT_OFFSET	29764
+#define QM_REG_MAXPQSIZETXSEL_24_RT_OFFSET	29765
+#define QM_REG_MAXPQSIZETXSEL_25_RT_OFFSET	29766
+#define QM_REG_MAXPQSIZETXSEL_26_RT_OFFSET	29767
+#define QM_REG_MAXPQSIZETXSEL_27_RT_OFFSET	29768
+#define QM_REG_MAXPQSIZETXSEL_28_RT_OFFSET	29769
+#define QM_REG_MAXPQSIZETXSEL_29_RT_OFFSET	29770
+#define QM_REG_MAXPQSIZETXSEL_30_RT_OFFSET	29771
+#define QM_REG_MAXPQSIZETXSEL_31_RT_OFFSET	29772
+#define QM_REG_MAXPQSIZETXSEL_32_RT_OFFSET	29773
+#define QM_REG_MAXPQSIZETXSEL_33_RT_OFFSET	29774
+#define QM_REG_MAXPQSIZETXSEL_34_RT_OFFSET	29775
+#define QM_REG_MAXPQSIZETXSEL_35_RT_OFFSET	29776
+#define QM_REG_MAXPQSIZETXSEL_36_RT_OFFSET	29777
+#define QM_REG_MAXPQSIZETXSEL_37_RT_OFFSET	29778
+#define QM_REG_MAXPQSIZETXSEL_38_RT_OFFSET	29779
+#define QM_REG_MAXPQSIZETXSEL_39_RT_OFFSET	29780
+#define QM_REG_MAXPQSIZETXSEL_40_RT_OFFSET	29781
+#define QM_REG_MAXPQSIZETXSEL_41_RT_OFFSET	29782
+#define QM_REG_MAXPQSIZETXSEL_42_RT_OFFSET	29783
+#define QM_REG_MAXPQSIZETXSEL_43_RT_OFFSET	29784
+#define QM_REG_MAXPQSIZETXSEL_44_RT_OFFSET	29785
+#define QM_REG_MAXPQSIZETXSEL_45_RT_OFFSET	29786
+#define QM_REG_MAXPQSIZETXSEL_46_RT_OFFSET	29787
+#define QM_REG_MAXPQSIZETXSEL_47_RT_OFFSET	29788
+#define QM_REG_MAXPQSIZETXSEL_48_RT_OFFSET	29789
+#define QM_REG_MAXPQSIZETXSEL_49_RT_OFFSET	29790
+#define QM_REG_MAXPQSIZETXSEL_50_RT_OFFSET	29791
+#define QM_REG_MAXPQSIZETXSEL_51_RT_OFFSET	29792
+#define QM_REG_MAXPQSIZETXSEL_52_RT_OFFSET	29793
+#define QM_REG_MAXPQSIZETXSEL_53_RT_OFFSET	29794
+#define QM_REG_MAXPQSIZETXSEL_54_RT_OFFSET	29795
+#define QM_REG_MAXPQSIZETXSEL_55_RT_OFFSET	29796
+#define QM_REG_MAXPQSIZETXSEL_56_RT_OFFSET	29797
+#define QM_REG_MAXPQSIZETXSEL_57_RT_OFFSET	29798
+#define QM_REG_MAXPQSIZETXSEL_58_RT_OFFSET	29799
+#define QM_REG_MAXPQSIZETXSEL_59_RT_OFFSET	29800
+#define QM_REG_MAXPQSIZETXSEL_60_RT_OFFSET	29801
+#define QM_REG_MAXPQSIZETXSEL_61_RT_OFFSET	29802
+#define QM_REG_MAXPQSIZETXSEL_62_RT_OFFSET	29803
+#define QM_REG_MAXPQSIZETXSEL_63_RT_OFFSET	29804
+#define QM_REG_BASEADDROTHERPQ_RT_OFFSET	29805
 #define QM_REG_BASEADDROTHERPQ_RT_SIZE	128
-#define QM_REG_VOQCRDLINE_RT_OFFSET	29839
-#define QM_REG_VOQCRDLINE_RT_SIZE	20
-#define QM_REG_VOQINITCRDLINE_RT_OFFSET	29859
-#define QM_REG_VOQINITCRDLINE_RT_SIZE	20
-#define QM_REG_AFULLQMBYPTHRPFWFQ_RT_OFFSET	29879
-#define QM_REG_AFULLQMBYPTHRVPWFQ_RT_OFFSET	29880
-#define QM_REG_AFULLQMBYPTHRPFRL_RT_OFFSET	29881
-#define QM_REG_AFULLQMBYPTHRGLBLRL_RT_OFFSET	29882
-#define QM_REG_AFULLOPRTNSTCCRDMASK_RT_OFFSET	29883
-#define QM_REG_WRROTHERPQGRP_0_RT_OFFSET	29884
-#define QM_REG_WRROTHERPQGRP_1_RT_OFFSET	29885
-#define QM_REG_WRROTHERPQGRP_2_RT_OFFSET	29886
-#define QM_REG_WRROTHERPQGRP_3_RT_OFFSET	29887
-#define QM_REG_WRROTHERPQGRP_4_RT_OFFSET	29888
-#define QM_REG_WRROTHERPQGRP_5_RT_OFFSET	29889
-#define QM_REG_WRROTHERPQGRP_6_RT_OFFSET	29890
-#define QM_REG_WRROTHERPQGRP_7_RT_OFFSET	29891
-#define QM_REG_WRROTHERPQGRP_8_RT_OFFSET	29892
-#define QM_REG_WRROTHERPQGRP_9_RT_OFFSET	29893
-#define QM_REG_WRROTHERPQGRP_10_RT_OFFSET	29894
-#define QM_REG_WRROTHERPQGRP_11_RT_OFFSET	29895
-#define QM_REG_WRROTHERPQGRP_12_RT_OFFSET	29896
-#define QM_REG_WRROTHERPQGRP_13_RT_OFFSET	29897
-#define QM_REG_WRROTHERPQGRP_14_RT_OFFSET	29898
-#define QM_REG_WRROTHERPQGRP_15_RT_OFFSET	29899
-#define QM_REG_WRROTHERGRPWEIGHT_0_RT_OFFSET	29900
-#define QM_REG_WRROTHERGRPWEIGHT_1_RT_OFFSET	29901
-#define QM_REG_WRROTHERGRPWEIGHT_2_RT_OFFSET	29902
-#define QM_REG_WRROTHERGRPWEIGHT_3_RT_OFFSET	29903
-#define QM_REG_WRRTXGRPWEIGHT_0_RT_OFFSET	29904
-#define QM_REG_WRRTXGRPWEIGHT_1_RT_OFFSET	29905
-#define QM_REG_PQTX2PF_0_RT_OFFSET	29906
-#define QM_REG_PQTX2PF_1_RT_OFFSET	29907
-#define QM_REG_PQTX2PF_2_RT_OFFSET	29908
-#define QM_REG_PQTX2PF_3_RT_OFFSET	29909
-#define QM_REG_PQTX2PF_4_RT_OFFSET	29910
-#define QM_REG_PQTX2PF_5_RT_OFFSET	29911
-#define QM_REG_PQTX2PF_6_RT_OFFSET	29912
-#define QM_REG_PQTX2PF_7_RT_OFFSET	29913
-#define QM_REG_PQTX2PF_8_RT_OFFSET	29914
-#define QM_REG_PQTX2PF_9_RT_OFFSET	29915
-#define QM_REG_PQTX2PF_10_RT_OFFSET	29916
-#define QM_REG_PQTX2PF_11_RT_OFFSET	29917
-#define QM_REG_PQTX2PF_12_RT_OFFSET	29918
-#define QM_REG_PQTX2PF_13_RT_OFFSET	29919
-#define QM_REG_PQTX2PF_14_RT_OFFSET	29920
-#define QM_REG_PQTX2PF_15_RT_OFFSET	29921
-#define QM_REG_PQTX2PF_16_RT_OFFSET	29922
-#define QM_REG_PQTX2PF_17_RT_OFFSET	29923
-#define QM_REG_PQTX2PF_18_RT_OFFSET	29924
-#define QM_REG_PQTX2PF_19_RT_OFFSET	29925
-#define QM_REG_PQTX2PF_20_RT_OFFSET	29926
-#define QM_REG_PQTX2PF_21_RT_OFFSET	29927
-#define QM_REG_PQTX2PF_22_RT_OFFSET	29928
-#define QM_REG_PQTX2PF_23_RT_OFFSET	29929
-#define QM_REG_PQTX2PF_24_RT_OFFSET	29930
-#define QM_REG_PQTX2PF_25_RT_OFFSET	29931
-#define QM_REG_PQTX2PF_26_RT_OFFSET	29932
-#define QM_REG_PQTX2PF_27_RT_OFFSET	29933
-#define QM_REG_PQTX2PF_28_RT_OFFSET	29934
-#define QM_REG_PQTX2PF_29_RT_OFFSET	29935
-#define QM_REG_PQTX2PF_30_RT_OFFSET	29936
-#define QM_REG_PQTX2PF_31_RT_OFFSET	29937
-#define QM_REG_PQTX2PF_32_RT_OFFSET	29938
-#define QM_REG_PQTX2PF_33_RT_OFFSET	29939
-#define QM_REG_PQTX2PF_34_RT_OFFSET	29940
-#define QM_REG_PQTX2PF_35_RT_OFFSET	29941
-#define QM_REG_PQTX2PF_36_RT_OFFSET	29942
-#define QM_REG_PQTX2PF_37_RT_OFFSET	29943
-#define QM_REG_PQTX2PF_38_RT_OFFSET	29944
-#define QM_REG_PQTX2PF_39_RT_OFFSET	29945
-#define QM_REG_PQTX2PF_40_RT_OFFSET	29946
-#define QM_REG_PQTX2PF_41_RT_OFFSET	29947
-#define QM_REG_PQTX2PF_42_RT_OFFSET	29948
-#define QM_REG_PQTX2PF_43_RT_OFFSET	29949
-#define QM_REG_PQTX2PF_44_RT_OFFSET	29950
-#define QM_REG_PQTX2PF_45_RT_OFFSET	29951
-#define QM_REG_PQTX2PF_46_RT_OFFSET	29952
-#define QM_REG_PQTX2PF_47_RT_OFFSET	29953
-#define QM_REG_PQTX2PF_48_RT_OFFSET	29954
-#define QM_REG_PQTX2PF_49_RT_OFFSET	29955
-#define QM_REG_PQTX2PF_50_RT_OFFSET	29956
-#define QM_REG_PQTX2PF_51_RT_OFFSET	29957
-#define QM_REG_PQTX2PF_52_RT_OFFSET	29958
-#define QM_REG_PQTX2PF_53_RT_OFFSET	29959
-#define QM_REG_PQTX2PF_54_RT_OFFSET	29960
-#define QM_REG_PQTX2PF_55_RT_OFFSET	29961
-#define QM_REG_PQTX2PF_56_RT_OFFSET	29962
-#define QM_REG_PQTX2PF_57_RT_OFFSET	29963
-#define QM_REG_PQTX2PF_58_RT_OFFSET	29964
-#define QM_REG_PQTX2PF_59_RT_OFFSET	29965
-#define QM_REG_PQTX2PF_60_RT_OFFSET	29966
-#define QM_REG_PQTX2PF_61_RT_OFFSET	29967
-#define QM_REG_PQTX2PF_62_RT_OFFSET	29968
-#define QM_REG_PQTX2PF_63_RT_OFFSET	29969
-#define QM_REG_PQOTHER2PF_0_RT_OFFSET	29970
-#define QM_REG_PQOTHER2PF_1_RT_OFFSET	29971
-#define QM_REG_PQOTHER2PF_2_RT_OFFSET	29972
-#define QM_REG_PQOTHER2PF_3_RT_OFFSET	29973
-#define QM_REG_PQOTHER2PF_4_RT_OFFSET	29974
-#define QM_REG_PQOTHER2PF_5_RT_OFFSET	29975
-#define QM_REG_PQOTHER2PF_6_RT_OFFSET	29976
-#define QM_REG_PQOTHER2PF_7_RT_OFFSET	29977
-#define QM_REG_PQOTHER2PF_8_RT_OFFSET	29978
-#define QM_REG_PQOTHER2PF_9_RT_OFFSET	29979
-#define QM_REG_PQOTHER2PF_10_RT_OFFSET	29980
-#define QM_REG_PQOTHER2PF_11_RT_OFFSET	29981
-#define QM_REG_PQOTHER2PF_12_RT_OFFSET	29982
-#define QM_REG_PQOTHER2PF_13_RT_OFFSET	29983
-#define QM_REG_PQOTHER2PF_14_RT_OFFSET	29984
-#define QM_REG_PQOTHER2PF_15_RT_OFFSET	29985
-#define QM_REG_RLGLBLPERIOD_0_RT_OFFSET	29986
-#define QM_REG_RLGLBLPERIOD_1_RT_OFFSET	29987
-#define QM_REG_RLGLBLPERIODTIMER_0_RT_OFFSET	29988
-#define QM_REG_RLGLBLPERIODTIMER_1_RT_OFFSET	29989
-#define QM_REG_RLGLBLPERIODSEL_0_RT_OFFSET	29990
-#define QM_REG_RLGLBLPERIODSEL_1_RT_OFFSET	29991
-#define QM_REG_RLGLBLPERIODSEL_2_RT_OFFSET	29992
-#define QM_REG_RLGLBLPERIODSEL_3_RT_OFFSET	29993
-#define QM_REG_RLGLBLPERIODSEL_4_RT_OFFSET	29994
-#define QM_REG_RLGLBLPERIODSEL_5_RT_OFFSET	29995
-#define QM_REG_RLGLBLPERIODSEL_6_RT_OFFSET	29996
-#define QM_REG_RLGLBLPERIODSEL_7_RT_OFFSET	29997
-#define QM_REG_RLGLBLINCVAL_RT_OFFSET	29998
+#define QM_REG_AFULLQMBYPTHRPFWFQ_RT_OFFSET	29933
+#define QM_REG_AFULLQMBYPTHRVPWFQ_RT_OFFSET	29934
+#define QM_REG_AFULLQMBYPTHRPFRL_RT_OFFSET	29935
+#define QM_REG_AFULLQMBYPTHRGLBLRL_RT_OFFSET	29936
+#define QM_REG_AFULLOPRTNSTCCRDMASK_RT_OFFSET	29937
+#define QM_REG_WRROTHERPQGRP_0_RT_OFFSET	29938
+#define QM_REG_WRROTHERPQGRP_1_RT_OFFSET	29939
+#define QM_REG_WRROTHERPQGRP_2_RT_OFFSET	29940
+#define QM_REG_WRROTHERPQGRP_3_RT_OFFSET	29941
+#define QM_REG_WRROTHERPQGRP_4_RT_OFFSET	29942
+#define QM_REG_WRROTHERPQGRP_5_RT_OFFSET	29943
+#define QM_REG_WRROTHERPQGRP_6_RT_OFFSET	29944
+#define QM_REG_WRROTHERPQGRP_7_RT_OFFSET	29945
+#define QM_REG_WRROTHERPQGRP_8_RT_OFFSET	29946
+#define QM_REG_WRROTHERPQGRP_9_RT_OFFSET	29947
+#define QM_REG_WRROTHERPQGRP_10_RT_OFFSET	29948
+#define QM_REG_WRROTHERPQGRP_11_RT_OFFSET	29949
+#define QM_REG_WRROTHERPQGRP_12_RT_OFFSET	29950
+#define QM_REG_WRROTHERPQGRP_13_RT_OFFSET	29951
+#define QM_REG_WRROTHERPQGRP_14_RT_OFFSET	29952
+#define QM_REG_WRROTHERPQGRP_15_RT_OFFSET	29953
+#define QM_REG_WRROTHERGRPWEIGHT_0_RT_OFFSET	29954
+#define QM_REG_WRROTHERGRPWEIGHT_1_RT_OFFSET	29955
+#define QM_REG_WRROTHERGRPWEIGHT_2_RT_OFFSET	29956
+#define QM_REG_WRROTHERGRPWEIGHT_3_RT_OFFSET	29957
+#define QM_REG_WRRTXGRPWEIGHT_0_RT_OFFSET	29958
+#define QM_REG_WRRTXGRPWEIGHT_1_RT_OFFSET	29959
+#define QM_REG_PQTX2PF_0_RT_OFFSET	29960
+#define QM_REG_PQTX2PF_1_RT_OFFSET	29961
+#define QM_REG_PQTX2PF_2_RT_OFFSET	29962
+#define QM_REG_PQTX2PF_3_RT_OFFSET	29963
+#define QM_REG_PQTX2PF_4_RT_OFFSET	29964
+#define QM_REG_PQTX2PF_5_RT_OFFSET	29965
+#define QM_REG_PQTX2PF_6_RT_OFFSET	29966
+#define QM_REG_PQTX2PF_7_RT_OFFSET	29967
+#define QM_REG_PQTX2PF_8_RT_OFFSET	29968
+#define QM_REG_PQTX2PF_9_RT_OFFSET	29969
+#define QM_REG_PQTX2PF_10_RT_OFFSET	29970
+#define QM_REG_PQTX2PF_11_RT_OFFSET	29971
+#define QM_REG_PQTX2PF_12_RT_OFFSET	29972
+#define QM_REG_PQTX2PF_13_RT_OFFSET	29973
+#define QM_REG_PQTX2PF_14_RT_OFFSET	29974
+#define QM_REG_PQTX2PF_15_RT_OFFSET	29975
+#define QM_REG_PQTX2PF_16_RT_OFFSET	29976
+#define QM_REG_PQTX2PF_17_RT_OFFSET	29977
+#define QM_REG_PQTX2PF_18_RT_OFFSET	29978
+#define QM_REG_PQTX2PF_19_RT_OFFSET	29979
+#define QM_REG_PQTX2PF_20_RT_OFFSET	29980
+#define QM_REG_PQTX2PF_21_RT_OFFSET	29981
+#define QM_REG_PQTX2PF_22_RT_OFFSET	29982
+#define QM_REG_PQTX2PF_23_RT_OFFSET	29983
+#define QM_REG_PQTX2PF_24_RT_OFFSET	29984
+#define QM_REG_PQTX2PF_25_RT_OFFSET	29985
+#define QM_REG_PQTX2PF_26_RT_OFFSET	29986
+#define QM_REG_PQTX2PF_27_RT_OFFSET	29987
+#define QM_REG_PQTX2PF_28_RT_OFFSET	29988
+#define QM_REG_PQTX2PF_29_RT_OFFSET	29989
+#define QM_REG_PQTX2PF_30_RT_OFFSET	29990
+#define QM_REG_PQTX2PF_31_RT_OFFSET	29991
+#define QM_REG_PQTX2PF_32_RT_OFFSET	29992
+#define QM_REG_PQTX2PF_33_RT_OFFSET	29993
+#define QM_REG_PQTX2PF_34_RT_OFFSET	29994
+#define QM_REG_PQTX2PF_35_RT_OFFSET	29995
+#define QM_REG_PQTX2PF_36_RT_OFFSET	29996
+#define QM_REG_PQTX2PF_37_RT_OFFSET	29997
+#define QM_REG_PQTX2PF_38_RT_OFFSET	29998
+#define QM_REG_PQTX2PF_39_RT_OFFSET	29999
+#define QM_REG_PQTX2PF_40_RT_OFFSET	30000
+#define QM_REG_PQTX2PF_41_RT_OFFSET	30001
+#define QM_REG_PQTX2PF_42_RT_OFFSET	30002
+#define QM_REG_PQTX2PF_43_RT_OFFSET	30003
+#define QM_REG_PQTX2PF_44_RT_OFFSET	30004
+#define QM_REG_PQTX2PF_45_RT_OFFSET	30005
+#define QM_REG_PQTX2PF_46_RT_OFFSET	30006
+#define QM_REG_PQTX2PF_47_RT_OFFSET	30007
+#define QM_REG_PQTX2PF_48_RT_OFFSET	30008
+#define QM_REG_PQTX2PF_49_RT_OFFSET	30009
+#define QM_REG_PQTX2PF_50_RT_OFFSET	30010
+#define QM_REG_PQTX2PF_51_RT_OFFSET	30011
+#define QM_REG_PQTX2PF_52_RT_OFFSET	30012
+#define QM_REG_PQTX2PF_53_RT_OFFSET	30013
+#define QM_REG_PQTX2PF_54_RT_OFFSET	30014
+#define QM_REG_PQTX2PF_55_RT_OFFSET	30015
+#define QM_REG_PQTX2PF_56_RT_OFFSET	30016
+#define QM_REG_PQTX2PF_57_RT_OFFSET	30017
+#define QM_REG_PQTX2PF_58_RT_OFFSET	30018
+#define QM_REG_PQTX2PF_59_RT_OFFSET	30019
+#define QM_REG_PQTX2PF_60_RT_OFFSET	30020
+#define QM_REG_PQTX2PF_61_RT_OFFSET	30021
+#define QM_REG_PQTX2PF_62_RT_OFFSET	30022
+#define QM_REG_PQTX2PF_63_RT_OFFSET	30023
+#define QM_REG_PQOTHER2PF_0_RT_OFFSET	30024
+#define QM_REG_PQOTHER2PF_1_RT_OFFSET	30025
+#define QM_REG_PQOTHER2PF_2_RT_OFFSET	30026
+#define QM_REG_PQOTHER2PF_3_RT_OFFSET	30027
+#define QM_REG_PQOTHER2PF_4_RT_OFFSET	30028
+#define QM_REG_PQOTHER2PF_5_RT_OFFSET	30029
+#define QM_REG_PQOTHER2PF_6_RT_OFFSET	30030
+#define QM_REG_PQOTHER2PF_7_RT_OFFSET	30031
+#define QM_REG_PQOTHER2PF_8_RT_OFFSET	30032
+#define QM_REG_PQOTHER2PF_9_RT_OFFSET	30033
+#define QM_REG_PQOTHER2PF_10_RT_OFFSET	30034
+#define QM_REG_PQOTHER2PF_11_RT_OFFSET	30035
+#define QM_REG_PQOTHER2PF_12_RT_OFFSET	30036
+#define QM_REG_PQOTHER2PF_13_RT_OFFSET	30037
+#define QM_REG_PQOTHER2PF_14_RT_OFFSET	30038
+#define QM_REG_PQOTHER2PF_15_RT_OFFSET	30039
+#define QM_REG_RLGLBLPERIOD_0_RT_OFFSET	30040
+#define QM_REG_RLGLBLPERIOD_1_RT_OFFSET	30041
+#define QM_REG_RLGLBLPERIODTIMER_0_RT_OFFSET	30042
+#define QM_REG_RLGLBLPERIODTIMER_1_RT_OFFSET	30043
+#define QM_REG_RLGLBLPERIODSEL_0_RT_OFFSET	30044
+#define QM_REG_RLGLBLPERIODSEL_1_RT_OFFSET	30045
+#define QM_REG_RLGLBLPERIODSEL_2_RT_OFFSET	30046
+#define QM_REG_RLGLBLPERIODSEL_3_RT_OFFSET	30047
+#define QM_REG_RLGLBLPERIODSEL_4_RT_OFFSET	30048
+#define QM_REG_RLGLBLPERIODSEL_5_RT_OFFSET	30049
+#define QM_REG_RLGLBLPERIODSEL_6_RT_OFFSET	30050
+#define QM_REG_RLGLBLPERIODSEL_7_RT_OFFSET	30051
+#define QM_REG_RLGLBLINCVAL_RT_OFFSET	30052
 #define QM_REG_RLGLBLINCVAL_RT_SIZE	256
-#define QM_REG_RLGLBLUPPERBOUND_RT_OFFSET	30254
+#define QM_REG_RLGLBLUPPERBOUND_RT_OFFSET	30308
 #define QM_REG_RLGLBLUPPERBOUND_RT_SIZE	256
-#define QM_REG_RLGLBLCRD_RT_OFFSET	30510
+#define QM_REG_RLGLBLCRD_RT_OFFSET	30564
 #define QM_REG_RLGLBLCRD_RT_SIZE	256
-#define QM_REG_RLGLBLENABLE_RT_OFFSET	30766
-#define QM_REG_RLPFPERIOD_RT_OFFSET	30767
-#define QM_REG_RLPFPERIODTIMER_RT_OFFSET	30768
-#define QM_REG_RLPFINCVAL_RT_OFFSET	30769
+#define QM_REG_RLGLBLENABLE_RT_OFFSET	30820
+#define QM_REG_RLPFPERIOD_RT_OFFSET	30821
+#define QM_REG_RLPFPERIODTIMER_RT_OFFSET	30822
+#define QM_REG_RLPFINCVAL_RT_OFFSET	30823
 #define QM_REG_RLPFINCVAL_RT_SIZE	16
-#define QM_REG_RLPFUPPERBOUND_RT_OFFSET	30785
+#define QM_REG_RLPFUPPERBOUND_RT_OFFSET	30839
 #define QM_REG_RLPFUPPERBOUND_RT_SIZE	16
-#define QM_REG_RLPFCRD_RT_OFFSET	30801
+#define QM_REG_RLPFCRD_RT_OFFSET	30855
 #define QM_REG_RLPFCRD_RT_SIZE	16
-#define QM_REG_RLPFENABLE_RT_OFFSET	30817
-#define QM_REG_RLPFVOQENABLE_RT_OFFSET	30818
-#define QM_REG_WFQPFWEIGHT_RT_OFFSET	30819
+#define QM_REG_RLPFENABLE_RT_OFFSET	30871
+#define QM_REG_RLPFVOQENABLE_RT_OFFSET	30872
+#define QM_REG_WFQPFWEIGHT_RT_OFFSET	30873
 #define QM_REG_WFQPFWEIGHT_RT_SIZE	16
-#define QM_REG_WFQPFUPPERBOUND_RT_OFFSET	30835
+#define QM_REG_WFQPFUPPERBOUND_RT_OFFSET	30889
 #define QM_REG_WFQPFUPPERBOUND_RT_SIZE	16
-#define QM_REG_WFQPFCRD_RT_OFFSET	30851
-#define QM_REG_WFQPFCRD_RT_SIZE	160
-#define QM_REG_WFQPFENABLE_RT_OFFSET	31011
-#define QM_REG_WFQVPENABLE_RT_OFFSET	31012
-#define QM_REG_BASEADDRTXPQ_RT_OFFSET	31013
+#define QM_REG_WFQPFCRD_RT_OFFSET	30905
+#define QM_REG_WFQPFCRD_RT_SIZE	256
+#define QM_REG_WFQPFENABLE_RT_OFFSET	31161
+#define QM_REG_WFQVPENABLE_RT_OFFSET	31162
+#define QM_REG_BASEADDRTXPQ_RT_OFFSET	31163
 #define QM_REG_BASEADDRTXPQ_RT_SIZE	512
-#define QM_REG_TXPQMAP_RT_OFFSET	31525
+#define QM_REG_TXPQMAP_RT_OFFSET	31675
 #define QM_REG_TXPQMAP_RT_SIZE	512
-#define QM_REG_WFQVPWEIGHT_RT_OFFSET	32037
+#define QM_REG_WFQVPWEIGHT_RT_OFFSET	32187
 #define QM_REG_WFQVPWEIGHT_RT_SIZE	512
-#define QM_REG_WFQVPCRD_RT_OFFSET	32549
+#define QM_REG_WFQVPCRD_RT_OFFSET	32699
 #define QM_REG_WFQVPCRD_RT_SIZE	512
-#define QM_REG_WFQVPMAP_RT_OFFSET	33061
+#define QM_REG_WFQVPMAP_RT_OFFSET	33211
 #define QM_REG_WFQVPMAP_RT_SIZE	512
-#define QM_REG_WFQPFCRD_MSB_RT_OFFSET	33573
-#define QM_REG_WFQPFCRD_MSB_RT_SIZE	160
-#define NIG_REG_TAG_ETHERTYPE_0_RT_OFFSET	33733
-#define NIG_REG_OUTER_TAG_VALUE_LIST0_RT_OFFSET	33734
-#define NIG_REG_OUTER_TAG_VALUE_LIST1_RT_OFFSET	33735
-#define NIG_REG_OUTER_TAG_VALUE_LIST2_RT_OFFSET	33736
-#define NIG_REG_OUTER_TAG_VALUE_LIST3_RT_OFFSET	33737
-#define NIG_REG_OUTER_TAG_VALUE_MASK_RT_OFFSET	33738
-#define NIG_REG_LLH_FUNC_TAGMAC_CLS_TYPE_RT_OFFSET	33739
-#define NIG_REG_LLH_FUNC_TAG_EN_RT_OFFSET	33740
+#define QM_REG_WFQPFCRD_MSB_RT_OFFSET	33723
+#define QM_REG_WFQPFCRD_MSB_RT_SIZE	320
+#define QM_REG_VOQCRDLINE_RT_OFFSET	34043
+#define QM_REG_VOQCRDLINE_RT_SIZE	36
+#define QM_REG_VOQINITCRDLINE_RT_OFFSET	34079
+#define QM_REG_VOQINITCRDLINE_RT_SIZE	36
+#define NIG_REG_TAG_ETHERTYPE_0_RT_OFFSET	34115
+#define NIG_REG_OUTER_TAG_VALUE_LIST0_RT_OFFSET	34116
+#define NIG_REG_OUTER_TAG_VALUE_LIST1_RT_OFFSET	34117
+#define NIG_REG_OUTER_TAG_VALUE_LIST2_RT_OFFSET	34118
+#define NIG_REG_OUTER_TAG_VALUE_LIST3_RT_OFFSET	34119
+#define NIG_REG_OUTER_TAG_VALUE_MASK_RT_OFFSET	34120
+#define NIG_REG_LLH_FUNC_TAGMAC_CLS_TYPE_RT_OFFSET	34121
+#define NIG_REG_LLH_FUNC_TAG_EN_RT_OFFSET	34122
 #define NIG_REG_LLH_FUNC_TAG_EN_RT_SIZE	4
-#define NIG_REG_LLH_FUNC_TAG_HDR_SEL_RT_OFFSET	33744
+#define NIG_REG_LLH_FUNC_TAG_HDR_SEL_RT_OFFSET	34126
 #define NIG_REG_LLH_FUNC_TAG_HDR_SEL_RT_SIZE	4
-#define NIG_REG_LLH_FUNC_TAG_VALUE_RT_OFFSET	33748
+#define NIG_REG_LLH_FUNC_TAG_VALUE_RT_OFFSET	34130
 #define NIG_REG_LLH_FUNC_TAG_VALUE_RT_SIZE	4
-#define NIG_REG_LLH_FUNC_NO_TAG_RT_OFFSET	33752
-#define NIG_REG_LLH_FUNC_FILTER_VALUE_RT_OFFSET	33753
+#define NIG_REG_LLH_FUNC_NO_TAG_RT_OFFSET	34134
+#define NIG_REG_LLH_FUNC_FILTER_VALUE_RT_OFFSET	34135
 #define NIG_REG_LLH_FUNC_FILTER_VALUE_RT_SIZE	32
-#define NIG_REG_LLH_FUNC_FILTER_EN_RT_OFFSET	33785
+#define NIG_REG_LLH_FUNC_FILTER_EN_RT_OFFSET	34167
 #define NIG_REG_LLH_FUNC_FILTER_EN_RT_SIZE	16
-#define NIG_REG_LLH_FUNC_FILTER_MODE_RT_OFFSET	33801
+#define NIG_REG_LLH_FUNC_FILTER_MODE_RT_OFFSET	34183
 #define NIG_REG_LLH_FUNC_FILTER_MODE_RT_SIZE	16
-#define NIG_REG_LLH_FUNC_FILTER_PROTOCOL_TYPE_RT_OFFSET	33817
+#define NIG_REG_LLH_FUNC_FILTER_PROTOCOL_TYPE_RT_OFFSET	34199
 #define NIG_REG_LLH_FUNC_FILTER_PROTOCOL_TYPE_RT_SIZE	16
-#define NIG_REG_LLH_FUNC_FILTER_HDR_SEL_RT_OFFSET	33833
+#define NIG_REG_LLH_FUNC_FILTER_HDR_SEL_RT_OFFSET	34215
 #define NIG_REG_LLH_FUNC_FILTER_HDR_SEL_RT_SIZE	16
-#define NIG_REG_TX_EDPM_CTRL_RT_OFFSET	33849
-#define NIG_REG_ROCE_DUPLICATE_TO_HOST_RT_OFFSET	33850
-#define CDU_REG_CID_ADDR_PARAMS_RT_OFFSET	33851
-#define CDU_REG_SEGMENT0_PARAMS_RT_OFFSET	33852
-#define CDU_REG_SEGMENT1_PARAMS_RT_OFFSET	33853
-#define CDU_REG_PF_SEG0_TYPE_OFFSET_RT_OFFSET	33854
-#define CDU_REG_PF_SEG1_TYPE_OFFSET_RT_OFFSET	33855
-#define CDU_REG_PF_SEG2_TYPE_OFFSET_RT_OFFSET	33856
-#define CDU_REG_PF_SEG3_TYPE_OFFSET_RT_OFFSET	33857
-#define CDU_REG_PF_FL_SEG0_TYPE_OFFSET_RT_OFFSET	33858
-#define CDU_REG_PF_FL_SEG1_TYPE_OFFSET_RT_OFFSET	33859
-#define CDU_REG_PF_FL_SEG2_TYPE_OFFSET_RT_OFFSET	33860
-#define CDU_REG_PF_FL_SEG3_TYPE_OFFSET_RT_OFFSET	33861
-#define CDU_REG_VF_SEG_TYPE_OFFSET_RT_OFFSET	33862
-#define CDU_REG_VF_FL_SEG_TYPE_OFFSET_RT_OFFSET	33863
-#define PBF_REG_TAG_ETHERTYPE_0_RT_OFFSET	33864
-#define PBF_REG_BTB_SHARED_AREA_SIZE_RT_OFFSET	33865
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ0_RT_OFFSET	33866
-#define PBF_REG_BTB_GUARANTEED_VOQ0_RT_OFFSET	33867
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ0_RT_OFFSET	33868
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ1_RT_OFFSET	33869
-#define PBF_REG_BTB_GUARANTEED_VOQ1_RT_OFFSET	33870
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ1_RT_OFFSET	33871
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ2_RT_OFFSET	33872
-#define PBF_REG_BTB_GUARANTEED_VOQ2_RT_OFFSET	33873
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ2_RT_OFFSET	33874
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ3_RT_OFFSET	33875
-#define PBF_REG_BTB_GUARANTEED_VOQ3_RT_OFFSET	33876
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ3_RT_OFFSET	33877
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ4_RT_OFFSET	33878
-#define PBF_REG_BTB_GUARANTEED_VOQ4_RT_OFFSET	33879
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ4_RT_OFFSET	33880
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ5_RT_OFFSET	33881
-#define PBF_REG_BTB_GUARANTEED_VOQ5_RT_OFFSET	33882
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ5_RT_OFFSET	33883
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ6_RT_OFFSET	33884
-#define PBF_REG_BTB_GUARANTEED_VOQ6_RT_OFFSET	33885
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ6_RT_OFFSET	33886
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ7_RT_OFFSET	33887
-#define PBF_REG_BTB_GUARANTEED_VOQ7_RT_OFFSET	33888
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ7_RT_OFFSET	33889
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ8_RT_OFFSET	33890
-#define PBF_REG_BTB_GUARANTEED_VOQ8_RT_OFFSET	33891
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ8_RT_OFFSET	33892
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ9_RT_OFFSET	33893
-#define PBF_REG_BTB_GUARANTEED_VOQ9_RT_OFFSET	33894
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ9_RT_OFFSET	33895
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ10_RT_OFFSET	33896
-#define PBF_REG_BTB_GUARANTEED_VOQ10_RT_OFFSET	33897
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ10_RT_OFFSET	33898
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ11_RT_OFFSET	33899
-#define PBF_REG_BTB_GUARANTEED_VOQ11_RT_OFFSET	33900
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ11_RT_OFFSET	33901
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ12_RT_OFFSET	33902
-#define PBF_REG_BTB_GUARANTEED_VOQ12_RT_OFFSET	33903
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ12_RT_OFFSET	33904
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ13_RT_OFFSET	33905
-#define PBF_REG_BTB_GUARANTEED_VOQ13_RT_OFFSET	33906
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ13_RT_OFFSET	33907
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ14_RT_OFFSET	33908
-#define PBF_REG_BTB_GUARANTEED_VOQ14_RT_OFFSET	33909
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ14_RT_OFFSET	33910
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ15_RT_OFFSET	33911
-#define PBF_REG_BTB_GUARANTEED_VOQ15_RT_OFFSET	33912
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ15_RT_OFFSET	33913
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ16_RT_OFFSET	33914
-#define PBF_REG_BTB_GUARANTEED_VOQ16_RT_OFFSET	33915
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ16_RT_OFFSET	33916
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ17_RT_OFFSET	33917
-#define PBF_REG_BTB_GUARANTEED_VOQ17_RT_OFFSET	33918
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ17_RT_OFFSET	33919
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ18_RT_OFFSET	33920
-#define PBF_REG_BTB_GUARANTEED_VOQ18_RT_OFFSET	33921
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ18_RT_OFFSET	33922
-#define PBF_REG_YCMD_QS_NUM_LINES_VOQ19_RT_OFFSET	33923
-#define PBF_REG_BTB_GUARANTEED_VOQ19_RT_OFFSET	33924
-#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ19_RT_OFFSET	33925
-#define XCM_REG_CON_PHY_Q3_RT_OFFSET	33926
-
-#define RUNTIME_ARRAY_SIZE 33927
+#define NIG_REG_TX_EDPM_CTRL_RT_OFFSET	34231
+#define NIG_REG_ROCE_DUPLICATE_TO_HOST_RT_OFFSET	34232
+#define CDU_REG_CID_ADDR_PARAMS_RT_OFFSET	34233
+#define CDU_REG_SEGMENT0_PARAMS_RT_OFFSET	34234
+#define CDU_REG_SEGMENT1_PARAMS_RT_OFFSET	34235
+#define CDU_REG_PF_SEG0_TYPE_OFFSET_RT_OFFSET	34236
+#define CDU_REG_PF_SEG1_TYPE_OFFSET_RT_OFFSET	34237
+#define CDU_REG_PF_SEG2_TYPE_OFFSET_RT_OFFSET	34238
+#define CDU_REG_PF_SEG3_TYPE_OFFSET_RT_OFFSET	34239
+#define CDU_REG_PF_FL_SEG0_TYPE_OFFSET_RT_OFFSET	34240
+#define CDU_REG_PF_FL_SEG1_TYPE_OFFSET_RT_OFFSET	34241
+#define CDU_REG_PF_FL_SEG2_TYPE_OFFSET_RT_OFFSET	34242
+#define CDU_REG_PF_FL_SEG3_TYPE_OFFSET_RT_OFFSET	34243
+#define CDU_REG_VF_SEG_TYPE_OFFSET_RT_OFFSET	34244
+#define CDU_REG_VF_FL_SEG_TYPE_OFFSET_RT_OFFSET	34245
+#define PBF_REG_TAG_ETHERTYPE_0_RT_OFFSET	34246
+#define PBF_REG_BTB_SHARED_AREA_SIZE_RT_OFFSET	34247
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ0_RT_OFFSET	34248
+#define PBF_REG_BTB_GUARANTEED_VOQ0_RT_OFFSET	34249
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ0_RT_OFFSET	34250
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ1_RT_OFFSET	34251
+#define PBF_REG_BTB_GUARANTEED_VOQ1_RT_OFFSET	34252
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ1_RT_OFFSET	34253
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ2_RT_OFFSET	34254
+#define PBF_REG_BTB_GUARANTEED_VOQ2_RT_OFFSET	34255
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ2_RT_OFFSET	34256
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ3_RT_OFFSET	34257
+#define PBF_REG_BTB_GUARANTEED_VOQ3_RT_OFFSET	34258
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ3_RT_OFFSET	34259
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ4_RT_OFFSET	34260
+#define PBF_REG_BTB_GUARANTEED_VOQ4_RT_OFFSET	34261
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ4_RT_OFFSET	34262
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ5_RT_OFFSET	34263
+#define PBF_REG_BTB_GUARANTEED_VOQ5_RT_OFFSET	34264
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ5_RT_OFFSET	34265
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ6_RT_OFFSET	34266
+#define PBF_REG_BTB_GUARANTEED_VOQ6_RT_OFFSET	34267
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ6_RT_OFFSET	34268
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ7_RT_OFFSET	34269
+#define PBF_REG_BTB_GUARANTEED_VOQ7_RT_OFFSET	34270
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ7_RT_OFFSET	34271
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ8_RT_OFFSET	34272
+#define PBF_REG_BTB_GUARANTEED_VOQ8_RT_OFFSET	34273
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ8_RT_OFFSET	34274
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ9_RT_OFFSET	34275
+#define PBF_REG_BTB_GUARANTEED_VOQ9_RT_OFFSET	34276
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ9_RT_OFFSET	34277
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ10_RT_OFFSET	34278
+#define PBF_REG_BTB_GUARANTEED_VOQ10_RT_OFFSET	34279
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ10_RT_OFFSET	34280
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ11_RT_OFFSET	34281
+#define PBF_REG_BTB_GUARANTEED_VOQ11_RT_OFFSET	34282
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ11_RT_OFFSET	34283
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ12_RT_OFFSET	34284
+#define PBF_REG_BTB_GUARANTEED_VOQ12_RT_OFFSET	34285
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ12_RT_OFFSET	34286
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ13_RT_OFFSET	34287
+#define PBF_REG_BTB_GUARANTEED_VOQ13_RT_OFFSET	34288
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ13_RT_OFFSET	34289
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ14_RT_OFFSET	34290
+#define PBF_REG_BTB_GUARANTEED_VOQ14_RT_OFFSET	34291
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ14_RT_OFFSET	34292
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ15_RT_OFFSET	34293
+#define PBF_REG_BTB_GUARANTEED_VOQ15_RT_OFFSET	34294
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ15_RT_OFFSET	34295
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ16_RT_OFFSET	34296
+#define PBF_REG_BTB_GUARANTEED_VOQ16_RT_OFFSET	34297
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ16_RT_OFFSET	34298
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ17_RT_OFFSET	34299
+#define PBF_REG_BTB_GUARANTEED_VOQ17_RT_OFFSET	34300
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ17_RT_OFFSET	34301
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ18_RT_OFFSET	34302
+#define PBF_REG_BTB_GUARANTEED_VOQ18_RT_OFFSET	34303
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ18_RT_OFFSET	34304
+#define PBF_REG_YCMD_QS_NUM_LINES_VOQ19_RT_OFFSET	34305
+#define PBF_REG_BTB_GUARANTEED_VOQ19_RT_OFFSET	34306
+#define PBF_REG_BTB_SHARED_AREA_SETUP_VOQ19_RT_OFFSET	34307
+#define XCM_REG_CON_PHY_Q3_RT_OFFSET	34308
+
+#define RUNTIME_ARRAY_SIZE 34309
 
 /* The eth storm context for the Tstorm */
 struct tstorm_eth_conn_st_ctx {
@@ -4307,7 +4648,7 @@ struct xstorm_eth_conn_ag_ctx {
 #define XSTORM_ETH_CONN_AG_CTX_TPH_ENABLE_SHIFT		6
 	u8 edpm_event_id;
 	__le16 physical_q0;
-	__le16 quota;
+	__le16 ereserved1;
 	__le16 edpm_num_bds;
 	__le16 tx_bd_cons;
 	__le16 tx_bd_prod;
@@ -4340,7 +4681,7 @@ struct xstorm_eth_conn_ag_ctx {
 	u8 byte13;
 	u8 byte14;
 	u8 byte15;
-	u8 byte16;
+	u8 ereserved;
 	__le16 word11;
 	__le32 reg10;
 	__le32 reg11;
@@ -4627,6 +4968,7 @@ enum eth_error_code {
 	ETH_FILTERS_PAIR_ADD_FAIL_ZERO_MAC,
 	ETH_FILTERS_VNI_ADD_FAIL_FULL,
 	ETH_FILTERS_VNI_ADD_FAIL_DUP,
+	ETH_FILTERS_GFT_UPDATE_FAIL,
 	MAX_ETH_ERROR_CODE
 };
 
@@ -4879,6 +5221,39 @@ enum gft_logic_filter_type {
 	MAX_GFT_LOGIC_FILTER_TYPE
 };
 
+struct rx_add_openflow_filter_data {
+	__le16 action_icid;
+	u8 priority;
+	u8 reserved0;
+	__le32 tenant_id;
+	__le16 dst_mac_hi;
+	__le16 dst_mac_mid;
+	__le16 dst_mac_lo;
+	__le16 src_mac_hi;
+	__le16 src_mac_mid;
+	__le16 src_mac_lo;
+	__le16 vlan_id;
+	__le16 l2_eth_type;
+	u8 ipv4_dscp;
+	u8 ipv4_frag_type;
+	u8 ipv4_over_ip;
+	u8 tenant_id_exists;
+	__le32 ipv4_dst_addr;
+	__le32 ipv4_src_addr;
+	__le16 l4_dst_port;
+	__le16 l4_src_port;
+};
+
+struct rx_create_gft_action_data {
+	u8 vport_id;
+	u8 reserved[7];
+};
+
+struct rx_create_openflow_action_data {
+	u8 vport_id;
+	u8 reserved[7];
+};
+
 /* Ramrod data for rx queue start ramrod */
 struct rx_queue_start_ramrod_data {
 	__le16 rx_queue_id;
@@ -4956,7 +5331,7 @@ struct rx_update_gft_filter_data {
 	u8 vport_id;
 	u8 filter_type;
 	u8 filter_action;
-	u8 reserved;
+	u8 assert_on_error;
 };
 
 /* Ramrod data for rx queue start ramrod */
@@ -5102,203 +5477,6 @@ struct vport_update_ramrod_data {
 	struct eth_vport_rss_config rss_config;
 };
 
-struct gft_cam_line {
-	__le32 camline;
-#define GFT_CAM_LINE_VALID_MASK		0x1
-#define GFT_CAM_LINE_VALID_SHIFT	0
-#define GFT_CAM_LINE_DATA_MASK		0x3FFF
-#define GFT_CAM_LINE_DATA_SHIFT		1
-#define GFT_CAM_LINE_MASK_BITS_MASK	0x3FFF
-#define GFT_CAM_LINE_MASK_BITS_SHIFT	15
-#define GFT_CAM_LINE_RESERVED1_MASK	0x7
-#define GFT_CAM_LINE_RESERVED1_SHIFT	29
-};
-
-struct gft_cam_line_mapped {
-	__le32 camline;
-#define GFT_CAM_LINE_MAPPED_VALID_MASK				0x1
-#define GFT_CAM_LINE_MAPPED_VALID_SHIFT				0
-#define GFT_CAM_LINE_MAPPED_IP_VERSION_MASK			0x1
-#define GFT_CAM_LINE_MAPPED_IP_VERSION_SHIFT			1
-#define GFT_CAM_LINE_MAPPED_TUNNEL_IP_VERSION_MASK		0x1
-#define GFT_CAM_LINE_MAPPED_TUNNEL_IP_VERSION_SHIFT		2
-#define GFT_CAM_LINE_MAPPED_UPPER_PROTOCOL_TYPE_MASK		0xF
-#define GFT_CAM_LINE_MAPPED_UPPER_PROTOCOL_TYPE_SHIFT		3
-#define GFT_CAM_LINE_MAPPED_TUNNEL_TYPE_MASK			0xF
-#define GFT_CAM_LINE_MAPPED_TUNNEL_TYPE_SHIFT			7
-#define GFT_CAM_LINE_MAPPED_PF_ID_MASK				0xF
-#define GFT_CAM_LINE_MAPPED_PF_ID_SHIFT				11
-#define GFT_CAM_LINE_MAPPED_IP_VERSION_MASK_MASK		0x1
-#define GFT_CAM_LINE_MAPPED_IP_VERSION_MASK_SHIFT		15
-#define GFT_CAM_LINE_MAPPED_TUNNEL_IP_VERSION_MASK_MASK		0x1
-#define GFT_CAM_LINE_MAPPED_TUNNEL_IP_VERSION_MASK_SHIFT	16
-#define GFT_CAM_LINE_MAPPED_UPPER_PROTOCOL_TYPE_MASK_MASK	0xF
-#define GFT_CAM_LINE_MAPPED_UPPER_PROTOCOL_TYPE_MASK_SHIFT	17
-#define GFT_CAM_LINE_MAPPED_TUNNEL_TYPE_MASK_MASK		0xF
-#define GFT_CAM_LINE_MAPPED_TUNNEL_TYPE_MASK_SHIFT		21
-#define GFT_CAM_LINE_MAPPED_PF_ID_MASK_MASK			0xF
-#define GFT_CAM_LINE_MAPPED_PF_ID_MASK_SHIFT			25
-#define GFT_CAM_LINE_MAPPED_RESERVED1_MASK			0x7
-#define GFT_CAM_LINE_MAPPED_RESERVED1_SHIFT			29
-};
-
-union gft_cam_line_union {
-	struct gft_cam_line cam_line;
-	struct gft_cam_line_mapped cam_line_mapped;
-};
-
-enum gft_profile_ip_version {
-	GFT_PROFILE_IPV4 = 0,
-	GFT_PROFILE_IPV6 = 1,
-	MAX_GFT_PROFILE_IP_VERSION
-};
-
-enum gft_profile_upper_protocol_type {
-	GFT_PROFILE_ROCE_PROTOCOL = 0,
-	GFT_PROFILE_RROCE_PROTOCOL = 1,
-	GFT_PROFILE_FCOE_PROTOCOL = 2,
-	GFT_PROFILE_ICMP_PROTOCOL = 3,
-	GFT_PROFILE_ARP_PROTOCOL = 4,
-	GFT_PROFILE_USER_TCP_SRC_PORT_1_INNER = 5,
-	GFT_PROFILE_USER_TCP_DST_PORT_1_INNER = 6,
-	GFT_PROFILE_TCP_PROTOCOL = 7,
-	GFT_PROFILE_USER_UDP_DST_PORT_1_INNER = 8,
-	GFT_PROFILE_USER_UDP_DST_PORT_2_OUTER = 9,
-	GFT_PROFILE_UDP_PROTOCOL = 10,
-	GFT_PROFILE_USER_IP_1_INNER = 11,
-	GFT_PROFILE_USER_IP_2_OUTER = 12,
-	GFT_PROFILE_USER_ETH_1_INNER = 13,
-	GFT_PROFILE_USER_ETH_2_OUTER = 14,
-	GFT_PROFILE_RAW = 15,
-	MAX_GFT_PROFILE_UPPER_PROTOCOL_TYPE
-};
-
-struct gft_ram_line {
-	__le32 low32bits;
-#define GFT_RAM_LINE_VLAN_SELECT_MASK			0x3
-#define GFT_RAM_LINE_VLAN_SELECT_SHIFT			0
-#define GFT_RAM_LINE_TUNNEL_ENTROPHY_MASK		0x1
-#define GFT_RAM_LINE_TUNNEL_ENTROPHY_SHIFT		2
-#define GFT_RAM_LINE_TUNNEL_TTL_EQUAL_ONE_MASK		0x1
-#define GFT_RAM_LINE_TUNNEL_TTL_EQUAL_ONE_SHIFT		3
-#define GFT_RAM_LINE_TUNNEL_TTL_MASK			0x1
-#define GFT_RAM_LINE_TUNNEL_TTL_SHIFT			4
-#define GFT_RAM_LINE_TUNNEL_ETHERTYPE_MASK		0x1
-#define GFT_RAM_LINE_TUNNEL_ETHERTYPE_SHIFT		5
-#define GFT_RAM_LINE_TUNNEL_DST_PORT_MASK		0x1
-#define GFT_RAM_LINE_TUNNEL_DST_PORT_SHIFT		6
-#define GFT_RAM_LINE_TUNNEL_SRC_PORT_MASK		0x1
-#define GFT_RAM_LINE_TUNNEL_SRC_PORT_SHIFT		7
-#define GFT_RAM_LINE_TUNNEL_DSCP_MASK			0x1
-#define GFT_RAM_LINE_TUNNEL_DSCP_SHIFT			8
-#define GFT_RAM_LINE_TUNNEL_OVER_IP_PROTOCOL_MASK	0x1
-#define GFT_RAM_LINE_TUNNEL_OVER_IP_PROTOCOL_SHIFT	9
-#define GFT_RAM_LINE_TUNNEL_DST_IP_MASK			0x1
-#define GFT_RAM_LINE_TUNNEL_DST_IP_SHIFT		10
-#define GFT_RAM_LINE_TUNNEL_SRC_IP_MASK			0x1
-#define GFT_RAM_LINE_TUNNEL_SRC_IP_SHIFT		11
-#define GFT_RAM_LINE_TUNNEL_PRIORITY_MASK		0x1
-#define GFT_RAM_LINE_TUNNEL_PRIORITY_SHIFT		12
-#define GFT_RAM_LINE_TUNNEL_PROVIDER_VLAN_MASK		0x1
-#define GFT_RAM_LINE_TUNNEL_PROVIDER_VLAN_SHIFT		13
-#define GFT_RAM_LINE_TUNNEL_VLAN_MASK			0x1
-#define GFT_RAM_LINE_TUNNEL_VLAN_SHIFT			14
-#define GFT_RAM_LINE_TUNNEL_DST_MAC_MASK		0x1
-#define GFT_RAM_LINE_TUNNEL_DST_MAC_SHIFT		15
-#define GFT_RAM_LINE_TUNNEL_SRC_MAC_MASK		0x1
-#define GFT_RAM_LINE_TUNNEL_SRC_MAC_SHIFT		16
-#define GFT_RAM_LINE_TTL_EQUAL_ONE_MASK			0x1
-#define GFT_RAM_LINE_TTL_EQUAL_ONE_SHIFT		17
-#define GFT_RAM_LINE_TTL_MASK				0x1
-#define GFT_RAM_LINE_TTL_SHIFT				18
-#define GFT_RAM_LINE_ETHERTYPE_MASK			0x1
-#define GFT_RAM_LINE_ETHERTYPE_SHIFT			19
-#define GFT_RAM_LINE_RESERVED0_MASK			0x1
-#define GFT_RAM_LINE_RESERVED0_SHIFT			20
-#define GFT_RAM_LINE_TCP_FLAG_FIN_MASK			0x1
-#define GFT_RAM_LINE_TCP_FLAG_FIN_SHIFT			21
-#define GFT_RAM_LINE_TCP_FLAG_SYN_MASK			0x1
-#define GFT_RAM_LINE_TCP_FLAG_SYN_SHIFT			22
-#define GFT_RAM_LINE_TCP_FLAG_RST_MASK			0x1
-#define GFT_RAM_LINE_TCP_FLAG_RST_SHIFT			23
-#define GFT_RAM_LINE_TCP_FLAG_PSH_MASK			0x1
-#define GFT_RAM_LINE_TCP_FLAG_PSH_SHIFT			24
-#define GFT_RAM_LINE_TCP_FLAG_ACK_MASK			0x1
-#define GFT_RAM_LINE_TCP_FLAG_ACK_SHIFT			25
-#define GFT_RAM_LINE_TCP_FLAG_URG_MASK			0x1
-#define GFT_RAM_LINE_TCP_FLAG_URG_SHIFT			26
-#define GFT_RAM_LINE_TCP_FLAG_ECE_MASK			0x1
-#define GFT_RAM_LINE_TCP_FLAG_ECE_SHIFT			27
-#define GFT_RAM_LINE_TCP_FLAG_CWR_MASK			0x1
-#define GFT_RAM_LINE_TCP_FLAG_CWR_SHIFT			28
-#define GFT_RAM_LINE_TCP_FLAG_NS_MASK			0x1
-#define GFT_RAM_LINE_TCP_FLAG_NS_SHIFT			29
-#define GFT_RAM_LINE_DST_PORT_MASK			0x1
-#define GFT_RAM_LINE_DST_PORT_SHIFT			30
-#define GFT_RAM_LINE_SRC_PORT_MASK			0x1
-#define GFT_RAM_LINE_SRC_PORT_SHIFT			31
-	__le32 high32bits;
-#define GFT_RAM_LINE_DSCP_MASK				0x1
-#define GFT_RAM_LINE_DSCP_SHIFT				0
-#define GFT_RAM_LINE_OVER_IP_PROTOCOL_MASK		0x1
-#define GFT_RAM_LINE_OVER_IP_PROTOCOL_SHIFT		1
-#define GFT_RAM_LINE_DST_IP_MASK			0x1
-#define GFT_RAM_LINE_DST_IP_SHIFT			2
-#define GFT_RAM_LINE_SRC_IP_MASK			0x1
-#define GFT_RAM_LINE_SRC_IP_SHIFT			3
-#define GFT_RAM_LINE_PRIORITY_MASK			0x1
-#define GFT_RAM_LINE_PRIORITY_SHIFT			4
-#define GFT_RAM_LINE_PROVIDER_VLAN_MASK			0x1
-#define GFT_RAM_LINE_PROVIDER_VLAN_SHIFT		5
-#define GFT_RAM_LINE_VLAN_MASK				0x1
-#define GFT_RAM_LINE_VLAN_SHIFT				6
-#define GFT_RAM_LINE_DST_MAC_MASK			0x1
-#define GFT_RAM_LINE_DST_MAC_SHIFT			7
-#define GFT_RAM_LINE_SRC_MAC_MASK			0x1
-#define GFT_RAM_LINE_SRC_MAC_SHIFT			8
-#define GFT_RAM_LINE_TENANT_ID_MASK			0x1
-#define GFT_RAM_LINE_TENANT_ID_SHIFT			9
-#define GFT_RAM_LINE_RESERVED1_MASK			0x3FFFFF
-#define GFT_RAM_LINE_RESERVED1_SHIFT			10
-};
-
-struct mstorm_eth_conn_ag_ctx {
-	u8 byte0;
-	u8 byte1;
-	u8 flags0;
-#define MSTORM_ETH_CONN_AG_CTX_EXIST_IN_QM0_MASK	0x1
-#define MSTORM_ETH_CONN_AG_CTX_EXIST_IN_QM0_SHIFT 0
-#define MSTORM_ETH_CONN_AG_CTX_BIT1_MASK	0x1
-#define MSTORM_ETH_CONN_AG_CTX_BIT1_SHIFT         1
-#define MSTORM_ETH_CONN_AG_CTX_CF0_MASK	0x3
-#define MSTORM_ETH_CONN_AG_CTX_CF0_SHIFT          2
-#define MSTORM_ETH_CONN_AG_CTX_CF1_MASK	0x3
-#define MSTORM_ETH_CONN_AG_CTX_CF1_SHIFT          4
-#define MSTORM_ETH_CONN_AG_CTX_CF2_MASK	0x3
-#define MSTORM_ETH_CONN_AG_CTX_CF2_SHIFT          6
-	u8 flags1;
-#define MSTORM_ETH_CONN_AG_CTX_CF0EN_MASK	0x1
-#define MSTORM_ETH_CONN_AG_CTX_CF0EN_SHIFT        0
-#define MSTORM_ETH_CONN_AG_CTX_CF1EN_MASK	0x1
-#define MSTORM_ETH_CONN_AG_CTX_CF1EN_SHIFT        1
-#define MSTORM_ETH_CONN_AG_CTX_CF2EN_MASK	0x1
-#define MSTORM_ETH_CONN_AG_CTX_CF2EN_SHIFT        2
-#define MSTORM_ETH_CONN_AG_CTX_RULE0EN_MASK	0x1
-#define MSTORM_ETH_CONN_AG_CTX_RULE0EN_SHIFT      3
-#define MSTORM_ETH_CONN_AG_CTX_RULE1EN_MASK	0x1
-#define MSTORM_ETH_CONN_AG_CTX_RULE1EN_SHIFT      4
-#define MSTORM_ETH_CONN_AG_CTX_RULE2EN_MASK	0x1
-#define MSTORM_ETH_CONN_AG_CTX_RULE2EN_SHIFT      5
-#define MSTORM_ETH_CONN_AG_CTX_RULE3EN_MASK	0x1
-#define MSTORM_ETH_CONN_AG_CTX_RULE3EN_SHIFT      6
-#define MSTORM_ETH_CONN_AG_CTX_RULE4EN_MASK	0x1
-#define MSTORM_ETH_CONN_AG_CTX_RULE4EN_SHIFT      7
-	__le16 word0;
-	__le16 word1;
-	__le32 reg0;
-	__le32 reg1;
-};
-
 struct xstorm_eth_conn_agctxdq_ext_ldpart {
 	u8 reserved0;
 	u8 eth_state;
@@ -5511,7 +5689,7 @@ struct xstorm_eth_conn_agctxdq_ext_ldpart {
 #define XSTORMETHCONNAGCTXDQEXTLDPART_TPH_ENABLE_SHIFT             6
 	u8 edpm_event_id;
 	__le16 physical_q0;
-	__le16 quota;
+	__le16 ereserved1;
 	__le16 edpm_num_bds;
 	__le16 tx_bd_cons;
 	__le16 tx_bd_prod;
@@ -5528,6 +5706,43 @@ struct xstorm_eth_conn_agctxdq_ext_ldpart {
 	__le32 reg4;
 };
 
+struct mstorm_eth_conn_ag_ctx {
+	u8 byte0;
+	u8 byte1;
+	u8 flags0;
+#define MSTORM_ETH_CONN_AG_CTX_EXIST_IN_QM0_MASK	0x1
+#define MSTORM_ETH_CONN_AG_CTX_EXIST_IN_QM0_SHIFT 0
+#define MSTORM_ETH_CONN_AG_CTX_BIT1_MASK	0x1
+#define MSTORM_ETH_CONN_AG_CTX_BIT1_SHIFT         1
+#define MSTORM_ETH_CONN_AG_CTX_CF0_MASK	0x3
+#define MSTORM_ETH_CONN_AG_CTX_CF0_SHIFT          2
+#define MSTORM_ETH_CONN_AG_CTX_CF1_MASK	0x3
+#define MSTORM_ETH_CONN_AG_CTX_CF1_SHIFT          4
+#define MSTORM_ETH_CONN_AG_CTX_CF2_MASK	0x3
+#define MSTORM_ETH_CONN_AG_CTX_CF2_SHIFT          6
+	u8 flags1;
+#define MSTORM_ETH_CONN_AG_CTX_CF0EN_MASK	0x1
+#define MSTORM_ETH_CONN_AG_CTX_CF0EN_SHIFT        0
+#define MSTORM_ETH_CONN_AG_CTX_CF1EN_MASK	0x1
+#define MSTORM_ETH_CONN_AG_CTX_CF1EN_SHIFT        1
+#define MSTORM_ETH_CONN_AG_CTX_CF2EN_MASK	0x1
+#define MSTORM_ETH_CONN_AG_CTX_CF2EN_SHIFT        2
+#define MSTORM_ETH_CONN_AG_CTX_RULE0EN_MASK	0x1
+#define MSTORM_ETH_CONN_AG_CTX_RULE0EN_SHIFT      3
+#define MSTORM_ETH_CONN_AG_CTX_RULE1EN_MASK	0x1
+#define MSTORM_ETH_CONN_AG_CTX_RULE1EN_SHIFT      4
+#define MSTORM_ETH_CONN_AG_CTX_RULE2EN_MASK	0x1
+#define MSTORM_ETH_CONN_AG_CTX_RULE2EN_SHIFT      5
+#define MSTORM_ETH_CONN_AG_CTX_RULE3EN_MASK	0x1
+#define MSTORM_ETH_CONN_AG_CTX_RULE3EN_SHIFT      6
+#define MSTORM_ETH_CONN_AG_CTX_RULE4EN_MASK	0x1
+#define MSTORM_ETH_CONN_AG_CTX_RULE4EN_SHIFT      7
+	__le16 word0;
+	__le16 word1;
+	__le32 reg0;
+	__le32 reg1;
+};
+
 struct xstorm_eth_hw_conn_ag_ctx {
 	u8 reserved0;
 	u8 eth_state;
@@ -5740,7 +5955,7 @@ struct xstorm_eth_hw_conn_ag_ctx {
 #define XSTORM_ETH_HW_CONN_AG_CTX_TPH_ENABLE_SHIFT             6
 	u8 edpm_event_id;
 	__le16 physical_q0;
-	__le16 quota;
+	__le16 ereserved1;
 	__le16 edpm_num_bds;
 	__le16 tx_bd_cons;
 	__le16 tx_bd_prod;
@@ -5748,6 +5963,200 @@ struct xstorm_eth_hw_conn_ag_ctx {
 	__le16 conn_dpi;
 };
 
+struct gft_cam_line {
+	__le32 camline;
+#define GFT_CAM_LINE_VALID_MASK		0x1
+#define GFT_CAM_LINE_VALID_SHIFT	0
+#define GFT_CAM_LINE_DATA_MASK		0x3FFF
+#define GFT_CAM_LINE_DATA_SHIFT		1
+#define GFT_CAM_LINE_MASK_BITS_MASK	0x3FFF
+#define GFT_CAM_LINE_MASK_BITS_SHIFT	15
+#define GFT_CAM_LINE_RESERVED1_MASK	0x7
+#define GFT_CAM_LINE_RESERVED1_SHIFT	29
+};
+
+struct gft_cam_line_mapped {
+	__le32 camline;
+#define GFT_CAM_LINE_MAPPED_VALID_MASK				0x1
+#define GFT_CAM_LINE_MAPPED_VALID_SHIFT				0
+#define GFT_CAM_LINE_MAPPED_IP_VERSION_MASK			0x1
+#define GFT_CAM_LINE_MAPPED_IP_VERSION_SHIFT			1
+#define GFT_CAM_LINE_MAPPED_TUNNEL_IP_VERSION_MASK		0x1
+#define GFT_CAM_LINE_MAPPED_TUNNEL_IP_VERSION_SHIFT		2
+#define GFT_CAM_LINE_MAPPED_UPPER_PROTOCOL_TYPE_MASK		0xF
+#define GFT_CAM_LINE_MAPPED_UPPER_PROTOCOL_TYPE_SHIFT		3
+#define GFT_CAM_LINE_MAPPED_TUNNEL_TYPE_MASK			0xF
+#define GFT_CAM_LINE_MAPPED_TUNNEL_TYPE_SHIFT			7
+#define GFT_CAM_LINE_MAPPED_PF_ID_MASK				0xF
+#define GFT_CAM_LINE_MAPPED_PF_ID_SHIFT				11
+#define GFT_CAM_LINE_MAPPED_IP_VERSION_MASK_MASK		0x1
+#define GFT_CAM_LINE_MAPPED_IP_VERSION_MASK_SHIFT		15
+#define GFT_CAM_LINE_MAPPED_TUNNEL_IP_VERSION_MASK_MASK		0x1
+#define GFT_CAM_LINE_MAPPED_TUNNEL_IP_VERSION_MASK_SHIFT	16
+#define GFT_CAM_LINE_MAPPED_UPPER_PROTOCOL_TYPE_MASK_MASK	0xF
+#define GFT_CAM_LINE_MAPPED_UPPER_PROTOCOL_TYPE_MASK_SHIFT	17
+#define GFT_CAM_LINE_MAPPED_TUNNEL_TYPE_MASK_MASK		0xF
+#define GFT_CAM_LINE_MAPPED_TUNNEL_TYPE_MASK_SHIFT		21
+#define GFT_CAM_LINE_MAPPED_PF_ID_MASK_MASK			0xF
+#define GFT_CAM_LINE_MAPPED_PF_ID_MASK_SHIFT			25
+#define GFT_CAM_LINE_MAPPED_RESERVED1_MASK			0x7
+#define GFT_CAM_LINE_MAPPED_RESERVED1_SHIFT			29
+};
+
+union gft_cam_line_union {
+	struct gft_cam_line cam_line;
+	struct gft_cam_line_mapped cam_line_mapped;
+};
+
+enum gft_profile_ip_version {
+	GFT_PROFILE_IPV4 = 0,
+	GFT_PROFILE_IPV6 = 1,
+	MAX_GFT_PROFILE_IP_VERSION
+};
+
+struct gft_profile_key {
+	__le16 profile_key;
+#define GFT_PROFILE_KEY_IP_VERSION_MASK           0x1
+#define GFT_PROFILE_KEY_IP_VERSION_SHIFT          0
+#define GFT_PROFILE_KEY_TUNNEL_IP_VERSION_MASK    0x1
+#define GFT_PROFILE_KEY_TUNNEL_IP_VERSION_SHIFT   1
+#define GFT_PROFILE_KEY_UPPER_PROTOCOL_TYPE_MASK  0xF
+#define GFT_PROFILE_KEY_UPPER_PROTOCOL_TYPE_SHIFT 2
+#define GFT_PROFILE_KEY_TUNNEL_TYPE_MASK          0xF
+#define GFT_PROFILE_KEY_TUNNEL_TYPE_SHIFT         6
+#define GFT_PROFILE_KEY_PF_ID_MASK                0xF
+#define GFT_PROFILE_KEY_PF_ID_SHIFT               10
+#define GFT_PROFILE_KEY_RESERVED0_MASK            0x3
+#define GFT_PROFILE_KEY_RESERVED0_SHIFT           14
+};
+
+enum gft_profile_tunnel_type {
+	GFT_PROFILE_NO_TUNNEL = 0,
+	GFT_PROFILE_VXLAN_TUNNEL = 1,
+	GFT_PROFILE_GRE_MAC_OR_NVGRE_TUNNEL = 2,
+	GFT_PROFILE_GRE_IP_TUNNEL = 3,
+	GFT_PROFILE_GENEVE_MAC_TUNNEL = 4,
+	GFT_PROFILE_GENEVE_IP_TUNNEL = 5,
+	MAX_GFT_PROFILE_TUNNEL_TYPE
+};
+
+enum gft_profile_upper_protocol_type {
+	GFT_PROFILE_ROCE_PROTOCOL = 0,
+	GFT_PROFILE_RROCE_PROTOCOL = 1,
+	GFT_PROFILE_FCOE_PROTOCOL = 2,
+	GFT_PROFILE_ICMP_PROTOCOL = 3,
+	GFT_PROFILE_ARP_PROTOCOL = 4,
+	GFT_PROFILE_USER_TCP_SRC_PORT_1_INNER = 5,
+	GFT_PROFILE_USER_TCP_DST_PORT_1_INNER = 6,
+	GFT_PROFILE_TCP_PROTOCOL = 7,
+	GFT_PROFILE_USER_UDP_DST_PORT_1_INNER = 8,
+	GFT_PROFILE_USER_UDP_DST_PORT_2_OUTER = 9,
+	GFT_PROFILE_UDP_PROTOCOL = 10,
+	GFT_PROFILE_USER_IP_1_INNER = 11,
+	GFT_PROFILE_USER_IP_2_OUTER = 12,
+	GFT_PROFILE_USER_ETH_1_INNER = 13,
+	GFT_PROFILE_USER_ETH_2_OUTER = 14,
+	GFT_PROFILE_RAW = 15,
+	MAX_GFT_PROFILE_UPPER_PROTOCOL_TYPE
+};
+
+struct gft_ram_line {
+	__le32 lo;
+#define GFT_RAM_LINE_VLAN_SELECT_MASK			0x3
+#define GFT_RAM_LINE_VLAN_SELECT_SHIFT			0
+#define GFT_RAM_LINE_TUNNEL_ENTROPHY_MASK		0x1
+#define GFT_RAM_LINE_TUNNEL_ENTROPHY_SHIFT		2
+#define GFT_RAM_LINE_TUNNEL_TTL_EQUAL_ONE_MASK		0x1
+#define GFT_RAM_LINE_TUNNEL_TTL_EQUAL_ONE_SHIFT		3
+#define GFT_RAM_LINE_TUNNEL_TTL_MASK			0x1
+#define GFT_RAM_LINE_TUNNEL_TTL_SHIFT			4
+#define GFT_RAM_LINE_TUNNEL_ETHERTYPE_MASK		0x1
+#define GFT_RAM_LINE_TUNNEL_ETHERTYPE_SHIFT		5
+#define GFT_RAM_LINE_TUNNEL_DST_PORT_MASK		0x1
+#define GFT_RAM_LINE_TUNNEL_DST_PORT_SHIFT		6
+#define GFT_RAM_LINE_TUNNEL_SRC_PORT_MASK		0x1
+#define GFT_RAM_LINE_TUNNEL_SRC_PORT_SHIFT		7
+#define GFT_RAM_LINE_TUNNEL_DSCP_MASK			0x1
+#define GFT_RAM_LINE_TUNNEL_DSCP_SHIFT			8
+#define GFT_RAM_LINE_TUNNEL_OVER_IP_PROTOCOL_MASK	0x1
+#define GFT_RAM_LINE_TUNNEL_OVER_IP_PROTOCOL_SHIFT	9
+#define GFT_RAM_LINE_TUNNEL_DST_IP_MASK			0x1
+#define GFT_RAM_LINE_TUNNEL_DST_IP_SHIFT		10
+#define GFT_RAM_LINE_TUNNEL_SRC_IP_MASK			0x1
+#define GFT_RAM_LINE_TUNNEL_SRC_IP_SHIFT		11
+#define GFT_RAM_LINE_TUNNEL_PRIORITY_MASK		0x1
+#define GFT_RAM_LINE_TUNNEL_PRIORITY_SHIFT		12
+#define GFT_RAM_LINE_TUNNEL_PROVIDER_VLAN_MASK		0x1
+#define GFT_RAM_LINE_TUNNEL_PROVIDER_VLAN_SHIFT		13
+#define GFT_RAM_LINE_TUNNEL_VLAN_MASK			0x1
+#define GFT_RAM_LINE_TUNNEL_VLAN_SHIFT			14
+#define GFT_RAM_LINE_TUNNEL_DST_MAC_MASK		0x1
+#define GFT_RAM_LINE_TUNNEL_DST_MAC_SHIFT		15
+#define GFT_RAM_LINE_TUNNEL_SRC_MAC_MASK		0x1
+#define GFT_RAM_LINE_TUNNEL_SRC_MAC_SHIFT		16
+#define GFT_RAM_LINE_TTL_EQUAL_ONE_MASK			0x1
+#define GFT_RAM_LINE_TTL_EQUAL_ONE_SHIFT		17
+#define GFT_RAM_LINE_TTL_MASK				0x1
+#define GFT_RAM_LINE_TTL_SHIFT				18
+#define GFT_RAM_LINE_ETHERTYPE_MASK			0x1
+#define GFT_RAM_LINE_ETHERTYPE_SHIFT			19
+#define GFT_RAM_LINE_RESERVED0_MASK			0x1
+#define GFT_RAM_LINE_RESERVED0_SHIFT			20
+#define GFT_RAM_LINE_TCP_FLAG_FIN_MASK			0x1
+#define GFT_RAM_LINE_TCP_FLAG_FIN_SHIFT			21
+#define GFT_RAM_LINE_TCP_FLAG_SYN_MASK			0x1
+#define GFT_RAM_LINE_TCP_FLAG_SYN_SHIFT			22
+#define GFT_RAM_LINE_TCP_FLAG_RST_MASK			0x1
+#define GFT_RAM_LINE_TCP_FLAG_RST_SHIFT			23
+#define GFT_RAM_LINE_TCP_FLAG_PSH_MASK			0x1
+#define GFT_RAM_LINE_TCP_FLAG_PSH_SHIFT			24
+#define GFT_RAM_LINE_TCP_FLAG_ACK_MASK			0x1
+#define GFT_RAM_LINE_TCP_FLAG_ACK_SHIFT			25
+#define GFT_RAM_LINE_TCP_FLAG_URG_MASK			0x1
+#define GFT_RAM_LINE_TCP_FLAG_URG_SHIFT			26
+#define GFT_RAM_LINE_TCP_FLAG_ECE_MASK			0x1
+#define GFT_RAM_LINE_TCP_FLAG_ECE_SHIFT			27
+#define GFT_RAM_LINE_TCP_FLAG_CWR_MASK			0x1
+#define GFT_RAM_LINE_TCP_FLAG_CWR_SHIFT			28
+#define GFT_RAM_LINE_TCP_FLAG_NS_MASK			0x1
+#define GFT_RAM_LINE_TCP_FLAG_NS_SHIFT			29
+#define GFT_RAM_LINE_DST_PORT_MASK			0x1
+#define GFT_RAM_LINE_DST_PORT_SHIFT			30
+#define GFT_RAM_LINE_SRC_PORT_MASK			0x1
+#define GFT_RAM_LINE_SRC_PORT_SHIFT			31
+	__le32 hi;
+#define GFT_RAM_LINE_DSCP_MASK				0x1
+#define GFT_RAM_LINE_DSCP_SHIFT				0
+#define GFT_RAM_LINE_OVER_IP_PROTOCOL_MASK		0x1
+#define GFT_RAM_LINE_OVER_IP_PROTOCOL_SHIFT		1
+#define GFT_RAM_LINE_DST_IP_MASK			0x1
+#define GFT_RAM_LINE_DST_IP_SHIFT			2
+#define GFT_RAM_LINE_SRC_IP_MASK			0x1
+#define GFT_RAM_LINE_SRC_IP_SHIFT			3
+#define GFT_RAM_LINE_PRIORITY_MASK			0x1
+#define GFT_RAM_LINE_PRIORITY_SHIFT			4
+#define GFT_RAM_LINE_PROVIDER_VLAN_MASK			0x1
+#define GFT_RAM_LINE_PROVIDER_VLAN_SHIFT		5
+#define GFT_RAM_LINE_VLAN_MASK				0x1
+#define GFT_RAM_LINE_VLAN_SHIFT				6
+#define GFT_RAM_LINE_DST_MAC_MASK			0x1
+#define GFT_RAM_LINE_DST_MAC_SHIFT			7
+#define GFT_RAM_LINE_SRC_MAC_MASK			0x1
+#define GFT_RAM_LINE_SRC_MAC_SHIFT			8
+#define GFT_RAM_LINE_TENANT_ID_MASK			0x1
+#define GFT_RAM_LINE_TENANT_ID_SHIFT			9
+#define GFT_RAM_LINE_RESERVED1_MASK			0x3FFFFF
+#define GFT_RAM_LINE_RESERVED1_SHIFT			10
+};
+
+enum gft_vlan_select {
+	INNER_PROVIDER_VLAN = 0,
+	INNER_VLAN = 1,
+	OUTER_PROVIDER_VLAN = 2,
+	OUTER_VLAN = 3,
+	MAX_GFT_VLAN_SELECT
+};
+
 struct mstorm_rdma_task_st_ctx {
 	struct regpair temp[4];
 };
@@ -5827,12 +6236,9 @@ struct rdma_init_func_hdr {
 	u8 cnq_start_offset;
 	u8 num_cnqs;
 	u8 cq_ring_mode;
-	u8 cnp_vlan_priority;
-	__le32 cnp_send_timeout;
-	u8 cnp_dscp;
 	u8 vf_id;
 	u8 vf_valid;
-	u8 reserved[5];
+	u8 reserved[3];
 };
 
 struct rdma_init_func_ramrod_data {
@@ -5856,54 +6262,55 @@ enum rdma_ramrod_cmd_id {
 };
 
 struct rdma_register_tid_ramrod_data {
-	__le32 flags;
-#define RDMA_REGISTER_TID_RAMROD_DATA_MAX_ID_MASK             0x3FFFF
-#define RDMA_REGISTER_TID_RAMROD_DATA_MAX_ID_SHIFT            0
-#define RDMA_REGISTER_TID_RAMROD_DATA_PAGE_SIZE_LOG_MASK      0x1F
-#define RDMA_REGISTER_TID_RAMROD_DATA_PAGE_SIZE_LOG_SHIFT     18
-#define RDMA_REGISTER_TID_RAMROD_DATA_TWO_LEVEL_PBL_MASK      0x1
-#define RDMA_REGISTER_TID_RAMROD_DATA_TWO_LEVEL_PBL_SHIFT     23
-#define RDMA_REGISTER_TID_RAMROD_DATA_ZERO_BASED_MASK         0x1
-#define RDMA_REGISTER_TID_RAMROD_DATA_ZERO_BASED_SHIFT        24
-#define RDMA_REGISTER_TID_RAMROD_DATA_PHY_MR_MASK             0x1
-#define RDMA_REGISTER_TID_RAMROD_DATA_PHY_MR_SHIFT            25
-#define RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_READ_MASK        0x1
-#define RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_READ_SHIFT       26
-#define RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_WRITE_MASK       0x1
-#define RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_WRITE_SHIFT      27
-#define RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_ATOMIC_MASK      0x1
-#define RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_ATOMIC_SHIFT     28
-#define RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_WRITE_MASK        0x1
-#define RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_WRITE_SHIFT       29
-#define RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_READ_MASK         0x1
-#define RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_READ_SHIFT        30
-#define RDMA_REGISTER_TID_RAMROD_DATA_ENABLE_MW_BIND_MASK     0x1
-#define RDMA_REGISTER_TID_RAMROD_DATA_ENABLE_MW_BIND_SHIFT    31
+	__le16 flags;
+#define RDMA_REGISTER_TID_RAMROD_DATA_PAGE_SIZE_LOG_MASK	0x1F
+#define RDMA_REGISTER_TID_RAMROD_DATA_PAGE_SIZE_LOG_SHIFT	0
+#define RDMA_REGISTER_TID_RAMROD_DATA_TWO_LEVEL_PBL_MASK	0x1
+#define RDMA_REGISTER_TID_RAMROD_DATA_TWO_LEVEL_PBL_SHIFT	5
+#define RDMA_REGISTER_TID_RAMROD_DATA_ZERO_BASED_MASK	0x1
+#define RDMA_REGISTER_TID_RAMROD_DATA_ZERO_BASED_SHIFT	6
+#define RDMA_REGISTER_TID_RAMROD_DATA_PHY_MR_MASK	0x1
+#define RDMA_REGISTER_TID_RAMROD_DATA_PHY_MR_SHIFT	7
+#define RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_READ_MASK	0x1
+#define RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_READ_SHIFT	8
+#define RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_WRITE_MASK	0x1
+#define RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_WRITE_SHIFT	9
+#define RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_ATOMIC_MASK	0x1
+#define RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_ATOMIC_SHIFT	10
+#define RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_WRITE_MASK	0x1
+#define RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_WRITE_SHIFT	11
+#define RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_READ_MASK	0x1
+#define RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_READ_SHIFT	12
+#define RDMA_REGISTER_TID_RAMROD_DATA_ENABLE_MW_BIND_MASK	0x1
+#define RDMA_REGISTER_TID_RAMROD_DATA_ENABLE_MW_BIND_SHIFT	13
+#define RDMA_REGISTER_TID_RAMROD_DATA_RESERVED_MASK	0x3
+#define RDMA_REGISTER_TID_RAMROD_DATA_RESERVED_SHIFT	14
 	u8 flags1;
-#define RDMA_REGISTER_TID_RAMROD_DATA_PBL_PAGE_SIZE_LOG_MASK  0x1F
+#define RDMA_REGISTER_TID_RAMROD_DATA_PBL_PAGE_SIZE_LOG_MASK	0x1F
 #define RDMA_REGISTER_TID_RAMROD_DATA_PBL_PAGE_SIZE_LOG_SHIFT 0
-#define RDMA_REGISTER_TID_RAMROD_DATA_TID_TYPE_MASK           0x7
-#define RDMA_REGISTER_TID_RAMROD_DATA_TID_TYPE_SHIFT          5
+#define RDMA_REGISTER_TID_RAMROD_DATA_TID_TYPE_MASK	0x7
+#define RDMA_REGISTER_TID_RAMROD_DATA_TID_TYPE_SHIFT	5
 	u8 flags2;
-#define RDMA_REGISTER_TID_RAMROD_DATA_DMA_MR_MASK             0x1
-#define RDMA_REGISTER_TID_RAMROD_DATA_DMA_MR_SHIFT            0
-#define RDMA_REGISTER_TID_RAMROD_DATA_DIF_ON_HOST_FLG_MASK    0x1
-#define RDMA_REGISTER_TID_RAMROD_DATA_DIF_ON_HOST_FLG_SHIFT   1
-#define RDMA_REGISTER_TID_RAMROD_DATA_RESERVED1_MASK          0x3F
-#define RDMA_REGISTER_TID_RAMROD_DATA_RESERVED1_SHIFT         2
+#define RDMA_REGISTER_TID_RAMROD_DATA_DMA_MR_MASK	0x1
+#define RDMA_REGISTER_TID_RAMROD_DATA_DMA_MR_SHIFT	0
+#define RDMA_REGISTER_TID_RAMROD_DATA_DIF_ON_HOST_FLG_MASK	0x1
+#define RDMA_REGISTER_TID_RAMROD_DATA_DIF_ON_HOST_FLG_SHIFT	1
+#define RDMA_REGISTER_TID_RAMROD_DATA_RESERVED1_MASK	0x3F
+#define RDMA_REGISTER_TID_RAMROD_DATA_RESERVED1_SHIFT	2
 	u8 key;
 	u8 length_hi;
 	u8 vf_id;
 	u8 vf_valid;
 	__le16 pd;
+	__le16 reserved2;
 	__le32 length_lo;
 	__le32 itid;
-	__le32 reserved2;
+	__le32 reserved3;
 	struct regpair va;
 	struct regpair pbl_base;
 	struct regpair dif_error_addr;
 	struct regpair dif_runt_addr;
-	__le32 reserved3[2];
+	__le32 reserved4[2];
 };
 
 struct rdma_resize_cq_output_params {
@@ -6149,6 +6556,233 @@ enum rdma_tid_type {
 	MAX_RDMA_TID_TYPE
 };
 
+struct xstorm_roce_conn_ag_ctx_dq_ext_ld_part {
+	u8 reserved0;
+	u8 state;
+	u8 flags0;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_EXIST_IN_QM0_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_EXIST_IN_QM0_SHIFT     0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT1_MASK              0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT1_SHIFT             1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT2_MASK              0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT2_SHIFT             2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_EXIST_IN_QM3_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_EXIST_IN_QM3_SHIFT     3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT4_MASK              0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT4_SHIFT             4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT5_MASK              0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT5_SHIFT             5
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT6_MASK              0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT6_SHIFT             6
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT7_MASK              0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT7_SHIFT             7
+	u8 flags1;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT8_MASK              0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT8_SHIFT             0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT9_MASK              0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT9_SHIFT             1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT10_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT10_SHIFT            2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT11_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT11_SHIFT            3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT12_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT12_SHIFT            4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_MSTORM_FLUSH_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_MSTORM_FLUSH_SHIFT     5
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT14_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT14_SHIFT            6
+#define XSTORMROCECONNAGCTXDQEXTLDPART_YSTORM_FLUSH_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_YSTORM_FLUSH_SHIFT     7
+	u8 flags2;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF0_MASK               0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF0_SHIFT              0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF1_MASK               0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF1_SHIFT              2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF2_MASK               0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF2_SHIFT              4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF3_MASK               0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF3_SHIFT              6
+	u8 flags3;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF4_MASK               0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF4_SHIFT              0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF5_MASK               0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF5_SHIFT              2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF6_MASK               0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF6_SHIFT              4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_FLUSH_Q0_CF_MASK       0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_FLUSH_Q0_CF_SHIFT      6
+	u8 flags4;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF8_MASK               0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF8_SHIFT              0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF9_MASK               0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF9_SHIFT              2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF10_MASK              0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF10_SHIFT             4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF11_MASK              0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF11_SHIFT             6
+	u8 flags5;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF12_MASK              0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF12_SHIFT             0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF13_MASK              0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF13_SHIFT             2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF14_MASK              0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF14_SHIFT             4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF15_MASK              0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF15_SHIFT             6
+	u8 flags6;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF16_MASK              0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF16_SHIFT             0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF17_MASK              0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF17_SHIFT             2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF18_MASK              0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF18_SHIFT             4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF19_MASK              0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF19_SHIFT             6
+	u8 flags7;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF20_MASK              0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF20_SHIFT             0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF21_MASK              0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF21_SHIFT             2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_SLOW_PATH_MASK         0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_SLOW_PATH_SHIFT        4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF0EN_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF0EN_SHIFT            6
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF1EN_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF1EN_SHIFT            7
+	u8 flags8;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF2EN_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF2EN_SHIFT            0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF3EN_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF3EN_SHIFT            1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF4EN_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF4EN_SHIFT            2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF5EN_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF5EN_SHIFT            3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF6EN_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF6EN_SHIFT            4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_FLUSH_Q0_CF_EN_MASK    0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_FLUSH_Q0_CF_EN_SHIFT   5
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF8EN_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF8EN_SHIFT            6
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF9EN_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF9EN_SHIFT            7
+	u8 flags9;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF10EN_MASK            0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF10EN_SHIFT           0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF11EN_MASK            0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF11EN_SHIFT           1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF12EN_MASK            0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF12EN_SHIFT           2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF13EN_MASK            0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF13EN_SHIFT           3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF14EN_MASK            0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF14EN_SHIFT           4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF15EN_MASK            0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF15EN_SHIFT           5
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF16EN_MASK            0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF16EN_SHIFT           6
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF17EN_MASK            0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF17EN_SHIFT           7
+	u8 flags10;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF18EN_MASK            0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF18EN_SHIFT           0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF19EN_MASK            0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF19EN_SHIFT           1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF20EN_MASK            0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF20EN_SHIFT           2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF21EN_MASK            0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF21EN_SHIFT           3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_SLOW_PATH_EN_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_SLOW_PATH_EN_SHIFT     4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF23EN_MASK            0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF23EN_SHIFT           5
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE0EN_MASK           0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE0EN_SHIFT          6
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE1EN_MASK           0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE1EN_SHIFT          7
+	u8 flags11;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE2EN_MASK           0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE2EN_SHIFT          0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE3EN_MASK           0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE3EN_SHIFT          1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE4EN_MASK           0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE4EN_SHIFT          2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE5EN_MASK           0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE5EN_SHIFT          3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE6EN_MASK           0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE6EN_SHIFT          4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE7EN_MASK           0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE7EN_SHIFT          5
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED1_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED1_SHIFT     6
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE9EN_MASK           0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE9EN_SHIFT          7
+	u8 flags12;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE10EN_MASK          0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE10EN_SHIFT         0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE11EN_MASK          0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE11EN_SHIFT         1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED2_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED2_SHIFT     2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED3_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED3_SHIFT     3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE14EN_MASK          0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE14EN_SHIFT         4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE15EN_MASK          0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE15EN_SHIFT         5
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE16EN_MASK          0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE16EN_SHIFT         6
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE17EN_MASK          0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE17EN_SHIFT         7
+	u8 flags13;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE18EN_MASK          0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE18EN_SHIFT         0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE19EN_MASK          0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE19EN_SHIFT         1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED4_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED4_SHIFT     2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED5_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED5_SHIFT     3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED6_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED6_SHIFT     4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED7_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED7_SHIFT     5
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED8_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED8_SHIFT     6
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED9_MASK      0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED9_SHIFT     7
+	u8 flags14;
+#define XSTORMROCECONNAGCTXDQEXTLDPART_MIGRATION_MASK         0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_MIGRATION_SHIFT        0
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT17_MASK             0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT17_SHIFT            1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_DPM_PORT_NUM_MASK      0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_DPM_PORT_NUM_SHIFT     2
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RESERVED_MASK          0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_RESERVED_SHIFT         4
+#define XSTORMROCECONNAGCTXDQEXTLDPART_ROCE_EDPM_ENABLE_MASK  0x1
+#define XSTORMROCECONNAGCTXDQEXTLDPART_ROCE_EDPM_ENABLE_SHIFT 5
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF23_MASK              0x3
+#define XSTORMROCECONNAGCTXDQEXTLDPART_CF23_SHIFT             6
+	u8 byte2;
+	__le16 physical_q0;
+	__le16 word1;
+	__le16 word2;
+	__le16 word3;
+	__le16 word4;
+	__le16 word5;
+	__le16 conn_dpi;
+	u8 byte3;
+	u8 byte4;
+	u8 byte5;
+	u8 byte6;
+	__le32 reg0;
+	__le32 reg1;
+	__le32 reg2;
+	__le32 snd_nxt_psn;
+	__le32 reg4;
+};
+
 struct mstorm_rdma_conn_ag_ctx {
 	u8 byte0;
 	u8 byte1;
@@ -6438,233 +7072,6 @@ struct ustorm_rdma_conn_ag_ctx {
 	__le16 word3;
 };
 
-struct xstorm_roce_conn_ag_ctx_dq_ext_ld_part {
-	u8 reserved0;
-	u8 state;
-	u8 flags0;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_EXIST_IN_QM0_MASK      0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_EXIST_IN_QM0_SHIFT     0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT1_MASK              0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT1_SHIFT             1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT2_MASK              0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT2_SHIFT             2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_EXIST_IN_QM3_MASK      0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_EXIST_IN_QM3_SHIFT     3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT4_MASK              0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT4_SHIFT             4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT5_MASK              0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT5_SHIFT             5
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT6_MASK              0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT6_SHIFT             6
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT7_MASK              0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT7_SHIFT             7
-	u8 flags1;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT8_MASK              0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT8_SHIFT             0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT9_MASK              0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT9_SHIFT             1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT10_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT10_SHIFT            2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT11_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT11_SHIFT            3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT12_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT12_SHIFT            4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT13_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT13_SHIFT            5
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT14_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT14_SHIFT            6
-#define XSTORMROCECONNAGCTXDQEXTLDPART_YSTORM_FLUSH_MASK      0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_YSTORM_FLUSH_SHIFT     7
-	u8 flags2;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF0_MASK               0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF0_SHIFT              0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF1_MASK               0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF1_SHIFT              2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF2_MASK               0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF2_SHIFT              4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF3_MASK               0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF3_SHIFT              6
-	u8 flags3;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF4_MASK               0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF4_SHIFT              0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF5_MASK               0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF5_SHIFT              2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF6_MASK               0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF6_SHIFT              4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_FLUSH_Q0_CF_MASK       0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_FLUSH_Q0_CF_SHIFT      6
-	u8 flags4;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF8_MASK               0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF8_SHIFT              0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF9_MASK               0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF9_SHIFT              2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF10_MASK              0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF10_SHIFT             4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF11_MASK              0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF11_SHIFT             6
-	u8 flags5;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF12_MASK              0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF12_SHIFT             0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF13_MASK              0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF13_SHIFT             2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF14_MASK              0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF14_SHIFT             4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF15_MASK              0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF15_SHIFT             6
-	u8 flags6;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF16_MASK              0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF16_SHIFT             0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF17_MASK              0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF17_SHIFT             2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF18_MASK              0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF18_SHIFT             4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF19_MASK              0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF19_SHIFT             6
-	u8 flags7;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF20_MASK              0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF20_SHIFT             0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF21_MASK              0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF21_SHIFT             2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_SLOW_PATH_MASK         0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_SLOW_PATH_SHIFT        4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF0EN_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF0EN_SHIFT            6
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF1EN_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF1EN_SHIFT            7
-	u8 flags8;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF2EN_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF2EN_SHIFT            0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF3EN_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF3EN_SHIFT            1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF4EN_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF4EN_SHIFT            2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF5EN_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF5EN_SHIFT            3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF6EN_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF6EN_SHIFT            4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_FLUSH_Q0_CF_EN_MASK    0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_FLUSH_Q0_CF_EN_SHIFT   5
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF8EN_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF8EN_SHIFT            6
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF9EN_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF9EN_SHIFT            7
-	u8 flags9;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF10EN_MASK            0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF10EN_SHIFT           0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF11EN_MASK            0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF11EN_SHIFT           1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF12EN_MASK            0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF12EN_SHIFT           2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF13EN_MASK            0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF13EN_SHIFT           3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF14EN_MASK            0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF14EN_SHIFT           4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF15EN_MASK            0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF15EN_SHIFT           5
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF16EN_MASK            0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF16EN_SHIFT           6
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF17EN_MASK            0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF17EN_SHIFT           7
-	u8 flags10;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF18EN_MASK            0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF18EN_SHIFT           0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF19EN_MASK            0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF19EN_SHIFT           1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF20EN_MASK            0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF20EN_SHIFT           2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF21EN_MASK            0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF21EN_SHIFT           3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_SLOW_PATH_EN_MASK      0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_SLOW_PATH_EN_SHIFT     4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF23EN_MASK            0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF23EN_SHIFT           5
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE0EN_MASK           0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE0EN_SHIFT          6
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE1EN_MASK           0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE1EN_SHIFT          7
-	u8 flags11;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE2EN_MASK           0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE2EN_SHIFT          0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE3EN_MASK           0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE3EN_SHIFT          1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE4EN_MASK           0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE4EN_SHIFT          2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE5EN_MASK           0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE5EN_SHIFT          3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE6EN_MASK           0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE6EN_SHIFT          4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE7EN_MASK           0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE7EN_SHIFT          5
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED1_MASK      0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED1_SHIFT     6
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE9EN_MASK           0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE9EN_SHIFT          7
-	u8 flags12;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE10EN_MASK          0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE10EN_SHIFT         0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE11EN_MASK          0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE11EN_SHIFT         1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED2_MASK      0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED2_SHIFT     2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED3_MASK      0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED3_SHIFT     3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE14EN_MASK          0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE14EN_SHIFT         4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE15EN_MASK          0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE15EN_SHIFT         5
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE16EN_MASK          0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE16EN_SHIFT         6
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE17EN_MASK          0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE17EN_SHIFT         7
-	u8 flags13;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE18EN_MASK          0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE18EN_SHIFT         0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE19EN_MASK          0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RULE19EN_SHIFT         1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED4_MASK      0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED4_SHIFT     2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED5_MASK      0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED5_SHIFT     3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED6_MASK      0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED6_SHIFT     4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED7_MASK      0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED7_SHIFT     5
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED8_MASK      0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED8_SHIFT     6
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED9_MASK      0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_A0_RESERVED9_SHIFT     7
-	u8 flags14;
-#define XSTORMROCECONNAGCTXDQEXTLDPART_MIGRATION_MASK         0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_MIGRATION_SHIFT        0
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT17_MASK             0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_BIT17_SHIFT            1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_DPM_PORT_NUM_MASK      0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_DPM_PORT_NUM_SHIFT     2
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RESERVED_MASK          0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_RESERVED_SHIFT         4
-#define XSTORMROCECONNAGCTXDQEXTLDPART_ROCE_EDPM_ENABLE_MASK  0x1
-#define XSTORMROCECONNAGCTXDQEXTLDPART_ROCE_EDPM_ENABLE_SHIFT 5
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF23_MASK              0x3
-#define XSTORMROCECONNAGCTXDQEXTLDPART_CF23_SHIFT             6
-	u8 byte2;
-	__le16 physical_q0;
-	__le16 word1;
-	__le16 word2;
-	__le16 word3;
-	__le16 word4;
-	__le16 word5;
-	__le16 conn_dpi;
-	u8 byte3;
-	u8 byte4;
-	u8 byte5;
-	u8 byte6;
-	__le32 reg0;
-	__le32 reg1;
-	__le32 reg2;
-	__le32 snd_nxt_psn;
-	__le32 reg4;
-};
-
 struct xstorm_rdma_conn_ag_ctx {
 	u8 reserved0;
 	u8 state;
@@ -6696,8 +7103,8 @@ struct xstorm_rdma_conn_ag_ctx {
 #define XSTORM_RDMA_CONN_AG_CTX_BIT11_SHIFT            3
 #define XSTORM_RDMA_CONN_AG_CTX_BIT12_MASK             0x1
 #define XSTORM_RDMA_CONN_AG_CTX_BIT12_SHIFT            4
-#define XSTORM_RDMA_CONN_AG_CTX_BIT13_MASK             0x1
-#define XSTORM_RDMA_CONN_AG_CTX_BIT13_SHIFT            5
+#define XSTORM_RDMA_CONN_AG_CTX_MSTORM_FLUSH_MASK      0x1
+#define XSTORM_RDMA_CONN_AG_CTX_MSTORM_FLUSH_SHIFT     5
 #define XSTORM_RDMA_CONN_AG_CTX_BIT14_MASK             0x1
 #define XSTORM_RDMA_CONN_AG_CTX_BIT14_SHIFT            6
 #define XSTORM_RDMA_CONN_AG_CTX_YSTORM_FLUSH_MASK      0x1
@@ -7093,16 +7500,35 @@ struct roce_destroy_qp_resp_ramrod_data {
 	struct regpair output_params_addr;
 };
 
+struct roce_events_stats {
+	__le16 silent_drops;
+	__le16 rnr_naks_sent;
+	__le32 retransmit_count;
+	__le32 icrc_error_count;
+	__le32 reserved;
+};
+
 enum roce_event_opcode {
 	ROCE_EVENT_CREATE_QP = 11,
 	ROCE_EVENT_MODIFY_QP,
 	ROCE_EVENT_QUERY_QP,
 	ROCE_EVENT_DESTROY_QP,
+	ROCE_EVENT_CREATE_UD_QP,
+	ROCE_EVENT_DESTROY_UD_QP,
 	MAX_ROCE_EVENT_OPCODE
 };
 
+struct roce_init_func_params {
+	u8 ll2_queue_id;
+	u8 cnp_vlan_priority;
+	u8 cnp_dscp;
+	u8 reserved;
+	__le32 cnp_send_timeout;
+};
+
 struct roce_init_func_ramrod_data {
 	struct rdma_init_func_ramrod_data rdma;
+	struct roce_init_func_params roce;
 };
 
 struct roce_modify_qp_req_ramrod_data {
@@ -7222,6 +7648,8 @@ enum roce_ramrod_cmd_id {
 	ROCE_RAMROD_MODIFY_QP,
 	ROCE_RAMROD_QUERY_QP,
 	ROCE_RAMROD_DESTROY_QP,
+	ROCE_RAMROD_CREATE_UD_QP,
+	ROCE_RAMROD_DESTROY_UD_QP,
 	MAX_ROCE_RAMROD_CMD_ID
 };
 
@@ -7299,13 +7727,6 @@ struct mstorm_roce_resp_conn_ag_ctx {
 	__le32 reg1;
 };
 
-enum roce_flavor {
-	PLAIN_ROCE /* RoCE v1 */ ,
-	RROCE_IPV4 /* RoCE v2 (Routable RoCE) over ipv4 */ ,
-	RROCE_IPV6 /* RoCE v2 (Routable RoCE) over ipv6 */ ,
-	MAX_ROCE_FLAVOR
-};
-
 struct tstorm_roce_req_conn_ag_ctx {
 	u8 reserved0;
 	u8 state;
@@ -7416,8 +7837,8 @@ struct tstorm_roce_resp_conn_ag_ctx {
 	u8 flags0;
 #define TSTORM_ROCE_RESP_CONN_AG_CTX_EXIST_IN_QM0_MASK        0x1
 #define TSTORM_ROCE_RESP_CONN_AG_CTX_EXIST_IN_QM0_SHIFT       0
-#define TSTORM_ROCE_RESP_CONN_AG_CTX_BIT1_MASK                0x1
-#define TSTORM_ROCE_RESP_CONN_AG_CTX_BIT1_SHIFT               1
+#define TSTORM_ROCE_RESP_CONN_AG_CTX_RX_ERROR_NOTIFY_REQUESTER_MASK  0x1
+#define TSTORM_ROCE_RESP_CONN_AG_CTX_RX_ERROR_NOTIFY_REQUESTER_SHIFT 1
 #define TSTORM_ROCE_RESP_CONN_AG_CTX_BIT2_MASK                0x1
 #define TSTORM_ROCE_RESP_CONN_AG_CTX_BIT2_SHIFT               2
 #define TSTORM_ROCE_RESP_CONN_AG_CTX_BIT3_MASK                0x1
@@ -8097,7 +8518,7 @@ struct xstorm_roce_resp_conn_ag_ctx {
 	__le16 irq_prod;
 	__le16 word3;
 	__le16 word4;
-	__le16 word5;
+	__le16 ereserved1;
 	__le16 irq_cons;
 	u8 rxmit_opcode;
 	u8 byte4;
@@ -8200,6 +8621,812 @@ struct ystorm_roce_resp_conn_ag_ctx {
 	__le32 reg3;
 };
 
+enum roce_flavor {
+	PLAIN_ROCE,
+	RROCE_IPV4,
+	RROCE_IPV6,
+	MAX_ROCE_FLAVOR
+};
+
+struct ystorm_iwarp_conn_st_ctx {
+	__le32 reserved[4];
+};
+
+struct pstorm_iwarp_conn_st_ctx {
+	__le32 reserved[36];
+};
+
+struct xstorm_iwarp_conn_st_ctx {
+	__le32 reserved[44];
+};
+
+struct xstorm_iwarp_conn_ag_ctx {
+	u8 reserved0;
+	u8 state;
+	u8 flags0;
+#define XSTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM0_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM0_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM1_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM1_SHIFT	1
+#define XSTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM2_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM2_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM3_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM3_SHIFT	3
+#define XSTORM_IWARP_CONN_AG_CTX_BIT4_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT4_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_RESERVED2_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_RESERVED2_SHIFT	5
+#define XSTORM_IWARP_CONN_AG_CTX_BIT6_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT6_SHIFT	6
+#define XSTORM_IWARP_CONN_AG_CTX_BIT7_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT7_SHIFT	7
+	u8 flags1;
+#define XSTORM_IWARP_CONN_AG_CTX_BIT8_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT8_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_BIT9_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT9_SHIFT	1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT10_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT10_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_BIT11_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT11_SHIFT	3
+#define XSTORM_IWARP_CONN_AG_CTX_BIT12_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT12_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_BIT13_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT13_SHIFT	5
+#define XSTORM_IWARP_CONN_AG_CTX_BIT14_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT14_SHIFT	6
+#define XSTORM_IWARP_CONN_AG_CTX_YSTORM_FLUSH_OR_REWIND_SND_MAX_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_YSTORM_FLUSH_OR_REWIND_SND_MAX_SHIFT	7
+	u8 flags2;
+#define XSTORM_IWARP_CONN_AG_CTX_CF0_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF0_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_CF1_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF1_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_CF2_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF2_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_TIMER_STOP_ALL_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_TIMER_STOP_ALL_SHIFT	6
+	u8 flags3;
+#define XSTORM_IWARP_CONN_AG_CTX_CF4_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF4_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_CF5_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF5_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_CF6_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF6_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_CF7_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF7_SHIFT	6
+	u8 flags4;
+#define XSTORM_IWARP_CONN_AG_CTX_CF8_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF8_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_CF9_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF9_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_CF10_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF10_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_CF11_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF11_SHIFT	6
+	u8 flags5;
+#define XSTORM_IWARP_CONN_AG_CTX_CF12_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF12_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_CF13_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF13_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_SQ_FLUSH_CF_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_SQ_FLUSH_CF_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_CF15_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF15_SHIFT	6
+	u8 flags6;
+#define XSTORM_IWARP_CONN_AG_CTX_MPA_OR_ERROR_WAKEUP_TRIGGER_CF_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_MPA_OR_ERROR_WAKEUP_TRIGGER_CF_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_CF17_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF17_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_CF18_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF18_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_DQ_FLUSH_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_DQ_FLUSH_SHIFT	6
+	u8 flags7;
+#define XSTORM_IWARP_CONN_AG_CTX_FLUSH_Q0_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_FLUSH_Q0_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_FLUSH_Q1_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_FLUSH_Q1_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_SLOW_PATH_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_SLOW_PATH_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_CF0EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF0EN_SHIFT	6
+#define XSTORM_IWARP_CONN_AG_CTX_CF1EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF1EN_SHIFT	7
+	u8 flags8;
+#define XSTORM_IWARP_CONN_AG_CTX_CF2EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF2EN_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_TIMER_STOP_ALL_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_TIMER_STOP_ALL_EN_SHIFT	1
+#define XSTORM_IWARP_CONN_AG_CTX_CF4EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF4EN_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_CF5EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF5EN_SHIFT	3
+#define XSTORM_IWARP_CONN_AG_CTX_CF6EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF6EN_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_CF7EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF7EN_SHIFT	5
+#define XSTORM_IWARP_CONN_AG_CTX_CF8EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF8EN_SHIFT	6
+#define XSTORM_IWARP_CONN_AG_CTX_CF9EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF9EN_SHIFT	7
+	u8 flags9;
+#define XSTORM_IWARP_CONN_AG_CTX_CF10EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF10EN_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_CF11EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF11EN_SHIFT	1
+#define XSTORM_IWARP_CONN_AG_CTX_CF12EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF12EN_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_CF13EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF13EN_SHIFT	3
+#define XSTORM_IWARP_CONN_AG_CTX_SQ_FLUSH_CF_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_SQ_FLUSH_CF_EN_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_CF15EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF15EN_SHIFT	5
+#define XSTORM_IWARP_CONN_AG_CTX_MPA_OR_ERROR_WAKEUP_TRIGGER_CF_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_MPA_OR_ERROR_WAKEUP_TRIGGER_CF_EN_SHIFT 6
+#define XSTORM_IWARP_CONN_AG_CTX_CF17EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF17EN_SHIFT	7
+	u8 flags10;
+#define XSTORM_IWARP_CONN_AG_CTX_CF18EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF18EN_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_DQ_FLUSH_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_DQ_FLUSH_EN_SHIFT	1
+#define XSTORM_IWARP_CONN_AG_CTX_FLUSH_Q0_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_FLUSH_Q0_EN_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_FLUSH_Q1_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_FLUSH_Q1_EN_SHIFT	3
+#define XSTORM_IWARP_CONN_AG_CTX_SLOW_PATH_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_SLOW_PATH_EN_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_CF23EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_CF23EN_SHIFT	5
+#define XSTORM_IWARP_CONN_AG_CTX_RULE0EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_RULE0EN_SHIFT	6
+#define XSTORM_IWARP_CONN_AG_CTX_MORE_TO_SEND_RULE_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_MORE_TO_SEND_RULE_EN_SHIFT	7
+	u8 flags11;
+#define XSTORM_IWARP_CONN_AG_CTX_TX_BLOCKED_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_TX_BLOCKED_EN_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_RULE3EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_RULE3EN_SHIFT	1
+#define XSTORM_IWARP_CONN_AG_CTX_RESERVED3_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_RESERVED3_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_RULE5EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_RULE5EN_SHIFT	3
+#define XSTORM_IWARP_CONN_AG_CTX_RULE6EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_RULE6EN_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_RULE7EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_RULE7EN_SHIFT	5
+#define XSTORM_IWARP_CONN_AG_CTX_A0_RESERVED1_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_A0_RESERVED1_SHIFT	6
+#define XSTORM_IWARP_CONN_AG_CTX_RULE9EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_RULE9EN_SHIFT	7
+	u8 flags12;
+#define XSTORM_IWARP_CONN_AG_CTX_SQ_NOT_EMPTY_RULE_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_SQ_NOT_EMPTY_RULE_EN_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_RULE11EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_RULE11EN_SHIFT	1
+#define XSTORM_IWARP_CONN_AG_CTX_A0_RESERVED2_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_A0_RESERVED2_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_A0_RESERVED3_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_A0_RESERVED3_SHIFT	3
+#define XSTORM_IWARP_CONN_AG_CTX_SQ_FENCE_RULE_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_SQ_FENCE_RULE_EN_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_RULE15EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_RULE15EN_SHIFT	5
+#define XSTORM_IWARP_CONN_AG_CTX_RULE16EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_RULE16EN_SHIFT	6
+#define XSTORM_IWARP_CONN_AG_CTX_RULE17EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_RULE17EN_SHIFT	7
+	u8 flags13;
+#define XSTORM_IWARP_CONN_AG_CTX_IRQ_NOT_EMPTY_RULE_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_IRQ_NOT_EMPTY_RULE_EN_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_HQ_NOT_FULL_RULE_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_HQ_NOT_FULL_RULE_EN_SHIFT	1
+#define XSTORM_IWARP_CONN_AG_CTX_ORQ_RD_FENCE_RULE_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_ORQ_RD_FENCE_RULE_EN_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_RULE21EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_RULE21EN_SHIFT	3
+#define XSTORM_IWARP_CONN_AG_CTX_A0_RESERVED6_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_A0_RESERVED6_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_ORQ_NOT_FULL_RULE_EN_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_ORQ_NOT_FULL_RULE_EN_SHIFT	5
+#define XSTORM_IWARP_CONN_AG_CTX_A0_RESERVED8_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_A0_RESERVED8_SHIFT	6
+#define XSTORM_IWARP_CONN_AG_CTX_A0_RESERVED9_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_A0_RESERVED9_SHIFT	7
+	u8 flags14;
+#define XSTORM_IWARP_CONN_AG_CTX_BIT16_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT16_SHIFT	0
+#define XSTORM_IWARP_CONN_AG_CTX_BIT17_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT17_SHIFT	1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT18_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_BIT18_SHIFT	2
+#define XSTORM_IWARP_CONN_AG_CTX_E5_RESERVED1_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_E5_RESERVED1_SHIFT	3
+#define XSTORM_IWARP_CONN_AG_CTX_E5_RESERVED2_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_E5_RESERVED2_SHIFT	4
+#define XSTORM_IWARP_CONN_AG_CTX_E5_RESERVED3_MASK	0x1
+#define XSTORM_IWARP_CONN_AG_CTX_E5_RESERVED3_SHIFT	5
+#define XSTORM_IWARP_CONN_AG_CTX_CF23_MASK	0x3
+#define XSTORM_IWARP_CONN_AG_CTX_CF23_SHIFT	6
+	u8 byte2;
+	__le16 physical_q0;
+	__le16 physical_q1;
+	__le16 sq_comp_cons;
+	__le16 sq_tx_cons;
+	__le16 sq_prod;
+	__le16 word5;
+	__le16 conn_dpi;
+	u8 byte3;
+	u8 byte4;
+	u8 byte5;
+	u8 byte6;
+	__le32 reg0;
+	__le32 reg1;
+	__le32 reg2;
+	__le32 more_to_send_seq;
+	__le32 reg4;
+	__le32 rewinded_snd_max;
+	__le32 rd_msn;
+	__le16 irq_prod_via_msdm;
+	__le16 irq_cons;
+	__le16 hq_cons_th_or_mpa_data;
+	__le16 hq_cons;
+	__le32 atom_msn;
+	__le32 orq_cons;
+	__le32 orq_cons_th;
+	u8 byte7;
+	u8 max_ord;
+	u8 wqe_data_pad_bytes;
+	u8 former_hq_prod;
+	u8 irq_prod_via_msem;
+	u8 byte12;
+	u8 max_pkt_pdu_size_lo;
+	u8 max_pkt_pdu_size_hi;
+	u8 byte15;
+	u8 e5_reserved;
+	__le16 e5_reserved4;
+	__le32 reg10;
+	__le32 reg11;
+	__le32 shared_queue_page_addr_lo;
+	__le32 shared_queue_page_addr_hi;
+	__le32 reg14;
+	__le32 reg15;
+	__le32 reg16;
+	__le32 reg17;
+};
+
+struct tstorm_iwarp_conn_ag_ctx {
+	u8 reserved0;
+	u8 state;
+	u8 flags0;
+#define TSTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM0_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM0_SHIFT	0
+#define TSTORM_IWARP_CONN_AG_CTX_BIT1_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_BIT1_SHIFT	1
+#define TSTORM_IWARP_CONN_AG_CTX_BIT2_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_BIT2_SHIFT	2
+#define TSTORM_IWARP_CONN_AG_CTX_MSTORM_FLUSH_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_MSTORM_FLUSH_SHIFT	3
+#define TSTORM_IWARP_CONN_AG_CTX_BIT4_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_BIT4_SHIFT	4
+#define TSTORM_IWARP_CONN_AG_CTX_CACHED_ORQ_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_CACHED_ORQ_SHIFT	5
+#define TSTORM_IWARP_CONN_AG_CTX_CF0_MASK	0x3
+#define TSTORM_IWARP_CONN_AG_CTX_CF0_SHIFT	6
+	u8 flags1;
+#define TSTORM_IWARP_CONN_AG_CTX_RQ_POST_CF_MASK	0x3
+#define TSTORM_IWARP_CONN_AG_CTX_RQ_POST_CF_SHIFT	0
+#define TSTORM_IWARP_CONN_AG_CTX_MPA_TIMEOUT_CF_MASK	0x3
+#define TSTORM_IWARP_CONN_AG_CTX_MPA_TIMEOUT_CF_SHIFT	2
+#define TSTORM_IWARP_CONN_AG_CTX_TIMER_STOP_ALL_MASK	0x3
+#define TSTORM_IWARP_CONN_AG_CTX_TIMER_STOP_ALL_SHIFT	4
+#define TSTORM_IWARP_CONN_AG_CTX_CF4_MASK	0x3
+#define TSTORM_IWARP_CONN_AG_CTX_CF4_SHIFT	6
+	u8 flags2;
+#define TSTORM_IWARP_CONN_AG_CTX_CF5_MASK	0x3
+#define TSTORM_IWARP_CONN_AG_CTX_CF5_SHIFT	0
+#define TSTORM_IWARP_CONN_AG_CTX_CF6_MASK	0x3
+#define TSTORM_IWARP_CONN_AG_CTX_CF6_SHIFT	2
+#define TSTORM_IWARP_CONN_AG_CTX_CF7_MASK	0x3
+#define TSTORM_IWARP_CONN_AG_CTX_CF7_SHIFT	4
+#define TSTORM_IWARP_CONN_AG_CTX_CF8_MASK	0x3
+#define TSTORM_IWARP_CONN_AG_CTX_CF8_SHIFT	6
+	u8 flags3;
+#define TSTORM_IWARP_CONN_AG_CTX_FLUSH_Q0_MASK	0x3
+#define TSTORM_IWARP_CONN_AG_CTX_FLUSH_Q0_SHIFT	0
+#define TSTORM_IWARP_CONN_AG_CTX_FLUSH_OR_ERROR_DETECTED_MASK	0x3
+#define TSTORM_IWARP_CONN_AG_CTX_FLUSH_OR_ERROR_DETECTED_SHIFT	2
+#define TSTORM_IWARP_CONN_AG_CTX_CF0EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_CF0EN_SHIFT	4
+#define TSTORM_IWARP_CONN_AG_CTX_RQ_POST_CF_EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_RQ_POST_CF_EN_SHIFT	5
+#define TSTORM_IWARP_CONN_AG_CTX_MPA_TIMEOUT_CF_EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_MPA_TIMEOUT_CF_EN_SHIFT	6
+#define TSTORM_IWARP_CONN_AG_CTX_TIMER_STOP_ALL_EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_TIMER_STOP_ALL_EN_SHIFT	7
+	u8 flags4;
+#define TSTORM_IWARP_CONN_AG_CTX_CF4EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_CF4EN_SHIFT	0
+#define TSTORM_IWARP_CONN_AG_CTX_CF5EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_CF5EN_SHIFT	1
+#define TSTORM_IWARP_CONN_AG_CTX_CF6EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_CF6EN_SHIFT	2
+#define TSTORM_IWARP_CONN_AG_CTX_CF7EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_CF7EN_SHIFT	3
+#define TSTORM_IWARP_CONN_AG_CTX_CF8EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_CF8EN_SHIFT	4
+#define TSTORM_IWARP_CONN_AG_CTX_FLUSH_Q0_EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_FLUSH_Q0_EN_SHIFT	5
+#define TSTORM_IWARP_CONN_AG_CTX_FLUSH_OR_ERROR_DETECTED_EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_FLUSH_OR_ERROR_DETECTED_EN_SHIFT 6
+#define TSTORM_IWARP_CONN_AG_CTX_RULE0EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_RULE0EN_SHIFT	7
+	u8 flags5;
+#define TSTORM_IWARP_CONN_AG_CTX_RULE1EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_RULE1EN_SHIFT	0
+#define TSTORM_IWARP_CONN_AG_CTX_RULE2EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_RULE2EN_SHIFT	1
+#define TSTORM_IWARP_CONN_AG_CTX_RULE3EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_RULE3EN_SHIFT	2
+#define TSTORM_IWARP_CONN_AG_CTX_RULE4EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_RULE4EN_SHIFT	3
+#define TSTORM_IWARP_CONN_AG_CTX_RULE5EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_RULE5EN_SHIFT	4
+#define TSTORM_IWARP_CONN_AG_CTX_SND_SQ_CONS_RULE_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_SND_SQ_CONS_RULE_SHIFT	5
+#define TSTORM_IWARP_CONN_AG_CTX_RULE7EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_RULE7EN_SHIFT	6
+#define TSTORM_IWARP_CONN_AG_CTX_RULE8EN_MASK	0x1
+#define TSTORM_IWARP_CONN_AG_CTX_RULE8EN_SHIFT	7
+	__le32 reg0;
+	__le32 reg1;
+	__le32 unaligned_nxt_seq;
+	__le32 reg3;
+	__le32 reg4;
+	__le32 reg5;
+	__le32 reg6;
+	__le32 reg7;
+	__le32 reg8;
+	u8 orq_cache_idx;
+	u8 hq_prod;
+	__le16 sq_tx_cons_th;
+	u8 orq_prod;
+	u8 irq_cons;
+	__le16 sq_tx_cons;
+	__le16 conn_dpi;
+	__le16 rq_prod;
+	__le32 snd_seq;
+	__le32 last_hq_sequence;
+};
+
+struct tstorm_iwarp_conn_st_ctx {
+	__le32 reserved[60];
+};
+
+struct mstorm_iwarp_conn_st_ctx {
+	__le32 reserved[32];
+};
+
+struct ustorm_iwarp_conn_st_ctx {
+	__le32 reserved[24];
+};
+
+struct iwarp_conn_context {
+	struct ystorm_iwarp_conn_st_ctx ystorm_st_context;
+	struct regpair ystorm_st_padding[2];
+	struct pstorm_iwarp_conn_st_ctx pstorm_st_context;
+	struct regpair pstorm_st_padding[2];
+	struct xstorm_iwarp_conn_st_ctx xstorm_st_context;
+	struct regpair xstorm_st_padding[2];
+	struct xstorm_iwarp_conn_ag_ctx xstorm_ag_context;
+	struct tstorm_iwarp_conn_ag_ctx tstorm_ag_context;
+	struct timers_context timer_context;
+	struct ustorm_rdma_conn_ag_ctx ustorm_ag_context;
+	struct tstorm_iwarp_conn_st_ctx tstorm_st_context;
+	struct regpair tstorm_st_padding[2];
+	struct mstorm_iwarp_conn_st_ctx mstorm_st_context;
+	struct ustorm_iwarp_conn_st_ctx ustorm_st_context;
+};
+
+struct iwarp_create_qp_ramrod_data {
+	u8 flags;
+#define IWARP_CREATE_QP_RAMROD_DATA_FMR_AND_RESERVED_EN_MASK	0x1
+#define IWARP_CREATE_QP_RAMROD_DATA_FMR_AND_RESERVED_EN_SHIFT 0
+#define IWARP_CREATE_QP_RAMROD_DATA_SIGNALED_COMP_MASK	0x1
+#define IWARP_CREATE_QP_RAMROD_DATA_SIGNALED_COMP_SHIFT	1
+#define IWARP_CREATE_QP_RAMROD_DATA_RDMA_RD_EN_MASK	0x1
+#define IWARP_CREATE_QP_RAMROD_DATA_RDMA_RD_EN_SHIFT	2
+#define IWARP_CREATE_QP_RAMROD_DATA_RDMA_WR_EN_MASK	0x1
+#define IWARP_CREATE_QP_RAMROD_DATA_RDMA_WR_EN_SHIFT	3
+#define IWARP_CREATE_QP_RAMROD_DATA_ATOMIC_EN_MASK	0x1
+#define IWARP_CREATE_QP_RAMROD_DATA_ATOMIC_EN_SHIFT	4
+#define IWARP_CREATE_QP_RAMROD_DATA_SRQ_FLG_MASK	0x1
+#define IWARP_CREATE_QP_RAMROD_DATA_SRQ_FLG_SHIFT	5
+#define IWARP_CREATE_QP_RAMROD_DATA_RESERVED0_MASK	0x3
+#define IWARP_CREATE_QP_RAMROD_DATA_RESERVED0_SHIFT	6
+	u8 reserved1;
+	__le16 pd;
+	__le16 sq_num_pages;
+	__le16 rq_num_pages;
+	__le32 reserved3[2];
+	struct regpair qp_handle_for_cqe;
+	struct rdma_srq_id srq_id;
+	__le32 cq_cid_for_sq;
+	__le32 cq_cid_for_rq;
+	__le16 dpi;
+	__le16 physical_q0;
+	__le16 physical_q1;
+	u8 reserved2[6];
+};
+
+enum iwarp_eqe_async_opcode {
+	IWARP_EVENT_TYPE_ASYNC_CONNECT_COMPLETE,
+	IWARP_EVENT_TYPE_ASYNC_ENHANCED_MPA_REPLY_ARRIVED,
+	IWARP_EVENT_TYPE_ASYNC_MPA_HANDSHAKE_COMPLETE,
+	IWARP_EVENT_TYPE_ASYNC_CID_CLEANED,
+	IWARP_EVENT_TYPE_ASYNC_EXCEPTION_DETECTED,
+	IWARP_EVENT_TYPE_ASYNC_QP_IN_ERROR_STATE,
+	IWARP_EVENT_TYPE_ASYNC_CQ_OVERFLOW,
+	MAX_IWARP_EQE_ASYNC_OPCODE
+};
+
+struct iwarp_eqe_data_mpa_async_completion {
+	__le16 ulp_data_len;
+	u8 reserved[6];
+};
+
+struct iwarp_eqe_data_tcp_async_completion {
+	__le16 ulp_data_len;
+	u8 mpa_handshake_mode;
+	u8 reserved[5];
+};
+
+enum iwarp_eqe_sync_opcode {
+	IWARP_EVENT_TYPE_TCP_OFFLOAD =
+	11,
+	IWARP_EVENT_TYPE_MPA_OFFLOAD,
+	IWARP_EVENT_TYPE_MPA_OFFLOAD_SEND_RTR,
+	IWARP_EVENT_TYPE_CREATE_QP,
+	IWARP_EVENT_TYPE_QUERY_QP,
+	IWARP_EVENT_TYPE_MODIFY_QP,
+	IWARP_EVENT_TYPE_DESTROY_QP,
+	MAX_IWARP_EQE_SYNC_OPCODE
+};
+
+enum iwarp_fw_return_code {
+	IWARP_CONN_ERROR_TCP_CONNECT_INVALID_PACKET = 5,
+	IWARP_CONN_ERROR_TCP_CONNECTION_RST,
+	IWARP_CONN_ERROR_TCP_CONNECT_TIMEOUT,
+	IWARP_CONN_ERROR_MPA_ERROR_REJECT,
+	IWARP_CONN_ERROR_MPA_NOT_SUPPORTED_VER,
+	IWARP_CONN_ERROR_MPA_RST,
+	IWARP_CONN_ERROR_MPA_FIN,
+	IWARP_CONN_ERROR_MPA_RTR_MISMATCH,
+	IWARP_CONN_ERROR_MPA_INSUF_IRD,
+	IWARP_CONN_ERROR_MPA_INVALID_PACKET,
+	IWARP_CONN_ERROR_MPA_LOCAL_ERROR,
+	IWARP_CONN_ERROR_MPA_TIMEOUT,
+	IWARP_CONN_ERROR_MPA_TERMINATE,
+	IWARP_QP_IN_ERROR_GOOD_CLOSE,
+	IWARP_QP_IN_ERROR_BAD_CLOSE,
+	IWARP_EXCEPTION_DETECTED_LLP_CLOSED,
+	IWARP_EXCEPTION_DETECTED_LLP_RESET,
+	IWARP_EXCEPTION_DETECTED_IRQ_FULL,
+	IWARP_EXCEPTION_DETECTED_RQ_EMPTY,
+	IWARP_EXCEPTION_DETECTED_LLP_TIMEOUT,
+	IWARP_EXCEPTION_DETECTED_REMOTE_PROTECTION_ERROR,
+	IWARP_EXCEPTION_DETECTED_CQ_OVERFLOW,
+	IWARP_EXCEPTION_DETECTED_LOCAL_CATASTROPHIC,
+	IWARP_EXCEPTION_DETECTED_LOCAL_ACCESS_ERROR,
+	IWARP_EXCEPTION_DETECTED_REMOTE_OPERATION_ERROR,
+	IWARP_EXCEPTION_DETECTED_TERMINATE_RECEIVED,
+	MAX_IWARP_FW_RETURN_CODE
+};
+
+struct iwarp_init_func_params {
+	u8 ll2_ooo_q_index;
+	u8 reserved1[7];
+};
+
+struct iwarp_init_func_ramrod_data {
+	struct rdma_init_func_ramrod_data rdma;
+	struct tcp_init_params tcp;
+	struct iwarp_init_func_params iwarp;
+};
+
+enum iwarp_modify_qp_new_state_type {
+	IWARP_MODIFY_QP_STATE_CLOSING = 1,
+	IWARP_MODIFY_QP_STATE_ERROR =
+	2,
+	MAX_IWARP_MODIFY_QP_NEW_STATE_TYPE
+};
+
+struct iwarp_modify_qp_ramrod_data {
+	__le16 transition_to_state;
+	__le16 flags;
+#define IWARP_MODIFY_QP_RAMROD_DATA_RDMA_RD_EN_MASK	0x1
+#define IWARP_MODIFY_QP_RAMROD_DATA_RDMA_RD_EN_SHIFT	0
+#define IWARP_MODIFY_QP_RAMROD_DATA_RDMA_WR_EN_MASK	0x1
+#define IWARP_MODIFY_QP_RAMROD_DATA_RDMA_WR_EN_SHIFT	1
+#define IWARP_MODIFY_QP_RAMROD_DATA_ATOMIC_EN_MASK	0x1
+#define IWARP_MODIFY_QP_RAMROD_DATA_ATOMIC_EN_SHIFT	2
+#define IWARP_MODIFY_QP_RAMROD_DATA_STATE_TRANS_EN_MASK	0x1
+#define IWARP_MODIFY_QP_RAMROD_DATA_STATE_TRANS_EN_SHIFT	3
+#define IWARP_MODIFY_QP_RAMROD_DATA_RDMA_OPS_EN_FLG_MASK	0x1
+#define IWARP_MODIFY_QP_RAMROD_DATA_RDMA_OPS_EN_FLG_SHIFT 4
+#define IWARP_MODIFY_QP_RAMROD_DATA_RESERVED_MASK	0x7FF
+#define IWARP_MODIFY_QP_RAMROD_DATA_RESERVED_SHIFT	5
+	__le32 reserved3[3];
+	__le32 reserved4[8];
+};
+
+struct mpa_rq_params {
+	__le32 ird;
+	__le32 ord;
+};
+
+struct mpa_ulp_buffer {
+	struct regpair addr;
+	__le16 len;
+	__le16 reserved[3];
+};
+
+struct mpa_outgoing_params {
+	u8 crc_needed;
+	u8 reject;
+	u8 reserved[6];
+	struct mpa_rq_params out_rq;
+	struct mpa_ulp_buffer outgoing_ulp_buffer;
+};
+
+struct iwarp_mpa_offload_ramrod_data {
+	struct mpa_outgoing_params common;
+	__le32 tcp_cid;
+	u8 mode;
+	u8 tcp_connect_side;
+	u8 rtr_pref;
+#define IWARP_MPA_OFFLOAD_RAMROD_DATA_RTR_SUPPORTED_MASK	0x7
+#define IWARP_MPA_OFFLOAD_RAMROD_DATA_RTR_SUPPORTED_SHIFT 0
+#define IWARP_MPA_OFFLOAD_RAMROD_DATA_RESERVED1_MASK	0x1F
+#define IWARP_MPA_OFFLOAD_RAMROD_DATA_RESERVED1_SHIFT	3
+	u8 reserved2;
+	struct mpa_ulp_buffer incoming_ulp_buffer;
+	struct regpair async_eqe_output_buf;
+	struct regpair handle_for_async;
+	struct regpair shared_queue_addr;
+	u8 stats_counter_id;
+	u8 reserved3[15];
+};
+
+struct iwarp_offload_params {
+	struct mpa_ulp_buffer incoming_ulp_buffer;
+	struct regpair async_eqe_output_buf;
+	struct regpair handle_for_async;
+	__le16 physical_q0;
+	__le16 physical_q1;
+	u8 stats_counter_id;
+	u8 mpa_mode;
+	u8 reserved[10];
+};
+
+struct iwarp_query_qp_output_params {
+	__le32 flags;
+#define IWARP_QUERY_QP_OUTPUT_PARAMS_ERROR_FLG_MASK	0x1
+#define IWARP_QUERY_QP_OUTPUT_PARAMS_ERROR_FLG_SHIFT 0
+#define IWARP_QUERY_QP_OUTPUT_PARAMS_RESERVED0_MASK	0x7FFFFFFF
+#define IWARP_QUERY_QP_OUTPUT_PARAMS_RESERVED0_SHIFT 1
+	u8 reserved1[4];
+};
+
+struct iwarp_query_qp_ramrod_data {
+	struct regpair output_params_addr;
+};
+
+enum iwarp_ramrod_cmd_id {
+	IWARP_RAMROD_CMD_ID_TCP_OFFLOAD =
+	11,
+	IWARP_RAMROD_CMD_ID_MPA_OFFLOAD,
+	IWARP_RAMROD_CMD_ID_MPA_OFFLOAD_SEND_RTR,
+	IWARP_RAMROD_CMD_ID_CREATE_QP,
+	IWARP_RAMROD_CMD_ID_QUERY_QP,
+	IWARP_RAMROD_CMD_ID_MODIFY_QP,
+	IWARP_RAMROD_CMD_ID_DESTROY_QP,
+	MAX_IWARP_RAMROD_CMD_ID
+};
+
+struct iwarp_rxmit_stats_drv {
+	struct regpair tx_go_to_slow_start_event_cnt;
+	struct regpair tx_fast_retransmit_event_cnt;
+};
+
+struct iwarp_tcp_offload_ramrod_data {
+	struct iwarp_offload_params iwarp;
+	struct tcp_offload_params_opt2 tcp;
+};
+
+enum mpa_negotiation_mode {
+	MPA_NEGOTIATION_TYPE_BASIC = 1,
+	MPA_NEGOTIATION_TYPE_ENHANCED = 2,
+	MAX_MPA_NEGOTIATION_MODE
+};
+
+enum mpa_rtr_type {
+	MPA_RTR_TYPE_NONE = 0,
+	MPA_RTR_TYPE_ZERO_SEND = 1,
+	MPA_RTR_TYPE_ZERO_WRITE = 2,
+	MPA_RTR_TYPE_ZERO_SEND_AND_WRITE = 3,
+	MPA_RTR_TYPE_ZERO_READ = 4,
+	MPA_RTR_TYPE_ZERO_SEND_AND_READ = 5,
+	MPA_RTR_TYPE_ZERO_WRITE_AND_READ = 6,
+	MPA_RTR_TYPE_ZERO_SEND_AND_WRITE_AND_READ = 7,
+	MAX_MPA_RTR_TYPE
+};
+
+struct unaligned_opaque_data {
+	__le16 first_mpa_offset;
+	u8 tcp_payload_offset;
+	u8 flags;
+#define UNALIGNED_OPAQUE_DATA_PKT_REACHED_WIN_RIGHT_EDGE_MASK	0x1
+#define UNALIGNED_OPAQUE_DATA_PKT_REACHED_WIN_RIGHT_EDGE_SHIFT 0
+#define UNALIGNED_OPAQUE_DATA_CONNECTION_CLOSED_MASK	0x1
+#define UNALIGNED_OPAQUE_DATA_CONNECTION_CLOSED_SHIFT	1
+#define UNALIGNED_OPAQUE_DATA_RESERVED_MASK	0x3F
+#define UNALIGNED_OPAQUE_DATA_RESERVED_SHIFT	2
+	__le32 cid;
+};
+
+struct mstorm_iwarp_conn_ag_ctx {
+	u8 reserved;
+	u8 state;
+	u8 flags0;
+#define MSTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM0_MASK	0x1
+#define MSTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM0_SHIFT	0
+#define MSTORM_IWARP_CONN_AG_CTX_BIT1_MASK	0x1
+#define MSTORM_IWARP_CONN_AG_CTX_BIT1_SHIFT	1
+#define MSTORM_IWARP_CONN_AG_CTX_INV_STAG_DONE_CF_MASK	0x3
+#define MSTORM_IWARP_CONN_AG_CTX_INV_STAG_DONE_CF_SHIFT	2
+#define MSTORM_IWARP_CONN_AG_CTX_CF1_MASK	0x3
+#define MSTORM_IWARP_CONN_AG_CTX_CF1_SHIFT	4
+#define MSTORM_IWARP_CONN_AG_CTX_CF2_MASK	0x3
+#define MSTORM_IWARP_CONN_AG_CTX_CF2_SHIFT	6
+	u8 flags1;
+#define MSTORM_IWARP_CONN_AG_CTX_INV_STAG_DONE_CF_EN_MASK	0x1
+#define MSTORM_IWARP_CONN_AG_CTX_INV_STAG_DONE_CF_EN_SHIFT 0
+#define MSTORM_IWARP_CONN_AG_CTX_CF1EN_MASK	0x1
+#define MSTORM_IWARP_CONN_AG_CTX_CF1EN_SHIFT	1
+#define MSTORM_IWARP_CONN_AG_CTX_CF2EN_MASK	0x1
+#define MSTORM_IWARP_CONN_AG_CTX_CF2EN_SHIFT	2
+#define MSTORM_IWARP_CONN_AG_CTX_RULE0EN_MASK	0x1
+#define MSTORM_IWARP_CONN_AG_CTX_RULE0EN_SHIFT	3
+#define MSTORM_IWARP_CONN_AG_CTX_RULE1EN_MASK	0x1
+#define MSTORM_IWARP_CONN_AG_CTX_RULE1EN_SHIFT	4
+#define MSTORM_IWARP_CONN_AG_CTX_RULE2EN_MASK	0x1
+#define MSTORM_IWARP_CONN_AG_CTX_RULE2EN_SHIFT	5
+#define MSTORM_IWARP_CONN_AG_CTX_RCQ_CONS_EN_MASK	0x1
+#define MSTORM_IWARP_CONN_AG_CTX_RCQ_CONS_EN_SHIFT	6
+#define MSTORM_IWARP_CONN_AG_CTX_RULE4EN_MASK	0x1
+#define MSTORM_IWARP_CONN_AG_CTX_RULE4EN_SHIFT	7
+	__le16 rcq_cons;
+	__le16 rcq_cons_th;
+	__le32 reg0;
+	__le32 reg1;
+};
+
+struct ustorm_iwarp_conn_ag_ctx {
+	u8 reserved;
+	u8 byte1;
+	u8 flags0;
+#define USTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM0_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_EXIST_IN_QM0_SHIFT	0
+#define USTORM_IWARP_CONN_AG_CTX_BIT1_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_BIT1_SHIFT	1
+#define USTORM_IWARP_CONN_AG_CTX_CF0_MASK	0x3
+#define USTORM_IWARP_CONN_AG_CTX_CF0_SHIFT	2
+#define USTORM_IWARP_CONN_AG_CTX_CF1_MASK	0x3
+#define USTORM_IWARP_CONN_AG_CTX_CF1_SHIFT	4
+#define USTORM_IWARP_CONN_AG_CTX_CF2_MASK	0x3
+#define USTORM_IWARP_CONN_AG_CTX_CF2_SHIFT	6
+	u8 flags1;
+#define USTORM_IWARP_CONN_AG_CTX_CF3_MASK	0x3
+#define USTORM_IWARP_CONN_AG_CTX_CF3_SHIFT	0
+#define USTORM_IWARP_CONN_AG_CTX_CQ_ARM_SE_CF_MASK	0x3
+#define USTORM_IWARP_CONN_AG_CTX_CQ_ARM_SE_CF_SHIFT	2
+#define USTORM_IWARP_CONN_AG_CTX_CQ_ARM_CF_MASK	0x3
+#define USTORM_IWARP_CONN_AG_CTX_CQ_ARM_CF_SHIFT	4
+#define USTORM_IWARP_CONN_AG_CTX_CF6_MASK	0x3
+#define USTORM_IWARP_CONN_AG_CTX_CF6_SHIFT	6
+	u8 flags2;
+#define USTORM_IWARP_CONN_AG_CTX_CF0EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_CF0EN_SHIFT	0
+#define USTORM_IWARP_CONN_AG_CTX_CF1EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_CF1EN_SHIFT	1
+#define USTORM_IWARP_CONN_AG_CTX_CF2EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_CF2EN_SHIFT	2
+#define USTORM_IWARP_CONN_AG_CTX_CF3EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_CF3EN_SHIFT	3
+#define USTORM_IWARP_CONN_AG_CTX_CQ_ARM_SE_CF_EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_CQ_ARM_SE_CF_EN_SHIFT 4
+#define USTORM_IWARP_CONN_AG_CTX_CQ_ARM_CF_EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_CQ_ARM_CF_EN_SHIFT	5
+#define USTORM_IWARP_CONN_AG_CTX_CF6EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_CF6EN_SHIFT	6
+#define USTORM_IWARP_CONN_AG_CTX_CQ_SE_EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_CQ_SE_EN_SHIFT	7
+	u8 flags3;
+#define USTORM_IWARP_CONN_AG_CTX_CQ_EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_CQ_EN_SHIFT	0
+#define USTORM_IWARP_CONN_AG_CTX_RULE2EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_RULE2EN_SHIFT	1
+#define USTORM_IWARP_CONN_AG_CTX_RULE3EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_RULE3EN_SHIFT	2
+#define USTORM_IWARP_CONN_AG_CTX_RULE4EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_RULE4EN_SHIFT	3
+#define USTORM_IWARP_CONN_AG_CTX_RULE5EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_RULE5EN_SHIFT	4
+#define USTORM_IWARP_CONN_AG_CTX_RULE6EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_RULE6EN_SHIFT	5
+#define USTORM_IWARP_CONN_AG_CTX_RULE7EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_RULE7EN_SHIFT	6
+#define USTORM_IWARP_CONN_AG_CTX_RULE8EN_MASK	0x1
+#define USTORM_IWARP_CONN_AG_CTX_RULE8EN_SHIFT	7
+	u8 byte2;
+	u8 byte3;
+	__le16 word0;
+	__le16 word1;
+	__le32 cq_cons;
+	__le32 cq_se_prod;
+	__le32 cq_prod;
+	__le32 reg3;
+	__le16 word2;
+	__le16 word3;
+};
+
+struct ystorm_iwarp_conn_ag_ctx {
+	u8 byte0;
+	u8 byte1;
+	u8 flags0;
+#define YSTORM_IWARP_CONN_AG_CTX_BIT0_MASK	0x1
+#define YSTORM_IWARP_CONN_AG_CTX_BIT0_SHIFT	0
+#define YSTORM_IWARP_CONN_AG_CTX_BIT1_MASK	0x1
+#define YSTORM_IWARP_CONN_AG_CTX_BIT1_SHIFT	1
+#define YSTORM_IWARP_CONN_AG_CTX_CF0_MASK	0x3
+#define YSTORM_IWARP_CONN_AG_CTX_CF0_SHIFT	2
+#define YSTORM_IWARP_CONN_AG_CTX_CF1_MASK	0x3
+#define YSTORM_IWARP_CONN_AG_CTX_CF1_SHIFT	4
+#define YSTORM_IWARP_CONN_AG_CTX_CF2_MASK	0x3
+#define YSTORM_IWARP_CONN_AG_CTX_CF2_SHIFT	6
+	u8 flags1;
+#define YSTORM_IWARP_CONN_AG_CTX_CF0EN_MASK	0x1
+#define YSTORM_IWARP_CONN_AG_CTX_CF0EN_SHIFT	0
+#define YSTORM_IWARP_CONN_AG_CTX_CF1EN_MASK	0x1
+#define YSTORM_IWARP_CONN_AG_CTX_CF1EN_SHIFT	1
+#define YSTORM_IWARP_CONN_AG_CTX_CF2EN_MASK	0x1
+#define YSTORM_IWARP_CONN_AG_CTX_CF2EN_SHIFT	2
+#define YSTORM_IWARP_CONN_AG_CTX_RULE0EN_MASK	0x1
+#define YSTORM_IWARP_CONN_AG_CTX_RULE0EN_SHIFT 3
+#define YSTORM_IWARP_CONN_AG_CTX_RULE1EN_MASK	0x1
+#define YSTORM_IWARP_CONN_AG_CTX_RULE1EN_SHIFT 4
+#define YSTORM_IWARP_CONN_AG_CTX_RULE2EN_MASK	0x1
+#define YSTORM_IWARP_CONN_AG_CTX_RULE2EN_SHIFT 5
+#define YSTORM_IWARP_CONN_AG_CTX_RULE3EN_MASK	0x1
+#define YSTORM_IWARP_CONN_AG_CTX_RULE3EN_SHIFT 6
+#define YSTORM_IWARP_CONN_AG_CTX_RULE4EN_MASK	0x1
+#define YSTORM_IWARP_CONN_AG_CTX_RULE4EN_SHIFT 7
+	u8 byte2;
+	u8 byte3;
+	__le16 word0;
+	__le32 reg0;
+	__le32 reg1;
+	__le16 word1;
+	__le16 word2;
+	__le16 word3;
+	__le16 word4;
+	__le32 reg2;
+	__le32 reg3;
+};
+
 struct ystorm_fcoe_conn_st_ctx {
 	u8 func_mode;
 	u8 cos;
@@ -9222,7 +10449,7 @@ struct xstorm_iscsi_conn_ag_ctx {
 	u8 byte13;
 	u8 byte14;
 	u8 byte15;
-	u8 byte16;
+	u8 ereserved;
 	__le16 word11;
 	__le32 reg10;
 	__le32 reg11;
@@ -10285,9 +11512,11 @@ struct public_drv_mb {
 
 #define DRV_MSG_CODE_BW_UPDATE_ACK		0x32000000
 #define DRV_MSG_CODE_NIG_DRAIN			0x30000000
+#define DRV_MSG_CODE_S_TAG_UPDATE_ACK		0x3b000000
 #define DRV_MSG_CODE_INITIATE_PF_FLR            0x02010000
 #define DRV_MSG_CODE_VF_DISABLED_DONE		0xc0000000
 #define DRV_MSG_CODE_CFG_VF_MSIX		0xc0010000
+#define DRV_MSG_CODE_CFG_PF_VFS_MSIX		0xc0020000
 #define DRV_MSG_CODE_NVM_GET_FILE_ATT		0x00030000
 #define DRV_MSG_CODE_NVM_READ_NVRAM		0x00050000
 #define DRV_MSG_CODE_MCP_RESET			0x00090000
@@ -10444,6 +11673,7 @@ struct public_drv_mb {
 #define FW_MSG_CODE_RESOURCE_ALLOC_OK           0x34000000
 #define FW_MSG_CODE_RESOURCE_ALLOC_UNKNOWN      0x35000000
 #define FW_MSG_CODE_RESOURCE_ALLOC_DEPRECATED   0x36000000
+#define FW_MSG_CODE_S_TAG_UPDATE_ACK_DONE	0x3b000000
 #define FW_MSG_CODE_DRV_CFG_VF_MSIX_DONE	0xb0010000
 
 #define FW_MSG_CODE_NVM_OK			0x00010000
@@ -10451,7 +11681,7 @@ struct public_drv_mb {
 
 #define FW_MSG_CODE_OS_WOL_SUPPORTED            0x00800000
 #define FW_MSG_CODE_OS_WOL_NOT_SUPPORTED        0x00810000
-
+#define FW_MSG_CODE_DRV_CFG_PF_VFS_MSIX_DONE	0x00870000
 #define FW_MSG_SEQ_NUMBER_MASK			0x0000ffff
 
 	u32 fw_mb_param;
@@ -10466,6 +11696,8 @@ struct public_drv_mb {
 #define FW_MB_PARAM_GET_PF_RDMA_IWARP		0x2
 #define FW_MB_PARAM_GET_PF_RDMA_BOTH		0x3
 
+#define FW_MB_PARAM_LOAD_DONE_DID_EFUSE_ERROR	(1 << 0)
+
 	u32 drv_pulse_mb;
 #define DRV_PULSE_SEQ_MASK			0x00007fff
 #define DRV_PULSE_SYSTEM_TIME_MASK		0xffff0000
@@ -10489,7 +11721,7 @@ enum MFW_DRV_MSG_TYPE {
 	MFW_DRV_MSG_DCBX_OPERATIONAL_MIB_UPDATED,
 	MFW_DRV_MSG_RESERVED4,
 	MFW_DRV_MSG_BW_UPDATE,
-	MFW_DRV_MSG_BW_UPDATE5,
+	MFW_DRV_MSG_S_TAG_UPDATE,
 	MFW_DRV_MSG_GET_LAN_STATS,
 	MFW_DRV_MSG_GET_FCOE_STATS,
 	MFW_DRV_MSG_GET_ISCSI_STATS,
@@ -10591,6 +11823,12 @@ struct nvm_cfg1_glob {
 	u32 led_global_settings;
 	u32 generic_cont1;
 	u32 mbi_version;
+#define NVM_CFG1_GLOB_MBI_VERSION_0_MASK		0x000000FF
+#define NVM_CFG1_GLOB_MBI_VERSION_0_OFFSET		0
+#define NVM_CFG1_GLOB_MBI_VERSION_1_MASK		0x0000FF00
+#define NVM_CFG1_GLOB_MBI_VERSION_1_OFFSET		8
+#define NVM_CFG1_GLOB_MBI_VERSION_2_MASK		0x00FF0000
+#define NVM_CFG1_GLOB_MBI_VERSION_2_OFFSET		16
 	u32 mbi_date;
 	u32 misc_sig;
 	u32 device_capabilities;
@@ -10758,6 +11996,8 @@ struct static_init {
 	u32 rsrv_persist[5];	/* Persist reserved for MFW upgrades */
 };
 
+#define NVM_MAGIC_VALUE		0x669955aa
+
 enum nvm_image_type {
 	NVM_TYPE_TIM1 = 0x01,
 	NVM_TYPE_TIM2 = 0x02,
diff --git a/drivers/net/ethernet/qlogic/qed/qed_init_fw_funcs.c b/drivers/net/ethernet/qlogic/qed/qed_init_fw_funcs.c
index 0a8fde629991..b069ad088269 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_init_fw_funcs.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_init_fw_funcs.c
@@ -40,31 +40,17 @@
 #include "qed_init_ops.h"
 #include "qed_reg_addr.h"
 
-enum cminterface {
-	MCM_SEC,
-	MCM_PRI,
-	UCM_SEC,
-	UCM_PRI,
-	TCM_SEC,
-	TCM_PRI,
-	YCM_SEC,
-	YCM_PRI,
-	XCM_SEC,
-	XCM_PRI,
-	NUM_OF_CM_INTERFACES
-};
-
-/* general constants */
+/* General constants */
 #define QM_PQ_MEM_4KB(pq_size)	(pq_size ? DIV_ROUND_UP((pq_size + 1) *	\
 							QM_PQ_ELEMENT_SIZE, \
 							0x1000) : 0)
 #define QM_PQ_SIZE_256B(pq_size)	(pq_size ? DIV_ROUND_UP(pq_size, \
 								0x100) - 1 : 0)
 #define QM_INVALID_PQ_ID                        0xffff
-/* feature enable */
+/* Feature enable */
 #define QM_BYPASS_EN                            1
 #define QM_BYTE_CRD_EN                          1
-/* other PQ constants */
+/* Other PQ constants */
 #define QM_OTHER_PQS_PER_PF                     4
 /* WFQ constants */
 #define QM_WFQ_UPPER_BOUND		62500000
@@ -106,20 +92,21 @@ enum cminterface {
 #define BTB_PURE_LB_FACTOR                      10
 #define BTB_PURE_LB_RATIO                       7
 /* QM stop command constants */
-#define QM_STOP_PQ_MASK_WIDTH                   32
-#define QM_STOP_CMD_ADDR                                0x2
-#define QM_STOP_CMD_STRUCT_SIZE                 2
+#define QM_STOP_PQ_MASK_WIDTH           32
+#define QM_STOP_CMD_ADDR                2
+#define QM_STOP_CMD_STRUCT_SIZE         2
 #define QM_STOP_CMD_PAUSE_MASK_OFFSET   0
 #define QM_STOP_CMD_PAUSE_MASK_SHIFT    0
-#define QM_STOP_CMD_PAUSE_MASK_MASK             -1
-#define QM_STOP_CMD_GROUP_ID_OFFSET             1
-#define QM_STOP_CMD_GROUP_ID_SHIFT              16
-#define QM_STOP_CMD_GROUP_ID_MASK               15
-#define QM_STOP_CMD_PQ_TYPE_OFFSET              1
-#define QM_STOP_CMD_PQ_TYPE_SHIFT               24
-#define QM_STOP_CMD_PQ_TYPE_MASK                1
-#define QM_STOP_CMD_MAX_POLL_COUNT              100
-#define QM_STOP_CMD_POLL_PERIOD_US              500
+#define QM_STOP_CMD_PAUSE_MASK_MASK     -1
+#define QM_STOP_CMD_GROUP_ID_OFFSET     1
+#define QM_STOP_CMD_GROUP_ID_SHIFT      16
+#define QM_STOP_CMD_GROUP_ID_MASK       15
+#define QM_STOP_CMD_PQ_TYPE_OFFSET      1
+#define QM_STOP_CMD_PQ_TYPE_SHIFT       24
+#define QM_STOP_CMD_PQ_TYPE_MASK        1
+#define QM_STOP_CMD_MAX_POLL_COUNT      100
+#define QM_STOP_CMD_POLL_PERIOD_US      500
+
 /* QM command macros */
 #define QM_CMD_STRUCT_SIZE(cmd)			cmd ## \
 	_STRUCT_SIZE
@@ -146,16 +133,17 @@ static void qed_enable_pf_rl(struct qed_hwfn *p_hwfn, bool pf_rl_en)
 {
 	STORE_RT_REG(p_hwfn, QM_REG_RLPFENABLE_RT_OFFSET, pf_rl_en ? 1 : 0);
 	if (pf_rl_en) {
-		/* enable RLs for all VOQs */
+		/* Enable RLs for all VOQs */
 		STORE_RT_REG(p_hwfn, QM_REG_RLPFVOQENABLE_RT_OFFSET,
 			     (1 << MAX_NUM_VOQS) - 1);
-		/* write RL period */
+		/* Write RL period */
 		STORE_RT_REG(p_hwfn,
 			     QM_REG_RLPFPERIOD_RT_OFFSET, QM_RL_PERIOD_CLK_25M);
 		STORE_RT_REG(p_hwfn,
 			     QM_REG_RLPFPERIODTIMER_RT_OFFSET,
 			     QM_RL_PERIOD_CLK_25M);
-		/* set credit threshold for QM bypass flow */
+
+		/* Set credit threshold for QM bypass flow */
 		if (QM_BYPASS_EN)
 			STORE_RT_REG(p_hwfn,
 				     QM_REG_AFULLQMBYPTHRPFRL_RT_OFFSET,
@@ -167,7 +155,8 @@ static void qed_enable_pf_rl(struct qed_hwfn *p_hwfn, bool pf_rl_en)
 static void qed_enable_pf_wfq(struct qed_hwfn *p_hwfn, bool pf_wfq_en)
 {
 	STORE_RT_REG(p_hwfn, QM_REG_WFQPFENABLE_RT_OFFSET, pf_wfq_en ? 1 : 0);
-	/* set credit threshold for QM bypass flow */
+
+	/* Set credit threshold for QM bypass flow */
 	if (pf_wfq_en && QM_BYPASS_EN)
 		STORE_RT_REG(p_hwfn,
 			     QM_REG_AFULLQMBYPTHRPFWFQ_RT_OFFSET,
@@ -180,14 +169,15 @@ static void qed_enable_vport_rl(struct qed_hwfn *p_hwfn, bool vport_rl_en)
 	STORE_RT_REG(p_hwfn, QM_REG_RLGLBLENABLE_RT_OFFSET,
 		     vport_rl_en ? 1 : 0);
 	if (vport_rl_en) {
-		/* write RL period (use timer 0 only) */
+		/* Write RL period (use timer 0 only) */
 		STORE_RT_REG(p_hwfn,
 			     QM_REG_RLGLBLPERIOD_0_RT_OFFSET,
 			     QM_RL_PERIOD_CLK_25M);
 		STORE_RT_REG(p_hwfn,
 			     QM_REG_RLGLBLPERIODTIMER_0_RT_OFFSET,
 			     QM_RL_PERIOD_CLK_25M);
-		/* set credit threshold for QM bypass flow */
+
+		/* Set credit threshold for QM bypass flow */
 		if (QM_BYPASS_EN)
 			STORE_RT_REG(p_hwfn,
 				     QM_REG_AFULLQMBYPTHRGLBLRL_RT_OFFSET,
@@ -200,7 +190,8 @@ static void qed_enable_vport_wfq(struct qed_hwfn *p_hwfn, bool vport_wfq_en)
 {
 	STORE_RT_REG(p_hwfn, QM_REG_WFQVPENABLE_RT_OFFSET,
 		     vport_wfq_en ? 1 : 0);
-	/* set credit threshold for QM bypass flow */
+
+	/* Set credit threshold for QM bypass flow */
 	if (vport_wfq_en && QM_BYPASS_EN)
 		STORE_RT_REG(p_hwfn,
 			     QM_REG_AFULLQMBYPTHRVPWFQ_RT_OFFSET,
@@ -208,7 +199,7 @@ static void qed_enable_vport_wfq(struct qed_hwfn *p_hwfn, bool vport_wfq_en)
 }
 
 /* Prepare runtime init values to allocate PBF command queue lines for
- * the specified VOQ
+ * the specified VOQ.
  */
 static void qed_cmdq_lines_voq_rt_init(struct qed_hwfn *p_hwfn,
 				       u8 voq, u16 cmdq_lines)
@@ -232,7 +223,7 @@ static void qed_cmdq_lines_rt_init(
 {
 	u8 tc, voq, port_id, num_tcs_in_port;
 
-	/* clear PBF lines for all VOQs */
+	/* Clear PBF lines for all VOQs */
 	for (voq = 0; voq < MAX_NUM_VOQS; voq++)
 		STORE_RT_REG(p_hwfn, PBF_CMDQ_LINES_RT_OFFSET(voq), 0);
 	for (port_id = 0; port_id < max_ports_per_engine; port_id++) {
@@ -285,7 +276,7 @@ static void qed_btb_blocks_rt_init(
 		if (!port_params[port_id].active)
 			continue;
 
-		/* subtract headroom blocks */
+		/* Subtract headroom blocks */
 		usable_blocks = port_params[port_id].num_btb_blocks -
 				BTB_HEADROOM_BLOCKS;
 
@@ -305,7 +296,7 @@ static void qed_btb_blocks_rt_init(
 		phys_blocks = (usable_blocks - pure_lb_blocks) /
 			      num_tcs_in_port;
 
-		/* init physical TCs */
+		/* Init physical TCs */
 		for (tc = 0; tc < NUM_OF_PHYS_TCS; tc++) {
 			if (((port_params[port_id].active_phys_tcs >>
 			      tc) & 0x1) != 1)
@@ -317,7 +308,7 @@ static void qed_btb_blocks_rt_init(
 				     phys_blocks);
 		}
 
-		/* init pure LB TC */
+		/* Init pure LB TC */
 		temp = LB_VOQ(port_id);
 		STORE_RT_REG(p_hwfn, PBF_BTB_GUARANTEED_RT_OFFSET(temp),
 			     pure_lb_blocks);
@@ -338,24 +329,24 @@ static void qed_tx_pq_map_rt_init(
 			    QM_PF_QUEUE_GROUP_SIZE;
 	u16 i, pq_id, pq_group;
 
-	/* a bit per Tx PQ indicating if the PQ is associated with a VF */
+	/* A bit per Tx PQ indicating if the PQ is associated with a VF */
 	u32 tx_pq_vf_mask[MAX_QM_TX_QUEUES / QM_PF_QUEUE_GROUP_SIZE] = { 0 };
 	u32 num_tx_pq_vf_masks = MAX_QM_TX_QUEUES / QM_PF_QUEUE_GROUP_SIZE;
 	u32 pq_mem_4kb = QM_PQ_MEM_4KB(p_params->num_pf_cids);
 	u32 vport_pq_mem_4kb = QM_PQ_MEM_4KB(p_params->num_vf_cids);
 	u32 mem_addr_4kb = base_mem_addr_4kb;
 
-	/* set mapping from PQ group to PF */
+	/* Set mapping from PQ group to PF */
 	for (pq_group = first_pq_group; pq_group <= last_pq_group; pq_group++)
 		STORE_RT_REG(p_hwfn, QM_REG_PQTX2PF_0_RT_OFFSET + pq_group,
 			     (u32)(p_params->pf_id));
-	/* set PQ sizes */
+	/* Set PQ sizes */
 	STORE_RT_REG(p_hwfn, QM_REG_MAXPQSIZE_0_RT_OFFSET,
 		     QM_PQ_SIZE_256B(p_params->num_pf_cids));
 	STORE_RT_REG(p_hwfn, QM_REG_MAXPQSIZE_1_RT_OFFSET,
 		     QM_PQ_SIZE_256B(p_params->num_vf_cids));
 
-	/* go over all Tx PQs */
+	/* Go over all Tx PQs */
 	for (i = 0, pq_id = p_params->start_pq; i < num_pqs; i++, pq_id++) {
 		u8 voq = VOQ(p_params->port_id, p_params->pq_params[i].tc_id,
 			     p_params->max_phys_tcs_per_port);
@@ -366,17 +357,18 @@ static void qed_tx_pq_map_rt_init(
 				(p_params->pq_params[i].vport_id <
 				 MAX_QM_GLOBAL_RLS);
 
-		/* update first Tx PQ of VPORT/TC */
+		/* Update first Tx PQ of VPORT/TC */
 		u8 vport_id_in_pf = p_params->pq_params[i].vport_id -
 				    p_params->start_vport;
 		u16 *pq_ids = &vport_params[vport_id_in_pf].first_tx_pq_id[0];
 		u16 first_tx_pq_id = pq_ids[p_params->pq_params[i].tc_id];
 
 		if (first_tx_pq_id == QM_INVALID_PQ_ID) {
-			/* create new VP PQ */
+			/* Create new VP PQ */
 			pq_ids[p_params->pq_params[i].tc_id] = pq_id;
 			first_tx_pq_id = pq_id;
-			/* map VP PQ to VOQ and PF */
+
+			/* Map VP PQ to VOQ and PF */
 			STORE_RT_REG(p_hwfn,
 				     QM_REG_WFQVPMAP_RT_OFFSET +
 				     first_tx_pq_id,
@@ -388,7 +380,7 @@ static void qed_tx_pq_map_rt_init(
 		if (p_params->pq_params[i].rl_valid && !rl_valid)
 			DP_NOTICE(p_hwfn,
 				  "Invalid VPORT ID for rate limiter configuration");
-		/* fill PQ map entry */
+		/* Fill PQ map entry */
 		memset(&tx_pq_map, 0, sizeof(tx_pq_map));
 		SET_FIELD(tx_pq_map.reg, QM_RF_PQ_MAP_PQ_VALID, 1);
 		SET_FIELD(tx_pq_map.reg,
@@ -400,18 +392,16 @@ static void qed_tx_pq_map_rt_init(
 		SET_FIELD(tx_pq_map.reg, QM_RF_PQ_MAP_VOQ, voq);
 		SET_FIELD(tx_pq_map.reg, QM_RF_PQ_MAP_WRR_WEIGHT_GROUP,
 			  p_params->pq_params[i].wrr_group);
-		/* write PQ map entry to CAM */
+		/* Write PQ map entry to CAM */
 		STORE_RT_REG(p_hwfn, QM_REG_TXPQMAP_RT_OFFSET + pq_id,
 			     *((u32 *)&tx_pq_map));
-		/* set base address */
+		/* Set base address */
 		STORE_RT_REG(p_hwfn,
 			     QM_REG_BASEADDRTXPQ_RT_OFFSET + pq_id,
 			     mem_addr_4kb);
-		/* check if VF PQ */
+
+		/* If VF PQ, add indication to PQ VF mask */
 		if (is_vf_pq) {
-			/* if PQ is associated with a VF, add indication
-			 * to PQ VF mask
-			 */
 			tx_pq_vf_mask[pq_id /
 				      QM_PF_QUEUE_GROUP_SIZE] |=
 			    BIT((pq_id % QM_PF_QUEUE_GROUP_SIZE));
@@ -421,16 +411,12 @@ static void qed_tx_pq_map_rt_init(
 		}
 	}
 
-	/* store Tx PQ VF mask to size select register */
-	for (i = 0; i < num_tx_pq_vf_masks; i++) {
-		if (tx_pq_vf_mask[i]) {
-			u32 addr;
-
-			addr = QM_REG_MAXPQSIZETXSEL_0_RT_OFFSET + i;
-			STORE_RT_REG(p_hwfn, addr,
+	/* Store Tx PQ VF mask to size select register */
+	for (i = 0; i < num_tx_pq_vf_masks; i++)
+		if (tx_pq_vf_mask[i])
+			STORE_RT_REG(p_hwfn,
+				     QM_REG_MAXPQSIZETXSEL_0_RT_OFFSET + i,
 				     tx_pq_vf_mask[i]);
-		}
-	}
 }
 
 /* Prepare Other PQ mapping runtime init values for the specified PF */
@@ -440,23 +426,25 @@ static void qed_other_pq_map_rt_init(struct qed_hwfn *p_hwfn,
 				     u32 num_pf_cids,
 				     u32 num_tids, u32 base_mem_addr_4kb)
 {
-	u16 i, pq_id;
+	u32 pq_size, pq_mem_4kb, mem_addr_4kb;
+	u16 i, pq_id, pq_group;
 
 	/* a single other PQ group is used in each PF,
 	 * where PQ group i is used in PF i.
 	 */
-	u16 pq_group = pf_id;
-	u32 pq_size = num_pf_cids + num_tids;
-	u32 pq_mem_4kb = QM_PQ_MEM_4KB(pq_size);
-	u32 mem_addr_4kb = base_mem_addr_4kb;
+	pq_group = pf_id;
+	pq_size = num_pf_cids + num_tids;
+	pq_mem_4kb = QM_PQ_MEM_4KB(pq_size);
+	mem_addr_4kb = base_mem_addr_4kb;
 
-	/* map PQ group to PF */
+	/* Map PQ group to PF */
 	STORE_RT_REG(p_hwfn, QM_REG_PQOTHER2PF_0_RT_OFFSET + pq_group,
 		     (u32)(pf_id));
-	/* set PQ sizes */
+	/* Set PQ sizes */
 	STORE_RT_REG(p_hwfn, QM_REG_MAXPQSIZE_2_RT_OFFSET,
 		     QM_PQ_SIZE_256B(pq_size));
-	/* set base address */
+
+	/* Set base address */
 	for (i = 0, pq_id = pf_id * QM_PF_QUEUE_GROUP_SIZE;
 	     i < QM_OTHER_PQS_PER_PF; i++, pq_id++) {
 		STORE_RT_REG(p_hwfn,
@@ -485,7 +473,7 @@ static int qed_pf_wfq_rt_init(struct qed_hwfn *p_hwfn,
 
 	inc_val = QM_WFQ_INC_VAL(p_params->pf_wfq);
 	if (!inc_val || inc_val > QM_WFQ_MAX_INC_VAL) {
-		DP_NOTICE(p_hwfn, "Invalid PF WFQ weight configuration");
+		DP_NOTICE(p_hwfn, "Invalid PF WFQ weight configuration\n");
 		return -1;
 	}
 
@@ -514,7 +502,7 @@ static int qed_pf_rl_rt_init(struct qed_hwfn *p_hwfn, u8 pf_id, u32 pf_rl)
 	u32 inc_val = QM_RL_INC_VAL(pf_rl);
 
 	if (inc_val > QM_RL_MAX_INC_VAL) {
-		DP_NOTICE(p_hwfn, "Invalid PF rate limit configuration");
+		DP_NOTICE(p_hwfn, "Invalid PF rate limit configuration\n");
 		return -1;
 	}
 	STORE_RT_REG(p_hwfn, QM_REG_RLPFCRD_RT_OFFSET + pf_id,
@@ -535,7 +523,7 @@ static int qed_vp_wfq_rt_init(struct qed_hwfn *p_hwfn,
 	u32 inc_val;
 	u8 tc, i;
 
-	/* go over all PF VPORTs */
+	/* Go over all PF VPORTs */
 	for (i = 0; i < num_vports; i++) {
 
 		if (!vport_params[i].vport_wfq)
@@ -544,7 +532,7 @@ static int qed_vp_wfq_rt_init(struct qed_hwfn *p_hwfn,
 		inc_val = QM_WFQ_INC_VAL(vport_params[i].vport_wfq);
 		if (inc_val > QM_WFQ_MAX_INC_VAL) {
 			DP_NOTICE(p_hwfn,
-				  "Invalid VPORT WFQ weight configuration");
+				  "Invalid VPORT WFQ weight configuration\n");
 			return -1;
 		}
 
@@ -578,17 +566,17 @@ static int qed_vport_rl_rt_init(struct qed_hwfn *p_hwfn,
 
 	if (start_vport + num_vports >= MAX_QM_GLOBAL_RLS) {
 		DP_NOTICE(p_hwfn,
-			  "Invalid VPORT ID for rate limiter configuration");
+			  "Invalid VPORT ID for rate limiter configuration\n");
 		return -1;
 	}
 
-	/* go over all PF VPORTs */
+	/* Go over all PF VPORTs */
 	for (i = 0, vport_id = start_vport; i < num_vports; i++, vport_id++) {
 		u32 inc_val = QM_RL_INC_VAL(vport_params[i].vport_rl);
 
 		if (inc_val > QM_RL_MAX_INC_VAL) {
 			DP_NOTICE(p_hwfn,
-				  "Invalid VPORT rate-limit configuration");
+				  "Invalid VPORT rate-limit configuration\n");
 			return -1;
 		}
 
@@ -617,7 +605,7 @@ static bool qed_poll_on_qm_cmd_ready(struct qed_hwfn *p_hwfn,
 		reg_val = qed_rd(p_hwfn, p_ptt, QM_REG_SDMCMDREADY);
 	}
 
-	/* check if timeout while waiting for SDM command ready */
+	/* Check if timeout while waiting for SDM command ready */
 	if (i == QM_STOP_CMD_MAX_POLL_COUNT) {
 		DP_VERBOSE(p_hwfn, NETIF_MSG_HW,
 			   "Timeout when waiting for QM SDM command ready signal\n");
@@ -701,16 +689,16 @@ int qed_qm_pf_rt_init(struct qed_hwfn *p_hwfn,
 				 QM_OTHER_PQS_PER_PF;
 	u8 tc, i;
 
-	/* clear first Tx PQ ID array for each VPORT */
+	/* Clear first Tx PQ ID array for each VPORT */
 	for (i = 0; i < p_params->num_vports; i++)
 		for (tc = 0; tc < NUM_OF_TCS; tc++)
 			vport_params[i].first_tx_pq_id[tc] = QM_INVALID_PQ_ID;
 
-	/* map Other PQs (if any) */
+	/* Map Other PQs (if any) */
 	qed_other_pq_map_rt_init(p_hwfn, p_params->port_id, p_params->pf_id,
 				 p_params->num_pf_cids, p_params->num_tids, 0);
 
-	/* map Tx PQs */
+	/* Map Tx PQs */
 	qed_tx_pq_map_rt_init(p_hwfn, p_ptt, p_params, other_mem_size_4kb);
 
 	if (p_params->pf_wfq)
@@ -736,7 +724,7 @@ int qed_init_pf_wfq(struct qed_hwfn *p_hwfn,
 	u32 inc_val = QM_WFQ_INC_VAL(pf_wfq);
 
 	if (!inc_val || inc_val > QM_WFQ_MAX_INC_VAL) {
-		DP_NOTICE(p_hwfn, "Invalid PF WFQ weight configuration");
+		DP_NOTICE(p_hwfn, "Invalid PF WFQ weight configuration\n");
 		return -1;
 	}
 
@@ -750,7 +738,7 @@ int qed_init_pf_rl(struct qed_hwfn *p_hwfn,
 	u32 inc_val = QM_RL_INC_VAL(pf_rl);
 
 	if (inc_val > QM_RL_MAX_INC_VAL) {
-		DP_NOTICE(p_hwfn, "Invalid PF rate limit configuration");
+		DP_NOTICE(p_hwfn, "Invalid PF rate limit configuration\n");
 		return -1;
 	}
 
@@ -766,17 +754,18 @@ int qed_init_vport_wfq(struct qed_hwfn *p_hwfn,
 		       struct qed_ptt *p_ptt,
 		       u16 first_tx_pq_id[NUM_OF_TCS], u16 vport_wfq)
 {
-	u32 inc_val = QM_WFQ_INC_VAL(vport_wfq);
+	u16 vport_pq_id;
+	u32 inc_val;
 	u8 tc;
 
+	inc_val = QM_WFQ_INC_VAL(vport_wfq);
 	if (!inc_val || inc_val > QM_WFQ_MAX_INC_VAL) {
-		DP_NOTICE(p_hwfn, "Invalid VPORT WFQ weight configuration");
+		DP_NOTICE(p_hwfn, "Invalid VPORT WFQ weight configuration\n");
 		return -1;
 	}
 
 	for (tc = 0; tc < NUM_OF_TCS; tc++) {
-		u16 vport_pq_id = first_tx_pq_id[tc];
-
+		vport_pq_id = first_tx_pq_id[tc];
 		if (vport_pq_id != QM_INVALID_PQ_ID)
 			qed_wr(p_hwfn, p_ptt,
 			       QM_REG_WFQVPWEIGHT + vport_pq_id * 4,
@@ -793,12 +782,12 @@ int qed_init_vport_rl(struct qed_hwfn *p_hwfn,
 
 	if (vport_id >= MAX_QM_GLOBAL_RLS) {
 		DP_NOTICE(p_hwfn,
-			  "Invalid VPORT ID for rate limiter configuration");
+			  "Invalid VPORT ID for rate limiter configuration\n");
 		return -1;
 	}
 
 	if (inc_val > QM_RL_MAX_INC_VAL) {
-		DP_NOTICE(p_hwfn, "Invalid VPORT rate-limit configuration");
+		DP_NOTICE(p_hwfn, "Invalid VPORT rate-limit configuration\n");
 		return -1;
 	}
 
@@ -818,15 +807,15 @@ bool qed_send_qm_stop_cmd(struct qed_hwfn *p_hwfn,
 	u32 cmd_arr[QM_CMD_STRUCT_SIZE(QM_STOP_CMD)] = { 0 };
 	u32 pq_mask = 0, last_pq = start_pq + num_pqs - 1, pq_id;
 
-	/* set command's PQ type */
+	/* Set command's PQ type */
 	QM_CMD_SET_FIELD(cmd_arr, QM_STOP_CMD, PQ_TYPE, is_tx_pq ? 0 : 1);
 
 	for (pq_id = start_pq; pq_id <= last_pq; pq_id++) {
-		/* set PQ bit in mask (stop command only) */
+		/* Set PQ bit in mask (stop command only) */
 		if (!is_release_cmd)
 			pq_mask |= (1 << (pq_id % QM_STOP_PQ_MASK_WIDTH));
 
-		/* if last PQ or end of PQ mask, write command */
+		/* If last PQ or end of PQ mask, write command */
 		if ((pq_id == last_pq) ||
 		    (pq_id % QM_STOP_PQ_MASK_WIDTH ==
 		     (QM_STOP_PQ_MASK_WIDTH - 1))) {
@@ -962,8 +951,10 @@ void qed_set_geneve_enable(struct qed_hwfn *p_hwfn,
 	       ip_geneve_enable ? 1 : 0);
 }
 
+#define T_ETH_PACKET_ACTION_GFT_EVENTID  23
+#define PARSER_ETH_CONN_GFT_ACTION_CM_HDR  272
 #define T_ETH_PACKET_MATCH_RFS_EVENTID 25
-#define PARSER_ETH_CONN_CM_HDR (0x0)
+#define PARSER_ETH_CONN_CM_HDR 0
 #define CAM_LINE_SIZE sizeof(u32)
 #define RAM_LINE_SIZE sizeof(u64)
 #define REG_SIZE sizeof(u32)
@@ -971,40 +962,26 @@ void qed_set_geneve_enable(struct qed_hwfn *p_hwfn,
 void qed_set_rfs_mode_disable(struct qed_hwfn *p_hwfn,
 			      struct qed_ptt *p_ptt, u16 pf_id)
 {
-	union gft_cam_line_union camline;
-	struct gft_ram_line ramline;
-	u32 *p_ramline, i;
-
-	p_ramline = (u32 *)&ramline;
+	u32 hw_addr = PRS_REG_GFT_PROFILE_MASK_RAM +
+		      pf_id * RAM_LINE_SIZE;
 
 	/*stop using gft logic */
 	qed_wr(p_hwfn, p_ptt, PRS_REG_SEARCH_GFT, 0);
 	qed_wr(p_hwfn, p_ptt, PRS_REG_CM_HDR_GFT, 0x0);
-	memset(&camline, 0, sizeof(union gft_cam_line_union));
-	qed_wr(p_hwfn, p_ptt, PRS_REG_GFT_CAM + CAM_LINE_SIZE * pf_id,
-	       camline.cam_line_mapped.camline);
-	memset(&ramline, 0, sizeof(ramline));
-
-	for (i = 0; i < RAM_LINE_SIZE / REG_SIZE; i++) {
-		u32 hw_addr = PRS_REG_GFT_PROFILE_MASK_RAM;
-
-		hw_addr += (RAM_LINE_SIZE * pf_id + i * REG_SIZE);
-
-		qed_wr(p_hwfn, p_ptt, hw_addr, *(p_ramline + i));
-	}
+	qed_wr(p_hwfn, p_ptt, PRS_REG_GFT_CAM + CAM_LINE_SIZE * pf_id, 0);
+	qed_wr(p_hwfn, p_ptt, hw_addr, 0);
+	qed_wr(p_hwfn, p_ptt, hw_addr + 4, 0);
 }
 
 void qed_set_rfs_mode_enable(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 			     u16 pf_id, bool tcp, bool udp,
 			     bool ipv4, bool ipv6)
 {
-	u32 rfs_cm_hdr_event_id, *p_ramline;
 	union gft_cam_line_union camline;
 	struct gft_ram_line ramline;
-	int i;
+	u32 rfs_cm_hdr_event_id;
 
 	rfs_cm_hdr_event_id = qed_rd(p_hwfn, p_ptt, PRS_REG_CM_HDR_GFT);
-	p_ramline = (u32 *)&ramline;
 
 	if (!ipv6 && !ipv4)
 		DP_NOTICE(p_hwfn,
@@ -1024,18 +1001,20 @@ void qed_set_rfs_mode_enable(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 	qed_wr(p_hwfn, p_ptt, PRS_REG_LOAD_L2_FILTER, 0);
 	camline.cam_line_mapped.camline = 0;
 
-	/* cam line is now valid!! */
+	/* Cam line is now valid!! */
 	SET_FIELD(camline.cam_line_mapped.camline,
 		  GFT_CAM_LINE_MAPPED_VALID, 1);
 
 	/* filters are per PF!! */
 	SET_FIELD(camline.cam_line_mapped.camline,
-		  GFT_CAM_LINE_MAPPED_PF_ID_MASK, 1);
+		  GFT_CAM_LINE_MAPPED_PF_ID_MASK,
+		  GFT_CAM_LINE_MAPPED_PF_ID_MASK_MASK);
 	SET_FIELD(camline.cam_line_mapped.camline,
 		  GFT_CAM_LINE_MAPPED_PF_ID, pf_id);
 	if (!(tcp && udp)) {
 		SET_FIELD(camline.cam_line_mapped.camline,
-			  GFT_CAM_LINE_MAPPED_UPPER_PROTOCOL_TYPE_MASK, 1);
+			  GFT_CAM_LINE_MAPPED_UPPER_PROTOCOL_TYPE_MASK,
+			  GFT_CAM_LINE_MAPPED_UPPER_PROTOCOL_TYPE_MASK_MASK);
 		if (tcp)
 			SET_FIELD(camline.cam_line_mapped.camline,
 				  GFT_CAM_LINE_MAPPED_UPPER_PROTOCOL_TYPE,
@@ -1059,34 +1038,38 @@ void qed_set_rfs_mode_enable(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 				  GFT_PROFILE_IPV6);
 	}
 
-	/* write characteristics to cam */
+	/* Write characteristics to cam */
 	qed_wr(p_hwfn, p_ptt, PRS_REG_GFT_CAM + CAM_LINE_SIZE * pf_id,
 	       camline.cam_line_mapped.camline);
 	camline.cam_line_mapped.camline = qed_rd(p_hwfn, p_ptt,
 						 PRS_REG_GFT_CAM +
 						 CAM_LINE_SIZE * pf_id);
 
-	/* write line to RAM - compare to filter 4 tuple */
-	ramline.low32bits = 0;
-	ramline.high32bits = 0;
-	SET_FIELD(ramline.high32bits, GFT_RAM_LINE_DST_IP, 1);
-	SET_FIELD(ramline.high32bits, GFT_RAM_LINE_SRC_IP, 1);
-	SET_FIELD(ramline.low32bits, GFT_RAM_LINE_SRC_PORT, 1);
-	SET_FIELD(ramline.low32bits, GFT_RAM_LINE_DST_PORT, 1);
-
-	/* each iteration write to reg */
-	for (i = 0; i < RAM_LINE_SIZE / REG_SIZE; i++)
-		qed_wr(p_hwfn, p_ptt,
-		       PRS_REG_GFT_PROFILE_MASK_RAM + RAM_LINE_SIZE * pf_id +
-		       i * REG_SIZE, *(p_ramline + i));
-
-	/* set default profile so that no filter match will happen */
-	ramline.low32bits = 0xffff;
-	ramline.high32bits = 0xffff;
-
-	for (i = 0; i < RAM_LINE_SIZE / REG_SIZE; i++)
-		qed_wr(p_hwfn, p_ptt,
-		       PRS_REG_GFT_PROFILE_MASK_RAM + RAM_LINE_SIZE *
-		       PRS_GFT_CAM_LINES_NO_MATCH + i * REG_SIZE,
-		       *(p_ramline + i));
+	/* Write line to RAM - compare to filter 4 tuple */
+	ramline.lo = 0;
+	ramline.hi = 0;
+	SET_FIELD(ramline.hi, GFT_RAM_LINE_DST_IP, 1);
+	SET_FIELD(ramline.hi, GFT_RAM_LINE_SRC_IP, 1);
+	SET_FIELD(ramline.hi, GFT_RAM_LINE_OVER_IP_PROTOCOL, 1);
+	SET_FIELD(ramline.lo, GFT_RAM_LINE_ETHERTYPE, 1);
+	SET_FIELD(ramline.lo, GFT_RAM_LINE_SRC_PORT, 1);
+	SET_FIELD(ramline.lo, GFT_RAM_LINE_DST_PORT, 1);
+
+	/* Each iteration write to reg */
+	qed_wr(p_hwfn, p_ptt,
+	       PRS_REG_GFT_PROFILE_MASK_RAM + RAM_LINE_SIZE * pf_id,
+	       ramline.lo);
+	qed_wr(p_hwfn, p_ptt,
+	       PRS_REG_GFT_PROFILE_MASK_RAM + RAM_LINE_SIZE * pf_id + 4,
+	       ramline.hi);
+
+	/* Set default profile so that no filter match will happen */
+	qed_wr(p_hwfn, p_ptt,
+	       PRS_REG_GFT_PROFILE_MASK_RAM +
+	       RAM_LINE_SIZE * PRS_GFT_CAM_LINES_NO_MATCH,
+	       ramline.lo);
+	qed_wr(p_hwfn, p_ptt,
+	       PRS_REG_GFT_PROFILE_MASK_RAM +
+	       RAM_LINE_SIZE * PRS_GFT_CAM_LINES_NO_MATCH + 4,
+	       ramline.hi);
 }
diff --git a/drivers/net/ethernet/qlogic/qed/qed_init_ops.c b/drivers/net/ethernet/qlogic/qed/qed_init_ops.c
index 4a2e7be5bf72..e3f368882f46 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_init_ops.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_init_ops.c
@@ -158,6 +158,7 @@ int qed_init_alloc(struct qed_hwfn *p_hwfn)
 				    GFP_KERNEL);
 	if (!rt_data->init_val) {
 		kfree(rt_data->b_valid);
+		rt_data->b_valid = NULL;
 		return -ENOMEM;
 	}
 
@@ -167,7 +168,9 @@ int qed_init_alloc(struct qed_hwfn *p_hwfn)
 void qed_init_free(struct qed_hwfn *p_hwfn)
 {
 	kfree(p_hwfn->rt_data.init_val);
+	p_hwfn->rt_data.init_val = NULL;
 	kfree(p_hwfn->rt_data.b_valid);
+	p_hwfn->rt_data.b_valid = NULL;
 }
 
 static int qed_init_array_dmae(struct qed_hwfn *p_hwfn,
@@ -525,6 +528,7 @@ int qed_init_run(struct qed_hwfn *p_hwfn,
 	}
 
 	kfree(p_hwfn->unzip_buf);
+	p_hwfn->unzip_buf = NULL;
 	return rc;
 }
 
diff --git a/drivers/net/ethernet/qlogic/qed/qed_int.c b/drivers/net/ethernet/qlogic/qed/qed_int.c
index 40f057edeafc..719cdbfe1695 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_int.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_int.c
@@ -90,6 +90,12 @@ struct aeu_invert_reg_bit {
 /* Multiple bits start with this offset */
 #define ATTENTION_OFFSET_MASK           (0x000ff000)
 #define ATTENTION_OFFSET_SHIFT          (12)
+
+#define ATTENTION_BB_MASK               (0x00700000)
+#define ATTENTION_BB_SHIFT              (20)
+#define ATTENTION_BB(value)             (value << ATTENTION_BB_SHIFT)
+#define ATTENTION_BB_DIFFERENT          BIT(23)
+
 	unsigned int flags;
 
 	/* Callback to call if attention will be triggered */
@@ -105,1215 +111,6 @@ struct aeu_invert_reg {
 #define MAX_ATTN_GRPS           (8)
 #define NUM_ATTN_REGS           (9)
 
-/* HW Attention register */
-struct attn_hw_reg {
-	u16 reg_idx;             /* Index of this register in its block */
-	u16 num_of_bits;         /* number of valid attention bits */
-	u32 sts_addr;            /* Address of the STS register */
-	u32 sts_clr_addr;        /* Address of the STS_CLR register */
-	u32 sts_wr_addr;         /* Address of the STS_WR register */
-	u32 mask_addr;           /* Address of the MASK register */
-};
-
-/* HW block attention registers */
-struct attn_hw_regs {
-	u16 num_of_int_regs;            /* Number of interrupt regs */
-	u16 num_of_prty_regs;           /* Number of parity regs */
-	struct attn_hw_reg **int_regs;  /* interrupt regs */
-	struct attn_hw_reg **prty_regs; /* parity regs */
-};
-
-/* HW block attention registers */
-struct attn_hw_block {
-	const char *name;                 /* Block name */
-	struct attn_hw_regs chip_regs[1];
-};
-
-static struct attn_hw_reg grc_int0_bb_b0 = {
-	0, 4, 0x50180, 0x5018c, 0x50188, 0x50184};
-
-static struct attn_hw_reg *grc_int_bb_b0_regs[1] = {
-	&grc_int0_bb_b0};
-
-static struct attn_hw_reg grc_prty1_bb_b0 = {
-	0, 2, 0x50200, 0x5020c, 0x50208, 0x50204};
-
-static struct attn_hw_reg *grc_prty_bb_b0_regs[1] = {
-	&grc_prty1_bb_b0};
-
-static struct attn_hw_reg miscs_int0_bb_b0 = {
-	0, 3, 0x9180, 0x918c, 0x9188, 0x9184};
-
-static struct attn_hw_reg miscs_int1_bb_b0 = {
-	1, 11, 0x9190, 0x919c, 0x9198, 0x9194};
-
-static struct attn_hw_reg *miscs_int_bb_b0_regs[2] = {
-	&miscs_int0_bb_b0, &miscs_int1_bb_b0};
-
-static struct attn_hw_reg miscs_prty0_bb_b0 = {
-	0, 1, 0x91a0, 0x91ac, 0x91a8, 0x91a4};
-
-static struct attn_hw_reg *miscs_prty_bb_b0_regs[1] = {
-	&miscs_prty0_bb_b0};
-
-static struct attn_hw_reg misc_int0_bb_b0 = {
-	0, 1, 0x8180, 0x818c, 0x8188, 0x8184};
-
-static struct attn_hw_reg *misc_int_bb_b0_regs[1] = {
-	&misc_int0_bb_b0};
-
-static struct attn_hw_reg pglue_b_int0_bb_b0 = {
-	0, 23, 0x2a8180, 0x2a818c, 0x2a8188, 0x2a8184};
-
-static struct attn_hw_reg *pglue_b_int_bb_b0_regs[1] = {
-	&pglue_b_int0_bb_b0};
-
-static struct attn_hw_reg pglue_b_prty0_bb_b0 = {
-	0, 1, 0x2a8190, 0x2a819c, 0x2a8198, 0x2a8194};
-
-static struct attn_hw_reg pglue_b_prty1_bb_b0 = {
-	1, 22, 0x2a8200, 0x2a820c, 0x2a8208, 0x2a8204};
-
-static struct attn_hw_reg *pglue_b_prty_bb_b0_regs[2] = {
-	&pglue_b_prty0_bb_b0, &pglue_b_prty1_bb_b0};
-
-static struct attn_hw_reg cnig_int0_bb_b0 = {
-	0, 6, 0x2182e8, 0x2182f4, 0x2182f0, 0x2182ec};
-
-static struct attn_hw_reg *cnig_int_bb_b0_regs[1] = {
-	&cnig_int0_bb_b0};
-
-static struct attn_hw_reg cnig_prty0_bb_b0 = {
-	0, 2, 0x218348, 0x218354, 0x218350, 0x21834c};
-
-static struct attn_hw_reg *cnig_prty_bb_b0_regs[1] = {
-	&cnig_prty0_bb_b0};
-
-static struct attn_hw_reg cpmu_int0_bb_b0 = {
-	0, 1, 0x303e0, 0x303ec, 0x303e8, 0x303e4};
-
-static struct attn_hw_reg *cpmu_int_bb_b0_regs[1] = {
-	&cpmu_int0_bb_b0};
-
-static struct attn_hw_reg ncsi_int0_bb_b0 = {
-	0, 1, 0x404cc, 0x404d8, 0x404d4, 0x404d0};
-
-static struct attn_hw_reg *ncsi_int_bb_b0_regs[1] = {
-	&ncsi_int0_bb_b0};
-
-static struct attn_hw_reg ncsi_prty1_bb_b0 = {
-	0, 1, 0x40000, 0x4000c, 0x40008, 0x40004};
-
-static struct attn_hw_reg *ncsi_prty_bb_b0_regs[1] = {
-	&ncsi_prty1_bb_b0};
-
-static struct attn_hw_reg opte_prty1_bb_b0 = {
-	0, 11, 0x53000, 0x5300c, 0x53008, 0x53004};
-
-static struct attn_hw_reg opte_prty0_bb_b0 = {
-	1, 1, 0x53208, 0x53214, 0x53210, 0x5320c};
-
-static struct attn_hw_reg *opte_prty_bb_b0_regs[2] = {
-	&opte_prty1_bb_b0, &opte_prty0_bb_b0};
-
-static struct attn_hw_reg bmb_int0_bb_b0 = {
-	0, 16, 0x5400c0, 0x5400cc, 0x5400c8, 0x5400c4};
-
-static struct attn_hw_reg bmb_int1_bb_b0 = {
-	1, 28, 0x5400d8, 0x5400e4, 0x5400e0, 0x5400dc};
-
-static struct attn_hw_reg bmb_int2_bb_b0 = {
-	2, 26, 0x5400f0, 0x5400fc, 0x5400f8, 0x5400f4};
-
-static struct attn_hw_reg bmb_int3_bb_b0 = {
-	3, 31, 0x540108, 0x540114, 0x540110, 0x54010c};
-
-static struct attn_hw_reg bmb_int4_bb_b0 = {
-	4, 27, 0x540120, 0x54012c, 0x540128, 0x540124};
-
-static struct attn_hw_reg bmb_int5_bb_b0 = {
-	5, 29, 0x540138, 0x540144, 0x540140, 0x54013c};
-
-static struct attn_hw_reg bmb_int6_bb_b0 = {
-	6, 30, 0x540150, 0x54015c, 0x540158, 0x540154};
-
-static struct attn_hw_reg bmb_int7_bb_b0 = {
-	7, 32, 0x540168, 0x540174, 0x540170, 0x54016c};
-
-static struct attn_hw_reg bmb_int8_bb_b0 = {
-	8, 32, 0x540184, 0x540190, 0x54018c, 0x540188};
-
-static struct attn_hw_reg bmb_int9_bb_b0 = {
-	9, 32, 0x54019c, 0x5401a8, 0x5401a4, 0x5401a0};
-
-static struct attn_hw_reg bmb_int10_bb_b0 = {
-	10, 3, 0x5401b4, 0x5401c0, 0x5401bc, 0x5401b8};
-
-static struct attn_hw_reg bmb_int11_bb_b0 = {
-	11, 4, 0x5401cc, 0x5401d8, 0x5401d4, 0x5401d0};
-
-static struct attn_hw_reg *bmb_int_bb_b0_regs[12] = {
-	&bmb_int0_bb_b0, &bmb_int1_bb_b0, &bmb_int2_bb_b0, &bmb_int3_bb_b0,
-	&bmb_int4_bb_b0, &bmb_int5_bb_b0, &bmb_int6_bb_b0, &bmb_int7_bb_b0,
-	&bmb_int8_bb_b0, &bmb_int9_bb_b0, &bmb_int10_bb_b0, &bmb_int11_bb_b0};
-
-static struct attn_hw_reg bmb_prty0_bb_b0 = {
-	0, 5, 0x5401dc, 0x5401e8, 0x5401e4, 0x5401e0};
-
-static struct attn_hw_reg bmb_prty1_bb_b0 = {
-	1, 31, 0x540400, 0x54040c, 0x540408, 0x540404};
-
-static struct attn_hw_reg bmb_prty2_bb_b0 = {
-	2, 15, 0x540410, 0x54041c, 0x540418, 0x540414};
-
-static struct attn_hw_reg *bmb_prty_bb_b0_regs[3] = {
-	&bmb_prty0_bb_b0, &bmb_prty1_bb_b0, &bmb_prty2_bb_b0};
-
-static struct attn_hw_reg pcie_prty1_bb_b0 = {
-	0, 17, 0x54000, 0x5400c, 0x54008, 0x54004};
-
-static struct attn_hw_reg *pcie_prty_bb_b0_regs[1] = {
-	&pcie_prty1_bb_b0};
-
-static struct attn_hw_reg mcp2_prty0_bb_b0 = {
-	0, 1, 0x52040, 0x5204c, 0x52048, 0x52044};
-
-static struct attn_hw_reg mcp2_prty1_bb_b0 = {
-	1, 12, 0x52204, 0x52210, 0x5220c, 0x52208};
-
-static struct attn_hw_reg *mcp2_prty_bb_b0_regs[2] = {
-	&mcp2_prty0_bb_b0, &mcp2_prty1_bb_b0};
-
-static struct attn_hw_reg pswhst_int0_bb_b0 = {
-	0, 18, 0x2a0180, 0x2a018c, 0x2a0188, 0x2a0184};
-
-static struct attn_hw_reg *pswhst_int_bb_b0_regs[1] = {
-	&pswhst_int0_bb_b0};
-
-static struct attn_hw_reg pswhst_prty0_bb_b0 = {
-	0, 1, 0x2a0190, 0x2a019c, 0x2a0198, 0x2a0194};
-
-static struct attn_hw_reg pswhst_prty1_bb_b0 = {
-	1, 17, 0x2a0200, 0x2a020c, 0x2a0208, 0x2a0204};
-
-static struct attn_hw_reg *pswhst_prty_bb_b0_regs[2] = {
-	&pswhst_prty0_bb_b0, &pswhst_prty1_bb_b0};
-
-static struct attn_hw_reg pswhst2_int0_bb_b0 = {
-	0, 5, 0x29e180, 0x29e18c, 0x29e188, 0x29e184};
-
-static struct attn_hw_reg *pswhst2_int_bb_b0_regs[1] = {
-	&pswhst2_int0_bb_b0};
-
-static struct attn_hw_reg pswhst2_prty0_bb_b0 = {
-	0, 1, 0x29e190, 0x29e19c, 0x29e198, 0x29e194};
-
-static struct attn_hw_reg *pswhst2_prty_bb_b0_regs[1] = {
-	&pswhst2_prty0_bb_b0};
-
-static struct attn_hw_reg pswrd_int0_bb_b0 = {
-	0, 3, 0x29c180, 0x29c18c, 0x29c188, 0x29c184};
-
-static struct attn_hw_reg *pswrd_int_bb_b0_regs[1] = {
-	&pswrd_int0_bb_b0};
-
-static struct attn_hw_reg pswrd_prty0_bb_b0 = {
-	0, 1, 0x29c190, 0x29c19c, 0x29c198, 0x29c194};
-
-static struct attn_hw_reg *pswrd_prty_bb_b0_regs[1] = {
-	&pswrd_prty0_bb_b0};
-
-static struct attn_hw_reg pswrd2_int0_bb_b0 = {
-	0, 5, 0x29d180, 0x29d18c, 0x29d188, 0x29d184};
-
-static struct attn_hw_reg *pswrd2_int_bb_b0_regs[1] = {
-	&pswrd2_int0_bb_b0};
-
-static struct attn_hw_reg pswrd2_prty0_bb_b0 = {
-	0, 1, 0x29d190, 0x29d19c, 0x29d198, 0x29d194};
-
-static struct attn_hw_reg pswrd2_prty1_bb_b0 = {
-	1, 31, 0x29d200, 0x29d20c, 0x29d208, 0x29d204};
-
-static struct attn_hw_reg pswrd2_prty2_bb_b0 = {
-	2, 3, 0x29d210, 0x29d21c, 0x29d218, 0x29d214};
-
-static struct attn_hw_reg *pswrd2_prty_bb_b0_regs[3] = {
-	&pswrd2_prty0_bb_b0, &pswrd2_prty1_bb_b0, &pswrd2_prty2_bb_b0};
-
-static struct attn_hw_reg pswwr_int0_bb_b0 = {
-	0, 16, 0x29a180, 0x29a18c, 0x29a188, 0x29a184};
-
-static struct attn_hw_reg *pswwr_int_bb_b0_regs[1] = {
-	&pswwr_int0_bb_b0};
-
-static struct attn_hw_reg pswwr_prty0_bb_b0 = {
-	0, 1, 0x29a190, 0x29a19c, 0x29a198, 0x29a194};
-
-static struct attn_hw_reg *pswwr_prty_bb_b0_regs[1] = {
-	&pswwr_prty0_bb_b0};
-
-static struct attn_hw_reg pswwr2_int0_bb_b0 = {
-	0, 19, 0x29b180, 0x29b18c, 0x29b188, 0x29b184};
-
-static struct attn_hw_reg *pswwr2_int_bb_b0_regs[1] = {
-	&pswwr2_int0_bb_b0};
-
-static struct attn_hw_reg pswwr2_prty0_bb_b0 = {
-	0, 1, 0x29b190, 0x29b19c, 0x29b198, 0x29b194};
-
-static struct attn_hw_reg pswwr2_prty1_bb_b0 = {
-	1, 31, 0x29b200, 0x29b20c, 0x29b208, 0x29b204};
-
-static struct attn_hw_reg pswwr2_prty2_bb_b0 = {
-	2, 31, 0x29b210, 0x29b21c, 0x29b218, 0x29b214};
-
-static struct attn_hw_reg pswwr2_prty3_bb_b0 = {
-	3, 31, 0x29b220, 0x29b22c, 0x29b228, 0x29b224};
-
-static struct attn_hw_reg pswwr2_prty4_bb_b0 = {
-	4, 20, 0x29b230, 0x29b23c, 0x29b238, 0x29b234};
-
-static struct attn_hw_reg *pswwr2_prty_bb_b0_regs[5] = {
-	&pswwr2_prty0_bb_b0, &pswwr2_prty1_bb_b0, &pswwr2_prty2_bb_b0,
-	&pswwr2_prty3_bb_b0, &pswwr2_prty4_bb_b0};
-
-static struct attn_hw_reg pswrq_int0_bb_b0 = {
-	0, 21, 0x280180, 0x28018c, 0x280188, 0x280184};
-
-static struct attn_hw_reg *pswrq_int_bb_b0_regs[1] = {
-	&pswrq_int0_bb_b0};
-
-static struct attn_hw_reg pswrq_prty0_bb_b0 = {
-	0, 1, 0x280190, 0x28019c, 0x280198, 0x280194};
-
-static struct attn_hw_reg *pswrq_prty_bb_b0_regs[1] = {
-	&pswrq_prty0_bb_b0};
-
-static struct attn_hw_reg pswrq2_int0_bb_b0 = {
-	0, 15, 0x240180, 0x24018c, 0x240188, 0x240184};
-
-static struct attn_hw_reg *pswrq2_int_bb_b0_regs[1] = {
-	&pswrq2_int0_bb_b0};
-
-static struct attn_hw_reg pswrq2_prty1_bb_b0 = {
-	0, 9, 0x240200, 0x24020c, 0x240208, 0x240204};
-
-static struct attn_hw_reg *pswrq2_prty_bb_b0_regs[1] = {
-	&pswrq2_prty1_bb_b0};
-
-static struct attn_hw_reg pglcs_int0_bb_b0 = {
-	0, 1, 0x1d00, 0x1d0c, 0x1d08, 0x1d04};
-
-static struct attn_hw_reg *pglcs_int_bb_b0_regs[1] = {
-	&pglcs_int0_bb_b0};
-
-static struct attn_hw_reg dmae_int0_bb_b0 = {
-	0, 2, 0xc180, 0xc18c, 0xc188, 0xc184};
-
-static struct attn_hw_reg *dmae_int_bb_b0_regs[1] = {
-	&dmae_int0_bb_b0};
-
-static struct attn_hw_reg dmae_prty1_bb_b0 = {
-	0, 3, 0xc200, 0xc20c, 0xc208, 0xc204};
-
-static struct attn_hw_reg *dmae_prty_bb_b0_regs[1] = {
-	&dmae_prty1_bb_b0};
-
-static struct attn_hw_reg ptu_int0_bb_b0 = {
-	0, 8, 0x560180, 0x56018c, 0x560188, 0x560184};
-
-static struct attn_hw_reg *ptu_int_bb_b0_regs[1] = {
-	&ptu_int0_bb_b0};
-
-static struct attn_hw_reg ptu_prty1_bb_b0 = {
-	0, 18, 0x560200, 0x56020c, 0x560208, 0x560204};
-
-static struct attn_hw_reg *ptu_prty_bb_b0_regs[1] = {
-	&ptu_prty1_bb_b0};
-
-static struct attn_hw_reg tcm_int0_bb_b0 = {
-	0, 8, 0x1180180, 0x118018c, 0x1180188, 0x1180184};
-
-static struct attn_hw_reg tcm_int1_bb_b0 = {
-	1, 32, 0x1180190, 0x118019c, 0x1180198, 0x1180194};
-
-static struct attn_hw_reg tcm_int2_bb_b0 = {
-	2, 1, 0x11801a0, 0x11801ac, 0x11801a8, 0x11801a4};
-
-static struct attn_hw_reg *tcm_int_bb_b0_regs[3] = {
-	&tcm_int0_bb_b0, &tcm_int1_bb_b0, &tcm_int2_bb_b0};
-
-static struct attn_hw_reg tcm_prty1_bb_b0 = {
-	0, 31, 0x1180200, 0x118020c, 0x1180208, 0x1180204};
-
-static struct attn_hw_reg tcm_prty2_bb_b0 = {
-	1, 2, 0x1180210, 0x118021c, 0x1180218, 0x1180214};
-
-static struct attn_hw_reg *tcm_prty_bb_b0_regs[2] = {
-	&tcm_prty1_bb_b0, &tcm_prty2_bb_b0};
-
-static struct attn_hw_reg mcm_int0_bb_b0 = {
-	0, 14, 0x1200180, 0x120018c, 0x1200188, 0x1200184};
-
-static struct attn_hw_reg mcm_int1_bb_b0 = {
-	1, 26, 0x1200190, 0x120019c, 0x1200198, 0x1200194};
-
-static struct attn_hw_reg mcm_int2_bb_b0 = {
-	2, 1, 0x12001a0, 0x12001ac, 0x12001a8, 0x12001a4};
-
-static struct attn_hw_reg *mcm_int_bb_b0_regs[3] = {
-	&mcm_int0_bb_b0, &mcm_int1_bb_b0, &mcm_int2_bb_b0};
-
-static struct attn_hw_reg mcm_prty1_bb_b0 = {
-	0, 31, 0x1200200, 0x120020c, 0x1200208, 0x1200204};
-
-static struct attn_hw_reg mcm_prty2_bb_b0 = {
-	1, 4, 0x1200210, 0x120021c, 0x1200218, 0x1200214};
-
-static struct attn_hw_reg *mcm_prty_bb_b0_regs[2] = {
-	&mcm_prty1_bb_b0, &mcm_prty2_bb_b0};
-
-static struct attn_hw_reg ucm_int0_bb_b0 = {
-	0, 17, 0x1280180, 0x128018c, 0x1280188, 0x1280184};
-
-static struct attn_hw_reg ucm_int1_bb_b0 = {
-	1, 29, 0x1280190, 0x128019c, 0x1280198, 0x1280194};
-
-static struct attn_hw_reg ucm_int2_bb_b0 = {
-	2, 1, 0x12801a0, 0x12801ac, 0x12801a8, 0x12801a4};
-
-static struct attn_hw_reg *ucm_int_bb_b0_regs[3] = {
-	&ucm_int0_bb_b0, &ucm_int1_bb_b0, &ucm_int2_bb_b0};
-
-static struct attn_hw_reg ucm_prty1_bb_b0 = {
-	0, 31, 0x1280200, 0x128020c, 0x1280208, 0x1280204};
-
-static struct attn_hw_reg ucm_prty2_bb_b0 = {
-	1, 7, 0x1280210, 0x128021c, 0x1280218, 0x1280214};
-
-static struct attn_hw_reg *ucm_prty_bb_b0_regs[2] = {
-	&ucm_prty1_bb_b0, &ucm_prty2_bb_b0};
-
-static struct attn_hw_reg xcm_int0_bb_b0 = {
-	0, 16, 0x1000180, 0x100018c, 0x1000188, 0x1000184};
-
-static struct attn_hw_reg xcm_int1_bb_b0 = {
-	1, 25, 0x1000190, 0x100019c, 0x1000198, 0x1000194};
-
-static struct attn_hw_reg xcm_int2_bb_b0 = {
-	2, 8, 0x10001a0, 0x10001ac, 0x10001a8, 0x10001a4};
-
-static struct attn_hw_reg *xcm_int_bb_b0_regs[3] = {
-	&xcm_int0_bb_b0, &xcm_int1_bb_b0, &xcm_int2_bb_b0};
-
-static struct attn_hw_reg xcm_prty1_bb_b0 = {
-	0, 31, 0x1000200, 0x100020c, 0x1000208, 0x1000204};
-
-static struct attn_hw_reg xcm_prty2_bb_b0 = {
-	1, 11, 0x1000210, 0x100021c, 0x1000218, 0x1000214};
-
-static struct attn_hw_reg *xcm_prty_bb_b0_regs[2] = {
-	&xcm_prty1_bb_b0, &xcm_prty2_bb_b0};
-
-static struct attn_hw_reg ycm_int0_bb_b0 = {
-	0, 13, 0x1080180, 0x108018c, 0x1080188, 0x1080184};
-
-static struct attn_hw_reg ycm_int1_bb_b0 = {
-	1, 23, 0x1080190, 0x108019c, 0x1080198, 0x1080194};
-
-static struct attn_hw_reg ycm_int2_bb_b0 = {
-	2, 1, 0x10801a0, 0x10801ac, 0x10801a8, 0x10801a4};
-
-static struct attn_hw_reg *ycm_int_bb_b0_regs[3] = {
-	&ycm_int0_bb_b0, &ycm_int1_bb_b0, &ycm_int2_bb_b0};
-
-static struct attn_hw_reg ycm_prty1_bb_b0 = {
-	0, 31, 0x1080200, 0x108020c, 0x1080208, 0x1080204};
-
-static struct attn_hw_reg ycm_prty2_bb_b0 = {
-	1, 3, 0x1080210, 0x108021c, 0x1080218, 0x1080214};
-
-static struct attn_hw_reg *ycm_prty_bb_b0_regs[2] = {
-	&ycm_prty1_bb_b0, &ycm_prty2_bb_b0};
-
-static struct attn_hw_reg pcm_int0_bb_b0 = {
-	0, 5, 0x1100180, 0x110018c, 0x1100188, 0x1100184};
-
-static struct attn_hw_reg pcm_int1_bb_b0 = {
-	1, 14, 0x1100190, 0x110019c, 0x1100198, 0x1100194};
-
-static struct attn_hw_reg pcm_int2_bb_b0 = {
-	2, 1, 0x11001a0, 0x11001ac, 0x11001a8, 0x11001a4};
-
-static struct attn_hw_reg *pcm_int_bb_b0_regs[3] = {
-	&pcm_int0_bb_b0, &pcm_int1_bb_b0, &pcm_int2_bb_b0};
-
-static struct attn_hw_reg pcm_prty1_bb_b0 = {
-	0, 11, 0x1100200, 0x110020c, 0x1100208, 0x1100204};
-
-static struct attn_hw_reg *pcm_prty_bb_b0_regs[1] = {
-	&pcm_prty1_bb_b0};
-
-static struct attn_hw_reg qm_int0_bb_b0 = {
-	0, 22, 0x2f0180, 0x2f018c, 0x2f0188, 0x2f0184};
-
-static struct attn_hw_reg *qm_int_bb_b0_regs[1] = {
-	&qm_int0_bb_b0};
-
-static struct attn_hw_reg qm_prty0_bb_b0 = {
-	0, 11, 0x2f0190, 0x2f019c, 0x2f0198, 0x2f0194};
-
-static struct attn_hw_reg qm_prty1_bb_b0 = {
-	1, 31, 0x2f0200, 0x2f020c, 0x2f0208, 0x2f0204};
-
-static struct attn_hw_reg qm_prty2_bb_b0 = {
-	2, 31, 0x2f0210, 0x2f021c, 0x2f0218, 0x2f0214};
-
-static struct attn_hw_reg qm_prty3_bb_b0 = {
-	3, 11, 0x2f0220, 0x2f022c, 0x2f0228, 0x2f0224};
-
-static struct attn_hw_reg *qm_prty_bb_b0_regs[4] = {
-	&qm_prty0_bb_b0, &qm_prty1_bb_b0, &qm_prty2_bb_b0, &qm_prty3_bb_b0};
-
-static struct attn_hw_reg tm_int0_bb_b0 = {
-	0, 32, 0x2c0180, 0x2c018c, 0x2c0188, 0x2c0184};
-
-static struct attn_hw_reg tm_int1_bb_b0 = {
-	1, 11, 0x2c0190, 0x2c019c, 0x2c0198, 0x2c0194};
-
-static struct attn_hw_reg *tm_int_bb_b0_regs[2] = {
-	&tm_int0_bb_b0, &tm_int1_bb_b0};
-
-static struct attn_hw_reg tm_prty1_bb_b0 = {
-	0, 17, 0x2c0200, 0x2c020c, 0x2c0208, 0x2c0204};
-
-static struct attn_hw_reg *tm_prty_bb_b0_regs[1] = {
-	&tm_prty1_bb_b0};
-
-static struct attn_hw_reg dorq_int0_bb_b0 = {
-	0, 9, 0x100180, 0x10018c, 0x100188, 0x100184};
-
-static struct attn_hw_reg *dorq_int_bb_b0_regs[1] = {
-	&dorq_int0_bb_b0};
-
-static struct attn_hw_reg dorq_prty0_bb_b0 = {
-	0, 1, 0x100190, 0x10019c, 0x100198, 0x100194};
-
-static struct attn_hw_reg dorq_prty1_bb_b0 = {
-	1, 6, 0x100200, 0x10020c, 0x100208, 0x100204};
-
-static struct attn_hw_reg *dorq_prty_bb_b0_regs[2] = {
-	&dorq_prty0_bb_b0, &dorq_prty1_bb_b0};
-
-static struct attn_hw_reg brb_int0_bb_b0 = {
-	0, 32, 0x3400c0, 0x3400cc, 0x3400c8, 0x3400c4};
-
-static struct attn_hw_reg brb_int1_bb_b0 = {
-	1, 30, 0x3400d8, 0x3400e4, 0x3400e0, 0x3400dc};
-
-static struct attn_hw_reg brb_int2_bb_b0 = {
-	2, 28, 0x3400f0, 0x3400fc, 0x3400f8, 0x3400f4};
-
-static struct attn_hw_reg brb_int3_bb_b0 = {
-	3, 31, 0x340108, 0x340114, 0x340110, 0x34010c};
-
-static struct attn_hw_reg brb_int4_bb_b0 = {
-	4, 27, 0x340120, 0x34012c, 0x340128, 0x340124};
-
-static struct attn_hw_reg brb_int5_bb_b0 = {
-	5, 1, 0x340138, 0x340144, 0x340140, 0x34013c};
-
-static struct attn_hw_reg brb_int6_bb_b0 = {
-	6, 8, 0x340150, 0x34015c, 0x340158, 0x340154};
-
-static struct attn_hw_reg brb_int7_bb_b0 = {
-	7, 32, 0x340168, 0x340174, 0x340170, 0x34016c};
-
-static struct attn_hw_reg brb_int8_bb_b0 = {
-	8, 17, 0x340184, 0x340190, 0x34018c, 0x340188};
-
-static struct attn_hw_reg brb_int9_bb_b0 = {
-	9, 1, 0x34019c, 0x3401a8, 0x3401a4, 0x3401a0};
-
-static struct attn_hw_reg brb_int10_bb_b0 = {
-	10, 14, 0x3401b4, 0x3401c0, 0x3401bc, 0x3401b8};
-
-static struct attn_hw_reg brb_int11_bb_b0 = {
-	11, 8, 0x3401cc, 0x3401d8, 0x3401d4, 0x3401d0};
-
-static struct attn_hw_reg *brb_int_bb_b0_regs[12] = {
-	&brb_int0_bb_b0, &brb_int1_bb_b0, &brb_int2_bb_b0, &brb_int3_bb_b0,
-	&brb_int4_bb_b0, &brb_int5_bb_b0, &brb_int6_bb_b0, &brb_int7_bb_b0,
-	&brb_int8_bb_b0, &brb_int9_bb_b0, &brb_int10_bb_b0, &brb_int11_bb_b0};
-
-static struct attn_hw_reg brb_prty0_bb_b0 = {
-	0, 5, 0x3401dc, 0x3401e8, 0x3401e4, 0x3401e0};
-
-static struct attn_hw_reg brb_prty1_bb_b0 = {
-	1, 31, 0x340400, 0x34040c, 0x340408, 0x340404};
-
-static struct attn_hw_reg brb_prty2_bb_b0 = {
-	2, 14, 0x340410, 0x34041c, 0x340418, 0x340414};
-
-static struct attn_hw_reg *brb_prty_bb_b0_regs[3] = {
-	&brb_prty0_bb_b0, &brb_prty1_bb_b0, &brb_prty2_bb_b0};
-
-static struct attn_hw_reg src_int0_bb_b0 = {
-	0, 1, 0x2381d8, 0x2381dc, 0x2381e0, 0x2381e4};
-
-static struct attn_hw_reg *src_int_bb_b0_regs[1] = {
-	&src_int0_bb_b0};
-
-static struct attn_hw_reg prs_int0_bb_b0 = {
-	0, 2, 0x1f0040, 0x1f004c, 0x1f0048, 0x1f0044};
-
-static struct attn_hw_reg *prs_int_bb_b0_regs[1] = {
-	&prs_int0_bb_b0};
-
-static struct attn_hw_reg prs_prty0_bb_b0 = {
-	0, 2, 0x1f0050, 0x1f005c, 0x1f0058, 0x1f0054};
-
-static struct attn_hw_reg prs_prty1_bb_b0 = {
-	1, 31, 0x1f0204, 0x1f0210, 0x1f020c, 0x1f0208};
-
-static struct attn_hw_reg prs_prty2_bb_b0 = {
-	2, 5, 0x1f0214, 0x1f0220, 0x1f021c, 0x1f0218};
-
-static struct attn_hw_reg *prs_prty_bb_b0_regs[3] = {
-	&prs_prty0_bb_b0, &prs_prty1_bb_b0, &prs_prty2_bb_b0};
-
-static struct attn_hw_reg tsdm_int0_bb_b0 = {
-	0, 26, 0xfb0040, 0xfb004c, 0xfb0048, 0xfb0044};
-
-static struct attn_hw_reg *tsdm_int_bb_b0_regs[1] = {
-	&tsdm_int0_bb_b0};
-
-static struct attn_hw_reg tsdm_prty1_bb_b0 = {
-	0, 10, 0xfb0200, 0xfb020c, 0xfb0208, 0xfb0204};
-
-static struct attn_hw_reg *tsdm_prty_bb_b0_regs[1] = {
-	&tsdm_prty1_bb_b0};
-
-static struct attn_hw_reg msdm_int0_bb_b0 = {
-	0, 26, 0xfc0040, 0xfc004c, 0xfc0048, 0xfc0044};
-
-static struct attn_hw_reg *msdm_int_bb_b0_regs[1] = {
-	&msdm_int0_bb_b0};
-
-static struct attn_hw_reg msdm_prty1_bb_b0 = {
-	0, 11, 0xfc0200, 0xfc020c, 0xfc0208, 0xfc0204};
-
-static struct attn_hw_reg *msdm_prty_bb_b0_regs[1] = {
-	&msdm_prty1_bb_b0};
-
-static struct attn_hw_reg usdm_int0_bb_b0 = {
-	0, 26, 0xfd0040, 0xfd004c, 0xfd0048, 0xfd0044};
-
-static struct attn_hw_reg *usdm_int_bb_b0_regs[1] = {
-	&usdm_int0_bb_b0};
-
-static struct attn_hw_reg usdm_prty1_bb_b0 = {
-	0, 10, 0xfd0200, 0xfd020c, 0xfd0208, 0xfd0204};
-
-static struct attn_hw_reg *usdm_prty_bb_b0_regs[1] = {
-	&usdm_prty1_bb_b0};
-
-static struct attn_hw_reg xsdm_int0_bb_b0 = {
-	0, 26, 0xf80040, 0xf8004c, 0xf80048, 0xf80044};
-
-static struct attn_hw_reg *xsdm_int_bb_b0_regs[1] = {
-	&xsdm_int0_bb_b0};
-
-static struct attn_hw_reg xsdm_prty1_bb_b0 = {
-	0, 10, 0xf80200, 0xf8020c, 0xf80208, 0xf80204};
-
-static struct attn_hw_reg *xsdm_prty_bb_b0_regs[1] = {
-	&xsdm_prty1_bb_b0};
-
-static struct attn_hw_reg ysdm_int0_bb_b0 = {
-	0, 26, 0xf90040, 0xf9004c, 0xf90048, 0xf90044};
-
-static struct attn_hw_reg *ysdm_int_bb_b0_regs[1] = {
-	&ysdm_int0_bb_b0};
-
-static struct attn_hw_reg ysdm_prty1_bb_b0 = {
-	0, 9, 0xf90200, 0xf9020c, 0xf90208, 0xf90204};
-
-static struct attn_hw_reg *ysdm_prty_bb_b0_regs[1] = {
-	&ysdm_prty1_bb_b0};
-
-static struct attn_hw_reg psdm_int0_bb_b0 = {
-	0, 26, 0xfa0040, 0xfa004c, 0xfa0048, 0xfa0044};
-
-static struct attn_hw_reg *psdm_int_bb_b0_regs[1] = {
-	&psdm_int0_bb_b0};
-
-static struct attn_hw_reg psdm_prty1_bb_b0 = {
-	0, 9, 0xfa0200, 0xfa020c, 0xfa0208, 0xfa0204};
-
-static struct attn_hw_reg *psdm_prty_bb_b0_regs[1] = {
-	&psdm_prty1_bb_b0};
-
-static struct attn_hw_reg tsem_int0_bb_b0 = {
-	0, 32, 0x1700040, 0x170004c, 0x1700048, 0x1700044};
-
-static struct attn_hw_reg tsem_int1_bb_b0 = {
-	1, 13, 0x1700050, 0x170005c, 0x1700058, 0x1700054};
-
-static struct attn_hw_reg tsem_fast_memory_int0_bb_b0 = {
-	2, 1, 0x1740040, 0x174004c, 0x1740048, 0x1740044};
-
-static struct attn_hw_reg *tsem_int_bb_b0_regs[3] = {
-	&tsem_int0_bb_b0, &tsem_int1_bb_b0, &tsem_fast_memory_int0_bb_b0};
-
-static struct attn_hw_reg tsem_prty0_bb_b0 = {
-	0, 3, 0x17000c8, 0x17000d4, 0x17000d0, 0x17000cc};
-
-static struct attn_hw_reg tsem_prty1_bb_b0 = {
-	1, 6, 0x1700200, 0x170020c, 0x1700208, 0x1700204};
-
-static struct attn_hw_reg tsem_fast_memory_vfc_config_prty1_bb_b0 = {
-	2, 6, 0x174a200, 0x174a20c, 0x174a208, 0x174a204};
-
-static struct attn_hw_reg *tsem_prty_bb_b0_regs[3] = {
-	&tsem_prty0_bb_b0, &tsem_prty1_bb_b0,
-	&tsem_fast_memory_vfc_config_prty1_bb_b0};
-
-static struct attn_hw_reg msem_int0_bb_b0 = {
-	0, 32, 0x1800040, 0x180004c, 0x1800048, 0x1800044};
-
-static struct attn_hw_reg msem_int1_bb_b0 = {
-	1, 13, 0x1800050, 0x180005c, 0x1800058, 0x1800054};
-
-static struct attn_hw_reg msem_fast_memory_int0_bb_b0 = {
-	2, 1, 0x1840040, 0x184004c, 0x1840048, 0x1840044};
-
-static struct attn_hw_reg *msem_int_bb_b0_regs[3] = {
-	&msem_int0_bb_b0, &msem_int1_bb_b0, &msem_fast_memory_int0_bb_b0};
-
-static struct attn_hw_reg msem_prty0_bb_b0 = {
-	0, 3, 0x18000c8, 0x18000d4, 0x18000d0, 0x18000cc};
-
-static struct attn_hw_reg msem_prty1_bb_b0 = {
-	1, 6, 0x1800200, 0x180020c, 0x1800208, 0x1800204};
-
-static struct attn_hw_reg *msem_prty_bb_b0_regs[2] = {
-	&msem_prty0_bb_b0, &msem_prty1_bb_b0};
-
-static struct attn_hw_reg usem_int0_bb_b0 = {
-	0, 32, 0x1900040, 0x190004c, 0x1900048, 0x1900044};
-
-static struct attn_hw_reg usem_int1_bb_b0 = {
-	1, 13, 0x1900050, 0x190005c, 0x1900058, 0x1900054};
-
-static struct attn_hw_reg usem_fast_memory_int0_bb_b0 = {
-	2, 1, 0x1940040, 0x194004c, 0x1940048, 0x1940044};
-
-static struct attn_hw_reg *usem_int_bb_b0_regs[3] = {
-	&usem_int0_bb_b0, &usem_int1_bb_b0, &usem_fast_memory_int0_bb_b0};
-
-static struct attn_hw_reg usem_prty0_bb_b0 = {
-	0, 3, 0x19000c8, 0x19000d4, 0x19000d0, 0x19000cc};
-
-static struct attn_hw_reg usem_prty1_bb_b0 = {
-	1, 6, 0x1900200, 0x190020c, 0x1900208, 0x1900204};
-
-static struct attn_hw_reg *usem_prty_bb_b0_regs[2] = {
-	&usem_prty0_bb_b0, &usem_prty1_bb_b0};
-
-static struct attn_hw_reg xsem_int0_bb_b0 = {
-	0, 32, 0x1400040, 0x140004c, 0x1400048, 0x1400044};
-
-static struct attn_hw_reg xsem_int1_bb_b0 = {
-	1, 13, 0x1400050, 0x140005c, 0x1400058, 0x1400054};
-
-static struct attn_hw_reg xsem_fast_memory_int0_bb_b0 = {
-	2, 1, 0x1440040, 0x144004c, 0x1440048, 0x1440044};
-
-static struct attn_hw_reg *xsem_int_bb_b0_regs[3] = {
-	&xsem_int0_bb_b0, &xsem_int1_bb_b0, &xsem_fast_memory_int0_bb_b0};
-
-static struct attn_hw_reg xsem_prty0_bb_b0 = {
-	0, 3, 0x14000c8, 0x14000d4, 0x14000d0, 0x14000cc};
-
-static struct attn_hw_reg xsem_prty1_bb_b0 = {
-	1, 7, 0x1400200, 0x140020c, 0x1400208, 0x1400204};
-
-static struct attn_hw_reg *xsem_prty_bb_b0_regs[2] = {
-	&xsem_prty0_bb_b0, &xsem_prty1_bb_b0};
-
-static struct attn_hw_reg ysem_int0_bb_b0 = {
-	0, 32, 0x1500040, 0x150004c, 0x1500048, 0x1500044};
-
-static struct attn_hw_reg ysem_int1_bb_b0 = {
-	1, 13, 0x1500050, 0x150005c, 0x1500058, 0x1500054};
-
-static struct attn_hw_reg ysem_fast_memory_int0_bb_b0 = {
-	2, 1, 0x1540040, 0x154004c, 0x1540048, 0x1540044};
-
-static struct attn_hw_reg *ysem_int_bb_b0_regs[3] = {
-	&ysem_int0_bb_b0, &ysem_int1_bb_b0, &ysem_fast_memory_int0_bb_b0};
-
-static struct attn_hw_reg ysem_prty0_bb_b0 = {
-	0, 3, 0x15000c8, 0x15000d4, 0x15000d0, 0x15000cc};
-
-static struct attn_hw_reg ysem_prty1_bb_b0 = {
-	1, 7, 0x1500200, 0x150020c, 0x1500208, 0x1500204};
-
-static struct attn_hw_reg *ysem_prty_bb_b0_regs[2] = {
-	&ysem_prty0_bb_b0, &ysem_prty1_bb_b0};
-
-static struct attn_hw_reg psem_int0_bb_b0 = {
-	0, 32, 0x1600040, 0x160004c, 0x1600048, 0x1600044};
-
-static struct attn_hw_reg psem_int1_bb_b0 = {
-	1, 13, 0x1600050, 0x160005c, 0x1600058, 0x1600054};
-
-static struct attn_hw_reg psem_fast_memory_int0_bb_b0 = {
-	2, 1, 0x1640040, 0x164004c, 0x1640048, 0x1640044};
-
-static struct attn_hw_reg *psem_int_bb_b0_regs[3] = {
-	&psem_int0_bb_b0, &psem_int1_bb_b0, &psem_fast_memory_int0_bb_b0};
-
-static struct attn_hw_reg psem_prty0_bb_b0 = {
-	0, 3, 0x16000c8, 0x16000d4, 0x16000d0, 0x16000cc};
-
-static struct attn_hw_reg psem_prty1_bb_b0 = {
-	1, 6, 0x1600200, 0x160020c, 0x1600208, 0x1600204};
-
-static struct attn_hw_reg psem_fast_memory_vfc_config_prty1_bb_b0 = {
-	2, 6, 0x164a200, 0x164a20c, 0x164a208, 0x164a204};
-
-static struct attn_hw_reg *psem_prty_bb_b0_regs[3] = {
-	&psem_prty0_bb_b0, &psem_prty1_bb_b0,
-	&psem_fast_memory_vfc_config_prty1_bb_b0};
-
-static struct attn_hw_reg rss_int0_bb_b0 = {
-	0, 12, 0x238980, 0x23898c, 0x238988, 0x238984};
-
-static struct attn_hw_reg *rss_int_bb_b0_regs[1] = {
-	&rss_int0_bb_b0};
-
-static struct attn_hw_reg rss_prty1_bb_b0 = {
-	0, 4, 0x238a00, 0x238a0c, 0x238a08, 0x238a04};
-
-static struct attn_hw_reg *rss_prty_bb_b0_regs[1] = {
-	&rss_prty1_bb_b0};
-
-static struct attn_hw_reg tmld_int0_bb_b0 = {
-	0, 6, 0x4d0180, 0x4d018c, 0x4d0188, 0x4d0184};
-
-static struct attn_hw_reg *tmld_int_bb_b0_regs[1] = {
-	&tmld_int0_bb_b0};
-
-static struct attn_hw_reg tmld_prty1_bb_b0 = {
-	0, 8, 0x4d0200, 0x4d020c, 0x4d0208, 0x4d0204};
-
-static struct attn_hw_reg *tmld_prty_bb_b0_regs[1] = {
-	&tmld_prty1_bb_b0};
-
-static struct attn_hw_reg muld_int0_bb_b0 = {
-	0, 6, 0x4e0180, 0x4e018c, 0x4e0188, 0x4e0184};
-
-static struct attn_hw_reg *muld_int_bb_b0_regs[1] = {
-	&muld_int0_bb_b0};
-
-static struct attn_hw_reg muld_prty1_bb_b0 = {
-	0, 10, 0x4e0200, 0x4e020c, 0x4e0208, 0x4e0204};
-
-static struct attn_hw_reg *muld_prty_bb_b0_regs[1] = {
-	&muld_prty1_bb_b0};
-
-static struct attn_hw_reg yuld_int0_bb_b0 = {
-	0, 6, 0x4c8180, 0x4c818c, 0x4c8188, 0x4c8184};
-
-static struct attn_hw_reg *yuld_int_bb_b0_regs[1] = {
-	&yuld_int0_bb_b0};
-
-static struct attn_hw_reg yuld_prty1_bb_b0 = {
-	0, 6, 0x4c8200, 0x4c820c, 0x4c8208, 0x4c8204};
-
-static struct attn_hw_reg *yuld_prty_bb_b0_regs[1] = {
-	&yuld_prty1_bb_b0};
-
-static struct attn_hw_reg xyld_int0_bb_b0 = {
-	0, 6, 0x4c0180, 0x4c018c, 0x4c0188, 0x4c0184};
-
-static struct attn_hw_reg *xyld_int_bb_b0_regs[1] = {
-	&xyld_int0_bb_b0};
-
-static struct attn_hw_reg xyld_prty1_bb_b0 = {
-	0, 9, 0x4c0200, 0x4c020c, 0x4c0208, 0x4c0204};
-
-static struct attn_hw_reg *xyld_prty_bb_b0_regs[1] = {
-	&xyld_prty1_bb_b0};
-
-static struct attn_hw_reg prm_int0_bb_b0 = {
-	0, 11, 0x230040, 0x23004c, 0x230048, 0x230044};
-
-static struct attn_hw_reg *prm_int_bb_b0_regs[1] = {
-	&prm_int0_bb_b0};
-
-static struct attn_hw_reg prm_prty0_bb_b0 = {
-	0, 1, 0x230050, 0x23005c, 0x230058, 0x230054};
-
-static struct attn_hw_reg prm_prty1_bb_b0 = {
-	1, 24, 0x230200, 0x23020c, 0x230208, 0x230204};
-
-static struct attn_hw_reg *prm_prty_bb_b0_regs[2] = {
-	&prm_prty0_bb_b0, &prm_prty1_bb_b0};
-
-static struct attn_hw_reg pbf_pb1_int0_bb_b0 = {
-	0, 9, 0xda0040, 0xda004c, 0xda0048, 0xda0044};
-
-static struct attn_hw_reg *pbf_pb1_int_bb_b0_regs[1] = {
-	&pbf_pb1_int0_bb_b0};
-
-static struct attn_hw_reg pbf_pb1_prty0_bb_b0 = {
-	0, 1, 0xda0050, 0xda005c, 0xda0058, 0xda0054};
-
-static struct attn_hw_reg *pbf_pb1_prty_bb_b0_regs[1] = {
-	&pbf_pb1_prty0_bb_b0};
-
-static struct attn_hw_reg pbf_pb2_int0_bb_b0 = {
-	0, 9, 0xda4040, 0xda404c, 0xda4048, 0xda4044};
-
-static struct attn_hw_reg *pbf_pb2_int_bb_b0_regs[1] = {
-	&pbf_pb2_int0_bb_b0};
-
-static struct attn_hw_reg pbf_pb2_prty0_bb_b0 = {
-	0, 1, 0xda4050, 0xda405c, 0xda4058, 0xda4054};
-
-static struct attn_hw_reg *pbf_pb2_prty_bb_b0_regs[1] = {
-	&pbf_pb2_prty0_bb_b0};
-
-static struct attn_hw_reg rpb_int0_bb_b0 = {
-	0, 9, 0x23c040, 0x23c04c, 0x23c048, 0x23c044};
-
-static struct attn_hw_reg *rpb_int_bb_b0_regs[1] = {
-	&rpb_int0_bb_b0};
-
-static struct attn_hw_reg rpb_prty0_bb_b0 = {
-	0, 1, 0x23c050, 0x23c05c, 0x23c058, 0x23c054};
-
-static struct attn_hw_reg *rpb_prty_bb_b0_regs[1] = {
-	&rpb_prty0_bb_b0};
-
-static struct attn_hw_reg btb_int0_bb_b0 = {
-	0, 16, 0xdb00c0, 0xdb00cc, 0xdb00c8, 0xdb00c4};
-
-static struct attn_hw_reg btb_int1_bb_b0 = {
-	1, 16, 0xdb00d8, 0xdb00e4, 0xdb00e0, 0xdb00dc};
-
-static struct attn_hw_reg btb_int2_bb_b0 = {
-	2, 4, 0xdb00f0, 0xdb00fc, 0xdb00f8, 0xdb00f4};
-
-static struct attn_hw_reg btb_int3_bb_b0 = {
-	3, 32, 0xdb0108, 0xdb0114, 0xdb0110, 0xdb010c};
-
-static struct attn_hw_reg btb_int4_bb_b0 = {
-	4, 23, 0xdb0120, 0xdb012c, 0xdb0128, 0xdb0124};
-
-static struct attn_hw_reg btb_int5_bb_b0 = {
-	5, 32, 0xdb0138, 0xdb0144, 0xdb0140, 0xdb013c};
-
-static struct attn_hw_reg btb_int6_bb_b0 = {
-	6, 1, 0xdb0150, 0xdb015c, 0xdb0158, 0xdb0154};
-
-static struct attn_hw_reg btb_int8_bb_b0 = {
-	7, 1, 0xdb0184, 0xdb0190, 0xdb018c, 0xdb0188};
-
-static struct attn_hw_reg btb_int9_bb_b0 = {
-	8, 1, 0xdb019c, 0xdb01a8, 0xdb01a4, 0xdb01a0};
-
-static struct attn_hw_reg btb_int10_bb_b0 = {
-	9, 1, 0xdb01b4, 0xdb01c0, 0xdb01bc, 0xdb01b8};
-
-static struct attn_hw_reg btb_int11_bb_b0 = {
-	10, 2, 0xdb01cc, 0xdb01d8, 0xdb01d4, 0xdb01d0};
-
-static struct attn_hw_reg *btb_int_bb_b0_regs[11] = {
-	&btb_int0_bb_b0, &btb_int1_bb_b0, &btb_int2_bb_b0, &btb_int3_bb_b0,
-	&btb_int4_bb_b0, &btb_int5_bb_b0, &btb_int6_bb_b0, &btb_int8_bb_b0,
-	&btb_int9_bb_b0, &btb_int10_bb_b0, &btb_int11_bb_b0};
-
-static struct attn_hw_reg btb_prty0_bb_b0 = {
-	0, 5, 0xdb01dc, 0xdb01e8, 0xdb01e4, 0xdb01e0};
-
-static struct attn_hw_reg btb_prty1_bb_b0 = {
-	1, 23, 0xdb0400, 0xdb040c, 0xdb0408, 0xdb0404};
-
-static struct attn_hw_reg *btb_prty_bb_b0_regs[2] = {
-	&btb_prty0_bb_b0, &btb_prty1_bb_b0};
-
-static struct attn_hw_reg pbf_int0_bb_b0 = {
-	0, 1, 0xd80180, 0xd8018c, 0xd80188, 0xd80184};
-
-static struct attn_hw_reg *pbf_int_bb_b0_regs[1] = {
-	&pbf_int0_bb_b0};
-
-static struct attn_hw_reg pbf_prty0_bb_b0 = {
-	0, 1, 0xd80190, 0xd8019c, 0xd80198, 0xd80194};
-
-static struct attn_hw_reg pbf_prty1_bb_b0 = {
-	1, 31, 0xd80200, 0xd8020c, 0xd80208, 0xd80204};
-
-static struct attn_hw_reg pbf_prty2_bb_b0 = {
-	2, 27, 0xd80210, 0xd8021c, 0xd80218, 0xd80214};
-
-static struct attn_hw_reg *pbf_prty_bb_b0_regs[3] = {
-	&pbf_prty0_bb_b0, &pbf_prty1_bb_b0, &pbf_prty2_bb_b0};
-
-static struct attn_hw_reg rdif_int0_bb_b0 = {
-	0, 8, 0x300180, 0x30018c, 0x300188, 0x300184};
-
-static struct attn_hw_reg *rdif_int_bb_b0_regs[1] = {
-	&rdif_int0_bb_b0};
-
-static struct attn_hw_reg rdif_prty0_bb_b0 = {
-	0, 1, 0x300190, 0x30019c, 0x300198, 0x300194};
-
-static struct attn_hw_reg *rdif_prty_bb_b0_regs[1] = {
-	&rdif_prty0_bb_b0};
-
-static struct attn_hw_reg tdif_int0_bb_b0 = {
-	0, 8, 0x310180, 0x31018c, 0x310188, 0x310184};
-
-static struct attn_hw_reg *tdif_int_bb_b0_regs[1] = {
-	&tdif_int0_bb_b0};
-
-static struct attn_hw_reg tdif_prty0_bb_b0 = {
-	0, 1, 0x310190, 0x31019c, 0x310198, 0x310194};
-
-static struct attn_hw_reg tdif_prty1_bb_b0 = {
-	1, 11, 0x310200, 0x31020c, 0x310208, 0x310204};
-
-static struct attn_hw_reg *tdif_prty_bb_b0_regs[2] = {
-	&tdif_prty0_bb_b0, &tdif_prty1_bb_b0};
-
-static struct attn_hw_reg cdu_int0_bb_b0 = {
-	0, 8, 0x5801c0, 0x5801c4, 0x5801c8, 0x5801cc};
-
-static struct attn_hw_reg *cdu_int_bb_b0_regs[1] = {
-	&cdu_int0_bb_b0};
-
-static struct attn_hw_reg cdu_prty1_bb_b0 = {
-	0, 5, 0x580200, 0x58020c, 0x580208, 0x580204};
-
-static struct attn_hw_reg *cdu_prty_bb_b0_regs[1] = {
-	&cdu_prty1_bb_b0};
-
-static struct attn_hw_reg ccfc_int0_bb_b0 = {
-	0, 2, 0x2e0180, 0x2e018c, 0x2e0188, 0x2e0184};
-
-static struct attn_hw_reg *ccfc_int_bb_b0_regs[1] = {
-	&ccfc_int0_bb_b0};
-
-static struct attn_hw_reg ccfc_prty1_bb_b0 = {
-	0, 2, 0x2e0200, 0x2e020c, 0x2e0208, 0x2e0204};
-
-static struct attn_hw_reg ccfc_prty0_bb_b0 = {
-	1, 6, 0x2e05e4, 0x2e05f0, 0x2e05ec, 0x2e05e8};
-
-static struct attn_hw_reg *ccfc_prty_bb_b0_regs[2] = {
-	&ccfc_prty1_bb_b0, &ccfc_prty0_bb_b0};
-
-static struct attn_hw_reg tcfc_int0_bb_b0 = {
-	0, 2, 0x2d0180, 0x2d018c, 0x2d0188, 0x2d0184};
-
-static struct attn_hw_reg *tcfc_int_bb_b0_regs[1] = {
-	&tcfc_int0_bb_b0};
-
-static struct attn_hw_reg tcfc_prty1_bb_b0 = {
-	0, 2, 0x2d0200, 0x2d020c, 0x2d0208, 0x2d0204};
-
-static struct attn_hw_reg tcfc_prty0_bb_b0 = {
-	1, 6, 0x2d05e4, 0x2d05f0, 0x2d05ec, 0x2d05e8};
-
-static struct attn_hw_reg *tcfc_prty_bb_b0_regs[2] = {
-	&tcfc_prty1_bb_b0, &tcfc_prty0_bb_b0};
-
-static struct attn_hw_reg igu_int0_bb_b0 = {
-	0, 11, 0x180180, 0x18018c, 0x180188, 0x180184};
-
-static struct attn_hw_reg *igu_int_bb_b0_regs[1] = {
-	&igu_int0_bb_b0};
-
-static struct attn_hw_reg igu_prty0_bb_b0 = {
-	0, 1, 0x180190, 0x18019c, 0x180198, 0x180194};
-
-static struct attn_hw_reg igu_prty1_bb_b0 = {
-	1, 31, 0x180200, 0x18020c, 0x180208, 0x180204};
-
-static struct attn_hw_reg igu_prty2_bb_b0 = {
-	2, 1, 0x180210, 0x18021c, 0x180218, 0x180214};
-
-static struct attn_hw_reg *igu_prty_bb_b0_regs[3] = {
-	&igu_prty0_bb_b0, &igu_prty1_bb_b0, &igu_prty2_bb_b0};
-
-static struct attn_hw_reg cau_int0_bb_b0 = {
-	0, 11, 0x1c00d4, 0x1c00d8, 0x1c00dc, 0x1c00e0};
-
-static struct attn_hw_reg *cau_int_bb_b0_regs[1] = {
-	&cau_int0_bb_b0};
-
-static struct attn_hw_reg cau_prty1_bb_b0 = {
-	0, 13, 0x1c0200, 0x1c020c, 0x1c0208, 0x1c0204};
-
-static struct attn_hw_reg *cau_prty_bb_b0_regs[1] = {
-	&cau_prty1_bb_b0};
-
-static struct attn_hw_reg dbg_int0_bb_b0 = {
-	0, 1, 0x10180, 0x1018c, 0x10188, 0x10184};
-
-static struct attn_hw_reg *dbg_int_bb_b0_regs[1] = {
-	&dbg_int0_bb_b0};
-
-static struct attn_hw_reg dbg_prty1_bb_b0 = {
-	0, 1, 0x10200, 0x1020c, 0x10208, 0x10204};
-
-static struct attn_hw_reg *dbg_prty_bb_b0_regs[1] = {
-	&dbg_prty1_bb_b0};
-
-static struct attn_hw_reg nig_int0_bb_b0 = {
-	0, 12, 0x500040, 0x50004c, 0x500048, 0x500044};
-
-static struct attn_hw_reg nig_int1_bb_b0 = {
-	1, 32, 0x500050, 0x50005c, 0x500058, 0x500054};
-
-static struct attn_hw_reg nig_int2_bb_b0 = {
-	2, 20, 0x500060, 0x50006c, 0x500068, 0x500064};
-
-static struct attn_hw_reg nig_int3_bb_b0 = {
-	3, 18, 0x500070, 0x50007c, 0x500078, 0x500074};
-
-static struct attn_hw_reg nig_int4_bb_b0 = {
-	4, 20, 0x500080, 0x50008c, 0x500088, 0x500084};
-
-static struct attn_hw_reg nig_int5_bb_b0 = {
-	5, 18, 0x500090, 0x50009c, 0x500098, 0x500094};
-
-static struct attn_hw_reg *nig_int_bb_b0_regs[6] = {
-	&nig_int0_bb_b0, &nig_int1_bb_b0, &nig_int2_bb_b0, &nig_int3_bb_b0,
-	&nig_int4_bb_b0, &nig_int5_bb_b0};
-
-static struct attn_hw_reg nig_prty0_bb_b0 = {
-	0, 1, 0x5000a0, 0x5000ac, 0x5000a8, 0x5000a4};
-
-static struct attn_hw_reg nig_prty1_bb_b0 = {
-	1, 31, 0x500200, 0x50020c, 0x500208, 0x500204};
-
-static struct attn_hw_reg nig_prty2_bb_b0 = {
-	2, 31, 0x500210, 0x50021c, 0x500218, 0x500214};
-
-static struct attn_hw_reg nig_prty3_bb_b0 = {
-	3, 31, 0x500220, 0x50022c, 0x500228, 0x500224};
-
-static struct attn_hw_reg nig_prty4_bb_b0 = {
-	4, 17, 0x500230, 0x50023c, 0x500238, 0x500234};
-
-static struct attn_hw_reg *nig_prty_bb_b0_regs[5] = {
-	&nig_prty0_bb_b0, &nig_prty1_bb_b0, &nig_prty2_bb_b0,
-	&nig_prty3_bb_b0, &nig_prty4_bb_b0};
-
-static struct attn_hw_reg ipc_int0_bb_b0 = {
-	0, 13, 0x2050c, 0x20518, 0x20514, 0x20510};
-
-static struct attn_hw_reg *ipc_int_bb_b0_regs[1] = {
-	&ipc_int0_bb_b0};
-
-static struct attn_hw_reg ipc_prty0_bb_b0 = {
-	0, 1, 0x2051c, 0x20528, 0x20524, 0x20520};
-
-static struct attn_hw_reg *ipc_prty_bb_b0_regs[1] = {
-	&ipc_prty0_bb_b0};
-
-static struct attn_hw_block attn_blocks[] = {
-	{"grc", {{1, 1, grc_int_bb_b0_regs, grc_prty_bb_b0_regs} } },
-	{"miscs", {{2, 1, miscs_int_bb_b0_regs, miscs_prty_bb_b0_regs} } },
-	{"misc", {{1, 0, misc_int_bb_b0_regs, NULL} } },
-	{"dbu", {{0, 0, NULL, NULL} } },
-	{"pglue_b", {{1, 2, pglue_b_int_bb_b0_regs,
-		      pglue_b_prty_bb_b0_regs} } },
-	{"cnig", {{1, 1, cnig_int_bb_b0_regs, cnig_prty_bb_b0_regs} } },
-	{"cpmu", {{1, 0, cpmu_int_bb_b0_regs, NULL} } },
-	{"ncsi", {{1, 1, ncsi_int_bb_b0_regs, ncsi_prty_bb_b0_regs} } },
-	{"opte", {{0, 2, NULL, opte_prty_bb_b0_regs} } },
-	{"bmb", {{12, 3, bmb_int_bb_b0_regs, bmb_prty_bb_b0_regs} } },
-	{"pcie", {{0, 1, NULL, pcie_prty_bb_b0_regs} } },
-	{"mcp", {{0, 0, NULL, NULL} } },
-	{"mcp2", {{0, 2, NULL, mcp2_prty_bb_b0_regs} } },
-	{"pswhst", {{1, 2, pswhst_int_bb_b0_regs, pswhst_prty_bb_b0_regs} } },
-	{"pswhst2", {{1, 1, pswhst2_int_bb_b0_regs,
-		      pswhst2_prty_bb_b0_regs} } },
-	{"pswrd", {{1, 1, pswrd_int_bb_b0_regs, pswrd_prty_bb_b0_regs} } },
-	{"pswrd2", {{1, 3, pswrd2_int_bb_b0_regs, pswrd2_prty_bb_b0_regs} } },
-	{"pswwr", {{1, 1, pswwr_int_bb_b0_regs, pswwr_prty_bb_b0_regs} } },
-	{"pswwr2", {{1, 5, pswwr2_int_bb_b0_regs, pswwr2_prty_bb_b0_regs} } },
-	{"pswrq", {{1, 1, pswrq_int_bb_b0_regs, pswrq_prty_bb_b0_regs} } },
-	{"pswrq2", {{1, 1, pswrq2_int_bb_b0_regs, pswrq2_prty_bb_b0_regs} } },
-	{"pglcs", {{1, 0, pglcs_int_bb_b0_regs, NULL} } },
-	{"dmae", {{1, 1, dmae_int_bb_b0_regs, dmae_prty_bb_b0_regs} } },
-	{"ptu", {{1, 1, ptu_int_bb_b0_regs, ptu_prty_bb_b0_regs} } },
-	{"tcm", {{3, 2, tcm_int_bb_b0_regs, tcm_prty_bb_b0_regs} } },
-	{"mcm", {{3, 2, mcm_int_bb_b0_regs, mcm_prty_bb_b0_regs} } },
-	{"ucm", {{3, 2, ucm_int_bb_b0_regs, ucm_prty_bb_b0_regs} } },
-	{"xcm", {{3, 2, xcm_int_bb_b0_regs, xcm_prty_bb_b0_regs} } },
-	{"ycm", {{3, 2, ycm_int_bb_b0_regs, ycm_prty_bb_b0_regs} } },
-	{"pcm", {{3, 1, pcm_int_bb_b0_regs, pcm_prty_bb_b0_regs} } },
-	{"qm", {{1, 4, qm_int_bb_b0_regs, qm_prty_bb_b0_regs} } },
-	{"tm", {{2, 1, tm_int_bb_b0_regs, tm_prty_bb_b0_regs} } },
-	{"dorq", {{1, 2, dorq_int_bb_b0_regs, dorq_prty_bb_b0_regs} } },
-	{"brb", {{12, 3, brb_int_bb_b0_regs, brb_prty_bb_b0_regs} } },
-	{"src", {{1, 0, src_int_bb_b0_regs, NULL} } },
-	{"prs", {{1, 3, prs_int_bb_b0_regs, prs_prty_bb_b0_regs} } },
-	{"tsdm", {{1, 1, tsdm_int_bb_b0_regs, tsdm_prty_bb_b0_regs} } },
-	{"msdm", {{1, 1, msdm_int_bb_b0_regs, msdm_prty_bb_b0_regs} } },
-	{"usdm", {{1, 1, usdm_int_bb_b0_regs, usdm_prty_bb_b0_regs} } },
-	{"xsdm", {{1, 1, xsdm_int_bb_b0_regs, xsdm_prty_bb_b0_regs} } },
-	{"ysdm", {{1, 1, ysdm_int_bb_b0_regs, ysdm_prty_bb_b0_regs} } },
-	{"psdm", {{1, 1, psdm_int_bb_b0_regs, psdm_prty_bb_b0_regs} } },
-	{"tsem", {{3, 3, tsem_int_bb_b0_regs, tsem_prty_bb_b0_regs} } },
-	{"msem", {{3, 2, msem_int_bb_b0_regs, msem_prty_bb_b0_regs} } },
-	{"usem", {{3, 2, usem_int_bb_b0_regs, usem_prty_bb_b0_regs} } },
-	{"xsem", {{3, 2, xsem_int_bb_b0_regs, xsem_prty_bb_b0_regs} } },
-	{"ysem", {{3, 2, ysem_int_bb_b0_regs, ysem_prty_bb_b0_regs} } },
-	{"psem", {{3, 3, psem_int_bb_b0_regs, psem_prty_bb_b0_regs} } },
-	{"rss", {{1, 1, rss_int_bb_b0_regs, rss_prty_bb_b0_regs} } },
-	{"tmld", {{1, 1, tmld_int_bb_b0_regs, tmld_prty_bb_b0_regs} } },
-	{"muld", {{1, 1, muld_int_bb_b0_regs, muld_prty_bb_b0_regs} } },
-	{"yuld", {{1, 1, yuld_int_bb_b0_regs, yuld_prty_bb_b0_regs} } },
-	{"xyld", {{1, 1, xyld_int_bb_b0_regs, xyld_prty_bb_b0_regs} } },
-	{"prm", {{1, 2, prm_int_bb_b0_regs, prm_prty_bb_b0_regs} } },
-	{"pbf_pb1", {{1, 1, pbf_pb1_int_bb_b0_regs,
-		      pbf_pb1_prty_bb_b0_regs} } },
-	{"pbf_pb2", {{1, 1, pbf_pb2_int_bb_b0_regs,
-		      pbf_pb2_prty_bb_b0_regs} } },
-	{"rpb", { {1, 1, rpb_int_bb_b0_regs, rpb_prty_bb_b0_regs} } },
-	{"btb", { {11, 2, btb_int_bb_b0_regs, btb_prty_bb_b0_regs} } },
-	{"pbf", { {1, 3, pbf_int_bb_b0_regs, pbf_prty_bb_b0_regs} } },
-	{"rdif", { {1, 1, rdif_int_bb_b0_regs, rdif_prty_bb_b0_regs} } },
-	{"tdif", { {1, 2, tdif_int_bb_b0_regs, tdif_prty_bb_b0_regs} } },
-	{"cdu", { {1, 1, cdu_int_bb_b0_regs, cdu_prty_bb_b0_regs} } },
-	{"ccfc", { {1, 2, ccfc_int_bb_b0_regs, ccfc_prty_bb_b0_regs} } },
-	{"tcfc", { {1, 2, tcfc_int_bb_b0_regs, tcfc_prty_bb_b0_regs} } },
-	{"igu", { {1, 3, igu_int_bb_b0_regs, igu_prty_bb_b0_regs} } },
-	{"cau", { {1, 1, cau_int_bb_b0_regs, cau_prty_bb_b0_regs} } },
-	{"umac", { {0, 0, NULL, NULL} } },
-	{"xmac", { {0, 0, NULL, NULL} } },
-	{"dbg", { {1, 1, dbg_int_bb_b0_regs, dbg_prty_bb_b0_regs} } },
-	{"nig", { {6, 5, nig_int_bb_b0_regs, nig_prty_bb_b0_regs} } },
-	{"wol", { {0, 0, NULL, NULL} } },
-	{"bmbn", { {0, 0, NULL, NULL} } },
-	{"ipc", { {1, 1, ipc_int_bb_b0_regs, ipc_prty_bb_b0_regs} } },
-	{"nwm", { {0, 0, NULL, NULL} } },
-	{"nws", { {0, 0, NULL, NULL} } },
-	{"ms", { {0, 0, NULL, NULL} } },
-	{"phy_pcie", { {0, 0, NULL, NULL} } },
-	{"misc_aeu", { {0, 0, NULL, NULL} } },
-	{"bar0_map", { {0, 0, NULL, NULL} } },};
-
 /* Specific HW attention callbacks */
 static int qed_mcp_attn_cb(struct qed_hwfn *p_hwfn)
 {
@@ -1590,6 +387,25 @@ static int qed_dorq_attn_cb(struct qed_hwfn *p_hwfn)
 	return -EINVAL;
 }
 
+/* Instead of major changes to the data-structure, we have a some 'special'
+ * identifiers for sources that changed meaning between adapters.
+ */
+enum aeu_invert_reg_special_type {
+	AEU_INVERT_REG_SPECIAL_CNIG_0,
+	AEU_INVERT_REG_SPECIAL_CNIG_1,
+	AEU_INVERT_REG_SPECIAL_CNIG_2,
+	AEU_INVERT_REG_SPECIAL_CNIG_3,
+	AEU_INVERT_REG_SPECIAL_MAX,
+};
+
+static struct aeu_invert_reg_bit
+aeu_descs_special[AEU_INVERT_REG_SPECIAL_MAX] = {
+	{"CNIG port 0", ATTENTION_SINGLE, NULL, BLOCK_CNIG},
+	{"CNIG port 1", ATTENTION_SINGLE, NULL, BLOCK_CNIG},
+	{"CNIG port 2", ATTENTION_SINGLE, NULL, BLOCK_CNIG},
+	{"CNIG port 3", ATTENTION_SINGLE, NULL, BLOCK_CNIG},
+};
+
 /* Notice aeu_invert_reg must be defined in the same order of bits as HW;  */
 static struct aeu_invert_reg aeu_descs[NUM_ATTN_REGS] = {
 	{
@@ -1636,8 +452,22 @@ static struct aeu_invert_reg aeu_descs[NUM_ATTN_REGS] = {
 			 (33 << ATTENTION_OFFSET_SHIFT), NULL, MAX_BLOCK_ID},
 			{"General Attention 35", ATTENTION_SINGLE,
 			 NULL, MAX_BLOCK_ID},
-			{"CNIG port %d", (4 << ATTENTION_LENGTH_SHIFT),
-			 NULL, BLOCK_CNIG},
+			{"NWS Parity",
+			 ATTENTION_PAR | ATTENTION_BB_DIFFERENT |
+			 ATTENTION_BB(AEU_INVERT_REG_SPECIAL_CNIG_0),
+			 NULL, BLOCK_NWS},
+			{"NWS Interrupt",
+			 ATTENTION_SINGLE | ATTENTION_BB_DIFFERENT |
+			 ATTENTION_BB(AEU_INVERT_REG_SPECIAL_CNIG_1),
+			 NULL, BLOCK_NWS},
+			{"NWM Parity",
+			 ATTENTION_PAR | ATTENTION_BB_DIFFERENT |
+			 ATTENTION_BB(AEU_INVERT_REG_SPECIAL_CNIG_2),
+			 NULL, BLOCK_NWM},
+			{"NWM Interrupt",
+			 ATTENTION_SINGLE | ATTENTION_BB_DIFFERENT |
+			 ATTENTION_BB(AEU_INVERT_REG_SPECIAL_CNIG_3),
+			 NULL, BLOCK_NWM},
 			{"MCP CPU", ATTENTION_SINGLE,
 			 qed_mcp_attn_cb, MAX_BLOCK_ID},
 			{"MCP Watchdog timer", ATTENTION_SINGLE,
@@ -1775,6 +605,27 @@ static struct aeu_invert_reg aeu_descs[NUM_ATTN_REGS] = {
 	},
 };
 
+static struct aeu_invert_reg_bit *
+qed_int_aeu_translate(struct qed_hwfn *p_hwfn,
+		      struct aeu_invert_reg_bit *p_bit)
+{
+	if (!QED_IS_BB(p_hwfn->cdev))
+		return p_bit;
+
+	if (!(p_bit->flags & ATTENTION_BB_DIFFERENT))
+		return p_bit;
+
+	return &aeu_descs_special[(p_bit->flags & ATTENTION_BB_MASK) >>
+				  ATTENTION_BB_SHIFT];
+}
+
+static bool qed_int_is_parity_flag(struct qed_hwfn *p_hwfn,
+				   struct aeu_invert_reg_bit *p_bit)
+{
+	return !!(qed_int_aeu_translate(p_hwfn, p_bit)->flags &
+		   ATTENTION_PARITY);
+}
+
 #define ATTN_STATE_BITS         (0xfff)
 #define ATTN_BITS_MASKABLE      (0x3ff)
 struct qed_sb_attn_info {
@@ -1863,26 +714,23 @@ static int qed_int_assertion(struct qed_hwfn *p_hwfn, u16 asserted_bits)
 	return 0;
 }
 
-static void qed_int_deassertion_print_bit(struct qed_hwfn *p_hwfn,
-					  struct attn_hw_reg *p_reg_desc,
-					  struct attn_hw_block *p_block,
-					  enum qed_attention_type type,
-					  u32 val, u32 mask)
+static void qed_int_attn_print(struct qed_hwfn *p_hwfn,
+			       enum block_id id,
+			       enum dbg_attn_type type, bool b_clear)
 {
-	int j;
+	struct dbg_attn_block_result attn_results;
+	enum dbg_status status;
 
-	for (j = 0; j < p_reg_desc->num_of_bits; j++) {
-		if (!(val & (1 << j)))
-			continue;
+	memset(&attn_results, 0, sizeof(attn_results));
 
+	status = qed_dbg_read_attn(p_hwfn, p_hwfn->p_dpc_ptt, id, type,
+				   b_clear, &attn_results);
+	if (status != DBG_STATUS_OK)
 		DP_NOTICE(p_hwfn,
-			  "%s (%s): reg %d [0x%08x], bit %d [%s]\n",
-			  p_block->name,
-			  type == QED_ATTN_TYPE_ATTN ? "Interrupt" :
-						       "Parity",
-			  p_reg_desc->reg_idx, p_reg_desc->sts_addr,
-			  j, (mask & (1 << j)) ? " [MASKED]" : "");
-	}
+			  "Failed to parse attention information [status: %s]\n",
+			  qed_dbg_get_status_str(status));
+	else
+		qed_dbg_parse_attn(p_hwfn, &attn_results);
 }
 
 /**
@@ -1901,53 +749,30 @@ static int
 qed_int_deassertion_aeu_bit(struct qed_hwfn *p_hwfn,
 			    struct aeu_invert_reg_bit *p_aeu,
 			    u32 aeu_en_reg,
-			    u32 bitmask)
+			    const char *p_bit_name, u32 bitmask)
 {
+	bool b_fatal = false;
 	int rc = -EINVAL;
 	u32 val;
 
 	DP_INFO(p_hwfn, "Deasserted attention `%s'[%08x]\n",
-		p_aeu->bit_name, bitmask);
+		p_bit_name, bitmask);
 
 	/* Call callback before clearing the interrupt status */
 	if (p_aeu->cb) {
 		DP_INFO(p_hwfn, "`%s (attention)': Calling Callback function\n",
-			p_aeu->bit_name);
+			p_bit_name);
 		rc = p_aeu->cb(p_hwfn);
 	}
 
-	/* Handle HW block interrupt registers */
-	if (p_aeu->block_index != MAX_BLOCK_ID) {
-		struct attn_hw_block *p_block;
-		u32 mask;
-		int i;
-
-		p_block = &attn_blocks[p_aeu->block_index];
-
-		/* Handle each interrupt register */
-		for (i = 0; i < p_block->chip_regs[0].num_of_int_regs; i++) {
-			struct attn_hw_reg *p_reg_desc;
-			u32 sts_addr;
+	if (rc)
+		b_fatal = true;
 
-			p_reg_desc = p_block->chip_regs[0].int_regs[i];
+	/* Print HW block interrupt registers */
+	if (p_aeu->block_index != MAX_BLOCK_ID)
+		qed_int_attn_print(p_hwfn, p_aeu->block_index,
+				   ATTN_TYPE_INTERRUPT, !b_fatal);
 
-			/* In case of fatal attention, don't clear the status
-			 * so it would appear in following idle check.
-			 */
-			if (rc == 0)
-				sts_addr = p_reg_desc->sts_clr_addr;
-			else
-				sts_addr = p_reg_desc->sts_addr;
-
-			val = qed_rd(p_hwfn, p_hwfn->p_dpc_ptt, sts_addr);
-			mask = qed_rd(p_hwfn, p_hwfn->p_dpc_ptt,
-				      p_reg_desc->mask_addr);
-			qed_int_deassertion_print_bit(p_hwfn, p_reg_desc,
-						      p_block,
-						      QED_ATTN_TYPE_ATTN,
-						      val, mask);
-		}
-	}
 
 	/* If the attention is benign, no need to prevent it */
 	if (!rc)
@@ -1957,66 +782,48 @@ qed_int_deassertion_aeu_bit(struct qed_hwfn *p_hwfn,
 	val = qed_rd(p_hwfn, p_hwfn->p_dpc_ptt, aeu_en_reg);
 	qed_wr(p_hwfn, p_hwfn->p_dpc_ptt, aeu_en_reg, (val & ~bitmask));
 	DP_INFO(p_hwfn, "`%s' - Disabled future attentions\n",
-		p_aeu->bit_name);
+		p_bit_name);
 
 out:
 	return rc;
 }
 
-static void qed_int_parity_print(struct qed_hwfn *p_hwfn,
-				 struct aeu_invert_reg_bit *p_aeu,
-				 struct attn_hw_block *p_block,
-				 u8 bit_index)
-{
-	int i;
-
-	for (i = 0; i < p_block->chip_regs[0].num_of_prty_regs; i++) {
-		struct attn_hw_reg *p_reg_desc;
-		u32 val, mask;
-
-		p_reg_desc = p_block->chip_regs[0].prty_regs[i];
-
-		val = qed_rd(p_hwfn, p_hwfn->p_dpc_ptt,
-			     p_reg_desc->sts_clr_addr);
-		mask = qed_rd(p_hwfn, p_hwfn->p_dpc_ptt,
-			      p_reg_desc->mask_addr);
-		qed_int_deassertion_print_bit(p_hwfn, p_reg_desc,
-					      p_block,
-					      QED_ATTN_TYPE_PARITY,
-					      val, mask);
-	}
-}
-
 /**
  * @brief qed_int_deassertion_parity - handle a single parity AEU source
  *
  * @param p_hwfn
  * @param p_aeu - descriptor of an AEU bit which caused the parity
+ * @param aeu_en_reg - address of the AEU enable register
  * @param bit_index
  */
 static void qed_int_deassertion_parity(struct qed_hwfn *p_hwfn,
 				       struct aeu_invert_reg_bit *p_aeu,
-				       u8 bit_index)
+				       u32 aeu_en_reg, u8 bit_index)
 {
-	u32 block_id = p_aeu->block_index;
+	u32 block_id = p_aeu->block_index, mask, val;
 
-	DP_INFO(p_hwfn->cdev, "%s[%d] parity attention is set\n",
-		p_aeu->bit_name, bit_index);
+	DP_NOTICE(p_hwfn->cdev,
+		  "%s parity attention is set [address 0x%08x, bit %d]\n",
+		  p_aeu->bit_name, aeu_en_reg, bit_index);
 
 	if (block_id != MAX_BLOCK_ID) {
-		qed_int_parity_print(p_hwfn, p_aeu, &attn_blocks[block_id],
-				     bit_index);
+		qed_int_attn_print(p_hwfn, block_id, ATTN_TYPE_PARITY, false);
 
 		/* In BB, there's a single parity bit for several blocks */
 		if (block_id == BLOCK_BTB) {
-			qed_int_parity_print(p_hwfn, p_aeu,
-					     &attn_blocks[BLOCK_OPTE],
-					     bit_index);
-			qed_int_parity_print(p_hwfn, p_aeu,
-					     &attn_blocks[BLOCK_MCP],
-					     bit_index);
+			qed_int_attn_print(p_hwfn, BLOCK_OPTE,
+					   ATTN_TYPE_PARITY, false);
+			qed_int_attn_print(p_hwfn, BLOCK_MCP,
+					   ATTN_TYPE_PARITY, false);
 		}
 	}
+
+	/* Prevent this parity error from being re-asserted */
+	mask = ~BIT(bit_index);
+	val = qed_rd(p_hwfn, p_hwfn->p_dpc_ptt, aeu_en_reg);
+	qed_wr(p_hwfn, p_hwfn->p_dpc_ptt, aeu_en_reg, val & mask);
+	DP_INFO(p_hwfn, "`%s' - Disabled future parity errors\n",
+		p_aeu->bit_name);
 }
 
 /**
@@ -2031,7 +838,7 @@ static int qed_int_deassertion(struct qed_hwfn  *p_hwfn,
 			       u16 deasserted_bits)
 {
 	struct qed_sb_attn_info *sb_attn_sw = p_hwfn->p_sb_attn;
-	u32 aeu_inv_arr[NUM_ATTN_REGS], aeu_mask;
+	u32 aeu_inv_arr[NUM_ATTN_REGS], aeu_mask, aeu_en, en;
 	u8 i, j, k, bit_idx;
 	int rc = 0;
 
@@ -2048,11 +855,11 @@ static int qed_int_deassertion(struct qed_hwfn  *p_hwfn,
 	/* Find parity attentions first */
 	for (i = 0; i < NUM_ATTN_REGS; i++) {
 		struct aeu_invert_reg *p_aeu = &sb_attn_sw->p_aeu_desc[i];
-		u32 en = qed_rd(p_hwfn, p_hwfn->p_dpc_ptt,
-				MISC_REG_AEU_ENABLE1_IGU_OUT_0 +
-				i * sizeof(u32));
 		u32 parities;
 
+		aeu_en = MISC_REG_AEU_ENABLE1_IGU_OUT_0 + i * sizeof(u32);
+		en = qed_rd(p_hwfn, p_hwfn->p_dpc_ptt, aeu_en);
+
 		/* Skip register in which no parity bit is currently set */
 		parities = sb_attn_sw->parity_mask[i] & aeu_inv_arr[i] & en;
 		if (!parities)
@@ -2061,10 +868,10 @@ static int qed_int_deassertion(struct qed_hwfn  *p_hwfn,
 		for (j = 0, bit_idx = 0; bit_idx < 32; j++) {
 			struct aeu_invert_reg_bit *p_bit = &p_aeu->bits[j];
 
-			if ((p_bit->flags & ATTENTION_PARITY) &&
+			if (qed_int_is_parity_flag(p_hwfn, p_bit) &&
 			    !!(parities & BIT(bit_idx)))
 				qed_int_deassertion_parity(p_hwfn, p_bit,
-							   bit_idx);
+							   aeu_en, bit_idx);
 
 			bit_idx += ATTENTION_LENGTH(p_bit->flags);
 		}
@@ -2079,10 +886,11 @@ static int qed_int_deassertion(struct qed_hwfn  *p_hwfn,
 			continue;
 
 		for (i = 0; i < NUM_ATTN_REGS; i++) {
-			u32 aeu_en = MISC_REG_AEU_ENABLE1_IGU_OUT_0 +
-				     i * sizeof(u32) +
-				     k * sizeof(u32) * NUM_ATTN_REGS;
-			u32 en, bits;
+			u32 bits;
+
+			aeu_en = MISC_REG_AEU_ENABLE1_IGU_OUT_0 +
+				 i * sizeof(u32) +
+				 k * sizeof(u32) * NUM_ATTN_REGS;
 
 			en = qed_rd(p_hwfn, p_hwfn->p_dpc_ptt, aeu_en);
 			bits = aeu_inv_arr[i] & en;
@@ -2096,29 +904,54 @@ static int qed_int_deassertion(struct qed_hwfn  *p_hwfn,
 			 * previous assertion.
 			 */
 			for (j = 0, bit_idx = 0; bit_idx < 32; j++) {
+				long unsigned int bitmask;
 				u8 bit, bit_len;
-				u32 bitmask;
 
 				p_aeu = &sb_attn_sw->p_aeu_desc[i].bits[j];
-
-				/* No need to handle parity-only bits */
-				if (p_aeu->flags == ATTENTION_PAR)
-					continue;
+				p_aeu = qed_int_aeu_translate(p_hwfn, p_aeu);
 
 				bit = bit_idx;
 				bit_len = ATTENTION_LENGTH(p_aeu->flags);
-				if (p_aeu->flags & ATTENTION_PAR_INT) {
+				if (qed_int_is_parity_flag(p_hwfn, p_aeu)) {
 					/* Skip Parity */
 					bit++;
 					bit_len--;
 				}
 
 				bitmask = bits & (((1 << bit_len) - 1) << bit);
+				bitmask >>= bit;
+
 				if (bitmask) {
+					u32 flags = p_aeu->flags;
+					char bit_name[30];
+					u8 num;
+
+					num = (u8)find_first_bit(&bitmask,
+								 bit_len);
+
+					/* Some bits represent more than a
+					 * a single interrupt. Correctly print
+					 * their name.
+					 */
+					if (ATTENTION_LENGTH(flags) > 2 ||
+					    ((flags & ATTENTION_PAR_INT) &&
+					     ATTENTION_LENGTH(flags) > 1))
+						snprintf(bit_name, 30,
+							 p_aeu->bit_name, num);
+					else
+						strncpy(bit_name,
+							p_aeu->bit_name, 30);
+
+					/* We now need to pass bitmask in its
+					 * correct position.
+					 */
+					bitmask <<= bit;
+
 					/* Handle source of the attention */
 					qed_int_deassertion_aeu_bit(p_hwfn,
 								    p_aeu,
 								    aeu_en,
+								    bit_name,
 								    bitmask);
 				}
 
@@ -2328,6 +1161,7 @@ static void qed_int_sb_attn_free(struct qed_hwfn *p_hwfn)
 				  SB_ATTN_ALIGNED_SIZE(p_hwfn),
 				  p_sb->sb_attn, p_sb->sb_phys);
 	kfree(p_sb);
+	p_hwfn->p_sb_attn = NULL;
 }
 
 static void qed_int_sb_attn_setup(struct qed_hwfn *p_hwfn,
@@ -2365,12 +1199,13 @@ static void qed_int_sb_attn_init(struct qed_hwfn *p_hwfn,
 	for (i = 0; i < NUM_ATTN_REGS; i++) {
 		/* j is array index, k is bit index */
 		for (j = 0, k = 0; k < 32; j++) {
-			unsigned int flags = aeu_descs[i].bits[j].flags;
+			struct aeu_invert_reg_bit *p_aeu;
 
-			if (flags & ATTENTION_PARITY)
+			p_aeu = &aeu_descs[i].bits[j];
+			if (qed_int_is_parity_flag(p_hwfn, p_aeu))
 				sb_info->parity_mask[i] |= 1 << k;
 
-			k += ATTENTION_LENGTH(flags);
+			k += ATTENTION_LENGTH(p_aeu->flags);
 		}
 		DP_VERBOSE(p_hwfn, NETIF_MSG_INTR,
 			   "Attn Mask [Reg %d]: 0x%08x\n",
@@ -2465,6 +1300,40 @@ void qed_init_cau_sb_entry(struct qed_hwfn *p_hwfn,
 	SET_FIELD(p_sb_entry->data, CAU_SB_ENTRY_STATE1, cau_state);
 }
 
+static void qed_int_cau_conf_pi(struct qed_hwfn *p_hwfn,
+				struct qed_ptt *p_ptt,
+				u16 igu_sb_id,
+				u32 pi_index,
+				enum qed_coalescing_fsm coalescing_fsm,
+				u8 timeset)
+{
+	struct cau_pi_entry pi_entry;
+	u32 sb_offset, pi_offset;
+
+	if (IS_VF(p_hwfn->cdev))
+		return;
+
+	sb_offset = igu_sb_id * PIS_PER_SB;
+	memset(&pi_entry, 0, sizeof(struct cau_pi_entry));
+
+	SET_FIELD(pi_entry.prod, CAU_PI_ENTRY_PI_TIMESET, timeset);
+	if (coalescing_fsm == QED_COAL_RX_STATE_MACHINE)
+		SET_FIELD(pi_entry.prod, CAU_PI_ENTRY_FSM_SEL, 0);
+	else
+		SET_FIELD(pi_entry.prod, CAU_PI_ENTRY_FSM_SEL, 1);
+
+	pi_offset = sb_offset + pi_index;
+	if (p_hwfn->hw_init_done) {
+		qed_wr(p_hwfn, p_ptt,
+		       CAU_REG_PI_MEMORY + pi_offset * sizeof(u32),
+		       *((u32 *)&(pi_entry)));
+	} else {
+		STORE_RT_REG(p_hwfn,
+			     CAU_REG_PI_MEMORY_RT_OFFSET + pi_offset,
+			     *((u32 *)&(pi_entry)));
+	}
+}
+
 void qed_int_cau_conf_sb(struct qed_hwfn *p_hwfn,
 			 struct qed_ptt *p_ptt,
 			 dma_addr_t sb_phys,
@@ -2531,40 +1400,6 @@ void qed_int_cau_conf_sb(struct qed_hwfn *p_hwfn,
 	}
 }
 
-void qed_int_cau_conf_pi(struct qed_hwfn *p_hwfn,
-			 struct qed_ptt *p_ptt,
-			 u16 igu_sb_id,
-			 u32 pi_index,
-			 enum qed_coalescing_fsm coalescing_fsm,
-			 u8 timeset)
-{
-	struct cau_pi_entry pi_entry;
-	u32 sb_offset, pi_offset;
-
-	if (IS_VF(p_hwfn->cdev))
-		return;
-
-	sb_offset = igu_sb_id * PIS_PER_SB;
-	memset(&pi_entry, 0, sizeof(struct cau_pi_entry));
-
-	SET_FIELD(pi_entry.prod, CAU_PI_ENTRY_PI_TIMESET, timeset);
-	if (coalescing_fsm == QED_COAL_RX_STATE_MACHINE)
-		SET_FIELD(pi_entry.prod, CAU_PI_ENTRY_FSM_SEL, 0);
-	else
-		SET_FIELD(pi_entry.prod, CAU_PI_ENTRY_FSM_SEL, 1);
-
-	pi_offset = sb_offset + pi_index;
-	if (p_hwfn->hw_init_done) {
-		qed_wr(p_hwfn, p_ptt,
-		       CAU_REG_PI_MEMORY + pi_offset * sizeof(u32),
-		       *((u32 *)&(pi_entry)));
-	} else {
-		STORE_RT_REG(p_hwfn,
-			     CAU_REG_PI_MEMORY_RT_OFFSET + pi_offset,
-			     *((u32 *)&(pi_entry)));
-	}
-}
-
 void qed_int_sb_setup(struct qed_hwfn *p_hwfn,
 		      struct qed_ptt *p_ptt, struct qed_sb_info *sb_info)
 {
@@ -2577,16 +1412,47 @@ void qed_int_sb_setup(struct qed_hwfn *p_hwfn,
 				    sb_info->igu_sb_id, 0, 0);
 }
 
-/**
- * @brief qed_get_igu_sb_id - given a sw sb_id return the
- *        igu_sb_id
- *
- * @param p_hwfn
- * @param sb_id
- *
- * @return u16
- */
-static u16 qed_get_igu_sb_id(struct qed_hwfn *p_hwfn, u16 sb_id)
+struct qed_igu_block *qed_get_igu_free_sb(struct qed_hwfn *p_hwfn, bool b_is_pf)
+{
+	struct qed_igu_block *p_block;
+	u16 igu_id;
+
+	for (igu_id = 0; igu_id < QED_MAPPING_MEMORY_SIZE(p_hwfn->cdev);
+	     igu_id++) {
+		p_block = &p_hwfn->hw_info.p_igu_info->entry[igu_id];
+
+		if (!(p_block->status & QED_IGU_STATUS_VALID) ||
+		    !(p_block->status & QED_IGU_STATUS_FREE))
+			continue;
+
+		if (!!(p_block->status & QED_IGU_STATUS_PF) == b_is_pf)
+			return p_block;
+	}
+
+	return NULL;
+}
+
+static u16 qed_get_pf_igu_sb_id(struct qed_hwfn *p_hwfn, u16 vector_id)
+{
+	struct qed_igu_block *p_block;
+	u16 igu_id;
+
+	for (igu_id = 0; igu_id < QED_MAPPING_MEMORY_SIZE(p_hwfn->cdev);
+	     igu_id++) {
+		p_block = &p_hwfn->hw_info.p_igu_info->entry[igu_id];
+
+		if (!(p_block->status & QED_IGU_STATUS_VALID) ||
+		    !p_block->is_pf ||
+		    p_block->vector_number != vector_id)
+			continue;
+
+		return igu_id;
+	}
+
+	return QED_SB_INVALID_IDX;
+}
+
+u16 qed_get_igu_sb_id(struct qed_hwfn *p_hwfn, u16 sb_id)
 {
 	u16 igu_sb_id;
 
@@ -2594,7 +1460,7 @@ static u16 qed_get_igu_sb_id(struct qed_hwfn *p_hwfn, u16 sb_id)
 	if (sb_id == QED_SP_SB_ID)
 		igu_sb_id = p_hwfn->hw_info.p_igu_info->igu_dsb_id;
 	else if (IS_PF(p_hwfn->cdev))
-		igu_sb_id = sb_id + p_hwfn->hw_info.p_igu_info->igu_base_sb;
+		igu_sb_id = qed_get_pf_igu_sb_id(p_hwfn, sb_id + 1);
 	else
 		igu_sb_id = qed_vf_get_igu_sb_id(p_hwfn, sb_id);
 
@@ -2619,8 +1485,19 @@ int qed_int_sb_init(struct qed_hwfn *p_hwfn,
 	sb_info->igu_sb_id = qed_get_igu_sb_id(p_hwfn, sb_id);
 
 	if (sb_id != QED_SP_SB_ID) {
-		p_hwfn->sbs_info[sb_id] = sb_info;
-		p_hwfn->num_sbs++;
+		if (IS_PF(p_hwfn->cdev)) {
+			struct qed_igu_info *p_info;
+			struct qed_igu_block *p_block;
+
+			p_info = p_hwfn->hw_info.p_igu_info;
+			p_block = &p_info->entry[sb_info->igu_sb_id];
+
+			p_block->sb_info = sb_info;
+			p_block->status &= ~QED_IGU_STATUS_FREE;
+			p_info->usage.free_cnt--;
+		} else {
+			qed_vf_set_sb_info(p_hwfn, sb_id, sb_info);
+		}
 	}
 
 	sb_info->cdev = p_hwfn->cdev;
@@ -2649,20 +1526,35 @@ int qed_int_sb_init(struct qed_hwfn *p_hwfn,
 int qed_int_sb_release(struct qed_hwfn *p_hwfn,
 		       struct qed_sb_info *sb_info, u16 sb_id)
 {
-	if (sb_id == QED_SP_SB_ID) {
-		DP_ERR(p_hwfn, "Do Not free sp sb using this function");
-		return -EINVAL;
-	}
+	struct qed_igu_block *p_block;
+	struct qed_igu_info *p_info;
+
+	if (!sb_info)
+		return 0;
 
 	/* zero status block and ack counter */
 	sb_info->sb_ack = 0;
 	memset(sb_info->sb_virt, 0, sizeof(*sb_info->sb_virt));
 
-	if (p_hwfn->sbs_info[sb_id] != NULL) {
-		p_hwfn->sbs_info[sb_id] = NULL;
-		p_hwfn->num_sbs--;
+	if (IS_VF(p_hwfn->cdev)) {
+		qed_vf_set_sb_info(p_hwfn, sb_id, NULL);
+		return 0;
 	}
 
+	p_info = p_hwfn->hw_info.p_igu_info;
+	p_block = &p_info->entry[sb_info->igu_sb_id];
+
+	/* Vector 0 is reserved to Default SB */
+	if (!p_block->vector_number) {
+		DP_ERR(p_hwfn, "Do Not free sp sb using this function");
+		return -EINVAL;
+	}
+
+	/* Lose reference to client's SB info, and fix counters */
+	p_block->sb_info = NULL;
+	p_block->status |= QED_IGU_STATUS_FREE;
+	p_info->usage.free_cnt++;
+
 	return 0;
 }
 
@@ -2679,6 +1571,7 @@ static void qed_int_sp_sb_free(struct qed_hwfn *p_hwfn)
 				  p_sb->sb_info.sb_virt,
 				  p_sb->sb_info.sb_phys);
 	kfree(p_sb);
+	p_hwfn->p_sp_sb = NULL;
 }
 
 static int qed_int_sp_sb_alloc(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
@@ -2780,10 +1673,9 @@ void qed_int_igu_enable_int(struct qed_hwfn *p_hwfn,
 	qed_wr(p_hwfn, p_ptt, IGU_REG_PF_CONFIGURATION, igu_pf_conf);
 }
 
-int qed_int_igu_enable(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
-		       enum qed_int_mode int_mode)
+static void qed_int_igu_enable_attn(struct qed_hwfn *p_hwfn,
+				    struct qed_ptt *p_ptt)
 {
-	int rc = 0;
 
 	/* Configure AEU signal change to produce attentions */
 	qed_wr(p_hwfn, p_ptt, IGU_REG_ATTENTION_ENABLE, 0);
@@ -2796,6 +1688,16 @@ int qed_int_igu_enable(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 
 	/* Unmask AEU signals toward IGU */
 	qed_wr(p_hwfn, p_ptt, MISC_REG_AEU_MASK_ATTN_IGU, 0xff);
+}
+
+int
+qed_int_igu_enable(struct qed_hwfn *p_hwfn,
+		   struct qed_ptt *p_ptt, enum qed_int_mode int_mode)
+{
+	int rc = 0;
+
+	qed_int_igu_enable_attn(p_hwfn, p_ptt);
+
 	if ((int_mode != QED_INT_MODE_INTA) || IS_LEAD_HWFN(p_hwfn)) {
 		rc = qed_slowpath_irq_req(p_hwfn);
 		if (rc) {
@@ -2824,10 +1726,11 @@ void qed_int_igu_disable_int(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 #define IGU_CLEANUP_SLEEP_LENGTH                (1000)
 static void qed_int_igu_cleanup_sb(struct qed_hwfn *p_hwfn,
 				   struct qed_ptt *p_ptt,
-				   u32 sb_id, bool cleanup_set, u16 opaque_fid)
+				   u16 igu_sb_id,
+				   bool cleanup_set, u16 opaque_fid)
 {
 	u32 cmd_ctrl = 0, val = 0, sb_bit = 0, sb_bit_addr = 0, data = 0;
-	u32 pxp_addr = IGU_CMD_INT_ACK_BASE + sb_id;
+	u32 pxp_addr = IGU_CMD_INT_ACK_BASE + igu_sb_id;
 	u32 sleep_cnt = IGU_CLEANUP_SLEEP_LENGTH;
 
 	/* Set the data field */
@@ -2850,8 +1753,8 @@ static void qed_int_igu_cleanup_sb(struct qed_hwfn *p_hwfn,
 	mmiowb();
 
 	/* calculate where to read the status bit from */
-	sb_bit = 1 << (sb_id % 32);
-	sb_bit_addr = sb_id / 32 * sizeof(u32);
+	sb_bit = 1 << (igu_sb_id % 32);
+	sb_bit_addr = igu_sb_id / 32 * sizeof(u32);
 
 	sb_bit_addr += IGU_REG_CLEANUP_STATUS_0;
 
@@ -2868,29 +1771,38 @@ static void qed_int_igu_cleanup_sb(struct qed_hwfn *p_hwfn,
 	if (!sleep_cnt)
 		DP_NOTICE(p_hwfn,
 			  "Timeout waiting for clear status 0x%08x [for sb %d]\n",
-			  val, sb_id);
+			  val, igu_sb_id);
 }
 
 void qed_int_igu_init_pure_rt_single(struct qed_hwfn *p_hwfn,
 				     struct qed_ptt *p_ptt,
-				     u32 sb_id, u16 opaque, bool b_set)
+				     u16 igu_sb_id, u16 opaque, bool b_set)
 {
+	struct qed_igu_block *p_block;
 	int pi, i;
 
+	p_block = &p_hwfn->hw_info.p_igu_info->entry[igu_sb_id];
+	DP_VERBOSE(p_hwfn, NETIF_MSG_INTR,
+		   "Cleaning SB [%04x]: func_id= %d is_pf = %d vector_num = 0x%0x\n",
+		   igu_sb_id,
+		   p_block->function_id,
+		   p_block->is_pf, p_block->vector_number);
+
 	/* Set */
 	if (b_set)
-		qed_int_igu_cleanup_sb(p_hwfn, p_ptt, sb_id, 1, opaque);
+		qed_int_igu_cleanup_sb(p_hwfn, p_ptt, igu_sb_id, 1, opaque);
 
 	/* Clear */
-	qed_int_igu_cleanup_sb(p_hwfn, p_ptt, sb_id, 0, opaque);
+	qed_int_igu_cleanup_sb(p_hwfn, p_ptt, igu_sb_id, 0, opaque);
 
 	/* Wait for the IGU SB to cleanup */
 	for (i = 0; i < IGU_CLEANUP_SLEEP_LENGTH; i++) {
 		u32 val;
 
 		val = qed_rd(p_hwfn, p_ptt,
-			     IGU_REG_WRITE_DONE_PENDING + ((sb_id / 32) * 4));
-		if (val & (1 << (sb_id % 32)))
+			     IGU_REG_WRITE_DONE_PENDING +
+			     ((igu_sb_id / 32) * 4));
+		if (val & BIT((igu_sb_id % 32)))
 			usleep_range(10, 20);
 		else
 			break;
@@ -2898,84 +1810,205 @@ void qed_int_igu_init_pure_rt_single(struct qed_hwfn *p_hwfn,
 	if (i == IGU_CLEANUP_SLEEP_LENGTH)
 		DP_NOTICE(p_hwfn,
 			  "Failed SB[0x%08x] still appearing in WRITE_DONE_PENDING\n",
-			  sb_id);
+			  igu_sb_id);
 
 	/* Clear the CAU for the SB */
 	for (pi = 0; pi < 12; pi++)
 		qed_wr(p_hwfn, p_ptt,
-		       CAU_REG_PI_MEMORY + (sb_id * 12 + pi) * 4, 0);
+		       CAU_REG_PI_MEMORY + (igu_sb_id * 12 + pi) * 4, 0);
 }
 
 void qed_int_igu_init_pure_rt(struct qed_hwfn *p_hwfn,
 			      struct qed_ptt *p_ptt,
 			      bool b_set, bool b_slowpath)
 {
-	u32 igu_base_sb = p_hwfn->hw_info.p_igu_info->igu_base_sb;
-	u32 igu_sb_cnt = p_hwfn->hw_info.p_igu_info->igu_sb_cnt;
-	u32 sb_id = 0, val = 0;
+	struct qed_igu_info *p_info = p_hwfn->hw_info.p_igu_info;
+	struct qed_igu_block *p_block;
+	u16 igu_sb_id = 0;
+	u32 val = 0;
 
 	val = qed_rd(p_hwfn, p_ptt, IGU_REG_BLOCK_CONFIGURATION);
 	val |= IGU_REG_BLOCK_CONFIGURATION_VF_CLEANUP_EN;
 	val &= ~IGU_REG_BLOCK_CONFIGURATION_PXP_TPH_INTERFACE_EN;
 	qed_wr(p_hwfn, p_ptt, IGU_REG_BLOCK_CONFIGURATION, val);
 
-	DP_VERBOSE(p_hwfn, NETIF_MSG_INTR,
-		   "IGU cleaning SBs [%d,...,%d]\n",
-		   igu_base_sb, igu_base_sb + igu_sb_cnt - 1);
+	for (igu_sb_id = 0;
+	     igu_sb_id < QED_MAPPING_MEMORY_SIZE(p_hwfn->cdev); igu_sb_id++) {
+		p_block = &p_info->entry[igu_sb_id];
 
-	for (sb_id = igu_base_sb; sb_id < igu_base_sb + igu_sb_cnt; sb_id++)
-		qed_int_igu_init_pure_rt_single(p_hwfn, p_ptt, sb_id,
+		if (!(p_block->status & QED_IGU_STATUS_VALID) ||
+		    !p_block->is_pf ||
+		    (p_block->status & QED_IGU_STATUS_DSB))
+			continue;
+
+		qed_int_igu_init_pure_rt_single(p_hwfn, p_ptt, igu_sb_id,
 						p_hwfn->hw_info.opaque_fid,
 						b_set);
+	}
 
-	if (!b_slowpath)
-		return;
-
-	sb_id = p_hwfn->hw_info.p_igu_info->igu_dsb_id;
-	DP_VERBOSE(p_hwfn, NETIF_MSG_INTR,
-		   "IGU cleaning slowpath SB [%d]\n", sb_id);
-	qed_int_igu_init_pure_rt_single(p_hwfn, p_ptt, sb_id,
-					p_hwfn->hw_info.opaque_fid, b_set);
+	if (b_slowpath)
+		qed_int_igu_init_pure_rt_single(p_hwfn, p_ptt,
+						p_info->igu_dsb_id,
+						p_hwfn->hw_info.opaque_fid,
+						b_set);
 }
 
-static u32 qed_int_igu_read_cam_block(struct qed_hwfn *p_hwfn,
-				      struct qed_ptt *p_ptt, u16 sb_id)
+int qed_int_igu_reset_cam(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
-	u32 val = qed_rd(p_hwfn, p_ptt,
-			 IGU_REG_MAPPING_MEMORY + sizeof(u32) * sb_id);
+	struct qed_igu_info *p_info = p_hwfn->hw_info.p_igu_info;
 	struct qed_igu_block *p_block;
+	int pf_sbs, vf_sbs;
+	u16 igu_sb_id;
+	u32 val, rval;
 
-	p_block = &p_hwfn->hw_info.p_igu_info->igu_map.igu_blocks[sb_id];
+	if (!RESC_NUM(p_hwfn, QED_SB)) {
+		p_info->b_allow_pf_vf_change = false;
+	} else {
+		/* Use the numbers the MFW have provided -
+		 * don't forget MFW accounts for the default SB as well.
+		 */
+		p_info->b_allow_pf_vf_change = true;
+
+		if (p_info->usage.cnt != RESC_NUM(p_hwfn, QED_SB) - 1) {
+			DP_INFO(p_hwfn,
+				"MFW notifies of 0x%04x PF SBs; IGU indicates of only 0x%04x\n",
+				RESC_NUM(p_hwfn, QED_SB) - 1,
+				p_info->usage.cnt);
+			p_info->usage.cnt = RESC_NUM(p_hwfn, QED_SB) - 1;
+		}
 
-	/* stop scanning when hit first invalid PF entry */
-	if (!GET_FIELD(val, IGU_MAPPING_LINE_VALID) &&
-	    GET_FIELD(val, IGU_MAPPING_LINE_PF_VALID))
-		goto out;
+		if (IS_PF_SRIOV(p_hwfn)) {
+			u16 vfs = p_hwfn->cdev->p_iov_info->total_vfs;
 
-	/* Fill the block information */
-	p_block->status		= QED_IGU_STATUS_VALID;
-	p_block->function_id	= GET_FIELD(val,
-					    IGU_MAPPING_LINE_FUNCTION_NUMBER);
-	p_block->is_pf		= GET_FIELD(val, IGU_MAPPING_LINE_PF_VALID);
-	p_block->vector_number	= GET_FIELD(val,
-					    IGU_MAPPING_LINE_VECTOR_NUMBER);
+			if (vfs != p_info->usage.iov_cnt)
+				DP_VERBOSE(p_hwfn,
+					   NETIF_MSG_INTR,
+					   "0x%04x VF SBs in IGU CAM != PCI configuration 0x%04x\n",
+					   p_info->usage.iov_cnt, vfs);
 
-	DP_VERBOSE(p_hwfn, NETIF_MSG_INTR,
-		   "IGU_BLOCK: [SB 0x%04x, Value in CAM 0x%08x] func_id = %d is_pf = %d vector_num = 0x%x\n",
-		   sb_id, val, p_block->function_id,
-		   p_block->is_pf, p_block->vector_number);
+			/* At this point we know how many SBs we have totally
+			 * in IGU + number of PF SBs. So we can validate that
+			 * we'd have sufficient for VF.
+			 */
+			if (vfs > p_info->usage.free_cnt +
+			    p_info->usage.free_cnt_iov - p_info->usage.cnt) {
+				DP_NOTICE(p_hwfn,
+					  "Not enough SBs for VFs - 0x%04x SBs, from which %04x PFs and %04x are required\n",
+					  p_info->usage.free_cnt +
+					  p_info->usage.free_cnt_iov,
+					  p_info->usage.cnt, vfs);
+				return -EINVAL;
+			}
 
-out:
-	return val;
+			/* Currently cap the number of VFs SBs by the
+			 * number of VFs.
+			 */
+			p_info->usage.iov_cnt = vfs;
+		}
+	}
+
+	/* Mark all SBs as free, now in the right PF/VFs division */
+	p_info->usage.free_cnt = p_info->usage.cnt;
+	p_info->usage.free_cnt_iov = p_info->usage.iov_cnt;
+	p_info->usage.orig = p_info->usage.cnt;
+	p_info->usage.iov_orig = p_info->usage.iov_cnt;
+
+	/* We now proceed to re-configure the IGU cam to reflect the initial
+	 * configuration. We can start with the Default SB.
+	 */
+	pf_sbs = p_info->usage.cnt;
+	vf_sbs = p_info->usage.iov_cnt;
+
+	for (igu_sb_id = p_info->igu_dsb_id;
+	     igu_sb_id < QED_MAPPING_MEMORY_SIZE(p_hwfn->cdev); igu_sb_id++) {
+		p_block = &p_info->entry[igu_sb_id];
+		val = 0;
+
+		if (!(p_block->status & QED_IGU_STATUS_VALID))
+			continue;
+
+		if (p_block->status & QED_IGU_STATUS_DSB) {
+			p_block->function_id = p_hwfn->rel_pf_id;
+			p_block->is_pf = 1;
+			p_block->vector_number = 0;
+			p_block->status = QED_IGU_STATUS_VALID |
+					  QED_IGU_STATUS_PF |
+					  QED_IGU_STATUS_DSB;
+		} else if (pf_sbs) {
+			pf_sbs--;
+			p_block->function_id = p_hwfn->rel_pf_id;
+			p_block->is_pf = 1;
+			p_block->vector_number = p_info->usage.cnt - pf_sbs;
+			p_block->status = QED_IGU_STATUS_VALID |
+					  QED_IGU_STATUS_PF |
+					  QED_IGU_STATUS_FREE;
+		} else if (vf_sbs) {
+			p_block->function_id =
+			    p_hwfn->cdev->p_iov_info->first_vf_in_pf +
+			    p_info->usage.iov_cnt - vf_sbs;
+			p_block->is_pf = 0;
+			p_block->vector_number = 0;
+			p_block->status = QED_IGU_STATUS_VALID |
+					  QED_IGU_STATUS_FREE;
+			vf_sbs--;
+		} else {
+			p_block->function_id = 0;
+			p_block->is_pf = 0;
+			p_block->vector_number = 0;
+		}
+
+		SET_FIELD(val, IGU_MAPPING_LINE_FUNCTION_NUMBER,
+			  p_block->function_id);
+		SET_FIELD(val, IGU_MAPPING_LINE_PF_VALID, p_block->is_pf);
+		SET_FIELD(val, IGU_MAPPING_LINE_VECTOR_NUMBER,
+			  p_block->vector_number);
+
+		/* VF entries would be enabled when VF is initializaed */
+		SET_FIELD(val, IGU_MAPPING_LINE_VALID, p_block->is_pf);
+
+		rval = qed_rd(p_hwfn, p_ptt,
+			      IGU_REG_MAPPING_MEMORY + sizeof(u32) * igu_sb_id);
+
+		if (rval != val) {
+			qed_wr(p_hwfn, p_ptt,
+			       IGU_REG_MAPPING_MEMORY +
+			       sizeof(u32) * igu_sb_id, val);
+
+			DP_VERBOSE(p_hwfn,
+				   NETIF_MSG_INTR,
+				   "IGU reset: [SB 0x%04x] func_id = %d is_pf = %d vector_num = 0x%x [%08x -> %08x]\n",
+				   igu_sb_id,
+				   p_block->function_id,
+				   p_block->is_pf,
+				   p_block->vector_number, rval, val);
+		}
+	}
+
+	return 0;
+}
+
+static void qed_int_igu_read_cam_block(struct qed_hwfn *p_hwfn,
+				       struct qed_ptt *p_ptt, u16 igu_sb_id)
+{
+	u32 val = qed_rd(p_hwfn, p_ptt,
+			 IGU_REG_MAPPING_MEMORY + sizeof(u32) * igu_sb_id);
+	struct qed_igu_block *p_block;
+
+	p_block = &p_hwfn->hw_info.p_igu_info->entry[igu_sb_id];
+
+	/* Fill the block information */
+	p_block->function_id = GET_FIELD(val, IGU_MAPPING_LINE_FUNCTION_NUMBER);
+	p_block->is_pf = GET_FIELD(val, IGU_MAPPING_LINE_PF_VALID);
+	p_block->vector_number = GET_FIELD(val, IGU_MAPPING_LINE_VECTOR_NUMBER);
+	p_block->igu_sb_id = igu_sb_id;
 }
 
 int qed_int_igu_read_cam(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
 	struct qed_igu_info *p_igu_info;
-	u32 val, min_vf = 0, max_vf = 0;
-	u16 sb_id, last_iov_sb_id = 0;
-	struct qed_igu_block *blk;
-	u16 prev_sb_id = 0xFF;
+	struct qed_igu_block *p_block;
+	u32 min_vf = 0, max_vf = 0;
+	u16 igu_sb_id;
 
 	p_hwfn->hw_info.p_igu_info = kzalloc(sizeof(*p_igu_info), GFP_KERNEL);
 	if (!p_hwfn->hw_info.p_igu_info)
@@ -2983,12 +2016,10 @@ int qed_int_igu_read_cam(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 
 	p_igu_info = p_hwfn->hw_info.p_igu_info;
 
-	/* Initialize base sb / sb cnt for PFs and VFs */
-	p_igu_info->igu_base_sb		= 0xffff;
-	p_igu_info->igu_sb_cnt		= 0;
-	p_igu_info->igu_dsb_id		= 0xffff;
-	p_igu_info->igu_base_sb_iov	= 0xffff;
+	/* Distinguish between existent and non-existent default SB */
+	p_igu_info->igu_dsb_id = QED_SB_INVALID_IDX;
 
+	/* Find the range of VF ids whose SB belong to this PF */
 	if (p_hwfn->cdev->p_iov_info) {
 		struct qed_hw_sriov_info *p_iov = p_hwfn->cdev->p_iov_info;
 
@@ -2996,113 +2027,69 @@ int qed_int_igu_read_cam(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 		max_vf	= p_iov->first_vf_in_pf + p_iov->total_vfs;
 	}
 
-	for (sb_id = 0; sb_id < QED_MAPPING_MEMORY_SIZE(p_hwfn->cdev);
-	     sb_id++) {
-		blk = &p_igu_info->igu_map.igu_blocks[sb_id];
-
-		val	= qed_int_igu_read_cam_block(p_hwfn, p_ptt, sb_id);
-
-		/* stop scanning when hit first invalid PF entry */
-		if (!GET_FIELD(val, IGU_MAPPING_LINE_VALID) &&
-		    GET_FIELD(val, IGU_MAPPING_LINE_PF_VALID))
-			break;
-
-		if (blk->is_pf) {
-			if (blk->function_id == p_hwfn->rel_pf_id) {
-				blk->status |= QED_IGU_STATUS_PF;
-
-				if (blk->vector_number == 0) {
-					if (p_igu_info->igu_dsb_id == 0xffff)
-						p_igu_info->igu_dsb_id = sb_id;
-				} else {
-					if (p_igu_info->igu_base_sb ==
-					    0xffff) {
-						p_igu_info->igu_base_sb = sb_id;
-					} else if (prev_sb_id != sb_id - 1) {
-						DP_NOTICE(p_hwfn->cdev,
-							  "consecutive igu vectors for HWFN %x broken",
-							  p_hwfn->rel_pf_id);
-						break;
-					}
-					prev_sb_id = sb_id;
-					/* we don't count the default */
-					(p_igu_info->igu_sb_cnt)++;
-				}
-			}
-		} else {
-			if ((blk->function_id >= min_vf) &&
-			    (blk->function_id < max_vf)) {
-				/* Available for VFs of this PF */
-				if (p_igu_info->igu_base_sb_iov == 0xffff) {
-					p_igu_info->igu_base_sb_iov = sb_id;
-				} else if (last_iov_sb_id != sb_id - 1) {
-					if (!val) {
-						DP_VERBOSE(p_hwfn->cdev,
-							   NETIF_MSG_INTR,
-							   "First uninitialized IGU CAM entry at index 0x%04x\n",
-							   sb_id);
-					} else {
-						DP_NOTICE(p_hwfn->cdev,
-							  "Consecutive igu vectors for HWFN %x vfs is broken [jumps from %04x to %04x]\n",
-							  p_hwfn->rel_pf_id,
-							  last_iov_sb_id,
-							  sb_id); }
-					break;
-				}
-				blk->status |= QED_IGU_STATUS_FREE;
-				p_hwfn->hw_info.p_igu_info->free_blks++;
-				last_iov_sb_id = sb_id;
-			}
+	for (igu_sb_id = 0;
+	     igu_sb_id < QED_MAPPING_MEMORY_SIZE(p_hwfn->cdev); igu_sb_id++) {
+		/* Read current entry; Notice it might not belong to this PF */
+		qed_int_igu_read_cam_block(p_hwfn, p_ptt, igu_sb_id);
+		p_block = &p_igu_info->entry[igu_sb_id];
+
+		if ((p_block->is_pf) &&
+		    (p_block->function_id == p_hwfn->rel_pf_id)) {
+			p_block->status = QED_IGU_STATUS_PF |
+					  QED_IGU_STATUS_VALID |
+					  QED_IGU_STATUS_FREE;
+
+			if (p_igu_info->igu_dsb_id != QED_SB_INVALID_IDX)
+				p_igu_info->usage.cnt++;
+		} else if (!(p_block->is_pf) &&
+			   (p_block->function_id >= min_vf) &&
+			   (p_block->function_id < max_vf)) {
+			/* Available for VFs of this PF */
+			p_block->status = QED_IGU_STATUS_VALID |
+					  QED_IGU_STATUS_FREE;
+
+			if (p_igu_info->igu_dsb_id != QED_SB_INVALID_IDX)
+				p_igu_info->usage.iov_cnt++;
 		}
-	}
 
-	/* There's a possibility the igu_sb_cnt_iov doesn't properly reflect
-	 * the number of VF SBs [especially for first VF on engine, as we can't
-	 * differentiate between empty entries and its entries].
-	 * Since we don't really support more SBs than VFs today, prevent any
-	 * such configuration by sanitizing the number of SBs to equal the
-	 * number of VFs.
-	 */
-	if (IS_PF_SRIOV(p_hwfn)) {
-		u16 total_vfs = p_hwfn->cdev->p_iov_info->total_vfs;
+		/* Mark the First entry belonging to the PF or its VFs
+		 * as the default SB [we'll reset IGU prior to first usage].
+		 */
+		if ((p_block->status & QED_IGU_STATUS_VALID) &&
+		    (p_igu_info->igu_dsb_id == QED_SB_INVALID_IDX)) {
+			p_igu_info->igu_dsb_id = igu_sb_id;
+			p_block->status |= QED_IGU_STATUS_DSB;
+		}
 
-		if (total_vfs < p_igu_info->free_blks) {
-			DP_VERBOSE(p_hwfn,
-				   (NETIF_MSG_INTR | QED_MSG_IOV),
-				   "Limiting number of SBs for IOV - %04x --> %04x\n",
-				   p_igu_info->free_blks,
-				   p_hwfn->cdev->p_iov_info->total_vfs);
-			p_igu_info->free_blks = total_vfs;
-		} else if (total_vfs > p_igu_info->free_blks) {
-			DP_NOTICE(p_hwfn,
-				  "IGU has only %04x SBs for VFs while the device has %04x VFs\n",
-				  p_igu_info->free_blks, total_vfs);
-			return -EINVAL;
+		/* limit number of prints by having each PF print only its
+		 * entries with the exception of PF0 which would print
+		 * everything.
+		 */
+		if ((p_block->status & QED_IGU_STATUS_VALID) ||
+		    (p_hwfn->abs_pf_id == 0)) {
+			DP_VERBOSE(p_hwfn, NETIF_MSG_INTR,
+				   "IGU_BLOCK: [SB 0x%04x] func_id = %d is_pf = %d vector_num = 0x%x\n",
+				   igu_sb_id, p_block->function_id,
+				   p_block->is_pf, p_block->vector_number);
 		}
 	}
-	p_igu_info->igu_sb_cnt_iov = p_igu_info->free_blks;
-
-	DP_VERBOSE(
-		p_hwfn,
-		NETIF_MSG_INTR,
-		"IGU igu_base_sb=0x%x [IOV 0x%x] igu_sb_cnt=%d [IOV 0x%x] igu_dsb_id=0x%x\n",
-		p_igu_info->igu_base_sb,
-		p_igu_info->igu_base_sb_iov,
-		p_igu_info->igu_sb_cnt,
-		p_igu_info->igu_sb_cnt_iov,
-		p_igu_info->igu_dsb_id);
-
-	if (p_igu_info->igu_base_sb == 0xffff ||
-	    p_igu_info->igu_dsb_id == 0xffff ||
-	    p_igu_info->igu_sb_cnt == 0) {
+
+	if (p_igu_info->igu_dsb_id == QED_SB_INVALID_IDX) {
 		DP_NOTICE(p_hwfn,
-			  "IGU CAM returned invalid values igu_base_sb=0x%x igu_sb_cnt=%d igu_dsb_id=0x%x\n",
-			   p_igu_info->igu_base_sb,
-			   p_igu_info->igu_sb_cnt,
-			   p_igu_info->igu_dsb_id);
+			  "IGU CAM returned invalid values igu_dsb_id=0x%x\n",
+			  p_igu_info->igu_dsb_id);
 		return -EINVAL;
 	}
 
+	/* All non default SB are considered free at this point */
+	p_igu_info->usage.free_cnt = p_igu_info->usage.cnt;
+	p_igu_info->usage.free_cnt_iov = p_igu_info->usage.iov_cnt;
+
+	DP_VERBOSE(p_hwfn, NETIF_MSG_INTR,
+		   "igu_dsb_id=0x%x, num Free SBs - PF: %04x VF: %04x [might change after resource allocation]\n",
+		   p_igu_info->igu_dsb_id,
+		   p_igu_info->usage.cnt, p_igu_info->usage.iov_cnt);
+
 	return 0;
 }
 
@@ -3157,6 +2144,7 @@ static int qed_int_sp_dpc_alloc(struct qed_hwfn *p_hwfn)
 static void qed_int_sp_dpc_free(struct qed_hwfn *p_hwfn)
 {
 	kfree(p_hwfn->sp_dpc);
+	p_hwfn->sp_dpc = NULL;
 }
 
 int qed_int_alloc(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
@@ -3198,31 +2186,7 @@ void qed_int_get_num_sbs(struct qed_hwfn	*p_hwfn,
 	if (!info || !p_sb_cnt_info)
 		return;
 
-	p_sb_cnt_info->sb_cnt		= info->igu_sb_cnt;
-	p_sb_cnt_info->sb_iov_cnt	= info->igu_sb_cnt_iov;
-	p_sb_cnt_info->sb_free_blk	= info->free_blks;
-}
-
-u16 qed_int_queue_id_from_sb_id(struct qed_hwfn *p_hwfn, u16 sb_id)
-{
-	struct qed_igu_info *p_info = p_hwfn->hw_info.p_igu_info;
-
-	/* Determine origin of SB id */
-	if ((sb_id >= p_info->igu_base_sb) &&
-	    (sb_id < p_info->igu_base_sb + p_info->igu_sb_cnt)) {
-		return sb_id - p_info->igu_base_sb;
-	} else if ((sb_id >= p_info->igu_base_sb_iov) &&
-		   (sb_id < p_info->igu_base_sb_iov + p_info->igu_sb_cnt_iov)) {
-		/* We want the first VF queue to be adjacent to the
-		 * last PF queue. Since L2 queues can be partial to
-		 * SBs, we'll use the feature instead.
-		 */
-		return sb_id - p_info->igu_base_sb_iov +
-		       FEAT_NUM(p_hwfn, QED_PF_L2_QUE);
-	} else {
-		DP_NOTICE(p_hwfn, "SB %d not in range for function\n", sb_id);
-		return 0;
-	}
+	memcpy(p_sb_cnt_info, &info->usage, sizeof(*p_sb_cnt_info));
 }
 
 void qed_int_disable_post_isr_release(struct qed_dev *cdev)
diff --git a/drivers/net/ethernet/qlogic/qed/qed_int.h b/drivers/net/ethernet/qlogic/qed/qed_int.h
index 0ae0bb4593ef..5199634ed630 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_int.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_int.h
@@ -79,24 +79,6 @@ enum qed_coalescing_fsm {
 };
 
 /**
- * @brief qed_int_cau_conf_pi - configure cau for a given
- *        status block
- *
- * @param p_hwfn
- * @param p_ptt
- * @param igu_sb_id
- * @param pi_index
- * @param state
- * @param timeset
- */
-void qed_int_cau_conf_pi(struct qed_hwfn *p_hwfn,
-			 struct qed_ptt *p_ptt,
-			 u16 igu_sb_id,
-			 u32 pi_index,
-			 enum qed_coalescing_fsm coalescing_fsm,
-			 u8 timeset);
-
-/**
  * @brief qed_int_igu_enable_int - enable device interrupts
  *
  * @param p_hwfn
@@ -217,32 +199,63 @@ void qed_int_disable_post_isr_release(struct qed_dev *cdev);
 #define SB_ALIGNED_SIZE(p_hwfn)	\
 	ALIGNED_TYPE_SIZE(struct status_block, p_hwfn)
 
+#define QED_SB_INVALID_IDX      0xffff
+
 struct qed_igu_block {
-	u8	status;
+	u8 status;
 #define QED_IGU_STATUS_FREE     0x01
 #define QED_IGU_STATUS_VALID    0x02
 #define QED_IGU_STATUS_PF       0x04
+#define QED_IGU_STATUS_DSB      0x08
 
-	u8	vector_number;
-	u8	function_id;
-	u8	is_pf;
-};
+	u8 vector_number;
+	u8 function_id;
+	u8 is_pf;
+
+	/* Index inside IGU [meant for back reference] */
+	u16 igu_sb_id;
 
-struct qed_igu_map {
-	struct qed_igu_block igu_blocks[MAX_TOT_SB_PER_PATH];
+	struct qed_sb_info *sb_info;
 };
 
 struct qed_igu_info {
-	struct qed_igu_map	igu_map;
-	u16			igu_dsb_id;
-	u16			igu_base_sb;
-	u16			igu_base_sb_iov;
-	u16			igu_sb_cnt;
-	u16			igu_sb_cnt_iov;
-	u16			free_blks;
+	struct qed_igu_block entry[MAX_TOT_SB_PER_PATH];
+	u16 igu_dsb_id;
+
+	struct qed_sb_cnt_info usage;
+
+	bool b_allow_pf_vf_change;
 };
 
-/* TODO Names of function may change... */
+/**
+ * @brief - Make sure the IGU CAM reflects the resources provided by MFW
+ *
+ * @param p_hwfn
+ * @param p_ptt
+ */
+int qed_int_igu_reset_cam(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
+
+/**
+ * @brief Translate the weakly-defined client sb-id into an IGU sb-id
+ *
+ * @param p_hwfn
+ * @param sb_id - user provided sb_id
+ *
+ * @return an index inside IGU CAM where the SB resides
+ */
+u16 qed_get_igu_sb_id(struct qed_hwfn *p_hwfn, u16 sb_id);
+
+/**
+ * @brief return a pointer to an unused valid SB
+ *
+ * @param p_hwfn
+ * @param b_is_pf - true iff we want a SB belonging to a PF
+ *
+ * @return point to an igu_block, NULL if none is available
+ */
+struct qed_igu_block *qed_get_igu_free_sb(struct qed_hwfn *p_hwfn,
+					  bool b_is_pf);
+
 void qed_int_igu_init_pure_rt(struct qed_hwfn *p_hwfn,
 			      struct qed_ptt *p_ptt,
 			      bool b_set,
@@ -321,13 +334,13 @@ u16 qed_int_get_sp_sb_id(struct qed_hwfn *p_hwfn);
  *
  * @param p_hwfn
  * @param p_ptt
- * @param sb_id		- igu status block id
+ * @param igu_sb_id	- igu status block id
  * @param opaque	- opaque fid of the sb owner.
  * @param b_set		- set(1) / clear(0)
  */
 void qed_int_igu_init_pure_rt_single(struct qed_hwfn *p_hwfn,
 				     struct qed_ptt *p_ptt,
-				     u32 sb_id,
+				     u16 igu_sb_id,
 				     u16 opaque,
 				     bool b_set);
 
@@ -377,16 +390,6 @@ void qed_int_setup(struct qed_hwfn *p_hwfn,
 		   struct qed_ptt *p_ptt);
 
 /**
- * @brief - Returns an Rx queue index appropriate for usage with given SB.
- *
- * @param p_hwfn
- * @param sb_id - absolute index of SB
- *
- * @return index of Rx queue
- */
-u16 qed_int_queue_id_from_sb_id(struct qed_hwfn *p_hwfn, u16 sb_id);
-
-/**
  * @brief - Enable Interrupt & Attention for hw function
  *
  * @param p_hwfn
diff --git a/drivers/net/ethernet/qlogic/qed/qed_iscsi.c b/drivers/net/ethernet/qlogic/qed/qed_iscsi.c
index 339c91dfa658..813c77cc857f 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_iscsi.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_iscsi.c
@@ -44,7 +44,6 @@
 #include <linux/slab.h>
 #include <linux/stddef.h>
 #include <linux/string.h>
-#include <linux/version.h>
 #include <linux/workqueue.h>
 #include <linux/errno.h>
 #include <linux/list.h>
@@ -63,6 +62,22 @@
 #include "qed_sriov.h"
 #include "qed_reg_addr.h"
 
+static int
+qed_iscsi_async_event(struct qed_hwfn *p_hwfn,
+		      u8 fw_event_code,
+		      u16 echo, union event_ring_data *data, u8 fw_return_code)
+{
+	if (p_hwfn->p_iscsi_info->event_cb) {
+		struct qed_iscsi_info *p_iscsi = p_hwfn->p_iscsi_info;
+
+		return p_iscsi->event_cb(p_iscsi->event_context,
+					 fw_event_code, data);
+	} else {
+		DP_NOTICE(p_hwfn, "iSCSI async completion is not set\n");
+		return -EINVAL;
+	}
+}
+
 struct qed_iscsi_conn {
 	struct list_head list_entry;
 	bool free_on_delete;
@@ -186,7 +201,7 @@ qed_sp_iscsi_func_start(struct qed_hwfn *p_hwfn,
 		DP_ERR(p_hwfn,
 		       "Cannot satisfy CQ amount. Queues requested %d, CQs available %d. Aborting function start\n",
 		       p_params->num_queues,
-		       p_hwfn->hw_info.resc_num[QED_ISCSI_CQ]);
+		       p_hwfn->hw_info.feat_num[QED_ISCSI_CQ]);
 		return -EINVAL;
 	}
 
@@ -221,7 +236,7 @@ qed_sp_iscsi_func_start(struct qed_hwfn *p_hwfn,
 	p_queue->cmdq_sb_pi = p_params->gl_cmd_pi;
 
 	for (i = 0; i < p_params->num_queues; i++) {
-		val = p_hwfn->sbs_info[i]->igu_sb_id;
+		val = qed_get_igu_sb_id(p_hwfn, i);
 		p_queue->cq_cmdq_sb_num_arr[i] = cpu_to_le16(val);
 	}
 
@@ -266,6 +281,9 @@ qed_sp_iscsi_func_start(struct qed_hwfn *p_hwfn,
 	p_hwfn->p_iscsi_info->event_context = event_context;
 	p_hwfn->p_iscsi_info->event_cb = async_event_cb;
 
+	qed_spq_register_async_cb(p_hwfn, PROTOCOLID_ISCSI,
+				  qed_iscsi_async_event);
+
 	return qed_spq_post(p_hwfn, p_ent, NULL);
 }
 
@@ -375,7 +393,6 @@ static int qed_sp_iscsi_conn_offload(struct qed_hwfn *p_hwfn,
 		p_tcp->ss_thresh = cpu_to_le32(p_conn->ss_thresh);
 		p_tcp->srtt = cpu_to_le16(p_conn->srtt);
 		p_tcp->rtt_var = cpu_to_le16(p_conn->rtt_var);
-		p_tcp->ts_time = cpu_to_le32(p_conn->ts_time);
 		p_tcp->ts_recent = cpu_to_le32(p_conn->ts_recent);
 		p_tcp->ts_recent_age = cpu_to_le32(p_conn->ts_recent_age);
 		p_tcp->total_rt = cpu_to_le32(p_conn->total_rt);
@@ -400,8 +417,6 @@ static int qed_sp_iscsi_conn_offload(struct qed_hwfn *p_hwfn,
 		p_tcp->mss = cpu_to_le16(p_conn->mss);
 		p_tcp->snd_wnd_scale = p_conn->snd_wnd_scale;
 		p_tcp->rcv_wnd_scale = p_conn->rcv_wnd_scale;
-		dval = p_conn->ts_ticks_per_second;
-		p_tcp->ts_ticks_per_second = cpu_to_le32(dval);
 		wval = p_conn->da_timeout_value;
 		p_tcp->da_timeout_value = cpu_to_le16(wval);
 		p_tcp->ack_frequency = p_conn->ack_frequency;
@@ -492,6 +507,54 @@ static int qed_sp_iscsi_conn_update(struct qed_hwfn *p_hwfn,
 	return qed_spq_post(p_hwfn, p_ent, NULL);
 }
 
+static int
+qed_sp_iscsi_mac_update(struct qed_hwfn *p_hwfn,
+			struct qed_iscsi_conn *p_conn,
+			enum spq_mode comp_mode,
+			struct qed_spq_comp_cb *p_comp_addr)
+{
+	struct iscsi_spe_conn_mac_update *p_ramrod = NULL;
+	struct qed_spq_entry *p_ent = NULL;
+	struct qed_sp_init_data init_data;
+	int rc = -EINVAL;
+	u8 ucval;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = p_conn->icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = comp_mode;
+	init_data.p_comp_data = p_comp_addr;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 ISCSI_RAMROD_CMD_ID_MAC_UPDATE,
+				 PROTOCOLID_ISCSI, &init_data);
+	if (rc)
+		return rc;
+
+	p_ramrod = &p_ent->ramrod.iscsi_conn_mac_update;
+	p_ramrod->hdr.op_code = ISCSI_RAMROD_CMD_ID_MAC_UPDATE;
+	SET_FIELD(p_ramrod->hdr.flags,
+		  ISCSI_SLOW_PATH_HDR_LAYER_CODE, p_conn->layer_code);
+
+	p_ramrod->conn_id = cpu_to_le16(p_conn->conn_id);
+	p_ramrod->fw_cid = cpu_to_le32(p_conn->icid);
+	ucval = p_conn->remote_mac[1];
+	((u8 *)(&p_ramrod->remote_mac_addr_hi))[0] = ucval;
+	ucval = p_conn->remote_mac[0];
+	((u8 *)(&p_ramrod->remote_mac_addr_hi))[1] = ucval;
+	ucval = p_conn->remote_mac[3];
+	((u8 *)(&p_ramrod->remote_mac_addr_mid))[0] = ucval;
+	ucval = p_conn->remote_mac[2];
+	((u8 *)(&p_ramrod->remote_mac_addr_mid))[1] = ucval;
+	ucval = p_conn->remote_mac[5];
+	((u8 *)(&p_ramrod->remote_mac_addr_lo))[0] = ucval;
+	ucval = p_conn->remote_mac[4];
+	((u8 *)(&p_ramrod->remote_mac_addr_lo))[1] = ucval;
+
+	return qed_spq_post(p_hwfn, p_ent, NULL);
+}
+
 static int qed_sp_iscsi_conn_terminate(struct qed_hwfn *p_hwfn,
 				       struct qed_iscsi_conn *p_conn,
 				       enum spq_mode comp_mode,
@@ -587,7 +650,10 @@ static int qed_sp_iscsi_func_stop(struct qed_hwfn *p_hwfn,
 	p_ramrod = &p_ent->ramrod.iscsi_destroy;
 	p_ramrod->hdr.op_code = ISCSI_RAMROD_CMD_ID_DESTROY_FUNC;
 
-	return qed_spq_post(p_hwfn, p_ent, NULL);
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+
+	qed_spq_unregister_async_cb(p_hwfn, PROTOCOLID_ISCSI);
+	return rc;
 }
 
 static void __iomem *qed_iscsi_get_db_addr(struct qed_hwfn *p_hwfn, u32 cid)
@@ -708,7 +774,7 @@ static int qed_iscsi_allocate_connection(struct qed_hwfn *p_hwfn,
 			     QED_CHAIN_USE_TO_CONSUME_PRODUCE,
 			     QED_CHAIN_MODE_PBL,
 			     QED_CHAIN_CNT_TYPE_U16,
-			     r2tq_num_elements, 0x80, &p_conn->r2tq);
+			     r2tq_num_elements, 0x80, &p_conn->r2tq, NULL);
 	if (rc)
 		goto nomem_r2tq;
 
@@ -719,7 +785,7 @@ static int qed_iscsi_allocate_connection(struct qed_hwfn *p_hwfn,
 			     QED_CHAIN_MODE_PBL,
 			     QED_CHAIN_CNT_TYPE_U16,
 			     uhq_num_elements,
-			     sizeof(struct iscsi_uhqe), &p_conn->uhq);
+			     sizeof(struct iscsi_uhqe), &p_conn->uhq, NULL);
 	if (rc)
 		goto nomem_uhq;
 
@@ -729,7 +795,7 @@ static int qed_iscsi_allocate_connection(struct qed_hwfn *p_hwfn,
 			     QED_CHAIN_MODE_PBL,
 			     QED_CHAIN_CNT_TYPE_U16,
 			     xhq_num_elements,
-			     sizeof(struct iscsi_xhqe), &p_conn->xhq);
+			     sizeof(struct iscsi_xhqe), &p_conn->xhq, NULL);
 	if (rc)
 		goto nomem;
 
@@ -822,29 +888,32 @@ void qed_iscsi_free_connection(struct qed_hwfn *p_hwfn,
 	kfree(p_conn);
 }
 
-struct qed_iscsi_info *qed_iscsi_alloc(struct qed_hwfn *p_hwfn)
+int qed_iscsi_alloc(struct qed_hwfn *p_hwfn)
 {
 	struct qed_iscsi_info *p_iscsi_info;
 
 	p_iscsi_info = kzalloc(sizeof(*p_iscsi_info), GFP_KERNEL);
 	if (!p_iscsi_info)
-		return NULL;
+		return -ENOMEM;
 
 	INIT_LIST_HEAD(&p_iscsi_info->free_list);
-	return p_iscsi_info;
+
+	p_hwfn->p_iscsi_info = p_iscsi_info;
+	return 0;
 }
 
-void qed_iscsi_setup(struct qed_hwfn *p_hwfn,
-		     struct qed_iscsi_info *p_iscsi_info)
+void qed_iscsi_setup(struct qed_hwfn *p_hwfn)
 {
-	spin_lock_init(&p_iscsi_info->lock);
+	spin_lock_init(&p_hwfn->p_iscsi_info->lock);
 }
 
-void qed_iscsi_free(struct qed_hwfn *p_hwfn,
-		    struct qed_iscsi_info *p_iscsi_info)
+void qed_iscsi_free(struct qed_hwfn *p_hwfn)
 {
 	struct qed_iscsi_conn *p_conn = NULL;
 
+	if (!p_hwfn->p_iscsi_info)
+		return;
+
 	while (!list_empty(&p_hwfn->p_iscsi_info->free_list)) {
 		p_conn = list_first_entry(&p_hwfn->p_iscsi_info->free_list,
 					  struct qed_iscsi_conn, list_entry);
@@ -854,7 +923,8 @@ void qed_iscsi_free(struct qed_hwfn *p_hwfn,
 		}
 	}
 
-	kfree(p_iscsi_info);
+	kfree(p_hwfn->p_iscsi_info);
+	p_hwfn->p_iscsi_info = NULL;
 }
 
 static void _qed_iscsi_get_tstats(struct qed_hwfn *p_hwfn,
@@ -1324,6 +1394,23 @@ static int qed_iscsi_stats(struct qed_dev *cdev, struct qed_iscsi_stats *stats)
 	return qed_iscsi_get_stats(QED_LEADING_HWFN(cdev), stats);
 }
 
+static int qed_iscsi_change_mac(struct qed_dev *cdev,
+				u32 handle, const u8 *mac)
+{
+	struct qed_hash_iscsi_con *hash_con;
+
+	hash_con = qed_iscsi_get_hash(cdev, handle);
+	if (!hash_con) {
+		DP_NOTICE(cdev, "Failed to find connection for handle %d\n",
+			  handle);
+		return -EINVAL;
+	}
+
+	return qed_sp_iscsi_mac_update(QED_LEADING_HWFN(cdev),
+				       hash_con->con,
+				       QED_SPQ_MODE_EBLOCK, NULL);
+}
+
 void qed_get_protocol_stats_iscsi(struct qed_dev *cdev,
 				  struct qed_mcp_iscsi_stats *stats)
 {
@@ -1358,6 +1445,7 @@ static const struct qed_iscsi_ops qed_iscsi_ops_pass = {
 	.destroy_conn = &qed_iscsi_destroy_conn,
 	.clear_sq = &qed_iscsi_clear_conn_sq,
 	.get_stats = &qed_iscsi_stats,
+	.change_mac = &qed_iscsi_change_mac,
 };
 
 const struct qed_iscsi_ops *qed_get_iscsi_ops(void)
diff --git a/drivers/net/ethernet/qlogic/qed/qed_iscsi.h b/drivers/net/ethernet/qlogic/qed/qed_iscsi.h
index ae98f772cbc0..225c75b02a06 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_iscsi.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_iscsi.h
@@ -57,13 +57,11 @@ extern const struct qed_ll2_ops qed_ll2_ops_pass;
 #endif
 
 #if IS_ENABLED(CONFIG_QED_ISCSI)
-struct qed_iscsi_info *qed_iscsi_alloc(struct qed_hwfn *p_hwfn);
+int qed_iscsi_alloc(struct qed_hwfn *p_hwfn);
 
-void qed_iscsi_setup(struct qed_hwfn *p_hwfn,
-		     struct qed_iscsi_info *p_iscsi_info);
+void qed_iscsi_setup(struct qed_hwfn *p_hwfn);
 
-void qed_iscsi_free(struct qed_hwfn *p_hwfn,
-		    struct qed_iscsi_info *p_iscsi_info);
+void qed_iscsi_free(struct qed_hwfn *p_hwfn);
 
 /**
  * @brief - Fills provided statistics struct with statistics.
@@ -74,12 +72,15 @@ void qed_iscsi_free(struct qed_hwfn *p_hwfn,
 void qed_get_protocol_stats_iscsi(struct qed_dev *cdev,
 				  struct qed_mcp_iscsi_stats *stats);
 #else /* IS_ENABLED(CONFIG_QED_ISCSI) */
-static inline struct qed_iscsi_info *qed_iscsi_alloc(
-		struct qed_hwfn *p_hwfn) { return NULL; }
-static inline void qed_iscsi_setup(struct qed_hwfn *p_hwfn,
-				   struct qed_iscsi_info *p_iscsi_info) {}
-static inline void qed_iscsi_free(struct qed_hwfn *p_hwfn,
-				 struct qed_iscsi_info *p_iscsi_info) {}
+static inline int qed_iscsi_alloc(struct qed_hwfn *p_hwfn)
+{
+	return -EINVAL;
+}
+
+static inline void qed_iscsi_setup(struct qed_hwfn *p_hwfn) {}
+
+static inline void qed_iscsi_free(struct qed_hwfn *p_hwfn) {}
+
 static inline void
 qed_get_protocol_stats_iscsi(struct qed_dev *cdev,
 			     struct qed_mcp_iscsi_stats *stats) {}
diff --git a/drivers/net/ethernet/qlogic/qed/qed_iwarp.c b/drivers/net/ethernet/qlogic/qed/qed_iwarp.c
new file mode 100644
index 000000000000..b251ebaec4db
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qed/qed_iwarp.c
@@ -0,0 +1,2408 @@
+/* QLogic qed NIC Driver
+ * Copyright (c) 2015-2017  QLogic Corporation
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and /or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+#include <linux/if_ether.h>
+#include <linux/if_vlan.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/spinlock.h>
+#include <linux/tcp.h>
+#include "qed_cxt.h"
+#include "qed_hw.h"
+#include "qed_ll2.h"
+#include "qed_rdma.h"
+#include "qed_reg_addr.h"
+#include "qed_sp.h"
+
+#define QED_IWARP_ORD_DEFAULT		32
+#define QED_IWARP_IRD_DEFAULT		32
+#define QED_IWARP_MAX_FW_MSS		4120
+
+#define QED_EP_SIG 0xecabcdef
+
+struct mpa_v2_hdr {
+	__be16 ird;
+	__be16 ord;
+};
+
+#define MPA_V2_PEER2PEER_MODEL  0x8000
+#define MPA_V2_SEND_RTR         0x4000	/* on ird */
+#define MPA_V2_READ_RTR         0x4000	/* on ord */
+#define MPA_V2_WRITE_RTR        0x8000
+#define MPA_V2_IRD_ORD_MASK     0x3FFF
+
+#define MPA_REV2(_mpa_rev) ((_mpa_rev) == MPA_NEGOTIATION_TYPE_ENHANCED)
+
+#define QED_IWARP_INVALID_TCP_CID	0xffffffff
+#define QED_IWARP_RCV_WND_SIZE_DEF	(256 * 1024)
+#define QED_IWARP_RCV_WND_SIZE_MIN	(64 * 1024)
+#define TIMESTAMP_HEADER_SIZE		(12)
+
+#define QED_IWARP_TS_EN			BIT(0)
+#define QED_IWARP_DA_EN			BIT(1)
+#define QED_IWARP_PARAM_CRC_NEEDED	(1)
+#define QED_IWARP_PARAM_P2P		(1)
+
+static int qed_iwarp_async_event(struct qed_hwfn *p_hwfn,
+				 u8 fw_event_code, u16 echo,
+				 union event_ring_data *data,
+				 u8 fw_return_code);
+
+/* Override devinfo with iWARP specific values */
+void qed_iwarp_init_devinfo(struct qed_hwfn *p_hwfn)
+{
+	struct qed_rdma_device *dev = p_hwfn->p_rdma_info->dev;
+
+	dev->max_inline = IWARP_REQ_MAX_INLINE_DATA_SIZE;
+	dev->max_qp = min_t(u32,
+			    IWARP_MAX_QPS,
+			    p_hwfn->p_rdma_info->num_qps) -
+		      QED_IWARP_PREALLOC_CNT;
+
+	dev->max_cq = dev->max_qp;
+
+	dev->max_qp_resp_rd_atomic_resc = QED_IWARP_IRD_DEFAULT;
+	dev->max_qp_req_rd_atomic_resc = QED_IWARP_ORD_DEFAULT;
+}
+
+void qed_iwarp_init_hw(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+{
+	p_hwfn->rdma_prs_search_reg = PRS_REG_SEARCH_TCP;
+	qed_wr(p_hwfn, p_ptt, p_hwfn->rdma_prs_search_reg, 1);
+	p_hwfn->b_rdma_enabled_in_prs = true;
+}
+
+/* We have two cid maps, one for tcp which should be used only from passive
+ * syn processing and replacing a pre-allocated ep in the list. The second
+ * for active tcp and for QPs.
+ */
+static void qed_iwarp_cid_cleaned(struct qed_hwfn *p_hwfn, u32 cid)
+{
+	cid -= qed_cxt_get_proto_cid_start(p_hwfn, p_hwfn->p_rdma_info->proto);
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+
+	if (cid < QED_IWARP_PREALLOC_CNT)
+		qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->tcp_cid_map,
+				    cid);
+	else
+		qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->cid_map, cid);
+
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+}
+
+static int qed_iwarp_alloc_cid(struct qed_hwfn *p_hwfn, u32 *cid)
+{
+	int rc;
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	rc = qed_rdma_bmap_alloc_id(p_hwfn, &p_hwfn->p_rdma_info->cid_map, cid);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+	if (rc) {
+		DP_NOTICE(p_hwfn, "Failed in allocating iwarp cid\n");
+		return rc;
+	}
+	*cid += qed_cxt_get_proto_cid_start(p_hwfn, p_hwfn->p_rdma_info->proto);
+
+	rc = qed_cxt_dynamic_ilt_alloc(p_hwfn, QED_ELEM_CXT, *cid);
+	if (rc)
+		qed_iwarp_cid_cleaned(p_hwfn, *cid);
+
+	return rc;
+}
+
+static void qed_iwarp_set_tcp_cid(struct qed_hwfn *p_hwfn, u32 cid)
+{
+	cid -= qed_cxt_get_proto_cid_start(p_hwfn, p_hwfn->p_rdma_info->proto);
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	qed_bmap_set_id(p_hwfn, &p_hwfn->p_rdma_info->tcp_cid_map, cid);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+}
+
+/* This function allocates a cid for passive tcp (called from syn receive)
+ * the reason it's separate from the regular cid allocation is because it
+ * is assured that these cids already have ilt allocated. They are preallocated
+ * to ensure that we won't need to allocate memory during syn processing
+ */
+static int qed_iwarp_alloc_tcp_cid(struct qed_hwfn *p_hwfn, u32 *cid)
+{
+	int rc;
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+
+	rc = qed_rdma_bmap_alloc_id(p_hwfn,
+				    &p_hwfn->p_rdma_info->tcp_cid_map, cid);
+
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "can't allocate iwarp tcp cid max-count=%d\n",
+			   p_hwfn->p_rdma_info->tcp_cid_map.max_count);
+
+		*cid = QED_IWARP_INVALID_TCP_CID;
+		return rc;
+	}
+
+	*cid += qed_cxt_get_proto_cid_start(p_hwfn,
+					    p_hwfn->p_rdma_info->proto);
+	return 0;
+}
+
+int qed_iwarp_create_qp(struct qed_hwfn *p_hwfn,
+			struct qed_rdma_qp *qp,
+			struct qed_rdma_create_qp_out_params *out_params)
+{
+	struct iwarp_create_qp_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	u16 physical_queue;
+	u32 cid;
+	int rc;
+
+	qp->shared_queue = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
+					      IWARP_SHARED_QUEUE_PAGE_SIZE,
+					      &qp->shared_queue_phys_addr,
+					      GFP_KERNEL);
+	if (!qp->shared_queue)
+		return -ENOMEM;
+
+	out_params->sq_pbl_virt = (u8 *)qp->shared_queue +
+	    IWARP_SHARED_QUEUE_PAGE_SQ_PBL_OFFSET;
+	out_params->sq_pbl_phys = qp->shared_queue_phys_addr +
+	    IWARP_SHARED_QUEUE_PAGE_SQ_PBL_OFFSET;
+	out_params->rq_pbl_virt = (u8 *)qp->shared_queue +
+	    IWARP_SHARED_QUEUE_PAGE_RQ_PBL_OFFSET;
+	out_params->rq_pbl_phys = qp->shared_queue_phys_addr +
+	    IWARP_SHARED_QUEUE_PAGE_RQ_PBL_OFFSET;
+
+	rc = qed_iwarp_alloc_cid(p_hwfn, &cid);
+	if (rc)
+		goto err1;
+
+	qp->icid = (u16)cid;
+
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.cid = qp->icid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 IWARP_RAMROD_CMD_ID_CREATE_QP,
+				 PROTOCOLID_IWARP, &init_data);
+	if (rc)
+		goto err2;
+
+	p_ramrod = &p_ent->ramrod.iwarp_create_qp;
+
+	SET_FIELD(p_ramrod->flags,
+		  IWARP_CREATE_QP_RAMROD_DATA_FMR_AND_RESERVED_EN,
+		  qp->fmr_and_reserved_lkey);
+
+	SET_FIELD(p_ramrod->flags,
+		  IWARP_CREATE_QP_RAMROD_DATA_SIGNALED_COMP, qp->signal_all);
+
+	SET_FIELD(p_ramrod->flags,
+		  IWARP_CREATE_QP_RAMROD_DATA_RDMA_RD_EN,
+		  qp->incoming_rdma_read_en);
+
+	SET_FIELD(p_ramrod->flags,
+		  IWARP_CREATE_QP_RAMROD_DATA_RDMA_WR_EN,
+		  qp->incoming_rdma_write_en);
+
+	SET_FIELD(p_ramrod->flags,
+		  IWARP_CREATE_QP_RAMROD_DATA_ATOMIC_EN,
+		  qp->incoming_atomic_en);
+
+	SET_FIELD(p_ramrod->flags,
+		  IWARP_CREATE_QP_RAMROD_DATA_SRQ_FLG, qp->use_srq);
+
+	p_ramrod->pd = qp->pd;
+	p_ramrod->sq_num_pages = qp->sq_num_pages;
+	p_ramrod->rq_num_pages = qp->rq_num_pages;
+
+	p_ramrod->qp_handle_for_cqe.hi = cpu_to_le32(qp->qp_handle.hi);
+	p_ramrod->qp_handle_for_cqe.lo = cpu_to_le32(qp->qp_handle.lo);
+
+	p_ramrod->cq_cid_for_sq =
+	    cpu_to_le32((p_hwfn->hw_info.opaque_fid << 16) | qp->sq_cq_id);
+	p_ramrod->cq_cid_for_rq =
+	    cpu_to_le32((p_hwfn->hw_info.opaque_fid << 16) | qp->rq_cq_id);
+
+	p_ramrod->dpi = cpu_to_le16(qp->dpi);
+
+	physical_queue = qed_get_cm_pq_idx(p_hwfn, PQ_FLAGS_OFLD);
+	p_ramrod->physical_q0 = cpu_to_le16(physical_queue);
+	physical_queue = qed_get_cm_pq_idx(p_hwfn, PQ_FLAGS_ACK);
+	p_ramrod->physical_q1 = cpu_to_le16(physical_queue);
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+	if (rc)
+		goto err2;
+
+	return rc;
+
+err2:
+	qed_iwarp_cid_cleaned(p_hwfn, cid);
+err1:
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+			  IWARP_SHARED_QUEUE_PAGE_SIZE,
+			  qp->shared_queue, qp->shared_queue_phys_addr);
+
+	return rc;
+}
+
+static int qed_iwarp_modify_fw(struct qed_hwfn *p_hwfn, struct qed_rdma_qp *qp)
+{
+	struct iwarp_modify_qp_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	int rc;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = qp->icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 IWARP_RAMROD_CMD_ID_MODIFY_QP,
+				 p_hwfn->p_rdma_info->proto, &init_data);
+	if (rc)
+		return rc;
+
+	p_ramrod = &p_ent->ramrod.iwarp_modify_qp;
+	SET_FIELD(p_ramrod->flags, IWARP_MODIFY_QP_RAMROD_DATA_STATE_TRANS_EN,
+		  0x1);
+	if (qp->iwarp_state == QED_IWARP_QP_STATE_CLOSING)
+		p_ramrod->transition_to_state = IWARP_MODIFY_QP_STATE_CLOSING;
+	else
+		p_ramrod->transition_to_state = IWARP_MODIFY_QP_STATE_ERROR;
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "QP(0x%x)rc=%d\n", qp->icid, rc);
+
+	return rc;
+}
+
+enum qed_iwarp_qp_state qed_roce2iwarp_state(enum qed_roce_qp_state state)
+{
+	switch (state) {
+	case QED_ROCE_QP_STATE_RESET:
+	case QED_ROCE_QP_STATE_INIT:
+	case QED_ROCE_QP_STATE_RTR:
+		return QED_IWARP_QP_STATE_IDLE;
+	case QED_ROCE_QP_STATE_RTS:
+		return QED_IWARP_QP_STATE_RTS;
+	case QED_ROCE_QP_STATE_SQD:
+		return QED_IWARP_QP_STATE_CLOSING;
+	case QED_ROCE_QP_STATE_ERR:
+		return QED_IWARP_QP_STATE_ERROR;
+	case QED_ROCE_QP_STATE_SQE:
+		return QED_IWARP_QP_STATE_TERMINATE;
+	default:
+		return QED_IWARP_QP_STATE_ERROR;
+	}
+}
+
+static enum qed_roce_qp_state
+qed_iwarp2roce_state(enum qed_iwarp_qp_state state)
+{
+	switch (state) {
+	case QED_IWARP_QP_STATE_IDLE:
+		return QED_ROCE_QP_STATE_INIT;
+	case QED_IWARP_QP_STATE_RTS:
+		return QED_ROCE_QP_STATE_RTS;
+	case QED_IWARP_QP_STATE_TERMINATE:
+		return QED_ROCE_QP_STATE_SQE;
+	case QED_IWARP_QP_STATE_CLOSING:
+		return QED_ROCE_QP_STATE_SQD;
+	case QED_IWARP_QP_STATE_ERROR:
+		return QED_ROCE_QP_STATE_ERR;
+	default:
+		return QED_ROCE_QP_STATE_ERR;
+	}
+}
+
+const char *iwarp_state_names[] = {
+	"IDLE",
+	"RTS",
+	"TERMINATE",
+	"CLOSING",
+	"ERROR",
+};
+
+int
+qed_iwarp_modify_qp(struct qed_hwfn *p_hwfn,
+		    struct qed_rdma_qp *qp,
+		    enum qed_iwarp_qp_state new_state, bool internal)
+{
+	enum qed_iwarp_qp_state prev_iw_state;
+	bool modify_fw = false;
+	int rc = 0;
+
+	/* modify QP can be called from upper-layer or as a result of async
+	 * RST/FIN... therefore need to protect
+	 */
+	spin_lock_bh(&p_hwfn->p_rdma_info->iwarp.qp_lock);
+	prev_iw_state = qp->iwarp_state;
+
+	if (prev_iw_state == new_state) {
+		spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.qp_lock);
+		return 0;
+	}
+
+	switch (prev_iw_state) {
+	case QED_IWARP_QP_STATE_IDLE:
+		switch (new_state) {
+		case QED_IWARP_QP_STATE_RTS:
+			qp->iwarp_state = QED_IWARP_QP_STATE_RTS;
+			break;
+		case QED_IWARP_QP_STATE_ERROR:
+			qp->iwarp_state = QED_IWARP_QP_STATE_ERROR;
+			if (!internal)
+				modify_fw = true;
+			break;
+		default:
+			break;
+		}
+		break;
+	case QED_IWARP_QP_STATE_RTS:
+		switch (new_state) {
+		case QED_IWARP_QP_STATE_CLOSING:
+			if (!internal)
+				modify_fw = true;
+
+			qp->iwarp_state = QED_IWARP_QP_STATE_CLOSING;
+			break;
+		case QED_IWARP_QP_STATE_ERROR:
+			if (!internal)
+				modify_fw = true;
+			qp->iwarp_state = QED_IWARP_QP_STATE_ERROR;
+			break;
+		default:
+			break;
+		}
+		break;
+	case QED_IWARP_QP_STATE_ERROR:
+		switch (new_state) {
+		case QED_IWARP_QP_STATE_IDLE:
+
+			qp->iwarp_state = new_state;
+			break;
+		case QED_IWARP_QP_STATE_CLOSING:
+			/* could happen due to race... do nothing.... */
+			break;
+		default:
+			rc = -EINVAL;
+		}
+		break;
+	case QED_IWARP_QP_STATE_TERMINATE:
+	case QED_IWARP_QP_STATE_CLOSING:
+		qp->iwarp_state = new_state;
+		break;
+	default:
+		break;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "QP(0x%x) %s --> %s%s\n",
+		   qp->icid,
+		   iwarp_state_names[prev_iw_state],
+		   iwarp_state_names[qp->iwarp_state],
+		   internal ? "internal" : "");
+
+	spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.qp_lock);
+
+	if (modify_fw)
+		rc = qed_iwarp_modify_fw(p_hwfn, qp);
+
+	return rc;
+}
+
+int qed_iwarp_fw_destroy(struct qed_hwfn *p_hwfn, struct qed_rdma_qp *qp)
+{
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	int rc;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = qp->icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 IWARP_RAMROD_CMD_ID_DESTROY_QP,
+				 p_hwfn->p_rdma_info->proto, &init_data);
+	if (rc)
+		return rc;
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "QP(0x%x) rc = %d\n", qp->icid, rc);
+
+	return rc;
+}
+
+static void qed_iwarp_destroy_ep(struct qed_hwfn *p_hwfn,
+				 struct qed_iwarp_ep *ep,
+				 bool remove_from_active_list)
+{
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+			  sizeof(*ep->ep_buffer_virt),
+			  ep->ep_buffer_virt, ep->ep_buffer_phys);
+
+	if (remove_from_active_list) {
+		spin_lock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+		list_del(&ep->list_entry);
+		spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+	}
+
+	if (ep->qp)
+		ep->qp->ep = NULL;
+
+	kfree(ep);
+}
+
+int qed_iwarp_destroy_qp(struct qed_hwfn *p_hwfn, struct qed_rdma_qp *qp)
+{
+	struct qed_iwarp_ep *ep = qp->ep;
+	int wait_count = 0;
+	int rc = 0;
+
+	if (qp->iwarp_state != QED_IWARP_QP_STATE_ERROR) {
+		rc = qed_iwarp_modify_qp(p_hwfn, qp,
+					 QED_IWARP_QP_STATE_ERROR, false);
+		if (rc)
+			return rc;
+	}
+
+	/* Make sure ep is closed before returning and freeing memory. */
+	if (ep) {
+		while (ep->state != QED_IWARP_EP_CLOSED && wait_count++ < 200)
+			msleep(100);
+
+		if (ep->state != QED_IWARP_EP_CLOSED)
+			DP_NOTICE(p_hwfn, "ep state close timeout state=%x\n",
+				  ep->state);
+
+		qed_iwarp_destroy_ep(p_hwfn, ep, false);
+	}
+
+	rc = qed_iwarp_fw_destroy(p_hwfn, qp);
+
+	if (qp->shared_queue)
+		dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+				  IWARP_SHARED_QUEUE_PAGE_SIZE,
+				  qp->shared_queue, qp->shared_queue_phys_addr);
+
+	return rc;
+}
+
+static int
+qed_iwarp_create_ep(struct qed_hwfn *p_hwfn, struct qed_iwarp_ep **ep_out)
+{
+	struct qed_iwarp_ep *ep;
+	int rc;
+
+	ep = kzalloc(sizeof(*ep), GFP_KERNEL);
+	if (!ep)
+		return -ENOMEM;
+
+	ep->state = QED_IWARP_EP_INIT;
+
+	ep->ep_buffer_virt = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
+						sizeof(*ep->ep_buffer_virt),
+						&ep->ep_buffer_phys,
+						GFP_KERNEL);
+	if (!ep->ep_buffer_virt) {
+		rc = -ENOMEM;
+		goto err;
+	}
+
+	ep->sig = QED_EP_SIG;
+
+	*ep_out = ep;
+
+	return 0;
+
+err:
+	kfree(ep);
+	return rc;
+}
+
+static void
+qed_iwarp_print_tcp_ramrod(struct qed_hwfn *p_hwfn,
+			   struct iwarp_tcp_offload_ramrod_data *p_tcp_ramrod)
+{
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "local_mac=%x %x %x, remote_mac=%x %x %x\n",
+		   p_tcp_ramrod->tcp.local_mac_addr_lo,
+		   p_tcp_ramrod->tcp.local_mac_addr_mid,
+		   p_tcp_ramrod->tcp.local_mac_addr_hi,
+		   p_tcp_ramrod->tcp.remote_mac_addr_lo,
+		   p_tcp_ramrod->tcp.remote_mac_addr_mid,
+		   p_tcp_ramrod->tcp.remote_mac_addr_hi);
+
+	if (p_tcp_ramrod->tcp.ip_version == TCP_IPV4) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "local_ip=%pI4h:%x, remote_ip=%pI4h%x, vlan=%x\n",
+			   p_tcp_ramrod->tcp.local_ip,
+			   p_tcp_ramrod->tcp.local_port,
+			   p_tcp_ramrod->tcp.remote_ip,
+			   p_tcp_ramrod->tcp.remote_port,
+			   p_tcp_ramrod->tcp.vlan_id);
+	} else {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "local_ip=%pI6h:%x, remote_ip=%pI6h:%x, vlan=%x\n",
+			   p_tcp_ramrod->tcp.local_ip,
+			   p_tcp_ramrod->tcp.local_port,
+			   p_tcp_ramrod->tcp.remote_ip,
+			   p_tcp_ramrod->tcp.remote_port,
+			   p_tcp_ramrod->tcp.vlan_id);
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+		   "flow_label=%x, ttl=%x, tos_or_tc=%x, mss=%x, rcv_wnd_scale=%x, connect_mode=%x, flags=%x\n",
+		   p_tcp_ramrod->tcp.flow_label,
+		   p_tcp_ramrod->tcp.ttl,
+		   p_tcp_ramrod->tcp.tos_or_tc,
+		   p_tcp_ramrod->tcp.mss,
+		   p_tcp_ramrod->tcp.rcv_wnd_scale,
+		   p_tcp_ramrod->tcp.connect_mode,
+		   p_tcp_ramrod->tcp.flags);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "syn_ip_payload_length=%x, lo=%x, hi=%x\n",
+		   p_tcp_ramrod->tcp.syn_ip_payload_length,
+		   p_tcp_ramrod->tcp.syn_phy_addr_lo,
+		   p_tcp_ramrod->tcp.syn_phy_addr_hi);
+}
+
+static int
+qed_iwarp_tcp_offload(struct qed_hwfn *p_hwfn, struct qed_iwarp_ep *ep)
+{
+	struct qed_iwarp_info *iwarp_info = &p_hwfn->p_rdma_info->iwarp;
+	struct iwarp_tcp_offload_ramrod_data *p_tcp_ramrod;
+	struct tcp_offload_params_opt2 *tcp;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	dma_addr_t async_output_phys;
+	dma_addr_t in_pdata_phys;
+	u16 physical_q;
+	u8 tcp_flags;
+	int rc;
+	int i;
+
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = ep->tcp_cid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	if (ep->connect_mode == TCP_CONNECT_PASSIVE)
+		init_data.comp_mode = QED_SPQ_MODE_CB;
+	else
+		init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 IWARP_RAMROD_CMD_ID_TCP_OFFLOAD,
+				 PROTOCOLID_IWARP, &init_data);
+	if (rc)
+		return rc;
+
+	p_tcp_ramrod = &p_ent->ramrod.iwarp_tcp_offload;
+
+	in_pdata_phys = ep->ep_buffer_phys +
+			offsetof(struct qed_iwarp_ep_memory, in_pdata);
+	DMA_REGPAIR_LE(p_tcp_ramrod->iwarp.incoming_ulp_buffer.addr,
+		       in_pdata_phys);
+
+	p_tcp_ramrod->iwarp.incoming_ulp_buffer.len =
+	    cpu_to_le16(sizeof(ep->ep_buffer_virt->in_pdata));
+
+	async_output_phys = ep->ep_buffer_phys +
+			    offsetof(struct qed_iwarp_ep_memory, async_output);
+	DMA_REGPAIR_LE(p_tcp_ramrod->iwarp.async_eqe_output_buf,
+		       async_output_phys);
+
+	p_tcp_ramrod->iwarp.handle_for_async.hi = cpu_to_le32(PTR_HI(ep));
+	p_tcp_ramrod->iwarp.handle_for_async.lo = cpu_to_le32(PTR_LO(ep));
+
+	physical_q = qed_get_cm_pq_idx(p_hwfn, PQ_FLAGS_OFLD);
+	p_tcp_ramrod->iwarp.physical_q0 = cpu_to_le16(physical_q);
+	physical_q = qed_get_cm_pq_idx(p_hwfn, PQ_FLAGS_ACK);
+	p_tcp_ramrod->iwarp.physical_q1 = cpu_to_le16(physical_q);
+	p_tcp_ramrod->iwarp.mpa_mode = iwarp_info->mpa_rev;
+
+	tcp = &p_tcp_ramrod->tcp;
+	qed_set_fw_mac_addr(&tcp->remote_mac_addr_hi,
+			    &tcp->remote_mac_addr_mid,
+			    &tcp->remote_mac_addr_lo, ep->remote_mac_addr);
+	qed_set_fw_mac_addr(&tcp->local_mac_addr_hi, &tcp->local_mac_addr_mid,
+			    &tcp->local_mac_addr_lo, ep->local_mac_addr);
+
+	tcp->vlan_id = cpu_to_le16(ep->cm_info.vlan);
+
+	tcp_flags = p_hwfn->p_rdma_info->iwarp.tcp_flags;
+	tcp->flags = 0;
+	SET_FIELD(tcp->flags, TCP_OFFLOAD_PARAMS_OPT2_TS_EN,
+		  !!(tcp_flags & QED_IWARP_TS_EN));
+
+	SET_FIELD(tcp->flags, TCP_OFFLOAD_PARAMS_OPT2_DA_EN,
+		  !!(tcp_flags & QED_IWARP_DA_EN));
+
+	tcp->ip_version = ep->cm_info.ip_version;
+
+	for (i = 0; i < 4; i++) {
+		tcp->remote_ip[i] = cpu_to_le32(ep->cm_info.remote_ip[i]);
+		tcp->local_ip[i] = cpu_to_le32(ep->cm_info.local_ip[i]);
+	}
+
+	tcp->remote_port = cpu_to_le16(ep->cm_info.remote_port);
+	tcp->local_port = cpu_to_le16(ep->cm_info.local_port);
+	tcp->mss = cpu_to_le16(ep->mss);
+	tcp->flow_label = 0;
+	tcp->ttl = 0x40;
+	tcp->tos_or_tc = 0;
+
+	tcp->rcv_wnd_scale = (u8)p_hwfn->p_rdma_info->iwarp.rcv_wnd_scale;
+	tcp->connect_mode = ep->connect_mode;
+
+	if (ep->connect_mode == TCP_CONNECT_PASSIVE) {
+		tcp->syn_ip_payload_length =
+			cpu_to_le16(ep->syn_ip_payload_length);
+		tcp->syn_phy_addr_hi = DMA_HI_LE(ep->syn_phy_addr);
+		tcp->syn_phy_addr_lo = DMA_LO_LE(ep->syn_phy_addr);
+	}
+
+	qed_iwarp_print_tcp_ramrod(p_hwfn, p_tcp_ramrod);
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+		   "EP(0x%x) Offload completed rc=%d\n", ep->tcp_cid, rc);
+
+	return rc;
+}
+
+static void
+qed_iwarp_mpa_received(struct qed_hwfn *p_hwfn, struct qed_iwarp_ep *ep)
+{
+	struct qed_iwarp_info *iwarp_info = &p_hwfn->p_rdma_info->iwarp;
+	struct qed_iwarp_cm_event_params params;
+	struct mpa_v2_hdr *mpa_v2;
+	union async_output *async_data;
+	u16 mpa_ord, mpa_ird;
+	u8 mpa_hdr_size = 0;
+	u8 mpa_rev;
+
+	async_data = &ep->ep_buffer_virt->async_output;
+
+	mpa_rev = async_data->mpa_request.mpa_handshake_mode;
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+		   "private_data_len=%x handshake_mode=%x private_data=(%x)\n",
+		   async_data->mpa_request.ulp_data_len,
+		   mpa_rev, *((u32 *)(ep->ep_buffer_virt->in_pdata)));
+
+	if (mpa_rev == MPA_NEGOTIATION_TYPE_ENHANCED) {
+		/* Read ord/ird values from private data buffer */
+		mpa_v2 = (struct mpa_v2_hdr *)ep->ep_buffer_virt->in_pdata;
+		mpa_hdr_size = sizeof(*mpa_v2);
+
+		mpa_ord = ntohs(mpa_v2->ord);
+		mpa_ird = ntohs(mpa_v2->ird);
+
+		/* Temprary store in cm_info incoming ord/ird requested, later
+		 * replace with negotiated value during accept
+		 */
+		ep->cm_info.ord = (u8)min_t(u16,
+					    (mpa_ord & MPA_V2_IRD_ORD_MASK),
+					    QED_IWARP_ORD_DEFAULT);
+
+		ep->cm_info.ird = (u8)min_t(u16,
+					    (mpa_ird & MPA_V2_IRD_ORD_MASK),
+					    QED_IWARP_IRD_DEFAULT);
+
+		/* Peer2Peer negotiation */
+		ep->rtr_type = MPA_RTR_TYPE_NONE;
+		if (mpa_ird & MPA_V2_PEER2PEER_MODEL) {
+			if (mpa_ord & MPA_V2_WRITE_RTR)
+				ep->rtr_type |= MPA_RTR_TYPE_ZERO_WRITE;
+
+			if (mpa_ord & MPA_V2_READ_RTR)
+				ep->rtr_type |= MPA_RTR_TYPE_ZERO_READ;
+
+			if (mpa_ird & MPA_V2_SEND_RTR)
+				ep->rtr_type |= MPA_RTR_TYPE_ZERO_SEND;
+
+			ep->rtr_type &= iwarp_info->rtr_type;
+
+			/* if we're left with no match send our capabilities */
+			if (ep->rtr_type == MPA_RTR_TYPE_NONE)
+				ep->rtr_type = iwarp_info->rtr_type;
+		}
+
+		ep->mpa_rev = MPA_NEGOTIATION_TYPE_ENHANCED;
+	} else {
+		ep->cm_info.ord = QED_IWARP_ORD_DEFAULT;
+		ep->cm_info.ird = QED_IWARP_IRD_DEFAULT;
+		ep->mpa_rev = MPA_NEGOTIATION_TYPE_BASIC;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+		   "MPA_NEGOTIATE (v%d): ORD: 0x%x IRD: 0x%x rtr:0x%x ulp_data_len = %x mpa_hdr_size = %x\n",
+		   mpa_rev, ep->cm_info.ord, ep->cm_info.ird, ep->rtr_type,
+		   async_data->mpa_request.ulp_data_len, mpa_hdr_size);
+
+	/* Strip mpa v2 hdr from private data before sending to upper layer */
+	ep->cm_info.private_data = ep->ep_buffer_virt->in_pdata + mpa_hdr_size;
+
+	ep->cm_info.private_data_len = async_data->mpa_request.ulp_data_len -
+				       mpa_hdr_size;
+
+	params.event = QED_IWARP_EVENT_MPA_REQUEST;
+	params.cm_info = &ep->cm_info;
+	params.ep_context = ep;
+	params.status = 0;
+
+	ep->state = QED_IWARP_EP_MPA_REQ_RCVD;
+	ep->event_cb(ep->cb_context, &params);
+}
+
+static int
+qed_iwarp_mpa_offload(struct qed_hwfn *p_hwfn, struct qed_iwarp_ep *ep)
+{
+	struct iwarp_mpa_offload_ramrod_data *p_mpa_ramrod;
+	struct qed_sp_init_data init_data;
+	dma_addr_t async_output_phys;
+	struct qed_spq_entry *p_ent;
+	dma_addr_t out_pdata_phys;
+	dma_addr_t in_pdata_phys;
+	struct qed_rdma_qp *qp;
+	bool reject;
+	int rc;
+
+	if (!ep)
+		return -EINVAL;
+
+	qp = ep->qp;
+	reject = !qp;
+
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = reject ? ep->tcp_cid : qp->icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+
+	if (ep->connect_mode == TCP_CONNECT_ACTIVE)
+		init_data.comp_mode = QED_SPQ_MODE_CB;
+	else
+		init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 IWARP_RAMROD_CMD_ID_MPA_OFFLOAD,
+				 PROTOCOLID_IWARP, &init_data);
+	if (rc)
+		return rc;
+
+	p_mpa_ramrod = &p_ent->ramrod.iwarp_mpa_offload;
+	out_pdata_phys = ep->ep_buffer_phys +
+			 offsetof(struct qed_iwarp_ep_memory, out_pdata);
+	DMA_REGPAIR_LE(p_mpa_ramrod->common.outgoing_ulp_buffer.addr,
+		       out_pdata_phys);
+	p_mpa_ramrod->common.outgoing_ulp_buffer.len =
+	    ep->cm_info.private_data_len;
+	p_mpa_ramrod->common.crc_needed = p_hwfn->p_rdma_info->iwarp.crc_needed;
+
+	p_mpa_ramrod->common.out_rq.ord = ep->cm_info.ord;
+	p_mpa_ramrod->common.out_rq.ird = ep->cm_info.ird;
+
+	p_mpa_ramrod->tcp_cid = p_hwfn->hw_info.opaque_fid << 16 | ep->tcp_cid;
+
+	in_pdata_phys = ep->ep_buffer_phys +
+			offsetof(struct qed_iwarp_ep_memory, in_pdata);
+	p_mpa_ramrod->tcp_connect_side = ep->connect_mode;
+	DMA_REGPAIR_LE(p_mpa_ramrod->incoming_ulp_buffer.addr,
+		       in_pdata_phys);
+	p_mpa_ramrod->incoming_ulp_buffer.len =
+	    cpu_to_le16(sizeof(ep->ep_buffer_virt->in_pdata));
+	async_output_phys = ep->ep_buffer_phys +
+			    offsetof(struct qed_iwarp_ep_memory, async_output);
+	DMA_REGPAIR_LE(p_mpa_ramrod->async_eqe_output_buf,
+		       async_output_phys);
+	p_mpa_ramrod->handle_for_async.hi = cpu_to_le32(PTR_HI(ep));
+	p_mpa_ramrod->handle_for_async.lo = cpu_to_le32(PTR_LO(ep));
+
+	if (!reject) {
+		DMA_REGPAIR_LE(p_mpa_ramrod->shared_queue_addr,
+			       qp->shared_queue_phys_addr);
+		p_mpa_ramrod->stats_counter_id =
+		    RESC_START(p_hwfn, QED_RDMA_STATS_QUEUE) + qp->stats_queue;
+	} else {
+		p_mpa_ramrod->common.reject = 1;
+	}
+
+	p_mpa_ramrod->mode = ep->mpa_rev;
+	SET_FIELD(p_mpa_ramrod->rtr_pref,
+		  IWARP_MPA_OFFLOAD_RAMROD_DATA_RTR_SUPPORTED, ep->rtr_type);
+
+	ep->state = QED_IWARP_EP_MPA_OFFLOADED;
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+	if (!reject)
+		ep->cid = qp->icid;	/* Now they're migrated. */
+
+	DP_VERBOSE(p_hwfn,
+		   QED_MSG_RDMA,
+		   "QP(0x%x) EP(0x%x) MPA Offload rc = %d IRD=0x%x ORD=0x%x rtr_type=%d mpa_rev=%d reject=%d\n",
+		   reject ? 0xffff : qp->icid,
+		   ep->tcp_cid,
+		   rc,
+		   ep->cm_info.ird,
+		   ep->cm_info.ord, ep->rtr_type, ep->mpa_rev, reject);
+	return rc;
+}
+
+static void
+qed_iwarp_return_ep(struct qed_hwfn *p_hwfn, struct qed_iwarp_ep *ep)
+{
+	ep->state = QED_IWARP_EP_INIT;
+	if (ep->qp)
+		ep->qp->ep = NULL;
+	ep->qp = NULL;
+	memset(&ep->cm_info, 0, sizeof(ep->cm_info));
+
+	if (ep->tcp_cid == QED_IWARP_INVALID_TCP_CID) {
+		/* We don't care about the return code, it's ok if tcp_cid
+		 * remains invalid...in this case we'll defer allocation
+		 */
+		qed_iwarp_alloc_tcp_cid(p_hwfn, &ep->tcp_cid);
+	}
+	spin_lock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+
+	list_del(&ep->list_entry);
+	list_add_tail(&ep->list_entry,
+		      &p_hwfn->p_rdma_info->iwarp.ep_free_list);
+
+	spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+}
+
+void
+qed_iwarp_parse_private_data(struct qed_hwfn *p_hwfn, struct qed_iwarp_ep *ep)
+{
+	struct mpa_v2_hdr *mpa_v2_params;
+	union async_output *async_data;
+	u16 mpa_ird, mpa_ord;
+	u8 mpa_data_size = 0;
+
+	if (MPA_REV2(p_hwfn->p_rdma_info->iwarp.mpa_rev)) {
+		mpa_v2_params =
+			(struct mpa_v2_hdr *)(ep->ep_buffer_virt->in_pdata);
+		mpa_data_size = sizeof(*mpa_v2_params);
+		mpa_ird = ntohs(mpa_v2_params->ird);
+		mpa_ord = ntohs(mpa_v2_params->ord);
+
+		ep->cm_info.ird = (u8)(mpa_ord & MPA_V2_IRD_ORD_MASK);
+		ep->cm_info.ord = (u8)(mpa_ird & MPA_V2_IRD_ORD_MASK);
+	}
+	async_data = &ep->ep_buffer_virt->async_output;
+
+	ep->cm_info.private_data = ep->ep_buffer_virt->in_pdata + mpa_data_size;
+	ep->cm_info.private_data_len = async_data->mpa_response.ulp_data_len -
+				       mpa_data_size;
+}
+
+void
+qed_iwarp_mpa_reply_arrived(struct qed_hwfn *p_hwfn, struct qed_iwarp_ep *ep)
+{
+	struct qed_iwarp_cm_event_params params;
+
+	if (ep->connect_mode == TCP_CONNECT_PASSIVE) {
+		DP_NOTICE(p_hwfn,
+			  "MPA reply event not expected on passive side!\n");
+		return;
+	}
+
+	params.event = QED_IWARP_EVENT_ACTIVE_MPA_REPLY;
+
+	qed_iwarp_parse_private_data(p_hwfn, ep);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+		   "MPA_NEGOTIATE (v%d): ORD: 0x%x IRD: 0x%x\n",
+		   ep->mpa_rev, ep->cm_info.ord, ep->cm_info.ird);
+
+	params.cm_info = &ep->cm_info;
+	params.ep_context = ep;
+	params.status = 0;
+
+	ep->mpa_reply_processed = true;
+
+	ep->event_cb(ep->cb_context, &params);
+}
+
+#define QED_IWARP_CONNECT_MODE_STRING(ep) \
+	((ep)->connect_mode == TCP_CONNECT_PASSIVE) ? "Passive" : "Active"
+
+/* Called as a result of the event:
+ * IWARP_EVENT_TYPE_ASYNC_MPA_HANDSHAKE_COMPLETE
+ */
+static void
+qed_iwarp_mpa_complete(struct qed_hwfn *p_hwfn,
+		       struct qed_iwarp_ep *ep, u8 fw_return_code)
+{
+	struct qed_iwarp_cm_event_params params;
+
+	if (ep->connect_mode == TCP_CONNECT_ACTIVE)
+		params.event = QED_IWARP_EVENT_ACTIVE_COMPLETE;
+	else
+		params.event = QED_IWARP_EVENT_PASSIVE_COMPLETE;
+
+	if (ep->connect_mode == TCP_CONNECT_ACTIVE && !ep->mpa_reply_processed)
+		qed_iwarp_parse_private_data(p_hwfn, ep);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+		   "MPA_NEGOTIATE (v%d): ORD: 0x%x IRD: 0x%x\n",
+		   ep->mpa_rev, ep->cm_info.ord, ep->cm_info.ird);
+
+	params.cm_info = &ep->cm_info;
+
+	params.ep_context = ep;
+
+	ep->state = QED_IWARP_EP_CLOSED;
+
+	switch (fw_return_code) {
+	case RDMA_RETURN_OK:
+		ep->qp->max_rd_atomic_req = ep->cm_info.ord;
+		ep->qp->max_rd_atomic_resp = ep->cm_info.ird;
+		qed_iwarp_modify_qp(p_hwfn, ep->qp, QED_IWARP_QP_STATE_RTS, 1);
+		ep->state = QED_IWARP_EP_ESTABLISHED;
+		params.status = 0;
+		break;
+	case IWARP_CONN_ERROR_MPA_TIMEOUT:
+		DP_NOTICE(p_hwfn, "%s(0x%x) MPA timeout\n",
+			  QED_IWARP_CONNECT_MODE_STRING(ep), ep->cid);
+		params.status = -EBUSY;
+		break;
+	case IWARP_CONN_ERROR_MPA_ERROR_REJECT:
+		DP_NOTICE(p_hwfn, "%s(0x%x) MPA Reject\n",
+			  QED_IWARP_CONNECT_MODE_STRING(ep), ep->cid);
+		params.status = -ECONNREFUSED;
+		break;
+	case IWARP_CONN_ERROR_MPA_RST:
+		DP_NOTICE(p_hwfn, "%s(0x%x) MPA reset(tcp cid: 0x%x)\n",
+			  QED_IWARP_CONNECT_MODE_STRING(ep), ep->cid,
+			  ep->tcp_cid);
+		params.status = -ECONNRESET;
+		break;
+	case IWARP_CONN_ERROR_MPA_FIN:
+		DP_NOTICE(p_hwfn, "%s(0x%x) MPA received FIN\n",
+			  QED_IWARP_CONNECT_MODE_STRING(ep), ep->cid);
+		params.status = -ECONNREFUSED;
+		break;
+	case IWARP_CONN_ERROR_MPA_INSUF_IRD:
+		DP_NOTICE(p_hwfn, "%s(0x%x) MPA insufficient ird\n",
+			  QED_IWARP_CONNECT_MODE_STRING(ep), ep->cid);
+		params.status = -ECONNREFUSED;
+		break;
+	case IWARP_CONN_ERROR_MPA_RTR_MISMATCH:
+		DP_NOTICE(p_hwfn, "%s(0x%x) MPA RTR MISMATCH\n",
+			  QED_IWARP_CONNECT_MODE_STRING(ep), ep->cid);
+		params.status = -ECONNREFUSED;
+		break;
+	case IWARP_CONN_ERROR_MPA_INVALID_PACKET:
+		DP_NOTICE(p_hwfn, "%s(0x%x) MPA Invalid Packet\n",
+			  QED_IWARP_CONNECT_MODE_STRING(ep), ep->cid);
+		params.status = -ECONNREFUSED;
+		break;
+	case IWARP_CONN_ERROR_MPA_LOCAL_ERROR:
+		DP_NOTICE(p_hwfn, "%s(0x%x) MPA Local Error\n",
+			  QED_IWARP_CONNECT_MODE_STRING(ep), ep->cid);
+		params.status = -ECONNREFUSED;
+		break;
+	case IWARP_CONN_ERROR_MPA_TERMINATE:
+		DP_NOTICE(p_hwfn, "%s(0x%x) MPA TERMINATE\n",
+			  QED_IWARP_CONNECT_MODE_STRING(ep), ep->cid);
+		params.status = -ECONNREFUSED;
+		break;
+	default:
+		params.status = -ECONNRESET;
+		break;
+	}
+
+	ep->event_cb(ep->cb_context, &params);
+
+	/* on passive side, if there is no associated QP (REJECT) we need to
+	 * return the ep to the pool, (in the regular case we add an element
+	 * in accept instead of this one.
+	 * In both cases we need to remove it from the ep_list.
+	 */
+	if (fw_return_code != RDMA_RETURN_OK) {
+		ep->tcp_cid = QED_IWARP_INVALID_TCP_CID;
+		if ((ep->connect_mode == TCP_CONNECT_PASSIVE) &&
+		    (!ep->qp)) {	/* Rejected */
+			qed_iwarp_return_ep(p_hwfn, ep);
+		} else {
+			spin_lock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+			list_del(&ep->list_entry);
+			spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+		}
+	}
+}
+
+static void
+qed_iwarp_mpa_v2_set_private(struct qed_hwfn *p_hwfn,
+			     struct qed_iwarp_ep *ep, u8 *mpa_data_size)
+{
+	struct mpa_v2_hdr *mpa_v2_params;
+	u16 mpa_ird, mpa_ord;
+
+	*mpa_data_size = 0;
+	if (MPA_REV2(ep->mpa_rev)) {
+		mpa_v2_params =
+		    (struct mpa_v2_hdr *)ep->ep_buffer_virt->out_pdata;
+		*mpa_data_size = sizeof(*mpa_v2_params);
+
+		mpa_ird = (u16)ep->cm_info.ird;
+		mpa_ord = (u16)ep->cm_info.ord;
+
+		if (ep->rtr_type != MPA_RTR_TYPE_NONE) {
+			mpa_ird |= MPA_V2_PEER2PEER_MODEL;
+
+			if (ep->rtr_type & MPA_RTR_TYPE_ZERO_SEND)
+				mpa_ird |= MPA_V2_SEND_RTR;
+
+			if (ep->rtr_type & MPA_RTR_TYPE_ZERO_WRITE)
+				mpa_ord |= MPA_V2_WRITE_RTR;
+
+			if (ep->rtr_type & MPA_RTR_TYPE_ZERO_READ)
+				mpa_ord |= MPA_V2_READ_RTR;
+		}
+
+		mpa_v2_params->ird = htons(mpa_ird);
+		mpa_v2_params->ord = htons(mpa_ord);
+
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_RDMA,
+			   "MPA_NEGOTIATE Header: [%x ord:%x ird] %x ord:%x ird:%x peer2peer:%x rtr_send:%x rtr_write:%x rtr_read:%x\n",
+			   mpa_v2_params->ird,
+			   mpa_v2_params->ord,
+			   *((u32 *)mpa_v2_params),
+			   mpa_ord & MPA_V2_IRD_ORD_MASK,
+			   mpa_ird & MPA_V2_IRD_ORD_MASK,
+			   !!(mpa_ird & MPA_V2_PEER2PEER_MODEL),
+			   !!(mpa_ird & MPA_V2_SEND_RTR),
+			   !!(mpa_ord & MPA_V2_WRITE_RTR),
+			   !!(mpa_ord & MPA_V2_READ_RTR));
+	}
+}
+
+int qed_iwarp_connect(void *rdma_cxt,
+		      struct qed_iwarp_connect_in *iparams,
+		      struct qed_iwarp_connect_out *oparams)
+{
+	struct qed_hwfn *p_hwfn = rdma_cxt;
+	struct qed_iwarp_info *iwarp_info;
+	struct qed_iwarp_ep *ep;
+	u8 mpa_data_size = 0;
+	u8 ts_hdr_size = 0;
+	u32 cid;
+	int rc;
+
+	if ((iparams->cm_info.ord > QED_IWARP_ORD_DEFAULT) ||
+	    (iparams->cm_info.ird > QED_IWARP_IRD_DEFAULT)) {
+		DP_NOTICE(p_hwfn,
+			  "QP(0x%x) ERROR: Invalid ord(0x%x)/ird(0x%x)\n",
+			  iparams->qp->icid, iparams->cm_info.ord,
+			  iparams->cm_info.ird);
+
+		return -EINVAL;
+	}
+
+	iwarp_info = &p_hwfn->p_rdma_info->iwarp;
+
+	/* Allocate ep object */
+	rc = qed_iwarp_alloc_cid(p_hwfn, &cid);
+	if (rc)
+		return rc;
+
+	rc = qed_iwarp_create_ep(p_hwfn, &ep);
+	if (rc)
+		goto err;
+
+	ep->tcp_cid = cid;
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+	list_add_tail(&ep->list_entry, &p_hwfn->p_rdma_info->iwarp.ep_list);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+
+	ep->qp = iparams->qp;
+	ep->qp->ep = ep;
+	ether_addr_copy(ep->remote_mac_addr, iparams->remote_mac_addr);
+	ether_addr_copy(ep->local_mac_addr, iparams->local_mac_addr);
+	memcpy(&ep->cm_info, &iparams->cm_info, sizeof(ep->cm_info));
+
+	ep->cm_info.ord = iparams->cm_info.ord;
+	ep->cm_info.ird = iparams->cm_info.ird;
+
+	ep->rtr_type = iwarp_info->rtr_type;
+	if (!iwarp_info->peer2peer)
+		ep->rtr_type = MPA_RTR_TYPE_NONE;
+
+	if ((ep->rtr_type & MPA_RTR_TYPE_ZERO_READ) && (ep->cm_info.ord == 0))
+		ep->cm_info.ord = 1;
+
+	ep->mpa_rev = iwarp_info->mpa_rev;
+
+	qed_iwarp_mpa_v2_set_private(p_hwfn, ep, &mpa_data_size);
+
+	ep->cm_info.private_data = ep->ep_buffer_virt->out_pdata;
+	ep->cm_info.private_data_len = iparams->cm_info.private_data_len +
+				       mpa_data_size;
+
+	memcpy((u8 *)ep->ep_buffer_virt->out_pdata + mpa_data_size,
+	       iparams->cm_info.private_data,
+	       iparams->cm_info.private_data_len);
+
+	if (p_hwfn->p_rdma_info->iwarp.tcp_flags & QED_IWARP_TS_EN)
+		ts_hdr_size = TIMESTAMP_HEADER_SIZE;
+
+	ep->mss = iparams->mss - ts_hdr_size;
+	ep->mss = min_t(u16, QED_IWARP_MAX_FW_MSS, ep->mss);
+
+	ep->event_cb = iparams->event_cb;
+	ep->cb_context = iparams->cb_context;
+	ep->connect_mode = TCP_CONNECT_ACTIVE;
+
+	oparams->ep_context = ep;
+
+	rc = qed_iwarp_tcp_offload(p_hwfn, ep);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "QP(0x%x) EP(0x%x) rc = %d\n",
+		   iparams->qp->icid, ep->tcp_cid, rc);
+
+	if (rc) {
+		qed_iwarp_destroy_ep(p_hwfn, ep, true);
+		goto err;
+	}
+
+	return rc;
+err:
+	qed_iwarp_cid_cleaned(p_hwfn, cid);
+
+	return rc;
+}
+
+static struct qed_iwarp_ep *qed_iwarp_get_free_ep(struct qed_hwfn *p_hwfn)
+{
+	struct qed_iwarp_ep *ep = NULL;
+	int rc;
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+
+	if (list_empty(&p_hwfn->p_rdma_info->iwarp.ep_free_list)) {
+		DP_ERR(p_hwfn, "Ep list is empty\n");
+		goto out;
+	}
+
+	ep = list_first_entry(&p_hwfn->p_rdma_info->iwarp.ep_free_list,
+			      struct qed_iwarp_ep, list_entry);
+
+	/* in some cases we could have failed allocating a tcp cid when added
+	 * from accept / failure... retry now..this is not the common case.
+	 */
+	if (ep->tcp_cid == QED_IWARP_INVALID_TCP_CID) {
+		rc = qed_iwarp_alloc_tcp_cid(p_hwfn, &ep->tcp_cid);
+
+		/* if we fail we could look for another entry with a valid
+		 * tcp_cid, but since we don't expect to reach this anyway
+		 * it's not worth the handling
+		 */
+		if (rc) {
+			ep->tcp_cid = QED_IWARP_INVALID_TCP_CID;
+			ep = NULL;
+			goto out;
+		}
+	}
+
+	list_del(&ep->list_entry);
+
+out:
+	spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+	return ep;
+}
+
+#define QED_IWARP_MAX_CID_CLEAN_TIME  100
+#define QED_IWARP_MAX_NO_PROGRESS_CNT 5
+
+/* This function waits for all the bits of a bmap to be cleared, as long as
+ * there is progress ( i.e. the number of bits left to be cleared decreases )
+ * the function continues.
+ */
+static int
+qed_iwarp_wait_cid_map_cleared(struct qed_hwfn *p_hwfn, struct qed_bmap *bmap)
+{
+	int prev_weight = 0;
+	int wait_count = 0;
+	int weight = 0;
+
+	weight = bitmap_weight(bmap->bitmap, bmap->max_count);
+	prev_weight = weight;
+
+	while (weight) {
+		msleep(QED_IWARP_MAX_CID_CLEAN_TIME);
+
+		weight = bitmap_weight(bmap->bitmap, bmap->max_count);
+
+		if (prev_weight == weight) {
+			wait_count++;
+		} else {
+			prev_weight = weight;
+			wait_count = 0;
+		}
+
+		if (wait_count > QED_IWARP_MAX_NO_PROGRESS_CNT) {
+			DP_NOTICE(p_hwfn,
+				  "%s bitmap wait timed out (%d cids pending)\n",
+				  bmap->name, weight);
+			return -EBUSY;
+		}
+	}
+	return 0;
+}
+
+static int qed_iwarp_wait_for_all_cids(struct qed_hwfn *p_hwfn)
+{
+	int rc;
+	int i;
+
+	rc = qed_iwarp_wait_cid_map_cleared(p_hwfn,
+					    &p_hwfn->p_rdma_info->tcp_cid_map);
+	if (rc)
+		return rc;
+
+	/* Now free the tcp cids from the main cid map */
+	for (i = 0; i < QED_IWARP_PREALLOC_CNT; i++)
+		qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->cid_map, i);
+
+	/* Now wait for all cids to be completed */
+	return qed_iwarp_wait_cid_map_cleared(p_hwfn,
+					      &p_hwfn->p_rdma_info->cid_map);
+}
+
+static void qed_iwarp_free_prealloc_ep(struct qed_hwfn *p_hwfn)
+{
+	struct qed_iwarp_ep *ep;
+
+	while (!list_empty(&p_hwfn->p_rdma_info->iwarp.ep_free_list)) {
+		spin_lock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+
+		ep = list_first_entry(&p_hwfn->p_rdma_info->iwarp.ep_free_list,
+				      struct qed_iwarp_ep, list_entry);
+
+		if (!ep) {
+			spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+			break;
+		}
+		list_del(&ep->list_entry);
+
+		spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+
+		if (ep->tcp_cid != QED_IWARP_INVALID_TCP_CID)
+			qed_iwarp_cid_cleaned(p_hwfn, ep->tcp_cid);
+
+		qed_iwarp_destroy_ep(p_hwfn, ep, false);
+	}
+}
+
+static int qed_iwarp_prealloc_ep(struct qed_hwfn *p_hwfn, bool init)
+{
+	struct qed_iwarp_ep *ep;
+	int rc = 0;
+	int count;
+	u32 cid;
+	int i;
+
+	count = init ? QED_IWARP_PREALLOC_CNT : 1;
+	for (i = 0; i < count; i++) {
+		rc = qed_iwarp_create_ep(p_hwfn, &ep);
+		if (rc)
+			return rc;
+
+		/* During initialization we allocate from the main pool,
+		 * afterwards we allocate only from the tcp_cid.
+		 */
+		if (init) {
+			rc = qed_iwarp_alloc_cid(p_hwfn, &cid);
+			if (rc)
+				goto err;
+			qed_iwarp_set_tcp_cid(p_hwfn, cid);
+		} else {
+			/* We don't care about the return code, it's ok if
+			 * tcp_cid remains invalid...in this case we'll
+			 * defer allocation
+			 */
+			qed_iwarp_alloc_tcp_cid(p_hwfn, &cid);
+		}
+
+		ep->tcp_cid = cid;
+
+		spin_lock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+		list_add_tail(&ep->list_entry,
+			      &p_hwfn->p_rdma_info->iwarp.ep_free_list);
+		spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+	}
+
+	return rc;
+
+err:
+	qed_iwarp_destroy_ep(p_hwfn, ep, false);
+
+	return rc;
+}
+
+int qed_iwarp_alloc(struct qed_hwfn *p_hwfn)
+{
+	int rc;
+
+	/* Allocate bitmap for tcp cid. These are used by passive side
+	 * to ensure it can allocate a tcp cid during dpc that was
+	 * pre-acquired and doesn't require dynamic allocation of ilt
+	 */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_hwfn->p_rdma_info->tcp_cid_map,
+				 QED_IWARP_PREALLOC_CNT, "TCP_CID");
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate tcp cid, rc = %d\n", rc);
+		return rc;
+	}
+
+	INIT_LIST_HEAD(&p_hwfn->p_rdma_info->iwarp.ep_free_list);
+	spin_lock_init(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+
+	return qed_iwarp_prealloc_ep(p_hwfn, true);
+}
+
+void qed_iwarp_resc_free(struct qed_hwfn *p_hwfn)
+{
+	qed_rdma_bmap_free(p_hwfn, &p_hwfn->p_rdma_info->tcp_cid_map, 1);
+}
+
+int qed_iwarp_accept(void *rdma_cxt, struct qed_iwarp_accept_in *iparams)
+{
+	struct qed_hwfn *p_hwfn = rdma_cxt;
+	struct qed_iwarp_ep *ep;
+	u8 mpa_data_size = 0;
+	int rc;
+
+	ep = iparams->ep_context;
+	if (!ep) {
+		DP_ERR(p_hwfn, "Ep Context receive in accept is NULL\n");
+		return -EINVAL;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "QP(0x%x) EP(0x%x)\n",
+		   iparams->qp->icid, ep->tcp_cid);
+
+	if ((iparams->ord > QED_IWARP_ORD_DEFAULT) ||
+	    (iparams->ird > QED_IWARP_IRD_DEFAULT)) {
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_RDMA,
+			   "QP(0x%x) EP(0x%x) ERROR: Invalid ord(0x%x)/ird(0x%x)\n",
+			   iparams->qp->icid,
+			   ep->tcp_cid, iparams->ord, iparams->ord);
+		return -EINVAL;
+	}
+
+	qed_iwarp_prealloc_ep(p_hwfn, false);
+
+	ep->cb_context = iparams->cb_context;
+	ep->qp = iparams->qp;
+	ep->qp->ep = ep;
+
+	if (ep->mpa_rev == MPA_NEGOTIATION_TYPE_ENHANCED) {
+		/* Negotiate ord/ird: if upperlayer requested ord larger than
+		 * ird advertised by remote, we need to decrease our ord
+		 */
+		if (iparams->ord > ep->cm_info.ird)
+			iparams->ord = ep->cm_info.ird;
+
+		if ((ep->rtr_type & MPA_RTR_TYPE_ZERO_READ) &&
+		    (iparams->ird == 0))
+			iparams->ird = 1;
+	}
+
+	/* Update cm_info ord/ird to be negotiated values */
+	ep->cm_info.ord = iparams->ord;
+	ep->cm_info.ird = iparams->ird;
+
+	qed_iwarp_mpa_v2_set_private(p_hwfn, ep, &mpa_data_size);
+
+	ep->cm_info.private_data = ep->ep_buffer_virt->out_pdata;
+	ep->cm_info.private_data_len = iparams->private_data_len +
+				       mpa_data_size;
+
+	memcpy((u8 *)ep->ep_buffer_virt->out_pdata + mpa_data_size,
+	       iparams->private_data, iparams->private_data_len);
+
+	rc = qed_iwarp_mpa_offload(p_hwfn, ep);
+	if (rc)
+		qed_iwarp_modify_qp(p_hwfn,
+				    iparams->qp, QED_IWARP_QP_STATE_ERROR, 1);
+
+	return rc;
+}
+
+int qed_iwarp_reject(void *rdma_cxt, struct qed_iwarp_reject_in *iparams)
+{
+	struct qed_hwfn *p_hwfn = rdma_cxt;
+	struct qed_iwarp_ep *ep;
+	u8 mpa_data_size = 0;
+
+	ep = iparams->ep_context;
+	if (!ep) {
+		DP_ERR(p_hwfn, "Ep Context receive in reject is NULL\n");
+		return -EINVAL;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "EP(0x%x)\n", ep->tcp_cid);
+
+	ep->cb_context = iparams->cb_context;
+	ep->qp = NULL;
+
+	qed_iwarp_mpa_v2_set_private(p_hwfn, ep, &mpa_data_size);
+
+	ep->cm_info.private_data = ep->ep_buffer_virt->out_pdata;
+	ep->cm_info.private_data_len = iparams->private_data_len +
+				       mpa_data_size;
+
+	memcpy((u8 *)ep->ep_buffer_virt->out_pdata + mpa_data_size,
+	       iparams->private_data, iparams->private_data_len);
+
+	return qed_iwarp_mpa_offload(p_hwfn, ep);
+}
+
+static void
+qed_iwarp_print_cm_info(struct qed_hwfn *p_hwfn,
+			struct qed_iwarp_cm_info *cm_info)
+{
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "ip_version = %d\n",
+		   cm_info->ip_version);
+
+	if (cm_info->ip_version == QED_TCP_IPV4)
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "remote_ip %pI4h:%x, local_ip %pI4h:%x vlan=%x\n",
+			   cm_info->remote_ip, cm_info->remote_port,
+			   cm_info->local_ip, cm_info->local_port,
+			   cm_info->vlan);
+	else
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "remote_ip %pI6h:%x, local_ip %pI6h:%x vlan=%x\n",
+			   cm_info->remote_ip, cm_info->remote_port,
+			   cm_info->local_ip, cm_info->local_port,
+			   cm_info->vlan);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+		   "private_data_len = %x ord = %d, ird = %d\n",
+		   cm_info->private_data_len, cm_info->ord, cm_info->ird);
+}
+
+static int
+qed_iwarp_ll2_post_rx(struct qed_hwfn *p_hwfn,
+		      struct qed_iwarp_ll2_buff *buf, u8 handle)
+{
+	int rc;
+
+	rc = qed_ll2_post_rx_buffer(p_hwfn, handle, buf->data_phys_addr,
+				    (u16)buf->buff_size, buf, 1);
+	if (rc) {
+		DP_NOTICE(p_hwfn,
+			  "Failed to repost rx buffer to ll2 rc = %d, handle=%d\n",
+			  rc, handle);
+		dma_free_coherent(&p_hwfn->cdev->pdev->dev, buf->buff_size,
+				  buf->data, buf->data_phys_addr);
+		kfree(buf);
+	}
+
+	return rc;
+}
+
+static bool
+qed_iwarp_ep_exists(struct qed_hwfn *p_hwfn, struct qed_iwarp_cm_info *cm_info)
+{
+	struct qed_iwarp_ep *ep = NULL;
+	bool found = false;
+
+	list_for_each_entry(ep,
+			    &p_hwfn->p_rdma_info->iwarp.ep_list,
+			    list_entry) {
+		if ((ep->cm_info.local_port == cm_info->local_port) &&
+		    (ep->cm_info.remote_port == cm_info->remote_port) &&
+		    (ep->cm_info.vlan == cm_info->vlan) &&
+		    !memcmp(&ep->cm_info.local_ip, cm_info->local_ip,
+			    sizeof(cm_info->local_ip)) &&
+		    !memcmp(&ep->cm_info.remote_ip, cm_info->remote_ip,
+			    sizeof(cm_info->remote_ip))) {
+			found = true;
+			break;
+		}
+	}
+
+	if (found) {
+		DP_NOTICE(p_hwfn,
+			  "SYN received on active connection - dropping\n");
+		qed_iwarp_print_cm_info(p_hwfn, cm_info);
+
+		return true;
+	}
+
+	return false;
+}
+
+static struct qed_iwarp_listener *
+qed_iwarp_get_listener(struct qed_hwfn *p_hwfn,
+		       struct qed_iwarp_cm_info *cm_info)
+{
+	struct qed_iwarp_listener *listener = NULL;
+	static const u32 ip_zero[4] = { 0, 0, 0, 0 };
+	bool found = false;
+
+	qed_iwarp_print_cm_info(p_hwfn, cm_info);
+
+	list_for_each_entry(listener,
+			    &p_hwfn->p_rdma_info->iwarp.listen_list,
+			    list_entry) {
+		if (listener->port == cm_info->local_port) {
+			if (!memcmp(listener->ip_addr,
+				    ip_zero, sizeof(ip_zero))) {
+				found = true;
+				break;
+			}
+
+			if (!memcmp(listener->ip_addr,
+				    cm_info->local_ip,
+				    sizeof(cm_info->local_ip)) &&
+			    (listener->vlan == cm_info->vlan)) {
+				found = true;
+				break;
+			}
+		}
+	}
+
+	if (found) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "listener found = %p\n",
+			   listener);
+		return listener;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "listener not found\n");
+	return NULL;
+}
+
+static int
+qed_iwarp_parse_rx_pkt(struct qed_hwfn *p_hwfn,
+		       struct qed_iwarp_cm_info *cm_info,
+		       void *buf,
+		       u8 *remote_mac_addr,
+		       u8 *local_mac_addr,
+		       int *payload_len, int *tcp_start_offset)
+{
+	struct vlan_ethhdr *vethh;
+	bool vlan_valid = false;
+	struct ipv6hdr *ip6h;
+	struct ethhdr *ethh;
+	struct tcphdr *tcph;
+	struct iphdr *iph;
+	int eth_hlen;
+	int ip_hlen;
+	int eth_type;
+	int i;
+
+	ethh = buf;
+	eth_type = ntohs(ethh->h_proto);
+	if (eth_type == ETH_P_8021Q) {
+		vlan_valid = true;
+		vethh = (struct vlan_ethhdr *)ethh;
+		cm_info->vlan = ntohs(vethh->h_vlan_TCI) & VLAN_VID_MASK;
+		eth_type = ntohs(vethh->h_vlan_encapsulated_proto);
+	}
+
+	eth_hlen = ETH_HLEN + (vlan_valid ? sizeof(u32) : 0);
+
+	ether_addr_copy(remote_mac_addr, ethh->h_source);
+	ether_addr_copy(local_mac_addr, ethh->h_dest);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "eth_type =%d source mac: %pM\n",
+		   eth_type, ethh->h_source);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "eth_hlen=%d destination mac: %pM\n",
+		   eth_hlen, ethh->h_dest);
+
+	iph = (struct iphdr *)((u8 *)(ethh) + eth_hlen);
+
+	if (eth_type == ETH_P_IP) {
+		cm_info->local_ip[0] = ntohl(iph->daddr);
+		cm_info->remote_ip[0] = ntohl(iph->saddr);
+		cm_info->ip_version = TCP_IPV4;
+
+		ip_hlen = (iph->ihl) * sizeof(u32);
+		*payload_len = ntohs(iph->tot_len) - ip_hlen;
+	} else if (eth_type == ETH_P_IPV6) {
+		ip6h = (struct ipv6hdr *)iph;
+		for (i = 0; i < 4; i++) {
+			cm_info->local_ip[i] =
+			    ntohl(ip6h->daddr.in6_u.u6_addr32[i]);
+			cm_info->remote_ip[i] =
+			    ntohl(ip6h->saddr.in6_u.u6_addr32[i]);
+		}
+		cm_info->ip_version = TCP_IPV6;
+
+		ip_hlen = sizeof(*ip6h);
+		*payload_len = ntohs(ip6h->payload_len);
+	} else {
+		DP_NOTICE(p_hwfn, "Unexpected ethertype on ll2 %x\n", eth_type);
+		return -EINVAL;
+	}
+
+	tcph = (struct tcphdr *)((u8 *)iph + ip_hlen);
+
+	if (!tcph->syn) {
+		DP_NOTICE(p_hwfn,
+			  "Only SYN type packet expected on this ll2 conn, iph->ihl=%d source=%d dest=%d\n",
+			  iph->ihl, tcph->source, tcph->dest);
+		return -EINVAL;
+	}
+
+	cm_info->local_port = ntohs(tcph->dest);
+	cm_info->remote_port = ntohs(tcph->source);
+
+	qed_iwarp_print_cm_info(p_hwfn, cm_info);
+
+	*tcp_start_offset = eth_hlen + ip_hlen;
+
+	return 0;
+}
+
+static void
+qed_iwarp_ll2_comp_syn_pkt(void *cxt, struct qed_ll2_comp_rx_data *data)
+{
+	struct qed_iwarp_ll2_buff *buf = data->cookie;
+	struct qed_iwarp_listener *listener;
+	struct qed_ll2_tx_pkt_info tx_pkt;
+	struct qed_iwarp_cm_info cm_info;
+	struct qed_hwfn *p_hwfn = cxt;
+	u8 remote_mac_addr[ETH_ALEN];
+	u8 local_mac_addr[ETH_ALEN];
+	struct qed_iwarp_ep *ep;
+	int tcp_start_offset;
+	u8 ts_hdr_size = 0;
+	u8 ll2_syn_handle;
+	int payload_len;
+	u32 hdr_size;
+	int rc;
+
+	memset(&cm_info, 0, sizeof(cm_info));
+	ll2_syn_handle = p_hwfn->p_rdma_info->iwarp.ll2_syn_handle;
+	if (GET_FIELD(data->parse_flags,
+		      PARSING_AND_ERR_FLAGS_L4CHKSMWASCALCULATED) &&
+	    GET_FIELD(data->parse_flags, PARSING_AND_ERR_FLAGS_L4CHKSMERROR)) {
+		DP_NOTICE(p_hwfn, "Syn packet received with checksum error\n");
+		goto err;
+	}
+
+	rc = qed_iwarp_parse_rx_pkt(p_hwfn, &cm_info, (u8 *)(buf->data) +
+				    data->u.placement_offset, remote_mac_addr,
+				    local_mac_addr, &payload_len,
+				    &tcp_start_offset);
+	if (rc)
+		goto err;
+
+	/* Check if there is a listener for this 4-tuple+vlan */
+	listener = qed_iwarp_get_listener(p_hwfn, &cm_info);
+	if (!listener) {
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_RDMA,
+			   "SYN received on tuple not listened on parse_flags=%d packet len=%d\n",
+			   data->parse_flags, data->length.packet_length);
+
+		memset(&tx_pkt, 0, sizeof(tx_pkt));
+		tx_pkt.num_of_bds = 1;
+		tx_pkt.vlan = data->vlan;
+
+		if (GET_FIELD(data->parse_flags,
+			      PARSING_AND_ERR_FLAGS_TAG8021QEXIST))
+			SET_FIELD(tx_pkt.bd_flags,
+				  CORE_TX_BD_DATA_VLAN_INSERTION, 1);
+
+		tx_pkt.l4_hdr_offset_w = (data->length.packet_length) >> 2;
+		tx_pkt.tx_dest = QED_LL2_TX_DEST_LB;
+		tx_pkt.first_frag = buf->data_phys_addr +
+				    data->u.placement_offset;
+		tx_pkt.first_frag_len = data->length.packet_length;
+		tx_pkt.cookie = buf;
+
+		rc = qed_ll2_prepare_tx_packet(p_hwfn, ll2_syn_handle,
+					       &tx_pkt, true);
+
+		if (rc) {
+			DP_NOTICE(p_hwfn,
+				  "Can't post SYN back to chip rc=%d\n", rc);
+			goto err;
+		}
+		return;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Received syn on listening port\n");
+	/* There may be an open ep on this connection if this is a syn
+	 * retrasnmit... need to make sure there isn't...
+	 */
+	if (qed_iwarp_ep_exists(p_hwfn, &cm_info))
+		goto err;
+
+	ep = qed_iwarp_get_free_ep(p_hwfn);
+	if (!ep)
+		goto err;
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+	list_add_tail(&ep->list_entry, &p_hwfn->p_rdma_info->iwarp.ep_list);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+
+	ether_addr_copy(ep->remote_mac_addr, remote_mac_addr);
+	ether_addr_copy(ep->local_mac_addr, local_mac_addr);
+
+	memcpy(&ep->cm_info, &cm_info, sizeof(ep->cm_info));
+
+	if (p_hwfn->p_rdma_info->iwarp.tcp_flags & QED_IWARP_TS_EN)
+		ts_hdr_size = TIMESTAMP_HEADER_SIZE;
+
+	hdr_size = ((cm_info.ip_version == QED_TCP_IPV4) ? 40 : 60) +
+		   ts_hdr_size;
+	ep->mss = p_hwfn->p_rdma_info->iwarp.max_mtu - hdr_size;
+	ep->mss = min_t(u16, QED_IWARP_MAX_FW_MSS, ep->mss);
+
+	ep->event_cb = listener->event_cb;
+	ep->cb_context = listener->cb_context;
+	ep->connect_mode = TCP_CONNECT_PASSIVE;
+
+	ep->syn = buf;
+	ep->syn_ip_payload_length = (u16)payload_len;
+	ep->syn_phy_addr = buf->data_phys_addr + data->u.placement_offset +
+			   tcp_start_offset;
+
+	rc = qed_iwarp_tcp_offload(p_hwfn, ep);
+	if (rc) {
+		qed_iwarp_return_ep(p_hwfn, ep);
+		goto err;
+	}
+
+	return;
+err:
+	qed_iwarp_ll2_post_rx(p_hwfn, buf, ll2_syn_handle);
+}
+
+static void qed_iwarp_ll2_rel_rx_pkt(void *cxt, u8 connection_handle,
+				     void *cookie, dma_addr_t rx_buf_addr,
+				     bool b_last_packet)
+{
+	struct qed_iwarp_ll2_buff *buffer = cookie;
+	struct qed_hwfn *p_hwfn = cxt;
+
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev, buffer->buff_size,
+			  buffer->data, buffer->data_phys_addr);
+	kfree(buffer);
+}
+
+static void qed_iwarp_ll2_comp_tx_pkt(void *cxt, u8 connection_handle,
+				      void *cookie, dma_addr_t first_frag_addr,
+				      bool b_last_fragment, bool b_last_packet)
+{
+	struct qed_iwarp_ll2_buff *buffer = cookie;
+	struct qed_hwfn *p_hwfn = cxt;
+
+	/* this was originally an rx packet, post it back */
+	qed_iwarp_ll2_post_rx(p_hwfn, buffer, connection_handle);
+}
+
+static void qed_iwarp_ll2_rel_tx_pkt(void *cxt, u8 connection_handle,
+				     void *cookie, dma_addr_t first_frag_addr,
+				     bool b_last_fragment, bool b_last_packet)
+{
+	struct qed_iwarp_ll2_buff *buffer = cookie;
+	struct qed_hwfn *p_hwfn = cxt;
+
+	if (!buffer)
+		return;
+
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev, buffer->buff_size,
+			  buffer->data, buffer->data_phys_addr);
+
+	kfree(buffer);
+}
+
+static int qed_iwarp_ll2_stop(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+{
+	struct qed_iwarp_info *iwarp_info = &p_hwfn->p_rdma_info->iwarp;
+	int rc = 0;
+
+	if (iwarp_info->ll2_syn_handle != QED_IWARP_HANDLE_INVAL) {
+		rc = qed_ll2_terminate_connection(p_hwfn,
+						  iwarp_info->ll2_syn_handle);
+		if (rc)
+			DP_INFO(p_hwfn, "Failed to terminate syn connection\n");
+
+		qed_ll2_release_connection(p_hwfn, iwarp_info->ll2_syn_handle);
+		iwarp_info->ll2_syn_handle = QED_IWARP_HANDLE_INVAL;
+	}
+
+	qed_llh_remove_mac_filter(p_hwfn,
+				  p_ptt, p_hwfn->p_rdma_info->iwarp.mac_addr);
+	return rc;
+}
+
+static int
+qed_iwarp_ll2_alloc_buffers(struct qed_hwfn *p_hwfn,
+			    int num_rx_bufs, int buff_size, u8 ll2_handle)
+{
+	struct qed_iwarp_ll2_buff *buffer;
+	int rc = 0;
+	int i;
+
+	for (i = 0; i < num_rx_bufs; i++) {
+		buffer = kzalloc(sizeof(*buffer), GFP_KERNEL);
+		if (!buffer) {
+			rc = -ENOMEM;
+			break;
+		}
+
+		buffer->data = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
+						  buff_size,
+						  &buffer->data_phys_addr,
+						  GFP_KERNEL);
+		if (!buffer->data) {
+			kfree(buffer);
+			rc = -ENOMEM;
+			break;
+		}
+
+		buffer->buff_size = buff_size;
+		rc = qed_iwarp_ll2_post_rx(p_hwfn, buffer, ll2_handle);
+		if (rc)
+			/* buffers will be deallocated by qed_ll2 */
+			break;
+	}
+	return rc;
+}
+
+#define QED_IWARP_MAX_BUF_SIZE(mtu)				     \
+	ALIGN((mtu) + ETH_HLEN + 2 * VLAN_HLEN + 2 + ETH_CACHE_LINE_SIZE, \
+		ETH_CACHE_LINE_SIZE)
+
+static int
+qed_iwarp_ll2_start(struct qed_hwfn *p_hwfn,
+		    struct qed_rdma_start_in_params *params,
+		    struct qed_ptt *p_ptt)
+{
+	struct qed_iwarp_info *iwarp_info;
+	struct qed_ll2_acquire_data data;
+	struct qed_ll2_cbs cbs;
+	int rc = 0;
+
+	iwarp_info = &p_hwfn->p_rdma_info->iwarp;
+	iwarp_info->ll2_syn_handle = QED_IWARP_HANDLE_INVAL;
+
+	iwarp_info->max_mtu = params->max_mtu;
+
+	ether_addr_copy(p_hwfn->p_rdma_info->iwarp.mac_addr, params->mac_addr);
+
+	rc = qed_llh_add_mac_filter(p_hwfn, p_ptt, params->mac_addr);
+	if (rc)
+		return rc;
+
+	/* Start SYN connection */
+	cbs.rx_comp_cb = qed_iwarp_ll2_comp_syn_pkt;
+	cbs.rx_release_cb = qed_iwarp_ll2_rel_rx_pkt;
+	cbs.tx_comp_cb = qed_iwarp_ll2_comp_tx_pkt;
+	cbs.tx_release_cb = qed_iwarp_ll2_rel_tx_pkt;
+	cbs.cookie = p_hwfn;
+
+	memset(&data, 0, sizeof(data));
+	data.input.conn_type = QED_LL2_TYPE_IWARP;
+	data.input.mtu = QED_IWARP_MAX_SYN_PKT_SIZE;
+	data.input.rx_num_desc = QED_IWARP_LL2_SYN_RX_SIZE;
+	data.input.tx_num_desc = QED_IWARP_LL2_SYN_TX_SIZE;
+	data.input.tx_max_bds_per_packet = 1;	/* will never be fragmented */
+	data.input.tx_tc = PKT_LB_TC;
+	data.input.tx_dest = QED_LL2_TX_DEST_LB;
+	data.p_connection_handle = &iwarp_info->ll2_syn_handle;
+	data.cbs = &cbs;
+
+	rc = qed_ll2_acquire_connection(p_hwfn, &data);
+	if (rc) {
+		DP_NOTICE(p_hwfn, "Failed to acquire LL2 connection\n");
+		qed_llh_remove_mac_filter(p_hwfn, p_ptt, params->mac_addr);
+		return rc;
+	}
+
+	rc = qed_ll2_establish_connection(p_hwfn, iwarp_info->ll2_syn_handle);
+	if (rc) {
+		DP_NOTICE(p_hwfn, "Failed to establish LL2 connection\n");
+		goto err;
+	}
+
+	rc = qed_iwarp_ll2_alloc_buffers(p_hwfn,
+					 QED_IWARP_LL2_SYN_RX_SIZE,
+					 QED_IWARP_MAX_SYN_PKT_SIZE,
+					 iwarp_info->ll2_syn_handle);
+	if (rc)
+		goto err;
+
+	return rc;
+err:
+	qed_iwarp_ll2_stop(p_hwfn, p_ptt);
+
+	return rc;
+}
+
+int qed_iwarp_setup(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
+		    struct qed_rdma_start_in_params *params)
+{
+	struct qed_iwarp_info *iwarp_info;
+	u32 rcv_wnd_size;
+
+	iwarp_info = &p_hwfn->p_rdma_info->iwarp;
+
+	iwarp_info->tcp_flags = QED_IWARP_TS_EN;
+	rcv_wnd_size = QED_IWARP_RCV_WND_SIZE_DEF;
+
+	/* value 0 is used for ilog2(QED_IWARP_RCV_WND_SIZE_MIN) */
+	iwarp_info->rcv_wnd_scale = ilog2(rcv_wnd_size) -
+	    ilog2(QED_IWARP_RCV_WND_SIZE_MIN);
+	iwarp_info->crc_needed = QED_IWARP_PARAM_CRC_NEEDED;
+	iwarp_info->mpa_rev = MPA_NEGOTIATION_TYPE_ENHANCED;
+
+	iwarp_info->peer2peer = QED_IWARP_PARAM_P2P;
+
+	iwarp_info->rtr_type =  MPA_RTR_TYPE_ZERO_SEND |
+				MPA_RTR_TYPE_ZERO_WRITE |
+				MPA_RTR_TYPE_ZERO_READ;
+
+	spin_lock_init(&p_hwfn->p_rdma_info->iwarp.qp_lock);
+	INIT_LIST_HEAD(&p_hwfn->p_rdma_info->iwarp.ep_list);
+	INIT_LIST_HEAD(&p_hwfn->p_rdma_info->iwarp.listen_list);
+
+	qed_spq_register_async_cb(p_hwfn, PROTOCOLID_IWARP,
+				  qed_iwarp_async_event);
+
+	return qed_iwarp_ll2_start(p_hwfn, params, p_ptt);
+}
+
+int qed_iwarp_stop(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+{
+	int rc;
+
+	qed_iwarp_free_prealloc_ep(p_hwfn);
+	rc = qed_iwarp_wait_for_all_cids(p_hwfn);
+	if (rc)
+		return rc;
+
+	qed_spq_unregister_async_cb(p_hwfn, PROTOCOLID_IWARP);
+
+	return qed_iwarp_ll2_stop(p_hwfn, p_ptt);
+}
+
+void qed_iwarp_qp_in_error(struct qed_hwfn *p_hwfn,
+			   struct qed_iwarp_ep *ep, u8 fw_return_code)
+{
+	struct qed_iwarp_cm_event_params params;
+
+	qed_iwarp_modify_qp(p_hwfn, ep->qp, QED_IWARP_QP_STATE_ERROR, true);
+
+	params.event = QED_IWARP_EVENT_CLOSE;
+	params.ep_context = ep;
+	params.cm_info = &ep->cm_info;
+	params.status = (fw_return_code == IWARP_QP_IN_ERROR_GOOD_CLOSE) ?
+			 0 : -ECONNRESET;
+
+	ep->state = QED_IWARP_EP_CLOSED;
+	spin_lock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+	list_del(&ep->list_entry);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+
+	ep->event_cb(ep->cb_context, &params);
+}
+
+void qed_iwarp_exception_received(struct qed_hwfn *p_hwfn,
+				  struct qed_iwarp_ep *ep, int fw_ret_code)
+{
+	struct qed_iwarp_cm_event_params params;
+	bool event_cb = false;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "EP(0x%x) fw_ret_code=%d\n",
+		   ep->cid, fw_ret_code);
+
+	switch (fw_ret_code) {
+	case IWARP_EXCEPTION_DETECTED_LLP_CLOSED:
+		params.status = 0;
+		params.event = QED_IWARP_EVENT_DISCONNECT;
+		event_cb = true;
+		break;
+	case IWARP_EXCEPTION_DETECTED_LLP_RESET:
+		params.status = -ECONNRESET;
+		params.event = QED_IWARP_EVENT_DISCONNECT;
+		event_cb = true;
+		break;
+	case IWARP_EXCEPTION_DETECTED_RQ_EMPTY:
+		params.event = QED_IWARP_EVENT_RQ_EMPTY;
+		event_cb = true;
+		break;
+	case IWARP_EXCEPTION_DETECTED_IRQ_FULL:
+		params.event = QED_IWARP_EVENT_IRQ_FULL;
+		event_cb = true;
+		break;
+	case IWARP_EXCEPTION_DETECTED_LLP_TIMEOUT:
+		params.event = QED_IWARP_EVENT_LLP_TIMEOUT;
+		event_cb = true;
+		break;
+	case IWARP_EXCEPTION_DETECTED_REMOTE_PROTECTION_ERROR:
+		params.event = QED_IWARP_EVENT_REMOTE_PROTECTION_ERROR;
+		event_cb = true;
+		break;
+	case IWARP_EXCEPTION_DETECTED_CQ_OVERFLOW:
+		params.event = QED_IWARP_EVENT_CQ_OVERFLOW;
+		event_cb = true;
+		break;
+	case IWARP_EXCEPTION_DETECTED_LOCAL_CATASTROPHIC:
+		params.event = QED_IWARP_EVENT_QP_CATASTROPHIC;
+		event_cb = true;
+		break;
+	case IWARP_EXCEPTION_DETECTED_LOCAL_ACCESS_ERROR:
+		params.event = QED_IWARP_EVENT_LOCAL_ACCESS_ERROR;
+		event_cb = true;
+		break;
+	case IWARP_EXCEPTION_DETECTED_REMOTE_OPERATION_ERROR:
+		params.event = QED_IWARP_EVENT_REMOTE_OPERATION_ERROR;
+		event_cb = true;
+		break;
+	case IWARP_EXCEPTION_DETECTED_TERMINATE_RECEIVED:
+		params.event = QED_IWARP_EVENT_TERMINATE_RECEIVED;
+		event_cb = true;
+		break;
+	default:
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Unhandled exception received...fw_ret_code=%d\n",
+			   fw_ret_code);
+		break;
+	}
+
+	if (event_cb) {
+		params.ep_context = ep;
+		params.cm_info = &ep->cm_info;
+		ep->event_cb(ep->cb_context, &params);
+	}
+}
+
+static void
+qed_iwarp_tcp_connect_unsuccessful(struct qed_hwfn *p_hwfn,
+				   struct qed_iwarp_ep *ep, u8 fw_return_code)
+{
+	struct qed_iwarp_cm_event_params params;
+
+	memset(&params, 0, sizeof(params));
+	params.event = QED_IWARP_EVENT_ACTIVE_COMPLETE;
+	params.ep_context = ep;
+	params.cm_info = &ep->cm_info;
+	ep->state = QED_IWARP_EP_CLOSED;
+
+	switch (fw_return_code) {
+	case IWARP_CONN_ERROR_TCP_CONNECT_INVALID_PACKET:
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "%s(0x%x) TCP connect got invalid packet\n",
+			   QED_IWARP_CONNECT_MODE_STRING(ep), ep->tcp_cid);
+		params.status = -ECONNRESET;
+		break;
+	case IWARP_CONN_ERROR_TCP_CONNECTION_RST:
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "%s(0x%x) TCP Connection Reset\n",
+			   QED_IWARP_CONNECT_MODE_STRING(ep), ep->tcp_cid);
+		params.status = -ECONNRESET;
+		break;
+	case IWARP_CONN_ERROR_TCP_CONNECT_TIMEOUT:
+		DP_NOTICE(p_hwfn, "%s(0x%x) TCP timeout\n",
+			  QED_IWARP_CONNECT_MODE_STRING(ep), ep->tcp_cid);
+		params.status = -EBUSY;
+		break;
+	case IWARP_CONN_ERROR_MPA_NOT_SUPPORTED_VER:
+		DP_NOTICE(p_hwfn, "%s(0x%x) MPA not supported VER\n",
+			  QED_IWARP_CONNECT_MODE_STRING(ep), ep->tcp_cid);
+		params.status = -ECONNREFUSED;
+		break;
+	case IWARP_CONN_ERROR_MPA_INVALID_PACKET:
+		DP_NOTICE(p_hwfn, "%s(0x%x) MPA Invalid Packet\n",
+			  QED_IWARP_CONNECT_MODE_STRING(ep), ep->tcp_cid);
+		params.status = -ECONNRESET;
+		break;
+	default:
+		DP_ERR(p_hwfn,
+		       "%s(0x%x) Unexpected return code tcp connect: %d\n",
+		       QED_IWARP_CONNECT_MODE_STRING(ep),
+		       ep->tcp_cid, fw_return_code);
+		params.status = -ECONNRESET;
+		break;
+	}
+
+	if (ep->connect_mode == TCP_CONNECT_PASSIVE) {
+		ep->tcp_cid = QED_IWARP_INVALID_TCP_CID;
+		qed_iwarp_return_ep(p_hwfn, ep);
+	} else {
+		ep->event_cb(ep->cb_context, &params);
+		spin_lock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+		list_del(&ep->list_entry);
+		spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+	}
+}
+
+void
+qed_iwarp_connect_complete(struct qed_hwfn *p_hwfn,
+			   struct qed_iwarp_ep *ep, u8 fw_return_code)
+{
+	u8 ll2_syn_handle = p_hwfn->p_rdma_info->iwarp.ll2_syn_handle;
+
+	if (ep->connect_mode == TCP_CONNECT_PASSIVE) {
+		/* Done with the SYN packet, post back to ll2 rx */
+		qed_iwarp_ll2_post_rx(p_hwfn, ep->syn, ll2_syn_handle);
+
+		ep->syn = NULL;
+
+		/* If connect failed - upper layer doesn't know about it */
+		if (fw_return_code == RDMA_RETURN_OK)
+			qed_iwarp_mpa_received(p_hwfn, ep);
+		else
+			qed_iwarp_tcp_connect_unsuccessful(p_hwfn, ep,
+							   fw_return_code);
+	} else {
+		if (fw_return_code == RDMA_RETURN_OK)
+			qed_iwarp_mpa_offload(p_hwfn, ep);
+		else
+			qed_iwarp_tcp_connect_unsuccessful(p_hwfn, ep,
+							   fw_return_code);
+	}
+}
+
+static inline bool
+qed_iwarp_check_ep_ok(struct qed_hwfn *p_hwfn, struct qed_iwarp_ep *ep)
+{
+	if (!ep || (ep->sig != QED_EP_SIG)) {
+		DP_ERR(p_hwfn, "ERROR ON ASYNC ep=%p\n", ep);
+		return false;
+	}
+
+	return true;
+}
+
+static int qed_iwarp_async_event(struct qed_hwfn *p_hwfn,
+				 u8 fw_event_code, u16 echo,
+				 union event_ring_data *data,
+				 u8 fw_return_code)
+{
+	struct regpair *fw_handle = &data->rdma_data.async_handle;
+	struct qed_iwarp_ep *ep = NULL;
+	u16 cid;
+
+	ep = (struct qed_iwarp_ep *)(uintptr_t)HILO_64(fw_handle->hi,
+						       fw_handle->lo);
+
+	switch (fw_event_code) {
+	case IWARP_EVENT_TYPE_ASYNC_CONNECT_COMPLETE:
+		/* Async completion after TCP 3-way handshake */
+		if (!qed_iwarp_check_ep_ok(p_hwfn, ep))
+			return -EINVAL;
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_RDMA,
+			   "EP(0x%x) IWARP_EVENT_TYPE_ASYNC_CONNECT_COMPLETE fw_ret_code=%d\n",
+			   ep->tcp_cid, fw_return_code);
+		qed_iwarp_connect_complete(p_hwfn, ep, fw_return_code);
+		break;
+	case IWARP_EVENT_TYPE_ASYNC_EXCEPTION_DETECTED:
+		if (!qed_iwarp_check_ep_ok(p_hwfn, ep))
+			return -EINVAL;
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_RDMA,
+			   "QP(0x%x) IWARP_EVENT_TYPE_ASYNC_EXCEPTION_DETECTED fw_ret_code=%d\n",
+			   ep->cid, fw_return_code);
+		qed_iwarp_exception_received(p_hwfn, ep, fw_return_code);
+		break;
+	case IWARP_EVENT_TYPE_ASYNC_QP_IN_ERROR_STATE:
+		/* Async completion for Close Connection ramrod */
+		if (!qed_iwarp_check_ep_ok(p_hwfn, ep))
+			return -EINVAL;
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_RDMA,
+			   "QP(0x%x) IWARP_EVENT_TYPE_ASYNC_QP_IN_ERROR_STATE fw_ret_code=%d\n",
+			   ep->cid, fw_return_code);
+		qed_iwarp_qp_in_error(p_hwfn, ep, fw_return_code);
+		break;
+	case IWARP_EVENT_TYPE_ASYNC_ENHANCED_MPA_REPLY_ARRIVED:
+		/* Async event for active side only */
+		if (!qed_iwarp_check_ep_ok(p_hwfn, ep))
+			return -EINVAL;
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_RDMA,
+			   "QP(0x%x) IWARP_EVENT_TYPE_ASYNC_MPA_HANDSHAKE_MPA_REPLY_ARRIVED fw_ret_code=%d\n",
+			   ep->cid, fw_return_code);
+		qed_iwarp_mpa_reply_arrived(p_hwfn, ep);
+		break;
+	case IWARP_EVENT_TYPE_ASYNC_MPA_HANDSHAKE_COMPLETE:
+		if (!qed_iwarp_check_ep_ok(p_hwfn, ep))
+			return -EINVAL;
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_RDMA,
+			   "QP(0x%x) IWARP_EVENT_TYPE_ASYNC_MPA_HANDSHAKE_COMPLETE fw_ret_code=%d\n",
+			   ep->cid, fw_return_code);
+		qed_iwarp_mpa_complete(p_hwfn, ep, fw_return_code);
+		break;
+	case IWARP_EVENT_TYPE_ASYNC_CID_CLEANED:
+		cid = (u16)le32_to_cpu(fw_handle->lo);
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "(0x%x)IWARP_EVENT_TYPE_ASYNC_CID_CLEANED\n", cid);
+		qed_iwarp_cid_cleaned(p_hwfn, cid);
+
+		break;
+	case IWARP_EVENT_TYPE_ASYNC_CQ_OVERFLOW:
+		DP_NOTICE(p_hwfn, "IWARP_EVENT_TYPE_ASYNC_CQ_OVERFLOW\n");
+
+		p_hwfn->p_rdma_info->events.affiliated_event(
+			p_hwfn->p_rdma_info->events.context,
+			QED_IWARP_EVENT_CQ_OVERFLOW,
+			(void *)fw_handle);
+		break;
+	default:
+		DP_ERR(p_hwfn, "Received unexpected async iwarp event %d\n",
+		       fw_event_code);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int
+qed_iwarp_create_listen(void *rdma_cxt,
+			struct qed_iwarp_listen_in *iparams,
+			struct qed_iwarp_listen_out *oparams)
+{
+	struct qed_hwfn *p_hwfn = rdma_cxt;
+	struct qed_iwarp_listener *listener;
+
+	listener = kzalloc(sizeof(*listener), GFP_KERNEL);
+	if (!listener)
+		return -ENOMEM;
+
+	listener->ip_version = iparams->ip_version;
+	memcpy(listener->ip_addr, iparams->ip_addr, sizeof(listener->ip_addr));
+	listener->port = iparams->port;
+	listener->vlan = iparams->vlan;
+
+	listener->event_cb = iparams->event_cb;
+	listener->cb_context = iparams->cb_context;
+	listener->max_backlog = iparams->max_backlog;
+	oparams->handle = listener;
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+	list_add_tail(&listener->list_entry,
+		      &p_hwfn->p_rdma_info->iwarp.listen_list);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+
+	DP_VERBOSE(p_hwfn,
+		   QED_MSG_RDMA,
+		   "callback=%p handle=%p ip=%x:%x:%x:%x port=0x%x vlan=0x%x\n",
+		   listener->event_cb,
+		   listener,
+		   listener->ip_addr[0],
+		   listener->ip_addr[1],
+		   listener->ip_addr[2],
+		   listener->ip_addr[3], listener->port, listener->vlan);
+
+	return 0;
+}
+
+int qed_iwarp_destroy_listen(void *rdma_cxt, void *handle)
+{
+	struct qed_iwarp_listener *listener = handle;
+	struct qed_hwfn *p_hwfn = rdma_cxt;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "handle=%p\n", handle);
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+	list_del(&listener->list_entry);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->iwarp.iw_lock);
+
+	kfree(listener);
+
+	return 0;
+}
+
+int qed_iwarp_send_rtr(void *rdma_cxt, struct qed_iwarp_send_rtr_in *iparams)
+{
+	struct qed_hwfn *p_hwfn = rdma_cxt;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	struct qed_iwarp_ep *ep;
+	struct qed_rdma_qp *qp;
+	int rc;
+
+	ep = iparams->ep_context;
+	if (!ep) {
+		DP_ERR(p_hwfn, "Ep Context receive in send_rtr is NULL\n");
+		return -EINVAL;
+	}
+
+	qp = ep->qp;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "QP(0x%x) EP(0x%x)\n",
+		   qp->icid, ep->tcp_cid);
+
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = qp->icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_CB;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 IWARP_RAMROD_CMD_ID_MPA_OFFLOAD_SEND_RTR,
+				 PROTOCOLID_IWARP, &init_data);
+
+	if (rc)
+		return rc;
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = 0x%x\n", rc);
+
+	return rc;
+}
+
+void
+qed_iwarp_query_qp(struct qed_rdma_qp *qp,
+		   struct qed_rdma_query_qp_out_params *out_params)
+{
+	out_params->state = qed_iwarp2roce_state(qp->iwarp_state);
+}
diff --git a/drivers/net/ethernet/qlogic/qed/qed_iwarp.h b/drivers/net/ethernet/qlogic/qed/qed_iwarp.h
new file mode 100644
index 000000000000..148ef3c33a5d
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qed/qed_iwarp.h
@@ -0,0 +1,189 @@
+/* QLogic qed NIC Driver
+ * Copyright (c) 2015-2017  QLogic Corporation
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and /or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+#ifndef _QED_IWARP_H
+#define _QED_IWARP_H
+
+enum qed_iwarp_qp_state {
+	QED_IWARP_QP_STATE_IDLE,
+	QED_IWARP_QP_STATE_RTS,
+	QED_IWARP_QP_STATE_TERMINATE,
+	QED_IWARP_QP_STATE_CLOSING,
+	QED_IWARP_QP_STATE_ERROR,
+};
+
+enum qed_iwarp_qp_state qed_roce2iwarp_state(enum qed_roce_qp_state state);
+
+#define QED_IWARP_PREALLOC_CNT  (256)
+
+#define QED_IWARP_LL2_SYN_TX_SIZE       (128)
+#define QED_IWARP_LL2_SYN_RX_SIZE       (256)
+#define QED_IWARP_MAX_SYN_PKT_SIZE      (128)
+#define QED_IWARP_HANDLE_INVAL			(0xff)
+
+struct qed_iwarp_ll2_buff {
+	void *data;
+	dma_addr_t data_phys_addr;
+	u32 buff_size;
+};
+
+struct qed_iwarp_info {
+	struct list_head listen_list;	/* qed_iwarp_listener */
+	struct list_head ep_list;	/* qed_iwarp_ep */
+	struct list_head ep_free_list;	/* pre-allocated ep's */
+	spinlock_t iw_lock;	/* for iwarp resources */
+	spinlock_t qp_lock;	/* for teardown races */
+	u32 rcv_wnd_scale;
+	u16 max_mtu;
+	u8 mac_addr[ETH_ALEN];
+	u8 crc_needed;
+	u8 tcp_flags;
+	u8 ll2_syn_handle;
+	u8 peer2peer;
+	enum mpa_negotiation_mode mpa_rev;
+	enum mpa_rtr_type rtr_type;
+};
+
+enum qed_iwarp_ep_state {
+	QED_IWARP_EP_INIT,
+	QED_IWARP_EP_MPA_REQ_RCVD,
+	QED_IWARP_EP_MPA_OFFLOADED,
+	QED_IWARP_EP_ESTABLISHED,
+	QED_IWARP_EP_CLOSED
+};
+
+union async_output {
+	struct iwarp_eqe_data_mpa_async_completion mpa_response;
+	struct iwarp_eqe_data_tcp_async_completion mpa_request;
+};
+
+#define QED_MAX_PRIV_DATA_LEN (512)
+struct qed_iwarp_ep_memory {
+	u8 in_pdata[QED_MAX_PRIV_DATA_LEN];
+	u8 out_pdata[QED_MAX_PRIV_DATA_LEN];
+	union async_output async_output;
+};
+
+/* Endpoint structure represents a TCP connection. This connection can be
+ * associated with a QP or not (in which case QP==NULL)
+ */
+struct qed_iwarp_ep {
+	struct list_head list_entry;
+	struct qed_rdma_qp *qp;
+	struct qed_iwarp_ep_memory *ep_buffer_virt;
+	dma_addr_t ep_buffer_phys;
+	enum qed_iwarp_ep_state state;
+	int sig;
+	struct qed_iwarp_cm_info cm_info;
+	enum tcp_connect_mode connect_mode;
+	enum mpa_rtr_type rtr_type;
+	enum mpa_negotiation_mode mpa_rev;
+	u32 tcp_cid;
+	u32 cid;
+	u16 mss;
+	u8 remote_mac_addr[6];
+	u8 local_mac_addr[6];
+	bool mpa_reply_processed;
+
+	/* For Passive side - syn packet related data */
+	u16 syn_ip_payload_length;
+	struct qed_iwarp_ll2_buff *syn;
+	dma_addr_t syn_phy_addr;
+
+	/* The event_cb function is called for asynchrounous events associated
+	 * with the ep. It is initialized at different entry points depending
+	 * on whether the ep is the tcp connection active side or passive side
+	 * The cb_context is passed to the event_cb function.
+	 */
+	iwarp_event_handler event_cb;
+	void *cb_context;
+};
+
+struct qed_iwarp_listener {
+	struct list_head list_entry;
+
+	/* The event_cb function is called for connection requests.
+	 * The cb_context is passed to the event_cb function.
+	 */
+	iwarp_event_handler event_cb;
+	void *cb_context;
+	u32 max_backlog;
+	u32 ip_addr[4];
+	u16 port;
+	u16 vlan;
+	u8 ip_version;
+};
+
+int qed_iwarp_alloc(struct qed_hwfn *p_hwfn);
+
+int qed_iwarp_setup(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
+		    struct qed_rdma_start_in_params *params);
+
+int qed_iwarp_stop(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
+
+void qed_iwarp_resc_free(struct qed_hwfn *p_hwfn);
+
+void qed_iwarp_init_devinfo(struct qed_hwfn *p_hwfn);
+
+void qed_iwarp_init_hw(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
+
+int qed_iwarp_create_qp(struct qed_hwfn *p_hwfn,
+			struct qed_rdma_qp *qp,
+			struct qed_rdma_create_qp_out_params *out_params);
+
+int qed_iwarp_modify_qp(struct qed_hwfn *p_hwfn, struct qed_rdma_qp *qp,
+			enum qed_iwarp_qp_state new_state, bool internal);
+
+int qed_iwarp_destroy_qp(struct qed_hwfn *p_hwfn, struct qed_rdma_qp *qp);
+
+int qed_iwarp_fw_destroy(struct qed_hwfn *p_hwfn, struct qed_rdma_qp *qp);
+
+void qed_iwarp_query_qp(struct qed_rdma_qp *qp,
+			struct qed_rdma_query_qp_out_params *out_params);
+
+int
+qed_iwarp_connect(void *rdma_cxt,
+		  struct qed_iwarp_connect_in *iparams,
+		  struct qed_iwarp_connect_out *oparams);
+
+int
+qed_iwarp_create_listen(void *rdma_cxt,
+			struct qed_iwarp_listen_in *iparams,
+			struct qed_iwarp_listen_out *oparams);
+
+int qed_iwarp_accept(void *rdma_cxt, struct qed_iwarp_accept_in *iparams);
+
+int qed_iwarp_reject(void *rdma_cxt, struct qed_iwarp_reject_in *iparams);
+int qed_iwarp_destroy_listen(void *rdma_cxt, void *handle);
+
+int qed_iwarp_send_rtr(void *rdma_cxt, struct qed_iwarp_send_rtr_in *iparams);
+
+#endif
diff --git a/drivers/net/ethernet/qlogic/qed/qed_l2.c b/drivers/net/ethernet/qlogic/qed/qed_l2.c
index 746fed4099c8..0ba5ec8a9814 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_l2.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_l2.c
@@ -43,7 +43,6 @@
 #include <linux/slab.h>
 #include <linux/stddef.h>
 #include <linux/string.h>
-#include <linux/version.h>
 #include <linux/workqueue.h>
 #include <linux/bitops.h>
 #include <linux/bug.h>
@@ -66,26 +65,161 @@
 #define QED_MAX_SGES_NUM 16
 #define CRC32_POLY 0x1edc6f41
 
+struct qed_l2_info {
+	u32 queues;
+	unsigned long **pp_qid_usage;
+
+	/* The lock is meant to synchronize access to the qid usage */
+	struct mutex lock;
+};
+
+int qed_l2_alloc(struct qed_hwfn *p_hwfn)
+{
+	struct qed_l2_info *p_l2_info;
+	unsigned long **pp_qids;
+	u32 i;
+
+	if (!QED_IS_L2_PERSONALITY(p_hwfn))
+		return 0;
+
+	p_l2_info = kzalloc(sizeof(*p_l2_info), GFP_KERNEL);
+	if (!p_l2_info)
+		return -ENOMEM;
+	p_hwfn->p_l2_info = p_l2_info;
+
+	if (IS_PF(p_hwfn->cdev)) {
+		p_l2_info->queues = RESC_NUM(p_hwfn, QED_L2_QUEUE);
+	} else {
+		u8 rx = 0, tx = 0;
+
+		qed_vf_get_num_rxqs(p_hwfn, &rx);
+		qed_vf_get_num_txqs(p_hwfn, &tx);
+
+		p_l2_info->queues = max_t(u8, rx, tx);
+	}
+
+	pp_qids = kzalloc(sizeof(unsigned long *) * p_l2_info->queues,
+			  GFP_KERNEL);
+	if (!pp_qids)
+		return -ENOMEM;
+	p_l2_info->pp_qid_usage = pp_qids;
+
+	for (i = 0; i < p_l2_info->queues; i++) {
+		pp_qids[i] = kzalloc(MAX_QUEUES_PER_QZONE / 8, GFP_KERNEL);
+		if (!pp_qids[i])
+			return -ENOMEM;
+	}
+
+	return 0;
+}
+
+void qed_l2_setup(struct qed_hwfn *p_hwfn)
+{
+	if (p_hwfn->hw_info.personality != QED_PCI_ETH &&
+	    p_hwfn->hw_info.personality != QED_PCI_ETH_ROCE)
+		return;
+
+	mutex_init(&p_hwfn->p_l2_info->lock);
+}
+
+void qed_l2_free(struct qed_hwfn *p_hwfn)
+{
+	u32 i;
+
+	if (p_hwfn->hw_info.personality != QED_PCI_ETH &&
+	    p_hwfn->hw_info.personality != QED_PCI_ETH_ROCE)
+		return;
+
+	if (!p_hwfn->p_l2_info)
+		return;
+
+	if (!p_hwfn->p_l2_info->pp_qid_usage)
+		goto out_l2_info;
+
+	/* Free until hit first uninitialized entry */
+	for (i = 0; i < p_hwfn->p_l2_info->queues; i++) {
+		if (!p_hwfn->p_l2_info->pp_qid_usage[i])
+			break;
+		kfree(p_hwfn->p_l2_info->pp_qid_usage[i]);
+	}
+
+	kfree(p_hwfn->p_l2_info->pp_qid_usage);
+
+out_l2_info:
+	kfree(p_hwfn->p_l2_info);
+	p_hwfn->p_l2_info = NULL;
+}
+
+static bool qed_eth_queue_qid_usage_add(struct qed_hwfn *p_hwfn,
+					struct qed_queue_cid *p_cid)
+{
+	struct qed_l2_info *p_l2_info = p_hwfn->p_l2_info;
+	u16 queue_id = p_cid->rel.queue_id;
+	bool b_rc = true;
+	u8 first;
+
+	mutex_lock(&p_l2_info->lock);
+
+	if (queue_id >= p_l2_info->queues) {
+		DP_NOTICE(p_hwfn,
+			  "Requested to increase usage for qzone %04x out of %08x\n",
+			  queue_id, p_l2_info->queues);
+		b_rc = false;
+		goto out;
+	}
+
+	first = (u8)find_first_zero_bit(p_l2_info->pp_qid_usage[queue_id],
+					MAX_QUEUES_PER_QZONE);
+	if (first >= MAX_QUEUES_PER_QZONE) {
+		b_rc = false;
+		goto out;
+	}
+
+	__set_bit(first, p_l2_info->pp_qid_usage[queue_id]);
+	p_cid->qid_usage_idx = first;
+
+out:
+	mutex_unlock(&p_l2_info->lock);
+	return b_rc;
+}
+
+static void qed_eth_queue_qid_usage_del(struct qed_hwfn *p_hwfn,
+					struct qed_queue_cid *p_cid)
+{
+	mutex_lock(&p_hwfn->p_l2_info->lock);
+
+	clear_bit(p_cid->qid_usage_idx,
+		  p_hwfn->p_l2_info->pp_qid_usage[p_cid->rel.queue_id]);
+
+	mutex_unlock(&p_hwfn->p_l2_info->lock);
+}
+
 void qed_eth_queue_cid_release(struct qed_hwfn *p_hwfn,
 			       struct qed_queue_cid *p_cid)
 {
-	/* VFs' CIDs are 0-based in PF-view, and uninitialized on VF */
-	if (!p_cid->is_vf && IS_PF(p_hwfn->cdev))
-		qed_cxt_release_cid(p_hwfn, p_cid->cid);
+	bool b_legacy_vf = !!(p_cid->vf_legacy & QED_QCID_LEGACY_VF_CID);
+
+	if (IS_PF(p_hwfn->cdev) && !b_legacy_vf)
+		_qed_cxt_release_cid(p_hwfn, p_cid->cid, p_cid->vfid);
+
+	/* For PF's VFs we maintain the index inside queue-zone in IOV */
+	if (p_cid->vfid == QED_QUEUE_CID_SELF)
+		qed_eth_queue_qid_usage_del(p_hwfn, p_cid);
+
 	vfree(p_cid);
 }
 
 /* The internal is only meant to be directly called by PFs initializeing CIDs
  * for their VFs.
  */
-struct qed_queue_cid *
+static struct qed_queue_cid *
 _qed_eth_queue_to_cid(struct qed_hwfn *p_hwfn,
 		      u16 opaque_fid,
 		      u32 cid,
-		      u8 vf_qid,
-		      struct qed_queue_start_common_params *p_params)
+		      struct qed_queue_start_common_params *p_params,
+		      bool b_is_rx,
+		      struct qed_queue_cid_vf_params *p_vf_params)
 {
-	bool b_is_same = (p_hwfn->hw_info.opaque_fid == opaque_fid);
 	struct qed_queue_cid *p_cid;
 	int rc;
 
@@ -96,10 +230,25 @@ _qed_eth_queue_to_cid(struct qed_hwfn *p_hwfn,
 
 	p_cid->opaque_fid = opaque_fid;
 	p_cid->cid = cid;
-	p_cid->vf_qid = vf_qid;
-	p_cid->rel = *p_params;
 	p_cid->p_owner = p_hwfn;
 
+	/* Fill in parameters */
+	p_cid->rel.vport_id = p_params->vport_id;
+	p_cid->rel.queue_id = p_params->queue_id;
+	p_cid->rel.stats_id = p_params->stats_id;
+	p_cid->sb_igu_id = p_params->p_sb->igu_sb_id;
+	p_cid->b_is_rx = b_is_rx;
+	p_cid->sb_idx = p_params->sb_idx;
+
+	/* Fill-in bits related to VFs' queues if information was provided */
+	if (p_vf_params) {
+		p_cid->vfid = p_vf_params->vfid;
+		p_cid->vf_qid = p_vf_params->vf_qid;
+		p_cid->vf_legacy = p_vf_params->vf_legacy;
+	} else {
+		p_cid->vfid = QED_QUEUE_CID_SELF;
+	}
+
 	/* Don't try calculating the absolute indices for VFs */
 	if (IS_VF(p_hwfn->cdev)) {
 		p_cid->abs = p_cid->rel;
@@ -121,7 +270,7 @@ _qed_eth_queue_to_cid(struct qed_hwfn *p_hwfn,
 	/* In case of a PF configuring its VF's queues, the stats-id is already
 	 * absolute [since there's a single index that's suitable per-VF].
 	 */
-	if (b_is_same) {
+	if (p_cid->vfid == QED_QUEUE_CID_SELF) {
 		rc = qed_fw_vport(p_hwfn, p_cid->rel.stats_id,
 				  &p_cid->abs.stats_id);
 		if (rc)
@@ -130,27 +279,29 @@ _qed_eth_queue_to_cid(struct qed_hwfn *p_hwfn,
 		p_cid->abs.stats_id = p_cid->rel.stats_id;
 	}
 
-	/* SBs relevant information was already provided as absolute */
-	p_cid->abs.sb = p_cid->rel.sb;
-	p_cid->abs.sb_idx = p_cid->rel.sb_idx;
-
-	/* This is tricky - we're actually interested in whehter this is a PF
-	 * entry meant for the VF.
-	 */
-	if (!b_is_same)
-		p_cid->is_vf = true;
 out:
+	/* VF-images have provided the qid_usage_idx on their own.
+	 * Otherwise, we need to allocate a unique one.
+	 */
+	if (!p_vf_params) {
+		if (!qed_eth_queue_qid_usage_add(p_hwfn, p_cid))
+			goto fail;
+	} else {
+		p_cid->qid_usage_idx = p_vf_params->qid_usage_idx;
+	}
+
 	DP_VERBOSE(p_hwfn,
 		   QED_MSG_SP,
-		   "opaque_fid: %04x CID %08x vport %02x [%02x] qzone %04x [%04x] stats %02x [%02x] SB %04x PI %02x\n",
+		   "opaque_fid: %04x CID %08x vport %02x [%02x] qzone %04x.%02x [%04x] stats %02x [%02x] SB %04x PI %02x\n",
 		   p_cid->opaque_fid,
 		   p_cid->cid,
 		   p_cid->rel.vport_id,
 		   p_cid->abs.vport_id,
 		   p_cid->rel.queue_id,
+		   p_cid->qid_usage_idx,
 		   p_cid->abs.queue_id,
 		   p_cid->rel.stats_id,
-		   p_cid->abs.stats_id, p_cid->abs.sb, p_cid->abs.sb_idx);
+		   p_cid->abs.stats_id, p_cid->sb_igu_id, p_cid->sb_idx);
 
 	return p_cid;
 
@@ -159,32 +310,61 @@ fail:
 	return NULL;
 }
 
-static struct qed_queue_cid *qed_eth_queue_to_cid(struct qed_hwfn *p_hwfn,
-						  u16 opaque_fid, struct
-						  qed_queue_start_common_params
-						  *p_params)
+struct qed_queue_cid *
+qed_eth_queue_to_cid(struct qed_hwfn *p_hwfn,
+		     u16 opaque_fid,
+		     struct qed_queue_start_common_params *p_params,
+		     bool b_is_rx,
+		     struct qed_queue_cid_vf_params *p_vf_params)
 {
 	struct qed_queue_cid *p_cid;
+	u8 vfid = QED_CXT_PF_CID;
+	bool b_legacy_vf = false;
 	u32 cid = 0;
 
+	/* In case of legacy VFs, The CID can be derived from the additional
+	 * VF parameters - the VF assumes queue X uses CID X, so we can simply
+	 * use the vf_qid for this purpose as well.
+	 */
+	if (p_vf_params) {
+		vfid = p_vf_params->vfid;
+
+		if (p_vf_params->vf_legacy & QED_QCID_LEGACY_VF_CID) {
+			b_legacy_vf = true;
+			cid = p_vf_params->vf_qid;
+		}
+	}
+
 	/* Get a unique firmware CID for this queue, in case it's a PF.
 	 * VF's don't need a CID as the queue configuration will be done
 	 * by PF.
 	 */
-	if (IS_PF(p_hwfn->cdev)) {
-		if (qed_cxt_acquire_cid(p_hwfn, PROTOCOLID_ETH, &cid)) {
+	if (IS_PF(p_hwfn->cdev) && !b_legacy_vf) {
+		if (_qed_cxt_acquire_cid(p_hwfn, PROTOCOLID_ETH,
+					 &cid, vfid)) {
 			DP_NOTICE(p_hwfn, "Failed to acquire cid\n");
 			return NULL;
 		}
 	}
 
-	p_cid = _qed_eth_queue_to_cid(p_hwfn, opaque_fid, cid, 0, p_params);
-	if (!p_cid && IS_PF(p_hwfn->cdev))
-		qed_cxt_release_cid(p_hwfn, cid);
+	p_cid = _qed_eth_queue_to_cid(p_hwfn, opaque_fid, cid,
+				      p_params, b_is_rx, p_vf_params);
+	if (!p_cid && IS_PF(p_hwfn->cdev) && !b_legacy_vf)
+		_qed_cxt_release_cid(p_hwfn, cid, vfid);
 
 	return p_cid;
 }
 
+static struct qed_queue_cid *
+qed_eth_queue_to_cid_pf(struct qed_hwfn *p_hwfn,
+			u16 opaque_fid,
+			bool b_is_rx,
+			struct qed_queue_start_common_params *p_params)
+{
+	return qed_eth_queue_to_cid(p_hwfn, opaque_fid, p_params, b_is_rx,
+				    NULL);
+}
+
 int qed_sp_eth_vport_start(struct qed_hwfn *p_hwfn,
 			   struct qed_sp_vport_start_params *p_params)
 {
@@ -682,7 +862,7 @@ int qed_eth_rxq_start_ramrod(struct qed_hwfn *p_hwfn,
 	DP_VERBOSE(p_hwfn, QED_MSG_SP,
 		   "opaque_fid=0x%x, cid=0x%x, rx_qzone=0x%x, vport_id=0x%x, sb_id=0x%x\n",
 		   p_cid->opaque_fid, p_cid->cid,
-		   p_cid->abs.queue_id, p_cid->abs.vport_id, p_cid->abs.sb);
+		   p_cid->abs.queue_id, p_cid->abs.vport_id, p_cid->sb_igu_id);
 
 	/* Get SPQ entry */
 	memset(&init_data, 0, sizeof(init_data));
@@ -698,8 +878,8 @@ int qed_eth_rxq_start_ramrod(struct qed_hwfn *p_hwfn,
 
 	p_ramrod = &p_ent->ramrod.rx_queue_start;
 
-	p_ramrod->sb_id = cpu_to_le16(p_cid->abs.sb);
-	p_ramrod->sb_index = p_cid->abs.sb_idx;
+	p_ramrod->sb_id = cpu_to_le16(p_cid->sb_igu_id);
+	p_ramrod->sb_index = p_cid->sb_idx;
 	p_ramrod->vport_id = p_cid->abs.vport_id;
 	p_ramrod->stats_counter_id = p_cid->abs.stats_id;
 	p_ramrod->rx_queue_id = cpu_to_le16(p_cid->abs.queue_id);
@@ -712,13 +892,15 @@ int qed_eth_rxq_start_ramrod(struct qed_hwfn *p_hwfn,
 	p_ramrod->num_of_pbl_pages = cpu_to_le16(cqe_pbl_size);
 	DMA_REGPAIR_LE(p_ramrod->cqe_pbl_addr, cqe_pbl_addr);
 
-	if (p_cid->is_vf) {
+	if (p_cid->vfid != QED_QUEUE_CID_SELF) {
+		bool b_legacy_vf = !!(p_cid->vf_legacy &
+				      QED_QCID_LEGACY_VF_RX_PROD);
+
 		p_ramrod->vf_rx_prod_index = p_cid->vf_qid;
 		DP_VERBOSE(p_hwfn, QED_MSG_SP,
 			   "Queue%s is meant for VF rxq[%02x]\n",
-			   !!p_cid->b_legacy_vf ? " [legacy]" : "",
-			   p_cid->vf_qid);
-		p_ramrod->vf_rx_prod_use_zone_a = !!p_cid->b_legacy_vf;
+			   b_legacy_vf ? " [legacy]" : "", p_cid->vf_qid);
+		p_ramrod->vf_rx_prod_use_zone_a = b_legacy_vf;
 	}
 
 	return qed_spq_post(p_hwfn, p_ent, NULL);
@@ -762,7 +944,7 @@ qed_eth_rx_queue_start(struct qed_hwfn *p_hwfn,
 	int rc;
 
 	/* Allocate a CID for the queue */
-	p_cid = qed_eth_queue_to_cid(p_hwfn, opaque_fid, p_params);
+	p_cid = qed_eth_queue_to_cid_pf(p_hwfn, opaque_fid, true, p_params);
 	if (!p_cid)
 		return -ENOMEM;
 
@@ -864,10 +1046,11 @@ qed_eth_pf_rx_queue_stop(struct qed_hwfn *p_hwfn,
 	/* Cleaning the queue requires the completion to arrive there.
 	 * In addition, VFs require the answer to come as eqe to PF.
 	 */
-	p_ramrod->complete_cqe_flg = (!p_cid->is_vf &&
+	p_ramrod->complete_cqe_flg = ((p_cid->vfid == QED_QUEUE_CID_SELF) &&
 				      !b_eq_completion_only) ||
 				     b_cqe_completion;
-	p_ramrod->complete_event_flg = p_cid->is_vf || b_eq_completion_only;
+	p_ramrod->complete_event_flg = (p_cid->vfid != QED_QUEUE_CID_SELF) ||
+				       b_eq_completion_only;
 
 	return qed_spq_post(p_hwfn, p_ent, NULL);
 }
@@ -916,8 +1099,8 @@ qed_eth_txq_start_ramrod(struct qed_hwfn *p_hwfn,
 	p_ramrod = &p_ent->ramrod.tx_queue_start;
 	p_ramrod->vport_id = p_cid->abs.vport_id;
 
-	p_ramrod->sb_id = cpu_to_le16(p_cid->abs.sb);
-	p_ramrod->sb_index = p_cid->abs.sb_idx;
+	p_ramrod->sb_id = cpu_to_le16(p_cid->sb_igu_id);
+	p_ramrod->sb_index = p_cid->sb_idx;
 	p_ramrod->stats_counter_id = p_cid->abs.stats_id;
 
 	p_ramrod->queue_zone_id = cpu_to_le16(p_cid->abs.queue_id);
@@ -966,7 +1149,7 @@ qed_eth_tx_queue_start(struct qed_hwfn *p_hwfn,
 	struct qed_queue_cid *p_cid;
 	int rc;
 
-	p_cid = qed_eth_queue_to_cid(p_hwfn, opaque_fid, p_params);
+	p_cid = qed_eth_queue_to_cid_pf(p_hwfn, opaque_fid, false, p_params);
 	if (!p_cid)
 		return -EINVAL;
 
@@ -1044,19 +1227,6 @@ static enum eth_filter_action qed_filter_action(enum qed_filter_opcode opcode)
 	return action;
 }
 
-static void qed_set_fw_mac_addr(__le16 *fw_msb,
-				__le16 *fw_mid,
-				__le16 *fw_lsb,
-				u8 *mac)
-{
-	((u8 *)fw_msb)[0] = mac[1];
-	((u8 *)fw_msb)[1] = mac[0];
-	((u8 *)fw_mid)[0] = mac[3];
-	((u8 *)fw_mid)[1] = mac[2];
-	((u8 *)fw_lsb)[0] = mac[5];
-	((u8 *)fw_lsb)[1] = mac[4];
-}
-
 static int
 qed_filter_ucast_common(struct qed_hwfn *p_hwfn,
 			u16 opaque_fid,
@@ -1935,15 +2105,26 @@ static int qed_fill_eth_dev_info(struct qed_dev *cdev,
 
 		ether_addr_copy(info->port_mac,
 				cdev->hwfns[0].hw_info.hw_mac_addr);
+
+		info->xdp_supported = true;
 	} else {
-		qed_vf_get_num_rxqs(QED_LEADING_HWFN(cdev), &info->num_queues);
-		if (cdev->num_hwfns > 1) {
-			u8 queues = 0;
+		u16 total_cids = 0;
 
-			qed_vf_get_num_rxqs(&cdev->hwfns[1], &queues);
+		/* Determine queues &  XDP support */
+		for_each_hwfn(cdev, i) {
+			struct qed_hwfn *p_hwfn = &cdev->hwfns[i];
+			u8 queues, cids;
+
+			qed_vf_get_num_cids(p_hwfn, &cids);
+			qed_vf_get_num_rxqs(p_hwfn, &queues);
 			info->num_queues += queues;
+			total_cids += cids;
 		}
 
+		/* Enable VF XDP in case PF guarntees sufficient connections */
+		if (total_cids >= info->num_queues * 3)
+			info->xdp_supported = true;
+
 		qed_vf_get_num_vlan_filters(&cdev->hwfns[0],
 					    (u8 *)&info->num_vlan_filters);
 		qed_vf_get_num_mac_filters(&cdev->hwfns[0],
@@ -2194,9 +2375,9 @@ static int qed_start_rxq(struct qed_dev *cdev,
 	}
 
 	DP_VERBOSE(cdev, (QED_MSG_SPQ | NETIF_MSG_IFUP),
-		   "Started RX-Q %d [rss_num %d] on V-PORT %d and SB %d\n",
+		   "Started RX-Q %d [rss_num %d] on V-PORT %d and SB igu %d\n",
 		   p_params->queue_id, rss_num, p_params->vport_id,
-		   p_params->sb);
+		   p_params->p_sb->igu_sb_id);
 
 	return 0;
 }
@@ -2244,9 +2425,9 @@ static int qed_start_txq(struct qed_dev *cdev,
 	}
 
 	DP_VERBOSE(cdev, (QED_MSG_SPQ | NETIF_MSG_IFUP),
-		   "Started TX-Q %d [rss_num %d] on V-PORT %d and SB %d\n",
+		   "Started TX-Q %d [rss_num %d] on V-PORT %d and SB igu %d\n",
 		   p_params->queue_id, rss_num, p_params->vport_id,
-		   p_params->sb);
+		   p_params->p_sb->igu_sb_id);
 
 	return 0;
 }
@@ -2301,14 +2482,25 @@ static int qed_tunn_configure(struct qed_dev *cdev,
 
 	for_each_hwfn(cdev, i) {
 		struct qed_hwfn *hwfn = &cdev->hwfns[i];
+		struct qed_ptt *p_ptt;
 		struct qed_tunnel_info *tun;
 
 		tun = &hwfn->cdev->tunnel;
+		if (IS_PF(cdev)) {
+			p_ptt = qed_ptt_acquire(hwfn);
+			if (!p_ptt)
+				return -EAGAIN;
+		} else {
+			p_ptt = NULL;
+		}
 
-		rc = qed_sp_pf_update_tunn_cfg(hwfn, &tunn_info,
+		rc = qed_sp_pf_update_tunn_cfg(hwfn, p_ptt, &tunn_info,
 					       QED_SPQ_MODE_EBLOCK, NULL);
-		if (rc)
+		if (rc) {
+			if (IS_PF(cdev))
+				qed_ptt_release(hwfn, p_ptt);
 			return rc;
+		}
 
 		if (IS_PF_SRIOV(hwfn)) {
 			u16 vxlan_port, geneve_port;
@@ -2325,6 +2517,8 @@ static int qed_tunn_configure(struct qed_dev *cdev,
 
 			qed_schedule_iov(hwfn, QED_IOV_WQ_BULLETIN_UPDATE_FLAG);
 		}
+		if (IS_PF(cdev))
+			qed_ptt_release(hwfn, p_ptt);
 	}
 
 	return 0;
diff --git a/drivers/net/ethernet/qlogic/qed/qed_l2.h b/drivers/net/ethernet/qlogic/qed/qed_l2.h
index 6f44229899eb..f8f09aadced7 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_l2.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_l2.h
@@ -277,40 +277,87 @@ void qed_get_vport_stats(struct qed_dev *cdev, struct qed_eth_stats *stats);
 
 void qed_reset_vport_stats(struct qed_dev *cdev);
 
-struct qed_queue_cid {
-	/* 'Relative' is a relative term ;-). Usually the indices [not counting
-	 * SBs] would be PF-relative, but there are some cases where that isn't
-	 * the case - specifically for a PF configuring its VF indices it's
-	 * possible some fields [E.g., stats-id] in 'rel' would already be abs.
+#define MAX_QUEUES_PER_QZONE    (sizeof(unsigned long) * 8)
+#define QED_QUEUE_CID_SELF	(0xff)
+
+/* Almost identical to the qed_queue_start_common_params,
+ * but here we maintain the SB index in IGU CAM.
+ */
+struct qed_queue_cid_params {
+	u8 vport_id;
+	u16 queue_id;
+	u8 stats_id;
+};
+
+/* Additional parameters required for initialization of the queue_cid
+ * and are relevant only for a PF initializing one for its VFs.
+ */
+struct qed_queue_cid_vf_params {
+	/* Should match the VF's relative index */
+	u8 vfid;
+
+	/* 0-based queue index. Should reflect the relative qzone the
+	 * VF thinks is associated with it [in its range].
+	 */
+	u8 vf_qid;
+
+	/* Indicates a VF is legacy, making it differ in several things:
+	 *  - Producers would be placed in a different place.
+	 *  - Makes assumptions regarding the CIDs.
 	 */
-	struct qed_queue_start_common_params rel;
-	struct qed_queue_start_common_params abs;
+	u8 vf_legacy;
+
+	u8 qid_usage_idx;
+};
+
+struct qed_queue_cid {
+	/* For stats-id, the `rel' is actually absolute as well */
+	struct qed_queue_cid_params rel;
+	struct qed_queue_cid_params abs;
+
+	/* These have no 'relative' meaning */
+	u16 sb_igu_id;
+	u8 sb_idx;
+
 	u32 cid;
 	u16 opaque_fid;
 
+	bool b_is_rx;
+
 	/* VFs queues are mapped differently, so we need to know the
 	 * relative queue associated with them [0-based].
 	 * Notice this is relevant on the *PF* queue-cid of its VF's queues,
 	 * and not on the VF itself.
 	 */
-	bool is_vf;
+	u8 vfid;
 	u8 vf_qid;
 
-	/* Legacy VFs might have Rx producer located elsewhere */
-	bool b_legacy_vf;
+	/* We need an additional index to differentiate between queues opened
+	 * for same queue-zone, as VFs would have to communicate the info
+	 * to the PF [otherwise PF has no way to differentiate].
+	 */
+	u8 qid_usage_idx;
+
+	u8 vf_legacy;
+#define QED_QCID_LEGACY_VF_RX_PROD	(BIT(0))
+#define QED_QCID_LEGACY_VF_CID		(BIT(1))
 
 	struct qed_hwfn *p_owner;
 };
 
+int qed_l2_alloc(struct qed_hwfn *p_hwfn);
+void qed_l2_setup(struct qed_hwfn *p_hwfn);
+void qed_l2_free(struct qed_hwfn *p_hwfn);
+
 void qed_eth_queue_cid_release(struct qed_hwfn *p_hwfn,
 			       struct qed_queue_cid *p_cid);
 
-struct qed_queue_cid *_qed_eth_queue_to_cid(struct qed_hwfn *p_hwfn,
-					    u16 opaque_fid,
-					    u32 cid,
-					    u8 vf_qid,
-					    struct qed_queue_start_common_params
-					    *p_params);
+struct qed_queue_cid *
+qed_eth_queue_to_cid(struct qed_hwfn *p_hwfn,
+		     u16 opaque_fid,
+		     struct qed_queue_start_common_params *p_params,
+		     bool b_is_rx,
+		     struct qed_queue_cid_vf_params *p_vf_params);
 
 int
 qed_sp_eth_vport_start(struct qed_hwfn *p_hwfn,
diff --git a/drivers/net/ethernet/qlogic/qed/qed_ll2.c b/drivers/net/ethernet/qlogic/qed/qed_ll2.c
index 09c86411918c..c06ad4f0758e 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_ll2.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_ll2.c
@@ -38,7 +38,6 @@
 #include <linux/pci.h>
 #include <linux/slab.h>
 #include <linux/stddef.h>
-#include <linux/version.h>
 #include <linux/workqueue.h>
 #include <net/ipv6.h>
 #include <linux/bitops.h>
@@ -62,7 +61,7 @@
 #include "qed_ooo.h"
 #include "qed_reg_addr.h"
 #include "qed_sp.h"
-#include "qed_roce.h"
+#include "qed_rdma.h"
 
 #define QED_LL2_RX_REGISTERED(ll2)	((ll2)->rx_queue.b_cb_registred)
 #define QED_LL2_TX_REGISTERED(ll2)	((ll2)->tx_queue.b_cb_registred)
@@ -74,7 +73,6 @@ struct qed_cb_ll2_info {
 	int rx_cnt;
 	u32 rx_size;
 	u8 handle;
-	bool frags_mapped;
 
 	/* Lock protecting LL2 buffer lists in sleepless context */
 	spinlock_t lock;
@@ -90,13 +88,14 @@ struct qed_ll2_buffer {
 	dma_addr_t phys_addr;
 };
 
-static void qed_ll2b_complete_tx_packet(struct qed_hwfn *p_hwfn,
+static void qed_ll2b_complete_tx_packet(void *cxt,
 					u8 connection_handle,
 					void *cookie,
 					dma_addr_t first_frag_addr,
 					bool b_last_fragment,
 					bool b_last_packet)
 {
+	struct qed_hwfn *p_hwfn = cxt;
 	struct qed_dev *cdev = p_hwfn->cdev;
 	struct sk_buff *skb = cookie;
 
@@ -108,12 +107,6 @@ static void qed_ll2b_complete_tx_packet(struct qed_hwfn *p_hwfn,
 		cdev->ll2->cbs->tx_cb(cdev->ll2->cb_cookie, skb,
 				      b_last_fragment);
 
-	if (cdev->ll2->frags_mapped)
-		/* Case where mapped frags were received, need to
-		 * free skb with nr_frags marked as 0
-		 */
-		skb_shinfo(skb)->nr_frags = 0;
-
 	dev_kfree_skb_any(skb);
 }
 
@@ -165,42 +158,34 @@ static void qed_ll2_kill_buffers(struct qed_dev *cdev)
 		qed_ll2_dealloc_buffer(cdev, buffer);
 }
 
-static void qed_ll2b_complete_rx_packet(struct qed_hwfn *p_hwfn,
-					u8 connection_handle,
-					struct qed_ll2_rx_packet *p_pkt,
-					struct core_rx_fast_path_cqe *p_cqe,
-					bool b_last_packet)
+void qed_ll2b_complete_rx_packet(void *cxt, struct qed_ll2_comp_rx_data *data)
 {
-	u16 packet_length = le16_to_cpu(p_cqe->packet_length);
-	struct qed_ll2_buffer *buffer = p_pkt->cookie;
+	struct qed_hwfn *p_hwfn = cxt;
+	struct qed_ll2_buffer *buffer = data->cookie;
 	struct qed_dev *cdev = p_hwfn->cdev;
-	u16 vlan = le16_to_cpu(p_cqe->vlan);
-	u32 opaque_data_0, opaque_data_1;
-	u8 pad = p_cqe->placement_offset;
 	dma_addr_t new_phys_addr;
 	struct sk_buff *skb;
 	bool reuse = false;
 	int rc = -EINVAL;
 	u8 *new_data;
 
-	opaque_data_0 = le32_to_cpu(p_cqe->opaque_data.data[0]);
-	opaque_data_1 = le32_to_cpu(p_cqe->opaque_data.data[1]);
-
 	DP_VERBOSE(p_hwfn,
 		   (NETIF_MSG_RX_STATUS | QED_MSG_STORAGE | NETIF_MSG_PKTDATA),
 		   "Got an LL2 Rx completion: [Buffer at phys 0x%llx, offset 0x%02x] Length 0x%04x Parse_flags 0x%04x vlan 0x%04x Opaque data [0x%08x:0x%08x]\n",
-		   (u64)p_pkt->rx_buf_addr, pad, packet_length,
-		   le16_to_cpu(p_cqe->parse_flags.flags), vlan,
-		   opaque_data_0, opaque_data_1);
+		   (u64)data->rx_buf_addr,
+		   data->u.placement_offset,
+		   data->length.packet_length,
+		   data->parse_flags,
+		   data->vlan, data->opaque_data_0, data->opaque_data_1);
 
 	if ((cdev->dp_module & NETIF_MSG_PKTDATA) && buffer->data) {
 		print_hex_dump(KERN_INFO, "",
 			       DUMP_PREFIX_OFFSET, 16, 1,
-			       buffer->data, packet_length, false);
+			       buffer->data, data->length.packet_length, false);
 	}
 
 	/* Determine if data is valid */
-	if (packet_length < ETH_HLEN)
+	if (data->length.packet_length < ETH_HLEN)
 		reuse = true;
 
 	/* Allocate a replacement for buffer; Reuse upon failure */
@@ -220,9 +205,9 @@ static void qed_ll2b_complete_rx_packet(struct qed_hwfn *p_hwfn,
 		goto out_post;
 	}
 
-	pad += NET_SKB_PAD;
-	skb_reserve(skb, pad);
-	skb_put(skb, packet_length);
+	data->u.placement_offset += NET_SKB_PAD;
+	skb_reserve(skb, data->u.placement_offset);
+	skb_put(skb, data->length.packet_length);
 	skb_checksum_none_assert(skb);
 
 	/* Get parital ethernet information instead of eth_type_trans(),
@@ -233,10 +218,12 @@ static void qed_ll2b_complete_rx_packet(struct qed_hwfn *p_hwfn,
 
 	/* Pass SKB onward */
 	if (cdev->ll2->cbs && cdev->ll2->cbs->rx_cb) {
-		if (vlan)
-			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan);
+		if (data->vlan)
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
+					       data->vlan);
 		cdev->ll2->cbs->rx_cb(cdev->ll2->cb_cookie, skb,
-				      opaque_data_0, opaque_data_1);
+				      data->opaque_data_0,
+				      data->opaque_data_1);
 	}
 
 	/* Update Buffer information and update FW producer */
@@ -322,7 +309,7 @@ static void qed_ll2_txq_flush(struct qed_hwfn *p_hwfn, u8 connection_handle)
 		list_del(&p_pkt->list_entry);
 		b_last_packet = list_empty(&p_tx->active_descq);
 		list_add_tail(&p_pkt->list_entry, &p_tx->free_descq);
-		if (p_ll2_conn->conn.conn_type == QED_LL2_TYPE_ISCSI_OOO) {
+		if (p_ll2_conn->input.conn_type == QED_LL2_TYPE_OOO) {
 			struct qed_ooo_buffer *p_buffer;
 
 			p_buffer = (struct qed_ooo_buffer *)p_pkt->cookie;
@@ -334,21 +321,12 @@ static void qed_ll2_txq_flush(struct qed_hwfn *p_hwfn, u8 connection_handle)
 			b_last_frag =
 				p_tx->cur_completing_bd_idx == p_pkt->bd_used;
 			tx_frag = p_pkt->bds_set[0].tx_frag;
-			if (p_ll2_conn->conn.gsi_enable)
-				qed_ll2b_release_tx_gsi_packet(p_hwfn,
-							       p_ll2_conn->
-							       my_id,
-							       p_pkt->cookie,
-							       tx_frag,
-							       b_last_frag,
-							       b_last_packet);
-			else
-				qed_ll2b_complete_tx_packet(p_hwfn,
-							    p_ll2_conn->my_id,
-							    p_pkt->cookie,
-							    tx_frag,
-							    b_last_frag,
-							    b_last_packet);
+			p_ll2_conn->cbs.tx_release_cb(p_ll2_conn->cbs.cookie,
+						      p_ll2_conn->my_id,
+						      p_pkt->cookie,
+						      tx_frag,
+						      b_last_frag,
+						      b_last_packet);
 		}
 	}
 }
@@ -361,7 +339,6 @@ static int qed_ll2_txq_completion(struct qed_hwfn *p_hwfn, void *p_cookie)
 	struct qed_ll2_tx_packet *p_pkt;
 	bool b_last_frag = false;
 	unsigned long flags;
-	dma_addr_t tx_frag;
 	int rc = -EINVAL;
 
 	spin_lock_irqsave(&p_tx->lock, flags);
@@ -402,19 +379,13 @@ static int qed_ll2_txq_completion(struct qed_hwfn *p_hwfn, void *p_cookie)
 		list_add_tail(&p_pkt->list_entry, &p_tx->free_descq);
 
 		spin_unlock_irqrestore(&p_tx->lock, flags);
-		tx_frag = p_pkt->bds_set[0].tx_frag;
-		if (p_ll2_conn->conn.gsi_enable)
-			qed_ll2b_complete_tx_gsi_packet(p_hwfn,
-							p_ll2_conn->my_id,
-							p_pkt->cookie,
-							tx_frag,
-							b_last_frag, !num_bds);
-		else
-			qed_ll2b_complete_tx_packet(p_hwfn,
-						    p_ll2_conn->my_id,
-						    p_pkt->cookie,
-						    tx_frag,
-						    b_last_frag, !num_bds);
+
+		p_ll2_conn->cbs.tx_comp_cb(p_ll2_conn->cbs.cookie,
+					   p_ll2_conn->my_id,
+					   p_pkt->cookie,
+					   p_pkt->bds_set[0].tx_frag,
+					   b_last_frag, !num_bds);
+
 		spin_lock_irqsave(&p_tx->lock, flags);
 	}
 
@@ -425,81 +396,71 @@ out:
 	return rc;
 }
 
-static int
-qed_ll2_rxq_completion_gsi(struct qed_hwfn *p_hwfn,
-			   struct qed_ll2_info *p_ll2_info,
-			   union core_rx_cqe_union *p_cqe,
-			   unsigned long lock_flags, bool b_last_cqe)
+static void qed_ll2_rxq_parse_gsi(struct qed_hwfn *p_hwfn,
+				  union core_rx_cqe_union *p_cqe,
+				  struct qed_ll2_comp_rx_data *data)
 {
-	struct qed_ll2_rx_queue *p_rx = &p_ll2_info->rx_queue;
-	struct qed_ll2_rx_packet *p_pkt = NULL;
-	u16 packet_length, parse_flags, vlan;
-	u32 src_mac_addrhi;
-	u16 src_mac_addrlo;
-
-	if (!list_empty(&p_rx->active_descq))
-		p_pkt = list_first_entry(&p_rx->active_descq,
-					 struct qed_ll2_rx_packet, list_entry);
-	if (!p_pkt) {
-		DP_NOTICE(p_hwfn,
-			  "GSI Rx completion but active_descq is empty\n");
-		return -EIO;
-	}
-
-	list_del(&p_pkt->list_entry);
-	parse_flags = le16_to_cpu(p_cqe->rx_cqe_gsi.parse_flags.flags);
-	packet_length = le16_to_cpu(p_cqe->rx_cqe_gsi.data_length);
-	vlan = le16_to_cpu(p_cqe->rx_cqe_gsi.vlan);
-	src_mac_addrhi = le32_to_cpu(p_cqe->rx_cqe_gsi.src_mac_addrhi);
-	src_mac_addrlo = le16_to_cpu(p_cqe->rx_cqe_gsi.src_mac_addrlo);
-	if (qed_chain_consume(&p_rx->rxq_chain) != p_pkt->rxq_bd)
-		DP_NOTICE(p_hwfn,
-			  "Mismatch between active_descq and the LL2 Rx chain\n");
-	list_add_tail(&p_pkt->list_entry, &p_rx->free_descq);
-
-	spin_unlock_irqrestore(&p_rx->lock, lock_flags);
-	qed_ll2b_complete_rx_gsi_packet(p_hwfn,
-					p_ll2_info->my_id,
-					p_pkt->cookie,
-					p_pkt->rx_buf_addr,
-					packet_length,
-					p_cqe->rx_cqe_gsi.data_length_error,
-					parse_flags,
-					vlan,
-					src_mac_addrhi,
-					src_mac_addrlo, b_last_cqe);
-	spin_lock_irqsave(&p_rx->lock, lock_flags);
+	data->parse_flags = le16_to_cpu(p_cqe->rx_cqe_gsi.parse_flags.flags);
+	data->length.data_length = le16_to_cpu(p_cqe->rx_cqe_gsi.data_length);
+	data->vlan = le16_to_cpu(p_cqe->rx_cqe_gsi.vlan);
+	data->opaque_data_0 = le32_to_cpu(p_cqe->rx_cqe_gsi.src_mac_addrhi);
+	data->opaque_data_1 = le16_to_cpu(p_cqe->rx_cqe_gsi.src_mac_addrlo);
+	data->u.data_length_error = p_cqe->rx_cqe_gsi.data_length_error;
+}
 
-	return 0;
+static void qed_ll2_rxq_parse_reg(struct qed_hwfn *p_hwfn,
+				  union core_rx_cqe_union *p_cqe,
+				  struct qed_ll2_comp_rx_data *data)
+{
+	data->parse_flags = le16_to_cpu(p_cqe->rx_cqe_fp.parse_flags.flags);
+	data->length.packet_length =
+	    le16_to_cpu(p_cqe->rx_cqe_fp.packet_length);
+	data->vlan = le16_to_cpu(p_cqe->rx_cqe_fp.vlan);
+	data->opaque_data_0 = le32_to_cpu(p_cqe->rx_cqe_fp.opaque_data.data[0]);
+	data->opaque_data_1 = le32_to_cpu(p_cqe->rx_cqe_fp.opaque_data.data[1]);
+	data->u.placement_offset = p_cqe->rx_cqe_fp.placement_offset;
 }
 
-static int qed_ll2_rxq_completion_reg(struct qed_hwfn *p_hwfn,
-				      struct qed_ll2_info *p_ll2_conn,
-				      union core_rx_cqe_union *p_cqe,
-				      unsigned long *p_lock_flags,
-				      bool b_last_cqe)
+static int
+qed_ll2_rxq_handle_completion(struct qed_hwfn *p_hwfn,
+			      struct qed_ll2_info *p_ll2_conn,
+			      union core_rx_cqe_union *p_cqe,
+			      unsigned long *p_lock_flags, bool b_last_cqe)
 {
 	struct qed_ll2_rx_queue *p_rx = &p_ll2_conn->rx_queue;
 	struct qed_ll2_rx_packet *p_pkt = NULL;
+	struct qed_ll2_comp_rx_data data;
 
 	if (!list_empty(&p_rx->active_descq))
 		p_pkt = list_first_entry(&p_rx->active_descq,
 					 struct qed_ll2_rx_packet, list_entry);
 	if (!p_pkt) {
 		DP_NOTICE(p_hwfn,
-			  "LL2 Rx completion but active_descq is empty\n");
+			  "[%d] LL2 Rx completion but active_descq is empty\n",
+			  p_ll2_conn->input.conn_type);
+
 		return -EIO;
 	}
 	list_del(&p_pkt->list_entry);
 
+	if (p_cqe->rx_cqe_sp.type == CORE_RX_CQE_TYPE_REGULAR)
+		qed_ll2_rxq_parse_reg(p_hwfn, p_cqe, &data);
+	else
+		qed_ll2_rxq_parse_gsi(p_hwfn, p_cqe, &data);
 	if (qed_chain_consume(&p_rx->rxq_chain) != p_pkt->rxq_bd)
 		DP_NOTICE(p_hwfn,
 			  "Mismatch between active_descq and the LL2 Rx chain\n");
+
 	list_add_tail(&p_pkt->list_entry, &p_rx->free_descq);
 
+	data.connection_handle = p_ll2_conn->my_id;
+	data.cookie = p_pkt->cookie;
+	data.rx_buf_addr = p_pkt->rx_buf_addr;
+	data.b_last_packet = b_last_cqe;
+
 	spin_unlock_irqrestore(&p_rx->lock, *p_lock_flags);
-	qed_ll2b_complete_rx_packet(p_hwfn, p_ll2_conn->my_id,
-				    p_pkt, &p_cqe->rx_cqe_fp, b_last_cqe);
+	p_ll2_conn->cbs.rx_comp_cb(p_ll2_conn->cbs.cookie, &data);
+
 	spin_lock_irqsave(&p_rx->lock, *p_lock_flags);
 
 	return 0;
@@ -507,7 +468,7 @@ static int qed_ll2_rxq_completion_reg(struct qed_hwfn *p_hwfn,
 
 static int qed_ll2_rxq_completion(struct qed_hwfn *p_hwfn, void *cookie)
 {
-	struct qed_ll2_info *p_ll2_conn = cookie;
+	struct qed_ll2_info *p_ll2_conn = (struct qed_ll2_info *)cookie;
 	struct qed_ll2_rx_queue *p_rx = &p_ll2_conn->rx_queue;
 	union core_rx_cqe_union *cqe = NULL;
 	u16 cq_new_idx = 0, cq_old_idx = 0;
@@ -521,7 +482,9 @@ static int qed_ll2_rxq_completion(struct qed_hwfn *p_hwfn, void *cookie)
 	while (cq_new_idx != cq_old_idx) {
 		bool b_last_cqe = (cq_new_idx == cq_old_idx);
 
-		cqe = qed_chain_consume(&p_rx->rcq_chain);
+		cqe =
+		    (union core_rx_cqe_union *)
+		    qed_chain_consume(&p_rx->rcq_chain);
 		cq_old_idx = qed_chain_get_cons_idx(&p_rx->rcq_chain);
 
 		DP_VERBOSE(p_hwfn,
@@ -535,13 +498,10 @@ static int qed_ll2_rxq_completion(struct qed_hwfn *p_hwfn, void *cookie)
 			rc = -EINVAL;
 			break;
 		case CORE_RX_CQE_TYPE_GSI_OFFLOAD:
-			rc = qed_ll2_rxq_completion_gsi(p_hwfn, p_ll2_conn,
-							cqe, flags, b_last_cqe);
-			break;
 		case CORE_RX_CQE_TYPE_REGULAR:
-			rc = qed_ll2_rxq_completion_reg(p_hwfn, p_ll2_conn,
-							cqe, &flags,
-							b_last_cqe);
+			rc = qed_ll2_rxq_handle_completion(p_hwfn, p_ll2_conn,
+							   cqe, &flags,
+							   b_last_cqe);
 			break;
 		default:
 			rc = -EIO;
@@ -565,10 +525,6 @@ static void qed_ll2_rxq_flush(struct qed_hwfn *p_hwfn, u8 connection_handle)
 	p_rx = &p_ll2_conn->rx_queue;
 
 	while (!list_empty(&p_rx->active_descq)) {
-		dma_addr_t rx_buf_addr;
-		void *cookie;
-		bool b_last;
-
 		p_pkt = list_first_entry(&p_rx->active_descq,
 					 struct qed_ll2_rx_packet, list_entry);
 		if (!p_pkt)
@@ -576,22 +532,26 @@ static void qed_ll2_rxq_flush(struct qed_hwfn *p_hwfn, u8 connection_handle)
 
 		list_move_tail(&p_pkt->list_entry, &p_rx->free_descq);
 
-		if (p_ll2_conn->conn.conn_type == QED_LL2_TYPE_ISCSI_OOO) {
+		if (p_ll2_conn->input.conn_type == QED_LL2_TYPE_OOO) {
 			struct qed_ooo_buffer *p_buffer;
 
 			p_buffer = (struct qed_ooo_buffer *)p_pkt->cookie;
 			qed_ooo_put_free_buffer(p_hwfn, p_hwfn->p_ooo_info,
 						p_buffer);
 		} else {
-			rx_buf_addr = p_pkt->rx_buf_addr;
-			cookie = p_pkt->cookie;
+			dma_addr_t rx_buf_addr = p_pkt->rx_buf_addr;
+			void *cookie = p_pkt->cookie;
+			bool b_last;
 
 			b_last = list_empty(&p_rx->active_descq);
+			p_ll2_conn->cbs.rx_release_cb(p_ll2_conn->cbs.cookie,
+						      p_ll2_conn->my_id,
+						      cookie,
+						      rx_buf_addr, b_last);
 		}
 	}
 }
 
-#if IS_ENABLED(CONFIG_QED_ISCSI)
 static u8 qed_ll2_convert_rx_parse_to_tx_flags(u16 parse_flags)
 {
 	u8 bd_flags = 0;
@@ -741,12 +701,13 @@ static void
 qed_ooo_submit_tx_buffers(struct qed_hwfn *p_hwfn,
 			  struct qed_ll2_info *p_ll2_conn)
 {
+	struct qed_ll2_tx_pkt_info tx_pkt;
 	struct qed_ooo_buffer *p_buffer;
-	int rc;
 	u16 l4_hdr_offset_w;
 	dma_addr_t first_frag;
 	u16 parse_flags;
 	u8 bd_flags;
+	int rc;
 
 	/* Submit Tx buffers here */
 	while ((p_buffer = qed_ooo_get_ready_buffer(p_hwfn,
@@ -761,13 +722,18 @@ qed_ooo_submit_tx_buffers(struct qed_hwfn *p_hwfn,
 		SET_FIELD(bd_flags, CORE_TX_BD_DATA_FORCE_VLAN_MODE, 1);
 		SET_FIELD(bd_flags, CORE_TX_BD_DATA_L4_PROTOCOL, 1);
 
-		rc = qed_ll2_prepare_tx_packet(p_hwfn, p_ll2_conn->my_id, 1,
-					       p_buffer->vlan, bd_flags,
-					       l4_hdr_offset_w,
-					       p_ll2_conn->conn.tx_dest, 0,
-					       first_frag,
-					       p_buffer->packet_length,
-					       p_buffer, true);
+		memset(&tx_pkt, 0, sizeof(tx_pkt));
+		tx_pkt.num_of_bds = 1;
+		tx_pkt.vlan = p_buffer->vlan;
+		tx_pkt.bd_flags = bd_flags;
+		tx_pkt.l4_hdr_offset_w = l4_hdr_offset_w;
+		tx_pkt.tx_dest = p_ll2_conn->tx_dest;
+		tx_pkt.first_frag = first_frag;
+		tx_pkt.first_frag_len = p_buffer->packet_length;
+		tx_pkt.cookie = p_buffer;
+
+		rc = qed_ll2_prepare_tx_packet(p_hwfn, p_ll2_conn->my_id,
+					       &tx_pkt, true);
 		if (rc) {
 			qed_ooo_put_ready_buffer(p_hwfn, p_hwfn->p_ooo_info,
 						 p_buffer, false);
@@ -874,85 +840,6 @@ static int qed_ll2_lb_txq_completion(struct qed_hwfn *p_hwfn, void *p_cookie)
 	return 0;
 }
 
-static int
-qed_ll2_acquire_connection_ooo(struct qed_hwfn *p_hwfn,
-			       struct qed_ll2_info *p_ll2_info,
-			       u16 rx_num_ooo_buffers, u16 mtu)
-{
-	struct qed_ooo_buffer *p_buf = NULL;
-	void *p_virt;
-	u16 buf_idx;
-	int rc = 0;
-
-	if (p_ll2_info->conn.conn_type != QED_LL2_TYPE_ISCSI_OOO)
-		return rc;
-
-	if (!rx_num_ooo_buffers)
-		return -EINVAL;
-
-	for (buf_idx = 0; buf_idx < rx_num_ooo_buffers; buf_idx++) {
-		p_buf = kzalloc(sizeof(*p_buf), GFP_KERNEL);
-		if (!p_buf) {
-			rc = -ENOMEM;
-			goto out;
-		}
-
-		p_buf->rx_buffer_size = mtu + 26 + ETH_CACHE_LINE_SIZE;
-		p_buf->rx_buffer_size = (p_buf->rx_buffer_size +
-					 ETH_CACHE_LINE_SIZE - 1) &
-					~(ETH_CACHE_LINE_SIZE - 1);
-		p_virt = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
-					    p_buf->rx_buffer_size,
-					    &p_buf->rx_buffer_phys_addr,
-					    GFP_KERNEL);
-		if (!p_virt) {
-			kfree(p_buf);
-			rc = -ENOMEM;
-			goto out;
-		}
-
-		p_buf->rx_buffer_virt_addr = p_virt;
-		qed_ooo_put_free_buffer(p_hwfn, p_hwfn->p_ooo_info, p_buf);
-	}
-
-	DP_VERBOSE(p_hwfn, QED_MSG_LL2,
-		   "Allocated [%04x] LL2 OOO buffers [each of size 0x%08x]\n",
-		   rx_num_ooo_buffers, p_buf->rx_buffer_size);
-
-out:
-	return rc;
-}
-
-static void
-qed_ll2_establish_connection_ooo(struct qed_hwfn *p_hwfn,
-				 struct qed_ll2_info *p_ll2_conn)
-{
-	if (p_ll2_conn->conn.conn_type != QED_LL2_TYPE_ISCSI_OOO)
-		return;
-
-	qed_ooo_release_all_isles(p_hwfn, p_hwfn->p_ooo_info);
-	qed_ooo_submit_rx_buffers(p_hwfn, p_ll2_conn);
-}
-
-static void qed_ll2_release_connection_ooo(struct qed_hwfn *p_hwfn,
-					   struct qed_ll2_info *p_ll2_conn)
-{
-	struct qed_ooo_buffer *p_buffer;
-
-	if (p_ll2_conn->conn.conn_type != QED_LL2_TYPE_ISCSI_OOO)
-		return;
-
-	qed_ooo_release_all_isles(p_hwfn, p_hwfn->p_ooo_info);
-	while ((p_buffer = qed_ooo_get_free_buffer(p_hwfn,
-						   p_hwfn->p_ooo_info))) {
-		dma_free_coherent(&p_hwfn->cdev->pdev->dev,
-				  p_buffer->rx_buffer_size,
-				  p_buffer->rx_buffer_virt_addr,
-				  p_buffer->rx_buffer_phys_addr);
-		kfree(p_buffer);
-	}
-}
-
 static void qed_ll2_stop_ooo(struct qed_dev *cdev)
 {
 	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
@@ -966,69 +853,11 @@ static void qed_ll2_stop_ooo(struct qed_dev *cdev)
 	*handle = QED_LL2_UNUSED_HANDLE;
 }
 
-static int qed_ll2_start_ooo(struct qed_dev *cdev,
-			     struct qed_ll2_params *params)
-{
-	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
-	u8 *handle = &hwfn->pf_params.iscsi_pf_params.ll2_ooo_queue_id;
-	struct qed_ll2_conn ll2_info = { 0 };
-	int rc;
-
-	ll2_info.conn_type = QED_LL2_TYPE_ISCSI_OOO;
-	ll2_info.mtu = params->mtu;
-	ll2_info.rx_drop_ttl0_flg = params->drop_ttl0_packets;
-	ll2_info.rx_vlan_removal_en = params->rx_vlan_stripping;
-	ll2_info.tx_tc = OOO_LB_TC;
-	ll2_info.tx_dest = CORE_TX_DEST_LB;
-
-	rc = qed_ll2_acquire_connection(hwfn, &ll2_info,
-					QED_LL2_RX_SIZE, QED_LL2_TX_SIZE,
-					handle);
-	if (rc) {
-		DP_INFO(cdev, "Failed to acquire LL2 OOO connection\n");
-		goto out;
-	}
-
-	rc = qed_ll2_establish_connection(hwfn, *handle);
-	if (rc) {
-		DP_INFO(cdev, "Failed to establist LL2 OOO connection\n");
-		goto fail;
-	}
-
-	return 0;
-
-fail:
-	qed_ll2_release_connection(hwfn, *handle);
-out:
-	*handle = QED_LL2_UNUSED_HANDLE;
-	return rc;
-}
-#else /* IS_ENABLED(CONFIG_QED_ISCSI) */
-static int qed_ll2_lb_rxq_completion(struct qed_hwfn *p_hwfn,
-				     void *p_cookie) { return -EINVAL; }
-static int qed_ll2_lb_txq_completion(struct qed_hwfn *p_hwfn,
-				     void *p_cookie) { return -EINVAL; }
-static inline int
-qed_ll2_acquire_connection_ooo(struct qed_hwfn *p_hwfn,
-			       struct qed_ll2_info *p_ll2_info,
-			       u16 rx_num_ooo_buffers, u16 mtu) { return 0; }
-static inline void
-qed_ll2_establish_connection_ooo(struct qed_hwfn *p_hwfn,
-				 struct qed_ll2_info *p_ll2_conn) { return; }
-static inline void
-qed_ll2_release_connection_ooo(struct qed_hwfn *p_hwfn,
-			       struct qed_ll2_info *p_ll2_conn) { return; }
-static inline void qed_ll2_stop_ooo(struct qed_dev *cdev) { return; }
-static inline int qed_ll2_start_ooo(struct qed_dev *cdev,
-				    struct qed_ll2_params *params)
-				    { return -EINVAL; }
-#endif /* IS_ENABLED(CONFIG_QED_ISCSI) */
-
 static int qed_sp_ll2_rx_queue_start(struct qed_hwfn *p_hwfn,
 				     struct qed_ll2_info *p_ll2_conn,
 				     u8 action_on_error)
 {
-	enum qed_ll2_conn_type conn_type = p_ll2_conn->conn.conn_type;
+	enum qed_ll2_conn_type conn_type = p_ll2_conn->input.conn_type;
 	struct qed_ll2_rx_queue *p_rx = &p_ll2_conn->rx_queue;
 	struct core_rx_start_ramrod_data *p_ramrod = NULL;
 	struct qed_spq_entry *p_ent = NULL;
@@ -1054,22 +883,21 @@ static int qed_sp_ll2_rx_queue_start(struct qed_hwfn *p_hwfn,
 	p_ramrod->sb_index = p_rx->rx_sb_index;
 	p_ramrod->complete_event_flg = 1;
 
-	p_ramrod->mtu = cpu_to_le16(p_ll2_conn->conn.mtu);
-	DMA_REGPAIR_LE(p_ramrod->bd_base,
-		       p_rx->rxq_chain.p_phys_addr);
+	p_ramrod->mtu = cpu_to_le16(p_ll2_conn->input.mtu);
+	DMA_REGPAIR_LE(p_ramrod->bd_base, p_rx->rxq_chain.p_phys_addr);
 	cqe_pbl_size = (u16)qed_chain_get_page_cnt(&p_rx->rcq_chain);
 	p_ramrod->num_of_pbl_pages = cpu_to_le16(cqe_pbl_size);
 	DMA_REGPAIR_LE(p_ramrod->cqe_pbl_addr,
 		       qed_chain_get_pbl_phys(&p_rx->rcq_chain));
 
-	p_ramrod->drop_ttl0_flg = p_ll2_conn->conn.rx_drop_ttl0_flg;
-	p_ramrod->inner_vlan_removal_en = p_ll2_conn->conn.rx_vlan_removal_en;
+	p_ramrod->drop_ttl0_flg = p_ll2_conn->input.rx_drop_ttl0_flg;
+	p_ramrod->inner_vlan_removal_en = p_ll2_conn->input.rx_vlan_removal_en;
 	p_ramrod->queue_id = p_ll2_conn->queue_id;
-	p_ramrod->main_func_queue = (conn_type == QED_LL2_TYPE_ISCSI_OOO) ? 0
-									  : 1;
+	p_ramrod->main_func_queue = (conn_type == QED_LL2_TYPE_OOO) ? 0 : 1;
 
 	if ((IS_MF_DEFAULT(p_hwfn) || IS_MF_SI(p_hwfn)) &&
-	    p_ramrod->main_func_queue && (conn_type != QED_LL2_TYPE_ROCE)) {
+	    p_ramrod->main_func_queue && (conn_type != QED_LL2_TYPE_ROCE) &&
+	    (conn_type != QED_LL2_TYPE_IWARP)) {
 		p_ramrod->mf_si_bcast_accept_all = 1;
 		p_ramrod->mf_si_mcast_accept_all = 1;
 	} else {
@@ -1078,14 +906,14 @@ static int qed_sp_ll2_rx_queue_start(struct qed_hwfn *p_hwfn,
 	}
 
 	p_ramrod->action_on_error.error_type = action_on_error;
-	p_ramrod->gsi_offload_flag = p_ll2_conn->conn.gsi_enable;
+	p_ramrod->gsi_offload_flag = p_ll2_conn->input.gsi_enable;
 	return qed_spq_post(p_hwfn, p_ent, NULL);
 }
 
 static int qed_sp_ll2_tx_queue_start(struct qed_hwfn *p_hwfn,
 				     struct qed_ll2_info *p_ll2_conn)
 {
-	enum qed_ll2_conn_type conn_type = p_ll2_conn->conn.conn_type;
+	enum qed_ll2_conn_type conn_type = p_ll2_conn->input.conn_type;
 	struct qed_ll2_tx_queue *p_tx = &p_ll2_conn->tx_queue;
 	struct core_tx_start_ramrod_data *p_ramrod = NULL;
 	struct qed_spq_entry *p_ent = NULL;
@@ -1096,7 +924,7 @@ static int qed_sp_ll2_tx_queue_start(struct qed_hwfn *p_hwfn,
 	if (!QED_LL2_TX_REGISTERED(p_ll2_conn))
 		return 0;
 
-	if (p_ll2_conn->conn.conn_type == QED_LL2_TYPE_ISCSI_OOO)
+	if (p_ll2_conn->input.conn_type == QED_LL2_TYPE_OOO)
 		p_ll2_conn->tx_stats_en = 0;
 	else
 		p_ll2_conn->tx_stats_en = 1;
@@ -1117,7 +945,7 @@ static int qed_sp_ll2_tx_queue_start(struct qed_hwfn *p_hwfn,
 
 	p_ramrod->sb_id = cpu_to_le16(qed_int_get_sp_sb_id(p_hwfn));
 	p_ramrod->sb_index = p_tx->tx_sb_index;
-	p_ramrod->mtu = cpu_to_le16(p_ll2_conn->conn.mtu);
+	p_ramrod->mtu = cpu_to_le16(p_ll2_conn->input.mtu);
 	p_ramrod->stats_en = p_ll2_conn->tx_stats_en;
 	p_ramrod->stats_id = p_ll2_conn->tx_stats_id;
 
@@ -1126,11 +954,11 @@ static int qed_sp_ll2_tx_queue_start(struct qed_hwfn *p_hwfn,
 	pbl_size = qed_chain_get_page_cnt(&p_tx->txq_chain);
 	p_ramrod->pbl_size = cpu_to_le16(pbl_size);
 
-	switch (p_ll2_conn->conn.tx_tc) {
-	case LB_TC:
+	switch (p_ll2_conn->input.tx_tc) {
+	case PURE_LB_TC:
 		pq_id = qed_get_cm_pq_idx(p_hwfn, PQ_FLAGS_LB);
 		break;
-	case OOO_LB_TC:
+	case PKT_LB_TC:
 		pq_id = qed_get_cm_pq_idx(p_hwfn, PQ_FLAGS_OOO);
 		break;
 	default:
@@ -1145,18 +973,27 @@ static int qed_sp_ll2_tx_queue_start(struct qed_hwfn *p_hwfn,
 		p_ramrod->conn_type = PROTOCOLID_FCOE;
 		break;
 	case QED_LL2_TYPE_ISCSI:
-	case QED_LL2_TYPE_ISCSI_OOO:
 		p_ramrod->conn_type = PROTOCOLID_ISCSI;
 		break;
 	case QED_LL2_TYPE_ROCE:
 		p_ramrod->conn_type = PROTOCOLID_ROCE;
 		break;
+	case QED_LL2_TYPE_IWARP:
+		p_ramrod->conn_type = PROTOCOLID_IWARP;
+		break;
+	case QED_LL2_TYPE_OOO:
+		if (p_hwfn->hw_info.personality == QED_PCI_ISCSI)
+			p_ramrod->conn_type = PROTOCOLID_ISCSI;
+		else
+			p_ramrod->conn_type = PROTOCOLID_IWARP;
+		break;
 	default:
 		p_ramrod->conn_type = PROTOCOLID_ETH;
 		DP_NOTICE(p_hwfn, "Unknown connection type: %d\n", conn_type);
 	}
 
-	p_ramrod->gsi_offload_flag = p_ll2_conn->conn.gsi_enable;
+	p_ramrod->gsi_offload_flag = p_ll2_conn->input.gsi_enable;
+
 	return qed_spq_post(p_hwfn, p_ent, NULL);
 }
 
@@ -1212,22 +1049,22 @@ static int qed_sp_ll2_tx_queue_stop(struct qed_hwfn *p_hwfn,
 
 static int
 qed_ll2_acquire_connection_rx(struct qed_hwfn *p_hwfn,
-			      struct qed_ll2_info *p_ll2_info, u16 rx_num_desc)
+			      struct qed_ll2_info *p_ll2_info)
 {
 	struct qed_ll2_rx_packet *p_descq;
 	u32 capacity;
 	int rc = 0;
 
-	if (!rx_num_desc)
+	if (!p_ll2_info->input.rx_num_desc)
 		goto out;
 
 	rc = qed_chain_alloc(p_hwfn->cdev,
 			     QED_CHAIN_USE_TO_CONSUME_PRODUCE,
 			     QED_CHAIN_MODE_NEXT_PTR,
 			     QED_CHAIN_CNT_TYPE_U16,
-			     rx_num_desc,
+			     p_ll2_info->input.rx_num_desc,
 			     sizeof(struct core_rx_bd),
-			     &p_ll2_info->rx_queue.rxq_chain);
+			     &p_ll2_info->rx_queue.rxq_chain, NULL);
 	if (rc) {
 		DP_NOTICE(p_hwfn, "Failed to allocate ll2 rxq chain\n");
 		goto out;
@@ -1247,9 +1084,9 @@ qed_ll2_acquire_connection_rx(struct qed_hwfn *p_hwfn,
 			     QED_CHAIN_USE_TO_CONSUME_PRODUCE,
 			     QED_CHAIN_MODE_PBL,
 			     QED_CHAIN_CNT_TYPE_U16,
-			     rx_num_desc,
+			     p_ll2_info->input.rx_num_desc,
 			     sizeof(struct core_rx_fast_path_cqe),
-			     &p_ll2_info->rx_queue.rcq_chain);
+			     &p_ll2_info->rx_queue.rcq_chain, NULL);
 	if (rc) {
 		DP_NOTICE(p_hwfn, "Failed to allocate ll2 rcq chain\n");
 		goto out;
@@ -1257,30 +1094,29 @@ qed_ll2_acquire_connection_rx(struct qed_hwfn *p_hwfn,
 
 	DP_VERBOSE(p_hwfn, QED_MSG_LL2,
 		   "Allocated LL2 Rxq [Type %08x] with 0x%08x buffers\n",
-		   p_ll2_info->conn.conn_type, rx_num_desc);
+		   p_ll2_info->input.conn_type, p_ll2_info->input.rx_num_desc);
 
 out:
 	return rc;
 }
 
 static int qed_ll2_acquire_connection_tx(struct qed_hwfn *p_hwfn,
-					 struct qed_ll2_info *p_ll2_info,
-					 u16 tx_num_desc)
+					 struct qed_ll2_info *p_ll2_info)
 {
 	struct qed_ll2_tx_packet *p_descq;
 	u32 capacity;
 	int rc = 0;
 
-	if (!tx_num_desc)
+	if (!p_ll2_info->input.tx_num_desc)
 		goto out;
 
 	rc = qed_chain_alloc(p_hwfn->cdev,
 			     QED_CHAIN_USE_TO_CONSUME_PRODUCE,
 			     QED_CHAIN_MODE_PBL,
 			     QED_CHAIN_CNT_TYPE_U16,
-			     tx_num_desc,
+			     p_ll2_info->input.tx_num_desc,
 			     sizeof(struct core_tx_bd),
-			     &p_ll2_info->tx_queue.txq_chain);
+			     &p_ll2_info->tx_queue.txq_chain, NULL);
 	if (rc)
 		goto out;
 
@@ -1295,28 +1131,112 @@ static int qed_ll2_acquire_connection_tx(struct qed_hwfn *p_hwfn,
 
 	DP_VERBOSE(p_hwfn, QED_MSG_LL2,
 		   "Allocated LL2 Txq [Type %08x] with 0x%08x buffers\n",
-		   p_ll2_info->conn.conn_type, tx_num_desc);
+		   p_ll2_info->input.conn_type, p_ll2_info->input.tx_num_desc);
 
 out:
 	if (rc)
 		DP_NOTICE(p_hwfn,
 			  "Can't allocate memory for Tx LL2 with 0x%08x buffers\n",
-			  tx_num_desc);
+			  p_ll2_info->input.tx_num_desc);
+	return rc;
+}
+
+static int
+qed_ll2_acquire_connection_ooo(struct qed_hwfn *p_hwfn,
+			       struct qed_ll2_info *p_ll2_info, u16 mtu)
+{
+	struct qed_ooo_buffer *p_buf = NULL;
+	void *p_virt;
+	u16 buf_idx;
+	int rc = 0;
+
+	if (p_ll2_info->input.conn_type != QED_LL2_TYPE_OOO)
+		return rc;
+
+	/* Correct number of requested OOO buffers if needed */
+	if (!p_ll2_info->input.rx_num_ooo_buffers) {
+		u16 num_desc = p_ll2_info->input.rx_num_desc;
+
+		if (!num_desc)
+			return -EINVAL;
+		p_ll2_info->input.rx_num_ooo_buffers = num_desc * 2;
+	}
+
+	for (buf_idx = 0; buf_idx < p_ll2_info->input.rx_num_ooo_buffers;
+	     buf_idx++) {
+		p_buf = kzalloc(sizeof(*p_buf), GFP_KERNEL);
+		if (!p_buf) {
+			rc = -ENOMEM;
+			goto out;
+		}
+
+		p_buf->rx_buffer_size = mtu + 26 + ETH_CACHE_LINE_SIZE;
+		p_buf->rx_buffer_size = (p_buf->rx_buffer_size +
+					 ETH_CACHE_LINE_SIZE - 1) &
+					~(ETH_CACHE_LINE_SIZE - 1);
+		p_virt = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
+					    p_buf->rx_buffer_size,
+					    &p_buf->rx_buffer_phys_addr,
+					    GFP_KERNEL);
+		if (!p_virt) {
+			kfree(p_buf);
+			rc = -ENOMEM;
+			goto out;
+		}
+
+		p_buf->rx_buffer_virt_addr = p_virt;
+		qed_ooo_put_free_buffer(p_hwfn, p_hwfn->p_ooo_info, p_buf);
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_LL2,
+		   "Allocated [%04x] LL2 OOO buffers [each of size 0x%08x]\n",
+		   p_ll2_info->input.rx_num_ooo_buffers, p_buf->rx_buffer_size);
+
+out:
 	return rc;
 }
 
-int qed_ll2_acquire_connection(struct qed_hwfn *p_hwfn,
-			       struct qed_ll2_conn *p_params,
-			       u16 rx_num_desc,
-			       u16 tx_num_desc,
-			       u8 *p_connection_handle)
+static int
+qed_ll2_set_cbs(struct qed_ll2_info *p_ll2_info, const struct qed_ll2_cbs *cbs)
 {
+	if (!cbs || (!cbs->rx_comp_cb ||
+		     !cbs->rx_release_cb ||
+		     !cbs->tx_comp_cb || !cbs->tx_release_cb || !cbs->cookie))
+		return -EINVAL;
+
+	p_ll2_info->cbs.rx_comp_cb = cbs->rx_comp_cb;
+	p_ll2_info->cbs.rx_release_cb = cbs->rx_release_cb;
+	p_ll2_info->cbs.tx_comp_cb = cbs->tx_comp_cb;
+	p_ll2_info->cbs.tx_release_cb = cbs->tx_release_cb;
+	p_ll2_info->cbs.cookie = cbs->cookie;
+
+	return 0;
+}
+
+static enum core_error_handle
+qed_ll2_get_error_choice(enum qed_ll2_error_handle err)
+{
+	switch (err) {
+	case QED_LL2_DROP_PACKET:
+		return LL2_DROP_PACKET;
+	case QED_LL2_DO_NOTHING:
+		return LL2_DO_NOTHING;
+	case QED_LL2_ASSERT:
+		return LL2_ASSERT;
+	default:
+		return LL2_DO_NOTHING;
+	}
+}
+
+int qed_ll2_acquire_connection(void *cxt, struct qed_ll2_acquire_data *data)
+{
+	struct qed_hwfn *p_hwfn = cxt;
 	qed_int_comp_cb_t comp_rx_cb, comp_tx_cb;
 	struct qed_ll2_info *p_ll2_info = NULL;
+	u8 i, *p_tx_max;
 	int rc;
-	u8 i;
 
-	if (!p_connection_handle || !p_hwfn->p_ll2_info)
+	if (!data->p_connection_handle || !p_hwfn->p_ll2_info)
 		return -EINVAL;
 
 	/* Find a free connection to be used */
@@ -1335,23 +1255,40 @@ int qed_ll2_acquire_connection(struct qed_hwfn *p_hwfn,
 	if (!p_ll2_info)
 		return -EBUSY;
 
-	p_ll2_info->conn = *p_params;
+	memcpy(&p_ll2_info->input, &data->input, sizeof(p_ll2_info->input));
+
+	p_ll2_info->tx_dest = (data->input.tx_dest == QED_LL2_TX_DEST_NW) ?
+			      CORE_TX_DEST_NW : CORE_TX_DEST_LB;
 
-	rc = qed_ll2_acquire_connection_rx(p_hwfn, p_ll2_info, rx_num_desc);
+	/* Correct maximum number of Tx BDs */
+	p_tx_max = &p_ll2_info->input.tx_max_bds_per_packet;
+	if (*p_tx_max == 0)
+		*p_tx_max = CORE_LL2_TX_MAX_BDS_PER_PACKET;
+	else
+		*p_tx_max = min_t(u8, *p_tx_max,
+				  CORE_LL2_TX_MAX_BDS_PER_PACKET);
+
+	rc = qed_ll2_set_cbs(p_ll2_info, data->cbs);
+	if (rc) {
+		DP_NOTICE(p_hwfn, "Invalid callback functions\n");
+		goto q_allocate_fail;
+	}
+
+	rc = qed_ll2_acquire_connection_rx(p_hwfn, p_ll2_info);
 	if (rc)
 		goto q_allocate_fail;
 
-	rc = qed_ll2_acquire_connection_tx(p_hwfn, p_ll2_info, tx_num_desc);
+	rc = qed_ll2_acquire_connection_tx(p_hwfn, p_ll2_info);
 	if (rc)
 		goto q_allocate_fail;
 
 	rc = qed_ll2_acquire_connection_ooo(p_hwfn, p_ll2_info,
-					    rx_num_desc * 2, p_params->mtu);
+					    data->input.mtu);
 	if (rc)
 		goto q_allocate_fail;
 
 	/* Register callbacks for the Rx/Tx queues */
-	if (p_params->conn_type == QED_LL2_TYPE_ISCSI_OOO) {
+	if (data->input.conn_type == QED_LL2_TYPE_OOO) {
 		comp_rx_cb = qed_ll2_lb_rxq_completion;
 		comp_tx_cb = qed_ll2_lb_txq_completion;
 	} else {
@@ -1359,7 +1296,7 @@ int qed_ll2_acquire_connection(struct qed_hwfn *p_hwfn,
 		comp_tx_cb = qed_ll2_txq_completion;
 	}
 
-	if (rx_num_desc) {
+	if (data->input.rx_num_desc) {
 		qed_int_register_cb(p_hwfn, comp_rx_cb,
 				    &p_hwfn->p_ll2_info[i],
 				    &p_ll2_info->rx_queue.rx_sb_index,
@@ -1367,7 +1304,7 @@ int qed_ll2_acquire_connection(struct qed_hwfn *p_hwfn,
 		p_ll2_info->rx_queue.b_cb_registred = true;
 	}
 
-	if (tx_num_desc) {
+	if (data->input.tx_num_desc) {
 		qed_int_register_cb(p_hwfn,
 				    comp_tx_cb,
 				    &p_hwfn->p_ll2_info[i],
@@ -1376,7 +1313,7 @@ int qed_ll2_acquire_connection(struct qed_hwfn *p_hwfn,
 		p_ll2_info->tx_queue.b_cb_registred = true;
 	}
 
-	*p_connection_handle = i;
+	*data->p_connection_handle = i;
 	return rc;
 
 q_allocate_fail:
@@ -1387,24 +1324,39 @@ q_allocate_fail:
 static int qed_ll2_establish_connection_rx(struct qed_hwfn *p_hwfn,
 					   struct qed_ll2_info *p_ll2_conn)
 {
+	enum qed_ll2_error_handle error_input;
+	enum core_error_handle error_mode;
 	u8 action_on_error = 0;
 
 	if (!QED_LL2_RX_REGISTERED(p_ll2_conn))
 		return 0;
 
 	DIRECT_REG_WR(p_ll2_conn->rx_queue.set_prod_addr, 0x0);
-
+	error_input = p_ll2_conn->input.ai_err_packet_too_big;
+	error_mode = qed_ll2_get_error_choice(error_input);
 	SET_FIELD(action_on_error,
-		  CORE_RX_ACTION_ON_ERROR_PACKET_TOO_BIG,
-		  p_ll2_conn->conn.ai_err_packet_too_big);
-	SET_FIELD(action_on_error,
-		  CORE_RX_ACTION_ON_ERROR_NO_BUFF, p_ll2_conn->conn.ai_err_no_buf);
+		  CORE_RX_ACTION_ON_ERROR_PACKET_TOO_BIG, error_mode);
+	error_input = p_ll2_conn->input.ai_err_no_buf;
+	error_mode = qed_ll2_get_error_choice(error_input);
+	SET_FIELD(action_on_error, CORE_RX_ACTION_ON_ERROR_NO_BUFF, error_mode);
 
 	return qed_sp_ll2_rx_queue_start(p_hwfn, p_ll2_conn, action_on_error);
 }
 
-int qed_ll2_establish_connection(struct qed_hwfn *p_hwfn, u8 connection_handle)
+static void
+qed_ll2_establish_connection_ooo(struct qed_hwfn *p_hwfn,
+				 struct qed_ll2_info *p_ll2_conn)
 {
+	if (p_ll2_conn->input.conn_type != QED_LL2_TYPE_OOO)
+		return;
+
+	qed_ooo_release_all_isles(p_hwfn, p_hwfn->p_ooo_info);
+	qed_ooo_submit_rx_buffers(p_hwfn, p_ll2_conn);
+}
+
+int qed_ll2_establish_connection(void *cxt, u8 connection_handle)
+{
+	struct qed_hwfn *p_hwfn = cxt;
 	struct qed_ll2_info *p_ll2_conn;
 	struct qed_ll2_rx_queue *p_rx;
 	struct qed_ll2_tx_queue *p_tx;
@@ -1477,12 +1429,12 @@ int qed_ll2_establish_connection(struct qed_hwfn *p_hwfn, u8 connection_handle)
 	if (rc)
 		goto out;
 
-	if (p_hwfn->hw_info.personality != QED_PCI_ETH_ROCE)
+	if (!QED_IS_RDMA_PERSONALITY(p_hwfn))
 		qed_wr(p_hwfn, p_ptt, PRS_REG_USE_LIGHT_L2, 1);
 
 	qed_ll2_establish_connection_ooo(p_hwfn, p_ll2_conn);
 
-	if (p_ll2_conn->conn.conn_type == QED_LL2_TYPE_FCOE) {
+	if (p_ll2_conn->input.conn_type == QED_LL2_TYPE_FCOE) {
 		qed_llh_add_protocol_filter(p_hwfn, p_ptt,
 					    0x8906, 0,
 					    QED_LLH_FILTER_ETHERTYPE);
@@ -1531,11 +1483,12 @@ static void qed_ll2_post_rx_buffer_notify_fw(struct qed_hwfn *p_hwfn,
 	DIRECT_REG_WR(p_rx->set_prod_addr, *((u32 *)&rx_prod));
 }
 
-int qed_ll2_post_rx_buffer(struct qed_hwfn *p_hwfn,
+int qed_ll2_post_rx_buffer(void *cxt,
 			   u8 connection_handle,
 			   dma_addr_t addr,
 			   u16 buf_len, void *cookie, u8 notify_fw)
 {
+	struct qed_hwfn *p_hwfn = cxt;
 	struct core_rx_bd_with_buff_len *p_curb = NULL;
 	struct qed_ll2_rx_packet *p_curp = NULL;
 	struct qed_ll2_info *p_ll2_conn;
@@ -1594,20 +1547,18 @@ out:
 static void qed_ll2_prepare_tx_packet_set(struct qed_hwfn *p_hwfn,
 					  struct qed_ll2_tx_queue *p_tx,
 					  struct qed_ll2_tx_packet *p_curp,
-					  u8 num_of_bds,
-					  dma_addr_t first_frag,
-					  u16 first_frag_len, void *p_cookie,
+					  struct qed_ll2_tx_pkt_info *pkt,
 					  u8 notify_fw)
 {
 	list_del(&p_curp->list_entry);
-	p_curp->cookie = p_cookie;
-	p_curp->bd_used = num_of_bds;
+	p_curp->cookie = pkt->cookie;
+	p_curp->bd_used = pkt->num_of_bds;
 	p_curp->notify_fw = notify_fw;
 	p_tx->cur_send_packet = p_curp;
 	p_tx->cur_send_frag_num = 0;
 
-	p_curp->bds_set[p_tx->cur_send_frag_num].tx_frag = first_frag;
-	p_curp->bds_set[p_tx->cur_send_frag_num].frag_len = first_frag_len;
+	p_curp->bds_set[p_tx->cur_send_frag_num].tx_frag = pkt->first_frag;
+	p_curp->bds_set[p_tx->cur_send_frag_num].frag_len = pkt->first_frag_len;
 	p_tx->cur_send_frag_num++;
 }
 
@@ -1615,51 +1566,52 @@ static void
 qed_ll2_prepare_tx_packet_set_bd(struct qed_hwfn *p_hwfn,
 				 struct qed_ll2_info *p_ll2,
 				 struct qed_ll2_tx_packet *p_curp,
-				 u8 num_of_bds,
-				 enum core_tx_dest tx_dest,
-				 u16 vlan,
-				 u8 bd_flags,
-				 u16 l4_hdr_offset_w,
-				 enum core_roce_flavor_type roce_flavor,
-				 dma_addr_t first_frag,
-				 u16 first_frag_len)
+				 struct qed_ll2_tx_pkt_info *pkt)
 {
 	struct qed_chain *p_tx_chain = &p_ll2->tx_queue.txq_chain;
 	u16 prod_idx = qed_chain_get_prod_idx(p_tx_chain);
 	struct core_tx_bd *start_bd = NULL;
+	enum core_roce_flavor_type roce_flavor;
+	enum core_tx_dest tx_dest;
 	u16 bd_data = 0, frag_idx;
 
+	roce_flavor = (pkt->qed_roce_flavor == QED_LL2_ROCE) ? CORE_ROCE
+							     : CORE_RROCE;
+
+	tx_dest = (pkt->tx_dest == QED_LL2_TX_DEST_NW) ? CORE_TX_DEST_NW
+						       : CORE_TX_DEST_LB;
+
 	start_bd = (struct core_tx_bd *)qed_chain_produce(p_tx_chain);
-	start_bd->nw_vlan_or_lb_echo = cpu_to_le16(vlan);
+	start_bd->nw_vlan_or_lb_echo = cpu_to_le16(pkt->vlan);
 	SET_FIELD(start_bd->bitfield1, CORE_TX_BD_L4_HDR_OFFSET_W,
-		  cpu_to_le16(l4_hdr_offset_w));
+		  cpu_to_le16(pkt->l4_hdr_offset_w));
 	SET_FIELD(start_bd->bitfield1, CORE_TX_BD_TX_DST, tx_dest);
-	bd_data |= bd_flags;
+	bd_data |= pkt->bd_flags;
 	SET_FIELD(bd_data, CORE_TX_BD_DATA_START_BD, 0x1);
-	SET_FIELD(bd_data, CORE_TX_BD_DATA_NBDS, num_of_bds);
+	SET_FIELD(bd_data, CORE_TX_BD_DATA_NBDS, pkt->num_of_bds);
 	SET_FIELD(bd_data, CORE_TX_BD_DATA_ROCE_FLAV, roce_flavor);
 	start_bd->bd_data.as_bitfield = cpu_to_le16(bd_data);
-	DMA_REGPAIR_LE(start_bd->addr, first_frag);
-	start_bd->nbytes = cpu_to_le16(first_frag_len);
+	DMA_REGPAIR_LE(start_bd->addr, pkt->first_frag);
+	start_bd->nbytes = cpu_to_le16(pkt->first_frag_len);
 
 	DP_VERBOSE(p_hwfn,
 		   (NETIF_MSG_TX_QUEUED | QED_MSG_LL2),
 		   "LL2 [q 0x%02x cid 0x%08x type 0x%08x] Tx Producer at [0x%04x] - set with a %04x bytes %02x BDs buffer at %08x:%08x\n",
 		   p_ll2->queue_id,
 		   p_ll2->cid,
-		   p_ll2->conn.conn_type,
+		   p_ll2->input.conn_type,
 		   prod_idx,
-		   first_frag_len,
-		   num_of_bds,
+		   pkt->first_frag_len,
+		   pkt->num_of_bds,
 		   le32_to_cpu(start_bd->addr.hi),
 		   le32_to_cpu(start_bd->addr.lo));
 
-	if (p_ll2->tx_queue.cur_send_frag_num == num_of_bds)
+	if (p_ll2->tx_queue.cur_send_frag_num == pkt->num_of_bds)
 		return;
 
 	/* Need to provide the packet with additional BDs for frags */
 	for (frag_idx = p_ll2->tx_queue.cur_send_frag_num;
-	     frag_idx < num_of_bds; frag_idx++) {
+	     frag_idx < pkt->num_of_bds; frag_idx++) {
 		struct core_tx_bd **p_bd = &p_curp->bds_set[frag_idx].txq_bd;
 
 		*p_bd = (struct core_tx_bd *)qed_chain_produce(p_tx_chain);
@@ -1722,26 +1674,20 @@ static void qed_ll2_tx_packet_notify(struct qed_hwfn *p_hwfn,
 		   (NETIF_MSG_TX_QUEUED | QED_MSG_LL2),
 		   "LL2 [q 0x%02x cid 0x%08x type 0x%08x] Doorbelled [producer 0x%04x]\n",
 		   p_ll2_conn->queue_id,
-		   p_ll2_conn->cid, p_ll2_conn->conn.conn_type, db_msg.spq_prod);
+		   p_ll2_conn->cid,
+		   p_ll2_conn->input.conn_type, db_msg.spq_prod);
 }
 
-int qed_ll2_prepare_tx_packet(struct qed_hwfn *p_hwfn,
+int qed_ll2_prepare_tx_packet(void *cxt,
 			      u8 connection_handle,
-			      u8 num_of_bds,
-			      u16 vlan,
-			      u8 bd_flags,
-			      u16 l4_hdr_offset_w,
-			      enum qed_ll2_tx_dest e_tx_dest,
-			      enum qed_ll2_roce_flavor_type qed_roce_flavor,
-			      dma_addr_t first_frag,
-			      u16 first_frag_len, void *cookie, u8 notify_fw)
+			      struct qed_ll2_tx_pkt_info *pkt,
+			      bool notify_fw)
 {
+	struct qed_hwfn *p_hwfn = cxt;
 	struct qed_ll2_tx_packet *p_curp = NULL;
 	struct qed_ll2_info *p_ll2_conn = NULL;
-	enum core_roce_flavor_type roce_flavor;
 	struct qed_ll2_tx_queue *p_tx;
 	struct qed_chain *p_tx_chain;
-	enum core_tx_dest tx_dest;
 	unsigned long flags;
 	int rc = 0;
 
@@ -1751,7 +1697,7 @@ int qed_ll2_prepare_tx_packet(struct qed_hwfn *p_hwfn,
 	p_tx = &p_ll2_conn->tx_queue;
 	p_tx_chain = &p_tx->txq_chain;
 
-	if (num_of_bds > CORE_LL2_TX_MAX_BDS_PER_PACKET)
+	if (pkt->num_of_bds > CORE_LL2_TX_MAX_BDS_PER_PACKET)
 		return -EIO;
 
 	spin_lock_irqsave(&p_tx->lock, flags);
@@ -1764,7 +1710,7 @@ int qed_ll2_prepare_tx_packet(struct qed_hwfn *p_hwfn,
 	if (!list_empty(&p_tx->free_descq))
 		p_curp = list_first_entry(&p_tx->free_descq,
 					  struct qed_ll2_tx_packet, list_entry);
-	if (p_curp && qed_chain_get_elem_left(p_tx_chain) < num_of_bds)
+	if (p_curp && qed_chain_get_elem_left(p_tx_chain) < pkt->num_of_bds)
 		p_curp = NULL;
 
 	if (!p_curp) {
@@ -1772,26 +1718,10 @@ int qed_ll2_prepare_tx_packet(struct qed_hwfn *p_hwfn,
 		goto out;
 	}
 
-	tx_dest = e_tx_dest == QED_LL2_TX_DEST_NW ? CORE_TX_DEST_NW :
-						    CORE_TX_DEST_LB;
-	if (qed_roce_flavor == QED_LL2_ROCE) {
-		roce_flavor = CORE_ROCE;
-	} else if (qed_roce_flavor == QED_LL2_RROCE) {
-		roce_flavor = CORE_RROCE;
-	} else {
-		rc = -EINVAL;
-		goto out;
-	}
-
 	/* Prepare packet and BD, and perhaps send a doorbell to FW */
-	qed_ll2_prepare_tx_packet_set(p_hwfn, p_tx, p_curp,
-				      num_of_bds, first_frag,
-				      first_frag_len, cookie, notify_fw);
-	qed_ll2_prepare_tx_packet_set_bd(p_hwfn, p_ll2_conn, p_curp,
-					 num_of_bds, tx_dest,
-					 vlan, bd_flags, l4_hdr_offset_w,
-					 roce_flavor,
-					 first_frag, first_frag_len);
+	qed_ll2_prepare_tx_packet_set(p_hwfn, p_tx, p_curp, pkt, notify_fw);
+
+	qed_ll2_prepare_tx_packet_set_bd(p_hwfn, p_ll2_conn, p_curp, pkt);
 
 	qed_ll2_tx_packet_notify(p_hwfn, p_ll2_conn);
 
@@ -1800,11 +1730,12 @@ out:
 	return rc;
 }
 
-int qed_ll2_set_fragment_of_tx_packet(struct qed_hwfn *p_hwfn,
+int qed_ll2_set_fragment_of_tx_packet(void *cxt,
 				      u8 connection_handle,
 				      dma_addr_t addr, u16 nbytes)
 {
 	struct qed_ll2_tx_packet *p_cur_send_packet = NULL;
+	struct qed_hwfn *p_hwfn = cxt;
 	struct qed_ll2_info *p_ll2_conn = NULL;
 	u16 cur_send_frag_num = 0;
 	struct core_tx_bd *p_bd;
@@ -1839,8 +1770,9 @@ int qed_ll2_set_fragment_of_tx_packet(struct qed_hwfn *p_hwfn,
 	return 0;
 }
 
-int qed_ll2_terminate_connection(struct qed_hwfn *p_hwfn, u8 connection_handle)
+int qed_ll2_terminate_connection(void *cxt, u8 connection_handle)
 {
+	struct qed_hwfn *p_hwfn = cxt;
 	struct qed_ll2_info *p_ll2_conn = NULL;
 	int rc = -EINVAL;
 	struct qed_ptt *p_ptt;
@@ -1870,10 +1802,10 @@ int qed_ll2_terminate_connection(struct qed_hwfn *p_hwfn, u8 connection_handle)
 		qed_ll2_rxq_flush(p_hwfn, connection_handle);
 	}
 
-	if (p_ll2_conn->conn.conn_type == QED_LL2_TYPE_ISCSI_OOO)
+	if (p_ll2_conn->input.conn_type == QED_LL2_TYPE_OOO)
 		qed_ooo_release_all_isles(p_hwfn, p_hwfn->p_ooo_info);
 
-	if (p_ll2_conn->conn.conn_type == QED_LL2_TYPE_FCOE) {
+	if (p_ll2_conn->input.conn_type == QED_LL2_TYPE_FCOE) {
 		qed_llh_remove_protocol_filter(p_hwfn, p_ptt,
 					       0x8906, 0,
 					       QED_LLH_FILTER_ETHERTYPE);
@@ -1887,8 +1819,28 @@ out:
 	return rc;
 }
 
-void qed_ll2_release_connection(struct qed_hwfn *p_hwfn, u8 connection_handle)
+static void qed_ll2_release_connection_ooo(struct qed_hwfn *p_hwfn,
+					   struct qed_ll2_info *p_ll2_conn)
 {
+	struct qed_ooo_buffer *p_buffer;
+
+	if (p_ll2_conn->input.conn_type != QED_LL2_TYPE_OOO)
+		return;
+
+	qed_ooo_release_all_isles(p_hwfn, p_hwfn->p_ooo_info);
+	while ((p_buffer = qed_ooo_get_free_buffer(p_hwfn,
+						   p_hwfn->p_ooo_info))) {
+		dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+				  p_buffer->rx_buffer_size,
+				  p_buffer->rx_buffer_virt_addr,
+				  p_buffer->rx_buffer_phys_addr);
+		kfree(p_buffer);
+	}
+}
+
+void qed_ll2_release_connection(void *cxt, u8 connection_handle)
+{
+	struct qed_hwfn *p_hwfn = cxt;
 	struct qed_ll2_info *p_ll2_conn = NULL;
 
 	p_ll2_conn = qed_ll2_handle_sanity(p_hwfn, connection_handle);
@@ -1921,7 +1873,7 @@ void qed_ll2_release_connection(struct qed_hwfn *p_hwfn, u8 connection_handle)
 	mutex_unlock(&p_ll2_conn->mutex);
 }
 
-struct qed_ll2_info *qed_ll2_alloc(struct qed_hwfn *p_hwfn)
+int qed_ll2_alloc(struct qed_hwfn *p_hwfn)
 {
 	struct qed_ll2_info *p_ll2_connections;
 	u8 i;
@@ -1931,28 +1883,52 @@ struct qed_ll2_info *qed_ll2_alloc(struct qed_hwfn *p_hwfn)
 				    sizeof(struct qed_ll2_info), GFP_KERNEL);
 	if (!p_ll2_connections) {
 		DP_NOTICE(p_hwfn, "Failed to allocate `struct qed_ll2'\n");
-		return NULL;
+		return -ENOMEM;
 	}
 
 	for (i = 0; i < QED_MAX_NUM_OF_LL2_CONNECTIONS; i++)
 		p_ll2_connections[i].my_id = i;
 
-	return p_ll2_connections;
+	p_hwfn->p_ll2_info = p_ll2_connections;
+	return 0;
 }
 
-void qed_ll2_setup(struct qed_hwfn *p_hwfn,
-		   struct qed_ll2_info *p_ll2_connections)
+void qed_ll2_setup(struct qed_hwfn *p_hwfn)
 {
 	int i;
 
 	for (i = 0; i < QED_MAX_NUM_OF_LL2_CONNECTIONS; i++)
-		mutex_init(&p_ll2_connections[i].mutex);
+		mutex_init(&p_hwfn->p_ll2_info[i].mutex);
 }
 
-void qed_ll2_free(struct qed_hwfn *p_hwfn,
-		  struct qed_ll2_info *p_ll2_connections)
+void qed_ll2_free(struct qed_hwfn *p_hwfn)
 {
-	kfree(p_ll2_connections);
+	if (!p_hwfn->p_ll2_info)
+		return;
+
+	kfree(p_hwfn->p_ll2_info);
+	p_hwfn->p_ll2_info = NULL;
+}
+
+static void _qed_ll2_get_port_stats(struct qed_hwfn *p_hwfn,
+				    struct qed_ptt *p_ptt,
+				    struct qed_ll2_stats *p_stats)
+{
+	struct core_ll2_port_stats port_stats;
+
+	memset(&port_stats, 0, sizeof(port_stats));
+	qed_memcpy_from(p_hwfn, p_ptt, &port_stats,
+			BAR0_MAP_REG_TSDM_RAM +
+			TSTORM_LL2_PORT_STAT_OFFSET(MFW_PORT(p_hwfn)),
+			sizeof(port_stats));
+
+	p_stats->gsi_invalid_hdr = HILO_64_REGPAIR(port_stats.gsi_invalid_hdr);
+	p_stats->gsi_invalid_pkt_length =
+	    HILO_64_REGPAIR(port_stats.gsi_invalid_pkt_length);
+	p_stats->gsi_unsupported_pkt_typ =
+	    HILO_64_REGPAIR(port_stats.gsi_unsupported_pkt_typ);
+	p_stats->gsi_crcchksm_error =
+	    HILO_64_REGPAIR(port_stats.gsi_crcchksm_error);
 }
 
 static void _qed_ll2_get_tstats(struct qed_hwfn *p_hwfn,
@@ -2018,9 +1994,10 @@ static void _qed_ll2_get_pstats(struct qed_hwfn *p_hwfn,
 	p_stats->sent_bcast_pkts = HILO_64_REGPAIR(pstats.sent_bcast_pkts);
 }
 
-int qed_ll2_get_stats(struct qed_hwfn *p_hwfn,
+int qed_ll2_get_stats(void *cxt,
 		      u8 connection_handle, struct qed_ll2_stats *p_stats)
 {
+	struct qed_hwfn *p_hwfn = cxt;
 	struct qed_ll2_info *p_ll2_conn = NULL;
 	struct qed_ptt *p_ptt;
 
@@ -2038,6 +2015,8 @@ int qed_ll2_get_stats(struct qed_hwfn *p_hwfn,
 		return -EINVAL;
 	}
 
+	if (p_ll2_conn->input.gsi_enable)
+		_qed_ll2_get_port_stats(p_hwfn, p_ptt, p_stats);
 	_qed_ll2_get_tstats(p_hwfn, p_ptt, p_ll2_conn, p_stats);
 	_qed_ll2_get_ustats(p_hwfn, p_ptt, p_ll2_conn, p_stats);
 	if (p_ll2_conn->tx_stats_en)
@@ -2047,6 +2026,17 @@ int qed_ll2_get_stats(struct qed_hwfn *p_hwfn,
 	return 0;
 }
 
+static void qed_ll2b_release_rx_packet(void *cxt,
+				       u8 connection_handle,
+				       void *cookie,
+				       dma_addr_t rx_buf_addr,
+				       bool b_last_packet)
+{
+	struct qed_hwfn *p_hwfn = cxt;
+
+	qed_ll2_dealloc_buffer(p_hwfn->cdev, cookie);
+}
+
 static void qed_ll2_register_cb_ops(struct qed_dev *cdev,
 				    const struct qed_ll2_cb_ops *ops,
 				    void *cookie)
@@ -2055,21 +2045,86 @@ static void qed_ll2_register_cb_ops(struct qed_dev *cdev,
 	cdev->ll2->cb_cookie = cookie;
 }
 
+struct qed_ll2_cbs ll2_cbs = {
+	.rx_comp_cb = &qed_ll2b_complete_rx_packet,
+	.rx_release_cb = &qed_ll2b_release_rx_packet,
+	.tx_comp_cb = &qed_ll2b_complete_tx_packet,
+	.tx_release_cb = &qed_ll2b_complete_tx_packet,
+};
+
+static void qed_ll2_set_conn_data(struct qed_dev *cdev,
+				  struct qed_ll2_acquire_data *data,
+				  struct qed_ll2_params *params,
+				  enum qed_ll2_conn_type conn_type,
+				  u8 *handle, bool lb)
+{
+	memset(data, 0, sizeof(*data));
+
+	data->input.conn_type = conn_type;
+	data->input.mtu = params->mtu;
+	data->input.rx_num_desc = QED_LL2_RX_SIZE;
+	data->input.rx_drop_ttl0_flg = params->drop_ttl0_packets;
+	data->input.rx_vlan_removal_en = params->rx_vlan_stripping;
+	data->input.tx_num_desc = QED_LL2_TX_SIZE;
+	data->p_connection_handle = handle;
+	data->cbs = &ll2_cbs;
+	ll2_cbs.cookie = QED_LEADING_HWFN(cdev);
+
+	if (lb) {
+		data->input.tx_tc = PKT_LB_TC;
+		data->input.tx_dest = QED_LL2_TX_DEST_LB;
+	} else {
+		data->input.tx_tc = 0;
+		data->input.tx_dest = QED_LL2_TX_DEST_NW;
+	}
+}
+
+static int qed_ll2_start_ooo(struct qed_dev *cdev,
+			     struct qed_ll2_params *params)
+{
+	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
+	u8 *handle = &hwfn->pf_params.iscsi_pf_params.ll2_ooo_queue_id;
+	struct qed_ll2_acquire_data data;
+	int rc;
+
+	qed_ll2_set_conn_data(cdev, &data, params,
+			      QED_LL2_TYPE_OOO, handle, true);
+
+	rc = qed_ll2_acquire_connection(hwfn, &data);
+	if (rc) {
+		DP_INFO(cdev, "Failed to acquire LL2 OOO connection\n");
+		goto out;
+	}
+
+	rc = qed_ll2_establish_connection(hwfn, *handle);
+	if (rc) {
+		DP_INFO(cdev, "Failed to establist LL2 OOO connection\n");
+		goto fail;
+	}
+
+	return 0;
+
+fail:
+	qed_ll2_release_connection(hwfn, *handle);
+out:
+	*handle = QED_LL2_UNUSED_HANDLE;
+	return rc;
+}
+
 static int qed_ll2_start(struct qed_dev *cdev, struct qed_ll2_params *params)
 {
-	struct qed_ll2_conn ll2_info;
 	struct qed_ll2_buffer *buffer, *tmp_buffer;
 	enum qed_ll2_conn_type conn_type;
+	struct qed_ll2_acquire_data data;
 	struct qed_ptt *p_ptt;
 	int rc, i;
-	u8 gsi_enable = 1;
+
 
 	/* Initialize LL2 locks & lists */
 	INIT_LIST_HEAD(&cdev->ll2->list);
 	spin_lock_init(&cdev->ll2->lock);
 	cdev->ll2->rx_size = NET_SKB_PAD + ETH_HLEN +
 			     L1_CACHE_BYTES + params->mtu;
-	cdev->ll2->frags_mapped = params->frags_mapped;
 
 	/*Allocate memory for LL2 */
 	DP_INFO(cdev, "Allocating LL2 buffers of size %08x bytes\n",
@@ -2094,11 +2149,9 @@ static int qed_ll2_start(struct qed_dev *cdev, struct qed_ll2_params *params)
 	switch (QED_LEADING_HWFN(cdev)->hw_info.personality) {
 	case QED_PCI_FCOE:
 		conn_type = QED_LL2_TYPE_FCOE;
-		gsi_enable = 0;
 		break;
 	case QED_PCI_ISCSI:
 		conn_type = QED_LL2_TYPE_ISCSI;
-		gsi_enable = 0;
 		break;
 	case QED_PCI_ETH_ROCE:
 		conn_type = QED_LL2_TYPE_ROCE;
@@ -2107,20 +2160,10 @@ static int qed_ll2_start(struct qed_dev *cdev, struct qed_ll2_params *params)
 		conn_type = QED_LL2_TYPE_TEST;
 	}
 
-	/* Prepare the temporary ll2 information */
-	memset(&ll2_info, 0, sizeof(ll2_info));
+	qed_ll2_set_conn_data(cdev, &data, params, conn_type,
+			      &cdev->ll2->handle, false);
 
-	ll2_info.conn_type = conn_type;
-	ll2_info.mtu = params->mtu;
-	ll2_info.rx_drop_ttl0_flg = params->drop_ttl0_packets;
-	ll2_info.rx_vlan_removal_en = params->rx_vlan_stripping;
-	ll2_info.tx_tc = 0;
-	ll2_info.tx_dest = CORE_TX_DEST_NW;
-	ll2_info.gsi_enable = gsi_enable;
-
-	rc = qed_ll2_acquire_connection(QED_LEADING_HWFN(cdev), &ll2_info,
-					QED_LL2_RX_SIZE, QED_LL2_TX_SIZE,
-					&cdev->ll2->handle);
+	rc = qed_ll2_acquire_connection(QED_LEADING_HWFN(cdev), &data);
 	if (rc) {
 		DP_INFO(cdev, "Failed to acquire LL2 connection\n");
 		goto fail;
@@ -2243,6 +2286,7 @@ fail:
 
 static int qed_ll2_start_xmit(struct qed_dev *cdev, struct sk_buff *skb)
 {
+	struct qed_ll2_tx_pkt_info pkt;
 	const skb_frag_t *frag;
 	int rc = -EINVAL, i;
 	dma_addr_t mapping;
@@ -2277,32 +2321,30 @@ static int qed_ll2_start_xmit(struct qed_dev *cdev, struct sk_buff *skb)
 		flags |= BIT(CORE_TX_BD_DATA_VLAN_INSERTION_SHIFT);
 	}
 
-	rc = qed_ll2_prepare_tx_packet(QED_LEADING_HWFN(cdev),
-				       cdev->ll2->handle,
-				       1 + skb_shinfo(skb)->nr_frags,
-				       vlan, flags, 0, QED_LL2_TX_DEST_NW,
-				       0 /* RoCE FLAVOR */,
-				       mapping, skb->len, skb, 1);
+	memset(&pkt, 0, sizeof(pkt));
+	pkt.num_of_bds = 1 + skb_shinfo(skb)->nr_frags;
+	pkt.vlan = vlan;
+	pkt.bd_flags = flags;
+	pkt.tx_dest = QED_LL2_TX_DEST_NW;
+	pkt.first_frag = mapping;
+	pkt.first_frag_len = skb->len;
+	pkt.cookie = skb;
+
+	rc = qed_ll2_prepare_tx_packet(&cdev->hwfns[0], cdev->ll2->handle,
+				       &pkt, 1);
 	if (rc)
 		goto err;
 
 	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
 		frag = &skb_shinfo(skb)->frags[i];
-		if (!cdev->ll2->frags_mapped) {
-			mapping = skb_frag_dma_map(&cdev->pdev->dev, frag, 0,
-						   skb_frag_size(frag),
-						   DMA_TO_DEVICE);
-
-			if (unlikely(dma_mapping_error(&cdev->pdev->dev,
-						       mapping))) {
-				DP_NOTICE(cdev,
-					  "Unable to map frag - dropping packet\n");
-				rc = -ENOMEM;
-				goto err;
-			}
-		} else {
-			mapping = page_to_phys(skb_frag_page(frag)) |
-			    frag->page_offset;
+
+		mapping = skb_frag_dma_map(&cdev->pdev->dev, frag, 0,
+					   skb_frag_size(frag), DMA_TO_DEVICE);
+
+		if (unlikely(dma_mapping_error(&cdev->pdev->dev, mapping))) {
+			DP_NOTICE(cdev,
+				  "Unable to map frag - dropping packet\n");
+			goto err;
 		}
 
 		rc = qed_ll2_set_fragment_of_tx_packet(QED_LEADING_HWFN(cdev),
diff --git a/drivers/net/ethernet/qlogic/qed/qed_ll2.h b/drivers/net/ethernet/qlogic/qed/qed_ll2.h
index 31a409033c41..a822528e9c63 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_ll2.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_ll2.h
@@ -47,29 +47,6 @@
 
 #define QED_MAX_NUM_OF_LL2_CONNECTIONS                    (4)
 
-enum qed_ll2_roce_flavor_type {
-	QED_LL2_ROCE,
-	QED_LL2_RROCE,
-	MAX_QED_LL2_ROCE_FLAVOR_TYPE
-};
-
-enum qed_ll2_conn_type {
-	QED_LL2_TYPE_FCOE,
-	QED_LL2_TYPE_ISCSI,
-	QED_LL2_TYPE_TEST,
-	QED_LL2_TYPE_ISCSI_OOO,
-	QED_LL2_TYPE_RESERVED2,
-	QED_LL2_TYPE_ROCE,
-	QED_LL2_TYPE_RESERVED3,
-	MAX_QED_LL2_RX_CONN_TYPE
-};
-
-enum qed_ll2_tx_dest {
-	QED_LL2_TX_DEST_NW, /* Light L2 TX Destination to the Network */
-	QED_LL2_TX_DEST_LB, /* Light L2 TX Destination to the Loopback */
-	QED_LL2_TX_DEST_MAX
-};
-
 struct qed_ll2_rx_packet {
 	struct list_head list_entry;
 	struct core_rx_bd_with_buff_len *rxq_bd;
@@ -135,30 +112,21 @@ struct qed_ll2_tx_queue {
 	bool b_completing_packet;
 };
 
-struct qed_ll2_conn {
-	enum qed_ll2_conn_type conn_type;
-	u16 mtu;
-	u8 rx_drop_ttl0_flg;
-	u8 rx_vlan_removal_en;
-	u8 tx_tc;
-	enum core_tx_dest tx_dest;
-	enum core_error_handle ai_err_packet_too_big;
-	enum core_error_handle ai_err_no_buf;
-	u8 gsi_enable;
-};
-
 struct qed_ll2_info {
 	/* Lock protecting the state of LL2 */
 	struct mutex mutex;
-	struct qed_ll2_conn conn;
+
+	struct qed_ll2_acquire_data_inputs input;
 	u32 cid;
 	u8 my_id;
 	u8 queue_id;
 	u8 tx_stats_id;
 	bool b_active;
+	enum core_tx_dest tx_dest;
 	u8 tx_stats_en;
 	struct qed_ll2_rx_queue rx_queue;
 	struct qed_ll2_tx_queue tx_queue;
+	struct qed_ll2_cbs cbs;
 };
 
 /**
@@ -166,38 +134,30 @@ struct qed_ll2_info {
  *        starts rx & tx (if relevant) queues pair. Provides
  *        connecion handler as output parameter.
  *
- * @param p_hwfn
- * @param p_params		Contain various configuration properties
- * @param rx_num_desc
- * @param tx_num_desc
- *
- * @param p_connection_handle  Output container for LL2 connection's handle
  *
- * @return 0 on success, failure otherwise
+ * @param cxt - pointer to the hw-function [opaque to some]
+ * @param data - describes connection parameters
+ * @return int
  */
-int qed_ll2_acquire_connection(struct qed_hwfn *p_hwfn,
-			       struct qed_ll2_conn *p_params,
-			       u16 rx_num_desc,
-			       u16 tx_num_desc,
-			       u8 *p_connection_handle);
+int qed_ll2_acquire_connection(void *cxt, struct qed_ll2_acquire_data *data);
 
 /**
  * @brief qed_ll2_establish_connection - start previously
  *        allocated LL2 queues pair
  *
- * @param p_hwfn
+ * @param cxt - pointer to the hw-function [opaque to some]
  * @param p_ptt
  * @param connection_handle	LL2 connection's handle obtained from
  *                              qed_ll2_require_connection
  *
  * @return 0 on success, failure otherwise
  */
-int qed_ll2_establish_connection(struct qed_hwfn *p_hwfn, u8 connection_handle);
+int qed_ll2_establish_connection(void *cxt, u8 connection_handle);
 
 /**
  * @brief qed_ll2_post_rx_buffers - submit buffers to LL2 Rx queue.
  *
- * @param p_hwfn
+ * @param cxt - pointer to the hw-function [opaque to some]
  * @param connection_handle	LL2 connection's handle obtained from
  *				qed_ll2_require_connection
  * @param addr			rx (physical address) buffers to submit
@@ -206,7 +166,7 @@ int qed_ll2_establish_connection(struct qed_hwfn *p_hwfn, u8 connection_handle);
  *
  * @return 0 on success, failure otherwise
  */
-int qed_ll2_post_rx_buffer(struct qed_hwfn *p_hwfn,
+int qed_ll2_post_rx_buffer(void *cxt,
 			   u8 connection_handle,
 			   dma_addr_t addr,
 			   u16 buf_len, void *cookie, u8 notify_fw);
@@ -215,53 +175,34 @@ int qed_ll2_post_rx_buffer(struct qed_hwfn *p_hwfn,
  * @brief qed_ll2_prepare_tx_packet - request for start Tx BD
  *				      to prepare Tx packet submission to FW.
  *
- * @param p_hwfn
- * @param connection_handle	LL2 connection's handle obtained from
- *				qed_ll2_require_connection
- * @param num_of_bds		a number of requested BD equals a number of
- *				fragments in Tx packet
- * @param vlan			VLAN to insert to packet (if insertion set)
- * @param bd_flags
- * @param l4_hdr_offset_w	L4 Header Offset from start of packet
- *				(in words). This is needed if both l4_csum
- *				and ipv6_ext are set
- * @param e_tx_dest             indicates if the packet is to be transmitted via
- *                              loopback or to the network
- * @param first_frag
- * @param first_frag_len
- * @param cookie
- *
- * @param notify_fw
+ * @param cxt - pointer to the hw-function [opaque to some]
+ * @param connection_handle
+ * @param pkt - info regarding the tx packet
+ * @param notify_fw - issue doorbell to fw for this packet
  *
  * @return 0 on success, failure otherwise
  */
-int qed_ll2_prepare_tx_packet(struct qed_hwfn *p_hwfn,
+int qed_ll2_prepare_tx_packet(void *cxt,
 			      u8 connection_handle,
-			      u8 num_of_bds,
-			      u16 vlan,
-			      u8 bd_flags,
-			      u16 l4_hdr_offset_w,
-			      enum qed_ll2_tx_dest e_tx_dest,
-			      enum qed_ll2_roce_flavor_type qed_roce_flavor,
-			      dma_addr_t first_frag,
-			      u16 first_frag_len, void *cookie, u8 notify_fw);
+			      struct qed_ll2_tx_pkt_info *pkt,
+			      bool notify_fw);
 
 /**
  * @brief qed_ll2_release_connection -	releases resources
  *					allocated for LL2 connection
  *
- * @param p_hwfn
+ * @param cxt - pointer to the hw-function [opaque to some]
  * @param connection_handle		LL2 connection's handle obtained from
  *					qed_ll2_require_connection
  */
-void qed_ll2_release_connection(struct qed_hwfn *p_hwfn, u8 connection_handle);
+void qed_ll2_release_connection(void *cxt, u8 connection_handle);
 
 /**
  * @brief qed_ll2_set_fragment_of_tx_packet -	provides fragments to fill
  *						Tx BD of BDs requested by
  *						qed_ll2_prepare_tx_packet
  *
- * @param p_hwfn
+ * @param cxt - pointer to the hw-function [opaque to some]
  * @param connection_handle			LL2 connection's handle
  *						obtained from
  *						qed_ll2_require_connection
@@ -270,7 +211,7 @@ void qed_ll2_release_connection(struct qed_hwfn *p_hwfn, u8 connection_handle);
  *
  * @return 0 on success, failure otherwise
  */
-int qed_ll2_set_fragment_of_tx_packet(struct qed_hwfn *p_hwfn,
+int qed_ll2_set_fragment_of_tx_packet(void *cxt,
 				      u8 connection_handle,
 				      dma_addr_t addr, u16 nbytes);
 
@@ -278,27 +219,27 @@ int qed_ll2_set_fragment_of_tx_packet(struct qed_hwfn *p_hwfn,
  * @brief qed_ll2_terminate_connection -	stops Tx/Rx queues
  *
  *
- * @param p_hwfn
+ * @param cxt - pointer to the hw-function [opaque to some]
  * @param connection_handle			LL2 connection's handle
  *						obtained from
  *						qed_ll2_require_connection
  *
  * @return 0 on success, failure otherwise
  */
-int qed_ll2_terminate_connection(struct qed_hwfn *p_hwfn, u8 connection_handle);
+int qed_ll2_terminate_connection(void *cxt, u8 connection_handle);
 
 /**
  * @brief qed_ll2_get_stats -	get LL2 queue's statistics
  *
  *
- * @param p_hwfn
+ * @param cxt - pointer to the hw-function [opaque to some]
  * @param connection_handle	LL2 connection's handle obtained from
  *				qed_ll2_require_connection
  * @param p_stats
  *
  * @return 0 on success, failure otherwise
  */
-int qed_ll2_get_stats(struct qed_hwfn *p_hwfn,
+int qed_ll2_get_stats(void *cxt,
 		      u8 connection_handle, struct qed_ll2_stats *p_stats);
 
 /**
@@ -306,27 +247,24 @@ int qed_ll2_get_stats(struct qed_hwfn *p_hwfn,
  *
  * @param p_hwfn
  *
- * @return pointer to alocated qed_ll2_info or NULL
+ * @return int
  */
-struct qed_ll2_info *qed_ll2_alloc(struct qed_hwfn *p_hwfn);
+int qed_ll2_alloc(struct qed_hwfn *p_hwfn);
 
 /**
  * @brief qed_ll2_setup - Inits LL2 connections set
  *
  * @param p_hwfn
- * @param p_ll2_connections
  *
  */
-void qed_ll2_setup(struct qed_hwfn *p_hwfn,
-		   struct qed_ll2_info *p_ll2_connections);
+void qed_ll2_setup(struct qed_hwfn *p_hwfn);
 
 /**
  * @brief qed_ll2_free - Releases LL2 connections set
  *
  * @param p_hwfn
- * @param p_ll2_connections
  *
  */
-void qed_ll2_free(struct qed_hwfn *p_hwfn,
-		  struct qed_ll2_info *p_ll2_connections);
+void qed_ll2_free(struct qed_hwfn *p_hwfn);
+
 #endif
diff --git a/drivers/net/ethernet/qlogic/qed/qed_main.c b/drivers/net/ethernet/qlogic/qed/qed_main.c
index 715b3aaf83ac..b11399606990 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_main.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_main.c
@@ -34,7 +34,6 @@
 #include <linux/pci.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
-#include <linux/version.h>
 #include <linux/delay.h>
 #include <asm/byteorder.h>
 #include <linux/dma-mapping.h>
@@ -123,7 +122,7 @@ static void qed_free_pci(struct qed_dev *cdev)
 {
 	struct pci_dev *pdev = cdev->pdev;
 
-	if (cdev->doorbells)
+	if (cdev->doorbells && cdev->db_size)
 		iounmap(cdev->doorbells);
 	if (cdev->regview)
 		iounmap(cdev->regview);
@@ -207,16 +206,24 @@ static int qed_init_pci(struct qed_dev *cdev, struct pci_dev *pdev)
 		goto err2;
 	}
 
-	if (IS_PF(cdev)) {
-		cdev->db_phys_addr = pci_resource_start(cdev->pdev, 2);
-		cdev->db_size = pci_resource_len(cdev->pdev, 2);
-		cdev->doorbells = ioremap_wc(cdev->db_phys_addr, cdev->db_size);
-		if (!cdev->doorbells) {
-			DP_NOTICE(cdev, "Cannot map doorbell space\n");
-			return -ENOMEM;
+	cdev->db_phys_addr = pci_resource_start(cdev->pdev, 2);
+	cdev->db_size = pci_resource_len(cdev->pdev, 2);
+	if (!cdev->db_size) {
+		if (IS_PF(cdev)) {
+			DP_NOTICE(cdev, "No Doorbell bar available\n");
+			return -EINVAL;
+		} else {
+			return 0;
 		}
 	}
 
+	cdev->doorbells = ioremap_wc(cdev->db_phys_addr, cdev->db_size);
+
+	if (!cdev->doorbells) {
+		DP_NOTICE(cdev, "Cannot map doorbell space\n");
+		return -ENOMEM;
+	}
+
 	return 0;
 
 err2:
@@ -230,6 +237,8 @@ err0:
 int qed_fill_dev_info(struct qed_dev *cdev,
 		      struct qed_dev_info *dev_info)
 {
+	struct qed_hwfn *p_hwfn = QED_LEADING_HWFN(cdev);
+	struct qed_hw_info *hw_info = &p_hwfn->hw_info;
 	struct qed_tunnel_info *tun = &cdev->tunnel;
 	struct qed_ptt  *ptt;
 
@@ -253,11 +262,10 @@ int qed_fill_dev_info(struct qed_dev *cdev,
 	dev_info->pci_mem_start = cdev->pci_params.mem_start;
 	dev_info->pci_mem_end = cdev->pci_params.mem_end;
 	dev_info->pci_irq = cdev->pci_params.irq;
-	dev_info->rdma_supported = (cdev->hwfns[0].hw_info.personality ==
-				    QED_PCI_ETH_ROCE);
+	dev_info->rdma_supported = QED_IS_RDMA_PERSONALITY(p_hwfn);
 	dev_info->is_mf_default = IS_MF_DEFAULT(&cdev->hwfns[0]);
 	dev_info->dev_type = cdev->type;
-	ether_addr_copy(dev_info->hw_mac, cdev->hwfns[0].hw_info.hw_mac_addr);
+	ether_addr_copy(dev_info->hw_mac, hw_info->hw_mac_addr);
 
 	if (IS_PF(cdev)) {
 		dev_info->fw_major = FW_MAJOR_VERSION;
@@ -267,9 +275,10 @@ int qed_fill_dev_info(struct qed_dev *cdev,
 		dev_info->mf_mode = cdev->mf_mode;
 		dev_info->tx_switching = true;
 
-		if (QED_LEADING_HWFN(cdev)->hw_info.b_wol_support ==
-		    QED_WOL_SUPPORT_PME)
+		if (hw_info->b_wol_support == QED_WOL_SUPPORT_PME)
 			dev_info->wol_support = true;
+
+		dev_info->abs_pf_id = QED_LEADING_HWFN(cdev)->abs_pf_id;
 	} else {
 		qed_vf_get_fw_version(&cdev->hwfns[0], &dev_info->fw_major,
 				      &dev_info->fw_minor, &dev_info->fw_rev,
@@ -282,6 +291,9 @@ int qed_fill_dev_info(struct qed_dev *cdev,
 			qed_mcp_get_mfw_ver(QED_LEADING_HWFN(cdev), ptt,
 					    &dev_info->mfw_rev, NULL);
 
+			qed_mcp_get_mbi_ver(QED_LEADING_HWFN(cdev), ptt,
+					    &dev_info->mbi_version);
+
 			qed_mcp_get_flash_size(QED_LEADING_HWFN(cdev), ptt,
 					       &dev_info->flash_size);
 
@@ -292,7 +304,7 @@ int qed_fill_dev_info(struct qed_dev *cdev,
 				    &dev_info->mfw_rev, NULL);
 	}
 
-	dev_info->mtu = QED_LEADING_HWFN(cdev)->hw_info.mtu;
+	dev_info->mtu = hw_info->mtu;
 
 	return 0;
 }
@@ -336,6 +348,7 @@ static struct qed_dev *qed_probe(struct pci_dev *pdev,
 	if (!cdev)
 		goto err0;
 
+	cdev->drv_type = DRV_ID_DRV_TYPE_LINUX;
 	cdev->protocol = params->protocol;
 
 	if (params->is_vf)
@@ -607,6 +620,18 @@ int qed_slowpath_irq_req(struct qed_hwfn *hwfn)
 	return rc;
 }
 
+static void qed_slowpath_tasklet_flush(struct qed_hwfn *p_hwfn)
+{
+	/* Calling the disable function will make sure that any
+	 * currently-running function is completed. The following call to the
+	 * enable function makes this sequence a flush-like operation.
+	 */
+	if (p_hwfn->b_sp_dpc_enabled) {
+		tasklet_disable(p_hwfn->sp_dpc);
+		tasklet_enable(p_hwfn->sp_dpc);
+	}
+}
+
 void qed_slowpath_irq_sync(struct qed_hwfn *p_hwfn)
 {
 	struct qed_dev *cdev = p_hwfn->cdev;
@@ -618,6 +643,8 @@ void qed_slowpath_irq_sync(struct qed_hwfn *p_hwfn)
 		synchronize_irq(cdev->int_params.msix_table[id].vector);
 	else
 		synchronize_irq(cdev->pdev->irq);
+
+	qed_slowpath_tasklet_flush(p_hwfn);
 }
 
 static void qed_slowpath_irq_free(struct qed_dev *cdev)
@@ -745,7 +772,7 @@ static int qed_slowpath_setup_int(struct qed_dev *cdev,
 	for_each_hwfn(cdev, i) {
 		memset(&sb_cnt_info, 0, sizeof(sb_cnt_info));
 		qed_int_get_num_sbs(&cdev->hwfns[i], &sb_cnt_info);
-		cdev->int_params.in.num_vectors += sb_cnt_info.sb_cnt;
+		cdev->int_params.in.num_vectors += sb_cnt_info.cnt;
 		cdev->int_params.in.num_vectors++; /* slowpath */
 	}
 
@@ -763,7 +790,7 @@ static int qed_slowpath_setup_int(struct qed_dev *cdev,
 				       cdev->num_hwfns;
 
 	if (!IS_ENABLED(CONFIG_QED_RDMA) ||
-	    QED_LEADING_HWFN(cdev)->hw_info.personality != QED_PCI_ETH_ROCE)
+	    !QED_IS_RDMA_PERSONALITY(QED_LEADING_HWFN(cdev)))
 		return 0;
 
 	for_each_hwfn(cdev, i)
@@ -904,8 +931,7 @@ static void qed_update_pf_params(struct qed_dev *cdev,
 	/* In case we might support RDMA, don't allow qede to be greedy
 	 * with the L2 contexts. Allow for 64 queues [rx, tx, xdp] per hwfn.
 	 */
-	if (QED_LEADING_HWFN(cdev)->hw_info.personality ==
-	    QED_PCI_ETH_ROCE) {
+	if (QED_IS_RDMA_PERSONALITY(QED_LEADING_HWFN(cdev))) {
 		u16 *num_cons;
 
 		num_cons = &params->eth_pf_params.num_cons;
@@ -1112,17 +1138,13 @@ static int qed_slowpath_stop(struct qed_dev *cdev)
 	return 0;
 }
 
-static void qed_set_id(struct qed_dev *cdev, char name[NAME_SIZE],
-		       char ver_str[VER_SIZE])
+static void qed_set_name(struct qed_dev *cdev, char name[NAME_SIZE])
 {
 	int i;
 
 	memcpy(cdev->name, name, NAME_SIZE);
 	for_each_hwfn(cdev, i)
 		snprintf(cdev->hwfns[i].name, NAME_SIZE, "%s-%d", name, i);
-
-	memcpy(cdev->ver_str, ver_str, VER_SIZE);
-	cdev->drv_type = DRV_ID_DRV_TYPE_LINUX;
 }
 
 static u32 qed_sb_init(struct qed_dev *cdev,
@@ -1520,6 +1542,21 @@ static int qed_drain(struct qed_dev *cdev)
 	return 0;
 }
 
+static int qed_nvm_get_image(struct qed_dev *cdev, enum qed_nvm_images type,
+			     u8 *buf, u16 len)
+{
+	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
+	struct qed_ptt *ptt = qed_ptt_acquire(hwfn);
+	int rc;
+
+	if (!ptt)
+		return -EAGAIN;
+
+	rc = qed_mcp_get_nvm_image(hwfn, ptt, type, buf, len);
+	qed_ptt_release(hwfn, ptt);
+	return rc;
+}
+
 static void qed_get_coalesce(struct qed_dev *cdev, u16 *rx_coal, u16 *tx_coal)
 {
 	*rx_coal = cdev->rx_coalesce_usecs;
@@ -1676,7 +1713,7 @@ const struct qed_common_ops qed_common_ops_pass = {
 	.probe = &qed_probe,
 	.remove = &qed_remove,
 	.set_power_state = &qed_set_power_state,
-	.set_id = &qed_set_id,
+	.set_name = &qed_set_name,
 	.update_pf_params = &qed_update_pf_params,
 	.slowpath_start = &qed_slowpath_start,
 	.slowpath_stop = &qed_slowpath_stop,
@@ -1697,6 +1734,7 @@ const struct qed_common_ops qed_common_ops_pass = {
 	.dbg_all_data_size = &qed_dbg_all_data_size,
 	.chain_alloc = &qed_chain_alloc,
 	.chain_free = &qed_chain_free,
+	.nvm_get_image = &qed_nvm_get_image,
 	.get_coalesce = &qed_get_coalesce,
 	.set_coalesce = &qed_set_coalesce,
 	.set_led = &qed_set_led,
diff --git a/drivers/net/ethernet/qlogic/qed/qed_mcp.c b/drivers/net/ethernet/qlogic/qed/qed_mcp.c
index 7266b36a2655..9da91045d167 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_mcp.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_mcp.c
@@ -177,6 +177,7 @@ int qed_mcp_free(struct qed_hwfn *p_hwfn)
 	}
 
 	kfree(p_hwfn->mcp_info);
+	p_hwfn->mcp_info = NULL;
 
 	return 0;
 }
@@ -1397,6 +1398,28 @@ static void qed_mcp_update_bw(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 		    &param);
 }
 
+static void qed_mcp_update_stag(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+{
+	struct public_func shmem_info;
+	u32 resp = 0, param = 0;
+
+	qed_mcp_get_shmem_func(p_hwfn, p_ptt, &shmem_info, MCP_PF_ID(p_hwfn));
+
+	p_hwfn->mcp_info->func_info.ovlan = (u16)shmem_info.ovlan_stag &
+						 FUNC_MF_CFG_OV_STAG_MASK;
+	p_hwfn->hw_info.ovlan = p_hwfn->mcp_info->func_info.ovlan;
+	if ((p_hwfn->hw_info.hw_mode & BIT(MODE_MF_SD)) &&
+	    (p_hwfn->hw_info.ovlan != QED_MCP_VLAN_UNSET)) {
+		qed_wr(p_hwfn, p_ptt,
+		       NIG_REG_LLH_FUNC_TAG_VALUE, p_hwfn->hw_info.ovlan);
+		qed_sp_pf_update_stag(p_hwfn);
+	}
+
+	/* Acknowledge the MFW */
+	qed_mcp_cmd(p_hwfn, p_ptt, DRV_MSG_CODE_S_TAG_UPDATE_ACK, 0,
+		    &resp, &param);
+}
+
 int qed_mcp_handle_events(struct qed_hwfn *p_hwfn,
 			  struct qed_ptt *p_ptt)
 {
@@ -1452,6 +1475,10 @@ int qed_mcp_handle_events(struct qed_hwfn *p_hwfn,
 		case MFW_DRV_MSG_BW_UPDATE:
 			qed_mcp_update_bw(p_hwfn, p_ptt);
 			break;
+		case MFW_DRV_MSG_S_TAG_UPDATE:
+			qed_mcp_update_stag(p_hwfn, p_ptt);
+			break;
+			break;
 		default:
 			DP_INFO(p_hwfn, "Unimplemented MFW message %d\n", i);
 			rc = -EINVAL;
@@ -1522,6 +1549,36 @@ int qed_mcp_get_mfw_ver(struct qed_hwfn *p_hwfn,
 	return 0;
 }
 
+int qed_mcp_get_mbi_ver(struct qed_hwfn *p_hwfn,
+			struct qed_ptt *p_ptt, u32 *p_mbi_ver)
+{
+	u32 nvm_cfg_addr, nvm_cfg1_offset, mbi_ver_addr;
+
+	if (IS_VF(p_hwfn->cdev))
+		return -EINVAL;
+
+	/* Read the address of the nvm_cfg */
+	nvm_cfg_addr = qed_rd(p_hwfn, p_ptt, MISC_REG_GEN_PURP_CR0);
+	if (!nvm_cfg_addr) {
+		DP_NOTICE(p_hwfn, "Shared memory not initialized\n");
+		return -EINVAL;
+	}
+
+	/* Read the offset of nvm_cfg1 */
+	nvm_cfg1_offset = qed_rd(p_hwfn, p_ptt, nvm_cfg_addr + 4);
+
+	mbi_ver_addr = MCP_REG_SCRATCH + nvm_cfg1_offset +
+		       offsetof(struct nvm_cfg1, glob) +
+		       offsetof(struct nvm_cfg1_glob, mbi_version);
+	*p_mbi_ver = qed_rd(p_hwfn, p_ptt,
+			    mbi_ver_addr) &
+		     (NVM_CFG1_GLOB_MBI_VERSION_0_MASK |
+		      NVM_CFG1_GLOB_MBI_VERSION_1_MASK |
+		      NVM_CFG1_GLOB_MBI_VERSION_2_MASK);
+
+	return 0;
+}
+
 int qed_mcp_get_media_type(struct qed_dev *cdev, u32 *p_media_type)
 {
 	struct qed_hwfn *p_hwfn = &cdev->hwfns[0];
@@ -1679,10 +1736,10 @@ int qed_mcp_fill_shmem_func_info(struct qed_hwfn *p_hwfn,
 		DP_NOTICE(p_hwfn, "MAC is 0 in shmem\n");
 	}
 
-	info->wwn_port = (u64)shmem_info.fcoe_wwn_port_name_upper |
-			 (((u64)shmem_info.fcoe_wwn_port_name_lower) << 32);
-	info->wwn_node = (u64)shmem_info.fcoe_wwn_node_name_upper |
-			 (((u64)shmem_info.fcoe_wwn_node_name_lower) << 32);
+	info->wwn_port = (u64)shmem_info.fcoe_wwn_port_name_lower |
+			 (((u64)shmem_info.fcoe_wwn_port_name_upper) << 32);
+	info->wwn_node = (u64)shmem_info.fcoe_wwn_node_name_lower |
+			 (((u64)shmem_info.fcoe_wwn_node_name_upper) << 32);
 
 	info->ovlan = (u16)(shmem_info.ovlan_stag & FUNC_MF_CFG_OV_STAG_MASK);
 
@@ -1770,8 +1827,9 @@ int qed_mcp_get_flash_size(struct qed_hwfn *p_hwfn,
 	return 0;
 }
 
-int qed_mcp_config_vf_msix(struct qed_hwfn *p_hwfn,
-			   struct qed_ptt *p_ptt, u8 vf_id, u8 num)
+static int
+qed_mcp_config_vf_msix_bb(struct qed_hwfn *p_hwfn,
+			  struct qed_ptt *p_ptt, u8 vf_id, u8 num)
 {
 	u32 resp = 0, param = 0, rc_param = 0;
 	int rc;
@@ -1801,6 +1859,36 @@ int qed_mcp_config_vf_msix(struct qed_hwfn *p_hwfn,
 	return rc;
 }
 
+static int
+qed_mcp_config_vf_msix_ah(struct qed_hwfn *p_hwfn,
+			  struct qed_ptt *p_ptt, u8 num)
+{
+	u32 resp = 0, param = num, rc_param = 0;
+	int rc;
+
+	rc = qed_mcp_cmd(p_hwfn, p_ptt, DRV_MSG_CODE_CFG_PF_VFS_MSIX,
+			 param, &resp, &rc_param);
+
+	if (resp != FW_MSG_CODE_DRV_CFG_PF_VFS_MSIX_DONE) {
+		DP_NOTICE(p_hwfn, "MFW failed to set MSI-X for VFs\n");
+		rc = -EINVAL;
+	} else {
+		DP_VERBOSE(p_hwfn, QED_MSG_IOV,
+			   "Requested 0x%02x MSI-x interrupts for VFs\n", num);
+	}
+
+	return rc;
+}
+
+int qed_mcp_config_vf_msix(struct qed_hwfn *p_hwfn,
+			   struct qed_ptt *p_ptt, u8 vf_id, u8 num)
+{
+	if (QED_IS_BB(p_hwfn->cdev))
+		return qed_mcp_config_vf_msix_bb(p_hwfn, p_ptt, vf_id, num);
+	else
+		return qed_mcp_config_vf_msix_ah(p_hwfn, p_ptt, num);
+}
+
 int
 qed_mcp_send_drv_version(struct qed_hwfn *p_hwfn,
 			 struct qed_ptt *p_ptt,
@@ -2222,6 +2310,95 @@ int qed_mcp_bist_nvm_test_get_image_att(struct qed_hwfn *p_hwfn,
 	return rc;
 }
 
+static int
+qed_mcp_get_nvm_image_att(struct qed_hwfn *p_hwfn,
+			  struct qed_ptt *p_ptt,
+			  enum qed_nvm_images image_id,
+			  struct qed_nvm_image_att *p_image_att)
+{
+	struct bist_nvm_image_att mfw_image_att;
+	enum nvm_image_type type;
+	u32 num_images, i;
+	int rc;
+
+	/* Translate image_id into MFW definitions */
+	switch (image_id) {
+	case QED_NVM_IMAGE_ISCSI_CFG:
+		type = NVM_TYPE_ISCSI_CFG;
+		break;
+	case QED_NVM_IMAGE_FCOE_CFG:
+		type = NVM_TYPE_FCOE_CFG;
+		break;
+	default:
+		DP_NOTICE(p_hwfn, "Unknown request of image_id %08x\n",
+			  image_id);
+		return -EINVAL;
+	}
+
+	/* Learn number of images, then traverse and see if one fits */
+	rc = qed_mcp_bist_nvm_test_get_num_images(p_hwfn, p_ptt, &num_images);
+	if (rc || !num_images)
+		return -EINVAL;
+
+	for (i = 0; i < num_images; i++) {
+		rc = qed_mcp_bist_nvm_test_get_image_att(p_hwfn, p_ptt,
+							 &mfw_image_att, i);
+		if (rc)
+			return rc;
+
+		if (type == mfw_image_att.image_type)
+			break;
+	}
+	if (i == num_images) {
+		DP_VERBOSE(p_hwfn, QED_MSG_STORAGE,
+			   "Failed to find nvram image of type %08x\n",
+			   image_id);
+		return -EINVAL;
+	}
+
+	p_image_att->start_addr = mfw_image_att.nvm_start_addr;
+	p_image_att->length = mfw_image_att.len;
+
+	return 0;
+}
+
+int qed_mcp_get_nvm_image(struct qed_hwfn *p_hwfn,
+			  struct qed_ptt *p_ptt,
+			  enum qed_nvm_images image_id,
+			  u8 *p_buffer, u32 buffer_len)
+{
+	struct qed_nvm_image_att image_att;
+	int rc;
+
+	memset(p_buffer, 0, buffer_len);
+
+	rc = qed_mcp_get_nvm_image_att(p_hwfn, p_ptt, image_id, &image_att);
+	if (rc)
+		return rc;
+
+	/* Validate sizes - both the image's and the supplied buffer's */
+	if (image_att.length <= 4) {
+		DP_VERBOSE(p_hwfn, QED_MSG_STORAGE,
+			   "Image [%d] is too small - only %d bytes\n",
+			   image_id, image_att.length);
+		return -EINVAL;
+	}
+
+	/* Each NVM image is suffixed by CRC; Upper-layer has no need for it */
+	image_att.length -= 4;
+
+	if (image_att.length > buffer_len) {
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_STORAGE,
+			   "Image [%d] is too big - %08x bytes where only %08x are available\n",
+			   image_id, image_att.length, buffer_len);
+		return -ENOMEM;
+	}
+
+	return qed_mcp_nvm_read(p_hwfn->cdev, image_att.start_addr,
+				p_buffer, image_att.length);
+}
+
 static enum resource_id_enum qed_mcp_get_mfw_res_id(enum qed_resources res_id)
 {
 	enum resource_id_enum mfw_res_id = RESOURCE_NUM_INVALID;
diff --git a/drivers/net/ethernet/qlogic/qed/qed_mcp.h b/drivers/net/ethernet/qlogic/qed/qed_mcp.h
index 2b09b8545236..af03b3651411 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_mcp.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_mcp.h
@@ -256,6 +256,18 @@ int qed_mcp_get_mfw_ver(struct qed_hwfn *p_hwfn,
 			u32 *p_mfw_ver, u32 *p_running_bundle_id);
 
 /**
+ * @brief Get the MBI version value
+ *
+ * @param p_hwfn
+ * @param p_ptt
+ * @param p_mbi_ver - A pointer to a variable to be filled with the MBI version.
+ *
+ * @return int - 0 - operation was successful.
+ */
+int qed_mcp_get_mbi_ver(struct qed_hwfn *p_hwfn,
+			struct qed_ptt *p_ptt, u32 *p_mbi_ver);
+
+/**
  * @brief Get media type value of the port.
  *
  * @param cdev      - qed dev pointer
@@ -418,6 +430,27 @@ int qed_mcp_set_led(struct qed_hwfn *p_hwfn,
  */
 int qed_mcp_nvm_read(struct qed_dev *cdev, u32 addr, u8 *p_buf, u32 len);
 
+struct qed_nvm_image_att {
+	u32 start_addr;
+	u32 length;
+};
+
+/**
+ * @brief Allows reading a whole nvram image
+ *
+ * @param p_hwfn
+ * @param p_ptt
+ * @param image_id - image requested for reading
+ * @param p_buffer - allocated buffer into which to fill data
+ * @param buffer_len - length of the allocated buffer.
+ *
+ * @return 0 iff p_buffer now contains the nvram image.
+ */
+int qed_mcp_get_nvm_image(struct qed_hwfn *p_hwfn,
+			  struct qed_ptt *p_ptt,
+			  enum qed_nvm_images image_id,
+			  u8 *p_buffer, u32 buffer_len);
+
 /**
  * @brief Bist register test
  *
@@ -482,7 +515,7 @@ int qed_mcp_bist_nvm_test_get_image_att(struct qed_hwfn *p_hwfn,
 #define MCP_PF_ID(p_hwfn) MCP_PF_ID_BY_REL(p_hwfn, (p_hwfn)->rel_pf_id)
 
 #define MFW_PORT(_p_hwfn)       ((_p_hwfn)->abs_pf_id %			  \
-				 ((_p_hwfn)->cdev->num_ports_in_engines * \
+				 ((_p_hwfn)->cdev->num_ports_in_engine * \
 				  qed_device_num_engines((_p_hwfn)->cdev)))
 
 struct qed_mcp_info {
diff --git a/drivers/net/ethernet/qlogic/qed/qed_ooo.c b/drivers/net/ethernet/qlogic/qed/qed_ooo.c
index db96670192c7..000636530111 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_ooo.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_ooo.c
@@ -99,7 +99,7 @@ void qed_ooo_save_history_entry(struct qed_hwfn *p_hwfn,
 	p_history->head_idx++;
 }
 
-struct qed_ooo_info *qed_ooo_alloc(struct qed_hwfn *p_hwfn)
+int qed_ooo_alloc(struct qed_hwfn *p_hwfn)
 {
 	u16 max_num_archipelagos = 0, cid_base;
 	struct qed_ooo_info *p_ooo_info;
@@ -109,7 +109,7 @@ struct qed_ooo_info *qed_ooo_alloc(struct qed_hwfn *p_hwfn)
 	if (p_hwfn->hw_info.personality != QED_PCI_ISCSI) {
 		DP_NOTICE(p_hwfn,
 			  "Failed to allocate qed_ooo_info: unknown personality\n");
-		return NULL;
+		return -EINVAL;
 	}
 
 	max_num_archipelagos = p_hwfn->pf_params.iscsi_pf_params.num_cons;
@@ -119,12 +119,12 @@ struct qed_ooo_info *qed_ooo_alloc(struct qed_hwfn *p_hwfn)
 	if (!max_num_archipelagos) {
 		DP_NOTICE(p_hwfn,
 			  "Failed to allocate qed_ooo_info: unknown amount of connections\n");
-		return NULL;
+		return -EINVAL;
 	}
 
 	p_ooo_info = kzalloc(sizeof(*p_ooo_info), GFP_KERNEL);
 	if (!p_ooo_info)
-		return NULL;
+		return -ENOMEM;
 
 	p_ooo_info->cid_base = cid_base;
 	p_ooo_info->max_num_archipelagos = max_num_archipelagos;
@@ -164,7 +164,8 @@ struct qed_ooo_info *qed_ooo_alloc(struct qed_hwfn *p_hwfn)
 
 	p_ooo_info->ooo_history.num_of_cqes = QED_MAX_NUM_OOO_HISTORY_ENTRIES;
 
-	return p_ooo_info;
+	p_hwfn->p_ooo_info = p_ooo_info;
+	return 0;
 
 no_history_mem:
 	kfree(p_ooo_info->p_archipelagos_mem);
@@ -172,7 +173,7 @@ no_archipelagos_mem:
 	kfree(p_ooo_info->p_isles_mem);
 no_isles_mem:
 	kfree(p_ooo_info);
-	return NULL;
+	return -ENOMEM;
 }
 
 void qed_ooo_release_connection_isles(struct qed_hwfn *p_hwfn,
@@ -249,19 +250,23 @@ void qed_ooo_release_all_isles(struct qed_hwfn *p_hwfn,
 				      &p_ooo_info->free_buffers_list);
 }
 
-void qed_ooo_setup(struct qed_hwfn *p_hwfn, struct qed_ooo_info *p_ooo_info)
+void qed_ooo_setup(struct qed_hwfn *p_hwfn)
 {
-	qed_ooo_release_all_isles(p_hwfn, p_ooo_info);
-	memset(p_ooo_info->ooo_history.p_cqes, 0,
-	       p_ooo_info->ooo_history.num_of_cqes *
+	qed_ooo_release_all_isles(p_hwfn, p_hwfn->p_ooo_info);
+	memset(p_hwfn->p_ooo_info->ooo_history.p_cqes, 0,
+	       p_hwfn->p_ooo_info->ooo_history.num_of_cqes *
 	       sizeof(struct ooo_opaque));
-	p_ooo_info->ooo_history.head_idx = 0;
+	p_hwfn->p_ooo_info->ooo_history.head_idx = 0;
 }
 
-void qed_ooo_free(struct qed_hwfn *p_hwfn, struct qed_ooo_info *p_ooo_info)
+void qed_ooo_free(struct qed_hwfn *p_hwfn)
 {
+	struct qed_ooo_info *p_ooo_info  = p_hwfn->p_ooo_info;
 	struct qed_ooo_buffer *p_buffer;
 
+	if (!p_ooo_info)
+		return;
+
 	qed_ooo_release_all_isles(p_hwfn, p_ooo_info);
 	while (!list_empty(&p_ooo_info->free_buffers_list)) {
 		p_buffer = list_first_entry(&p_ooo_info->free_buffers_list,
@@ -282,6 +287,7 @@ void qed_ooo_free(struct qed_hwfn *p_hwfn, struct qed_ooo_info *p_ooo_info)
 	kfree(p_ooo_info->p_archipelagos_mem);
 	kfree(p_ooo_info->ooo_history.p_cqes);
 	kfree(p_ooo_info);
+	p_hwfn->p_ooo_info = NULL;
 }
 
 void qed_ooo_put_free_buffer(struct qed_hwfn *p_hwfn,
diff --git a/drivers/net/ethernet/qlogic/qed/qed_ooo.h b/drivers/net/ethernet/qlogic/qed/qed_ooo.h
index 791ad0f8b759..e8ed40b848f5 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_ooo.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_ooo.h
@@ -88,7 +88,11 @@ void qed_ooo_save_history_entry(struct qed_hwfn *p_hwfn,
 				struct qed_ooo_info *p_ooo_info,
 				struct ooo_opaque *p_cqe);
 
-struct qed_ooo_info *qed_ooo_alloc(struct qed_hwfn *p_hwfn);
+int qed_ooo_alloc(struct qed_hwfn *p_hwfn);
+
+void qed_ooo_setup(struct qed_hwfn *p_hwfn);
+
+void qed_ooo_free(struct qed_hwfn *p_hwfn);
 
 void qed_ooo_release_connection_isles(struct qed_hwfn *p_hwfn,
 				      struct qed_ooo_info *p_ooo_info,
@@ -97,10 +101,6 @@ void qed_ooo_release_connection_isles(struct qed_hwfn *p_hwfn,
 void qed_ooo_release_all_isles(struct qed_hwfn *p_hwfn,
 			       struct qed_ooo_info *p_ooo_info);
 
-void qed_ooo_setup(struct qed_hwfn *p_hwfn, struct qed_ooo_info *p_ooo_info);
-
-void qed_ooo_free(struct qed_hwfn *p_hwfn, struct qed_ooo_info *p_ooo_info);
-
 void qed_ooo_put_free_buffer(struct qed_hwfn *p_hwfn,
 			     struct qed_ooo_info *p_ooo_info,
 			     struct qed_ooo_buffer *p_buffer);
@@ -140,8 +140,14 @@ static inline void qed_ooo_save_history_entry(struct qed_hwfn *p_hwfn,
 					      struct qed_ooo_info *p_ooo_info,
 					      struct ooo_opaque *p_cqe) {}
 
-static inline struct qed_ooo_info *qed_ooo_alloc(
-				struct qed_hwfn *p_hwfn) { return NULL; }
+static inline int qed_ooo_alloc(struct qed_hwfn *p_hwfn)
+{
+	return -EINVAL;
+}
+
+static inline void qed_ooo_setup(struct qed_hwfn *p_hwfn) {}
+
+static inline void qed_ooo_free(struct qed_hwfn *p_hwfn) {}
 
 static inline void
 qed_ooo_release_connection_isles(struct qed_hwfn *p_hwfn,
@@ -152,12 +158,6 @@ static inline void qed_ooo_release_all_isles(struct qed_hwfn *p_hwfn,
 					     struct qed_ooo_info *p_ooo_info)
 					     {}
 
-static inline void qed_ooo_setup(struct qed_hwfn *p_hwfn,
-				 struct qed_ooo_info *p_ooo_info) {}
-
-static inline void qed_ooo_free(struct qed_hwfn *p_hwfn,
-				struct qed_ooo_info *p_ooo_info) {}
-
 static inline void qed_ooo_put_free_buffer(struct qed_hwfn *p_hwfn,
 					   struct qed_ooo_info *p_ooo_info,
 					   struct qed_ooo_buffer *p_buffer) {}
diff --git a/drivers/net/ethernet/qlogic/qed/qed_ptp.c b/drivers/net/ethernet/qlogic/qed/qed_ptp.c
index 434a164a76ed..5a90d69dc2f8 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_ptp.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_ptp.c
@@ -80,7 +80,7 @@ static int qed_ptp_res_lock(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 		/* MFW doesn't support resource locking, first PF on the port
 		 * has lock ownership.
 		 */
-		if (p_hwfn->abs_pf_id < p_hwfn->cdev->num_ports_in_engines)
+		if (p_hwfn->abs_pf_id < p_hwfn->cdev->num_ports_in_engine)
 			return 0;
 
 		DP_INFO(p_hwfn, "PF doesn't have lock ownership\n");
@@ -108,7 +108,7 @@ static int qed_ptp_res_unlock(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 	rc = qed_mcp_resc_unlock(p_hwfn, p_ptt, &params);
 	if (rc == -EINVAL) {
 		/* MFW doesn't support locking, first PF has lock ownership */
-		if (p_hwfn->abs_pf_id < p_hwfn->cdev->num_ports_in_engines) {
+		if (p_hwfn->abs_pf_id < p_hwfn->cdev->num_ports_in_engine) {
 			rc = 0;
 		} else {
 			DP_INFO(p_hwfn, "PF doesn't have lock ownership\n");
diff --git a/drivers/net/ethernet/qlogic/qed/qed_rdma.c b/drivers/net/ethernet/qlogic/qed/qed_rdma.c
new file mode 100644
index 000000000000..6fb99518a61f
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qed/qed_rdma.c
@@ -0,0 +1,1787 @@
+/* QLogic qed NIC Driver
+ * Copyright (c) 2015-2017  QLogic Corporation
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and /or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+#include <linux/types.h>
+#include <asm/byteorder.h>
+#include <linux/bitops.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/errno.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/pci.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/string.h>
+#include "qed.h"
+#include "qed_cxt.h"
+#include "qed_hsi.h"
+#include "qed_hw.h"
+#include "qed_init_ops.h"
+#include "qed_int.h"
+#include "qed_ll2.h"
+#include "qed_mcp.h"
+#include "qed_reg_addr.h"
+#include <linux/qed/qed_rdma_if.h>
+#include "qed_rdma.h"
+#include "qed_roce.h"
+#include "qed_sp.h"
+
+
+int qed_rdma_bmap_alloc(struct qed_hwfn *p_hwfn,
+			struct qed_bmap *bmap, u32 max_count, char *name)
+{
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "max_count = %08x\n", max_count);
+
+	bmap->max_count = max_count;
+
+	bmap->bitmap = kcalloc(BITS_TO_LONGS(max_count), sizeof(long),
+			       GFP_KERNEL);
+	if (!bmap->bitmap)
+		return -ENOMEM;
+
+	snprintf(bmap->name, QED_RDMA_MAX_BMAP_NAME, "%s", name);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "0\n");
+	return 0;
+}
+
+int qed_rdma_bmap_alloc_id(struct qed_hwfn *p_hwfn,
+			   struct qed_bmap *bmap, u32 *id_num)
+{
+	*id_num = find_first_zero_bit(bmap->bitmap, bmap->max_count);
+	if (*id_num >= bmap->max_count)
+		return -EINVAL;
+
+	__set_bit(*id_num, bmap->bitmap);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "%s bitmap: allocated id %d\n",
+		   bmap->name, *id_num);
+
+	return 0;
+}
+
+void qed_bmap_set_id(struct qed_hwfn *p_hwfn,
+		     struct qed_bmap *bmap, u32 id_num)
+{
+	if (id_num >= bmap->max_count)
+		return;
+
+	__set_bit(id_num, bmap->bitmap);
+}
+
+void qed_bmap_release_id(struct qed_hwfn *p_hwfn,
+			 struct qed_bmap *bmap, u32 id_num)
+{
+	bool b_acquired;
+
+	if (id_num >= bmap->max_count)
+		return;
+
+	b_acquired = test_and_clear_bit(id_num, bmap->bitmap);
+	if (!b_acquired) {
+		DP_NOTICE(p_hwfn, "%s bitmap: id %d already released\n",
+			  bmap->name, id_num);
+		return;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "%s bitmap: released id %d\n",
+		   bmap->name, id_num);
+}
+
+int qed_bmap_test_id(struct qed_hwfn *p_hwfn,
+		     struct qed_bmap *bmap, u32 id_num)
+{
+	if (id_num >= bmap->max_count)
+		return -1;
+
+	return test_bit(id_num, bmap->bitmap);
+}
+
+static bool qed_bmap_is_empty(struct qed_bmap *bmap)
+{
+	return bmap->max_count == find_first_bit(bmap->bitmap, bmap->max_count);
+}
+
+u32 qed_rdma_get_sb_id(void *p_hwfn, u32 rel_sb_id)
+{
+	/* First sb id for RoCE is after all the l2 sb */
+	return FEAT_NUM((struct qed_hwfn *)p_hwfn, QED_PF_L2_QUE) + rel_sb_id;
+}
+
+static int qed_rdma_alloc(struct qed_hwfn *p_hwfn,
+			  struct qed_ptt *p_ptt,
+			  struct qed_rdma_start_in_params *params)
+{
+	struct qed_rdma_info *p_rdma_info;
+	u32 num_cons, num_tasks;
+	int rc = -ENOMEM;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Allocating RDMA\n");
+
+	/* Allocate a struct with current pf rdma info */
+	p_rdma_info = kzalloc(sizeof(*p_rdma_info), GFP_KERNEL);
+	if (!p_rdma_info)
+		return rc;
+
+	p_hwfn->p_rdma_info = p_rdma_info;
+	p_rdma_info->proto = PROTOCOLID_ROCE;
+
+	num_cons = qed_cxt_get_proto_cid_count(p_hwfn, p_rdma_info->proto,
+					       NULL);
+
+	if (QED_IS_IWARP_PERSONALITY(p_hwfn))
+		p_rdma_info->num_qps = num_cons;
+	else
+		p_rdma_info->num_qps = num_cons / 2; /* 2 cids per qp */
+
+	num_tasks = qed_cxt_get_proto_tid_count(p_hwfn, PROTOCOLID_ROCE);
+
+	/* Each MR uses a single task */
+	p_rdma_info->num_mrs = num_tasks;
+
+	/* Queue zone lines are shared between RoCE and L2 in such a way that
+	 * they can be used by each without obstructing the other.
+	 */
+	p_rdma_info->queue_zone_base = (u16)RESC_START(p_hwfn, QED_L2_QUEUE);
+	p_rdma_info->max_queue_zones = (u16)RESC_NUM(p_hwfn, QED_L2_QUEUE);
+
+	/* Allocate a struct with device params and fill it */
+	p_rdma_info->dev = kzalloc(sizeof(*p_rdma_info->dev), GFP_KERNEL);
+	if (!p_rdma_info->dev)
+		goto free_rdma_info;
+
+	/* Allocate a struct with port params and fill it */
+	p_rdma_info->port = kzalloc(sizeof(*p_rdma_info->port), GFP_KERNEL);
+	if (!p_rdma_info->port)
+		goto free_rdma_dev;
+
+	/* Allocate bit map for pd's */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->pd_map, RDMA_MAX_PDS,
+				 "PD");
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate pd_map, rc = %d\n",
+			   rc);
+		goto free_rdma_port;
+	}
+
+	/* Allocate DPI bitmap */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->dpi_map,
+				 p_hwfn->dpi_count, "DPI");
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate DPI bitmap, rc = %d\n", rc);
+		goto free_pd_map;
+	}
+
+	/* Allocate bitmap for cq's. The maximum number of CQs is bounded to
+	 * twice the number of QPs.
+	 */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->cq_map,
+				 p_rdma_info->num_qps * 2, "CQ");
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate cq bitmap, rc = %d\n", rc);
+		goto free_dpi_map;
+	}
+
+	/* Allocate bitmap for toggle bit for cq icids
+	 * We toggle the bit every time we create or resize cq for a given icid.
+	 * The maximum number of CQs is bounded to  twice the number of QPs.
+	 */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->toggle_bits,
+				 p_rdma_info->num_qps * 2, "Toggle");
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate toogle bits, rc = %d\n", rc);
+		goto free_cq_map;
+	}
+
+	/* Allocate bitmap for itids */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->tid_map,
+				 p_rdma_info->num_mrs, "MR");
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate itids bitmaps, rc = %d\n", rc);
+		goto free_toggle_map;
+	}
+
+	/* Allocate bitmap for cids used for qps. */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->cid_map, num_cons,
+				 "CID");
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate cid bitmap, rc = %d\n", rc);
+		goto free_tid_map;
+	}
+
+	/* Allocate bitmap for cids used for responders/requesters. */
+	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->real_cid_map, num_cons,
+				 "REAL_CID");
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+			   "Failed to allocate real cid bitmap, rc = %d\n", rc);
+		goto free_cid_map;
+	}
+
+	if (QED_IS_IWARP_PERSONALITY(p_hwfn))
+		rc = qed_iwarp_alloc(p_hwfn);
+
+	if (rc)
+		goto free_cid_map;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Allocation successful\n");
+	return 0;
+
+free_cid_map:
+	kfree(p_rdma_info->cid_map.bitmap);
+free_tid_map:
+	kfree(p_rdma_info->tid_map.bitmap);
+free_toggle_map:
+	kfree(p_rdma_info->toggle_bits.bitmap);
+free_cq_map:
+	kfree(p_rdma_info->cq_map.bitmap);
+free_dpi_map:
+	kfree(p_rdma_info->dpi_map.bitmap);
+free_pd_map:
+	kfree(p_rdma_info->pd_map.bitmap);
+free_rdma_port:
+	kfree(p_rdma_info->port);
+free_rdma_dev:
+	kfree(p_rdma_info->dev);
+free_rdma_info:
+	kfree(p_rdma_info);
+
+	return rc;
+}
+
+void qed_rdma_bmap_free(struct qed_hwfn *p_hwfn,
+			struct qed_bmap *bmap, bool check)
+{
+	int weight = bitmap_weight(bmap->bitmap, bmap->max_count);
+	int last_line = bmap->max_count / (64 * 8);
+	int last_item = last_line * 8 +
+	    DIV_ROUND_UP(bmap->max_count % (64 * 8), 64);
+	u64 *pmap = (u64 *)bmap->bitmap;
+	int line, item, offset;
+	u8 str_last_line[200] = { 0 };
+
+	if (!weight || !check)
+		goto end;
+
+	DP_NOTICE(p_hwfn,
+		  "%s bitmap not free - size=%d, weight=%d, 512 bits per line\n",
+		  bmap->name, bmap->max_count, weight);
+
+	/* print aligned non-zero lines, if any */
+	for (item = 0, line = 0; line < last_line; line++, item += 8)
+		if (bitmap_weight((unsigned long *)&pmap[item], 64 * 8))
+			DP_NOTICE(p_hwfn,
+				  "line 0x%04x: 0x%016llx 0x%016llx 0x%016llx 0x%016llx 0x%016llx 0x%016llx 0x%016llx 0x%016llx\n",
+				  line,
+				  pmap[item],
+				  pmap[item + 1],
+				  pmap[item + 2],
+				  pmap[item + 3],
+				  pmap[item + 4],
+				  pmap[item + 5],
+				  pmap[item + 6], pmap[item + 7]);
+
+	/* print last unaligned non-zero line, if any */
+	if ((bmap->max_count % (64 * 8)) &&
+	    (bitmap_weight((unsigned long *)&pmap[item],
+			   bmap->max_count - item * 64))) {
+		offset = sprintf(str_last_line, "line 0x%04x: ", line);
+		for (; item < last_item; item++)
+			offset += sprintf(str_last_line + offset,
+					  "0x%016llx ", pmap[item]);
+		DP_NOTICE(p_hwfn, "%s\n", str_last_line);
+	}
+
+end:
+	kfree(bmap->bitmap);
+	bmap->bitmap = NULL;
+}
+
+static void qed_rdma_resc_free(struct qed_hwfn *p_hwfn)
+{
+	struct qed_rdma_info *p_rdma_info = p_hwfn->p_rdma_info;
+
+	if (QED_IS_IWARP_PERSONALITY(p_hwfn))
+		qed_iwarp_resc_free(p_hwfn);
+
+	qed_rdma_bmap_free(p_hwfn, &p_hwfn->p_rdma_info->cid_map, 1);
+	qed_rdma_bmap_free(p_hwfn, &p_hwfn->p_rdma_info->pd_map, 1);
+	qed_rdma_bmap_free(p_hwfn, &p_hwfn->p_rdma_info->dpi_map, 1);
+	qed_rdma_bmap_free(p_hwfn, &p_hwfn->p_rdma_info->cq_map, 1);
+	qed_rdma_bmap_free(p_hwfn, &p_hwfn->p_rdma_info->toggle_bits, 0);
+	qed_rdma_bmap_free(p_hwfn, &p_hwfn->p_rdma_info->tid_map, 1);
+
+	kfree(p_rdma_info->port);
+	kfree(p_rdma_info->dev);
+
+	kfree(p_rdma_info);
+}
+
+static void qed_rdma_free(struct qed_hwfn *p_hwfn)
+{
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Freeing RDMA\n");
+
+	qed_rdma_resc_free(p_hwfn);
+}
+
+static void qed_rdma_get_guid(struct qed_hwfn *p_hwfn, u8 *guid)
+{
+	guid[0] = p_hwfn->hw_info.hw_mac_addr[0] ^ 2;
+	guid[1] = p_hwfn->hw_info.hw_mac_addr[1];
+	guid[2] = p_hwfn->hw_info.hw_mac_addr[2];
+	guid[3] = 0xff;
+	guid[4] = 0xfe;
+	guid[5] = p_hwfn->hw_info.hw_mac_addr[3];
+	guid[6] = p_hwfn->hw_info.hw_mac_addr[4];
+	guid[7] = p_hwfn->hw_info.hw_mac_addr[5];
+}
+
+static void qed_rdma_init_events(struct qed_hwfn *p_hwfn,
+				 struct qed_rdma_start_in_params *params)
+{
+	struct qed_rdma_events *events;
+
+	events = &p_hwfn->p_rdma_info->events;
+
+	events->unaffiliated_event = params->events->unaffiliated_event;
+	events->affiliated_event = params->events->affiliated_event;
+	events->context = params->events->context;
+}
+
+static void qed_rdma_init_devinfo(struct qed_hwfn *p_hwfn,
+				  struct qed_rdma_start_in_params *params)
+{
+	struct qed_rdma_device *dev = p_hwfn->p_rdma_info->dev;
+	struct qed_dev *cdev = p_hwfn->cdev;
+	u32 pci_status_control;
+	u32 num_qps;
+
+	/* Vendor specific information */
+	dev->vendor_id = cdev->vendor_id;
+	dev->vendor_part_id = cdev->device_id;
+	dev->hw_ver = 0;
+	dev->fw_ver = (FW_MAJOR_VERSION << 24) | (FW_MINOR_VERSION << 16) |
+		      (FW_REVISION_VERSION << 8) | (FW_ENGINEERING_VERSION);
+
+	qed_rdma_get_guid(p_hwfn, (u8 *)&dev->sys_image_guid);
+	dev->node_guid = dev->sys_image_guid;
+
+	dev->max_sge = min_t(u32, RDMA_MAX_SGE_PER_SQ_WQE,
+			     RDMA_MAX_SGE_PER_RQ_WQE);
+
+	if (cdev->rdma_max_sge)
+		dev->max_sge = min_t(u32, cdev->rdma_max_sge, dev->max_sge);
+
+	dev->max_inline = ROCE_REQ_MAX_INLINE_DATA_SIZE;
+
+	dev->max_inline = (cdev->rdma_max_inline) ?
+			  min_t(u32, cdev->rdma_max_inline, dev->max_inline) :
+			  dev->max_inline;
+
+	dev->max_wqe = QED_RDMA_MAX_WQE;
+	dev->max_cnq = (u8)FEAT_NUM(p_hwfn, QED_RDMA_CNQ);
+
+	/* The number of QPs may be higher than QED_ROCE_MAX_QPS, because
+	 * it is up-aligned to 16 and then to ILT page size within qed cxt.
+	 * This is OK in terms of ILT but we don't want to configure the FW
+	 * above its abilities
+	 */
+	num_qps = ROCE_MAX_QPS;
+	num_qps = min_t(u64, num_qps, p_hwfn->p_rdma_info->num_qps);
+	dev->max_qp = num_qps;
+
+	/* CQs uses the same icids that QPs use hence they are limited by the
+	 * number of icids. There are two icids per QP.
+	 */
+	dev->max_cq = num_qps * 2;
+
+	/* The number of mrs is smaller by 1 since the first is reserved */
+	dev->max_mr = p_hwfn->p_rdma_info->num_mrs - 1;
+	dev->max_mr_size = QED_RDMA_MAX_MR_SIZE;
+
+	/* The maximum CQE capacity per CQ supported.
+	 * max number of cqes will be in two layer pbl,
+	 * 8 is the pointer size in bytes
+	 * 32 is the size of cq element in bytes
+	 */
+	if (params->cq_mode == QED_RDMA_CQ_MODE_32_BITS)
+		dev->max_cqe = QED_RDMA_MAX_CQE_32_BIT;
+	else
+		dev->max_cqe = QED_RDMA_MAX_CQE_16_BIT;
+
+	dev->max_mw = 0;
+	dev->max_fmr = QED_RDMA_MAX_FMR;
+	dev->max_mr_mw_fmr_pbl = (PAGE_SIZE / 8) * (PAGE_SIZE / 8);
+	dev->max_mr_mw_fmr_size = dev->max_mr_mw_fmr_pbl * PAGE_SIZE;
+	dev->max_pkey = QED_RDMA_MAX_P_KEY;
+
+	dev->max_qp_resp_rd_atomic_resc = RDMA_RING_PAGE_SIZE /
+					  (RDMA_RESP_RD_ATOMIC_ELM_SIZE * 2);
+	dev->max_qp_req_rd_atomic_resc = RDMA_RING_PAGE_SIZE /
+					 RDMA_REQ_RD_ATOMIC_ELM_SIZE;
+	dev->max_dev_resp_rd_atomic_resc = dev->max_qp_resp_rd_atomic_resc *
+					   p_hwfn->p_rdma_info->num_qps;
+	dev->page_size_caps = QED_RDMA_PAGE_SIZE_CAPS;
+	dev->dev_ack_delay = QED_RDMA_ACK_DELAY;
+	dev->max_pd = RDMA_MAX_PDS;
+	dev->max_ah = p_hwfn->p_rdma_info->num_qps;
+	dev->max_stats_queues = (u8)RESC_NUM(p_hwfn, QED_RDMA_STATS_QUEUE);
+
+	/* Set capablities */
+	dev->dev_caps = 0;
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_RNR_NAK, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_PORT_ACTIVE_EVENT, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_PORT_CHANGE_EVENT, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_RESIZE_CQ, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_BASE_MEMORY_EXT, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_BASE_QUEUE_EXT, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_ZBVA, 1);
+	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_LOCAL_INV_FENCE, 1);
+
+	/* Check atomic operations support in PCI configuration space. */
+	pci_read_config_dword(cdev->pdev,
+			      cdev->pdev->pcie_cap + PCI_EXP_DEVCTL2,
+			      &pci_status_control);
+
+	if (pci_status_control & PCI_EXP_DEVCTL2_LTR_EN)
+		SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_ATOMIC_OP, 1);
+
+	if (QED_IS_IWARP_PERSONALITY(p_hwfn))
+		qed_iwarp_init_devinfo(p_hwfn);
+}
+
+static void qed_rdma_init_port(struct qed_hwfn *p_hwfn)
+{
+	struct qed_rdma_port *port = p_hwfn->p_rdma_info->port;
+	struct qed_rdma_device *dev = p_hwfn->p_rdma_info->dev;
+
+	port->port_state = p_hwfn->mcp_info->link_output.link_up ?
+			   QED_RDMA_PORT_UP : QED_RDMA_PORT_DOWN;
+
+	port->max_msg_size = min_t(u64,
+				   (dev->max_mr_mw_fmr_size *
+				    p_hwfn->cdev->rdma_max_sge),
+				   BIT(31));
+
+	port->pkey_bad_counter = 0;
+}
+
+static int qed_rdma_init_hw(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+{
+	int rc = 0;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Initializing HW\n");
+	p_hwfn->b_rdma_enabled_in_prs = false;
+
+	if (QED_IS_IWARP_PERSONALITY(p_hwfn))
+		qed_iwarp_init_hw(p_hwfn, p_ptt);
+	else
+		rc = qed_roce_init_hw(p_hwfn, p_ptt);
+
+	return rc;
+}
+
+static int qed_rdma_start_fw(struct qed_hwfn *p_hwfn,
+			     struct qed_rdma_start_in_params *params,
+			     struct qed_ptt *p_ptt)
+{
+	struct rdma_init_func_ramrod_data *p_ramrod;
+	struct qed_rdma_cnq_params *p_cnq_pbl_list;
+	struct rdma_init_func_hdr *p_params_header;
+	struct rdma_cnq_params *p_cnq_params;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	u32 cnq_id, sb_id;
+	u16 igu_sb_id;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Starting FW\n");
+
+	/* Save the number of cnqs for the function close ramrod */
+	p_hwfn->p_rdma_info->num_cnqs = params->desired_cnq;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent, RDMA_RAMROD_FUNC_INIT,
+				 p_hwfn->p_rdma_info->proto, &init_data);
+	if (rc)
+		return rc;
+
+	if (QED_IS_IWARP_PERSONALITY(p_hwfn))
+		p_ramrod = &p_ent->ramrod.iwarp_init_func.rdma;
+	else
+		p_ramrod = &p_ent->ramrod.roce_init_func.rdma;
+
+	p_params_header = &p_ramrod->params_header;
+	p_params_header->cnq_start_offset = (u8)RESC_START(p_hwfn,
+							   QED_RDMA_CNQ_RAM);
+	p_params_header->num_cnqs = params->desired_cnq;
+
+	if (params->cq_mode == QED_RDMA_CQ_MODE_16_BITS)
+		p_params_header->cq_ring_mode = 1;
+	else
+		p_params_header->cq_ring_mode = 0;
+
+	for (cnq_id = 0; cnq_id < params->desired_cnq; cnq_id++) {
+		sb_id = qed_rdma_get_sb_id(p_hwfn, cnq_id);
+		igu_sb_id = qed_get_igu_sb_id(p_hwfn, sb_id);
+		p_ramrod->cnq_params[cnq_id].sb_num = cpu_to_le16(igu_sb_id);
+		p_cnq_params = &p_ramrod->cnq_params[cnq_id];
+		p_cnq_pbl_list = &params->cnq_pbl_list[cnq_id];
+
+		p_cnq_params->sb_index = p_hwfn->pf_params.rdma_pf_params.gl_pi;
+		p_cnq_params->num_pbl_pages = p_cnq_pbl_list->num_pbl_pages;
+
+		DMA_REGPAIR_LE(p_cnq_params->pbl_base_addr,
+			       p_cnq_pbl_list->pbl_ptr);
+
+		/* we assume here that cnq_id and qz_offset are the same */
+		p_cnq_params->queue_zone_num =
+			cpu_to_le16(p_hwfn->p_rdma_info->queue_zone_base +
+				    cnq_id);
+	}
+
+	return qed_spq_post(p_hwfn, p_ent, NULL);
+}
+
+static int qed_rdma_alloc_tid(void *rdma_cxt, u32 *itid)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Allocate TID\n");
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	rc = qed_rdma_bmap_alloc_id(p_hwfn,
+				    &p_hwfn->p_rdma_info->tid_map, itid);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+	if (rc)
+		goto out;
+
+	rc = qed_cxt_dynamic_ilt_alloc(p_hwfn, QED_ELEM_TASK, *itid);
+out:
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Allocate TID - done, rc = %d\n", rc);
+	return rc;
+}
+
+static int qed_rdma_reserve_lkey(struct qed_hwfn *p_hwfn)
+{
+	struct qed_rdma_device *dev = p_hwfn->p_rdma_info->dev;
+
+	/* The first DPI is reserved for the Kernel */
+	__set_bit(0, p_hwfn->p_rdma_info->dpi_map.bitmap);
+
+	/* Tid 0 will be used as the key for "reserved MR".
+	 * The driver should allocate memory for it so it can be loaded but no
+	 * ramrod should be passed on it.
+	 */
+	qed_rdma_alloc_tid(p_hwfn, &dev->reserved_lkey);
+	if (dev->reserved_lkey != RDMA_RESERVED_LKEY) {
+		DP_NOTICE(p_hwfn,
+			  "Reserved lkey should be equal to RDMA_RESERVED_LKEY\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int qed_rdma_setup(struct qed_hwfn *p_hwfn,
+			  struct qed_ptt *p_ptt,
+			  struct qed_rdma_start_in_params *params)
+{
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA setup\n");
+
+	spin_lock_init(&p_hwfn->p_rdma_info->lock);
+
+	qed_rdma_init_devinfo(p_hwfn, params);
+	qed_rdma_init_port(p_hwfn);
+	qed_rdma_init_events(p_hwfn, params);
+
+	rc = qed_rdma_reserve_lkey(p_hwfn);
+	if (rc)
+		return rc;
+
+	rc = qed_rdma_init_hw(p_hwfn, p_ptt);
+	if (rc)
+		return rc;
+
+	if (QED_IS_IWARP_PERSONALITY(p_hwfn)) {
+		rc = qed_iwarp_setup(p_hwfn, p_ptt, params);
+		if (rc)
+			return rc;
+	} else {
+		rc = qed_roce_setup(p_hwfn);
+		if (rc)
+			return rc;
+	}
+
+	return qed_rdma_start_fw(p_hwfn, params, p_ptt);
+}
+
+int qed_rdma_stop(void *rdma_cxt)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct rdma_close_func_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	struct qed_ptt *p_ptt;
+	u32 ll2_ethertype_en;
+	int rc = -EBUSY;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA stop\n");
+
+	p_ptt = qed_ptt_acquire(p_hwfn);
+	if (!p_ptt) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Failed to acquire PTT\n");
+		return rc;
+	}
+
+	/* Disable RoCE search */
+	qed_wr(p_hwfn, p_ptt, p_hwfn->rdma_prs_search_reg, 0);
+	p_hwfn->b_rdma_enabled_in_prs = false;
+
+	qed_wr(p_hwfn, p_ptt, PRS_REG_ROCE_DEST_QP_MAX_PF, 0);
+
+	ll2_ethertype_en = qed_rd(p_hwfn, p_ptt, PRS_REG_LIGHT_L2_ETHERTYPE_EN);
+
+	qed_wr(p_hwfn, p_ptt, PRS_REG_LIGHT_L2_ETHERTYPE_EN,
+	       (ll2_ethertype_en & 0xFFFE));
+
+	if (QED_IS_IWARP_PERSONALITY(p_hwfn)) {
+		rc = qed_iwarp_stop(p_hwfn, p_ptt);
+		if (rc) {
+			qed_ptt_release(p_hwfn, p_ptt);
+			return rc;
+		}
+	} else {
+		qed_roce_stop(p_hwfn);
+	}
+
+	qed_ptt_release(p_hwfn, p_ptt);
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	/* Stop RoCE */
+	rc = qed_sp_init_request(p_hwfn, &p_ent, RDMA_RAMROD_FUNC_CLOSE,
+				 p_hwfn->p_rdma_info->proto, &init_data);
+	if (rc)
+		goto out;
+
+	p_ramrod = &p_ent->ramrod.rdma_close_func;
+
+	p_ramrod->num_cnqs = p_hwfn->p_rdma_info->num_cnqs;
+	p_ramrod->cnq_start_offset = (u8)RESC_START(p_hwfn, QED_RDMA_CNQ_RAM);
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+
+out:
+	qed_rdma_free(p_hwfn);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA stop done, rc = %d\n", rc);
+	return rc;
+}
+
+static int qed_rdma_add_user(void *rdma_cxt,
+			     struct qed_rdma_add_user_out_params *out_params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	u32 dpi_start_offset;
+	u32 returned_id = 0;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Adding User\n");
+
+	/* Allocate DPI */
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	rc = qed_rdma_bmap_alloc_id(p_hwfn, &p_hwfn->p_rdma_info->dpi_map,
+				    &returned_id);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+
+	out_params->dpi = (u16)returned_id;
+
+	/* Calculate the corresponding DPI address */
+	dpi_start_offset = p_hwfn->dpi_start_offset;
+
+	out_params->dpi_addr = (u64)((u8 __iomem *)p_hwfn->doorbells +
+				     dpi_start_offset +
+				     ((out_params->dpi) * p_hwfn->dpi_size));
+
+	out_params->dpi_phys_addr = p_hwfn->cdev->db_phys_addr +
+				    dpi_start_offset +
+				    ((out_params->dpi) * p_hwfn->dpi_size);
+
+	out_params->dpi_size = p_hwfn->dpi_size;
+	out_params->wid_count = p_hwfn->wid_count;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Adding user - done, rc = %d\n", rc);
+	return rc;
+}
+
+static struct qed_rdma_port *qed_rdma_query_port(void *rdma_cxt)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct qed_rdma_port *p_port = p_hwfn->p_rdma_info->port;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA Query port\n");
+
+	/* Link may have changed */
+	p_port->port_state = p_hwfn->mcp_info->link_output.link_up ?
+			     QED_RDMA_PORT_UP : QED_RDMA_PORT_DOWN;
+
+	p_port->link_speed = p_hwfn->mcp_info->link_output.speed;
+
+	p_port->max_msg_size = RDMA_MAX_DATA_SIZE_IN_WQE;
+
+	return p_port;
+}
+
+static struct qed_rdma_device *qed_rdma_query_device(void *rdma_cxt)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Query device\n");
+
+	/* Return struct with device parameters */
+	return p_hwfn->p_rdma_info->dev;
+}
+
+static void qed_rdma_free_tid(void *rdma_cxt, u32 itid)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "itid = %08x\n", itid);
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->tid_map, itid);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+}
+
+static void qed_rdma_cnq_prod_update(void *rdma_cxt, u8 qz_offset, u16 prod)
+{
+	struct qed_hwfn *p_hwfn;
+	u16 qz_num;
+	u32 addr;
+
+	p_hwfn = (struct qed_hwfn *)rdma_cxt;
+
+	if (qz_offset > p_hwfn->p_rdma_info->max_queue_zones) {
+		DP_NOTICE(p_hwfn,
+			  "queue zone offset %d is too large (max is %d)\n",
+			  qz_offset, p_hwfn->p_rdma_info->max_queue_zones);
+		return;
+	}
+
+	qz_num = p_hwfn->p_rdma_info->queue_zone_base + qz_offset;
+	addr = GTT_BAR0_MAP_REG_USDM_RAM +
+	       USTORM_COMMON_QUEUE_CONS_OFFSET(qz_num);
+
+	REG_WR16(p_hwfn, addr, prod);
+
+	/* keep prod updates ordered */
+	wmb();
+}
+
+static int qed_fill_rdma_dev_info(struct qed_dev *cdev,
+				  struct qed_dev_rdma_info *info)
+{
+	struct qed_hwfn *p_hwfn = QED_LEADING_HWFN(cdev);
+
+	memset(info, 0, sizeof(*info));
+
+	info->rdma_type = QED_IS_ROCE_PERSONALITY(p_hwfn) ?
+	    QED_RDMA_TYPE_ROCE : QED_RDMA_TYPE_IWARP;
+
+	info->user_dpm_enabled = (p_hwfn->db_bar_no_edpm == 0);
+
+	qed_fill_dev_info(cdev, &info->common);
+
+	return 0;
+}
+
+static int qed_rdma_get_sb_start(struct qed_dev *cdev)
+{
+	int feat_num;
+
+	if (cdev->num_hwfns > 1)
+		feat_num = FEAT_NUM(QED_LEADING_HWFN(cdev), QED_PF_L2_QUE);
+	else
+		feat_num = FEAT_NUM(QED_LEADING_HWFN(cdev), QED_PF_L2_QUE) *
+			   cdev->num_hwfns;
+
+	return feat_num;
+}
+
+static int qed_rdma_get_min_cnq_msix(struct qed_dev *cdev)
+{
+	int n_cnq = FEAT_NUM(QED_LEADING_HWFN(cdev), QED_RDMA_CNQ);
+	int n_msix = cdev->int_params.rdma_msix_cnt;
+
+	return min_t(int, n_cnq, n_msix);
+}
+
+static int qed_rdma_set_int(struct qed_dev *cdev, u16 cnt)
+{
+	int limit = 0;
+
+	/* Mark the fastpath as free/used */
+	cdev->int_params.fp_initialized = cnt ? true : false;
+
+	if (cdev->int_params.out.int_mode != QED_INT_MODE_MSIX) {
+		DP_ERR(cdev,
+		       "qed roce supports only MSI-X interrupts (detected %d).\n",
+		       cdev->int_params.out.int_mode);
+		return -EINVAL;
+	} else if (cdev->int_params.fp_msix_cnt) {
+		limit = cdev->int_params.rdma_msix_cnt;
+	}
+
+	if (!limit)
+		return -ENOMEM;
+
+	return min_t(int, cnt, limit);
+}
+
+static int qed_rdma_get_int(struct qed_dev *cdev, struct qed_int_info *info)
+{
+	memset(info, 0, sizeof(*info));
+
+	if (!cdev->int_params.fp_initialized) {
+		DP_INFO(cdev,
+			"Protocol driver requested interrupt information, but its support is not yet configured\n");
+		return -EINVAL;
+	}
+
+	if (cdev->int_params.out.int_mode == QED_INT_MODE_MSIX) {
+		int msix_base = cdev->int_params.rdma_msix_base;
+
+		info->msix_cnt = cdev->int_params.rdma_msix_cnt;
+		info->msix = &cdev->int_params.msix_table[msix_base];
+
+		DP_VERBOSE(cdev, QED_MSG_RDMA, "msix_cnt = %d msix_base=%d\n",
+			   info->msix_cnt, msix_base);
+	}
+
+	return 0;
+}
+
+static int qed_rdma_alloc_pd(void *rdma_cxt, u16 *pd)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	u32 returned_id;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Alloc PD\n");
+
+	/* Allocates an unused protection domain */
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	rc = qed_rdma_bmap_alloc_id(p_hwfn,
+				    &p_hwfn->p_rdma_info->pd_map, &returned_id);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+
+	*pd = (u16)returned_id;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Alloc PD - done, rc = %d\n", rc);
+	return rc;
+}
+
+static void qed_rdma_free_pd(void *rdma_cxt, u16 pd)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "pd = %08x\n", pd);
+
+	/* Returns a previously allocated protection domain for reuse */
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->pd_map, pd);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+}
+
+static enum qed_rdma_toggle_bit
+qed_rdma_toggle_bit_create_resize_cq(struct qed_hwfn *p_hwfn, u16 icid)
+{
+	struct qed_rdma_info *p_info = p_hwfn->p_rdma_info;
+	enum qed_rdma_toggle_bit toggle_bit;
+	u32 bmap_id;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", icid);
+
+	/* the function toggle the bit that is related to a given icid
+	 * and returns the new toggle bit's value
+	 */
+	bmap_id = icid - qed_cxt_get_proto_cid_start(p_hwfn, p_info->proto);
+
+	spin_lock_bh(&p_info->lock);
+	toggle_bit = !test_and_change_bit(bmap_id,
+					  p_info->toggle_bits.bitmap);
+	spin_unlock_bh(&p_info->lock);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "QED_RDMA_TOGGLE_BIT_= %d\n",
+		   toggle_bit);
+
+	return toggle_bit;
+}
+
+static int qed_rdma_create_cq(void *rdma_cxt,
+			      struct qed_rdma_create_cq_in_params *params,
+			      u16 *icid)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct qed_rdma_info *p_info = p_hwfn->p_rdma_info;
+	struct rdma_create_cq_ramrod_data *p_ramrod;
+	enum qed_rdma_toggle_bit toggle_bit;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	u32 returned_id, start_cid;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "cq_handle = %08x%08x\n",
+		   params->cq_handle_hi, params->cq_handle_lo);
+
+	/* Allocate icid */
+	spin_lock_bh(&p_info->lock);
+	rc = qed_rdma_bmap_alloc_id(p_hwfn, &p_info->cq_map, &returned_id);
+	spin_unlock_bh(&p_info->lock);
+
+	if (rc) {
+		DP_NOTICE(p_hwfn, "Can't create CQ, rc = %d\n", rc);
+		return rc;
+	}
+
+	start_cid = qed_cxt_get_proto_cid_start(p_hwfn,
+						p_info->proto);
+	*icid = returned_id + start_cid;
+
+	/* Check if icid requires a page allocation */
+	rc = qed_cxt_dynamic_ilt_alloc(p_hwfn, QED_ELEM_CXT, *icid);
+	if (rc)
+		goto err;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = *icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	/* Send create CQ ramrod */
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 RDMA_RAMROD_CREATE_CQ,
+				 p_info->proto, &init_data);
+	if (rc)
+		goto err;
+
+	p_ramrod = &p_ent->ramrod.rdma_create_cq;
+
+	p_ramrod->cq_handle.hi = cpu_to_le32(params->cq_handle_hi);
+	p_ramrod->cq_handle.lo = cpu_to_le32(params->cq_handle_lo);
+	p_ramrod->dpi = cpu_to_le16(params->dpi);
+	p_ramrod->is_two_level_pbl = params->pbl_two_level;
+	p_ramrod->max_cqes = cpu_to_le32(params->cq_size);
+	DMA_REGPAIR_LE(p_ramrod->pbl_addr, params->pbl_ptr);
+	p_ramrod->pbl_num_pages = cpu_to_le16(params->pbl_num_pages);
+	p_ramrod->cnq_id = (u8)RESC_START(p_hwfn, QED_RDMA_CNQ_RAM) +
+			   params->cnq_id;
+	p_ramrod->int_timeout = params->int_timeout;
+
+	/* toggle the bit for every resize or create cq for a given icid */
+	toggle_bit = qed_rdma_toggle_bit_create_resize_cq(p_hwfn, *icid);
+
+	p_ramrod->toggle_bit = toggle_bit;
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+	if (rc) {
+		/* restore toggle bit */
+		qed_rdma_toggle_bit_create_resize_cq(p_hwfn, *icid);
+		goto err;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Created CQ, rc = %d\n", rc);
+	return rc;
+
+err:
+	/* release allocated icid */
+	spin_lock_bh(&p_info->lock);
+	qed_bmap_release_id(p_hwfn, &p_info->cq_map, returned_id);
+	spin_unlock_bh(&p_info->lock);
+	DP_NOTICE(p_hwfn, "Create CQ failed, rc = %d\n", rc);
+
+	return rc;
+}
+
+static int
+qed_rdma_destroy_cq(void *rdma_cxt,
+		    struct qed_rdma_destroy_cq_in_params *in_params,
+		    struct qed_rdma_destroy_cq_out_params *out_params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct rdma_destroy_cq_output_params *p_ramrod_res;
+	struct rdma_destroy_cq_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	dma_addr_t ramrod_res_phys;
+	enum protocol_type proto;
+	int rc = -ENOMEM;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", in_params->icid);
+
+	p_ramrod_res =
+	    (struct rdma_destroy_cq_output_params *)
+	    dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
+			       sizeof(struct rdma_destroy_cq_output_params),
+			       &ramrod_res_phys, GFP_KERNEL);
+	if (!p_ramrod_res) {
+		DP_NOTICE(p_hwfn,
+			  "qed destroy cq failed: cannot allocate memory (ramrod)\n");
+		return rc;
+	}
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = in_params->icid;
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+	proto = p_hwfn->p_rdma_info->proto;
+	/* Send destroy CQ ramrod */
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 RDMA_RAMROD_DESTROY_CQ,
+				 proto, &init_data);
+	if (rc)
+		goto err;
+
+	p_ramrod = &p_ent->ramrod.rdma_destroy_cq;
+	DMA_REGPAIR_LE(p_ramrod->output_params_addr, ramrod_res_phys);
+
+	rc = qed_spq_post(p_hwfn, p_ent, NULL);
+	if (rc)
+		goto err;
+
+	out_params->num_cq_notif = le16_to_cpu(p_ramrod_res->cnq_num);
+
+	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+			  sizeof(struct rdma_destroy_cq_output_params),
+			  p_ramrod_res, ramrod_res_phys);
+
+	/* Free icid */
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+
+	qed_bmap_release_id(p_hwfn,
+			    &p_hwfn->p_rdma_info->cq_map,
+			    (in_params->icid -
+			     qed_cxt_get_proto_cid_start(p_hwfn, proto)));
+
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Destroyed CQ, rc = %d\n", rc);
+	return rc;
+
+err:	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+			  sizeof(struct rdma_destroy_cq_output_params),
+			  p_ramrod_res, ramrod_res_phys);
+
+	return rc;
+}
+
+void qed_rdma_set_fw_mac(u16 *p_fw_mac, u8 *p_qed_mac)
+{
+	p_fw_mac[0] = cpu_to_le16((p_qed_mac[0] << 8) + p_qed_mac[1]);
+	p_fw_mac[1] = cpu_to_le16((p_qed_mac[2] << 8) + p_qed_mac[3]);
+	p_fw_mac[2] = cpu_to_le16((p_qed_mac[4] << 8) + p_qed_mac[5]);
+}
+
+static int qed_rdma_query_qp(void *rdma_cxt,
+			     struct qed_rdma_qp *qp,
+			     struct qed_rdma_query_qp_out_params *out_params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	int rc = 0;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", qp->icid);
+
+	/* The following fields are filled in from qp and not FW as they can't
+	 * be modified by FW
+	 */
+	out_params->mtu = qp->mtu;
+	out_params->dest_qp = qp->dest_qp;
+	out_params->incoming_atomic_en = qp->incoming_atomic_en;
+	out_params->e2e_flow_control_en = qp->e2e_flow_control_en;
+	out_params->incoming_rdma_read_en = qp->incoming_rdma_read_en;
+	out_params->incoming_rdma_write_en = qp->incoming_rdma_write_en;
+	out_params->dgid = qp->dgid;
+	out_params->flow_label = qp->flow_label;
+	out_params->hop_limit_ttl = qp->hop_limit_ttl;
+	out_params->traffic_class_tos = qp->traffic_class_tos;
+	out_params->timeout = qp->ack_timeout;
+	out_params->rnr_retry = qp->rnr_retry_cnt;
+	out_params->retry_cnt = qp->retry_cnt;
+	out_params->min_rnr_nak_timer = qp->min_rnr_nak_timer;
+	out_params->pkey_index = 0;
+	out_params->max_rd_atomic = qp->max_rd_atomic_req;
+	out_params->max_dest_rd_atomic = qp->max_rd_atomic_resp;
+	out_params->sqd_async = qp->sqd_async;
+
+	if (QED_IS_IWARP_PERSONALITY(p_hwfn))
+		qed_iwarp_query_qp(qp, out_params);
+	else
+		rc = qed_roce_query_qp(p_hwfn, qp, out_params);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Query QP, rc = %d\n", rc);
+	return rc;
+}
+
+static int qed_rdma_destroy_qp(void *rdma_cxt, struct qed_rdma_qp *qp)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	int rc = 0;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", qp->icid);
+
+	if (QED_IS_IWARP_PERSONALITY(p_hwfn))
+		rc = qed_iwarp_destroy_qp(p_hwfn, qp);
+	else
+		rc = qed_roce_destroy_qp(p_hwfn, qp);
+
+	/* free qp params struct */
+	kfree(qp);
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "QP destroyed\n");
+	return rc;
+}
+
+static struct qed_rdma_qp *
+qed_rdma_create_qp(void *rdma_cxt,
+		   struct qed_rdma_create_qp_in_params *in_params,
+		   struct qed_rdma_create_qp_out_params *out_params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct qed_rdma_qp *qp;
+	u8 max_stats_queues;
+	int rc;
+
+	if (!rdma_cxt || !in_params || !out_params || !p_hwfn->p_rdma_info) {
+		DP_ERR(p_hwfn->cdev,
+		       "qed roce create qp failed due to NULL entry (rdma_cxt=%p, in=%p, out=%p, roce_info=?\n",
+		       rdma_cxt, in_params, out_params);
+		return NULL;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+		   "qed rdma create qp called with qp_handle = %08x%08x\n",
+		   in_params->qp_handle_hi, in_params->qp_handle_lo);
+
+	/* Some sanity checks... */
+	max_stats_queues = p_hwfn->p_rdma_info->dev->max_stats_queues;
+	if (in_params->stats_queue >= max_stats_queues) {
+		DP_ERR(p_hwfn->cdev,
+		       "qed rdma create qp failed due to invalid statistics queue %d. maximum is %d\n",
+		       in_params->stats_queue, max_stats_queues);
+		return NULL;
+	}
+
+	if (QED_IS_IWARP_PERSONALITY(p_hwfn)) {
+		if (in_params->sq_num_pages * sizeof(struct regpair) >
+		    IWARP_SHARED_QUEUE_PAGE_SQ_PBL_MAX_SIZE) {
+			DP_NOTICE(p_hwfn->cdev,
+				  "Sq num pages: %d exceeds maximum\n",
+				  in_params->sq_num_pages);
+			return NULL;
+		}
+		if (in_params->rq_num_pages * sizeof(struct regpair) >
+		    IWARP_SHARED_QUEUE_PAGE_RQ_PBL_MAX_SIZE) {
+			DP_NOTICE(p_hwfn->cdev,
+				  "Rq num pages: %d exceeds maximum\n",
+				  in_params->rq_num_pages);
+			return NULL;
+		}
+	}
+
+	qp = kzalloc(sizeof(*qp), GFP_KERNEL);
+	if (!qp)
+		return NULL;
+
+	qp->cur_state = QED_ROCE_QP_STATE_RESET;
+	qp->qp_handle.hi = cpu_to_le32(in_params->qp_handle_hi);
+	qp->qp_handle.lo = cpu_to_le32(in_params->qp_handle_lo);
+	qp->qp_handle_async.hi = cpu_to_le32(in_params->qp_handle_async_hi);
+	qp->qp_handle_async.lo = cpu_to_le32(in_params->qp_handle_async_lo);
+	qp->use_srq = in_params->use_srq;
+	qp->signal_all = in_params->signal_all;
+	qp->fmr_and_reserved_lkey = in_params->fmr_and_reserved_lkey;
+	qp->pd = in_params->pd;
+	qp->dpi = in_params->dpi;
+	qp->sq_cq_id = in_params->sq_cq_id;
+	qp->sq_num_pages = in_params->sq_num_pages;
+	qp->sq_pbl_ptr = in_params->sq_pbl_ptr;
+	qp->rq_cq_id = in_params->rq_cq_id;
+	qp->rq_num_pages = in_params->rq_num_pages;
+	qp->rq_pbl_ptr = in_params->rq_pbl_ptr;
+	qp->srq_id = in_params->srq_id;
+	qp->req_offloaded = false;
+	qp->resp_offloaded = false;
+	qp->e2e_flow_control_en = qp->use_srq ? false : true;
+	qp->stats_queue = in_params->stats_queue;
+
+	if (QED_IS_IWARP_PERSONALITY(p_hwfn)) {
+		rc = qed_iwarp_create_qp(p_hwfn, qp, out_params);
+		qp->qpid = qp->icid;
+	} else {
+		rc = qed_roce_alloc_cid(p_hwfn, &qp->icid);
+		qp->qpid = ((0xFF << 16) | qp->icid);
+	}
+
+	if (rc) {
+		kfree(qp);
+		return NULL;
+	}
+
+	out_params->icid = qp->icid;
+	out_params->qp_id = qp->qpid;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Create QP, rc = %d\n", rc);
+	return qp;
+}
+
+static int qed_rdma_modify_qp(void *rdma_cxt,
+			      struct qed_rdma_qp *qp,
+			      struct qed_rdma_modify_qp_in_params *params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	enum qed_roce_qp_state prev_state;
+	int rc = 0;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x params->new_state=%d\n",
+		   qp->icid, params->new_state);
+
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
+		return rc;
+	}
+
+	if (GET_FIELD(params->modify_flags,
+		      QED_RDMA_MODIFY_QP_VALID_RDMA_OPS_EN)) {
+		qp->incoming_rdma_read_en = params->incoming_rdma_read_en;
+		qp->incoming_rdma_write_en = params->incoming_rdma_write_en;
+		qp->incoming_atomic_en = params->incoming_atomic_en;
+	}
+
+	/* Update QP structure with the updated values */
+	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_ROCE_MODE))
+		qp->roce_mode = params->roce_mode;
+	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_PKEY))
+		qp->pkey = params->pkey;
+	if (GET_FIELD(params->modify_flags,
+		      QED_ROCE_MODIFY_QP_VALID_E2E_FLOW_CONTROL_EN))
+		qp->e2e_flow_control_en = params->e2e_flow_control_en;
+	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_DEST_QP))
+		qp->dest_qp = params->dest_qp;
+	if (GET_FIELD(params->modify_flags,
+		      QED_ROCE_MODIFY_QP_VALID_ADDRESS_VECTOR)) {
+		/* Indicates that the following parameters have changed:
+		 * Traffic class, flow label, hop limit, source GID,
+		 * destination GID, loopback indicator
+		 */
+		qp->traffic_class_tos = params->traffic_class_tos;
+		qp->flow_label = params->flow_label;
+		qp->hop_limit_ttl = params->hop_limit_ttl;
+
+		qp->sgid = params->sgid;
+		qp->dgid = params->dgid;
+		qp->udp_src_port = 0;
+		qp->vlan_id = params->vlan_id;
+		qp->mtu = params->mtu;
+		qp->lb_indication = params->lb_indication;
+		memcpy((u8 *)&qp->remote_mac_addr[0],
+		       (u8 *)&params->remote_mac_addr[0], ETH_ALEN);
+		if (params->use_local_mac) {
+			memcpy((u8 *)&qp->local_mac_addr[0],
+			       (u8 *)&params->local_mac_addr[0], ETH_ALEN);
+		} else {
+			memcpy((u8 *)&qp->local_mac_addr[0],
+			       (u8 *)&p_hwfn->hw_info.hw_mac_addr, ETH_ALEN);
+		}
+	}
+	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_RQ_PSN))
+		qp->rq_psn = params->rq_psn;
+	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_SQ_PSN))
+		qp->sq_psn = params->sq_psn;
+	if (GET_FIELD(params->modify_flags,
+		      QED_RDMA_MODIFY_QP_VALID_MAX_RD_ATOMIC_REQ))
+		qp->max_rd_atomic_req = params->max_rd_atomic_req;
+	if (GET_FIELD(params->modify_flags,
+		      QED_RDMA_MODIFY_QP_VALID_MAX_RD_ATOMIC_RESP))
+		qp->max_rd_atomic_resp = params->max_rd_atomic_resp;
+	if (GET_FIELD(params->modify_flags,
+		      QED_ROCE_MODIFY_QP_VALID_ACK_TIMEOUT))
+		qp->ack_timeout = params->ack_timeout;
+	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_RETRY_CNT))
+		qp->retry_cnt = params->retry_cnt;
+	if (GET_FIELD(params->modify_flags,
+		      QED_ROCE_MODIFY_QP_VALID_RNR_RETRY_CNT))
+		qp->rnr_retry_cnt = params->rnr_retry_cnt;
+	if (GET_FIELD(params->modify_flags,
+		      QED_ROCE_MODIFY_QP_VALID_MIN_RNR_NAK_TIMER))
+		qp->min_rnr_nak_timer = params->min_rnr_nak_timer;
+
+	qp->sqd_async = params->sqd_async;
+
+	prev_state = qp->cur_state;
+	if (GET_FIELD(params->modify_flags,
+		      QED_RDMA_MODIFY_QP_VALID_NEW_STATE)) {
+		qp->cur_state = params->new_state;
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "qp->cur_state=%d\n",
+			   qp->cur_state);
+	}
+
+	if (QED_IS_IWARP_PERSONALITY(p_hwfn)) {
+		enum qed_iwarp_qp_state new_state =
+		    qed_roce2iwarp_state(qp->cur_state);
+
+		rc = qed_iwarp_modify_qp(p_hwfn, qp, new_state, 0);
+	} else {
+		rc = qed_roce_modify_qp(p_hwfn, qp, prev_state, params);
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Modify QP, rc = %d\n", rc);
+	return rc;
+}
+
+static int
+qed_rdma_register_tid(void *rdma_cxt,
+		      struct qed_rdma_register_tid_in_params *params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct rdma_register_tid_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	enum rdma_tid_type tid_type;
+	u8 fw_return_code;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "itid = %08x\n", params->itid);
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent, RDMA_RAMROD_REGISTER_MR,
+				 p_hwfn->p_rdma_info->proto, &init_data);
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
+		return rc;
+	}
+
+	if (p_hwfn->p_rdma_info->last_tid < params->itid)
+		p_hwfn->p_rdma_info->last_tid = params->itid;
+
+	p_ramrod = &p_ent->ramrod.rdma_register_tid;
+
+	p_ramrod->flags = 0;
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_TWO_LEVEL_PBL,
+		  params->pbl_two_level);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_ZERO_BASED, params->zbva);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_PHY_MR, params->phy_mr);
+
+	/* Don't initialize D/C field, as it may override other bits. */
+	if (!(params->tid_type == QED_RDMA_TID_FMR) && !(params->dma_mr))
+		SET_FIELD(p_ramrod->flags,
+			  RDMA_REGISTER_TID_RAMROD_DATA_PAGE_SIZE_LOG,
+			  params->page_size_log - 12);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_READ,
+		  params->remote_read);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_WRITE,
+		  params->remote_write);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_ATOMIC,
+		  params->remote_atomic);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_WRITE,
+		  params->local_write);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_READ, params->local_read);
+
+	SET_FIELD(p_ramrod->flags,
+		  RDMA_REGISTER_TID_RAMROD_DATA_ENABLE_MW_BIND,
+		  params->mw_bind);
+
+	SET_FIELD(p_ramrod->flags1,
+		  RDMA_REGISTER_TID_RAMROD_DATA_PBL_PAGE_SIZE_LOG,
+		  params->pbl_page_size_log - 12);
+
+	SET_FIELD(p_ramrod->flags2,
+		  RDMA_REGISTER_TID_RAMROD_DATA_DMA_MR, params->dma_mr);
+
+	switch (params->tid_type) {
+	case QED_RDMA_TID_REGISTERED_MR:
+		tid_type = RDMA_TID_REGISTERED_MR;
+		break;
+	case QED_RDMA_TID_FMR:
+		tid_type = RDMA_TID_FMR;
+		break;
+	case QED_RDMA_TID_MW_TYPE1:
+		tid_type = RDMA_TID_MW_TYPE1;
+		break;
+	case QED_RDMA_TID_MW_TYPE2A:
+		tid_type = RDMA_TID_MW_TYPE2A;
+		break;
+	default:
+		rc = -EINVAL;
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
+		return rc;
+	}
+	SET_FIELD(p_ramrod->flags1,
+		  RDMA_REGISTER_TID_RAMROD_DATA_TID_TYPE, tid_type);
+
+	p_ramrod->itid = cpu_to_le32(params->itid);
+	p_ramrod->key = params->key;
+	p_ramrod->pd = cpu_to_le16(params->pd);
+	p_ramrod->length_hi = (u8)(params->length >> 32);
+	p_ramrod->length_lo = DMA_LO_LE(params->length);
+	if (params->zbva) {
+		/* Lower 32 bits of the registered MR address.
+		 * In case of zero based MR, will hold FBO
+		 */
+		p_ramrod->va.hi = 0;
+		p_ramrod->va.lo = cpu_to_le32(params->fbo);
+	} else {
+		DMA_REGPAIR_LE(p_ramrod->va, params->vaddr);
+	}
+	DMA_REGPAIR_LE(p_ramrod->pbl_base, params->pbl_ptr);
+
+	/* DIF */
+	if (params->dif_enabled) {
+		SET_FIELD(p_ramrod->flags2,
+			  RDMA_REGISTER_TID_RAMROD_DATA_DIF_ON_HOST_FLG, 1);
+		DMA_REGPAIR_LE(p_ramrod->dif_error_addr,
+			       params->dif_error_addr);
+		DMA_REGPAIR_LE(p_ramrod->dif_runt_addr, params->dif_runt_addr);
+	}
+
+	rc = qed_spq_post(p_hwfn, p_ent, &fw_return_code);
+	if (rc)
+		return rc;
+
+	if (fw_return_code != RDMA_RETURN_OK) {
+		DP_NOTICE(p_hwfn, "fw_return_code = %d\n", fw_return_code);
+		return -EINVAL;
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Register TID, rc = %d\n", rc);
+	return rc;
+}
+
+static int qed_rdma_deregister_tid(void *rdma_cxt, u32 itid)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct rdma_deregister_tid_ramrod_data *p_ramrod;
+	struct qed_sp_init_data init_data;
+	struct qed_spq_entry *p_ent;
+	struct qed_ptt *p_ptt;
+	u8 fw_return_code;
+	int rc;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "itid = %08x\n", itid);
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent, RDMA_RAMROD_DEREGISTER_MR,
+				 p_hwfn->p_rdma_info->proto, &init_data);
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
+		return rc;
+	}
+
+	p_ramrod = &p_ent->ramrod.rdma_deregister_tid;
+	p_ramrod->itid = cpu_to_le32(itid);
+
+	rc = qed_spq_post(p_hwfn, p_ent, &fw_return_code);
+	if (rc) {
+		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
+		return rc;
+	}
+
+	if (fw_return_code == RDMA_RETURN_DEREGISTER_MR_BAD_STATE_ERR) {
+		DP_NOTICE(p_hwfn, "fw_return_code = %d\n", fw_return_code);
+		return -EINVAL;
+	} else if (fw_return_code == RDMA_RETURN_NIG_DRAIN_REQ) {
+		/* Bit indicating that the TID is in use and a nig drain is
+		 * required before sending the ramrod again
+		 */
+		p_ptt = qed_ptt_acquire(p_hwfn);
+		if (!p_ptt) {
+			rc = -EBUSY;
+			DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+				   "Failed to acquire PTT\n");
+			return rc;
+		}
+
+		rc = qed_mcp_drain(p_hwfn, p_ptt);
+		if (rc) {
+			qed_ptt_release(p_hwfn, p_ptt);
+			DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+				   "Drain failed\n");
+			return rc;
+		}
+
+		qed_ptt_release(p_hwfn, p_ptt);
+
+		/* Resend the ramrod */
+		rc = qed_sp_init_request(p_hwfn, &p_ent,
+					 RDMA_RAMROD_DEREGISTER_MR,
+					 p_hwfn->p_rdma_info->proto,
+					 &init_data);
+		if (rc) {
+			DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+				   "Failed to init sp-element\n");
+			return rc;
+		}
+
+		rc = qed_spq_post(p_hwfn, p_ent, &fw_return_code);
+		if (rc) {
+			DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+				   "Ramrod failed\n");
+			return rc;
+		}
+
+		if (fw_return_code != RDMA_RETURN_OK) {
+			DP_NOTICE(p_hwfn, "fw_return_code = %d\n",
+				  fw_return_code);
+			return rc;
+		}
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "De-registered TID, rc = %d\n", rc);
+	return rc;
+}
+
+static void *qed_rdma_get_rdma_ctx(struct qed_dev *cdev)
+{
+	return QED_LEADING_HWFN(cdev);
+}
+
+bool qed_rdma_allocated_qps(struct qed_hwfn *p_hwfn)
+{
+	bool result;
+
+	/* if rdma info has not been allocated, naturally there are no qps */
+	if (!p_hwfn->p_rdma_info)
+		return false;
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	if (!p_hwfn->p_rdma_info->cid_map.bitmap)
+		result = false;
+	else
+		result = !qed_bmap_is_empty(&p_hwfn->p_rdma_info->cid_map);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+	return result;
+}
+
+void qed_rdma_dpm_conf(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+{
+	u32 val;
+
+	val = (p_hwfn->dcbx_no_edpm || p_hwfn->db_bar_no_edpm) ? 0 : 1;
+
+	qed_wr(p_hwfn, p_ptt, DORQ_REG_PF_DPM_ENABLE, val);
+	DP_VERBOSE(p_hwfn, (QED_MSG_DCB | QED_MSG_RDMA),
+		   "Changing DPM_EN state to %d (DCBX=%d, DB_BAR=%d)\n",
+		   val, p_hwfn->dcbx_no_edpm, p_hwfn->db_bar_no_edpm);
+}
+
+
+void qed_rdma_dpm_bar(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+{
+	p_hwfn->db_bar_no_edpm = true;
+
+	qed_rdma_dpm_conf(p_hwfn, p_ptt);
+}
+
+static int qed_rdma_start(void *rdma_cxt,
+			  struct qed_rdma_start_in_params *params)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	struct qed_ptt *p_ptt;
+	int rc = -EBUSY;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
+		   "desired_cnq = %08x\n", params->desired_cnq);
+
+	p_ptt = qed_ptt_acquire(p_hwfn);
+	if (!p_ptt)
+		goto err;
+
+	rc = qed_rdma_alloc(p_hwfn, p_ptt, params);
+	if (rc)
+		goto err1;
+
+	rc = qed_rdma_setup(p_hwfn, p_ptt, params);
+	if (rc)
+		goto err2;
+
+	qed_ptt_release(p_hwfn, p_ptt);
+
+	return rc;
+
+err2:
+	qed_rdma_free(p_hwfn);
+err1:
+	qed_ptt_release(p_hwfn, p_ptt);
+err:
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA start - error, rc = %d\n", rc);
+	return rc;
+}
+
+static int qed_rdma_init(struct qed_dev *cdev,
+			 struct qed_rdma_start_in_params *params)
+{
+	return qed_rdma_start(QED_LEADING_HWFN(cdev), params);
+}
+
+static void qed_rdma_remove_user(void *rdma_cxt, u16 dpi)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "dpi = %08x\n", dpi);
+
+	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
+	qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->dpi_map, dpi);
+	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
+}
+
+static int qed_roce_ll2_set_mac_filter(struct qed_dev *cdev,
+				       u8 *old_mac_address,
+				       u8 *new_mac_address)
+{
+	struct qed_hwfn *p_hwfn = QED_LEADING_HWFN(cdev);
+	struct qed_ptt *p_ptt;
+	int rc = 0;
+
+	p_ptt = qed_ptt_acquire(p_hwfn);
+	if (!p_ptt) {
+		DP_ERR(cdev,
+		       "qed roce ll2 mac filter set: failed to acquire PTT\n");
+		return -EINVAL;
+	}
+
+	if (old_mac_address)
+		qed_llh_remove_mac_filter(p_hwfn, p_ptt, old_mac_address);
+	if (new_mac_address)
+		rc = qed_llh_add_mac_filter(p_hwfn, p_ptt, new_mac_address);
+
+	qed_ptt_release(p_hwfn, p_ptt);
+
+	if (rc)
+		DP_ERR(cdev,
+		       "qed roce ll2 mac filter set: failed to add MAC filter\n");
+
+	return rc;
+}
+
+static const struct qed_rdma_ops qed_rdma_ops_pass = {
+	.common = &qed_common_ops_pass,
+	.fill_dev_info = &qed_fill_rdma_dev_info,
+	.rdma_get_rdma_ctx = &qed_rdma_get_rdma_ctx,
+	.rdma_init = &qed_rdma_init,
+	.rdma_add_user = &qed_rdma_add_user,
+	.rdma_remove_user = &qed_rdma_remove_user,
+	.rdma_stop = &qed_rdma_stop,
+	.rdma_query_port = &qed_rdma_query_port,
+	.rdma_query_device = &qed_rdma_query_device,
+	.rdma_get_start_sb = &qed_rdma_get_sb_start,
+	.rdma_get_rdma_int = &qed_rdma_get_int,
+	.rdma_set_rdma_int = &qed_rdma_set_int,
+	.rdma_get_min_cnq_msix = &qed_rdma_get_min_cnq_msix,
+	.rdma_cnq_prod_update = &qed_rdma_cnq_prod_update,
+	.rdma_alloc_pd = &qed_rdma_alloc_pd,
+	.rdma_dealloc_pd = &qed_rdma_free_pd,
+	.rdma_create_cq = &qed_rdma_create_cq,
+	.rdma_destroy_cq = &qed_rdma_destroy_cq,
+	.rdma_create_qp = &qed_rdma_create_qp,
+	.rdma_modify_qp = &qed_rdma_modify_qp,
+	.rdma_query_qp = &qed_rdma_query_qp,
+	.rdma_destroy_qp = &qed_rdma_destroy_qp,
+	.rdma_alloc_tid = &qed_rdma_alloc_tid,
+	.rdma_free_tid = &qed_rdma_free_tid,
+	.rdma_register_tid = &qed_rdma_register_tid,
+	.rdma_deregister_tid = &qed_rdma_deregister_tid,
+	.ll2_acquire_connection = &qed_ll2_acquire_connection,
+	.ll2_establish_connection = &qed_ll2_establish_connection,
+	.ll2_terminate_connection = &qed_ll2_terminate_connection,
+	.ll2_release_connection = &qed_ll2_release_connection,
+	.ll2_post_rx_buffer = &qed_ll2_post_rx_buffer,
+	.ll2_prepare_tx_packet = &qed_ll2_prepare_tx_packet,
+	.ll2_set_fragment_of_tx_packet = &qed_ll2_set_fragment_of_tx_packet,
+	.ll2_set_mac_filter = &qed_roce_ll2_set_mac_filter,
+	.ll2_get_stats = &qed_ll2_get_stats,
+	.iwarp_connect = &qed_iwarp_connect,
+	.iwarp_create_listen = &qed_iwarp_create_listen,
+	.iwarp_destroy_listen = &qed_iwarp_destroy_listen,
+	.iwarp_accept = &qed_iwarp_accept,
+	.iwarp_reject = &qed_iwarp_reject,
+	.iwarp_send_rtr = &qed_iwarp_send_rtr,
+};
+
+const struct qed_rdma_ops *qed_get_rdma_ops(void)
+{
+	return &qed_rdma_ops_pass;
+}
+EXPORT_SYMBOL(qed_get_rdma_ops);
diff --git a/drivers/net/ethernet/qlogic/qed/qed_rdma.h b/drivers/net/ethernet/qlogic/qed/qed_rdma.h
new file mode 100644
index 000000000000..18ec9cbd84f5
--- /dev/null
+++ b/drivers/net/ethernet/qlogic/qed/qed_rdma.h
@@ -0,0 +1,206 @@
+/* QLogic qed NIC Driver
+ * Copyright (c) 2015-2017  QLogic Corporation
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and /or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+#ifndef _QED_RDMA_H
+#define _QED_RDMA_H
+#include <linux/types.h>
+#include <linux/bitops.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/qed/qed_if.h>
+#include <linux/qed/qed_rdma_if.h>
+#include "qed.h"
+#include "qed_dev_api.h"
+#include "qed_hsi.h"
+#include "qed_iwarp.h"
+#include "qed_roce.h"
+
+#define QED_RDMA_MAX_FMR                    (RDMA_MAX_TIDS)
+#define QED_RDMA_MAX_P_KEY                  (1)
+#define QED_RDMA_MAX_WQE                    (0x7FFF)
+#define QED_RDMA_MAX_SRQ_WQE_ELEM           (0x7FFF)
+#define QED_RDMA_PAGE_SIZE_CAPS             (0xFFFFF000)
+#define QED_RDMA_ACK_DELAY                  (15)
+#define QED_RDMA_MAX_MR_SIZE                (0x10000000000ULL)
+#define QED_RDMA_MAX_CQS                    (RDMA_MAX_CQS)
+#define QED_RDMA_MAX_MRS                    (RDMA_MAX_TIDS)
+/* Add 1 for header element */
+#define QED_RDMA_MAX_SRQ_ELEM_PER_WQE	    (RDMA_MAX_SGE_PER_RQ_WQE + 1)
+#define QED_RDMA_MAX_SGE_PER_SRQ_WQE        (RDMA_MAX_SGE_PER_RQ_WQE)
+#define QED_RDMA_SRQ_WQE_ELEM_SIZE          (16)
+#define QED_RDMA_MAX_SRQS                   (32 * 1024)
+
+#define QED_RDMA_MAX_CQE_32_BIT             (0x7FFFFFFF - 1)
+#define QED_RDMA_MAX_CQE_16_BIT             (0x7FFF - 1)
+
+enum qed_rdma_toggle_bit {
+	QED_RDMA_TOGGLE_BIT_CLEAR = 0,
+	QED_RDMA_TOGGLE_BIT_SET = 1
+};
+
+#define QED_RDMA_MAX_BMAP_NAME	(10)
+struct qed_bmap {
+	unsigned long *bitmap;
+	u32 max_count;
+	char name[QED_RDMA_MAX_BMAP_NAME];
+};
+
+struct qed_rdma_info {
+	/* spin lock to protect bitmaps */
+	spinlock_t lock;
+
+	struct qed_bmap cq_map;
+	struct qed_bmap pd_map;
+	struct qed_bmap tid_map;
+	struct qed_bmap qp_map;
+	struct qed_bmap srq_map;
+	struct qed_bmap cid_map;
+	struct qed_bmap tcp_cid_map;
+	struct qed_bmap real_cid_map;
+	struct qed_bmap dpi_map;
+	struct qed_bmap toggle_bits;
+	struct qed_rdma_events events;
+	struct qed_rdma_device *dev;
+	struct qed_rdma_port *port;
+	u32 last_tid;
+	u8 num_cnqs;
+	u32 num_qps;
+	u32 num_mrs;
+	u16 queue_zone_base;
+	u16 max_queue_zones;
+	enum protocol_type proto;
+	struct qed_iwarp_info iwarp;
+};
+
+struct qed_rdma_qp {
+	struct regpair qp_handle;
+	struct regpair qp_handle_async;
+	u32 qpid;
+	u16 icid;
+	enum qed_roce_qp_state cur_state;
+	enum qed_iwarp_qp_state iwarp_state;
+	bool use_srq;
+	bool signal_all;
+	bool fmr_and_reserved_lkey;
+
+	bool incoming_rdma_read_en;
+	bool incoming_rdma_write_en;
+	bool incoming_atomic_en;
+	bool e2e_flow_control_en;
+
+	u16 pd;
+	u16 pkey;
+	u32 dest_qp;
+	u16 mtu;
+	u16 srq_id;
+	u8 traffic_class_tos;
+	u8 hop_limit_ttl;
+	u16 dpi;
+	u32 flow_label;
+	bool lb_indication;
+	u16 vlan_id;
+	u32 ack_timeout;
+	u8 retry_cnt;
+	u8 rnr_retry_cnt;
+	u8 min_rnr_nak_timer;
+	bool sqd_async;
+	union qed_gid sgid;
+	union qed_gid dgid;
+	enum roce_mode roce_mode;
+	u16 udp_src_port;
+	u8 stats_queue;
+
+	/* requeseter */
+	u8 max_rd_atomic_req;
+	u32 sq_psn;
+	u16 sq_cq_id;
+	u16 sq_num_pages;
+	dma_addr_t sq_pbl_ptr;
+	void *orq;
+	dma_addr_t orq_phys_addr;
+	u8 orq_num_pages;
+	bool req_offloaded;
+
+	/* responder */
+	u8 max_rd_atomic_resp;
+	u32 rq_psn;
+	u16 rq_cq_id;
+	u16 rq_num_pages;
+	dma_addr_t rq_pbl_ptr;
+	void *irq;
+	dma_addr_t irq_phys_addr;
+	u8 irq_num_pages;
+	bool resp_offloaded;
+	u32 cq_prod;
+
+	u8 remote_mac_addr[6];
+	u8 local_mac_addr[6];
+
+	void *shared_queue;
+	dma_addr_t shared_queue_phys_addr;
+	struct qed_iwarp_ep *ep;
+};
+
+#if IS_ENABLED(CONFIG_QED_RDMA)
+void qed_rdma_dpm_bar(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
+void qed_rdma_dpm_conf(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
+#else
+static inline void qed_rdma_dpm_conf(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt) {}
+static inline void qed_rdma_dpm_bar(struct qed_hwfn *p_hwfn,
+				    struct qed_ptt *p_ptt) {}
+#endif
+
+int
+qed_rdma_bmap_alloc(struct qed_hwfn *p_hwfn,
+		    struct qed_bmap *bmap, u32 max_count, char *name);
+
+void
+qed_rdma_bmap_free(struct qed_hwfn *p_hwfn, struct qed_bmap *bmap, bool check);
+
+int
+qed_rdma_bmap_alloc_id(struct qed_hwfn *p_hwfn,
+		       struct qed_bmap *bmap, u32 *id_num);
+
+void
+qed_bmap_set_id(struct qed_hwfn *p_hwfn, struct qed_bmap *bmap, u32 id_num);
+
+void
+qed_bmap_release_id(struct qed_hwfn *p_hwfn, struct qed_bmap *bmap, u32 id_num);
+
+int
+qed_bmap_test_id(struct qed_hwfn *p_hwfn, struct qed_bmap *bmap, u32 id_num);
+
+void qed_rdma_set_fw_mac(u16 *p_fw_mac, u8 *p_qed_mac);
+
+bool qed_rdma_allocated_qps(struct qed_hwfn *p_hwfn);
+#endif
diff --git a/drivers/net/ethernet/qlogic/qed/qed_reg_addr.h b/drivers/net/ethernet/qlogic/qed/qed_reg_addr.h
index 1ae73b2d6d1e..0cdb4337b3a0 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_reg_addr.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_reg_addr.h
@@ -242,6 +242,8 @@
 	0x50196cUL
 #define NIG_REG_LLH_CLS_TYPE_DUALMODE \
 	0x501964UL
+#define NIG_REG_LLH_FUNC_TAG_EN 0x5019b0UL
+#define NIG_REG_LLH_FUNC_TAG_VALUE 0x5019d0UL
 #define NIG_REG_LLH_FUNC_FILTER_VALUE \
 	0x501a00UL
 #define NIG_REG_LLH_FUNC_FILTER_VALUE_SIZE \
@@ -558,6 +560,7 @@
 	0x2aae60UL
 #define PGLUE_B_REG_PF_BAR1_SIZE \
 	0x2aae64UL
+#define PGLUE_B_REG_VF_BAR1_SIZE 0x2aae68UL
 #define PRS_REG_ENCAPSULATION_TYPE_EN	0x1f0730UL
 #define PRS_REG_GRE_PROTOCOL		0x1f0734UL
 #define PRS_REG_VXLAN_PORT		0x1f0738UL
@@ -592,15 +595,15 @@
 #define QM_REG_WFQPFWEIGHT	0x2f4e80UL
 #define QM_REG_WFQVPWEIGHT	0x2fa000UL
 
-#define PGLCS_REG_DBG_SELECT \
+#define PGLCS_REG_DBG_SELECT_K2 \
 	0x001d14UL
-#define PGLCS_REG_DBG_DWORD_ENABLE \
+#define PGLCS_REG_DBG_DWORD_ENABLE_K2 \
 	0x001d18UL
-#define PGLCS_REG_DBG_SHIFT \
+#define PGLCS_REG_DBG_SHIFT_K2 \
 	0x001d1cUL
-#define PGLCS_REG_DBG_FORCE_VALID \
+#define PGLCS_REG_DBG_FORCE_VALID_K2 \
 	0x001d20UL
-#define PGLCS_REG_DBG_FORCE_FRAME \
+#define PGLCS_REG_DBG_FORCE_FRAME_K2 \
 	0x001d24UL
 #define MISC_REG_RESET_PL_PDA_VMAIN_1 \
 	0x008070UL
@@ -612,7 +615,7 @@
 	0x009050UL
 #define MISCS_REG_RESET_PL_HV \
 	0x009060UL
-#define MISCS_REG_RESET_PL_HV_2	\
+#define MISCS_REG_RESET_PL_HV_2_K2	\
 	0x009150UL
 #define DMAE_REG_DBG_SELECT \
 	0x00c510UL
@@ -644,15 +647,15 @@
 	0x0500b0UL
 #define GRC_REG_DBG_FORCE_FRAME	\
 	0x0500b4UL
-#define UMAC_REG_DBG_SELECT \
+#define UMAC_REG_DBG_SELECT_K2 \
 	0x051094UL
-#define UMAC_REG_DBG_DWORD_ENABLE \
+#define UMAC_REG_DBG_DWORD_ENABLE_K2 \
 	0x051098UL
-#define UMAC_REG_DBG_SHIFT \
+#define UMAC_REG_DBG_SHIFT_K2 \
 	0x05109cUL
-#define UMAC_REG_DBG_FORCE_VALID \
+#define UMAC_REG_DBG_FORCE_VALID_K2 \
 	0x0510a0UL
-#define UMAC_REG_DBG_FORCE_FRAME \
+#define UMAC_REG_DBG_FORCE_FRAME_K2 \
 	0x0510a4UL
 #define MCP2_REG_DBG_SELECT \
 	0x052400UL
@@ -924,15 +927,15 @@
 	0x4c160cUL
 #define XYLD_REG_DBG_FORCE_FRAME \
 	0x4c1610UL
-#define YULD_REG_DBG_SELECT \
+#define YULD_REG_DBG_SELECT_BB_K2 \
 	0x4c9600UL
-#define YULD_REG_DBG_DWORD_ENABLE \
+#define YULD_REG_DBG_DWORD_ENABLE_BB_K2 \
 	0x4c9604UL
-#define YULD_REG_DBG_SHIFT \
+#define YULD_REG_DBG_SHIFT_BB_K2 \
 	0x4c9608UL
-#define YULD_REG_DBG_FORCE_VALID \
+#define YULD_REG_DBG_FORCE_VALID_BB_K2 \
 	0x4c960cUL
-#define YULD_REG_DBG_FORCE_FRAME \
+#define YULD_REG_DBG_FORCE_FRAME_BB_K2 \
 	0x4c9610UL
 #define TMLD_REG_DBG_SELECT \
 	0x4d1600UL
@@ -994,35 +997,35 @@
 	0x580710UL
 #define CDU_REG_DBG_FORCE_FRAME	\
 	0x580714UL
-#define WOL_REG_DBG_SELECT \
+#define WOL_REG_DBG_SELECT_K2 \
 	0x600140UL
-#define WOL_REG_DBG_DWORD_ENABLE \
+#define WOL_REG_DBG_DWORD_ENABLE_K2 \
 	0x600144UL
-#define WOL_REG_DBG_SHIFT \
+#define WOL_REG_DBG_SHIFT_K2 \
 	0x600148UL
-#define WOL_REG_DBG_FORCE_VALID	\
+#define WOL_REG_DBG_FORCE_VALID_K2 \
 	0x60014cUL
-#define WOL_REG_DBG_FORCE_FRAME	\
+#define WOL_REG_DBG_FORCE_FRAME_K2 \
 	0x600150UL
-#define BMBN_REG_DBG_SELECT \
+#define BMBN_REG_DBG_SELECT_K2 \
 	0x610140UL
-#define BMBN_REG_DBG_DWORD_ENABLE \
+#define BMBN_REG_DBG_DWORD_ENABLE_K2 \
 	0x610144UL
-#define BMBN_REG_DBG_SHIFT \
+#define BMBN_REG_DBG_SHIFT_K2 \
 	0x610148UL
-#define BMBN_REG_DBG_FORCE_VALID \
+#define BMBN_REG_DBG_FORCE_VALID_K2 \
 	0x61014cUL
-#define BMBN_REG_DBG_FORCE_FRAME \
+#define BMBN_REG_DBG_FORCE_FRAME_K2 \
 	0x610150UL
-#define NWM_REG_DBG_SELECT \
+#define NWM_REG_DBG_SELECT_K2 \
 	0x8000ecUL
-#define NWM_REG_DBG_DWORD_ENABLE \
+#define NWM_REG_DBG_DWORD_ENABLE_K2 \
 	0x8000f0UL
-#define NWM_REG_DBG_SHIFT \
+#define NWM_REG_DBG_SHIFT_K2 \
 	0x8000f4UL
-#define NWM_REG_DBG_FORCE_VALID	\
+#define NWM_REG_DBG_FORCE_VALID_K2 \
 	0x8000f8UL
-#define NWM_REG_DBG_FORCE_FRAME	\
+#define NWM_REG_DBG_FORCE_FRAME_K2\
 	0x8000fcUL
 #define PBF_REG_DBG_SELECT \
 	0xd80060UL
@@ -1244,35 +1247,35 @@
 	0x1901534UL
 #define USEM_REG_DBG_FORCE_FRAME \
 	0x1901538UL
-#define NWS_REG_DBG_SELECT \
+#define NWS_REG_DBG_SELECT_K2 \
 	0x700128UL
-#define NWS_REG_DBG_DWORD_ENABLE \
+#define NWS_REG_DBG_DWORD_ENABLE_K2 \
 	0x70012cUL
-#define NWS_REG_DBG_SHIFT \
+#define NWS_REG_DBG_SHIFT_K2 \
 	0x700130UL
-#define NWS_REG_DBG_FORCE_VALID	\
+#define NWS_REG_DBG_FORCE_VALID_K2 \
 	0x700134UL
-#define NWS_REG_DBG_FORCE_FRAME	\
+#define NWS_REG_DBG_FORCE_FRAME_K2 \
 	0x700138UL
-#define MS_REG_DBG_SELECT \
+#define MS_REG_DBG_SELECT_K2 \
 	0x6a0228UL
-#define MS_REG_DBG_DWORD_ENABLE \
+#define MS_REG_DBG_DWORD_ENABLE_K2 \
 	0x6a022cUL
-#define MS_REG_DBG_SHIFT \
+#define MS_REG_DBG_SHIFT_K2 \
 	0x6a0230UL
-#define MS_REG_DBG_FORCE_VALID \
+#define MS_REG_DBG_FORCE_VALID_K2 \
 	0x6a0234UL
-#define MS_REG_DBG_FORCE_FRAME \
+#define MS_REG_DBG_FORCE_FRAME_K2 \
 	0x6a0238UL
-#define PCIE_REG_DBG_COMMON_SELECT \
+#define PCIE_REG_DBG_COMMON_SELECT_K2 \
 	0x054398UL
-#define PCIE_REG_DBG_COMMON_DWORD_ENABLE \
+#define PCIE_REG_DBG_COMMON_DWORD_ENABLE_K2 \
 	0x05439cUL
-#define PCIE_REG_DBG_COMMON_SHIFT \
+#define PCIE_REG_DBG_COMMON_SHIFT_K2 \
 	0x0543a0UL
-#define PCIE_REG_DBG_COMMON_FORCE_VALID	\
+#define PCIE_REG_DBG_COMMON_FORCE_VALID_K2 \
 	0x0543a4UL
-#define PCIE_REG_DBG_COMMON_FORCE_FRAME	\
+#define PCIE_REG_DBG_COMMON_FORCE_FRAME_K2 \
 	0x0543a8UL
 #define MISC_REG_RESET_PL_UA \
 	0x008050UL
@@ -1328,85 +1331,85 @@
 	0x128170cUL
 #define UCM_REG_SM_TASK_CTX \
 	0x1281710UL
-#define XSEM_REG_SLOW_DBG_EMPTY	\
+#define XSEM_REG_SLOW_DBG_EMPTY_BB_K2	\
 	0x1401140UL
 #define XSEM_REG_SYNC_DBG_EMPTY	\
 	0x1401160UL
-#define XSEM_REG_SLOW_DBG_ACTIVE \
+#define XSEM_REG_SLOW_DBG_ACTIVE_BB_K2 \
 	0x1401400UL
-#define XSEM_REG_SLOW_DBG_MODE \
+#define XSEM_REG_SLOW_DBG_MODE_BB_K2 \
 	0x1401404UL
-#define XSEM_REG_DBG_FRAME_MODE	\
+#define XSEM_REG_DBG_FRAME_MODE_BB_K2	\
 	0x1401408UL
-#define XSEM_REG_DBG_MODE1_CFG \
+#define XSEM_REG_DBG_MODE1_CFG_BB_K2 \
 	0x1401420UL
 #define XSEM_REG_FAST_MEMORY \
 	0x1440000UL
 #define YSEM_REG_SYNC_DBG_EMPTY	\
 	0x1501160UL
-#define YSEM_REG_SLOW_DBG_ACTIVE \
+#define YSEM_REG_SLOW_DBG_ACTIVE_BB_K2 \
 	0x1501400UL
-#define YSEM_REG_SLOW_DBG_MODE \
+#define YSEM_REG_SLOW_DBG_MODE_BB_K2 \
 	0x1501404UL
-#define YSEM_REG_DBG_FRAME_MODE	\
+#define YSEM_REG_DBG_FRAME_MODE_BB_K2	\
 	0x1501408UL
-#define YSEM_REG_DBG_MODE1_CFG \
+#define YSEM_REG_DBG_MODE1_CFG_BB_K2 \
 	0x1501420UL
 #define YSEM_REG_FAST_MEMORY \
 	0x1540000UL
-#define PSEM_REG_SLOW_DBG_EMPTY	\
+#define PSEM_REG_SLOW_DBG_EMPTY_BB_K2	\
 	0x1601140UL
 #define PSEM_REG_SYNC_DBG_EMPTY	\
 	0x1601160UL
-#define PSEM_REG_SLOW_DBG_ACTIVE \
+#define PSEM_REG_SLOW_DBG_ACTIVE_BB_K2 \
 	0x1601400UL
-#define PSEM_REG_SLOW_DBG_MODE \
+#define PSEM_REG_SLOW_DBG_MODE_BB_K2 \
 	0x1601404UL
-#define PSEM_REG_DBG_FRAME_MODE	\
+#define PSEM_REG_DBG_FRAME_MODE_BB_K2	\
 	0x1601408UL
-#define PSEM_REG_DBG_MODE1_CFG \
+#define PSEM_REG_DBG_MODE1_CFG_BB_K2 \
 	0x1601420UL
 #define PSEM_REG_FAST_MEMORY \
 	0x1640000UL
-#define TSEM_REG_SLOW_DBG_EMPTY	\
+#define TSEM_REG_SLOW_DBG_EMPTY_BB_K2	\
 	0x1701140UL
 #define TSEM_REG_SYNC_DBG_EMPTY	\
 	0x1701160UL
-#define TSEM_REG_SLOW_DBG_ACTIVE \
+#define TSEM_REG_SLOW_DBG_ACTIVE_BB_K2 \
 	0x1701400UL
-#define TSEM_REG_SLOW_DBG_MODE \
+#define TSEM_REG_SLOW_DBG_MODE_BB_K2 \
 	0x1701404UL
-#define TSEM_REG_DBG_FRAME_MODE	\
+#define TSEM_REG_DBG_FRAME_MODE_BB_K2	\
 	0x1701408UL
-#define TSEM_REG_DBG_MODE1_CFG \
+#define TSEM_REG_DBG_MODE1_CFG_BB_K2 \
 	0x1701420UL
 #define TSEM_REG_FAST_MEMORY \
 	0x1740000UL
-#define MSEM_REG_SLOW_DBG_EMPTY	\
+#define MSEM_REG_SLOW_DBG_EMPTY_BB_K2	\
 	0x1801140UL
 #define MSEM_REG_SYNC_DBG_EMPTY	\
 	0x1801160UL
-#define MSEM_REG_SLOW_DBG_ACTIVE \
+#define MSEM_REG_SLOW_DBG_ACTIVE_BB_K2 \
 	0x1801400UL
-#define MSEM_REG_SLOW_DBG_MODE \
+#define MSEM_REG_SLOW_DBG_MODE_BB_K2 \
 	0x1801404UL
-#define MSEM_REG_DBG_FRAME_MODE	\
+#define MSEM_REG_DBG_FRAME_MODE_BB_K2	\
 	0x1801408UL
-#define MSEM_REG_DBG_MODE1_CFG \
+#define MSEM_REG_DBG_MODE1_CFG_BB_K2 \
 	0x1801420UL
 #define MSEM_REG_FAST_MEMORY \
 	0x1840000UL
-#define USEM_REG_SLOW_DBG_EMPTY	\
+#define USEM_REG_SLOW_DBG_EMPTY_BB_K2	\
 	0x1901140UL
 #define USEM_REG_SYNC_DBG_EMPTY	\
 	0x1901160UL
-#define USEM_REG_SLOW_DBG_ACTIVE \
+#define USEM_REG_SLOW_DBG_ACTIVE_BB_K2 \
 	0x1901400UL
-#define USEM_REG_SLOW_DBG_MODE \
+#define USEM_REG_SLOW_DBG_MODE_BB_K2 \
 	0x1901404UL
-#define USEM_REG_DBG_FRAME_MODE	\
+#define USEM_REG_DBG_FRAME_MODE_BB_K2	\
 	0x1901408UL
-#define USEM_REG_DBG_MODE1_CFG \
+#define USEM_REG_DBG_MODE1_CFG_BB_K2 \
 	0x1901420UL
 #define USEM_REG_FAST_MEMORY \
 	0x1940000UL
@@ -1430,7 +1433,7 @@
 	0x340800UL
 #define BRB_REG_BIG_RAM_DATA \
 	0x341500UL
-#define SEM_FAST_REG_STALL_0 \
+#define SEM_FAST_REG_STALL_0_BB_K2 \
 	0x000488UL
 #define SEM_FAST_REG_STALLED \
 	0x000494UL
@@ -1480,37 +1483,37 @@
 	4
 #define MISC_REG_BLOCK_256B_EN \
 	0x008c14UL
-#define NWS_REG_NWS_CMU	\
+#define NWS_REG_NWS_CMU_K2	\
 	0x720000UL
-#define PHY_NW_IP_REG_PHY0_TOP_TBUS_ADDR_7_0 \
+#define PHY_NW_IP_REG_PHY0_TOP_TBUS_ADDR_7_0_K2 \
 	0x000680UL
-#define PHY_NW_IP_REG_PHY0_TOP_TBUS_ADDR_15_8 \
+#define PHY_NW_IP_REG_PHY0_TOP_TBUS_ADDR_15_8_K2 \
 	0x000684UL
-#define PHY_NW_IP_REG_PHY0_TOP_TBUS_DATA_7_0 \
+#define PHY_NW_IP_REG_PHY0_TOP_TBUS_DATA_7_0_K2 \
 	0x0006c0UL
-#define PHY_NW_IP_REG_PHY0_TOP_TBUS_DATA_11_8 \
+#define PHY_NW_IP_REG_PHY0_TOP_TBUS_DATA_11_8_K2 \
 	0x0006c4UL
-#define MS_REG_MS_CMU \
+#define MS_REG_MS_CMU_K2 \
 	0x6a4000UL
-#define PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X130 \
+#define PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X130_K2 \
 	0x000208UL
-#define PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X132 \
-	0x000210UL
-#define PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X131 \
+#define PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X131_K2 \
 	0x00020cUL
-#define PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X133 \
+#define PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X132_K2 \
+	0x000210UL
+#define PHY_SGMII_IP_REG_AHB_CMU_CSR_0_X133_K2 \
 	0x000214UL
-#define PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X130 \
+#define PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X130_K2 \
 	0x000208UL
-#define PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X131 \
+#define PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X131_K2 \
 	0x00020cUL
-#define PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X132 \
+#define PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X132_K2 \
 	0x000210UL
-#define PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X133 \
+#define PHY_PCIE_IP_REG_AHB_CMU_CSR_0_X133_K2 \
 	0x000214UL
-#define PHY_PCIE_REG_PHY0 \
+#define PHY_PCIE_REG_PHY0_K2 \
 	0x620000UL
-#define PHY_PCIE_REG_PHY1 \
+#define PHY_PCIE_REG_PHY1_K2 \
 	0x624000UL
 #define NIG_REG_ROCE_DUPLICATE_TO_HOST 0x5088f0UL
 #define PRS_REG_LIGHT_L2_ETHERTYPE_EN 0x1f0968UL
@@ -1557,9 +1560,16 @@
 #define PGLUE_B_REG_PGL_ADDR_EC_F0_K2 0x2aaf9cUL
 #define PGLUE_B_REG_PGL_ADDR_F0_F0_K2 0x2aafa0UL
 #define PGLUE_B_REG_PGL_ADDR_F4_F0_K2 0x2aafa4UL
+#define PGLUE_B_REG_MASTER_WRITE_PAD_ENABLE 0x2aae30UL
 #define NIG_REG_TSGEN_FREECNT_UPDATE_K2 0x509008UL
 #define CNIG_REG_NIG_PORT0_CONF_K2 0x218200UL
 
+#define NIG_REG_TX_EDPM_CTRL 0x501f0cUL
+#define NIG_REG_TX_EDPM_CTRL_TX_EDPM_EN (0x1 << 0)
+#define NIG_REG_TX_EDPM_CTRL_TX_EDPM_EN_SHIFT 0
+#define NIG_REG_TX_EDPM_CTRL_TX_EDPM_TC_EN (0xff << 1)
+#define NIG_REG_TX_EDPM_CTRL_TX_EDPM_TC_EN_SHIFT 1
+
 #define PRS_REG_SEARCH_GFT 0x1f11bcUL
 #define PRS_REG_CM_HDR_GFT 0x1f11c8UL
 #define PRS_REG_GFT_CAM 0x1f1100UL
diff --git a/drivers/net/ethernet/qlogic/qed/qed_roce.c b/drivers/net/ethernet/qlogic/qed/qed_roce.c
index 56289d7cd306..fb7c2d1562ae 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_roce.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_roce.c
@@ -35,12 +35,7 @@
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
 #include <linux/errno.h>
-#include <linux/etherdevice.h>
-#include <linux/if_ether.h>
-#include <linux/if_vlan.h>
 #include <linux/io.h>
-#include <linux/ip.h>
-#include <linux/ipv6.h>
 #include <linux/kernel.h>
 #include <linux/list.h>
 #include <linux/module.h>
@@ -49,10 +44,6 @@
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/string.h>
-#include <linux/tcp.h>
-#include <linux/bitops.h>
-#include <linux/qed/qed_roce_if.h>
-#include <linux/qed/qed_roce_if.h>
 #include "qed.h"
 #include "qed_cxt.h"
 #include "qed_hsi.h"
@@ -62,18 +53,21 @@
 #include "qed_ll2.h"
 #include "qed_mcp.h"
 #include "qed_reg_addr.h"
-#include "qed_sp.h"
+#include <linux/qed/qed_rdma_if.h>
+#include "qed_rdma.h"
 #include "qed_roce.h"
-#include "qed_ll2.h"
+#include "qed_sp.h"
 
 static void qed_roce_free_real_icid(struct qed_hwfn *p_hwfn, u16 icid);
 
-void qed_roce_async_event(struct qed_hwfn *p_hwfn,
-			  u8 fw_event_code, union rdma_eqe_data *rdma_data)
+static int
+qed_roce_async_event(struct qed_hwfn *p_hwfn,
+		     u8 fw_event_code,
+		     u16 echo, union event_ring_data *data, u8 fw_return_code)
 {
 	if (fw_event_code == ROCE_ASYNC_EVENT_DESTROY_QP_DONE) {
 		u16 icid =
-		    (u16)le32_to_cpu(rdma_data->rdma_destroy_qp_data.cid);
+		    (u16)le32_to_cpu(data->rdma_data.rdma_destroy_qp_data.cid);
 
 		/* icid release in this async event can occur only if the icid
 		 * was offloaded to the FW. In case it wasn't offloaded this is
@@ -85,290 +79,15 @@ void qed_roce_async_event(struct qed_hwfn *p_hwfn,
 
 		events->affiliated_event(p_hwfn->p_rdma_info->events.context,
 					 fw_event_code,
-					 &rdma_data->async_handle);
-	}
-}
-
-static int qed_rdma_bmap_alloc(struct qed_hwfn *p_hwfn,
-			       struct qed_bmap *bmap, u32 max_count, char *name)
-{
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "max_count = %08x\n", max_count);
-
-	bmap->max_count = max_count;
-
-	bmap->bitmap = kzalloc(BITS_TO_LONGS(max_count) * sizeof(long),
-			       GFP_KERNEL);
-	if (!bmap->bitmap) {
-		DP_NOTICE(p_hwfn,
-			  "qed bmap alloc failed: cannot allocate memory (bitmap)\n");
-		return -ENOMEM;
+				     (void *)&data->rdma_data.async_handle);
 	}
 
-	snprintf(bmap->name, QED_RDMA_MAX_BMAP_NAME, "%s", name);
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "0\n");
-	return 0;
-}
-
-static int qed_rdma_bmap_alloc_id(struct qed_hwfn *p_hwfn,
-				  struct qed_bmap *bmap, u32 *id_num)
-{
-	*id_num = find_first_zero_bit(bmap->bitmap, bmap->max_count);
-	if (*id_num >= bmap->max_count)
-		return -EINVAL;
-
-	__set_bit(*id_num, bmap->bitmap);
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "%s bitmap: allocated id %d\n",
-		   bmap->name, *id_num);
-
 	return 0;
 }
 
-static void qed_bmap_set_id(struct qed_hwfn *p_hwfn,
-			    struct qed_bmap *bmap, u32 id_num)
-{
-	if (id_num >= bmap->max_count)
-		return;
-
-	__set_bit(id_num, bmap->bitmap);
-}
-
-static void qed_bmap_release_id(struct qed_hwfn *p_hwfn,
-				struct qed_bmap *bmap, u32 id_num)
-{
-	bool b_acquired;
-
-	if (id_num >= bmap->max_count)
-		return;
-
-	b_acquired = test_and_clear_bit(id_num, bmap->bitmap);
-	if (!b_acquired) {
-		DP_NOTICE(p_hwfn, "%s bitmap: id %d already released\n",
-			  bmap->name, id_num);
-		return;
-	}
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "%s bitmap: released id %d\n",
-		   bmap->name, id_num);
-}
-
-static int qed_bmap_test_id(struct qed_hwfn *p_hwfn,
-			    struct qed_bmap *bmap, u32 id_num)
-{
-	if (id_num >= bmap->max_count)
-		return -1;
-
-	return test_bit(id_num, bmap->bitmap);
-}
-
-static u32 qed_rdma_get_sb_id(void *p_hwfn, u32 rel_sb_id)
-{
-	/* First sb id for RoCE is after all the l2 sb */
-	return FEAT_NUM((struct qed_hwfn *)p_hwfn, QED_PF_L2_QUE) + rel_sb_id;
-}
-
-static int qed_rdma_alloc(struct qed_hwfn *p_hwfn,
-			  struct qed_ptt *p_ptt,
-			  struct qed_rdma_start_in_params *params)
-{
-	struct qed_rdma_info *p_rdma_info;
-	u32 num_cons, num_tasks;
-	int rc = -ENOMEM;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Allocating RDMA\n");
-
-	/* Allocate a struct with current pf rdma info */
-	p_rdma_info = kzalloc(sizeof(*p_rdma_info), GFP_KERNEL);
-	if (!p_rdma_info) {
-		DP_NOTICE(p_hwfn,
-			  "qed rdma alloc failed: cannot allocate memory (rdma info). rc = %d\n",
-			  rc);
-		return rc;
-	}
-
-	p_hwfn->p_rdma_info = p_rdma_info;
-	p_rdma_info->proto = PROTOCOLID_ROCE;
-
-	num_cons = qed_cxt_get_proto_cid_count(p_hwfn, p_rdma_info->proto,
-					       NULL);
-
-	p_rdma_info->num_qps = num_cons / 2;
-
-	num_tasks = qed_cxt_get_proto_tid_count(p_hwfn, PROTOCOLID_ROCE);
-
-	/* Each MR uses a single task */
-	p_rdma_info->num_mrs = num_tasks;
-
-	/* Queue zone lines are shared between RoCE and L2 in such a way that
-	 * they can be used by each without obstructing the other.
-	 */
-	p_rdma_info->queue_zone_base = (u16)RESC_START(p_hwfn, QED_L2_QUEUE);
-	p_rdma_info->max_queue_zones = (u16)RESC_NUM(p_hwfn, QED_L2_QUEUE);
-
-	/* Allocate a struct with device params and fill it */
-	p_rdma_info->dev = kzalloc(sizeof(*p_rdma_info->dev), GFP_KERNEL);
-	if (!p_rdma_info->dev) {
-		DP_NOTICE(p_hwfn,
-			  "qed rdma alloc failed: cannot allocate memory (rdma info dev). rc = %d\n",
-			  rc);
-		goto free_rdma_info;
-	}
-
-	/* Allocate a struct with port params and fill it */
-	p_rdma_info->port = kzalloc(sizeof(*p_rdma_info->port), GFP_KERNEL);
-	if (!p_rdma_info->port) {
-		DP_NOTICE(p_hwfn,
-			  "qed rdma alloc failed: cannot allocate memory (rdma info port). rc = %d\n",
-			  rc);
-		goto free_rdma_dev;
-	}
-
-	/* Allocate bit map for pd's */
-	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->pd_map, RDMA_MAX_PDS,
-				 "PD");
-	if (rc) {
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
-			   "Failed to allocate pd_map, rc = %d\n",
-			   rc);
-		goto free_rdma_port;
-	}
-
-	/* Allocate DPI bitmap */
-	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->dpi_map,
-				 p_hwfn->dpi_count, "DPI");
-	if (rc) {
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
-			   "Failed to allocate DPI bitmap, rc = %d\n", rc);
-		goto free_pd_map;
-	}
-
-	/* Allocate bitmap for cq's. The maximum number of CQs is bounded to
-	 * twice the number of QPs.
-	 */
-	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->cq_map,
-				 p_rdma_info->num_qps * 2, "CQ");
-	if (rc) {
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
-			   "Failed to allocate cq bitmap, rc = %d\n", rc);
-		goto free_dpi_map;
-	}
-
-	/* Allocate bitmap for toggle bit for cq icids
-	 * We toggle the bit every time we create or resize cq for a given icid.
-	 * The maximum number of CQs is bounded to  twice the number of QPs.
-	 */
-	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->toggle_bits,
-				 p_rdma_info->num_qps * 2, "Toggle");
-	if (rc) {
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
-			   "Failed to allocate toogle bits, rc = %d\n", rc);
-		goto free_cq_map;
-	}
-
-	/* Allocate bitmap for itids */
-	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->tid_map,
-				 p_rdma_info->num_mrs, "MR");
-	if (rc) {
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
-			   "Failed to allocate itids bitmaps, rc = %d\n", rc);
-		goto free_toggle_map;
-	}
-
-	/* Allocate bitmap for cids used for qps. */
-	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->cid_map, num_cons,
-				 "CID");
-	if (rc) {
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
-			   "Failed to allocate cid bitmap, rc = %d\n", rc);
-		goto free_tid_map;
-	}
-
-	/* Allocate bitmap for cids used for responders/requesters. */
-	rc = qed_rdma_bmap_alloc(p_hwfn, &p_rdma_info->real_cid_map, num_cons,
-				 "REAL_CID");
-	if (rc) {
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
-			   "Failed to allocate real cid bitmap, rc = %d\n", rc);
-		goto free_cid_map;
-	}
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Allocation successful\n");
-	return 0;
-
-free_cid_map:
-	kfree(p_rdma_info->cid_map.bitmap);
-free_tid_map:
-	kfree(p_rdma_info->tid_map.bitmap);
-free_toggle_map:
-	kfree(p_rdma_info->toggle_bits.bitmap);
-free_cq_map:
-	kfree(p_rdma_info->cq_map.bitmap);
-free_dpi_map:
-	kfree(p_rdma_info->dpi_map.bitmap);
-free_pd_map:
-	kfree(p_rdma_info->pd_map.bitmap);
-free_rdma_port:
-	kfree(p_rdma_info->port);
-free_rdma_dev:
-	kfree(p_rdma_info->dev);
-free_rdma_info:
-	kfree(p_rdma_info);
-
-	return rc;
-}
-
-static void qed_rdma_bmap_free(struct qed_hwfn *p_hwfn,
-			       struct qed_bmap *bmap, bool check)
-{
-	int weight = bitmap_weight(bmap->bitmap, bmap->max_count);
-	int last_line = bmap->max_count / (64 * 8);
-	int last_item = last_line * 8 +
-	    DIV_ROUND_UP(bmap->max_count % (64 * 8), 64);
-	u64 *pmap = (u64 *)bmap->bitmap;
-	int line, item, offset;
-	u8 str_last_line[200] = { 0 };
-
-	if (!weight || !check)
-		goto end;
-
-	DP_NOTICE(p_hwfn,
-		  "%s bitmap not free - size=%d, weight=%d, 512 bits per line\n",
-		  bmap->name, bmap->max_count, weight);
-
-	/* print aligned non-zero lines, if any */
-	for (item = 0, line = 0; line < last_line; line++, item += 8)
-		if (bitmap_weight((unsigned long *)&pmap[item], 64 * 8))
-			DP_NOTICE(p_hwfn,
-				  "line 0x%04x: 0x%016llx 0x%016llx 0x%016llx 0x%016llx 0x%016llx 0x%016llx 0x%016llx 0x%016llx\n",
-				  line,
-				  pmap[item],
-				  pmap[item + 1],
-				  pmap[item + 2],
-				  pmap[item + 3],
-				  pmap[item + 4],
-				  pmap[item + 5],
-				  pmap[item + 6], pmap[item + 7]);
-
-	/* print last unaligned non-zero line, if any */
-	if ((bmap->max_count % (64 * 8)) &&
-	    (bitmap_weight((unsigned long *)&pmap[item],
-			   bmap->max_count - item * 64))) {
-		offset = sprintf(str_last_line, "line 0x%04x: ", line);
-		for (; item < last_item; item++)
-			offset += sprintf(str_last_line + offset,
-					  "0x%016llx ", pmap[item]);
-		DP_NOTICE(p_hwfn, "%s\n", str_last_line);
-	}
-
-end:
-	kfree(bmap->bitmap);
-	bmap->bitmap = NULL;
-}
-
-static void qed_rdma_resc_free(struct qed_hwfn *p_hwfn)
+void qed_roce_stop(struct qed_hwfn *p_hwfn)
 {
 	struct qed_bmap *rcid_map = &p_hwfn->p_rdma_info->real_cid_map;
-	struct qed_rdma_info *p_rdma_info = p_hwfn->p_rdma_info;
 	int wait_count = 0;
 
 	/* when destroying a_RoCE QP the control is returned to the user after
@@ -383,780 +102,7 @@ static void qed_rdma_resc_free(struct qed_hwfn *p_hwfn)
 			break;
 		}
 	}
-
-	qed_rdma_bmap_free(p_hwfn, &p_hwfn->p_rdma_info->cid_map, 1);
-	qed_rdma_bmap_free(p_hwfn, &p_hwfn->p_rdma_info->pd_map, 1);
-	qed_rdma_bmap_free(p_hwfn, &p_hwfn->p_rdma_info->dpi_map, 1);
-	qed_rdma_bmap_free(p_hwfn, &p_hwfn->p_rdma_info->cq_map, 1);
-	qed_rdma_bmap_free(p_hwfn, &p_hwfn->p_rdma_info->toggle_bits, 0);
-	qed_rdma_bmap_free(p_hwfn, &p_hwfn->p_rdma_info->tid_map, 1);
-
-	kfree(p_rdma_info->port);
-	kfree(p_rdma_info->dev);
-
-	kfree(p_rdma_info);
-}
-
-static void qed_rdma_free(struct qed_hwfn *p_hwfn)
-{
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Freeing RDMA\n");
-
-	qed_rdma_resc_free(p_hwfn);
-}
-
-static void qed_rdma_get_guid(struct qed_hwfn *p_hwfn, u8 *guid)
-{
-	guid[0] = p_hwfn->hw_info.hw_mac_addr[0] ^ 2;
-	guid[1] = p_hwfn->hw_info.hw_mac_addr[1];
-	guid[2] = p_hwfn->hw_info.hw_mac_addr[2];
-	guid[3] = 0xff;
-	guid[4] = 0xfe;
-	guid[5] = p_hwfn->hw_info.hw_mac_addr[3];
-	guid[6] = p_hwfn->hw_info.hw_mac_addr[4];
-	guid[7] = p_hwfn->hw_info.hw_mac_addr[5];
-}
-
-static void qed_rdma_init_events(struct qed_hwfn *p_hwfn,
-				 struct qed_rdma_start_in_params *params)
-{
-	struct qed_rdma_events *events;
-
-	events = &p_hwfn->p_rdma_info->events;
-
-	events->unaffiliated_event = params->events->unaffiliated_event;
-	events->affiliated_event = params->events->affiliated_event;
-	events->context = params->events->context;
-}
-
-static void qed_rdma_init_devinfo(struct qed_hwfn *p_hwfn,
-				  struct qed_rdma_start_in_params *params)
-{
-	struct qed_rdma_device *dev = p_hwfn->p_rdma_info->dev;
-	struct qed_dev *cdev = p_hwfn->cdev;
-	u32 pci_status_control;
-	u32 num_qps;
-
-	/* Vendor specific information */
-	dev->vendor_id = cdev->vendor_id;
-	dev->vendor_part_id = cdev->device_id;
-	dev->hw_ver = 0;
-	dev->fw_ver = (FW_MAJOR_VERSION << 24) | (FW_MINOR_VERSION << 16) |
-		      (FW_REVISION_VERSION << 8) | (FW_ENGINEERING_VERSION);
-
-	qed_rdma_get_guid(p_hwfn, (u8 *)&dev->sys_image_guid);
-	dev->node_guid = dev->sys_image_guid;
-
-	dev->max_sge = min_t(u32, RDMA_MAX_SGE_PER_SQ_WQE,
-			     RDMA_MAX_SGE_PER_RQ_WQE);
-
-	if (cdev->rdma_max_sge)
-		dev->max_sge = min_t(u32, cdev->rdma_max_sge, dev->max_sge);
-
-	dev->max_inline = ROCE_REQ_MAX_INLINE_DATA_SIZE;
-
-	dev->max_inline = (cdev->rdma_max_inline) ?
-			  min_t(u32, cdev->rdma_max_inline, dev->max_inline) :
-			  dev->max_inline;
-
-	dev->max_wqe = QED_RDMA_MAX_WQE;
-	dev->max_cnq = (u8)FEAT_NUM(p_hwfn, QED_RDMA_CNQ);
-
-	/* The number of QPs may be higher than QED_ROCE_MAX_QPS, because
-	 * it is up-aligned to 16 and then to ILT page size within qed cxt.
-	 * This is OK in terms of ILT but we don't want to configure the FW
-	 * above its abilities
-	 */
-	num_qps = ROCE_MAX_QPS;
-	num_qps = min_t(u64, num_qps, p_hwfn->p_rdma_info->num_qps);
-	dev->max_qp = num_qps;
-
-	/* CQs uses the same icids that QPs use hence they are limited by the
-	 * number of icids. There are two icids per QP.
-	 */
-	dev->max_cq = num_qps * 2;
-
-	/* The number of mrs is smaller by 1 since the first is reserved */
-	dev->max_mr = p_hwfn->p_rdma_info->num_mrs - 1;
-	dev->max_mr_size = QED_RDMA_MAX_MR_SIZE;
-
-	/* The maximum CQE capacity per CQ supported.
-	 * max number of cqes will be in two layer pbl,
-	 * 8 is the pointer size in bytes
-	 * 32 is the size of cq element in bytes
-	 */
-	if (params->cq_mode == QED_RDMA_CQ_MODE_32_BITS)
-		dev->max_cqe = QED_RDMA_MAX_CQE_32_BIT;
-	else
-		dev->max_cqe = QED_RDMA_MAX_CQE_16_BIT;
-
-	dev->max_mw = 0;
-	dev->max_fmr = QED_RDMA_MAX_FMR;
-	dev->max_mr_mw_fmr_pbl = (PAGE_SIZE / 8) * (PAGE_SIZE / 8);
-	dev->max_mr_mw_fmr_size = dev->max_mr_mw_fmr_pbl * PAGE_SIZE;
-	dev->max_pkey = QED_RDMA_MAX_P_KEY;
-
-	dev->max_qp_resp_rd_atomic_resc = RDMA_RING_PAGE_SIZE /
-					  (RDMA_RESP_RD_ATOMIC_ELM_SIZE * 2);
-	dev->max_qp_req_rd_atomic_resc = RDMA_RING_PAGE_SIZE /
-					 RDMA_REQ_RD_ATOMIC_ELM_SIZE;
-	dev->max_dev_resp_rd_atomic_resc = dev->max_qp_resp_rd_atomic_resc *
-					   p_hwfn->p_rdma_info->num_qps;
-	dev->page_size_caps = QED_RDMA_PAGE_SIZE_CAPS;
-	dev->dev_ack_delay = QED_RDMA_ACK_DELAY;
-	dev->max_pd = RDMA_MAX_PDS;
-	dev->max_ah = p_hwfn->p_rdma_info->num_qps;
-	dev->max_stats_queues = (u8)RESC_NUM(p_hwfn, QED_RDMA_STATS_QUEUE);
-
-	/* Set capablities */
-	dev->dev_caps = 0;
-	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_RNR_NAK, 1);
-	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_PORT_ACTIVE_EVENT, 1);
-	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_PORT_CHANGE_EVENT, 1);
-	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_RESIZE_CQ, 1);
-	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_BASE_MEMORY_EXT, 1);
-	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_BASE_QUEUE_EXT, 1);
-	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_ZBVA, 1);
-	SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_LOCAL_INV_FENCE, 1);
-
-	/* Check atomic operations support in PCI configuration space. */
-	pci_read_config_dword(cdev->pdev,
-			      cdev->pdev->pcie_cap + PCI_EXP_DEVCTL2,
-			      &pci_status_control);
-
-	if (pci_status_control & PCI_EXP_DEVCTL2_LTR_EN)
-		SET_FIELD(dev->dev_caps, QED_RDMA_DEV_CAP_ATOMIC_OP, 1);
-}
-
-static void qed_rdma_init_port(struct qed_hwfn *p_hwfn)
-{
-	struct qed_rdma_port *port = p_hwfn->p_rdma_info->port;
-	struct qed_rdma_device *dev = p_hwfn->p_rdma_info->dev;
-
-	port->port_state = p_hwfn->mcp_info->link_output.link_up ?
-			   QED_RDMA_PORT_UP : QED_RDMA_PORT_DOWN;
-
-	port->max_msg_size = min_t(u64,
-				   (dev->max_mr_mw_fmr_size *
-				    p_hwfn->cdev->rdma_max_sge),
-				   BIT(31));
-
-	port->pkey_bad_counter = 0;
-}
-
-static int qed_rdma_init_hw(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
-{
-	u32 ll2_ethertype_en;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Initializing HW\n");
-	p_hwfn->b_rdma_enabled_in_prs = false;
-
-	qed_wr(p_hwfn, p_ptt, PRS_REG_ROCE_DEST_QP_MAX_PF, 0);
-
-	p_hwfn->rdma_prs_search_reg = PRS_REG_SEARCH_ROCE;
-
-	/* We delay writing to this reg until first cid is allocated. See
-	 * qed_cxt_dynamic_ilt_alloc function for more details
-	 */
-	ll2_ethertype_en = qed_rd(p_hwfn, p_ptt, PRS_REG_LIGHT_L2_ETHERTYPE_EN);
-	qed_wr(p_hwfn, p_ptt, PRS_REG_LIGHT_L2_ETHERTYPE_EN,
-	       (ll2_ethertype_en | 0x01));
-
-	if (qed_cxt_get_proto_cid_start(p_hwfn, PROTOCOLID_ROCE) % 2) {
-		DP_NOTICE(p_hwfn, "The first RoCE's cid should be even\n");
-		return -EINVAL;
-	}
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Initializing HW - Done\n");
-	return 0;
-}
-
-static int qed_rdma_start_fw(struct qed_hwfn *p_hwfn,
-			     struct qed_rdma_start_in_params *params,
-			     struct qed_ptt *p_ptt)
-{
-	struct rdma_init_func_ramrod_data *p_ramrod;
-	struct qed_rdma_cnq_params *p_cnq_pbl_list;
-	struct rdma_init_func_hdr *p_params_header;
-	struct rdma_cnq_params *p_cnq_params;
-	struct qed_sp_init_data init_data;
-	struct qed_spq_entry *p_ent;
-	u32 cnq_id, sb_id;
-	int rc;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Starting FW\n");
-
-	/* Save the number of cnqs for the function close ramrod */
-	p_hwfn->p_rdma_info->num_cnqs = params->desired_cnq;
-
-	/* Get SPQ entry */
-	memset(&init_data, 0, sizeof(init_data));
-	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
-	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
-
-	rc = qed_sp_init_request(p_hwfn, &p_ent, RDMA_RAMROD_FUNC_INIT,
-				 p_hwfn->p_rdma_info->proto, &init_data);
-	if (rc)
-		return rc;
-
-	p_ramrod = &p_ent->ramrod.roce_init_func.rdma;
-
-	p_params_header = &p_ramrod->params_header;
-	p_params_header->cnq_start_offset = (u8)RESC_START(p_hwfn,
-							   QED_RDMA_CNQ_RAM);
-	p_params_header->num_cnqs = params->desired_cnq;
-
-	if (params->cq_mode == QED_RDMA_CQ_MODE_16_BITS)
-		p_params_header->cq_ring_mode = 1;
-	else
-		p_params_header->cq_ring_mode = 0;
-
-	for (cnq_id = 0; cnq_id < params->desired_cnq; cnq_id++) {
-		sb_id = qed_rdma_get_sb_id(p_hwfn, cnq_id);
-		p_cnq_params = &p_ramrod->cnq_params[cnq_id];
-		p_cnq_pbl_list = &params->cnq_pbl_list[cnq_id];
-		p_cnq_params->sb_num =
-			cpu_to_le16(p_hwfn->sbs_info[sb_id]->igu_sb_id);
-
-		p_cnq_params->sb_index = p_hwfn->pf_params.rdma_pf_params.gl_pi;
-		p_cnq_params->num_pbl_pages = p_cnq_pbl_list->num_pbl_pages;
-
-		DMA_REGPAIR_LE(p_cnq_params->pbl_base_addr,
-			       p_cnq_pbl_list->pbl_ptr);
-
-		/* we assume here that cnq_id and qz_offset are the same */
-		p_cnq_params->queue_zone_num =
-			cpu_to_le16(p_hwfn->p_rdma_info->queue_zone_base +
-				    cnq_id);
-	}
-
-	return qed_spq_post(p_hwfn, p_ent, NULL);
-}
-
-static int qed_rdma_alloc_tid(void *rdma_cxt, u32 *itid)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	int rc;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Allocate TID\n");
-
-	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
-	rc = qed_rdma_bmap_alloc_id(p_hwfn,
-				    &p_hwfn->p_rdma_info->tid_map, itid);
-	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
-	if (rc)
-		goto out;
-
-	rc = qed_cxt_dynamic_ilt_alloc(p_hwfn, QED_ELEM_TASK, *itid);
-out:
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Allocate TID - done, rc = %d\n", rc);
-	return rc;
-}
-
-static int qed_rdma_reserve_lkey(struct qed_hwfn *p_hwfn)
-{
-	struct qed_rdma_device *dev = p_hwfn->p_rdma_info->dev;
-
-	/* The first DPI is reserved for the Kernel */
-	__set_bit(0, p_hwfn->p_rdma_info->dpi_map.bitmap);
-
-	/* Tid 0 will be used as the key for "reserved MR".
-	 * The driver should allocate memory for it so it can be loaded but no
-	 * ramrod should be passed on it.
-	 */
-	qed_rdma_alloc_tid(p_hwfn, &dev->reserved_lkey);
-	if (dev->reserved_lkey != RDMA_RESERVED_LKEY) {
-		DP_NOTICE(p_hwfn,
-			  "Reserved lkey should be equal to RDMA_RESERVED_LKEY\n");
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int qed_rdma_setup(struct qed_hwfn *p_hwfn,
-			  struct qed_ptt *p_ptt,
-			  struct qed_rdma_start_in_params *params)
-{
-	int rc;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA setup\n");
-
-	spin_lock_init(&p_hwfn->p_rdma_info->lock);
-
-	qed_rdma_init_devinfo(p_hwfn, params);
-	qed_rdma_init_port(p_hwfn);
-	qed_rdma_init_events(p_hwfn, params);
-
-	rc = qed_rdma_reserve_lkey(p_hwfn);
-	if (rc)
-		return rc;
-
-	rc = qed_rdma_init_hw(p_hwfn, p_ptt);
-	if (rc)
-		return rc;
-
-	return qed_rdma_start_fw(p_hwfn, params, p_ptt);
-}
-
-static int qed_rdma_stop(void *rdma_cxt)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	struct rdma_close_func_ramrod_data *p_ramrod;
-	struct qed_sp_init_data init_data;
-	struct qed_spq_entry *p_ent;
-	struct qed_ptt *p_ptt;
-	u32 ll2_ethertype_en;
-	int rc = -EBUSY;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA stop\n");
-
-	p_ptt = qed_ptt_acquire(p_hwfn);
-	if (!p_ptt) {
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Failed to acquire PTT\n");
-		return rc;
-	}
-
-	/* Disable RoCE search */
-	qed_wr(p_hwfn, p_ptt, p_hwfn->rdma_prs_search_reg, 0);
-	p_hwfn->b_rdma_enabled_in_prs = false;
-
-	qed_wr(p_hwfn, p_ptt, PRS_REG_ROCE_DEST_QP_MAX_PF, 0);
-
-	ll2_ethertype_en = qed_rd(p_hwfn, p_ptt, PRS_REG_LIGHT_L2_ETHERTYPE_EN);
-
-	qed_wr(p_hwfn, p_ptt, PRS_REG_LIGHT_L2_ETHERTYPE_EN,
-	       (ll2_ethertype_en & 0xFFFE));
-
-	qed_ptt_release(p_hwfn, p_ptt);
-
-	/* Get SPQ entry */
-	memset(&init_data, 0, sizeof(init_data));
-	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
-	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
-
-	/* Stop RoCE */
-	rc = qed_sp_init_request(p_hwfn, &p_ent, RDMA_RAMROD_FUNC_CLOSE,
-				 p_hwfn->p_rdma_info->proto, &init_data);
-	if (rc)
-		goto out;
-
-	p_ramrod = &p_ent->ramrod.rdma_close_func;
-
-	p_ramrod->num_cnqs = p_hwfn->p_rdma_info->num_cnqs;
-	p_ramrod->cnq_start_offset = (u8)RESC_START(p_hwfn, QED_RDMA_CNQ_RAM);
-
-	rc = qed_spq_post(p_hwfn, p_ent, NULL);
-
-out:
-	qed_rdma_free(p_hwfn);
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA stop done, rc = %d\n", rc);
-	return rc;
-}
-
-static int qed_rdma_add_user(void *rdma_cxt,
-			     struct qed_rdma_add_user_out_params *out_params)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	u32 dpi_start_offset;
-	u32 returned_id = 0;
-	int rc;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Adding User\n");
-
-	/* Allocate DPI */
-	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
-	rc = qed_rdma_bmap_alloc_id(p_hwfn, &p_hwfn->p_rdma_info->dpi_map,
-				    &returned_id);
-	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
-
-	out_params->dpi = (u16)returned_id;
-
-	/* Calculate the corresponding DPI address */
-	dpi_start_offset = p_hwfn->dpi_start_offset;
-
-	out_params->dpi_addr = (u64)((u8 __iomem *)p_hwfn->doorbells +
-				     dpi_start_offset +
-				     ((out_params->dpi) * p_hwfn->dpi_size));
-
-	out_params->dpi_phys_addr = p_hwfn->cdev->db_phys_addr +
-				    dpi_start_offset +
-				    ((out_params->dpi) * p_hwfn->dpi_size);
-
-	out_params->dpi_size = p_hwfn->dpi_size;
-	out_params->wid_count = p_hwfn->wid_count;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Adding user - done, rc = %d\n", rc);
-	return rc;
-}
-
-static struct qed_rdma_port *qed_rdma_query_port(void *rdma_cxt)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	struct qed_rdma_port *p_port = p_hwfn->p_rdma_info->port;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA Query port\n");
-
-	/* Link may have changed */
-	p_port->port_state = p_hwfn->mcp_info->link_output.link_up ?
-			     QED_RDMA_PORT_UP : QED_RDMA_PORT_DOWN;
-
-	p_port->link_speed = p_hwfn->mcp_info->link_output.speed;
-
-	p_port->max_msg_size = RDMA_MAX_DATA_SIZE_IN_WQE;
-
-	return p_port;
-}
-
-static struct qed_rdma_device *qed_rdma_query_device(void *rdma_cxt)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Query device\n");
-
-	/* Return struct with device parameters */
-	return p_hwfn->p_rdma_info->dev;
-}
-
-static void qed_rdma_free_tid(void *rdma_cxt, u32 itid)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "itid = %08x\n", itid);
-
-	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
-	qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->tid_map, itid);
-	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
-}
-
-static void qed_rdma_cnq_prod_update(void *rdma_cxt, u8 qz_offset, u16 prod)
-{
-	struct qed_hwfn *p_hwfn;
-	u16 qz_num;
-	u32 addr;
-
-	p_hwfn = (struct qed_hwfn *)rdma_cxt;
-
-	if (qz_offset > p_hwfn->p_rdma_info->max_queue_zones) {
-		DP_NOTICE(p_hwfn,
-			  "queue zone offset %d is too large (max is %d)\n",
-			  qz_offset, p_hwfn->p_rdma_info->max_queue_zones);
-		return;
-	}
-
-	qz_num = p_hwfn->p_rdma_info->queue_zone_base + qz_offset;
-	addr = GTT_BAR0_MAP_REG_USDM_RAM +
-	       USTORM_COMMON_QUEUE_CONS_OFFSET(qz_num);
-
-	REG_WR16(p_hwfn, addr, prod);
-
-	/* keep prod updates ordered */
-	wmb();
-}
-
-static int qed_fill_rdma_dev_info(struct qed_dev *cdev,
-				  struct qed_dev_rdma_info *info)
-{
-	struct qed_hwfn *p_hwfn = QED_LEADING_HWFN(cdev);
-
-	memset(info, 0, sizeof(*info));
-
-	info->rdma_type = QED_RDMA_TYPE_ROCE;
-	info->user_dpm_enabled = (p_hwfn->db_bar_no_edpm == 0);
-
-	qed_fill_dev_info(cdev, &info->common);
-
-	return 0;
-}
-
-static int qed_rdma_get_sb_start(struct qed_dev *cdev)
-{
-	int feat_num;
-
-	if (cdev->num_hwfns > 1)
-		feat_num = FEAT_NUM(QED_LEADING_HWFN(cdev), QED_PF_L2_QUE);
-	else
-		feat_num = FEAT_NUM(QED_LEADING_HWFN(cdev), QED_PF_L2_QUE) *
-			   cdev->num_hwfns;
-
-	return feat_num;
-}
-
-static int qed_rdma_get_min_cnq_msix(struct qed_dev *cdev)
-{
-	int n_cnq = FEAT_NUM(QED_LEADING_HWFN(cdev), QED_RDMA_CNQ);
-	int n_msix = cdev->int_params.rdma_msix_cnt;
-
-	return min_t(int, n_cnq, n_msix);
-}
-
-static int qed_rdma_set_int(struct qed_dev *cdev, u16 cnt)
-{
-	int limit = 0;
-
-	/* Mark the fastpath as free/used */
-	cdev->int_params.fp_initialized = cnt ? true : false;
-
-	if (cdev->int_params.out.int_mode != QED_INT_MODE_MSIX) {
-		DP_ERR(cdev,
-		       "qed roce supports only MSI-X interrupts (detected %d).\n",
-		       cdev->int_params.out.int_mode);
-		return -EINVAL;
-	} else if (cdev->int_params.fp_msix_cnt) {
-		limit = cdev->int_params.rdma_msix_cnt;
-	}
-
-	if (!limit)
-		return -ENOMEM;
-
-	return min_t(int, cnt, limit);
-}
-
-static int qed_rdma_get_int(struct qed_dev *cdev, struct qed_int_info *info)
-{
-	memset(info, 0, sizeof(*info));
-
-	if (!cdev->int_params.fp_initialized) {
-		DP_INFO(cdev,
-			"Protocol driver requested interrupt information, but its support is not yet configured\n");
-		return -EINVAL;
-	}
-
-	if (cdev->int_params.out.int_mode == QED_INT_MODE_MSIX) {
-		int msix_base = cdev->int_params.rdma_msix_base;
-
-		info->msix_cnt = cdev->int_params.rdma_msix_cnt;
-		info->msix = &cdev->int_params.msix_table[msix_base];
-
-		DP_VERBOSE(cdev, QED_MSG_RDMA, "msix_cnt = %d msix_base=%d\n",
-			   info->msix_cnt, msix_base);
-	}
-
-	return 0;
-}
-
-static int qed_rdma_alloc_pd(void *rdma_cxt, u16 *pd)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	u32 returned_id;
-	int rc;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Alloc PD\n");
-
-	/* Allocates an unused protection domain */
-	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
-	rc = qed_rdma_bmap_alloc_id(p_hwfn,
-				    &p_hwfn->p_rdma_info->pd_map, &returned_id);
-	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
-
-	*pd = (u16)returned_id;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Alloc PD - done, rc = %d\n", rc);
-	return rc;
-}
-
-static void qed_rdma_free_pd(void *rdma_cxt, u16 pd)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "pd = %08x\n", pd);
-
-	/* Returns a previously allocated protection domain for reuse */
-	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
-	qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->pd_map, pd);
-	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
-}
-
-static enum qed_rdma_toggle_bit
-qed_rdma_toggle_bit_create_resize_cq(struct qed_hwfn *p_hwfn, u16 icid)
-{
-	struct qed_rdma_info *p_info = p_hwfn->p_rdma_info;
-	enum qed_rdma_toggle_bit toggle_bit;
-	u32 bmap_id;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", icid);
-
-	/* the function toggle the bit that is related to a given icid
-	 * and returns the new toggle bit's value
-	 */
-	bmap_id = icid - qed_cxt_get_proto_cid_start(p_hwfn, p_info->proto);
-
-	spin_lock_bh(&p_info->lock);
-	toggle_bit = !test_and_change_bit(bmap_id,
-					  p_info->toggle_bits.bitmap);
-	spin_unlock_bh(&p_info->lock);
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "QED_RDMA_TOGGLE_BIT_= %d\n",
-		   toggle_bit);
-
-	return toggle_bit;
-}
-
-static int qed_rdma_create_cq(void *rdma_cxt,
-			      struct qed_rdma_create_cq_in_params *params,
-			      u16 *icid)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	struct qed_rdma_info *p_info = p_hwfn->p_rdma_info;
-	struct rdma_create_cq_ramrod_data *p_ramrod;
-	enum qed_rdma_toggle_bit toggle_bit;
-	struct qed_sp_init_data init_data;
-	struct qed_spq_entry *p_ent;
-	u32 returned_id, start_cid;
-	int rc;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "cq_handle = %08x%08x\n",
-		   params->cq_handle_hi, params->cq_handle_lo);
-
-	/* Allocate icid */
-	spin_lock_bh(&p_info->lock);
-	rc = qed_rdma_bmap_alloc_id(p_hwfn, &p_info->cq_map, &returned_id);
-	spin_unlock_bh(&p_info->lock);
-
-	if (rc) {
-		DP_NOTICE(p_hwfn, "Can't create CQ, rc = %d\n", rc);
-		return rc;
-	}
-
-	start_cid = qed_cxt_get_proto_cid_start(p_hwfn,
-						p_info->proto);
-	*icid = returned_id + start_cid;
-
-	/* Check if icid requires a page allocation */
-	rc = qed_cxt_dynamic_ilt_alloc(p_hwfn, QED_ELEM_CXT, *icid);
-	if (rc)
-		goto err;
-
-	/* Get SPQ entry */
-	memset(&init_data, 0, sizeof(init_data));
-	init_data.cid = *icid;
-	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
-	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
-
-	/* Send create CQ ramrod */
-	rc = qed_sp_init_request(p_hwfn, &p_ent,
-				 RDMA_RAMROD_CREATE_CQ,
-				 p_info->proto, &init_data);
-	if (rc)
-		goto err;
-
-	p_ramrod = &p_ent->ramrod.rdma_create_cq;
-
-	p_ramrod->cq_handle.hi = cpu_to_le32(params->cq_handle_hi);
-	p_ramrod->cq_handle.lo = cpu_to_le32(params->cq_handle_lo);
-	p_ramrod->dpi = cpu_to_le16(params->dpi);
-	p_ramrod->is_two_level_pbl = params->pbl_two_level;
-	p_ramrod->max_cqes = cpu_to_le32(params->cq_size);
-	DMA_REGPAIR_LE(p_ramrod->pbl_addr, params->pbl_ptr);
-	p_ramrod->pbl_num_pages = cpu_to_le16(params->pbl_num_pages);
-	p_ramrod->cnq_id = (u8)RESC_START(p_hwfn, QED_RDMA_CNQ_RAM) +
-			   params->cnq_id;
-	p_ramrod->int_timeout = params->int_timeout;
-
-	/* toggle the bit for every resize or create cq for a given icid */
-	toggle_bit = qed_rdma_toggle_bit_create_resize_cq(p_hwfn, *icid);
-
-	p_ramrod->toggle_bit = toggle_bit;
-
-	rc = qed_spq_post(p_hwfn, p_ent, NULL);
-	if (rc) {
-		/* restore toggle bit */
-		qed_rdma_toggle_bit_create_resize_cq(p_hwfn, *icid);
-		goto err;
-	}
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Created CQ, rc = %d\n", rc);
-	return rc;
-
-err:
-	/* release allocated icid */
-	spin_lock_bh(&p_info->lock);
-	qed_bmap_release_id(p_hwfn, &p_info->cq_map, returned_id);
-	spin_unlock_bh(&p_info->lock);
-	DP_NOTICE(p_hwfn, "Create CQ failed, rc = %d\n", rc);
-
-	return rc;
-}
-
-static int
-qed_rdma_destroy_cq(void *rdma_cxt,
-		    struct qed_rdma_destroy_cq_in_params *in_params,
-		    struct qed_rdma_destroy_cq_out_params *out_params)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	struct rdma_destroy_cq_output_params *p_ramrod_res;
-	struct rdma_destroy_cq_ramrod_data *p_ramrod;
-	struct qed_sp_init_data init_data;
-	struct qed_spq_entry *p_ent;
-	dma_addr_t ramrod_res_phys;
-	int rc = -ENOMEM;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", in_params->icid);
-
-	p_ramrod_res =
-	    (struct rdma_destroy_cq_output_params *)
-	    dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
-			       sizeof(struct rdma_destroy_cq_output_params),
-			       &ramrod_res_phys, GFP_KERNEL);
-	if (!p_ramrod_res) {
-		DP_NOTICE(p_hwfn,
-			  "qed destroy cq failed: cannot allocate memory (ramrod)\n");
-		return rc;
-	}
-
-	/* Get SPQ entry */
-	memset(&init_data, 0, sizeof(init_data));
-	init_data.cid = in_params->icid;
-	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
-	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
-
-	/* Send destroy CQ ramrod */
-	rc = qed_sp_init_request(p_hwfn, &p_ent,
-				 RDMA_RAMROD_DESTROY_CQ,
-				 p_hwfn->p_rdma_info->proto, &init_data);
-	if (rc)
-		goto err;
-
-	p_ramrod = &p_ent->ramrod.rdma_destroy_cq;
-	DMA_REGPAIR_LE(p_ramrod->output_params_addr, ramrod_res_phys);
-
-	rc = qed_spq_post(p_hwfn, p_ent, NULL);
-	if (rc)
-		goto err;
-
-	out_params->num_cq_notif = le16_to_cpu(p_ramrod_res->cnq_num);
-
-	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
-			  sizeof(struct rdma_destroy_cq_output_params),
-			  p_ramrod_res, ramrod_res_phys);
-
-	/* Free icid */
-	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
-
-	qed_bmap_release_id(p_hwfn,
-			    &p_hwfn->p_rdma_info->cq_map,
-			    (in_params->icid -
-			     qed_cxt_get_proto_cid_start(p_hwfn,
-							 p_hwfn->
-							 p_rdma_info->proto)));
-
-	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Destroyed CQ, rc = %d\n", rc);
-	return rc;
-
-err:	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
-			  sizeof(struct rdma_destroy_cq_output_params),
-			  p_ramrod_res, ramrod_res_phys);
-
-	return rc;
-}
-
-static void qed_rdma_set_fw_mac(u16 *p_fw_mac, u8 *p_qed_mac)
-{
-	p_fw_mac[0] = cpu_to_le16((p_qed_mac[0] << 8) + p_qed_mac[1]);
-	p_fw_mac[1] = cpu_to_le16((p_qed_mac[2] << 8) + p_qed_mac[3]);
-	p_fw_mac[2] = cpu_to_le16((p_qed_mac[4] << 8) + p_qed_mac[5]);
+	qed_spq_unregister_async_cb(p_hwfn, PROTOCOLID_ROCE);
 }
 
 static void qed_rdma_copy_gids(struct qed_rdma_qp *qp, __le32 *src_gid,
@@ -1210,7 +156,7 @@ void qed_roce_free_cid_pair(struct qed_hwfn *p_hwfn, u16 cid)
 	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
 }
 
-static int qed_roce_alloc_cid(struct qed_hwfn *p_hwfn, u16 *cid)
+int qed_roce_alloc_cid(struct qed_hwfn *p_hwfn, u16 *cid)
 {
 	struct qed_rdma_info *p_rdma_info = p_hwfn->p_rdma_info;
 	u32 responder_icid;
@@ -1870,9 +816,9 @@ err:
 	return rc;
 }
 
-static int qed_roce_query_qp(struct qed_hwfn *p_hwfn,
-			     struct qed_rdma_qp *qp,
-			     struct qed_rdma_query_qp_out_params *out_params)
+int qed_roce_query_qp(struct qed_hwfn *p_hwfn,
+		      struct qed_rdma_qp *qp,
+		      struct qed_rdma_query_qp_out_params *out_params)
 {
 	struct roce_query_qp_resp_output_params *p_resp_ramrod_res;
 	struct roce_query_qp_req_output_params *p_req_ramrod_res;
@@ -2011,7 +957,7 @@ err_resp:
 	return rc;
 }
 
-static int qed_roce_destroy_qp(struct qed_hwfn *p_hwfn, struct qed_rdma_qp *qp)
+int qed_roce_destroy_qp(struct qed_hwfn *p_hwfn, struct qed_rdma_qp *qp)
 {
 	u32 num_invalidated_mw = 0;
 	u32 num_bound_mw = 0;
@@ -2050,138 +996,10 @@ static int qed_roce_destroy_qp(struct qed_hwfn *p_hwfn, struct qed_rdma_qp *qp)
 	return 0;
 }
 
-static int qed_rdma_query_qp(void *rdma_cxt,
-			     struct qed_rdma_qp *qp,
-			     struct qed_rdma_query_qp_out_params *out_params)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	int rc;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", qp->icid);
-
-	/* The following fields are filled in from qp and not FW as they can't
-	 * be modified by FW
-	 */
-	out_params->mtu = qp->mtu;
-	out_params->dest_qp = qp->dest_qp;
-	out_params->incoming_atomic_en = qp->incoming_atomic_en;
-	out_params->e2e_flow_control_en = qp->e2e_flow_control_en;
-	out_params->incoming_rdma_read_en = qp->incoming_rdma_read_en;
-	out_params->incoming_rdma_write_en = qp->incoming_rdma_write_en;
-	out_params->dgid = qp->dgid;
-	out_params->flow_label = qp->flow_label;
-	out_params->hop_limit_ttl = qp->hop_limit_ttl;
-	out_params->traffic_class_tos = qp->traffic_class_tos;
-	out_params->timeout = qp->ack_timeout;
-	out_params->rnr_retry = qp->rnr_retry_cnt;
-	out_params->retry_cnt = qp->retry_cnt;
-	out_params->min_rnr_nak_timer = qp->min_rnr_nak_timer;
-	out_params->pkey_index = 0;
-	out_params->max_rd_atomic = qp->max_rd_atomic_req;
-	out_params->max_dest_rd_atomic = qp->max_rd_atomic_resp;
-	out_params->sqd_async = qp->sqd_async;
-
-	rc = qed_roce_query_qp(p_hwfn, qp, out_params);
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Query QP, rc = %d\n", rc);
-	return rc;
-}
-
-static int qed_rdma_destroy_qp(void *rdma_cxt, struct qed_rdma_qp *qp)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	int rc = 0;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x\n", qp->icid);
-
-	rc = qed_roce_destroy_qp(p_hwfn, qp);
-
-	/* free qp params struct */
-	kfree(qp);
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "QP destroyed\n");
-	return rc;
-}
-
-static struct qed_rdma_qp *
-qed_rdma_create_qp(void *rdma_cxt,
-		   struct qed_rdma_create_qp_in_params *in_params,
-		   struct qed_rdma_create_qp_out_params *out_params)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	struct qed_rdma_qp *qp;
-	u8 max_stats_queues;
-	int rc;
-
-	if (!rdma_cxt || !in_params || !out_params || !p_hwfn->p_rdma_info) {
-		DP_ERR(p_hwfn->cdev,
-		       "qed roce create qp failed due to NULL entry (rdma_cxt=%p, in=%p, out=%p, roce_info=?\n",
-		       rdma_cxt, in_params, out_params);
-		return NULL;
-	}
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
-		   "qed rdma create qp called with qp_handle = %08x%08x\n",
-		   in_params->qp_handle_hi, in_params->qp_handle_lo);
-
-	/* Some sanity checks... */
-	max_stats_queues = p_hwfn->p_rdma_info->dev->max_stats_queues;
-	if (in_params->stats_queue >= max_stats_queues) {
-		DP_ERR(p_hwfn->cdev,
-		       "qed rdma create qp failed due to invalid statistics queue %d. maximum is %d\n",
-		       in_params->stats_queue, max_stats_queues);
-		return NULL;
-	}
-
-	qp = kzalloc(sizeof(*qp), GFP_KERNEL);
-	if (!qp) {
-		DP_NOTICE(p_hwfn, "Failed to allocate qed_rdma_qp\n");
-		return NULL;
-	}
-
-	rc = qed_roce_alloc_cid(p_hwfn, &qp->icid);
-	qp->qpid = ((0xFF << 16) | qp->icid);
-
-	DP_INFO(p_hwfn, "ROCE qpid=%x\n", qp->qpid);
-
-	if (rc) {
-		kfree(qp);
-		return NULL;
-	}
-
-	qp->cur_state = QED_ROCE_QP_STATE_RESET;
-	qp->qp_handle.hi = cpu_to_le32(in_params->qp_handle_hi);
-	qp->qp_handle.lo = cpu_to_le32(in_params->qp_handle_lo);
-	qp->qp_handle_async.hi = cpu_to_le32(in_params->qp_handle_async_hi);
-	qp->qp_handle_async.lo = cpu_to_le32(in_params->qp_handle_async_lo);
-	qp->use_srq = in_params->use_srq;
-	qp->signal_all = in_params->signal_all;
-	qp->fmr_and_reserved_lkey = in_params->fmr_and_reserved_lkey;
-	qp->pd = in_params->pd;
-	qp->dpi = in_params->dpi;
-	qp->sq_cq_id = in_params->sq_cq_id;
-	qp->sq_num_pages = in_params->sq_num_pages;
-	qp->sq_pbl_ptr = in_params->sq_pbl_ptr;
-	qp->rq_cq_id = in_params->rq_cq_id;
-	qp->rq_num_pages = in_params->rq_num_pages;
-	qp->rq_pbl_ptr = in_params->rq_pbl_ptr;
-	qp->srq_id = in_params->srq_id;
-	qp->req_offloaded = false;
-	qp->resp_offloaded = false;
-	qp->e2e_flow_control_en = qp->use_srq ? false : true;
-	qp->stats_queue = in_params->stats_queue;
-
-	out_params->icid = qp->icid;
-	out_params->qp_id = qp->qpid;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Create QP, rc = %d\n", rc);
-	return qp;
-}
-
-static int qed_roce_modify_qp(struct qed_hwfn *p_hwfn,
-			      struct qed_rdma_qp *qp,
-			      enum qed_roce_qp_state prev_state,
-			      struct qed_rdma_modify_qp_in_params *params)
+int qed_roce_modify_qp(struct qed_hwfn *p_hwfn,
+		       struct qed_rdma_qp *qp,
+		       enum qed_roce_qp_state prev_state,
+		       struct qed_rdma_modify_qp_in_params *params)
 {
 	u32 num_invalidated_mw = 0, num_bound_mw = 0;
 	int rc = 0;
@@ -2286,331 +1104,6 @@ static int qed_roce_modify_qp(struct qed_hwfn *p_hwfn,
 	return rc;
 }
 
-static int qed_rdma_modify_qp(void *rdma_cxt,
-			      struct qed_rdma_qp *qp,
-			      struct qed_rdma_modify_qp_in_params *params)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	enum qed_roce_qp_state prev_state;
-	int rc = 0;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "icid = %08x params->new_state=%d\n",
-		   qp->icid, params->new_state);
-
-	if (rc) {
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
-		return rc;
-	}
-
-	if (GET_FIELD(params->modify_flags,
-		      QED_RDMA_MODIFY_QP_VALID_RDMA_OPS_EN)) {
-		qp->incoming_rdma_read_en = params->incoming_rdma_read_en;
-		qp->incoming_rdma_write_en = params->incoming_rdma_write_en;
-		qp->incoming_atomic_en = params->incoming_atomic_en;
-	}
-
-	/* Update QP structure with the updated values */
-	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_ROCE_MODE))
-		qp->roce_mode = params->roce_mode;
-	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_PKEY))
-		qp->pkey = params->pkey;
-	if (GET_FIELD(params->modify_flags,
-		      QED_ROCE_MODIFY_QP_VALID_E2E_FLOW_CONTROL_EN))
-		qp->e2e_flow_control_en = params->e2e_flow_control_en;
-	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_DEST_QP))
-		qp->dest_qp = params->dest_qp;
-	if (GET_FIELD(params->modify_flags,
-		      QED_ROCE_MODIFY_QP_VALID_ADDRESS_VECTOR)) {
-		/* Indicates that the following parameters have changed:
-		 * Traffic class, flow label, hop limit, source GID,
-		 * destination GID, loopback indicator
-		 */
-		qp->traffic_class_tos = params->traffic_class_tos;
-		qp->flow_label = params->flow_label;
-		qp->hop_limit_ttl = params->hop_limit_ttl;
-
-		qp->sgid = params->sgid;
-		qp->dgid = params->dgid;
-		qp->udp_src_port = 0;
-		qp->vlan_id = params->vlan_id;
-		qp->mtu = params->mtu;
-		qp->lb_indication = params->lb_indication;
-		memcpy((u8 *)&qp->remote_mac_addr[0],
-		       (u8 *)&params->remote_mac_addr[0], ETH_ALEN);
-		if (params->use_local_mac) {
-			memcpy((u8 *)&qp->local_mac_addr[0],
-			       (u8 *)&params->local_mac_addr[0], ETH_ALEN);
-		} else {
-			memcpy((u8 *)&qp->local_mac_addr[0],
-			       (u8 *)&p_hwfn->hw_info.hw_mac_addr, ETH_ALEN);
-		}
-	}
-	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_RQ_PSN))
-		qp->rq_psn = params->rq_psn;
-	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_SQ_PSN))
-		qp->sq_psn = params->sq_psn;
-	if (GET_FIELD(params->modify_flags,
-		      QED_RDMA_MODIFY_QP_VALID_MAX_RD_ATOMIC_REQ))
-		qp->max_rd_atomic_req = params->max_rd_atomic_req;
-	if (GET_FIELD(params->modify_flags,
-		      QED_RDMA_MODIFY_QP_VALID_MAX_RD_ATOMIC_RESP))
-		qp->max_rd_atomic_resp = params->max_rd_atomic_resp;
-	if (GET_FIELD(params->modify_flags,
-		      QED_ROCE_MODIFY_QP_VALID_ACK_TIMEOUT))
-		qp->ack_timeout = params->ack_timeout;
-	if (GET_FIELD(params->modify_flags, QED_ROCE_MODIFY_QP_VALID_RETRY_CNT))
-		qp->retry_cnt = params->retry_cnt;
-	if (GET_FIELD(params->modify_flags,
-		      QED_ROCE_MODIFY_QP_VALID_RNR_RETRY_CNT))
-		qp->rnr_retry_cnt = params->rnr_retry_cnt;
-	if (GET_FIELD(params->modify_flags,
-		      QED_ROCE_MODIFY_QP_VALID_MIN_RNR_NAK_TIMER))
-		qp->min_rnr_nak_timer = params->min_rnr_nak_timer;
-
-	qp->sqd_async = params->sqd_async;
-
-	prev_state = qp->cur_state;
-	if (GET_FIELD(params->modify_flags,
-		      QED_RDMA_MODIFY_QP_VALID_NEW_STATE)) {
-		qp->cur_state = params->new_state;
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "qp->cur_state=%d\n",
-			   qp->cur_state);
-	}
-
-	rc = qed_roce_modify_qp(p_hwfn, qp, prev_state, params);
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Modify QP, rc = %d\n", rc);
-	return rc;
-}
-
-static int
-qed_rdma_register_tid(void *rdma_cxt,
-		      struct qed_rdma_register_tid_in_params *params)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	struct rdma_register_tid_ramrod_data *p_ramrod;
-	struct qed_sp_init_data init_data;
-	struct qed_spq_entry *p_ent;
-	enum rdma_tid_type tid_type;
-	u8 fw_return_code;
-	int rc;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "itid = %08x\n", params->itid);
-
-	/* Get SPQ entry */
-	memset(&init_data, 0, sizeof(init_data));
-	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
-	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
-
-	rc = qed_sp_init_request(p_hwfn, &p_ent, RDMA_RAMROD_REGISTER_MR,
-				 p_hwfn->p_rdma_info->proto, &init_data);
-	if (rc) {
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
-		return rc;
-	}
-
-	if (p_hwfn->p_rdma_info->last_tid < params->itid)
-		p_hwfn->p_rdma_info->last_tid = params->itid;
-
-	p_ramrod = &p_ent->ramrod.rdma_register_tid;
-
-	p_ramrod->flags = 0;
-	SET_FIELD(p_ramrod->flags,
-		  RDMA_REGISTER_TID_RAMROD_DATA_TWO_LEVEL_PBL,
-		  params->pbl_two_level);
-
-	SET_FIELD(p_ramrod->flags,
-		  RDMA_REGISTER_TID_RAMROD_DATA_ZERO_BASED, params->zbva);
-
-	SET_FIELD(p_ramrod->flags,
-		  RDMA_REGISTER_TID_RAMROD_DATA_PHY_MR, params->phy_mr);
-
-	/* Don't initialize D/C field, as it may override other bits. */
-	if (!(params->tid_type == QED_RDMA_TID_FMR) && !(params->dma_mr))
-		SET_FIELD(p_ramrod->flags,
-			  RDMA_REGISTER_TID_RAMROD_DATA_PAGE_SIZE_LOG,
-			  params->page_size_log - 12);
-
-	SET_FIELD(p_ramrod->flags,
-		  RDMA_REGISTER_TID_RAMROD_DATA_MAX_ID,
-		  p_hwfn->p_rdma_info->last_tid);
-
-	SET_FIELD(p_ramrod->flags,
-		  RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_READ,
-		  params->remote_read);
-
-	SET_FIELD(p_ramrod->flags,
-		  RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_WRITE,
-		  params->remote_write);
-
-	SET_FIELD(p_ramrod->flags,
-		  RDMA_REGISTER_TID_RAMROD_DATA_REMOTE_ATOMIC,
-		  params->remote_atomic);
-
-	SET_FIELD(p_ramrod->flags,
-		  RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_WRITE,
-		  params->local_write);
-
-	SET_FIELD(p_ramrod->flags,
-		  RDMA_REGISTER_TID_RAMROD_DATA_LOCAL_READ, params->local_read);
-
-	SET_FIELD(p_ramrod->flags,
-		  RDMA_REGISTER_TID_RAMROD_DATA_ENABLE_MW_BIND,
-		  params->mw_bind);
-
-	SET_FIELD(p_ramrod->flags1,
-		  RDMA_REGISTER_TID_RAMROD_DATA_PBL_PAGE_SIZE_LOG,
-		  params->pbl_page_size_log - 12);
-
-	SET_FIELD(p_ramrod->flags2,
-		  RDMA_REGISTER_TID_RAMROD_DATA_DMA_MR, params->dma_mr);
-
-	switch (params->tid_type) {
-	case QED_RDMA_TID_REGISTERED_MR:
-		tid_type = RDMA_TID_REGISTERED_MR;
-		break;
-	case QED_RDMA_TID_FMR:
-		tid_type = RDMA_TID_FMR;
-		break;
-	case QED_RDMA_TID_MW_TYPE1:
-		tid_type = RDMA_TID_MW_TYPE1;
-		break;
-	case QED_RDMA_TID_MW_TYPE2A:
-		tid_type = RDMA_TID_MW_TYPE2A;
-		break;
-	default:
-		rc = -EINVAL;
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
-		return rc;
-	}
-	SET_FIELD(p_ramrod->flags1,
-		  RDMA_REGISTER_TID_RAMROD_DATA_TID_TYPE, tid_type);
-
-	p_ramrod->itid = cpu_to_le32(params->itid);
-	p_ramrod->key = params->key;
-	p_ramrod->pd = cpu_to_le16(params->pd);
-	p_ramrod->length_hi = (u8)(params->length >> 32);
-	p_ramrod->length_lo = DMA_LO_LE(params->length);
-	if (params->zbva) {
-		/* Lower 32 bits of the registered MR address.
-		 * In case of zero based MR, will hold FBO
-		 */
-		p_ramrod->va.hi = 0;
-		p_ramrod->va.lo = cpu_to_le32(params->fbo);
-	} else {
-		DMA_REGPAIR_LE(p_ramrod->va, params->vaddr);
-	}
-	DMA_REGPAIR_LE(p_ramrod->pbl_base, params->pbl_ptr);
-
-	/* DIF */
-	if (params->dif_enabled) {
-		SET_FIELD(p_ramrod->flags2,
-			  RDMA_REGISTER_TID_RAMROD_DATA_DIF_ON_HOST_FLG, 1);
-		DMA_REGPAIR_LE(p_ramrod->dif_error_addr,
-			       params->dif_error_addr);
-		DMA_REGPAIR_LE(p_ramrod->dif_runt_addr, params->dif_runt_addr);
-	}
-
-	rc = qed_spq_post(p_hwfn, p_ent, &fw_return_code);
-	if (rc)
-		return rc;
-
-	if (fw_return_code != RDMA_RETURN_OK) {
-		DP_NOTICE(p_hwfn, "fw_return_code = %d\n", fw_return_code);
-		return -EINVAL;
-	}
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Register TID, rc = %d\n", rc);
-	return rc;
-}
-
-static int qed_rdma_deregister_tid(void *rdma_cxt, u32 itid)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	struct rdma_deregister_tid_ramrod_data *p_ramrod;
-	struct qed_sp_init_data init_data;
-	struct qed_spq_entry *p_ent;
-	struct qed_ptt *p_ptt;
-	u8 fw_return_code;
-	int rc;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "itid = %08x\n", itid);
-
-	/* Get SPQ entry */
-	memset(&init_data, 0, sizeof(init_data));
-	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
-	init_data.comp_mode = QED_SPQ_MODE_EBLOCK;
-
-	rc = qed_sp_init_request(p_hwfn, &p_ent, RDMA_RAMROD_DEREGISTER_MR,
-				 p_hwfn->p_rdma_info->proto, &init_data);
-	if (rc) {
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
-		return rc;
-	}
-
-	p_ramrod = &p_ent->ramrod.rdma_deregister_tid;
-	p_ramrod->itid = cpu_to_le32(itid);
-
-	rc = qed_spq_post(p_hwfn, p_ent, &fw_return_code);
-	if (rc) {
-		DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "rc = %d\n", rc);
-		return rc;
-	}
-
-	if (fw_return_code == RDMA_RETURN_DEREGISTER_MR_BAD_STATE_ERR) {
-		DP_NOTICE(p_hwfn, "fw_return_code = %d\n", fw_return_code);
-		return -EINVAL;
-	} else if (fw_return_code == RDMA_RETURN_NIG_DRAIN_REQ) {
-		/* Bit indicating that the TID is in use and a nig drain is
-		 * required before sending the ramrod again
-		 */
-		p_ptt = qed_ptt_acquire(p_hwfn);
-		if (!p_ptt) {
-			rc = -EBUSY;
-			DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
-				   "Failed to acquire PTT\n");
-			return rc;
-		}
-
-		rc = qed_mcp_drain(p_hwfn, p_ptt);
-		if (rc) {
-			qed_ptt_release(p_hwfn, p_ptt);
-			DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
-				   "Drain failed\n");
-			return rc;
-		}
-
-		qed_ptt_release(p_hwfn, p_ptt);
-
-		/* Resend the ramrod */
-		rc = qed_sp_init_request(p_hwfn, &p_ent,
-					 RDMA_RAMROD_DEREGISTER_MR,
-					 p_hwfn->p_rdma_info->proto,
-					 &init_data);
-		if (rc) {
-			DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
-				   "Failed to init sp-element\n");
-			return rc;
-		}
-
-		rc = qed_spq_post(p_hwfn, p_ent, &fw_return_code);
-		if (rc) {
-			DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
-				   "Ramrod failed\n");
-			return rc;
-		}
-
-		if (fw_return_code != RDMA_RETURN_OK) {
-			DP_NOTICE(p_hwfn, "fw_return_code = %d\n",
-				  fw_return_code);
-			return rc;
-		}
-	}
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "De-registered TID, rc = %d\n", rc);
-	return rc;
-}
-
 static void qed_roce_free_real_icid(struct qed_hwfn *p_hwfn, u16 icid)
 {
 	struct qed_rdma_info *p_rdma_info = p_hwfn->p_rdma_info;
@@ -2636,414 +1129,43 @@ static void qed_roce_free_real_icid(struct qed_hwfn *p_hwfn, u16 icid)
 	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
 }
 
-static void *qed_rdma_get_rdma_ctx(struct qed_dev *cdev)
+void qed_roce_dpm_dcbx(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
-	return QED_LEADING_HWFN(cdev);
-}
+	u8 val;
 
-static void qed_rdma_dpm_conf(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
-{
-	u32 val;
-
-	val = (p_hwfn->dcbx_no_edpm || p_hwfn->db_bar_no_edpm) ? 0 : 1;
-
-	qed_wr(p_hwfn, p_ptt, DORQ_REG_PF_DPM_ENABLE, val);
-	DP_VERBOSE(p_hwfn, (QED_MSG_DCB | QED_MSG_RDMA),
-		   "Changing DPM_EN state to %d (DCBX=%d, DB_BAR=%d)\n",
-		   val, p_hwfn->dcbx_no_edpm, p_hwfn->db_bar_no_edpm);
-}
-
-void qed_rdma_dpm_bar(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
-{
-	p_hwfn->db_bar_no_edpm = true;
+	/* if any QPs are already active, we want to disable DPM, since their
+	 * context information contains information from before the latest DCBx
+	 * update. Otherwise enable it.
+	 */
+	val = qed_rdma_allocated_qps(p_hwfn) ? true : false;
+	p_hwfn->dcbx_no_edpm = (u8)val;
 
 	qed_rdma_dpm_conf(p_hwfn, p_ptt);
 }
 
-static int qed_rdma_start(void *rdma_cxt,
-			  struct qed_rdma_start_in_params *params)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-	struct qed_ptt *p_ptt;
-	int rc = -EBUSY;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA,
-		   "desired_cnq = %08x\n", params->desired_cnq);
-
-	p_ptt = qed_ptt_acquire(p_hwfn);
-	if (!p_ptt)
-		goto err;
-
-	rc = qed_rdma_alloc(p_hwfn, p_ptt, params);
-	if (rc)
-		goto err1;
-
-	rc = qed_rdma_setup(p_hwfn, p_ptt, params);
-	if (rc)
-		goto err2;
-
-	qed_ptt_release(p_hwfn, p_ptt);
-
-	return rc;
-
-err2:
-	qed_rdma_free(p_hwfn);
-err1:
-	qed_ptt_release(p_hwfn, p_ptt);
-err:
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "RDMA start - error, rc = %d\n", rc);
-	return rc;
-}
-
-static int qed_rdma_init(struct qed_dev *cdev,
-			 struct qed_rdma_start_in_params *params)
+int qed_roce_setup(struct qed_hwfn *p_hwfn)
 {
-	return qed_rdma_start(QED_LEADING_HWFN(cdev), params);
-}
-
-static void qed_rdma_remove_user(void *rdma_cxt, u16 dpi)
-{
-	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
-
-	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "dpi = %08x\n", dpi);
-
-	spin_lock_bh(&p_hwfn->p_rdma_info->lock);
-	qed_bmap_release_id(p_hwfn, &p_hwfn->p_rdma_info->dpi_map, dpi);
-	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
-}
-
-void qed_ll2b_complete_tx_gsi_packet(struct qed_hwfn *p_hwfn,
-				     u8 connection_handle,
-				     void *cookie,
-				     dma_addr_t first_frag_addr,
-				     bool b_last_fragment, bool b_last_packet)
-{
-	struct qed_roce_ll2_packet *packet = cookie;
-	struct qed_roce_ll2_info *roce_ll2 = p_hwfn->ll2;
-
-	roce_ll2->cbs.tx_cb(roce_ll2->cb_cookie, packet);
+	return qed_spq_register_async_cb(p_hwfn, PROTOCOLID_ROCE,
+					 qed_roce_async_event);
 }
 
-void qed_ll2b_release_tx_gsi_packet(struct qed_hwfn *p_hwfn,
-				    u8 connection_handle,
-				    void *cookie,
-				    dma_addr_t first_frag_addr,
-				    bool b_last_fragment, bool b_last_packet)
+int qed_roce_init_hw(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
 {
-	qed_ll2b_complete_tx_gsi_packet(p_hwfn, connection_handle,
-					cookie, first_frag_addr,
-					b_last_fragment, b_last_packet);
-}
-
-void qed_ll2b_complete_rx_gsi_packet(struct qed_hwfn *p_hwfn,
-				     u8 connection_handle,
-				     void *cookie,
-				     dma_addr_t rx_buf_addr,
-				     u16 data_length,
-				     u8 data_length_error,
-				     u16 parse_flags,
-				     u16 vlan,
-				     u32 src_mac_addr_hi,
-				     u16 src_mac_addr_lo, bool b_last_packet)
-{
-	struct qed_roce_ll2_info *roce_ll2 = p_hwfn->ll2;
-	struct qed_roce_ll2_rx_params params;
-	struct qed_dev *cdev = p_hwfn->cdev;
-	struct qed_roce_ll2_packet pkt;
-
-	DP_VERBOSE(cdev,
-		   QED_MSG_LL2,
-		   "roce ll2 rx complete: bus_addr=%p, len=%d, data_len_err=%d\n",
-		   (void *)(uintptr_t)rx_buf_addr,
-		   data_length, data_length_error);
-
-	memset(&pkt, 0, sizeof(pkt));
-	pkt.n_seg = 1;
-	pkt.payload[0].baddr = rx_buf_addr;
-	pkt.payload[0].len = data_length;
-
-	memset(&params, 0, sizeof(params));
-	params.vlan_id = vlan;
-	*((u32 *)&params.smac[0]) = ntohl(src_mac_addr_hi);
-	*((u16 *)&params.smac[4]) = ntohs(src_mac_addr_lo);
-
-	if (data_length_error) {
-		DP_ERR(cdev,
-		       "roce ll2 rx complete: data length error %d, length=%d\n",
-		       data_length_error, data_length);
-		params.rc = -EINVAL;
-	}
-
-	roce_ll2->cbs.rx_cb(roce_ll2->cb_cookie, &pkt, &params);
-}
-
-static int qed_roce_ll2_set_mac_filter(struct qed_dev *cdev,
-				       u8 *old_mac_address,
-				       u8 *new_mac_address)
-{
-	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
-	struct qed_ptt *p_ptt;
-	int rc = 0;
-
-	if (!hwfn->ll2 || hwfn->ll2->handle == QED_LL2_UNUSED_HANDLE) {
-		DP_ERR(cdev,
-		       "qed roce mac filter failed - roce_info/ll2 NULL\n");
-		return -EINVAL;
-	}
-
-	p_ptt = qed_ptt_acquire(QED_LEADING_HWFN(cdev));
-	if (!p_ptt) {
-		DP_ERR(cdev,
-		       "qed roce ll2 mac filter set: failed to acquire PTT\n");
-		return -EINVAL;
-	}
-
-	mutex_lock(&hwfn->ll2->lock);
-	if (old_mac_address)
-		qed_llh_remove_mac_filter(QED_LEADING_HWFN(cdev), p_ptt,
-					  old_mac_address);
-	if (new_mac_address)
-		rc = qed_llh_add_mac_filter(QED_LEADING_HWFN(cdev), p_ptt,
-					    new_mac_address);
-	mutex_unlock(&hwfn->ll2->lock);
-
-	qed_ptt_release(QED_LEADING_HWFN(cdev), p_ptt);
-
-	if (rc)
-		DP_ERR(cdev,
-		       "qed roce ll2 mac filter set: failed to add mac filter\n");
-
-	return rc;
-}
-
-static int qed_roce_ll2_start(struct qed_dev *cdev,
-			      struct qed_roce_ll2_params *params)
-{
-	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
-	struct qed_roce_ll2_info *roce_ll2;
-	struct qed_ll2_conn ll2_params;
-	int rc;
-
-	if (!params) {
-		DP_ERR(cdev, "qed roce ll2 start: failed due to NULL params\n");
-		return -EINVAL;
-	}
-	if (!params->cbs.tx_cb || !params->cbs.rx_cb) {
-		DP_ERR(cdev,
-		       "qed roce ll2 start: failed due to NULL tx/rx. tx_cb=%p, rx_cb=%p\n",
-		       params->cbs.tx_cb, params->cbs.rx_cb);
-		return -EINVAL;
-	}
-	if (!is_valid_ether_addr(params->mac_address)) {
-		DP_ERR(cdev,
-		       "qed roce ll2 start: failed due to invalid Ethernet address %pM\n",
-		       params->mac_address);
-		return -EINVAL;
-	}
-
-	/* Initialize */
-	roce_ll2 = kzalloc(sizeof(*roce_ll2), GFP_ATOMIC);
-	if (!roce_ll2) {
-		DP_ERR(cdev, "qed roce ll2 start: failed memory allocation\n");
-		return -ENOMEM;
-	}
-	roce_ll2->handle = QED_LL2_UNUSED_HANDLE;
-	roce_ll2->cbs = params->cbs;
-	roce_ll2->cb_cookie = params->cb_cookie;
-	mutex_init(&roce_ll2->lock);
-
-	memset(&ll2_params, 0, sizeof(ll2_params));
-	ll2_params.conn_type = QED_LL2_TYPE_ROCE;
-	ll2_params.mtu = params->mtu;
-	ll2_params.rx_drop_ttl0_flg = true;
-	ll2_params.rx_vlan_removal_en = false;
-	ll2_params.tx_dest = CORE_TX_DEST_NW;
-	ll2_params.ai_err_packet_too_big = LL2_DROP_PACKET;
-	ll2_params.ai_err_no_buf = LL2_DROP_PACKET;
-	ll2_params.gsi_enable = true;
-
-	rc = qed_ll2_acquire_connection(QED_LEADING_HWFN(cdev), &ll2_params,
-					params->max_rx_buffers,
-					params->max_tx_buffers,
-					&roce_ll2->handle);
-	if (rc) {
-		DP_ERR(cdev,
-		       "qed roce ll2 start: failed to acquire LL2 connection (rc=%d)\n",
-		       rc);
-		goto err;
-	}
-
-	rc = qed_ll2_establish_connection(QED_LEADING_HWFN(cdev),
-					  roce_ll2->handle);
-	if (rc) {
-		DP_ERR(cdev,
-		       "qed roce ll2 start: failed to establish LL2 connection (rc=%d)\n",
-		       rc);
-		goto err1;
-	}
-
-	hwfn->ll2 = roce_ll2;
-
-	rc = qed_roce_ll2_set_mac_filter(cdev, NULL, params->mac_address);
-	if (rc) {
-		hwfn->ll2 = NULL;
-		goto err2;
-	}
-	ether_addr_copy(roce_ll2->mac_address, params->mac_address);
-
-	return 0;
-
-err2:
-	qed_ll2_terminate_connection(QED_LEADING_HWFN(cdev), roce_ll2->handle);
-err1:
-	qed_ll2_release_connection(QED_LEADING_HWFN(cdev), roce_ll2->handle);
-err:
-	kfree(roce_ll2);
-	return rc;
-}
-
-static int qed_roce_ll2_stop(struct qed_dev *cdev)
-{
-	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
-	struct qed_roce_ll2_info *roce_ll2 = hwfn->ll2;
-	int rc;
-
-	if (roce_ll2->handle == QED_LL2_UNUSED_HANDLE) {
-		DP_ERR(cdev, "qed roce ll2 stop: cannot stop an unused LL2\n");
-		return -EINVAL;
-	}
-
-	/* remove LL2 MAC address filter */
-	rc = qed_roce_ll2_set_mac_filter(cdev, roce_ll2->mac_address, NULL);
-	eth_zero_addr(roce_ll2->mac_address);
-
-	rc = qed_ll2_terminate_connection(QED_LEADING_HWFN(cdev),
-					  roce_ll2->handle);
-	if (rc)
-		DP_ERR(cdev,
-		       "qed roce ll2 stop: failed to terminate LL2 connection (rc=%d)\n",
-		       rc);
-
-	qed_ll2_release_connection(QED_LEADING_HWFN(cdev), roce_ll2->handle);
-
-	roce_ll2->handle = QED_LL2_UNUSED_HANDLE;
+	u32 ll2_ethertype_en;
 
-	kfree(roce_ll2);
+	qed_wr(p_hwfn, p_ptt, PRS_REG_ROCE_DEST_QP_MAX_PF, 0);
 
-	return rc;
-}
+	p_hwfn->rdma_prs_search_reg = PRS_REG_SEARCH_ROCE;
 
-static int qed_roce_ll2_tx(struct qed_dev *cdev,
-			   struct qed_roce_ll2_packet *pkt,
-			   struct qed_roce_ll2_tx_params *params)
-{
-	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
-	struct qed_roce_ll2_info *roce_ll2 = hwfn->ll2;
-	enum qed_ll2_roce_flavor_type qed_roce_flavor;
-	u8 flags = 0;
-	int rc;
-	int i;
+	ll2_ethertype_en = qed_rd(p_hwfn, p_ptt, PRS_REG_LIGHT_L2_ETHERTYPE_EN);
+	qed_wr(p_hwfn, p_ptt, PRS_REG_LIGHT_L2_ETHERTYPE_EN,
+	       (ll2_ethertype_en | 0x01));
 
-	if (!pkt || !params) {
-		DP_ERR(cdev,
-		       "roce ll2 tx: failed tx because one of the following is NULL - drv=%p, pkt=%p, params=%p\n",
-		       cdev, pkt, params);
+	if (qed_cxt_get_proto_cid_start(p_hwfn, PROTOCOLID_ROCE) % 2) {
+		DP_NOTICE(p_hwfn, "The first RoCE's cid should be even\n");
 		return -EINVAL;
 	}
 
-	qed_roce_flavor = (pkt->roce_mode == ROCE_V1) ? QED_LL2_ROCE
-						      : QED_LL2_RROCE;
-
-	if (pkt->roce_mode == ROCE_V2_IPV4)
-		flags |= BIT(CORE_TX_BD_DATA_IP_CSUM_SHIFT);
-
-	/* Tx header */
-	rc = qed_ll2_prepare_tx_packet(QED_LEADING_HWFN(cdev), roce_ll2->handle,
-				       1 + pkt->n_seg, 0, flags, 0,
-				       QED_LL2_TX_DEST_NW,
-				       qed_roce_flavor, pkt->header.baddr,
-				       pkt->header.len, pkt, 1);
-	if (rc) {
-		DP_ERR(cdev, "roce ll2 tx: header failed (rc=%d)\n", rc);
-		return QED_ROCE_TX_HEAD_FAILURE;
-	}
-
-	/* Tx payload */
-	for (i = 0; i < pkt->n_seg; i++) {
-		rc = qed_ll2_set_fragment_of_tx_packet(QED_LEADING_HWFN(cdev),
-						       roce_ll2->handle,
-						       pkt->payload[i].baddr,
-						       pkt->payload[i].len);
-		if (rc) {
-			/* If failed not much to do here, partial packet has
-			 * been posted * we can't free memory, will need to wait
-			 * for completion
-			 */
-			DP_ERR(cdev,
-			       "roce ll2 tx: payload failed (rc=%d)\n", rc);
-			return QED_ROCE_TX_FRAG_FAILURE;
-		}
-	}
-
+	DP_VERBOSE(p_hwfn, QED_MSG_RDMA, "Initializing HW - Done\n");
 	return 0;
 }
-
-static int qed_roce_ll2_post_rx_buffer(struct qed_dev *cdev,
-				       struct qed_roce_ll2_buffer *buf,
-				       u64 cookie, u8 notify_fw)
-{
-	return qed_ll2_post_rx_buffer(QED_LEADING_HWFN(cdev),
-				      QED_LEADING_HWFN(cdev)->ll2->handle,
-				      buf->baddr, buf->len,
-				      (void *)(uintptr_t)cookie, notify_fw);
-}
-
-static int qed_roce_ll2_stats(struct qed_dev *cdev, struct qed_ll2_stats *stats)
-{
-	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
-	struct qed_roce_ll2_info *roce_ll2 = hwfn->ll2;
-
-	return qed_ll2_get_stats(QED_LEADING_HWFN(cdev),
-				 roce_ll2->handle, stats);
-}
-
-static const struct qed_rdma_ops qed_rdma_ops_pass = {
-	.common = &qed_common_ops_pass,
-	.fill_dev_info = &qed_fill_rdma_dev_info,
-	.rdma_get_rdma_ctx = &qed_rdma_get_rdma_ctx,
-	.rdma_init = &qed_rdma_init,
-	.rdma_add_user = &qed_rdma_add_user,
-	.rdma_remove_user = &qed_rdma_remove_user,
-	.rdma_stop = &qed_rdma_stop,
-	.rdma_query_port = &qed_rdma_query_port,
-	.rdma_query_device = &qed_rdma_query_device,
-	.rdma_get_start_sb = &qed_rdma_get_sb_start,
-	.rdma_get_rdma_int = &qed_rdma_get_int,
-	.rdma_set_rdma_int = &qed_rdma_set_int,
-	.rdma_get_min_cnq_msix = &qed_rdma_get_min_cnq_msix,
-	.rdma_cnq_prod_update = &qed_rdma_cnq_prod_update,
-	.rdma_alloc_pd = &qed_rdma_alloc_pd,
-	.rdma_dealloc_pd = &qed_rdma_free_pd,
-	.rdma_create_cq = &qed_rdma_create_cq,
-	.rdma_destroy_cq = &qed_rdma_destroy_cq,
-	.rdma_create_qp = &qed_rdma_create_qp,
-	.rdma_modify_qp = &qed_rdma_modify_qp,
-	.rdma_query_qp = &qed_rdma_query_qp,
-	.rdma_destroy_qp = &qed_rdma_destroy_qp,
-	.rdma_alloc_tid = &qed_rdma_alloc_tid,
-	.rdma_free_tid = &qed_rdma_free_tid,
-	.rdma_register_tid = &qed_rdma_register_tid,
-	.rdma_deregister_tid = &qed_rdma_deregister_tid,
-	.roce_ll2_start = &qed_roce_ll2_start,
-	.roce_ll2_stop = &qed_roce_ll2_stop,
-	.roce_ll2_tx = &qed_roce_ll2_tx,
-	.roce_ll2_post_rx_buffer = &qed_roce_ll2_post_rx_buffer,
-	.roce_ll2_set_mac_filter = &qed_roce_ll2_set_mac_filter,
-	.roce_ll2_stats = &qed_roce_ll2_stats,
-};
-
-const struct qed_rdma_ops *qed_get_rdma_ops(void)
-{
-	return &qed_rdma_ops_pass;
-}
-EXPORT_SYMBOL(qed_get_rdma_ops);
diff --git a/drivers/net/ethernet/qlogic/qed/qed_roce.h b/drivers/net/ethernet/qlogic/qed/qed_roce.h
index 9742af516183..f801f39fde61 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_roce.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_roce.h
@@ -32,191 +32,28 @@
 #ifndef _QED_ROCE_H
 #define _QED_ROCE_H
 #include <linux/types.h>
-#include <linux/bitops.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
 #include <linux/slab.h>
-#include <linux/spinlock.h>
-#include <linux/qed/qed_if.h>
-#include <linux/qed/qed_roce_if.h>
-#include "qed.h"
-#include "qed_dev_api.h"
-#include "qed_hsi.h"
-#include "qed_ll2.h"
 
-#define QED_RDMA_MAX_FMR                    (RDMA_MAX_TIDS)
-#define QED_RDMA_MAX_P_KEY                  (1)
-#define QED_RDMA_MAX_WQE                    (0x7FFF)
-#define QED_RDMA_MAX_SRQ_WQE_ELEM           (0x7FFF)
-#define QED_RDMA_PAGE_SIZE_CAPS             (0xFFFFF000)
-#define QED_RDMA_ACK_DELAY                  (15)
-#define QED_RDMA_MAX_MR_SIZE                (0x10000000000ULL)
-#define QED_RDMA_MAX_CQS                    (RDMA_MAX_CQS)
-#define QED_RDMA_MAX_MRS                    (RDMA_MAX_TIDS)
-/* Add 1 for header element */
-#define QED_RDMA_MAX_SRQ_ELEM_PER_WQE	    (RDMA_MAX_SGE_PER_RQ_WQE + 1)
-#define QED_RDMA_MAX_SGE_PER_SRQ_WQE        (RDMA_MAX_SGE_PER_RQ_WQE)
-#define QED_RDMA_SRQ_WQE_ELEM_SIZE          (16)
-#define QED_RDMA_MAX_SRQS                   (32 * 1024)
-
-#define QED_RDMA_MAX_CQE_32_BIT             (0x7FFFFFFF - 1)
-#define QED_RDMA_MAX_CQE_16_BIT             (0x7FFF - 1)
-
-enum qed_rdma_toggle_bit {
-	QED_RDMA_TOGGLE_BIT_CLEAR = 0,
-	QED_RDMA_TOGGLE_BIT_SET = 1
-};
-
-#define QED_RDMA_MAX_BMAP_NAME	(10)
-struct qed_bmap {
-	unsigned long *bitmap;
-	u32 max_count;
-	char name[QED_RDMA_MAX_BMAP_NAME];
-};
-
-struct qed_rdma_info {
-	/* spin lock to protect bitmaps */
-	spinlock_t lock;
-
-	struct qed_bmap cq_map;
-	struct qed_bmap pd_map;
-	struct qed_bmap tid_map;
-	struct qed_bmap qp_map;
-	struct qed_bmap srq_map;
-	struct qed_bmap cid_map;
-	struct qed_bmap real_cid_map;
-	struct qed_bmap dpi_map;
-	struct qed_bmap toggle_bits;
-	struct qed_rdma_events events;
-	struct qed_rdma_device *dev;
-	struct qed_rdma_port *port;
-	u32 last_tid;
-	u8 num_cnqs;
-	u32 num_qps;
-	u32 num_mrs;
-	u16 queue_zone_base;
-	u16 max_queue_zones;
-	enum protocol_type proto;
-};
-
-struct qed_rdma_qp {
-	struct regpair qp_handle;
-	struct regpair qp_handle_async;
-	u32 qpid;
-	u16 icid;
-	enum qed_roce_qp_state cur_state;
-	bool use_srq;
-	bool signal_all;
-	bool fmr_and_reserved_lkey;
-
-	bool incoming_rdma_read_en;
-	bool incoming_rdma_write_en;
-	bool incoming_atomic_en;
-	bool e2e_flow_control_en;
-
-	u16 pd;
-	u16 pkey;
-	u32 dest_qp;
-	u16 mtu;
-	u16 srq_id;
-	u8 traffic_class_tos;
-	u8 hop_limit_ttl;
-	u16 dpi;
-	u32 flow_label;
-	bool lb_indication;
-	u16 vlan_id;
-	u32 ack_timeout;
-	u8 retry_cnt;
-	u8 rnr_retry_cnt;
-	u8 min_rnr_nak_timer;
-	bool sqd_async;
-	union qed_gid sgid;
-	union qed_gid dgid;
-	enum roce_mode roce_mode;
-	u16 udp_src_port;
-	u8 stats_queue;
-
-	/* requeseter */
-	u8 max_rd_atomic_req;
-	u32 sq_psn;
-	u16 sq_cq_id;
-	u16 sq_num_pages;
-	dma_addr_t sq_pbl_ptr;
-	void *orq;
-	dma_addr_t orq_phys_addr;
-	u8 orq_num_pages;
-	bool req_offloaded;
+#if IS_ENABLED(CONFIG_QED_RDMA)
+void qed_roce_dpm_dcbx(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
+#else
+static inline void qed_roce_dpm_dcbx(struct qed_hwfn *p_hwfn,
+				     struct qed_ptt *p_ptt) {}
+#endif
 
-	/* responder */
-	u8 max_rd_atomic_resp;
-	u32 rq_psn;
-	u16 rq_cq_id;
-	u16 rq_num_pages;
-	dma_addr_t rq_pbl_ptr;
-	void *irq;
-	dma_addr_t irq_phys_addr;
-	u8 irq_num_pages;
-	bool resp_offloaded;
-	u32 cq_prod;
+int qed_roce_setup(struct qed_hwfn *p_hwfn);
+void qed_roce_stop(struct qed_hwfn *p_hwfn);
+int qed_roce_init_hw(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
+int qed_roce_alloc_cid(struct qed_hwfn *p_hwfn, u16 *cid);
+int qed_roce_destroy_qp(struct qed_hwfn *p_hwfn, struct qed_rdma_qp *qp);
 
-	u8 remote_mac_addr[6];
-	u8 local_mac_addr[6];
+int qed_roce_query_qp(struct qed_hwfn *p_hwfn,
+		      struct qed_rdma_qp *qp,
+		      struct qed_rdma_query_qp_out_params *out_params);
 
-	void *shared_queue;
-	dma_addr_t shared_queue_phys_addr;
-};
+int qed_roce_modify_qp(struct qed_hwfn *p_hwfn,
+		       struct qed_rdma_qp *qp,
+		       enum qed_roce_qp_state prev_state,
+		       struct qed_rdma_modify_qp_in_params *params);
 
-#if IS_ENABLED(CONFIG_QED_RDMA)
-void qed_rdma_dpm_bar(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
-void qed_roce_async_event(struct qed_hwfn *p_hwfn,
-			  u8 fw_event_code, union rdma_eqe_data *rdma_data);
-void qed_ll2b_complete_tx_gsi_packet(struct qed_hwfn *p_hwfn,
-				     u8 connection_handle,
-				     void *cookie,
-				     dma_addr_t first_frag_addr,
-				     bool b_last_fragment, bool b_last_packet);
-void qed_ll2b_release_tx_gsi_packet(struct qed_hwfn *p_hwfn,
-				    u8 connection_handle,
-				    void *cookie,
-				    dma_addr_t first_frag_addr,
-				    bool b_last_fragment, bool b_last_packet);
-void qed_ll2b_complete_rx_gsi_packet(struct qed_hwfn *p_hwfn,
-				     u8 connection_handle,
-				     void *cookie,
-				     dma_addr_t rx_buf_addr,
-				     u16 data_length,
-				     u8 data_length_error,
-				     u16 parse_flags,
-				     u16 vlan,
-				     u32 src_mac_addr_hi,
-				     u16 src_mac_addr_lo, bool b_last_packet);
-#else
-static inline void qed_rdma_dpm_bar(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt) {}
-static inline void qed_roce_async_event(struct qed_hwfn *p_hwfn,
-					u8 fw_event_code,
-					union rdma_eqe_data *rdma_data) {}
-static inline void qed_ll2b_complete_tx_gsi_packet(struct qed_hwfn *p_hwfn,
-						   u8 connection_handle,
-						   void *cookie,
-						   dma_addr_t first_frag_addr,
-						   bool b_last_fragment,
-						   bool b_last_packet) {}
-static inline void qed_ll2b_release_tx_gsi_packet(struct qed_hwfn *p_hwfn,
-						  u8 connection_handle,
-						  void *cookie,
-						  dma_addr_t first_frag_addr,
-						  bool b_last_fragment,
-						  bool b_last_packet) {}
-static inline void qed_ll2b_complete_rx_gsi_packet(struct qed_hwfn *p_hwfn,
-						   u8 connection_handle,
-						   void *cookie,
-						   dma_addr_t rx_buf_addr,
-						   u16 data_length,
-						   u8 data_length_error,
-						   u16 parse_flags,
-						   u16 vlan,
-						   u32 src_mac_addr_hi,
-						   u16 src_mac_addr_lo,
-						   bool b_last_packet) {}
-#endif
 #endif
diff --git a/drivers/net/ethernet/qlogic/qed/qed_sp.h b/drivers/net/ethernet/qlogic/qed/qed_sp.h
index 3357bbefa445..ab4ad8a1e2a5 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_sp.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_sp.h
@@ -104,12 +104,17 @@ union ramrod_data {
 	struct roce_query_qp_req_ramrod_data roce_query_qp_req;
 	struct roce_destroy_qp_resp_ramrod_data roce_destroy_qp_resp;
 	struct roce_destroy_qp_req_ramrod_data roce_destroy_qp_req;
+	struct roce_init_func_ramrod_data roce_init_func;
 	struct rdma_create_cq_ramrod_data rdma_create_cq;
 	struct rdma_destroy_cq_ramrod_data rdma_destroy_cq;
 	struct rdma_srq_create_ramrod_data rdma_create_srq;
 	struct rdma_srq_destroy_ramrod_data rdma_destroy_srq;
 	struct rdma_srq_modify_ramrod_data rdma_modify_srq;
-	struct roce_init_func_ramrod_data roce_init_func;
+	struct iwarp_create_qp_ramrod_data iwarp_create_qp;
+	struct iwarp_tcp_offload_ramrod_data iwarp_tcp_offload;
+	struct iwarp_mpa_offload_ramrod_data iwarp_mpa_offload;
+	struct iwarp_modify_qp_ramrod_data iwarp_modify_qp;
+	struct iwarp_init_func_ramrod_data iwarp_init_func;
 	struct fcoe_init_ramrod_params fcoe_init;
 	struct fcoe_conn_offload_ramrod_params fcoe_conn_ofld;
 	struct fcoe_conn_terminate_ramrod_params fcoe_conn_terminate;
@@ -120,6 +125,7 @@ union ramrod_data {
 	struct iscsi_spe_func_dstry iscsi_destroy;
 	struct iscsi_spe_conn_offload iscsi_conn_offload;
 	struct iscsi_conn_update_ramrod_params iscsi_conn_update;
+	struct iscsi_spe_conn_mac_update iscsi_conn_mac_update;
 	struct iscsi_spe_conn_termination iscsi_conn_terminate;
 
 	struct vf_start_ramrod_data vf_start;
@@ -173,6 +179,22 @@ struct qed_consq {
 	struct qed_chain chain;
 };
 
+typedef int
+(*qed_spq_async_comp_cb)(struct qed_hwfn *p_hwfn,
+			 u8 opcode,
+			 u16 echo,
+			 union event_ring_data *data,
+			 u8 fw_return_code);
+
+int
+qed_spq_register_async_cb(struct qed_hwfn *p_hwfn,
+			  enum protocol_type protocol_id,
+			  qed_spq_async_comp_cb cb);
+
+void
+qed_spq_unregister_async_cb(struct qed_hwfn *p_hwfn,
+			    enum protocol_type protocol_id);
+
 struct qed_spq {
 	spinlock_t		lock; /* SPQ lock */
 
@@ -202,6 +224,7 @@ struct qed_spq {
 	u32			comp_count;
 
 	u32			cid;
+	qed_spq_async_comp_cb async_comp_cb[MAX_PROTOCOL_TYPE];
 };
 
 /**
@@ -270,28 +293,23 @@ void qed_spq_return_entry(struct qed_hwfn *p_hwfn,
  * @param p_hwfn
  * @param num_elem number of elements in the eq
  *
- * @return struct qed_eq* - a newly allocated structure; NULL upon error.
+ * @return int
  */
-struct qed_eq *qed_eq_alloc(struct qed_hwfn *p_hwfn,
-			    u16 num_elem);
+int qed_eq_alloc(struct qed_hwfn *p_hwfn, u16 num_elem);
 
 /**
- * @brief qed_eq_setup - Reset the SPQ to its start state.
+ * @brief qed_eq_setup - Reset the EQ to its start state.
  *
  * @param p_hwfn
- * @param p_eq
  */
-void qed_eq_setup(struct qed_hwfn *p_hwfn,
-		  struct qed_eq *p_eq);
+void qed_eq_setup(struct qed_hwfn *p_hwfn);
 
 /**
- * @brief qed_eq_deallocate - deallocates the given EQ struct.
+ * @brief qed_eq_free - deallocates the given EQ struct.
  *
  * @param p_hwfn
- * @param p_eq
  */
-void qed_eq_free(struct qed_hwfn *p_hwfn,
-		 struct qed_eq *p_eq);
+void qed_eq_free(struct qed_hwfn *p_hwfn);
 
 /**
  * @brief qed_eq_prod_update - update the FW with default EQ producer
@@ -342,28 +360,23 @@ u32 qed_spq_get_cid(struct qed_hwfn *p_hwfn);
  *
  * @param p_hwfn
  *
- * @return struct qed_eq* - a newly allocated structure; NULL upon error.
+ * @return int
  */
-struct qed_consq *qed_consq_alloc(struct qed_hwfn *p_hwfn);
+int qed_consq_alloc(struct qed_hwfn *p_hwfn);
 
 /**
- * @brief qed_consq_setup - Reset the ConsQ to its start
- *        state.
+ * @brief qed_consq_setup - Reset the ConsQ to its start state.
  *
  * @param p_hwfn
- * @param p_eq
  */
-void qed_consq_setup(struct qed_hwfn *p_hwfn,
-		     struct qed_consq *p_consq);
+void qed_consq_setup(struct qed_hwfn *p_hwfn);
 
 /**
  * @brief qed_consq_free - deallocates the given ConsQ struct.
  *
  * @param p_hwfn
- * @param p_eq
  */
-void qed_consq_free(struct qed_hwfn *p_hwfn,
-		    struct qed_consq *p_consq);
+void qed_consq_free(struct qed_hwfn *p_hwfn);
 
 /**
  * @file
@@ -401,6 +414,7 @@ int qed_sp_init_request(struct qed_hwfn *p_hwfn,
  * to the internal RAM of the UStorm by the Function Start Ramrod.
  *
  * @param p_hwfn
+ * @param p_ptt
  * @param p_tunn
  * @param mode
  * @param allow_npar_tx_switch
@@ -409,6 +423,7 @@ int qed_sp_init_request(struct qed_hwfn *p_hwfn,
  */
 
 int qed_sp_pf_start(struct qed_hwfn *p_hwfn,
+		    struct qed_ptt *p_ptt,
 		    struct qed_tunnel_info *p_tunn,
 		    enum qed_mf_mode mode, bool allow_npar_tx_switch);
 
@@ -426,6 +441,15 @@ int qed_sp_pf_start(struct qed_hwfn *p_hwfn,
 int qed_sp_pf_update(struct qed_hwfn *p_hwfn);
 
 /**
+ * @brief qed_sp_pf_update_stag - Update firmware of new outer tag
+ *
+ * @param p_hwfn
+ *
+ * @return int
+ */
+int qed_sp_pf_update_stag(struct qed_hwfn *p_hwfn);
+
+/**
  * @brief qed_sp_pf_stop - PF Function Stop Ramrod
  *
  * This ramrod is sent to close a Physical Function (PF). It is the last ramrod
@@ -442,6 +466,7 @@ int qed_sp_pf_update(struct qed_hwfn *p_hwfn);
 int qed_sp_pf_stop(struct qed_hwfn *p_hwfn);
 
 int qed_sp_pf_update_tunn_cfg(struct qed_hwfn *p_hwfn,
+			      struct qed_ptt *p_ptt,
 			      struct qed_tunnel_info *p_tunn,
 			      enum spq_mode comp_mode,
 			      struct qed_spq_comp_cb *p_comp_data);
diff --git a/drivers/net/ethernet/qlogic/qed/qed_sp_commands.c b/drivers/net/ethernet/qlogic/qed/qed_sp_commands.c
index bc3694e91b85..46d0c3cb83a5 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_sp_commands.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_sp_commands.c
@@ -185,22 +185,20 @@ static void qed_set_tunn_ports(struct qed_tunnel_info *p_tun,
 }
 
 static void
-__qed_set_ramrod_tunnel_param(u8 *p_tunn_cls, u8 *p_enable_tx_clas,
+__qed_set_ramrod_tunnel_param(u8 *p_tunn_cls,
 			      struct qed_tunn_update_type *tun_type)
 {
 	*p_tunn_cls = tun_type->tun_cls;
-
-	if (tun_type->b_mode_enabled)
-		*p_enable_tx_clas = 1;
 }
 
 static void
-qed_set_ramrod_tunnel_param(u8 *p_tunn_cls, u8 *p_enable_tx_clas,
+qed_set_ramrod_tunnel_param(u8 *p_tunn_cls,
 			    struct qed_tunn_update_type *tun_type,
-			    u8 *p_update_port, __le16 *p_port,
+			    u8 *p_update_port,
+			    __le16 *p_port,
 			    struct qed_tunn_update_udp_port *p_udp_port)
 {
-	__qed_set_ramrod_tunnel_param(p_tunn_cls, p_enable_tx_clas, tun_type);
+	__qed_set_ramrod_tunnel_param(p_tunn_cls, tun_type);
 	if (p_udp_port->b_update_port) {
 		*p_update_port = 1;
 		*p_port = cpu_to_le16(p_udp_port->port);
@@ -219,33 +217,27 @@ qed_tunn_set_pf_update_params(struct qed_hwfn *p_hwfn,
 	qed_set_tunn_ports(p_tun, p_src);
 
 	qed_set_ramrod_tunnel_param(&p_tunn_cfg->tunnel_clss_vxlan,
-				    &p_tunn_cfg->tx_enable_vxlan,
 				    &p_tun->vxlan,
 				    &p_tunn_cfg->set_vxlan_udp_port_flg,
 				    &p_tunn_cfg->vxlan_udp_port,
 				    &p_tun->vxlan_port);
 
 	qed_set_ramrod_tunnel_param(&p_tunn_cfg->tunnel_clss_l2geneve,
-				    &p_tunn_cfg->tx_enable_l2geneve,
 				    &p_tun->l2_geneve,
 				    &p_tunn_cfg->set_geneve_udp_port_flg,
 				    &p_tunn_cfg->geneve_udp_port,
 				    &p_tun->geneve_port);
 
 	__qed_set_ramrod_tunnel_param(&p_tunn_cfg->tunnel_clss_ipgeneve,
-				      &p_tunn_cfg->tx_enable_ipgeneve,
 				      &p_tun->ip_geneve);
 
 	__qed_set_ramrod_tunnel_param(&p_tunn_cfg->tunnel_clss_l2gre,
-				      &p_tunn_cfg->tx_enable_l2gre,
 				      &p_tun->l2_gre);
 
 	__qed_set_ramrod_tunnel_param(&p_tunn_cfg->tunnel_clss_ipgre,
-				      &p_tunn_cfg->tx_enable_ipgre,
 				      &p_tun->ip_gre);
 
 	p_tunn_cfg->update_rx_pf_clss = p_tun->b_update_rx_cls;
-	p_tunn_cfg->update_tx_pf_clss = p_tun->b_update_tx_cls;
 }
 
 static void qed_set_hw_tunn_mode(struct qed_hwfn *p_hwfn,
@@ -261,17 +253,18 @@ static void qed_set_hw_tunn_mode(struct qed_hwfn *p_hwfn,
 }
 
 static void qed_set_hw_tunn_mode_port(struct qed_hwfn *p_hwfn,
+				      struct qed_ptt *p_ptt,
 				      struct qed_tunnel_info *p_tunn)
 {
 	if (p_tunn->vxlan_port.b_update_port)
-		qed_set_vxlan_dest_port(p_hwfn, p_hwfn->p_main_ptt,
+		qed_set_vxlan_dest_port(p_hwfn, p_ptt,
 					p_tunn->vxlan_port.port);
 
 	if (p_tunn->geneve_port.b_update_port)
-		qed_set_geneve_dest_port(p_hwfn, p_hwfn->p_main_ptt,
+		qed_set_geneve_dest_port(p_hwfn, p_ptt,
 					 p_tunn->geneve_port.port);
 
-	qed_set_hw_tunn_mode(p_hwfn, p_hwfn->p_main_ptt, p_tunn);
+	qed_set_hw_tunn_mode(p_hwfn, p_ptt, p_tunn);
 }
 
 static void
@@ -289,33 +282,29 @@ qed_tunn_set_pf_start_params(struct qed_hwfn *p_hwfn,
 	qed_set_tunn_ports(p_tun, p_src);
 
 	qed_set_ramrod_tunnel_param(&p_tunn_cfg->tunnel_clss_vxlan,
-				    &p_tunn_cfg->tx_enable_vxlan,
 				    &p_tun->vxlan,
 				    &p_tunn_cfg->set_vxlan_udp_port_flg,
 				    &p_tunn_cfg->vxlan_udp_port,
 				    &p_tun->vxlan_port);
 
 	qed_set_ramrod_tunnel_param(&p_tunn_cfg->tunnel_clss_l2geneve,
-				    &p_tunn_cfg->tx_enable_l2geneve,
 				    &p_tun->l2_geneve,
 				    &p_tunn_cfg->set_geneve_udp_port_flg,
 				    &p_tunn_cfg->geneve_udp_port,
 				    &p_tun->geneve_port);
 
 	__qed_set_ramrod_tunnel_param(&p_tunn_cfg->tunnel_clss_ipgeneve,
-				      &p_tunn_cfg->tx_enable_ipgeneve,
 				      &p_tun->ip_geneve);
 
 	__qed_set_ramrod_tunnel_param(&p_tunn_cfg->tunnel_clss_l2gre,
-				      &p_tunn_cfg->tx_enable_l2gre,
 				      &p_tun->l2_gre);
 
 	__qed_set_ramrod_tunnel_param(&p_tunn_cfg->tunnel_clss_ipgre,
-				      &p_tunn_cfg->tx_enable_ipgre,
 				      &p_tun->ip_gre);
 }
 
 int qed_sp_pf_start(struct qed_hwfn *p_hwfn,
+		    struct qed_ptt *p_ptt,
 		    struct qed_tunnel_info *p_tunn,
 		    enum qed_mf_mode mode, bool allow_npar_tx_switch)
 {
@@ -412,7 +401,8 @@ int qed_sp_pf_start(struct qed_hwfn *p_hwfn,
 	rc = qed_spq_post(p_hwfn, p_ent, NULL);
 
 	if (p_tunn)
-		qed_set_hw_tunn_mode_port(p_hwfn, &p_hwfn->cdev->tunnel);
+		qed_set_hw_tunn_mode_port(p_hwfn, p_ptt,
+					  &p_hwfn->cdev->tunnel);
 
 	return rc;
 }
@@ -443,6 +433,7 @@ int qed_sp_pf_update(struct qed_hwfn *p_hwfn)
 
 /* Set pf update ramrod command params */
 int qed_sp_pf_update_tunn_cfg(struct qed_hwfn *p_hwfn,
+			      struct qed_ptt *p_ptt,
 			      struct qed_tunnel_info *p_tunn,
 			      enum spq_mode comp_mode,
 			      struct qed_spq_comp_cb *p_comp_data)
@@ -477,7 +468,7 @@ int qed_sp_pf_update_tunn_cfg(struct qed_hwfn *p_hwfn,
 	if (rc)
 		return rc;
 
-	qed_set_hw_tunn_mode_port(p_hwfn, &p_hwfn->cdev->tunnel);
+	qed_set_hw_tunn_mode_port(p_hwfn, p_ptt, &p_hwfn->cdev->tunnel);
 
 	return rc;
 }
@@ -523,3 +514,27 @@ int qed_sp_heartbeat_ramrod(struct qed_hwfn *p_hwfn)
 
 	return qed_spq_post(p_hwfn, p_ent, NULL);
 }
+
+int qed_sp_pf_update_stag(struct qed_hwfn *p_hwfn)
+{
+	struct qed_spq_entry *p_ent = NULL;
+	struct qed_sp_init_data init_data;
+	int rc = -EINVAL;
+
+	/* Get SPQ entry */
+	memset(&init_data, 0, sizeof(init_data));
+	init_data.cid = qed_spq_get_cid(p_hwfn);
+	init_data.opaque_fid = p_hwfn->hw_info.opaque_fid;
+	init_data.comp_mode = QED_SPQ_MODE_CB;
+
+	rc = qed_sp_init_request(p_hwfn, &p_ent,
+				 COMMON_RAMROD_PF_UPDATE, PROTOCOLID_COMMON,
+				 &init_data);
+	if (rc)
+		return rc;
+
+	p_ent->ramrod.pf_update.update_mf_vlan_flag = true;
+	p_ent->ramrod.pf_update.mf_vlan = cpu_to_le16(p_hwfn->hw_info.ovlan);
+
+	return qed_spq_post(p_hwfn, p_ent, NULL);
+}
diff --git a/drivers/net/ethernet/qlogic/qed/qed_spq.c b/drivers/net/ethernet/qlogic/qed/qed_spq.c
index f6423a139ca0..be48d9abd001 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_spq.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_spq.c
@@ -54,7 +54,7 @@
 #include "qed_reg_addr.h"
 #include "qed_sp.h"
 #include "qed_sriov.h"
-#include "qed_roce.h"
+#include "qed_rdma.h"
 
 /***************************************************************************
 * Structures & Definitions
@@ -302,32 +302,16 @@ static int
 qed_async_event_completion(struct qed_hwfn *p_hwfn,
 			   struct event_ring_entry *p_eqe)
 {
-	switch (p_eqe->protocol_id) {
-#if IS_ENABLED(CONFIG_QED_RDMA)
-	case PROTOCOLID_ROCE:
-		qed_roce_async_event(p_hwfn, p_eqe->opcode,
-				     &p_eqe->data.rdma_data);
-		return 0;
-#endif
-	case PROTOCOLID_COMMON:
-		return qed_sriov_eqe_event(p_hwfn,
-					   p_eqe->opcode,
-					   p_eqe->echo, &p_eqe->data);
-	case PROTOCOLID_ISCSI:
-		if (!IS_ENABLED(CONFIG_QED_ISCSI))
-			return -EINVAL;
+	qed_spq_async_comp_cb cb;
 
-		if (p_hwfn->p_iscsi_info->event_cb) {
-			struct qed_iscsi_info *p_iscsi = p_hwfn->p_iscsi_info;
+	if (!p_hwfn->p_spq || (p_eqe->protocol_id >= MAX_PROTOCOL_TYPE))
+		return -EINVAL;
 
-			return p_iscsi->event_cb(p_iscsi->event_context,
-						 p_eqe->opcode, &p_eqe->data);
-		} else {
-			DP_NOTICE(p_hwfn,
-				  "iSCSI async completion is not set\n");
-			return -EINVAL;
-		}
-	default:
+	cb = p_hwfn->p_spq->async_comp_cb[p_eqe->protocol_id];
+	if (cb) {
+		return cb(p_hwfn, p_eqe->opcode, p_eqe->echo,
+			  &p_eqe->data, p_eqe->fw_return_code);
+	} else {
 		DP_NOTICE(p_hwfn,
 			  "Unknown Async completion for protocol: %d\n",
 			  p_eqe->protocol_id);
@@ -335,6 +319,28 @@ qed_async_event_completion(struct qed_hwfn *p_hwfn,
 	}
 }
 
+int
+qed_spq_register_async_cb(struct qed_hwfn *p_hwfn,
+			  enum protocol_type protocol_id,
+			  qed_spq_async_comp_cb cb)
+{
+	if (!p_hwfn->p_spq || (protocol_id >= MAX_PROTOCOL_TYPE))
+		return -EINVAL;
+
+	p_hwfn->p_spq->async_comp_cb[protocol_id] = cb;
+	return 0;
+}
+
+void
+qed_spq_unregister_async_cb(struct qed_hwfn *p_hwfn,
+			    enum protocol_type protocol_id)
+{
+	if (!p_hwfn->p_spq || (protocol_id >= MAX_PROTOCOL_TYPE))
+		return;
+
+	p_hwfn->p_spq->async_comp_cb[protocol_id] = NULL;
+}
+
 /***************************************************************************
 * EQ API
 ***************************************************************************/
@@ -403,14 +409,14 @@ int qed_eq_completion(struct qed_hwfn *p_hwfn, void *cookie)
 	return rc;
 }
 
-struct qed_eq *qed_eq_alloc(struct qed_hwfn *p_hwfn, u16 num_elem)
+int qed_eq_alloc(struct qed_hwfn *p_hwfn, u16 num_elem)
 {
 	struct qed_eq *p_eq;
 
 	/* Allocate EQ struct */
 	p_eq = kzalloc(sizeof(*p_eq), GFP_KERNEL);
 	if (!p_eq)
-		return NULL;
+		return -ENOMEM;
 
 	/* Allocate and initialize EQ chain*/
 	if (qed_chain_alloc(p_hwfn->cdev,
@@ -419,31 +425,35 @@ struct qed_eq *qed_eq_alloc(struct qed_hwfn *p_hwfn, u16 num_elem)
 			    QED_CHAIN_CNT_TYPE_U16,
 			    num_elem,
 			    sizeof(union event_ring_element),
-			    &p_eq->chain))
+			    &p_eq->chain, NULL))
 		goto eq_allocate_fail;
 
 	/* register EQ completion on the SP SB */
 	qed_int_register_cb(p_hwfn, qed_eq_completion,
 			    p_eq, &p_eq->eq_sb_index, &p_eq->p_fw_cons);
 
-	return p_eq;
+	p_hwfn->p_eq = p_eq;
+	return 0;
 
 eq_allocate_fail:
-	qed_eq_free(p_hwfn, p_eq);
-	return NULL;
+	kfree(p_eq);
+	return -ENOMEM;
 }
 
-void qed_eq_setup(struct qed_hwfn *p_hwfn, struct qed_eq *p_eq)
+void qed_eq_setup(struct qed_hwfn *p_hwfn)
 {
-	qed_chain_reset(&p_eq->chain);
+	qed_chain_reset(&p_hwfn->p_eq->chain);
 }
 
-void qed_eq_free(struct qed_hwfn *p_hwfn, struct qed_eq *p_eq)
+void qed_eq_free(struct qed_hwfn *p_hwfn)
 {
-	if (!p_eq)
+	if (!p_hwfn->p_eq)
 		return;
-	qed_chain_free(p_hwfn->cdev, &p_eq->chain);
-	kfree(p_eq);
+
+	qed_chain_free(p_hwfn->cdev, &p_hwfn->p_eq->chain);
+
+	kfree(p_hwfn->p_eq);
+	p_hwfn->p_eq = NULL;
 }
 
 /***************************************************************************
@@ -543,7 +553,7 @@ int qed_spq_alloc(struct qed_hwfn *p_hwfn)
 			    QED_CHAIN_CNT_TYPE_U16,
 			    0,   /* N/A when the mode is SINGLE */
 			    sizeof(struct slow_path_element),
-			    &p_spq->chain))
+			    &p_spq->chain, NULL))
 		goto spq_allocate_fail;
 
 	/* allocate and fill the SPQ elements (incl. ramrod data list) */
@@ -583,8 +593,8 @@ void qed_spq_free(struct qed_hwfn *p_hwfn)
 	}
 
 	qed_chain_free(p_hwfn->cdev, &p_spq->chain);
-	;
 	kfree(p_spq);
+	p_hwfn->p_spq = NULL;
 }
 
 int qed_spq_get_entry(struct qed_hwfn *p_hwfn, struct qed_spq_entry **pp_ent)
@@ -934,14 +944,14 @@ int qed_spq_completion(struct qed_hwfn *p_hwfn,
 	return rc;
 }
 
-struct qed_consq *qed_consq_alloc(struct qed_hwfn *p_hwfn)
+int qed_consq_alloc(struct qed_hwfn *p_hwfn)
 {
 	struct qed_consq *p_consq;
 
 	/* Allocate ConsQ struct */
 	p_consq = kzalloc(sizeof(*p_consq), GFP_KERNEL);
 	if (!p_consq)
-		return NULL;
+		return -ENOMEM;
 
 	/* Allocate and initialize EQ chain*/
 	if (qed_chain_alloc(p_hwfn->cdev,
@@ -949,25 +959,29 @@ struct qed_consq *qed_consq_alloc(struct qed_hwfn *p_hwfn)
 			    QED_CHAIN_MODE_PBL,
 			    QED_CHAIN_CNT_TYPE_U16,
 			    QED_CHAIN_PAGE_SIZE / 0x80,
-			    0x80, &p_consq->chain))
+			    0x80, &p_consq->chain, NULL))
 		goto consq_allocate_fail;
 
-	return p_consq;
+	p_hwfn->p_consq = p_consq;
+	return 0;
 
 consq_allocate_fail:
-	qed_consq_free(p_hwfn, p_consq);
-	return NULL;
+	kfree(p_consq);
+	return -ENOMEM;
 }
 
-void qed_consq_setup(struct qed_hwfn *p_hwfn, struct qed_consq *p_consq)
+void qed_consq_setup(struct qed_hwfn *p_hwfn)
 {
-	qed_chain_reset(&p_consq->chain);
+	qed_chain_reset(&p_hwfn->p_consq->chain);
 }
 
-void qed_consq_free(struct qed_hwfn *p_hwfn, struct qed_consq *p_consq)
+void qed_consq_free(struct qed_hwfn *p_hwfn)
 {
-	if (!p_consq)
+	if (!p_hwfn->p_consq)
 		return;
-	qed_chain_free(p_hwfn->cdev, &p_consq->chain);
-	kfree(p_consq);
+
+	qed_chain_free(p_hwfn->cdev, &p_hwfn->p_consq->chain);
+
+	kfree(p_hwfn->p_consq);
+	p_hwfn->p_consq = NULL;
 }
diff --git a/drivers/net/ethernet/qlogic/qed/qed_sriov.c b/drivers/net/ethernet/qlogic/qed/qed_sriov.c
index f5ed54d611ec..2cfd3bd9a031 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_sriov.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_sriov.c
@@ -44,6 +44,26 @@
 #include "qed_sp.h"
 #include "qed_sriov.h"
 #include "qed_vf.h"
+static int qed_sriov_eqe_event(struct qed_hwfn *p_hwfn,
+			       u8 opcode,
+			       __le16 echo,
+			       union event_ring_data *data, u8 fw_return_code);
+
+
+static u8 qed_vf_calculate_legacy(struct qed_vf_info *p_vf)
+{
+	u8 legacy = 0;
+
+	if (p_vf->acquire.vfdev_info.eth_fp_hsi_minor ==
+	    ETH_HSI_VER_NO_PKT_LEN_TUNN)
+		legacy |= QED_QCID_LEGACY_VF_RX_PROD;
+
+	if (!(p_vf->acquire.vfdev_info.capabilities &
+	      VFPF_ACQUIRE_CAP_QUEUE_QIDS))
+		legacy |= QED_QCID_LEGACY_VF_CID;
+
+	return legacy;
+}
 
 /* IOV ramrods */
 static int qed_sp_vf_start(struct qed_hwfn *p_hwfn, struct qed_vf_info *p_vf)
@@ -178,6 +198,19 @@ static struct qed_vf_info *qed_iov_get_vf_info(struct qed_hwfn *p_hwfn,
 	return vf;
 }
 
+static struct qed_queue_cid *
+qed_iov_get_vf_rx_queue_cid(struct qed_vf_queue *p_queue)
+{
+	int i;
+
+	for (i = 0; i < MAX_QUEUES_PER_QZONE; i++) {
+		if (p_queue->cids[i].p_cid && !p_queue->cids[i].b_is_tx)
+			return p_queue->cids[i].p_cid;
+	}
+
+	return NULL;
+}
+
 enum qed_iov_validate_q_mode {
 	QED_IOV_VALIDATE_Q_NA,
 	QED_IOV_VALIDATE_Q_ENABLE,
@@ -190,12 +223,24 @@ static bool qed_iov_validate_queue_mode(struct qed_hwfn *p_hwfn,
 					enum qed_iov_validate_q_mode mode,
 					bool b_is_tx)
 {
+	int i;
+
 	if (mode == QED_IOV_VALIDATE_Q_NA)
 		return true;
 
-	if ((b_is_tx && p_vf->vf_queues[qid].p_tx_cid) ||
-	    (!b_is_tx && p_vf->vf_queues[qid].p_rx_cid))
+	for (i = 0; i < MAX_QUEUES_PER_QZONE; i++) {
+		struct qed_vf_queue_cid *p_qcid;
+
+		p_qcid = &p_vf->vf_queues[qid].cids[i];
+
+		if (!p_qcid->p_cid)
+			continue;
+
+		if (p_qcid->b_is_tx != b_is_tx)
+			continue;
+
 		return mode == QED_IOV_VALIDATE_Q_ENABLE;
+	}
 
 	/* In case we haven't found any valid cid, then its disabled */
 	return mode == QED_IOV_VALIDATE_Q_DISABLE;
@@ -378,33 +423,6 @@ static int qed_iov_pci_cfg_info(struct qed_dev *cdev)
 	return 0;
 }
 
-static void qed_iov_clear_vf_igu_blocks(struct qed_hwfn *p_hwfn,
-					struct qed_ptt *p_ptt)
-{
-	struct qed_igu_block *p_sb;
-	u16 sb_id;
-	u32 val;
-
-	if (!p_hwfn->hw_info.p_igu_info) {
-		DP_ERR(p_hwfn,
-		       "qed_iov_clear_vf_igu_blocks IGU Info not initialized\n");
-		return;
-	}
-
-	for (sb_id = 0; sb_id < QED_MAPPING_MEMORY_SIZE(p_hwfn->cdev);
-	     sb_id++) {
-		p_sb = &p_hwfn->hw_info.p_igu_info->igu_map.igu_blocks[sb_id];
-		if ((p_sb->status & QED_IGU_STATUS_FREE) &&
-		    !(p_sb->status & QED_IGU_STATUS_PF)) {
-			val = qed_rd(p_hwfn, p_ptt,
-				     IGU_REG_MAPPING_MEMORY + sb_id * 4);
-			SET_FIELD(val, IGU_MAPPING_LINE_VALID, 0);
-			qed_wr(p_hwfn, p_ptt,
-			       IGU_REG_MAPPING_MEMORY + 4 * sb_id, val);
-		}
-	}
-}
-
 static void qed_iov_setup_vfdb(struct qed_hwfn *p_hwfn)
 {
 	struct qed_hw_sriov_info *p_iov = p_hwfn->cdev->p_iov_info;
@@ -552,20 +570,24 @@ int qed_iov_alloc(struct qed_hwfn *p_hwfn)
 
 	p_hwfn->pf_iov_info = p_sriov;
 
+	qed_spq_register_async_cb(p_hwfn, PROTOCOLID_COMMON,
+				  qed_sriov_eqe_event);
+
 	return qed_iov_allocate_vfdb(p_hwfn);
 }
 
-void qed_iov_setup(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+void qed_iov_setup(struct qed_hwfn *p_hwfn)
 {
 	if (!IS_PF_SRIOV(p_hwfn) || !IS_PF_SRIOV_ALLOC(p_hwfn))
 		return;
 
 	qed_iov_setup_vfdb(p_hwfn);
-	qed_iov_clear_vf_igu_blocks(p_hwfn, p_ptt);
 }
 
 void qed_iov_free(struct qed_hwfn *p_hwfn)
 {
+	qed_spq_unregister_async_cb(p_hwfn, PROTOCOLID_COMMON);
+
 	if (IS_PF_SRIOV_ALLOC(p_hwfn)) {
 		qed_iov_free_vfdb(p_hwfn);
 		kfree(p_hwfn->pf_iov_info);
@@ -747,6 +769,35 @@ static void qed_iov_vf_igu_set_int(struct qed_hwfn *p_hwfn,
 	qed_fid_pretend(p_hwfn, p_ptt, (u16) p_hwfn->hw_info.concrete_fid);
 }
 
+static int
+qed_iov_enable_vf_access_msix(struct qed_hwfn *p_hwfn,
+			      struct qed_ptt *p_ptt, u8 abs_vf_id, u8 num_sbs)
+{
+	u8 current_max = 0;
+	int i;
+
+	/* For AH onward, configuration is per-PF. Find maximum of all
+	 * the currently enabled child VFs, and set the number to be that.
+	 */
+	if (!QED_IS_BB(p_hwfn->cdev)) {
+		qed_for_each_vf(p_hwfn, i) {
+			struct qed_vf_info *p_vf;
+
+			p_vf = qed_iov_get_vf_info(p_hwfn, (u16)i, true);
+			if (!p_vf)
+				continue;
+
+			current_max = max_t(u8, current_max, p_vf->num_sbs);
+		}
+	}
+
+	if (num_sbs > current_max)
+		return qed_mcp_config_vf_msix(p_hwfn, p_ptt,
+					      abs_vf_id, num_sbs);
+
+	return 0;
+}
+
 static int qed_iov_enable_vf_access(struct qed_hwfn *p_hwfn,
 				    struct qed_ptt *p_ptt,
 				    struct qed_vf_info *vf)
@@ -771,7 +822,8 @@ static int qed_iov_enable_vf_access(struct qed_hwfn *p_hwfn,
 
 	qed_iov_vf_igu_reset(p_hwfn, p_ptt, vf);
 
-	rc = qed_mcp_config_vf_msix(p_hwfn, p_ptt, vf->abs_vf_id, vf->num_sbs);
+	rc = qed_iov_enable_vf_access_msix(p_hwfn, p_ptt,
+					   vf->abs_vf_id, vf->num_sbs);
 	if (rc)
 		return rc;
 
@@ -838,45 +890,36 @@ static u8 qed_iov_alloc_vf_igu_sbs(struct qed_hwfn *p_hwfn,
 				   struct qed_ptt *p_ptt,
 				   struct qed_vf_info *vf, u16 num_rx_queues)
 {
-	struct qed_igu_block *igu_blocks;
-	int qid = 0, igu_id = 0;
+	struct qed_igu_block *p_block;
+	struct cau_sb_entry sb_entry;
+	int qid = 0;
 	u32 val = 0;
 
-	igu_blocks = p_hwfn->hw_info.p_igu_info->igu_map.igu_blocks;
-
-	if (num_rx_queues > p_hwfn->hw_info.p_igu_info->free_blks)
-		num_rx_queues = p_hwfn->hw_info.p_igu_info->free_blks;
-	p_hwfn->hw_info.p_igu_info->free_blks -= num_rx_queues;
+	if (num_rx_queues > p_hwfn->hw_info.p_igu_info->usage.free_cnt_iov)
+		num_rx_queues = p_hwfn->hw_info.p_igu_info->usage.free_cnt_iov;
+	p_hwfn->hw_info.p_igu_info->usage.free_cnt_iov -= num_rx_queues;
 
 	SET_FIELD(val, IGU_MAPPING_LINE_FUNCTION_NUMBER, vf->abs_vf_id);
 	SET_FIELD(val, IGU_MAPPING_LINE_VALID, 1);
 	SET_FIELD(val, IGU_MAPPING_LINE_PF_VALID, 0);
 
-	while ((qid < num_rx_queues) &&
-	       (igu_id < QED_MAPPING_MEMORY_SIZE(p_hwfn->cdev))) {
-		if (igu_blocks[igu_id].status & QED_IGU_STATUS_FREE) {
-			struct cau_sb_entry sb_entry;
-
-			vf->igu_sbs[qid] = (u16)igu_id;
-			igu_blocks[igu_id].status &= ~QED_IGU_STATUS_FREE;
-
-			SET_FIELD(val, IGU_MAPPING_LINE_VECTOR_NUMBER, qid);
-
-			qed_wr(p_hwfn, p_ptt,
-			       IGU_REG_MAPPING_MEMORY + sizeof(u32) * igu_id,
-			       val);
-
-			/* Configure igu sb in CAU which were marked valid */
-			qed_init_cau_sb_entry(p_hwfn, &sb_entry,
-					      p_hwfn->rel_pf_id,
-					      vf->abs_vf_id, 1);
-			qed_dmae_host2grc(p_hwfn, p_ptt,
-					  (u64)(uintptr_t)&sb_entry,
-					  CAU_REG_SB_VAR_MEMORY +
-					  igu_id * sizeof(u64), 2, 0);
-			qid++;
-		}
-		igu_id++;
+	for (qid = 0; qid < num_rx_queues; qid++) {
+		p_block = qed_get_igu_free_sb(p_hwfn, false);
+		vf->igu_sbs[qid] = p_block->igu_sb_id;
+		p_block->status &= ~QED_IGU_STATUS_FREE;
+		SET_FIELD(val, IGU_MAPPING_LINE_VECTOR_NUMBER, qid);
+
+		qed_wr(p_hwfn, p_ptt,
+		       IGU_REG_MAPPING_MEMORY +
+		       sizeof(u32) * p_block->igu_sb_id, val);
+
+		/* Configure igu sb in CAU which were marked valid */
+		qed_init_cau_sb_entry(p_hwfn, &sb_entry,
+				      p_hwfn->rel_pf_id, vf->abs_vf_id, 1);
+		qed_dmae_host2grc(p_hwfn, p_ptt,
+				  (u64)(uintptr_t)&sb_entry,
+				  CAU_REG_SB_VAR_MEMORY +
+				  p_block->igu_sb_id * sizeof(u64), 2, 0);
 	}
 
 	vf->num_sbs = (u8) num_rx_queues;
@@ -901,10 +944,8 @@ static void qed_iov_free_vf_igu_sbs(struct qed_hwfn *p_hwfn,
 		SET_FIELD(val, IGU_MAPPING_LINE_VALID, 0);
 		qed_wr(p_hwfn, p_ptt, addr, val);
 
-		p_info->igu_map.igu_blocks[igu_id].status |=
-		    QED_IGU_STATUS_FREE;
-
-		p_hwfn->hw_info.p_igu_info->free_blks++;
+		p_info->entry[igu_id].status |= QED_IGU_STATUS_FREE;
+		p_hwfn->hw_info.p_igu_info->usage.free_cnt_iov++;
 	}
 
 	vf->num_sbs = 0;
@@ -1028,20 +1069,15 @@ static int qed_iov_init_hw_for_vf(struct qed_hwfn *p_hwfn,
 	vf->num_txqs = num_of_vf_avaiable_chains;
 
 	for (i = 0; i < vf->num_rxqs; i++) {
-		struct qed_vf_q_info *p_queue = &vf->vf_queues[i];
+		struct qed_vf_queue *p_queue = &vf->vf_queues[i];
 
 		p_queue->fw_rx_qid = p_params->req_rx_queue[i];
 		p_queue->fw_tx_qid = p_params->req_tx_queue[i];
 
-		/* CIDs are per-VF, so no problem having them 0-based. */
-		p_queue->fw_cid = i;
-
 		DP_VERBOSE(p_hwfn, QED_MSG_IOV,
-			   "VF[%d] - Q[%d] SB %04x, qid [Rx %04x Tx %04x]  CID %04x\n",
-			   vf->relative_vf_id,
-			   i, vf->igu_sbs[i],
-			   p_queue->fw_rx_qid,
-			   p_queue->fw_tx_qid, p_queue->fw_cid);
+			   "VF[%d] - Q[%d] SB %04x, qid [Rx %04x Tx %04x]\n",
+			   vf->relative_vf_id, i, vf->igu_sbs[i],
+			   p_queue->fw_rx_qid, p_queue->fw_tx_qid);
 	}
 
 	/* Update the link configuration in bulletin */
@@ -1328,7 +1364,7 @@ static void qed_iov_clean_vf(struct qed_hwfn *p_hwfn, u8 vfid)
 static void qed_iov_vf_cleanup(struct qed_hwfn *p_hwfn,
 			       struct qed_vf_info *p_vf)
 {
-	u32 i;
+	u32 i, j;
 
 	p_vf->vf_bulletin = 0;
 	p_vf->vport_instance = 0;
@@ -1341,16 +1377,15 @@ static void qed_iov_vf_cleanup(struct qed_hwfn *p_hwfn,
 	p_vf->num_active_rxqs = 0;
 
 	for (i = 0; i < QED_MAX_VF_CHAINS_PER_PF; i++) {
-		struct qed_vf_q_info *p_queue = &p_vf->vf_queues[i];
+		struct qed_vf_queue *p_queue = &p_vf->vf_queues[i];
 
-		if (p_queue->p_rx_cid) {
-			qed_eth_queue_cid_release(p_hwfn, p_queue->p_rx_cid);
-			p_queue->p_rx_cid = NULL;
-		}
+		for (j = 0; j < MAX_QUEUES_PER_QZONE; j++) {
+			if (!p_queue->cids[j].p_cid)
+				continue;
 
-		if (p_queue->p_tx_cid) {
-			qed_eth_queue_cid_release(p_hwfn, p_queue->p_tx_cid);
-			p_queue->p_tx_cid = NULL;
+			qed_eth_queue_cid_release(p_hwfn,
+						  p_queue->cids[j].p_cid);
+			p_queue->cids[j].p_cid = NULL;
 		}
 	}
 
@@ -1359,13 +1394,67 @@ static void qed_iov_vf_cleanup(struct qed_hwfn *p_hwfn,
 	qed_iov_clean_vf(p_hwfn, p_vf->relative_vf_id);
 }
 
+/* Returns either 0, or log(size) */
+static u32 qed_iov_vf_db_bar_size(struct qed_hwfn *p_hwfn,
+				  struct qed_ptt *p_ptt)
+{
+	u32 val = qed_rd(p_hwfn, p_ptt, PGLUE_B_REG_VF_BAR1_SIZE);
+
+	if (val)
+		return val + 11;
+	return 0;
+}
+
+static void
+qed_iov_vf_mbx_acquire_resc_cids(struct qed_hwfn *p_hwfn,
+				 struct qed_ptt *p_ptt,
+				 struct qed_vf_info *p_vf,
+				 struct vf_pf_resc_request *p_req,
+				 struct pf_vf_resc *p_resp)
+{
+	u8 num_vf_cons = p_hwfn->pf_params.eth_pf_params.num_vf_cons;
+	u8 db_size = qed_db_addr_vf(1, DQ_DEMS_LEGACY) -
+		     qed_db_addr_vf(0, DQ_DEMS_LEGACY);
+	u32 bar_size;
+
+	p_resp->num_cids = min_t(u8, p_req->num_cids, num_vf_cons);
+
+	/* If VF didn't bother asking for QIDs than don't bother limiting
+	 * number of CIDs. The VF doesn't care about the number, and this
+	 * has the likely result of causing an additional acquisition.
+	 */
+	if (!(p_vf->acquire.vfdev_info.capabilities &
+	      VFPF_ACQUIRE_CAP_QUEUE_QIDS))
+		return;
+
+	/* If doorbell bar was mapped by VF, limit the VF CIDs to an amount
+	 * that would make sure doorbells for all CIDs fall within the bar.
+	 * If it doesn't, make sure regview window is sufficient.
+	 */
+	if (p_vf->acquire.vfdev_info.capabilities &
+	    VFPF_ACQUIRE_CAP_PHYSICAL_BAR) {
+		bar_size = qed_iov_vf_db_bar_size(p_hwfn, p_ptt);
+		if (bar_size)
+			bar_size = 1 << bar_size;
+
+		if (p_hwfn->cdev->num_hwfns > 1)
+			bar_size /= 2;
+	} else {
+		bar_size = PXP_VF_BAR0_DQ_LENGTH;
+	}
+
+	if (bar_size / db_size < 256)
+		p_resp->num_cids = min_t(u8, p_resp->num_cids,
+					 (u8)(bar_size / db_size));
+}
+
 static u8 qed_iov_vf_mbx_acquire_resc(struct qed_hwfn *p_hwfn,
 				      struct qed_ptt *p_ptt,
 				      struct qed_vf_info *p_vf,
 				      struct vf_pf_resc_request *p_req,
 				      struct pf_vf_resc *p_resp)
 {
-	int i;
+	u8 i;
 
 	/* Queue related information */
 	p_resp->num_rxqs = p_vf->num_rxqs;
@@ -1383,7 +1472,7 @@ static u8 qed_iov_vf_mbx_acquire_resc(struct qed_hwfn *p_hwfn,
 	for (i = 0; i < p_resp->num_rxqs; i++) {
 		qed_fw_l2_queue(p_hwfn, p_vf->vf_queues[i].fw_rx_qid,
 				(u16 *)&p_resp->hw_qid[i]);
-		p_resp->cid[i] = p_vf->vf_queues[i].fw_cid;
+		p_resp->cid[i] = i;
 	}
 
 	/* Filter related information */
@@ -1392,6 +1481,8 @@ static u8 qed_iov_vf_mbx_acquire_resc(struct qed_hwfn *p_hwfn,
 	p_resp->num_vlan_filters = min_t(u8, p_vf->num_vlan_filters,
 					 p_req->num_vlan_filters);
 
+	qed_iov_vf_mbx_acquire_resc_cids(p_hwfn, p_ptt, p_vf, p_req, p_resp);
+
 	/* This isn't really needed/enforced, but some legacy VFs might depend
 	 * on the correct filling of this field.
 	 */
@@ -1403,10 +1494,11 @@ static u8 qed_iov_vf_mbx_acquire_resc(struct qed_hwfn *p_hwfn,
 	    p_resp->num_sbs < p_req->num_sbs ||
 	    p_resp->num_mac_filters < p_req->num_mac_filters ||
 	    p_resp->num_vlan_filters < p_req->num_vlan_filters ||
-	    p_resp->num_mc_filters < p_req->num_mc_filters) {
+	    p_resp->num_mc_filters < p_req->num_mc_filters ||
+	    p_resp->num_cids < p_req->num_cids) {
 		DP_VERBOSE(p_hwfn,
 			   QED_MSG_IOV,
-			   "VF[%d] - Insufficient resources: rxq [%02x/%02x] txq [%02x/%02x] sbs [%02x/%02x] mac [%02x/%02x] vlan [%02x/%02x] mc [%02x/%02x]\n",
+			   "VF[%d] - Insufficient resources: rxq [%02x/%02x] txq [%02x/%02x] sbs [%02x/%02x] mac [%02x/%02x] vlan [%02x/%02x] mc [%02x/%02x] cids [%02x/%02x]\n",
 			   p_vf->abs_vf_id,
 			   p_req->num_rxqs,
 			   p_resp->num_rxqs,
@@ -1418,7 +1510,9 @@ static u8 qed_iov_vf_mbx_acquire_resc(struct qed_hwfn *p_hwfn,
 			   p_resp->num_mac_filters,
 			   p_req->num_vlan_filters,
 			   p_resp->num_vlan_filters,
-			   p_req->num_mc_filters, p_resp->num_mc_filters);
+			   p_req->num_mc_filters,
+			   p_resp->num_mc_filters,
+			   p_req->num_cids, p_resp->num_cids);
 
 		/* Some legacy OSes are incapable of correctly handling this
 		 * failure.
@@ -1534,6 +1628,15 @@ static void qed_iov_vf_mbx_acquire(struct qed_hwfn *p_hwfn,
 	if (p_hwfn->cdev->num_hwfns > 1)
 		pfdev_info->capabilities |= PFVF_ACQUIRE_CAP_100G;
 
+	/* Share our ability to use multiple queue-ids only with VFs
+	 * that request it.
+	 */
+	if (req->vfdev_info.capabilities & VFPF_ACQUIRE_CAP_QUEUE_QIDS)
+		pfdev_info->capabilities |= PFVF_ACQUIRE_CAP_QUEUE_QIDS;
+
+	/* Share the sizes of the bars with VF */
+	resp->pfdev_info.bar_size = qed_iov_vf_db_bar_size(p_hwfn, p_ptt);
+
 	qed_iov_vf_mbx_acquire_stats(p_hwfn, &pfdev_info->stats_info);
 
 	memcpy(pfdev_info->port_mac, p_hwfn->hw_info.hw_mac_addr, ETH_ALEN);
@@ -1758,9 +1861,11 @@ static int qed_iov_configure_vport_forced(struct qed_hwfn *p_hwfn,
 
 		/* Update all the Rx queues */
 		for (i = 0; i < QED_MAX_VF_CHAINS_PER_PF; i++) {
-			struct qed_queue_cid *p_cid;
+			struct qed_vf_queue *p_queue = &p_vf->vf_queues[i];
+			struct qed_queue_cid *p_cid = NULL;
 
-			p_cid = p_vf->vf_queues[i].p_rx_cid;
+			/* There can be at most 1 Rx queue on qzone. Find it */
+			p_cid = qed_iov_get_vf_rx_queue_cid(p_queue);
 			if (!p_cid)
 				continue;
 
@@ -1951,16 +2056,55 @@ static void qed_iov_vf_mbx_start_rxq_resp(struct qed_hwfn *p_hwfn,
 	qed_iov_send_response(p_hwfn, p_ptt, vf, length, status);
 }
 
+static u8 qed_iov_vf_mbx_qid(struct qed_hwfn *p_hwfn,
+			     struct qed_vf_info *p_vf, bool b_is_tx)
+{
+	struct qed_iov_vf_mbx *p_mbx = &p_vf->vf_mbx;
+	struct vfpf_qid_tlv *p_qid_tlv;
+
+	/* Search for the qid if the VF published its going to provide it */
+	if (!(p_vf->acquire.vfdev_info.capabilities &
+	      VFPF_ACQUIRE_CAP_QUEUE_QIDS)) {
+		if (b_is_tx)
+			return QED_IOV_LEGACY_QID_TX;
+		else
+			return QED_IOV_LEGACY_QID_RX;
+	}
+
+	p_qid_tlv = (struct vfpf_qid_tlv *)
+		    qed_iov_search_list_tlvs(p_hwfn, p_mbx->req_virt,
+					     CHANNEL_TLV_QID);
+	if (!p_qid_tlv) {
+		DP_VERBOSE(p_hwfn, QED_MSG_IOV,
+			   "VF[%2x]: Failed to provide qid\n",
+			   p_vf->relative_vf_id);
+
+		return QED_IOV_QID_INVALID;
+	}
+
+	if (p_qid_tlv->qid >= MAX_QUEUES_PER_QZONE) {
+		DP_VERBOSE(p_hwfn, QED_MSG_IOV,
+			   "VF[%02x]: Provided qid out-of-bounds %02x\n",
+			   p_vf->relative_vf_id, p_qid_tlv->qid);
+		return QED_IOV_QID_INVALID;
+	}
+
+	return p_qid_tlv->qid;
+}
+
 static void qed_iov_vf_mbx_start_rxq(struct qed_hwfn *p_hwfn,
 				     struct qed_ptt *p_ptt,
 				     struct qed_vf_info *vf)
 {
 	struct qed_queue_start_common_params params;
+	struct qed_queue_cid_vf_params vf_params;
 	struct qed_iov_vf_mbx *mbx = &vf->vf_mbx;
 	u8 status = PFVF_STATUS_NO_RESOURCE;
-	struct qed_vf_q_info *p_queue;
+	u8 qid_usage_idx, vf_legacy = 0;
 	struct vfpf_start_rxq_tlv *req;
-	bool b_legacy_vf = false;
+	struct qed_vf_queue *p_queue;
+	struct qed_queue_cid *p_cid;
+	struct qed_sb_info sb_dummy;
 	int rc;
 
 	req = &mbx->req_virt->start_rxq;
@@ -1970,53 +2114,64 @@ static void qed_iov_vf_mbx_start_rxq(struct qed_hwfn *p_hwfn,
 	    !qed_iov_validate_sb(p_hwfn, vf, req->hw_sb))
 		goto out;
 
-	/* Acquire a new queue-cid */
+	qid_usage_idx = qed_iov_vf_mbx_qid(p_hwfn, vf, false);
+	if (qid_usage_idx == QED_IOV_QID_INVALID)
+		goto out;
+
 	p_queue = &vf->vf_queues[req->rx_qid];
+	if (p_queue->cids[qid_usage_idx].p_cid)
+		goto out;
+
+	vf_legacy = qed_vf_calculate_legacy(vf);
 
+	/* Acquire a new queue-cid */
 	memset(&params, 0, sizeof(params));
 	params.queue_id = p_queue->fw_rx_qid;
 	params.vport_id = vf->vport_id;
 	params.stats_id = vf->abs_vf_id + 0x10;
-	params.sb = req->hw_sb;
+	/* Since IGU index is passed via sb_info, construct a dummy one */
+	memset(&sb_dummy, 0, sizeof(sb_dummy));
+	sb_dummy.igu_sb_id = req->hw_sb;
+	params.p_sb = &sb_dummy;
 	params.sb_idx = req->sb_index;
 
-	p_queue->p_rx_cid = _qed_eth_queue_to_cid(p_hwfn,
-						  vf->opaque_fid,
-						  p_queue->fw_cid,
-						  req->rx_qid, &params);
-	if (!p_queue->p_rx_cid)
+	memset(&vf_params, 0, sizeof(vf_params));
+	vf_params.vfid = vf->relative_vf_id;
+	vf_params.vf_qid = (u8)req->rx_qid;
+	vf_params.vf_legacy = vf_legacy;
+	vf_params.qid_usage_idx = qid_usage_idx;
+	p_cid = qed_eth_queue_to_cid(p_hwfn, vf->opaque_fid,
+				     &params, true, &vf_params);
+	if (!p_cid)
 		goto out;
 
 	/* Legacy VFs have their Producers in a different location, which they
 	 * calculate on their own and clean the producer prior to this.
 	 */
-	if (vf->acquire.vfdev_info.eth_fp_hsi_minor ==
-	    ETH_HSI_VER_NO_PKT_LEN_TUNN) {
-		b_legacy_vf = true;
-	} else {
+	if (!(vf_legacy & QED_QCID_LEGACY_VF_RX_PROD))
 		REG_WR(p_hwfn,
 		       GTT_BAR0_MAP_REG_MSDM_RAM +
 		       MSTORM_ETH_VF_PRODS_OFFSET(vf->abs_vf_id, req->rx_qid),
 		       0);
-	}
-	p_queue->p_rx_cid->b_legacy_vf = b_legacy_vf;
 
-	rc = qed_eth_rxq_start_ramrod(p_hwfn,
-				      p_queue->p_rx_cid,
+	rc = qed_eth_rxq_start_ramrod(p_hwfn, p_cid,
 				      req->bd_max_bytes,
 				      req->rxq_addr,
 				      req->cqe_pbl_addr, req->cqe_pbl_size);
 	if (rc) {
 		status = PFVF_STATUS_FAILURE;
-		qed_eth_queue_cid_release(p_hwfn, p_queue->p_rx_cid);
-		p_queue->p_rx_cid = NULL;
+		qed_eth_queue_cid_release(p_hwfn, p_cid);
 	} else {
+		p_queue->cids[qid_usage_idx].p_cid = p_cid;
+		p_queue->cids[qid_usage_idx].b_is_tx = false;
 		status = PFVF_STATUS_SUCCESS;
 		vf->num_active_rxqs++;
 	}
 
 out:
-	qed_iov_vf_mbx_start_rxq_resp(p_hwfn, p_ptt, vf, status, b_legacy_vf);
+	qed_iov_vf_mbx_start_rxq_resp(p_hwfn, p_ptt, vf, status,
+				      !!(vf_legacy &
+					 QED_QCID_LEGACY_VF_RX_PROD));
 }
 
 static void
@@ -2209,7 +2364,7 @@ static void qed_iov_vf_mbx_update_tunn_param(struct qed_hwfn *p_hwfn,
 	if (b_update_required) {
 		u16 geneve_port;
 
-		rc = qed_sp_pf_update_tunn_cfg(p_hwfn, &tunn,
+		rc = qed_sp_pf_update_tunn_cfg(p_hwfn, p_ptt, &tunn,
 					       QED_SPQ_MODE_EBLOCK, NULL);
 		if (rc)
 			status = PFVF_STATUS_FAILURE;
@@ -2235,7 +2390,8 @@ send_resp:
 
 static void qed_iov_vf_mbx_start_txq_resp(struct qed_hwfn *p_hwfn,
 					  struct qed_ptt *p_ptt,
-					  struct qed_vf_info *p_vf, u8 status)
+					  struct qed_vf_info *p_vf,
+					  u32 cid, u8 status)
 {
 	struct qed_iov_vf_mbx *mbx = &p_vf->vf_mbx;
 	struct pfvf_start_queue_resp_tlv *p_tlv;
@@ -2263,12 +2419,8 @@ static void qed_iov_vf_mbx_start_txq_resp(struct qed_hwfn *p_hwfn,
 		    sizeof(struct channel_list_end_tlv));
 
 	/* Update the TLV with the response */
-	if ((status == PFVF_STATUS_SUCCESS) && !b_legacy) {
-		u16 qid = mbx->req_virt->start_txq.tx_qid;
-
-		p_tlv->offset = qed_db_addr_vf(p_vf->vf_queues[qid].fw_cid,
-					       DQ_DEMS_LEGACY);
-	}
+	if ((status == PFVF_STATUS_SUCCESS) && !b_legacy)
+		p_tlv->offset = qed_db_addr_vf(cid, DQ_DEMS_LEGACY);
 
 	qed_iov_send_response(p_hwfn, p_ptt, p_vf, length, status);
 }
@@ -2278,10 +2430,15 @@ static void qed_iov_vf_mbx_start_txq(struct qed_hwfn *p_hwfn,
 				     struct qed_vf_info *vf)
 {
 	struct qed_queue_start_common_params params;
+	struct qed_queue_cid_vf_params vf_params;
 	struct qed_iov_vf_mbx *mbx = &vf->vf_mbx;
 	u8 status = PFVF_STATUS_NO_RESOURCE;
 	struct vfpf_start_txq_tlv *req;
-	struct qed_vf_q_info *p_queue;
+	struct qed_vf_queue *p_queue;
+	struct qed_queue_cid *p_cid;
+	struct qed_sb_info sb_dummy;
+	u8 qid_usage_idx, vf_legacy;
+	u32 cid = 0;
 	int rc;
 	u16 pq;
 
@@ -2289,89 +2446,126 @@ static void qed_iov_vf_mbx_start_txq(struct qed_hwfn *p_hwfn,
 	req = &mbx->req_virt->start_txq;
 
 	if (!qed_iov_validate_txq(p_hwfn, vf, req->tx_qid,
-				  QED_IOV_VALIDATE_Q_DISABLE) ||
+				  QED_IOV_VALIDATE_Q_NA) ||
 	    !qed_iov_validate_sb(p_hwfn, vf, req->hw_sb))
 		goto out;
 
-	/* Acquire a new queue-cid */
+	qid_usage_idx = qed_iov_vf_mbx_qid(p_hwfn, vf, true);
+	if (qid_usage_idx == QED_IOV_QID_INVALID)
+		goto out;
+
 	p_queue = &vf->vf_queues[req->tx_qid];
+	if (p_queue->cids[qid_usage_idx].p_cid)
+		goto out;
+
+	vf_legacy = qed_vf_calculate_legacy(vf);
 
+	/* Acquire a new queue-cid */
 	params.queue_id = p_queue->fw_tx_qid;
 	params.vport_id = vf->vport_id;
 	params.stats_id = vf->abs_vf_id + 0x10;
-	params.sb = req->hw_sb;
+
+	/* Since IGU index is passed via sb_info, construct a dummy one */
+	memset(&sb_dummy, 0, sizeof(sb_dummy));
+	sb_dummy.igu_sb_id = req->hw_sb;
+	params.p_sb = &sb_dummy;
 	params.sb_idx = req->sb_index;
 
-	p_queue->p_tx_cid = _qed_eth_queue_to_cid(p_hwfn,
-						  vf->opaque_fid,
-						  p_queue->fw_cid,
-						  req->tx_qid, &params);
-	if (!p_queue->p_tx_cid)
+	memset(&vf_params, 0, sizeof(vf_params));
+	vf_params.vfid = vf->relative_vf_id;
+	vf_params.vf_qid = (u8)req->tx_qid;
+	vf_params.vf_legacy = vf_legacy;
+	vf_params.qid_usage_idx = qid_usage_idx;
+
+	p_cid = qed_eth_queue_to_cid(p_hwfn, vf->opaque_fid,
+				     &params, false, &vf_params);
+	if (!p_cid)
 		goto out;
 
 	pq = qed_get_cm_pq_idx_vf(p_hwfn, vf->relative_vf_id);
-	rc = qed_eth_txq_start_ramrod(p_hwfn, p_queue->p_tx_cid,
+	rc = qed_eth_txq_start_ramrod(p_hwfn, p_cid,
 				      req->pbl_addr, req->pbl_size, pq);
 	if (rc) {
 		status = PFVF_STATUS_FAILURE;
-		qed_eth_queue_cid_release(p_hwfn, p_queue->p_tx_cid);
-		p_queue->p_tx_cid = NULL;
+		qed_eth_queue_cid_release(p_hwfn, p_cid);
 	} else {
 		status = PFVF_STATUS_SUCCESS;
+		p_queue->cids[qid_usage_idx].p_cid = p_cid;
+		p_queue->cids[qid_usage_idx].b_is_tx = true;
+		cid = p_cid->cid;
 	}
 
 out:
-	qed_iov_vf_mbx_start_txq_resp(p_hwfn, p_ptt, vf, status);
+	qed_iov_vf_mbx_start_txq_resp(p_hwfn, p_ptt, vf, cid, status);
 }
 
 static int qed_iov_vf_stop_rxqs(struct qed_hwfn *p_hwfn,
 				struct qed_vf_info *vf,
-				u16 rxq_id, bool cqe_completion)
+				u16 rxq_id,
+				u8 qid_usage_idx, bool cqe_completion)
 {
-	struct qed_vf_q_info *p_queue;
+	struct qed_vf_queue *p_queue;
 	int rc = 0;
 
-	if (!qed_iov_validate_rxq(p_hwfn, vf, rxq_id,
-				  QED_IOV_VALIDATE_Q_ENABLE)) {
+	if (!qed_iov_validate_rxq(p_hwfn, vf, rxq_id, QED_IOV_VALIDATE_Q_NA)) {
 		DP_VERBOSE(p_hwfn,
 			   QED_MSG_IOV,
-			   "VF[%d] Tried Closing Rx 0x%04x which is inactive\n",
-			   vf->relative_vf_id, rxq_id);
+			   "VF[%d] Tried Closing Rx 0x%04x.%02x which is inactive\n",
+			   vf->relative_vf_id, rxq_id, qid_usage_idx);
 		return -EINVAL;
 	}
 
 	p_queue = &vf->vf_queues[rxq_id];
 
+	/* We've validated the index and the existence of the active RXQ -
+	 * now we need to make sure that it's using the correct qid.
+	 */
+	if (!p_queue->cids[qid_usage_idx].p_cid ||
+	    p_queue->cids[qid_usage_idx].b_is_tx) {
+		struct qed_queue_cid *p_cid;
+
+		p_cid = qed_iov_get_vf_rx_queue_cid(p_queue);
+		DP_VERBOSE(p_hwfn,
+			   QED_MSG_IOV,
+			   "VF[%d] - Tried Closing Rx 0x%04x.%02x, but Rx is at %04x.%02x\n",
+			   vf->relative_vf_id,
+			   rxq_id, qid_usage_idx, rxq_id, p_cid->qid_usage_idx);
+		return -EINVAL;
+	}
+
+	/* Now that we know we have a valid Rx-queue - close it */
 	rc = qed_eth_rx_queue_stop(p_hwfn,
-				   p_queue->p_rx_cid,
+				   p_queue->cids[qid_usage_idx].p_cid,
 				   false, cqe_completion);
 	if (rc)
 		return rc;
 
-	p_queue->p_rx_cid = NULL;
+	p_queue->cids[qid_usage_idx].p_cid = NULL;
 	vf->num_active_rxqs--;
 
 	return 0;
 }
 
 static int qed_iov_vf_stop_txqs(struct qed_hwfn *p_hwfn,
-				struct qed_vf_info *vf, u16 txq_id)
+				struct qed_vf_info *vf,
+				u16 txq_id, u8 qid_usage_idx)
 {
-	struct qed_vf_q_info *p_queue;
+	struct qed_vf_queue *p_queue;
 	int rc = 0;
 
-	if (!qed_iov_validate_txq(p_hwfn, vf, txq_id,
-				  QED_IOV_VALIDATE_Q_ENABLE))
+	if (!qed_iov_validate_txq(p_hwfn, vf, txq_id, QED_IOV_VALIDATE_Q_NA))
 		return -EINVAL;
 
 	p_queue = &vf->vf_queues[txq_id];
+	if (!p_queue->cids[qid_usage_idx].p_cid ||
+	    !p_queue->cids[qid_usage_idx].b_is_tx)
+		return -EINVAL;
 
-	rc = qed_eth_tx_queue_stop(p_hwfn, p_queue->p_tx_cid);
+	rc = qed_eth_tx_queue_stop(p_hwfn, p_queue->cids[qid_usage_idx].p_cid);
 	if (rc)
 		return rc;
 
-	p_queue->p_tx_cid = NULL;
-
+	p_queue->cids[qid_usage_idx].p_cid = NULL;
 	return 0;
 }
 
@@ -2383,6 +2577,7 @@ static void qed_iov_vf_mbx_stop_rxqs(struct qed_hwfn *p_hwfn,
 	struct qed_iov_vf_mbx *mbx = &vf->vf_mbx;
 	u8 status = PFVF_STATUS_FAILURE;
 	struct vfpf_stop_rxqs_tlv *req;
+	u8 qid_usage_idx;
 	int rc;
 
 	/* There has never been an official driver that used this interface
@@ -2398,8 +2593,13 @@ static void qed_iov_vf_mbx_stop_rxqs(struct qed_hwfn *p_hwfn,
 		goto out;
 	}
 
+	/* Find which qid-index is associated with the queue */
+	qid_usage_idx = qed_iov_vf_mbx_qid(p_hwfn, vf, false);
+	if (qid_usage_idx == QED_IOV_QID_INVALID)
+		goto out;
+
 	rc = qed_iov_vf_stop_rxqs(p_hwfn, vf, req->rx_qid,
-				  req->cqe_completion);
+				  qid_usage_idx, req->cqe_completion);
 	if (!rc)
 		status = PFVF_STATUS_SUCCESS;
 out:
@@ -2415,6 +2615,7 @@ static void qed_iov_vf_mbx_stop_txqs(struct qed_hwfn *p_hwfn,
 	struct qed_iov_vf_mbx *mbx = &vf->vf_mbx;
 	u8 status = PFVF_STATUS_FAILURE;
 	struct vfpf_stop_txqs_tlv *req;
+	u8 qid_usage_idx;
 	int rc;
 
 	/* There has never been an official driver that used this interface
@@ -2429,7 +2630,13 @@ static void qed_iov_vf_mbx_stop_txqs(struct qed_hwfn *p_hwfn,
 		status = PFVF_STATUS_NOT_SUPPORTED;
 		goto out;
 	}
-	rc = qed_iov_vf_stop_txqs(p_hwfn, vf, req->tx_qid);
+
+	/* Find which qid-index is associated with the queue */
+	qid_usage_idx = qed_iov_vf_mbx_qid(p_hwfn, vf, true);
+	if (qid_usage_idx == QED_IOV_QID_INVALID)
+		goto out;
+
+	rc = qed_iov_vf_stop_txqs(p_hwfn, vf, req->tx_qid, qid_usage_idx);
 	if (!rc)
 		status = PFVF_STATUS_SUCCESS;
 
@@ -2449,7 +2656,7 @@ static void qed_iov_vf_mbx_update_rxqs(struct qed_hwfn *p_hwfn,
 	u8 status = PFVF_STATUS_FAILURE;
 	u8 complete_event_flg;
 	u8 complete_cqe_flg;
-	u16 qid;
+	u8 qid_usage_idx;
 	int rc;
 	u8 i;
 
@@ -2457,19 +2664,42 @@ static void qed_iov_vf_mbx_update_rxqs(struct qed_hwfn *p_hwfn,
 	complete_cqe_flg = !!(req->flags & VFPF_RXQ_UPD_COMPLETE_CQE_FLAG);
 	complete_event_flg = !!(req->flags & VFPF_RXQ_UPD_COMPLETE_EVENT_FLAG);
 
-	/* Validate inputs */
-	for (i = req->rx_qid; i < req->rx_qid + req->num_rxqs; i++)
+	qid_usage_idx = qed_iov_vf_mbx_qid(p_hwfn, vf, false);
+	if (qid_usage_idx == QED_IOV_QID_INVALID)
+		goto out;
+
+	/* There shouldn't exist a VF that uses queue-qids yet uses this
+	 * API with multiple Rx queues. Validate this.
+	 */
+	if ((vf->acquire.vfdev_info.capabilities &
+	     VFPF_ACQUIRE_CAP_QUEUE_QIDS) && req->num_rxqs != 1) {
+		DP_VERBOSE(p_hwfn, QED_MSG_IOV,
+			   "VF[%d] supports QIDs but sends multiple queues\n",
+			   vf->relative_vf_id);
+		goto out;
+	}
+
+	/* Validate inputs - for the legacy case this is still true since
+	 * qid_usage_idx for each Rx queue would be LEGACY_QID_RX.
+	 */
+	for (i = req->rx_qid; i < req->rx_qid + req->num_rxqs; i++) {
 		if (!qed_iov_validate_rxq(p_hwfn, vf, i,
-					  QED_IOV_VALIDATE_Q_ENABLE)) {
-			DP_INFO(p_hwfn, "VF[%d]: Incorrect Rxqs [%04x, %02x]\n",
-				vf->relative_vf_id, req->rx_qid, req->num_rxqs);
+					  QED_IOV_VALIDATE_Q_NA) ||
+		    !vf->vf_queues[i].cids[qid_usage_idx].p_cid ||
+		    vf->vf_queues[i].cids[qid_usage_idx].b_is_tx) {
+			DP_VERBOSE(p_hwfn, QED_MSG_IOV,
+				   "VF[%d]: Incorrect Rxqs [%04x, %02x]\n",
+				   vf->relative_vf_id, req->rx_qid,
+				   req->num_rxqs);
 			goto out;
 		}
+	}
 
 	/* Prepare the handlers */
 	for (i = 0; i < req->num_rxqs; i++) {
-		qid = req->rx_qid + i;
-		handlers[i] = vf->vf_queues[qid].p_rx_cid;
+		u16 qid = req->rx_qid + i;
+
+		handlers[i] = vf->vf_queues[qid].cids[qid_usage_idx].p_cid;
 	}
 
 	rc = qed_sp_eth_rx_queues_update(p_hwfn, (void **)&handlers,
@@ -2683,6 +2913,8 @@ qed_iov_vp_update_rss_param(struct qed_hwfn *p_hwfn,
 			   (1 << p_rss_tlv->rss_table_size_log));
 
 	for (i = 0; i < table_size; i++) {
+		struct qed_queue_cid *p_cid;
+
 		q_idx = p_rss_tlv->rss_ind_table[i];
 		if (!qed_iov_validate_rxq(p_hwfn, vf, q_idx,
 					  QED_IOV_VALIDATE_Q_ENABLE)) {
@@ -2694,7 +2926,8 @@ qed_iov_vp_update_rss_param(struct qed_hwfn *p_hwfn,
 			goto out;
 		}
 
-		p_rss->rss_ind_table[i] = vf->vf_queues[q_idx].p_rx_cid;
+		p_cid = qed_iov_get_vf_rx_queue_cid(&vf->vf_queues[q_idx]);
+		p_rss->rss_ind_table[i] = p_cid;
 	}
 
 	p_data->rss_params = p_rss;
@@ -3610,8 +3843,10 @@ static void qed_sriov_vfpf_malicious(struct qed_hwfn *p_hwfn,
 	}
 }
 
-int qed_sriov_eqe_event(struct qed_hwfn *p_hwfn,
-			u8 opcode, __le16 echo, union event_ring_data *data)
+static int qed_sriov_eqe_event(struct qed_hwfn *p_hwfn,
+			       u8 opcode,
+			       __le16 echo,
+			       union event_ring_data *data, u8 fw_return_code)
 {
 	switch (opcode) {
 	case COMMON_EVENT_VF_PF_CHANNEL:
diff --git a/drivers/net/ethernet/qlogic/qed/qed_sriov.h b/drivers/net/ethernet/qlogic/qed/qed_sriov.h
index 81a497ce6585..c2e44bce398c 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_sriov.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_sriov.h
@@ -149,12 +149,21 @@ struct qed_iov_vf_mbx {
 	struct vfpf_first_tlv first_tlv;
 };
 
-struct qed_vf_q_info {
+#define QED_IOV_LEGACY_QID_RX (0)
+#define QED_IOV_LEGACY_QID_TX (1)
+#define QED_IOV_QID_INVALID (0xFE)
+
+struct qed_vf_queue_cid {
+	bool b_is_tx;
+	struct qed_queue_cid *p_cid;
+};
+
+/* Describes a qzone associated with the VF */
+struct qed_vf_queue {
 	u16 fw_rx_qid;
-	struct qed_queue_cid *p_rx_cid;
 	u16 fw_tx_qid;
-	struct qed_queue_cid *p_tx_cid;
-	u8 fw_cid;
+
+	struct qed_vf_queue_cid cids[MAX_QUEUES_PER_QZONE];
 };
 
 enum vf_state {
@@ -212,7 +221,8 @@ struct qed_vf_info {
 
 	u8 num_mac_filters;
 	u8 num_vlan_filters;
-	struct qed_vf_q_info vf_queues[QED_MAX_VF_CHAINS_PER_PF];
+
+	struct qed_vf_queue vf_queues[QED_MAX_VF_CHAINS_PER_PF];
 	u16 igu_sbs[QED_MAX_VF_CHAINS_PER_PF];
 	u8 num_active_rxqs;
 	struct qed_public_vf_info p_vf_info;
@@ -316,9 +326,8 @@ int qed_iov_alloc(struct qed_hwfn *p_hwfn);
  * @brief qed_iov_setup - setup sriov related resources
  *
  * @param p_hwfn
- * @param p_ptt
  */
-void qed_iov_setup(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt);
+void qed_iov_setup(struct qed_hwfn *p_hwfn);
 
 /**
  * @brief qed_iov_free - free sriov related resources
@@ -335,17 +344,6 @@ void qed_iov_free(struct qed_hwfn *p_hwfn);
 void qed_iov_free_hw_info(struct qed_dev *cdev);
 
 /**
- * @brief qed_sriov_eqe_event - handle async sriov event arrived on eqe.
- *
- * @param p_hwfn
- * @param opcode
- * @param echo
- * @param data
- */
-int qed_sriov_eqe_event(struct qed_hwfn *p_hwfn,
-			u8 opcode, __le16 echo, union event_ring_data *data);
-
-/**
  * @brief Mark structs of vfs that have been FLR-ed.
  *
  * @param p_hwfn
@@ -397,7 +395,7 @@ static inline int qed_iov_alloc(struct qed_hwfn *p_hwfn)
 	return 0;
 }
 
-static inline void qed_iov_setup(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt)
+static inline void qed_iov_setup(struct qed_hwfn *p_hwfn)
 {
 }
 
@@ -409,13 +407,6 @@ static inline void qed_iov_free_hw_info(struct qed_dev *cdev)
 {
 }
 
-static inline int qed_sriov_eqe_event(struct qed_hwfn *p_hwfn,
-				      u8 opcode,
-				      __le16 echo, union event_ring_data *data)
-{
-	return -EINVAL;
-}
-
 static inline bool qed_iov_mark_vf_flr(struct qed_hwfn *p_hwfn,
 				       u32 *disabled_vfs)
 {
diff --git a/drivers/net/ethernet/qlogic/qed/qed_vf.c b/drivers/net/ethernet/qlogic/qed/qed_vf.c
index 11d71e5eea14..1926d1ed439f 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_vf.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_vf.c
@@ -153,6 +153,77 @@ static int qed_send_msg2pf(struct qed_hwfn *p_hwfn, u8 *done, u32 resp_size)
 	return rc;
 }
 
+static void qed_vf_pf_add_qid(struct qed_hwfn *p_hwfn,
+			      struct qed_queue_cid *p_cid)
+{
+	struct qed_vf_iov *p_iov = p_hwfn->vf_iov_info;
+	struct vfpf_qid_tlv *p_qid_tlv;
+
+	/* Only add QIDs for the queue if it was negotiated with PF */
+	if (!(p_iov->acquire_resp.pfdev_info.capabilities &
+	      PFVF_ACQUIRE_CAP_QUEUE_QIDS))
+		return;
+
+	p_qid_tlv = qed_add_tlv(p_hwfn, &p_iov->offset,
+				CHANNEL_TLV_QID, sizeof(*p_qid_tlv));
+	p_qid_tlv->qid = p_cid->qid_usage_idx;
+}
+
+int _qed_vf_pf_release(struct qed_hwfn *p_hwfn, bool b_final)
+{
+	struct qed_vf_iov *p_iov = p_hwfn->vf_iov_info;
+	struct pfvf_def_resp_tlv *resp;
+	struct vfpf_first_tlv *req;
+	u32 size;
+	int rc;
+
+	/* clear mailbox and prep first tlv */
+	req = qed_vf_pf_prep(p_hwfn, CHANNEL_TLV_RELEASE, sizeof(*req));
+
+	/* add list termination tlv */
+	qed_add_tlv(p_hwfn, &p_iov->offset,
+		    CHANNEL_TLV_LIST_END, sizeof(struct channel_list_end_tlv));
+
+	resp = &p_iov->pf2vf_reply->default_resp;
+	rc = qed_send_msg2pf(p_hwfn, &resp->hdr.status, sizeof(*resp));
+
+	if (!rc && resp->hdr.status != PFVF_STATUS_SUCCESS)
+		rc = -EAGAIN;
+
+	qed_vf_pf_req_end(p_hwfn, rc);
+	if (!b_final)
+		return rc;
+
+	p_hwfn->b_int_enabled = 0;
+
+	if (p_iov->vf2pf_request)
+		dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+				  sizeof(union vfpf_tlvs),
+				  p_iov->vf2pf_request,
+				  p_iov->vf2pf_request_phys);
+	if (p_iov->pf2vf_reply)
+		dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+				  sizeof(union pfvf_tlvs),
+				  p_iov->pf2vf_reply, p_iov->pf2vf_reply_phys);
+
+	if (p_iov->bulletin.p_virt) {
+		size = sizeof(struct qed_bulletin_content);
+		dma_free_coherent(&p_hwfn->cdev->pdev->dev,
+				  size,
+				  p_iov->bulletin.p_virt, p_iov->bulletin.phys);
+	}
+
+	kfree(p_hwfn->vf_iov_info);
+	p_hwfn->vf_iov_info = NULL;
+
+	return rc;
+}
+
+int qed_vf_pf_release(struct qed_hwfn *p_hwfn)
+{
+	return _qed_vf_pf_release(p_hwfn, true);
+}
+
 #define VF_ACQUIRE_THRESH 3
 static void qed_vf_pf_acquire_reduce_resc(struct qed_hwfn *p_hwfn,
 					  struct vf_pf_resc_request *p_req,
@@ -160,7 +231,7 @@ static void qed_vf_pf_acquire_reduce_resc(struct qed_hwfn *p_hwfn,
 {
 	DP_VERBOSE(p_hwfn,
 		   QED_MSG_IOV,
-		   "PF unwilling to fullill resource request: rxq [%02x/%02x] txq [%02x/%02x] sbs [%02x/%02x] mac [%02x/%02x] vlan [%02x/%02x] mc [%02x/%02x]. Try PF recommended amount\n",
+		   "PF unwilling to fullill resource request: rxq [%02x/%02x] txq [%02x/%02x] sbs [%02x/%02x] mac [%02x/%02x] vlan [%02x/%02x] mc [%02x/%02x] cids [%02x/%02x]. Try PF recommended amount\n",
 		   p_req->num_rxqs,
 		   p_resp->num_rxqs,
 		   p_req->num_rxqs,
@@ -171,7 +242,8 @@ static void qed_vf_pf_acquire_reduce_resc(struct qed_hwfn *p_hwfn,
 		   p_resp->num_mac_filters,
 		   p_req->num_vlan_filters,
 		   p_resp->num_vlan_filters,
-		   p_req->num_mc_filters, p_resp->num_mc_filters);
+		   p_req->num_mc_filters,
+		   p_resp->num_mc_filters, p_req->num_cids, p_resp->num_cids);
 
 	/* humble our request */
 	p_req->num_txqs = p_resp->num_txqs;
@@ -180,6 +252,7 @@ static void qed_vf_pf_acquire_reduce_resc(struct qed_hwfn *p_hwfn,
 	p_req->num_mac_filters = p_resp->num_mac_filters;
 	p_req->num_vlan_filters = p_resp->num_vlan_filters;
 	p_req->num_mc_filters = p_resp->num_mc_filters;
+	p_req->num_cids = p_resp->num_cids;
 }
 
 static int qed_vf_pf_acquire(struct qed_hwfn *p_hwfn)
@@ -204,6 +277,7 @@ static int qed_vf_pf_acquire(struct qed_hwfn *p_hwfn)
 	p_resc->num_sbs = QED_MAX_VF_CHAINS_PER_PF;
 	p_resc->num_mac_filters = QED_ETH_VF_NUM_MAC_FILTERS;
 	p_resc->num_vlan_filters = QED_ETH_VF_NUM_VLAN_FILTERS;
+	p_resc->num_cids = QED_ETH_VF_DEFAULT_NUM_CIDS;
 
 	req->vfdev_info.os_type = VFPF_ACQUIRE_OS_LINUX;
 	req->vfdev_info.fw_major = FW_MAJOR_VERSION;
@@ -216,6 +290,13 @@ static int qed_vf_pf_acquire(struct qed_hwfn *p_hwfn)
 	/* Fill capability field with any non-deprecated config we support */
 	req->vfdev_info.capabilities |= VFPF_ACQUIRE_CAP_100G;
 
+	/* If we've mapped the doorbell bar, try using queue qids */
+	if (p_iov->b_doorbell_bar) {
+		req->vfdev_info.capabilities |= VFPF_ACQUIRE_CAP_PHYSICAL_BAR |
+						VFPF_ACQUIRE_CAP_QUEUE_QIDS;
+		p_resc->num_cids = QED_ETH_VF_MAX_NUM_CIDS;
+	}
+
 	/* pf 2 vf bulletin board address */
 	req->bulletin_addr = p_iov->bulletin.phys;
 	req->bulletin_size = p_iov->bulletin.size;
@@ -307,6 +388,13 @@ static int qed_vf_pf_acquire(struct qed_hwfn *p_hwfn)
 	if (req->vfdev_info.capabilities & VFPF_ACQUIRE_CAP_PRE_FP_HSI)
 		p_iov->b_pre_fp_hsi = true;
 
+	/* In case PF doesn't support multi-queue Tx, update the number of
+	 * CIDs to reflect the number of queues [older PFs didn't fill that
+	 * field].
+	 */
+	if (!(resp->pfdev_info.capabilities & PFVF_ACQUIRE_CAP_QUEUE_QIDS))
+		resp->resc.num_cids = resp->resc.num_rxqs + resp->resc.num_txqs;
+
 	/* Update bulletin board size with response from PF */
 	p_iov->bulletin.size = resp->bulletin_size;
 
@@ -338,10 +426,27 @@ exit:
 	return rc;
 }
 
+u32 qed_vf_hw_bar_size(struct qed_hwfn *p_hwfn, enum BAR_ID bar_id)
+{
+	u32 bar_size;
+
+	/* Regview size is fixed */
+	if (bar_id == BAR_ID_0)
+		return 1 << 17;
+
+	/* Doorbell is received from PF */
+	bar_size = p_hwfn->vf_iov_info->acquire_resp.pfdev_info.bar_size;
+	if (bar_size)
+		return 1 << bar_size;
+	return 0;
+}
+
 int qed_vf_hw_prepare(struct qed_hwfn *p_hwfn)
 {
+	struct qed_hwfn *p_lead = QED_LEADING_HWFN(p_hwfn->cdev);
 	struct qed_vf_iov *p_iov;
 	u32 reg;
+	int rc;
 
 	/* Set number of hwfns - might be overriden once leading hwfn learns
 	 * actual configuration from PF.
@@ -349,10 +454,6 @@ int qed_vf_hw_prepare(struct qed_hwfn *p_hwfn)
 	if (IS_LEAD_HWFN(p_hwfn))
 		p_hwfn->cdev->num_hwfns = 1;
 
-	/* Set the doorbell bar. Assumption: regview is set */
-	p_hwfn->doorbells = (u8 __iomem *)p_hwfn->regview +
-					  PXP_VF_BAR0_START_DQ;
-
 	reg = PXP_VF_BAR0_ME_OPAQUE_ADDRESS;
 	p_hwfn->hw_info.opaque_fid = (u16)REG_RD(p_hwfn, reg);
 
@@ -364,6 +465,30 @@ int qed_vf_hw_prepare(struct qed_hwfn *p_hwfn)
 	if (!p_iov)
 		return -ENOMEM;
 
+	/* Doorbells are tricky; Upper-layer has alreday set the hwfn doorbell
+	 * value, but there are several incompatibily scenarios where that
+	 * would be incorrect and we'd need to override it.
+	 */
+	if (!p_hwfn->doorbells) {
+		p_hwfn->doorbells = (u8 __iomem *)p_hwfn->regview +
+						  PXP_VF_BAR0_START_DQ;
+	} else if (p_hwfn == p_lead) {
+		/* For leading hw-function, value is always correct, but need
+		 * to handle scenario where legacy PF would not support 100g
+		 * mapped bars later.
+		 */
+		p_iov->b_doorbell_bar = true;
+	} else {
+		/* here, value would be correct ONLY if the leading hwfn
+		 * received indication that mapped-bars are supported.
+		 */
+		if (p_lead->vf_iov_info->b_doorbell_bar)
+			p_iov->b_doorbell_bar = true;
+		else
+			p_hwfn->doorbells = (u8 __iomem *)
+			    p_hwfn->regview + PXP_VF_BAR0_START_DQ;
+	}
+
 	/* Allocate vf2pf msg */
 	p_iov->vf2pf_request = dma_alloc_coherent(&p_hwfn->cdev->pdev->dev,
 						  sizeof(union vfpf_tlvs),
@@ -403,7 +528,33 @@ int qed_vf_hw_prepare(struct qed_hwfn *p_hwfn)
 
 	p_hwfn->hw_info.personality = QED_PCI_ETH;
 
-	return qed_vf_pf_acquire(p_hwfn);
+	rc = qed_vf_pf_acquire(p_hwfn);
+
+	/* If VF is 100g using a mapped bar and PF is too old to support that,
+	 * acquisition would succeed - but the VF would have no way knowing
+	 * the size of the doorbell bar configured in HW and thus will not
+	 * know how to split it for 2nd hw-function.
+	 * In this case we re-try without the indication of the mapped
+	 * doorbell.
+	 */
+	if (!rc && p_iov->b_doorbell_bar &&
+	    !qed_vf_hw_bar_size(p_hwfn, BAR_ID_1) &&
+	    (p_hwfn->cdev->num_hwfns > 1)) {
+		rc = _qed_vf_pf_release(p_hwfn, false);
+		if (rc)
+			return rc;
+
+		p_iov->b_doorbell_bar = false;
+		p_hwfn->doorbells = (u8 __iomem *)p_hwfn->regview +
+						  PXP_VF_BAR0_START_DQ;
+		rc = qed_vf_pf_acquire(p_hwfn);
+	}
+
+	DP_VERBOSE(p_hwfn, QED_MSG_IOV,
+		   "Regview [%p], Doorbell [%p], Device-doorbell [%p]\n",
+		   p_hwfn->regview, p_hwfn->doorbells, p_hwfn->cdev->doorbells);
+
+	return rc;
 
 free_vf2pf_request:
 	dma_free_coherent(&p_hwfn->cdev->pdev->dev,
@@ -588,8 +739,8 @@ qed_vf_pf_rxq_start(struct qed_hwfn *p_hwfn,
 	req->cqe_pbl_addr = cqe_pbl_addr;
 	req->cqe_pbl_size = cqe_pbl_size;
 	req->rxq_addr = bd_chain_phys_addr;
-	req->hw_sb = p_cid->rel.sb;
-	req->sb_index = p_cid->rel.sb_idx;
+	req->hw_sb = p_cid->sb_igu_id;
+	req->sb_index = p_cid->sb_idx;
 	req->bd_max_bytes = bd_max_bytes;
 	req->stat_id = -1;
 
@@ -609,6 +760,9 @@ qed_vf_pf_rxq_start(struct qed_hwfn *p_hwfn,
 		__internal_ram_wr(p_hwfn, *pp_prod, sizeof(u32),
 				  (u32 *)(&init_prod_val));
 	}
+
+	qed_vf_pf_add_qid(p_hwfn, p_cid);
+
 	/* add list termination tlv */
 	qed_add_tlv(p_hwfn, &p_iov->offset,
 		    CHANNEL_TLV_LIST_END, sizeof(struct channel_list_end_tlv));
@@ -657,6 +811,8 @@ int qed_vf_pf_rxq_stop(struct qed_hwfn *p_hwfn,
 	req->num_rxqs = 1;
 	req->cqe_completion = cqe_completion;
 
+	qed_vf_pf_add_qid(p_hwfn, p_cid);
+
 	/* add list termination tlv */
 	qed_add_tlv(p_hwfn, &p_iov->offset,
 		    CHANNEL_TLV_LIST_END, sizeof(struct channel_list_end_tlv));
@@ -697,8 +853,10 @@ qed_vf_pf_txq_start(struct qed_hwfn *p_hwfn,
 	/* Tx */
 	req->pbl_addr = pbl_addr;
 	req->pbl_size = pbl_size;
-	req->hw_sb = p_cid->rel.sb;
-	req->sb_index = p_cid->rel.sb_idx;
+	req->hw_sb = p_cid->sb_igu_id;
+	req->sb_index = p_cid->sb_idx;
+
+	qed_vf_pf_add_qid(p_hwfn, p_cid);
 
 	/* add list termination tlv */
 	qed_add_tlv(p_hwfn, &p_iov->offset,
@@ -728,8 +886,8 @@ qed_vf_pf_txq_start(struct qed_hwfn *p_hwfn,
 	}
 
 	DP_VERBOSE(p_hwfn, QED_MSG_IOV,
-		   "Txq[0x%02x]: doorbell at %p [offset 0x%08x]\n",
-		   qid, *pp_doorbell, resp->offset);
+		   "Txq[0x%02x.%02x]: doorbell at %p [offset 0x%08x]\n",
+		   qid, p_cid->qid_usage_idx, *pp_doorbell, resp->offset);
 exit:
 	qed_vf_pf_req_end(p_hwfn, rc);
 
@@ -749,6 +907,8 @@ int qed_vf_pf_txq_stop(struct qed_hwfn *p_hwfn, struct qed_queue_cid *p_cid)
 	req->tx_qid = p_cid->rel.queue_id;
 	req->num_txqs = 1;
 
+	qed_vf_pf_add_qid(p_hwfn, p_cid);
+
 	/* add list termination tlv */
 	qed_add_tlv(p_hwfn, &p_iov->offset,
 		    CHANNEL_TLV_LIST_END, sizeof(struct channel_list_end_tlv));
@@ -792,9 +952,12 @@ int qed_vf_pf_vport_start(struct qed_hwfn *p_hwfn,
 	req->only_untagged = only_untagged;
 
 	/* status blocks */
-	for (i = 0; i < p_hwfn->vf_iov_info->acquire_resp.resc.num_sbs; i++)
-		if (p_hwfn->sbs_info[i])
-			req->sb_addr[i] = p_hwfn->sbs_info[i]->sb_phys;
+	for (i = 0; i < p_hwfn->vf_iov_info->acquire_resp.resc.num_sbs; i++) {
+		struct qed_sb_info *p_sb = p_hwfn->vf_iov_info->sbs_info[i];
+
+		if (p_sb)
+			req->sb_addr[i] = p_sb->sb_phys;
+	}
 
 	/* add list termination tlv */
 	qed_add_tlv(p_hwfn, &p_iov->offset,
@@ -1095,54 +1258,6 @@ exit:
 	return rc;
 }
 
-int qed_vf_pf_release(struct qed_hwfn *p_hwfn)
-{
-	struct qed_vf_iov *p_iov = p_hwfn->vf_iov_info;
-	struct pfvf_def_resp_tlv *resp;
-	struct vfpf_first_tlv *req;
-	u32 size;
-	int rc;
-
-	/* clear mailbox and prep first tlv */
-	req = qed_vf_pf_prep(p_hwfn, CHANNEL_TLV_RELEASE, sizeof(*req));
-
-	/* add list termination tlv */
-	qed_add_tlv(p_hwfn, &p_iov->offset,
-		    CHANNEL_TLV_LIST_END, sizeof(struct channel_list_end_tlv));
-
-	resp = &p_iov->pf2vf_reply->default_resp;
-	rc = qed_send_msg2pf(p_hwfn, &resp->hdr.status, sizeof(*resp));
-
-	if (!rc && resp->hdr.status != PFVF_STATUS_SUCCESS)
-		rc = -EAGAIN;
-
-	qed_vf_pf_req_end(p_hwfn, rc);
-
-	p_hwfn->b_int_enabled = 0;
-
-	if (p_iov->vf2pf_request)
-		dma_free_coherent(&p_hwfn->cdev->pdev->dev,
-				  sizeof(union vfpf_tlvs),
-				  p_iov->vf2pf_request,
-				  p_iov->vf2pf_request_phys);
-	if (p_iov->pf2vf_reply)
-		dma_free_coherent(&p_hwfn->cdev->pdev->dev,
-				  sizeof(union pfvf_tlvs),
-				  p_iov->pf2vf_reply, p_iov->pf2vf_reply_phys);
-
-	if (p_iov->bulletin.p_virt) {
-		size = sizeof(struct qed_bulletin_content);
-		dma_free_coherent(&p_hwfn->cdev->pdev->dev,
-				  size,
-				  p_iov->bulletin.p_virt, p_iov->bulletin.phys);
-	}
-
-	kfree(p_hwfn->vf_iov_info);
-	p_hwfn->vf_iov_info = NULL;
-
-	return rc;
-}
-
 void qed_vf_pf_filter_mcast(struct qed_hwfn *p_hwfn,
 			    struct qed_filter_mcast *p_filter_cmd)
 {
@@ -1240,6 +1355,24 @@ u16 qed_vf_get_igu_sb_id(struct qed_hwfn *p_hwfn, u16 sb_id)
 	return p_iov->acquire_resp.resc.hw_sbs[sb_id].hw_sb_id;
 }
 
+void qed_vf_set_sb_info(struct qed_hwfn *p_hwfn,
+			u16 sb_id, struct qed_sb_info *p_sb)
+{
+	struct qed_vf_iov *p_iov = p_hwfn->vf_iov_info;
+
+	if (!p_iov) {
+		DP_NOTICE(p_hwfn, "vf_sriov_info isn't initialized\n");
+		return;
+	}
+
+	if (sb_id >= PFVF_MAX_SBS_PER_VF) {
+		DP_NOTICE(p_hwfn, "Can't configure SB %04x\n", sb_id);
+		return;
+	}
+
+	p_iov->sbs_info[sb_id] = p_sb;
+}
+
 int qed_vf_read_bulletin(struct qed_hwfn *p_hwfn, u8 *p_change)
 {
 	struct qed_vf_iov *p_iov = p_hwfn->vf_iov_info;
@@ -1342,6 +1475,16 @@ void qed_vf_get_num_rxqs(struct qed_hwfn *p_hwfn, u8 *num_rxqs)
 	*num_rxqs = p_hwfn->vf_iov_info->acquire_resp.resc.num_rxqs;
 }
 
+void qed_vf_get_num_txqs(struct qed_hwfn *p_hwfn, u8 *num_txqs)
+{
+	*num_txqs = p_hwfn->vf_iov_info->acquire_resp.resc.num_txqs;
+}
+
+void qed_vf_get_num_cids(struct qed_hwfn *p_hwfn, u8 *num_cids)
+{
+	*num_cids = p_hwfn->vf_iov_info->acquire_resp.resc.num_cids;
+}
+
 void qed_vf_get_port_mac(struct qed_hwfn *p_hwfn, u8 *port_mac)
 {
 	memcpy(port_mac,
diff --git a/drivers/net/ethernet/qlogic/qed/qed_vf.h b/drivers/net/ethernet/qlogic/qed/qed_vf.h
index 34ac70b0e5fe..34d9b882a780 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_vf.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_vf.h
@@ -46,7 +46,8 @@ struct vf_pf_resc_request {
 	u8 num_mac_filters;
 	u8 num_vlan_filters;
 	u8 num_mc_filters;
-	u16 padding;
+	u8 num_cids;
+	u8 padding;
 };
 
 struct hw_sb_info {
@@ -113,6 +114,17 @@ struct vfpf_acquire_tlv {
 	struct vf_pf_vfdev_info {
 #define VFPF_ACQUIRE_CAP_PRE_FP_HSI     (1 << 0) /* VF pre-FP hsi version */
 #define VFPF_ACQUIRE_CAP_100G		(1 << 1) /* VF can support 100g */
+	/* A requirement for supporting multi-Tx queues on a single queue-zone,
+	 * VF would pass qids as additional information whenever passing queue
+	 * references.
+	 */
+#define VFPF_ACQUIRE_CAP_QUEUE_QIDS     BIT(2)
+
+	/* The VF is using the physical bar. While this is mostly internal
+	 * to the VF, might affect the number of CIDs supported assuming
+	 * QUEUE_QIDS is set.
+	 */
+#define VFPF_ACQUIRE_CAP_PHYSICAL_BAR   BIT(3)
 		u64 capabilities;
 		u8 fw_major;
 		u8 fw_minor;
@@ -185,6 +197,9 @@ struct pfvf_acquire_resp_tlv {
  */
 #define PFVF_ACQUIRE_CAP_POST_FW_OVERRIDE	BIT(2)
 
+	/* PF expects queues to be received with additional qids */
+#define PFVF_ACQUIRE_CAP_QUEUE_QIDS             BIT(3)
+
 		u16 db_size;
 		u8 indices_per_sb;
 		u8 os_type;
@@ -193,7 +208,8 @@ struct pfvf_acquire_resp_tlv {
 		u16 chip_rev;
 		u8 dev_type;
 
-		u8 padding;
+		/* Doorbell bar size configured in HW: log(size) or 0 */
+		u8 bar_size;
 
 		struct pfvf_stats_info stats_info;
 
@@ -221,7 +237,8 @@ struct pfvf_acquire_resp_tlv {
 		u8 num_mac_filters;
 		u8 num_vlan_filters;
 		u8 num_mc_filters;
-		u8 padding[2];
+		u8 num_cids;
+		u8 padding;
 	} resc;
 
 	u32 bulletin_size;
@@ -234,6 +251,16 @@ struct pfvf_start_queue_resp_tlv {
 	u8 padding[4];
 };
 
+/* Extended queue information - additional index for reference inside qzone.
+ * If commmunicated between VF/PF, each TLV relating to queues should be
+ * extended by one such [or have a future base TLV that already contains info].
+ */
+struct vfpf_qid_tlv {
+	struct channel_tlv tl;
+	u8 qid;
+	u8 padding[3];
+};
+
 /* Setup Queue */
 struct vfpf_start_rxq_tlv {
 	struct vfpf_first_tlv first_tlv;
@@ -597,6 +624,8 @@ enum {
 	CHANNEL_TLV_VPORT_UPDATE_ACCEPT_ANY_VLAN,
 	CHANNEL_TLV_VPORT_UPDATE_SGE_TPA,
 	CHANNEL_TLV_UPDATE_TUNN_PARAM,
+	CHANNEL_TLV_RESERVED,
+	CHANNEL_TLV_QID,
 	CHANNEL_TLV_MAX,
 
 	/* Required for iterating over vport-update tlvs.
@@ -605,6 +634,12 @@ enum {
 	CHANNEL_TLV_VPORT_UPDATE_MAX = CHANNEL_TLV_VPORT_UPDATE_SGE_TPA + 1,
 };
 
+/* Default number of CIDs [total of both Rx and Tx] to be requested
+ * by default, and maximum possible number.
+ */
+#define QED_ETH_VF_DEFAULT_NUM_CIDS (32)
+#define QED_ETH_VF_MAX_NUM_CIDS (250)
+
 /* This data is held in the qed_hwfn structure for VFs only. */
 struct qed_vf_iov {
 	union vfpf_tlvs *vf2pf_request;
@@ -627,6 +662,19 @@ struct qed_vf_iov {
 	 * this has to be propagated as it affects the fastpath.
 	 */
 	bool b_pre_fp_hsi;
+
+	/* Current day VFs are passing the SBs physical address on vport
+	 * start, and as they lack an IGU mapping they need to store the
+	 * addresses of previously registered SBs.
+	 * Even if we were to change configuration flow, due to backward
+	 * compatibility [with older PFs] we'd still need to store these.
+	 */
+	struct qed_sb_info *sbs_info[PFVF_MAX_SBS_PER_VF];
+
+	/* Determines whether VF utilizes doorbells via limited register
+	 * bar or via the doorbell bar.
+	 */
+	bool b_doorbell_bar;
 };
 
 #ifdef CONFIG_QED_SRIOV
@@ -676,6 +724,22 @@ void qed_vf_get_link_caps(struct qed_hwfn *p_hwfn,
 void qed_vf_get_num_rxqs(struct qed_hwfn *p_hwfn, u8 *num_rxqs);
 
 /**
+ * @brief Get number of Rx queues allocated for VF by qed
+ *
+ *  @param p_hwfn
+ *  @param num_txqs - allocated RX queues
+ */
+void qed_vf_get_num_txqs(struct qed_hwfn *p_hwfn, u8 *num_txqs);
+
+/**
+ * @brief Get number of available connections [both Rx and Tx] for VF
+ *
+ * @param p_hwfn
+ * @param num_cids - allocated number of connections
+ */
+void qed_vf_get_num_cids(struct qed_hwfn *p_hwfn, u8 *num_cids);
+
+/**
  * @brief Get port mac address for VF
  *
  * @param p_hwfn
@@ -837,6 +901,16 @@ int qed_vf_pf_release(struct qed_hwfn *p_hwfn);
 u16 qed_vf_get_igu_sb_id(struct qed_hwfn *p_hwfn, u16 sb_id);
 
 /**
+ * @brief Stores [or removes] a configured sb_info.
+ *
+ * @param p_hwfn
+ * @param sb_id - zero-based SB index [for fastpath]
+ * @param sb_info - may be NULL [during removal].
+ */
+void qed_vf_set_sb_info(struct qed_hwfn *p_hwfn,
+			u16 sb_id, struct qed_sb_info *p_sb);
+
+/**
  * @brief qed_vf_pf_vport_start - perform vport start for VF.
  *
  * @param p_hwfn
@@ -917,6 +991,8 @@ void qed_iov_vf_task(struct work_struct *work);
 void qed_vf_set_vf_start_tunn_update_param(struct qed_tunnel_info *p_tun);
 int qed_vf_pf_tunnel_param_update(struct qed_hwfn *p_hwfn,
 				  struct qed_tunnel_info *p_tunn);
+
+u32 qed_vf_hw_bar_size(struct qed_hwfn *p_hwfn, enum BAR_ID bar_id);
 #else
 static inline void qed_vf_get_link_params(struct qed_hwfn *p_hwfn,
 					  struct qed_mcp_link_params *params)
@@ -938,6 +1014,14 @@ static inline void qed_vf_get_num_rxqs(struct qed_hwfn *p_hwfn, u8 *num_rxqs)
 {
 }
 
+static inline void qed_vf_get_num_txqs(struct qed_hwfn *p_hwfn, u8 *num_txqs)
+{
+}
+
+static inline void qed_vf_get_num_cids(struct qed_hwfn *p_hwfn, u8 *num_cids)
+{
+}
+
 static inline void qed_vf_get_port_mac(struct qed_hwfn *p_hwfn, u8 *port_mac)
 {
 }
@@ -1021,6 +1105,11 @@ static inline u16 qed_vf_get_igu_sb_id(struct qed_hwfn *p_hwfn, u16 sb_id)
 	return 0;
 }
 
+static inline void qed_vf_set_sb_info(struct qed_hwfn *p_hwfn, u16 sb_id,
+				      struct qed_sb_info *p_sb)
+{
+}
+
 static inline int qed_vf_pf_vport_start(struct qed_hwfn *p_hwfn,
 					u8 vport_id,
 					u16 mtu,
@@ -1089,6 +1178,13 @@ static inline int qed_vf_pf_tunnel_param_update(struct qed_hwfn *p_hwfn,
 {
 	return -EINVAL;
 }
+
+static inline u32
+qed_vf_hw_bar_size(struct qed_hwfn  *p_hwfn,
+		   enum BAR_ID bar_id)
+{
+	return 0;
+}
 #endif
 
 #endif
diff --git a/drivers/net/ethernet/qlogic/qede/Makefile b/drivers/net/ethernet/qlogic/qede/Makefile
index bc5f7c3b277d..75408fbb7680 100644
--- a/drivers/net/ethernet/qlogic/qede/Makefile
+++ b/drivers/net/ethernet/qlogic/qede/Makefile
@@ -2,4 +2,4 @@ obj-$(CONFIG_QEDE) := qede.o
 
 qede-y := qede_main.o qede_fp.o qede_filter.o qede_ethtool.o qede_ptp.o
 qede-$(CONFIG_DCB) += qede_dcbnl.o
-qede-$(CONFIG_QED_RDMA) += qede_roce.o
+qede-$(CONFIG_QED_RDMA) += qede_rdma.o
diff --git a/drivers/net/ethernet/qlogic/qede/qede.h b/drivers/net/ethernet/qlogic/qede/qede.h
index 9b4f08b6f9b9..4dfb238221f9 100644
--- a/drivers/net/ethernet/qlogic/qede/qede.h
+++ b/drivers/net/ethernet/qlogic/qede/qede.h
@@ -40,6 +40,7 @@
 #include <linux/kernel.h>
 #include <linux/mutex.h>
 #include <linux/bpf.h>
+#include <linux/qed/qede_rdma.h>
 #include <linux/io.h>
 #ifdef CONFIG_RFS_ACCEL
 #include <linux/cpu_rmap.h>
@@ -153,8 +154,8 @@ struct qede_vlan {
 struct qede_rdma_dev {
 	struct qedr_dev *qedr_dev;
 	struct list_head entry;
-	struct list_head roce_event_list;
-	struct workqueue_struct *roce_wq;
+	struct list_head rdma_event_list;
+	struct workqueue_struct *rdma_wq;
 };
 
 struct qede_ptp;
@@ -197,7 +198,6 @@ struct qede_dev {
 #define QEDE_TSS_COUNT(edev)	((edev)->num_queues - (edev)->fp_num_rx)
 
 	struct qed_int_info		int_info;
-	unsigned char			primary_mac[ETH_ALEN];
 
 	/* Smaller private varaiant of the RTNL lock */
 	struct mutex			qede_lock;
diff --git a/drivers/net/ethernet/qlogic/qede/qede_dcbnl.c b/drivers/net/ethernet/qlogic/qede/qede_dcbnl.c
index a9e7379313db..6e7747b9b95e 100644
--- a/drivers/net/ethernet/qlogic/qede/qede_dcbnl.c
+++ b/drivers/net/ethernet/qlogic/qede/qede_dcbnl.c
@@ -313,7 +313,6 @@ static const struct dcbnl_rtnl_ops qede_dcbnl_ops = {
 	.ieee_setets = qede_dcbnl_ieee_setets,
 	.ieee_getapp = qede_dcbnl_ieee_getapp,
 	.ieee_setapp = qede_dcbnl_ieee_setapp,
-	.getdcbx = qede_dcbnl_getdcbx,
 	.ieee_peer_getpfc = qede_dcbnl_ieee_peer_getpfc,
 	.ieee_peer_getets = qede_dcbnl_ieee_peer_getets,
 	.getstate = qede_dcbnl_getstate,
diff --git a/drivers/net/ethernet/qlogic/qede/qede_ethtool.c b/drivers/net/ethernet/qlogic/qede/qede_ethtool.c
index 172b292241a5..6a03d3e66cff 100644
--- a/drivers/net/ethernet/qlogic/qede/qede_ethtool.c
+++ b/drivers/net/ethernet/qlogic/qede/qede_ethtool.c
@@ -506,6 +506,14 @@ static int qede_set_link_ksettings(struct net_device *dev,
 		params.autoneg = false;
 		params.forced_speed = base->speed;
 		switch (base->speed) {
+		case SPEED_1000:
+			if (!(current_link.supported_caps &
+			      QED_LM_1000baseT_Full_BIT)) {
+				DP_INFO(edev, "1G speed not supported\n");
+				return -EINVAL;
+			}
+			params.adv_speeds = QED_LM_1000baseT_Full_BIT;
+			break;
 		case SPEED_10000:
 			if (!(current_link.supported_caps &
 			      QED_LM_10000baseKR_Full_BIT)) {
@@ -1282,7 +1290,8 @@ static int qede_selftest_transmit_traffic(struct qede_dev *edev,
 	struct qede_tx_queue *txq = NULL;
 	struct eth_tx_1st_bd *first_bd;
 	dma_addr_t mapping;
-	int i, idx, val;
+	int i, idx;
+	u16 val;
 
 	for_each_queue(i) {
 		if (edev->fp_array[i].type & QEDE_FASTPATH_TX) {
@@ -1297,14 +1306,15 @@ static int qede_selftest_transmit_traffic(struct qede_dev *edev,
 	}
 
 	/* Fill the entry in the SW ring and the BDs in the FW ring */
-	idx = txq->sw_tx_prod & NUM_TX_BDS_MAX;
+	idx = txq->sw_tx_prod;
 	txq->sw_tx_ring.skbs[idx].skb = skb;
 	first_bd = qed_chain_produce(&txq->tx_pbl);
 	memset(first_bd, 0, sizeof(*first_bd));
 	val = 1 << ETH_TX_1ST_BD_FLAGS_START_BD_SHIFT;
 	first_bd->data.bd_flags.bitfields = val;
 	val = skb->len & ETH_TX_DATA_1ST_BD_PKT_LEN_MASK;
-	first_bd->data.bitfields |= (val << ETH_TX_DATA_1ST_BD_PKT_LEN_SHIFT);
+	val = val << ETH_TX_DATA_1ST_BD_PKT_LEN_SHIFT;
+	first_bd->data.bitfields |= cpu_to_le16(val);
 
 	/* Map skb linear data for DMA and set in the first BD */
 	mapping = dma_map_single(&edev->pdev->dev, skb->data,
@@ -1317,10 +1327,10 @@ static int qede_selftest_transmit_traffic(struct qede_dev *edev,
 
 	/* update the first BD with the actual num BDs */
 	first_bd->data.nbds = 1;
-	txq->sw_tx_prod++;
+	txq->sw_tx_prod = (txq->sw_tx_prod + 1) % txq->num_tx_buffers;
 	/* 'next page' entries are counted in the producer value */
-	val = cpu_to_le16(qed_chain_get_prod_idx(&txq->tx_pbl));
-	txq->tx_db.data.bd_prod = val;
+	val = qed_chain_get_prod_idx(&txq->tx_pbl);
+	txq->tx_db.data.bd_prod = cpu_to_le16(val);
 
 	/* wmb makes sure that the BDs data is updated before updating the
 	 * producer, otherwise FW may read old data from the BDs.
@@ -1351,7 +1361,7 @@ static int qede_selftest_transmit_traffic(struct qede_dev *edev,
 	first_bd = (struct eth_tx_1st_bd *)qed_chain_consume(&txq->tx_pbl);
 	dma_unmap_single(&edev->pdev->dev, BD_UNMAP_ADDR(first_bd),
 			 BD_UNMAP_LEN(first_bd), DMA_TO_DEVICE);
-	txq->sw_tx_cons++;
+	txq->sw_tx_cons = (txq->sw_tx_cons + 1) % txq->num_tx_buffers;
 	txq->sw_tx_ring.skbs[idx].skb = NULL;
 
 	return 0;
diff --git a/drivers/net/ethernet/qlogic/qede/qede_filter.c b/drivers/net/ethernet/qlogic/qede/qede_filter.c
index 333876c19d7d..f939db5bac5f 100644
--- a/drivers/net/ethernet/qlogic/qede/qede_filter.c
+++ b/drivers/net/ethernet/qlogic/qede/qede_filter.c
@@ -495,12 +495,16 @@ void qede_force_mac(void *dev, u8 *mac, bool forced)
 {
 	struct qede_dev *edev = dev;
 
+	__qede_lock(edev);
+
 	/* MAC hints take effect only if we haven't set one already */
-	if (is_valid_ether_addr(edev->ndev->dev_addr) && !forced)
+	if (is_valid_ether_addr(edev->ndev->dev_addr) && !forced) {
+		__qede_unlock(edev);
 		return;
+	}
 
 	ether_addr_copy(edev->ndev->dev_addr, mac);
-	ether_addr_copy(edev->primary_mac, mac);
+	__qede_unlock(edev);
 }
 
 void qede_fill_rss_params(struct qede_dev *edev,
@@ -1033,6 +1037,7 @@ int qede_xdp(struct net_device *dev, struct netdev_xdp *xdp)
 		return qede_xdp_set(edev, xdp->prog);
 	case XDP_QUERY_PROG:
 		xdp->prog_attached = !!edev->xdp_prog;
+		xdp->prog_id = edev->xdp_prog ? edev->xdp_prog->aux->id : 0;
 		return 0;
 	default:
 		return -EINVAL;
@@ -1061,41 +1066,51 @@ int qede_set_mac_addr(struct net_device *ndev, void *p)
 {
 	struct qede_dev *edev = netdev_priv(ndev);
 	struct sockaddr *addr = p;
-	int rc;
-
-	ASSERT_RTNL(); /* @@@TBD To be removed */
+	int rc = 0;
 
-	DP_INFO(edev, "Set_mac_addr called\n");
+	/* Make sure the state doesn't transition while changing the MAC.
+	 * Also, all flows accessing the dev_addr field are doing that under
+	 * this lock.
+	 */
+	__qede_lock(edev);
 
 	if (!is_valid_ether_addr(addr->sa_data)) {
 		DP_NOTICE(edev, "The MAC address is not valid\n");
-		return -EFAULT;
+		rc = -EFAULT;
+		goto out;
 	}
 
 	if (!edev->ops->check_mac(edev->cdev, addr->sa_data)) {
-		DP_NOTICE(edev, "qed prevents setting MAC\n");
-		return -EINVAL;
+		DP_NOTICE(edev, "qed prevents setting MAC %pM\n",
+			  addr->sa_data);
+		rc = -EINVAL;
+		goto out;
+	}
+
+	if (edev->state == QEDE_STATE_OPEN) {
+		/* Remove the previous primary mac */
+		rc = qede_set_ucast_rx_mac(edev, QED_FILTER_XCAST_TYPE_DEL,
+					   ndev->dev_addr);
+		if (rc)
+			goto out;
 	}
 
 	ether_addr_copy(ndev->dev_addr, addr->sa_data);
+	DP_INFO(edev, "Setting device MAC to %pM\n", addr->sa_data);
 
-	if (!netif_running(ndev))  {
-		DP_NOTICE(edev, "The device is currently down\n");
-		return 0;
+	if (edev->state != QEDE_STATE_OPEN) {
+		DP_VERBOSE(edev, NETIF_MSG_IFDOWN,
+			   "The device is currently down\n");
+		goto out;
 	}
 
-	/* Remove the previous primary mac */
-	rc = qede_set_ucast_rx_mac(edev, QED_FILTER_XCAST_TYPE_DEL,
-				   edev->primary_mac);
-	if (rc)
-		return rc;
-
-	edev->ops->common->update_mac(edev->cdev, addr->sa_data);
+	edev->ops->common->update_mac(edev->cdev, ndev->dev_addr);
 
-	/* Add MAC filter according to the new unicast HW MAC address */
-	ether_addr_copy(edev->primary_mac, ndev->dev_addr);
-	return qede_set_ucast_rx_mac(edev, QED_FILTER_XCAST_TYPE_ADD,
-				      edev->primary_mac);
+	rc = qede_set_ucast_rx_mac(edev, QED_FILTER_XCAST_TYPE_ADD,
+				   ndev->dev_addr);
+out:
+	__qede_unlock(edev);
+	return rc;
 }
 
 static int
@@ -1200,7 +1215,7 @@ void qede_config_rx_mode(struct net_device *ndev)
 	 * (configrue / leave the primary mac)
 	 */
 	rc = qede_set_ucast_rx_mac(edev, QED_FILTER_XCAST_TYPE_REPLACE,
-				   edev->primary_mac);
+				   edev->ndev->dev_addr);
 	if (rc)
 		goto out;
 
diff --git a/drivers/net/ethernet/qlogic/qede/qede_fp.c b/drivers/net/ethernet/qlogic/qede/qede_fp.c
index 7b6f41d06245..6fc854b120b0 100644
--- a/drivers/net/ethernet/qlogic/qede/qede_fp.c
+++ b/drivers/net/ethernet/qlogic/qede/qede_fp.c
@@ -99,7 +99,7 @@ int qede_alloc_rx_buffer(struct qede_rx_queue *rxq, bool allow_lazy)
 /* Unmap the data and free skb */
 int qede_free_tx_pkt(struct qede_dev *edev, struct qede_tx_queue *txq, int *len)
 {
-	u16 idx = txq->sw_tx_cons & NUM_TX_BDS_MAX;
+	u16 idx = txq->sw_tx_cons;
 	struct sk_buff *skb = txq->sw_tx_ring.skbs[idx].skb;
 	struct eth_tx_1st_bd *first_bd;
 	struct eth_tx_bd *tx_data_bd;
@@ -156,7 +156,7 @@ static void qede_free_failed_tx_pkt(struct qede_tx_queue *txq,
 				    struct eth_tx_1st_bd *first_bd,
 				    int nbd, bool data_split)
 {
-	u16 idx = txq->sw_tx_prod & NUM_TX_BDS_MAX;
+	u16 idx = txq->sw_tx_prod;
 	struct sk_buff *skb = txq->sw_tx_ring.skbs[idx].skb;
 	struct eth_tx_bd *tx_data_bd;
 	int i, split_bd_len = 0;
@@ -333,8 +333,9 @@ static int qede_xdp_xmit(struct qede_dev *edev, struct qede_fastpath *fp,
 			 struct sw_rx_data *metadata, u16 padding, u16 length)
 {
 	struct qede_tx_queue *txq = fp->xdp_tx;
-	u16 idx = txq->sw_tx_prod & NUM_TX_BDS_MAX;
 	struct eth_tx_1st_bd *first_bd;
+	u16 idx = txq->sw_tx_prod;
+	u16 val;
 
 	if (!qed_chain_get_elem_left(&txq->tx_pbl)) {
 		txq->stopped_cnt++;
@@ -346,9 +347,11 @@ static int qede_xdp_xmit(struct qede_dev *edev, struct qede_fastpath *fp,
 	memset(first_bd, 0, sizeof(*first_bd));
 	first_bd->data.bd_flags.bitfields =
 	    BIT(ETH_TX_1ST_BD_FLAGS_START_BD_SHIFT);
-	first_bd->data.bitfields |=
-	    (length & ETH_TX_DATA_1ST_BD_PKT_LEN_MASK) <<
-	    ETH_TX_DATA_1ST_BD_PKT_LEN_SHIFT;
+
+	val = (length & ETH_TX_DATA_1ST_BD_PKT_LEN_MASK) <<
+	       ETH_TX_DATA_1ST_BD_PKT_LEN_SHIFT;
+
+	first_bd->data.bitfields |= cpu_to_le16(val);
 	first_bd->data.nbds = 1;
 
 	/* We can safely ignore the offset, as it's 0 for XDP */
@@ -363,7 +366,7 @@ static int qede_xdp_xmit(struct qede_dev *edev, struct qede_fastpath *fp,
 
 	txq->sw_tx_ring.xdp[idx].page = metadata->data;
 	txq->sw_tx_ring.xdp[idx].mapping = metadata->mapping;
-	txq->sw_tx_prod++;
+	txq->sw_tx_prod = (txq->sw_tx_prod + 1) % txq->num_tx_buffers;
 
 	/* Mark the fastpath for future XDP doorbell */
 	fp->xdp_xmit = 1;
@@ -393,14 +396,14 @@ static void qede_xdp_tx_int(struct qede_dev *edev, struct qede_tx_queue *txq)
 
 	while (hw_bd_cons != qed_chain_get_cons_idx(&txq->tx_pbl)) {
 		qed_chain_consume(&txq->tx_pbl);
-		idx = txq->sw_tx_cons & NUM_TX_BDS_MAX;
+		idx = txq->sw_tx_cons;
 
 		dma_unmap_page(&edev->pdev->dev,
 			       txq->sw_tx_ring.xdp[idx].mapping,
 			       PAGE_SIZE, DMA_BIDIRECTIONAL);
 		__free_page(txq->sw_tx_ring.xdp[idx].page);
 
-		txq->sw_tx_cons++;
+		txq->sw_tx_cons = (txq->sw_tx_cons + 1) % txq->num_tx_buffers;
 		txq->xmit_pkts++;
 	}
 }
@@ -430,7 +433,7 @@ static int qede_tx_int(struct qede_dev *edev, struct qede_tx_queue *txq)
 
 		bytes_compl += len;
 		pkts_compl++;
-		txq->sw_tx_cons++;
+		txq->sw_tx_cons = (txq->sw_tx_cons + 1) % txq->num_tx_buffers;
 		txq->xmit_pkts++;
 	}
 
@@ -1076,8 +1079,7 @@ static struct sk_buff *qede_rx_allocate_skb(struct qede_dev *edev,
 	 * re-use the already allcoated & mapped memory.
 	 */
 	if (len + pad <= edev->rx_copybreak) {
-		memcpy(skb_put(skb, len),
-		       page_address(page) + offset, len);
+		skb_put_data(skb, page_address(page) + offset, len);
 		qede_reuse_page(rxq, bd);
 		goto out;
 	}
@@ -1424,7 +1426,7 @@ netdev_tx_t qede_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	struct eth_tx_2nd_bd *second_bd = NULL;
 	struct eth_tx_3rd_bd *third_bd = NULL;
 	struct eth_tx_bd *tx_data_bd = NULL;
-	u16 txq_index;
+	u16 txq_index, val = 0;
 	u8 nbd = 0;
 	dma_addr_t mapping;
 	int rc, frag_idx = 0, ipv6_ext = 0;
@@ -1455,7 +1457,7 @@ netdev_tx_t qede_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 #endif
 
 	/* Fill the entry in the SW ring and the BDs in the FW ring */
-	idx = txq->sw_tx_prod & NUM_TX_BDS_MAX;
+	idx = txq->sw_tx_prod;
 	txq->sw_tx_ring.skbs[idx].skb = skb;
 	first_bd = (struct eth_tx_1st_bd *)
 		   qed_chain_produce(&txq->tx_pbl);
@@ -1513,8 +1515,8 @@ netdev_tx_t qede_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 		if (xmit_type & XMIT_ENC) {
 			first_bd->data.bd_flags.bitfields |=
 				1 << ETH_TX_1ST_BD_FLAGS_IP_CSUM_SHIFT;
-			first_bd->data.bitfields |=
-			    1 << ETH_TX_DATA_1ST_BD_TUNN_FLAG_SHIFT;
+
+			val |= (1 << ETH_TX_DATA_1ST_BD_TUNN_FLAG_SHIFT);
 		}
 
 		/* Legacy FW had flipped behavior in regard to this bit -
@@ -1522,8 +1524,7 @@ netdev_tx_t qede_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 		 * packets when it didn't need to.
 		 */
 		if (unlikely(txq->is_legacy))
-			first_bd->data.bitfields ^=
-			    1 << ETH_TX_DATA_1ST_BD_TUNN_FLAG_SHIFT;
+			val ^= (1 << ETH_TX_DATA_1ST_BD_TUNN_FLAG_SHIFT);
 
 		/* If the packet is IPv6 with extension header, indicate that
 		 * to FW and pass few params, since the device cracker doesn't
@@ -1587,11 +1588,12 @@ netdev_tx_t qede_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 			data_split = true;
 		}
 	} else {
-		first_bd->data.bitfields |=
-		    (skb->len & ETH_TX_DATA_1ST_BD_PKT_LEN_MASK) <<
-		    ETH_TX_DATA_1ST_BD_PKT_LEN_SHIFT;
+		val |= ((skb->len & ETH_TX_DATA_1ST_BD_PKT_LEN_MASK) <<
+			 ETH_TX_DATA_1ST_BD_PKT_LEN_SHIFT);
 	}
 
+	first_bd->data.bitfields = cpu_to_le16(val);
+
 	/* Handle fragmented skb */
 	/* special handle for frags inside 2nd and 3rd bds.. */
 	while (tx_data_bd && frag_idx < skb_shinfo(skb)->nr_frags) {
@@ -1639,7 +1641,7 @@ netdev_tx_t qede_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	/* Advance packet producer only before sending the packet since mapping
 	 * of pages may fail.
 	 */
-	txq->sw_tx_prod++;
+	txq->sw_tx_prod = (txq->sw_tx_prod + 1) % txq->num_tx_buffers;
 
 	/* 'next page' entries are counted in the producer value */
 	txq->tx_db.data.bd_prod =
diff --git a/drivers/net/ethernet/qlogic/qede/qede_main.c b/drivers/net/ethernet/qlogic/qede/qede_main.c
index 38b77bbfe4ee..06ca13dd9ddb 100644
--- a/drivers/net/ethernet/qlogic/qede/qede_main.c
+++ b/drivers/net/ethernet/qlogic/qede/qede_main.c
@@ -60,7 +60,6 @@
 #include <net/ip6_checksum.h>
 #include <linux/bitops.h>
 #include <linux/vmalloc.h>
-#include <linux/qed/qede_roce.h>
 #include "qede.h"
 #include "qede_ptp.h"
 
@@ -259,11 +258,11 @@ static int qede_netdev_event(struct notifier_block *this, unsigned long event,
 		/* Notify qed of the name change */
 		if (!edev->ops || !edev->ops->common)
 			goto done;
-		edev->ops->common->set_id(edev->cdev, edev->ndev->name, "qede");
+		edev->ops->common->set_name(edev->cdev, edev->ndev->name);
 		break;
 	case NETDEV_CHANGEADDR:
 		edev = netdev_priv(ndev);
-		qede_roce_event_changeaddr(edev);
+		qede_rdma_event_changeaddr(edev);
 		break;
 	}
 
@@ -580,6 +579,24 @@ static const struct net_device_ops qede_netdev_vf_ops = {
 	.ndo_features_check = qede_features_check,
 };
 
+static const struct net_device_ops qede_netdev_vf_xdp_ops = {
+	.ndo_open = qede_open,
+	.ndo_stop = qede_close,
+	.ndo_start_xmit = qede_start_xmit,
+	.ndo_set_rx_mode = qede_set_rx_mode,
+	.ndo_set_mac_address = qede_set_mac_addr,
+	.ndo_validate_addr = eth_validate_addr,
+	.ndo_change_mtu = qede_change_mtu,
+	.ndo_vlan_rx_add_vid = qede_vlan_rx_add_vid,
+	.ndo_vlan_rx_kill_vid = qede_vlan_rx_kill_vid,
+	.ndo_set_features = qede_set_features,
+	.ndo_get_stats64 = qede_get_stats64,
+	.ndo_udp_tunnel_add = qede_udp_tunnel_add,
+	.ndo_udp_tunnel_del = qede_udp_tunnel_del,
+	.ndo_features_check = qede_features_check,
+	.ndo_xdp = qede_xdp,
+};
+
 /* -------------------------------------------------------------------------
  * START OF PROBE / REMOVE
  * -------------------------------------------------------------------------
@@ -618,6 +635,12 @@ static struct qede_dev *qede_alloc_etherdev(struct qed_dev *cdev,
 	memset(&edev->stats, 0, sizeof(edev->stats));
 	memcpy(&edev->dev_info, info, sizeof(*info));
 
+	/* As ethtool doesn't have the ability to show WoL behavior as
+	 * 'default', if device supports it declare it's enabled.
+	 */
+	if (edev->dev_info.common.wol_support)
+		edev->wol_enabled = true;
+
 	INIT_LIST_HEAD(&edev->vlan_list);
 
 	return edev;
@@ -639,10 +662,14 @@ static void qede_init_ndev(struct qede_dev *edev)
 
 	ndev->watchdog_timeo = TX_TIMEOUT;
 
-	if (IS_VF(edev))
-		ndev->netdev_ops = &qede_netdev_vf_ops;
-	else
+	if (IS_VF(edev)) {
+		if (edev->dev_info.xdp_supported)
+			ndev->netdev_ops = &qede_netdev_vf_xdp_ops;
+		else
+			ndev->netdev_ops = &qede_netdev_vf_ops;
+	} else {
 		ndev->netdev_ops = &qede_netdev_ops;
+	}
 
 	qede_set_ethtool_ops(ndev);
 
@@ -840,12 +867,55 @@ static void qede_update_pf_params(struct qed_dev *cdev)
 	/* 64 rx + 64 tx + 64 XDP */
 	memset(&pf_params, 0, sizeof(struct qed_pf_params));
 	pf_params.eth_pf_params.num_cons = (MAX_SB_PER_PF_MIMD - 1) * 3;
+
+	/* Same for VFs - make sure they'll have sufficient connections
+	 * to support XDP Tx queues.
+	 */
+	pf_params.eth_pf_params.num_vf_cons = 48;
+
 #ifdef CONFIG_RFS_ACCEL
 	pf_params.eth_pf_params.num_arfs_filters = QEDE_RFS_MAX_FLTR;
 #endif
 	qed_ops->common->update_pf_params(cdev, &pf_params);
 }
 
+#define QEDE_FW_VER_STR_SIZE	80
+
+static void qede_log_probe(struct qede_dev *edev)
+{
+	struct qed_dev_info *p_dev_info = &edev->dev_info.common;
+	u8 buf[QEDE_FW_VER_STR_SIZE];
+	size_t left_size;
+
+	snprintf(buf, QEDE_FW_VER_STR_SIZE,
+		 "Storm FW %d.%d.%d.%d, Management FW %d.%d.%d.%d",
+		 p_dev_info->fw_major, p_dev_info->fw_minor, p_dev_info->fw_rev,
+		 p_dev_info->fw_eng,
+		 (p_dev_info->mfw_rev & QED_MFW_VERSION_3_MASK) >>
+		 QED_MFW_VERSION_3_OFFSET,
+		 (p_dev_info->mfw_rev & QED_MFW_VERSION_2_MASK) >>
+		 QED_MFW_VERSION_2_OFFSET,
+		 (p_dev_info->mfw_rev & QED_MFW_VERSION_1_MASK) >>
+		 QED_MFW_VERSION_1_OFFSET,
+		 (p_dev_info->mfw_rev & QED_MFW_VERSION_0_MASK) >>
+		 QED_MFW_VERSION_0_OFFSET);
+
+	left_size = QEDE_FW_VER_STR_SIZE - strlen(buf);
+	if (p_dev_info->mbi_version && left_size)
+		snprintf(buf + strlen(buf), left_size,
+			 " [MBI %d.%d.%d]",
+			 (p_dev_info->mbi_version & QED_MBI_VERSION_2_MASK) >>
+			 QED_MBI_VERSION_2_OFFSET,
+			 (p_dev_info->mbi_version & QED_MBI_VERSION_1_MASK) >>
+			 QED_MBI_VERSION_1_OFFSET,
+			 (p_dev_info->mbi_version & QED_MBI_VERSION_0_MASK) >>
+			 QED_MBI_VERSION_0_OFFSET);
+
+	pr_info("qede %02x:%02x.%02x: %s [%s]\n", edev->pdev->bus->number,
+		PCI_SLOT(edev->pdev->devfn), PCI_FUNC(edev->pdev->devfn),
+		buf, edev->ndev->name);
+}
+
 enum qede_probe_mode {
 	QEDE_PROBE_NORMAL,
 };
@@ -907,7 +977,7 @@ static int __qede_probe(struct pci_dev *pdev, u32 dp_module, u8 dp_level,
 
 	qede_init_ndev(edev);
 
-	rc = qede_roce_dev_add(edev);
+	rc = qede_rdma_dev_add(edev);
 	if (rc)
 		goto err3;
 
@@ -924,7 +994,7 @@ static int __qede_probe(struct pci_dev *pdev, u32 dp_module, u8 dp_level,
 		goto err4;
 	}
 
-	edev->ops->common->set_id(cdev, edev->ndev->name, DRV_MODULE_VERSION);
+	edev->ops->common->set_name(cdev, edev->ndev->name);
 
 	/* PTP not supported on VFs */
 	if (!is_vf)
@@ -939,12 +1009,11 @@ static int __qede_probe(struct pci_dev *pdev, u32 dp_module, u8 dp_level,
 
 	edev->rx_copybreak = QEDE_RX_HDR_SIZE;
 
-	DP_INFO(edev, "Ending successfully qede probe\n");
-
+	qede_log_probe(edev);
 	return 0;
 
 err4:
-	qede_roce_dev_remove(edev);
+	qede_rdma_dev_remove(edev);
 err3:
 	free_netdev(edev->ndev);
 err2:
@@ -995,7 +1064,7 @@ static void __qede_remove(struct pci_dev *pdev, enum qede_remove_mode mode)
 
 	qede_ptp_disable(edev);
 
-	qede_roce_dev_remove(edev);
+	qede_rdma_dev_remove(edev);
 
 	edev->ops->common->set_power_state(cdev, PCI_D0);
 
@@ -1066,12 +1135,15 @@ static int qede_set_num_queues(struct qede_dev *edev)
 	return rc;
 }
 
-static void qede_free_mem_sb(struct qede_dev *edev,
-			     struct qed_sb_info *sb_info)
+static void qede_free_mem_sb(struct qede_dev *edev, struct qed_sb_info *sb_info,
+			     u16 sb_id)
 {
-	if (sb_info->sb_virt)
+	if (sb_info->sb_virt) {
+		edev->ops->common->sb_release(edev->cdev, sb_info, sb_id);
 		dma_free_coherent(&edev->pdev->dev, sizeof(*sb_info->sb_virt),
 				  (void *)sb_info->sb_virt, sb_info->sb_phys);
+		memset(sb_info, 0, sizeof(*sb_info));
+	}
 }
 
 /* This function allocates fast-path status block memory */
@@ -1244,8 +1316,7 @@ static int qede_alloc_mem_rxq(struct qede_dev *edev, struct qede_rx_queue *rxq)
 					    QED_CHAIN_CNT_TYPE_U16,
 					    RX_RING_SIZE,
 					    sizeof(struct eth_rx_bd),
-					    &rxq->rx_bd_ring);
-
+					    &rxq->rx_bd_ring, NULL);
 	if (rc)
 		goto err;
 
@@ -1256,7 +1327,7 @@ static int qede_alloc_mem_rxq(struct qede_dev *edev, struct qede_rx_queue *rxq)
 					    QED_CHAIN_CNT_TYPE_U16,
 					    RX_RING_SIZE,
 					    sizeof(union eth_rx_cqe),
-					    &rxq->rx_comp_ring);
+					    &rxq->rx_comp_ring, NULL);
 	if (rc)
 		goto err;
 
@@ -1298,12 +1369,12 @@ static int qede_alloc_mem_txq(struct qede_dev *edev, struct qede_tx_queue *txq)
 
 	/* Allocate the parallel driver ring for Tx buffers */
 	if (txq->is_xdp) {
-		size = sizeof(*txq->sw_tx_ring.xdp) * TX_RING_SIZE;
+		size = sizeof(*txq->sw_tx_ring.xdp) * txq->num_tx_buffers;
 		txq->sw_tx_ring.xdp = kzalloc(size, GFP_KERNEL);
 		if (!txq->sw_tx_ring.xdp)
 			goto err;
 	} else {
-		size = sizeof(*txq->sw_tx_ring.skbs) * TX_RING_SIZE;
+		size = sizeof(*txq->sw_tx_ring.skbs) * txq->num_tx_buffers;
 		txq->sw_tx_ring.skbs = kzalloc(size, GFP_KERNEL);
 		if (!txq->sw_tx_ring.skbs)
 			goto err;
@@ -1313,8 +1384,9 @@ static int qede_alloc_mem_txq(struct qede_dev *edev, struct qede_tx_queue *txq)
 					    QED_CHAIN_USE_TO_CONSUME_PRODUCE,
 					    QED_CHAIN_MODE_PBL,
 					    QED_CHAIN_CNT_TYPE_U16,
-					    TX_RING_SIZE,
-					    sizeof(*p_virt), &txq->tx_pbl);
+					    txq->num_tx_buffers,
+					    sizeof(*p_virt),
+					    &txq->tx_pbl, NULL);
 	if (rc)
 		goto err;
 
@@ -1328,7 +1400,7 @@ err:
 /* This function frees all memory of a single fp */
 static void qede_free_mem_fp(struct qede_dev *edev, struct qede_fastpath *fp)
 {
-	qede_free_mem_sb(edev, fp->sb_info);
+	qede_free_mem_sb(edev, fp->sb_info, fp->id);
 
 	if (fp->type & QEDE_FASTPATH_RX)
 		qede_free_mem_rxq(edev, fp->rxq);
@@ -1725,7 +1797,7 @@ static int qede_start_txq(struct qede_dev *edev,
 	else
 		params.queue_id = txq->index;
 
-	params.sb = fp->sb_info->igu_sb_id;
+	params.p_sb = fp->sb_info;
 	params.sb_idx = sb_idx;
 
 	rc = edev->ops->q_tx_start(edev->cdev, rss_id, &params, phys_table,
@@ -1804,7 +1876,7 @@ static int qede_start_queues(struct qede_dev *edev, bool clear_stats)
 			memset(&q_params, 0, sizeof(q_params));
 			q_params.queue_id = rxq->rxq_id;
 			q_params.vport_id = 0;
-			q_params.sb = fp->sb_info->igu_sb_id;
+			q_params.p_sb = fp->sb_info;
 			q_params.sb_idx = RX_PI;
 
 			p_phys_table =
@@ -1890,9 +1962,10 @@ static void qede_unload(struct qede_dev *edev, enum qede_unload_mode mode,
 	if (!is_locked)
 		__qede_lock(edev);
 
-	qede_roce_dev_event_close(edev);
 	edev->state = QEDE_STATE_CLOSED;
 
+	qede_rdma_dev_event_close(edev);
+
 	/* Close OS Tx */
 	netif_tx_disable(edev->ndev);
 	netif_carrier_off(edev->ndev);
@@ -1988,9 +2061,6 @@ static int qede_load(struct qede_dev *edev, enum qede_load_mode mode,
 		goto err4;
 	DP_INFO(edev, "Start VPORT, RXQ and TXQ succeeded\n");
 
-	/* Add primary mac and set Rx filters */
-	ether_addr_copy(edev->primary_mac, edev->ndev->dev_addr);
-
 	/* Program un-configured VLANs */
 	qede_configure_vlan_filters(edev);
 
@@ -1999,7 +2069,7 @@ static int qede_load(struct qede_dev *edev, enum qede_load_mode mode,
 	link_params.link_up = true;
 	edev->ops->common->set_link(edev->cdev, &link_params);
 
-	qede_roce_dev_event_open(edev);
+	qede_rdma_dev_event_open(edev);
 
 	edev->state = QEDE_STATE_OPEN;
 
diff --git a/drivers/net/ethernet/qlogic/qede/qede_ptp.c b/drivers/net/ethernet/qlogic/qede/qede_ptp.c
index 24f06e2ef43e..9b2280badaf7 100644
--- a/drivers/net/ethernet/qlogic/qede/qede_ptp.c
+++ b/drivers/net/ethernet/qlogic/qede/qede_ptp.c
@@ -244,6 +244,7 @@ static int qede_ptp_cfg_filters(struct qede_dev *edev)
 		break;
 	case HWTSTAMP_FILTER_ALL:
 	case HWTSTAMP_FILTER_SOME:
+	case HWTSTAMP_FILTER_NTP_ALL:
 		ptp->rx_filter = HWTSTAMP_FILTER_NONE;
 		rx_filter = QED_PTP_FILTER_ALL;
 		break;
diff --git a/drivers/net/ethernet/qlogic/qede/qede_roce.c b/drivers/net/ethernet/qlogic/qede/qede_rdma.c
index f00657ce7c8f..50b142fad6b8 100644
--- a/drivers/net/ethernet/qlogic/qede/qede_roce.c
+++ b/drivers/net/ethernet/qlogic/qede/qede_rdma.c
@@ -33,19 +33,19 @@
 #include <linux/netdevice.h>
 #include <linux/list.h>
 #include <linux/mutex.h>
-#include <linux/qed/qede_roce.h>
+#include <linux/qed/qede_rdma.h>
 #include "qede.h"
 
 static struct qedr_driver *qedr_drv;
 static LIST_HEAD(qedr_dev_list);
 static DEFINE_MUTEX(qedr_dev_list_lock);
 
-bool qede_roce_supported(struct qede_dev *dev)
+bool qede_rdma_supported(struct qede_dev *dev)
 {
 	return dev->dev_info.common.rdma_supported;
 }
 
-static void _qede_roce_dev_add(struct qede_dev *edev)
+static void _qede_rdma_dev_add(struct qede_dev *edev)
 {
 	if (!qedr_drv)
 		return;
@@ -54,11 +54,11 @@ static void _qede_roce_dev_add(struct qede_dev *edev)
 						 edev->ndev);
 }
 
-static int qede_roce_create_wq(struct qede_dev *edev)
+static int qede_rdma_create_wq(struct qede_dev *edev)
 {
-	INIT_LIST_HEAD(&edev->rdma_info.roce_event_list);
-	edev->rdma_info.roce_wq = create_singlethread_workqueue("roce_wq");
-	if (!edev->rdma_info.roce_wq) {
+	INIT_LIST_HEAD(&edev->rdma_info.rdma_event_list);
+	edev->rdma_info.rdma_wq = create_singlethread_workqueue("rdma_wq");
+	if (!edev->rdma_info.rdma_wq) {
 		DP_NOTICE(edev, "qedr: Could not create workqueue\n");
 		return -ENOMEM;
 	}
@@ -66,14 +66,14 @@ static int qede_roce_create_wq(struct qede_dev *edev)
 	return 0;
 }
 
-static void qede_roce_cleanup_event(struct qede_dev *edev)
+static void qede_rdma_cleanup_event(struct qede_dev *edev)
 {
-	struct list_head *head = &edev->rdma_info.roce_event_list;
-	struct qede_roce_event_work *event_node;
+	struct list_head *head = &edev->rdma_info.rdma_event_list;
+	struct qede_rdma_event_work *event_node;
 
-	flush_workqueue(edev->rdma_info.roce_wq);
+	flush_workqueue(edev->rdma_info.rdma_wq);
 	while (!list_empty(head)) {
-		event_node = list_entry(head->next, struct qede_roce_event_work,
+		event_node = list_entry(head->next, struct qede_rdma_event_work,
 					list);
 		cancel_work_sync(&event_node->work);
 		list_del(&event_node->list);
@@ -81,85 +81,85 @@ static void qede_roce_cleanup_event(struct qede_dev *edev)
 	}
 }
 
-static void qede_roce_destroy_wq(struct qede_dev *edev)
+static void qede_rdma_destroy_wq(struct qede_dev *edev)
 {
-	qede_roce_cleanup_event(edev);
-	destroy_workqueue(edev->rdma_info.roce_wq);
+	qede_rdma_cleanup_event(edev);
+	destroy_workqueue(edev->rdma_info.rdma_wq);
 }
 
-int qede_roce_dev_add(struct qede_dev *edev)
+int qede_rdma_dev_add(struct qede_dev *edev)
 {
 	int rc = 0;
 
-	if (qede_roce_supported(edev)) {
-		rc = qede_roce_create_wq(edev);
+	if (qede_rdma_supported(edev)) {
+		rc = qede_rdma_create_wq(edev);
 		if (rc)
 			return rc;
 
 		INIT_LIST_HEAD(&edev->rdma_info.entry);
 		mutex_lock(&qedr_dev_list_lock);
 		list_add_tail(&edev->rdma_info.entry, &qedr_dev_list);
-		_qede_roce_dev_add(edev);
+		_qede_rdma_dev_add(edev);
 		mutex_unlock(&qedr_dev_list_lock);
 	}
 
 	return rc;
 }
 
-static void _qede_roce_dev_remove(struct qede_dev *edev)
+static void _qede_rdma_dev_remove(struct qede_dev *edev)
 {
 	if (qedr_drv && qedr_drv->remove && edev->rdma_info.qedr_dev)
 		qedr_drv->remove(edev->rdma_info.qedr_dev);
 	edev->rdma_info.qedr_dev = NULL;
 }
 
-void qede_roce_dev_remove(struct qede_dev *edev)
+void qede_rdma_dev_remove(struct qede_dev *edev)
 {
-	if (!qede_roce_supported(edev))
+	if (!qede_rdma_supported(edev))
 		return;
 
-	qede_roce_destroy_wq(edev);
+	qede_rdma_destroy_wq(edev);
 	mutex_lock(&qedr_dev_list_lock);
-	_qede_roce_dev_remove(edev);
+	_qede_rdma_dev_remove(edev);
 	list_del(&edev->rdma_info.entry);
 	mutex_unlock(&qedr_dev_list_lock);
 }
 
-static void _qede_roce_dev_open(struct qede_dev *edev)
+static void _qede_rdma_dev_open(struct qede_dev *edev)
 {
 	if (qedr_drv && edev->rdma_info.qedr_dev && qedr_drv->notify)
 		qedr_drv->notify(edev->rdma_info.qedr_dev, QEDE_UP);
 }
 
-static void qede_roce_dev_open(struct qede_dev *edev)
+static void qede_rdma_dev_open(struct qede_dev *edev)
 {
-	if (!qede_roce_supported(edev))
+	if (!qede_rdma_supported(edev))
 		return;
 
 	mutex_lock(&qedr_dev_list_lock);
-	_qede_roce_dev_open(edev);
+	_qede_rdma_dev_open(edev);
 	mutex_unlock(&qedr_dev_list_lock);
 }
 
-static void _qede_roce_dev_close(struct qede_dev *edev)
+static void _qede_rdma_dev_close(struct qede_dev *edev)
 {
 	if (qedr_drv && edev->rdma_info.qedr_dev && qedr_drv->notify)
 		qedr_drv->notify(edev->rdma_info.qedr_dev, QEDE_DOWN);
 }
 
-static void qede_roce_dev_close(struct qede_dev *edev)
+static void qede_rdma_dev_close(struct qede_dev *edev)
 {
-	if (!qede_roce_supported(edev))
+	if (!qede_rdma_supported(edev))
 		return;
 
 	mutex_lock(&qedr_dev_list_lock);
-	_qede_roce_dev_close(edev);
+	_qede_rdma_dev_close(edev);
 	mutex_unlock(&qedr_dev_list_lock);
 }
 
-static void qede_roce_dev_shutdown(struct qede_dev *edev)
+static void qede_rdma_dev_shutdown(struct qede_dev *edev)
 {
-	if (!qede_roce_supported(edev))
+	if (!qede_rdma_supported(edev))
 		return;
 
 	mutex_lock(&qedr_dev_list_lock);
@@ -168,7 +168,7 @@ static void qede_roce_dev_shutdown(struct qede_dev *edev)
 	mutex_unlock(&qedr_dev_list_lock);
 }
 
-int qede_roce_register_driver(struct qedr_driver *drv)
+int qede_rdma_register_driver(struct qedr_driver *drv)
 {
 	struct qede_dev *edev;
 	u8 qedr_counter = 0;
@@ -184,52 +184,52 @@ int qede_roce_register_driver(struct qedr_driver *drv)
 		struct net_device *ndev;
 
 		qedr_counter++;
-		_qede_roce_dev_add(edev);
+		_qede_rdma_dev_add(edev);
 		ndev = edev->ndev;
 		if (netif_running(ndev) && netif_oper_up(ndev))
-			_qede_roce_dev_open(edev);
+			_qede_rdma_dev_open(edev);
 	}
 	mutex_unlock(&qedr_dev_list_lock);
 
-	pr_notice("qedr: discovered and registered %d RoCE funcs\n",
+	pr_notice("qedr: discovered and registered %d RDMA funcs\n",
 		  qedr_counter);
 
 	return 0;
 }
-EXPORT_SYMBOL(qede_roce_register_driver);
+EXPORT_SYMBOL(qede_rdma_register_driver);
 
-void qede_roce_unregister_driver(struct qedr_driver *drv)
+void qede_rdma_unregister_driver(struct qedr_driver *drv)
 {
 	struct qede_dev *edev;
 
 	mutex_lock(&qedr_dev_list_lock);
 	list_for_each_entry(edev, &qedr_dev_list, rdma_info.entry) {
 		if (edev->rdma_info.qedr_dev)
-			_qede_roce_dev_remove(edev);
+			_qede_rdma_dev_remove(edev);
 	}
 	qedr_drv = NULL;
 	mutex_unlock(&qedr_dev_list_lock);
 }
-EXPORT_SYMBOL(qede_roce_unregister_driver);
+EXPORT_SYMBOL(qede_rdma_unregister_driver);
 
-static void qede_roce_changeaddr(struct qede_dev *edev)
+static void qede_rdma_changeaddr(struct qede_dev *edev)
 {
-	if (!qede_roce_supported(edev))
+	if (!qede_rdma_supported(edev))
 		return;
 
 	if (qedr_drv && edev->rdma_info.qedr_dev && qedr_drv->notify)
 		qedr_drv->notify(edev->rdma_info.qedr_dev, QEDE_CHANGE_ADDR);
 }
 
-struct qede_roce_event_work *qede_roce_get_free_event_node(struct qede_dev
-							   *edev)
+static struct qede_rdma_event_work *
+qede_rdma_get_free_event_node(struct qede_dev *edev)
 {
-	struct qede_roce_event_work *event_node = NULL;
+	struct qede_rdma_event_work *event_node = NULL;
 	struct list_head *list_node = NULL;
 	bool found = false;
 
-	list_for_each(list_node, &edev->rdma_info.roce_event_list) {
-		event_node = list_entry(list_node, struct qede_roce_event_work,
+	list_for_each(list_node, &edev->rdma_info.rdma_event_list) {
+		event_node = list_entry(list_node, struct qede_rdma_event_work,
 					list);
 		if (!work_pending(&event_node->work)) {
 			found = true;
@@ -241,74 +241,74 @@ struct qede_roce_event_work *qede_roce_get_free_event_node(struct qede_dev
 		event_node = kzalloc(sizeof(*event_node), GFP_KERNEL);
 		if (!event_node) {
 			DP_NOTICE(edev,
-				  "qedr: Could not allocate memory for roce work\n");
+				  "qedr: Could not allocate memory for rdma work\n");
 			return NULL;
 		}
 		list_add_tail(&event_node->list,
-			      &edev->rdma_info.roce_event_list);
+			      &edev->rdma_info.rdma_event_list);
 	}
 
 	return event_node;
 }
 
-static void qede_roce_handle_event(struct work_struct *work)
+static void qede_rdma_handle_event(struct work_struct *work)
 {
-	struct qede_roce_event_work *event_node;
-	enum qede_roce_event event;
+	struct qede_rdma_event_work *event_node;
+	enum qede_rdma_event event;
 	struct qede_dev *edev;
 
-	event_node = container_of(work, struct qede_roce_event_work, work);
+	event_node = container_of(work, struct qede_rdma_event_work, work);
 	event = event_node->event;
 	edev = event_node->ptr;
 
 	switch (event) {
 	case QEDE_UP:
-		qede_roce_dev_open(edev);
+		qede_rdma_dev_open(edev);
 		break;
 	case QEDE_DOWN:
-		qede_roce_dev_close(edev);
+		qede_rdma_dev_close(edev);
 		break;
 	case QEDE_CLOSE:
-		qede_roce_dev_shutdown(edev);
+		qede_rdma_dev_shutdown(edev);
 		break;
 	case QEDE_CHANGE_ADDR:
-		qede_roce_changeaddr(edev);
+		qede_rdma_changeaddr(edev);
 		break;
 	default:
-		DP_NOTICE(edev, "Invalid roce event %d", event);
+		DP_NOTICE(edev, "Invalid rdma event %d", event);
 	}
 }
 
-static void qede_roce_add_event(struct qede_dev *edev,
-				enum qede_roce_event event)
+static void qede_rdma_add_event(struct qede_dev *edev,
+				enum qede_rdma_event event)
 {
-	struct qede_roce_event_work *event_node;
+	struct qede_rdma_event_work *event_node;
 
 	if (!edev->rdma_info.qedr_dev)
 		return;
 
-	event_node = qede_roce_get_free_event_node(edev);
+	event_node = qede_rdma_get_free_event_node(edev);
 	if (!event_node)
 		return;
 
 	event_node->event = event;
 	event_node->ptr = edev;
 
-	INIT_WORK(&event_node->work, qede_roce_handle_event);
-	queue_work(edev->rdma_info.roce_wq, &event_node->work);
+	INIT_WORK(&event_node->work, qede_rdma_handle_event);
+	queue_work(edev->rdma_info.rdma_wq, &event_node->work);
 }
 
-void qede_roce_dev_event_open(struct qede_dev *edev)
+void qede_rdma_dev_event_open(struct qede_dev *edev)
 {
-	qede_roce_add_event(edev, QEDE_UP);
+	qede_rdma_add_event(edev, QEDE_UP);
 }
 
-void qede_roce_dev_event_close(struct qede_dev *edev)
+void qede_rdma_dev_event_close(struct qede_dev *edev)
 {
-	qede_roce_add_event(edev, QEDE_DOWN);
+	qede_rdma_add_event(edev, QEDE_DOWN);
 }
 
-void qede_roce_event_changeaddr(struct qede_dev *edev)
+void qede_rdma_event_changeaddr(struct qede_dev *edev)
 {
-	qede_roce_add_event(edev, QEDE_CHANGE_ADDR);
+	qede_rdma_add_event(edev, QEDE_CHANGE_ADDR);
 }
diff --git a/drivers/net/ethernet/qlogic/qlge/qlge_main.c b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
index 1188d420fe53..9feec7009443 100644
--- a/drivers/net/ethernet/qlogic/qlge/qlge_main.c
+++ b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
@@ -1577,7 +1577,7 @@ static void ql_process_mac_rx_page(struct ql_adapter *qdev,
 		rx_ring->rx_dropped++;
 		goto err_out;
 	}
-	memcpy(skb_put(skb, hlen), addr, hlen);
+	skb_put_data(skb, addr, hlen);
 	netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
 		     "%d bytes of headers and data in large. Chain page to new skb and pull tail.\n",
 		     length);
@@ -1654,7 +1654,7 @@ static void ql_process_mac_rx_skb(struct ql_adapter *qdev,
 				    dma_unmap_len(sbq_desc, maplen),
 				    PCI_DMA_FROMDEVICE);
 
-	memcpy(skb_put(new_skb, length), skb->data, length);
+	skb_put_data(new_skb, skb->data, length);
 
 	pci_dma_sync_single_for_device(qdev->pdev,
 				       dma_unmap_addr(sbq_desc, mapaddr),
@@ -1817,8 +1817,7 @@ static struct sk_buff *ql_build_rx_skb(struct ql_adapter *qdev,
 						    dma_unmap_len
 						    (sbq_desc, maplen),
 						    PCI_DMA_FROMDEVICE);
-			memcpy(skb_put(skb, length),
-			       sbq_desc->p.skb->data, length);
+			skb_put_data(skb, sbq_desc->p.skb->data, length);
 			pci_dma_sync_single_for_device(qdev->pdev,
 						       dma_unmap_addr
 						       (sbq_desc,
diff --git a/drivers/net/ethernet/qualcomm/Kconfig b/drivers/net/ethernet/qualcomm/Kconfig
index d7720bf92d49..877675a27b9f 100644
--- a/drivers/net/ethernet/qualcomm/Kconfig
+++ b/drivers/net/ethernet/qualcomm/Kconfig
@@ -16,7 +16,13 @@ config NET_VENDOR_QUALCOMM
 if NET_VENDOR_QUALCOMM
 
 config QCA7000
-	tristate "Qualcomm Atheros QCA7000 support"
+	tristate
+	help
+	  This enables support for the Qualcomm Atheros QCA7000.
+
+config QCA7000_SPI
+	tristate "Qualcomm Atheros QCA7000 SPI support"
+	select QCA7000
 	depends on SPI_MASTER && OF
 	---help---
 	  This SPI protocol driver supports the Qualcomm Atheros QCA7000.
@@ -24,6 +30,22 @@ config QCA7000
 	  To compile this driver as a module, choose M here. The module
 	  will be called qcaspi.
 
+config QCA7000_UART
+	tristate "Qualcomm Atheros QCA7000 UART support"
+	select QCA7000
+	depends on SERIAL_DEV_BUS && OF
+	---help---
+	  This UART protocol driver supports the Qualcomm Atheros QCA7000.
+
+	  Currently the driver assumes these device UART settings:
+	    Data bits: 8
+	    Parity: None
+	    Stop bits: 1
+	    Flow control: None
+
+	  To compile this driver as a module, choose M here. The module
+	  will be called qcauart.
+
 config QCOM_EMAC
 	tristate "Qualcomm Technologies, Inc. EMAC Gigabit Ethernet support"
 	depends on HAS_DMA && HAS_IOMEM
diff --git a/drivers/net/ethernet/qualcomm/Makefile b/drivers/net/ethernet/qualcomm/Makefile
index aacb0a585c68..92fa7c4da90a 100644
--- a/drivers/net/ethernet/qualcomm/Makefile
+++ b/drivers/net/ethernet/qualcomm/Makefile
@@ -2,7 +2,10 @@
 # Makefile for the Qualcomm network device drivers.
 #
 
-obj-$(CONFIG_QCA7000) += qcaspi.o
-qcaspi-objs := qca_spi.o qca_framing.o qca_7k.o qca_debug.o
+obj-$(CONFIG_QCA7000) += qca_7k_common.o
+obj-$(CONFIG_QCA7000_SPI) += qcaspi.o
+qcaspi-objs := qca_7k.o qca_debug.o qca_spi.o
+obj-$(CONFIG_QCA7000_UART) += qcauart.o
+qcauart-objs := qca_uart.o
 
 obj-y += emac/
diff --git a/drivers/net/ethernet/qualcomm/emac/emac-sgmii.c b/drivers/net/ethernet/qualcomm/emac/emac-sgmii.c
index 18c184ee1f3c..29ba37a08372 100644
--- a/drivers/net/ethernet/qualcomm/emac/emac-sgmii.c
+++ b/drivers/net/ethernet/qualcomm/emac/emac-sgmii.c
@@ -297,6 +297,14 @@ static const struct of_device_id emac_sgmii_dt_match[] = {
 	{}
 };
 
+/* Dummy function for systems without an internal PHY. This avoids having
+ * to check for NULL pointers before calling the functions.
+ */
+static int emac_sgmii_dummy(struct emac_adapter *adpt)
+{
+	return 0;
+}
+
 int emac_sgmii_config(struct platform_device *pdev, struct emac_adapter *adpt)
 {
 	struct platform_device *sgmii_pdev = NULL;
@@ -311,8 +319,19 @@ int emac_sgmii_config(struct platform_device *pdev, struct emac_adapter *adpt)
 					emac_sgmii_acpi_match);
 
 		if (!dev) {
-			dev_err(&pdev->dev, "cannot find internal phy node\n");
-			return -ENODEV;
+			dev_warn(&pdev->dev, "cannot find internal phy node\n");
+			/* There is typically no internal PHY on emulation
+			 * systems, so if we can't find the node, assume
+			 * we are on an emulation system and stub-out
+			 * support for the internal PHY.  These systems only
+			 * use ACPI.
+			 */
+			phy->open = emac_sgmii_dummy;
+			phy->close = emac_sgmii_dummy;
+			phy->link_up = emac_sgmii_dummy;
+			phy->link_down = emac_sgmii_dummy;
+
+			return 0;
 		}
 
 		sgmii_pdev = to_platform_device(dev);
diff --git a/drivers/net/ethernet/qualcomm/emac/emac.c b/drivers/net/ethernet/qualcomm/emac/emac.c
index 98a326faea29..746d94e28470 100644
--- a/drivers/net/ethernet/qualcomm/emac/emac.c
+++ b/drivers/net/ethernet/qualcomm/emac/emac.c
@@ -683,8 +683,6 @@ static int emac_probe(struct platform_device *pdev)
 		goto err_undo_mdiobus;
 	}
 
-	emac_mac_reset(adpt);
-
 	/* set hw features */
 	netdev->features = NETIF_F_SG | NETIF_F_HW_CSUM | NETIF_F_RXCSUM |
 			NETIF_F_TSO | NETIF_F_TSO6 | NETIF_F_HW_VLAN_CTAG_RX |
@@ -762,6 +760,19 @@ static int emac_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static void emac_shutdown(struct platform_device *pdev)
+{
+	struct net_device *netdev = dev_get_drvdata(&pdev->dev);
+	struct emac_adapter *adpt = netdev_priv(netdev);
+	struct emac_sgmii *sgmii = &adpt->phy;
+
+	/* Closing the SGMII turns off its interrupts */
+	sgmii->close(adpt);
+
+	/* Resetting the MAC turns off all DMA and its interrupts */
+	emac_mac_reset(adpt);
+}
+
 static struct platform_driver emac_platform_driver = {
 	.probe	= emac_probe,
 	.remove	= emac_remove,
@@ -770,6 +781,7 @@ static struct platform_driver emac_platform_driver = {
 		.of_match_table = emac_dt_match,
 		.acpi_match_table = ACPI_PTR(emac_acpi_match),
 	},
+	.shutdown = emac_shutdown,
 };
 
 module_platform_driver(emac_platform_driver);
diff --git a/drivers/net/ethernet/qualcomm/qca_7k.c b/drivers/net/ethernet/qualcomm/qca_7k.c
index f0066fbb44a6..ffe7a16bdfc8 100644
--- a/drivers/net/ethernet/qualcomm/qca_7k.c
+++ b/drivers/net/ethernet/qualcomm/qca_7k.c
@@ -23,11 +23,9 @@
  *   kernel-based SPI device.
  */
 
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
 #include <linux/spi/spi.h>
-#include <linux/version.h>
 
 #include "qca_7k.h"
 
@@ -123,27 +121,3 @@ qcaspi_write_register(struct qcaspi *qca, u16 reg, u16 value)
 
 	return ret;
 }
-
-int
-qcaspi_tx_cmd(struct qcaspi *qca, u16 cmd)
-{
-	__be16 tx_data;
-	struct spi_message *msg = &qca->spi_msg1;
-	struct spi_transfer *transfer = &qca->spi_xfer1;
-	int ret;
-
-	tx_data = cpu_to_be16(cmd);
-	transfer->len = sizeof(tx_data);
-	transfer->tx_buf = &tx_data;
-	transfer->rx_buf = NULL;
-
-	ret = spi_sync(qca->spi_dev, msg);
-
-	if (!ret)
-		ret = msg->status;
-
-	if (ret)
-		qcaspi_spi_error(qca);
-
-	return ret;
-}
diff --git a/drivers/net/ethernet/qualcomm/qca_7k.h b/drivers/net/ethernet/qualcomm/qca_7k.h
index 1cad851ee507..27124c2bb77a 100644
--- a/drivers/net/ethernet/qualcomm/qca_7k.h
+++ b/drivers/net/ethernet/qualcomm/qca_7k.h
@@ -54,19 +54,18 @@
 #define SPI_REG_ACTION_CTRL     0x1B00
 
 /*   SPI_CONFIG register definition;             */
-#define QCASPI_SLAVE_RESET_BIT (1 << 6)
+#define QCASPI_SLAVE_RESET_BIT  BIT(6)
 
 /*   INTR_CAUSE/ENABLE register definition.      */
-#define SPI_INT_WRBUF_BELOW_WM (1 << 10)
-#define SPI_INT_CPU_ON         (1 << 6)
-#define SPI_INT_ADDR_ERR       (1 << 3)
-#define SPI_INT_WRBUF_ERR      (1 << 2)
-#define SPI_INT_RDBUF_ERR      (1 << 1)
-#define SPI_INT_PKT_AVLBL      (1 << 0)
+#define SPI_INT_WRBUF_BELOW_WM  BIT(10)
+#define SPI_INT_CPU_ON          BIT(6)
+#define SPI_INT_ADDR_ERR        BIT(3)
+#define SPI_INT_WRBUF_ERR       BIT(2)
+#define SPI_INT_RDBUF_ERR       BIT(1)
+#define SPI_INT_PKT_AVLBL       BIT(0)
 
 void qcaspi_spi_error(struct qcaspi *qca);
 int qcaspi_read_register(struct qcaspi *qca, u16 reg, u16 *result);
 int qcaspi_write_register(struct qcaspi *qca, u16 reg, u16 value);
-int qcaspi_tx_cmd(struct qcaspi *qca, u16 cmd);
 
 #endif /* _QCA_7K_H */
diff --git a/drivers/net/ethernet/qualcomm/qca_framing.c b/drivers/net/ethernet/qualcomm/qca_7k_common.c
index faa924c85e29..6b511f05df61 100644
--- a/drivers/net/ethernet/qualcomm/qca_framing.c
+++ b/drivers/net/ethernet/qualcomm/qca_7k_common.c
@@ -21,9 +21,11 @@
  *   by an atheros frame while transmitted over a serial channel;
  */
 
+#include <linux/init.h>
 #include <linux/kernel.h>
+#include <linux/module.h>
 
-#include "qca_framing.h"
+#include "qca_7k_common.h"
 
 u16
 qcafrm_create_header(u8 *buf, u16 length)
@@ -46,6 +48,7 @@ qcafrm_create_header(u8 *buf, u16 length)
 
 	return QCAFRM_HEADER_LEN;
 }
+EXPORT_SYMBOL_GPL(qcafrm_create_header);
 
 u16
 qcafrm_create_footer(u8 *buf)
@@ -57,6 +60,7 @@ qcafrm_create_footer(u8 *buf)
 	buf[1] = 0x55;
 	return QCAFRM_FOOTER_LEN;
 }
+EXPORT_SYMBOL_GPL(qcafrm_create_footer);
 
 /*   Gather received bytes and try to extract a full ethernet frame by
  *   following a simple state machine.
@@ -83,7 +87,7 @@ qcafrm_fsm_decode(struct qcafrm_handle *handle, u8 *buf, u16 buf_len, u8 recv_by
 
 		if (recv_byte != 0x00) {
 			/* first two bytes of length must be 0 */
-			handle->state = QCAFRM_HW_LEN0;
+			handle->state = handle->init;
 		}
 		break;
 	case QCAFRM_HW_LEN2:
@@ -97,7 +101,7 @@ qcafrm_fsm_decode(struct qcafrm_handle *handle, u8 *buf, u16 buf_len, u8 recv_by
 	case QCAFRM_WAIT_AA4:
 		if (recv_byte != 0xAA) {
 			ret = QCAFRM_NOHEAD;
-			handle->state = QCAFRM_HW_LEN0;
+			handle->state = handle->init;
 		} else {
 			handle->state--;
 		}
@@ -117,9 +121,9 @@ qcafrm_fsm_decode(struct qcafrm_handle *handle, u8 *buf, u16 buf_len, u8 recv_by
 		break;
 	case QCAFRM_WAIT_RSVD_BYTE2:
 		len = handle->offset;
-		if (len > buf_len || len < QCAFRM_ETHMINLEN) {
+		if (len > buf_len || len < QCAFRM_MIN_LEN) {
 			ret = QCAFRM_INVLEN;
-			handle->state = QCAFRM_HW_LEN0;
+			handle->state = handle->init;
 		} else {
 			handle->state = (enum qcafrm_state)(len + 1);
 			/* Remaining number of bytes. */
@@ -135,7 +139,7 @@ qcafrm_fsm_decode(struct qcafrm_handle *handle, u8 *buf, u16 buf_len, u8 recv_by
 	case QCAFRM_WAIT_551:
 		if (recv_byte != 0x55) {
 			ret = QCAFRM_NOTAIL;
-			handle->state = QCAFRM_HW_LEN0;
+			handle->state = handle->init;
 		} else {
 			handle->state = QCAFRM_WAIT_552;
 		}
@@ -143,14 +147,20 @@ qcafrm_fsm_decode(struct qcafrm_handle *handle, u8 *buf, u16 buf_len, u8 recv_by
 	case QCAFRM_WAIT_552:
 		if (recv_byte != 0x55) {
 			ret = QCAFRM_NOTAIL;
-			handle->state = QCAFRM_HW_LEN0;
+			handle->state = handle->init;
 		} else {
 			ret = handle->offset;
 			/* Frame is fully received. */
-			handle->state = QCAFRM_HW_LEN0;
+			handle->state = handle->init;
 		}
 		break;
 	}
 
 	return ret;
 }
+EXPORT_SYMBOL_GPL(qcafrm_fsm_decode);
+
+MODULE_DESCRIPTION("Qualcomm Atheros QCA7000 common");
+MODULE_AUTHOR("Qualcomm Atheros Communications");
+MODULE_AUTHOR("Stefan Wahren <stefan.wahren@i2se.com>");
+MODULE_LICENSE("Dual BSD/GPL");
diff --git a/drivers/net/ethernet/qualcomm/qca_framing.h b/drivers/net/ethernet/qualcomm/qca_7k_common.h
index d5e795dcdf47..928554f11e35 100644
--- a/drivers/net/ethernet/qualcomm/qca_framing.h
+++ b/drivers/net/ethernet/qualcomm/qca_7k_common.h
@@ -44,12 +44,12 @@
 #define QCAFRM_INVFRAME (QCAFRM_ERR_BASE - 4)
 
 /* Min/Max Ethernet MTU: 46/1500 */
-#define QCAFRM_ETHMINMTU (ETH_ZLEN - ETH_HLEN)
-#define QCAFRM_ETHMAXMTU ETH_DATA_LEN
+#define QCAFRM_MIN_MTU (ETH_ZLEN - ETH_HLEN)
+#define QCAFRM_MAX_MTU ETH_DATA_LEN
 
 /* Min/Max frame lengths */
-#define QCAFRM_ETHMINLEN (QCAFRM_ETHMINMTU + ETH_HLEN)
-#define QCAFRM_ETHMAXLEN (QCAFRM_ETHMAXMTU + VLAN_ETH_HLEN)
+#define QCAFRM_MIN_LEN (QCAFRM_MIN_MTU + ETH_HLEN)
+#define QCAFRM_MAX_LEN (QCAFRM_MAX_MTU + VLAN_ETH_HLEN)
 
 /* QCA7K header len */
 #define QCAFRM_HEADER_LEN 8
@@ -61,6 +61,7 @@
 #define QCAFRM_ERR_BASE -1000
 
 enum qcafrm_state {
+	/* HW length is only available on SPI */
 	QCAFRM_HW_LEN0 = 0x8000,
 	QCAFRM_HW_LEN1 = QCAFRM_HW_LEN0 - 1,
 	QCAFRM_HW_LEN2 = QCAFRM_HW_LEN1 - 1,
@@ -101,9 +102,11 @@ enum qcafrm_state {
 struct qcafrm_handle {
 	/*  Current decoding state */
 	enum qcafrm_state state;
+	/* Initial state depends on connection type */
+	enum qcafrm_state init;
 
 	/* Offset in buffer (borrowed for length too) */
-	s16 offset;
+	u16 offset;
 
 	/* Frame length as kept by this module */
 	u16 len;
@@ -113,9 +116,16 @@ u16 qcafrm_create_header(u8 *buf, u16 len);
 
 u16 qcafrm_create_footer(u8 *buf);
 
-static inline void qcafrm_fsm_init(struct qcafrm_handle *handle)
+static inline void qcafrm_fsm_init_spi(struct qcafrm_handle *handle)
 {
-	handle->state = QCAFRM_HW_LEN0;
+	handle->init = QCAFRM_HW_LEN0;
+	handle->state = handle->init;
+}
+
+static inline void qcafrm_fsm_init_uart(struct qcafrm_handle *handle)
+{
+	handle->init = QCAFRM_WAIT_AA1;
+	handle->state = handle->init;
 }
 
 /*   Gather received bytes and try to extract a full Ethernet frame
diff --git a/drivers/net/ethernet/qualcomm/qca_debug.c b/drivers/net/ethernet/qualcomm/qca_debug.c
index d145df98feff..92b6be9c4429 100644
--- a/drivers/net/ethernet/qualcomm/qca_debug.c
+++ b/drivers/net/ethernet/qualcomm/qca_debug.c
@@ -275,6 +275,7 @@ qcaspi_get_ringparam(struct net_device *dev, struct ethtool_ringparam *ring)
 static int
 qcaspi_set_ringparam(struct net_device *dev, struct ethtool_ringparam *ring)
 {
+	const struct net_device_ops *ops = dev->netdev_ops;
 	struct qcaspi *qca = netdev_priv(dev);
 
 	if ((ring->rx_pending) ||
@@ -283,13 +284,13 @@ qcaspi_set_ringparam(struct net_device *dev, struct ethtool_ringparam *ring)
 		return -EINVAL;
 
 	if (netif_running(dev))
-		qcaspi_netdev_close(dev);
+		ops->ndo_stop(dev);
 
 	qca->txr.count = max_t(u32, ring->tx_pending, TX_RING_MIN_LEN);
 	qca->txr.count = min_t(u16, qca->txr.count, TX_RING_MAX_LEN);
 
 	if (netif_running(dev))
-		qcaspi_netdev_open(dev);
+		ops->ndo_open(dev);
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/qualcomm/qca_spi.c b/drivers/net/ethernet/qualcomm/qca_spi.c
index 24ca7df15d07..9c236298fe21 100644
--- a/drivers/net/ethernet/qualcomm/qca_spi.c
+++ b/drivers/net/ethernet/qualcomm/qca_spi.c
@@ -43,8 +43,8 @@
 #include <linux/types.h>
 
 #include "qca_7k.h"
+#include "qca_7k_common.h"
 #include "qca_debug.h"
-#include "qca_framing.h"
 #include "qca_spi.h"
 
 #define MAX_DMA_BURST_LEN 5000
@@ -69,7 +69,6 @@ static int qcaspi_pluggable = QCASPI_PLUGGABLE_MIN;
 module_param(qcaspi_pluggable, int, 0);
 MODULE_PARM_DESC(qcaspi_pluggable, "Pluggable SPI connection (yes/no).");
 
-#define QCASPI_MTU QCAFRM_ETHMAXMTU
 #define QCASPI_TX_TIMEOUT (1 * HZ)
 #define QCASPI_QCA7K_REBOOT_TIME_MS 1000
 
@@ -193,6 +192,30 @@ qcaspi_read_legacy(struct qcaspi *qca, u8 *dst, u32 len)
 }
 
 static int
+qcaspi_tx_cmd(struct qcaspi *qca, u16 cmd)
+{
+	__be16 tx_data;
+	struct spi_message *msg = &qca->spi_msg1;
+	struct spi_transfer *transfer = &qca->spi_xfer1;
+	int ret;
+
+	tx_data = cpu_to_be16(cmd);
+	transfer->len = sizeof(tx_data);
+	transfer->tx_buf = &tx_data;
+	transfer->rx_buf = NULL;
+
+	ret = spi_sync(qca->spi_dev, msg);
+
+	if (!ret)
+		ret = msg->status;
+
+	if (ret)
+		qcaspi_spi_error(qca);
+
+	return ret;
+}
+
+static int
 qcaspi_tx_frame(struct qcaspi *qca, struct sk_buff *skb)
 {
 	u32 count;
@@ -403,7 +426,7 @@ qcaspi_tx_ring_has_space(struct tx_ring *txr)
 	if (txr->skb[txr->tail])
 		return 0;
 
-	return (txr->size + QCAFRM_ETHMAXLEN < QCASPI_HW_BUF_LEN) ? 1 : 0;
+	return (txr->size + QCAFRM_MAX_LEN < QCASPI_HW_BUF_LEN) ? 1 : 0;
 }
 
 /*   Flush the tx ring. This function is only safe to
@@ -603,7 +626,7 @@ qcaspi_intr_handler(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-int
+static int
 qcaspi_netdev_open(struct net_device *dev)
 {
 	struct qcaspi *qca = netdev_priv(dev);
@@ -615,7 +638,7 @@ qcaspi_netdev_open(struct net_device *dev)
 	qca->intr_req = 1;
 	qca->intr_svc = 0;
 	qca->sync = QCASPI_SYNC_UNKNOWN;
-	qcafrm_fsm_init(&qca->frm_handle);
+	qcafrm_fsm_init_spi(&qca->frm_handle);
 
 	qca->spi_thread = kthread_run((void *)qcaspi_spi_thread,
 				      qca, "%s", dev->name);
@@ -640,7 +663,7 @@ qcaspi_netdev_open(struct net_device *dev)
 	return 0;
 }
 
-int
+static int
 qcaspi_netdev_close(struct net_device *dev)
 {
 	struct qcaspi *qca = netdev_priv(dev);
@@ -667,8 +690,8 @@ qcaspi_netdev_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct sk_buff *tskb;
 	u8 pad_len = 0;
 
-	if (skb->len < QCAFRM_ETHMINLEN)
-		pad_len = QCAFRM_ETHMINLEN - skb->len;
+	if (skb->len < QCAFRM_MIN_LEN)
+		pad_len = QCAFRM_MIN_LEN - skb->len;
 
 	if (qca->txr.skb[qca->txr.tail]) {
 		netdev_warn(qca->net_dev, "queue was unexpectedly full!\n");
@@ -696,8 +719,7 @@ qcaspi_netdev_xmit(struct sk_buff *skb, struct net_device *dev)
 	qcafrm_create_header(ptmp, frame_len);
 
 	if (pad_len) {
-		ptmp = skb_put(skb, pad_len);
-		memset(ptmp, 0, pad_len);
+		ptmp = skb_put_zero(skb, pad_len);
 	}
 
 	ptmp = skb_put(skb, QCAFRM_FOOTER_LEN);
@@ -746,7 +768,7 @@ qcaspi_netdev_init(struct net_device *dev)
 {
 	struct qcaspi *qca = netdev_priv(dev);
 
-	dev->mtu = QCASPI_MTU;
+	dev->mtu = QCAFRM_MAX_MTU;
 	dev->type = ARPHRD_ETHER;
 	qca->clkspeed = qcaspi_clkspeed;
 	qca->burst_len = qcaspi_burst_len;
@@ -805,8 +827,8 @@ qcaspi_netdev_setup(struct net_device *dev)
 	dev->tx_queue_len = 100;
 
 	/* MTU range: 46 - 1500 */
-	dev->min_mtu = QCAFRM_ETHMINMTU;
-	dev->max_mtu = QCAFRM_ETHMAXMTU;
+	dev->min_mtu = QCAFRM_MIN_MTU;
+	dev->max_mtu = QCAFRM_MAX_MTU;
 
 	qca = netdev_priv(dev);
 	memset(qca, 0, sizeof(struct qcaspi));
@@ -894,6 +916,7 @@ qca_spi_probe(struct spi_device *spi)
 		return -ENOMEM;
 
 	qcaspi_netdev_setup(qcaspi_devs);
+	SET_NETDEV_DEV(qcaspi_devs, &spi->dev);
 
 	qca = netdev_priv(qcaspi_devs);
 	if (!qca) {
@@ -975,7 +998,7 @@ static struct spi_driver qca_spi_driver = {
 };
 module_spi_driver(qca_spi_driver);
 
-MODULE_DESCRIPTION("Qualcomm Atheros SPI Driver");
+MODULE_DESCRIPTION("Qualcomm Atheros QCA7000 SPI Driver");
 MODULE_AUTHOR("Qualcomm Atheros Communications");
 MODULE_AUTHOR("Stefan Wahren <stefan.wahren@i2se.com>");
 MODULE_LICENSE("Dual BSD/GPL");
diff --git a/drivers/net/ethernet/qualcomm/qca_spi.h b/drivers/net/ethernet/qualcomm/qca_spi.h
index 6e31a0e744a4..fc4beb1b32d1 100644
--- a/drivers/net/ethernet/qualcomm/qca_spi.h
+++ b/drivers/net/ethernet/qualcomm/qca_spi.h
@@ -32,7 +32,7 @@
 #include <linux/spi/spi.h>
 #include <linux/types.h>
 
-#include "qca_framing.h"
+#include "qca_7k_common.h"
 
 #define QCASPI_DRV_VERSION "0.2.7-i"
 #define QCASPI_DRV_NAME    "qcaspi"
@@ -108,7 +108,4 @@ struct qcaspi {
 	u16 burst_len;
 };
 
-int qcaspi_netdev_open(struct net_device *dev);
-int qcaspi_netdev_close(struct net_device *dev);
-
 #endif /* _QCA_SPI_H */
diff --git a/drivers/net/ethernet/qualcomm/qca_uart.c b/drivers/net/ethernet/qualcomm/qca_uart.c
new file mode 100644
index 000000000000..db6068cd7a1f
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/qca_uart.c
@@ -0,0 +1,423 @@
+/*
+ *   Copyright (c) 2011, 2012, Qualcomm Atheros Communications Inc.
+ *   Copyright (c) 2017, I2SE GmbH
+ *
+ *   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.
+ */
+
+/*   This module implements the Qualcomm Atheros UART protocol for
+ *   kernel-based UART device; it is essentially an Ethernet-to-UART
+ *   serial converter;
+ */
+
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/etherdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_net.h>
+#include <linux/sched.h>
+#include <linux/serdev.h>
+#include <linux/skbuff.h>
+#include <linux/types.h>
+
+#include "qca_7k_common.h"
+
+#define QCAUART_DRV_VERSION "0.1.0"
+#define QCAUART_DRV_NAME "qcauart"
+#define QCAUART_TX_TIMEOUT (1 * HZ)
+
+struct qcauart {
+	struct net_device *net_dev;
+	spinlock_t lock;			/* transmit lock */
+	struct work_struct tx_work;		/* Flushes transmit buffer   */
+
+	struct serdev_device *serdev;
+	struct qcafrm_handle frm_handle;
+	struct sk_buff *rx_skb;
+
+	unsigned char *tx_head;			/* pointer to next XMIT byte */
+	int tx_left;				/* bytes left in XMIT queue  */
+	unsigned char *tx_buffer;
+};
+
+static int
+qca_tty_receive(struct serdev_device *serdev, const unsigned char *data,
+		size_t count)
+{
+	struct qcauart *qca = serdev_device_get_drvdata(serdev);
+	struct net_device *netdev = qca->net_dev;
+	struct net_device_stats *n_stats = &netdev->stats;
+	size_t i;
+
+	if (!qca->rx_skb) {
+		qca->rx_skb = netdev_alloc_skb_ip_align(netdev,
+							netdev->mtu +
+							VLAN_ETH_HLEN);
+		if (!qca->rx_skb) {
+			n_stats->rx_errors++;
+			n_stats->rx_dropped++;
+			return 0;
+		}
+	}
+
+	for (i = 0; i < count; i++) {
+		s32 retcode;
+
+		retcode = qcafrm_fsm_decode(&qca->frm_handle,
+					    qca->rx_skb->data,
+					    skb_tailroom(qca->rx_skb),
+					    data[i]);
+
+		switch (retcode) {
+		case QCAFRM_GATHER:
+		case QCAFRM_NOHEAD:
+			break;
+		case QCAFRM_NOTAIL:
+			netdev_dbg(netdev, "recv: no RX tail\n");
+			n_stats->rx_errors++;
+			n_stats->rx_dropped++;
+			break;
+		case QCAFRM_INVLEN:
+			netdev_dbg(netdev, "recv: invalid RX length\n");
+			n_stats->rx_errors++;
+			n_stats->rx_dropped++;
+			break;
+		default:
+			n_stats->rx_packets++;
+			n_stats->rx_bytes += retcode;
+			skb_put(qca->rx_skb, retcode);
+			qca->rx_skb->protocol = eth_type_trans(
+						qca->rx_skb, qca->rx_skb->dev);
+			qca->rx_skb->ip_summed = CHECKSUM_UNNECESSARY;
+			netif_rx_ni(qca->rx_skb);
+			qca->rx_skb = netdev_alloc_skb_ip_align(netdev,
+								netdev->mtu +
+								VLAN_ETH_HLEN);
+			if (!qca->rx_skb) {
+				netdev_dbg(netdev, "recv: out of RX resources\n");
+				n_stats->rx_errors++;
+				return i;
+			}
+		}
+	}
+
+	return i;
+}
+
+/* Write out any remaining transmit buffer. Scheduled when tty is writable */
+static void qcauart_transmit(struct work_struct *work)
+{
+	struct qcauart *qca = container_of(work, struct qcauart, tx_work);
+	struct net_device_stats *n_stats = &qca->net_dev->stats;
+	int written;
+
+	spin_lock_bh(&qca->lock);
+
+	/* First make sure we're connected. */
+	if (!netif_running(qca->net_dev)) {
+		spin_unlock_bh(&qca->lock);
+		return;
+	}
+
+	if (qca->tx_left <= 0)  {
+		/* Now serial buffer is almost free & we can start
+		 * transmission of another packet
+		 */
+		n_stats->tx_packets++;
+		spin_unlock_bh(&qca->lock);
+		netif_wake_queue(qca->net_dev);
+		return;
+	}
+
+	written = serdev_device_write_buf(qca->serdev, qca->tx_head,
+					  qca->tx_left);
+	if (written > 0) {
+		qca->tx_left -= written;
+		qca->tx_head += written;
+	}
+	spin_unlock_bh(&qca->lock);
+}
+
+/* Called by the driver when there's room for more data.
+ * Schedule the transmit.
+ */
+static void qca_tty_wakeup(struct serdev_device *serdev)
+{
+	struct qcauart *qca = serdev_device_get_drvdata(serdev);
+
+	schedule_work(&qca->tx_work);
+}
+
+static struct serdev_device_ops qca_serdev_ops = {
+	.receive_buf = qca_tty_receive,
+	.write_wakeup = qca_tty_wakeup,
+};
+
+static int qcauart_netdev_open(struct net_device *dev)
+{
+	struct qcauart *qca = netdev_priv(dev);
+
+	netif_start_queue(qca->net_dev);
+
+	return 0;
+}
+
+static int qcauart_netdev_close(struct net_device *dev)
+{
+	struct qcauart *qca = netdev_priv(dev);
+
+	netif_stop_queue(dev);
+	flush_work(&qca->tx_work);
+
+	spin_lock_bh(&qca->lock);
+	qca->tx_left = 0;
+	spin_unlock_bh(&qca->lock);
+
+	return 0;
+}
+
+static netdev_tx_t
+qcauart_netdev_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct net_device_stats *n_stats = &dev->stats;
+	struct qcauart *qca = netdev_priv(dev);
+	u8 pad_len = 0;
+	int written;
+	u8 *pos;
+
+	spin_lock(&qca->lock);
+
+	WARN_ON(qca->tx_left);
+
+	if (!netif_running(dev))  {
+		spin_unlock(&qca->lock);
+		netdev_warn(qca->net_dev, "xmit: iface is down\n");
+		goto out;
+	}
+
+	pos = qca->tx_buffer;
+
+	if (skb->len < QCAFRM_MIN_LEN)
+		pad_len = QCAFRM_MIN_LEN - skb->len;
+
+	pos += qcafrm_create_header(pos, skb->len + pad_len);
+
+	memcpy(pos, skb->data, skb->len);
+	pos += skb->len;
+
+	if (pad_len) {
+		memset(pos, 0, pad_len);
+		pos += pad_len;
+	}
+
+	pos += qcafrm_create_footer(pos);
+
+	netif_stop_queue(qca->net_dev);
+
+	written = serdev_device_write_buf(qca->serdev, qca->tx_buffer,
+					  pos - qca->tx_buffer);
+	if (written > 0) {
+		qca->tx_left = (pos - qca->tx_buffer) - written;
+		qca->tx_head = qca->tx_buffer + written;
+		n_stats->tx_bytes += written;
+	}
+	spin_unlock(&qca->lock);
+
+	netif_trans_update(dev);
+out:
+	dev_kfree_skb_any(skb);
+	return NETDEV_TX_OK;
+}
+
+static void qcauart_netdev_tx_timeout(struct net_device *dev)
+{
+	struct qcauart *qca = netdev_priv(dev);
+
+	netdev_info(qca->net_dev, "Transmit timeout at %ld, latency %ld\n",
+		    jiffies, dev_trans_start(dev));
+	dev->stats.tx_errors++;
+	dev->stats.tx_dropped++;
+}
+
+static int qcauart_netdev_init(struct net_device *dev)
+{
+	struct qcauart *qca = netdev_priv(dev);
+	size_t len;
+
+	/* Finish setting up the device info. */
+	dev->mtu = QCAFRM_MAX_MTU;
+	dev->type = ARPHRD_ETHER;
+
+	len = QCAFRM_HEADER_LEN + QCAFRM_MAX_LEN + QCAFRM_FOOTER_LEN;
+	qca->tx_buffer = devm_kmalloc(&qca->serdev->dev, len, GFP_KERNEL);
+	if (!qca->tx_buffer)
+		return -ENOMEM;
+
+	qca->rx_skb = netdev_alloc_skb_ip_align(qca->net_dev,
+						qca->net_dev->mtu +
+						VLAN_ETH_HLEN);
+	if (!qca->rx_skb)
+		return -ENOBUFS;
+
+	return 0;
+}
+
+static void qcauart_netdev_uninit(struct net_device *dev)
+{
+	struct qcauart *qca = netdev_priv(dev);
+
+	if (qca->rx_skb)
+		dev_kfree_skb(qca->rx_skb);
+}
+
+static const struct net_device_ops qcauart_netdev_ops = {
+	.ndo_init = qcauart_netdev_init,
+	.ndo_uninit = qcauart_netdev_uninit,
+	.ndo_open = qcauart_netdev_open,
+	.ndo_stop = qcauart_netdev_close,
+	.ndo_start_xmit = qcauart_netdev_xmit,
+	.ndo_set_mac_address = eth_mac_addr,
+	.ndo_tx_timeout = qcauart_netdev_tx_timeout,
+	.ndo_validate_addr = eth_validate_addr,
+};
+
+static void qcauart_netdev_setup(struct net_device *dev)
+{
+	dev->netdev_ops = &qcauart_netdev_ops;
+	dev->watchdog_timeo = QCAUART_TX_TIMEOUT;
+	dev->priv_flags &= ~IFF_TX_SKB_SHARING;
+	dev->tx_queue_len = 100;
+
+	/* MTU range: 46 - 1500 */
+	dev->min_mtu = QCAFRM_MIN_MTU;
+	dev->max_mtu = QCAFRM_MAX_MTU;
+}
+
+static const struct of_device_id qca_uart_of_match[] = {
+	{
+	 .compatible = "qca,qca7000",
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, qca_uart_of_match);
+
+static int qca_uart_probe(struct serdev_device *serdev)
+{
+	struct net_device *qcauart_dev = alloc_etherdev(sizeof(struct qcauart));
+	struct qcauart *qca;
+	const char *mac;
+	u32 speed = 115200;
+	int ret;
+
+	if (!qcauart_dev)
+		return -ENOMEM;
+
+	qcauart_netdev_setup(qcauart_dev);
+	SET_NETDEV_DEV(qcauart_dev, &serdev->dev);
+
+	qca = netdev_priv(qcauart_dev);
+	if (!qca) {
+		pr_err("qca_uart: Fail to retrieve private structure\n");
+		ret = -ENOMEM;
+		goto free;
+	}
+	qca->net_dev = qcauart_dev;
+	qca->serdev = serdev;
+	qcafrm_fsm_init_uart(&qca->frm_handle);
+
+	spin_lock_init(&qca->lock);
+	INIT_WORK(&qca->tx_work, qcauart_transmit);
+
+	of_property_read_u32(serdev->dev.of_node, "current-speed", &speed);
+
+	mac = of_get_mac_address(serdev->dev.of_node);
+
+	if (mac)
+		ether_addr_copy(qca->net_dev->dev_addr, mac);
+
+	if (!is_valid_ether_addr(qca->net_dev->dev_addr)) {
+		eth_hw_addr_random(qca->net_dev);
+		dev_info(&serdev->dev, "Using random MAC address: %pM\n",
+			 qca->net_dev->dev_addr);
+	}
+
+	netif_carrier_on(qca->net_dev);
+	serdev_device_set_drvdata(serdev, qca);
+	serdev_device_set_client_ops(serdev, &qca_serdev_ops);
+
+	ret = serdev_device_open(serdev);
+	if (ret) {
+		dev_err(&serdev->dev, "Unable to open device %s\n",
+			qcauart_dev->name);
+		goto free;
+	}
+
+	speed = serdev_device_set_baudrate(serdev, speed);
+	dev_info(&serdev->dev, "Using baudrate: %u\n", speed);
+
+	serdev_device_set_flow_control(serdev, false);
+
+	ret = register_netdev(qcauart_dev);
+	if (ret) {
+		dev_err(&serdev->dev, "Unable to register net device %s\n",
+			qcauart_dev->name);
+		serdev_device_close(serdev);
+		cancel_work_sync(&qca->tx_work);
+		goto free;
+	}
+
+	return 0;
+
+free:
+	free_netdev(qcauart_dev);
+	return ret;
+}
+
+static void qca_uart_remove(struct serdev_device *serdev)
+{
+	struct qcauart *qca = serdev_device_get_drvdata(serdev);
+
+	unregister_netdev(qca->net_dev);
+
+	/* Flush any pending characters in the driver. */
+	serdev_device_close(serdev);
+	cancel_work_sync(&qca->tx_work);
+
+	free_netdev(qca->net_dev);
+}
+
+static struct serdev_device_driver qca_uart_driver = {
+	.probe = qca_uart_probe,
+	.remove = qca_uart_remove,
+	.driver = {
+		.name = QCAUART_DRV_NAME,
+		.of_match_table = of_match_ptr(qca_uart_of_match),
+	},
+};
+
+module_serdev_device_driver(qca_uart_driver);
+
+MODULE_DESCRIPTION("Qualcomm Atheros QCA7000 UART Driver");
+MODULE_AUTHOR("Qualcomm Atheros Communications");
+MODULE_AUTHOR("Stefan Wahren <stefan.wahren@i2se.com>");
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_VERSION(QCAUART_DRV_VERSION);
diff --git a/drivers/net/ethernet/realtek/8139cp.c b/drivers/net/ethernet/realtek/8139cp.c
index 72233ab9474b..e7ab23e87de2 100644
--- a/drivers/net/ethernet/realtek/8139cp.c
+++ b/drivers/net/ethernet/realtek/8139cp.c
@@ -1410,14 +1410,13 @@ static int cp_get_link_ksettings(struct net_device *dev,
 				 struct ethtool_link_ksettings *cmd)
 {
 	struct cp_private *cp = netdev_priv(dev);
-	int rc;
 	unsigned long flags;
 
 	spin_lock_irqsave(&cp->lock, flags);
-	rc = mii_ethtool_get_link_ksettings(&cp->mii_if, cmd);
+	mii_ethtool_get_link_ksettings(&cp->mii_if, cmd);
 	spin_unlock_irqrestore(&cp->lock, flags);
 
-	return rc;
+	return 0;
 }
 
 static int cp_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/realtek/r8169.c b/drivers/net/ethernet/realtek/r8169.c
index 0a8f2817ea60..bd07a15d3b7c 100644
--- a/drivers/net/ethernet/realtek/r8169.c
+++ b/drivers/net/ethernet/realtek/r8169.c
@@ -2148,7 +2148,9 @@ static int rtl8169_get_link_ksettings_xmii(struct net_device *dev,
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
 
-	return mii_ethtool_get_link_ksettings(&tp->mii, cmd);
+	mii_ethtool_get_link_ksettings(&tp->mii, cmd);
+
+	return 0;
 }
 
 static int rtl8169_get_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/renesas/ravb_main.c b/drivers/net/ethernet/renesas/ravb_main.c
index 784782da3a85..5931e859876c 100644
--- a/drivers/net/ethernet/renesas/ravb_main.c
+++ b/drivers/net/ethernet/renesas/ravb_main.c
@@ -1076,16 +1076,16 @@ static int ravb_get_link_ksettings(struct net_device *ndev,
 				   struct ethtool_link_ksettings *cmd)
 {
 	struct ravb_private *priv = netdev_priv(ndev);
-	int error = -ENODEV;
 	unsigned long flags;
 
-	if (ndev->phydev) {
-		spin_lock_irqsave(&priv->lock, flags);
-		error = phy_ethtool_ksettings_get(ndev->phydev, cmd);
-		spin_unlock_irqrestore(&priv->lock, flags);
-	}
+	if (!ndev->phydev)
+		return -ENODEV;
 
-	return error;
+	spin_lock_irqsave(&priv->lock, flags);
+	phy_ethtool_ksettings_get(ndev->phydev, cmd);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
 }
 
 static int ravb_set_link_ksettings(struct net_device *ndev,
diff --git a/drivers/net/ethernet/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index 2d686ccf971b..d2dc0a8ef305 100644
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -1915,16 +1915,15 @@ static int sh_eth_get_link_ksettings(struct net_device *ndev,
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
 	unsigned long flags;
-	int ret;
 
 	if (!ndev->phydev)
 		return -ENODEV;
 
 	spin_lock_irqsave(&mdp->lock, flags);
-	ret = phy_ethtool_ksettings_get(ndev->phydev, cmd);
+	phy_ethtool_ksettings_get(ndev->phydev, cmd);
 	spin_unlock_irqrestore(&mdp->lock, flags);
 
-	return ret;
+	return 0;
 }
 
 static int sh_eth_set_link_ksettings(struct net_device *ndev,
@@ -2558,6 +2557,17 @@ static int sh_eth_do_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd)
 	return phy_mii_ioctl(phydev, rq, cmd);
 }
 
+static int sh_eth_change_mtu(struct net_device *ndev, int new_mtu)
+{
+	if (netif_running(ndev))
+		return -EBUSY;
+
+	ndev->mtu = new_mtu;
+	netdev_update_features(ndev);
+
+	return 0;
+}
+
 /* For TSU_POSTn. Please refer to the manual about this (strange) bitfields */
 static void *sh_eth_tsu_get_post_reg_offset(struct sh_eth_private *mdp,
 					    int entry)
@@ -3029,6 +3039,7 @@ static const struct net_device_ops sh_eth_netdev_ops = {
 	.ndo_set_rx_mode	= sh_eth_set_rx_mode,
 	.ndo_tx_timeout		= sh_eth_tx_timeout,
 	.ndo_do_ioctl		= sh_eth_do_ioctl,
+	.ndo_change_mtu		= sh_eth_change_mtu,
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_set_mac_address	= eth_mac_addr,
 };
@@ -3043,6 +3054,7 @@ static const struct net_device_ops sh_eth_netdev_ops_tsu = {
 	.ndo_vlan_rx_kill_vid	= sh_eth_vlan_rx_kill_vid,
 	.ndo_tx_timeout		= sh_eth_tx_timeout,
 	.ndo_do_ioctl		= sh_eth_do_ioctl,
+	.ndo_change_mtu		= sh_eth_change_mtu,
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_set_mac_address	= eth_mac_addr,
 };
@@ -3171,6 +3183,13 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 	}
 	sh_eth_set_default_cpu_data(mdp->cd);
 
+	/* User's manual states max MTU should be 2048 but due to the
+	 * alignment calculations in sh_eth_ring_init() the practical
+	 * MTU is a bit less. Maybe this can be optimized some more.
+	 */
+	ndev->max_mtu = 2000 - (ETH_HLEN + VLAN_HLEN + ETH_FCS_LEN);
+	ndev->min_mtu = ETH_MIN_MTU;
+
 	/* set function */
 	if (mdp->cd->tsu)
 		ndev->netdev_ops = &sh_eth_netdev_ops_tsu;
diff --git a/drivers/net/ethernet/rocker/rocker.h b/drivers/net/ethernet/rocker/rocker.h
index ee9675db5bf9..748fb12260a6 100644
--- a/drivers/net/ethernet/rocker/rocker.h
+++ b/drivers/net/ethernet/rocker/rocker.h
@@ -105,32 +105,27 @@ struct rocker_world_ops {
 	int (*port_open)(struct rocker_port *rocker_port);
 	void (*port_stop)(struct rocker_port *rocker_port);
 	int (*port_attr_stp_state_set)(struct rocker_port *rocker_port,
-				       u8 state,
-				       struct switchdev_trans *trans);
+				       u8 state);
 	int (*port_attr_bridge_flags_set)(struct rocker_port *rocker_port,
 					  unsigned long brport_flags,
 					  struct switchdev_trans *trans);
 	int (*port_attr_bridge_flags_get)(const struct rocker_port *rocker_port,
 					  unsigned long *p_brport_flags);
+	int (*port_attr_bridge_flags_support_get)(const struct rocker_port *
+						  rocker_port,
+						  unsigned long *
+						  p_brport_flags);
 	int (*port_attr_bridge_ageing_time_set)(struct rocker_port *rocker_port,
 						u32 ageing_time,
 						struct switchdev_trans *trans);
 	int (*port_obj_vlan_add)(struct rocker_port *rocker_port,
-				 const struct switchdev_obj_port_vlan *vlan,
-				 struct switchdev_trans *trans);
+				 const struct switchdev_obj_port_vlan *vlan);
 	int (*port_obj_vlan_del)(struct rocker_port *rocker_port,
 				 const struct switchdev_obj_port_vlan *vlan);
-	int (*port_obj_vlan_dump)(const struct rocker_port *rocker_port,
-				  struct switchdev_obj_port_vlan *vlan,
-				  switchdev_obj_dump_cb_t *cb);
 	int (*port_obj_fdb_add)(struct rocker_port *rocker_port,
-				const struct switchdev_obj_port_fdb *fdb,
-				struct switchdev_trans *trans);
+				u16 vid, const unsigned char *addr);
 	int (*port_obj_fdb_del)(struct rocker_port *rocker_port,
-				const struct switchdev_obj_port_fdb *fdb);
-	int (*port_obj_fdb_dump)(const struct rocker_port *rocker_port,
-				 struct switchdev_obj_port_fdb *fdb,
-				 switchdev_obj_dump_cb_t *cb);
+				u16 vid, const unsigned char *addr);
 	int (*port_master_linked)(struct rocker_port *rocker_port,
 				  struct net_device *master);
 	int (*port_master_unlinked)(struct rocker_port *rocker_port,
diff --git a/drivers/net/ethernet/rocker/rocker_main.c b/drivers/net/ethernet/rocker/rocker_main.c
index bab13613b138..b1e5c07099fa 100644
--- a/drivers/net/ethernet/rocker/rocker_main.c
+++ b/drivers/net/ethernet/rocker/rocker_main.c
@@ -1557,7 +1557,11 @@ static int rocker_world_port_attr_stp_state_set(struct rocker_port *rocker_port,
 
 	if (!wops->port_attr_stp_state_set)
 		return -EOPNOTSUPP;
-	return wops->port_attr_stp_state_set(rocker_port, state, trans);
+
+	if (switchdev_trans_ph_prepare(trans))
+		return 0;
+
+	return wops->port_attr_stp_state_set(rocker_port, state);
 }
 
 static int
@@ -1569,6 +1573,10 @@ rocker_world_port_attr_bridge_flags_set(struct rocker_port *rocker_port,
 
 	if (!wops->port_attr_bridge_flags_set)
 		return -EOPNOTSUPP;
+
+	if (switchdev_trans_ph_prepare(trans))
+		return 0;
+
 	return wops->port_attr_bridge_flags_set(rocker_port, brport_flags,
 						trans);
 }
@@ -1585,6 +1593,20 @@ rocker_world_port_attr_bridge_flags_get(const struct rocker_port *rocker_port,
 }
 
 static int
+rocker_world_port_attr_bridge_flags_support_get(const struct rocker_port *
+						rocker_port,
+						unsigned long *
+						p_brport_flags_support)
+{
+	struct rocker_world_ops *wops = rocker_port->rocker->wops;
+
+	if (!wops->port_attr_bridge_flags_support_get)
+		return -EOPNOTSUPP;
+	return wops->port_attr_bridge_flags_support_get(rocker_port,
+							p_brport_flags_support);
+}
+
+static int
 rocker_world_port_attr_bridge_ageing_time_set(struct rocker_port *rocker_port,
 					      u32 ageing_time,
 					      struct switchdev_trans *trans)
@@ -1594,6 +1616,10 @@ rocker_world_port_attr_bridge_ageing_time_set(struct rocker_port *rocker_port,
 
 	if (!wops->port_attr_bridge_ageing_time_set)
 		return -EOPNOTSUPP;
+
+	if (switchdev_trans_ph_prepare(trans))
+		return 0;
+
 	return wops->port_attr_bridge_ageing_time_set(rocker_port, ageing_time,
 						      trans);
 }
@@ -1607,7 +1633,11 @@ rocker_world_port_obj_vlan_add(struct rocker_port *rocker_port,
 
 	if (!wops->port_obj_vlan_add)
 		return -EOPNOTSUPP;
-	return wops->port_obj_vlan_add(rocker_port, vlan, trans);
+
+	if (switchdev_trans_ph_prepare(trans))
+		return 0;
+
+	return wops->port_obj_vlan_add(rocker_port, vlan);
 }
 
 static int
@@ -1622,50 +1652,26 @@ rocker_world_port_obj_vlan_del(struct rocker_port *rocker_port,
 }
 
 static int
-rocker_world_port_obj_vlan_dump(const struct rocker_port *rocker_port,
-				struct switchdev_obj_port_vlan *vlan,
-				switchdev_obj_dump_cb_t *cb)
-{
-	struct rocker_world_ops *wops = rocker_port->rocker->wops;
-
-	if (!wops->port_obj_vlan_dump)
-		return -EOPNOTSUPP;
-	return wops->port_obj_vlan_dump(rocker_port, vlan, cb);
-}
-
-static int
-rocker_world_port_obj_fdb_add(struct rocker_port *rocker_port,
-			      const struct switchdev_obj_port_fdb *fdb,
-			      struct switchdev_trans *trans)
+rocker_world_port_fdb_add(struct rocker_port *rocker_port,
+			  struct switchdev_notifier_fdb_info *info)
 {
 	struct rocker_world_ops *wops = rocker_port->rocker->wops;
 
 	if (!wops->port_obj_fdb_add)
 		return -EOPNOTSUPP;
-	return wops->port_obj_fdb_add(rocker_port, fdb, trans);
-}
-
-static int
-rocker_world_port_obj_fdb_del(struct rocker_port *rocker_port,
-			      const struct switchdev_obj_port_fdb *fdb)
-{
-	struct rocker_world_ops *wops = rocker_port->rocker->wops;
 
-	if (!wops->port_obj_fdb_del)
-		return -EOPNOTSUPP;
-	return wops->port_obj_fdb_del(rocker_port, fdb);
+	return wops->port_obj_fdb_add(rocker_port, info->vid, info->addr);
 }
 
 static int
-rocker_world_port_obj_fdb_dump(const struct rocker_port *rocker_port,
-			       struct switchdev_obj_port_fdb *fdb,
-			       switchdev_obj_dump_cb_t *cb)
+rocker_world_port_fdb_del(struct rocker_port *rocker_port,
+			  struct switchdev_notifier_fdb_info *info)
 {
 	struct rocker_world_ops *wops = rocker_port->rocker->wops;
 
-	if (!wops->port_obj_fdb_dump)
+	if (!wops->port_obj_fdb_del)
 		return -EOPNOTSUPP;
-	return wops->port_obj_fdb_dump(rocker_port, fdb, cb);
+	return wops->port_obj_fdb_del(rocker_port, info->vid, info->addr);
 }
 
 static int rocker_world_port_master_linked(struct rocker_port *rocker_port,
@@ -2022,12 +2028,6 @@ static const struct net_device_ops rocker_port_netdev_ops = {
 	.ndo_start_xmit			= rocker_port_xmit,
 	.ndo_set_mac_address		= rocker_port_set_mac_address,
 	.ndo_change_mtu			= rocker_port_change_mtu,
-	.ndo_bridge_getlink		= switchdev_port_bridge_getlink,
-	.ndo_bridge_setlink		= switchdev_port_bridge_setlink,
-	.ndo_bridge_dellink		= switchdev_port_bridge_dellink,
-	.ndo_fdb_add			= switchdev_port_fdb_add,
-	.ndo_fdb_del			= switchdev_port_fdb_del,
-	.ndo_fdb_dump			= switchdev_port_fdb_dump,
 	.ndo_get_phys_port_name		= rocker_port_get_phys_port_name,
 	.ndo_change_proto_down		= rocker_port_change_proto_down,
 	.ndo_neigh_destroy		= rocker_port_neigh_destroy,
@@ -2053,6 +2053,10 @@ static int rocker_port_attr_get(struct net_device *dev,
 		err = rocker_world_port_attr_bridge_flags_get(rocker_port,
 							      &attr->u.brport_flags);
 		break;
+	case SWITCHDEV_ATTR_ID_PORT_BRIDGE_FLAGS_SUPPORT:
+		err = rocker_world_port_attr_bridge_flags_support_get(rocker_port,
+								      &attr->u.brport_flags_support);
+		break;
 	default:
 		return -EOPNOTSUPP;
 	}
@@ -2104,11 +2108,6 @@ static int rocker_port_obj_add(struct net_device *dev,
 						     SWITCHDEV_OBJ_PORT_VLAN(obj),
 						     trans);
 		break;
-	case SWITCHDEV_OBJ_ID_PORT_FDB:
-		err = rocker_world_port_obj_fdb_add(rocker_port,
-						    SWITCHDEV_OBJ_PORT_FDB(obj),
-						    trans);
-		break;
 	default:
 		err = -EOPNOTSUPP;
 		break;
@@ -2128,36 +2127,6 @@ static int rocker_port_obj_del(struct net_device *dev,
 		err = rocker_world_port_obj_vlan_del(rocker_port,
 						     SWITCHDEV_OBJ_PORT_VLAN(obj));
 		break;
-	case SWITCHDEV_OBJ_ID_PORT_FDB:
-		err = rocker_world_port_obj_fdb_del(rocker_port,
-						    SWITCHDEV_OBJ_PORT_FDB(obj));
-		break;
-	default:
-		err = -EOPNOTSUPP;
-		break;
-	}
-
-	return err;
-}
-
-static int rocker_port_obj_dump(struct net_device *dev,
-				struct switchdev_obj *obj,
-				switchdev_obj_dump_cb_t *cb)
-{
-	const struct rocker_port *rocker_port = netdev_priv(dev);
-	int err = 0;
-
-	switch (obj->id) {
-	case SWITCHDEV_OBJ_ID_PORT_FDB:
-		err = rocker_world_port_obj_fdb_dump(rocker_port,
-						     SWITCHDEV_OBJ_PORT_FDB(obj),
-						     cb);
-		break;
-	case SWITCHDEV_OBJ_ID_PORT_VLAN:
-		err = rocker_world_port_obj_vlan_dump(rocker_port,
-						      SWITCHDEV_OBJ_PORT_VLAN(obj),
-						      cb);
-		break;
 	default:
 		err = -EOPNOTSUPP;
 		break;
@@ -2171,7 +2140,6 @@ static const struct switchdev_ops rocker_port_switchdev_ops = {
 	.switchdev_port_attr_set	= rocker_port_attr_set,
 	.switchdev_port_obj_add		= rocker_port_obj_add,
 	.switchdev_port_obj_del		= rocker_port_obj_del,
-	.switchdev_port_obj_dump	= rocker_port_obj_dump,
 };
 
 struct rocker_fib_event_work {
@@ -2729,6 +2697,109 @@ static void rocker_msix_fini(const struct rocker *rocker)
 	kfree(rocker->msix_entries);
 }
 
+static bool rocker_port_dev_check(const struct net_device *dev)
+{
+	return dev->netdev_ops == &rocker_port_netdev_ops;
+}
+
+struct rocker_switchdev_event_work {
+	struct work_struct work;
+	struct switchdev_notifier_fdb_info fdb_info;
+	struct rocker_port *rocker_port;
+	unsigned long event;
+};
+
+static void
+rocker_fdb_offload_notify(struct rocker_port *rocker_port,
+			  struct switchdev_notifier_fdb_info *recv_info)
+{
+	struct switchdev_notifier_fdb_info info;
+
+	info.addr = recv_info->addr;
+	info.vid = recv_info->vid;
+	call_switchdev_notifiers(SWITCHDEV_FDB_OFFLOADED,
+				 rocker_port->dev, &info.info);
+}
+
+static void rocker_switchdev_event_work(struct work_struct *work)
+{
+	struct rocker_switchdev_event_work *switchdev_work =
+		container_of(work, struct rocker_switchdev_event_work, work);
+	struct rocker_port *rocker_port = switchdev_work->rocker_port;
+	struct switchdev_notifier_fdb_info *fdb_info;
+	int err;
+
+	rtnl_lock();
+	switch (switchdev_work->event) {
+	case SWITCHDEV_FDB_ADD_TO_DEVICE:
+		fdb_info = &switchdev_work->fdb_info;
+		err = rocker_world_port_fdb_add(rocker_port, fdb_info);
+		if (err) {
+			netdev_dbg(rocker_port->dev, "fdb add failed err=%d\n", err);
+			break;
+		}
+		rocker_fdb_offload_notify(rocker_port, fdb_info);
+		break;
+	case SWITCHDEV_FDB_DEL_TO_DEVICE:
+		fdb_info = &switchdev_work->fdb_info;
+		err = rocker_world_port_fdb_del(rocker_port, fdb_info);
+		if (err)
+			netdev_dbg(rocker_port->dev, "fdb add failed err=%d\n", err);
+		break;
+	}
+	rtnl_unlock();
+
+	kfree(switchdev_work->fdb_info.addr);
+	kfree(switchdev_work);
+	dev_put(rocker_port->dev);
+}
+
+/* called under rcu_read_lock() */
+static int rocker_switchdev_event(struct notifier_block *unused,
+				  unsigned long event, void *ptr)
+{
+	struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
+	struct rocker_switchdev_event_work *switchdev_work;
+	struct switchdev_notifier_fdb_info *fdb_info = ptr;
+	struct rocker_port *rocker_port;
+
+	if (!rocker_port_dev_check(dev))
+		return NOTIFY_DONE;
+
+	rocker_port = netdev_priv(dev);
+	switchdev_work = kzalloc(sizeof(*switchdev_work), GFP_ATOMIC);
+	if (WARN_ON(!switchdev_work))
+		return NOTIFY_BAD;
+
+	INIT_WORK(&switchdev_work->work, rocker_switchdev_event_work);
+	switchdev_work->rocker_port = rocker_port;
+	switchdev_work->event = event;
+
+	switch (event) {
+	case SWITCHDEV_FDB_ADD_TO_DEVICE: /* fall through */
+	case SWITCHDEV_FDB_DEL_TO_DEVICE:
+		memcpy(&switchdev_work->fdb_info, ptr,
+		       sizeof(switchdev_work->fdb_info));
+		switchdev_work->fdb_info.addr = kzalloc(ETH_ALEN, GFP_ATOMIC);
+		ether_addr_copy((u8 *)switchdev_work->fdb_info.addr,
+				fdb_info->addr);
+		/* Take a reference on the rocker device */
+		dev_hold(dev);
+		break;
+	default:
+		kfree(switchdev_work);
+		return NOTIFY_DONE;
+	}
+
+	queue_work(rocker_port->rocker->rocker_owq,
+		   &switchdev_work->work);
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block rocker_switchdev_notifier = {
+	.notifier_call = rocker_switchdev_event,
+};
+
 static int rocker_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	struct rocker *rocker;
@@ -2834,6 +2905,12 @@ static int rocker_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (err)
 		goto err_register_fib_notifier;
 
+	err = register_switchdev_notifier(&rocker_switchdev_notifier);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to register switchdev notifier\n");
+		goto err_register_switchdev_notifier;
+	}
+
 	rocker->hw.id = rocker_read64(rocker, SWITCH_ID);
 
 	err = rocker_probe_ports(rocker);
@@ -2848,6 +2925,8 @@ static int rocker_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	return 0;
 
 err_probe_ports:
+	unregister_switchdev_notifier(&rocker_switchdev_notifier);
+err_register_switchdev_notifier:
 	unregister_fib_notifier(&rocker->fib_nb);
 err_register_fib_notifier:
 	destroy_workqueue(rocker->rocker_owq);
@@ -2878,6 +2957,7 @@ static void rocker_remove(struct pci_dev *pdev)
 	struct rocker *rocker = pci_get_drvdata(pdev);
 
 	rocker_remove_ports(rocker);
+	unregister_switchdev_notifier(&rocker_switchdev_notifier);
 	unregister_fib_notifier(&rocker->fib_nb);
 	rocker_write32(rocker, CONTROL, ROCKER_CONTROL_RESET);
 	destroy_workqueue(rocker->rocker_owq);
@@ -2902,11 +2982,6 @@ static struct pci_driver rocker_pci_driver = {
  * Net device notifier event handler
  ************************************/
 
-static bool rocker_port_dev_check(const struct net_device *dev)
-{
-	return dev->netdev_ops == &rocker_port_netdev_ops;
-}
-
 static bool rocker_port_dev_check_under(const struct net_device *dev,
 					struct rocker *rocker)
 {
diff --git a/drivers/net/ethernet/rocker/rocker_ofdpa.c b/drivers/net/ethernet/rocker/rocker_ofdpa.c
index 2ae852454780..600e30e8f0be 100644
--- a/drivers/net/ethernet/rocker/rocker_ofdpa.c
+++ b/drivers/net/ethernet/rocker/rocker_ofdpa.c
@@ -300,64 +300,6 @@ static bool ofdpa_flags_nowait(int flags)
 	return flags & OFDPA_OP_FLAG_NOWAIT;
 }
 
-static void *__ofdpa_mem_alloc(struct switchdev_trans *trans, int flags,
-			       size_t size)
-{
-	struct switchdev_trans_item *elem = NULL;
-	gfp_t gfp_flags = (flags & OFDPA_OP_FLAG_NOWAIT) ?
-			  GFP_ATOMIC : GFP_KERNEL;
-
-	/* If in transaction prepare phase, allocate the memory
-	 * and enqueue it on a transaction.  If in transaction
-	 * commit phase, dequeue the memory from the transaction
-	 * rather than re-allocating the memory.  The idea is the
-	 * driver code paths for prepare and commit are identical
-	 * so the memory allocated in the prepare phase is the
-	 * memory used in the commit phase.
-	 */
-
-	if (!trans) {
-		elem = kzalloc(size + sizeof(*elem), gfp_flags);
-	} else if (switchdev_trans_ph_prepare(trans)) {
-		elem = kzalloc(size + sizeof(*elem), gfp_flags);
-		if (!elem)
-			return NULL;
-		switchdev_trans_item_enqueue(trans, elem, kfree, elem);
-	} else {
-		elem = switchdev_trans_item_dequeue(trans);
-	}
-
-	return elem ? elem + 1 : NULL;
-}
-
-static void *ofdpa_kzalloc(struct switchdev_trans *trans, int flags,
-			   size_t size)
-{
-	return __ofdpa_mem_alloc(trans, flags, size);
-}
-
-static void *ofdpa_kcalloc(struct switchdev_trans *trans, int flags,
-			   size_t n, size_t size)
-{
-	return __ofdpa_mem_alloc(trans, flags, n * size);
-}
-
-static void ofdpa_kfree(struct switchdev_trans *trans, const void *mem)
-{
-	struct switchdev_trans_item *elem;
-
-	/* Frees are ignored if in transaction prepare phase.  The
-	 * memory remains on the per-port list until freed in the
-	 * commit phase.
-	 */
-
-	if (switchdev_trans_ph_prepare(trans))
-		return;
-
-	elem = (struct switchdev_trans_item *) mem - 1;
-	kfree(elem);
-}
-
 /*************************************************************
  * Flow, group, FDB, internal VLAN and neigh command prepares
  *************************************************************/
@@ -815,8 +757,7 @@ ofdpa_flow_tbl_find(const struct ofdpa *ofdpa,
 }
 
 static int ofdpa_flow_tbl_add(struct ofdpa_port *ofdpa_port,
-			      struct switchdev_trans *trans, int flags,
-			      struct ofdpa_flow_tbl_entry *match)
+			      int flags, struct ofdpa_flow_tbl_entry *match)
 {
 	struct ofdpa *ofdpa = ofdpa_port->ofdpa;
 	struct ofdpa_flow_tbl_entry *found;
@@ -831,9 +772,8 @@ static int ofdpa_flow_tbl_add(struct ofdpa_port *ofdpa_port,
 
 	if (found) {
 		match->cookie = found->cookie;
-		if (!switchdev_trans_ph_prepare(trans))
-			hash_del(&found->entry);
-		ofdpa_kfree(trans, found);
+		hash_del(&found->entry);
+		kfree(found);
 		found = match;
 		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_FLOW_MOD;
 	} else {
@@ -842,22 +782,18 @@ static int ofdpa_flow_tbl_add(struct ofdpa_port *ofdpa_port,
 		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_FLOW_ADD;
 	}
 
-	if (!switchdev_trans_ph_prepare(trans))
-		hash_add(ofdpa->flow_tbl, &found->entry, found->key_crc32);
-
+	hash_add(ofdpa->flow_tbl, &found->entry, found->key_crc32);
 	spin_unlock_irqrestore(&ofdpa->flow_tbl_lock, lock_flags);
 
-	if (!switchdev_trans_ph_prepare(trans))
-		return rocker_cmd_exec(ofdpa_port->rocker_port,
-				       ofdpa_flags_nowait(flags),
-				       ofdpa_cmd_flow_tbl_add,
-				       found, NULL, NULL);
+	return rocker_cmd_exec(ofdpa_port->rocker_port,
+			       ofdpa_flags_nowait(flags),
+			       ofdpa_cmd_flow_tbl_add,
+			       found, NULL, NULL);
 	return 0;
 }
 
 static int ofdpa_flow_tbl_del(struct ofdpa_port *ofdpa_port,
-			      struct switchdev_trans *trans, int flags,
-			      struct ofdpa_flow_tbl_entry *match)
+			      int flags, struct ofdpa_flow_tbl_entry *match)
 {
 	struct ofdpa *ofdpa = ofdpa_port->ofdpa;
 	struct ofdpa_flow_tbl_entry *found;
@@ -872,45 +808,41 @@ static int ofdpa_flow_tbl_del(struct ofdpa_port *ofdpa_port,
 	found = ofdpa_flow_tbl_find(ofdpa, match);
 
 	if (found) {
-		if (!switchdev_trans_ph_prepare(trans))
-			hash_del(&found->entry);
+		hash_del(&found->entry);
 		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_FLOW_DEL;
 	}
 
 	spin_unlock_irqrestore(&ofdpa->flow_tbl_lock, lock_flags);
 
-	ofdpa_kfree(trans, match);
+	kfree(match);
 
 	if (found) {
-		if (!switchdev_trans_ph_prepare(trans))
-			err = rocker_cmd_exec(ofdpa_port->rocker_port,
-					      ofdpa_flags_nowait(flags),
-					      ofdpa_cmd_flow_tbl_del,
-					      found, NULL, NULL);
-		ofdpa_kfree(trans, found);
+		err = rocker_cmd_exec(ofdpa_port->rocker_port,
+				      ofdpa_flags_nowait(flags),
+				      ofdpa_cmd_flow_tbl_del,
+				      found, NULL, NULL);
+		kfree(found);
 	}
 
 	return err;
 }
 
-static int ofdpa_flow_tbl_do(struct ofdpa_port *ofdpa_port,
-			     struct switchdev_trans *trans, int flags,
+static int ofdpa_flow_tbl_do(struct ofdpa_port *ofdpa_port, int flags,
 			     struct ofdpa_flow_tbl_entry *entry)
 {
 	if (flags & OFDPA_OP_FLAG_REMOVE)
-		return ofdpa_flow_tbl_del(ofdpa_port, trans, flags, entry);
+		return ofdpa_flow_tbl_del(ofdpa_port, flags, entry);
 	else
-		return ofdpa_flow_tbl_add(ofdpa_port, trans, flags, entry);
+		return ofdpa_flow_tbl_add(ofdpa_port, flags, entry);
 }
 
-static int ofdpa_flow_tbl_ig_port(struct ofdpa_port *ofdpa_port,
-				  struct switchdev_trans *trans, int flags,
+static int ofdpa_flow_tbl_ig_port(struct ofdpa_port *ofdpa_port, int flags,
 				  u32 in_pport, u32 in_pport_mask,
 				  enum rocker_of_dpa_table_id goto_tbl)
 {
 	struct ofdpa_flow_tbl_entry *entry;
 
-	entry = ofdpa_kzalloc(trans, flags, sizeof(*entry));
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
 	if (!entry)
 		return -ENOMEM;
 
@@ -920,11 +852,11 @@ static int ofdpa_flow_tbl_ig_port(struct ofdpa_port *ofdpa_port,
 	entry->key.ig_port.in_pport_mask = in_pport_mask;
 	entry->key.ig_port.goto_tbl = goto_tbl;
 
-	return ofdpa_flow_tbl_do(ofdpa_port, trans, flags, entry);
+	return ofdpa_flow_tbl_do(ofdpa_port, flags, entry);
 }
 
 static int ofdpa_flow_tbl_vlan(struct ofdpa_port *ofdpa_port,
-			       struct switchdev_trans *trans, int flags,
+			       int flags,
 			       u32 in_pport, __be16 vlan_id,
 			       __be16 vlan_id_mask,
 			       enum rocker_of_dpa_table_id goto_tbl,
@@ -932,7 +864,7 @@ static int ofdpa_flow_tbl_vlan(struct ofdpa_port *ofdpa_port,
 {
 	struct ofdpa_flow_tbl_entry *entry;
 
-	entry = ofdpa_kzalloc(trans, flags, sizeof(*entry));
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
 	if (!entry)
 		return -ENOMEM;
 
@@ -946,11 +878,10 @@ static int ofdpa_flow_tbl_vlan(struct ofdpa_port *ofdpa_port,
 	entry->key.vlan.untagged = untagged;
 	entry->key.vlan.new_vlan_id = new_vlan_id;
 
-	return ofdpa_flow_tbl_do(ofdpa_port, trans, flags, entry);
+	return ofdpa_flow_tbl_do(ofdpa_port, flags, entry);
 }
 
 static int ofdpa_flow_tbl_term_mac(struct ofdpa_port *ofdpa_port,
-				   struct switchdev_trans *trans,
 				   u32 in_pport, u32 in_pport_mask,
 				   __be16 eth_type, const u8 *eth_dst,
 				   const u8 *eth_dst_mask, __be16 vlan_id,
@@ -959,7 +890,7 @@ static int ofdpa_flow_tbl_term_mac(struct ofdpa_port *ofdpa_port,
 {
 	struct ofdpa_flow_tbl_entry *entry;
 
-	entry = ofdpa_kzalloc(trans, flags, sizeof(*entry));
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
 	if (!entry)
 		return -ENOMEM;
 
@@ -983,13 +914,13 @@ static int ofdpa_flow_tbl_term_mac(struct ofdpa_port *ofdpa_port,
 	entry->key.term_mac.vlan_id_mask = vlan_id_mask;
 	entry->key.term_mac.copy_to_cpu = copy_to_cpu;
 
-	return ofdpa_flow_tbl_do(ofdpa_port, trans, flags, entry);
+	return ofdpa_flow_tbl_do(ofdpa_port, flags, entry);
 }
 
 static int ofdpa_flow_tbl_bridge(struct ofdpa_port *ofdpa_port,
-				 struct switchdev_trans *trans, int flags,
-				 const u8 *eth_dst, const u8 *eth_dst_mask,
-				 __be16 vlan_id, u32 tunnel_id,
+				 int flags, const u8 *eth_dst,
+				 const u8 *eth_dst_mask,  __be16 vlan_id,
+				 u32 tunnel_id,
 				 enum rocker_of_dpa_table_id goto_tbl,
 				 u32 group_id, bool copy_to_cpu)
 {
@@ -999,7 +930,7 @@ static int ofdpa_flow_tbl_bridge(struct ofdpa_port *ofdpa_port,
 	bool dflt = !eth_dst || (eth_dst && eth_dst_mask);
 	bool wild = false;
 
-	entry = ofdpa_kzalloc(trans, flags, sizeof(*entry));
+	entry = kzalloc(sizeof(*entry), GFP_ATOMIC);
 	if (!entry)
 		return -ENOMEM;
 
@@ -1037,11 +968,10 @@ static int ofdpa_flow_tbl_bridge(struct ofdpa_port *ofdpa_port,
 	entry->key.bridge.group_id = group_id;
 	entry->key.bridge.copy_to_cpu = copy_to_cpu;
 
-	return ofdpa_flow_tbl_do(ofdpa_port, trans, flags, entry);
+	return ofdpa_flow_tbl_do(ofdpa_port, flags, entry);
 }
 
 static int ofdpa_flow_tbl_ucast4_routing(struct ofdpa_port *ofdpa_port,
-					 struct switchdev_trans *trans,
 					 __be16 eth_type, __be32 dst,
 					 __be32 dst_mask, u32 priority,
 					 enum rocker_of_dpa_table_id goto_tbl,
@@ -1050,7 +980,7 @@ static int ofdpa_flow_tbl_ucast4_routing(struct ofdpa_port *ofdpa_port,
 {
 	struct ofdpa_flow_tbl_entry *entry;
 
-	entry = ofdpa_kzalloc(trans, flags, sizeof(*entry));
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
 	if (!entry)
 		return -ENOMEM;
 
@@ -1065,11 +995,10 @@ static int ofdpa_flow_tbl_ucast4_routing(struct ofdpa_port *ofdpa_port,
 				  ucast_routing.group_id);
 	entry->fi = fi;
 
-	return ofdpa_flow_tbl_do(ofdpa_port, trans, flags, entry);
+	return ofdpa_flow_tbl_do(ofdpa_port, flags, entry);
 }
 
-static int ofdpa_flow_tbl_acl(struct ofdpa_port *ofdpa_port,
-			      struct switchdev_trans *trans, int flags,
+static int ofdpa_flow_tbl_acl(struct ofdpa_port *ofdpa_port, int flags,
 			      u32 in_pport, u32 in_pport_mask,
 			      const u8 *eth_src, const u8 *eth_src_mask,
 			      const u8 *eth_dst, const u8 *eth_dst_mask,
@@ -1081,7 +1010,7 @@ static int ofdpa_flow_tbl_acl(struct ofdpa_port *ofdpa_port,
 	u32 priority;
 	struct ofdpa_flow_tbl_entry *entry;
 
-	entry = ofdpa_kzalloc(trans, flags, sizeof(*entry));
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
 	if (!entry)
 		return -ENOMEM;
 
@@ -1116,7 +1045,7 @@ static int ofdpa_flow_tbl_acl(struct ofdpa_port *ofdpa_port,
 	entry->key.acl.ip_tos_mask = ip_tos_mask;
 	entry->key.acl.group_id = group_id;
 
-	return ofdpa_flow_tbl_do(ofdpa_port, trans, flags, entry);
+	return ofdpa_flow_tbl_do(ofdpa_port, flags, entry);
 }
 
 static struct ofdpa_group_tbl_entry *
@@ -1134,22 +1063,20 @@ ofdpa_group_tbl_find(const struct ofdpa *ofdpa,
 	return NULL;
 }
 
-static void ofdpa_group_tbl_entry_free(struct switchdev_trans *trans,
-				       struct ofdpa_group_tbl_entry *entry)
+static void ofdpa_group_tbl_entry_free(struct ofdpa_group_tbl_entry *entry)
 {
 	switch (ROCKER_GROUP_TYPE_GET(entry->group_id)) {
 	case ROCKER_OF_DPA_GROUP_TYPE_L2_FLOOD:
 	case ROCKER_OF_DPA_GROUP_TYPE_L2_MCAST:
-		ofdpa_kfree(trans, entry->group_ids);
+		kfree(entry->group_ids);
 		break;
 	default:
 		break;
 	}
-	ofdpa_kfree(trans, entry);
+	kfree(entry);
 }
 
-static int ofdpa_group_tbl_add(struct ofdpa_port *ofdpa_port,
-			       struct switchdev_trans *trans, int flags,
+static int ofdpa_group_tbl_add(struct ofdpa_port *ofdpa_port, int flags,
 			       struct ofdpa_group_tbl_entry *match)
 {
 	struct ofdpa *ofdpa = ofdpa_port->ofdpa;
@@ -1161,9 +1088,8 @@ static int ofdpa_group_tbl_add(struct ofdpa_port *ofdpa_port,
 	found = ofdpa_group_tbl_find(ofdpa, match);
 
 	if (found) {
-		if (!switchdev_trans_ph_prepare(trans))
-			hash_del(&found->entry);
-		ofdpa_group_tbl_entry_free(trans, found);
+		hash_del(&found->entry);
+		ofdpa_group_tbl_entry_free(found);
 		found = match;
 		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_GROUP_MOD;
 	} else {
@@ -1171,21 +1097,17 @@ static int ofdpa_group_tbl_add(struct ofdpa_port *ofdpa_port,
 		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_GROUP_ADD;
 	}
 
-	if (!switchdev_trans_ph_prepare(trans))
-		hash_add(ofdpa->group_tbl, &found->entry, found->group_id);
+	hash_add(ofdpa->group_tbl, &found->entry, found->group_id);
 
 	spin_unlock_irqrestore(&ofdpa->group_tbl_lock, lock_flags);
 
-	if (!switchdev_trans_ph_prepare(trans))
-		return rocker_cmd_exec(ofdpa_port->rocker_port,
-				       ofdpa_flags_nowait(flags),
-				       ofdpa_cmd_group_tbl_add,
-				       found, NULL, NULL);
-	return 0;
+	return rocker_cmd_exec(ofdpa_port->rocker_port,
+			       ofdpa_flags_nowait(flags),
+			       ofdpa_cmd_group_tbl_add,
+			       found, NULL, NULL);
 }
 
-static int ofdpa_group_tbl_del(struct ofdpa_port *ofdpa_port,
-			       struct switchdev_trans *trans, int flags,
+static int ofdpa_group_tbl_del(struct ofdpa_port *ofdpa_port, int flags,
 			       struct ofdpa_group_tbl_entry *match)
 {
 	struct ofdpa *ofdpa = ofdpa_port->ofdpa;
@@ -1198,97 +1120,90 @@ static int ofdpa_group_tbl_del(struct ofdpa_port *ofdpa_port,
 	found = ofdpa_group_tbl_find(ofdpa, match);
 
 	if (found) {
-		if (!switchdev_trans_ph_prepare(trans))
-			hash_del(&found->entry);
+		hash_del(&found->entry);
 		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_GROUP_DEL;
 	}
 
 	spin_unlock_irqrestore(&ofdpa->group_tbl_lock, lock_flags);
 
-	ofdpa_group_tbl_entry_free(trans, match);
+	ofdpa_group_tbl_entry_free(match);
 
 	if (found) {
-		if (!switchdev_trans_ph_prepare(trans))
-			err = rocker_cmd_exec(ofdpa_port->rocker_port,
-					      ofdpa_flags_nowait(flags),
-					      ofdpa_cmd_group_tbl_del,
-					      found, NULL, NULL);
-		ofdpa_group_tbl_entry_free(trans, found);
+		err = rocker_cmd_exec(ofdpa_port->rocker_port,
+				      ofdpa_flags_nowait(flags),
+				      ofdpa_cmd_group_tbl_del,
+				      found, NULL, NULL);
+		ofdpa_group_tbl_entry_free(found);
 	}
 
 	return err;
 }
 
-static int ofdpa_group_tbl_do(struct ofdpa_port *ofdpa_port,
-			      struct switchdev_trans *trans, int flags,
+static int ofdpa_group_tbl_do(struct ofdpa_port *ofdpa_port, int flags,
 			      struct ofdpa_group_tbl_entry *entry)
 {
 	if (flags & OFDPA_OP_FLAG_REMOVE)
-		return ofdpa_group_tbl_del(ofdpa_port, trans, flags, entry);
+		return ofdpa_group_tbl_del(ofdpa_port, flags, entry);
 	else
-		return ofdpa_group_tbl_add(ofdpa_port, trans, flags, entry);
+		return ofdpa_group_tbl_add(ofdpa_port, flags, entry);
 }
 
 static int ofdpa_group_l2_interface(struct ofdpa_port *ofdpa_port,
-				    struct switchdev_trans *trans, int flags,
-				    __be16 vlan_id, u32 out_pport,
-				    int pop_vlan)
+				    int flags, __be16 vlan_id,
+				    u32 out_pport, int pop_vlan)
 {
 	struct ofdpa_group_tbl_entry *entry;
 
-	entry = ofdpa_kzalloc(trans, flags, sizeof(*entry));
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
 	if (!entry)
 		return -ENOMEM;
 
 	entry->group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, out_pport);
 	entry->l2_interface.pop_vlan = pop_vlan;
 
-	return ofdpa_group_tbl_do(ofdpa_port, trans, flags, entry);
+	return ofdpa_group_tbl_do(ofdpa_port, flags, entry);
 }
 
 static int ofdpa_group_l2_fan_out(struct ofdpa_port *ofdpa_port,
-				  struct switchdev_trans *trans,
 				  int flags, u8 group_count,
 				  const u32 *group_ids, u32 group_id)
 {
 	struct ofdpa_group_tbl_entry *entry;
 
-	entry = ofdpa_kzalloc(trans, flags, sizeof(*entry));
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
 	if (!entry)
 		return -ENOMEM;
 
 	entry->group_id = group_id;
 	entry->group_count = group_count;
 
-	entry->group_ids = ofdpa_kcalloc(trans, flags,
-					 group_count, sizeof(u32));
+	entry->group_ids = kcalloc(flags, group_count, sizeof(u32));
 	if (!entry->group_ids) {
-		ofdpa_kfree(trans, entry);
+		kfree(entry);
 		return -ENOMEM;
 	}
 	memcpy(entry->group_ids, group_ids, group_count * sizeof(u32));
 
-	return ofdpa_group_tbl_do(ofdpa_port, trans, flags, entry);
+	return ofdpa_group_tbl_do(ofdpa_port, flags, entry);
 }
 
 static int ofdpa_group_l2_flood(struct ofdpa_port *ofdpa_port,
-				struct switchdev_trans *trans, int flags,
-				__be16 vlan_id, u8 group_count,
-				const u32 *group_ids, u32 group_id)
+				int flags, __be16 vlan_id,
+				u8 group_count,	const u32 *group_ids,
+				u32 group_id)
 {
-	return ofdpa_group_l2_fan_out(ofdpa_port, trans, flags,
+	return ofdpa_group_l2_fan_out(ofdpa_port, flags,
 				      group_count, group_ids,
 				      group_id);
 }
 
-static int ofdpa_group_l3_unicast(struct ofdpa_port *ofdpa_port,
-				  struct switchdev_trans *trans, int flags,
+static int ofdpa_group_l3_unicast(struct ofdpa_port *ofdpa_port, int flags,
 				  u32 index, const u8 *src_mac, const u8 *dst_mac,
 				  __be16 vlan_id, bool ttl_check, u32 pport)
 {
 	struct ofdpa_group_tbl_entry *entry;
 
-	entry = ofdpa_kzalloc(trans, flags, sizeof(*entry));
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
 	if (!entry)
 		return -ENOMEM;
 
@@ -1301,7 +1216,7 @@ static int ofdpa_group_l3_unicast(struct ofdpa_port *ofdpa_port,
 	entry->l3_unicast.ttl_check = ttl_check;
 	entry->l3_unicast.group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, pport);
 
-	return ofdpa_group_tbl_do(ofdpa_port, trans, flags, entry);
+	return ofdpa_group_tbl_do(ofdpa_port, flags, entry);
 }
 
 static struct ofdpa_neigh_tbl_entry *
@@ -1318,43 +1233,34 @@ ofdpa_neigh_tbl_find(const struct ofdpa *ofdpa, __be32 ip_addr)
 }
 
 static void ofdpa_neigh_add(struct ofdpa *ofdpa,
-			    struct switchdev_trans *trans,
 			    struct ofdpa_neigh_tbl_entry *entry)
 {
-	if (!switchdev_trans_ph_commit(trans))
-		entry->index = ofdpa->neigh_tbl_next_index++;
-	if (switchdev_trans_ph_prepare(trans))
-		return;
+	entry->index = ofdpa->neigh_tbl_next_index++;
 	entry->ref_count++;
 	hash_add(ofdpa->neigh_tbl, &entry->entry,
 		 be32_to_cpu(entry->ip_addr));
 }
 
-static void ofdpa_neigh_del(struct switchdev_trans *trans,
-			    struct ofdpa_neigh_tbl_entry *entry)
+static void ofdpa_neigh_del(struct ofdpa_neigh_tbl_entry *entry)
 {
-	if (switchdev_trans_ph_prepare(trans))
-		return;
 	if (--entry->ref_count == 0) {
 		hash_del(&entry->entry);
-		ofdpa_kfree(trans, entry);
+		kfree(entry);
 	}
 }
 
 static void ofdpa_neigh_update(struct ofdpa_neigh_tbl_entry *entry,
-			       struct switchdev_trans *trans,
 			       const u8 *eth_dst, bool ttl_check)
 {
 	if (eth_dst) {
 		ether_addr_copy(entry->eth_dst, eth_dst);
 		entry->ttl_check = ttl_check;
-	} else if (!switchdev_trans_ph_prepare(trans)) {
+	} else {
 		entry->ref_count++;
 	}
 }
 
 static int ofdpa_port_ipv4_neigh(struct ofdpa_port *ofdpa_port,
-				 struct switchdev_trans *trans,
 				 int flags, __be32 ip_addr, const u8 *eth_dst)
 {
 	struct ofdpa *ofdpa = ofdpa_port->ofdpa;
@@ -1371,7 +1277,7 @@ static int ofdpa_port_ipv4_neigh(struct ofdpa_port *ofdpa_port,
 	bool removing;
 	int err = 0;
 
-	entry = ofdpa_kzalloc(trans, flags, sizeof(*entry));
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
 	if (!entry)
 		return -ENOMEM;
 
@@ -1388,12 +1294,12 @@ static int ofdpa_port_ipv4_neigh(struct ofdpa_port *ofdpa_port,
 		entry->dev = ofdpa_port->dev;
 		ether_addr_copy(entry->eth_dst, eth_dst);
 		entry->ttl_check = true;
-		ofdpa_neigh_add(ofdpa, trans, entry);
+		ofdpa_neigh_add(ofdpa, entry);
 	} else if (removing) {
 		memcpy(entry, found, sizeof(*entry));
-		ofdpa_neigh_del(trans, found);
+		ofdpa_neigh_del(found);
 	} else if (updating) {
-		ofdpa_neigh_update(found, trans, eth_dst, true);
+		ofdpa_neigh_update(found, eth_dst, true);
 		memcpy(entry, found, sizeof(*entry));
 	} else {
 		err = -ENOENT;
@@ -1410,7 +1316,7 @@ static int ofdpa_port_ipv4_neigh(struct ofdpa_port *ofdpa_port,
 	 * other routes' nexthops.
 	 */
 
-	err = ofdpa_group_l3_unicast(ofdpa_port, trans, flags,
+	err = ofdpa_group_l3_unicast(ofdpa_port, flags,
 				     entry->index,
 				     ofdpa_port->dev->dev_addr,
 				     entry->eth_dst,
@@ -1425,7 +1331,7 @@ static int ofdpa_port_ipv4_neigh(struct ofdpa_port *ofdpa_port,
 
 	if (adding || removing) {
 		group_id = ROCKER_GROUP_L3_UNICAST(entry->index);
-		err = ofdpa_flow_tbl_ucast4_routing(ofdpa_port, trans,
+		err = ofdpa_flow_tbl_ucast4_routing(ofdpa_port,
 						    eth_type, ip_addr,
 						    inet_make_mask(32),
 						    priority, goto_tbl,
@@ -1438,13 +1344,12 @@ static int ofdpa_port_ipv4_neigh(struct ofdpa_port *ofdpa_port,
 
 err_out:
 	if (!adding)
-		ofdpa_kfree(trans, entry);
+		kfree(entry);
 
 	return err;
 }
 
 static int ofdpa_port_ipv4_resolve(struct ofdpa_port *ofdpa_port,
-				   struct switchdev_trans *trans,
 				   __be32 ip_addr)
 {
 	struct net_device *dev = ofdpa_port->dev;
@@ -1463,7 +1368,7 @@ static int ofdpa_port_ipv4_resolve(struct ofdpa_port *ofdpa_port,
 	 */
 
 	if (n->nud_state & NUD_VALID)
-		err = ofdpa_port_ipv4_neigh(ofdpa_port, trans, 0,
+		err = ofdpa_port_ipv4_neigh(ofdpa_port, 0,
 					    ip_addr, n->ha);
 	else
 		neigh_event_send(n, NULL);
@@ -1473,8 +1378,7 @@ static int ofdpa_port_ipv4_resolve(struct ofdpa_port *ofdpa_port,
 }
 
 static int ofdpa_port_ipv4_nh(struct ofdpa_port *ofdpa_port,
-			      struct switchdev_trans *trans, int flags,
-			      __be32 ip_addr, u32 *index)
+			      int flags, __be32 ip_addr, u32 *index)
 {
 	struct ofdpa *ofdpa = ofdpa_port->ofdpa;
 	struct ofdpa_neigh_tbl_entry *entry;
@@ -1486,7 +1390,7 @@ static int ofdpa_port_ipv4_nh(struct ofdpa_port *ofdpa_port,
 	bool resolved = true;
 	int err = 0;
 
-	entry = ofdpa_kzalloc(trans, flags, sizeof(*entry));
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
 	if (!entry)
 		return -ENOMEM;
 
@@ -1501,14 +1405,14 @@ static int ofdpa_port_ipv4_nh(struct ofdpa_port *ofdpa_port,
 	if (adding) {
 		entry->ip_addr = ip_addr;
 		entry->dev = ofdpa_port->dev;
-		ofdpa_neigh_add(ofdpa, trans, entry);
+		ofdpa_neigh_add(ofdpa, entry);
 		*index = entry->index;
 		resolved = false;
 	} else if (removing) {
-		ofdpa_neigh_del(trans, found);
 		*index = found->index;
+		ofdpa_neigh_del(found);
 	} else if (updating) {
-		ofdpa_neigh_update(found, trans, NULL, false);
+		ofdpa_neigh_update(found, NULL, false);
 		resolved = !is_zero_ether_addr(found->eth_dst);
 		*index = found->index;
 	} else {
@@ -1518,7 +1422,7 @@ static int ofdpa_port_ipv4_nh(struct ofdpa_port *ofdpa_port,
 	spin_unlock_irqrestore(&ofdpa->neigh_tbl_lock, lock_flags);
 
 	if (!adding)
-		ofdpa_kfree(trans, entry);
+		kfree(entry);
 
 	if (err)
 		return err;
@@ -1526,7 +1430,7 @@ static int ofdpa_port_ipv4_nh(struct ofdpa_port *ofdpa_port,
 	/* Resolved means neigh ip_addr is resolved to neigh mac. */
 
 	if (!resolved)
-		err = ofdpa_port_ipv4_resolve(ofdpa_port, trans, ip_addr);
+		err = ofdpa_port_ipv4_resolve(ofdpa_port, ip_addr);
 
 	return err;
 }
@@ -1541,7 +1445,6 @@ static struct ofdpa_port *ofdpa_port_get(const struct ofdpa *ofdpa,
 }
 
 static int ofdpa_port_vlan_flood_group(struct ofdpa_port *ofdpa_port,
-				       struct switchdev_trans *trans,
 				       int flags, __be16 vlan_id)
 {
 	struct ofdpa_port *p;
@@ -1553,7 +1456,7 @@ static int ofdpa_port_vlan_flood_group(struct ofdpa_port *ofdpa_port,
 	int err = 0;
 	int i;
 
-	group_ids = ofdpa_kcalloc(trans, flags, port_count, sizeof(u32));
+	group_ids = kcalloc(flags, port_count, sizeof(u32));
 	if (!group_ids)
 		return -ENOMEM;
 
@@ -1578,18 +1481,17 @@ static int ofdpa_port_vlan_flood_group(struct ofdpa_port *ofdpa_port,
 	if (group_count == 0)
 		goto no_ports_in_vlan;
 
-	err = ofdpa_group_l2_flood(ofdpa_port, trans, flags, vlan_id,
+	err = ofdpa_group_l2_flood(ofdpa_port, flags, vlan_id,
 				   group_count, group_ids, group_id);
 	if (err)
 		netdev_err(ofdpa_port->dev, "Error (%d) port VLAN l2 flood group\n", err);
 
 no_ports_in_vlan:
-	ofdpa_kfree(trans, group_ids);
+	kfree(group_ids);
 	return err;
 }
 
-static int ofdpa_port_vlan_l2_groups(struct ofdpa_port *ofdpa_port,
-				     struct switchdev_trans *trans, int flags,
+static int ofdpa_port_vlan_l2_groups(struct ofdpa_port *ofdpa_port, int flags,
 				     __be16 vlan_id, bool pop_vlan)
 {
 	const struct ofdpa *ofdpa = ofdpa_port->ofdpa;
@@ -1608,7 +1510,7 @@ static int ofdpa_port_vlan_l2_groups(struct ofdpa_port *ofdpa_port,
 	if (ofdpa_port->stp_state == BR_STATE_LEARNING ||
 	    ofdpa_port->stp_state == BR_STATE_FORWARDING) {
 		out_pport = ofdpa_port->pport;
-		err = ofdpa_group_l2_interface(ofdpa_port, trans, flags,
+		err = ofdpa_group_l2_interface(ofdpa_port, flags,
 					       vlan_id, out_pport, pop_vlan);
 		if (err) {
 			netdev_err(ofdpa_port->dev, "Error (%d) port VLAN l2 group for pport %d\n",
@@ -1632,7 +1534,7 @@ static int ofdpa_port_vlan_l2_groups(struct ofdpa_port *ofdpa_port,
 		return 0;
 
 	out_pport = 0;
-	err = ofdpa_group_l2_interface(ofdpa_port, trans, flags,
+	err = ofdpa_group_l2_interface(ofdpa_port, flags,
 				       vlan_id, out_pport, pop_vlan);
 	if (err) {
 		netdev_err(ofdpa_port->dev, "Error (%d) port VLAN l2 group for CPU port\n", err);
@@ -1693,8 +1595,7 @@ static struct ofdpa_ctrl {
 	},
 };
 
-static int ofdpa_port_ctrl_vlan_acl(struct ofdpa_port *ofdpa_port,
-				    struct switchdev_trans *trans, int flags,
+static int ofdpa_port_ctrl_vlan_acl(struct ofdpa_port *ofdpa_port, int flags,
 				    const struct ofdpa_ctrl *ctrl, __be16 vlan_id)
 {
 	u32 in_pport = ofdpa_port->pport;
@@ -1710,7 +1611,7 @@ static int ofdpa_port_ctrl_vlan_acl(struct ofdpa_port *ofdpa_port,
 	u32 group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, out_pport);
 	int err;
 
-	err = ofdpa_flow_tbl_acl(ofdpa_port, trans, flags,
+	err = ofdpa_flow_tbl_acl(ofdpa_port, flags,
 				 in_pport, in_pport_mask,
 				 eth_src, eth_src_mask,
 				 ctrl->eth_dst, ctrl->eth_dst_mask,
@@ -1727,9 +1628,7 @@ static int ofdpa_port_ctrl_vlan_acl(struct ofdpa_port *ofdpa_port,
 }
 
 static int ofdpa_port_ctrl_vlan_bridge(struct ofdpa_port *ofdpa_port,
-				       struct switchdev_trans *trans,
-				       int flags,
-				       const struct ofdpa_ctrl *ctrl,
+				       int flags, const struct ofdpa_ctrl *ctrl,
 				       __be16 vlan_id)
 {
 	enum rocker_of_dpa_table_id goto_tbl =
@@ -1741,7 +1640,7 @@ static int ofdpa_port_ctrl_vlan_bridge(struct ofdpa_port *ofdpa_port,
 	if (!ofdpa_port_is_bridged(ofdpa_port))
 		return 0;
 
-	err = ofdpa_flow_tbl_bridge(ofdpa_port, trans, flags,
+	err = ofdpa_flow_tbl_bridge(ofdpa_port, flags,
 				    ctrl->eth_dst, ctrl->eth_dst_mask,
 				    vlan_id, tunnel_id,
 				    goto_tbl, group_id, ctrl->copy_to_cpu);
@@ -1752,8 +1651,7 @@ static int ofdpa_port_ctrl_vlan_bridge(struct ofdpa_port *ofdpa_port,
 	return err;
 }
 
-static int ofdpa_port_ctrl_vlan_term(struct ofdpa_port *ofdpa_port,
-				     struct switchdev_trans *trans, int flags,
+static int ofdpa_port_ctrl_vlan_term(struct ofdpa_port *ofdpa_port, int flags,
 				     const struct ofdpa_ctrl *ctrl, __be16 vlan_id)
 {
 	u32 in_pport_mask = 0xffffffff;
@@ -1763,8 +1661,7 @@ static int ofdpa_port_ctrl_vlan_term(struct ofdpa_port *ofdpa_port,
 	if (ntohs(vlan_id) == 0)
 		vlan_id = ofdpa_port->internal_vlan_id;
 
-	err = ofdpa_flow_tbl_term_mac(ofdpa_port, trans,
-				      ofdpa_port->pport, in_pport_mask,
+	err = ofdpa_flow_tbl_term_mac(ofdpa_port, ofdpa_port->pport, in_pport_mask,
 				      ctrl->eth_type, ctrl->eth_dst,
 				      ctrl->eth_dst_mask, vlan_id,
 				      vlan_id_mask, ctrl->copy_to_cpu,
@@ -1776,26 +1673,24 @@ static int ofdpa_port_ctrl_vlan_term(struct ofdpa_port *ofdpa_port,
 	return err;
 }
 
-static int ofdpa_port_ctrl_vlan(struct ofdpa_port *ofdpa_port,
-				struct switchdev_trans *trans, int flags,
+static int ofdpa_port_ctrl_vlan(struct ofdpa_port *ofdpa_port, int flags,
 				const struct ofdpa_ctrl *ctrl, __be16 vlan_id)
 {
 	if (ctrl->acl)
-		return ofdpa_port_ctrl_vlan_acl(ofdpa_port, trans, flags,
+		return ofdpa_port_ctrl_vlan_acl(ofdpa_port, flags,
 						ctrl, vlan_id);
 	if (ctrl->bridge)
-		return ofdpa_port_ctrl_vlan_bridge(ofdpa_port, trans, flags,
+		return ofdpa_port_ctrl_vlan_bridge(ofdpa_port, flags,
 						   ctrl, vlan_id);
 
 	if (ctrl->term)
-		return ofdpa_port_ctrl_vlan_term(ofdpa_port, trans, flags,
+		return ofdpa_port_ctrl_vlan_term(ofdpa_port, flags,
 						 ctrl, vlan_id);
 
 	return -EOPNOTSUPP;
 }
 
-static int ofdpa_port_ctrl_vlan_add(struct ofdpa_port *ofdpa_port,
-				    struct switchdev_trans *trans, int flags,
+static int ofdpa_port_ctrl_vlan_add(struct ofdpa_port *ofdpa_port, int flags,
 				    __be16 vlan_id)
 {
 	int err = 0;
@@ -1803,7 +1698,7 @@ static int ofdpa_port_ctrl_vlan_add(struct ofdpa_port *ofdpa_port,
 
 	for (i = 0; i < OFDPA_CTRL_MAX; i++) {
 		if (ofdpa_port->ctrls[i]) {
-			err = ofdpa_port_ctrl_vlan(ofdpa_port, trans, flags,
+			err = ofdpa_port_ctrl_vlan(ofdpa_port, flags,
 						   &ofdpa_ctrls[i], vlan_id);
 			if (err)
 				return err;
@@ -1813,8 +1708,7 @@ static int ofdpa_port_ctrl_vlan_add(struct ofdpa_port *ofdpa_port,
 	return err;
 }
 
-static int ofdpa_port_ctrl(struct ofdpa_port *ofdpa_port,
-			   struct switchdev_trans *trans, int flags,
+static int ofdpa_port_ctrl(struct ofdpa_port *ofdpa_port, int flags,
 			   const struct ofdpa_ctrl *ctrl)
 {
 	u16 vid;
@@ -1823,7 +1717,7 @@ static int ofdpa_port_ctrl(struct ofdpa_port *ofdpa_port,
 	for (vid = 1; vid < VLAN_N_VID; vid++) {
 		if (!test_bit(vid, ofdpa_port->vlan_bitmap))
 			continue;
-		err = ofdpa_port_ctrl_vlan(ofdpa_port, trans, flags,
+		err = ofdpa_port_ctrl_vlan(ofdpa_port, flags,
 					   ctrl, htons(vid));
 		if (err)
 			break;
@@ -1832,8 +1726,8 @@ static int ofdpa_port_ctrl(struct ofdpa_port *ofdpa_port,
 	return err;
 }
 
-static int ofdpa_port_vlan(struct ofdpa_port *ofdpa_port,
-			   struct switchdev_trans *trans, int flags, u16 vid)
+static int ofdpa_port_vlan(struct ofdpa_port *ofdpa_port, int flags,
+			   u16 vid)
 {
 	enum rocker_of_dpa_table_id goto_tbl =
 			ROCKER_OF_DPA_TABLE_ID_TERMINATION_MAC;
@@ -1857,43 +1751,44 @@ static int ofdpa_port_vlan(struct ofdpa_port *ofdpa_port,
 	change_bit(ntohs(internal_vlan_id), ofdpa_port->vlan_bitmap);
 
 	if (adding) {
-		err = ofdpa_port_ctrl_vlan_add(ofdpa_port, trans, flags,
+		err = ofdpa_port_ctrl_vlan_add(ofdpa_port, flags,
 					       internal_vlan_id);
 		if (err) {
 			netdev_err(ofdpa_port->dev, "Error (%d) port ctrl vlan add\n", err);
-			goto err_out;
+			goto err_vlan_add;
 		}
 	}
 
-	err = ofdpa_port_vlan_l2_groups(ofdpa_port, trans, flags,
+	err = ofdpa_port_vlan_l2_groups(ofdpa_port, flags,
 					internal_vlan_id, untagged);
 	if (err) {
 		netdev_err(ofdpa_port->dev, "Error (%d) port VLAN l2 groups\n", err);
-		goto err_out;
+		goto err_vlan_l2_groups;
 	}
 
-	err = ofdpa_port_vlan_flood_group(ofdpa_port, trans, flags,
+	err = ofdpa_port_vlan_flood_group(ofdpa_port, flags,
 					  internal_vlan_id);
 	if (err) {
 		netdev_err(ofdpa_port->dev, "Error (%d) port VLAN l2 flood group\n", err);
-		goto err_out;
+		goto err_flood_group;
 	}
 
-	err = ofdpa_flow_tbl_vlan(ofdpa_port, trans, flags,
+	err = ofdpa_flow_tbl_vlan(ofdpa_port, flags,
 				  in_pport, vlan_id, vlan_id_mask,
 				  goto_tbl, untagged, internal_vlan_id);
 	if (err)
 		netdev_err(ofdpa_port->dev, "Error (%d) port VLAN table\n", err);
 
-err_out:
-	if (switchdev_trans_ph_prepare(trans))
-		change_bit(ntohs(internal_vlan_id), ofdpa_port->vlan_bitmap);
+	return 0;
 
+err_vlan_add:
+err_vlan_l2_groups:
+err_flood_group:
+	change_bit(ntohs(internal_vlan_id), ofdpa_port->vlan_bitmap);
 	return err;
 }
 
-static int ofdpa_port_ig_tbl(struct ofdpa_port *ofdpa_port,
-			     struct switchdev_trans *trans, int flags)
+static int ofdpa_port_ig_tbl(struct ofdpa_port *ofdpa_port, int flags)
 {
 	enum rocker_of_dpa_table_id goto_tbl;
 	u32 in_pport;
@@ -1908,7 +1803,7 @@ static int ofdpa_port_ig_tbl(struct ofdpa_port *ofdpa_port,
 	in_pport_mask = 0xffff0000;
 	goto_tbl = ROCKER_OF_DPA_TABLE_ID_VLAN;
 
-	err = ofdpa_flow_tbl_ig_port(ofdpa_port, trans, flags,
+	err = ofdpa_flow_tbl_ig_port(ofdpa_port, flags,
 				     in_pport, in_pport_mask,
 				     goto_tbl);
 	if (err)
@@ -1920,7 +1815,6 @@ static int ofdpa_port_ig_tbl(struct ofdpa_port *ofdpa_port,
 struct ofdpa_fdb_learn_work {
 	struct work_struct work;
 	struct ofdpa_port *ofdpa_port;
-	struct switchdev_trans *trans;
 	int flags;
 	u8 addr[ETH_ALEN];
 	u16 vid;
@@ -1939,19 +1833,18 @@ static void ofdpa_port_fdb_learn_work(struct work_struct *work)
 
 	rtnl_lock();
 	if (learned && removing)
-		call_switchdev_notifiers(SWITCHDEV_FDB_DEL,
+		call_switchdev_notifiers(SWITCHDEV_FDB_DEL_TO_BRIDGE,
 					 lw->ofdpa_port->dev, &info.info);
 	else if (learned && !removing)
-		call_switchdev_notifiers(SWITCHDEV_FDB_ADD,
+		call_switchdev_notifiers(SWITCHDEV_FDB_ADD_TO_BRIDGE,
 					 lw->ofdpa_port->dev, &info.info);
 	rtnl_unlock();
 
-	ofdpa_kfree(lw->trans, work);
+	kfree(work);
 }
 
 static int ofdpa_port_fdb_learn(struct ofdpa_port *ofdpa_port,
-				struct switchdev_trans *trans, int flags,
-				const u8 *addr, __be16 vlan_id)
+				int flags, const u8 *addr, __be16 vlan_id)
 {
 	struct ofdpa_fdb_learn_work *lw;
 	enum rocker_of_dpa_table_id goto_tbl =
@@ -1959,7 +1852,6 @@ static int ofdpa_port_fdb_learn(struct ofdpa_port *ofdpa_port,
 	u32 out_pport = ofdpa_port->pport;
 	u32 tunnel_id = 0;
 	u32 group_id = ROCKER_GROUP_NONE;
-	bool syncing = !!(ofdpa_port->brport_flags & BR_LEARNING_SYNC);
 	bool copy_to_cpu = false;
 	int err;
 
@@ -1967,36 +1859,28 @@ static int ofdpa_port_fdb_learn(struct ofdpa_port *ofdpa_port,
 		group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, out_pport);
 
 	if (!(flags & OFDPA_OP_FLAG_REFRESH)) {
-		err = ofdpa_flow_tbl_bridge(ofdpa_port, trans, flags, addr,
+		err = ofdpa_flow_tbl_bridge(ofdpa_port, flags, addr,
 					    NULL, vlan_id, tunnel_id, goto_tbl,
 					    group_id, copy_to_cpu);
 		if (err)
 			return err;
 	}
 
-	if (!syncing)
-		return 0;
-
 	if (!ofdpa_port_is_bridged(ofdpa_port))
 		return 0;
 
-	lw = ofdpa_kzalloc(trans, flags, sizeof(*lw));
+	lw = kzalloc(sizeof(*lw), GFP_ATOMIC);
 	if (!lw)
 		return -ENOMEM;
 
 	INIT_WORK(&lw->work, ofdpa_port_fdb_learn_work);
 
 	lw->ofdpa_port = ofdpa_port;
-	lw->trans = trans;
 	lw->flags = flags;
 	ether_addr_copy(lw->addr, addr);
 	lw->vid = ofdpa_port_vlan_to_vid(ofdpa_port, vlan_id);
 
-	if (switchdev_trans_ph_prepare(trans))
-		ofdpa_kfree(trans, lw);
-	else
-		schedule_work(&lw->work);
-
+	schedule_work(&lw->work);
 	return 0;
 }
 
@@ -2014,7 +1898,6 @@ ofdpa_fdb_tbl_find(const struct ofdpa *ofdpa,
 }
 
 static int ofdpa_port_fdb(struct ofdpa_port *ofdpa_port,
-			  struct switchdev_trans *trans,
 			  const unsigned char *addr,
 			  __be16 vlan_id, int flags)
 {
@@ -2024,7 +1907,7 @@ static int ofdpa_port_fdb(struct ofdpa_port *ofdpa_port,
 	bool removing = (flags & OFDPA_OP_FLAG_REMOVE);
 	unsigned long lock_flags;
 
-	fdb = ofdpa_kzalloc(trans, flags, sizeof(*fdb));
+	fdb = kzalloc(sizeof(*fdb), GFP_KERNEL);
 	if (!fdb)
 		return -ENOMEM;
 
@@ -2042,32 +1925,29 @@ static int ofdpa_port_fdb(struct ofdpa_port *ofdpa_port,
 	if (found) {
 		found->touched = jiffies;
 		if (removing) {
-			ofdpa_kfree(trans, fdb);
-			if (!switchdev_trans_ph_prepare(trans))
-				hash_del(&found->entry);
+			kfree(fdb);
+			hash_del(&found->entry);
 		}
 	} else if (!removing) {
-		if (!switchdev_trans_ph_prepare(trans))
-			hash_add(ofdpa->fdb_tbl, &fdb->entry,
-				 fdb->key_crc32);
+		hash_add(ofdpa->fdb_tbl, &fdb->entry,
+			 fdb->key_crc32);
 	}
 
 	spin_unlock_irqrestore(&ofdpa->fdb_tbl_lock, lock_flags);
 
 	/* Check if adding and already exists, or removing and can't find */
 	if (!found != !removing) {
-		ofdpa_kfree(trans, fdb);
+		kfree(fdb);
 		if (!found && removing)
 			return 0;
 		/* Refreshing existing to update aging timers */
 		flags |= OFDPA_OP_FLAG_REFRESH;
 	}
 
-	return ofdpa_port_fdb_learn(ofdpa_port, trans, flags, addr, vlan_id);
+	return ofdpa_port_fdb_learn(ofdpa_port, flags, addr, vlan_id);
 }
 
-static int ofdpa_port_fdb_flush(struct ofdpa_port *ofdpa_port,
-				struct switchdev_trans *trans, int flags)
+static int ofdpa_port_fdb_flush(struct ofdpa_port *ofdpa_port, int flags)
 {
 	struct ofdpa *ofdpa = ofdpa_port->ofdpa;
 	struct ofdpa_fdb_tbl_entry *found;
@@ -2089,13 +1969,12 @@ static int ofdpa_port_fdb_flush(struct ofdpa_port *ofdpa_port,
 			continue;
 		if (!found->learned)
 			continue;
-		err = ofdpa_port_fdb_learn(ofdpa_port, trans, flags,
+		err = ofdpa_port_fdb_learn(ofdpa_port, flags,
 					   found->key.addr,
 					   found->key.vlan_id);
 		if (err)
 			goto err_out;
-		if (!switchdev_trans_ph_prepare(trans))
-			hash_del(&found->entry);
+		hash_del(&found->entry);
 	}
 
 err_out:
@@ -2125,8 +2004,8 @@ static void ofdpa_fdb_cleanup(unsigned long data)
 		ofdpa_port = entry->key.ofdpa_port;
 		expires = entry->touched + ofdpa_port->ageing_time;
 		if (time_before_eq(expires, jiffies)) {
-			ofdpa_port_fdb_learn(ofdpa_port, NULL,
-					     flags, entry->key.addr,
+			ofdpa_port_fdb_learn(ofdpa_port, flags,
+					     entry->key.addr,
 					     entry->key.vlan_id);
 			hash_del(&entry->entry);
 		} else if (time_before(expires, next_timer)) {
@@ -2140,8 +2019,7 @@ static void ofdpa_fdb_cleanup(unsigned long data)
 }
 
 static int ofdpa_port_router_mac(struct ofdpa_port *ofdpa_port,
-				 struct switchdev_trans *trans, int flags,
-				 __be16 vlan_id)
+				 int flags, __be16 vlan_id)
 {
 	u32 in_pport_mask = 0xffffffff;
 	__be16 eth_type;
@@ -2154,26 +2032,25 @@ static int ofdpa_port_router_mac(struct ofdpa_port *ofdpa_port,
 		vlan_id = ofdpa_port->internal_vlan_id;
 
 	eth_type = htons(ETH_P_IP);
-	err = ofdpa_flow_tbl_term_mac(ofdpa_port, trans,
-				      ofdpa_port->pport, in_pport_mask,
-				      eth_type, ofdpa_port->dev->dev_addr,
+	err = ofdpa_flow_tbl_term_mac(ofdpa_port, ofdpa_port->pport,
+				      in_pport_mask, eth_type,
+				      ofdpa_port->dev->dev_addr,
 				      dst_mac_mask, vlan_id, vlan_id_mask,
 				      copy_to_cpu, flags);
 	if (err)
 		return err;
 
 	eth_type = htons(ETH_P_IPV6);
-	err = ofdpa_flow_tbl_term_mac(ofdpa_port, trans,
-				      ofdpa_port->pport, in_pport_mask,
-				      eth_type, ofdpa_port->dev->dev_addr,
+	err = ofdpa_flow_tbl_term_mac(ofdpa_port, ofdpa_port->pport,
+				      in_pport_mask, eth_type,
+				      ofdpa_port->dev->dev_addr,
 				      dst_mac_mask, vlan_id, vlan_id_mask,
 				      copy_to_cpu, flags);
 
 	return err;
 }
 
-static int ofdpa_port_fwding(struct ofdpa_port *ofdpa_port,
-			     struct switchdev_trans *trans, int flags)
+static int ofdpa_port_fwding(struct ofdpa_port *ofdpa_port, int flags)
 {
 	bool pop_vlan;
 	u32 out_pport;
@@ -2198,7 +2075,7 @@ static int ofdpa_port_fwding(struct ofdpa_port *ofdpa_port,
 			continue;
 		vlan_id = htons(vid);
 		pop_vlan = ofdpa_vlan_id_is_internal(vlan_id);
-		err = ofdpa_group_l2_interface(ofdpa_port, trans, flags,
+		err = ofdpa_group_l2_interface(ofdpa_port, flags,
 					       vlan_id, out_pport, pop_vlan);
 		if (err) {
 			netdev_err(ofdpa_port->dev, "Error (%d) port VLAN l2 group for pport %d\n",
@@ -2211,7 +2088,6 @@ static int ofdpa_port_fwding(struct ofdpa_port *ofdpa_port,
 }
 
 static int ofdpa_port_stp_update(struct ofdpa_port *ofdpa_port,
-				 struct switchdev_trans *trans,
 				 int flags, u8 state)
 {
 	bool want[OFDPA_CTRL_MAX] = { 0, };
@@ -2220,11 +2096,12 @@ static int ofdpa_port_stp_update(struct ofdpa_port *ofdpa_port,
 	int err;
 	int i;
 
+	memcpy(prev_ctrls, ofdpa_port->ctrls, sizeof(prev_ctrls));
 	prev_state = ofdpa_port->stp_state;
-	if (prev_state == state)
+
+	if (ofdpa_port->stp_state == state)
 		return 0;
 
-	memcpy(prev_ctrls, ofdpa_port->ctrls, sizeof(prev_ctrls));
 	ofdpa_port->stp_state = state;
 
 	switch (state) {
@@ -2254,26 +2131,29 @@ static int ofdpa_port_stp_update(struct ofdpa_port *ofdpa_port,
 		if (want[i] != ofdpa_port->ctrls[i]) {
 			int ctrl_flags = flags |
 					 (want[i] ? 0 : OFDPA_OP_FLAG_REMOVE);
-			err = ofdpa_port_ctrl(ofdpa_port, trans, ctrl_flags,
+			err = ofdpa_port_ctrl(ofdpa_port, ctrl_flags,
 					      &ofdpa_ctrls[i]);
 			if (err)
-				goto err_out;
+				goto err_port_ctrl;
 			ofdpa_port->ctrls[i] = want[i];
 		}
 	}
 
-	err = ofdpa_port_fdb_flush(ofdpa_port, trans, flags);
+	err = ofdpa_port_fdb_flush(ofdpa_port, flags);
 	if (err)
-		goto err_out;
+		goto err_fdb_flush;
 
-	err = ofdpa_port_fwding(ofdpa_port, trans, flags);
+	err = ofdpa_port_fwding(ofdpa_port, flags);
+	if (err)
+		goto err_port_fwding;
 
-err_out:
-	if (switchdev_trans_ph_prepare(trans)) {
-		memcpy(ofdpa_port->ctrls, prev_ctrls, sizeof(prev_ctrls));
-		ofdpa_port->stp_state = prev_state;
-	}
+	return 0;
 
+err_port_ctrl:
+err_fdb_flush:
+err_port_fwding:
+	memcpy(ofdpa_port->ctrls, prev_ctrls, sizeof(prev_ctrls));
+	ofdpa_port->stp_state = prev_state;
 	return err;
 }
 
@@ -2284,7 +2164,7 @@ static int ofdpa_port_fwd_enable(struct ofdpa_port *ofdpa_port, int flags)
 		return 0;
 
 	/* port is not bridged, so simulate going to FORWARDING state */
-	return ofdpa_port_stp_update(ofdpa_port, NULL, flags,
+	return ofdpa_port_stp_update(ofdpa_port, flags,
 				     BR_STATE_FORWARDING);
 }
 
@@ -2295,25 +2175,24 @@ static int ofdpa_port_fwd_disable(struct ofdpa_port *ofdpa_port, int flags)
 		return 0;
 
 	/* port is not bridged, so simulate going to DISABLED state */
-	return ofdpa_port_stp_update(ofdpa_port, NULL, flags,
+	return ofdpa_port_stp_update(ofdpa_port, flags,
 				     BR_STATE_DISABLED);
 }
 
 static int ofdpa_port_vlan_add(struct ofdpa_port *ofdpa_port,
-			       struct switchdev_trans *trans,
 			       u16 vid, u16 flags)
 {
 	int err;
 
 	/* XXX deal with flags for PVID and untagged */
 
-	err = ofdpa_port_vlan(ofdpa_port, trans, 0, vid);
+	err = ofdpa_port_vlan(ofdpa_port, 0, vid);
 	if (err)
 		return err;
 
-	err = ofdpa_port_router_mac(ofdpa_port, trans, 0, htons(vid));
+	err = ofdpa_port_router_mac(ofdpa_port, 0, htons(vid));
 	if (err)
-		ofdpa_port_vlan(ofdpa_port, trans,
+		ofdpa_port_vlan(ofdpa_port,
 				OFDPA_OP_FLAG_REMOVE, vid);
 
 	return err;
@@ -2324,13 +2203,13 @@ static int ofdpa_port_vlan_del(struct ofdpa_port *ofdpa_port,
 {
 	int err;
 
-	err = ofdpa_port_router_mac(ofdpa_port, NULL,
-				    OFDPA_OP_FLAG_REMOVE, htons(vid));
+	err = ofdpa_port_router_mac(ofdpa_port, OFDPA_OP_FLAG_REMOVE,
+				    htons(vid));
 	if (err)
 		return err;
 
-	return ofdpa_port_vlan(ofdpa_port, NULL,
-			       OFDPA_OP_FLAG_REMOVE, vid);
+	return ofdpa_port_vlan(ofdpa_port, OFDPA_OP_FLAG_REMOVE,
+			       vid);
 }
 
 static struct ofdpa_internal_vlan_tbl_entry *
@@ -2389,10 +2268,9 @@ found:
 	return found->vlan_id;
 }
 
-static int ofdpa_port_fib_ipv4(struct ofdpa_port *ofdpa_port,
-			       struct switchdev_trans *trans, __be32 dst,
-			       int dst_len, struct fib_info *fi,
-			       u32 tb_id, int flags)
+static int ofdpa_port_fib_ipv4(struct ofdpa_port *ofdpa_port,  __be32 dst,
+			       int dst_len, struct fib_info *fi, u32 tb_id,
+			       int flags)
 {
 	const struct fib_nh *nh;
 	__be16 eth_type = htons(ETH_P_IP);
@@ -2414,7 +2292,7 @@ static int ofdpa_port_fib_ipv4(struct ofdpa_port *ofdpa_port,
 	has_gw = !!nh->nh_gw;
 
 	if (has_gw && nh_on_port) {
-		err = ofdpa_port_ipv4_nh(ofdpa_port, trans, flags,
+		err = ofdpa_port_ipv4_nh(ofdpa_port, flags,
 					 nh->nh_gw, &index);
 		if (err)
 			return err;
@@ -2425,7 +2303,7 @@ static int ofdpa_port_fib_ipv4(struct ofdpa_port *ofdpa_port,
 		group_id = ROCKER_GROUP_L2_INTERFACE(internal_vlan_id, 0);
 	}
 
-	err = ofdpa_flow_tbl_ucast4_routing(ofdpa_port, trans, eth_type, dst,
+	err = ofdpa_flow_tbl_ucast4_routing(ofdpa_port, eth_type, dst,
 					    dst_mask, priority, goto_tbl,
 					    group_id, fi, flags);
 	if (err)
@@ -2550,7 +2428,7 @@ static int ofdpa_port_pre_init(struct rocker_port *rocker_port)
 	ofdpa_port->rocker_port = rocker_port;
 	ofdpa_port->dev = rocker_port->dev;
 	ofdpa_port->pport = rocker_port->pport;
-	ofdpa_port->brport_flags = BR_LEARNING | BR_LEARNING_SYNC;
+	ofdpa_port->brport_flags = BR_LEARNING;
 	ofdpa_port->ageing_time = BR_DEFAULT_AGEING_TIME;
 	return 0;
 }
@@ -2563,7 +2441,7 @@ static int ofdpa_port_init(struct rocker_port *rocker_port)
 	rocker_port_set_learning(rocker_port,
 				 !!(ofdpa_port->brport_flags & BR_LEARNING));
 
-	err = ofdpa_port_ig_tbl(ofdpa_port, NULL, 0);
+	err = ofdpa_port_ig_tbl(ofdpa_port, 0);
 	if (err) {
 		netdev_err(ofdpa_port->dev, "install ig port table failed\n");
 		return err;
@@ -2573,7 +2451,7 @@ static int ofdpa_port_init(struct rocker_port *rocker_port)
 		ofdpa_port_internal_vlan_id_get(ofdpa_port,
 						ofdpa_port->dev->ifindex);
 
-	err = ofdpa_port_vlan_add(ofdpa_port, NULL, OFDPA_UNTAGGED_VID, 0);
+	err = ofdpa_port_vlan_add(ofdpa_port, OFDPA_UNTAGGED_VID, 0);
 	if (err) {
 		netdev_err(ofdpa_port->dev, "install untagged VLAN failed\n");
 		goto err_untagged_vlan;
@@ -2581,7 +2459,7 @@ static int ofdpa_port_init(struct rocker_port *rocker_port)
 	return 0;
 
 err_untagged_vlan:
-	ofdpa_port_ig_tbl(ofdpa_port, NULL, OFDPA_OP_FLAG_REMOVE);
+	ofdpa_port_ig_tbl(ofdpa_port, OFDPA_OP_FLAG_REMOVE);
 	return err;
 }
 
@@ -2589,7 +2467,7 @@ static void ofdpa_port_fini(struct rocker_port *rocker_port)
 {
 	struct ofdpa_port *ofdpa_port = rocker_port->wpriv;
 
-	ofdpa_port_ig_tbl(ofdpa_port, NULL, OFDPA_OP_FLAG_REMOVE);
+	ofdpa_port_ig_tbl(ofdpa_port, OFDPA_OP_FLAG_REMOVE);
 }
 
 static int ofdpa_port_open(struct rocker_port *rocker_port)
@@ -2607,12 +2485,11 @@ static void ofdpa_port_stop(struct rocker_port *rocker_port)
 }
 
 static int ofdpa_port_attr_stp_state_set(struct rocker_port *rocker_port,
-					 u8 state,
-					 struct switchdev_trans *trans)
+					 u8 state)
 {
 	struct ofdpa_port *ofdpa_port = rocker_port->wpriv;
 
-	return ofdpa_port_stp_update(ofdpa_port, trans, 0, state);
+	return ofdpa_port_stp_update(ofdpa_port, 0, state);
 }
 
 static int ofdpa_port_attr_bridge_flags_set(struct rocker_port *rocker_port,
@@ -2647,6 +2524,16 @@ ofdpa_port_attr_bridge_flags_get(const struct rocker_port *rocker_port,
 }
 
 static int
+ofdpa_port_attr_bridge_flags_support_get(const struct rocker_port *
+					 rocker_port,
+					 unsigned long *
+					 p_brport_flags_support)
+{
+	*p_brport_flags_support = BR_LEARNING;
+	return 0;
+}
+
+static int
 ofdpa_port_attr_bridge_ageing_time_set(struct rocker_port *rocker_port,
 				       u32 ageing_time,
 				       struct switchdev_trans *trans)
@@ -2665,15 +2552,14 @@ ofdpa_port_attr_bridge_ageing_time_set(struct rocker_port *rocker_port,
 }
 
 static int ofdpa_port_obj_vlan_add(struct rocker_port *rocker_port,
-				   const struct switchdev_obj_port_vlan *vlan,
-				   struct switchdev_trans *trans)
+				   const struct switchdev_obj_port_vlan *vlan)
 {
 	struct ofdpa_port *ofdpa_port = rocker_port->wpriv;
 	u16 vid;
 	int err;
 
 	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
-		err = ofdpa_port_vlan_add(ofdpa_port, trans, vid, vlan->flags);
+		err = ofdpa_port_vlan_add(ofdpa_port, vid, vlan->flags);
 		if (err)
 			return err;
 	}
@@ -2697,82 +2583,29 @@ static int ofdpa_port_obj_vlan_del(struct rocker_port *rocker_port,
 	return 0;
 }
 
-static int ofdpa_port_obj_vlan_dump(const struct rocker_port *rocker_port,
-				    struct switchdev_obj_port_vlan *vlan,
-				    switchdev_obj_dump_cb_t *cb)
-{
-	const struct ofdpa_port *ofdpa_port = rocker_port->wpriv;
-	u16 vid;
-	int err = 0;
-
-	for (vid = 1; vid < VLAN_N_VID; vid++) {
-		if (!test_bit(vid, ofdpa_port->vlan_bitmap))
-			continue;
-		vlan->flags = 0;
-		if (ofdpa_vlan_id_is_internal(htons(vid)))
-			vlan->flags |= BRIDGE_VLAN_INFO_PVID;
-		vlan->vid_begin = vlan->vid_end = vid;
-		err = cb(&vlan->obj);
-		if (err)
-			break;
-	}
-
-	return err;
-}
-
 static int ofdpa_port_obj_fdb_add(struct rocker_port *rocker_port,
-				  const struct switchdev_obj_port_fdb *fdb,
-				  struct switchdev_trans *trans)
+				  u16 vid, const unsigned char *addr)
 {
 	struct ofdpa_port *ofdpa_port = rocker_port->wpriv;
-	__be16 vlan_id = ofdpa_port_vid_to_vlan(ofdpa_port, fdb->vid, NULL);
+	__be16 vlan_id = ofdpa_port_vid_to_vlan(ofdpa_port, vid, NULL);
 
 	if (!ofdpa_port_is_bridged(ofdpa_port))
 		return -EINVAL;
 
-	return ofdpa_port_fdb(ofdpa_port, trans, fdb->addr, vlan_id, 0);
+	return ofdpa_port_fdb(ofdpa_port, addr, vlan_id, 0);
 }
 
 static int ofdpa_port_obj_fdb_del(struct rocker_port *rocker_port,
-				  const struct switchdev_obj_port_fdb *fdb)
+				  u16 vid, const unsigned char *addr)
 {
 	struct ofdpa_port *ofdpa_port = rocker_port->wpriv;
-	__be16 vlan_id = ofdpa_port_vid_to_vlan(ofdpa_port, fdb->vid, NULL);
+	__be16 vlan_id = ofdpa_port_vid_to_vlan(ofdpa_port, vid, NULL);
 	int flags = OFDPA_OP_FLAG_REMOVE;
 
 	if (!ofdpa_port_is_bridged(ofdpa_port))
 		return -EINVAL;
 
-	return ofdpa_port_fdb(ofdpa_port, NULL, fdb->addr, vlan_id, flags);
-}
-
-static int ofdpa_port_obj_fdb_dump(const struct rocker_port *rocker_port,
-				   struct switchdev_obj_port_fdb *fdb,
-				   switchdev_obj_dump_cb_t *cb)
-{
-	const struct ofdpa_port *ofdpa_port = rocker_port->wpriv;
-	struct ofdpa *ofdpa = ofdpa_port->ofdpa;
-	struct ofdpa_fdb_tbl_entry *found;
-	struct hlist_node *tmp;
-	unsigned long lock_flags;
-	int bkt;
-	int err = 0;
-
-	spin_lock_irqsave(&ofdpa->fdb_tbl_lock, lock_flags);
-	hash_for_each_safe(ofdpa->fdb_tbl, bkt, tmp, found, entry) {
-		if (found->key.ofdpa_port != ofdpa_port)
-			continue;
-		ether_addr_copy(fdb->addr, found->key.addr);
-		fdb->ndm_state = NUD_REACHABLE;
-		fdb->vid = ofdpa_port_vlan_to_vid(ofdpa_port,
-						  found->key.vlan_id);
-		err = cb(&fdb->obj);
-		if (err)
-			break;
-	}
-	spin_unlock_irqrestore(&ofdpa->fdb_tbl_lock, lock_flags);
-
-	return err;
+	return ofdpa_port_fdb(ofdpa_port, addr, vlan_id, flags);
 }
 
 static int ofdpa_port_bridge_join(struct ofdpa_port *ofdpa_port,
@@ -2797,7 +2630,7 @@ static int ofdpa_port_bridge_join(struct ofdpa_port *ofdpa_port,
 
 	ofdpa_port->bridge_dev = bridge;
 
-	return ofdpa_port_vlan_add(ofdpa_port, NULL, OFDPA_UNTAGGED_VID, 0);
+	return ofdpa_port_vlan_add(ofdpa_port, OFDPA_UNTAGGED_VID, 0);
 }
 
 static int ofdpa_port_bridge_leave(struct ofdpa_port *ofdpa_port)
@@ -2816,7 +2649,7 @@ static int ofdpa_port_bridge_leave(struct ofdpa_port *ofdpa_port)
 
 	ofdpa_port->bridge_dev = NULL;
 
-	err = ofdpa_port_vlan_add(ofdpa_port, NULL, OFDPA_UNTAGGED_VID, 0);
+	err = ofdpa_port_vlan_add(ofdpa_port, OFDPA_UNTAGGED_VID, 0);
 	if (err)
 		return err;
 
@@ -2875,7 +2708,7 @@ static int ofdpa_port_neigh_update(struct rocker_port *rocker_port,
 						    OFDPA_OP_FLAG_NOWAIT;
 	__be32 ip_addr = *(__be32 *) n->primary_key;
 
-	return ofdpa_port_ipv4_neigh(ofdpa_port, NULL, flags, ip_addr, n->ha);
+	return ofdpa_port_ipv4_neigh(ofdpa_port, flags, ip_addr, n->ha);
 }
 
 static int ofdpa_port_neigh_destroy(struct rocker_port *rocker_port,
@@ -2885,7 +2718,7 @@ static int ofdpa_port_neigh_destroy(struct rocker_port *rocker_port,
 	int flags = OFDPA_OP_FLAG_REMOVE | OFDPA_OP_FLAG_NOWAIT;
 	__be32 ip_addr = *(__be32 *) n->primary_key;
 
-	return ofdpa_port_ipv4_neigh(ofdpa_port, NULL, flags, ip_addr, n->ha);
+	return ofdpa_port_ipv4_neigh(ofdpa_port, flags, ip_addr, n->ha);
 }
 
 static int ofdpa_port_ev_mac_vlan_seen(struct rocker_port *rocker_port,
@@ -2899,7 +2732,7 @@ static int ofdpa_port_ev_mac_vlan_seen(struct rocker_port *rocker_port,
 	    ofdpa_port->stp_state != BR_STATE_FORWARDING)
 		return 0;
 
-	return ofdpa_port_fdb(ofdpa_port, NULL, addr, vlan_id, flags);
+	return ofdpa_port_fdb(ofdpa_port, addr, vlan_id, flags);
 }
 
 static struct ofdpa_port *ofdpa_port_dev_lower_find(struct net_device *dev,
@@ -2923,7 +2756,7 @@ static int ofdpa_fib4_add(struct rocker *rocker,
 	ofdpa_port = ofdpa_port_dev_lower_find(fen_info->fi->fib_dev, rocker);
 	if (!ofdpa_port)
 		return 0;
-	err = ofdpa_port_fib_ipv4(ofdpa_port, NULL, htonl(fen_info->dst),
+	err = ofdpa_port_fib_ipv4(ofdpa_port, htonl(fen_info->dst),
 				  fen_info->dst_len, fen_info->fi,
 				  fen_info->tb_id, 0);
 	if (err)
@@ -2944,7 +2777,7 @@ static int ofdpa_fib4_del(struct rocker *rocker,
 	if (!ofdpa_port)
 		return 0;
 	fib_info_offload_dec(fen_info->fi);
-	return ofdpa_port_fib_ipv4(ofdpa_port, NULL, htonl(fen_info->dst),
+	return ofdpa_port_fib_ipv4(ofdpa_port, htonl(fen_info->dst),
 				   fen_info->dst_len, fen_info->fi,
 				   fen_info->tb_id, OFDPA_OP_FLAG_REMOVE);
 }
@@ -2971,7 +2804,7 @@ static void ofdpa_fib4_abort(struct rocker *rocker)
 		if (!ofdpa_port)
 			continue;
 		fib_info_offload_dec(flow_entry->fi);
-		ofdpa_flow_tbl_del(ofdpa_port, NULL, OFDPA_OP_FLAG_REMOVE,
+		ofdpa_flow_tbl_del(ofdpa_port, OFDPA_OP_FLAG_REMOVE,
 				   flow_entry);
 	}
 	spin_unlock_irqrestore(&ofdpa->flow_tbl_lock, flags);
@@ -2993,13 +2826,12 @@ struct rocker_world_ops rocker_ofdpa_ops = {
 	.port_attr_stp_state_set = ofdpa_port_attr_stp_state_set,
 	.port_attr_bridge_flags_set = ofdpa_port_attr_bridge_flags_set,
 	.port_attr_bridge_flags_get = ofdpa_port_attr_bridge_flags_get,
+	.port_attr_bridge_flags_support_get = ofdpa_port_attr_bridge_flags_support_get,
 	.port_attr_bridge_ageing_time_set = ofdpa_port_attr_bridge_ageing_time_set,
 	.port_obj_vlan_add = ofdpa_port_obj_vlan_add,
 	.port_obj_vlan_del = ofdpa_port_obj_vlan_del,
-	.port_obj_vlan_dump = ofdpa_port_obj_vlan_dump,
 	.port_obj_fdb_add = ofdpa_port_obj_fdb_add,
 	.port_obj_fdb_del = ofdpa_port_obj_fdb_del,
-	.port_obj_fdb_dump = ofdpa_port_obj_fdb_dump,
 	.port_master_linked = ofdpa_port_master_linked,
 	.port_master_unlinked = ofdpa_port_master_unlinked,
 	.port_neigh_update = ofdpa_port_neigh_update,
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c b/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
index 1e594351a60f..89831adb8eb7 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
@@ -1418,8 +1418,7 @@ static netdev_tx_t sxgbe_xmit(struct sk_buff *skb, struct net_device *dev)
 		priv->hw->desc->tx_enable_tstamp(first_desc);
 	}
 
-	if (!tqueue->hwts_tx_en)
-		skb_tx_timestamp(skb);
+	skb_tx_timestamp(skb);
 
 	priv->hw->dma->enable_dma_transmission(priv->ioaddr, txq_index);
 
diff --git a/drivers/net/ethernet/sfc/ef10.c b/drivers/net/ethernet/sfc/ef10.c
index 78efb2822b86..761c518b2f92 100644
--- a/drivers/net/ethernet/sfc/ef10.c
+++ b/drivers/net/ethernet/sfc/ef10.c
@@ -4172,7 +4172,7 @@ found:
 	 * recipients
 	 */
 	if (is_mc_recip) {
-		MCDI_DECLARE_BUF(inbuf, MC_CMD_FILTER_OP_IN_LEN);
+		MCDI_DECLARE_BUF(inbuf, MC_CMD_FILTER_OP_EXT_IN_LEN);
 		unsigned int depth, i;
 
 		memset(inbuf, 0, sizeof(inbuf));
@@ -4320,7 +4320,7 @@ static int efx_ef10_filter_remove_internal(struct efx_nic *efx,
 			efx_ef10_filter_set_entry(table, filter_idx, NULL, 0);
 		} else {
 			efx_mcdi_display_error(efx, MC_CMD_FILTER_OP,
-					       MC_CMD_FILTER_OP_IN_LEN,
+					       MC_CMD_FILTER_OP_EXT_IN_LEN,
 					       NULL, 0, rc);
 		}
 	}
@@ -4453,7 +4453,7 @@ static s32 efx_ef10_filter_rfs_insert(struct efx_nic *efx,
 				      struct efx_filter_spec *spec)
 {
 	struct efx_ef10_filter_table *table = efx->filter_state;
-	MCDI_DECLARE_BUF(inbuf, MC_CMD_FILTER_OP_IN_LEN);
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_FILTER_OP_EXT_IN_LEN);
 	struct efx_filter_spec *saved_spec;
 	unsigned int hash, i, depth = 1;
 	bool replacing = false;
@@ -4940,7 +4940,7 @@ not_restored:
 static void efx_ef10_filter_table_remove(struct efx_nic *efx)
 {
 	struct efx_ef10_filter_table *table = efx->filter_state;
-	MCDI_DECLARE_BUF(inbuf, MC_CMD_FILTER_OP_IN_LEN);
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_FILTER_OP_EXT_IN_LEN);
 	struct efx_filter_spec *spec;
 	unsigned int filter_idx;
 	int rc;
@@ -5105,6 +5105,7 @@ static int efx_ef10_filter_insert_addr_list(struct efx_nic *efx,
 
 	/* Insert/renew filters */
 	for (i = 0; i < addr_count; i++) {
+		EFX_WARN_ON_PARANOID(ids[i] != EFX_EF10_FILTER_ID_INVALID);
 		efx_filter_init_rx(&spec, EFX_FILTER_PRI_AUTO, filter_flags, 0);
 		efx_filter_set_eth_local(&spec, vlan->vid, addr_list[i].addr);
 		rc = efx_ef10_filter_insert(efx, &spec, true);
@@ -5122,11 +5123,11 @@ static int efx_ef10_filter_insert_addr_list(struct efx_nic *efx,
 				}
 				return rc;
 			} else {
-				/* mark as not inserted, and carry on */
-				rc = EFX_EF10_FILTER_ID_INVALID;
+				/* keep invalid ID, and carry on */
 			}
+		} else {
+			ids[i] = efx_ef10_filter_get_unsafe_id(rc);
 		}
-		ids[i] = efx_ef10_filter_get_unsafe_id(rc);
 	}
 
 	if (multicast && rollback) {
@@ -6068,6 +6069,7 @@ static int efx_ef10_ptp_set_ts_config(struct efx_nic *efx,
 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+	case HWTSTAMP_FILTER_NTP_ALL:
 		init->rx_filter = HWTSTAMP_FILTER_ALL;
 		rc = efx_ptp_change_mode(efx, true, 0);
 		if (!rc)
diff --git a/drivers/net/ethernet/sfc/ef10_sriov.c b/drivers/net/ethernet/sfc/ef10_sriov.c
index b7e4345c990d..019cef1d3cf7 100644
--- a/drivers/net/ethernet/sfc/ef10_sriov.c
+++ b/drivers/net/ethernet/sfc/ef10_sriov.c
@@ -661,8 +661,6 @@ restore_filters:
 		up_write(&vf->efx->filter_sem);
 		mutex_unlock(&vf->efx->mac_lock);
 
-		up_write(&vf->efx->filter_sem);
-
 		rc2 = efx_net_open(vf->efx->net_dev);
 		if (rc2)
 			goto reset_nic;
diff --git a/drivers/net/ethernet/sfc/efx.h b/drivers/net/ethernet/sfc/efx.h
index a0c52e328102..fcea9371ab7f 100644
--- a/drivers/net/ethernet/sfc/efx.h
+++ b/drivers/net/ethernet/sfc/efx.h
@@ -32,8 +32,8 @@ netdev_tx_t efx_hard_start_xmit(struct sk_buff *skb,
 				struct net_device *net_dev);
 netdev_tx_t efx_enqueue_skb(struct efx_tx_queue *tx_queue, struct sk_buff *skb);
 void efx_xmit_done(struct efx_tx_queue *tx_queue, unsigned int index);
-int efx_setup_tc(struct net_device *net_dev, u32 handle, __be16 proto,
-		 struct tc_to_netdev *tc);
+int efx_setup_tc(struct net_device *net_dev, u32 handle, u32 chain_index,
+		 __be16 proto, struct tc_to_netdev *tc);
 unsigned int efx_tx_max_skb_descs(struct efx_nic *efx);
 extern unsigned int efx_piobuf_size;
 extern bool efx_separate_tx_channels;
diff --git a/drivers/net/ethernet/sfc/falcon/efx.h b/drivers/net/ethernet/sfc/falcon/efx.h
index c89456fa148c..e5a7a40cc8b6 100644
--- a/drivers/net/ethernet/sfc/falcon/efx.h
+++ b/drivers/net/ethernet/sfc/falcon/efx.h
@@ -32,8 +32,8 @@ netdev_tx_t ef4_hard_start_xmit(struct sk_buff *skb,
 				struct net_device *net_dev);
 netdev_tx_t ef4_enqueue_skb(struct ef4_tx_queue *tx_queue, struct sk_buff *skb);
 void ef4_xmit_done(struct ef4_tx_queue *tx_queue, unsigned int index);
-int ef4_setup_tc(struct net_device *net_dev, u32 handle, __be16 proto,
-		 struct tc_to_netdev *tc);
+int ef4_setup_tc(struct net_device *net_dev, u32 handle, u32 chain_index,
+		 __be16 proto, struct tc_to_netdev *tc);
 unsigned int ef4_tx_max_skb_descs(struct ef4_nic *efx);
 extern bool ef4_separate_tx_channels;
 
diff --git a/drivers/net/ethernet/sfc/falcon/selftest.c b/drivers/net/ethernet/sfc/falcon/selftest.c
index 92bc34c91547..55c0fbbc4fb8 100644
--- a/drivers/net/ethernet/sfc/falcon/selftest.c
+++ b/drivers/net/ethernet/sfc/falcon/selftest.c
@@ -431,8 +431,7 @@ static int ef4_begin_loopback(struct ef4_tx_queue *tx_queue)
 
 		/* Copy the payload in, incrementing the source address to
 		 * exercise the rss vectors */
-		payload = ((struct ef4_loopback_payload *)
-			   skb_put(skb, sizeof(state->payload)));
+		payload = skb_put(skb, sizeof(state->payload));
 		memcpy(payload, &state->payload, sizeof(state->payload));
 		payload->ip.saddr = htonl(INADDR_LOOPBACK | (i << 2));
 
diff --git a/drivers/net/ethernet/sfc/falcon/tx.c b/drivers/net/ethernet/sfc/falcon/tx.c
index f6daf09b8627..f1520a404ac6 100644
--- a/drivers/net/ethernet/sfc/falcon/tx.c
+++ b/drivers/net/ethernet/sfc/falcon/tx.c
@@ -425,8 +425,8 @@ void ef4_init_tx_queue_core_txq(struct ef4_tx_queue *tx_queue)
 				     efx->n_tx_channels : 0));
 }
 
-int ef4_setup_tc(struct net_device *net_dev, u32 handle, __be16 proto,
-		 struct tc_to_netdev *ntc)
+int ef4_setup_tc(struct net_device *net_dev, u32 handle, u32 chain_index,
+		 __be16 proto, struct tc_to_netdev *ntc)
 {
 	struct ef4_nic *efx = netdev_priv(net_dev);
 	struct ef4_channel *channel;
diff --git a/drivers/net/ethernet/sfc/mcdi.c b/drivers/net/ethernet/sfc/mcdi.c
index b9422450deb8..3df872f56289 100644
--- a/drivers/net/ethernet/sfc/mcdi.c
+++ b/drivers/net/ethernet/sfc/mcdi.c
@@ -1301,7 +1301,7 @@ static void efx_mcdi_abandon(struct efx_nic *efx)
 	efx_schedule_reset(efx, RESET_TYPE_MCDI_TIMEOUT);
 }
 
-/* Called from  falcon_process_eventq for MCDI events */
+/* Called from efx_farch_ev_process and efx_ef10_ev_process for MCDI events */
 void efx_mcdi_process_event(struct efx_channel *channel,
 			    efx_qword_t *event)
 {
@@ -1389,8 +1389,9 @@ void efx_mcdi_process_event(struct efx_channel *channel,
 				MCDI_EVENT_FIELD(*event, PROXY_RESPONSE_RC));
 		break;
 	default:
-		netif_err(efx, hw, efx->net_dev, "Unknown MCDI event 0x%x\n",
-			  code);
+		netif_err(efx, hw, efx->net_dev,
+			  "Unknown MCDI event " EFX_QWORD_FMT "\n",
+			  EFX_QWORD_VAL(*event));
 	}
 }
 
diff --git a/drivers/net/ethernet/sfc/selftest.c b/drivers/net/ethernet/sfc/selftest.c
index dab286a337a6..f6936949fc85 100644
--- a/drivers/net/ethernet/sfc/selftest.c
+++ b/drivers/net/ethernet/sfc/selftest.c
@@ -431,8 +431,7 @@ static int efx_begin_loopback(struct efx_tx_queue *tx_queue)
 
 		/* Copy the payload in, incrementing the source address to
 		 * exercise the rss vectors */
-		payload = ((struct efx_loopback_payload *)
-			   skb_put(skb, sizeof(state->payload)));
+		payload = skb_put(skb, sizeof(state->payload));
 		memcpy(payload, &state->payload, sizeof(state->payload));
 		payload->ip.saddr = htonl(INADDR_LOOPBACK | (i << 2));
 
diff --git a/drivers/net/ethernet/sfc/tx.c b/drivers/net/ethernet/sfc/tx.c
index 3bdf87f31087..02d41eb4a8e9 100644
--- a/drivers/net/ethernet/sfc/tx.c
+++ b/drivers/net/ethernet/sfc/tx.c
@@ -653,8 +653,8 @@ void efx_init_tx_queue_core_txq(struct efx_tx_queue *tx_queue)
 				     efx->n_tx_channels : 0));
 }
 
-int efx_setup_tc(struct net_device *net_dev, u32 handle, __be16 proto,
-		 struct tc_to_netdev *ntc)
+int efx_setup_tc(struct net_device *net_dev, u32 handle, u32 chain_index,
+		 __be16 proto, struct tc_to_netdev *ntc)
 {
 	struct efx_nic *efx = netdev_priv(net_dev);
 	struct efx_channel *channel;
diff --git a/drivers/net/ethernet/sgi/ioc3-eth.c b/drivers/net/ethernet/sgi/ioc3-eth.c
index 52ead5524de7..b607936e1b3e 100644
--- a/drivers/net/ethernet/sgi/ioc3-eth.c
+++ b/drivers/net/ethernet/sgi/ioc3-eth.c
@@ -1562,13 +1562,12 @@ static int ioc3_get_link_ksettings(struct net_device *dev,
 				   struct ethtool_link_ksettings *cmd)
 {
 	struct ioc3_private *ip = netdev_priv(dev);
-	int rc;
 
 	spin_lock_irq(&ip->ioc3_lock);
-	rc = mii_ethtool_get_link_ksettings(&ip->mii, cmd);
+	mii_ethtool_get_link_ksettings(&ip->mii, cmd);
 	spin_unlock_irq(&ip->ioc3_lock);
 
-	return rc;
+	return 0;
 }
 
 static int ioc3_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/silan/sc92031.c b/drivers/net/ethernet/silan/sc92031.c
index 751c81848f35..c07fd594fe71 100644
--- a/drivers/net/ethernet/silan/sc92031.c
+++ b/drivers/net/ethernet/silan/sc92031.c
@@ -795,12 +795,12 @@ static void _sc92031_rx_tasklet(struct net_device *dev)
 		}
 
 		if ((rx_ring_offset + pkt_size) > RX_BUF_LEN) {
-			memcpy(skb_put(skb, RX_BUF_LEN - rx_ring_offset),
-				rx_ring + rx_ring_offset, RX_BUF_LEN - rx_ring_offset);
-			memcpy(skb_put(skb, pkt_size - (RX_BUF_LEN - rx_ring_offset)),
-				rx_ring, pkt_size - (RX_BUF_LEN - rx_ring_offset));
+			skb_put_data(skb, rx_ring + rx_ring_offset,
+				     RX_BUF_LEN - rx_ring_offset);
+			skb_put_data(skb, rx_ring,
+				     pkt_size - (RX_BUF_LEN - rx_ring_offset));
 		} else {
-			memcpy(skb_put(skb, pkt_size), rx_ring + rx_ring_offset, pkt_size);
+			skb_put_data(skb, rx_ring + rx_ring_offset, pkt_size);
 		}
 
 		skb->protocol = eth_type_trans(skb, dev);
diff --git a/drivers/net/ethernet/sis/sis190.c b/drivers/net/ethernet/sis/sis190.c
index 02da106c6e04..445109bd6910 100644
--- a/drivers/net/ethernet/sis/sis190.c
+++ b/drivers/net/ethernet/sis/sis190.c
@@ -1739,7 +1739,9 @@ static int sis190_get_link_ksettings(struct net_device *dev,
 {
 	struct sis190_private *tp = netdev_priv(dev);
 
-	return mii_ethtool_get_link_ksettings(&tp->mii_if, cmd);
+	mii_ethtool_get_link_ksettings(&tp->mii_if, cmd);
+
+	return 0;
 }
 
 static int sis190_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/smsc/epic100.c b/drivers/net/ethernet/smsc/epic100.c
index db6dcb06193d..6a0e1d4b597c 100644
--- a/drivers/net/ethernet/smsc/epic100.c
+++ b/drivers/net/ethernet/smsc/epic100.c
@@ -1391,13 +1391,12 @@ static int netdev_get_link_ksettings(struct net_device *dev,
 				     struct ethtool_link_ksettings *cmd)
 {
 	struct epic_private *np = netdev_priv(dev);
-	int rc;
 
 	spin_lock_irq(&np->lock);
-	rc = mii_ethtool_get_link_ksettings(&np->mii, cmd);
+	mii_ethtool_get_link_ksettings(&np->mii, cmd);
 	spin_unlock_irq(&np->lock);
 
-	return rc;
+	return 0;
 }
 
 static int netdev_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/smsc/smc911x.c b/drivers/net/ethernet/smsc/smc911x.c
index 36307d34f641..05157442a980 100644
--- a/drivers/net/ethernet/smsc/smc911x.c
+++ b/drivers/net/ethernet/smsc/smc911x.c
@@ -1450,7 +1450,7 @@ smc911x_ethtool_get_link_ksettings(struct net_device *dev,
 				   struct ethtool_link_ksettings *cmd)
 {
 	struct smc911x_local *lp = netdev_priv(dev);
-	int ret, status;
+	int status;
 	unsigned long flags;
 	u32 supported;
 
@@ -1458,7 +1458,7 @@ smc911x_ethtool_get_link_ksettings(struct net_device *dev,
 
 	if (lp->phy_type != 0) {
 		spin_lock_irqsave(&lp->lock, flags);
-		ret = mii_ethtool_get_link_ksettings(&lp->mii, cmd);
+		mii_ethtool_get_link_ksettings(&lp->mii, cmd);
 		spin_unlock_irqrestore(&lp->lock, flags);
 	} else {
 		supported = SUPPORTED_10baseT_Half |
@@ -1480,10 +1480,9 @@ smc911x_ethtool_get_link_ksettings(struct net_device *dev,
 		ethtool_convert_legacy_u32_to_link_mode(
 			cmd->link_modes.supported, supported);
 
-		ret = 0;
 	}
 
-	return ret;
+	return 0;
 }
 
 static int
diff --git a/drivers/net/ethernet/smsc/smc91c92_cs.c b/drivers/net/ethernet/smsc/smc91c92_cs.c
index 976aa876789a..92c927aec66d 100644
--- a/drivers/net/ethernet/smsc/smc91c92_cs.c
+++ b/drivers/net/ethernet/smsc/smc91c92_cs.c
@@ -1843,8 +1843,8 @@ static int smc_link_ok(struct net_device *dev)
     }
 }
 
-static int smc_netdev_get_ecmd(struct net_device *dev,
-			       struct ethtool_link_ksettings *ecmd)
+static void smc_netdev_get_ecmd(struct net_device *dev,
+				struct ethtool_link_ksettings *ecmd)
 {
 	u16 tmp;
 	unsigned int ioaddr = dev->base_addr;
@@ -1865,8 +1865,6 @@ static int smc_netdev_get_ecmd(struct net_device *dev,
 
 	ethtool_convert_legacy_u32_to_link_mode(ecmd->link_modes.supported,
 						supported);
-
-	return 0;
 }
 
 static int smc_netdev_set_ecmd(struct net_device *dev,
@@ -1918,18 +1916,17 @@ static int smc_get_link_ksettings(struct net_device *dev,
 	struct smc_private *smc = netdev_priv(dev);
 	unsigned int ioaddr = dev->base_addr;
 	u16 saved_bank = inw(ioaddr + BANK_SELECT);
-	int ret;
 	unsigned long flags;
 
 	spin_lock_irqsave(&smc->lock, flags);
 	SMC_SELECT_BANK(3);
 	if (smc->cfg & CFG_MII_SELECT)
-		ret = mii_ethtool_get_link_ksettings(&smc->mii_if, ecmd);
+		mii_ethtool_get_link_ksettings(&smc->mii_if, ecmd);
 	else
-		ret = smc_netdev_get_ecmd(dev, ecmd);
+		smc_netdev_get_ecmd(dev, ecmd);
 	SMC_SELECT_BANK(saved_bank);
 	spin_unlock_irqrestore(&smc->lock, flags);
-	return ret;
+	return 0;
 }
 
 static int smc_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/smsc/smc91x.c b/drivers/net/ethernet/smsc/smc91x.c
index 91e9bd7159ab..080428762858 100644
--- a/drivers/net/ethernet/smsc/smc91x.c
+++ b/drivers/net/ethernet/smsc/smc91x.c
@@ -1539,11 +1539,10 @@ smc_ethtool_get_link_ksettings(struct net_device *dev,
 			       struct ethtool_link_ksettings *cmd)
 {
 	struct smc_local *lp = netdev_priv(dev);
-	int ret;
 
 	if (lp->phy_type != 0) {
 		spin_lock_irq(&lp->lock);
-		ret = mii_ethtool_get_link_ksettings(&lp->mii, cmd);
+		mii_ethtool_get_link_ksettings(&lp->mii, cmd);
 		spin_unlock_irq(&lp->lock);
 	} else {
 		u32 supported = SUPPORTED_10baseT_Half |
@@ -1562,11 +1561,9 @@ smc_ethtool_get_link_ksettings(struct net_device *dev,
 
 		ethtool_convert_legacy_u32_to_link_mode(
 			cmd->link_modes.supported, supported);
-
-		ret = 0;
 	}
 
-	return ret;
+	return 0;
 }
 
 static int
@@ -2488,7 +2485,7 @@ static int smc_drv_resume(struct device *dev)
 	return 0;
 }
 
-static struct dev_pm_ops smc_drv_pm_ops = {
+static const struct dev_pm_ops smc_drv_pm_ops = {
 	.suspend	= smc_drv_suspend,
 	.resume		= smc_drv_resume,
 };
diff --git a/drivers/net/ethernet/stmicro/stmmac/Kconfig b/drivers/net/ethernet/stmicro/stmmac/Kconfig
index cfbe3634dfa1..85c0e41f8021 100644
--- a/drivers/net/ethernet/stmicro/stmmac/Kconfig
+++ b/drivers/net/ethernet/stmicro/stmmac/Kconfig
@@ -145,6 +145,17 @@ config DWMAC_SUNXI
 	  This selects Allwinner SoC glue layer support for the
 	  stmmac device driver. This driver is used for A20/A31
 	  GMAC ethernet controller.
+
+config DWMAC_SUN8I
+	tristate "Allwinner sun8i GMAC support"
+	default ARCH_SUNXI
+	depends on OF && (ARCH_SUNXI || COMPILE_TEST)
+	---help---
+	  Support for Allwinner H3 A83T A64 EMAC ethernet controllers.
+
+	  This selects Allwinner SoC glue layer support for the
+	  stmmac device driver. This driver is used for H3/A83T/A64
+	  EMAC ethernet controller.
 endif
 
 config STMMAC_PCI
diff --git a/drivers/net/ethernet/stmicro/stmmac/Makefile b/drivers/net/ethernet/stmicro/stmmac/Makefile
index 700c60336674..fd4937a7fcab 100644
--- a/drivers/net/ethernet/stmicro/stmmac/Makefile
+++ b/drivers/net/ethernet/stmicro/stmmac/Makefile
@@ -16,6 +16,7 @@ obj-$(CONFIG_DWMAC_SOCFPGA)	+= dwmac-altr-socfpga.o
 obj-$(CONFIG_DWMAC_STI)		+= dwmac-sti.o
 obj-$(CONFIG_DWMAC_STM32)	+= dwmac-stm32.o
 obj-$(CONFIG_DWMAC_SUNXI)	+= dwmac-sunxi.o
+obj-$(CONFIG_DWMAC_SUN8I)	+= dwmac-sun8i.o
 obj-$(CONFIG_DWMAC_DWC_QOS_ETH)	+= dwmac-dwc-qos-eth.o
 obj-$(CONFIG_DWMAC_GENERIC)	+= dwmac-generic.o
 stmmac-platform-objs:= stmmac_platform.o
diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
index b7ce3fbb5375..e82b4b70b7be 100644
--- a/drivers/net/ethernet/stmicro/stmmac/common.h
+++ b/drivers/net/ethernet/stmicro/stmmac/common.h
@@ -549,9 +549,11 @@ extern const struct stmmac_hwtimestamp stmmac_ptp;
 extern const struct stmmac_mode_ops dwmac4_ring_mode_ops;
 
 struct mac_link {
-	int port;
-	int duplex;
-	int speed;
+	u32 speed_mask;
+	u32 speed10;
+	u32 speed100;
+	u32 speed1000;
+	u32 duplex;
 };
 
 struct mii_regs {
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-sun8i.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-sun8i.c
new file mode 100644
index 000000000000..6c2d1da05588
--- /dev/null
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-sun8i.c
@@ -0,0 +1,1011 @@
+/*
+ * dwmac-sun8i.c - Allwinner sun8i DWMAC specific glue layer
+ *
+ * Copyright (C) 2017 Corentin Labbe <clabbe.montjoie@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/iopoll.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/of_mdio.h>
+#include <linux/of_net.h>
+#include <linux/phy.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/regmap.h>
+#include <linux/stmmac.h>
+
+#include "stmmac.h"
+#include "stmmac_platform.h"
+
+/* General notes on dwmac-sun8i:
+ * Locking: no locking is necessary in this file because all necessary locking
+ *		is done in the "stmmac files"
+ */
+
+/* struct emac_variant - Descrive dwmac-sun8i hardware variant
+ * @default_syscon_value:	The default value of the EMAC register in syscon
+ *				This value is used for disabling properly EMAC
+ *				and used as a good starting value in case of the
+ *				boot process(uboot) leave some stuff.
+ * @internal_phy:		Does the MAC embed an internal PHY
+ * @support_mii:		Does the MAC handle MII
+ * @support_rmii:		Does the MAC handle RMII
+ * @support_rgmii:		Does the MAC handle RGMII
+ */
+struct emac_variant {
+	u32 default_syscon_value;
+	int internal_phy;
+	bool support_mii;
+	bool support_rmii;
+	bool support_rgmii;
+};
+
+/* struct sunxi_priv_data - hold all sunxi private data
+ * @tx_clk:	reference to MAC TX clock
+ * @ephy_clk:	reference to the optional EPHY clock for the internal PHY
+ * @regulator:	reference to the optional regulator
+ * @rst_ephy:	reference to the optional EPHY reset for the internal PHY
+ * @variant:	reference to the current board variant
+ * @regmap:	regmap for using the syscon
+ * @use_internal_phy: Does the current PHY choice imply using the internal PHY
+ */
+struct sunxi_priv_data {
+	struct clk *tx_clk;
+	struct clk *ephy_clk;
+	struct regulator *regulator;
+	struct reset_control *rst_ephy;
+	const struct emac_variant *variant;
+	struct regmap *regmap;
+	bool use_internal_phy;
+};
+
+static const struct emac_variant emac_variant_h3 = {
+	.default_syscon_value = 0x58000,
+	.internal_phy = PHY_INTERFACE_MODE_MII,
+	.support_mii = true,
+	.support_rmii = true,
+	.support_rgmii = true
+};
+
+static const struct emac_variant emac_variant_v3s = {
+	.default_syscon_value = 0x38000,
+	.internal_phy = PHY_INTERFACE_MODE_MII,
+	.support_mii = true
+};
+
+static const struct emac_variant emac_variant_a83t = {
+	.default_syscon_value = 0,
+	.internal_phy = 0,
+	.support_mii = true,
+	.support_rgmii = true
+};
+
+static const struct emac_variant emac_variant_a64 = {
+	.default_syscon_value = 0,
+	.internal_phy = 0,
+	.support_mii = true,
+	.support_rmii = true,
+	.support_rgmii = true
+};
+
+#define EMAC_BASIC_CTL0 0x00
+#define EMAC_BASIC_CTL1 0x04
+#define EMAC_INT_STA    0x08
+#define EMAC_INT_EN     0x0C
+#define EMAC_TX_CTL0    0x10
+#define EMAC_TX_CTL1    0x14
+#define EMAC_TX_FLOW_CTL        0x1C
+#define EMAC_TX_DESC_LIST 0x20
+#define EMAC_RX_CTL0    0x24
+#define EMAC_RX_CTL1    0x28
+#define EMAC_RX_DESC_LIST 0x34
+#define EMAC_RX_FRM_FLT 0x38
+#define EMAC_MDIO_CMD   0x48
+#define EMAC_MDIO_DATA  0x4C
+#define EMAC_MACADDR_HI(reg) (0x50 + (reg) * 8)
+#define EMAC_MACADDR_LO(reg) (0x54 + (reg) * 8)
+#define EMAC_TX_DMA_STA 0xB0
+#define EMAC_TX_CUR_DESC        0xB4
+#define EMAC_TX_CUR_BUF 0xB8
+#define EMAC_RX_DMA_STA 0xC0
+#define EMAC_RX_CUR_DESC        0xC4
+#define EMAC_RX_CUR_BUF 0xC8
+
+/* Use in EMAC_BASIC_CTL0 */
+#define EMAC_DUPLEX_FULL	BIT(0)
+#define EMAC_LOOPBACK		BIT(1)
+#define EMAC_SPEED_1000 0
+#define EMAC_SPEED_100 (0x03 << 2)
+#define EMAC_SPEED_10 (0x02 << 2)
+
+/* Use in EMAC_BASIC_CTL1 */
+#define EMAC_BURSTLEN_SHIFT		24
+
+/* Used in EMAC_RX_FRM_FLT */
+#define EMAC_FRM_FLT_RXALL              BIT(0)
+#define EMAC_FRM_FLT_CTL                BIT(13)
+#define EMAC_FRM_FLT_MULTICAST          BIT(16)
+
+/* Used in RX_CTL1*/
+#define EMAC_RX_MD              BIT(1)
+#define EMAC_RX_TH_MASK		GENMASK(4, 5)
+#define EMAC_RX_TH_32		0
+#define EMAC_RX_TH_64		(0x1 << 4)
+#define EMAC_RX_TH_96		(0x2 << 4)
+#define EMAC_RX_TH_128		(0x3 << 4)
+#define EMAC_RX_DMA_EN  BIT(30)
+#define EMAC_RX_DMA_START       BIT(31)
+
+/* Used in TX_CTL1*/
+#define EMAC_TX_MD              BIT(1)
+#define EMAC_TX_NEXT_FRM        BIT(2)
+#define EMAC_TX_TH_MASK		GENMASK(8, 10)
+#define EMAC_TX_TH_64		0
+#define EMAC_TX_TH_128		(0x1 << 8)
+#define EMAC_TX_TH_192		(0x2 << 8)
+#define EMAC_TX_TH_256		(0x3 << 8)
+#define EMAC_TX_DMA_EN  BIT(30)
+#define EMAC_TX_DMA_START       BIT(31)
+
+/* Used in RX_CTL0 */
+#define EMAC_RX_RECEIVER_EN             BIT(31)
+#define EMAC_RX_DO_CRC BIT(27)
+#define EMAC_RX_FLOW_CTL_EN             BIT(16)
+
+/* Used in TX_CTL0 */
+#define EMAC_TX_TRANSMITTER_EN  BIT(31)
+
+/* Used in EMAC_TX_FLOW_CTL */
+#define EMAC_TX_FLOW_CTL_EN             BIT(0)
+
+/* Used in EMAC_INT_STA */
+#define EMAC_TX_INT             BIT(0)
+#define EMAC_TX_DMA_STOP_INT    BIT(1)
+#define EMAC_TX_BUF_UA_INT      BIT(2)
+#define EMAC_TX_TIMEOUT_INT     BIT(3)
+#define EMAC_TX_UNDERFLOW_INT   BIT(4)
+#define EMAC_TX_EARLY_INT       BIT(5)
+#define EMAC_RX_INT             BIT(8)
+#define EMAC_RX_BUF_UA_INT      BIT(9)
+#define EMAC_RX_DMA_STOP_INT    BIT(10)
+#define EMAC_RX_TIMEOUT_INT     BIT(11)
+#define EMAC_RX_OVERFLOW_INT    BIT(12)
+#define EMAC_RX_EARLY_INT       BIT(13)
+#define EMAC_RGMII_STA_INT      BIT(16)
+
+#define MAC_ADDR_TYPE_DST BIT(31)
+
+/* H3 specific bits for EPHY */
+#define H3_EPHY_ADDR_SHIFT	20
+#define H3_EPHY_CLK_SEL		BIT(18) /* 1: 24MHz, 0: 25MHz */
+#define H3_EPHY_LED_POL		BIT(17) /* 1: active low, 0: active high */
+#define H3_EPHY_SHUTDOWN	BIT(16) /* 1: shutdown, 0: power up */
+#define H3_EPHY_SELECT		BIT(15) /* 1: internal PHY, 0: external PHY */
+
+/* H3/A64 specific bits */
+#define SYSCON_RMII_EN		BIT(13) /* 1: enable RMII (overrides EPIT) */
+
+/* Generic system control EMAC_CLK bits */
+#define SYSCON_ETXDC_MASK		GENMASK(2, 0)
+#define SYSCON_ETXDC_SHIFT		10
+#define SYSCON_ERXDC_MASK		GENMASK(4, 0)
+#define SYSCON_ERXDC_SHIFT		5
+/* EMAC PHY Interface Type */
+#define SYSCON_EPIT			BIT(2) /* 1: RGMII, 0: MII */
+#define SYSCON_ETCS_MASK		GENMASK(1, 0)
+#define SYSCON_ETCS_MII		0x0
+#define SYSCON_ETCS_EXT_GMII	0x1
+#define SYSCON_ETCS_INT_GMII	0x2
+#define SYSCON_EMAC_REG		0x30
+
+/* sun8i_dwmac_dma_reset() - reset the EMAC
+ * Called from stmmac via stmmac_dma_ops->reset
+ */
+static int sun8i_dwmac_dma_reset(void __iomem *ioaddr)
+{
+	writel(0, ioaddr + EMAC_RX_CTL1);
+	writel(0, ioaddr + EMAC_TX_CTL1);
+	writel(0, ioaddr + EMAC_RX_FRM_FLT);
+	writel(0, ioaddr + EMAC_RX_DESC_LIST);
+	writel(0, ioaddr + EMAC_TX_DESC_LIST);
+	writel(0, ioaddr + EMAC_INT_EN);
+	writel(0x1FFFFFF, ioaddr + EMAC_INT_STA);
+	return 0;
+}
+
+/* sun8i_dwmac_dma_init() - initialize the EMAC
+ * Called from stmmac via stmmac_dma_ops->init
+ */
+static void sun8i_dwmac_dma_init(void __iomem *ioaddr,
+				 struct stmmac_dma_cfg *dma_cfg,
+				 u32 dma_tx, u32 dma_rx, int atds)
+{
+	/* Write TX and RX descriptors address */
+	writel(dma_rx, ioaddr + EMAC_RX_DESC_LIST);
+	writel(dma_tx, ioaddr + EMAC_TX_DESC_LIST);
+
+	writel(EMAC_RX_INT | EMAC_TX_INT, ioaddr + EMAC_INT_EN);
+	writel(0x1FFFFFF, ioaddr + EMAC_INT_STA);
+}
+
+/* sun8i_dwmac_dump_regs() - Dump EMAC address space
+ * Called from stmmac_dma_ops->dump_regs
+ * Used for ethtool
+ */
+static void sun8i_dwmac_dump_regs(void __iomem *ioaddr, u32 *reg_space)
+{
+	int i;
+
+	for (i = 0; i < 0xC8; i += 4) {
+		if (i == 0x32 || i == 0x3C)
+			continue;
+		reg_space[i / 4] = readl(ioaddr + i);
+	}
+}
+
+/* sun8i_dwmac_dump_mac_regs() - Dump EMAC address space
+ * Called from stmmac_ops->dump_regs
+ * Used for ethtool
+ */
+static void sun8i_dwmac_dump_mac_regs(struct mac_device_info *hw,
+				      u32 *reg_space)
+{
+	int i;
+	void __iomem *ioaddr = hw->pcsr;
+
+	for (i = 0; i < 0xC8; i += 4) {
+		if (i == 0x32 || i == 0x3C)
+			continue;
+		reg_space[i / 4] = readl(ioaddr + i);
+	}
+}
+
+static void sun8i_dwmac_enable_dma_irq(void __iomem *ioaddr, u32 chan)
+{
+	writel(EMAC_RX_INT | EMAC_TX_INT, ioaddr + EMAC_INT_EN);
+}
+
+static void sun8i_dwmac_disable_dma_irq(void __iomem *ioaddr, u32 chan)
+{
+	writel(0, ioaddr + EMAC_INT_EN);
+}
+
+static void sun8i_dwmac_dma_start_tx(void __iomem *ioaddr, u32 chan)
+{
+	u32 v;
+
+	v = readl(ioaddr + EMAC_TX_CTL1);
+	v |= EMAC_TX_DMA_START;
+	v |= EMAC_TX_DMA_EN;
+	writel(v, ioaddr + EMAC_TX_CTL1);
+}
+
+static void sun8i_dwmac_enable_dma_transmission(void __iomem *ioaddr)
+{
+	u32 v;
+
+	v = readl(ioaddr + EMAC_TX_CTL1);
+	v |= EMAC_TX_DMA_START;
+	v |= EMAC_TX_DMA_EN;
+	writel(v, ioaddr + EMAC_TX_CTL1);
+}
+
+static void sun8i_dwmac_dma_stop_tx(void __iomem *ioaddr, u32 chan)
+{
+	u32 v;
+
+	v = readl(ioaddr + EMAC_TX_CTL1);
+	v &= ~EMAC_TX_DMA_EN;
+	writel(v, ioaddr + EMAC_TX_CTL1);
+}
+
+static void sun8i_dwmac_dma_start_rx(void __iomem *ioaddr, u32 chan)
+{
+	u32 v;
+
+	v = readl(ioaddr + EMAC_RX_CTL1);
+	v |= EMAC_RX_DMA_START;
+	v |= EMAC_RX_DMA_EN;
+	writel(v, ioaddr + EMAC_RX_CTL1);
+}
+
+static void sun8i_dwmac_dma_stop_rx(void __iomem *ioaddr, u32 chan)
+{
+	u32 v;
+
+	v = readl(ioaddr + EMAC_RX_CTL1);
+	v &= ~EMAC_RX_DMA_EN;
+	writel(v, ioaddr + EMAC_RX_CTL1);
+}
+
+static int sun8i_dwmac_dma_interrupt(void __iomem *ioaddr,
+				     struct stmmac_extra_stats *x, u32 chan)
+{
+	u32 v;
+	int ret = 0;
+
+	v = readl(ioaddr + EMAC_INT_STA);
+
+	if (v & EMAC_TX_INT) {
+		ret |= handle_tx;
+		x->tx_normal_irq_n++;
+	}
+
+	if (v & EMAC_TX_DMA_STOP_INT)
+		x->tx_process_stopped_irq++;
+
+	if (v & EMAC_TX_BUF_UA_INT)
+		x->tx_process_stopped_irq++;
+
+	if (v & EMAC_TX_TIMEOUT_INT)
+		ret |= tx_hard_error;
+
+	if (v & EMAC_TX_UNDERFLOW_INT) {
+		ret |= tx_hard_error;
+		x->tx_undeflow_irq++;
+	}
+
+	if (v & EMAC_TX_EARLY_INT)
+		x->tx_early_irq++;
+
+	if (v & EMAC_RX_INT) {
+		ret |= handle_rx;
+		x->rx_normal_irq_n++;
+	}
+
+	if (v & EMAC_RX_BUF_UA_INT)
+		x->rx_buf_unav_irq++;
+
+	if (v & EMAC_RX_DMA_STOP_INT)
+		x->rx_process_stopped_irq++;
+
+	if (v & EMAC_RX_TIMEOUT_INT)
+		ret |= tx_hard_error;
+
+	if (v & EMAC_RX_OVERFLOW_INT) {
+		ret |= tx_hard_error;
+		x->rx_overflow_irq++;
+	}
+
+	if (v & EMAC_RX_EARLY_INT)
+		x->rx_early_irq++;
+
+	if (v & EMAC_RGMII_STA_INT)
+		x->irq_rgmii_n++;
+
+	writel(v, ioaddr + EMAC_INT_STA);
+
+	return ret;
+}
+
+static void sun8i_dwmac_dma_operation_mode(void __iomem *ioaddr, int txmode,
+					   int rxmode, int rxfifosz)
+{
+	u32 v;
+
+	v = readl(ioaddr + EMAC_TX_CTL1);
+	if (txmode == SF_DMA_MODE) {
+		v |= EMAC_TX_MD;
+		/* Undocumented bit (called TX_NEXT_FRM in BSP), the original
+		 * comment is
+		 * "Operating on second frame increase the performance
+		 * especially when transmit store-and-forward is used."
+		 */
+		v |= EMAC_TX_NEXT_FRM;
+	} else {
+		v &= ~EMAC_TX_MD;
+		v &= ~EMAC_TX_TH_MASK;
+		if (txmode < 64)
+			v |= EMAC_TX_TH_64;
+		else if (txmode < 128)
+			v |= EMAC_TX_TH_128;
+		else if (txmode < 192)
+			v |= EMAC_TX_TH_192;
+		else if (txmode < 256)
+			v |= EMAC_TX_TH_256;
+	}
+	writel(v, ioaddr + EMAC_TX_CTL1);
+
+	v = readl(ioaddr + EMAC_RX_CTL1);
+	if (rxmode == SF_DMA_MODE) {
+		v |= EMAC_RX_MD;
+	} else {
+		v &= ~EMAC_RX_MD;
+		v &= ~EMAC_RX_TH_MASK;
+		if (rxmode < 32)
+			v |= EMAC_RX_TH_32;
+		else if (rxmode < 64)
+			v |= EMAC_RX_TH_64;
+		else if (rxmode < 96)
+			v |= EMAC_RX_TH_96;
+		else if (rxmode < 128)
+			v |= EMAC_RX_TH_128;
+	}
+	writel(v, ioaddr + EMAC_RX_CTL1);
+}
+
+static const struct stmmac_dma_ops sun8i_dwmac_dma_ops = {
+	.reset = sun8i_dwmac_dma_reset,
+	.init = sun8i_dwmac_dma_init,
+	.dump_regs = sun8i_dwmac_dump_regs,
+	.dma_mode = sun8i_dwmac_dma_operation_mode,
+	.enable_dma_transmission = sun8i_dwmac_enable_dma_transmission,
+	.enable_dma_irq = sun8i_dwmac_enable_dma_irq,
+	.disable_dma_irq = sun8i_dwmac_disable_dma_irq,
+	.start_tx = sun8i_dwmac_dma_start_tx,
+	.stop_tx = sun8i_dwmac_dma_stop_tx,
+	.start_rx = sun8i_dwmac_dma_start_rx,
+	.stop_rx = sun8i_dwmac_dma_stop_rx,
+	.dma_interrupt = sun8i_dwmac_dma_interrupt,
+};
+
+static int sun8i_dwmac_init(struct platform_device *pdev, void *priv)
+{
+	struct sunxi_priv_data *gmac = priv;
+	int ret;
+
+	if (gmac->regulator) {
+		ret = regulator_enable(gmac->regulator);
+		if (ret) {
+			dev_err(&pdev->dev, "Fail to enable regulator\n");
+			return ret;
+		}
+	}
+
+	ret = clk_prepare_enable(gmac->tx_clk);
+	if (ret) {
+		if (gmac->regulator)
+			regulator_disable(gmac->regulator);
+		dev_err(&pdev->dev, "Could not enable AHB clock\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static void sun8i_dwmac_core_init(struct mac_device_info *hw, int mtu)
+{
+	void __iomem *ioaddr = hw->pcsr;
+	u32 v;
+
+	v = (8 << EMAC_BURSTLEN_SHIFT); /* burst len */
+	writel(v, ioaddr + EMAC_BASIC_CTL1);
+}
+
+static void sun8i_dwmac_set_mac(void __iomem *ioaddr, bool enable)
+{
+	u32 t, r;
+
+	t = readl(ioaddr + EMAC_TX_CTL0);
+	r = readl(ioaddr + EMAC_RX_CTL0);
+	if (enable) {
+		t |= EMAC_TX_TRANSMITTER_EN;
+		r |= EMAC_RX_RECEIVER_EN;
+	} else {
+		t &= ~EMAC_TX_TRANSMITTER_EN;
+		r &= ~EMAC_RX_RECEIVER_EN;
+	}
+	writel(t, ioaddr + EMAC_TX_CTL0);
+	writel(r, ioaddr + EMAC_RX_CTL0);
+}
+
+/* Set MAC address at slot reg_n
+ * All slot > 0 need to be enabled with MAC_ADDR_TYPE_DST
+ * If addr is NULL, clear the slot
+ */
+static void sun8i_dwmac_set_umac_addr(struct mac_device_info *hw,
+				      unsigned char *addr,
+				      unsigned int reg_n)
+{
+	void __iomem *ioaddr = hw->pcsr;
+	u32 v;
+
+	if (!addr) {
+		writel(0, ioaddr + EMAC_MACADDR_HI(reg_n));
+		return;
+	}
+
+	stmmac_set_mac_addr(ioaddr, addr, EMAC_MACADDR_HI(reg_n),
+			    EMAC_MACADDR_LO(reg_n));
+	if (reg_n > 0) {
+		v = readl(ioaddr + EMAC_MACADDR_HI(reg_n));
+		v |= MAC_ADDR_TYPE_DST;
+		writel(v, ioaddr + EMAC_MACADDR_HI(reg_n));
+	}
+}
+
+static void sun8i_dwmac_get_umac_addr(struct mac_device_info *hw,
+				      unsigned char *addr,
+				      unsigned int reg_n)
+{
+	void __iomem *ioaddr = hw->pcsr;
+
+	stmmac_get_mac_addr(ioaddr, addr, EMAC_MACADDR_HI(reg_n),
+			    EMAC_MACADDR_LO(reg_n));
+}
+
+/* caution this function must return non 0 to work */
+static int sun8i_dwmac_rx_ipc_enable(struct mac_device_info *hw)
+{
+	void __iomem *ioaddr = hw->pcsr;
+	u32 v;
+
+	v = readl(ioaddr + EMAC_RX_CTL0);
+	v |= EMAC_RX_DO_CRC;
+	writel(v, ioaddr + EMAC_RX_CTL0);
+
+	return 1;
+}
+
+static void sun8i_dwmac_set_filter(struct mac_device_info *hw,
+				   struct net_device *dev)
+{
+	void __iomem *ioaddr = hw->pcsr;
+	u32 v;
+	int i = 1;
+	struct netdev_hw_addr *ha;
+	int macaddrs = netdev_uc_count(dev) + netdev_mc_count(dev) + 1;
+
+	v = EMAC_FRM_FLT_CTL;
+
+	if (dev->flags & IFF_PROMISC) {
+		v = EMAC_FRM_FLT_RXALL;
+	} else if (dev->flags & IFF_ALLMULTI) {
+		v |= EMAC_FRM_FLT_MULTICAST;
+	} else if (macaddrs <= hw->unicast_filter_entries) {
+		if (!netdev_mc_empty(dev)) {
+			netdev_for_each_mc_addr(ha, dev) {
+				sun8i_dwmac_set_umac_addr(hw, ha->addr, i);
+				i++;
+			}
+		}
+		if (!netdev_uc_empty(dev)) {
+			netdev_for_each_uc_addr(ha, dev) {
+				sun8i_dwmac_set_umac_addr(hw, ha->addr, i);
+				i++;
+			}
+		}
+	} else {
+		netdev_info(dev, "Too many address, switching to promiscuous\n");
+		v = EMAC_FRM_FLT_RXALL;
+	}
+
+	/* Disable unused address filter slots */
+	while (i < hw->unicast_filter_entries)
+		sun8i_dwmac_set_umac_addr(hw, NULL, i++);
+
+	writel(v, ioaddr + EMAC_RX_FRM_FLT);
+}
+
+static void sun8i_dwmac_flow_ctrl(struct mac_device_info *hw,
+				  unsigned int duplex, unsigned int fc,
+				  unsigned int pause_time, u32 tx_cnt)
+{
+	void __iomem *ioaddr = hw->pcsr;
+	u32 v;
+
+	v = readl(ioaddr + EMAC_RX_CTL0);
+	if (fc == FLOW_AUTO)
+		v |= EMAC_RX_FLOW_CTL_EN;
+	else
+		v &= ~EMAC_RX_FLOW_CTL_EN;
+	writel(v, ioaddr + EMAC_RX_CTL0);
+
+	v = readl(ioaddr + EMAC_TX_FLOW_CTL);
+	if (fc == FLOW_AUTO)
+		v |= EMAC_TX_FLOW_CTL_EN;
+	else
+		v &= ~EMAC_TX_FLOW_CTL_EN;
+	writel(v, ioaddr + EMAC_TX_FLOW_CTL);
+}
+
+static int sun8i_dwmac_reset(struct stmmac_priv *priv)
+{
+	u32 v;
+	int err;
+
+	v = readl(priv->ioaddr + EMAC_BASIC_CTL1);
+	writel(v | 0x01, priv->ioaddr + EMAC_BASIC_CTL1);
+
+	/* The timeout was previoulsy set to 10ms, but some board (OrangePI0)
+	 * need more if no cable plugged. 100ms seems OK
+	 */
+	err = readl_poll_timeout(priv->ioaddr + EMAC_BASIC_CTL1, v,
+				 !(v & 0x01), 100, 100000);
+
+	if (err) {
+		dev_err(priv->device, "EMAC reset timeout\n");
+		return -EFAULT;
+	}
+	return 0;
+}
+
+static int sun8i_dwmac_set_syscon(struct stmmac_priv *priv)
+{
+	struct sunxi_priv_data *gmac = priv->plat->bsp_priv;
+	struct device_node *node = priv->device->of_node;
+	int ret, phy_interface;
+	u32 reg, val;
+
+	regmap_read(gmac->regmap, SYSCON_EMAC_REG, &val);
+	reg = gmac->variant->default_syscon_value;
+	if (reg != val)
+		dev_warn(priv->device,
+			 "Current syscon value is not the default %x (expect %x)\n",
+			 val, reg);
+
+	if (gmac->variant->internal_phy) {
+		if (!gmac->use_internal_phy) {
+			/* switch to external PHY interface */
+			reg &= ~H3_EPHY_SELECT;
+		} else {
+			reg |= H3_EPHY_SELECT;
+			reg &= ~H3_EPHY_SHUTDOWN;
+			dev_dbg(priv->device, "Select internal_phy %x\n", reg);
+
+			if (of_property_read_bool(priv->plat->phy_node,
+						  "allwinner,leds-active-low"))
+				reg |= H3_EPHY_LED_POL;
+			else
+				reg &= ~H3_EPHY_LED_POL;
+
+			/* Force EPHY xtal frequency to 24MHz. */
+			reg |= H3_EPHY_CLK_SEL;
+
+			ret = of_mdio_parse_addr(priv->device,
+						 priv->plat->phy_node);
+			if (ret < 0) {
+				dev_err(priv->device, "Could not parse MDIO addr\n");
+				return ret;
+			}
+			/* of_mdio_parse_addr returns a valid (0 ~ 31) PHY
+			 * address. No need to mask it again.
+			 */
+			reg |= ret << H3_EPHY_ADDR_SHIFT;
+		}
+	}
+
+	if (!of_property_read_u32(node, "allwinner,tx-delay-ps", &val)) {
+		if (val % 100) {
+			dev_err(priv->device, "tx-delay must be a multiple of 100\n");
+			return -EINVAL;
+		}
+		val /= 100;
+		dev_dbg(priv->device, "set tx-delay to %x\n", val);
+		if (val <= SYSCON_ETXDC_MASK) {
+			reg &= ~(SYSCON_ETXDC_MASK << SYSCON_ETXDC_SHIFT);
+			reg |= (val << SYSCON_ETXDC_SHIFT);
+		} else {
+			dev_err(priv->device, "Invalid TX clock delay: %d\n",
+				val);
+			return -EINVAL;
+		}
+	}
+
+	if (!of_property_read_u32(node, "allwinner,rx-delay-ps", &val)) {
+		if (val % 100) {
+			dev_err(priv->device, "rx-delay must be a multiple of 100\n");
+			return -EINVAL;
+		}
+		val /= 100;
+		dev_dbg(priv->device, "set rx-delay to %x\n", val);
+		if (val <= SYSCON_ERXDC_MASK) {
+			reg &= ~(SYSCON_ERXDC_MASK << SYSCON_ERXDC_SHIFT);
+			reg |= (val << SYSCON_ERXDC_SHIFT);
+		} else {
+			dev_err(priv->device, "Invalid RX clock delay: %d\n",
+				val);
+			return -EINVAL;
+		}
+	}
+
+	/* Clear interface mode bits */
+	reg &= ~(SYSCON_ETCS_MASK | SYSCON_EPIT);
+	if (gmac->variant->support_rmii)
+		reg &= ~SYSCON_RMII_EN;
+
+	phy_interface = priv->plat->interface;
+	/* if PHY is internal, select the mode (xMII) used by the SoC */
+	if (gmac->use_internal_phy)
+		phy_interface = gmac->variant->internal_phy;
+	switch (phy_interface) {
+	case PHY_INTERFACE_MODE_MII:
+		/* default */
+		break;
+	case PHY_INTERFACE_MODE_RGMII:
+		reg |= SYSCON_EPIT | SYSCON_ETCS_INT_GMII;
+		break;
+	case PHY_INTERFACE_MODE_RMII:
+		reg |= SYSCON_RMII_EN | SYSCON_ETCS_EXT_GMII;
+		break;
+	default:
+		dev_err(priv->device, "Unsupported interface mode: %s",
+			phy_modes(priv->plat->interface));
+		return -EINVAL;
+	}
+
+	regmap_write(gmac->regmap, SYSCON_EMAC_REG, reg);
+
+	return 0;
+}
+
+static void sun8i_dwmac_unset_syscon(struct sunxi_priv_data *gmac)
+{
+	u32 reg = gmac->variant->default_syscon_value;
+
+	regmap_write(gmac->regmap, SYSCON_EMAC_REG, reg);
+}
+
+static int sun8i_dwmac_power_internal_phy(struct stmmac_priv *priv)
+{
+	struct sunxi_priv_data *gmac = priv->plat->bsp_priv;
+	int ret;
+
+	if (!gmac->use_internal_phy)
+		return 0;
+
+	ret = clk_prepare_enable(gmac->ephy_clk);
+	if (ret) {
+		dev_err(priv->device, "Cannot enable ephy\n");
+		return ret;
+	}
+
+	/* Make sure the EPHY is properly reseted, as U-Boot may leave
+	 * it at deasserted state, and thus it may fail to reset EMAC.
+	 */
+	reset_control_assert(gmac->rst_ephy);
+
+	ret = reset_control_deassert(gmac->rst_ephy);
+	if (ret) {
+		dev_err(priv->device, "Cannot deassert ephy\n");
+		clk_disable_unprepare(gmac->ephy_clk);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int sun8i_dwmac_unpower_internal_phy(struct sunxi_priv_data *gmac)
+{
+	if (!gmac->use_internal_phy)
+		return 0;
+
+	clk_disable_unprepare(gmac->ephy_clk);
+	reset_control_assert(gmac->rst_ephy);
+	return 0;
+}
+
+/* sun8i_power_phy() - Activate the PHY:
+ * In case of error, no need to call sun8i_unpower_phy(),
+ * it will be called anyway by sun8i_dwmac_exit()
+ */
+static int sun8i_power_phy(struct stmmac_priv *priv)
+{
+	int ret;
+
+	ret = sun8i_dwmac_power_internal_phy(priv);
+	if (ret)
+		return ret;
+
+	ret = sun8i_dwmac_set_syscon(priv);
+	if (ret)
+		return ret;
+
+	/* After changing syscon value, the MAC need reset or it will use
+	 * the last value (and so the last PHY set.
+	 */
+	ret = sun8i_dwmac_reset(priv);
+	if (ret)
+		return ret;
+	return 0;
+}
+
+static void sun8i_unpower_phy(struct sunxi_priv_data *gmac)
+{
+	sun8i_dwmac_unset_syscon(gmac);
+	sun8i_dwmac_unpower_internal_phy(gmac);
+}
+
+static void sun8i_dwmac_exit(struct platform_device *pdev, void *priv)
+{
+	struct sunxi_priv_data *gmac = priv;
+
+	sun8i_unpower_phy(gmac);
+
+	clk_disable_unprepare(gmac->tx_clk);
+
+	if (gmac->regulator)
+		regulator_disable(gmac->regulator);
+}
+
+static const struct stmmac_ops sun8i_dwmac_ops = {
+	.core_init = sun8i_dwmac_core_init,
+	.set_mac = sun8i_dwmac_set_mac,
+	.dump_regs = sun8i_dwmac_dump_mac_regs,
+	.rx_ipc = sun8i_dwmac_rx_ipc_enable,
+	.set_filter = sun8i_dwmac_set_filter,
+	.flow_ctrl = sun8i_dwmac_flow_ctrl,
+	.set_umac_addr = sun8i_dwmac_set_umac_addr,
+	.get_umac_addr = sun8i_dwmac_get_umac_addr,
+};
+
+static struct mac_device_info *sun8i_dwmac_setup(void *ppriv)
+{
+	struct mac_device_info *mac;
+	struct stmmac_priv *priv = ppriv;
+	int ret;
+
+	mac = devm_kzalloc(priv->device, sizeof(*mac), GFP_KERNEL);
+	if (!mac)
+		return NULL;
+
+	ret = sun8i_power_phy(priv);
+	if (ret)
+		return NULL;
+
+	mac->pcsr = priv->ioaddr;
+	mac->mac = &sun8i_dwmac_ops;
+	mac->dma = &sun8i_dwmac_dma_ops;
+
+	/* The loopback bit seems to be re-set when link change
+	 * Simply mask it each time
+	 * Speed 10/100/1000 are set in BIT(2)/BIT(3)
+	 */
+	mac->link.speed_mask = GENMASK(3, 2) | EMAC_LOOPBACK;
+	mac->link.speed10 = EMAC_SPEED_10;
+	mac->link.speed100 = EMAC_SPEED_100;
+	mac->link.speed1000 = EMAC_SPEED_1000;
+	mac->link.duplex = EMAC_DUPLEX_FULL;
+	mac->mii.addr = EMAC_MDIO_CMD;
+	mac->mii.data = EMAC_MDIO_DATA;
+	mac->mii.reg_shift = 4;
+	mac->mii.reg_mask = GENMASK(8, 4);
+	mac->mii.addr_shift = 12;
+	mac->mii.addr_mask = GENMASK(16, 12);
+	mac->mii.clk_csr_shift = 20;
+	mac->mii.clk_csr_mask = GENMASK(22, 20);
+	mac->unicast_filter_entries = 8;
+
+	/* Synopsys Id is not available */
+	priv->synopsys_id = 0;
+
+	return mac;
+}
+
+static int sun8i_dwmac_probe(struct platform_device *pdev)
+{
+	struct plat_stmmacenet_data *plat_dat;
+	struct stmmac_resources stmmac_res;
+	struct sunxi_priv_data *gmac;
+	struct device *dev = &pdev->dev;
+	int ret;
+
+	ret = stmmac_get_platform_resources(pdev, &stmmac_res);
+	if (ret)
+		return ret;
+
+	plat_dat = stmmac_probe_config_dt(pdev, &stmmac_res.mac);
+	if (IS_ERR(plat_dat))
+		return PTR_ERR(plat_dat);
+
+	gmac = devm_kzalloc(dev, sizeof(*gmac), GFP_KERNEL);
+	if (!gmac)
+		return -ENOMEM;
+
+	gmac->variant = of_device_get_match_data(&pdev->dev);
+	if (!gmac->variant) {
+		dev_err(&pdev->dev, "Missing dwmac-sun8i variant\n");
+		return -EINVAL;
+	}
+
+	gmac->tx_clk = devm_clk_get(dev, "stmmaceth");
+	if (IS_ERR(gmac->tx_clk)) {
+		dev_err(dev, "Could not get TX clock\n");
+		return PTR_ERR(gmac->tx_clk);
+	}
+
+	/* Optional regulator for PHY */
+	gmac->regulator = devm_regulator_get_optional(dev, "phy");
+	if (IS_ERR(gmac->regulator)) {
+		if (PTR_ERR(gmac->regulator) == -EPROBE_DEFER)
+			return -EPROBE_DEFER;
+		dev_info(dev, "No regulator found\n");
+		gmac->regulator = NULL;
+	}
+
+	gmac->regmap = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
+						       "syscon");
+	if (IS_ERR(gmac->regmap)) {
+		ret = PTR_ERR(gmac->regmap);
+		dev_err(&pdev->dev, "Unable to map syscon: %d\n", ret);
+		return ret;
+	}
+
+	plat_dat->interface = of_get_phy_mode(dev->of_node);
+	if (plat_dat->interface == PHY_INTERFACE_MODE_INTERNAL) {
+		dev_info(&pdev->dev, "Will use internal PHY\n");
+		gmac->use_internal_phy = true;
+		gmac->ephy_clk = of_clk_get(plat_dat->phy_node, 0);
+		if (IS_ERR(gmac->ephy_clk)) {
+			ret = PTR_ERR(gmac->ephy_clk);
+			dev_err(&pdev->dev, "Cannot get EPHY clock: %d\n", ret);
+			return -EINVAL;
+		}
+
+		gmac->rst_ephy = of_reset_control_get(plat_dat->phy_node, NULL);
+		if (IS_ERR(gmac->rst_ephy)) {
+			ret = PTR_ERR(gmac->rst_ephy);
+			if (ret == -EPROBE_DEFER)
+				return ret;
+			dev_err(&pdev->dev, "No EPHY reset control found %d\n",
+				ret);
+			return -EINVAL;
+		}
+	} else {
+		dev_info(&pdev->dev, "Will use external PHY\n");
+		gmac->use_internal_phy = false;
+	}
+
+	/* platform data specifying hardware features and callbacks.
+	 * hardware features were copied from Allwinner drivers.
+	 */
+	plat_dat->rx_coe = STMMAC_RX_COE_TYPE2;
+	plat_dat->tx_coe = 1;
+	plat_dat->has_sun8i = true;
+	plat_dat->bsp_priv = gmac;
+	plat_dat->init = sun8i_dwmac_init;
+	plat_dat->exit = sun8i_dwmac_exit;
+	plat_dat->setup = sun8i_dwmac_setup;
+
+	ret = sun8i_dwmac_init(pdev, plat_dat->bsp_priv);
+	if (ret)
+		return ret;
+
+	ret = stmmac_dvr_probe(&pdev->dev, plat_dat, &stmmac_res);
+	if (ret)
+		sun8i_dwmac_exit(pdev, plat_dat->bsp_priv);
+
+	return ret;
+}
+
+static const struct of_device_id sun8i_dwmac_match[] = {
+	{ .compatible = "allwinner,sun8i-h3-emac",
+		.data = &emac_variant_h3 },
+	{ .compatible = "allwinner,sun8i-v3s-emac",
+		.data = &emac_variant_v3s },
+	{ .compatible = "allwinner,sun8i-a83t-emac",
+		.data = &emac_variant_a83t },
+	{ .compatible = "allwinner,sun50i-a64-emac",
+		.data = &emac_variant_a64 },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sun8i_dwmac_match);
+
+static struct platform_driver sun8i_dwmac_driver = {
+	.probe  = sun8i_dwmac_probe,
+	.remove = stmmac_pltfr_remove,
+	.driver = {
+		.name           = "dwmac-sun8i",
+		.pm		= &stmmac_pltfr_pm_ops,
+		.of_match_table = sun8i_dwmac_match,
+	},
+};
+module_platform_driver(sun8i_dwmac_driver);
+
+MODULE_AUTHOR("Corentin Labbe <clabbe.montjoie@gmail.com>");
+MODULE_DESCRIPTION("Allwinner sun8i DWMAC specific glue layer");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
index f3d9305e5f70..8a86340ff2d3 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
@@ -45,15 +45,17 @@ static void dwmac1000_core_init(struct mac_device_info *hw, int mtu)
 	if (hw->ps) {
 		value |= GMAC_CONTROL_TE;
 
-		if (hw->ps == SPEED_1000) {
-			value &= ~GMAC_CONTROL_PS;
-		} else {
-			value |= GMAC_CONTROL_PS;
-
-			if (hw->ps == SPEED_10)
-				value &= ~GMAC_CONTROL_FES;
-			else
-				value |= GMAC_CONTROL_FES;
+		value &= ~hw->link.speed_mask;
+		switch (hw->ps) {
+		case SPEED_1000:
+			value |= hw->link.speed1000;
+			break;
+		case SPEED_100:
+			value |= hw->link.speed100;
+			break;
+		case SPEED_10:
+			value |= hw->link.speed10;
+			break;
 		}
 	}
 
@@ -531,9 +533,11 @@ struct mac_device_info *dwmac1000_setup(void __iomem *ioaddr, int mcbins,
 	mac->mac = &dwmac1000_ops;
 	mac->dma = &dwmac1000_dma_ops;
 
-	mac->link.port = GMAC_CONTROL_PS;
 	mac->link.duplex = GMAC_CONTROL_DM;
-	mac->link.speed = GMAC_CONTROL_FES;
+	mac->link.speed10 = GMAC_CONTROL_PS;
+	mac->link.speed100 = GMAC_CONTROL_PS | GMAC_CONTROL_FES;
+	mac->link.speed1000 = 0;
+	mac->link.speed_mask = GMAC_CONTROL_PS | GMAC_CONTROL_FES;
 	mac->mii.addr = GMAC_MII_ADDR;
 	mac->mii.data = GMAC_MII_DATA;
 	mac->mii.addr_shift = 11;
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c
index 471a9aa6ac94..22cf6353ba04 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c
@@ -205,8 +205,8 @@ static void dwmac1000_dump_dma_regs(void __iomem *ioaddr, u32 *reg_space)
 {
 	int i;
 
-	for (i = 0; i < 22; i++)
-		if ((i < 9) || (i > 17))
+	for (i = 0; i < 23; i++)
+		if ((i < 12) || (i > 17))
 			reg_space[DMA_BUS_MODE / 4 + i] =
 				readl(ioaddr + DMA_BUS_MODE + i * 4);
 }
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c
index 1b3609105484..8ef517356313 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c
@@ -175,9 +175,11 @@ struct mac_device_info *dwmac100_setup(void __iomem *ioaddr, int *synopsys_id)
 	mac->mac = &dwmac100_ops;
 	mac->dma = &dwmac100_dma_ops;
 
-	mac->link.port = MAC_CONTROL_PS;
 	mac->link.duplex = MAC_CONTROL_F;
-	mac->link.speed = 0;
+	mac->link.speed10 = 0;
+	mac->link.speed100 = 0;
+	mac->link.speed1000 = 0;
+	mac->link.speed_mask = MAC_CONTROL_PS;
 	mac->mii.addr = MAC_MII_ADDR;
 	mac->mii.data = MAC_MII_DATA;
 	mac->mii.addr_shift = 11;
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
index 48793f2e9307..f233bf8b4ebb 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
@@ -35,15 +35,17 @@ static void dwmac4_core_init(struct mac_device_info *hw, int mtu)
 	if (hw->ps) {
 		value |= GMAC_CONFIG_TE;
 
-		if (hw->ps == SPEED_1000) {
-			value &= ~GMAC_CONFIG_PS;
-		} else {
-			value |= GMAC_CONFIG_PS;
-
-			if (hw->ps == SPEED_10)
-				value &= ~GMAC_CONFIG_FES;
-			else
-				value |= GMAC_CONFIG_FES;
+		value &= hw->link.speed_mask;
+		switch (hw->ps) {
+		case SPEED_1000:
+			value |= hw->link.speed1000;
+			break;
+		case SPEED_100:
+			value |= hw->link.speed100;
+			break;
+		case SPEED_10:
+			value |= hw->link.speed10;
+			break;
 		}
 	}
 
@@ -747,9 +749,11 @@ struct mac_device_info *dwmac4_setup(void __iomem *ioaddr, int mcbins,
 	if (mac->multicast_filter_bins)
 		mac->mcast_bits_log2 = ilog2(mac->multicast_filter_bins);
 
-	mac->link.port = GMAC_CONFIG_PS;
 	mac->link.duplex = GMAC_CONFIG_DM;
-	mac->link.speed = GMAC_CONFIG_FES;
+	mac->link.speed10 = GMAC_CONFIG_PS;
+	mac->link.speed100 = GMAC_CONFIG_FES | GMAC_CONFIG_PS;
+	mac->link.speed1000 = 0;
+	mac->link.speed_mask = GMAC_CONFIG_FES | GMAC_CONFIG_PS;
 	mac->mii.addr = GMAC_MDIO_ADDR;
 	mac->mii.data = GMAC_MDIO_DATA;
 	mac->mii.addr_shift = 21;
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c b/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c
index aa6476439aee..e0ef02f9503b 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c
@@ -214,13 +214,13 @@ static int dwmac4_wrback_get_tx_timestamp_status(struct dma_desc *p)
 {
 	/* Context type from W/B descriptor must be zero */
 	if (le32_to_cpu(p->des3) & TDES3_CONTEXT_TYPE)
-		return -EINVAL;
+		return 0;
 
 	/* Tx Timestamp Status is 1 so des0 and des1'll have valid values */
 	if (le32_to_cpu(p->des3) & TDES3_TIMESTAMP_STATUS)
-		return 0;
+		return 1;
 
-	return 1;
+	return 0;
 }
 
 static inline u64 dwmac4_get_timestamp(void *desc, u32 ats)
@@ -282,7 +282,10 @@ static int dwmac4_wrback_get_rx_timestamp_status(void *desc, u32 ats)
 		}
 	}
 exit:
-	return ret;
+	if (likely(ret == 0))
+		return 1;
+
+	return 0;
 }
 
 static void dwmac4_rd_init_rx_desc(struct dma_desc *p, int disable_rx_ic,
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c b/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c
index eec8463057fd..e84831e1b63b 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c
@@ -71,9 +71,9 @@ static void dwmac4_dma_axi(void __iomem *ioaddr, struct stmmac_axi *axi)
 	writel(value, ioaddr + DMA_SYS_BUS_MODE);
 }
 
-void dwmac4_dma_init_rx_chan(void __iomem *ioaddr,
-			     struct stmmac_dma_cfg *dma_cfg,
-			     u32 dma_rx_phy, u32 chan)
+static void dwmac4_dma_init_rx_chan(void __iomem *ioaddr,
+				    struct stmmac_dma_cfg *dma_cfg,
+				    u32 dma_rx_phy, u32 chan)
 {
 	u32 value;
 	u32 rxpbl = dma_cfg->rxpbl ?: dma_cfg->pbl;
@@ -85,9 +85,9 @@ void dwmac4_dma_init_rx_chan(void __iomem *ioaddr,
 	writel(dma_rx_phy, ioaddr + DMA_CHAN_RX_BASE_ADDR(chan));
 }
 
-void dwmac4_dma_init_tx_chan(void __iomem *ioaddr,
-			     struct stmmac_dma_cfg *dma_cfg,
-			     u32 dma_tx_phy, u32 chan)
+static void dwmac4_dma_init_tx_chan(void __iomem *ioaddr,
+				    struct stmmac_dma_cfg *dma_cfg,
+				    u32 dma_tx_phy, u32 chan)
 {
 	u32 value;
 	u32 txpbl = dma_cfg->txpbl ?: dma_cfg->pbl;
@@ -99,8 +99,8 @@ void dwmac4_dma_init_tx_chan(void __iomem *ioaddr,
 	writel(dma_tx_phy, ioaddr + DMA_CHAN_TX_BASE_ADDR(chan));
 }
 
-void dwmac4_dma_init_channel(void __iomem *ioaddr,
-			     struct stmmac_dma_cfg *dma_cfg, u32 chan)
+static void dwmac4_dma_init_channel(void __iomem *ioaddr,
+				    struct stmmac_dma_cfg *dma_cfg, u32 chan)
 {
 	u32 value;
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c b/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c
index 38f94305aab5..67af0bdd7f10 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c
@@ -248,6 +248,7 @@ void stmmac_set_mac_addr(void __iomem *ioaddr, u8 addr[6],
 	data = (addr[3] << 24) | (addr[2] << 16) | (addr[1] << 8) | addr[0];
 	writel(data, ioaddr + low);
 }
+EXPORT_SYMBOL_GPL(stmmac_set_mac_addr);
 
 /* Enable disable MAC RX/TX */
 void stmmac_set_mac(void __iomem *ioaddr, bool enable)
@@ -279,4 +280,4 @@ void stmmac_get_mac_addr(void __iomem *ioaddr, unsigned char *addr,
 	addr[4] = hi_addr & 0xff;
 	addr[5] = (hi_addr >> 8) & 0xff;
 }
-
+EXPORT_SYMBOL_GPL(stmmac_get_mac_addr);
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index 33efe7038cab..a916e13624eb 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -104,7 +104,7 @@ struct stmmac_priv {
 	/* TX Queue */
 	struct stmmac_tx_queue tx_queue[MTL_MAX_TX_QUEUES];
 
-	int oldlink;
+	bool oldlink;
 	int speed;
 	int oldduplex;
 	unsigned int flow_ctrl;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
index 16808e48ca1c..babb39c646ff 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
@@ -29,7 +29,7 @@
 #include "stmmac.h"
 #include "dwmac_dma.h"
 
-#define REG_SPACE_SIZE	0x1054
+#define REG_SPACE_SIZE	0x1060
 #define MAC100_ETHTOOL_NAME	"st_mac100"
 #define GMAC_ETHTOOL_NAME	"st_gmac"
 
@@ -273,7 +273,6 @@ static int stmmac_ethtool_get_link_ksettings(struct net_device *dev,
 {
 	struct stmmac_priv *priv = netdev_priv(dev);
 	struct phy_device *phy = dev->phydev;
-	int rc;
 
 	if (priv->hw->pcs & STMMAC_PCS_RGMII ||
 	    priv->hw->pcs & STMMAC_PCS_SGMII) {
@@ -364,8 +363,8 @@ static int stmmac_ethtool_get_link_ksettings(struct net_device *dev,
 		"link speed / duplex setting\n", dev->name);
 		return -EBUSY;
 	}
-	rc = phy_ethtool_ksettings_get(phy, cmd);
-	return rc;
+	phy_ethtool_ksettings_get(phy, cmd);
+	return 0;
 }
 
 static int
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 12236daf7bb6..19bba6281dab 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -235,6 +235,17 @@ static void stmmac_clk_csr_set(struct stmmac_priv *priv)
 		else if ((clk_rate >= CSR_F_250M) && (clk_rate < CSR_F_300M))
 			priv->clk_csr = STMMAC_CSR_250_300M;
 	}
+
+	if (priv->plat->has_sun8i) {
+		if (clk_rate > 160000000)
+			priv->clk_csr = 0x03;
+		else if (clk_rate > 80000000)
+			priv->clk_csr = 0x02;
+		else if (clk_rate > 40000000)
+			priv->clk_csr = 0x01;
+		else
+			priv->clk_csr = 0;
+	}
 }
 
 static void print_pkt(unsigned char *buf, int len)
@@ -434,14 +445,14 @@ static void stmmac_get_tx_hwtstamp(struct stmmac_priv *priv,
 		return;
 
 	/* check tx tstamp status */
-	if (!priv->hw->desc->get_tx_timestamp_status(p)) {
+	if (priv->hw->desc->get_tx_timestamp_status(p)) {
 		/* get the valid tstamp */
 		ns = priv->hw->desc->get_timestamp(p, priv->adv_ts);
 
 		memset(&shhwtstamp, 0, sizeof(struct skb_shared_hwtstamps));
 		shhwtstamp.hwtstamp = ns_to_ktime(ns);
 
-		netdev_info(priv->dev, "get valid TX hw timestamp %llu\n", ns);
+		netdev_dbg(priv->dev, "get valid TX hw timestamp %llu\n", ns);
 		/* pass tstamp to stack */
 		skb_tstamp_tx(skb, &shhwtstamp);
 	}
@@ -468,19 +479,19 @@ static void stmmac_get_rx_hwtstamp(struct stmmac_priv *priv, struct dma_desc *p,
 		return;
 
 	/* Check if timestamp is available */
-	if (!priv->hw->desc->get_rx_timestamp_status(p, priv->adv_ts)) {
+	if (priv->hw->desc->get_rx_timestamp_status(p, priv->adv_ts)) {
 		/* For GMAC4, the valid timestamp is from CTX next desc. */
 		if (priv->plat->has_gmac4)
 			ns = priv->hw->desc->get_timestamp(np, priv->adv_ts);
 		else
 			ns = priv->hw->desc->get_timestamp(p, priv->adv_ts);
 
-		netdev_info(priv->dev, "get valid RX hw timestamp %llu\n", ns);
+		netdev_dbg(priv->dev, "get valid RX hw timestamp %llu\n", ns);
 		shhwtstamp = skb_hwtstamps(skb);
 		memset(shhwtstamp, 0, sizeof(struct skb_shared_hwtstamps));
 		shhwtstamp->hwtstamp = ns_to_ktime(ns);
 	} else  {
-		netdev_err(priv->dev, "cannot get RX hw timestamp\n");
+		netdev_dbg(priv->dev, "cannot get RX hw timestamp\n");
 	}
 }
 
@@ -546,7 +557,10 @@ static int stmmac_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
 			/* PTP v1, UDP, any kind of event packet */
 			config.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
 			/* take time stamp for all event messages */
-			snap_type_sel = PTP_TCR_SNAPTYPSEL_1;
+			if (priv->plat->has_gmac4)
+				snap_type_sel = PTP_GMAC4_TCR_SNAPTYPSEL_1;
+			else
+				snap_type_sel = PTP_TCR_SNAPTYPSEL_1;
 
 			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
@@ -578,7 +592,10 @@ static int stmmac_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
 			config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
 			ptp_v2 = PTP_TCR_TSVER2ENA;
 			/* take time stamp for all event messages */
-			snap_type_sel = PTP_TCR_SNAPTYPSEL_1;
+			if (priv->plat->has_gmac4)
+				snap_type_sel = PTP_GMAC4_TCR_SNAPTYPSEL_1;
+			else
+				snap_type_sel = PTP_TCR_SNAPTYPSEL_1;
 
 			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
@@ -612,7 +629,10 @@ static int stmmac_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
 			config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 			ptp_v2 = PTP_TCR_TSVER2ENA;
 			/* take time stamp for all event messages */
-			snap_type_sel = PTP_TCR_SNAPTYPSEL_1;
+			if (priv->plat->has_gmac4)
+				snap_type_sel = PTP_GMAC4_TCR_SNAPTYPSEL_1;
+			else
+				snap_type_sel = PTP_TCR_SNAPTYPSEL_1;
 
 			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
@@ -644,6 +664,7 @@ static int stmmac_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
 			ptp_over_ethernet = PTP_TCR_TSIPENA;
 			break;
 
+		case HWTSTAMP_FILTER_NTP_ALL:
 		case HWTSTAMP_FILTER_ALL:
 			/* time stamp any incoming packet */
 			config.rx_filter = HWTSTAMP_FILTER_ALL;
@@ -774,7 +795,7 @@ static void stmmac_adjust_link(struct net_device *dev)
 	struct stmmac_priv *priv = netdev_priv(dev);
 	struct phy_device *phydev = dev->phydev;
 	unsigned long flags;
-	int new_state = 0;
+	bool new_state = false;
 
 	if (!phydev)
 		return;
@@ -787,8 +808,8 @@ static void stmmac_adjust_link(struct net_device *dev)
 		/* Now we make sure that we can be in full duplex mode.
 		 * If not, we operate in half-duplex mode. */
 		if (phydev->duplex != priv->oldduplex) {
-			new_state = 1;
-			if (!(phydev->duplex))
+			new_state = true;
+			if (!phydev->duplex)
 				ctrl &= ~priv->hw->link.duplex;
 			else
 				ctrl |= priv->hw->link.duplex;
@@ -799,30 +820,17 @@ static void stmmac_adjust_link(struct net_device *dev)
 			stmmac_mac_flow_ctrl(priv, phydev->duplex);
 
 		if (phydev->speed != priv->speed) {
-			new_state = 1;
+			new_state = true;
+			ctrl &= ~priv->hw->link.speed_mask;
 			switch (phydev->speed) {
-			case 1000:
-				if (priv->plat->has_gmac ||
-				    priv->plat->has_gmac4)
-					ctrl &= ~priv->hw->link.port;
+			case SPEED_1000:
+				ctrl |= priv->hw->link.speed1000;
 				break;
-			case 100:
-				if (priv->plat->has_gmac ||
-				    priv->plat->has_gmac4) {
-					ctrl |= priv->hw->link.port;
-					ctrl |= priv->hw->link.speed;
-				} else {
-					ctrl &= ~priv->hw->link.port;
-				}
+			case SPEED_100:
+				ctrl |= priv->hw->link.speed100;
 				break;
-			case 10:
-				if (priv->plat->has_gmac ||
-				    priv->plat->has_gmac4) {
-					ctrl |= priv->hw->link.port;
-					ctrl &= ~(priv->hw->link.speed);
-				} else {
-					ctrl &= ~priv->hw->link.port;
-				}
+			case SPEED_10:
+				ctrl |= priv->hw->link.speed10;
 				break;
 			default:
 				netif_warn(priv, link, priv->dev,
@@ -838,12 +846,12 @@ static void stmmac_adjust_link(struct net_device *dev)
 		writel(ctrl, priv->ioaddr + MAC_CTRL_REG);
 
 		if (!priv->oldlink) {
-			new_state = 1;
-			priv->oldlink = 1;
+			new_state = true;
+			priv->oldlink = true;
 		}
 	} else if (priv->oldlink) {
-		new_state = 1;
-		priv->oldlink = 0;
+		new_state = true;
+		priv->oldlink = false;
 		priv->speed = SPEED_UNKNOWN;
 		priv->oldduplex = DUPLEX_UNKNOWN;
 	}
@@ -906,7 +914,7 @@ static int stmmac_init_phy(struct net_device *dev)
 	char bus_id[MII_BUS_ID_SIZE];
 	int interface = priv->plat->interface;
 	int max_speed = priv->plat->max_speed;
-	priv->oldlink = 0;
+	priv->oldlink = false;
 	priv->speed = SPEED_UNKNOWN;
 	priv->oldduplex = DUPLEX_UNKNOWN;
 
@@ -2822,7 +2830,6 @@ static netdev_tx_t stmmac_tso_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	tx_q->tx_skbuff_dma[first_entry].buf = des;
 	tx_q->tx_skbuff_dma[first_entry].len = skb_headlen(skb);
-	tx_q->tx_skbuff[first_entry] = skb;
 
 	first->des0 = cpu_to_le32(des);
 
@@ -2856,6 +2863,14 @@ static netdev_tx_t stmmac_tso_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	tx_q->tx_skbuff_dma[tx_q->cur_tx].last_segment = true;
 
+	/* Only the last descriptor gets to point to the skb. */
+	tx_q->tx_skbuff[tx_q->cur_tx] = skb;
+
+	/* We've used all descriptors we need for this skb, however,
+	 * advance cur_tx so that it references a fresh descriptor.
+	 * ndo_start_xmit will fill this descriptor the next time it's
+	 * called and stmmac_tx_clean may clean up to this descriptor.
+	 */
 	tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, DMA_TX_SIZE);
 
 	if (unlikely(stmmac_tx_avail(priv, queue) <= (MAX_SKB_FRAGS + 1))) {
@@ -2879,8 +2894,7 @@ static netdev_tx_t stmmac_tso_xmit(struct sk_buff *skb, struct net_device *dev)
 		priv->xstats.tx_set_ic_bit++;
 	}
 
-	if (!priv->hwts_tx_en)
-		skb_tx_timestamp(skb);
+	skb_tx_timestamp(skb);
 
 	if (unlikely((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
 		     priv->hwts_tx_en)) {
@@ -2958,7 +2972,7 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* Manage oversized TCP frames for GMAC4 device */
 	if (skb_is_gso(skb) && priv->tso) {
-		if (ip_hdr(skb)->protocol == IPPROTO_TCP)
+		if (skb_shinfo(skb)->gso_type & (SKB_GSO_TCPV4 | SKB_GSO_TCPV6))
 			return stmmac_tso_xmit(skb, dev);
 	}
 
@@ -2989,8 +3003,6 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	first = desc;
 
-	tx_q->tx_skbuff[first_entry] = skb;
-
 	enh_desc = priv->plat->enh_desc;
 	/* To program the descriptors according to the size of the frame */
 	if (enh_desc)
@@ -3038,8 +3050,15 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 						skb->len);
 	}
 
-	entry = STMMAC_GET_ENTRY(entry, DMA_TX_SIZE);
+	/* Only the last descriptor gets to point to the skb. */
+	tx_q->tx_skbuff[entry] = skb;
 
+	/* We've used all descriptors we need for this skb, however,
+	 * advance cur_tx so that it references a fresh descriptor.
+	 * ndo_start_xmit will fill this descriptor the next time it's
+	 * called and stmmac_tx_clean may clean up to this descriptor.
+	 */
+	entry = STMMAC_GET_ENTRY(entry, DMA_TX_SIZE);
 	tx_q->cur_tx = entry;
 
 	if (netif_msg_pktdata(priv)) {
@@ -3084,8 +3103,7 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 		priv->xstats.tx_set_ic_bit++;
 	}
 
-	if (!priv->hwts_tx_en)
-		skb_tx_timestamp(skb);
+	skb_tx_timestamp(skb);
 
 	/* Ready to fill the first descriptor and set the OWN bit w/o any
 	 * problems because all the descriptors are actually ready to be
@@ -3948,7 +3966,9 @@ static int stmmac_hw_init(struct stmmac_priv *priv)
 	struct mac_device_info *mac;
 
 	/* Identify the MAC HW device */
-	if (priv->plat->has_gmac) {
+	if (priv->plat->setup) {
+		mac = priv->plat->setup(priv);
+	} else if (priv->plat->has_gmac) {
 		priv->dev->priv_flags |= IFF_UNICAST_FLT;
 		mac = dwmac1000_setup(priv->ioaddr,
 				      priv->plat->multicast_filter_bins,
@@ -3968,6 +3988,10 @@ static int stmmac_hw_init(struct stmmac_priv *priv)
 
 	priv->hw = mac;
 
+	/* dwmac-sun8i only work in chain mode */
+	if (priv->plat->has_sun8i)
+		chain_mode = 1;
+
 	/* To use the chained or ring mode */
 	if (priv->synopsys_id >= DWMAC_CORE_4_00) {
 		priv->hw->mode = &dwmac4_ring_mode_ops;
@@ -4114,7 +4138,7 @@ int stmmac_dvr_probe(struct device *device,
 			    NETIF_F_RXCSUM;
 
 	if ((priv->plat->tso_en) && (priv->dma_cap.tsoen)) {
-		ndev->hw_features |= NETIF_F_TSO;
+		ndev->hw_features |= NETIF_F_TSO | NETIF_F_TSO6;
 		priv->tso = true;
 		dev_info(priv->device, "TSO feature enabled\n");
 	}
@@ -4293,7 +4317,7 @@ int stmmac_suspend(struct device *dev)
 	}
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	priv->oldlink = 0;
+	priv->oldlink = false;
 	priv->speed = SPEED_UNKNOWN;
 	priv->oldduplex = DUPLEX_UNKNOWN;
 	return 0;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
index 22f910795be4..8d375e51a526 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
@@ -30,42 +30,39 @@
  * negative value of the address means that MAC controller is not connected
  * with PHY.
  */
-struct stmmac_pci_dmi_data {
-	const char *name;
-	const char *asset_tag;
+struct stmmac_pci_func_data {
 	unsigned int func;
 	int phy_addr;
 };
 
+struct stmmac_pci_dmi_data {
+	const struct stmmac_pci_func_data *func;
+	size_t nfuncs;
+};
+
 struct stmmac_pci_info {
-	struct pci_dev *pdev;
-	int (*setup)(struct plat_stmmacenet_data *plat,
-		     struct stmmac_pci_info *info);
-	struct stmmac_pci_dmi_data *dmi;
+	int (*setup)(struct pci_dev *pdev, struct plat_stmmacenet_data *plat);
 };
 
-static int stmmac_pci_find_phy_addr(struct stmmac_pci_info *info)
+static int stmmac_pci_find_phy_addr(struct pci_dev *pdev,
+				    const struct dmi_system_id *dmi_list)
 {
-	const char *name = dmi_get_system_info(DMI_BOARD_NAME);
-	const char *asset_tag = dmi_get_system_info(DMI_BOARD_ASSET_TAG);
-	unsigned int func = PCI_FUNC(info->pdev->devfn);
-	struct stmmac_pci_dmi_data *dmi;
+	const struct stmmac_pci_func_data *func_data;
+	const struct stmmac_pci_dmi_data *dmi_data;
+	const struct dmi_system_id *dmi_id;
+	int func = PCI_FUNC(pdev->devfn);
+	size_t n;
 
-	/*
-	 * Galileo boards with old firmware don't support DMI. We always return
-	 * 1 here, so at least first found MAC controller would be probed.
-	 */
-	if (!name)
-		return 1;
-
-	for (dmi = info->dmi; dmi->name && *dmi->name; dmi++) {
-		if (!strcmp(dmi->name, name) && dmi->func == func) {
-			/* If asset tag is provided, match on it as well. */
-			if (dmi->asset_tag && strcmp(dmi->asset_tag, asset_tag))
-				continue;
-			return dmi->phy_addr;
-		}
-	}
+	dmi_id = dmi_first_match(dmi_list);
+	if (!dmi_id)
+		return -ENODEV;
+
+	dmi_data = dmi_id->driver_data;
+	func_data = dmi_data->func;
+
+	for (n = 0; n < dmi_data->nfuncs; n++, func_data++)
+		if (func_data->func == func)
+			return func_data->phy_addr;
 
 	return -ENODEV;
 }
@@ -100,7 +97,8 @@ static void common_default_data(struct plat_stmmacenet_data *plat)
 	plat->rx_queues_cfg[0].pkt_route = 0x0;
 }
 
-static void stmmac_default_data(struct plat_stmmacenet_data *plat)
+static int stmmac_default_data(struct pci_dev *pdev,
+			       struct plat_stmmacenet_data *plat)
 {
 	/* Set common default data first */
 	common_default_data(plat);
@@ -112,12 +110,77 @@ static void stmmac_default_data(struct plat_stmmacenet_data *plat)
 	plat->dma_cfg->pbl = 32;
 	plat->dma_cfg->pblx8 = true;
 	/* TODO: AXI */
+
+	return 0;
 }
 
-static int quark_default_data(struct plat_stmmacenet_data *plat,
-			      struct stmmac_pci_info *info)
+static const struct stmmac_pci_info stmmac_pci_info = {
+	.setup = stmmac_default_data,
+};
+
+static const struct stmmac_pci_func_data galileo_stmmac_func_data[] = {
+	{
+		.func = 6,
+		.phy_addr = 1,
+	},
+};
+
+static const struct stmmac_pci_dmi_data galileo_stmmac_dmi_data = {
+	.func = galileo_stmmac_func_data,
+	.nfuncs = ARRAY_SIZE(galileo_stmmac_func_data),
+};
+
+static const struct stmmac_pci_func_data iot2040_stmmac_func_data[] = {
+	{
+		.func = 6,
+		.phy_addr = 1,
+	},
+	{
+		.func = 7,
+		.phy_addr = 1,
+	},
+};
+
+static const struct stmmac_pci_dmi_data iot2040_stmmac_dmi_data = {
+	.func = iot2040_stmmac_func_data,
+	.nfuncs = ARRAY_SIZE(iot2040_stmmac_func_data),
+};
+
+static const struct dmi_system_id quark_pci_dmi[] = {
+	{
+		.matches = {
+			DMI_EXACT_MATCH(DMI_BOARD_NAME, "Galileo"),
+		},
+		.driver_data = (void *)&galileo_stmmac_dmi_data,
+	},
+	{
+		.matches = {
+			DMI_EXACT_MATCH(DMI_BOARD_NAME, "GalileoGen2"),
+		},
+		.driver_data = (void *)&galileo_stmmac_dmi_data,
+	},
+	{
+		.matches = {
+			DMI_EXACT_MATCH(DMI_BOARD_NAME, "SIMATIC IOT2000"),
+			DMI_EXACT_MATCH(DMI_BOARD_ASSET_TAG,
+					"6ES7647-0AA00-0YA2"),
+		},
+		.driver_data = (void *)&galileo_stmmac_dmi_data,
+	},
+	{
+		.matches = {
+			DMI_EXACT_MATCH(DMI_BOARD_NAME, "SIMATIC IOT2000"),
+			DMI_EXACT_MATCH(DMI_BOARD_ASSET_TAG,
+					"6ES7647-0AA00-1YA2"),
+		},
+		.driver_data = (void *)&iot2040_stmmac_dmi_data,
+	},
+	{}
+};
+
+static int quark_default_data(struct pci_dev *pdev,
+			      struct plat_stmmacenet_data *plat)
 {
-	struct pci_dev *pdev = info->pdev;
 	int ret;
 
 	/* Set common default data first */
@@ -127,9 +190,19 @@ static int quark_default_data(struct plat_stmmacenet_data *plat,
 	 * Refuse to load the driver and register net device if MAC controller
 	 * does not connect to any PHY interface.
 	 */
-	ret = stmmac_pci_find_phy_addr(info);
-	if (ret < 0)
-		return ret;
+	ret = stmmac_pci_find_phy_addr(pdev, quark_pci_dmi);
+	if (ret < 0) {
+		/* Return error to the caller on DMI enabled boards. */
+		if (dmi_get_system_info(DMI_BOARD_NAME))
+			return ret;
+
+		/*
+		 * Galileo boards with old firmware don't support DMI. We always
+		 * use 1 here as PHY address, so at least the first found MAC
+		 * controller would be probed.
+		 */
+		ret = 1;
+	}
 
 	plat->bus_id = PCI_DEVID(pdev->bus->number, pdev->devfn);
 	plat->phy_addr = ret;
@@ -143,41 +216,8 @@ static int quark_default_data(struct plat_stmmacenet_data *plat,
 	return 0;
 }
 
-static struct stmmac_pci_dmi_data quark_pci_dmi_data[] = {
-	{
-		.name = "Galileo",
-		.func = 6,
-		.phy_addr = 1,
-	},
-	{
-		.name = "GalileoGen2",
-		.func = 6,
-		.phy_addr = 1,
-	},
-	{
-		.name = "SIMATIC IOT2000",
-		.asset_tag = "6ES7647-0AA00-0YA2",
-		.func = 6,
-		.phy_addr = 1,
-	},
-	{
-		.name = "SIMATIC IOT2000",
-		.asset_tag = "6ES7647-0AA00-1YA2",
-		.func = 6,
-		.phy_addr = 1,
-	},
-	{
-		.name = "SIMATIC IOT2000",
-		.asset_tag = "6ES7647-0AA00-1YA2",
-		.func = 7,
-		.phy_addr = 1,
-	},
-	{}
-};
-
-static struct stmmac_pci_info quark_pci_info = {
+static const struct stmmac_pci_info quark_pci_info = {
 	.setup = quark_default_data,
-	.dmi = quark_pci_dmi_data,
 };
 
 /**
@@ -236,15 +276,9 @@ static int stmmac_pci_probe(struct pci_dev *pdev,
 
 	pci_set_master(pdev);
 
-	if (info) {
-		info->pdev = pdev;
-		if (info->setup) {
-			ret = info->setup(plat, info);
-			if (ret)
-				return ret;
-		}
-	} else
-		stmmac_default_data(plat);
+	ret = info->setup(pdev, plat);
+	if (ret)
+		return ret;
 
 	pci_enable_msi(pdev);
 
@@ -270,14 +304,21 @@ static void stmmac_pci_remove(struct pci_dev *pdev)
 
 static SIMPLE_DEV_PM_OPS(stmmac_pm_ops, stmmac_suspend, stmmac_resume);
 
-#define STMMAC_VENDOR_ID 0x700
+/* synthetic ID, no official vendor */
+#define PCI_VENDOR_ID_STMMAC 0x700
+
 #define STMMAC_QUARK_ID  0x0937
 #define STMMAC_DEVICE_ID 0x1108
 
+#define STMMAC_DEVICE(vendor_id, dev_id, info)	{	\
+	PCI_VDEVICE(vendor_id, dev_id),			\
+	.driver_data = (kernel_ulong_t)&info		\
+	}
+
 static const struct pci_device_id stmmac_id_table[] = {
-	{PCI_DEVICE(STMMAC_VENDOR_ID, STMMAC_DEVICE_ID)},
-	{PCI_DEVICE(PCI_VENDOR_ID_STMICRO, PCI_DEVICE_ID_STMICRO_MAC)},
-	{PCI_VDEVICE(INTEL, STMMAC_QUARK_ID), (kernel_ulong_t)&quark_pci_info},
+	STMMAC_DEVICE(STMMAC, STMMAC_DEVICE_ID, stmmac_pci_info),
+	STMMAC_DEVICE(STMICRO, PCI_DEVICE_ID_STMICRO_MAC, stmmac_pci_info),
+	STMMAC_DEVICE(INTEL, STMMAC_QUARK_ID, quark_pci_info),
 	{}
 };
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
index 7fc3a1ef395a..a366b3747eeb 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
@@ -309,6 +309,13 @@ static int stmmac_dt_phy(struct plat_stmmacenet_data *plat,
 			 struct device_node *np, struct device *dev)
 {
 	bool mdio = true;
+	static const struct of_device_id need_mdio_ids[] = {
+		{ .compatible = "snps,dwc-qos-ethernet-4.10" },
+		{ .compatible = "allwinner,sun8i-a83t-emac" },
+		{ .compatible = "allwinner,sun8i-h3-emac" },
+		{ .compatible = "allwinner,sun8i-v3s-emac" },
+		{ .compatible = "allwinner,sun50i-a64-emac" },
+	};
 
 	/* If phy-handle property is passed from DT, use it as the PHY */
 	plat->phy_node = of_parse_phandle(np, "phy-handle", 0);
@@ -325,8 +332,7 @@ static int stmmac_dt_phy(struct plat_stmmacenet_data *plat,
 		mdio = false;
 	}
 
-	/* exception for dwmac-dwc-qos-eth glue logic */
-	if (of_device_is_compatible(np, "snps,dwc-qos-ethernet-4.10")) {
+	if (of_match_node(need_mdio_ids, np)) {
 		plat->mdio_node = of_get_child_by_name(np, "mdio");
 	} else {
 		/**
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h
index 48fb72fc423c..f4b31d69f60e 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h
@@ -59,7 +59,8 @@
 /* Enable Snapshot for Messages Relevant to Master */
 #define	PTP_TCR_TSMSTRENA	BIT(15)
 /* Select PTP packets for Taking Snapshots */
-#define	PTP_TCR_SNAPTYPSEL_1	GENMASK(17, 16)
+#define	PTP_TCR_SNAPTYPSEL_1	BIT(16)
+#define	PTP_GMAC4_TCR_SNAPTYPSEL_1	GENMASK(17, 16)
 /* Enable MAC address for PTP Frame Filtering */
 #define	PTP_TCR_TSENMACADDR	BIT(18)
 
diff --git a/drivers/net/ethernet/sun/ldmvsw.c b/drivers/net/ethernet/sun/ldmvsw.c
index 5b56c24b6ed2..8603e397097e 100644
--- a/drivers/net/ethernet/sun/ldmvsw.c
+++ b/drivers/net/ethernet/sun/ldmvsw.c
@@ -248,7 +248,7 @@ static struct net_device *vsw_alloc_netdev(u8 hwaddr[],
 	dev->ethtool_ops = &vsw_ethtool_ops;
 	dev->watchdog_timeo = VSW_TX_TIMEOUT;
 
-	dev->hw_features = NETIF_F_HW_CSUM | NETIF_F_SG;
+	dev->hw_features = NETIF_F_IP_CSUM | NETIF_F_SG;
 	dev->features = dev->hw_features;
 
 	/* MTU range: 68 - 65535 */
diff --git a/drivers/net/ethernet/sun/niu.c b/drivers/net/ethernet/sun/niu.c
index 2dcca249eb9c..46cb7f8955a2 100644
--- a/drivers/net/ethernet/sun/niu.c
+++ b/drivers/net/ethernet/sun/niu.c
@@ -6667,7 +6667,7 @@ static netdev_tx_t niu_start_xmit(struct sk_buff *skb,
 	headroom = align + sizeof(struct tx_pkt_hdr);
 
 	ehdr = (struct ethhdr *) skb->data;
-	tp = (struct tx_pkt_hdr *) skb_push(skb, headroom);
+	tp = skb_push(skb, headroom);
 
 	len = skb->len - sizeof(struct tx_pkt_hdr);
 	tp->flags = cpu_to_le64(niu_compute_tx_flags(skb, ehdr, align, len));
diff --git a/drivers/net/ethernet/sun/sunvnet.c b/drivers/net/ethernet/sun/sunvnet.c
index 0b95105f7060..75b167e3fe98 100644
--- a/drivers/net/ethernet/sun/sunvnet.c
+++ b/drivers/net/ethernet/sun/sunvnet.c
@@ -312,7 +312,7 @@ static struct vnet *vnet_new(const u64 *local_mac,
 	dev->watchdog_timeo = VNET_TX_TIMEOUT;
 
 	dev->hw_features = NETIF_F_TSO | NETIF_F_GSO | NETIF_F_GSO_SOFTWARE |
-			   NETIF_F_HW_CSUM | NETIF_F_SG;
+			   NETIF_F_IP_CSUM | NETIF_F_SG;
 	dev->features = dev->hw_features;
 
 	/* MTU range: 68 - 65535 */
diff --git a/drivers/net/ethernet/ti/cpsw-common.c b/drivers/net/ethernet/ti/cpsw-common.c
index 1562ab4151e1..56ba411421f0 100644
--- a/drivers/net/ethernet/ti/cpsw-common.c
+++ b/drivers/net/ethernet/ti/cpsw-common.c
@@ -90,7 +90,7 @@ int ti_cm_get_macid(struct device *dev, int slave, u8 *mac_addr)
 	if (of_device_is_compatible(dev->of_node, "ti,dm816-emac"))
 		return cpsw_am33xx_cm_get_macid(dev, 0x30, slave, mac_addr);
 
-	if (of_machine_is_compatible("ti,am4372"))
+	if (of_machine_is_compatible("ti,am43"))
 		return cpsw_am33xx_cm_get_macid(dev, 0x630, slave, mac_addr);
 
 	if (of_machine_is_compatible("ti,dra7"))
diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index f4d7aec50479..1850e348f555 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -1236,6 +1236,7 @@ static inline int cpsw_tx_packet_submit(struct cpsw_priv *priv,
 {
 	struct cpsw_common *cpsw = priv->cpsw;
 
+	skb_tx_timestamp(skb);
 	return cpdma_chan_submit(txch, skb, skb->data, skb->len,
 				 priv->emac_port + cpsw->data.dual_emac);
 }
@@ -1597,6 +1598,7 @@ static netdev_tx_t cpsw_ndo_start_xmit(struct sk_buff *skb,
 {
 	struct cpsw_priv *priv = netdev_priv(ndev);
 	struct cpsw_common *cpsw = priv->cpsw;
+	struct cpts *cpts = cpsw->cpts;
 	struct netdev_queue *txq;
 	struct cpdma_chan *txch;
 	int ret, q_idx;
@@ -1608,11 +1610,9 @@ static netdev_tx_t cpsw_ndo_start_xmit(struct sk_buff *skb,
 	}
 
 	if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP &&
-	    cpts_is_tx_enabled(cpsw->cpts))
+	    cpts_is_tx_enabled(cpts) && cpts_can_timestamp(cpts, skb))
 		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 
-	skb_tx_timestamp(skb);
-
 	q_idx = skb_get_queue_mapping(skb);
 	if (q_idx >= cpsw->tx_ch_num)
 		q_idx = q_idx % cpsw->tx_ch_num;
@@ -1731,10 +1731,14 @@ static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 		cpts_rx_enable(cpts, 0);
 		break;
 	case HWTSTAMP_FILTER_ALL:
+	case HWTSTAMP_FILTER_NTP_ALL:
+		return -ERANGE;
 	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
 	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
 	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
-		return -ERANGE;
+		cpts_rx_enable(cpts, HWTSTAMP_FILTER_PTP_V1_L4_EVENT);
+		cfg.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
+		break;
 	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
 	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
@@ -1744,7 +1748,7 @@ static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
-		cpts_rx_enable(cpts, 1);
+		cpts_rx_enable(cpts, HWTSTAMP_FILTER_PTP_V2_EVENT);
 		cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 		break;
 	default:
@@ -1783,7 +1787,7 @@ static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
 	cfg.tx_type = cpts_is_tx_enabled(cpts) ?
 		      HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
 	cfg.rx_filter = (cpts_is_rx_enabled(cpts) ?
-			 HWTSTAMP_FILTER_PTP_V2_EVENT : HWTSTAMP_FILTER_NONE);
+			 cpts->rx_enable : HWTSTAMP_FILTER_NONE);
 
 	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
 }
@@ -2140,6 +2144,7 @@ static int cpsw_get_ts_info(struct net_device *ndev,
 		(1 << HWTSTAMP_TX_ON);
 	info->rx_filters =
 		(1 << HWTSTAMP_FILTER_NONE) |
+		(1 << HWTSTAMP_FILTER_PTP_V1_L4_EVENT) |
 		(1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
 	return 0;
 }
@@ -2165,11 +2170,11 @@ static int cpsw_get_link_ksettings(struct net_device *ndev,
 	struct cpsw_common *cpsw = priv->cpsw;
 	int slave_no = cpsw_slave_index(cpsw, priv);
 
-	if (cpsw->slaves[slave_no].phy)
-		return phy_ethtool_ksettings_get(cpsw->slaves[slave_no].phy,
-						 ecmd);
-	else
+	if (!cpsw->slaves[slave_no].phy)
 		return -EOPNOTSUPP;
+
+	phy_ethtool_ksettings_get(cpsw->slaves[slave_no].phy, ecmd);
+	return 0;
 }
 
 static int cpsw_set_link_ksettings(struct net_device *ndev,
diff --git a/drivers/net/ethernet/ti/cpts.h b/drivers/net/ethernet/ti/cpts.h
index c96eca2b1b46..01ea82ba9cdc 100644
--- a/drivers/net/ethernet/ti/cpts.h
+++ b/drivers/net/ethernet/ti/cpts.h
@@ -30,6 +30,7 @@
 #include <linux/of.h>
 #include <linux/ptp_clock_kernel.h>
 #include <linux/skbuff.h>
+#include <linux/ptp_classify.h>
 #include <linux/timecounter.h>
 
 struct cpsw_cpts {
@@ -155,6 +156,16 @@ static inline bool cpts_is_tx_enabled(struct cpts *cpts)
 	return !!cpts->tx_enable;
 }
 
+static inline bool cpts_can_timestamp(struct cpts *cpts, struct sk_buff *skb)
+{
+	unsigned int class = ptp_classify_raw(skb);
+
+	if (class == PTP_CLASS_NONE)
+		return false;
+
+	return true;
+}
+
 #else
 struct cpts;
 
@@ -203,6 +214,11 @@ static inline bool cpts_is_tx_enabled(struct cpts *cpts)
 {
 	return false;
 }
+
+static inline bool cpts_can_timestamp(struct cpts *cpts, struct sk_buff *skb)
+{
+	return false;
+}
 #endif
 
 
diff --git a/drivers/net/ethernet/ti/davinci_cpdma.c b/drivers/net/ethernet/ti/davinci_cpdma.c
index 7ecc6b70e7e8..e4d6edf387b3 100644
--- a/drivers/net/ethernet/ti/davinci_cpdma.c
+++ b/drivers/net/ethernet/ti/davinci_cpdma.c
@@ -645,7 +645,7 @@ EXPORT_SYMBOL_GPL(cpdma_ctlr_destroy);
 int cpdma_ctlr_int_ctrl(struct cpdma_ctlr *ctlr, bool enable)
 {
 	unsigned long flags;
-	int i, reg;
+	int i;
 
 	spin_lock_irqsave(&ctlr->lock, flags);
 	if (ctlr->state != CPDMA_STATE_ACTIVE) {
@@ -653,9 +653,6 @@ int cpdma_ctlr_int_ctrl(struct cpdma_ctlr *ctlr, bool enable)
 		return -EINVAL;
 	}
 
-	reg = enable ? CPDMA_DMAINTMASKSET : CPDMA_DMAINTMASKCLEAR;
-	dma_reg_write(ctlr, reg, CPDMA_DMAINT_HOSTERR);
-
 	for (i = 0; i < ARRAY_SIZE(ctlr->channels); i++) {
 		if (ctlr->channels[i])
 			cpdma_chan_int_ctrl(ctlr->channels[i], enable);
diff --git a/drivers/net/ethernet/ti/netcp_core.c b/drivers/net/ethernet/ti/netcp_core.c
index e6222e535019..9d52c3a78621 100644
--- a/drivers/net/ethernet/ti/netcp_core.c
+++ b/drivers/net/ethernet/ti/netcp_core.c
@@ -1877,8 +1877,8 @@ static u16 netcp_select_queue(struct net_device *dev, struct sk_buff *skb,
 	return 0;
 }
 
-static int netcp_setup_tc(struct net_device *dev, u32 handle, __be16 proto,
-			  struct tc_to_netdev *tc)
+static int netcp_setup_tc(struct net_device *dev, u32 handle, u32 chain_index,
+			  __be16 proto, struct tc_to_netdev *tc)
 {
 	u8 num_tc;
 	int i;
diff --git a/drivers/net/ethernet/ti/netcp_ethss.c b/drivers/net/ethernet/ti/netcp_ethss.c
index dd92950a4615..28cb38af1a34 100644
--- a/drivers/net/ethernet/ti/netcp_ethss.c
+++ b/drivers/net/ethernet/ti/netcp_ethss.c
@@ -1927,7 +1927,6 @@ static int keystone_get_link_ksettings(struct net_device *ndev,
 	struct netcp_intf *netcp = netdev_priv(ndev);
 	struct phy_device *phy = ndev->phydev;
 	struct gbe_intf *gbe_intf;
-	int ret;
 
 	if (!phy)
 		return -EINVAL;
@@ -1939,11 +1938,10 @@ static int keystone_get_link_ksettings(struct net_device *ndev,
 	if (!gbe_intf->slave)
 		return -EINVAL;
 
-	ret = phy_ethtool_ksettings_get(phy, cmd);
-	if (!ret)
-		cmd->base.port = gbe_intf->slave->phy_port_t;
+	phy_ethtool_ksettings_get(phy, cmd);
+	cmd->base.port = gbe_intf->slave->phy_port_t;
 
-	return ret;
+	return 0;
 }
 
 static int keystone_set_link_ksettings(struct net_device *ndev,
@@ -2505,24 +2503,8 @@ static bool gbe_need_txtstamp(struct gbe_intf *gbe_intf,
 			      const struct netcp_packet *p_info)
 {
 	struct sk_buff *skb = p_info->skb;
-	unsigned int class = ptp_classify_raw(skb);
-
-	if (class == PTP_CLASS_NONE)
-		return false;
-
-	switch (class) {
-	case PTP_CLASS_V1_IPV4:
-	case PTP_CLASS_V1_IPV6:
-	case PTP_CLASS_V2_IPV4:
-	case PTP_CLASS_V2_IPV6:
-	case PTP_CLASS_V2_L2:
-	case (PTP_CLASS_V2_VLAN | PTP_CLASS_L2):
-	case (PTP_CLASS_V2_VLAN | PTP_CLASS_IPV4):
-	case (PTP_CLASS_V2_VLAN | PTP_CLASS_IPV6):
-		return true;
-	}
 
-	return false;
+	return cpts_can_timestamp(gbe_intf->gbe_dev->cpts, skb);
 }
 
 static int gbe_txtstamp_mark_pkt(struct gbe_intf *gbe_intf,
diff --git a/drivers/net/ethernet/tile/tilegx.c b/drivers/net/ethernet/tile/tilegx.c
index 7c634bc75615..aec95382ea5c 100644
--- a/drivers/net/ethernet/tile/tilegx.c
+++ b/drivers/net/ethernet/tile/tilegx.c
@@ -512,6 +512,7 @@ static int tile_hwtstamp_set(struct net_device *dev, struct ifreq *rq)
 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+	case HWTSTAMP_FILTER_NTP_ALL:
 		config.rx_filter = HWTSTAMP_FILTER_ALL;
 		break;
 	default:
diff --git a/drivers/net/ethernet/toshiba/ps3_gelic_net.c b/drivers/net/ethernet/toshiba/ps3_gelic_net.c
index fa6a06571187..88d74aef218a 100644
--- a/drivers/net/ethernet/toshiba/ps3_gelic_net.c
+++ b/drivers/net/ethernet/toshiba/ps3_gelic_net.c
@@ -754,7 +754,7 @@ static struct sk_buff *gelic_put_vlan_tag(struct sk_buff *skb,
 			return NULL;
 		dev_kfree_skb_any(sk_tmp);
 	}
-	veth = (struct vlan_ethhdr *)skb_push(skb, VLAN_HLEN);
+	veth = skb_push(skb, VLAN_HLEN);
 
 	/* Move the mac addresses to the top of buffer */
 	memmove(skb->data, skb->data + VLAN_HLEN, 2 * ETH_ALEN);
diff --git a/drivers/net/ethernet/tundra/tsi108_eth.c b/drivers/net/ethernet/tundra/tsi108_eth.c
index 5ac6eaa9e785..c2d15d9c0c33 100644
--- a/drivers/net/ethernet/tundra/tsi108_eth.c
+++ b/drivers/net/ethernet/tundra/tsi108_eth.c
@@ -1504,13 +1504,12 @@ static int tsi108_get_link_ksettings(struct net_device *dev,
 {
 	struct tsi108_prv_data *data = netdev_priv(dev);
 	unsigned long flags;
-	int rc;
 
 	spin_lock_irqsave(&data->txlock, flags);
-	rc = mii_ethtool_get_link_ksettings(&data->mii_if, cmd);
+	mii_ethtool_get_link_ksettings(&data->mii_if, cmd);
 	spin_unlock_irqrestore(&data->txlock, flags);
 
-	return rc;
+	return 0;
 }
 
 static int tsi108_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/ethernet/via/via-rhine.c b/drivers/net/ethernet/via/via-rhine.c
index 4cf41f779d0e..acd29d60174a 100644
--- a/drivers/net/ethernet/via/via-rhine.c
+++ b/drivers/net/ethernet/via/via-rhine.c
@@ -2307,13 +2307,12 @@ static int netdev_get_link_ksettings(struct net_device *dev,
 				     struct ethtool_link_ksettings *cmd)
 {
 	struct rhine_private *rp = netdev_priv(dev);
-	int rc;
 
 	mutex_lock(&rp->task_lock);
-	rc = mii_ethtool_get_link_ksettings(&rp->mii_if, cmd);
+	mii_ethtool_get_link_ksettings(&rp->mii_if, cmd);
 	mutex_unlock(&rp->task_lock);
 
-	return rc;
+	return 0;
 }
 
 static int netdev_set_link_ksettings(struct net_device *dev,
diff --git a/drivers/net/fjes/fjes_main.c b/drivers/net/fjes/fjes_main.c
index ae48c809bac9..750954be5a74 100644
--- a/drivers/net/fjes/fjes_main.c
+++ b/drivers/net/fjes/fjes_main.c
@@ -1156,8 +1156,7 @@ static int fjes_poll(struct napi_struct *napi, int budget)
 				hw->ep_shm_info[cur_epid].net_stats
 							 .rx_errors += 1;
 			} else {
-				memcpy(skb_put(skb, frame_len),
-				       frame, frame_len);
+				skb_put_data(skb, frame, frame_len);
 				skb->protocol = eth_type_trans(skb, netdev);
 				skb->ip_summed = CHECKSUM_UNNECESSARY;
 
@@ -1348,7 +1347,6 @@ static void fjes_netdev_setup(struct net_device *netdev)
 	netdev->mtu = fjes_support_mtu[3];
 	netdev->min_mtu = fjes_support_mtu[0];
 	netdev->max_mtu = fjes_support_mtu[3];
-	netdev->flags |= IFF_BROADCAST;
 	netdev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
 }
 
diff --git a/drivers/net/geneve.c b/drivers/net/geneve.c
index 6ebb0f559a42..de8156c6b292 100644
--- a/drivers/net/geneve.c
+++ b/drivers/net/geneve.c
@@ -45,9 +45,17 @@ struct geneve_net {
 
 static unsigned int geneve_net_id;
 
+struct geneve_dev_node {
+	struct hlist_node hlist;
+	struct geneve_dev *geneve;
+};
+
 /* Pseudo network device */
 struct geneve_dev {
-	struct hlist_node  hlist;	/* vni hash table */
+	struct geneve_dev_node hlist4;	/* vni hash table for IPv4 socket */
+#if IS_ENABLED(CONFIG_IPV6)
+	struct geneve_dev_node hlist6;	/* vni hash table for IPv6 socket */
+#endif
 	struct net	   *net;	/* netns for packet i/o */
 	struct net_device  *dev;	/* netdev for geneve tunnel */
 	struct ip_tunnel_info info;
@@ -123,16 +131,16 @@ static struct geneve_dev *geneve_lookup(struct geneve_sock *gs,
 					__be32 addr, u8 vni[])
 {
 	struct hlist_head *vni_list_head;
-	struct geneve_dev *geneve;
+	struct geneve_dev_node *node;
 	__u32 hash;
 
 	/* Find the device for this VNI */
 	hash = geneve_net_vni_hash(vni);
 	vni_list_head = &gs->vni_list[hash];
-	hlist_for_each_entry_rcu(geneve, vni_list_head, hlist) {
-		if (eq_tun_id_and_vni((u8 *)&geneve->info.key.tun_id, vni) &&
-		    addr == geneve->info.key.u.ipv4.dst)
-			return geneve;
+	hlist_for_each_entry_rcu(node, vni_list_head, hlist) {
+		if (eq_tun_id_and_vni((u8 *)&node->geneve->info.key.tun_id, vni) &&
+		    addr == node->geneve->info.key.u.ipv4.dst)
+			return node->geneve;
 	}
 	return NULL;
 }
@@ -142,16 +150,16 @@ static struct geneve_dev *geneve6_lookup(struct geneve_sock *gs,
 					 struct in6_addr addr6, u8 vni[])
 {
 	struct hlist_head *vni_list_head;
-	struct geneve_dev *geneve;
+	struct geneve_dev_node *node;
 	__u32 hash;
 
 	/* Find the device for this VNI */
 	hash = geneve_net_vni_hash(vni);
 	vni_list_head = &gs->vni_list[hash];
-	hlist_for_each_entry_rcu(geneve, vni_list_head, hlist) {
-		if (eq_tun_id_and_vni((u8 *)&geneve->info.key.tun_id, vni) &&
-		    ipv6_addr_equal(&addr6, &geneve->info.key.u.ipv6.dst))
-			return geneve;
+	hlist_for_each_entry_rcu(node, vni_list_head, hlist) {
+		if (eq_tun_id_and_vni((u8 *)&node->geneve->info.key.tun_id, vni) &&
+		    ipv6_addr_equal(&addr6, &node->geneve->info.key.u.ipv6.dst))
+			return node->geneve;
 	}
 	return NULL;
 }
@@ -212,6 +220,7 @@ static void geneve_rx(struct geneve_dev *geneve, struct geneve_sock *gs,
 	struct genevehdr *gnvh = geneve_hdr(skb);
 	struct metadata_dst *tun_dst = NULL;
 	struct pcpu_sw_netstats *stats;
+	unsigned int len;
 	int err = 0;
 	void *oiph;
 
@@ -225,8 +234,10 @@ static void geneve_rx(struct geneve_dev *geneve, struct geneve_sock *gs,
 		tun_dst = udp_tun_rx_dst(skb, geneve_get_sk_family(gs), flags,
 					 vni_to_tunnel_id(gnvh->vni),
 					 gnvh->opt_len * 4);
-		if (!tun_dst)
+		if (!tun_dst) {
+			geneve->dev->stats.rx_dropped++;
 			goto drop;
+		}
 		/* Update tunnel dst according to Geneve options. */
 		ip_tunnel_info_opts_set(&tun_dst->u.tun_info,
 					gnvh->options, gnvh->opt_len * 4);
@@ -234,8 +245,11 @@ static void geneve_rx(struct geneve_dev *geneve, struct geneve_sock *gs,
 		/* Drop packets w/ critical options,
 		 * since we don't support any...
 		 */
-		if (gnvh->critical)
+		if (gnvh->critical) {
+			geneve->dev->stats.rx_frame_errors++;
+			geneve->dev->stats.rx_errors++;
 			goto drop;
+		}
 	}
 
 	skb_reset_mac_header(skb);
@@ -246,8 +260,10 @@ static void geneve_rx(struct geneve_dev *geneve, struct geneve_sock *gs,
 		skb_dst_set(skb, &tun_dst->dst);
 
 	/* Ignore packet loops (and multicast echo) */
-	if (ether_addr_equal(eth_hdr(skb)->h_source, geneve->dev->dev_addr))
+	if (ether_addr_equal(eth_hdr(skb)->h_source, geneve->dev->dev_addr)) {
+		geneve->dev->stats.rx_errors++;
 		goto drop;
+	}
 
 	oiph = skb_network_header(skb);
 	skb_reset_network_header(skb);
@@ -279,13 +295,15 @@ static void geneve_rx(struct geneve_dev *geneve, struct geneve_sock *gs,
 		}
 	}
 
-	stats = this_cpu_ptr(geneve->dev->tstats);
-	u64_stats_update_begin(&stats->syncp);
-	stats->rx_packets++;
-	stats->rx_bytes += skb->len;
-	u64_stats_update_end(&stats->syncp);
-
-	gro_cells_receive(&geneve->gro_cells, skb);
+	len = skb->len;
+	err = gro_cells_receive(&geneve->gro_cells, skb);
+	if (likely(err == NET_RX_SUCCESS)) {
+		stats = this_cpu_ptr(geneve->dev->tstats);
+		u64_stats_update_begin(&stats->syncp);
+		stats->rx_packets++;
+		stats->rx_bytes += len;
+		u64_stats_update_end(&stats->syncp);
+	}
 	return;
 drop:
 	/* Consume bad packet */
@@ -334,7 +352,7 @@ static int geneve_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
 	struct geneve_sock *gs;
 	int opts_len;
 
-	/* Need Geneve and inner Ethernet header to be present */
+	/* Need UDP and Geneve header to be present */
 	if (unlikely(!pskb_may_pull(skb, GENEVE_BASE_HLEN)))
 		goto drop;
 
@@ -357,8 +375,10 @@ static int geneve_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
 	opts_len = geneveh->opt_len * 4;
 	if (iptunnel_pull_header(skb, GENEVE_BASE_HLEN + opts_len,
 				 htons(ETH_P_TEB),
-				 !net_eq(geneve->net, dev_net(geneve->dev))))
+				 !net_eq(geneve->net, dev_net(geneve->dev)))) {
+		geneve->dev->stats.rx_dropped++;
 		goto drop;
+	}
 
 	geneve_rx(geneve, gs, skb);
 	return 0;
@@ -579,6 +599,7 @@ static int geneve_sock_add(struct geneve_dev *geneve, bool ipv6)
 {
 	struct net *net = geneve->net;
 	struct geneve_net *gn = net_generic(net, geneve_net_id);
+	struct geneve_dev_node *node;
 	struct geneve_sock *gs;
 	__u8 vni[3];
 	__u32 hash;
@@ -597,15 +618,20 @@ static int geneve_sock_add(struct geneve_dev *geneve, bool ipv6)
 out:
 	gs->collect_md = geneve->collect_md;
 #if IS_ENABLED(CONFIG_IPV6)
-	if (ipv6)
+	if (ipv6) {
 		rcu_assign_pointer(geneve->sock6, gs);
-	else
+		node = &geneve->hlist6;
+	} else
 #endif
+	{
 		rcu_assign_pointer(geneve->sock4, gs);
+		node = &geneve->hlist4;
+	}
+	node->geneve = geneve;
 
 	tunnel_id_to_vni(geneve->info.key.tun_id, vni);
 	hash = geneve_net_vni_hash(vni);
-	hlist_add_head_rcu(&geneve->hlist, &gs->vni_list[hash]);
+	hlist_add_head_rcu(&node->hlist, &gs->vni_list[hash]);
 	return 0;
 }
 
@@ -632,8 +658,10 @@ static int geneve_stop(struct net_device *dev)
 {
 	struct geneve_dev *geneve = netdev_priv(dev);
 
-	if (!hlist_unhashed(&geneve->hlist))
-		hlist_del_rcu(&geneve->hlist);
+	hlist_del_init_rcu(&geneve->hlist4.hlist);
+#if IS_ENABLED(CONFIG_IPV6)
+	hlist_del_init_rcu(&geneve->hlist6.hlist);
+#endif
 	geneve_sock_release(geneve);
 	return 0;
 }
@@ -675,8 +703,7 @@ static int geneve_build_skb(struct dst_entry *dst, struct sk_buff *skb,
 	if (err)
 		goto free_dst;
 
-	gnvh = (struct genevehdr *)__skb_push(skb, sizeof(*gnvh) +
-						   info->options_len);
+	gnvh = __skb_push(skb, sizeof(*gnvh) + info->options_len);
 	geneve_build_header(gnvh, info);
 	skb_set_inner_protocol(skb, htons(ETH_P_TEB));
 	return 0;
@@ -1007,7 +1034,7 @@ static void geneve_setup(struct net_device *dev)
 
 	dev->netdev_ops = &geneve_netdev_ops;
 	dev->ethtool_ops = &geneve_ethtool_ops;
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 
 	SET_NETDEV_DEVTYPE(dev, &geneve_type);
 
@@ -1047,7 +1074,8 @@ static const struct nla_policy geneve_policy[IFLA_GENEVE_MAX + 1] = {
 	[IFLA_GENEVE_UDP_ZERO_CSUM6_RX]	= { .type = NLA_U8 },
 };
 
-static int geneve_validate(struct nlattr *tb[], struct nlattr *data[])
+static int geneve_validate(struct nlattr *tb[], struct nlattr *data[],
+			   struct netlink_ext_ack *extack)
 {
 	if (tb[IFLA_ADDRESS]) {
 		if (nla_len(tb[IFLA_ADDRESS]) != ETH_ALEN)
@@ -1170,7 +1198,8 @@ static void init_tnl_info(struct ip_tunnel_info *info, __u16 dst_port)
 }
 
 static int geneve_newlink(struct net *net, struct net_device *dev,
-			  struct nlattr *tb[], struct nlattr *data[])
+			  struct nlattr *tb[], struct nlattr *data[],
+			  struct netlink_ext_ack *extack)
 {
 	bool use_udp6_rx_checksums = false;
 	struct ip_tunnel_info info;
diff --git a/drivers/net/gtp.c b/drivers/net/gtp.c
index 7b652bb7ebe4..1542e837fdfa 100644
--- a/drivers/net/gtp.c
+++ b/drivers/net/gtp.c
@@ -398,7 +398,7 @@ static inline void gtp0_push_header(struct sk_buff *skb, struct pdp_ctx *pctx)
 	int payload_len = skb->len;
 	struct gtp0_header *gtp0;
 
-	gtp0 = (struct gtp0_header *) skb_push(skb, sizeof(*gtp0));
+	gtp0 = skb_push(skb, sizeof(*gtp0));
 
 	gtp0->flags	= 0x1e; /* v0, GTP-non-prime. */
 	gtp0->type	= GTP_TPDU;
@@ -415,7 +415,7 @@ static inline void gtp1_push_header(struct sk_buff *skb, struct pdp_ctx *pctx)
 	int payload_len = skb->len;
 	struct gtp1_header *gtp1;
 
-	gtp1 = (struct gtp1_header *) skb_push(skb, sizeof(*gtp1));
+	gtp1 = skb_push(skb, sizeof(*gtp1));
 
 	/* Bits    8  7  6  5  4  3  2	1
 	 *	  +--+--+--+--+--+--+--+--+
@@ -611,7 +611,7 @@ static const struct net_device_ops gtp_netdev_ops = {
 static void gtp_link_setup(struct net_device *dev)
 {
 	dev->netdev_ops		= &gtp_netdev_ops;
-	dev->destructor		= free_netdev;
+	dev->needs_free_netdev	= true;
 
 	dev->hard_header_len = 0;
 	dev->addr_len = 0;
@@ -636,7 +636,8 @@ static void gtp_hashtable_free(struct gtp_dev *gtp);
 static int gtp_encap_enable(struct gtp_dev *gtp, struct nlattr *data[]);
 
 static int gtp_newlink(struct net *src_net, struct net_device *dev,
-			struct nlattr *tb[], struct nlattr *data[])
+		       struct nlattr *tb[], struct nlattr *data[],
+		       struct netlink_ext_ack *extack)
 {
 	struct gtp_dev *gtp;
 	struct gtp_net *gn;
@@ -697,7 +698,8 @@ static const struct nla_policy gtp_policy[IFLA_GTP_MAX + 1] = {
 	[IFLA_GTP_ROLE]			= { .type = NLA_U32 },
 };
 
-static int gtp_validate(struct nlattr *tb[], struct nlattr *data[])
+static int gtp_validate(struct nlattr *tb[], struct nlattr *data[],
+			struct netlink_ext_ack *extack)
 {
 	if (!data)
 		return -EINVAL;
diff --git a/drivers/net/hamradio/6pack.c b/drivers/net/hamradio/6pack.c
index 922bf440e9f1..021a8ec411ab 100644
--- a/drivers/net/hamradio/6pack.c
+++ b/drivers/net/hamradio/6pack.c
@@ -311,7 +311,7 @@ static void sp_setup(struct net_device *dev)
 {
 	/* Finish setting up the DEVICE info. */
 	dev->netdev_ops		= &sp_netdev_ops;
-	dev->destructor		= free_netdev;
+	dev->needs_free_netdev	= true;
 	dev->mtu		= SIXP_MTU;
 	dev->hard_header_len	= AX25_MAX_HEADER_LEN;
 	dev->header_ops 	= &ax25_header_ops;
diff --git a/drivers/net/hamradio/bpqether.c b/drivers/net/hamradio/bpqether.c
index f62e7f325cf9..78a6414c5fd9 100644
--- a/drivers/net/hamradio/bpqether.c
+++ b/drivers/net/hamradio/bpqether.c
@@ -476,7 +476,7 @@ static const struct net_device_ops bpq_netdev_ops = {
 static void bpq_setup(struct net_device *dev)
 {
 	dev->netdev_ops	     = &bpq_netdev_ops;
-	dev->destructor	     = free_netdev;
+	dev->needs_free_netdev = true;
 
 	memcpy(dev->broadcast, &ax25_bcast, AX25_ADDR_LEN);
 	memcpy(dev->dev_addr,  &ax25_defaddr, AX25_ADDR_LEN);
diff --git a/drivers/net/hamradio/mkiss.c b/drivers/net/hamradio/mkiss.c
index 4a40a3d825b4..aec6c26563cf 100644
--- a/drivers/net/hamradio/mkiss.c
+++ b/drivers/net/hamradio/mkiss.c
@@ -298,7 +298,7 @@ static void ax_bump(struct mkiss *ax)
 		return;
 	}
 
-	memcpy(skb_put(skb,count), ax->rbuff, count);
+	skb_put_data(skb, ax->rbuff, count);
 	skb->protocol = ax25_type_trans(skb, ax->dev);
 	netif_rx(skb);
 	ax->dev->stats.rx_packets++;
diff --git a/drivers/net/hamradio/scc.c b/drivers/net/hamradio/scc.c
index 6754cd01c605..295f267b73ea 100644
--- a/drivers/net/hamradio/scc.c
+++ b/drivers/net/hamradio/scc.c
@@ -540,7 +540,7 @@ static inline void scc_rxint(struct scc_channel *scc)
 		}
 		
 		scc->rx_buff = skb;
-		*(skb_put(skb, 1)) = 0;	/* KISS data */
+		skb_put_u8(skb, 0);	/* KISS data */
 	}
 	
 	if (skb->len >= scc->stat.bufsize)
@@ -555,7 +555,7 @@ static inline void scc_rxint(struct scc_channel *scc)
 		return;
 	}
 
-	*(skb_put(skb, 1)) = Inb(scc->data);
+	skb_put_u8(skb, Inb(scc->data));
 }
 
 
diff --git a/drivers/net/hippi/rrunner.c b/drivers/net/hippi/rrunner.c
index 1ce6239a4849..71ddadbf2368 100644
--- a/drivers/net/hippi/rrunner.c
+++ b/drivers/net/hippi/rrunner.c
@@ -962,8 +962,8 @@ static void rx_int(struct net_device *dev, u32 rxlimit, u32 index)
 								    pkt_len,
 								    PCI_DMA_FROMDEVICE);
 
-					memcpy(skb_put(skb, pkt_len),
-					       rx_skb->data, pkt_len);
+					skb_put_data(skb, rx_skb->data,
+						     pkt_len);
 
 					pci_dma_sync_single_for_device(rrpriv->pci_dev,
 								       desc->addr.addrlo,
@@ -1422,7 +1422,7 @@ static netdev_tx_t rr_start_xmit(struct sk_buff *skb,
 		skb = new_skb;
 	}
 
-	ifield = (u32 *)skb_push(skb, 8);
+	ifield = skb_push(skb, 8);
 
 	ifield[0] = 0;
 	ifield[1] = hcb->ifield;
diff --git a/drivers/net/hyperv/hyperv_net.h b/drivers/net/hyperv/hyperv_net.h
index 262b2ea576a3..d6c25580f8dd 100644
--- a/drivers/net/hyperv/hyperv_net.h
+++ b/drivers/net/hyperv/hyperv_net.h
@@ -146,7 +146,6 @@ struct hv_netvsc_packet {
 
 struct netvsc_device_info {
 	unsigned char mac_adr[ETH_ALEN];
-	bool link_state;	/* 0 - link up, 1 - link down */
 	int  ring_size;
 	u32  max_num_vrss_chns;
 	u32  num_chn;
@@ -165,12 +164,16 @@ struct rndis_device {
 	struct net_device *ndev;
 
 	enum rndis_device_state state;
-	bool link_state;
+
 	atomic_t new_req_id;
 
 	spinlock_t request_lock;
 	struct list_head req_list;
 
+	struct work_struct mcast_work;
+
+	bool link_state;        /* 0 - link up, 1 - link down */
+
 	u8 hw_mac_adr[ETH_ALEN];
 	u8 rss_key[NETVSC_HASH_KEYLEN];
 	u16 ind_table[ITAB_NUM];
@@ -201,6 +204,7 @@ int rndis_filter_open(struct netvsc_device *nvdev);
 int rndis_filter_close(struct netvsc_device *nvdev);
 int rndis_filter_device_add(struct hv_device *dev,
 			    struct netvsc_device_info *info);
+void rndis_filter_update(struct netvsc_device *nvdev);
 void rndis_filter_device_remove(struct hv_device *dev,
 				struct netvsc_device *nvdev);
 int rndis_filter_set_rss_param(struct rndis_device *rdev,
@@ -211,7 +215,6 @@ int rndis_filter_receive(struct net_device *ndev,
 			 struct vmbus_channel *channel,
 			 void *data, u32 buflen);
 
-int rndis_filter_set_packet_filter(struct rndis_device *dev, u32 new_filter);
 int rndis_filter_set_device_mac(struct net_device *ndev, char *mac);
 
 void netvsc_switch_datapath(struct net_device *nv_dev, bool vf);
@@ -696,7 +699,6 @@ struct net_device_context {
 	/* list protection */
 	spinlock_t lock;
 
-	struct work_struct work;
 	u32 msg_enable; /* debug level */
 
 	u32 tx_checksum_mask;
@@ -715,6 +717,8 @@ struct net_device_context {
 	u32 vf_alloc;
 	/* Serial number of the VF to team with */
 	u32 vf_serial;
+
+	bool datapath;	/* 0 - synthetic, 1 - VF nic */
 };
 
 /* Per channel data */
@@ -763,8 +767,7 @@ struct netvsc_device {
 
 	refcount_t sc_offered;
 
-	/* Holds rndis device info */
-	void *extension;
+	struct rndis_device *extension;
 
 	int ring_size;
 
diff --git a/drivers/net/hyperv/netvsc.c b/drivers/net/hyperv/netvsc.c
index 652453d9fb08..0a9167dd72fb 100644
--- a/drivers/net/hyperv/netvsc.c
+++ b/drivers/net/hyperv/netvsc.c
@@ -57,6 +57,8 @@ void netvsc_switch_datapath(struct net_device *ndev, bool vf)
 			       sizeof(struct nvsp_message),
 			       (unsigned long)init_pkt,
 			       VM_PKT_DATA_INBAND, 0);
+
+	net_device_ctx->datapath = vf;
 }
 
 static struct netvsc_device *alloc_net_device(void)
@@ -97,16 +99,6 @@ static void free_netvsc_device_rcu(struct netvsc_device *nvdev)
 	call_rcu(&nvdev->rcu, free_netvsc_device);
 }
 
-static struct netvsc_device *get_outbound_net_device(struct hv_device *device)
-{
-	struct netvsc_device *net_device = hv_device_to_netvsc_device(device);
-
-	if (net_device && net_device->destroy)
-		net_device = NULL;
-
-	return net_device;
-}
-
 static void netvsc_destroy_buf(struct hv_device *device)
 {
 	struct nvsp_message *revoke_packet;
@@ -243,18 +235,15 @@ static void netvsc_destroy_buf(struct hv_device *device)
 	kfree(net_device->send_section_map);
 }
 
-static int netvsc_init_buf(struct hv_device *device)
+static int netvsc_init_buf(struct hv_device *device,
+			   struct netvsc_device *net_device)
 {
 	int ret = 0;
-	struct netvsc_device *net_device;
 	struct nvsp_message *init_packet;
 	struct net_device *ndev;
 	size_t map_words;
 	int node;
 
-	net_device = get_outbound_net_device(device);
-	if (!net_device)
-		return -ENODEV;
 	ndev = hv_get_drvdata(device);
 
 	node = cpu_to_node(device->channel->target_cpu);
@@ -285,9 +274,7 @@ static int netvsc_init_buf(struct hv_device *device)
 
 	/* Notify the NetVsp of the gpadl handle */
 	init_packet = &net_device->channel_init_pkt;
-
 	memset(init_packet, 0, sizeof(struct nvsp_message));
-
 	init_packet->hdr.msg_type = NVSP_MSG1_TYPE_SEND_RECV_BUF;
 	init_packet->msg.v1_msg.send_recv_buf.
 		gpadl_handle = net_device->recv_buf_gpadl_handle;
@@ -486,20 +473,15 @@ static int negotiate_nvsp_ver(struct hv_device *device,
 	return ret;
 }
 
-static int netvsc_connect_vsp(struct hv_device *device)
+static int netvsc_connect_vsp(struct hv_device *device,
+			      struct netvsc_device *net_device)
 {
-	int ret;
-	struct netvsc_device *net_device;
-	struct nvsp_message *init_packet;
-	int ndis_version;
 	const u32 ver_list[] = {
 		NVSP_PROTOCOL_VERSION_1, NVSP_PROTOCOL_VERSION_2,
-		NVSP_PROTOCOL_VERSION_4, NVSP_PROTOCOL_VERSION_5 };
-	int i;
-
-	net_device = get_outbound_net_device(device);
-	if (!net_device)
-		return -ENODEV;
+		NVSP_PROTOCOL_VERSION_4, NVSP_PROTOCOL_VERSION_5
+	};
+	struct nvsp_message *init_packet;
+	int ndis_version, i, ret;
 
 	init_packet = &net_device->channel_init_pkt;
 
@@ -549,7 +531,7 @@ static int netvsc_connect_vsp(struct hv_device *device)
 		net_device->recv_buf_size = NETVSC_RECEIVE_BUFFER_SIZE;
 	net_device->send_buf_size = NETVSC_SEND_BUFFER_SIZE;
 
-	ret = netvsc_init_buf(device);
+	ret = netvsc_init_buf(device, net_device);
 
 cleanup:
 	return ret;
@@ -843,7 +825,7 @@ int netvsc_send(struct hv_device *device,
 		struct hv_page_buffer **pb,
 		struct sk_buff *skb)
 {
-	struct netvsc_device *net_device;
+	struct netvsc_device *net_device = hv_device_to_netvsc_device(device);
 	int ret = 0;
 	struct netvsc_channel *nvchan;
 	u32 pktlen = packet->total_data_buflen, msd_len = 0;
@@ -854,15 +836,15 @@ int netvsc_send(struct hv_device *device,
 	bool try_batch;
 	bool xmit_more = (skb != NULL) ? skb->xmit_more : false;
 
-	net_device = get_outbound_net_device(device);
-	if (!net_device)
+	/* If device is rescinded, return error and packet will get dropped. */
+	if (unlikely(net_device->destroy))
 		return -ENODEV;
 
 	/* We may race with netvsc_connect_vsp()/netvsc_init_buf() and get
 	 * here before the negotiation with the host is finished and
 	 * send_section_map may not be allocated yet.
 	 */
-	if (!net_device->send_section_map)
+	if (unlikely(!net_device->send_section_map))
 		return -EAGAIN;
 
 	nvchan = &net_device->chan_table[packet->q_idx];
@@ -1349,7 +1331,7 @@ int netvsc_device_add(struct hv_device *device,
 	rcu_assign_pointer(net_device_ctx->nvdev, net_device);
 
 	/* Connect with the NetVsp */
-	ret = netvsc_connect_vsp(device);
+	ret = netvsc_connect_vsp(device, net_device);
 	if (ret != 0) {
 		netdev_err(ndev,
 			"unable to connect to NetVSP - %d\n", ret);
@@ -1368,4 +1350,5 @@ cleanup:
 	free_netvsc_device(&net_device->rcu);
 
 	return ret;
+
 }
diff --git a/drivers/net/hyperv/netvsc_drv.c b/drivers/net/hyperv/netvsc_drv.c
index 4421a6d00375..63c98bbbc596 100644
--- a/drivers/net/hyperv/netvsc_drv.c
+++ b/drivers/net/hyperv/netvsc_drv.c
@@ -37,6 +37,8 @@
 #include <net/route.h>
 #include <net/sock.h>
 #include <net/pkt_sched.h>
+#include <net/checksum.h>
+#include <net/ip6_checksum.h>
 
 #include "hyperv_net.h"
 
@@ -56,42 +58,18 @@ static int debug = -1;
 module_param(debug, int, S_IRUGO);
 MODULE_PARM_DESC(debug, "Debug level (0=none,...,16=all)");
 
-static void do_set_multicast(struct work_struct *w)
-{
-	struct net_device_context *ndevctx =
-		container_of(w, struct net_device_context, work);
-	struct hv_device *device_obj = ndevctx->device_ctx;
-	struct net_device *ndev = hv_get_drvdata(device_obj);
-	struct netvsc_device *nvdev = rcu_dereference(ndevctx->nvdev);
-	struct rndis_device *rdev;
-
-	if (!nvdev)
-		return;
-
-	rdev = nvdev->extension;
-	if (rdev == NULL)
-		return;
-
-	if (ndev->flags & IFF_PROMISC)
-		rndis_filter_set_packet_filter(rdev,
-			NDIS_PACKET_TYPE_PROMISCUOUS);
-	else
-		rndis_filter_set_packet_filter(rdev,
-			NDIS_PACKET_TYPE_BROADCAST |
-			NDIS_PACKET_TYPE_ALL_MULTICAST |
-			NDIS_PACKET_TYPE_DIRECTED);
-}
-
 static void netvsc_set_multicast_list(struct net_device *net)
 {
 	struct net_device_context *net_device_ctx = netdev_priv(net);
+	struct netvsc_device *nvdev = rtnl_dereference(net_device_ctx->nvdev);
 
-	schedule_work(&net_device_ctx->work);
+	rndis_filter_update(nvdev);
 }
 
 static int netvsc_open(struct net_device *net)
 {
-	struct netvsc_device *nvdev = net_device_to_netvsc_device(net);
+	struct net_device_context *ndev_ctx = netdev_priv(net);
+	struct netvsc_device *nvdev = ndev_ctx->nvdev;
 	struct rndis_device *rdev;
 	int ret = 0;
 
@@ -107,7 +85,7 @@ static int netvsc_open(struct net_device *net)
 	netif_tx_wake_all_queues(net);
 
 	rdev = nvdev->extension;
-	if (!rdev->link_state)
+	if (!rdev->link_state && !ndev_ctx->datapath)
 		netif_carrier_on(net);
 
 	return ret;
@@ -118,13 +96,11 @@ static int netvsc_close(struct net_device *net)
 	struct net_device_context *net_device_ctx = netdev_priv(net);
 	struct netvsc_device *nvdev = rtnl_dereference(net_device_ctx->nvdev);
 	int ret;
-	u32 aread, awrite, i, msec = 10, retry = 0, retry_max = 20;
+	u32 aread, i, msec = 10, retry = 0, retry_max = 20;
 	struct vmbus_channel *chn;
 
 	netif_tx_disable(net);
 
-	/* Make sure netvsc_set_multicast_list doesn't re-enable filter! */
-	cancel_work_sync(&net_device_ctx->work);
 	ret = rndis_filter_close(nvdev);
 	if (ret != 0) {
 		netdev_err(net, "unable to close device (ret %d).\n", ret);
@@ -139,15 +115,11 @@ static int netvsc_close(struct net_device *net)
 			if (!chn)
 				continue;
 
-			hv_get_ringbuffer_availbytes(&chn->inbound, &aread,
-						     &awrite);
-
+			aread = hv_get_bytes_to_read(&chn->inbound);
 			if (aread)
 				break;
 
-			hv_get_ringbuffer_availbytes(&chn->outbound, &aread,
-						     &awrite);
-
+			aread = hv_get_bytes_to_read(&chn->outbound);
 			if (aread)
 				break;
 		}
@@ -343,34 +315,14 @@ static u32 init_page_array(void *hdr, u32 len, struct sk_buff *skb,
 	return slots_used;
 }
 
-static int count_skb_frag_slots(struct sk_buff *skb)
-{
-	int i, frags = skb_shinfo(skb)->nr_frags;
-	int pages = 0;
-
-	for (i = 0; i < frags; i++) {
-		skb_frag_t *frag = skb_shinfo(skb)->frags + i;
-		unsigned long size = skb_frag_size(frag);
-		unsigned long offset = frag->page_offset;
-
-		/* Skip unused frames from start of page */
-		offset &= ~PAGE_MASK;
-		pages += PFN_UP(offset + size);
-	}
-	return pages;
-}
-
-static int netvsc_get_slots(struct sk_buff *skb)
+/* Estimate number of page buffers neede to transmit
+ * Need at most 2 for RNDIS header plus skb body and fragments.
+ */
+static unsigned int netvsc_get_slots(const struct sk_buff *skb)
 {
-	char *data = skb->data;
-	unsigned int offset = offset_in_page(data);
-	unsigned int len = skb_headlen(skb);
-	int slots;
-	int frag_slots;
-
-	slots = DIV_ROUND_UP(offset + len, PAGE_SIZE);
-	frag_slots = count_skb_frag_slots(skb);
-	return slots + frag_slots;
+	return PFN_UP(offset_in_page(skb->data) + skb_headlen(skb))
+		+ skb_shinfo(skb)->nr_frags
+		+ 2;
 }
 
 static u32 net_checksum_info(struct sk_buff *skb)
@@ -408,21 +360,18 @@ static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
 	struct hv_page_buffer page_buf[MAX_PAGE_BUFFER_COUNT];
 	struct hv_page_buffer *pb = page_buf;
 
-	/* We will atmost need two pages to describe the rndis
-	 * header. We can only transmit MAX_PAGE_BUFFER_COUNT number
+	/* We can only transmit MAX_PAGE_BUFFER_COUNT number
 	 * of pages in a single packet. If skb is scattered around
 	 * more pages we try linearizing it.
 	 */
-
-	num_data_pgs = netvsc_get_slots(skb) + 2;
-
+	num_data_pgs = netvsc_get_slots(skb);
 	if (unlikely(num_data_pgs > MAX_PAGE_BUFFER_COUNT)) {
 		++net_device_ctx->eth_stats.tx_scattered;
 
 		if (skb_linearize(skb))
 			goto no_memory;
 
-		num_data_pgs = netvsc_get_slots(skb) + 2;
+		num_data_pgs = netvsc_get_slots(skb);
 		if (num_data_pgs > MAX_PAGE_BUFFER_COUNT) {
 			++net_device_ctx->eth_stats.tx_too_big;
 			goto drop;
@@ -645,7 +594,7 @@ static struct sk_buff *netvsc_alloc_recv_skb(struct net_device *net,
 	 * Copy to skb. This copy is needed here since the memory pointed by
 	 * hv_netvsc_packet cannot be deallocated
 	 */
-	memcpy(skb_put(skb, buflen), data, buflen);
+	skb_put_data(skb, data, buflen);
 
 	skb->protocol = eth_type_trans(skb, net);
 
@@ -803,7 +752,7 @@ static int netvsc_set_channels(struct net_device *net,
 	    channels->rx_count || channels->tx_count || channels->other_count)
 		return -EINVAL;
 
-	if (count > net->num_tx_queues || count > net->num_rx_queues)
+	if (count > net->num_tx_queues || count > VRSS_CHANNEL_MAX)
 		return -EINVAL;
 
 	if (!nvdev || nvdev->destroy)
@@ -1028,7 +977,7 @@ static const struct {
 static int netvsc_get_sset_count(struct net_device *dev, int string_set)
 {
 	struct net_device_context *ndc = netdev_priv(dev);
-	struct netvsc_device *nvdev = rcu_dereference(ndc->nvdev);
+	struct netvsc_device *nvdev = rtnl_dereference(ndc->nvdev);
 
 	if (!nvdev)
 		return -ENODEV;
@@ -1158,11 +1107,22 @@ netvsc_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *info,
 }
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
-static void netvsc_poll_controller(struct net_device *net)
+static void netvsc_poll_controller(struct net_device *dev)
 {
-	/* As netvsc_start_xmit() works synchronous we don't have to
-	 * trigger anything here.
-	 */
+	struct net_device_context *ndc = netdev_priv(dev);
+	struct netvsc_device *ndev;
+	int i;
+
+	rcu_read_lock();
+	ndev = rcu_dereference(ndc->nvdev);
+	if (ndev) {
+		for (i = 0; i < ndev->num_chn; i++) {
+			struct netvsc_channel *nvchan = &ndev->chan_table[i];
+
+			napi_schedule(&nvchan->napi);
+		}
+	}
+	rcu_read_unlock();
 }
 #endif
 
@@ -1219,7 +1179,7 @@ static int netvsc_set_rxfh(struct net_device *dev, const u32 *indir,
 	rndis_dev = ndev->extension;
 	if (indir) {
 		for (i = 0; i < ITAB_NUM; i++)
-			if (indir[i] >= dev->num_rx_queues)
+			if (indir[i] >= VRSS_CHANNEL_MAX)
 				return -EINVAL;
 
 		for (i = 0; i < ITAB_NUM; i++)
@@ -1325,7 +1285,8 @@ static void netvsc_link_change(struct work_struct *w)
 	case RNDIS_STATUS_MEDIA_CONNECT:
 		if (rdev->link_state) {
 			rdev->link_state = false;
-			netif_carrier_on(net);
+			if (!ndev_ctx->datapath)
+				netif_carrier_on(net);
 			netif_tx_wake_all_queues(net);
 		} else {
 			notify = true;
@@ -1552,7 +1513,6 @@ static int netvsc_probe(struct hv_device *dev,
 	hv_set_drvdata(dev, net);
 
 	INIT_DELAYED_WORK(&net_device_ctx->dwork, netvsc_link_change);
-	INIT_WORK(&net_device_ctx->work, do_set_multicast);
 
 	spin_lock_init(&net_device_ctx->lock);
 	INIT_LIST_HEAD(&net_device_ctx->reconfig_events);
@@ -1622,7 +1582,6 @@ static int netvsc_remove(struct hv_device *dev)
 	netif_device_detach(net);
 
 	cancel_delayed_work_sync(&ndev_ctx->dwork);
-	cancel_work_sync(&ndev_ctx->work);
 
 	/*
 	 * Call to the vsc driver to let it know that the device is being
diff --git a/drivers/net/hyperv/rndis_filter.c b/drivers/net/hyperv/rndis_filter.c
index f9d5b0b8209a..85c00e1c52b6 100644
--- a/drivers/net/hyperv/rndis_filter.c
+++ b/drivers/net/hyperv/rndis_filter.c
@@ -31,6 +31,7 @@
 
 #include "hyperv_net.h"
 
+static void rndis_set_multicast(struct work_struct *w);
 
 #define RNDIS_EXT_LEN PAGE_SIZE
 struct rndis_request {
@@ -76,6 +77,7 @@ static struct rndis_device *get_rndis_device(void)
 	spin_lock_init(&device->request_lock);
 
 	INIT_LIST_HEAD(&device->req_list);
+	INIT_WORK(&device->mcast_work, rndis_set_multicast);
 
 	device->state = RNDIS_DEV_UNINITIALIZED;
 
@@ -815,7 +817,8 @@ static int rndis_filter_query_link_speed(struct rndis_device *dev)
 	return ret;
 }
 
-int rndis_filter_set_packet_filter(struct rndis_device *dev, u32 new_filter)
+static int rndis_filter_set_packet_filter(struct rndis_device *dev,
+					  u32 new_filter)
 {
 	struct rndis_request *request;
 	struct rndis_set_request *set;
@@ -846,6 +849,28 @@ int rndis_filter_set_packet_filter(struct rndis_device *dev, u32 new_filter)
 	return ret;
 }
 
+static void rndis_set_multicast(struct work_struct *w)
+{
+	struct rndis_device *rdev
+		= container_of(w, struct rndis_device, mcast_work);
+
+	if (rdev->ndev->flags & IFF_PROMISC)
+		rndis_filter_set_packet_filter(rdev,
+					       NDIS_PACKET_TYPE_PROMISCUOUS);
+	else
+		rndis_filter_set_packet_filter(rdev,
+					       NDIS_PACKET_TYPE_BROADCAST |
+					       NDIS_PACKET_TYPE_ALL_MULTICAST |
+					       NDIS_PACKET_TYPE_DIRECTED);
+}
+
+void rndis_filter_update(struct netvsc_device *nvdev)
+{
+	struct rndis_device *rdev = nvdev->extension;
+
+	schedule_work(&rdev->mcast_work);
+}
+
 static int rndis_filter_init_device(struct rndis_device *dev)
 {
 	struct rndis_request *request;
@@ -973,6 +998,9 @@ static int rndis_filter_close_device(struct rndis_device *dev)
 	if (dev->state != RNDIS_DEV_DATAINITIALIZED)
 		return 0;
 
+	/* Make sure rndis_set_multicast doesn't re-enable filter! */
+	cancel_work_sync(&dev->mcast_work);
+
 	ret = rndis_filter_set_packet_filter(dev, 0);
 	if (ret == -ENODEV)
 		ret = 0;
@@ -1157,11 +1185,9 @@ int rndis_filter_device_add(struct hv_device *dev,
 
 	rndis_filter_query_device_link_status(rndis_device);
 
-	device_info->link_state = rndis_device->link_state;
-
 	netdev_dbg(net, "Device MAC %pM link state %s\n",
 		   rndis_device->hw_mac_adr,
-		   device_info->link_state ? "down" : "up");
+		   rndis_device->link_state ? "down" : "up");
 
 	if (net_device->nvsp_version < NVSP_PROTOCOL_VERSION_5)
 		return 0;
diff --git a/drivers/net/ieee802154/at86rf230.c b/drivers/net/ieee802154/at86rf230.c
index 76ba7ecfe142..548d9d026a85 100644
--- a/drivers/net/ieee802154/at86rf230.c
+++ b/drivers/net/ieee802154/at86rf230.c
@@ -723,7 +723,7 @@ at86rf230_rx_read_frame_complete(void *context)
 		return;
 	}
 
-	memcpy(skb_put(skb, len), buf + 2, len);
+	skb_put_data(skb, buf + 2, len);
 	ieee802154_rx_irqsafe(lp->hw, skb, lqi);
 	kfree(ctx);
 }
diff --git a/drivers/net/ieee802154/ca8210.c b/drivers/net/ieee802154/ca8210.c
index 25fd3b04b3c0..a626c539fb17 100644
--- a/drivers/net/ieee802154/ca8210.c
+++ b/drivers/net/ieee802154/ca8210.c
@@ -912,7 +912,7 @@ static int ca8210_spi_transfer(
 )
 {
 	int i, status = 0;
-	struct ca8210_priv *priv = spi_get_drvdata(spi);
+	struct ca8210_priv *priv;
 	struct cas_control *cas_ctl;
 
 	if (!spi) {
@@ -923,6 +923,7 @@ static int ca8210_spi_transfer(
 		return -ENODEV;
 	}
 
+	priv = spi_get_drvdata(spi);
 	reinit_completion(&priv->spi_transfer_complete);
 
 	dev_dbg(&spi->dev, "ca8210_spi_transfer called\n");
@@ -1808,10 +1809,9 @@ static int ca8210_skb_rx(
 
 	/* Allocate mtu size buffer for every rx packet */
 	skb = dev_alloc_skb(IEEE802154_MTU + sizeof(hdr));
-	if (!skb) {
-		dev_crit(&priv->spi->dev, "dev_alloc_skb failed\n");
+	if (!skb)
 		return -ENOMEM;
-	}
+
 	skb_reserve(skb, sizeof(hdr));
 
 	msdulen = data_ind[22]; /* msdu_length */
@@ -1875,7 +1875,7 @@ static int ca8210_skb_rx(
 copy_payload:
 	/* Add <msdulen> bytes of space to the back of the buffer */
 	/* Copy msdu to skb */
-	memcpy(skb_put(skb, msdulen), &data_ind[29], msdulen);
+	skb_put_data(skb, &data_ind[29], msdulen);
 
 	ieee802154_rx_irqsafe(hw, skb, mpdulinkquality);
 	return 0;
@@ -3143,10 +3143,6 @@ static int ca8210_probe(struct spi_device *spi_device)
 
 	pdata = kmalloc(sizeof(*pdata), GFP_KERNEL);
 	if (!pdata) {
-		dev_crit(
-			&spi_device->dev,
-			"Could not allocate platform data\n"
-		);
 		ret = -ENOMEM;
 		goto error;
 	}
diff --git a/drivers/net/ieee802154/mrf24j40.c b/drivers/net/ieee802154/mrf24j40.c
index bd63289c55e8..7d334963dc08 100644
--- a/drivers/net/ieee802154/mrf24j40.c
+++ b/drivers/net/ieee802154/mrf24j40.c
@@ -774,7 +774,7 @@ static void mrf24j40_handle_rx_read_buf_complete(void *context)
 		return;
 	}
 
-	memcpy(skb_put(skb, len), rx_local_buf, len);
+	skb_put_data(skb, rx_local_buf, len);
 	ieee802154_rx_irqsafe(devrec->hw, skb, 0);
 
 #ifdef DEBUG
diff --git a/drivers/net/ifb.c b/drivers/net/ifb.c
index 312fce7302d3..8870bd2a2e8a 100644
--- a/drivers/net/ifb.c
+++ b/drivers/net/ifb.c
@@ -207,7 +207,6 @@ static void ifb_dev_free(struct net_device *dev)
 		__skb_queue_purge(&txp->tq);
 	}
 	kfree(dp->tx_private);
-	free_netdev(dev);
 }
 
 static void ifb_setup(struct net_device *dev)
@@ -230,7 +229,8 @@ static void ifb_setup(struct net_device *dev)
 	dev->priv_flags &= ~IFF_TX_SKB_SHARING;
 	netif_keep_dst(dev);
 	eth_hw_addr_random(dev);
-	dev->destructor = ifb_dev_free;
+	dev->needs_free_netdev = true;
+	dev->priv_destructor = ifb_dev_free;
 }
 
 static netdev_tx_t ifb_xmit(struct sk_buff *skb, struct net_device *dev)
@@ -273,7 +273,8 @@ static int ifb_open(struct net_device *dev)
 	return 0;
 }
 
-static int ifb_validate(struct nlattr *tb[], struct nlattr *data[])
+static int ifb_validate(struct nlattr *tb[], struct nlattr *data[],
+			struct netlink_ext_ack *extack)
 {
 	if (tb[IFLA_ADDRESS]) {
 		if (nla_len(tb[IFLA_ADDRESS]) != ETH_ALEN)
diff --git a/drivers/net/ipvlan/ipvlan.h b/drivers/net/ipvlan/ipvlan.h
index 7919369c0a72..ba8173a0b62e 100644
--- a/drivers/net/ipvlan/ipvlan.h
+++ b/drivers/net/ipvlan/ipvlan.h
@@ -140,7 +140,8 @@ unsigned int ipvlan_nf_input(void *priv, struct sk_buff *skb,
 void ipvlan_count_rx(const struct ipvl_dev *ipvlan,
 		     unsigned int len, bool success, bool mcast);
 int ipvlan_link_new(struct net *src_net, struct net_device *dev,
-		    struct nlattr *tb[], struct nlattr *data[]);
+		    struct nlattr *tb[], struct nlattr *data[],
+		    struct netlink_ext_ack *extack);
 void ipvlan_link_delete(struct net_device *dev, struct list_head *head);
 void ipvlan_link_setup(struct net_device *dev);
 int ipvlan_link_register(struct rtnl_link_ops *ops);
diff --git a/drivers/net/ipvlan/ipvlan_main.c b/drivers/net/ipvlan/ipvlan_main.c
index 618ed88fad0f..f37e3c1fd4e7 100644
--- a/drivers/net/ipvlan/ipvlan_main.c
+++ b/drivers/net/ipvlan/ipvlan_main.c
@@ -455,7 +455,8 @@ static const struct ethtool_ops ipvlan_ethtool_ops = {
 };
 
 static int ipvlan_nl_changelink(struct net_device *dev,
-				struct nlattr *tb[], struct nlattr *data[])
+				struct nlattr *tb[], struct nlattr *data[],
+				struct netlink_ext_ack *extack)
 {
 	struct ipvl_dev *ipvlan = netdev_priv(dev);
 	struct ipvl_port *port = ipvlan_port_get_rtnl(ipvlan->phy_dev);
@@ -476,7 +477,8 @@ static size_t ipvlan_nl_getsize(const struct net_device *dev)
 		);
 }
 
-static int ipvlan_nl_validate(struct nlattr *tb[], struct nlattr *data[])
+static int ipvlan_nl_validate(struct nlattr *tb[], struct nlattr *data[],
+			      struct netlink_ext_ack *extack)
 {
 	if (data && data[IFLA_IPVLAN_MODE]) {
 		u16 mode = nla_get_u16(data[IFLA_IPVLAN_MODE]);
@@ -508,7 +510,8 @@ err:
 }
 
 int ipvlan_link_new(struct net *src_net, struct net_device *dev,
-		    struct nlattr *tb[], struct nlattr *data[])
+		    struct nlattr *tb[], struct nlattr *data[],
+		    struct netlink_ext_ack *extack)
 {
 	struct ipvl_dev *ipvlan = netdev_priv(dev);
 	struct ipvl_port *port;
@@ -632,7 +635,7 @@ void ipvlan_link_setup(struct net_device *dev)
 	dev->priv_flags &= ~(IFF_XMIT_DST_RELEASE | IFF_TX_SKB_SHARING);
 	dev->priv_flags |= IFF_UNICAST_FLT | IFF_NO_QUEUE;
 	dev->netdev_ops = &ipvlan_netdev_ops;
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 	dev->header_ops = &ipvlan_header_ops;
 	dev->ethtool_ops = &ipvlan_ethtool_ops;
 }
@@ -824,6 +827,33 @@ static int ipvlan_addr6_event(struct notifier_block *unused,
 	return NOTIFY_OK;
 }
 
+static int ipvlan_addr6_validator_event(struct notifier_block *unused,
+					unsigned long event, void *ptr)
+{
+	struct in6_validator_info *i6vi = (struct in6_validator_info *)ptr;
+	struct net_device *dev = (struct net_device *)i6vi->i6vi_dev->dev;
+	struct ipvl_dev *ipvlan = netdev_priv(dev);
+
+	/* FIXME IPv6 autoconf calls us from bh without RTNL */
+	if (in_softirq())
+		return NOTIFY_DONE;
+
+	if (!netif_is_ipvlan(dev))
+		return NOTIFY_DONE;
+
+	if (!ipvlan || !ipvlan->port)
+		return NOTIFY_DONE;
+
+	switch (event) {
+	case NETDEV_UP:
+		if (ipvlan_addr_busy(ipvlan->port, &i6vi->i6vi_addr, true))
+			return notifier_from_errno(-EADDRINUSE);
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
 static int ipvlan_add_addr4(struct ipvl_dev *ipvlan, struct in_addr *ip4_addr)
 {
 	if (ipvlan_addr_busy(ipvlan->port, ip4_addr, false)) {
@@ -871,10 +901,37 @@ static int ipvlan_addr4_event(struct notifier_block *unused,
 	return NOTIFY_OK;
 }
 
+static int ipvlan_addr4_validator_event(struct notifier_block *unused,
+					unsigned long event, void *ptr)
+{
+	struct in_validator_info *ivi = (struct in_validator_info *)ptr;
+	struct net_device *dev = (struct net_device *)ivi->ivi_dev->dev;
+	struct ipvl_dev *ipvlan = netdev_priv(dev);
+
+	if (!netif_is_ipvlan(dev))
+		return NOTIFY_DONE;
+
+	if (!ipvlan || !ipvlan->port)
+		return NOTIFY_DONE;
+
+	switch (event) {
+	case NETDEV_UP:
+		if (ipvlan_addr_busy(ipvlan->port, &ivi->ivi_addr, false))
+			return notifier_from_errno(-EADDRINUSE);
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
 static struct notifier_block ipvlan_addr4_notifier_block __read_mostly = {
 	.notifier_call = ipvlan_addr4_event,
 };
 
+static struct notifier_block ipvlan_addr4_vtor_notifier_block __read_mostly = {
+	.notifier_call = ipvlan_addr4_validator_event,
+};
+
 static struct notifier_block ipvlan_notifier_block __read_mostly = {
 	.notifier_call = ipvlan_device_event,
 };
@@ -883,6 +940,10 @@ static struct notifier_block ipvlan_addr6_notifier_block __read_mostly = {
 	.notifier_call = ipvlan_addr6_event,
 };
 
+static struct notifier_block ipvlan_addr6_vtor_notifier_block __read_mostly = {
+	.notifier_call = ipvlan_addr6_validator_event,
+};
+
 static void ipvlan_ns_exit(struct net *net)
 {
 	struct ipvlan_netns *vnet = net_generic(net, ipvlan_netid);
@@ -907,7 +968,10 @@ static int __init ipvlan_init_module(void)
 	ipvlan_init_secret();
 	register_netdevice_notifier(&ipvlan_notifier_block);
 	register_inet6addr_notifier(&ipvlan_addr6_notifier_block);
+	register_inet6addr_validator_notifier(
+	    &ipvlan_addr6_vtor_notifier_block);
 	register_inetaddr_notifier(&ipvlan_addr4_notifier_block);
+	register_inetaddr_validator_notifier(&ipvlan_addr4_vtor_notifier_block);
 
 	err = register_pernet_subsys(&ipvlan_net_ops);
 	if (err < 0)
@@ -922,7 +986,11 @@ static int __init ipvlan_init_module(void)
 	return 0;
 error:
 	unregister_inetaddr_notifier(&ipvlan_addr4_notifier_block);
+	unregister_inetaddr_validator_notifier(
+	    &ipvlan_addr4_vtor_notifier_block);
 	unregister_inet6addr_notifier(&ipvlan_addr6_notifier_block);
+	unregister_inet6addr_validator_notifier(
+	    &ipvlan_addr6_vtor_notifier_block);
 	unregister_netdevice_notifier(&ipvlan_notifier_block);
 	return err;
 }
@@ -933,7 +1001,11 @@ static void __exit ipvlan_cleanup_module(void)
 	unregister_pernet_subsys(&ipvlan_net_ops);
 	unregister_netdevice_notifier(&ipvlan_notifier_block);
 	unregister_inetaddr_notifier(&ipvlan_addr4_notifier_block);
+	unregister_inetaddr_validator_notifier(
+	    &ipvlan_addr4_vtor_notifier_block);
 	unregister_inet6addr_notifier(&ipvlan_addr6_notifier_block);
+	unregister_inet6addr_validator_notifier(
+	    &ipvlan_addr6_vtor_notifier_block);
 }
 
 module_init(ipvlan_init_module);
diff --git a/drivers/net/ipvlan/ipvtap.c b/drivers/net/ipvlan/ipvtap.c
index 2b713b63b62c..22f133ea8d7b 100644
--- a/drivers/net/ipvlan/ipvtap.c
+++ b/drivers/net/ipvlan/ipvtap.c
@@ -73,10 +73,9 @@ static void ipvtap_update_features(struct tap_dev *tap,
 	netdev_update_features(vlan->dev);
 }
 
-static int ipvtap_newlink(struct net *src_net,
-			  struct net_device *dev,
-			  struct nlattr *tb[],
-			  struct nlattr *data[])
+static int ipvtap_newlink(struct net *src_net, struct net_device *dev,
+			  struct nlattr *tb[], struct nlattr *data[],
+			  struct netlink_ext_ack *extack)
 {
 	struct ipvtap_dev *vlantap = netdev_priv(dev);
 	int err;
@@ -98,7 +97,7 @@ static int ipvtap_newlink(struct net *src_net,
 	/* Don't put anything that may fail after macvlan_common_newlink
 	 * because we can't undo what it does.
 	 */
-	err =  ipvlan_link_new(src_net, dev, tb, data);
+	err =  ipvlan_link_new(src_net, dev, tb, data, extack);
 	if (err) {
 		netdev_rx_handler_unregister(dev);
 		return err;
diff --git a/drivers/net/irda/smsc-ircc2.c b/drivers/net/irda/smsc-ircc2.c
index 23ed89ae5ddc..19a55cba6beb 100644
--- a/drivers/net/irda/smsc-ircc2.c
+++ b/drivers/net/irda/smsc-ircc2.c
@@ -1459,7 +1459,7 @@ static void smsc_ircc_dma_receive_complete(struct smsc_ircc_cb *self)
 	/* Make sure IP header gets aligned */
 	skb_reserve(skb, 1);
 
-	memcpy(skb_put(skb, len), self->rx_buff.data, len);
+	skb_put_data(skb, self->rx_buff.data, len);
 	self->netdev->stats.rx_packets++;
 	self->netdev->stats.rx_bytes += len;
 
diff --git a/drivers/net/irda/vlsi_ir.c b/drivers/net/irda/vlsi_ir.c
index 15b920086251..6638784c082e 100644
--- a/drivers/net/irda/vlsi_ir.c
+++ b/drivers/net/irda/vlsi_ir.c
@@ -578,7 +578,7 @@ static int vlsi_process_rx(struct vlsi_ring *r, struct ring_descr *rd)
 	skb = rd->skb;
 	rd->skb = NULL;
 	skb->dev = ndev;
-	memcpy(skb_put(skb,len), rd->buf, len);
+	skb_put_data(skb, rd->buf, len);
 	skb_reset_mac_header(skb);
 	if (in_interrupt())
 		netif_rx(skb);
diff --git a/drivers/net/loopback.c b/drivers/net/loopback.c
index 224f65cb576b..30612497643c 100644
--- a/drivers/net/loopback.c
+++ b/drivers/net/loopback.c
@@ -159,7 +159,6 @@ static void loopback_dev_free(struct net_device *dev)
 {
 	dev_net(dev)->loopback_dev = NULL;
 	free_percpu(dev->lstats);
-	free_netdev(dev);
 }
 
 static const struct net_device_ops loopback_ops = {
@@ -196,7 +195,8 @@ static void loopback_setup(struct net_device *dev)
 	dev->ethtool_ops	= &loopback_ethtool_ops;
 	dev->header_ops		= &eth_header_ops;
 	dev->netdev_ops		= &loopback_ops;
-	dev->destructor		= loopback_dev_free;
+	dev->needs_free_netdev	= true;
+	dev->priv_destructor	= loopback_dev_free;
 }
 
 /* Setup and register the loopback device. */
diff --git a/drivers/net/macsec.c b/drivers/net/macsec.c
index cdc347be68f2..5e1ab1160856 100644
--- a/drivers/net/macsec.c
+++ b/drivers/net/macsec.c
@@ -588,8 +588,6 @@ static void count_tx(struct net_device *dev, int ret, int len)
 		stats->tx_packets++;
 		stats->tx_bytes += len;
 		u64_stats_update_end(&stats->syncp);
-	} else {
-		dev->stats.tx_dropped++;
 	}
 }
 
@@ -699,7 +697,7 @@ static struct sk_buff *macsec_encrypt(struct sk_buff *skb,
 	unprotected_len = skb->len;
 	eth = eth_hdr(skb);
 	sci_present = send_sci(secy);
-	hh = (struct macsec_eth_header *)skb_push(skb, macsec_extra_len(sci_present));
+	hh = skb_push(skb, macsec_extra_len(sci_present));
 	memmove(hh, eth, 2 * ETH_ALEN);
 
 	pn = tx_sa_update_pn(tx_sa, secy);
@@ -742,7 +740,12 @@ static struct sk_buff *macsec_encrypt(struct sk_buff *skb,
 	macsec_fill_iv(iv, secy->sci, pn);
 
 	sg_init_table(sg, ret);
-	skb_to_sgvec(skb, sg, 0, skb->len);
+	ret = skb_to_sgvec(skb, sg, 0, skb->len);
+	if (unlikely(ret < 0)) {
+		macsec_txsa_put(tx_sa);
+		kfree_skb(skb);
+		return ERR_PTR(ret);
+	}
 
 	if (tx_sc->encrypt) {
 		int len = skb->len - macsec_hdr_len(sci_present) -
@@ -883,7 +886,7 @@ static void macsec_decrypt_done(struct crypto_async_request *base, int err)
 	struct macsec_dev *macsec = macsec_priv(dev);
 	struct macsec_rx_sa *rx_sa = macsec_skb_cb(skb)->rx_sa;
 	struct macsec_rx_sc *rx_sc = rx_sa->sc;
-	int len, ret;
+	int len;
 	u32 pn;
 
 	aead_request_free(macsec_skb_cb(skb)->req);
@@ -904,11 +907,8 @@ static void macsec_decrypt_done(struct crypto_async_request *base, int err)
 	macsec_reset_skb(skb, macsec->secy.netdev);
 
 	len = skb->len;
-	ret = gro_cells_receive(&macsec->gro_cells, skb);
-	if (ret == NET_RX_SUCCESS)
+	if (gro_cells_receive(&macsec->gro_cells, skb) == NET_RX_SUCCESS)
 		count_rx(dev, len);
-	else
-		macsec->secy.netdev->stats.rx_dropped++;
 
 	rcu_read_unlock_bh();
 
@@ -952,7 +952,11 @@ static struct sk_buff *macsec_decrypt(struct sk_buff *skb,
 	macsec_fill_iv(iv, sci, ntohl(hdr->packet_number));
 
 	sg_init_table(sg, ret);
-	skb_to_sgvec(skb, sg, 0, skb->len);
+	ret = skb_to_sgvec(skb, sg, 0, skb->len);
+	if (unlikely(ret < 0)) {
+		kfree_skb(skb);
+		return ERR_PTR(ret);
+	}
 
 	if (hdr->tci_an & MACSEC_TCI_E) {
 		/* confidentiality: ethernet + macsec header
@@ -1037,7 +1041,6 @@ static void handle_not_macsec(struct sk_buff *skb)
 	 */
 	list_for_each_entry_rcu(macsec, &rxd->secys, secys) {
 		struct sk_buff *nskb;
-		int ret;
 		struct pcpu_secy_stats *secy_stats = this_cpu_ptr(macsec->stats);
 
 		if (macsec->secy.validate_frames == MACSEC_VALIDATE_STRICT) {
@@ -1054,13 +1057,10 @@ static void handle_not_macsec(struct sk_buff *skb)
 
 		nskb->dev = macsec->secy.netdev;
 
-		ret = netif_rx(nskb);
-		if (ret == NET_RX_SUCCESS) {
+		if (netif_rx(nskb) == NET_RX_SUCCESS) {
 			u64_stats_update_begin(&secy_stats->syncp);
 			secy_stats->stats.InPktsUntagged++;
 			u64_stats_update_end(&secy_stats->syncp);
-		} else {
-			macsec->secy.netdev->stats.rx_dropped++;
 		}
 	}
 
@@ -2996,7 +2996,6 @@ static void macsec_free_netdev(struct net_device *dev)
 	free_percpu(macsec->secy.tx_sc.stats);
 
 	dev_put(real_dev);
-	free_netdev(dev);
 }
 
 static void macsec_setup(struct net_device *dev)
@@ -3006,7 +3005,8 @@ static void macsec_setup(struct net_device *dev)
 	dev->max_mtu = ETH_MAX_MTU;
 	dev->priv_flags |= IFF_NO_QUEUE;
 	dev->netdev_ops = &macsec_netdev_ops;
-	dev->destructor = macsec_free_netdev;
+	dev->needs_free_netdev = true;
+	dev->priv_destructor = macsec_free_netdev;
 	SET_NETDEV_DEVTYPE(dev, &macsec_type);
 
 	eth_zero_addr(dev->broadcast);
@@ -3056,7 +3056,8 @@ static void macsec_changelink_common(struct net_device *dev,
 }
 
 static int macsec_changelink(struct net_device *dev, struct nlattr *tb[],
-			     struct nlattr *data[])
+			     struct nlattr *data[],
+			     struct netlink_ext_ack *extack)
 {
 	if (!data)
 		return 0;
@@ -3203,7 +3204,8 @@ static int macsec_add_dev(struct net_device *dev, sci_t sci, u8 icv_len)
 }
 
 static int macsec_newlink(struct net *net, struct net_device *dev,
-			  struct nlattr *tb[], struct nlattr *data[])
+			  struct nlattr *tb[], struct nlattr *data[],
+			  struct netlink_ext_ack *extack)
 {
 	struct macsec_dev *macsec = macsec_priv(dev);
 	struct net_device *real_dev;
@@ -3285,7 +3287,8 @@ unregister:
 	return err;
 }
 
-static int macsec_validate_attr(struct nlattr *tb[], struct nlattr *data[])
+static int macsec_validate_attr(struct nlattr *tb[], struct nlattr *data[],
+				struct netlink_ext_ack *extack)
 {
 	u64 csid = MACSEC_DEFAULT_CIPHER_ID;
 	u8 icv_len = DEFAULT_ICV_LEN;
diff --git a/drivers/net/macvlan.c b/drivers/net/macvlan.c
index 346ad2ff3998..0f581ee74fe4 100644
--- a/drivers/net/macvlan.c
+++ b/drivers/net/macvlan.c
@@ -39,16 +39,20 @@
 #define MACVLAN_HASH_SIZE	(1<<MACVLAN_HASH_BITS)
 #define MACVLAN_BC_QUEUE_LEN	1000
 
+#define MACVLAN_F_PASSTHRU	1
+#define MACVLAN_F_ADDRCHANGE	2
+
 struct macvlan_port {
 	struct net_device	*dev;
 	struct hlist_head	vlan_hash[MACVLAN_HASH_SIZE];
 	struct list_head	vlans;
 	struct sk_buff_head	bc_queue;
 	struct work_struct	bc_work;
-	bool 			passthru;
+	u32			flags;
 	int			count;
 	struct hlist_head	vlan_source_hash[MACVLAN_HASH_SIZE];
 	DECLARE_BITMAP(mc_filter, MACVLAN_MC_FILTER_SZ);
+	unsigned char           perm_addr[ETH_ALEN];
 };
 
 struct macvlan_source_entry {
@@ -66,6 +70,31 @@ struct macvlan_skb_cb {
 
 static void macvlan_port_destroy(struct net_device *dev);
 
+static inline bool macvlan_passthru(const struct macvlan_port *port)
+{
+	return port->flags & MACVLAN_F_PASSTHRU;
+}
+
+static inline void macvlan_set_passthru(struct macvlan_port *port)
+{
+	port->flags |= MACVLAN_F_PASSTHRU;
+}
+
+static inline bool macvlan_addr_change(const struct macvlan_port *port)
+{
+	return port->flags & MACVLAN_F_ADDRCHANGE;
+}
+
+static inline void macvlan_set_addr_change(struct macvlan_port *port)
+{
+	port->flags |= MACVLAN_F_ADDRCHANGE;
+}
+
+static inline void macvlan_clear_addr_change(struct macvlan_port *port)
+{
+	port->flags &= ~MACVLAN_F_ADDRCHANGE;
+}
+
 /* Hash Ethernet address */
 static u32 macvlan_eth_hash(const unsigned char *addr)
 {
@@ -181,11 +210,12 @@ static void macvlan_hash_change_addr(struct macvlan_dev *vlan,
 static bool macvlan_addr_busy(const struct macvlan_port *port,
 			      const unsigned char *addr)
 {
-	/* Test to see if the specified multicast address is
+	/* Test to see if the specified address is
 	 * currently in use by the underlying device or
 	 * another macvlan.
 	 */
-	if (ether_addr_equal_64bits(port->dev->dev_addr, addr))
+	if (!macvlan_passthru(port) && !macvlan_addr_change(port) &&
+	    ether_addr_equal_64bits(port->dev->dev_addr, addr))
 		return true;
 
 	if (macvlan_hash_lookup(port, addr))
@@ -445,7 +475,7 @@ static rx_handler_result_t macvlan_handle_frame(struct sk_buff **pskb)
 	}
 
 	macvlan_forward_source(skb, port, eth->h_source);
-	if (port->passthru)
+	if (macvlan_passthru(port))
 		vlan = list_first_or_null_rcu(&port->vlans,
 					      struct macvlan_dev, list);
 	else
@@ -574,7 +604,7 @@ static int macvlan_open(struct net_device *dev)
 	struct net_device *lowerdev = vlan->lowerdev;
 	int err;
 
-	if (vlan->port->passthru) {
+	if (macvlan_passthru(vlan->port)) {
 		if (!(vlan->flags & MACVLAN_FLAG_NOPROMISC)) {
 			err = dev_set_promiscuity(lowerdev, 1);
 			if (err < 0)
@@ -649,7 +679,7 @@ static int macvlan_stop(struct net_device *dev)
 	dev_uc_unsync(lowerdev, dev);
 	dev_mc_unsync(lowerdev, dev);
 
-	if (vlan->port->passthru) {
+	if (macvlan_passthru(vlan->port)) {
 		if (!(vlan->flags & MACVLAN_FLAG_NOPROMISC))
 			dev_set_promiscuity(lowerdev, -1);
 		goto hash_del;
@@ -672,6 +702,7 @@ static int macvlan_sync_address(struct net_device *dev, unsigned char *addr)
 {
 	struct macvlan_dev *vlan = netdev_priv(dev);
 	struct net_device *lowerdev = vlan->lowerdev;
+	struct macvlan_port *port = vlan->port;
 	int err;
 
 	if (!(dev->flags & IFF_UP)) {
@@ -682,7 +713,7 @@ static int macvlan_sync_address(struct net_device *dev, unsigned char *addr)
 		if (macvlan_addr_busy(vlan->port, addr))
 			return -EBUSY;
 
-		if (!vlan->port->passthru) {
+		if (!macvlan_passthru(port)) {
 			err = dev_uc_add(lowerdev, addr);
 			if (err)
 				return err;
@@ -692,6 +723,15 @@ static int macvlan_sync_address(struct net_device *dev, unsigned char *addr)
 
 		macvlan_hash_change_addr(vlan, addr);
 	}
+	if (macvlan_passthru(port) && !macvlan_addr_change(port)) {
+		/* Since addr_change isn't set, we are here due to lower
+		 * device change.  Save the lower-dev address so we can
+		 * restore it later.
+		 */
+		ether_addr_copy(vlan->port->perm_addr,
+				lowerdev->dev_addr);
+	}
+	macvlan_clear_addr_change(port);
 	return 0;
 }
 
@@ -703,9 +743,13 @@ static int macvlan_set_mac_address(struct net_device *dev, void *p)
 	if (!is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
 
-	if (vlan->mode == MACVLAN_MODE_PASSTHRU) {
-		dev_set_mac_address(vlan->lowerdev, addr);
+	/* If the addresses are the same, this is a no-op */
+	if (ether_addr_equal(dev->dev_addr, addr->sa_data))
 		return 0;
+
+	if (vlan->mode == MACVLAN_MODE_PASSTHRU) {
+		macvlan_set_addr_change(vlan->port);
+		return dev_set_mac_address(vlan->lowerdev, addr);
 	}
 
 	return macvlan_sync_address(dev, addr->sa_data);
@@ -928,7 +972,7 @@ static int macvlan_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 	/* Support unicast filter only on passthru devices.
 	 * Multicast filter should be allowed on all devices.
 	 */
-	if (!vlan->port->passthru && is_unicast_ether_addr(addr))
+	if (!macvlan_passthru(vlan->port) && is_unicast_ether_addr(addr))
 		return -EOPNOTSUPP;
 
 	if (flags & NLM_F_REPLACE)
@@ -952,7 +996,7 @@ static int macvlan_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
 	/* Support unicast filter only on passthru devices.
 	 * Multicast filter should be allowed on all devices.
 	 */
-	if (!vlan->port->passthru && is_unicast_ether_addr(addr))
+	if (!macvlan_passthru(vlan->port) && is_unicast_ether_addr(addr))
 		return -EOPNOTSUPP;
 
 	if (is_unicast_ether_addr(addr))
@@ -1092,7 +1136,7 @@ void macvlan_common_setup(struct net_device *dev)
 	netif_keep_dst(dev);
 	dev->priv_flags	       |= IFF_UNICAST_FLT;
 	dev->netdev_ops		= &macvlan_netdev_ops;
-	dev->destructor		= free_netdev;
+	dev->needs_free_netdev	= true;
 	dev->header_ops		= &macvlan_hard_header_ops;
 	dev->ethtool_ops	= &macvlan_ethtool_ops;
 }
@@ -1120,8 +1164,8 @@ static int macvlan_port_create(struct net_device *dev)
 	if (port == NULL)
 		return -ENOMEM;
 
-	port->passthru = false;
 	port->dev = dev;
+	ether_addr_copy(port->perm_addr, dev->dev_addr);
 	INIT_LIST_HEAD(&port->vlans);
 	for (i = 0; i < MACVLAN_HASH_SIZE; i++)
 		INIT_HLIST_HEAD(&port->vlan_hash[i]);
@@ -1161,10 +1205,23 @@ static void macvlan_port_destroy(struct net_device *dev)
 		kfree_skb(skb);
 	}
 
+	/* If the lower device address has been changed by passthru
+	 * macvlan, put it back.
+	 */
+	if (macvlan_passthru(port) &&
+	    !ether_addr_equal(port->dev->dev_addr, port->perm_addr)) {
+		struct sockaddr sa;
+
+		sa.sa_family = port->dev->type;
+		memcpy(&sa.sa_data, port->perm_addr, port->dev->addr_len);
+		dev_set_mac_address(port->dev, &sa);
+	}
+
 	kfree(port);
 }
 
-static int macvlan_validate(struct nlattr *tb[], struct nlattr *data[])
+static int macvlan_validate(struct nlattr *tb[], struct nlattr *data[],
+			    struct netlink_ext_ack *extack)
 {
 	if (tb[IFLA_ADDRESS]) {
 		if (nla_len(tb[IFLA_ADDRESS]) != ETH_ALEN)
@@ -1326,7 +1383,7 @@ int macvlan_common_newlink(struct net *src_net, struct net_device *dev,
 	port = macvlan_port_get_rtnl(lowerdev);
 
 	/* Only 1 macvlan device can be created in passthru mode */
-	if (port->passthru) {
+	if (macvlan_passthru(port)) {
 		/* The macvlan port must be not created this time,
 		 * still goto destroy_macvlan_port for readability.
 		 */
@@ -1352,7 +1409,7 @@ int macvlan_common_newlink(struct net *src_net, struct net_device *dev,
 			err = -EINVAL;
 			goto destroy_macvlan_port;
 		}
-		port->passthru = true;
+		macvlan_set_passthru(port);
 		eth_hw_addr_inherit(dev, lowerdev);
 	}
 
@@ -1392,7 +1449,8 @@ destroy_macvlan_port:
 EXPORT_SYMBOL_GPL(macvlan_common_newlink);
 
 static int macvlan_newlink(struct net *src_net, struct net_device *dev,
-			   struct nlattr *tb[], struct nlattr *data[])
+			   struct nlattr *tb[], struct nlattr *data[],
+			   struct netlink_ext_ack *extack)
 {
 	return macvlan_common_newlink(src_net, dev, tb, data);
 }
@@ -1410,7 +1468,8 @@ void macvlan_dellink(struct net_device *dev, struct list_head *head)
 EXPORT_SYMBOL_GPL(macvlan_dellink);
 
 static int macvlan_changelink(struct net_device *dev,
-		struct nlattr *tb[], struct nlattr *data[])
+			      struct nlattr *tb[], struct nlattr *data[],
+			      struct netlink_ext_ack *extack)
 {
 	struct macvlan_dev *vlan = netdev_priv(dev);
 	enum macvlan_mode mode;
@@ -1434,7 +1493,7 @@ static int macvlan_changelink(struct net_device *dev,
 	if (data && data[IFLA_MACVLAN_FLAGS]) {
 		__u16 flags = nla_get_u16(data[IFLA_MACVLAN_FLAGS]);
 		bool promisc = (flags ^ vlan->flags) & MACVLAN_FLAG_NOPROMISC;
-		if (vlan->port->passthru && promisc) {
+		if (macvlan_passthru(vlan->port) && promisc) {
 			int err;
 
 			if (flags & MACVLAN_FLAG_NOPROMISC)
@@ -1597,7 +1656,7 @@ static int macvlan_device_event(struct notifier_block *unused,
 		}
 		break;
 	case NETDEV_CHANGEADDR:
-		if (!port->passthru)
+		if (!macvlan_passthru(port))
 			return NOTIFY_DONE;
 
 		vlan = list_first_entry_or_null(&port->vlans,
diff --git a/drivers/net/macvtap.c b/drivers/net/macvtap.c
index da85057680d6..91e7b19bbf86 100644
--- a/drivers/net/macvtap.c
+++ b/drivers/net/macvtap.c
@@ -77,10 +77,9 @@ static void macvtap_update_features(struct tap_dev *tap,
 	netdev_update_features(vlan->dev);
 }
 
-static int macvtap_newlink(struct net *src_net,
-			   struct net_device *dev,
-			   struct nlattr *tb[],
-			   struct nlattr *data[])
+static int macvtap_newlink(struct net *src_net, struct net_device *dev,
+			   struct nlattr *tb[], struct nlattr *data[],
+			   struct netlink_ext_ack *extack)
 {
 	struct macvtap_dev *vlantap = netdev_priv(dev);
 	int err;
diff --git a/drivers/net/mii.c b/drivers/net/mii.c
index 6d953c53eed6..44612122338b 100644
--- a/drivers/net/mii.c
+++ b/drivers/net/mii.c
@@ -141,11 +141,9 @@ int mii_ethtool_gset(struct mii_if_info *mii, struct ethtool_cmd *ecmd)
  *
  * The @cmd parameter is expected to have been cleared before calling
  * mii_ethtool_get_link_ksettings().
- *
- * Returns 0 for success, negative on error.
  */
-int mii_ethtool_get_link_ksettings(struct mii_if_info *mii,
-				   struct ethtool_link_ksettings *cmd)
+void mii_ethtool_get_link_ksettings(struct mii_if_info *mii,
+				    struct ethtool_link_ksettings *cmd)
 {
 	struct net_device *dev = mii->dev;
 	u16 bmcr, bmsr, ctrl1000 = 0, stat1000 = 0;
@@ -227,8 +225,6 @@ int mii_ethtool_get_link_ksettings(struct mii_if_info *mii,
 						lp_advertising);
 
 	/* ignore maxtxpkt, maxrxpkt for now */
-
-	return 0;
 }
 
 /**
diff --git a/drivers/net/netconsole.c b/drivers/net/netconsole.c
index 06ee6395117f..0e27920c2b6b 100644
--- a/drivers/net/netconsole.c
+++ b/drivers/net/netconsole.c
@@ -358,7 +358,7 @@ static ssize_t enabled_store(struct config_item *item,
 		if (err)
 			goto out_unlock;
 
-		pr_info("netconsole: network logging started\n");
+		pr_info("network logging started\n");
 	} else {	/* false */
 		/* We need to disable the netconsole before cleaning it up
 		 * otherwise we might end up in write_msg() with
diff --git a/drivers/net/nlmon.c b/drivers/net/nlmon.c
index b91603835d26..4b22955de191 100644
--- a/drivers/net/nlmon.c
+++ b/drivers/net/nlmon.c
@@ -113,7 +113,7 @@ static void nlmon_setup(struct net_device *dev)
 
 	dev->netdev_ops	= &nlmon_ops;
 	dev->ethtool_ops = &nlmon_ethtool_ops;
-	dev->destructor	= free_netdev;
+	dev->needs_free_netdev = true;
 
 	dev->features = NETIF_F_SG | NETIF_F_FRAGLIST |
 			NETIF_F_HIGHDMA | NETIF_F_LLTX;
@@ -127,7 +127,8 @@ static void nlmon_setup(struct net_device *dev)
 	dev->min_mtu = sizeof(struct nlmsghdr);
 }
 
-static int nlmon_validate(struct nlattr *tb[], struct nlattr *data[])
+static int nlmon_validate(struct nlattr *tb[], struct nlattr *data[],
+			  struct netlink_ext_ack *extack)
 {
 	if (tb[IFLA_ADDRESS])
 		return -EINVAL;
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index c360dd6ead22..2dda72004a7d 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -127,6 +127,7 @@ config MDIO_THUNDER
 	tristate "ThunderX SOCs MDIO buses"
 	depends on 64BIT
 	depends on PCI
+	depends on !(MDIO_DEVICE=y && PHYLIB=m)
 	select MDIO_CAVIUM
 	help
 	  This driver supports the MDIO interfaces found on Cavium
@@ -234,6 +235,11 @@ config CICADA_PHY
 	---help---
 	  Currently supports the cis8204
 
+config CORTINA_PHY
+	tristate "Cortina EDC CDR 10G Ethernet PHY"
+	---help---
+	  Currently supports the CS4340 phy.
+
 config DAVICOM_PHY
 	tristate "Davicom PHYs"
 	---help---
@@ -287,6 +293,11 @@ config MARVELL_PHY
 	---help---
 	  Currently has a driver for the 88E1011S
 
+config MARVELL_10G_PHY
+	tristate "Marvell Alaska 10Gbit PHYs"
+	---help---
+	  Support for the Marvell Alaska MV88X3310 and compatible PHYs.
+
 config MESON_GXL_PHY
 	tristate "Amlogic Meson GXL Internal PHY"
 	depends on ARCH_MESON || COMPILE_TEST
diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
index e36db9a2ba38..8e9b9f349384 100644
--- a/drivers/net/phy/Makefile
+++ b/drivers/net/phy/Makefile
@@ -1,6 +1,6 @@
 # Makefile for Linux PHY drivers and MDIO bus drivers
 
-libphy-y			:= phy.o phy-core.o phy_device.o
+libphy-y			:= phy.o phy-c45.o phy-core.o phy_device.o
 mdio-bus-y			+= mdio_bus.o mdio_device.o
 
 ifdef CONFIG_MDIO_DEVICE
@@ -46,6 +46,7 @@ obj-$(CONFIG_BCM_CYGNUS_PHY)	+= bcm-cygnus.o
 obj-$(CONFIG_BCM_NET_PHYLIB)	+= bcm-phy-lib.o
 obj-$(CONFIG_BROADCOM_PHY)	+= broadcom.o
 obj-$(CONFIG_CICADA_PHY)	+= cicada.o
+obj-$(CONFIG_CORTINA_PHY)	+= cortina.o
 obj-$(CONFIG_DAVICOM_PHY)	+= davicom.o
 obj-$(CONFIG_DP83640_PHY)	+= dp83640.o
 obj-$(CONFIG_DP83848_PHY)	+= dp83848.o
@@ -56,6 +57,7 @@ obj-$(CONFIG_INTEL_XWAY_PHY)	+= intel-xway.o
 obj-$(CONFIG_LSI_ET1011C_PHY)	+= et1011c.o
 obj-$(CONFIG_LXT_PHY)		+= lxt.o
 obj-$(CONFIG_MARVELL_PHY)	+= marvell.o
+obj-$(CONFIG_MARVELL_10G_PHY)	+= marvell10g.o
 obj-$(CONFIG_MESON_GXL_PHY)	+= meson-gxl.o
 obj-$(CONFIG_MICREL_KS8995MA)	+= spi_ks8995.o
 obj-$(CONFIG_MICREL_PHY)	+= micrel.o
diff --git a/drivers/net/phy/broadcom.c b/drivers/net/phy/broadcom.c
index a32dc5d11e89..1e9ad30a35c8 100644
--- a/drivers/net/phy/broadcom.c
+++ b/drivers/net/phy/broadcom.c
@@ -540,7 +540,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5411",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= bcm54xx_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -551,7 +551,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5421",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= bcm54xx_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -562,7 +562,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM54210E",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= bcm54xx_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -573,7 +573,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5461",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= bcm54xx_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -584,7 +584,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM54612E",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= bcm54xx_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -595,7 +595,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM54616S",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= bcm54xx_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -606,7 +606,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5464",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= bcm54xx_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -617,7 +617,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5481",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= bcm54xx_config_init,
 	.config_aneg	= bcm5481_config_aneg,
 	.read_status	= genphy_read_status,
@@ -628,7 +628,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask    = 0xfffffff0,
 	.name           = "Broadcom BCM54810",
 	.features       = PHY_GBIT_FEATURES,
-	.flags          = PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags          = PHY_HAS_INTERRUPT,
 	.config_init    = bcm54xx_config_init,
 	.config_aneg    = bcm5481_config_aneg,
 	.read_status    = genphy_read_status,
@@ -639,7 +639,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5482",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= bcm5482_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= bcm5482_read_status,
@@ -650,7 +650,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM50610",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= bcm54xx_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -661,7 +661,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM50610M",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= bcm54xx_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -672,7 +672,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM57780",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= bcm54xx_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -683,7 +683,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCMAC131",
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= brcm_fet_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -694,7 +694,7 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5241",
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= brcm_fet_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
diff --git a/drivers/net/phy/cortina.c b/drivers/net/phy/cortina.c
new file mode 100644
index 000000000000..72f4228a63bb
--- /dev/null
+++ b/drivers/net/phy/cortina.c
@@ -0,0 +1,118 @@
+/*
+ *    Copyright 2017 NXP
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the 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.
+ *
+ *    CORTINA is a registered trademark of Cortina Systems, Inc.
+ *
+ */
+#include <linux/module.h>
+#include <linux/phy.h>
+
+#define PHY_ID_CS4340	0x13e51002
+
+#define VILLA_GLOBAL_CHIP_ID_LSB			0x0
+#define VILLA_GLOBAL_CHIP_ID_MSB			0x1
+
+#define VILLA_GLOBAL_GPIO_1_INTS			0x017
+
+static int cortina_read_reg(struct phy_device *phydev, u16 regnum)
+{
+	return mdiobus_read(phydev->mdio.bus, phydev->mdio.addr,
+			    MII_ADDR_C45 | regnum);
+}
+
+static int cortina_config_aneg(struct phy_device *phydev)
+{
+	phydev->supported = SUPPORTED_10000baseT_Full;
+	phydev->advertising = SUPPORTED_10000baseT_Full;
+
+	return 0;
+}
+
+static int cortina_read_status(struct phy_device *phydev)
+{
+	int gpio_int_status, ret = 0;
+
+	gpio_int_status = cortina_read_reg(phydev, VILLA_GLOBAL_GPIO_1_INTS);
+	if (gpio_int_status < 0) {
+		ret = gpio_int_status;
+		goto err;
+	}
+
+	if (gpio_int_status & 0x8) {
+		/* up when edc_convergedS set */
+		phydev->speed = SPEED_10000;
+		phydev->duplex = DUPLEX_FULL;
+		phydev->link = 1;
+	} else {
+		phydev->link = 0;
+	}
+
+err:
+	return ret;
+}
+
+static int cortina_soft_reset(struct phy_device *phydev)
+{
+	return 0;
+}
+
+static int cortina_probe(struct phy_device *phydev)
+{
+	u32 phy_id = 0;
+	int id_lsb = 0, id_msb = 0;
+
+	/* Read device id from phy registers. */
+	id_lsb = cortina_read_reg(phydev, VILLA_GLOBAL_CHIP_ID_LSB);
+	if (id_lsb < 0)
+		return -ENXIO;
+
+	phy_id = id_lsb << 16;
+
+	id_msb = cortina_read_reg(phydev, VILLA_GLOBAL_CHIP_ID_MSB);
+	if (id_msb < 0)
+		return -ENXIO;
+
+	phy_id |= id_msb;
+
+	/* Make sure the device tree binding matched the driver with the
+	 * right device.
+	 */
+	if (phy_id != phydev->drv->phy_id) {
+		phydev_err(phydev, "Error matching phy with %s driver\n",
+			   phydev->drv->name);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static struct phy_driver cortina_driver[] = {
+{
+	.phy_id		= PHY_ID_CS4340,
+	.phy_id_mask	= 0xffffffff,
+	.name		= "Cortina CS4340",
+	.config_aneg	= cortina_config_aneg,
+	.read_status	= cortina_read_status,
+	.soft_reset	= cortina_soft_reset,
+	.probe		= cortina_probe,
+},
+};
+
+module_phy_driver(cortina_driver);
+
+static struct mdio_device_id __maybe_unused cortina_tbl[] = {
+	{ PHY_ID_CS4340, 0xffffffff},
+	{},
+};
+
+MODULE_DEVICE_TABLE(mdio, cortina_tbl);
diff --git a/drivers/net/phy/dp83640.c b/drivers/net/phy/dp83640.c
index ed0d10f54f26..c3065236ffcc 100644
--- a/drivers/net/phy/dp83640.c
+++ b/drivers/net/phy/dp83640.c
@@ -908,7 +908,7 @@ static void decode_txts(struct dp83640_private *dp83640,
 	if (overflow) {
 		pr_debug("tx timestamp queue overflow, count %d\n", overflow);
 		while (skb) {
-			skb_complete_tx_timestamp(skb, NULL);
+			kfree_skb(skb);
 			skb = skb_dequeue(&dp83640->tx_queue);
 		}
 		return;
diff --git a/drivers/net/phy/dp83867.c b/drivers/net/phy/dp83867.c
index b57f20e552ba..c1ab976cc800 100644
--- a/drivers/net/phy/dp83867.c
+++ b/drivers/net/phy/dp83867.c
@@ -91,6 +91,7 @@ struct dp83867_private {
 	int fifo_depth;
 	int io_impedance;
 	int port_mirroring;
+	bool rxctrl_strap_quirk;
 };
 
 static int dp83867_ack_interrupt(struct phy_device *phydev)
@@ -164,6 +165,9 @@ static int dp83867_of_init(struct phy_device *phydev)
 	else if (of_property_read_bool(of_node, "ti,min-output-impedance"))
 		dp83867->io_impedance = DP83867_IO_MUX_CFG_IO_IMPEDANCE_MIN;
 
+	dp83867->rxctrl_strap_quirk = of_property_read_bool(of_node,
+					"ti,dp83867-rxctrl-strap-quirk");
+
 	ret = of_property_read_u32(of_node, "ti,rx-internal-delay",
 				   &dp83867->rx_id_delay);
 	if (ret &&
@@ -214,6 +218,13 @@ static int dp83867_config_init(struct phy_device *phydev)
 		dp83867 = (struct dp83867_private *)phydev->priv;
 	}
 
+	/* RX_DV/RX_CTRL strapped in mode 1 or mode 2 workaround */
+	if (dp83867->rxctrl_strap_quirk) {
+		val = phy_read_mmd(phydev, DP83867_DEVADDR, DP83867_CFG4);
+		val &= ~BIT(7);
+		phy_write_mmd(phydev, DP83867_DEVADDR, DP83867_CFG4, val);
+	}
+
 	if (phy_interface_is_rgmii(phydev)) {
 		val = phy_read(phydev, MII_DP83867_PHYCTRL);
 		if (val < 0)
diff --git a/drivers/net/phy/lxt.c b/drivers/net/phy/lxt.c
index 8d198a1f0031..09d215177fff 100644
--- a/drivers/net/phy/lxt.c
+++ b/drivers/net/phy/lxt.c
@@ -152,7 +152,6 @@ static int lxt973a2_read_status(struct phy_device *phydev)
 	int adv;
 	int err;
 	int lpa;
-	int lpagb = 0;
 
 	/* Update the link, but return if there was an error */
 	err = lxt973a2_update_link(phydev);
@@ -178,18 +177,15 @@ static int lxt973a2_read_status(struct phy_device *phydev)
 			*/
 		} while (lpa == adv && retry--);
 
+		phydev->lp_advertising = mii_lpa_to_ethtool_lpa_t(lpa);
+
 		lpa &= adv;
 
 		phydev->speed = SPEED_10;
 		phydev->duplex = DUPLEX_HALF;
 		phydev->pause = phydev->asym_pause = 0;
 
-		if (lpagb & (LPA_1000FULL | LPA_1000HALF)) {
-			phydev->speed = SPEED_1000;
-
-			if (lpagb & LPA_1000FULL)
-				phydev->duplex = DUPLEX_FULL;
-		} else if (lpa & (LPA_100FULL | LPA_100HALF)) {
+		if (lpa & (LPA_100FULL | LPA_100HALF)) {
 			phydev->speed = SPEED_100;
 
 			if (lpa & LPA_100FULL)
@@ -222,6 +218,7 @@ static int lxt973a2_read_status(struct phy_device *phydev)
 			phydev->speed = SPEED_10;
 
 		phydev->pause = phydev->asym_pause = 0;
+		phydev->lp_advertising = 0;
 	}
 
 	return 0;
diff --git a/drivers/net/phy/marvell.c b/drivers/net/phy/marvell.c
index 57297ba23987..5d314f143aea 100644
--- a/drivers/net/phy/marvell.c
+++ b/drivers/net/phy/marvell.c
@@ -41,6 +41,12 @@
 #include <linux/uaccess.h>
 
 #define MII_MARVELL_PHY_PAGE		22
+#define MII_MARVELL_COPPER_PAGE		0x00
+#define MII_MARVELL_FIBER_PAGE		0x01
+#define MII_MARVELL_MSCR_PAGE		0x02
+#define MII_MARVELL_LED_PAGE		0x03
+#define MII_MARVELL_MISC_TEST_PAGE	0x06
+#define MII_MARVELL_WOL_PAGE		0x11
 
 #define MII_M1011_IEVENT		0x13
 #define MII_M1011_IEVENT_CLEAR		0x0000
@@ -54,7 +60,6 @@
 #define MII_M1011_PHY_SCR_MDI_X		0x0020
 #define MII_M1011_PHY_SCR_AUTO_CROSS	0x0060
 
-#define MII_M1145_PHY_EXT_ADDR_PAGE	0x16
 #define MII_M1145_PHY_EXT_SR		0x1b
 #define MII_M1145_PHY_EXT_CR		0x14
 #define MII_M1145_RGMII_RX_DELAY	0x0080
@@ -83,10 +88,6 @@
 #define MII_M1111_HWCFG_FIBER_COPPER_AUTO	0x8000
 #define MII_M1111_HWCFG_FIBER_COPPER_RES	0x2000
 
-#define MII_M1111_COPPER		0
-#define MII_M1111_FIBER			1
-
-#define MII_88E1121_PHY_MSCR_PAGE	2
 #define MII_88E1121_PHY_MSCR_REG	21
 #define MII_88E1121_PHY_MSCR_RX_DELAY	BIT(5)
 #define MII_88E1121_PHY_MSCR_TX_DELAY	BIT(4)
@@ -112,7 +113,6 @@
 #define MII_88E1318S_PHY_CSIER_WOL_EIE                      BIT(7)
 
 /* LED Timer Control Register */
-#define MII_88E1318S_PHY_LED_PAGE                           0x03
 #define MII_88E1318S_PHY_LED_TCR                            0x12
 #define MII_88E1318S_PHY_LED_TCR_FORCE_INT                  BIT(15)
 #define MII_88E1318S_PHY_LED_TCR_INTn_ENABLE                BIT(7)
@@ -123,13 +123,11 @@
 #define MII_88E1318S_PHY_MAGIC_PACKET_WORD1                 0x18
 #define MII_88E1318S_PHY_MAGIC_PACKET_WORD0                 0x19
 
-#define MII_88E1318S_PHY_WOL_PAGE                           0x11
 #define MII_88E1318S_PHY_WOL_CTRL                           0x10
 #define MII_88E1318S_PHY_WOL_CTRL_CLEAR_WOL_STATUS          BIT(12)
 #define MII_88E1318S_PHY_WOL_CTRL_MAGIC_PACKET_MATCH_ENABLE BIT(14)
 
 #define MII_88E1121_PHY_LED_CTRL	16
-#define MII_88E1121_PHY_LED_PAGE	3
 #define MII_88E1121_PHY_LED_DEF		0x0030
 
 #define MII_M1011_PHY_STATUS		0x11
@@ -189,6 +187,29 @@ struct marvell_priv {
 	struct device *hwmon_dev;
 };
 
+static int marvell_get_page(struct phy_device *phydev)
+{
+	return phy_read(phydev, MII_MARVELL_PHY_PAGE);
+}
+
+static int marvell_set_page(struct phy_device *phydev, int page)
+{
+	return phy_write(phydev, MII_MARVELL_PHY_PAGE, page);
+}
+
+static int marvell_get_set_page(struct phy_device *phydev, int page)
+{
+	int oldpage = marvell_get_page(phydev);
+
+	if (oldpage < 0)
+		return oldpage;
+
+	if (page != oldpage)
+		return marvell_set_page(phydev, page);
+
+	return 0;
+}
+
 static int marvell_ack_interrupt(struct phy_device *phydev)
 {
 	int err;
@@ -207,9 +228,11 @@ static int marvell_config_intr(struct phy_device *phydev)
 	int err;
 
 	if (phydev->interrupts == PHY_INTERRUPT_ENABLED)
-		err = phy_write(phydev, MII_M1011_IMASK, MII_M1011_IMASK_INIT);
+		err = phy_write(phydev, MII_M1011_IMASK,
+				MII_M1011_IMASK_INIT);
 	else
-		err = phy_write(phydev, MII_M1011_IMASK, MII_M1011_IMASK_CLEAR);
+		err = phy_write(phydev, MII_M1011_IMASK,
+				MII_M1011_IMASK_CLEAR);
 
 	return err;
 }
@@ -271,8 +294,7 @@ static int marvell_config_aneg(struct phy_device *phydev)
 	if (phydev->autoneg != AUTONEG_ENABLE) {
 		int bmcr;
 
-		/*
-		 * A write to speed/duplex bits (that is performed by
+		/* A write to speed/duplex bits (that is performed by
 		 * genphy_config_aneg() call above) must be followed by
 		 * a software reset. Otherwise, the write has no effect.
 		 */
@@ -367,8 +389,7 @@ static int m88e1111_config_aneg(struct phy_device *phydev)
 }
 
 #ifdef CONFIG_OF_MDIO
-/*
- * Set and/or override some configuration registers based on the
+/* Set and/or override some configuration registers based on the
  * marvell,reg-init property stored in the of_node for the phydev.
  *
  * marvell,reg-init = <reg-page reg mask value>,...;
@@ -394,7 +415,7 @@ static int marvell_of_reg_init(struct phy_device *phydev)
 	if (!paddr || len < (4 * sizeof(*paddr)))
 		return 0;
 
-	saved_page = phy_read(phydev, MII_MARVELL_PHY_PAGE);
+	saved_page = marvell_get_page(phydev);
 	if (saved_page < 0)
 		return saved_page;
 	current_page = saved_page;
@@ -402,15 +423,15 @@ static int marvell_of_reg_init(struct phy_device *phydev)
 	ret = 0;
 	len /= sizeof(*paddr);
 	for (i = 0; i < len - 3; i += 4) {
-		u16 reg_page = be32_to_cpup(paddr + i);
+		u16 page = be32_to_cpup(paddr + i);
 		u16 reg = be32_to_cpup(paddr + i + 1);
 		u16 mask = be32_to_cpup(paddr + i + 2);
 		u16 val_bits = be32_to_cpup(paddr + i + 3);
 		int val;
 
-		if (reg_page != current_page) {
-			current_page = reg_page;
-			ret = phy_write(phydev, MII_MARVELL_PHY_PAGE, reg_page);
+		if (page != current_page) {
+			current_page = page;
+			ret = marvell_set_page(phydev, page);
 			if (ret < 0)
 				goto err;
 		}
@@ -429,11 +450,10 @@ static int marvell_of_reg_init(struct phy_device *phydev)
 		ret = phy_write(phydev, reg, val);
 		if (ret < 0)
 			goto err;
-
 	}
 err:
 	if (current_page != saved_page) {
-		i = phy_write(phydev, MII_MARVELL_PHY_PAGE, saved_page);
+		i = marvell_set_page(phydev, saved_page);
 		if (ret == 0)
 			ret = i;
 	}
@@ -450,15 +470,11 @@ static int m88e1121_config_aneg(struct phy_device *phydev)
 {
 	int err, oldpage, mscr;
 
-	oldpage = phy_read(phydev, MII_MARVELL_PHY_PAGE);
-
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE,
-			MII_88E1121_PHY_MSCR_PAGE);
-	if (err < 0)
-		return err;
+	oldpage = marvell_get_set_page(phydev, MII_MARVELL_MSCR_PAGE);
+	if (oldpage < 0)
+		return oldpage;
 
 	if (phy_interface_is_rgmii(phydev)) {
-
 		mscr = phy_read(phydev, MII_88E1121_PHY_MSCR_REG) &
 			MII_88E1121_PHY_MSCR_DELAY_MASK;
 
@@ -475,7 +491,7 @@ static int m88e1121_config_aneg(struct phy_device *phydev)
 			return err;
 	}
 
-	phy_write(phydev, MII_MARVELL_PHY_PAGE, oldpage);
+	marvell_set_page(phydev, oldpage);
 
 	err = phy_write(phydev, MII_BMCR, BMCR_RESET);
 	if (err < 0)
@@ -493,12 +509,9 @@ static int m88e1318_config_aneg(struct phy_device *phydev)
 {
 	int err, oldpage, mscr;
 
-	oldpage = phy_read(phydev, MII_MARVELL_PHY_PAGE);
-
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE,
-			MII_88E1121_PHY_MSCR_PAGE);
-	if (err < 0)
-		return err;
+	oldpage = marvell_get_set_page(phydev, MII_MARVELL_MSCR_PAGE);
+	if (oldpage < 0)
+		return oldpage;
 
 	mscr = phy_read(phydev, MII_88E1318S_PHY_MSCR1_REG);
 	mscr |= MII_88E1318S_PHY_MSCR1_PAD_ODD;
@@ -507,7 +520,7 @@ static int m88e1318_config_aneg(struct phy_device *phydev)
 	if (err < 0)
 		return err;
 
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, oldpage);
+	err = marvell_set_page(phydev, oldpage);
 	if (err < 0)
 		return err;
 
@@ -607,7 +620,7 @@ static int m88e1510_config_aneg(struct phy_device *phydev)
 {
 	int err;
 
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_M1111_COPPER);
+	err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 	if (err < 0)
 		goto error;
 
@@ -617,7 +630,7 @@ static int m88e1510_config_aneg(struct phy_device *phydev)
 		goto error;
 
 	/* Then the fiber link */
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_M1111_FIBER);
+	err = marvell_set_page(phydev, MII_MARVELL_FIBER_PAGE);
 	if (err < 0)
 		goto error;
 
@@ -625,10 +638,10 @@ static int m88e1510_config_aneg(struct phy_device *phydev)
 	if (err < 0)
 		goto error;
 
-	return phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_M1111_COPPER);
+	return marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 
 error:
-	phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_M1111_COPPER);
+	marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 	return err;
 }
 
@@ -651,7 +664,7 @@ static int m88e1116r_config_init(struct phy_device *phydev)
 
 	mdelay(500);
 
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, 0);
+	err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 	if (err < 0)
 		return err;
 
@@ -663,7 +676,7 @@ static int m88e1116r_config_init(struct phy_device *phydev)
 	if (err < 0)
 		return err;
 
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, 2);
+	err = marvell_set_page(phydev, MII_MARVELL_MSCR_PAGE);
 	if (err < 0)
 		return err;
 	temp = phy_read(phydev, MII_M1116R_CONTROL_REG_MAC);
@@ -672,7 +685,7 @@ static int m88e1116r_config_init(struct phy_device *phydev)
 	err = phy_write(phydev, MII_M1116R_CONTROL_REG_MAC, temp);
 	if (err < 0)
 		return err;
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, 0);
+	err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 	if (err < 0)
 		return err;
 
@@ -706,103 +719,129 @@ static int m88e3016_config_init(struct phy_device *phydev)
 	return marvell_config_init(phydev);
 }
 
-static int m88e1111_config_init(struct phy_device *phydev)
+static int m88e1111_config_init_rgmii(struct phy_device *phydev)
 {
 	int err;
 	int temp;
 
-	if (phy_interface_is_rgmii(phydev)) {
+	temp = phy_read(phydev, MII_M1111_PHY_EXT_CR);
+	if (temp < 0)
+		return temp;
 
-		temp = phy_read(phydev, MII_M1111_PHY_EXT_CR);
-		if (temp < 0)
-			return temp;
+	if (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID) {
+		temp |= (MII_M1111_RX_DELAY | MII_M1111_TX_DELAY);
+	} else if (phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID) {
+		temp &= ~MII_M1111_TX_DELAY;
+		temp |= MII_M1111_RX_DELAY;
+	} else if (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID) {
+		temp &= ~MII_M1111_RX_DELAY;
+		temp |= MII_M1111_TX_DELAY;
+	}
 
-		if (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID) {
-			temp |= (MII_M1111_RX_DELAY | MII_M1111_TX_DELAY);
-		} else if (phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID) {
-			temp &= ~MII_M1111_TX_DELAY;
-			temp |= MII_M1111_RX_DELAY;
-		} else if (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID) {
-			temp &= ~MII_M1111_RX_DELAY;
-			temp |= MII_M1111_TX_DELAY;
-		}
+	err = phy_write(phydev, MII_M1111_PHY_EXT_CR, temp);
+	if (err < 0)
+		return err;
 
-		err = phy_write(phydev, MII_M1111_PHY_EXT_CR, temp);
-		if (err < 0)
-			return err;
+	temp = phy_read(phydev, MII_M1111_PHY_EXT_SR);
+	if (temp < 0)
+		return temp;
 
-		temp = phy_read(phydev, MII_M1111_PHY_EXT_SR);
-		if (temp < 0)
-			return temp;
+	temp &= ~(MII_M1111_HWCFG_MODE_MASK);
 
-		temp &= ~(MII_M1111_HWCFG_MODE_MASK);
+	if (temp & MII_M1111_HWCFG_FIBER_COPPER_RES)
+		temp |= MII_M1111_HWCFG_MODE_FIBER_RGMII;
+	else
+		temp |= MII_M1111_HWCFG_MODE_COPPER_RGMII;
 
-		if (temp & MII_M1111_HWCFG_FIBER_COPPER_RES)
-			temp |= MII_M1111_HWCFG_MODE_FIBER_RGMII;
-		else
-			temp |= MII_M1111_HWCFG_MODE_COPPER_RGMII;
+	return phy_write(phydev, MII_M1111_PHY_EXT_SR, temp);
+}
 
-		err = phy_write(phydev, MII_M1111_PHY_EXT_SR, temp);
-		if (err < 0)
-			return err;
-	}
+static int m88e1111_config_init_sgmii(struct phy_device *phydev)
+{
+	int err;
+	int temp;
 
-	if (phydev->interface == PHY_INTERFACE_MODE_SGMII) {
-		temp = phy_read(phydev, MII_M1111_PHY_EXT_SR);
-		if (temp < 0)
-			return temp;
+	temp = phy_read(phydev, MII_M1111_PHY_EXT_SR);
+	if (temp < 0)
+		return temp;
 
-		temp &= ~(MII_M1111_HWCFG_MODE_MASK);
-		temp |= MII_M1111_HWCFG_MODE_SGMII_NO_CLK;
-		temp |= MII_M1111_HWCFG_FIBER_COPPER_AUTO;
+	temp &= ~(MII_M1111_HWCFG_MODE_MASK);
+	temp |= MII_M1111_HWCFG_MODE_SGMII_NO_CLK;
+	temp |= MII_M1111_HWCFG_FIBER_COPPER_AUTO;
 
-		err = phy_write(phydev, MII_M1111_PHY_EXT_SR, temp);
-		if (err < 0)
-			return err;
+	err = phy_write(phydev, MII_M1111_PHY_EXT_SR, temp);
+	if (err < 0)
+		return err;
 
-		/* make sure copper is selected */
-		err = phy_read(phydev, MII_M1145_PHY_EXT_ADDR_PAGE);
-		if (err < 0)
-			return err;
+	/* make sure copper is selected */
+	return marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
+}
 
-		err = phy_write(phydev, MII_M1145_PHY_EXT_ADDR_PAGE,
-				err & (~0xff));
-		if (err < 0)
-			return err;
-	}
+static int m88e1111_config_init_rtbi(struct phy_device *phydev)
+{
+	int err;
+	int temp;
 
-	if (phydev->interface == PHY_INTERFACE_MODE_RTBI) {
-		temp = phy_read(phydev, MII_M1111_PHY_EXT_CR);
-		if (temp < 0)
-			return temp;
-		temp |= (MII_M1111_RX_DELAY | MII_M1111_TX_DELAY);
-		err = phy_write(phydev, MII_M1111_PHY_EXT_CR, temp);
-		if (err < 0)
-			return err;
+	temp = phy_read(phydev, MII_M1111_PHY_EXT_CR);
+	if (temp < 0)
+		return temp;
 
-		temp = phy_read(phydev, MII_M1111_PHY_EXT_SR);
-		if (temp < 0)
-			return temp;
-		temp &= ~(MII_M1111_HWCFG_MODE_MASK | MII_M1111_HWCFG_FIBER_COPPER_RES);
-		temp |= 0x7 | MII_M1111_HWCFG_FIBER_COPPER_AUTO;
-		err = phy_write(phydev, MII_M1111_PHY_EXT_SR, temp);
-		if (err < 0)
+	temp |= (MII_M1111_RX_DELAY | MII_M1111_TX_DELAY);
+	err = phy_write(phydev, MII_M1111_PHY_EXT_CR, temp);
+	if (err < 0)
+		return err;
+
+	temp = phy_read(phydev, MII_M1111_PHY_EXT_SR);
+	if (temp < 0)
+		return temp;
+
+	temp &= ~(MII_M1111_HWCFG_MODE_MASK |
+		  MII_M1111_HWCFG_FIBER_COPPER_RES);
+	temp |= 0x7 | MII_M1111_HWCFG_FIBER_COPPER_AUTO;
+
+	err = phy_write(phydev, MII_M1111_PHY_EXT_SR, temp);
+	if (err < 0)
+		return err;
+
+	/* soft reset */
+	err = phy_write(phydev, MII_BMCR, BMCR_RESET);
+	if (err < 0)
+		return err;
+
+	do
+		temp = phy_read(phydev, MII_BMCR);
+	while (temp & BMCR_RESET);
+
+	temp = phy_read(phydev, MII_M1111_PHY_EXT_SR);
+	if (temp < 0)
+		return temp;
+
+	temp &= ~(MII_M1111_HWCFG_MODE_MASK |
+		  MII_M1111_HWCFG_FIBER_COPPER_RES);
+	temp |= MII_M1111_HWCFG_MODE_COPPER_RTBI |
+		MII_M1111_HWCFG_FIBER_COPPER_AUTO;
+
+	return phy_write(phydev, MII_M1111_PHY_EXT_SR, temp);
+}
+
+static int m88e1111_config_init(struct phy_device *phydev)
+{
+	int err;
+
+	if (phy_interface_is_rgmii(phydev)) {
+		err = m88e1111_config_init_rgmii(phydev);
+		if (err)
 			return err;
+	}
 
-		/* soft reset */
-		err = phy_write(phydev, MII_BMCR, BMCR_RESET);
+	if (phydev->interface == PHY_INTERFACE_MODE_SGMII) {
+		err = m88e1111_config_init_sgmii(phydev);
 		if (err < 0)
 			return err;
-		do
-			temp = phy_read(phydev, MII_BMCR);
-		while (temp & BMCR_RESET);
+	}
 
-		temp = phy_read(phydev, MII_M1111_PHY_EXT_SR);
-		if (temp < 0)
-			return temp;
-		temp &= ~(MII_M1111_HWCFG_MODE_MASK | MII_M1111_HWCFG_FIBER_COPPER_RES);
-		temp |= MII_M1111_HWCFG_MODE_COPPER_RTBI | MII_M1111_HWCFG_FIBER_COPPER_AUTO;
-		err = phy_write(phydev, MII_M1111_PHY_EXT_SR, temp);
+	if (phydev->interface == PHY_INTERFACE_MODE_RTBI) {
+		err = m88e1111_config_init_rtbi(phydev);
 		if (err < 0)
 			return err;
 	}
@@ -818,11 +857,9 @@ static int m88e1121_config_init(struct phy_device *phydev)
 {
 	int err, oldpage;
 
-	oldpage = phy_read(phydev, MII_MARVELL_PHY_PAGE);
-
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_88E1121_PHY_LED_PAGE);
-	if (err < 0)
-		return err;
+	oldpage = marvell_get_set_page(phydev, MII_MARVELL_LED_PAGE);
+	if (oldpage < 0)
+		return oldpage;
 
 	/* Default PHY LED config: LED[0] .. Link, LED[1] .. Activity */
 	err = phy_write(phydev, MII_88E1121_PHY_LED_CTRL,
@@ -830,7 +867,7 @@ static int m88e1121_config_init(struct phy_device *phydev)
 	if (err < 0)
 		return err;
 
-	phy_write(phydev, MII_MARVELL_PHY_PAGE, oldpage);
+	marvell_set_page(phydev, oldpage);
 
 	/* Set marvell,reg-init configuration from device tree */
 	return marvell_config_init(phydev);
@@ -844,7 +881,7 @@ static int m88e1510_config_init(struct phy_device *phydev)
 	/* SGMII-to-Copper mode initialization */
 	if (phydev->interface == PHY_INTERFACE_MODE_SGMII) {
 		/* Select page 18 */
-		err = phy_write(phydev, MII_MARVELL_PHY_PAGE, 18);
+		err = marvell_set_page(phydev, 18);
 		if (err < 0)
 			return err;
 
@@ -863,7 +900,7 @@ static int m88e1510_config_init(struct phy_device *phydev)
 			return err;
 
 		/* Reset page selection */
-		err = phy_write(phydev, MII_MARVELL_PHY_PAGE, 0);
+		err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 		if (err < 0)
 			return err;
 	}
@@ -893,7 +930,7 @@ static int m88e1118_config_init(struct phy_device *phydev)
 	int err;
 
 	/* Change address */
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, 0x0002);
+	err = marvell_set_page(phydev, MII_MARVELL_MSCR_PAGE);
 	if (err < 0)
 		return err;
 
@@ -903,7 +940,7 @@ static int m88e1118_config_init(struct phy_device *phydev)
 		return err;
 
 	/* Change address */
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, 0x0003);
+	err = marvell_set_page(phydev, MII_MARVELL_LED_PAGE);
 	if (err < 0)
 		return err;
 
@@ -920,7 +957,7 @@ static int m88e1118_config_init(struct phy_device *phydev)
 		return err;
 
 	/* Reset address */
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, 0x0);
+	err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 	if (err < 0)
 		return err;
 
@@ -932,7 +969,7 @@ static int m88e1149_config_init(struct phy_device *phydev)
 	int err;
 
 	/* Change address */
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, 0x0002);
+	err = marvell_set_page(phydev, MII_MARVELL_MSCR_PAGE);
 	if (err < 0)
 		return err;
 
@@ -946,17 +983,70 @@ static int m88e1149_config_init(struct phy_device *phydev)
 		return err;
 
 	/* Reset address */
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, 0x0);
+	err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 	if (err < 0)
 		return err;
 
 	return phy_write(phydev, MII_BMCR, BMCR_RESET);
 }
 
+static int m88e1145_config_init_rgmii(struct phy_device *phydev)
+{
+	int err;
+	int temp = phy_read(phydev, MII_M1145_PHY_EXT_CR);
+
+	if (temp < 0)
+		return temp;
+
+	temp |= (MII_M1145_RGMII_RX_DELAY | MII_M1145_RGMII_TX_DELAY);
+
+	err = phy_write(phydev, MII_M1145_PHY_EXT_CR, temp);
+	if (err < 0)
+		return err;
+
+	if (phydev->dev_flags & MARVELL_PHY_M1145_FLAGS_RESISTANCE) {
+		err = phy_write(phydev, 0x1d, 0x0012);
+		if (err < 0)
+			return err;
+
+		temp = phy_read(phydev, 0x1e);
+		if (temp < 0)
+			return temp;
+
+		temp &= 0xf03f;
+		temp |= 2 << 9;	/* 36 ohm */
+		temp |= 2 << 6;	/* 39 ohm */
+
+		err = phy_write(phydev, 0x1e, temp);
+		if (err < 0)
+			return err;
+
+		err = phy_write(phydev, 0x1d, 0x3);
+		if (err < 0)
+			return err;
+
+		err = phy_write(phydev, 0x1e, 0x8000);
+	}
+	return err;
+}
+
+static int m88e1145_config_init_sgmii(struct phy_device *phydev)
+{
+	int temp = phy_read(phydev, MII_M1145_PHY_EXT_SR);
+
+	if (temp < 0)
+		return temp;
+
+	temp &= ~MII_M1145_HWCFG_MODE_MASK;
+	temp |= MII_M1145_HWCFG_MODE_SGMII_NO_CLK;
+	temp |= MII_M1145_HWCFG_FIBER_COPPER_AUTO;
+
+	return phy_write(phydev, MII_M1145_PHY_EXT_SR, temp);
+}
+
 static int m88e1145_config_init(struct phy_device *phydev)
 {
 	int err;
-	int temp;
 
 	/* Take care of errata E0 & E1 */
 	err = phy_write(phydev, 0x1d, 0x001b);
@@ -976,53 +1066,13 @@ static int m88e1145_config_init(struct phy_device *phydev)
 		return err;
 
 	if (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID) {
-		int temp = phy_read(phydev, MII_M1145_PHY_EXT_CR);
-		if (temp < 0)
-			return temp;
-
-		temp |= (MII_M1145_RGMII_RX_DELAY | MII_M1145_RGMII_TX_DELAY);
-
-		err = phy_write(phydev, MII_M1145_PHY_EXT_CR, temp);
+		err = m88e1145_config_init_rgmii(phydev);
 		if (err < 0)
 			return err;
-
-		if (phydev->dev_flags & MARVELL_PHY_M1145_FLAGS_RESISTANCE) {
-			err = phy_write(phydev, 0x1d, 0x0012);
-			if (err < 0)
-				return err;
-
-			temp = phy_read(phydev, 0x1e);
-			if (temp < 0)
-				return temp;
-
-			temp &= 0xf03f;
-			temp |= 2 << 9;	/* 36 ohm */
-			temp |= 2 << 6;	/* 39 ohm */
-
-			err = phy_write(phydev, 0x1e, temp);
-			if (err < 0)
-				return err;
-
-			err = phy_write(phydev, 0x1d, 0x3);
-			if (err < 0)
-				return err;
-
-			err = phy_write(phydev, 0x1e, 0x8000);
-			if (err < 0)
-				return err;
-		}
 	}
 
 	if (phydev->interface == PHY_INTERFACE_MODE_SGMII) {
-		temp = phy_read(phydev, MII_M1145_PHY_EXT_SR);
-		if (temp < 0)
-			return temp;
-
-		temp &= ~MII_M1145_HWCFG_MODE_MASK;
-		temp |= MII_M1145_HWCFG_MODE_SGMII_NO_CLK;
-		temp |= MII_M1145_HWCFG_FIBER_COPPER_AUTO;
-
-		err = phy_write(phydev, MII_M1145_PHY_EXT_SR, temp);
+		err = m88e1145_config_init_sgmii(phydev);
 		if (err < 0)
 			return err;
 	}
@@ -1065,7 +1115,8 @@ static int marvell_update_link(struct phy_device *phydev, int fiber)
 	int status;
 
 	/* Use the generic register for copper link, or specific
-	 * register for fiber case */
+	 * register for fiber case
+	 */
 	if (fiber) {
 		status = phy_read(phydev, MII_M1011_PHY_STATUS);
 		if (status < 0)
@@ -1082,123 +1133,136 @@ static int marvell_update_link(struct phy_device *phydev, int fiber)
 	return 0;
 }
 
-/* marvell_read_status_page
- *
- * Description:
- *   Check the link, then figure out the current state
- *   by comparing what we advertise with what the link partner
- *   advertises.  Start by checking the gigabit possibilities,
- *   then move on to 10/100.
- */
-static int marvell_read_status_page(struct phy_device *phydev, int page)
+static int marvell_read_status_page_an(struct phy_device *phydev,
+				       int fiber)
 {
-	int adv;
-	int err;
+	int status;
 	int lpa;
 	int lpagb;
-	int status = 0;
-	int fiber;
-
-	/* Detect and update the link, but return if there
-	 * was an error */
-	if (page == MII_M1111_FIBER)
-		fiber = 1;
-	else
-		fiber = 0;
 
-	err = marvell_update_link(phydev, fiber);
-	if (err)
-		return err;
+	status = phy_read(phydev, MII_M1011_PHY_STATUS);
+	if (status < 0)
+		return status;
 
-	if (AUTONEG_ENABLE == phydev->autoneg) {
-		status = phy_read(phydev, MII_M1011_PHY_STATUS);
-		if (status < 0)
-			return status;
+	lpa = phy_read(phydev, MII_LPA);
+	if (lpa < 0)
+		return lpa;
 
-		lpa = phy_read(phydev, MII_LPA);
-		if (lpa < 0)
-			return lpa;
+	lpagb = phy_read(phydev, MII_STAT1000);
+	if (lpagb < 0)
+		return lpagb;
 
-		lpagb = phy_read(phydev, MII_STAT1000);
-		if (lpagb < 0)
-			return lpagb;
+	if (status & MII_M1011_PHY_STATUS_FULLDUPLEX)
+		phydev->duplex = DUPLEX_FULL;
+	else
+		phydev->duplex = DUPLEX_HALF;
 
-		adv = phy_read(phydev, MII_ADVERTISE);
-		if (adv < 0)
-			return adv;
+	status = status & MII_M1011_PHY_STATUS_SPD_MASK;
+	phydev->pause = 0;
+	phydev->asym_pause = 0;
 
-		if (status & MII_M1011_PHY_STATUS_FULLDUPLEX)
-			phydev->duplex = DUPLEX_FULL;
-		else
-			phydev->duplex = DUPLEX_HALF;
+	switch (status) {
+	case MII_M1011_PHY_STATUS_1000:
+		phydev->speed = SPEED_1000;
+		break;
 
-		status = status & MII_M1011_PHY_STATUS_SPD_MASK;
-		phydev->pause = phydev->asym_pause = 0;
+	case MII_M1011_PHY_STATUS_100:
+		phydev->speed = SPEED_100;
+		break;
 
-		switch (status) {
-		case MII_M1011_PHY_STATUS_1000:
-			phydev->speed = SPEED_1000;
-			break;
+	default:
+		phydev->speed = SPEED_10;
+		break;
+	}
 
-		case MII_M1011_PHY_STATUS_100:
-			phydev->speed = SPEED_100;
-			break;
+	if (!fiber) {
+		phydev->lp_advertising =
+			mii_stat1000_to_ethtool_lpa_t(lpagb) |
+			mii_lpa_to_ethtool_lpa_t(lpa);
 
-		default:
-			phydev->speed = SPEED_10;
-			break;
+		if (phydev->duplex == DUPLEX_FULL) {
+			phydev->pause = lpa & LPA_PAUSE_CAP ? 1 : 0;
+			phydev->asym_pause = lpa & LPA_PAUSE_ASYM ? 1 : 0;
 		}
-
-		if (!fiber) {
-			phydev->lp_advertising = mii_stat1000_to_ethtool_lpa_t(lpagb) |
-					 mii_lpa_to_ethtool_lpa_t(lpa);
-
-			if (phydev->duplex == DUPLEX_FULL) {
-				phydev->pause = lpa & LPA_PAUSE_CAP ? 1 : 0;
-				phydev->asym_pause = lpa & LPA_PAUSE_ASYM ? 1 : 0;
-			}
-		} else {
-			/* The fiber link is only 1000M capable */
-			phydev->lp_advertising = fiber_lpa_to_ethtool_lpa_t(lpa);
-
-			if (phydev->duplex == DUPLEX_FULL) {
-				if (!(lpa & LPA_PAUSE_FIBER)) {
-					phydev->pause = 0;
-					phydev->asym_pause = 0;
-				} else if ((lpa & LPA_PAUSE_ASYM_FIBER)) {
-					phydev->pause = 1;
-					phydev->asym_pause = 1;
-				} else {
-					phydev->pause = 1;
-					phydev->asym_pause = 0;
-				}
+	} else {
+		/* The fiber link is only 1000M capable */
+		phydev->lp_advertising = fiber_lpa_to_ethtool_lpa_t(lpa);
+
+		if (phydev->duplex == DUPLEX_FULL) {
+			if (!(lpa & LPA_PAUSE_FIBER)) {
+				phydev->pause = 0;
+				phydev->asym_pause = 0;
+			} else if ((lpa & LPA_PAUSE_ASYM_FIBER)) {
+				phydev->pause = 1;
+				phydev->asym_pause = 1;
+			} else {
+				phydev->pause = 1;
+				phydev->asym_pause = 0;
 			}
 		}
-	} else {
-		int bmcr = phy_read(phydev, MII_BMCR);
+	}
+	return 0;
+}
 
-		if (bmcr < 0)
-			return bmcr;
+static int marvell_read_status_page_fixed(struct phy_device *phydev)
+{
+	int bmcr = phy_read(phydev, MII_BMCR);
 
-		if (bmcr & BMCR_FULLDPLX)
-			phydev->duplex = DUPLEX_FULL;
-		else
-			phydev->duplex = DUPLEX_HALF;
+	if (bmcr < 0)
+		return bmcr;
 
-		if (bmcr & BMCR_SPEED1000)
-			phydev->speed = SPEED_1000;
-		else if (bmcr & BMCR_SPEED100)
-			phydev->speed = SPEED_100;
-		else
-			phydev->speed = SPEED_10;
+	if (bmcr & BMCR_FULLDPLX)
+		phydev->duplex = DUPLEX_FULL;
+	else
+		phydev->duplex = DUPLEX_HALF;
 
-		phydev->pause = phydev->asym_pause = 0;
-		phydev->lp_advertising = 0;
-	}
+	if (bmcr & BMCR_SPEED1000)
+		phydev->speed = SPEED_1000;
+	else if (bmcr & BMCR_SPEED100)
+		phydev->speed = SPEED_100;
+	else
+		phydev->speed = SPEED_10;
+
+	phydev->pause = 0;
+	phydev->asym_pause = 0;
+	phydev->lp_advertising = 0;
 
 	return 0;
 }
 
+/* marvell_read_status_page
+ *
+ * Description:
+ *   Check the link, then figure out the current state
+ *   by comparing what we advertise with what the link partner
+ *   advertises.  Start by checking the gigabit possibilities,
+ *   then move on to 10/100.
+ */
+static int marvell_read_status_page(struct phy_device *phydev, int page)
+{
+	int fiber;
+	int err;
+
+	/* Detect and update the link, but return if there
+	 * was an error
+	 */
+	if (page == MII_MARVELL_FIBER_PAGE)
+		fiber = 1;
+	else
+		fiber = 0;
+
+	err = marvell_update_link(phydev, fiber);
+	if (err)
+		return err;
+
+	if (phydev->autoneg == AUTONEG_ENABLE)
+		err = marvell_read_status_page_an(phydev, fiber);
+	else
+		err = marvell_read_status_page_fixed(phydev);
+
+	return err;
+}
+
 /* marvell_read_status
  *
  * Some Marvell's phys have two modes: fiber and copper.
@@ -1215,33 +1279,34 @@ static int marvell_read_status(struct phy_device *phydev)
 	/* Check the fiber mode first */
 	if (phydev->supported & SUPPORTED_FIBRE &&
 	    phydev->interface != PHY_INTERFACE_MODE_SGMII) {
-		err = phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_M1111_FIBER);
+		err = marvell_set_page(phydev, MII_MARVELL_FIBER_PAGE);
 		if (err < 0)
 			goto error;
 
-		err = marvell_read_status_page(phydev, MII_M1111_FIBER);
+		err = marvell_read_status_page(phydev, MII_MARVELL_FIBER_PAGE);
 		if (err < 0)
 			goto error;
 
-		/* If the fiber link is up, it is the selected and used link.
-		 * In this case, we need to stay in the fiber page.
-		 * Please to be careful about that, avoid to restore Copper page
-		 * in other functions which could break the behaviour
-		 * for some fiber phy like 88E1512.
-		 * */
+		/* If the fiber link is up, it is the selected and
+		 * used link. In this case, we need to stay in the
+		 * fiber page. Please to be careful about that, avoid
+		 * to restore Copper page in other functions which
+		 * could break the behaviour for some fiber phy like
+		 * 88E1512.
+		 */
 		if (phydev->link)
 			return 0;
 
 		/* If fiber link is down, check and save copper mode state */
-		err = phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_M1111_COPPER);
+		err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 		if (err < 0)
 			goto error;
 	}
 
-	return marvell_read_status_page(phydev, MII_M1111_COPPER);
+	return marvell_read_status_page(phydev, MII_MARVELL_COPPER_PAGE);
 
 error:
-	phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_M1111_COPPER);
+	marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 	return err;
 }
 
@@ -1256,7 +1321,7 @@ static int marvell_suspend(struct phy_device *phydev)
 
 	/* Suspend the fiber mode first */
 	if (!(phydev->supported & SUPPORTED_FIBRE)) {
-		err = phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_M1111_FIBER);
+		err = marvell_set_page(phydev, MII_MARVELL_FIBER_PAGE);
 		if (err < 0)
 			goto error;
 
@@ -1266,7 +1331,7 @@ static int marvell_suspend(struct phy_device *phydev)
 			goto error;
 
 		/* Then, the copper link */
-		err = phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_M1111_COPPER);
+		err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 		if (err < 0)
 			goto error;
 	}
@@ -1275,7 +1340,7 @@ static int marvell_suspend(struct phy_device *phydev)
 	return genphy_suspend(phydev);
 
 error:
-	phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_M1111_COPPER);
+	marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 	return err;
 }
 
@@ -1290,7 +1355,7 @@ static int marvell_resume(struct phy_device *phydev)
 
 	/* Resume the fiber mode first */
 	if (!(phydev->supported & SUPPORTED_FIBRE)) {
-		err = phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_M1111_FIBER);
+		err = marvell_set_page(phydev, MII_MARVELL_FIBER_PAGE);
 		if (err < 0)
 			goto error;
 
@@ -1300,7 +1365,7 @@ static int marvell_resume(struct phy_device *phydev)
 			goto error;
 
 		/* Then, the copper link */
-		err = phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_M1111_COPPER);
+		err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 		if (err < 0)
 			goto error;
 	}
@@ -1309,13 +1374,14 @@ static int marvell_resume(struct phy_device *phydev)
 	return genphy_resume(phydev);
 
 error:
-	phy_write(phydev, MII_MARVELL_PHY_PAGE, MII_M1111_COPPER);
+	marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 	return err;
 }
 
 static int marvell_aneg_done(struct phy_device *phydev)
 {
 	int retval = phy_read(phydev, MII_M1011_PHY_STATUS);
+
 	return (retval < 0) ? retval : (retval & MII_M1011_PHY_STATUS_RESOLVED);
 }
 
@@ -1331,32 +1397,33 @@ static int m88e1121_did_interrupt(struct phy_device *phydev)
 	return 0;
 }
 
-static void m88e1318_get_wol(struct phy_device *phydev, struct ethtool_wolinfo *wol)
+static void m88e1318_get_wol(struct phy_device *phydev,
+			     struct ethtool_wolinfo *wol)
 {
 	wol->supported = WAKE_MAGIC;
 	wol->wolopts = 0;
 
-	if (phy_write(phydev, MII_MARVELL_PHY_PAGE,
-		      MII_88E1318S_PHY_WOL_PAGE) < 0)
+	if (marvell_set_page(phydev, MII_MARVELL_WOL_PAGE) < 0)
 		return;
 
 	if (phy_read(phydev, MII_88E1318S_PHY_WOL_CTRL) &
 	    MII_88E1318S_PHY_WOL_CTRL_MAGIC_PACKET_MATCH_ENABLE)
 		wol->wolopts |= WAKE_MAGIC;
 
-	if (phy_write(phydev, MII_MARVELL_PHY_PAGE, 0x00) < 0)
+	if (marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE) < 0)
 		return;
 }
 
-static int m88e1318_set_wol(struct phy_device *phydev, struct ethtool_wolinfo *wol)
+static int m88e1318_set_wol(struct phy_device *phydev,
+			    struct ethtool_wolinfo *wol)
 {
 	int err, oldpage, temp;
 
-	oldpage = phy_read(phydev, MII_MARVELL_PHY_PAGE);
+	oldpage = marvell_get_page(phydev);
 
 	if (wol->wolopts & WAKE_MAGIC) {
 		/* Explicitly switch to page 0x00, just to be sure */
-		err = phy_write(phydev, MII_MARVELL_PHY_PAGE, 0x00);
+		err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 		if (err < 0)
 			return err;
 
@@ -1367,8 +1434,7 @@ static int m88e1318_set_wol(struct phy_device *phydev, struct ethtool_wolinfo *w
 		if (err < 0)
 			return err;
 
-		err = phy_write(phydev, MII_MARVELL_PHY_PAGE,
-				MII_88E1318S_PHY_LED_PAGE);
+		err = marvell_set_page(phydev, MII_MARVELL_LED_PAGE);
 		if (err < 0)
 			return err;
 
@@ -1381,8 +1447,7 @@ static int m88e1318_set_wol(struct phy_device *phydev, struct ethtool_wolinfo *w
 		if (err < 0)
 			return err;
 
-		err = phy_write(phydev, MII_MARVELL_PHY_PAGE,
-				MII_88E1318S_PHY_WOL_PAGE);
+		err = marvell_set_page(phydev, MII_MARVELL_WOL_PAGE);
 		if (err < 0)
 			return err;
 
@@ -1411,8 +1476,7 @@ static int m88e1318_set_wol(struct phy_device *phydev, struct ethtool_wolinfo *w
 		if (err < 0)
 			return err;
 	} else {
-		err = phy_write(phydev, MII_MARVELL_PHY_PAGE,
-				MII_88E1318S_PHY_WOL_PAGE);
+		err = marvell_set_page(phydev, MII_MARVELL_WOL_PAGE);
 		if (err < 0)
 			return err;
 
@@ -1425,7 +1489,7 @@ static int m88e1318_set_wol(struct phy_device *phydev, struct ethtool_wolinfo *w
 			return err;
 	}
 
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE, oldpage);
+	err = marvell_set_page(phydev, oldpage);
 	if (err < 0)
 		return err;
 
@@ -1457,13 +1521,11 @@ static u64 marvell_get_stat(struct phy_device *phydev, int i)
 {
 	struct marvell_hw_stat stat = marvell_hw_stats[i];
 	struct marvell_priv *priv = phydev->priv;
-	int err, oldpage, val;
+	int oldpage, val;
 	u64 ret;
 
-	oldpage = phy_read(phydev, MII_MARVELL_PHY_PAGE);
-	err = phy_write(phydev, MII_MARVELL_PHY_PAGE,
-			stat.page);
-	if (err < 0)
+	oldpage = marvell_get_set_page(phydev, stat.page);
+	if (oldpage < 0)
 		return UINT64_MAX;
 
 	val = phy_read(phydev, stat.reg);
@@ -1475,7 +1537,7 @@ static u64 marvell_get_stat(struct phy_device *phydev, int i)
 		ret = priv->stats[i];
 	}
 
-	phy_write(phydev, MII_MARVELL_PHY_PAGE, oldpage);
+	marvell_set_page(phydev, oldpage);
 
 	return ret;
 }
@@ -1492,6 +1554,7 @@ static void marvell_get_stats(struct phy_device *phydev,
 #ifdef CONFIG_HWMON
 static int m88e1121_get_temp(struct phy_device *phydev, long *temp)
 {
+	int oldpage;
 	int ret;
 	int val;
 
@@ -1499,9 +1562,11 @@ static int m88e1121_get_temp(struct phy_device *phydev, long *temp)
 
 	mutex_lock(&phydev->lock);
 
-	ret = phy_write(phydev, MII_M1145_PHY_EXT_ADDR_PAGE, 0x6);
-	if (ret < 0)
-		goto error;
+	oldpage = marvell_get_set_page(phydev, MII_MARVELL_MISC_TEST_PAGE);
+	if (oldpage < 0) {
+		mutex_unlock(&phydev->lock);
+		return oldpage;
+	}
 
 	/* Enable temperature sensor */
 	ret = phy_read(phydev, MII_88E1121_MISC_TEST);
@@ -1531,7 +1596,7 @@ static int m88e1121_get_temp(struct phy_device *phydev, long *temp)
 	*temp = ((val & MII_88E1121_MISC_TEST_TEMP_MASK) - 5) * 5000;
 
 error:
-	phy_write(phydev, MII_M1145_PHY_EXT_ADDR_PAGE, 0x0);
+	marvell_set_page(phydev, oldpage);
 	mutex_unlock(&phydev->lock);
 
 	return ret;
@@ -1608,15 +1673,18 @@ static const struct hwmon_chip_info m88e1121_hwmon_chip_info = {
 
 static int m88e1510_get_temp(struct phy_device *phydev, long *temp)
 {
+	int oldpage;
 	int ret;
 
 	*temp = 0;
 
 	mutex_lock(&phydev->lock);
 
-	ret = phy_write(phydev, MII_M1145_PHY_EXT_ADDR_PAGE, 0x6);
-	if (ret < 0)
-		goto error;
+	oldpage = marvell_get_set_page(phydev, MII_MARVELL_MISC_TEST_PAGE);
+	if (oldpage < 0) {
+		mutex_unlock(&phydev->lock);
+		return oldpage;
+	}
 
 	ret = phy_read(phydev, MII_88E1510_TEMP_SENSOR);
 	if (ret < 0)
@@ -1625,23 +1693,26 @@ static int m88e1510_get_temp(struct phy_device *phydev, long *temp)
 	*temp = ((ret & MII_88E1510_TEMP_SENSOR_MASK) - 25) * 1000;
 
 error:
-	phy_write(phydev, MII_M1145_PHY_EXT_ADDR_PAGE, 0x0);
+	marvell_set_page(phydev, oldpage);
 	mutex_unlock(&phydev->lock);
 
 	return ret;
 }
 
-int m88e1510_get_temp_critical(struct phy_device *phydev, long *temp)
+static int m88e1510_get_temp_critical(struct phy_device *phydev, long *temp)
 {
+	int oldpage;
 	int ret;
 
 	*temp = 0;
 
 	mutex_lock(&phydev->lock);
 
-	ret = phy_write(phydev, MII_M1145_PHY_EXT_ADDR_PAGE, 0x6);
-	if (ret < 0)
-		goto error;
+	oldpage = marvell_get_set_page(phydev, MII_MARVELL_MISC_TEST_PAGE);
+	if (oldpage < 0) {
+		mutex_unlock(&phydev->lock);
+		return oldpage;
+	}
 
 	ret = phy_read(phydev, MII_88E1121_MISC_TEST);
 	if (ret < 0)
@@ -1653,21 +1724,24 @@ int m88e1510_get_temp_critical(struct phy_device *phydev, long *temp)
 	*temp *= 1000;
 
 error:
-	phy_write(phydev, MII_M1145_PHY_EXT_ADDR_PAGE, 0x0);
+	marvell_set_page(phydev, oldpage);
 	mutex_unlock(&phydev->lock);
 
 	return ret;
 }
 
-int m88e1510_set_temp_critical(struct phy_device *phydev, long temp)
+static int m88e1510_set_temp_critical(struct phy_device *phydev, long temp)
 {
+	int oldpage;
 	int ret;
 
 	mutex_lock(&phydev->lock);
 
-	ret = phy_write(phydev, MII_M1145_PHY_EXT_ADDR_PAGE, 0x6);
-	if (ret < 0)
-		goto error;
+	oldpage = marvell_get_set_page(phydev, MII_MARVELL_MISC_TEST_PAGE);
+	if (oldpage < 0) {
+		mutex_unlock(&phydev->lock);
+		return oldpage;
+	}
 
 	ret = phy_read(phydev, MII_88E1121_MISC_TEST);
 	if (ret < 0)
@@ -1680,23 +1754,26 @@ int m88e1510_set_temp_critical(struct phy_device *phydev, long temp)
 			(temp << MII_88E1510_MISC_TEST_TEMP_THRESHOLD_SHIFT));
 
 error:
-	phy_write(phydev, MII_M1145_PHY_EXT_ADDR_PAGE, 0x0);
+	marvell_set_page(phydev, oldpage);
 	mutex_unlock(&phydev->lock);
 
 	return ret;
 }
 
-int m88e1510_get_temp_alarm(struct phy_device *phydev, long *alarm)
+static int m88e1510_get_temp_alarm(struct phy_device *phydev, long *alarm)
 {
+	int oldpage;
 	int ret;
 
 	*alarm = false;
 
 	mutex_lock(&phydev->lock);
 
-	ret = phy_write(phydev, MII_M1145_PHY_EXT_ADDR_PAGE, 0x6);
-	if (ret < 0)
-		goto error;
+	oldpage = marvell_get_set_page(phydev, MII_MARVELL_MISC_TEST_PAGE);
+	if (oldpage < 0) {
+		mutex_unlock(&phydev->lock);
+		return oldpage;
+	}
 
 	ret = phy_read(phydev, MII_88E1121_MISC_TEST);
 	if (ret < 0)
@@ -1704,7 +1781,7 @@ int m88e1510_get_temp_alarm(struct phy_device *phydev, long *alarm)
 	*alarm = !!(ret & MII_88E1510_MISC_TEST_TEMP_IRQ);
 
 error:
-	phy_write(phydev, MII_M1145_PHY_EXT_ADDR_PAGE, 0x0);
+	marvell_set_page(phydev, oldpage);
 	mutex_unlock(&phydev->lock);
 
 	return ret;
@@ -2094,6 +2171,7 @@ static struct phy_driver marvell_drivers[] = {
 		.get_sset_count = marvell_get_sset_count,
 		.get_strings = marvell_get_strings,
 		.get_stats = marvell_get_stats,
+		.set_loopback = genphy_loopback,
 	},
 	{
 		.phy_id = MARVELL_PHY_ID_88E1540,
diff --git a/drivers/net/phy/marvell10g.c b/drivers/net/phy/marvell10g.c
new file mode 100644
index 000000000000..aebc08beceba
--- /dev/null
+++ b/drivers/net/phy/marvell10g.c
@@ -0,0 +1,368 @@
+/*
+ * Marvell 10G 88x3310 PHY driver
+ *
+ * Based upon the ID registers, this PHY appears to be a mixture of IPs
+ * from two different companies.
+ *
+ * There appears to be several different data paths through the PHY which
+ * are automatically managed by the PHY.  The following has been determined
+ * via observation and experimentation:
+ *
+ *       SGMII PHYXS -- BASE-T PCS -- 10G PMA -- AN -- Copper (for <= 1G)
+ *  10GBASE-KR PHYXS -- BASE-T PCS -- 10G PMA -- AN -- Copper (for 10G)
+ *  10GBASE-KR PHYXS -- BASE-R PCS -- Fiber
+ *
+ * If both the fiber and copper ports are connected, the first to gain
+ * link takes priority and the other port is completely locked out.
+ */
+#include <linux/phy.h>
+
+enum {
+	MV_PCS_BASE_T		= 0x0000,
+	MV_PCS_BASE_R		= 0x1000,
+	MV_PCS_1000BASEX	= 0x2000,
+
+	/* These registers appear at 0x800X and 0xa00X - the 0xa00X control
+	 * registers appear to set themselves to the 0x800X when AN is
+	 * restarted, but status registers appear readable from either.
+	 */
+	MV_AN_CTRL1000		= 0x8000, /* 1000base-T control register */
+	MV_AN_STAT1000		= 0x8001, /* 1000base-T status register */
+
+	/* This register appears to reflect the copper status */
+	MV_AN_RESULT		= 0xa016,
+	MV_AN_RESULT_SPD_10	= BIT(12),
+	MV_AN_RESULT_SPD_100	= BIT(13),
+	MV_AN_RESULT_SPD_1000	= BIT(14),
+	MV_AN_RESULT_SPD_10000	= BIT(15),
+};
+
+static int mv3310_modify(struct phy_device *phydev, int devad, u16 reg,
+			 u16 mask, u16 bits)
+{
+	int old, val, ret;
+
+	old = phy_read_mmd(phydev, devad, reg);
+	if (old < 0)
+		return old;
+
+	val = (old & ~mask) | (bits & mask);
+	if (val == old)
+		return 0;
+
+	ret = phy_write_mmd(phydev, devad, reg, val);
+
+	return ret < 0 ? ret : 1;
+}
+
+static int mv3310_probe(struct phy_device *phydev)
+{
+	u32 mmd_mask = MDIO_DEVS_PMAPMD | MDIO_DEVS_AN;
+
+	if (!phydev->is_c45 ||
+	    (phydev->c45_ids.devices_in_package & mmd_mask) != mmd_mask)
+		return -ENODEV;
+
+	return 0;
+}
+
+/*
+ * Resetting the MV88X3310 causes it to become non-responsive.  Avoid
+ * setting the reset bit(s).
+ */
+static int mv3310_soft_reset(struct phy_device *phydev)
+{
+	return 0;
+}
+
+static int mv3310_config_init(struct phy_device *phydev)
+{
+	__ETHTOOL_DECLARE_LINK_MODE_MASK(supported) = { 0, };
+	u32 mask;
+	int val;
+
+	/* Check that the PHY interface type is compatible */
+	if (phydev->interface != PHY_INTERFACE_MODE_SGMII &&
+	    phydev->interface != PHY_INTERFACE_MODE_XGMII &&
+	    phydev->interface != PHY_INTERFACE_MODE_XAUI &&
+	    phydev->interface != PHY_INTERFACE_MODE_RXAUI &&
+	    phydev->interface != PHY_INTERFACE_MODE_10GKR)
+		return -ENODEV;
+
+	__set_bit(ETHTOOL_LINK_MODE_Pause_BIT, supported);
+	__set_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT, supported);
+
+	if (phydev->c45_ids.devices_in_package & MDIO_DEVS_AN) {
+		val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_STAT1);
+		if (val < 0)
+			return val;
+
+		if (val & MDIO_AN_STAT1_ABLE)
+			__set_bit(ETHTOOL_LINK_MODE_Autoneg_BIT, supported);
+	}
+
+	val = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_STAT2);
+	if (val < 0)
+		return val;
+
+	/* Ethtool does not support the WAN mode bits */
+	if (val & (MDIO_PMA_STAT2_10GBSR | MDIO_PMA_STAT2_10GBLR |
+		   MDIO_PMA_STAT2_10GBER | MDIO_PMA_STAT2_10GBLX4 |
+		   MDIO_PMA_STAT2_10GBSW | MDIO_PMA_STAT2_10GBLW |
+		   MDIO_PMA_STAT2_10GBEW))
+		__set_bit(ETHTOOL_LINK_MODE_FIBRE_BIT, supported);
+	if (val & MDIO_PMA_STAT2_10GBSR)
+		__set_bit(ETHTOOL_LINK_MODE_10000baseSR_Full_BIT, supported);
+	if (val & MDIO_PMA_STAT2_10GBLR)
+		__set_bit(ETHTOOL_LINK_MODE_10000baseLR_Full_BIT, supported);
+	if (val & MDIO_PMA_STAT2_10GBER)
+		__set_bit(ETHTOOL_LINK_MODE_10000baseER_Full_BIT, supported);
+
+	if (val & MDIO_PMA_STAT2_EXTABLE) {
+		val = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_PMA_EXTABLE);
+		if (val < 0)
+			return val;
+
+		if (val & (MDIO_PMA_EXTABLE_10GBT | MDIO_PMA_EXTABLE_1000BT |
+			   MDIO_PMA_EXTABLE_100BTX | MDIO_PMA_EXTABLE_10BT))
+			__set_bit(ETHTOOL_LINK_MODE_TP_BIT, supported);
+		if (val & MDIO_PMA_EXTABLE_10GBLRM)
+			__set_bit(ETHTOOL_LINK_MODE_FIBRE_BIT, supported);
+		if (val & (MDIO_PMA_EXTABLE_10GBKX4 | MDIO_PMA_EXTABLE_10GBKR |
+			   MDIO_PMA_EXTABLE_1000BKX))
+			__set_bit(ETHTOOL_LINK_MODE_Backplane_BIT, supported);
+		if (val & MDIO_PMA_EXTABLE_10GBLRM)
+			__set_bit(ETHTOOL_LINK_MODE_10000baseLRM_Full_BIT,
+				  supported);
+		if (val & MDIO_PMA_EXTABLE_10GBT)
+			__set_bit(ETHTOOL_LINK_MODE_10000baseT_Full_BIT,
+				  supported);
+		if (val & MDIO_PMA_EXTABLE_10GBKX4)
+			__set_bit(ETHTOOL_LINK_MODE_10000baseKX4_Full_BIT,
+				  supported);
+		if (val & MDIO_PMA_EXTABLE_10GBKR)
+			__set_bit(ETHTOOL_LINK_MODE_10000baseKR_Full_BIT,
+				  supported);
+		if (val & MDIO_PMA_EXTABLE_1000BT)
+			__set_bit(ETHTOOL_LINK_MODE_1000baseT_Full_BIT,
+				  supported);
+		if (val & MDIO_PMA_EXTABLE_1000BKX)
+			__set_bit(ETHTOOL_LINK_MODE_1000baseKX_Full_BIT,
+				  supported);
+		if (val & MDIO_PMA_EXTABLE_100BTX)
+			__set_bit(ETHTOOL_LINK_MODE_100baseT_Full_BIT,
+				  supported);
+		if (val & MDIO_PMA_EXTABLE_10BT)
+			__set_bit(ETHTOOL_LINK_MODE_10baseT_Full_BIT,
+				  supported);
+	}
+
+	if (!ethtool_convert_link_mode_to_legacy_u32(&mask, supported))
+		dev_warn(&phydev->mdio.dev,
+			 "PHY supports (%*pb) more modes than phylib supports, some modes not supported.\n",
+			 __ETHTOOL_LINK_MODE_MASK_NBITS, supported);
+
+	phydev->supported &= mask;
+	phydev->advertising &= phydev->supported;
+
+	return 0;
+}
+
+static int mv3310_config_aneg(struct phy_device *phydev)
+{
+	bool changed = false;
+	u32 advertising;
+	int ret;
+
+	if (phydev->autoneg == AUTONEG_DISABLE) {
+		ret = genphy_c45_pma_setup_forced(phydev);
+		if (ret < 0)
+			return ret;
+
+		return genphy_c45_an_disable_aneg(phydev);
+	}
+
+	phydev->advertising &= phydev->supported;
+	advertising = phydev->advertising;
+
+	ret = mv3310_modify(phydev, MDIO_MMD_AN, MDIO_AN_ADVERTISE,
+			    ADVERTISE_ALL | ADVERTISE_100BASE4 |
+			    ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM,
+			    ethtool_adv_to_mii_adv_t(advertising));
+	if (ret < 0)
+		return ret;
+	if (ret > 0)
+		changed = true;
+
+	ret = mv3310_modify(phydev, MDIO_MMD_AN, MV_AN_CTRL1000,
+			    ADVERTISE_1000FULL | ADVERTISE_1000HALF,
+			    ethtool_adv_to_mii_ctrl1000_t(advertising));
+	if (ret < 0)
+		return ret;
+	if (ret > 0)
+		changed = true;
+
+	/* 10G control register */
+	ret = mv3310_modify(phydev, MDIO_MMD_AN, MDIO_AN_10GBT_CTRL,
+			    MDIO_AN_10GBT_CTRL_ADV10G,
+			    advertising & ADVERTISED_10000baseT_Full ?
+				MDIO_AN_10GBT_CTRL_ADV10G : 0);
+	if (ret < 0)
+		return ret;
+	if (ret > 0)
+		changed = true;
+
+	if (changed)
+		ret = genphy_c45_restart_aneg(phydev);
+
+	return ret;
+}
+
+static int mv3310_aneg_done(struct phy_device *phydev)
+{
+	int val;
+
+	val = phy_read_mmd(phydev, MDIO_MMD_PCS, MV_PCS_BASE_R + MDIO_STAT1);
+	if (val < 0)
+		return val;
+
+	if (val & MDIO_STAT1_LSTATUS)
+		return 1;
+
+	return genphy_c45_aneg_done(phydev);
+}
+
+/* 10GBASE-ER,LR,LRM,SR do not support autonegotiation. */
+static int mv3310_read_10gbr_status(struct phy_device *phydev)
+{
+	phydev->link = 1;
+	phydev->speed = SPEED_10000;
+	phydev->duplex = DUPLEX_FULL;
+
+	if (phydev->interface == PHY_INTERFACE_MODE_SGMII)
+		phydev->interface = PHY_INTERFACE_MODE_10GKR;
+
+	return 0;
+}
+
+static int mv3310_read_status(struct phy_device *phydev)
+{
+	u32 mmd_mask = phydev->c45_ids.devices_in_package;
+	int val;
+
+	/* The vendor devads do not report link status.  Avoid the PHYXS
+	 * instance as there are three, and its status depends on the MAC
+	 * being appropriately configured for the negotiated speed.
+	 */
+	mmd_mask &= ~(BIT(MDIO_MMD_VEND1) | BIT(MDIO_MMD_VEND2) |
+		      BIT(MDIO_MMD_PHYXS));
+
+	phydev->speed = SPEED_UNKNOWN;
+	phydev->duplex = DUPLEX_UNKNOWN;
+	phydev->lp_advertising = 0;
+	phydev->link = 0;
+	phydev->pause = 0;
+	phydev->asym_pause = 0;
+
+	val = phy_read_mmd(phydev, MDIO_MMD_PCS, MV_PCS_BASE_R + MDIO_STAT1);
+	if (val < 0)
+		return val;
+
+	if (val & MDIO_STAT1_LSTATUS)
+		return mv3310_read_10gbr_status(phydev);
+
+	val = genphy_c45_read_link(phydev, mmd_mask);
+	if (val < 0)
+		return val;
+
+	phydev->link = val > 0 ? 1 : 0;
+
+	val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_STAT1);
+	if (val < 0)
+		return val;
+
+	if (val & MDIO_AN_STAT1_COMPLETE) {
+		val = genphy_c45_read_lpa(phydev);
+		if (val < 0)
+			return val;
+
+		/* Read the link partner's 1G advertisment */
+		val = phy_read_mmd(phydev, MDIO_MMD_AN, MV_AN_STAT1000);
+		if (val < 0)
+			return val;
+
+		phydev->lp_advertising |= mii_stat1000_to_ethtool_lpa_t(val);
+
+		if (phydev->autoneg == AUTONEG_ENABLE) {
+			val = phy_read_mmd(phydev, MDIO_MMD_AN, MV_AN_RESULT);
+			if (val < 0)
+				return val;
+
+			if (val & MV_AN_RESULT_SPD_10000)
+				phydev->speed = SPEED_10000;
+			else if (val & MV_AN_RESULT_SPD_1000)
+				phydev->speed = SPEED_1000;
+			else if (val & MV_AN_RESULT_SPD_100)
+				phydev->speed = SPEED_100;
+			else if (val & MV_AN_RESULT_SPD_10)
+				phydev->speed = SPEED_10;
+
+			phydev->duplex = DUPLEX_FULL;
+		}
+	}
+
+	if (phydev->autoneg != AUTONEG_ENABLE) {
+		val = genphy_c45_read_pma(phydev);
+		if (val < 0)
+			return val;
+	}
+
+	if ((phydev->interface == PHY_INTERFACE_MODE_SGMII ||
+	     phydev->interface == PHY_INTERFACE_MODE_10GKR) && phydev->link) {
+		/* The PHY automatically switches its serdes interface (and
+		 * active PHYXS instance) between Cisco SGMII and 10GBase-KR
+		 * modes according to the speed.  Florian suggests setting
+		 * phydev->interface to communicate this to the MAC. Only do
+		 * this if we are already in either SGMII or 10GBase-KR mode.
+		 */
+		if (phydev->speed == SPEED_10000)
+			phydev->interface = PHY_INTERFACE_MODE_10GKR;
+		else if (phydev->speed >= SPEED_10 &&
+			 phydev->speed < SPEED_10000)
+			phydev->interface = PHY_INTERFACE_MODE_SGMII;
+	}
+
+	return 0;
+}
+
+static struct phy_driver mv3310_drivers[] = {
+	{
+		.phy_id		= 0x002b09aa,
+		.phy_id_mask	= 0xffffffff,
+		.name		= "mv88x3310",
+		.features	= SUPPORTED_10baseT_Full |
+				  SUPPORTED_100baseT_Full |
+				  SUPPORTED_1000baseT_Full |
+				  SUPPORTED_Autoneg |
+				  SUPPORTED_TP |
+				  SUPPORTED_FIBRE |
+				  SUPPORTED_10000baseT_Full |
+				  SUPPORTED_Backplane,
+		.probe		= mv3310_probe,
+		.soft_reset	= mv3310_soft_reset,
+		.config_init	= mv3310_config_init,
+		.config_aneg	= mv3310_config_aneg,
+		.aneg_done	= mv3310_aneg_done,
+		.read_status	= mv3310_read_status,
+	},
+};
+
+module_phy_driver(mv3310_drivers);
+
+static struct mdio_device_id __maybe_unused mv3310_tbl[] = {
+	{ 0x002b09aa, 0xffffffff },
+	{ },
+};
+MODULE_DEVICE_TABLE(mdio, mv3310_tbl);
+MODULE_DESCRIPTION("Marvell Alaska X 10Gigabit Ethernet PHY driver (MV88X3310)");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/phy/mdio-mux.c b/drivers/net/phy/mdio-mux.c
index 599ce24c514f..00755b6a42cf 100644
--- a/drivers/net/phy/mdio-mux.c
+++ b/drivers/net/phy/mdio-mux.c
@@ -133,29 +133,35 @@ int mdio_mux_init(struct device *dev,
 
 	ret_val = -ENODEV;
 	for_each_available_child_of_node(dev->of_node, child_bus_node) {
-		u32 v;
+		int v;
 
-		r = of_property_read_u32(child_bus_node, "reg", &v);
-		if (r)
+		v = of_mdio_parse_addr(dev, child_bus_node);
+		if (v < 0) {
+			dev_err(dev,
+				"Error: Failed to find reg for child %s\n",
+				of_node_full_name(child_bus_node));
 			continue;
+		}
 
 		cb = devm_kzalloc(dev, sizeof(*cb), GFP_KERNEL);
 		if (cb == NULL) {
 			dev_err(dev,
-				"Error: Failed to allocate memory for child\n");
+				"Error: Failed to allocate memory for child %s\n",
+				of_node_full_name(child_bus_node));
 			ret_val = -ENOMEM;
-			of_node_put(child_bus_node);
-			break;
+			continue;
 		}
 		cb->bus_number = v;
 		cb->parent = pb;
 
 		cb->mii_bus = mdiobus_alloc();
 		if (!cb->mii_bus) {
+			dev_err(dev,
+				"Error: Failed to allocate MDIO bus for child %s\n",
+				of_node_full_name(child_bus_node));
 			ret_val = -ENOMEM;
 			devm_kfree(dev, cb);
-			of_node_put(child_bus_node);
-			break;
+			continue;
 		}
 		cb->mii_bus->priv = cb;
 
@@ -167,6 +173,9 @@ int mdio_mux_init(struct device *dev,
 		cb->mii_bus->write = mdio_mux_write;
 		r = of_mdiobus_register(cb->mii_bus, child_bus_node);
 		if (r) {
+			dev_err(dev,
+				"Error: Failed to register MDIO bus for child %s\n",
+				of_node_full_name(child_bus_node));
 			mdiobus_free(cb->mii_bus);
 			devm_kfree(dev, cb);
 		} else {
@@ -180,6 +189,7 @@ int mdio_mux_init(struct device *dev,
 		return 0;
 	}
 
+	dev_err(dev, "Error: No acceptable child buses found\n");
 	devm_kfree(dev, pb);
 err_pb_kz:
 	/* balance the reference of_mdio_find_bus() took */
diff --git a/drivers/net/phy/mdio-xgene.c b/drivers/net/phy/mdio-xgene.c
index 3e2ac07b6e37..bfd3090fb055 100644
--- a/drivers/net/phy/mdio-xgene.c
+++ b/drivers/net/phy/mdio-xgene.c
@@ -34,76 +34,73 @@
 
 static bool xgene_mdio_status;
 
-static u32 xgene_enet_rd_mac(void __iomem *base_addr, u32 rd_addr)
+u32 xgene_mdio_rd_mac(struct xgene_mdio_pdata *pdata, u32 rd_addr)
 {
 	void __iomem *addr, *rd, *cmd, *cmd_done;
 	u32 done, rd_data = BUSY_MASK;
 	u8 wait = 10;
 
-	addr = base_addr + MAC_ADDR_REG_OFFSET;
-	rd = base_addr + MAC_READ_REG_OFFSET;
-	cmd = base_addr + MAC_COMMAND_REG_OFFSET;
-	cmd_done = base_addr + MAC_COMMAND_DONE_REG_OFFSET;
+	addr = pdata->mac_csr_addr + MAC_ADDR_REG_OFFSET;
+	rd = pdata->mac_csr_addr + MAC_READ_REG_OFFSET;
+	cmd = pdata->mac_csr_addr + MAC_COMMAND_REG_OFFSET;
+	cmd_done = pdata->mac_csr_addr + MAC_COMMAND_DONE_REG_OFFSET;
 
+	spin_lock(&pdata->mac_lock);
 	iowrite32(rd_addr, addr);
 	iowrite32(XGENE_ENET_RD_CMD, cmd);
 
-	while (wait--) {
-		done = ioread32(cmd_done);
-		if (done)
-			break;
+	while (!(done = ioread32(cmd_done)) && wait--)
 		udelay(1);
-	}
 
-	if (!done)
-		return rd_data;
+	if (done)
+		rd_data = ioread32(rd);
 
-	rd_data = ioread32(rd);
 	iowrite32(0, cmd);
+	spin_unlock(&pdata->mac_lock);
 
 	return rd_data;
 }
+EXPORT_SYMBOL(xgene_mdio_rd_mac);
 
-static void xgene_enet_wr_mac(void __iomem *base_addr, u32 wr_addr, u32 wr_data)
+void xgene_mdio_wr_mac(struct xgene_mdio_pdata *pdata, u32 wr_addr, u32 data)
 {
 	void __iomem *addr, *wr, *cmd, *cmd_done;
 	u8 wait = 10;
 	u32 done;
 
-	addr = base_addr + MAC_ADDR_REG_OFFSET;
-	wr = base_addr + MAC_WRITE_REG_OFFSET;
-	cmd = base_addr + MAC_COMMAND_REG_OFFSET;
-	cmd_done = base_addr + MAC_COMMAND_DONE_REG_OFFSET;
+	addr = pdata->mac_csr_addr + MAC_ADDR_REG_OFFSET;
+	wr = pdata->mac_csr_addr + MAC_WRITE_REG_OFFSET;
+	cmd = pdata->mac_csr_addr + MAC_COMMAND_REG_OFFSET;
+	cmd_done = pdata->mac_csr_addr + MAC_COMMAND_DONE_REG_OFFSET;
 
+	spin_lock(&pdata->mac_lock);
 	iowrite32(wr_addr, addr);
-	iowrite32(wr_data, wr);
+	iowrite32(data, wr);
 	iowrite32(XGENE_ENET_WR_CMD, cmd);
 
-	while (wait--) {
-		done = ioread32(cmd_done);
-		if (done)
-			break;
+	while (!(done = ioread32(cmd_done)) && wait--)
 		udelay(1);
-	}
 
 	if (!done)
 		pr_err("MCX mac write failed, addr: 0x%04x\n", wr_addr);
 
 	iowrite32(0, cmd);
+	spin_unlock(&pdata->mac_lock);
 }
+EXPORT_SYMBOL(xgene_mdio_wr_mac);
 
 int xgene_mdio_rgmii_read(struct mii_bus *bus, int phy_id, int reg)
 {
-	void __iomem *addr = (void __iomem *)bus->priv;
+	struct xgene_mdio_pdata *pdata = (struct xgene_mdio_pdata *)bus->priv;
 	u32 data, done;
 	u8 wait = 10;
 
 	data = SET_VAL(PHY_ADDR, phy_id) | SET_VAL(REG_ADDR, reg);
-	xgene_enet_wr_mac(addr, MII_MGMT_ADDRESS_ADDR, data);
-	xgene_enet_wr_mac(addr, MII_MGMT_COMMAND_ADDR, READ_CYCLE_MASK);
+	xgene_mdio_wr_mac(pdata, MII_MGMT_ADDRESS_ADDR, data);
+	xgene_mdio_wr_mac(pdata, MII_MGMT_COMMAND_ADDR, READ_CYCLE_MASK);
 	do {
 		usleep_range(5, 10);
-		done = xgene_enet_rd_mac(addr, MII_MGMT_INDICATORS_ADDR);
+		done = xgene_mdio_rd_mac(pdata, MII_MGMT_INDICATORS_ADDR);
 	} while ((done & BUSY_MASK) && wait--);
 
 	if (done & BUSY_MASK) {
@@ -111,8 +108,8 @@ int xgene_mdio_rgmii_read(struct mii_bus *bus, int phy_id, int reg)
 		return -EBUSY;
 	}
 
-	data = xgene_enet_rd_mac(addr, MII_MGMT_STATUS_ADDR);
-	xgene_enet_wr_mac(addr, MII_MGMT_COMMAND_ADDR, 0);
+	data = xgene_mdio_rd_mac(pdata, MII_MGMT_STATUS_ADDR);
+	xgene_mdio_wr_mac(pdata, MII_MGMT_COMMAND_ADDR, 0);
 
 	return data;
 }
@@ -120,17 +117,17 @@ EXPORT_SYMBOL(xgene_mdio_rgmii_read);
 
 int xgene_mdio_rgmii_write(struct mii_bus *bus, int phy_id, int reg, u16 data)
 {
-	void __iomem *addr = (void __iomem *)bus->priv;
+	struct xgene_mdio_pdata *pdata = (struct xgene_mdio_pdata *)bus->priv;
 	u32 val, done;
 	u8 wait = 10;
 
 	val = SET_VAL(PHY_ADDR, phy_id) | SET_VAL(REG_ADDR, reg);
-	xgene_enet_wr_mac(addr, MII_MGMT_ADDRESS_ADDR, val);
+	xgene_mdio_wr_mac(pdata, MII_MGMT_ADDRESS_ADDR, val);
 
-	xgene_enet_wr_mac(addr, MII_MGMT_CONTROL_ADDR, data);
+	xgene_mdio_wr_mac(pdata, MII_MGMT_CONTROL_ADDR, data);
 	do {
 		usleep_range(5, 10);
-		done = xgene_enet_rd_mac(addr, MII_MGMT_INDICATORS_ADDR);
+		done = xgene_mdio_rd_mac(pdata, MII_MGMT_INDICATORS_ADDR);
 	} while ((done & BUSY_MASK) && wait--);
 
 	if (done & BUSY_MASK) {
@@ -174,8 +171,8 @@ static int xgene_enet_ecc_init(struct xgene_mdio_pdata *pdata)
 
 static void xgene_gmac_reset(struct xgene_mdio_pdata *pdata)
 {
-	xgene_enet_wr_mac(pdata->mac_csr_addr, MAC_CONFIG_1_ADDR, SOFT_RESET);
-	xgene_enet_wr_mac(pdata->mac_csr_addr, MAC_CONFIG_1_ADDR, 0);
+	xgene_mdio_wr_mac(pdata, MAC_CONFIG_1_ADDR, SOFT_RESET);
+	xgene_mdio_wr_mac(pdata, MAC_CONFIG_1_ADDR, 0);
 }
 
 static int xgene_mdio_reset(struct xgene_mdio_pdata *pdata)
@@ -375,6 +372,9 @@ static int xgene_mdio_probe(struct platform_device *pdev)
 	pdata->mdio_csr_addr = csr_base + BLOCK_XG_MDIO_CSR_OFFSET;
 	pdata->diag_csr_addr = csr_base + BLOCK_DIAG_CSR_OFFSET;
 
+	if (mdio_id == XGENE_MDIO_RGMII)
+		spin_lock_init(&pdata->mac_lock);
+
 	if (dev->of_node) {
 		pdata->clk = devm_clk_get(dev, NULL);
 		if (IS_ERR(pdata->clk)) {
@@ -396,7 +396,7 @@ static int xgene_mdio_probe(struct platform_device *pdev)
 	if (mdio_id == XGENE_MDIO_RGMII) {
 		mdio_bus->read = xgene_mdio_rgmii_read;
 		mdio_bus->write = xgene_mdio_rgmii_write;
-		mdio_bus->priv = (void __force *)pdata->mac_csr_addr;
+		mdio_bus->priv = (void __force *)pdata;
 		snprintf(mdio_bus->id, MII_BUS_ID_SIZE, "%s",
 			 "xgene-mii-rgmii");
 	} else {
diff --git a/drivers/net/phy/mdio-xgene.h b/drivers/net/phy/mdio-xgene.h
index 594a11d42401..3c85f3e30baa 100644
--- a/drivers/net/phy/mdio-xgene.h
+++ b/drivers/net/phy/mdio-xgene.h
@@ -102,6 +102,7 @@ struct xgene_mdio_pdata {
 	void __iomem *mdio_csr_addr;
 	struct mii_bus *mdio_bus;
 	int mdio_id;
+	spinlock_t mac_lock; /* mac lock */
 };
 
 /* Set the specified value into a bit-field defined by its starting position
@@ -132,6 +133,8 @@ static inline u64 xgene_enet_get_field_value(int pos, int len, u64 src)
 #define GET_BIT(field, src) \
 		xgene_enet_get_field_value(field ## _POS, 1, src)
 
+u32 xgene_mdio_rd_mac(struct xgene_mdio_pdata *pdata, u32 rd_addr);
+void xgene_mdio_wr_mac(struct xgene_mdio_pdata *pdata, u32 wr_addr, u32 data);
 int xgene_mdio_rgmii_read(struct mii_bus *bus, int phy_id, int reg);
 int xgene_mdio_rgmii_write(struct mii_bus *bus, int phy_id, int reg, u16 data);
 struct phy_device *xgene_enet_phy_register(struct mii_bus *bus, int phy_addr);
diff --git a/drivers/net/phy/mdio_bus.c b/drivers/net/phy/mdio_bus.c
index f99c21f78b63..2df7b62c1a36 100644
--- a/drivers/net/phy/mdio_bus.c
+++ b/drivers/net/phy/mdio_bus.c
@@ -263,21 +263,10 @@ static void of_mdiobus_link_mdiodev(struct mii_bus *bus,
 
 	for_each_available_child_of_node(bus->dev.of_node, child) {
 		int addr;
-		int ret;
 
-		ret = of_property_read_u32(child, "reg", &addr);
-		if (ret < 0) {
-			dev_err(dev, "%s has invalid MDIO address\n",
-				child->full_name);
+		addr = of_mdio_parse_addr(dev, child);
+		if (addr < 0)
 			continue;
-		}
-
-		/* A MDIO device must have a reg property in the range [0-31] */
-		if (addr >= PHY_MAX_ADDR) {
-			dev_err(dev, "%s MDIO address %i is too large\n",
-				child->full_name, addr);
-			continue;
-		}
 
 		if (addr == mdiodev->addr) {
 			dev->of_node = child;
@@ -364,33 +353,18 @@ int __mdiobus_register(struct mii_bus *bus, struct module *owner)
 
 	mutex_init(&bus->mdio_lock);
 
-	/* de-assert bus level PHY GPIO resets */
-	if (bus->num_reset_gpios > 0) {
-		bus->reset_gpiod = devm_kcalloc(&bus->dev,
-						 bus->num_reset_gpios,
-						 sizeof(struct gpio_desc *),
-						 GFP_KERNEL);
-		if (!bus->reset_gpiod)
-			return -ENOMEM;
-	}
-
-	for (i = 0; i < bus->num_reset_gpios; i++) {
-		gpiod = devm_gpiod_get_index(&bus->dev, "reset", i,
-					     GPIOD_OUT_LOW);
-		if (IS_ERR(gpiod)) {
-			err = PTR_ERR(gpiod);
-			if (err != -ENOENT) {
-				dev_err(&bus->dev,
-					"mii_bus %s couldn't get reset GPIO\n",
-					bus->id);
-				return err;
-			}
-		} else {
-			bus->reset_gpiod[i] = gpiod;
-			gpiod_set_value_cansleep(gpiod, 1);
-			udelay(bus->reset_delay_us);
-			gpiod_set_value_cansleep(gpiod, 0);
-		}
+	/* de-assert bus level PHY GPIO reset */
+	gpiod = devm_gpiod_get_optional(&bus->dev, "reset", GPIOD_OUT_LOW);
+	if (IS_ERR(gpiod)) {
+		dev_err(&bus->dev, "mii_bus %s couldn't get reset GPIO\n",
+			bus->id);
+		return PTR_ERR(gpiod);
+	} else	if (gpiod) {
+		bus->reset_gpiod = gpiod;
+
+		gpiod_set_value_cansleep(gpiod, 1);
+		udelay(bus->reset_delay_us);
+		gpiod_set_value_cansleep(gpiod, 0);
 	}
 
 	if (bus->reset)
@@ -425,10 +399,8 @@ error:
 	}
 
 	/* Put PHYs in RESET to save power */
-	for (i = 0; i < bus->num_reset_gpios; i++) {
-		if (bus->reset_gpiod[i])
-			gpiod_set_value_cansleep(bus->reset_gpiod[i], 1);
-	}
+	if (bus->reset_gpiod)
+		gpiod_set_value_cansleep(bus->reset_gpiod, 1);
 
 	device_del(&bus->dev);
 	return err;
@@ -453,10 +425,8 @@ void mdiobus_unregister(struct mii_bus *bus)
 	}
 
 	/* Put PHYs in RESET to save power */
-	for (i = 0; i < bus->num_reset_gpios; i++) {
-		if (bus->reset_gpiod[i])
-			gpiod_set_value_cansleep(bus->reset_gpiod[i], 1);
-	}
+	if (bus->reset_gpiod)
+		gpiod_set_value_cansleep(bus->reset_gpiod, 1);
 
 	device_del(&bus->dev);
 }
diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index b9252b8d81ff..fdb43dd9b5cd 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -20,6 +20,7 @@
  *			   ksz8081, ksz8091,
  *			   ksz8061,
  *		Switch : ksz8873, ksz886x
+ *			 ksz9477
  */
 
 #include <linux/kernel.h>
@@ -619,6 +620,8 @@ static int ksz9031_read_status(struct phy_device *phydev)
 	if ((regval & 0xFF) == 0xFF) {
 		phy_init_hw(phydev);
 		phydev->link = 0;
+		if (phydev->drv->config_intr && phy_interrupt_is_valid(phydev))
+			phydev->drv->config_intr(phydev);
 	}
 
 	return 0;
@@ -793,7 +796,7 @@ static struct phy_driver ksphy_driver[] = {
 	.phy_id_mask	= MICREL_PHY_ID_MASK,
 	.name		= "Micrel KS8737",
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.driver_data	= &ks8737_type,
 	.config_init	= kszphy_config_init,
 	.config_aneg	= genphy_config_aneg,
@@ -807,7 +810,7 @@ static struct phy_driver ksphy_driver[] = {
 	.phy_id_mask	= 0x00ffffff,
 	.name		= "Micrel KSZ8021 or KSZ8031",
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.driver_data	= &ksz8021_type,
 	.probe		= kszphy_probe,
 	.config_init	= kszphy_config_init,
@@ -825,7 +828,7 @@ static struct phy_driver ksphy_driver[] = {
 	.phy_id_mask	= 0x00ffffff,
 	.name		= "Micrel KSZ8031",
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.driver_data	= &ksz8021_type,
 	.probe		= kszphy_probe,
 	.config_init	= kszphy_config_init,
@@ -843,7 +846,7 @@ static struct phy_driver ksphy_driver[] = {
 	.phy_id_mask	= MICREL_PHY_ID_MASK,
 	.name		= "Micrel KSZ8041",
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.driver_data	= &ksz8041_type,
 	.probe		= kszphy_probe,
 	.config_init	= ksz8041_config_init,
@@ -861,7 +864,7 @@ static struct phy_driver ksphy_driver[] = {
 	.phy_id_mask	= MICREL_PHY_ID_MASK,
 	.name		= "Micrel KSZ8041RNLI",
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.driver_data	= &ksz8041_type,
 	.probe		= kszphy_probe,
 	.config_init	= kszphy_config_init,
@@ -879,7 +882,7 @@ static struct phy_driver ksphy_driver[] = {
 	.phy_id_mask	= MICREL_PHY_ID_MASK,
 	.name		= "Micrel KSZ8051",
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.driver_data	= &ksz8051_type,
 	.probe		= kszphy_probe,
 	.config_init	= kszphy_config_init,
@@ -897,7 +900,7 @@ static struct phy_driver ksphy_driver[] = {
 	.name		= "Micrel KSZ8001 or KS8721",
 	.phy_id_mask	= 0x00fffffc,
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.driver_data	= &ksz8041_type,
 	.probe		= kszphy_probe,
 	.config_init	= kszphy_config_init,
@@ -915,7 +918,7 @@ static struct phy_driver ksphy_driver[] = {
 	.name		= "Micrel KSZ8081 or KSZ8091",
 	.phy_id_mask	= MICREL_PHY_ID_MASK,
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.driver_data	= &ksz8081_type,
 	.probe		= kszphy_probe,
 	.config_init	= kszphy_config_init,
@@ -933,7 +936,7 @@ static struct phy_driver ksphy_driver[] = {
 	.name		= "Micrel KSZ8061",
 	.phy_id_mask	= MICREL_PHY_ID_MASK,
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= kszphy_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -946,7 +949,7 @@ static struct phy_driver ksphy_driver[] = {
 	.phy_id_mask	= 0x000ffffe,
 	.name		= "Micrel KSZ9021 Gigabit PHY",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.driver_data	= &ksz9021_type,
 	.probe		= kszphy_probe,
 	.config_init	= ksz9021_config_init,
@@ -966,7 +969,7 @@ static struct phy_driver ksphy_driver[] = {
 	.phy_id_mask	= MICREL_PHY_ID_MASK,
 	.name		= "Micrel KSZ9031 Gigabit PHY",
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.driver_data	= &ksz9021_type,
 	.probe		= kszphy_probe,
 	.config_init	= ksz9031_config_init,
@@ -983,7 +986,6 @@ static struct phy_driver ksphy_driver[] = {
 	.phy_id		= PHY_ID_KSZ8873MLL,
 	.phy_id_mask	= MICREL_PHY_ID_MASK,
 	.name		= "Micrel KSZ8873MLL Switch",
-	.flags		= PHY_HAS_MAGICANEG,
 	.config_init	= kszphy_config_init,
 	.config_aneg	= ksz8873mll_config_aneg,
 	.read_status	= ksz8873mll_read_status,
@@ -994,7 +996,7 @@ static struct phy_driver ksphy_driver[] = {
 	.phy_id_mask	= MICREL_PHY_ID_MASK,
 	.name		= "Micrel KSZ886X Switch",
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= kszphy_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -1005,12 +1007,22 @@ static struct phy_driver ksphy_driver[] = {
 	.phy_id_mask	= MICREL_PHY_ID_MASK,
 	.name		= "Micrel KSZ8795",
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.flags		= PHY_HAS_INTERRUPT,
 	.config_init	= kszphy_config_init,
 	.config_aneg	= ksz8873mll_config_aneg,
 	.read_status	= ksz8873mll_read_status,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
+}, {
+	.phy_id		= PHY_ID_KSZ9477,
+	.phy_id_mask	= MICREL_PHY_ID_MASK,
+	.name		= "Microchip KSZ9477",
+	.features	= PHY_GBIT_FEATURES,
+	.config_init	= kszphy_config_init,
+	.config_aneg	= genphy_config_aneg,
+	.read_status	= genphy_read_status,
+	.suspend	= genphy_suspend,
+	.resume		= genphy_resume,
 } };
 
 module_phy_driver(ksphy_driver);
diff --git a/drivers/net/phy/microchip.c b/drivers/net/phy/microchip.c
index 2b2f543cf9f0..37ee856c7680 100644
--- a/drivers/net/phy/microchip.c
+++ b/drivers/net/phy/microchip.c
@@ -146,7 +146,7 @@ static struct phy_driver microchip_phy_driver[] = {
 	.name		= "Microchip LAN88xx",
 
 	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_INTERRUPT | PHY_HAS_MAGICANEG,
+	.flags		= PHY_HAS_INTERRUPT,
 
 	.probe		= lan88xx_probe,
 	.remove		= lan88xx_remove,
diff --git a/drivers/net/phy/phy-c45.c b/drivers/net/phy/phy-c45.c
new file mode 100644
index 000000000000..dada819c6b78
--- /dev/null
+++ b/drivers/net/phy/phy-c45.c
@@ -0,0 +1,298 @@
+/*
+ * Clause 45 PHY support
+ */
+#include <linux/ethtool.h>
+#include <linux/export.h>
+#include <linux/mdio.h>
+#include <linux/mii.h>
+#include <linux/phy.h>
+
+/**
+ * genphy_c45_setup_forced - configures a forced speed
+ * @phydev: target phy_device struct
+ */
+int genphy_c45_pma_setup_forced(struct phy_device *phydev)
+{
+	int ctrl1, ctrl2, ret;
+
+	/* Half duplex is not supported */
+	if (phydev->duplex != DUPLEX_FULL)
+		return -EINVAL;
+
+	ctrl1 = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_CTRL1);
+	if (ctrl1 < 0)
+		return ctrl1;
+
+	ctrl2 = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_CTRL2);
+	if (ctrl2 < 0)
+		return ctrl2;
+
+	ctrl1 &= ~MDIO_CTRL1_SPEEDSEL;
+	/*
+	 * PMA/PMD type selection is 1.7.5:0 not 1.7.3:0.  See 45.2.1.6.1
+	 * in 802.3-2012 and 802.3-2015.
+	 */
+	ctrl2 &= ~(MDIO_PMA_CTRL2_TYPE | 0x30);
+
+	switch (phydev->speed) {
+	case SPEED_10:
+		ctrl2 |= MDIO_PMA_CTRL2_10BT;
+		break;
+	case SPEED_100:
+		ctrl1 |= MDIO_PMA_CTRL1_SPEED100;
+		ctrl2 |= MDIO_PMA_CTRL2_100BTX;
+		break;
+	case SPEED_1000:
+		ctrl1 |= MDIO_PMA_CTRL1_SPEED1000;
+		/* Assume 1000base-T */
+		ctrl2 |= MDIO_PMA_CTRL2_1000BT;
+		break;
+	case SPEED_10000:
+		ctrl1 |= MDIO_CTRL1_SPEED10G;
+		/* Assume 10Gbase-T */
+		ctrl2 |= MDIO_PMA_CTRL2_10GBT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = phy_write_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_CTRL1, ctrl1);
+	if (ret < 0)
+		return ret;
+
+	return phy_write_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_CTRL2, ctrl2);
+}
+EXPORT_SYMBOL_GPL(genphy_c45_pma_setup_forced);
+
+/**
+ * genphy_c45_an_disable_aneg - disable auto-negotiation
+ * @phydev: target phy_device struct
+ *
+ * Disable auto-negotiation in the Clause 45 PHY. The link parameters
+ * parameters are controlled through the PMA/PMD MMD registers.
+ *
+ * Returns zero on success, negative errno code on failure.
+ */
+int genphy_c45_an_disable_aneg(struct phy_device *phydev)
+{
+	int val;
+
+	val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1);
+	if (val < 0)
+		return val;
+
+	val &= ~(MDIO_AN_CTRL1_ENABLE | MDIO_AN_CTRL1_RESTART);
+
+	return phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1, val);
+}
+EXPORT_SYMBOL_GPL(genphy_c45_an_disable_aneg);
+
+/**
+ * genphy_c45_restart_aneg - Enable and restart auto-negotiation
+ * @phydev: target phy_device struct
+ *
+ * This assumes that the auto-negotiation MMD is present.
+ *
+ * Enable and restart auto-negotiation.
+ */
+int genphy_c45_restart_aneg(struct phy_device *phydev)
+{
+	int val;
+
+	val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1);
+	if (val < 0)
+		return val;
+
+	val |= MDIO_AN_CTRL1_ENABLE | MDIO_AN_CTRL1_RESTART;
+
+	return phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1, val);
+}
+EXPORT_SYMBOL_GPL(genphy_c45_restart_aneg);
+
+/**
+ * genphy_c45_aneg_done - return auto-negotiation complete status
+ * @phydev: target phy_device struct
+ *
+ * This assumes that the auto-negotiation MMD is present.
+ *
+ * Reads the status register from the auto-negotiation MMD, returning:
+ * - positive if auto-negotiation is complete
+ * - negative errno code on error
+ * - zero otherwise
+ */
+int genphy_c45_aneg_done(struct phy_device *phydev)
+{
+	int val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_STAT1);
+
+	return val < 0 ? val : val & MDIO_AN_STAT1_COMPLETE ? 1 : 0;
+}
+EXPORT_SYMBOL_GPL(genphy_c45_aneg_done);
+
+/**
+ * genphy_c45_read_link - read the overall link status from the MMDs
+ * @phydev: target phy_device struct
+ * @mmd_mask: MMDs to read status from
+ *
+ * Read the link status from the specified MMDs, and if they all indicate
+ * that the link is up, return positive.  If an error is encountered,
+ * a negative errno will be returned, otherwise zero.
+ */
+int genphy_c45_read_link(struct phy_device *phydev, u32 mmd_mask)
+{
+	int val, devad;
+	bool link = true;
+
+	while (mmd_mask) {
+		devad = __ffs(mmd_mask);
+		mmd_mask &= ~BIT(devad);
+
+		/* The link state is latched low so that momentary link
+		 * drops can be detected.  Do not double-read the status
+		 * register if the link is down.
+		 */
+		val = phy_read_mmd(phydev, devad, MDIO_STAT1);
+		if (val < 0)
+			return val;
+
+		if (!(val & MDIO_STAT1_LSTATUS))
+			link = false;
+	}
+
+	return link;
+}
+EXPORT_SYMBOL_GPL(genphy_c45_read_link);
+
+/**
+ * genphy_c45_read_lpa - read the link partner advertisment and pause
+ * @phydev: target phy_device struct
+ *
+ * Read the Clause 45 defined base (7.19) and 10G (7.33) status registers,
+ * filling in the link partner advertisment, pause and asym_pause members
+ * in @phydev.  This assumes that the auto-negotiation MMD is present, and
+ * the backplane bit (7.48.0) is clear.  Clause 45 PHY drivers are expected
+ * to fill in the remainder of the link partner advert from vendor registers.
+ */
+int genphy_c45_read_lpa(struct phy_device *phydev)
+{
+	int val;
+
+	/* Read the link partner's base page advertisment */
+	val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_LPA);
+	if (val < 0)
+		return val;
+
+	phydev->lp_advertising = mii_lpa_to_ethtool_lpa_t(val);
+	phydev->pause = val & LPA_PAUSE_CAP ? 1 : 0;
+	phydev->asym_pause = val & LPA_PAUSE_ASYM ? 1 : 0;
+
+	/* Read the link partner's 10G advertisment */
+	val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_10GBT_STAT);
+	if (val < 0)
+		return val;
+
+	if (val & MDIO_AN_10GBT_STAT_LP10G)
+		phydev->lp_advertising |= ADVERTISED_10000baseT_Full;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(genphy_c45_read_lpa);
+
+/**
+ * genphy_c45_read_pma - read link speed etc from PMA
+ * @phydev: target phy_device struct
+ */
+int genphy_c45_read_pma(struct phy_device *phydev)
+{
+	int val;
+
+	val = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_CTRL1);
+	if (val < 0)
+		return val;
+
+	switch (val & MDIO_CTRL1_SPEEDSEL) {
+	case 0:
+		phydev->speed = SPEED_10;
+		break;
+	case MDIO_PMA_CTRL1_SPEED100:
+		phydev->speed = SPEED_100;
+		break;
+	case MDIO_PMA_CTRL1_SPEED1000:
+		phydev->speed = SPEED_1000;
+		break;
+	case MDIO_CTRL1_SPEED10G:
+		phydev->speed = SPEED_10000;
+		break;
+	default:
+		phydev->speed = SPEED_UNKNOWN;
+		break;
+	}
+
+	phydev->duplex = DUPLEX_FULL;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(genphy_c45_read_pma);
+
+/* The gen10g_* functions are the old Clause 45 stub */
+
+static int gen10g_config_aneg(struct phy_device *phydev)
+{
+	return 0;
+}
+
+static int gen10g_read_status(struct phy_device *phydev)
+{
+	u32 mmd_mask = phydev->c45_ids.devices_in_package;
+	int ret;
+
+	/* For now just lie and say it's 10G all the time */
+	phydev->speed = SPEED_10000;
+	phydev->duplex = DUPLEX_FULL;
+
+	/* Avoid reading the vendor MMDs */
+	mmd_mask &= ~(BIT(MDIO_MMD_VEND1) | BIT(MDIO_MMD_VEND2));
+
+	ret = genphy_c45_read_link(phydev, mmd_mask);
+
+	phydev->link = ret > 0 ? 1 : 0;
+
+	return 0;
+}
+
+static int gen10g_soft_reset(struct phy_device *phydev)
+{
+	/* Do nothing for now */
+	return 0;
+}
+
+static int gen10g_config_init(struct phy_device *phydev)
+{
+	/* Temporarily just say we support everything */
+	phydev->supported = SUPPORTED_10000baseT_Full;
+	phydev->advertising = SUPPORTED_10000baseT_Full;
+
+	return 0;
+}
+
+static int gen10g_suspend(struct phy_device *phydev)
+{
+	return 0;
+}
+
+static int gen10g_resume(struct phy_device *phydev)
+{
+	return 0;
+}
+
+struct phy_driver genphy_10g_driver = {
+	.phy_id         = 0xffffffff,
+	.phy_id_mask    = 0xffffffff,
+	.name           = "Generic 10G PHY",
+	.soft_reset	= gen10g_soft_reset,
+	.config_init    = gen10g_config_init,
+	.features       = 0,
+	.config_aneg    = gen10g_config_aneg,
+	.read_status    = gen10g_read_status,
+	.suspend        = gen10g_suspend,
+	.resume         = gen10g_resume,
+};
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index 7524caa0f29d..d0626bf5c540 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -54,6 +54,8 @@ static const char *phy_speed_to_str(int speed)
 		return "5Gbps";
 	case SPEED_10000:
 		return "10Gbps";
+	case SPEED_14000:
+		return "14Gbps";
 	case SPEED_20000:
 		return "20Gbps";
 	case SPEED_25000:
@@ -149,6 +151,25 @@ static int phy_config_interrupt(struct phy_device *phydev, u32 interrupts)
 	return 0;
 }
 
+/**
+ * phy_restart_aneg - restart auto-negotiation
+ * @phydev: target phy_device struct
+ *
+ * Restart the autonegotiation on @phydev.  Returns >= 0 on success or
+ * negative errno on error.
+ */
+int phy_restart_aneg(struct phy_device *phydev)
+{
+	int ret;
+
+	if (phydev->is_c45 && !(phydev->c45_ids.devices_in_package & BIT(0)))
+		ret = genphy_c45_restart_aneg(phydev);
+	else
+		ret = genphy_restart_aneg(phydev);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(phy_restart_aneg);
 
 /**
  * phy_aneg_done - return auto-negotiation status
@@ -163,6 +184,12 @@ int phy_aneg_done(struct phy_device *phydev)
 	if (phydev->drv && phydev->drv->aneg_done)
 		return phydev->drv->aneg_done(phydev);
 
+	/* Avoid genphy_aneg_done() if the Clause 45 PHY does not
+	 * implement Clause 22 registers
+	 */
+	if (phydev->is_c45 && !(phydev->c45_ids.devices_in_package & BIT(0)))
+		return -EINVAL;
+
 	return genphy_aneg_done(phydev);
 }
 EXPORT_SYMBOL(phy_aneg_done);
@@ -377,6 +404,7 @@ static void phy_sanitize_settings(struct phy_device *phydev)
  * @cmd: ethtool_cmd
  *
  * A few notes about parameter checking:
+ *
  * - We don't set port or transceiver, so we don't care what they
  *   were set to.
  * - phy_start_aneg() will make sure forced settings are sane, and
@@ -484,32 +512,8 @@ int phy_ethtool_ksettings_set(struct phy_device *phydev,
 }
 EXPORT_SYMBOL(phy_ethtool_ksettings_set);
 
-int phy_ethtool_gset(struct phy_device *phydev, struct ethtool_cmd *cmd)
-{
-	cmd->supported = phydev->supported;
-
-	cmd->advertising = phydev->advertising;
-	cmd->lp_advertising = phydev->lp_advertising;
-
-	ethtool_cmd_speed_set(cmd, phydev->speed);
-	cmd->duplex = phydev->duplex;
-	if (phydev->interface == PHY_INTERFACE_MODE_MOCA)
-		cmd->port = PORT_BNC;
-	else
-		cmd->port = PORT_MII;
-	cmd->phy_address = phydev->mdio.addr;
-	cmd->transceiver = phy_is_internal(phydev) ?
-		XCVR_INTERNAL : XCVR_EXTERNAL;
-	cmd->autoneg = phydev->autoneg;
-	cmd->eth_tp_mdix_ctrl = phydev->mdix_ctrl;
-	cmd->eth_tp_mdix = phydev->mdix;
-
-	return 0;
-}
-EXPORT_SYMBOL(phy_ethtool_gset);
-
-int phy_ethtool_ksettings_get(struct phy_device *phydev,
-			      struct ethtool_link_ksettings *cmd)
+void phy_ethtool_ksettings_get(struct phy_device *phydev,
+			       struct ethtool_link_ksettings *cmd)
 {
 	ethtool_convert_legacy_u32_to_link_mode(cmd->link_modes.supported,
 						phydev->supported);
@@ -531,8 +535,6 @@ int phy_ethtool_ksettings_get(struct phy_device *phydev,
 	cmd->base.autoneg = phydev->autoneg;
 	cmd->base.eth_tp_mdix_ctrl = phydev->mdix_ctrl;
 	cmd->base.eth_tp_mdix = phydev->mdix;
-
-	return 0;
 }
 EXPORT_SYMBOL(phy_ethtool_ksettings_get);
 
@@ -1415,7 +1417,7 @@ int phy_ethtool_set_eee(struct phy_device *phydev, struct ethtool_eee *data)
 		/* Restart autonegotiation so the new modes get sent to the
 		 * link partner.
 		 */
-		ret = genphy_restart_aneg(phydev);
+		ret = phy_restart_aneg(phydev);
 		if (ret < 0)
 			return ret;
 	}
@@ -1448,7 +1450,9 @@ int phy_ethtool_get_link_ksettings(struct net_device *ndev,
 	if (!phydev)
 		return -ENODEV;
 
-	return phy_ethtool_ksettings_get(phydev, cmd);
+	phy_ethtool_ksettings_get(phydev, cmd);
+
+	return 0;
 }
 EXPORT_SYMBOL(phy_ethtool_get_link_ksettings);
 
@@ -1474,6 +1478,6 @@ int phy_ethtool_nway_reset(struct net_device *ndev)
 	if (!phydev->drv)
 		return -EIO;
 
-	return genphy_restart_aneg(phydev);
+	return phy_restart_aneg(phydev);
 }
 EXPORT_SYMBOL(phy_ethtool_nway_reset);
diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index 1219eeab69d1..1790f7fec125 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -69,13 +69,8 @@ static void phy_mdio_device_remove(struct mdio_device *mdiodev)
 	phy_device_remove(phydev);
 }
 
-enum genphy_driver {
-	GENPHY_DRV_1G,
-	GENPHY_DRV_10G,
-	GENPHY_DRV_MAX
-};
-
-static struct phy_driver genphy_driver[GENPHY_DRV_MAX];
+static struct phy_driver genphy_driver;
+extern struct phy_driver genphy_10g_driver;
 
 static LIST_HEAD(phy_fixup_list);
 static DEFINE_MUTEX(phy_fixup_lock);
@@ -928,11 +923,9 @@ int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
 	 */
 	if (!d->driver) {
 		if (phydev->is_c45)
-			d->driver =
-				&genphy_driver[GENPHY_DRV_10G].mdiodrv.driver;
+			d->driver = &genphy_10g_driver.mdiodrv.driver;
 		else
-			d->driver =
-				&genphy_driver[GENPHY_DRV_1G].mdiodrv.driver;
+			d->driver = &genphy_driver.mdiodrv.driver;
 
 		using_genphy = true;
 	}
@@ -961,6 +954,27 @@ int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
 	phydev->attached_dev = dev;
 	dev->phydev = phydev;
 
+	/* Some Ethernet drivers try to connect to a PHY device before
+	 * calling register_netdevice() -> netdev_register_kobject() and
+	 * does the dev->dev.kobj initialization. Here we only check for
+	 * success which indicates that the network device kobject is
+	 * ready. Once we do that we still need to keep track of whether
+	 * links were successfully set up or not for phy_detach() to
+	 * remove them accordingly.
+	 */
+	phydev->sysfs_links = false;
+
+	err = sysfs_create_link(&phydev->mdio.dev.kobj, &dev->dev.kobj,
+				"attached_dev");
+	if (!err) {
+		err = sysfs_create_link(&dev->dev.kobj, &phydev->mdio.dev.kobj,
+					"phydev");
+		if (err)
+			goto error;
+
+		phydev->sysfs_links = true;
+	}
+
 	phydev->dev_flags = flags;
 
 	phydev->interface = interface;
@@ -1048,8 +1062,11 @@ void phy_detach(struct phy_device *phydev)
 	struct net_device *dev = phydev->attached_dev;
 	struct module *ndev_owner = dev->dev.parent->driver->owner;
 	struct mii_bus *bus;
-	int i;
 
+	if (phydev->sysfs_links) {
+		sysfs_remove_link(&dev->dev.kobj, "phydev");
+		sysfs_remove_link(&phydev->mdio.dev.kobj, "attached_dev");
+	}
 	phydev->attached_dev->phydev = NULL;
 	phydev->attached_dev = NULL;
 	phy_suspend(phydev);
@@ -1063,13 +1080,9 @@ void phy_detach(struct phy_device *phydev)
 	 * from the generic driver so that there's a chance a
 	 * real driver could be loaded
 	 */
-	for (i = 0; i < ARRAY_SIZE(genphy_driver); i++) {
-		if (phydev->mdio.dev.driver ==
-		    &genphy_driver[i].mdiodrv.driver) {
-			device_release_driver(&phydev->mdio.dev);
-			break;
-		}
-	}
+	if (phydev->mdio.dev.driver == &genphy_10g_driver.mdiodrv.driver ||
+	    phydev->mdio.dev.driver == &genphy_driver.mdiodrv.driver)
+		device_release_driver(&phydev->mdio.dev);
 
 	/*
 	 * The phydev might go away on the put_device() below, so avoid
@@ -1123,6 +1136,39 @@ int phy_resume(struct phy_device *phydev)
 }
 EXPORT_SYMBOL(phy_resume);
 
+int phy_loopback(struct phy_device *phydev, bool enable)
+{
+	struct phy_driver *phydrv = to_phy_driver(phydev->mdio.dev.driver);
+	int ret = 0;
+
+	mutex_lock(&phydev->lock);
+
+	if (enable && phydev->loopback_enabled) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	if (!enable && !phydev->loopback_enabled) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (phydev->drv && phydrv->set_loopback)
+		ret = phydrv->set_loopback(phydev, enable);
+	else
+		ret = -EOPNOTSUPP;
+
+	if (ret)
+		goto out;
+
+	phydev->loopback_enabled = enable;
+
+out:
+	mutex_unlock(&phydev->lock);
+	return ret;
+}
+EXPORT_SYMBOL(phy_loopback);
+
 /* Generic PHY support and helper functions */
 
 /**
@@ -1343,11 +1389,6 @@ int genphy_aneg_done(struct phy_device *phydev)
 }
 EXPORT_SYMBOL(genphy_aneg_done);
 
-static int gen10g_config_aneg(struct phy_device *phydev)
-{
-	return 0;
-}
-
 /**
  * genphy_update_link - update link status in @phydev
  * @phydev: target phy_device struct
@@ -1481,33 +1522,6 @@ int genphy_read_status(struct phy_device *phydev)
 }
 EXPORT_SYMBOL(genphy_read_status);
 
-static int gen10g_read_status(struct phy_device *phydev)
-{
-	int devad, reg;
-	u32 mmd_mask = phydev->c45_ids.devices_in_package;
-
-	phydev->link = 1;
-
-	/* For now just lie and say it's 10G all the time */
-	phydev->speed = SPEED_10000;
-	phydev->duplex = DUPLEX_FULL;
-
-	for (devad = 0; mmd_mask; devad++, mmd_mask = mmd_mask >> 1) {
-		if (!(mmd_mask & 1))
-			continue;
-
-		/* Read twice because link state is latched and a
-		 * read moves the current state into the register
-		 */
-		phy_read_mmd(phydev, devad, MDIO_STAT1);
-		reg = phy_read_mmd(phydev, devad, MDIO_STAT1);
-		if (reg < 0 || !(reg & MDIO_STAT1_LSTATUS))
-			phydev->link = 0;
-	}
-
-	return 0;
-}
-
 /**
  * genphy_soft_reset - software reset the PHY via BMCR_RESET bit
  * @phydev: target phy_device struct
@@ -1571,23 +1585,8 @@ int genphy_config_init(struct phy_device *phydev)
 
 	return 0;
 }
-
-static int gen10g_soft_reset(struct phy_device *phydev)
-{
-	/* Do nothing for now */
-	return 0;
-}
 EXPORT_SYMBOL(genphy_config_init);
 
-static int gen10g_config_init(struct phy_device *phydev)
-{
-	/* Temporarily just say we support everything */
-	phydev->supported = SUPPORTED_10000baseT_Full;
-	phydev->advertising = SUPPORTED_10000baseT_Full;
-
-	return 0;
-}
-
 int genphy_suspend(struct phy_device *phydev)
 {
 	int value;
@@ -1603,11 +1602,6 @@ int genphy_suspend(struct phy_device *phydev)
 }
 EXPORT_SYMBOL(genphy_suspend);
 
-static int gen10g_suspend(struct phy_device *phydev)
-{
-	return 0;
-}
-
 int genphy_resume(struct phy_device *phydev)
 {
 	int value;
@@ -1623,10 +1617,22 @@ int genphy_resume(struct phy_device *phydev)
 }
 EXPORT_SYMBOL(genphy_resume);
 
-static int gen10g_resume(struct phy_device *phydev)
+int genphy_loopback(struct phy_device *phydev, bool enable)
 {
-	return 0;
+	int value;
+
+	value = phy_read(phydev, MII_BMCR);
+	if (value < 0)
+		return value;
+
+	if (enable)
+		value |= BMCR_LOOPBACK;
+	else
+		value &= ~BMCR_LOOPBACK;
+
+	return phy_write(phydev, MII_BMCR, value);
 }
+EXPORT_SYMBOL(genphy_loopback);
 
 static int __set_phy_supported(struct phy_device *phydev, u32 max_speed)
 {
@@ -1859,8 +1865,7 @@ void phy_drivers_unregister(struct phy_driver *drv, int n)
 }
 EXPORT_SYMBOL(phy_drivers_unregister);
 
-static struct phy_driver genphy_driver[] = {
-{
+static struct phy_driver genphy_driver = {
 	.phy_id		= 0xffffffff,
 	.phy_id_mask	= 0xffffffff,
 	.name		= "Generic PHY",
@@ -1874,18 +1879,8 @@ static struct phy_driver genphy_driver[] = {
 	.read_status	= genphy_read_status,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-}, {
-	.phy_id         = 0xffffffff,
-	.phy_id_mask    = 0xffffffff,
-	.name           = "Generic 10G PHY",
-	.soft_reset	= gen10g_soft_reset,
-	.config_init    = gen10g_config_init,
-	.features       = 0,
-	.config_aneg    = gen10g_config_aneg,
-	.read_status    = gen10g_read_status,
-	.suspend        = gen10g_suspend,
-	.resume         = gen10g_resume,
-} };
+	.set_loopback   = genphy_loopback,
+};
 
 static int __init phy_init(void)
 {
@@ -1895,18 +1890,24 @@ static int __init phy_init(void)
 	if (rc)
 		return rc;
 
-	rc = phy_drivers_register(genphy_driver,
-				  ARRAY_SIZE(genphy_driver), THIS_MODULE);
+	rc = phy_driver_register(&genphy_10g_driver, THIS_MODULE);
 	if (rc)
+		goto err_10g;
+
+	rc = phy_driver_register(&genphy_driver, THIS_MODULE);
+	if (rc) {
+		phy_driver_unregister(&genphy_10g_driver);
+err_10g:
 		mdio_bus_exit();
+	}
 
 	return rc;
 }
 
 static void __exit phy_exit(void)
 {
-	phy_drivers_unregister(genphy_driver,
-			       ARRAY_SIZE(genphy_driver));
+	phy_driver_unregister(&genphy_10g_driver);
+	phy_driver_unregister(&genphy_driver);
 	mdio_bus_exit();
 }
 
diff --git a/drivers/net/phy/smsc.c b/drivers/net/phy/smsc.c
index cef6967b0396..2306bfae057f 100644
--- a/drivers/net/phy/smsc.c
+++ b/drivers/net/phy/smsc.c
@@ -25,6 +25,16 @@
 #include <linux/netdevice.h>
 #include <linux/smscphy.h>
 
+struct smsc_hw_stat {
+	const char *string;
+	u8 reg;
+	u8 bits;
+};
+
+static struct smsc_hw_stat smsc_hw_stats[] = {
+	{ "phy_symbol_errors", 26, 16},
+};
+
 struct smsc_phy_priv {
 	bool energy_enable;
 };
@@ -143,6 +153,48 @@ static int lan87xx_read_status(struct phy_device *phydev)
 	return err;
 }
 
+static int smsc_get_sset_count(struct phy_device *phydev)
+{
+	return ARRAY_SIZE(smsc_hw_stats);
+}
+
+static void smsc_get_strings(struct phy_device *phydev, u8 *data)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(smsc_hw_stats); i++) {
+		strncpy(data + i * ETH_GSTRING_LEN,
+		       smsc_hw_stats[i].string, ETH_GSTRING_LEN);
+	}
+}
+
+#ifndef UINT64_MAX
+#define UINT64_MAX              (u64)(~((u64)0))
+#endif
+static u64 smsc_get_stat(struct phy_device *phydev, int i)
+{
+	struct smsc_hw_stat stat = smsc_hw_stats[i];
+	int val;
+	u64 ret;
+
+	val = phy_read(phydev, stat.reg);
+	if (val < 0)
+		ret = UINT64_MAX;
+	else
+		ret = val;
+
+	return ret;
+}
+
+static void smsc_get_stats(struct phy_device *phydev,
+			   struct ethtool_stats *stats, u64 *data)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(smsc_hw_stats); i++)
+		data[i] = smsc_get_stat(phydev, i);
+}
+
 static int smsc_phy_probe(struct phy_device *phydev)
 {
 	struct device *dev = &phydev->mdio.dev;
@@ -170,7 +222,7 @@ static struct phy_driver smsc_phy_driver[] = {
 	.name		= "SMSC LAN83C185",
 
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_INTERRUPT | PHY_HAS_MAGICANEG,
+	.flags		= PHY_HAS_INTERRUPT,
 
 	.probe		= smsc_phy_probe,
 
@@ -192,7 +244,7 @@ static struct phy_driver smsc_phy_driver[] = {
 	.name		= "SMSC LAN8187",
 
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_INTERRUPT | PHY_HAS_MAGICANEG,
+	.flags		= PHY_HAS_INTERRUPT,
 
 	.probe		= smsc_phy_probe,
 
@@ -206,6 +258,11 @@ static struct phy_driver smsc_phy_driver[] = {
 	.ack_interrupt	= smsc_phy_ack_interrupt,
 	.config_intr	= smsc_phy_config_intr,
 
+	/* Statistics */
+	.get_sset_count = smsc_get_sset_count,
+	.get_strings	= smsc_get_strings,
+	.get_stats	= smsc_get_stats,
+
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
 }, {
@@ -214,7 +271,7 @@ static struct phy_driver smsc_phy_driver[] = {
 	.name		= "SMSC LAN8700",
 
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_INTERRUPT | PHY_HAS_MAGICANEG,
+	.flags		= PHY_HAS_INTERRUPT,
 
 	.probe		= smsc_phy_probe,
 
@@ -228,6 +285,11 @@ static struct phy_driver smsc_phy_driver[] = {
 	.ack_interrupt	= smsc_phy_ack_interrupt,
 	.config_intr	= smsc_phy_config_intr,
 
+	/* Statistics */
+	.get_sset_count = smsc_get_sset_count,
+	.get_strings	= smsc_get_strings,
+	.get_stats	= smsc_get_stats,
+
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
 }, {
@@ -236,7 +298,7 @@ static struct phy_driver smsc_phy_driver[] = {
 	.name		= "SMSC LAN911x Internal PHY",
 
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_INTERRUPT | PHY_HAS_MAGICANEG,
+	.flags		= PHY_HAS_INTERRUPT,
 
 	.probe		= smsc_phy_probe,
 
@@ -257,7 +319,7 @@ static struct phy_driver smsc_phy_driver[] = {
 	.name		= "SMSC LAN8710/LAN8720",
 
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_INTERRUPT | PHY_HAS_MAGICANEG,
+	.flags		= PHY_HAS_INTERRUPT,
 
 	.probe		= smsc_phy_probe,
 
@@ -271,6 +333,11 @@ static struct phy_driver smsc_phy_driver[] = {
 	.ack_interrupt	= smsc_phy_ack_interrupt,
 	.config_intr	= smsc_phy_config_intr,
 
+	/* Statistics */
+	.get_sset_count = smsc_get_sset_count,
+	.get_strings	= smsc_get_strings,
+	.get_stats	= smsc_get_stats,
+
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
 }, {
@@ -279,7 +346,7 @@ static struct phy_driver smsc_phy_driver[] = {
 	.name		= "SMSC LAN8740",
 
 	.features	= PHY_BASIC_FEATURES,
-	.flags		= PHY_HAS_INTERRUPT | PHY_HAS_MAGICANEG,
+	.flags		= PHY_HAS_INTERRUPT,
 
 	.probe		= smsc_phy_probe,
 
@@ -293,6 +360,11 @@ static struct phy_driver smsc_phy_driver[] = {
 	.ack_interrupt	= smsc_phy_ack_interrupt,
 	.config_intr	= smsc_phy_config_intr,
 
+	/* Statistics */
+	.get_sset_count = smsc_get_sset_count,
+	.get_strings	= smsc_get_strings,
+	.get_stats	= smsc_get_stats,
+
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
 } };
diff --git a/drivers/net/ppp/ppp_async.c b/drivers/net/ppp/ppp_async.c
index feb9569e3345..814fd8fae67d 100644
--- a/drivers/net/ppp/ppp_async.c
+++ b/drivers/net/ppp/ppp_async.c
@@ -802,7 +802,7 @@ process_input_packet(struct asyncppp *ap)
 	proto = p[0];
 	if (proto & 1) {
 		/* protocol is compressed */
-		skb_push(skb, 1)[0] = 0;
+		*(u8 *)skb_push(skb, 1) = 0;
 	} else {
 		if (skb->len < 2)
 			goto err;
@@ -894,8 +894,7 @@ ppp_async_input(struct asyncppp *ap, const unsigned char *buf,
 				/* packet overflowed MRU */
 				ap->state |= SC_TOSS;
 			} else {
-				sp = skb_put(skb, n);
-				memcpy(sp, buf, n);
+				sp = skb_put_data(skb, buf, n);
 				if (ap->state & SC_ESCAPE) {
 					sp[0] ^= PPP_TRANS;
 					ap->state &= ~SC_ESCAPE;
diff --git a/drivers/net/ppp/ppp_generic.c b/drivers/net/ppp/ppp_generic.c
index f9c0e62716ea..13028833bee3 100644
--- a/drivers/net/ppp/ppp_generic.c
+++ b/drivers/net/ppp/ppp_generic.c
@@ -1061,7 +1061,8 @@ static const struct nla_policy ppp_nl_policy[IFLA_PPP_MAX + 1] = {
 	[IFLA_PPP_DEV_FD]	= { .type = NLA_S32 },
 };
 
-static int ppp_nl_validate(struct nlattr *tb[], struct nlattr *data[])
+static int ppp_nl_validate(struct nlattr *tb[], struct nlattr *data[],
+			   struct netlink_ext_ack *extack)
 {
 	if (!data)
 		return -EINVAL;
@@ -1075,7 +1076,8 @@ static int ppp_nl_validate(struct nlattr *tb[], struct nlattr *data[])
 }
 
 static int ppp_nl_newlink(struct net *src_net, struct net_device *dev,
-			  struct nlattr *tb[], struct nlattr *data[])
+			  struct nlattr *tb[], struct nlattr *data[],
+			  struct netlink_ext_ack *extack)
 {
 	struct ppp_config conf = {
 		.unit = -1,
@@ -1490,7 +1492,7 @@ ppp_send_frame(struct ppp *ppp, struct sk_buff *skb)
 		/* check if we should pass this packet */
 		/* the filter instructions are constructed assuming
 		   a four-byte PPP header on each packet */
-		*skb_push(skb, 2) = 1;
+		*(u8 *)skb_push(skb, 2) = 1;
 		if (ppp->pass_filter &&
 		    BPF_PROG_RUN(ppp->pass_filter, skb) == 0) {
 			if (ppp->debug & 1)
@@ -1618,7 +1620,7 @@ ppp_push(struct ppp *ppp)
 		list = list->next;
 		pch = list_entry(list, struct channel, clist);
 
-		spin_lock_bh(&pch->downl);
+		spin_lock(&pch->downl);
 		if (pch->chan) {
 			if (pch->chan->ops->start_xmit(pch->chan, skb))
 				ppp->xmit_pending = NULL;
@@ -1627,7 +1629,7 @@ ppp_push(struct ppp *ppp)
 			kfree_skb(skb);
 			ppp->xmit_pending = NULL;
 		}
-		spin_unlock_bh(&pch->downl);
+		spin_unlock(&pch->downl);
 		return;
 	}
 
@@ -1757,7 +1759,7 @@ static int ppp_mp_explode(struct ppp *ppp, struct sk_buff *skb)
 		}
 
 		/* check the channel's mtu and whether it is still attached. */
-		spin_lock_bh(&pch->downl);
+		spin_lock(&pch->downl);
 		if (pch->chan == NULL) {
 			/* can't use this channel, it's being deregistered */
 			if (pch->speed == 0)
@@ -1765,7 +1767,7 @@ static int ppp_mp_explode(struct ppp *ppp, struct sk_buff *skb)
 			else
 				totspeed -= pch->speed;
 
-			spin_unlock_bh(&pch->downl);
+			spin_unlock(&pch->downl);
 			pch->avail = 0;
 			totlen = len;
 			totfree--;
@@ -1816,7 +1818,7 @@ static int ppp_mp_explode(struct ppp *ppp, struct sk_buff *skb)
 		 */
 		if (flen <= 0) {
 			pch->avail = 2;
-			spin_unlock_bh(&pch->downl);
+			spin_unlock(&pch->downl);
 			continue;
 		}
 
@@ -1861,14 +1863,14 @@ static int ppp_mp_explode(struct ppp *ppp, struct sk_buff *skb)
 		len -= flen;
 		++ppp->nxseq;
 		bits = 0;
-		spin_unlock_bh(&pch->downl);
+		spin_unlock(&pch->downl);
 	}
 	ppp->nxchan = i;
 
 	return 1;
 
  noskb:
-	spin_unlock_bh(&pch->downl);
+	spin_unlock(&pch->downl);
 	if (ppp->debug & 1)
 		netdev_err(ppp->dev, "PPP: no memory (fragment)\n");
 	++ppp->dev->stats.tx_errors;
@@ -1883,7 +1885,7 @@ static void __ppp_channel_push(struct channel *pch)
 	struct sk_buff *skb;
 	struct ppp *ppp;
 
-	spin_lock_bh(&pch->downl);
+	spin_lock(&pch->downl);
 	if (pch->chan) {
 		while (!skb_queue_empty(&pch->file.xq)) {
 			skb = skb_dequeue(&pch->file.xq);
@@ -1897,14 +1899,14 @@ static void __ppp_channel_push(struct channel *pch)
 		/* channel got deregistered */
 		skb_queue_purge(&pch->file.xq);
 	}
-	spin_unlock_bh(&pch->downl);
+	spin_unlock(&pch->downl);
 	/* see if there is anything from the attached unit to be sent */
 	if (skb_queue_empty(&pch->file.xq)) {
-		read_lock_bh(&pch->upl);
+		read_lock(&pch->upl);
 		ppp = pch->ppp;
 		if (ppp)
 			__ppp_xmit_process(ppp);
-		read_unlock_bh(&pch->upl);
+		read_unlock(&pch->upl);
 	}
 }
 
@@ -2133,7 +2135,7 @@ ppp_receive_nonmp_frame(struct ppp *ppp, struct sk_buff *skb)
 			if (skb_unclone(skb, GFP_ATOMIC))
 				goto err;
 
-			*skb_push(skb, 2) = 0;
+			*(u8 *)skb_push(skb, 2) = 0;
 			if (ppp->pass_filter &&
 			    BPF_PROG_RUN(ppp->pass_filter, skb) == 0) {
 				if (ppp->debug & 1)
@@ -2267,7 +2269,7 @@ ppp_receive_mp_frame(struct ppp *ppp, struct sk_buff *skb, struct channel *pch)
 	 * Do protocol ID decompression on the first fragment of each packet.
 	 */
 	if ((PPP_MP_CB(skb)->BEbits & B) && (skb->data[0] & 1))
-		*skb_push(skb, 1) = 0;
+		*(u8 *)skb_push(skb, 1) = 0;
 
 	/*
 	 * Expand sequence number to 32 bits, making it as close
diff --git a/drivers/net/ppp/ppp_mppe.c b/drivers/net/ppp/ppp_mppe.c
index f60f7660b451..6c7fd98cb00a 100644
--- a/drivers/net/ppp/ppp_mppe.c
+++ b/drivers/net/ppp/ppp_mppe.c
@@ -298,21 +298,14 @@ mppe_init(void *arg, unsigned char *options, int optlen, int unit, int debug,
 	mppe_rekey(state, 1);
 
 	if (debug) {
-		int i;
-		char mkey[sizeof(state->master_key) * 2 + 1];
-		char skey[sizeof(state->session_key) * 2 + 1];
-
 		printk(KERN_DEBUG "%s[%d]: initialized with %d-bit %s mode\n",
 		       debugstr, unit, (state->keylen == 16) ? 128 : 40,
 		       (state->stateful) ? "stateful" : "stateless");
-
-		for (i = 0; i < sizeof(state->master_key); i++)
-			sprintf(mkey + i * 2, "%02x", state->master_key[i]);
-		for (i = 0; i < sizeof(state->session_key); i++)
-			sprintf(skey + i * 2, "%02x", state->session_key[i]);
 		printk(KERN_DEBUG
-		       "%s[%d]: keys: master: %s initial session: %s\n",
-		       debugstr, unit, mkey, skey);
+		       "%s[%d]: keys: master: %*phN initial session: %*phN\n",
+		       debugstr, unit,
+		       (int)sizeof(state->master_key), state->master_key,
+		       (int)sizeof(state->session_key), state->session_key);
 	}
 
 	/*
diff --git a/drivers/net/ppp/ppp_synctty.c b/drivers/net/ppp/ppp_synctty.c
index 9ae53986cb4a..7868c29071d4 100644
--- a/drivers/net/ppp/ppp_synctty.c
+++ b/drivers/net/ppp/ppp_synctty.c
@@ -697,8 +697,7 @@ ppp_sync_input(struct syncppp *ap, const unsigned char *buf,
 		goto err;
 	}
 
-	p = skb_put(skb, count);
-	memcpy(p, buf, count);
+	skb_put_data(skb, buf, count);
 
 	/* strip address/control field if present */
 	p = skb->data;
@@ -712,7 +711,7 @@ ppp_sync_input(struct syncppp *ap, const unsigned char *buf,
 	/* decompress protocol field if compressed */
 	if (p[0] & 1) {
 		/* protocol is compressed */
-		skb_push(skb, 1)[0] = 0;
+		*(u8 *)skb_push(skb, 1) = 0;
 	} else if (skb->len < 2)
 		goto err;
 
diff --git a/drivers/net/ppp/pppoe.c b/drivers/net/ppp/pppoe.c
index d7e405268983..4e1da1645b15 100644
--- a/drivers/net/ppp/pppoe.c
+++ b/drivers/net/ppp/pppoe.c
@@ -877,7 +877,7 @@ static int pppoe_sendmsg(struct socket *sock, struct msghdr *m,
 	skb->priority = sk->sk_priority;
 	skb->protocol = cpu_to_be16(ETH_P_PPP_SES);
 
-	ph = (struct pppoe_hdr *)skb_put(skb, total_len + sizeof(struct pppoe_hdr));
+	ph = skb_put(skb, total_len + sizeof(struct pppoe_hdr));
 	start = (char *)&ph->tag[0];
 
 	error = memcpy_from_msg(start, m, total_len);
diff --git a/drivers/net/ppp/pptp.c b/drivers/net/ppp/pptp.c
index 1951b1085cb8..eac499c58aa7 100644
--- a/drivers/net/ppp/pptp.c
+++ b/drivers/net/ppp/pptp.c
@@ -328,7 +328,7 @@ allow_packet:
 
 		if ((*skb->data) & 1) {
 			/* protocol is compressed */
-			skb_push(skb, 1)[0] = 0;
+			*(u8 *)skb_push(skb, 1) = 0;
 		}
 
 		skb->ip_summed = CHECKSUM_NONE;
@@ -506,7 +506,6 @@ static int pptp_release(struct socket *sock)
 {
 	struct sock *sk = sock->sk;
 	struct pppox_sock *po;
-	struct pptp_opt *opt;
 	int error = 0;
 
 	if (!sk)
@@ -520,7 +519,6 @@ static int pptp_release(struct socket *sock)
 	}
 
 	po = pppox_sk(sk);
-	opt = &po->proto.pptp;
 	del_chan(po);
 
 	pppox_unbind_sock(sk);
diff --git a/drivers/net/rionet.c b/drivers/net/rionet.c
index 300bb1479b3a..e9f101c9bae2 100644
--- a/drivers/net/rionet.c
+++ b/drivers/net/rionet.c
@@ -201,7 +201,7 @@ static int rionet_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 				rionet_queue_tx_msg(skb, ndev,
 					nets[rnet->mport->id].active[i]);
 				if (count)
-					atomic_inc(&skb->users);
+					refcount_inc(&skb->users);
 				count++;
 			}
 	} else if (RIONET_MAC_MATCH(eth->h_dest)) {
diff --git a/drivers/net/slip/slip.c b/drivers/net/slip/slip.c
index 1da31dc47f86..436dd78c396a 100644
--- a/drivers/net/slip/slip.c
+++ b/drivers/net/slip/slip.c
@@ -364,7 +364,7 @@ static void sl_bump(struct slip *sl)
 		return;
 	}
 	skb->dev = dev;
-	memcpy(skb_put(skb, count), sl->rbuff, count);
+	skb_put_data(skb, sl->rbuff, count);
 	skb_reset_mac_header(skb);
 	skb->protocol = htons(ETH_P_IP);
 	netif_rx_ni(skb);
@@ -629,7 +629,7 @@ static void sl_uninit(struct net_device *dev)
 static void sl_free_netdev(struct net_device *dev)
 {
 	int i = dev->base_addr;
-	free_netdev(dev);
+
 	slip_devs[i] = NULL;
 }
 
@@ -651,7 +651,8 @@ static const struct net_device_ops sl_netdev_ops = {
 static void sl_setup(struct net_device *dev)
 {
 	dev->netdev_ops		= &sl_netdev_ops;
-	dev->destructor		= sl_free_netdev;
+	dev->needs_free_netdev	= true;
+	dev->priv_destructor	= sl_free_netdev;
 
 	dev->hard_header_len	= 0;
 	dev->addr_len		= 0;
@@ -1369,8 +1370,6 @@ static void __exit slip_exit(void)
 		if (sl->tty) {
 			printk(KERN_ERR "%s: tty discipline still running\n",
 			       dev->name);
-			/* Intentionally leak the control block. */
-			dev->destructor = NULL;
 		}
 
 		unregister_netdev(dev);
diff --git a/drivers/net/sungem_phy.c b/drivers/net/sungem_phy.c
index 92578d72e4ee..63a8ff816e59 100644
--- a/drivers/net/sungem_phy.c
+++ b/drivers/net/sungem_phy.c
@@ -886,7 +886,7 @@ static int marvell_read_link(struct mii_phy *phy)
 	 SUPPORTED_1000baseT_Half | SUPPORTED_1000baseT_Full)
 
 /* Broadcom BCM 5201 */
-static struct mii_phy_ops bcm5201_phy_ops = {
+static const struct mii_phy_ops bcm5201_phy_ops = {
 	.init		= bcm5201_init,
 	.suspend	= bcm5201_suspend,
 	.setup_aneg	= genmii_setup_aneg,
@@ -905,7 +905,7 @@ static struct mii_phy_def bcm5201_phy_def = {
 };
 
 /* Broadcom BCM 5221 */
-static struct mii_phy_ops bcm5221_phy_ops = {
+static const struct mii_phy_ops bcm5221_phy_ops = {
 	.suspend	= bcm5221_suspend,
 	.init		= bcm5221_init,
 	.setup_aneg	= genmii_setup_aneg,
@@ -924,7 +924,7 @@ static struct mii_phy_def bcm5221_phy_def = {
 };
 
 /* Broadcom BCM 5241 */
-static struct mii_phy_ops bcm5241_phy_ops = {
+static const struct mii_phy_ops bcm5241_phy_ops = {
 	.suspend	= bcm5241_suspend,
 	.init		= bcm5241_init,
 	.setup_aneg	= genmii_setup_aneg,
@@ -942,7 +942,7 @@ static struct mii_phy_def bcm5241_phy_def = {
 };
 
 /* Broadcom BCM 5400 */
-static struct mii_phy_ops bcm5400_phy_ops = {
+static const struct mii_phy_ops bcm5400_phy_ops = {
 	.init		= bcm5400_init,
 	.suspend	= bcm5400_suspend,
 	.setup_aneg	= bcm54xx_setup_aneg,
@@ -961,7 +961,7 @@ static struct mii_phy_def bcm5400_phy_def = {
 };
 
 /* Broadcom BCM 5401 */
-static struct mii_phy_ops bcm5401_phy_ops = {
+static const struct mii_phy_ops bcm5401_phy_ops = {
 	.init		= bcm5401_init,
 	.suspend	= bcm5401_suspend,
 	.setup_aneg	= bcm54xx_setup_aneg,
@@ -980,7 +980,7 @@ static struct mii_phy_def bcm5401_phy_def = {
 };
 
 /* Broadcom BCM 5411 */
-static struct mii_phy_ops bcm5411_phy_ops = {
+static const struct mii_phy_ops bcm5411_phy_ops = {
 	.init		= bcm5411_init,
 	.suspend	= generic_suspend,
 	.setup_aneg	= bcm54xx_setup_aneg,
@@ -999,7 +999,7 @@ static struct mii_phy_def bcm5411_phy_def = {
 };
 
 /* Broadcom BCM 5421 */
-static struct mii_phy_ops bcm5421_phy_ops = {
+static const struct mii_phy_ops bcm5421_phy_ops = {
 	.init		= bcm5421_init,
 	.suspend	= generic_suspend,
 	.setup_aneg	= bcm54xx_setup_aneg,
@@ -1019,7 +1019,7 @@ static struct mii_phy_def bcm5421_phy_def = {
 };
 
 /* Broadcom BCM 5421 built-in K2 */
-static struct mii_phy_ops bcm5421k2_phy_ops = {
+static const struct mii_phy_ops bcm5421k2_phy_ops = {
 	.init		= bcm5421_init,
 	.suspend	= generic_suspend,
 	.setup_aneg	= bcm54xx_setup_aneg,
@@ -1037,7 +1037,7 @@ static struct mii_phy_def bcm5421k2_phy_def = {
 	.ops		= &bcm5421k2_phy_ops
 };
 
-static struct mii_phy_ops bcm5461_phy_ops = {
+static const struct mii_phy_ops bcm5461_phy_ops = {
 	.init		= bcm5421_init,
 	.suspend	= generic_suspend,
 	.setup_aneg	= bcm54xx_setup_aneg,
@@ -1057,7 +1057,7 @@ static struct mii_phy_def bcm5461_phy_def = {
 };
 
 /* Broadcom BCM 5462 built-in Vesta */
-static struct mii_phy_ops bcm5462V_phy_ops = {
+static const struct mii_phy_ops bcm5462V_phy_ops = {
 	.init		= bcm5421_init,
 	.suspend	= generic_suspend,
 	.setup_aneg	= bcm54xx_setup_aneg,
@@ -1076,7 +1076,7 @@ static struct mii_phy_def bcm5462V_phy_def = {
 };
 
 /* Marvell 88E1101 amd 88E1111 */
-static struct mii_phy_ops marvell88e1101_phy_ops = {
+static const struct mii_phy_ops marvell88e1101_phy_ops = {
 	.suspend	= generic_suspend,
 	.setup_aneg	= marvell_setup_aneg,
 	.setup_forced	= marvell_setup_forced,
@@ -1084,7 +1084,7 @@ static struct mii_phy_ops marvell88e1101_phy_ops = {
 	.read_link	= marvell_read_link
 };
 
-static struct mii_phy_ops marvell88e1111_phy_ops = {
+static const struct mii_phy_ops marvell88e1111_phy_ops = {
 	.init		= marvell88e1111_init,
 	.suspend	= generic_suspend,
 	.setup_aneg	= marvell_setup_aneg,
@@ -1122,7 +1122,7 @@ static struct mii_phy_def marvell88e1111_phy_def = {
 };
 
 /* Generic implementation for most 10/100 PHYs */
-static struct mii_phy_ops generic_phy_ops = {
+static const struct mii_phy_ops generic_phy_ops = {
 	.setup_aneg	= genmii_setup_aneg,
 	.setup_forced	= genmii_setup_forced,
 	.poll_link	= genmii_poll_link,
diff --git a/drivers/net/tap.c b/drivers/net/tap.c
index 4d4173d25dd0..9af3239d6ad5 100644
--- a/drivers/net/tap.c
+++ b/drivers/net/tap.c
@@ -824,15 +824,17 @@ done:
 
 static ssize_t tap_do_read(struct tap_queue *q,
 			   struct iov_iter *to,
-			   int noblock)
+			   int noblock, struct sk_buff *skb)
 {
 	DEFINE_WAIT(wait);
-	struct sk_buff *skb;
 	ssize_t ret = 0;
 
 	if (!iov_iter_count(to))
 		return 0;
 
+	if (skb)
+		goto put;
+
 	while (1) {
 		if (!noblock)
 			prepare_to_wait(sk_sleep(&q->sk), &wait,
@@ -856,6 +858,7 @@ static ssize_t tap_do_read(struct tap_queue *q,
 	if (!noblock)
 		finish_wait(sk_sleep(&q->sk), &wait);
 
+put:
 	if (skb) {
 		ret = tap_put_user(q, skb, to);
 		if (unlikely(ret < 0))
@@ -872,7 +875,7 @@ static ssize_t tap_read_iter(struct kiocb *iocb, struct iov_iter *to)
 	struct tap_queue *q = file->private_data;
 	ssize_t len = iov_iter_count(to), ret;
 
-	ret = tap_do_read(q, to, file->f_flags & O_NONBLOCK);
+	ret = tap_do_read(q, to, file->f_flags & O_NONBLOCK, NULL);
 	ret = min_t(ssize_t, ret, len);
 	if (ret > 0)
 		iocb->ki_pos = ret;
@@ -1155,7 +1158,8 @@ static int tap_recvmsg(struct socket *sock, struct msghdr *m,
 	int ret;
 	if (flags & ~(MSG_DONTWAIT|MSG_TRUNC))
 		return -EINVAL;
-	ret = tap_do_read(q, &m->msg_iter, flags & MSG_DONTWAIT);
+	ret = tap_do_read(q, &m->msg_iter, flags & MSG_DONTWAIT,
+			  m->msg_control);
 	if (ret > total_len) {
 		m->msg_flags |= MSG_TRUNC;
 		ret = flags & MSG_TRUNC ? ret : total_len;
@@ -1193,6 +1197,19 @@ struct socket *tap_get_socket(struct file *file)
 }
 EXPORT_SYMBOL_GPL(tap_get_socket);
 
+struct skb_array *tap_get_skb_array(struct file *file)
+{
+	struct tap_queue *q;
+
+	if (file->f_op != &tap_fops)
+		return ERR_PTR(-EINVAL);
+	q = file->private_data;
+	if (!q)
+		return ERR_PTR(-EBADFD);
+	return &q->skb_array;
+}
+EXPORT_SYMBOL_GPL(tap_get_skb_array);
+
 int tap_queue_resize(struct tap_dev *tap)
 {
 	struct net_device *dev = tap->dev;
diff --git a/drivers/net/team/team.c b/drivers/net/team/team.c
index 6c5d5ef46f75..464570409796 100644
--- a/drivers/net/team/team.c
+++ b/drivers/net/team/team.c
@@ -1643,7 +1643,6 @@ static void team_destructor(struct net_device *dev)
 	struct team *team = netdev_priv(dev);
 
 	free_percpu(team->pcpu_stats);
-	free_netdev(dev);
 }
 
 static int team_open(struct net_device *dev)
@@ -2005,12 +2004,6 @@ static const struct net_device_ops team_netdev_ops = {
 	.ndo_del_slave		= team_del_slave,
 	.ndo_fix_features	= team_fix_features,
 	.ndo_change_carrier     = team_change_carrier,
-	.ndo_bridge_setlink	= switchdev_port_bridge_setlink,
-	.ndo_bridge_getlink	= switchdev_port_bridge_getlink,
-	.ndo_bridge_dellink	= switchdev_port_bridge_dellink,
-	.ndo_fdb_add		= switchdev_port_fdb_add,
-	.ndo_fdb_del		= switchdev_port_fdb_del,
-	.ndo_fdb_dump		= switchdev_port_fdb_dump,
 	.ndo_features_check	= passthru_features_check,
 };
 
@@ -2079,7 +2072,8 @@ static void team_setup(struct net_device *dev)
 
 	dev->netdev_ops = &team_netdev_ops;
 	dev->ethtool_ops = &team_ethtool_ops;
-	dev->destructor	= team_destructor;
+	dev->needs_free_netdev = true;
+	dev->priv_destructor = team_destructor;
 	dev->priv_flags &= ~(IFF_XMIT_DST_RELEASE | IFF_TX_SKB_SHARING);
 	dev->priv_flags |= IFF_NO_QUEUE;
 	dev->priv_flags |= IFF_TEAM;
@@ -2107,7 +2101,8 @@ static void team_setup(struct net_device *dev)
 }
 
 static int team_newlink(struct net *src_net, struct net_device *dev,
-			struct nlattr *tb[], struct nlattr *data[])
+			struct nlattr *tb[], struct nlattr *data[],
+			struct netlink_ext_ack *extack)
 {
 	if (tb[IFLA_ADDRESS] == NULL)
 		eth_hw_addr_random(dev);
@@ -2115,7 +2110,8 @@ static int team_newlink(struct net *src_net, struct net_device *dev,
 	return register_netdevice(dev);
 }
 
-static int team_validate(struct nlattr *tb[], struct nlattr *data[])
+static int team_validate(struct nlattr *tb[], struct nlattr *data[],
+			 struct netlink_ext_ack *extack)
 {
 	if (tb[IFLA_ADDRESS]) {
 		if (nla_len(tb[IFLA_ADDRESS]) != ETH_ALEN)
diff --git a/drivers/net/team/team_mode_activebackup.c b/drivers/net/team/team_mode_activebackup.c
index 3f189823ba3b..ddd16a0c1fc1 100644
--- a/drivers/net/team/team_mode_activebackup.c
+++ b/drivers/net/team/team_mode_activebackup.c
@@ -146,4 +146,4 @@ module_exit(ab_cleanup_module);
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Jiri Pirko <jpirko@redhat.com>");
 MODULE_DESCRIPTION("Active-backup mode for team");
-MODULE_ALIAS("team-mode-activebackup");
+MODULE_ALIAS_TEAM_MODE("activebackup");
diff --git a/drivers/net/team/team_mode_broadcast.c b/drivers/net/team/team_mode_broadcast.c
index 302ff35b0cbc..e4eac3de1862 100644
--- a/drivers/net/team/team_mode_broadcast.c
+++ b/drivers/net/team/team_mode_broadcast.c
@@ -75,4 +75,4 @@ 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");
+MODULE_ALIAS_TEAM_MODE("broadcast");
diff --git a/drivers/net/team/team_mode_loadbalance.c b/drivers/net/team/team_mode_loadbalance.c
index b228bea7931f..1468ddf424cc 100644
--- a/drivers/net/team/team_mode_loadbalance.c
+++ b/drivers/net/team/team_mode_loadbalance.c
@@ -695,4 +695,4 @@ module_exit(lb_cleanup_module);
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Jiri Pirko <jpirko@redhat.com>");
 MODULE_DESCRIPTION("Load-balancing mode for team");
-MODULE_ALIAS("team-mode-loadbalance");
+MODULE_ALIAS_TEAM_MODE("loadbalance");
diff --git a/drivers/net/team/team_mode_random.c b/drivers/net/team/team_mode_random.c
index 215f845782db..c20b9446e2e4 100644
--- a/drivers/net/team/team_mode_random.c
+++ b/drivers/net/team/team_mode_random.c
@@ -65,4 +65,4 @@ module_exit(rnd_cleanup_module);
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Jiri Pirko <jiri@resnulli.us>");
 MODULE_DESCRIPTION("Random mode for team");
-MODULE_ALIAS("team-mode-random");
+MODULE_ALIAS_TEAM_MODE("random");
diff --git a/drivers/net/team/team_mode_roundrobin.c b/drivers/net/team/team_mode_roundrobin.c
index 0aa234118c03..66c3209dc4a6 100644
--- a/drivers/net/team/team_mode_roundrobin.c
+++ b/drivers/net/team/team_mode_roundrobin.c
@@ -77,4 +77,4 @@ module_exit(rr_cleanup_module);
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Jiri Pirko <jpirko@redhat.com>");
 MODULE_DESCRIPTION("Round-robin mode for team");
-MODULE_ALIAS("team-mode-roundrobin");
+MODULE_ALIAS_TEAM_MODE("roundrobin");
diff --git a/drivers/net/tun.c b/drivers/net/tun.c
index bbd707b9ef7a..3d4c24572ecd 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -465,7 +465,7 @@ static u16 tun_select_queue(struct net_device *dev, struct sk_buff *skb,
 	rcu_read_lock();
 	numqueues = ACCESS_ONCE(tun->numqueues);
 
-	txq = skb_get_hash(skb);
+	txq = __skb_get_hash_symmetric(skb);
 	if (txq) {
 		e = tun_flow_find(&tun->flows[tun_hashfn(txq)], txq);
 		if (e) {
@@ -867,7 +867,7 @@ static netdev_tx_t tun_net_xmit(struct sk_buff *skb, struct net_device *dev)
 		 */
 		__u32 rxhash;
 
-		rxhash = skb_get_hash(skb);
+		rxhash = __skb_get_hash_symmetric(skb);
 		if (rxhash) {
 			struct tun_flow_entry *e;
 			e = tun_flow_find(&tun->flows[tun_hashfn(rxhash)],
@@ -1334,7 +1334,7 @@ static ssize_t tun_get_user(struct tun_struct *tun, struct tun_file *tfile,
 	skb_reset_network_header(skb);
 	skb_probe_transport_header(skb, 0);
 
-	rxhash = skb_get_hash(skb);
+	rxhash = __skb_get_hash_symmetric(skb);
 #ifndef CONFIG_4KSTACKS
 	tun_rx_batched(tun, tfile, skb, more);
 #else
@@ -1510,9 +1510,8 @@ out:
 
 static ssize_t tun_do_read(struct tun_struct *tun, struct tun_file *tfile,
 			   struct iov_iter *to,
-			   int noblock)
+			   int noblock, struct sk_buff *skb)
 {
-	struct sk_buff *skb;
 	ssize_t ret;
 	int err;
 
@@ -1521,10 +1520,12 @@ static ssize_t tun_do_read(struct tun_struct *tun, struct tun_file *tfile,
 	if (!iov_iter_count(to))
 		return 0;
 
-	/* Read frames from ring */
-	skb = tun_ring_recv(tfile, noblock, &err);
-	if (!skb)
-		return err;
+	if (!skb) {
+		/* Read frames from ring */
+		skb = tun_ring_recv(tfile, noblock, &err);
+		if (!skb)
+			return err;
+	}
 
 	ret = tun_put_user(tun, tfile, skb, to);
 	if (unlikely(ret < 0))
@@ -1544,7 +1545,7 @@ static ssize_t tun_chr_read_iter(struct kiocb *iocb, struct iov_iter *to)
 
 	if (!tun)
 		return -EBADFD;
-	ret = tun_do_read(tun, tfile, to, file->f_flags & O_NONBLOCK);
+	ret = tun_do_read(tun, tfile, to, file->f_flags & O_NONBLOCK, NULL);
 	ret = min_t(ssize_t, ret, len);
 	if (ret > 0)
 		iocb->ki_pos = ret;
@@ -1560,7 +1561,6 @@ static void tun_free_netdev(struct net_device *dev)
 	free_percpu(tun->pcpu_stats);
 	tun_flow_uninit(tun);
 	security_tun_dev_free_security(tun->security);
-	free_netdev(dev);
 }
 
 static void tun_setup(struct net_device *dev)
@@ -1571,7 +1571,8 @@ static void tun_setup(struct net_device *dev)
 	tun->group = INVALID_GID;
 
 	dev->ethtool_ops = &tun_ethtool_ops;
-	dev->destructor = tun_free_netdev;
+	dev->needs_free_netdev = true;
+	dev->priv_destructor = tun_free_netdev;
 	/* We prefer our own queue length */
 	dev->tx_queue_len = TUN_READQ_SIZE;
 }
@@ -1579,7 +1580,8 @@ static void tun_setup(struct net_device *dev)
 /* Trivial set of netlink ops to allow deleting tun or tap
  * device with netlink.
  */
-static int tun_validate(struct nlattr *tb[], struct nlattr *data[])
+static int tun_validate(struct nlattr *tb[], struct nlattr *data[],
+			struct netlink_ext_ack *extack)
 {
 	return -EINVAL;
 }
@@ -1646,7 +1648,8 @@ static int tun_recvmsg(struct socket *sock, struct msghdr *m, size_t total_len,
 					 SOL_PACKET, TUN_TX_TIMESTAMP);
 		goto out;
 	}
-	ret = tun_do_read(tun, tfile, &m->msg_iter, flags & MSG_DONTWAIT);
+	ret = tun_do_read(tun, tfile, &m->msg_iter, flags & MSG_DONTWAIT,
+			  m->msg_control);
 	if (ret > (ssize_t)total_len) {
 		m->msg_flags |= MSG_TRUNC;
 		ret = flags & MSG_TRUNC ? ret : total_len;
@@ -2626,6 +2629,19 @@ struct socket *tun_get_socket(struct file *file)
 }
 EXPORT_SYMBOL_GPL(tun_get_socket);
 
+struct skb_array *tun_get_skb_array(struct file *file)
+{
+	struct tun_file *tfile;
+
+	if (file->f_op != &tun_fops)
+		return ERR_PTR(-EINVAL);
+	tfile = file->private_data;
+	if (!tfile)
+		return ERR_PTR(-EBADFD);
+	return &tfile->tx_array;
+}
+EXPORT_SYMBOL_GPL(tun_get_skb_array);
+
 module_init(tun_init);
 module_exit(tun_cleanup);
 MODULE_DESCRIPTION(DRV_DESCRIPTION);
diff --git a/drivers/net/usb/asix_common.c b/drivers/net/usb/asix_common.c
index 125cff57c759..7847436c441e 100644
--- a/drivers/net/usb/asix_common.c
+++ b/drivers/net/usb/asix_common.c
@@ -113,7 +113,6 @@ int asix_rx_fixup_internal(struct usbnet *dev, struct sk_buff *skb,
 
 	while (offset + sizeof(u16) <= skb->len) {
 		u16 copy_length;
-		unsigned char *data;
 
 		if (!rx->remaining) {
 			if (skb->len - offset == sizeof(u16)) {
@@ -167,8 +166,8 @@ int asix_rx_fixup_internal(struct usbnet *dev, struct sk_buff *skb,
 		}
 
 		if (rx->ax_skb) {
-			data = skb_put(rx->ax_skb, copy_length);
-			memcpy(data, skb->data + offset, copy_length);
+			skb_put_data(rx->ax_skb, skb->data + offset,
+				     copy_length);
 			if (!rx->remaining)
 				usbnet_skb_return(dev, rx->ax_skb);
 		}
diff --git a/drivers/net/usb/ax88179_178a.c b/drivers/net/usb/ax88179_178a.c
index 51cf60092a18..f32261ecd215 100644
--- a/drivers/net/usb/ax88179_178a.c
+++ b/drivers/net/usb/ax88179_178a.c
@@ -624,7 +624,10 @@ static int ax88179_get_link_ksettings(struct net_device *net,
 				      struct ethtool_link_ksettings *cmd)
 {
 	struct usbnet *dev = netdev_priv(net);
-	return mii_ethtool_get_link_ksettings(&dev->mii, cmd);
+
+	mii_ethtool_get_link_ksettings(&dev->mii, cmd);
+
+	return 0;
 }
 
 static int ax88179_set_link_ksettings(struct net_device *net,
@@ -1722,6 +1725,18 @@ static const struct driver_info lenovo_info = {
 	.tx_fixup = ax88179_tx_fixup,
 };
 
+static const struct driver_info belkin_info = {
+	.description = "Belkin USB Ethernet Adapter",
+	.bind	= ax88179_bind,
+	.unbind = ax88179_unbind,
+	.status = ax88179_status,
+	.link_reset = ax88179_link_reset,
+	.reset	= ax88179_reset,
+	.flags	= FLAG_ETHER | FLAG_FRAMING_AX,
+	.rx_fixup = ax88179_rx_fixup,
+	.tx_fixup = ax88179_tx_fixup,
+};
+
 static const struct usb_device_id products[] = {
 {
 	/* ASIX AX88179 10/100/1000 */
@@ -1751,6 +1766,10 @@ static const struct usb_device_id products[] = {
 	/* Lenovo OneLinkDock Gigabit LAN */
 	USB_DEVICE(0x17ef, 0x304b),
 	.driver_info = (unsigned long)&lenovo_info,
+}, {
+	/* Belkin B2B128 USB 3.0 Hub + Gigabit Ethernet Adapter */
+	USB_DEVICE(0x050d, 0x0128),
+	.driver_info = (unsigned long)&belkin_info,
 },
 	{ },
 };
diff --git a/drivers/net/usb/cdc-phonet.c b/drivers/net/usb/cdc-phonet.c
index eb52de8205f0..2952cb570996 100644
--- a/drivers/net/usb/cdc-phonet.c
+++ b/drivers/net/usb/cdc-phonet.c
@@ -162,7 +162,7 @@ static void rx_complete(struct urb *req)
 			skb = pnd->rx_skb = netdev_alloc_skb(dev, 12);
 			if (likely(skb)) {
 				/* Can't use pskb_pull() on page in IRQ */
-				memcpy(skb_put(skb, 1), page_address(page), 1);
+				skb_put_data(skb, page_address(page), 1);
 				skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
 						page, 1, req->actual_length,
 						PAGE_SIZE);
@@ -298,7 +298,7 @@ static void usbpn_setup(struct net_device *dev)
 	dev->addr_len		= 1;
 	dev->tx_queue_len	= 3;
 
-	dev->destructor		= free_netdev;
+	dev->needs_free_netdev	= true;
 }
 
 /*
diff --git a/drivers/net/usb/cdc_mbim.c b/drivers/net/usb/cdc_mbim.c
index a6b997cffd3b..7220cd620717 100644
--- a/drivers/net/usb/cdc_mbim.c
+++ b/drivers/net/usb/cdc_mbim.c
@@ -399,7 +399,7 @@ static struct sk_buff *cdc_mbim_process_dgram(struct usbnet *dev, u8 *buf, size_
 	memcpy(eth_hdr(skb)->h_dest, dev->net->dev_addr, ETH_ALEN);
 
 	/* add datagram */
-	memcpy(skb_put(skb, len), buf, len);
+	skb_put_data(skb, buf, len);
 
 	/* map MBIM session to VLAN */
 	if (tci)
@@ -643,6 +643,13 @@ static const struct usb_device_id mbim_devs[] = {
 	  .driver_info = (unsigned long)&cdc_mbim_info_ndp_to_end,
 	},
 
+	/* The HP lt4132 (03f0:a31d) is a rebranded Huawei ME906s-158,
+	 * therefore it too requires the above "NDP to end" quirk.
+	 */
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x03f0, 0xa31d, USB_CLASS_COMM, USB_CDC_SUBCLASS_MBIM, USB_CDC_PROTO_NONE),
+	  .driver_info = (unsigned long)&cdc_mbim_info_ndp_to_end,
+	},
+
 	/* Telit LE922A6 in MBIM composition */
 	{ USB_DEVICE_AND_INTERFACE_INFO(0x1bc7, 0x1041, USB_CLASS_COMM, USB_CDC_SUBCLASS_MBIM, USB_CDC_PROTO_NONE),
 	  .driver_info = (unsigned long)&cdc_mbim_info_avoid_altsetting_toggle,
diff --git a/drivers/net/usb/cdc_ncm.c b/drivers/net/usb/cdc_ncm.c
index b5cec1824a78..d103a1d4fb36 100644
--- a/drivers/net/usb/cdc_ncm.c
+++ b/drivers/net/usb/cdc_ncm.c
@@ -89,6 +89,8 @@ static const struct cdc_ncm_stats cdc_ncm_gstrings_stats[] = {
 	CDC_NCM_SIMPLE_STAT(rx_ntbs),
 };
 
+#define CDC_NCM_LOW_MEM_MAX_CNT 10
+
 static int cdc_ncm_get_sset_count(struct net_device __always_unused *netdev, int sset)
 {
 	switch (sset) {
@@ -1017,7 +1019,7 @@ static void cdc_ncm_align_tail(struct sk_buff *skb, size_t modulus, size_t remai
 	if (skb->len + align > max)
 		align = max - skb->len;
 	if (align && skb_tailroom(skb) >= align)
-		memset(skb_put(skb, align), 0, align);
+		skb_put_zero(skb, align);
 }
 
 /* return a pointer to a valid struct usb_cdc_ncm_ndp16 of type sign, possibly
@@ -1055,10 +1057,10 @@ static struct usb_cdc_ncm_ndp16 *cdc_ncm_ndp(struct cdc_ncm_ctx *ctx, struct sk_
 
 	/* align new NDP */
 	if (!(ctx->drvflags & CDC_NCM_FLAG_NDP_TO_END))
-		cdc_ncm_align_tail(skb, ctx->tx_ndp_modulus, 0, ctx->tx_max);
+		cdc_ncm_align_tail(skb, ctx->tx_ndp_modulus, 0, ctx->tx_curr_size);
 
 	/* verify that there is room for the NDP and the datagram (reserve) */
-	if ((ctx->tx_max - skb->len - reserve) < ctx->max_ndp_size)
+	if ((ctx->tx_curr_size - skb->len - reserve) < ctx->max_ndp_size)
 		return NULL;
 
 	/* link to it */
@@ -1069,7 +1071,7 @@ static struct usb_cdc_ncm_ndp16 *cdc_ncm_ndp(struct cdc_ncm_ctx *ctx, struct sk_
 
 	/* push a new empty NDP */
 	if (!(ctx->drvflags & CDC_NCM_FLAG_NDP_TO_END))
-		ndp16 = (struct usb_cdc_ncm_ndp16 *)memset(skb_put(skb, ctx->max_ndp_size), 0, ctx->max_ndp_size);
+		ndp16 = skb_put_zero(skb, ctx->max_ndp_size);
 	else
 		ndp16 = ctx->delayed_ndp16;
 
@@ -1111,16 +1113,44 @@ cdc_ncm_fill_tx_frame(struct usbnet *dev, struct sk_buff *skb, __le32 sign)
 
 	/* allocate a new OUT skb */
 	if (!skb_out) {
-		skb_out = alloc_skb(ctx->tx_max, GFP_ATOMIC);
+		if (ctx->tx_low_mem_val == 0) {
+			ctx->tx_curr_size = ctx->tx_max;
+			skb_out = alloc_skb(ctx->tx_curr_size, GFP_ATOMIC);
+			/* If the memory allocation fails we will wait longer
+			 * each time before attempting another full size
+			 * allocation again to not overload the system
+			 * further.
+			 */
+			if (skb_out == NULL) {
+				ctx->tx_low_mem_max_cnt = min(ctx->tx_low_mem_max_cnt + 1,
+							      (unsigned)CDC_NCM_LOW_MEM_MAX_CNT);
+				ctx->tx_low_mem_val = ctx->tx_low_mem_max_cnt;
+			}
+		}
 		if (skb_out == NULL) {
-			if (skb != NULL) {
-				dev_kfree_skb_any(skb);
-				dev->net->stats.tx_dropped++;
+			/* See if a very small allocation is possible.
+			 * We will send this packet immediately and hope
+			 * that there is more memory available later.
+			 */
+			if (skb)
+				ctx->tx_curr_size = max(skb->len,
+					(u32)USB_CDC_NCM_NTB_MIN_OUT_SIZE);
+			else
+				ctx->tx_curr_size = USB_CDC_NCM_NTB_MIN_OUT_SIZE;
+			skb_out = alloc_skb(ctx->tx_curr_size, GFP_ATOMIC);
+
+			/* No allocation possible so we will abort */
+			if (skb_out == NULL) {
+				if (skb != NULL) {
+					dev_kfree_skb_any(skb);
+					dev->net->stats.tx_dropped++;
+				}
+				goto exit_no_skb;
 			}
-			goto exit_no_skb;
+			ctx->tx_low_mem_val--;
 		}
 		/* fill out the initial 16-bit NTB header */
-		nth16 = (struct usb_cdc_ncm_nth16 *)memset(skb_put(skb_out, sizeof(struct usb_cdc_ncm_nth16)), 0, sizeof(struct usb_cdc_ncm_nth16));
+		nth16 = skb_put_zero(skb_out, sizeof(struct usb_cdc_ncm_nth16));
 		nth16->dwSignature = cpu_to_le32(USB_CDC_NCM_NTH16_SIGN);
 		nth16->wHeaderLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth16));
 		nth16->wSequence = cpu_to_le16(ctx->tx_seq++);
@@ -1148,10 +1178,10 @@ cdc_ncm_fill_tx_frame(struct usbnet *dev, struct sk_buff *skb, __le32 sign)
 		ndp16 = cdc_ncm_ndp(ctx, skb_out, sign, skb->len + ctx->tx_modulus + ctx->tx_remainder);
 
 		/* align beginning of next frame */
-		cdc_ncm_align_tail(skb_out,  ctx->tx_modulus, ctx->tx_remainder, ctx->tx_max);
+		cdc_ncm_align_tail(skb_out,  ctx->tx_modulus, ctx->tx_remainder, ctx->tx_curr_size);
 
 		/* check if we had enough room left for both NDP and frame */
-		if (!ndp16 || skb_out->len + skb->len + delayed_ndp_size > ctx->tx_max) {
+		if (!ndp16 || skb_out->len + skb->len + delayed_ndp_size > ctx->tx_curr_size) {
 			if (n == 0) {
 				/* won't fit, MTU problem? */
 				dev_kfree_skb_any(skb);
@@ -1180,7 +1210,7 @@ cdc_ncm_fill_tx_frame(struct usbnet *dev, struct sk_buff *skb, __le32 sign)
 		ndp16->dpe16[index].wDatagramLength = cpu_to_le16(skb->len);
 		ndp16->dpe16[index].wDatagramIndex = cpu_to_le16(skb_out->len);
 		ndp16->wLength = cpu_to_le16(ndplen + sizeof(struct usb_cdc_ncm_dpe16));
-		memcpy(skb_put(skb_out, skb->len), skb->data, skb->len);
+		skb_put_data(skb_out, skb->data, skb->len);
 		ctx->tx_curr_frame_payload += skb->len;	/* count real tx payload data */
 		dev_kfree_skb_any(skb);
 		skb = NULL;
@@ -1227,9 +1257,9 @@ cdc_ncm_fill_tx_frame(struct usbnet *dev, struct sk_buff *skb, __le32 sign)
 	/* If requested, put NDP at end of frame. */
 	if (ctx->drvflags & CDC_NCM_FLAG_NDP_TO_END) {
 		nth16 = (struct usb_cdc_ncm_nth16 *)skb_out->data;
-		cdc_ncm_align_tail(skb_out, ctx->tx_ndp_modulus, 0, ctx->tx_max);
+		cdc_ncm_align_tail(skb_out, ctx->tx_ndp_modulus, 0, ctx->tx_curr_size);
 		nth16->wNdpIndex = cpu_to_le16(skb_out->len);
-		memcpy(skb_put(skb_out, ctx->max_ndp_size), ctx->delayed_ndp16, ctx->max_ndp_size);
+		skb_put_data(skb_out, ctx->delayed_ndp16, ctx->max_ndp_size);
 
 		/* Zero out delayed NDP - signature checking will naturally fail. */
 		ndp16 = memset(ctx->delayed_ndp16, 0, ctx->max_ndp_size);
@@ -1246,11 +1276,11 @@ cdc_ncm_fill_tx_frame(struct usbnet *dev, struct sk_buff *skb, __le32 sign)
 	 */
 	if (!(dev->driver_info->flags & FLAG_SEND_ZLP) &&
 	    skb_out->len > ctx->min_tx_pkt) {
-		padding_count = ctx->tx_max - skb_out->len;
-		memset(skb_put(skb_out, padding_count), 0, padding_count);
-	} else if (skb_out->len < ctx->tx_max &&
+		padding_count = ctx->tx_curr_size - skb_out->len;
+		skb_put_zero(skb_out, padding_count);
+	} else if (skb_out->len < ctx->tx_curr_size &&
 		   (skb_out->len % dev->maxpacket) == 0) {
-		*skb_put(skb_out, 1) = 0;	/* force short packet */
+		skb_put_u8(skb_out, 0);	/* force short packet */
 	}
 
 	/* set final frame length */
@@ -1497,7 +1527,7 @@ next_ndp:
 			skb = netdev_alloc_skb_ip_align(dev->net,  len);
 			if (!skb)
 				goto error;
-			memcpy(skb_put(skb, len), skb_in->data + offset, len);
+			skb_put_data(skb, skb_in->data + offset, len);
 			usbnet_skb_return(dev, skb);
 			payload += len;	/* count payload bytes in this NTB */
 		}
diff --git a/drivers/net/usb/gl620a.c b/drivers/net/usb/gl620a.c
index 1cc24e6f23e2..ba1ce1006c4f 100644
--- a/drivers/net/usb/gl620a.c
+++ b/drivers/net/usb/gl620a.c
@@ -121,8 +121,7 @@ static int genelink_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 		if (gl_skb) {
 
 			// copy the packet data to the new skb
-			memcpy(skb_put(gl_skb, size),
-					packet->packet_data, size);
+			skb_put_data(gl_skb, packet->packet_data, size);
 			usbnet_skb_return(dev, gl_skb);
 		}
 
@@ -175,7 +174,7 @@ genelink_tx_fixup(struct usbnet *dev, struct sk_buff *skb, gfp_t flags)
 	}
 
 	// attach the packet count to the header
-	packet_count = (__le32 *) skb_push(skb, (4 + 4*1));
+	packet_count = skb_push(skb, (4 + 4 * 1));
 	packet_len = packet_count + 1;
 
 	*packet_count = cpu_to_le32(1);
diff --git a/drivers/net/usb/hso.c b/drivers/net/usb/hso.c
index 00067a0c51ca..d7a3379ea668 100644
--- a/drivers/net/usb/hso.c
+++ b/drivers/net/usb/hso.c
@@ -861,7 +861,6 @@ static void packetizeRx(struct hso_net *odev, unsigned char *ip_pkt,
 	unsigned short temp_bytes;
 	unsigned short buffer_offset = 0;
 	unsigned short frame_len;
-	unsigned char *tmp_rx_buf;
 
 	/* log if needed */
 	hso_dbg(0x1, "Rx %d bytes\n", count);
@@ -911,11 +910,9 @@ static void packetizeRx(struct hso_net *odev, unsigned char *ip_pkt,
 
 				/* Copy what we got so far. make room for iphdr
 				 * after tail. */
-				tmp_rx_buf =
-				    skb_put(odev->skb_rx_buf,
-					    sizeof(struct iphdr));
-				memcpy(tmp_rx_buf, (char *)&(odev->rx_ip_hdr),
-				       sizeof(struct iphdr));
+				skb_put_data(odev->skb_rx_buf,
+					     (char *)&(odev->rx_ip_hdr),
+					     sizeof(struct iphdr));
 
 				/* ETH_HLEN */
 				odev->rx_buf_size = sizeof(struct iphdr);
@@ -934,8 +931,9 @@ static void packetizeRx(struct hso_net *odev, unsigned char *ip_pkt,
 			/* Copy the rest of the bytes that are left in the
 			 * buffer into the waiting sk_buf. */
 			/* Make room for temp_bytes after tail. */
-			tmp_rx_buf = skb_put(odev->skb_rx_buf, temp_bytes);
-			memcpy(tmp_rx_buf, ip_pkt + buffer_offset, temp_bytes);
+			skb_put_data(odev->skb_rx_buf,
+				     ip_pkt + buffer_offset,
+				     temp_bytes);
 
 			odev->rx_buf_missing -= temp_bytes;
 			count -= temp_bytes;
diff --git a/drivers/net/usb/int51x1.c b/drivers/net/usb/int51x1.c
index 5a43b77a6b9c..ae2b2563460b 100644
--- a/drivers/net/usb/int51x1.c
+++ b/drivers/net/usb/int51x1.c
@@ -106,11 +106,11 @@ static struct sk_buff *int51x1_tx_fixup(struct usbnet *dev,
 	pack_len += need_tail;
 	pack_len &= 0x07ff;
 
-	len = (__le16 *) __skb_push(skb, INT51X1_HEADER_SIZE);
+	len = __skb_push(skb, INT51X1_HEADER_SIZE);
 	*len = cpu_to_le16(pack_len);
 
 	if(need_tail)
-		memset(__skb_put(skb, need_tail), 0, need_tail);
+		__skb_put_zero(skb, need_tail);
 
 	return skb;
 }
diff --git a/drivers/net/usb/ipheth.c b/drivers/net/usb/ipheth.c
index 76465b117b72..0f213ea22c75 100644
--- a/drivers/net/usb/ipheth.c
+++ b/drivers/net/usb/ipheth.c
@@ -253,7 +253,7 @@ static void ipheth_rcvbulk_callback(struct urb *urb)
 		return;
 	}
 
-	memcpy(skb_put(skb, len), buf, len);
+	skb_put_data(skb, buf, len);
 	skb->dev = dev->net;
 	skb->protocol = eth_type_trans(skb, dev->net);
 
diff --git a/drivers/net/usb/kalmia.c b/drivers/net/usb/kalmia.c
index 8aefb282c862..ce0b0b4e3a57 100644
--- a/drivers/net/usb/kalmia.c
+++ b/drivers/net/usb/kalmia.c
@@ -217,7 +217,7 @@ done:
 	remainder = skb->len % KALMIA_ALIGN_SIZE;
 	if (remainder > 0) {
 		padlen = KALMIA_ALIGN_SIZE - remainder;
-		memset(skb_put(skb, padlen), 0, padlen);
+		skb_put_zero(skb, padlen);
 	}
 
 	netdev_dbg(dev->net,
diff --git a/drivers/net/usb/kaweth.c b/drivers/net/usb/kaweth.c
index 37fb621fde86..92e4fd29ae44 100644
--- a/drivers/net/usb/kaweth.c
+++ b/drivers/net/usb/kaweth.c
@@ -809,7 +809,7 @@ static netdev_tx_t kaweth_start_xmit(struct sk_buff *skb,
 		return NETDEV_TX_OK;
 	}
 
-	private_header = (__le16 *)__skb_push(skb, 2);
+	private_header = __skb_push(skb, 2);
 	*private_header = cpu_to_le16(skb->len-2);
 	kaweth->tx_skb = skb;
 
diff --git a/drivers/net/usb/lan78xx.c b/drivers/net/usb/lan78xx.c
index 9eff97a650ae..5833f7e2a127 100644
--- a/drivers/net/usb/lan78xx.c
+++ b/drivers/net/usb/lan78xx.c
@@ -1490,7 +1490,7 @@ static int lan78xx_get_link_ksettings(struct net_device *net,
 	if (ret < 0)
 		return ret;
 
-	ret = phy_ethtool_ksettings_get(phydev, cmd);
+	phy_ethtool_ksettings_get(phydev, cmd);
 
 	usb_autopm_put_interface(dev->intf);
 
diff --git a/drivers/net/usb/lg-vl600.c b/drivers/net/usb/lg-vl600.c
index 5714107533bb..dbabd7ca5268 100644
--- a/drivers/net/usb/lg-vl600.c
+++ b/drivers/net/usb/lg-vl600.c
@@ -135,7 +135,7 @@ static int vl600_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 		}
 
 		buf = s->current_rx_buf;
-		memcpy(skb_put(buf, skb->len), skb->data, skb->len);
+		skb_put_data(buf, skb->data, skb->len);
 	} else if (skb->len < 4) {
 		netif_err(dev, ifup, dev->net, "Frame too short\n");
 		dev->net->stats.rx_length_errors++;
@@ -304,7 +304,7 @@ encapsulate:
 	memset(&packet->dummy, 0, sizeof(packet->dummy));
 	packet->len = cpu_to_le32(orig_len);
 
-	frame = (struct vl600_frame_hdr *) skb_push(skb, sizeof(*frame));
+	frame = skb_push(skb, sizeof(*frame));
 	memset(frame, 0, sizeof(*frame));
 	frame->len = cpu_to_le32(full_len);
 	frame->serial = cpu_to_le32(serial++);
diff --git a/drivers/net/usb/net1080.c b/drivers/net/usb/net1080.c
index 4cbdb1307f3e..18a13aa5fcbb 100644
--- a/drivers/net/usb/net1080.c
+++ b/drivers/net/usb/net1080.c
@@ -264,17 +264,9 @@ static inline void nc_dump_status(struct usbnet *dev, u16 status)
  * TTL register
  */
 
-#define	TTL_THIS(ttl)	(0x00ff & ttl)
 #define	TTL_OTHER(ttl)	(0x00ff & (ttl >> 8))
 #define MK_TTL(this,other)	((u16)(((other)<<8)|(0x00ff&(this))))
 
-static inline void nc_dump_ttl(struct usbnet *dev, u16 ttl)
-{
-	netif_dbg(dev, link, dev->net, "net1080 %s-%s ttl 0x%x this = %d, other = %d\n",
-		  dev->udev->bus->bus_name, dev->udev->devpath,
-		  ttl, TTL_THIS(ttl), TTL_OTHER(ttl));
-}
-
 /*-------------------------------------------------------------------------*/
 
 static int net1080_reset(struct usbnet *dev)
@@ -308,7 +300,6 @@ static int net1080_reset(struct usbnet *dev)
 		goto done;
 	}
 	ttl = vp;
-	// nc_dump_ttl(dev, ttl);
 
 	nc_register_write(dev, REG_TTL,
 			MK_TTL(NC_READ_TTL_MS, TTL_OTHER(ttl)) );
@@ -475,15 +466,15 @@ net1080_tx_fixup(struct usbnet *dev, struct sk_buff *skb, gfp_t flags)
 
 encapsulate:
 	/* header first */
-	header = (struct nc_header *) skb_push(skb, sizeof *header);
+	header = skb_push(skb, sizeof *header);
 	header->hdr_len = cpu_to_le16(sizeof (*header));
 	header->packet_len = cpu_to_le16(len);
 	header->packet_id = cpu_to_le16((u16)dev->xid++);
 
 	/* maybe pad; then trailer */
 	if (!((skb->len + sizeof *trailer) & 0x01))
-		*skb_put(skb, 1) = PAD_BYTE;
-	trailer = (struct nc_trailer *) skb_put(skb, sizeof *trailer);
+		skb_put_u8(skb, PAD_BYTE);
+	trailer = skb_put(skb, sizeof *trailer);
 	put_unaligned(header->packet_id, &trailer->packet_id);
 #if 0
 	netdev_dbg(dev->net, "frame >tx h %d p %d id %d\n",
diff --git a/drivers/net/usb/qmi_wwan.c b/drivers/net/usb/qmi_wwan.c
index 8f923a147fa9..5894e3c9468f 100644
--- a/drivers/net/usb/qmi_wwan.c
+++ b/drivers/net/usb/qmi_wwan.c
@@ -101,7 +101,7 @@ static netdev_tx_t qmimux_start_xmit(struct sk_buff *skb, struct net_device *dev
 	unsigned int len = skb->len;
 	struct qmimux_hdr *hdr;
 
-	hdr = (struct qmimux_hdr *)skb_push(skb, sizeof(struct qmimux_hdr));
+	hdr = skb_push(skb, sizeof(struct qmimux_hdr));
 	hdr->pad = 0;
 	hdr->mux_id = priv->mux_id;
 	hdr->pkt_len = cpu_to_be16(len);
@@ -123,7 +123,7 @@ static void qmimux_setup(struct net_device *dev)
 	dev->addr_len        = 0;
 	dev->flags           = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
 	dev->netdev_ops      = &qmimux_netdev_ops;
-	dev->destructor      = free_netdev;
+	dev->needs_free_netdev = true;
 }
 
 static struct net_device *qmimux_find_dev(struct usbnet *dev, u8 mux_id)
@@ -188,7 +188,7 @@ static int qmimux_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 			goto skip;
 		}
 
-		memcpy(skb_put(skbn, len), skb->data + offset, len);
+		skb_put_data(skbn, skb->data + offset, len);
 		if (netif_rx(skbn) != NET_RX_SUCCESS)
 			return 0;
 
@@ -1192,6 +1192,8 @@ static const struct usb_device_id products[] = {
 	{QMI_FIXED_INTF(0x1199, 0x9056, 8)},	/* Sierra Wireless Modem */
 	{QMI_FIXED_INTF(0x1199, 0x9057, 8)},
 	{QMI_FIXED_INTF(0x1199, 0x9061, 8)},	/* Sierra Wireless Modem */
+	{QMI_FIXED_INTF(0x1199, 0x9063, 8)},	/* Sierra Wireless EM7305 */
+	{QMI_FIXED_INTF(0x1199, 0x9063, 10)},	/* Sierra Wireless EM7305 */
 	{QMI_FIXED_INTF(0x1199, 0x9071, 8)},	/* Sierra Wireless MC74xx */
 	{QMI_FIXED_INTF(0x1199, 0x9071, 10)},	/* Sierra Wireless MC74xx */
 	{QMI_FIXED_INTF(0x1199, 0x9079, 8)},	/* Sierra Wireless EM74xx */
@@ -1206,6 +1208,8 @@ static const struct usb_device_id products[] = {
 	{QMI_FIXED_INTF(0x1bc7, 0x1100, 3)},	/* Telit ME910 */
 	{QMI_FIXED_INTF(0x1bc7, 0x1200, 5)},	/* Telit LE920 */
 	{QMI_QUIRK_SET_DTR(0x1bc7, 0x1201, 2)},	/* Telit LE920, LE920A4 */
+	{QMI_FIXED_INTF(0x1c9e, 0x9801, 3)},	/* Telewell TW-3G HSPA+ */
+	{QMI_FIXED_INTF(0x1c9e, 0x9803, 4)},	/* Telewell TW-3G HSPA+ */
 	{QMI_FIXED_INTF(0x1c9e, 0x9b01, 3)},	/* XS Stick W100-2 from 4G Systems */
 	{QMI_FIXED_INTF(0x0b3c, 0xc000, 4)},	/* Olivetti Olicard 100 */
 	{QMI_FIXED_INTF(0x0b3c, 0xc001, 4)},	/* Olivetti Olicard 120 */
diff --git a/drivers/net/usb/r8152.c b/drivers/net/usb/r8152.c
index ddc62cb69be8..6cfffeff6108 100644
--- a/drivers/net/usb/r8152.c
+++ b/drivers/net/usb/r8152.c
@@ -29,7 +29,7 @@
 #include <linux/acpi.h>
 
 /* Information for net-next */
-#define NETNEXT_VERSION		"08"
+#define NETNEXT_VERSION		"09"
 
 /* Information for net */
 #define NET_VERSION		"9"
@@ -51,11 +51,14 @@
 #define PLA_FMC			0xc0b4
 #define PLA_CFG_WOL		0xc0b6
 #define PLA_TEREDO_CFG		0xc0bc
+#define PLA_TEREDO_WAKE_BASE	0xc0c4
 #define PLA_MAR			0xcd00
 #define PLA_BACKUP		0xd000
 #define PAL_BDC_CR		0xd1a0
 #define PLA_TEREDO_TIMER	0xd2cc
 #define PLA_REALWOW_TIMER	0xd2e8
+#define PLA_EFUSE_DATA		0xdd00
+#define PLA_EFUSE_CMD		0xdd02
 #define PLA_LEDSEL		0xdd90
 #define PLA_LED_FEATURE		0xdd92
 #define PLA_PHYAR		0xde00
@@ -105,7 +108,9 @@
 #define USB_CSR_DUMMY2		0xb466
 #define USB_DEV_STAT		0xb808
 #define USB_CONNECT_TIMER	0xcbf8
+#define USB_MSC_TIMER		0xcbfc
 #define USB_BURST_SIZE		0xcfc0
+#define USB_LPM_CONFIG		0xcfd8
 #define USB_USB_CTRL		0xd406
 #define USB_PHY_CTRL		0xd408
 #define USB_TX_AGG		0xd40a
@@ -113,15 +118,20 @@
 #define USB_USB_TIMER		0xd428
 #define USB_RX_EARLY_TIMEOUT	0xd42c
 #define USB_RX_EARLY_SIZE	0xd42e
-#define USB_PM_CTRL_STATUS	0xd432
+#define USB_PM_CTRL_STATUS	0xd432	/* RTL8153A */
+#define USB_RX_EXTRA_AGGR_TMR	0xd432	/* RTL8153B */
 #define USB_TX_DMA		0xd434
+#define USB_UPT_RXDMA_OWN	0xd437
 #define USB_TOLERANCE		0xd490
 #define USB_LPM_CTRL		0xd41a
 #define USB_BMU_RESET		0xd4b0
+#define USB_U1U2_TIMER		0xd4da
 #define USB_UPS_CTRL		0xd800
-#define USB_MISC_0		0xd81a
 #define USB_POWER_CUT		0xd80a
+#define USB_MISC_0		0xd81a
 #define USB_AFE_CTRL2		0xd824
+#define USB_UPS_CFG		0xd842
+#define USB_UPS_FLAGS		0xd848
 #define USB_WDT11_CTRL		0xe43c
 #define USB_BP_BA		0xfc26
 #define USB_BP_0		0xfc28
@@ -133,6 +143,15 @@
 #define USB_BP_6		0xfc34
 #define USB_BP_7		0xfc36
 #define USB_BP_EN		0xfc38
+#define USB_BP_8		0xfc38
+#define USB_BP_9		0xfc3a
+#define USB_BP_10		0xfc3c
+#define USB_BP_11		0xfc3e
+#define USB_BP_12		0xfc40
+#define USB_BP_13		0xfc42
+#define USB_BP_14		0xfc44
+#define USB_BP_15		0xfc46
+#define USB_BP2_EN		0xfc48
 
 /* OCP Registers */
 #define OCP_ALDPS_CONFIG	0x2010
@@ -143,6 +162,7 @@
 #define OCP_EEE_AR		0xa41a
 #define OCP_EEE_DATA		0xa41c
 #define OCP_PHY_STATUS		0xa420
+#define OCP_NCTL_CFG		0xa42c
 #define OCP_POWER_CFG		0xa430
 #define OCP_EEE_CFG		0xa432
 #define OCP_SRAM_ADDR		0xa436
@@ -152,9 +172,14 @@
 #define OCP_EEE_ADV		0xa5d0
 #define OCP_EEE_LPABLE		0xa5d2
 #define OCP_PHY_STATE		0xa708		/* nway state for 8153 */
+#define OCP_PHY_PATCH_STAT	0xb800
+#define OCP_PHY_PATCH_CMD	0xb820
+#define OCP_ADC_IOFFSET		0xbcfc
 #define OCP_ADC_CFG		0xbc06
+#define OCP_SYSCLK_CFG		0xc416
 
 /* SRAM Register */
+#define SRAM_GREEN_CFG		0x8011
 #define SRAM_LPF_CFG		0x8012
 #define SRAM_10M_AMP1		0x8080
 #define SRAM_10M_AMP2		0x8082
@@ -252,6 +277,10 @@
 /* PAL_BDC_CR */
 #define ALDPS_PROXY_MODE	0x0001
 
+/* PLA_EFUSE_CMD */
+#define EFUSE_READ_CMD		BIT(15)
+#define EFUSE_DATA_BIT16	BIT(7)
+
 /* PLA_CONFIG34 */
 #define LINK_ON_WAKE_EN		0x0010
 #define LINK_OFF_WAKE_EN	0x0008
@@ -277,6 +306,7 @@
 
 /* PLA_MAC_PWR_CTRL2 */
 #define EEE_SPDWN_RATIO		0x8007
+#define MAC_CLK_SPDWN_EN	BIT(15)
 
 /* PLA_MAC_PWR_CTRL3 */
 #define PKT_AVAIL_SPDWN_EN	0x0100
@@ -328,6 +358,9 @@
 #define STAT_SPEED_HIGH		0x0000
 #define STAT_SPEED_FULL		0x0002
 
+/* USB_LPM_CONFIG */
+#define LPM_U1U2_EN		BIT(0)
+
 /* USB_TX_AGG */
 #define TX_AGG_MAX_THRESHOLD	0x03
 
@@ -335,6 +368,7 @@
 #define RX_THR_SUPPER		0x0c350180
 #define RX_THR_HIGH		0x7a120180
 #define RX_THR_SLOW		0xffff0180
+#define RX_THR_B		0x00010001
 
 /* USB_TX_DMA */
 #define TEST_MODE_DISABLE	0x00000001
@@ -344,6 +378,10 @@
 #define BMU_RESET_EP_IN		0x01
 #define BMU_RESET_EP_OUT	0x02
 
+/* USB_UPT_RXDMA_OWN */
+#define OWN_UPDATE		BIT(0)
+#define OWN_CLEAR		BIT(1)
+
 /* USB_UPS_CTRL */
 #define POWER_CUT		0x0100
 
@@ -360,6 +398,8 @@
 /* USB_POWER_CUT */
 #define PWR_EN			0x0001
 #define PHASE2_EN		0x0008
+#define UPS_EN			BIT(4)
+#define USP_PREWAKE		BIT(5)
 
 /* USB_MISC_0 */
 #define PCUT_STATUS		0x0001
@@ -386,6 +426,37 @@
 #define SEN_VAL_NORMAL		0xa000
 #define SEL_RXIDLE		0x0100
 
+/* USB_UPS_CFG */
+#define SAW_CNT_1MS_MASK	0x0fff
+
+/* USB_UPS_FLAGS */
+#define UPS_FLAGS_R_TUNE		BIT(0)
+#define UPS_FLAGS_EN_10M_CKDIV		BIT(1)
+#define UPS_FLAGS_250M_CKDIV		BIT(2)
+#define UPS_FLAGS_EN_ALDPS		BIT(3)
+#define UPS_FLAGS_CTAP_SHORT_DIS	BIT(4)
+#define UPS_FLAGS_SPEED_MASK		(0xf << 16)
+#define ups_flags_speed(x)		((x) << 16)
+#define UPS_FLAGS_EN_EEE		BIT(20)
+#define UPS_FLAGS_EN_500M_EEE		BIT(21)
+#define UPS_FLAGS_EN_EEE_CKDIV		BIT(22)
+#define UPS_FLAGS_EEE_PLLOFF_GIGA	BIT(24)
+#define UPS_FLAGS_EEE_CMOD_LV_EN	BIT(25)
+#define UPS_FLAGS_EN_GREEN		BIT(26)
+#define UPS_FLAGS_EN_FLOW_CTR		BIT(27)
+
+enum spd_duplex {
+	NWAY_10M_HALF = 1,
+	NWAY_10M_FULL,
+	NWAY_100M_HALF,
+	NWAY_100M_FULL,
+	NWAY_1000M_FULL,
+	FORCE_10M_HALF,
+	FORCE_10M_FULL,
+	FORCE_100M_HALF,
+	FORCE_100M_FULL,
+};
+
 /* OCP_ALDPS_CONFIG */
 #define ENPWRSAVE		0x8000
 #define ENPDNPS			0x0200
@@ -394,9 +465,13 @@
 
 /* OCP_PHY_STATUS */
 #define PHY_STAT_MASK		0x0007
+#define PHY_STAT_EXT_INIT	2
 #define PHY_STAT_LAN_ON		3
 #define PHY_STAT_PWRDN		5
 
+/* OCP_NCTL_CFG */
+#define PGA_RETURN_EN		BIT(1)
+
 /* OCP_POWER_CFG */
 #define EEE_CLKDIV_EN		0x8000
 #define EN_ALDPS		0x0004
@@ -438,17 +513,34 @@
 #define EEE10_EN		0x0010
 
 /* OCP_DOWN_SPEED */
+#define EN_EEE_CMODE		BIT(14)
+#define EN_EEE_1000		BIT(13)
+#define EN_EEE_100		BIT(12)
+#define EN_10M_CLKDIV		BIT(11)
 #define EN_10M_BGOFF		0x0080
 
 /* OCP_PHY_STATE */
 #define TXDIS_STATE		0x01
 #define ABD_STATE		0x02
 
+/* OCP_PHY_PATCH_STAT */
+#define PATCH_READY		BIT(6)
+
+/* OCP_PHY_PATCH_CMD */
+#define PATCH_REQUEST		BIT(4)
+
 /* OCP_ADC_CFG */
 #define CKADSEL_L		0x0100
 #define ADC_EN			0x0080
 #define EN_EMI_L		0x0040
 
+/* OCP_SYSCLK_CFG */
+#define clk_div_expo(x)		(min(x, 5) << 8)
+
+/* SRAM_GREEN_CFG */
+#define GREEN_ETH_EN		BIT(15)
+#define R_TUNE_EN		BIT(11)
+
 /* SRAM_LPF_CFG */
 #define LPF_AUTO_TUNE		0x8000
 
@@ -477,7 +569,6 @@ enum rtl_register_content {
 #define RTL8152_MAX_TX		4
 #define RTL8152_MAX_RX		10
 #define INTBUFSIZE		2
-#define CRC_SIZE		4
 #define TX_ALIGN		4
 #define RX_ALIGN		8
 
@@ -496,12 +587,13 @@ enum rtl_register_content {
 #define BYTE_EN_END_MASK	0xf0
 
 #define RTL8153_MAX_PACKET	9216 /* 9K */
-#define RTL8153_MAX_MTU		(RTL8153_MAX_PACKET - VLAN_ETH_HLEN - VLAN_HLEN)
-#define RTL8152_RMS		(VLAN_ETH_FRAME_LEN + VLAN_HLEN)
+#define RTL8153_MAX_MTU		(RTL8153_MAX_PACKET - VLAN_ETH_HLEN - \
+				 ETH_FCS_LEN)
+#define RTL8152_RMS		(VLAN_ETH_FRAME_LEN + ETH_FCS_LEN)
 #define RTL8153_RMS		RTL8153_MAX_PACKET
 #define RTL8152_TX_TIMEOUT	(5 * HZ)
 #define RTL8152_NAPI_WEIGHT	64
-#define rx_reserved_size(x)	((x) + VLAN_ETH_HLEN + CRC_SIZE + \
+#define rx_reserved_size(x)	((x) + VLAN_ETH_HLEN + ETH_FCS_LEN + \
 				 sizeof(struct rx_desc) + RX_ALIGN)
 
 /* rtl8152 flags */
@@ -513,6 +605,7 @@ enum rtl8152_flags {
 	SELECTIVE_SUSPEND,
 	PHY_RESET,
 	SCHEDULE_NAPI,
+	GREEN_ETHERNET,
 };
 
 /* Define these values to match your device */
@@ -658,6 +751,9 @@ enum rtl_version {
 	RTL_VER_04,
 	RTL_VER_05,
 	RTL_VER_06,
+	RTL_VER_07,
+	RTL_VER_08,
+	RTL_VER_09,
 	RTL_VER_MAX
 };
 
@@ -674,7 +770,7 @@ static const int multicast_filter_limit = 32;
 static unsigned int agg_buf_sz = 16384;
 
 #define RTL_LIMITED_TSO_SIZE	(agg_buf_sz - sizeof(struct tx_desc) - \
-				 VLAN_ETH_HLEN - VLAN_HLEN)
+				 VLAN_ETH_HLEN - ETH_FCS_LEN)
 
 static
 int get_registers(struct r8152 *tp, u16 value, u16 index, u16 size, void *data)
@@ -841,12 +937,6 @@ int pla_ocp_write(struct r8152 *tp, u16 index, u16 byteen, u16 size, void *data)
 }
 
 static inline
-int usb_ocp_read(struct r8152 *tp, u16 index, u16 size, void *data)
-{
-	return generic_ocp_read(tp, index, size, data, MCU_TYPE_USB);
-}
-
-static inline
 int usb_ocp_write(struct r8152 *tp, u16 index, u16 byteen, u16 size, void *data)
 {
 	return generic_ocp_write(tp, index, byteen, size, data, MCU_TYPE_USB);
@@ -872,11 +962,13 @@ static u16 ocp_read_word(struct r8152 *tp, u16 type, u16 index)
 {
 	u32 data;
 	__le32 tmp;
+	u16 byen = BYTE_EN_WORD;
 	u8 shift = index & 2;
 
 	index &= ~3;
+	byen <<= shift;
 
-	generic_ocp_read(tp, index, sizeof(tmp), &tmp, type);
+	generic_ocp_read(tp, index, sizeof(tmp), &tmp, type | byen);
 
 	data = __le32_to_cpu(tmp);
 	data >>= (shift * 8);
@@ -988,6 +1080,12 @@ static void sram_write(struct r8152 *tp, u16 addr, u16 data)
 	ocp_reg_write(tp, OCP_SRAM_DATA, data);
 }
 
+static u16 sram_read(struct r8152 *tp, u16 addr)
+{
+	ocp_reg_write(tp, OCP_SRAM_ADDR, addr);
+	return ocp_reg_read(tp, OCP_SRAM_DATA);
+}
+
 static int read_mii_word(struct net_device *netdev, int phy_id, int reg)
 {
 	struct r8152 *tp = netdev_priv(netdev);
@@ -1818,6 +1916,10 @@ static int rx_bottom(struct r8152 *tp, int budget)
 			unsigned int pkt_len;
 			struct sk_buff *skb;
 
+			/* limite the skb numbers for rx_queue */
+			if (unlikely(skb_queue_len(&tp->rx_queue) >= 1000))
+				break;
+
 			pkt_len = le32_to_cpu(rx_desc->opts1) & RX_LEN_MASK;
 			if (pkt_len < ETH_ZLEN)
 				break;
@@ -1826,7 +1928,7 @@ static int rx_bottom(struct r8152 *tp, int budget)
 			if (urb->actual_length < len_used)
 				break;
 
-			pkt_len -= CRC_SIZE;
+			pkt_len -= ETH_FCS_LEN;
 			rx_data += sizeof(struct rx_desc);
 
 			skb = napi_alloc_skb(napi, pkt_len);
@@ -1850,7 +1952,7 @@ static int rx_bottom(struct r8152 *tp, int budget)
 			}
 
 find_next_rx:
-			rx_data = rx_agg_align(rx_data + pkt_len + CRC_SIZE);
+			rx_data = rx_agg_align(rx_data + pkt_len + ETH_FCS_LEN);
 			rx_desc = (struct rx_desc *)rx_data;
 			len_used = (int)(rx_data - (u8 *)agg->head);
 			len_used += sizeof(struct rx_desc);
@@ -1939,7 +2041,8 @@ static int r8152_poll(struct napi_struct *napi, int budget)
 	bottom_half(tp);
 
 	if (work_done < budget) {
-		napi_complete(napi);
+		if (!napi_complete_done(napi, work_done))
+			goto out;
 		if (!list_empty(&tp->rx_done))
 			napi_schedule(napi);
 		else if (!skb_queue_empty(&tp->tx_queue) &&
@@ -1947,6 +2050,7 @@ static int r8152_poll(struct napi_struct *napi, int budget)
 			napi_schedule(napi);
 	}
 
+out:
 	return work_done;
 }
 
@@ -2138,7 +2242,7 @@ static void set_tx_qlen(struct r8152 *tp)
 {
 	struct net_device *netdev = tp->netdev;
 
-	tp->tx_qlen = agg_buf_sz / (netdev->mtu + VLAN_ETH_HLEN + VLAN_HLEN +
+	tp->tx_qlen = agg_buf_sz / (netdev->mtu + VLAN_ETH_HLEN + ETH_FCS_LEN +
 				    sizeof(struct tx_desc));
 }
 
@@ -2249,18 +2353,64 @@ static int rtl8152_enable(struct r8152 *tp)
 	return rtl_enable(tp);
 }
 
+static inline void r8153b_rx_agg_chg_indicate(struct r8152 *tp)
+{
+	ocp_write_byte(tp, MCU_TYPE_USB, USB_UPT_RXDMA_OWN,
+		       OWN_UPDATE | OWN_CLEAR);
+}
+
 static void r8153_set_rx_early_timeout(struct r8152 *tp)
 {
 	u32 ocp_data = tp->coalesce / 8;
 
-	ocp_write_word(tp, MCU_TYPE_USB, USB_RX_EARLY_TIMEOUT, ocp_data);
+	switch (tp->version) {
+	case RTL_VER_03:
+	case RTL_VER_04:
+	case RTL_VER_05:
+	case RTL_VER_06:
+		ocp_write_word(tp, MCU_TYPE_USB, USB_RX_EARLY_TIMEOUT,
+			       ocp_data);
+		break;
+
+	case RTL_VER_08:
+	case RTL_VER_09:
+		/* The RTL8153B uses USB_RX_EXTRA_AGGR_TMR for rx timeout
+		 * primarily. For USB_RX_EARLY_TIMEOUT, we fix it to 128ns.
+		 */
+		ocp_write_word(tp, MCU_TYPE_USB, USB_RX_EARLY_TIMEOUT,
+			       128 / 8);
+		ocp_write_word(tp, MCU_TYPE_USB, USB_RX_EXTRA_AGGR_TMR,
+			       ocp_data);
+		r8153b_rx_agg_chg_indicate(tp);
+		break;
+
+	default:
+		break;
+	}
 }
 
 static void r8153_set_rx_early_size(struct r8152 *tp)
 {
-	u32 ocp_data = (agg_buf_sz - rx_reserved_size(tp->netdev->mtu)) / 4;
+	u32 ocp_data = agg_buf_sz - rx_reserved_size(tp->netdev->mtu);
 
-	ocp_write_word(tp, MCU_TYPE_USB, USB_RX_EARLY_SIZE, ocp_data);
+	switch (tp->version) {
+	case RTL_VER_03:
+	case RTL_VER_04:
+	case RTL_VER_05:
+	case RTL_VER_06:
+		ocp_write_word(tp, MCU_TYPE_USB, USB_RX_EARLY_SIZE,
+			       ocp_data / 4);
+		break;
+	case RTL_VER_08:
+	case RTL_VER_09:
+		ocp_write_word(tp, MCU_TYPE_USB, USB_RX_EARLY_SIZE,
+			       ocp_data / 8);
+		r8153b_rx_agg_chg_indicate(tp);
+		break;
+	default:
+		WARN_ON_ONCE(1);
+		break;
+	}
 }
 
 static int rtl8153_enable(struct r8152 *tp)
@@ -2268,7 +2418,6 @@ static int rtl8153_enable(struct r8152 *tp)
 	if (test_bit(RTL8152_UNPLUG, &tp->flags))
 		return -ENODEV;
 
-	usb_disable_lpm(tp->udev);
 	set_tx_qlen(tp);
 	rtl_set_eee_plus(tp);
 	r8153_set_rx_early_timeout(tp);
@@ -2434,6 +2583,29 @@ static void __rtl_set_wol(struct r8152 *tp, u32 wolopts)
 		device_set_wakeup_enable(&tp->udev->dev, false);
 }
 
+static void r8153_mac_clk_spd(struct r8152 *tp, bool enable)
+{
+	/* MAC clock speed down */
+	if (enable) {
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL,
+			       ALDPS_SPDWN_RATIO);
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL2,
+			       EEE_SPDWN_RATIO);
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL3,
+			       PKT_AVAIL_SPDWN_EN | SUSPEND_SPDWN_EN |
+			       U1U2_SPDWN_EN | L1_SPDWN_EN);
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL4,
+			       PWRSAVE_SPDWN_EN | RXDV_SPDWN_EN | TX10MIDLE_EN |
+			       TP100_SPDWN_EN | TP500_SPDWN_EN | EEE_SPDWN_EN |
+			       TP1000_SPDWN_EN);
+	} else {
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL, 0);
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL2, 0);
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL3, 0);
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL4, 0);
+	}
+}
+
 static void r8153_u1u2en(struct r8152 *tp, bool enable)
 {
 	u8 u1u2[8];
@@ -2446,18 +2618,133 @@ static void r8153_u1u2en(struct r8152 *tp, bool enable)
 	usb_ocp_write(tp, USB_TOLERANCE, BYTE_EN_SIX_BYTES, sizeof(u1u2), u1u2);
 }
 
+static void r8153b_u1u2en(struct r8152 *tp, bool enable)
+{
+	u32 ocp_data;
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_LPM_CONFIG);
+	if (enable)
+		ocp_data |= LPM_U1U2_EN;
+	else
+		ocp_data &= ~LPM_U1U2_EN;
+
+	ocp_write_word(tp, MCU_TYPE_USB, USB_LPM_CONFIG, ocp_data);
+}
+
 static void r8153_u2p3en(struct r8152 *tp, bool enable)
 {
 	u32 ocp_data;
 
 	ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_U2P3_CTRL);
-	if (enable && tp->version != RTL_VER_03 && tp->version != RTL_VER_04)
+	if (enable)
 		ocp_data |= U2P3_ENABLE;
 	else
 		ocp_data &= ~U2P3_ENABLE;
 	ocp_write_word(tp, MCU_TYPE_USB, USB_U2P3_CTRL, ocp_data);
 }
 
+static void r8153b_ups_flags_w1w0(struct r8152 *tp, u32 set, u32 clear)
+{
+	u32 ocp_data;
+
+	ocp_data = ocp_read_dword(tp, MCU_TYPE_USB, USB_UPS_FLAGS);
+	ocp_data &= ~clear;
+	ocp_data |= set;
+	ocp_write_dword(tp, MCU_TYPE_USB, USB_UPS_FLAGS, ocp_data);
+}
+
+static void r8153b_green_en(struct r8152 *tp, bool enable)
+{
+	u16 data;
+
+	if (enable) {
+		sram_write(tp, 0x8045, 0);	/* 10M abiq&ldvbias */
+		sram_write(tp, 0x804d, 0x1222);	/* 100M short abiq&ldvbias */
+		sram_write(tp, 0x805d, 0x0022);	/* 1000M short abiq&ldvbias */
+	} else {
+		sram_write(tp, 0x8045, 0x2444);	/* 10M abiq&ldvbias */
+		sram_write(tp, 0x804d, 0x2444);	/* 100M short abiq&ldvbias */
+		sram_write(tp, 0x805d, 0x2444);	/* 1000M short abiq&ldvbias */
+	}
+
+	data = sram_read(tp, SRAM_GREEN_CFG);
+	data |= GREEN_ETH_EN;
+	sram_write(tp, SRAM_GREEN_CFG, data);
+
+	r8153b_ups_flags_w1w0(tp, UPS_FLAGS_EN_GREEN, 0);
+}
+
+static u16 r8153_phy_status(struct r8152 *tp, u16 desired)
+{
+	u16 data;
+	int i;
+
+	for (i = 0; i < 500; i++) {
+		data = ocp_reg_read(tp, OCP_PHY_STATUS);
+		data &= PHY_STAT_MASK;
+		if (desired) {
+			if (data == desired)
+				break;
+		} else if (data == PHY_STAT_LAN_ON || data == PHY_STAT_PWRDN ||
+			   data == PHY_STAT_EXT_INIT) {
+			break;
+		}
+
+		msleep(20);
+	}
+
+	return data;
+}
+
+static void r8153b_ups_en(struct r8152 *tp, bool enable)
+{
+	u32 ocp_data = ocp_read_byte(tp, MCU_TYPE_USB, USB_POWER_CUT);
+
+	if (enable) {
+		ocp_data |= UPS_EN | USP_PREWAKE | PHASE2_EN;
+		ocp_write_byte(tp, MCU_TYPE_USB, USB_POWER_CUT, ocp_data);
+
+		ocp_data = ocp_read_byte(tp, MCU_TYPE_USB, 0xcfff);
+		ocp_data |= BIT(0);
+		ocp_write_byte(tp, MCU_TYPE_USB, 0xcfff, ocp_data);
+	} else {
+		u16 data;
+
+		ocp_data &= ~(UPS_EN | USP_PREWAKE);
+		ocp_write_byte(tp, MCU_TYPE_USB, USB_POWER_CUT, ocp_data);
+
+		ocp_data = ocp_read_byte(tp, MCU_TYPE_USB, 0xcfff);
+		ocp_data &= ~BIT(0);
+		ocp_write_byte(tp, MCU_TYPE_USB, 0xcfff, ocp_data);
+
+		ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_MISC_0);
+		ocp_data &= ~PCUT_STATUS;
+		ocp_write_word(tp, MCU_TYPE_USB, USB_MISC_0, ocp_data);
+
+		data = r8153_phy_status(tp, 0);
+
+		switch (data) {
+		case PHY_STAT_PWRDN:
+		case PHY_STAT_EXT_INIT:
+			r8153b_green_en(tp,
+					test_bit(GREEN_ETHERNET, &tp->flags));
+
+			data = r8152_mdio_read(tp, MII_BMCR);
+			data &= ~BMCR_PDOWN;
+			data |= BMCR_RESET;
+			r8152_mdio_write(tp, MII_BMCR, data);
+
+			data = r8153_phy_status(tp, PHY_STAT_LAN_ON);
+
+		default:
+			if (data != PHY_STAT_LAN_ON)
+				netif_warn(tp, link, tp->netdev,
+					   "PHY not ready");
+			break;
+		}
+	}
+}
+
 static void r8153_power_cut_en(struct r8152 *tp, bool enable)
 {
 	u32 ocp_data;
@@ -2474,6 +2761,38 @@ static void r8153_power_cut_en(struct r8152 *tp, bool enable)
 	ocp_write_word(tp, MCU_TYPE_USB, USB_MISC_0, ocp_data);
 }
 
+static void r8153b_power_cut_en(struct r8152 *tp, bool enable)
+{
+	u32 ocp_data;
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_POWER_CUT);
+	if (enable)
+		ocp_data |= PWR_EN | PHASE2_EN;
+	else
+		ocp_data &= ~PWR_EN;
+	ocp_write_word(tp, MCU_TYPE_USB, USB_POWER_CUT, ocp_data);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_MISC_0);
+	ocp_data &= ~PCUT_STATUS;
+	ocp_write_word(tp, MCU_TYPE_USB, USB_MISC_0, ocp_data);
+}
+
+static void r8153b_queue_wake(struct r8152 *tp, bool enable)
+{
+	u32 ocp_data;
+
+	ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, 0xd38a);
+	if (enable)
+		ocp_data |= BIT(0);
+	else
+		ocp_data &= ~BIT(0);
+	ocp_write_byte(tp, MCU_TYPE_PLA, 0xd38a, ocp_data);
+
+	ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, 0xd38c);
+	ocp_data &= ~BIT(0);
+	ocp_write_byte(tp, MCU_TYPE_PLA, 0xd38c, ocp_data);
+}
+
 static bool rtl_can_wakeup(struct r8152 *tp)
 {
 	struct usb_device *udev = tp->udev;
@@ -2512,24 +2831,76 @@ static void rtl_runtime_suspend_enable(struct r8152 *tp, bool enable)
 
 static void rtl8153_runtime_enable(struct r8152 *tp, bool enable)
 {
-	rtl_runtime_suspend_enable(tp, enable);
-
 	if (enable) {
 		r8153_u1u2en(tp, false);
 		r8153_u2p3en(tp, false);
+		r8153_mac_clk_spd(tp, true);
+		rtl_runtime_suspend_enable(tp, true);
 	} else {
-		r8153_u2p3en(tp, true);
+		rtl_runtime_suspend_enable(tp, false);
+		r8153_mac_clk_spd(tp, false);
+
+		switch (tp->version) {
+		case RTL_VER_03:
+		case RTL_VER_04:
+			break;
+		case RTL_VER_05:
+		case RTL_VER_06:
+		default:
+			r8153_u2p3en(tp, true);
+			break;
+		}
+
 		r8153_u1u2en(tp, true);
 	}
 }
 
+static void rtl8153b_runtime_enable(struct r8152 *tp, bool enable)
+{
+	if (enable) {
+		r8153b_queue_wake(tp, true);
+		r8153b_u1u2en(tp, false);
+		r8153_u2p3en(tp, false);
+		rtl_runtime_suspend_enable(tp, true);
+		r8153b_ups_en(tp, true);
+	} else {
+		r8153b_ups_en(tp, false);
+		r8153b_queue_wake(tp, false);
+		rtl_runtime_suspend_enable(tp, false);
+		r8153_u2p3en(tp, true);
+		r8153b_u1u2en(tp, true);
+	}
+}
+
 static void r8153_teredo_off(struct r8152 *tp)
 {
 	u32 ocp_data;
 
-	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_TEREDO_CFG);
-	ocp_data &= ~(TEREDO_SEL | TEREDO_RS_EVENT_MASK | OOB_TEREDO_EN);
-	ocp_write_word(tp, MCU_TYPE_PLA, PLA_TEREDO_CFG, ocp_data);
+	switch (tp->version) {
+	case RTL_VER_01:
+	case RTL_VER_02:
+	case RTL_VER_03:
+	case RTL_VER_04:
+	case RTL_VER_05:
+	case RTL_VER_06:
+	case RTL_VER_07:
+		ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_TEREDO_CFG);
+		ocp_data &= ~(TEREDO_SEL | TEREDO_RS_EVENT_MASK |
+			      OOB_TEREDO_EN);
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_TEREDO_CFG, ocp_data);
+		break;
+
+	case RTL_VER_08:
+	case RTL_VER_09:
+		/* The bit 0 ~ 7 are relative with teredo settings. They are
+		 * W1C (write 1 to clear), so set all 1 to disable it.
+		 */
+		ocp_write_byte(tp, MCU_TYPE_PLA, PLA_TEREDO_CFG, 0xff);
+		break;
+
+	default:
+		break;
+	}
 
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_WDT6_CTRL, WDT6_SET_MODE);
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_REALWOW_TIMER, 0);
@@ -2775,6 +3146,33 @@ static void r8152b_enter_oob(struct r8152 *tp)
 	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RCR, ocp_data);
 }
 
+static int r8153_patch_request(struct r8152 *tp, bool request)
+{
+	u16 data;
+	int i;
+
+	data = ocp_reg_read(tp, OCP_PHY_PATCH_CMD);
+	if (request)
+		data |= PATCH_REQUEST;
+	else
+		data &= ~PATCH_REQUEST;
+	ocp_reg_write(tp, OCP_PHY_PATCH_CMD, data);
+
+	for (i = 0; request && i < 5000; i++) {
+		usleep_range(1000, 2000);
+		if (ocp_reg_read(tp, OCP_PHY_PATCH_STAT) & PATCH_READY)
+			break;
+	}
+
+	if (request && !(ocp_reg_read(tp, OCP_PHY_PATCH_STAT) & PATCH_READY)) {
+		netif_err(tp, drv, tp->netdev, "patch request fail\n");
+		r8153_patch_request(tp, false);
+		return -ETIME;
+	} else {
+		return 0;
+	}
+}
+
 static void r8153_aldps_en(struct r8152 *tp, bool enable)
 {
 	u16 data;
@@ -2784,12 +3182,28 @@ static void r8153_aldps_en(struct r8152 *tp, bool enable)
 		data |= EN_ALDPS;
 		ocp_reg_write(tp, OCP_POWER_CFG, data);
 	} else {
+		int i;
+
 		data &= ~EN_ALDPS;
 		ocp_reg_write(tp, OCP_POWER_CFG, data);
-		msleep(20);
+		for (i = 0; i < 20; i++) {
+			usleep_range(1000, 2000);
+			if (ocp_read_word(tp, MCU_TYPE_PLA, 0xe000) & 0x0100)
+				break;
+		}
 	}
 }
 
+static void r8153b_aldps_en(struct r8152 *tp, bool enable)
+{
+	r8153_aldps_en(tp, enable);
+
+	if (enable)
+		r8153b_ups_flags_w1w0(tp, UPS_FLAGS_EN_ALDPS, 0);
+	else
+		r8153b_ups_flags_w1w0(tp, 0, UPS_FLAGS_EN_ALDPS);
+}
+
 static void r8153_eee_en(struct r8152 *tp, bool enable)
 {
 	u32 ocp_data;
@@ -2810,6 +3224,22 @@ static void r8153_eee_en(struct r8152 *tp, bool enable)
 	ocp_reg_write(tp, OCP_EEE_CFG, config);
 }
 
+static void r8153b_eee_en(struct r8152 *tp, bool enable)
+{
+	r8153_eee_en(tp, enable);
+
+	if (enable)
+		r8153b_ups_flags_w1w0(tp, UPS_FLAGS_EN_EEE, 0);
+	else
+		r8153b_ups_flags_w1w0(tp, 0, UPS_FLAGS_EN_EEE);
+}
+
+static void r8153b_enable_fc(struct r8152 *tp)
+{
+	r8152b_enable_fc(tp);
+	r8153b_ups_flags_w1w0(tp, UPS_FLAGS_EN_FLOW_CTR, 0);
+}
+
 static void r8153_hw_phy_cfg(struct r8152 *tp)
 {
 	u32 ocp_data;
@@ -2857,6 +3287,111 @@ static void r8153_hw_phy_cfg(struct r8152 *tp)
 	r8153_aldps_en(tp, true);
 	r8152b_enable_fc(tp);
 
+	switch (tp->version) {
+	case RTL_VER_03:
+	case RTL_VER_04:
+		break;
+	case RTL_VER_05:
+	case RTL_VER_06:
+	default:
+		r8153_u2p3en(tp, true);
+		break;
+	}
+
+	set_bit(PHY_RESET, &tp->flags);
+}
+
+static u32 r8152_efuse_read(struct r8152 *tp, u8 addr)
+{
+	u32 ocp_data;
+
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_EFUSE_CMD, EFUSE_READ_CMD | addr);
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_EFUSE_CMD);
+	ocp_data = (ocp_data & EFUSE_DATA_BIT16) << 9;	/* data of bit16 */
+	ocp_data |= ocp_read_word(tp, MCU_TYPE_PLA, PLA_EFUSE_DATA);
+
+	return ocp_data;
+}
+
+static void r8153b_hw_phy_cfg(struct r8152 *tp)
+{
+	u32 ocp_data, ups_flags = 0;
+	u16 data;
+
+	/* disable ALDPS before updating the PHY parameters */
+	r8153b_aldps_en(tp, false);
+
+	/* disable EEE before updating the PHY parameters */
+	r8153b_eee_en(tp, false);
+	ocp_reg_write(tp, OCP_EEE_ADV, 0);
+
+	r8153b_green_en(tp, test_bit(GREEN_ETHERNET, &tp->flags));
+
+	data = sram_read(tp, SRAM_GREEN_CFG);
+	data |= R_TUNE_EN;
+	sram_write(tp, SRAM_GREEN_CFG, data);
+	data = ocp_reg_read(tp, OCP_NCTL_CFG);
+	data |= PGA_RETURN_EN;
+	ocp_reg_write(tp, OCP_NCTL_CFG, data);
+
+	/* ADC Bias Calibration:
+	 * read efuse offset 0x7d to get a 17-bit data. Remove the dummy/fake
+	 * bit (bit3) to rebuild the real 16-bit data. Write the data to the
+	 * ADC ioffset.
+	 */
+	ocp_data = r8152_efuse_read(tp, 0x7d);
+	data = (u16)(((ocp_data & 0x1fff0) >> 1) | (ocp_data & 0x7));
+	if (data != 0xffff)
+		ocp_reg_write(tp, OCP_ADC_IOFFSET, data);
+
+	/* ups mode tx-link-pulse timing adjustment:
+	 * rg_saw_cnt = OCP reg 0xC426 Bit[13:0]
+	 * swr_cnt_1ms_ini = 16000000 / rg_saw_cnt
+	 */
+	ocp_data = ocp_reg_read(tp, 0xc426);
+	ocp_data &= 0x3fff;
+	if (ocp_data) {
+		u32 swr_cnt_1ms_ini;
+
+		swr_cnt_1ms_ini = (16000000 / ocp_data) & SAW_CNT_1MS_MASK;
+		ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_UPS_CFG);
+		ocp_data = (ocp_data & ~SAW_CNT_1MS_MASK) | swr_cnt_1ms_ini;
+		ocp_write_word(tp, MCU_TYPE_USB, USB_UPS_CFG, ocp_data);
+	}
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_PHY_PWR);
+	ocp_data |= PFM_PWM_SWITCH;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_PHY_PWR, ocp_data);
+
+	/* Advnace EEE */
+	if (!r8153_patch_request(tp, true)) {
+		data = ocp_reg_read(tp, OCP_POWER_CFG);
+		data |= EEE_CLKDIV_EN;
+		ocp_reg_write(tp, OCP_POWER_CFG, data);
+
+		data = ocp_reg_read(tp, OCP_DOWN_SPEED);
+		data |= EN_EEE_CMODE | EN_EEE_1000 | EN_10M_CLKDIV;
+		ocp_reg_write(tp, OCP_DOWN_SPEED, data);
+
+		ocp_reg_write(tp, OCP_SYSCLK_CFG, 0);
+		ocp_reg_write(tp, OCP_SYSCLK_CFG, clk_div_expo(5));
+
+		ups_flags |= UPS_FLAGS_EN_10M_CKDIV | UPS_FLAGS_250M_CKDIV |
+			     UPS_FLAGS_EN_EEE_CKDIV | UPS_FLAGS_EEE_CMOD_LV_EN |
+			     UPS_FLAGS_EEE_PLLOFF_GIGA;
+
+		r8153_patch_request(tp, false);
+	}
+
+	r8153b_ups_flags_w1w0(tp, ups_flags, 0);
+
+	r8153b_eee_en(tp, true);
+	ocp_reg_write(tp, OCP_EEE_ADV, MDIO_EEE_1000T | MDIO_EEE_100TX);
+
+	r8153b_aldps_en(tp, true);
+	r8153b_enable_fc(tp);
+	r8153_u2p3en(tp, true);
+
 	set_bit(PHY_RESET, &tp->flags);
 }
 
@@ -2865,6 +3400,7 @@ static void r8153_first_init(struct r8152 *tp)
 	u32 ocp_data;
 	int i;
 
+	r8153_mac_clk_spd(tp, false);
 	rxdy_gated_en(tp, true);
 	r8153_teredo_off(tp);
 
@@ -2903,7 +3439,7 @@ static void r8153_first_init(struct r8152 *tp)
 
 	rtl_rx_vlan_en(tp, tp->netdev->features & NETIF_F_HW_VLAN_CTAG_RX);
 
-	ocp_data = tp->netdev->mtu + VLAN_ETH_HLEN + CRC_SIZE;
+	ocp_data = tp->netdev->mtu + VLAN_ETH_HLEN + ETH_FCS_LEN;
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_RMS, ocp_data);
 	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_MTPS, MTPS_JUMBO);
 
@@ -2919,11 +3455,6 @@ static void r8153_first_init(struct r8152 *tp)
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_RXFIFO_CTRL2, RXFIFO_THR3_NORMAL);
 	/* TX share fifo free credit full threshold */
 	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_TXFIFO_CTRL, TXFIFO_THR_NORMAL2);
-
-	/* rx aggregation */
-	ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_USB_CTRL);
-	ocp_data &= ~(RX_AGG_DISABLE | RX_ZERO_EN);
-	ocp_write_word(tp, MCU_TYPE_USB, USB_USB_CTRL, ocp_data);
 }
 
 static void r8153_enter_oob(struct r8152 *tp)
@@ -2931,6 +3462,8 @@ static void r8153_enter_oob(struct r8152 *tp)
 	u32 ocp_data;
 	int i;
 
+	r8153_mac_clk_spd(tp, true);
+
 	ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
 	ocp_data &= ~NOW_IS_OOB;
 	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL, ocp_data);
@@ -2956,12 +3489,31 @@ static void r8153_enter_oob(struct r8152 *tp)
 		usleep_range(1000, 2000);
 	}
 
-	ocp_data = tp->netdev->mtu + VLAN_ETH_HLEN + CRC_SIZE;
+	ocp_data = tp->netdev->mtu + VLAN_ETH_HLEN + ETH_FCS_LEN;
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_RMS, ocp_data);
 
-	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_TEREDO_CFG);
-	ocp_data &= ~TEREDO_WAKE_MASK;
-	ocp_write_word(tp, MCU_TYPE_PLA, PLA_TEREDO_CFG, ocp_data);
+	switch (tp->version) {
+	case RTL_VER_03:
+	case RTL_VER_04:
+	case RTL_VER_05:
+	case RTL_VER_06:
+		ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_TEREDO_CFG);
+		ocp_data &= ~TEREDO_WAKE_MASK;
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_TEREDO_CFG, ocp_data);
+		break;
+
+	case RTL_VER_08:
+	case RTL_VER_09:
+		/* Clear teredo wake event. bit[15:8] is the teredo wakeup
+		 * type. Set it to zero. bits[7:0] are the W1C bits about
+		 * the events. Set them to all 1 to clear them.
+		 */
+		ocp_write_word(tp, MCU_TYPE_PLA, PLA_TEREDO_WAKE_BASE, 0x00ff);
+		break;
+
+	default:
+		break;
+	}
 
 	rtl_rx_vlan_en(tp, true);
 
@@ -2986,12 +3538,20 @@ static void rtl8153_disable(struct r8152 *tp)
 	rtl_disable(tp);
 	rtl_reset_bmu(tp);
 	r8153_aldps_en(tp, true);
-	usb_enable_lpm(tp->udev);
+}
+
+static void rtl8153b_disable(struct r8152 *tp)
+{
+	r8153b_aldps_en(tp, false);
+	rtl_disable(tp);
+	rtl_reset_bmu(tp);
+	r8153b_aldps_en(tp, true);
 }
 
 static int rtl8152_set_speed(struct r8152 *tp, u8 autoneg, u16 speed, u8 duplex)
 {
 	u16 bmcr, anar, gbcr;
+	enum spd_duplex speed_duplex;
 	int ret = 0;
 
 	anar = r8152_mdio_read(tp, MII_ADVERTISE);
@@ -3008,32 +3568,43 @@ static int rtl8152_set_speed(struct r8152 *tp, u8 autoneg, u16 speed, u8 duplex)
 		if (speed == SPEED_10) {
 			bmcr = 0;
 			anar |= ADVERTISE_10HALF | ADVERTISE_10FULL;
+			speed_duplex = FORCE_10M_HALF;
 		} else if (speed == SPEED_100) {
 			bmcr = BMCR_SPEED100;
 			anar |= ADVERTISE_100HALF | ADVERTISE_100FULL;
+			speed_duplex = FORCE_100M_HALF;
 		} else if (speed == SPEED_1000 && tp->mii.supports_gmii) {
 			bmcr = BMCR_SPEED1000;
 			gbcr |= ADVERTISE_1000FULL | ADVERTISE_1000HALF;
+			speed_duplex = NWAY_1000M_FULL;
 		} else {
 			ret = -EINVAL;
 			goto out;
 		}
 
-		if (duplex == DUPLEX_FULL)
+		if (duplex == DUPLEX_FULL) {
 			bmcr |= BMCR_FULLDPLX;
+			if (speed != SPEED_1000)
+				speed_duplex++;
+		}
 	} else {
 		if (speed == SPEED_10) {
-			if (duplex == DUPLEX_FULL)
+			if (duplex == DUPLEX_FULL) {
 				anar |= ADVERTISE_10HALF | ADVERTISE_10FULL;
-			else
+				speed_duplex = NWAY_10M_FULL;
+			} else {
 				anar |= ADVERTISE_10HALF;
+				speed_duplex = NWAY_10M_HALF;
+			}
 		} else if (speed == SPEED_100) {
 			if (duplex == DUPLEX_FULL) {
 				anar |= ADVERTISE_10HALF | ADVERTISE_10FULL;
 				anar |= ADVERTISE_100HALF | ADVERTISE_100FULL;
+				speed_duplex = NWAY_100M_FULL;
 			} else {
 				anar |= ADVERTISE_10HALF;
 				anar |= ADVERTISE_100HALF;
+				speed_duplex = NWAY_100M_HALF;
 			}
 		} else if (speed == SPEED_1000 && tp->mii.supports_gmii) {
 			if (duplex == DUPLEX_FULL) {
@@ -3045,6 +3616,7 @@ static int rtl8152_set_speed(struct r8152 *tp, u8 autoneg, u16 speed, u8 duplex)
 				anar |= ADVERTISE_100HALF;
 				gbcr |= ADVERTISE_1000HALF;
 			}
+			speed_duplex = NWAY_1000M_FULL;
 		} else {
 			ret = -EINVAL;
 			goto out;
@@ -3062,6 +3634,17 @@ static int rtl8152_set_speed(struct r8152 *tp, u8 autoneg, u16 speed, u8 duplex)
 	r8152_mdio_write(tp, MII_ADVERTISE, anar);
 	r8152_mdio_write(tp, MII_BMCR, bmcr);
 
+	switch (tp->version) {
+	case RTL_VER_08:
+	case RTL_VER_09:
+		r8153b_ups_flags_w1w0(tp, ups_flags_speed(speed_duplex),
+				      UPS_FLAGS_SPEED_MASK);
+		break;
+
+	default:
+		break;
+	}
+
 	if (bmcr & BMCR_RESET) {
 		int i;
 
@@ -3105,12 +3688,23 @@ static void rtl8153_up(struct r8152 *tp)
 		return;
 
 	r8153_u1u2en(tp, false);
+	r8153_u2p3en(tp, false);
 	r8153_aldps_en(tp, false);
 	r8153_first_init(tp);
 	r8153_aldps_en(tp, true);
-	r8153_u2p3en(tp, true);
+
+	switch (tp->version) {
+	case RTL_VER_03:
+	case RTL_VER_04:
+		break;
+	case RTL_VER_05:
+	case RTL_VER_06:
+	default:
+		r8153_u2p3en(tp, true);
+		break;
+	}
+
 	r8153_u1u2en(tp, true);
-	usb_enable_lpm(tp->udev);
 }
 
 static void rtl8153_down(struct r8152 *tp)
@@ -3128,6 +3722,38 @@ static void rtl8153_down(struct r8152 *tp)
 	r8153_aldps_en(tp, true);
 }
 
+static void rtl8153b_up(struct r8152 *tp)
+{
+	if (test_bit(RTL8152_UNPLUG, &tp->flags))
+		return;
+
+	r8153b_u1u2en(tp, false);
+	r8153_u2p3en(tp, false);
+	r8153b_aldps_en(tp, false);
+
+	r8153_first_init(tp);
+	ocp_write_dword(tp, MCU_TYPE_USB, USB_RX_BUF_TH, RX_THR_B);
+
+	r8153b_aldps_en(tp, true);
+	r8153_u2p3en(tp, true);
+	r8153b_u1u2en(tp, true);
+}
+
+static void rtl8153b_down(struct r8152 *tp)
+{
+	if (test_bit(RTL8152_UNPLUG, &tp->flags)) {
+		rtl_drop_queued_tx(tp);
+		return;
+	}
+
+	r8153b_u1u2en(tp, false);
+	r8153_u2p3en(tp, false);
+	r8153b_power_cut_en(tp, false);
+	r8153b_aldps_en(tp, false);
+	r8153_enter_oob(tp);
+	r8153b_aldps_en(tp, true);
+}
+
 static bool rtl8152_in_nway(struct r8152 *tp)
 {
 	u16 nway_state;
@@ -3426,12 +4052,7 @@ static void r8153_init(struct r8152 *tp)
 		msleep(20);
 	}
 
-	for (i = 0; i < 500; i++) {
-		ocp_data = ocp_reg_read(tp, OCP_PHY_STATUS) & PHY_STAT_MASK;
-		if (ocp_data == PHY_STAT_LAN_ON || ocp_data == PHY_STAT_PWRDN)
-			break;
-		msleep(20);
-	}
+	data = r8153_phy_status(tp, 0);
 
 	if (tp->version == RTL_VER_03 || tp->version == RTL_VER_04 ||
 	    tp->version == RTL_VER_05)
@@ -3443,14 +4064,8 @@ static void r8153_init(struct r8152 *tp)
 		r8152_mdio_write(tp, MII_BMCR, data);
 	}
 
-	for (i = 0; i < 500; i++) {
-		ocp_data = ocp_reg_read(tp, OCP_PHY_STATUS) & PHY_STAT_MASK;
-		if (ocp_data == PHY_STAT_LAN_ON)
-			break;
-		msleep(20);
-	}
+	data = r8153_phy_status(tp, PHY_STAT_LAN_ON);
 
-	usb_disable_lpm(tp->udev);
 	r8153_u2p3en(tp, false);
 
 	if (tp->version == RTL_VER_04) {
@@ -3510,15 +4125,88 @@ static void r8153_init(struct r8152 *tp)
 
 	r8153_power_cut_en(tp, false);
 	r8153_u1u2en(tp, true);
+	r8153_mac_clk_spd(tp, false);
+	usb_enable_lpm(tp->udev);
+
+	/* rx aggregation */
+	ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_USB_CTRL);
+	ocp_data &= ~(RX_AGG_DISABLE | RX_ZERO_EN);
+	ocp_write_word(tp, MCU_TYPE_USB, USB_USB_CTRL, ocp_data);
+
+	rtl_tally_reset(tp);
+
+	switch (tp->udev->speed) {
+	case USB_SPEED_SUPER:
+	case USB_SPEED_SUPER_PLUS:
+		tp->coalesce = COALESCE_SUPER;
+		break;
+	case USB_SPEED_HIGH:
+		tp->coalesce = COALESCE_HIGH;
+		break;
+	default:
+		tp->coalesce = COALESCE_SLOW;
+		break;
+	}
+}
+
+static void r8153b_init(struct r8152 *tp)
+{
+	u32 ocp_data;
+	u16 data;
+	int i;
+
+	if (test_bit(RTL8152_UNPLUG, &tp->flags))
+		return;
+
+	r8153b_u1u2en(tp, false);
+
+	for (i = 0; i < 500; i++) {
+		if (ocp_read_word(tp, MCU_TYPE_PLA, PLA_BOOT_CTRL) &
+		    AUTOLOAD_DONE)
+			break;
+		msleep(20);
+	}
+
+	data = r8153_phy_status(tp, 0);
+
+	data = r8152_mdio_read(tp, MII_BMCR);
+	if (data & BMCR_PDOWN) {
+		data &= ~BMCR_PDOWN;
+		r8152_mdio_write(tp, MII_BMCR, data);
+	}
+
+	data = r8153_phy_status(tp, PHY_STAT_LAN_ON);
+
+	r8153_u2p3en(tp, false);
+
+	/* MSC timer = 0xfff * 8ms = 32760 ms */
+	ocp_write_word(tp, MCU_TYPE_USB, USB_MSC_TIMER, 0x0fff);
+
+	/* U1/U2/L1 idle timer. 500 us */
+	ocp_write_word(tp, MCU_TYPE_USB, USB_U1U2_TIMER, 500);
+
+	r8153b_power_cut_en(tp, false);
+	r8153b_ups_en(tp, false);
+	r8153b_queue_wake(tp, false);
+	rtl_runtime_suspend_enable(tp, false);
+	r8153b_u1u2en(tp, true);
+	usb_enable_lpm(tp->udev);
 
 	/* MAC clock speed down */
-	ocp_write_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL, 0);
-	ocp_write_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL2, 0);
-	ocp_write_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL3, 0);
-	ocp_write_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL4, 0);
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL2);
+	ocp_data |= MAC_CLK_SPDWN_EN;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_MAC_PWR_CTRL2, ocp_data);
+
+	set_bit(GREEN_ETHERNET, &tp->flags);
+
+	/* rx aggregation */
+	ocp_data = ocp_read_word(tp, MCU_TYPE_USB, USB_USB_CTRL);
+	ocp_data &= ~(RX_AGG_DISABLE | RX_ZERO_EN);
+	ocp_write_word(tp, MCU_TYPE_USB, USB_USB_CTRL, ocp_data);
 
 	rtl_tally_reset(tp);
-	r8153_u2p3en(tp, true);
+
+	tp->coalesce = 15000;	/* 15 us */
 }
 
 static int rtl8152_pre_reset(struct usb_interface *intf)
@@ -3601,6 +4289,61 @@ static bool delay_autosuspend(struct r8152 *tp)
 		return false;
 }
 
+static int rtl8152_runtime_resume(struct r8152 *tp)
+{
+	struct net_device *netdev = tp->netdev;
+
+	if (netif_running(netdev) && netdev->flags & IFF_UP) {
+		struct napi_struct *napi = &tp->napi;
+
+		tp->rtl_ops.autosuspend_en(tp, false);
+		napi_disable(napi);
+		set_bit(WORK_ENABLE, &tp->flags);
+
+		if (netif_carrier_ok(netdev)) {
+			if (rtl8152_get_speed(tp) & LINK_STATUS) {
+				rtl_start_rx(tp);
+			} else {
+				netif_carrier_off(netdev);
+				tp->rtl_ops.disable(tp);
+				netif_info(tp, link, netdev, "linking down\n");
+			}
+		}
+
+		napi_enable(napi);
+		clear_bit(SELECTIVE_SUSPEND, &tp->flags);
+		smp_mb__after_atomic();
+
+		if (!list_empty(&tp->rx_done))
+			napi_schedule(&tp->napi);
+
+		usb_submit_urb(tp->intr_urb, GFP_NOIO);
+	} else {
+		if (netdev->flags & IFF_UP)
+			tp->rtl_ops.autosuspend_en(tp, false);
+
+		clear_bit(SELECTIVE_SUSPEND, &tp->flags);
+	}
+
+	return 0;
+}
+
+static int rtl8152_system_resume(struct r8152 *tp)
+{
+	struct net_device *netdev = tp->netdev;
+
+	netif_device_attach(netdev);
+
+	if (netif_running(netdev) && netdev->flags & IFF_UP) {
+		tp->rtl_ops.up(tp);
+		netif_carrier_off(netdev);
+		set_bit(WORK_ENABLE, &tp->flags);
+		usb_submit_urb(tp->intr_urb, GFP_NOIO);
+	}
+
+	return 0;
+}
+
 static int rtl8152_runtime_suspend(struct r8152 *tp)
 {
 	struct net_device *netdev = tp->netdev;
@@ -3612,13 +4355,6 @@ static int rtl8152_runtime_suspend(struct r8152 *tp)
 	if (netif_running(netdev) && test_bit(WORK_ENABLE, &tp->flags)) {
 		u32 rcr = 0;
 
-		if (delay_autosuspend(tp)) {
-			clear_bit(SELECTIVE_SUSPEND, &tp->flags);
-			smp_mb__after_atomic();
-			ret = -EBUSY;
-			goto out1;
-		}
-
 		if (netif_carrier_ok(netdev)) {
 			u32 ocp_data;
 
@@ -3652,6 +4388,11 @@ static int rtl8152_runtime_suspend(struct r8152 *tp)
 			ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RCR, rcr);
 			napi_enable(napi);
 		}
+
+		if (delay_autosuspend(tp)) {
+			rtl8152_runtime_resume(tp);
+			ret = -EBUSY;
+		}
 	}
 
 out1:
@@ -3699,53 +4440,18 @@ static int rtl8152_suspend(struct usb_interface *intf, pm_message_t message)
 static int rtl8152_resume(struct usb_interface *intf)
 {
 	struct r8152 *tp = usb_get_intfdata(intf);
-	struct net_device *netdev = tp->netdev;
+	int ret;
 
 	mutex_lock(&tp->control);
 
-	if (!test_bit(SELECTIVE_SUSPEND, &tp->flags)) {
-		tp->rtl_ops.init(tp);
-		queue_delayed_work(system_long_wq, &tp->hw_phy_work, 0);
-		netif_device_attach(netdev);
-	}
-
-	if (netif_running(netdev) && netdev->flags & IFF_UP) {
-		if (test_bit(SELECTIVE_SUSPEND, &tp->flags)) {
-			struct napi_struct *napi = &tp->napi;
-
-			tp->rtl_ops.autosuspend_en(tp, false);
-			napi_disable(napi);
-			set_bit(WORK_ENABLE, &tp->flags);
-			if (netif_carrier_ok(netdev)) {
-				if (rtl8152_get_speed(tp) & LINK_STATUS) {
-					rtl_start_rx(tp);
-				} else {
-					netif_carrier_off(netdev);
-					tp->rtl_ops.disable(tp);
-					netif_info(tp, link, netdev,
-						   "linking down\n");
-				}
-			}
-			napi_enable(napi);
-			clear_bit(SELECTIVE_SUSPEND, &tp->flags);
-			smp_mb__after_atomic();
-			if (!list_empty(&tp->rx_done))
-				napi_schedule(&tp->napi);
-		} else {
-			tp->rtl_ops.up(tp);
-			netif_carrier_off(netdev);
-			set_bit(WORK_ENABLE, &tp->flags);
-		}
-		usb_submit_urb(tp->intr_urb, GFP_KERNEL);
-	} else if (test_bit(SELECTIVE_SUSPEND, &tp->flags)) {
-		if (netdev->flags & IFF_UP)
-			tp->rtl_ops.autosuspend_en(tp, false);
-		clear_bit(SELECTIVE_SUSPEND, &tp->flags);
-	}
+	if (test_bit(SELECTIVE_SUSPEND, &tp->flags))
+		ret = rtl8152_runtime_resume(tp);
+	else
+		ret = rtl8152_system_resume(tp);
 
 	mutex_unlock(&tp->control);
 
-	return 0;
+	return ret;
 }
 
 static int rtl8152_reset_resume(struct usb_interface *intf)
@@ -3753,6 +4459,10 @@ static int rtl8152_reset_resume(struct usb_interface *intf)
 	struct r8152 *tp = usb_get_intfdata(intf);
 
 	clear_bit(SELECTIVE_SUSPEND, &tp->flags);
+	mutex_lock(&tp->control);
+	tp->rtl_ops.init(tp);
+	queue_delayed_work(system_long_wq, &tp->hw_phy_work, 0);
+	mutex_unlock(&tp->control);
 	return rtl8152_resume(intf);
 }
 
@@ -3841,7 +4551,7 @@ int rtl8152_get_link_ksettings(struct net_device *netdev,
 
 	mutex_lock(&tp->control);
 
-	ret = mii_ethtool_get_link_ksettings(&tp->mii, cmd);
+	mii_ethtool_get_link_ksettings(&tp->mii, cmd);
 
 	mutex_unlock(&tp->control);
 
@@ -4022,6 +4732,20 @@ static int r8153_set_eee(struct r8152 *tp, struct ethtool_eee *eee)
 	return 0;
 }
 
+static int r8153b_set_eee(struct r8152 *tp, struct ethtool_eee *eee)
+{
+	u16 val = ethtool_adv_to_mmd_eee_adv_t(eee->advertised);
+
+	r8153b_eee_en(tp, eee->eee_enabled);
+
+	if (!eee->eee_enabled)
+		val = 0;
+
+	ocp_reg_write(tp, OCP_EEE_ADV, val);
+
+	return 0;
+}
+
 static int
 rtl_ethtool_get_eee(struct net_device *net, struct ethtool_eee *edata)
 {
@@ -4097,6 +4821,7 @@ static int rtl8152_get_coalesce(struct net_device *netdev,
 	switch (tp->version) {
 	case RTL_VER_01:
 	case RTL_VER_02:
+	case RTL_VER_07:
 		return -EOPNOTSUPP;
 	default:
 		break;
@@ -4116,6 +4841,7 @@ static int rtl8152_set_coalesce(struct net_device *netdev,
 	switch (tp->version) {
 	case RTL_VER_01:
 	case RTL_VER_02:
+	case RTL_VER_07:
 		return -EOPNOTSUPP;
 	default:
 		break;
@@ -4215,6 +4941,7 @@ static int rtl8152_change_mtu(struct net_device *dev, int new_mtu)
 	switch (tp->version) {
 	case RTL_VER_01:
 	case RTL_VER_02:
+	case RTL_VER_07:
 		dev->mtu = new_mtu;
 		return 0;
 	default:
@@ -4230,7 +4957,7 @@ static int rtl8152_change_mtu(struct net_device *dev, int new_mtu)
 	dev->mtu = new_mtu;
 
 	if (netif_running(dev)) {
-		u32 rms = new_mtu + VLAN_ETH_HLEN + CRC_SIZE;
+		u32 rms = new_mtu + VLAN_ETH_HLEN + ETH_FCS_LEN;
 
 		ocp_write_word(tp, MCU_TYPE_PLA, PLA_RMS, rms);
 
@@ -4276,6 +5003,14 @@ static void rtl8153_unload(struct r8152 *tp)
 	r8153_power_cut_en(tp, false);
 }
 
+static void rtl8153b_unload(struct r8152 *tp)
+{
+	if (test_bit(RTL8152_UNPLUG, &tp->flags))
+		return;
+
+	r8153b_power_cut_en(tp, false);
+}
+
 static int rtl_ops_init(struct r8152 *tp)
 {
 	struct rtl_ops *ops = &tp->rtl_ops;
@@ -4284,6 +5019,7 @@ static int rtl_ops_init(struct r8152 *tp)
 	switch (tp->version) {
 	case RTL_VER_01:
 	case RTL_VER_02:
+	case RTL_VER_07:
 		ops->init		= r8152b_init;
 		ops->enable		= rtl8152_enable;
 		ops->disable		= rtl8152_disable;
@@ -4314,6 +5050,21 @@ static int rtl_ops_init(struct r8152 *tp)
 		ops->autosuspend_en	= rtl8153_runtime_enable;
 		break;
 
+	case RTL_VER_08:
+	case RTL_VER_09:
+		ops->init		= r8153b_init;
+		ops->enable		= rtl8153_enable;
+		ops->disable		= rtl8153b_disable;
+		ops->up			= rtl8153b_up;
+		ops->down		= rtl8153b_down;
+		ops->unload		= rtl8153b_unload;
+		ops->eee_get		= r8153_get_eee;
+		ops->eee_set		= r8153b_set_eee;
+		ops->in_nway		= rtl8153_in_nway;
+		ops->hw_phy_cfg		= r8153b_hw_phy_cfg;
+		ops->autosuspend_en	= rtl8153b_runtime_enable;
+		break;
+
 	default:
 		ret = -ENODEV;
 		netif_err(tp, probe, tp->netdev, "Unknown Device\n");
@@ -4362,12 +5113,23 @@ static u8 rtl_get_version(struct usb_interface *intf)
 	case 0x5c30:
 		version = RTL_VER_06;
 		break;
+	case 0x4800:
+		version = RTL_VER_07;
+		break;
+	case 0x6000:
+		version = RTL_VER_08;
+		break;
+	case 0x6010:
+		version = RTL_VER_09;
+		break;
 	default:
 		version = RTL_VER_UNKNOWN;
 		dev_info(&intf->dev, "Unknown version 0x%04x\n", ocp_data);
 		break;
 	}
 
+	dev_dbg(&intf->dev, "Detected version 0x%04x\n", version);
+
 	return version;
 }
 
@@ -4407,6 +5169,7 @@ static int rtl8152_probe(struct usb_interface *intf,
 	switch (version) {
 	case RTL_VER_01:
 	case RTL_VER_02:
+	case RTL_VER_07:
 		tp->mii.supports_gmii = 0;
 		break;
 	default:
@@ -4464,19 +5227,6 @@ static int rtl8152_probe(struct usb_interface *intf,
 	tp->mii.reg_num_mask = 0x1f;
 	tp->mii.phy_id = R8152_PHY_ID;
 
-	switch (udev->speed) {
-	case USB_SPEED_SUPER:
-	case USB_SPEED_SUPER_PLUS:
-		tp->coalesce = COALESCE_SUPER;
-		break;
-	case USB_SPEED_HIGH:
-		tp->coalesce = COALESCE_HIGH;
-		break;
-	default:
-		tp->coalesce = COALESCE_SLOW;
-		break;
-	}
-
 	tp->autoneg = AUTONEG_ENABLE;
 	tp->speed = tp->mii.supports_gmii ? SPEED_1000 : SPEED_100;
 	tp->duplex = DUPLEX_FULL;
@@ -4554,6 +5304,7 @@ static void rtl8152_disconnect(struct usb_interface *intf)
 
 /* table of devices that work with this driver */
 static struct usb_device_id rtl8152_table[] = {
+	{REALTEK_USB_DEVICE(VENDOR_ID_REALTEK, 0x8050)},
 	{REALTEK_USB_DEVICE(VENDOR_ID_REALTEK, 0x8152)},
 	{REALTEK_USB_DEVICE(VENDOR_ID_REALTEK, 0x8153)},
 	{REALTEK_USB_DEVICE(VENDOR_ID_MICROSOFT, 0x07ab)},
diff --git a/drivers/net/usb/rndis_host.c b/drivers/net/usb/rndis_host.c
index e96e2e5673d7..a151f267aebb 100644
--- a/drivers/net/usb/rndis_host.c
+++ b/drivers/net/usb/rndis_host.c
@@ -578,7 +578,7 @@ rndis_tx_fixup(struct usbnet *dev, struct sk_buff *skb, gfp_t flags)
 	 * packets; Linux minimizes wasted bandwidth through tx queues.
 	 */
 fill:
-	hdr = (void *) __skb_push(skb, sizeof *hdr);
+	hdr = __skb_push(skb, sizeof *hdr);
 	memset(hdr, 0, sizeof *hdr);
 	hdr->msg_type = cpu_to_le32(RNDIS_MSG_PACKET);
 	hdr->msg_len = cpu_to_le32(skb->len);
diff --git a/drivers/net/usb/usbnet.c b/drivers/net/usb/usbnet.c
index 79048e72c1bd..6510e5cc1817 100644
--- a/drivers/net/usb/usbnet.c
+++ b/drivers/net/usb/usbnet.c
@@ -956,7 +956,9 @@ int usbnet_get_link_ksettings(struct net_device *net,
 	if (!dev->mii.mdio_read)
 		return -EOPNOTSUPP;
 
-	return mii_ethtool_get_link_ksettings(&dev->mii, cmd);
+	mii_ethtool_get_link_ksettings(&dev->mii, cmd);
+
+	return 0;
 }
 EXPORT_SYMBOL_GPL(usbnet_get_link_ksettings);
 
diff --git a/drivers/net/usb/zaurus.c b/drivers/net/usb/zaurus.c
index 6aaa6eb9df72..9c2196c3fd11 100644
--- a/drivers/net/usb/zaurus.c
+++ b/drivers/net/usb/zaurus.c
@@ -74,10 +74,10 @@ done:
 		fcs = crc32_le(~0, skb->data, skb->len);
 		fcs = ~fcs;
 
-		*skb_put (skb, 1) = fcs       & 0xff;
-		*skb_put (skb, 1) = (fcs>> 8) & 0xff;
-		*skb_put (skb, 1) = (fcs>>16) & 0xff;
-		*skb_put (skb, 1) = (fcs>>24) & 0xff;
+		skb_put_u8(skb, fcs & 0xff);
+		skb_put_u8(skb, (fcs >> 8) & 0xff);
+		skb_put_u8(skb, (fcs >> 16) & 0xff);
+		skb_put_u8(skb, (fcs >> 24) & 0xff);
 	}
 	return skb;
 }
diff --git a/drivers/net/veth.c b/drivers/net/veth.c
index 38f0f03a29c8..f5438d0978ca 100644
--- a/drivers/net/veth.c
+++ b/drivers/net/veth.c
@@ -222,7 +222,6 @@ static int veth_dev_init(struct net_device *dev)
 static void veth_dev_free(struct net_device *dev)
 {
 	free_percpu(dev->vstats);
-	free_netdev(dev);
 }
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
@@ -317,7 +316,8 @@ static void veth_setup(struct net_device *dev)
 			       NETIF_F_HW_VLAN_STAG_TX |
 			       NETIF_F_HW_VLAN_CTAG_RX |
 			       NETIF_F_HW_VLAN_STAG_RX);
-	dev->destructor = veth_dev_free;
+	dev->needs_free_netdev = true;
+	dev->priv_destructor = veth_dev_free;
 	dev->max_mtu = ETH_MAX_MTU;
 
 	dev->hw_features = VETH_FEATURES;
@@ -329,7 +329,8 @@ static void veth_setup(struct net_device *dev)
  * netlink interface
  */
 
-static int veth_validate(struct nlattr *tb[], struct nlattr *data[])
+static int veth_validate(struct nlattr *tb[], struct nlattr *data[],
+			 struct netlink_ext_ack *extack)
 {
 	if (tb[IFLA_ADDRESS]) {
 		if (nla_len(tb[IFLA_ADDRESS]) != ETH_ALEN)
@@ -347,7 +348,8 @@ static int veth_validate(struct nlattr *tb[], struct nlattr *data[])
 static struct rtnl_link_ops veth_link_ops;
 
 static int veth_newlink(struct net *src_net, struct net_device *dev,
-			 struct nlattr *tb[], struct nlattr *data[])
+			struct nlattr *tb[], struct nlattr *data[],
+			struct netlink_ext_ack *extack)
 {
 	int err;
 	struct net_device *peer;
@@ -373,7 +375,7 @@ static int veth_newlink(struct net *src_net, struct net_device *dev,
 		if (err < 0)
 			return err;
 
-		err = veth_validate(peer_tb, NULL);
+		err = veth_validate(peer_tb, NULL, extack);
 		if (err < 0)
 			return err;
 
@@ -383,7 +385,7 @@ static int veth_newlink(struct net *src_net, struct net_device *dev,
 		tbp = tb;
 	}
 
-	if (tbp[IFLA_IFNAME]) {
+	if (ifmp && tbp[IFLA_IFNAME]) {
 		nla_strlcpy(ifname, tbp[IFLA_IFNAME], IFNAMSIZ);
 		name_assign_type = NET_NAME_USER;
 	} else {
@@ -402,7 +404,7 @@ static int veth_newlink(struct net *src_net, struct net_device *dev,
 		return PTR_ERR(peer);
 	}
 
-	if (tbp[IFLA_ADDRESS] == NULL)
+	if (!ifmp || !tbp[IFLA_ADDRESS])
 		eth_hw_addr_random(peer);
 
 	if (ifmp && (dev->ifindex != 0))
diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index a871f45ecc79..2e69bcdc5b07 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -305,7 +305,7 @@ static struct sk_buff *page_to_skb(struct virtnet_info *vi,
 	copy = len;
 	if (copy > skb_tailroom(skb))
 		copy = skb_tailroom(skb);
-	memcpy(skb_put(skb, copy), p, copy);
+	skb_put_data(skb, p, copy);
 
 	len -= copy;
 	offset += copy;
@@ -1150,7 +1150,7 @@ static int xmit_skb(struct send_queue *sq, struct sk_buff *skb)
 	struct virtio_net_hdr_mrg_rxbuf *hdr;
 	const unsigned char *dest = ((struct ethhdr *)skb->data)->h_dest;
 	struct virtnet_info *vi = sq->vq->vdev->priv;
-	unsigned num_sg;
+	int num_sg;
 	unsigned hdr_len = vi->hdr_len;
 	bool can_push;
 
@@ -1177,11 +1177,16 @@ static int xmit_skb(struct send_queue *sq, struct sk_buff *skb)
 	if (can_push) {
 		__skb_push(skb, hdr_len);
 		num_sg = skb_to_sgvec(skb, sq->sg, 0, skb->len);
+		if (unlikely(num_sg < 0))
+			return num_sg;
 		/* Pull header back to avoid skew in tx bytes calculations. */
 		__skb_pull(skb, hdr_len);
 	} else {
 		sg_set_buf(sq->sg, hdr, hdr_len);
-		num_sg = skb_to_sgvec(skb, sq->sg + 1, 0, skb->len) + 1;
+		num_sg = skb_to_sgvec(skb, sq->sg + 1, 0, skb->len);
+		if (unlikely(num_sg < 0))
+			return num_sg;
+		num_sg++;
 	}
 	return virtqueue_add_outbuf(sq->vq, sq->sg, num_sg, skb, GFP_ATOMIC);
 }
@@ -1797,6 +1802,7 @@ static void virtnet_freeze_down(struct virtio_device *vdev)
 	flush_work(&vi->config_work);
 
 	netif_device_detach(vi->dev);
+	netif_tx_disable(vi->dev);
 	cancel_delayed_work_sync(&vi->refill);
 
 	if (netif_running(vi->dev)) {
@@ -1950,16 +1956,18 @@ virtio_reset_err:
 	return err;
 }
 
-static bool virtnet_xdp_query(struct net_device *dev)
+static u32 virtnet_xdp_query(struct net_device *dev)
 {
 	struct virtnet_info *vi = netdev_priv(dev);
+	const struct bpf_prog *xdp_prog;
 	int i;
 
 	for (i = 0; i < vi->max_queue_pairs; i++) {
-		if (vi->rq[i].xdp_prog)
-			return true;
+		xdp_prog = rtnl_dereference(vi->rq[i].xdp_prog);
+		if (xdp_prog)
+			return xdp_prog->aux->id;
 	}
-	return false;
+	return 0;
 }
 
 static int virtnet_xdp(struct net_device *dev, struct netdev_xdp *xdp)
@@ -1968,7 +1976,8 @@ static int virtnet_xdp(struct net_device *dev, struct netdev_xdp *xdp)
 	case XDP_SETUP_PROG:
 		return virtnet_xdp_set(dev, xdp->prog, xdp->extack);
 	case XDP_QUERY_PROG:
-		xdp->prog_attached = virtnet_xdp_query(dev);
+		xdp->prog_id = virtnet_xdp_query(dev);
+		xdp->prog_attached = !!xdp->prog_id;
 		return 0;
 	default:
 		return -EINVAL;
diff --git a/drivers/net/vrf.c b/drivers/net/vrf.c
index db882493875c..f4d0054981c6 100644
--- a/drivers/net/vrf.c
+++ b/drivers/net/vrf.c
@@ -36,12 +36,14 @@
 #include <net/addrconf.h>
 #include <net/l3mdev.h>
 #include <net/fib_rules.h>
+#include <net/netns/generic.h>
 
 #define DRV_NAME	"vrf"
 #define DRV_VERSION	"1.0"
 
 #define FIB_RULE_PREF  1000       /* default preference for FIB rules */
-static bool add_fib_rules = true;
+
+static unsigned int vrf_net_id;
 
 struct net_vrf {
 	struct rtable __rcu	*rth;
@@ -381,7 +383,7 @@ static int vrf_finish_direct(struct net *net, struct sock *sk,
 
 	if (!list_empty(&vrf_dev->ptype_all) &&
 	    likely(skb_headroom(skb) >= ETH_HLEN)) {
-		struct ethhdr *eth = (struct ethhdr *)skb_push(skb, ETH_HLEN);
+		struct ethhdr *eth = skb_push(skb, ETH_HLEN);
 
 		ether_addr_copy(eth->h_source, vrf_dev->dev_addr);
 		eth_zero_addr(eth->h_dest);
@@ -561,7 +563,7 @@ static void vrf_rt6_release(struct net_device *dev, struct net_vrf *vrf)
 
 static int vrf_rt6_create(struct net_device *dev)
 {
-	int flags = DST_HOST | DST_NOPOLICY | DST_NOXFRM | DST_NOCACHE;
+	int flags = DST_HOST | DST_NOPOLICY | DST_NOXFRM;
 	struct net_vrf *vrf = netdev_priv(dev);
 	struct net *net = dev_net(dev);
 	struct fib6_table *rt6i_table;
@@ -581,8 +583,6 @@ static int vrf_rt6_create(struct net_device *dev)
 	if (!rt6)
 		goto out;
 
-	dst_hold(&rt6->dst);
-
 	rt6->rt6i_table = rt6i_table;
 	rt6->dst.output	= vrf_output6;
 
@@ -595,8 +595,6 @@ static int vrf_rt6_create(struct net_device *dev)
 		goto out;
 	}
 
-	dst_hold(&rt6_local->dst);
-
 	rt6_local->rt6i_idev  = in6_dev_get(dev);
 	rt6_local->rt6i_flags = RTF_UP | RTF_NONEXTHOP | RTF_LOCAL;
 	rt6_local->rt6i_table = rt6i_table;
@@ -1348,7 +1346,7 @@ static void vrf_setup(struct net_device *dev)
 	dev->netdev_ops = &vrf_netdev_ops;
 	dev->l3mdev_ops = &vrf_l3mdev_ops;
 	dev->ethtool_ops = &vrf_ethtool_ops;
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 
 	/* Fill in device structure with ethernet-generic values. */
 	eth_hw_addr_random(dev);
@@ -1374,7 +1372,8 @@ static void vrf_setup(struct net_device *dev)
 	dev->priv_flags |= IFF_NO_QUEUE;
 }
 
-static int vrf_validate(struct nlattr *tb[], struct nlattr *data[])
+static int vrf_validate(struct nlattr *tb[], struct nlattr *data[],
+			struct netlink_ext_ack *extack)
 {
 	if (tb[IFLA_ADDRESS]) {
 		if (nla_len(tb[IFLA_ADDRESS]) != ETH_ALEN)
@@ -1391,9 +1390,12 @@ static void vrf_dellink(struct net_device *dev, struct list_head *head)
 }
 
 static int vrf_newlink(struct net *src_net, struct net_device *dev,
-		       struct nlattr *tb[], struct nlattr *data[])
+		       struct nlattr *tb[], struct nlattr *data[],
+		       struct netlink_ext_ack *extack)
 {
 	struct net_vrf *vrf = netdev_priv(dev);
+	bool *add_fib_rules;
+	struct net *net;
 	int err;
 
 	if (!data || !data[IFLA_VRF_TABLE])
@@ -1409,13 +1411,15 @@ static int vrf_newlink(struct net *src_net, struct net_device *dev,
 	if (err)
 		goto out;
 
-	if (add_fib_rules) {
+	net = dev_net(dev);
+	add_fib_rules = net_generic(net, vrf_net_id);
+	if (*add_fib_rules) {
 		err = vrf_add_fib_rules(dev);
 		if (err) {
 			unregister_netdevice(dev);
 			goto out;
 		}
-		add_fib_rules = false;
+		*add_fib_rules = false;
 	}
 
 out:
@@ -1498,16 +1502,38 @@ static struct notifier_block vrf_notifier_block __read_mostly = {
 	.notifier_call = vrf_device_event,
 };
 
+/* Initialize per network namespace state */
+static int __net_init vrf_netns_init(struct net *net)
+{
+	bool *add_fib_rules = net_generic(net, vrf_net_id);
+
+	*add_fib_rules = true;
+
+	return 0;
+}
+
+static struct pernet_operations vrf_net_ops __net_initdata = {
+	.init = vrf_netns_init,
+	.id   = &vrf_net_id,
+	.size = sizeof(bool),
+};
+
 static int __init vrf_init_module(void)
 {
 	int rc;
 
 	register_netdevice_notifier(&vrf_notifier_block);
 
-	rc = rtnl_link_register(&vrf_link_ops);
+	rc = register_pernet_subsys(&vrf_net_ops);
 	if (rc < 0)
 		goto error;
 
+	rc = rtnl_link_register(&vrf_link_ops);
+	if (rc < 0) {
+		unregister_pernet_subsys(&vrf_net_ops);
+		goto error;
+	}
+
 	return 0;
 
 error:
diff --git a/drivers/net/vsockmon.c b/drivers/net/vsockmon.c
index 7f0136f2dd9d..c28bdce14fd5 100644
--- a/drivers/net/vsockmon.c
+++ b/drivers/net/vsockmon.c
@@ -135,7 +135,7 @@ static void vsockmon_setup(struct net_device *dev)
 
 	dev->netdev_ops	= &vsockmon_ops;
 	dev->ethtool_ops = &vsockmon_ethtool_ops;
-	dev->destructor	= free_netdev;
+	dev->needs_free_netdev = true;
 
 	dev->features = NETIF_F_SG | NETIF_F_FRAGLIST |
 			NETIF_F_HIGHDMA | NETIF_F_LLTX;
diff --git a/drivers/net/vxlan.c b/drivers/net/vxlan.c
index a6b5052c1d36..96aa7e6cf214 100644
--- a/drivers/net/vxlan.c
+++ b/drivers/net/vxlan.c
@@ -226,26 +226,37 @@ static struct vxlan_sock *vxlan_find_sock(struct net *net, sa_family_t family,
 	return NULL;
 }
 
-static struct vxlan_dev *vxlan_vs_find_vni(struct vxlan_sock *vs, __be32 vni)
+static struct vxlan_dev *vxlan_vs_find_vni(struct vxlan_sock *vs, int ifindex,
+					   __be32 vni)
 {
-	struct vxlan_dev *vxlan;
+	struct vxlan_dev_node *node;
 
 	/* For flow based devices, map all packets to VNI 0 */
 	if (vs->flags & VXLAN_F_COLLECT_METADATA)
 		vni = 0;
 
-	hlist_for_each_entry_rcu(vxlan, vni_head(vs, vni), hlist) {
-		if (vxlan->default_dst.remote_vni == vni)
-			return vxlan;
+	hlist_for_each_entry_rcu(node, vni_head(vs, vni), hlist) {
+		if (node->vxlan->default_dst.remote_vni != vni)
+			continue;
+
+		if (IS_ENABLED(CONFIG_IPV6)) {
+			const struct vxlan_config *cfg = &node->vxlan->cfg;
+
+			if ((cfg->flags & VXLAN_F_IPV6_LINKLOCAL) &&
+			    cfg->remote_ifindex != ifindex)
+				continue;
+		}
+
+		return node->vxlan;
 	}
 
 	return NULL;
 }
 
 /* Look up VNI in a per net namespace table */
-static struct vxlan_dev *vxlan_find_vni(struct net *net, __be32 vni,
-					sa_family_t family, __be16 port,
-					u32 flags)
+static struct vxlan_dev *vxlan_find_vni(struct net *net, int ifindex,
+					__be32 vni, sa_family_t family,
+					__be16 port, u32 flags)
 {
 	struct vxlan_sock *vs;
 
@@ -253,7 +264,7 @@ static struct vxlan_dev *vxlan_find_vni(struct net *net, __be32 vni,
 	if (!vs)
 		return NULL;
 
-	return vxlan_vs_find_vni(vs, vni);
+	return vxlan_vs_find_vni(vs, ifindex, vni);
 }
 
 /* Fill in neighbour message in skbuff. */
@@ -305,7 +316,7 @@ static int vxlan_fdb_info(struct sk_buff *skb, struct vxlan_dev *vxlan,
 	if (rdst->remote_vni != vxlan->default_dst.remote_vni &&
 	    nla_put_u32(skb, NDA_VNI, be32_to_cpu(rdst->remote_vni)))
 		goto nla_put_failure;
-	if ((vxlan->flags & VXLAN_F_COLLECT_METADATA) && fdb->vni &&
+	if ((vxlan->cfg.flags & VXLAN_F_COLLECT_METADATA) && fdb->vni &&
 	    nla_put_u32(skb, NDA_SRC_VNI,
 			be32_to_cpu(fdb->vni)))
 		goto nla_put_failure;
@@ -419,7 +430,7 @@ static u32 eth_vni_hash(const unsigned char *addr, __be32 vni)
 static inline struct hlist_head *vxlan_fdb_head(struct vxlan_dev *vxlan,
 						const u8 *mac, __be32 vni)
 {
-	if (vxlan->flags & VXLAN_F_COLLECT_METADATA)
+	if (vxlan->cfg.flags & VXLAN_F_COLLECT_METADATA)
 		return &vxlan->fdb_head[eth_vni_hash(mac, vni)];
 	else
 		return &vxlan->fdb_head[eth_hash(mac)];
@@ -434,7 +445,7 @@ static struct vxlan_fdb *__vxlan_find_mac(struct vxlan_dev *vxlan,
 
 	hlist_for_each_entry_rcu(f, head, hlist) {
 		if (ether_addr_equal(mac, f->eth_addr)) {
-			if (vxlan->flags & VXLAN_F_COLLECT_METADATA) {
+			if (vxlan->cfg.flags & VXLAN_F_COLLECT_METADATA) {
 				if (vni == f->vni)
 					return f;
 			} else {
@@ -957,20 +968,28 @@ out:
  */
 static bool vxlan_snoop(struct net_device *dev,
 			union vxlan_addr *src_ip, const u8 *src_mac,
-			__be32 vni)
+			u32 src_ifindex, __be32 vni)
 {
 	struct vxlan_dev *vxlan = netdev_priv(dev);
 	struct vxlan_fdb *f;
+	u32 ifindex = 0;
+
+#if IS_ENABLED(CONFIG_IPV6)
+	if (src_ip->sa.sa_family == AF_INET6 &&
+	    (ipv6_addr_type(&src_ip->sin6.sin6_addr) & IPV6_ADDR_LINKLOCAL))
+		ifindex = src_ifindex;
+#endif
 
 	f = vxlan_find_mac(vxlan, src_mac, vni);
 	if (likely(f)) {
 		struct vxlan_rdst *rdst = first_remote_rcu(f);
 
-		if (likely(vxlan_addr_equal(&rdst->remote_ip, src_ip)))
+		if (likely(vxlan_addr_equal(&rdst->remote_ip, src_ip) &&
+			   rdst->remote_ifindex == ifindex))
 			return false;
 
 		/* Don't migrate static entries, drop packets */
-		if (f->state & NUD_NOARP)
+		if (f->state & (NUD_PERMANENT | NUD_NOARP))
 			return true;
 
 		if (net_ratelimit())
@@ -993,7 +1012,7 @@ static bool vxlan_snoop(struct net_device *dev,
 					 vxlan->cfg.dst_port,
 					 vni,
 					 vxlan->default_dst.remote_vni,
-					 0, NTF_SELF);
+					 ifindex, NTF_SELF);
 		spin_unlock(&vxlan->hash_lock);
 	}
 
@@ -1015,11 +1034,11 @@ static bool vxlan_group_used(struct vxlan_net *vn, struct vxlan_dev *dev)
 	/* The vxlan_sock is only used by dev, leaving group has
 	 * no effect on other vxlan devices.
 	 */
-	if (family == AF_INET && sock4 && atomic_read(&sock4->refcnt) == 1)
+	if (family == AF_INET && sock4 && refcount_read(&sock4->refcnt) == 1)
 		return false;
 #if IS_ENABLED(CONFIG_IPV6)
 	sock6 = rtnl_dereference(dev->vn6_sock);
-	if (family == AF_INET6 && sock6 && atomic_read(&sock6->refcnt) == 1)
+	if (family == AF_INET6 && sock6 && refcount_read(&sock6->refcnt) == 1)
 		return false;
 #endif
 
@@ -1056,7 +1075,7 @@ static bool __vxlan_sock_release_prep(struct vxlan_sock *vs)
 
 	if (!vs)
 		return false;
-	if (!atomic_dec_and_test(&vs->refcnt))
+	if (!refcount_dec_and_test(&vs->refcnt))
 		return false;
 
 	vn = net_generic(sock_net(vs->sock->sk), vxlan_net_id);
@@ -1077,10 +1096,10 @@ static void vxlan_sock_release(struct vxlan_dev *vxlan)
 #if IS_ENABLED(CONFIG_IPV6)
 	struct vxlan_sock *sock6 = rtnl_dereference(vxlan->vn6_sock);
 
-	rcu_assign_pointer(vxlan->vn6_sock, NULL);
+	RCU_INIT_POINTER(vxlan->vn6_sock, NULL);
 #endif
 
-	rcu_assign_pointer(vxlan->vn4_sock, NULL);
+	RCU_INIT_POINTER(vxlan->vn4_sock, NULL);
 	synchronize_net();
 
 	vxlan_vs_del_dev(vxlan);
@@ -1264,6 +1283,7 @@ static bool vxlan_set_mac(struct vxlan_dev *vxlan,
 			  struct sk_buff *skb, __be32 vni)
 {
 	union vxlan_addr saddr;
+	u32 ifindex = skb->dev->ifindex;
 
 	skb_reset_mac_header(skb);
 	skb->protocol = eth_type_trans(skb, vxlan->dev);
@@ -1284,8 +1304,8 @@ static bool vxlan_set_mac(struct vxlan_dev *vxlan,
 #endif
 	}
 
-	if ((vxlan->flags & VXLAN_F_LEARN) &&
-	    vxlan_snoop(skb->dev, &saddr, eth_hdr(skb)->h_source, vni))
+	if ((vxlan->cfg.flags & VXLAN_F_LEARN) &&
+	    vxlan_snoop(skb->dev, &saddr, eth_hdr(skb)->h_source, ifindex, vni))
 		return false;
 
 	return true;
@@ -1351,7 +1371,7 @@ static int vxlan_rcv(struct sock *sk, struct sk_buff *skb)
 
 	vni = vxlan_vni(vxlan_hdr(skb)->vx_vni);
 
-	vxlan = vxlan_vs_find_vni(vs, vni);
+	vxlan = vxlan_vs_find_vni(vs, skb->dev->ifindex, vni);
 	if (!vxlan)
 		goto drop;
 
@@ -1507,7 +1527,7 @@ static int arp_reduce(struct net_device *dev, struct sk_buff *skb, __be32 vni)
 
 		if (netif_rx_ni(reply) == NET_RX_DROP)
 			dev->stats.rx_dropped++;
-	} else if (vxlan->flags & VXLAN_F_L3MISS) {
+	} else if (vxlan->cfg.flags & VXLAN_F_L3MISS) {
 		union vxlan_addr ipa = {
 			.sin.sin_addr.s_addr = tip,
 			.sin.sin_family = AF_INET,
@@ -1584,10 +1604,8 @@ static struct sk_buff *vxlan_na_create(struct sk_buff *request,
 	skb_pull(reply, sizeof(struct ipv6hdr));
 	skb_reset_transport_header(reply);
 
-	na = (struct nd_msg *)skb_put(reply, sizeof(*na) + na_olen);
-
 	/* Neighbor Advertisement */
-	memset(na, 0, sizeof(*na)+na_olen);
+	na = skb_put_zero(reply, sizeof(*na) + na_olen);
 	na->icmph.icmp6_type = NDISC_NEIGHBOUR_ADVERTISEMENT;
 	na->icmph.icmp6_router = isrouter;
 	na->icmph.icmp6_override = 1;
@@ -1667,7 +1685,7 @@ static int neigh_reduce(struct net_device *dev, struct sk_buff *skb, __be32 vni)
 		if (netif_rx_ni(reply) == NET_RX_DROP)
 			dev->stats.rx_dropped++;
 
-	} else if (vxlan->flags & VXLAN_F_L3MISS) {
+	} else if (vxlan->cfg.flags & VXLAN_F_L3MISS) {
 		union vxlan_addr ipa = {
 			.sin6.sin6_addr = msg->target,
 			.sin6.sin6_family = AF_INET6,
@@ -1700,7 +1718,7 @@ static bool route_shortcircuit(struct net_device *dev, struct sk_buff *skb)
 			return false;
 		pip = ip_hdr(skb);
 		n = neigh_lookup(&arp_tbl, &pip->daddr, dev);
-		if (!n && (vxlan->flags & VXLAN_F_L3MISS)) {
+		if (!n && (vxlan->cfg.flags & VXLAN_F_L3MISS)) {
 			union vxlan_addr ipa = {
 				.sin.sin_addr.s_addr = pip->daddr,
 				.sin.sin_family = AF_INET,
@@ -1721,7 +1739,7 @@ static bool route_shortcircuit(struct net_device *dev, struct sk_buff *skb)
 			return false;
 		pip6 = ipv6_hdr(skb);
 		n = neigh_lookup(ipv6_stub->nd_tbl, &pip6->daddr, dev);
-		if (!n && (vxlan->flags & VXLAN_F_L3MISS)) {
+		if (!n && (vxlan->cfg.flags & VXLAN_F_L3MISS)) {
 			union vxlan_addr ipa = {
 				.sin6.sin6_addr = pip6->daddr,
 				.sin6.sin6_family = AF_INET6,
@@ -1829,7 +1847,7 @@ static int vxlan_build_skb(struct sk_buff *skb, struct dst_entry *dst,
 	if (err)
 		return err;
 
-	vxh = (struct vxlanhdr *) __skb_push(skb, sizeof(*vxh));
+	vxh = __skb_push(skb, sizeof(*vxh));
 	vxh->vx_flags = VXLAN_HF_VNI;
 	vxh->vx_vni = vxlan_vni_field(vni);
 
@@ -1995,8 +2013,9 @@ static void vxlan_encap_bypass(struct sk_buff *skb, struct vxlan_dev *src_vxlan,
 #endif
 	}
 
-	if (dst_vxlan->flags & VXLAN_F_LEARN)
-		vxlan_snoop(skb->dev, &loopback, eth_hdr(skb)->h_source, vni);
+	if (dst_vxlan->cfg.flags & VXLAN_F_LEARN)
+		vxlan_snoop(skb->dev, &loopback, eth_hdr(skb)->h_source, 0,
+			    vni);
 
 	u64_stats_update_begin(&tx_stats->syncp);
 	tx_stats->tx_packets++;
@@ -2014,8 +2033,10 @@ static void vxlan_encap_bypass(struct sk_buff *skb, struct vxlan_dev *src_vxlan,
 }
 
 static int encap_bypass_if_local(struct sk_buff *skb, struct net_device *dev,
-				 struct vxlan_dev *vxlan, union vxlan_addr *daddr,
-				 __be16 dst_port, __be32 vni, struct dst_entry *dst,
+				 struct vxlan_dev *vxlan,
+				 union vxlan_addr *daddr,
+				 __be16 dst_port, int dst_ifindex, __be32 vni,
+				 struct dst_entry *dst,
 				 u32 rt_flags)
 {
 #if IS_ENABLED(CONFIG_IPV6)
@@ -2031,9 +2052,9 @@ static int encap_bypass_if_local(struct sk_buff *skb, struct net_device *dev,
 		struct vxlan_dev *dst_vxlan;
 
 		dst_release(dst);
-		dst_vxlan = vxlan_find_vni(vxlan->net, vni,
+		dst_vxlan = vxlan_find_vni(vxlan->net, dst_ifindex, vni,
 					   daddr->sa.sa_family, dst_port,
-					   vxlan->flags);
+					   vxlan->cfg.flags);
 		if (!dst_vxlan) {
 			dev->stats.tx_errors++;
 			kfree_skb(skb);
@@ -2063,8 +2084,9 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 	struct dst_entry *ndst = NULL;
 	__be32 vni, label;
 	__u8 tos, ttl;
+	int ifindex;
 	int err;
-	u32 flags = vxlan->flags;
+	u32 flags = vxlan->cfg.flags;
 	bool udp_sum = false;
 	bool xnet = !net_eq(vxlan->net, dev_net(vxlan->dev));
 
@@ -2083,6 +2105,7 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 
 		dst_port = rdst->remote_port ? rdst->remote_port : vxlan->cfg.dst_port;
 		vni = (rdst->remote_vni) ? : default_vni;
+		ifindex = rdst->remote_ifindex;
 		local_ip = vxlan->cfg.saddr;
 		dst_cache = &rdst->dst_cache;
 		md->gbp = skb->mark;
@@ -2116,6 +2139,7 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 		dst = &remote_ip;
 		dst_port = info->key.tp_dst ? : vxlan->cfg.dst_port;
 		vni = tunnel_id_to_key32(info->key.tun_id);
+		ifindex = 0;
 		dst_cache = &info->dst_cache;
 		if (info->options_len)
 			md = ip_tunnel_info_opts(info);
@@ -2133,8 +2157,7 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 		struct rtable *rt;
 		__be16 df = 0;
 
-		rt = vxlan_get_route(vxlan, dev, sock4, skb,
-				     rdst ? rdst->remote_ifindex : 0, tos,
+		rt = vxlan_get_route(vxlan, dev, sock4, skb, ifindex, tos,
 				     dst->sin.sin_addr.s_addr,
 				     &local_ip.sin.sin_addr.s_addr,
 				     dst_port, src_port,
@@ -2147,8 +2170,8 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 		/* Bypass encapsulation if the destination is local */
 		if (!info) {
 			err = encap_bypass_if_local(skb, dev, vxlan, dst,
-						    dst_port, vni, &rt->dst,
-						    rt->rt_flags);
+						    dst_port, ifindex, vni,
+						    &rt->dst, rt->rt_flags);
 			if (err)
 				goto out_unlock;
 		} else if (info->key.tun_flags & TUNNEL_DONT_FRAGMENT) {
@@ -2170,8 +2193,7 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 	} else {
 		struct vxlan_sock *sock6 = rcu_dereference(vxlan->vn6_sock);
 
-		ndst = vxlan6_get_route(vxlan, dev, sock6, skb,
-					rdst ? rdst->remote_ifindex : 0, tos,
+		ndst = vxlan6_get_route(vxlan, dev, sock6, skb, ifindex, tos,
 					label, &dst->sin6.sin6_addr,
 					&local_ip.sin6.sin6_addr,
 					dst_port, src_port,
@@ -2186,8 +2208,8 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 			u32 rt6i_flags = ((struct rt6_info *)ndst)->rt6i_flags;
 
 			err = encap_bypass_if_local(skb, dev, vxlan, dst,
-						    dst_port, vni, ndst,
-						    rt6i_flags);
+						    dst_port, ifindex, vni,
+						    ndst, rt6i_flags);
 			if (err)
 				goto out_unlock;
 		}
@@ -2246,7 +2268,7 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	skb_reset_mac_header(skb);
 
-	if (vxlan->flags & VXLAN_F_COLLECT_METADATA) {
+	if (vxlan->cfg.flags & VXLAN_F_COLLECT_METADATA) {
 		if (info && info->mode & IP_TUNNEL_INFO_BRIDGE &&
 		    info->mode & IP_TUNNEL_INFO_TX) {
 			vni = tunnel_id_to_key32(info->key.tun_id);
@@ -2259,7 +2281,7 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 		}
 	}
 
-	if (vxlan->flags & VXLAN_F_PROXY) {
+	if (vxlan->cfg.flags & VXLAN_F_PROXY) {
 		eth = eth_hdr(skb);
 		if (ntohs(eth->h_proto) == ETH_P_ARP)
 			return arp_reduce(dev, skb, vni);
@@ -2279,7 +2301,7 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 	f = vxlan_find_mac(vxlan, eth->h_dest, vni);
 	did_rsc = false;
 
-	if (f && (f->flags & NTF_ROUTER) && (vxlan->flags & VXLAN_F_RSC) &&
+	if (f && (f->flags & NTF_ROUTER) && (vxlan->cfg.flags & VXLAN_F_RSC) &&
 	    (ntohs(eth->h_proto) == ETH_P_IP ||
 	     ntohs(eth->h_proto) == ETH_P_IPV6)) {
 		did_rsc = route_shortcircuit(dev, skb);
@@ -2290,7 +2312,7 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (f == NULL) {
 		f = vxlan_find_mac(vxlan, all_zeros_mac, vni);
 		if (f == NULL) {
-			if ((vxlan->flags & VXLAN_F_L2MISS) &&
+			if ((vxlan->cfg.flags & VXLAN_F_L2MISS) &&
 			    !is_multicast_ether_addr(eth->h_dest))
 				vxlan_fdb_miss(vxlan, eth->h_dest);
 
@@ -2365,17 +2387,22 @@ static void vxlan_vs_del_dev(struct vxlan_dev *vxlan)
 	struct vxlan_net *vn = net_generic(vxlan->net, vxlan_net_id);
 
 	spin_lock(&vn->sock_lock);
-	hlist_del_init_rcu(&vxlan->hlist);
+	hlist_del_init_rcu(&vxlan->hlist4.hlist);
+#if IS_ENABLED(CONFIG_IPV6)
+	hlist_del_init_rcu(&vxlan->hlist6.hlist);
+#endif
 	spin_unlock(&vn->sock_lock);
 }
 
-static void vxlan_vs_add_dev(struct vxlan_sock *vs, struct vxlan_dev *vxlan)
+static void vxlan_vs_add_dev(struct vxlan_sock *vs, struct vxlan_dev *vxlan,
+			     struct vxlan_dev_node *node)
 {
 	struct vxlan_net *vn = net_generic(vxlan->net, vxlan_net_id);
 	__be32 vni = vxlan->default_dst.remote_vni;
 
+	node->vxlan = vxlan;
 	spin_lock(&vn->sock_lock);
-	hlist_add_head_rcu(&vxlan->hlist, vni_head(vs, vni));
+	hlist_add_head_rcu(&node->hlist, vni_head(vs, vni));
 	spin_unlock(&vn->sock_lock);
 }
 
@@ -2486,10 +2513,7 @@ static int vxlan_change_mtu(struct net_device *dev, int new_mtu)
 	struct vxlan_rdst *dst = &vxlan->default_dst;
 	struct net_device *lowerdev = __dev_get_by_index(vxlan->net,
 							 dst->remote_ifindex);
-	bool use_ipv6 = false;
-
-	if (dst->remote_ip.sa.sa_family == AF_INET6)
-		use_ipv6 = true;
+	bool use_ipv6 = !!(vxlan->cfg.flags & VXLAN_F_IPV6);
 
 	/* This check is different than dev->max_mtu, because it looks at
 	 * the lowerdev->mtu, rather than the static dev->max_mtu
@@ -2611,7 +2635,7 @@ static void vxlan_setup(struct net_device *dev)
 	eth_hw_addr_random(dev);
 	ether_setup(dev);
 
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 	SET_NETDEV_DEVTYPE(dev, &vxlan_type);
 
 	dev->features	|= NETIF_F_LLTX;
@@ -2625,6 +2649,10 @@ static void vxlan_setup(struct net_device *dev)
 	netif_keep_dst(dev);
 	dev->priv_flags |= IFF_NO_QUEUE;
 
+	/* MTU range: 68 - 65535 */
+	dev->min_mtu = ETH_MIN_MTU;
+	dev->max_mtu = ETH_MAX_MTU;
+
 	INIT_LIST_HEAD(&vxlan->next);
 	spin_lock_init(&vxlan->hash_lock);
 
@@ -2632,8 +2660,6 @@ static void vxlan_setup(struct net_device *dev)
 	vxlan->age_timer.function = vxlan_cleanup;
 	vxlan->age_timer.data = (unsigned long) vxlan;
 
-	vxlan->cfg.dst_port = htons(vxlan_port);
-
 	vxlan->dev = dev;
 
 	gro_cells_init(&vxlan->gro_cells, dev);
@@ -2689,7 +2715,8 @@ static const struct nla_policy vxlan_policy[IFLA_VXLAN_MAX + 1] = {
 	[IFLA_VXLAN_REMCSUM_NOPARTIAL]	= { .type = NLA_FLAG },
 };
 
-static int vxlan_validate(struct nlattr *tb[], struct nlattr *data[])
+static int vxlan_validate(struct nlattr *tb[], struct nlattr *data[],
+			  struct netlink_ext_ack *extack)
 {
 	if (tb[IFLA_ADDRESS]) {
 		if (nla_len(tb[IFLA_ADDRESS]) != ETH_ALEN) {
@@ -2703,11 +2730,19 @@ static int vxlan_validate(struct nlattr *tb[], struct nlattr *data[])
 		}
 	}
 
+	if (tb[IFLA_MTU]) {
+		u32 mtu = nla_get_u32(tb[IFLA_MTU]);
+
+		if (mtu < ETH_MIN_MTU || mtu > ETH_MAX_MTU)
+			return -EINVAL;
+	}
+
 	if (!data)
 		return -EINVAL;
 
 	if (data[IFLA_VXLAN_ID]) {
-		__u32 id = nla_get_u32(data[IFLA_VXLAN_ID]);
+		u32 id = nla_get_u32(data[IFLA_VXLAN_ID]);
+
 		if (id >= VXLAN_N_VID)
 			return -ERANGE;
 	}
@@ -2790,7 +2825,7 @@ static struct vxlan_sock *vxlan_socket_create(struct net *net, bool ipv6,
 	}
 
 	vs->sock = sock;
-	atomic_set(&vs->refcnt, 1);
+	refcount_set(&vs->refcnt, 1);
 	vs->flags = (flags & VXLAN_F_RCV_FLAGS);
 
 	spin_lock(&vn->sock_lock);
@@ -2819,12 +2854,13 @@ static int __vxlan_sock_add(struct vxlan_dev *vxlan, bool ipv6)
 {
 	struct vxlan_net *vn = net_generic(vxlan->net, vxlan_net_id);
 	struct vxlan_sock *vs = NULL;
+	struct vxlan_dev_node *node;
 
 	if (!vxlan->cfg.no_share) {
 		spin_lock(&vn->sock_lock);
 		vs = vxlan_find_sock(vxlan->net, ipv6 ? AF_INET6 : AF_INET,
-				     vxlan->cfg.dst_port, vxlan->flags);
-		if (vs && !atomic_add_unless(&vs->refcnt, 1, 0)) {
+				     vxlan->cfg.dst_port, vxlan->cfg.flags);
+		if (vs && !refcount_inc_not_zero(&vs->refcnt)) {
 			spin_unlock(&vn->sock_lock);
 			return -EBUSY;
 		}
@@ -2832,23 +2868,27 @@ static int __vxlan_sock_add(struct vxlan_dev *vxlan, bool ipv6)
 	}
 	if (!vs)
 		vs = vxlan_socket_create(vxlan->net, ipv6,
-					 vxlan->cfg.dst_port, vxlan->flags);
+					 vxlan->cfg.dst_port, vxlan->cfg.flags);
 	if (IS_ERR(vs))
 		return PTR_ERR(vs);
 #if IS_ENABLED(CONFIG_IPV6)
-	if (ipv6)
+	if (ipv6) {
 		rcu_assign_pointer(vxlan->vn6_sock, vs);
-	else
+		node = &vxlan->hlist6;
+	} else
 #endif
+	{
 		rcu_assign_pointer(vxlan->vn4_sock, vs);
-	vxlan_vs_add_dev(vs, vxlan);
+		node = &vxlan->hlist4;
+	}
+	vxlan_vs_add_dev(vs, vxlan, node);
 	return 0;
 }
 
 static int vxlan_sock_add(struct vxlan_dev *vxlan)
 {
-	bool metadata = vxlan->flags & VXLAN_F_COLLECT_METADATA;
-	bool ipv6 = vxlan->flags & VXLAN_F_IPV6 || metadata;
+	bool metadata = vxlan->cfg.flags & VXLAN_F_COLLECT_METADATA;
+	bool ipv6 = vxlan->cfg.flags & VXLAN_F_IPV6 || metadata;
 	bool ipv4 = !ipv6 || metadata;
 	int ret = 0;
 
@@ -2868,116 +2908,176 @@ static int vxlan_sock_add(struct vxlan_dev *vxlan)
 	return ret;
 }
 
-static int vxlan_dev_configure(struct net *src_net, struct net_device *dev,
-			       struct vxlan_config *conf,
-			       bool changelink)
+static int vxlan_config_validate(struct net *src_net, struct vxlan_config *conf,
+				 struct net_device **lower,
+				 struct vxlan_dev *old)
 {
 	struct vxlan_net *vn = net_generic(src_net, vxlan_net_id);
-	struct vxlan_dev *vxlan = netdev_priv(dev), *tmp;
-	struct vxlan_rdst *dst = &vxlan->default_dst;
-	unsigned short needed_headroom = ETH_HLEN;
+	struct vxlan_dev *tmp;
 	bool use_ipv6 = false;
-	__be16 default_port = vxlan->cfg.dst_port;
-	struct net_device *lowerdev = NULL;
 
-	if (!changelink) {
-		if (conf->flags & VXLAN_F_GPE) {
-			/* For now, allow GPE only together with
-			 * COLLECT_METADATA. This can be relaxed later; in such
-			 * case, the other side of the PtP link will have to be
-			 * provided.
-			 */
-			if ((conf->flags & ~VXLAN_F_ALLOWED_GPE) ||
-			    !(conf->flags & VXLAN_F_COLLECT_METADATA)) {
-				pr_info("unsupported combination of extensions\n");
-				return -EINVAL;
-			}
-			vxlan_raw_setup(dev);
-		} else {
-			vxlan_ether_setup(dev);
+	if (conf->flags & VXLAN_F_GPE) {
+		/* For now, allow GPE only together with
+		 * COLLECT_METADATA. This can be relaxed later; in such
+		 * case, the other side of the PtP link will have to be
+		 * provided.
+		 */
+		if ((conf->flags & ~VXLAN_F_ALLOWED_GPE) ||
+		    !(conf->flags & VXLAN_F_COLLECT_METADATA)) {
+			return -EINVAL;
 		}
-
-		/* MTU range: 68 - 65535 */
-		dev->min_mtu = ETH_MIN_MTU;
-		dev->max_mtu = ETH_MAX_MTU;
-		vxlan->net = src_net;
 	}
 
-	dst->remote_vni = conf->vni;
+	if (!conf->remote_ip.sa.sa_family && !conf->saddr.sa.sa_family) {
+		/* Unless IPv6 is explicitly requested, assume IPv4 */
+		conf->remote_ip.sa.sa_family = AF_INET;
+		conf->saddr.sa.sa_family = AF_INET;
+	} else if (!conf->remote_ip.sa.sa_family) {
+		conf->remote_ip.sa.sa_family = conf->saddr.sa.sa_family;
+	} else if (!conf->saddr.sa.sa_family) {
+		conf->saddr.sa.sa_family = conf->remote_ip.sa.sa_family;
+	}
 
-	memcpy(&dst->remote_ip, &conf->remote_ip, sizeof(conf->remote_ip));
+	if (conf->saddr.sa.sa_family != conf->remote_ip.sa.sa_family)
+		return -EINVAL;
 
-	/* Unless IPv6 is explicitly requested, assume IPv4 */
-	if (!dst->remote_ip.sa.sa_family)
-		dst->remote_ip.sa.sa_family = AF_INET;
+	if (vxlan_addr_multicast(&conf->saddr))
+		return -EINVAL;
 
-	if (dst->remote_ip.sa.sa_family == AF_INET6 ||
-	    vxlan->cfg.saddr.sa.sa_family == AF_INET6) {
+	if (conf->saddr.sa.sa_family == AF_INET6) {
 		if (!IS_ENABLED(CONFIG_IPV6))
 			return -EPFNOSUPPORT;
 		use_ipv6 = true;
-		vxlan->flags |= VXLAN_F_IPV6;
+		conf->flags |= VXLAN_F_IPV6;
+
+		if (!(conf->flags & VXLAN_F_COLLECT_METADATA)) {
+			int local_type =
+				ipv6_addr_type(&conf->saddr.sin6.sin6_addr);
+			int remote_type =
+				ipv6_addr_type(&conf->remote_ip.sin6.sin6_addr);
+
+			if (local_type & IPV6_ADDR_LINKLOCAL) {
+				if (!(remote_type & IPV6_ADDR_LINKLOCAL) &&
+				    (remote_type != IPV6_ADDR_ANY))
+					return -EINVAL;
+
+				conf->flags |= VXLAN_F_IPV6_LINKLOCAL;
+			} else {
+				if (remote_type ==
+				    (IPV6_ADDR_UNICAST | IPV6_ADDR_LINKLOCAL))
+					return -EINVAL;
+
+				conf->flags &= ~VXLAN_F_IPV6_LINKLOCAL;
+			}
+		}
 	}
 
-	if (conf->label && !use_ipv6) {
-		pr_info("label only supported in use with IPv6\n");
+	if (conf->label && !use_ipv6)
 		return -EINVAL;
-	}
 
-	if (conf->remote_ifindex &&
-	    conf->remote_ifindex != vxlan->cfg.remote_ifindex) {
-		lowerdev = __dev_get_by_index(src_net, conf->remote_ifindex);
-		dst->remote_ifindex = conf->remote_ifindex;
+	if (conf->remote_ifindex) {
+		struct net_device *lowerdev;
 
-		if (!lowerdev) {
-			pr_info("ifindex %d does not exist\n",
-				dst->remote_ifindex);
+		lowerdev = __dev_get_by_index(src_net, conf->remote_ifindex);
+		if (!lowerdev)
 			return -ENODEV;
-		}
 
 #if IS_ENABLED(CONFIG_IPV6)
 		if (use_ipv6) {
 			struct inet6_dev *idev = __in6_dev_get(lowerdev);
-			if (idev && idev->cnf.disable_ipv6) {
-				pr_info("IPv6 is disabled via sysctl\n");
+			if (idev && idev->cnf.disable_ipv6)
 				return -EPERM;
-			}
 		}
 #endif
 
-		if (!conf->mtu)
-			dev->mtu = lowerdev->mtu -
-				   (use_ipv6 ? VXLAN6_HEADROOM : VXLAN_HEADROOM);
+		*lower = lowerdev;
+	} else {
+		if (vxlan_addr_multicast(&conf->remote_ip))
+			return -EINVAL;
 
-		needed_headroom = lowerdev->hard_header_len;
-	} else if (!conf->remote_ifindex &&
-		   vxlan_addr_multicast(&dst->remote_ip)) {
-		pr_info("multicast destination requires interface to be specified\n");
-		return -EINVAL;
+#if IS_ENABLED(CONFIG_IPV6)
+		if (conf->flags & VXLAN_F_IPV6_LINKLOCAL)
+			return -EINVAL;
+#endif
+
+		*lower = NULL;
 	}
 
-	if (lowerdev) {
-		dev->gso_max_size = lowerdev->gso_max_size;
-		dev->gso_max_segs = lowerdev->gso_max_segs;
+	if (!conf->dst_port) {
+		if (conf->flags & VXLAN_F_GPE)
+			conf->dst_port = htons(4790); /* IANA VXLAN-GPE port */
+		else
+			conf->dst_port = htons(vxlan_port);
 	}
 
-	if (conf->mtu) {
-		int max_mtu = ETH_MAX_MTU;
+	if (!conf->age_interval)
+		conf->age_interval = FDB_AGE_DEFAULT;
 
-		if (lowerdev)
-			max_mtu = lowerdev->mtu;
+	list_for_each_entry(tmp, &vn->vxlan_list, next) {
+		if (tmp == old)
+			continue;
 
-		max_mtu -= (use_ipv6 ? VXLAN6_HEADROOM : VXLAN_HEADROOM);
+		if (tmp->cfg.vni != conf->vni)
+			continue;
+		if (tmp->cfg.dst_port != conf->dst_port)
+			continue;
+		if ((tmp->cfg.flags & (VXLAN_F_RCV_FLAGS | VXLAN_F_IPV6)) !=
+		    (conf->flags & (VXLAN_F_RCV_FLAGS | VXLAN_F_IPV6)))
+			continue;
 
-		if (conf->mtu < dev->min_mtu || conf->mtu > dev->max_mtu)
-			return -EINVAL;
+		if ((conf->flags & VXLAN_F_IPV6_LINKLOCAL) &&
+		    tmp->cfg.remote_ifindex != conf->remote_ifindex)
+			continue;
+
+		return -EEXIST;
+	}
 
-		dev->mtu = conf->mtu;
+	return 0;
+}
 
-		if (conf->mtu > max_mtu)
-			dev->mtu = max_mtu;
+static void vxlan_config_apply(struct net_device *dev,
+			       struct vxlan_config *conf,
+			       struct net_device *lowerdev,
+			       struct net *src_net,
+			       bool changelink)
+{
+	struct vxlan_dev *vxlan = netdev_priv(dev);
+	struct vxlan_rdst *dst = &vxlan->default_dst;
+	unsigned short needed_headroom = ETH_HLEN;
+	bool use_ipv6 = !!(conf->flags & VXLAN_F_IPV6);
+	int max_mtu = ETH_MAX_MTU;
+
+	if (!changelink) {
+		if (conf->flags & VXLAN_F_GPE)
+			vxlan_raw_setup(dev);
+		else
+			vxlan_ether_setup(dev);
+
+		if (conf->mtu)
+			dev->mtu = conf->mtu;
+
+		vxlan->net = src_net;
 	}
 
+	dst->remote_vni = conf->vni;
+
+	memcpy(&dst->remote_ip, &conf->remote_ip, sizeof(conf->remote_ip));
+
+	if (lowerdev) {
+		dst->remote_ifindex = conf->remote_ifindex;
+
+		dev->gso_max_size = lowerdev->gso_max_size;
+		dev->gso_max_segs = lowerdev->gso_max_segs;
+
+		needed_headroom = lowerdev->hard_header_len;
+
+		max_mtu = lowerdev->mtu - (use_ipv6 ? VXLAN6_HEADROOM :
+					   VXLAN_HEADROOM);
+	}
+
+	if (dev->mtu > max_mtu)
+		dev->mtu = max_mtu;
+
 	if (use_ipv6 || conf->flags & VXLAN_F_COLLECT_METADATA)
 		needed_headroom += VXLAN6_HEADROOM;
 	else
@@ -2985,31 +3085,21 @@ static int vxlan_dev_configure(struct net *src_net, struct net_device *dev,
 	dev->needed_headroom = needed_headroom;
 
 	memcpy(&vxlan->cfg, conf, sizeof(*conf));
-	if (!vxlan->cfg.dst_port) {
-		if (conf->flags & VXLAN_F_GPE)
-			vxlan->cfg.dst_port = htons(4790); /* IANA VXLAN-GPE port */
-		else
-			vxlan->cfg.dst_port = default_port;
-	}
-	vxlan->flags |= conf->flags;
+}
 
-	if (!vxlan->cfg.age_interval)
-		vxlan->cfg.age_interval = FDB_AGE_DEFAULT;
+static int vxlan_dev_configure(struct net *src_net, struct net_device *dev,
+			       struct vxlan_config *conf,
+			       bool changelink)
+{
+	struct vxlan_dev *vxlan = netdev_priv(dev);
+	struct net_device *lowerdev;
+	int ret;
 
-	if (changelink)
-		return 0;
+	ret = vxlan_config_validate(src_net, conf, &lowerdev, vxlan);
+	if (ret)
+		return ret;
 
-	list_for_each_entry(tmp, &vn->vxlan_list, next) {
-		if (tmp->cfg.vni == conf->vni &&
-		    (tmp->default_dst.remote_ip.sa.sa_family == AF_INET6 ||
-		     tmp->cfg.saddr.sa.sa_family == AF_INET6) == use_ipv6 &&
-		    tmp->cfg.dst_port == vxlan->cfg.dst_port &&
-		    (tmp->flags & VXLAN_F_RCV_FLAGS) ==
-		    (vxlan->flags & VXLAN_F_RCV_FLAGS)) {
-			pr_info("duplicate VNI %u\n", be32_to_cpu(conf->vni));
-			return -EEXIST;
-		}
-	}
+	vxlan_config_apply(dev, conf, lowerdev, src_net, changelink);
 
 	return 0;
 }
@@ -3073,22 +3163,35 @@ static int vxlan_nl2conf(struct nlattr *tb[], struct nlattr *data[],
 	}
 
 	if (data[IFLA_VXLAN_GROUP]) {
+		if (changelink && (conf->remote_ip.sa.sa_family != AF_INET))
+			return -EOPNOTSUPP;
+
 		conf->remote_ip.sin.sin_addr.s_addr = nla_get_in_addr(data[IFLA_VXLAN_GROUP]);
+		conf->remote_ip.sa.sa_family = AF_INET;
 	} else if (data[IFLA_VXLAN_GROUP6]) {
 		if (!IS_ENABLED(CONFIG_IPV6))
 			return -EPFNOSUPPORT;
 
+		if (changelink && (conf->remote_ip.sa.sa_family != AF_INET6))
+			return -EOPNOTSUPP;
+
 		conf->remote_ip.sin6.sin6_addr = nla_get_in6_addr(data[IFLA_VXLAN_GROUP6]);
 		conf->remote_ip.sa.sa_family = AF_INET6;
 	}
 
 	if (data[IFLA_VXLAN_LOCAL]) {
+		if (changelink && (conf->saddr.sa.sa_family != AF_INET))
+			return -EOPNOTSUPP;
+
 		conf->saddr.sin.sin_addr.s_addr = nla_get_in_addr(data[IFLA_VXLAN_LOCAL]);
 		conf->saddr.sa.sa_family = AF_INET;
 	} else if (data[IFLA_VXLAN_LOCAL6]) {
 		if (!IS_ENABLED(CONFIG_IPV6))
 			return -EPFNOSUPPORT;
 
+		if (changelink && (conf->saddr.sa.sa_family != AF_INET6))
+			return -EOPNOTSUPP;
+
 		/* TODO: respect scope id */
 		conf->saddr.sin6.sin6_addr = nla_get_in6_addr(data[IFLA_VXLAN_LOCAL6]);
 		conf->saddr.sa.sa_family = AF_INET6;
@@ -3108,12 +3211,10 @@ static int vxlan_nl2conf(struct nlattr *tb[], struct nlattr *data[],
 			     IPV6_FLOWLABEL_MASK;
 
 	if (data[IFLA_VXLAN_LEARNING]) {
-		if (nla_get_u8(data[IFLA_VXLAN_LEARNING])) {
+		if (nla_get_u8(data[IFLA_VXLAN_LEARNING]))
 			conf->flags |= VXLAN_F_LEARN;
-		} else {
+		else
 			conf->flags &= ~VXLAN_F_LEARN;
-			vxlan->flags &= ~VXLAN_F_LEARN;
-		}
 	} else if (!changelink) {
 		/* default to learn on a new device */
 		conf->flags |= VXLAN_F_LEARN;
@@ -3246,7 +3347,8 @@ static int vxlan_nl2conf(struct nlattr *tb[], struct nlattr *data[],
 }
 
 static int vxlan_newlink(struct net *src_net, struct net_device *dev,
-			 struct nlattr *tb[], struct nlattr *data[])
+			 struct nlattr *tb[], struct nlattr *data[],
+			 struct netlink_ext_ack *extack)
 {
 	struct vxlan_config conf;
 	int err;
@@ -3259,7 +3361,8 @@ static int vxlan_newlink(struct net *src_net, struct net_device *dev,
 }
 
 static int vxlan_changelink(struct net_device *dev, struct nlattr *tb[],
-			    struct nlattr *data[])
+			    struct nlattr *data[],
+			    struct netlink_ext_ack *extack)
 {
 	struct vxlan_dev *vxlan = netdev_priv(dev);
 	struct vxlan_rdst *dst = &vxlan->default_dst;
@@ -3396,43 +3499,44 @@ static int vxlan_fill_info(struct sk_buff *skb, const struct net_device *dev)
 	    nla_put_u8(skb, IFLA_VXLAN_TOS, vxlan->cfg.tos) ||
 	    nla_put_be32(skb, IFLA_VXLAN_LABEL, vxlan->cfg.label) ||
 	    nla_put_u8(skb, IFLA_VXLAN_LEARNING,
-			!!(vxlan->flags & VXLAN_F_LEARN)) ||
+			!!(vxlan->cfg.flags & VXLAN_F_LEARN)) ||
 	    nla_put_u8(skb, IFLA_VXLAN_PROXY,
-			!!(vxlan->flags & VXLAN_F_PROXY)) ||
-	    nla_put_u8(skb, IFLA_VXLAN_RSC, !!(vxlan->flags & VXLAN_F_RSC)) ||
+			!!(vxlan->cfg.flags & VXLAN_F_PROXY)) ||
+	    nla_put_u8(skb, IFLA_VXLAN_RSC,
+		       !!(vxlan->cfg.flags & VXLAN_F_RSC)) ||
 	    nla_put_u8(skb, IFLA_VXLAN_L2MISS,
-			!!(vxlan->flags & VXLAN_F_L2MISS)) ||
+			!!(vxlan->cfg.flags & VXLAN_F_L2MISS)) ||
 	    nla_put_u8(skb, IFLA_VXLAN_L3MISS,
-			!!(vxlan->flags & VXLAN_F_L3MISS)) ||
+			!!(vxlan->cfg.flags & VXLAN_F_L3MISS)) ||
 	    nla_put_u8(skb, IFLA_VXLAN_COLLECT_METADATA,
-		       !!(vxlan->flags & VXLAN_F_COLLECT_METADATA)) ||
+		       !!(vxlan->cfg.flags & VXLAN_F_COLLECT_METADATA)) ||
 	    nla_put_u32(skb, IFLA_VXLAN_AGEING, vxlan->cfg.age_interval) ||
 	    nla_put_u32(skb, IFLA_VXLAN_LIMIT, vxlan->cfg.addrmax) ||
 	    nla_put_be16(skb, IFLA_VXLAN_PORT, vxlan->cfg.dst_port) ||
 	    nla_put_u8(skb, IFLA_VXLAN_UDP_CSUM,
-			!(vxlan->flags & VXLAN_F_UDP_ZERO_CSUM_TX)) ||
+			!(vxlan->cfg.flags & VXLAN_F_UDP_ZERO_CSUM_TX)) ||
 	    nla_put_u8(skb, IFLA_VXLAN_UDP_ZERO_CSUM6_TX,
-			!!(vxlan->flags & VXLAN_F_UDP_ZERO_CSUM6_TX)) ||
+			!!(vxlan->cfg.flags & VXLAN_F_UDP_ZERO_CSUM6_TX)) ||
 	    nla_put_u8(skb, IFLA_VXLAN_UDP_ZERO_CSUM6_RX,
-			!!(vxlan->flags & VXLAN_F_UDP_ZERO_CSUM6_RX)) ||
+			!!(vxlan->cfg.flags & VXLAN_F_UDP_ZERO_CSUM6_RX)) ||
 	    nla_put_u8(skb, IFLA_VXLAN_REMCSUM_TX,
-			!!(vxlan->flags & VXLAN_F_REMCSUM_TX)) ||
+			!!(vxlan->cfg.flags & VXLAN_F_REMCSUM_TX)) ||
 	    nla_put_u8(skb, IFLA_VXLAN_REMCSUM_RX,
-			!!(vxlan->flags & VXLAN_F_REMCSUM_RX)))
+			!!(vxlan->cfg.flags & VXLAN_F_REMCSUM_RX)))
 		goto nla_put_failure;
 
 	if (nla_put(skb, IFLA_VXLAN_PORT_RANGE, sizeof(ports), &ports))
 		goto nla_put_failure;
 
-	if (vxlan->flags & VXLAN_F_GBP &&
+	if (vxlan->cfg.flags & VXLAN_F_GBP &&
 	    nla_put_flag(skb, IFLA_VXLAN_GBP))
 		goto nla_put_failure;
 
-	if (vxlan->flags & VXLAN_F_GPE &&
+	if (vxlan->cfg.flags & VXLAN_F_GPE &&
 	    nla_put_flag(skb, IFLA_VXLAN_GPE))
 		goto nla_put_failure;
 
-	if (vxlan->flags & VXLAN_F_REMCSUM_NOPARTIAL &&
+	if (vxlan->cfg.flags & VXLAN_F_REMCSUM_NOPARTIAL &&
 	    nla_put_flag(skb, IFLA_VXLAN_REMCSUM_NOPARTIAL))
 		goto nla_put_failure;
 
diff --git a/drivers/net/wan/dlci.c b/drivers/net/wan/dlci.c
index 65ee2a6f248c..a0d76f70c428 100644
--- a/drivers/net/wan/dlci.c
+++ b/drivers/net/wan/dlci.c
@@ -475,7 +475,7 @@ static void dlci_setup(struct net_device *dev)
 	dev->flags		= 0;
 	dev->header_ops		= &dlci_header_ops;
 	dev->netdev_ops		= &dlci_netdev_ops;
-	dev->destructor		= free_netdev;
+	dev->needs_free_netdev	= true;
 
 	dlp->receive		= dlci_receive;
 
diff --git a/drivers/net/wan/farsync.c b/drivers/net/wan/farsync.c
index 33265eb50420..bd46b2552980 100644
--- a/drivers/net/wan/farsync.c
+++ b/drivers/net/wan/farsync.c
@@ -857,7 +857,7 @@ fst_rx_dma_complete(struct fst_card_info *card, struct fst_port_info *port,
 
 	dbg(DBG_TX, "fst_rx_dma_complete\n");
 	pi = port->index;
-	memcpy(skb_put(skb, len), card->rx_dma_handle_host, len);
+	skb_put_data(skb, card->rx_dma_handle_host, len);
 
 	/* Reset buffer descriptor */
 	FST_WRB(card, rxDescrRing[pi][rxp].bits, DMA_OWN);
diff --git a/drivers/net/wan/fsl_ucc_hdlc.c b/drivers/net/wan/fsl_ucc_hdlc.c
index 6742ae605660..33df76405b86 100644
--- a/drivers/net/wan/fsl_ucc_hdlc.c
+++ b/drivers/net/wan/fsl_ucc_hdlc.c
@@ -36,7 +36,6 @@
 #define DRV_NAME "ucc_hdlc"
 
 #define TDM_PPPOHT_SLIC_MAXIN
-#define BROKEN_FRAME_INFO
 
 static struct ucc_tdm_info utdm_primary_info = {
 	.uf_info = {
@@ -99,6 +98,13 @@ static int uhdlc_init(struct ucc_hdlc_private *priv)
 		uf_info->tsa = 1;
 		uf_info->ctsp = 1;
 	}
+
+	/* This sets HPM register in CMXUCR register which configures a
+	 * open drain connected HDLC bus
+	 */
+	if (priv->hdlc_bus)
+		uf_info->brkpt_support = 1;
+
 	uf_info->uccm_mask = ((UCC_HDLC_UCCE_RXB | UCC_HDLC_UCCE_RXF |
 				UCC_HDLC_UCCE_TXB) << 16);
 
@@ -114,6 +120,9 @@ static int uhdlc_init(struct ucc_hdlc_private *priv)
 	/* Loopback mode */
 	if (priv->loopback) {
 		dev_info(priv->dev, "Loopback Mode\n");
+		/* use the same clock when work in loopback */
+		qe_setbrg(ut_info->uf_info.rx_clock, 20000000, 1);
+
 		gumr = ioread32be(&priv->uf_regs->gumr);
 		gumr |= (UCC_FAST_GUMR_LOOPBACK | UCC_FAST_GUMR_CDS |
 			 UCC_FAST_GUMR_TCI);
@@ -133,11 +142,33 @@ static int uhdlc_init(struct ucc_hdlc_private *priv)
 	/* Set UPSMR normal mode (need fixed)*/
 	iowrite32be(0, &priv->uf_regs->upsmr);
 
+	/* hdlc_bus mode */
+	if (priv->hdlc_bus) {
+		u32 upsmr;
+
+		dev_info(priv->dev, "HDLC bus Mode\n");
+		upsmr = ioread32be(&priv->uf_regs->upsmr);
+
+		/* bus mode and retransmit enable, with collision window
+		 * set to 8 bytes
+		 */
+		upsmr |= UCC_HDLC_UPSMR_RTE | UCC_HDLC_UPSMR_BUS |
+				UCC_HDLC_UPSMR_CW8;
+		iowrite32be(upsmr, &priv->uf_regs->upsmr);
+
+		/* explicitly disable CDS & CTSP */
+		gumr = ioread32be(&priv->uf_regs->gumr);
+		gumr &= ~(UCC_FAST_GUMR_CDS | UCC_FAST_GUMR_CTSP);
+		/* set automatic sync to explicitly ignore CD signal */
+		gumr |= UCC_FAST_GUMR_SYNL_AUTO;
+		iowrite32be(gumr, &priv->uf_regs->gumr);
+	}
+
 	priv->rx_ring_size = RX_BD_RING_LEN;
 	priv->tx_ring_size = TX_BD_RING_LEN;
 	/* Alloc Rx BD */
 	priv->rx_bd_base = dma_alloc_coherent(priv->dev,
-			RX_BD_RING_LEN * sizeof(struct qe_bd *),
+			RX_BD_RING_LEN * sizeof(struct qe_bd),
 			&priv->dma_rx_bd, GFP_KERNEL);
 
 	if (!priv->rx_bd_base) {
@@ -148,7 +179,7 @@ static int uhdlc_init(struct ucc_hdlc_private *priv)
 
 	/* Alloc Tx BD */
 	priv->tx_bd_base = dma_alloc_coherent(priv->dev,
-			TX_BD_RING_LEN * sizeof(struct qe_bd *),
+			TX_BD_RING_LEN * sizeof(struct qe_bd),
 			&priv->dma_tx_bd, GFP_KERNEL);
 
 	if (!priv->tx_bd_base) {
@@ -158,7 +189,7 @@ static int uhdlc_init(struct ucc_hdlc_private *priv)
 	}
 
 	/* Alloc parameter ram for ucc hdlc */
-	priv->ucc_pram_offset = qe_muram_alloc(sizeof(priv->ucc_pram),
+	priv->ucc_pram_offset = qe_muram_alloc(sizeof(struct ucc_hdlc_param),
 				ALIGNMENT_OF_UCC_HDLC_PRAM);
 
 	if (priv->ucc_pram_offset < 0) {
@@ -295,11 +326,11 @@ free_ucc_pram:
 	qe_muram_free(priv->ucc_pram_offset);
 free_tx_bd:
 	dma_free_coherent(priv->dev,
-			  TX_BD_RING_LEN * sizeof(struct qe_bd *),
+			  TX_BD_RING_LEN * sizeof(struct qe_bd),
 			  priv->tx_bd_base, priv->dma_tx_bd);
 free_rx_bd:
 	dma_free_coherent(priv->dev,
-			  RX_BD_RING_LEN * sizeof(struct qe_bd *),
+			  RX_BD_RING_LEN * sizeof(struct qe_bd),
 			  priv->rx_bd_base, priv->dma_rx_bd);
 free_uccf:
 	ucc_fast_free(priv->uccf);
@@ -314,8 +345,6 @@ static netdev_tx_t ucc_hdlc_tx(struct sk_buff *skb, struct net_device *dev)
 	struct qe_bd __iomem *bd;
 	u16 bd_status;
 	unsigned long flags;
-	u8 *send_buf;
-	int i;
 	u16 *proto_head;
 
 	switch (dev->type) {
@@ -352,16 +381,6 @@ static netdev_tx_t ucc_hdlc_tx(struct sk_buff *skb, struct net_device *dev)
 		dev_kfree_skb(skb);
 		return -ENOMEM;
 	}
-
-	pr_info("Tx data skb->len:%d ", skb->len);
-	send_buf = (u8 *)skb->data;
-	pr_info("\nTransmitted data:\n");
-	for (i = 0; i < 16; i++) {
-		if (i == skb->len)
-			pr_info("++++");
-		else
-		pr_info("%02x\n", send_buf[i]);
-	}
 	spin_lock_irqsave(&priv->lock, flags);
 
 	/* Start from the next BD that should be filled */
@@ -423,7 +442,6 @@ static int hdlc_tx_done(struct ucc_hdlc_private *priv)
 		skb = priv->tx_skbuff[priv->skb_dirtytx];
 		if (!skb)
 			break;
-		pr_info("TxBD: %x\n", bd_status);
 		dev->stats.tx_packets++;
 		memset(priv->tx_buffer +
 		       (be32_to_cpu(bd->buf) - priv->dma_tx_addr),
@@ -454,14 +472,12 @@ static int hdlc_tx_done(struct ucc_hdlc_private *priv)
 static int hdlc_rx_done(struct ucc_hdlc_private *priv, int rx_work_limit)
 {
 	struct net_device *dev = priv->ndev;
-	struct sk_buff *skb;
+	struct sk_buff *skb = NULL;
 	hdlc_device *hdlc = dev_to_hdlc(dev);
 	struct qe_bd *bd;
 	u16 bd_status;
 	u16 length, howmany = 0;
 	u8 *bdbuffer;
-	int i;
-	static int entry;
 
 	bd = priv->currx_bd;
 	bd_status = ioread16be(&bd->status);
@@ -471,9 +487,6 @@ static int hdlc_rx_done(struct ucc_hdlc_private *priv, int rx_work_limit)
 		if (bd_status & R_OV_S)
 			dev->stats.rx_over_errors++;
 		if (bd_status & R_CR_S) {
-#ifdef BROKEN_FRAME_INFO
-			pr_info("Broken Frame with RxBD: %x\n", bd_status);
-#endif
 			dev->stats.rx_crc_errors++;
 			dev->stats.rx_dropped++;
 			goto recycle;
@@ -482,17 +495,6 @@ static int hdlc_rx_done(struct ucc_hdlc_private *priv, int rx_work_limit)
 			(priv->currx_bdnum * MAX_RX_BUF_LENGTH);
 		length = ioread16be(&bd->length);
 
-		pr_info("Received data length:%d", length);
-		pr_info("while entry times:%d", entry++);
-
-		pr_info("\nReceived data:\n");
-		for (i = 0; (i < 16); i++) {
-			if (i == length)
-				pr_info("++++");
-			else
-			pr_info("%02x\n", bdbuffer[i]);
-		}
-
 		switch (dev->type) {
 		case ARPHRD_RAWHDLC:
 			bdbuffer += HDLC_HEAD_LEN;
@@ -531,7 +533,6 @@ static int hdlc_rx_done(struct ucc_hdlc_private *priv, int rx_work_limit)
 		howmany++;
 		if (hdlc->proto)
 			skb->protocol = hdlc_type_trans(skb, dev);
-		pr_info("skb->protocol:%x\n", skb->protocol);
 		netif_receive_skb(skb);
 
 recycle:
@@ -566,7 +567,7 @@ static int ucc_hdlc_poll(struct napi_struct *napi, int budget)
 
 	/* Tx event processing */
 	spin_lock(&priv->lock);
-		hdlc_tx_done(priv);
+	hdlc_tx_done(priv);
 	spin_unlock(&priv->lock);
 
 	howmany = 0;
@@ -597,7 +598,6 @@ static irqreturn_t ucc_hdlc_irq_handler(int irq, void *dev_id)
 	uccm = ioread32be(uccf->p_uccm);
 	ucce &= uccm;
 	iowrite32be(ucce, uccf->p_ucce);
-	pr_info("irq ucce:%x\n", ucce);
 	if (!ucce)
 		return IRQ_NONE;
 
@@ -688,7 +688,7 @@ static void uhdlc_memclean(struct ucc_hdlc_private *priv)
 
 	if (priv->rx_bd_base) {
 		dma_free_coherent(priv->dev,
-				  RX_BD_RING_LEN * sizeof(struct qe_bd *),
+				  RX_BD_RING_LEN * sizeof(struct qe_bd),
 				  priv->rx_bd_base, priv->dma_rx_bd);
 
 		priv->rx_bd_base = NULL;
@@ -697,7 +697,7 @@ static void uhdlc_memclean(struct ucc_hdlc_private *priv)
 
 	if (priv->tx_bd_base) {
 		dma_free_coherent(priv->dev,
-				  TX_BD_RING_LEN * sizeof(struct qe_bd *),
+				  TX_BD_RING_LEN * sizeof(struct qe_bd),
 				  priv->tx_bd_base, priv->dma_tx_bd);
 
 		priv->tx_bd_base = NULL;
@@ -855,7 +855,6 @@ static int uhdlc_suspend(struct device *dev)
 	/* save power */
 	ucc_fast_disable(priv->uccf, COMM_DIR_RX | COMM_DIR_TX);
 
-	dev_dbg(dev, "ucc hdlc suspend\n");
 	return 0;
 }
 
@@ -1001,7 +1000,7 @@ static int ucc_hdlc_probe(struct platform_device *pdev)
 	struct device_node *np = pdev->dev.of_node;
 	struct ucc_hdlc_private *uhdlc_priv = NULL;
 	struct ucc_tdm_info *ut_info;
-	struct ucc_tdm *utdm;
+	struct ucc_tdm *utdm = NULL;
 	struct resource res;
 	struct net_device *dev;
 	hdlc_device *hdlc;
@@ -1054,10 +1053,6 @@ static int ucc_hdlc_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	/* use the same clock when work in loopback */
-	if (ut_info->uf_info.rx_clock == ut_info->uf_info.tx_clock)
-		qe_setbrg(ut_info->uf_info.rx_clock, 20000000, 1);
-
 	ret = of_address_to_resource(np, 0, &res);
 	if (ret)
 		return -EINVAL;
@@ -1080,6 +1075,9 @@ static int ucc_hdlc_probe(struct platform_device *pdev)
 	if (of_get_property(np, "fsl,ucc-internal-loopback", NULL))
 		uhdlc_priv->loopback = 1;
 
+	if (of_get_property(np, "fsl,hdlc-bus", NULL))
+		uhdlc_priv->hdlc_bus = 1;
+
 	if (uhdlc_priv->tsa == 1) {
 		utdm = kzalloc(sizeof(*utdm), GFP_KERNEL);
 		if (!utdm) {
diff --git a/drivers/net/wan/fsl_ucc_hdlc.h b/drivers/net/wan/fsl_ucc_hdlc.h
index 881ecdeef076..c21134c1f180 100644
--- a/drivers/net/wan/fsl_ucc_hdlc.h
+++ b/drivers/net/wan/fsl_ucc_hdlc.h
@@ -78,6 +78,7 @@ struct ucc_hdlc_private {
 	u16 tsa;
 	bool hdlc_busy;
 	bool loopback;
+	bool hdlc_bus;
 
 	u8 *tx_buffer;
 	u8 *rx_buffer;
diff --git a/drivers/net/wan/hdlc_fr.c b/drivers/net/wan/hdlc_fr.c
index eb915281197e..78596e42a3f3 100644
--- a/drivers/net/wan/hdlc_fr.c
+++ b/drivers/net/wan/hdlc_fr.c
@@ -1106,7 +1106,7 @@ static int fr_add_pvc(struct net_device *frad, unsigned int dlci, int type)
 		return -EIO;
 	}
 
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 	*get_dev_p(pvc, type) = dev;
 	if (!used) {
 		state(hdlc)->dce_changed = 1;
diff --git a/drivers/net/wan/hdlc_ppp.c b/drivers/net/wan/hdlc_ppp.c
index 47fdb87d3567..0d2e00ece804 100644
--- a/drivers/net/wan/hdlc_ppp.c
+++ b/drivers/net/wan/hdlc_ppp.c
@@ -228,15 +228,15 @@ static void ppp_tx_cp(struct net_device *dev, u16 pid, u8 code,
 	}
 	skb_reserve(skb, sizeof(struct hdlc_header));
 
-	cp = (struct cp_header *)skb_put(skb, sizeof(struct cp_header));
+	cp = skb_put(skb, sizeof(struct cp_header));
 	cp->code = code;
 	cp->id = id;
 	cp->len = htons(sizeof(struct cp_header) + magic_len + len);
 
 	if (magic_len)
-		memcpy(skb_put(skb, magic_len), &magic, magic_len);
+		skb_put_data(skb, &magic, magic_len);
 	if (len)
-		memcpy(skb_put(skb, len), data, len);
+		skb_put_data(skb, data, len);
 
 #if DEBUG_CP
 	BUG_ON(code >= CP_CODES);
@@ -448,7 +448,7 @@ static int ppp_rx(struct sk_buff *skb)
 	/* Check HDLC header */
 	if (skb->len < sizeof(struct hdlc_header))
 		goto rx_error;
-	cp = (struct cp_header*)skb_pull(skb, sizeof(struct hdlc_header));
+	cp = skb_pull(skb, sizeof(struct hdlc_header));
 	if (hdr->address != HDLC_ADDR_ALLSTATIONS ||
 	    hdr->control != HDLC_CTRL_UI)
 		goto rx_error;
diff --git a/drivers/net/wan/hdlc_raw_eth.c b/drivers/net/wan/hdlc_raw_eth.c
index 2f11836078ab..8bd3ed905813 100644
--- a/drivers/net/wan/hdlc_raw_eth.c
+++ b/drivers/net/wan/hdlc_raw_eth.c
@@ -57,7 +57,8 @@ static int raw_eth_ioctl(struct net_device *dev, struct ifreq *ifr)
 	const size_t size = sizeof(raw_hdlc_proto);
 	raw_hdlc_proto new_settings;
 	hdlc_device *hdlc = dev_to_hdlc(dev);
-	int result, old_qlen;
+	unsigned int old_qlen;
+	int result;
 
 	switch (ifr->ifr_settings.type) {
 	case IF_GET_PROTO:
diff --git a/drivers/net/wan/lapbether.c b/drivers/net/wan/lapbether.c
index 9df9ed62beff..63f749078a1f 100644
--- a/drivers/net/wan/lapbether.c
+++ b/drivers/net/wan/lapbether.c
@@ -306,7 +306,7 @@ static const struct net_device_ops lapbeth_netdev_ops = {
 static void lapbeth_setup(struct net_device *dev)
 {
 	dev->netdev_ops	     = &lapbeth_netdev_ops;
-	dev->destructor	     = free_netdev;
+	dev->needs_free_netdev = true;
 	dev->type            = ARPHRD_X25;
 	dev->hard_header_len = 3;
 	dev->mtu             = 1000;
diff --git a/drivers/net/wan/x25_asy.c b/drivers/net/wan/x25_asy.c
index 878b05d06fc7..40ee80c03c94 100644
--- a/drivers/net/wan/x25_asy.c
+++ b/drivers/net/wan/x25_asy.c
@@ -202,7 +202,7 @@ static void x25_asy_bump(struct x25_asy *sl)
 		return;
 	}
 	skb_push(skb, 1);	/* LAPB internal control */
-	memcpy(skb_put(skb, count), sl->rbuff, count);
+	skb_put_data(skb, sl->rbuff, count);
 	skb->protocol = x25_type_trans(skb, sl->dev);
 	err = lapb_data_received(skb->dev, skb);
 	if (err != LAPB_OK) {
diff --git a/drivers/net/wimax/i2400m/netdev.c b/drivers/net/wimax/i2400m/netdev.c
index 7f64e74d746b..a654687b5fa2 100644
--- a/drivers/net/wimax/i2400m/netdev.c
+++ b/drivers/net/wimax/i2400m/netdev.c
@@ -221,7 +221,7 @@ void i2400m_tx_prep_header(struct sk_buff *skb)
 {
 	struct i2400m_pl_data_hdr *pl_hdr;
 	skb_pull(skb, ETH_HLEN);
-	pl_hdr = (struct i2400m_pl_data_hdr *) skb_push(skb, sizeof(*pl_hdr));
+	pl_hdr = skb_push(skb, sizeof(*pl_hdr));
 	pl_hdr->reserved = 0;
 }
 
@@ -488,7 +488,7 @@ void i2400m_net_rx(struct i2400m *i2400m, struct sk_buff *skb_rx,
 			net_dev->stats.rx_dropped++;
 			goto error_skb_realloc;
 		}
-		memcpy(skb_put(skb, buf_len), buf, buf_len);
+		skb_put_data(skb, buf, buf_len);
 	}
 	i2400m_rx_fake_eth_header(i2400m->wimax_dev.net_dev,
 				  skb->data - ETH_HLEN,
diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index 8f5a3f4a43f2..166920ae23f8 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -45,6 +45,7 @@ source "drivers/net/wireless/rsi/Kconfig"
 source "drivers/net/wireless/st/Kconfig"
 source "drivers/net/wireless/ti/Kconfig"
 source "drivers/net/wireless/zydas/Kconfig"
+source "drivers/net/wireless/quantenna/Kconfig"
 
 config PCMCIA_RAYCS
 	tristate "Aviator/Raytheon 2.4GHz wireless support"
diff --git a/drivers/net/wireless/Makefile b/drivers/net/wireless/Makefile
index f00d42953fb8..54b41ac5f9c8 100644
--- a/drivers/net/wireless/Makefile
+++ b/drivers/net/wireless/Makefile
@@ -17,6 +17,7 @@ obj-$(CONFIG_WLAN_VENDOR_RSI) += rsi/
 obj-$(CONFIG_WLAN_VENDOR_ST) += st/
 obj-$(CONFIG_WLAN_VENDOR_TI) += ti/
 obj-$(CONFIG_WLAN_VENDOR_ZYDAS) += zydas/
+obj-$(CONFIG_WLAN_VENDOR_QUANTENNA) += quantenna/
 
 # 16-bit wireless PCMCIA client drivers
 obj-$(CONFIG_PCMCIA_RAYCS)	+= ray_cs.o
diff --git a/drivers/net/wireless/admtek/adm8211.c b/drivers/net/wireless/admtek/adm8211.c
index ed626f568b58..3b0802fc5bf5 100644
--- a/drivers/net/wireless/admtek/adm8211.c
+++ b/drivers/net/wireless/admtek/adm8211.c
@@ -390,9 +390,9 @@ static void adm8211_interrupt_rci(struct ieee80211_hw *dev)
 					priv->pdev,
 					priv->rx_buffers[entry].mapping,
 					pktlen, PCI_DMA_FROMDEVICE);
-				memcpy(skb_put(skb, pktlen),
-				       skb_tail_pointer(priv->rx_buffers[entry].skb),
-				       pktlen);
+				skb_put_data(skb,
+					     skb_tail_pointer(priv->rx_buffers[entry].skb),
+					     pktlen);
 				pci_dma_sync_single_for_device(
 					priv->pdev,
 					priv->rx_buffers[entry].mapping,
@@ -1700,7 +1700,7 @@ static void adm8211_tx(struct ieee80211_hw *dev,
 	skb_pull(skb, hdrlen);
 	payload_len = skb->len;
 
-	txhdr = (struct adm8211_tx_hdr *) skb_push(skb, sizeof(*txhdr));
+	txhdr = skb_push(skb, sizeof(*txhdr));
 	memset(txhdr, 0, sizeof(*txhdr));
 	memcpy(txhdr->da, ieee80211_get_DA(hdr), ETH_ALEN);
 	txhdr->signal = plcp_signal;
diff --git a/drivers/net/wireless/ath/ar5523/ar5523.c b/drivers/net/wireless/ath/ar5523/ar5523.c
index f2f4ccfdf8da..106d6f8d471a 100644
--- a/drivers/net/wireless/ath/ar5523/ar5523.c
+++ b/drivers/net/wireless/ath/ar5523/ar5523.c
@@ -829,8 +829,8 @@ static void ar5523_tx_work_locked(struct ar5523 *ar)
 		data->ar = ar;
 		data->urb = urb;
 
-		desc = (struct ar5523_tx_desc *)skb_push(skb, sizeof(*desc));
-		chunk = (struct ar5523_chunk *)skb_push(skb, sizeof(*chunk));
+		desc = skb_push(skb, sizeof(*desc));
+		chunk = skb_push(skb, sizeof(*chunk));
 
 		chunk->seqnum = 0;
 		chunk->flags = UATH_CFLAGS_FINAL;
diff --git a/drivers/net/wireless/ath/ath10k/Kconfig b/drivers/net/wireless/ath/ath10k/Kconfig
index b4241cf9b7ed..412eb1380dcc 100644
--- a/drivers/net/wireless/ath/ath10k/Kconfig
+++ b/drivers/net/wireless/ath/ath10k/Kconfig
@@ -22,6 +22,13 @@ config ATH10K_AHB
 	---help---
 	  This module adds support for AHB bus
 
+config ATH10K_SDIO
+	tristate "Atheros ath10k SDIO support (EXPERIMENTAL)"
+	depends on ATH10K && MMC
+	---help---
+	  This module adds experimental support for SDIO/MMC bus. Currently
+	  work in progress and will not fully work.
+
 config ATH10K_DEBUG
 	bool "Atheros ath10k debugging"
 	depends on ATH10K
diff --git a/drivers/net/wireless/ath/ath10k/Makefile b/drivers/net/wireless/ath/ath10k/Makefile
index 930fadd940d8..b0b19a7eb98b 100644
--- a/drivers/net/wireless/ath/ath10k/Makefile
+++ b/drivers/net/wireless/ath/ath10k/Makefile
@@ -27,5 +27,8 @@ ath10k_pci-y += pci.o \
 
 ath10k_pci-$(CONFIG_ATH10K_AHB) += ahb.o
 
+obj-$(CONFIG_ATH10K_SDIO) += ath10k_sdio.o
+ath10k_sdio-y += sdio.o
+
 # for tracing framework to find trace.h
 CFLAGS_trace.o := -I$(src)
diff --git a/drivers/net/wireless/ath/ath10k/bmi.c b/drivers/net/wireless/ath/ath10k/bmi.c
index abeee200310b..2d3a2f31123d 100644
--- a/drivers/net/wireless/ath/ath10k/bmi.c
+++ b/drivers/net/wireless/ath/ath10k/bmi.c
@@ -97,6 +97,77 @@ int ath10k_bmi_get_target_info(struct ath10k *ar,
 	return 0;
 }
 
+#define TARGET_VERSION_SENTINAL 0xffffffffu
+
+int ath10k_bmi_get_target_info_sdio(struct ath10k *ar,
+				    struct bmi_target_info *target_info)
+{
+	struct bmi_cmd cmd;
+	union bmi_resp resp;
+	u32 cmdlen = sizeof(cmd.id) + sizeof(cmd.get_target_info);
+	u32 resplen, ver_len;
+	__le32 tmp;
+	int ret;
+
+	ath10k_dbg(ar, ATH10K_DBG_BMI, "bmi get target info SDIO\n");
+
+	if (ar->bmi.done_sent) {
+		ath10k_warn(ar, "BMI Get Target Info Command disallowed\n");
+		return -EBUSY;
+	}
+
+	cmd.id = __cpu_to_le32(BMI_GET_TARGET_INFO);
+
+	/* Step 1: Read 4 bytes of the target info and check if it is
+	 * the special sentinal version word or the first word in the
+	 * version response.
+	 */
+	resplen = sizeof(u32);
+	ret = ath10k_hif_exchange_bmi_msg(ar, &cmd, cmdlen, &tmp, &resplen);
+	if (ret) {
+		ath10k_warn(ar, "unable to read from device\n");
+		return ret;
+	}
+
+	/* Some SDIO boards have a special sentinal byte before the real
+	 * version response.
+	 */
+	if (__le32_to_cpu(tmp) == TARGET_VERSION_SENTINAL) {
+		/* Step 1b: Read the version length */
+		resplen = sizeof(u32);
+		ret = ath10k_hif_exchange_bmi_msg(ar, NULL, 0, &tmp,
+						  &resplen);
+		if (ret) {
+			ath10k_warn(ar, "unable to read from device\n");
+			return ret;
+		}
+	}
+
+	ver_len = __le32_to_cpu(tmp);
+
+	/* Step 2: Check the target info length */
+	if (ver_len != sizeof(resp.get_target_info)) {
+		ath10k_warn(ar, "Unexpected target info len: %u. Expected: %zu\n",
+			    ver_len, sizeof(resp.get_target_info));
+		return -EINVAL;
+	}
+
+	/* Step 3: Read the rest of the version response */
+	resplen = sizeof(resp.get_target_info) - sizeof(u32);
+	ret = ath10k_hif_exchange_bmi_msg(ar, NULL, 0,
+					  &resp.get_target_info.version,
+					  &resplen);
+	if (ret) {
+		ath10k_warn(ar, "unable to read from device\n");
+		return ret;
+	}
+
+	target_info->version = __le32_to_cpu(resp.get_target_info.version);
+	target_info->type    = __le32_to_cpu(resp.get_target_info.type);
+
+	return 0;
+}
+
 int ath10k_bmi_read_memory(struct ath10k *ar,
 			   u32 address, void *buffer, u32 length)
 {
diff --git a/drivers/net/wireless/ath/ath10k/bmi.h b/drivers/net/wireless/ath/ath10k/bmi.h
index cc45b63ade15..9c0839b2ca8f 100644
--- a/drivers/net/wireless/ath/ath10k/bmi.h
+++ b/drivers/net/wireless/ath/ath10k/bmi.h
@@ -83,6 +83,8 @@ enum bmi_cmd_id {
 #define BMI_NVRAM_SEG_NAME_SZ 16
 
 #define BMI_PARAM_GET_EEPROM_BOARD_ID 0x10
+#define BMI_PARAM_GET_FLASH_BOARD_ID 0x8000
+#define BMI_PARAM_FLASH_SECTION_ALL 0x10000
 
 #define ATH10K_BMI_BOARD_ID_FROM_OTP_MASK   0x7c00
 #define ATH10K_BMI_BOARD_ID_FROM_OTP_LSB    10
@@ -188,8 +190,8 @@ struct bmi_target_info {
 	u32 type;
 };
 
-/* in msec */
-#define BMI_COMMUNICATION_TIMEOUT_HZ (2 * HZ)
+/* in jiffies */
+#define BMI_COMMUNICATION_TIMEOUT_HZ (3 * HZ)
 
 #define BMI_CE_NUM_TO_TARG 0
 #define BMI_CE_NUM_TO_HOST 1
@@ -198,6 +200,8 @@ void ath10k_bmi_start(struct ath10k *ar);
 int ath10k_bmi_done(struct ath10k *ar);
 int ath10k_bmi_get_target_info(struct ath10k *ar,
 			       struct bmi_target_info *target_info);
+int ath10k_bmi_get_target_info_sdio(struct ath10k *ar,
+				    struct bmi_target_info *target_info);
 int ath10k_bmi_read_memory(struct ath10k *ar, u32 address,
 			   void *buffer, u32 length);
 int ath10k_bmi_write_memory(struct ath10k *ar, u32 address,
diff --git a/drivers/net/wireless/ath/ath10k/ce.c b/drivers/net/wireless/ath/ath10k/ce.c
index ee1090ca2eac..08b84c8c3614 100644
--- a/drivers/net/wireless/ath/ath10k/ce.c
+++ b/drivers/net/wireless/ath/ath10k/ce.c
@@ -59,205 +59,243 @@
  * the buffer is sent/received.
  */
 
+static inline unsigned int
+ath10k_set_ring_byte(unsigned int offset,
+		     struct ath10k_hw_ce_regs_addr_map *addr_map)
+{
+	return ((offset << addr_map->lsb) & addr_map->mask);
+}
+
+static inline unsigned int
+ath10k_get_ring_byte(unsigned int offset,
+		     struct ath10k_hw_ce_regs_addr_map *addr_map)
+{
+	return ((offset & addr_map->mask) >> (addr_map->lsb));
+}
+
 static inline void ath10k_ce_dest_ring_write_index_set(struct ath10k *ar,
 						       u32 ce_ctrl_addr,
 						       unsigned int n)
 {
-	ath10k_pci_write32(ar, ce_ctrl_addr + DST_WR_INDEX_ADDRESS, n);
+	ath10k_pci_write32(ar, ce_ctrl_addr +
+			ar->hw_ce_regs->dst_wr_index_addr, n);
 }
 
 static inline u32 ath10k_ce_dest_ring_write_index_get(struct ath10k *ar,
 						      u32 ce_ctrl_addr)
 {
-	return ath10k_pci_read32(ar, ce_ctrl_addr + DST_WR_INDEX_ADDRESS);
+	return ath10k_pci_read32(ar, ce_ctrl_addr +
+			ar->hw_ce_regs->dst_wr_index_addr);
 }
 
 static inline void ath10k_ce_src_ring_write_index_set(struct ath10k *ar,
 						      u32 ce_ctrl_addr,
 						      unsigned int n)
 {
-	ath10k_pci_write32(ar, ce_ctrl_addr + SR_WR_INDEX_ADDRESS, n);
+	ath10k_pci_write32(ar, ce_ctrl_addr +
+			ar->hw_ce_regs->sr_wr_index_addr, n);
 }
 
 static inline u32 ath10k_ce_src_ring_write_index_get(struct ath10k *ar,
 						     u32 ce_ctrl_addr)
 {
-	return ath10k_pci_read32(ar, ce_ctrl_addr + SR_WR_INDEX_ADDRESS);
+	return ath10k_pci_read32(ar, ce_ctrl_addr +
+			ar->hw_ce_regs->sr_wr_index_addr);
 }
 
 static inline u32 ath10k_ce_src_ring_read_index_get(struct ath10k *ar,
 						    u32 ce_ctrl_addr)
 {
-	return ath10k_pci_read32(ar, ce_ctrl_addr + CURRENT_SRRI_ADDRESS);
+	return ath10k_pci_read32(ar, ce_ctrl_addr +
+			ar->hw_ce_regs->current_srri_addr);
 }
 
 static inline void ath10k_ce_src_ring_base_addr_set(struct ath10k *ar,
 						    u32 ce_ctrl_addr,
 						    unsigned int addr)
 {
-	ath10k_pci_write32(ar, ce_ctrl_addr + SR_BA_ADDRESS, addr);
+	ath10k_pci_write32(ar, ce_ctrl_addr +
+			ar->hw_ce_regs->sr_base_addr, addr);
 }
 
 static inline void ath10k_ce_src_ring_size_set(struct ath10k *ar,
 					       u32 ce_ctrl_addr,
 					       unsigned int n)
 {
-	ath10k_pci_write32(ar, ce_ctrl_addr + SR_SIZE_ADDRESS, n);
+	ath10k_pci_write32(ar, ce_ctrl_addr +
+			ar->hw_ce_regs->sr_size_addr, n);
 }
 
 static inline void ath10k_ce_src_ring_dmax_set(struct ath10k *ar,
 					       u32 ce_ctrl_addr,
 					       unsigned int n)
 {
-	u32 ctrl1_addr = ath10k_pci_read32((ar),
-					   (ce_ctrl_addr) + CE_CTRL1_ADDRESS);
+	struct ath10k_hw_ce_ctrl1 *ctrl_regs = ar->hw_ce_regs->ctrl1_regs;
+	u32 ctrl1_addr = ath10k_pci_read32(ar,
+					   ce_ctrl_addr + ctrl_regs->addr);
 
-	ath10k_pci_write32(ar, ce_ctrl_addr + CE_CTRL1_ADDRESS,
-			   (ctrl1_addr &  ~CE_CTRL1_DMAX_LENGTH_MASK) |
-			   CE_CTRL1_DMAX_LENGTH_SET(n));
+	ath10k_pci_write32(ar, ce_ctrl_addr + ctrl_regs->addr,
+			   (ctrl1_addr &  ~(ctrl_regs->dmax->mask)) |
+			   ath10k_set_ring_byte(n, ctrl_regs->dmax));
 }
 
 static inline void ath10k_ce_src_ring_byte_swap_set(struct ath10k *ar,
 						    u32 ce_ctrl_addr,
 						    unsigned int n)
 {
-	u32 ctrl1_addr = ath10k_pci_read32(ar, ce_ctrl_addr + CE_CTRL1_ADDRESS);
+	struct ath10k_hw_ce_ctrl1 *ctrl_regs = ar->hw_ce_regs->ctrl1_regs;
+	u32 ctrl1_addr = ath10k_pci_read32(ar, ce_ctrl_addr + ctrl_regs->addr);
 
-	ath10k_pci_write32(ar, ce_ctrl_addr + CE_CTRL1_ADDRESS,
-			   (ctrl1_addr & ~CE_CTRL1_SRC_RING_BYTE_SWAP_EN_MASK) |
-			   CE_CTRL1_SRC_RING_BYTE_SWAP_EN_SET(n));
+	ath10k_pci_write32(ar, ce_ctrl_addr + ctrl_regs->addr,
+			   (ctrl1_addr & ~(ctrl_regs->src_ring->mask)) |
+			   ath10k_set_ring_byte(n, ctrl_regs->src_ring));
 }
 
 static inline void ath10k_ce_dest_ring_byte_swap_set(struct ath10k *ar,
 						     u32 ce_ctrl_addr,
 						     unsigned int n)
 {
-	u32 ctrl1_addr = ath10k_pci_read32(ar, ce_ctrl_addr + CE_CTRL1_ADDRESS);
+	struct ath10k_hw_ce_ctrl1 *ctrl_regs = ar->hw_ce_regs->ctrl1_regs;
+	u32 ctrl1_addr = ath10k_pci_read32(ar, ce_ctrl_addr + ctrl_regs->addr);
 
-	ath10k_pci_write32(ar, ce_ctrl_addr + CE_CTRL1_ADDRESS,
-			   (ctrl1_addr & ~CE_CTRL1_DST_RING_BYTE_SWAP_EN_MASK) |
-			   CE_CTRL1_DST_RING_BYTE_SWAP_EN_SET(n));
+	ath10k_pci_write32(ar, ce_ctrl_addr + ctrl_regs->addr,
+			   (ctrl1_addr & ~(ctrl_regs->dst_ring->mask)) |
+			   ath10k_set_ring_byte(n, ctrl_regs->dst_ring));
 }
 
 static inline u32 ath10k_ce_dest_ring_read_index_get(struct ath10k *ar,
 						     u32 ce_ctrl_addr)
 {
-	return ath10k_pci_read32(ar, ce_ctrl_addr + CURRENT_DRRI_ADDRESS);
+	return ath10k_pci_read32(ar, ce_ctrl_addr +
+				 ar->hw_ce_regs->current_drri_addr);
 }
 
 static inline void ath10k_ce_dest_ring_base_addr_set(struct ath10k *ar,
 						     u32 ce_ctrl_addr,
 						     u32 addr)
 {
-	ath10k_pci_write32(ar, ce_ctrl_addr + DR_BA_ADDRESS, addr);
+	ath10k_pci_write32(ar, ce_ctrl_addr +
+			ar->hw_ce_regs->dr_base_addr, addr);
 }
 
 static inline void ath10k_ce_dest_ring_size_set(struct ath10k *ar,
 						u32 ce_ctrl_addr,
 						unsigned int n)
 {
-	ath10k_pci_write32(ar, ce_ctrl_addr + DR_SIZE_ADDRESS, n);
+	ath10k_pci_write32(ar, ce_ctrl_addr +
+			ar->hw_ce_regs->dr_size_addr, n);
 }
 
 static inline void ath10k_ce_src_ring_highmark_set(struct ath10k *ar,
 						   u32 ce_ctrl_addr,
 						   unsigned int n)
 {
-	u32 addr = ath10k_pci_read32(ar, ce_ctrl_addr + SRC_WATERMARK_ADDRESS);
+	struct ath10k_hw_ce_dst_src_wm_regs *srcr_wm = ar->hw_ce_regs->wm_srcr;
+	u32 addr = ath10k_pci_read32(ar, ce_ctrl_addr + srcr_wm->addr);
 
-	ath10k_pci_write32(ar, ce_ctrl_addr + SRC_WATERMARK_ADDRESS,
-			   (addr & ~SRC_WATERMARK_HIGH_MASK) |
-			   SRC_WATERMARK_HIGH_SET(n));
+	ath10k_pci_write32(ar, ce_ctrl_addr + srcr_wm->addr,
+			   (addr & ~(srcr_wm->wm_high->mask)) |
+			   (ath10k_set_ring_byte(n, srcr_wm->wm_high)));
 }
 
 static inline void ath10k_ce_src_ring_lowmark_set(struct ath10k *ar,
 						  u32 ce_ctrl_addr,
 						  unsigned int n)
 {
-	u32 addr = ath10k_pci_read32(ar, ce_ctrl_addr + SRC_WATERMARK_ADDRESS);
+	struct ath10k_hw_ce_dst_src_wm_regs *srcr_wm = ar->hw_ce_regs->wm_srcr;
+	u32 addr = ath10k_pci_read32(ar, ce_ctrl_addr + srcr_wm->addr);
 
-	ath10k_pci_write32(ar, ce_ctrl_addr + SRC_WATERMARK_ADDRESS,
-			   (addr & ~SRC_WATERMARK_LOW_MASK) |
-			   SRC_WATERMARK_LOW_SET(n));
+	ath10k_pci_write32(ar, ce_ctrl_addr + srcr_wm->addr,
+			   (addr & ~(srcr_wm->wm_low->mask)) |
+			   (ath10k_set_ring_byte(n, srcr_wm->wm_low)));
 }
 
 static inline void ath10k_ce_dest_ring_highmark_set(struct ath10k *ar,
 						    u32 ce_ctrl_addr,
 						    unsigned int n)
 {
-	u32 addr = ath10k_pci_read32(ar, ce_ctrl_addr + DST_WATERMARK_ADDRESS);
+	struct ath10k_hw_ce_dst_src_wm_regs *dstr_wm = ar->hw_ce_regs->wm_dstr;
+	u32 addr = ath10k_pci_read32(ar, ce_ctrl_addr + dstr_wm->addr);
 
-	ath10k_pci_write32(ar, ce_ctrl_addr + DST_WATERMARK_ADDRESS,
-			   (addr & ~DST_WATERMARK_HIGH_MASK) |
-			   DST_WATERMARK_HIGH_SET(n));
+	ath10k_pci_write32(ar, ce_ctrl_addr + dstr_wm->addr,
+			   (addr & ~(dstr_wm->wm_high->mask)) |
+			   (ath10k_set_ring_byte(n, dstr_wm->wm_high)));
 }
 
 static inline void ath10k_ce_dest_ring_lowmark_set(struct ath10k *ar,
 						   u32 ce_ctrl_addr,
 						   unsigned int n)
 {
-	u32 addr = ath10k_pci_read32(ar, ce_ctrl_addr + DST_WATERMARK_ADDRESS);
+	struct ath10k_hw_ce_dst_src_wm_regs *dstr_wm = ar->hw_ce_regs->wm_dstr;
+	u32 addr = ath10k_pci_read32(ar, ce_ctrl_addr + dstr_wm->addr);
 
-	ath10k_pci_write32(ar, ce_ctrl_addr + DST_WATERMARK_ADDRESS,
-			   (addr & ~DST_WATERMARK_LOW_MASK) |
-			   DST_WATERMARK_LOW_SET(n));
+	ath10k_pci_write32(ar, ce_ctrl_addr + dstr_wm->addr,
+			   (addr & ~(dstr_wm->wm_low->mask)) |
+			   (ath10k_set_ring_byte(n, dstr_wm->wm_low)));
 }
 
 static inline void ath10k_ce_copy_complete_inter_enable(struct ath10k *ar,
 							u32 ce_ctrl_addr)
 {
-	u32 host_ie_addr = ath10k_pci_read32(ar,
-					     ce_ctrl_addr + HOST_IE_ADDRESS);
+	struct ath10k_hw_ce_host_ie *host_ie = ar->hw_ce_regs->host_ie;
+	u32 host_ie_addr = ath10k_pci_read32(ar, ce_ctrl_addr +
+					ar->hw_ce_regs->host_ie_addr);
 
-	ath10k_pci_write32(ar, ce_ctrl_addr + HOST_IE_ADDRESS,
-			   host_ie_addr | HOST_IE_COPY_COMPLETE_MASK);
+	ath10k_pci_write32(ar, ce_ctrl_addr + ar->hw_ce_regs->host_ie_addr,
+			   host_ie_addr | host_ie->copy_complete->mask);
 }
 
 static inline void ath10k_ce_copy_complete_intr_disable(struct ath10k *ar,
 							u32 ce_ctrl_addr)
 {
-	u32 host_ie_addr = ath10k_pci_read32(ar,
-					     ce_ctrl_addr + HOST_IE_ADDRESS);
+	struct ath10k_hw_ce_host_ie *host_ie = ar->hw_ce_regs->host_ie;
+	u32 host_ie_addr = ath10k_pci_read32(ar, ce_ctrl_addr +
+					ar->hw_ce_regs->host_ie_addr);
 
-	ath10k_pci_write32(ar, ce_ctrl_addr + HOST_IE_ADDRESS,
-			   host_ie_addr & ~HOST_IE_COPY_COMPLETE_MASK);
+	ath10k_pci_write32(ar, ce_ctrl_addr + ar->hw_ce_regs->host_ie_addr,
+			   host_ie_addr & ~(host_ie->copy_complete->mask));
 }
 
 static inline void ath10k_ce_watermark_intr_disable(struct ath10k *ar,
 						    u32 ce_ctrl_addr)
 {
-	u32 host_ie_addr = ath10k_pci_read32(ar,
-					     ce_ctrl_addr + HOST_IE_ADDRESS);
+	struct ath10k_hw_ce_host_wm_regs *wm_regs = ar->hw_ce_regs->wm_regs;
+	u32 host_ie_addr = ath10k_pci_read32(ar, ce_ctrl_addr +
+					ar->hw_ce_regs->host_ie_addr);
 
-	ath10k_pci_write32(ar, ce_ctrl_addr + HOST_IE_ADDRESS,
-			   host_ie_addr & ~CE_WATERMARK_MASK);
+	ath10k_pci_write32(ar, ce_ctrl_addr + ar->hw_ce_regs->host_ie_addr,
+			   host_ie_addr & ~(wm_regs->wm_mask));
 }
 
 static inline void ath10k_ce_error_intr_enable(struct ath10k *ar,
 					       u32 ce_ctrl_addr)
 {
-	u32 misc_ie_addr = ath10k_pci_read32(ar,
-					     ce_ctrl_addr + MISC_IE_ADDRESS);
+	struct ath10k_hw_ce_misc_regs *misc_regs = ar->hw_ce_regs->misc_regs;
+	u32 misc_ie_addr = ath10k_pci_read32(ar, ce_ctrl_addr +
+					ar->hw_ce_regs->misc_ie_addr);
 
-	ath10k_pci_write32(ar, ce_ctrl_addr + MISC_IE_ADDRESS,
-			   misc_ie_addr | CE_ERROR_MASK);
+	ath10k_pci_write32(ar, ce_ctrl_addr + ar->hw_ce_regs->misc_ie_addr,
+			   misc_ie_addr | misc_regs->err_mask);
 }
 
 static inline void ath10k_ce_error_intr_disable(struct ath10k *ar,
 						u32 ce_ctrl_addr)
 {
-	u32 misc_ie_addr = ath10k_pci_read32(ar,
-					     ce_ctrl_addr + MISC_IE_ADDRESS);
+	struct ath10k_hw_ce_misc_regs *misc_regs = ar->hw_ce_regs->misc_regs;
+	u32 misc_ie_addr = ath10k_pci_read32(ar, ce_ctrl_addr +
+					ar->hw_ce_regs->misc_ie_addr);
 
-	ath10k_pci_write32(ar, ce_ctrl_addr + MISC_IE_ADDRESS,
-			   misc_ie_addr & ~CE_ERROR_MASK);
+	ath10k_pci_write32(ar, ce_ctrl_addr + ar->hw_ce_regs->misc_ie_addr,
+			   misc_ie_addr & ~(misc_regs->err_mask));
 }
 
 static inline void ath10k_ce_engine_int_status_clear(struct ath10k *ar,
 						     u32 ce_ctrl_addr,
 						     unsigned int mask)
 {
-	ath10k_pci_write32(ar, ce_ctrl_addr + HOST_IS_ADDRESS, mask);
+	struct ath10k_hw_ce_host_wm_regs *wm_regs = ar->hw_ce_regs->wm_regs;
+
+	ath10k_pci_write32(ar, ce_ctrl_addr + wm_regs->addr, mask);
 }
 
 /*
@@ -594,6 +632,7 @@ int ath10k_ce_completed_send_next_nolock(struct ath10k_ce_pipe *ce_state,
 	unsigned int nentries_mask = src_ring->nentries_mask;
 	unsigned int sw_index = src_ring->sw_index;
 	unsigned int read_index;
+	struct ce_desc *desc;
 
 	if (src_ring->hw_index == sw_index) {
 		/*
@@ -623,6 +662,9 @@ int ath10k_ce_completed_send_next_nolock(struct ath10k_ce_pipe *ce_state,
 
 	/* sanity */
 	src_ring->per_transfer_context[sw_index] = NULL;
+	desc = CE_SRC_RING_TO_DESC(src_ring->base_addr_owner_space,
+				   sw_index);
+	desc->nbytes = 0;
 
 	/* Update sw_index */
 	sw_index = CE_RING_IDX_INCR(nentries_mask, sw_index);
@@ -715,13 +757,13 @@ void ath10k_ce_per_engine_service(struct ath10k *ar, unsigned int ce_id)
 {
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 	struct ath10k_ce_pipe *ce_state = &ar_pci->ce_states[ce_id];
+	struct ath10k_hw_ce_host_wm_regs *wm_regs = ar->hw_ce_regs->wm_regs;
 	u32 ctrl_addr = ce_state->ctrl_addr;
 
 	spin_lock_bh(&ar_pci->ce_lock);
 
 	/* Clear the copy-complete interrupts that will be handled here. */
-	ath10k_ce_engine_int_status_clear(ar, ctrl_addr,
-					  HOST_IS_COPY_COMPLETE_MASK);
+	ath10k_ce_engine_int_status_clear(ar, ctrl_addr, wm_regs->cc_mask);
 
 	spin_unlock_bh(&ar_pci->ce_lock);
 
@@ -737,7 +779,7 @@ void ath10k_ce_per_engine_service(struct ath10k *ar, unsigned int ce_id)
 	 * Misc CE interrupts are not being handled, but still need
 	 * to be cleared.
 	 */
-	ath10k_ce_engine_int_status_clear(ar, ctrl_addr, CE_WATERMARK_MASK);
+	ath10k_ce_engine_int_status_clear(ar, ctrl_addr, wm_regs->wm_mask);
 
 	spin_unlock_bh(&ar_pci->ce_lock);
 }
diff --git a/drivers/net/wireless/ath/ath10k/ce.h b/drivers/net/wireless/ath/ath10k/ce.h
index e76a98242b98..95743a57525d 100644
--- a/drivers/net/wireless/ath/ath10k/ce.h
+++ b/drivers/net/wireless/ath/ath10k/ce.h
@@ -263,143 +263,11 @@ struct ce_attr {
 	void (*recv_cb)(struct ath10k_ce_pipe *);
 };
 
-#define SR_BA_ADDRESS		0x0000
-#define SR_SIZE_ADDRESS		0x0004
-#define DR_BA_ADDRESS		0x0008
-#define DR_SIZE_ADDRESS		0x000c
-#define CE_CMD_ADDRESS		0x0018
-
-#define CE_CTRL1_DST_RING_BYTE_SWAP_EN_MSB	17
-#define CE_CTRL1_DST_RING_BYTE_SWAP_EN_LSB	17
-#define CE_CTRL1_DST_RING_BYTE_SWAP_EN_MASK	0x00020000
-#define CE_CTRL1_DST_RING_BYTE_SWAP_EN_SET(x) \
-	(((0 | (x)) << CE_CTRL1_DST_RING_BYTE_SWAP_EN_LSB) & \
-	CE_CTRL1_DST_RING_BYTE_SWAP_EN_MASK)
-
-#define CE_CTRL1_SRC_RING_BYTE_SWAP_EN_MSB	16
-#define CE_CTRL1_SRC_RING_BYTE_SWAP_EN_LSB	16
-#define CE_CTRL1_SRC_RING_BYTE_SWAP_EN_MASK	0x00010000
-#define CE_CTRL1_SRC_RING_BYTE_SWAP_EN_GET(x) \
-	(((x) & CE_CTRL1_SRC_RING_BYTE_SWAP_EN_MASK) >> \
-	 CE_CTRL1_SRC_RING_BYTE_SWAP_EN_LSB)
-#define CE_CTRL1_SRC_RING_BYTE_SWAP_EN_SET(x) \
-	(((0 | (x)) << CE_CTRL1_SRC_RING_BYTE_SWAP_EN_LSB) & \
-	 CE_CTRL1_SRC_RING_BYTE_SWAP_EN_MASK)
-
-#define CE_CTRL1_DMAX_LENGTH_MSB		15
-#define CE_CTRL1_DMAX_LENGTH_LSB		0
-#define CE_CTRL1_DMAX_LENGTH_MASK		0x0000ffff
-#define CE_CTRL1_DMAX_LENGTH_GET(x) \
-	(((x) & CE_CTRL1_DMAX_LENGTH_MASK) >> CE_CTRL1_DMAX_LENGTH_LSB)
-#define CE_CTRL1_DMAX_LENGTH_SET(x) \
-	(((0 | (x)) << CE_CTRL1_DMAX_LENGTH_LSB) & CE_CTRL1_DMAX_LENGTH_MASK)
-
-#define CE_CTRL1_ADDRESS			0x0010
-#define CE_CTRL1_HW_MASK			0x0007ffff
-#define CE_CTRL1_SW_MASK			0x0007ffff
-#define CE_CTRL1_HW_WRITE_MASK			0x00000000
-#define CE_CTRL1_SW_WRITE_MASK			0x0007ffff
-#define CE_CTRL1_RSTMASK			0xffffffff
-#define CE_CTRL1_RESET				0x00000080
-
-#define CE_CMD_HALT_STATUS_MSB			3
-#define CE_CMD_HALT_STATUS_LSB			3
-#define CE_CMD_HALT_STATUS_MASK			0x00000008
-#define CE_CMD_HALT_STATUS_GET(x) \
-	(((x) & CE_CMD_HALT_STATUS_MASK) >> CE_CMD_HALT_STATUS_LSB)
-#define CE_CMD_HALT_STATUS_SET(x) \
-	(((0 | (x)) << CE_CMD_HALT_STATUS_LSB) & CE_CMD_HALT_STATUS_MASK)
-#define CE_CMD_HALT_STATUS_RESET		0
-#define CE_CMD_HALT_MSB				0
-#define CE_CMD_HALT_MASK			0x00000001
-
-#define HOST_IE_COPY_COMPLETE_MSB		0
-#define HOST_IE_COPY_COMPLETE_LSB		0
-#define HOST_IE_COPY_COMPLETE_MASK		0x00000001
-#define HOST_IE_COPY_COMPLETE_GET(x) \
-	(((x) & HOST_IE_COPY_COMPLETE_MASK) >> HOST_IE_COPY_COMPLETE_LSB)
-#define HOST_IE_COPY_COMPLETE_SET(x) \
-	(((0 | (x)) << HOST_IE_COPY_COMPLETE_LSB) & HOST_IE_COPY_COMPLETE_MASK)
-#define HOST_IE_COPY_COMPLETE_RESET		0
-#define HOST_IE_ADDRESS				0x002c
-
-#define HOST_IS_DST_RING_LOW_WATERMARK_MASK	0x00000010
-#define HOST_IS_DST_RING_HIGH_WATERMARK_MASK	0x00000008
-#define HOST_IS_SRC_RING_LOW_WATERMARK_MASK	0x00000004
-#define HOST_IS_SRC_RING_HIGH_WATERMARK_MASK	0x00000002
-#define HOST_IS_COPY_COMPLETE_MASK		0x00000001
-#define HOST_IS_ADDRESS				0x0030
-
-#define MISC_IE_ADDRESS				0x0034
-
-#define MISC_IS_AXI_ERR_MASK			0x00000400
-
-#define MISC_IS_DST_ADDR_ERR_MASK		0x00000200
-#define MISC_IS_SRC_LEN_ERR_MASK		0x00000100
-#define MISC_IS_DST_MAX_LEN_VIO_MASK		0x00000080
-#define MISC_IS_DST_RING_OVERFLOW_MASK		0x00000040
-#define MISC_IS_SRC_RING_OVERFLOW_MASK		0x00000020
-
-#define MISC_IS_ADDRESS				0x0038
-
-#define SR_WR_INDEX_ADDRESS			0x003c
-
-#define DST_WR_INDEX_ADDRESS			0x0040
-
-#define CURRENT_SRRI_ADDRESS			0x0044
-
-#define CURRENT_DRRI_ADDRESS			0x0048
-
-#define SRC_WATERMARK_LOW_MSB			31
-#define SRC_WATERMARK_LOW_LSB			16
-#define SRC_WATERMARK_LOW_MASK			0xffff0000
-#define SRC_WATERMARK_LOW_GET(x) \
-	(((x) & SRC_WATERMARK_LOW_MASK) >> SRC_WATERMARK_LOW_LSB)
-#define SRC_WATERMARK_LOW_SET(x) \
-	(((0 | (x)) << SRC_WATERMARK_LOW_LSB) & SRC_WATERMARK_LOW_MASK)
-#define SRC_WATERMARK_LOW_RESET			0
-#define SRC_WATERMARK_HIGH_MSB			15
-#define SRC_WATERMARK_HIGH_LSB			0
-#define SRC_WATERMARK_HIGH_MASK			0x0000ffff
-#define SRC_WATERMARK_HIGH_GET(x) \
-	(((x) & SRC_WATERMARK_HIGH_MASK) >> SRC_WATERMARK_HIGH_LSB)
-#define SRC_WATERMARK_HIGH_SET(x) \
-	(((0 | (x)) << SRC_WATERMARK_HIGH_LSB) & SRC_WATERMARK_HIGH_MASK)
-#define SRC_WATERMARK_HIGH_RESET		0
-#define SRC_WATERMARK_ADDRESS			0x004c
-
-#define DST_WATERMARK_LOW_LSB			16
-#define DST_WATERMARK_LOW_MASK			0xffff0000
-#define DST_WATERMARK_LOW_SET(x) \
-	(((0 | (x)) << DST_WATERMARK_LOW_LSB) & DST_WATERMARK_LOW_MASK)
-#define DST_WATERMARK_LOW_RESET			0
-#define DST_WATERMARK_HIGH_MSB			15
-#define DST_WATERMARK_HIGH_LSB			0
-#define DST_WATERMARK_HIGH_MASK			0x0000ffff
-#define DST_WATERMARK_HIGH_GET(x) \
-	(((x) & DST_WATERMARK_HIGH_MASK) >> DST_WATERMARK_HIGH_LSB)
-#define DST_WATERMARK_HIGH_SET(x) \
-	(((0 | (x)) << DST_WATERMARK_HIGH_LSB) & DST_WATERMARK_HIGH_MASK)
-#define DST_WATERMARK_HIGH_RESET		0
-#define DST_WATERMARK_ADDRESS			0x0050
-
 static inline u32 ath10k_ce_base_address(struct ath10k *ar, unsigned int ce_id)
 {
 	return CE0_BASE_ADDRESS + (CE1_BASE_ADDRESS - CE0_BASE_ADDRESS) * ce_id;
 }
 
-#define CE_WATERMARK_MASK (HOST_IS_SRC_RING_LOW_WATERMARK_MASK  | \
-			   HOST_IS_SRC_RING_HIGH_WATERMARK_MASK | \
-			   HOST_IS_DST_RING_LOW_WATERMARK_MASK  | \
-			   HOST_IS_DST_RING_HIGH_WATERMARK_MASK)
-
-#define CE_ERROR_MASK	(MISC_IS_AXI_ERR_MASK           | \
-			 MISC_IS_DST_ADDR_ERR_MASK      | \
-			 MISC_IS_SRC_LEN_ERR_MASK       | \
-			 MISC_IS_DST_MAX_LEN_VIO_MASK   | \
-			 MISC_IS_DST_RING_OVERFLOW_MASK | \
-			 MISC_IS_SRC_RING_OVERFLOW_MASK)
-
 #define CE_SRC_RING_TO_DESC(baddr, idx) \
 	(&(((struct ce_desc *)baddr)[idx]))
 
diff --git a/drivers/net/wireless/ath/ath10k/core.c b/drivers/net/wireless/ath/ath10k/core.c
index 5a0638915874..75c5c903c8a6 100644
--- a/drivers/net/wireless/ath/ath10k/core.c
+++ b/drivers/net/wireless/ath/ath10k/core.c
@@ -72,6 +72,8 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.hw_ops = &qca988x_ops,
 		.decap_align_bytes = 4,
 		.spectral_bin_discard = 0,
+		.vht160_mcs_rx_highest = 0,
+		.vht160_mcs_tx_highest = 0,
 	},
 	{
 		.id = QCA9887_HW_1_0_VERSION,
@@ -93,6 +95,8 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.hw_ops = &qca988x_ops,
 		.decap_align_bytes = 4,
 		.spectral_bin_discard = 0,
+		.vht160_mcs_rx_highest = 0,
+		.vht160_mcs_tx_highest = 0,
 	},
 	{
 		.id = QCA6174_HW_2_1_VERSION,
@@ -113,6 +117,8 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.hw_ops = &qca988x_ops,
 		.decap_align_bytes = 4,
 		.spectral_bin_discard = 0,
+		.vht160_mcs_rx_highest = 0,
+		.vht160_mcs_tx_highest = 0,
 	},
 	{
 		.id = QCA6174_HW_2_1_VERSION,
@@ -133,6 +139,8 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.hw_ops = &qca988x_ops,
 		.decap_align_bytes = 4,
 		.spectral_bin_discard = 0,
+		.vht160_mcs_rx_highest = 0,
+		.vht160_mcs_tx_highest = 0,
 	},
 	{
 		.id = QCA6174_HW_3_0_VERSION,
@@ -153,6 +161,8 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.hw_ops = &qca988x_ops,
 		.decap_align_bytes = 4,
 		.spectral_bin_discard = 0,
+		.vht160_mcs_rx_highest = 0,
+		.vht160_mcs_tx_highest = 0,
 	},
 	{
 		.id = QCA6174_HW_3_2_VERSION,
@@ -176,6 +186,8 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.target_cpu_freq = 176000000,
 		.decap_align_bytes = 4,
 		.spectral_bin_discard = 0,
+		.vht160_mcs_rx_highest = 0,
+		.vht160_mcs_tx_highest = 0,
 	},
 	{
 		.id = QCA99X0_HW_2_0_DEV_VERSION,
@@ -202,6 +214,8 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.hw_ops = &qca99x0_ops,
 		.decap_align_bytes = 1,
 		.spectral_bin_discard = 4,
+		.vht160_mcs_rx_highest = 0,
+		.vht160_mcs_tx_highest = 0,
 	},
 	{
 		.id = QCA9984_HW_1_0_DEV_VERSION,
@@ -229,6 +243,12 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.hw_ops = &qca99x0_ops,
 		.decap_align_bytes = 1,
 		.spectral_bin_discard = 12,
+
+		/* Can do only 2x2 VHT160 or 80+80. 1560Mbps is 4x4 80Mhz
+		 * or 2x2 160Mhz, long-guard-interval.
+		 */
+		.vht160_mcs_rx_highest = 1560,
+		.vht160_mcs_tx_highest = 1560,
 	},
 	{
 		.id = QCA9888_HW_2_0_DEV_VERSION,
@@ -255,6 +275,12 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.hw_ops = &qca99x0_ops,
 		.decap_align_bytes = 1,
 		.spectral_bin_discard = 12,
+
+		/* Can do only 1x1 VHT160 or 80+80. 780Mbps is 2x2 80Mhz or
+		 * 1x1 160Mhz, long-guard-interval.
+		 */
+		.vht160_mcs_rx_highest = 780,
+		.vht160_mcs_tx_highest = 780,
 	},
 	{
 		.id = QCA9377_HW_1_0_DEV_VERSION,
@@ -275,6 +301,8 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.hw_ops = &qca988x_ops,
 		.decap_align_bytes = 4,
 		.spectral_bin_discard = 0,
+		.vht160_mcs_rx_highest = 0,
+		.vht160_mcs_tx_highest = 0,
 	},
 	{
 		.id = QCA9377_HW_1_1_DEV_VERSION,
@@ -297,6 +325,8 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.target_cpu_freq = 176000000,
 		.decap_align_bytes = 4,
 		.spectral_bin_discard = 0,
+		.vht160_mcs_rx_highest = 0,
+		.vht160_mcs_tx_highest = 0,
 	},
 	{
 		.id = QCA4019_HW_1_0_DEV_VERSION,
@@ -324,6 +354,8 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.hw_ops = &qca99x0_ops,
 		.decap_align_bytes = 1,
 		.spectral_bin_discard = 4,
+		.vht160_mcs_rx_highest = 0,
+		.vht160_mcs_tx_highest = 0,
 	},
 };
 
@@ -389,6 +421,21 @@ static void ath10k_send_suspend_complete(struct ath10k *ar)
 	complete(&ar->target_suspend);
 }
 
+static void ath10k_init_sdio(struct ath10k *ar)
+{
+	u32 param = 0;
+
+	ath10k_bmi_write32(ar, hi_mbox_io_block_sz, 256);
+	ath10k_bmi_write32(ar, hi_mbox_isr_yield_limit, 99);
+	ath10k_bmi_read32(ar, hi_acs_flags, &param);
+
+	param |= (HI_ACS_FLAGS_SDIO_SWAP_MAILBOX_SET |
+		  HI_ACS_FLAGS_SDIO_REDUCE_TX_COMPL_SET |
+		  HI_ACS_FLAGS_ALT_DATA_CREDIT_SIZE);
+
+	ath10k_bmi_write32(ar, hi_acs_flags, param);
+}
+
 static int ath10k_init_configure_target(struct ath10k *ar)
 {
 	u32 param_host;
@@ -676,7 +723,7 @@ static int ath10k_core_get_board_id_from_otp(struct ath10k *ar)
 {
 	u32 result, address;
 	u8 board_id, chip_id;
-	int ret;
+	int ret, bmi_board_id_param;
 
 	address = ar->hw_params.patch_load_addr;
 
@@ -700,8 +747,13 @@ static int ath10k_core_get_board_id_from_otp(struct ath10k *ar)
 		return ret;
 	}
 
-	ret = ath10k_bmi_execute(ar, address, BMI_PARAM_GET_EEPROM_BOARD_ID,
-				 &result);
+	if (ar->cal_mode == ATH10K_PRE_CAL_MODE_DT ||
+	    ar->cal_mode == ATH10K_PRE_CAL_MODE_FILE)
+		bmi_board_id_param = BMI_PARAM_GET_FLASH_BOARD_ID;
+	else
+		bmi_board_id_param = BMI_PARAM_GET_EEPROM_BOARD_ID;
+
+	ret = ath10k_bmi_execute(ar, address, bmi_board_id_param, &result);
 	if (ret) {
 		ath10k_err(ar, "could not execute otp for board id check: %d\n",
 			   ret);
@@ -830,6 +882,11 @@ static int ath10k_download_and_run_otp(struct ath10k *ar)
 		return ret;
 	}
 
+	/* As of now pre-cal is valid for 10_4 variants */
+	if (ar->cal_mode == ATH10K_PRE_CAL_MODE_DT ||
+	    ar->cal_mode == ATH10K_PRE_CAL_MODE_FILE)
+		bmi_otp_exe_param = BMI_PARAM_FLASH_SECTION_ALL;
+
 	ret = ath10k_bmi_execute(ar, address, bmi_otp_exe_param, &result);
 	if (ret) {
 		ath10k_err(ar, "could not execute otp (%d)\n", ret);
@@ -1395,7 +1452,18 @@ err:
 static void ath10k_core_get_fw_name(struct ath10k *ar, char *fw_name,
 				    size_t fw_name_len, int fw_api)
 {
-	scnprintf(fw_name, fw_name_len, "%s-%d.bin", ATH10K_FW_FILE_BASE, fw_api);
+	switch (ar->hif.bus) {
+	case ATH10K_BUS_SDIO:
+		scnprintf(fw_name, fw_name_len, "%s-%s-%d.bin",
+			  ATH10K_FW_FILE_BASE, ath10k_bus_str(ar->hif.bus),
+			  fw_api);
+		break;
+	case ATH10K_BUS_PCI:
+	case ATH10K_BUS_AHB:
+		scnprintf(fw_name, fw_name_len, "%s-%d.bin",
+			  ATH10K_FW_FILE_BASE, fw_api);
+		break;
+	}
 }
 
 static int ath10k_core_fetch_firmware_files(struct ath10k *ar)
@@ -1953,6 +2021,9 @@ int ath10k_core_start(struct ath10k *ar, enum ath10k_firmware_mode mode,
 	if (status)
 		goto err;
 
+	if (ar->hif.bus == ATH10K_BUS_SDIO)
+		ath10k_init_sdio(ar);
+
 	ar->htc.htc_ops.target_send_suspend_complete =
 		ath10k_send_suspend_complete;
 
@@ -2200,7 +2271,10 @@ static int ath10k_core_probe_fw(struct ath10k *ar)
 	}
 
 	memset(&target_info, 0, sizeof(target_info));
-	ret = ath10k_bmi_get_target_info(ar, &target_info);
+	if (ar->hif.bus == ATH10K_BUS_SDIO)
+		ret = ath10k_bmi_get_target_info_sdio(ar, &target_info);
+	else
+		ret = ath10k_bmi_get_target_info(ar, &target_info);
 	if (ret) {
 		ath10k_err(ar, "could not get target info (%d)\n", ret);
 		goto err_power_down;
@@ -2417,24 +2491,29 @@ struct ath10k *ath10k_core_create(size_t priv_size, struct device *dev,
 	case ATH10K_HW_QCA988X:
 	case ATH10K_HW_QCA9887:
 		ar->regs = &qca988x_regs;
+		ar->hw_ce_regs = &qcax_ce_regs;
 		ar->hw_values = &qca988x_values;
 		break;
 	case ATH10K_HW_QCA6174:
 	case ATH10K_HW_QCA9377:
 		ar->regs = &qca6174_regs;
+		ar->hw_ce_regs = &qcax_ce_regs;
 		ar->hw_values = &qca6174_values;
 		break;
 	case ATH10K_HW_QCA99X0:
 	case ATH10K_HW_QCA9984:
 		ar->regs = &qca99x0_regs;
+		ar->hw_ce_regs = &qcax_ce_regs;
 		ar->hw_values = &qca99x0_values;
 		break;
 	case ATH10K_HW_QCA9888:
 		ar->regs = &qca99x0_regs;
+		ar->hw_ce_regs = &qcax_ce_regs;
 		ar->hw_values = &qca9888_values;
 		break;
 	case ATH10K_HW_QCA4019:
 		ar->regs = &qca4019_regs;
+		ar->hw_ce_regs = &qcax_ce_regs;
 		ar->hw_values = &qca4019_values;
 		break;
 	default:
diff --git a/drivers/net/wireless/ath/ath10k/core.h b/drivers/net/wireless/ath/ath10k/core.h
index bf091514ecc6..1aa5cf12fce0 100644
--- a/drivers/net/wireless/ath/ath10k/core.h
+++ b/drivers/net/wireless/ath/ath10k/core.h
@@ -91,6 +91,7 @@ struct ath10k;
 enum ath10k_bus {
 	ATH10K_BUS_PCI,
 	ATH10K_BUS_AHB,
+	ATH10K_BUS_SDIO,
 };
 
 static inline const char *ath10k_bus_str(enum ath10k_bus bus)
@@ -100,6 +101,8 @@ static inline const char *ath10k_bus_str(enum ath10k_bus bus)
 		return "pci";
 	case ATH10K_BUS_AHB:
 		return "ahb";
+	case ATH10K_BUS_SDIO:
+		return "sdio";
 	}
 
 	return "unknown";
@@ -791,6 +794,7 @@ struct ath10k {
 	struct completion target_suspend;
 
 	const struct ath10k_hw_regs *regs;
+	const struct ath10k_hw_ce_regs *hw_ce_regs;
 	const struct ath10k_hw_values *hw_values;
 	struct ath10k_bmi bmi;
 	struct ath10k_wmi wmi;
diff --git a/drivers/net/wireless/ath/ath10k/debug.c b/drivers/net/wireless/ath/ath10k/debug.c
index 4cd2a0fd49d6..389fcb7a9fd0 100644
--- a/drivers/net/wireless/ath/ath10k/debug.c
+++ b/drivers/net/wireless/ath/ath10k/debug.c
@@ -625,17 +625,21 @@ static ssize_t ath10k_write_simulate_fw_crash(struct file *file,
 					      size_t count, loff_t *ppos)
 {
 	struct ath10k *ar = file->private_data;
-	char buf[32];
+	char buf[32] = {0};
+	ssize_t rc;
 	int ret;
 
-	simple_write_to_buffer(buf, sizeof(buf) - 1, ppos, user_buf, count);
+	/* filter partial writes and invalid commands */
+	if (*ppos != 0 || count >= sizeof(buf) || count == 0)
+		return -EINVAL;
 
-	/* make sure that buf is null terminated */
-	buf[sizeof(buf) - 1] = 0;
+	rc = simple_write_to_buffer(buf, sizeof(buf) - 1, ppos, user_buf, count);
+	if (rc < 0)
+		return rc;
 
 	/* drop the possible '\n' from the end */
-	if (buf[count - 1] == '\n')
-		buf[count - 1] = 0;
+	if (buf[*ppos - 1] == '\n')
+		buf[*ppos - 1] = '\0';
 
 	mutex_lock(&ar->conf_mutex);
 
diff --git a/drivers/net/wireless/ath/ath10k/debug.h b/drivers/net/wireless/ath/ath10k/debug.h
index 2368f47314ae..257d10985c6e 100644
--- a/drivers/net/wireless/ath/ath10k/debug.h
+++ b/drivers/net/wireless/ath/ath10k/debug.h
@@ -38,6 +38,8 @@ enum ath10k_debug_mask {
 	ATH10K_DBG_WMI_PRINT	= 0x00002000,
 	ATH10K_DBG_PCI_PS	= 0x00004000,
 	ATH10K_DBG_AHB		= 0x00008000,
+	ATH10K_DBG_SDIO		= 0x00010000,
+	ATH10K_DBG_SDIO_DUMP	= 0x00020000,
 	ATH10K_DBG_ANY		= 0xffffffff,
 };
 
diff --git a/drivers/net/wireless/ath/ath10k/htc.c b/drivers/net/wireless/ath/ath10k/htc.c
index b7669b2e94aa..e5c80f582ff5 100644
--- a/drivers/net/wireless/ath/ath10k/htc.c
+++ b/drivers/net/wireless/ath/ath10k/htc.c
@@ -57,8 +57,8 @@ static inline void ath10k_htc_restore_tx_skb(struct ath10k_htc *htc,
 	skb_pull(skb, sizeof(struct ath10k_htc_hdr));
 }
 
-static void ath10k_htc_notify_tx_completion(struct ath10k_htc_ep *ep,
-					    struct sk_buff *skb)
+void ath10k_htc_notify_tx_completion(struct ath10k_htc_ep *ep,
+				     struct sk_buff *skb)
 {
 	struct ath10k *ar = ep->htc->ar;
 
@@ -75,6 +75,7 @@ static void ath10k_htc_notify_tx_completion(struct ath10k_htc_ep *ep,
 
 	ep->ep_ops.ep_tx_complete(ep->htc->ar, skb);
 }
+EXPORT_SYMBOL(ath10k_htc_notify_tx_completion);
 
 static void ath10k_htc_prepare_tx_skb(struct ath10k_htc_ep *ep,
 				      struct sk_buff *skb)
@@ -230,12 +231,79 @@ ath10k_htc_process_credit_report(struct ath10k_htc *htc,
 	spin_unlock_bh(&htc->tx_lock);
 }
 
-static int ath10k_htc_process_trailer(struct ath10k_htc *htc,
-				      u8 *buffer,
-				      int length,
-				      enum ath10k_htc_ep_id src_eid)
+static int
+ath10k_htc_process_lookahead(struct ath10k_htc *htc,
+			     const struct ath10k_htc_lookahead_report *report,
+			     int len,
+			     enum ath10k_htc_ep_id eid,
+			     void *next_lookaheads,
+			     int *next_lookaheads_len)
 {
 	struct ath10k *ar = htc->ar;
+
+	/* Invalid lookahead flags are actually transmitted by
+	 * the target in the HTC control message.
+	 * Since this will happen at every boot we silently ignore
+	 * the lookahead in this case
+	 */
+	if (report->pre_valid != ((~report->post_valid) & 0xFF))
+		return 0;
+
+	if (next_lookaheads && next_lookaheads_len) {
+		ath10k_dbg(ar, ATH10K_DBG_HTC,
+			   "htc rx lookahead found pre_valid 0x%x post_valid 0x%x\n",
+			   report->pre_valid, report->post_valid);
+
+		/* look ahead bytes are valid, copy them over */
+		memcpy((u8 *)next_lookaheads, report->lookahead, 4);
+
+		*next_lookaheads_len = 1;
+	}
+
+	return 0;
+}
+
+static int
+ath10k_htc_process_lookahead_bundle(struct ath10k_htc *htc,
+				    const struct ath10k_htc_lookahead_bundle *report,
+				    int len,
+				    enum ath10k_htc_ep_id eid,
+				    void *next_lookaheads,
+				    int *next_lookaheads_len)
+{
+	struct ath10k *ar = htc->ar;
+	int bundle_cnt = len / sizeof(*report);
+
+	if (!bundle_cnt || (bundle_cnt > HTC_HOST_MAX_MSG_PER_BUNDLE)) {
+		ath10k_warn(ar, "Invalid lookahead bundle count: %d\n",
+			    bundle_cnt);
+		return -EINVAL;
+	}
+
+	if (next_lookaheads && next_lookaheads_len) {
+		int i;
+
+		for (i = 0; i < bundle_cnt; i++) {
+			memcpy(((u8 *)next_lookaheads) + 4 * i,
+			       report->lookahead, 4);
+			report++;
+		}
+
+		*next_lookaheads_len = bundle_cnt;
+	}
+
+	return 0;
+}
+
+int ath10k_htc_process_trailer(struct ath10k_htc *htc,
+			       u8 *buffer,
+			       int length,
+			       enum ath10k_htc_ep_id src_eid,
+			       void *next_lookaheads,
+			       int *next_lookaheads_len)
+{
+	struct ath10k_htc_lookahead_bundle *bundle;
+	struct ath10k *ar = htc->ar;
 	int status = 0;
 	struct ath10k_htc_record *record;
 	u8 *orig_buffer;
@@ -274,6 +342,29 @@ static int ath10k_htc_process_trailer(struct ath10k_htc *htc,
 							 record->hdr.len,
 							 src_eid);
 			break;
+		case ATH10K_HTC_RECORD_LOOKAHEAD:
+			len = sizeof(struct ath10k_htc_lookahead_report);
+			if (record->hdr.len < len) {
+				ath10k_warn(ar, "Lookahead report too long\n");
+				status = -EINVAL;
+				break;
+			}
+			status = ath10k_htc_process_lookahead(htc,
+							      record->lookahead_report,
+							      record->hdr.len,
+							      src_eid,
+							      next_lookaheads,
+							      next_lookaheads_len);
+			break;
+		case ATH10K_HTC_RECORD_LOOKAHEAD_BUNDLE:
+			bundle = record->lookahead_bundle;
+			status = ath10k_htc_process_lookahead_bundle(htc,
+								     bundle,
+								     record->hdr.len,
+								     src_eid,
+								     next_lookaheads,
+								     next_lookaheads_len);
+			break;
 		default:
 			ath10k_warn(ar, "Unhandled record: id:%d length:%d\n",
 				    record->hdr.id, record->hdr.len);
@@ -294,6 +385,7 @@ static int ath10k_htc_process_trailer(struct ath10k_htc *htc,
 
 	return status;
 }
+EXPORT_SYMBOL(ath10k_htc_process_trailer);
 
 void ath10k_htc_rx_completion_handler(struct ath10k *ar, struct sk_buff *skb)
 {
@@ -360,7 +452,8 @@ void ath10k_htc_rx_completion_handler(struct ath10k *ar, struct sk_buff *skb)
 		trailer += payload_len;
 		trailer -= trailer_len;
 		status = ath10k_htc_process_trailer(htc, trailer,
-						    trailer_len, hdr->eid);
+						    trailer_len, hdr->eid,
+						    NULL, NULL);
 		if (status)
 			goto out;
 
@@ -371,42 +464,6 @@ void ath10k_htc_rx_completion_handler(struct ath10k *ar, struct sk_buff *skb)
 		/* zero length packet with trailer data, just drop these */
 		goto out;
 
-	if (eid == ATH10K_HTC_EP_0) {
-		struct ath10k_htc_msg *msg = (struct ath10k_htc_msg *)skb->data;
-
-		switch (__le16_to_cpu(msg->hdr.message_id)) {
-		case ATH10K_HTC_MSG_READY_ID:
-		case ATH10K_HTC_MSG_CONNECT_SERVICE_RESP_ID:
-			/* handle HTC control message */
-			if (completion_done(&htc->ctl_resp)) {
-				/*
-				 * this is a fatal error, target should not be
-				 * sending unsolicited messages on the ep 0
-				 */
-				ath10k_warn(ar, "HTC rx ctrl still processing\n");
-				complete(&htc->ctl_resp);
-				goto out;
-			}
-
-			htc->control_resp_len =
-				min_t(int, skb->len,
-				      ATH10K_HTC_MAX_CTRL_MSG_LEN);
-
-			memcpy(htc->control_resp_buffer, skb->data,
-			       htc->control_resp_len);
-
-			complete(&htc->ctl_resp);
-			break;
-		case ATH10K_HTC_MSG_SEND_SUSPEND_COMPLETE:
-			htc->htc_ops.target_send_suspend_complete(ar);
-			break;
-		default:
-			ath10k_warn(ar, "ignoring unsolicited htc ep0 event\n");
-			break;
-		}
-		goto out;
-	}
-
 	ath10k_dbg(ar, ATH10K_DBG_HTC, "htc rx completion ep %d skb %pK\n",
 		   eid, skb);
 	ep->ep_ops.ep_rx_complete(ar, skb);
@@ -421,10 +478,40 @@ EXPORT_SYMBOL(ath10k_htc_rx_completion_handler);
 static void ath10k_htc_control_rx_complete(struct ath10k *ar,
 					   struct sk_buff *skb)
 {
-	/* This is unexpected. FW is not supposed to send regular rx on this
-	 * endpoint.
-	 */
-	ath10k_warn(ar, "unexpected htc rx\n");
+	struct ath10k_htc *htc = &ar->htc;
+	struct ath10k_htc_msg *msg = (struct ath10k_htc_msg *)skb->data;
+
+	switch (__le16_to_cpu(msg->hdr.message_id)) {
+	case ATH10K_HTC_MSG_READY_ID:
+	case ATH10K_HTC_MSG_CONNECT_SERVICE_RESP_ID:
+		/* handle HTC control message */
+		if (completion_done(&htc->ctl_resp)) {
+			/* this is a fatal error, target should not be
+			 * sending unsolicited messages on the ep 0
+			 */
+			ath10k_warn(ar, "HTC rx ctrl still processing\n");
+			complete(&htc->ctl_resp);
+			goto out;
+		}
+
+		htc->control_resp_len =
+			min_t(int, skb->len,
+			      ATH10K_HTC_MAX_CTRL_MSG_LEN);
+
+		memcpy(htc->control_resp_buffer, skb->data,
+		       htc->control_resp_len);
+
+		complete(&htc->ctl_resp);
+		break;
+	case ATH10K_HTC_MSG_SEND_SUSPEND_COMPLETE:
+		htc->htc_ops.target_send_suspend_complete(ar);
+		break;
+	default:
+		ath10k_warn(ar, "ignoring unsolicited htc ep0 event\n");
+		break;
+	}
+
+out:
 	kfree_skb(skb);
 }
 
@@ -497,12 +584,8 @@ int ath10k_htc_wait_target(struct ath10k_htc *htc)
 	struct ath10k *ar = htc->ar;
 	int i, status = 0;
 	unsigned long time_left;
-	struct ath10k_htc_svc_conn_req conn_req;
-	struct ath10k_htc_svc_conn_resp conn_resp;
 	struct ath10k_htc_msg *msg;
 	u16 message_id;
-	u16 credit_count;
-	u16 credit_size;
 
 	time_left = wait_for_completion_timeout(&htc->ctl_resp,
 						ATH10K_HTC_WAIT_TIMEOUT_HZ);
@@ -539,16 +622,14 @@ int ath10k_htc_wait_target(struct ath10k_htc *htc)
 
 	msg = (struct ath10k_htc_msg *)htc->control_resp_buffer;
 	message_id   = __le16_to_cpu(msg->hdr.message_id);
-	credit_count = __le16_to_cpu(msg->ready.credit_count);
-	credit_size  = __le16_to_cpu(msg->ready.credit_size);
 
 	if (message_id != ATH10K_HTC_MSG_READY_ID) {
 		ath10k_err(ar, "Invalid HTC ready msg: 0x%x\n", message_id);
 		return -ECOMM;
 	}
 
-	htc->total_transmit_credits = credit_count;
-	htc->target_credit_size = credit_size;
+	htc->total_transmit_credits = __le16_to_cpu(msg->ready.credit_count);
+	htc->target_credit_size = __le16_to_cpu(msg->ready.credit_size);
 
 	ath10k_dbg(ar, ATH10K_DBG_HTC,
 		   "Target ready! transmit resources: %d size:%d\n",
@@ -561,20 +642,17 @@ int ath10k_htc_wait_target(struct ath10k_htc *htc)
 		return -ECOMM;
 	}
 
-	/* setup our pseudo HTC control endpoint connection */
-	memset(&conn_req, 0, sizeof(conn_req));
-	memset(&conn_resp, 0, sizeof(conn_resp));
-	conn_req.ep_ops.ep_tx_complete = ath10k_htc_control_tx_complete;
-	conn_req.ep_ops.ep_rx_complete = ath10k_htc_control_rx_complete;
-	conn_req.max_send_queue_depth = ATH10K_NUM_CONTROL_TX_BUFFERS;
-	conn_req.service_id = ATH10K_HTC_SVC_ID_RSVD_CTRL;
-
-	/* connect fake service */
-	status = ath10k_htc_connect_service(htc, &conn_req, &conn_resp);
-	if (status) {
-		ath10k_err(ar, "could not connect to htc service (%d)\n",
-			   status);
-		return status;
+	/* The only way to determine if the ready message is an extended
+	 * message is from the size.
+	 */
+	if (htc->control_resp_len >=
+	    sizeof(msg->hdr) + sizeof(msg->ready_ext)) {
+		htc->max_msgs_per_htc_bundle =
+			min_t(u8, msg->ready_ext.max_msgs_per_htc_bundle,
+			      HTC_HOST_MAX_MSG_PER_BUNDLE);
+		ath10k_dbg(ar, ATH10K_DBG_HTC,
+			   "Extended ready message. RX bundle size: %d\n",
+			   htc->max_msgs_per_htc_bundle);
 	}
 
 	return 0;
@@ -772,6 +850,13 @@ int ath10k_htc_start(struct ath10k_htc *htc)
 	msg->hdr.message_id =
 		__cpu_to_le16(ATH10K_HTC_MSG_SETUP_COMPLETE_EX_ID);
 
+	if (ar->hif.bus == ATH10K_BUS_SDIO) {
+		/* Extra setup params used by SDIO */
+		msg->setup_complete_ext.flags =
+			__cpu_to_le32(ATH10K_HTC_SETUP_COMPLETE_FLAGS_RX_BNDL_EN);
+		msg->setup_complete_ext.max_msgs_per_bundled_recv =
+			htc->max_msgs_per_htc_bundle;
+	}
 	ath10k_dbg(ar, ATH10K_DBG_HTC, "HTC is using TX credit flow control\n");
 
 	status = ath10k_htc_send(htc, ATH10K_HTC_EP_0, skb);
@@ -786,8 +871,10 @@ int ath10k_htc_start(struct ath10k_htc *htc)
 /* registered target arrival callback from the HIF layer */
 int ath10k_htc_init(struct ath10k *ar)
 {
-	struct ath10k_htc_ep *ep = NULL;
+	int status;
 	struct ath10k_htc *htc = &ar->htc;
+	struct ath10k_htc_svc_conn_req conn_req;
+	struct ath10k_htc_svc_conn_resp conn_resp;
 
 	spin_lock_init(&htc->tx_lock);
 
@@ -795,10 +882,21 @@ int ath10k_htc_init(struct ath10k *ar)
 
 	htc->ar = ar;
 
-	/* Get HIF default pipe for HTC message exchange */
-	ep = &htc->endpoint[ATH10K_HTC_EP_0];
+	/* setup our pseudo HTC control endpoint connection */
+	memset(&conn_req, 0, sizeof(conn_req));
+	memset(&conn_resp, 0, sizeof(conn_resp));
+	conn_req.ep_ops.ep_tx_complete = ath10k_htc_control_tx_complete;
+	conn_req.ep_ops.ep_rx_complete = ath10k_htc_control_rx_complete;
+	conn_req.max_send_queue_depth = ATH10K_NUM_CONTROL_TX_BUFFERS;
+	conn_req.service_id = ATH10K_HTC_SVC_ID_RSVD_CTRL;
 
-	ath10k_hif_get_default_pipe(ar, &ep->ul_pipe_id, &ep->dl_pipe_id);
+	/* connect fake service */
+	status = ath10k_htc_connect_service(htc, &conn_req, &conn_resp);
+	if (status) {
+		ath10k_err(ar, "could not connect to htc service (%d)\n",
+			   status);
+		return status;
+	}
 
 	init_completion(&htc->ctl_resp);
 
diff --git a/drivers/net/wireless/ath/ath10k/htc.h b/drivers/net/wireless/ath/ath10k/htc.h
index 6ababa345e2b..24663b07eeac 100644
--- a/drivers/net/wireless/ath/ath10k/htc.h
+++ b/drivers/net/wireless/ath/ath10k/htc.h
@@ -50,6 +50,8 @@ struct ath10k;
  * 4-byte aligned.
  */
 
+#define HTC_HOST_MAX_MSG_PER_BUNDLE        8
+
 enum ath10k_htc_tx_flags {
 	ATH10K_HTC_FLAG_NEED_CREDIT_UPDATE = 0x01,
 	ATH10K_HTC_FLAG_SEND_BUNDLE        = 0x02
@@ -110,6 +112,10 @@ enum ath10k_htc_conn_svc_status {
 	ATH10K_HTC_CONN_SVC_STATUS_NO_MORE_EP   = 4
 };
 
+enum ath10k_htc_setup_complete_flags {
+	ATH10K_HTC_SETUP_COMPLETE_FLAGS_RX_BNDL_EN = 1
+};
+
 struct ath10k_ath10k_htc_msg_hdr {
 	__le16 message_id; /* @enum htc_message_id */
 } __packed;
@@ -174,8 +180,10 @@ struct ath10k_htc_msg {
 } __packed __aligned(4);
 
 enum ath10k_ath10k_htc_record_id {
-	ATH10K_HTC_RECORD_NULL    = 0,
-	ATH10K_HTC_RECORD_CREDITS = 1
+	ATH10K_HTC_RECORD_NULL             = 0,
+	ATH10K_HTC_RECORD_CREDITS          = 1,
+	ATH10K_HTC_RECORD_LOOKAHEAD        = 2,
+	ATH10K_HTC_RECORD_LOOKAHEAD_BUNDLE = 3,
 };
 
 struct ath10k_ath10k_htc_record_hdr {
@@ -192,10 +200,28 @@ struct ath10k_htc_credit_report {
 	u8 pad1;
 } __packed;
 
+struct ath10k_htc_lookahead_report {
+	u8 pre_valid;
+	u8 pad0;
+	u8 pad1;
+	u8 pad2;
+	u8 lookahead[4];
+	u8 post_valid;
+	u8 pad3;
+	u8 pad4;
+	u8 pad5;
+} __packed;
+
+struct ath10k_htc_lookahead_bundle {
+	u8 lookahead[4];
+} __packed;
+
 struct ath10k_htc_record {
 	struct ath10k_ath10k_htc_record_hdr hdr;
 	union {
 		struct ath10k_htc_credit_report credit_report[0];
+		struct ath10k_htc_lookahead_report lookahead_report[0];
+		struct ath10k_htc_lookahead_bundle lookahead_bundle[0];
 		u8 pauload[0];
 	};
 } __packed __aligned(4);
@@ -338,6 +364,7 @@ struct ath10k_htc {
 
 	int total_transmit_credits;
 	int target_credit_size;
+	u8 max_msgs_per_htc_bundle;
 };
 
 int ath10k_htc_init(struct ath10k *ar);
@@ -351,5 +378,13 @@ int ath10k_htc_send(struct ath10k_htc *htc, enum ath10k_htc_ep_id eid,
 struct sk_buff *ath10k_htc_alloc_skb(struct ath10k *ar, int size);
 void ath10k_htc_tx_completion_handler(struct ath10k *ar, struct sk_buff *skb);
 void ath10k_htc_rx_completion_handler(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_htc_notify_tx_completion(struct ath10k_htc_ep *ep,
+				     struct sk_buff *skb);
+int ath10k_htc_process_trailer(struct ath10k_htc *htc,
+			       u8 *buffer,
+			       int length,
+			       enum ath10k_htc_ep_id src_eid,
+			       void *next_lookaheads,
+			       int *next_lookaheads_len);
 
 #endif
diff --git a/drivers/net/wireless/ath/ath10k/htt_rx.c b/drivers/net/wireless/ath/ath10k/htt_rx.c
index 84b6067ff6e7..398dda978d6e 100644
--- a/drivers/net/wireless/ath/ath10k/htt_rx.c
+++ b/drivers/net/wireless/ath/ath10k/htt_rx.c
@@ -829,6 +829,19 @@ static void ath10k_htt_rx_h_signal(struct ath10k *ar,
 				   struct ieee80211_rx_status *status,
 				   struct htt_rx_desc *rxd)
 {
+	int i;
+
+	for (i = 0; i < IEEE80211_MAX_CHAINS ; i++) {
+		status->chains &= ~BIT(i);
+
+		if (rxd->ppdu_start.rssi_chains[i].pri20_mhz != 0x80) {
+			status->chain_signal[i] = ATH10K_DEFAULT_NOISE_FLOOR +
+				rxd->ppdu_start.rssi_chains[i].pri20_mhz;
+
+			status->chains |= BIT(i);
+		}
+	}
+
 	/* FIXME: Get real NF */
 	status->signal = ATH10K_DEFAULT_NOISE_FLOOR +
 			 rxd->ppdu_start.rssi_comb;
@@ -2229,9 +2242,15 @@ ath10k_update_per_peer_tx_stats(struct ath10k *ar,
 	txrate.mcs = ATH10K_HW_MCS_RATE(peer_stats->ratecode);
 	sgi = ATH10K_HW_GI(peer_stats->flags);
 
-	if (((txrate.flags == WMI_RATE_PREAMBLE_HT) ||
-	     (txrate.flags == WMI_RATE_PREAMBLE_VHT)) && txrate.mcs > 9) {
-		ath10k_warn(ar, "Invalid mcs %hhd peer stats", txrate.mcs);
+	if (txrate.flags == WMI_RATE_PREAMBLE_VHT && txrate.mcs > 9) {
+		ath10k_warn(ar, "Invalid VHT mcs %hhd peer stats",  txrate.mcs);
+		return;
+	}
+
+	if (txrate.flags == WMI_RATE_PREAMBLE_HT &&
+	    (txrate.mcs > 7 || txrate.nss < 1)) {
+		ath10k_warn(ar, "Invalid HT mcs %hhd nss %hhd peer stats",
+			    txrate.mcs, txrate.nss);
 		return;
 	}
 
@@ -2254,7 +2273,7 @@ ath10k_update_per_peer_tx_stats(struct ath10k *ar,
 		arsta->txrate.legacy = rate;
 	} else if (txrate.flags == WMI_RATE_PREAMBLE_HT) {
 		arsta->txrate.flags = RATE_INFO_FLAGS_MCS;
-		arsta->txrate.mcs = txrate.mcs;
+		arsta->txrate.mcs = txrate.mcs + 8 * (txrate.nss - 1);
 	} else {
 		arsta->txrate.flags = RATE_INFO_FLAGS_VHT_MCS;
 		arsta->txrate.mcs = txrate.mcs;
diff --git a/drivers/net/wireless/ath/ath10k/hw.c b/drivers/net/wireless/ath/ath10k/hw.c
index c866ab524571..afb0c01cbb55 100644
--- a/drivers/net/wireless/ath/ath10k/hw.c
+++ b/drivers/net/wireless/ath/ath10k/hw.c
@@ -15,6 +15,7 @@
  */
 
 #include <linux/types.h>
+#include <linux/bitops.h>
 #include "core.h"
 #include "hw.h"
 #include "hif.h"
@@ -191,6 +192,142 @@ const struct ath10k_hw_values qca4019_values = {
 	.ce_desc_meta_data_lsb          = 4,
 };
 
+static struct ath10k_hw_ce_regs_addr_map qcax_src_ring = {
+	.msb	= 0x00000010,
+	.lsb	= 0x00000010,
+	.mask	= GENMASK(16, 16),
+};
+
+static struct ath10k_hw_ce_regs_addr_map qcax_dst_ring = {
+	.msb	= 0x00000011,
+	.lsb	= 0x00000011,
+	.mask	= GENMASK(17, 17),
+};
+
+static struct ath10k_hw_ce_regs_addr_map qcax_dmax = {
+	.msb	= 0x0000000f,
+	.lsb	= 0x00000000,
+	.mask	= GENMASK(15, 0),
+};
+
+static struct ath10k_hw_ce_ctrl1 qcax_ctrl1 = {
+	.addr		= 0x00000010,
+	.hw_mask	= 0x0007ffff,
+	.sw_mask	= 0x0007ffff,
+	.hw_wr_mask	= 0x00000000,
+	.sw_wr_mask	= 0x0007ffff,
+	.reset_mask	= 0xffffffff,
+	.reset		= 0x00000080,
+	.src_ring	= &qcax_src_ring,
+	.dst_ring	= &qcax_dst_ring,
+	.dmax		= &qcax_dmax,
+};
+
+static struct ath10k_hw_ce_regs_addr_map qcax_cmd_halt_status = {
+	.msb	= 0x00000003,
+	.lsb	= 0x00000003,
+	.mask	= GENMASK(3, 3),
+};
+
+static struct ath10k_hw_ce_cmd_halt qcax_cmd_halt = {
+	.msb		= 0x00000000,
+	.mask		= GENMASK(0, 0),
+	.status_reset	= 0x00000000,
+	.status		= &qcax_cmd_halt_status,
+};
+
+static struct ath10k_hw_ce_regs_addr_map qcax_host_ie_cc = {
+	.msb	= 0x00000000,
+	.lsb	= 0x00000000,
+	.mask	= GENMASK(0, 0),
+};
+
+static struct ath10k_hw_ce_host_ie qcax_host_ie = {
+	.copy_complete_reset	= 0x00000000,
+	.copy_complete		= &qcax_host_ie_cc,
+};
+
+static struct ath10k_hw_ce_host_wm_regs qcax_wm_reg = {
+	.dstr_lmask	= 0x00000010,
+	.dstr_hmask	= 0x00000008,
+	.srcr_lmask	= 0x00000004,
+	.srcr_hmask	= 0x00000002,
+	.cc_mask	= 0x00000001,
+	.wm_mask	= 0x0000001E,
+	.addr		= 0x00000030,
+};
+
+static struct ath10k_hw_ce_misc_regs qcax_misc_reg = {
+	.axi_err	= 0x00000400,
+	.dstr_add_err	= 0x00000200,
+	.srcr_len_err	= 0x00000100,
+	.dstr_mlen_vio	= 0x00000080,
+	.dstr_overflow	= 0x00000040,
+	.srcr_overflow	= 0x00000020,
+	.err_mask	= 0x000007E0,
+	.addr		= 0x00000038,
+};
+
+static struct ath10k_hw_ce_regs_addr_map qcax_src_wm_low = {
+	.msb    = 0x0000001f,
+	.lsb	= 0x00000010,
+	.mask	= GENMASK(31, 16),
+};
+
+static struct ath10k_hw_ce_regs_addr_map qcax_src_wm_high = {
+	.msb	= 0x0000000f,
+	.lsb	= 0x00000000,
+	.mask	= GENMASK(15, 0),
+};
+
+static struct ath10k_hw_ce_dst_src_wm_regs qcax_wm_src_ring = {
+	.addr		= 0x0000004c,
+	.low_rst	= 0x00000000,
+	.high_rst	= 0x00000000,
+	.wm_low		= &qcax_src_wm_low,
+	.wm_high        = &qcax_src_wm_high,
+};
+
+static struct ath10k_hw_ce_regs_addr_map qcax_dst_wm_low = {
+	.lsb	= 0x00000010,
+	.mask	= GENMASK(31, 16),
+};
+
+static struct ath10k_hw_ce_regs_addr_map qcax_dst_wm_high = {
+	.msb	= 0x0000000f,
+	.lsb	= 0x00000000,
+	.mask	= GENMASK(15, 0),
+};
+
+static struct ath10k_hw_ce_dst_src_wm_regs qcax_wm_dst_ring = {
+	.addr		= 0x00000050,
+	.low_rst	= 0x00000000,
+	.high_rst	= 0x00000000,
+	.wm_low		= &qcax_dst_wm_low,
+	.wm_high	= &qcax_dst_wm_high,
+};
+
+struct ath10k_hw_ce_regs qcax_ce_regs = {
+	.sr_base_addr		= 0x00000000,
+	.sr_size_addr		= 0x00000004,
+	.dr_base_addr		= 0x00000008,
+	.dr_size_addr		= 0x0000000c,
+	.ce_cmd_addr		= 0x00000018,
+	.misc_ie_addr		= 0x00000034,
+	.sr_wr_index_addr	= 0x0000003c,
+	.dst_wr_index_addr	= 0x00000040,
+	.current_srri_addr	= 0x00000044,
+	.current_drri_addr	= 0x00000048,
+	.host_ie_addr		= 0x0000002c,
+	.ctrl1_regs		= &qcax_ctrl1,
+	.cmd_halt		= &qcax_cmd_halt,
+	.host_ie		= &qcax_host_ie,
+	.wm_regs		= &qcax_wm_reg,
+	.misc_regs		= &qcax_misc_reg,
+	.wm_srcr		= &qcax_wm_src_ring,
+	.wm_dstr                = &qcax_wm_dst_ring,
+};
+
 const struct ath10k_hw_clk_params qca6174_clk[ATH10K_HW_REFCLK_COUNT] = {
 	{
 		.refclk = 48000000,
diff --git a/drivers/net/wireless/ath/ath10k/hw.h b/drivers/net/wireless/ath/ath10k/hw.h
index 5b1e90bb2a4d..97dc1479f44e 100644
--- a/drivers/net/wireless/ath/ath10k/hw.h
+++ b/drivers/net/wireless/ath/ath10k/hw.h
@@ -268,6 +268,86 @@ extern const struct ath10k_hw_regs qca6174_regs;
 extern const struct ath10k_hw_regs qca99x0_regs;
 extern const struct ath10k_hw_regs qca4019_regs;
 
+struct ath10k_hw_ce_regs_addr_map {
+	u32 msb;
+	u32 lsb;
+	u32 mask;
+};
+
+struct ath10k_hw_ce_ctrl1 {
+	u32 addr;
+	u32 hw_mask;
+	u32 sw_mask;
+	u32 hw_wr_mask;
+	u32 sw_wr_mask;
+	u32 reset_mask;
+	u32 reset;
+	struct ath10k_hw_ce_regs_addr_map *src_ring;
+	struct ath10k_hw_ce_regs_addr_map *dst_ring;
+	struct ath10k_hw_ce_regs_addr_map *dmax; };
+
+struct ath10k_hw_ce_cmd_halt {
+	u32 status_reset;
+	u32 msb;
+	u32 mask;
+	struct ath10k_hw_ce_regs_addr_map *status; };
+
+struct ath10k_hw_ce_host_ie {
+	u32 copy_complete_reset;
+	struct ath10k_hw_ce_regs_addr_map *copy_complete; };
+
+struct ath10k_hw_ce_host_wm_regs {
+	u32 dstr_lmask;
+	u32 dstr_hmask;
+	u32 srcr_lmask;
+	u32 srcr_hmask;
+	u32 cc_mask;
+	u32 wm_mask;
+	u32 addr;
+};
+
+struct ath10k_hw_ce_misc_regs {
+	u32 axi_err;
+	u32 dstr_add_err;
+	u32 srcr_len_err;
+	u32 dstr_mlen_vio;
+	u32 dstr_overflow;
+	u32 srcr_overflow;
+	u32 err_mask;
+	u32 addr;
+};
+
+struct ath10k_hw_ce_dst_src_wm_regs {
+	u32 addr;
+	u32 low_rst;
+	u32 high_rst;
+	struct ath10k_hw_ce_regs_addr_map *wm_low;
+	struct ath10k_hw_ce_regs_addr_map *wm_high; };
+
+struct ath10k_hw_ce_regs {
+	u32 sr_base_addr;
+	u32 sr_size_addr;
+	u32 dr_base_addr;
+	u32 dr_size_addr;
+	u32 ce_cmd_addr;
+	u32 misc_ie_addr;
+	u32 sr_wr_index_addr;
+	u32 dst_wr_index_addr;
+	u32 current_srri_addr;
+	u32 current_drri_addr;
+	u32 ddr_addr_for_rri_low;
+	u32 ddr_addr_for_rri_high;
+	u32 ce_rri_low;
+	u32 ce_rri_high;
+	u32 host_ie_addr;
+	struct ath10k_hw_ce_host_wm_regs *wm_regs;
+	struct ath10k_hw_ce_misc_regs *misc_regs;
+	struct ath10k_hw_ce_ctrl1 *ctrl1_regs;
+	struct ath10k_hw_ce_cmd_halt *cmd_halt;
+	struct ath10k_hw_ce_host_ie *host_ie;
+	struct ath10k_hw_ce_dst_src_wm_regs *wm_srcr;
+	struct ath10k_hw_ce_dst_src_wm_regs *wm_dstr; };
+
 struct ath10k_hw_values {
 	u32 rtc_state_val_on;
 	u8 ce_count;
@@ -282,6 +362,7 @@ extern const struct ath10k_hw_values qca6174_values;
 extern const struct ath10k_hw_values qca99x0_values;
 extern const struct ath10k_hw_values qca9888_values;
 extern const struct ath10k_hw_values qca4019_values;
+extern struct ath10k_hw_ce_regs qcax_ce_regs;
 
 void ath10k_hw_fill_survey_time(struct ath10k *ar, struct survey_info *survey,
 				u32 cc, u32 rcc, u32 cc_prev, u32 rcc_prev);
@@ -454,6 +535,12 @@ struct ath10k_hw_params {
 
 	/* Number of bytes to be discarded for each FFT sample */
 	int spectral_bin_discard;
+
+	/* The board may have a restricted NSS for 160 or 80+80 vs what it
+	 * can do for 80Mhz.
+	 */
+	int vht160_mcs_rx_highest;
+	int vht160_mcs_tx_highest;
 };
 
 struct htt_rx_desc;
@@ -863,6 +950,59 @@ ath10k_rx_desc_get_l3_pad_bytes(struct ath10k_hw_params *hw,
 #define QCA9887_EEPROM_ADDR_LO_MASK		0x00ff0000
 #define QCA9887_EEPROM_ADDR_LO_LSB		16
 
+#define MBOX_RESET_CONTROL_ADDRESS		0x00000000
+#define MBOX_HOST_INT_STATUS_ADDRESS		0x00000800
+#define MBOX_HOST_INT_STATUS_ERROR_LSB		7
+#define MBOX_HOST_INT_STATUS_ERROR_MASK		0x00000080
+#define MBOX_HOST_INT_STATUS_CPU_LSB		6
+#define MBOX_HOST_INT_STATUS_CPU_MASK		0x00000040
+#define MBOX_HOST_INT_STATUS_COUNTER_LSB	4
+#define MBOX_HOST_INT_STATUS_COUNTER_MASK	0x00000010
+#define MBOX_CPU_INT_STATUS_ADDRESS		0x00000801
+#define MBOX_ERROR_INT_STATUS_ADDRESS		0x00000802
+#define MBOX_ERROR_INT_STATUS_WAKEUP_LSB	2
+#define MBOX_ERROR_INT_STATUS_WAKEUP_MASK	0x00000004
+#define MBOX_ERROR_INT_STATUS_RX_UNDERFLOW_LSB	1
+#define MBOX_ERROR_INT_STATUS_RX_UNDERFLOW_MASK	0x00000002
+#define MBOX_ERROR_INT_STATUS_TX_OVERFLOW_LSB	0
+#define MBOX_ERROR_INT_STATUS_TX_OVERFLOW_MASK	0x00000001
+#define MBOX_COUNTER_INT_STATUS_ADDRESS		0x00000803
+#define MBOX_COUNTER_INT_STATUS_COUNTER_LSB	0
+#define MBOX_COUNTER_INT_STATUS_COUNTER_MASK	0x000000ff
+#define MBOX_RX_LOOKAHEAD_VALID_ADDRESS		0x00000805
+#define MBOX_INT_STATUS_ENABLE_ADDRESS		0x00000828
+#define MBOX_INT_STATUS_ENABLE_ERROR_LSB	7
+#define MBOX_INT_STATUS_ENABLE_ERROR_MASK	0x00000080
+#define MBOX_INT_STATUS_ENABLE_CPU_LSB		6
+#define MBOX_INT_STATUS_ENABLE_CPU_MASK		0x00000040
+#define MBOX_INT_STATUS_ENABLE_INT_LSB		5
+#define MBOX_INT_STATUS_ENABLE_INT_MASK		0x00000020
+#define MBOX_INT_STATUS_ENABLE_COUNTER_LSB	4
+#define MBOX_INT_STATUS_ENABLE_COUNTER_MASK	0x00000010
+#define MBOX_INT_STATUS_ENABLE_MBOX_DATA_LSB	0
+#define MBOX_INT_STATUS_ENABLE_MBOX_DATA_MASK	0x0000000f
+#define MBOX_CPU_INT_STATUS_ENABLE_ADDRESS	0x00000819
+#define MBOX_CPU_INT_STATUS_ENABLE_BIT_LSB	0
+#define MBOX_CPU_INT_STATUS_ENABLE_BIT_MASK	0x000000ff
+#define MBOX_ERROR_STATUS_ENABLE_ADDRESS	0x0000081a
+#define MBOX_ERROR_STATUS_ENABLE_RX_UNDERFLOW_LSB  1
+#define MBOX_ERROR_STATUS_ENABLE_RX_UNDERFLOW_MASK 0x00000002
+#define MBOX_ERROR_STATUS_ENABLE_TX_OVERFLOW_LSB   0
+#define MBOX_ERROR_STATUS_ENABLE_TX_OVERFLOW_MASK  0x00000001
+#define MBOX_COUNTER_INT_STATUS_ENABLE_ADDRESS	0x0000081b
+#define MBOX_COUNTER_INT_STATUS_ENABLE_BIT_LSB	0
+#define MBOX_COUNTER_INT_STATUS_ENABLE_BIT_MASK	0x000000ff
+#define MBOX_COUNT_ADDRESS			0x00000820
+#define MBOX_COUNT_DEC_ADDRESS			0x00000840
+#define MBOX_WINDOW_DATA_ADDRESS		0x00000874
+#define MBOX_WINDOW_WRITE_ADDR_ADDRESS		0x00000878
+#define MBOX_WINDOW_READ_ADDR_ADDRESS		0x0000087c
+#define MBOX_CPU_DBG_SEL_ADDRESS		0x00000883
+#define MBOX_CPU_DBG_ADDRESS			0x00000884
+#define MBOX_RTC_BASE_ADDRESS			0x00000000
+#define MBOX_GPIO_BASE_ADDRESS			0x00005000
+#define MBOX_MBOX_BASE_ADDRESS			0x00008000
+
 #define RTC_STATE_V_GET(x) (((x) & RTC_STATE_V_MASK) >> RTC_STATE_V_LSB)
 
 /* Register definitions for first generation ath10k cards. These cards include
diff --git a/drivers/net/wireless/ath/ath10k/mac.c b/drivers/net/wireless/ath/ath10k/mac.c
index 4674ff33d320..55c808f03a84 100644
--- a/drivers/net/wireless/ath/ath10k/mac.c
+++ b/drivers/net/wireless/ath/ath10k/mac.c
@@ -1392,7 +1392,7 @@ static int ath10k_vdev_stop(struct ath10k_vif *arvif)
 
 	ret = ath10k_vdev_setup_sync(ar);
 	if (ret) {
-		ath10k_warn(ar, "failed to syncronise setup for vdev %i: %d\n",
+		ath10k_warn(ar, "failed to synchronize setup for vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 		return ret;
 	}
@@ -2519,6 +2519,20 @@ static void ath10k_peer_assoc_h_vht(struct ath10k *ar,
 
 	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vht peer %pM max_mpdu %d flags 0x%x\n",
 		   sta->addr, arg->peer_max_mpdu, arg->peer_flags);
+
+	if (arg->peer_vht_rates.rx_max_rate &&
+	    (sta->vht_cap.cap & IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_MASK)) {
+		switch (arg->peer_vht_rates.rx_max_rate) {
+		case 1560:
+			/* Must be 2x2 at 160Mhz is all it can do. */
+			arg->peer_bw_rxnss_override = 2;
+			break;
+		case 780:
+			/* Can only do 1x1 at 160Mhz (Long Guard Interval) */
+			arg->peer_bw_rxnss_override = 1;
+			break;
+		}
+	}
 }
 
 static void ath10k_peer_assoc_h_qos(struct ath10k *ar,
@@ -3475,9 +3489,8 @@ static void ath10k_tx_h_add_p2p_noa_ie(struct ath10k *ar,
 		if (arvif->u.ap.noa_data)
 			if (!pskb_expand_head(skb, 0, arvif->u.ap.noa_len,
 					      GFP_ATOMIC))
-				memcpy(skb_put(skb, arvif->u.ap.noa_len),
-				       arvif->u.ap.noa_data,
-				       arvif->u.ap.noa_len);
+				skb_put_data(skb, arvif->u.ap.noa_data,
+					     arvif->u.ap.noa_len);
 		spin_unlock_bh(&ar->data_lock);
 	}
 }
@@ -4362,6 +4375,7 @@ static int ath10k_mac_get_vht_cap_bf_sound_dim(struct ath10k *ar)
 static struct ieee80211_sta_vht_cap ath10k_create_vht_cap(struct ath10k *ar)
 {
 	struct ieee80211_sta_vht_cap vht_cap = {0};
+	struct ath10k_hw_params *hw = &ar->hw_params;
 	u16 mcs_map;
 	u32 val;
 	int i;
@@ -4391,7 +4405,7 @@ static struct ieee80211_sta_vht_cap ath10k_create_vht_cap(struct ath10k *ar)
 	 * mode until that's resolved.
 	 */
 	if ((ar->vht_cap_info & IEEE80211_VHT_CAP_SHORT_GI_160) &&
-	    !(ar->vht_cap_info & IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160_80PLUS80MHZ))
+	    (ar->vht_cap_info & IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_MASK) == 0)
 		vht_cap.cap |= IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160MHZ;
 
 	mcs_map = 0;
@@ -4408,6 +4422,17 @@ static struct ieee80211_sta_vht_cap ath10k_create_vht_cap(struct ath10k *ar)
 	vht_cap.vht_mcs.rx_mcs_map = cpu_to_le16(mcs_map);
 	vht_cap.vht_mcs.tx_mcs_map = cpu_to_le16(mcs_map);
 
+	/* If we are supporting 160Mhz or 80+80, then the NIC may be able to do
+	 * a restricted NSS for 160 or 80+80 vs what it can do for 80Mhz.  Give
+	 * user-space a clue if that is the case.
+	 */
+	if ((vht_cap.cap & IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_MASK) &&
+	    (hw->vht160_mcs_rx_highest != 0 ||
+	     hw->vht160_mcs_tx_highest != 0)) {
+		vht_cap.vht_mcs.rx_highest = cpu_to_le16(hw->vht160_mcs_rx_highest);
+		vht_cap.vht_mcs.tx_highest = cpu_to_le16(hw->vht160_mcs_tx_highest);
+	}
+
 	return vht_cap;
 }
 
@@ -6073,6 +6098,20 @@ static int ath10k_sta_state(struct ieee80211_hw *hw,
 			   ar->num_stations + 1, ar->max_num_stations,
 			   ar->num_peers + 1, ar->max_num_peers);
 
+		num_tdls_stations = ath10k_mac_tdls_vif_stations_count(hw, vif);
+		num_tdls_vifs = ath10k_mac_tdls_vifs_count(hw);
+
+		if (sta->tdls) {
+			if (num_tdls_stations >= ar->max_num_tdls_vdevs) {
+				ath10k_warn(ar, "vdev %i exceeded maximum number of tdls vdevs %i\n",
+					    arvif->vdev_id,
+					    ar->max_num_tdls_vdevs);
+				ret = -ELNRNG;
+				goto exit;
+			}
+			peer_type = WMI_PEER_TYPE_TDLS;
+		}
+
 		ret = ath10k_mac_inc_num_stations(arvif, sta);
 		if (ret) {
 			ath10k_warn(ar, "refusing to associate station: too many connected already (%d)\n",
@@ -6080,9 +6119,6 @@ static int ath10k_sta_state(struct ieee80211_hw *hw,
 			goto exit;
 		}
 
-		if (sta->tdls)
-			peer_type = WMI_PEER_TYPE_TDLS;
-
 		ret = ath10k_peer_create(ar, vif, sta, arvif->vdev_id,
 					 sta->addr, peer_type);
 		if (ret) {
@@ -6113,35 +6149,17 @@ static int ath10k_sta_state(struct ieee80211_hw *hw,
 		if (!sta->tdls)
 			goto exit;
 
-		num_tdls_stations = ath10k_mac_tdls_vif_stations_count(hw, vif);
-		num_tdls_vifs = ath10k_mac_tdls_vifs_count(hw);
-
-		if (num_tdls_vifs >= ar->max_num_tdls_vdevs &&
-		    num_tdls_stations == 0) {
-			ath10k_warn(ar, "vdev %i exceeded maximum number of tdls vdevs %i\n",
-				    arvif->vdev_id, ar->max_num_tdls_vdevs);
-			ath10k_peer_delete(ar, arvif->vdev_id, sta->addr);
+		ret = ath10k_wmi_update_fw_tdls_state(ar, arvif->vdev_id,
+						      WMI_TDLS_ENABLE_ACTIVE);
+		if (ret) {
+			ath10k_warn(ar, "failed to update fw tdls state on vdev %i: %i\n",
+				    arvif->vdev_id, ret);
+			ath10k_peer_delete(ar, arvif->vdev_id,
+					   sta->addr);
 			ath10k_mac_dec_num_stations(arvif, sta);
-			ret = -ENOBUFS;
 			goto exit;
 		}
 
-		if (num_tdls_stations == 0) {
-			/* This is the first tdls peer in current vif */
-			enum wmi_tdls_state state = WMI_TDLS_ENABLE_ACTIVE;
-
-			ret = ath10k_wmi_update_fw_tdls_state(ar, arvif->vdev_id,
-							      state);
-			if (ret) {
-				ath10k_warn(ar, "failed to update fw tdls state on vdev %i: %i\n",
-					    arvif->vdev_id, ret);
-				ath10k_peer_delete(ar, arvif->vdev_id,
-						   sta->addr);
-				ath10k_mac_dec_num_stations(arvif, sta);
-				goto exit;
-			}
-		}
-
 		ret = ath10k_mac_tdls_peer_update(ar, arvif->vdev_id, sta,
 						  WMI_TDLS_PEER_STATE_PEERING);
 		if (ret) {
diff --git a/drivers/net/wireless/ath/ath10k/pci.c b/drivers/net/wireless/ath/ath10k/pci.c
index 1e9806f57ee4..7ebfc409018d 100644
--- a/drivers/net/wireless/ath/ath10k/pci.c
+++ b/drivers/net/wireless/ath/ath10k/pci.c
@@ -101,7 +101,8 @@ static int ath10k_pci_init_irq(struct ath10k *ar);
 static int ath10k_pci_deinit_irq(struct ath10k *ar);
 static int ath10k_pci_request_irq(struct ath10k *ar);
 static void ath10k_pci_free_irq(struct ath10k *ar);
-static int ath10k_pci_bmi_wait(struct ath10k_ce_pipe *tx_pipe,
+static int ath10k_pci_bmi_wait(struct ath10k *ar,
+			       struct ath10k_ce_pipe *tx_pipe,
 			       struct ath10k_ce_pipe *rx_pipe,
 			       struct bmi_xfer *xfer);
 static int ath10k_pci_qca99x0_chip_reset(struct ath10k *ar);
@@ -468,7 +469,7 @@ static int ath10k_pci_wake_wait(struct ath10k *ar)
 	while (tot_delay < PCIE_WAKE_TIMEOUT) {
 		if (ath10k_pci_is_awake(ar)) {
 			if (tot_delay > PCIE_WAKE_LATE_US)
-				ath10k_warn(ar, "device wakeup took %d ms which is unusally long, otherwise it works normally.\n",
+				ath10k_warn(ar, "device wakeup took %d ms which is unusually long, otherwise it works normally.\n",
 					    tot_delay / 1000);
 			return 0;
 		}
@@ -1846,7 +1847,7 @@ int ath10k_pci_hif_exchange_bmi_msg(struct ath10k *ar,
 	if (ret)
 		goto err_resp;
 
-	ret = ath10k_pci_bmi_wait(ce_tx, ce_rx, &xfer);
+	ret = ath10k_pci_bmi_wait(ar, ce_tx, ce_rx, &xfer);
 	if (ret) {
 		u32 unused_buffer;
 		unsigned int unused_nbytes;
@@ -1913,23 +1914,37 @@ static void ath10k_pci_bmi_recv_data(struct ath10k_ce_pipe *ce_state)
 	xfer->rx_done = true;
 }
 
-static int ath10k_pci_bmi_wait(struct ath10k_ce_pipe *tx_pipe,
+static int ath10k_pci_bmi_wait(struct ath10k *ar,
+			       struct ath10k_ce_pipe *tx_pipe,
 			       struct ath10k_ce_pipe *rx_pipe,
 			       struct bmi_xfer *xfer)
 {
 	unsigned long timeout = jiffies + BMI_COMMUNICATION_TIMEOUT_HZ;
+	unsigned long started = jiffies;
+	unsigned long dur;
+	int ret;
 
 	while (time_before_eq(jiffies, timeout)) {
 		ath10k_pci_bmi_send_done(tx_pipe);
 		ath10k_pci_bmi_recv_data(rx_pipe);
 
-		if (xfer->tx_done && (xfer->rx_done == xfer->wait_for_resp))
-			return 0;
+		if (xfer->tx_done && (xfer->rx_done == xfer->wait_for_resp)) {
+			ret = 0;
+			goto out;
+		}
 
 		schedule();
 	}
 
-	return -ETIMEDOUT;
+	ret = -ETIMEDOUT;
+
+out:
+	dur = jiffies - started;
+	if (dur > HZ)
+		ath10k_dbg(ar, ATH10K_DBG_BMI,
+			   "bmi cmd took %lu jiffies hz %d ret %d\n",
+			   dur, HZ, ret);
+	return ret;
 }
 
 /*
diff --git a/drivers/net/wireless/ath/ath10k/sdio.c b/drivers/net/wireless/ath/ath10k/sdio.c
new file mode 100644
index 000000000000..859ed870bd97
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/sdio.c
@@ -0,0 +1,2113 @@
+/*
+ * Copyright (c) 2004-2011 Atheros Communications Inc.
+ * Copyright (c) 2011-2012,2017 Qualcomm Atheros, Inc.
+ * Copyright (c) 2016-2017 Erik Stromdahl <erik.stromdahl@gmail.com>
+ *
+ * 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/module.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/mmc.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/mmc/sd.h>
+#include <linux/bitfield.h>
+#include "core.h"
+#include "bmi.h"
+#include "debug.h"
+#include "hif.h"
+#include "htc.h"
+#include "targaddrs.h"
+#include "trace.h"
+#include "sdio.h"
+
+/* inlined helper functions */
+
+static inline int ath10k_sdio_calc_txrx_padded_len(struct ath10k_sdio *ar_sdio,
+						   size_t len)
+{
+	return __ALIGN_MASK((len), ar_sdio->mbox_info.block_mask);
+}
+
+static inline enum ath10k_htc_ep_id pipe_id_to_eid(u8 pipe_id)
+{
+	return (enum ath10k_htc_ep_id)pipe_id;
+}
+
+static inline void ath10k_sdio_mbox_free_rx_pkt(struct ath10k_sdio_rx_data *pkt)
+{
+	dev_kfree_skb(pkt->skb);
+	pkt->skb = NULL;
+	pkt->alloc_len = 0;
+	pkt->act_len = 0;
+	pkt->trailer_only = false;
+}
+
+static inline int ath10k_sdio_mbox_alloc_rx_pkt(struct ath10k_sdio_rx_data *pkt,
+						size_t act_len, size_t full_len,
+						bool part_of_bundle,
+						bool last_in_bundle)
+{
+	pkt->skb = dev_alloc_skb(full_len);
+	if (!pkt->skb)
+		return -ENOMEM;
+
+	pkt->act_len = act_len;
+	pkt->alloc_len = full_len;
+	pkt->part_of_bundle = part_of_bundle;
+	pkt->last_in_bundle = last_in_bundle;
+	pkt->trailer_only = false;
+
+	return 0;
+}
+
+static inline bool is_trailer_only_msg(struct ath10k_sdio_rx_data *pkt)
+{
+	bool trailer_only = false;
+	struct ath10k_htc_hdr *htc_hdr =
+		(struct ath10k_htc_hdr *)pkt->skb->data;
+	u16 len = __le16_to_cpu(htc_hdr->len);
+
+	if (len == htc_hdr->trailer_len)
+		trailer_only = true;
+
+	return trailer_only;
+}
+
+/* sdio/mmc functions */
+
+static inline void ath10k_sdio_set_cmd52_arg(u32 *arg, u8 write, u8 raw,
+					     unsigned int address,
+					     unsigned char val)
+{
+	*arg = FIELD_PREP(BIT(31), write) |
+	       FIELD_PREP(BIT(27), raw) |
+	       FIELD_PREP(BIT(26), 1) |
+	       FIELD_PREP(GENMASK(25, 9), address) |
+	       FIELD_PREP(BIT(8), 1) |
+	       FIELD_PREP(GENMASK(7, 0), val);
+}
+
+static int ath10k_sdio_func0_cmd52_wr_byte(struct mmc_card *card,
+					   unsigned int address,
+					   unsigned char byte)
+{
+	struct mmc_command io_cmd;
+
+	memset(&io_cmd, 0, sizeof(io_cmd));
+	ath10k_sdio_set_cmd52_arg(&io_cmd.arg, 1, 0, address, byte);
+	io_cmd.opcode = SD_IO_RW_DIRECT;
+	io_cmd.flags = MMC_RSP_R5 | MMC_CMD_AC;
+
+	return mmc_wait_for_cmd(card->host, &io_cmd, 0);
+}
+
+static int ath10k_sdio_func0_cmd52_rd_byte(struct mmc_card *card,
+					   unsigned int address,
+					   unsigned char *byte)
+{
+	struct mmc_command io_cmd;
+	int ret;
+
+	memset(&io_cmd, 0, sizeof(io_cmd));
+	ath10k_sdio_set_cmd52_arg(&io_cmd.arg, 0, 0, address, 0);
+	io_cmd.opcode = SD_IO_RW_DIRECT;
+	io_cmd.flags = MMC_RSP_R5 | MMC_CMD_AC;
+
+	ret = mmc_wait_for_cmd(card->host, &io_cmd, 0);
+	if (!ret)
+		*byte = io_cmd.resp[0];
+
+	return ret;
+}
+
+static int ath10k_sdio_config(struct ath10k *ar)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct sdio_func *func = ar_sdio->func;
+	unsigned char byte, asyncintdelay = 2;
+	int ret;
+
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "sdio configuration\n");
+
+	sdio_claim_host(func);
+
+	byte = 0;
+	ret = ath10k_sdio_func0_cmd52_rd_byte(func->card,
+					      SDIO_CCCR_DRIVE_STRENGTH,
+					      &byte);
+
+	byte &= ~ATH10K_SDIO_DRIVE_DTSX_MASK;
+	byte |= FIELD_PREP(ATH10K_SDIO_DRIVE_DTSX_MASK,
+			   ATH10K_SDIO_DRIVE_DTSX_TYPE_D);
+
+	ret = ath10k_sdio_func0_cmd52_wr_byte(func->card,
+					      SDIO_CCCR_DRIVE_STRENGTH,
+					      byte);
+
+	byte = 0;
+	ret = ath10k_sdio_func0_cmd52_rd_byte(
+		func->card,
+		CCCR_SDIO_DRIVER_STRENGTH_ENABLE_ADDR,
+		&byte);
+
+	byte |= (CCCR_SDIO_DRIVER_STRENGTH_ENABLE_A |
+		 CCCR_SDIO_DRIVER_STRENGTH_ENABLE_C |
+		 CCCR_SDIO_DRIVER_STRENGTH_ENABLE_D);
+
+	ret = ath10k_sdio_func0_cmd52_wr_byte(func->card,
+					      CCCR_SDIO_DRIVER_STRENGTH_ENABLE_ADDR,
+					      byte);
+	if (ret) {
+		ath10k_warn(ar, "failed to enable driver strength: %d\n", ret);
+		goto out;
+	}
+
+	byte = 0;
+	ret = ath10k_sdio_func0_cmd52_rd_byte(func->card,
+					      CCCR_SDIO_IRQ_MODE_REG_SDIO3,
+					      &byte);
+
+	byte |= SDIO_IRQ_MODE_ASYNC_4BIT_IRQ_SDIO3;
+
+	ret = ath10k_sdio_func0_cmd52_wr_byte(func->card,
+					      CCCR_SDIO_IRQ_MODE_REG_SDIO3,
+					      byte);
+	if (ret) {
+		ath10k_warn(ar, "failed to enable 4-bit async irq mode: %d\n",
+			    ret);
+		goto out;
+	}
+
+	byte = 0;
+	ret = ath10k_sdio_func0_cmd52_rd_byte(func->card,
+					      CCCR_SDIO_ASYNC_INT_DELAY_ADDRESS,
+					      &byte);
+
+	byte &= ~CCCR_SDIO_ASYNC_INT_DELAY_MASK;
+	byte |= FIELD_PREP(CCCR_SDIO_ASYNC_INT_DELAY_MASK, asyncintdelay);
+
+	ret = ath10k_sdio_func0_cmd52_wr_byte(func->card,
+					      CCCR_SDIO_ASYNC_INT_DELAY_ADDRESS,
+					      byte);
+
+	/* give us some time to enable, in ms */
+	func->enable_timeout = 100;
+
+	ret = sdio_set_block_size(func, ar_sdio->mbox_info.block_size);
+	if (ret) {
+		ath10k_warn(ar, "failed to set sdio block size to %d: %d\n",
+			    ar_sdio->mbox_info.block_size, ret);
+		goto out;
+	}
+
+out:
+	sdio_release_host(func);
+	return ret;
+}
+
+static int ath10k_sdio_write32(struct ath10k *ar, u32 addr, u32 val)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct sdio_func *func = ar_sdio->func;
+	int ret;
+
+	sdio_claim_host(func);
+
+	sdio_writel(func, val, addr, &ret);
+	if (ret) {
+		ath10k_warn(ar, "failed to write 0x%x to address 0x%x: %d\n",
+			    val, addr, ret);
+		goto out;
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_SDIO, "sdio write32 addr 0x%x val 0x%x\n",
+		   addr, val);
+
+out:
+	sdio_release_host(func);
+
+	return ret;
+}
+
+static int ath10k_sdio_writesb32(struct ath10k *ar, u32 addr, u32 val)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct sdio_func *func = ar_sdio->func;
+	__le32 *buf;
+	int ret;
+
+	buf = kzalloc(sizeof(*buf), GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	*buf = cpu_to_le32(val);
+
+	sdio_claim_host(func);
+
+	ret = sdio_writesb(func, addr, buf, sizeof(*buf));
+	if (ret) {
+		ath10k_warn(ar, "failed to write value 0x%x to fixed sb address 0x%x: %d\n",
+			    val, addr, ret);
+		goto out;
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_SDIO, "sdio writesb32 addr 0x%x val 0x%x\n",
+		   addr, val);
+
+out:
+	sdio_release_host(func);
+
+	kfree(buf);
+
+	return ret;
+}
+
+static int ath10k_sdio_read32(struct ath10k *ar, u32 addr, u32 *val)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct sdio_func *func = ar_sdio->func;
+	int ret;
+
+	sdio_claim_host(func);
+	*val = sdio_readl(func, addr, &ret);
+	if (ret) {
+		ath10k_warn(ar, "failed to read from address 0x%x: %d\n",
+			    addr, ret);
+		goto out;
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_SDIO, "sdio read32 addr 0x%x val 0x%x\n",
+		   addr, *val);
+
+out:
+	sdio_release_host(func);
+
+	return ret;
+}
+
+static int ath10k_sdio_read(struct ath10k *ar, u32 addr, void *buf, size_t len)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct sdio_func *func = ar_sdio->func;
+	int ret;
+
+	sdio_claim_host(func);
+
+	ret = sdio_memcpy_fromio(func, buf, addr, len);
+	if (ret) {
+		ath10k_warn(ar, "failed to read from address 0x%x: %d\n",
+			    addr, ret);
+		goto out;
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_SDIO, "sdio read addr 0x%x buf 0x%p len %zu\n",
+		   addr, buf, len);
+	ath10k_dbg_dump(ar, ATH10K_DBG_SDIO_DUMP, NULL, "sdio read ", buf, len);
+
+out:
+	sdio_release_host(func);
+
+	return ret;
+}
+
+static int ath10k_sdio_write(struct ath10k *ar, u32 addr, const void *buf, size_t len)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct sdio_func *func = ar_sdio->func;
+	int ret;
+
+	sdio_claim_host(func);
+
+	/* For some reason toio() doesn't have const for the buffer, need
+	 * an ugly hack to workaround that.
+	 */
+	ret = sdio_memcpy_toio(func, addr, (void *)buf, len);
+	if (ret) {
+		ath10k_warn(ar, "failed to write to address 0x%x: %d\n",
+			    addr, ret);
+		goto out;
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_SDIO, "sdio write addr 0x%x buf 0x%p len %zu\n",
+		   addr, buf, len);
+	ath10k_dbg_dump(ar, ATH10K_DBG_SDIO_DUMP, NULL, "sdio write ", buf, len);
+
+out:
+	sdio_release_host(func);
+
+	return ret;
+}
+
+static int ath10k_sdio_readsb(struct ath10k *ar, u32 addr, void *buf, size_t len)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct sdio_func *func = ar_sdio->func;
+	int ret;
+
+	sdio_claim_host(func);
+
+	len = round_down(len, ar_sdio->mbox_info.block_size);
+
+	ret = sdio_readsb(func, buf, addr, len);
+	if (ret) {
+		ath10k_warn(ar, "failed to read from fixed (sb) address 0x%x: %d\n",
+			    addr, ret);
+		goto out;
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_SDIO, "sdio readsb addr 0x%x buf 0x%p len %zu\n",
+		   addr, buf, len);
+	ath10k_dbg_dump(ar, ATH10K_DBG_SDIO_DUMP, NULL, "sdio readsb ", buf, len);
+
+out:
+	sdio_release_host(func);
+
+	return ret;
+}
+
+/* HIF mbox functions */
+
+static int ath10k_sdio_mbox_rx_process_packet(struct ath10k *ar,
+					      struct ath10k_sdio_rx_data *pkt,
+					      u32 *lookaheads,
+					      int *n_lookaheads)
+{
+	struct ath10k_htc *htc = &ar->htc;
+	struct sk_buff *skb = pkt->skb;
+	struct ath10k_htc_hdr *htc_hdr = (struct ath10k_htc_hdr *)skb->data;
+	bool trailer_present = htc_hdr->flags & ATH10K_HTC_FLAG_TRAILER_PRESENT;
+	enum ath10k_htc_ep_id eid;
+	u16 payload_len;
+	u8 *trailer;
+	int ret;
+
+	payload_len = le16_to_cpu(htc_hdr->len);
+
+	if (trailer_present) {
+		trailer = skb->data + sizeof(*htc_hdr) +
+			  payload_len - htc_hdr->trailer_len;
+
+		eid = pipe_id_to_eid(htc_hdr->eid);
+
+		ret = ath10k_htc_process_trailer(htc,
+						 trailer,
+						 htc_hdr->trailer_len,
+						 eid,
+						 lookaheads,
+						 n_lookaheads);
+		if (ret)
+			return ret;
+
+		if (is_trailer_only_msg(pkt))
+			pkt->trailer_only = true;
+
+		skb_trim(skb, skb->len - htc_hdr->trailer_len);
+	}
+
+	skb_pull(skb, sizeof(*htc_hdr));
+
+	return 0;
+}
+
+static int ath10k_sdio_mbox_rx_process_packets(struct ath10k *ar,
+					       u32 lookaheads[],
+					       int *n_lookahead)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct ath10k_htc *htc = &ar->htc;
+	struct ath10k_sdio_rx_data *pkt;
+	struct ath10k_htc_ep *ep;
+	enum ath10k_htc_ep_id id;
+	int ret, i, *n_lookahead_local;
+	u32 *lookaheads_local;
+
+	for (i = 0; i < ar_sdio->n_rx_pkts; i++) {
+		lookaheads_local = lookaheads;
+		n_lookahead_local = n_lookahead;
+
+		id = ((struct ath10k_htc_hdr *)&lookaheads[i])->eid;
+
+		if (id >= ATH10K_HTC_EP_COUNT) {
+			ath10k_warn(ar, "invalid endpoint in look-ahead: %d\n",
+				    id);
+			ret = -ENOMEM;
+			goto out;
+		}
+
+		ep = &htc->endpoint[id];
+
+		if (ep->service_id == 0) {
+			ath10k_warn(ar, "ep %d is not connected\n", id);
+			ret = -ENOMEM;
+			goto out;
+		}
+
+		pkt = &ar_sdio->rx_pkts[i];
+
+		if (pkt->part_of_bundle && !pkt->last_in_bundle) {
+			/* Only read lookahead's from RX trailers
+			 * for the last packet in a bundle.
+			 */
+			lookaheads_local = NULL;
+			n_lookahead_local = NULL;
+		}
+
+		ret = ath10k_sdio_mbox_rx_process_packet(ar,
+							 pkt,
+							 lookaheads_local,
+							 n_lookahead_local);
+		if (ret)
+			goto out;
+
+		if (!pkt->trailer_only)
+			ep->ep_ops.ep_rx_complete(ar_sdio->ar, pkt->skb);
+		else
+			kfree_skb(pkt->skb);
+
+		/* The RX complete handler now owns the skb...*/
+		pkt->skb = NULL;
+		pkt->alloc_len = 0;
+	}
+
+	ret = 0;
+
+out:
+	/* Free all packets that was not passed on to the RX completion
+	 * handler...
+	 */
+	for (; i < ar_sdio->n_rx_pkts; i++)
+		ath10k_sdio_mbox_free_rx_pkt(&ar_sdio->rx_pkts[i]);
+
+	return ret;
+}
+
+static int ath10k_sdio_mbox_alloc_pkt_bundle(struct ath10k *ar,
+					     struct ath10k_sdio_rx_data *rx_pkts,
+					     struct ath10k_htc_hdr *htc_hdr,
+					     size_t full_len, size_t act_len,
+					     size_t *bndl_cnt)
+{
+	int ret, i;
+
+	*bndl_cnt = FIELD_GET(ATH10K_HTC_FLAG_BUNDLE_MASK, htc_hdr->flags);
+
+	if (*bndl_cnt > HTC_HOST_MAX_MSG_PER_BUNDLE) {
+		ath10k_warn(ar,
+			    "HTC bundle length %u exceeds maximum %u\n",
+			    le16_to_cpu(htc_hdr->len),
+			    HTC_HOST_MAX_MSG_PER_BUNDLE);
+		return -ENOMEM;
+	}
+
+	/* Allocate bndl_cnt extra skb's for the bundle.
+	 * The package containing the
+	 * ATH10K_HTC_FLAG_BUNDLE_MASK flag is not included
+	 * in bndl_cnt. The skb for that packet will be
+	 * allocated separately.
+	 */
+	for (i = 0; i < *bndl_cnt; i++) {
+		ret = ath10k_sdio_mbox_alloc_rx_pkt(&rx_pkts[i],
+						    act_len,
+						    full_len,
+						    true,
+						    false);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int ath10k_sdio_mbox_rx_alloc(struct ath10k *ar,
+				     u32 lookaheads[], int n_lookaheads)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct ath10k_htc_hdr *htc_hdr;
+	size_t full_len, act_len;
+	bool last_in_bundle;
+	int ret, i;
+
+	if (n_lookaheads > ATH10K_SDIO_MAX_RX_MSGS) {
+		ath10k_warn(ar,
+			    "the total number of pkgs to be fetched (%u) exceeds maximum %u\n",
+			    n_lookaheads,
+			    ATH10K_SDIO_MAX_RX_MSGS);
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	for (i = 0; i < n_lookaheads; i++) {
+		htc_hdr = (struct ath10k_htc_hdr *)&lookaheads[i];
+		last_in_bundle = false;
+
+		if (le16_to_cpu(htc_hdr->len) >
+		    ATH10K_HTC_MBOX_MAX_PAYLOAD_LENGTH) {
+			ath10k_warn(ar,
+				    "payload length %d exceeds max htc length: %zu\n",
+				    le16_to_cpu(htc_hdr->len),
+				    ATH10K_HTC_MBOX_MAX_PAYLOAD_LENGTH);
+			ret = -ENOMEM;
+			goto err;
+		}
+
+		act_len = le16_to_cpu(htc_hdr->len) + sizeof(*htc_hdr);
+		full_len = ath10k_sdio_calc_txrx_padded_len(ar_sdio, act_len);
+
+		if (full_len > ATH10K_SDIO_MAX_BUFFER_SIZE) {
+			ath10k_warn(ar,
+				    "rx buffer requested with invalid htc_hdr length (%d, 0x%x): %d\n",
+				    htc_hdr->eid, htc_hdr->flags,
+				    le16_to_cpu(htc_hdr->len));
+			ret = -EINVAL;
+			goto err;
+		}
+
+		if (htc_hdr->flags & ATH10K_HTC_FLAG_BUNDLE_MASK) {
+			/* HTC header indicates that every packet to follow
+			 * has the same padded length so that it can be
+			 * optimally fetched as a full bundle.
+			 */
+			size_t bndl_cnt;
+
+			ret = ath10k_sdio_mbox_alloc_pkt_bundle(ar,
+								&ar_sdio->rx_pkts[i],
+								htc_hdr,
+								full_len,
+								act_len,
+								&bndl_cnt);
+
+			n_lookaheads += bndl_cnt;
+			i += bndl_cnt;
+			/*Next buffer will be the last in the bundle */
+			last_in_bundle = true;
+		}
+
+		/* Allocate skb for packet. If the packet had the
+		 * ATH10K_HTC_FLAG_BUNDLE_MASK flag set, all bundled
+		 * packet skb's have been allocated in the previous step.
+		 */
+		ret = ath10k_sdio_mbox_alloc_rx_pkt(&ar_sdio->rx_pkts[i],
+						    act_len,
+						    full_len,
+						    last_in_bundle,
+						    last_in_bundle);
+	}
+
+	ar_sdio->n_rx_pkts = i;
+
+	return 0;
+
+err:
+	for (i = 0; i < ATH10K_SDIO_MAX_RX_MSGS; i++) {
+		if (!ar_sdio->rx_pkts[i].alloc_len)
+			break;
+		ath10k_sdio_mbox_free_rx_pkt(&ar_sdio->rx_pkts[i]);
+	}
+
+	return ret;
+}
+
+static int ath10k_sdio_mbox_rx_packet(struct ath10k *ar,
+				      struct ath10k_sdio_rx_data *pkt)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct sk_buff *skb = pkt->skb;
+	int ret;
+
+	ret = ath10k_sdio_readsb(ar, ar_sdio->mbox_info.htc_addr,
+				 skb->data, pkt->alloc_len);
+	pkt->status = ret;
+	if (!ret)
+		skb_put(skb, pkt->act_len);
+
+	return ret;
+}
+
+static int ath10k_sdio_mbox_rx_fetch(struct ath10k *ar)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	int ret, i;
+
+	for (i = 0; i < ar_sdio->n_rx_pkts; i++) {
+		ret = ath10k_sdio_mbox_rx_packet(ar,
+						 &ar_sdio->rx_pkts[i]);
+		if (ret)
+			goto err;
+	}
+
+	return 0;
+
+err:
+	/* Free all packets that was not successfully fetched. */
+	for (; i < ar_sdio->n_rx_pkts; i++)
+		ath10k_sdio_mbox_free_rx_pkt(&ar_sdio->rx_pkts[i]);
+
+	return ret;
+}
+
+/* This is the timeout for mailbox processing done in the sdio irq
+ * handler. The timeout is deliberately set quite high since SDIO dump logs
+ * over serial port can/will add a substantial overhead to the processing
+ * (if enabled).
+ */
+#define SDIO_MBOX_PROCESSING_TIMEOUT_HZ (20 * HZ)
+
+static int ath10k_sdio_mbox_rxmsg_pending_handler(struct ath10k *ar,
+						  u32 msg_lookahead, bool *done)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	u32 lookaheads[ATH10K_SDIO_MAX_RX_MSGS];
+	int n_lookaheads = 1;
+	unsigned long timeout;
+	int ret;
+
+	*done = true;
+
+	/* Copy the lookahead obtained from the HTC register table into our
+	 * temp array as a start value.
+	 */
+	lookaheads[0] = msg_lookahead;
+
+	timeout = jiffies + SDIO_MBOX_PROCESSING_TIMEOUT_HZ;
+	while (time_before(jiffies, timeout)) {
+		/* Try to allocate as many HTC RX packets indicated by
+		 * n_lookaheads.
+		 */
+		ret = ath10k_sdio_mbox_rx_alloc(ar, lookaheads,
+						n_lookaheads);
+		if (ret)
+			break;
+
+		if (ar_sdio->n_rx_pkts >= 2)
+			/* A recv bundle was detected, force IRQ status
+			 * re-check again.
+			 */
+			*done = false;
+
+		ret = ath10k_sdio_mbox_rx_fetch(ar);
+
+		/* Process fetched packets. This will potentially update
+		 * n_lookaheads depending on if the packets contain lookahead
+		 * reports.
+		 */
+		n_lookaheads = 0;
+		ret = ath10k_sdio_mbox_rx_process_packets(ar,
+							  lookaheads,
+							  &n_lookaheads);
+
+		if (!n_lookaheads || ret)
+			break;
+
+		/* For SYNCH processing, if we get here, we are running
+		 * through the loop again due to updated lookaheads. Set
+		 * flag that we should re-check IRQ status registers again
+		 * before leaving IRQ processing, this can net better
+		 * performance in high throughput situations.
+		 */
+		*done = false;
+	}
+
+	if (ret && (ret != -ECANCELED))
+		ath10k_warn(ar, "failed to get pending recv messages: %d\n",
+			    ret);
+
+	return ret;
+}
+
+static int ath10k_sdio_mbox_proc_dbg_intr(struct ath10k *ar)
+{
+	u32 val;
+	int ret;
+
+	/* TODO: Add firmware crash handling */
+	ath10k_warn(ar, "firmware crashed\n");
+
+	/* read counter to clear the interrupt, the debug error interrupt is
+	 * counter 0.
+	 */
+	ret = ath10k_sdio_read32(ar, MBOX_COUNT_DEC_ADDRESS, &val);
+	if (ret)
+		ath10k_warn(ar, "failed to clear debug interrupt: %d\n", ret);
+
+	return ret;
+}
+
+static int ath10k_sdio_mbox_proc_counter_intr(struct ath10k *ar)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct ath10k_sdio_irq_data *irq_data = &ar_sdio->irq_data;
+	u8 counter_int_status;
+	int ret;
+
+	mutex_lock(&irq_data->mtx);
+	counter_int_status = irq_data->irq_proc_reg->counter_int_status &
+			     irq_data->irq_en_reg->cntr_int_status_en;
+
+	/* NOTE: other modules like GMBOX may use the counter interrupt for
+	 * credit flow control on other counters, we only need to check for
+	 * the debug assertion counter interrupt.
+	 */
+	if (counter_int_status & ATH10K_SDIO_TARGET_DEBUG_INTR_MASK)
+		ret = ath10k_sdio_mbox_proc_dbg_intr(ar);
+	else
+		ret = 0;
+
+	mutex_unlock(&irq_data->mtx);
+
+	return ret;
+}
+
+static int ath10k_sdio_mbox_proc_err_intr(struct ath10k *ar)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct ath10k_sdio_irq_data *irq_data = &ar_sdio->irq_data;
+	u8 error_int_status;
+	int ret;
+
+	ath10k_dbg(ar, ATH10K_DBG_SDIO, "sdio error interrupt\n");
+
+	error_int_status = irq_data->irq_proc_reg->error_int_status & 0x0F;
+	if (!error_int_status) {
+		ath10k_warn(ar, "invalid error interrupt status: 0x%x\n",
+			    error_int_status);
+		return -EIO;
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_SDIO,
+		   "sdio error_int_status 0x%x\n", error_int_status);
+
+	if (FIELD_GET(MBOX_ERROR_INT_STATUS_WAKEUP_MASK,
+		      error_int_status))
+		ath10k_dbg(ar, ATH10K_DBG_SDIO, "sdio interrupt error wakeup\n");
+
+	if (FIELD_GET(MBOX_ERROR_INT_STATUS_RX_UNDERFLOW_MASK,
+		      error_int_status))
+		ath10k_warn(ar, "rx underflow interrupt error\n");
+
+	if (FIELD_GET(MBOX_ERROR_INT_STATUS_TX_OVERFLOW_MASK,
+		      error_int_status))
+		ath10k_warn(ar, "tx overflow interrupt error\n");
+
+	/* Clear the interrupt */
+	irq_data->irq_proc_reg->error_int_status &= ~error_int_status;
+
+	/* set W1C value to clear the interrupt, this hits the register first */
+	ret = ath10k_sdio_writesb32(ar, MBOX_ERROR_INT_STATUS_ADDRESS,
+				    error_int_status);
+	if (ret) {
+		ath10k_warn(ar, "unable to write to error int status address: %d\n",
+			    ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ath10k_sdio_mbox_proc_cpu_intr(struct ath10k *ar)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct ath10k_sdio_irq_data *irq_data = &ar_sdio->irq_data;
+	u8 cpu_int_status;
+	int ret;
+
+	mutex_lock(&irq_data->mtx);
+	cpu_int_status = irq_data->irq_proc_reg->cpu_int_status &
+			 irq_data->irq_en_reg->cpu_int_status_en;
+	if (!cpu_int_status) {
+		ath10k_warn(ar, "CPU interrupt status is zero\n");
+		ret = -EIO;
+		goto out;
+	}
+
+	/* Clear the interrupt */
+	irq_data->irq_proc_reg->cpu_int_status &= ~cpu_int_status;
+
+	/* Set up the register transfer buffer to hit the register 4 times,
+	 * this is done to make the access 4-byte aligned to mitigate issues
+	 * with host bus interconnects that restrict bus transfer lengths to
+	 * be a multiple of 4-bytes.
+	 *
+	 * Set W1C value to clear the interrupt, this hits the register first.
+	 */
+	ret = ath10k_sdio_writesb32(ar, MBOX_CPU_INT_STATUS_ADDRESS,
+				    cpu_int_status);
+	if (ret) {
+		ath10k_warn(ar, "unable to write to cpu interrupt status address: %d\n",
+			    ret);
+		goto out;
+	}
+
+out:
+	mutex_unlock(&irq_data->mtx);
+	return ret;
+}
+
+static int ath10k_sdio_mbox_read_int_status(struct ath10k *ar,
+					    u8 *host_int_status,
+					    u32 *lookahead)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct ath10k_sdio_irq_data *irq_data = &ar_sdio->irq_data;
+	struct ath10k_sdio_irq_proc_regs *irq_proc_reg = irq_data->irq_proc_reg;
+	struct ath10k_sdio_irq_enable_regs *irq_en_reg = irq_data->irq_en_reg;
+	u8 htc_mbox = FIELD_PREP(ATH10K_HTC_MAILBOX_MASK, 1);
+	int ret;
+
+	mutex_lock(&irq_data->mtx);
+
+	*lookahead = 0;
+	*host_int_status = 0;
+
+	/* int_status_en is supposed to be non zero, otherwise interrupts
+	 * shouldn't be enabled. There is however a short time frame during
+	 * initialization between the irq register and int_status_en init
+	 * where this can happen.
+	 * We silently ignore this condition.
+	 */
+	if (!irq_en_reg->int_status_en) {
+		ret = 0;
+		goto out;
+	}
+
+	/* Read the first sizeof(struct ath10k_irq_proc_registers)
+	 * bytes of the HTC register table. This
+	 * will yield us the value of different int status
+	 * registers and the lookahead registers.
+	 */
+	ret = ath10k_sdio_read(ar, MBOX_HOST_INT_STATUS_ADDRESS,
+			       irq_proc_reg, sizeof(*irq_proc_reg));
+	if (ret)
+		goto out;
+
+	/* Update only those registers that are enabled */
+	*host_int_status = irq_proc_reg->host_int_status &
+			   irq_en_reg->int_status_en;
+
+	/* Look at mbox status */
+	if (!(*host_int_status & htc_mbox)) {
+		*lookahead = 0;
+		ret = 0;
+		goto out;
+	}
+
+	/* Mask out pending mbox value, we use look ahead as
+	 * the real flag for mbox processing.
+	 */
+	*host_int_status &= ~htc_mbox;
+	if (irq_proc_reg->rx_lookahead_valid & htc_mbox) {
+		*lookahead = le32_to_cpu(
+			irq_proc_reg->rx_lookahead[ATH10K_HTC_MAILBOX]);
+		if (!*lookahead)
+			ath10k_warn(ar, "sdio mbox lookahead is zero\n");
+	}
+
+out:
+	mutex_unlock(&irq_data->mtx);
+	return ret;
+}
+
+static int ath10k_sdio_mbox_proc_pending_irqs(struct ath10k *ar,
+					      bool *done)
+{
+	u8 host_int_status;
+	u32 lookahead;
+	int ret;
+
+	/* NOTE: HIF implementation guarantees that the context of this
+	 * call allows us to perform SYNCHRONOUS I/O, that is we can block,
+	 * sleep or call any API that can block or switch thread/task
+	 * contexts. This is a fully schedulable context.
+	 */
+
+	ret = ath10k_sdio_mbox_read_int_status(ar,
+					       &host_int_status,
+					       &lookahead);
+	if (ret) {
+		*done = true;
+		goto out;
+	}
+
+	if (!host_int_status && !lookahead) {
+		ret = 0;
+		*done = true;
+		goto out;
+	}
+
+	if (lookahead) {
+		ath10k_dbg(ar, ATH10K_DBG_SDIO,
+			   "sdio pending mailbox msg lookahead 0x%08x\n",
+			   lookahead);
+
+		ret = ath10k_sdio_mbox_rxmsg_pending_handler(ar,
+							     lookahead,
+							     done);
+		if (ret)
+			goto out;
+	}
+
+	/* now, handle the rest of the interrupts */
+	ath10k_dbg(ar, ATH10K_DBG_SDIO,
+		   "sdio host_int_status 0x%x\n", host_int_status);
+
+	if (FIELD_GET(MBOX_HOST_INT_STATUS_CPU_MASK, host_int_status)) {
+		/* CPU Interrupt */
+		ret = ath10k_sdio_mbox_proc_cpu_intr(ar);
+		if (ret)
+			goto out;
+	}
+
+	if (FIELD_GET(MBOX_HOST_INT_STATUS_ERROR_MASK, host_int_status)) {
+		/* Error Interrupt */
+		ret = ath10k_sdio_mbox_proc_err_intr(ar);
+		if (ret)
+			goto out;
+	}
+
+	if (FIELD_GET(MBOX_HOST_INT_STATUS_COUNTER_MASK, host_int_status))
+		/* Counter Interrupt */
+		ret = ath10k_sdio_mbox_proc_counter_intr(ar);
+
+	ret = 0;
+
+out:
+	/* An optimization to bypass reading the IRQ status registers
+	 * unecessarily which can re-wake the target, if upper layers
+	 * determine that we are in a low-throughput mode, we can rely on
+	 * taking another interrupt rather than re-checking the status
+	 * registers which can re-wake the target.
+	 *
+	 * NOTE : for host interfaces that makes use of detecting pending
+	 * mbox messages at hif can not use this optimization due to
+	 * possible side effects, SPI requires the host to drain all
+	 * messages from the mailbox before exiting the ISR routine.
+	 */
+
+	ath10k_dbg(ar, ATH10K_DBG_SDIO,
+		   "sdio pending irqs done %d status %d",
+		   *done, ret);
+
+	return ret;
+}
+
+static void ath10k_sdio_set_mbox_info(struct ath10k *ar)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct ath10k_mbox_info *mbox_info = &ar_sdio->mbox_info;
+	u16 device = ar_sdio->func->device, dev_id_base, dev_id_chiprev;
+
+	mbox_info->htc_addr = ATH10K_HIF_MBOX_BASE_ADDR;
+	mbox_info->block_size = ATH10K_HIF_MBOX_BLOCK_SIZE;
+	mbox_info->block_mask = ATH10K_HIF_MBOX_BLOCK_SIZE - 1;
+	mbox_info->gmbox_addr = ATH10K_HIF_GMBOX_BASE_ADDR;
+	mbox_info->gmbox_sz = ATH10K_HIF_GMBOX_WIDTH;
+
+	mbox_info->ext_info[0].htc_ext_addr = ATH10K_HIF_MBOX0_EXT_BASE_ADDR;
+
+	dev_id_base = FIELD_GET(QCA_MANUFACTURER_ID_BASE, device);
+	dev_id_chiprev = FIELD_GET(QCA_MANUFACTURER_ID_REV_MASK, device);
+	switch (dev_id_base) {
+	case QCA_MANUFACTURER_ID_AR6005_BASE:
+		if (dev_id_chiprev < 4)
+			mbox_info->ext_info[0].htc_ext_sz =
+				ATH10K_HIF_MBOX0_EXT_WIDTH;
+		else
+			/* from QCA6174 2.0(0x504), the width has been extended
+			 * to 56K
+			 */
+			mbox_info->ext_info[0].htc_ext_sz =
+				ATH10K_HIF_MBOX0_EXT_WIDTH_ROME_2_0;
+		break;
+	case QCA_MANUFACTURER_ID_QCA9377_BASE:
+		mbox_info->ext_info[0].htc_ext_sz =
+			ATH10K_HIF_MBOX0_EXT_WIDTH_ROME_2_0;
+		break;
+	default:
+		mbox_info->ext_info[0].htc_ext_sz =
+				ATH10K_HIF_MBOX0_EXT_WIDTH;
+	}
+
+	mbox_info->ext_info[1].htc_ext_addr =
+		mbox_info->ext_info[0].htc_ext_addr +
+		mbox_info->ext_info[0].htc_ext_sz +
+		ATH10K_HIF_MBOX_DUMMY_SPACE_SIZE;
+	mbox_info->ext_info[1].htc_ext_sz = ATH10K_HIF_MBOX1_EXT_WIDTH;
+}
+
+/* BMI functions */
+
+static int ath10k_sdio_bmi_credits(struct ath10k *ar)
+{
+	u32 addr, cmd_credits;
+	unsigned long timeout;
+	int ret;
+
+	/* Read the counter register to get the command credits */
+	addr = MBOX_COUNT_DEC_ADDRESS + ATH10K_HIF_MBOX_NUM_MAX * 4;
+	timeout = jiffies + BMI_COMMUNICATION_TIMEOUT_HZ;
+	cmd_credits = 0;
+
+	while (time_before(jiffies, timeout) && !cmd_credits) {
+		/* Hit the credit counter with a 4-byte access, the first byte
+		 * read will hit the counter and cause a decrement, while the
+		 * remaining 3 bytes has no effect. The rationale behind this
+		 * is to make all HIF accesses 4-byte aligned.
+		 */
+		ret = ath10k_sdio_read32(ar, addr, &cmd_credits);
+		if (ret) {
+			ath10k_warn(ar,
+				    "unable to decrement the command credit count register: %d\n",
+				    ret);
+			return ret;
+		}
+
+		/* The counter is only 8 bits.
+		 * Ignore anything in the upper 3 bytes
+		 */
+		cmd_credits &= 0xFF;
+	}
+
+	if (!cmd_credits) {
+		ath10k_warn(ar, "bmi communication timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int ath10k_sdio_bmi_get_rx_lookahead(struct ath10k *ar)
+{
+	unsigned long timeout;
+	u32 rx_word;
+	int ret;
+
+	timeout = jiffies + BMI_COMMUNICATION_TIMEOUT_HZ;
+	rx_word = 0;
+
+	while ((time_before(jiffies, timeout)) && !rx_word) {
+		ret = ath10k_sdio_read32(ar,
+					 MBOX_HOST_INT_STATUS_ADDRESS,
+					 &rx_word);
+		if (ret) {
+			ath10k_warn(ar, "unable to read RX_LOOKAHEAD_VALID: %d\n", ret);
+			return ret;
+		}
+
+		 /* all we really want is one bit */
+		rx_word &= 1;
+	}
+
+	if (!rx_word) {
+		ath10k_warn(ar, "bmi_recv_buf FIFO empty\n");
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static int ath10k_sdio_bmi_exchange_msg(struct ath10k *ar,
+					void *req, u32 req_len,
+					void *resp, u32 *resp_len)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	u32 addr;
+	int ret;
+
+	if (req) {
+		ret = ath10k_sdio_bmi_credits(ar);
+		if (ret)
+			return ret;
+
+		addr = ar_sdio->mbox_info.htc_addr;
+
+		memcpy(ar_sdio->bmi_buf, req, req_len);
+		ret = ath10k_sdio_write(ar, addr, ar_sdio->bmi_buf, req_len);
+		if (ret) {
+			ath10k_warn(ar,
+				    "unable to send the bmi data to the device: %d\n",
+				    ret);
+			return ret;
+		}
+	}
+
+	if (!resp || !resp_len)
+		/* No response expected */
+		return 0;
+
+	/* During normal bootup, small reads may be required.
+	 * Rather than issue an HIF Read and then wait as the Target
+	 * adds successive bytes to the FIFO, we wait here until
+	 * we know that response data is available.
+	 *
+	 * This allows us to cleanly timeout on an unexpected
+	 * Target failure rather than risk problems at the HIF level.
+	 * In particular, this avoids SDIO timeouts and possibly garbage
+	 * data on some host controllers.  And on an interconnect
+	 * such as Compact Flash (as well as some SDIO masters) which
+	 * does not provide any indication on data timeout, it avoids
+	 * a potential hang or garbage response.
+	 *
+	 * Synchronization is more difficult for reads larger than the
+	 * size of the MBOX FIFO (128B), because the Target is unable
+	 * to push the 129th byte of data until AFTER the Host posts an
+	 * HIF Read and removes some FIFO data.  So for large reads the
+	 * Host proceeds to post an HIF Read BEFORE all the data is
+	 * actually available to read.  Fortunately, large BMI reads do
+	 * not occur in practice -- they're supported for debug/development.
+	 *
+	 * So Host/Target BMI synchronization is divided into these cases:
+	 *  CASE 1: length < 4
+	 *        Should not happen
+	 *
+	 *  CASE 2: 4 <= length <= 128
+	 *        Wait for first 4 bytes to be in FIFO
+	 *        If CONSERVATIVE_BMI_READ is enabled, also wait for
+	 *        a BMI command credit, which indicates that the ENTIRE
+	 *        response is available in the the FIFO
+	 *
+	 *  CASE 3: length > 128
+	 *        Wait for the first 4 bytes to be in FIFO
+	 *
+	 * For most uses, a small timeout should be sufficient and we will
+	 * usually see a response quickly; but there may be some unusual
+	 * (debug) cases of BMI_EXECUTE where we want an larger timeout.
+	 * For now, we use an unbounded busy loop while waiting for
+	 * BMI_EXECUTE.
+	 *
+	 * If BMI_EXECUTE ever needs to support longer-latency execution,
+	 * especially in production, this code needs to be enhanced to sleep
+	 * and yield.  Also note that BMI_COMMUNICATION_TIMEOUT is currently
+	 * a function of Host processor speed.
+	 */
+	ret = ath10k_sdio_bmi_get_rx_lookahead(ar);
+	if (ret)
+		return ret;
+
+	/* We always read from the start of the mbox address */
+	addr = ar_sdio->mbox_info.htc_addr;
+	ret = ath10k_sdio_read(ar, addr, ar_sdio->bmi_buf, *resp_len);
+	if (ret) {
+		ath10k_warn(ar,
+			    "unable to read the bmi data from the device: %d\n",
+			    ret);
+		return ret;
+	}
+
+	memcpy(resp, ar_sdio->bmi_buf, *resp_len);
+
+	return 0;
+}
+
+/* sdio async handling functions */
+
+static struct ath10k_sdio_bus_request
+*ath10k_sdio_alloc_busreq(struct ath10k *ar)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct ath10k_sdio_bus_request *bus_req;
+
+	spin_lock_bh(&ar_sdio->lock);
+
+	if (list_empty(&ar_sdio->bus_req_freeq)) {
+		bus_req = NULL;
+		goto out;
+	}
+
+	bus_req = list_first_entry(&ar_sdio->bus_req_freeq,
+				   struct ath10k_sdio_bus_request, list);
+	list_del(&bus_req->list);
+
+out:
+	spin_unlock_bh(&ar_sdio->lock);
+	return bus_req;
+}
+
+static void ath10k_sdio_free_bus_req(struct ath10k *ar,
+				     struct ath10k_sdio_bus_request *bus_req)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+
+	memset(bus_req, 0, sizeof(*bus_req));
+
+	spin_lock_bh(&ar_sdio->lock);
+	list_add_tail(&bus_req->list, &ar_sdio->bus_req_freeq);
+	spin_unlock_bh(&ar_sdio->lock);
+}
+
+static void __ath10k_sdio_write_async(struct ath10k *ar,
+				      struct ath10k_sdio_bus_request *req)
+{
+	struct ath10k_htc_ep *ep;
+	struct sk_buff *skb;
+	int ret;
+
+	skb = req->skb;
+	ret = ath10k_sdio_write(ar, req->address, skb->data, skb->len);
+	if (ret)
+		ath10k_warn(ar, "failed to write skb to 0x%x asynchronously: %d",
+			    req->address, ret);
+
+	if (req->htc_msg) {
+		ep = &ar->htc.endpoint[req->eid];
+		ath10k_htc_notify_tx_completion(ep, skb);
+	} else if (req->comp) {
+		complete(req->comp);
+	}
+
+	ath10k_sdio_free_bus_req(ar, req);
+}
+
+static void ath10k_sdio_write_async_work(struct work_struct *work)
+{
+	struct ath10k_sdio *ar_sdio = container_of(work, struct ath10k_sdio,
+						   wr_async_work);
+	struct ath10k *ar = ar_sdio->ar;
+	struct ath10k_sdio_bus_request *req, *tmp_req;
+
+	spin_lock_bh(&ar_sdio->wr_async_lock);
+
+	list_for_each_entry_safe(req, tmp_req, &ar_sdio->wr_asyncq, list) {
+		list_del(&req->list);
+		spin_unlock_bh(&ar_sdio->wr_async_lock);
+		__ath10k_sdio_write_async(ar, req);
+		spin_lock_bh(&ar_sdio->wr_async_lock);
+	}
+
+	spin_unlock_bh(&ar_sdio->wr_async_lock);
+}
+
+static int ath10k_sdio_prep_async_req(struct ath10k *ar, u32 addr,
+				      struct sk_buff *skb,
+				      struct completion *comp,
+				      bool htc_msg, enum ath10k_htc_ep_id eid)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct ath10k_sdio_bus_request *bus_req;
+
+	/* Allocate a bus request for the message and queue it on the
+	 * SDIO workqueue.
+	 */
+	bus_req = ath10k_sdio_alloc_busreq(ar);
+	if (!bus_req) {
+		ath10k_warn(ar,
+			    "unable to allocate bus request for async request\n");
+		return -ENOMEM;
+	}
+
+	bus_req->skb = skb;
+	bus_req->eid = eid;
+	bus_req->address = addr;
+	bus_req->htc_msg = htc_msg;
+	bus_req->comp = comp;
+
+	spin_lock_bh(&ar_sdio->wr_async_lock);
+	list_add_tail(&bus_req->list, &ar_sdio->wr_asyncq);
+	spin_unlock_bh(&ar_sdio->wr_async_lock);
+
+	return 0;
+}
+
+/* IRQ handler */
+
+static void ath10k_sdio_irq_handler(struct sdio_func *func)
+{
+	struct ath10k_sdio *ar_sdio = sdio_get_drvdata(func);
+	struct ath10k *ar = ar_sdio->ar;
+	unsigned long timeout;
+	bool done = false;
+	int ret;
+
+	/* Release the host during interrupts so we can pick it back up when
+	 * we process commands.
+	 */
+	sdio_release_host(ar_sdio->func);
+
+	timeout = jiffies + ATH10K_SDIO_HIF_COMMUNICATION_TIMEOUT_HZ;
+	while (time_before(jiffies, timeout) && !done) {
+		ret = ath10k_sdio_mbox_proc_pending_irqs(ar, &done);
+		if (ret)
+			break;
+	}
+
+	sdio_claim_host(ar_sdio->func);
+
+	wake_up(&ar_sdio->irq_wq);
+
+	if (ret && ret != -ECANCELED)
+		ath10k_warn(ar, "failed to process pending SDIO interrupts: %d\n",
+			    ret);
+}
+
+/* sdio HIF functions */
+
+static int ath10k_sdio_hif_disable_intrs(struct ath10k *ar)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct ath10k_sdio_irq_data *irq_data = &ar_sdio->irq_data;
+	struct ath10k_sdio_irq_enable_regs *regs = irq_data->irq_en_reg;
+	int ret;
+
+	mutex_lock(&irq_data->mtx);
+
+	memset(regs, 0, sizeof(*regs));
+	ret = ath10k_sdio_write(ar, MBOX_INT_STATUS_ENABLE_ADDRESS,
+				&regs->int_status_en, sizeof(*regs));
+	if (ret)
+		ath10k_warn(ar, "unable to disable sdio interrupts: %d\n", ret);
+
+	mutex_unlock(&irq_data->mtx);
+
+	return ret;
+}
+
+static int ath10k_sdio_hif_power_up(struct ath10k *ar)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct sdio_func *func = ar_sdio->func;
+	int ret;
+
+	if (!ar_sdio->is_disabled)
+		return 0;
+
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "sdio power on\n");
+
+	sdio_claim_host(func);
+
+	ret = sdio_enable_func(func);
+	if (ret) {
+		ath10k_warn(ar, "unable to enable sdio function: %d)\n", ret);
+		sdio_release_host(func);
+		return ret;
+	}
+
+	sdio_release_host(func);
+
+	/* Wait for hardware to initialise. It should take a lot less than
+	 * 20 ms but let's be conservative here.
+	 */
+	msleep(20);
+
+	ar_sdio->is_disabled = false;
+
+	ret = ath10k_sdio_hif_disable_intrs(ar);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static void ath10k_sdio_hif_power_down(struct ath10k *ar)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	int ret;
+
+	if (ar_sdio->is_disabled)
+		return;
+
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "sdio power off\n");
+
+	/* Disable the card */
+	sdio_claim_host(ar_sdio->func);
+	ret = sdio_disable_func(ar_sdio->func);
+	sdio_release_host(ar_sdio->func);
+
+	if (ret)
+		ath10k_warn(ar, "unable to disable sdio function: %d\n", ret);
+
+	ar_sdio->is_disabled = true;
+}
+
+static int ath10k_sdio_hif_tx_sg(struct ath10k *ar, u8 pipe_id,
+				 struct ath10k_hif_sg_item *items, int n_items)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	enum ath10k_htc_ep_id eid;
+	struct sk_buff *skb;
+	int ret, i;
+
+	eid = pipe_id_to_eid(pipe_id);
+
+	for (i = 0; i < n_items; i++) {
+		size_t padded_len;
+		u32 address;
+
+		skb = items[i].transfer_context;
+		padded_len = ath10k_sdio_calc_txrx_padded_len(ar_sdio,
+							      skb->len);
+		skb_trim(skb, padded_len);
+
+		/* Write TX data to the end of the mbox address space */
+		address = ar_sdio->mbox_addr[eid] + ar_sdio->mbox_size[eid] -
+			  skb->len;
+		ret = ath10k_sdio_prep_async_req(ar, address, skb,
+						 NULL, true, eid);
+		if (ret)
+			return ret;
+	}
+
+	queue_work(ar_sdio->workqueue, &ar_sdio->wr_async_work);
+
+	return 0;
+}
+
+static int ath10k_sdio_hif_enable_intrs(struct ath10k *ar)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct ath10k_sdio_irq_data *irq_data = &ar_sdio->irq_data;
+	struct ath10k_sdio_irq_enable_regs *regs = irq_data->irq_en_reg;
+	int ret;
+
+	mutex_lock(&irq_data->mtx);
+
+	/* Enable all but CPU interrupts */
+	regs->int_status_en = FIELD_PREP(MBOX_INT_STATUS_ENABLE_ERROR_MASK, 1) |
+			      FIELD_PREP(MBOX_INT_STATUS_ENABLE_CPU_MASK, 1) |
+			      FIELD_PREP(MBOX_INT_STATUS_ENABLE_COUNTER_MASK, 1);
+
+	/* NOTE: There are some cases where HIF can do detection of
+	 * pending mbox messages which is disabled now.
+	 */
+	regs->int_status_en |=
+		FIELD_PREP(MBOX_INT_STATUS_ENABLE_MBOX_DATA_MASK, 1);
+
+	/* Set up the CPU Interrupt status Register */
+	regs->cpu_int_status_en = 0;
+
+	/* Set up the Error Interrupt status Register */
+	regs->err_int_status_en =
+		FIELD_PREP(MBOX_ERROR_STATUS_ENABLE_RX_UNDERFLOW_MASK, 1) |
+		FIELD_PREP(MBOX_ERROR_STATUS_ENABLE_TX_OVERFLOW_MASK, 1);
+
+	/* Enable Counter interrupt status register to get fatal errors for
+	 * debugging.
+	 */
+	regs->cntr_int_status_en =
+		FIELD_PREP(MBOX_COUNTER_INT_STATUS_ENABLE_BIT_MASK,
+			   ATH10K_SDIO_TARGET_DEBUG_INTR_MASK);
+
+	ret = ath10k_sdio_write(ar, MBOX_INT_STATUS_ENABLE_ADDRESS,
+				&regs->int_status_en, sizeof(*regs));
+	if (ret)
+		ath10k_warn(ar,
+			    "failed to update mbox interrupt status register : %d\n",
+			    ret);
+
+	mutex_unlock(&irq_data->mtx);
+	return ret;
+}
+
+static int ath10k_sdio_hif_set_mbox_sleep(struct ath10k *ar, bool enable_sleep)
+{
+	u32 val;
+	int ret;
+
+	ret = ath10k_sdio_read32(ar, ATH10K_FIFO_TIMEOUT_AND_CHIP_CONTROL, &val);
+	if (ret) {
+		ath10k_warn(ar, "failed to read fifo/chip control register: %d\n",
+			    ret);
+		return ret;
+	}
+
+	if (enable_sleep)
+		val &= ATH10K_FIFO_TIMEOUT_AND_CHIP_CONTROL_DISABLE_SLEEP_OFF;
+	else
+		val |= ATH10K_FIFO_TIMEOUT_AND_CHIP_CONTROL_DISABLE_SLEEP_ON;
+
+	ret = ath10k_sdio_write32(ar, ATH10K_FIFO_TIMEOUT_AND_CHIP_CONTROL, val);
+	if (ret) {
+		ath10k_warn(ar, "failed to write to FIFO_TIMEOUT_AND_CHIP_CONTROL: %d",
+			    ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* HIF diagnostics */
+
+static int ath10k_sdio_hif_diag_read(struct ath10k *ar, u32 address, void *buf,
+				     size_t buf_len)
+{
+	int ret;
+
+	/* set window register to start read cycle */
+	ret = ath10k_sdio_write32(ar, MBOX_WINDOW_READ_ADDR_ADDRESS, address);
+	if (ret) {
+		ath10k_warn(ar, "failed to set mbox window read address: %d", ret);
+		return ret;
+	}
+
+	/* read the data */
+	ret = ath10k_sdio_read(ar, MBOX_WINDOW_DATA_ADDRESS, buf, buf_len);
+	if (ret) {
+		ath10k_warn(ar, "failed to read from mbox window data address: %d\n",
+			    ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ath10k_sdio_hif_diag_read32(struct ath10k *ar, u32 address,
+				       u32 *value)
+{
+	__le32 *val;
+	int ret;
+
+	val = kzalloc(sizeof(*val), GFP_KERNEL);
+	if (!val)
+		return -ENOMEM;
+
+	ret = ath10k_sdio_hif_diag_read(ar, address, val, sizeof(*val));
+	if (ret)
+		goto out;
+
+	*value = __le32_to_cpu(*val);
+
+out:
+	kfree(val);
+
+	return ret;
+}
+
+static int ath10k_sdio_hif_diag_write_mem(struct ath10k *ar, u32 address,
+					  const void *data, int nbytes)
+{
+	int ret;
+
+	/* set write data */
+	ret = ath10k_sdio_write(ar, MBOX_WINDOW_DATA_ADDRESS, data, nbytes);
+	if (ret) {
+		ath10k_warn(ar,
+			    "failed to write 0x%p to mbox window data address: %d\n",
+			    data, ret);
+		return ret;
+	}
+
+	/* set window register, which starts the write cycle */
+	ret = ath10k_sdio_write32(ar, MBOX_WINDOW_WRITE_ADDR_ADDRESS, address);
+	if (ret) {
+		ath10k_warn(ar, "failed to set mbox window write address: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* HIF start/stop */
+
+static int ath10k_sdio_hif_start(struct ath10k *ar)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	u32 addr, val;
+	int ret;
+
+	/* Sleep 20 ms before HIF interrupts are disabled.
+	 * This will give target plenty of time to process the BMI done
+	 * request before interrupts are disabled.
+	 */
+	msleep(20);
+	ret = ath10k_sdio_hif_disable_intrs(ar);
+	if (ret)
+		return ret;
+
+	/* eid 0 always uses the lower part of the extended mailbox address
+	 * space (ext_info[0].htc_ext_addr).
+	 */
+	ar_sdio->mbox_addr[0] = ar_sdio->mbox_info.ext_info[0].htc_ext_addr;
+	ar_sdio->mbox_size[0] = ar_sdio->mbox_info.ext_info[0].htc_ext_sz;
+
+	sdio_claim_host(ar_sdio->func);
+
+	/* Register the isr */
+	ret =  sdio_claim_irq(ar_sdio->func, ath10k_sdio_irq_handler);
+	if (ret) {
+		ath10k_warn(ar, "failed to claim sdio interrupt: %d\n", ret);
+		sdio_release_host(ar_sdio->func);
+		return ret;
+	}
+
+	sdio_release_host(ar_sdio->func);
+
+	ret = ath10k_sdio_hif_enable_intrs(ar);
+	if (ret)
+		ath10k_warn(ar, "failed to enable sdio interrupts: %d\n", ret);
+
+	addr = host_interest_item_address(HI_ITEM(hi_acs_flags));
+
+	ret = ath10k_sdio_hif_diag_read32(ar, addr, &val);
+	if (ret) {
+		ath10k_warn(ar, "unable to read hi_acs_flags address: %d\n", ret);
+		return ret;
+	}
+
+	if (val & HI_ACS_FLAGS_SDIO_SWAP_MAILBOX_FW_ACK) {
+		ath10k_dbg(ar, ATH10K_DBG_SDIO,
+			   "sdio mailbox swap service enabled\n");
+		ar_sdio->swap_mbox = true;
+	}
+
+	/* Enable sleep and then disable it again */
+	ret = ath10k_sdio_hif_set_mbox_sleep(ar, true);
+	if (ret)
+		return ret;
+
+	/* Wait for 20ms for the written value to take effect */
+	msleep(20);
+
+	ret = ath10k_sdio_hif_set_mbox_sleep(ar, false);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+#define SDIO_IRQ_DISABLE_TIMEOUT_HZ (3 * HZ)
+
+static void ath10k_sdio_irq_disable(struct ath10k *ar)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct ath10k_sdio_irq_data *irq_data = &ar_sdio->irq_data;
+	struct ath10k_sdio_irq_enable_regs *regs = irq_data->irq_en_reg;
+	struct sk_buff *skb;
+	struct completion irqs_disabled_comp;
+	int ret;
+
+	skb = dev_alloc_skb(sizeof(*regs));
+	if (!skb)
+		return;
+
+	mutex_lock(&irq_data->mtx);
+
+	memset(regs, 0, sizeof(*regs)); /* disable all interrupts */
+	memcpy(skb->data, regs, sizeof(*regs));
+	skb_put(skb, sizeof(*regs));
+
+	mutex_unlock(&irq_data->mtx);
+
+	init_completion(&irqs_disabled_comp);
+	ret = ath10k_sdio_prep_async_req(ar, MBOX_INT_STATUS_ENABLE_ADDRESS,
+					 skb, &irqs_disabled_comp, false, 0);
+	if (ret)
+		goto out;
+
+	queue_work(ar_sdio->workqueue, &ar_sdio->wr_async_work);
+
+	/* Wait for the completion of the IRQ disable request.
+	 * If there is a timeout we will try to disable irq's anyway.
+	 */
+	ret = wait_for_completion_timeout(&irqs_disabled_comp,
+					  SDIO_IRQ_DISABLE_TIMEOUT_HZ);
+	if (!ret)
+		ath10k_warn(ar, "sdio irq disable request timed out\n");
+
+	sdio_claim_host(ar_sdio->func);
+
+	ret = sdio_release_irq(ar_sdio->func);
+	if (ret)
+		ath10k_warn(ar, "failed to release sdio interrupt: %d\n", ret);
+
+	sdio_release_host(ar_sdio->func);
+
+out:
+	kfree_skb(skb);
+}
+
+static void ath10k_sdio_hif_stop(struct ath10k *ar)
+{
+	struct ath10k_sdio_bus_request *req, *tmp_req;
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+
+	ath10k_sdio_irq_disable(ar);
+
+	cancel_work_sync(&ar_sdio->wr_async_work);
+
+	spin_lock_bh(&ar_sdio->wr_async_lock);
+
+	/* Free all bus requests that have not been handled */
+	list_for_each_entry_safe(req, tmp_req, &ar_sdio->wr_asyncq, list) {
+		struct ath10k_htc_ep *ep;
+
+		list_del(&req->list);
+
+		if (req->htc_msg) {
+			ep = &ar->htc.endpoint[req->eid];
+			ath10k_htc_notify_tx_completion(ep, req->skb);
+		} else if (req->skb) {
+			kfree_skb(req->skb);
+		}
+		ath10k_sdio_free_bus_req(ar, req);
+	}
+
+	spin_unlock_bh(&ar_sdio->wr_async_lock);
+}
+
+#ifdef CONFIG_PM
+
+static int ath10k_sdio_hif_suspend(struct ath10k *ar)
+{
+	return -EOPNOTSUPP;
+}
+
+static int ath10k_sdio_hif_resume(struct ath10k *ar)
+{
+	switch (ar->state) {
+	case ATH10K_STATE_OFF:
+		ath10k_dbg(ar, ATH10K_DBG_SDIO,
+			   "sdio resume configuring sdio\n");
+
+		/* need to set sdio settings after power is cut from sdio */
+		ath10k_sdio_config(ar);
+		break;
+
+	case ATH10K_STATE_ON:
+	default:
+		break;
+	}
+
+	return 0;
+}
+#endif
+
+static int ath10k_sdio_hif_map_service_to_pipe(struct ath10k *ar,
+					       u16 service_id,
+					       u8 *ul_pipe, u8 *dl_pipe)
+{
+	struct ath10k_sdio *ar_sdio = ath10k_sdio_priv(ar);
+	struct ath10k_htc *htc = &ar->htc;
+	u32 htt_addr, wmi_addr, htt_mbox_size, wmi_mbox_size;
+	enum ath10k_htc_ep_id eid;
+	bool ep_found = false;
+	int i;
+
+	/* For sdio, we are interested in the mapping between eid
+	 * and pipeid rather than service_id to pipe_id.
+	 * First we find out which eid has been allocated to the
+	 * service...
+	 */
+	for (i = 0; i < ATH10K_HTC_EP_COUNT; i++) {
+		if (htc->endpoint[i].service_id == service_id) {
+			eid = htc->endpoint[i].eid;
+			ep_found = true;
+			break;
+		}
+	}
+
+	if (!ep_found)
+		return -EINVAL;
+
+	/* Then we create the simplest mapping possible between pipeid
+	 * and eid
+	 */
+	*ul_pipe = *dl_pipe = (u8)eid;
+
+	/* Normally, HTT will use the upper part of the extended
+	 * mailbox address space (ext_info[1].htc_ext_addr) and WMI ctrl
+	 * the lower part (ext_info[0].htc_ext_addr).
+	 * If fw wants swapping of mailbox addresses, the opposite is true.
+	 */
+	if (ar_sdio->swap_mbox) {
+		htt_addr = ar_sdio->mbox_info.ext_info[0].htc_ext_addr;
+		wmi_addr = ar_sdio->mbox_info.ext_info[1].htc_ext_addr;
+		htt_mbox_size = ar_sdio->mbox_info.ext_info[0].htc_ext_sz;
+		wmi_mbox_size = ar_sdio->mbox_info.ext_info[1].htc_ext_sz;
+	} else {
+		htt_addr = ar_sdio->mbox_info.ext_info[1].htc_ext_addr;
+		wmi_addr = ar_sdio->mbox_info.ext_info[0].htc_ext_addr;
+		htt_mbox_size = ar_sdio->mbox_info.ext_info[1].htc_ext_sz;
+		wmi_mbox_size = ar_sdio->mbox_info.ext_info[0].htc_ext_sz;
+	}
+
+	switch (service_id) {
+	case ATH10K_HTC_SVC_ID_RSVD_CTRL:
+		/* HTC ctrl ep mbox address has already been setup in
+		 * ath10k_sdio_hif_start
+		 */
+		break;
+	case ATH10K_HTC_SVC_ID_WMI_CONTROL:
+		ar_sdio->mbox_addr[eid] = wmi_addr;
+		ar_sdio->mbox_size[eid] = wmi_mbox_size;
+		ath10k_dbg(ar, ATH10K_DBG_SDIO,
+			   "sdio wmi ctrl mbox_addr 0x%x mbox_size %d\n",
+			   ar_sdio->mbox_addr[eid], ar_sdio->mbox_size[eid]);
+		break;
+	case ATH10K_HTC_SVC_ID_HTT_DATA_MSG:
+		ar_sdio->mbox_addr[eid] = htt_addr;
+		ar_sdio->mbox_size[eid] = htt_mbox_size;
+		ath10k_dbg(ar, ATH10K_DBG_SDIO,
+			   "sdio htt data mbox_addr 0x%x mbox_size %d\n",
+			   ar_sdio->mbox_addr[eid], ar_sdio->mbox_size[eid]);
+		break;
+	default:
+		ath10k_warn(ar, "unsupported HTC service id: %d\n",
+			    service_id);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void ath10k_sdio_hif_get_default_pipe(struct ath10k *ar,
+					     u8 *ul_pipe, u8 *dl_pipe)
+{
+	ath10k_dbg(ar, ATH10K_DBG_SDIO, "sdio hif get default pipe\n");
+
+	/* HTC ctrl ep (SVC id 1) always has eid (and pipe_id in our
+	 * case) == 0
+	 */
+	*ul_pipe = 0;
+	*dl_pipe = 0;
+}
+
+/* This op is currently only used by htc_wait_target if the HTC ready
+ * message times out. It is not applicable for SDIO since there is nothing
+ * we can do if the HTC ready message does not arrive in time.
+ * TODO: Make this op non mandatory by introducing a NULL check in the
+ * hif op wrapper.
+ */
+static void ath10k_sdio_hif_send_complete_check(struct ath10k *ar,
+						u8 pipe, int force)
+{
+}
+
+static const struct ath10k_hif_ops ath10k_sdio_hif_ops = {
+	.tx_sg			= ath10k_sdio_hif_tx_sg,
+	.diag_read		= ath10k_sdio_hif_diag_read,
+	.diag_write		= ath10k_sdio_hif_diag_write_mem,
+	.exchange_bmi_msg	= ath10k_sdio_bmi_exchange_msg,
+	.start			= ath10k_sdio_hif_start,
+	.stop			= ath10k_sdio_hif_stop,
+	.map_service_to_pipe	= ath10k_sdio_hif_map_service_to_pipe,
+	.get_default_pipe	= ath10k_sdio_hif_get_default_pipe,
+	.send_complete_check	= ath10k_sdio_hif_send_complete_check,
+	.power_up		= ath10k_sdio_hif_power_up,
+	.power_down		= ath10k_sdio_hif_power_down,
+#ifdef CONFIG_PM
+	.suspend		= ath10k_sdio_hif_suspend,
+	.resume			= ath10k_sdio_hif_resume,
+#endif
+};
+
+#ifdef CONFIG_PM_SLEEP
+
+/* Empty handlers so that mmc subsystem doesn't remove us entirely during
+ * suspend. We instead follow cfg80211 suspend/resume handlers.
+ */
+static int ath10k_sdio_pm_suspend(struct device *device)
+{
+	return 0;
+}
+
+static int ath10k_sdio_pm_resume(struct device *device)
+{
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(ath10k_sdio_pm_ops, ath10k_sdio_pm_suspend,
+			 ath10k_sdio_pm_resume);
+
+#define ATH10K_SDIO_PM_OPS (&ath10k_sdio_pm_ops)
+
+#else
+
+#define ATH10K_SDIO_PM_OPS NULL
+
+#endif /* CONFIG_PM_SLEEP */
+
+static int ath10k_sdio_probe(struct sdio_func *func,
+			     const struct sdio_device_id *id)
+{
+	struct ath10k_sdio *ar_sdio;
+	struct ath10k *ar;
+	enum ath10k_hw_rev hw_rev;
+	u32 chip_id, dev_id_base;
+	int ret, i;
+
+	/* Assumption: All SDIO based chipsets (so far) are QCA6174 based.
+	 * If there will be newer chipsets that does not use the hw reg
+	 * setup as defined in qca6174_regs and qca6174_values, this
+	 * assumption is no longer valid and hw_rev must be setup differently
+	 * depending on chipset.
+	 */
+	hw_rev = ATH10K_HW_QCA6174;
+
+	ar = ath10k_core_create(sizeof(*ar_sdio), &func->dev, ATH10K_BUS_SDIO,
+				hw_rev, &ath10k_sdio_hif_ops);
+	if (!ar) {
+		dev_err(&func->dev, "failed to allocate core\n");
+		return -ENOMEM;
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_BOOT,
+		   "sdio new func %d vendor 0x%x device 0x%x block 0x%x/0x%x\n",
+		   func->num, func->vendor, func->device,
+		   func->max_blksize, func->cur_blksize);
+
+	ar_sdio = ath10k_sdio_priv(ar);
+
+	ar_sdio->irq_data.irq_proc_reg =
+		kzalloc(sizeof(struct ath10k_sdio_irq_proc_regs),
+			GFP_KERNEL);
+	if (!ar_sdio->irq_data.irq_proc_reg) {
+		ret = -ENOMEM;
+		goto err_core_destroy;
+	}
+
+	ar_sdio->irq_data.irq_en_reg =
+		kzalloc(sizeof(struct ath10k_sdio_irq_enable_regs),
+			GFP_KERNEL);
+	if (!ar_sdio->irq_data.irq_en_reg) {
+		ret = -ENOMEM;
+		goto err_free_proc_reg;
+	}
+
+	ar_sdio->bmi_buf = kzalloc(BMI_MAX_CMDBUF_SIZE, GFP_KERNEL);
+	if (!ar_sdio->bmi_buf) {
+		ret = -ENOMEM;
+		goto err_free_en_reg;
+	}
+
+	ar_sdio->func = func;
+	sdio_set_drvdata(func, ar_sdio);
+
+	ar_sdio->is_disabled = true;
+	ar_sdio->ar = ar;
+
+	spin_lock_init(&ar_sdio->lock);
+	spin_lock_init(&ar_sdio->wr_async_lock);
+	mutex_init(&ar_sdio->irq_data.mtx);
+
+	INIT_LIST_HEAD(&ar_sdio->bus_req_freeq);
+	INIT_LIST_HEAD(&ar_sdio->wr_asyncq);
+
+	INIT_WORK(&ar_sdio->wr_async_work, ath10k_sdio_write_async_work);
+	ar_sdio->workqueue = create_singlethread_workqueue("ath10k_sdio_wq");
+	if (!ar_sdio->workqueue) {
+		ret = -ENOMEM;
+		goto err_free_bmi_buf;
+	}
+
+	init_waitqueue_head(&ar_sdio->irq_wq);
+
+	for (i = 0; i < ATH10K_SDIO_BUS_REQUEST_MAX_NUM; i++)
+		ath10k_sdio_free_bus_req(ar, &ar_sdio->bus_req[i]);
+
+	dev_id_base = FIELD_GET(QCA_MANUFACTURER_ID_BASE, id->device);
+	switch (dev_id_base) {
+	case QCA_MANUFACTURER_ID_AR6005_BASE:
+	case QCA_MANUFACTURER_ID_QCA9377_BASE:
+		ar->dev_id = QCA9377_1_0_DEVICE_ID;
+		break;
+	default:
+		ret = -ENODEV;
+		ath10k_err(ar, "unsupported device id %u (0x%x)\n",
+			   dev_id_base, id->device);
+		goto err_free_bmi_buf;
+	}
+
+	ar->id.vendor = id->vendor;
+	ar->id.device = id->device;
+
+	ath10k_sdio_set_mbox_info(ar);
+
+	ret = ath10k_sdio_config(ar);
+	if (ret) {
+		ath10k_err(ar, "failed to config sdio: %d\n", ret);
+		goto err_free_wq;
+	}
+
+	/* TODO: don't know yet how to get chip_id with SDIO */
+	chip_id = 0;
+	ret = ath10k_core_register(ar, chip_id);
+	if (ret) {
+		ath10k_err(ar, "failed to register driver core: %d\n", ret);
+		goto err_free_wq;
+	}
+
+	/* TODO: remove this once SDIO support is fully implemented */
+	ath10k_warn(ar, "WARNING: ath10k SDIO support is incomplete, don't expect anything to work!\n");
+
+	return 0;
+
+err_free_wq:
+	destroy_workqueue(ar_sdio->workqueue);
+err_free_bmi_buf:
+	kfree(ar_sdio->bmi_buf);
+err_free_en_reg:
+	kfree(ar_sdio->irq_data.irq_en_reg);
+err_free_proc_reg:
+	kfree(ar_sdio->irq_data.irq_proc_reg);
+err_core_destroy:
+	ath10k_core_destroy(ar);
+
+	return ret;
+}
+
+static void ath10k_sdio_remove(struct sdio_func *func)
+{
+	struct ath10k_sdio *ar_sdio = sdio_get_drvdata(func);
+	struct ath10k *ar = ar_sdio->ar;
+
+	ath10k_dbg(ar, ATH10K_DBG_BOOT,
+		   "sdio removed func %d vendor 0x%x device 0x%x\n",
+		   func->num, func->vendor, func->device);
+
+	(void)ath10k_sdio_hif_disable_intrs(ar);
+	cancel_work_sync(&ar_sdio->wr_async_work);
+	ath10k_core_unregister(ar);
+	ath10k_core_destroy(ar);
+}
+
+static const struct sdio_device_id ath10k_sdio_devices[] = {
+	{SDIO_DEVICE(QCA_MANUFACTURER_CODE,
+		     (QCA_SDIO_ID_AR6005_BASE | 0xA))},
+	{SDIO_DEVICE(QCA_MANUFACTURER_CODE,
+		     (QCA_SDIO_ID_QCA9377_BASE | 0x1))},
+	{},
+};
+
+MODULE_DEVICE_TABLE(sdio, ath10k_sdio_devices);
+
+static struct sdio_driver ath10k_sdio_driver = {
+	.name = "ath10k_sdio",
+	.id_table = ath10k_sdio_devices,
+	.probe = ath10k_sdio_probe,
+	.remove = ath10k_sdio_remove,
+	.drv.pm = ATH10K_SDIO_PM_OPS,
+};
+
+static int __init ath10k_sdio_init(void)
+{
+	int ret;
+
+	ret = sdio_register_driver(&ath10k_sdio_driver);
+	if (ret)
+		pr_err("sdio driver registration failed: %d\n", ret);
+
+	return ret;
+}
+
+static void __exit ath10k_sdio_exit(void)
+{
+	sdio_unregister_driver(&ath10k_sdio_driver);
+}
+
+module_init(ath10k_sdio_init);
+module_exit(ath10k_sdio_exit);
+
+MODULE_AUTHOR("Qualcomm Atheros");
+MODULE_DESCRIPTION("Driver support for Qualcomm Atheros 802.11ac WLAN SDIO devices");
+MODULE_LICENSE("Dual BSD/GPL");
diff --git a/drivers/net/wireless/ath/ath10k/sdio.h b/drivers/net/wireless/ath/ath10k/sdio.h
new file mode 100644
index 000000000000..3f61c67c601d
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/sdio.h
@@ -0,0 +1,229 @@
+/*
+ * Copyright (c) 2004-2011 Atheros Communications Inc.
+ * Copyright (c) 2011-2012 Qualcomm Atheros, Inc.
+ * Copyright (c) 2016-2017 Erik Stromdahl <erik.stromdahl@gmail.com>
+ *
+ * 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 _SDIO_H_
+#define _SDIO_H_
+
+#define ATH10K_HIF_MBOX_BLOCK_SIZE              256
+
+#define QCA_MANUFACTURER_ID_BASE                GENMASK(11, 8)
+#define QCA_MANUFACTURER_ID_AR6005_BASE         0x5
+#define QCA_MANUFACTURER_ID_QCA9377_BASE        0x7
+#define QCA_SDIO_ID_AR6005_BASE                 0x500
+#define QCA_SDIO_ID_QCA9377_BASE                0x700
+#define QCA_MANUFACTURER_ID_REV_MASK            0x00FF
+#define QCA_MANUFACTURER_CODE                   0x271 /* Qualcomm/Atheros */
+
+#define ATH10K_SDIO_MAX_BUFFER_SIZE             4096 /*Unsure of this constant*/
+
+/* Mailbox address in SDIO address space */
+#define ATH10K_HIF_MBOX_BASE_ADDR               0x1000
+#define ATH10K_HIF_MBOX_WIDTH                   0x800
+
+#define ATH10K_HIF_MBOX_TOT_WIDTH \
+	(ATH10K_HIF_MBOX_NUM_MAX * ATH10K_HIF_MBOX_WIDTH)
+
+#define ATH10K_HIF_MBOX0_EXT_BASE_ADDR          0x5000
+#define ATH10K_HIF_MBOX0_EXT_WIDTH              (36 * 1024)
+#define ATH10K_HIF_MBOX0_EXT_WIDTH_ROME_2_0     (56 * 1024)
+#define ATH10K_HIF_MBOX1_EXT_WIDTH              (36 * 1024)
+#define ATH10K_HIF_MBOX_DUMMY_SPACE_SIZE        (2 * 1024)
+
+#define ATH10K_HTC_MBOX_MAX_PAYLOAD_LENGTH \
+	(ATH10K_SDIO_MAX_BUFFER_SIZE - sizeof(struct ath10k_htc_hdr))
+
+#define ATH10K_HIF_MBOX_NUM_MAX                 4
+#define ATH10K_SDIO_BUS_REQUEST_MAX_NUM         64
+
+#define ATH10K_SDIO_HIF_COMMUNICATION_TIMEOUT_HZ (100 * HZ)
+
+/* HTC runs over mailbox 0 */
+#define ATH10K_HTC_MAILBOX                      0
+#define ATH10K_HTC_MAILBOX_MASK                 BIT(ATH10K_HTC_MAILBOX)
+
+/* GMBOX addresses */
+#define ATH10K_HIF_GMBOX_BASE_ADDR              0x7000
+#define ATH10K_HIF_GMBOX_WIDTH                  0x4000
+
+/* Modified versions of the sdio.h macros.
+ * The macros in sdio.h can't be used easily with the FIELD_{PREP|GET}
+ * macros in bitfield.h, so we define our own macros here.
+ */
+#define ATH10K_SDIO_DRIVE_DTSX_MASK \
+	(SDIO_DRIVE_DTSx_MASK << SDIO_DRIVE_DTSx_SHIFT)
+
+#define ATH10K_SDIO_DRIVE_DTSX_TYPE_B           0
+#define ATH10K_SDIO_DRIVE_DTSX_TYPE_A           1
+#define ATH10K_SDIO_DRIVE_DTSX_TYPE_C           2
+#define ATH10K_SDIO_DRIVE_DTSX_TYPE_D           3
+
+/* SDIO CCCR register definitions */
+#define CCCR_SDIO_IRQ_MODE_REG                  0xF0
+#define CCCR_SDIO_IRQ_MODE_REG_SDIO3            0x16
+
+#define CCCR_SDIO_DRIVER_STRENGTH_ENABLE_ADDR   0xF2
+
+#define CCCR_SDIO_DRIVER_STRENGTH_ENABLE_A      0x02
+#define CCCR_SDIO_DRIVER_STRENGTH_ENABLE_C      0x04
+#define CCCR_SDIO_DRIVER_STRENGTH_ENABLE_D      0x08
+
+#define CCCR_SDIO_ASYNC_INT_DELAY_ADDRESS       0xF0
+#define CCCR_SDIO_ASYNC_INT_DELAY_MASK          0xC0
+
+/* mode to enable special 4-bit interrupt assertion without clock */
+#define SDIO_IRQ_MODE_ASYNC_4BIT_IRQ            BIT(0)
+#define SDIO_IRQ_MODE_ASYNC_4BIT_IRQ_SDIO3      BIT(1)
+
+#define ATH10K_SDIO_TARGET_DEBUG_INTR_MASK      0x01
+
+/* The theoretical maximum number of RX messages that can be fetched
+ * from the mbox interrupt handler in one loop is derived in the following
+ * way:
+ *
+ * Let's assume that each packet in a bundle of the maximum bundle size
+ * (HTC_HOST_MAX_MSG_PER_BUNDLE) has the HTC header bundle count set
+ * to the maximum value (HTC_HOST_MAX_MSG_PER_BUNDLE).
+ *
+ * in this case the driver must allocate
+ * (HTC_HOST_MAX_MSG_PER_BUNDLE * HTC_HOST_MAX_MSG_PER_BUNDLE) skb's.
+ */
+#define ATH10K_SDIO_MAX_RX_MSGS \
+	(HTC_HOST_MAX_MSG_PER_BUNDLE * HTC_HOST_MAX_MSG_PER_BUNDLE)
+
+#define ATH10K_FIFO_TIMEOUT_AND_CHIP_CONTROL   0x00000868u
+#define ATH10K_FIFO_TIMEOUT_AND_CHIP_CONTROL_DISABLE_SLEEP_OFF 0xFFFEFFFF
+#define ATH10K_FIFO_TIMEOUT_AND_CHIP_CONTROL_DISABLE_SLEEP_ON 0x10000
+
+struct ath10k_sdio_bus_request {
+	struct list_head list;
+
+	/* sdio address */
+	u32 address;
+
+	struct sk_buff *skb;
+	enum ath10k_htc_ep_id eid;
+	int status;
+	/* Specifies if the current request is an HTC message.
+	 * If not, the eid is not applicable an the TX completion handler
+	 * associated with the endpoint will not be invoked.
+	 */
+	bool htc_msg;
+	/* Completion that (if set) will be invoked for non HTC requests
+	 * (htc_msg == false) when the request has been processed.
+	 */
+	struct completion *comp;
+};
+
+struct ath10k_sdio_rx_data {
+	struct sk_buff *skb;
+	size_t alloc_len;
+	size_t act_len;
+	enum ath10k_htc_ep_id eid;
+	bool part_of_bundle;
+	bool last_in_bundle;
+	bool trailer_only;
+	int status;
+};
+
+struct ath10k_sdio_irq_proc_regs {
+	u8 host_int_status;
+	u8 cpu_int_status;
+	u8 error_int_status;
+	u8 counter_int_status;
+	u8 mbox_frame;
+	u8 rx_lookahead_valid;
+	u8 host_int_status2;
+	u8 gmbox_rx_avail;
+	__le32 rx_lookahead[2];
+	__le32 rx_gmbox_lookahead_alias[2];
+};
+
+struct ath10k_sdio_irq_enable_regs {
+	u8 int_status_en;
+	u8 cpu_int_status_en;
+	u8 err_int_status_en;
+	u8 cntr_int_status_en;
+};
+
+struct ath10k_sdio_irq_data {
+	/* protects irq_proc_reg and irq_en_reg below.
+	 * We use a mutex here and not a spinlock since we will have the
+	 * mutex locked while calling the sdio_memcpy_ functions.
+	 * These function require non atomic context, and hence, spinlocks
+	 * can be held while calling these functions.
+	 */
+	struct mutex mtx;
+	struct ath10k_sdio_irq_proc_regs *irq_proc_reg;
+	struct ath10k_sdio_irq_enable_regs *irq_en_reg;
+};
+
+struct ath10k_mbox_ext_info {
+	u32 htc_ext_addr;
+	u32 htc_ext_sz;
+};
+
+struct ath10k_mbox_info {
+	u32 htc_addr;
+	struct ath10k_mbox_ext_info ext_info[2];
+	u32 block_size;
+	u32 block_mask;
+	u32 gmbox_addr;
+	u32 gmbox_sz;
+};
+
+struct ath10k_sdio {
+	struct sdio_func *func;
+
+	struct ath10k_mbox_info mbox_info;
+	bool swap_mbox;
+	u32 mbox_addr[ATH10K_HTC_EP_COUNT];
+	u32 mbox_size[ATH10K_HTC_EP_COUNT];
+
+	/* available bus requests */
+	struct ath10k_sdio_bus_request bus_req[ATH10K_SDIO_BUS_REQUEST_MAX_NUM];
+	/* free list of bus requests */
+	struct list_head bus_req_freeq;
+	/* protects access to bus_req_freeq */
+	spinlock_t lock;
+
+	struct ath10k_sdio_rx_data rx_pkts[ATH10K_SDIO_MAX_RX_MSGS];
+	size_t n_rx_pkts;
+
+	struct ath10k *ar;
+	struct ath10k_sdio_irq_data irq_data;
+
+	/* temporary buffer for BMI requests */
+	u8 *bmi_buf;
+
+	wait_queue_head_t irq_wq;
+
+	bool is_disabled;
+
+	struct workqueue_struct *workqueue;
+	struct work_struct wr_async_work;
+	struct list_head wr_asyncq;
+	/* protects access to wr_asyncq */
+	spinlock_t wr_async_lock;
+};
+
+static inline struct ath10k_sdio *ath10k_sdio_priv(struct ath10k *ar)
+{
+	return (struct ath10k_sdio *)ar->drv_priv;
+}
+
+#endif
diff --git a/drivers/net/wireless/ath/ath10k/targaddrs.h b/drivers/net/wireless/ath/ath10k/targaddrs.h
index cbac9e4252d6..8bded5da9d0d 100644
--- a/drivers/net/wireless/ath/ath10k/targaddrs.h
+++ b/drivers/net/wireless/ath/ath10k/targaddrs.h
@@ -205,6 +205,24 @@ struct host_interest {
 	 */
 	/* Bit 1 - unused */
 	u32 hi_fw_swap;					/* 0x104 */
+
+	/* global arenas pointer address, used by host driver debug */
+	u32 hi_dynamic_mem_arenas_addr;			/* 0x108 */
+
+	/* allocated bytes of DRAM use by allocated */
+	u32 hi_dynamic_mem_allocated;			/* 0x10C */
+
+	/* remaining bytes of DRAM */
+	u32 hi_dynamic_mem_remaining;			/* 0x110 */
+
+	/* memory track count, configured by host */
+	u32 hi_dynamic_mem_track_max;			/* 0x114 */
+
+	/* minidump buffer */
+	u32 hi_minidump;				/* 0x118 */
+
+	/* bdata's sig and key addr */
+	u32 hi_bd_sig_key;				/* 0x11c */
 } __packed;
 
 #define HI_ITEM(item)  offsetof(struct host_interest, item)
@@ -319,6 +337,12 @@ struct host_interest {
 #define HI_ACS_FLAGS_USE_WWAN       (1 << 1)
 /* Use test VAP */
 #define HI_ACS_FLAGS_TEST_VAP       (1 << 2)
+/* SDIO/mailbox ACS flag definitions */
+#define HI_ACS_FLAGS_SDIO_SWAP_MAILBOX_SET       (1 << 0)
+#define HI_ACS_FLAGS_SDIO_REDUCE_TX_COMPL_SET    (1 << 1)
+#define HI_ACS_FLAGS_ALT_DATA_CREDIT_SIZE        (1 << 2)
+#define HI_ACS_FLAGS_SDIO_SWAP_MAILBOX_FW_ACK    (1 << 16)
+#define HI_ACS_FLAGS_SDIO_REDUCE_TX_COMPL_FW_ACK (1 << 17)
 
 /*
  * CONSOLE FLAGS
diff --git a/drivers/net/wireless/ath/ath10k/testmode.c b/drivers/net/wireless/ath/ath10k/testmode.c
index d8564624415c..9d3eb258ac2f 100644
--- a/drivers/net/wireless/ath/ath10k/testmode.c
+++ b/drivers/net/wireless/ath/ath10k/testmode.c
@@ -137,6 +137,13 @@ static int ath10k_tm_cmd_get_version(struct ath10k *ar, struct nlattr *tb[])
 		return ret;
 	}
 
+	ret = nla_put_u32(skb, ATH10K_TM_ATTR_WMI_OP_VERSION,
+			  ar->normal_mode_fw.fw_file.wmi_op_version);
+	if (ret) {
+		kfree_skb(skb);
+		return ret;
+	}
+
 	return cfg80211_testmode_reply(skb);
 }
 
diff --git a/drivers/net/wireless/ath/ath10k/testmode_i.h b/drivers/net/wireless/ath/ath10k/testmode_i.h
index ba81bf66ce85..191a8f34c8ea 100644
--- a/drivers/net/wireless/ath/ath10k/testmode_i.h
+++ b/drivers/net/wireless/ath/ath10k/testmode_i.h
@@ -33,6 +33,7 @@ enum ath10k_tm_attr {
 	ATH10K_TM_ATTR_WMI_CMDID	= 3,
 	ATH10K_TM_ATTR_VERSION_MAJOR	= 4,
 	ATH10K_TM_ATTR_VERSION_MINOR	= 5,
+	ATH10K_TM_ATTR_WMI_OP_VERSION	= 6,
 
 	/* keep last */
 	__ATH10K_TM_ATTR_AFTER_LAST,
diff --git a/drivers/net/wireless/ath/ath10k/thermal.c b/drivers/net/wireless/ath/ath10k/thermal.c
index 87948aff1bd5..ef717b631ff8 100644
--- a/drivers/net/wireless/ath/ath10k/thermal.c
+++ b/drivers/net/wireless/ath/ath10k/thermal.c
@@ -63,7 +63,7 @@ ath10k_thermal_set_cur_throttle_state(struct thermal_cooling_device *cdev,
 	return 0;
 }
 
-static struct thermal_cooling_device_ops ath10k_thermal_ops = {
+static const struct thermal_cooling_device_ops ath10k_thermal_ops = {
 	.get_max_state = ath10k_thermal_get_max_throttle_state,
 	.get_cur_state = ath10k_thermal_get_cur_throttle_state,
 	.set_cur_state = ath10k_thermal_set_cur_throttle_state,
diff --git a/drivers/net/wireless/ath/ath10k/wmi-tlv.c b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
index f9188027a6f6..7616c1c4bbd3 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-tlv.c
+++ b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
@@ -2022,7 +2022,7 @@ ath10k_wmi_tlv_op_gen_sta_keepalive(struct ath10k *ar,
 	arp->dest_ip4_addr = arg->dest_ip4_addr;
 	ether_addr_copy(arp->dest_mac_addr.addr, arg->dest_mac_addr);
 
-	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv sta keepalive vdev %d enabled %d method %d inverval %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv sta keepalive vdev %d enabled %d method %d interval %d\n",
 		   arg->vdev_id, arg->enabled, arg->method, arg->interval);
 	return skb;
 }
diff --git a/drivers/net/wireless/ath/ath10k/wmi.c b/drivers/net/wireless/ath/ath10k/wmi.c
index 6afc8d27f0d5..3efb404b83c0 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.c
+++ b/drivers/net/wireless/ath/ath10k/wmi.c
@@ -3304,9 +3304,8 @@ static void ath10k_wmi_update_noa(struct ath10k *ar, struct ath10k_vif *arvif,
 
 	if (arvif->u.ap.noa_data)
 		if (!pskb_expand_head(bcn, 0, arvif->u.ap.noa_len, GFP_ATOMIC))
-			memcpy(skb_put(bcn, arvif->u.ap.noa_len),
-			       arvif->u.ap.noa_data,
-			       arvif->u.ap.noa_len);
+			skb_put_data(bcn, arvif->u.ap.noa_data,
+			             arvif->u.ap.noa_len);
 }
 
 static int ath10k_wmi_op_pull_swba_ev(struct ath10k *ar, struct sk_buff *skb,
@@ -4482,31 +4481,17 @@ static int ath10k_wmi_alloc_chunk(struct ath10k *ar, u32 req_id,
 				  u32 num_units, u32 unit_len)
 {
 	dma_addr_t paddr;
-	u32 pool_size = 0;
+	u32 pool_size;
 	int idx = ar->wmi.num_mem_chunks;
-	void *vaddr = NULL;
-
-	if (ar->wmi.num_mem_chunks == ARRAY_SIZE(ar->wmi.mem_chunks))
-		return -ENOMEM;
+	void *vaddr;
 
-	while (!vaddr && num_units) {
-		pool_size = num_units * round_up(unit_len, 4);
-		if (!pool_size)
-			return -EINVAL;
+	pool_size = num_units * round_up(unit_len, 4);
+	vaddr = dma_alloc_coherent(ar->dev, pool_size, &paddr, GFP_KERNEL);
 
-		vaddr = kzalloc(pool_size, GFP_KERNEL | __GFP_NOWARN);
-		if (!vaddr)
-			num_units /= 2;
-	}
-
-	if (!num_units)
+	if (!vaddr)
 		return -ENOMEM;
 
-	paddr = dma_map_single(ar->dev, vaddr, pool_size, DMA_BIDIRECTIONAL);
-	if (dma_mapping_error(ar->dev, paddr)) {
-		kfree(vaddr);
-		return -ENOMEM;
-	}
+	memset(vaddr, 0, pool_size);
 
 	ar->wmi.mem_chunks[idx].vaddr = vaddr;
 	ar->wmi.mem_chunks[idx].paddr = paddr;
@@ -5948,15 +5933,6 @@ static struct sk_buff *ath10k_wmi_10_4_op_gen_init(struct ath10k *ar)
 
 int ath10k_wmi_start_scan_verify(const struct wmi_start_scan_arg *arg)
 {
-	if (arg->ie_len && !arg->ie)
-		return -EINVAL;
-	if (arg->n_channels && !arg->channels)
-		return -EINVAL;
-	if (arg->n_ssids && !arg->ssids)
-		return -EINVAL;
-	if (arg->n_bssids && !arg->bssids)
-		return -EINVAL;
-
 	if (arg->ie_len > WLAN_SCAN_PARAMS_MAX_IE_LEN)
 		return -EINVAL;
 	if (arg->n_channels > ARRAY_SIZE(arg->channels))
@@ -6757,7 +6733,12 @@ ath10k_wmi_peer_assoc_fill_10_4(struct ath10k *ar, void *buf,
 	struct wmi_10_4_peer_assoc_complete_cmd *cmd = buf;
 
 	ath10k_wmi_peer_assoc_fill_10_2(ar, buf, arg);
-	cmd->peer_bw_rxnss_override = 0;
+	if (arg->peer_bw_rxnss_override)
+		cmd->peer_bw_rxnss_override =
+			__cpu_to_le32((arg->peer_bw_rxnss_override - 1) |
+				      BIT(PEER_BW_RXNSS_OVERRIDE_OFFSET));
+	else
+		cmd->peer_bw_rxnss_override = 0;
 }
 
 static int
@@ -8290,11 +8271,10 @@ void ath10k_wmi_free_host_mem(struct ath10k *ar)
 
 	/* free the host memory chunks requested by firmware */
 	for (i = 0; i < ar->wmi.num_mem_chunks; i++) {
-		dma_unmap_single(ar->dev,
-				 ar->wmi.mem_chunks[i].paddr,
-				 ar->wmi.mem_chunks[i].len,
-				 DMA_BIDIRECTIONAL);
-		kfree(ar->wmi.mem_chunks[i].vaddr);
+		dma_free_coherent(ar->dev,
+				  ar->wmi.mem_chunks[i].len,
+				  ar->wmi.mem_chunks[i].vaddr,
+				  ar->wmi.mem_chunks[i].paddr);
 	}
 
 	ar->wmi.num_mem_chunks = 0;
diff --git a/drivers/net/wireless/ath/ath10k/wmi.h b/drivers/net/wireless/ath/ath10k/wmi.h
index 1b4865a55595..baa38c8f847c 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.h
+++ b/drivers/net/wireless/ath/ath10k/wmi.h
@@ -6028,6 +6028,8 @@ struct wmi_10_2_peer_assoc_complete_cmd {
 	__le32 info0; /* WMI_PEER_ASSOC_INFO0_ */
 } __packed;
 
+#define PEER_BW_RXNSS_OVERRIDE_OFFSET  31
+
 struct wmi_10_4_peer_assoc_complete_cmd {
 	struct wmi_10_2_peer_assoc_complete_cmd cmd;
 	__le32 peer_bw_rxnss_override;
@@ -6051,6 +6053,7 @@ struct wmi_peer_assoc_complete_arg {
 	u32 peer_vht_caps;
 	enum wmi_phy_mode peer_phymode;
 	struct wmi_vht_rate_set_arg peer_vht_rates;
+	u32 peer_bw_rxnss_override;
 };
 
 struct wmi_peer_add_wds_entry_cmd {
diff --git a/drivers/net/wireless/ath/ath5k/debug.c b/drivers/net/wireless/ath/ath5k/debug.c
index d068df520e7a..bd7f6d7b199e 100644
--- a/drivers/net/wireless/ath/ath5k/debug.c
+++ b/drivers/net/wireless/ath/ath5k/debug.c
@@ -938,7 +938,10 @@ static int open_file_eeprom(struct inode *inode, struct file *file)
 	}
 
 	for (i = 0; i < eesize; ++i) {
-		AR5K_EEPROM_READ(i, val);
+		if (!ath5k_hw_nvram_read(ah, i, &val)) {
+			ret = -EIO;
+			goto freebuf;
+		}
 		buf[i] = val;
 	}
 
diff --git a/drivers/net/wireless/ath/ath6kl/debug.c b/drivers/net/wireless/ath/ath6kl/debug.c
index e2b7809d7886..1eea6c23976f 100644
--- a/drivers/net/wireless/ath/ath6kl/debug.c
+++ b/drivers/net/wireless/ath/ath6kl/debug.c
@@ -348,7 +348,7 @@ void ath6kl_debug_fwlog_event(struct ath6kl *ar, const void *buf, size_t len)
 	if (!skb)
 		return;
 
-	slot = (struct ath6kl_fwlog_slot *) skb_put(skb, slot_len);
+	slot = skb_put(skb, slot_len);
 	slot->timestamp = cpu_to_le32(jiffies);
 	slot->length = cpu_to_le32(len);
 	memcpy(slot->payload, buf, len);
diff --git a/drivers/net/wireless/ath/ath6kl/htc_pipe.c b/drivers/net/wireless/ath/ath6kl/htc_pipe.c
index d127a08d60df..546243e11737 100644
--- a/drivers/net/wireless/ath/ath6kl/htc_pipe.c
+++ b/drivers/net/wireless/ath/ath6kl/htc_pipe.c
@@ -228,8 +228,7 @@ static int htc_issue_packets(struct htc_target *target,
 		payload_len = packet->act_len;
 
 		/* setup HTC frame header */
-		htc_hdr = (struct htc_frame_hdr *) skb_push(skb,
-							    sizeof(*htc_hdr));
+		htc_hdr = skb_push(skb, sizeof(*htc_hdr));
 		if (!htc_hdr) {
 			WARN_ON_ONCE(1);
 			status = -EINVAL;
@@ -383,7 +382,7 @@ static enum htc_send_queue_result htc_try_send(struct htc_target *target,
 			list_for_each_entry_safe(packet, tmp_pkt,
 						 txq, list) {
 				ath6kl_dbg(ATH6KL_DBG_HTC,
-					   "%s: Indicat overflowed TX pkts: %p\n",
+					   "%s: Indicate overflowed TX pkts: %p\n",
 					   __func__, packet);
 				action = ep->ep_cb.tx_full(ep->target, packet);
 				if (action == HTC_SEND_FULL_DROP) {
@@ -1274,8 +1273,7 @@ static int ath6kl_htc_pipe_conn_service(struct htc_target *target,
 		length = sizeof(struct htc_conn_service_msg);
 
 		/* assemble connect service message */
-		conn_msg = (struct htc_conn_service_msg *) skb_put(skb,
-								   length);
+		conn_msg = skb_put(skb, length);
 		if (conn_msg == NULL) {
 			WARN_ON_ONCE(1);
 			status = -EINVAL;
@@ -1504,8 +1502,7 @@ static int ath6kl_htc_pipe_start(struct htc_target *target)
 	skb = packet->skb;
 
 	/* assemble setup complete message */
-	setup = (struct htc_setup_comp_ext_msg *) skb_put(skb,
-							  sizeof(*setup));
+	setup = skb_put(skb, sizeof(*setup));
 	memset(setup, 0, sizeof(struct htc_setup_comp_ext_msg));
 	setup->msg_id = cpu_to_le16(HTC_MSG_SETUP_COMPLETE_EX_ID);
 
diff --git a/drivers/net/wireless/ath/ath6kl/main.c b/drivers/net/wireless/ath/ath6kl/main.c
index 91ee542de3d7..b90c77ef792e 100644
--- a/drivers/net/wireless/ath/ath6kl/main.c
+++ b/drivers/net/wireless/ath/ath6kl/main.c
@@ -1287,7 +1287,7 @@ void init_netdev(struct net_device *dev)
 	struct ath6kl *ar = ath6kl_priv(dev);
 
 	dev->netdev_ops = &ath6kl_netdev_ops;
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 	dev->watchdog_timeo = ATH6KL_TX_TIMEOUT;
 
 	dev->needed_headroom = ETH_HLEN;
diff --git a/drivers/net/wireless/ath/ath6kl/txrx.c b/drivers/net/wireless/ath/ath6kl/txrx.c
index a531e0c5c1e2..e6b2517e6334 100644
--- a/drivers/net/wireless/ath/ath6kl/txrx.c
+++ b/drivers/net/wireless/ath/ath6kl/txrx.c
@@ -399,15 +399,10 @@ int ath6kl_data_tx(struct sk_buff *skb, struct net_device *dev)
 			csum_dest = skb->csum_offset + csum_start;
 		}
 
-		if (skb_headroom(skb) < dev->needed_headroom) {
-			struct sk_buff *tmp_skb = skb;
-
-			skb = skb_realloc_headroom(skb, dev->needed_headroom);
-			kfree_skb(tmp_skb);
-			if (skb == NULL) {
-				dev->stats.tx_dropped++;
-				return 0;
-			}
+		if (skb_cow_head(skb, dev->needed_headroom)) {
+			dev->stats.tx_dropped++;
+			kfree_skb(skb);
+			return 0;
 		}
 
 		if (ath6kl_wmi_dix_2_dot3(ar->wmi, skb)) {
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.c b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
index ae3043559b6d..fe5102ca5010 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
@@ -1821,8 +1821,6 @@ static void ar9003_hw_spectral_scan_wait(struct ath_hw *ah)
 static void ar9003_hw_tx99_start(struct ath_hw *ah, u32 qnum)
 {
 	REG_SET_BIT(ah, AR_PHY_TEST, PHY_AGC_CLR);
-	REG_SET_BIT(ah, 0x9864, 0x7f000);
-	REG_SET_BIT(ah, 0x9924, 0x7f00fe);
 	REG_CLR_BIT(ah, AR_DIAG_SW, AR_DIAG_RX_DIS);
 	REG_WRITE(ah, AR_CR, AR_CR_RXD);
 	REG_WRITE(ah, AR_DLCL_IFS(qnum), 0);
diff --git a/drivers/net/wireless/ath/ath9k/channel.c b/drivers/net/wireless/ath/ath9k/channel.c
index b84539d89f1a..f0439f2d566b 100644
--- a/drivers/net/wireless/ath/ath9k/channel.c
+++ b/drivers/net/wireless/ath/ath9k/channel.c
@@ -1005,7 +1005,7 @@ static void ath_scan_send_probe(struct ath_softc *sc,
 		info->flags |= IEEE80211_TX_CTL_NO_CCK_RATE;
 
 	if (req->ie_len)
-		memcpy(skb_put(skb, req->ie_len), req->ie, req->ie_len);
+		skb_put_data(skb, req->ie, req->ie_len);
 
 	skb_set_queue_mapping(skb, IEEE80211_AC_VO);
 
@@ -1521,13 +1521,11 @@ void ath9k_beacon_add_noa(struct ath_softc *sc, struct ath_vif *avp,
 	noa_desc = !!avp->offchannel_duration + !!avp->noa_duration;
 	noa_len = 2 + sizeof(struct ieee80211_p2p_noa_desc) * noa_desc;
 
-	hdr = skb_put(skb, sizeof(noa_ie_hdr));
-	memcpy(hdr, noa_ie_hdr, sizeof(noa_ie_hdr));
+	hdr = skb_put_data(skb, noa_ie_hdr, sizeof(noa_ie_hdr));
 	hdr[1] = sizeof(noa_ie_hdr) + noa_len - 2;
 	hdr[7] = noa_len;
 
-	noa = (void *) skb_put(skb, noa_len);
-	memset(noa, 0, noa_len);
+	noa = skb_put_zero(skb, noa_len);
 
 	noa->index = avp->noa_index;
 	noa->oppps_ctwindow = ath9k_get_ctwin(sc, avp);
diff --git a/drivers/net/wireless/ath/ath9k/common.c b/drivers/net/wireless/ath/ath9k/common.c
index c67d0e08bd4c..099f3d45c594 100644
--- a/drivers/net/wireless/ath/ath9k/common.c
+++ b/drivers/net/wireless/ath/ath9k/common.c
@@ -369,7 +369,7 @@ void ath9k_cmn_update_txpow(struct ath_hw *ah, u16 cur_txpow,
 {
 	struct ath_regulatory *reg = ath9k_hw_regulatory(ah);
 
-	if (reg->power_limit != new_txpow)
+	if (ah->curchan && reg->power_limit != new_txpow)
 		ath9k_hw_set_txpowerlimit(ah, new_txpow, false);
 
 	/* read back in case value is clamped */
diff --git a/drivers/net/wireless/ath/ath9k/eeprom.c b/drivers/net/wireless/ath/ath9k/eeprom.c
index 6ccf24814514..6fbd5559c0c0 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom.c
@@ -143,7 +143,7 @@ bool ath9k_hw_nvram_read(struct ath_hw *ah, u32 off, u16 *data)
 
 	if (ah->eeprom_blob)
 		ret = ath9k_hw_nvram_read_firmware(ah->eeprom_blob, off, data);
-	else if (pdata && !pdata->use_eeprom && pdata->eeprom_data)
+	else if (pdata && !pdata->use_eeprom)
 		ret = ath9k_hw_nvram_read_pdata(pdata, off, data);
 	else
 		ret = common->bus_ops->eeprom_read(common, off, data);
diff --git a/drivers/net/wireless/ath/ath9k/hif_usb.c b/drivers/net/wireless/ath/ath9k/hif_usb.c
index 12aa8abbcba4..0d9687a2aa98 100644
--- a/drivers/net/wireless/ath/ath9k/hif_usb.c
+++ b/drivers/net/wireless/ath/ath9k/hif_usb.c
@@ -199,7 +199,7 @@ static int hif_usb_send_mgmt(struct hif_device_usb *hif_dev,
 	cmd->skb = skb;
 	cmd->hif_dev = hif_dev;
 
-	hdr = (__le16 *) skb_push(skb, 4);
+	hdr = skb_push(skb, 4);
 	*hdr++ = cpu_to_le16(skb->len - 4);
 	*hdr++ = cpu_to_le16(ATH_USB_TX_STREAM_MODE_TAG);
 
diff --git a/drivers/net/wireless/ath/ath9k/htc_hst.c b/drivers/net/wireless/ath/ath9k/htc_hst.c
index 8e6dae23669b..1bf63a4efb4c 100644
--- a/drivers/net/wireless/ath/ath9k/htc_hst.c
+++ b/drivers/net/wireless/ath/ath9k/htc_hst.c
@@ -26,8 +26,7 @@ static int htc_issue_send(struct htc_target *target, struct sk_buff* skb,
 	struct htc_endpoint *endpoint = &target->endpoint[epid];
 	int status;
 
-	hdr = (struct htc_frame_hdr *)
-		skb_push(skb, sizeof(struct htc_frame_hdr));
+	hdr = skb_push(skb, sizeof(struct htc_frame_hdr));
 	hdr->endpoint_id = epid;
 	hdr->flags = flags;
 	hdr->payload_len = cpu_to_be16(len);
@@ -156,8 +155,7 @@ static int htc_config_pipe_credits(struct htc_target *target)
 	}
 	skb_reserve(skb, sizeof(struct htc_frame_hdr));
 
-	cp_msg = (struct htc_config_pipe_msg *)
-		skb_put(skb, sizeof(struct htc_config_pipe_msg));
+	cp_msg = skb_put(skb, sizeof(struct htc_config_pipe_msg));
 
 	cp_msg->message_id = cpu_to_be16(HTC_MSG_CONFIG_PIPE_ID);
 	cp_msg->pipe_id = USB_WLAN_TX_PIPE;
@@ -195,8 +193,7 @@ static int htc_setup_complete(struct htc_target *target)
 	}
 	skb_reserve(skb, sizeof(struct htc_frame_hdr));
 
-	comp_msg = (struct htc_comp_msg *)
-		skb_put(skb, sizeof(struct htc_comp_msg));
+	comp_msg = skb_put(skb, sizeof(struct htc_comp_msg));
 	comp_msg->msg_id = cpu_to_be16(HTC_MSG_SETUP_COMPLETE_ID);
 
 	target->htc_flags |= HTC_OP_START_WAIT;
@@ -265,8 +262,7 @@ int htc_connect_service(struct htc_target *target,
 
 	skb_reserve(skb, sizeof(struct htc_frame_hdr));
 
-	conn_msg = (struct htc_conn_svc_msg *)
-			skb_put(skb, sizeof(struct htc_conn_svc_msg));
+	conn_msg = skb_put(skb, sizeof(struct htc_conn_svc_msg));
 	conn_msg->service_id = cpu_to_be16(service_connreq->service_id);
 	conn_msg->msg_id = cpu_to_be16(HTC_MSG_CONNECT_SERVICE_ID);
 	conn_msg->con_flags = cpu_to_be16(service_connreq->con_flags);
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index 9e65d14e7b1e..8b4ac7f0a09b 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -731,12 +731,12 @@ static int ath9k_start(struct ieee80211_hw *hw)
 
 	spin_unlock_bh(&sc->sc_pcu_lock);
 
+	ath9k_rng_start(sc);
+
 	mutex_unlock(&sc->mutex);
 
 	ath9k_ps_restore(sc);
 
-	ath9k_rng_start(sc);
-
 	return 0;
 }
 
@@ -826,10 +826,10 @@ static void ath9k_stop(struct ieee80211_hw *hw)
 
 	ath9k_deinit_channel_context(sc);
 
-	ath9k_rng_stop(sc);
-
 	mutex_lock(&sc->mutex);
 
+	ath9k_rng_stop(sc);
+
 	ath_cancel_work(sc);
 
 	if (test_bit(ATH_OP_INVALID, &common->op_flags)) {
diff --git a/drivers/net/wireless/ath/ath9k/mci.c b/drivers/net/wireless/ath/ath9k/mci.c
index 66596b95273f..cf23fd815211 100644
--- a/drivers/net/wireless/ath/ath9k/mci.c
+++ b/drivers/net/wireless/ath/ath9k/mci.c
@@ -548,7 +548,7 @@ void ath_mci_intr(struct ath_softc *sc)
 
 	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);
+		ar9003_mci_state(ah, MCI_STATE_LAST_SCHD_MSG_OFFSET);
 	}
 
 	if (mci_int_rxmsg & AR_MCI_INTERRUPT_RX_MSG_GPM) {
diff --git a/drivers/net/wireless/ath/ath9k/rng.c b/drivers/net/wireless/ath/ath9k/rng.c
index 568b1c6c2b2b..f9d3d6eedd3c 100644
--- a/drivers/net/wireless/ath/ath9k/rng.c
+++ b/drivers/net/wireless/ath/ath9k/rng.c
@@ -24,6 +24,8 @@
 #define ATH9K_RNG_BUF_SIZE	320
 #define ATH9K_RNG_ENTROPY(x)	(((x) * 8 * 10) >> 5) /* quality: 10/32 */
 
+static DECLARE_WAIT_QUEUE_HEAD(rng_queue);
+
 static int ath9k_rng_data_read(struct ath_softc *sc, u32 *buf, u32 buf_size)
 {
 	int i, j;
@@ -85,7 +87,9 @@ static int ath9k_rng_kthread(void *data)
 						 ATH9K_RNG_BUF_SIZE);
 		if (unlikely(!bytes_read)) {
 			delay = ath9k_rng_delay_get(++fail_stats);
-			msleep_interruptible(delay);
+			wait_event_interruptible_timeout(rng_queue,
+							 kthread_should_stop(),
+							 msecs_to_jiffies(delay));
 			continue;
 		}
 
@@ -120,6 +124,8 @@ void ath9k_rng_start(struct ath_softc *sc)
 
 void ath9k_rng_stop(struct ath_softc *sc)
 {
-	if (sc->rng_task)
+	if (sc->rng_task) {
 		kthread_stop(sc->rng_task);
+		sc->rng_task = NULL;
+	}
 }
diff --git a/drivers/net/wireless/ath/ath9k/tx99.c b/drivers/net/wireless/ath/ath9k/tx99.c
index 16aca9e28b77..49ed1afb913c 100644
--- a/drivers/net/wireless/ath/ath9k/tx99.c
+++ b/drivers/net/wireless/ath/ath9k/tx99.c
@@ -153,7 +153,7 @@ static int ath9k_tx99_init(struct ath_softc *sc)
 		sc->tx99_power,
 		sc->tx99_power / 2);
 
-	/* We leave the harware awake as it will be chugging on */
+	/* We leave the hardware awake as it will be chugging on */
 
 	return 0;
 }
@@ -189,22 +189,27 @@ static ssize_t write_file_tx99(struct file *file, const char __user *user_buf,
 	if (strtobool(buf, &start))
 		return -EINVAL;
 
+	mutex_lock(&sc->mutex);
+
 	if (start == sc->tx99_state) {
 		if (!start)
-			return count;
+			goto out;
 		ath_dbg(common, XMIT, "Resetting TX99\n");
 		ath9k_tx99_deinit(sc);
 	}
 
 	if (!start) {
 		ath9k_tx99_deinit(sc);
-		return count;
+		goto out;
 	}
 
 	r = ath9k_tx99_init(sc);
-	if (r)
+	if (r) {
+		mutex_unlock(&sc->mutex);
 		return r;
-
+	}
+out:
+	mutex_unlock(&sc->mutex);
 	return count;
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/wmi.c b/drivers/net/wireless/ath/ath9k/wmi.c
index 9c16e2a6d185..64a354fa78ab 100644
--- a/drivers/net/wireless/ath/ath9k/wmi.c
+++ b/drivers/net/wireless/ath/ath9k/wmi.c
@@ -277,7 +277,7 @@ static int ath9k_wmi_cmd_issue(struct wmi *wmi,
 	struct wmi_cmd_hdr *hdr;
 	unsigned long flags;
 
-	hdr = (struct wmi_cmd_hdr *) skb_push(skb, sizeof(struct wmi_cmd_hdr));
+	hdr = skb_push(skb, sizeof(struct wmi_cmd_hdr));
 	hdr->command_id = cpu_to_be16(cmd);
 	hdr->seq_no = cpu_to_be16(++wmi->tx_seq_id);
 
@@ -298,7 +298,6 @@ int ath9k_wmi_cmd(struct wmi *wmi, enum wmi_cmd_id cmd_id,
 	u16 headroom = sizeof(struct htc_frame_hdr) +
 		       sizeof(struct wmi_cmd_hdr);
 	struct sk_buff *skb;
-	u8 *data;
 	unsigned long time_left;
 	int ret = 0;
 
@@ -312,8 +311,7 @@ int ath9k_wmi_cmd(struct wmi *wmi, enum wmi_cmd_id cmd_id,
 	skb_reserve(skb, headroom);
 
 	if (cmd_len != 0 && cmd_buf != NULL) {
-		data = (u8 *) skb_put(skb, cmd_len);
-		memcpy(data, cmd_buf, cmd_len);
+		skb_put_data(skb, cmd_buf, cmd_len);
 	}
 
 	mutex_lock(&wmi->op_mutex);
diff --git a/drivers/net/wireless/ath/carl9170/rx.c b/drivers/net/wireless/ath/carl9170/rx.c
index b2166726b05d..705063259c8f 100644
--- a/drivers/net/wireless/ath/carl9170/rx.c
+++ b/drivers/net/wireless/ath/carl9170/rx.c
@@ -481,7 +481,7 @@ static struct sk_buff *carl9170_rx_copy_data(u8 *buf, int len)
 	skb = dev_alloc_skb(len + reserved);
 	if (likely(skb)) {
 		skb_reserve(skb, reserved);
-		memcpy(skb_put(skb, len), buf, len);
+		skb_put_data(skb, buf, len);
 	}
 
 	return skb;
@@ -916,7 +916,7 @@ static void carl9170_rx_stream(struct ar9170 *ar, void *buf, unsigned int len)
 				}
 			}
 
-			memcpy(skb_put(ar->rx_failover, tlen), tbuf, tlen);
+			skb_put_data(ar->rx_failover, tbuf, tlen);
 			ar->rx_failover_missing -= tlen;
 
 			if (ar->rx_failover_missing <= 0) {
@@ -958,7 +958,7 @@ static void carl9170_rx_stream(struct ar9170 *ar, void *buf, unsigned int len)
 			 * the rx - descriptor comes round again.
 			 */
 
-			memcpy(skb_put(ar->rx_failover, tlen), tbuf, tlen);
+			skb_put_data(ar->rx_failover, tbuf, tlen);
 			ar->rx_failover_missing = clen - tlen;
 			return;
 		}
diff --git a/drivers/net/wireless/ath/carl9170/tx.c b/drivers/net/wireless/ath/carl9170/tx.c
index 2bf04c9edc98..0cb5b58925dc 100644
--- a/drivers/net/wireless/ath/carl9170/tx.c
+++ b/drivers/net/wireless/ath/carl9170/tx.c
@@ -991,7 +991,7 @@ static int carl9170_tx_prepare(struct ar9170 *ar,
 	else
 		cvif = NULL;
 
-	txc = (void *)skb_push(skb, sizeof(*txc));
+	txc = skb_push(skb, sizeof(*txc));
 	memset(txc, 0, sizeof(*txc));
 
 	SET_VAL(CARL9170_TX_SUPER_MISC_QUEUE, txc->s.misc, hw_queue);
diff --git a/drivers/net/wireless/ath/wil6210/Makefile b/drivers/net/wireless/ath/wil6210/Makefile
index 89bf2f9eca1d..4ae21da78e9e 100644
--- a/drivers/net/wireless/ath/wil6210/Makefile
+++ b/drivers/net/wireless/ath/wil6210/Makefile
@@ -10,7 +10,6 @@ wil6210-y += interrupt.o
 wil6210-y += txrx.o
 wil6210-y += debug.o
 wil6210-y += rx_reorder.o
-wil6210-y += ioctl.o
 wil6210-y += fw.o
 wil6210-y += pm.o
 wil6210-y += pmc.o
diff --git a/drivers/net/wireless/ath/wil6210/cfg80211.c b/drivers/net/wireless/ath/wil6210/cfg80211.c
index fdaa99c541ac..0b5383a62d42 100644
--- a/drivers/net/wireless/ath/wil6210/cfg80211.c
+++ b/drivers/net/wireless/ath/wil6210/cfg80211.c
@@ -16,6 +16,7 @@
 
 #include <linux/etherdevice.h>
 #include <linux/moduleparam.h>
+#include <net/netlink.h>
 #include "wil6210.h"
 #include "wmi.h"
 
@@ -41,6 +42,126 @@ static struct ieee80211_channel wil_60ghz_channels[] = {
 /* channel 4 not supported yet */
 };
 
+/* Vendor id to be used in vendor specific command and events
+ * to user space.
+ * NOTE: The authoritative place for definition of QCA_NL80211_VENDOR_ID,
+ * vendor subcmd definitions prefixed with QCA_NL80211_VENDOR_SUBCMD, and
+ * qca_wlan_vendor_attr is open source file src/common/qca-vendor.h in
+ * git://w1.fi/srv/git/hostap.git; the values here are just a copy of that
+ */
+
+#define QCA_NL80211_VENDOR_ID	0x001374
+
+#define WIL_MAX_RF_SECTORS (128)
+#define WIL_CID_ALL (0xff)
+
+enum qca_wlan_vendor_attr_rf_sector {
+	QCA_ATTR_MAC_ADDR = 6,
+	QCA_ATTR_PAD = 13,
+	QCA_ATTR_TSF = 29,
+	QCA_ATTR_DMG_RF_SECTOR_INDEX = 30,
+	QCA_ATTR_DMG_RF_SECTOR_TYPE = 31,
+	QCA_ATTR_DMG_RF_MODULE_MASK = 32,
+	QCA_ATTR_DMG_RF_SECTOR_CFG = 33,
+	QCA_ATTR_DMG_RF_SECTOR_MAX,
+};
+
+enum qca_wlan_vendor_attr_dmg_rf_sector_type {
+	QCA_ATTR_DMG_RF_SECTOR_TYPE_RX,
+	QCA_ATTR_DMG_RF_SECTOR_TYPE_TX,
+	QCA_ATTR_DMG_RF_SECTOR_TYPE_MAX
+};
+
+enum qca_wlan_vendor_attr_dmg_rf_sector_cfg {
+	QCA_ATTR_DMG_RF_SECTOR_CFG_INVALID = 0,
+	QCA_ATTR_DMG_RF_SECTOR_CFG_MODULE_INDEX,
+	QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE0,
+	QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE1,
+	QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE2,
+	QCA_ATTR_DMG_RF_SECTOR_CFG_PSH_HI,
+	QCA_ATTR_DMG_RF_SECTOR_CFG_PSH_LO,
+	QCA_ATTR_DMG_RF_SECTOR_CFG_DTYPE_X16,
+
+	/* keep last */
+	QCA_ATTR_DMG_RF_SECTOR_CFG_AFTER_LAST,
+	QCA_ATTR_DMG_RF_SECTOR_CFG_MAX =
+	QCA_ATTR_DMG_RF_SECTOR_CFG_AFTER_LAST - 1
+};
+
+static const struct
+nla_policy wil_rf_sector_policy[QCA_ATTR_DMG_RF_SECTOR_MAX + 1] = {
+	[QCA_ATTR_MAC_ADDR] = { .len = ETH_ALEN },
+	[QCA_ATTR_DMG_RF_SECTOR_INDEX] = { .type = NLA_U16 },
+	[QCA_ATTR_DMG_RF_SECTOR_TYPE] = { .type = NLA_U8 },
+	[QCA_ATTR_DMG_RF_MODULE_MASK] = { .type = NLA_U32 },
+	[QCA_ATTR_DMG_RF_SECTOR_CFG] = { .type = NLA_NESTED },
+};
+
+static const struct
+nla_policy wil_rf_sector_cfg_policy[QCA_ATTR_DMG_RF_SECTOR_CFG_MAX + 1] = {
+	[QCA_ATTR_DMG_RF_SECTOR_CFG_MODULE_INDEX] = { .type = NLA_U8 },
+	[QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE0] = { .type = NLA_U32 },
+	[QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE1] = { .type = NLA_U32 },
+	[QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE2] = { .type = NLA_U32 },
+	[QCA_ATTR_DMG_RF_SECTOR_CFG_PSH_HI] = { .type = NLA_U32 },
+	[QCA_ATTR_DMG_RF_SECTOR_CFG_PSH_LO] = { .type = NLA_U32 },
+	[QCA_ATTR_DMG_RF_SECTOR_CFG_DTYPE_X16] = { .type = NLA_U32 },
+};
+
+enum qca_nl80211_vendor_subcmds {
+	QCA_NL80211_VENDOR_SUBCMD_DMG_RF_GET_SECTOR_CFG = 139,
+	QCA_NL80211_VENDOR_SUBCMD_DMG_RF_SET_SECTOR_CFG = 140,
+	QCA_NL80211_VENDOR_SUBCMD_DMG_RF_GET_SELECTED_SECTOR = 141,
+	QCA_NL80211_VENDOR_SUBCMD_DMG_RF_SET_SELECTED_SECTOR = 142,
+};
+
+static int wil_rf_sector_get_cfg(struct wiphy *wiphy,
+				 struct wireless_dev *wdev,
+				 const void *data, int data_len);
+static int wil_rf_sector_set_cfg(struct wiphy *wiphy,
+				 struct wireless_dev *wdev,
+				 const void *data, int data_len);
+static int wil_rf_sector_get_selected(struct wiphy *wiphy,
+				      struct wireless_dev *wdev,
+				      const void *data, int data_len);
+static int wil_rf_sector_set_selected(struct wiphy *wiphy,
+				      struct wireless_dev *wdev,
+				      const void *data, int data_len);
+
+/* vendor specific commands */
+static const struct wiphy_vendor_command wil_nl80211_vendor_commands[] = {
+	{
+		.info.vendor_id = QCA_NL80211_VENDOR_ID,
+		.info.subcmd = QCA_NL80211_VENDOR_SUBCMD_DMG_RF_GET_SECTOR_CFG,
+		.flags = WIPHY_VENDOR_CMD_NEED_WDEV |
+			 WIPHY_VENDOR_CMD_NEED_RUNNING,
+		.doit = wil_rf_sector_get_cfg
+	},
+	{
+		.info.vendor_id = QCA_NL80211_VENDOR_ID,
+		.info.subcmd = QCA_NL80211_VENDOR_SUBCMD_DMG_RF_SET_SECTOR_CFG,
+		.flags = WIPHY_VENDOR_CMD_NEED_WDEV |
+			 WIPHY_VENDOR_CMD_NEED_RUNNING,
+		.doit = wil_rf_sector_set_cfg
+	},
+	{
+		.info.vendor_id = QCA_NL80211_VENDOR_ID,
+		.info.subcmd =
+			QCA_NL80211_VENDOR_SUBCMD_DMG_RF_GET_SELECTED_SECTOR,
+		.flags = WIPHY_VENDOR_CMD_NEED_WDEV |
+			 WIPHY_VENDOR_CMD_NEED_RUNNING,
+		.doit = wil_rf_sector_get_selected
+	},
+	{
+		.info.vendor_id = QCA_NL80211_VENDOR_ID,
+		.info.subcmd =
+			QCA_NL80211_VENDOR_SUBCMD_DMG_RF_SET_SELECTED_SECTOR,
+		.flags = WIPHY_VENDOR_CMD_NEED_WDEV |
+			 WIPHY_VENDOR_CMD_NEED_RUNNING,
+		.doit = wil_rf_sector_set_selected
+	},
+};
+
 static struct ieee80211_supported_band wil_band_60ghz = {
 	.channels = wil_60ghz_channels,
 	.n_channels = ARRAY_SIZE(wil_60ghz_channels),
@@ -1325,6 +1446,8 @@ static int wil_cfg80211_stop_ap(struct wiphy *wiphy,
 	wil6210_bus_request(wil, WIL_DEFAULT_BUS_REQUEST_KBPS);
 	wil_set_recovery_state(wil, fw_recovery_idle);
 
+	set_bit(wil_status_resetting, wil->status);
+
 	mutex_lock(&wil->mutex);
 
 	wmi_pcp_stop(wil);
@@ -1571,6 +1694,42 @@ static int wil_cfg80211_set_power_mgmt(struct wiphy *wiphy,
 	return wil_ps_update(wil, ps_profile);
 }
 
+static int wil_cfg80211_suspend(struct wiphy *wiphy,
+				struct cfg80211_wowlan *wow)
+{
+	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
+	int rc;
+
+	/* Setting the wakeup trigger based on wow is TBD */
+
+	if (test_bit(wil_status_suspended, wil->status)) {
+		wil_dbg_pm(wil, "trying to suspend while suspended\n");
+		return 0;
+	}
+
+	rc = wil_can_suspend(wil, false);
+	if (rc)
+		goto out;
+
+	wil_dbg_pm(wil, "suspending\n");
+
+	wil_p2p_stop_discovery(wil);
+
+	wil_abort_scan(wil, true);
+
+out:
+	return rc;
+}
+
+static int wil_cfg80211_resume(struct wiphy *wiphy)
+{
+	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
+
+	wil_dbg_pm(wil, "resuming\n");
+
+	return 0;
+}
+
 static const struct cfg80211_ops wil_cfg80211_ops = {
 	.add_virtual_intf = wil_cfg80211_add_iface,
 	.del_virtual_intf = wil_cfg80211_del_iface,
@@ -1602,6 +1761,8 @@ static const struct cfg80211_ops wil_cfg80211_ops = {
 	.start_p2p_device = wil_cfg80211_start_p2p_device,
 	.stop_p2p_device = wil_cfg80211_stop_p2p_device,
 	.set_power_mgmt = wil_cfg80211_set_power_mgmt,
+	.suspend = wil_cfg80211_suspend,
+	.resume = wil_cfg80211_resume,
 };
 
 static void wil_wiphy_init(struct wiphy *wiphy)
@@ -1637,6 +1798,9 @@ static void wil_wiphy_init(struct wiphy *wiphy)
 	wiphy->n_cipher_suites = ARRAY_SIZE(wil_cipher_suites);
 	wiphy->mgmt_stypes = wil_mgmt_stypes;
 	wiphy->features |= NL80211_FEATURE_SK_TX_STATUS;
+
+	wiphy->n_vendor_commands = ARRAY_SIZE(wil_nl80211_vendor_commands);
+	wiphy->vendor_commands = wil_nl80211_vendor_commands;
 }
 
 struct wireless_dev *wil_cfg80211_init(struct device *dev)
@@ -1695,3 +1859,452 @@ void wil_p2p_wdev_free(struct wil6210_priv *wil)
 		kfree(p2p_wdev);
 	}
 }
+
+static int wil_rf_sector_status_to_rc(u8 status)
+{
+	switch (status) {
+	case WMI_RF_SECTOR_STATUS_SUCCESS:
+		return 0;
+	case WMI_RF_SECTOR_STATUS_BAD_PARAMETERS_ERROR:
+		return -EINVAL;
+	case WMI_RF_SECTOR_STATUS_BUSY_ERROR:
+		return -EAGAIN;
+	case WMI_RF_SECTOR_STATUS_NOT_SUPPORTED_ERROR:
+		return -EOPNOTSUPP;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int wil_rf_sector_get_cfg(struct wiphy *wiphy,
+				 struct wireless_dev *wdev,
+				 const void *data, int data_len)
+{
+	struct wil6210_priv *wil = wdev_to_wil(wdev);
+	int rc;
+	struct nlattr *tb[QCA_ATTR_DMG_RF_SECTOR_MAX + 1];
+	u16 sector_index;
+	u8 sector_type;
+	u32 rf_modules_vec;
+	struct wmi_get_rf_sector_params_cmd cmd;
+	struct {
+		struct wmi_cmd_hdr wmi;
+		struct wmi_get_rf_sector_params_done_event evt;
+	} __packed reply;
+	struct sk_buff *msg;
+	struct nlattr *nl_cfgs, *nl_cfg;
+	u32 i;
+	struct wmi_rf_sector_info *si;
+
+	if (!test_bit(WMI_FW_CAPABILITY_RF_SECTORS, wil->fw_capabilities))
+		return -EOPNOTSUPP;
+
+	rc = nla_parse(tb, QCA_ATTR_DMG_RF_SECTOR_MAX, data, data_len,
+		       wil_rf_sector_policy, NULL);
+	if (rc) {
+		wil_err(wil, "Invalid rf sector ATTR\n");
+		return rc;
+	}
+
+	if (!tb[QCA_ATTR_DMG_RF_SECTOR_INDEX] ||
+	    !tb[QCA_ATTR_DMG_RF_SECTOR_TYPE] ||
+	    !tb[QCA_ATTR_DMG_RF_MODULE_MASK]) {
+		wil_err(wil, "Invalid rf sector spec\n");
+		return -EINVAL;
+	}
+
+	sector_index = nla_get_u16(
+		tb[QCA_ATTR_DMG_RF_SECTOR_INDEX]);
+	if (sector_index >= WIL_MAX_RF_SECTORS) {
+		wil_err(wil, "Invalid sector index %d\n", sector_index);
+		return -EINVAL;
+	}
+
+	sector_type = nla_get_u8(tb[QCA_ATTR_DMG_RF_SECTOR_TYPE]);
+	if (sector_type >= QCA_ATTR_DMG_RF_SECTOR_TYPE_MAX) {
+		wil_err(wil, "Invalid sector type %d\n", sector_type);
+		return -EINVAL;
+	}
+
+	rf_modules_vec = nla_get_u32(
+		tb[QCA_ATTR_DMG_RF_MODULE_MASK]);
+	if (rf_modules_vec >= BIT(WMI_MAX_RF_MODULES_NUM)) {
+		wil_err(wil, "Invalid rf module mask 0x%x\n", rf_modules_vec);
+		return -EINVAL;
+	}
+
+	cmd.sector_idx = cpu_to_le16(sector_index);
+	cmd.sector_type = sector_type;
+	cmd.rf_modules_vec = rf_modules_vec & 0xFF;
+	memset(&reply, 0, sizeof(reply));
+	rc = wmi_call(wil, WMI_GET_RF_SECTOR_PARAMS_CMDID, &cmd, sizeof(cmd),
+		      WMI_GET_RF_SECTOR_PARAMS_DONE_EVENTID,
+		      &reply, sizeof(reply),
+		      500);
+	if (rc)
+		return rc;
+	if (reply.evt.status) {
+		wil_err(wil, "get rf sector cfg failed with status %d\n",
+			reply.evt.status);
+		return wil_rf_sector_status_to_rc(reply.evt.status);
+	}
+
+	msg = cfg80211_vendor_cmd_alloc_reply_skb(
+		wiphy, 64 * WMI_MAX_RF_MODULES_NUM);
+	if (!msg)
+		return -ENOMEM;
+
+	if (nla_put_u64_64bit(msg, QCA_ATTR_TSF,
+			      le64_to_cpu(reply.evt.tsf),
+			      QCA_ATTR_PAD))
+		goto nla_put_failure;
+
+	nl_cfgs = nla_nest_start(msg, QCA_ATTR_DMG_RF_SECTOR_CFG);
+	if (!nl_cfgs)
+		goto nla_put_failure;
+	for (i = 0; i < WMI_MAX_RF_MODULES_NUM; i++) {
+		if (!(rf_modules_vec & BIT(i)))
+			continue;
+		nl_cfg = nla_nest_start(msg, i);
+		if (!nl_cfg)
+			goto nla_put_failure;
+		si = &reply.evt.sectors_info[i];
+		if (nla_put_u8(msg, QCA_ATTR_DMG_RF_SECTOR_CFG_MODULE_INDEX,
+			       i) ||
+		    nla_put_u32(msg, QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE0,
+				le32_to_cpu(si->etype0)) ||
+		    nla_put_u32(msg, QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE1,
+				le32_to_cpu(si->etype1)) ||
+		    nla_put_u32(msg, QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE2,
+				le32_to_cpu(si->etype2)) ||
+		    nla_put_u32(msg, QCA_ATTR_DMG_RF_SECTOR_CFG_PSH_HI,
+				le32_to_cpu(si->psh_hi)) ||
+		    nla_put_u32(msg, QCA_ATTR_DMG_RF_SECTOR_CFG_PSH_LO,
+				le32_to_cpu(si->psh_lo)) ||
+		    nla_put_u32(msg, QCA_ATTR_DMG_RF_SECTOR_CFG_DTYPE_X16,
+				le32_to_cpu(si->dtype_swch_off)))
+			goto nla_put_failure;
+		nla_nest_end(msg, nl_cfg);
+	}
+
+	nla_nest_end(msg, nl_cfgs);
+	rc = cfg80211_vendor_cmd_reply(msg);
+	return rc;
+nla_put_failure:
+	kfree_skb(msg);
+	return -ENOBUFS;
+}
+
+static int wil_rf_sector_set_cfg(struct wiphy *wiphy,
+				 struct wireless_dev *wdev,
+				 const void *data, int data_len)
+{
+	struct wil6210_priv *wil = wdev_to_wil(wdev);
+	int rc, tmp;
+	struct nlattr *tb[QCA_ATTR_DMG_RF_SECTOR_MAX + 1];
+	struct nlattr *tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_MAX + 1];
+	u16 sector_index, rf_module_index;
+	u8 sector_type;
+	u32 rf_modules_vec = 0;
+	struct wmi_set_rf_sector_params_cmd cmd;
+	struct {
+		struct wmi_cmd_hdr wmi;
+		struct wmi_set_rf_sector_params_done_event evt;
+	} __packed reply;
+	struct nlattr *nl_cfg;
+	struct wmi_rf_sector_info *si;
+
+	if (!test_bit(WMI_FW_CAPABILITY_RF_SECTORS, wil->fw_capabilities))
+		return -EOPNOTSUPP;
+
+	rc = nla_parse(tb, QCA_ATTR_DMG_RF_SECTOR_MAX, data, data_len,
+		       wil_rf_sector_policy, NULL);
+	if (rc) {
+		wil_err(wil, "Invalid rf sector ATTR\n");
+		return rc;
+	}
+
+	if (!tb[QCA_ATTR_DMG_RF_SECTOR_INDEX] ||
+	    !tb[QCA_ATTR_DMG_RF_SECTOR_TYPE] ||
+	    !tb[QCA_ATTR_DMG_RF_SECTOR_CFG]) {
+		wil_err(wil, "Invalid rf sector spec\n");
+		return -EINVAL;
+	}
+
+	sector_index = nla_get_u16(
+		tb[QCA_ATTR_DMG_RF_SECTOR_INDEX]);
+	if (sector_index >= WIL_MAX_RF_SECTORS) {
+		wil_err(wil, "Invalid sector index %d\n", sector_index);
+		return -EINVAL;
+	}
+
+	sector_type = nla_get_u8(tb[QCA_ATTR_DMG_RF_SECTOR_TYPE]);
+	if (sector_type >= QCA_ATTR_DMG_RF_SECTOR_TYPE_MAX) {
+		wil_err(wil, "Invalid sector type %d\n", sector_type);
+		return -EINVAL;
+	}
+
+	memset(&cmd, 0, sizeof(cmd));
+
+	cmd.sector_idx = cpu_to_le16(sector_index);
+	cmd.sector_type = sector_type;
+	nla_for_each_nested(nl_cfg, tb[QCA_ATTR_DMG_RF_SECTOR_CFG],
+			    tmp) {
+		rc = nla_parse_nested(tb2, QCA_ATTR_DMG_RF_SECTOR_CFG_MAX,
+				      nl_cfg, wil_rf_sector_cfg_policy,
+				      NULL);
+		if (rc) {
+			wil_err(wil, "invalid sector cfg\n");
+			return -EINVAL;
+		}
+
+		if (!tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_MODULE_INDEX] ||
+		    !tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE0] ||
+		    !tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE1] ||
+		    !tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE2] ||
+		    !tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_PSH_HI] ||
+		    !tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_PSH_LO] ||
+		    !tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_DTYPE_X16]) {
+			wil_err(wil, "missing cfg params\n");
+			return -EINVAL;
+		}
+
+		rf_module_index = nla_get_u8(
+			tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_MODULE_INDEX]);
+		if (rf_module_index >= WMI_MAX_RF_MODULES_NUM) {
+			wil_err(wil, "invalid RF module index %d\n",
+				rf_module_index);
+			return -EINVAL;
+		}
+		rf_modules_vec |= BIT(rf_module_index);
+		si = &cmd.sectors_info[rf_module_index];
+		si->etype0 = cpu_to_le32(nla_get_u32(
+			tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE0]));
+		si->etype1 = cpu_to_le32(nla_get_u32(
+			tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE1]));
+		si->etype2 = cpu_to_le32(nla_get_u32(
+			tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_ETYPE2]));
+		si->psh_hi = cpu_to_le32(nla_get_u32(
+			tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_PSH_HI]));
+		si->psh_lo = cpu_to_le32(nla_get_u32(
+			tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_PSH_LO]));
+		si->dtype_swch_off = cpu_to_le32(nla_get_u32(
+			tb2[QCA_ATTR_DMG_RF_SECTOR_CFG_DTYPE_X16]));
+	}
+
+	cmd.rf_modules_vec = rf_modules_vec & 0xFF;
+	memset(&reply, 0, sizeof(reply));
+	rc = wmi_call(wil, WMI_SET_RF_SECTOR_PARAMS_CMDID, &cmd, sizeof(cmd),
+		      WMI_SET_RF_SECTOR_PARAMS_DONE_EVENTID,
+		      &reply, sizeof(reply),
+		      500);
+	if (rc)
+		return rc;
+	return wil_rf_sector_status_to_rc(reply.evt.status);
+}
+
+static int wil_rf_sector_get_selected(struct wiphy *wiphy,
+				      struct wireless_dev *wdev,
+				      const void *data, int data_len)
+{
+	struct wil6210_priv *wil = wdev_to_wil(wdev);
+	int rc;
+	struct nlattr *tb[QCA_ATTR_DMG_RF_SECTOR_MAX + 1];
+	u8 sector_type, mac_addr[ETH_ALEN];
+	int cid = 0;
+	struct wmi_get_selected_rf_sector_index_cmd cmd;
+	struct {
+		struct wmi_cmd_hdr wmi;
+		struct wmi_get_selected_rf_sector_index_done_event evt;
+	} __packed reply;
+	struct sk_buff *msg;
+
+	if (!test_bit(WMI_FW_CAPABILITY_RF_SECTORS, wil->fw_capabilities))
+		return -EOPNOTSUPP;
+
+	rc = nla_parse(tb, QCA_ATTR_DMG_RF_SECTOR_MAX, data, data_len,
+		       wil_rf_sector_policy, NULL);
+	if (rc) {
+		wil_err(wil, "Invalid rf sector ATTR\n");
+		return rc;
+	}
+
+	if (!tb[QCA_ATTR_DMG_RF_SECTOR_TYPE]) {
+		wil_err(wil, "Invalid rf sector spec\n");
+		return -EINVAL;
+	}
+	sector_type = nla_get_u8(tb[QCA_ATTR_DMG_RF_SECTOR_TYPE]);
+	if (sector_type >= QCA_ATTR_DMG_RF_SECTOR_TYPE_MAX) {
+		wil_err(wil, "Invalid sector type %d\n", sector_type);
+		return -EINVAL;
+	}
+
+	if (tb[QCA_ATTR_MAC_ADDR]) {
+		ether_addr_copy(mac_addr, nla_data(tb[QCA_ATTR_MAC_ADDR]));
+		cid = wil_find_cid(wil, mac_addr);
+		if (cid < 0) {
+			wil_err(wil, "invalid MAC address %pM\n", mac_addr);
+			return -ENOENT;
+		}
+	} else {
+		if (test_bit(wil_status_fwconnected, wil->status)) {
+			wil_err(wil, "must specify MAC address when connected\n");
+			return -EINVAL;
+		}
+	}
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.cid = (u8)cid;
+	cmd.sector_type = sector_type;
+	memset(&reply, 0, sizeof(reply));
+	rc = wmi_call(wil, WMI_GET_SELECTED_RF_SECTOR_INDEX_CMDID,
+		      &cmd, sizeof(cmd),
+		      WMI_GET_SELECTED_RF_SECTOR_INDEX_DONE_EVENTID,
+		      &reply, sizeof(reply),
+		      500);
+	if (rc)
+		return rc;
+	if (reply.evt.status) {
+		wil_err(wil, "get rf selected sector cfg failed with status %d\n",
+			reply.evt.status);
+		return wil_rf_sector_status_to_rc(reply.evt.status);
+	}
+
+	msg = cfg80211_vendor_cmd_alloc_reply_skb(
+		wiphy, 64 * WMI_MAX_RF_MODULES_NUM);
+	if (!msg)
+		return -ENOMEM;
+
+	if (nla_put_u64_64bit(msg, QCA_ATTR_TSF,
+			      le64_to_cpu(reply.evt.tsf),
+			      QCA_ATTR_PAD) ||
+	    nla_put_u16(msg, QCA_ATTR_DMG_RF_SECTOR_INDEX,
+			le16_to_cpu(reply.evt.sector_idx)))
+		goto nla_put_failure;
+
+	rc = cfg80211_vendor_cmd_reply(msg);
+	return rc;
+nla_put_failure:
+	kfree_skb(msg);
+	return -ENOBUFS;
+}
+
+static int wil_rf_sector_wmi_set_selected(struct wil6210_priv *wil,
+					  u16 sector_index,
+					  u8 sector_type, u8 cid)
+{
+	struct wmi_set_selected_rf_sector_index_cmd cmd;
+	struct {
+		struct wmi_cmd_hdr wmi;
+		struct wmi_set_selected_rf_sector_index_done_event evt;
+	} __packed reply;
+	int rc;
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.sector_idx = cpu_to_le16(sector_index);
+	cmd.sector_type = sector_type;
+	cmd.cid = (u8)cid;
+	memset(&reply, 0, sizeof(reply));
+	rc = wmi_call(wil, WMI_SET_SELECTED_RF_SECTOR_INDEX_CMDID,
+		      &cmd, sizeof(cmd),
+		      WMI_SET_SELECTED_RF_SECTOR_INDEX_DONE_EVENTID,
+		      &reply, sizeof(reply),
+		      500);
+	if (rc)
+		return rc;
+	return wil_rf_sector_status_to_rc(reply.evt.status);
+}
+
+static int wil_rf_sector_set_selected(struct wiphy *wiphy,
+				      struct wireless_dev *wdev,
+				      const void *data, int data_len)
+{
+	struct wil6210_priv *wil = wdev_to_wil(wdev);
+	int rc;
+	struct nlattr *tb[QCA_ATTR_DMG_RF_SECTOR_MAX + 1];
+	u16 sector_index;
+	u8 sector_type, mac_addr[ETH_ALEN], i;
+	int cid = 0;
+
+	if (!test_bit(WMI_FW_CAPABILITY_RF_SECTORS, wil->fw_capabilities))
+		return -EOPNOTSUPP;
+
+	rc = nla_parse(tb, QCA_ATTR_DMG_RF_SECTOR_MAX, data, data_len,
+		       wil_rf_sector_policy, NULL);
+	if (rc) {
+		wil_err(wil, "Invalid rf sector ATTR\n");
+		return rc;
+	}
+
+	if (!tb[QCA_ATTR_DMG_RF_SECTOR_INDEX] ||
+	    !tb[QCA_ATTR_DMG_RF_SECTOR_TYPE]) {
+		wil_err(wil, "Invalid rf sector spec\n");
+		return -EINVAL;
+	}
+
+	sector_index = nla_get_u16(
+		tb[QCA_ATTR_DMG_RF_SECTOR_INDEX]);
+	if (sector_index >= WIL_MAX_RF_SECTORS &&
+	    sector_index != WMI_INVALID_RF_SECTOR_INDEX) {
+		wil_err(wil, "Invalid sector index %d\n", sector_index);
+		return -EINVAL;
+	}
+
+	sector_type = nla_get_u8(tb[QCA_ATTR_DMG_RF_SECTOR_TYPE]);
+	if (sector_type >= QCA_ATTR_DMG_RF_SECTOR_TYPE_MAX) {
+		wil_err(wil, "Invalid sector type %d\n", sector_type);
+		return -EINVAL;
+	}
+
+	if (tb[QCA_ATTR_MAC_ADDR]) {
+		ether_addr_copy(mac_addr, nla_data(tb[QCA_ATTR_MAC_ADDR]));
+		if (!is_broadcast_ether_addr(mac_addr)) {
+			cid = wil_find_cid(wil, mac_addr);
+			if (cid < 0) {
+				wil_err(wil, "invalid MAC address %pM\n",
+					mac_addr);
+				return -ENOENT;
+			}
+		} else {
+			if (sector_index != WMI_INVALID_RF_SECTOR_INDEX) {
+				wil_err(wil, "broadcast MAC valid only with unlocking\n");
+				return -EINVAL;
+			}
+			cid = -1;
+		}
+	} else {
+		if (test_bit(wil_status_fwconnected, wil->status)) {
+			wil_err(wil, "must specify MAC address when connected\n");
+			return -EINVAL;
+		}
+		/* otherwise, using cid=0 for unassociated station */
+	}
+
+	if (cid >= 0) {
+		rc = wil_rf_sector_wmi_set_selected(wil, sector_index,
+						    sector_type, cid);
+	} else {
+		/* unlock all cids */
+		rc = wil_rf_sector_wmi_set_selected(
+			wil, WMI_INVALID_RF_SECTOR_INDEX, sector_type,
+			WIL_CID_ALL);
+		if (rc == -EINVAL) {
+			for (i = 0; i < WIL6210_MAX_CID; i++) {
+				rc = wil_rf_sector_wmi_set_selected(
+					wil, WMI_INVALID_RF_SECTOR_INDEX,
+					sector_type, i);
+				/* the FW will silently ignore and return
+				 * success for unused cid, so abort the loop
+				 * on any other error
+				 */
+				if (rc) {
+					wil_err(wil, "unlock cid %d failed with status %d\n",
+						i, rc);
+					break;
+				}
+			}
+		}
+	}
+
+	return rc;
+}
diff --git a/drivers/net/wireless/ath/wil6210/debugfs.c b/drivers/net/wireless/ath/wil6210/debugfs.c
index 5648ebbd0e16..f82506d276d3 100644
--- a/drivers/net/wireless/ath/wil6210/debugfs.c
+++ b/drivers/net/wireless/ath/wil6210/debugfs.c
@@ -509,6 +509,10 @@ static ssize_t wil_read_file_ioblob(struct file *file, char __user *user_buf,
 	void *buf;
 	size_t ret;
 
+	if (test_bit(wil_status_suspending, wil_blob->wil->status) ||
+	    test_bit(wil_status_suspended, wil_blob->wil->status))
+		return 0;
+
 	if (pos < 0)
 		return -EINVAL;
 
@@ -795,15 +799,11 @@ static ssize_t wil_write_file_txmgmt(struct file *file, const char __user *buf,
 	struct wireless_dev *wdev = wil_to_wdev(wil);
 	struct cfg80211_mgmt_tx_params params;
 	int rc;
-	void *frame = kmalloc(len, GFP_KERNEL);
+	void *frame;
 
-	if (!frame)
-		return -ENOMEM;
-
-	if (copy_from_user(frame, buf, len)) {
-		kfree(frame);
-		return -EIO;
-	}
+	frame = memdup_user(buf, len);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
 
 	params.buf = frame;
 	params.len = len;
@@ -1604,6 +1604,49 @@ static const struct file_operations fops_fw_version = {
 	.llseek		= seq_lseek,
 };
 
+/*---------suspend_stats---------*/
+static ssize_t wil_write_suspend_stats(struct file *file,
+				       const char __user *buf,
+				       size_t len, loff_t *ppos)
+{
+	struct wil6210_priv *wil = file->private_data;
+
+	memset(&wil->suspend_stats, 0, sizeof(wil->suspend_stats));
+
+	return len;
+}
+
+static ssize_t wil_read_suspend_stats(struct file *file,
+				      char __user *user_buf,
+				      size_t count, loff_t *ppos)
+{
+	struct wil6210_priv *wil = file->private_data;
+	static char text[400];
+	int n;
+
+	n = snprintf(text, sizeof(text),
+		     "Suspend statistics:\n"
+		     "successful suspends:%ld failed suspends:%ld\n"
+		     "successful resumes:%ld failed resumes:%ld\n"
+		     "rejected by host:%ld rejected by device:%ld\n",
+		     wil->suspend_stats.successful_suspends,
+		     wil->suspend_stats.failed_suspends,
+		     wil->suspend_stats.successful_resumes,
+		     wil->suspend_stats.failed_resumes,
+		     wil->suspend_stats.rejected_by_host,
+		     wil->suspend_stats.rejected_by_device);
+
+	n = min_t(int, n, sizeof(text));
+
+	return simple_read_from_buffer(user_buf, count, ppos, text, n);
+}
+
+static const struct file_operations fops_suspend_stats = {
+	.read = wil_read_suspend_stats,
+	.write = wil_write_suspend_stats,
+	.open  = simple_open,
+};
+
 /*----------------*/
 static void wil6210_debugfs_init_blobs(struct wil6210_priv *wil,
 				       struct dentry *dbg)
@@ -1656,6 +1699,7 @@ static const struct {
 	{"led_blink_time",	0644,	&fops_led_blink_time},
 	{"fw_capabilities",	0444,	&fops_fw_capabilities},
 	{"fw_version",	0444,		&fops_fw_version},
+	{"suspend_stats",	0644,	&fops_suspend_stats},
 };
 
 static void wil6210_debugfs_init_files(struct wil6210_priv *wil,
@@ -1702,6 +1746,7 @@ static const struct dbg_off dbg_wil_off[] = {
 	WIL_FIELD(discovery_mode, 0644,	doff_u8),
 	WIL_FIELD(chip_revision, 0444,	doff_u8),
 	WIL_FIELD(abft_len, 0644,		doff_u8),
+	WIL_FIELD(wakeup_trigger, 0644,		doff_u8),
 	{},
 };
 
diff --git a/drivers/net/wireless/ath/wil6210/interrupt.c b/drivers/net/wireless/ath/wil6210/interrupt.c
index cab1e5c0e374..cad8a95c4e4e 100644
--- a/drivers/net/wireless/ath/wil6210/interrupt.c
+++ b/drivers/net/wireless/ath/wil6210/interrupt.c
@@ -467,6 +467,12 @@ static irqreturn_t wil6210_thread_irq(int irq, void *cookie)
 
 	wil6210_unmask_irq_pseudo(wil);
 
+	if (wil->suspend_resp_rcvd) {
+		wil_dbg_irq(wil, "set suspend_resp_comp to true\n");
+		wil->suspend_resp_comp = true;
+		wake_up_interruptible(&wil->wq);
+	}
+
 	return IRQ_HANDLED;
 }
 
diff --git a/drivers/net/wireless/ath/wil6210/ioctl.c b/drivers/net/wireless/ath/wil6210/ioctl.c
deleted file mode 100644
index 630380078236..000000000000
--- a/drivers/net/wireless/ath/wil6210/ioctl.c
+++ /dev/null
@@ -1,180 +0,0 @@
-/*
- * Copyright (c) 2014 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/uaccess.h>
-
-#include "wil6210.h"
-#include <uapi/linux/wil6210_uapi.h>
-
-#define wil_hex_dump_ioctl(prefix_str, buf, len) \
-	print_hex_dump_debug("DBG[IOC ]" prefix_str, \
-			     DUMP_PREFIX_OFFSET, 16, 1, buf, len, true)
-#define wil_dbg_ioctl(wil, fmt, arg...) wil_dbg(wil, "DBG[IOC ]" fmt, ##arg)
-
-static void __iomem *wil_ioc_addr(struct wil6210_priv *wil, uint32_t addr,
-				  uint32_t size, enum wil_memio_op op)
-{
-	void __iomem *a;
-	u32 off;
-
-	switch (op & wil_mmio_addr_mask) {
-	case wil_mmio_addr_linker:
-		a = wmi_buffer(wil, cpu_to_le32(addr));
-		break;
-	case wil_mmio_addr_ahb:
-		a = wmi_addr(wil, addr);
-		break;
-	case wil_mmio_addr_bar:
-		a = wmi_addr(wil, addr + WIL6210_FW_HOST_OFF);
-		break;
-	default:
-		wil_err(wil, "Unsupported address mode, op = 0x%08x\n", op);
-		return NULL;
-	}
-
-	off = a - wil->csr;
-	if (size >= WIL6210_MEM_SIZE - off) {
-		wil_err(wil, "Requested block does not fit into memory: "
-			"off = 0x%08x size = 0x%08x\n", off, size);
-		return NULL;
-	}
-
-	return a;
-}
-
-static int wil_ioc_memio_dword(struct wil6210_priv *wil, void __user *data)
-{
-	struct wil_memio io;
-	void __iomem *a;
-	bool need_copy = false;
-
-	if (copy_from_user(&io, data, sizeof(io)))
-		return -EFAULT;
-
-	wil_dbg_ioctl(wil, "IO: addr = 0x%08x val = 0x%08x op = 0x%08x\n",
-		      io.addr, io.val, io.op);
-
-	a = wil_ioc_addr(wil, io.addr, sizeof(u32), io.op);
-	if (!a) {
-		wil_err(wil, "invalid address 0x%08x, op = 0x%08x\n", io.addr,
-			io.op);
-		return -EINVAL;
-	}
-	/* operation */
-	switch (io.op & wil_mmio_op_mask) {
-	case wil_mmio_read:
-		io.val = readl(a);
-		need_copy = true;
-		break;
-	case wil_mmio_write:
-		writel(io.val, a);
-		wmb(); /* make sure write propagated to HW */
-		break;
-	default:
-		wil_err(wil, "Unsupported operation, op = 0x%08x\n", io.op);
-		return -EINVAL;
-	}
-
-	if (need_copy) {
-		wil_dbg_ioctl(wil, "IO done: addr = 0x%08x"
-			      " val = 0x%08x op = 0x%08x\n",
-			      io.addr, io.val, io.op);
-		if (copy_to_user(data, &io, sizeof(io)))
-			return -EFAULT;
-	}
-
-	return 0;
-}
-
-static int wil_ioc_memio_block(struct wil6210_priv *wil, void __user *data)
-{
-	struct wil_memio_block io;
-	void *block;
-	void __iomem *a;
-	int rc = 0;
-
-	if (copy_from_user(&io, data, sizeof(io)))
-		return -EFAULT;
-
-	wil_dbg_ioctl(wil, "IO: addr = 0x%08x size = 0x%08x op = 0x%08x\n",
-		      io.addr, io.size, io.op);
-
-	/* size */
-	if (io.size % 4) {
-		wil_err(wil, "size is not multiple of 4:  0x%08x\n", io.size);
-		return -EINVAL;
-	}
-
-	a = wil_ioc_addr(wil, io.addr, io.size, io.op);
-	if (!a) {
-		wil_err(wil, "invalid address 0x%08x, op = 0x%08x\n", io.addr,
-			io.op);
-		return -EINVAL;
-	}
-
-	block = kmalloc(io.size, GFP_USER);
-	if (!block)
-		return -ENOMEM;
-
-	/* operation */
-	switch (io.op & wil_mmio_op_mask) {
-	case wil_mmio_read:
-		wil_memcpy_fromio_32(block, a, io.size);
-		wil_hex_dump_ioctl("Read  ", block, io.size);
-		if (copy_to_user(io.block, block, io.size)) {
-			rc = -EFAULT;
-			goto out_free;
-		}
-		break;
-	case wil_mmio_write:
-		if (copy_from_user(block, io.block, io.size)) {
-			rc = -EFAULT;
-			goto out_free;
-		}
-		wil_memcpy_toio_32(a, block, io.size);
-		wmb(); /* make sure write propagated to HW */
-		wil_hex_dump_ioctl("Write ", block, io.size);
-		break;
-	default:
-		wil_err(wil, "Unsupported operation, op = 0x%08x\n", io.op);
-		rc = -EINVAL;
-		break;
-	}
-
-out_free:
-	kfree(block);
-	return rc;
-}
-
-int wil_ioctl(struct wil6210_priv *wil, void __user *data, int cmd)
-{
-	int ret;
-
-	switch (cmd) {
-	case WIL_IOCTL_MEMIO:
-		ret = wil_ioc_memio_dword(wil, data);
-		break;
-	case WIL_IOCTL_MEMIO_BLOCK:
-		ret = wil_ioc_memio_block(wil, data);
-		break;
-	default:
-		wil_dbg_ioctl(wil, "Unsupported IOCTL 0x%04x\n", cmd);
-		return -ENOIOCTLCMD;
-	}
-
-	wil_dbg_ioctl(wil, "ioctl(0x%04x) -> %d\n", cmd, ret);
-	return ret;
-}
diff --git a/drivers/net/wireless/ath/wil6210/main.c b/drivers/net/wireless/ath/wil6210/main.c
index 32086792dfc3..daf944a71901 100644
--- a/drivers/net/wireless/ath/wil6210/main.c
+++ b/drivers/net/wireless/ath/wil6210/main.c
@@ -576,6 +576,9 @@ int wil_priv_init(struct wil6210_priv *wil)
 
 	wil->ps_profile =  WMI_PS_PROFILE_TYPE_DEFAULT;
 
+	wil->wakeup_trigger = WMI_WAKEUP_TRIGGER_UCAST |
+			      WMI_WAKEUP_TRIGGER_BCAST;
+
 	return 0;
 
 out_wmi_wq:
@@ -586,8 +589,10 @@ out_wmi_wq:
 
 void wil6210_bus_request(struct wil6210_priv *wil, u32 kbps)
 {
-	if (wil->platform_ops.bus_request)
+	if (wil->platform_ops.bus_request) {
+		wil->bus_request_kbps = kbps;
 		wil->platform_ops.bus_request(wil->platform_handle, kbps);
+	}
 }
 
 /**
diff --git a/drivers/net/wireless/ath/wil6210/netdev.c b/drivers/net/wireless/ath/wil6210/netdev.c
index 708facd5f667..4a6ab2d0fdf1 100644
--- a/drivers/net/wireless/ath/wil6210/netdev.c
+++ b/drivers/net/wireless/ath/wil6210/netdev.c
@@ -42,20 +42,12 @@ static int wil_stop(struct net_device *ndev)
 	return wil_down(wil);
 }
 
-static int wil_do_ioctl(struct net_device *ndev, struct ifreq *ifr, int cmd)
-{
-	struct wil6210_priv *wil = ndev_to_wil(ndev);
-
-	return wil_ioctl(wil, ifr->ifr_data, cmd);
-}
-
 static const struct net_device_ops wil_netdev_ops = {
 	.ndo_open		= wil_open,
 	.ndo_stop		= wil_stop,
 	.ndo_start_xmit		= wil_start_xmit,
 	.ndo_set_mac_address	= eth_mac_addr,
 	.ndo_validate_addr	= eth_validate_addr,
-	.ndo_do_ioctl		= wil_do_ioctl,
 };
 
 static int wil6210_netdev_poll_rx(struct napi_struct *napi, int budget)
diff --git a/drivers/net/wireless/ath/wil6210/pcie_bus.c b/drivers/net/wireless/ath/wil6210/pcie_bus.c
index b38515fc7ce7..d571feb2370e 100644
--- a/drivers/net/wireless/ath/wil6210/pcie_bus.c
+++ b/drivers/net/wireless/ath/wil6210/pcie_bus.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012-2016 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2017 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
@@ -26,6 +26,10 @@ static bool use_msi = true;
 module_param(use_msi, bool, 0444);
 MODULE_PARM_DESC(use_msi, " Use MSI interrupt, default - true");
 
+static bool ftm_mode;
+module_param(ftm_mode, bool, 0444);
+MODULE_PARM_DESC(ftm_mode, " Set factory test mode, default - false");
+
 #ifdef CONFIG_PM
 #ifdef CONFIG_PM_SLEEP
 static int wil6210_pm_notify(struct notifier_block *notify_block,
@@ -36,13 +40,15 @@ static int wil6210_pm_notify(struct notifier_block *notify_block,
 static
 void wil_set_capabilities(struct wil6210_priv *wil)
 {
+	const char *wil_fw_name;
 	u32 jtag_id = wil_r(wil, RGF_USER_JTAG_DEV_ID);
 	u8 chip_revision = (wil_r(wil, RGF_USER_REVISION_ID) &
 			    RGF_USER_REVISION_ID_MASK);
 
 	bitmap_zero(wil->hw_capabilities, hw_capability_last);
 	bitmap_zero(wil->fw_capabilities, WMI_FW_CAPABILITY_MAX);
-	wil->wil_fw_name = WIL_FW_NAME_DEFAULT;
+	wil->wil_fw_name = ftm_mode ? WIL_FW_NAME_FTM_DEFAULT :
+			   WIL_FW_NAME_DEFAULT;
 	wil->chip_revision = chip_revision;
 
 	switch (jtag_id) {
@@ -51,9 +57,11 @@ void wil_set_capabilities(struct wil6210_priv *wil)
 		case REVISION_ID_SPARROW_D0:
 			wil->hw_name = "Sparrow D0";
 			wil->hw_version = HW_VER_SPARROW_D0;
-			if (wil_fw_verify_file_exists(wil,
-						      WIL_FW_NAME_SPARROW_PLUS))
-				wil->wil_fw_name = WIL_FW_NAME_SPARROW_PLUS;
+			wil_fw_name = ftm_mode ? WIL_FW_NAME_FTM_SPARROW_PLUS :
+				      WIL_FW_NAME_SPARROW_PLUS;
+
+			if (wil_fw_verify_file_exists(wil, wil_fw_name))
+				wil->wil_fw_name = wil_fw_name;
 			break;
 		case REVISION_ID_SPARROW_B0:
 			wil->hw_name = "Sparrow B0";
@@ -104,8 +112,6 @@ static int wil_if_pcie_enable(struct wil6210_priv *wil)
 
 	wil_dbg_misc(wil, "if_pcie_enable, wmi_only %d\n", wmi_only);
 
-	pdev->msi_enabled = 0;
-
 	pci_set_master(pdev);
 
 	wil_dbg_misc(wil, "Setup %s interrupt\n", use_msi ? "MSI" : "INTx");
@@ -183,6 +189,13 @@ static int wil_platform_rop_fw_recovery(void *wil_handle)
 	return 0;
 }
 
+static void wil_platform_ops_uninit(struct wil6210_priv *wil)
+{
+	if (wil->platform_ops.uninit)
+		wil->platform_ops.uninit(wil->platform_handle);
+	memset(&wil->platform_ops, 0, sizeof(wil->platform_ops));
+}
+
 static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	struct wil6210_priv *wil;
@@ -192,16 +205,18 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		.ramdump = wil_platform_rop_ramdump,
 		.fw_recovery = wil_platform_rop_fw_recovery,
 	};
+	u32 bar_size = pci_resource_len(pdev, 0);
 
 	/* check HW */
 	dev_info(&pdev->dev, WIL_NAME
-		 " device found [%04x:%04x] (rev %x)\n",
-		 (int)pdev->vendor, (int)pdev->device, (int)pdev->revision);
-
-	if (pci_resource_len(pdev, 0) != WIL6210_MEM_SIZE) {
-		dev_err(&pdev->dev, "Not " WIL_NAME "? "
-			"BAR0 size is %lu while expecting %lu\n",
-			(ulong)pci_resource_len(pdev, 0), WIL6210_MEM_SIZE);
+		 " device found [%04x:%04x] (rev %x) bar size 0x%x\n",
+		 (int)pdev->vendor, (int)pdev->device, (int)pdev->revision,
+		 bar_size);
+
+	if ((bar_size < WIL6210_MIN_MEM_SIZE) ||
+	    (bar_size > WIL6210_MAX_MEM_SIZE)) {
+		dev_err(&pdev->dev, "Unexpected BAR0 size 0x%x\n",
+			bar_size);
 		return -ENODEV;
 	}
 
@@ -214,6 +229,7 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	wil->pdev = pdev;
 	pci_set_drvdata(pdev, wil);
+	wil->bar_size = bar_size;
 	/* rollback to if_free */
 
 	wil->platform_handle =
@@ -241,7 +257,7 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	}
 
 	rc = pci_enable_device(pdev);
-	if (rc) {
+	if (rc && pdev->msi_enabled == 0) {
 		wil_err(wil,
 			"pci_enable_device failed, retry with MSI only\n");
 		/* Work around for platforms that can't allocate IRQ:
@@ -256,6 +272,7 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto err_plat;
 	}
 	/* rollback to err_disable_pdev */
+	pci_set_power_state(pdev, PCI_D0);
 
 	rc = pci_request_region(pdev, 0, WIL_NAME);
 	if (rc) {
@@ -276,6 +293,15 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	wil_set_capabilities(wil);
 	wil6210_clear_irq(wil);
 
+	wil->keep_radio_on_during_sleep =
+		wil->platform_ops.keep_radio_on_during_sleep &&
+		wil->platform_ops.keep_radio_on_during_sleep(
+			wil->platform_handle) &&
+		test_bit(WMI_FW_CAPABILITY_D3_SUSPEND, wil->fw_capabilities);
+
+	wil_info(wil, "keep_radio_on_during_sleep (%d)\n",
+		 wil->keep_radio_on_during_sleep);
+
 	/* FW should raise IRQ when ready */
 	rc = wil_if_pcie_enable(wil);
 	if (rc) {
@@ -316,8 +342,7 @@ err_release_reg:
 err_disable_pdev:
 	pci_disable_device(pdev);
 err_plat:
-	if (wil->platform_ops.uninit)
-		wil->platform_ops.uninit(wil->platform_handle);
+	wil_platform_ops_uninit(wil);
 if_free:
 	wil_if_free(wil);
 
@@ -346,8 +371,7 @@ static void wil_pcie_remove(struct pci_dev *pdev)
 	pci_iounmap(pdev, csr);
 	pci_release_region(pdev, 0);
 	pci_disable_device(pdev);
-	if (wil->platform_ops.uninit)
-		wil->platform_ops.uninit(wil->platform_handle);
+	wil_platform_ops_uninit(wil);
 	wil_if_free(wil);
 }
 
@@ -374,15 +398,16 @@ static int wil6210_suspend(struct device *dev, bool is_runtime)
 		goto out;
 
 	rc = wil_suspend(wil, is_runtime);
-	if (rc)
-		goto out;
-
-	/* TODO: how do I bring card in low power state? */
-
-	/* disable bus mastering */
-	pci_clear_master(pdev);
-	/* PCI will call pci_save_state(pdev) and pci_prepare_to_sleep(pdev) */
+	if (!rc) {
+		wil->suspend_stats.successful_suspends++;
 
+		/* If platform device supports keep_radio_on_during_sleep
+		 * it will control PCIe master
+		 */
+		if (!wil->keep_radio_on_during_sleep)
+			/* disable bus mastering */
+			pci_clear_master(pdev);
+	}
 out:
 	return rc;
 }
@@ -395,12 +420,21 @@ static int wil6210_resume(struct device *dev, bool is_runtime)
 
 	wil_dbg_pm(wil, "resume: %s\n", is_runtime ? "runtime" : "system");
 
-	/* allow master */
-	pci_set_master(pdev);
-
+	/* If platform device supports keep_radio_on_during_sleep it will
+	 * control PCIe master
+	 */
+	if (!wil->keep_radio_on_during_sleep)
+		/* allow master */
+		pci_set_master(pdev);
 	rc = wil_resume(wil, is_runtime);
-	if (rc)
-		pci_clear_master(pdev);
+	if (rc) {
+		wil_err(wil, "device failed to resume (%d)\n", rc);
+		wil->suspend_stats.failed_resumes++;
+		if (!wil->keep_radio_on_during_sleep)
+			pci_clear_master(pdev);
+	} else {
+		wil->suspend_stats.successful_resumes++;
+	}
 
 	return rc;
 }
diff --git a/drivers/net/wireless/ath/wil6210/pm.c b/drivers/net/wireless/ath/wil6210/pm.c
index 2ae4fe85cc8c..ce1f384e7f8e 100644
--- a/drivers/net/wireless/ath/wil6210/pm.c
+++ b/drivers/net/wireless/ath/wil6210/pm.c
@@ -15,6 +15,7 @@
  */
 
 #include "wil6210.h"
+#include <linux/jiffies.h>
 
 int wil_can_suspend(struct wil6210_priv *wil, bool is_runtime)
 {
@@ -61,20 +62,170 @@ out:
 	wil_dbg_pm(wil, "can_suspend: %s => %s (%d)\n",
 		   is_runtime ? "runtime" : "system", rc ? "No" : "Yes", rc);
 
+	if (rc)
+		wil->suspend_stats.rejected_by_host++;
+
 	return rc;
 }
 
-int wil_suspend(struct wil6210_priv *wil, bool is_runtime)
+static int wil_resume_keep_radio_on(struct wil6210_priv *wil)
 {
 	int rc = 0;
-	struct net_device *ndev = wil_to_ndev(wil);
 
-	wil_dbg_pm(wil, "suspend: %s\n", is_runtime ? "runtime" : "system");
+	/* wil_status_resuming will be cleared when getting
+	 * WMI_TRAFFIC_RESUME_EVENTID
+	 */
+	set_bit(wil_status_resuming, wil->status);
+	clear_bit(wil_status_suspended, wil->status);
+	wil_c(wil, RGF_USER_CLKS_CTL_0, BIT_USER_CLKS_RST_PWGD);
+	wil_unmask_irq(wil);
 
-	if (test_bit(wil_status_suspended, wil->status)) {
-		wil_dbg_pm(wil, "trying to suspend while suspended\n");
-		return 0;
+	wil6210_bus_request(wil, wil->bus_request_kbps_pre_suspend);
+
+	/* Send WMI resume request to the device */
+	rc = wmi_resume(wil);
+	if (rc) {
+		wil_err(wil, "device failed to resume (%d), resetting\n", rc);
+		rc = wil_down(wil);
+		if (rc) {
+			wil_err(wil, "wil_down failed (%d)\n", rc);
+			goto out;
+		}
+		rc = wil_up(wil);
+		if (rc) {
+			wil_err(wil, "wil_up failed (%d)\n", rc);
+			goto out;
+		}
+	}
+
+	/* Wake all queues */
+	if (test_bit(wil_status_fwconnected, wil->status))
+		wil_update_net_queues_bh(wil, NULL, false);
+
+out:
+	if (rc)
+		set_bit(wil_status_suspended, wil->status);
+	return rc;
+}
+
+static int wil_suspend_keep_radio_on(struct wil6210_priv *wil)
+{
+	int rc = 0;
+	unsigned long start, data_comp_to;
+
+	wil_dbg_pm(wil, "suspend keep radio on\n");
+
+	/* Prevent handling of new tx and wmi commands */
+	set_bit(wil_status_suspending, wil->status);
+	wil_update_net_queues_bh(wil, NULL, true);
+
+	if (!wil_is_tx_idle(wil)) {
+		wil_dbg_pm(wil, "Pending TX data, reject suspend\n");
+		wil->suspend_stats.rejected_by_host++;
+		goto reject_suspend;
+	}
+
+	if (!wil_is_rx_idle(wil)) {
+		wil_dbg_pm(wil, "Pending RX data, reject suspend\n");
+		wil->suspend_stats.rejected_by_host++;
+		goto reject_suspend;
+	}
+
+	if (!wil_is_wmi_idle(wil)) {
+		wil_dbg_pm(wil, "Pending WMI events, reject suspend\n");
+		wil->suspend_stats.rejected_by_host++;
+		goto reject_suspend;
+	}
+
+	/* Send WMI suspend request to the device */
+	rc = wmi_suspend(wil);
+	if (rc) {
+		wil_dbg_pm(wil, "wmi_suspend failed, reject suspend (%d)\n",
+			   rc);
+		goto reject_suspend;
+	}
+
+	/* Wait for completion of the pending RX packets */
+	start = jiffies;
+	data_comp_to = jiffies + msecs_to_jiffies(WIL_DATA_COMPLETION_TO_MS);
+	if (test_bit(wil_status_napi_en, wil->status)) {
+		while (!wil_is_rx_idle(wil)) {
+			if (time_after(jiffies, data_comp_to)) {
+				if (wil_is_rx_idle(wil))
+					break;
+				wil_err(wil,
+					"TO waiting for idle RX, suspend failed\n");
+				wil->suspend_stats.failed_suspends++;
+				goto resume_after_fail;
+			}
+			wil_dbg_ratelimited(wil, "rx vring is not empty -> NAPI\n");
+			napi_synchronize(&wil->napi_rx);
+			msleep(20);
+		}
+	}
+
+	/* In case of pending WMI events, reject the suspend
+	 * and resume the device.
+	 * This can happen if the device sent the WMI events before
+	 * approving the suspend.
+	 */
+	if (!wil_is_wmi_idle(wil)) {
+		wil_err(wil, "suspend failed due to pending WMI events\n");
+		wil->suspend_stats.failed_suspends++;
+		goto resume_after_fail;
+	}
+
+	wil_mask_irq(wil);
+
+	/* Disable device reset on PERST */
+	wil_s(wil, RGF_USER_CLKS_CTL_0, BIT_USER_CLKS_RST_PWGD);
+
+	if (wil->platform_ops.suspend) {
+		rc = wil->platform_ops.suspend(wil->platform_handle, true);
+		if (rc) {
+			wil_err(wil, "platform device failed to suspend (%d)\n",
+				rc);
+			wil->suspend_stats.failed_suspends++;
+			wil_c(wil, RGF_USER_CLKS_CTL_0, BIT_USER_CLKS_RST_PWGD);
+			wil_unmask_irq(wil);
+			goto resume_after_fail;
+		}
+	}
+
+	/* Save the current bus request to return to the same in resume */
+	wil->bus_request_kbps_pre_suspend = wil->bus_request_kbps;
+	wil6210_bus_request(wil, 0);
+
+	set_bit(wil_status_suspended, wil->status);
+	clear_bit(wil_status_suspending, wil->status);
+
+	return rc;
+
+resume_after_fail:
+	set_bit(wil_status_resuming, wil->status);
+	clear_bit(wil_status_suspending, wil->status);
+	rc = wmi_resume(wil);
+	/* if resume succeeded, reject the suspend */
+	if (!rc) {
+		rc = -EBUSY;
+		if (test_bit(wil_status_fwconnected, wil->status))
+			wil_update_net_queues_bh(wil, NULL, false);
 	}
+	return rc;
+
+reject_suspend:
+	clear_bit(wil_status_suspending, wil->status);
+	if (test_bit(wil_status_fwconnected, wil->status))
+		wil_update_net_queues_bh(wil, NULL, false);
+	return -EBUSY;
+}
+
+static int wil_suspend_radio_off(struct wil6210_priv *wil)
+{
+	int rc = 0;
+	struct net_device *ndev = wil_to_ndev(wil);
+
+	wil_dbg_pm(wil, "suspend radio off\n");
 
 	/* if netif up, hardware is alive, shut it down */
 	if (ndev->flags & IFF_UP) {
@@ -90,7 +241,7 @@ int wil_suspend(struct wil6210_priv *wil, bool is_runtime)
 	wil_disable_irq(wil);
 
 	if (wil->platform_ops.suspend) {
-		rc = wil->platform_ops.suspend(wil->platform_handle);
+		rc = wil->platform_ops.suspend(wil->platform_handle, false);
 		if (rc) {
 			wil_enable_irq(wil);
 			goto out;
@@ -100,6 +251,50 @@ int wil_suspend(struct wil6210_priv *wil, bool is_runtime)
 	set_bit(wil_status_suspended, wil->status);
 
 out:
+	wil_dbg_pm(wil, "suspend radio off: %d\n", rc);
+
+	return rc;
+}
+
+static int wil_resume_radio_off(struct wil6210_priv *wil)
+{
+	int rc = 0;
+	struct net_device *ndev = wil_to_ndev(wil);
+
+	wil_dbg_pm(wil, "Enabling PCIe IRQ\n");
+	wil_enable_irq(wil);
+	/* if netif up, bring hardware up
+	 * During open(), IFF_UP set after actual device method
+	 * invocation. This prevent recursive call to wil_up()
+	 * wil_status_suspended will be cleared in wil_reset
+	 */
+	if (ndev->flags & IFF_UP)
+		rc = wil_up(wil);
+	else
+		clear_bit(wil_status_suspended, wil->status);
+
+	return rc;
+}
+
+int wil_suspend(struct wil6210_priv *wil, bool is_runtime)
+{
+	int rc = 0;
+	struct net_device *ndev = wil_to_ndev(wil);
+	bool keep_radio_on = ndev->flags & IFF_UP &&
+			     wil->keep_radio_on_during_sleep;
+
+	wil_dbg_pm(wil, "suspend: %s\n", is_runtime ? "runtime" : "system");
+
+	if (test_bit(wil_status_suspended, wil->status)) {
+		wil_dbg_pm(wil, "trying to suspend while suspended\n");
+		return 0;
+	}
+
+	if (!keep_radio_on)
+		rc = wil_suspend_radio_off(wil);
+	else
+		rc = wil_suspend_keep_radio_on(wil);
+
 	wil_dbg_pm(wil, "suspend: %s => %d\n",
 		   is_runtime ? "runtime" : "system", rc);
 
@@ -110,29 +305,24 @@ int wil_resume(struct wil6210_priv *wil, bool is_runtime)
 {
 	int rc = 0;
 	struct net_device *ndev = wil_to_ndev(wil);
+	bool keep_radio_on = ndev->flags & IFF_UP &&
+			     wil->keep_radio_on_during_sleep;
 
 	wil_dbg_pm(wil, "resume: %s\n", is_runtime ? "runtime" : "system");
 
 	if (wil->platform_ops.resume) {
-		rc = wil->platform_ops.resume(wil->platform_handle);
+		rc = wil->platform_ops.resume(wil->platform_handle,
+					      keep_radio_on);
 		if (rc) {
 			wil_err(wil, "platform_ops.resume : %d\n", rc);
 			goto out;
 		}
 	}
 
-	wil_dbg_pm(wil, "Enabling PCIe IRQ\n");
-	wil_enable_irq(wil);
-
-	/* if netif up, bring hardware up
-	 * During open(), IFF_UP set after actual device method
-	 * invocation. This prevent recursive call to wil_up().
-	 * wil_status_suspended will be cleared in wil_reset
-	 */
-	if (ndev->flags & IFF_UP)
-		rc = wil_up(wil);
+	if (keep_radio_on)
+		rc = wil_resume_keep_radio_on(wil);
 	else
-		clear_bit(wil_status_suspended, wil->status);
+		rc = wil_resume_radio_off(wil);
 
 out:
 	wil_dbg_pm(wil, "resume: %s => %d\n",
diff --git a/drivers/net/wireless/ath/wil6210/txrx.c b/drivers/net/wireless/ath/wil6210/txrx.c
index edab4c0a900f..ec57bcce9601 100644
--- a/drivers/net/wireless/ath/wil6210/txrx.c
+++ b/drivers/net/wireless/ath/wil6210/txrx.c
@@ -104,6 +104,51 @@ static inline int wil_vring_avail_high(struct vring *vring)
 	return wil_vring_avail_tx(vring) > wil_vring_wmark_high(vring);
 }
 
+/* returns true when all tx vrings are empty */
+bool wil_is_tx_idle(struct wil6210_priv *wil)
+{
+	int i;
+	unsigned long data_comp_to;
+
+	for (i = 0; i < WIL6210_MAX_TX_RINGS; i++) {
+		struct vring *vring = &wil->vring_tx[i];
+		int vring_index = vring - wil->vring_tx;
+		struct vring_tx_data *txdata = &wil->vring_tx_data[vring_index];
+
+		spin_lock(&txdata->lock);
+
+		if (!vring->va || !txdata->enabled) {
+			spin_unlock(&txdata->lock);
+			continue;
+		}
+
+		data_comp_to = jiffies + msecs_to_jiffies(
+					WIL_DATA_COMPLETION_TO_MS);
+		if (test_bit(wil_status_napi_en, wil->status)) {
+			while (!wil_vring_is_empty(vring)) {
+				if (time_after(jiffies, data_comp_to)) {
+					wil_dbg_pm(wil,
+						   "TO waiting for idle tx\n");
+					spin_unlock(&txdata->lock);
+					return false;
+				}
+				wil_dbg_ratelimited(wil,
+						    "tx vring is not empty -> NAPI\n");
+				spin_unlock(&txdata->lock);
+				napi_synchronize(&wil->napi_tx);
+				msleep(20);
+				spin_lock(&txdata->lock);
+				if (!vring->va || !txdata->enabled)
+					break;
+			}
+		}
+
+		spin_unlock(&txdata->lock);
+	}
+
+	return true;
+}
+
 /* wil_val_in_range - check if value in [min,max) */
 static inline bool wil_val_in_range(int val, int min, int max)
 {
@@ -363,7 +408,7 @@ static void wil_rx_add_radiotap_header(struct wil6210_priv *wil,
 		return;
 	}
 
-	rtap_vendor = (void *)skb_push(skb, rtap_len);
+	rtap_vendor = skb_push(skb, rtap_len);
 	memset(rtap_vendor, 0, rtap_len);
 
 	rtap_vendor->rtap.rthdr.it_version = PKTHDR_RADIOTAP_VERSION;
@@ -406,6 +451,18 @@ static inline int wil_is_back_req(u8 fc)
 	       (IEEE80211_FTYPE_CTL | IEEE80211_STYPE_BACK_REQ);
 }
 
+bool wil_is_rx_idle(struct wil6210_priv *wil)
+{
+	struct vring_rx_desc *_d;
+	struct vring *vring = &wil->vring_rx;
+
+	_d = (struct vring_rx_desc *)&vring->va[vring->swhead].rx;
+	if (_d->dma.status & RX_DMA_STATUS_DU)
+		return false;
+
+	return true;
+}
+
 /**
  * reap 1 frame from @swhead
  *
@@ -1812,6 +1869,15 @@ static int wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
 
 	spin_lock(&txdata->lock);
 
+	if (test_bit(wil_status_suspending, wil->status) ||
+	    test_bit(wil_status_suspended, wil->status) ||
+	    test_bit(wil_status_resuming, wil->status)) {
+		wil_dbg_txrx(wil,
+			     "suspend/resume in progress. drop packet\n");
+		spin_unlock(&txdata->lock);
+		return -EINVAL;
+	}
+
 	rc = (skb_is_gso(skb) ? __wil_tx_vring_tso : __wil_tx_vring)
 	     (wil, vring, skb);
 
@@ -1864,6 +1930,11 @@ static inline void __wil_update_net_queues(struct wil6210_priv *wil,
 		return;
 	}
 
+	/* Do not wake the queues in suspend flow */
+	if (test_bit(wil_status_suspending, wil->status) ||
+	    test_bit(wil_status_suspended, wil->status))
+		return;
+
 	/* check wake */
 	for (i = 0; i < WIL6210_MAX_TX_RINGS; i++) {
 		struct vring *cur_vring = &wil->vring_tx[i];
diff --git a/drivers/net/wireless/ath/wil6210/wil6210.h b/drivers/net/wireless/ath/wil6210/wil6210.h
index b00c803a1e83..d085ccfc7228 100644
--- a/drivers/net/wireless/ath/wil6210/wil6210.h
+++ b/drivers/net/wireless/ath/wil6210/wil6210.h
@@ -37,8 +37,13 @@ extern bool debug_fw;
 extern bool disable_ap_sme;
 
 #define WIL_NAME "wil6210"
-#define WIL_FW_NAME_DEFAULT "wil6210.fw" /* code Sparrow B0 */
-#define WIL_FW_NAME_SPARROW_PLUS "wil6210_sparrow_plus.fw" /* code Sparrow D0 */
+
+#define WIL_FW_NAME_DEFAULT "wil6210.fw"
+#define WIL_FW_NAME_FTM_DEFAULT "wil6210_ftm.fw"
+
+#define WIL_FW_NAME_SPARROW_PLUS "wil6210_sparrow_plus.fw"
+#define WIL_FW_NAME_FTM_SPARROW_PLUS "wil6210_sparrow_plus_ftm.fw"
+
 #define WIL_BOARD_FILE_NAME "wil6210.brd" /* board & radio parameters */
 
 #define WIL_DEFAULT_BUS_REQUEST_KBPS 128000 /* ~1Gbps */
@@ -53,7 +58,8 @@ static inline u32 WIL_GET_BITS(u32 x, int b0, int b1)
 	return (x >> b0) & ((1 << (b1 - b0 + 1)) - 1);
 }
 
-#define WIL6210_MEM_SIZE (2*1024*1024UL)
+#define WIL6210_MIN_MEM_SIZE (2 * 1024 * 1024UL)
+#define WIL6210_MAX_MEM_SIZE (4 * 1024 * 1024UL)
 
 #define WIL_TX_Q_LEN_DEFAULT		(4000)
 #define WIL_RX_RING_SIZE_ORDER_DEFAULT	(10)
@@ -77,6 +83,15 @@ static inline u32 WIL_GET_BITS(u32 x, int b0, int b1)
  */
 #define WIL_MAX_MPDU_OVERHEAD	(62)
 
+struct wil_suspend_stats {
+	unsigned long successful_suspends;
+	unsigned long failed_suspends;
+	unsigned long successful_resumes;
+	unsigned long failed_resumes;
+	unsigned long rejected_by_device;
+	unsigned long rejected_by_host;
+};
+
 /* Calculate MAC buffer size for the firmware. It includes all overhead,
  * as it will go over the air, and need to be 8 byte aligned
  */
@@ -284,6 +299,8 @@ enum {
 #define ISR_MISC_MBOX_EVT	BIT_DMA_EP_MISC_ICR_FW_INT(1)
 #define ISR_MISC_FW_ERROR	BIT_DMA_EP_MISC_ICR_FW_INT(3)
 
+#define WIL_DATA_COMPLETION_TO_MS 200
+
 /* Hardware definitions end */
 struct fw_map {
 	u32 from; /* linker address - from, inclusive */
@@ -412,7 +429,9 @@ enum { /* for wil6210_priv.status */
 	wil_status_irqen, /* FIXME: interrupts enabled - for debug */
 	wil_status_napi_en, /* NAPI enabled protected by wil->mutex */
 	wil_status_resetting, /* reset in progress */
+	wil_status_suspending, /* suspend in progress */
 	wil_status_suspended, /* suspend completed, device is suspended */
+	wil_status_resuming, /* resume in progress */
 	wil_status_last /* keep last */
 };
 
@@ -594,6 +613,7 @@ extern u8 led_polarity;
 
 struct wil6210_priv {
 	struct pci_dev *pdev;
+	u32 bar_size;
 	struct wireless_dev *wdev;
 	void __iomem *csr;
 	DECLARE_BITMAP(status, wil_status_last);
@@ -676,9 +696,12 @@ struct wil6210_priv {
 	struct wil_blob_wrapper blobs[ARRAY_SIZE(fw_mapping)];
 	u8 discovery_mode;
 	u8 abft_len;
+	u8 wakeup_trigger;
+	struct wil_suspend_stats suspend_stats;
 
 	void *platform_handle;
 	struct wil_platform_ops platform_ops;
+	bool keep_radio_on_during_sleep;
 
 	struct pmc_ctx pmc;
 
@@ -701,6 +724,11 @@ struct wil6210_priv {
 	struct notifier_block pm_notify;
 #endif /* CONFIG_PM_SLEEP */
 #endif /* CONFIG_PM */
+
+	bool suspend_resp_rcvd;
+	bool suspend_resp_comp;
+	u32 bus_request_kbps;
+	u32 bus_request_kbps_pre_suspend;
 };
 
 #define wil_to_wiphy(i) (i->wdev->wiphy)
@@ -949,7 +977,6 @@ void wil6210_unmask_irq_rx(struct wil6210_priv *wil);
 
 int wil_iftype_nl2wmi(enum nl80211_iftype type);
 
-int wil_ioctl(struct wil6210_priv *wil, void __user *data, int cmd);
 int wil_request_firmware(struct wil6210_priv *wil, const char *name,
 			 bool load);
 bool wil_fw_verify_file_exists(struct wil6210_priv *wil, const char *name);
@@ -957,6 +984,11 @@ bool wil_fw_verify_file_exists(struct wil6210_priv *wil, const char *name);
 int wil_can_suspend(struct wil6210_priv *wil, bool is_runtime);
 int wil_suspend(struct wil6210_priv *wil, bool is_runtime);
 int wil_resume(struct wil6210_priv *wil, bool is_runtime);
+bool wil_is_wmi_idle(struct wil6210_priv *wil);
+int wmi_resume(struct wil6210_priv *wil);
+int wmi_suspend(struct wil6210_priv *wil);
+bool wil_is_tx_idle(struct wil6210_priv *wil);
+bool wil_is_rx_idle(struct wil6210_priv *wil);
 
 int wil_fw_copy_crash_dump(struct wil6210_priv *wil, void *dest, u32 size);
 void wil_fw_core_dump(struct wil6210_priv *wil);
diff --git a/drivers/net/wireless/ath/wil6210/wil_platform.h b/drivers/net/wireless/ath/wil6210/wil_platform.h
index f8c41172a3f4..5d9e4bfcb045 100644
--- a/drivers/net/wireless/ath/wil6210/wil_platform.h
+++ b/drivers/net/wireless/ath/wil6210/wil_platform.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2016 Qualcomm Atheros, Inc.
+ * Copyright (c) 2014-2017 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
@@ -33,10 +33,11 @@ enum wil_platform_event {
  */
 struct wil_platform_ops {
 	int (*bus_request)(void *handle, uint32_t kbps /* KBytes/Sec */);
-	int (*suspend)(void *handle);
-	int (*resume)(void *handle);
+	int (*suspend)(void *handle, bool keep_device_power);
+	int (*resume)(void *handle, bool device_powered_on);
 	void (*uninit)(void *handle);
 	int (*notify)(void *handle, enum wil_platform_event evt);
+	bool (*keep_radio_on_during_sleep)(void *handle);
 };
 
 /**
diff --git a/drivers/net/wireless/ath/wil6210/wmi.c b/drivers/net/wireless/ath/wil6210/wmi.c
index 814c35645b73..65ef67321fc0 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.c
+++ b/drivers/net/wireless/ath/wil6210/wmi.c
@@ -37,6 +37,8 @@ module_param(led_id, byte, 0444);
 MODULE_PARM_DESC(led_id,
 		 " 60G device led enablement. Set the led ID (0-2) to enable");
 
+#define WIL_WAIT_FOR_SUSPEND_RESUME_COMP 200
+
 /**
  * WMI event receiving - theory of operations
  *
@@ -157,7 +159,7 @@ void __iomem *wmi_buffer(struct wil6210_priv *wil, __le32 ptr_)
 		return NULL;
 
 	off = HOSTADDR(ptr);
-	if (off > WIL6210_MEM_SIZE - 4)
+	if (off > wil->bar_size - 4)
 		return NULL;
 
 	return wil->csr + off;
@@ -177,7 +179,7 @@ void __iomem *wmi_addr(struct wil6210_priv *wil, u32 ptr)
 		return NULL;
 
 	off = HOSTADDR(ptr);
-	if (off > WIL6210_MEM_SIZE - 4)
+	if (off > wil->bar_size - 4)
 		return NULL;
 
 	return wil->csr + off;
@@ -233,6 +235,16 @@ static int __wmi_send(struct wil6210_priv *wil, u16 cmdid, void *buf, u16 len)
 		return -EAGAIN;
 	}
 
+	/* Allow sending only suspend / resume commands during susepnd flow */
+	if ((test_bit(wil_status_suspending, wil->status) ||
+	     test_bit(wil_status_suspended, wil->status) ||
+	     test_bit(wil_status_resuming, wil->status)) &&
+	     ((cmdid != WMI_TRAFFIC_SUSPEND_CMDID) &&
+	      (cmdid != WMI_TRAFFIC_RESUME_CMDID))) {
+		wil_err(wil, "WMI: reject send_command during suspend\n");
+		return -EINVAL;
+	}
+
 	if (!head) {
 		wil_err(wil, "WMI head is garbage: 0x%08x\n", r->head);
 		return -EINVAL;
@@ -677,11 +689,11 @@ static void wmi_evt_eapol_rx(struct wil6210_priv *wil, int id,
 		return;
 	}
 
-	eth = (struct ethhdr *)skb_put(skb, ETH_HLEN);
+	eth = skb_put(skb, ETH_HLEN);
 	ether_addr_copy(eth->h_dest, ndev->dev_addr);
 	ether_addr_copy(eth->h_source, evt->src_mac);
 	eth->h_proto = cpu_to_be16(ETH_P_PAE);
-	memcpy(skb_put(skb, eapol_len), evt->eapol, eapol_len);
+	skb_put_data(skb, evt->eapol, eapol_len);
 	skb->protocol = eth_type_trans(skb, ndev);
 	if (likely(netif_rx_ni(skb) == NET_RX_SUCCESS)) {
 		ndev->stats.rx_packets++;
@@ -862,6 +874,11 @@ void wmi_recv_cmd(struct wil6210_priv *wil)
 		return;
 	}
 
+	if (test_bit(wil_status_suspended, wil->status)) {
+		wil_err(wil, "suspended. cannot handle WMI event\n");
+		return;
+	}
+
 	for (n = 0;; n++) {
 		u16 len;
 		bool q;
@@ -914,6 +931,15 @@ void wmi_recv_cmd(struct wil6210_priv *wil)
 			struct wmi_cmd_hdr *wmi = &evt->event.wmi;
 			u16 id = le16_to_cpu(wmi->command_id);
 			u32 tstamp = le32_to_cpu(wmi->fw_timestamp);
+			if (test_bit(wil_status_resuming, wil->status)) {
+				if (id == WMI_TRAFFIC_RESUME_EVENTID)
+					clear_bit(wil_status_resuming,
+						  wil->status);
+				else
+					wil_err(wil,
+						"WMI evt %d while resuming\n",
+						id);
+			}
 			spin_lock_irqsave(&wil->wmi_ev_lock, flags);
 			if (wil->reply_id && wil->reply_id == id) {
 				if (wil->reply_buf) {
@@ -921,6 +947,11 @@ void wmi_recv_cmd(struct wil6210_priv *wil)
 					       min(len, wil->reply_size));
 					immed_reply = true;
 				}
+				if (id == WMI_TRAFFIC_SUSPEND_EVENTID) {
+					wil_dbg_wmi(wil,
+						    "set suspend_resp_rcvd\n");
+					wil->suspend_resp_rcvd = true;
+				}
 			}
 			spin_unlock_irqrestore(&wil->wmi_ev_lock, flags);
 
@@ -1762,6 +1793,85 @@ void wmi_event_flush(struct wil6210_priv *wil)
 	spin_unlock_irqrestore(&wil->wmi_ev_lock, flags);
 }
 
+int wmi_suspend(struct wil6210_priv *wil)
+{
+	int rc;
+	struct wmi_traffic_suspend_cmd cmd = {
+		.wakeup_trigger = wil->wakeup_trigger,
+	};
+	struct {
+		struct wmi_cmd_hdr wmi;
+		struct wmi_traffic_suspend_event evt;
+	} __packed reply;
+	u32 suspend_to = WIL_WAIT_FOR_SUSPEND_RESUME_COMP;
+
+	wil->suspend_resp_rcvd = false;
+	wil->suspend_resp_comp = false;
+
+	reply.evt.status = WMI_TRAFFIC_SUSPEND_REJECTED;
+
+	rc = wmi_call(wil, WMI_TRAFFIC_SUSPEND_CMDID, &cmd, sizeof(cmd),
+		      WMI_TRAFFIC_SUSPEND_EVENTID, &reply, sizeof(reply),
+		      suspend_to);
+	if (rc) {
+		wil_err(wil, "wmi_call for suspend req failed, rc=%d\n", rc);
+		if (rc == -ETIME)
+			/* wmi_call TO */
+			wil->suspend_stats.rejected_by_device++;
+		else
+			wil->suspend_stats.rejected_by_host++;
+		goto out;
+	}
+
+	wil_dbg_wmi(wil, "waiting for suspend_response_completed\n");
+
+	rc = wait_event_interruptible_timeout(wil->wq,
+					      wil->suspend_resp_comp,
+					      msecs_to_jiffies(suspend_to));
+	if (rc == 0) {
+		wil_err(wil, "TO waiting for suspend_response_completed\n");
+		if (wil->suspend_resp_rcvd)
+			/* Device responded but we TO due to another reason */
+			wil->suspend_stats.rejected_by_host++;
+		else
+			wil->suspend_stats.rejected_by_device++;
+		rc = -EBUSY;
+		goto out;
+	}
+
+	wil_dbg_wmi(wil, "suspend_response_completed rcvd\n");
+	if (reply.evt.status == WMI_TRAFFIC_SUSPEND_REJECTED) {
+		wil_dbg_pm(wil, "device rejected the suspend\n");
+		wil->suspend_stats.rejected_by_device++;
+	}
+	rc = reply.evt.status;
+
+out:
+	wil->suspend_resp_rcvd = false;
+	wil->suspend_resp_comp = false;
+
+	return rc;
+}
+
+int wmi_resume(struct wil6210_priv *wil)
+{
+	int rc;
+	struct {
+		struct wmi_cmd_hdr wmi;
+		struct wmi_traffic_resume_event evt;
+	} __packed reply;
+
+	reply.evt.status = WMI_TRAFFIC_RESUME_FAILED;
+
+	rc = wmi_call(wil, WMI_TRAFFIC_RESUME_CMDID, NULL, 0,
+		      WMI_TRAFFIC_RESUME_EVENTID, &reply, sizeof(reply),
+		      WIL_WAIT_FOR_SUSPEND_RESUME_COMP);
+	if (rc)
+		return rc;
+
+	return reply.evt.status;
+}
+
 static bool wmi_evt_call_handler(struct wil6210_priv *wil, int id,
 				 void *d, int len)
 {
@@ -1851,3 +1961,36 @@ void wmi_event_worker(struct work_struct *work)
 	}
 	wil_dbg_wmi(wil, "event_worker: Finished\n");
 }
+
+bool wil_is_wmi_idle(struct wil6210_priv *wil)
+{
+	ulong flags;
+	struct wil6210_mbox_ring *r = &wil->mbox_ctl.rx;
+	bool rc = false;
+
+	spin_lock_irqsave(&wil->wmi_ev_lock, flags);
+
+	/* Check if there are pending WMI events in the events queue */
+	if (!list_empty(&wil->pending_wmi_ev)) {
+		wil_dbg_pm(wil, "Pending WMI events in queue\n");
+		goto out;
+	}
+
+	/* Check if there is a pending WMI call */
+	if (wil->reply_id) {
+		wil_dbg_pm(wil, "Pending WMI call\n");
+		goto out;
+	}
+
+	/* Check if there are pending RX events in mbox */
+	r->head = wil_r(wil, RGF_MBOX +
+			offsetof(struct wil6210_mbox_ctl, rx.head));
+	if (r->tail != r->head)
+		wil_dbg_pm(wil, "Pending WMI mbox events\n");
+	else
+		rc = true;
+
+out:
+	spin_unlock_irqrestore(&wil->wmi_ev_lock, flags);
+	return rc;
+}
diff --git a/drivers/net/wireless/ath/wil6210/wmi.h b/drivers/net/wireless/ath/wil6210/wmi.h
index f7f5f4f801e3..256f63c57da0 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.h
+++ b/drivers/net/wireless/ath/wil6210/wmi.h
@@ -59,6 +59,7 @@ enum wmi_fw_capability {
 	WMI_FW_CAPABILITY_DISABLE_AP_SME	= 4,
 	WMI_FW_CAPABILITY_WMI_ONLY		= 5,
 	WMI_FW_CAPABILITY_THERMAL_THROTTLING	= 7,
+	WMI_FW_CAPABILITY_D3_SUSPEND		= 8,
 	WMI_FW_CAPABILITY_MAX,
 };
 
@@ -157,7 +158,7 @@ enum wmi_command_id {
 	WMI_FLASH_READ_CMDID				= 0x902,
 	WMI_FLASH_WRITE_CMDID				= 0x903,
 	/* Power management */
-	WMI_TRAFFIC_DEFERRAL_CMDID			= 0x904,
+	WMI_TRAFFIC_SUSPEND_CMDID			= 0x904,
 	WMI_TRAFFIC_RESUME_CMDID			= 0x905,
 	/* P2P */
 	WMI_P2P_CFG_CMDID				= 0x910,
@@ -500,8 +501,14 @@ struct wmi_port_delete_cmd {
 	u8 reserved[3];
 } __packed;
 
-/* WMI_TRAFFIC_DEFERRAL_CMDID */
-struct wmi_traffic_deferral_cmd {
+/* WMI_TRAFFIC_SUSPEND_CMD wakeup trigger bit mask values */
+enum wmi_wakeup_trigger {
+	WMI_WAKEUP_TRIGGER_UCAST	= 0x01,
+	WMI_WAKEUP_TRIGGER_BCAST	= 0x02,
+};
+
+/* WMI_TRAFFIC_SUSPEND_CMDID */
+struct wmi_traffic_suspend_cmd {
 	/* Bit vector: bit[0] - wake on Unicast, bit[1] - wake on Broadcast */
 	u8 wakeup_trigger;
 } __packed;
@@ -1084,7 +1091,7 @@ enum wmi_event_id {
 	WMI_FLASH_READ_DONE_EVENTID			= 0x1902,
 	WMI_FLASH_WRITE_DONE_EVENTID			= 0x1903,
 	/* Power management */
-	WMI_TRAFFIC_DEFERRAL_EVENTID			= 0x1904,
+	WMI_TRAFFIC_SUSPEND_EVENTID			= 0x1904,
 	WMI_TRAFFIC_RESUME_EVENTID			= 0x1905,
 	/* P2P */
 	WMI_P2P_CFG_DONE_EVENTID			= 0x1910,
@@ -1926,14 +1933,14 @@ struct wmi_link_maintain_cfg_read_done_event {
 	struct wmi_link_maintain_cfg lm_cfg;
 } __packed;
 
-enum wmi_traffic_deferral_status {
-	WMI_TRAFFIC_DEFERRAL_APPROVED	= 0x0,
-	WMI_TRAFFIC_DEFERRAL_REJECTED	= 0x1,
+enum wmi_traffic_suspend_status {
+	WMI_TRAFFIC_SUSPEND_APPROVED	= 0x0,
+	WMI_TRAFFIC_SUSPEND_REJECTED	= 0x1,
 };
 
-/* WMI_TRAFFIC_DEFERRAL_EVENTID */
-struct wmi_traffic_deferral_event {
-	/* enum wmi_traffic_deferral_status_e */
+/* WMI_TRAFFIC_SUSPEND_EVENTID */
+struct wmi_traffic_suspend_event {
+	/* enum wmi_traffic_suspend_status_e */
 	u8 status;
 } __packed;
 
diff --git a/drivers/net/wireless/atmel/atmel.c b/drivers/net/wireless/atmel/atmel.c
index 27b110dc8cc6..b68436b23a63 100644
--- a/drivers/net/wireless/atmel/atmel.c
+++ b/drivers/net/wireless/atmel/atmel.c
@@ -1036,9 +1036,8 @@ static void frag_rx_path(struct atmel_private *priv,
 				priv->dev->stats.rx_dropped++;
 			} else {
 				skb_reserve(skb, 2);
-				memcpy(skb_put(skb, priv->frag_len + 12),
-				       priv->rx_buf,
-				       priv->frag_len + 12);
+				skb_put_data(skb, priv->rx_buf,
+				             priv->frag_len + 12);
 				skb->protocol = eth_type_trans(skb, priv->dev);
 				skb->ip_summed = CHECKSUM_NONE;
 				netif_rx(skb);
diff --git a/drivers/net/wireless/broadcom/b43/main.c b/drivers/net/wireless/broadcom/b43/main.c
index d23aac7503d3..b37e7391f55d 100644
--- a/drivers/net/wireless/broadcom/b43/main.c
+++ b/drivers/net/wireless/broadcom/b43/main.c
@@ -71,8 +71,18 @@ MODULE_FIRMWARE("b43/ucode11.fw");
 MODULE_FIRMWARE("b43/ucode13.fw");
 MODULE_FIRMWARE("b43/ucode14.fw");
 MODULE_FIRMWARE("b43/ucode15.fw");
+MODULE_FIRMWARE("b43/ucode16_lp.fw");
 MODULE_FIRMWARE("b43/ucode16_mimo.fw");
+MODULE_FIRMWARE("b43/ucode24_lcn.fw");
+MODULE_FIRMWARE("b43/ucode25_lcn.fw");
+MODULE_FIRMWARE("b43/ucode25_mimo.fw");
+MODULE_FIRMWARE("b43/ucode26_mimo.fw");
+MODULE_FIRMWARE("b43/ucode29_mimo.fw");
+MODULE_FIRMWARE("b43/ucode33_lcn40.fw");
+MODULE_FIRMWARE("b43/ucode30_mimo.fw");
 MODULE_FIRMWARE("b43/ucode5.fw");
+MODULE_FIRMWARE("b43/ucode40.fw");
+MODULE_FIRMWARE("b43/ucode42.fw");
 MODULE_FIRMWARE("b43/ucode9.fw");
 
 static int modparam_bad_frames_preempt;
diff --git a/drivers/net/wireless/broadcom/b43legacy/dma.c b/drivers/net/wireless/broadcom/b43legacy/dma.c
index f9dd892b9f27..cfa617ddb2f1 100644
--- a/drivers/net/wireless/broadcom/b43legacy/dma.c
+++ b/drivers/net/wireless/broadcom/b43legacy/dma.c
@@ -1072,7 +1072,7 @@ static int dma_tx_fragment(struct b43legacy_dmaring *ring,
 			goto out_unmap_hdr;
 		}
 
-		memcpy(skb_put(bounce_skb, skb->len), skb->data, skb->len);
+		skb_put_data(bounce_skb, skb->data, skb->len);
 		memcpy(bounce_skb->cb, skb->cb, sizeof(skb->cb));
 		bounce_skb->dev = skb->dev;
 		skb_set_queue_mapping(bounce_skb, skb_get_queue_mapping(skb));
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bcmsdh.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bcmsdh.c
index 9b970dc2b922..984c1d0560b1 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bcmsdh.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bcmsdh.c
@@ -108,12 +108,14 @@ int brcmf_sdiod_intr_register(struct brcmf_sdio_dev *sdiodev)
 	int ret = 0;
 	u8 data;
 	u32 addr, gpiocontrol;
-	unsigned long flags;
 
 	pdata = &sdiodev->settings->bus.sdio;
 	if (pdata->oob_irq_supported) {
 		brcmf_dbg(SDIO, "Enter, register OOB IRQ %d\n",
 			  pdata->oob_irq_nr);
+		spin_lock_init(&sdiodev->irq_en_lock);
+		sdiodev->irq_en = true;
+
 		ret = request_irq(pdata->oob_irq_nr, brcmf_sdiod_oob_irqhandler,
 				  pdata->oob_irq_flags, "brcmf_oob_intr",
 				  &sdiodev->func[1]->dev);
@@ -122,10 +124,6 @@ int brcmf_sdiod_intr_register(struct brcmf_sdio_dev *sdiodev)
 			return ret;
 		}
 		sdiodev->oob_irq_requested = true;
-		spin_lock_init(&sdiodev->irq_en_lock);
-		spin_lock_irqsave(&sdiodev->irq_en_lock, flags);
-		sdiodev->irq_en = true;
-		spin_unlock_irqrestore(&sdiodev->irq_en_lock, flags);
 
 		ret = enable_irq_wake(pdata->oob_irq_nr);
 		if (ret != 0) {
@@ -706,7 +704,7 @@ done:
 int brcmf_sdiod_recv_chain(struct brcmf_sdio_dev *sdiodev,
 			   struct sk_buff_head *pktq, uint totlen)
 {
-	struct sk_buff *glom_skb;
+	struct sk_buff *glom_skb = NULL;
 	struct sk_buff *skb;
 	u32 addr = sdiodev->sbwad;
 	int err = 0;
@@ -727,10 +725,8 @@ int brcmf_sdiod_recv_chain(struct brcmf_sdio_dev *sdiodev,
 			return -ENOMEM;
 		err = brcmf_sdiod_buffrw(sdiodev, SDIO_FUNC_2, false, addr,
 					 glom_skb);
-		if (err) {
-			brcmu_pkt_buf_free_skb(glom_skb);
+		if (err)
 			goto done;
-		}
 
 		skb_queue_walk(pktq, skb) {
 			memcpy(skb->data, glom_skb->data, skb->len);
@@ -741,6 +737,7 @@ int brcmf_sdiod_recv_chain(struct brcmf_sdio_dev *sdiodev,
 					    pktq);
 
 done:
+	brcmu_pkt_buf_free_skb(glom_skb);
 	return err;
 }
 
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/btcoex.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/btcoex.c
index 14a70d4b4e86..3559fb5b8fb0 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/btcoex.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/btcoex.c
@@ -380,9 +380,7 @@ int brcmf_btcoex_attach(struct brcmf_cfg80211_info *cfg)
 	/* Set up timer for BT  */
 	btci->timer_on = false;
 	btci->timeout = BRCMF_BTCOEX_OPPR_WIN_TIME;
-	init_timer(&btci->timer);
-	btci->timer.data = (ulong)btci;
-	btci->timer.function = brcmf_btcoex_timerfunc;
+	setup_timer(&btci->timer, brcmf_btcoex_timerfunc, (ulong)btci);
 	btci->cfg = cfg;
 	btci->saved_regs_part1 = false;
 	btci->saved_regs_part2 = false;
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bus.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bus.h
index b55c3293c4b4..163ddc49f951 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bus.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bus.h
@@ -113,6 +113,17 @@ struct brcmf_bus_msgbuf {
 
 
 /**
+ * struct brcmf_bus_stats - bus statistic counters.
+ *
+ * @pktcowed: packets cowed for extra headroom/unorphan.
+ * @pktcow_failed: packets dropped due to failed cow-ing.
+ */
+struct brcmf_bus_stats {
+	atomic_t pktcowed;
+	atomic_t pktcow_failed;
+};
+
+/**
  * struct brcmf_bus - interface structure between common and bus layer
  *
  * @bus_priv: pointer to private bus device.
@@ -120,11 +131,10 @@ struct brcmf_bus_msgbuf {
  * @dev: device pointer of bus device.
  * @drvr: public driver information.
  * @state: operational state of the bus interface.
+ * @stats: statistics shared between common and bus layer.
  * @maxctl: maximum size for rxctl request message.
- * @tx_realloc: number of tx packets realloced for headroom.
- * @dstats: dongle-based statistical data.
- * @dcmd_list: bus/device specific dongle initialization commands.
  * @chip: device identifier of the dongle chip.
+ * @always_use_fws_queue: bus wants use queue also when fwsignal is inactive.
  * @wowl_supported: is wowl supported by bus driver.
  * @chiprev: revision of the dongle chip.
  */
@@ -138,8 +148,8 @@ struct brcmf_bus {
 	struct device *dev;
 	struct brcmf_pub *drvr;
 	enum brcmf_bus_state state;
+	struct brcmf_bus_stats stats;
 	uint maxctl;
-	unsigned long tx_realloc;
 	u32 chip;
 	u32 chiprev;
 	bool always_use_fws_queue;
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
index cd1d6730eab7..dcde596c9eb9 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
@@ -625,6 +625,7 @@ struct wireless_dev *brcmf_ap_add_vif(struct wiphy *wiphy, const char *name,
 	err = brcmf_net_attach(ifp, true);
 	if (err) {
 		brcmf_err("Registering netdevice failed\n");
+		free_netdev(ifp->ndev);
 		goto fail;
 	}
 
@@ -719,6 +720,8 @@ s32 brcmf_notify_escan_complete(struct brcmf_cfg80211_info *cfg,
 {
 	struct brcmf_scan_params_le params_le;
 	struct cfg80211_scan_request *scan_request;
+	u64 reqid;
+	u32 bucket;
 	s32 err = 0;
 
 	brcmf_dbg(SCAN, "Enter\n");
@@ -749,7 +752,7 @@ s32 brcmf_notify_escan_complete(struct brcmf_cfg80211_info *cfg,
 		err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SCAN,
 					     &params_le, sizeof(params_le));
 		if (err)
-			brcmf_err("Scan abort  failed\n");
+			brcmf_err("Scan abort failed\n");
 	}
 
 	brcmf_scan_config_mpc(ifp, 1);
@@ -758,11 +761,21 @@ s32 brcmf_notify_escan_complete(struct brcmf_cfg80211_info *cfg,
 	 * e-scan can be initiated internally
 	 * which takes precedence.
 	 */
-	if (cfg->internal_escan) {
-		brcmf_dbg(SCAN, "scheduled scan completed\n");
-		cfg->internal_escan = false;
-		if (!aborted)
-			cfg80211_sched_scan_results(cfg_to_wiphy(cfg), 0);
+	if (cfg->int_escan_map) {
+		brcmf_dbg(SCAN, "scheduled scan completed (%x)\n",
+			  cfg->int_escan_map);
+		while (cfg->int_escan_map) {
+			bucket = __ffs(cfg->int_escan_map);
+			cfg->int_escan_map &= ~BIT(bucket);
+			reqid = brcmf_pno_find_reqid_by_bucket(cfg->pno,
+							       bucket);
+			if (!aborted) {
+				brcmf_dbg(SCAN, "report results: reqid=%llu\n",
+					  reqid);
+				cfg80211_sched_scan_results(cfg_to_wiphy(cfg),
+							    reqid);
+			}
+		}
 	} else if (scan_request) {
 		struct cfg80211_scan_info info = {
 			.aborted = aborted,
@@ -1011,7 +1024,7 @@ static void brcmf_escan_prep(struct brcmf_cfg80211_info *cfg,
 			if (!ssid_le.SSID_len)
 				brcmf_dbg(SCAN, "%d: Broadcast scan\n", i);
 			else
-				brcmf_dbg(SCAN, "%d: scan for  %s size =%d\n",
+				brcmf_dbg(SCAN, "%d: scan for  %.32s size=%d\n",
 					  i, ssid_le.SSID, ssid_le.SSID_len);
 			memcpy(ptr, &ssid_le, sizeof(ssid_le));
 			ptr += sizeof(ssid_le);
@@ -1344,6 +1357,27 @@ static u16 brcmf_map_fw_linkdown_reason(const struct brcmf_event_msg *e)
 	return reason;
 }
 
+static int brcmf_set_pmk(struct brcmf_if *ifp, const u8 *pmk_data, u16 pmk_len)
+{
+	struct brcmf_wsec_pmk_le pmk;
+	int i, err;
+
+	/* convert to firmware key format */
+	pmk.key_len = cpu_to_le16(pmk_len << 1);
+	pmk.flags = cpu_to_le16(BRCMF_WSEC_PASSPHRASE);
+	for (i = 0; i < pmk_len; i++)
+		snprintf(&pmk.key[2 * i], 3, "%02x", pmk_data[i]);
+
+	/* store psk in firmware */
+	err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SET_WSEC_PMK,
+				     &pmk, sizeof(pmk));
+	if (err < 0)
+		brcmf_err("failed to change PSK in firmware (len=%u)\n",
+			  pmk_len);
+
+	return err;
+}
+
 static void brcmf_link_down(struct brcmf_cfg80211_vif *vif, u16 reason)
 {
 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(vif->wdev.wiphy);
@@ -1366,6 +1400,10 @@ static void brcmf_link_down(struct brcmf_cfg80211_vif *vif, u16 reason)
 	clear_bit(BRCMF_VIF_STATUS_CONNECTING, &vif->sme_state);
 	clear_bit(BRCMF_SCAN_STATUS_SUPPRESS, &cfg->scan_status);
 	brcmf_btcoex_set_mode(vif, BRCMF_BTCOEX_ENABLED, 0);
+	if (vif->profile.use_fwsup != BRCMF_PROFILE_FWSUP_NONE) {
+		brcmf_set_pmk(vif->ifp, NULL, 0);
+		vif->profile.use_fwsup = BRCMF_PROFILE_FWSUP_NONE;
+	}
 	brcmf_dbg(TRACE, "Exit\n");
 }
 
@@ -1679,6 +1717,7 @@ static s32
 brcmf_set_key_mgmt(struct net_device *ndev, struct cfg80211_connect_params *sme)
 {
 	struct brcmf_if *ifp = netdev_priv(ndev);
+	struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
 	s32 val;
 	s32 err;
 	const struct brcmf_tlv *rsn_ie;
@@ -1689,6 +1728,8 @@ brcmf_set_key_mgmt(struct net_device *ndev, struct cfg80211_connect_params *sme)
 	u32 mfp;
 	u16 count;
 
+	profile->use_fwsup = BRCMF_PROFILE_FWSUP_NONE;
+
 	if (!sme->crypto.n_akm_suites)
 		return 0;
 
@@ -1701,6 +1742,8 @@ brcmf_set_key_mgmt(struct net_device *ndev, struct cfg80211_connect_params *sme)
 		switch (sme->crypto.akm_suites[0]) {
 		case WLAN_AKM_SUITE_8021X:
 			val = WPA_AUTH_UNSPECIFIED;
+			if (sme->want_1x)
+				profile->use_fwsup = BRCMF_PROFILE_FWSUP_1X;
 			break;
 		case WLAN_AKM_SUITE_PSK:
 			val = WPA_AUTH_PSK;
@@ -1714,9 +1757,13 @@ brcmf_set_key_mgmt(struct net_device *ndev, struct cfg80211_connect_params *sme)
 		switch (sme->crypto.akm_suites[0]) {
 		case WLAN_AKM_SUITE_8021X:
 			val = WPA2_AUTH_UNSPECIFIED;
+			if (sme->want_1x)
+				profile->use_fwsup = BRCMF_PROFILE_FWSUP_1X;
 			break;
 		case WLAN_AKM_SUITE_8021X_SHA256:
 			val = WPA2_AUTH_1X_SHA256;
+			if (sme->want_1x)
+				profile->use_fwsup = BRCMF_PROFILE_FWSUP_1X;
 			break;
 		case WLAN_AKM_SUITE_PSK_SHA256:
 			val = WPA2_AUTH_PSK_SHA256;
@@ -1731,6 +1778,9 @@ brcmf_set_key_mgmt(struct net_device *ndev, struct cfg80211_connect_params *sme)
 		}
 	}
 
+	if (profile->use_fwsup == BRCMF_PROFILE_FWSUP_1X)
+		brcmf_dbg(INFO, "using 1X offload\n");
+
 	if (!brcmf_feat_is_enabled(ifp, BRCMF_FEAT_MFP))
 		goto skip_mfp_config;
 	/* The MFP mode (1 or 2) needs to be determined, parse IEs. The
@@ -1903,6 +1953,7 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 {
 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
 	struct brcmf_if *ifp = netdev_priv(ndev);
+	struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
 	struct ieee80211_channel *chan = sme->channel;
 	struct brcmf_join_params join_params;
 	size_t join_params_size;
@@ -1999,6 +2050,31 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 		goto done;
 	}
 
+	if (sme->crypto.psk) {
+		if (WARN_ON(profile->use_fwsup != BRCMF_PROFILE_FWSUP_NONE)) {
+			err = -EINVAL;
+			goto done;
+		}
+		brcmf_dbg(INFO, "using PSK offload\n");
+		profile->use_fwsup = BRCMF_PROFILE_FWSUP_PSK;
+	}
+
+	if (profile->use_fwsup != BRCMF_PROFILE_FWSUP_NONE) {
+		/* enable firmware supplicant for this interface */
+		err = brcmf_fil_iovar_int_set(ifp, "sup_wpa", 1);
+		if (err < 0) {
+			brcmf_err("failed to enable fw supplicant\n");
+			goto done;
+		}
+	}
+
+	if (profile->use_fwsup == BRCMF_PROFILE_FWSUP_PSK) {
+		err = brcmf_set_pmk(ifp, sme->crypto.psk,
+				    BRCMF_WSEC_MAX_PSK_LEN);
+		if (err)
+			goto done;
+	}
+
 	/* Join with specific BSSID and cached SSID
 	 * If SSID is zero join based on BSSID only
 	 */
@@ -3011,7 +3087,7 @@ void brcmf_abort_scanning(struct brcmf_cfg80211_info *cfg)
 	struct escan_info *escan = &cfg->escan_info;
 
 	set_bit(BRCMF_SCAN_STATUS_ABORT, &cfg->scan_status);
-	if (cfg->internal_escan || cfg->scan_request) {
+	if (cfg->int_escan_map || cfg->scan_request) {
 		escan->escan_state = WL_ESCAN_STATE_IDLE;
 		brcmf_notify_escan_complete(cfg, escan->ifp, true, true);
 	}
@@ -3034,7 +3110,7 @@ static void brcmf_escan_timeout(unsigned long data)
 	struct brcmf_cfg80211_info *cfg =
 			(struct brcmf_cfg80211_info *)data;
 
-	if (cfg->internal_escan || cfg->scan_request) {
+	if (cfg->int_escan_map || cfg->scan_request) {
 		brcmf_err("timer expired\n");
 		schedule_work(&cfg->escan_timeout_work);
 	}
@@ -3120,7 +3196,7 @@ brcmf_cfg80211_escan_handler(struct brcmf_if *ifp,
 		if (brcmf_p2p_scan_finding_common_channel(cfg, bss_info_le))
 			goto exit;
 
-		if (!cfg->internal_escan && !cfg->scan_request) {
+		if (!cfg->int_escan_map && !cfg->scan_request) {
 			brcmf_dbg(SCAN, "result without cfg80211 request\n");
 			goto exit;
 		}
@@ -3166,7 +3242,7 @@ brcmf_cfg80211_escan_handler(struct brcmf_if *ifp,
 		cfg->escan_info.escan_state = WL_ESCAN_STATE_IDLE;
 		if (brcmf_p2p_scan_finding_common_channel(cfg, NULL))
 			goto exit;
-		if (cfg->internal_escan || cfg->scan_request) {
+		if (cfg->int_escan_map || cfg->scan_request) {
 			brcmf_inform_bss(cfg);
 			aborted = status != BRCMF_E_STATUS_SUCCESS;
 			brcmf_notify_escan_complete(cfg, ifp, aborted, false);
@@ -3248,17 +3324,21 @@ static int brcmf_internal_escan_add_info(struct cfg80211_scan_request *req,
 	return 0;
 }
 
-static int brcmf_start_internal_escan(struct brcmf_if *ifp,
+static int brcmf_start_internal_escan(struct brcmf_if *ifp, u32 fwmap,
 				      struct cfg80211_scan_request *request)
 {
 	struct brcmf_cfg80211_info *cfg = ifp->drvr->config;
 	int err;
 
 	if (test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status)) {
+		if (cfg->int_escan_map)
+			brcmf_dbg(SCAN, "aborting internal scan: map=%u\n",
+				  cfg->int_escan_map);
 		/* Abort any on-going scan */
 		brcmf_abort_scanning(cfg);
 	}
 
+	brcmf_dbg(SCAN, "start internal scan: map=%u\n", fwmap);
 	set_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
 	cfg->escan_info.run = brcmf_run_escan;
 	err = brcmf_do_escan(ifp, request);
@@ -3266,7 +3346,7 @@ static int brcmf_start_internal_escan(struct brcmf_if *ifp,
 		clear_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
 		return err;
 	}
-	cfg->internal_escan = true;
+	cfg->int_escan_map = fwmap;
 	return 0;
 }
 
@@ -3308,6 +3388,7 @@ brcmf_notify_sched_scan_results(struct brcmf_if *ifp,
 	struct wiphy *wiphy = cfg_to_wiphy(cfg);
 	int i, err = 0;
 	struct brcmf_pno_scanresults_le *pfn_result;
+	u32 bucket_map;
 	u32 result_count;
 	u32 status;
 	u32 datalen;
@@ -3352,6 +3433,7 @@ brcmf_notify_sched_scan_results(struct brcmf_if *ifp,
 		goto out_err;
 	}
 
+	bucket_map = 0;
 	for (i = 0; i < result_count; i++) {
 		netinfo = &netinfo_start[i];
 
@@ -3359,6 +3441,7 @@ brcmf_notify_sched_scan_results(struct brcmf_if *ifp,
 			netinfo->SSID_len = IEEE80211_MAX_SSID_LEN;
 		brcmf_dbg(SCAN, "SSID:%.32s Channel:%d\n",
 			  netinfo->SSID, netinfo->channel);
+		bucket_map |= brcmf_pno_get_bucket_map(cfg->pno, netinfo);
 		err = brcmf_internal_escan_add_info(request,
 						    netinfo->SSID,
 						    netinfo->SSID_len,
@@ -3367,7 +3450,10 @@ brcmf_notify_sched_scan_results(struct brcmf_if *ifp,
 			goto out_err;
 	}
 
-	err = brcmf_start_internal_escan(ifp, request);
+	if (!bucket_map)
+		goto free_req;
+
+	err = brcmf_start_internal_escan(ifp, bucket_map, request);
 	if (!err)
 		goto free_req;
 
@@ -3386,11 +3472,11 @@ brcmf_cfg80211_sched_scan_start(struct wiphy *wiphy,
 	struct brcmf_if *ifp = netdev_priv(ndev);
 	struct brcmf_cfg80211_info *cfg = wiphy_priv(wiphy);
 
-	brcmf_dbg(SCAN, "Enter n_match_sets:%d n_ssids:%d\n",
+	brcmf_dbg(SCAN, "Enter: n_match_sets=%d n_ssids=%d\n",
 		  req->n_match_sets, req->n_ssids);
 
 	if (test_bit(BRCMF_SCAN_STATUS_SUPPRESS, &cfg->scan_status)) {
-		brcmf_err("Scanning suppressed: status (%lu)\n",
+		brcmf_err("Scanning suppressed: status=%lu\n",
 			  cfg->scan_status);
 		return -EAGAIN;
 	}
@@ -3411,8 +3497,8 @@ static int brcmf_cfg80211_sched_scan_stop(struct wiphy *wiphy,
 	struct brcmf_if *ifp = netdev_priv(ndev);
 
 	brcmf_dbg(SCAN, "enter\n");
-	brcmf_pno_clean(ifp);
-	if (cfg->internal_escan)
+	brcmf_pno_stop_sched_scan(ifp, reqid);
+	if (cfg->int_escan_map)
 		brcmf_notify_escan_complete(cfg, ifp, true, true);
 	return 0;
 }
@@ -4674,9 +4760,6 @@ static int brcmf_cfg80211_stop_ap(struct wiphy *wiphy, struct net_device *ndev)
 		err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_AP, 0);
 		if (err < 0)
 			brcmf_err("setting AP mode failed %d\n", err);
-		err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_INFRA, 0);
-		if (err < 0)
-			brcmf_err("setting INFRA mode failed %d\n", err);
 		if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_MBSS))
 			brcmf_fil_iovar_int_set(ifp, "mbss", 0);
 		brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_REGULATORY,
@@ -5131,6 +5214,34 @@ brcmf_cfg80211_set_rekey_data(struct wiphy *wiphy, struct net_device *ndev,
 }
 #endif
 
+static int brcmf_cfg80211_set_pmk(struct wiphy *wiphy, struct net_device *dev,
+				  const struct cfg80211_pmk_conf *conf)
+{
+	struct brcmf_if *ifp;
+
+	brcmf_dbg(TRACE, "enter\n");
+
+	/* expect using firmware supplicant for 1X */
+	ifp = netdev_priv(dev);
+	if (WARN_ON(ifp->vif->profile.use_fwsup != BRCMF_PROFILE_FWSUP_1X))
+		return -EINVAL;
+
+	return brcmf_set_pmk(ifp, conf->pmk, conf->pmk_len);
+}
+
+static int brcmf_cfg80211_del_pmk(struct wiphy *wiphy, struct net_device *dev,
+				  const u8 *aa)
+{
+	struct brcmf_if *ifp;
+
+	brcmf_dbg(TRACE, "enter\n");
+	ifp = netdev_priv(dev);
+	if (WARN_ON(ifp->vif->profile.use_fwsup != BRCMF_PROFILE_FWSUP_1X))
+		return -EINVAL;
+
+	return brcmf_set_pmk(ifp, NULL, 0);
+}
+
 static struct cfg80211_ops brcmf_cfg80211_ops = {
 	.add_virtual_intf = brcmf_cfg80211_add_iface,
 	.del_virtual_intf = brcmf_cfg80211_del_iface,
@@ -5174,6 +5285,8 @@ static struct cfg80211_ops brcmf_cfg80211_ops = {
 	.crit_proto_stop = brcmf_cfg80211_crit_proto_stop,
 	.tdls_oper = brcmf_cfg80211_tdls_oper,
 	.update_connect_params = brcmf_cfg80211_update_conn_params,
+	.set_pmk = brcmf_cfg80211_set_pmk,
+	.del_pmk = brcmf_cfg80211_del_pmk,
 };
 
 struct brcmf_cfg80211_vif *brcmf_alloc_vif(struct brcmf_cfg80211_info *cfg,
@@ -5225,19 +5338,33 @@ void brcmf_cfg80211_free_netdev(struct net_device *ndev)
 
 	if (vif)
 		brcmf_free_vif(vif);
-	free_netdev(ndev);
 }
 
-static bool brcmf_is_linkup(const struct brcmf_event_msg *e)
+static bool brcmf_is_linkup(struct brcmf_cfg80211_vif *vif,
+			    const struct brcmf_event_msg *e)
 {
 	u32 event = e->event_code;
 	u32 status = e->status;
 
+	if (vif->profile.use_fwsup == BRCMF_PROFILE_FWSUP_PSK &&
+	    event == BRCMF_E_PSK_SUP &&
+	    status == BRCMF_E_STATUS_FWSUP_COMPLETED)
+		set_bit(BRCMF_VIF_STATUS_EAP_SUCCESS, &vif->sme_state);
 	if (event == BRCMF_E_SET_SSID && status == BRCMF_E_STATUS_SUCCESS) {
 		brcmf_dbg(CONN, "Processing set ssid\n");
-		return true;
+		memcpy(vif->profile.bssid, e->addr, ETH_ALEN);
+		if (vif->profile.use_fwsup != BRCMF_PROFILE_FWSUP_PSK)
+			return true;
+
+		set_bit(BRCMF_VIF_STATUS_ASSOC_SUCCESS, &vif->sme_state);
 	}
 
+	if (test_bit(BRCMF_VIF_STATUS_EAP_SUCCESS, &vif->sme_state) &&
+	    test_bit(BRCMF_VIF_STATUS_ASSOC_SUCCESS, &vif->sme_state)) {
+		clear_bit(BRCMF_VIF_STATUS_EAP_SUCCESS, &vif->sme_state);
+		clear_bit(BRCMF_VIF_STATUS_ASSOC_SUCCESS, &vif->sme_state);
+		return true;
+	}
 	return false;
 }
 
@@ -5272,6 +5399,13 @@ static bool brcmf_is_nonetwork(struct brcmf_cfg80211_info *cfg,
 		return true;
 	}
 
+	if (event == BRCMF_E_PSK_SUP &&
+	    status != BRCMF_E_STATUS_FWSUP_COMPLETED) {
+		brcmf_dbg(CONN, "Processing failed supplicant state: %u\n",
+			  status);
+		return true;
+	}
+
 	return false;
 }
 
@@ -5422,27 +5556,28 @@ brcmf_bss_connect_done(struct brcmf_cfg80211_info *cfg,
 	struct brcmf_if *ifp = netdev_priv(ndev);
 	struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
 	struct brcmf_cfg80211_connect_info *conn_info = cfg_to_conn(cfg);
+	struct cfg80211_connect_resp_params conn_params;
 
 	brcmf_dbg(TRACE, "Enter\n");
 
 	if (test_and_clear_bit(BRCMF_VIF_STATUS_CONNECTING,
 			       &ifp->vif->sme_state)) {
+		memset(&conn_params, 0, sizeof(conn_params));
 		if (completed) {
 			brcmf_get_assoc_ies(cfg, ifp);
-			memcpy(profile->bssid, e->addr, ETH_ALEN);
 			brcmf_update_bss_info(cfg, ifp);
 			set_bit(BRCMF_VIF_STATUS_CONNECTED,
 				&ifp->vif->sme_state);
+			conn_params.status = WLAN_STATUS_SUCCESS;
+		} else {
+			conn_params.status = WLAN_STATUS_AUTH_TIMEOUT;
 		}
-		cfg80211_connect_result(ndev,
-					(u8 *)profile->bssid,
-					conn_info->req_ie,
-					conn_info->req_ie_len,
-					conn_info->resp_ie,
-					conn_info->resp_ie_len,
-					completed ? WLAN_STATUS_SUCCESS :
-						    WLAN_STATUS_AUTH_TIMEOUT,
-					GFP_KERNEL);
+		conn_params.bssid = profile->bssid;
+		conn_params.req_ie = conn_info->req_ie;
+		conn_params.req_ie_len = conn_info->req_ie_len;
+		conn_params.resp_ie = conn_info->resp_ie;
+		conn_params.resp_ie_len = conn_info->resp_ie_len;
+		cfg80211_connect_done(ndev, &conn_params, GFP_KERNEL);
 		brcmf_dbg(CONN, "Report connect result - connection %s\n",
 			  completed ? "succeeded" : "failed");
 	}
@@ -5508,7 +5643,7 @@ brcmf_notify_connect_status(struct brcmf_if *ifp,
 
 	if (brcmf_is_apmode(ifp->vif)) {
 		err = brcmf_notify_connect_status_ap(cfg, ndev, e, data);
-	} else if (brcmf_is_linkup(e)) {
+	} else if (brcmf_is_linkup(ifp->vif, e)) {
 		brcmf_dbg(CONN, "Linkup\n");
 		if (brcmf_is_ibssmode(ifp->vif)) {
 			brcmf_inform_ibss(cfg, ndev, e->addr);
@@ -5676,6 +5811,8 @@ static void brcmf_register_event_handlers(struct brcmf_cfg80211_info *cfg)
 			    brcmf_p2p_notify_action_tx_complete);
 	brcmf_fweh_register(cfg->pub, BRCMF_E_ACTION_FRAME_OFF_CHAN_COMPLETE,
 			    brcmf_p2p_notify_action_tx_complete);
+	brcmf_fweh_register(cfg->pub, BRCMF_E_PSK_SUP,
+			    brcmf_notify_connect_status);
 }
 
 static void brcmf_deinit_priv_mem(struct brcmf_cfg80211_info *cfg)
@@ -6378,16 +6515,6 @@ err:
 	return -ENOMEM;
 }
 
-static void brcmf_wiphy_pno_params(struct wiphy *wiphy)
-{
-	/* scheduled scan settings */
-	wiphy->max_sched_scan_reqs = 1;
-	wiphy->max_sched_scan_ssids = BRCMF_PNO_MAX_PFN_COUNT;
-	wiphy->max_match_sets = BRCMF_PNO_MAX_PFN_COUNT;
-	wiphy->max_sched_scan_ie_len = BRCMF_SCAN_IE_LEN_MAX;
-	wiphy->max_sched_scan_plan_interval = BRCMF_PNO_SCHED_SCAN_MAX_PERIOD;
-}
-
 #ifdef CONFIG_PM
 static const struct wiphy_wowlan_support brcmf_wowlan_support = {
 	.flags = WIPHY_WOWLAN_MAGIC_PKT | WIPHY_WOWLAN_DISCONNECT,
@@ -6434,6 +6561,7 @@ static int brcmf_setup_wiphy(struct wiphy *wiphy, struct brcmf_if *ifp)
 	const struct ieee80211_iface_combination *combo;
 	struct ieee80211_supported_band *band;
 	u16 max_interfaces = 0;
+	bool gscan;
 	__le32 bandlist[3];
 	u32 n_bands;
 	int err, i;
@@ -6481,11 +6609,18 @@ static int brcmf_setup_wiphy(struct wiphy *wiphy, struct brcmf_if *ifp)
 		wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS;
 	if (!ifp->drvr->settings->roamoff)
 		wiphy->flags |= WIPHY_FLAG_SUPPORTS_FW_ROAM;
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_FWSUP)) {
+		wiphy_ext_feature_set(wiphy,
+				      NL80211_EXT_FEATURE_4WAY_HANDSHAKE_STA_PSK);
+		wiphy_ext_feature_set(wiphy,
+				      NL80211_EXT_FEATURE_4WAY_HANDSHAKE_STA_1X);
+	}
 	wiphy->mgmt_stypes = brcmf_txrx_stypes;
 	wiphy->max_remain_on_channel_duration = 5000;
-	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_PNO))
-		brcmf_wiphy_pno_params(wiphy);
-
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_PNO)) {
+		gscan = brcmf_feat_is_enabled(ifp, BRCMF_FEAT_GSCAN);
+		brcmf_pno_wiphy_params(wiphy, gscan);
+	}
 	/* vendor commands/events support */
 	wiphy->vendor_commands = brcmf_vendor_cmds;
 	wiphy->n_vendor_commands = BRCMF_VNDR_CMDS_LAST - 1;
@@ -6767,7 +6902,7 @@ static void brcmf_cfg80211_reg_notifier(struct wiphy *wiphy,
 	/* ignore non-ISO3166 country codes */
 	for (i = 0; i < sizeof(req->alpha2); i++)
 		if (req->alpha2[i] < 'A' || req->alpha2[i] > 'Z') {
-			brcmf_err("not a ISO3166 code (0x%02x 0x%02x)\n",
+			brcmf_err("not an ISO3166 code (0x%02x 0x%02x)\n",
 				  req->alpha2[0], req->alpha2[1]);
 			return;
 		}
@@ -6851,7 +6986,7 @@ struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct brcmf_pub *drvr,
 	wiphy = wiphy_new(ops, sizeof(struct brcmf_cfg80211_info));
 	if (!wiphy) {
 		brcmf_err("Could not allocate wiphy device\n");
-		return NULL;
+		goto ops_out;
 	}
 	memcpy(wiphy->perm_addr, drvr->mac, ETH_ALEN);
 	set_wiphy_dev(wiphy, busdev);
@@ -6952,6 +7087,13 @@ struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct brcmf_pub *drvr,
 		brcmf_p2p_detach(&cfg->p2p);
 		goto wiphy_unreg_out;
 	}
+	err = brcmf_pno_attach(cfg);
+	if (err) {
+		brcmf_err("PNO initialisation failed (%d)\n", err);
+		brcmf_btcoex_detach(cfg);
+		brcmf_p2p_detach(&cfg->p2p);
+		goto wiphy_unreg_out;
+	}
 
 	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_TDLS)) {
 		err = brcmf_fil_iovar_int_set(ifp, "tdls_enable", 1);
@@ -6984,6 +7126,7 @@ struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct brcmf_pub *drvr,
 	return cfg;
 
 detach:
+	brcmf_pno_detach(cfg);
 	brcmf_btcoex_detach(cfg);
 	brcmf_p2p_detach(&cfg->p2p);
 wiphy_unreg_out:
@@ -6994,6 +7137,7 @@ priv_out:
 	ifp->vif = NULL;
 wiphy_out:
 	brcmf_free_wiphy(wiphy);
+ops_out:
 	kfree(ops);
 	return NULL;
 }
@@ -7003,6 +7147,7 @@ void brcmf_cfg80211_detach(struct brcmf_cfg80211_info *cfg)
 	if (!cfg)
 		return;
 
+	brcmf_pno_detach(cfg);
 	brcmf_btcoex_detach(cfg);
 	wiphy_unregister(cfg->wiphy);
 	kfree(cfg->ops);
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.h
index 8f19d95d4175..7b2835e5e434 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.h
@@ -24,6 +24,8 @@
 #include "fwil_types.h"
 #include "p2p.h"
 
+#define BRCMF_SCAN_IE_LEN_MAX		2048
+
 #define WL_NUM_SCAN_MAX			10
 #define WL_TLV_INFO_MAX			1024
 #define WL_BSS_INFO_MAX			2048
@@ -113,6 +115,12 @@ struct brcmf_cfg80211_security {
 	u32 cipher_group;
 };
 
+enum brcmf_profile_fwsup {
+	BRCMF_PROFILE_FWSUP_NONE,
+	BRCMF_PROFILE_FWSUP_PSK,
+	BRCMF_PROFILE_FWSUP_1X
+};
+
 /**
  * struct brcmf_cfg80211_profile - profile information.
  *
@@ -124,6 +132,7 @@ struct brcmf_cfg80211_profile {
 	u8 bssid[ETH_ALEN];
 	struct brcmf_cfg80211_security sec;
 	struct brcmf_wsec_key key[BRCMF_MAX_DEFAULT_KEYS];
+	enum brcmf_profile_fwsup use_fwsup;
 };
 
 /**
@@ -131,16 +140,20 @@ struct brcmf_cfg80211_profile {
  *
  * @BRCMF_VIF_STATUS_READY: ready for operation.
  * @BRCMF_VIF_STATUS_CONNECTING: connect/join in progress.
- * @BRCMF_VIF_STATUS_CONNECTED: connected/joined succesfully.
+ * @BRCMF_VIF_STATUS_CONNECTED: connected/joined successfully.
  * @BRCMF_VIF_STATUS_DISCONNECTING: disconnect/disable in progress.
  * @BRCMF_VIF_STATUS_AP_CREATED: AP operation started.
+ * @BRCMF_VIF_STATUS_EAP_SUCCUSS: EAPOL handshake successful.
+ * @BRCMF_VIF_STATUS_ASSOC_SUCCESS: successful SET_SSID received.
  */
 enum brcmf_vif_status {
 	BRCMF_VIF_STATUS_READY,
 	BRCMF_VIF_STATUS_CONNECTING,
 	BRCMF_VIF_STATUS_CONNECTED,
 	BRCMF_VIF_STATUS_DISCONNECTING,
-	BRCMF_VIF_STATUS_AP_CREATED
+	BRCMF_VIF_STATUS_AP_CREATED,
+	BRCMF_VIF_STATUS_EAP_SUCCESS,
+	BRCMF_VIF_STATUS_ASSOC_SUCCESS,
 };
 
 /**
@@ -271,7 +284,7 @@ struct brcmf_cfg80211_wowl {
  * @pub: common driver information.
  * @channel: current channel.
  * @active_scan: current scan mode.
- * @internal_escan: indicates internally initiated e-scan is running.
+ * @int_escan_map: bucket map for which internal e-scan is done.
  * @ibss_starter: indicates this sta is ibss starter.
  * @pwr_save: indicate whether dongle to support power save mode.
  * @dongle_up: indicate whether dongle up or not.
@@ -287,6 +300,7 @@ struct brcmf_cfg80211_wowl {
  * @vif_cnt: number of vif instances.
  * @vif_event: vif event signalling.
  * @wowl: wowl related information.
+ * @pno: information of pno module.
  */
 struct brcmf_cfg80211_info {
 	struct wiphy *wiphy;
@@ -303,7 +317,7 @@ struct brcmf_cfg80211_info {
 	struct brcmf_pub *pub;
 	u32 channel;
 	bool active_scan;
-	bool internal_escan;
+	u32 int_escan_map;
 	bool ibss_starter;
 	bool pwr_save;
 	bool dongle_up;
@@ -320,6 +334,7 @@ struct brcmf_cfg80211_info {
 	struct brcmu_d11inf d11inf;
 	struct brcmf_assoclist_le assoclist;
 	struct brcmf_cfg80211_wowl wowl;
+	struct brcmf_pno_info *pno;
 };
 
 /**
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
index a3d82368f1a9..2153e8062b4c 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
@@ -30,6 +30,7 @@
 #include "debug.h"
 #include "fwil_types.h"
 #include "p2p.h"
+#include "pno.h"
 #include "cfg80211.h"
 #include "fwil.h"
 #include "feature.h"
@@ -198,6 +199,7 @@ static netdev_tx_t brcmf_netdev_start_xmit(struct sk_buff *skb,
 	struct brcmf_if *ifp = netdev_priv(ndev);
 	struct brcmf_pub *drvr = ifp->drvr;
 	struct ethhdr *eh;
+	int head_delta;
 
 	brcmf_dbg(DATA, "Enter, bsscfgidx=%d\n", ifp->bsscfgidx);
 
@@ -210,13 +212,21 @@ static netdev_tx_t brcmf_netdev_start_xmit(struct sk_buff *skb,
 		goto done;
 	}
 
-	/* Make sure there's enough writable headroom*/
-	ret = skb_cow_head(skb, drvr->hdrlen);
-	if (ret < 0) {
-		brcmf_err("%s: skb_cow_head failed\n",
-			  brcmf_ifname(ifp));
-		dev_kfree_skb(skb);
-		goto done;
+	/* Make sure there's enough writeable headroom */
+	if (skb_headroom(skb) < drvr->hdrlen || skb_header_cloned(skb)) {
+		head_delta = drvr->hdrlen - skb_headroom(skb);
+
+		brcmf_dbg(INFO, "%s: insufficient headroom (%d)\n",
+			  brcmf_ifname(ifp), head_delta);
+		atomic_inc(&drvr->bus_if->stats.pktcowed);
+		ret = pskb_expand_head(skb, ALIGN(head_delta, NET_SKB_PAD), 0,
+				       GFP_ATOMIC);
+		if (ret < 0) {
+			brcmf_err("%s: failed to expand headroom\n",
+				  brcmf_ifname(ifp));
+			atomic_inc(&drvr->bus_if->stats.pktcow_failed);
+			goto done;
+		}
 	}
 
 	/* validate length for ether packet */
@@ -484,13 +494,13 @@ int brcmf_net_attach(struct brcmf_if *ifp, bool rtnl_locked)
 		goto fail;
 	}
 
+	ndev->priv_destructor = brcmf_cfg80211_free_netdev;
 	brcmf_dbg(INFO, "%s: Broadcom Dongle Host Driver\n", ndev->name);
 	return 0;
 
 fail:
 	drvr->iflist[ifp->bsscfgidx] = NULL;
 	ndev->netdev_ops = NULL;
-	free_netdev(ndev);
 	return -EBADE;
 }
 
@@ -503,6 +513,7 @@ static void brcmf_net_detach(struct net_device *ndev, bool rtnl_locked)
 			unregister_netdev(ndev);
 	} else {
 		brcmf_cfg80211_free_netdev(ndev);
+		free_netdev(ndev);
 	}
 }
 
@@ -579,7 +590,6 @@ static int brcmf_net_p2p_attach(struct brcmf_if *ifp)
 fail:
 	ifp->drvr->iflist[ifp->bsscfgidx] = NULL;
 	ndev->netdev_ops = NULL;
-	free_netdev(ndev);
 	return -EBADE;
 }
 
@@ -624,7 +634,7 @@ struct brcmf_if *brcmf_add_if(struct brcmf_pub *drvr, s32 bsscfgidx, s32 ifidx,
 		if (!ndev)
 			return ERR_PTR(-ENOMEM);
 
-		ndev->destructor = brcmf_cfg80211_free_netdev;
+		ndev->needs_free_netdev = true;
 		ifp = netdev_priv(ndev);
 		ifp->ndev = ndev;
 		/* store mapping ifidx to bsscfgidx */
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/debug.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/debug.h
index fe264a5798f1..35919d9e8e13 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/debug.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/debug.h
@@ -78,6 +78,7 @@ do {								\
 #define BRCMF_EVENT_ON()	(brcmf_msg_level & BRCMF_EVENT_VAL)
 #define BRCMF_FIL_ON()		(brcmf_msg_level & BRCMF_FIL_VAL)
 #define BRCMF_FWCON_ON()	(brcmf_msg_level & BRCMF_FWCON_VAL)
+#define BRCMF_SCAN_ON()		(brcmf_msg_level & BRCMF_SCAN_VAL)
 
 #else /* defined(DEBUG) || defined(CONFIG_BRCM_TRACING) */
 
@@ -96,6 +97,7 @@ do {								\
 #define BRCMF_EVENT_ON()	0
 #define BRCMF_FIL_ON()		0
 #define BRCMF_FWCON_ON()	0
+#define BRCMF_SCAN_ON()		0
 
 #endif /* defined(DEBUG) || defined(CONFIG_BRCM_TRACING) */
 
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
index 62985f2c0853..d21258d277ce 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
@@ -27,6 +27,7 @@
 #include "feature.h"
 #include "common.h"
 
+#define BRCMF_FW_UNSUPPORTED	23
 
 /*
  * expand feature list to array of feature strings.
@@ -113,6 +114,22 @@ static void brcmf_feat_iovar_int_get(struct brcmf_if *ifp,
 	}
 }
 
+static void brcmf_feat_iovar_data_set(struct brcmf_if *ifp,
+				      enum brcmf_feat_id id, char *name,
+				      const void *data, size_t len)
+{
+	int err;
+
+	err = brcmf_fil_iovar_data_set(ifp, name, data, len);
+	if (err != -BRCMF_FW_UNSUPPORTED) {
+		brcmf_dbg(INFO, "enabling feature: %s\n", brcmf_feat_names[id]);
+		ifp->drvr->feat_flags |= BIT(id);
+	} else {
+		brcmf_dbg(TRACE, "%s feature check failed: %d\n",
+			  brcmf_feat_names[id], err);
+	}
+}
+
 static void brcmf_feat_firmware_capabilities(struct brcmf_if *ifp)
 {
 	char caps[256];
@@ -136,11 +153,14 @@ void brcmf_feat_attach(struct brcmf_pub *drvr)
 {
 	struct brcmf_if *ifp = brcmf_get_ifp(drvr, 0);
 	struct brcmf_pno_macaddr_le pfn_mac;
+	struct brcmf_gscan_config gscan_cfg;
 	u32 wowl_cap;
 	s32 err;
 
 	brcmf_feat_firmware_capabilities(ifp);
-
+	memset(&gscan_cfg, 0, sizeof(gscan_cfg));
+	brcmf_feat_iovar_data_set(ifp, BRCMF_FEAT_GSCAN, "pfn_gscan_cfg",
+				  &gscan_cfg, sizeof(gscan_cfg));
 	brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_PNO, "pfn");
 	if (drvr->bus_if->wowl_supported)
 		brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_WOWL, "wowl");
@@ -175,6 +195,7 @@ void brcmf_feat_attach(struct brcmf_pub *drvr)
 			  drvr->settings->feature_disable);
 		ifp->drvr->feat_flags &= ~drvr->settings->feature_disable;
 	}
+	brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_FWSUP, "sup_wpa");
 
 	/* set chip related quirks */
 	switch (drvr->bus_if->chip) {
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
index db4733a95e28..1ab4f1617112 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
@@ -31,6 +31,8 @@
  * WOWL_GTK: (WOWL) GTK rekeying offload
  * WOWL_ARP_ND: ARP and Neighbor Discovery offload support during WOWL.
  * MFP: 802.11w Management Frame Protection.
+ * GSCAN: enhanced scan offload feature.
+ * FWSUP: Firmware supplicant.
  */
 #define BRCMF_FEAT_LIST \
 	BRCMF_FEAT_DEF(MBSS) \
@@ -44,7 +46,9 @@
 	BRCMF_FEAT_DEF(WOWL_ND) \
 	BRCMF_FEAT_DEF(WOWL_GTK) \
 	BRCMF_FEAT_DEF(WOWL_ARP_ND) \
-	BRCMF_FEAT_DEF(MFP)
+	BRCMF_FEAT_DEF(MFP) \
+	BRCMF_FEAT_DEF(GSCAN) \
+	BRCMF_FEAT_DEF(FWSUP)
 
 /*
  * Quirks:
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/firmware.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/firmware.c
index c7c1e9906500..d231042f19d6 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/firmware.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/firmware.c
@@ -442,7 +442,7 @@ struct brcmf_fw {
 	const char *nvram_name;
 	u16 domain_nr;
 	u16 bus_nr;
-	void (*done)(struct device *dev, const struct firmware *fw,
+	void (*done)(struct device *dev, int err, const struct firmware *fw,
 		     void *nvram_image, u32 nvram_len);
 };
 
@@ -477,52 +477,51 @@ static void brcmf_fw_request_nvram_done(const struct firmware *fw, void *ctx)
 	if (!nvram && !(fwctx->flags & BRCMF_FW_REQ_NV_OPTIONAL))
 		goto fail;
 
-	fwctx->done(fwctx->dev, fwctx->code, nvram, nvram_length);
+	fwctx->done(fwctx->dev, 0, fwctx->code, nvram, nvram_length);
 	kfree(fwctx);
 	return;
 
 fail:
 	brcmf_dbg(TRACE, "failed: dev=%s\n", dev_name(fwctx->dev));
 	release_firmware(fwctx->code);
-	device_release_driver(fwctx->dev);
+	fwctx->done(fwctx->dev, -ENOENT, NULL, NULL, 0);
 	kfree(fwctx);
 }
 
 static void brcmf_fw_request_code_done(const struct firmware *fw, void *ctx)
 {
 	struct brcmf_fw *fwctx = ctx;
-	int ret;
+	int ret = 0;
 
 	brcmf_dbg(TRACE, "enter: dev=%s\n", dev_name(fwctx->dev));
-	if (!fw)
+	if (!fw) {
+		ret = -ENOENT;
 		goto fail;
-
-	/* only requested code so done here */
-	if (!(fwctx->flags & BRCMF_FW_REQUEST_NVRAM)) {
-		fwctx->done(fwctx->dev, fw, NULL, 0);
-		kfree(fwctx);
-		return;
 	}
+	/* only requested code so done here */
+	if (!(fwctx->flags & BRCMF_FW_REQUEST_NVRAM))
+		goto done;
+
 	fwctx->code = fw;
 	ret = request_firmware_nowait(THIS_MODULE, true, fwctx->nvram_name,
 				      fwctx->dev, GFP_KERNEL, fwctx,
 				      brcmf_fw_request_nvram_done);
 
-	if (!ret)
-		return;
-
-	brcmf_fw_request_nvram_done(NULL, fwctx);
+	/* pass NULL to nvram callback for bcm47xx fallback */
+	if (ret)
+		brcmf_fw_request_nvram_done(NULL, fwctx);
 	return;
 
 fail:
 	brcmf_dbg(TRACE, "failed: dev=%s\n", dev_name(fwctx->dev));
-	device_release_driver(fwctx->dev);
+done:
+	fwctx->done(fwctx->dev, ret, fw, NULL, 0);
 	kfree(fwctx);
 }
 
 int brcmf_fw_get_firmwares_pcie(struct device *dev, u16 flags,
 				const char *code, const char *nvram,
-				void (*fw_cb)(struct device *dev,
+				void (*fw_cb)(struct device *dev, int err,
 					      const struct firmware *fw,
 					      void *nvram_image, u32 nvram_len),
 				u16 domain_nr, u16 bus_nr)
@@ -555,7 +554,7 @@ int brcmf_fw_get_firmwares_pcie(struct device *dev, u16 flags,
 
 int brcmf_fw_get_firmwares(struct device *dev, u16 flags,
 			   const char *code, const char *nvram,
-			   void (*fw_cb)(struct device *dev,
+			   void (*fw_cb)(struct device *dev, int err,
 					 const struct firmware *fw,
 					 void *nvram_image, u32 nvram_len))
 {
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/firmware.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/firmware.h
index d3c9f0d52ae3..8fa4b7e1ab3d 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/firmware.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/firmware.h
@@ -73,13 +73,13 @@ void brcmf_fw_nvram_free(void *nvram);
  */
 int brcmf_fw_get_firmwares_pcie(struct device *dev, u16 flags,
 				const char *code, const char *nvram,
-				void (*fw_cb)(struct device *dev,
+				void (*fw_cb)(struct device *dev, int err,
 					      const struct firmware *fw,
 					      void *nvram_image, u32 nvram_len),
 				u16 domain_nr, u16 bus_nr);
 int brcmf_fw_get_firmwares(struct device *dev, u16 flags,
 			   const char *code, const char *nvram,
-			   void (*fw_cb)(struct device *dev,
+			   void (*fw_cb)(struct device *dev, int err,
 					 const struct firmware *fw,
 					 void *nvram_image, u32 nvram_len));
 
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fweh.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fweh.h
index 5fba4b49f3b3..816f80ea925b 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fweh.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fweh.h
@@ -142,6 +142,16 @@ enum brcmf_fweh_event_code {
 #define BRCMF_E_STATUS_CS_ABORT			15
 #define BRCMF_E_STATUS_ERROR			16
 
+/* status field values for PSK_SUP event */
+#define BRCMF_E_STATUS_FWSUP_WAIT_M1		4
+#define BRCMF_E_STATUS_FWSUP_PREP_M2		5
+#define BRCMF_E_STATUS_FWSUP_COMPLETED		6
+#define BRCMF_E_STATUS_FWSUP_TIMEOUT		7
+#define BRCMF_E_STATUS_FWSUP_WAIT_M3		8
+#define BRCMF_E_STATUS_FWSUP_PREP_M4		9
+#define BRCMF_E_STATUS_FWSUP_WAIT_G1		10
+#define BRCMF_E_STATUS_FWSUP_PREP_G2		11
+
 /* reason field values in struct brcmf_event_msg */
 #define BRCMF_E_REASON_INITIAL_ASSOC		0
 #define BRCMF_E_REASON_LOW_RSSI			1
@@ -161,6 +171,26 @@ enum brcmf_fweh_event_code {
 #define BRCMF_E_REASON_TDLS_PEER_CONNECTED	1
 #define BRCMF_E_REASON_TDLS_PEER_DISCONNECTED	2
 
+/* reason field values for PSK_SUP event */
+#define BRCMF_E_REASON_FWSUP_OTHER		0
+#define BRCMF_E_REASON_FWSUP_DECRYPT_KEY_DATA	1
+#define BRCMF_E_REASON_FWSUP_BAD_UCAST_WEP128	2
+#define BRCMF_E_REASON_FWSUP_BAD_UCAST_WEP40	3
+#define BRCMF_E_REASON_FWSUP_UNSUP_KEY_LEN	4
+#define BRCMF_E_REASON_FWSUP_PW_KEY_CIPHER	5
+#define BRCMF_E_REASON_FWSUP_MSG3_TOO_MANY_IE	6
+#define BRCMF_E_REASON_FWSUP_MSG3_IE_MISMATCH	7
+#define BRCMF_E_REASON_FWSUP_NO_INSTALL_FLAG	8
+#define BRCMF_E_REASON_FWSUP_MSG3_NO_GTK	9
+#define BRCMF_E_REASON_FWSUP_GRP_KEY_CIPHER	10
+#define BRCMF_E_REASON_FWSUP_GRP_MSG1_NO_GTK	11
+#define BRCMF_E_REASON_FWSUP_GTK_DECRYPT_FAIL	12
+#define BRCMF_E_REASON_FWSUP_SEND_FAIL		13
+#define BRCMF_E_REASON_FWSUP_DEAUTH		14
+#define BRCMF_E_REASON_FWSUP_WPA_PSK_TMO	15
+#define BRCMF_E_REASON_FWSUP_WPA_PSK_M1_TMO	16
+#define BRCMF_E_REASON_FWSUP_WPA_PSK_M3_TMO	17
+
 /* action field values for brcmf_ifevent */
 #define BRCMF_E_IF_ADD				1
 #define BRCMF_E_IF_DEL				2
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil.h
index 3a9a76dd9222..63b1287e2e6d 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil.h
@@ -85,6 +85,7 @@
 #define BRCMF_C_SET_SCAN_PASSIVE_TIME		258
 #define BRCMF_C_GET_VAR				262
 #define BRCMF_C_SET_VAR				263
+#define BRCMF_C_SET_WSEC_PMK			268
 
 s32 brcmf_fil_cmd_data_set(struct brcmf_if *ifp, u32 cmd, void *data, u32 len);
 s32 brcmf_fil_cmd_data_get(struct brcmf_if *ifp, u32 cmd, void *data, u32 len);
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h
index 9a1eb5ab6c4b..8391989b1882 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h
@@ -45,6 +45,9 @@
 #define BRCMF_SCAN_PARAMS_COUNT_MASK	0x0000ffff
 #define BRCMF_SCAN_PARAMS_NSSID_SHIFT	16
 
+#define BRCMF_WSEC_MAX_PSK_LEN		32
+#define	BRCMF_WSEC_PASSPHRASE		BIT(0)
+
 /* primary (ie tx) key */
 #define BRCMF_PRIMARY_KEY		(1 << 1)
 #define DOT11_BSSTYPE_ANY		2
@@ -470,6 +473,19 @@ struct brcmf_wsec_key_le {
 	u8 ea[ETH_ALEN];	/* per station */
 };
 
+/**
+ * struct brcmf_wsec_pmk_le - firmware pmk material.
+ *
+ * @key_len: number of octets in key material.
+ * @flags: key handling qualifiers.
+ * @key: PMK key material.
+ */
+struct brcmf_wsec_pmk_le {
+	__le16  key_len;
+	__le16  flags;
+	u8 key[2 * BRCMF_WSEC_MAX_PSK_LEN + 1];
+};
+
 /* Used to get specific STA parameters */
 struct brcmf_scb_val_le {
 	__le32 val;
@@ -806,6 +822,17 @@ struct brcmf_pno_macaddr_le {
 };
 
 /**
+ * struct brcmf_pno_bssid_le - bssid configuration for PNO scan.
+ *
+ * @bssid: BSS network identifier.
+ * @flags: flags for this BSSID.
+ */
+struct brcmf_pno_bssid_le {
+	u8 bssid[ETH_ALEN];
+	__le16 flags;
+};
+
+/**
  * struct brcmf_pktcnt_le - packet counters.
  *
  * @rx_good_pkt: packets (MSDUs & MMPDUs) received from this station
@@ -835,4 +862,69 @@ struct brcmf_gtk_keyinfo_le {
 	u8 replay_counter[BRCMF_RSN_REPLAY_LEN];
 };
 
+#define BRCMF_PNO_REPORT_NO_BATCH	BIT(2)
+
+/**
+ * struct brcmf_gscan_bucket_config - configuration data for channel bucket.
+ *
+ * @bucket_end_index: last channel index in @channel_list in
+ *	@struct brcmf_pno_config_le.
+ * @bucket_freq_multiple: scan interval expressed in N * @scan_freq.
+ * @flag: channel bucket report flags.
+ * @reserved: for future use.
+ * @repeat: number of scan at interval for exponential scan.
+ * @max_freq_multiple: maximum scan interval for exponential scan.
+ */
+struct brcmf_gscan_bucket_config {
+	u8 bucket_end_index;
+	u8 bucket_freq_multiple;
+	u8 flag;
+	u8 reserved;
+	__le16 repeat;
+	__le16 max_freq_multiple;
+};
+
+/* version supported which must match firmware */
+#define BRCMF_GSCAN_CFG_VERSION                     2
+
+/**
+ * enum brcmf_gscan_cfg_flags - bit values for gscan flags.
+ *
+ * @BRCMF_GSCAN_CFG_FLAGS_ALL_RESULTS: send probe responses/beacons to host.
+ * @BRCMF_GSCAN_CFG_ALL_BUCKETS_IN_1ST_SCAN: all buckets will be included in
+ *	first scan cycle.
+ * @BRCMF_GSCAN_CFG_FLAGS_CHANGE_ONLY: indicated only flags member is changed.
+ */
+enum brcmf_gscan_cfg_flags {
+	BRCMF_GSCAN_CFG_FLAGS_ALL_RESULTS = BIT(0),
+	BRCMF_GSCAN_CFG_ALL_BUCKETS_IN_1ST_SCAN = BIT(3),
+	BRCMF_GSCAN_CFG_FLAGS_CHANGE_ONLY = BIT(7),
+};
+
+/**
+ * struct brcmf_gscan_config - configuration data for gscan.
+ *
+ * @version: version of the api to match firmware.
+ * @flags: flags according %enum brcmf_gscan_cfg_flags.
+ * @buffer_threshold: percentage threshold of buffer to generate an event.
+ * @swc_nbssid_threshold: number of BSSIDs with significant change that
+ *	will generate an event.
+ * @swc_rssi_window_size: size of rssi cache buffer (max=8).
+ * @count_of_channel_buckets: number of array members in @bucket.
+ * @retry_threshold: !unknown!
+ * @lost_ap_window: !unknown!
+ * @bucket: array of channel buckets.
+ */
+struct brcmf_gscan_config {
+	__le16 version;
+	u8 flags;
+	u8 buffer_threshold;
+	u8 swc_nbssid_threshold;
+	u8 swc_rssi_window_size;
+	u8 count_of_channel_buckets;
+	u8 retry_threshold;
+	__le16  lost_ap_window;
+	struct brcmf_gscan_bucket_config bucket[1];
+};
+
 #endif /* FWIL_TYPES_H_ */
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwsignal.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwsignal.c
index 72373e59308e..f59642b2c935 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwsignal.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwsignal.c
@@ -2145,7 +2145,7 @@ void brcmf_fws_add_interface(struct brcmf_if *ifp)
 	struct brcmf_fws_info *fws = drvr_to_fws(ifp->drvr);
 	struct brcmf_fws_mac_descriptor *entry;
 
-	if (!ifp->ndev || fws->fcmode == BRCMF_FWS_FCMODE_NONE)
+	if (!ifp->ndev || !brcmf_fws_queue_skbs(fws))
 		return;
 
 	entry = &fws->desc.iface[ifp->ifidx];
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/p2p.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/p2p.c
index aa299c47bfa2..2ce675ab40ef 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/p2p.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/p2p.c
@@ -2208,6 +2208,7 @@ struct wireless_dev *brcmf_p2p_add_vif(struct wiphy *wiphy, const char *name,
 	err = brcmf_net_attach(ifp, true);
 	if (err) {
 		brcmf_err("Registering netdevice failed\n");
+		free_netdev(ifp->ndev);
 		goto fail;
 	}
 
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pcie.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pcie.c
index f36b96dc6acd..f878706613e6 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pcie.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pcie.c
@@ -1650,16 +1650,23 @@ static const struct brcmf_buscore_ops brcmf_pcie_buscore_ops = {
 	.write32 = brcmf_pcie_buscore_write32,
 };
 
-static void brcmf_pcie_setup(struct device *dev, const struct firmware *fw,
+static void brcmf_pcie_setup(struct device *dev, int ret,
+			     const struct firmware *fw,
 			     void *nvram, u32 nvram_len)
 {
-	struct brcmf_bus *bus = dev_get_drvdata(dev);
-	struct brcmf_pciedev *pcie_bus_dev = bus->bus_priv.pcie;
-	struct brcmf_pciedev_info *devinfo = pcie_bus_dev->devinfo;
+	struct brcmf_bus *bus;
+	struct brcmf_pciedev *pcie_bus_dev;
+	struct brcmf_pciedev_info *devinfo;
 	struct brcmf_commonring **flowrings;
-	int ret;
 	u32 i;
 
+	/* check firmware loading result */
+	if (ret)
+		goto fail;
+
+	bus = dev_get_drvdata(dev);
+	pcie_bus_dev = bus->bus_priv.pcie;
+	devinfo = pcie_bus_dev->devinfo;
 	brcmf_pcie_attach(devinfo);
 
 	/* Some of the firmwares have the size of the memory of the device
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.c
index 6c3bde83d070..ffa243e2e2d0 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.c
@@ -14,6 +14,7 @@
  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 #include <linux/netdevice.h>
+#include <linux/gcd.h>
 #include <net/cfg80211.h>
 
 #include "core.h"
@@ -35,6 +36,67 @@
 #define BRCMF_PNO_HIDDEN_BIT		2
 #define BRCMF_PNO_SCHED_SCAN_PERIOD	30
 
+#define BRCMF_PNO_MAX_BUCKETS		16
+#define GSCAN_BATCH_NO_THR_SET			101
+#define GSCAN_RETRY_THRESHOLD			3
+
+struct brcmf_pno_info {
+	int n_reqs;
+	struct cfg80211_sched_scan_request *reqs[BRCMF_PNO_MAX_BUCKETS];
+	struct mutex req_lock;
+};
+
+#define ifp_to_pno(_ifp)	((_ifp)->drvr->config->pno)
+
+static int brcmf_pno_store_request(struct brcmf_pno_info *pi,
+				   struct cfg80211_sched_scan_request *req)
+{
+	if (WARN(pi->n_reqs == BRCMF_PNO_MAX_BUCKETS,
+		 "pno request storage full\n"))
+		return -ENOSPC;
+
+	brcmf_dbg(SCAN, "reqid=%llu\n", req->reqid);
+	mutex_lock(&pi->req_lock);
+	pi->reqs[pi->n_reqs++] = req;
+	mutex_unlock(&pi->req_lock);
+	return 0;
+}
+
+static int brcmf_pno_remove_request(struct brcmf_pno_info *pi, u64 reqid)
+{
+	int i, err = 0;
+
+	mutex_lock(&pi->req_lock);
+
+	/* find request */
+	for (i = 0; i < pi->n_reqs; i++) {
+		if (pi->reqs[i]->reqid == reqid)
+			break;
+	}
+	/* request not found */
+	if (WARN(i == pi->n_reqs, "reqid not found\n")) {
+		err = -ENOENT;
+		goto done;
+	}
+
+	brcmf_dbg(SCAN, "reqid=%llu\n", reqid);
+	pi->n_reqs--;
+
+	/* if last we are done */
+	if (!pi->n_reqs || i == pi->n_reqs)
+		goto done;
+
+	/* fill the gap with remaining requests */
+	while (i <= pi->n_reqs - 1) {
+		pi->reqs[i] = pi->reqs[i + 1];
+		i++;
+	}
+
+done:
+	mutex_unlock(&pi->req_lock);
+	return err;
+}
+
 static int brcmf_pno_channel_config(struct brcmf_if *ifp,
 				    struct brcmf_pno_config_le *cfg)
 {
@@ -57,16 +119,11 @@ static int brcmf_pno_config(struct brcmf_if *ifp, u32 scan_freq,
 
 	/* set extra pno params */
 	flags = BIT(BRCMF_PNO_IMMEDIATE_SCAN_BIT) |
-		BIT(BRCMF_PNO_REPORT_SEPARATELY_BIT) |
 		BIT(BRCMF_PNO_ENABLE_ADAPTSCAN_BIT);
 	pfn_param.repeat = BRCMF_PNO_REPEAT;
 	pfn_param.exp = BRCMF_PNO_FREQ_EXPO_MAX;
 
 	/* set up pno scan fr */
-	if (scan_freq < BRCMF_PNO_SCHED_SCAN_MIN_PERIOD) {
-		brcmf_dbg(SCAN, "scan period too small, using minimum\n");
-		scan_freq = BRCMF_PNO_SCHED_SCAN_MIN_PERIOD;
-	}
 	pfn_param.scan_freq = cpu_to_le32(scan_freq);
 
 	if (mscan) {
@@ -101,12 +158,24 @@ exit:
 	return err;
 }
 
-static int brcmf_pno_set_random(struct brcmf_if *ifp, u8 *mac_addr,
-				u8 *mac_mask)
+static int brcmf_pno_set_random(struct brcmf_if *ifp, struct brcmf_pno_info *pi)
 {
 	struct brcmf_pno_macaddr_le pfn_mac;
+	u8 *mac_addr = NULL;
+	u8 *mac_mask = NULL;
 	int err, i;
 
+	for (i = 0; i < pi->n_reqs; i++)
+		if (pi->reqs[i]->flags & NL80211_SCAN_FLAG_RANDOM_ADDR) {
+			mac_addr = pi->reqs[i]->mac_addr;
+			mac_mask = pi->reqs[i]->mac_addr_mask;
+			break;
+		}
+
+	/* no random mac requested */
+	if (!mac_addr)
+		return 0;
+
 	pfn_mac.version = BRCMF_PFN_MACADDR_CFG_VER;
 	pfn_mac.flags = BRCMF_PFN_MAC_OUI_ONLY | BRCMF_PFN_SET_MAC_UNASSOC;
 
@@ -120,6 +189,8 @@ static int brcmf_pno_set_random(struct brcmf_if *ifp, u8 *mac_addr,
 	/* Set locally administered */
 	pfn_mac.mac[0] |= 0x02;
 
+	brcmf_dbg(SCAN, "enabling random mac: reqid=%llu mac=%pM\n",
+		  pi->reqs[i]->reqid, pfn_mac.mac);
 	err = brcmf_fil_iovar_data_set(ifp, "pfn_macaddr", &pfn_mac,
 				       sizeof(pfn_mac));
 	if (err)
@@ -132,6 +203,7 @@ static int brcmf_pno_add_ssid(struct brcmf_if *ifp, struct cfg80211_ssid *ssid,
 			      bool active)
 {
 	struct brcmf_pno_net_param_le pfn;
+	int err;
 
 	pfn.auth = cpu_to_le32(WLAN_AUTH_OPEN);
 	pfn.wpa_auth = cpu_to_le32(BRCMF_PNO_WPA_AUTH_ANY);
@@ -142,7 +214,28 @@ static int brcmf_pno_add_ssid(struct brcmf_if *ifp, struct cfg80211_ssid *ssid,
 		pfn.flags = cpu_to_le32(1 << BRCMF_PNO_HIDDEN_BIT);
 	pfn.ssid.SSID_len = cpu_to_le32(ssid->ssid_len);
 	memcpy(pfn.ssid.SSID, ssid->ssid, ssid->ssid_len);
-	return brcmf_fil_iovar_data_set(ifp, "pfn_add", &pfn, sizeof(pfn));
+
+	brcmf_dbg(SCAN, "adding ssid=%.32s (active=%d)\n", ssid->ssid, active);
+	err = brcmf_fil_iovar_data_set(ifp, "pfn_add", &pfn, sizeof(pfn));
+	if (err < 0)
+		brcmf_err("adding failed: err=%d\n", err);
+	return err;
+}
+
+static int brcmf_pno_add_bssid(struct brcmf_if *ifp, const u8 *bssid)
+{
+	struct brcmf_pno_bssid_le bssid_cfg;
+	int err;
+
+	memcpy(bssid_cfg.bssid, bssid, ETH_ALEN);
+	bssid_cfg.flags = 0;
+
+	brcmf_dbg(SCAN, "adding bssid=%pM\n", bssid);
+	err = brcmf_fil_iovar_data_set(ifp, "pfn_add_bssid", &bssid_cfg,
+				       sizeof(bssid_cfg));
+	if (err < 0)
+		brcmf_err("adding failed: err=%d\n", err);
+	return err;
 }
 
 static bool brcmf_is_ssid_active(struct cfg80211_ssid *ssid,
@@ -163,7 +256,7 @@ static bool brcmf_is_ssid_active(struct cfg80211_ssid *ssid,
 	return false;
 }
 
-int brcmf_pno_clean(struct brcmf_if *ifp)
+static int brcmf_pno_clean(struct brcmf_if *ifp)
 {
 	int ret;
 
@@ -179,63 +272,320 @@ int brcmf_pno_clean(struct brcmf_if *ifp)
 	return ret;
 }
 
-int brcmf_pno_start_sched_scan(struct brcmf_if *ifp,
-			       struct cfg80211_sched_scan_request *req)
+static int brcmf_pno_get_bucket_channels(struct cfg80211_sched_scan_request *r,
+					 struct brcmf_pno_config_le *pno_cfg)
 {
-	struct brcmf_pno_config_le pno_cfg;
-	struct cfg80211_ssid *ssid;
+	u32 n_chan = le32_to_cpu(pno_cfg->channel_num);
 	u16 chan;
-	int i, ret;
+	int i, err = 0;
 
-	/* clean up everything */
-	ret = brcmf_pno_clean(ifp);
-	if  (ret < 0) {
-		brcmf_err("failed error=%d\n", ret);
-		return ret;
+	for (i = 0; i < r->n_channels; i++) {
+		if (n_chan >= BRCMF_NUMCHANNELS) {
+			err = -ENOSPC;
+			goto done;
+		}
+		chan = r->channels[i]->hw_value;
+		brcmf_dbg(SCAN, "[%d] Chan : %u\n", n_chan, chan);
+		pno_cfg->channel_list[n_chan++] = cpu_to_le16(chan);
 	}
+	/* return number of channels */
+	err = n_chan;
+done:
+	pno_cfg->channel_num = cpu_to_le32(n_chan);
+	return err;
+}
 
-	/* configure pno */
-	ret = brcmf_pno_config(ifp, req->scan_plans[0].interval, 0, 0);
-	if (ret < 0)
-		return ret;
-
-	/* configure random mac */
-	if (req->flags & NL80211_SCAN_FLAG_RANDOM_ADDR) {
-		ret = brcmf_pno_set_random(ifp, req->mac_addr,
-					   req->mac_addr_mask);
-		if (ret < 0)
-			return ret;
+static int brcmf_pno_prep_fwconfig(struct brcmf_pno_info *pi,
+				   struct brcmf_pno_config_le *pno_cfg,
+				   struct brcmf_gscan_bucket_config **buckets,
+				   u32 *scan_freq)
+{
+	struct cfg80211_sched_scan_request *sr;
+	struct brcmf_gscan_bucket_config *fw_buckets;
+	int i, err, chidx;
+
+	brcmf_dbg(SCAN, "n_reqs=%d\n", pi->n_reqs);
+	if (WARN_ON(!pi->n_reqs))
+		return -ENODATA;
+
+	/*
+	 * actual scan period is determined using gcd() for each
+	 * scheduled scan period.
+	 */
+	*scan_freq = pi->reqs[0]->scan_plans[0].interval;
+	for (i = 1; i < pi->n_reqs; i++) {
+		sr = pi->reqs[i];
+		*scan_freq = gcd(sr->scan_plans[0].interval, *scan_freq);
+	}
+	if (*scan_freq < BRCMF_PNO_SCHED_SCAN_MIN_PERIOD) {
+		brcmf_dbg(SCAN, "scan period too small, using minimum\n");
+		*scan_freq = BRCMF_PNO_SCHED_SCAN_MIN_PERIOD;
 	}
 
-	/* configure channels to use */
-	for (i = 0; i < req->n_channels; i++) {
-		chan = req->channels[i]->hw_value;
-		pno_cfg.channel_list[i] = cpu_to_le16(chan);
+	*buckets = NULL;
+	fw_buckets = kcalloc(pi->n_reqs, sizeof(*fw_buckets), GFP_KERNEL);
+	if (!fw_buckets)
+		return -ENOMEM;
+
+	memset(pno_cfg, 0, sizeof(*pno_cfg));
+	for (i = 0; i < pi->n_reqs; i++) {
+		sr = pi->reqs[i];
+		chidx = brcmf_pno_get_bucket_channels(sr, pno_cfg);
+		if (chidx < 0) {
+			err = chidx;
+			goto fail;
+		}
+		fw_buckets[i].bucket_end_index = chidx - 1;
+		fw_buckets[i].bucket_freq_multiple =
+			sr->scan_plans[0].interval / *scan_freq;
+		/* assure period is non-zero */
+		if (!fw_buckets[i].bucket_freq_multiple)
+			fw_buckets[i].bucket_freq_multiple = 1;
+		fw_buckets[i].flag = BRCMF_PNO_REPORT_NO_BATCH;
 	}
-	if (req->n_channels) {
-		pno_cfg.channel_num = cpu_to_le32(req->n_channels);
-		brcmf_pno_channel_config(ifp, &pno_cfg);
+
+	if (BRCMF_SCAN_ON()) {
+		brcmf_err("base period=%u\n", *scan_freq);
+		for (i = 0; i < pi->n_reqs; i++) {
+			brcmf_err("[%d] period %u max %u repeat %u flag %x idx %u\n",
+				  i, fw_buckets[i].bucket_freq_multiple,
+				  le16_to_cpu(fw_buckets[i].max_freq_multiple),
+				  fw_buckets[i].repeat, fw_buckets[i].flag,
+				  fw_buckets[i].bucket_end_index);
+		}
 	}
+	*buckets = fw_buckets;
+	return pi->n_reqs;
 
-	/* configure each match set */
-	for (i = 0; i < req->n_match_sets; i++) {
-		ssid = &req->match_sets[i].ssid;
-		if (!ssid->ssid_len) {
-			brcmf_err("skip broadcast ssid\n");
-			continue;
+fail:
+	kfree(fw_buckets);
+	return err;
+}
+
+static int brcmf_pno_config_networks(struct brcmf_if *ifp,
+				     struct brcmf_pno_info *pi)
+{
+	struct cfg80211_sched_scan_request *r;
+	struct cfg80211_match_set *ms;
+	bool active;
+	int i, j, err = 0;
+
+	for (i = 0; i < pi->n_reqs; i++) {
+		r = pi->reqs[i];
+
+		for (j = 0; j < r->n_match_sets; j++) {
+			ms = &r->match_sets[j];
+			if (ms->ssid.ssid_len) {
+				active = brcmf_is_ssid_active(&ms->ssid, r);
+				err = brcmf_pno_add_ssid(ifp, &ms->ssid,
+							 active);
+			}
+			if (!err && is_valid_ether_addr(ms->bssid))
+				err = brcmf_pno_add_bssid(ifp, ms->bssid);
+
+			if (err < 0)
+				return err;
 		}
+	}
+	return 0;
+}
+
+static int brcmf_pno_config_sched_scans(struct brcmf_if *ifp)
+{
+	struct brcmf_pno_info *pi;
+	struct brcmf_gscan_config *gscan_cfg;
+	struct brcmf_gscan_bucket_config *buckets;
+	struct brcmf_pno_config_le pno_cfg;
+	size_t gsz;
+	u32 scan_freq;
+	int err, n_buckets;
+
+	pi = ifp_to_pno(ifp);
+	n_buckets = brcmf_pno_prep_fwconfig(pi, &pno_cfg, &buckets,
+					    &scan_freq);
+	if (n_buckets < 0)
+		return n_buckets;
+
+	gsz = sizeof(*gscan_cfg) + (n_buckets - 1) * sizeof(*buckets);
+	gscan_cfg = kzalloc(gsz, GFP_KERNEL);
+	if (!gscan_cfg) {
+		err = -ENOMEM;
+		goto free_buckets;
+	}
 
-		ret = brcmf_pno_add_ssid(ifp, ssid,
-					 brcmf_is_ssid_active(ssid, req));
-		if (ret < 0)
-			brcmf_dbg(SCAN, ">>> PNO filter %s for ssid (%s)\n",
-				  ret == 0 ? "set" : "failed", ssid->ssid);
+	/* clean up everything */
+	err = brcmf_pno_clean(ifp);
+	if  (err < 0) {
+		brcmf_err("failed error=%d\n", err);
+		goto free_gscan;
 	}
+
+	/* configure pno */
+	err = brcmf_pno_config(ifp, scan_freq, 0, 0);
+	if (err < 0)
+		goto free_gscan;
+
+	err = brcmf_pno_channel_config(ifp, &pno_cfg);
+	if (err < 0)
+		goto clean;
+
+	gscan_cfg->version = cpu_to_le16(BRCMF_GSCAN_CFG_VERSION);
+	gscan_cfg->retry_threshold = GSCAN_RETRY_THRESHOLD;
+	gscan_cfg->buffer_threshold = GSCAN_BATCH_NO_THR_SET;
+	gscan_cfg->flags = BRCMF_GSCAN_CFG_ALL_BUCKETS_IN_1ST_SCAN;
+
+	gscan_cfg->count_of_channel_buckets = n_buckets;
+	memcpy(&gscan_cfg->bucket[0], buckets,
+	       n_buckets * sizeof(*buckets));
+
+	err = brcmf_fil_iovar_data_set(ifp, "pfn_gscan_cfg", gscan_cfg, gsz);
+
+	if (err < 0)
+		goto clean;
+
+	/* configure random mac */
+	err = brcmf_pno_set_random(ifp, pi);
+	if (err < 0)
+		goto clean;
+
+	err = brcmf_pno_config_networks(ifp, pi);
+	if (err < 0)
+		goto clean;
+
 	/* Enable the PNO */
-	ret = brcmf_fil_iovar_int_set(ifp, "pfn", 1);
+	err = brcmf_fil_iovar_int_set(ifp, "pfn", 1);
+
+clean:
+	if (err < 0)
+		brcmf_pno_clean(ifp);
+free_gscan:
+	kfree(gscan_cfg);
+free_buckets:
+	kfree(buckets);
+	return err;
+}
+
+int brcmf_pno_start_sched_scan(struct brcmf_if *ifp,
+			       struct cfg80211_sched_scan_request *req)
+{
+	struct brcmf_pno_info *pi;
+	int ret;
+
+	brcmf_dbg(TRACE, "reqid=%llu\n", req->reqid);
+
+	pi = ifp_to_pno(ifp);
+	ret = brcmf_pno_store_request(pi, req);
 	if (ret < 0)
-		brcmf_err("PNO enable failed!! ret=%d\n", ret);
+		return ret;
 
-	return ret;
+	ret = brcmf_pno_config_sched_scans(ifp);
+	if (ret < 0) {
+		brcmf_pno_remove_request(pi, req->reqid);
+		if (pi->n_reqs)
+			(void)brcmf_pno_config_sched_scans(ifp);
+		return ret;
+	}
+	return 0;
 }
 
+int brcmf_pno_stop_sched_scan(struct brcmf_if *ifp, u64 reqid)
+{
+	struct brcmf_pno_info *pi;
+	int err;
+
+	brcmf_dbg(TRACE, "reqid=%llu\n", reqid);
+
+	pi = ifp_to_pno(ifp);
+	err = brcmf_pno_remove_request(pi, reqid);
+	if (err)
+		return err;
+
+	brcmf_pno_clean(ifp);
+
+	if (pi->n_reqs)
+		(void)brcmf_pno_config_sched_scans(ifp);
+
+	return 0;
+}
+
+int brcmf_pno_attach(struct brcmf_cfg80211_info *cfg)
+{
+	struct brcmf_pno_info *pi;
+
+	brcmf_dbg(TRACE, "enter\n");
+	pi = kzalloc(sizeof(*pi), GFP_KERNEL);
+	if (!pi)
+		return -ENOMEM;
+
+	cfg->pno = pi;
+	mutex_init(&pi->req_lock);
+	return 0;
+}
+
+void brcmf_pno_detach(struct brcmf_cfg80211_info *cfg)
+{
+	struct brcmf_pno_info *pi;
+
+	brcmf_dbg(TRACE, "enter\n");
+	pi = cfg->pno;
+	cfg->pno = NULL;
+
+	WARN_ON(pi->n_reqs);
+	mutex_destroy(&pi->req_lock);
+	kfree(pi);
+}
+
+void brcmf_pno_wiphy_params(struct wiphy *wiphy, bool gscan)
+{
+	/* scheduled scan settings */
+	wiphy->max_sched_scan_reqs = gscan ? BRCMF_PNO_MAX_BUCKETS : 1;
+	wiphy->max_sched_scan_ssids = BRCMF_PNO_MAX_PFN_COUNT;
+	wiphy->max_match_sets = BRCMF_PNO_MAX_PFN_COUNT;
+	wiphy->max_sched_scan_ie_len = BRCMF_SCAN_IE_LEN_MAX;
+	wiphy->max_sched_scan_plan_interval = BRCMF_PNO_SCHED_SCAN_MAX_PERIOD;
+}
+
+u64 brcmf_pno_find_reqid_by_bucket(struct brcmf_pno_info *pi, u32 bucket)
+{
+	u64 reqid = 0;
+
+	mutex_lock(&pi->req_lock);
+
+	if (bucket < pi->n_reqs)
+		reqid = pi->reqs[bucket]->reqid;
+
+	mutex_unlock(&pi->req_lock);
+	return reqid;
+}
+
+u32 brcmf_pno_get_bucket_map(struct brcmf_pno_info *pi,
+			     struct brcmf_pno_net_info_le *ni)
+{
+	struct cfg80211_sched_scan_request *req;
+	struct cfg80211_match_set *ms;
+	u32 bucket_map = 0;
+	int i, j;
+
+	mutex_lock(&pi->req_lock);
+	for (i = 0; i < pi->n_reqs; i++) {
+		req = pi->reqs[i];
+
+		if (!req->n_match_sets)
+			continue;
+		for (j = 0; j < req->n_match_sets; j++) {
+			ms = &req->match_sets[j];
+			if (ms->ssid.ssid_len == ni->SSID_len &&
+			    !memcmp(ms->ssid.ssid, ni->SSID, ni->SSID_len)) {
+				bucket_map |= BIT(i);
+				break;
+			}
+			if (is_valid_ether_addr(ms->bssid) &&
+			    !memcmp(ms->bssid, ni->bssid, ETH_ALEN)) {
+				bucket_map |= BIT(i);
+				break;
+			}
+		}
+	}
+	mutex_unlock(&pi->req_lock);
+	return bucket_map;
+}
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.h
index bae55b2af78c..cd9e35ae3b21 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.h
@@ -21,12 +21,8 @@
 #define BRCMF_PNO_SCHED_SCAN_MIN_PERIOD	10
 #define BRCMF_PNO_SCHED_SCAN_MAX_PERIOD	508
 
-/**
- * brcmf_pno_clean - disable and clear pno in firmware.
- *
- * @ifp: interface object used.
- */
-int brcmf_pno_clean(struct brcmf_if *ifp);
+/* forward declaration */
+struct brcmf_pno_info;
 
 /**
  * brcmf_pno_start_sched_scan - initiate scheduled scan on device.
@@ -37,4 +33,51 @@ int brcmf_pno_clean(struct brcmf_if *ifp);
 int brcmf_pno_start_sched_scan(struct brcmf_if *ifp,
 			       struct cfg80211_sched_scan_request *req);
 
+/**
+ * brcmf_pno_stop_sched_scan - terminate scheduled scan on device.
+ *
+ * @ifp: interface object used.
+ * @reqid: unique identifier of scan to be stopped.
+ */
+int brcmf_pno_stop_sched_scan(struct brcmf_if *ifp, u64 reqid);
+
+/**
+ * brcmf_pno_wiphy_params - fill scheduled scan parameters in wiphy instance.
+ *
+ * @wiphy: wiphy instance to be used.
+ * @gscan: indicates whether the device has support for g-scan feature.
+ */
+void brcmf_pno_wiphy_params(struct wiphy *wiphy, bool gscan);
+
+/**
+ * brcmf_pno_attach - allocate and attach module information.
+ *
+ * @cfg: cfg80211 context used.
+ */
+int brcmf_pno_attach(struct brcmf_cfg80211_info *cfg);
+
+/**
+ * brcmf_pno_detach - detach and free module information.
+ *
+ * @cfg: cfg80211 context used.
+ */
+void brcmf_pno_detach(struct brcmf_cfg80211_info *cfg);
+
+/**
+ * brcmf_pno_find_reqid_by_bucket - find request id for given bucket index.
+ *
+ * @pi: pno instance used.
+ * @bucket: index of firmware bucket.
+ */
+u64 brcmf_pno_find_reqid_by_bucket(struct brcmf_pno_info *pi, u32 bucket);
+
+/**
+ * brcmf_pno_get_bucket_map - determine bucket map for given netinfo.
+ *
+ * @pi: pno instance used.
+ * @netinfo: netinfo to compare with bucket configuration.
+ */
+u32 brcmf_pno_get_bucket_map(struct brcmf_pno_info *pi,
+			     struct brcmf_pno_net_info_le *netinfo);
+
 #endif /* _BRCMF_PNO_H */
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/sdio.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/sdio.c
index e03450059b06..fbcbb4325936 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/sdio.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/sdio.c
@@ -612,7 +612,9 @@ BRCMF_FW_NVRAM_DEF(43340, "brcmfmac43340-sdio.bin", "brcmfmac43340-sdio.txt");
 BRCMF_FW_NVRAM_DEF(4335, "brcmfmac4335-sdio.bin", "brcmfmac4335-sdio.txt");
 BRCMF_FW_NVRAM_DEF(43362, "brcmfmac43362-sdio.bin", "brcmfmac43362-sdio.txt");
 BRCMF_FW_NVRAM_DEF(4339, "brcmfmac4339-sdio.bin", "brcmfmac4339-sdio.txt");
-BRCMF_FW_NVRAM_DEF(43430, "brcmfmac43430-sdio.bin", "brcmfmac43430-sdio.txt");
+BRCMF_FW_NVRAM_DEF(43430A0, "brcmfmac43430a0-sdio.bin", "brcmfmac43430a0-sdio.txt");
+/* Note the names are not postfixed with a1 for backward compatibility */
+BRCMF_FW_NVRAM_DEF(43430A1, "brcmfmac43430-sdio.bin", "brcmfmac43430-sdio.txt");
 BRCMF_FW_NVRAM_DEF(43455, "brcmfmac43455-sdio.bin", "brcmfmac43455-sdio.txt");
 BRCMF_FW_NVRAM_DEF(4354, "brcmfmac4354-sdio.bin", "brcmfmac4354-sdio.txt");
 BRCMF_FW_NVRAM_DEF(4356, "brcmfmac4356-sdio.bin", "brcmfmac4356-sdio.txt");
@@ -630,7 +632,8 @@ static struct brcmf_firmware_mapping brcmf_sdio_fwnames[] = {
 	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4335_CHIP_ID, 0xFFFFFFFF, 4335),
 	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43362_CHIP_ID, 0xFFFFFFFE, 43362),
 	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4339_CHIP_ID, 0xFFFFFFFF, 4339),
-	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43430_CHIP_ID, 0xFFFFFFFF, 43430),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43430_CHIP_ID, 0x00000001, 43430A0),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43430_CHIP_ID, 0xFFFFFFFE, 43430A1),
 	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4345_CHIP_ID, 0xFFFFFFC0, 43455),
 	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4354_CHIP_ID, 0xFFFFFFFF, 4354),
 	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4356_CHIP_ID, 0xFFFFFFFF, 4356)
@@ -2034,6 +2037,7 @@ brcmf_sdio_wait_event_wakeup(struct brcmf_sdio *bus)
 
 static int brcmf_sdio_txpkt_hdalign(struct brcmf_sdio *bus, struct sk_buff *pkt)
 {
+	struct brcmf_bus_stats *stats;
 	u16 head_pad;
 	u8 *dat_buf;
 
@@ -2043,16 +2047,18 @@ static int brcmf_sdio_txpkt_hdalign(struct brcmf_sdio *bus, struct sk_buff *pkt)
 	head_pad = ((unsigned long)dat_buf % bus->head_align);
 	if (head_pad) {
 		if (skb_headroom(pkt) < head_pad) {
-			bus->sdiodev->bus_if->tx_realloc++;
-			head_pad = 0;
-			if (skb_cow(pkt, head_pad))
+			stats = &bus->sdiodev->bus_if->stats;
+			atomic_inc(&stats->pktcowed);
+			if (skb_cow_head(pkt, head_pad)) {
+				atomic_inc(&stats->pktcow_failed);
 				return -ENOMEM;
+			}
 		}
 		skb_push(pkt, head_pad);
 		dat_buf = (u8 *)(pkt->data);
-		memset(dat_buf, 0, head_pad + bus->tx_hdrlen);
 	}
-	return head_pad;
+	memset(dat_buf, 0, head_pad + bus->tx_hdrlen);
+	return 0;
 }
 
 /**
@@ -3982,21 +3988,26 @@ static const struct brcmf_bus_ops brcmf_sdio_bus_ops = {
 	.get_memdump = brcmf_sdio_bus_get_memdump,
 };
 
-static void brcmf_sdio_firmware_callback(struct device *dev,
+static void brcmf_sdio_firmware_callback(struct device *dev, int err,
 					 const struct firmware *code,
 					 void *nvram, u32 nvram_len)
 {
-	struct brcmf_bus *bus_if = dev_get_drvdata(dev);
-	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
-	struct brcmf_sdio *bus = sdiodev->bus;
-	int err = 0;
+	struct brcmf_bus *bus_if;
+	struct brcmf_sdio_dev *sdiodev;
+	struct brcmf_sdio *bus;
 	u8 saveclk;
 
-	brcmf_dbg(TRACE, "Enter: dev=%s\n", dev_name(dev));
+	brcmf_dbg(TRACE, "Enter: dev=%s, err=%d\n", dev_name(dev), err);
+	bus_if = dev_get_drvdata(dev);
+	sdiodev = bus_if->bus_priv.sdio;
+	if (err)
+		goto fail;
 
 	if (!bus_if->drvr)
 		return;
 
+	bus = sdiodev->bus;
+
 	/* try to download image and nvram to the dongle */
 	bus->alp_only = true;
 	err = brcmf_sdio_download_firmware(bus, code, nvram, nvram_len);
@@ -4083,6 +4094,7 @@ release:
 fail:
 	brcmf_dbg(TRACE, "failed: dev=%s, err=%d\n", dev_name(dev), err);
 	device_release_driver(dev);
+	device_release_driver(&sdiodev->func[2]->dev);
 }
 
 struct brcmf_sdio *brcmf_sdio_probe(struct brcmf_sdio_dev *sdiodev)
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/usb.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/usb.c
index e4d545f9edee..0eea48e73331 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/usb.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/usb.c
@@ -1159,17 +1159,18 @@ fail:
 	return ret;
 }
 
-static void brcmf_usb_probe_phase2(struct device *dev,
+static void brcmf_usb_probe_phase2(struct device *dev, int ret,
 				   const struct firmware *fw,
 				   void *nvram, u32 nvlen)
 {
 	struct brcmf_bus *bus = dev_get_drvdata(dev);
-	struct brcmf_usbdev_info *devinfo;
-	int ret;
+	struct brcmf_usbdev_info *devinfo = bus->bus_priv.usb->devinfo;
+
+	if (ret)
+		goto error;
 
 	brcmf_dbg(USB, "Start fw downloading\n");
 
-	devinfo = bus->bus_priv.usb->devinfo;
 	ret = check_file(fw->data);
 	if (ret < 0) {
 		brcmf_err("invalid firmware\n");
diff --git a/drivers/net/wireless/cisco/airo.c b/drivers/net/wireless/cisco/airo.c
index 1b7e125a28e2..84143a02adce 100644
--- a/drivers/net/wireless/cisco/airo.c
+++ b/drivers/net/wireless/cisco/airo.c
@@ -3066,7 +3066,7 @@ static int airo_thread(void *data) {
 		if (ai->jobs) {
 			locked = down_interruptible(&ai->sem);
 		} else {
-			wait_queue_t wait;
+			wait_queue_entry_t wait;
 
 			init_waitqueue_entry(&wait, current);
 			add_wait_queue(&ai->thr_wait, &wait);
@@ -3330,7 +3330,7 @@ static void airo_handle_rx(struct airo_info *ai)
 	}
 
 	skb_reserve(skb, 2); /* This way the IP header is aligned */
-	buffer = (__le16 *) skb_put(skb, len + hdrlen);
+	buffer = skb_put(skb, len + hdrlen);
 	if (test_bit(FLAG_802_11, &ai->flags)) {
 		buffer[0] = fc;
 		bap_read(ai, buffer + 1, hdrlen - 2, BAP0);
@@ -3734,7 +3734,7 @@ static void mpi_receive_802_11(struct airo_info *ai)
 		ai->dev->stats.rx_dropped++;
 		goto badrx;
 	}
-	buffer = (u16*)skb_put (skb, len + hdrlen);
+	buffer = skb_put(skb, len + hdrlen);
 	memcpy ((char *)buffer, ptr, hdrlen);
 	ptr += hdrlen;
 	if (hdrlen == 24)
diff --git a/drivers/net/wireless/intel/ipw2x00/ipw2100.c b/drivers/net/wireless/intel/ipw2x00/ipw2100.c
index f922859acf40..aaaca4d08e2b 100644
--- a/drivers/net/wireless/intel/ipw2x00/ipw2100.c
+++ b/drivers/net/wireless/intel/ipw2x00/ipw2100.c
@@ -4160,12 +4160,12 @@ static ssize_t show_bssinfo(struct device *d, struct device_attribute *attr,
 static DEVICE_ATTR(bssinfo, S_IRUGO, show_bssinfo, NULL);
 
 #ifdef CONFIG_IPW2100_DEBUG
-static ssize_t show_debug_level(struct device_driver *d, char *buf)
+static ssize_t debug_level_show(struct device_driver *d, char *buf)
 {
 	return sprintf(buf, "0x%08X\n", ipw2100_debug_level);
 }
 
-static ssize_t store_debug_level(struct device_driver *d,
+static ssize_t debug_level_store(struct device_driver *d,
 				 const char *buf, size_t count)
 {
 	u32 val;
@@ -4179,9 +4179,7 @@ static ssize_t store_debug_level(struct device_driver *d,
 
 	return strnlen(buf, count);
 }
-
-static DRIVER_ATTR(debug_level, S_IWUSR | S_IRUGO, show_debug_level,
-		   store_debug_level);
+static DRIVER_ATTR_RW(debug_level);
 #endif				/* CONFIG_IPW2100_DEBUG */
 
 static ssize_t show_fatal_error(struct device *d,
diff --git a/drivers/net/wireless/intel/ipw2x00/ipw2200.c b/drivers/net/wireless/intel/ipw2x00/ipw2200.c
index bbc579b647b6..9368abdf18e2 100644
--- a/drivers/net/wireless/intel/ipw2x00/ipw2200.c
+++ b/drivers/net/wireless/intel/ipw2x00/ipw2200.c
@@ -1195,12 +1195,12 @@ static void ipw_led_shutdown(struct ipw_priv *priv)
  *
  * See the level definitions in ipw for details.
  */
-static ssize_t show_debug_level(struct device_driver *d, char *buf)
+static ssize_t debug_level_show(struct device_driver *d, char *buf)
 {
 	return sprintf(buf, "0x%08X\n", ipw_debug_level);
 }
 
-static ssize_t store_debug_level(struct device_driver *d, const char *buf,
+static ssize_t debug_level_store(struct device_driver *d, const char *buf,
 				 size_t count)
 {
 	char *p = (char *)buf;
@@ -1221,9 +1221,7 @@ static ssize_t store_debug_level(struct device_driver *d, const char *buf,
 
 	return strnlen(buf, count);
 }
-
-static DRIVER_ATTR(debug_level, S_IWUSR | S_IRUGO,
-		   show_debug_level, store_debug_level);
+static DRIVER_ATTR_RW(debug_level);
 
 static inline u32 ipw_get_event_log_len(struct ipw_priv *priv)
 {
@@ -10274,8 +10272,9 @@ static int ipw_tx_skb(struct ipw_priv *priv, struct libipw_txb *txb,
 
 				printk(KERN_INFO "Adding frag %d %d...\n",
 				       j, size);
-				memcpy(skb_put(skb, size),
-				       txb->fragments[j]->data + hdr_len, size);
+				skb_put_data(skb,
+					     txb->fragments[j]->data + hdr_len,
+					     size);
 			}
 			dev_kfree_skb_any(txb->fragments[i]);
 			txb->fragments[i] = skb;
@@ -10370,7 +10369,7 @@ static void ipw_handle_promiscuous_tx(struct ipw_priv *priv,
 		if (!dst)
 			continue;
 
-		rt_hdr = (void *)skb_put(dst, sizeof(*rt_hdr));
+		rt_hdr = skb_put(dst, sizeof(*rt_hdr));
 
 		rt_hdr->it_version = PKTHDR_RADIOTAP_VERSION;
 		rt_hdr->it_pad = 0;
diff --git a/drivers/net/wireless/intel/ipw2x00/libipw_tx.c b/drivers/net/wireless/intel/ipw2x00/libipw_tx.c
index 048f1e3ada11..84205aa508df 100644
--- a/drivers/net/wireless/intel/ipw2x00/libipw_tx.c
+++ b/drivers/net/wireless/intel/ipw2x00/libipw_tx.c
@@ -359,7 +359,7 @@ netdev_tx_t libipw_xmit(struct sk_buff *skb, struct net_device *dev)
 			goto failed;
 
 		skb_reserve(skb_new, crypt->ops->extra_msdu_prefix_len);
-		memcpy(skb_put(skb_new, hdr_len), &header, hdr_len);
+		skb_put_data(skb_new, &header, hdr_len);
 		snapped = 1;
 		libipw_copy_snap(skb_put(skb_new, SNAP_SIZE + sizeof(u16)),
 				    ether_type);
@@ -439,8 +439,7 @@ netdev_tx_t libipw_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	if (rts_required) {
 		skb_frag = txb->fragments[0];
-		frag_hdr =
-		    (struct libipw_hdr_3addrqos *)skb_put(skb_frag, hdr_len);
+		frag_hdr = skb_put(skb_frag, hdr_len);
 
 		/*
 		 * Set header frame_ctl to the RTS.
@@ -470,9 +469,7 @@ netdev_tx_t libipw_xmit(struct sk_buff *skb, struct net_device *dev)
 			skb_reserve(skb_frag,
 				    crypt->ops->extra_mpdu_prefix_len);
 
-		frag_hdr =
-		    (struct libipw_hdr_3addrqos *)skb_put(skb_frag, hdr_len);
-		memcpy(frag_hdr, &header, hdr_len);
+		frag_hdr = skb_put_data(skb_frag, &header, hdr_len);
 
 		/* If this is not the last fragment, then add the MOREFRAGS
 		 * bit to the frame control */
diff --git a/drivers/net/wireless/intel/iwlegacy/3945.c b/drivers/net/wireless/intel/iwlegacy/3945.c
index 080ea8155b90..dbf164d48ed3 100644
--- a/drivers/net/wireless/intel/iwlegacy/3945.c
+++ b/drivers/net/wireless/intel/iwlegacy/3945.c
@@ -520,7 +520,7 @@ il3945_pass_packet_to_mac80211(struct il_priv *il, struct il_rx_buf *rxb,
 	 * and do not consume a full page
 	 */
 	if (len <= SMALL_PACKET_SIZE) {
-		memcpy(skb_put(skb, len), rx_hdr->payload, len);
+		skb_put_data(skb, rx_hdr->payload, len);
 	} else {
 		skb_add_rx_frag(skb, 0, rxb->page,
 				(void *)rx_hdr->payload - (void *)pkt, len,
diff --git a/drivers/net/wireless/intel/iwlegacy/4965-mac.c b/drivers/net/wireless/intel/iwlegacy/4965-mac.c
index 49a2ff15ddae..5b51fba75595 100644
--- a/drivers/net/wireless/intel/iwlegacy/4965-mac.c
+++ b/drivers/net/wireless/intel/iwlegacy/4965-mac.c
@@ -606,7 +606,7 @@ il4965_pass_packet_to_mac80211(struct il_priv *il, struct ieee80211_hdr *hdr,
 	}
 
 	if (len <= SMALL_PACKET_SIZE) {
-		memcpy(skb_put(skb, len), hdr, len);
+		skb_put_data(skb, hdr, len);
 	} else {
 		skb_add_rx_frag(skb, 0, rxb->page, (void *)hdr - rxb_addr(rxb),
 				len, PAGE_SIZE << il->hw_params.rx_page_order);
diff --git a/drivers/net/wireless/intel/iwlegacy/common.c b/drivers/net/wireless/intel/iwlegacy/common.c
index 140b6ea8f7cc..8d5acda92a9b 100644
--- a/drivers/net/wireless/intel/iwlegacy/common.c
+++ b/drivers/net/wireless/intel/iwlegacy/common.c
@@ -5147,6 +5147,8 @@ set_ch_out:
 
 	if (changed & (IEEE80211_CONF_CHANGE_PS | IEEE80211_CONF_CHANGE_IDLE)) {
 		il->power_data.ps_disabled = !(conf->flags & IEEE80211_CONF_PS);
+		if (!il->power_data.ps_disabled)
+			IL_WARN_ONCE("Enabling power save might cause firmware crashes\n");
 		ret = il_power_update_mode(il, false);
 		if (ret)
 			D_MAC80211("Error setting sleep level\n");
diff --git a/drivers/net/wireless/intel/iwlegacy/common.h b/drivers/net/wireless/intel/iwlegacy/common.h
index 3bba521d2cd9..18c60c92e3a3 100644
--- a/drivers/net/wireless/intel/iwlegacy/common.h
+++ b/drivers/net/wireless/intel/iwlegacy/common.h
@@ -45,6 +45,7 @@ struct il_tx_queue;
 
 #define IL_ERR(f, a...) dev_err(&il->pci_dev->dev, f, ## a)
 #define IL_WARN(f, a...) dev_warn(&il->pci_dev->dev, f, ## a)
+#define IL_WARN_ONCE(f, a...) dev_warn_once(&il->pci_dev->dev, f, ## a)
 #define IL_INFO(f, a...) dev_info(&il->pci_dev->dev, f, ## a)
 
 #define RX_QUEUE_SIZE                         256
diff --git a/drivers/net/wireless/intel/iwlwifi/Makefile b/drivers/net/wireless/intel/iwlwifi/Makefile
index 411cb91c102f..20bd261223af 100644
--- a/drivers/net/wireless/intel/iwlwifi/Makefile
+++ b/drivers/net/wireless/intel/iwlwifi/Makefile
@@ -3,14 +3,14 @@ obj-$(CONFIG_IWLWIFI)	+= iwlwifi.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-eeprom-read.o iwl-eeprom-parse.o
 iwlwifi-objs		+= iwl-phy-db.o iwl-nvm-parse.o
 iwlwifi-objs		+= pcie/drv.o pcie/rx.o pcie/tx.o pcie/trans.o
 iwlwifi-objs		+= pcie/ctxt-info.o pcie/trans-gen2.o pcie/tx-gen2.o
-iwlwifi-$(CONFIG_IWLDVM) += iwl-1000.o iwl-2000.o iwl-5000.o iwl-6000.o
-iwlwifi-$(CONFIG_IWLMVM) += iwl-7000.o iwl-8000.o iwl-9000.o iwl-a000.o
+iwlwifi-$(CONFIG_IWLDVM) += cfg/1000.o cfg/2000.o cfg/5000.o cfg/6000.o
+iwlwifi-$(CONFIG_IWLMVM) += cfg/7000.o cfg/8000.o cfg/9000.o cfg/a000.o
 iwlwifi-objs		+= iwl-trans.o
+iwlwifi-objs		+= fw/notif-wait.o
 
 iwlwifi-objs += $(iwlwifi-m)
 
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-1000.c b/drivers/net/wireless/intel/iwlwifi/cfg/1000.c
index b2573b1d1506..b2573b1d1506 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-1000.c
+++ b/drivers/net/wireless/intel/iwlwifi/cfg/1000.c
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-2000.c b/drivers/net/wireless/intel/iwlwifi/cfg/2000.c
index 1b32ad413b9e..1b32ad413b9e 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-2000.c
+++ b/drivers/net/wireless/intel/iwlwifi/cfg/2000.c
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-5000.c b/drivers/net/wireless/intel/iwlwifi/cfg/5000.c
index 4aa8f0a05c8a..4aa8f0a05c8a 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-5000.c
+++ b/drivers/net/wireless/intel/iwlwifi/cfg/5000.c
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-6000.c b/drivers/net/wireless/intel/iwlwifi/cfg/6000.c
index 39335b7b0c16..39335b7b0c16 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-6000.c
+++ b/drivers/net/wireless/intel/iwlwifi/cfg/6000.c
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-7000.c b/drivers/net/wireless/intel/iwlwifi/cfg/7000.c
index 45e2efc70d19..45e2efc70d19 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-7000.c
+++ b/drivers/net/wireless/intel/iwlwifi/cfg/7000.c
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-8000.c b/drivers/net/wireless/intel/iwlwifi/cfg/8000.c
index 89137717c1fc..5081720608af 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-8000.c
+++ b/drivers/net/wireless/intel/iwlwifi/cfg/8000.c
@@ -70,8 +70,8 @@
 #include "iwl-agn-hw.h"
 
 /* Highest firmware API version supported */
-#define IWL8000_UCODE_API_MAX	30
-#define IWL8265_UCODE_API_MAX	30
+#define IWL8000_UCODE_API_MAX	33
+#define IWL8265_UCODE_API_MAX	33
 
 /* Lowest firmware API version supported */
 #define IWL8000_UCODE_API_MIN	22
@@ -98,7 +98,6 @@
 	IWL8265_FW_PRE __stringify(api) ".ucode"
 
 #define NVM_HW_SECTION_NUM_FAMILY_8000		10
-#define DEFAULT_NVM_FILE_FAMILY_8000B		"nvmData-8000B"
 #define DEFAULT_NVM_FILE_FAMILY_8000C		"nvmData-8000C"
 
 /* Max SDIO RX/TX aggregation sizes of the ADDBA request/response */
@@ -162,10 +161,11 @@ static const struct iwl_tt_params iwl8000_tt_params = {
 	.dccm2_len = IWL8260_DCCM2_LEN,					\
 	.smem_offset = IWL8260_SMEM_OFFSET,				\
 	.smem_len = IWL8260_SMEM_LEN,					\
-	.default_nvm_file_B_step = DEFAULT_NVM_FILE_FAMILY_8000B,	\
 	.default_nvm_file_C_step = DEFAULT_NVM_FILE_FAMILY_8000C,	\
 	.thermal_params = &iwl8000_tt_params,				\
-	.apmg_not_supported = true
+	.apmg_not_supported = true,					\
+	.ext_nvm = true,						\
+	.dbgc_supported = true
 
 #define IWL_DEVICE_8000							\
 	IWL_DEVICE_8000_COMMON,						\
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-9000.c b/drivers/net/wireless/intel/iwlwifi/cfg/9000.c
index 110ceefccc15..b4ecd1fe1374 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-9000.c
+++ b/drivers/net/wireless/intel/iwlwifi/cfg/9000.c
@@ -55,7 +55,7 @@
 #include "iwl-agn-hw.h"
 
 /* Highest firmware API version supported */
-#define IWL9000_UCODE_API_MAX	30
+#define IWL9000_UCODE_API_MAX	33
 
 /* Lowest firmware API version supported */
 #define IWL9000_UCODE_API_MIN	30
@@ -73,10 +73,13 @@
 #define IWL9000_SMEM_LEN		0x68000
 
 #define  IWL9000_FW_PRE "iwlwifi-9000-pu-a0-jf-a0-"
+#define  IWL9000RFB_FW_PRE "iwlwifi-9000-pu-a0-jf-b0-"
 #define  IWL9260A_FW_PRE "iwlwifi-9260-th-a0-jf-a0-"
 #define  IWL9260B_FW_PRE "iwlwifi-9260-th-b0-jf-b0-"
 #define IWL9000_MODULE_FIRMWARE(api) \
 	IWL9000_FW_PRE "-" __stringify(api) ".ucode"
+#define IWL9000RFB_MODULE_FIRMWARE(api) \
+	IWL9000RFB_FW_PRE "-" __stringify(api) ".ucode"
 #define IWL9260A_MODULE_FIRMWARE(api) \
 	IWL9260A_FW_PRE "-" __stringify(api) ".ucode"
 #define IWL9260B_MODULE_FIRMWARE(api) \
@@ -125,7 +128,7 @@ static const struct iwl_tt_params iwl9000_tt_params = {
 #define IWL_DEVICE_9000							\
 	.ucode_api_max = IWL9000_UCODE_API_MAX,				\
 	.ucode_api_min = IWL9000_UCODE_API_MIN,				\
-	.device_family = IWL_DEVICE_FAMILY_8000,			\
+	.device_family = IWL_DEVICE_FAMILY_9000,			\
 	.max_inst_size = IWL60_RTC_INST_SIZE,				\
 	.max_data_size = IWL60_RTC_DATA_SIZE,				\
 	.base_params = &iwl9000_base_params,				\
@@ -144,7 +147,9 @@ static const struct iwl_tt_params iwl9000_tt_params = {
 	.mq_rx_supported = true,					\
 	.vht_mu_mimo_supported = true,					\
 	.mac_addr_from_csr = true,					\
-	.rf_id = true
+	.rf_id = true,							\
+	.ext_nvm = true,						\
+	.dbgc_supported = true
 
 const struct iwl_cfg iwl9160_2ac_cfg = {
 	.name = "Intel(R) Dual Band Wireless AC 9160",
@@ -182,6 +187,7 @@ const struct iwl_cfg iwl9270_2ac_cfg = {
 const struct iwl_cfg iwl9460_2ac_cfg = {
 	.name = "Intel(R) Dual Band Wireless AC 9460",
 	.fw_name_pre = IWL9000_FW_PRE,
+	.fw_name_pre_rf_next_step = IWL9000RFB_FW_PRE,
 	IWL_DEVICE_9000,
 	.ht_params = &iwl9000_ht_params,
 	.nvm_ver = IWL9000_NVM_VERSION,
@@ -193,6 +199,7 @@ const struct iwl_cfg iwl9460_2ac_cfg = {
 const struct iwl_cfg iwl9560_2ac_cfg = {
 	.name = "Intel(R) Dual Band Wireless AC 9560",
 	.fw_name_pre = IWL9000_FW_PRE,
+	.fw_name_pre_rf_next_step = IWL9000RFB_FW_PRE,
 	IWL_DEVICE_9000,
 	.ht_params = &iwl9000_ht_params,
 	.nvm_ver = IWL9000_NVM_VERSION,
@@ -202,5 +209,6 @@ const struct iwl_cfg iwl9560_2ac_cfg = {
 };
 
 MODULE_FIRMWARE(IWL9000_MODULE_FIRMWARE(IWL9000_UCODE_API_MAX));
+MODULE_FIRMWARE(IWL9000RFB_MODULE_FIRMWARE(IWL9000_UCODE_API_MAX));
 MODULE_FIRMWARE(IWL9260A_MODULE_FIRMWARE(IWL9000_UCODE_API_MAX));
 MODULE_FIRMWARE(IWL9260B_MODULE_FIRMWARE(IWL9000_UCODE_API_MAX));
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-a000.c b/drivers/net/wireless/intel/iwlwifi/cfg/a000.c
index c648cfb981a3..98f24cd1b44f 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-a000.c
+++ b/drivers/net/wireless/intel/iwlwifi/cfg/a000.c
@@ -55,7 +55,7 @@
 #include "iwl-agn-hw.h"
 
 /* Highest firmware API version supported */
-#define IWL_A000_UCODE_API_MAX	30
+#define IWL_A000_UCODE_API_MAX	33
 
 /* Lowest firmware API version supported */
 #define IWL_A000_UCODE_API_MIN	24
@@ -74,7 +74,7 @@
 
 #define IWL_A000_JF_FW_PRE	"iwlwifi-Qu-a0-jf-b0-"
 #define IWL_A000_HR_FW_PRE	"iwlwifi-Qu-a0-hr-a0-"
-#define IWL_A000_HR_CDB_FW_PRE	"iwlwifi-QuIcp-a0-hrcdb-a0-"
+#define IWL_A000_HR_CDB_FW_PRE	"iwlwifi-QuIcp-z0-hrcdb-a0-"
 
 #define IWL_A000_HR_MODULE_FIRMWARE(api) \
 	IWL_A000_HR_FW_PRE "-" __stringify(api) ".ucode"
@@ -103,7 +103,7 @@ static const struct iwl_ht_params iwl_a000_ht_params = {
 #define IWL_DEVICE_A000							\
 	.ucode_api_max = IWL_A000_UCODE_API_MAX,			\
 	.ucode_api_min = IWL_A000_UCODE_API_MIN,			\
-	.device_family = IWL_DEVICE_FAMILY_8000,			\
+	.device_family = IWL_DEVICE_FAMILY_A000,			\
 	.max_inst_size = IWL60_RTC_INST_SIZE,				\
 	.max_data_size = IWL60_RTC_DATA_SIZE,				\
 	.base_params = &iwl_a000_base_params,				\
@@ -123,7 +123,9 @@ static const struct iwl_ht_params iwl_a000_ht_params = {
 	.mac_addr_from_csr = true,					\
 	.use_tfh = true,						\
 	.rf_id = true,							\
-	.gen2 = true
+	.gen2 = true,							\
+	.ext_nvm = true,						\
+	.dbgc_supported = true
 
 const struct iwl_cfg iwla000_2ac_cfg_hr = {
 		.name = "Intel(R) Dual Band Wireless AC a000",
@@ -156,5 +158,15 @@ const struct iwl_cfg iwla000_2ac_cfg_jf = {
 		.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K,
 };
 
+const struct iwl_cfg iwla000_2ax_cfg_hr = {
+		.name = "Intel(R) Dual Band Wireless AX a000",
+		.fw_name_pre = IWL_A000_HR_FW_PRE,
+		IWL_DEVICE_A000,
+		.ht_params = &iwl_a000_ht_params,
+		.nvm_ver = IWL_A000_NVM_VERSION,
+		.nvm_calib_ver = IWL_A000_TX_POWER_VERSION,
+		.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K,
+};
+
 MODULE_FIRMWARE(IWL_A000_HR_MODULE_FIRMWARE(IWL_A000_UCODE_API_MAX));
 MODULE_FIRMWARE(IWL_A000_JF_MODULE_FIRMWARE(IWL_A000_UCODE_API_MAX));
diff --git a/drivers/net/wireless/intel/iwlwifi/dvm/debugfs.c b/drivers/net/wireless/intel/iwlwifi/dvm/debugfs.c
index 376c79337a0e..482ac8fdc67b 100644
--- a/drivers/net/wireless/intel/iwlwifi/dvm/debugfs.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/debugfs.c
@@ -681,11 +681,10 @@ DEBUGFS_READ_FILE_OPS(temperature);
 DEBUGFS_READ_WRITE_FILE_OPS(sleep_level_override);
 DEBUGFS_READ_FILE_OPS(current_sleep_command);
 
-static const char *fmt_value = "  %-30s %10u\n";
-static const char *fmt_hex   = "  %-30s       0x%02X\n";
-static const char *fmt_table = "  %-30s %10u  %10u  %10u  %10u\n";
-static const char *fmt_header =
-	"%-32s    current  cumulative       delta         max\n";
+#define fmt_value	"  %-30s %10u\n"
+#define fmt_hex		"  %-30s       0x%02X\n"
+#define fmt_table	"  %-30s %10u  %10u  %10u  %10u\n"
+#define fmt_header	"%-32s    current  cumulative       delta         max\n"
 
 static int iwl_statistics_flag(struct iwl_priv *priv, char *buf, int bufsz)
 {
@@ -2309,10 +2308,10 @@ static ssize_t iwl_dbgfs_fw_restart_write(struct file *file,
 					  size_t count, loff_t *ppos)
 {
 	struct iwl_priv *priv = file->private_data;
-	bool restart_fw = iwlwifi_mod_params.restart_fw;
+	bool fw_restart = iwlwifi_mod_params.fw_restart;
 	int __maybe_unused ret;
 
-	iwlwifi_mod_params.restart_fw = true;
+	iwlwifi_mod_params.fw_restart = true;
 
 	mutex_lock(&priv->mutex);
 
@@ -2321,7 +2320,7 @@ static ssize_t iwl_dbgfs_fw_restart_write(struct file *file,
 
 	mutex_unlock(&priv->mutex);
 
-	iwlwifi_mod_params.restart_fw = restart_fw;
+	iwlwifi_mod_params.fw_restart = fw_restart;
 
 	return count;
 }
diff --git a/drivers/net/wireless/intel/iwlwifi/dvm/dev.h b/drivers/net/wireless/intel/iwlwifi/dvm/dev.h
index 8148df61a916..cceb4cd8e501 100644
--- a/drivers/net/wireless/intel/iwlwifi/dvm/dev.h
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/dev.h
@@ -38,13 +38,13 @@
 #include <linux/slab.h>
 #include <linux/mutex.h>
 
-#include "iwl-fw.h"
+#include "fw/img.h"
 #include "iwl-eeprom-parse.h"
 #include "iwl-csr.h"
 #include "iwl-debug.h"
 #include "iwl-agn-hw.h"
 #include "iwl-op-mode.h"
-#include "iwl-notif-wait.h"
+#include "fw/notif-wait.h"
 #include "iwl-trans.h"
 
 #include "led.h"
diff --git a/drivers/net/wireless/intel/iwlwifi/dvm/lib.c b/drivers/net/wireless/intel/iwlwifi/dvm/lib.c
index 74e52f7c5aa1..2b6ffbc46fa5 100644
--- a/drivers/net/wireless/intel/iwlwifi/dvm/lib.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/lib.c
@@ -1157,7 +1157,7 @@ int iwlagn_suspend(struct iwl_priv *priv, struct cfg80211_wowlan *wowlan)
 	if (ret)
 		goto out;
 
-	if (!iwlwifi_mod_params.sw_crypto) {
+	if (!iwlwifi_mod_params.swcrypto) {
 		/* mark all keys clear */
 		priv->ucode_key_table = 0;
 		ctx->key_mapping_keys = 0;
diff --git a/drivers/net/wireless/intel/iwlwifi/dvm/mac80211.c b/drivers/net/wireless/intel/iwlwifi/dvm/mac80211.c
index 444c74371929..82caae02dd09 100644
--- a/drivers/net/wireless/intel/iwlwifi/dvm/mac80211.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/mac80211.c
@@ -138,7 +138,7 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 	 * packets, so enabling it with software crypto isn't safe)
 	 */
 	if (priv->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_MFP &&
-	    !iwlwifi_mod_params.sw_crypto)
+	    !iwlwifi_mod_params.swcrypto)
 		ieee80211_hw_set(hw, MFP_CAPABLE);
 
 	hw->sta_data_size = sizeof(struct iwl_station_priv);
@@ -171,7 +171,7 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 					     WIPHY_WOWLAN_DISCONNECT |
 					     WIPHY_WOWLAN_EAP_IDENTITY_REQ |
 					     WIPHY_WOWLAN_RFKILL_RELEASE;
-		if (!iwlwifi_mod_params.sw_crypto)
+		if (!iwlwifi_mod_params.swcrypto)
 			priv->wowlan_support.flags |=
 				WIPHY_WOWLAN_SUPPORTS_GTK_REKEY |
 				WIPHY_WOWLAN_GTK_REKEY_FAILURE;
@@ -348,7 +348,7 @@ static void iwlagn_mac_set_rekey_data(struct ieee80211_hw *hw,
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
-	if (iwlwifi_mod_params.sw_crypto)
+	if (iwlwifi_mod_params.swcrypto)
 		return;
 
 	IWL_DEBUG_MAC80211(priv, "enter\n");
@@ -624,7 +624,7 @@ static int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 
 	IWL_DEBUG_MAC80211(priv, "enter\n");
 
-	if (iwlwifi_mod_params.sw_crypto) {
+	if (iwlwifi_mod_params.swcrypto) {
 		IWL_DEBUG_MAC80211(priv, "leave - hwcrypto disabled\n");
 		return -EOPNOTSUPP;
 	}
diff --git a/drivers/net/wireless/intel/iwlwifi/dvm/main.c b/drivers/net/wireless/intel/iwlwifi/dvm/main.c
index b49848683587..2acd94da9efe 100644
--- a/drivers/net/wireless/intel/iwlwifi/dvm/main.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/main.c
@@ -1371,7 +1371,7 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 
 	/* is antenna coupling more than 35dB ? */
 	priv->bt_ant_couple_ok =
-		(iwlwifi_mod_params.ant_coupling >
+		(iwlwifi_mod_params.antenna_coupling >
 			IWL_BT_ANTENNA_COUPLING_THRESHOLD) ?
 			true : false;
 
@@ -1513,7 +1513,7 @@ out_destroy_workqueue:
 out_free_eeprom_blob:
 	kfree(priv->eeprom_blob);
 out_free_eeprom:
-	iwl_free_nvm_data(priv->nvm_data);
+	kfree(priv->nvm_data);
 out_free_hw:
 	ieee80211_free_hw(priv->hw);
 out:
@@ -1532,7 +1532,7 @@ static void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode)
 	iwl_tt_exit(priv);
 
 	kfree(priv->eeprom_blob);
-	iwl_free_nvm_data(priv->nvm_data);
+	kfree(priv->nvm_data);
 
 	/*netif_stop_queue(dev); */
 	flush_workqueue(priv->workqueue);
@@ -1958,7 +1958,7 @@ static void iwlagn_fw_error(struct iwl_priv *priv, bool ondemand)
 	}
 
 	if (!test_bit(STATUS_EXIT_PENDING, &priv->status)) {
-		if (iwlwifi_mod_params.restart_fw) {
+		if (iwlwifi_mod_params.fw_restart) {
 			IWL_DEBUG_FW_ERRORS(priv,
 				  "Restarting adapter due to uCode error.\n");
 			queue_work(priv->workqueue, &priv->restart);
diff --git a/drivers/net/wireless/intel/iwlwifi/dvm/rx.c b/drivers/net/wireless/intel/iwlwifi/dvm/rx.c
index adfd6307edca..c942830af2b5 100644
--- a/drivers/net/wireless/intel/iwlwifi/dvm/rx.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/rx.c
@@ -639,7 +639,7 @@ static void iwlagn_pass_packet_to_mac80211(struct iwl_priv *priv,
 	}
 
 	/* In case of HW accelerated crypto and bad decryption, drop */
-	if (!iwlwifi_mod_params.sw_crypto &&
+	if (!iwlwifi_mod_params.swcrypto &&
 	    iwlagn_set_decrypted_flag(priv, hdr, ampdu_status, stats))
 		return;
 
@@ -657,7 +657,7 @@ static void iwlagn_pass_packet_to_mac80211(struct iwl_priv *priv,
 	 */
 	hdrlen = (len <= skb_tailroom(skb)) ? len : sizeof(*hdr);
 
-	memcpy(skb_put(skb, hdrlen), hdr, hdrlen);
+	skb_put_data(skb, hdr, hdrlen);
 	fraglen = len - hdrlen;
 
 	if (fraglen) {
diff --git a/drivers/net/wireless/intel/iwlwifi/dvm/rxon.c b/drivers/net/wireless/intel/iwlwifi/dvm/rxon.c
index 087e579854ab..8f3e5586eda9 100644
--- a/drivers/net/wireless/intel/iwlwifi/dvm/rxon.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/rxon.c
@@ -1120,7 +1120,7 @@ int iwlagn_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 		return 0;
 	}
 
-	iwl_set_rxon_hwcrypto(priv, ctx, !iwlwifi_mod_params.sw_crypto);
+	iwl_set_rxon_hwcrypto(priv, ctx, !iwlwifi_mod_params.swcrypto);
 
 	IWL_DEBUG_INFO(priv,
 		       "Going to commit RXON\n"
diff --git a/drivers/net/wireless/intel/iwlwifi/dvm/tx.c b/drivers/net/wireless/intel/iwlwifi/dvm/tx.c
index 4b97371c3b42..adaa2f0097cc 100644
--- a/drivers/net/wireless/intel/iwlwifi/dvm/tx.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/tx.c
@@ -319,8 +319,7 @@ int iwlagn_tx_skb(struct iwl_priv *priv,
 		if (noa_data &&
 		    pskb_expand_head(skb, 0, noa_data->length,
 				     GFP_ATOMIC) == 0) {
-			memcpy(skb_put(skb, noa_data->length),
-			       noa_data->data, noa_data->length);
+			skb_put_data(skb, noa_data->data, noa_data->length);
 			hdr = (struct ieee80211_hdr *)skb->data;
 		}
 	}
diff --git a/drivers/net/wireless/intel/iwlwifi/fw/api.h b/drivers/net/wireless/intel/iwlwifi/fw/api.h
new file mode 100644
index 000000000000..0e107f916ce3
--- /dev/null
+++ b/drivers/net/wireless/intel/iwlwifi/fw/api.h
@@ -0,0 +1,229 @@
+/******************************************************************************
+ *
+ * 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) 2007 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <linuxwifi@intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#ifndef __iwl_fw_api_h__
+#define __iwl_fw_api_h__
+
+/**
+ * DOC: Host command section
+ *
+ * A host command is a command issued by the upper layer to the fw. There are
+ * several versions of fw that have several APIs. The transport layer is
+ * completely agnostic to these differences.
+ * The transport does provide helper functionality (i.e. SYNC / ASYNC mode),
+ */
+#define SEQ_TO_QUEUE(s)	(((s) >> 8) & 0x1f)
+#define QUEUE_TO_SEQ(q)	(((q) & 0x1f) << 8)
+#define SEQ_TO_INDEX(s)	((s) & 0xff)
+#define INDEX_TO_SEQ(i)	((i) & 0xff)
+#define SEQ_RX_FRAME	cpu_to_le16(0x8000)
+
+/*
+ * those functions retrieve specific information from
+ * the id field in the iwl_host_cmd struct which contains
+ * the command id, the group id and the version of the command
+ * and vice versa
+*/
+static inline u8 iwl_cmd_opcode(u32 cmdid)
+{
+	return cmdid & 0xFF;
+}
+
+static inline u8 iwl_cmd_groupid(u32 cmdid)
+{
+	return ((cmdid & 0xFF00) >> 8);
+}
+
+static inline u8 iwl_cmd_version(u32 cmdid)
+{
+	return ((cmdid & 0xFF0000) >> 16);
+}
+
+static inline u32 iwl_cmd_id(u8 opcode, u8 groupid, u8 version)
+{
+	return opcode + (groupid << 8) + (version << 16);
+}
+
+/* make u16 wide id out of u8 group and opcode */
+#define WIDE_ID(grp, opcode) (((grp) << 8) | (opcode))
+#define DEF_ID(opcode) ((1 << 8) | (opcode))
+
+/* due to the conversion, this group is special; new groups
+ * should be defined in the appropriate fw-api header files
+ */
+#define IWL_ALWAYS_LONG_GROUP	1
+
+/**
+ * struct iwl_cmd_header
+ *
+ * This header format appears in the beginning of each command sent from the
+ * driver, and each response/notification received from uCode.
+ */
+struct iwl_cmd_header {
+	u8 cmd;		/* Command ID:  REPLY_RXON, etc. */
+	u8 group_id;
+	/*
+	 * The driver sets up the sequence number to values of its choosing.
+	 * uCode does not use this value, but passes it back to the driver
+	 * when sending the response to each driver-originated command, so
+	 * the driver can match the response to the command.  Since the values
+	 * don't get used by uCode, the driver may set up an arbitrary format.
+	 *
+	 * There is one exception:  uCode sets bit 15 when it originates
+	 * the response/notification, i.e. when the response/notification
+	 * is not a direct response to a command sent by the driver.  For
+	 * example, uCode issues REPLY_RX when it sends a received frame
+	 * to the driver; it is not a direct response to any driver command.
+	 *
+	 * The Linux driver uses the following format:
+	 *
+	 *  0:7		tfd index - position within TX queue
+	 *  8:12	TX queue id
+	 *  13:14	reserved
+	 *  15		unsolicited RX or uCode-originated notification
+	 */
+	__le16 sequence;
+} __packed;
+
+/**
+ * struct iwl_cmd_header_wide
+ *
+ * This header format appears in the beginning of each command sent from the
+ * driver, and each response/notification received from uCode.
+ * this is the wide version that contains more information about the command
+ * like length, version and command type
+ */
+struct iwl_cmd_header_wide {
+	u8 cmd;
+	u8 group_id;
+	__le16 sequence;
+	__le16 length;
+	u8 reserved;
+	u8 version;
+} __packed;
+
+/**
+ * iwl_tx_queue_cfg_actions - TXQ config options
+ * @TX_QUEUE_CFG_ENABLE_QUEUE: enable a queue
+ * @TX_QUEUE_CFG_TFD_SHORT_FORMAT: use short TFD format
+ */
+enum iwl_tx_queue_cfg_actions {
+	TX_QUEUE_CFG_ENABLE_QUEUE		= BIT(0),
+	TX_QUEUE_CFG_TFD_SHORT_FORMAT		= BIT(1),
+};
+
+/**
+ * struct iwl_tx_queue_cfg_cmd - txq hw scheduler config command
+ * @sta_id: station id
+ * @tid: tid of the queue
+ * @flags: see &enum iwl_tx_queue_cfg_actions
+ * @cb_size: size of TFD cyclic buffer. Value is exponent - 3.
+ *	Minimum value 0 (8 TFDs), maximum value 5 (256 TFDs)
+ * @byte_cnt_addr: address of byte count table
+ * @tfdq_addr: address of TFD circular buffer
+ */
+struct iwl_tx_queue_cfg_cmd {
+	u8 sta_id;
+	u8 tid;
+	__le16 flags;
+	__le32 cb_size;
+	__le64 byte_cnt_addr;
+	__le64 tfdq_addr;
+} __packed; /* TX_QUEUE_CFG_CMD_API_S_VER_2 */
+
+/**
+ * struct iwl_tx_queue_cfg_rsp - response to txq hw scheduler config
+ * @queue_number: queue number assigned to this RA -TID
+ * @flags: set on failure
+ * @write_pointer: initial value for write pointer
+ */
+struct iwl_tx_queue_cfg_rsp {
+	__le16 queue_number;
+	__le16 flags;
+	__le16 write_pointer;
+	__le16 reserved;
+} __packed; /* TX_QUEUE_CFG_RSP_API_S_VER_2 */
+
+/**
+ * struct iwl_calib_res_notif_phy_db - Receive phy db chunk after calibrations
+ * @type: type of the result - mostly ignored
+ * @length: length of the data
+ * @data: data, length in @length
+ */
+struct iwl_calib_res_notif_phy_db {
+	__le16 type;
+	__le16 length;
+	u8 data[];
+} __packed;
+
+/**
+ * struct iwl_phy_db_cmd - configure operational ucode
+ * @type: type of the data
+ * @length: length of the data
+ * @data: data, length in @length
+ */
+struct iwl_phy_db_cmd {
+	__le16 type;
+	__le16 length;
+	u8 data[];
+} __packed;
+
+#endif /* __iwl_fw_api_h__*/
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-fw-error-dump.h b/drivers/net/wireless/intel/iwlwifi/fw/error-dump.h
index 420c31dab263..cfebde68a391 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-fw-error-dump.h
+++ b/drivers/net/wireless/intel/iwlwifi/fw/error-dump.h
@@ -116,7 +116,7 @@ enum iwl_fw_error_dump_type {
 
 /**
  * struct iwl_fw_error_dump_data - data for one type
- * @type: %enum iwl_fw_error_dump_type
+ * @type: &enum iwl_fw_error_dump_type
  * @len: the length starting from %data
  * @data: the data itself
  */
@@ -130,7 +130,7 @@ struct iwl_fw_error_dump_data {
  * struct iwl_fw_error_dump_file - the layout of the header of the file
  * @barker: must be %IWL_FW_ERROR_DUMP_BARKER
  * @file_len: the length of all the file starting from %barker
- * @data: array of %struct iwl_fw_error_dump_data
+ * @data: array of &struct iwl_fw_error_dump_data
  */
 struct iwl_fw_error_dump_file {
 	__le32 barker;
@@ -225,7 +225,7 @@ enum iwl_fw_error_dump_mem_type {
 
 /**
  * struct iwl_fw_error_dump_mem - chunk of memory
- * @type: %enum iwl_fw_error_dump_mem_type
+ * @type: &enum iwl_fw_error_dump_mem_type
  * @offset: the offset from which the memory was read
  * @data: the content of the memory
  */
@@ -324,7 +324,7 @@ enum iwl_fw_dbg_trigger {
 
 /**
  * struct iwl_fw_error_dump_trigger_desc - describes the trigger condition
- * @type: %enum iwl_fw_dbg_trigger
+ * @type: &enum iwl_fw_dbg_trigger
  * @data: raw data about what happened
  */
 struct iwl_fw_error_dump_trigger_desc {
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-fw-file.h b/drivers/net/wireless/intel/iwlwifi/fw/file.h
index 44419e82da1b..0fa8c473f1e2 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-fw-file.h
+++ b/drivers/net/wireless/intel/iwlwifi/fw/file.h
@@ -244,10 +244,13 @@ typedef unsigned int __bitwise iwl_ucode_tlv_api_t;
  * @IWL_UCODE_TLV_API_TKIP_MIC_KEYS: This ucode supports version 2 of
  *	ADD_MODIFY_STA_KEY_API_S_VER_2.
  * @IWL_UCODE_TLV_API_STA_TYPE: This ucode supports station type assignement.
+ * @IWL_UCODE_TLV_API_NAN2_VER2: This ucode supports NAN API version 2
+ * @IWL_UCODE_TLV_API_NEW_RX_STATS: should new RX STATISTICS API be used
  *
  * @NUM_IWL_UCODE_TLV_API: number of bits used
  */
 enum iwl_ucode_tlv_api {
+	/* API Set 0 */
 	IWL_UCODE_TLV_API_FRAGMENTED_SCAN	= (__force iwl_ucode_tlv_api_t)8,
 	IWL_UCODE_TLV_API_WIFI_MCC_UPDATE	= (__force iwl_ucode_tlv_api_t)9,
 	IWL_UCODE_TLV_API_LQ_SS_PARAMS		= (__force iwl_ucode_tlv_api_t)18,
@@ -255,6 +258,9 @@ enum iwl_ucode_tlv_api {
 	IWL_UCODE_TLV_API_SCAN_TSF_REPORT	= (__force iwl_ucode_tlv_api_t)28,
 	IWL_UCODE_TLV_API_TKIP_MIC_KEYS		= (__force iwl_ucode_tlv_api_t)29,
 	IWL_UCODE_TLV_API_STA_TYPE		= (__force iwl_ucode_tlv_api_t)30,
+	IWL_UCODE_TLV_API_NAN2_VER2		= (__force iwl_ucode_tlv_api_t)31,
+	/* API Set 1 */
+	IWL_UCODE_TLV_API_NEW_RX_STATS		= (__force iwl_ucode_tlv_api_t)35,
 
 	NUM_IWL_UCODE_TLV_API
 #ifdef __CHECKER__
@@ -351,6 +357,7 @@ enum iwl_ucode_tlv_capa {
 	IWL_UCODE_TLV_CAPA_STA_PM_NOTIF			= (__force iwl_ucode_tlv_capa_t)38,
 	IWL_UCODE_TLV_CAPA_BINDING_CDB_SUPPORT		= (__force iwl_ucode_tlv_capa_t)39,
 	IWL_UCODE_TLV_CAPA_CDB_SUPPORT			= (__force iwl_ucode_tlv_capa_t)40,
+	IWL_UCODE_TLV_CAPA_D0I3_END_FIRST		= (__force iwl_ucode_tlv_capa_t)41,
 	IWL_UCODE_TLV_CAPA_EXTENDED_DTS_MEASURE		= (__force iwl_ucode_tlv_capa_t)64,
 	IWL_UCODE_TLV_CAPA_SHORT_PM_TIMEOUTS		= (__force iwl_ucode_tlv_capa_t)65,
 	IWL_UCODE_TLV_CAPA_BT_MPLUT_SUPPORT		= (__force iwl_ucode_tlv_capa_t)67,
@@ -395,8 +402,8 @@ enum iwl_ucode_tlv_capa {
 #define IWL_UCODE_API(ver)	(((ver) & 0x0000FF00) >> 8)
 #define IWL_UCODE_SERIAL(ver)	((ver) & 0x000000FF)
 
-/*
- * Calibration control struct.
+/**
+ * struct iwl_tlv_calib_ctrl - Calibration control struct.
  * Sent as part of the phy configuration command.
  * @flow_trigger: bitmap for which calibrations to perform according to
  *		flow triggers.
@@ -468,7 +475,7 @@ enum iwl_fw_dbg_reg_operator {
 /**
  * struct iwl_fw_dbg_reg_op - an operation on a register
  *
- * @op: %enum iwl_fw_dbg_reg_operator
+ * @op: &enum iwl_fw_dbg_reg_operator
  * @addr: offset of the register
  * @val: value
  */
@@ -526,7 +533,7 @@ struct iwl_fw_dbg_mem_seg_tlv {
  * struct iwl_fw_dbg_dest_tlv - configures the destination of the debug data
  *
  * @version: version of the TLV - currently 0
- * @monitor_mode: %enum iwl_fw_dbg_monitor_mode
+ * @monitor_mode: &enum iwl_fw_dbg_monitor_mode
  * @size_power: buffer size will be 2^(size_power + 11)
  * @base_reg: addr of the base addr register (PRPH)
  * @end_reg:  addr of the end addr register (PRPH)
@@ -595,15 +602,15 @@ enum iwl_fw_dbg_trigger_vif_type {
 
 /**
  * struct iwl_fw_dbg_trigger_tlv - a TLV that describes the trigger
- * @id: %enum iwl_fw_dbg_trigger
- * @vif_type: %enum iwl_fw_dbg_trigger_vif_type
+ * @id: &enum iwl_fw_dbg_trigger
+ * @vif_type: &enum iwl_fw_dbg_trigger_vif_type
  * @stop_conf_ids: bitmap of configurations this trigger relates to.
  *	if the mode is %IWL_FW_DBG_TRIGGER_STOP, then if the bit corresponding
  *	to the currently running configuration is set, the data should be
  *	collected.
  * @stop_delay: how many milliseconds to wait before collecting the data
  *	after the STOP trigger fires.
- * @mode: %enum iwl_fw_dbg_trigger_mode - can be stop / start of both
+ * @mode: &enum iwl_fw_dbg_trigger_mode - can be stop / start of both
  * @start_conf_id: if mode is %IWL_FW_DBG_TRIGGER_START, this defines what
  *	configuration should be applied when the triggers kicks in.
  * @occurrences: number of occurrences. 0 means the trigger will never fire.
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-fw.h b/drivers/net/wireless/intel/iwlwifi/fw/img.h
index d323b70b510a..e6bc9cb60700 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-fw.h
+++ b/drivers/net/wireless/intel/iwlwifi/fw/img.h
@@ -64,12 +64,12 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *****************************************************************************/
 
-#ifndef __iwl_fw_h__
-#define __iwl_fw_h__
+#ifndef __iwl_fw_img_h__
+#define __iwl_fw_img_h__
 #include <linux/types.h>
 
-#include "iwl-fw-file.h"
-#include "iwl-fw-error-dump.h"
+#include "file.h"
+#include "error-dump.h"
 
 /**
  * enum iwl_ucode_type
@@ -339,4 +339,4 @@ iwl_get_ucode_image(const struct iwl_fw *fw, enum iwl_ucode_type ucode_type)
 	return &fw->img[ucode_type];
 }
 
-#endif  /* __iwl_fw_h__ */
+#endif  /* __iwl_fw_img_h__ */
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-notif-wait.c b/drivers/net/wireless/intel/iwlwifi/fw/notif-wait.c
index 68412ff2112e..29bb92e3df59 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-notif-wait.c
+++ b/drivers/net/wireless/intel/iwlwifi/fw/notif-wait.c
@@ -65,7 +65,7 @@
 #include <linux/export.h>
 
 #include "iwl-drv.h"
-#include "iwl-notif-wait.h"
+#include "notif-wait.h"
 
 
 void iwl_notification_wait_init(struct iwl_notif_wait_data *notif_wait)
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-notif-wait.h b/drivers/net/wireless/intel/iwlwifi/fw/notif-wait.h
index 368884be4e7c..368884be4e7c 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-notif-wait.h
+++ b/drivers/net/wireless/intel/iwlwifi/fw/notif-wait.h
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-config.h b/drivers/net/wireless/intel/iwlwifi/iwl-config.h
index a12197e3ce78..c52623cb7c2a 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-config.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-config.h
@@ -88,6 +88,8 @@ enum iwl_device_family {
 	IWL_DEVICE_FAMILY_6150,
 	IWL_DEVICE_FAMILY_7000,
 	IWL_DEVICE_FAMILY_8000,
+	IWL_DEVICE_FAMILY_9000,
+	IWL_DEVICE_FAMILY_A000,
 };
 
 /*
@@ -129,6 +131,7 @@ enum iwl_led_mode {
 
 /* Antenna presence definitions */
 #define	ANT_NONE	0x0
+#define	ANT_INVALID	0xff
 #define	ANT_A		BIT(0)
 #define	ANT_B		BIT(1)
 #define ANT_C		BIT(2)
@@ -275,6 +278,8 @@ struct iwl_pwr_tx_backoff {
  *	filename is constructed as fw_name_pre<api>.ucode.
  * @fw_name_pre_next_step: same as @fw_name_pre, only for next step
  *	(if supported)
+ * @fw_name_pre_rf_next_step: same as @fw_name_pre_next_step, only for rf next
+ *	step. Supported only in integrated solutions.
  * @ucode_api_max: Highest version of uCode API supported by driver.
  * @ucode_api_min: Lowest version of uCode API supported by driver.
  * @max_inst_size: The maximal length of the fw inst section
@@ -315,6 +320,7 @@ struct iwl_pwr_tx_backoff {
  * @integrated: discrete or integrated
  * @gen2: a000 and on transport operation
  * @cdb: CDB support
+ * @ext_nvm: extended NVM format
  *
  * We enable the driver to be backward compatible wrt. hardware features.
  * API differences in uCode shouldn't be handled here but through TLVs
@@ -325,13 +331,13 @@ struct iwl_cfg {
 	const char *name;
 	const char *fw_name_pre;
 	const char *fw_name_pre_next_step;
+	const char *fw_name_pre_rf_next_step;
 	/* params not likely to change within a device family */
 	const struct iwl_base_params *base_params;
 	/* params likely to change within a device family */
 	const struct iwl_ht_params *ht_params;
 	const struct iwl_eeprom_params *eeprom_params;
 	const struct iwl_pwr_tx_backoff *pwr_tx_backoffs;
-	const char *default_nvm_file_B_step;
 	const char *default_nvm_file_C_step;
 	const struct iwl_tt_params *thermal_params;
 	enum iwl_device_family device_family;
@@ -362,7 +368,9 @@ struct iwl_cfg {
 	    integrated:1,
 	    use_tfh:1,
 	    gen2:1,
-	    cdb:1;
+	    cdb:1,
+	    ext_nvm:1,
+	    dbgc_supported:1;
 	u8 valid_tx_ant;
 	u8 valid_rx_ant;
 	u8 non_shared_ant;
@@ -454,6 +462,7 @@ extern const struct iwl_cfg iwl9560_2ac_cfg;
 extern const struct iwl_cfg iwla000_2ac_cfg_hr;
 extern const struct iwl_cfg iwla000_2ac_cfg_hr_cdb;
 extern const struct iwl_cfg iwla000_2ac_cfg_jf;
+extern const struct iwl_cfg iwla000_2ax_cfg_hr;
 #endif /* CONFIG_IWLMVM */
 
 #endif /* __IWL_CONFIG_H__ */
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-csr.h b/drivers/net/wireless/intel/iwlwifi/iwl-csr.h
index fa120fb55373..c6c1876c1ad4 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-csr.h
@@ -153,6 +153,10 @@
 /* GIO Chicken Bits (PCI Express bus link power management) */
 #define CSR_GIO_CHICKEN_BITS    (CSR_BASE+0x100)
 
+/* host chicken bits */
+#define CSR_HOST_CHICKEN	(CSR_BASE + 0x204)
+#define CSR_HOST_CHICKEN_PM_IDLE_SRC_DIS_SB_PME	BIT(19)
+
 /* Analog phase-lock-loop configuration  */
 #define CSR_ANA_PLL_CFG         (CSR_BASE+0x20c)
 
@@ -308,7 +312,7 @@
 #define CSR_GP_CNTRL_REG_VAL_MAC_ACCESS_EN           (0x00000001)
 
 #define CSR_GP_CNTRL_REG_MSK_POWER_SAVE_TYPE         (0x07000000)
-#define CSR_GP_CNTRL_REG_FLAG_MAC_POWER_SAVE         (0x04000000)
+#define CSR_GP_CNTRL_REG_FLAG_RFKILL_WAKE_L1A_EN     (0x04000000)
 #define CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW          (0x08000000)
 
 
@@ -316,6 +320,11 @@
 #define CSR_HW_REV_DASH(_val)          (((_val) & 0x0000003) >> 0)
 #define CSR_HW_REV_STEP(_val)          (((_val) & 0x000000C) >> 2)
 
+/* HW RFID */
+#define CSR_HW_RFID_FLAVOR(_val)       (((_val) & 0x000000F) >> 0)
+#define CSR_HW_RFID_DASH(_val)         (((_val) & 0x00000F0) >> 4)
+#define CSR_HW_RFID_STEP(_val)         (((_val) & 0x0000F00) >> 8)
+#define CSR_HW_RFID_TYPE(_val)         (((_val) & 0x0FFF000) >> 12)
 
 /**
  *  hw_rev values
@@ -348,7 +357,8 @@ enum {
 
 /* RF_ID value */
 #define CSR_HW_RF_ID_TYPE_JF		(0x00105000)
-#define CSR_HW_RF_ID_TYPE_HR		(0x00109000)
+#define CSR_HW_RF_ID_TYPE_HR		(0x0010A000)
+#define CSR_HW_RF_ID_TYPE_HRCDB		(0x00109000)
 
 /* EEPROM REG */
 #define CSR_EEPROM_REG_READ_VALID_MSK	(0x00000001)
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-data.h b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-data.h
index d80312b46f16..a80e4202cd03 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-data.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-data.h
@@ -35,19 +35,20 @@
 
 TRACE_EVENT(iwlwifi_dev_tx_data,
 	TP_PROTO(const struct device *dev,
-		 struct sk_buff *skb,
-		 u8 hdr_len, size_t data_len),
-	TP_ARGS(dev, skb, hdr_len, data_len),
+		 struct sk_buff *skb, u8 hdr_len),
+	TP_ARGS(dev, skb, hdr_len),
 	TP_STRUCT__entry(
 		DEV_ENTRY
 
-		__dynamic_array(u8, data, iwl_trace_data(skb) ? data_len : 0)
+		__dynamic_array(u8, data,
+				iwl_trace_data(skb) ? skb->len - hdr_len : 0)
 	),
 	TP_fast_assign(
 		DEV_ASSIGN;
 		if (iwl_trace_data(skb))
 			skb_copy_bits(skb, hdr_len,
-				      __get_dynamic_array(data), data_len);
+				      __get_dynamic_array(data),
+				      skb->len - hdr_len);
 	),
 	TP_printk("[%s] TX frame data", __get_str(dev))
 );
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-io.h b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-io.h
index 1dccae6532cf..4164dc1745ed 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-io.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-io.h
@@ -1,7 +1,7 @@
 /******************************************************************************
  *
  * Copyright(c) 2009 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2016 Intel Deutschland GmbH
+ * Copyright(c) 2016-2017 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -29,6 +29,7 @@
 #define __IWLWIFI_DEVICE_TRACE_IO
 
 #include <linux/tracepoint.h>
+#include <linux/pci.h>
 
 #undef TRACE_SYSTEM
 #define TRACE_SYSTEM iwlwifi_io
@@ -165,6 +166,29 @@ TRACE_EVENT(iwlwifi_dev_irq,
 	TP_printk("%d", 0)
 );
 
+TRACE_EVENT(iwlwifi_dev_irq_msix,
+	TP_PROTO(const struct device *dev, struct msix_entry *msix_entry,
+		 bool defirq, u32 inta_fh, u32 inta_hw),
+	TP_ARGS(dev, msix_entry, defirq, inta_fh, inta_hw),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u32, entry)
+		__field(u8, defirq)
+		__field(u32, inta_fh)
+		__field(u32, inta_hw)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->entry = msix_entry->entry;
+		__entry->defirq = defirq;
+		__entry->inta_fh = inta_fh;
+		__entry->inta_hw = inta_hw;
+	),
+	TP_printk("entry:%d defirq:%d fh:0x%x, hw:0x%x",
+		  __entry->entry, __entry->defirq,
+		  __entry->inta_fh, __entry->inta_hw)
+);
+
 TRACE_EVENT(iwlwifi_dev_ict_read,
 	TP_PROTO(const struct device *dev, u32 index, u32 value),
 	TP_ARGS(dev, index, value),
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-iwlwifi.h b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-iwlwifi.h
index f02e2c89abbb..7f16dcce0995 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-iwlwifi.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-iwlwifi.h
@@ -2,7 +2,7 @@
  *
  * Copyright(c) 2009 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2015 Intel Mobile Communications GmbH
- * Copyright(c) 2016        Intel Deutschland GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -91,8 +91,8 @@ TRACE_EVENT(iwlwifi_dev_tx,
 	TP_PROTO(const struct device *dev, struct sk_buff *skb,
 		 void *tfd, size_t tfdlen,
 		 void *buf0, size_t buf0_len,
-		 void *buf1, size_t buf1_len),
-	TP_ARGS(dev, skb, tfd, tfdlen, buf0, buf0_len, buf1, buf1_len),
+		 int hdr_len),
+	TP_ARGS(dev, skb, tfd, tfdlen, buf0, buf0_len, hdr_len),
 	TP_STRUCT__entry(
 		DEV_ENTRY
 
@@ -105,15 +105,20 @@ TRACE_EVENT(iwlwifi_dev_tx,
 		 * for the possible padding).
 		 */
 		__dynamic_array(u8, buf0, buf0_len)
-		__dynamic_array(u8, buf1, iwl_trace_data(skb) ? 0 : buf1_len)
+		__dynamic_array(u8, buf1, hdr_len > 0 && iwl_trace_data(skb) ?
+						0 : skb->len - hdr_len)
 	),
 	TP_fast_assign(
 		DEV_ASSIGN;
-		__entry->framelen = buf0_len + buf1_len;
+		__entry->framelen = buf0_len;
+		if (hdr_len > 0)
+			__entry->framelen += skb->len - hdr_len;
 		memcpy(__get_dynamic_array(tfd), tfd, tfdlen);
 		memcpy(__get_dynamic_array(buf0), buf0, buf0_len);
-		if (!iwl_trace_data(skb))
-			memcpy(__get_dynamic_array(buf1), buf1, buf1_len);
+		if (hdr_len > 0 && !iwl_trace_data(skb))
+			skb_copy_bits(skb, hdr_len,
+				      __get_dynamic_array(buf1),
+				      skb->len - hdr_len);
 	),
 	TP_printk("[%s] TX %.2x (%zu bytes)",
 		  __get_str(dev), ((u8 *)__get_dynamic_array(buf0))[0],
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-drv.c b/drivers/net/wireless/intel/iwlwifi/iwl-drv.c
index 5cfacb0bca84..6fdb5921e17f 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-drv.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-drv.c
@@ -76,7 +76,7 @@
 #include "iwl-trans.h"
 #include "iwl-op-mode.h"
 #include "iwl-agn-hw.h"
-#include "iwl-fw.h"
+#include "fw/img.h"
 #include "iwl-config.h"
 #include "iwl-modparams.h"
 
@@ -215,9 +215,13 @@ static int iwl_request_firmware(struct iwl_drv *drv, bool first)
 	char tag[8];
 	const char *fw_pre_name;
 
-	if (drv->trans->cfg->device_family == IWL_DEVICE_FAMILY_8000 &&
+	if (drv->trans->cfg->device_family == IWL_DEVICE_FAMILY_9000 &&
 	    CSR_HW_REV_STEP(drv->trans->hw_rev) == SILICON_B_STEP)
 		fw_pre_name = cfg->fw_name_pre_next_step;
+	else if (drv->trans->cfg->integrated &&
+		 CSR_HW_RFID_STEP(drv->trans->hw_rf_id) == SILICON_B_STEP &&
+		 cfg->fw_name_pre_rf_next_step)
+		fw_pre_name = cfg->fw_name_pre_rf_next_step;
 	else
 		fw_pre_name = cfg->fw_name_pre;
 
@@ -1611,11 +1615,11 @@ void iwl_drv_stop(struct iwl_drv *drv)
 
 /* shared module parameters */
 struct iwl_mod_params iwlwifi_mod_params = {
-	.restart_fw = true,
+	.fw_restart = true,
 	.bt_coex_active = true,
 	.power_level = IWL_POWER_INDEX_1,
 	.d0i3_disable = true,
-	.d0i3_entry_delay = 1000,
+	.d0i3_timeout = 1000,
 	.uapsd_disable = IWL_DISABLE_UAPSD_BSS | IWL_DISABLE_UAPSD_P2P_CLIENT,
 	/* the rest are 0 by default */
 };
@@ -1707,7 +1711,7 @@ module_param_named(debug, iwlwifi_mod_params.debug_level, uint,
 MODULE_PARM_DESC(debug, "debug output mask");
 #endif
 
-module_param_named(swcrypto, iwlwifi_mod_params.sw_crypto, int, S_IRUGO);
+module_param_named(swcrypto, iwlwifi_mod_params.swcrypto, int, S_IRUGO);
 MODULE_PARM_DESC(swcrypto, "using crypto in software (default 0 [hardware])");
 module_param_named(11n_disable, iwlwifi_mod_params.disable_11n, uint, S_IRUGO);
 MODULE_PARM_DESC(11n_disable,
@@ -1716,10 +1720,10 @@ module_param_named(amsdu_size, iwlwifi_mod_params.amsdu_size,
 		   int, S_IRUGO);
 MODULE_PARM_DESC(amsdu_size,
 		 "amsdu size 0: 12K for multi Rx queue devices, 4K for other devices 1:4K 2:8K 3:12K (default 0)");
-module_param_named(fw_restart, iwlwifi_mod_params.restart_fw, bool, S_IRUGO);
+module_param_named(fw_restart, iwlwifi_mod_params.fw_restart, bool, S_IRUGO);
 MODULE_PARM_DESC(fw_restart, "restart firmware in case of error (default true)");
 
-module_param_named(antenna_coupling, iwlwifi_mod_params.ant_coupling,
+module_param_named(antenna_coupling, iwlwifi_mod_params.antenna_coupling,
 		   int, S_IRUGO);
 MODULE_PARM_DESC(antenna_coupling,
 		 "specify antenna coupling in dB (default: 0 dB)");
@@ -1778,7 +1782,7 @@ module_param_named(fw_monitor, iwlwifi_mod_params.fw_monitor, bool, S_IRUGO);
 MODULE_PARM_DESC(fw_monitor,
 		 "firmware monitor - to debug FW (default: false - needs lots of memory)");
 
-module_param_named(d0i3_timeout, iwlwifi_mod_params.d0i3_entry_delay,
+module_param_named(d0i3_timeout, iwlwifi_mod_params.d0i3_timeout,
 		   uint, S_IRUGO);
 MODULE_PARM_DESC(d0i3_timeout, "Timeout to D0i3 entry when idle (ms)");
 
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-drv.h b/drivers/net/wireless/intel/iwlwifi/iwl-drv.h
index 6c537e04864e..1f8a2eeb7dff 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-drv.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-drv.h
@@ -79,12 +79,12 @@
 #define NVM_RF_CFG_TX_ANT_MSK(x) ((x >> 8)  & 0xF) /* bits 8-11  */
 #define NVM_RF_CFG_RX_ANT_MSK(x) ((x >> 12) & 0xF) /* bits 12-15 */
 
-#define NVM_RF_CFG_FLAVOR_MSK_FAMILY_8000(x)   (x & 0xF)
-#define NVM_RF_CFG_DASH_MSK_FAMILY_8000(x)   ((x >> 4) & 0xF)
-#define NVM_RF_CFG_STEP_MSK_FAMILY_8000(x)   ((x >> 8) & 0xF)
-#define NVM_RF_CFG_TYPE_MSK_FAMILY_8000(x)   ((x >> 12) & 0xFFF)
-#define NVM_RF_CFG_TX_ANT_MSK_FAMILY_8000(x) ((x >> 24) & 0xF)
-#define NVM_RF_CFG_RX_ANT_MSK_FAMILY_8000(x) ((x >> 28) & 0xF)
+#define EXT_NVM_RF_CFG_FLAVOR_MSK(x)   ((x) & 0xF)
+#define EXT_NVM_RF_CFG_DASH_MSK(x)   (((x) >> 4) & 0xF)
+#define EXT_NVM_RF_CFG_STEP_MSK(x)   (((x) >> 8) & 0xF)
+#define EXT_NVM_RF_CFG_TYPE_MSK(x)   (((x) >> 12) & 0xFFF)
+#define EXT_NVM_RF_CFG_TX_ANT_MSK(x) (((x) >> 24) & 0xF)
+#define EXT_NVM_RF_CFG_RX_ANT_MSK(x) (((x) >> 28) & 0xF)
 
 /**
  * DOC: Driver system flows - drv component
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-eeprom-parse.h b/drivers/net/wireless/intel/iwlwifi/iwl-eeprom-parse.h
index e04a91d70a15..b33888991b94 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-eeprom-parse.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-eeprom-parse.h
@@ -121,15 +121,6 @@ struct iwl_nvm_data *
 iwl_parse_eeprom_data(struct device *dev, const struct iwl_cfg *cfg,
 		      const u8 *eeprom, size_t eeprom_size);
 
-/**
- * iwl_free_nvm_data - free NVM data
- * @data: the data to free
- */
-static inline void iwl_free_nvm_data(struct iwl_nvm_data *data)
-{
-	kfree(data);
-}
-
 int iwl_nvm_check_version(struct iwl_nvm_data *data,
 			  struct iwl_trans *trans);
 
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-fh.h b/drivers/net/wireless/intel/iwlwifi/iwl-fh.h
index 62f9fe926d78..66e5db41e559 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-fh.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-fh.h
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2015 - 2016 Intel Deutschland GmbH
+ * Copyright(c) 2015 - 2017 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2015 - 2016 Intel Deutschland GmbH
+ * Copyright(c) 2015 - 2017 Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -66,6 +66,7 @@
 #define __iwl_fh_h__
 
 #include <linux/types.h>
+#include <linux/bitfield.h>
 
 /****************************/
 /* Flow Handler Definitions */
@@ -77,6 +78,8 @@
  */
 #define FH_MEM_LOWER_BOUND                   (0x1000)
 #define FH_MEM_UPPER_BOUND                   (0x2000)
+#define FH_MEM_LOWER_BOUND_GEN2              (0xa06000)
+#define FH_MEM_UPPER_BOUND_GEN2              (0xa08000)
 
 /**
  * Keep-Warm (KW) buffer base address.
@@ -476,13 +479,12 @@ static inline unsigned int FH_MEM_CBBC_QUEUE(struct iwl_trans *trans,
 #define RFH_GEN_CFG	0xA09800
 #define RFH_GEN_CFG_SERVICE_DMA_SNOOP	BIT(0)
 #define RFH_GEN_CFG_RFH_DMA_SNOOP	BIT(1)
-#define RFH_GEN_CFG_RB_CHUNK_SIZE_POS	4
+#define RFH_GEN_CFG_RB_CHUNK_SIZE	BIT(4)
 #define RFH_GEN_CFG_RB_CHUNK_SIZE_128	1
 #define RFH_GEN_CFG_RB_CHUNK_SIZE_64	0
-#define RFH_GEN_CFG_DEFAULT_RXQ_NUM_MASK 0xF00
-#define RFH_GEN_CFG_DEFAULT_RXQ_NUM_POS 8
-
-#define DEFAULT_RXQ_NUM			0
+/* the driver assumes everywhere that the default RXQ is 0 */
+#define RFH_GEN_CFG_DEFAULT_RXQ_NUM	0xF00
+#define RFH_GEN_CFG_VAL(_n, _v)		FIELD_PREP(RFH_GEN_CFG_ ## _n, _v)
 
 /* end of 9000 rx series registers */
 
@@ -653,6 +655,17 @@ static inline u8 iwl_get_dma_hi_addr(dma_addr_t addr)
 {
 	return (sizeof(addr) > sizeof(u32) ? upper_32_bits(addr) : 0) & 0xF;
 }
+
+/**
+ * enum iwl_tfd_tb_hi_n_len - TB hi_n_len bits
+ * @TB_HI_N_LEN_ADDR_HI_MSK: high 4 bits (to make it 36) of DMA address
+ * @TB_HI_N_LEN_LEN_MSK: length of the TB
+ */
+enum iwl_tfd_tb_hi_n_len {
+	TB_HI_N_LEN_ADDR_HI_MSK	= 0xf,
+	TB_HI_N_LEN_LEN_MSK	= 0xfff0,
+};
+
 /**
  * struct iwl_tfd_tb transmit buffer descriptor within transmit frame descriptor
  *
@@ -660,8 +673,7 @@ static inline u8 iwl_get_dma_hi_addr(dma_addr_t addr)
  *
  * @lo: low [31:0] portion of the dma address of TX buffer
  * 	every even is unaligned on 16 bit boundary
- * @hi_n_len 0-3 [35:32] portion of dma
- *	     4-15 length of the tx buffer
+ * @hi_n_len: &enum iwl_tfd_tb_hi_n_len
  */
 struct iwl_tfd_tb {
 	__le32 lo;
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-io.c b/drivers/net/wireless/intel/iwlwifi/iwl-io.c
index 9c8b09cf1f7b..c527b8c10370 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-io.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-io.c
@@ -241,12 +241,12 @@ IWL_EXPORT_SYMBOL(iwl_clear_bits_prph);
 
 void iwl_force_nmi(struct iwl_trans *trans)
 {
-	if (trans->cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+	if (trans->cfg->device_family < IWL_DEVICE_FAMILY_8000) {
 		iwl_write_prph(trans, DEVICE_SET_NMI_REG,
 			       DEVICE_SET_NMI_VAL_DRV);
 		iwl_write_prph(trans, DEVICE_SET_NMI_REG,
 			       DEVICE_SET_NMI_VAL_HW);
-	} else if (trans->cfg->gen2) {
+	} else if (trans->cfg->device_family == IWL_DEVICE_FAMILY_A000) {
 		iwl_write_prph(trans, UREG_NIC_SET_NMI_DRIVER,
 			       DEVICE_SET_NMI_8000_VAL);
 	} else {
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-modparams.h b/drivers/net/wireless/intel/iwlwifi/iwl-modparams.h
index 4d32b10fe50c..a41c46e63eb1 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-modparams.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-modparams.h
@@ -102,32 +102,32 @@ enum iwl_uapsd_disable {
  *
  * Holds the module parameters
  *
- * @sw_crypto: using hardware encryption, default = 0
+ * @swcrypto: using hardware encryption, default = 0
  * @disable_11n: disable 11n capabilities, default = 0,
  *	use IWL_[DIS,EN]ABLE_HT_* constants
  * @amsdu_size: See &enum iwl_amsdu_size.
- * @restart_fw: restart firmware, default = 1
+ * @fw_restart: restart firmware, default = 1
  * @bt_coex_active: enable bt coex, default = true
  * @led_mode: system default, default = 0
  * @power_save: enable power save, default = false
  * @power_level: power level, default = 1
  * @debug_level: levels are IWL_DL_*
- * @ant_coupling: antenna coupling in dB, default = 0
+ * @antenna_coupling: antenna coupling in dB, default = 0
  * @nvm_file: specifies a external NVM file
- * @uapsd_disable: disable U-APSD, see %enum iwl_uapsd_disable, default =
+ * @uapsd_disable: disable U-APSD, see &enum iwl_uapsd_disable, default =
  *	IWL_DISABLE_UAPSD_BSS | IWL_DISABLE_UAPSD_P2P_CLIENT
  * @d0i3_disable: disable d0i3, default = 1,
- * @d0i3_entry_delay: time to wait after no refs are taken before
+ * @d0i3_timeout: time to wait after no refs are taken before
  *	entering D0i3 (in msecs)
  * @lar_disable: disable LAR (regulatory), default = 0
  * @fw_monitor: allow to use firmware monitor
  * @disable_11ac: disable VHT capabilities, default = false.
  */
 struct iwl_mod_params {
-	int sw_crypto;
+	int swcrypto;
 	unsigned int disable_11n;
 	int amsdu_size;
-	bool restart_fw;
+	bool fw_restart;
 	bool bt_coex_active;
 	int led_mode;
 	bool power_save;
@@ -135,11 +135,11 @@ struct iwl_mod_params {
 #ifdef CONFIG_IWLWIFI_DEBUG
 	u32 debug_level;
 #endif
-	int ant_coupling;
+	int antenna_coupling;
 	char *nvm_file;
 	u32 uapsd_disable;
 	bool d0i3_disable;
-	unsigned int d0i3_entry_delay;
+	unsigned int d0i3_timeout;
 	bool lar_disable;
 	bool fw_monitor;
 	bool disable_11ac;
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-nvm-parse.c b/drivers/net/wireless/intel/iwlwifi/iwl-nvm-parse.c
index 721ae6bef5da..5c08f4d40f6a 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-nvm-parse.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-nvm-parse.c
@@ -94,30 +94,21 @@ enum wkp_nvm_offsets {
 	XTAL_CALIB = 0x316 - NVM_CALIB_SECTION
 };
 
-enum family_8000_nvm_offsets {
+enum ext_nvm_offsets {
 	/* NVM HW-Section offset (in words) definitions */
-	HW_ADDR0_WFPM_FAMILY_8000 = 0x12,
-	HW_ADDR1_WFPM_FAMILY_8000 = 0x16,
-	HW_ADDR0_PCIE_FAMILY_8000 = 0x8A,
-	HW_ADDR1_PCIE_FAMILY_8000 = 0x8E,
-	MAC_ADDRESS_OVERRIDE_FAMILY_8000 = 1,
+	MAC_ADDRESS_OVERRIDE_EXT_NVM = 1,
 
 	/* NVM SW-Section offset (in words) definitions */
-	NVM_SW_SECTION_FAMILY_8000 = 0x1C0,
-	NVM_VERSION_FAMILY_8000 = 0,
-	RADIO_CFG_FAMILY_8000 = 0,
+	NVM_VERSION_EXT_NVM = 0,
+	RADIO_CFG_FAMILY_EXT_NVM = 0,
 	SKU_FAMILY_8000 = 2,
 	N_HW_ADDRS_FAMILY_8000 = 3,
 
 	/* NVM REGULATORY -Section offset (in words) definitions */
-	NVM_CHANNELS_FAMILY_8000 = 0,
-	NVM_LAR_OFFSET_FAMILY_8000_OLD = 0x4C7,
-	NVM_LAR_OFFSET_FAMILY_8000 = 0x507,
-	NVM_LAR_ENABLED_FAMILY_8000 = 0x7,
-
-	/* NVM calibration section offset (in words) definitions */
-	NVM_CALIB_SECTION_FAMILY_8000 = 0x2B8,
-	XTAL_CALIB_FAMILY_8000 = 0x316 - NVM_CALIB_SECTION_FAMILY_8000
+	NVM_CHANNELS_EXTENDED = 0,
+	NVM_LAR_OFFSET_OLD = 0x4C7,
+	NVM_LAR_OFFSET = 0x507,
+	NVM_LAR_ENABLED = 0x7,
 };
 
 /* SKU Capabilities (actual values from NVM definition) */
@@ -141,7 +132,7 @@ static const u8 iwl_nvm_channels[] = {
 	149, 153, 157, 161, 165
 };
 
-static const u8 iwl_nvm_channels_family_8000[] = {
+static const u8 iwl_ext_nvm_channels[] = {
 	/* 2.4 GHz */
 	1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
 	/* 5 GHz */
@@ -151,9 +142,9 @@ static const u8 iwl_nvm_channels_family_8000[] = {
 };
 
 #define IWL_NUM_CHANNELS		ARRAY_SIZE(iwl_nvm_channels)
-#define IWL_NUM_CHANNELS_FAMILY_8000	ARRAY_SIZE(iwl_nvm_channels_family_8000)
+#define IWL_NUM_CHANNELS_EXT	ARRAY_SIZE(iwl_ext_nvm_channels)
 #define NUM_2GHZ_CHANNELS		14
-#define NUM_2GHZ_CHANNELS_FAMILY_8000	14
+#define NUM_2GHZ_CHANNELS_EXT	14
 #define FIRST_2GHZ_HT_MINUS		5
 #define LAST_2GHZ_HT_PLUS		9
 #define LAST_5GHZ_HT			165
@@ -219,7 +210,7 @@ static u32 iwl_get_channel_flags(u8 ch_num, int ch_idx, bool is_5ghz,
 	u32 flags = IEEE80211_CHAN_NO_HT40;
 	u32 last_5ghz_ht = LAST_5GHZ_HT;
 
-	if (cfg->device_family == IWL_DEVICE_FAMILY_8000)
+	if (cfg->ext_nvm)
 		last_5ghz_ht = LAST_5GHZ_HT_FAMILY_8000;
 
 	if (!is_5ghz && (nvm_flags & NVM_CHANNEL_40MHZ)) {
@@ -273,14 +264,14 @@ static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
 	int num_of_ch, num_2ghz_channels;
 	const u8 *nvm_chan;
 
-	if (cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+	if (!cfg->ext_nvm) {
 		num_of_ch = IWL_NUM_CHANNELS;
 		nvm_chan = &iwl_nvm_channels[0];
 		num_2ghz_channels = NUM_2GHZ_CHANNELS;
 	} else {
-		num_of_ch = IWL_NUM_CHANNELS_FAMILY_8000;
-		nvm_chan = &iwl_nvm_channels_family_8000[0];
-		num_2ghz_channels = NUM_2GHZ_CHANNELS_FAMILY_8000;
+		num_of_ch = IWL_NUM_CHANNELS_EXT;
+		nvm_chan = &iwl_ext_nvm_channels[0];
+		num_2ghz_channels = NUM_2GHZ_CHANNELS_EXT;
 	}
 
 	for (ch_idx = 0; ch_idx < num_of_ch; ch_idx++) {
@@ -479,7 +470,7 @@ IWL_EXPORT_SYMBOL(iwl_init_sbands);
 static int iwl_get_sku(const struct iwl_cfg *cfg, const __le16 *nvm_sw,
 		       const __le16 *phy_sku)
 {
-	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
+	if (!cfg->ext_nvm)
 		return le16_to_cpup(nvm_sw + SKU);
 
 	return le32_to_cpup((__le32 *)(phy_sku + SKU_FAMILY_8000));
@@ -487,20 +478,20 @@ static int iwl_get_sku(const struct iwl_cfg *cfg, const __le16 *nvm_sw,
 
 static int iwl_get_nvm_version(const struct iwl_cfg *cfg, const __le16 *nvm_sw)
 {
-	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
+	if (!cfg->ext_nvm)
 		return le16_to_cpup(nvm_sw + NVM_VERSION);
 	else
 		return le32_to_cpup((__le32 *)(nvm_sw +
-					       NVM_VERSION_FAMILY_8000));
+					       NVM_VERSION_EXT_NVM));
 }
 
 static int iwl_get_radio_cfg(const struct iwl_cfg *cfg, const __le16 *nvm_sw,
 			     const __le16 *phy_sku)
 {
-	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
+	if (!cfg->ext_nvm)
 		return le16_to_cpup(nvm_sw + RADIO_CFG);
 
-	return le32_to_cpup((__le32 *)(phy_sku + RADIO_CFG_FAMILY_8000));
+	return le32_to_cpup((__le32 *)(phy_sku + RADIO_CFG_FAMILY_EXT_NVM));
 
 }
 
@@ -508,7 +499,7 @@ static int iwl_get_n_hw_addrs(const struct iwl_cfg *cfg, const __le16 *nvm_sw)
 {
 	int n_hw_addr;
 
-	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
+	if (!cfg->ext_nvm)
 		return le16_to_cpup(nvm_sw + N_HW_ADDRS);
 
 	n_hw_addr = le32_to_cpup((__le32 *)(nvm_sw + N_HW_ADDRS_FAMILY_8000));
@@ -520,7 +511,7 @@ static void iwl_set_radio_cfg(const struct iwl_cfg *cfg,
 			      struct iwl_nvm_data *data,
 			      u32 radio_cfg)
 {
-	if (cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+	if (!cfg->ext_nvm) {
 		data->radio_cfg_type = NVM_RF_CFG_TYPE_MSK(radio_cfg);
 		data->radio_cfg_step = NVM_RF_CFG_STEP_MSK(radio_cfg);
 		data->radio_cfg_dash = NVM_RF_CFG_DASH_MSK(radio_cfg);
@@ -529,12 +520,12 @@ static void iwl_set_radio_cfg(const struct iwl_cfg *cfg,
 	}
 
 	/* set the radio configuration for family 8000 */
-	data->radio_cfg_type = NVM_RF_CFG_TYPE_MSK_FAMILY_8000(radio_cfg);
-	data->radio_cfg_step = NVM_RF_CFG_STEP_MSK_FAMILY_8000(radio_cfg);
-	data->radio_cfg_dash = NVM_RF_CFG_DASH_MSK_FAMILY_8000(radio_cfg);
-	data->radio_cfg_pnum = NVM_RF_CFG_FLAVOR_MSK_FAMILY_8000(radio_cfg);
-	data->valid_tx_ant = NVM_RF_CFG_TX_ANT_MSK_FAMILY_8000(radio_cfg);
-	data->valid_rx_ant = NVM_RF_CFG_RX_ANT_MSK_FAMILY_8000(radio_cfg);
+	data->radio_cfg_type = EXT_NVM_RF_CFG_TYPE_MSK(radio_cfg);
+	data->radio_cfg_step = EXT_NVM_RF_CFG_STEP_MSK(radio_cfg);
+	data->radio_cfg_dash = EXT_NVM_RF_CFG_DASH_MSK(radio_cfg);
+	data->radio_cfg_pnum = EXT_NVM_RF_CFG_FLAVOR_MSK(radio_cfg);
+	data->valid_tx_ant = EXT_NVM_RF_CFG_TX_ANT_MSK(radio_cfg);
+	data->valid_rx_ant = EXT_NVM_RF_CFG_RX_ANT_MSK(radio_cfg);
 }
 
 static void iwl_flip_hw_address(__le32 mac_addr0, __le32 mac_addr1, u8 *dest)
@@ -587,7 +578,7 @@ static void iwl_set_hw_address_family_8000(struct iwl_trans *trans,
 		};
 
 		hw_addr = (const u8 *)(mac_override +
-				 MAC_ADDRESS_OVERRIDE_FAMILY_8000);
+				 MAC_ADDRESS_OVERRIDE_EXT_NVM);
 
 		/*
 		 * Store the MAC address from MAO section.
@@ -629,7 +620,7 @@ static int iwl_set_hw_address(struct iwl_trans *trans,
 {
 	if (cfg->mac_addr_from_csr) {
 		iwl_set_hw_address_from_csr(trans, data);
-	} else if (cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+	} else if (!cfg->ext_nvm) {
 		const u8 *hw_addr = (const u8 *)(nvm_hw + HW_ADDR);
 
 		/* The byte order is little endian 16 bit, meaning 214365 */
@@ -649,6 +640,8 @@ static int iwl_set_hw_address(struct iwl_trans *trans,
 		return -EINVAL;
 	}
 
+	IWL_INFO(trans, "base HW address: %pM\n", data->hw_addr);
+
 	return 0;
 }
 
@@ -666,7 +659,7 @@ iwl_parse_nvm_data(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	u16 lar_config;
 	const __le16 *ch_section;
 
-	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
+	if (!cfg->ext_nvm)
 		data = kzalloc(sizeof(*data) +
 			       sizeof(struct ieee80211_channel) *
 			       IWL_NUM_CHANNELS,
@@ -674,7 +667,7 @@ iwl_parse_nvm_data(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	else
 		data = kzalloc(sizeof(*data) +
 			       sizeof(struct ieee80211_channel) *
-			       IWL_NUM_CHANNELS_FAMILY_8000,
+			       IWL_NUM_CHANNELS_EXT,
 			       GFP_KERNEL);
 	if (!data)
 		return NULL;
@@ -700,7 +693,7 @@ iwl_parse_nvm_data(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 
 	data->n_hw_addrs = iwl_get_n_hw_addrs(cfg, nvm_sw);
 
-	if (cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+	if (!cfg->ext_nvm) {
 		/* Checking for required sections */
 		if (!nvm_calib) {
 			IWL_ERR(trans,
@@ -715,14 +708,14 @@ iwl_parse_nvm_data(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 		ch_section = &nvm_sw[NVM_CHANNELS];
 	} else {
 		u16 lar_offset = data->nvm_version < 0xE39 ?
-				 NVM_LAR_OFFSET_FAMILY_8000_OLD :
-				 NVM_LAR_OFFSET_FAMILY_8000;
+				 NVM_LAR_OFFSET_OLD :
+				 NVM_LAR_OFFSET;
 
 		lar_config = le16_to_cpup(regulatory + lar_offset);
 		data->lar_enabled = !!(lar_config &
-				       NVM_LAR_ENABLED_FAMILY_8000);
+				       NVM_LAR_ENABLED);
 		lar_enabled = data->lar_enabled;
-		ch_section = &regulatory[NVM_CHANNELS_FAMILY_8000];
+		ch_section = &regulatory[NVM_CHANNELS_EXTENDED];
 	}
 
 	/* If no valid mac address was found - bail out */
@@ -746,7 +739,7 @@ static u32 iwl_nvm_get_regdom_bw_flags(const u8 *nvm_chan,
 	u32 flags = NL80211_RRF_NO_HT40;
 	u32 last_5ghz_ht = LAST_5GHZ_HT;
 
-	if (cfg->device_family == IWL_DEVICE_FAMILY_8000)
+	if (cfg->ext_nvm)
 		last_5ghz_ht = LAST_5GHZ_HT_FAMILY_8000;
 
 	if (ch_idx < NUM_2GHZ_CHANNELS &&
@@ -793,8 +786,8 @@ iwl_parse_nvm_mcc_info(struct device *dev, const struct iwl_cfg *cfg,
 {
 	int ch_idx;
 	u16 ch_flags, prev_ch_flags = 0;
-	const u8 *nvm_chan = cfg->device_family == IWL_DEVICE_FAMILY_8000 ?
-			     iwl_nvm_channels_family_8000 : iwl_nvm_channels;
+	const u8 *nvm_chan = cfg->ext_nvm ?
+			     iwl_ext_nvm_channels : iwl_nvm_channels;
 	struct ieee80211_regdomain *regd;
 	int size_of_regd;
 	struct ieee80211_reg_rule *rule;
@@ -802,8 +795,8 @@ iwl_parse_nvm_mcc_info(struct device *dev, const struct iwl_cfg *cfg,
 	int center_freq, prev_center_freq = 0;
 	int valid_rules = 0;
 	bool new_rule;
-	int max_num_ch = cfg->device_family == IWL_DEVICE_FAMILY_8000 ?
-			 IWL_NUM_CHANNELS_FAMILY_8000 : IWL_NUM_CHANNELS;
+	int max_num_ch = cfg->ext_nvm ?
+			 IWL_NUM_CHANNELS_EXT : IWL_NUM_CHANNELS;
 
 	if (WARN_ON_ONCE(num_of_ch > NL80211_MAX_SUPP_REG_RULES))
 		return ERR_PTR(-EINVAL);
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-phy-db.c b/drivers/net/wireless/intel/iwlwifi/iwl-phy-db.c
index 2893826d7d2b..b7cd813ba70f 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-phy-db.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-phy-db.c
@@ -112,30 +112,12 @@ enum iwl_phy_db_section_type {
 
 #define PHY_DB_CMD 0x6c
 
-/*
- * phy db - configure operational ucode
- */
-struct iwl_phy_db_cmd {
-	__le16 type;
-	__le16 length;
-	u8 data[];
-} __packed;
-
 /* for parsing of tx power channel group data that comes from the firmware*/
 struct iwl_phy_db_chg_txp {
 	__le32 space;
 	__le16 max_channel_idx;
 } __packed;
 
-/*
- * phy db - Receive phy db chunk after calibrations
- */
-struct iwl_calib_res_notif_phy_db {
-	__le16 type;
-	__le16 length;
-	u8 data[];
-} __packed;
-
 struct iwl_phy_db *iwl_phy_db_init(struct iwl_trans *trans)
 {
 	struct iwl_phy_db *phy_db = kzalloc(sizeof(struct iwl_phy_db),
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-prph.h b/drivers/net/wireless/intel/iwlwifi/iwl-prph.h
index 77efbb78e867..6772c59b7764 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-prph.h
@@ -66,6 +66,7 @@
 
 #ifndef	__iwl_prph_h__
 #define __iwl_prph_h__
+#include <linux/bitfield.h>
 
 /*
  * Registers in this file are internal, not PCI bus memory mapped.
@@ -247,14 +248,14 @@
 #define SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN (19)
 #define SCD_QUEUE_STTS_REG_MSK		(0x017F0000)
 
-#define SCD_QUEUE_CTX_REG1_CREDIT_POS		(8)
-#define SCD_QUEUE_CTX_REG1_CREDIT_MSK		(0x00FFFF00)
-#define SCD_QUEUE_CTX_REG1_SUPER_CREDIT_POS	(24)
-#define SCD_QUEUE_CTX_REG1_SUPER_CREDIT_MSK	(0xFF000000)
-#define SCD_QUEUE_CTX_REG2_WIN_SIZE_POS		(0)
-#define SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK		(0x0000007F)
-#define SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS	(16)
-#define SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK	(0x007F0000)
+#define SCD_QUEUE_CTX_REG1_CREDIT		(0x00FFFF00)
+#define SCD_QUEUE_CTX_REG1_SUPER_CREDIT		(0xFF000000)
+#define SCD_QUEUE_CTX_REG1_VAL(_n, _v)		FIELD_PREP(SCD_QUEUE_CTX_REG1_ ## _n, _v)
+
+#define SCD_QUEUE_CTX_REG2_WIN_SIZE		(0x0000007F)
+#define SCD_QUEUE_CTX_REG2_FRAME_LIMIT		(0x007F0000)
+#define SCD_QUEUE_CTX_REG2_VAL(_n, _v)		FIELD_PREP(SCD_QUEUE_CTX_REG2_ ## _n, _v)
+
 #define SCD_GP_CTRL_ENABLE_31_QUEUES		BIT(0)
 #define SCD_GP_CTRL_AUTO_ACTIVE_MODE		BIT(18)
 
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-trans.c b/drivers/net/wireless/intel/iwlwifi/iwl-trans.c
index 0bde26bab15d..784bdd0ed233 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-trans.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-trans.c
@@ -102,6 +102,8 @@ struct iwl_trans *iwl_trans_alloc(unsigned int priv_size,
 	if (!trans->dev_cmd_pool)
 		return NULL;
 
+	WARN_ON(!ops->wait_txq_empty && !ops->wait_tx_queues_empty);
+
 	return trans;
 }
 
@@ -115,7 +117,7 @@ int iwl_trans_send_cmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 	int ret;
 
 	if (unlikely(!(cmd->flags & CMD_SEND_IN_RFKILL) &&
-		     test_bit(STATUS_RFKILL, &trans->status)))
+		     test_bit(STATUS_RFKILL_OPMODE, &trans->status)))
 		return -ERFKILL;
 
 	if (unlikely(test_bit(STATUS_FW_ERROR, &trans->status)))
@@ -141,6 +143,9 @@ int iwl_trans_send_cmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 	if (!(cmd->flags & CMD_ASYNC))
 		lock_map_release(&trans->sync_cmd_lockdep_map);
 
+	if (WARN_ON((cmd->flags & CMD_WANT_SKB) && !ret && !cmd->resp_pkt))
+		return -EIO;
+
 	return ret;
 }
 IWL_EXPORT_SYMBOL(iwl_trans_send_cmd);
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-trans.h b/drivers/net/wireless/intel/iwlwifi/iwl-trans.h
index 0ebfdbb22992..eb6842abb4c7 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-trans.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-trans.h
@@ -74,8 +74,9 @@
 
 #include "iwl-debug.h"
 #include "iwl-config.h"
-#include "iwl-fw.h"
+#include "fw/img.h"
 #include "iwl-op-mode.h"
+#include "fw/api.h"
 
 /**
  * DOC: Transport layer - what is it ?
@@ -111,104 +112,6 @@
  *	6) Eventually, the free function will be called.
  */
 
-/**
- * DOC: Host command section
- *
- * A host command is a command issued by the upper layer to the fw. There are
- * several versions of fw that have several APIs. The transport layer is
- * completely agnostic to these differences.
- * The transport does provide helper functionality (i.e. SYNC / ASYNC mode),
- */
-#define SEQ_TO_QUEUE(s)	(((s) >> 8) & 0x1f)
-#define QUEUE_TO_SEQ(q)	(((q) & 0x1f) << 8)
-#define SEQ_TO_INDEX(s)	((s) & 0xff)
-#define INDEX_TO_SEQ(i)	((i) & 0xff)
-#define SEQ_RX_FRAME	cpu_to_le16(0x8000)
-
-/*
- * those functions retrieve specific information from
- * the id field in the iwl_host_cmd struct which contains
- * the command id, the group id and the version of the command
- * and vice versa
-*/
-static inline u8 iwl_cmd_opcode(u32 cmdid)
-{
-	return cmdid & 0xFF;
-}
-
-static inline u8 iwl_cmd_groupid(u32 cmdid)
-{
-	return ((cmdid & 0xFF00) >> 8);
-}
-
-static inline u8 iwl_cmd_version(u32 cmdid)
-{
-	return ((cmdid & 0xFF0000) >> 16);
-}
-
-static inline u32 iwl_cmd_id(u8 opcode, u8 groupid, u8 version)
-{
-	return opcode + (groupid << 8) + (version << 16);
-}
-
-/* make u16 wide id out of u8 group and opcode */
-#define WIDE_ID(grp, opcode) ((grp << 8) | opcode)
-#define DEF_ID(opcode) ((1 << 8) | (opcode))
-
-/* due to the conversion, this group is special; new groups
- * should be defined in the appropriate fw-api header files
- */
-#define IWL_ALWAYS_LONG_GROUP	1
-
-/**
- * struct iwl_cmd_header
- *
- * This header format appears in the beginning of each command sent from the
- * driver, and each response/notification received from uCode.
- */
-struct iwl_cmd_header {
-	u8 cmd;		/* Command ID:  REPLY_RXON, etc. */
-	u8 group_id;
-	/*
-	 * The driver sets up the sequence number to values of its choosing.
-	 * uCode does not use this value, but passes it back to the driver
-	 * when sending the response to each driver-originated command, so
-	 * the driver can match the response to the command.  Since the values
-	 * don't get used by uCode, the driver may set up an arbitrary format.
-	 *
-	 * There is one exception:  uCode sets bit 15 when it originates
-	 * the response/notification, i.e. when the response/notification
-	 * is not a direct response to a command sent by the driver.  For
-	 * example, uCode issues REPLY_RX when it sends a received frame
-	 * to the driver; it is not a direct response to any driver command.
-	 *
-	 * The Linux driver uses the following format:
-	 *
-	 *  0:7		tfd index - position within TX queue
-	 *  8:12	TX queue id
-	 *  13:14	reserved
-	 *  15		unsolicited RX or uCode-originated notification
-	 */
-	__le16 sequence;
-} __packed;
-
-/**
- * struct iwl_cmd_header_wide
- *
- * This header format appears in the beginning of each command sent from the
- * driver, and each response/notification received from uCode.
- * this is the wide version that contains more information about the command
- * like length, version and command type
- */
-struct iwl_cmd_header_wide {
-	u8 cmd;
-	u8 group_id;
-	__le16 sequence;
-	__le16 length;
-	u8 reserved;
-	u8 version;
-} __packed;
-
 #define FH_RSCSR_FRAME_SIZE_MSK		0x00003FFF	/* bits 0-13 */
 #define FH_RSCSR_FRAME_INVALID		0x55550000
 #define FH_RSCSR_FRAME_ALIGN		0x40
@@ -308,7 +211,7 @@ struct iwl_device_cmd {
 #define IWL_MAX_CMD_TBS_PER_TFD	2
 
 /**
- * struct iwl_hcmd_dataflag - flag for each one of the chunks of the command
+ * enum iwl_hcmd_dataflag - flag for each one of the chunks of the command
  *
  * @IWL_HCMD_DFL_NOCOPY: By default, the command is copied to the host command's
  *	ring. The transport layer doesn't map the command's buffer to DMA, but
@@ -419,7 +322,8 @@ enum iwl_d3_status {
  * @STATUS_DEVICE_ENABLED: APM is enabled
  * @STATUS_TPOWER_PMI: the device might be asleep (need to wake it up)
  * @STATUS_INT_ENABLED: interrupts are enabled
- * @STATUS_RFKILL: the HW RFkill switch is in KILL position
+ * @STATUS_RFKILL_HW: the actual HW state of the RF-kill switch
+ * @STATUS_RFKILL_OPMODE: RF-kill state reported to opmode
  * @STATUS_FW_ERROR: the fw is in error state
  * @STATUS_TRANS_GOING_IDLE: shutting down the trans, only special commands
  *	are sent
@@ -431,7 +335,8 @@ enum iwl_trans_status {
 	STATUS_DEVICE_ENABLED,
 	STATUS_TPOWER_PMI,
 	STATUS_INT_ENABLED,
-	STATUS_RFKILL,
+	STATUS_RFKILL_HW,
+	STATUS_RFKILL_OPMODE,
 	STATUS_FW_ERROR,
 	STATUS_TRANS_GOING_IDLE,
 	STATUS_TRANS_IDLE,
@@ -533,44 +438,6 @@ struct iwl_trans_txq_scd_cfg {
 	int frame_limit;
 };
 
-/* Available options for &struct iwl_tx_queue_cfg_cmd */
-enum iwl_tx_queue_cfg_actions {
-	TX_QUEUE_CFG_ENABLE_QUEUE		= BIT(0),
-	TX_QUEUE_CFG_TFD_SHORT_FORMAT		= BIT(1),
-};
-
-/**
- * struct iwl_tx_queue_cfg_cmd - txq hw scheduler config command
- * @sta_id: station id
- * @tid: tid of the queue
- * @flags: Bit 0 - on enable, off - disable, Bit 1 - short TFD format
- * @cb_size: size of TFD cyclic buffer. Value is exponent - 3.
- *	Minimum value 0 (8 TFDs), maximum value 5 (256 TFDs)
- * @byte_cnt_addr: address of byte count table
- * @tfdq_addr: address of TFD circular buffer
- */
-struct iwl_tx_queue_cfg_cmd {
-	u8 sta_id;
-	u8 tid;
-	__le16 flags;
-	__le32 cb_size;
-	__le64 byte_cnt_addr;
-	__le64 tfdq_addr;
-} __packed; /* TX_QUEUE_CFG_CMD_API_S_VER_2 */
-
-/**
- * struct iwl_tx_queue_cfg_rsp - response to txq hw scheduler config
- * @queue_number: queue number assigned to this RA -TID
- * @flags: set on failure
- * @write_pointer: initial value for write pointer
- */
-struct iwl_tx_queue_cfg_rsp {
-	__le16 queue_number;
-	__le16 flags;
-	__le16 write_pointer;
-	__le16 reserved;
-} __packed; /* TX_QUEUE_CFG_RSP_API_S_VER_2 */
-
 /**
  * struct iwl_trans_ops - transport specific operations
  *
@@ -615,11 +482,14 @@ struct iwl_tx_queue_cfg_rsp {
  *	iwl_trans_ac_txq_enable wrapper. fw_alive must have been called before
  *	this one. The op_mode must not configure the HCMD queue. The scheduler
  *	configuration may be %NULL, in which case the hardware will not be
- *	configured. May sleep.
+ *	configured. If true is returned, the operation mode needs to increment
+ *	the sequence number of the packets routed to this queue because of a
+ *	hardware scheduler bug. May sleep.
  * @txq_disable: de-configure a Tx queue to send AMPDUs
  *	Must be atomic
  * @txq_set_shared_mode: change Tx queue shared/unshared marking
- * @wait_tx_queue_empty: wait until tx queues are empty. May sleep.
+ * @wait_tx_queues_empty: wait until tx queues are empty. May sleep.
+ * @wait_txq_empty: wait until specific tx queue is empty. May sleep.
  * @freeze_txq_timer: prevents the timer of the queue from firing until the
  *	queue is set to awake. Must be atomic.
  * @block_txq_ptrs: stop updating the write pointers of the Tx queues. Note
@@ -676,7 +546,7 @@ struct iwl_trans_ops {
 	void (*reclaim)(struct iwl_trans *trans, int queue, int ssn,
 			struct sk_buff_head *skbs);
 
-	void (*txq_enable)(struct iwl_trans *trans, int queue, u16 ssn,
+	bool (*txq_enable)(struct iwl_trans *trans, int queue, u16 ssn,
 			   const struct iwl_trans_txq_scd_cfg *cfg,
 			   unsigned int queue_wdg_timeout);
 	void (*txq_disable)(struct iwl_trans *trans, int queue,
@@ -692,6 +562,7 @@ struct iwl_trans_ops {
 				    bool shared);
 
 	int (*wait_tx_queues_empty)(struct iwl_trans *trans, u32 txq_bm);
+	int (*wait_txq_empty)(struct iwl_trans *trans, int queue);
 	void (*freeze_txq_timer)(struct iwl_trans *trans, unsigned long txqs,
 				 bool freeze);
 	void (*block_txq_ptrs)(struct iwl_trans *trans, bool block);
@@ -1041,13 +912,7 @@ iwl_trans_dump_data(struct iwl_trans *trans,
 static inline struct iwl_device_cmd *
 iwl_trans_alloc_tx_cmd(struct iwl_trans *trans)
 {
-	struct iwl_device_cmd *dev_cmd_ptr =
-		kmem_cache_alloc(trans->dev_cmd_pool, GFP_ATOMIC);
-
-	if (unlikely(dev_cmd_ptr == NULL))
-		return NULL;
-
-	return dev_cmd_ptr;
+	return kmem_cache_alloc(trans->dev_cmd_pool, GFP_ATOMIC);
 }
 
 int iwl_trans_send_cmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd);
@@ -1089,7 +954,7 @@ static inline void iwl_trans_txq_disable(struct iwl_trans *trans, int queue,
 	trans->ops->txq_disable(trans, queue, configure_scd);
 }
 
-static inline void
+static inline bool
 iwl_trans_txq_enable_cfg(struct iwl_trans *trans, int queue, u16 ssn,
 			 const struct iwl_trans_txq_scd_cfg *cfg,
 			 unsigned int queue_wdg_timeout)
@@ -1098,10 +963,11 @@ iwl_trans_txq_enable_cfg(struct iwl_trans *trans, int queue, u16 ssn,
 
 	if (WARN_ON_ONCE(trans->state != IWL_TRANS_FW_ALIVE)) {
 		IWL_ERR(trans, "%s bad state = %d\n", __func__, trans->state);
-		return;
+		return false;
 	}
 
-	trans->ops->txq_enable(trans, queue, ssn, cfg, queue_wdg_timeout);
+	return trans->ops->txq_enable(trans, queue, ssn,
+				      cfg, queue_wdg_timeout);
 }
 
 static inline void
@@ -1198,6 +1064,9 @@ static inline void iwl_trans_block_txq_ptrs(struct iwl_trans *trans,
 static inline int iwl_trans_wait_tx_queues_empty(struct iwl_trans *trans,
 						 u32 txqs)
 {
+	if (WARN_ON_ONCE(!trans->ops->wait_tx_queues_empty))
+		return -ENOTSUPP;
+
 	if (WARN_ON_ONCE(trans->state != IWL_TRANS_FW_ALIVE)) {
 		IWL_ERR(trans, "%s bad state = %d\n", __func__, trans->state);
 		return -EIO;
@@ -1206,6 +1075,19 @@ static inline int iwl_trans_wait_tx_queues_empty(struct iwl_trans *trans,
 	return trans->ops->wait_tx_queues_empty(trans, txqs);
 }
 
+static inline int iwl_trans_wait_txq_empty(struct iwl_trans *trans, int queue)
+{
+	if (WARN_ON_ONCE(!trans->ops->wait_txq_empty))
+		return -ENOTSUPP;
+
+	if (WARN_ON_ONCE(trans->state != IWL_TRANS_FW_ALIVE)) {
+		IWL_ERR(trans, "%s bad state = %d\n", __func__, trans->state);
+		return -EIO;
+	}
+
+	return trans->ops->wait_txq_empty(trans, queue);
+}
+
 static inline void iwl_trans_write8(struct iwl_trans *trans, u32 ofs, u8 val)
 {
 	trans->ops->write8(trans, ofs, val);
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/coex.c b/drivers/net/wireless/intel/iwlwifi/mvm/coex.c
index 49b4418e6c35..34dd5c40ce77 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/coex.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/coex.c
@@ -406,7 +406,7 @@ iwl_get_coex_type(struct iwl_mvm *mvm, const struct ieee80211_vif *vif)
 	return ret;
 }
 
-int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
+int iwl_mvm_send_bt_init_conf(struct iwl_mvm *mvm)
 {
 	struct iwl_bt_coex_cmd bt_cmd = {};
 	u32 mode;
@@ -914,7 +914,8 @@ void iwl_mvm_rx_ant_coupling_notif(struct iwl_mvm *mvm,
 				   struct iwl_rx_cmd_buffer *rxb)
 {
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	u32 ant_isolation = le32_to_cpup((void *)pkt->data);
+	struct iwl_mvm_antenna_coupling_notif *notif = (void *)pkt->data;
+	u32 ant_isolation = le32_to_cpu(notif->isolation);
 	struct iwl_bt_coex_corun_lut_update_cmd cmd = {};
 	u8 __maybe_unused lower_bound, upper_bound;
 	u8 lut;
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/constants.h b/drivers/net/wireless/intel/iwlwifi/mvm/constants.h
index 4eeb6b78d952..6fda8627b726 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/constants.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/constants.h
@@ -110,6 +110,7 @@
 #define IWL_MVM_TOF_IS_RESPONDER		0
 #define IWL_MVM_SW_TX_CSUM_OFFLOAD		0
 #define IWL_MVM_HW_CSUM_DISABLE			0
+#define IWL_MVM_PARSE_NVM			0
 #define IWL_MVM_COLLECT_FW_ERR_DUMP		1
 #define IWL_MVM_RS_NUM_TRY_BEFORE_ANT_TOGGLE    1
 #define IWL_MVM_RS_HT_VHT_RETRIES_PER_RATE      2
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/d3.c b/drivers/net/wireless/intel/iwlwifi/mvm/d3.c
index 119a3bd92c50..5de19ea10575 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/d3.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/d3.c
@@ -7,7 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
- * Copyright(c) 2016   Intel Deutschland GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -34,7 +34,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
- * Copyright(c) 2016   Intel Deutschland GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -83,7 +83,7 @@ void iwl_mvm_set_rekey_data(struct ieee80211_hw *hw,
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 
-	if (iwlwifi_mod_params.sw_crypto)
+	if (iwlwifi_mod_params.swcrypto)
 		return;
 
 	mutex_lock(&mvm->mutex);
@@ -1054,7 +1054,7 @@ iwl_mvm_wowlan_config(struct iwl_mvm *mvm,
 			return ret;
 	}
 
-	if (!iwlwifi_mod_params.sw_crypto) {
+	if (!iwlwifi_mod_params.swcrypto) {
 		/*
 		 * This needs to be unlocked due to lock ordering
 		 * constraints. Since we're in the suspend path
@@ -1280,8 +1280,8 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
 
 		if (!unified_image) {
 			iwl_mvm_ref(mvm, IWL_MVM_REF_UCODE_DOWN);
-			if (mvm->restart_fw > 0) {
-				mvm->restart_fw--;
+			if (mvm->fw_restart > 0) {
+				mvm->fw_restart--;
 				ieee80211_restart_hw(mvm->hw);
 			}
 		}
@@ -1431,7 +1431,7 @@ static void iwl_mvm_report_wakeup_reasons(struct iwl_mvm *mvm,
 			if (!pkt)
 				goto report;
 
-			memcpy(skb_put(pkt, hdrlen), pktdata, hdrlen);
+			skb_put_data(pkt, pktdata, hdrlen);
 			pktdata += hdrlen;
 			pktsize -= hdrlen;
 
@@ -1463,7 +1463,7 @@ static void iwl_mvm_report_wakeup_reasons(struct iwl_mvm *mvm,
 			pktsize -= ivlen + icvlen;
 			pktdata += ivlen;
 
-			memcpy(skb_put(pkt, pktsize), pktdata, pktsize);
+			skb_put_data(pkt, pktdata, pktsize);
 
 			if (ieee80211_data_to_8023(pkt, vif->addr, vif->type))
 				goto report;
@@ -1795,12 +1795,6 @@ iwl_mvm_get_wakeup_status(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 		return ERR_PTR(ret);
 	}
 
-	/* RF-kill already asserted again... */
-	if (!cmd.resp_pkt) {
-		fw_status = ERR_PTR(-ERFKILL);
-		goto out_free_resp;
-	}
-
 	status_size = sizeof(*fw_status);
 
 	len = iwl_rx_packet_payload_len(cmd.resp_pkt);
@@ -1925,12 +1919,6 @@ iwl_mvm_netdetect_query_results(struct iwl_mvm *mvm,
 		return ret;
 	}
 
-	/* RF-kill already asserted again... */
-	if (!cmd.resp_pkt) {
-		ret = -ERFKILL;
-		goto out_free_resp;
-	}
-
 	len = iwl_rx_packet_payload_len(cmd.resp_pkt);
 	if (len < sizeof(*query)) {
 		IWL_ERR(mvm, "Invalid scan offload profiles query response!\n");
@@ -2087,9 +2075,8 @@ static int __iwl_mvm_resume(struct iwl_mvm *mvm, bool test)
 	bool keep = false;
 	bool unified_image = fw_has_capa(&mvm->fw->ucode_capa,
 					 IWL_UCODE_TLV_CAPA_CNSLDTD_D3_D0_IMG);
-
-	u32 flags = CMD_ASYNC | CMD_HIGH_PRIO | CMD_SEND_IN_IDLE |
-				    CMD_WAKE_UP_TRANS;
+	bool d0i3_first = fw_has_capa(&mvm->fw->ucode_capa,
+				      IWL_UCODE_TLV_CAPA_D0I3_END_FIRST);
 
 	mutex_lock(&mvm->mutex);
 
@@ -2110,6 +2097,15 @@ static int __iwl_mvm_resume(struct iwl_mvm *mvm, bool test)
 	/* query SRAM first in case we want event logging */
 	iwl_mvm_read_d3_sram(mvm);
 
+	if (d0i3_first) {
+		ret = iwl_mvm_send_cmd_pdu(mvm, D0I3_END_CMD, 0, 0, NULL);
+		if (ret < 0) {
+			IWL_ERR(mvm, "Failed to send D0I3_END_CMD first (%d)\n",
+				ret);
+			goto err;
+		}
+	}
+
 	/*
 	 * Query the current location and source from the D3 firmware so we
 	 * can play it back when we re-intiailize the D0 firmware
@@ -2155,9 +2151,14 @@ out_iterate:
 			iwl_mvm_d3_disconnect_iter, keep ? vif : NULL);
 
 out:
+	/* no need to reset the device in unified images, if successful */
 	if (unified_image && !ret) {
-		ret = iwl_mvm_send_cmd_pdu(mvm, D0I3_END_CMD, flags, 0, NULL);
-		if (!ret) /* D3 ended successfully - no need to reset device */
+		/* nothing else to do if we already sent D0I3_END_CMD */
+		if (d0i3_first)
+			return 0;
+
+		ret = iwl_mvm_send_cmd_pdu(mvm, D0I3_END_CMD, 0, 0, NULL);
+		if (!ret)
 			return 0;
 	}
 
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/debugfs-vif.c b/drivers/net/wireless/intel/iwlwifi/mvm/debugfs-vif.c
index 5d475b4850ae..a7ac281e5cde 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/debugfs-vif.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/debugfs-vif.c
@@ -7,7 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
- * Copyright(c) 2016        Intel Deutschland GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -34,7 +34,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
- * Copyright(c) 2016        Intel Deutschland GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -1304,11 +1304,11 @@ static ssize_t iwl_dbgfs_low_latency_read(struct file *file,
 	char buf[30] = {};
 	int len;
 
-	len = snprintf(buf, sizeof(buf) - 1,
-		       "traffic=%d\ndbgfs=%d\nvcmd=%d\n",
-		       mvmvif->low_latency_traffic,
-		       mvmvif->low_latency_dbgfs,
-		       mvmvif->low_latency_vcmd);
+	len = scnprintf(buf, sizeof(buf) - 1,
+			"traffic=%d\ndbgfs=%d\nvcmd=%d\n",
+			mvmvif->low_latency_traffic,
+			mvmvif->low_latency_dbgfs,
+			mvmvif->low_latency_vcmd);
 	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
 }
 
@@ -1385,10 +1385,12 @@ static ssize_t iwl_dbgfs_rx_phyinfo_read(struct file *file,
 	struct ieee80211_vif *vif = file->private_data;
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	char buf[8];
+	int len;
 
-	snprintf(buf, sizeof(buf), "0x%04x\n", mvmvif->mvm->dbgfs_rx_phyinfo);
+	len = scnprintf(buf, sizeof(buf), "0x%04x\n",
+			mvmvif->mvm->dbgfs_rx_phyinfo);
 
-	return simple_read_from_buffer(user_buf, count, ppos, buf, sizeof(buf));
+	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
 }
 
 static void iwl_dbgfs_quota_check(void *data, u8 *mac,
@@ -1439,7 +1441,7 @@ static ssize_t iwl_dbgfs_quota_min_read(struct file *file,
 	char buf[10];
 	int len;
 
-	len = snprintf(buf, sizeof(buf), "%d\n", mvmvif->dbgfs_quota_min);
+	len = scnprintf(buf, sizeof(buf), "%d\n", mvmvif->dbgfs_quota_min);
 
 	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
 }
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/debugfs.c b/drivers/net/wireless/intel/iwlwifi/mvm/debugfs.c
index 402846650cbe..c1c9c489edc9 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/debugfs.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/debugfs.c
@@ -7,7 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
- * Copyright(c) 2016 Intel Deutschland GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -34,6 +34,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -72,7 +73,7 @@
 #include "sta.h"
 #include "iwl-io.h"
 #include "debugfs.h"
-#include "iwl-fw-error-dump.h"
+#include "fw/error-dump.h"
 
 static ssize_t iwl_dbgfs_ctdp_budget_read(struct file *file,
 					  char __user *user_buf,
@@ -82,7 +83,8 @@ static ssize_t iwl_dbgfs_ctdp_budget_read(struct file *file,
 	char buf[16];
 	int pos, budget;
 
-	if (!mvm->ucode_loaded || mvm->cur_ucode != IWL_UCODE_REGULAR)
+	if (!iwl_mvm_firmware_running(mvm) ||
+	    mvm->cur_ucode != IWL_UCODE_REGULAR)
 		return -EIO;
 
 	mutex_lock(&mvm->mutex);
@@ -102,7 +104,8 @@ static ssize_t iwl_dbgfs_stop_ctdp_write(struct iwl_mvm *mvm, char *buf,
 {
 	int ret;
 
-	if (!mvm->ucode_loaded || mvm->cur_ucode != IWL_UCODE_REGULAR)
+	if (!iwl_mvm_firmware_running(mvm) ||
+	    mvm->cur_ucode != IWL_UCODE_REGULAR)
 		return -EIO;
 
 	mutex_lock(&mvm->mutex);
@@ -116,18 +119,30 @@ static ssize_t iwl_dbgfs_tx_flush_write(struct iwl_mvm *mvm, char *buf,
 					size_t count, loff_t *ppos)
 {
 	int ret;
-	u32 scd_q_msk;
+	u32 flush_arg;
 
-	if (!mvm->ucode_loaded || mvm->cur_ucode != IWL_UCODE_REGULAR)
+	if (!iwl_mvm_firmware_running(mvm) ||
+	    mvm->cur_ucode != IWL_UCODE_REGULAR)
 		return -EIO;
 
-	if (sscanf(buf, "%x", &scd_q_msk) != 1)
+	if (kstrtou32(buf, 0, &flush_arg))
 		return -EINVAL;
 
-	IWL_ERR(mvm, "FLUSHING queues: scd_q_msk = 0x%x\n", scd_q_msk);
+	if (iwl_mvm_has_new_tx_api(mvm)) {
+		IWL_DEBUG_TX_QUEUES(mvm,
+				    "FLUSHING all tids queues on sta_id = %d\n",
+				    flush_arg);
+		mutex_lock(&mvm->mutex);
+		ret = iwl_mvm_flush_sta_tids(mvm, flush_arg, 0xFF, 0) ? : count;
+		mutex_unlock(&mvm->mutex);
+		return ret;
+	}
+
+	IWL_DEBUG_TX_QUEUES(mvm, "FLUSHING queues mask to flush = 0x%x\n",
+			    flush_arg);
 
 	mutex_lock(&mvm->mutex);
-	ret =  iwl_mvm_flush_tx_path(mvm, scd_q_msk, 0) ? : count;
+	ret =  iwl_mvm_flush_tx_path(mvm, flush_arg, 0) ? : count;
 	mutex_unlock(&mvm->mutex);
 
 	return ret;
@@ -139,7 +154,8 @@ static ssize_t iwl_dbgfs_sta_drain_write(struct iwl_mvm *mvm, char *buf,
 	struct iwl_mvm_sta *mvmsta;
 	int sta_id, drain, ret;
 
-	if (!mvm->ucode_loaded || mvm->cur_ucode != IWL_UCODE_REGULAR)
+	if (!iwl_mvm_firmware_running(mvm) ||
+	    mvm->cur_ucode != IWL_UCODE_REGULAR)
 		return -EIO;
 
 	if (sscanf(buf, "%d %d", &sta_id, &drain) != 2)
@@ -172,7 +188,7 @@ static ssize_t iwl_dbgfs_sram_read(struct file *file, char __user *user_buf,
 	size_t ret;
 	u8 *ptr;
 
-	if (!mvm->ucode_loaded)
+	if (!iwl_mvm_firmware_running(mvm))
 		return -EINVAL;
 
 	/* default is to dump the entire data segment */
@@ -205,7 +221,7 @@ static ssize_t iwl_dbgfs_sram_write(struct iwl_mvm *mvm, char *buf,
 	u32 offset, len;
 	u32 img_offset, img_len;
 
-	if (!mvm->ucode_loaded)
+	if (!iwl_mvm_firmware_running(mvm))
 		return -EINVAL;
 
 	img = &mvm->fw->img[mvm->cur_ucode];
@@ -258,7 +274,7 @@ static ssize_t iwl_dbgfs_set_nic_temperature_write(struct iwl_mvm *mvm,
 {
 	int temperature;
 
-	if (!mvm->ucode_loaded && !mvm->temperature_test)
+	if (!iwl_mvm_firmware_running(mvm) && !mvm->temperature_test)
 		return -EIO;
 
 	if (kstrtoint(buf, 10, &temperature))
@@ -305,7 +321,7 @@ static ssize_t iwl_dbgfs_nic_temp_read(struct file *file,
 	int pos, ret;
 	s32 temp;
 
-	if (!mvm->ucode_loaded)
+	if (!iwl_mvm_firmware_running(mvm))
 		return -EIO;
 
 	mutex_lock(&mvm->mutex);
@@ -320,6 +336,49 @@ static ssize_t iwl_dbgfs_nic_temp_read(struct file *file,
 	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 }
 
+#ifdef CONFIG_ACPI
+static ssize_t iwl_dbgfs_sar_geo_profile_read(struct file *file,
+					      char __user *user_buf,
+					      size_t count, loff_t *ppos)
+{
+	struct iwl_mvm *mvm = file->private_data;
+	char buf[256];
+	int pos = 0;
+	int bufsz = sizeof(buf);
+	int tbl_idx;
+	u8 *value;
+
+	if (!iwl_mvm_firmware_running(mvm))
+		return -EIO;
+
+	mutex_lock(&mvm->mutex);
+	tbl_idx = iwl_mvm_get_sar_geo_profile(mvm);
+	if (tbl_idx < 0) {
+		mutex_unlock(&mvm->mutex);
+		return tbl_idx;
+	}
+
+	if (!tbl_idx) {
+		pos = scnprintf(buf, bufsz,
+				"SAR geographic profile disabled\n");
+	} else {
+		value = &mvm->geo_profiles[tbl_idx - 1].values[0];
+
+		pos += scnprintf(buf + pos, bufsz - pos,
+				 "Use geographic profile %d\n", tbl_idx);
+		pos += scnprintf(buf + pos, bufsz - pos,
+				 "2.4GHz:\n\tChain A offset: %hhd dBm\n\tChain B offset: %hhd dBm\n\tmax tx power: %hhd dBm\n",
+				 value[1], value[2], value[0]);
+		pos += scnprintf(buf + pos, bufsz - pos,
+				 "5.2GHz:\n\tChain A offset: %hhd dBm\n\tChain B offset: %hhd dBm\n\tmax tx power: %hhd dBm\n",
+				 value[4], value[5], value[3]);
+	}
+	mutex_unlock(&mvm->mutex);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+#endif
+
 static ssize_t iwl_dbgfs_stations_read(struct file *file, char __user *user_buf,
 				       size_t count, loff_t *ppos)
 {
@@ -371,7 +430,7 @@ static ssize_t iwl_dbgfs_disable_power_off_write(struct iwl_mvm *mvm, char *buf,
 {
 	int ret, val;
 
-	if (!mvm->ucode_loaded)
+	if (!iwl_mvm_firmware_running(mvm))
 		return -EIO;
 
 	if (!strncmp("disable_power_off_d0=", buf, 21)) {
@@ -583,7 +642,11 @@ iwl_dbgfs_bt_force_ant_write(struct iwl_mvm *mvm, char *buf,
 	mvm->bt_force_ant_mode = bt_force_ant_mode;
 	IWL_DEBUG_COEX(mvm, "Force mode: %s\n",
 		       modes_str[mvm->bt_force_ant_mode]);
-	ret = iwl_send_bt_init_conf(mvm);
+
+	if (iwl_mvm_firmware_running(mvm))
+		ret = iwl_mvm_send_bt_init_conf(mvm);
+	else
+		ret = 0;
 
 out:
 	mutex_unlock(&mvm->mutex);
@@ -605,15 +668,15 @@ static ssize_t iwl_dbgfs_fw_rx_stats_read(struct file *file,
 	int pos = 0;
 	char *buf;
 	int ret;
-	/* 43 is the size of each data line, 33 is the size of each header */
-	size_t bufsz =
-		((sizeof(struct mvm_statistics_rx) / sizeof(__le32)) * 43) +
-		(4 * 33) + 1;
+	size_t bufsz;
 
-	struct mvm_statistics_rx_phy *ofdm;
-	struct mvm_statistics_rx_phy *cck;
-	struct mvm_statistics_rx_non_phy *general;
-	struct mvm_statistics_rx_ht_phy *ht;
+	if (iwl_mvm_has_new_rx_stats_api(mvm))
+		bufsz = ((sizeof(struct mvm_statistics_rx) /
+			  sizeof(__le32)) * 43) + (4 * 33) + 1;
+	else
+		/* 43 = size of each data line; 33 = size of each header */
+		bufsz = ((sizeof(struct mvm_statistics_rx_v3) /
+			  sizeof(__le32)) * 43) + (4 * 33) + 1;
 
 	buf = kzalloc(bufsz, GFP_KERNEL);
 	if (!buf)
@@ -621,96 +684,157 @@ static ssize_t iwl_dbgfs_fw_rx_stats_read(struct file *file,
 
 	mutex_lock(&mvm->mutex);
 
-	ofdm = &mvm->rx_stats.ofdm;
-	cck = &mvm->rx_stats.cck;
-	general = &mvm->rx_stats.general;
-	ht = &mvm->rx_stats.ofdm_ht;
-
 	pos += scnprintf(buf + pos, bufsz - pos, fmt_header,
 			 "Statistics_Rx - OFDM");
-	PRINT_STATS_LE32(ofdm, ina_cnt);
-	PRINT_STATS_LE32(ofdm, fina_cnt);
-	PRINT_STATS_LE32(ofdm, plcp_err);
-	PRINT_STATS_LE32(ofdm, crc32_err);
-	PRINT_STATS_LE32(ofdm, overrun_err);
-	PRINT_STATS_LE32(ofdm, early_overrun_err);
-	PRINT_STATS_LE32(ofdm, crc32_good);
-	PRINT_STATS_LE32(ofdm, false_alarm_cnt);
-	PRINT_STATS_LE32(ofdm, fina_sync_err_cnt);
-	PRINT_STATS_LE32(ofdm, sfd_timeout);
-	PRINT_STATS_LE32(ofdm, fina_timeout);
-	PRINT_STATS_LE32(ofdm, unresponded_rts);
-	PRINT_STATS_LE32(ofdm, rxe_frame_lmt_overrun);
-	PRINT_STATS_LE32(ofdm, sent_ack_cnt);
-	PRINT_STATS_LE32(ofdm, sent_cts_cnt);
-	PRINT_STATS_LE32(ofdm, sent_ba_rsp_cnt);
-	PRINT_STATS_LE32(ofdm, dsp_self_kill);
-	PRINT_STATS_LE32(ofdm, mh_format_err);
-	PRINT_STATS_LE32(ofdm, re_acq_main_rssi_sum);
-	PRINT_STATS_LE32(ofdm, reserved);
+	if (!iwl_mvm_has_new_rx_stats_api(mvm)) {
+		struct mvm_statistics_rx_phy_v2 *ofdm = &mvm->rx_stats_v3.ofdm;
+
+		PRINT_STATS_LE32(ofdm, ina_cnt);
+		PRINT_STATS_LE32(ofdm, fina_cnt);
+		PRINT_STATS_LE32(ofdm, plcp_err);
+		PRINT_STATS_LE32(ofdm, crc32_err);
+		PRINT_STATS_LE32(ofdm, overrun_err);
+		PRINT_STATS_LE32(ofdm, early_overrun_err);
+		PRINT_STATS_LE32(ofdm, crc32_good);
+		PRINT_STATS_LE32(ofdm, false_alarm_cnt);
+		PRINT_STATS_LE32(ofdm, fina_sync_err_cnt);
+		PRINT_STATS_LE32(ofdm, sfd_timeout);
+		PRINT_STATS_LE32(ofdm, fina_timeout);
+		PRINT_STATS_LE32(ofdm, unresponded_rts);
+		PRINT_STATS_LE32(ofdm, rxe_frame_lmt_overrun);
+		PRINT_STATS_LE32(ofdm, sent_ack_cnt);
+		PRINT_STATS_LE32(ofdm, sent_cts_cnt);
+		PRINT_STATS_LE32(ofdm, sent_ba_rsp_cnt);
+		PRINT_STATS_LE32(ofdm, dsp_self_kill);
+		PRINT_STATS_LE32(ofdm, mh_format_err);
+		PRINT_STATS_LE32(ofdm, re_acq_main_rssi_sum);
+		PRINT_STATS_LE32(ofdm, reserved);
+	} else {
+		struct mvm_statistics_rx_phy *ofdm = &mvm->rx_stats.ofdm;
+
+		PRINT_STATS_LE32(ofdm, unresponded_rts);
+		PRINT_STATS_LE32(ofdm, rxe_frame_lmt_overrun);
+		PRINT_STATS_LE32(ofdm, sent_ba_rsp_cnt);
+		PRINT_STATS_LE32(ofdm, dsp_self_kill);
+		PRINT_STATS_LE32(ofdm, reserved);
+	}
 
 	pos += scnprintf(buf + pos, bufsz - pos, fmt_header,
 			 "Statistics_Rx - CCK");
-	PRINT_STATS_LE32(cck, ina_cnt);
-	PRINT_STATS_LE32(cck, fina_cnt);
-	PRINT_STATS_LE32(cck, plcp_err);
-	PRINT_STATS_LE32(cck, crc32_err);
-	PRINT_STATS_LE32(cck, overrun_err);
-	PRINT_STATS_LE32(cck, early_overrun_err);
-	PRINT_STATS_LE32(cck, crc32_good);
-	PRINT_STATS_LE32(cck, false_alarm_cnt);
-	PRINT_STATS_LE32(cck, fina_sync_err_cnt);
-	PRINT_STATS_LE32(cck, sfd_timeout);
-	PRINT_STATS_LE32(cck, fina_timeout);
-	PRINT_STATS_LE32(cck, unresponded_rts);
-	PRINT_STATS_LE32(cck, rxe_frame_lmt_overrun);
-	PRINT_STATS_LE32(cck, sent_ack_cnt);
-	PRINT_STATS_LE32(cck, sent_cts_cnt);
-	PRINT_STATS_LE32(cck, sent_ba_rsp_cnt);
-	PRINT_STATS_LE32(cck, dsp_self_kill);
-	PRINT_STATS_LE32(cck, mh_format_err);
-	PRINT_STATS_LE32(cck, re_acq_main_rssi_sum);
-	PRINT_STATS_LE32(cck, reserved);
+	if (!iwl_mvm_has_new_rx_stats_api(mvm)) {
+		struct mvm_statistics_rx_phy_v2 *cck = &mvm->rx_stats_v3.cck;
+
+		PRINT_STATS_LE32(cck, ina_cnt);
+		PRINT_STATS_LE32(cck, fina_cnt);
+		PRINT_STATS_LE32(cck, plcp_err);
+		PRINT_STATS_LE32(cck, crc32_err);
+		PRINT_STATS_LE32(cck, overrun_err);
+		PRINT_STATS_LE32(cck, early_overrun_err);
+		PRINT_STATS_LE32(cck, crc32_good);
+		PRINT_STATS_LE32(cck, false_alarm_cnt);
+		PRINT_STATS_LE32(cck, fina_sync_err_cnt);
+		PRINT_STATS_LE32(cck, sfd_timeout);
+		PRINT_STATS_LE32(cck, fina_timeout);
+		PRINT_STATS_LE32(cck, unresponded_rts);
+		PRINT_STATS_LE32(cck, rxe_frame_lmt_overrun);
+		PRINT_STATS_LE32(cck, sent_ack_cnt);
+		PRINT_STATS_LE32(cck, sent_cts_cnt);
+		PRINT_STATS_LE32(cck, sent_ba_rsp_cnt);
+		PRINT_STATS_LE32(cck, dsp_self_kill);
+		PRINT_STATS_LE32(cck, mh_format_err);
+		PRINT_STATS_LE32(cck, re_acq_main_rssi_sum);
+		PRINT_STATS_LE32(cck, reserved);
+	} else {
+		struct mvm_statistics_rx_phy *cck = &mvm->rx_stats.cck;
+
+		PRINT_STATS_LE32(cck, unresponded_rts);
+		PRINT_STATS_LE32(cck, rxe_frame_lmt_overrun);
+		PRINT_STATS_LE32(cck, sent_ba_rsp_cnt);
+		PRINT_STATS_LE32(cck, dsp_self_kill);
+		PRINT_STATS_LE32(cck, reserved);
+	}
 
 	pos += scnprintf(buf + pos, bufsz - pos, fmt_header,
 			 "Statistics_Rx - GENERAL");
-	PRINT_STATS_LE32(general, bogus_cts);
-	PRINT_STATS_LE32(general, bogus_ack);
-	PRINT_STATS_LE32(general, non_bssid_frames);
-	PRINT_STATS_LE32(general, filtered_frames);
-	PRINT_STATS_LE32(general, non_channel_beacons);
-	PRINT_STATS_LE32(general, channel_beacons);
-	PRINT_STATS_LE32(general, num_missed_bcon);
-	PRINT_STATS_LE32(general, adc_rx_saturation_time);
-	PRINT_STATS_LE32(general, ina_detection_search_time);
-	PRINT_STATS_LE32(general, beacon_silence_rssi_a);
-	PRINT_STATS_LE32(general, beacon_silence_rssi_b);
-	PRINT_STATS_LE32(general, beacon_silence_rssi_c);
-	PRINT_STATS_LE32(general, interference_data_flag);
-	PRINT_STATS_LE32(general, channel_load);
-	PRINT_STATS_LE32(general, dsp_false_alarms);
-	PRINT_STATS_LE32(general, beacon_rssi_a);
-	PRINT_STATS_LE32(general, beacon_rssi_b);
-	PRINT_STATS_LE32(general, beacon_rssi_c);
-	PRINT_STATS_LE32(general, beacon_energy_a);
-	PRINT_STATS_LE32(general, beacon_energy_b);
-	PRINT_STATS_LE32(general, beacon_energy_c);
-	PRINT_STATS_LE32(general, num_bt_kills);
-	PRINT_STATS_LE32(general, mac_id);
-	PRINT_STATS_LE32(general, directed_data_mpdu);
+	if (!iwl_mvm_has_new_rx_stats_api(mvm)) {
+		struct mvm_statistics_rx_non_phy_v3 *general =
+			&mvm->rx_stats_v3.general;
+
+		PRINT_STATS_LE32(general, bogus_cts);
+		PRINT_STATS_LE32(general, bogus_ack);
+		PRINT_STATS_LE32(general, non_bssid_frames);
+		PRINT_STATS_LE32(general, filtered_frames);
+		PRINT_STATS_LE32(general, non_channel_beacons);
+		PRINT_STATS_LE32(general, channel_beacons);
+		PRINT_STATS_LE32(general, num_missed_bcon);
+		PRINT_STATS_LE32(general, adc_rx_saturation_time);
+		PRINT_STATS_LE32(general, ina_detection_search_time);
+		PRINT_STATS_LE32(general, beacon_silence_rssi_a);
+		PRINT_STATS_LE32(general, beacon_silence_rssi_b);
+		PRINT_STATS_LE32(general, beacon_silence_rssi_c);
+		PRINT_STATS_LE32(general, interference_data_flag);
+		PRINT_STATS_LE32(general, channel_load);
+		PRINT_STATS_LE32(general, dsp_false_alarms);
+		PRINT_STATS_LE32(general, beacon_rssi_a);
+		PRINT_STATS_LE32(general, beacon_rssi_b);
+		PRINT_STATS_LE32(general, beacon_rssi_c);
+		PRINT_STATS_LE32(general, beacon_energy_a);
+		PRINT_STATS_LE32(general, beacon_energy_b);
+		PRINT_STATS_LE32(general, beacon_energy_c);
+		PRINT_STATS_LE32(general, num_bt_kills);
+		PRINT_STATS_LE32(general, mac_id);
+		PRINT_STATS_LE32(general, directed_data_mpdu);
+	} else {
+		struct mvm_statistics_rx_non_phy *general =
+			&mvm->rx_stats.general;
+
+		PRINT_STATS_LE32(general, bogus_cts);
+		PRINT_STATS_LE32(general, bogus_ack);
+		PRINT_STATS_LE32(general, non_channel_beacons);
+		PRINT_STATS_LE32(general, channel_beacons);
+		PRINT_STATS_LE32(general, num_missed_bcon);
+		PRINT_STATS_LE32(general, adc_rx_saturation_time);
+		PRINT_STATS_LE32(general, ina_detection_search_time);
+		PRINT_STATS_LE32(general, beacon_silence_rssi_a);
+		PRINT_STATS_LE32(general, beacon_silence_rssi_b);
+		PRINT_STATS_LE32(general, beacon_silence_rssi_c);
+		PRINT_STATS_LE32(general, interference_data_flag);
+		PRINT_STATS_LE32(general, channel_load);
+		PRINT_STATS_LE32(general, beacon_rssi_a);
+		PRINT_STATS_LE32(general, beacon_rssi_b);
+		PRINT_STATS_LE32(general, beacon_rssi_c);
+		PRINT_STATS_LE32(general, beacon_energy_a);
+		PRINT_STATS_LE32(general, beacon_energy_b);
+		PRINT_STATS_LE32(general, beacon_energy_c);
+		PRINT_STATS_LE32(general, num_bt_kills);
+		PRINT_STATS_LE32(general, mac_id);
+	}
 
 	pos += scnprintf(buf + pos, bufsz - pos, fmt_header,
 			 "Statistics_Rx - HT");
-	PRINT_STATS_LE32(ht, plcp_err);
-	PRINT_STATS_LE32(ht, overrun_err);
-	PRINT_STATS_LE32(ht, early_overrun_err);
-	PRINT_STATS_LE32(ht, crc32_good);
-	PRINT_STATS_LE32(ht, crc32_err);
-	PRINT_STATS_LE32(ht, mh_format_err);
-	PRINT_STATS_LE32(ht, agg_crc32_good);
-	PRINT_STATS_LE32(ht, agg_mpdu_cnt);
-	PRINT_STATS_LE32(ht, agg_cnt);
-	PRINT_STATS_LE32(ht, unsupport_mcs);
+	if (!iwl_mvm_has_new_rx_stats_api(mvm)) {
+		struct mvm_statistics_rx_ht_phy_v1 *ht =
+			&mvm->rx_stats_v3.ofdm_ht;
+
+		PRINT_STATS_LE32(ht, plcp_err);
+		PRINT_STATS_LE32(ht, overrun_err);
+		PRINT_STATS_LE32(ht, early_overrun_err);
+		PRINT_STATS_LE32(ht, crc32_good);
+		PRINT_STATS_LE32(ht, crc32_err);
+		PRINT_STATS_LE32(ht, mh_format_err);
+		PRINT_STATS_LE32(ht, agg_crc32_good);
+		PRINT_STATS_LE32(ht, agg_mpdu_cnt);
+		PRINT_STATS_LE32(ht, agg_cnt);
+		PRINT_STATS_LE32(ht, unsupport_mcs);
+	} else {
+		struct mvm_statistics_rx_ht_phy *ht =
+			&mvm->rx_stats.ofdm_ht;
+
+		PRINT_STATS_LE32(ht, mh_format_err);
+		PRINT_STATS_LE32(ht, agg_mpdu_cnt);
+		PRINT_STATS_LE32(ht, agg_cnt);
+		PRINT_STATS_LE32(ht, unsupport_mcs);
+	}
 
 	mutex_unlock(&mvm->mutex);
 
@@ -800,11 +924,14 @@ static ssize_t iwl_dbgfs_fw_restart_write(struct iwl_mvm *mvm, char *buf,
 {
 	int __maybe_unused ret;
 
+	if (!iwl_mvm_firmware_running(mvm))
+		return -EIO;
+
 	mutex_lock(&mvm->mutex);
 
 	/* allow one more restart that we're provoking here */
-	if (mvm->restart_fw >= 0)
-		mvm->restart_fw++;
+	if (mvm->fw_restart >= 0)
+		mvm->fw_restart++;
 
 	/* take the return value to make compiler happy - it will fail anyway */
 	ret = iwl_mvm_send_cmd_pdu(mvm, REPLY_ERROR, 0, 0, NULL);
@@ -817,7 +944,12 @@ static ssize_t iwl_dbgfs_fw_restart_write(struct iwl_mvm *mvm, char *buf,
 static ssize_t iwl_dbgfs_fw_nmi_write(struct iwl_mvm *mvm, char *buf,
 				      size_t count, loff_t *ppos)
 {
-	int ret = iwl_mvm_ref_sync(mvm, IWL_MVM_REF_NMI);
+	int ret;
+
+	if (!iwl_mvm_firmware_running(mvm))
+		return -EIO;
+
+	ret = iwl_mvm_ref_sync(mvm, IWL_MVM_REF_NMI);
 	if (ret)
 		return ret;
 
@@ -857,6 +989,9 @@ iwl_dbgfs_scan_ant_rxchain_write(struct iwl_mvm *mvm, char *buf,
 {
 	u8 scan_rx_ant;
 
+	if (!iwl_mvm_firmware_running(mvm))
+		return -EIO;
+
 	if (sscanf(buf, "%hhx", &scan_rx_ant) != 1)
 		return -EINVAL;
 	if (scan_rx_ant > ANT_ABC)
@@ -911,7 +1046,11 @@ static ssize_t iwl_dbgfs_indirection_tbl_write(struct iwl_mvm *mvm,
 	netdev_rss_key_fill(cmd.secret_key, sizeof(cmd.secret_key));
 
 	mutex_lock(&mvm->mutex);
-	ret = iwl_mvm_send_cmd_pdu(mvm, RSS_CONFIG_CMD, 0, sizeof(cmd), &cmd);
+	if (iwl_mvm_firmware_running(mvm))
+		ret = iwl_mvm_send_cmd_pdu(mvm, RSS_CONFIG_CMD, 0,
+					   sizeof(cmd), &cmd);
+	else
+		ret = 0;
 	mutex_unlock(&mvm->mutex);
 
 	return ret ?: count;
@@ -931,6 +1070,9 @@ static ssize_t iwl_dbgfs_inject_packet_write(struct iwl_mvm *mvm,
 	int bin_len = count / 2;
 	int ret = -EINVAL;
 
+	if (!iwl_mvm_firmware_running(mvm))
+		return -EIO;
+
 	/* supporting only 9000 descriptor */
 	if (!mvm->trans->cfg->mq_rx_supported)
 		return -ENOTSUPP;
@@ -1004,11 +1146,14 @@ static ssize_t iwl_dbgfs_cont_recording_write(struct iwl_mvm *mvm,
 	struct iwl_continuous_record_cmd cont_rec = {};
 	int ret, rec_mode;
 
+	if (!iwl_mvm_firmware_running(mvm))
+		return -EIO;
+
 	if (!dest)
 		return -EOPNOTSUPP;
 
 	if (dest->monitor_mode != SMEM_MODE ||
-	    trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
+	    trans->cfg->device_family < IWL_DEVICE_FAMILY_8000)
 		return -EOPNOTSUPP;
 
 	ret = kstrtoint(buf, 0, &rec_mode);
@@ -1034,6 +1179,9 @@ static ssize_t iwl_dbgfs_fw_dbg_conf_write(struct iwl_mvm *mvm,
 	unsigned int conf_id;
 	int ret;
 
+	if (!iwl_mvm_firmware_running(mvm))
+		return -EIO;
+
 	ret = kstrtouint(buf, 0, &conf_id);
 	if (ret)
 		return ret;
@@ -1052,8 +1200,12 @@ static ssize_t iwl_dbgfs_fw_dbg_collect_write(struct iwl_mvm *mvm,
 					      char *buf, size_t count,
 					      loff_t *ppos)
 {
-	int ret = iwl_mvm_ref_sync(mvm, IWL_MVM_REF_PRPH_WRITE);
+	int ret;
+
+	if (!iwl_mvm_firmware_running(mvm))
+		return -EIO;
 
+	ret = iwl_mvm_ref_sync(mvm, IWL_MVM_REF_PRPH_WRITE);
 	if (ret)
 		return ret;
 	if (count == 0)
@@ -1184,7 +1336,8 @@ static ssize_t iwl_dbgfs_bcast_filters_write(struct iwl_mvm *mvm, char *buf,
 	       &filter, sizeof(filter));
 
 	/* send updated bcast filtering configuration */
-	if (mvm->dbgfs_bcast_filtering.override &&
+	if (iwl_mvm_firmware_running(mvm) &&
+	    mvm->dbgfs_bcast_filtering.override &&
 	    iwl_mvm_bcast_filter_build_cmd(mvm, &cmd))
 		err = iwl_mvm_send_cmd_pdu(mvm, BCAST_FILTER_CMD, 0,
 					   sizeof(cmd), &cmd);
@@ -1256,7 +1409,8 @@ static ssize_t iwl_dbgfs_bcast_filters_macs_write(struct iwl_mvm *mvm,
 	       &mac, sizeof(mac));
 
 	/* send updated bcast filtering configuration */
-	if (mvm->dbgfs_bcast_filtering.override &&
+	if (iwl_mvm_firmware_running(mvm) &&
+	    mvm->dbgfs_bcast_filtering.override &&
 	    iwl_mvm_bcast_filter_build_cmd(mvm, &cmd))
 		err = iwl_mvm_send_cmd_pdu(mvm, BCAST_FILTER_CMD, 0,
 					   sizeof(cmd), &cmd);
@@ -1473,6 +1627,9 @@ iwl_dbgfs_send_echo_cmd_write(struct iwl_mvm *mvm, char *buf,
 {
 	int ret;
 
+	if (!iwl_mvm_firmware_running(mvm))
+		return -EIO;
+
 	mutex_lock(&mvm->mutex);
 	ret = iwl_mvm_send_cmd_pdu(mvm, ECHO_CMD, 0, 0, NULL);
 	mutex_unlock(&mvm->mutex);
@@ -1519,6 +1676,9 @@ MVM_DEBUGFS_READ_WRITE_FILE_OPS(bcast_filters_macs, 256);
 #ifdef CONFIG_PM_SLEEP
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(d3_sram, 8);
 #endif
+#ifdef CONFIG_ACPI
+MVM_DEBUGFS_READ_FILE_OPS(sar_geo_profile);
+#endif
 
 static ssize_t iwl_dbgfs_mem_read(struct file *file, char __user *user_buf,
 				  size_t count, loff_t *ppos)
@@ -1534,6 +1694,9 @@ static ssize_t iwl_dbgfs_mem_read(struct file *file, char __user *user_buf,
 	size_t delta;
 	ssize_t ret, len;
 
+	if (!iwl_mvm_firmware_running(mvm))
+		return -EIO;
+
 	hcmd.id = iwl_cmd_id(*ppos >> 24 ? UMAC_RD_WR : LMAC_RD_WR,
 			     DEBUG_GROUP, 0);
 	cmd.op = cpu_to_le32(DEBUG_MEM_OP_READ);
@@ -1586,6 +1749,9 @@ static ssize_t iwl_dbgfs_mem_write(struct file *file,
 	u32 op, len;
 	ssize_t ret;
 
+	if (!iwl_mvm_firmware_running(mvm))
+		return -EIO;
+
 	hcmd.id = iwl_cmd_id(*ppos >> 24 ? UMAC_RD_WR : LMAC_RD_WR,
 			     DEBUG_GROUP, 0);
 
@@ -1685,6 +1851,10 @@ int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir)
 	MVM_DEBUGFS_ADD_FILE(cont_recording, mvm->debugfs_dir, S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(indirection_tbl, mvm->debugfs_dir, S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(inject_packet, mvm->debugfs_dir, S_IWUSR);
+#ifdef CONFIG_ACPI
+	MVM_DEBUGFS_ADD_FILE(sar_geo_profile, dbgfs_dir, S_IRUSR);
+#endif
+
 	if (!debugfs_create_bool("enable_scan_iteration_notif",
 				 S_IRUSR | S_IWUSR,
 				 mvm->debugfs_dir,
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-coex.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-coex.h
index 204c1b13988b..8cd06aaa1f54 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-coex.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-coex.h
@@ -7,6 +7,7 @@
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2017        Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -33,6 +34,7 @@
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2017        Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -99,8 +101,8 @@ enum iwl_bt_coex_enabled_modules {
 
 /**
  * struct iwl_bt_coex_cmd - bt coex configuration command
- * @mode: enum %iwl_bt_coex_mode
- * @enabled_modules: enum %iwl_bt_coex_enabled_modules
+ * @mode: &enum iwl_bt_coex_mode
+ * @enabled_modules: &enum iwl_bt_coex_enabled_modules
  *
  * The structure is used for the BT_COEX command.
  */
@@ -132,10 +134,10 @@ struct iwl_bt_coex_reduced_txp_update_cmd {
 
 /**
  * struct iwl_bt_coex_ci_cmd - bt coex channel inhibition command
- * @bt_primary_ci:
- * @primary_ch_phy_id:
- * @bt_secondary_ci:
- * @secondary_ch_phy_id:
+ * @bt_primary_ci: primary channel inhibition bitmap
+ * @primary_ch_phy_id: primary channel PHY ID
+ * @bt_secondary_ci: secondary channel inhibition bitmap
+ * @secondary_ch_phy_id: secondary channel PHY ID
  *
  * Used for BT_COEX_CI command
  */
@@ -234,10 +236,11 @@ enum iwl_bt_ci_compliance {
  * @mbox_msg: message from BT to WiFi
  * @msg_idx: the index of the message
  * @bt_ci_compliance: enum %iwl_bt_ci_compliance
- * @primary_ch_lut: LUT used for primary channel enum %iwl_bt_coex_lut_type
- * @secondary_ch_lut: LUT used for secondary channel enume %iwl_bt_coex_lut_type
- * @bt_activity_grading: the activity of BT enum %iwl_bt_activity_grading
+ * @primary_ch_lut: LUT used for primary channel &enum iwl_bt_coex_lut_type
+ * @secondary_ch_lut: LUT used for secondary channel &enum iwl_bt_coex_lut_type
+ * @bt_activity_grading: the activity of BT &enum iwl_bt_activity_grading
  * @ttc_rrc_status: is TTC or RRC enabled - one bit per PHY
+ * @reserved: reserved
  */
 struct iwl_bt_coex_profile_notif {
 	__le32 mbox_msg[4];
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-d3.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-d3.h
index 5f22cc7ac26a..d4a4c28b7192 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-d3.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-d3.h
@@ -7,7 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
- * Copyright(c) 2015 Intel Deutschland GmbH
+ * Copyright(c) 2015 - 2017 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -34,6 +34,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2015 - 2017 Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -138,6 +139,7 @@ struct iwl_proto_offload_cmd_common {
  *	for each target address
  * @target_ipv6_addr: our target addresses
  * @ndp_mac_addr: neighbor solicitation response MAC address
+ * @reserved2: reserved
  */
 struct iwl_proto_offload_cmd_v1 {
 	struct iwl_proto_offload_cmd_common common;
@@ -156,6 +158,8 @@ struct iwl_proto_offload_cmd_v1 {
  *	for each target address
  * @target_ipv6_addr: our target addresses
  * @ndp_mac_addr: neighbor solicitation response MAC address
+ * @num_valid_ipv6_addrs: number of valid IPv6 addresses
+ * @reserved2: reserved
  */
 struct iwl_proto_offload_cmd_v2 {
 	struct iwl_proto_offload_cmd_common common;
@@ -163,7 +167,7 @@ struct iwl_proto_offload_cmd_v2 {
 	u8 solicited_node_ipv6_addr[16];
 	u8 target_ipv6_addr[IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V2][16];
 	u8 ndp_mac_addr[ETH_ALEN];
-	u8 numValidIPv6Addresses;
+	u8 num_valid_ipv6_addrs;
 	u8 reserved2[3];
 } __packed; /* PROT_OFFLOAD_CONFIG_CMD_DB_S_VER_2 */
 
@@ -182,7 +186,8 @@ struct iwl_targ_addr {
 /**
  * struct iwl_proto_offload_cmd_v3_small - ARP/NS offload configuration
  * @common: common/IPv4 configuration
- * @target_ipv6_addr: target IPv6 addresses
+ * @num_valid_ipv6_addrs: number of valid IPv6 addresses
+ * @targ_addrs: target IPv6 addresses
  * @ns_config: NS offload configurations
  */
 struct iwl_proto_offload_cmd_v3_small {
@@ -195,7 +200,8 @@ struct iwl_proto_offload_cmd_v3_small {
 /**
  * struct iwl_proto_offload_cmd_v3_large - ARP/NS offload configuration
  * @common: common/IPv4 configuration
- * @target_ipv6_addr: target IPv6 addresses
+ * @num_valid_ipv6_addrs: number of valid IPv6 addresses
+ * @targ_addrs: target IPv6 addresses
  * @ns_config: NS offload configurations
  */
 struct iwl_proto_offload_cmd_v3_large {
@@ -254,6 +260,17 @@ enum iwl_wowlan_flags {
 	ENABLE_STORE_BEACON	= BIT(4),
 };
 
+/**
+ * struct iwl_wowlan_config_cmd - WoWLAN configuration
+ * @wakeup_filter: filter from &enum iwl_wowlan_wakeup_filters
+ * @non_qos_seq: non-QoS sequence counter to use next
+ * @qos_seq: QoS sequence counters to use next
+ * @wowlan_ba_teardown_tids: bitmap of BA sessions to tear down
+ * @is_11n_connection: indicates HT connection
+ * @offloading_tid: TID reserved for firmware use
+ * @flags: extra flags, see &enum iwl_wowlan_flags
+ * @reserved: reserved
+ */
 struct iwl_wowlan_config_cmd {
 	__le32 wakeup_filter;
 	__le16 non_qos_seq;
@@ -370,6 +387,21 @@ struct iwl_wowlan_gtk_status {
 	struct iwl_wowlan_rsc_tsc_params_cmd rsc;
 } __packed; /* WOWLAN_GTK_MATERIAL_VER_1 */
 
+/**
+ * struct iwl_wowlan_status - WoWLAN status
+ * @gtk: GTK data
+ * @replay_ctr: GTK rekey replay counter
+ * @pattern_number: number of the matched pattern
+ * @non_qos_seq_ctr: non-QoS sequence counter to use next
+ * @qos_seq_ctr: QoS sequence counters to use next
+ * @wakeup_reasons: wakeup reasons, see &enum iwl_wowlan_wakeup_reason
+ * @num_of_gtk_rekeys: number of GTK rekeys
+ * @transmitted_ndps: number of transmitted neighbor discovery packets
+ * @received_beacons: number of received beacons
+ * @wake_packet_length: wakeup packet length
+ * @wake_packet_bufsize: wakeup packet buffer size
+ * @wake_packet: wakeup packet
+ */
 struct iwl_wowlan_status {
 	struct iwl_wowlan_gtk_status gtk;
 	__le64 replay_ctr;
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-mac.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-mac.h
index 970b030ed28d..0c3350ad2f2f 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-mac.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-mac.h
@@ -199,6 +199,7 @@ struct iwl_mac_data_ibss {
  * @dtim_reciprocal: 2^32 / dtim_interval , applicable only when associated
  * @listen_interval: in beacon intervals, applicable only when associated
  * @assoc_id: unique ID assigned by the AP during association
+ * @assoc_beacon_arrive_time: TSF of first beacon after association
  */
 struct iwl_mac_data_sta {
 	__le32 is_assoc;
@@ -329,19 +330,20 @@ struct iwl_ac_qos {
  * ( MAC_CONTEXT_CMD = 0x28 )
  * @id_and_color: ID and color of the MAC
  * @action: action to perform, one of FW_CTXT_ACTION_*
- * @mac_type: one of FW_MAC_TYPE_*
- * @tsd_id: TSF HW timer, one of TSF_ID_*
+ * @mac_type: one of &enum iwl_mac_types
+ * @tsf_id: TSF HW timer, one of &enum iwl_tsf_id
  * @node_addr: MAC address
+ * @reserved_for_node_addr: reserved
  * @bssid_addr: BSSID
+ * @reserved_for_bssid_addr: reserved
  * @cck_rates: basic rates available for CCK
  * @ofdm_rates: basic rates available for OFDM
- * @protection_flags: combination of MAC_PROT_FLG_FLAG_*
+ * @protection_flags: combination of &enum iwl_mac_protection_flags
  * @cck_short_preamble: 0x20 for enabling short preamble, 0 otherwise
  * @short_slot: 0x10 for enabling short slots, 0 otherwise
- * @filter_flags: combination of MAC_FILTER_*
- * @qos_flags: from MAC_QOS_FLG_*
+ * @filter_flags: combination of &enum iwl_mac_filter_flags
+ * @qos_flags: from &enum iwl_mac_qos_flags
  * @ac: one iwl_mac_qos configuration for each AC
- * @mac_specific: one of struct iwl_mac_data_*, according to mac_type
  */
 struct iwl_mac_ctx_cmd {
 	/* COMMON_INDEX_HDR_API_S_VER_1 */
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-power.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-power.h
index 750510aff70b..7da57ef2454e 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-power.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-power.h
@@ -82,6 +82,8 @@
  * @LTR_CFG_FLAG_SW_SET_SHORT: fixed static short LTR register
  * @LTR_CFG_FLAG_SW_SET_LONG: fixed static short LONG register
  * @LTR_CFG_FLAG_DENIE_C10_ON_PD: allow going into C10 on PD
+ * @LTR_CFG_FLAG_UPDATE_VALUES: update config values and short
+ *	idle timeout
  */
 enum iwl_ltr_config_flags {
 	LTR_CFG_FLAG_FEATURE_ENABLE = BIT(0),
@@ -91,11 +93,14 @@ enum iwl_ltr_config_flags {
 	LTR_CFG_FLAG_SW_SET_SHORT = BIT(4),
 	LTR_CFG_FLAG_SW_SET_LONG = BIT(5),
 	LTR_CFG_FLAG_DENIE_C10_ON_PD = BIT(6),
+	LTR_CFG_FLAG_UPDATE_VALUES = BIT(7),
 };
 
 /**
  * struct iwl_ltr_config_cmd_v1 - configures the LTR
- * @flags: See %enum iwl_ltr_config_flags
+ * @flags: See &enum iwl_ltr_config_flags
+ * @static_long: static LTR Long register value.
+ * @static_short: static LTR Short register value.
  */
 struct iwl_ltr_config_cmd_v1 {
 	__le32 flags;
@@ -107,11 +112,14 @@ struct iwl_ltr_config_cmd_v1 {
 
 /**
  * struct iwl_ltr_config_cmd - configures the LTR
- * @flags: See %enum iwl_ltr_config_flags
- * @static_long:
- * @static_short:
- * @ltr_cfg_values:
- * @ltr_short_idle_timeout:
+ * @flags: See &enum iwl_ltr_config_flags
+ * @static_long: static LTR Long register value.
+ * @static_short: static LTR Short register value.
+ * @ltr_cfg_values: LTR parameters table values (in usec) in folowing order:
+ *	TX, RX, Short Idle, Long Idle. Used only if %LTR_CFG_FLAG_UPDATE_VALUES
+ *	is set.
+ * @ltr_short_idle_timeout: LTR Short Idle timeout (in usec). Used only if
+ *	%LTR_CFG_FLAG_UPDATE_VALUES is set.
  */
 struct iwl_ltr_config_cmd {
 	__le32 flags;
@@ -140,7 +148,7 @@ struct iwl_ltr_config_cmd {
  *		PBW Snoozing enabled
  * @POWER_FLAGS_ADVANCE_PM_ENA_MSK: Advanced PM (uAPSD) enable mask
  * @POWER_FLAGS_LPRX_ENA_MSK: Low Power RX enable.
- * @POWER_FLAGS_AP_UAPSD_MISBEHAVING_ENA_MSK: AP/GO's uAPSD misbehaving
+ * @POWER_FLAGS_UAPSD_MISBEHAVING_ENA_MSK: AP/GO's uAPSD misbehaving
  *		detection enablement
 */
 enum iwl_power_flags {
@@ -166,6 +174,7 @@ enum iwl_power_flags {
  *			Minimum allowed:- 3 * DTIM. Keep alive period must be
  *			set regardless of power scheme or current power state.
  *			FW use this value also when PM is disabled.
+ * @debug_flags:	debug flags
  * @rx_data_timeout:    Minimum time (usec) from last Rx packet for AM to
  *			PSM transition - legacy PM
  * @tx_data_timeout:    Minimum time (usec) from last Tx packet for AM to
@@ -191,7 +200,8 @@ struct iwl_powertable_cmd {
 
 /**
  * enum iwl_device_power_flags - masks for device power command flags
- * @DEVIC_POWER_FLAGS_POWER_SAVE_ENA_MSK: '1' Allow to save power by turning off
+ * @DEVICE_POWER_FLAGS_POWER_SAVE_ENA_MSK:
+ *	'1' Allow to save power by turning off
  *	receiver and transmitter. '0' - does not allow.
 */
 enum iwl_device_power_flags {
@@ -202,7 +212,8 @@ enum iwl_device_power_flags {
  * struct iwl_device_power_cmd - device wide power command.
  * DEVICE_POWER_CMD = 0x77 (command, has simple generic response)
  *
- * @flags:	Power table command flags from DEVICE_POWER_FLAGS_*
+ * @flags:	Power table command flags from &enum iwl_device_power_flags
+ * @reserved: reserved (padding)
  */
 struct iwl_device_power_cmd {
 	/* PM_POWER_TABLE_CMD_API_S_VER_6 */
@@ -213,7 +224,7 @@ struct iwl_device_power_cmd {
 /**
  * struct iwl_mac_power_cmd - New power command containing uAPSD support
  * MAC_PM_POWER_TABLE = 0xA9 (command, has simple generic response)
- * @id_and_color:	MAC contex identifier
+ * @id_and_color:	MAC contex identifier, &enum iwl_mvm_id_and_color
  * @flags:		Power table command flags from POWER_FLAGS_*
  * @keep_alive_seconds:	Keep alive period in seconds. Default - 25 sec.
  *			Minimum allowed:- 3 * DTIM. Keep alive period must be
@@ -223,7 +234,6 @@ struct iwl_device_power_cmd {
  *			PSM transition - legacy PM
  * @tx_data_timeout:    Minimum time (usec) from last Tx packet for AM to
  *			PSM transition - legacy PM
- * @sleep_interval:	not in use
  * @skip_dtim_periods:	Number of DTIM periods to skip if Skip over DTIM flag
  *			is set. For example, if it is required to skip over
  *			one DTIM, this value need to be set to 2 (DTIM periods).
@@ -233,7 +243,6 @@ struct iwl_device_power_cmd {
  *			PSM transition - uAPSD
  * @lprx_rssi_threshold: Signal strength up to which LP RX can be enabled.
  *			Default: 80dbm
- * @num_skip_dtim:	Number of DTIMs to skip if Skip over DTIM flag is set
  * @snooze_interval:	Maximum time between attempts to retrieve buffered data
  *			from the AP [msec]
  * @snooze_window:	A window of time in which PBW snoozing insures that all
@@ -251,8 +260,9 @@ struct iwl_device_power_cmd {
  * @heavy_rx_thld_packets:	RX threshold measured in number of packets
  * @heavy_tx_thld_percentage:	TX threshold measured in load's percentage
  * @heavy_rx_thld_percentage:	RX threshold measured in load's percentage
- * @limited_ps_threshold:
-*/
+ * @limited_ps_threshold: (unused)
+ * @reserved: reserved (padding)
+ */
 struct iwl_mac_power_cmd {
 	/* CONTEXT_DESC_API_T_VER_1 */
 	__le32 id_and_color;
@@ -343,6 +353,7 @@ struct iwl_dev_tx_power_cmd_v3 {
  * @v3: version 3 of the command, embedded here for easier software handling
  * @enable_ack_reduction: enable or disable close range ack TX power
  *	reduction.
+ * @reserved: reserved (padding)
  */
 struct iwl_dev_tx_power_cmd {
 	/* v4 is just an extension of v3 - keep this here */
@@ -352,6 +363,7 @@ struct iwl_dev_tx_power_cmd {
 } __packed; /* TX_REDUCED_POWER_API_S_VER_4 */
 
 #define IWL_NUM_GEO_PROFILES	3
+#define IWL_GEO_PER_CHAIN_SIZE	3
 
 /**
  * enum iwl_geo_per_chain_offset_operation - type of operation
@@ -391,9 +403,16 @@ struct iwl_geo_tx_power_profiles_cmd {
 } __packed; /* GEO_TX_POWER_LIMIT */
 
 /**
+ * struct iwl_geo_tx_power_profiles_resp -  response to GEO_TX_POWER_LIMIT cmd
+ * @profile_idx: current geo profile in use
+ */
+struct iwl_geo_tx_power_profiles_resp {
+	__le32 profile_idx;
+} __packed; /* GEO_TX_POWER_LIMIT_RESP */
+
+/**
  * struct iwl_beacon_filter_cmd
  * REPLY_BEACON_FILTERING_CMD = 0xd2 (command)
- * @id_and_color: MAC contex identifier
  * @bf_energy_delta: Used for RSSI filtering, if in 'normal' state. Send beacon
  *      to driver if delta in Energy values calculated for this and last
  *      passed beacon is greater than this threshold. Zero value means that
@@ -411,7 +430,7 @@ struct iwl_geo_tx_power_profiles_cmd {
  *      Threshold. Typical energy threshold is -72dBm.
  * @bf_temp_threshold: This threshold determines the type of temperature
  *	filtering (Slow or Fast) that is selected (Units are in Celsuis):
- *      If the current temperature is above this threshold - Fast filter
+ *	If the current temperature is above this threshold - Fast filter
  *	will be used, If the current temperature is below this threshold -
  *	Slow filter will be used.
  * @bf_temp_fast_filter: Send Beacon to driver if delta in temperature values
@@ -425,7 +444,8 @@ struct iwl_geo_tx_power_profiles_cmd {
  *      beacon filtering; beacons will not be forced to be sent to driver
  *      regardless of whether its temerature has been changed.
  * @bf_enable_beacon_filter: 1, beacon filtering is enabled; 0, disabled.
- * @bf_filter_escape_timer: Send beacons to to driver if no beacons were passed
+ * @bf_debug_flag: beacon filtering debug configuration
+ * @bf_escape_timer: Send beacons to to driver if no beacons were passed
  *      for a specific period of time. Units: Beacons.
  * @ba_escape_timer: Fully receive and parse beacon if no beacons were passed
  *      for a longer period of time then this escape-timeout. Units: Beacons.
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-rs.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-rs.h
index a10c6aae9ab9..bdf1228d050b 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-rs.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-rs.h
@@ -368,6 +368,7 @@ enum {
 /**
  * struct iwl_lq_cmd - link quality command
  * @sta_id: station to update
+ * @reduced_tpc: reduced transmit power control value
  * @control: not used
  * @flags: combination of LQ_FLAG_*
  * @mimo_delim: the first SISO index in rs_table, which separates MIMO
@@ -385,6 +386,7 @@ enum {
  *	0: no limit
  *	1: no aggregation (one frame per aggregation)
  *	2 - 0x3f: maximal number of frames (up to 3f == 63)
+ * @reserved2: reserved
  * @rs_table: array of rates for each TX try, each is rate_n_flags,
  *	meaning it is a combination of RATE_MCS_* and IWL_RATE_*_PLCP
  * @ss_params: single stream features. declare whether STBC or BFER are allowed.
@@ -392,7 +394,7 @@ enum {
 struct iwl_lq_cmd {
 	u8 sta_id;
 	u8 reduced_tpc;
-	u16 control;
+	__le16 control;
 	/* LINK_QUAL_GENERAL_PARAMS_API_S_VER_1 */
 	u8 flags;
 	u8 mimo_delim;
@@ -407,4 +409,5 @@ struct iwl_lq_cmd {
 	__le32 rs_table[LQ_MAX_RETRY_NUM];
 	__le32 ss_params;
 }; /* LINK_QUALITY_CMD_API_S_VER_1 */
+
 #endif /* __fw_api_rs_h__ */
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-rx.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-rx.h
index b530fa47d68a..59038ade08d8 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-rx.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-rx.h
@@ -7,7 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
- * Copyright(c) 2015 - 2016 Intel Deutschland GmbH
+ * Copyright(c) 2015 - 2017 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -34,7 +34,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
- * Copyright(c) 2015 - 2016 Intel Deutschland GmbH
+ * Copyright(c) 2015 - 2017 Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -90,13 +90,13 @@ enum iwl_mac_context_info {
  * @non_cfg_phy_cnt: non configurable DSP phy data byte count
  * @cfg_phy_cnt: configurable DSP phy data byte count
  * @stat_id: configurable DSP phy data set ID
- * @reserved1:
+ * @reserved1: reserved
  * @system_timestamp: GP2  at on air rise
  * @timestamp: TSF at on air rise
  * @beacon_time_stamp: beacon at on-air rise
  * @phy_flags: general phy flags: band, modulation, ...
  * @channel: channel number
- * @non_cfg_phy_buf: for various implementations of non_cfg_phy
+ * @non_cfg_phy: for various implementations of non_cfg_phy
  * @rate_n_flags: RATE_MCS_*
  * @byte_count: frame's byte-count
  * @frame_time: frame's time on the air, based on byte count and frame rate
@@ -147,7 +147,8 @@ enum iwl_csum_rx_assist_info {
 
 /**
  * struct iwl_rx_mpdu_res_start - phy info
- * @assist: see CSUM_RX_ASSIST_ above
+ * @byte_count: byte count of the frame
+ * @assist: see &enum iwl_csum_rx_assist_info
  */
 struct iwl_rx_mpdu_res_start {
 	__le16 byte_count;
@@ -157,10 +158,11 @@ struct iwl_rx_mpdu_res_start {
 /**
  * enum iwl_rx_phy_flags - to parse %iwl_rx_phy_info phy_flags
  * @RX_RES_PHY_FLAGS_BAND_24: true if the packet was received on 2.4 band
- * @RX_RES_PHY_FLAGS_MOD_CCK:
+ * @RX_RES_PHY_FLAGS_MOD_CCK: modulation is CCK
  * @RX_RES_PHY_FLAGS_SHORT_PREAMBLE: true if packet's preamble was short
- * @RX_RES_PHY_FLAGS_NARROW_BAND:
+ * @RX_RES_PHY_FLAGS_NARROW_BAND: narrow band (<20 MHz) receive
  * @RX_RES_PHY_FLAGS_ANTENNA: antenna on which the packet was received
+ * @RX_RES_PHY_FLAGS_ANTENNA_POS: antenna bit position
  * @RX_RES_PHY_FLAGS_AGG: set if the packet was part of an A-MPDU
  * @RX_RES_PHY_FLAGS_OFDM_HT: The frame was an HT frame
  * @RX_RES_PHY_FLAGS_OFDM_GF: The frame used GF preamble
@@ -183,9 +185,9 @@ enum iwl_rx_phy_flags {
  * enum iwl_mvm_rx_status - written by fw for each Rx packet
  * @RX_MPDU_RES_STATUS_CRC_OK: CRC is fine
  * @RX_MPDU_RES_STATUS_OVERRUN_OK: there was no RXE overflow
- * @RX_MPDU_RES_STATUS_SRC_STA_FOUND:
- * @RX_MPDU_RES_STATUS_KEY_VALID:
- * @RX_MPDU_RES_STATUS_KEY_PARAM_OK:
+ * @RX_MPDU_RES_STATUS_SRC_STA_FOUND: station was found
+ * @RX_MPDU_RES_STATUS_KEY_VALID: key was valid
+ * @RX_MPDU_RES_STATUS_KEY_PARAM_OK: key parameters were usable
  * @RX_MPDU_RES_STATUS_ICV_OK: ICV is fine, if not, the packet is destroyed
  * @RX_MPDU_RES_STATUS_MIC_OK: used for CCM alg only. TKIP MIC is checked
  *	in the driver.
@@ -197,21 +199,21 @@ enum iwl_rx_phy_flags {
  * @RX_MPDU_RES_STATUS_SEC_WEP_ENC: this frame is encrypted using WEP
  * @RX_MPDU_RES_STATUS_SEC_CCM_ENC: this frame is encrypted using CCM
  * @RX_MPDU_RES_STATUS_SEC_TKIP_ENC: this frame is encrypted using TKIP
+ * @RX_MPDU_RES_STATUS_SEC_EXT_ENC: this frame is encrypted using extension
+ *	algorithm
  * @RX_MPDU_RES_STATUS_SEC_CCM_CMAC_ENC: this frame is encrypted using CCM_CMAC
  * @RX_MPDU_RES_STATUS_SEC_ENC_ERR: this frame couldn't be decrypted
  * @RX_MPDU_RES_STATUS_SEC_ENC_MSK: bitmask of the encryption algorithm
  * @RX_MPDU_RES_STATUS_DEC_DONE: this frame has been successfully decrypted
- * @RX_MPDU_RES_STATUS_PROTECT_FRAME_BIT_CMP:
- * @RX_MPDU_RES_STATUS_EXT_IV_BIT_CMP:
- * @RX_MPDU_RES_STATUS_KEY_ID_CMP_BIT:
+ * @RX_MPDU_RES_STATUS_EXT_IV_BIT_CMP: extended IV (set with TKIP)
+ * @RX_MPDU_RES_STATUS_KEY_ID_CMP_BIT: key ID comparison done
  * @RX_MPDU_RES_STATUS_ROBUST_MNG_FRAME: this frame is an 11w management frame
  * @RX_MPDU_RES_STATUS_CSUM_DONE: checksum was done by the hw
  * @RX_MPDU_RES_STATUS_CSUM_OK: checksum found no errors
- * @RX_MPDU_RES_STATUS_HASH_INDEX_MSK:
- * @RX_MPDU_RES_STATUS_STA_ID_MSK:
- * @RX_MPDU_RES_STATUS_RRF_KILL:
- * @RX_MPDU_RES_STATUS_FILTERING_MSK:
- * @RX_MPDU_RES_STATUS2_FILTERING_MSK:
+ * @RX_MPDU_RES_STATUS_STA_ID_MSK: station ID mask
+ * @RX_MDPU_RES_STATUS_STA_ID_SHIFT: station ID bit shift
+ * @RX_MPDU_RES_STATUS_FILTERING_MSK: filter status
+ * @RX_MPDU_RES_STATUS2_FILTERING_MSK: filter status 2
  */
 enum iwl_mvm_rx_status {
 	RX_MPDU_RES_STATUS_CRC_OK			= BIT(0),
@@ -232,16 +234,13 @@ enum iwl_mvm_rx_status {
 	RX_MPDU_RES_STATUS_SEC_ENC_ERR			= (7 << 8),
 	RX_MPDU_RES_STATUS_SEC_ENC_MSK			= (7 << 8),
 	RX_MPDU_RES_STATUS_DEC_DONE			= BIT(11),
-	RX_MPDU_RES_STATUS_PROTECT_FRAME_BIT_CMP	= BIT(12),
 	RX_MPDU_RES_STATUS_EXT_IV_BIT_CMP		= BIT(13),
 	RX_MPDU_RES_STATUS_KEY_ID_CMP_BIT		= BIT(14),
 	RX_MPDU_RES_STATUS_ROBUST_MNG_FRAME		= BIT(15),
 	RX_MPDU_RES_STATUS_CSUM_DONE			= BIT(16),
 	RX_MPDU_RES_STATUS_CSUM_OK			= BIT(17),
-	RX_MPDU_RES_STATUS_HASH_INDEX_MSK		= (0x3F0000),
 	RX_MDPU_RES_STATUS_STA_ID_SHIFT			= 24,
 	RX_MPDU_RES_STATUS_STA_ID_MSK			= 0x1f << RX_MDPU_RES_STATUS_STA_ID_SHIFT,
-	RX_MPDU_RES_STATUS_RRF_KILL			= BIT(29),
 	RX_MPDU_RES_STATUS_FILTERING_MSK		= (0xc00000),
 	RX_MPDU_RES_STATUS2_FILTERING_MSK		= (0xc0000000),
 };
@@ -348,35 +347,106 @@ enum iwl_rx_mpdu_mac_info {
 	IWL_RX_MPDU_PHY_PHY_INDEX_MASK		= 0xf0,
 };
 
+/**
+ * struct iwl_rx_mpdu_desc - RX MPDU descriptor
+ */
 struct iwl_rx_mpdu_desc {
 	/* DW2 */
+	/**
+	 * @mpdu_len: MPDU length
+	 */
 	__le16 mpdu_len;
+	/**
+	 * @mac_flags1: &enum iwl_rx_mpdu_mac_flags1
+	 */
 	u8 mac_flags1;
+	/**
+	 * @mac_flags2: &enum iwl_rx_mpdu_mac_flags2
+	 */
 	u8 mac_flags2;
 	/* DW3 */
+	/**
+	 * @amsdu_info: &enum iwl_rx_mpdu_amsdu_info
+	 */
 	u8 amsdu_info;
+	/**
+	 * @phy_info: &enum iwl_rx_mpdu_phy_info
+	 */
 	__le16 phy_info;
+	/**
+	 * @mac_phy_idx: MAC/PHY index
+	 */
 	u8 mac_phy_idx;
 	/* DW4 - carries csum data only when rpa_en == 1 */
-	__le16 raw_csum; /* alledgedly unreliable */
+	/**
+	 * @raw_csum: raw checksum (alledgedly unreliable)
+	 */
+	__le16 raw_csum;
+	/**
+	 * @l3l4_flags: &enum iwl_rx_l3l4_flags
+	 */
 	__le16 l3l4_flags;
 	/* DW5 */
+	/**
+	 * @status: &enum iwl_rx_mpdu_status
+	 */
 	__le16 status;
+	/**
+	 * @hash_filter: hash filter value
+	 */
 	u8 hash_filter;
+	/**
+	 * @sta_id_flags: &enum iwl_rx_mpdu_sta_id_flags
+	 */
 	u8 sta_id_flags;
 	/* DW6 */
+	/**
+	 * @reorder_data: &enum iwl_rx_mpdu_reorder_data
+	 */
 	__le32 reorder_data;
 	/* DW7 - carries rss_hash only when rpa_en == 1 */
+	/**
+	 * @rss_hash: RSS hash value
+	 */
 	__le32 rss_hash;
 	/* DW8 - carries filter_match only when rpa_en == 1 */
+	/**
+	 * @filter_match: filter match value
+	 */
 	__le32 filter_match;
 	/* DW9 */
+	/**
+	 * @rate_n_flags: RX rate/flags encoding
+	 */
 	__le32 rate_n_flags;
 	/* DW10 */
-	u8 energy_a, energy_b, channel, mac_context;
+	/**
+	 * @energy_a: energy chain A
+	 */
+	u8 energy_a;
+	/**
+	 * @energy_b: energy chain B
+	 */
+	u8 energy_b;
+	/**
+	 * @channel: channel number
+	 */
+	u8 channel;
+	/**
+	 * @mac_context: MAC context mask
+	 */
+	u8 mac_context;
 	/* DW11 */
+	/**
+	 * @gp2_on_air_rise: GP2 timer value on air rise (INA)
+	 */
 	__le32 gp2_on_air_rise;
-	/* DW12 & DW13 - carries TSF only TSF_OVERLOAD bit == 0 */
+	/* DW12 & DW13 */
+	/**
+	 * @tsf_on_air_rise:
+	 * TSF value on air rise (INA), only valid if
+	 * %IWL_RX_MPDU_PHY_TSF_OVERLOAD isn't set
+	 */
 	__le64 tsf_on_air_rise;
 } __packed;
 
@@ -404,6 +474,7 @@ enum iwl_rss_hash_func_en {
  *
  * @flags: 1 - enable, 0 - disable
  * @hash_mask: Type of RSS to use. Values are from %iwl_rss_hash_func_en
+ * @reserved: reserved
  * @secret_key: 320 bit input of random key configuration from driver
  * @indirection_table: indirection table
  */
@@ -447,7 +518,7 @@ struct iwl_rxq_sync_notification {
 } __packed; /* MULTI_QUEUE_DRV_SYNC_HDR_CMD_API_S_VER_1 */
 
 /**
- * Internal message identifier
+ * enum iwl_mvm_rxq_notif_type - Internal message identifier
  *
  * @IWL_MVM_RXQ_EMPTY: empty sync notification
  * @IWL_MVM_RXQ_NOTIF_DEL_BA: notify RSS queues of delBA
@@ -491,13 +562,13 @@ enum iwl_mvm_pm_event {
 /**
  * struct iwl_mvm_pm_state_notification - station PM state notification
  * @sta_id: station ID of the station changing state
- * @type: the new powersave state, see IWL_MVM_PM_EVENT_ above
+ * @type: the new powersave state, see &enum iwl_mvm_pm_event
  */
 struct iwl_mvm_pm_state_notification {
 	u8 sta_id;
 	u8 type;
 	/* private: */
-	u16 reserved;
+	__le16 reserved;
 } __packed; /* PEER_PM_NTFY_API_S_VER_1 */
 
 #endif /* __fw_api_rx_h__ */
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-scan.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-scan.h
index 3178eb96e395..1cd7cc087936 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-scan.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-scan.h
@@ -7,7 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
- * Copyright(c) 2016 Intel Deutschland GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -34,6 +34,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -67,8 +68,6 @@
 #ifndef __fw_api_scan_h__
 #define __fw_api_scan_h__
 
-#include "fw-api.h"
-
 /* Scan Commands, Responses, Notifications */
 
 /* Max number of IEs for direct SSID scans in a command */
@@ -81,6 +80,10 @@
  * selected by "type" bit field in struct iwl_scan_channel;
  * each channel may select different ssids from among the 20 entries.
  * SSID IEs get transmitted in reverse order of entry.
+ *
+ * @id: element ID
+ * @len: element length
+ * @ssid: element (SSID) data
  */
 struct iwl_ssid_ie {
 	u8 id;
@@ -111,7 +114,7 @@ enum scan_framework_client {
 };
 
 /**
- * iwl_scan_offload_blacklist - SCAN_OFFLOAD_BLACKLIST_S
+ * struct iwl_scan_offload_blacklist - SCAN_OFFLOAD_BLACKLIST_S
  * @ssid:		MAC address to filter out
  * @reported_rssi:	AP rssi reported to the host
  * @client_bitmap: clients ignore this entry  - enum scan_framework_client
@@ -135,13 +138,14 @@ enum iwl_scan_offload_band_selection {
 };
 
 /**
- * iwl_scan_offload_profile - SCAN_OFFLOAD_PROFILE_S
+ * struct iwl_scan_offload_profile - SCAN_OFFLOAD_PROFILE_S
  * @ssid_index:		index to ssid list in fixed part
  * @unicast_cipher:	encryption algorithm to match - bitmap
- * @aut_alg:		authentication algorithm to match - bitmap
+ * @auth_alg:		authentication algorithm to match - bitmap
  * @network_type:	enum iwl_scan_offload_network_type
  * @band_selection:	enum iwl_scan_offload_band_selection
  * @client_bitmap:	clients waiting for match - enum scan_framework_client
+ * @reserved:		reserved
  */
 struct iwl_scan_offload_profile {
 	u8 ssid_index;
@@ -154,8 +158,7 @@ struct iwl_scan_offload_profile {
 } __packed;
 
 /**
- * iwl_scan_offload_profile_cfg - SCAN_OFFLOAD_PROFILES_CFG_API_S_VER_1
- * @blaclist:		AP list to filter off from scan results
+ * struct iwl_scan_offload_profile_cfg - SCAN_OFFLOAD_PROFILES_CFG_API_S_VER_1
  * @profiles:		profiles to search for match
  * @blacklist_len:	length of blacklist
  * @num_profiles:	num of profiles in the list
@@ -163,6 +166,7 @@ struct iwl_scan_offload_profile {
  * @pass_match:		clients waiting for the results
  * @active_clients:	active clients bitmap - enum scan_framework_client
  * @any_beacon_notify:	clients waiting for match notification without match
+ * @reserved:		reserved
  */
 struct iwl_scan_offload_profile_cfg {
 	struct iwl_scan_offload_profile profiles[IWL_SCAN_MAX_PROFILES];
@@ -176,7 +180,7 @@ struct iwl_scan_offload_profile_cfg {
 } __packed;
 
 /**
- * iwl_scan_schedule_lmac - schedule of scan offload
+ * struct iwl_scan_schedule_lmac - schedule of scan offload
  * @delay:		delay between iterations, in seconds.
  * @iterations:		num of scan iterations
  * @full_scan_mul:	number of partial scans before each full scan
@@ -200,7 +204,7 @@ enum iwl_scan_ebs_status {
 };
 
 /**
- * iwl_scan_req_tx_cmd - SCAN_REQ_TX_CMD_API_S
+ * struct iwl_scan_req_tx_cmd - SCAN_REQ_TX_CMD_API_S
  * @tx_flags: combination of TX_CMD_FLG_*
  * @rate_n_flags: rate for *all* Tx attempts, if TX_CMD_FLG_STA_RATE_MSK is
  *	cleared. Combination of RATE_MCS_*
@@ -220,10 +224,10 @@ enum iwl_scan_channel_flags_lmac {
 };
 
 /**
- * iwl_scan_channel_cfg_lmac - SCAN_CHANNEL_CFG_S_VER2
+ * struct iwl_scan_channel_cfg_lmac - SCAN_CHANNEL_CFG_S_VER2
  * @flags:		bits 1-20: directed scan to i'th ssid
  *			other bits &enum iwl_scan_channel_flags_lmac
- * @channel_number:	channel number 1-13 etc
+ * @channel_num:	channel number 1-13 etc
  * @iter_count:		scan iteration on this channel
  * @iter_interval:	interval in seconds between iterations on one channel
  */
@@ -235,7 +239,7 @@ struct iwl_scan_channel_cfg_lmac {
 } __packed;
 
 /*
- * iwl_scan_probe_segment - PROBE_SEGMENT_API_S_VER_1
+ * struct iwl_scan_probe_segment - PROBE_SEGMENT_API_S_VER_1
  * @offset: offset in the data block
  * @len: length of the segment
  */
@@ -263,7 +267,7 @@ enum iwl_scan_channel_flags {
 	IWL_SCAN_CHANNEL_FLAG_CACHE_ADD		= BIT(2),
 };
 
-/* iwl_scan_channel_opt - CHANNEL_OPTIMIZATION_API_S
+/* struct iwl_scan_channel_opt - CHANNEL_OPTIMIZATION_API_S
  * @flags: enum iwl_scan_channel_flags
  * @non_ebs_ratio: defines the ratio of number of scan iterations where EBS is
  *	involved.
@@ -276,13 +280,13 @@ struct iwl_scan_channel_opt {
 } __packed;
 
 /**
- * iwl_mvm_lmac_scan_flags
+ * enum iwl_mvm_lmac_scan_flags - LMAC scan flags
  * @IWL_MVM_LMAC_SCAN_FLAG_PASS_ALL: pass all beacons and probe responses
  *	without filtering.
  * @IWL_MVM_LMAC_SCAN_FLAG_PASSIVE: force passive scan on all channels
  * @IWL_MVM_LMAC_SCAN_FLAG_PRE_CONNECTION: single channel scan
  * @IWL_MVM_LMAC_SCAN_FLAG_ITER_COMPLETE: send iteration complete notification
- * @IWL_MVM_LMAC_SCAN_FLAG_MULTIPLE_SSIDS multiple SSID matching
+ * @IWL_MVM_LMAC_SCAN_FLAG_MULTIPLE_SSIDS: multiple SSID matching
  * @IWL_MVM_LMAC_SCAN_FLAG_FRAGMENTED: all passive scans will be fragmented
  * @IWL_MVM_LMAC_SCAN_FLAGS_RRM_ENABLED: insert WFA vendor-specific TPC report
  *	and DS parameter set IEs into probe requests.
@@ -320,15 +324,15 @@ enum iwl_scan_priority_ext {
 };
 
 /**
- * iwl_scan_req_lmac - SCAN_REQUEST_CMD_API_S_VER_1
+ * struct iwl_scan_req_lmac - SCAN_REQUEST_CMD_API_S_VER_1
  * @reserved1: for alignment and future use
- * @channel_num: num of channels to scan
- * @active-dwell: dwell time for active channels
- * @passive-dwell: dwell time for passive channels
- * @fragmented-dwell: dwell time for fragmented passive scan
+ * @n_channels: num of channels to scan
+ * @active_dwell: dwell time for active channels
+ * @passive_dwell: dwell time for passive channels
+ * @fragmented_dwell: dwell time for fragmented passive scan
  * @extended_dwell: dwell time for channels 1, 6 and 11 (in certain cases)
  * @reserved2: for alignment and future use
- * @rx_chain_selct: PHY_RX_CHAIN_* flags
+ * @rx_chain_select: PHY_RX_CHAIN_* flags
  * @scan_flags: &enum iwl_mvm_lmac_scan_flags
  * @max_out_time: max time (in TU) to be out of associated channel
  * @suspend_time: pause scan this long (TUs) when returning to service channel
@@ -410,12 +414,13 @@ struct iwl_lmac_scan_complete_notif {
 } __packed;
 
 /**
- * iwl_scan_offload_complete - PERIODIC_SCAN_COMPLETE_NTF_API_S_VER_2
+ * struct iwl_scan_offload_complete - PERIODIC_SCAN_COMPLETE_NTF_API_S_VER_2
  * @last_schedule_line: last schedule line executed (fast or regular)
  * @last_schedule_iteration: last scan iteration executed before scan abort
- * @status: enum iwl_scan_offload_complete_status
+ * @status: &enum iwl_scan_offload_complete_status
  * @ebs_status: EBS success status &enum iwl_scan_ebs_status
- * @time_after_last_iter; time in seconds elapsed after last iteration
+ * @time_after_last_iter: time in seconds elapsed after last iteration
+ * @reserved: reserved
  */
 struct iwl_periodic_scan_complete {
 	u8 last_schedule_line;
@@ -547,12 +552,12 @@ struct iwl_scan_config {
 } __packed; /* SCAN_CONFIG_DB_CMD_API_S_3 */
 
 /**
- * iwl_umac_scan_flags
- *@IWL_UMAC_SCAN_FLAG_PREEMPTIVE: scan process triggered by this scan request
+ * enum iwl_umac_scan_flags - UMAC scan flags
+ * @IWL_UMAC_SCAN_FLAG_PREEMPTIVE: scan process triggered by this scan request
  *	can be preempted by other scan requests with higher priority.
  *	The low priority scan will be resumed when the higher proirity scan is
  *	completed.
- *@IWL_UMAC_SCAN_FLAG_START_NOTIF: notification will be sent to the driver
+ * @IWL_UMAC_SCAN_FLAG_START_NOTIF: notification will be sent to the driver
  *	when scan starts.
  */
 enum iwl_umac_scan_flags {
@@ -701,8 +706,8 @@ struct iwl_umac_scan_abort {
  * struct iwl_umac_scan_complete
  * @uid: scan id, &enum iwl_umac_scan_uid_offsets
  * @last_schedule: last scheduling line
- * @last_iter:	last scan iteration number
- * @scan status: &enum iwl_scan_offload_complete_status
+ * @last_iter: last scan iteration number
+ * @status: &enum iwl_scan_offload_complete_status
  * @ebs_status: &enum iwl_scan_ebs_status
  * @time_from_last_iter: time elapsed from last iteration
  * @reserved: for future use
@@ -721,9 +726,10 @@ struct iwl_umac_scan_complete {
 /**
  * struct iwl_scan_offload_profile_match - match information
  * @bssid: matched bssid
+ * @reserved: reserved
  * @channel: channel where the match occurred
- * @energy:
- * @matching_feature:
+ * @energy: energy
+ * @matching_feature: feature matches
  * @matching_channels: bitmap of channels that matched, referencing
  *	the channels passed in tue scan offload request
  */
@@ -771,7 +777,7 @@ struct iwl_scan_offload_profiles_query {
  * @last_channel: last channel that was scanned
  * @start_tsf: TSF timer in usecs of the scan start time for the mac specified
  *	in &struct iwl_scan_req_umac.
- * @results: array of scan results, only "scanned_channels" of them are valid
+ * @results: array of scan results, length in @scanned_channels
  */
 struct iwl_umac_scan_iter_complete_notif {
 	__le32 uid;
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-sta.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-sta.h
index 421b9dd1fb66..81f0a3463bac 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-sta.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-sta.h
@@ -69,8 +69,8 @@
 
 /**
  * enum iwl_sta_flags - flags for the ADD_STA host command
- * @STA_FLG_REDUCED_TX_PWR_CTRL:
- * @STA_FLG_REDUCED_TX_PWR_DATA:
+ * @STA_FLG_REDUCED_TX_PWR_CTRL: reduced TX power (control frames)
+ * @STA_FLG_REDUCED_TX_PWR_DATA: reduced TX power (data frames)
  * @STA_FLG_DISABLE_TX: set if TX should be disabled
  * @STA_FLG_PS: set if STA is in Power Save
  * @STA_FLG_INVALID: set if STA is invalid
@@ -78,18 +78,40 @@
  * @STA_FLG_SET_ALL_KEYS: the current key applies to all key IDs
  * @STA_FLG_DRAIN_FLOW: drain flow
  * @STA_FLG_PAN: STA is for PAN interface
- * @STA_FLG_CLASS_AUTH:
- * @STA_FLG_CLASS_ASSOC:
- * @STA_FLG_CLASS_MIMO_PROT:
- * @STA_FLG_MAX_AGG_SIZE_MSK: maximal size for A-MPDU
+ * @STA_FLG_CLASS_AUTH: station is authenticated
+ * @STA_FLG_CLASS_ASSOC: station is associated
+ * @STA_FLG_RTS_MIMO_PROT: station requires RTS MIMO protection (dynamic SMPS)
+ * @STA_FLG_MAX_AGG_SIZE_MSK: maximal size for A-MPDU (mask)
+ * @STA_FLG_MAX_AGG_SIZE_SHIFT: maximal size for A-MPDU (bit shift)
+ * @STA_FLG_MAX_AGG_SIZE_8K: maximal size for A-MPDU (8k supported)
+ * @STA_FLG_MAX_AGG_SIZE_16K: maximal size for A-MPDU (16k supported)
+ * @STA_FLG_MAX_AGG_SIZE_32K: maximal size for A-MPDU (32k supported)
+ * @STA_FLG_MAX_AGG_SIZE_64K: maximal size for A-MPDU (64k supported)
+ * @STA_FLG_MAX_AGG_SIZE_128K: maximal size for A-MPDU (128k supported)
+ * @STA_FLG_MAX_AGG_SIZE_256K: maximal size for A-MPDU (256k supported)
+ * @STA_FLG_MAX_AGG_SIZE_512K: maximal size for A-MPDU (512k supported)
+ * @STA_FLG_MAX_AGG_SIZE_1024K: maximal size for A-MPDU (1024k supported)
  * @STA_FLG_AGG_MPDU_DENS_MSK: maximal MPDU density for Tx aggregation
  * @STA_FLG_FAT_EN_MSK: support for channel width (for Tx). This flag is
  *	initialised by driver and can be updated by fw upon reception of
  *	action frames that can change the channel width. When cleared the fw
  *	will send all the frames in 20MHz even when FAT channel is requested.
+ * @STA_FLG_FAT_EN_20MHZ: no wide channels are supported, only 20 MHz
+ * @STA_FLG_FAT_EN_40MHZ: wide channels up to 40 MHz supported
+ * @STA_FLG_FAT_EN_80MHZ: wide channels up to 80 MHz supported
+ * @STA_FLG_FAT_EN_160MHZ: wide channels up to 160 MHz supported
  * @STA_FLG_MIMO_EN_MSK: support for MIMO. This flag is initialised by the
  *	driver and can be updated by fw upon reception of action frames.
+ * @STA_FLG_MIMO_EN_SISO: no support for MIMO
+ * @STA_FLG_MIMO_EN_MIMO2: 2 streams supported
+ * @STA_FLG_MIMO_EN_MIMO3: 3 streams supported
  * @STA_FLG_MFP_EN: Management Frame Protection
+ * @STA_FLG_AGG_MPDU_DENS_MSK: A-MPDU density (mask)
+ * @STA_FLG_AGG_MPDU_DENS_SHIFT: A-MPDU density (bit shift)
+ * @STA_FLG_AGG_MPDU_DENS_2US: A-MPDU density (2 usec gap)
+ * @STA_FLG_AGG_MPDU_DENS_4US: A-MPDU density (4 usec gap)
+ * @STA_FLG_AGG_MPDU_DENS_8US: A-MPDU density (8 usec gap)
+ * @STA_FLG_AGG_MPDU_DENS_16US: A-MPDU density (16 usec gap)
  */
 enum iwl_sta_flags {
 	STA_FLG_REDUCED_TX_PWR_CTRL	= BIT(3),
@@ -148,9 +170,10 @@ enum iwl_sta_flags {
  * @STA_KEY_FLG_WEP_KEY_MAP: wep is either a group key (0 - legacy WEP) or from
  *	station info array (1 - n 1X mode)
  * @STA_KEY_FLG_KEYID_MSK: the index of the key
+ * @STA_KEY_FLG_KEYID_POS: key index bit position
  * @STA_KEY_NOT_VALID: key is invalid
  * @STA_KEY_FLG_WEP_13BYTES: set for 13 bytes WEP key
- * @STA_KEY_FLG_KEY_32BYTES for non-wep key set for 32 bytes key
+ * @STA_KEY_FLG_KEY_32BYTES: for non-wep key set for 32 bytes key
  * @STA_KEY_MULTICAST: set for multical key
  * @STA_KEY_MFP: key is used for Management Frame Protection
  */
@@ -183,7 +206,7 @@ enum iwl_sta_key_flag {
  * @STA_MODIFY_ADD_BA_TID: this command modifies %add_immediate_ba_tid
  * @STA_MODIFY_REMOVE_BA_TID: this command modifies %remove_immediate_ba_tid
  * @STA_MODIFY_SLEEPING_STA_TX_COUNT: this command modifies %sleep_tx_count
- * @STA_MODIFY_PROT_TH:
+ * @STA_MODIFY_PROT_TH: modify RTS threshold
  * @STA_MODIFY_QUEUES: modify the queues used by this station
  */
 enum iwl_sta_modify_flag {
@@ -197,13 +220,21 @@ enum iwl_sta_modify_flag {
 	STA_MODIFY_QUEUES			= BIT(7),
 };
 
-#define STA_MODE_MODIFY	1
+/**
+ * enum iwl_sta_mode - station command mode
+ * @STA_MODE_ADD: add new station
+ * @STA_MODE_MODIFY: modify the station
+ */
+enum iwl_sta_mode {
+	STA_MODE_ADD	= 0,
+	STA_MODE_MODIFY	= 1,
+};
 
 /**
  * enum iwl_sta_sleep_flag - type of sleep of the station
- * @STA_SLEEP_STATE_AWAKE:
- * @STA_SLEEP_STATE_PS_POLL:
- * @STA_SLEEP_STATE_UAPSD:
+ * @STA_SLEEP_STATE_AWAKE: station is awake
+ * @STA_SLEEP_STATE_PS_POLL: station is PS-polling
+ * @STA_SLEEP_STATE_UAPSD: station uses U-APSD
  * @STA_SLEEP_STATE_MOREDATA: set more-data bit on
  *	(last) released frame
  */
@@ -223,10 +254,12 @@ enum iwl_sta_sleep_flag {
 
 /**
  * struct iwl_mvm_keyinfo - key information
- * @key_flags: type %iwl_sta_key_flag
+ * @key_flags: type &enum iwl_sta_key_flag
  * @tkip_rx_tsc_byte2: TSC[2] for key mix ph1 detection
+ * @reserved1: reserved
  * @tkip_rx_ttak: 10-byte unicast TKIP TTAK for Rx
  * @key_offset: key offset in the fw's key table
+ * @reserved2: reserved
  * @key: 16-byte unicast decryption key
  * @tx_secur_seq_cnt: initial RSC / PN needed for replay check
  * @hw_tkip_mic_rx_key: byte: MIC Rx Key - used for TKIP only
@@ -253,17 +286,21 @@ struct iwl_mvm_keyinfo {
 /**
  * struct iwl_mvm_add_sta_cmd_v7 - Add/modify a station in the fw's sta table.
  * ( REPLY_ADD_STA = 0x18 )
- * @add_modify: 1: modify existing, 0: add new station
- * @awake_acs:
+ * @add_modify: see &enum iwl_sta_mode
+ * @awake_acs: ACs to transmit data on while station is sleeping (for U-APSD)
  * @tid_disable_tx: is tid BIT(tid) enabled for Tx. Clear BIT(x) to enable
  *	AMPDU for tid x. Set %STA_MODIFY_TID_DISABLE_TX to change this field.
- * @mac_id_n_color: the Mac context this station belongs to
- * @addr[ETH_ALEN]: station's MAC address
+ * @mac_id_n_color: the Mac context this station belongs to,
+ *	see &enum iwl_mvm_id_and_color
+ * @addr: station's MAC address
+ * @reserved2: reserved
  * @sta_id: index of station in uCode's station table
  * @modify_mask: STA_MODIFY_*, selects which parameters to modify vs. leave
  *	alone. 1 - modify, 0 - don't change.
- * @station_flags: look at %iwl_sta_flags
- * @station_flags_msk: what of %station_flags have changed
+ * @reserved3: reserved
+ * @station_flags: look at &enum iwl_sta_flags
+ * @station_flags_msk: what of %station_flags have changed,
+ *	also &enum iwl_sta_flags
  * @add_immediate_ba_tid: tid for which to add block-ack support (Rx)
  *	Set %STA_MODIFY_ADD_BA_TID to use this field, and also set
  *	add_immediate_ba_ssn.
@@ -274,7 +311,7 @@ struct iwl_mvm_keyinfo {
  * @sleep_tx_count: number of packets to transmit to station even though it is
  *	asleep. Used to synchronise PS-poll and u-APSD responses while ucode
  *	keeps track of STA sleep state.
- * @sleep_state_flags: Look at %iwl_sta_sleep_flag.
+ * @sleep_state_flags: Look at &enum iwl_sta_sleep_flag.
  * @assoc_id: assoc_id to be sent in VHT PLCP (9-bit), for grp use 0, for AP
  *	mac-addr.
  * @beamform_flags: beam forming controls
@@ -330,17 +367,21 @@ enum iwl_sta_type {
 /**
  * struct iwl_mvm_add_sta_cmd - Add/modify a station in the fw's sta table.
  * ( REPLY_ADD_STA = 0x18 )
- * @add_modify: 1: modify existing, 0: add new station
- * @awake_acs:
+ * @add_modify: see &enum iwl_sta_mode
+ * @awake_acs: ACs to transmit data on while station is sleeping (for U-APSD)
  * @tid_disable_tx: is tid BIT(tid) enabled for Tx. Clear BIT(x) to enable
  *	AMPDU for tid x. Set %STA_MODIFY_TID_DISABLE_TX to change this field.
- * @mac_id_n_color: the Mac context this station belongs to
- * @addr[ETH_ALEN]: station's MAC address
+ * @mac_id_n_color: the Mac context this station belongs to,
+ *	see &enum iwl_mvm_id_and_color
+ * @addr: station's MAC address
+ * @reserved2: reserved
  * @sta_id: index of station in uCode's station table
  * @modify_mask: STA_MODIFY_*, selects which parameters to modify vs. leave
  *	alone. 1 - modify, 0 - don't change.
- * @station_flags: look at %iwl_sta_flags
- * @station_flags_msk: what of %station_flags have changed
+ * @reserved3: reserved
+ * @station_flags: look at &enum iwl_sta_flags
+ * @station_flags_msk: what of %station_flags have changed,
+ *	also &enum iwl_sta_flags
  * @add_immediate_ba_tid: tid for which to add block-ack support (Rx)
  *	Set %STA_MODIFY_ADD_BA_TID to use this field, and also set
  *	add_immediate_ba_ssn.
@@ -352,7 +393,7 @@ enum iwl_sta_type {
  *	asleep. Used to synchronise PS-poll and u-APSD responses while ucode
  *	keeps track of STA sleep state.
  * @station_type: type of this station. See &enum iwl_sta_type.
- * @sleep_state_flags: Look at %iwl_sta_sleep_flag.
+ * @sleep_state_flags: Look at &enum iwl_sta_sleep_flag.
  * @assoc_id: assoc_id to be sent in VHT PLCP (9-bit), for grp use 0, for AP
  *	mac-addr.
  * @beamform_flags: beam forming controls
@@ -401,7 +442,7 @@ struct iwl_mvm_add_sta_cmd {
  * ( REPLY_ADD_STA_KEY = 0x17 )
  * @sta_id: index of station in uCode's station table
  * @key_offset: key offset in key storage
- * @key_flags: type %iwl_sta_key_flag
+ * @key_flags: type &enum iwl_sta_key_flag
  * @key: key material data
  * @rx_secur_seq_cnt: RX security sequence counter for the key
  */
@@ -417,6 +458,7 @@ struct iwl_mvm_add_sta_key_common {
  * struct iwl_mvm_add_sta_key_cmd_v1 - add/modify sta key
  * @common: see &struct iwl_mvm_add_sta_key_common
  * @tkip_rx_tsc_byte2: TSC[2] for key mix ph1 detection
+ * @reserved: reserved
  * @tkip_rx_ttak: 10-byte unicast TKIP TTAK for Rx
  */
 struct iwl_mvm_add_sta_key_cmd_v1 {
@@ -459,6 +501,7 @@ enum iwl_mvm_add_sta_rsp_status {
  * struct iwl_mvm_rm_sta_cmd - Add / modify a station in the fw's station table
  * ( REMOVE_STA = 0x19 )
  * @sta_id: the station id of the station to be removed
+ * @reserved: reserved
  */
 struct iwl_mvm_rm_sta_cmd {
 	u8 sta_id;
@@ -468,12 +511,12 @@ struct iwl_mvm_rm_sta_cmd {
 /**
  * struct iwl_mvm_mgmt_mcast_key_cmd_v1
  * ( MGMT_MCAST_KEY = 0x1f )
- * @ctrl_flags: %iwl_sta_key_flag
- * @igtk:
+ * @ctrl_flags: &enum iwl_sta_key_flag
+ * @igtk: IGTK key material
  * @k1: unused
  * @k2: unused
  * @sta_id: station ID that support IGTK
- * @key_id:
+ * @key_id: key ID
  * @receive_seq_cnt: initial RSC/PN needed for replay check
  */
 struct iwl_mvm_mgmt_mcast_key_cmd_v1 {
@@ -489,10 +532,10 @@ struct iwl_mvm_mgmt_mcast_key_cmd_v1 {
 /**
  * struct iwl_mvm_mgmt_mcast_key_cmd
  * ( MGMT_MCAST_KEY = 0x1f )
- * @ctrl_flags: %iwl_sta_key_flag
+ * @ctrl_flags: &enum iwl_sta_key_flag
  * @igtk: IGTK master key
  * @sta_id: station ID that support IGTK
- * @key_id:
+ * @key_id: key ID
  * @receive_seq_cnt: initial RSC/PN needed for replay check
  */
 struct iwl_mvm_mgmt_mcast_key_cmd {
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-stats.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-stats.h
index 6371c342b96d..c7531da508fd 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-stats.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-stats.h
@@ -7,7 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
- * Copyright(c) 2016 Intel Deutschland GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -72,7 +72,7 @@ struct mvm_statistics_dbg {
 	__le32 burst_check;
 	__le32 burst_count;
 	__le32 wait_for_silence_timeout_cnt;
-	__le32 reserved[3];
+	u8 reserved[12];
 } __packed; /* STATISTICS_DEBUG_API_S_VER_2 */
 
 struct mvm_statistics_div {
@@ -84,7 +84,55 @@ struct mvm_statistics_div {
 	__le32 reserved2;
 } __packed; /* STATISTICS_SLOW_DIV_API_S_VER_2 */
 
+/**
+ * struct mvm_statistics_rx_non_phy
+ * @bogus_cts: CTS received when not expecting CTS
+ * @bogus_ack: ACK received when not expecting ACK
+ * @non_channel_beacons: beacons with our bss id but not on our serving channel
+ * @channel_beacons: beacons with our bss id and in our serving channel
+ * @num_missed_bcon: number of missed beacons
+ * @adc_rx_saturation_time: count in 0.8us units the time the ADC was in
+ *	saturation
+ * @ina_detection_search_time: total time (in 0.8us) searched for INA
+ * @beacon_silence_rssi_a: RSSI silence after beacon frame
+ * @beacon_silence_rssi_b: RSSI silence after beacon frame
+ * @beacon_silence_rssi_c: RSSI silence after beacon frame
+ * @interference_data_flag: flag for interference data availability. 1 when data
+ *	is available.
+ * @channel_load: counts RX Enable time in uSec
+ * @beacon_rssi_a: beacon RSSI on anntena A
+ * @beacon_rssi_b: beacon RSSI on antenna B
+ * @beacon_rssi_c: beacon RSSI on antenna C
+ * @beacon_energy_a: beacon energy on antenna A
+ * @beacon_energy_b: beacon energy on antenna B
+ * @beacon_energy_c: beacon energy on antenna C
+ * @num_bt_kills: number of BT "kills" (frame TX aborts)
+ * @mac_id: mac ID
+ */
 struct mvm_statistics_rx_non_phy {
+	__le32 bogus_cts;
+	__le32 bogus_ack;
+	__le32 non_channel_beacons;
+	__le32 channel_beacons;
+	__le32 num_missed_bcon;
+	__le32 adc_rx_saturation_time;
+	__le32 ina_detection_search_time;
+	__le32 beacon_silence_rssi_a;
+	__le32 beacon_silence_rssi_b;
+	__le32 beacon_silence_rssi_c;
+	__le32 interference_data_flag;
+	__le32 channel_load;
+	__le32 beacon_rssi_a;
+	__le32 beacon_rssi_b;
+	__le32 beacon_rssi_c;
+	__le32 beacon_energy_a;
+	__le32 beacon_energy_b;
+	__le32 beacon_energy_c;
+	__le32 num_bt_kills;
+	__le32 mac_id;
+} __packed; /* STATISTICS_RX_NON_PHY_API_S_VER_4 */
+
+struct mvm_statistics_rx_non_phy_v3 {
 	__le32 bogus_cts;	/* CTS received when not expecting CTS */
 	__le32 bogus_ack;	/* ACK received when not expecting ACK */
 	__le32 non_bssid_frames;	/* number of frames with BSSID that
@@ -121,6 +169,14 @@ struct mvm_statistics_rx_non_phy {
 } __packed; /* STATISTICS_RX_NON_PHY_API_S_VER_3 */
 
 struct mvm_statistics_rx_phy {
+	__le32 unresponded_rts;
+	__le32 rxe_frame_lmt_overrun;
+	__le32 sent_ba_rsp_cnt;
+	__le32 dsp_self_kill;
+	__le32 reserved;
+} __packed; /* STATISTICS_RX_PHY_API_S_VER_3 */
+
+struct mvm_statistics_rx_phy_v2 {
 	__le32 ina_cnt;
 	__le32 fina_cnt;
 	__le32 plcp_err;
@@ -143,7 +199,7 @@ struct mvm_statistics_rx_phy {
 	__le32 reserved;
 } __packed; /* STATISTICS_RX_PHY_API_S_VER_2 */
 
-struct mvm_statistics_rx_ht_phy {
+struct mvm_statistics_rx_ht_phy_v1 {
 	__le32 plcp_err;
 	__le32 overrun_err;
 	__le32 early_overrun_err;
@@ -156,7 +212,14 @@ struct mvm_statistics_rx_ht_phy {
 	__le32 unsupport_mcs;
 } __packed;  /* STATISTICS_HT_RX_PHY_API_S_VER_1 */
 
-struct mvm_statistics_tx_non_phy {
+struct mvm_statistics_rx_ht_phy {
+	__le32 mh_format_err;
+	__le32 agg_mpdu_cnt;
+	__le32 agg_cnt;
+	__le32 unsupport_mcs;
+} __packed;  /* STATISTICS_HT_RX_PHY_API_S_VER_2 */
+
+struct mvm_statistics_tx_non_phy_v3 {
 	__le32 preamble_cnt;
 	__le32 rx_detected_cnt;
 	__le32 bt_prio_defer_cnt;
@@ -173,6 +236,19 @@ struct mvm_statistics_tx_non_phy {
 	__le32 ack_or_ba_timeout_collision;
 } __packed; /* STATISTICS_TX_NON_PHY_API_S_VER_3 */
 
+struct mvm_statistics_tx_non_phy {
+	__le32 bt_prio_defer_cnt;
+	__le32 bt_prio_kill_cnt;
+	__le32 few_bytes_cnt;
+	__le32 cts_timeout;
+	__le32 ack_timeout;
+	__le32 dump_msdu_cnt;
+	__le32 burst_abort_next_frame_mismatch_cnt;
+	__le32 burst_abort_missing_next_frame_cnt;
+	__le32 cts_timeout_collision;
+	__le32 ack_or_ba_timeout_collision;
+} __packed; /* STATISTICS_TX_NON_PHY_API_S_VER_4 */
+
 #define MAX_CHAINS 3
 
 struct mvm_statistics_tx_non_phy_agg {
@@ -202,11 +278,17 @@ struct mvm_statistics_tx_channel_width {
 	__le32 fail_per_ch_width[4];
 }; /* STATISTICS_TX_CHANNEL_WIDTH_API_S_VER_1 */
 
+struct mvm_statistics_tx_v4 {
+	struct mvm_statistics_tx_non_phy_v3 general;
+	struct mvm_statistics_tx_non_phy_agg agg;
+	struct mvm_statistics_tx_channel_width channel_width;
+} __packed; /* STATISTICS_TX_API_S_VER_4 */
+
 struct mvm_statistics_tx {
 	struct mvm_statistics_tx_non_phy general;
 	struct mvm_statistics_tx_non_phy_agg agg;
 	struct mvm_statistics_tx_channel_width channel_width;
-} __packed; /* STATISTICS_TX_API_S_VER_4 */
+} __packed; /* STATISTICS_TX_API_S_VER_5 */
 
 
 struct mvm_statistics_bt_activity {
@@ -220,7 +302,7 @@ struct mvm_statistics_bt_activity {
 	__le32 lo_priority_rx_denied_cnt;
 } __packed;  /* STATISTICS_BT_ACTIVITY_API_S_VER_1 */
 
-struct mvm_statistics_general_common {
+struct mvm_statistics_general_common_v19 {
 	__le32 radio_temperature;
 	__le32 radio_voltage;
 	struct mvm_statistics_dbg dbg;
@@ -250,20 +332,56 @@ struct mvm_statistics_general_common {
 	__le64 tx_time;
 } __packed;
 
+struct mvm_statistics_general_common {
+	__le32 radio_temperature;
+	struct mvm_statistics_dbg dbg;
+	__le32 sleep_time;
+	__le32 slots_out;
+	__le32 slots_idle;
+	__le32 ttl_timestamp;
+	struct mvm_statistics_div slow_div;
+	__le32 rx_enable_counter;
+	/*
+	 * num_of_sos_states:
+	 *  count the number of times we have to re-tune
+	 *  in order to get out of bad PHY status
+	 */
+	__le32 num_of_sos_states;
+	__le32 beacon_filtered;
+	__le32 missed_beacons;
+	u8 beacon_filter_average_energy;
+	u8 beacon_filter_reason;
+	u8 beacon_filter_current_energy;
+	u8 beacon_filter_reserved;
+	__le32 beacon_filter_delta_time;
+	struct mvm_statistics_bt_activity bt_activity;
+	__le64 rx_time;
+	__le64 on_time_rf;
+	__le64 on_time_scan;
+	__le64 tx_time;
+} __packed; /* STATISTICS_GENERAL_API_S_VER_10 */
+
 struct mvm_statistics_general_v8 {
-	struct mvm_statistics_general_common common;
+	struct mvm_statistics_general_common_v19 common;
 	__le32 beacon_counter[NUM_MAC_INDEX];
 	u8 beacon_average_energy[NUM_MAC_INDEX];
 	u8 reserved[4 - (NUM_MAC_INDEX % 4)];
 } __packed; /* STATISTICS_GENERAL_API_S_VER_8 */
 
-struct mvm_statistics_general_cdb {
-	struct mvm_statistics_general_common common;
+struct mvm_statistics_general_cdb_v9 {
+	struct mvm_statistics_general_common_v19 common;
 	__le32 beacon_counter[NUM_MAC_INDEX_CDB];
 	u8 beacon_average_energy[NUM_MAC_INDEX_CDB];
 	u8 reserved[4 - (NUM_MAC_INDEX_CDB % 4)];
 } __packed; /* STATISTICS_GENERAL_API_S_VER_9 */
 
+struct mvm_statistics_general_cdb {
+	struct mvm_statistics_general_common common;
+	__le32 beacon_counter[MAC_INDEX_AUX];
+	u8 beacon_average_energy[MAC_INDEX_AUX];
+	u8 reserved[8 - MAC_INDEX_AUX];
+} __packed; /* STATISTICS_GENERAL_API_S_VER_10 */
+
 /**
  * struct mvm_statistics_load - RX statistics for multi-queue devices
  * @air_time: accumulated air time, per mac
@@ -272,24 +390,31 @@ struct mvm_statistics_general_cdb {
  * @avg_energy: average RSSI, per station
  */
 struct mvm_statistics_load {
+	__le32 air_time[MAC_INDEX_AUX];
+	__le32 byte_count[MAC_INDEX_AUX];
+	__le32 pkt_count[MAC_INDEX_AUX];
+	u8 avg_energy[IWL_MVM_STATION_COUNT];
+} __packed; /* STATISTICS_RX_MAC_STATION_S_VER_3 */
+
+struct mvm_statistics_load_v1 {
 	__le32 air_time[NUM_MAC_INDEX];
 	__le32 byte_count[NUM_MAC_INDEX];
 	__le32 pkt_count[NUM_MAC_INDEX];
 	u8 avg_energy[IWL_MVM_STATION_COUNT];
 } __packed; /* STATISTICS_RX_MAC_STATION_S_VER_1 */
 
-struct mvm_statistics_load_cdb {
-	__le32 air_time[NUM_MAC_INDEX_CDB];
-	__le32 byte_count[NUM_MAC_INDEX_CDB];
-	__le32 pkt_count[NUM_MAC_INDEX_CDB];
-	u8 avg_energy[IWL_MVM_STATION_COUNT];
-} __packed; /* STATISTICS_RX_MAC_STATION_S_VER_2 */
-
 struct mvm_statistics_rx {
 	struct mvm_statistics_rx_phy ofdm;
 	struct mvm_statistics_rx_phy cck;
 	struct mvm_statistics_rx_non_phy general;
 	struct mvm_statistics_rx_ht_phy ofdm_ht;
+} __packed; /* STATISTICS_RX_API_S_VER_4 */
+
+struct mvm_statistics_rx_v3 {
+	struct mvm_statistics_rx_phy_v2 ofdm;
+	struct mvm_statistics_rx_phy_v2 cck;
+	struct mvm_statistics_rx_non_phy_v3 general;
+	struct mvm_statistics_rx_ht_phy_v1 ofdm_ht;
 } __packed; /* STATISTICS_RX_API_S_VER_3 */
 
 /*
@@ -302,17 +427,17 @@ struct mvm_statistics_rx {
 
 struct iwl_notif_statistics_v10 {
 	__le32 flag;
-	struct mvm_statistics_rx rx;
-	struct mvm_statistics_tx tx;
+	struct mvm_statistics_rx_v3 rx;
+	struct mvm_statistics_tx_v4 tx;
 	struct mvm_statistics_general_v8 general;
 } __packed; /* STATISTICS_NTFY_API_S_VER_10 */
 
 struct iwl_notif_statistics_v11 {
 	__le32 flag;
-	struct mvm_statistics_rx rx;
-	struct mvm_statistics_tx tx;
+	struct mvm_statistics_rx_v3 rx;
+	struct mvm_statistics_tx_v4 tx;
 	struct mvm_statistics_general_v8 general;
-	struct mvm_statistics_load load_stats;
+	struct mvm_statistics_load_v1 load_stats;
 } __packed; /* STATISTICS_NTFY_API_S_VER_11 */
 
 struct iwl_notif_statistics_cdb {
@@ -320,12 +445,33 @@ struct iwl_notif_statistics_cdb {
 	struct mvm_statistics_rx rx;
 	struct mvm_statistics_tx tx;
 	struct mvm_statistics_general_cdb general;
-	struct mvm_statistics_load_cdb load_stats;
-} __packed; /* STATISTICS_NTFY_API_S_VER_12 */
+	struct mvm_statistics_load load_stats;
+} __packed; /* STATISTICS_NTFY_API_S_VER_13 */
 
-#define IWL_STATISTICS_FLG_CLEAR		0x1
-#define IWL_STATISTICS_FLG_DISABLE_NOTIF	0x2
+/**
+ * enum iwl_statistics_notif_flags - flags used in statistics notification
+ * @IWL_STATISTICS_REPLY_FLG_CLEAR: statistics were cleared after this report
+ */
+enum iwl_statistics_notif_flags {
+	IWL_STATISTICS_REPLY_FLG_CLEAR		= 0x1,
+};
 
+/**
+ * enum iwl_statistics_cmd_flags - flags used in statistics command
+ * @IWL_STATISTICS_FLG_CLEAR: request to clear statistics after the report
+ *	that's sent after this command
+ * @IWL_STATISTICS_FLG_DISABLE_NOTIF: disable unilateral statistics
+ *	notifications
+ */
+enum iwl_statistics_cmd_flags {
+	IWL_STATISTICS_FLG_CLEAR		= 0x1,
+	IWL_STATISTICS_FLG_DISABLE_NOTIF	= 0x2,
+};
+
+/**
+ * struct iwl_statistics_cmd - statistics config command
+ * @flags: flags from &enum iwl_statistics_cmd_flags
+ */
 struct iwl_statistics_cmd {
 	__le32 flags;
 } __packed; /* STATISTICS_CMD_API_S_VER_1 */
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-tof.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-tof.h
index 86aa51b2210e..8658a983c463 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-tof.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-tof.h
@@ -5,7 +5,7 @@
  *
  * GPL LICENSE SUMMARY
  *
- * Copyright(c) 2015 Intel Deutschland GmbH
+ * Copyright(c) 2015 - 2017 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -30,7 +30,7 @@
  *
  * BSD LICENSE
  *
- * Copyright(c) 2015 Intel Deutschland GmbH
+ * Copyright(c) 2015 - 2017 Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -63,8 +63,6 @@
 #ifndef __fw_api_tof_h__
 #define __fw_api_tof_h__
 
-#include "fw-api.h"
-
 /* ToF sub-group command IDs */
 enum iwl_mvm_tof_sub_grp_ids {
 	TOF_RANGE_REQ_CMD = 0x1,
@@ -118,11 +116,17 @@ struct iwl_tof_config_cmd {
  * @bandwidth: current AP Bandwidth: 0  20MHz, 1  40MHz, 2  80MHz
  * @rate: current AP rate
  * @ctrl_ch_position: coding of the control channel position relative to
- *	     the center frequency.
- *	     40MHz  0 below center, 1 above center
- *	     80MHz  bits [0..1]: 0  the near 20MHz to the center,
- *				 1  the far  20MHz to the center
- *		    bit[2]  as above 40MHz
+ *	the center frequency:
+ *
+ *	40 MHz
+ *		0 below center, 1 above center
+ *
+ *	80 MHz
+ *		bits [0..1]
+ *		 * 0  the near 20MHz to the center,
+ *		 * 1  the far  20MHz to the center
+ *		bit[2]
+ *		 as above 40MHz
  * @ftm_per_burst: FTMs per Burst
  * @ftm_resp_ts_avail: '0' - we don't measure over the Initial FTM Response,
  *		  '1' - we measure over the Initial FTM Response
@@ -159,6 +163,7 @@ struct iwl_tof_responder_config_cmd {
 /**
  * struct iwl_tof_range_request_ext_cmd - extended range req for WLS
  * @tsf_timer_offset_msec: the recommended time offset (mSec) from the AP's TSF
+ * @reserved: reserved
  * @min_delta_ftm: Minimal time between two consecutive measurements,
  *		   in units of 100us. 0 means no preference by station
  * @ftm_format_and_bw20M: FTM Channel Spacing/Format for 20MHz: recommended
@@ -268,6 +273,7 @@ enum iwl_tof_response_mode {
  *	            '1' Use MAC Address randomization according to the below
  * @macaddr_mask: Bits set to 0 shall be copied from the MAC address template.
  *		  Bits set to 1 shall be randomized by the UMAC
+ * @ap: per-AP request data
  */
 struct iwl_tof_range_req_cmd {
 	__le32 sub_grp_cmd_id;
@@ -294,7 +300,9 @@ struct iwl_tof_gen_resp_cmd {
 
 /**
  * struct iwl_tof_range_rsp_ap_entry_ntfy - AP parameters (response)
- * @measure_status: current APs measurement status
+ * @bssid: BSSID of the AP
+ * @measure_status: current APs measurement status, one of
+ *	&enum iwl_tof_entry_status.
  * @measure_bw: Current AP Bandwidth: 0  20MHz, 1  40MHz, 2  80MHz
  * @rtt: The Round Trip Time that took for the last measurement for
  *	 current AP [nSec]
@@ -304,6 +312,7 @@ struct iwl_tof_gen_resp_cmd {
  * @rssi: RSSI as uploaded in the Channel Estimation notification
  * @rssi_spread: The Difference between the maximum and the minimum RSSI values
  *	        measured for current AP in the current session
+ * @reserved: reserved
  * @range: Measured range [cm]
  * @range_variance: Measured range variance [cm]
  * @timestamp: The GP2 Clock [usec] where Channel Estimation notification was
@@ -330,6 +339,7 @@ struct iwl_tof_range_rsp_ap_entry_ntfy {
  * @request_status: status of current measurement session
  * @last_in_batch: reprot policy (when not all responses are uploaded at once)
  * @num_of_aps: Number of APs to measure (error if > IWL_MVM_TOF_MAX_APS)
+ * @ap: per-AP data
  */
 struct iwl_tof_range_rsp_ntfy {
 	u8 request_id;
@@ -344,6 +354,7 @@ struct iwl_tof_range_rsp_ntfy {
  * struct iwl_tof_mcsi_notif - used for debug
  * @token: token ID for the current session
  * @role: '0' - initiator, '1' - responder
+ * @reserved: reserved
  * @initiator_bssid: initiator machine
  * @responder_bssid: responder machine
  * @mcsi_buffer: debug data
@@ -376,6 +387,7 @@ struct iwl_tof_neighbor_report {
 /**
  * struct iwl_tof_range_abort_cmd
  * @request_id: corresponds to a range request
+ * @reserved: reserved
  */
 struct iwl_tof_range_abort_cmd {
 	__le32 sub_grp_cmd_id;
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-tx.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-tx.h
index 1360ebfdc51b..97d7eed32622 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-tx.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-tx.h
@@ -74,6 +74,7 @@
  *	Otherwise, use rate_n_flags from the TX command
  * @TX_CMD_FLG_BAR: this frame is a BA request, immediate BAR is expected
  *	Must set TX_CMD_FLG_ACK with this flag.
+ * @TX_CMD_FLG_TXOP_PROT: TXOP protection requested
  * @TX_CMD_FLG_VHT_NDPA: mark frame is NDPA for VHT beamformer sequence
  * @TX_CMD_FLG_HT_NDPA: mark frame is NDPA for HT beamformer sequence
  * @TX_CMD_FLG_CSI_FDBK2HOST: mark to send feedback to host (only if good CRC)
@@ -177,29 +178,6 @@ enum iwl_tx_cmd_sec_ctrl {
 	TX_CMD_SEC_KEY_FROM_TABLE	= 0x10,
 };
 
-/* TODO: how does these values are OK with only 16 bit variable??? */
-/*
- * TX command next frame info
- *
- * bits 0:2 - security control (TX_CMD_SEC_*)
- * bit 3 - immediate ACK required
- * bit 4 - rate is taken from STA table
- * bit 5 - frame belongs to BA stream
- * bit 6 - immediate BA response expected
- * bit 7 - unused
- * bits 8:15 - Station ID
- * bits 16:31 - rate
- */
-#define TX_CMD_NEXT_FRAME_ACK_MSK		(0x8)
-#define TX_CMD_NEXT_FRAME_STA_RATE_MSK		(0x10)
-#define TX_CMD_NEXT_FRAME_BA_MSK		(0x20)
-#define TX_CMD_NEXT_FRAME_IMM_BA_RSP_MSK	(0x40)
-#define TX_CMD_NEXT_FRAME_FLAGS_MSK		(0xf8)
-#define TX_CMD_NEXT_FRAME_STA_ID_MSK		(0xff00)
-#define TX_CMD_NEXT_FRAME_STA_ID_POS		(8)
-#define TX_CMD_NEXT_FRAME_RATE_MSK		(0xffff0000)
-#define TX_CMD_NEXT_FRAME_RATE_POS		(16)
-
 /*
  * TX command Frame life time in us - to be written in pm_frame_timeout
  */
@@ -224,7 +202,7 @@ enum iwl_tx_cmd_sec_ctrl {
 
 /**
  * enum iwl_tx_offload_assist_flags_pos -  set %iwl_tx_cmd offload_assist values
- * @TX_CMD_OFFLD_IP_HDR_OFFSET: offset to start of IP header (in words)
+ * @TX_CMD_OFFLD_IP_HDR: offset to start of IP header (in words)
  *	from mac header end. For normal case it is 4 words for SNAP.
  *	note: tx_cmd, mac header and pad are not counted in the offset.
  *	This is used to help the offload in case there is tunneling such as
@@ -258,22 +236,27 @@ enum iwl_tx_offload_assist_flags_pos {
  * @len: in bytes of the payload, see below for details
  * @offload_assist: TX offload configuration
  * @tx_flags: combination of TX_CMD_FLG_*
+ * @scratch: scratch buffer used by the device
  * @rate_n_flags: rate for *all* Tx attempts, if TX_CMD_FLG_STA_RATE_MSK is
  *	cleared. Combination of RATE_MCS_*
  * @sta_id: index of destination station in FW station table
  * @sec_ctl: security control, TX_CMD_SEC_*
  * @initial_rate_index: index into the the rate table for initial TX attempt.
  *	Applied if TX_CMD_FLG_STA_RATE_MSK is set, normally 0 for data frames.
+ * @reserved2: reserved
  * @key: security key
- * @next_frame_flags: TX_CMD_SEC_* and TX_CMD_NEXT_FRAME_*
+ * @reserved3: reserved
  * @life_time: frame life time (usecs??)
  * @dram_lsb_ptr: Physical address of scratch area in the command (try_cnt +
  *	btkill_cnd + reserved), first 32 bits. "0" disables usage.
  * @dram_msb_ptr: upper bits of the scratch physical address
  * @rts_retry_limit: max attempts for RTS
  * @data_retry_limit: max attempts to send the data packet
- * @tid_spec: TID/tspec
+ * @tid_tspec: TID/tspec
  * @pm_frame_timeout: PM TX frame timeout
+ * @reserved4: reserved
+ * @payload: payload (same as @hdr)
+ * @hdr: 802.11 header (same as @payload)
  *
  * The byte count (both len and next_frame_len) includes MAC header
  * (24/26/30/32 bytes)
@@ -327,10 +310,11 @@ struct iwl_dram_sec_info {
  * ( TX_CMD = 0x1c )
  * @len: in bytes of the payload, see below for details
  * @offload_assist: TX offload configuration
- * @tx_flags: combination of &iwl_tx_cmd_flags
+ * @flags: combination of &enum iwl_tx_cmd_flags
  * @dram_info: FW internal DRAM storage
  * @rate_n_flags: rate for *all* Tx attempts, if TX_CMD_FLG_STA_RATE_MSK is
  *	cleared. Combination of RATE_MCS_*
+ * @hdr: 802.11 header
  */
 struct iwl_tx_cmd_gen2 {
 	__le16 len;
@@ -504,7 +488,7 @@ enum iwl_tx_agg_status {
 
 /**
  * struct agg_tx_status - per packet TX aggregation status
- * @status: enum iwl_tx_agg_status
+ * @status: See &enum iwl_tx_agg_status
  * @sequence: Sequence # for this frame's Tx cmd (not SSN!)
  */
 struct agg_tx_status {
@@ -529,6 +513,63 @@ struct agg_tx_status {
 #define IWL_MVM_TX_RES_GET_RA(_ra_tid) ((_ra_tid) >> 4)
 
 /**
+ * struct iwl_mvm_tx_resp_v3 - notifies that fw is TXing a packet
+ * ( REPLY_TX = 0x1c )
+ * @frame_count: 1 no aggregation, >1 aggregation
+ * @bt_kill_count: num of times blocked by bluetooth (unused for agg)
+ * @failure_rts: num of failures due to unsuccessful RTS
+ * @failure_frame: num failures due to no ACK (unused for agg)
+ * @initial_rate: for non-agg: rate of the successful Tx. For agg: rate of the
+ *	Tx of all the batch. RATE_MCS_*
+ * @wireless_media_time: for non-agg: RTS + CTS + frame tx attempts time + ACK.
+ *	for agg: RTS + CTS + aggregation tx time + block-ack time.
+ *	in usec.
+ * @pa_status: tx power info
+ * @pa_integ_res_a: tx power info
+ * @pa_integ_res_b: tx power info
+ * @pa_integ_res_c: tx power info
+ * @measurement_req_id: tx power info
+ * @reduced_tpc: transmit power reduction used
+ * @reserved: reserved
+ * @tfd_info: TFD information set by the FH
+ * @seq_ctl: sequence control from the Tx cmd
+ * @byte_cnt: byte count from the Tx cmd
+ * @tlc_info: TLC rate info
+ * @ra_tid: bits [3:0] = ra, bits [7:4] = tid
+ * @frame_ctrl: frame control
+ * @status: for non-agg:  frame status TX_STATUS_*
+ *	for agg: status of 1st frame, AGG_TX_STATE_*; other frame status fields
+ *	follow this one, up to frame_count. Length in @frame_count.
+ *
+ * After the array of statuses comes the SSN of the SCD. Look at
+ * %iwl_mvm_get_scd_ssn for more details.
+ */
+struct iwl_mvm_tx_resp_v3 {
+	u8 frame_count;
+	u8 bt_kill_count;
+	u8 failure_rts;
+	u8 failure_frame;
+	__le32 initial_rate;
+	__le16 wireless_media_time;
+
+	u8 pa_status;
+	u8 pa_integ_res_a[3];
+	u8 pa_integ_res_b[3];
+	u8 pa_integ_res_c[3];
+	__le16 measurement_req_id;
+	u8 reduced_tpc;
+	u8 reserved;
+
+	__le32 tfd_info;
+	__le16 seq_ctl;
+	__le16 byte_cnt;
+	u8 tlc_info;
+	u8 ra_tid;
+	__le16 frame_ctrl;
+	struct agg_tx_status status[];
+} __packed; /* TX_RSP_API_S_VER_3 */
+
+/**
  * struct iwl_mvm_tx_resp - notifies that fw is TXing a packet
  * ( REPLY_TX = 0x1c )
  * @frame_count: 1 no aggregation, >1 aggregation
@@ -545,6 +586,8 @@ struct agg_tx_status {
  * @pa_integ_res_b: tx power info
  * @pa_integ_res_c: tx power info
  * @measurement_req_id: tx power info
+ * @reduced_tpc: transmit power reduction used
+ * @reserved: reserved
  * @tfd_info: TFD information set by the FH
  * @seq_ctl: sequence control from the Tx cmd
  * @byte_cnt: byte count from the Tx cmd
@@ -552,9 +595,8 @@ struct agg_tx_status {
  * @ra_tid: bits [3:0] = ra, bits [7:4] = tid
  * @frame_ctrl: frame control
  * @tx_queue: TX queue for this response
+ * @reserved2: reserved for padding/alignment
  * @status: for non-agg:  frame status TX_STATUS_*
- *	for agg: status of 1st frame, AGG_TX_STATE_*; other frame status fields
- *	follow this one, up to frame_count.
  *	For version 6 TX response isn't received for aggregation at all.
  *
  * After the array of statuses comes the SSN of the SCD. Look at
@@ -582,26 +624,19 @@ struct iwl_mvm_tx_resp {
 	u8 tlc_info;
 	u8 ra_tid;
 	__le16 frame_ctrl;
-	union {
-		struct {
-			struct agg_tx_status status;
-		} v3;/* TX_RSP_API_S_VER_3 */
-		struct {
-			__le16 tx_queue;
-			__le16 reserved2;
-			struct agg_tx_status status;
-		} v6;
-	};
+	__le16 tx_queue;
+	__le16 reserved2;
+	struct agg_tx_status status;
 } __packed; /* TX_RSP_API_S_VER_6 */
 
 /**
  * struct iwl_mvm_ba_notif - notifies about reception of BA
  * ( BA_NOTIF = 0xc5 )
- * @sta_addr_lo32: lower 32 bits of the MAC address
- * @sta_addr_hi16: upper 16 bits of the MAC address
+ * @sta_addr: MAC address
+ * @reserved: reserved
  * @sta_id: Index of recipient (BA-sending) station in fw's station table
  * @tid: tid of the session
- * @seq_ctl:
+ * @seq_ctl: sequence control field
  * @bitmap: the bitmap of the BA notification as seen in the air
  * @scd_flow: the tx queue this BA relates to
  * @scd_ssn: the index of the last contiguously sent packet
@@ -610,10 +645,10 @@ struct iwl_mvm_tx_resp {
  * @reduced_txp: power reduced according to TPC. This is the actual value and
  *	not a copy from the LQ command. Thus, if not the first rate was used
  *	for Tx-ing then this value will be set to 0 by FW.
+ * @reserved1: reserved
  */
 struct iwl_mvm_ba_notif {
-	__le32 sta_addr_lo32;
-	__le16 sta_addr_hi16;
+	u8 sta_addr[ETH_ALEN];
 	__le16 reserved;
 
 	u8 sta_id;
@@ -633,13 +668,15 @@ struct iwl_mvm_ba_notif {
  * @q_num: TFD queue number
  * @tfd_index: Index of first un-acked frame in the  TFD queue
  * @scd_queue: For debug only - the physical queue the TFD queue is bound to
+ * @tid: TID of the queue (0-7)
+ * @reserved: reserved for alignment
  */
 struct iwl_mvm_compressed_ba_tfd {
 	__le16 q_num;
 	__le16 tfd_index;
 	u8 scd_queue;
-	u8 reserved;
-	__le16 reserved2;
+	u8 tid;
+	u8 reserved[2];
 } __packed; /* COMPRESSED_BA_TFD_API_S_VER_1 */
 
 /**
@@ -687,11 +724,12 @@ enum iwl_mvm_ba_resp_flags {
  * @query_frame_cnt: SCD query frame count
  * @txed: number of frames sent in the aggregation (all-TIDs)
  * @done: number of frames that were Acked by the BA (all-TIDs)
+ * @reserved: reserved (for alignment)
  * @wireless_time: Wireless-media time
  * @tx_rate: the rate the aggregation was sent at
  * @tfd_cnt: number of TFD-Q elements
  * @ra_tid_cnt: number of RATID-Q elements
- * @ba_tfd: array of TFD queue status updates. See &iwl_mvm_compressed_ba_tfd
+ * @tfd: array of TFD queue status updates. See &iwl_mvm_compressed_ba_tfd
  *	for details.
  * @ra_tid: array of RA-TID queue status updates. For debug purposes only. See
  *	&iwl_mvm_compressed_ba_ratid for more details.
@@ -765,6 +803,7 @@ struct iwl_mac_beacon_cmd_v7 {
  * struct iwl_mac_beacon_cmd - beacon template command with offloaded CSA
  * @byte_cnt: byte count of the beacon frame
  * @flags: for future use
+ * @reserved: reserved
  * @data: see &iwl_mac_beacon_cmd_data
  */
 struct iwl_mac_beacon_cmd {
@@ -809,12 +848,24 @@ enum iwl_dump_control {
  * @flush_ctl: control flags
  * @reserved: reserved
  */
-struct iwl_tx_path_flush_cmd {
+struct iwl_tx_path_flush_cmd_v1 {
 	__le32 queues_ctl;
 	__le16 flush_ctl;
 	__le16 reserved;
 } __packed; /* TX_PATH_FLUSH_CMD_API_S_VER_1 */
 
+/**
+ * struct iwl_tx_path_flush_cmd -- queue/FIFO flush command
+ * @sta_id: station ID to flush
+ * @tid_mask: TID mask to flush
+ * @reserved: reserved
+ */
+struct iwl_tx_path_flush_cmd {
+	__le32 sta_id;
+	__le16 tid_mask;
+	__le16 reserved;
+} __packed; /* TX_PATH_FLUSH_CMD_API_S_VER_2 */
+
 /* Available options for the SCD_QUEUE_CFG HCMD */
 enum iwl_scd_cfg_actions {
 	SCD_CFG_DISABLE_QUEUE		= 0x0,
@@ -824,16 +875,17 @@ enum iwl_scd_cfg_actions {
 
 /**
  * struct iwl_scd_txq_cfg_cmd - New txq hw scheduler config command
- * @token:
+ * @token: unused
  * @sta_id: station id
- * @tid:
+ * @tid: TID
  * @scd_queue: scheduler queue to confiug
  * @action: 1 queue enable, 0 queue disable, 2 change txq's tid owner
- *	Value is one of %iwl_scd_cfg_actions options
+ *	Value is one of &enum iwl_scd_cfg_actions options
  * @aggregate: 1 aggregated queue, 0 otherwise
- * @tx_fifo: %enum iwl_mvm_tx_fifo
+ * @tx_fifo: &enum iwl_mvm_tx_fifo
  * @window: BA window size
  * @ssn: SSN for the BA agreement
+ * @reserved: reserved
  */
 struct iwl_scd_txq_cfg_cmd {
 	u8 token;
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api.h
index f545c5f9e4e3..aad265dcfaf5 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-api.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api.h
@@ -130,181 +130,547 @@ enum iwl_mvm_tx_fifo {
 };
 
 
-/* commands */
-enum {
+/**
+ * enum iwl_legacy_cmds - legacy group command IDs
+ */
+enum iwl_legacy_cmds {
+	/**
+	 * @MVM_ALIVE:
+	 * Alive data from the firmware, as described in
+	 * &struct mvm_alive_resp_v3 or &struct mvm_alive_resp.
+	 */
 	MVM_ALIVE = 0x1,
+
+	/**
+	 * @REPLY_ERROR: Cause an error in the firmware, for testing purposes.
+	 */
 	REPLY_ERROR = 0x2,
+
+	/**
+	 * @ECHO_CMD: Send data to the device to have it returned immediately.
+	 */
 	ECHO_CMD = 0x3,
 
+	/**
+	 * @INIT_COMPLETE_NOTIF: Notification that initialization is complete.
+	 */
 	INIT_COMPLETE_NOTIF = 0x4,
 
-	/* PHY context commands */
+	/**
+	 * @PHY_CONTEXT_CMD:
+	 * Add/modify/remove a PHY context, using &struct iwl_phy_context_cmd.
+	 */
 	PHY_CONTEXT_CMD = 0x8,
+
+	/**
+	 * @DBG_CFG: Debug configuration command.
+	 */
 	DBG_CFG = 0x9,
+
+	/**
+	 * @ANTENNA_COUPLING_NOTIFICATION:
+	 * Antenna coupling data, &struct iwl_mvm_antenna_coupling_notif
+	 */
 	ANTENNA_COUPLING_NOTIFICATION = 0xa,
 
-	/* UMAC scan commands */
+	/**
+	 * @SCAN_ITERATION_COMPLETE_UMAC:
+	 * Firmware indicates a scan iteration completed, using
+	 * &struct iwl_umac_scan_iter_complete_notif.
+	 */
 	SCAN_ITERATION_COMPLETE_UMAC = 0xb5,
+
+	/**
+	 * @SCAN_CFG_CMD:
+	 * uses &struct iwl_scan_config_v1 or &struct iwl_scan_config
+	 */
 	SCAN_CFG_CMD = 0xc,
+
+	/**
+	 * @SCAN_REQ_UMAC: uses &struct iwl_scan_req_umac
+	 */
 	SCAN_REQ_UMAC = 0xd,
+
+	/**
+	 * @SCAN_ABORT_UMAC: uses &struct iwl_umac_scan_abort
+	 */
 	SCAN_ABORT_UMAC = 0xe,
+
+	/**
+	 * @SCAN_COMPLETE_UMAC: uses &struct iwl_umac_scan_complete
+	 */
 	SCAN_COMPLETE_UMAC = 0xf,
 
+	/**
+	 * @BA_WINDOW_STATUS_NOTIFICATION_ID:
+	 * uses &struct iwl_ba_window_status_notif
+	 */
 	BA_WINDOW_STATUS_NOTIFICATION_ID = 0x13,
 
-	/* station table */
+	/**
+	 * @ADD_STA_KEY:
+	 * &struct iwl_mvm_add_sta_key_cmd_v1 or
+	 * &struct iwl_mvm_add_sta_key_cmd.
+	 */
 	ADD_STA_KEY = 0x17,
+
+	/**
+	 * @ADD_STA:
+	 * &struct iwl_mvm_add_sta_cmd or &struct iwl_mvm_add_sta_cmd_v7.
+	 */
 	ADD_STA = 0x18,
+
+	/**
+	 * @REMOVE_STA: &struct iwl_mvm_rm_sta_cmd
+	 */
 	REMOVE_STA = 0x19,
 
-	/* paging get item */
+	/**
+	 * @FW_GET_ITEM_CMD: uses &struct iwl_fw_get_item_cmd
+	 */
 	FW_GET_ITEM_CMD = 0x1a,
 
-	/* TX */
+	/**
+	 * @TX_CMD: uses &struct iwl_tx_cmd or &struct iwl_tx_cmd_gen2,
+	 *	response in &struct iwl_mvm_tx_resp or
+	 *	&struct iwl_mvm_tx_resp_v3
+	 */
 	TX_CMD = 0x1c,
+
+	/**
+	 * @TXPATH_FLUSH: &struct iwl_tx_path_flush_cmd
+	 */
 	TXPATH_FLUSH = 0x1e,
+
+	/**
+	 * @MGMT_MCAST_KEY:
+	 * &struct iwl_mvm_mgmt_mcast_key_cmd or
+	 * &struct iwl_mvm_mgmt_mcast_key_cmd_v1
+	 */
 	MGMT_MCAST_KEY = 0x1f,
 
 	/* scheduler config */
+	/**
+	 * @SCD_QUEUE_CFG: &struct iwl_scd_txq_cfg_cmd for older hardware,
+	 *	&struct iwl_tx_queue_cfg_cmd with &struct iwl_tx_queue_cfg_rsp
+	 *	for newer (A000) hardware.
+	 */
 	SCD_QUEUE_CFG = 0x1d,
 
-	/* global key */
+	/**
+	 * @WEP_KEY: uses &struct iwl_mvm_wep_key_cmd
+	 */
 	WEP_KEY = 0x20,
 
-	/* Memory */
+	/**
+	 * @SHARED_MEM_CFG:
+	 * retrieve shared memory configuration - response in
+	 * &struct iwl_shared_mem_cfg
+	 */
 	SHARED_MEM_CFG = 0x25,
 
-	/* TDLS */
+	/**
+	 * @TDLS_CHANNEL_SWITCH_CMD: uses &struct iwl_tdls_channel_switch_cmd
+	 */
 	TDLS_CHANNEL_SWITCH_CMD = 0x27,
+
+	/**
+	 * @TDLS_CHANNEL_SWITCH_NOTIFICATION:
+	 * uses &struct iwl_tdls_channel_switch_notif
+	 */
 	TDLS_CHANNEL_SWITCH_NOTIFICATION = 0xaa,
+
+	/**
+	 * @TDLS_CONFIG_CMD:
+	 * &struct iwl_tdls_config_cmd, response in &struct iwl_tdls_config_res
+	 */
 	TDLS_CONFIG_CMD = 0xa7,
 
-	/* MAC and Binding commands */
+	/**
+	 * @MAC_CONTEXT_CMD: &struct iwl_mac_ctx_cmd
+	 */
 	MAC_CONTEXT_CMD = 0x28,
+
+	/**
+	 * @TIME_EVENT_CMD:
+	 * &struct iwl_time_event_cmd, response in &struct iwl_time_event_resp
+	 */
 	TIME_EVENT_CMD = 0x29, /* both CMD and response */
+
+	/**
+	 * @TIME_EVENT_NOTIFICATION: &struct iwl_time_event_notif
+	 */
 	TIME_EVENT_NOTIFICATION = 0x2a,
+
+	/**
+	 * @BINDING_CONTEXT_CMD:
+	 * &struct iwl_binding_cmd or &struct iwl_binding_cmd_v1
+	 */
 	BINDING_CONTEXT_CMD = 0x2b,
+
+	/**
+	 * @TIME_QUOTA_CMD: &struct iwl_time_quota_cmd
+	 */
 	TIME_QUOTA_CMD = 0x2c,
+
+	/**
+	 * @NON_QOS_TX_COUNTER_CMD:
+	 * command is &struct iwl_nonqos_seq_query_cmd
+	 */
 	NON_QOS_TX_COUNTER_CMD = 0x2d,
 
+	/**
+	 * @LQ_CMD: using &struct iwl_lq_cmd
+	 */
 	LQ_CMD = 0x4e,
 
-	/* paging block to FW cpu2 */
+	/**
+	 * @FW_PAGING_BLOCK_CMD:
+	 * &struct iwl_fw_paging_cmd
+	 */
 	FW_PAGING_BLOCK_CMD = 0x4f,
 
-	/* Scan offload */
+	/**
+	 * @SCAN_OFFLOAD_REQUEST_CMD: uses &struct iwl_scan_req_lmac
+	 */
 	SCAN_OFFLOAD_REQUEST_CMD = 0x51,
+
+	/**
+	 * @SCAN_OFFLOAD_ABORT_CMD: abort the scan - no further contents
+	 */
 	SCAN_OFFLOAD_ABORT_CMD = 0x52,
+
+	/**
+	 * @HOT_SPOT_CMD: uses &struct iwl_hs20_roc_req
+	 */
 	HOT_SPOT_CMD = 0x53,
+
+	/**
+	 * @SCAN_OFFLOAD_COMPLETE:
+	 * notification, &struct iwl_periodic_scan_complete
+	 */
 	SCAN_OFFLOAD_COMPLETE = 0x6D,
+
+	/**
+	 * @SCAN_OFFLOAD_UPDATE_PROFILES_CMD:
+	 * update scan offload (scheduled scan) profiles/blacklist/etc.
+	 */
 	SCAN_OFFLOAD_UPDATE_PROFILES_CMD = 0x6E,
-	SCAN_OFFLOAD_CONFIG_CMD = 0x6f,
+
+	/**
+	 * @MATCH_FOUND_NOTIFICATION: scan match found
+	 */
 	MATCH_FOUND_NOTIFICATION = 0xd9,
+
+	/**
+	 * @SCAN_ITERATION_COMPLETE:
+	 * uses &struct iwl_lmac_scan_complete_notif
+	 */
 	SCAN_ITERATION_COMPLETE = 0xe7,
 
 	/* Phy */
+	/**
+	 * @PHY_CONFIGURATION_CMD: &struct iwl_phy_cfg_cmd
+	 */
 	PHY_CONFIGURATION_CMD = 0x6a,
+
+	/**
+	 * @CALIB_RES_NOTIF_PHY_DB: &struct iwl_calib_res_notif_phy_db
+	 */
 	CALIB_RES_NOTIF_PHY_DB = 0x6b,
+
+	/**
+	 * @PHY_DB_CMD: &struct iwl_phy_db_cmd
+	 */
 	PHY_DB_CMD = 0x6c,
 
-	/* ToF - 802.11mc FTM */
+	/**
+	 * @TOF_CMD: &struct iwl_tof_config_cmd
+	 */
 	TOF_CMD = 0x10,
+
+	/**
+	 * @TOF_NOTIFICATION: &struct iwl_tof_gen_resp_cmd
+	 */
 	TOF_NOTIFICATION = 0x11,
 
-	/* Power - legacy power table command */
+	/**
+	 * @POWER_TABLE_CMD: &struct iwl_device_power_cmd
+	 */
 	POWER_TABLE_CMD = 0x77,
+
+	/**
+	 * @PSM_UAPSD_AP_MISBEHAVING_NOTIFICATION:
+	 * &struct iwl_uapsd_misbehaving_ap_notif
+	 */
 	PSM_UAPSD_AP_MISBEHAVING_NOTIFICATION = 0x78,
+
+	/**
+	 * @LTR_CONFIG: &struct iwl_ltr_config_cmd
+	 */
 	LTR_CONFIG = 0xee,
 
-	/* Thermal Throttling*/
+	/**
+	 * @REPLY_THERMAL_MNG_BACKOFF:
+	 * Thermal throttling command
+	 */
 	REPLY_THERMAL_MNG_BACKOFF = 0x7e,
 
-	/* Set/Get DC2DC frequency tune */
+	/**
+	 * @DC2DC_CONFIG_CMD:
+	 * Set/Get DC2DC frequency tune
+	 * Command is &struct iwl_dc2dc_config_cmd,
+	 * response is &struct iwl_dc2dc_config_resp
+	 */
 	DC2DC_CONFIG_CMD = 0x83,
 
-	/* NVM */
+	/**
+	 * @NVM_ACCESS_CMD: using &struct iwl_nvm_access_cmd
+	 */
 	NVM_ACCESS_CMD = 0x88,
 
-	SET_CALIB_DEFAULT_CMD = 0x8e,
-
+	/**
+	 * @BEACON_NOTIFICATION: &struct iwl_extended_beacon_notif
+	 */
 	BEACON_NOTIFICATION = 0x90,
+
+	/**
+	 * @BEACON_TEMPLATE_CMD:
+	 *	Uses one of &struct iwl_mac_beacon_cmd_v6,
+	 *	&struct iwl_mac_beacon_cmd_v7 or &struct iwl_mac_beacon_cmd
+	 *	depending on the device version.
+	 */
 	BEACON_TEMPLATE_CMD = 0x91,
+	/**
+	 * @TX_ANT_CONFIGURATION_CMD: &struct iwl_tx_ant_cfg_cmd
+	 */
 	TX_ANT_CONFIGURATION_CMD = 0x98,
+
+	/**
+	 * @STATISTICS_CMD:
+	 * one of &struct iwl_statistics_cmd,
+	 * &struct iwl_notif_statistics_v11,
+	 * &struct iwl_notif_statistics_v10,
+	 * &struct iwl_notif_statistics_cdb
+	 */
 	STATISTICS_CMD = 0x9c,
+
+	/**
+	 * @STATISTICS_NOTIFICATION:
+	 * one of &struct iwl_notif_statistics_v10,
+	 * &struct iwl_notif_statistics_v11,
+	 * &struct iwl_notif_statistics_cdb
+	 */
 	STATISTICS_NOTIFICATION = 0x9d,
+
+	/**
+	 * @EOSP_NOTIFICATION:
+	 * Notify that a service period ended,
+	 * &struct iwl_mvm_eosp_notification
+	 */
 	EOSP_NOTIFICATION = 0x9e,
+
+	/**
+	 * @REDUCE_TX_POWER_CMD:
+	 * &struct iwl_dev_tx_power_cmd_v3 or &struct iwl_dev_tx_power_cmd
+	 */
 	REDUCE_TX_POWER_CMD = 0x9f,
 
-	/* RF-KILL commands and notifications */
-	CARD_STATE_CMD = 0xa0,
+	/**
+	 * @CARD_STATE_NOTIFICATION:
+	 * Card state (RF/CT kill) notification,
+	 * uses &struct iwl_card_state_notif
+	 */
 	CARD_STATE_NOTIFICATION = 0xa1,
 
+	/**
+	 * @MISSED_BEACONS_NOTIFICATION: &struct iwl_missed_beacons_notif
+	 */
 	MISSED_BEACONS_NOTIFICATION = 0xa2,
 
-	/* Power - new power table command */
+	/**
+	 * @MAC_PM_POWER_TABLE: using &struct iwl_mac_power_cmd
+	 */
 	MAC_PM_POWER_TABLE = 0xa9,
 
+	/**
+	 * @MFUART_LOAD_NOTIFICATION: &struct iwl_mfuart_load_notif
+	 */
 	MFUART_LOAD_NOTIFICATION = 0xb1,
 
+	/**
+	 * @RSS_CONFIG_CMD: &struct iwl_rss_config_cmd
+	 */
 	RSS_CONFIG_CMD = 0xb3,
 
+	/**
+	 * @REPLY_RX_PHY_CMD: &struct iwl_rx_phy_info
+	 */
 	REPLY_RX_PHY_CMD = 0xc0,
+
+	/**
+	 * @REPLY_RX_MPDU_CMD:
+	 * &struct iwl_rx_mpdu_res_start or &struct iwl_rx_mpdu_desc
+	 */
 	REPLY_RX_MPDU_CMD = 0xc1,
+
+	/**
+	 * @FRAME_RELEASE:
+	 * Frame release (reorder helper) notification, uses
+	 * &struct iwl_frame_release
+	 */
 	FRAME_RELEASE = 0xc3,
+
+	/**
+	 * @BA_NOTIF:
+	 * BlockAck notification, uses &struct iwl_mvm_compressed_ba_notif
+	 * or &struct iwl_mvm_ba_notif depending on the HW
+	 */
 	BA_NOTIF = 0xc5,
 
 	/* Location Aware Regulatory */
+	/**
+	 * @MCC_UPDATE_CMD: using &struct iwl_mcc_update_cmd
+	 */
 	MCC_UPDATE_CMD = 0xc8,
+
+	/**
+	 * @MCC_CHUB_UPDATE_CMD: using &struct iwl_mcc_chub_notif
+	 */
 	MCC_CHUB_UPDATE_CMD = 0xc9,
 
+	/**
+	 * @MARKER_CMD: trace marker command, uses &struct iwl_mvm_marker
+	 */
 	MARKER_CMD = 0xcb,
 
-	/* BT Coex */
-	BT_COEX_PRIO_TABLE = 0xcc,
-	BT_COEX_PROT_ENV = 0xcd,
+	/**
+	 * @BT_PROFILE_NOTIFICATION: &struct iwl_bt_coex_profile_notif
+	 */
 	BT_PROFILE_NOTIFICATION = 0xce,
+
+	/**
+	 * @BT_CONFIG: &struct iwl_bt_coex_cmd
+	 */
 	BT_CONFIG = 0x9b,
-	BT_COEX_UPDATE_SW_BOOST = 0x5a,
+
+	/**
+	 * @BT_COEX_UPDATE_CORUN_LUT:
+	 * &struct iwl_bt_coex_corun_lut_update_cmd
+	 */
 	BT_COEX_UPDATE_CORUN_LUT = 0x5b,
+
+	/**
+	 * @BT_COEX_UPDATE_REDUCED_TXP:
+	 * &struct iwl_bt_coex_reduced_txp_update_cmd
+	 */
 	BT_COEX_UPDATE_REDUCED_TXP = 0x5c,
+
+	/**
+	 * @BT_COEX_CI: &struct iwl_bt_coex_ci_cmd
+	 */
 	BT_COEX_CI = 0x5d,
 
+	/**
+	 * @REPLY_SF_CFG_CMD: &struct iwl_sf_cfg_cmd
+	 */
 	REPLY_SF_CFG_CMD = 0xd1,
+	/**
+	 * @REPLY_BEACON_FILTERING_CMD: &struct iwl_beacon_filter_cmd
+	 */
 	REPLY_BEACON_FILTERING_CMD = 0xd2,
 
-	/* DTS measurements */
-	CMD_DTS_MEASUREMENT_TRIGGER = 0xdc,
+	/**
+	 * @DTS_MEASUREMENT_NOTIFICATION:
+	 * &struct iwl_dts_measurement_notif_v1 or
+	 * &struct iwl_dts_measurement_notif_v2
+	 */
 	DTS_MEASUREMENT_NOTIFICATION = 0xdd,
 
-	REPLY_DEBUG_CMD = 0xf0,
+	/**
+	 * @LDBG_CONFIG_CMD: configure continuous trace recording
+	 */
 	LDBG_CONFIG_CMD = 0xf6,
+
+	/**
+	 * @DEBUG_LOG_MSG: Debugging log data from firmware
+	 */
 	DEBUG_LOG_MSG = 0xf7,
 
+	/**
+	 * @BCAST_FILTER_CMD: &struct iwl_bcast_filter_cmd
+	 */
 	BCAST_FILTER_CMD = 0xcf,
+
+	/**
+	 * @MCAST_FILTER_CMD: &struct iwl_mcast_filter_cmd
+	 */
 	MCAST_FILTER_CMD = 0xd0,
 
-	/* D3 commands/notifications */
+	/**
+	 * @D3_CONFIG_CMD: &struct iwl_d3_manager_config
+	 */
 	D3_CONFIG_CMD = 0xd3,
+
+	/**
+	 * @PROT_OFFLOAD_CONFIG_CMD: Depending on firmware, uses one of
+	 * &struct iwl_proto_offload_cmd_v1, &struct iwl_proto_offload_cmd_v2,
+	 * &struct iwl_proto_offload_cmd_v3_small,
+	 * &struct iwl_proto_offload_cmd_v3_large
+	 */
 	PROT_OFFLOAD_CONFIG_CMD = 0xd4,
+
+	/**
+	 * @OFFLOADS_QUERY_CMD:
+	 * No data in command, response in &struct iwl_wowlan_status
+	 */
 	OFFLOADS_QUERY_CMD = 0xd5,
+
+	/**
+	 * @REMOTE_WAKE_CONFIG_CMD: &struct iwl_wowlan_remote_wake_config
+	 */
 	REMOTE_WAKE_CONFIG_CMD = 0xd6,
+
+	/**
+	 * @D0I3_END_CMD: End D0i3/D3 state, no command data
+	 */
 	D0I3_END_CMD = 0xed,
 
-	/* for WoWLAN in particular */
+	/**
+	 * @WOWLAN_PATTERNS: &struct iwl_wowlan_patterns_cmd
+	 */
 	WOWLAN_PATTERNS = 0xe0,
+
+	/**
+	 * @WOWLAN_CONFIGURATION: &struct iwl_wowlan_config_cmd
+	 */
 	WOWLAN_CONFIGURATION = 0xe1,
+
+	/**
+	 * @WOWLAN_TSC_RSC_PARAM: &struct iwl_wowlan_rsc_tsc_params_cmd
+	 */
 	WOWLAN_TSC_RSC_PARAM = 0xe2,
+
+	/**
+	 * @WOWLAN_TKIP_PARAM: &struct iwl_wowlan_tkip_params_cmd
+	 */
 	WOWLAN_TKIP_PARAM = 0xe3,
+
+	/**
+	 * @WOWLAN_KEK_KCK_MATERIAL: &struct iwl_wowlan_kek_kck_material_cmd
+	 */
 	WOWLAN_KEK_KCK_MATERIAL = 0xe4,
+
+	/**
+	 * @WOWLAN_GET_STATUSES: response in &struct iwl_wowlan_status
+	 */
 	WOWLAN_GET_STATUSES = 0xe5,
-	WOWLAN_TX_POWER_PER_DB = 0xe6,
 
-	/* and for NetDetect */
+	/**
+	 * @SCAN_OFFLOAD_PROFILES_QUERY_CMD:
+	 * No command data, response is &struct iwl_scan_offload_profiles_query
+	 */
 	SCAN_OFFLOAD_PROFILES_QUERY_CMD = 0x56,
-	SCAN_OFFLOAD_HOTSPOTS_CONFIG_CMD = 0x58,
-	SCAN_OFFLOAD_HOTSPOTS_QUERY_CMD = 0x59,
-
-	REPLY_MAX = 0xff,
 };
 
 /* Please keep this enum *SORTED* by hex value.
@@ -316,45 +682,169 @@ enum iwl_mac_conf_subcmd_ids {
 	CHANNEL_SWITCH_NOA_NOTIF = 0xFF,
 };
 
+/**
+ * enum iwl_phy_ops_subcmd_ids - PHY group commands
+ */
 enum iwl_phy_ops_subcmd_ids {
+	/**
+	 * @CMD_DTS_MEASUREMENT_TRIGGER_WIDE:
+	 * Uses either &struct iwl_dts_measurement_cmd or
+	 * &struct iwl_ext_dts_measurement_cmd
+	 */
 	CMD_DTS_MEASUREMENT_TRIGGER_WIDE = 0x0,
+
+	/**
+	 * @CTDP_CONFIG_CMD: &struct iwl_mvm_ctdp_cmd
+	 */
 	CTDP_CONFIG_CMD = 0x03,
+
+	/**
+	 * @TEMP_REPORTING_THRESHOLDS_CMD: &struct temp_report_ths_cmd
+	 */
 	TEMP_REPORTING_THRESHOLDS_CMD = 0x04,
+
+	/**
+	 * @GEO_TX_POWER_LIMIT: &struct iwl_geo_tx_power_profiles_cmd
+	 */
 	GEO_TX_POWER_LIMIT = 0x05,
+
+	/**
+	 * @CT_KILL_NOTIFICATION: &struct ct_kill_notif
+	 */
 	CT_KILL_NOTIFICATION = 0xFE,
+
+	/**
+	 * @DTS_MEASUREMENT_NOTIF_WIDE:
+	 * &struct iwl_dts_measurement_notif_v1 or
+	 * &struct iwl_dts_measurement_notif_v2
+	 */
 	DTS_MEASUREMENT_NOTIF_WIDE = 0xFF,
 };
 
+/**
+ * enum iwl_system_subcmd_ids - system group command IDs
+ */
 enum iwl_system_subcmd_ids {
+	/**
+	 * @SHARED_MEM_CFG_CMD:
+	 * response in &struct iwl_shared_mem_cfg or
+	 * &struct iwl_shared_mem_cfg_v2
+	 */
 	SHARED_MEM_CFG_CMD = 0x0,
+
+	/**
+	 * @INIT_EXTENDED_CFG_CMD: &struct iwl_init_extended_cfg_cmd
+	 */
 	INIT_EXTENDED_CFG_CMD = 0x03,
 };
 
+/**
+ * enum iwl_data_path_subcmd_ids - data path group commands
+ */
 enum iwl_data_path_subcmd_ids {
+	/**
+	 * @DQA_ENABLE_CMD: &struct iwl_dqa_enable_cmd
+	 */
 	DQA_ENABLE_CMD = 0x0,
+
+	/**
+	 * @UPDATE_MU_GROUPS_CMD: &struct iwl_mu_group_mgmt_cmd
+	 */
 	UPDATE_MU_GROUPS_CMD = 0x1,
+
+	/**
+	 * @TRIGGER_RX_QUEUES_NOTIF_CMD: &struct iwl_rxq_sync_cmd
+	 */
 	TRIGGER_RX_QUEUES_NOTIF_CMD = 0x2,
+
+	/**
+	 * @STA_PM_NOTIF: &struct iwl_mvm_pm_state_notification
+	 */
 	STA_PM_NOTIF = 0xFD,
+
+	/**
+	 * @MU_GROUP_MGMT_NOTIF: &struct iwl_mu_group_mgmt_notif
+	 */
 	MU_GROUP_MGMT_NOTIF = 0xFE,
+
+	/**
+	 * @RX_QUEUES_NOTIFICATION: &struct iwl_rxq_sync_notification
+	 */
 	RX_QUEUES_NOTIFICATION = 0xFF,
 };
 
+/**
+ * enum iwl_prot_offload_subcmd_ids - protocol offload commands
+ */
 enum iwl_prot_offload_subcmd_ids {
+	/**
+	 * @STORED_BEACON_NTF: &struct iwl_stored_beacon_notif
+	 */
 	STORED_BEACON_NTF = 0xFF,
 };
 
+/**
+ * enum iwl_regulatory_and_nvm_subcmd_ids - regulatory/NVM commands
+ */
 enum iwl_regulatory_and_nvm_subcmd_ids {
+	/**
+	 * @NVM_ACCESS_COMPLETE: &struct iwl_nvm_access_complete_cmd
+	 */
 	NVM_ACCESS_COMPLETE = 0x0,
+
+	/**
+	 * @NVM_GET_INFO:
+	 * Command is &struct iwl_nvm_get_info,
+	 * response is &struct iwl_nvm_get_info_rsp
+	 */
+	NVM_GET_INFO = 0x2,
 };
 
+/**
+ * enum iwl_debug_cmds - debug commands
+ */
 enum iwl_debug_cmds {
+	/**
+	 * @LMAC_RD_WR:
+	 * LMAC memory read/write, using &struct iwl_dbg_mem_access_cmd and
+	 * &struct iwl_dbg_mem_access_rsp
+	 */
 	LMAC_RD_WR = 0x0,
+	/**
+	 * @UMAC_RD_WR:
+	 * UMAC memory read/write, using &struct iwl_dbg_mem_access_cmd and
+	 * &struct iwl_dbg_mem_access_rsp
+	 */
 	UMAC_RD_WR = 0x1,
+	/**
+	 * @MFU_ASSERT_DUMP_NTF:
+	 * &struct iwl_mfu_assert_dump_notif
+	 */
 	MFU_ASSERT_DUMP_NTF = 0xFE,
 };
 
-/* command groups */
-enum {
+/**
+ * enum iwl_mvm_command_groups - command groups for the firmware
+ * @LEGACY_GROUP: legacy group, uses command IDs from &enum iwl_legacy_cmds
+ * @LONG_GROUP: legacy group with long header, also uses command IDs
+ *	from &enum iwl_legacy_cmds
+ * @SYSTEM_GROUP: system group, uses command IDs from
+ *	&enum iwl_system_subcmd_ids
+ * @MAC_CONF_GROUP: MAC configuration group, uses command IDs from
+ *	&enum iwl_mac_conf_subcmd_ids
+ * @PHY_OPS_GROUP: PHY operations group, uses command IDs from
+ *	&enum iwl_phy_ops_subcmd_ids
+ * @DATA_PATH_GROUP: data path group, uses command IDs from
+ *	&enum iwl_data_path_subcmd_ids
+ * @NAN_GROUP: NAN group, uses command IDs from &enum iwl_nan_subcmd_ids
+ * @TOF_GROUP: TOF group, uses command IDs from &enum iwl_tof_subcmd_ids
+ * @PROT_OFFLOAD_GROUP: protocol offload group, uses command IDs from
+ *	&enum iwl_prot_offload_subcmd_ids
+ * @REGULATORY_AND_NVM_GROUP: regulatory/NVM group, uses command IDs from
+ *	&enum iwl_regulatory_and_nvm_subcmd_ids
+ * @DEBUG_GROUP: Debug group, uses command IDs from &enum iwl_debug_cmds
+ */
+enum iwl_mvm_command_groups {
 	LEGACY_GROUP = 0x0,
 	LONG_GROUP = 0x1,
 	SYSTEM_GROUP = 0x2,
@@ -390,13 +880,13 @@ struct iwl_tx_ant_cfg_cmd {
 	__le32 valid;
 } __packed;
 
-/*
- * Calibration control struct.
+/**
+ * struct iwl_calib_ctrl - Calibration control struct.
  * Sent as part of the phy configuration command.
  * @flow_trigger: bitmap for which calibrations to perform according to
- *		flow triggers.
+ *		flow triggers, using &enum iwl_calib_cfg
  * @event_trigger: bitmap for which calibrations to perform according to
- *		event triggers.
+ *		event triggers, using &enum iwl_calib_cfg
  */
 struct iwl_calib_ctrl {
 	__le32 flow_trigger;
@@ -428,8 +918,10 @@ enum iwl_calib_cfg {
 	IWL_CALIB_CFG_AGC_IDX			= BIT(18),
 };
 
-/*
- * Phy configuration command.
+/**
+ * struct iwl_phy_cfg_cmd - Phy configuration command
+ * @phy_cfg: PHY configuration value, uses &enum iwl_fw_phy_cfg
+ * @calib_control: calibration control data
  */
 struct iwl_phy_cfg_cmd {
 	__le32	phy_cfg;
@@ -448,15 +940,39 @@ struct iwl_phy_cfg_cmd {
 #define PHY_CFG_RX_CHAIN_C	BIT(14)
 
 
-/* Target of the NVM_ACCESS_CMD */
-enum {
+/**
+ * enum iwl_nvm_access_op - NVM access opcode
+ * @IWL_NVM_READ: read NVM
+ * @IWL_NVM_WRITE: write NVM
+ */
+enum iwl_nvm_access_op {
+	IWL_NVM_READ	= 0,
+	IWL_NVM_WRITE	= 1,
+};
+
+/**
+ * enum iwl_nvm_access_target - target of the NVM_ACCESS_CMD
+ * @NVM_ACCESS_TARGET_CACHE: access the cache
+ * @NVM_ACCESS_TARGET_OTP: access the OTP
+ * @NVM_ACCESS_TARGET_EEPROM: access the EEPROM
+ */
+enum iwl_nvm_access_target {
 	NVM_ACCESS_TARGET_CACHE = 0,
 	NVM_ACCESS_TARGET_OTP = 1,
 	NVM_ACCESS_TARGET_EEPROM = 2,
 };
 
-/* Section types for NVM_ACCESS_CMD */
-enum {
+/**
+ * enum iwl_nvm_section_type - section types for NVM_ACCESS_CMD
+ * @NVM_SECTION_TYPE_SW: software section
+ * @NVM_SECTION_TYPE_REGULATORY: regulatory section
+ * @NVM_SECTION_TYPE_CALIBRATION: calibration section
+ * @NVM_SECTION_TYPE_PRODUCTION: production section
+ * @NVM_SECTION_TYPE_MAC_OVERRIDE: MAC override section
+ * @NVM_SECTION_TYPE_PHY_SKU: PHY SKU section
+ * @NVM_MAX_NUM_SECTIONS: number of sections
+ */
+enum iwl_nvm_section_type {
 	NVM_SECTION_TYPE_SW = 1,
 	NVM_SECTION_TYPE_REGULATORY = 3,
 	NVM_SECTION_TYPE_CALIBRATION = 4,
@@ -467,10 +983,10 @@ enum {
 };
 
 /**
- * struct iwl_nvm_access_cmd_ver2 - Request the device to send an NVM section
- * @op_code: 0 - read, 1 - write
- * @target: NVM_ACCESS_TARGET_*
- * @type: NVM_SECTION_TYPE_*
+ * struct iwl_nvm_access_cmd - Request the device to send an NVM section
+ * @op_code: &enum iwl_nvm_access_op
+ * @target: &enum iwl_nvm_access_target
+ * @type: &enum iwl_nvm_section_type
  * @offset: offset in bytes into the section
  * @length: in bytes, to read/write
  * @data: if write operation, the data to write. On read its empty
@@ -486,7 +1002,7 @@ struct iwl_nvm_access_cmd {
 
 #define NUM_OF_FW_PAGING_BLOCKS	33 /* 32 for data and 1 block for CSS */
 
-/*
+/**
  * struct iwl_fw_paging_cmd - paging layout
  *
  * (FW_PAGING_BLOCK_CMD = 0x4f)
@@ -497,17 +1013,13 @@ struct iwl_nvm_access_cmd {
  * @block_size: the block size in powers of 2
  * @block_num: number of blocks specified in the command.
  * @device_phy_addr: virtual addresses from device side
- *	32 bit address for API version 1, 64 bit address for API version 2.
-*/
+ */
 struct iwl_fw_paging_cmd {
 	__le32 flags;
 	__le32 block_size;
 	__le32 block_num;
-	union {
-		__le32 addr32[NUM_OF_FW_PAGING_BLOCKS];
-		__le64 addr64[NUM_OF_FW_PAGING_BLOCKS];
-	} device_phy_addr;
-} __packed; /* FW_PAGING_BLOCK_CMD_API_S_VER_2 */
+	__le32 device_phy_addr[NUM_OF_FW_PAGING_BLOCKS];
+} __packed; /* FW_PAGING_BLOCK_CMD_API_S_VER_1 */
 
 /*
  * Fw items ID's
@@ -660,6 +1172,7 @@ enum {
  * ( REPLY_ERROR = 0x2 )
  * @error_type: one of FW_ERR_*
  * @cmd_id: the command ID for which the error occured
+ * @reserved1: reserved
  * @bad_cmd_seq_num: sequence number of the erroneous command
  * @error_service: which service created the error, applicable only if
  *	error_type = 2, otherwise 0
@@ -679,12 +1192,21 @@ struct iwl_error_resp {
 #define MAX_MACS_IN_BINDING	(3)
 #define MAX_BINDINGS		(4)
 
-/* Used to extract ID and color from the context dword */
-#define FW_CTXT_ID_POS	  (0)
-#define FW_CTXT_ID_MSK	  (0xff << FW_CTXT_ID_POS)
-#define FW_CTXT_COLOR_POS (8)
-#define FW_CTXT_COLOR_MSK (0xff << FW_CTXT_COLOR_POS)
-#define FW_CTXT_INVALID	  (0xffffffff)
+/**
+ * enum iwl_mvm_id_and_color - ID and color fields in context dword
+ * @FW_CTXT_ID_POS: position of the ID
+ * @FW_CTXT_ID_MSK: mask of the ID
+ * @FW_CTXT_COLOR_POS: position of the color
+ * @FW_CTXT_COLOR_MSK: mask of the color
+ * @FW_CTXT_INVALID: value used to indicate unused/invalid
+ */
+enum iwl_mvm_id_and_color {
+	FW_CTXT_ID_POS		= 0,
+	FW_CTXT_ID_MSK		= 0xff << FW_CTXT_ID_POS,
+	FW_CTXT_COLOR_POS	= 8,
+	FW_CTXT_COLOR_MSK	= 0xff << FW_CTXT_COLOR_POS,
+	FW_CTXT_INVALID		= 0xffffffff,
+};
 
 #define FW_CMD_ID_AND_COLOR(_id, _color) ((_id << FW_CTXT_ID_POS) |\
 					  (_color << FW_CTXT_COLOR_POS))
@@ -832,7 +1354,8 @@ enum {
 #define TE_V2_PLACEMENT_POS	12
 #define TE_V2_ABSENCE_POS	15
 
-/* Time event policy values
+/**
+ * enum iwl_time_event_policy - Time event policy values
  * A notification (both event and fragment) includes a status indicating weather
  * the FW was able to schedule the event or not. For fragment start/end
  * notification the status is always success. There is no start/end fragment
@@ -847,12 +1370,13 @@ enum {
  * @TE_V2_NOTIF_HOST_FRAG_END:request/receive notification on frag end
  * @TE_V2_NOTIF_INTERNAL_FRAG_START: internal FW use.
  * @TE_V2_NOTIF_INTERNAL_FRAG_END: internal FW use.
+ * @T2_V2_START_IMMEDIATELY: start time event immediately
  * @TE_V2_DEP_OTHER: depends on another time event
  * @TE_V2_DEP_TSF: depends on a specific time
  * @TE_V2_EVENT_SOCIOPATHIC: can't co-exist with other events of tha same MAC
  * @TE_V2_ABSENCE: are we present or absent during the Time Event.
  */
-enum {
+enum iwl_time_event_policy {
 	TE_V2_DEFAULT_POLICY = 0x0,
 
 	/* notifications (event start/stop, fragment start/stop) */
@@ -867,8 +1391,6 @@ enum {
 	TE_V2_NOTIF_INTERNAL_FRAG_END = BIT(7),
 	T2_V2_START_IMMEDIATELY = BIT(11),
 
-	TE_V2_NOTIF_MSK = 0xff,
-
 	/* placement characteristics */
 	TE_V2_DEP_OTHER = BIT(TE_V2_PLACEMENT_POS),
 	TE_V2_DEP_TSF = BIT(TE_V2_PLACEMENT_POS + 1),
@@ -879,12 +1401,13 @@ enum {
 };
 
 /**
- * struct iwl_time_event_cmd_api - configuring Time Events
+ * struct iwl_time_event_cmd - configuring Time Events
  * with struct MAC_TIME_EVENT_DATA_API_S_VER_2 (see also
  * with version 1. determined by IWL_UCODE_TLV_FLAGS)
  * ( TIME_EVENT_CMD = 0x29 )
- * @id_and_color: ID and color of the relevant MAC
- * @action: action to perform, one of FW_CTXT_ACTION_*
+ * @id_and_color: ID and color of the relevant MAC,
+ *	&enum iwl_mvm_id_and_color
+ * @action: action to perform, one of &enum iwl_phy_ctxt_action
  * @id: this field has two meanings, depending on the action:
  *	If the action is ADD, then it means the type of event to add.
  *	For all other actions it is the unique event ID assigned when the
@@ -900,7 +1423,8 @@ enum {
  *	on event and/or fragment start and/or end
  *	using one of TE_INDEPENDENT, TE_DEP_OTHER, TE_DEP_TSF
  *	TE_EVENT_SOCIOPATHIC
- *	using TE_ABSENCE and using TE_NOTIF_*
+ *	using TE_ABSENCE and using TE_NOTIF_*,
+ *	&enum iwl_time_event_policy
  */
 struct iwl_time_event_cmd {
 	/* COMMON_INDEX_HDR_API_S_VER_1 */
@@ -923,7 +1447,8 @@ struct iwl_time_event_cmd {
  * @status: bit 0 indicates success, all others specify errors
  * @id: the Time Event type
  * @unique_id: the unique ID assigned (in ADD) or given (others) to the TE
- * @id_and_color: ID and color of the relevant MAC
+ * @id_and_color: ID and color of the relevant MAC,
+ *	&enum iwl_mvm_id_and_color
  */
 struct iwl_time_event_resp {
 	__le32 status;
@@ -939,7 +1464,7 @@ struct iwl_time_event_resp {
  * @session_id: session's unique id
  * @unique_id: unique id of the Time Event itself
  * @id_and_color: ID and color of the relevant MAC
- * @action: one of TE_NOTIF_START or TE_NOTIF_END
+ * @action: &enum iwl_time_event_policy
  * @status: true if scheduled, false otherwise (not executed)
  */
 struct iwl_time_event_notif {
@@ -955,12 +1480,35 @@ struct iwl_time_event_notif {
 /* Bindings and Time Quota */
 
 /**
+ * struct iwl_binding_cmd_v1 - configuring bindings
+ * ( BINDING_CONTEXT_CMD = 0x2b )
+ * @id_and_color: ID and color of the relevant Binding,
+ *	&enum iwl_mvm_id_and_color
+ * @action: action to perform, one of FW_CTXT_ACTION_*
+ * @macs: array of MAC id and colors which belong to the binding,
+ *	&enum iwl_mvm_id_and_color
+ * @phy: PHY id and color which belongs to the binding,
+ *	&enum iwl_mvm_id_and_color
+ */
+struct iwl_binding_cmd_v1 {
+	/* COMMON_INDEX_HDR_API_S_VER_1 */
+	__le32 id_and_color;
+	__le32 action;
+	/* BINDING_DATA_API_S_VER_1 */
+	__le32 macs[MAX_MACS_IN_BINDING];
+	__le32 phy;
+} __packed; /* BINDING_CMD_API_S_VER_1 */
+
+/**
  * struct iwl_binding_cmd - configuring bindings
  * ( BINDING_CONTEXT_CMD = 0x2b )
- * @id_and_color: ID and color of the relevant Binding
+ * @id_and_color: ID and color of the relevant Binding,
+ *	&enum iwl_mvm_id_and_color
  * @action: action to perform, one of FW_CTXT_ACTION_*
  * @macs: array of MAC id and colors which belong to the binding
+ *	&enum iwl_mvm_id_and_color
  * @phy: PHY id and color which belongs to the binding
+ *	&enum iwl_mvm_id_and_color
  * @lmac_id: the lmac id the binding belongs to
  */
 struct iwl_binding_cmd {
@@ -970,11 +1518,10 @@ struct iwl_binding_cmd {
 	/* BINDING_DATA_API_S_VER_1 */
 	__le32 macs[MAX_MACS_IN_BINDING];
 	__le32 phy;
-	/* BINDING_CMD_API_S_VER_1 */
 	__le32 lmac_id;
 } __packed; /* BINDING_CMD_API_S_VER_2 */
 
-#define IWL_BINDING_CMD_SIZE_V1	offsetof(struct iwl_binding_cmd, lmac_id)
+#define IWL_BINDING_CMD_SIZE_V1	sizeof(struct iwl_binding_cmd_v1)
 #define IWL_LMAC_24G_INDEX		0
 #define IWL_LMAC_5G_INDEX		1
 
@@ -983,7 +1530,8 @@ struct iwl_binding_cmd {
 
 /**
  * struct iwl_time_quota_data - configuration of time quota per binding
- * @id_and_color: ID and color of the relevant Binding
+ * @id_and_color: ID and color of the relevant Binding,
+ *	&enum iwl_mvm_id_and_color
  * @quota: absolute time quota in TU. The scheduler will try to divide the
  *	remainig quota (after Time Events) according to this quota.
  * @max_duration: max uninterrupted context duration in TU
@@ -1087,7 +1635,7 @@ struct iwl_fw_channel_info {
  * @apply_time: 0 means immediate apply and context switch.
  *	other value means apply new params after X usecs
  * @tx_param_color: ???
- * @channel_info:
+ * @ci: channel info
  * @txchain_info: ???
  * @rxchain_info: ???
  * @acquisition_data: ???
@@ -1176,9 +1724,9 @@ struct iwl_hs20_roc_res {
 /**
  * struct iwl_radio_version_notif - information on the radio version
  * ( RADIO_VERSION_NOTIFICATION = 0x68 )
- * @radio_flavor:
- * @radio_step:
- * @radio_dash:
+ * @radio_flavor: radio flavor
+ * @radio_step: radio version step
+ * @radio_dash: radio version dash
  */
 struct iwl_radio_version_notif {
 	__le32 radio_flavor;
@@ -1212,8 +1760,8 @@ struct iwl_card_state_notif {
  * @consec_missed_beacons_since_last_rx: number of consecutive missed
  *	beacons since last RX.
  * @consec_missed_beacons: number of consecutive missed beacons
- * @num_expected_beacons:
- * @num_recvd_beacons:
+ * @num_expected_beacons: number of expected beacons
+ * @num_recvd_beacons: number of received beacons
  */
 struct iwl_missed_beacons_notif {
 	__le32 mac_id;
@@ -1260,19 +1808,6 @@ struct iwl_mfu_assert_dump_notif {
 	__le32   data[0];
 } __packed; /*MFU_DUMP_ASSERT_API_S_VER_1*/
 
-/**
- * struct iwl_set_calib_default_cmd - set default value for calibration.
- * ( SET_CALIB_DEFAULT_CMD = 0x8e )
- * @calib_index: the calibration to set value for
- * @length: of data
- * @data: the value to set for the calibration result
- */
-struct iwl_set_calib_default_cmd {
-	__le16 calib_index;
-	__le16 length;
-	u8 data[0];
-} __packed; /* PHY_CALIB_OVERRIDE_VALUES_S */
-
 #define MAX_PORT_ID_NUM	2
 #define MAX_MCAST_FILTERING_ADDRESSES 256
 
@@ -1286,6 +1821,7 @@ struct iwl_set_calib_default_cmd {
  * @count:	Number of MAC addresses in the array
  * @pass_all:	Set 1 to pass all multicast packets.
  * @bssid:	current association BSSID.
+ * @reserved:	reserved
  * @addr_list:	Place holder for array of MAC addresses.
  *		IMPORTANT: add padding if necessary to ensure DWORD alignment.
  */
@@ -1317,7 +1853,8 @@ enum iwl_mvm_bcast_filter_attr_offset {
  * struct iwl_fw_bcast_filter_attr - broadcast filter attribute
  * @offset_type:	&enum iwl_mvm_bcast_filter_attr_offset.
  * @offset:	starting offset of this pattern.
- * @val:		value to match - big endian (MSB is the first
+ * @reserved1:	reserved
+ * @val:	value to match - big endian (MSB is the first
  *		byte to match from offset pos).
  * @mask:	mask to match (big endian).
  */
@@ -1343,6 +1880,7 @@ enum iwl_mvm_bcast_filter_frame_type {
  * struct iwl_fw_bcast_filter - broadcast filter
  * @discard: discard frame (1) or let it pass (0).
  * @frame_type: &enum iwl_mvm_bcast_filter_frame_type.
+ * @reserved1: reserved
  * @num_attrs: number of valid attributes in this filter.
  * @attrs: attributes of this filter. a filter is considered matched
  *	only when all its attributes are matched (i.e. AND relationship)
@@ -1378,6 +1916,7 @@ struct iwl_ba_window_status_notif {
 /**
  * struct iwl_fw_bcast_mac - per-mac broadcast filtering configuration.
  * @default_discard: default action for this mac (discard (1) / pass (0)).
+ * @reserved1: reserved
  * @attached_filters: bitmap of relevant filters for this mac.
  */
 struct iwl_fw_bcast_mac {
@@ -1391,6 +1930,7 @@ struct iwl_fw_bcast_mac {
  * @disable: enable (0) / disable (1)
  * @max_bcast_filters: max number of filters (MAX_BCAST_FILTERS)
  * @max_macs: max number of macs (NUM_MAC_INDEX_DRIVER)
+ * @reserved1: reserved
  * @filters: broadcast filters
  * @macs: broadcast filtering configuration per-mac
  */
@@ -1428,8 +1968,8 @@ enum iwl_mvm_marker_id {
  * @metadata: additional meta data that will be written to the unsiffer log
  */
 struct iwl_mvm_marker {
-	u8 dwLen;
-	u8 markerId;
+	u8 dw_len;
+	u8 marker_id;
 	__le16 reserved;
 	__le64 timestamp;
 	__le32 metadata[0];
@@ -1539,8 +2079,8 @@ enum iwl_sf_scenario {
 #define SF_CFG_DUMMY_NOTIF_OFF	BIT(16)
 
 /**
- * Smart Fifo configuration command.
- * @state: smart fifo state, types listed in enum %iwl_sf_sate.
+ * struct iwl_sf_cfg_cmd - Smart Fifo configuration command.
+ * @state: smart fifo state, types listed in &enum iwl_sf_state.
  * @watermark: Minimum allowed availabe free space in RXF for transient state.
  * @long_delay_timeouts: aging and idle timer values for each scenario
  * in long delay state.
@@ -1590,11 +2130,11 @@ struct iwl_mcc_update_cmd {
 	u8 source_id;
 	u8 reserved;
 	__le32 key;
-	__le32 reserved2[5];
+	u8 reserved2[20];
 } __packed; /* LAR_UPDATE_MCC_CMD_API_S_VER_2 */
 
 /**
- * iwl_mcc_update_resp_v1  - response to MCC_UPDATE_CMD.
+ * struct iwl_mcc_update_resp_v1  - response to MCC_UPDATE_CMD.
  * Contains the new channel control profile map, if changed, and the new MCC
  * (mobile country code).
  * The new MCC may be different than what was requested in MCC_UPDATE_CMD.
@@ -1617,7 +2157,7 @@ struct iwl_mcc_update_resp_v1  {
 } __packed; /* LAR_UPDATE_MCC_CMD_RESP_S_VER_1 */
 
 /**
- * iwl_mcc_update_resp - response to MCC_UPDATE_CMD.
+ * struct iwl_mcc_update_resp - response to MCC_UPDATE_CMD.
  * Contains the new channel control profile map, if changed, and the new MCC
  * (mobile country code).
  * The new MCC may be different than what was requested in MCC_UPDATE_CMD.
@@ -1659,7 +2199,7 @@ struct iwl_mcc_update_resp {
  * @reserved1: reserved for alignment
  */
 struct iwl_mcc_chub_notif {
-	u16 mcc;
+	__le16 mcc;
 	u8 source_id;
 	u8 reserved1;
 } __packed; /* LAR_MCC_NOTIFY_S */
@@ -1699,10 +2239,10 @@ enum iwl_dts_measurement_flags {
 };
 
 /**
- * iwl_dts_measurement_cmd - request DTS temperature and/or voltage measurements
+ * struct iwl_dts_measurement_cmd - request DTS temp and/or voltage measurements
  *
- * @flags: indicates which measurements we want as specified in &enum
- *	   iwl_dts_measurement_flags
+ * @flags: indicates which measurements we want as specified in
+ *	&enum iwl_dts_measurement_flags
  */
 struct iwl_dts_measurement_cmd {
 	__le32 flags;
@@ -1733,7 +2273,7 @@ enum iwl_dts_control_measurement_mode {
 * @DTS_USE_CHAIN_A: chain A
 * @DTS_USE_CHAIN_B: chain B
 * @DTS_USE_CHAIN_C: chain C
-* @XTAL_TEMPERATURE - read temperature from xtal
+* @XTAL_TEMPERATURE: read temperature from xtal
 */
 enum iwl_dts_used {
 	DTS_USE_TOP		= 0,
@@ -1754,7 +2294,7 @@ enum iwl_dts_bit_mode {
 };
 
 /**
- * iwl_ext_dts_measurement_cmd - request extended DTS temperature measurements
+ * struct iwl_ext_dts_measurement_cmd - request extended DTS temp measurements
  * @control_mode: see &enum iwl_dts_control_measurement_mode
  * @temperature: used when over write DTS mode is selected
  * @sensor: set temperature sensor to use. See &enum iwl_dts_used
@@ -1810,7 +2350,7 @@ struct ct_kill_notif {
 * enum ctdp_cmd_operation - CTDP command operations
 * @CTDP_CMD_OPERATION_START: update the current budget
 * @CTDP_CMD_OPERATION_STOP: stop ctdp
-* @CTDP_CMD_OPERATION_REPORT: get the avgerage budget
+* @CTDP_CMD_OPERATION_REPORT: get the average budget
 */
 enum iwl_mvm_ctdp_cmd_operation {
 	CTDP_CMD_OPERATION_START	= 0x1,
@@ -1834,7 +2374,7 @@ struct iwl_mvm_ctdp_cmd {
 #define IWL_MAX_DTS_TRIPS	8
 
 /**
- * struct iwl_temp_report_ths_cmd - set temperature thresholds
+ * struct temp_report_ths_cmd - set temperature thresholds
  *
  * @num_temps: number of temperature thresholds passed
  * @thresholds: array with the thresholds to be configured
@@ -1856,7 +2396,7 @@ enum iwl_tdls_channel_switch_type {
 }; /* TDLS_STA_CHANNEL_SWITCH_CMD_TYPE_API_E_VER_1 */
 
 /**
- * Switch timing sub-element in a TDLS channel-switch command
+ * struct iwl_tdls_channel_switch_timing - Switch timing in TDLS channel-switch
  * @frame_timestamp: GP2 timestamp of channel-switch request/response packet
  *	received from peer
  * @max_offchan_duration: What amount of microseconds out of a DTIM is given
@@ -1864,7 +2404,7 @@ enum iwl_tdls_channel_switch_type {
  *	200TU and the TDLS peer is to be given 25% of the time, the value
  *	given will be 50TU, or 50 * 1024 if translated into microseconds.
  * @switch_time: switch time the peer sent in its channel switch timing IE
- * @switch_timout: switch timeout the peer sent in its channel switch timing IE
+ * @switch_timeout: switch timeout the peer sent in its channel switch timing IE
  */
 struct iwl_tdls_channel_switch_timing {
 	__le32 frame_timestamp; /* GP2 time of peer packet Rx */
@@ -1876,7 +2416,7 @@ struct iwl_tdls_channel_switch_timing {
 #define IWL_TDLS_CH_SW_FRAME_MAX_SIZE 200
 
 /**
- * TDLS channel switch frame template
+ * struct iwl_tdls_channel_switch_frame - TDLS channel switch frame template
  *
  * A template representing a TDLS channel-switch request or response frame
  *
@@ -1891,7 +2431,7 @@ struct iwl_tdls_channel_switch_frame {
 } __packed; /* TDLS_STA_CHANNEL_SWITCH_FRAME_API_S_VER_1 */
 
 /**
- * TDLS channel switch command
+ * struct iwl_tdls_channel_switch_cmd - TDLS channel switch command
  *
  * The command is sent to initiate a channel switch and also in response to
  * incoming TDLS channel-switch request/response packets from remote peers.
@@ -1911,7 +2451,7 @@ struct iwl_tdls_channel_switch_cmd {
 } __packed; /* TDLS_STA_CHANNEL_SWITCH_CMD_API_S_VER_1 */
 
 /**
- * TDLS channel switch start notification
+ * struct iwl_tdls_channel_switch_notif - TDLS channel switch start notification
  *
  * @status: non-zero on success
  * @offchannel_duration: duration given in microseconds
@@ -1924,7 +2464,7 @@ struct iwl_tdls_channel_switch_notif {
 } __packed; /* TDLS_STA_CHANNEL_SWITCH_NTFY_API_S_VER_1 */
 
 /**
- * TDLS station info
+ * struct iwl_tdls_sta_info - TDLS station info
  *
  * @sta_id: station id of the TDLS peer
  * @tx_to_peer_tid: TID reserved vs. the peer for FW based Tx
@@ -1939,7 +2479,7 @@ struct iwl_tdls_sta_info {
 } __packed; /* TDLS_STA_INFO_VER_1 */
 
 /**
- * TDLS basic config command
+ * struct iwl_tdls_config_cmd - TDLS basic config command
  *
  * @id_and_color: MAC id and color being configured
  * @tdls_peer_count: amount of currently connected TDLS peers
@@ -1963,7 +2503,7 @@ struct iwl_tdls_config_cmd {
 } __packed; /* TDLS_CONFIG_CMD_API_S_VER_1 */
 
 /**
- * TDLS per-station config information from FW
+ * struct iwl_tdls_config_sta_info_res - TDLS per-station config information
  *
  * @sta_id: station id of the TDLS peer
  * @tx_to_peer_last_seq: last sequence number used by FW during FW-based Tx to
@@ -1975,7 +2515,7 @@ struct iwl_tdls_config_sta_info_res {
 } __packed; /* TDLS_STA_INFO_RSP_VER_1 */
 
 /**
- * TDLS config information from FW
+ * struct iwl_tdls_config_res - TDLS config information from FW
  *
  * @tx_to_ap_last_seq: last sequence number used by FW during FW-based Tx to AP
  * @sta_info: per-station TDLS config information
@@ -1991,7 +2531,7 @@ struct iwl_tdls_config_res {
 #define TX_FIFO_INTERNAL_MAX_NUM	6
 
 /**
- * Shared memory configuration information from the FW
+ * struct iwl_shared_mem_cfg_v2 - Shared memory configuration information
  *
  * @shared_mem_addr: shared memory addr (pre 8000 HW set to 0x0 as MARBH is not
  *	accessible)
@@ -2013,7 +2553,7 @@ struct iwl_tdls_config_res {
  * NOTE: on firmware that don't have IWL_UCODE_TLV_CAPA_EXTEND_SHARED_MEM_CFG
  *	 set, the last 3 members don't exist.
  */
-struct iwl_shared_mem_cfg_v1 {
+struct iwl_shared_mem_cfg_v2 {
 	__le32 shared_mem_addr;
 	__le32 shared_mem_size;
 	__le32 sample_buff_addr;
@@ -2045,7 +2585,7 @@ struct iwl_shared_mem_lmac_cfg {
 } __packed; /* SHARED_MEM_ALLOC_LMAC_API_S_VER_1 */
 
 /**
- * Shared memory configuration information from the FW
+ * struct iwl_shared_mem_cfg - Shared memory configuration information
  *
  * @shared_mem_addr: shared memory address
  * @shared_mem_size: shared memory size
@@ -2073,8 +2613,9 @@ struct iwl_shared_mem_cfg {
 } __packed; /* SHARED_MEM_ALLOC_API_S_VER_3 */
 
 /**
- * VHT MU-MIMO group configuration
+ * struct iwl_mu_group_mgmt_cmd - VHT MU-MIMO group configuration
  *
+ * @reserved: reserved
  * @membership_status: a bitmap of MU groups
  * @user_position:the position of station in a group. If the station is in the
  *	group then bits (group * 2) is the position -1
@@ -2100,7 +2641,7 @@ struct iwl_mu_group_mgmt_notif {
 #define MAX_STORED_BEACON_SIZE 600
 
 /**
- * Stored beacon notification
+ * struct iwl_stored_beacon_notif - Stored beacon notification
  *
  * @system_time: system time on air rise
  * @tsf: TSF on air rise
@@ -2109,6 +2650,7 @@ struct iwl_mu_group_mgmt_notif {
  * @channel: channel this beacon was received on
  * @rates: rate in ucode internal format
  * @byte_count: frame's byte count
+ * @data: beacon data, length in @byte_count
  */
 struct iwl_stored_beacon_notif {
 	__le32 system_time;
@@ -2135,8 +2677,8 @@ enum iwl_lqm_status {
 };
 
 /**
- * Link Quality Measurement command
- * @cmd_operatrion: command operation to be performed (start or stop)
+ * struct iwl_link_qual_msrmnt_cmd - Link Quality Measurement command
+ * @cmd_operation: command operation to be performed (start or stop)
  *	as defined above.
  * @mac_id: MAC ID the measurement applies to.
  * @measurement_time: time of the total measurement to be performed, in uSec.
@@ -2150,7 +2692,7 @@ struct iwl_link_qual_msrmnt_cmd {
 } __packed /* LQM_CMD_API_S_VER_1 */;
 
 /**
- * Link Quality Measurement notification
+ * struct iwl_link_qual_msrmnt_notif - Link Quality Measurement notification
  *
  * @frequent_stations_air_time: an array containing the total air time
  *	(in uSec) used by the most frequently transmitting stations.
@@ -2174,11 +2716,11 @@ struct iwl_link_qual_msrmnt_notif {
 	__le32 tx_frame_dropped;
 	__le32 mac_id;
 	__le32 status;
-	__le32 reserved[3];
+	u8 reserved[12];
 } __packed; /* LQM_MEASUREMENT_COMPLETE_NTF_API_S_VER1 */
 
 /**
- * Channel switch NOA notification
+ * struct iwl_channel_switch_noa_notif - Channel switch NOA notification
  *
  * @id_and_color: ID and color of the MAC
  */
@@ -2232,6 +2774,7 @@ struct iwl_dbg_mem_access_rsp {
 
 /**
  * struct iwl_nvm_access_complete_cmd - NVM_ACCESS commands are completed
+ * @reserved: reserved
  */
 struct iwl_nvm_access_complete_cmd {
 	__le32 reserved;
@@ -2259,4 +2802,89 @@ struct iwl_init_extended_cfg_cmd {
 	__le32 init_flags;
 } __packed; /* INIT_EXTENDED_CFG_CMD_API_S_VER_1 */
 
+/*
+ * struct iwl_nvm_get_info - request to get NVM data
+ */
+struct iwl_nvm_get_info {
+	__le32 reserved;
+} __packed; /* GRP_REGULATORY_NVM_GET_INFO_CMD_S_VER_1 */
+
+/**
+ * struct iwl_nvm_get_info_general - general NVM data
+ * @flags: 1 - empty, 0 - valid
+ * @nvm_version: nvm version
+ * @board_type: board type
+ * @reserved: reserved
+ */
+struct iwl_nvm_get_info_general {
+	__le32 flags;
+	__le16 nvm_version;
+	u8 board_type;
+	u8 reserved;
+} __packed; /* GRP_REGULATORY_NVM_GET_INFO_GENERAL_S_VER_1 */
+
+/**
+ * struct iwl_nvm_get_info_sku - mac information
+ * @enable_24g: band 2.4G enabled
+ * @enable_5g: band 5G enabled
+ * @enable_11n: 11n enabled
+ * @enable_11ac: 11ac enabled
+ * @mimo_disable: MIMO enabled
+ * @ext_crypto: Extended crypto enabled
+ */
+struct iwl_nvm_get_info_sku {
+	__le32 enable_24g;
+	__le32 enable_5g;
+	__le32 enable_11n;
+	__le32 enable_11ac;
+	__le32 mimo_disable;
+	__le32 ext_crypto;
+} __packed; /* GRP_REGULATORY_NVM_GET_INFO_MAC_SKU_SECTION_S_VER_1 */
+
+/**
+ * struct iwl_nvm_get_info_phy - phy information
+ * @tx_chains: BIT 0 chain A, BIT 1 chain B
+ * @rx_chains: BIT 0 chain A, BIT 1 chain B
+ */
+struct iwl_nvm_get_info_phy {
+	__le32 tx_chains;
+	__le32 rx_chains;
+} __packed; /* GRP_REGULATORY_NVM_GET_INFO_PHY_SKU_SECTION_S_VER_1 */
+
+#define IWL_NUM_CHANNELS (51)
+
+/**
+ * struct iwl_nvm_get_info_regulatory - regulatory information
+ * @lar_enabled: is LAR enabled
+ * @channel_profile: regulatory data of this channel
+ * @reserved: reserved
+ */
+struct iwl_nvm_get_info_regulatory {
+	__le32 lar_enabled;
+	__le16 channel_profile[IWL_NUM_CHANNELS];
+	__le16 reserved;
+} __packed; /* GRP_REGULATORY_NVM_GET_INFO_REGULATORY_S_VER_1 */
+
+/**
+ * struct iwl_nvm_get_info_rsp - response to get NVM data
+ * @general: general NVM data
+ * @mac_sku: data relating to MAC sku
+ * @phy_sku: data relating to PHY sku
+ * @regulatory: regulatory data
+ */
+struct iwl_nvm_get_info_rsp {
+	struct iwl_nvm_get_info_general general;
+	struct iwl_nvm_get_info_sku mac_sku;
+	struct iwl_nvm_get_info_phy phy_sku;
+	struct iwl_nvm_get_info_regulatory regulatory;
+} __packed; /* GRP_REGULATORY_NVM_GET_INFO_CMD_RSP_S_VER_1 */
+
+/**
+ * struct iwl_mvm_antenna_coupling_notif - antenna coupling notification
+ * @isolation: antenna isolation value
+ */
+struct iwl_mvm_antenna_coupling_notif {
+	__le32 isolation;
+} __packed;
+
 #endif /* __fw_api_h__ */
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.c b/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.c
index c8712e6eea74..1602b360353c 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.c
@@ -319,10 +319,8 @@ static void iwl_mvm_dump_fifos(struct iwl_mvm *mvm,
 
 void iwl_mvm_free_fw_dump_desc(struct iwl_mvm *mvm)
 {
-	if (mvm->fw_dump_desc == &iwl_mvm_dump_desc_assert)
-		return;
-
-	kfree(mvm->fw_dump_desc);
+	if (mvm->fw_dump_desc != &iwl_mvm_dump_desc_assert)
+		kfree(mvm->fw_dump_desc);
 	mvm->fw_dump_desc = NULL;
 }
 
@@ -640,18 +638,21 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 		}
 
 		/* Make room for PRPH registers */
-		for (i = 0; i < ARRAY_SIZE(iwl_prph_dump_addr_comm); i++) {
-			/* The range includes both boundaries */
-			int num_bytes_in_chunk =
-				iwl_prph_dump_addr_comm[i].end -
-				iwl_prph_dump_addr_comm[i].start + 4;
-
-			prph_len += sizeof(*dump_data) +
-				sizeof(struct iwl_fw_error_dump_prph) +
-				num_bytes_in_chunk;
+		if (!mvm->trans->cfg->gen2) {
+			for (i = 0; i < ARRAY_SIZE(iwl_prph_dump_addr_comm);
+			     i++) {
+				/* The range includes both boundaries */
+				int num_bytes_in_chunk =
+					iwl_prph_dump_addr_comm[i].end -
+					iwl_prph_dump_addr_comm[i].start + 4;
+
+				prph_len += sizeof(*dump_data) +
+					sizeof(struct iwl_fw_error_dump_prph) +
+					num_bytes_in_chunk;
+			}
 		}
 
-		if (mvm->cfg->mq_rx_supported) {
+		if (!mvm->trans->cfg->gen2 && mvm->cfg->mq_rx_supported) {
 			for (i = 0; i <
 				ARRAY_SIZE(iwl_prph_dump_addr_9000); i++) {
 				/* The range includes both boundaries */
@@ -691,7 +692,8 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 	}
 
 	/* Make room for fw's virtual image pages, if it exists */
-	if (mvm->fw->img[mvm->cur_ucode].paging_mem_size &&
+	if (!mvm->trans->cfg->gen2 &&
+	    mvm->fw->img[mvm->cur_ucode].paging_mem_size &&
 	    mvm->fw_paging_db[0].fw_paging_block)
 		file_len += mvm->num_of_paging_blk *
 			(sizeof(*dump_data) +
@@ -704,14 +706,6 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 			   sizeof(*dump_info);
 	}
 
-	/*
-	 * In 8000 HW family B-step include the ICCM (which resides separately)
-	 */
-	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000 &&
-	    CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_B_STEP)
-		file_len += sizeof(*dump_data) + sizeof(*dump_mem) +
-			    IWL8260_ICCM_LEN;
-
 	if (mvm->fw_dump_desc)
 		file_len += sizeof(*dump_data) + sizeof(*dump_trig) +
 			    mvm->fw_dump_desc->len;
@@ -836,21 +830,9 @@ void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 		dump_data = iwl_fw_error_next_data(dump_data);
 	}
 
-	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000 &&
-	    CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_B_STEP) {
-		dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM);
-		dump_data->len = cpu_to_le32(IWL8260_ICCM_LEN +
-					     sizeof(*dump_mem));
-		dump_mem = (void *)dump_data->data;
-		dump_mem->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM_SRAM);
-		dump_mem->offset = cpu_to_le32(IWL8260_ICCM_OFFSET);
-		iwl_trans_read_mem_bytes(mvm->trans, IWL8260_ICCM_OFFSET,
-					 dump_mem->data, IWL8260_ICCM_LEN);
-		dump_data = iwl_fw_error_next_data(dump_data);
-	}
-
 	/* Dump fw's virtual image */
-	if (mvm->fw->img[mvm->cur_ucode].paging_mem_size &&
+	if (!mvm->trans->cfg->gen2 &&
+	    mvm->fw->img[mvm->cur_ucode].paging_mem_size &&
 	    mvm->fw_paging_db[0].fw_paging_block) {
 		for (i = 1; i < mvm->num_of_paging_blk + 1; i++) {
 			struct iwl_fw_error_dump_paging *paging;
@@ -931,6 +913,10 @@ int iwl_mvm_fw_dbg_collect_desc(struct iwl_mvm *mvm,
 	if (trigger)
 		delay = msecs_to_jiffies(le32_to_cpu(trigger->stop_delay));
 
+	if (WARN(mvm->trans->state == IWL_TRANS_NO_FW,
+		 "Can't collect dbg data when FW isn't alive\n"))
+		return -EIO;
+
 	if (test_and_set_bit(IWL_MVM_STATUS_DUMPING_FW_LOG, &mvm->status))
 		return -EBUSY;
 
@@ -943,7 +929,7 @@ int iwl_mvm_fw_dbg_collect_desc(struct iwl_mvm *mvm,
 	mvm->fw_dump_desc = desc;
 	mvm->fw_dump_trig = trigger;
 
-	queue_delayed_work(system_wq, &mvm->fw_dump_wk, delay);
+	schedule_delayed_work(&mvm->fw_dump_wk, delay);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.h
index e9f1be9da7d4..4a5287a0c617 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.h
@@ -65,8 +65,8 @@
 
 #ifndef __mvm_fw_dbg_h__
 #define __mvm_fw_dbg_h__
-#include "iwl-fw-file.h"
-#include "iwl-fw-error-dump.h"
+#include "fw/file.h"
+#include "fw/error-dump.h"
 #include "mvm.h"
 
 void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm);
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw.c b/drivers/net/wireless/intel/iwlwifi/mvm/fw.c
index e6c9528eeeda..79e7a7a285dc 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/fw.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw.c
@@ -70,7 +70,7 @@
 
 #include "iwl-trans.h"
 #include "iwl-op-mode.h"
-#include "iwl-fw.h"
+#include "fw/img.h"
 #include "iwl-debug.h"
 #include "iwl-csr.h" /* for iwl_mvm_rx_card_state_notif */
 #include "iwl-io.h" /* for iwl_mvm_rx_card_state_notif */
@@ -385,40 +385,28 @@ static int iwl_save_fw_paging(struct iwl_mvm *mvm,
 static int iwl_send_paging_cmd(struct iwl_mvm *mvm, const struct fw_img *fw)
 {
 	struct iwl_fw_paging_cmd paging_cmd = {
-		.flags =
-			cpu_to_le32(PAGING_CMD_IS_SECURED |
-				    PAGING_CMD_IS_ENABLED |
-				    (mvm->num_of_pages_in_last_blk <<
-				    PAGING_CMD_NUM_OF_PAGES_IN_LAST_GRP_POS)),
+		.flags = cpu_to_le32(PAGING_CMD_IS_SECURED |
+				     PAGING_CMD_IS_ENABLED |
+				     (mvm->num_of_pages_in_last_blk <<
+				      PAGING_CMD_NUM_OF_PAGES_IN_LAST_GRP_POS)),
 		.block_size = cpu_to_le32(BLOCK_2_EXP_SIZE),
 		.block_num = cpu_to_le32(mvm->num_of_paging_blk),
 	};
-	int blk_idx, size = sizeof(paging_cmd);
-
-	/* A bit hard coded - but this is the old API and will be deprecated */
-	if (!iwl_mvm_has_new_tx_api(mvm))
-		size -= NUM_OF_FW_PAGING_BLOCKS * 4;
+	int blk_idx;
 
 	/* loop for for all paging blocks + CSS block */
 	for (blk_idx = 0; blk_idx < mvm->num_of_paging_blk + 1; blk_idx++) {
 		dma_addr_t addr = mvm->fw_paging_db[blk_idx].fw_paging_phys;
+		__le32 phy_addr;
 
 		addr = addr >> PAGE_2_EXP_SIZE;
-
-		if (iwl_mvm_has_new_tx_api(mvm)) {
-			__le64 phy_addr = cpu_to_le64(addr);
-
-			paging_cmd.device_phy_addr.addr64[blk_idx] = phy_addr;
-		} else {
-			__le32 phy_addr = cpu_to_le32(addr);
-
-			paging_cmd.device_phy_addr.addr32[blk_idx] = phy_addr;
-		}
+		phy_addr = cpu_to_le32(addr);
+		paging_cmd.device_phy_addr[blk_idx] = phy_addr;
 	}
 
 	return iwl_mvm_send_cmd_pdu(mvm, iwl_cmd_id(FW_PAGING_BLOCK_CMD,
 						    IWL_ALWAYS_LONG_GROUP, 0),
-				    0, size, &paging_cmd);
+				    0, sizeof(paging_cmd), &paging_cmd);
 }
 
 /*
@@ -619,7 +607,7 @@ static int iwl_mvm_load_ucode_wait_alive(struct iwl_mvm *mvm,
 	if (WARN_ON(!fw))
 		return -EINVAL;
 	mvm->cur_ucode = ucode_type;
-	mvm->ucode_loaded = false;
+	clear_bit(IWL_MVM_STATUS_FIRMWARE_RUNNING, &mvm->status);
 
 	iwl_init_notification_wait(&mvm->notif_wait, &alive_wait,
 				   alive_cmd, ARRAY_SIZE(alive_cmd),
@@ -641,12 +629,12 @@ static int iwl_mvm_load_ucode_wait_alive(struct iwl_mvm *mvm,
 	if (ret) {
 		struct iwl_trans *trans = mvm->trans;
 
-		if (trans->cfg->gen2)
+		if (trans->cfg->device_family == IWL_DEVICE_FAMILY_A000)
 			IWL_ERR(mvm,
 				"SecBoot CPU1 Status: 0x%x, CPU2 Status: 0x%x\n",
 				iwl_read_prph(trans, UMAG_SB_CPU_1_STATUS),
 				iwl_read_prph(trans, UMAG_SB_CPU_2_STATUS));
-		else if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+		else if (trans->cfg->device_family >= IWL_DEVICE_FAMILY_8000)
 			IWL_ERR(mvm,
 				"SecBoot CPU1 Status: 0x%x, CPU2 Status: 0x%x\n",
 				iwl_read_prph(trans, SB_CPU_1_STATUS),
@@ -693,7 +681,7 @@ static int iwl_mvm_load_ucode_wait_alive(struct iwl_mvm *mvm,
 	for (i = 0; i < IEEE80211_MAX_QUEUES; i++)
 		atomic_set(&mvm->mac80211_queue_stop_count[i], 0);
 
-	mvm->ucode_loaded = true;
+	set_bit(IWL_MVM_STATUS_FIRMWARE_RUNNING, &mvm->status);
 
 	return 0;
 }
@@ -738,9 +726,13 @@ static int iwl_run_unified_mvm_ucode(struct iwl_mvm *mvm, bool read_nvm)
 		goto error;
 	}
 
-	/* Read the NVM only at driver load time, no need to do this twice */
-	if (read_nvm) {
-		/* Read nvm */
+	/* Load NVM to NIC if needed */
+	if (mvm->nvm_file_name) {
+		iwl_mvm_read_external_nvm(mvm);
+		iwl_mvm_load_nvm_to_nic(mvm);
+	}
+
+	if (IWL_MVM_PARSE_NVM && read_nvm) {
 		ret = iwl_nvm_init(mvm, true);
 		if (ret) {
 			IWL_ERR(mvm, "Failed to read NVM: %d\n", ret);
@@ -748,14 +740,6 @@ static int iwl_run_unified_mvm_ucode(struct iwl_mvm *mvm, bool read_nvm)
 		}
 	}
 
-	/* In case we read the NVM from external file, load it to the NIC */
-	if (mvm->nvm_file_name)
-		iwl_mvm_load_nvm_to_nic(mvm);
-
-	ret = iwl_nvm_check_version(mvm->nvm_data, mvm->trans);
-	if (WARN_ON(ret))
-		goto error;
-
 	ret = iwl_mvm_send_cmd_pdu(mvm, WIDE_ID(REGULATORY_AND_NVM_GROUP,
 						NVM_ACCESS_COMPLETE), 0,
 				   sizeof(nvm_complete), &nvm_complete);
@@ -766,8 +750,21 @@ static int iwl_run_unified_mvm_ucode(struct iwl_mvm *mvm, bool read_nvm)
 	}
 
 	/* We wait for the INIT complete notification */
-	return iwl_wait_notification(&mvm->notif_wait, &init_wait,
-				     MVM_UCODE_ALIVE_TIMEOUT);
+	ret = iwl_wait_notification(&mvm->notif_wait, &init_wait,
+				    MVM_UCODE_ALIVE_TIMEOUT);
+	if (ret)
+		return ret;
+
+	/* Read the NVM only at driver load time, no need to do this twice */
+	if (!IWL_MVM_PARSE_NVM && read_nvm) {
+		ret = iwl_mvm_nvm_get_from_fw(mvm);
+		if (ret) {
+			IWL_ERR(mvm, "Failed to read NVM: %d\n", ret);
+			return ret;
+		}
+	}
+
+	return 0;
 
 error:
 	iwl_remove_notification(&mvm->notif_wait, &init_wait);
@@ -824,9 +821,11 @@ int iwl_run_init_mvm_ucode(struct iwl_mvm *mvm, bool read_nvm)
 		goto error;
 	}
 
-	ret = iwl_send_bt_init_conf(mvm);
-	if (ret)
-		goto error;
+	if (mvm->cfg->device_family < IWL_DEVICE_FAMILY_8000) {
+		ret = iwl_mvm_send_bt_init_conf(mvm);
+		if (ret)
+			goto error;
+	}
 
 	/* Read the NVM only at driver load time, no need to do this twice */
 	if (read_nvm) {
@@ -941,7 +940,7 @@ static void iwl_mvm_parse_shared_mem_a000(struct iwl_mvm *mvm,
 static void iwl_mvm_parse_shared_mem(struct iwl_mvm *mvm,
 				     struct iwl_rx_packet *pkt)
 {
-	struct iwl_shared_mem_cfg_v1 *mem_cfg = (void *)pkt->data;
+	struct iwl_shared_mem_cfg_v2 *mem_cfg = (void *)pkt->data;
 	int i;
 
 	mvm->smem_cfg.num_lmacs = 1;
@@ -1231,15 +1230,15 @@ out_free:
 	return ret;
 }
 
-static int iwl_mvm_sar_get_wgds_table(struct iwl_mvm *mvm,
-				      struct iwl_mvm_geo_table *geo_table)
+static int iwl_mvm_sar_get_wgds_table(struct iwl_mvm *mvm)
 {
 	union acpi_object *wifi_pkg;
 	acpi_handle root_handle;
 	acpi_handle handle;
 	struct acpi_buffer wgds = {ACPI_ALLOCATE_BUFFER, NULL};
 	acpi_status status;
-	int i, ret;
+	int i, j, ret;
+	int idx = 1;
 
 	root_handle = ACPI_HANDLE(mvm->dev);
 	if (!root_handle) {
@@ -1270,15 +1269,17 @@ static int iwl_mvm_sar_get_wgds_table(struct iwl_mvm *mvm,
 		goto out_free;
 	}
 
-	for (i = 0; i < ACPI_WGDS_WIFI_DATA_SIZE; i++) {
-		union acpi_object *entry;
+	for (i = 0; i < IWL_NUM_GEO_PROFILES; i++) {
+		for (j = 0; j < IWL_MVM_GEO_TABLE_SIZE; j++) {
+			union acpi_object *entry;
 
-		entry = &wifi_pkg->package.elements[i + 1];
-		if ((entry->type != ACPI_TYPE_INTEGER) ||
-		    (entry->integer.value > U8_MAX))
-			return -EINVAL;
+			entry = &wifi_pkg->package.elements[idx++];
+			if ((entry->type != ACPI_TYPE_INTEGER) ||
+			    (entry->integer.value > U8_MAX))
+				return -EINVAL;
 
-		geo_table->values[i] = entry->integer.value;
+			mvm->geo_profiles[i].values[j] = entry->integer.value;
+		}
 	}
 	ret = 0;
 out_free:
@@ -1339,16 +1340,47 @@ int iwl_mvm_sar_select_profile(struct iwl_mvm *mvm, int prof_a, int prof_b)
 	return iwl_mvm_send_cmd_pdu(mvm, REDUCE_TX_POWER_CMD, 0, len, &cmd);
 }
 
+int iwl_mvm_get_sar_geo_profile(struct iwl_mvm *mvm)
+{
+	struct iwl_geo_tx_power_profiles_resp *resp;
+	int ret;
+
+	struct iwl_geo_tx_power_profiles_cmd geo_cmd = {
+		.ops = cpu_to_le32(IWL_PER_CHAIN_OFFSET_GET_CURRENT_TABLE),
+	};
+	struct iwl_host_cmd cmd = {
+		.id =  WIDE_ID(PHY_OPS_GROUP, GEO_TX_POWER_LIMIT),
+		.len = { sizeof(geo_cmd), },
+		.flags = CMD_WANT_SKB,
+		.data = { &geo_cmd },
+	};
+
+	ret = iwl_mvm_send_cmd(mvm, &cmd);
+	if (ret) {
+		IWL_ERR(mvm, "Failed to get geographic profile info %d\n", ret);
+		return ret;
+	}
+
+	resp = (void *)cmd.resp_pkt->data;
+	ret = le32_to_cpu(resp->profile_idx);
+	if (WARN_ON(ret > IWL_NUM_GEO_PROFILES)) {
+		ret = -EIO;
+		IWL_WARN(mvm, "Invalid geographic profile idx (%d)\n", ret);
+	}
+
+	iwl_free_resp(&cmd);
+	return ret;
+}
+
 static int iwl_mvm_sar_geo_init(struct iwl_mvm *mvm)
 {
-	struct iwl_mvm_geo_table geo_table;
 	struct iwl_geo_tx_power_profiles_cmd cmd = {
 		.ops = cpu_to_le32(IWL_PER_CHAIN_OFFSET_SET_TABLES),
 	};
-	int ret, i, j, idx;
+	int ret, i, j;
 	u16 cmd_wide_id =  WIDE_ID(PHY_OPS_GROUP, GEO_TX_POWER_LIMIT);
 
-	ret = iwl_mvm_sar_get_wgds_table(mvm, &geo_table);
+	ret = iwl_mvm_sar_get_wgds_table(mvm);
 	if (ret < 0) {
 		IWL_DEBUG_RADIO(mvm,
 				"Geo SAR BIOS table invalid or unavailable. (%d)\n",
@@ -1369,9 +1401,8 @@ static int iwl_mvm_sar_geo_init(struct iwl_mvm *mvm)
 		for (j = 0; j < ACPI_WGDS_NUM_BANDS; j++) {
 			u8 *value;
 
-			idx = i * ACPI_WGDS_NUM_BANDS * ACPI_WGDS_TABLE_SIZE +
-				j * ACPI_WGDS_TABLE_SIZE;
-			value = &geo_table.values[idx];
+			value = &mvm->geo_profiles[i].values[j *
+				IWL_GEO_PER_CHAIN_SIZE];
 			chain[j].max_tx_power = cpu_to_le16(value[0]);
 			chain[j].chain_a = value[1];
 			chain[j].chain_b = value[2];
@@ -1501,10 +1532,6 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 	if (ret)
 		goto error;
 
-	ret = iwl_send_bt_init_conf(mvm);
-	if (ret)
-		goto error;
-
 	/* Send phy db control command and then phy db calibration*/
 	if (!iwl_mvm_has_new_tx_api(mvm)) {
 		ret = iwl_send_phy_db_data(mvm->phy_db);
@@ -1516,6 +1543,10 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 			goto error;
 	}
 
+	ret = iwl_mvm_send_bt_init_conf(mvm);
+	if (ret)
+		goto error;
+
 	/* Init RSS configuration */
 	/* TODO - remove a000 disablement when we have RXQ config API */
 	if (iwl_mvm_has_new_rx_api(mvm) && !iwl_mvm_has_new_tx_api(mvm)) {
@@ -1627,7 +1658,8 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 	IWL_DEBUG_INFO(mvm, "RT uCode started.\n");
 	return 0;
  error:
-	iwl_mvm_stop_device(mvm);
+	if (!iwlmvm_mod_params.init_dbg)
+		iwl_mvm_stop_device(mvm);
 	return ret;
 }
 
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/led.c b/drivers/net/wireless/intel/iwlwifi/mvm/led.c
index 1e51fbe95f7c..3cac4278a5fd 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/led.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/led.c
@@ -123,14 +123,17 @@ int iwl_mvm_leds_init(struct iwl_mvm *mvm)
 		return ret;
 	}
 
+	mvm->init_status |= IWL_MVM_INIT_STATUS_LEDS_INIT_COMPLETE;
 	return 0;
 }
 
 void iwl_mvm_leds_exit(struct iwl_mvm *mvm)
 {
-	if (iwlwifi_mod_params.led_mode == IWL_LED_DISABLE)
+	if (iwlwifi_mod_params.led_mode == IWL_LED_DISABLE ||
+	    !(mvm->init_status & IWL_MVM_INIT_STATUS_LEDS_INIT_COMPLETE))
 		return;
 
 	led_classdev_unregister(&mvm->led);
 	kfree(mvm->led.name);
+	mvm->init_status &= ~IWL_MVM_INIT_STATUS_LEDS_INIT_COMPLETE;
 }
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/mac-ctxt.c b/drivers/net/wireless/intel/iwlwifi/mvm/mac-ctxt.c
index fd2fc46e2fe5..dc631b23e189 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/mac-ctxt.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/mac-ctxt.c
@@ -257,7 +257,7 @@ unsigned long iwl_mvm_get_used_hw_queues(struct iwl_mvm *mvm,
 	};
 
 	if (iwl_mvm_is_dqa_supported(mvm))
-		data.used_hw_queues |= BIT(IWL_MVM_DQA_CMD_QUEUE);
+		data.used_hw_queues |= BIT(IWL_MVM_DQA_GCAST_QUEUE);
 	else
 		data.used_hw_queues |= BIT(IWL_MVM_CMD_QUEUE);
 
@@ -268,6 +268,14 @@ unsigned long iwl_mvm_get_used_hw_queues(struct iwl_mvm *mvm,
 		mvm->hw, IEEE80211_IFACE_ITER_RESUME_ALL,
 		iwl_mvm_iface_hw_queues_iter, &data);
 
+	/*
+	 * for DQA, the hw_queue in mac80211 is never really used for
+	 * real traffic (only the few queue IDs covered above), so
+	 * we can reuse the real HW queue IDs the stations use
+	 */
+	if (iwl_mvm_is_dqa_supported(mvm))
+		return data.used_hw_queues;
+
 	/* don't assign the same hw queues as TDLS stations */
 	ieee80211_iterate_stations_atomic(mvm->hw,
 					  iwl_mvm_mac_sta_hw_queues_iter,
@@ -344,7 +352,7 @@ static int iwl_mvm_mac_ctxt_allocate_resources(struct iwl_mvm *mvm,
 		.found_vif = false,
 	};
 	u32 ac;
-	int ret, i;
+	int ret, i, queue_limit;
 	unsigned long used_hw_queues;
 
 	/*
@@ -430,17 +438,29 @@ static int iwl_mvm_mac_ctxt_allocate_resources(struct iwl_mvm *mvm,
 		return 0;
 	}
 
+	if (iwl_mvm_is_dqa_supported(mvm)) {
+		/*
+		 * queues in mac80211 almost entirely independent of
+		 * the ones here - no real limit
+		 */
+		queue_limit = IEEE80211_MAX_QUEUES;
+		BUILD_BUG_ON(IEEE80211_MAX_QUEUES >
+			     BITS_PER_BYTE *
+			     sizeof(mvm->hw_queue_to_mac80211[0]));
+	} else {
+		/* need to not use too many in this case */
+		queue_limit = mvm->first_agg_queue;
+	}
+
 	/*
 	 * Find available queues, and allocate them to the ACs. When in
 	 * DQA-mode they aren't really used, and this is done only so the
 	 * mac80211 ieee80211_check_queues() function won't fail
 	 */
 	for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
-		u8 queue = find_first_zero_bit(&used_hw_queues,
-					       mvm->first_agg_queue);
+		u8 queue = find_first_zero_bit(&used_hw_queues, queue_limit);
 
-		if (!iwl_mvm_is_dqa_supported(mvm) &&
-		    queue >= mvm->first_agg_queue) {
+		if (queue >= queue_limit) {
 			IWL_ERR(mvm, "Failed to allocate queue\n");
 			ret = -EIO;
 			goto exit_fail;
@@ -846,6 +866,8 @@ static int iwl_mvm_mac_ctxt_cmd_sta(struct iwl_mvm *mvm,
 			cpu_to_le64(vif->bss_conf.sync_tsf + dtim_offs);
 		ctxt_sta->dtim_time =
 			cpu_to_le32(vif->bss_conf.sync_device_ts + dtim_offs);
+		ctxt_sta->assoc_beacon_arrive_time =
+			cpu_to_le32(vif->bss_conf.sync_device_ts);
 
 		IWL_DEBUG_INFO(mvm, "DTIM TBTT is 0x%llx/0x%x, offset %d\n",
 			       le64_to_cpu(ctxt_sta->dtim_tsf),
@@ -1457,6 +1479,7 @@ void iwl_mvm_rx_beacon_notif(struct iwl_mvm *mvm,
 
 	beacon_notify_hdr = &beacon->beacon_notify_hdr;
 	mvm->ap_last_beacon_gp2 = le32_to_cpu(beacon->gp2);
+	mvm->ibss_manager = beacon->ibss_mgr_status != 0;
 
 	agg_status = iwl_mvm_get_agg_status(mvm, beacon_notify_hdr);
 	status = le16_to_cpu(agg_status->status) & TX_STATUS_MSK;
@@ -1596,7 +1619,7 @@ void iwl_mvm_rx_stored_beacon_notif(struct iwl_mvm *mvm,
 					       rx_status.band);
 
 	/* copy the data */
-	memcpy(skb_put(skb, size), sb->data, size);
+	skb_put_data(skb, sb->data, size);
 	memcpy(IEEE80211_SKB_RXCB(skb), &rx_status, sizeof(rx_status));
 
 	/* pass it as regular rx to mac80211 */
@@ -1632,9 +1655,9 @@ void iwl_mvm_channel_switch_noa_notif(struct iwl_mvm *mvm,
 
 	IWL_DEBUG_INFO(mvm, "Channel Switch Started Notification\n");
 
-	queue_delayed_work(system_wq, &mvm->cs_tx_unblock_dwork,
-			   msecs_to_jiffies(IWL_MVM_CS_UNBLOCK_TX_TIMEOUT *
-					    csa_vif->bss_conf.beacon_int));
+	schedule_delayed_work(&mvm->cs_tx_unblock_dwork,
+			      msecs_to_jiffies(IWL_MVM_CS_UNBLOCK_TX_TIMEOUT *
+					       csa_vif->bss_conf.beacon_int));
 
 	ieee80211_csa_finish(csa_vif);
 
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/intel/iwlwifi/mvm/mac80211.c
index a67aa1f5a51c..bcde1ba0f1c8 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/mac80211.c
@@ -84,7 +84,7 @@
 #include "iwl-eeprom-parse.h"
 #include "iwl-phy-db.h"
 #include "testmode.h"
-#include "iwl-fw-error-dump.h"
+#include "fw/error-dump.h"
 #include "iwl-prph.h"
 #include "iwl-nvm-parse.h"
 #include "fw-dbg.h"
@@ -493,7 +493,7 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	 * firmware will interpret some mgmt packets, so enabling it
 	 * with software crypto isn't safe).
 	 */
-	if (!iwlwifi_mod_params.sw_crypto) {
+	if (!iwlwifi_mod_params.swcrypto) {
 		ieee80211_hw_set(hw, MFP_CAPABLE);
 		mvm->ciphers[hw->wiphy->n_cipher_suites] =
 			WLAN_CIPHER_SUITE_AES_CMAC;
@@ -687,7 +687,7 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 				     WIPHY_WOWLAN_EAP_IDENTITY_REQ |
 				     WIPHY_WOWLAN_RFKILL_RELEASE |
 				     WIPHY_WOWLAN_NET_DETECT;
-		if (!iwlwifi_mod_params.sw_crypto)
+		if (!iwlwifi_mod_params.swcrypto)
 			mvm->wowlan.flags |= WIPHY_WOWLAN_SUPPORTS_GTK_REKEY |
 					     WIPHY_WOWLAN_GTK_REKEY_FAILURE |
 					     WIPHY_WOWLAN_4WAY_HANDSHAKE;
@@ -735,6 +735,7 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	ret = ieee80211_register_hw(mvm->hw);
 	if (ret)
 		iwl_mvm_leds_exit(mvm);
+	mvm->init_status |= IWL_MVM_INIT_STATUS_REG_HW_INIT_COMPLETE;
 
 	if (mvm->cfg->vht_mu_mimo_supported)
 		wiphy_ext_feature_set(hw->wiphy,
@@ -1243,6 +1244,17 @@ static void iwl_mvm_mac_stop(struct ieee80211_hw *hw)
 	flush_work(&mvm->d0i3_exit_work);
 	flush_work(&mvm->async_handlers_wk);
 	flush_work(&mvm->add_stream_wk);
+
+	/*
+	 * Lock and clear the firmware running bit here already, so that
+	 * new commands coming in elsewhere, e.g. from debugfs, will not
+	 * be able to proceed. This is important here because one of those
+	 * debugfs files causes the fw_dump_wk to be triggered, and if we
+	 * don't stop debugfs accesses before canceling that it could be
+	 * retriggered after we flush it but before we've cleared the bit.
+	 */
+	clear_bit(IWL_MVM_STATUS_FIRMWARE_RUNNING, &mvm->status);
+
 	cancel_delayed_work_sync(&mvm->fw_dump_wk);
 	cancel_delayed_work_sync(&mvm->cs_tx_unblock_dwork);
 	cancel_delayed_work_sync(&mvm->scan_timeout_dwork);
@@ -1451,7 +1463,7 @@ static void iwl_mvm_prepare_mac_removal(struct iwl_mvm *mvm,
 {
 	u32 tfd_msk = iwl_mvm_mac_get_queues_mask(vif);
 
-	if (tfd_msk) {
+	if (tfd_msk && !iwl_mvm_is_dqa_supported(mvm)) {
 		/*
 		 * mac80211 first removes all the stations of the vif and
 		 * then removes the vif. When it removes a station it also
@@ -1460,6 +1472,8 @@ static void iwl_mvm_prepare_mac_removal(struct iwl_mvm *mvm,
 		 * of these AMPDU sessions are properly closed.
 		 * We still need to take care of the shared queues of the vif.
 		 * Flush them here.
+		 * For DQA mode there is no need - broacast and multicast queue
+		 * are flushed separately.
 		 */
 		mutex_lock(&mvm->mutex);
 		iwl_mvm_flush_tx_path(mvm, tfd_msk, 0);
@@ -1974,14 +1988,32 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 			WARN_ONCE(iwl_mvm_sf_update(mvm, vif, false),
 				  "Failed to update SF upon disassociation\n");
 
-			/* remove AP station now that the MAC is unassoc */
-			ret = iwl_mvm_rm_sta_id(mvm, vif, mvmvif->ap_sta_id);
-			if (ret)
-				IWL_ERR(mvm, "failed to remove AP station\n");
+			/*
+			 * If we get an assert during the connection (after the
+			 * station has been added, but before the vif is set
+			 * to associated), mac80211 will re-add the station and
+			 * then configure the vif. Since the vif is not
+			 * associated, we would remove the station here and
+			 * this would fail the recovery.
+			 */
+			if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART,
+				      &mvm->status)) {
+				/*
+				 * Remove AP station now that
+				 * the MAC is unassoc
+				 */
+				ret = iwl_mvm_rm_sta_id(mvm, vif,
+							mvmvif->ap_sta_id);
+				if (ret)
+					IWL_ERR(mvm,
+						"failed to remove AP station\n");
+
+				if (mvm->d0i3_ap_sta_id == mvmvif->ap_sta_id)
+					mvm->d0i3_ap_sta_id =
+						IWL_MVM_INVALID_STA;
+				mvmvif->ap_sta_id = IWL_MVM_INVALID_STA;
+			}
 
-			if (mvm->d0i3_ap_sta_id == mvmvif->ap_sta_id)
-				mvm->d0i3_ap_sta_id = IWL_MVM_INVALID_STA;
-			mvmvif->ap_sta_id = IWL_MVM_INVALID_STA;
 			/* remove quota for this interface */
 			ret = iwl_mvm_update_quotas(mvm, false, NULL);
 			if (ret)
@@ -2381,7 +2413,7 @@ static void __iwl_mvm_mac_sta_notify(struct ieee80211_hw *hw,
 
 		__set_bit(tid_data->txq_id, &txqs);
 
-		if (iwl_mvm_tid_queued(tid_data) == 0)
+		if (iwl_mvm_tid_queued(mvm, tid_data) == 0)
 			continue;
 
 		__set_bit(tid, &tids);
@@ -2856,7 +2888,7 @@ static int iwl_mvm_mac_set_key(struct ieee80211_hw *hw,
 	int ret;
 	u8 key_offset;
 
-	if (iwlwifi_mod_params.sw_crypto) {
+	if (iwlwifi_mod_params.swcrypto) {
 		IWL_DEBUG_MAC80211(mvm, "leave - hwcrypto disabled\n");
 		return -EOPNOTSUPP;
 	}
@@ -2869,7 +2901,8 @@ static int iwl_mvm_mac_set_key(struct ieee80211_hw *hw,
 	case WLAN_CIPHER_SUITE_CCMP:
 	case WLAN_CIPHER_SUITE_GCMP:
 	case WLAN_CIPHER_SUITE_GCMP_256:
-		key->flags |= IEEE80211_KEY_FLAG_PUT_IV_SPACE;
+		if (!iwl_mvm_has_new_tx_api(mvm))
+			key->flags |= IEEE80211_KEY_FLAG_PUT_IV_SPACE;
 		break;
 	case WLAN_CIPHER_SUITE_AES_CMAC:
 	case WLAN_CIPHER_SUITE_BIP_GMAC_128:
@@ -2915,8 +2948,13 @@ static int iwl_mvm_mac_set_key(struct ieee80211_hw *hw,
 				ret = -EOPNOTSUPP;
 			else
 				ret = 0;
-			key->hw_key_idx = STA_KEY_IDX_INVALID;
-			break;
+
+			if (key->cipher != WLAN_CIPHER_SUITE_GCMP &&
+			    key->cipher != WLAN_CIPHER_SUITE_GCMP_256 &&
+			    !iwl_mvm_has_new_tx_api(mvm)) {
+				key->hw_key_idx = STA_KEY_IDX_INVALID;
+				break;
+			}
 		}
 
 		/* During FW restart, in order to restore the state as it was,
@@ -3717,6 +3755,13 @@ static int iwl_mvm_switch_vif_chanctx(struct ieee80211_hw *hw,
 	return ret;
 }
 
+static int iwl_mvm_tx_last_beacon(struct ieee80211_hw *hw)
+{
+	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+
+	return mvm->ibss_manager;
+}
+
 static int iwl_mvm_set_tim(struct ieee80211_hw *hw,
 			   struct ieee80211_sta *sta,
 			   bool set)
@@ -3988,21 +4033,23 @@ static void iwl_mvm_mac_flush(struct ieee80211_hw *hw,
 		/* make sure only TDLS peers or the AP are flushed */
 		WARN_ON(i != mvmvif->ap_sta_id && !sta->tdls);
 
-		msk |= mvmsta->tfd_queue_msk;
+		if (drop) {
+			if (iwl_mvm_flush_sta(mvm, mvmsta, false, 0))
+				IWL_ERR(mvm, "flush request fail\n");
+		} else {
+			msk |= mvmsta->tfd_queue_msk;
+			if (iwl_mvm_has_new_tx_api(mvm))
+				iwl_mvm_wait_sta_queues_empty(mvm, mvmsta);
+		}
 	}
 
-	if (drop) {
-		if (iwl_mvm_flush_tx_path(mvm, msk, 0))
-			IWL_ERR(mvm, "flush request fail\n");
-		mutex_unlock(&mvm->mutex);
-	} else {
-		mutex_unlock(&mvm->mutex);
+	mutex_unlock(&mvm->mutex);
 
-		/* this can take a while, and we may need/want other operations
-		 * to succeed while doing this, so do it without the mutex held
-		 */
+	/* this can take a while, and we may need/want other operations
+	 * to succeed while doing this, so do it without the mutex held
+	 */
+	if (!drop && !iwl_mvm_has_new_tx_api(mvm))
 		iwl_trans_wait_tx_queues_empty(mvm->trans, msk);
-	}
 }
 
 static int iwl_mvm_mac_get_survey(struct ieee80211_hw *hw, int idx,
@@ -4023,7 +4070,7 @@ static int iwl_mvm_mac_get_survey(struct ieee80211_hw *hw, int idx,
 
 	mutex_lock(&mvm->mutex);
 
-	if (mvm->ucode_loaded) {
+	if (iwl_mvm_firmware_running(mvm)) {
 		ret = iwl_mvm_request_statistics(mvm, false);
 		if (ret)
 			goto out;
@@ -4104,11 +4151,11 @@ static void iwl_mvm_event_mlme_callback(struct iwl_mvm *mvm,
 					struct ieee80211_vif *vif,
 					const struct ieee80211_event *event)
 {
-#define CHECK_MLME_TRIGGER(_mvm, _trig, _buf, _cnt, _fmt...)	\
+#define CHECK_MLME_TRIGGER(_cnt, _fmt...)			\
 	do {							\
-		if ((_cnt) && --(_cnt))				\
+		if ((trig_mlme->_cnt) && --(trig_mlme->_cnt))	\
 			break;					\
-		iwl_mvm_fw_dbg_collect_trig(_mvm, _trig, _fmt);\
+		iwl_mvm_fw_dbg_collect_trig(mvm, trig, _fmt);	\
 	} while (0)
 
 	struct iwl_fw_dbg_trigger_tlv *trig;
@@ -4124,31 +4171,25 @@ static void iwl_mvm_event_mlme_callback(struct iwl_mvm *mvm,
 
 	if (event->u.mlme.data == ASSOC_EVENT) {
 		if (event->u.mlme.status == MLME_DENIED)
-			CHECK_MLME_TRIGGER(mvm, trig, buf,
-					   trig_mlme->stop_assoc_denied,
+			CHECK_MLME_TRIGGER(stop_assoc_denied,
 					   "DENIED ASSOC: reason %d",
 					    event->u.mlme.reason);
 		else if (event->u.mlme.status == MLME_TIMEOUT)
-			CHECK_MLME_TRIGGER(mvm, trig, buf,
-					   trig_mlme->stop_assoc_timeout,
+			CHECK_MLME_TRIGGER(stop_assoc_timeout,
 					   "ASSOC TIMEOUT");
 	} else if (event->u.mlme.data == AUTH_EVENT) {
 		if (event->u.mlme.status == MLME_DENIED)
-			CHECK_MLME_TRIGGER(mvm, trig, buf,
-					   trig_mlme->stop_auth_denied,
+			CHECK_MLME_TRIGGER(stop_auth_denied,
 					   "DENIED AUTH: reason %d",
 					   event->u.mlme.reason);
 		else if (event->u.mlme.status == MLME_TIMEOUT)
-			CHECK_MLME_TRIGGER(mvm, trig, buf,
-					   trig_mlme->stop_auth_timeout,
+			CHECK_MLME_TRIGGER(stop_auth_timeout,
 					   "AUTH TIMEOUT");
 	} else if (event->u.mlme.data == DEAUTH_RX_EVENT) {
-		CHECK_MLME_TRIGGER(mvm, trig, buf,
-				   trig_mlme->stop_rx_deauth,
+		CHECK_MLME_TRIGGER(stop_rx_deauth,
 				   "DEAUTH RX %d", event->u.mlme.reason);
 	} else if (event->u.mlme.data == DEAUTH_TX_EVENT) {
-		CHECK_MLME_TRIGGER(mvm, trig, buf,
-				   trig_mlme->stop_tx_deauth,
+		CHECK_MLME_TRIGGER(stop_tx_deauth,
 				   "DEAUTH TX %d", event->u.mlme.reason);
 	}
 #undef CHECK_MLME_TRIGGER
@@ -4248,11 +4289,13 @@ void iwl_mvm_sync_rx_queues_internal(struct iwl_mvm *mvm,
 		goto out;
 	}
 
-	if (notif->sync)
+	if (notif->sync) {
 		ret = wait_event_timeout(mvm->rx_sync_waitq,
-					 atomic_read(&mvm->queue_sync_counter) == 0,
+					 atomic_read(&mvm->queue_sync_counter) == 0 ||
+					 iwl_mvm_is_radio_killed(mvm),
 					 HZ);
-	WARN_ON_ONCE(!ret);
+		WARN_ON_ONCE(!ret && !iwl_mvm_is_radio_killed(mvm));
+	}
 
 out:
 	atomic_set(&mvm->queue_sync_counter, 0);
@@ -4316,6 +4359,8 @@ const struct ieee80211_ops iwl_mvm_hw_ops = {
 	.join_ibss = iwl_mvm_start_ap_ibss,
 	.leave_ibss = iwl_mvm_stop_ap_ibss,
 
+	.tx_last_beacon = iwl_mvm_tx_last_beacon,
+
 	.set_tim = iwl_mvm_set_tim,
 
 	.channel_switch = iwl_mvm_channel_switch,
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/mvm.h b/drivers/net/wireless/intel/iwlwifi/mvm/mvm.h
index 52f8d7a6a7dc..eaacfaf37206 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/mvm.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/mvm.h
@@ -79,9 +79,9 @@
 
 #include "iwl-op-mode.h"
 #include "iwl-trans.h"
-#include "iwl-notif-wait.h"
+#include "fw/notif-wait.h"
 #include "iwl-eeprom-parse.h"
-#include "iwl-fw-file.h"
+#include "fw/file.h"
 #include "iwl-config.h"
 #include "sta.h"
 #include "fw-api.h"
@@ -724,14 +724,14 @@ enum iwl_mvm_queue_status {
 #ifdef CONFIG_ACPI
 #define IWL_MVM_SAR_TABLE_SIZE		10
 #define IWL_MVM_SAR_PROFILE_NUM		4
-#define IWL_MVM_GEO_TABLE_SIZE		18
+#define IWL_MVM_GEO_TABLE_SIZE		6
 
 struct iwl_mvm_sar_profile {
 	bool enabled;
 	u8 table[IWL_MVM_SAR_TABLE_SIZE];
 };
 
-struct iwl_mvm_geo_table {
+struct iwl_mvm_geo_profile {
 	u8 values[IWL_MVM_GEO_TABLE_SIZE];
 };
 #endif
@@ -754,6 +754,8 @@ struct iwl_mvm {
 
 	struct work_struct roc_done_wk;
 
+	unsigned long init_status;
+
 	unsigned long status;
 
 	u32 queue_sync_cookie;
@@ -765,7 +767,6 @@ struct iwl_mvm {
 	struct iwl_mvm_vif *bf_allowed_vif;
 
 	enum iwl_ucode_type cur_ucode;
-	bool ucode_loaded;
 	bool hw_registered;
 	bool calibrating;
 	u32 error_event_table[2];
@@ -779,7 +780,10 @@ struct iwl_mvm {
 
 	struct iwl_notif_wait_data notif_wait;
 
-	struct mvm_statistics_rx rx_stats;
+	union {
+		struct mvm_statistics_rx_v3 rx_stats_v3;
+		struct mvm_statistics_rx rx_stats;
+	};
 
 	struct {
 		u64 rx_time;
@@ -788,7 +792,7 @@ struct iwl_mvm {
 		u64 on_time_scan;
 	} radio_stats, accu_radio_stats;
 
-	u8 hw_queue_to_mac80211[IWL_MAX_TVQM_QUEUES];
+	u16 hw_queue_to_mac80211[IWL_MAX_TVQM_QUEUES];
 
 	struct {
 		u8 hw_queue_refcount;
@@ -920,7 +924,7 @@ struct iwl_mvm {
 	u8 vif_count;
 
 	/* -1 for always, 0 for never, >0 for that many times */
-	s8 restart_fw;
+	s8 fw_restart;
 	u8 fw_dbg_conf;
 	struct delayed_work fw_dump_wk;
 	const struct iwl_mvm_dump_desc *fw_dump_desc;
@@ -1020,6 +1024,9 @@ struct iwl_mvm {
 	/* system time of last beacon (for AP/GO interface) */
 	u32 ap_last_beacon_gp2;
 
+	/* indicates that we transmitted the last beacon */
+	bool ibss_manager;
+
 	bool lar_regdom_set;
 	enum iwl_mcc_source mcc_src;
 
@@ -1067,6 +1074,7 @@ struct iwl_mvm {
 	struct delayed_work cs_tx_unblock_dwork;
 #ifdef CONFIG_ACPI
 	struct iwl_mvm_sar_profile sar_profiles[IWL_MVM_SAR_PROFILE_NUM];
+	struct iwl_mvm_geo_profile geo_profiles[IWL_NUM_GEO_PROFILES];
 #endif
 };
 
@@ -1077,6 +1085,18 @@ struct iwl_mvm {
 #define IWL_MAC80211_GET_MVM(_hw)			\
 	IWL_OP_MODE_GET_MVM((struct iwl_op_mode *)((_hw)->priv))
 
+/**
+ * enum iwl_mvm_status - MVM status bits
+ * @IWL_MVM_STATUS_HW_RFKILL: HW RF-kill is asserted
+ * @IWL_MVM_STATUS_HW_CTKILL: CT-kill is active
+ * @IWL_MVM_STATUS_ROC_RUNNING: remain-on-channel is running
+ * @IWL_MVM_STATUS_IN_HW_RESTART: HW restart is active
+ * @IWL_MVM_STATUS_IN_D0I3: NIC is in D0i3
+ * @IWL_MVM_STATUS_ROC_AUX_RUNNING: AUX remain-on-channel is running
+ * @IWL_MVM_STATUS_D3_RECONFIG: D3 reconfiguration is being done
+ * @IWL_MVM_STATUS_DUMPING_FW_LOG: FW log is being dumped
+ * @IWL_MVM_STATUS_FIRMWARE_RUNNING: firmware is running
+ */
 enum iwl_mvm_status {
 	IWL_MVM_STATUS_HW_RFKILL,
 	IWL_MVM_STATUS_HW_CTKILL,
@@ -1086,6 +1106,15 @@ enum iwl_mvm_status {
 	IWL_MVM_STATUS_ROC_AUX_RUNNING,
 	IWL_MVM_STATUS_D3_RECONFIG,
 	IWL_MVM_STATUS_DUMPING_FW_LOG,
+	IWL_MVM_STATUS_FIRMWARE_RUNNING,
+};
+
+/* Keep track of completed init configuration */
+enum iwl_mvm_init_status {
+	IWL_MVM_INIT_STATUS_THERMAL_INIT_COMPLETE = BIT(0),
+	IWL_MVM_INIT_STATUS_LEDS_INIT_COMPLETE = BIT(1),
+	IWL_MVM_INIT_STATUS_REG_HW_INIT_COMPLETE = BIT(2),
+	IWL_MVM_INIT_STATUS_TOF_INIT_COMPLETE = BIT(3),
 };
 
 static inline bool iwl_mvm_is_radio_killed(struct iwl_mvm *mvm)
@@ -1099,6 +1128,11 @@ static inline bool iwl_mvm_is_radio_hw_killed(struct iwl_mvm *mvm)
 	return test_bit(IWL_MVM_STATUS_HW_RFKILL, &mvm->status);
 }
 
+static inline bool iwl_mvm_firmware_running(struct iwl_mvm *mvm)
+{
+	return test_bit(IWL_MVM_STATUS_FIRMWARE_RUNNING, &mvm->status);
+}
+
 /* Must be called with rcu_read_lock() held and it can only be
  * released when mvmsta is not needed anymore.
  */
@@ -1188,7 +1222,7 @@ static inline bool iwl_mvm_is_lar_supported(struct iwl_mvm *mvm)
 	 * Enable LAR only if it is supported by the FW (TLV) &&
 	 * enabled in the NVM
 	 */
-	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+	if (mvm->cfg->ext_nvm)
 		return nvm_lar && tlv_lar;
 	else
 		return tlv_lar;
@@ -1266,14 +1300,19 @@ static inline bool iwl_mvm_is_cdb_supported(struct iwl_mvm *mvm)
 			   IWL_UCODE_TLV_CAPA_CDB_SUPPORT);
 }
 
-static inline struct agg_tx_status*
-iwl_mvm_get_agg_status(struct iwl_mvm *mvm,
-		       struct iwl_mvm_tx_resp *tx_resp)
+static inline bool iwl_mvm_has_new_rx_stats_api(struct iwl_mvm *mvm)
+{
+	return fw_has_api(&mvm->fw->ucode_capa,
+			  IWL_UCODE_TLV_API_NEW_RX_STATS);
+}
+
+static inline struct agg_tx_status *
+iwl_mvm_get_agg_status(struct iwl_mvm *mvm, void *tx_resp)
 {
 	if (iwl_mvm_has_new_tx_api(mvm))
-		return &tx_resp->v6.status;
+		return &((struct iwl_mvm_tx_resp *)tx_resp)->status;
 	else
-		return &tx_resp->v3.status;
+		return ((struct iwl_mvm_tx_resp_v3 *)tx_resp)->status;
 }
 
 static inline bool iwl_mvm_is_tt_in_fw(struct iwl_mvm *mvm)
@@ -1355,6 +1394,10 @@ const char *iwl_mvm_get_tx_fail_reason(u32 status);
 static inline const char *iwl_mvm_get_tx_fail_reason(u32 status) { return ""; }
 #endif
 int iwl_mvm_flush_tx_path(struct iwl_mvm *mvm, u32 tfd_msk, u32 flags);
+int iwl_mvm_flush_sta(struct iwl_mvm *mvm, void *sta, bool internal, u32 flags);
+int iwl_mvm_flush_sta_tids(struct iwl_mvm *mvm, u32 sta_id,
+			   u16 tids, u32 flags);
+
 void iwl_mvm_async_handlers_purge(struct iwl_mvm *mvm);
 
 static inline void iwl_mvm_set_tx_cmd_ccmp(struct ieee80211_tx_info *info,
@@ -1381,7 +1424,9 @@ void iwl_mvm_accu_radio_stats(struct iwl_mvm *mvm);
 
 /* NVM */
 int iwl_nvm_init(struct iwl_mvm *mvm, bool read_nvm_from_nic);
+int iwl_mvm_nvm_get_from_fw(struct iwl_mvm *mvm);
 int iwl_mvm_load_nvm_to_nic(struct iwl_mvm *mvm);
+int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm);
 
 static inline u8 iwl_mvm_get_valid_tx_ant(struct iwl_mvm *mvm)
 {
@@ -1644,7 +1689,7 @@ int iwl_mvm_exit_d0i3(struct iwl_op_mode *op_mode);
 int _iwl_mvm_exit_d0i3(struct iwl_mvm *mvm);
 
 /* BT Coex */
-int iwl_send_bt_init_conf(struct iwl_mvm *mvm);
+int iwl_mvm_send_bt_init_conf(struct iwl_mvm *mvm);
 void iwl_mvm_rx_bt_coex_notif(struct iwl_mvm *mvm,
 			      struct iwl_rx_cmd_buffer *rxb);
 void iwl_mvm_bt_rssi_event(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
@@ -1711,7 +1756,7 @@ static inline bool iwl_mvm_vif_low_latency(struct iwl_mvm_vif *mvmvif)
 }
 
 /* hw scheduler queue config */
-void iwl_mvm_enable_txq(struct iwl_mvm *mvm, int queue, int mac80211_queue,
+bool iwl_mvm_enable_txq(struct iwl_mvm *mvm, int queue, int mac80211_queue,
 			u16 ssn, const struct iwl_trans_txq_scd_cfg *cfg,
 			unsigned int wdg_timeout);
 int iwl_mvm_tvqm_enable_txq(struct iwl_mvm *mvm, int mac80211_queue,
@@ -1755,7 +1800,7 @@ static inline void iwl_mvm_stop_device(struct iwl_mvm *mvm)
 {
 	if (!iwl_mvm_has_new_tx_api(mvm))
 		iwl_free_fw_paging(mvm);
-	mvm->ucode_loaded = false;
+	clear_bit(IWL_MVM_STATUS_FIRMWARE_RUNNING, &mvm->status);
 	mvm->fw_dbg_conf = FW_DBG_INVALID;
 	iwl_trans_stop_device(mvm->trans);
 }
@@ -1854,12 +1899,19 @@ bool iwl_mvm_lqm_active(struct iwl_mvm *mvm);
 
 #ifdef CONFIG_ACPI
 int iwl_mvm_sar_select_profile(struct iwl_mvm *mvm, int prof_a, int prof_b);
+int iwl_mvm_get_sar_geo_profile(struct iwl_mvm *mvm);
 #else
 static inline
 int iwl_mvm_sar_select_profile(struct iwl_mvm *mvm, int prof_a, int prof_b)
 {
 	return -ENOENT;
 }
+
+static inline
+int iwl_mvm_get_sar_geo_profile(struct iwl_mvm *mvm)
+{
+	return -ENOENT;
+}
 #endif /* CONFIG_ACPI */
 
 #endif /* __IWL_MVM_H__ */
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/nvm.c b/drivers/net/wireless/intel/iwlwifi/mvm/nvm.c
index 283c41df622c..dac7e542a190 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/nvm.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/nvm.c
@@ -77,7 +77,7 @@
 /* Default NVM size to read */
 #define IWL_NVM_DEFAULT_CHUNK_SIZE (2*1024)
 #define IWL_MAX_NVM_SECTION_SIZE	0x1b58
-#define IWL_MAX_NVM_8000_SECTION_SIZE	0x1ffc
+#define IWL_MAX_EXT_NVM_SECTION_SIZE	0x1ffc
 
 #define NVM_WRITE_OPCODE 1
 #define NVM_READ_OPCODE 0
@@ -118,10 +118,6 @@ static int iwl_nvm_write_chunk(struct iwl_mvm *mvm, u16 section,
 		return ret;
 
 	pkt = cmd.resp_pkt;
-	if (!pkt) {
-		IWL_ERR(mvm, "Error in NVM_ACCESS response\n");
-		return -EINVAL;
-	}
 	/* Extract & check NVM write response */
 	nvm_resp = (void *)pkt->data;
 	if (le16_to_cpu(nvm_resp->status) != READ_NVM_CHUNK_SUCCEED) {
@@ -300,7 +296,7 @@ iwl_parse_nvm_sections(struct iwl_mvm *mvm)
 	bool lar_enabled;
 
 	/* Checking for required sections */
-	if (mvm->trans->cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+	if (!mvm->trans->cfg->ext_nvm) {
 		if (!mvm->nvm_sections[NVM_SECTION_TYPE_SW].data ||
 		    !mvm->nvm_sections[mvm->cfg->nvm_hw_section_num].data) {
 			IWL_ERR(mvm, "Can't parse empty OTP/NVM sections\n");
@@ -374,7 +370,7 @@ iwl_parse_nvm_sections(struct iwl_mvm *mvm)
  *
  * 4. save as "iNVM_xxx.bin" under /lib/firmware
  */
-static int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
+int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
 {
 	int ret, section_size;
 	u16 section_id;
@@ -391,19 +387,19 @@ static int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
 
 #define NVM_WORD1_LEN(x) (8 * (x & 0x03FF))
 #define NVM_WORD2_ID(x) (x >> 12)
-#define NVM_WORD2_LEN_FAMILY_8000(x) (2 * ((x & 0xFF) << 8 | x >> 8))
-#define NVM_WORD1_ID_FAMILY_8000(x) (x >> 4)
+#define EXT_NVM_WORD2_LEN(x) (2 * (((x) & 0xFF) << 8 | (x) >> 8))
+#define EXT_NVM_WORD1_ID(x) ((x) >> 4)
 #define NVM_HEADER_0	(0x2A504C54)
 #define NVM_HEADER_1	(0x4E564D2A)
 #define NVM_HEADER_SIZE	(4 * sizeof(u32))
 
 	IWL_DEBUG_EEPROM(mvm->trans->dev, "Read from external NVM\n");
 
-	/* Maximal size depends on HW family and step */
-	if (mvm->trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
+	/* Maximal size depends on NVM version */
+	if (!mvm->trans->cfg->ext_nvm)
 		max_section_size = IWL_MAX_NVM_SECTION_SIZE;
 	else
-		max_section_size = IWL_MAX_NVM_8000_SECTION_SIZE;
+		max_section_size = IWL_MAX_EXT_NVM_SECTION_SIZE;
 
 	/*
 	 * Obtain NVM image via request_firmware. Since we already used
@@ -447,10 +443,9 @@ static int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
 			 le32_to_cpu(dword_buff[3]));
 
 		/* nvm file validation, dword_buff[2] holds the file version */
-		if ((CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_C_STEP &&
-		     le32_to_cpu(dword_buff[2]) < 0xE4A) ||
-		    (CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_B_STEP &&
-		     le32_to_cpu(dword_buff[2]) >= 0xE4A)) {
+		if (mvm->trans->cfg->device_family == IWL_DEVICE_FAMILY_8000 &&
+		    CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_C_STEP &&
+		    le32_to_cpu(dword_buff[2]) < 0xE4A) {
 			ret = -EFAULT;
 			goto out;
 		}
@@ -472,14 +467,14 @@ static int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
 			break;
 		}
 
-		if (mvm->trans->cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+		if (!mvm->trans->cfg->ext_nvm) {
 			section_size =
 				2 * NVM_WORD1_LEN(le16_to_cpu(file_sec->word1));
 			section_id = NVM_WORD2_ID(le16_to_cpu(file_sec->word2));
 		} else {
-			section_size = 2 * NVM_WORD2_LEN_FAMILY_8000(
+			section_size = 2 * EXT_NVM_WORD2_LEN(
 						le16_to_cpu(file_sec->word2));
-			section_id = NVM_WORD1_ID_FAMILY_8000(
+			section_id = EXT_NVM_WORD1_ID(
 						le16_to_cpu(file_sec->word1));
 		}
 
@@ -551,12 +546,105 @@ int iwl_mvm_load_nvm_to_nic(struct iwl_mvm *mvm)
 	return ret;
 }
 
+int iwl_mvm_nvm_get_from_fw(struct iwl_mvm *mvm)
+{
+	struct iwl_nvm_get_info cmd = {};
+	struct iwl_nvm_get_info_rsp *rsp;
+	struct iwl_trans *trans = mvm->trans;
+	struct iwl_host_cmd hcmd = {
+		.flags = CMD_WANT_SKB | CMD_SEND_IN_RFKILL,
+		.data = { &cmd, },
+		.len = { sizeof(cmd) },
+		.id = WIDE_ID(REGULATORY_AND_NVM_GROUP, NVM_GET_INFO)
+	};
+	int  ret;
+	bool lar_fw_supported = !iwlwifi_mod_params.lar_disable &&
+				fw_has_capa(&mvm->fw->ucode_capa,
+					    IWL_UCODE_TLV_CAPA_LAR_SUPPORT);
+
+	lockdep_assert_held(&mvm->mutex);
+
+	ret = iwl_mvm_send_cmd(mvm, &hcmd);
+	if (ret)
+		return ret;
+
+	if (WARN(iwl_rx_packet_payload_len(hcmd.resp_pkt) != sizeof(*rsp),
+		 "Invalid payload len in NVM response from FW %d",
+		 iwl_rx_packet_payload_len(hcmd.resp_pkt))) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	rsp = (void *)hcmd.resp_pkt->data;
+	if (le32_to_cpu(rsp->general.flags)) {
+		IWL_ERR(mvm, "Invalid NVM data from FW\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	mvm->nvm_data = kzalloc(sizeof(*mvm->nvm_data) +
+				sizeof(struct ieee80211_channel) *
+				IWL_NUM_CHANNELS, GFP_KERNEL);
+	if (!mvm->nvm_data) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	iwl_set_hw_address_from_csr(trans, mvm->nvm_data);
+	/* TODO: if platform NVM has MAC address - override it here */
+
+	if (!is_valid_ether_addr(mvm->nvm_data->hw_addr)) {
+		IWL_ERR(trans, "no valid mac address was found\n");
+		ret = -EINVAL;
+		goto err_free;
+	}
+
+	IWL_INFO(trans, "base HW address: %pM\n", mvm->nvm_data->hw_addr);
+
+	/* Initialize general data */
+	mvm->nvm_data->nvm_version = le16_to_cpu(rsp->general.nvm_version);
+
+	/* Initialize MAC sku data */
+	mvm->nvm_data->sku_cap_11ac_enable =
+		le32_to_cpu(rsp->mac_sku.enable_11ac);
+	mvm->nvm_data->sku_cap_11n_enable =
+		le32_to_cpu(rsp->mac_sku.enable_11n);
+	mvm->nvm_data->sku_cap_band_24GHz_enable =
+		le32_to_cpu(rsp->mac_sku.enable_24g);
+	mvm->nvm_data->sku_cap_band_52GHz_enable =
+		le32_to_cpu(rsp->mac_sku.enable_5g);
+	mvm->nvm_data->sku_cap_mimo_disabled =
+		le32_to_cpu(rsp->mac_sku.mimo_disable);
+
+	/* Initialize PHY sku data */
+	mvm->nvm_data->valid_tx_ant = (u8)le32_to_cpu(rsp->phy_sku.tx_chains);
+	mvm->nvm_data->valid_rx_ant = (u8)le32_to_cpu(rsp->phy_sku.rx_chains);
+
+	/* Initialize regulatory data */
+	mvm->nvm_data->lar_enabled =
+		le32_to_cpu(rsp->regulatory.lar_enabled) && lar_fw_supported;
+
+	iwl_init_sbands(trans->dev, trans->cfg, mvm->nvm_data,
+			rsp->regulatory.channel_profile,
+			mvm->nvm_data->valid_tx_ant & mvm->fw->valid_tx_ant,
+			mvm->nvm_data->valid_rx_ant & mvm->fw->valid_rx_ant,
+			rsp->regulatory.lar_enabled && lar_fw_supported);
+
+	iwl_free_resp(&hcmd);
+	return 0;
+
+err_free:
+	kfree(mvm->nvm_data);
+out:
+	iwl_free_resp(&hcmd);
+	return ret;
+}
+
 int iwl_nvm_init(struct iwl_mvm *mvm, bool read_nvm_from_nic)
 {
 	int ret, section;
 	u32 size_read = 0;
 	u8 *nvm_buffer, *temp;
-	const char *nvm_file_B = mvm->cfg->default_nvm_file_B_step;
 	const char *nvm_file_C = mvm->cfg->default_nvm_file_C_step;
 
 	if (WARN_ON_ONCE(mvm->cfg->nvm_hw_section_num >= NVM_MAX_NUM_SECTIONS))
@@ -626,14 +714,7 @@ int iwl_nvm_init(struct iwl_mvm *mvm, bool read_nvm_from_nic)
 		/* read External NVM file from the mod param */
 		ret = iwl_mvm_read_external_nvm(mvm);
 		if (ret) {
-			/* choose the nvm_file name according to the
-			 * HW step
-			 */
-			if (CSR_HW_REV_STEP(mvm->trans->hw_rev) ==
-			    SILICON_B_STEP)
-				mvm->nvm_file_name = nvm_file_B;
-			else
-				mvm->nvm_file_name = nvm_file_C;
+			mvm->nvm_file_name = nvm_file_C;
 
 			if ((ret == -EFAULT || ret == -ENOENT) &&
 			    mvm->nvm_file_name) {
@@ -704,6 +785,10 @@ iwl_mvm_update_mcc(struct iwl_mvm *mvm, const char *alpha2,
 		resp_len = sizeof(struct iwl_mcc_update_resp) +
 			   n_channels * sizeof(__le32);
 		resp_cp = kmemdup(mcc_resp, resp_len, GFP_KERNEL);
+		if (!resp_cp) {
+			resp_cp = ERR_PTR(-ENOMEM);
+			goto exit;
+		}
 	} else {
 		struct iwl_mcc_update_resp_v1 *mcc_resp_v1 = (void *)pkt->data;
 
@@ -711,21 +796,18 @@ iwl_mvm_update_mcc(struct iwl_mvm *mvm, const char *alpha2,
 		resp_len = sizeof(struct iwl_mcc_update_resp) +
 			   n_channels * sizeof(__le32);
 		resp_cp = kzalloc(resp_len, GFP_KERNEL);
-
-		if (resp_cp) {
-			resp_cp->status = mcc_resp_v1->status;
-			resp_cp->mcc = mcc_resp_v1->mcc;
-			resp_cp->cap = mcc_resp_v1->cap;
-			resp_cp->source_id = mcc_resp_v1->source_id;
-			resp_cp->n_channels = mcc_resp_v1->n_channels;
-			memcpy(resp_cp->channels, mcc_resp_v1->channels,
-			       n_channels * sizeof(__le32));
+		if (!resp_cp) {
+			resp_cp = ERR_PTR(-ENOMEM);
+			goto exit;
 		}
-	}
 
-	if (!resp_cp) {
-		ret = -ENOMEM;
-		goto exit;
+		resp_cp->status = mcc_resp_v1->status;
+		resp_cp->mcc = mcc_resp_v1->mcc;
+		resp_cp->cap = mcc_resp_v1->cap;
+		resp_cp->source_id = mcc_resp_v1->source_id;
+		resp_cp->n_channels = mcc_resp_v1->n_channels;
+		memcpy(resp_cp->channels, mcc_resp_v1->channels,
+		       n_channels * sizeof(__le32));
 	}
 
 	status = le32_to_cpu(resp_cp->status);
@@ -745,8 +827,6 @@ iwl_mvm_update_mcc(struct iwl_mvm *mvm, const char *alpha2,
 
 exit:
 	iwl_free_resp(&cmd);
-	if (ret)
-		return ERR_PTR(ret);
 	return resp_cp;
 }
 
@@ -758,7 +838,7 @@ int iwl_mvm_init_mcc(struct iwl_mvm *mvm)
 	struct ieee80211_regdomain *regd;
 	char mcc[3];
 
-	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000) {
+	if (mvm->cfg->ext_nvm) {
 		tlv_lar = fw_has_capa(&mvm->fw->ucode_capa,
 				      IWL_UCODE_TLV_CAPA_LAR_SUPPORT);
 		nvm_lar = mvm->nvm_data->lar_enabled;
@@ -825,8 +905,8 @@ void iwl_mvm_rx_chub_update_mcc(struct iwl_mvm *mvm,
 	if (WARN_ON_ONCE(!iwl_mvm_is_lar_supported(mvm)))
 		return;
 
-	mcc[0] = notif->mcc >> 8;
-	mcc[1] = notif->mcc & 0xff;
+	mcc[0] = le16_to_cpu(notif->mcc) >> 8;
+	mcc[1] = le16_to_cpu(notif->mcc) & 0xff;
 	mcc[2] = '\0';
 	src = notif->source_id;
 
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/ops.c b/drivers/net/wireless/intel/iwlwifi/mvm/ops.c
index 3da5ec40aaea..4d1188b8736a 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/ops.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/ops.c
@@ -7,6 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -33,7 +34,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
- * Copyright(c) 2016 Intel Deutschland GmbH
+ * Copyright(c) 2016 - 2017 Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -67,10 +68,10 @@
 #include <linux/vmalloc.h>
 #include <net/mac80211.h>
 
-#include "iwl-notif-wait.h"
+#include "fw/notif-wait.h"
 #include "iwl-trans.h"
 #include "iwl-op-mode.h"
-#include "iwl-fw.h"
+#include "fw/img.h"
 #include "iwl-debug.h"
 #include "iwl-drv.h"
 #include "iwl-modparams.h"
@@ -172,13 +173,14 @@ static void iwl_mvm_nic_config(struct iwl_op_mode *op_mode)
 		 ~CSR_HW_IF_CONFIG_REG_MSK_PHY_TYPE);
 
 	/*
-	 * TODO: Bits 7-8 of CSR in 8000 HW family set the ADC sampling, and
-	 * shouldn't be set to any non-zero value. The same is supposed to be
-	 * true of the other HW, but unsetting them (such as the 7260) causes
-	 * automatic tests to fail on seemingly unrelated errors. Need to
-	 * further investigate this, but for now we'll separate cases.
+	 * TODO: Bits 7-8 of CSR in 8000 HW family and higher set the ADC
+	 * sampling, and shouldn't be set to any non-zero value.
+	 * The same is supposed to be true of the other HW, but unsetting
+	 * them (such as the 7260) causes automatic tests to fail on seemingly
+	 * unrelated errors. Need to further investigate this, but for now
+	 * we'll separate cases.
 	 */
-	if (mvm->trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
+	if (mvm->trans->cfg->device_family < IWL_DEVICE_FAMILY_8000)
 		reg_val |= CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI;
 
 	iwl_trans_set_bits_mask(mvm->trans, CSR_HW_IF_CONFIG_REG,
@@ -355,9 +357,6 @@ static const struct iwl_hcmd_names iwl_mvm_legacy_names[] = {
 	HCMD_NAME(SCAN_OFFLOAD_ABORT_CMD),
 	HCMD_NAME(HOT_SPOT_CMD),
 	HCMD_NAME(SCAN_OFFLOAD_PROFILES_QUERY_CMD),
-	HCMD_NAME(SCAN_OFFLOAD_HOTSPOTS_CONFIG_CMD),
-	HCMD_NAME(SCAN_OFFLOAD_HOTSPOTS_QUERY_CMD),
-	HCMD_NAME(BT_COEX_UPDATE_SW_BOOST),
 	HCMD_NAME(BT_COEX_UPDATE_CORUN_LUT),
 	HCMD_NAME(BT_COEX_UPDATE_REDUCED_TXP),
 	HCMD_NAME(BT_COEX_CI),
@@ -366,13 +365,11 @@ static const struct iwl_hcmd_names iwl_mvm_legacy_names[] = {
 	HCMD_NAME(PHY_DB_CMD),
 	HCMD_NAME(SCAN_OFFLOAD_COMPLETE),
 	HCMD_NAME(SCAN_OFFLOAD_UPDATE_PROFILES_CMD),
-	HCMD_NAME(SCAN_OFFLOAD_CONFIG_CMD),
 	HCMD_NAME(POWER_TABLE_CMD),
 	HCMD_NAME(PSM_UAPSD_AP_MISBEHAVING_NOTIFICATION),
 	HCMD_NAME(REPLY_THERMAL_MNG_BACKOFF),
 	HCMD_NAME(DC2DC_CONFIG_CMD),
 	HCMD_NAME(NVM_ACCESS_CMD),
-	HCMD_NAME(SET_CALIB_DEFAULT_CMD),
 	HCMD_NAME(BEACON_NOTIFICATION),
 	HCMD_NAME(BEACON_TEMPLATE_CMD),
 	HCMD_NAME(TX_ANT_CONFIGURATION_CMD),
@@ -381,7 +378,6 @@ static const struct iwl_hcmd_names iwl_mvm_legacy_names[] = {
 	HCMD_NAME(STATISTICS_NOTIFICATION),
 	HCMD_NAME(EOSP_NOTIFICATION),
 	HCMD_NAME(REDUCE_TX_POWER_CMD),
-	HCMD_NAME(CARD_STATE_CMD),
 	HCMD_NAME(CARD_STATE_NOTIFICATION),
 	HCMD_NAME(MISSED_BEACONS_NOTIFICATION),
 	HCMD_NAME(TDLS_CONFIG_CMD),
@@ -396,8 +392,6 @@ static const struct iwl_hcmd_names iwl_mvm_legacy_names[] = {
 	HCMD_NAME(MCC_UPDATE_CMD),
 	HCMD_NAME(MCC_CHUB_UPDATE_CMD),
 	HCMD_NAME(MARKER_CMD),
-	HCMD_NAME(BT_COEX_PRIO_TABLE),
-	HCMD_NAME(BT_COEX_PROT_ENV),
 	HCMD_NAME(BT_PROFILE_NOTIFICATION),
 	HCMD_NAME(BCAST_FILTER_CMD),
 	HCMD_NAME(MCAST_FILTER_CMD),
@@ -408,7 +402,6 @@ static const struct iwl_hcmd_names iwl_mvm_legacy_names[] = {
 	HCMD_NAME(OFFLOADS_QUERY_CMD),
 	HCMD_NAME(REMOTE_WAKE_CONFIG_CMD),
 	HCMD_NAME(MATCH_FOUND_NOTIFICATION),
-	HCMD_NAME(CMD_DTS_MEASUREMENT_TRIGGER),
 	HCMD_NAME(DTS_MEASUREMENT_NOTIFICATION),
 	HCMD_NAME(WOWLAN_PATTERNS),
 	HCMD_NAME(WOWLAN_CONFIGURATION),
@@ -416,11 +409,9 @@ static const struct iwl_hcmd_names iwl_mvm_legacy_names[] = {
 	HCMD_NAME(WOWLAN_TKIP_PARAM),
 	HCMD_NAME(WOWLAN_KEK_KCK_MATERIAL),
 	HCMD_NAME(WOWLAN_GET_STATUSES),
-	HCMD_NAME(WOWLAN_TX_POWER_PER_DB),
 	HCMD_NAME(SCAN_ITERATION_COMPLETE),
 	HCMD_NAME(D0I3_END_CMD),
 	HCMD_NAME(LTR_CONFIG),
-	HCMD_NAME(REPLY_DEBUG_CMD),
 };
 
 /* Please keep this array *SORTED* by hex value.
@@ -483,6 +474,7 @@ static const struct iwl_hcmd_names iwl_mvm_prot_offload_names[] = {
  */
 static const struct iwl_hcmd_names iwl_mvm_regulatory_and_nvm_names[] = {
 	HCMD_NAME(NVM_ACCESS_COMPLETE),
+	HCMD_NAME(NVM_GET_INFO),
 };
 
 static const struct iwl_hcmd_arr iwl_mvm_groups[] = {
@@ -588,6 +580,8 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	mvm->fw = fw;
 	mvm->hw = hw;
 
+	mvm->init_status = 0;
+
 	if (iwl_mvm_has_new_rx_api(mvm)) {
 		op_mode->ops = &iwl_mvm_ops_mq;
 		trans->rx_mpdu_cmd_hdr_size = sizeof(struct iwl_rx_mpdu_desc);
@@ -600,7 +594,7 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 			goto out_free;
 	}
 
-	mvm->restart_fw = iwlwifi_mod_params.restart_fw ? -1 : 0;
+	mvm->fw_restart = iwlwifi_mod_params.fw_restart ? -1 : 0;
 
 	if (!iwl_mvm_is_dqa_supported(mvm)) {
 		mvm->last_agg_queue = mvm->cfg->base_params->num_of_queues - 1;
@@ -608,9 +602,13 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 		if (mvm->cfg->base_params->num_of_queues == 16) {
 			mvm->aux_queue = 11;
 			mvm->first_agg_queue = 12;
+			BUILD_BUG_ON(BITS_PER_BYTE *
+				     sizeof(mvm->hw_queue_to_mac80211[0]) < 12);
 		} else {
 			mvm->aux_queue = 15;
 			mvm->first_agg_queue = 16;
+			BUILD_BUG_ON(BITS_PER_BYTE *
+				     sizeof(mvm->hw_queue_to_mac80211[0]) < 16);
 		}
 	} else {
 		mvm->aux_queue = IWL_MVM_DQA_AUX_QUEUE;
@@ -747,12 +745,12 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	mutex_lock(&mvm->mutex);
 	iwl_mvm_ref(mvm, IWL_MVM_REF_INIT_UCODE);
 	err = iwl_run_init_mvm_ucode(mvm, true);
-	if (!err || !iwlmvm_mod_params.init_dbg)
+	if (!iwlmvm_mod_params.init_dbg)
 		iwl_mvm_stop_device(mvm);
 	iwl_mvm_unref(mvm, IWL_MVM_REF_INIT_UCODE);
 	mutex_unlock(&mvm->mutex);
 	/* returns 0 if successful, 1 if success but in rfkill */
-	if (err < 0 && !iwlmvm_mod_params.init_dbg) {
+	if (err < 0) {
 		IWL_ERR(mvm, "Failed to run INIT ucode: %d\n", err);
 		goto out_free;
 	}
@@ -778,7 +776,11 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	if (err)
 		goto out_unregister;
 
-	memset(&mvm->rx_stats, 0, sizeof(struct mvm_statistics_rx));
+	if (!iwl_mvm_has_new_rx_stats_api(mvm))
+		memset(&mvm->rx_stats_v3, 0,
+		       sizeof(struct mvm_statistics_rx_v3));
+	else
+		memset(&mvm->rx_stats, 0, sizeof(struct mvm_statistics_rx));
 
 	/* The transport always starts with a taken reference, we can
 	 * release it now if d0i3 is supported */
@@ -790,12 +792,18 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	return op_mode;
 
  out_unregister:
+	if (iwlmvm_mod_params.init_dbg)
+		return op_mode;
+
 	ieee80211_unregister_hw(mvm->hw);
 	mvm->hw_registered = false;
 	iwl_mvm_leds_exit(mvm);
 	iwl_mvm_thermal_exit(mvm);
  out_free:
 	flush_delayed_work(&mvm->fw_dump_wk);
+
+	if (iwlmvm_mod_params.init_dbg)
+		return op_mode;
 	iwl_phy_db_free(mvm->phy_db);
 	kfree(mvm->scan_cmd);
 	iwl_trans_op_mode_leave(trans);
@@ -820,7 +828,10 @@ static void iwl_op_mode_mvm_stop(struct iwl_op_mode *op_mode)
 
 	iwl_mvm_thermal_exit(mvm);
 
-	ieee80211_unregister_hw(mvm->hw);
+	if (mvm->init_status & IWL_MVM_INIT_STATUS_REG_HW_INIT_COMPLETE) {
+		ieee80211_unregister_hw(mvm->hw);
+		mvm->init_status &= ~IWL_MVM_INIT_STATUS_REG_HW_INIT_COMPLETE;
+	}
 
 	kfree(mvm->scan_cmd);
 	kfree(mvm->mcast_filter_cmd);
@@ -835,7 +846,7 @@ static void iwl_op_mode_mvm_stop(struct iwl_op_mode *op_mode)
 	iwl_phy_db_free(mvm->phy_db);
 	mvm->phy_db = NULL;
 
-	iwl_free_nvm_data(mvm->nvm_data);
+	kfree(mvm->nvm_data);
 	for (i = 0; i < NVM_MAX_NUM_SECTIONS; i++)
 		kfree(mvm->nvm_sections[i].data);
 
@@ -1080,6 +1091,16 @@ static void iwl_mvm_wake_sw_queue(struct iwl_op_mode *op_mode, int hw_queue)
 	iwl_mvm_start_mac_queues(mvm, mq);
 }
 
+static void iwl_mvm_set_rfkill_state(struct iwl_mvm *mvm)
+{
+	bool state = iwl_mvm_is_radio_killed(mvm);
+
+	if (state)
+		wake_up(&mvm->rx_sync_waitq);
+
+	wiphy_rfkill_set_hw_state(mvm->hw->wiphy, state);
+}
+
 void iwl_mvm_set_hw_ctkill_state(struct iwl_mvm *mvm, bool state)
 {
 	if (state)
@@ -1087,7 +1108,7 @@ void iwl_mvm_set_hw_ctkill_state(struct iwl_mvm *mvm, bool state)
 	else
 		clear_bit(IWL_MVM_STATUS_HW_CTKILL, &mvm->status);
 
-	wiphy_rfkill_set_hw_state(mvm->hw->wiphy, iwl_mvm_is_radio_killed(mvm));
+	iwl_mvm_set_rfkill_state(mvm);
 }
 
 static bool iwl_mvm_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state)
@@ -1100,7 +1121,7 @@ static bool iwl_mvm_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state)
 	else
 		clear_bit(IWL_MVM_STATUS_HW_RFKILL, &mvm->status);
 
-	wiphy_rfkill_set_hw_state(mvm->hw->wiphy, iwl_mvm_is_radio_killed(mvm));
+	iwl_mvm_set_rfkill_state(mvm);
 
 	/* iwl_run_init_mvm_ucode is waiting for results, abort it */
 	if (calibrating)
@@ -1157,9 +1178,13 @@ static void iwl_mvm_fw_error_dump_wk(struct work_struct *work)
 
 		/* start recording again if the firmware is not crashed */
 		if (!test_bit(STATUS_FW_ERROR, &mvm->trans->status) &&
-		    mvm->fw->dbg_dest_tlv)
+		    mvm->fw->dbg_dest_tlv) {
 			iwl_clear_bits_prph(mvm->trans,
 					    MON_BUFF_SAMPLE_CTL, 0x100);
+			iwl_clear_bits_prph(mvm->trans,
+					    MON_BUFF_SAMPLE_CTL, 0x1);
+			iwl_set_bits_prph(mvm->trans, MON_BUFF_SAMPLE_CTL, 0x1);
+		}
 	} else {
 		u32 in_sample = iwl_read_prph(mvm->trans, DBGC_IN_SAMPLE);
 		u32 out_ctrl = iwl_read_prph(mvm->trans, DBGC_OUT_CTRL);
@@ -1208,7 +1233,7 @@ void iwl_mvm_nic_restart(struct iwl_mvm *mvm, bool fw_error)
 	 * If WoWLAN fw asserted, don't restart either, mac80211
 	 * can't recover this since we're already half suspended.
 	 */
-	if (!mvm->restart_fw && fw_error) {
+	if (!mvm->fw_restart && fw_error) {
 		iwl_mvm_fw_dbg_collect_desc(mvm, &iwl_mvm_dump_desc_assert,
 					    NULL);
 	} else if (test_and_set_bit(IWL_MVM_STATUS_IN_HW_RESTART,
@@ -1241,8 +1266,8 @@ void iwl_mvm_nic_restart(struct iwl_mvm *mvm, bool fw_error)
 		/* don't let the transport/FW power down */
 		iwl_mvm_ref(mvm, IWL_MVM_REF_UCODE_DOWN);
 
-		if (fw_error && mvm->restart_fw > 0)
-			mvm->restart_fw--;
+		if (fw_error && mvm->fw_restart > 0)
+			mvm->fw_restart--;
 		ieee80211_restart_hw(mvm->hw);
 	}
 }
@@ -1299,7 +1324,7 @@ static bool iwl_mvm_disallow_offloading(struct iwl_mvm *mvm,
 		 * for offloading in order to prevent reuse of the same
 		 * qos seq counters.
 		 */
-		if (iwl_mvm_tid_queued(tid_data))
+		if (iwl_mvm_tid_queued(mvm, tid_data))
 			continue;
 
 		if (tid_data->state != IWL_AGG_OFF)
@@ -1449,9 +1474,15 @@ int iwl_mvm_enter_d0i3(struct iwl_op_mode *op_mode)
 	synchronize_net();
 
 	/* Flush the hw queues, in case something got queued during entry */
-	ret = iwl_mvm_flush_tx_path(mvm, iwl_mvm_flushable_queues(mvm), flags);
-	if (ret)
-		return ret;
+	/* TODO new tx api */
+	if (iwl_mvm_has_new_tx_api(mvm)) {
+		WARN_ONCE(1, "d0i3: Need to implement flush TX queue\n");
+	} else {
+		ret = iwl_mvm_flush_tx_path(mvm, iwl_mvm_flushable_queues(mvm),
+					    flags);
+		if (ret)
+			return ret;
+	}
 
 	/* configure wowlan configuration only if needed */
 	if (mvm->d0i3_ap_sta_id != IWL_MVM_INVALID_STA) {
@@ -1597,9 +1628,6 @@ static void iwl_mvm_d0i3_exit_work(struct work_struct *wk)
 	if (ret)
 		goto out;
 
-	if (!get_status_cmd.resp_pkt)
-		goto out;
-
 	status = (void *)get_status_cmd.resp_pkt->data;
 	wakeup_reasons = le32_to_cpu(status->wakeup_reasons);
 	qos_seq = status->qos_seq_ctr;
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/phy-ctxt.c b/drivers/net/wireless/intel/iwlwifi/mvm/phy-ctxt.c
index d59efe804356..fb9eaf003ea5 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/phy-ctxt.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/phy-ctxt.c
@@ -255,8 +255,8 @@ int iwl_mvm_phy_ctxt_changed(struct iwl_mvm *mvm, struct iwl_mvm_phy_ctxt *ctxt,
 
 	lockdep_assert_held(&mvm->mutex);
 
-	/* In CDB mode we cannot modify PHY context between bands so... */
-	if (iwl_mvm_has_new_tx_api(mvm) &&
+	if (fw_has_capa(&mvm->fw->ucode_capa,
+			IWL_UCODE_TLV_CAPA_BINDING_CDB_SUPPORT) &&
 	    ctxt->channel->band != chandef->chan->band) {
 		int ret;
 
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/rs.c b/drivers/net/wireless/intel/iwlwifi/mvm/rs.c
index aa785cf3cf68..65beca3a457a 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/rs.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/rs.c
@@ -2836,7 +2836,11 @@ static void rs_initialize_lq(struct iwl_mvm *mvm,
 	rs_get_initial_rate(mvm, sta, lq_sta, band, rate);
 	rs_init_optimal_rate(mvm, sta, lq_sta);
 
-	WARN_ON_ONCE(rate->ant != ANT_A && rate->ant != ANT_B);
+	WARN_ONCE(rate->ant != ANT_A && rate->ant != ANT_B,
+		  "ant: 0x%x, chains 0x%x, fw tx ant: 0x%x, nvm tx ant: 0x%x\n",
+		  rate->ant, lq_sta->pers.chains, mvm->fw->valid_tx_ant,
+		  mvm->nvm_data ? mvm->nvm_data->valid_tx_ant : ANT_INVALID);
+
 	tbl->column = rs_get_column_from_rate(rate);
 
 	rs_set_expected_tpt_table(lq_sta, tbl);
@@ -3229,7 +3233,11 @@ static void rs_build_rates_table_from_fixed(struct iwl_mvm *mvm,
 	if (num_of_ant(ant) == 1)
 		lq_cmd->single_stream_ant_msk = ant;
 
-	lq_cmd->agg_frame_cnt_limit = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
+	if (!mvm->trans->cfg->gen2)
+		lq_cmd->agg_frame_cnt_limit = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
+	else
+		lq_cmd->agg_frame_cnt_limit =
+			LINK_QUAL_AGG_FRAME_LIMIT_GEN2_DEF;
 }
 #endif /* CONFIG_MAC80211_DEBUGFS */
 
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/rs.h b/drivers/net/wireless/intel/iwlwifi/mvm/rs.h
index 3abde1cb0303..32b4d66debea 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/rs.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/rs.h
@@ -145,6 +145,8 @@ enum {
 
 #define LINK_QUAL_AGG_FRAME_LIMIT_DEF	(63)
 #define LINK_QUAL_AGG_FRAME_LIMIT_MAX	(63)
+#define LINK_QUAL_AGG_FRAME_LIMIT_GEN2_DEF	(64)
+#define LINK_QUAL_AGG_FRAME_LIMIT_GEN2_MAX	(64)
 #define LINK_QUAL_AGG_FRAME_LIMIT_MIN	(0)
 
 #define LQ_SIZE		2	/* 2 mode tables:  "Active" and "Search" */
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/rx.c b/drivers/net/wireless/intel/iwlwifi/mvm/rx.c
index fd1dd06c4f18..622d543abb70 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/rx.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/rx.c
@@ -133,7 +133,7 @@ static void iwl_mvm_pass_packet_to_mac80211(struct iwl_mvm *mvm,
 	 */
 	hdrlen = (len <= skb_tailroom(skb)) ? len : hdrlen + crypt_len + 8;
 
-	memcpy(skb_put(skb, hdrlen), hdr, hdrlen);
+	skb_put_data(skb, hdr, hdrlen);
 	fraglen = len - hdrlen;
 
 	if (fraglen) {
@@ -504,14 +504,6 @@ void iwl_mvm_rx_rx_mpdu(struct iwl_mvm *mvm, struct napi_struct *napi,
 		iwl_mvm_unref(mvm, IWL_MVM_REF_RX);
 }
 
-static void iwl_mvm_update_rx_statistics(struct iwl_mvm *mvm,
-					 struct mvm_statistics_rx *rx_stats)
-{
-	lockdep_assert_held(&mvm->mutex);
-
-	mvm->rx_stats = *rx_stats;
-}
-
 struct iwl_mvm_stat_data {
 	struct iwl_mvm *mvm;
 	__le32 mac_id;
@@ -555,7 +547,6 @@ static void iwl_mvm_stat_iterator(void *_data, u8 *mac,
 			mvmvif->beacon_stats.avg_signal =
 				-general->beacon_average_energy[vif_id];
 		}
-
 	}
 
 	if (mvmvif->id != id)
@@ -651,7 +642,6 @@ iwl_mvm_rx_stats_check_trigger(struct iwl_mvm *mvm, struct iwl_rx_packet *pkt)
 void iwl_mvm_handle_rx_statistics(struct iwl_mvm *mvm,
 				  struct iwl_rx_packet *pkt)
 {
-	struct iwl_notif_statistics_cdb *stats = (void *)&pkt->data;
 	struct iwl_mvm_stat_data data = {
 		.mvm = mvm,
 	};
@@ -659,13 +649,16 @@ void iwl_mvm_handle_rx_statistics(struct iwl_mvm *mvm,
 	int i;
 	u8 *energy;
 	__le32 *bytes, *air_time;
+	__le32 flags;
 
-	if (iwl_mvm_is_cdb_supported(mvm))
-		expected_size = sizeof(*stats);
-	else if (iwl_mvm_has_new_rx_api(mvm))
-		expected_size = sizeof(struct iwl_notif_statistics_v11);
-	else
-		expected_size = sizeof(struct iwl_notif_statistics_v10);
+	if (!iwl_mvm_has_new_rx_stats_api(mvm)) {
+		if (iwl_mvm_has_new_rx_api(mvm))
+			expected_size = sizeof(struct iwl_notif_statistics_v11);
+		else
+			expected_size = sizeof(struct iwl_notif_statistics_v10);
+	} else {
+		expected_size = sizeof(struct iwl_notif_statistics_cdb);
+	}
 
 	if (iwl_rx_packet_payload_len(pkt) != expected_size) {
 		IWL_ERR(mvm, "received invalid statistics size (%d)!\n",
@@ -673,20 +666,49 @@ void iwl_mvm_handle_rx_statistics(struct iwl_mvm *mvm,
 		return;
 	}
 
-	data.mac_id = stats->rx.general.mac_id;
-	data.beacon_filter_average_energy =
-		stats->general.common.beacon_filter_average_energy;
+	if (!iwl_mvm_has_new_rx_stats_api(mvm)) {
+		struct iwl_notif_statistics_v11 *stats = (void *)&pkt->data;
 
-	iwl_mvm_update_rx_statistics(mvm, &stats->rx);
+		data.mac_id = stats->rx.general.mac_id;
+		data.beacon_filter_average_energy =
+			stats->general.common.beacon_filter_average_energy;
 
-	mvm->radio_stats.rx_time = le64_to_cpu(stats->general.common.rx_time);
-	mvm->radio_stats.tx_time = le64_to_cpu(stats->general.common.tx_time);
-	mvm->radio_stats.on_time_rf =
-		le64_to_cpu(stats->general.common.on_time_rf);
-	mvm->radio_stats.on_time_scan =
-		le64_to_cpu(stats->general.common.on_time_scan);
+		mvm->rx_stats_v3 = stats->rx;
 
-	data.general = &stats->general;
+		mvm->radio_stats.rx_time =
+			le64_to_cpu(stats->general.common.rx_time);
+		mvm->radio_stats.tx_time =
+			le64_to_cpu(stats->general.common.tx_time);
+		mvm->radio_stats.on_time_rf =
+			le64_to_cpu(stats->general.common.on_time_rf);
+		mvm->radio_stats.on_time_scan =
+			le64_to_cpu(stats->general.common.on_time_scan);
+
+		data.general = &stats->general;
+
+		flags = stats->flag;
+	} else {
+		struct iwl_notif_statistics_cdb *stats = (void *)&pkt->data;
+
+		data.mac_id = stats->rx.general.mac_id;
+		data.beacon_filter_average_energy =
+			stats->general.common.beacon_filter_average_energy;
+
+		mvm->rx_stats = stats->rx;
+
+		mvm->radio_stats.rx_time =
+			le64_to_cpu(stats->general.common.rx_time);
+		mvm->radio_stats.tx_time =
+			le64_to_cpu(stats->general.common.tx_time);
+		mvm->radio_stats.on_time_rf =
+			le64_to_cpu(stats->general.common.on_time_rf);
+		mvm->radio_stats.on_time_scan =
+			le64_to_cpu(stats->general.common.on_time_scan);
+
+		data.general = &stats->general;
+
+		flags = stats->flag;
+	}
 
 	iwl_mvm_rx_stats_check_trigger(mvm, pkt);
 
@@ -698,14 +720,15 @@ void iwl_mvm_handle_rx_statistics(struct iwl_mvm *mvm,
 	if (!iwl_mvm_has_new_rx_api(mvm))
 		return;
 
-	if (!iwl_mvm_is_cdb_supported(mvm)) {
-		struct iwl_notif_statistics_v11 *v11 =
-			(void *)&pkt->data;
+	if (!iwl_mvm_has_new_rx_stats_api(mvm)) {
+		struct iwl_notif_statistics_v11 *v11 = (void *)&pkt->data;
 
 		energy = (void *)&v11->load_stats.avg_energy;
 		bytes = (void *)&v11->load_stats.byte_count;
 		air_time = (void *)&v11->load_stats.air_time;
 	} else {
+		struct iwl_notif_statistics_cdb *stats = (void *)&pkt->data;
+
 		energy = (void *)&stats->load_stats.avg_energy;
 		bytes = (void *)&stats->load_stats.byte_count;
 		air_time = (void *)&stats->load_stats.air_time;
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/rxmq.c b/drivers/net/wireless/intel/iwlwifi/mvm/rxmq.c
index 966cd7543629..f3e608196369 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/rxmq.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/rxmq.c
@@ -183,9 +183,8 @@ static void iwl_mvm_create_skb(struct sk_buff *skb, struct ieee80211_hdr *hdr,
 	 * present before copying packet data.
 	 */
 	hdrlen += crypt_len;
-	memcpy(skb_put(skb, hdrlen), hdr, hdrlen);
-	memcpy(skb_put(skb, headlen - hdrlen), (u8 *)hdr + hdrlen + pad_len,
-	       headlen - hdrlen);
+	skb_put_data(skb, hdr, hdrlen);
+	skb_put_data(skb, (u8 *)hdr + hdrlen + pad_len, headlen - hdrlen);
 
 	fraglen = len - headlen;
 
@@ -503,7 +502,7 @@ void iwl_mvm_reorder_timer_expired(unsigned long data)
 			     buf->sta_id, sn);
 		iwl_mvm_release_frames(buf->mvm, sta, NULL, buf, sn);
 		rcu_read_unlock();
-	} else if (buf->num_stored) {
+	} else {
 		/*
 		 * If no frame expired and there are stored frames, index is now
 		 * pointing to the first unexpired frame - modify timer
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/scan.c b/drivers/net/wireless/intel/iwlwifi/mvm/scan.c
index 8d1b994ae79f..35e813bdfbe5 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/scan.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/scan.c
@@ -1421,8 +1421,8 @@ int iwl_mvm_reg_scan_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	mvm->scan_vif = iwl_mvm_vif_from_mac80211(vif);
 	iwl_mvm_ref(mvm, IWL_MVM_REF_SCAN);
 
-	queue_delayed_work(system_wq, &mvm->scan_timeout_dwork,
-			   msecs_to_jiffies(SCAN_TIMEOUT));
+	schedule_delayed_work(&mvm->scan_timeout_dwork,
+			      msecs_to_jiffies(SCAN_TIMEOUT));
 
 	return 0;
 }
@@ -1695,7 +1695,7 @@ void iwl_mvm_report_scan_aborted(struct iwl_mvm *mvm)
 			mvm->scan_uid_status[uid] = 0;
 		}
 		uid = iwl_mvm_scan_uid_by_status(mvm, IWL_MVM_SCAN_SCHED);
-		if (uid >= 0 && !mvm->restart_fw) {
+		if (uid >= 0 && !mvm->fw_restart) {
 			ieee80211_sched_scan_stopped(mvm->hw);
 			mvm->sched_scan_pass_all = SCHED_SCAN_PASS_ALL_DISABLED;
 			mvm->scan_uid_status[uid] = 0;
@@ -1725,7 +1725,7 @@ void iwl_mvm_report_scan_aborted(struct iwl_mvm *mvm)
 		 * restarted.
 		 */
 		if ((mvm->scan_status & IWL_MVM_SCAN_SCHED) &&
-		    !mvm->restart_fw) {
+		    !mvm->fw_restart) {
 			ieee80211_sched_scan_stopped(mvm->hw);
 			mvm->sched_scan_pass_all = SCHED_SCAN_PASS_ALL_DISABLED;
 		}
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/sta.c b/drivers/net/wireless/intel/iwlwifi/mvm/sta.c
index 614d67810d05..4df5f13fcdae 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/sta.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/sta.c
@@ -398,7 +398,7 @@ static int iwl_mvm_get_queue_agg_tids(struct iwl_mvm *mvm, int queue)
 	struct iwl_mvm_sta *mvmsta;
 	unsigned long tid_bitmap;
 	unsigned long agg_tids = 0;
-	s8 sta_id;
+	u8 sta_id;
 	int tid;
 
 	lockdep_assert_held(&mvm->mutex);
@@ -734,7 +734,6 @@ static int iwl_mvm_sta_alloc_queue_tvqm(struct iwl_mvm *mvm,
 	spin_lock_bh(&mvmsta->lock);
 	mvmsta->tid_data[tid].txq_id = queue;
 	mvmsta->tid_data[tid].is_tid_active = true;
-	mvmsta->tfd_queue_msk |= BIT(queue);
 	spin_unlock_bh(&mvmsta->lock);
 
 	return 0;
@@ -758,7 +757,7 @@ static int iwl_mvm_sta_alloc_queue(struct iwl_mvm *mvm,
 	bool using_inactive_queue = false, same_sta = false;
 	unsigned long disable_agg_tids = 0;
 	enum iwl_mvm_agg_state queue_state;
-	bool shared_queue = false;
+	bool shared_queue = false, inc_ssn;
 	int ssn;
 	unsigned long tfd_queue_mask;
 	int ret;
@@ -885,8 +884,12 @@ static int iwl_mvm_sta_alloc_queue(struct iwl_mvm *mvm,
 	}
 
 	ssn = IEEE80211_SEQ_TO_SN(le16_to_cpu(hdr->seq_ctrl));
-	iwl_mvm_enable_txq(mvm, queue, mac_queue, ssn, &cfg,
-			   wdg_timeout);
+	inc_ssn = iwl_mvm_enable_txq(mvm, queue, mac_queue,
+				     ssn, &cfg, wdg_timeout);
+	if (inc_ssn) {
+		ssn = (ssn + 1) & IEEE80211_SCTL_SEQ;
+		le16_add_cpu(&hdr->seq_ctrl, 0x10);
+	}
 
 	/*
 	 * Mark queue as shared in transport if shared
@@ -898,6 +901,13 @@ static int iwl_mvm_sta_alloc_queue(struct iwl_mvm *mvm,
 		iwl_trans_txq_set_shared_mode(mvm->trans, queue, true);
 
 	spin_lock_bh(&mvmsta->lock);
+	/*
+	 * This looks racy, but it is not. We have only one packet for
+	 * this ra/tid in our Tx path since we stop the Qdisc when we
+	 * need to allocate a new TFD queue.
+	 */
+	if (inc_ssn)
+		mvmsta->tid_data[tid].seq_number += 0x10;
 	mvmsta->tid_data[tid].txq_id = queue;
 	mvmsta->tid_data[tid].is_tid_active = true;
 	mvmsta->tfd_queue_msk |= BIT(queue);
@@ -979,7 +989,7 @@ static void iwl_mvm_unshare_queue(struct iwl_mvm *mvm, int queue)
 {
 	struct ieee80211_sta *sta;
 	struct iwl_mvm_sta *mvmsta;
-	s8 sta_id;
+	u8 sta_id;
 	int tid = -1;
 	unsigned long tid_bitmap;
 	unsigned int wdg_timeout;
@@ -1344,7 +1354,10 @@ int iwl_mvm_add_sta(struct iwl_mvm *mvm,
 	mvm_sta->mac_id_n_color = FW_CMD_ID_AND_COLOR(mvmvif->id,
 						      mvmvif->color);
 	mvm_sta->vif = vif;
-	mvm_sta->max_agg_bufsize = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
+	if (!mvm->trans->cfg->gen2)
+		mvm_sta->max_agg_bufsize = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
+	else
+		mvm_sta->max_agg_bufsize = LINK_QUAL_AGG_FRAME_LIMIT_GEN2_DEF;
 	mvm_sta->tx_protection = 0;
 	mvm_sta->tt_tx_protection = false;
 	mvm_sta->sta_type = sta->tdls ? IWL_STA_TDLS_LINK : IWL_STA_LINK;
@@ -1389,11 +1402,24 @@ int iwl_mvm_add_sta(struct iwl_mvm *mvm,
 
 	if (iwl_mvm_has_new_rx_api(mvm) &&
 	    !test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
+		int q;
+
 		dup_data = kcalloc(mvm->trans->num_rx_queues,
-				   sizeof(*dup_data),
-				   GFP_KERNEL);
+				   sizeof(*dup_data), GFP_KERNEL);
 		if (!dup_data)
 			return -ENOMEM;
+		/*
+		 * Initialize all the last_seq values to 0xffff which can never
+		 * compare equal to the frame's seq_ctrl in the check in
+		 * iwl_mvm_is_dup() since the lower 4 bits are the fragment
+		 * number and fragmented packets don't reach that function.
+		 *
+		 * This thus allows receiving a packet with seqno 0 and the
+		 * retry bit set as the very first packet on a new TID.
+		 */
+		for (q = 0; q < mvm->trans->num_rx_queues; q++)
+			memset(dup_data[q].last_seq, 0xff,
+			       sizeof(dup_data[q].last_seq));
 		mvm_sta->dup_data = dup_data;
 	}
 
@@ -1590,6 +1616,29 @@ static void iwl_mvm_disable_sta_queues(struct iwl_mvm *mvm,
 	}
 }
 
+int iwl_mvm_wait_sta_queues_empty(struct iwl_mvm *mvm,
+				  struct iwl_mvm_sta *mvm_sta)
+{
+	int i, ret;
+
+	for (i = 0; i < ARRAY_SIZE(mvm_sta->tid_data); i++) {
+		u16 txq_id;
+
+		spin_lock_bh(&mvm_sta->lock);
+		txq_id = mvm_sta->tid_data[i].txq_id;
+		spin_unlock_bh(&mvm_sta->lock);
+
+		if (txq_id == IWL_MVM_INVALID_QUEUE)
+			continue;
+
+		ret = iwl_trans_wait_txq_empty(mvm->trans, txq_id);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
 int iwl_mvm_rm_sta(struct iwl_mvm *mvm,
 		   struct ieee80211_vif *vif,
 		   struct ieee80211_sta *sta)
@@ -1611,11 +1660,17 @@ int iwl_mvm_rm_sta(struct iwl_mvm *mvm,
 		if (ret)
 			return ret;
 		/* flush its queues here since we are freeing mvm_sta */
-		ret = iwl_mvm_flush_tx_path(mvm, mvm_sta->tfd_queue_msk, 0);
+		ret = iwl_mvm_flush_sta(mvm, mvm_sta, false, 0);
 		if (ret)
 			return ret;
-		ret = iwl_trans_wait_tx_queues_empty(mvm->trans,
-						     mvm_sta->tfd_queue_msk);
+		if (iwl_mvm_has_new_tx_api(mvm)) {
+			ret = iwl_mvm_wait_sta_queues_empty(mvm, mvm_sta);
+		} else {
+			u32 q_mask = mvm_sta->tfd_queue_msk;
+
+			ret = iwl_trans_wait_tx_queues_empty(mvm->trans,
+							     q_mask);
+		}
 		if (ret)
 			return ret;
 		ret = iwl_mvm_drain_sta(mvm, mvm_sta, false);
@@ -1964,8 +2019,6 @@ int iwl_mvm_send_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 			mvm->probe_queue = queue;
 		else if (vif->type == NL80211_IFTYPE_P2P_DEVICE)
 			mvm->p2p_dev_queue = queue;
-
-		bsta->tfd_queue_msk |= BIT(queue);
 	}
 
 	return 0;
@@ -1975,27 +2028,32 @@ static void iwl_mvm_free_bcast_sta_queues(struct iwl_mvm *mvm,
 					  struct ieee80211_vif *vif)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	int queue;
 
 	lockdep_assert_held(&mvm->mutex);
 
-	if (vif->type == NL80211_IFTYPE_AP ||
-	    vif->type == NL80211_IFTYPE_ADHOC)
-		iwl_mvm_disable_txq(mvm, vif->cab_queue, vif->cab_queue,
-				    IWL_MAX_TID_COUNT, 0);
+	iwl_mvm_flush_sta(mvm, &mvmvif->bcast_sta, true, 0);
 
-	if (mvmvif->bcast_sta.tfd_queue_msk & BIT(mvm->probe_queue)) {
-		iwl_mvm_disable_txq(mvm, mvm->probe_queue,
-				    vif->hw_queue[0], IWL_MAX_TID_COUNT,
-				    0);
-		mvmvif->bcast_sta.tfd_queue_msk &= ~BIT(mvm->probe_queue);
+	switch (vif->type) {
+	case NL80211_IFTYPE_AP:
+	case NL80211_IFTYPE_ADHOC:
+		queue = mvm->probe_queue;
+		break;
+	case NL80211_IFTYPE_P2P_DEVICE:
+		queue = mvm->p2p_dev_queue;
+		break;
+	default:
+		WARN(1, "Can't free bcast queue on vif type %d\n",
+		     vif->type);
+		return;
 	}
 
-	if (mvmvif->bcast_sta.tfd_queue_msk & BIT(mvm->p2p_dev_queue)) {
-		iwl_mvm_disable_txq(mvm, mvm->p2p_dev_queue,
-				    vif->hw_queue[0], IWL_MAX_TID_COUNT,
-				    0);
-		mvmvif->bcast_sta.tfd_queue_msk &= ~BIT(mvm->p2p_dev_queue);
-	}
+	iwl_mvm_disable_txq(mvm, queue, vif->hw_queue[0], IWL_MAX_TID_COUNT, 0);
+	if (iwl_mvm_has_new_tx_api(mvm))
+		return;
+
+	WARN_ON(!(mvmvif->bcast_sta.tfd_queue_msk & BIT(queue)));
+	mvmvif->bcast_sta.tfd_queue_msk &= ~BIT(queue);
 }
 
 /* Send the FW a request to remove the station from it's internal data
@@ -2187,6 +2245,8 @@ int iwl_mvm_rm_mcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 	if (!iwl_mvm_is_dqa_supported(mvm))
 		return 0;
 
+	iwl_mvm_flush_sta(mvm, &mvmvif->mcast_sta, true, 0);
+
 	iwl_mvm_disable_txq(mvm, mvmvif->cab_queue, vif->cab_queue,
 			    IWL_MAX_TID_COUNT, 0);
 
@@ -2496,6 +2556,7 @@ int iwl_mvm_sta_tx_agg_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 {
 	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 	struct iwl_mvm_tid_data *tid_data;
+	u16 normalized_ssn;
 	int txq_id;
 	int ret;
 
@@ -2583,7 +2644,15 @@ int iwl_mvm_sta_tx_agg_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 			    mvmsta->sta_id, tid, txq_id, tid_data->ssn,
 			    tid_data->next_reclaimed);
 
-	if (tid_data->ssn == tid_data->next_reclaimed) {
+	/*
+	 * In A000 HW, the next_reclaimed index is only 8 bit, so we'll need
+	 * to align the wrap around of ssn so we compare relevant values.
+	 */
+	normalized_ssn = tid_data->ssn;
+	if (mvm->trans->cfg->gen2)
+		normalized_ssn &= 0xff;
+
+	if (normalized_ssn == tid_data->next_reclaimed) {
 		tid_data->state = IWL_AGG_STARTING;
 		ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
 	} else {
@@ -2624,7 +2693,11 @@ int iwl_mvm_sta_tx_agg_oper(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	BUILD_BUG_ON((sizeof(mvmsta->agg_tids) * BITS_PER_BYTE)
 		     != IWL_MAX_TID_COUNT);
 
-	buf_size = min_t(int, buf_size, LINK_QUAL_AGG_FRAME_LIMIT_DEF);
+	if (!mvm->trans->cfg->gen2)
+		buf_size = min_t(int, buf_size, LINK_QUAL_AGG_FRAME_LIMIT_DEF);
+	else
+		buf_size = min_t(int, buf_size,
+				 LINK_QUAL_AGG_FRAME_LIMIT_GEN2_DEF);
 
 	spin_lock_bh(&mvmsta->lock);
 	ssn = tid_data->ssn;
@@ -2781,8 +2854,13 @@ int iwl_mvm_sta_tx_agg_stop(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 				    "ssn = %d, next_recl = %d\n",
 				    tid_data->ssn, tid_data->next_reclaimed);
 
-		/* There are still packets for this RA / TID in the HW */
-		if (tid_data->ssn != tid_data->next_reclaimed) {
+		/*
+		 * There are still packets for this RA / TID in the HW.
+		 * Not relevant for DQA mode, since there is no need to disable
+		 * the queue.
+		 */
+		if (!iwl_mvm_is_dqa_supported(mvm) &&
+		    tid_data->ssn != tid_data->next_reclaimed) {
 			tid_data->state = IWL_EMPTYING_HW_QUEUE_DELBA;
 			err = 0;
 			break;
@@ -2855,10 +2933,18 @@ int iwl_mvm_sta_tx_agg_flush(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 	if (old_state >= IWL_AGG_ON) {
 		iwl_mvm_drain_sta(mvm, mvmsta, true);
-		if (iwl_mvm_flush_tx_path(mvm, BIT(txq_id), 0))
-			IWL_ERR(mvm, "Couldn't flush the AGG queue\n");
-		iwl_trans_wait_tx_queues_empty(mvm->trans,
-					       mvmsta->tfd_queue_msk);
+
+		if (iwl_mvm_has_new_tx_api(mvm)) {
+			if (iwl_mvm_flush_sta_tids(mvm, mvmsta->sta_id,
+						   BIT(tid), 0))
+				IWL_ERR(mvm, "Couldn't flush the AGG queue\n");
+			iwl_trans_wait_txq_empty(mvm->trans, txq_id);
+		} else {
+			if (iwl_mvm_flush_tx_path(mvm, BIT(txq_id), 0))
+				IWL_ERR(mvm, "Couldn't flush the AGG queue\n");
+			iwl_trans_wait_tx_queues_empty(mvm->trans, BIT(txq_id));
+		}
+
 		iwl_mvm_drain_sta(mvm, mvmsta, false);
 
 		iwl_mvm_sta_tx_agg(mvm, sta, tid, txq_id, false);
@@ -2937,7 +3023,7 @@ static struct iwl_mvm_sta *iwl_mvm_get_key_sta(struct iwl_mvm *mvm,
 }
 
 static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
-				struct iwl_mvm_sta *mvm_sta,
+				u32 sta_id,
 				struct ieee80211_key_conf *key, bool mcast,
 				u32 tkip_iv32, u16 *tkip_p1k, u32 cmd_flags,
 				u8 key_offset)
@@ -2955,6 +3041,9 @@ static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
 	bool new_api = fw_has_api(&mvm->fw->ucode_capa,
 				  IWL_UCODE_TLV_API_TKIP_MIC_KEYS);
 
+	if (sta_id == IWL_MVM_INVALID_STA)
+		return -EINVAL;
+
 	keyidx = (key->keyidx << STA_KEY_FLG_KEYID_POS) &
 		 STA_KEY_FLG_KEYID_MSK;
 	key_flags = cpu_to_le16(keyidx);
@@ -3013,7 +3102,7 @@ static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
 
 	u.cmd.common.key_offset = key_offset;
 	u.cmd.common.key_flags = key_flags;
-	u.cmd.common.sta_id = mvm_sta->sta_id;
+	u.cmd.common.sta_id = sta_id;
 
 	if (new_api) {
 		u.cmd.transmit_seq_cnt = cpu_to_le64(pn);
@@ -3146,19 +3235,37 @@ static int __iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
 				 u8 key_offset,
 				 bool mcast)
 {
-	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 	int ret;
 	const u8 *addr;
 	struct ieee80211_key_seq seq;
 	u16 p1k[5];
+	u32 sta_id;
+
+	if (sta) {
+		struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
+
+		sta_id = mvm_sta->sta_id;
+	} else if (vif->type == NL80211_IFTYPE_AP &&
+		   !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE)) {
+		struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+
+		sta_id = mvmvif->mcast_sta.sta_id;
+	} else {
+		IWL_ERR(mvm, "Failed to find station id\n");
+		return -EINVAL;
+	}
 
 	switch (keyconf->cipher) {
 	case WLAN_CIPHER_SUITE_TKIP:
+		if (vif->type == NL80211_IFTYPE_AP) {
+			ret = -EINVAL;
+			break;
+		}
 		addr = iwl_mvm_get_mac_addr(mvm, vif, sta);
 		/* get phase 1 key from mac80211 */
 		ieee80211_get_key_rx_seq(keyconf, 0, &seq);
 		ieee80211_get_tkip_rx_p1k(keyconf, addr, seq.tkip.iv32, p1k);
-		ret = iwl_mvm_send_sta_key(mvm, mvm_sta, keyconf, mcast,
+		ret = iwl_mvm_send_sta_key(mvm, sta_id, keyconf, mcast,
 					   seq.tkip.iv32, p1k, 0, key_offset);
 		break;
 	case WLAN_CIPHER_SUITE_CCMP:
@@ -3166,11 +3273,11 @@ static int __iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
 	case WLAN_CIPHER_SUITE_WEP104:
 	case WLAN_CIPHER_SUITE_GCMP:
 	case WLAN_CIPHER_SUITE_GCMP_256:
-		ret = iwl_mvm_send_sta_key(mvm, mvm_sta, keyconf, mcast,
+		ret = iwl_mvm_send_sta_key(mvm, sta_id, keyconf, mcast,
 					   0, NULL, 0, key_offset);
 		break;
 	default:
-		ret = iwl_mvm_send_sta_key(mvm, mvm_sta, keyconf, mcast,
+		ret = iwl_mvm_send_sta_key(mvm, sta_id, keyconf, mcast,
 					   0, NULL, 0, key_offset);
 	}
 
@@ -3191,6 +3298,9 @@ static int __iwl_mvm_remove_sta_key(struct iwl_mvm *mvm, u8 sta_id,
 	int ret, size;
 	u32 status;
 
+	if (sta_id == IWL_MVM_INVALID_STA)
+		return -EINVAL;
+
 	key_flags = cpu_to_le16((keyconf->keyidx << STA_KEY_FLG_KEYID_POS) &
 				 STA_KEY_FLG_KEYID_MSK);
 	key_flags |= cpu_to_le16(STA_KEY_FLG_NO_ENC | STA_KEY_FLG_WEP_KEY_MAP);
@@ -3234,42 +3344,48 @@ int iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
 {
 	bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
 	struct iwl_mvm_sta *mvm_sta;
-	u8 sta_id;
+	u8 sta_id = IWL_MVM_INVALID_STA;
 	int ret;
 	static const u8 __maybe_unused zero_addr[ETH_ALEN] = {0};
 
 	lockdep_assert_held(&mvm->mutex);
 
-	/* Get the station id from the mvm local station table */
-	mvm_sta = iwl_mvm_get_key_sta(mvm, vif, sta);
-	if (!mvm_sta) {
-		IWL_ERR(mvm, "Failed to find station\n");
-		return -EINVAL;
-	}
-	sta_id = mvm_sta->sta_id;
+	if (vif->type != NL80211_IFTYPE_AP ||
+	    keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE) {
+		/* Get the station id from the mvm local station table */
+		mvm_sta = iwl_mvm_get_key_sta(mvm, vif, sta);
+		if (!mvm_sta) {
+			IWL_ERR(mvm, "Failed to find station\n");
+			return -EINVAL;
+		}
+		sta_id = mvm_sta->sta_id;
 
-	if (keyconf->cipher == WLAN_CIPHER_SUITE_AES_CMAC ||
-	    keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_128 ||
-	    keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_256) {
-		ret = iwl_mvm_send_sta_igtk(mvm, keyconf, sta_id, false);
-		goto end;
-	}
+		if (keyconf->cipher == WLAN_CIPHER_SUITE_AES_CMAC ||
+		    keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_128 ||
+		    keyconf->cipher == WLAN_CIPHER_SUITE_BIP_GMAC_256) {
+			ret = iwl_mvm_send_sta_igtk(mvm, keyconf, sta_id,
+						    false);
+			goto end;
+		}
 
-	/*
-	 * It is possible that the 'sta' parameter is NULL, and thus
-	 * there is a need to retrieve  the sta from the local station table.
-	 */
-	if (!sta) {
-		sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
-						lockdep_is_held(&mvm->mutex));
-		if (IS_ERR_OR_NULL(sta)) {
-			IWL_ERR(mvm, "Invalid station id\n");
-			return -EINVAL;
+		/*
+		 * It is possible that the 'sta' parameter is NULL, and thus
+		 * there is a need to retrieve  the sta from the local station
+		 * table.
+		 */
+		if (!sta) {
+			sta = rcu_dereference_protected(
+				mvm->fw_id_to_mac_id[sta_id],
+				lockdep_is_held(&mvm->mutex));
+			if (IS_ERR_OR_NULL(sta)) {
+				IWL_ERR(mvm, "Invalid station id\n");
+				return -EINVAL;
+			}
 		}
-	}
 
-	if (WARN_ON_ONCE(iwl_mvm_sta_from_mac80211(sta)->vif != vif))
-		return -EINVAL;
+		if (WARN_ON_ONCE(iwl_mvm_sta_from_mac80211(sta)->vif != vif))
+			return -EINVAL;
+	}
 
 	/* If the key_offset is not pre-assigned, we need to find a
 	 * new offset to use.  In normal cases, the offset is not
@@ -3299,8 +3415,9 @@ int iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
 	 * to the same key slot (offset).
 	 * If this fails, remove the original as well.
 	 */
-	if (keyconf->cipher == WLAN_CIPHER_SUITE_WEP40 ||
-	    keyconf->cipher == WLAN_CIPHER_SUITE_WEP104) {
+	if ((keyconf->cipher == WLAN_CIPHER_SUITE_WEP40 ||
+	     keyconf->cipher == WLAN_CIPHER_SUITE_WEP104) &&
+	    sta) {
 		ret = __iwl_mvm_set_sta_key(mvm, vif, sta, keyconf,
 					    key_offset, !mcast);
 		if (ret) {
@@ -3334,6 +3451,9 @@ int iwl_mvm_remove_sta_key(struct iwl_mvm *mvm,
 	mvm_sta = iwl_mvm_get_key_sta(mvm, vif, sta);
 	if (mvm_sta)
 		sta_id = mvm_sta->sta_id;
+	else if (!sta && vif->type == NL80211_IFTYPE_AP && mcast)
+		sta_id = iwl_mvm_vif_from_mac80211(vif)->mcast_sta.sta_id;
+
 
 	IWL_DEBUG_WEP(mvm, "mvm remove dynamic key: idx=%d sta=%d\n",
 		      keyconf->keyidx, sta_id);
@@ -3356,7 +3476,7 @@ int iwl_mvm_remove_sta_key(struct iwl_mvm *mvm,
 	}
 	mvm->fw_key_deleted[keyconf->hw_key_idx] = 0;
 
-	if (!mvm_sta) {
+	if (sta && !mvm_sta) {
 		IWL_DEBUG_WEP(mvm, "station non-existent, early return.\n");
 		return 0;
 	}
@@ -3387,7 +3507,7 @@ void iwl_mvm_update_tkip_key(struct iwl_mvm *mvm,
 	mvm_sta = iwl_mvm_get_key_sta(mvm, vif, sta);
 	if (WARN_ON_ONCE(!mvm_sta))
 		goto unlock;
-	iwl_mvm_send_sta_key(mvm, mvm_sta, keyconf, mcast,
+	iwl_mvm_send_sta_key(mvm, mvm_sta->sta_id, keyconf, mcast,
 			     iv32, phase1key, CMD_ASYNC, keyconf->hw_key_idx);
 
  unlock:
@@ -3463,7 +3583,7 @@ void iwl_mvm_sta_modify_sleep_tx_count(struct iwl_mvm *mvm,
 				return;
 			}
 
-			n_queued = iwl_mvm_tid_queued(tid_data);
+			n_queued = iwl_mvm_tid_queued(mvm, tid_data);
 			if (n_queued > remaining) {
 				more_data = true;
 				remaining = 0;
@@ -3645,3 +3765,17 @@ void iwl_mvm_csa_client_absent(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 
 	rcu_read_unlock();
 }
+
+u16 iwl_mvm_tid_queued(struct iwl_mvm *mvm, struct iwl_mvm_tid_data *tid_data)
+{
+	u16 sn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
+
+	/*
+	 * In A000 HW, the next_reclaimed index is only 8 bit, so we'll need
+	 * to align the wrap around of ssn so we compare relevant values.
+	 */
+	if (mvm->trans->cfg->gen2)
+		sn &= 0xff;
+
+	return ieee80211_sn_sub(sn, tid_data->next_reclaimed);
+}
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/sta.h b/drivers/net/wireless/intel/iwlwifi/mvm/sta.h
index ad62b67dceb2..05fecbe87da4 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/sta.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/sta.h
@@ -341,12 +341,6 @@ struct iwl_mvm_tid_data {
 	bool is_tid_active;
 };
 
-static inline u16 iwl_mvm_tid_queued(struct iwl_mvm_tid_data *tid_data)
-{
-	return ieee80211_sn_sub(IEEE80211_SEQ_TO_SN(tid_data->seq_number),
-				tid_data->next_reclaimed);
-}
-
 struct iwl_mvm_key_pn {
 	struct rcu_head rcu_head;
 	struct {
@@ -447,6 +441,8 @@ struct iwl_mvm_sta {
 	u8 avg_energy;
 };
 
+u16 iwl_mvm_tid_queued(struct iwl_mvm *mvm, struct iwl_mvm_tid_data *tid_data);
+
 static inline struct iwl_mvm_sta *
 iwl_mvm_sta_from_mac80211(struct ieee80211_sta *sta)
 {
@@ -489,6 +485,8 @@ static inline int iwl_mvm_update_sta(struct iwl_mvm *mvm,
 	return iwl_mvm_sta_send_to_fw(mvm, sta, true, 0);
 }
 
+int iwl_mvm_wait_sta_queues_empty(struct iwl_mvm *mvm,
+				  struct iwl_mvm_sta *mvm_sta);
 int iwl_mvm_rm_sta(struct iwl_mvm *mvm,
 		   struct ieee80211_vif *vif,
 		   struct ieee80211_sta *sta);
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/tdls.c b/drivers/net/wireless/intel/iwlwifi/mvm/tdls.c
index df7cd87199ea..3d97436bbdf5 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/tdls.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/tdls.c
@@ -551,8 +551,7 @@ void iwl_mvm_tdls_ch_switch_work(struct work_struct *work)
 
 	/* retry after a DTIM if we failed sending now */
 	delay = TU_TO_MS(vif->bss_conf.dtim_period * vif->bss_conf.beacon_int);
-	queue_delayed_work(system_wq, &mvm->tdls_cs.dwork,
-			   msecs_to_jiffies(delay));
+	schedule_delayed_work(&mvm->tdls_cs.dwork, msecs_to_jiffies(delay));
 out:
 	mutex_unlock(&mvm->mutex);
 }
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/time-event.c b/drivers/net/wireless/intel/iwlwifi/mvm/time-event.c
index 2c12789e7550..5a682722adce 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/time-event.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/time-event.c
@@ -66,7 +66,7 @@
 #include <linux/jiffies.h>
 #include <net/mac80211.h>
 
-#include "iwl-notif-wait.h"
+#include "fw/notif-wait.h"
 #include "iwl-trans.h"
 #include "fw-api.h"
 #include "time-event.h"
@@ -130,7 +130,10 @@ void iwl_mvm_roc_done_wk(struct work_struct *wk)
 	 * issue as it will have to complete before the next command is
 	 * executed, and a new time event means a new command.
 	 */
-	iwl_mvm_flush_tx_path(mvm, queues, CMD_ASYNC);
+	if (iwl_mvm_is_dqa_supported(mvm))
+		iwl_mvm_flush_sta(mvm, &mvm->aux_sta, true, CMD_ASYNC);
+	else
+		iwl_mvm_flush_tx_path(mvm, queues, CMD_ASYNC);
 }
 
 static void iwl_mvm_roc_finished(struct iwl_mvm *mvm)
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/tof.c b/drivers/net/wireless/intel/iwlwifi/mvm/tof.c
index 16ce8a56b5b9..634175b2480c 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/tof.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/tof.c
@@ -93,17 +93,21 @@ void iwl_mvm_tof_init(struct iwl_mvm *mvm)
 		cpu_to_le32(TOF_RANGE_REQ_EXT_CMD);
 
 	mvm->tof_data.active_range_request = IWL_MVM_TOF_RANGE_REQ_MAX_ID;
+	mvm->init_status |= IWL_MVM_INIT_STATUS_TOF_INIT_COMPLETE;
 }
 
 void iwl_mvm_tof_clean(struct iwl_mvm *mvm)
 {
 	struct iwl_mvm_tof_data *tof_data = &mvm->tof_data;
 
-	if (!fw_has_capa(&mvm->fw->ucode_capa, IWL_UCODE_TLV_CAPA_TOF_SUPPORT))
+	if (!fw_has_capa(&mvm->fw->ucode_capa,
+			 IWL_UCODE_TLV_CAPA_TOF_SUPPORT) ||
+	    !(mvm->init_status & IWL_MVM_INIT_STATUS_TOF_INIT_COMPLETE))
 		return;
 
 	memset(tof_data, 0, sizeof(*tof_data));
 	mvm->tof_data.active_range_request = IWL_MVM_TOF_RANGE_REQ_MAX_ID;
+	mvm->init_status &= ~IWL_MVM_INIT_STATUS_TOF_INIT_COMPLETE;
 }
 
 static void iwl_tof_iterator(void *_data, u8 *mac,
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/tt.c b/drivers/net/wireless/intel/iwlwifi/mvm/tt.c
index 506d58104e1c..453a785a3ea5 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/tt.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/tt.c
@@ -628,7 +628,8 @@ static int iwl_mvm_tzone_get_temp(struct thermal_zone_device *device,
 
 	mutex_lock(&mvm->mutex);
 
-	if (!mvm->ucode_loaded || !(mvm->cur_ucode == IWL_UCODE_REGULAR)) {
+	if (!iwl_mvm_firmware_running(mvm) ||
+	    mvm->cur_ucode != IWL_UCODE_REGULAR) {
 		ret = -EIO;
 		goto out;
 	}
@@ -678,7 +679,8 @@ static int iwl_mvm_tzone_set_trip_temp(struct thermal_zone_device *device,
 
 	mutex_lock(&mvm->mutex);
 
-	if (!mvm->ucode_loaded || !(mvm->cur_ucode == IWL_UCODE_REGULAR)) {
+	if (!iwl_mvm_firmware_running(mvm) ||
+	    mvm->cur_ucode != IWL_UCODE_REGULAR) {
 		ret = -EIO;
 		goto out;
 	}
@@ -792,7 +794,8 @@ static int iwl_mvm_tcool_set_cur_state(struct thermal_cooling_device *cdev,
 
 	mutex_lock(&mvm->mutex);
 
-	if (!mvm->ucode_loaded || !(mvm->cur_ucode == IWL_UCODE_REGULAR)) {
+	if (!iwl_mvm_firmware_running(mvm) ||
+	    mvm->cur_ucode != IWL_UCODE_REGULAR) {
 		ret = -EIO;
 		goto unlock;
 	}
@@ -884,10 +887,14 @@ void iwl_mvm_thermal_initialize(struct iwl_mvm *mvm, u32 min_backoff)
 	iwl_mvm_cooling_device_register(mvm);
 	iwl_mvm_thermal_zone_register(mvm);
 #endif
+	mvm->init_status |= IWL_MVM_INIT_STATUS_THERMAL_INIT_COMPLETE;
 }
 
 void iwl_mvm_thermal_exit(struct iwl_mvm *mvm)
 {
+	if (!(mvm->init_status & IWL_MVM_INIT_STATUS_THERMAL_INIT_COMPLETE))
+		return;
+
 	cancel_delayed_work_sync(&mvm->thermal_throttle.ct_kill_exit);
 	IWL_DEBUG_TEMP(mvm, "Exit Thermal Throttling\n");
 
@@ -895,4 +902,5 @@ void iwl_mvm_thermal_exit(struct iwl_mvm *mvm)
 	iwl_mvm_cooling_device_unregister(mvm);
 	iwl_mvm_thermal_zone_unregister(mvm);
 #endif
+	mvm->init_status &= ~IWL_MVM_INIT_STATUS_THERMAL_INIT_COMPLETE;
 }
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/tx.c b/drivers/net/wireless/intel/iwlwifi/mvm/tx.c
index f21901cd4a4f..60360ed73f26 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/tx.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/tx.c
@@ -105,9 +105,9 @@ iwl_mvm_bar_check_trigger(struct iwl_mvm *mvm, const u8 *addr,
 
 static u16 iwl_mvm_tx_csum(struct iwl_mvm *mvm, struct sk_buff *skb,
 			   struct ieee80211_hdr *hdr,
-			   struct ieee80211_tx_info *info)
+			   struct ieee80211_tx_info *info,
+			   u16 offload_assist)
 {
-	u16 offload_assist = 0;
 #if IS_ENABLED(CONFIG_INET)
 	u16 mh_len = ieee80211_hdrlen(hdr->frame_control);
 	u8 protocol = 0;
@@ -207,6 +207,7 @@ void iwl_mvm_set_tx_cmd(struct iwl_mvm *mvm, struct sk_buff *skb,
 	__le16 fc = hdr->frame_control;
 	u32 tx_flags = le32_to_cpu(tx_cmd->tx_flags);
 	u32 len = skb->len + FCS_LEN;
+	u16 offload_assist = 0;
 	u8 ac;
 
 	if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
@@ -225,8 +226,7 @@ void iwl_mvm_set_tx_cmd(struct iwl_mvm *mvm, struct sk_buff *skb,
 		tx_cmd->tid_tspec = qc[0] & 0xf;
 		tx_flags &= ~TX_CMD_FLG_SEQ_CTL;
 		if (*qc & IEEE80211_QOS_CTL_A_MSDU_PRESENT)
-			tx_cmd->offload_assist |=
-				cpu_to_le16(BIT(TX_CMD_OFFLD_AMSDU));
+			offload_assist |= BIT(TX_CMD_OFFLD_AMSDU);
 	} else if (ieee80211_is_back_req(fc)) {
 		struct ieee80211_bar *bar = (void *)skb->data;
 		u16 control = le16_to_cpu(bar->control);
@@ -291,11 +291,12 @@ void iwl_mvm_set_tx_cmd(struct iwl_mvm *mvm, struct sk_buff *skb,
 
 	/* padding is inserted later in transport */
 	if (ieee80211_hdrlen(fc) % 4 &&
-	    !(tx_cmd->offload_assist & cpu_to_le16(BIT(TX_CMD_OFFLD_AMSDU))))
-		tx_cmd->offload_assist |= cpu_to_le16(BIT(TX_CMD_OFFLD_PAD));
+	    !(offload_assist & BIT(TX_CMD_OFFLD_AMSDU)))
+		offload_assist |= BIT(TX_CMD_OFFLD_PAD);
 
 	tx_cmd->offload_assist |=
-		cpu_to_le16(iwl_mvm_tx_csum(mvm, skb, hdr, info));
+		cpu_to_le16(iwl_mvm_tx_csum(mvm, skb, hdr, info,
+					    offload_assist));
 }
 
 static u32 iwl_mvm_get_tx_rate(struct iwl_mvm *mvm,
@@ -473,15 +474,27 @@ iwl_mvm_set_tx_params(struct iwl_mvm *mvm, struct sk_buff *skb,
 	if (unlikely(!dev_cmd))
 		return NULL;
 
-	memset(dev_cmd, 0, sizeof(*dev_cmd));
+	/* Make sure we zero enough of dev_cmd */
+	BUILD_BUG_ON(sizeof(struct iwl_tx_cmd_gen2) > sizeof(*tx_cmd));
+
+	memset(dev_cmd, 0, sizeof(dev_cmd->hdr) + sizeof(*tx_cmd));
 	dev_cmd->hdr.cmd = TX_CMD;
 
 	if (iwl_mvm_has_new_tx_api(mvm)) {
 		struct iwl_tx_cmd_gen2 *cmd = (void *)dev_cmd->payload;
-		u16 offload_assist = iwl_mvm_tx_csum(mvm, skb, hdr, info);
+		u16 offload_assist = 0;
+
+		if (ieee80211_is_data_qos(hdr->frame_control)) {
+			u8 *qc = ieee80211_get_qos_ctl(hdr);
+
+			if (*qc & IEEE80211_QOS_CTL_A_MSDU_PRESENT)
+				offload_assist |= BIT(TX_CMD_OFFLD_AMSDU);
+		}
+
+		offload_assist = iwl_mvm_tx_csum(mvm, skb, hdr, info,
+						 offload_assist);
 
 		/* padding is inserted later in transport */
-		/* FIXME - check for AMSDU may need to be removed */
 		if (ieee80211_hdrlen(hdr->frame_control) % 4 &&
 		    !(offload_assist & BIT(TX_CMD_OFFLD_AMSDU)))
 			offload_assist |= BIT(TX_CMD_OFFLD_PAD);
@@ -538,22 +551,32 @@ static void iwl_mvm_skb_prepare_status(struct sk_buff *skb,
 static int iwl_mvm_get_ctrl_vif_queue(struct iwl_mvm *mvm,
 				      struct ieee80211_tx_info *info, __le16 fc)
 {
+	struct iwl_mvm_vif *mvmvif;
+
 	if (!iwl_mvm_is_dqa_supported(mvm))
 		return info->hw_queue;
 
+	mvmvif = iwl_mvm_vif_from_mac80211(info->control.vif);
+
 	switch (info->control.vif->type) {
 	case NL80211_IFTYPE_AP:
 	case NL80211_IFTYPE_ADHOC:
 		/*
-		 * Handle legacy hostapd as well, where station may be added
-		 * only after assoc. Take care of the case where we send a
-		 * deauth to a station that we don't have.
+		 * Handle legacy hostapd as well, where station will be added
+		 * only just before sending the association response.
+		 * Also take care of the case where we send a deauth to a
+		 * station that we don't have, or similarly an association
+		 * response (with non-success status) for a station we can't
+		 * accept.
+		 * Also, disassociate frames might happen, particular with
+		 * reason 7 ("Class 3 frame received from nonassociated STA").
 		 */
 		if (ieee80211_is_probe_resp(fc) || ieee80211_is_auth(fc) ||
-		    ieee80211_is_deauth(fc))
+		    ieee80211_is_deauth(fc) || ieee80211_is_assoc_resp(fc) ||
+		    ieee80211_is_disassoc(fc))
 			return mvm->probe_queue;
 		if (info->hw_queue == info->control.vif->cab_queue)
-			return info->hw_queue;
+			return mvmvif->cab_queue;
 
 		WARN_ONCE(info->control.vif->type != NL80211_IFTYPE_ADHOC,
 			  "fc=0x%02x", le16_to_cpu(fc));
@@ -562,7 +585,7 @@ static int iwl_mvm_get_ctrl_vif_queue(struct iwl_mvm *mvm,
 		if (ieee80211_is_mgmt(fc))
 			return mvm->p2p_dev_queue;
 		if (info->hw_queue == info->control.vif->cab_queue)
-			return info->hw_queue;
+			return mvmvif->cab_queue;
 
 		WARN_ON_ONCE(1);
 		return mvm->p2p_dev_queue;
@@ -611,10 +634,6 @@ int iwl_mvm_tx_skb_non_sta(struct iwl_mvm *mvm, struct sk_buff *skb)
 	 * (this is not possible for unicast packets as a TLDS discovery
 	 * response are sent without a station entry); otherwise use the
 	 * AUX station.
-	 * In DQA mode, if vif is of type STATION and frames are not multicast
-	 * or offchannel, they should be sent from the BSS queue.
-	 * For example, TDLS setup frames should be sent on this queue,
-	 * as they go through the AP.
 	 */
 	sta_id = mvm->aux_sta.sta_id;
 	if (info.control.vif) {
@@ -629,9 +648,6 @@ int iwl_mvm_tx_skb_non_sta(struct iwl_mvm *mvm, struct sk_buff *skb)
 							   hdr->frame_control);
 			if (queue < 0)
 				return -1;
-
-			if (queue == info.control.vif->cab_queue)
-				queue = mvmvif->cab_queue;
 		} else if (info.control.vif->type == NL80211_IFTYPE_STATION &&
 			   is_multicast_ether_addr(hdr->addr1)) {
 			u8 ap_sta_id = ACCESS_ONCE(mvmvif->ap_sta_id);
@@ -639,9 +655,8 @@ int iwl_mvm_tx_skb_non_sta(struct iwl_mvm *mvm, struct sk_buff *skb)
 			if (ap_sta_id != IWL_MVM_INVALID_STA)
 				sta_id = ap_sta_id;
 		} else if (iwl_mvm_is_dqa_supported(mvm) &&
-			   info.control.vif->type == NL80211_IFTYPE_STATION &&
-			   queue != mvm->aux_queue) {
-			queue = IWL_MVM_DQA_BSS_CLIENT_QUEUE;
+			   info.control.vif->type == NL80211_IFTYPE_MONITOR) {
+			queue = mvm->aux_queue;
 		}
 	}
 
@@ -695,11 +710,6 @@ static int iwl_mvm_tx_tso(struct iwl_mvm *mvm, struct sk_buff *skb,
 	snap_ip_tcp = 8 + skb_transport_header(skb) - skb_network_header(skb) +
 		tcp_hdrlen(skb);
 
-	qc = ieee80211_get_qos_ctl(hdr);
-	tid = *qc & IEEE80211_QOS_CTL_TID_MASK;
-	if (WARN_ON_ONCE(tid >= IWL_MAX_TID_COUNT))
-		return -EINVAL;
-
 	dbg_max_amsdu_len = ACCESS_ONCE(mvm->max_amsdu_len);
 
 	if (!sta->max_amsdu_len ||
@@ -710,6 +720,11 @@ static int iwl_mvm_tx_tso(struct iwl_mvm *mvm, struct sk_buff *skb,
 		goto segment;
 	}
 
+	qc = ieee80211_get_qos_ctl(hdr);
+	tid = *qc & IEEE80211_QOS_CTL_TID_MASK;
+	if (WARN_ON_ONCE(tid >= IWL_MAX_TID_COUNT))
+		return -EINVAL;
+
 	/*
 	 * Do not build AMSDU for IPv6 with extension headers.
 	 * ask stack to segment and checkum the generated MPDUs for us.
@@ -829,11 +844,13 @@ segment:
 		if (tcp_payload_len > mss) {
 			skb_shinfo(tmp)->gso_size = mss;
 		} else {
-			qc = ieee80211_get_qos_ctl((void *)tmp->data);
+			if (ieee80211_is_data_qos(hdr->frame_control)) {
+				qc = ieee80211_get_qos_ctl((void *)tmp->data);
 
-			if (ipv4)
-				ip_send_check(ip_hdr(tmp));
-			*qc &= ~IEEE80211_QOS_CTL_A_MSDU_PRESENT;
+				if (ipv4)
+					ip_send_check(ip_hdr(tmp));
+				*qc &= ~IEEE80211_QOS_CTL_A_MSDU_PRESENT;
+			}
 			skb_shinfo(tmp)->gso_size = 0;
 		}
 
@@ -1120,13 +1137,14 @@ static void iwl_mvm_check_ratid_empty(struct iwl_mvm *mvm,
 	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 	struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
 	struct ieee80211_vif *vif = mvmsta->vif;
+	u16 normalized_ssn;
 
 	lockdep_assert_held(&mvmsta->lock);
 
 	if ((tid_data->state == IWL_AGG_ON ||
 	     tid_data->state == IWL_EMPTYING_HW_QUEUE_DELBA ||
 	     iwl_mvm_is_dqa_supported(mvm)) &&
-	    iwl_mvm_tid_queued(tid_data) == 0) {
+	    iwl_mvm_tid_queued(mvm, tid_data) == 0) {
 		/*
 		 * Now that this aggregation or DQA queue is empty tell
 		 * mac80211 so it knows we no longer have frames buffered for
@@ -1135,7 +1153,15 @@ static void iwl_mvm_check_ratid_empty(struct iwl_mvm *mvm,
 		ieee80211_sta_set_buffered(sta, tid, false);
 	}
 
-	if (tid_data->ssn != tid_data->next_reclaimed)
+	/*
+	 * In A000 HW, the next_reclaimed index is only 8 bit, so we'll need
+	 * to align the wrap around of ssn so we compare relevant values.
+	 */
+	normalized_ssn = tid_data->ssn;
+	if (mvm->trans->cfg->gen2)
+		normalized_ssn &= 0xff;
+
+	if (normalized_ssn != tid_data->next_reclaimed)
 		return;
 
 	switch (tid_data->state) {
@@ -1313,6 +1339,7 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
 	struct ieee80211_sta *sta;
 	u16 sequence = le16_to_cpu(pkt->hdr.sequence);
 	int txq_id = SEQ_TO_QUEUE(sequence);
+	/* struct iwl_mvm_tx_resp_v3 is almost the same */
 	struct iwl_mvm_tx_resp *tx_resp = (void *)pkt->data;
 	int sta_id = IWL_MVM_TX_RES_GET_RA(tx_resp->ra_tid);
 	int tid = IWL_MVM_TX_RES_GET_TID(tx_resp->ra_tid);
@@ -1330,7 +1357,7 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
 	__skb_queue_head_init(&skbs);
 
 	if (iwl_mvm_has_new_tx_api(mvm))
-		txq_id = le16_to_cpu(tx_resp->v6.tx_queue);
+		txq_id = le16_to_cpu(tx_resp->tx_queue);
 
 	seq_ctl = le16_to_cpu(tx_resp->seq_ctl);
 
@@ -1479,7 +1506,7 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
 			if (mvmsta->sleep_tx_count) {
 				mvmsta->sleep_tx_count--;
 				if (mvmsta->sleep_tx_count &&
-				    !iwl_mvm_tid_queued(tid_data)) {
+				    !iwl_mvm_tid_queued(mvm, tid_data)) {
 					/*
 					 * The number of frames in the queue
 					 * dropped to 0 even if we sent less
@@ -1791,6 +1818,7 @@ void iwl_mvm_rx_ba_notif(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb)
 	if (iwl_mvm_has_new_tx_api(mvm)) {
 		struct iwl_mvm_compressed_ba_notif *ba_res =
 			(void *)pkt->data;
+		int i;
 
 		sta_id = ba_res->sta_id;
 		ba_info.status.ampdu_ack_len = (u8)le16_to_cpu(ba_res->done);
@@ -1803,22 +1831,17 @@ void iwl_mvm_rx_ba_notif(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb)
 		if (!le16_to_cpu(ba_res->tfd_cnt))
 			goto out;
 
-		/*
-		 * TODO:
-		 * When supporting multi TID aggregations - we need to move
-		 * next_reclaimed to be per TXQ and not per TID or handle it
-		 * in a different way.
-		 * This will go together with SN and AddBA offload and cannot
-		 * be handled properly for now.
-		 */
-		WARN_ON(le16_to_cpu(ba_res->ra_tid_cnt) != 1);
-		tid = ba_res->ra_tid[0].tid;
-		if (tid == IWL_MGMT_TID)
-			tid = IWL_MAX_TID_COUNT;
-		iwl_mvm_tx_reclaim(mvm, sta_id, tid,
-				   (int)(le16_to_cpu(ba_res->tfd[0].q_num)),
-				   le16_to_cpu(ba_res->tfd[0].tfd_index),
-				   &ba_info, le32_to_cpu(ba_res->tx_rate));
+		/* Free per TID */
+		for (i = 0; i < le16_to_cpu(ba_res->tfd_cnt); i++) {
+			struct iwl_mvm_compressed_ba_tfd *ba_tfd =
+				&ba_res->tfd[i];
+
+			iwl_mvm_tx_reclaim(mvm, sta_id, ba_tfd->tid,
+					   (int)(le16_to_cpu(ba_tfd->q_num)),
+					   le16_to_cpu(ba_tfd->tfd_index),
+					   &ba_info,
+					   le32_to_cpu(ba_res->tx_rate));
+		}
 
 out:
 		IWL_DEBUG_TX_REPLY(mvm,
@@ -1860,7 +1883,7 @@ out:
 
 	IWL_DEBUG_TX_REPLY(mvm,
 			   "BA_NOTIFICATION Received from %pM, sta_id = %d\n",
-			   (u8 *)&ba_notif->sta_addr_lo32, ba_notif->sta_id);
+			   ba_notif->sta_addr, ba_notif->sta_id);
 
 	IWL_DEBUG_TX_REPLY(mvm,
 			   "TID = %d, SeqCtl = %d, bitmap = 0x%llx, scd_flow = %d, scd_ssn = %d sent:%d, acked:%d\n",
@@ -1883,14 +1906,55 @@ out:
 int iwl_mvm_flush_tx_path(struct iwl_mvm *mvm, u32 tfd_msk, u32 flags)
 {
 	int ret;
-	struct iwl_tx_path_flush_cmd flush_cmd = {
+	struct iwl_tx_path_flush_cmd_v1 flush_cmd = {
 		.queues_ctl = cpu_to_le32(tfd_msk),
 		.flush_ctl = cpu_to_le16(DUMP_TX_FIFO_FLUSH),
 	};
 
+	WARN_ON(iwl_mvm_has_new_tx_api(mvm));
+
+	ret = iwl_mvm_send_cmd_pdu(mvm, TXPATH_FLUSH, flags,
+				   sizeof(flush_cmd), &flush_cmd);
+	if (ret)
+		IWL_ERR(mvm, "Failed to send flush command (%d)\n", ret);
+	return ret;
+}
+
+int iwl_mvm_flush_sta_tids(struct iwl_mvm *mvm, u32 sta_id,
+			   u16 tids, u32 flags)
+{
+	int ret;
+	struct iwl_tx_path_flush_cmd flush_cmd = {
+		.sta_id = cpu_to_le32(sta_id),
+		.tid_mask = cpu_to_le16(tids),
+	};
+
+	WARN_ON(!iwl_mvm_has_new_tx_api(mvm));
+
 	ret = iwl_mvm_send_cmd_pdu(mvm, TXPATH_FLUSH, flags,
 				   sizeof(flush_cmd), &flush_cmd);
 	if (ret)
 		IWL_ERR(mvm, "Failed to send flush command (%d)\n", ret);
 	return ret;
 }
+
+int iwl_mvm_flush_sta(struct iwl_mvm *mvm, void *sta, bool internal, u32 flags)
+{
+	struct iwl_mvm_int_sta *int_sta = sta;
+	struct iwl_mvm_sta *mvm_sta = sta;
+
+	if (iwl_mvm_has_new_tx_api(mvm)) {
+		if (internal)
+			return iwl_mvm_flush_sta_tids(mvm, int_sta->sta_id,
+						      BIT(IWL_MGMT_TID), flags);
+
+		return iwl_mvm_flush_sta_tids(mvm, mvm_sta->sta_id,
+					      0xFF, flags);
+	}
+
+	if (internal)
+		return iwl_mvm_flush_tx_path(mvm, int_sta->tfd_queue_msk,
+					     flags);
+
+	return iwl_mvm_flush_tx_path(mvm, mvm_sta->tfd_queue_msk, flags);
+}
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/utils.c b/drivers/net/wireless/intel/iwlwifi/mvm/utils.c
index 8f4f176e204e..fc5a490880d0 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/utils.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/utils.c
@@ -69,6 +69,7 @@
 #include "iwl-debug.h"
 #include "iwl-io.h"
 #include "iwl-prph.h"
+#include "iwl-csr.h"
 #include "fw-dbg.h"
 #include "mvm.h"
 #include "fw-api-rs.h"
@@ -168,11 +169,6 @@ int iwl_mvm_send_cmd_status(struct iwl_mvm *mvm, struct iwl_host_cmd *cmd,
 	}
 
 	pkt = cmd->resp_pkt;
-	/* Can happen if RFKILL is asserted */
-	if (!pkt) {
-		ret = 0;
-		goto out_free_resp;
-	}
 
 	resp_len = iwl_rx_packet_payload_len(pkt);
 	if (WARN_ON_ONCE(resp_len != sizeof(*resp))) {
@@ -502,6 +498,7 @@ static void iwl_mvm_dump_lmac_error_log(struct iwl_mvm *mvm, u32 base)
 {
 	struct iwl_trans *trans = mvm->trans;
 	struct iwl_error_event_table table;
+	u32 val;
 
 	if (mvm->cur_ucode == IWL_UCODE_INIT) {
 		if (!base)
@@ -520,6 +517,36 @@ static void iwl_mvm_dump_lmac_error_log(struct iwl_mvm *mvm, u32 base)
 		return;
 	}
 
+	/* check if there is a HW error */
+	val = iwl_trans_read_mem32(trans, base);
+	if (((val & ~0xf) == 0xa5a5a5a0) || ((val & ~0xf) == 0x5a5a5a50)) {
+		int err;
+
+		IWL_ERR(trans, "HW error, resetting before reading\n");
+
+		/* reset the device */
+		iwl_set_bit(trans, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
+		usleep_range(5000, 6000);
+
+		/* set INIT_DONE flag */
+		iwl_set_bit(trans, CSR_GP_CNTRL,
+			    CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+
+		/* and wait for clock stabilization */
+		if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+			udelay(2);
+
+		err = 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 (err < 0) {
+			IWL_DEBUG_INFO(trans,
+				       "Failed to reset the card for the dump\n");
+			return;
+		}
+	}
+
 	iwl_trans_read_mem_bytes(trans, base, &table, sizeof(table));
 
 	if (ERROR_START_OFFSET <= table.valid * ERROR_ELEM_SIZE) {
@@ -671,7 +698,13 @@ static bool iwl_mvm_update_txq_mapping(struct iwl_mvm *mvm, int queue,
 	if (mvm->queue_info[queue].hw_queue_refcount > 0)
 		enable_queue = false;
 
-	mvm->hw_queue_to_mac80211[queue] |= BIT(mac80211_queue);
+	if (mac80211_queue != IEEE80211_INVAL_HW_QUEUE) {
+		WARN(mac80211_queue >=
+		     BITS_PER_BYTE * sizeof(mvm->hw_queue_to_mac80211[0]),
+		     "cannot track mac80211 queue %d (queue %d, sta %d, tid %d)\n",
+		     mac80211_queue, queue, sta_id, tid);
+		mvm->hw_queue_to_mac80211[queue] |= BIT(mac80211_queue);
+	}
 
 	mvm->queue_info[queue].hw_queue_refcount++;
 	mvm->queue_info[queue].tid_bitmap |= BIT(tid);
@@ -730,35 +763,39 @@ int iwl_mvm_tvqm_enable_txq(struct iwl_mvm *mvm, int mac80211_queue,
 	return queue;
 }
 
-void iwl_mvm_enable_txq(struct iwl_mvm *mvm, int queue, int mac80211_queue,
+bool iwl_mvm_enable_txq(struct iwl_mvm *mvm, int queue, int mac80211_queue,
 			u16 ssn, const struct iwl_trans_txq_scd_cfg *cfg,
 			unsigned int wdg_timeout)
 {
+	struct iwl_scd_txq_cfg_cmd cmd = {
+		.scd_queue = queue,
+		.action = SCD_CFG_ENABLE_QUEUE,
+		.window = cfg->frame_limit,
+		.sta_id = cfg->sta_id,
+		.ssn = cpu_to_le16(ssn),
+		.tx_fifo = cfg->fifo,
+		.aggregate = cfg->aggregate,
+		.tid = cfg->tid,
+	};
+	bool inc_ssn;
+
 	if (WARN_ON(iwl_mvm_has_new_tx_api(mvm)))
-		return;
+		return false;
 
 	/* Send the enabling command if we need to */
-	if (iwl_mvm_update_txq_mapping(mvm, queue, mac80211_queue,
-				       cfg->sta_id, cfg->tid)) {
-		struct iwl_scd_txq_cfg_cmd cmd = {
-			.scd_queue = queue,
-			.action = SCD_CFG_ENABLE_QUEUE,
-			.window = cfg->frame_limit,
-			.sta_id = cfg->sta_id,
-			.ssn = cpu_to_le16(ssn),
-			.tx_fifo = cfg->fifo,
-			.aggregate = cfg->aggregate,
-			.tid = cfg->tid,
-		};
-
-		iwl_trans_txq_enable_cfg(mvm->trans, queue, ssn, NULL,
-					 wdg_timeout);
-		WARN(iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0,
-					  sizeof(struct iwl_scd_txq_cfg_cmd),
-					  &cmd),
-		     "Failed to configure queue %d on FIFO %d\n", queue,
-		     cfg->fifo);
-	}
+	if (!iwl_mvm_update_txq_mapping(mvm, queue, mac80211_queue,
+					cfg->sta_id, cfg->tid))
+		return false;
+
+	inc_ssn = iwl_trans_txq_enable_cfg(mvm->trans, queue, ssn,
+					   NULL, wdg_timeout);
+	if (inc_ssn)
+		le16_add_cpu(&cmd.ssn, 1);
+
+	WARN(iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0, sizeof(cmd), &cmd),
+	     "Failed to configure queue %d on FIFO %d\n", queue, cfg->fifo);
+
+	return inc_ssn;
 }
 
 int iwl_mvm_disable_txq(struct iwl_mvm *mvm, int queue, int mac80211_queue,
@@ -1186,7 +1223,11 @@ static void iwl_mvm_remove_inactive_tids(struct iwl_mvm *mvm,
 	/* Go over all non-active TIDs, incl. IWL_MAX_TID_COUNT (for mgmt) */
 	for_each_set_bit(tid, &tid_bitmap, IWL_MAX_TID_COUNT + 1) {
 		/* If some TFDs are still queued - don't mark TID as inactive */
-		if (iwl_mvm_tid_queued(&mvmsta->tid_data[tid]))
+		if (iwl_mvm_tid_queued(mvm, &mvmsta->tid_data[tid]))
+			tid_bitmap &= ~BIT(tid);
+
+		/* Don't mark as inactive any TID that has an active BA */
+		if (mvmsta->tid_data[tid].state != IWL_AGG_OFF)
 			tid_bitmap &= ~BIT(tid);
 	}
 
diff --git a/drivers/net/wireless/intel/iwlwifi/pcie/ctxt-info.c b/drivers/net/wireless/intel/iwlwifi/pcie/ctxt-info.c
index b1f43397bb59..eddaca76d514 100644
--- a/drivers/net/wireless/intel/iwlwifi/pcie/ctxt-info.c
+++ b/drivers/net/wireless/intel/iwlwifi/pcie/ctxt-info.c
@@ -103,6 +103,7 @@ static void iwl_pcie_ctxt_info_free_fw_img(struct iwl_trans *trans)
 
 	kfree(dram->fw);
 	dram->fw_cnt = 0;
+	dram->fw = NULL;
 }
 
 void iwl_pcie_ctxt_info_free_paging(struct iwl_trans *trans)
@@ -124,6 +125,7 @@ void iwl_pcie_ctxt_info_free_paging(struct iwl_trans *trans)
 
 	kfree(dram->paging);
 	dram->paging_cnt = 0;
+	dram->paging = NULL;
 }
 
 static int iwl_pcie_ctxt_info_init_fw_sec(struct iwl_trans *trans,
@@ -135,6 +137,11 @@ static int iwl_pcie_ctxt_info_init_fw_sec(struct iwl_trans *trans,
 	struct iwl_context_info_dram *ctxt_dram = &ctxt_info->dram;
 	int i, ret, lmac_cnt, umac_cnt, paging_cnt;
 
+	if (WARN(dram->paging,
+		 "paging shouldn't already be initialized (%d pages)\n",
+		 dram->paging_cnt))
+		iwl_pcie_ctxt_info_free_paging(trans);
+
 	lmac_cnt = iwl_pcie_get_num_sections(fw, 0);
 	/* add 1 due to separator */
 	umac_cnt = iwl_pcie_get_num_sections(fw, lmac_cnt + 1);
diff --git a/drivers/net/wireless/intel/iwlwifi/pcie/drv.c b/drivers/net/wireless/intel/iwlwifi/pcie/drv.c
index e51760e752d4..f16c1bb9bf94 100644
--- a/drivers/net/wireless/intel/iwlwifi/pcie/drv.c
+++ b/drivers/net/wireless/intel/iwlwifi/pcie/drv.c
@@ -7,7 +7,7 @@
  *
  * Copyright(c) 2007 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
- * Copyright(c) 2016 Intel Deutschland GmbH
+ * Copyright(c) 2016-2017 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -35,6 +35,7 @@
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
+ * Copyright(c) 2017 Intel Deutschland GmbH
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -535,10 +536,27 @@ static const struct pci_device_id iwl_hw_card_ids[] = {
 	{IWL_PCI_DEVICE(0x31DC, 0x0030, iwl9560_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x2526, 0x1030, iwl9560_2ac_cfg)},
 	{IWL_PCI_DEVICE(0xA370, 0x1030, iwl9560_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x9DF0, 0x0034, iwl9560_2ac_cfg)},
+	{IWL_PCI_DEVICE(0xA370, 0x0034, iwl9560_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x31DC, 0x0034, iwl9560_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x2526, 0x0038, iwl9560_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x2526, 0x003C, iwl9560_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x9DF0, 0x0038, iwl9560_2ac_cfg)},
+	{IWL_PCI_DEVICE(0xA370, 0x0038, iwl9560_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x31DC, 0x0038, iwl9560_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x9DF0, 0x003C, iwl9560_2ac_cfg)},
+	{IWL_PCI_DEVICE(0xA370, 0x003C, iwl9560_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x31DC, 0x003C, iwl9560_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x2526, 0x0034, iwl9560_2ac_cfg)},
 
 /* a000 Series */
 	{IWL_PCI_DEVICE(0x2720, 0x0A10, iwla000_2ac_cfg_hr_cdb)},
-	{IWL_PCI_DEVICE(0x2722, 0x0A10, iwla000_2ac_cfg_hr)},
+	{IWL_PCI_DEVICE(0x34F0, 0x0310, iwla000_2ac_cfg_jf)},
+	{IWL_PCI_DEVICE(0x2720, 0x0000, iwla000_2ax_cfg_hr)},
+	{IWL_PCI_DEVICE(0x34F0, 0x0070, iwla000_2ax_cfg_hr)},
+	{IWL_PCI_DEVICE(0x2720, 0x0078, iwla000_2ax_cfg_hr)},
+	{IWL_PCI_DEVICE(0x2720, 0x0070, iwla000_2ax_cfg_hr)},
+	{IWL_PCI_DEVICE(0x2720, 0x1080, iwla000_2ax_cfg_hr)},
 #endif /* CONFIG_IWLMVM */
 
 	{0}
@@ -672,10 +690,12 @@ static int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		iwl_trans->cfg = cfg_7265d;
 	}
 
-	if (iwl_trans->cfg->rf_id &&
-	    (cfg == &iwla000_2ac_cfg_hr || cfg == &iwla000_2ac_cfg_hr_cdb) &&
-	     iwl_trans->hw_rf_id == CSR_HW_RF_ID_TYPE_JF) {
-		cfg = &iwla000_2ac_cfg_jf;
+	if (iwl_trans->cfg->rf_id && cfg == &iwla000_2ac_cfg_hr_cdb) {
+		if (iwl_trans->hw_rf_id == CSR_HW_RF_ID_TYPE_JF)
+			cfg = &iwla000_2ac_cfg_jf;
+		else if (iwl_trans->hw_rf_id == CSR_HW_RF_ID_TYPE_HR)
+			cfg = &iwla000_2ac_cfg_hr;
+
 		iwl_trans->cfg = cfg;
 	}
 #endif
@@ -703,7 +723,7 @@ static int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		pm_runtime_set_active(&pdev->dev);
 
 		pm_runtime_set_autosuspend_delay(&pdev->dev,
-					 iwlwifi_mod_params.d0i3_entry_delay);
+					 iwlwifi_mod_params.d0i3_timeout);
 		pm_runtime_use_autosuspend(&pdev->dev);
 
 		/* We are not supposed to call pm_runtime_allow() by
@@ -764,7 +784,6 @@ static int iwl_pci_resume(struct device *device)
 	struct pci_dev *pdev = to_pci_dev(device);
 	struct iwl_trans *trans = pci_get_drvdata(pdev);
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-	bool hw_rfkill;
 
 	/* Before you put code here, think about WoWLAN. You cannot check here
 	 * whether WoWLAN is enabled or not, and your code will run even if
@@ -780,17 +799,17 @@ static int iwl_pci_resume(struct device *device)
 	if (!trans->op_mode)
 		return 0;
 
+	/* reconfigure the MSI-X mapping to get the correct IRQ for rfkill */
+	iwl_pcie_conf_msix_hw(trans_pcie);
+
 	/*
-	 * Enable rfkill interrupt (in order to keep track of
-	 * the rfkill status). Must be locked to avoid processing
-	 * a possible rfkill interrupt between reading the state
-	 * and calling iwl_trans_pcie_rf_kill() with it.
+	 * Enable rfkill interrupt (in order to keep track of the rfkill
+	 * status). Must be locked to avoid processing a possible rfkill
+	 * interrupt while in iwl_trans_check_hw_rf_kill().
 	 */
 	mutex_lock(&trans_pcie->mutex);
 	iwl_enable_rfkill_int(trans);
-
-	hw_rfkill = iwl_is_rfkill_set(trans);
-	iwl_trans_pcie_rf_kill(trans, hw_rfkill);
+	iwl_trans_check_hw_rf_kill(trans);
 	mutex_unlock(&trans_pcie->mutex);
 
 	return 0;
diff --git a/drivers/net/wireless/intel/iwlwifi/pcie/internal.h b/drivers/net/wireless/intel/iwlwifi/pcie/internal.h
index fd4faaaa1484..fa315d84e98e 100644
--- a/drivers/net/wireless/intel/iwlwifi/pcie/internal.h
+++ b/drivers/net/wireless/intel/iwlwifi/pcie/internal.h
@@ -403,7 +403,8 @@ struct iwl_trans_pcie {
 	dma_addr_t ict_tbl_dma;
 	int ict_index;
 	bool use_ict;
-	bool is_down;
+	bool is_down, opmode_down;
+	bool debug_rfkill;
 	struct isr_statistics isr_stats;
 
 	spinlock_t irq_lock;
@@ -515,7 +516,7 @@ int iwl_pcie_gen2_tx_init(struct iwl_trans *trans);
 void iwl_pcie_tx_start(struct iwl_trans *trans, u32 scd_base_addr);
 int iwl_pcie_tx_stop(struct iwl_trans *trans);
 void iwl_pcie_tx_free(struct iwl_trans *trans);
-void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int queue, u16 ssn,
+bool iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int queue, u16 ssn,
 			       const struct iwl_trans_txq_scd_cfg *cfg,
 			       unsigned int wdg_timeout);
 void iwl_trans_pcie_txq_disable(struct iwl_trans *trans, int queue,
@@ -653,6 +654,13 @@ static inline void iwl_enable_fw_load_int(struct iwl_trans *trans)
 	}
 }
 
+static inline void iwl_pcie_sw_reset(struct iwl_trans *trans)
+{
+	/* Reset entire device - do controller reset (results in SHRD_HW_RST) */
+	iwl_set_bit(trans, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
+	usleep_range(5000, 6000);
+}
+
 static inline void *iwl_pcie_get_tfd(struct iwl_trans_pcie *trans_pcie,
 				     struct iwl_txq *txq, int idx)
 {
@@ -673,8 +681,20 @@ static inline void iwl_enable_rfkill_int(struct iwl_trans *trans)
 		iwl_enable_hw_int_msk_msix(trans,
 					   MSIX_HW_INT_CAUSES_REG_RF_KILL);
 	}
+
+	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_9000) {
+		/*
+		 * On 9000-series devices this bit isn't enabled by default, so
+		 * when we power down the device we need set the bit to allow it
+		 * to wake up the PCI-E bus for RF-kill interrupts.
+		 */
+		iwl_set_bit(trans, CSR_GP_CNTRL,
+			    CSR_GP_CNTRL_REG_FLAG_RFKILL_WAKE_L1A_EN);
+	}
 }
 
+void iwl_pcie_handle_rfkill_irq(struct iwl_trans *trans);
+
 static inline void iwl_wake_queue(struct iwl_trans *trans,
 				  struct iwl_txq *txq)
 {
@@ -713,7 +733,12 @@ static inline u8 get_cmd_index(struct iwl_txq *q, u32 index)
 
 static inline bool iwl_is_rfkill_set(struct iwl_trans *trans)
 {
-	lockdep_assert_held(&IWL_TRANS_GET_PCIE_TRANS(trans)->mutex);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+
+	lockdep_assert_held(&trans_pcie->mutex);
+
+	if (trans_pcie->debug_rfkill)
+		return true;
 
 	return !(iwl_read32(trans, CSR_GP_CNTRL) &
 		CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW);
@@ -767,9 +792,11 @@ void iwl_pcie_apm_config(struct iwl_trans *trans);
 int iwl_pcie_prepare_card_hw(struct iwl_trans *trans);
 void iwl_pcie_synchronize_irqs(struct iwl_trans *trans);
 bool iwl_trans_check_hw_rf_kill(struct iwl_trans *trans);
+void iwl_trans_pcie_handle_stop_rfkill(struct iwl_trans *trans,
+				       bool was_in_rfkill);
 void iwl_pcie_txq_free_tfd(struct iwl_trans *trans, struct iwl_txq *txq);
 int iwl_queue_space(const struct iwl_txq *q);
-int iwl_pcie_apm_stop_master(struct iwl_trans *trans);
+void iwl_pcie_apm_stop_master(struct iwl_trans *trans);
 void iwl_pcie_conf_msix_hw(struct iwl_trans_pcie *trans_pcie);
 int iwl_pcie_txq_init(struct iwl_trans *trans, struct iwl_txq *txq,
 		      int slots_num, bool cmd_queue);
@@ -779,6 +806,9 @@ int iwl_pcie_alloc_dma_ptr(struct iwl_trans *trans,
 			   struct iwl_dma_ptr *ptr, size_t size);
 void iwl_pcie_free_dma_ptr(struct iwl_trans *trans, struct iwl_dma_ptr *ptr);
 void iwl_pcie_apply_destination(struct iwl_trans *trans);
+#ifdef CONFIG_INET
+struct iwl_tso_hdr_page *get_page_hdr(struct iwl_trans *trans, size_t len);
+#endif
 
 /* transport gen 2 exported functions */
 int iwl_trans_pcie_gen2_start_fw(struct iwl_trans *trans,
diff --git a/drivers/net/wireless/intel/iwlwifi/pcie/rx.c b/drivers/net/wireless/intel/iwlwifi/pcie/rx.c
index 1da2de205cdf..351c4423125a 100644
--- a/drivers/net/wireless/intel/iwlwifi/pcie/rx.c
+++ b/drivers/net/wireless/intel/iwlwifi/pcie/rx.c
@@ -761,6 +761,15 @@ static void iwl_pcie_rx_hw_init(struct iwl_trans *trans, struct iwl_rxq *rxq)
 
 void iwl_pcie_enable_rx_wake(struct iwl_trans *trans, bool enable)
 {
+	if (trans->cfg->device_family != IWL_DEVICE_FAMILY_9000)
+		return;
+
+	if (CSR_HW_REV_STEP(trans->hw_rev) != SILICON_A_STEP)
+		return;
+
+	if (!trans->cfg->integrated)
+		return;
+
 	/*
 	 * Turn on the chicken-bits that cause MAC wakeup for RX-related
 	 * values.
@@ -768,12 +777,10 @@ void iwl_pcie_enable_rx_wake(struct iwl_trans *trans, bool enable)
 	 * bug where shadow registers are not in the retention list and their
 	 * value is lost when NIC powers down
 	 */
-	if (trans->cfg->integrated) {
-		iwl_set_bit(trans, CSR_MAC_SHADOW_REG_CTRL,
-			    CSR_MAC_SHADOW_REG_CTRL_RX_WAKE);
-		iwl_set_bit(trans, CSR_MAC_SHADOW_REG_CTL2,
-			    CSR_MAC_SHADOW_REG_CTL2_RX_WAKE);
-	}
+	iwl_set_bit(trans, CSR_MAC_SHADOW_REG_CTRL,
+		    CSR_MAC_SHADOW_REG_CTRL_RX_WAKE);
+	iwl_set_bit(trans, CSR_MAC_SHADOW_REG_CTL2,
+		    CSR_MAC_SHADOW_REG_CTL2_RX_WAKE);
 }
 
 static void iwl_pcie_rx_mq_hw_init(struct iwl_trans *trans)
@@ -845,14 +852,14 @@ static void iwl_pcie_rx_mq_hw_init(struct iwl_trans *trans)
 	 * Set RX DMA chunk size to 64B for IOSF and 128B for PCIe
 	 * Default queue is 0
 	 */
-	iwl_write_prph_no_grab(trans, RFH_GEN_CFG, RFH_GEN_CFG_RFH_DMA_SNOOP |
-			       (DEFAULT_RXQ_NUM <<
-				RFH_GEN_CFG_DEFAULT_RXQ_NUM_POS) |
+	iwl_write_prph_no_grab(trans, RFH_GEN_CFG,
+			       RFH_GEN_CFG_RFH_DMA_SNOOP |
+			       RFH_GEN_CFG_VAL(DEFAULT_RXQ_NUM, 0) |
 			       RFH_GEN_CFG_SERVICE_DMA_SNOOP |
-			       (trans->cfg->integrated ?
-				RFH_GEN_CFG_RB_CHUNK_SIZE_64 :
-				RFH_GEN_CFG_RB_CHUNK_SIZE_128) <<
-			       RFH_GEN_CFG_RB_CHUNK_SIZE_POS);
+			       RFH_GEN_CFG_VAL(RB_CHUNK_SIZE,
+					       trans->cfg->integrated ?
+					       RFH_GEN_CFG_RB_CHUNK_SIZE_64 :
+					       RFH_GEN_CFG_RB_CHUNK_SIZE_128));
 	/* Enable the relevant rx queues */
 	iwl_write_prph_no_grab(trans, RFH_RXF_RXQ_ACTIVE, enabled);
 
@@ -1119,15 +1126,23 @@ static void iwl_pcie_rx_handle_rb(struct iwl_trans *trans,
 
 		pkt = rxb_addr(&rxcb);
 
-		if (pkt->len_n_flags == cpu_to_le32(FH_RSCSR_FRAME_INVALID))
+		if (pkt->len_n_flags == cpu_to_le32(FH_RSCSR_FRAME_INVALID)) {
+			IWL_DEBUG_RX(trans,
+				     "Q %d: RB end marker at offset %d\n",
+				     rxq->id, offset);
 			break;
+		}
 
-		WARN_ON((le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_RXQ_MASK) >>
-			FH_RSCSR_RXQ_POS != rxq->id);
+		WARN((le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_RXQ_MASK) >>
+			FH_RSCSR_RXQ_POS != rxq->id,
+		     "frame on invalid queue - is on %d and indicates %d\n",
+		     rxq->id,
+		     (le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_RXQ_MASK) >>
+			FH_RSCSR_RXQ_POS);
 
 		IWL_DEBUG_RX(trans,
-			     "cmd at offset %d: %s (%.2x.%2x, seq 0x%x)\n",
-			     rxcb._offset,
+			     "Q %d: cmd at offset %d: %s (%.2x.%2x, seq 0x%x)\n",
+			     rxq->id, offset,
 			     iwl_get_cmd_string(trans,
 						iwl_cmd_id(pkt->hdr.cmd,
 							   pkt->hdr.group_id,
@@ -1376,6 +1391,8 @@ irqreturn_t iwl_pcie_irq_rx_msix_handler(int irq, void *dev_id)
 	struct iwl_trans_pcie *trans_pcie = iwl_pcie_get_trans_pcie(entry);
 	struct iwl_trans *trans = trans_pcie->trans;
 
+	trace_iwlwifi_dev_irq_msix(trans->dev, entry, false, 0, 0);
+
 	if (WARN_ON(entry->entry >= trans->num_rx_queues))
 		return IRQ_NONE;
 
@@ -1413,18 +1430,16 @@ static void iwl_pcie_irq_handle_error(struct iwl_trans *trans)
 		return;
 	}
 
-	local_bh_disable();
-	/* The STATUS_FW_ERROR bit is set in this function. This must happen
-	 * before we wake up the command caller, to ensure a proper cleanup. */
-	iwl_trans_fw_error(trans);
-	local_bh_enable();
-
 	for (i = 0; i < trans->cfg->base_params->num_of_queues; i++) {
 		if (!trans_pcie->txq[i])
 			continue;
 		del_timer(&trans_pcie->txq[i]->stuck_timer);
 	}
 
+	/* The STATUS_FW_ERROR bit is set in this function. This must happen
+	 * before we wake up the command caller, to ensure a proper cleanup. */
+	iwl_trans_fw_error(trans);
+
 	clear_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status);
 	wake_up(&trans_pcie->wait_command_queue);
 }
@@ -1509,6 +1524,46 @@ static u32 iwl_pcie_int_cause_ict(struct iwl_trans *trans)
 	return inta;
 }
 
+void iwl_pcie_handle_rfkill_irq(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;
+	bool hw_rfkill, prev, report;
+
+	mutex_lock(&trans_pcie->mutex);
+	prev = test_bit(STATUS_RFKILL_OPMODE, &trans->status);
+	hw_rfkill = iwl_is_rfkill_set(trans);
+	if (hw_rfkill) {
+		set_bit(STATUS_RFKILL_OPMODE, &trans->status);
+		set_bit(STATUS_RFKILL_HW, &trans->status);
+	}
+	if (trans_pcie->opmode_down)
+		report = hw_rfkill;
+	else
+		report = test_bit(STATUS_RFKILL_OPMODE, &trans->status);
+
+	IWL_WARN(trans, "RF_KILL bit toggled to %s.\n",
+		 hw_rfkill ? "disable radio" : "enable radio");
+
+	isr_stats->rfkill++;
+
+	if (prev != report)
+		iwl_trans_pcie_rf_kill(trans, report);
+	mutex_unlock(&trans_pcie->mutex);
+
+	if (hw_rfkill) {
+		if (test_and_clear_bit(STATUS_SYNC_HCMD_ACTIVE,
+				       &trans->status))
+			IWL_DEBUG_RF_KILL(trans,
+					  "Rfkill while SYNC HCMD in flight\n");
+		wake_up(&trans_pcie->wait_command_queue);
+	} else {
+		clear_bit(STATUS_RFKILL_HW, &trans->status);
+		if (trans_pcie->opmode_down)
+			clear_bit(STATUS_RFKILL_OPMODE, &trans->status);
+	}
+}
+
 irqreturn_t iwl_pcie_irq_handler(int irq, void *dev_id)
 {
 	struct iwl_trans *trans = dev_id;
@@ -1632,30 +1687,7 @@ irqreturn_t iwl_pcie_irq_handler(int irq, void *dev_id)
 
 	/* HW RF KILL switch toggled */
 	if (inta & CSR_INT_BIT_RF_KILL) {
-		bool hw_rfkill;
-
-		mutex_lock(&trans_pcie->mutex);
-		hw_rfkill = iwl_is_rfkill_set(trans);
-		if (hw_rfkill)
-			set_bit(STATUS_RFKILL, &trans->status);
-
-		IWL_WARN(trans, "RF_KILL bit toggled to %s.\n",
-			 hw_rfkill ? "disable radio" : "enable radio");
-
-		isr_stats->rfkill++;
-
-		iwl_trans_pcie_rf_kill(trans, hw_rfkill);
-		mutex_unlock(&trans_pcie->mutex);
-		if (hw_rfkill) {
-			if (test_and_clear_bit(STATUS_SYNC_HCMD_ACTIVE,
-					       &trans->status))
-				IWL_DEBUG_RF_KILL(trans,
-						  "Rfkill while SYNC HCMD in flight\n");
-			wake_up(&trans_pcie->wait_command_queue);
-		} else {
-			clear_bit(STATUS_RFKILL, &trans->status);
-		}
-
+		iwl_pcie_handle_rfkill_irq(trans);
 		handled |= CSR_INT_BIT_RF_KILL;
 	}
 
@@ -1902,6 +1934,8 @@ irqreturn_t iwl_pcie_irq_msix_handler(int irq, void *dev_id)
 	iwl_write32(trans, CSR_MSIX_HW_INT_CAUSES_AD, inta_hw);
 	spin_unlock(&trans_pcie->irq_lock);
 
+	trace_iwlwifi_dev_irq_msix(trans->dev, entry, true, inta_fh, inta_hw);
+
 	if (unlikely(!(inta_fh | inta_hw))) {
 		IWL_DEBUG_ISR(trans, "Ignore interrupt, inta == 0\n");
 		lock_map_release(&trans->sync_cmd_lockdep_map);
@@ -1982,31 +2016,8 @@ irqreturn_t iwl_pcie_irq_msix_handler(int irq, void *dev_id)
 	}
 
 	/* HW RF KILL switch toggled */
-	if (inta_hw & MSIX_HW_INT_CAUSES_REG_RF_KILL) {
-		bool hw_rfkill;
-
-		mutex_lock(&trans_pcie->mutex);
-		hw_rfkill = iwl_is_rfkill_set(trans);
-		if (hw_rfkill)
-			set_bit(STATUS_RFKILL, &trans->status);
-
-		IWL_WARN(trans, "RF_KILL bit toggled to %s.\n",
-			 hw_rfkill ? "disable radio" : "enable radio");
-
-		isr_stats->rfkill++;
-
-		iwl_trans_pcie_rf_kill(trans, hw_rfkill);
-		mutex_unlock(&trans_pcie->mutex);
-		if (hw_rfkill) {
-			if (test_and_clear_bit(STATUS_SYNC_HCMD_ACTIVE,
-					       &trans->status))
-				IWL_DEBUG_RF_KILL(trans,
-						  "Rfkill while SYNC HCMD in flight\n");
-			wake_up(&trans_pcie->wait_command_queue);
-		} else {
-			clear_bit(STATUS_RFKILL, &trans->status);
-		}
-	}
+	if (inta_hw & MSIX_HW_INT_CAUSES_REG_RF_KILL)
+		iwl_pcie_handle_rfkill_irq(trans);
 
 	if (inta_hw & MSIX_HW_INT_CAUSES_REG_HW_ERR) {
 		IWL_ERR(trans,
diff --git a/drivers/net/wireless/intel/iwlwifi/pcie/trans-gen2.c b/drivers/net/wireless/intel/iwlwifi/pcie/trans-gen2.c
index ac60a282d6de..b84b78293e7b 100644
--- a/drivers/net/wireless/intel/iwlwifi/pcie/trans-gen2.c
+++ b/drivers/net/wireless/intel/iwlwifi/pcie/trans-gen2.c
@@ -136,9 +136,7 @@ static void iwl_pcie_gen2_apm_stop(struct iwl_trans *trans, bool op_mode_leave)
 	/* Stop device's DMA activity */
 	iwl_pcie_apm_stop_master(trans);
 
-	/* Reset the entire device */
-	iwl_set_bit(trans, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
-	usleep_range(1000, 2000);
+	iwl_pcie_sw_reset(trans);
 
 	/*
 	 * Clear "initialization complete" bit to move adapter from
@@ -150,7 +148,6 @@ static void iwl_pcie_gen2_apm_stop(struct iwl_trans *trans, bool op_mode_leave)
 void _iwl_trans_pcie_gen2_stop_device(struct iwl_trans *trans, bool low_power)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-	bool hw_rfkill, was_hw_rfkill;
 
 	lockdep_assert_held(&trans_pcie->mutex);
 
@@ -159,8 +156,6 @@ void _iwl_trans_pcie_gen2_stop_device(struct iwl_trans *trans, bool low_power)
 
 	trans_pcie->is_down = true;
 
-	was_hw_rfkill = iwl_is_rfkill_set(trans);
-
 	/* tell the device to stop sending interrupts */
 	iwl_disable_interrupts(trans);
 
@@ -191,9 +186,7 @@ void _iwl_trans_pcie_gen2_stop_device(struct iwl_trans *trans, bool low_power)
 	/* Stop the device, and put it in low power state */
 	iwl_pcie_gen2_apm_stop(trans, false);
 
-	/* stop and reset the on-board processor */
-	iwl_write32(trans, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
-	usleep_range(1000, 2000);
+	iwl_pcie_sw_reset(trans);
 
 	/*
 	 * Upon stop, the IVAR table gets erased, so msi-x won't
@@ -217,7 +210,6 @@ void _iwl_trans_pcie_gen2_stop_device(struct iwl_trans *trans, bool low_power)
 	clear_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status);
 	clear_bit(STATUS_INT_ENABLED, &trans->status);
 	clear_bit(STATUS_TPOWER_PMI, &trans->status);
-	clear_bit(STATUS_RFKILL, &trans->status);
 
 	/*
 	 * Even if we stop the HW, we still want the RF kill
@@ -225,26 +217,6 @@ void _iwl_trans_pcie_gen2_stop_device(struct iwl_trans *trans, bool low_power)
 	 */
 	iwl_enable_rfkill_int(trans);
 
-	/*
-	 * Check again since the RF kill state may have changed while
-	 * all the interrupts were disabled, in this case we couldn't
-	 * receive the RF kill interrupt and update the state in the
-	 * op_mode.
-	 * Don't call the op_mode if the rkfill state hasn't changed.
-	 * This allows the op_mode to call stop_device from the rfkill
-	 * notification without endless recursion. Under very rare
-	 * circumstances, we might have a small recursion if the rfkill
-	 * state changed exactly now while we were called from stop_device.
-	 * This is very unlikely but can happen and is supported.
-	 */
-	hw_rfkill = iwl_is_rfkill_set(trans);
-	if (hw_rfkill)
-		set_bit(STATUS_RFKILL, &trans->status);
-	else
-		clear_bit(STATUS_RFKILL, &trans->status);
-	if (hw_rfkill != was_hw_rfkill)
-		iwl_trans_pcie_rf_kill(trans, hw_rfkill);
-
 	/* re-take ownership to prevent other users from stealing the device */
 	iwl_pcie_prepare_card_hw(trans);
 }
@@ -252,9 +224,13 @@ void _iwl_trans_pcie_gen2_stop_device(struct iwl_trans *trans, bool low_power)
 void iwl_trans_pcie_gen2_stop_device(struct iwl_trans *trans, bool low_power)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	bool was_in_rfkill;
 
 	mutex_lock(&trans_pcie->mutex);
+	trans_pcie->opmode_down = true;
+	was_in_rfkill = test_bit(STATUS_RFKILL_OPMODE, &trans->status);
 	_iwl_trans_pcie_gen2_stop_device(trans, low_power);
+	iwl_trans_pcie_handle_stop_rfkill(trans, was_in_rfkill);
 	mutex_unlock(&trans_pcie->mutex);
 }
 
diff --git a/drivers/net/wireless/intel/iwlwifi/pcie/trans.c b/drivers/net/wireless/intel/iwlwifi/pcie/trans.c
index 93cbc7a69bcd..92b3a55d0fbc 100644
--- a/drivers/net/wireless/intel/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/intel/iwlwifi/pcie/trans.c
@@ -80,7 +80,7 @@
 #include "iwl-prph.h"
 #include "iwl-scd.h"
 #include "iwl-agn-hw.h"
-#include "iwl-fw-error-dump.h"
+#include "fw/error-dump.h"
 #include "internal.h"
 #include "iwl-fh.h"
 
@@ -224,9 +224,9 @@ void iwl_pcie_apm_config(struct iwl_trans *trans)
 
 	pcie_capability_read_word(trans_pcie->pci_dev, PCI_EXP_DEVCTL2, &cap);
 	trans->ltr_enabled = cap & PCI_EXP_DEVCTL2_LTR_EN;
-	dev_info(trans->dev, "L1 %sabled - LTR %sabled\n",
-		 (lctl & PCI_EXP_LNKCTL_ASPM_L1) ? "En" : "Dis",
-		 trans->ltr_enabled ? "En" : "Dis");
+	IWL_DEBUG_POWER(trans, "L1 %sabled - LTR %sabled\n",
+			(lctl & PCI_EXP_LNKCTL_ASPM_L1) ? "En" : "Dis",
+			trans->ltr_enabled ? "En" : "Dis");
 }
 
 /*
@@ -236,7 +236,8 @@ void iwl_pcie_apm_config(struct iwl_trans *trans)
  */
 static int iwl_pcie_apm_init(struct iwl_trans *trans)
 {
-	int ret = 0;
+	int ret;
+
 	IWL_DEBUG_INFO(trans, "Init card's basic functions\n");
 
 	/*
@@ -245,7 +246,7 @@ static int iwl_pcie_apm_init(struct iwl_trans *trans)
 	 */
 
 	/* Disable L0S exit timer (platform NMI Work/Around) */
-	if (trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
+	if (trans->cfg->device_family < IWL_DEVICE_FAMILY_8000)
 		iwl_set_bit(trans, CSR_GIO_CHICKEN_BITS,
 			    CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER);
 
@@ -287,8 +288,8 @@ static int iwl_pcie_apm_init(struct iwl_trans *trans)
 			   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;
+		IWL_ERR(trans, "Failed to init the card\n");
+		return ret;
 	}
 
 	if (trans->cfg->host_interrupt_operation_mode) {
@@ -336,8 +337,7 @@ static int iwl_pcie_apm_init(struct iwl_trans *trans)
 
 	set_bit(STATUS_DEVICE_ENABLED, &trans->status);
 
-out:
-	return ret;
+	return 0;
 }
 
 /*
@@ -358,9 +358,7 @@ static void iwl_pcie_apm_lp_xtal_enable(struct iwl_trans *trans)
 	__iwl_trans_pcie_set_bit(trans, CSR_GP_CNTRL,
 				 CSR_GP_CNTRL_REG_FLAG_XTAL_ON);
 
-	/* Reset entire device - do controller reset (results in SHRD_HW_RST) */
-	iwl_set_bit(trans, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
-	usleep_range(1000, 2000);
+	iwl_pcie_sw_reset(trans);
 
 	/*
 	 * Set "initialization complete" bit to move adapter from
@@ -401,12 +399,7 @@ static void iwl_pcie_apm_lp_xtal_enable(struct iwl_trans *trans)
 				 apmg_xtal_cfg_reg |
 				 SHR_APMG_XTAL_CFG_XTAL_ON_REQ);
 
-	/*
-	 * Reset entire device again - do controller reset (results in
-	 * SHRD_HW_RST). Turn MAC off before proceeding.
-	 */
-	iwl_set_bit(trans, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
-	usleep_range(1000, 2000);
+	iwl_pcie_sw_reset(trans);
 
 	/* Enable LP XTAL by indirect access through CSR */
 	apmg_gp1_reg = iwl_trans_pcie_read_shr(trans, SHR_APMG_GP1_REG);
@@ -448,9 +441,9 @@ static void iwl_pcie_apm_lp_xtal_enable(struct iwl_trans *trans)
 				 ~SHR_APMG_XTAL_CFG_XTAL_ON_REQ);
 }
 
-int iwl_pcie_apm_stop_master(struct iwl_trans *trans)
+void iwl_pcie_apm_stop_master(struct iwl_trans *trans)
 {
-	int ret = 0;
+	int ret;
 
 	/* stop device's busmaster DMA activity */
 	iwl_set_bit(trans, CSR_RESET, CSR_RESET_REG_FLAG_STOP_MASTER);
@@ -462,8 +455,6 @@ int iwl_pcie_apm_stop_master(struct iwl_trans *trans)
 		IWL_WARN(trans, "Master Disable Timed Out, 100 usec\n");
 
 	IWL_DEBUG_INFO(trans, "stop master\n");
-
-	return ret;
 }
 
 static void iwl_pcie_apm_stop(struct iwl_trans *trans, bool op_mode_leave)
@@ -478,7 +469,7 @@ static void iwl_pcie_apm_stop(struct iwl_trans *trans, bool op_mode_leave)
 		if (trans->cfg->device_family == IWL_DEVICE_FAMILY_7000)
 			iwl_set_bits_prph(trans, APMG_PCIDEV_STT_REG,
 					  APMG_PCIDEV_STT_VAL_WAKE_ME);
-		else if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000) {
+		else if (trans->cfg->device_family >= IWL_DEVICE_FAMILY_8000) {
 			iwl_set_bit(trans, CSR_DBG_LINK_PWR_MGMT_REG,
 				    CSR_RESET_LINK_PWR_MGMT_DISABLED);
 			iwl_set_bit(trans, CSR_HW_IF_CONFIG_REG,
@@ -501,9 +492,7 @@ static void iwl_pcie_apm_stop(struct iwl_trans *trans, bool op_mode_leave)
 		return;
 	}
 
-	/* Reset the entire device */
-	iwl_set_bit(trans, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
-	usleep_range(1000, 2000);
+	iwl_pcie_sw_reset(trans);
 
 	/*
 	 * Clear "initialization complete" bit to move adapter from
@@ -516,13 +505,16 @@ static void iwl_pcie_apm_stop(struct iwl_trans *trans, bool op_mode_leave)
 static int iwl_pcie_nic_init(struct iwl_trans *trans)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	int ret;
 
 	/* nic_init */
 	spin_lock(&trans_pcie->irq_lock);
-	iwl_pcie_apm_init(trans);
-
+	ret = iwl_pcie_apm_init(trans);
 	spin_unlock(&trans_pcie->irq_lock);
 
+	if (ret)
+		return ret;
+
 	iwl_pcie_set_pwr(trans, false);
 
 	iwl_op_mode_nic_config(trans->op_mode);
@@ -892,7 +884,7 @@ monitor:
 	if (dest->monitor_mode == EXTERNAL_MODE && trans_pcie->fw_mon_size) {
 		iwl_write_prph(trans, le32_to_cpu(dest->base_reg),
 			       trans_pcie->fw_mon_phys >> dest->base_shift);
-		if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+		if (trans->cfg->device_family >= IWL_DEVICE_FAMILY_8000)
 			iwl_write_prph(trans, le32_to_cpu(dest->end_reg),
 				       (trans_pcie->fw_mon_phys +
 					trans_pcie->fw_mon_size - 256) >>
@@ -996,14 +988,24 @@ static int iwl_pcie_load_given_ucode_8000(struct iwl_trans *trans,
 
 bool iwl_trans_check_hw_rf_kill(struct iwl_trans *trans)
 {
+	struct iwl_trans_pcie *trans_pcie =  IWL_TRANS_GET_PCIE_TRANS(trans);
 	bool hw_rfkill = iwl_is_rfkill_set(trans);
+	bool prev = test_bit(STATUS_RFKILL_OPMODE, &trans->status);
+	bool report;
 
-	if (hw_rfkill)
-		set_bit(STATUS_RFKILL, &trans->status);
-	else
-		clear_bit(STATUS_RFKILL, &trans->status);
+	if (hw_rfkill) {
+		set_bit(STATUS_RFKILL_HW, &trans->status);
+		set_bit(STATUS_RFKILL_OPMODE, &trans->status);
+	} else {
+		clear_bit(STATUS_RFKILL_HW, &trans->status);
+		if (trans_pcie->opmode_down)
+			clear_bit(STATUS_RFKILL_OPMODE, &trans->status);
+	}
 
-	iwl_trans_pcie_rf_kill(trans, hw_rfkill);
+	report = test_bit(STATUS_RFKILL_OPMODE, &trans->status);
+
+	if (prev != report)
+		iwl_trans_pcie_rf_kill(trans, report);
 
 	return hw_rfkill;
 }
@@ -1128,7 +1130,6 @@ static void iwl_pcie_init_msix(struct iwl_trans_pcie *trans_pcie)
 static void _iwl_trans_pcie_stop_device(struct iwl_trans *trans, bool low_power)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-	bool hw_rfkill, was_hw_rfkill;
 
 	lockdep_assert_held(&trans_pcie->mutex);
 
@@ -1137,8 +1138,6 @@ static void _iwl_trans_pcie_stop_device(struct iwl_trans *trans, bool low_power)
 
 	trans_pcie->is_down = true;
 
-	was_hw_rfkill = iwl_is_rfkill_set(trans);
-
 	/* tell the device to stop sending interrupts */
 	iwl_disable_interrupts(trans);
 
@@ -1173,9 +1172,7 @@ static void _iwl_trans_pcie_stop_device(struct iwl_trans *trans, bool low_power)
 	/* Stop the device, and put it in low power state */
 	iwl_pcie_apm_stop(trans, false);
 
-	/* stop and reset the on-board processor */
-	iwl_write32(trans, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
-	usleep_range(1000, 2000);
+	iwl_pcie_sw_reset(trans);
 
 	/*
 	 * Upon stop, the IVAR table gets erased, so msi-x won't
@@ -1199,7 +1196,6 @@ static void _iwl_trans_pcie_stop_device(struct iwl_trans *trans, bool low_power)
 	clear_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status);
 	clear_bit(STATUS_INT_ENABLED, &trans->status);
 	clear_bit(STATUS_TPOWER_PMI, &trans->status);
-	clear_bit(STATUS_RFKILL, &trans->status);
 
 	/*
 	 * Even if we stop the HW, we still want the RF kill
@@ -1207,26 +1203,6 @@ static void _iwl_trans_pcie_stop_device(struct iwl_trans *trans, bool low_power)
 	 */
 	iwl_enable_rfkill_int(trans);
 
-	/*
-	 * Check again since the RF kill state may have changed while
-	 * all the interrupts were disabled, in this case we couldn't
-	 * receive the RF kill interrupt and update the state in the
-	 * op_mode.
-	 * Don't call the op_mode if the rkfill state hasn't changed.
-	 * This allows the op_mode to call stop_device from the rfkill
-	 * notification without endless recursion. Under very rare
-	 * circumstances, we might have a small recursion if the rfkill
-	 * state changed exactly now while we were called from stop_device.
-	 * This is very unlikely but can happen and is supported.
-	 */
-	hw_rfkill = iwl_is_rfkill_set(trans);
-	if (hw_rfkill)
-		set_bit(STATUS_RFKILL, &trans->status);
-	else
-		clear_bit(STATUS_RFKILL, &trans->status);
-	if (hw_rfkill != was_hw_rfkill)
-		iwl_trans_pcie_rf_kill(trans, hw_rfkill);
-
 	/* re-take ownership to prevent other users from stealing the device */
 	iwl_pcie_prepare_card_hw(trans);
 }
@@ -1318,7 +1294,7 @@ static int iwl_trans_pcie_start_fw(struct iwl_trans *trans,
 	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
 
 	/* Load the given image to the HW */
-	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+	if (trans->cfg->device_family >= IWL_DEVICE_FAMILY_8000)
 		ret = iwl_pcie_load_given_ucode_8000(trans, fw);
 	else
 		ret = iwl_pcie_load_given_ucode(trans, fw);
@@ -1339,12 +1315,45 @@ static void iwl_trans_pcie_fw_alive(struct iwl_trans *trans, u32 scd_addr)
 	iwl_pcie_tx_start(trans, scd_addr);
 }
 
+void iwl_trans_pcie_handle_stop_rfkill(struct iwl_trans *trans,
+				       bool was_in_rfkill)
+{
+	bool hw_rfkill;
+
+	/*
+	 * Check again since the RF kill state may have changed while
+	 * all the interrupts were disabled, in this case we couldn't
+	 * receive the RF kill interrupt and update the state in the
+	 * op_mode.
+	 * Don't call the op_mode if the rkfill state hasn't changed.
+	 * This allows the op_mode to call stop_device from the rfkill
+	 * notification without endless recursion. Under very rare
+	 * circumstances, we might have a small recursion if the rfkill
+	 * state changed exactly now while we were called from stop_device.
+	 * This is very unlikely but can happen and is supported.
+	 */
+	hw_rfkill = iwl_is_rfkill_set(trans);
+	if (hw_rfkill) {
+		set_bit(STATUS_RFKILL_HW, &trans->status);
+		set_bit(STATUS_RFKILL_OPMODE, &trans->status);
+	} else {
+		clear_bit(STATUS_RFKILL_HW, &trans->status);
+		clear_bit(STATUS_RFKILL_OPMODE, &trans->status);
+	}
+	if (hw_rfkill != was_in_rfkill)
+		iwl_trans_pcie_rf_kill(trans, hw_rfkill);
+}
+
 static void iwl_trans_pcie_stop_device(struct iwl_trans *trans, bool low_power)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	bool was_in_rfkill;
 
 	mutex_lock(&trans_pcie->mutex);
+	trans_pcie->opmode_down = true;
+	was_in_rfkill = test_bit(STATUS_RFKILL_OPMODE, &trans->status);
 	_iwl_trans_pcie_stop_device(trans, low_power);
+	iwl_trans_pcie_handle_stop_rfkill(trans, was_in_rfkill);
 	mutex_unlock(&trans_pcie->mutex);
 }
 
@@ -1355,6 +1364,8 @@ void iwl_trans_pcie_rf_kill(struct iwl_trans *trans, bool state)
 
 	lockdep_assert_held(&trans_pcie->mutex);
 
+	IWL_WARN(trans, "reporting RF_KILL (radio %s)\n",
+		 state ? "disabled" : "enabled");
 	if (iwl_op_mode_hw_rf_kill(trans->op_mode, state)) {
 		if (trans->cfg->gen2)
 			_iwl_trans_pcie_gen2_stop_device(trans, true);
@@ -1435,7 +1446,7 @@ static int iwl_trans_pcie_d3_resume(struct iwl_trans *trans,
 	iwl_set_bit(trans, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
 	iwl_set_bit(trans, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
 
-	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+	if (trans->cfg->device_family >= IWL_DEVICE_FAMILY_8000)
 		udelay(2);
 
 	ret = iwl_poll_bit(trans, CSR_GP_CNTRL,
@@ -1635,17 +1646,19 @@ static int _iwl_trans_pcie_start_hw(struct iwl_trans *trans, bool low_power)
 		return err;
 	}
 
-	/* Reset the entire device */
-	iwl_write32(trans, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
-	usleep_range(1000, 2000);
+	iwl_pcie_sw_reset(trans);
 
-	iwl_pcie_apm_init(trans);
+	err = iwl_pcie_apm_init(trans);
+	if (err)
+		return err;
 
 	iwl_pcie_init_msix(trans_pcie);
 
 	/* From now on, the op_mode will be kept updated about RF kill state */
 	iwl_enable_rfkill_int(trans);
 
+	trans_pcie->opmode_down = false;
+
 	/* Set is_down to false here so that...*/
 	trans_pcie->is_down = false;
 
@@ -1822,7 +1835,7 @@ static bool iwl_trans_pcie_grab_nic_access(struct iwl_trans *trans,
 	/* this bit wakes up the NIC */
 	__iwl_trans_pcie_set_bit(trans, CSR_GP_CNTRL,
 				 CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
-	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+	if (trans->cfg->device_family >= IWL_DEVICE_FAMILY_8000)
 		udelay(2);
 
 	/*
@@ -2045,17 +2058,52 @@ void iwl_trans_pcie_log_scd_error(struct iwl_trans *trans, struct iwl_txq *txq)
 		iwl_read_direct32(trans, FH_TX_TRB_REG(fifo)));
 }
 
-static int iwl_trans_pcie_wait_txq_empty(struct iwl_trans *trans, u32 txq_bm)
+static int iwl_trans_pcie_wait_txq_empty(struct iwl_trans *trans, int txq_idx)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_txq *txq;
-	int cnt;
 	unsigned long now = jiffies;
+	u8 wr_ptr;
+
+	if (!test_bit(txq_idx, trans_pcie->queue_used))
+		return -EINVAL;
+
+	IWL_DEBUG_TX_QUEUES(trans, "Emptying queue %d...\n", txq_idx);
+	txq = trans_pcie->txq[txq_idx];
+	wr_ptr = ACCESS_ONCE(txq->write_ptr);
+
+	while (txq->read_ptr != ACCESS_ONCE(txq->write_ptr) &&
+	       !time_after(jiffies,
+			   now + msecs_to_jiffies(IWL_FLUSH_WAIT_MS))) {
+		u8 write_ptr = ACCESS_ONCE(txq->write_ptr);
+
+		if (WARN_ONCE(wr_ptr != write_ptr,
+			      "WR pointer moved while flushing %d -> %d\n",
+			      wr_ptr, write_ptr))
+			return -ETIMEDOUT;
+		usleep_range(1000, 2000);
+	}
+
+	if (txq->read_ptr != txq->write_ptr) {
+		IWL_ERR(trans,
+			"fail to flush all tx fifo queues Q %d\n", txq_idx);
+		iwl_trans_pcie_log_scd_error(trans, txq);
+		return -ETIMEDOUT;
+	}
+
+	IWL_DEBUG_TX_QUEUES(trans, "Queue %d is now empty.\n", txq_idx);
+
+	return 0;
+}
+
+static int iwl_trans_pcie_wait_txqs_empty(struct iwl_trans *trans, u32 txq_bm)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	int cnt;
 	int ret = 0;
 
 	/* waiting for all the tx frames complete might take a while */
 	for (cnt = 0; cnt < trans->cfg->base_params->num_of_queues; cnt++) {
-		u8 wr_ptr;
 
 		if (cnt == trans_pcie->cmd_queue)
 			continue;
@@ -2064,34 +2112,11 @@ static int iwl_trans_pcie_wait_txq_empty(struct iwl_trans *trans, u32 txq_bm)
 		if (!(BIT(cnt) & txq_bm))
 			continue;
 
-		IWL_DEBUG_TX_QUEUES(trans, "Emptying queue %d...\n", cnt);
-		txq = trans_pcie->txq[cnt];
-		wr_ptr = ACCESS_ONCE(txq->write_ptr);
-
-		while (txq->read_ptr != ACCESS_ONCE(txq->write_ptr) &&
-		       !time_after(jiffies,
-				   now + msecs_to_jiffies(IWL_FLUSH_WAIT_MS))) {
-			u8 write_ptr = ACCESS_ONCE(txq->write_ptr);
-
-			if (WARN_ONCE(wr_ptr != write_ptr,
-				      "WR pointer moved while flushing %d -> %d\n",
-				      wr_ptr, write_ptr))
-				return -ETIMEDOUT;
-			usleep_range(1000, 2000);
-		}
-
-		if (txq->read_ptr != txq->write_ptr) {
-			IWL_ERR(trans,
-				"fail to flush all tx fifo queues Q %d\n", cnt);
-			ret = -ETIMEDOUT;
+		ret = iwl_trans_pcie_wait_txq_empty(trans, cnt);
+		if (ret)
 			break;
-		}
-		IWL_DEBUG_TX_QUEUES(trans, "Queue %d is now empty.\n", cnt);
 	}
 
-	if (ret)
-		iwl_trans_pcie_log_scd_error(trans, txq);
-
 	return ret;
 }
 
@@ -2393,17 +2418,12 @@ static ssize_t iwl_dbgfs_interrupt_write(struct file *file,
 	struct iwl_trans *trans = file->private_data;
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct isr_statistics *isr_stats = &trans_pcie->isr_stats;
-
-	char buf[8];
-	int buf_size;
 	u32 reset_flag;
+	int ret;
 
-	memset(buf, 0, sizeof(buf));
-	buf_size = min(count, sizeof(buf) -  1);
-	if (copy_from_user(buf, user_buf, buf_size))
-		return -EFAULT;
-	if (sscanf(buf, "%x", &reset_flag) != 1)
-		return -EFAULT;
+	ret = kstrtou32_from_user(user_buf, count, 16, &reset_flag);
+	if (ret)
+		return ret;
 	if (reset_flag == 0)
 		memset(isr_stats, 0, sizeof(*isr_stats));
 
@@ -2415,16 +2435,6 @@ static ssize_t iwl_dbgfs_csr_write(struct file *file,
 				   size_t count, loff_t *ppos)
 {
 	struct iwl_trans *trans = file->private_data;
-	char buf[8];
-	int buf_size;
-	int csr;
-
-	memset(buf, 0, sizeof(buf));
-	buf_size = min(count, sizeof(buf) -  1);
-	if (copy_from_user(buf, user_buf, buf_size))
-		return -EFAULT;
-	if (sscanf(buf, "%d", &csr) != 1)
-		return -EFAULT;
 
 	iwl_pcie_dump_csr(trans);
 
@@ -2449,11 +2459,50 @@ static ssize_t iwl_dbgfs_fh_reg_read(struct file *file,
 	return ret;
 }
 
+static ssize_t iwl_dbgfs_rfkill_read(struct file *file,
+				     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);
+	char buf[100];
+	int pos;
+
+	pos = scnprintf(buf, sizeof(buf), "debug: %d\nhw: %d\n",
+			trans_pcie->debug_rfkill,
+			!(iwl_read32(trans, CSR_GP_CNTRL) &
+				CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW));
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t iwl_dbgfs_rfkill_write(struct file *file,
+				      const 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);
+	bool old = trans_pcie->debug_rfkill;
+	int ret;
+
+	ret = kstrtobool_from_user(user_buf, count, &trans_pcie->debug_rfkill);
+	if (ret)
+		return ret;
+	if (old == trans_pcie->debug_rfkill)
+		return count;
+	IWL_WARN(trans, "changing debug rfkill %d->%d\n",
+		 old, trans_pcie->debug_rfkill);
+	iwl_pcie_handle_rfkill_irq(trans);
+
+	return count;
+}
+
 DEBUGFS_READ_WRITE_FILE_OPS(interrupt);
 DEBUGFS_READ_FILE_OPS(fh_reg);
 DEBUGFS_READ_FILE_OPS(rx_queue);
 DEBUGFS_READ_FILE_OPS(tx_queue);
 DEBUGFS_WRITE_FILE_OPS(csr);
+DEBUGFS_READ_WRITE_FILE_OPS(rfkill);
 
 /* Create the debugfs files and directories */
 int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans)
@@ -2465,6 +2514,7 @@ int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans)
 	DEBUGFS_ADD_FILE(interrupt, dir, S_IWUSR | S_IRUSR);
 	DEBUGFS_ADD_FILE(csr, dir, S_IWUSR);
 	DEBUGFS_ADD_FILE(fh_reg, dir, S_IRUSR);
+	DEBUGFS_ADD_FILE(rfkill, dir, S_IWUSR | S_IRUSR);
 	return 0;
 
 err:
@@ -2563,8 +2613,15 @@ static u32 iwl_trans_pcie_fh_regs_dump(struct iwl_trans *trans,
 	(*data)->len = cpu_to_le32(fh_regs_len);
 	val = (void *)(*data)->data;
 
-	for (i = FH_MEM_LOWER_BOUND; i < FH_MEM_UPPER_BOUND; i += sizeof(u32))
-		*val++ = cpu_to_le32(iwl_trans_pcie_read32(trans, i));
+	if (!trans->cfg->gen2)
+		for (i = FH_MEM_LOWER_BOUND; i < FH_MEM_UPPER_BOUND;
+		     i += sizeof(u32))
+			*val++ = cpu_to_le32(iwl_trans_pcie_read32(trans, i));
+	else
+		for (i = FH_MEM_LOWER_BOUND_GEN2; i < FH_MEM_UPPER_BOUND_GEN2;
+		     i += sizeof(u32))
+			*val++ = cpu_to_le32(iwl_trans_pcie_read_prph(trans,
+								      i));
 
 	iwl_trans_release_nic_access(trans, &flags);
 
@@ -2714,7 +2771,7 @@ static struct iwl_trans_dump_data
 		      trans->dbg_dest_tlv->end_shift;
 
 		/* Make "end" point to the actual end */
-		if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000 ||
+		if (trans->cfg->device_family >= IWL_DEVICE_FAMILY_8000 ||
 		    trans->dbg_dest_tlv->monitor_mode == MARBH_MODE)
 			end += (1 << trans->dbg_dest_tlv->end_shift);
 		monitor_len = end - base;
@@ -2740,7 +2797,12 @@ static struct iwl_trans_dump_data
 	len += sizeof(*data) + IWL_CSR_TO_DUMP;
 
 	/* FH registers */
-	len += sizeof(*data) + (FH_MEM_UPPER_BOUND - FH_MEM_LOWER_BOUND);
+	if (trans->cfg->gen2)
+		len += sizeof(*data) +
+		       (FH_MEM_UPPER_BOUND_GEN2 - FH_MEM_LOWER_BOUND_GEN2);
+	else
+		len += sizeof(*data) +
+		       (FH_MEM_UPPER_BOUND - FH_MEM_LOWER_BOUND);
 
 	if (dump_rbs) {
 		/* Dump RBs is supported only for pre-9000 devices (1 queue) */
@@ -2754,6 +2816,13 @@ static struct iwl_trans_dump_data
 				  (PAGE_SIZE << trans_pcie->rx_page_order));
 	}
 
+	/* Paged memory for gen2 HW */
+	if (trans->cfg->gen2)
+		for (i = 0; i < trans_pcie->init_dram.paging_cnt; i++)
+			len += sizeof(*data) +
+			       sizeof(struct iwl_fw_error_dump_paging) +
+			       trans_pcie->init_dram.paging[i].size;
+
 	dump_data = vzalloc(len);
 	if (!dump_data)
 		return NULL;
@@ -2793,6 +2862,28 @@ static struct iwl_trans_dump_data
 	if (dump_rbs)
 		len += iwl_trans_pcie_dump_rbs(trans, &data, num_rbs);
 
+	/* Paged memory for gen2 HW */
+	if (trans->cfg->gen2) {
+		for (i = 0; i < trans_pcie->init_dram.paging_cnt; i++) {
+			struct iwl_fw_error_dump_paging *paging;
+			dma_addr_t addr =
+				trans_pcie->init_dram.paging[i].physical;
+			u32 page_len = trans_pcie->init_dram.paging[i].size;
+
+			data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_PAGING);
+			data->len = cpu_to_le32(sizeof(*paging) + page_len);
+			paging = (void *)data->data;
+			paging->index = cpu_to_le32(i);
+			dma_sync_single_for_cpu(trans->dev, addr, page_len,
+						DMA_BIDIRECTIONAL);
+			memcpy(paging->data,
+			       trans_pcie->init_dram.paging[i].block, page_len);
+			data = iwl_fw_error_next_data(data);
+
+			len += sizeof(*data) + sizeof(*paging) + page_len;
+		}
+	}
+
 	len += iwl_trans_pcie_dump_monitor(trans, &data, monitor_len);
 
 	dump_data->len = len;
@@ -2835,7 +2926,6 @@ static void iwl_trans_pcie_resume(struct iwl_trans *trans)
 	.ref = iwl_trans_pcie_ref,					\
 	.unref = iwl_trans_pcie_unref,					\
 	.dump_data = iwl_trans_pcie_dump_data,				\
-	.wait_tx_queues_empty = iwl_trans_pcie_wait_txq_empty,		\
 	.d3_suspend = iwl_trans_pcie_d3_suspend,			\
 	.d3_resume = iwl_trans_pcie_d3_resume
 
@@ -2865,6 +2955,8 @@ static const struct iwl_trans_ops trans_ops_pcie = {
 
 	.txq_set_shared_mode = iwl_trans_pcie_txq_set_shared_mode,
 
+	.wait_tx_queues_empty = iwl_trans_pcie_wait_txqs_empty,
+
 	.freeze_txq_timer = iwl_trans_pcie_freeze_txq_timer,
 	.block_txq_ptrs = iwl_trans_pcie_block_txq_ptrs,
 };
@@ -2884,6 +2976,7 @@ static const struct iwl_trans_ops trans_ops_pcie_gen2 = {
 
 	.txq_alloc = iwl_trans_pcie_dyn_txq_alloc,
 	.txq_free = iwl_trans_pcie_dyn_txq_free,
+	.wait_txq_empty = iwl_trans_pcie_wait_txq_empty,
 };
 
 struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
@@ -2910,6 +3003,7 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
 	trans_pcie->trans = trans;
+	trans_pcie->opmode_down = true;
 	spin_lock_init(&trans_pcie->irq_lock);
 	spin_lock_init(&trans_pcie->reg_lock);
 	mutex_init(&trans_pcie->mutex);
@@ -2988,7 +3082,7 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	 * "dash" value). To keep hw_rev backwards compatible - we'll store it
 	 * in the old format.
 	 */
-	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000) {
+	if (trans->cfg->device_family >= IWL_DEVICE_FAMILY_8000) {
 		unsigned long flags;
 
 		trans->hw_rev = (trans->hw_rev & 0xfff0) |
@@ -3032,6 +3126,17 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 		}
 	}
 
+	/*
+	 * 9000-series integrated A-step has a problem with suspend/resume
+	 * and sometimes even causes the whole platform to get stuck. This
+	 * workaround makes the hardware not go into the problematic state.
+	 */
+	if (trans->cfg->integrated &&
+	    trans->cfg->device_family == IWL_DEVICE_FAMILY_9000 &&
+	    CSR_HW_REV_STEP(trans->hw_rev) == SILICON_A_STEP)
+		iwl_set_bit(trans, CSR_HOST_CHICKEN,
+			    CSR_HOST_CHICKEN_PM_IDLE_SRC_DIS_SB_PME);
+
 	trans->hw_rf_id = iwl_read32(trans, CSR_HW_RF_ID);
 
 	iwl_pcie_set_interrupt_capa(pdev, trans);
diff --git a/drivers/net/wireless/intel/iwlwifi/pcie/tx-gen2.c b/drivers/net/wireless/intel/iwlwifi/pcie/tx-gen2.c
index 9c9bfbbabdf1..a3795ba0d7b9 100644
--- a/drivers/net/wireless/intel/iwlwifi/pcie/tx-gen2.c
+++ b/drivers/net/wireless/intel/iwlwifi/pcie/tx-gen2.c
@@ -49,6 +49,7 @@
  *
  *****************************************************************************/
 #include <linux/pm_runtime.h>
+#include <net/tso.h>
 
 #include "iwl-debug.h"
 #include "iwl-csr.h"
@@ -226,6 +227,143 @@ static int iwl_pcie_gen2_set_tb(struct iwl_trans *trans,
 	return idx;
 }
 
+static int iwl_pcie_gen2_build_amsdu(struct iwl_trans *trans,
+				     struct sk_buff *skb,
+				     struct iwl_tfh_tfd *tfd, int start_len,
+				     u8 hdr_len, struct iwl_device_cmd *dev_cmd)
+{
+#ifdef CONFIG_INET
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_tx_cmd *tx_cmd = (void *)dev_cmd->payload;
+	struct ieee80211_hdr *hdr = (void *)skb->data;
+	unsigned int snap_ip_tcp_hdrlen, ip_hdrlen, total_len, hdr_room;
+	unsigned int mss = skb_shinfo(skb)->gso_size;
+	u16 length, iv_len, amsdu_pad;
+	u8 *start_hdr;
+	struct iwl_tso_hdr_page *hdr_page;
+	struct page **page_ptr;
+	struct tso_t tso;
+
+	/* if the packet is protected, then it must be CCMP or GCMP */
+	iv_len = ieee80211_has_protected(hdr->frame_control) ?
+		IEEE80211_CCMP_HDR_LEN : 0;
+
+	trace_iwlwifi_dev_tx(trans->dev, skb, tfd, sizeof(*tfd),
+			     &dev_cmd->hdr, start_len, 0);
+
+	ip_hdrlen = skb_transport_header(skb) - skb_network_header(skb);
+	snap_ip_tcp_hdrlen = 8 + ip_hdrlen + tcp_hdrlen(skb);
+	total_len = skb->len - snap_ip_tcp_hdrlen - hdr_len - iv_len;
+	amsdu_pad = 0;
+
+	/* total amount of header we may need for this A-MSDU */
+	hdr_room = DIV_ROUND_UP(total_len, mss) *
+		(3 + snap_ip_tcp_hdrlen + sizeof(struct ethhdr)) + iv_len;
+
+	/* Our device supports 9 segments at most, it will fit in 1 page */
+	hdr_page = get_page_hdr(trans, hdr_room);
+	if (!hdr_page)
+		return -ENOMEM;
+
+	get_page(hdr_page->page);
+	start_hdr = hdr_page->pos;
+	page_ptr = (void *)((u8 *)skb->cb + trans_pcie->page_offs);
+	*page_ptr = hdr_page->page;
+	memcpy(hdr_page->pos, skb->data + hdr_len, iv_len);
+	hdr_page->pos += iv_len;
+
+	/*
+	 * Pull the ieee80211 header + IV to be able to use TSO core,
+	 * we will restore it for the tx_status flow.
+	 */
+	skb_pull(skb, hdr_len + iv_len);
+
+	/*
+	 * Remove the length of all the headers that we don't actually
+	 * have in the MPDU by themselves, but that we duplicate into
+	 * all the different MSDUs inside the A-MSDU.
+	 */
+	le16_add_cpu(&tx_cmd->len, -snap_ip_tcp_hdrlen);
+
+	tso_start(skb, &tso);
+
+	while (total_len) {
+		/* this is the data left for this subframe */
+		unsigned int data_left = min_t(unsigned int, mss, total_len);
+		struct sk_buff *csum_skb = NULL;
+		unsigned int tb_len;
+		dma_addr_t tb_phys;
+		struct tcphdr *tcph;
+		u8 *iph, *subf_hdrs_start = hdr_page->pos;
+
+		total_len -= data_left;
+
+		memset(hdr_page->pos, 0, amsdu_pad);
+		hdr_page->pos += amsdu_pad;
+		amsdu_pad = (4 - (sizeof(struct ethhdr) + snap_ip_tcp_hdrlen +
+				  data_left)) & 0x3;
+		ether_addr_copy(hdr_page->pos, ieee80211_get_DA(hdr));
+		hdr_page->pos += ETH_ALEN;
+		ether_addr_copy(hdr_page->pos, ieee80211_get_SA(hdr));
+		hdr_page->pos += ETH_ALEN;
+
+		length = snap_ip_tcp_hdrlen + data_left;
+		*((__be16 *)hdr_page->pos) = cpu_to_be16(length);
+		hdr_page->pos += sizeof(length);
+
+		/*
+		 * This will copy the SNAP as well which will be considered
+		 * as MAC header.
+		 */
+		tso_build_hdr(skb, hdr_page->pos, &tso, data_left, !total_len);
+		iph = hdr_page->pos + 8;
+		tcph = (void *)(iph + ip_hdrlen);
+
+		hdr_page->pos += snap_ip_tcp_hdrlen;
+
+		tb_len = hdr_page->pos - start_hdr;
+		tb_phys = dma_map_single(trans->dev, start_hdr,
+					 tb_len, DMA_TO_DEVICE);
+		if (unlikely(dma_mapping_error(trans->dev, tb_phys))) {
+			dev_kfree_skb(csum_skb);
+			goto out_err;
+		}
+		iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, tb_len);
+		trace_iwlwifi_dev_tx_tso_chunk(trans->dev, start_hdr, tb_len);
+		/* add this subframe's headers' length to the tx_cmd */
+		le16_add_cpu(&tx_cmd->len, hdr_page->pos - subf_hdrs_start);
+
+		/* prepare the start_hdr for the next subframe */
+		start_hdr = hdr_page->pos;
+
+		/* put the payload */
+		while (data_left) {
+			tb_len = min_t(unsigned int, tso.size, data_left);
+			tb_phys = dma_map_single(trans->dev, tso.data,
+						 tb_len, DMA_TO_DEVICE);
+			if (unlikely(dma_mapping_error(trans->dev, tb_phys))) {
+				dev_kfree_skb(csum_skb);
+				goto out_err;
+			}
+			iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, tb_len);
+			trace_iwlwifi_dev_tx_tso_chunk(trans->dev, tso.data,
+						       tb_len);
+
+			data_left -= tb_len;
+			tso_build_data(skb, &tso, tb_len);
+		}
+	}
+
+	/* re -add the WiFi header and IV */
+	skb_push(skb, hdr_len + iv_len);
+
+	return 0;
+
+out_err:
+#endif
+	return -EINVAL;
+}
+
 static
 struct iwl_tfh_tfd *iwl_pcie_gen2_build_tfd(struct iwl_trans *trans,
 					    struct iwl_txq *txq,
@@ -238,15 +376,21 @@ struct iwl_tfh_tfd *iwl_pcie_gen2_build_tfd(struct iwl_trans *trans,
 	struct iwl_tfh_tfd *tfd =
 		iwl_pcie_get_tfd(trans_pcie, txq, txq->write_ptr);
 	dma_addr_t tb_phys;
+	bool amsdu;
 	int i, len, tb1_len, tb2_len, hdr_len;
 	void *tb1_addr;
 
 	memset(tfd, 0, sizeof(*tfd));
 
+	amsdu = ieee80211_is_data_qos(hdr->frame_control) &&
+		(*ieee80211_get_qos_ctl(hdr) &
+		 IEEE80211_QOS_CTL_A_MSDU_PRESENT);
+
 	tb_phys = iwl_pcie_get_first_tb_dma(txq, txq->write_ptr);
 	/* The first TB points to bi-directional DMA data */
-	memcpy(&txq->first_tb_bufs[txq->write_ptr], &dev_cmd->hdr,
-	       IWL_FIRST_TB_SIZE);
+	if (!amsdu)
+		memcpy(&txq->first_tb_bufs[txq->write_ptr], &dev_cmd->hdr,
+		       IWL_FIRST_TB_SIZE);
 
 	iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, IWL_FIRST_TB_SIZE);
 
@@ -262,7 +406,11 @@ struct iwl_tfh_tfd *iwl_pcie_gen2_build_tfd(struct iwl_trans *trans,
 	len = sizeof(struct iwl_tx_cmd_gen2) + sizeof(struct iwl_cmd_header) +
 	      ieee80211_hdrlen(hdr->frame_control) - IWL_FIRST_TB_SIZE;
 
-	tb1_len = ALIGN(len, 4);
+	/* do not align A-MSDU to dword as the subframe header aligns it */
+	if (amsdu)
+		tb1_len = len;
+	else
+		tb1_len = ALIGN(len, 4);
 
 	/* map the data for TB1 */
 	tb1_addr = ((u8 *)&dev_cmd->hdr) + IWL_FIRST_TB_SIZE;
@@ -271,8 +419,24 @@ struct iwl_tfh_tfd *iwl_pcie_gen2_build_tfd(struct iwl_trans *trans,
 		goto out_err;
 	iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, tb1_len);
 
-	/* set up TFD's third entry to point to remainder of skb's head */
 	hdr_len = ieee80211_hdrlen(hdr->frame_control);
+
+	if (amsdu) {
+		if (!iwl_pcie_gen2_build_amsdu(trans, skb, tfd,
+					       tb1_len + IWL_FIRST_TB_SIZE,
+					       hdr_len, dev_cmd))
+			goto out_err;
+
+		/*
+		 * building the A-MSDU might have changed this data, so memcpy
+		 * it now
+		 */
+		memcpy(&txq->first_tb_bufs[txq->write_ptr], &dev_cmd->hdr,
+		       IWL_FIRST_TB_SIZE);
+		return tfd;
+	}
+
+	/* set up TFD's third entry to point to remainder of skb's head */
 	tb2_len = skb_headlen(skb) - hdr_len;
 
 	if (tb2_len > 0) {
@@ -303,10 +467,8 @@ struct iwl_tfh_tfd *iwl_pcie_gen2_build_tfd(struct iwl_trans *trans,
 	}
 
 	trace_iwlwifi_dev_tx(trans->dev, skb, tfd, sizeof(*tfd), &dev_cmd->hdr,
-			     IWL_FIRST_TB_SIZE + tb1_len,
-			     skb->data + hdr_len, tb2_len);
-	trace_iwlwifi_dev_tx_data(trans->dev, skb, hdr_len,
-				  skb->len - hdr_len);
+			     IWL_FIRST_TB_SIZE + tb1_len, hdr_len);
+	trace_iwlwifi_dev_tx_data(trans->dev, skb, hdr_len);
 
 	return tfd;
 
@@ -699,7 +861,7 @@ static int iwl_pcie_gen2_send_hcmd_sync(struct iwl_trans *trans,
 	}
 
 	if (!(cmd->flags & CMD_SEND_IN_RFKILL) &&
-	    test_bit(STATUS_RFKILL, &trans->status)) {
+	    test_bit(STATUS_RFKILL_OPMODE, &trans->status)) {
 		IWL_DEBUG_RF_KILL(trans, "RFKILL in SYNC CMD... no rsp\n");
 		ret = -ERFKILL;
 		goto cancel;
@@ -736,7 +898,7 @@ int iwl_trans_pcie_gen2_send_hcmd(struct iwl_trans *trans,
 				  struct iwl_host_cmd *cmd)
 {
 	if (!(cmd->flags & CMD_SEND_IN_RFKILL) &&
-	    test_bit(STATUS_RFKILL, &trans->status)) {
+	    test_bit(STATUS_RFKILL_OPMODE, &trans->status)) {
 		IWL_DEBUG_RF_KILL(trans, "Dropping CMD 0x%x: RF KILL\n",
 				  cmd->id);
 		return -ERFKILL;
@@ -912,7 +1074,7 @@ int iwl_trans_pcie_dyn_txq_alloc(struct iwl_trans *trans,
 	rsp = (void *)hcmd.resp_pkt->data;
 	qid = le16_to_cpu(rsp->queue_number);
 
-	if (qid > ARRAY_SIZE(trans_pcie->txq)) {
+	if (qid >= ARRAY_SIZE(trans_pcie->txq)) {
 		WARN_ONCE(1, "queue index %d unsupported", qid);
 		ret = -EIO;
 		goto error_free_resp;
diff --git a/drivers/net/wireless/intel/iwlwifi/pcie/tx.c b/drivers/net/wireless/intel/iwlwifi/pcie/tx.c
index 386950a2d616..de50418adae5 100644
--- a/drivers/net/wireless/intel/iwlwifi/pcie/tx.c
+++ b/drivers/net/wireless/intel/iwlwifi/pcie/tx.c
@@ -762,7 +762,7 @@ void iwl_pcie_tx_start(struct iwl_trans *trans, u32 scd_base_addr)
 			   reg_val | FH_TX_CHICKEN_BITS_SCD_AUTO_RETRY_EN);
 
 	/* Enable L1-Active */
-	if (trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
+	if (trans->cfg->device_family < IWL_DEVICE_FAMILY_8000)
 		iwl_clear_bits_prph(trans, APMG_PCIDEV_STT_REG,
 				    APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
 }
@@ -1277,13 +1277,14 @@ static int iwl_pcie_txq_set_ratid_map(struct iwl_trans *trans, u16 ra_tid,
  * combined with Traffic ID (QOS priority), in format used by Tx Scheduler */
 #define BUILD_RAxTID(sta_id, tid)	(((sta_id) << 4) + (tid))
 
-void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, u16 ssn,
+bool iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, u16 ssn,
 			       const struct iwl_trans_txq_scd_cfg *cfg,
 			       unsigned int wdg_timeout)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_txq *txq = trans_pcie->txq[txq_id];
 	int fifo = -1;
+	bool scd_bug = false;
 
 	if (test_and_set_bit(txq_id, trans_pcie->queue_used))
 		WARN_ONCE(1, "queue %d already used - expect issues", txq_id);
@@ -1324,6 +1325,23 @@ void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, u16 ssn,
 
 			ssn = txq->read_ptr;
 		}
+	} else {
+		/*
+		 * If we need to move the SCD write pointer by steps of
+		 * 0x40, 0x80 or 0xc0, it gets stuck. Avoids this and let
+		 * the op_mode know by returning true later.
+		 * Do this only in case cfg is NULL since this trick can
+		 * be done only if we have DQA enabled which is true for mvm
+		 * only. And mvm never sets a cfg pointer.
+		 * This is really ugly, but this is the easiest way out for
+		 * this sad hardware issue.
+		 * This bug has been fixed on devices 9000 and up.
+		 */
+		scd_bug = !trans->cfg->mq_rx_supported &&
+			!((ssn - txq->write_ptr) & 0x3f) &&
+			(ssn != txq->write_ptr);
+		if (scd_bug)
+			ssn++;
 	}
 
 	/* Place first TFD at index corresponding to start sequence number.
@@ -1344,10 +1362,8 @@ void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, u16 ssn,
 		iwl_trans_write_mem32(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));
+			SCD_QUEUE_CTX_REG2_VAL(WIN_SIZE, frame_limit) |
+			SCD_QUEUE_CTX_REG2_VAL(FRAME_LIMIT, frame_limit));
 
 		/* Set up status area in SRAM, map to Tx DMA/FIFO, activate */
 		iwl_write_prph(trans, SCD_QUEUE_STATUS_BITS(txq_id),
@@ -1369,6 +1385,8 @@ void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, u16 ssn,
 				    "Activate queue %d WrPtr: %d\n",
 				    txq_id, ssn & 0xff);
 	}
+
+	return scd_bug;
 }
 
 void iwl_trans_pcie_txq_set_shared_mode(struct iwl_trans *trans, u32 txq_id,
@@ -1708,7 +1726,7 @@ void iwl_pcie_hcmd_complete(struct iwl_trans *trans,
 {
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 	u16 sequence = le16_to_cpu(pkt->hdr.sequence);
-	u8 group_id = iwl_cmd_groupid(pkt->hdr.group_id);
+	u8 group_id;
 	u32 cmd_id;
 	int txq_id = SEQ_TO_QUEUE(sequence);
 	int index = SEQ_TO_INDEX(sequence);
@@ -1734,6 +1752,7 @@ void iwl_pcie_hcmd_complete(struct iwl_trans *trans,
 	cmd_index = get_cmd_index(txq, index);
 	cmd = txq->entries[cmd_index].cmd;
 	meta = &txq->entries[cmd_index].meta;
+	group_id = cmd->hdr.group_id;
 	cmd_id = iwl_cmd_id(cmd->hdr.cmd, group_id, 0);
 
 	iwl_pcie_tfd_unmap(trans, meta, txq, index);
@@ -1876,7 +1895,7 @@ static int iwl_pcie_send_hcmd_sync(struct iwl_trans *trans,
 	}
 
 	if (!(cmd->flags & CMD_SEND_IN_RFKILL) &&
-	    test_bit(STATUS_RFKILL, &trans->status)) {
+	    test_bit(STATUS_RFKILL_OPMODE, &trans->status)) {
 		IWL_DEBUG_RF_KILL(trans, "RFKILL in SYNC CMD... no rsp\n");
 		ret = -ERFKILL;
 		goto cancel;
@@ -1913,7 +1932,7 @@ cancel:
 int iwl_trans_pcie_send_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 {
 	if (!(cmd->flags & CMD_SEND_IN_RFKILL) &&
-	    test_bit(STATUS_RFKILL, &trans->status)) {
+	    test_bit(STATUS_RFKILL_OPMODE, &trans->status)) {
 		IWL_DEBUG_RF_KILL(trans, "Dropping CMD 0x%x: RF KILL\n",
 				  cmd->id);
 		return -ERFKILL;
@@ -1980,15 +1999,13 @@ static int iwl_fill_data_tbs(struct iwl_trans *trans, struct sk_buff *skb,
 			     iwl_pcie_get_tfd(trans_pcie, txq, txq->write_ptr),
 			     trans_pcie->tfd_size,
 			     &dev_cmd->hdr, IWL_FIRST_TB_SIZE + tb1_len,
-			     skb->data + hdr_len, tb2_len);
-	trace_iwlwifi_dev_tx_data(trans->dev, skb,
-				  hdr_len, skb->len - hdr_len);
+			     hdr_len);
+	trace_iwlwifi_dev_tx_data(trans->dev, skb, hdr_len);
 	return 0;
 }
 
 #ifdef CONFIG_INET
-static struct iwl_tso_hdr_page *
-get_page_hdr(struct iwl_trans *trans, size_t len)
+struct iwl_tso_hdr_page *get_page_hdr(struct iwl_trans *trans, size_t len)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_tso_hdr_page *p = this_cpu_ptr(trans_pcie->tso_hdr_page);
@@ -2055,8 +2072,7 @@ static int iwl_fill_data_tbs_amsdu(struct iwl_trans *trans, struct sk_buff *skb,
 	trace_iwlwifi_dev_tx(trans->dev, skb,
 			     iwl_pcie_get_tfd(trans_pcie, txq, txq->write_ptr),
 			     trans_pcie->tfd_size,
-			     &dev_cmd->hdr, IWL_FIRST_TB_SIZE + tb1_len,
-			     NULL, 0);
+			     &dev_cmd->hdr, IWL_FIRST_TB_SIZE + tb1_len, 0);
 
 	ip_hdrlen = skb_transport_header(skb) - skb_network_header(skb);
 	snap_ip_tcp_hdrlen = 8 + ip_hdrlen + tcp_hdrlen(skb);
@@ -2141,8 +2157,7 @@ static int iwl_fill_data_tbs_amsdu(struct iwl_trans *trans, struct sk_buff *skb,
 							htons(ETH_P_IPV6),
 						    data_left);
 
-			memcpy(skb_put(csum_skb, tcp_hdrlen(skb)),
-			       tcph, tcp_hdrlen(skb));
+			skb_put_data(csum_skb, tcph, tcp_hdrlen(skb));
 			skb_reset_transport_header(csum_skb);
 			csum_skb->csum_start =
 				(unsigned char *)tcp_hdr(csum_skb) -
@@ -2176,7 +2191,7 @@ static int iwl_fill_data_tbs_amsdu(struct iwl_trans *trans, struct sk_buff *skb,
 			dma_addr_t tb_phys;
 
 			if (trans_pcie->sw_csum_tx)
-				memcpy(skb_put(csum_skb, size), tso.data, size);
+				skb_put_data(csum_skb, tso.data, size);
 
 			tb_phys = dma_map_single(trans->dev, tso.data,
 						 size, DMA_TO_DEVICE);
diff --git a/drivers/net/wireless/intersil/hostap/hostap_80211_rx.c b/drivers/net/wireless/intersil/hostap/hostap_80211_rx.c
index 34dbddbf3f9b..6d8b64ca1a63 100644
--- a/drivers/net/wireless/intersil/hostap/hostap_80211_rx.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_80211_rx.c
@@ -131,8 +131,7 @@ int prism2_rx_80211(struct net_device *dev, struct sk_buff *skb,
 
 	if (prism_header == 1) {
 		struct linux_wlan_ng_prism_hdr *hdr;
-		hdr = (struct linux_wlan_ng_prism_hdr *)
-			skb_push(skb, phdrlen);
+		hdr = skb_push(skb, phdrlen);
 		memset(hdr, 0, phdrlen);
 		hdr->msgcode = LWNG_CAP_DID_BASE;
 		hdr->msglen = sizeof(*hdr);
@@ -153,8 +152,7 @@ hdr->f.status = s; hdr->f.len = l; hdr->f.data = d
 #undef LWNG_SETVAL
 	} else if (prism_header == 2) {
 		struct linux_wlan_ng_cap_hdr *hdr;
-		hdr = (struct linux_wlan_ng_cap_hdr *)
-			skb_push(skb, phdrlen);
+		hdr = skb_push(skb, phdrlen);
 		memset(hdr, 0, phdrlen);
 		hdr->version    = htonl(LWNG_CAPHDR_VERSION);
 		hdr->length     = htonl(phdrlen);
@@ -172,7 +170,7 @@ hdr->f.status = s; hdr->f.len = l; hdr->f.data = d
 		hdr->encoding   = htonl(1); /* cck */
 	} else if (prism_header == 3) {
 		struct hostap_radiotap_rx *hdr;
-		hdr = (struct hostap_radiotap_rx *)skb_push(skb, phdrlen);
+		hdr = skb_push(skb, phdrlen);
 		memset(hdr, 0, phdrlen);
 		hdr->hdr.it_len = cpu_to_le16(phdrlen);
 		hdr->hdr.it_present =
diff --git a/drivers/net/wireless/intersil/hostap/hostap_80211_tx.c b/drivers/net/wireless/intersil/hostap/hostap_80211_tx.c
index 055e11d353ca..c1b10d5117ad 100644
--- a/drivers/net/wireless/intersil/hostap/hostap_80211_tx.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_80211_tx.c
@@ -242,7 +242,7 @@ netdev_tx_t hostap_data_start_xmit(struct sk_buff *skb,
 		memcpy(skb_push(skb, encaps_len), encaps_data, encaps_len);
 	memcpy(skb_push(skb, hdr_len), &hdr, hdr_len);
 	if (use_wds == WDS_OWN_FRAME) {
-		memcpy(skb_put(skb, ETH_ALEN), &hdr.addr4, ETH_ALEN);
+		skb_put_data(skb, &hdr.addr4, ETH_ALEN);
 	}
 
 	iface->stats.tx_packets++;
diff --git a/drivers/net/wireless/intersil/hostap/hostap_ap.c b/drivers/net/wireless/intersil/hostap/hostap_ap.c
index c995ace153ee..eb9cd6fa9c4d 100644
--- a/drivers/net/wireless/intersil/hostap/hostap_ap.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_ap.c
@@ -998,11 +998,9 @@ static void prism2_send_mgmt(struct net_device *dev,
 
 	fc = type_subtype;
 	hdrlen = hostap_80211_get_hdrlen(cpu_to_le16(type_subtype));
-	hdr = (struct ieee80211_hdr *) skb_put(skb, hdrlen);
+	hdr = skb_put_zero(skb, hdrlen);
 	if (body)
-		memcpy(skb_put(skb, body_len), body, body_len);
-
-	memset(hdr, 0, hdrlen);
+		skb_put_data(skb, body, body_len);
 
 	/* FIX: ctrl::ack sending used special HFA384X_TX_CTRL_802_11
 	 * tx_control instead of using local->tx_control */
@@ -1325,8 +1323,7 @@ static char * ap_auth_make_challenge(struct ap_data *ap)
 	}
 
 	skb_reserve(skb, ap->crypt->extra_mpdu_prefix_len);
-	memset(skb_put(skb, WLAN_AUTH_CHALLENGE_LEN), 0,
-	       WLAN_AUTH_CHALLENGE_LEN);
+	skb_put_zero(skb, WLAN_AUTH_CHALLENGE_LEN);
 	if (ap->crypt->encrypt_mpdu(skb, 0, ap->crypt_priv)) {
 		dev_kfree_skb(skb);
 		kfree(tmpbuf);
@@ -2364,7 +2361,7 @@ static void schedule_packet_send(local_info_t *local, struct sta_info *sta)
 		return;
 	}
 
-	hdr = (struct ieee80211_hdr *) skb_put(skb, 16);
+	hdr = skb_put(skb, 16);
 
 	/* Generate a fake pspoll frame to start packet delivery */
 	hdr->frame_control = cpu_to_le16(
diff --git a/drivers/net/wireless/intersil/hostap/hostap_hw.c b/drivers/net/wireless/intersil/hostap/hostap_hw.c
index 04dfd040a650..72b46eaf3de2 100644
--- a/drivers/net/wireless/intersil/hostap/hostap_hw.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_hw.c
@@ -190,7 +190,7 @@ static inline void __hostap_cmd_queue_free(local_info_t *local,
 		}
 	}
 
-	if (atomic_dec_and_test(&entry->usecnt) && entry->del_req)
+	if (refcount_dec_and_test(&entry->usecnt) && entry->del_req)
 		kfree(entry);
 }
 
@@ -228,7 +228,7 @@ static void prism2_clear_cmd_queue(local_info_t *local)
 	spin_lock_irqsave(&local->cmdlock, flags);
 	list_for_each_safe(ptr, n, &local->cmd_queue) {
 		entry = list_entry(ptr, struct hostap_cmd_queue, list);
-		atomic_inc(&entry->usecnt);
+		refcount_inc(&entry->usecnt);
 		printk(KERN_DEBUG "%s: removed pending cmd_queue entry "
 		       "(type=%d, cmd=0x%04x, param0=0x%04x)\n",
 		       local->dev->name, entry->type, entry->cmd,
@@ -350,7 +350,7 @@ static int hfa384x_cmd(struct net_device *dev, u16 cmd, u16 param0,
 	if (entry == NULL)
 		return -ENOMEM;
 
-	atomic_set(&entry->usecnt, 1);
+	refcount_set(&entry->usecnt, 1);
 	entry->type = CMD_SLEEP;
 	entry->cmd = cmd;
 	entry->param0 = param0;
@@ -516,7 +516,7 @@ static int hfa384x_cmd_callback(struct net_device *dev, u16 cmd, u16 param0,
 	if (entry == NULL)
 		return -ENOMEM;
 
-	atomic_set(&entry->usecnt, 1);
+	refcount_set(&entry->usecnt, 1);
 	entry->type = CMD_CALLBACK;
 	entry->cmd = cmd;
 	entry->param0 = param0;
@@ -666,7 +666,7 @@ static void prism2_cmd_ev(struct net_device *dev)
 	if (!list_empty(&local->cmd_queue)) {
 		entry = list_entry(local->cmd_queue.next,
 				   struct hostap_cmd_queue, list);
-		atomic_inc(&entry->usecnt);
+		refcount_inc(&entry->usecnt);
 		list_del_init(&entry->list);
 		local->cmd_queue_len--;
 
@@ -718,7 +718,7 @@ static void prism2_cmd_ev(struct net_device *dev)
 			entry = NULL;
 		}
 		if (entry)
-			atomic_inc(&entry->usecnt);
+			refcount_inc(&entry->usecnt);
 	}
 	spin_unlock(&local->cmdlock);
 
@@ -2005,7 +2005,7 @@ static void prism2_rx(local_info_t *local)
 		goto rx_dropped;
 	}
 	skb->dev = dev;
-	memcpy(skb_put(skb, hdr_len), &rxdesc, hdr_len);
+	skb_put_data(skb, &rxdesc, hdr_len);
 
 	if (len > 0)
 		res = hfa384x_from_bap(dev, BAP0, skb_put(skb, len), len);
@@ -2209,9 +2209,9 @@ static void hostap_tx_callback(local_info_t *local,
 		return;
 	}
 
-	memcpy(skb_put(skb, hdrlen), (void *) &txdesc->frame_control, hdrlen);
+	skb_put_data(skb, (void *)&txdesc->frame_control, hdrlen);
 	if (payload)
-		memcpy(skb_put(skb, len), payload, len);
+		skb_put_data(skb, payload, len);
 
 	skb->dev = local->dev;
 	skb_reset_mac_header(skb);
@@ -2362,8 +2362,7 @@ static void prism2_txexc(local_info_t *local)
 		struct sk_buff *skb;
 		skb = dev_alloc_skb(sizeof(txdesc));
 		if (skb) {
-			memcpy(skb_put(skb, sizeof(txdesc)), &txdesc,
-			       sizeof(txdesc));
+			skb_put_data(skb, &txdesc, sizeof(txdesc));
 			skb_queue_tail(&local->sta_tx_exc_list, skb);
 			tasklet_schedule(&local->sta_tx_exc_tasklet);
 		}
@@ -2460,7 +2459,7 @@ static void prism2_info(local_info_t *local)
 		goto out;
 	}
 
-	memcpy(skb_put(skb, sizeof(info)), &info, sizeof(info));
+	skb_put_data(skb, &info, sizeof(info));
 	if (left > 0 && hfa384x_from_bap(dev, BAP0, skb_put(skb, left), left))
 	{
 		spin_unlock(&local->baplock);
diff --git a/drivers/net/wireless/intersil/hostap/hostap_ioctl.c b/drivers/net/wireless/intersil/hostap/hostap_ioctl.c
index b2c6b065b542..ff153ce29539 100644
--- a/drivers/net/wireless/intersil/hostap/hostap_ioctl.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_ioctl.c
@@ -2544,7 +2544,7 @@ static int prism2_ioctl_priv_prism2_param(struct net_device *dev,
 			ret = -EINVAL;
 		}
 		if (local->iw_mode == IW_MODE_MASTER) {
-			wait_queue_t __wait;
+			wait_queue_entry_t __wait;
 			init_waitqueue_entry(&__wait, current);
 			add_wait_queue(&local->hostscan_wq, &__wait);
 			set_current_state(TASK_INTERRUPTIBLE);
diff --git a/drivers/net/wireless/intersil/hostap/hostap_main.c b/drivers/net/wireless/intersil/hostap/hostap_main.c
index 544fc09dcb62..a3c066f90afc 100644
--- a/drivers/net/wireless/intersil/hostap/hostap_main.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_main.c
@@ -73,7 +73,7 @@ struct net_device * hostap_add_interface(struct local_info *local,
 	dev->mem_end = mdev->mem_end;
 
 	hostap_setup_dev(dev, local, type);
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 
 	sprintf(dev->name, "%s%s", prefix, name);
 	if (!rtnl_locked)
@@ -1039,15 +1039,13 @@ int prism2_sta_send_mgmt(local_info_t *local, u8 *dst, u16 stype,
 	if (skb == NULL)
 		return -ENOMEM;
 
-	mgmt = (struct hostap_ieee80211_mgmt *)
-		skb_put(skb, IEEE80211_MGMT_HDR_LEN);
-	memset(mgmt, 0, IEEE80211_MGMT_HDR_LEN);
+	mgmt = skb_put_zero(skb, IEEE80211_MGMT_HDR_LEN);
 	mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT | stype);
 	memcpy(mgmt->da, dst, ETH_ALEN);
 	memcpy(mgmt->sa, dev->dev_addr, ETH_ALEN);
 	memcpy(mgmt->bssid, dst, ETH_ALEN);
 	if (body)
-		memcpy(skb_put(skb, bodylen), body, bodylen);
+		skb_put_data(skb, body, bodylen);
 
 	meta = (struct hostap_skb_tx_data *) skb->cb;
 	memset(meta, 0, sizeof(*meta));
diff --git a/drivers/net/wireless/intersil/hostap/hostap_wlan.h b/drivers/net/wireless/intersil/hostap/hostap_wlan.h
index ca25283e1c92..5352adb94d50 100644
--- a/drivers/net/wireless/intersil/hostap/hostap_wlan.h
+++ b/drivers/net/wireless/intersil/hostap/hostap_wlan.h
@@ -6,6 +6,7 @@
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/mutex.h>
+#include <linux/refcount.h>
 #include <net/iw_handler.h>
 #include <net/ieee80211_radiotap.h>
 #include <net/lib80211.h>
@@ -557,7 +558,7 @@ struct hostap_cmd_queue {
 	u16 resp0, res;
 	volatile int issued, issuing;
 
-	atomic_t usecnt;
+	refcount_t usecnt;
 	int del_req;
 };
 
diff --git a/drivers/net/wireless/intersil/orinoco/main.c b/drivers/net/wireless/intersil/orinoco/main.c
index d9128bb25e85..28dac36d7c4c 100644
--- a/drivers/net/wireless/intersil/orinoco/main.c
+++ b/drivers/net/wireless/intersil/orinoco/main.c
@@ -396,7 +396,7 @@ int orinoco_process_xmit_skb(struct sk_buff *skb,
 		memcpy(hdr.encap, encaps_hdr, sizeof(encaps_hdr));
 
 		/* Make room for the new header, and copy it in */
-		eh = (struct ethhdr *) skb_push(skb, ENCAPS_OVERHEAD);
+		eh = skb_push(skb, ENCAPS_OVERHEAD);
 		memcpy(eh, &hdr, sizeof(hdr));
 	}
 
@@ -792,7 +792,7 @@ static void orinoco_rx_monitor(struct net_device *dev, u16 rxfid,
 	}
 
 	/* Copy the 802.11 header to the skb */
-	memcpy(skb_put(skb, hdrlen), &(desc->frame_ctl), hdrlen);
+	skb_put_data(skb, &(desc->frame_ctl), hdrlen);
 	skb_reset_mac_header(skb);
 
 	/* If any, copy the data from the card to the skb */
@@ -1029,11 +1029,10 @@ static void orinoco_rx(struct net_device *dev,
 		/* These indicate a SNAP within 802.2 LLC within
 		   802.11 frame which we'll need to de-encapsulate to
 		   the original EthernetII frame. */
-		hdr = (struct ethhdr *)skb_push(skb,
-						ETH_HLEN - ENCAPS_OVERHEAD);
+		hdr = skb_push(skb, ETH_HLEN - ENCAPS_OVERHEAD);
 	} else {
 		/* 802.3 frame - prepend 802.3 header as is */
-		hdr = (struct ethhdr *)skb_push(skb, ETH_HLEN);
+		hdr = skb_push(skb, ETH_HLEN);
 		hdr->h_proto = htons(length);
 	}
 	memcpy(hdr->h_dest, desc->addr1, ETH_ALEN);
diff --git a/drivers/net/wireless/intersil/orinoco/orinoco_usb.c b/drivers/net/wireless/intersil/orinoco/orinoco_usb.c
index 132f5fbda58b..c84fd8490601 100644
--- a/drivers/net/wireless/intersil/orinoco/orinoco_usb.c
+++ b/drivers/net/wireless/intersil/orinoco/orinoco_usb.c
@@ -64,6 +64,7 @@
 #include <linux/etherdevice.h>
 #include <linux/wireless.h>
 #include <linux/firmware.h>
+#include <linux/refcount.h>
 
 #include "mic.h"
 #include "orinoco.h"
@@ -268,7 +269,7 @@ enum ezusb_state {
 
 struct request_context {
 	struct list_head list;
-	atomic_t refcount;
+	refcount_t refcount;
 	struct completion done;	/* Signals that CTX is dead */
 	int killed;
 	struct urb *outurb;	/* OUT for req pkt */
@@ -298,7 +299,7 @@ static inline u8 ezusb_reply_inc(u8 count)
 
 static void ezusb_request_context_put(struct request_context *ctx)
 {
-	if (!atomic_dec_and_test(&ctx->refcount))
+	if (!refcount_dec_and_test(&ctx->refcount))
 		return;
 
 	WARN_ON(!ctx->done.done);
@@ -328,7 +329,7 @@ static void ezusb_request_timerfn(u_long _ctx)
 	} else {
 		ctx->state = EZUSB_CTX_RESP_TIMEOUT;
 		dev_dbg(&ctx->outurb->dev->dev, "couldn't unlink\n");
-		atomic_inc(&ctx->refcount);
+		refcount_inc(&ctx->refcount);
 		ctx->killed = 1;
 		ezusb_ctx_complete(ctx);
 		ezusb_request_context_put(ctx);
@@ -361,7 +362,7 @@ static struct request_context *ezusb_alloc_ctx(struct ezusb_priv *upriv,
 	ctx->out_rid = out_rid;
 	ctx->in_rid = in_rid;
 
-	atomic_set(&ctx->refcount, 1);
+	refcount_set(&ctx->refcount, 1);
 	init_completion(&ctx->done);
 
 	setup_timer(&ctx->timer, ezusb_request_timerfn, (u_long)ctx);
@@ -469,7 +470,7 @@ static void ezusb_req_queue_run(struct ezusb_priv *upriv)
 	list_move_tail(&ctx->list, &upriv->req_active);
 
 	if (ctx->state == EZUSB_CTX_QUEUED) {
-		atomic_inc(&ctx->refcount);
+		refcount_inc(&ctx->refcount);
 		result = usb_submit_urb(ctx->outurb, GFP_ATOMIC);
 		if (result) {
 			ctx->state = EZUSB_CTX_REQSUBMIT_FAIL;
@@ -507,7 +508,7 @@ static void ezusb_req_enqueue_run(struct ezusb_priv *upriv,
 		spin_unlock_irqrestore(&upriv->req_lock, flags);
 		goto done;
 	}
-	atomic_inc(&ctx->refcount);
+	refcount_inc(&ctx->refcount);
 	list_add_tail(&ctx->list, &upriv->req_pending);
 	spin_unlock_irqrestore(&upriv->req_lock, flags);
 
@@ -1477,7 +1478,7 @@ static inline void ezusb_delete(struct ezusb_priv *upriv)
 		int err;
 
 		ctx = list_entry(item, struct request_context, list);
-		atomic_inc(&ctx->refcount);
+		refcount_inc(&ctx->refcount);
 
 		ctx->outurb->transfer_flags |= URB_ASYNC_UNLINK;
 		err = usb_unlink_urb(ctx->outurb);
diff --git a/drivers/net/wireless/intersil/p54/fwio.c b/drivers/net/wireless/intersil/p54/fwio.c
index 4ac6764f4897..52c095c7765f 100644
--- a/drivers/net/wireless/intersil/p54/fwio.c
+++ b/drivers/net/wireless/intersil/p54/fwio.c
@@ -176,8 +176,9 @@ int p54_parse_firmware(struct ieee80211_hw *dev, const struct firmware *fw)
 		 * keeping a extra list for uploaded keys.
 		 */
 
-		priv->used_rxkeys = kzalloc(BITS_TO_LONGS(
-			priv->rx_keycache_size), GFP_KERNEL);
+		priv->used_rxkeys = kcalloc(BITS_TO_LONGS(priv->rx_keycache_size),
+					    sizeof(long),
+					    GFP_KERNEL);
 
 		if (!priv->used_rxkeys)
 			return -ENOMEM;
@@ -205,7 +206,7 @@ static struct sk_buff *p54_alloc_skb(struct p54_common *priv, u16 hdr_flags,
 		return NULL;
 	skb_reserve(skb, priv->tx_hdr_len);
 
-	hdr = (struct p54_hdr *) skb_put(skb, sizeof(*hdr));
+	hdr = skb_put(skb, sizeof(*hdr));
 	hdr->flags = cpu_to_le16(hdr_flags);
 	hdr->len = cpu_to_le16(payload_len);
 	hdr->type = cpu_to_le16(type);
@@ -235,8 +236,7 @@ int p54_download_eeprom(struct p54_common *priv, void *buf,
 
 	mutex_lock(&priv->eeprom_mutex);
 	priv->eeprom = buf;
-	eeprom_hdr = (struct p54_eeprom_lm86 *) skb_put(skb,
-		eeprom_hdr_size + len);
+	eeprom_hdr = skb_put(skb, eeprom_hdr_size + len);
 
 	if (priv->fw_var < 0x509) {
 		eeprom_hdr->v1.offset = cpu_to_le16(offset);
@@ -272,7 +272,7 @@ int p54_update_beacon_tim(struct p54_common *priv, u16 aid, bool set)
 	if (unlikely(!skb))
 		return -ENOMEM;
 
-	tim = (struct p54_tim *) skb_put(skb, sizeof(*tim));
+	tim = skb_put(skb, sizeof(*tim));
 	tim->count = 1;
 	tim->entry[0] = cpu_to_le16(set ? (aid | 0x8000) : aid);
 	p54_tx(priv, skb);
@@ -289,7 +289,7 @@ int p54_sta_unlock(struct p54_common *priv, u8 *addr)
 	if (unlikely(!skb))
 		return -ENOMEM;
 
-	sta = (struct p54_sta_unlock *)skb_put(skb, sizeof(*sta));
+	sta = skb_put(skb, sizeof(*sta));
 	memcpy(sta->addr, addr, ETH_ALEN);
 	p54_tx(priv, skb);
 	return 0;
@@ -309,7 +309,7 @@ int p54_tx_cancel(struct p54_common *priv, __le32 req_id)
 	if (unlikely(!skb))
 		return -ENOMEM;
 
-	cancel = (struct p54_txcancel *)skb_put(skb, sizeof(*cancel));
+	cancel = skb_put(skb, sizeof(*cancel));
 	cancel->req_id = req_id;
 	p54_tx(priv, skb);
 	return 0;
@@ -326,7 +326,7 @@ int p54_setup_mac(struct p54_common *priv)
 	if (!skb)
 		return -ENOMEM;
 
-	setup = (struct p54_setup_mac *) skb_put(skb, sizeof(*setup));
+	setup = skb_put(skb, sizeof(*setup));
 	if (!(priv->hw->conf.flags & IEEE80211_CONF_IDLE)) {
 		switch (priv->mode) {
 		case NL80211_IFTYPE_STATION:
@@ -412,18 +412,18 @@ int p54_scan(struct p54_common *priv, u16 mode, u16 dwell)
 	if (!skb)
 		return -ENOMEM;
 
-	head = (struct p54_scan_head *) skb_put(skb, sizeof(*head));
+	head = skb_put(skb, sizeof(*head));
 	memset(head->scan_params, 0, sizeof(head->scan_params));
 	head->mode = cpu_to_le16(mode);
 	head->dwell = cpu_to_le16(dwell);
 	head->freq = freq;
 
 	if (priv->rxhw == PDR_SYNTH_FRONTEND_LONGBOW) {
-		__le16 *pa_power_points = (__le16 *) skb_put(skb, 2);
+		__le16 *pa_power_points = skb_put(skb, 2);
 		*pa_power_points = cpu_to_le16(0x0c);
 	}
 
-	iq_autocal = (void *) skb_put(skb, sizeof(*iq_autocal));
+	iq_autocal = skb_put(skb, sizeof(*iq_autocal));
 	for (i = 0; i < priv->iq_autocal_len; i++) {
 		if (priv->iq_autocal[i].freq != freq)
 			continue;
@@ -436,9 +436,9 @@ int p54_scan(struct p54_common *priv, u16 mode, u16 dwell)
 		goto err;
 
 	if (priv->rxhw == PDR_SYNTH_FRONTEND_LONGBOW)
-		body = (void *) skb_put(skb, sizeof(body->longbow));
+		body = skb_put(skb, sizeof(body->longbow));
 	else
-		body = (void *) skb_put(skb, sizeof(body->normal));
+		body = skb_put(skb, sizeof(body->normal));
 
 	for (i = 0; i < priv->output_limit->entries; i++) {
 		__le16 *entry_freq = (void *) (priv->output_limit->data +
@@ -499,25 +499,25 @@ int p54_scan(struct p54_common *priv, u16 mode, u16 dwell)
 		goto err;
 
 	if ((priv->fw_var >= 0x500) && (priv->fw_var < 0x509)) {
-		rate = (void *) skb_put(skb, sizeof(*rate));
+		rate = skb_put(skb, sizeof(*rate));
 		rate->basic_rate_mask = cpu_to_le32(priv->basic_rate_mask);
 		for (i = 0; i < sizeof(rate->rts_rates); i++)
 			rate->rts_rates[i] = i;
 	}
 
-	rssi = (struct pda_rssi_cal_entry *) skb_put(skb, sizeof(*rssi));
+	rssi = skb_put(skb, sizeof(*rssi));
 	rssi_data = p54_rssi_find(priv, le16_to_cpu(freq));
 	rssi->mul = cpu_to_le16(rssi_data->mul);
 	rssi->add = cpu_to_le16(rssi_data->add);
 	if (priv->rxhw == PDR_SYNTH_FRONTEND_LONGBOW) {
 		/* Longbow frontend needs ever more */
-		rssi = (void *) skb_put(skb, sizeof(*rssi));
+		rssi = skb_put(skb, sizeof(*rssi));
 		rssi->mul = cpu_to_le16(rssi_data->longbow_unkn);
 		rssi->add = cpu_to_le16(rssi_data->longbow_unk2);
 	}
 
 	if (priv->fw_var >= 0x509) {
-		rate = (void *) skb_put(skb, sizeof(*rate));
+		rate = skb_put(skb, sizeof(*rate));
 		rate->basic_rate_mask = cpu_to_le32(priv->basic_rate_mask);
 		for (i = 0; i < sizeof(rate->rts_rates); i++)
 			rate->rts_rates[i] = i;
@@ -549,7 +549,7 @@ int p54_set_leds(struct p54_common *priv)
 	if (unlikely(!skb))
 		return -ENOMEM;
 
-	led = (struct p54_led *) skb_put(skb, sizeof(*led));
+	led = skb_put(skb, sizeof(*led));
 	led->flags = cpu_to_le16(0x0003);
 	led->mask[0] = led->mask[1] = cpu_to_le16(priv->softled_state);
 	led->delay[0] = cpu_to_le16(1);
@@ -569,7 +569,7 @@ int p54_set_edcf(struct p54_common *priv)
 	if (unlikely(!skb))
 		return -ENOMEM;
 
-	edcf = (struct p54_edcf *)skb_put(skb, sizeof(*edcf));
+	edcf = skb_put(skb, sizeof(*edcf));
 	if (priv->use_short_slot) {
 		edcf->slottime = 9;
 		edcf->sifs = 0x10;
@@ -614,7 +614,7 @@ int p54_set_ps(struct p54_common *priv)
 	if (!skb)
 		return -ENOMEM;
 
-	psm = (struct p54_psm *)skb_put(skb, sizeof(*psm));
+	psm = skb_put(skb, sizeof(*psm));
 	psm->mode = cpu_to_le16(mode);
 	psm->aid = cpu_to_le16(priv->aid);
 	for (i = 0; i < ARRAY_SIZE(psm->intervals); i++) {
@@ -643,7 +643,7 @@ int p54_init_xbow_synth(struct p54_common *priv)
 	if (unlikely(!skb))
 		return -ENOMEM;
 
-	xbow = (struct p54_xbow_synth *)skb_put(skb, sizeof(*xbow));
+	xbow = skb_put(skb, sizeof(*xbow));
 	xbow->magic1 = cpu_to_le16(0x1);
 	xbow->magic2 = cpu_to_le16(0x2);
 	xbow->freq = cpu_to_le16(5390);
@@ -663,7 +663,7 @@ int p54_upload_key(struct p54_common *priv, u8 algo, int slot, u8 idx, u8 len,
 	if (unlikely(!skb))
 		return -ENOMEM;
 
-	rxkey = (struct p54_keycache *)skb_put(skb, sizeof(*rxkey));
+	rxkey = skb_put(skb, sizeof(*rxkey));
 	rxkey->entry = slot;
 	rxkey->key_id = idx;
 	rxkey->key_type = algo;
@@ -743,7 +743,7 @@ int p54_set_groupfilter(struct p54_common *priv)
 	if (!skb)
 		return -ENOMEM;
 
-	grp = (struct p54_group_address_table *)skb_put(skb, sizeof(*grp));
+	grp = skb_put(skb, sizeof(*grp));
 
 	on = !(priv->filter_flags & FIF_ALLMULTI) &&
 	     (priv->mc_maclist_num > 0 &&
diff --git a/drivers/net/wireless/intersil/p54/p54spi.c b/drivers/net/wireless/intersil/p54/p54spi.c
index 7ab2f43ab425..e41bf042352e 100644
--- a/drivers/net/wireless/intersil/p54/p54spi.c
+++ b/drivers/net/wireless/intersil/p54/p54spi.c
@@ -372,9 +372,9 @@ static int p54spi_rx(struct p54s_priv *priv)
 	}
 
 	if (len <= READAHEAD_SZ) {
-		memcpy(skb_put(skb, len), rx_head + 1, len);
+		skb_put_data(skb, rx_head + 1, len);
 	} else {
-		memcpy(skb_put(skb, READAHEAD_SZ), rx_head + 1, READAHEAD_SZ);
+		skb_put_data(skb, rx_head + 1, READAHEAD_SZ);
 		p54spi_spi_read(priv, SPI_ADRS_DMA_DATA,
 				skb_put(skb, len - READAHEAD_SZ),
 				len - READAHEAD_SZ);
diff --git a/drivers/net/wireless/intersil/p54/txrx.c b/drivers/net/wireless/intersil/p54/txrx.c
index 5e1c91a80c58..3a4214d362ff 100644
--- a/drivers/net/wireless/intersil/p54/txrx.c
+++ b/drivers/net/wireless/intersil/p54/txrx.c
@@ -815,8 +815,8 @@ void p54_tx_80211(struct ieee80211_hw *dev,
 		}
 	}
 
-	txhdr = (struct p54_tx_data *) skb_push(skb, sizeof(*txhdr) + padding);
-	hdr = (struct p54_hdr *) skb_push(skb, sizeof(*hdr));
+	txhdr = skb_push(skb, sizeof(*txhdr) + padding);
+	hdr = skb_push(skb, sizeof(*hdr));
 
 	if (padding)
 		hdr_flags |= P54_HDR_FLAG_DATA_ALIGN;
@@ -905,13 +905,13 @@ void p54_tx_80211(struct ieee80211_hw *dev,
 		if (info->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP) {
 			/* reserve space for the MIC key */
 			len += 8;
-			memcpy(skb_put(skb, 8), &(info->control.hw_key->key
-				[NL80211_TKIP_DATA_OFFSET_TX_MIC_KEY]), 8);
+			skb_put_data(skb,
+				     &(info->control.hw_key->key[NL80211_TKIP_DATA_OFFSET_TX_MIC_KEY]),
+				     8);
 		}
 		/* reserve some space for ICV */
 		len += info->control.hw_key->icv_len;
-		memset(skb_put(skb, info->control.hw_key->icv_len), 0,
-		       info->control.hw_key->icv_len);
+		skb_put_zero(skb, info->control.hw_key->icv_len);
 	} else {
 		txhdr->key_type = 0;
 		txhdr->key_len = 0;
diff --git a/drivers/net/wireless/intersil/prism54/islpci_eth.c b/drivers/net/wireless/intersil/prism54/islpci_eth.c
index d83f6332019e..9b0ded733294 100644
--- a/drivers/net/wireless/intersil/prism54/islpci_eth.c
+++ b/drivers/net/wireless/intersil/prism54/islpci_eth.c
@@ -276,10 +276,7 @@ islpci_monitor_rx(islpci_private *priv, struct sk_buff **skb)
 		}
 
 		/* make room for the new header and fill it. */
-		avs =
-		    (struct avs_80211_1_header *) skb_push(*skb,
-							   sizeof (struct
-								   avs_80211_1_header));
+		avs = skb_push(*skb, sizeof(struct avs_80211_1_header));
 
 		avs->version = cpu_to_be32(P80211CAPTURE_VERSION);
 		avs->length = cpu_to_be32(sizeof (struct avs_80211_1_header));
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index 002b25cff5b6..c8852acc1462 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -650,7 +650,7 @@ static void hwsim_send_ps_poll(void *dat, u8 *mac, struct ieee80211_vif *vif)
 	skb = dev_alloc_skb(sizeof(*pspoll));
 	if (!skb)
 		return;
-	pspoll = (void *) skb_put(skb, sizeof(*pspoll));
+	pspoll = skb_put(skb, sizeof(*pspoll));
 	pspoll->frame_control = cpu_to_le16(IEEE80211_FTYPE_CTL |
 					    IEEE80211_STYPE_PSPOLL |
 					    IEEE80211_FCTL_PM);
@@ -681,7 +681,7 @@ static void hwsim_send_nullfunc(struct mac80211_hwsim_data *data, u8 *mac,
 	skb = dev_alloc_skb(sizeof(*hdr));
 	if (!skb)
 		return;
-	hdr = (void *) skb_put(skb, sizeof(*hdr) - ETH_ALEN);
+	hdr = skb_put(skb, sizeof(*hdr) - ETH_ALEN);
 	hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
 					 IEEE80211_STYPE_NULLFUNC |
 					 (ps ? IEEE80211_FCTL_PM : 0));
@@ -848,7 +848,7 @@ static void mac80211_hwsim_monitor_rx(struct ieee80211_hw *hw,
 	if (skb == NULL)
 		return;
 
-	hdr = (struct hwsim_radiotap_hdr *) skb_push(skb, sizeof(*hdr));
+	hdr = skb_push(skb, sizeof(*hdr));
 	hdr->hdr.it_version = PKTHDR_RADIOTAP_VERSION;
 	hdr->hdr.it_pad = 0;
 	hdr->hdr.it_len = cpu_to_le16(sizeof(*hdr));
@@ -892,7 +892,7 @@ static void mac80211_hwsim_monitor_ack(struct ieee80211_channel *chan,
 	if (skb == NULL)
 		return;
 
-	hdr = (struct hwsim_radiotap_ack_hdr *) skb_put(skb, sizeof(*hdr));
+	hdr = skb_put(skb, sizeof(*hdr));
 	hdr->hdr.it_version = PKTHDR_RADIOTAP_VERSION;
 	hdr->hdr.it_pad = 0;
 	hdr->hdr.it_len = cpu_to_le16(sizeof(*hdr));
@@ -904,7 +904,7 @@ static void mac80211_hwsim_monitor_ack(struct ieee80211_channel *chan,
 	flags = IEEE80211_CHAN_2GHZ;
 	hdr->rt_chbitmask = cpu_to_le16(flags);
 
-	hdr11 = (struct ieee80211_hdr *) skb_put(skb, 10);
+	hdr11 = skb_put(skb, 10);
 	hdr11->frame_control = cpu_to_le16(IEEE80211_FTYPE_CTL |
 					   IEEE80211_STYPE_ACK);
 	hdr11->duration_id = cpu_to_le16(0);
@@ -1146,7 +1146,7 @@ static void mac80211_hwsim_add_vendor_rtap(struct sk_buff *skb)
 	 * Note that this code requires the headroom in the SKB
 	 * that was allocated earlier.
 	 */
-	rtap = (void *)skb_push(skb, sizeof(*rtap) + 8 + 4);
+	rtap = skb_push(skb, sizeof(*rtap) + 8 + 4);
 	rtap->oui[0] = HWSIM_RADIOTAP_OUI[0];
 	rtap->oui[1] = HWSIM_RADIOTAP_OUI[1];
 	rtap->oui[2] = HWSIM_RADIOTAP_OUI[2];
@@ -2020,8 +2020,7 @@ static void hw_scan_work(struct work_struct *work)
 			memcpy(mgmt->bssid, req->bssid, ETH_ALEN);
 
 			if (req->ie_len)
-				memcpy(skb_put(probe, req->ie_len), req->ie,
-				       req->ie_len);
+				skb_put_data(probe, req->ie, req->ie_len);
 
 			local_bh_disable();
 			mac80211_hwsim_tx_frame(hwsim->hw, probe,
@@ -2861,7 +2860,7 @@ static const struct net_device_ops hwsim_netdev_ops = {
 static void hwsim_mon_setup(struct net_device *dev)
 {
 	dev->netdev_ops = &hwsim_netdev_ops;
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 	ether_setup(dev);
 	dev->priv_flags |= IFF_NO_QUEUE;
 	dev->type = ARPHRD_IEEE80211_RADIOTAP;
@@ -3021,7 +3020,7 @@ static int hwsim_cloned_frame_received_nl(struct sk_buff *skb_2,
 		goto err;
 
 	/* Copy the data */
-	memcpy(skb_put(skb, frame_data_len), frame_data, frame_data_len);
+	skb_put_data(skb, frame_data, frame_data_len);
 
 	data2 = get_hwsim_data_ref_from_addr(dst);
 	if (!data2)
diff --git a/drivers/net/wireless/marvell/libertas/cfg.c b/drivers/net/wireless/marvell/libertas/cfg.c
index a0463fef79b0..71ba2c8d09b5 100644
--- a/drivers/net/wireless/marvell/libertas/cfg.c
+++ b/drivers/net/wireless/marvell/libertas/cfg.c
@@ -443,17 +443,12 @@ static int lbs_cfg_set_monitor_channel(struct wiphy *wiphy,
 	struct lbs_private *priv = wiphy_priv(wiphy);
 	int ret = -ENOTSUPP;
 
-	lbs_deb_enter_args(LBS_DEB_CFG80211, "freq %d, type %d",
-			   chandef->chan->center_freq,
-			   cfg80211_get_chandef_type(chandef));
-
 	if (cfg80211_get_chandef_type(chandef) != NL80211_CHAN_NO_HT)
 		goto out;
 
 	ret = lbs_set_channel(priv, chandef->chan->hw_value);
 
  out:
-	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
 	return ret;
 }
 
@@ -464,16 +459,12 @@ static int lbs_cfg_set_mesh_channel(struct wiphy *wiphy,
 	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);
 	return ret;
 }
 
@@ -512,8 +503,6 @@ static int lbs_ret_scan(struct lbs_private *priv, unsigned long dummy,
 	int i;
 	int ret = -EILSEQ;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	bsssize = get_unaligned_le16(&scanresp->bssdescriptsize);
 
 	lbs_deb_scan("scan response: %d BSSs (%d bytes); resp size %d bytes\n",
@@ -665,7 +654,6 @@ static int lbs_ret_scan(struct lbs_private *priv, unsigned long dummy,
 	ret = 0;
 
  done:
-	lbs_deb_leave_args(LBS_DEB_SCAN, "ret %d", ret);
 	return ret;
 }
 
@@ -693,11 +681,9 @@ static void lbs_scan_worker(struct work_struct *work)
 	int last_channel;
 	int running, carrier;
 
-	lbs_deb_enter(LBS_DEB_SCAN);
-
 	scan_cmd = kzalloc(LBS_SCAN_MAX_CMD_SIZE, GFP_KERNEL);
 	if (scan_cmd == NULL)
-		goto out_no_scan_cmd;
+		return;
 
 	/* prepare fixed part of scan command */
 	scan_cmd->bsstype = CMD_BSS_TYPE_ANY;
@@ -766,16 +752,11 @@ static void lbs_scan_worker(struct work_struct *work)
 		lbs_deb_scan("scan: waking up waiters\n");
 		wake_up_all(&priv->scan_q);
 	}
-
- out_no_scan_cmd:
-	lbs_deb_leave(LBS_DEB_SCAN);
 }
 
 static void _internal_start_scan(struct lbs_private *priv, bool internal,
 	struct cfg80211_scan_request *request)
 {
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	lbs_deb_scan("scan: ssids %d, channels %d, ie_len %zd\n",
 		request->n_ssids, request->n_channels, request->ie_len);
 
@@ -785,8 +766,6 @@ static void _internal_start_scan(struct lbs_private *priv, bool internal,
 
 	queue_delayed_work(priv->work_thread, &priv->scan_work,
 		msecs_to_jiffies(50));
-
-	lbs_deb_leave(LBS_DEB_CFG80211);
 }
 
 /*
@@ -815,8 +794,6 @@ static int lbs_cfg_scan(struct wiphy *wiphy,
 	struct lbs_private *priv = wiphy_priv(wiphy);
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	if (priv->scan_req || delayed_work_pending(&priv->scan_work)) {
 		/* old scan request not yet processed */
 		ret = -EAGAIN;
@@ -829,7 +806,6 @@ static int lbs_cfg_scan(struct wiphy *wiphy,
 		ret = -EIO;
 
  out:
-	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
 	return ret;
 }
 
@@ -843,18 +819,12 @@ static int lbs_cfg_scan(struct wiphy *wiphy,
 void lbs_send_disconnect_notification(struct lbs_private *priv,
 				      bool locally_generated)
 {
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	cfg80211_disconnected(priv->dev, 0, NULL, 0, locally_generated,
 			      GFP_KERNEL);
-
-	lbs_deb_leave(LBS_DEB_CFG80211);
 }
 
 void lbs_send_mic_failureevent(struct lbs_private *priv, u32 event)
 {
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	cfg80211_michael_mic_failure(priv->dev,
 		priv->assoc_bss,
 		event == MACREG_INT_CODE_MIC_ERR_MULTICAST ?
@@ -863,8 +833,6 @@ void lbs_send_mic_failureevent(struct lbs_private *priv, u32 event)
 		-1,
 		NULL,
 		GFP_KERNEL);
-
-	lbs_deb_leave(LBS_DEB_CFG80211);
 }
 
 
@@ -883,8 +851,6 @@ static int lbs_remove_wep_keys(struct lbs_private *priv)
 	struct cmd_ds_802_11_set_wep cmd;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.keyindex = cpu_to_le16(priv->wep_tx_key);
@@ -892,7 +858,6 @@ static int lbs_remove_wep_keys(struct lbs_private *priv)
 
 	ret = lbs_cmd_with_response(priv, CMD_802_11_SET_WEP, &cmd);
 
-	lbs_deb_leave(LBS_DEB_CFG80211);
 	return ret;
 }
 
@@ -905,8 +870,6 @@ static int lbs_set_wep_keys(struct lbs_private *priv)
 	int i;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	/*
 	 * command         13 00
 	 * size            50 00
@@ -956,7 +919,6 @@ static int lbs_set_wep_keys(struct lbs_private *priv)
 		ret = lbs_remove_wep_keys(priv);
 	}
 
-	lbs_deb_leave(LBS_DEB_CFG80211);
 	return ret;
 }
 
@@ -969,8 +931,6 @@ static int lbs_enable_rsn(struct lbs_private *priv, int enable)
 	struct cmd_ds_802_11_enable_rsn cmd;
 	int ret;
 
-	lbs_deb_enter_args(LBS_DEB_CFG80211, "%d", enable);
-
 	/*
 	 * cmd       2f 00
 	 * size      0c 00
@@ -986,7 +946,6 @@ static int lbs_enable_rsn(struct lbs_private *priv, int enable)
 
 	ret = lbs_cmd_with_response(priv, CMD_802_11_ENABLE_RSN, &cmd);
 
-	lbs_deb_leave(LBS_DEB_CFG80211);
 	return ret;
 }
 
@@ -1014,8 +973,6 @@ static int lbs_set_key_material(struct lbs_private *priv,
 	struct cmd_key_material cmd;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	/*
 	 * Example for WPA (TKIP):
 	 *
@@ -1044,7 +1001,6 @@ static int lbs_set_key_material(struct lbs_private *priv,
 
 	ret = lbs_cmd_with_response(priv, CMD_802_11_KEY_MATERIAL, &cmd);
 
-	lbs_deb_leave(LBS_DEB_CFG80211);
 	return ret;
 }
 
@@ -1061,8 +1017,6 @@ static int lbs_set_authtype(struct lbs_private *priv,
 	struct cmd_ds_802_11_authenticate cmd;
 	int ret;
 
-	lbs_deb_enter_args(LBS_DEB_CFG80211, "%d", sme->auth_type);
-
 	/*
 	 * cmd        11 00
 	 * size       19 00
@@ -1085,7 +1039,6 @@ static int lbs_set_authtype(struct lbs_private *priv,
 	ret = lbs_cmd_with_response(priv, CMD_802_11_AUTHENTICATE, &cmd);
 
  done:
-	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
 	return ret;
 }
 
@@ -1116,8 +1069,6 @@ static int lbs_associate(struct lbs_private *priv,
 	u8 *pos;
 	u8 *tmp;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	if (!cmd) {
 		ret = -ENOMEM;
 		goto done;
@@ -1262,7 +1213,6 @@ static int lbs_associate(struct lbs_private *priv,
 
 	kfree(cmd);
 done:
-	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
 	return ret;
 }
 
@@ -1329,8 +1279,6 @@ static int lbs_cfg_connect(struct wiphy *wiphy, struct net_device *dev,
 	if (dev == priv->mesh_dev)
 		return -EOPNOTSUPP;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	if (!sme->bssid) {
 		struct cfg80211_scan_request *creq;
 
@@ -1442,7 +1390,6 @@ static int lbs_cfg_connect(struct wiphy *wiphy, struct net_device *dev,
  done:
 	if (bss)
 		cfg80211_put_bss(wiphy, bss);
-	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
 	return ret;
 }
 
@@ -1478,8 +1425,6 @@ static int lbs_cfg_disconnect(struct wiphy *wiphy, struct net_device *dev,
 	if (dev == priv->mesh_dev)
 		return -EOPNOTSUPP;
 
-	lbs_deb_enter_args(LBS_DEB_CFG80211, "reason_code %d", reason_code);
-
 	/* store for lbs_cfg_ret_disconnect() */
 	priv->disassoc_reason = reason_code;
 
@@ -1496,8 +1441,6 @@ static int lbs_cfg_set_default_key(struct wiphy *wiphy,
 	if (netdev == priv->mesh_dev)
 		return -EOPNOTSUPP;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	if (key_index != priv->wep_tx_key) {
 		lbs_deb_assoc("set_default_key: to %d\n", key_index);
 		priv->wep_tx_key = key_index;
@@ -1520,8 +1463,6 @@ static int lbs_cfg_add_key(struct wiphy *wiphy, struct net_device *netdev,
 	if (netdev == priv->mesh_dev)
 		return -EOPNOTSUPP;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	lbs_deb_assoc("add_key: cipher 0x%x, mac_addr %pM\n",
 		      params->cipher, mac_addr);
 	lbs_deb_assoc("add_key: key index %d, key len %d\n",
@@ -1575,8 +1516,6 @@ static int lbs_cfg_del_key(struct wiphy *wiphy, struct net_device *netdev,
 			   u8 key_index, bool pairwise, const u8 *mac_addr)
 {
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	lbs_deb_assoc("del_key: key_idx %d, mac_addr %pM\n",
 		      key_index, mac_addr);
 
@@ -1619,8 +1558,6 @@ static int lbs_cfg_get_station(struct wiphy *wiphy, struct net_device *dev,
 	int ret;
 	size_t i;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	sinfo->filled |= BIT(NL80211_STA_INFO_TX_BYTES) |
 			 BIT(NL80211_STA_INFO_TX_PACKETS) |
 			 BIT(NL80211_STA_INFO_RX_BYTES) |
@@ -1675,15 +1612,12 @@ static int lbs_change_intf(struct wiphy *wiphy, struct net_device *dev,
 		return -EOPNOTSUPP;
 	}
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	if (priv->iface_running)
 		ret = lbs_set_iface_type(priv, type);
 
 	if (!ret)
 		priv->wdev->iftype = type;
 
-	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
 	return ret;
 }
 
@@ -1713,8 +1647,6 @@ static void lbs_join_post(struct lbs_private *priv,
 	u8 *fake = fake_ie;
 	struct cfg80211_bss *bss;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	/*
 	 * For cfg80211_inform_bss, we'll need a fake IE, as we can't get
 	 * the real IE from the firmware. So we fabricate a fake IE based on
@@ -1777,8 +1709,6 @@ static void lbs_join_post(struct lbs_private *priv,
 	netif_carrier_on(priv->dev);
 	if (!priv->tx_pending_len)
 		netif_wake_queue(priv->dev);
-
-	lbs_deb_leave(LBS_DEB_CFG80211);
 }
 
 static int lbs_ibss_join_existing(struct lbs_private *priv,
@@ -1790,8 +1720,6 @@ static int lbs_ibss_join_existing(struct lbs_private *priv,
 	u8 preamble = RADIO_PREAMBLE_SHORT;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	/* TODO: set preamble based on scan result */
 	ret = lbs_set_radio(priv, preamble, 1);
 	if (ret)
@@ -1888,7 +1816,6 @@ static int lbs_ibss_join_existing(struct lbs_private *priv,
 	lbs_join_post(priv, params, bss->bssid, bss->capability);
 
  out:
-	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
 	return ret;
 }
 
@@ -1904,8 +1831,6 @@ static int lbs_ibss_start_new(struct lbs_private *priv,
 	int ret = 0;
 	u16 capability;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	ret = lbs_set_radio(priv, preamble, 1);
 	if (ret)
 		goto out;
@@ -1975,7 +1900,6 @@ static int lbs_ibss_start_new(struct lbs_private *priv,
 	lbs_join_post(priv, params, resp->bssid, capability);
 
  out:
-	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
 	return ret;
 }
 
@@ -1990,8 +1914,6 @@ static int lbs_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 	if (dev == priv->mesh_dev)
 		return -EOPNOTSUPP;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	if (!params->chandef.chan) {
 		ret = -ENOTSUPP;
 		goto out;
@@ -2015,7 +1937,6 @@ static int lbs_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 
 
  out:
-	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
 	return ret;
 }
 
@@ -2029,8 +1950,6 @@ static int lbs_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
 	if (dev == priv->mesh_dev)
 		return -EOPNOTSUPP;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	ret = lbs_cmd_with_response(priv, CMD_802_11_AD_HOC_STOP, &cmd);
@@ -2038,7 +1957,6 @@ static int lbs_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
 	/* TODO: consider doing this at MACREG_INT_CODE_ADHOC_BCN_LOST time */
 	lbs_mac_event_disconnected(priv, true);
 
-	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
 	return ret;
 }
 
@@ -2114,8 +2032,6 @@ struct wireless_dev *lbs_cfg_alloc(struct device *dev)
 	int ret = 0;
 	struct wireless_dev *wdev;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
 	if (!wdev)
 		return ERR_PTR(-ENOMEM);
@@ -2127,12 +2043,10 @@ struct wireless_dev *lbs_cfg_alloc(struct device *dev)
 		goto err_wiphy_new;
 	}
 
-	lbs_deb_leave(LBS_DEB_CFG80211);
 	return wdev;
 
  err_wiphy_new:
 	kfree(wdev);
-	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
 	return ERR_PTR(ret);
 }
 
@@ -2155,15 +2069,11 @@ static void lbs_cfg_set_regulatory_hint(struct lbs_private *priv)
 	};
 	size_t i;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	for (i = 0; i < ARRAY_SIZE(regmap); i++)
 		if (regmap[i].code == priv->regioncode) {
 			regulatory_hint(priv->wdev->wiphy, regmap[i].cn);
 			break;
 		}
-
-	lbs_deb_leave(LBS_DEB_CFG80211);
 }
 
 static void lbs_reg_notifier(struct wiphy *wiphy,
@@ -2171,15 +2081,9 @@ static void lbs_reg_notifier(struct wiphy *wiphy,
 {
 	struct lbs_private *priv = wiphy_priv(wiphy);
 
-	lbs_deb_enter_args(LBS_DEB_CFG80211, "cfg80211 regulatory domain "
-			"callback for domain %c%c\n", request->alpha2[0],
-			request->alpha2[1]);
-
 	memcpy(priv->country_code, request->alpha2, sizeof(request->alpha2));
 	if (lbs_iface_active(priv))
 		lbs_set_11d_domain_info(priv);
-
-	lbs_deb_leave(LBS_DEB_CFG80211);
 }
 
 /*
@@ -2192,8 +2096,6 @@ int lbs_cfg_register(struct lbs_private *priv)
 	struct wireless_dev *wdev = priv->wdev;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	wdev->wiphy->max_scan_ssids = 1;
 	wdev->wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
 
@@ -2229,13 +2131,11 @@ int lbs_cfg_register(struct lbs_private *priv)
 
 	lbs_cfg_set_regulatory_hint(priv);
 
-	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
 	return ret;
 }
 
 void lbs_scan_deinit(struct lbs_private *priv)
 {
-	lbs_deb_enter(LBS_DEB_CFG80211);
 	cancel_delayed_work_sync(&priv->scan_work);
 }
 
@@ -2244,8 +2144,6 @@ void lbs_cfg_free(struct lbs_private *priv)
 {
 	struct wireless_dev *wdev = priv->wdev;
 
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
 	if (!wdev)
 		return;
 
diff --git a/drivers/net/wireless/marvell/libertas/cmd.c b/drivers/net/wireless/marvell/libertas/cmd.c
index 033ff881c751..c1f422918737 100644
--- a/drivers/net/wireless/marvell/libertas/cmd.c
+++ b/drivers/net/wireless/marvell/libertas/cmd.c
@@ -91,8 +91,6 @@ int lbs_update_hw_spec(struct lbs_private *priv)
 	int ret = -1;
 	u32 i;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	memcpy(cmd.permanentaddr, priv->current_addr, ETH_ALEN);
@@ -159,14 +157,12 @@ int lbs_update_hw_spec(struct lbs_private *priv)
 	}
 
 out:
-	lbs_deb_leave(LBS_DEB_CMD);
 	return ret;
 }
 
 static int lbs_ret_host_sleep_cfg(struct lbs_private *priv, unsigned long dummy,
 			struct cmd_header *resp)
 {
-	lbs_deb_enter(LBS_DEB_CMD);
 	if (priv->is_host_sleep_activated) {
 		priv->is_host_sleep_configured = 0;
 		if (priv->psstate == PS_STATE_FULL_POWER) {
@@ -176,7 +172,7 @@ static int lbs_ret_host_sleep_cfg(struct lbs_private *priv, unsigned long dummy,
 	} else {
 		priv->is_host_sleep_configured = 1;
 	}
-	lbs_deb_leave(LBS_DEB_CMD);
+
 	return 0;
 }
 
@@ -236,8 +232,6 @@ int lbs_set_ps_mode(struct lbs_private *priv, u16 cmd_action, bool block)
 	struct cmd_ds_802_11_ps_mode cmd;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.action = cpu_to_le16(cmd_action);
@@ -262,7 +256,6 @@ int lbs_set_ps_mode(struct lbs_private *priv, u16 cmd_action, bool block)
 		lbs_cmd_async(priv, CMD_802_11_PS_MODE, &cmd.hdr, sizeof (cmd));
 
 out:
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
@@ -272,8 +265,6 @@ int lbs_cmd_802_11_sleep_params(struct lbs_private *priv, uint16_t cmd_action,
 	struct cmd_ds_802_11_sleep_params cmd;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	if (cmd_action == CMD_ACT_GET) {
 		memset(&cmd, 0, sizeof(cmd));
 	} else {
@@ -304,7 +295,6 @@ int lbs_cmd_802_11_sleep_params(struct lbs_private *priv, uint16_t cmd_action,
 		sp->sp_reserved = le16_to_cpu(cmd.reserved);
 	}
 
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
@@ -312,8 +302,6 @@ static int lbs_wait_for_ds_awake(struct lbs_private *priv)
 {
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	if (priv->is_deep_sleep) {
 		if (!wait_event_interruptible_timeout(priv->ds_awake_q,
 					!priv->is_deep_sleep, (10 * HZ))) {
@@ -322,7 +310,6 @@ static int lbs_wait_for_ds_awake(struct lbs_private *priv)
 		}
 	}
 
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
@@ -330,8 +317,6 @@ int lbs_set_deep_sleep(struct lbs_private *priv, int deep_sleep)
 {
 	int ret =  0;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	if (deep_sleep) {
 		if (priv->is_deep_sleep != 1) {
 			lbs_deb_cmd("deep sleep: sleep\n");
@@ -358,7 +343,6 @@ int lbs_set_deep_sleep(struct lbs_private *priv, int deep_sleep)
 		}
 	}
 
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
@@ -366,10 +350,9 @@ static int lbs_ret_host_sleep_activate(struct lbs_private *priv,
 		unsigned long dummy,
 		struct cmd_header *cmd)
 {
-	lbs_deb_enter(LBS_DEB_FW);
 	priv->is_host_sleep_activated = 1;
 	wake_up_interruptible(&priv->host_sleep_q);
-	lbs_deb_leave(LBS_DEB_FW);
+
 	return 0;
 }
 
@@ -379,8 +362,6 @@ int lbs_set_host_sleep(struct lbs_private *priv, int host_sleep)
 	int ret = 0;
 	uint32_t criteria = EHS_REMOVE_WAKEUP;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	if (host_sleep) {
 		if (priv->is_host_sleep_activated != 1) {
 			memset(&cmd, 0, sizeof(cmd));
@@ -438,8 +419,6 @@ int lbs_set_snmp_mib(struct lbs_private *priv, u32 oid, u16 val)
 	struct cmd_ds_802_11_snmp_mib cmd;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	memset(&cmd, 0, sizeof (cmd));
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.action = cpu_to_le16(CMD_ACT_SET);
@@ -470,7 +449,6 @@ int lbs_set_snmp_mib(struct lbs_private *priv, u32 oid, u16 val)
 	ret = lbs_cmd_with_response(priv, CMD_802_11_SNMP_MIB, &cmd);
 
 out:
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
@@ -488,8 +466,6 @@ int lbs_get_snmp_mib(struct lbs_private *priv, u32 oid, u16 *out_val)
 	struct cmd_ds_802_11_snmp_mib cmd;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	memset(&cmd, 0, sizeof (cmd));
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.action = cpu_to_le16(CMD_ACT_GET);
@@ -513,7 +489,6 @@ int lbs_get_snmp_mib(struct lbs_private *priv, u32 oid, u16 *out_val)
 	}
 
 out:
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
@@ -533,8 +508,6 @@ int lbs_get_tx_power(struct lbs_private *priv, s16 *curlevel, s16 *minlevel,
 	struct cmd_ds_802_11_rf_tx_power cmd;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.action = cpu_to_le16(CMD_ACT_GET);
@@ -548,7 +521,6 @@ int lbs_get_tx_power(struct lbs_private *priv, s16 *curlevel, s16 *minlevel,
 			*maxlevel = cmd.maxlevel;
 	}
 
-	lbs_deb_leave(LBS_DEB_CMD);
 	return ret;
 }
 
@@ -565,8 +537,6 @@ int lbs_set_tx_power(struct lbs_private *priv, s16 dbm)
 	struct cmd_ds_802_11_rf_tx_power cmd;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.action = cpu_to_le16(CMD_ACT_SET);
@@ -576,7 +546,6 @@ int lbs_set_tx_power(struct lbs_private *priv, s16 dbm)
 
 	ret = lbs_cmd_with_response(priv, CMD_802_11_RF_TX_POWER, &cmd);
 
-	lbs_deb_leave(LBS_DEB_CMD);
 	return ret;
 }
 
@@ -608,7 +577,6 @@ int lbs_set_monitor_mode(struct lbs_private *priv, int enable)
 						ARPHRD_ETHER;
 	}
 
-	lbs_deb_leave(LBS_DEB_CMD);
 	return ret;
 }
 
@@ -624,8 +592,6 @@ static int lbs_get_channel(struct lbs_private *priv)
 	struct cmd_ds_802_11_rf_channel cmd;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.action = cpu_to_le16(CMD_OPT_802_11_RF_CHANNEL_GET);
@@ -638,7 +604,6 @@ static int lbs_get_channel(struct lbs_private *priv)
 	lbs_deb_cmd("current radio channel is %d\n", ret);
 
 out:
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
@@ -647,14 +612,12 @@ int lbs_update_channel(struct lbs_private *priv)
 	int ret;
 
 	/* the channel in f/w could be out of sync; get the current channel */
-	lbs_deb_enter(LBS_DEB_ASSOC);
-
 	ret = lbs_get_channel(priv);
 	if (ret > 0) {
 		priv->channel = ret;
 		ret = 0;
 	}
-	lbs_deb_leave_args(LBS_DEB_ASSOC, "ret %d", ret);
+
 	return ret;
 }
 
@@ -674,8 +637,6 @@ int lbs_set_channel(struct lbs_private *priv, u8 channel)
 #endif
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.action = cpu_to_le16(CMD_OPT_802_11_RF_CHANNEL_SET);
@@ -690,7 +651,6 @@ int lbs_set_channel(struct lbs_private *priv, u8 channel)
 		priv->channel);
 
 out:
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
@@ -708,8 +668,6 @@ int lbs_get_rssi(struct lbs_private *priv, s8 *rssi, s8 *nf)
 	struct cmd_ds_802_11_rssi cmd;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	BUG_ON(rssi == NULL);
 	BUG_ON(nf == NULL);
 
@@ -724,7 +682,6 @@ int lbs_get_rssi(struct lbs_private *priv, s8 *rssi, s8 *nf)
 		*rssi = CAL_RSSI(le16_to_cpu(cmd.n_or_snr), le16_to_cpu(cmd.nf));
 	}
 
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
@@ -752,7 +709,6 @@ int lbs_set_11d_domain_info(struct lbs_private *priv)
 	size_t triplet_size;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_11D);
 	if (!priv->country_code[0])
 		goto out;
 
@@ -849,7 +805,6 @@ 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;
 }
 
@@ -869,8 +824,6 @@ int lbs_get_reg(struct lbs_private *priv, u16 reg, u16 offset, u32 *value)
 	struct cmd_ds_reg_access cmd;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	BUG_ON(value == NULL);
 
 	memset(&cmd, 0, sizeof(cmd));
@@ -894,7 +847,6 @@ int lbs_get_reg(struct lbs_private *priv, u16 reg, u16 offset, u32 *value)
 	}
 
 out:
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
@@ -914,8 +866,6 @@ int lbs_set_reg(struct lbs_private *priv, u16 reg, u16 offset, u32 value)
 	struct cmd_ds_reg_access cmd;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.action = cpu_to_le16(CMD_ACT_SET);
@@ -933,7 +883,6 @@ int lbs_set_reg(struct lbs_private *priv, u16 reg, u16 offset, u32 value)
 	ret = lbs_cmd_with_response(priv, reg, &cmd);
 
 out:
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
@@ -943,15 +892,13 @@ static void lbs_queue_cmd(struct lbs_private *priv,
 	unsigned long flags;
 	int addtail = 1;
 
-	lbs_deb_enter(LBS_DEB_HOST);
-
 	if (!cmdnode) {
 		lbs_deb_host("QUEUE_CMD: cmdnode is NULL\n");
-		goto done;
+		return;
 	}
 	if (!cmdnode->cmdbuf->size) {
 		lbs_deb_host("DNLD_CMD: cmd size is zero\n");
-		goto done;
+		return;
 	}
 	cmdnode->result = 0;
 
@@ -979,9 +926,6 @@ static void lbs_queue_cmd(struct lbs_private *priv,
 
 	lbs_deb_host("QUEUE_CMD: inserted command 0x%04x into cmdpendingq\n",
 		     le16_to_cpu(cmdnode->cmdbuf->command));
-
-done:
-	lbs_deb_leave(LBS_DEB_HOST);
 }
 
 static void lbs_submit_command(struct lbs_private *priv,
@@ -994,8 +938,6 @@ static void lbs_submit_command(struct lbs_private *priv,
 	int timeo = 3 * HZ;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_HOST);
-
 	cmd = cmdnode->cmdbuf;
 
 	spin_lock_irqsave(&priv->driver_lock, flags);
@@ -1036,8 +978,6 @@ static void lbs_submit_command(struct lbs_private *priv,
 		/* Setup the timer after transmit command */
 		mod_timer(&priv->command_timer, jiffies + timeo);
 	}
-
-	lbs_deb_leave(LBS_DEB_HOST);
 }
 
 /*
@@ -1047,10 +987,8 @@ static void lbs_submit_command(struct lbs_private *priv,
 static void __lbs_cleanup_and_insert_cmd(struct lbs_private *priv,
 					 struct cmd_ctrl_node *cmdnode)
 {
-	lbs_deb_enter(LBS_DEB_HOST);
-
 	if (!cmdnode)
-		goto out;
+		return;
 
 	cmdnode->callback = NULL;
 	cmdnode->callback_arg = 0;
@@ -1058,8 +996,6 @@ static void __lbs_cleanup_and_insert_cmd(struct lbs_private *priv,
 	memset(cmdnode->cmdbuf, 0, LBS_CMD_BUFFER_SIZE);
 
 	list_add_tail(&cmdnode->list, &priv->cmdfreeq);
- out:
-	lbs_deb_leave(LBS_DEB_HOST);
 }
 
 static void lbs_cleanup_and_insert_cmd(struct lbs_private *priv,
@@ -1107,8 +1043,6 @@ int lbs_set_radio(struct lbs_private *priv, u8 preamble, u8 radio_on)
 	struct cmd_ds_802_11_radio_control cmd;
 	int ret = -EINVAL;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.action = cpu_to_le16(CMD_ACT_SET);
 	cmd.control = 0;
@@ -1141,7 +1075,6 @@ int lbs_set_radio(struct lbs_private *priv, u8 preamble, u8 radio_on)
 	ret = lbs_cmd_with_response(priv, CMD_802_11_RADIO_CONTROL, &cmd);
 
 out:
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
@@ -1149,15 +1082,11 @@ void lbs_set_mac_control(struct lbs_private *priv)
 {
 	struct cmd_ds_mac_control cmd;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.action = cpu_to_le16(priv->mac_control);
 	cmd.reserved = 0;
 
 	lbs_cmd_async(priv, CMD_MAC_CONTROL, &cmd.hdr, sizeof(cmd));
-
-	lbs_deb_leave(LBS_DEB_CMD);
 }
 
 int lbs_set_mac_control_sync(struct lbs_private *priv)
@@ -1165,14 +1094,11 @@ int lbs_set_mac_control_sync(struct lbs_private *priv)
 	struct cmd_ds_mac_control cmd;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.action = cpu_to_le16(priv->mac_control);
 	cmd.reserved = 0;
 	ret = lbs_cmd_with_response(priv, CMD_MAC_CONTROL, &cmd);
 
-	lbs_deb_leave(LBS_DEB_CMD);
 	return ret;
 }
 
@@ -1191,8 +1117,6 @@ int lbs_allocate_cmd_buffer(struct lbs_private *priv)
 	u32 i;
 	struct cmd_ctrl_node *cmdarray;
 
-	lbs_deb_enter(LBS_DEB_HOST);
-
 	/* Allocate and initialize the command array */
 	bufsize = sizeof(struct cmd_ctrl_node) * LBS_NUM_CMD_BUFFERS;
 	if (!(cmdarray = kzalloc(bufsize, GFP_KERNEL))) {
@@ -1219,7 +1143,6 @@ int lbs_allocate_cmd_buffer(struct lbs_private *priv)
 	ret = 0;
 
 done:
-	lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
 	return ret;
 }
 
@@ -1235,8 +1158,6 @@ int lbs_free_cmd_buffer(struct lbs_private *priv)
 	struct cmd_ctrl_node *cmdarray;
 	unsigned int i;
 
-	lbs_deb_enter(LBS_DEB_HOST);
-
 	/* need to check if cmd array is allocated or not */
 	if (priv->cmd_array == NULL) {
 		lbs_deb_host("FREE_CMD_BUF: cmd_array is NULL\n");
@@ -1260,7 +1181,6 @@ int lbs_free_cmd_buffer(struct lbs_private *priv)
 	}
 
 done:
-	lbs_deb_leave(LBS_DEB_HOST);
 	return 0;
 }
 
@@ -1278,8 +1198,6 @@ static struct cmd_ctrl_node *lbs_get_free_cmd_node(struct lbs_private *priv)
 	struct cmd_ctrl_node *tempnode;
 	unsigned long flags;
 
-	lbs_deb_enter(LBS_DEB_HOST);
-
 	if (!priv)
 		return NULL;
 
@@ -1296,7 +1214,6 @@ static struct cmd_ctrl_node *lbs_get_free_cmd_node(struct lbs_private *priv)
 
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
 
-	lbs_deb_leave(LBS_DEB_HOST);
 	return tempnode;
 }
 
@@ -1318,8 +1235,6 @@ int lbs_execute_next_command(struct lbs_private *priv)
 	/* Debug group is LBS_DEB_THREAD and not LBS_DEB_HOST, because the
 	 * only caller to us is lbs_thread() and we get even when a
 	 * data packet is received */
-	lbs_deb_enter(LBS_DEB_THREAD);
-
 	spin_lock_irqsave(&priv->driver_lock, flags);
 
 	if (priv->cur_cmd) {
@@ -1440,7 +1355,6 @@ int lbs_execute_next_command(struct lbs_private *priv)
 
 	ret = 0;
 done:
-	lbs_deb_leave(LBS_DEB_THREAD);
 	return ret;
 }
 
@@ -1449,7 +1363,6 @@ static void lbs_send_confirmsleep(struct lbs_private *priv)
 	unsigned long flags;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_HOST);
 	lbs_deb_hex(LBS_DEB_HOST, "sleep confirm", (u8 *) &confirm_sleep,
 		sizeof(confirm_sleep));
 
@@ -1457,7 +1370,7 @@ static void lbs_send_confirmsleep(struct lbs_private *priv)
 		sizeof(confirm_sleep));
 	if (ret) {
 		netdev_alert(priv->dev, "confirm_sleep failed\n");
-		goto out;
+		return;
 	}
 
 	spin_lock_irqsave(&priv->driver_lock, flags);
@@ -1475,9 +1388,6 @@ static void lbs_send_confirmsleep(struct lbs_private *priv)
 		priv->psstate = PS_STATE_SLEEP;
 
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
-
-out:
-	lbs_deb_leave(LBS_DEB_HOST);
 }
 
 /**
@@ -1493,8 +1403,6 @@ void lbs_ps_confirm_sleep(struct lbs_private *priv)
 	unsigned long flags =0;
 	int allowed = 1;
 
-	lbs_deb_enter(LBS_DEB_HOST);
-
 	spin_lock_irqsave(&priv->driver_lock, flags);
 	if (priv->dnld_sent) {
 		allowed = 0;
@@ -1520,8 +1428,6 @@ void lbs_ps_confirm_sleep(struct lbs_private *priv)
 	} else {
 		lbs_deb_host("sleep confirm has been delayed\n");
 	}
-
-	lbs_deb_leave(LBS_DEB_HOST);
 }
 
 
@@ -1596,8 +1502,6 @@ struct cmd_ctrl_node *__lbs_cmd_async(struct lbs_private *priv,
 {
 	struct cmd_ctrl_node *cmdnode;
 
-	lbs_deb_enter(LBS_DEB_HOST);
-
 	if (priv->surpriseremoved) {
 		lbs_deb_host("PREP_CMD: card removed\n");
 		cmdnode = ERR_PTR(-ENOENT);
@@ -1643,17 +1547,14 @@ struct cmd_ctrl_node *__lbs_cmd_async(struct lbs_private *priv,
 	wake_up(&priv->waitq);
 
  done:
-	lbs_deb_leave_args(LBS_DEB_HOST, "ret %p", cmdnode);
 	return cmdnode;
 }
 
 void lbs_cmd_async(struct lbs_private *priv, uint16_t command,
 	struct cmd_header *in_cmd, int in_cmd_size)
 {
-	lbs_deb_enter(LBS_DEB_CMD);
 	__lbs_cmd_async(priv, command, in_cmd, in_cmd_size,
 		lbs_cmd_async_callback, 0);
-	lbs_deb_leave(LBS_DEB_CMD);
 }
 
 int __lbs_cmd(struct lbs_private *priv, uint16_t command,
@@ -1665,8 +1566,6 @@ int __lbs_cmd(struct lbs_private *priv, uint16_t command,
 	unsigned long flags;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_HOST);
-
 	cmdnode = __lbs_cmd_async(priv, command, in_cmd, in_cmd_size,
 				  callback, callback_arg);
 	if (IS_ERR(cmdnode)) {
@@ -1693,7 +1592,6 @@ int __lbs_cmd(struct lbs_private *priv, uint16_t command,
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
 
 done:
-	lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(__lbs_cmd);
diff --git a/drivers/net/wireless/marvell/libertas/cmdresp.c b/drivers/net/wireless/marvell/libertas/cmdresp.c
index c753e36c2c0e..aaf01619de59 100644
--- a/drivers/net/wireless/marvell/libertas/cmdresp.c
+++ b/drivers/net/wireless/marvell/libertas/cmdresp.c
@@ -32,8 +32,6 @@ void lbs_mac_event_disconnected(struct lbs_private *priv,
 	if (priv->connect_status != LBS_CONNECTED)
 		return;
 
-	lbs_deb_enter(LBS_DEB_ASSOC);
-
 	/*
 	 * Cisco AP sends EAP failure and de-auth in less than 0.5 ms.
 	 * It causes problem in the Supplicant
@@ -61,7 +59,6 @@ void lbs_mac_event_disconnected(struct lbs_private *priv,
 		lbs_deb_cmd("disconnected, so exit PS mode\n");
 		lbs_set_ps_mode(priv, PS_MODE_ACTION_EXIT_PS, false);
 	}
-	lbs_deb_leave(LBS_DEB_ASSOC);
 }
 
 int lbs_process_command_response(struct lbs_private *priv, u8 *data, u32 len)
@@ -72,8 +69,6 @@ int lbs_process_command_response(struct lbs_private *priv, u8 *data, u32 len)
 	unsigned long flags;
 	uint16_t result;
 
-	lbs_deb_enter(LBS_DEB_HOST);
-
 	mutex_lock(&priv->lock);
 	spin_lock_irqsave(&priv->driver_lock, flags);
 
@@ -221,7 +216,6 @@ int lbs_process_command_response(struct lbs_private *priv, u8 *data, u32 len)
 
 done:
 	mutex_unlock(&priv->lock);
-	lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
 	return ret;
 }
 
@@ -230,8 +224,6 @@ int lbs_process_event(struct lbs_private *priv, u32 event)
 	int ret = 0;
 	struct cmd_header cmd;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	switch (event) {
 	case MACREG_INT_CODE_LINK_SENSED:
 		lbs_deb_cmd("EVENT: link sensed\n");
@@ -359,6 +351,5 @@ int lbs_process_event(struct lbs_private *priv, u32 event)
 		break;
 	}
 
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
diff --git a/drivers/net/wireless/marvell/libertas/defs.h b/drivers/net/wireless/marvell/libertas/defs.h
index 407784aca627..d3221444e51c 100644
--- a/drivers/net/wireless/marvell/libertas/defs.h
+++ b/drivers/net/wireless/marvell/libertas/defs.h
@@ -55,15 +55,6 @@ do { if ((lbs_debug & (grp)) == (grp)) \
 #define LBS_DEB_LL(grp, grpnam, fmt, args...) do {} while (0)
 #endif
 
-#define lbs_deb_enter(grp) \
-  LBS_DEB_LL(grp | LBS_DEB_ENTER, " enter", "%s()\n", __func__);
-#define lbs_deb_enter_args(grp, fmt, args...) \
-  LBS_DEB_LL(grp | LBS_DEB_ENTER, " enter", "%s(" fmt ")\n", __func__, ## args);
-#define lbs_deb_leave(grp) \
-  LBS_DEB_LL(grp | LBS_DEB_LEAVE, " leave", "%s()\n", __func__);
-#define lbs_deb_leave_args(grp, fmt, args...) \
-  LBS_DEB_LL(grp | LBS_DEB_LEAVE, " leave", "%s(), " fmt "\n", \
-  __func__, ##args);
 #define lbs_deb_main(fmt, args...)      LBS_DEB_LL(LBS_DEB_MAIN, " main", fmt, ##args)
 #define lbs_deb_net(fmt, args...)       LBS_DEB_LL(LBS_DEB_NET, " net", fmt, ##args)
 #define lbs_deb_mesh(fmt, args...)      LBS_DEB_LL(LBS_DEB_MESH, " mesh", fmt, ##args)
diff --git a/drivers/net/wireless/marvell/libertas/ethtool.c b/drivers/net/wireless/marvell/libertas/ethtool.c
index f955b2d66ed6..693868f16921 100644
--- a/drivers/net/wireless/marvell/libertas/ethtool.c
+++ b/drivers/net/wireless/marvell/libertas/ethtool.c
@@ -41,8 +41,6 @@ static int lbs_ethtool_get_eeprom(struct net_device *dev,
 	struct cmd_ds_802_11_eeprom_access cmd;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_ETHTOOL);
-
 	if (eeprom->offset + eeprom->len > LBS_EEPROM_LEN ||
 	    eeprom->len > LBS_EEPROM_READ_LEN) {
 		ret = -EINVAL;
@@ -59,7 +57,6 @@ static int lbs_ethtool_get_eeprom(struct net_device *dev,
 		memcpy(bytes, cmd.value, eeprom->len);
 
 out:
-	lbs_deb_leave_args(LBS_DEB_ETHTOOL, "ret %d", ret);
         return ret;
 }
 
diff --git a/drivers/net/wireless/marvell/libertas/if_cs.c b/drivers/net/wireless/marvell/libertas/if_cs.c
index f499efc6abcf..7d88223f890b 100644
--- a/drivers/net/wireless/marvell/libertas/if_cs.c
+++ b/drivers/net/wireless/marvell/libertas/if_cs.c
@@ -336,13 +336,11 @@ static inline u32 get_model(u16 manf_id, u16 card_id)
 
 static inline void if_cs_enable_ints(struct if_cs_card *card)
 {
-	lbs_deb_enter(LBS_DEB_CS);
 	if_cs_write16(card, IF_CS_HOST_INT_MASK, 0);
 }
 
 static inline void if_cs_disable_ints(struct if_cs_card *card)
 {
-	lbs_deb_enter(LBS_DEB_CS);
 	if_cs_write16(card, IF_CS_HOST_INT_MASK, IF_CS_BIT_MASK);
 }
 
@@ -355,7 +353,6 @@ static int if_cs_send_cmd(struct lbs_private *priv, u8 *buf, u16 nb)
 	int ret = -1;
 	int loops = 0;
 
-	lbs_deb_enter(LBS_DEB_CS);
 	if_cs_disable_ints(card);
 
 	/* Is hardware ready? */
@@ -388,7 +385,6 @@ static int if_cs_send_cmd(struct lbs_private *priv, u8 *buf, u16 nb)
 
 done:
 	if_cs_enable_ints(card);
-	lbs_deb_leave_args(LBS_DEB_CS, "ret %d", ret);
 	return ret;
 }
 
@@ -400,7 +396,6 @@ static void if_cs_send_data(struct lbs_private *priv, u8 *buf, u16 nb)
 	struct if_cs_card *card = (struct if_cs_card *)priv->card;
 	u16 status;
 
-	lbs_deb_enter(LBS_DEB_CS);
 	if_cs_disable_ints(card);
 
 	status = if_cs_read16(card, IF_CS_CARD_STATUS);
@@ -416,8 +411,6 @@ static void if_cs_send_data(struct lbs_private *priv, u8 *buf, u16 nb)
 	if_cs_write16(card, IF_CS_HOST_STATUS, IF_CS_BIT_TX);
 	if_cs_write16(card, IF_CS_HOST_INT_CAUSE, IF_CS_BIT_TX);
 	if_cs_enable_ints(card);
-
-	lbs_deb_leave(LBS_DEB_CS);
 }
 
 /*
@@ -429,8 +422,6 @@ static int if_cs_receive_cmdres(struct lbs_private *priv, u8 *data, u32 *len)
 	int ret = -1;
 	u16 status;
 
-	lbs_deb_enter(LBS_DEB_CS);
-
 	/* is hardware ready? */
 	status = if_cs_read16(priv->card, IF_CS_CARD_STATUS);
 	if ((status & IF_CS_BIT_RESP) == 0) {
@@ -463,7 +454,6 @@ static int if_cs_receive_cmdres(struct lbs_private *priv, u8 *data, u32 *len)
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
 
 out:
-	lbs_deb_leave_args(LBS_DEB_CS, "ret %d, len %d", ret, *len);
 	return ret;
 }
 
@@ -473,8 +463,6 @@ static struct sk_buff *if_cs_receive_data(struct lbs_private *priv)
 	u16 len;
 	u8 *data;
 
-	lbs_deb_enter(LBS_DEB_CS);
-
 	len = if_cs_read16(priv->card, IF_CS_READ_LEN);
 	if (len == 0 || len > MRVDRV_ETH_RX_PACKET_BUFFER_SIZE) {
 		netdev_err(priv->dev,
@@ -501,7 +489,6 @@ dat_err:
 	if_cs_write16(priv->card, IF_CS_HOST_INT_CAUSE, IF_CS_BIT_RX);
 
 out:
-	lbs_deb_leave_args(LBS_DEB_CS, "ret %p", skb);
 	return skb;
 }
 
@@ -511,8 +498,6 @@ static irqreturn_t if_cs_interrupt(int irq, void *data)
 	struct lbs_private *priv = card->priv;
 	u16 cause;
 
-	lbs_deb_enter(LBS_DEB_CS);
-
 	/* Ask card interrupt cause register if there is something for us */
 	cause = if_cs_read16(card, IF_CS_CARD_INT_CAUSE);
 	lbs_deb_cs("cause 0x%04x\n", cause);
@@ -569,7 +554,6 @@ static irqreturn_t if_cs_interrupt(int irq, void *data)
 	/* Clear interrupt cause */
 	if_cs_write16(card, IF_CS_CARD_INT_CAUSE, cause & IF_CS_BIT_MASK);
 
-	lbs_deb_leave(LBS_DEB_CS);
 	return IRQ_HANDLED;
 }
 
@@ -591,8 +575,6 @@ static int if_cs_prog_helper(struct if_cs_card *card, const struct firmware *fw)
 	int sent = 0;
 	u8  scratch;
 
-	lbs_deb_enter(LBS_DEB_CS);
-
 	/*
 	 * This is the only place where an unaligned register access happens on
 	 * the CF8305 card, therefore for the sake of speed of the driver, we do
@@ -671,7 +653,6 @@ static int if_cs_prog_helper(struct if_cs_card *card, const struct firmware *fw)
 	}
 
 done:
-	lbs_deb_leave_args(LBS_DEB_CS, "ret %d", ret);
 	return ret;
 }
 
@@ -683,8 +664,6 @@ static int if_cs_prog_real(struct if_cs_card *card, const struct firmware *fw)
 	int len = 0;
 	int sent;
 
-	lbs_deb_enter(LBS_DEB_CS);
-
 	lbs_deb_cs("fw size %td\n", fw->size);
 
 	ret = if_cs_poll_while_fw_download(card, IF_CS_SQ_READ_LOW,
@@ -734,7 +713,6 @@ static int if_cs_prog_real(struct if_cs_card *card, const struct firmware *fw)
 		pr_err("firmware download failed\n");
 
 done:
-	lbs_deb_leave_args(LBS_DEB_CS, "ret %d", ret);
 	return ret;
 }
 
@@ -792,8 +770,6 @@ static int if_cs_host_to_card(struct lbs_private *priv,
 {
 	int ret = -1;
 
-	lbs_deb_enter_args(LBS_DEB_CS, "type %d, bytes %d", type, nb);
-
 	switch (type) {
 	case MVMS_DAT:
 		priv->dnld_sent = DNLD_DATA_SENT;
@@ -809,7 +785,6 @@ static int if_cs_host_to_card(struct lbs_private *priv,
 			   __func__, type);
 	}
 
-	lbs_deb_leave_args(LBS_DEB_CS, "ret %d", ret);
 	return ret;
 }
 
@@ -818,14 +793,10 @@ static void if_cs_release(struct pcmcia_device *p_dev)
 {
 	struct if_cs_card *card = p_dev->priv;
 
-	lbs_deb_enter(LBS_DEB_CS);
-
 	free_irq(p_dev->irq, card);
 	pcmcia_disable_device(p_dev);
 	if (card->iobase)
 		ioport_unmap(card->iobase);
-
-	lbs_deb_leave(LBS_DEB_CS);
 }
 
 
@@ -850,8 +821,6 @@ static int if_cs_probe(struct pcmcia_device *p_dev)
 	struct lbs_private *priv;
 	struct if_cs_card *card;
 
-	lbs_deb_enter(LBS_DEB_CS);
-
 	card = kzalloc(sizeof(struct if_cs_card), GFP_KERNEL);
 	if (!card)
 		goto out;
@@ -961,7 +930,6 @@ out2:
 out1:
 	pcmcia_disable_device(p_dev);
 out:
-	lbs_deb_leave_args(LBS_DEB_CS, "ret %d", ret);
 	return ret;
 }
 
@@ -970,15 +938,11 @@ static void if_cs_detach(struct pcmcia_device *p_dev)
 {
 	struct if_cs_card *card = p_dev->priv;
 
-	lbs_deb_enter(LBS_DEB_CS);
-
 	lbs_stop_card(card->priv);
 	lbs_remove_card(card->priv);
 	if_cs_disable_ints(card);
 	if_cs_release(p_dev);
 	kfree(card);
-
-	lbs_deb_leave(LBS_DEB_CS);
 }
 
 
diff --git a/drivers/net/wireless/marvell/libertas/if_sdio.c b/drivers/net/wireless/marvell/libertas/if_sdio.c
index 47f4a14c84fe..2300e796c6ab 100644
--- a/drivers/net/wireless/marvell/libertas/if_sdio.c
+++ b/drivers/net/wireless/marvell/libertas/if_sdio.c
@@ -211,8 +211,6 @@ static int if_sdio_handle_cmd(struct if_sdio_card *card,
 	unsigned long flags;
 	u8 i;
 
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	if (size > LBS_CMD_BUFFER_SIZE) {
 		lbs_deb_sdio("response packet too large (%d bytes)\n",
 			(int)size);
@@ -233,7 +231,6 @@ static int if_sdio_handle_cmd(struct if_sdio_card *card,
 	ret = 0;
 
 out:
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
 	return ret;
 }
 
@@ -242,9 +239,6 @@ static int if_sdio_handle_data(struct if_sdio_card *card,
 {
 	int ret;
 	struct sk_buff *skb;
-	char *data;
-
-	lbs_deb_enter(LBS_DEB_SDIO);
 
 	if (size > MRVDRV_ETH_RX_PACKET_BUFFER_SIZE) {
 		lbs_deb_sdio("response packet too large (%d bytes)\n",
@@ -261,17 +255,13 @@ static int if_sdio_handle_data(struct if_sdio_card *card,
 
 	skb_reserve(skb, NET_IP_ALIGN);
 
-	data = skb_put(skb, size);
-
-	memcpy(data, buffer, size);
+	skb_put_data(skb, buffer, size);
 
 	lbs_process_rxed_packet(card->priv, skb);
 
 	ret = 0;
 
 out:
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
-
 	return ret;
 }
 
@@ -281,8 +271,6 @@ static int if_sdio_handle_event(struct if_sdio_card *card,
 	int ret;
 	u32 event;
 
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	if (card->model == MODEL_8385) {
 		event = sdio_readb(card->func, IF_SDIO_EVENT, &ret);
 		if (ret)
@@ -307,8 +295,6 @@ static int if_sdio_handle_event(struct if_sdio_card *card,
 	ret = 0;
 
 out:
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
-
 	return ret;
 }
 
@@ -337,8 +323,6 @@ static int if_sdio_card_to_host(struct if_sdio_card *card)
 	int ret;
 	u16 size, type, chunk;
 
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	size = if_sdio_read_rx_len(card, &ret);
 	if (ret)
 		goto out;
@@ -410,8 +394,6 @@ out:
 	if (ret)
 		pr_err("problem fetching packet from firmware\n");
 
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
-
 	return ret;
 }
 
@@ -422,8 +404,6 @@ static void if_sdio_host_to_card_worker(struct work_struct *work)
 	int ret;
 	unsigned long flags;
 
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	card = container_of(work, struct if_sdio_card, packet_worker);
 
 	while (1) {
@@ -451,8 +431,6 @@ static void if_sdio_host_to_card_worker(struct work_struct *work)
 
 		kfree(packet);
 	}
-
-	lbs_deb_leave(LBS_DEB_SDIO);
 }
 
 /********************************************************************/
@@ -471,8 +449,6 @@ static int if_sdio_prog_helper(struct if_sdio_card *card,
 	const u8 *firmware;
 	size_t size;
 
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	chunk_buffer = kzalloc(64, GFP_KERNEL);
 	if (!chunk_buffer) {
 		ret = -ENOMEM;
@@ -556,7 +532,6 @@ out:
 	if (ret)
 		pr_err("failed to load helper firmware\n");
 
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
 	return ret;
 }
 
@@ -570,8 +545,6 @@ static int if_sdio_prog_real(struct if_sdio_card *card,
 	const u8 *firmware;
 	size_t size, req_size;
 
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	chunk_buffer = kzalloc(512, GFP_KERNEL);
 	if (!chunk_buffer) {
 		ret = -ENOMEM;
@@ -691,7 +664,6 @@ out:
 	if (ret)
 		pr_err("failed to load firmware\n");
 
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
 	return ret;
 }
 
@@ -725,8 +697,6 @@ static int if_sdio_prog_firmware(struct if_sdio_card *card)
 	int ret;
 	u16 scratch;
 
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	/*
 	 * Disable interrupts
 	 */
@@ -769,7 +739,6 @@ static int if_sdio_prog_firmware(struct if_sdio_card *card)
 				     fw_table, if_sdio_do_prog_firmware);
 
 out:
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
 	return ret;
 }
 
@@ -948,8 +917,6 @@ static int if_sdio_host_to_card(struct lbs_private *priv,
 	u16 size;
 	unsigned long flags;
 
-	lbs_deb_enter_args(LBS_DEB_SDIO, "type %d, bytes %d", type, nb);
-
 	card = priv->card;
 
 	if (nb > (65536 - sizeof(struct if_sdio_packet) - 4)) {
@@ -1013,8 +980,6 @@ static int if_sdio_host_to_card(struct lbs_private *priv,
 	ret = 0;
 
 out:
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
-
 	return ret;
 }
 
@@ -1040,7 +1005,6 @@ static int if_sdio_exit_deep_sleep(struct lbs_private *priv)
 	struct if_sdio_card *card = priv->card;
 	int ret = -1;
 
-	lbs_deb_enter(LBS_DEB_SDIO);
 	sdio_claim_host(card->func);
 
 	sdio_writeb(card->func, HOST_POWER_UP, CONFIGURATION_REG, &ret);
@@ -1048,7 +1012,7 @@ static int if_sdio_exit_deep_sleep(struct lbs_private *priv)
 		netdev_err(priv->dev, "sdio_writeb failed!\n");
 
 	sdio_release_host(card->func);
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
+
 	return ret;
 }
 
@@ -1057,7 +1021,6 @@ static int if_sdio_reset_deep_sleep_wakeup(struct lbs_private *priv)
 	struct if_sdio_card *card = priv->card;
 	int ret = -1;
 
-	lbs_deb_enter(LBS_DEB_SDIO);
 	sdio_claim_host(card->func);
 
 	sdio_writeb(card->func, 0, CONFIGURATION_REG, &ret);
@@ -1065,7 +1028,7 @@ static int if_sdio_reset_deep_sleep_wakeup(struct lbs_private *priv)
 		netdev_err(priv->dev, "sdio_writeb failed!\n");
 
 	sdio_release_host(card->func);
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
+
 	return ret;
 
 }
@@ -1143,19 +1106,17 @@ static void if_sdio_interrupt(struct sdio_func *func)
 	struct if_sdio_card *card;
 	u8 cause;
 
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	card = sdio_get_drvdata(func);
 
 	cause = sdio_readb(card->func, IF_SDIO_H_INT_STATUS, &ret);
 	if (ret || !cause)
-		goto out;
+		return;
 
 	lbs_deb_sdio("interrupt: 0x%X\n", (unsigned)cause);
 
 	sdio_writeb(card->func, ~cause, IF_SDIO_H_INT_STATUS, &ret);
 	if (ret)
-		goto out;
+		return;
 
 	/*
 	 * Ignore the define name, this really means the card has
@@ -1169,13 +1130,8 @@ static void if_sdio_interrupt(struct sdio_func *func)
 	if (cause & IF_SDIO_H_INT_UPLD) {
 		ret = if_sdio_card_to_host(card);
 		if (ret)
-			goto out;
+			return;
 	}
-
-	ret = 0;
-
-out:
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
 }
 
 static int if_sdio_probe(struct sdio_func *func,
@@ -1187,8 +1143,6 @@ static int if_sdio_probe(struct sdio_func *func,
 	unsigned int model;
 	struct if_sdio_packet *packet;
 
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	for (i = 0;i < func->card->num_info;i++) {
 		if (sscanf(func->card->info[i],
 				"802.11 SDIO ID: %x", &model) == 1)
@@ -1273,8 +1227,6 @@ static int if_sdio_probe(struct sdio_func *func,
 		goto err_activate_card;
 
 out:
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
-
 	return ret;
 
 err_activate_card:
@@ -1298,8 +1250,6 @@ static void if_sdio_remove(struct sdio_func *func)
 	struct if_sdio_card *card;
 	struct if_sdio_packet *packet;
 
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	card = sdio_get_drvdata(func);
 
 	/* Undo decrement done above in if_sdio_probe */
@@ -1335,7 +1285,6 @@ static void if_sdio_remove(struct sdio_func *func)
 	}
 
 	kfree(card);
-	lbs_deb_leave(LBS_DEB_SDIO);
 }
 
 static int if_sdio_suspend(struct device *dev)
@@ -1415,8 +1364,6 @@ static int __init if_sdio_init_module(void)
 {
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	printk(KERN_INFO "libertas_sdio: Libertas SDIO driver\n");
 	printk(KERN_INFO "libertas_sdio: Copyright Pierre Ossman\n");
 
@@ -1425,23 +1372,17 @@ static int __init if_sdio_init_module(void)
 	/* Clear the flag in case user removes the card. */
 	user_rmmod = 0;
 
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
-
 	return ret;
 }
 
 static void __exit if_sdio_exit_module(void)
 {
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	/* Set the flag as user is removing this module. */
 	user_rmmod = 1;
 
 	cancel_work_sync(&card_reset_work);
 
 	sdio_unregister_driver(&if_sdio_driver);
-
-	lbs_deb_leave(LBS_DEB_SDIO);
 }
 
 module_init(if_sdio_init_module);
diff --git a/drivers/net/wireless/marvell/libertas/if_spi.c b/drivers/net/wireless/marvell/libertas/if_spi.c
index 7b4955cc38db..e9aec6cb1105 100644
--- a/drivers/net/wireless/marvell/libertas/if_spi.c
+++ b/drivers/net/wireless/marvell/libertas/if_spi.c
@@ -466,8 +466,6 @@ static int if_spi_prog_helper_firmware(struct if_spi_card *card,
 	const u8 *fw;
 	u8 temp[HELPER_FW_LOAD_CHUNK_SZ];
 
-	lbs_deb_enter(LBS_DEB_SPI);
-
 	err = spu_set_interrupt_mode(card, 1, 0);
 	if (err)
 		goto out;
@@ -533,7 +531,7 @@ static int if_spi_prog_helper_firmware(struct if_spi_card *card,
 out:
 	if (err)
 		pr_err("failed to load helper firmware (err=%d)\n", err);
-	lbs_deb_leave_args(LBS_DEB_SPI, "err %d", err);
+
 	return err;
 }
 
@@ -588,8 +586,6 @@ static int if_spi_prog_main_firmware(struct if_spi_card *card,
 	const u8 *fw;
 	u16 num_crc_errs;
 
-	lbs_deb_enter(LBS_DEB_SPI);
-
 	err = spu_set_interrupt_mode(card, 1, 0);
 	if (err)
 		goto out;
@@ -666,7 +662,7 @@ static int if_spi_prog_main_firmware(struct if_spi_card *card,
 out:
 	if (err)
 		pr_err("failed to load firmware (err=%d)\n", err);
-	lbs_deb_leave_args(LBS_DEB_SPI, "err %d", err);
+
 	return err;
 }
 
@@ -699,8 +695,6 @@ static int if_spi_c2h_cmd(struct if_spi_card *card)
 	 */
 	BUILD_BUG_ON(IF_SPI_CMD_BUF_SIZE % 4 != 0);
 
-	lbs_deb_enter(LBS_DEB_SPI);
-
 	/* How many bytes are there to read? */
 	err = spu_read_u16(card, IF_SPI_SCRATCH_2_REG, &len);
 	if (err)
@@ -735,7 +729,7 @@ static int if_spi_c2h_cmd(struct if_spi_card *card)
 out:
 	if (err)
 		netdev_err(priv->dev, "%s: err=%d\n", __func__, err);
-	lbs_deb_leave(LBS_DEB_SPI);
+
 	return err;
 }
 
@@ -748,8 +742,6 @@ static int if_spi_c2h_data(struct if_spi_card *card)
 	u16 len;
 	int err = 0;
 
-	lbs_deb_enter(LBS_DEB_SPI);
-
 	/* How many bytes are there to read? */
 	err = spu_read_u16(card, IF_SPI_SCRATCH_1_REG, &len);
 	if (err)
@@ -794,7 +786,7 @@ free_skb:
 out:
 	if (err)
 		netdev_err(priv->dev, "%s: err=%d\n", __func__, err);
-	lbs_deb_leave(LBS_DEB_SPI);
+
 	return err;
 }
 
@@ -870,8 +862,6 @@ static void if_spi_host_to_card_worker(struct work_struct *work)
 	card = container_of(work, struct if_spi_card, packet_work);
 	priv = card->priv;
 
-	lbs_deb_enter(LBS_DEB_SPI);
-
 	/*
 	 * Read the host interrupt status register to see what we
 	 * can do.
@@ -943,8 +933,6 @@ static void if_spi_host_to_card_worker(struct work_struct *work)
 err:
 	if (err)
 		netdev_err(priv->dev, "%s: got error %d\n", __func__, err);
-
-	lbs_deb_leave(LBS_DEB_SPI);
 }
 
 /*
@@ -962,8 +950,6 @@ static int if_spi_host_to_card(struct lbs_private *priv,
 	struct if_spi_packet *packet;
 	u16 blen;
 
-	lbs_deb_enter_args(LBS_DEB_SPI, "type %d, bytes %d", type, nb);
-
 	if (nb == 0) {
 		netdev_err(priv->dev, "%s: invalid size requested: %d\n",
 			   __func__, nb);
@@ -1004,7 +990,6 @@ static int if_spi_host_to_card(struct lbs_private *priv,
 	/* Queue spi xfer work */
 	queue_work(card->workqueue, &card->packet_work);
 out:
-	lbs_deb_leave_args(LBS_DEB_SPI, "err=%d", err);
 	return err;
 }
 
@@ -1035,8 +1020,6 @@ static int if_spi_init_card(struct if_spi_card *card)
 	const struct firmware *helper = NULL;
 	const struct firmware *mainfw = NULL;
 
-	lbs_deb_enter(LBS_DEB_SPI);
-
 	err = spu_init(card, card->pdata->use_dummy_writes);
 	if (err)
 		goto out;
@@ -1093,7 +1076,6 @@ static int if_spi_init_card(struct if_spi_card *card)
 		goto out;
 
 out:
-	lbs_deb_leave_args(LBS_DEB_SPI, "err %d\n", err);
 	return err;
 }
 
@@ -1126,8 +1108,6 @@ static int if_spi_probe(struct spi_device *spi)
 	struct libertas_spi_platform_data *pdata = dev_get_platdata(&spi->dev);
 	int err = 0;
 
-	lbs_deb_enter(LBS_DEB_SPI);
-
 	if (!pdata) {
 		err = -EINVAL;
 		goto out;
@@ -1221,7 +1201,6 @@ teardown:
 	if (pdata->teardown)
 		pdata->teardown(spi);
 out:
-	lbs_deb_leave_args(LBS_DEB_SPI, "err %d\n", err);
 	return err;
 }
 
@@ -1231,7 +1210,6 @@ static int libertas_spi_remove(struct spi_device *spi)
 	struct lbs_private *priv = card->priv;
 
 	lbs_deb_spi("libertas_spi_remove\n");
-	lbs_deb_enter(LBS_DEB_SPI);
 
 	cancel_work_sync(&card->resume_work);
 
@@ -1243,7 +1221,7 @@ static int libertas_spi_remove(struct spi_device *spi)
 	if (card->pdata->teardown)
 		card->pdata->teardown(spi);
 	free_if_spi_card(card);
-	lbs_deb_leave(LBS_DEB_SPI);
+
 	return 0;
 }
 
@@ -1297,18 +1275,16 @@ static struct spi_driver libertas_spi_driver = {
 static int __init if_spi_init_module(void)
 {
 	int ret = 0;
-	lbs_deb_enter(LBS_DEB_SPI);
+
 	printk(KERN_INFO "libertas_spi: Libertas SPI driver\n");
 	ret = spi_register_driver(&libertas_spi_driver);
-	lbs_deb_leave(LBS_DEB_SPI);
+
 	return ret;
 }
 
 static void __exit if_spi_exit_module(void)
 {
-	lbs_deb_enter(LBS_DEB_SPI);
 	spi_unregister_driver(&libertas_spi_driver);
-	lbs_deb_leave(LBS_DEB_SPI);
 }
 
 module_init(if_spi_init_module);
diff --git a/drivers/net/wireless/marvell/libertas/if_usb.c b/drivers/net/wireless/marvell/libertas/if_usb.c
index aba0c9995b14..e53025ea6689 100644
--- a/drivers/net/wireless/marvell/libertas/if_usb.c
+++ b/drivers/net/wireless/marvell/libertas/if_usb.c
@@ -111,8 +111,6 @@ static void if_usb_write_bulk_callback(struct urb *urb)
  */
 static void if_usb_free(struct if_usb_card *cardp)
 {
-	lbs_deb_enter(LBS_DEB_USB);
-
 	/* Unlink tx & rx urb */
 	usb_kill_urb(cardp->tx_urb);
 	usb_kill_urb(cardp->rx_urb);
@@ -125,8 +123,6 @@ static void if_usb_free(struct if_usb_card *cardp)
 
 	kfree(cardp->ep_out_buf);
 	cardp->ep_out_buf = NULL;
-
-	lbs_deb_leave(LBS_DEB_USB);
 }
 
 static void if_usb_setup_firmware(struct lbs_private *priv)
@@ -306,8 +302,6 @@ static void if_usb_disconnect(struct usb_interface *intf)
 	struct if_usb_card *cardp = usb_get_intfdata(intf);
 	struct lbs_private *priv = cardp->priv;
 
-	lbs_deb_enter(LBS_DEB_MAIN);
-
 	cardp->surprise_removed = 1;
 
 	if (priv) {
@@ -320,8 +314,6 @@ static void if_usb_disconnect(struct usb_interface *intf)
 
 	usb_set_intfdata(intf, NULL);
 	usb_put_dev(interface_to_usbdev(intf));
-
-	lbs_deb_leave(LBS_DEB_MAIN);
 }
 
 /**
@@ -388,8 +380,6 @@ static int if_usb_reset_device(struct if_usb_card *cardp)
 	struct cmd_header *cmd = cardp->ep_out_buf + 4;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_USB);
-
 	*(__le32 *)cardp->ep_out_buf = cpu_to_le32(CMD_TYPE_REQUEST);
 
 	cmd->command = cpu_to_le16(CMD_802_11_RESET);
@@ -407,8 +397,6 @@ static int if_usb_reset_device(struct if_usb_card *cardp)
 		if_usb_reset_olpc_card(NULL);
 #endif
 
-	lbs_deb_leave_args(LBS_DEB_USB, "ret %d", ret);
-
 	return ret;
 }
 
@@ -671,8 +659,6 @@ static void if_usb_receive(struct urb *urb)
 	__le32 *pkt = (__le32 *)(skb->data + IPFIELD_ALIGN_OFFSET);
 	uint32_t event;
 
-	lbs_deb_enter(LBS_DEB_USB);
-
 	if (recvlength) {
 		if (urb->status) {
 			lbs_deb_usbd(&cardp->udev->dev, "RX URB failed: %d\n",
@@ -688,7 +674,7 @@ static void if_usb_receive(struct urb *urb)
 			    recvlength, recvtype);
 	} else if (urb->status) {
 		kfree_skb(skb);
-		goto rx_exit;
+		return;
 	}
 
 	switch (recvtype) {
@@ -724,8 +710,6 @@ static void if_usb_receive(struct urb *urb)
 
 setup_for_next:
 	if_usb_submit_rx_urb(cardp);
-rx_exit:
-	lbs_deb_leave(LBS_DEB_USB);
 }
 
 /**
@@ -835,8 +819,6 @@ static void if_usb_prog_firmware(struct lbs_private *priv, int ret,
 	int i = 0;
 	static int reset_count = 10;
 
-	lbs_deb_enter(LBS_DEB_USB);
-
 	if (ret) {
 		pr_err("failed to find firmware (%d)\n", ret);
 		goto done;
@@ -942,7 +924,6 @@ restart:
 
  done:
 	cardp->fw = NULL;
-	lbs_deb_leave(LBS_DEB_USB);
 }
 
 
@@ -953,8 +934,6 @@ static int if_usb_suspend(struct usb_interface *intf, pm_message_t message)
 	struct lbs_private *priv = cardp->priv;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_USB);
-
 	if (priv->psstate != PS_STATE_FULL_POWER) {
 		ret = -1;
 		goto out;
@@ -978,7 +957,6 @@ static int if_usb_suspend(struct usb_interface *intf, pm_message_t message)
 	usb_kill_urb(cardp->rx_urb);
 
  out:
-	lbs_deb_leave(LBS_DEB_USB);
 	return ret;
 }
 
@@ -987,13 +965,10 @@ static int if_usb_resume(struct usb_interface *intf)
 	struct if_usb_card *cardp = usb_get_intfdata(intf);
 	struct lbs_private *priv = cardp->priv;
 
-	lbs_deb_enter(LBS_DEB_USB);
-
 	if_usb_submit_rx_urb(cardp);
 
 	lbs_resume(priv);
 
-	lbs_deb_leave(LBS_DEB_USB);
 	return 0;
 }
 #else
diff --git a/drivers/net/wireless/marvell/libertas/main.c b/drivers/net/wireless/marvell/libertas/main.c
index e3500203715c..aefa88f4f29c 100644
--- a/drivers/net/wireless/marvell/libertas/main.c
+++ b/drivers/net/wireless/marvell/libertas/main.c
@@ -180,7 +180,6 @@ static int lbs_dev_open(struct net_device *dev)
 	struct lbs_private *priv = dev->ml_priv;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_NET);
 	if (!priv->iface_running) {
 		ret = lbs_start_iface(priv);
 		if (ret)
@@ -197,7 +196,6 @@ static int lbs_dev_open(struct net_device *dev)
 	spin_unlock_irq(&priv->driver_lock);
 
 out:
-	lbs_deb_leave_args(LBS_DEB_NET, "ret %d", ret);
 	return ret;
 }
 
@@ -216,8 +214,6 @@ int lbs_stop_iface(struct lbs_private *priv)
 	unsigned long flags;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_MAIN);
-
 	spin_lock_irqsave(&priv->driver_lock, flags);
 	priv->iface_running = false;
 	kfree_skb(priv->currenttxskb);
@@ -236,7 +232,6 @@ int lbs_stop_iface(struct lbs_private *priv)
 	if (priv->power_save)
 		ret = priv->power_save(priv);
 
-	lbs_deb_leave(LBS_DEB_MAIN);
 	return ret;
 }
 
@@ -250,8 +245,6 @@ static int lbs_eth_stop(struct net_device *dev)
 {
 	struct lbs_private *priv = dev->ml_priv;
 
-	lbs_deb_enter(LBS_DEB_NET);
-
 	if (priv->connect_status == LBS_CONNECTED)
 		lbs_disconnect(priv, WLAN_REASON_DEAUTH_LEAVING);
 
@@ -269,7 +262,6 @@ static int lbs_eth_stop(struct net_device *dev)
 	if (!lbs_iface_active(priv))
 		lbs_stop_iface(priv);
 
-	lbs_deb_leave(LBS_DEB_NET);
 	return 0;
 }
 
@@ -277,8 +269,6 @@ void lbs_host_to_card_done(struct lbs_private *priv)
 {
 	unsigned long flags;
 
-	lbs_deb_enter(LBS_DEB_THREAD);
-
 	spin_lock_irqsave(&priv->driver_lock, flags);
 	del_timer(&priv->tx_lockup_timer);
 
@@ -291,7 +281,6 @@ void lbs_host_to_card_done(struct lbs_private *priv)
 	}
 
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
-	lbs_deb_leave(LBS_DEB_THREAD);
 }
 EXPORT_SYMBOL_GPL(lbs_host_to_card_done);
 
@@ -301,8 +290,6 @@ int lbs_set_mac_address(struct net_device *dev, void *addr)
 	struct lbs_private *priv = dev->ml_priv;
 	struct sockaddr *phwaddr = addr;
 
-	lbs_deb_enter(LBS_DEB_NET);
-
 	/*
 	 * Can only set MAC address when all interfaces are down, to be written
 	 * to the hardware when one of them is brought up.
@@ -318,7 +305,6 @@ int lbs_set_mac_address(struct net_device *dev, void *addr)
 	if (priv->mesh_dev)
 		memcpy(priv->mesh_dev->dev_addr, phwaddr->sa_data, ETH_ALEN);
 
-	lbs_deb_leave_args(LBS_DEB_NET, "ret %d", ret);
 	return ret;
 }
 
@@ -378,8 +364,6 @@ void lbs_update_mcast(struct lbs_private *priv)
 	int nr_addrs;
 	int old_mac_control = priv->mac_control;
 
-	lbs_deb_enter(LBS_DEB_NET);
-
 	if (netif_running(priv->dev))
 		dev_flags |= priv->dev->flags;
 	if (priv->mesh_dev && netif_running(priv->mesh_dev))
@@ -424,8 +408,6 @@ void lbs_update_mcast(struct lbs_private *priv)
  out_set_mac_control:
 	if (priv->mac_control != old_mac_control)
 		lbs_set_mac_control(priv);
-
-	lbs_deb_leave(LBS_DEB_NET);
 }
 
 static void lbs_set_mcast_worker(struct work_struct *work)
@@ -453,9 +435,7 @@ static int lbs_thread(void *data)
 {
 	struct net_device *dev = data;
 	struct lbs_private *priv = dev->ml_priv;
-	wait_queue_t wait;
-
-	lbs_deb_enter(LBS_DEB_THREAD);
+	wait_queue_entry_t wait;
 
 	init_waitqueue_entry(&wait, current);
 
@@ -648,7 +628,6 @@ static int lbs_thread(void *data)
 	del_timer(&priv->tx_lockup_timer);
 	del_timer(&priv->auto_deepsleep_timer);
 
-	lbs_deb_leave(LBS_DEB_THREAD);
 	return 0;
 }
 
@@ -664,8 +643,6 @@ static int lbs_setup_firmware(struct lbs_private *priv)
 	int ret = -1;
 	s16 curlevel = 0, minlevel = 0, maxlevel = 0;
 
-	lbs_deb_enter(LBS_DEB_FW);
-
 	/* Read MAC address from firmware */
 	eth_broadcast_addr(priv->current_addr);
 	ret = lbs_update_hw_spec(priv);
@@ -687,7 +664,6 @@ static int lbs_setup_firmware(struct lbs_private *priv)
 
 	ret = lbs_set_mac_control_sync(priv);
 done:
-	lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
 	return ret;
 }
 
@@ -695,8 +671,6 @@ int lbs_suspend(struct lbs_private *priv)
 {
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_FW);
-
 	if (priv->is_deep_sleep) {
 		ret = lbs_set_deep_sleep(priv, 0);
 		if (ret) {
@@ -713,7 +687,6 @@ int lbs_suspend(struct lbs_private *priv)
 	if (priv->mesh_dev)
 		netif_device_detach(priv->mesh_dev);
 
-	lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(lbs_suspend);
@@ -722,8 +695,6 @@ int lbs_resume(struct lbs_private *priv)
 {
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_FW);
-
 	ret = lbs_set_host_sleep(priv, 0);
 
 	netif_device_attach(priv->dev);
@@ -741,7 +712,6 @@ int lbs_resume(struct lbs_private *priv)
 	if (priv->setup_fw_on_resume)
 		ret = lbs_setup_firmware(priv);
 
-	lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(lbs_resume);
@@ -757,7 +727,6 @@ static void lbs_cmd_timeout_handler(unsigned long data)
 	struct lbs_private *priv = (struct lbs_private *)data;
 	unsigned long flags;
 
-	lbs_deb_enter(LBS_DEB_CMD);
 	spin_lock_irqsave(&priv->driver_lock, flags);
 
 	if (!priv->cur_cmd)
@@ -778,7 +747,6 @@ static void lbs_cmd_timeout_handler(unsigned long data)
 	wake_up(&priv->waitq);
 out:
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
-	lbs_deb_leave(LBS_DEB_CMD);
 }
 
 /**
@@ -793,7 +761,6 @@ static void lbs_tx_lockup_handler(unsigned long data)
 	struct lbs_private *priv = (struct lbs_private *)data;
 	unsigned long flags;
 
-	lbs_deb_enter(LBS_DEB_TX);
 	spin_lock_irqsave(&priv->driver_lock, flags);
 
 	netdev_info(priv->dev, "TX lockup detected\n");
@@ -804,7 +771,6 @@ static void lbs_tx_lockup_handler(unsigned long data)
 	wake_up_interruptible(&priv->waitq);
 
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
-	lbs_deb_leave(LBS_DEB_TX);
 }
 
 /**
@@ -817,8 +783,6 @@ static void auto_deepsleep_timer_fn(unsigned long data)
 {
 	struct lbs_private *priv = (struct lbs_private *)data;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	if (priv->is_activity_detected) {
 		priv->is_activity_detected = 0;
 	} else {
@@ -836,32 +800,25 @@ static void auto_deepsleep_timer_fn(unsigned long data)
 	}
 	mod_timer(&priv->auto_deepsleep_timer , jiffies +
 				(priv->auto_deep_sleep_timeout * HZ)/1000);
-	lbs_deb_leave(LBS_DEB_CMD);
 }
 
 int lbs_enter_auto_deep_sleep(struct lbs_private *priv)
 {
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	priv->is_auto_deep_sleep_enabled = 1;
 	if (priv->is_deep_sleep)
 		priv->wakeup_dev_required = 1;
 	mod_timer(&priv->auto_deepsleep_timer ,
 			jiffies + (priv->auto_deep_sleep_timeout * HZ)/1000);
 
-	lbs_deb_leave(LBS_DEB_SDIO);
 	return 0;
 }
 
 int lbs_exit_auto_deep_sleep(struct lbs_private *priv)
 {
-	lbs_deb_enter(LBS_DEB_SDIO);
-
 	priv->is_auto_deep_sleep_enabled = 0;
 	priv->auto_deep_sleep_timeout = 0;
 	del_timer(&priv->auto_deepsleep_timer);
 
-	lbs_deb_leave(LBS_DEB_SDIO);
 	return 0;
 }
 
@@ -869,8 +826,6 @@ static int lbs_init_adapter(struct lbs_private *priv)
 {
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_MAIN);
-
 	eth_broadcast_addr(priv->current_addr);
 
 	priv->connect_status = LBS_DISCONNECTED;
@@ -921,22 +876,16 @@ static int lbs_init_adapter(struct lbs_private *priv)
 	}
 
 out:
-	lbs_deb_leave_args(LBS_DEB_MAIN, "ret %d", ret);
-
 	return ret;
 }
 
 static void lbs_free_adapter(struct lbs_private *priv)
 {
-	lbs_deb_enter(LBS_DEB_MAIN);
-
 	lbs_free_cmd_buffer(priv);
 	kfifo_free(&priv->event_fifo);
 	del_timer(&priv->command_timer);
 	del_timer(&priv->tx_lockup_timer);
 	del_timer(&priv->auto_deepsleep_timer);
-
-	lbs_deb_leave(LBS_DEB_MAIN);
 }
 
 static const struct net_device_ops lbs_netdev_ops = {
@@ -962,8 +911,6 @@ struct lbs_private *lbs_add_card(void *card, struct device *dmdev)
 	struct wireless_dev *wdev;
 	struct lbs_private *priv = NULL;
 
-	lbs_deb_enter(LBS_DEB_MAIN);
-
 	/* Allocate an Ethernet device and register it */
 	wdev = lbs_cfg_alloc(dmdev);
 	if (IS_ERR(wdev)) {
@@ -1031,7 +978,6 @@ struct lbs_private *lbs_add_card(void *card, struct device *dmdev)
 	priv = NULL;
 
 done:
-	lbs_deb_leave_args(LBS_DEB_MAIN, "priv %p", priv);
 	return priv;
 }
 EXPORT_SYMBOL_GPL(lbs_add_card);
@@ -1041,8 +987,6 @@ void lbs_remove_card(struct lbs_private *priv)
 {
 	struct net_device *dev = priv->dev;
 
-	lbs_deb_enter(LBS_DEB_MAIN);
-
 	lbs_remove_mesh(priv);
 
 	if (priv->wiphy_registered)
@@ -1083,8 +1027,6 @@ void lbs_remove_card(struct lbs_private *priv)
 	lbs_free_adapter(priv);
 	lbs_cfg_free(priv);
 	free_netdev(dev);
-
-	lbs_deb_leave(LBS_DEB_MAIN);
 }
 EXPORT_SYMBOL_GPL(lbs_remove_card);
 
@@ -1105,8 +1047,6 @@ int lbs_start_card(struct lbs_private *priv)
 	struct net_device *dev = priv->dev;
 	int ret = -1;
 
-	lbs_deb_enter(LBS_DEB_MAIN);
-
 	/* poke the firmware */
 	ret = lbs_setup_firmware(priv);
 	if (ret)
@@ -1133,7 +1073,6 @@ int lbs_start_card(struct lbs_private *priv)
 	ret = 0;
 
 done:
-	lbs_deb_leave_args(LBS_DEB_MAIN, "ret %d", ret);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(lbs_start_card);
@@ -1143,16 +1082,14 @@ void lbs_stop_card(struct lbs_private *priv)
 {
 	struct net_device *dev;
 
-	lbs_deb_enter(LBS_DEB_MAIN);
-
 	if (!priv)
-		goto out;
+		return;
 	dev = priv->dev;
 
 	/* If the netdev isn't registered, it means that lbs_start_card() was
 	 * never called so we have nothing to do here. */
 	if (dev->reg_state != NETREG_REGISTERED)
-		goto out;
+		return;
 
 	netif_stop_queue(dev);
 	netif_carrier_off(dev);
@@ -1160,9 +1097,6 @@ void lbs_stop_card(struct lbs_private *priv)
 	lbs_debugfs_remove_one(priv);
 	lbs_deinit_mesh(priv);
 	unregister_netdev(dev);
-
-out:
-	lbs_deb_leave(LBS_DEB_MAIN);
 }
 EXPORT_SYMBOL_GPL(lbs_stop_card);
 
@@ -1171,7 +1105,6 @@ void lbs_queue_event(struct lbs_private *priv, u32 event)
 {
 	unsigned long flags;
 
-	lbs_deb_enter(LBS_DEB_THREAD);
 	spin_lock_irqsave(&priv->driver_lock, flags);
 
 	if (priv->psstate == PS_STATE_SLEEP)
@@ -1182,14 +1115,11 @@ void lbs_queue_event(struct lbs_private *priv, u32 event)
 	wake_up(&priv->waitq);
 
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
-	lbs_deb_leave(LBS_DEB_THREAD);
 }
 EXPORT_SYMBOL_GPL(lbs_queue_event);
 
 void lbs_notify_command_response(struct lbs_private *priv, u8 resp_idx)
 {
-	lbs_deb_enter(LBS_DEB_THREAD);
-
 	if (priv->psstate == PS_STATE_SLEEP)
 		priv->psstate = PS_STATE_AWAKE;
 
@@ -1198,28 +1128,23 @@ void lbs_notify_command_response(struct lbs_private *priv, u8 resp_idx)
 	priv->resp_idx = resp_idx;
 
 	wake_up(&priv->waitq);
-
-	lbs_deb_leave(LBS_DEB_THREAD);
 }
 EXPORT_SYMBOL_GPL(lbs_notify_command_response);
 
 static int __init lbs_init_module(void)
 {
-	lbs_deb_enter(LBS_DEB_MAIN);
 	memset(&confirm_sleep, 0, sizeof(confirm_sleep));
 	confirm_sleep.hdr.command = cpu_to_le16(CMD_802_11_PS_MODE);
 	confirm_sleep.hdr.size = cpu_to_le16(sizeof(confirm_sleep));
 	confirm_sleep.action = cpu_to_le16(PS_MODE_ACTION_SLEEP_CONFIRMED);
 	lbs_debugfs_init();
-	lbs_deb_leave(LBS_DEB_MAIN);
+
 	return 0;
 }
 
 static void __exit lbs_exit_module(void)
 {
-	lbs_deb_enter(LBS_DEB_MAIN);
 	lbs_debugfs_remove();
-	lbs_deb_leave(LBS_DEB_MAIN);
 }
 
 module_init(lbs_init_module);
diff --git a/drivers/net/wireless/marvell/libertas/mesh.c b/drivers/net/wireless/marvell/libertas/mesh.c
index d0c881dd5846..37ace5cb309d 100644
--- a/drivers/net/wireless/marvell/libertas/mesh.c
+++ b/drivers/net/wireless/marvell/libertas/mesh.c
@@ -26,8 +26,6 @@ static int lbs_mesh_access(struct lbs_private *priv, uint16_t cmd_action,
 {
 	int ret;
 
-	lbs_deb_enter_args(LBS_DEB_CMD, "action %d", cmd_action);
-
 	cmd->hdr.command = cpu_to_le16(CMD_MESH_ACCESS);
 	cmd->hdr.size = cpu_to_le16(sizeof(*cmd));
 	cmd->hdr.result = 0;
@@ -36,7 +34,6 @@ static int lbs_mesh_access(struct lbs_private *priv, uint16_t cmd_action,
 
 	ret = lbs_cmd_with_response(priv, CMD_MESH_ACCESS, cmd);
 
-	lbs_deb_leave(LBS_DEB_CMD);
 	return ret;
 }
 
@@ -47,8 +44,6 @@ static int __lbs_mesh_config_send(struct lbs_private *priv,
 	int ret;
 	u16 command = CMD_MESH_CONFIG_OLD;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
 	/*
 	 * Command id is 0xac for v10 FW along with mesh interface
 	 * id in bits 14-13-12.
@@ -66,7 +61,6 @@ static int __lbs_mesh_config_send(struct lbs_private *priv,
 
 	ret = lbs_cmd_with_response(priv, command, cmd);
 
-	lbs_deb_leave(LBS_DEB_CMD);
 	return ret;
 }
 
@@ -239,8 +233,9 @@ static ssize_t lbs_prb_rsp_limit_set(struct device *dev,
 	memset(&mesh_access, 0, sizeof(mesh_access));
 	mesh_access.data[0] = cpu_to_le32(CMD_ACT_SET);
 
-	if (!kstrtoul(buf, 10, &retry_limit))
-		return -ENOTSUPP;
+	ret = kstrtoul(buf, 10, &retry_limit);
+	if (ret)
+		return ret;
 	if (retry_limit > 15)
 		return -ENOTSUPP;
 
@@ -823,8 +818,6 @@ int lbs_init_mesh(struct lbs_private *priv)
 {
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_MESH);
-
 	/* Determine mesh_fw_ver from fwrelease and fwcapinfo */
 	/* 5.0.16p0 9.0.0.p0 is known to NOT support any mesh */
 	/* 5.110.22 have mesh command with 0xa3 command id */
@@ -870,7 +863,6 @@ int lbs_init_mesh(struct lbs_private *priv)
 		ret = 1;
 	}
 
-	lbs_deb_leave_args(LBS_DEB_MESH, "ret %d", ret);
 	return ret;
 }
 
@@ -887,14 +879,11 @@ int lbs_deinit_mesh(struct lbs_private *priv)
 	struct net_device *dev = priv->dev;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_MESH);
-
 	if (priv->mesh_tlv) {
 		device_remove_file(&dev->dev, &dev_attr_lbs_mesh);
 		ret = 1;
 	}
 
-	lbs_deb_leave_args(LBS_DEB_MESH, "ret %d", ret);
 	return ret;
 }
 
@@ -909,7 +898,6 @@ static int lbs_mesh_stop(struct net_device *dev)
 {
 	struct lbs_private *priv = dev->ml_priv;
 
-	lbs_deb_enter(LBS_DEB_MESH);
 	lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_STOP,
 		lbs_mesh_get_channel(priv));
 
@@ -924,7 +912,6 @@ static int lbs_mesh_stop(struct net_device *dev)
 	if (!lbs_iface_active(priv))
 		lbs_stop_iface(priv);
 
-	lbs_deb_leave(LBS_DEB_MESH);
 	return 0;
 }
 
@@ -939,7 +926,6 @@ static int lbs_mesh_dev_open(struct net_device *dev)
 	struct lbs_private *priv = dev->ml_priv;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_NET);
 	if (!priv->iface_running) {
 		ret = lbs_start_iface(priv);
 		if (ret)
@@ -965,7 +951,6 @@ static int lbs_mesh_dev_open(struct net_device *dev)
 		lbs_mesh_get_channel(priv));
 
 out:
-	lbs_deb_leave_args(LBS_DEB_NET, "ret %d", ret);
 	return ret;
 }
 
@@ -989,8 +974,6 @@ static int lbs_add_mesh(struct lbs_private *priv)
 	struct wireless_dev *mesh_wdev;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_MESH);
-
 	/* Allocate a virtual mesh device */
 	mesh_wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
 	if (!mesh_wdev) {
@@ -1048,7 +1031,6 @@ err_free_wdev:
 	kfree(mesh_wdev);
 
 done:
-	lbs_deb_leave_args(LBS_DEB_MESH, "ret %d", ret);
 	return ret;
 }
 
@@ -1060,7 +1042,6 @@ void lbs_remove_mesh(struct lbs_private *priv)
 	if (!mesh_dev)
 		return;
 
-	lbs_deb_enter(LBS_DEB_MESH);
 	netif_stop_queue(mesh_dev);
 	netif_carrier_off(mesh_dev);
 	sysfs_remove_group(&(mesh_dev->dev.kobj), &lbs_mesh_attr_group);
@@ -1069,7 +1050,6 @@ void lbs_remove_mesh(struct lbs_private *priv)
 	priv->mesh_dev = NULL;
 	kfree(mesh_dev->ieee80211_ptr);
 	free_netdev(mesh_dev);
-	lbs_deb_leave(LBS_DEB_MESH);
 }
 
 
@@ -1108,15 +1088,15 @@ void lbs_mesh_set_txpd(struct lbs_private *priv,
  * Ethtool related
  */
 
-static const char * const mesh_stat_strings[] = {
-			"drop_duplicate_bcast",
-			"drop_ttl_zero",
-			"drop_no_fwd_route",
-			"drop_no_buffers",
-			"fwded_unicast_cnt",
-			"fwded_bcast_cnt",
-			"drop_blind_table",
-			"tx_failed_cnt"
+static const char mesh_stat_strings[MESH_STATS_NUM][ETH_GSTRING_LEN] = {
+	"drop_duplicate_bcast",
+	"drop_ttl_zero",
+	"drop_no_fwd_route",
+	"drop_no_buffers",
+	"fwded_unicast_cnt",
+	"fwded_bcast_cnt",
+	"drop_blind_table",
+	"tx_failed_cnt"
 };
 
 void lbs_mesh_ethtool_get_stats(struct net_device *dev,
@@ -1126,8 +1106,6 @@ void lbs_mesh_ethtool_get_stats(struct net_device *dev,
 	struct cmd_ds_mesh_access mesh_access;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_ETHTOOL);
-
 	/* Get Mesh Statistics */
 	ret = lbs_mesh_access(priv, CMD_ACT_MESH_GET_STATS, &mesh_access);
 
@@ -1153,8 +1131,6 @@ void lbs_mesh_ethtool_get_stats(struct net_device *dev,
 	data[5] = priv->mstats.fwd_bcast_cnt;
 	data[6] = priv->mstats.drop_blind;
 	data[7] = priv->mstats.tx_failed_cnt;
-
-	lbs_deb_enter(LBS_DEB_ETHTOOL);
 }
 
 int lbs_mesh_ethtool_get_sset_count(struct net_device *dev, int sset)
@@ -1170,18 +1146,9 @@ int lbs_mesh_ethtool_get_sset_count(struct net_device *dev, int sset)
 void lbs_mesh_ethtool_get_strings(struct net_device *dev,
 	uint32_t stringset, uint8_t *s)
 {
-	int i;
-
-	lbs_deb_enter(LBS_DEB_ETHTOOL);
-
 	switch (stringset) {
 	case ETH_SS_STATS:
-		for (i = 0; i < MESH_STATS_NUM; i++) {
-			memcpy(s + i * ETH_GSTRING_LEN,
-					mesh_stat_strings[i],
-					ETH_GSTRING_LEN);
-		}
+		memcpy(s, mesh_stat_strings, sizeof(mesh_stat_strings));
 		break;
 	}
-	lbs_deb_enter(LBS_DEB_ETHTOOL);
 }
diff --git a/drivers/net/wireless/marvell/libertas/rx.c b/drivers/net/wireless/marvell/libertas/rx.c
index e446fed7b345..7586ff681b23 100644
--- a/drivers/net/wireless/marvell/libertas/rx.c
+++ b/drivers/net/wireless/marvell/libertas/rx.c
@@ -65,8 +65,6 @@ int lbs_process_rxed_packet(struct lbs_private *priv, struct sk_buff *skb)
 		0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00
 	};
 
-	lbs_deb_enter(LBS_DEB_RX);
-
 	BUG_ON(!skb);
 
 	skb->ip_summed = CHECKSUM_NONE;
@@ -158,7 +156,6 @@ int lbs_process_rxed_packet(struct lbs_private *priv, struct sk_buff *skb)
 
 	ret = 0;
 done:
-	lbs_deb_leave_args(LBS_DEB_RX, "ret %d", ret);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(lbs_process_rxed_packet);
@@ -221,8 +218,6 @@ static int process_rxed_802_11_packet(struct lbs_private *priv,
 	struct rx_radiotap_hdr radiotap_hdr;
 	struct rx_radiotap_hdr *pradiotap_hdr;
 
-	lbs_deb_enter(LBS_DEB_RX);
-
 	p_rx_pkt = (struct rx80211packethdr *) skb->data;
 	prxpd = &p_rx_pkt->rx_pd;
 
@@ -262,7 +257,7 @@ static int process_rxed_802_11_packet(struct lbs_private *priv,
 		goto done;
 	}
 
-	pradiotap_hdr = (void *)skb_push(skb, sizeof(struct rx_radiotap_hdr));
+	pradiotap_hdr = skb_push(skb, sizeof(struct rx_radiotap_hdr));
 	memcpy(pradiotap_hdr, &radiotap_hdr, sizeof(struct rx_radiotap_hdr));
 
 	priv->cur_rate = lbs_fw_index_to_data_rate(prxpd->rx_rate);
@@ -281,6 +276,5 @@ static int process_rxed_802_11_packet(struct lbs_private *priv,
 	ret = 0;
 
 done:
-	lbs_deb_leave_args(LBS_DEB_RX, "ret %d", ret);
 	return ret;
 }
diff --git a/drivers/net/wireless/marvell/libertas/tx.c b/drivers/net/wireless/marvell/libertas/tx.c
index c025f9c18282..723ba5fd0bfe 100644
--- a/drivers/net/wireless/marvell/libertas/tx.c
+++ b/drivers/net/wireless/marvell/libertas/tx.c
@@ -70,8 +70,6 @@ netdev_tx_t lbs_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	uint16_t pkt_len;
 	netdev_tx_t ret = NETDEV_TX_OK;
 
-	lbs_deb_enter(LBS_DEB_TX);
-
 	/* We need to protect against the queues being restarted before
 	   we get round to stopping them */
 	spin_lock_irqsave(&priv->driver_lock, flags);
@@ -166,7 +164,6 @@ netdev_tx_t lbs_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
 	wake_up(&priv->waitq);
 
-	lbs_deb_leave_args(LBS_DEB_TX, "ret %d", ret);
 	return ret;
 }
 
diff --git a/drivers/net/wireless/marvell/libertas_tf/main.c b/drivers/net/wireless/marvell/libertas_tf/main.c
index d80333117989..81228bf73043 100644
--- a/drivers/net/wireless/marvell/libertas_tf/main.c
+++ b/drivers/net/wireless/marvell/libertas_tf/main.c
@@ -260,7 +260,7 @@ static void lbtf_tx_work(struct work_struct *work)
 
 	len = skb->len;
 	info  = IEEE80211_SKB_CB(skb);
-	txpd = (struct txpd *)  skb_push(skb, sizeof(struct txpd));
+	txpd = skb_push(skb, sizeof(struct txpd));
 
 	if (priv->surpriseremoved) {
 		dev_kfree_skb_any(skb);
diff --git a/drivers/net/wireless/marvell/mwifiex/11h.c b/drivers/net/wireless/marvell/mwifiex/11h.c
index 366eb4991a7d..238accfe4f41 100644
--- a/drivers/net/wireless/marvell/mwifiex/11h.c
+++ b/drivers/net/wireless/marvell/mwifiex/11h.c
@@ -128,9 +128,6 @@ void mwifiex_dfs_cac_work_queue(struct work_struct *work)
 			container_of(delayed_work, struct mwifiex_private,
 				     dfs_cac_work);
 
-	if (WARN_ON(!priv))
-		return;
-
 	chandef = priv->dfs_chandef;
 	if (priv->wdev.cac_started) {
 		mwifiex_dbg(priv->adapter, MSG,
@@ -289,9 +286,6 @@ void mwifiex_dfs_chan_sw_work_queue(struct work_struct *work)
 			container_of(delayed_work, struct mwifiex_private,
 				     dfs_chan_sw_work);
 
-	if (WARN_ON(!priv))
-		return;
-
 	bss_cfg = &priv->bss_cfg;
 	if (!bss_cfg->beacon_period) {
 		mwifiex_dbg(priv->adapter, ERROR,
diff --git a/drivers/net/wireless/marvell/mwifiex/11n.c b/drivers/net/wireless/marvell/mwifiex/11n.c
index c174e79e6df2..16c77c27f1b6 100644
--- a/drivers/net/wireless/marvell/mwifiex/11n.c
+++ b/drivers/net/wireless/marvell/mwifiex/11n.c
@@ -653,11 +653,13 @@ int mwifiex_send_delba(struct mwifiex_private *priv, int tid, u8 *peer_mac,
 void mwifiex_11n_delba(struct mwifiex_private *priv, int tid)
 {
 	struct mwifiex_rx_reorder_tbl *rx_reor_tbl_ptr;
+	unsigned long flags;
 
+	spin_lock_irqsave(&priv->rx_reorder_tbl_lock, flags);
 	if (list_empty(&priv->rx_reorder_tbl_ptr)) {
 		dev_dbg(priv->adapter->dev,
 			"mwifiex_11n_delba: rx_reorder_tbl_ptr empty\n");
-		return;
+		goto exit;
 	}
 
 	list_for_each_entry(rx_reor_tbl_ptr, &priv->rx_reorder_tbl_ptr, list) {
@@ -666,9 +668,11 @@ void mwifiex_11n_delba(struct mwifiex_private *priv, int tid)
 				"Send delba to tid=%d, %pM\n",
 				tid, rx_reor_tbl_ptr->ta);
 			mwifiex_send_delba(priv, tid, rx_reor_tbl_ptr->ta, 0);
-			return;
+			goto exit;
 		}
 	}
+exit:
+	spin_unlock_irqrestore(&priv->rx_reorder_tbl_lock, flags);
 }
 
 /*
@@ -764,14 +768,9 @@ void mwifiex_del_tx_ba_stream_tbl_by_ra(struct mwifiex_private *priv, u8 *ra)
 		return;
 
 	spin_lock_irqsave(&priv->tx_ba_stream_tbl_lock, flags);
-	list_for_each_entry_safe(tbl, tmp, &priv->tx_ba_stream_tbl_ptr, list) {
-		if (!memcmp(tbl->ra, ra, ETH_ALEN)) {
-			spin_unlock_irqrestore(&priv->tx_ba_stream_tbl_lock,
-					       flags);
+	list_for_each_entry_safe(tbl, tmp, &priv->tx_ba_stream_tbl_ptr, list)
+		if (!memcmp(tbl->ra, ra, ETH_ALEN))
 			mwifiex_11n_delete_tx_ba_stream_tbl_entry(priv, tbl);
-			spin_lock_irqsave(&priv->tx_ba_stream_tbl_lock, flags);
-		}
-	}
 	spin_unlock_irqrestore(&priv->tx_ba_stream_tbl_lock, flags);
 
 	return;
diff --git a/drivers/net/wireless/marvell/mwifiex/11n_aggr.c b/drivers/net/wireless/marvell/mwifiex/11n_aggr.c
index a75013ac84d7..042a1d07f686 100644
--- a/drivers/net/wireless/marvell/mwifiex/11n_aggr.c
+++ b/drivers/net/wireless/marvell/mwifiex/11n_aggr.c
@@ -62,7 +62,7 @@ mwifiex_11n_form_amsdu_pkt(struct sk_buff *skb_aggr,
 	};
 	struct tx_packet_hdr *tx_header;
 
-	tx_header = (void *)skb_put(skb_aggr, sizeof(*tx_header));
+	tx_header = skb_put(skb_aggr, sizeof(*tx_header));
 
 	/* Copy DA and SA */
 	dt_offset = 2 * ETH_ALEN;
@@ -81,7 +81,7 @@ mwifiex_11n_form_amsdu_pkt(struct sk_buff *skb_aggr,
 	tx_header->eth803_hdr.h_proto = htons(skb_src->len + LLC_SNAP_LEN);
 
 	/* Add payload */
-	memcpy(skb_put(skb_aggr, skb_src->len), skb_src->data, skb_src->len);
+	skb_put_data(skb_aggr, skb_src->data, skb_src->len);
 
 	/* Add padding for new MSDU to start from 4 byte boundary */
 	*pad = (4 - ((unsigned long)skb_aggr->tail & 0x3)) % 4;
@@ -164,7 +164,7 @@ mwifiex_11n_aggregate_pkt(struct mwifiex_private *priv,
 	int pad = 0, aggr_num = 0, ret;
 	struct mwifiex_tx_param tx_param;
 	struct txpd *ptx_pd = NULL;
-	int headroom = adapter->iface_type == MWIFIEX_USB ? 0 : INTF_HEADER_LEN;
+	int headroom = adapter->intf_hdr_len;
 
 	skb_src = skb_peek(&pra_list->skb_head);
 	if (!skb_src) {
@@ -250,15 +250,15 @@ mwifiex_11n_aggregate_pkt(struct mwifiex_private *priv,
 		return 0;
 	}
 
+	if (skb_src)
+		tx_param.next_pkt_len = skb_src->len + sizeof(struct txpd);
+	else
+		tx_param.next_pkt_len = 0;
+
 	if (adapter->iface_type == MWIFIEX_USB) {
 		ret = adapter->if_ops.host_to_card(adapter, priv->usb_port,
-						   skb_aggr, NULL);
+						   skb_aggr, &tx_param);
 	} else {
-		if (skb_src)
-			tx_param.next_pkt_len =
-					skb_src->len + sizeof(struct txpd);
-		else
-			tx_param.next_pkt_len = 0;
 
 		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_DATA,
 						   skb_aggr, &tx_param);
diff --git a/drivers/net/wireless/marvell/mwifiex/cfg80211.c b/drivers/net/wireless/marvell/mwifiex/cfg80211.c
index 7ec06bf13413..06ad2d50f9b0 100644
--- a/drivers/net/wireless/marvell/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/marvell/mwifiex/cfg80211.c
@@ -176,12 +176,10 @@ mwifiex_form_mgmt_frame(struct sk_buff *skb, const u8 *buf, size_t len)
 	memcpy(skb_push(skb, sizeof(pkt_type)), &pkt_type, sizeof(pkt_type));
 
 	/* Add packet data and address4 */
-	memcpy(skb_put(skb, sizeof(struct ieee80211_hdr_3addr)), buf,
-	       sizeof(struct ieee80211_hdr_3addr));
-	memcpy(skb_put(skb, ETH_ALEN), addr, ETH_ALEN);
-	memcpy(skb_put(skb, len - sizeof(struct ieee80211_hdr_3addr)),
-	       buf + sizeof(struct ieee80211_hdr_3addr),
-	       len - sizeof(struct ieee80211_hdr_3addr));
+	skb_put_data(skb, buf, sizeof(struct ieee80211_hdr_3addr));
+	skb_put_data(skb, addr, ETH_ALEN);
+	skb_put_data(skb, buf + sizeof(struct ieee80211_hdr_3addr),
+		     len - sizeof(struct ieee80211_hdr_3addr));
 
 	skb->priority = LOW_PRIO_TID;
 	__net_timestamp(skb);
@@ -894,24 +892,20 @@ mwifiex_init_new_priv_params(struct mwifiex_private *priv,
 		priv->bss_num = mwifiex_get_unused_bss_num(adapter,
 			 MWIFIEX_BSS_TYPE_STA);
 		priv->bss_role =  MWIFIEX_BSS_ROLE_STA;
-		priv->bss_type = MWIFIEX_BSS_TYPE_STA;
 		break;
 	case NL80211_IFTYPE_P2P_CLIENT:
 		priv->bss_num = mwifiex_get_unused_bss_num(adapter,
 			 MWIFIEX_BSS_TYPE_P2P);
 		priv->bss_role =  MWIFIEX_BSS_ROLE_STA;
-		priv->bss_type = MWIFIEX_BSS_TYPE_P2P;
 		break;
 	case NL80211_IFTYPE_P2P_GO:
 		priv->bss_num = mwifiex_get_unused_bss_num(adapter,
 			 MWIFIEX_BSS_TYPE_P2P);
 		priv->bss_role =  MWIFIEX_BSS_ROLE_UAP;
-		priv->bss_type = MWIFIEX_BSS_TYPE_P2P;
 		break;
 	case NL80211_IFTYPE_AP:
 		priv->bss_num = mwifiex_get_unused_bss_num(adapter,
 			 MWIFIEX_BSS_TYPE_UAP);
-		priv->bss_type = MWIFIEX_BSS_TYPE_UAP;
 		priv->bss_role = MWIFIEX_BSS_ROLE_UAP;
 		break;
 	default:
@@ -1983,7 +1977,7 @@ static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
 
 	if (mwifiex_set_secure_params(priv, bss_cfg, params)) {
 		mwifiex_dbg(priv->adapter, ERROR,
-			    "Failed to parse secuirty parameters!\n");
+			    "Failed to parse security parameters!\n");
 		goto out;
 	}
 
@@ -2964,10 +2958,8 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 	if (!dev) {
 		mwifiex_dbg(adapter, ERROR,
 			    "no memory available for netdevice\n");
-		memset(&priv->wdev, 0, sizeof(priv->wdev));
-		priv->wdev.iftype = NL80211_IFTYPE_UNSPECIFIED;
-		priv->bss_mode = NL80211_IFTYPE_UNSPECIFIED;
-		return ERR_PTR(-ENOMEM);
+		ret = -ENOMEM;
+		goto err_alloc_netdev;
 	}
 
 	mwifiex_init_priv_params(priv, dev);
@@ -2976,11 +2968,11 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 	ret = mwifiex_send_cmd(priv, HostCmd_CMD_SET_BSS_MODE,
 			       HostCmd_ACT_GEN_SET, 0, NULL, true);
 	if (ret)
-		return ERR_PTR(ret);
+		goto err_set_bss_mode;
 
 	ret = mwifiex_sta_init_cmd(priv, false, false);
 	if (ret)
-		return ERR_PTR(ret);
+		goto err_sta_init;
 
 	mwifiex_setup_ht_caps(&wiphy->bands[NL80211_BAND_2GHZ]->ht_cap, priv);
 	if (adapter->is_hw_11ac_capable)
@@ -3011,31 +3003,14 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 
 	SET_NETDEV_DEV(dev, adapter->dev);
 
-	/* Register network device */
-	if (register_netdevice(dev)) {
-		mwifiex_dbg(adapter, ERROR,
-			    "cannot register virtual network device\n");
-		free_netdev(dev);
-		priv->bss_mode = NL80211_IFTYPE_UNSPECIFIED;
-		priv->netdev = NULL;
-		memset(&priv->wdev, 0, sizeof(priv->wdev));
-		priv->wdev.iftype = NL80211_IFTYPE_UNSPECIFIED;
-		return ERR_PTR(-EFAULT);
-	}
-
 	priv->dfs_cac_workqueue = alloc_workqueue("MWIFIEX_DFS_CAC%s",
 						  WQ_HIGHPRI |
 						  WQ_MEM_RECLAIM |
 						  WQ_UNBOUND, 1, name);
 	if (!priv->dfs_cac_workqueue) {
-		mwifiex_dbg(adapter, ERROR,
-			    "cannot register virtual network device\n");
-		free_netdev(dev);
-		priv->bss_mode = NL80211_IFTYPE_UNSPECIFIED;
-		priv->netdev = NULL;
-		memset(&priv->wdev, 0, sizeof(priv->wdev));
-		priv->wdev.iftype = NL80211_IFTYPE_UNSPECIFIED;
-		return ERR_PTR(-ENOMEM);
+		mwifiex_dbg(adapter, ERROR, "cannot alloc DFS CAC queue\n");
+		ret = -ENOMEM;
+		goto err_alloc_cac;
 	}
 
 	INIT_DELAYED_WORK(&priv->dfs_cac_work, mwifiex_dfs_cac_work_queue);
@@ -3044,22 +3019,22 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 						      WQ_HIGHPRI | WQ_UNBOUND |
 						      WQ_MEM_RECLAIM, 1, name);
 	if (!priv->dfs_chan_sw_workqueue) {
-		mwifiex_dbg(adapter, ERROR,
-			    "cannot register virtual network device\n");
-		free_netdev(dev);
-		priv->bss_mode = NL80211_IFTYPE_UNSPECIFIED;
-		priv->netdev = NULL;
-		memset(&priv->wdev, 0, sizeof(priv->wdev));
-		priv->wdev.iftype = NL80211_IFTYPE_UNSPECIFIED;
-		destroy_workqueue(priv->dfs_cac_workqueue);
-		priv->dfs_cac_workqueue = NULL;
-		return ERR_PTR(-ENOMEM);
+		mwifiex_dbg(adapter, ERROR, "cannot alloc DFS channel sw queue\n");
+		ret = -ENOMEM;
+		goto err_alloc_chsw;
 	}
 
 	INIT_DELAYED_WORK(&priv->dfs_chan_sw_work,
 			  mwifiex_dfs_chan_sw_work_queue);
 
-	sema_init(&priv->async_sem, 1);
+	mutex_init(&priv->async_mutex);
+
+	/* Register network device */
+	if (register_netdevice(dev)) {
+		mwifiex_dbg(adapter, ERROR, "cannot register network device\n");
+		ret = -EFAULT;
+		goto err_reg_netdev;
+	}
 
 	mwifiex_dbg(adapter, INFO,
 		    "info: %s: Marvell 802.11 Adapter\n", dev->name);
@@ -3081,11 +3056,29 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 		adapter->curr_iface_comb.p2p_intf++;
 		break;
 	default:
+		/* This should be dead code; checked above */
 		mwifiex_dbg(adapter, ERROR, "type not supported\n");
 		return ERR_PTR(-EINVAL);
 	}
 
 	return &priv->wdev;
+
+err_reg_netdev:
+	destroy_workqueue(priv->dfs_chan_sw_workqueue);
+	priv->dfs_chan_sw_workqueue = NULL;
+err_alloc_chsw:
+	destroy_workqueue(priv->dfs_cac_workqueue);
+	priv->dfs_cac_workqueue = NULL;
+err_alloc_cac:
+	free_netdev(dev);
+	priv->netdev = NULL;
+err_sta_init:
+err_set_bss_mode:
+err_alloc_netdev:
+	memset(&priv->wdev, 0, sizeof(priv->wdev));
+	priv->wdev.iftype = NL80211_IFTYPE_UNSPECIFIED;
+	priv->bss_mode = NL80211_IFTYPE_UNSPECIFIED;
+	return ERR_PTR(ret);
 }
 EXPORT_SYMBOL_GPL(mwifiex_add_virtual_intf);
 
diff --git a/drivers/net/wireless/marvell/mwifiex/cfp.c b/drivers/net/wireless/marvell/mwifiex/cfp.c
index 1ff22055e54f..6e2994308526 100644
--- a/drivers/net/wireless/marvell/mwifiex/cfp.c
+++ b/drivers/net/wireless/marvell/mwifiex/cfp.c
@@ -350,7 +350,7 @@ mwifiex_get_cfp(struct mwifiex_private *priv, u8 band, u16 channel, u32 freq)
 		}
 	}
 	if (i == sband->n_channels) {
-		mwifiex_dbg(priv->adapter, ERROR,
+		mwifiex_dbg(priv->adapter, WARN,
 			    "%s: cannot find cfp by band %d\t"
 			    "& channel=%d freq=%d\n",
 			    __func__, band, channel, freq);
diff --git a/drivers/net/wireless/marvell/mwifiex/cmdevt.c b/drivers/net/wireless/marvell/mwifiex/cmdevt.c
index 0c3b217247b1..8dad52886034 100644
--- a/drivers/net/wireless/marvell/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/marvell/mwifiex/cmdevt.c
@@ -258,10 +258,10 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 		if (ret == -EBUSY)
 			cmd_node->cmd_skb = NULL;
 	} else {
-		skb_push(cmd_node->cmd_skb, INTF_HEADER_LEN);
+		skb_push(cmd_node->cmd_skb, adapter->intf_hdr_len);
 		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_CMD,
 						   cmd_node->cmd_skb, NULL);
-		skb_pull(cmd_node->cmd_skb, INTF_HEADER_LEN);
+		skb_pull(cmd_node->cmd_skb, adapter->intf_hdr_len);
 	}
 
 	if (ret == -1) {
@@ -351,10 +351,10 @@ static int mwifiex_dnld_sleep_confirm_cmd(struct mwifiex_adapter *adapter)
 		if (ret != -EBUSY)
 			dev_kfree_skb_any(sleep_cfm_tmp);
 	} else {
-		skb_push(adapter->sleep_cfm, INTF_HEADER_LEN);
+		skb_push(adapter->sleep_cfm, adapter->intf_hdr_len);
 		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_CMD,
 						   adapter->sleep_cfm, NULL);
-		skb_pull(adapter->sleep_cfm, INTF_HEADER_LEN);
+		skb_pull(adapter->sleep_cfm, adapter->intf_hdr_len);
 	}
 
 	if (ret == -1) {
@@ -622,8 +622,7 @@ int mwifiex_send_cmd(struct mwifiex_private *priv, u16 cmd_no,
 		return -1;
 	}
 
-	memset(skb_put(cmd_node->cmd_skb, sizeof(struct host_cmd_ds_command)),
-	       0, sizeof(struct host_cmd_ds_command));
+	skb_put_zero(cmd_node->cmd_skb, sizeof(struct host_cmd_ds_command));
 
 	cmd_ptr = (struct host_cmd_ds_command *) (cmd_node->cmd_skb->data);
 	cmd_ptr->command = cpu_to_le16(cmd_no);
@@ -761,8 +760,6 @@ int mwifiex_exec_next_cmd(struct mwifiex_adapter *adapter)
 	}
 	cmd_node = list_first_entry(&adapter->cmd_pending_q,
 				    struct cmd_ctrl_node, list);
-	spin_unlock_irqrestore(&adapter->cmd_pending_q_lock,
-			       cmd_pending_q_flags);
 
 	host_cmd = (struct host_cmd_ds_command *) (cmd_node->cmd_skb->data);
 	priv = cmd_node->priv;
@@ -771,11 +768,12 @@ int mwifiex_exec_next_cmd(struct mwifiex_adapter *adapter)
 		mwifiex_dbg(adapter, ERROR,
 			    "%s: cannot send cmd in sleep state,\t"
 			    "this should not happen\n", __func__);
+		spin_unlock_irqrestore(&adapter->cmd_pending_q_lock,
+				       cmd_pending_q_flags);
 		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, cmd_flags);
 		return ret;
 	}
 
-	spin_lock_irqsave(&adapter->cmd_pending_q_lock, cmd_pending_q_flags);
 	list_del(&cmd_node->list);
 	spin_unlock_irqrestore(&adapter->cmd_pending_q_lock,
 			       cmd_pending_q_flags);
@@ -1056,12 +1054,10 @@ mwifiex_cancel_all_pending_cmd(struct mwifiex_adapter *adapter)
 	list_for_each_entry_safe(cmd_node, tmp_node,
 				 &adapter->cmd_pending_q, list) {
 		list_del(&cmd_node->list);
-		spin_unlock_irqrestore(&adapter->cmd_pending_q_lock, flags);
 
 		if (cmd_node->wait_q_enabled)
 			adapter->cmd_wait_q.status = -1;
 		mwifiex_recycle_cmd_node(adapter, cmd_node);
-		spin_lock_irqsave(&adapter->cmd_pending_q_lock, flags);
 	}
 	spin_unlock_irqrestore(&adapter->cmd_pending_q_lock, flags);
 	spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, cmd_flags);
diff --git a/drivers/net/wireless/marvell/mwifiex/debugfs.c b/drivers/net/wireless/marvell/mwifiex/debugfs.c
index ae2b69db5994..f6f105a7d3ff 100644
--- a/drivers/net/wireless/marvell/mwifiex/debugfs.c
+++ b/drivers/net/wireless/marvell/mwifiex/debugfs.c
@@ -1046,6 +1046,5 @@ mwifiex_debugfs_init(void)
 void
 mwifiex_debugfs_remove(void)
 {
-	if (mwifiex_dfs_dir)
-		debugfs_remove(mwifiex_dfs_dir);
+	debugfs_remove(mwifiex_dfs_dir);
 }
diff --git a/drivers/net/wireless/marvell/mwifiex/fw.h b/drivers/net/wireless/marvell/mwifiex/fw.h
index 6cf9ab9133ea..9e75522d248a 100644
--- a/drivers/net/wireless/marvell/mwifiex/fw.h
+++ b/drivers/net/wireless/marvell/mwifiex/fw.h
@@ -247,11 +247,6 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 
 #define MWIFIEX_DEF_AMPDU	IEEE80211_HT_AMPDU_PARM_FACTOR
 
-#define GET_RXSTBC(x) (x & IEEE80211_HT_CAP_RX_STBC)
-#define MWIFIEX_RX_STBC1	0x0100
-#define MWIFIEX_RX_STBC12	0x0200
-#define MWIFIEX_RX_STBC123	0x0300
-
 /* dev_cap bitmap
  * BIT
  * 0-16		reserved
@@ -405,6 +400,7 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define HostCmd_CMD_TDLS_OPER                         0x0122
 #define HostCmd_CMD_SDIO_SP_RX_AGGR_CFG               0x0223
 #define HostCmd_CMD_CHAN_REGION_CFG		      0x0242
+#define HostCmd_CMD_PACKET_AGGR_CTRL		      0x0251
 
 #define PROTOCOL_NO_SECURITY        0x01
 #define PROTOCOL_STATIC_WEP         0x02
@@ -2268,6 +2264,14 @@ struct host_cmd_ds_chan_region_cfg {
 	__le16 action;
 } __packed;
 
+struct host_cmd_ds_pkt_aggr_ctrl {
+	__le16 action;
+	__le16 enable;
+	__le16 tx_aggr_max_size;
+	__le16 tx_aggr_max_num;
+	__le16 tx_aggr_align;
+} __packed;
+
 struct host_cmd_ds_command {
 	__le16 command;
 	__le16 size;
@@ -2343,6 +2347,7 @@ struct host_cmd_ds_command {
 		struct host_cmd_ds_wakeup_reason hs_wakeup_reason;
 		struct host_cmd_ds_gtk_rekey_params rekey;
 		struct host_cmd_ds_chan_region_cfg reg_cfg;
+		struct host_cmd_ds_pkt_aggr_ctrl pkt_aggr_ctrl;
 	} params;
 } __packed;
 
diff --git a/drivers/net/wireless/marvell/mwifiex/init.c b/drivers/net/wireless/marvell/mwifiex/init.c
index 756948385b60..3ecb59f7405b 100644
--- a/drivers/net/wireless/marvell/mwifiex/init.c
+++ b/drivers/net/wireless/marvell/mwifiex/init.c
@@ -217,6 +217,11 @@ static void mwifiex_init_adapter(struct mwifiex_adapter *adapter)
 	else
 		adapter->data_sent = false;
 
+	if (adapter->iface_type == MWIFIEX_USB)
+		adapter->intf_hdr_len = 0;
+	else
+		adapter->intf_hdr_len = INTF_HEADER_LEN;
+
 	adapter->cmd_resp_received = false;
 	adapter->event_received = false;
 	adapter->data_received = false;
@@ -409,11 +414,6 @@ static void mwifiex_free_lock_list(struct mwifiex_adapter *adapter)
 static void
 mwifiex_adapter_cleanup(struct mwifiex_adapter *adapter)
 {
-	if (!adapter) {
-		pr_err("%s: adapter is NULL\n", __func__);
-		return;
-	}
-
 	del_timer(&adapter->wakeup_timer);
 	mwifiex_cancel_all_pending_cmd(adapter);
 	wake_up_interruptible(&adapter->cmd_wait_q.wait);
@@ -439,7 +439,6 @@ int mwifiex_init_lock_list(struct mwifiex_adapter *adapter)
 	struct mwifiex_private *priv;
 	s32 i, j;
 
-	spin_lock_init(&adapter->mwifiex_lock);
 	spin_lock_init(&adapter->int_lock);
 	spin_lock_init(&adapter->main_proc_lock);
 	spin_lock_init(&adapter->mwifiex_cmd_lock);
@@ -670,8 +669,7 @@ mwifiex_shutdown_drv(struct mwifiex_adapter *adapter)
 
 			mwifiex_clean_auto_tdls(priv);
 			mwifiex_abort_cac(priv);
-			mwifiex_clean_txrx(priv);
-			mwifiex_delete_bss_prio_tbl(priv);
+			mwifiex_free_priv(priv);
 		}
 	}
 
@@ -694,11 +692,8 @@ mwifiex_shutdown_drv(struct mwifiex_adapter *adapter)
 
 	spin_unlock_irqrestore(&adapter->rx_proc_lock, flags);
 
-	spin_lock(&adapter->mwifiex_lock);
-
 	mwifiex_adapter_cleanup(adapter);
 
-	spin_unlock(&adapter->mwifiex_lock);
 	adapter->hw_status = MWIFIEX_HW_STATUS_NOT_READY;
 }
 
diff --git a/drivers/net/wireless/marvell/mwifiex/main.c b/drivers/net/wireless/marvell/mwifiex/main.c
index dd87b9ff64c3..f2600b827e81 100644
--- a/drivers/net/wireless/marvell/mwifiex/main.c
+++ b/drivers/net/wireless/marvell/mwifiex/main.c
@@ -44,6 +44,10 @@ bool mfg_mode;
 module_param(mfg_mode, bool, 0);
 MODULE_PARM_DESC(mfg_mode, "manufacturing mode enable:1, disable:0");
 
+bool aggr_ctrl;
+module_param(aggr_ctrl, bool, 0000);
+MODULE_PARM_DESC(aggr_ctrl, "usb tx aggreataon enable:1, disable:0");
+
 /*
  * This function registers the device and performs all the necessary
  * initializations.
@@ -1280,7 +1284,7 @@ void mwifiex_init_priv_params(struct mwifiex_private *priv,
 			      struct net_device *dev)
 {
 	dev->netdev_ops = &mwifiex_netdev_ops;
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 	/* Initialize private structure */
 	priv->current_key_index = 0;
 	priv->media_connected = false;
diff --git a/drivers/net/wireless/marvell/mwifiex/main.h b/drivers/net/wireless/marvell/mwifiex/main.h
index bb2a467d8b13..f8cf3079ac7d 100644
--- a/drivers/net/wireless/marvell/mwifiex/main.h
+++ b/drivers/net/wireless/marvell/mwifiex/main.h
@@ -60,6 +60,7 @@
 
 extern const char driver_version[];
 extern bool mfg_mode;
+extern bool aggr_ctrl;
 
 struct mwifiex_adapter;
 struct mwifiex_private;
@@ -628,7 +629,7 @@ struct mwifiex_private {
 	struct dentry *dfs_dev_dir;
 #endif
 	u16 current_key_index;
-	struct semaphore async_sem;
+	struct mutex async_mutex;
 	struct cfg80211_scan_request *scan_request;
 	u8 cfg_bssid[6];
 	struct wps wps;
@@ -798,6 +799,18 @@ struct mwifiex_auto_tdls_peer {
 	u8 do_setup;
 };
 
+#define MWIFIEX_TYPE_AGGR_DATA_V2 11
+#define MWIFIEX_BUS_AGGR_MODE_LEN_V2 (2)
+#define MWIFIEX_BUS_AGGR_MAX_LEN 16000
+#define MWIFIEX_BUS_AGGR_MAX_NUM 10
+struct bus_aggr_params {
+	u16 enable;
+	u16 mode;
+	u16 tx_aggr_max_size;
+	u16 tx_aggr_max_num;
+	u16 tx_aggr_align;
+};
+
 struct mwifiex_if_ops {
 	int (*init_if) (struct mwifiex_adapter *);
 	void (*cleanup_if) (struct mwifiex_adapter *);
@@ -849,6 +862,7 @@ struct mwifiex_adapter {
 	u8 perm_addr[ETH_ALEN];
 	bool surprise_removed;
 	u32 fw_release_number;
+	u8 intf_hdr_len;
 	u16 init_wait_q_woken;
 	wait_queue_head_t init_wait_q;
 	void *card;
@@ -870,8 +884,6 @@ struct mwifiex_adapter {
 	bool rx_locked;
 	bool main_locked;
 	struct mwifiex_bss_prio_tbl bss_prio_tbl[MWIFIEX_MAX_BSS_NUM];
-	/* spin lock for init/shutdown */
-	spinlock_t mwifiex_lock;
 	/* spin lock for main process */
 	spinlock_t main_proc_lock;
 	u32 mwifiex_processing;
@@ -1017,6 +1029,8 @@ struct mwifiex_adapter {
 	/* Wake-on-WLAN (WoWLAN) */
 	int irq_wakeup;
 	bool wake_by_wifi;
+	/* Aggregation parameters*/
+	struct bus_aggr_params bus_aggr;
 };
 
 void mwifiex_process_tx_queue(struct mwifiex_adapter *adapter);
@@ -1235,7 +1249,8 @@ mwifiex_queuing_ra_based(struct mwifiex_private *priv)
 	 * Currently we assume if we are in Infra, then DA=RA. This might not be
 	 * true in the future
 	 */
-	if ((priv->bss_mode == NL80211_IFTYPE_STATION) &&
+	if ((priv->bss_mode == NL80211_IFTYPE_STATION ||
+	     priv->bss_mode == NL80211_IFTYPE_P2P_CLIENT) &&
 	    (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA))
 		return false;
 
diff --git a/drivers/net/wireless/marvell/mwifiex/pcie.c b/drivers/net/wireless/marvell/mwifiex/pcie.c
index ac62bce50e96..b53ecf1eddda 100644
--- a/drivers/net/wireless/marvell/mwifiex/pcie.c
+++ b/drivers/net/wireless/marvell/mwifiex/pcie.c
@@ -370,7 +370,6 @@ static void mwifiex_pcie_reset_notify(struct pci_dev *pdev, bool prepare)
 		 * PCIe and HW.
 		 */
 		mwifiex_shutdown_sw(adapter);
-		adapter->surprise_removed = true;
 		clear_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP, &card->work_flags);
 		clear_bit(MWIFIEX_IFACE_WORK_CARD_RESET, &card->work_flags);
 	} else {
@@ -378,7 +377,6 @@ static void mwifiex_pcie_reset_notify(struct pci_dev *pdev, bool prepare)
 		 * after performing FLR respectively. Reconfigure the software
 		 * and firmware including firmware redownload
 		 */
-		adapter->surprise_removed = false;
 		ret = mwifiex_reinit_sw(adapter);
 		if (ret) {
 			dev_err(&pdev->dev, "reinit failed: %d\n", ret);
@@ -1391,7 +1389,7 @@ static int mwifiex_pcie_process_recv_data(struct mwifiex_adapter *adapter)
 		 * first 2 bytes for len, next 2 bytes is for type
 		 */
 		rx_len = get_unaligned_le16(skb_data->data);
-		if (WARN_ON(rx_len <= INTF_HEADER_LEN ||
+		if (WARN_ON(rx_len <= adapter->intf_hdr_len ||
 			    rx_len > MWIFIEX_RX_DATA_BUF_SIZE)) {
 			mwifiex_dbg(adapter, ERROR,
 				    "Invalid RX len %d, Rd=%#x, Wr=%#x\n",
@@ -1402,7 +1400,7 @@ static int mwifiex_pcie_process_recv_data(struct mwifiex_adapter *adapter)
 			mwifiex_dbg(adapter, DATA,
 				    "info: RECV DATA: Rd=%#x, Wr=%#x, Len=%d\n",
 				    card->rxbd_rdptr, wrptr, rx_len);
-			skb_pull(skb_data, INTF_HEADER_LEN);
+			skb_pull(skb_data, adapter->intf_hdr_len);
 			if (adapter->rx_work_enabled) {
 				skb_queue_tail(&adapter->rx_data_q, skb_data);
 				adapter->data_received = true;
@@ -1736,7 +1734,7 @@ static int mwifiex_pcie_process_cmd_complete(struct mwifiex_adapter *adapter)
 						       MWIFIEX_MAX_DELAY_COUNT);
 			mwifiex_unmap_pci_memory(adapter, skb,
 						 PCI_DMA_FROMDEVICE);
-			skb_pull(skb, INTF_HEADER_LEN);
+			skb_pull(skb, adapter->intf_hdr_len);
 			while (reg->sleep_cookie && (count++ < 10) &&
 			       mwifiex_pcie_ok_to_access_hw(adapter))
 				usleep_range(50, 60);
@@ -1749,12 +1747,12 @@ static int mwifiex_pcie_process_cmd_complete(struct mwifiex_adapter *adapter)
 		}
 		memcpy(adapter->upld_buf, skb->data,
 		       min_t(u32, MWIFIEX_SIZE_OF_CMD_BUFFER, skb->len));
-		skb_push(skb, INTF_HEADER_LEN);
+		skb_push(skb, adapter->intf_hdr_len);
 		if (mwifiex_map_pci_memory(adapter, skb, MWIFIEX_UPLD_SIZE,
 					   PCI_DMA_FROMDEVICE))
 			return -1;
 	} else if (mwifiex_pcie_ok_to_access_hw(adapter)) {
-		skb_pull(skb, INTF_HEADER_LEN);
+		skb_pull(skb, adapter->intf_hdr_len);
 		adapter->curr_cmd->resp_skb = skb;
 		adapter->cmd_resp_received = true;
 		/* Take the pointer and set it to CMD node and will
@@ -1791,7 +1789,7 @@ static int mwifiex_pcie_cmdrsp_complete(struct mwifiex_adapter *adapter,
 
 	if (skb) {
 		card->cmdrsp_buf = skb;
-		skb_push(card->cmdrsp_buf, INTF_HEADER_LEN);
+		skb_push(card->cmdrsp_buf, adapter->intf_hdr_len);
 		if (mwifiex_map_pci_memory(adapter, skb, MWIFIEX_UPLD_SIZE,
 					   PCI_DMA_FROMDEVICE))
 			return -1;
@@ -1856,14 +1854,15 @@ static int mwifiex_pcie_process_event_ready(struct mwifiex_adapter *adapter)
 		desc = card->evtbd_ring[rdptr];
 		memset(desc, 0, sizeof(*desc));
 
-		event = get_unaligned_le32(&skb_cmd->data[INTF_HEADER_LEN]);
+		event = get_unaligned_le32(
+			&skb_cmd->data[adapter->intf_hdr_len]);
 		adapter->event_cause = event;
 		/* The first 4bytes will be the event transfer header
 		   len is 2 bytes followed by type which is 2 bytes */
 		memcpy(&data_len, skb_cmd->data, sizeof(__le16));
 		evt_len = le16_to_cpu(data_len);
 		skb_trim(skb_cmd, evt_len);
-		skb_pull(skb_cmd, INTF_HEADER_LEN);
+		skb_pull(skb_cmd, adapter->intf_hdr_len);
 		mwifiex_dbg(adapter, EVENT,
 			    "info: Event length: %d\n", evt_len);
 
@@ -1922,7 +1921,7 @@ static int mwifiex_pcie_event_complete(struct mwifiex_adapter *adapter,
 	}
 
 	if (!card->evt_buf_list[rdptr]) {
-		skb_push(skb, INTF_HEADER_LEN);
+		skb_push(skb, adapter->intf_hdr_len);
 		skb_put(skb, MAX_EVENT_SIZE - skb->len);
 		if (mwifiex_map_pci_memory(adapter, skb,
 					   MAX_EVENT_SIZE,
@@ -2380,11 +2379,6 @@ static irqreturn_t mwifiex_pcie_interrupt(int irq, void *context)
 	struct pcie_service_card *card;
 	struct mwifiex_adapter *adapter;
 
-	if (!pdev) {
-		pr_err("info: %s: pdev is NULL\n", __func__);
-		goto exit;
-	}
-
 	card = pci_get_drvdata(pdev);
 
 	if (!card->adapter) {
@@ -2822,6 +2816,13 @@ static void mwifiex_pcie_device_dump_work(struct mwifiex_adapter *adapter)
 	mwifiex_upload_device_dump(adapter, drv_info, drv_info_size);
 }
 
+static void mwifiex_pcie_card_reset_work(struct mwifiex_adapter *adapter)
+{
+	struct pcie_service_card *card = adapter->card;
+
+	pci_reset_function(card->dev);
+}
+
 static void mwifiex_pcie_work(struct work_struct *work)
 {
 	struct pcie_service_card *card =
@@ -2830,6 +2831,9 @@ static void mwifiex_pcie_work(struct work_struct *work)
 	if (test_and_clear_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP,
 			       &card->work_flags))
 		mwifiex_pcie_device_dump_work(card->adapter);
+	if (test_and_clear_bit(MWIFIEX_IFACE_WORK_CARD_RESET,
+			       &card->work_flags))
+		mwifiex_pcie_card_reset_work(card->adapter);
 }
 
 /* This function dumps FW information */
@@ -2837,12 +2841,72 @@ static void mwifiex_pcie_device_dump(struct mwifiex_adapter *adapter)
 {
 	struct pcie_service_card *card = adapter->card;
 
-	if (test_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP, &card->work_flags))
-		return;
+	if (!test_and_set_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP,
+			      &card->work_flags))
+		schedule_work(&card->work);
+}
+
+static void mwifiex_pcie_card_reset(struct mwifiex_adapter *adapter)
+{
+	struct pcie_service_card *card = adapter->card;
+
+	if (!test_and_set_bit(MWIFIEX_IFACE_WORK_CARD_RESET, &card->work_flags))
+		schedule_work(&card->work);
+}
+
+static int mwifiex_pcie_alloc_buffers(struct mwifiex_adapter *adapter)
+{
+	struct pcie_service_card *card = adapter->card;
+	const struct mwifiex_pcie_card_reg *reg = card->pcie.reg;
+	int ret;
+
+	card->cmdrsp_buf = NULL;
+	ret = mwifiex_pcie_create_txbd_ring(adapter);
+	if (ret) {
+		mwifiex_dbg(adapter, ERROR, "Failed to create txbd ring\n");
+		goto err_cre_txbd;
+	}
+
+	ret = mwifiex_pcie_create_rxbd_ring(adapter);
+	if (ret) {
+		mwifiex_dbg(adapter, ERROR, "Failed to create rxbd ring\n");
+		goto err_cre_rxbd;
+	}
+
+	ret = mwifiex_pcie_create_evtbd_ring(adapter);
+	if (ret) {
+		mwifiex_dbg(adapter, ERROR, "Failed to create evtbd ring\n");
+		goto err_cre_evtbd;
+	}
 
-	set_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP, &card->work_flags);
+	ret = mwifiex_pcie_alloc_cmdrsp_buf(adapter);
+	if (ret) {
+		mwifiex_dbg(adapter, ERROR, "Failed to allocate cmdbuf buffer\n");
+		goto err_alloc_cmdbuf;
+	}
 
-	schedule_work(&card->work);
+	if (reg->sleep_cookie) {
+		ret = mwifiex_pcie_alloc_sleep_cookie_buf(adapter);
+		if (ret) {
+			mwifiex_dbg(adapter, ERROR, "Failed to allocate sleep_cookie buffer\n");
+			goto err_alloc_cookie;
+		}
+	} else {
+		card->sleep_cookie_vbase = NULL;
+	}
+
+	return 0;
+
+err_alloc_cookie:
+	mwifiex_pcie_delete_cmdrsp_buf(adapter);
+err_alloc_cmdbuf:
+	mwifiex_pcie_delete_evtbd_ring(adapter);
+err_cre_evtbd:
+	mwifiex_pcie_delete_rxbd_ring(adapter);
+err_cre_rxbd:
+	mwifiex_pcie_delete_txbd_ring(adapter);
+err_cre_txbd:
+	return ret;
 }
 
 static void mwifiex_pcie_free_buffers(struct mwifiex_adapter *adapter)
@@ -2862,20 +2926,12 @@ static void mwifiex_pcie_free_buffers(struct mwifiex_adapter *adapter)
 
 /*
  * This function initializes the PCI-E host memory space, WCB rings, etc.
- *
- * The following initializations steps are followed -
- *      - Allocate TXBD ring buffers
- *      - Allocate RXBD ring buffers
- *      - Allocate event BD ring buffers
- *      - Allocate command response ring buffer
- *      - Allocate sleep cookie buffer
  */
 static int mwifiex_init_pcie(struct mwifiex_adapter *adapter)
 {
 	struct pcie_service_card *card = adapter->card;
 	int ret;
 	struct pci_dev *pdev = card->dev;
-	const struct mwifiex_pcie_card_reg *reg = card->pcie.reg;
 
 	pci_set_drvdata(pdev, card);
 
@@ -2924,37 +2980,13 @@ static int mwifiex_init_pcie(struct mwifiex_adapter *adapter)
 	pr_notice("PCI memory map Virt0: %p PCI memory map Virt2: %p\n",
 		  card->pci_mmap, card->pci_mmap1);
 
-	card->cmdrsp_buf = NULL;
-	ret = mwifiex_pcie_create_txbd_ring(adapter);
-	if (ret)
-		goto err_cre_txbd;
-	ret = mwifiex_pcie_create_rxbd_ring(adapter);
+	ret = mwifiex_pcie_alloc_buffers(adapter);
 	if (ret)
-		goto err_cre_rxbd;
-	ret = mwifiex_pcie_create_evtbd_ring(adapter);
-	if (ret)
-		goto err_cre_evtbd;
-	ret = mwifiex_pcie_alloc_cmdrsp_buf(adapter);
-	if (ret)
-		goto err_alloc_cmdbuf;
-	if (reg->sleep_cookie) {
-		ret = mwifiex_pcie_alloc_sleep_cookie_buf(adapter);
-		if (ret)
-			goto err_alloc_cookie;
-	} else {
-		card->sleep_cookie_vbase = NULL;
-	}
-	return ret;
+		goto err_alloc_buffers;
 
-err_alloc_cookie:
-	mwifiex_pcie_delete_cmdrsp_buf(adapter);
-err_alloc_cmdbuf:
-	mwifiex_pcie_delete_evtbd_ring(adapter);
-err_cre_evtbd:
-	mwifiex_pcie_delete_rxbd_ring(adapter);
-err_cre_rxbd:
-	mwifiex_pcie_delete_txbd_ring(adapter);
-err_cre_txbd:
+	return 0;
+
+err_alloc_buffers:
 	pci_iounmap(pdev, card->pci_mmap1);
 err_iomap2:
 	pci_release_region(pdev, 2);
@@ -3168,73 +3200,25 @@ static void mwifiex_unregister_dev(struct mwifiex_adapter *adapter)
 	card->adapter = NULL;
 }
 
-/* This function initializes the PCI-E host memory space, WCB rings, etc.
- *
- * The following initializations steps are followed -
- *      - Allocate TXBD ring buffers
- *      - Allocate RXBD ring buffers
- *      - Allocate event BD ring buffers
- *      - Allocate command response ring buffer
- *      - Allocate sleep cookie buffer
- * Part of mwifiex_init_pcie(), not reset the PCIE registers
+/*
+ * This function initializes the PCI-E host memory space, WCB rings, etc.,
+ * similar to mwifiex_init_pcie(), but without resetting PCI-E state.
  */
 static void mwifiex_pcie_up_dev(struct mwifiex_adapter *adapter)
 {
 	struct pcie_service_card *card = adapter->card;
 	int ret;
 	struct pci_dev *pdev = card->dev;
-	const struct mwifiex_pcie_card_reg *reg = card->pcie.reg;
 
 	/* tx_buf_size might be changed to 3584 by firmware during
 	 * data transfer, we should reset it to default size.
 	 */
 	adapter->tx_buf_size = card->pcie.tx_buf_size;
 
-	card->cmdrsp_buf = NULL;
-	ret = mwifiex_pcie_create_txbd_ring(adapter);
-	if (ret) {
-		mwifiex_dbg(adapter, ERROR, "Failed to create txbd ring\n");
-		goto err_cre_txbd;
-	}
-
-	ret = mwifiex_pcie_create_rxbd_ring(adapter);
-	if (ret) {
-		mwifiex_dbg(adapter, ERROR, "Failed to create rxbd ring\n");
-		goto err_cre_rxbd;
-	}
-
-	ret = mwifiex_pcie_create_evtbd_ring(adapter);
-	if (ret) {
-		mwifiex_dbg(adapter, ERROR, "Failed to create evtbd ring\n");
-		goto err_cre_evtbd;
-	}
-
-	ret = mwifiex_pcie_alloc_cmdrsp_buf(adapter);
-	if (ret) {
-		mwifiex_dbg(adapter, ERROR, "Failed to allocate cmdbuf buffer\n");
-		goto err_alloc_cmdbuf;
-	}
-
-	if (reg->sleep_cookie) {
-		ret = mwifiex_pcie_alloc_sleep_cookie_buf(adapter);
-		if (ret) {
-			mwifiex_dbg(adapter, ERROR, "Failed to allocate sleep_cookie buffer\n");
-			goto err_alloc_cookie;
-		}
-	} else {
-		card->sleep_cookie_vbase = NULL;
-	}
-	return;
+	ret = mwifiex_pcie_alloc_buffers(adapter);
+	if (!ret)
+		return;
 
-err_alloc_cookie:
-	mwifiex_pcie_delete_cmdrsp_buf(adapter);
-err_alloc_cmdbuf:
-	mwifiex_pcie_delete_evtbd_ring(adapter);
-err_cre_evtbd:
-	mwifiex_pcie_delete_rxbd_ring(adapter);
-err_cre_rxbd:
-	mwifiex_pcie_delete_txbd_ring(adapter);
-err_cre_txbd:
 	pci_iounmap(pdev, card->pci_mmap1);
 }
 
@@ -3274,6 +3258,7 @@ static struct mwifiex_if_ops pcie_ops = {
 	.cleanup_mpa_buf =		NULL,
 	.init_fw_port =			mwifiex_pcie_init_fw_port,
 	.clean_pcie_ring =		mwifiex_clean_pcie_ring_buf,
+	.card_reset =			mwifiex_pcie_card_reset,
 	.reg_dump =			mwifiex_pcie_reg_dump,
 	.device_dump =			mwifiex_pcie_device_dump,
 	.down_dev =			mwifiex_pcie_down_dev,
diff --git a/drivers/net/wireless/marvell/mwifiex/scan.c b/drivers/net/wireless/marvell/mwifiex/scan.c
index ce6936d0c5c0..ae9630b49342 100644
--- a/drivers/net/wireless/marvell/mwifiex/scan.c
+++ b/drivers/net/wireless/marvell/mwifiex/scan.c
@@ -2809,7 +2809,7 @@ int mwifiex_request_scan(struct mwifiex_private *priv,
 {
 	int ret;
 
-	if (down_interruptible(&priv->async_sem)) {
+	if (mutex_lock_interruptible(&priv->async_mutex)) {
 		mwifiex_dbg(priv->adapter, ERROR,
 			    "%s: acquire semaphore fail\n",
 			    __func__);
@@ -2825,7 +2825,7 @@ int mwifiex_request_scan(struct mwifiex_private *priv,
 		/* Normal scan */
 		ret = mwifiex_scan_networks(priv, NULL);
 
-	up(&priv->async_sem);
+	mutex_unlock(&priv->async_mutex);
 
 	return ret;
 }
diff --git a/drivers/net/wireless/marvell/mwifiex/sdio.c b/drivers/net/wireless/marvell/mwifiex/sdio.c
index 0af1c6733c92..f81a006668f3 100644
--- a/drivers/net/wireless/marvell/mwifiex/sdio.c
+++ b/drivers/net/wireless/marvell/mwifiex/sdio.c
@@ -1125,7 +1125,7 @@ static void mwifiex_deaggr_sdio_pkt(struct mwifiex_adapter *adapter,
 	data = skb->data;
 	total_pkt_len = skb->len;
 
-	while (total_pkt_len >= (SDIO_HEADER_OFFSET + INTF_HEADER_LEN)) {
+	while (total_pkt_len >= (SDIO_HEADER_OFFSET + adapter->intf_hdr_len)) {
 		if (total_pkt_len < adapter->sdio_rx_block_size)
 			break;
 		blk_num = *(data + BLOCK_NUMBER_OFFSET);
@@ -1152,7 +1152,7 @@ static void mwifiex_deaggr_sdio_pkt(struct mwifiex_adapter *adapter,
 			break;
 		skb_put(skb_deaggr, pkt_len);
 		memcpy(skb_deaggr->data, data + SDIO_HEADER_OFFSET, pkt_len);
-		skb_pull(skb_deaggr, INTF_HEADER_LEN);
+		skb_pull(skb_deaggr, adapter->intf_hdr_len);
 
 		mwifiex_handle_rx_packet(adapter, skb_deaggr);
 		data += blk_size;
@@ -1178,7 +1178,7 @@ static int mwifiex_decode_rx_packet(struct mwifiex_adapter *adapter,
 
 	if (upld_typ != MWIFIEX_TYPE_AGGR_DATA) {
 		skb_trim(skb, pkt_len);
-		skb_pull(skb, INTF_HEADER_LEN);
+		skb_pull(skb, adapter->intf_hdr_len);
 	}
 
 	switch (upld_typ) {
@@ -1537,7 +1537,7 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 		rx_len = card->mp_regs[reg->cmd_rd_len_1] << 8;
 		rx_len |= (u16)card->mp_regs[reg->cmd_rd_len_0];
 		rx_blocks = DIV_ROUND_UP(rx_len, MWIFIEX_SDIO_BLOCK_SIZE);
-		if (rx_len <= INTF_HEADER_LEN ||
+		if (rx_len <= adapter->intf_hdr_len ||
 		    (rx_blocks * MWIFIEX_SDIO_BLOCK_SIZE) >
 		     MWIFIEX_RX_DATA_BUF_SIZE)
 			return -1;
@@ -1635,7 +1635,7 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 			rx_blocks =
 				(rx_len + MWIFIEX_SDIO_BLOCK_SIZE -
 				 1) / MWIFIEX_SDIO_BLOCK_SIZE;
-			if (rx_len <= INTF_HEADER_LEN ||
+			if (rx_len <= adapter->intf_hdr_len ||
 			    (card->mpa_rx.enabled &&
 			     ((rx_blocks * MWIFIEX_SDIO_BLOCK_SIZE) >
 			      card->mpa_rx.buf_size))) {
@@ -1896,7 +1896,7 @@ static int mwifiex_sdio_host_to_card(struct mwifiex_adapter *adapter,
 		adapter->cmd_sent = true;
 		/* Type must be MWIFIEX_TYPE_CMD */
 
-		if (pkt_len <= INTF_HEADER_LEN ||
+		if (pkt_len <= adapter->intf_hdr_len ||
 		    pkt_len > MWIFIEX_UPLD_SIZE)
 			mwifiex_dbg(adapter, ERROR,
 				    "%s: payload=%p, nb=%d\n",
@@ -2533,12 +2533,8 @@ static void mwifiex_sdio_card_reset(struct mwifiex_adapter *adapter)
 {
 	struct sdio_mmc_card *card = adapter->card;
 
-	if (test_bit(MWIFIEX_IFACE_WORK_CARD_RESET, &card->work_flags))
-		return;
-
-	set_bit(MWIFIEX_IFACE_WORK_CARD_RESET, &card->work_flags);
-
-	schedule_work(&card->work);
+	if (!test_and_set_bit(MWIFIEX_IFACE_WORK_CARD_RESET, &card->work_flags))
+		schedule_work(&card->work);
 }
 
 /* This function dumps FW information */
@@ -2546,11 +2542,9 @@ static void mwifiex_sdio_device_dump(struct mwifiex_adapter *adapter)
 {
 	struct sdio_mmc_card *card = adapter->card;
 
-	if (test_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP, &card->work_flags))
-		return;
-
-	set_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP, &card->work_flags);
-	schedule_work(&card->work);
+	if (!test_and_set_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP,
+			      &card->work_flags))
+		schedule_work(&card->work);
 }
 
 /* Function to dump SDIO function registers and SDIO scratch registers in case
diff --git a/drivers/net/wireless/marvell/mwifiex/sta_cmd.c b/drivers/net/wireless/marvell/mwifiex/sta_cmd.c
index 83916c1439af..534d94a206a5 100644
--- a/drivers/net/wireless/marvell/mwifiex/sta_cmd.c
+++ b/drivers/net/wireless/marvell/mwifiex/sta_cmd.c
@@ -2064,6 +2064,15 @@ int mwifiex_sta_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
 	case HostCmd_CMD_11AC_CFG:
 		ret = mwifiex_cmd_11ac_cfg(priv, cmd_ptr, cmd_action, data_buf);
 		break;
+	case HostCmd_CMD_PACKET_AGGR_CTRL:
+		cmd_ptr->command = cpu_to_le16(cmd_no);
+		cmd_ptr->params.pkt_aggr_ctrl.action = cpu_to_le16(cmd_action);
+		cmd_ptr->params.pkt_aggr_ctrl.enable =
+						cpu_to_le16(*(u16 *)data_buf);
+		cmd_ptr->size =
+			cpu_to_le16(sizeof(struct host_cmd_ds_pkt_aggr_ctrl) +
+				    S_DS_GEN);
+		break;
 	case HostCmd_CMD_P2P_MODE_CFG:
 		cmd_ptr->command = cpu_to_le16(cmd_no);
 		cmd_ptr->params.mode_cfg.action = cpu_to_le16(cmd_action);
@@ -2241,6 +2250,7 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta, bool init)
 	enum state_11d_t state_11d;
 	struct mwifiex_ds_11n_tx_cfg tx_cfg;
 	u8 sdio_sp_rx_aggr_enable;
+	u16 packet_aggr_enable;
 	int data;
 
 	if (first_sta) {
@@ -2387,6 +2397,14 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta, bool init)
 				    "11D: failed to enable 11D\n");
 	}
 
+	/* Pacekt aggregation handshake with firmware */
+	if (aggr_ctrl) {
+		packet_aggr_enable = true;
+		mwifiex_send_cmd(priv, HostCmd_CMD_PACKET_AGGR_CTRL,
+				 HostCmd_ACT_GEN_SET, 0,
+				 &packet_aggr_enable, true);
+	}
+
 	/* Send cmd to FW to configure 11n specific configuration
 	 * (Short GI, Channel BW, Green field support etc.) for transmit
 	 */
diff --git a/drivers/net/wireless/marvell/mwifiex/sta_cmdresp.c b/drivers/net/wireless/marvell/mwifiex/sta_cmdresp.c
index f1d1f56fc23f..2945775e83c5 100644
--- a/drivers/net/wireless/marvell/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/marvell/mwifiex/sta_cmdresp.c
@@ -1154,6 +1154,27 @@ static int mwifiex_ret_chan_region_cfg(struct mwifiex_private *priv,
 	return 0;
 }
 
+static int mwifiex_ret_pkt_aggr_ctrl(struct mwifiex_private *priv,
+				     struct host_cmd_ds_command *resp)
+{
+	struct host_cmd_ds_pkt_aggr_ctrl *pkt_aggr_ctrl =
+					&resp->params.pkt_aggr_ctrl;
+	struct mwifiex_adapter *adapter = priv->adapter;
+
+	adapter->bus_aggr.enable = le16_to_cpu(pkt_aggr_ctrl->enable);
+	if (adapter->bus_aggr.enable)
+		adapter->intf_hdr_len = INTF_HEADER_LEN;
+	adapter->bus_aggr.mode = MWIFIEX_BUS_AGGR_MODE_LEN_V2;
+	adapter->bus_aggr.tx_aggr_max_size =
+				le16_to_cpu(pkt_aggr_ctrl->tx_aggr_max_size);
+	adapter->bus_aggr.tx_aggr_max_num =
+				le16_to_cpu(pkt_aggr_ctrl->tx_aggr_max_num);
+	adapter->bus_aggr.tx_aggr_align =
+				le16_to_cpu(pkt_aggr_ctrl->tx_aggr_align);
+
+	return 0;
+}
+
 /*
  * This function handles the command responses.
  *
@@ -1255,6 +1276,9 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 		break;
 	case HostCmd_CMD_11AC_CFG:
 		break;
+	case HostCmd_CMD_PACKET_AGGR_CTRL:
+		ret = mwifiex_ret_pkt_aggr_ctrl(priv, resp);
+		break;
 	case HostCmd_CMD_P2P_MODE_CFG:
 		ret = mwifiex_ret_p2p_mode_cfg(priv, resp, data_buf);
 		break;
diff --git a/drivers/net/wireless/marvell/mwifiex/sta_tx.c b/drivers/net/wireless/marvell/mwifiex/sta_tx.c
index f6683ea6bd5d..620f8650a742 100644
--- a/drivers/net/wireless/marvell/mwifiex/sta_tx.c
+++ b/drivers/net/wireless/marvell/mwifiex/sta_tx.c
@@ -49,8 +49,7 @@ void *mwifiex_process_sta_txpd(struct mwifiex_private *priv,
 	struct mwifiex_txinfo *tx_info = MWIFIEX_SKB_TXCB(skb);
 	unsigned int pad;
 	u16 pkt_type, pkt_offset;
-	int hroom = (priv->adapter->iface_type == MWIFIEX_USB) ? 0 :
-		       INTF_HEADER_LEN;
+	int hroom = adapter->intf_hdr_len;
 
 	if (!skb->len) {
 		mwifiex_dbg(adapter, ERROR,
@@ -116,7 +115,7 @@ void *mwifiex_process_sta_txpd(struct mwifiex_private *priv,
 
 	local_tx_pd->tx_pkt_offset = cpu_to_le16(pkt_offset);
 
-	/* make space for INTF_HEADER_LEN */
+	/* make space for adapter->intf_hdr_len */
 	skb_push(skb, hroom);
 
 	if (!local_tx_pd->tx_control)
@@ -165,8 +164,9 @@ int mwifiex_send_null_packet(struct mwifiex_private *priv, u8 flags)
 	memset(tx_info, 0, sizeof(*tx_info));
 	tx_info->bss_num = priv->bss_num;
 	tx_info->bss_type = priv->bss_type;
-	tx_info->pkt_len = data_len - (sizeof(struct txpd) + INTF_HEADER_LEN);
-	skb_reserve(skb, sizeof(struct txpd) + INTF_HEADER_LEN);
+	tx_info->pkt_len = data_len -
+			(sizeof(struct txpd) + adapter->intf_hdr_len);
+	skb_reserve(skb, sizeof(struct txpd) + adapter->intf_hdr_len);
 	skb_push(skb, sizeof(struct txpd));
 
 	local_tx_pd = (struct txpd *) skb->data;
@@ -177,11 +177,11 @@ int mwifiex_send_null_packet(struct mwifiex_private *priv, u8 flags)
 	local_tx_pd->bss_num = priv->bss_num;
 	local_tx_pd->bss_type = priv->bss_type;
 
+	skb_push(skb, adapter->intf_hdr_len);
 	if (adapter->iface_type == MWIFIEX_USB) {
 		ret = adapter->if_ops.host_to_card(adapter, priv->usb_port,
 						   skb, NULL);
 	} else {
-		skb_push(skb, INTF_HEADER_LEN);
 		tx_param.next_pkt_len = 0;
 		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_DATA,
 						   skb, &tx_param);
diff --git a/drivers/net/wireless/marvell/mwifiex/tdls.c b/drivers/net/wireless/marvell/mwifiex/tdls.c
index 7d0d3ff3dd4c..39cd677d4159 100644
--- a/drivers/net/wireless/marvell/mwifiex/tdls.c
+++ b/drivers/net/wireless/marvell/mwifiex/tdls.c
@@ -55,11 +55,8 @@ static void mwifiex_restore_tdls_packets(struct mwifiex_private *priv,
 			tx_info->flags |= MWIFIEX_BUF_FLAG_TDLS_PKT;
 		} else {
 			tid_list = &priv->wmm.tid_tbl_ptr[tid_down].ra_list;
-			if (!list_empty(tid_list))
-				ra_list = list_first_entry(tid_list,
-					      struct mwifiex_ra_list_tbl, list);
-			else
-				ra_list = NULL;
+			ra_list = list_first_entry_or_null(tid_list,
+					struct mwifiex_ra_list_tbl, list);
 			tx_info->flags &= ~MWIFIEX_BUF_FLAG_TDLS_PKT;
 		}
 
@@ -161,7 +158,7 @@ static void mwifiex_tdls_add_aid(struct mwifiex_private *priv,
 	u8 *pos;
 
 	assoc_rsp = (struct ieee_types_assoc_rsp *)&priv->assoc_rsp_buf;
-	pos = (void *)skb_put(skb, 4);
+	pos = skb_put(skb, 4);
 	*pos++ = WLAN_EID_AID;
 	*pos++ = 2;
 	memcpy(pos, &assoc_rsp->a_id, sizeof(assoc_rsp->a_id));
@@ -175,7 +172,7 @@ static int mwifiex_tdls_add_vht_capab(struct mwifiex_private *priv,
 	struct ieee80211_vht_cap vht_cap;
 	u8 *pos;
 
-	pos = (void *)skb_put(skb, sizeof(struct ieee80211_vht_cap) + 2);
+	pos = skb_put(skb, sizeof(struct ieee80211_vht_cap) + 2);
 	*pos++ = WLAN_EID_VHT_CAPABILITY;
 	*pos++ = sizeof(struct ieee80211_vht_cap);
 
@@ -210,7 +207,7 @@ mwifiex_tdls_add_ht_oper(struct mwifiex_private *priv, const u8 *mac,
 		return 0;
 	}
 
-	pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_operation) + 2);
+	pos = skb_put(skb, sizeof(struct ieee80211_ht_operation) + 2);
 	*pos++ = WLAN_EID_HT_OPERATION;
 	*pos++ = sizeof(struct ieee80211_ht_operation);
 	ht_oper = (void *)pos;
@@ -275,7 +272,7 @@ static int mwifiex_tdls_add_vht_oper(struct mwifiex_private *priv,
 		ap_vht_cap = bss_desc->bcn_vht_cap;
 	}
 
-	pos = (void *)skb_put(skb, sizeof(struct ieee80211_vht_operation) + 2);
+	pos = skb_put(skb, sizeof(struct ieee80211_vht_operation) + 2);
 	*pos++ = WLAN_EID_VHT_OPERATION;
 	*pos++ = sizeof(struct ieee80211_vht_operation);
 	vht_oper = (struct ieee80211_vht_operation *)pos;
@@ -362,7 +359,7 @@ static void mwifiex_tdls_add_ext_capab(struct mwifiex_private *priv,
 {
 	struct ieee_types_extcap *extcap;
 
-	extcap = (void *)skb_put(skb, sizeof(struct ieee_types_extcap));
+	extcap = skb_put(skb, sizeof(struct ieee_types_extcap));
 	extcap->ieee_hdr.element_id = WLAN_EID_EXT_CAPABILITY;
 	extcap->ieee_hdr.len = 8;
 	memset(extcap->ext_capab, 0, 8);
@@ -375,7 +372,7 @@ static void mwifiex_tdls_add_ext_capab(struct mwifiex_private *priv,
 
 static void mwifiex_tdls_add_qos_capab(struct sk_buff *skb)
 {
-	u8 *pos = (void *)skb_put(skb, 3);
+	u8 *pos = skb_put(skb, 3);
 
 	*pos++ = WLAN_EID_QOS_CAPA;
 	*pos++ = 1;
@@ -391,8 +388,7 @@ mwifiex_tdls_add_wmm_param_ie(struct mwifiex_private *priv, struct sk_buff *skb)
 	u8 ac_be[] = {0x03, 0xa4, 0x00, 0x00};
 	u8 ac_bk[] = {0x27, 0xa4, 0x00, 0x00};
 
-	wmm = (void *)skb_put(skb, sizeof(*wmm));
-	memset(wmm, 0, sizeof(*wmm));
+	wmm = skb_put_zero(skb, sizeof(*wmm));
 
 	wmm->element_id = WLAN_EID_VENDOR_SPECIFIC;
 	wmm->len = sizeof(*wmm) - 2;
@@ -417,8 +413,8 @@ mwifiex_add_wmm_info_ie(struct mwifiex_private *priv, struct sk_buff *skb,
 {
 	u8 *buf;
 
-	buf = (void *)skb_put(skb, MWIFIEX_TDLS_WMM_INFO_SIZE +
-			      sizeof(struct ieee_types_header));
+	buf = skb_put(skb,
+		      MWIFIEX_TDLS_WMM_INFO_SIZE + sizeof(struct ieee_types_header));
 
 	*buf++ = WLAN_EID_VENDOR_SPECIFIC;
 	*buf++ = 7; /* len */
@@ -435,7 +431,7 @@ static void mwifiex_tdls_add_bss_co_2040(struct sk_buff *skb)
 {
 	struct ieee_types_bss_co_2040 *bssco;
 
-	bssco = (void *)skb_put(skb, sizeof(struct ieee_types_bss_co_2040));
+	bssco = skb_put(skb, sizeof(struct ieee_types_bss_co_2040));
 	bssco->ieee_hdr.element_id = WLAN_EID_BSS_COEX_2040;
 	bssco->ieee_hdr.len = sizeof(struct ieee_types_bss_co_2040) -
 			      sizeof(struct ieee_types_header);
@@ -447,8 +443,8 @@ static void mwifiex_tdls_add_supported_chan(struct sk_buff *skb)
 	struct ieee_types_generic *supp_chan;
 	u8 chan_supp[] = {1, 11};
 
-	supp_chan = (void *)skb_put(skb, (sizeof(struct ieee_types_header) +
-					  sizeof(chan_supp)));
+	supp_chan = skb_put(skb,
+			    (sizeof(struct ieee_types_header) + sizeof(chan_supp)));
 	supp_chan->ieee_hdr.element_id = WLAN_EID_SUPPORTED_CHANNELS;
 	supp_chan->ieee_hdr.len = sizeof(chan_supp);
 	memcpy(supp_chan->data, chan_supp, sizeof(chan_supp));
@@ -459,8 +455,8 @@ static void mwifiex_tdls_add_oper_class(struct sk_buff *skb)
 	struct ieee_types_generic *reg_class;
 	u8 rc_list[] = {1,
 		1, 2, 3, 4, 12, 22, 23, 24, 25, 27, 28, 29, 30, 32, 33};
-	reg_class = (void *)skb_put(skb, (sizeof(struct ieee_types_header) +
-					  sizeof(rc_list)));
+	reg_class = skb_put(skb,
+			    (sizeof(struct ieee_types_header) + sizeof(rc_list)));
 	reg_class->ieee_hdr.element_id = WLAN_EID_SUPPORTED_REGULATORY_CLASSES;
 	reg_class->ieee_hdr.len = sizeof(rc_list);
 	memcpy(reg_class->data, rc_list, sizeof(rc_list));
@@ -479,7 +475,7 @@ static int mwifiex_prep_tdls_encap_data(struct mwifiex_private *priv,
 
 	capab = priv->curr_bss_params.bss_descriptor.cap_info_bitmap;
 
-	tf = (void *)skb_put(skb, offsetof(struct ieee80211_tdls_data, u));
+	tf = skb_put(skb, offsetof(struct ieee80211_tdls_data, u));
 	memcpy(tf->da, peer, ETH_ALEN);
 	memcpy(tf->sa, priv->curr_addr, ETH_ALEN);
 	tf->ether_type = cpu_to_be16(ETH_P_TDLS);
@@ -498,7 +494,7 @@ static int mwifiex_prep_tdls_encap_data(struct mwifiex_private *priv,
 			return ret;
 		}
 
-		pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
+		pos = skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
 		*pos++ = WLAN_EID_HT_CAPABILITY;
 		*pos++ = sizeof(struct ieee80211_ht_cap);
 		ht_cap = (void *)pos;
@@ -538,7 +534,7 @@ static int mwifiex_prep_tdls_encap_data(struct mwifiex_private *priv,
 			return ret;
 		}
 
-		pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
+		pos = skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
 		*pos++ = WLAN_EID_HT_CAPABILITY;
 		*pos++ = sizeof(struct ieee80211_ht_cap);
 		ht_cap = (void *)pos;
@@ -620,7 +616,7 @@ mwifiex_tdls_add_link_ie(struct sk_buff *skb, const u8 *src_addr,
 {
 	struct ieee80211_tdls_lnkie *lnkid;
 
-	lnkid = (void *)skb_put(skb, sizeof(struct ieee80211_tdls_lnkie));
+	lnkid = skb_put(skb, sizeof(struct ieee80211_tdls_lnkie));
 	lnkid->ie_type = WLAN_EID_LINK_ID;
 	lnkid->ie_len = sizeof(struct ieee80211_tdls_lnkie) -
 			sizeof(struct ieee_types_header);
@@ -683,8 +679,7 @@ int mwifiex_send_tdls_data_frame(struct mwifiex_private *priv, const u8 *peer,
 			return ret;
 		}
 		if (extra_ies_len)
-			memcpy(skb_put(skb, extra_ies_len), extra_ies,
-			       extra_ies_len);
+			skb_put_data(skb, extra_ies, extra_ies_len);
 		mwifiex_tdls_add_link_ie(skb, priv->curr_addr, peer,
 					 priv->cfg_bssid);
 		break;
@@ -697,8 +692,7 @@ int mwifiex_send_tdls_data_frame(struct mwifiex_private *priv, const u8 *peer,
 			return ret;
 		}
 		if (extra_ies_len)
-			memcpy(skb_put(skb, extra_ies_len), extra_ies,
-			       extra_ies_len);
+			skb_put_data(skb, extra_ies, extra_ies_len);
 		mwifiex_tdls_add_link_ie(skb, peer, priv->curr_addr,
 					 priv->cfg_bssid);
 		break;
@@ -747,7 +741,7 @@ mwifiex_construct_tdls_action_frame(struct mwifiex_private *priv,
 
 	capab = priv->curr_bss_params.bss_descriptor.cap_info_bitmap;
 
-	mgmt = (void *)skb_put(skb, offsetof(struct ieee80211_mgmt, u));
+	mgmt = skb_put(skb, offsetof(struct ieee80211_mgmt, u));
 
 	memset(mgmt, 0, 24);
 	memcpy(mgmt->da, peer, ETH_ALEN);
@@ -781,7 +775,7 @@ mwifiex_construct_tdls_action_frame(struct mwifiex_private *priv,
 			return ret;
 		}
 
-		pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
+		pos = skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
 		*pos++ = WLAN_EID_HT_CAPABILITY;
 		*pos++ = sizeof(struct ieee80211_ht_cap);
 		ht_cap = (void *)pos;
@@ -856,8 +850,8 @@ int mwifiex_send_tdls_action_frame(struct mwifiex_private *priv, const u8 *peer,
 
 	pkt_type = PKT_TYPE_MGMT;
 	tx_control = 0;
-	pos = skb_put(skb, MWIFIEX_MGMT_FRAME_HEADER_SIZE + sizeof(pkt_len));
-	memset(pos, 0, MWIFIEX_MGMT_FRAME_HEADER_SIZE + sizeof(pkt_len));
+	pos = skb_put_zero(skb,
+			   MWIFIEX_MGMT_FRAME_HEADER_SIZE + sizeof(pkt_len));
 	memcpy(pos, &pkt_type, sizeof(pkt_type));
 	memcpy(pos + sizeof(pkt_type), &tx_control, sizeof(tx_control));
 
@@ -869,7 +863,7 @@ int mwifiex_send_tdls_action_frame(struct mwifiex_private *priv, const u8 *peer,
 	}
 
 	if (extra_ies_len)
-		memcpy(skb_put(skb, extra_ies_len), extra_ies, extra_ies_len);
+		skb_put_data(skb, extra_ies, extra_ies_len);
 
 	/* the TDLS link IE is always added last we are the responder */
 
diff --git a/drivers/net/wireless/marvell/mwifiex/txrx.c b/drivers/net/wireless/marvell/mwifiex/txrx.c
index fac28bd8fbee..d848933466d9 100644
--- a/drivers/net/wireless/marvell/mwifiex/txrx.c
+++ b/drivers/net/wireless/marvell/mwifiex/txrx.c
@@ -91,7 +91,7 @@ int mwifiex_process_tx(struct mwifiex_private *priv, struct sk_buff *skb,
 	struct mwifiex_sta_node *dest_node;
 	struct ethhdr *hdr = (void *)skb->data;
 
-	hroom = (adapter->iface_type == MWIFIEX_USB) ? 0 : INTF_HEADER_LEN;
+	hroom = adapter->intf_hdr_len;
 
 	if (priv->bss_role == MWIFIEX_BSS_ROLE_UAP) {
 		dest_node = mwifiex_get_sta_entry(priv, hdr->h_dest);
@@ -117,7 +117,7 @@ int mwifiex_process_tx(struct mwifiex_private *priv, struct sk_buff *skb,
 		if (adapter->iface_type == MWIFIEX_USB) {
 			ret = adapter->if_ops.host_to_card(adapter,
 							   priv->usb_port,
-							   skb, NULL);
+							   skb, tx_param);
 		} else {
 			ret = adapter->if_ops.host_to_card(adapter,
 							   MWIFIEX_TYPE_DATA,
@@ -179,18 +179,13 @@ static int mwifiex_host_to_card(struct mwifiex_adapter *adapter,
 		mwifiex_write_data_complete(adapter, skb, 0, 0);
 		return ret;
 	}
-	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA) {
-		if (adapter->iface_type == MWIFIEX_USB)
-			local_tx_pd = (struct txpd *)head_ptr;
-		else
-			local_tx_pd = (struct txpd *) (head_ptr +
-				INTF_HEADER_LEN);
-	}
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA)
+		local_tx_pd = (struct txpd *)(head_ptr + adapter->intf_hdr_len);
 
 	if (adapter->iface_type == MWIFIEX_USB) {
 		ret = adapter->if_ops.host_to_card(adapter,
 						   priv->usb_port,
-						   skb, NULL);
+						   skb, tx_param);
 	} else {
 		ret = adapter->if_ops.host_to_card(adapter,
 						   MWIFIEX_TYPE_DATA,
diff --git a/drivers/net/wireless/marvell/mwifiex/uap_cmd.c b/drivers/net/wireless/marvell/mwifiex/uap_cmd.c
index 35d8636bdb91..477c29c9f5d9 100644
--- a/drivers/net/wireless/marvell/mwifiex/uap_cmd.c
+++ b/drivers/net/wireless/marvell/mwifiex/uap_cmd.c
@@ -160,7 +160,6 @@ mwifiex_set_ht_params(struct mwifiex_private *priv,
 		      struct cfg80211_ap_settings *params)
 {
 	const u8 *ht_ie;
-	u16 cap_info;
 
 	if (!ISSUPP_11NENABLED(priv->adapter->fw_cap_info))
 		return;
@@ -170,27 +169,6 @@ mwifiex_set_ht_params(struct mwifiex_private *priv,
 	if (ht_ie) {
 		memcpy(&bss_cfg->ht_cap, ht_ie + 2,
 		       sizeof(struct ieee80211_ht_cap));
-		cap_info = le16_to_cpu(bss_cfg->ht_cap.cap_info);
-		memset(&bss_cfg->ht_cap.mcs, 0,
-		       priv->adapter->number_of_antenna);
-		switch (GET_RXSTBC(cap_info)) {
-		case MWIFIEX_RX_STBC1:
-			/* HT_CAP 1X1 mode */
-			bss_cfg->ht_cap.mcs.rx_mask[0] = 0xff;
-			break;
-		case MWIFIEX_RX_STBC12:	/* fall through */
-		case MWIFIEX_RX_STBC123:
-			/* HT_CAP 2X2 mode */
-			bss_cfg->ht_cap.mcs.rx_mask[0] = 0xff;
-			bss_cfg->ht_cap.mcs.rx_mask[1] = 0xff;
-			break;
-		default:
-			mwifiex_dbg(priv->adapter, WARN,
-				    "Unsupported RX-STBC, default to 2x2\n");
-			bss_cfg->ht_cap.mcs.rx_mask[0] = 0xff;
-			bss_cfg->ht_cap.mcs.rx_mask[1] = 0xff;
-			break;
-		}
 		priv->ap_11n_enabled = 1;
 	} else {
 		memset(&bss_cfg->ht_cap, 0, sizeof(struct ieee80211_ht_cap));
diff --git a/drivers/net/wireless/marvell/mwifiex/uap_event.c b/drivers/net/wireless/marvell/mwifiex/uap_event.c
index e10b2a52e78f..e8c8728db15a 100644
--- a/drivers/net/wireless/marvell/mwifiex/uap_event.c
+++ b/drivers/net/wireless/marvell/mwifiex/uap_event.c
@@ -312,6 +312,17 @@ int mwifiex_process_uap_event(struct mwifiex_private *priv)
 					    adapter->event_skb->len -
 					    sizeof(eventcause));
 		break;
+
+	case EVENT_REMAIN_ON_CHAN_EXPIRED:
+		mwifiex_dbg(adapter, EVENT,
+			    "event: uap: Remain on channel expired\n");
+		cfg80211_remain_on_channel_expired(&priv->wdev,
+						   priv->roc_cfg.cookie,
+						   &priv->roc_cfg.chan,
+						   GFP_ATOMIC);
+		memset(&priv->roc_cfg, 0x00, sizeof(struct mwifiex_roc_cfg));
+		break;
+
 	default:
 		mwifiex_dbg(adapter, EVENT,
 			    "event: unknown event id: %#x\n", eventcause);
diff --git a/drivers/net/wireless/marvell/mwifiex/uap_txrx.c b/drivers/net/wireless/marvell/mwifiex/uap_txrx.c
index bf5660eb27d3..1e6a62c69ac5 100644
--- a/drivers/net/wireless/marvell/mwifiex/uap_txrx.c
+++ b/drivers/net/wireless/marvell/mwifiex/uap_txrx.c
@@ -468,8 +468,7 @@ void *mwifiex_process_uap_txpd(struct mwifiex_private *priv,
 	struct mwifiex_txinfo *tx_info = MWIFIEX_SKB_TXCB(skb);
 	int pad;
 	u16 pkt_type, pkt_offset;
-	int hroom = (priv->adapter->iface_type == MWIFIEX_USB) ? 0 :
-		       INTF_HEADER_LEN;
+	int hroom = adapter->intf_hdr_len;
 
 	if (!skb->len) {
 		mwifiex_dbg(adapter, ERROR,
@@ -521,7 +520,7 @@ void *mwifiex_process_uap_txpd(struct mwifiex_private *priv,
 
 	txpd->tx_pkt_offset = cpu_to_le16(pkt_offset);
 
-	/* make space for INTF_HEADER_LEN */
+	/* make space for adapter->intf_hdr_len */
 	skb_push(skb, hroom);
 
 	if (!txpd->tx_control)
diff --git a/drivers/net/wireless/marvell/mwifiex/usb.c b/drivers/net/wireless/marvell/mwifiex/usb.c
index 2f7705c50161..cb1753e43ef4 100644
--- a/drivers/net/wireless/marvell/mwifiex/usb.c
+++ b/drivers/net/wireless/marvell/mwifiex/usb.c
@@ -363,6 +363,7 @@ static void mwifiex_usb_free(struct usb_card_rec *card)
 	for (i = 0; i < MWIFIEX_TX_DATA_PORT; i++) {
 		port = &card->port[i];
 		for (j = 0; j < MWIFIEX_TX_DATA_URB; j++) {
+			usb_kill_urb(port->tx_data_list[j].urb);
 			usb_free_urb(port->tx_data_list[j].urb);
 			port->tx_data_list[j].urb = NULL;
 		}
@@ -424,7 +425,8 @@ static int mwifiex_usb_probe(struct usb_interface *intf,
 	card->intf = intf;
 
 	pr_debug("info: bcdUSB=%#x Device Class=%#x SubClass=%#x Protocol=%#x\n",
-		 udev->descriptor.bcdUSB, udev->descriptor.bDeviceClass,
+		 le16_to_cpu(udev->descriptor.bcdUSB),
+		 udev->descriptor.bDeviceClass,
 		 udev->descriptor.bDeviceSubClass,
 		 udev->descriptor.bDeviceProtocol);
 
@@ -661,75 +663,6 @@ static struct usb_driver mwifiex_usb_driver = {
 	.soft_unbind = 1,
 };
 
-static int mwifiex_usb_tx_init(struct mwifiex_adapter *adapter)
-{
-	struct usb_card_rec *card = (struct usb_card_rec *)adapter->card;
-	struct usb_tx_data_port *port;
-	int i, j;
-
-	card->tx_cmd.adapter = adapter;
-	card->tx_cmd.ep = card->tx_cmd_ep;
-
-	card->tx_cmd.urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!card->tx_cmd.urb)
-		return -ENOMEM;
-
-	for (i = 0; i < MWIFIEX_TX_DATA_PORT; i++) {
-		port = &card->port[i];
-		if (!port->tx_data_ep)
-			continue;
-		port->tx_data_ix = 0;
-		if (port->tx_data_ep == MWIFIEX_USB_EP_DATA)
-			port->block_status = false;
-		else
-			port->block_status = true;
-		for (j = 0; j < MWIFIEX_TX_DATA_URB; j++) {
-			port->tx_data_list[j].adapter = adapter;
-			port->tx_data_list[j].ep = port->tx_data_ep;
-			port->tx_data_list[j].urb =
-					usb_alloc_urb(0, GFP_KERNEL);
-			if (!port->tx_data_list[j].urb)
-				return -ENOMEM;
-		}
-	}
-
-	return 0;
-}
-
-static int mwifiex_usb_rx_init(struct mwifiex_adapter *adapter)
-{
-	struct usb_card_rec *card = (struct usb_card_rec *)adapter->card;
-	int i;
-
-	card->rx_cmd.adapter = adapter;
-	card->rx_cmd.ep = card->rx_cmd_ep;
-
-	card->rx_cmd.urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!card->rx_cmd.urb)
-		return -ENOMEM;
-
-	card->rx_cmd.skb = dev_alloc_skb(MWIFIEX_RX_CMD_BUF_SIZE);
-	if (!card->rx_cmd.skb)
-		return -ENOMEM;
-
-	if (mwifiex_usb_submit_rx_urb(&card->rx_cmd, MWIFIEX_RX_CMD_BUF_SIZE))
-		return -1;
-
-	for (i = 0; i < MWIFIEX_RX_DATA_URB; i++) {
-		card->rx_data_list[i].adapter = adapter;
-		card->rx_data_list[i].ep = card->rx_data_ep;
-
-		card->rx_data_list[i].urb = usb_alloc_urb(0, GFP_KERNEL);
-		if (!card->rx_data_list[i].urb)
-			return -1;
-		if (mwifiex_usb_submit_rx_urb(&card->rx_data_list[i],
-					      MWIFIEX_RX_DATA_BUF_SIZE))
-			return -1;
-	}
-
-	return 0;
-}
-
 static int mwifiex_write_data_sync(struct mwifiex_adapter *adapter, u8 *pbuf,
 				   u32 *len, u8 ep, u32 timeout)
 {
@@ -845,6 +778,364 @@ static inline u8 mwifiex_usb_data_sent(struct mwifiex_adapter *adapter)
 	return true;
 }
 
+static int mwifiex_usb_construct_send_urb(struct mwifiex_adapter *adapter,
+					  struct usb_tx_data_port *port, u8 ep,
+					  struct urb_context *context,
+					  struct sk_buff *skb_send)
+{
+	struct usb_card_rec *card = adapter->card;
+	int ret = -EINPROGRESS;
+	struct urb *tx_urb;
+
+	context->adapter = adapter;
+	context->ep = ep;
+	context->skb = skb_send;
+	tx_urb = context->urb;
+
+	if (ep == card->tx_cmd_ep &&
+	    card->tx_cmd_ep_type == USB_ENDPOINT_XFER_INT)
+		usb_fill_int_urb(tx_urb, card->udev,
+				 usb_sndintpipe(card->udev, ep), skb_send->data,
+				 skb_send->len, mwifiex_usb_tx_complete,
+				 (void *)context, card->tx_cmd_interval);
+	else
+		usb_fill_bulk_urb(tx_urb, card->udev,
+				  usb_sndbulkpipe(card->udev, ep),
+				  skb_send->data, skb_send->len,
+				  mwifiex_usb_tx_complete, (void *)context);
+
+	tx_urb->transfer_flags |= URB_ZERO_PACKET;
+
+	if (ep == card->tx_cmd_ep)
+		atomic_inc(&card->tx_cmd_urb_pending);
+	else
+		atomic_inc(&port->tx_data_urb_pending);
+
+	if (ep != card->tx_cmd_ep &&
+	    atomic_read(&port->tx_data_urb_pending) ==
+					MWIFIEX_TX_DATA_URB) {
+		port->block_status = true;
+		adapter->data_sent = mwifiex_usb_data_sent(adapter);
+		ret = -ENOSR;
+	}
+
+	if (usb_submit_urb(tx_urb, GFP_ATOMIC)) {
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: usb_submit_urb failed\n", __func__);
+		if (ep == card->tx_cmd_ep) {
+			atomic_dec(&card->tx_cmd_urb_pending);
+		} else {
+			atomic_dec(&port->tx_data_urb_pending);
+			port->block_status = false;
+			adapter->data_sent = false;
+			if (port->tx_data_ix)
+				port->tx_data_ix--;
+			else
+				port->tx_data_ix = MWIFIEX_TX_DATA_URB;
+		}
+		ret = -1;
+	}
+
+	return ret;
+}
+
+static int mwifiex_usb_prepare_tx_aggr_skb(struct mwifiex_adapter *adapter,
+					   struct usb_tx_data_port *port,
+					   struct sk_buff **skb_send)
+{
+	struct sk_buff *skb_aggr, *skb_tmp;
+	u8 *payload, pad;
+	u16 align = adapter->bus_aggr.tx_aggr_align;
+	struct mwifiex_txinfo *tx_info = NULL;
+	bool is_txinfo_set = false;
+
+	/* Packets in aggr_list will be send in either skb_aggr or
+	 * write complete, delete the tx_aggr timer
+	 */
+	if (port->tx_aggr.timer_cnxt.is_hold_timer_set) {
+		del_timer(&port->tx_aggr.timer_cnxt.hold_timer);
+		port->tx_aggr.timer_cnxt.is_hold_timer_set = false;
+		port->tx_aggr.timer_cnxt.hold_tmo_msecs = 0;
+	}
+
+	skb_aggr = mwifiex_alloc_dma_align_buf(port->tx_aggr.aggr_len,
+					       GFP_ATOMIC);
+	if (!skb_aggr) {
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: alloc skb_aggr failed\n", __func__);
+
+		while ((skb_tmp = skb_dequeue(&port->tx_aggr.aggr_list)))
+			mwifiex_write_data_complete(adapter, skb_tmp, 0, -1);
+
+		port->tx_aggr.aggr_num = 0;
+		port->tx_aggr.aggr_len = 0;
+		return -EBUSY;
+	}
+
+	tx_info = MWIFIEX_SKB_TXCB(skb_aggr);
+	memset(tx_info, 0, sizeof(*tx_info));
+
+	while ((skb_tmp = skb_dequeue(&port->tx_aggr.aggr_list))) {
+		/* padding for aligning next packet header*/
+		pad = (align - (skb_tmp->len & (align - 1))) % align;
+		payload = skb_put(skb_aggr, skb_tmp->len + pad);
+		memcpy(payload, skb_tmp->data, skb_tmp->len);
+		if (skb_queue_empty(&port->tx_aggr.aggr_list)) {
+			/* do not padding for last packet*/
+			*(u16 *)payload = cpu_to_le16(skb_tmp->len);
+			*(u16 *)&payload[2] =
+				cpu_to_le16(MWIFIEX_TYPE_AGGR_DATA_V2 | 0x80);
+			skb_trim(skb_aggr, skb_aggr->len - pad);
+		} else {
+			/* add aggregation interface header */
+			*(u16 *)payload = cpu_to_le16(skb_tmp->len + pad);
+			*(u16 *)&payload[2] =
+				cpu_to_le16(MWIFIEX_TYPE_AGGR_DATA_V2);
+		}
+
+		if (!is_txinfo_set) {
+			tx_info->bss_num = MWIFIEX_SKB_TXCB(skb_tmp)->bss_num;
+			tx_info->bss_type = MWIFIEX_SKB_TXCB(skb_tmp)->bss_type;
+			is_txinfo_set = true;
+		}
+
+		port->tx_aggr.aggr_num--;
+		port->tx_aggr.aggr_len -= (skb_tmp->len + pad);
+		mwifiex_write_data_complete(adapter, skb_tmp, 0, 0);
+	}
+
+	tx_info->pkt_len = skb_aggr->len -
+			(sizeof(struct txpd) + adapter->intf_hdr_len);
+	tx_info->flags |= MWIFIEX_BUF_FLAG_AGGR_PKT;
+
+	port->tx_aggr.aggr_num = 0;
+	port->tx_aggr.aggr_len = 0;
+	*skb_send = skb_aggr;
+
+	return 0;
+}
+
+/* This function prepare data packet to be send under usb tx aggregation
+ * protocol, check current usb aggregation status, link packet to aggrgation
+ * list if possible, work flow as below:
+ * (1) if only 1 packet available, add usb tx aggregation header and send.
+ * (2) if packet is able to aggregated, link it to current aggregation list.
+ * (3) if packet is not able to aggregated, aggregate and send exist packets
+ *     in aggrgation list. Then, link packet in the list if there is more
+ *     packet in transmit queue, otherwise try to transmit single packet.
+ */
+static int mwifiex_usb_aggr_tx_data(struct mwifiex_adapter *adapter, u8 ep,
+				    struct sk_buff *skb,
+				    struct mwifiex_tx_param *tx_param,
+				    struct usb_tx_data_port *port)
+{
+	u8 *payload, pad;
+	u16 align = adapter->bus_aggr.tx_aggr_align;
+	struct sk_buff *skb_send = NULL;
+	struct urb_context *context = NULL;
+	struct txpd *local_tx_pd =
+		(struct txpd *)((u8 *)skb->data + adapter->intf_hdr_len);
+	u8 f_send_aggr_buf = 0;
+	u8 f_send_cur_buf = 0;
+	u8 f_precopy_cur_buf = 0;
+	u8 f_postcopy_cur_buf = 0;
+	u32 timeout;
+	int ret;
+
+	/* padding to ensure each packet alginment */
+	pad = (align - (skb->len & (align - 1))) % align;
+
+	if (tx_param && tx_param->next_pkt_len) {
+		/* next packet available in tx queue*/
+		if (port->tx_aggr.aggr_len + skb->len + pad >
+		    adapter->bus_aggr.tx_aggr_max_size) {
+			f_send_aggr_buf = 1;
+			f_postcopy_cur_buf = 1;
+		} else {
+			/* current packet could be aggregated*/
+			f_precopy_cur_buf = 1;
+
+			if (port->tx_aggr.aggr_len + skb->len + pad +
+			    tx_param->next_pkt_len >
+			    adapter->bus_aggr.tx_aggr_max_size ||
+			    port->tx_aggr.aggr_num + 2 >
+			    adapter->bus_aggr.tx_aggr_max_num) {
+			    /* next packet could not be aggregated
+			     * send current aggregation buffer
+			     */
+				f_send_aggr_buf = 1;
+			}
+		}
+	} else {
+		/* last packet in tx queue */
+		if (port->tx_aggr.aggr_num > 0) {
+			/* pending packets in aggregation buffer*/
+			if (port->tx_aggr.aggr_len + skb->len + pad >
+			    adapter->bus_aggr.tx_aggr_max_size) {
+				/* current packet not be able to aggregated,
+				 * send aggr buffer first, then send packet.
+				 */
+				f_send_cur_buf = 1;
+			} else {
+				/* last packet, Aggregation and send */
+				f_precopy_cur_buf = 1;
+			}
+
+			f_send_aggr_buf = 1;
+		} else {
+			/* no pending packets in aggregation buffer,
+			 * send current packet immediately
+			 */
+			 f_send_cur_buf = 1;
+		}
+	}
+
+	if (local_tx_pd->flags & MWIFIEX_TxPD_POWER_MGMT_NULL_PACKET) {
+		/* Send NULL packet immediately*/
+		if (f_precopy_cur_buf) {
+			if (skb_queue_empty(&port->tx_aggr.aggr_list)) {
+				f_precopy_cur_buf = 0;
+				f_send_aggr_buf = 0;
+				f_send_cur_buf = 1;
+			} else {
+				f_send_aggr_buf = 1;
+			}
+		} else if (f_postcopy_cur_buf) {
+			f_send_cur_buf = 1;
+			f_postcopy_cur_buf = 0;
+		}
+	}
+
+	if (f_precopy_cur_buf) {
+		skb_queue_tail(&port->tx_aggr.aggr_list, skb);
+		port->tx_aggr.aggr_len += (skb->len + pad);
+		port->tx_aggr.aggr_num++;
+		if (f_send_aggr_buf)
+			goto send_aggr_buf;
+
+		/* packet will not been send immediately,
+		 * set a timer to make sure it will be sent under
+		 * strict time limit. Dynamically fit the timeout
+		 * value, according to packets number in aggr_list
+		 */
+		if (!port->tx_aggr.timer_cnxt.is_hold_timer_set) {
+			port->tx_aggr.timer_cnxt.hold_tmo_msecs =
+					MWIFIEX_USB_TX_AGGR_TMO_MIN;
+			timeout =
+				port->tx_aggr.timer_cnxt.hold_tmo_msecs;
+			mod_timer(&port->tx_aggr.timer_cnxt.hold_timer,
+				  jiffies + msecs_to_jiffies(timeout));
+			port->tx_aggr.timer_cnxt.is_hold_timer_set = true;
+		} else {
+			if (port->tx_aggr.timer_cnxt.hold_tmo_msecs <
+			    MWIFIEX_USB_TX_AGGR_TMO_MAX) {
+				/* Dyanmic fit timeout */
+				timeout =
+				++port->tx_aggr.timer_cnxt.hold_tmo_msecs;
+				mod_timer(&port->tx_aggr.timer_cnxt.hold_timer,
+					  jiffies + msecs_to_jiffies(timeout));
+			}
+		}
+	}
+
+send_aggr_buf:
+	if (f_send_aggr_buf) {
+		ret = mwifiex_usb_prepare_tx_aggr_skb(adapter, port, &skb_send);
+		if (!ret) {
+			context = &port->tx_data_list[port->tx_data_ix++];
+			ret = mwifiex_usb_construct_send_urb(adapter, port, ep,
+							     context, skb_send);
+			if (ret == -1)
+				mwifiex_write_data_complete(adapter, skb_send,
+							    0, -1);
+		}
+	}
+
+	if (f_send_cur_buf) {
+		if (f_send_aggr_buf) {
+			if (atomic_read(&port->tx_data_urb_pending) >=
+			    MWIFIEX_TX_DATA_URB) {
+				port->block_status = true;
+				adapter->data_sent =
+					mwifiex_usb_data_sent(adapter);
+				/* no available urb, postcopy packet*/
+				f_postcopy_cur_buf = 1;
+				goto postcopy_cur_buf;
+			}
+
+			if (port->tx_data_ix >= MWIFIEX_TX_DATA_URB)
+				port->tx_data_ix = 0;
+		}
+
+		payload = skb->data;
+		*(u16 *)&payload[2] =
+			cpu_to_le16(MWIFIEX_TYPE_AGGR_DATA_V2 | 0x80);
+		*(u16 *)payload = cpu_to_le16(skb->len);
+		skb_send = skb;
+		context = &port->tx_data_list[port->tx_data_ix++];
+		return mwifiex_usb_construct_send_urb(adapter, port, ep,
+						      context, skb_send);
+	}
+
+postcopy_cur_buf:
+	if (f_postcopy_cur_buf) {
+		skb_queue_tail(&port->tx_aggr.aggr_list, skb);
+		port->tx_aggr.aggr_len += (skb->len + pad);
+		port->tx_aggr.aggr_num++;
+		/* New aggregation begin, start timer */
+		if (!port->tx_aggr.timer_cnxt.is_hold_timer_set) {
+			port->tx_aggr.timer_cnxt.hold_tmo_msecs =
+					MWIFIEX_USB_TX_AGGR_TMO_MIN;
+			timeout = port->tx_aggr.timer_cnxt.hold_tmo_msecs;
+			mod_timer(&port->tx_aggr.timer_cnxt.hold_timer,
+				  jiffies + msecs_to_jiffies(timeout));
+			port->tx_aggr.timer_cnxt.is_hold_timer_set = true;
+		}
+	}
+
+	return -EINPROGRESS;
+}
+
+static void mwifiex_usb_tx_aggr_tmo(unsigned long context)
+{
+	struct urb_context *urb_cnxt = NULL;
+	struct sk_buff *skb_send = NULL;
+	struct tx_aggr_tmr_cnxt *timer_context =
+		(struct tx_aggr_tmr_cnxt *)context;
+	struct mwifiex_adapter *adapter = timer_context->adapter;
+	struct usb_tx_data_port *port = timer_context->port;
+	unsigned long flags;
+	int err = 0;
+
+	spin_lock_irqsave(&port->tx_aggr_lock, flags);
+	err = mwifiex_usb_prepare_tx_aggr_skb(adapter, port, &skb_send);
+	if (err) {
+		mwifiex_dbg(adapter, ERROR,
+			    "prepare tx aggr skb failed, err=%d\n", err);
+		return;
+	}
+
+	if (atomic_read(&port->tx_data_urb_pending) >=
+	    MWIFIEX_TX_DATA_URB) {
+		port->block_status = true;
+		adapter->data_sent =
+			mwifiex_usb_data_sent(adapter);
+		err = -1;
+		goto done;
+	}
+
+	if (port->tx_data_ix >= MWIFIEX_TX_DATA_URB)
+		port->tx_data_ix = 0;
+
+	urb_cnxt = &port->tx_data_list[port->tx_data_ix++];
+	err = mwifiex_usb_construct_send_urb(adapter, port, port->tx_data_ep,
+					     urb_cnxt, skb_send);
+done:
+	if (err == -1)
+		mwifiex_write_data_complete(adapter, skb_send, 0, -1);
+	spin_unlock_irqrestore(&port->tx_aggr_lock, flags);
+}
+
 /* This function write a command/data packet to card. */
 static int mwifiex_usb_host_to_card(struct mwifiex_adapter *adapter, u8 ep,
 				    struct sk_buff *skb,
@@ -853,9 +1144,8 @@ static int mwifiex_usb_host_to_card(struct mwifiex_adapter *adapter, u8 ep,
 	struct usb_card_rec *card = adapter->card;
 	struct urb_context *context = NULL;
 	struct usb_tx_data_port *port = NULL;
-	u8 *data = (u8 *)skb->data;
-	struct urb *tx_urb;
-	int idx, ret = -EINPROGRESS;
+	unsigned long flags;
+	int idx, ret;
 
 	if (adapter->is_suspended) {
 		mwifiex_dbg(adapter, ERROR,
@@ -873,6 +1163,7 @@ static int mwifiex_usb_host_to_card(struct mwifiex_adapter *adapter, u8 ep,
 	if (ep == card->tx_cmd_ep) {
 		context = &card->tx_cmd;
 	} else {
+		/* get the data port structure for endpoint */
 		for (idx = 0; idx < MWIFIEX_TX_DATA_PORT; idx++) {
 			if (ep == card->port[idx].tx_data_ep) {
 				port = &card->port[idx];
@@ -885,67 +1176,105 @@ static int mwifiex_usb_host_to_card(struct mwifiex_adapter *adapter, u8 ep,
 				}
 				if (port->tx_data_ix >= MWIFIEX_TX_DATA_URB)
 					port->tx_data_ix = 0;
-				context =
-					&port->tx_data_list[port->tx_data_ix++];
 				break;
 			}
 		}
+
 		if (!port) {
 			mwifiex_dbg(adapter, ERROR, "Wrong usb tx data port\n");
 			return -1;
 		}
-	}
 
-	context->adapter = adapter;
-	context->ep = ep;
-	context->skb = skb;
-	tx_urb = context->urb;
+		if (adapter->bus_aggr.enable) {
+			spin_lock_irqsave(&port->tx_aggr_lock, flags);
+			ret =  mwifiex_usb_aggr_tx_data(adapter, ep, skb,
+							tx_param, port);
+			spin_unlock_irqrestore(&port->tx_aggr_lock, flags);
+			return ret;
+		}
 
-	if (ep == card->tx_cmd_ep &&
-	    card->tx_cmd_ep_type == USB_ENDPOINT_XFER_INT)
-		usb_fill_int_urb(tx_urb, card->udev,
-				 usb_sndintpipe(card->udev, ep), data,
-				 skb->len, mwifiex_usb_tx_complete,
-				 (void *)context, card->tx_cmd_interval);
-	else
-		usb_fill_bulk_urb(tx_urb, card->udev,
-				  usb_sndbulkpipe(card->udev, ep), data,
-				  skb->len, mwifiex_usb_tx_complete,
-				  (void *)context);
+		context = &port->tx_data_list[port->tx_data_ix++];
+	}
 
-	tx_urb->transfer_flags |= URB_ZERO_PACKET;
+	return mwifiex_usb_construct_send_urb(adapter, port, ep, context, skb);
+}
 
-	if (ep == card->tx_cmd_ep)
-		atomic_inc(&card->tx_cmd_urb_pending);
-	else
-		atomic_inc(&port->tx_data_urb_pending);
+static int mwifiex_usb_tx_init(struct mwifiex_adapter *adapter)
+{
+	struct usb_card_rec *card = (struct usb_card_rec *)adapter->card;
+	struct usb_tx_data_port *port;
+	int i, j;
 
-	if (ep != card->tx_cmd_ep &&
-	    atomic_read(&port->tx_data_urb_pending) ==
-					MWIFIEX_TX_DATA_URB) {
-		port->block_status = true;
-		adapter->data_sent = mwifiex_usb_data_sent(adapter);
-		ret = -ENOSR;
-	}
+	card->tx_cmd.adapter = adapter;
+	card->tx_cmd.ep = card->tx_cmd_ep;
 
-	if (usb_submit_urb(tx_urb, GFP_ATOMIC)) {
-		mwifiex_dbg(adapter, ERROR,
-			    "%s: usb_submit_urb failed\n", __func__);
-		if (ep == card->tx_cmd_ep) {
-			atomic_dec(&card->tx_cmd_urb_pending);
-		} else {
-			atomic_dec(&port->tx_data_urb_pending);
+	card->tx_cmd.urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!card->tx_cmd.urb)
+		return -ENOMEM;
+
+	for (i = 0; i < MWIFIEX_TX_DATA_PORT; i++) {
+		port = &card->port[i];
+		if (!port->tx_data_ep)
+			continue;
+		port->tx_data_ix = 0;
+		skb_queue_head_init(&port->tx_aggr.aggr_list);
+		if (port->tx_data_ep == MWIFIEX_USB_EP_DATA)
 			port->block_status = false;
-			adapter->data_sent = false;
-			if (port->tx_data_ix)
-				port->tx_data_ix--;
-			else
-				port->tx_data_ix = MWIFIEX_TX_DATA_URB;
+		else
+			port->block_status = true;
+		for (j = 0; j < MWIFIEX_TX_DATA_URB; j++) {
+			port->tx_data_list[j].adapter = adapter;
+			port->tx_data_list[j].ep = port->tx_data_ep;
+			port->tx_data_list[j].urb =
+					usb_alloc_urb(0, GFP_KERNEL);
+			if (!port->tx_data_list[j].urb)
+				return -ENOMEM;
 		}
-		ret = -1;
+
+		port->tx_aggr.timer_cnxt.adapter = adapter;
+		port->tx_aggr.timer_cnxt.port = port;
+		port->tx_aggr.timer_cnxt.is_hold_timer_set = false;
+		port->tx_aggr.timer_cnxt.hold_tmo_msecs = 0;
+		setup_timer(&port->tx_aggr.timer_cnxt.hold_timer,
+			    mwifiex_usb_tx_aggr_tmo,
+			    (unsigned long)&port->tx_aggr.timer_cnxt);
 	}
 
-	return ret;
+	return 0;
+}
+
+static int mwifiex_usb_rx_init(struct mwifiex_adapter *adapter)
+{
+	struct usb_card_rec *card = (struct usb_card_rec *)adapter->card;
+	int i;
+
+	card->rx_cmd.adapter = adapter;
+	card->rx_cmd.ep = card->rx_cmd_ep;
+
+	card->rx_cmd.urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!card->rx_cmd.urb)
+		return -ENOMEM;
+
+	card->rx_cmd.skb = dev_alloc_skb(MWIFIEX_RX_CMD_BUF_SIZE);
+	if (!card->rx_cmd.skb)
+		return -ENOMEM;
+
+	if (mwifiex_usb_submit_rx_urb(&card->rx_cmd, MWIFIEX_RX_CMD_BUF_SIZE))
+		return -1;
+
+	for (i = 0; i < MWIFIEX_RX_DATA_URB; i++) {
+		card->rx_data_list[i].adapter = adapter;
+		card->rx_data_list[i].ep = card->rx_data_ep;
+
+		card->rx_data_list[i].urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!card->rx_data_list[i].urb)
+			return -1;
+		if (mwifiex_usb_submit_rx_urb(&card->rx_data_list[i],
+					      MWIFIEX_RX_DATA_BUF_SIZE))
+			return -1;
+	}
+
+	return 0;
 }
 
 /* This function register usb device and initialize parameter. */
@@ -988,10 +1317,32 @@ static int mwifiex_register_dev(struct mwifiex_adapter *adapter)
 	return 0;
 }
 
+static void mwifiex_usb_cleanup_tx_aggr(struct mwifiex_adapter *adapter)
+{
+	struct usb_card_rec *card = (struct usb_card_rec *)adapter->card;
+	struct usb_tx_data_port *port;
+	struct sk_buff *skb_tmp;
+	int idx;
+
+	for (idx = 0; idx < MWIFIEX_TX_DATA_PORT; idx++) {
+		port = &card->port[idx];
+		if (adapter->bus_aggr.enable)
+			while ((skb_tmp =
+				skb_dequeue(&port->tx_aggr.aggr_list)))
+				mwifiex_write_data_complete(adapter, skb_tmp,
+							    0, -1);
+		del_timer_sync(&port->tx_aggr.timer_cnxt.hold_timer);
+		port->tx_aggr.timer_cnxt.is_hold_timer_set = false;
+		port->tx_aggr.timer_cnxt.hold_tmo_msecs = 0;
+	}
+}
+
 static void mwifiex_unregister_dev(struct mwifiex_adapter *adapter)
 {
 	struct usb_card_rec *card = (struct usb_card_rec *)adapter->card;
 
+	mwifiex_usb_cleanup_tx_aggr(adapter);
+
 	card->adapter = NULL;
 }
 
diff --git a/drivers/net/wireless/marvell/mwifiex/usb.h b/drivers/net/wireless/marvell/mwifiex/usb.h
index e36bd63172ff..37abd228a84f 100644
--- a/drivers/net/wireless/marvell/mwifiex/usb.h
+++ b/drivers/net/wireless/marvell/mwifiex/usb.h
@@ -64,12 +64,35 @@ struct urb_context {
 	u8 ep;
 };
 
+#define MWIFIEX_USB_TX_AGGR_TMO_MIN	1
+#define MWIFIEX_USB_TX_AGGR_TMO_MAX	4
+
+struct tx_aggr_tmr_cnxt {
+	struct mwifiex_adapter *adapter;
+	struct usb_tx_data_port *port;
+	struct timer_list hold_timer;
+	bool is_hold_timer_set;
+	u32 hold_tmo_msecs;
+};
+
+struct usb_tx_aggr {
+	struct sk_buff_head aggr_list;
+	int aggr_len;
+	int aggr_num;
+	struct tx_aggr_tmr_cnxt timer_cnxt;
+};
+
 struct usb_tx_data_port {
 	u8 tx_data_ep;
 	u8 block_status;
 	atomic_t tx_data_urb_pending;
 	int tx_data_ix;
 	struct urb_context tx_data_list[MWIFIEX_TX_DATA_URB];
+	/* usb tx aggregation*/
+	struct usb_tx_aggr tx_aggr;
+	struct sk_buff *skb_aggr[MWIFIEX_TX_DATA_URB];
+	/* lock for protect tx aggregation data path*/
+	spinlock_t tx_aggr_lock;
 };
 
 struct usb_card_rec {
diff --git a/drivers/net/wireless/marvell/mwifiex/wmm.c b/drivers/net/wireless/marvell/mwifiex/wmm.c
index e4ff3b973850..0edd26881321 100644
--- a/drivers/net/wireless/marvell/mwifiex/wmm.c
+++ b/drivers/net/wireless/marvell/mwifiex/wmm.c
@@ -868,12 +868,8 @@ mwifiex_wmm_add_buf_txqueue(struct mwifiex_private *priv,
 			return;
 		default:
 			list_head = priv->wmm.tid_tbl_ptr[tid_down].ra_list;
-			if (!list_empty(&list_head))
-				ra_list = list_first_entry(
-					&list_head, struct mwifiex_ra_list_tbl,
-					list);
-			else
-				ra_list = NULL;
+			ra_list = list_first_entry_or_null(&list_head,
+					struct mwifiex_ra_list_tbl, list);
 			break;
 		}
 	} else {
@@ -1363,13 +1359,13 @@ mwifiex_send_processed_packet(struct mwifiex_private *priv,
 
 	spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock, ra_list_flags);
 
+	tx_param.next_pkt_len =
+		((skb_next) ? skb_next->len +
+		 sizeof(struct txpd) : 0);
 	if (adapter->iface_type == MWIFIEX_USB) {
 		ret = adapter->if_ops.host_to_card(adapter, priv->usb_port,
-						   skb, NULL);
+						   skb, &tx_param);
 	} else {
-		tx_param.next_pkt_len =
-			((skb_next) ? skb_next->len +
-			 sizeof(struct txpd) : 0);
 		ret = adapter->if_ops.host_to_card(adapter, MWIFIEX_TYPE_DATA,
 						   skb, &tx_param);
 	}
diff --git a/drivers/net/wireless/mediatek/mt7601u/dma.c b/drivers/net/wireless/mediatek/mt7601u/dma.c
index a8bc064bc14f..660267b359e4 100644
--- a/drivers/net/wireless/mediatek/mt7601u/dma.c
+++ b/drivers/net/wireless/mediatek/mt7601u/dma.c
@@ -52,7 +52,7 @@ mt7601u_rx_skb_from_seg(struct mt7601u_dev *dev, struct mt7601u_rxwi *rxwi,
 		goto bad_frame;
 
 	if (rxwi->rxinfo & cpu_to_le32(MT_RXINFO_L2PAD)) {
-		memcpy(skb_put(skb, hdr_len), data, hdr_len);
+		skb_put_data(skb, data, hdr_len);
 
 		data += hdr_len + 2;
 		true_len -= hdr_len;
@@ -63,7 +63,7 @@ mt7601u_rx_skb_from_seg(struct mt7601u_dev *dev, struct mt7601u_rxwi *rxwi,
 	copy = (true_len <= skb_tailroom(skb)) ? true_len : hdr_len + 8;
 	frag = true_len - copy;
 
-	memcpy(skb_put(skb, copy), data, copy);
+	skb_put_data(skb, data, copy);
 	data += copy;
 
 	if (frag) {
diff --git a/drivers/net/wireless/mediatek/mt7601u/mcu.c b/drivers/net/wireless/mediatek/mt7601u/mcu.c
index a9f5f398b2f8..65a8004418ea 100644
--- a/drivers/net/wireless/mediatek/mt7601u/mcu.c
+++ b/drivers/net/wireless/mediatek/mt7601u/mcu.c
@@ -68,7 +68,7 @@ mt7601u_mcu_msg_alloc(struct mt7601u_dev *dev, const void *data, int len)
 	skb = alloc_skb(len + MT_DMA_HDR_LEN + 4, GFP_KERNEL);
 	if (skb) {
 		skb_reserve(skb, MT_DMA_HDR_LEN);
-		memcpy(skb_put(skb, len), data, len);
+		skb_put_data(skb, data, len);
 	}
 
 	return skb;
diff --git a/drivers/net/wireless/mediatek/mt7601u/tx.c b/drivers/net/wireless/mediatek/mt7601u/tx.c
index ad77bec1ba0f..3600e911a63e 100644
--- a/drivers/net/wireless/mediatek/mt7601u/tx.c
+++ b/drivers/net/wireless/mediatek/mt7601u/tx.c
@@ -148,7 +148,7 @@ mt7601u_push_txwi(struct mt7601u_dev *dev, struct sk_buff *skb,
 	u16 rate_ctl;
 	u8 nss;
 
-	txwi = (struct mt76_txwi *)skb_push(skb, sizeof(struct mt76_txwi));
+	txwi = skb_push(skb, sizeof(struct mt76_txwi));
 	memset(txwi, 0, sizeof(*txwi));
 
 	if (!wcid->tx_rate_set)
diff --git a/drivers/net/wireless/quantenna/Kconfig b/drivers/net/wireless/quantenna/Kconfig
new file mode 100644
index 000000000000..30943656e989
--- /dev/null
+++ b/drivers/net/wireless/quantenna/Kconfig
@@ -0,0 +1,16 @@
+config WLAN_VENDOR_QUANTENNA
+	bool "Quantenna wireless cards support"
+	default y
+	---help---
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
+
+if WLAN_VENDOR_QUANTENNA
+
+source "drivers/net/wireless/quantenna/qtnfmac/Kconfig"
+
+endif # WLAN_VENDOR_QUANTENNA
diff --git a/drivers/net/wireless/quantenna/Makefile b/drivers/net/wireless/quantenna/Makefile
new file mode 100644
index 000000000000..baebfbde119e
--- /dev/null
+++ b/drivers/net/wireless/quantenna/Makefile
@@ -0,0 +1,6 @@
+#
+# Copyright (c) 2015-2016 Quantenna Communications, Inc.
+# All rights reserved.
+#
+
+obj-$(CONFIG_QTNFMAC)	+= qtnfmac/
diff --git a/drivers/net/wireless/quantenna/qtnfmac/Kconfig b/drivers/net/wireless/quantenna/qtnfmac/Kconfig
new file mode 100644
index 000000000000..025fa6018550
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/Kconfig
@@ -0,0 +1,19 @@
+config QTNFMAC
+	tristate
+	depends on QTNFMAC_PEARL_PCIE
+	default m if QTNFMAC_PEARL_PCIE=m
+	default y if QTNFMAC_PEARL_PCIE=y
+
+config QTNFMAC_PEARL_PCIE
+	tristate "Quantenna QSR10g PCIe support"
+	default n
+	depends on HAS_DMA && PCI && CFG80211
+	select QTNFMAC
+	select FW_LOADER
+	select CRC32
+	---help---
+	  This option adds support for wireless adapters based on Quantenna
+	  802.11ac QSR10g (aka Pearl) FullMAC chipset running over PCIe.
+
+	  If you choose to build it as a module, two modules will be built:
+	  qtnfmac.ko and qtnfmac_pearl_pcie.ko.
diff --git a/drivers/net/wireless/quantenna/qtnfmac/Makefile b/drivers/net/wireless/quantenna/qtnfmac/Makefile
new file mode 100644
index 000000000000..0d618e5e5f5b
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/Makefile
@@ -0,0 +1,31 @@
+#
+# Copyright (c) 2015-2016 Quantenna Communications, Inc.
+# All rights reserved.
+#
+
+ccflags-y += \
+	-Idrivers/net/wireless/quantenna/qtnfmac
+
+obj-$(CONFIG_QTNFMAC) += qtnfmac.o
+qtnfmac-objs += \
+	core.o \
+	commands.o \
+	trans.o \
+	cfg80211.o \
+	event.o \
+	util.o \
+	qlink_util.o
+
+#
+
+obj-$(CONFIG_QTNFMAC_PEARL_PCIE) += qtnfmac_pearl_pcie.o
+
+qtnfmac_pearl_pcie-objs += \
+	shm_ipc.o \
+	pearl/pcie.o
+
+qtnfmac_pearl_pcie-$(CONFIG_DEBUG_FS) += debug.o
+
+#
+
+ccflags-y += -D__CHECK_ENDIAN
diff --git a/drivers/net/wireless/quantenna/qtnfmac/bus.h b/drivers/net/wireless/quantenna/qtnfmac/bus.h
new file mode 100644
index 000000000000..dda05003d522
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/bus.h
@@ -0,0 +1,139 @@
+/*
+ * Copyright (c) 2015 Quantenna Communications
+ *
+ * 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 QTNFMAC_BUS_H
+#define QTNFMAC_BUS_H
+
+#include <linux/netdevice.h>
+#include <linux/workqueue.h>
+
+#define QTNF_MAX_MAC		3
+
+enum qtnf_fw_state {
+	QTNF_FW_STATE_RESET,
+	QTNF_FW_STATE_FW_DNLD_DONE,
+	QTNF_FW_STATE_BOOT_DONE,
+	QTNF_FW_STATE_ACTIVE,
+	QTNF_FW_STATE_DEAD,
+};
+
+struct qtnf_bus;
+
+struct qtnf_bus_ops {
+	/* mgmt methods */
+	int (*preinit)(struct qtnf_bus *);
+	void (*stop)(struct qtnf_bus *);
+
+	/* control path methods */
+	int (*control_tx)(struct qtnf_bus *, struct sk_buff *);
+
+	/* data xfer methods */
+	int (*data_tx)(struct qtnf_bus *, struct sk_buff *);
+	void (*data_tx_timeout)(struct qtnf_bus *, struct net_device *);
+	void (*data_rx_start)(struct qtnf_bus *);
+	void (*data_rx_stop)(struct qtnf_bus *);
+};
+
+struct qtnf_bus {
+	struct device *dev;
+	enum qtnf_fw_state fw_state;
+	u32 chip;
+	u32 chiprev;
+	const struct qtnf_bus_ops *bus_ops;
+	struct qtnf_wmac *mac[QTNF_MAX_MAC];
+	struct qtnf_qlink_transport trans;
+	struct qtnf_hw_info hw_info;
+	char fwname[32];
+	struct napi_struct mux_napi;
+	struct net_device mux_dev;
+	struct completion request_firmware_complete;
+	struct workqueue_struct *workqueue;
+	struct work_struct event_work;
+	struct mutex bus_lock; /* lock during command/event processing */
+	struct dentry *dbg_dir;
+	/* bus private data */
+	char bus_priv[0] __aligned(sizeof(void *));
+};
+
+static inline void *get_bus_priv(struct qtnf_bus *bus)
+{
+	if (WARN(!bus, "qtnfmac: invalid bus pointer"))
+		return NULL;
+
+	return &bus->bus_priv;
+}
+
+/* callback wrappers */
+
+static inline int qtnf_bus_preinit(struct qtnf_bus *bus)
+{
+	if (!bus->bus_ops->preinit)
+		return 0;
+	return bus->bus_ops->preinit(bus);
+}
+
+static inline void qtnf_bus_stop(struct qtnf_bus *bus)
+{
+	if (!bus->bus_ops->stop)
+		return;
+	bus->bus_ops->stop(bus);
+}
+
+static inline int qtnf_bus_data_tx(struct qtnf_bus *bus, struct sk_buff *skb)
+{
+	return bus->bus_ops->data_tx(bus, skb);
+}
+
+static inline void
+qtnf_bus_data_tx_timeout(struct qtnf_bus *bus, struct net_device *ndev)
+{
+	return bus->bus_ops->data_tx_timeout(bus, ndev);
+}
+
+static inline int qtnf_bus_control_tx(struct qtnf_bus *bus, struct sk_buff *skb)
+{
+	return bus->bus_ops->control_tx(bus, skb);
+}
+
+static inline void qtnf_bus_data_rx_start(struct qtnf_bus *bus)
+{
+	return bus->bus_ops->data_rx_start(bus);
+}
+
+static inline void qtnf_bus_data_rx_stop(struct qtnf_bus *bus)
+{
+	return bus->bus_ops->data_rx_stop(bus);
+}
+
+static __always_inline void qtnf_bus_lock(struct qtnf_bus *bus)
+{
+	mutex_lock(&bus->bus_lock);
+}
+
+static __always_inline void qtnf_bus_unlock(struct qtnf_bus *bus)
+{
+	mutex_unlock(&bus->bus_lock);
+}
+
+/* interface functions from common layer */
+
+void qtnf_rx_frame(struct device *dev, struct sk_buff *rxp);
+int qtnf_core_attach(struct qtnf_bus *bus);
+void qtnf_core_detach(struct qtnf_bus *bus);
+void qtnf_txflowblock(struct device *dev, bool state);
+void qtnf_txcomplete(struct device *dev, struct sk_buff *txp, bool success);
+
+#endif /* QTNFMAC_BUS_H */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/cfg80211.c b/drivers/net/wireless/quantenna/qtnfmac/cfg80211.c
new file mode 100644
index 000000000000..e3c090008125
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/cfg80211.c
@@ -0,0 +1,995 @@
+/*
+ * Copyright (c) 2012-2012 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/etherdevice.h>
+#include <linux/vmalloc.h>
+#include <linux/ieee80211.h>
+#include <net/cfg80211.h>
+#include <net/netlink.h>
+
+#include "cfg80211.h"
+#include "commands.h"
+#include "core.h"
+#include "util.h"
+#include "bus.h"
+
+/* Supported rates to be advertised to the cfg80211 */
+static struct ieee80211_rate qtnf_rates_2g[] = {
+	{.bitrate = 10, .hw_value = 2, },
+	{.bitrate = 20, .hw_value = 4, },
+	{.bitrate = 55, .hw_value = 11, },
+	{.bitrate = 110, .hw_value = 22, },
+	{.bitrate = 60, .hw_value = 12, },
+	{.bitrate = 90, .hw_value = 18, },
+	{.bitrate = 120, .hw_value = 24, },
+	{.bitrate = 180, .hw_value = 36, },
+	{.bitrate = 240, .hw_value = 48, },
+	{.bitrate = 360, .hw_value = 72, },
+	{.bitrate = 480, .hw_value = 96, },
+	{.bitrate = 540, .hw_value = 108, },
+};
+
+/* Supported rates to be advertised to the cfg80211 */
+static struct ieee80211_rate qtnf_rates_5g[] = {
+	{.bitrate = 60, .hw_value = 12, },
+	{.bitrate = 90, .hw_value = 18, },
+	{.bitrate = 120, .hw_value = 24, },
+	{.bitrate = 180, .hw_value = 36, },
+	{.bitrate = 240, .hw_value = 48, },
+	{.bitrate = 360, .hw_value = 72, },
+	{.bitrate = 480, .hw_value = 96, },
+	{.bitrate = 540, .hw_value = 108, },
+};
+
+/* Supported crypto cipher suits to be advertised to cfg80211 */
+static const u32 qtnf_cipher_suites[] = {
+	WLAN_CIPHER_SUITE_TKIP,
+	WLAN_CIPHER_SUITE_CCMP,
+	WLAN_CIPHER_SUITE_AES_CMAC,
+};
+
+/* Supported mgmt frame types to be advertised to cfg80211 */
+static const struct ieee80211_txrx_stypes
+qtnf_mgmt_stypes[NUM_NL80211_IFTYPES] = {
+	[NL80211_IFTYPE_STATION] = {
+		.tx = BIT(IEEE80211_STYPE_ACTION >> 4),
+		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+		      BIT(IEEE80211_STYPE_PROBE_REQ >> 4),
+	},
+	[NL80211_IFTYPE_AP] = {
+		.tx = BIT(IEEE80211_STYPE_ACTION >> 4),
+		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+		      BIT(IEEE80211_STYPE_PROBE_REQ >> 4),
+	},
+};
+
+static int
+qtnf_change_virtual_intf(struct wiphy *wiphy,
+			 struct net_device *dev,
+			 enum nl80211_iftype type,
+			 struct vif_params *params)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(dev);
+	u8 *mac_addr;
+	int ret;
+
+	if (params)
+		mac_addr = params->macaddr;
+	else
+		mac_addr = NULL;
+
+	qtnf_scan_done(vif->mac, true);
+
+	ret = qtnf_cmd_send_change_intf_type(vif, type, mac_addr);
+	if (ret) {
+		pr_err("VIF%u.%u: failed to change VIF type: %d\n",
+		       vif->mac->macid, vif->vifid, ret);
+		return ret;
+	}
+
+	vif->wdev.iftype = type;
+	return 0;
+}
+
+int qtnf_del_virtual_intf(struct wiphy *wiphy, struct wireless_dev *wdev)
+{
+	struct net_device *netdev =  wdev->netdev;
+	struct qtnf_vif *vif;
+
+	if (WARN_ON(!netdev))
+		return -EFAULT;
+
+	vif = qtnf_netdev_get_priv(wdev->netdev);
+
+	if (qtnf_cmd_send_del_intf(vif))
+		pr_err("VIF%u.%u: failed to delete VIF\n", vif->mac->macid,
+		       vif->vifid);
+
+	/* Stop data */
+	netif_tx_stop_all_queues(netdev);
+	if (netif_carrier_ok(netdev))
+		netif_carrier_off(netdev);
+
+	if (netdev->reg_state == NETREG_REGISTERED)
+		unregister_netdevice(netdev);
+
+	vif->netdev->ieee80211_ptr = NULL;
+	vif->netdev = NULL;
+	vif->wdev.iftype = NL80211_IFTYPE_UNSPECIFIED;
+	eth_zero_addr(vif->mac_addr);
+
+	return 0;
+}
+
+static struct wireless_dev *qtnf_add_virtual_intf(struct wiphy *wiphy,
+						  const char *name,
+						  unsigned char name_assign_t,
+						  enum nl80211_iftype type,
+						  struct vif_params *params)
+{
+	struct qtnf_wmac *mac;
+	struct qtnf_vif *vif;
+	u8 *mac_addr = NULL;
+
+	mac = wiphy_priv(wiphy);
+
+	if (!mac)
+		return ERR_PTR(-EFAULT);
+
+	switch (type) {
+	case NL80211_IFTYPE_STATION:
+	case NL80211_IFTYPE_AP:
+		vif = qtnf_mac_get_free_vif(mac);
+		if (!vif) {
+			pr_err("MAC%u: no free VIF available\n", mac->macid);
+			return ERR_PTR(-EFAULT);
+		}
+
+		eth_zero_addr(vif->mac_addr);
+		vif->bss_priority = QTNF_DEF_BSS_PRIORITY;
+		vif->wdev.wiphy = wiphy;
+		vif->wdev.iftype = type;
+		vif->sta_state = QTNF_STA_DISCONNECTED;
+		break;
+	default:
+		pr_err("MAC%u: unsupported IF type %d\n", mac->macid, type);
+		return ERR_PTR(-ENOTSUPP);
+	}
+
+	if (params)
+		mac_addr = params->macaddr;
+
+	if (qtnf_cmd_send_add_intf(vif, type, mac_addr)) {
+		pr_err("VIF%u.%u: failed to add VIF\n", mac->macid, vif->vifid);
+		goto err_cmd;
+	}
+
+	if (!is_valid_ether_addr(vif->mac_addr)) {
+		pr_err("VIF%u.%u: FW reported bad MAC: %pM\n",
+		       mac->macid, vif->vifid, vif->mac_addr);
+		goto err_mac;
+	}
+
+	if (qtnf_core_net_attach(mac, vif, name, name_assign_t, type)) {
+		pr_err("VIF%u.%u: failed to attach netdev\n", mac->macid,
+		       vif->vifid);
+		goto err_net;
+	}
+
+	vif->wdev.netdev = vif->netdev;
+	return &vif->wdev;
+
+err_net:
+	vif->netdev = NULL;
+err_mac:
+	qtnf_cmd_send_del_intf(vif);
+err_cmd:
+	vif->wdev.iftype = NL80211_IFTYPE_UNSPECIFIED;
+
+	return ERR_PTR(-EFAULT);
+}
+
+static int qtnf_mgmt_set_appie(struct qtnf_vif *vif,
+			       const struct cfg80211_beacon_data *info)
+{
+	int ret = 0;
+
+	if (!info->beacon_ies || !info->beacon_ies_len) {
+		ret = qtnf_cmd_send_mgmt_set_appie(vif, QLINK_MGMT_FRAME_BEACON,
+						   NULL, 0);
+	} else {
+		ret = qtnf_cmd_send_mgmt_set_appie(vif, QLINK_MGMT_FRAME_BEACON,
+						   info->beacon_ies,
+						   info->beacon_ies_len);
+	}
+
+	if (ret)
+		goto out;
+
+	if (!info->proberesp_ies || !info->proberesp_ies_len) {
+		ret = qtnf_cmd_send_mgmt_set_appie(vif,
+						   QLINK_MGMT_FRAME_PROBE_RESP,
+						   NULL, 0);
+	} else {
+		ret = qtnf_cmd_send_mgmt_set_appie(vif,
+						   QLINK_MGMT_FRAME_PROBE_RESP,
+						   info->proberesp_ies,
+						   info->proberesp_ies_len);
+	}
+
+	if (ret)
+		goto out;
+
+	if (!info->assocresp_ies || !info->assocresp_ies_len) {
+		ret = qtnf_cmd_send_mgmt_set_appie(vif,
+						   QLINK_MGMT_FRAME_ASSOC_RESP,
+						   NULL, 0);
+	} else {
+		ret = qtnf_cmd_send_mgmt_set_appie(vif,
+						   QLINK_MGMT_FRAME_ASSOC_RESP,
+						   info->assocresp_ies,
+						   info->assocresp_ies_len);
+	}
+
+out:
+	return ret;
+}
+
+static int qtnf_change_beacon(struct wiphy *wiphy, struct net_device *dev,
+			      struct cfg80211_beacon_data *info)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(dev);
+
+	if (!(vif->bss_status & QTNF_STATE_AP_START)) {
+		pr_err("VIF%u.%u: not started\n", vif->mac->macid, vif->vifid);
+		return -EFAULT;
+	}
+
+	return qtnf_mgmt_set_appie(vif, info);
+}
+
+static int qtnf_start_ap(struct wiphy *wiphy, struct net_device *dev,
+			 struct cfg80211_ap_settings *settings)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(dev);
+	struct qtnf_bss_config *bss_cfg;
+	int ret;
+
+	bss_cfg = &vif->bss_cfg;
+
+	memset(bss_cfg, 0, sizeof(*bss_cfg));
+
+	bss_cfg->bcn_period = settings->beacon_interval;
+	bss_cfg->dtim = settings->dtim_period;
+	bss_cfg->auth_type = settings->auth_type;
+	bss_cfg->privacy = settings->privacy;
+
+	bss_cfg->ssid_len = settings->ssid_len;
+	memcpy(&bss_cfg->ssid, settings->ssid, bss_cfg->ssid_len);
+
+	memcpy(&bss_cfg->chandef, &settings->chandef,
+	       sizeof(struct cfg80211_chan_def));
+	memcpy(&bss_cfg->crypto, &settings->crypto,
+	       sizeof(struct cfg80211_crypto_settings));
+
+	ret = qtnf_cmd_send_config_ap(vif);
+	if (ret) {
+		pr_err("VIF%u.%u: failed to push config to FW\n",
+		       vif->mac->macid, vif->vifid);
+		goto out;
+	}
+
+	if (!(vif->bss_status & QTNF_STATE_AP_CONFIG)) {
+		pr_err("VIF%u.%u: AP config failed in FW\n", vif->mac->macid,
+		       vif->vifid);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	ret = qtnf_mgmt_set_appie(vif, &settings->beacon);
+	if (ret) {
+		pr_err("VIF%u.%u: failed to add IEs to beacon\n",
+		       vif->mac->macid, vif->vifid);
+		goto out;
+	}
+
+	ret = qtnf_cmd_send_start_ap(vif);
+	if (ret) {
+		pr_err("VIF%u.%u: failed to start AP\n", vif->mac->macid,
+		       vif->vifid);
+		goto out;
+	}
+
+	if (!(vif->bss_status & QTNF_STATE_AP_START)) {
+		pr_err("VIF%u.%u: FW failed to start AP operation\n",
+		       vif->mac->macid, vif->vifid);
+		ret = -EFAULT;
+	}
+
+out:
+	return ret;
+}
+
+static int qtnf_stop_ap(struct wiphy *wiphy, struct net_device *dev)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(dev);
+	int ret;
+
+	ret = qtnf_cmd_send_stop_ap(vif);
+	if (ret) {
+		pr_err("VIF%u.%u: failed to stop AP operation in FW\n",
+		       vif->mac->macid, vif->vifid);
+		vif->bss_status &= ~QTNF_STATE_AP_START;
+		vif->bss_status &= ~QTNF_STATE_AP_CONFIG;
+
+		netif_carrier_off(vif->netdev);
+	}
+
+	return ret;
+}
+
+static int qtnf_set_wiphy_params(struct wiphy *wiphy, u32 changed)
+{
+	struct qtnf_wmac *mac = wiphy_priv(wiphy);
+	struct qtnf_vif *vif;
+	int ret;
+
+	vif = qtnf_mac_get_base_vif(mac);
+	if (!vif) {
+		pr_err("MAC%u: primary VIF is not configured\n", mac->macid);
+		return -EFAULT;
+	}
+
+	if (changed & (WIPHY_PARAM_RETRY_LONG | WIPHY_PARAM_RETRY_SHORT)) {
+		pr_err("MAC%u: can't modify retry params\n", mac->macid);
+		return -EOPNOTSUPP;
+	}
+
+	ret = qtnf_cmd_send_update_phy_params(mac, changed);
+	if (ret)
+		pr_err("MAC%u: failed to update PHY params\n", mac->macid);
+
+	return ret;
+}
+
+static void
+qtnf_mgmt_frame_register(struct wiphy *wiphy, struct wireless_dev *wdev,
+			 u16 frame_type, bool reg)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(wdev->netdev);
+	u16 mgmt_type;
+	u16 new_mask;
+	u16 qlink_frame_type = 0;
+
+	mgmt_type = (frame_type & IEEE80211_FCTL_STYPE) >> 4;
+
+	if (reg)
+		new_mask = vif->mgmt_frames_bitmask | BIT(mgmt_type);
+	else
+		new_mask = vif->mgmt_frames_bitmask & ~BIT(mgmt_type);
+
+	if (new_mask == vif->mgmt_frames_bitmask)
+		return;
+
+	switch (frame_type & IEEE80211_FCTL_STYPE) {
+	case IEEE80211_STYPE_PROBE_REQ:
+		qlink_frame_type = QLINK_MGMT_FRAME_PROBE_REQ;
+		break;
+	case IEEE80211_STYPE_ACTION:
+		qlink_frame_type = QLINK_MGMT_FRAME_ACTION;
+		break;
+	default:
+		pr_warn("VIF%u.%u: unsupported frame type: %X\n",
+			vif->mac->macid, vif->vifid,
+			(frame_type & IEEE80211_FCTL_STYPE) >> 4);
+		return;
+	}
+
+	if (qtnf_cmd_send_register_mgmt(vif, qlink_frame_type, reg)) {
+		pr_warn("VIF%u.%u: failed to %sregister mgmt frame type 0x%x\n",
+			vif->mac->macid, vif->vifid, reg ? "" : "un",
+			frame_type);
+		return;
+	}
+
+	vif->mgmt_frames_bitmask = new_mask;
+	pr_debug("VIF%u.%u: %sregistered mgmt frame type 0x%x\n",
+		 vif->mac->macid, vif->vifid, reg ? "" : "un", frame_type);
+}
+
+static int
+qtnf_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
+	     struct cfg80211_mgmt_tx_params *params, u64 *cookie)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(wdev->netdev);
+	const struct ieee80211_mgmt *mgmt_frame = (void *)params->buf;
+	u32 short_cookie = prandom_u32();
+	u16 flags = 0;
+
+	*cookie = short_cookie;
+
+	if (params->offchan)
+		flags |= QLINK_MGMT_FRAME_TX_FLAG_OFFCHAN;
+
+	if (params->no_cck)
+		flags |= QLINK_MGMT_FRAME_TX_FLAG_NO_CCK;
+
+	if (params->dont_wait_for_ack)
+		flags |= QLINK_MGMT_FRAME_TX_FLAG_ACK_NOWAIT;
+
+	pr_debug("%s freq:%u; FC:%.4X; DA:%pM; len:%zu; C:%.8X; FL:%.4X\n",
+		 wdev->netdev->name, params->chan->center_freq,
+		 le16_to_cpu(mgmt_frame->frame_control), mgmt_frame->da,
+		 params->len, short_cookie, flags);
+
+	return qtnf_cmd_send_mgmt_frame(vif, short_cookie, flags,
+					params->chan->center_freq,
+					params->buf, params->len);
+}
+
+static int
+qtnf_get_station(struct wiphy *wiphy, struct net_device *dev,
+		 const u8 *mac, struct station_info *sinfo)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(dev);
+
+	return qtnf_cmd_get_sta_info(vif, mac, sinfo);
+}
+
+static int
+qtnf_dump_station(struct wiphy *wiphy, struct net_device *dev,
+		  int idx, u8 *mac, struct station_info *sinfo)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(dev);
+	const struct qtnf_sta_node *sta_node;
+	int ret;
+
+	sta_node = qtnf_sta_list_lookup_index(&vif->sta_list, idx);
+
+	if (unlikely(!sta_node))
+		return -ENOENT;
+
+	ether_addr_copy(mac, sta_node->mac_addr);
+
+	ret = qtnf_cmd_get_sta_info(vif, sta_node->mac_addr, sinfo);
+
+	if (unlikely(ret == -ENOENT)) {
+		qtnf_sta_list_del(&vif->sta_list, mac);
+		cfg80211_del_sta(vif->netdev, mac, GFP_KERNEL);
+		sinfo->filled = 0;
+	}
+
+	return ret;
+}
+
+static int qtnf_add_key(struct wiphy *wiphy, struct net_device *dev,
+			u8 key_index, bool pairwise, const u8 *mac_addr,
+			struct key_params *params)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(dev);
+	int ret;
+
+	ret = qtnf_cmd_send_add_key(vif, key_index, pairwise, mac_addr, params);
+	if (ret)
+		pr_err("VIF%u.%u: failed to add key: cipher=%x idx=%u pw=%u\n",
+		       vif->mac->macid, vif->vifid, params->cipher, key_index,
+		       pairwise);
+
+	return ret;
+}
+
+static int qtnf_del_key(struct wiphy *wiphy, struct net_device *dev,
+			u8 key_index, bool pairwise, const u8 *mac_addr)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(dev);
+	int ret;
+
+	ret = qtnf_cmd_send_del_key(vif, key_index, pairwise, mac_addr);
+	if (ret)
+		pr_err("VIF%u.%u: failed to delete key: idx=%u pw=%u\n",
+		       vif->mac->macid, vif->vifid, key_index, pairwise);
+
+	return ret;
+}
+
+static int qtnf_set_default_key(struct wiphy *wiphy, struct net_device *dev,
+				u8 key_index, bool unicast, bool multicast)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(dev);
+	int ret;
+
+	ret = qtnf_cmd_send_set_default_key(vif, key_index, unicast, multicast);
+	if (ret)
+		pr_err("VIF%u.%u: failed to set dflt key: idx=%u uc=%u mc=%u\n",
+		       vif->mac->macid, vif->vifid, key_index, unicast,
+		       multicast);
+
+	return ret;
+}
+
+static int
+qtnf_set_default_mgmt_key(struct wiphy *wiphy, struct net_device *dev,
+			  u8 key_index)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(dev);
+	int ret;
+
+	ret = qtnf_cmd_send_set_default_mgmt_key(vif, key_index);
+	if (ret)
+		pr_err("VIF%u.%u: failed to set default MGMT key: idx=%u\n",
+		       vif->mac->macid, vif->vifid, key_index);
+
+	return ret;
+}
+
+static int
+qtnf_change_station(struct wiphy *wiphy, struct net_device *dev,
+		    const u8 *mac, struct station_parameters *params)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(dev);
+	int ret;
+
+	ret = qtnf_cmd_send_change_sta(vif, mac, params);
+	if (ret)
+		pr_err("VIF%u.%u: failed to change STA %pM\n",
+		       vif->mac->macid, vif->vifid, mac);
+
+	return ret;
+}
+
+static int
+qtnf_del_station(struct wiphy *wiphy, struct net_device *dev,
+		 struct station_del_parameters *params)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(dev);
+	int ret;
+
+	if (params->mac &&
+	    (vif->wdev.iftype == NL80211_IFTYPE_AP) &&
+	    !is_broadcast_ether_addr(params->mac) &&
+	    !qtnf_sta_list_lookup(&vif->sta_list, params->mac))
+		return 0;
+
+	qtnf_scan_done(vif->mac, true);
+
+	ret = qtnf_cmd_send_del_sta(vif, params);
+	if (ret)
+		pr_err("VIF%u.%u: failed to delete STA %pM\n",
+		       vif->mac->macid, vif->vifid, params->mac);
+	return ret;
+}
+
+static int
+qtnf_scan(struct wiphy *wiphy, struct cfg80211_scan_request *request)
+{
+	struct qtnf_wmac *mac = wiphy_priv(wiphy);
+	int ret;
+
+	mac->scan_req = request;
+
+	ret = qtnf_cmd_send_scan(mac);
+	if (ret)
+		pr_err("MAC%u: failed to start scan\n", mac->macid);
+
+	return ret;
+}
+
+static int
+qtnf_connect(struct wiphy *wiphy, struct net_device *dev,
+	     struct cfg80211_connect_params *sme)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(dev);
+	struct qtnf_bss_config *bss_cfg;
+	int ret;
+
+	if (vif->wdev.iftype != NL80211_IFTYPE_STATION)
+		return -EOPNOTSUPP;
+
+	if (vif->sta_state != QTNF_STA_DISCONNECTED)
+		return -EBUSY;
+
+	bss_cfg = &vif->bss_cfg;
+	memset(bss_cfg, 0, sizeof(*bss_cfg));
+
+	bss_cfg->ssid_len = sme->ssid_len;
+	memcpy(&bss_cfg->ssid, sme->ssid, bss_cfg->ssid_len);
+	bss_cfg->chandef.chan = sme->channel;
+	bss_cfg->auth_type = sme->auth_type;
+	bss_cfg->privacy = sme->privacy;
+	bss_cfg->mfp = sme->mfp;
+
+	if ((sme->bg_scan_period > 0) &&
+	    (sme->bg_scan_period <= QTNF_MAX_BG_SCAN_PERIOD))
+		bss_cfg->bg_scan_period = sme->bg_scan_period;
+	else if (sme->bg_scan_period == -1)
+		bss_cfg->bg_scan_period = QTNF_DEFAULT_BG_SCAN_PERIOD;
+	else
+		bss_cfg->bg_scan_period = 0; /* disabled */
+
+	bss_cfg->connect_flags = 0;
+
+	if (sme->flags & ASSOC_REQ_DISABLE_HT)
+		bss_cfg->connect_flags |= QLINK_STA_CONNECT_DISABLE_HT;
+	if (sme->flags & ASSOC_REQ_DISABLE_VHT)
+		bss_cfg->connect_flags |= QLINK_STA_CONNECT_DISABLE_VHT;
+	if (sme->flags & ASSOC_REQ_USE_RRM)
+		bss_cfg->connect_flags |= QLINK_STA_CONNECT_USE_RRM;
+
+	memcpy(&bss_cfg->crypto, &sme->crypto, sizeof(bss_cfg->crypto));
+	if (sme->bssid)
+		ether_addr_copy(bss_cfg->bssid, sme->bssid);
+	else
+		eth_zero_addr(bss_cfg->bssid);
+
+	ret = qtnf_cmd_send_connect(vif, sme);
+	if (ret) {
+		pr_err("VIF%u.%u: failed to connect\n", vif->mac->macid,
+		       vif->vifid);
+		return ret;
+	}
+
+	vif->sta_state = QTNF_STA_CONNECTING;
+	return 0;
+}
+
+static int
+qtnf_disconnect(struct wiphy *wiphy, struct net_device *dev,
+		u16 reason_code)
+{
+	struct qtnf_wmac *mac = wiphy_priv(wiphy);
+	struct qtnf_vif *vif;
+	int ret;
+
+	vif = qtnf_mac_get_base_vif(mac);
+	if (!vif) {
+		pr_err("MAC%u: primary VIF is not configured\n", mac->macid);
+		return -EFAULT;
+	}
+
+	if (vif->wdev.iftype != NL80211_IFTYPE_STATION)
+		return -EOPNOTSUPP;
+
+	if (vif->sta_state == QTNF_STA_DISCONNECTED)
+		return 0;
+
+	ret = qtnf_cmd_send_disconnect(vif, reason_code);
+	if (ret) {
+		pr_err("VIF%u.%u: failed to disconnect\n", mac->macid,
+		       vif->vifid);
+		return ret;
+	}
+
+	vif->sta_state = QTNF_STA_DISCONNECTED;
+	return 0;
+}
+
+static struct cfg80211_ops qtn_cfg80211_ops = {
+	.add_virtual_intf	= qtnf_add_virtual_intf,
+	.change_virtual_intf	= qtnf_change_virtual_intf,
+	.del_virtual_intf	= qtnf_del_virtual_intf,
+	.start_ap		= qtnf_start_ap,
+	.change_beacon		= qtnf_change_beacon,
+	.stop_ap		= qtnf_stop_ap,
+	.set_wiphy_params	= qtnf_set_wiphy_params,
+	.mgmt_frame_register	= qtnf_mgmt_frame_register,
+	.mgmt_tx		= qtnf_mgmt_tx,
+	.change_station		= qtnf_change_station,
+	.del_station		= qtnf_del_station,
+	.get_station		= qtnf_get_station,
+	.dump_station		= qtnf_dump_station,
+	.add_key		= qtnf_add_key,
+	.del_key		= qtnf_del_key,
+	.set_default_key	= qtnf_set_default_key,
+	.set_default_mgmt_key	= qtnf_set_default_mgmt_key,
+	.scan			= qtnf_scan,
+	.connect		= qtnf_connect,
+	.disconnect		= qtnf_disconnect
+};
+
+static void qtnf_cfg80211_reg_notifier(struct wiphy *wiphy,
+				       struct regulatory_request *req)
+{
+	struct qtnf_wmac *mac = wiphy_priv(wiphy);
+	struct qtnf_bus *bus;
+	struct qtnf_vif *vif;
+	struct qtnf_wmac *chan_mac;
+	int i;
+	enum nl80211_band band;
+
+	bus = mac->bus;
+
+	pr_debug("MAC%u: initiator=%d alpha=%c%c\n", mac->macid, req->initiator,
+		 req->alpha2[0], req->alpha2[1]);
+
+	vif = qtnf_mac_get_base_vif(mac);
+	if (!vif) {
+		pr_err("MAC%u: primary VIF is not configured\n", mac->macid);
+		return;
+	}
+
+	/* ignore non-ISO3166 country codes */
+	for (i = 0; i < sizeof(req->alpha2); i++) {
+		if (req->alpha2[i] < 'A' || req->alpha2[i] > 'Z') {
+			pr_err("MAC%u: not an ISO3166 code\n", mac->macid);
+			return;
+		}
+	}
+	if (!strncasecmp(req->alpha2, bus->hw_info.alpha2_code,
+			 sizeof(req->alpha2))) {
+		pr_warn("MAC%u: unchanged country code\n", mac->macid);
+		return;
+	}
+
+	if (qtnf_cmd_send_regulatory_config(mac, req->alpha2)) {
+		pr_err("MAC%u: failed to configure regulatory\n", mac->macid);
+		return;
+	}
+
+	for (i = 0; i < bus->hw_info.num_mac; i++) {
+		chan_mac = bus->mac[i];
+
+		if (!chan_mac)
+			continue;
+
+		if (!(bus->hw_info.mac_bitmap & BIT(i)))
+			continue;
+
+		for (band = 0; band < NUM_NL80211_BANDS; ++band) {
+			if (!wiphy->bands[band])
+				continue;
+
+			if (qtnf_cmd_get_mac_chan_info(chan_mac,
+						       wiphy->bands[band])) {
+				pr_err("MAC%u: can't get channel info\n",
+				       chan_mac->macid);
+				qtnf_core_detach(bus);
+
+				return;
+			}
+		}
+	}
+}
+
+void qtnf_band_setup_htvht_caps(struct qtnf_mac_info *macinfo,
+				struct ieee80211_supported_band *band)
+{
+	struct ieee80211_sta_ht_cap *ht_cap;
+	struct ieee80211_sta_vht_cap *vht_cap;
+
+	ht_cap = &band->ht_cap;
+	ht_cap->ht_supported = true;
+	memcpy(&ht_cap->cap, &macinfo->ht_cap.cap_info,
+	       sizeof(u16));
+	ht_cap->ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
+	ht_cap->ampdu_density = IEEE80211_HT_MPDU_DENSITY_NONE;
+	memcpy(&ht_cap->mcs, &macinfo->ht_cap.mcs,
+	       sizeof(ht_cap->mcs));
+
+	if (macinfo->phymode_cap & QLINK_PHYMODE_AC) {
+		vht_cap = &band->vht_cap;
+		vht_cap->vht_supported = true;
+		memcpy(&vht_cap->cap,
+		       &macinfo->vht_cap.vht_cap_info, sizeof(u32));
+		/* Update MCS support for VHT */
+		memcpy(&vht_cap->vht_mcs,
+		       &macinfo->vht_cap.supp_mcs,
+		       sizeof(struct ieee80211_vht_mcs_info));
+	}
+}
+
+struct wiphy *qtnf_wiphy_allocate(struct qtnf_bus *bus)
+{
+	struct wiphy *wiphy;
+
+	wiphy = wiphy_new(&qtn_cfg80211_ops, sizeof(struct qtnf_wmac));
+	if (!wiphy)
+		return NULL;
+
+	set_wiphy_dev(wiphy, bus->dev);
+
+	return wiphy;
+}
+
+static int qtnf_wiphy_setup_if_comb(struct wiphy *wiphy,
+				    struct ieee80211_iface_combination *if_comb,
+				    const struct qtnf_mac_info *mac_info)
+{
+	size_t max_interfaces = 0;
+	u16 interface_modes = 0;
+	size_t i;
+
+	if (unlikely(!mac_info->limits || !mac_info->n_limits))
+		return -ENOENT;
+
+	if_comb->limits = mac_info->limits;
+	if_comb->n_limits = mac_info->n_limits;
+
+	for (i = 0; i < mac_info->n_limits; i++) {
+		max_interfaces += mac_info->limits[i].max;
+		interface_modes |= mac_info->limits[i].types;
+	}
+
+	if_comb->num_different_channels = 1;
+	if_comb->beacon_int_infra_match = true;
+	if_comb->max_interfaces = max_interfaces;
+	if_comb->radar_detect_widths = mac_info->radar_detect_widths;
+	wiphy->interface_modes = interface_modes;
+
+	return 0;
+}
+
+int qtnf_wiphy_register(struct qtnf_hw_info *hw_info, struct qtnf_wmac *mac)
+{
+	struct wiphy *wiphy = priv_to_wiphy(mac);
+	struct ieee80211_iface_combination *iface_comb = NULL;
+	int ret;
+
+	if (!wiphy) {
+		pr_err("invalid wiphy pointer\n");
+		return -EFAULT;
+	}
+
+	iface_comb = kzalloc(sizeof(*iface_comb), GFP_KERNEL);
+	if (!iface_comb) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = qtnf_wiphy_setup_if_comb(wiphy, iface_comb, &mac->macinfo);
+	if (ret)
+		goto out;
+
+	pr_info("MAC%u: phymode=%#x radar=%#x\n", mac->macid,
+		mac->macinfo.phymode_cap, mac->macinfo.radar_detect_widths);
+
+	wiphy->frag_threshold = mac->macinfo.frag_thr;
+	wiphy->rts_threshold = mac->macinfo.rts_thr;
+	wiphy->retry_short = mac->macinfo.sretry_limit;
+	wiphy->retry_long = mac->macinfo.lretry_limit;
+	wiphy->coverage_class = mac->macinfo.coverage_class;
+
+	wiphy->max_scan_ssids = QTNF_MAX_SSID_LIST_LENGTH;
+	wiphy->max_scan_ie_len = QTNF_MAX_VSIE_LEN;
+	wiphy->mgmt_stypes = qtnf_mgmt_stypes;
+	wiphy->max_remain_on_channel_duration = 5000;
+
+	wiphy->iface_combinations = iface_comb;
+	wiphy->n_iface_combinations = 1;
+
+	/* Initialize cipher suits */
+	wiphy->cipher_suites = qtnf_cipher_suites;
+	wiphy->n_cipher_suites = ARRAY_SIZE(qtnf_cipher_suites);
+	wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
+	wiphy->flags |= WIPHY_FLAG_HAVE_AP_SME |
+			WIPHY_FLAG_AP_PROBE_RESP_OFFLOAD |
+			WIPHY_FLAG_AP_UAPSD;
+
+	wiphy->probe_resp_offload = NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS |
+				    NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS2;
+
+	wiphy->available_antennas_tx = mac->macinfo.num_tx_chain;
+	wiphy->available_antennas_rx = mac->macinfo.num_rx_chain;
+
+	wiphy->max_ap_assoc_sta = mac->macinfo.max_ap_assoc_sta;
+
+	ether_addr_copy(wiphy->perm_addr, mac->macaddr);
+
+	if (hw_info->hw_capab & QLINK_HW_SUPPORTS_REG_UPDATE) {
+		pr_debug("device supports REG_UPDATE\n");
+		wiphy->reg_notifier = qtnf_cfg80211_reg_notifier;
+		pr_debug("hint regulatory about EP region: %c%c\n",
+			 hw_info->alpha2_code[0],
+			 hw_info->alpha2_code[1]);
+		regulatory_hint(wiphy, hw_info->alpha2_code);
+	} else {
+		pr_debug("device doesn't support REG_UPDATE\n");
+		wiphy->regulatory_flags |= REGULATORY_WIPHY_SELF_MANAGED;
+	}
+
+	ret = wiphy_register(wiphy);
+
+out:
+	if (ret < 0) {
+		kfree(iface_comb);
+		return ret;
+	}
+
+	return 0;
+}
+
+void qtnf_netdev_updown(struct net_device *ndev, bool up)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(ndev);
+
+	if (qtnf_cmd_send_updown_intf(vif, up))
+		pr_err("failed to send up/down command to FW\n");
+}
+
+void qtnf_virtual_intf_cleanup(struct net_device *ndev)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(ndev);
+	struct qtnf_wmac *mac = wiphy_priv(vif->wdev.wiphy);
+
+	if (vif->wdev.iftype == NL80211_IFTYPE_STATION) {
+		switch (vif->sta_state) {
+		case QTNF_STA_DISCONNECTED:
+			break;
+		case QTNF_STA_CONNECTING:
+			cfg80211_connect_result(vif->netdev,
+						vif->bss_cfg.bssid, NULL, 0,
+						NULL, 0,
+						WLAN_STATUS_UNSPECIFIED_FAILURE,
+						GFP_KERNEL);
+			qtnf_disconnect(vif->wdev.wiphy, ndev,
+					WLAN_REASON_DEAUTH_LEAVING);
+			break;
+		case QTNF_STA_CONNECTED:
+			cfg80211_disconnected(vif->netdev,
+					      WLAN_REASON_DEAUTH_LEAVING,
+					      NULL, 0, 1, GFP_KERNEL);
+			qtnf_disconnect(vif->wdev.wiphy, ndev,
+					WLAN_REASON_DEAUTH_LEAVING);
+			break;
+		}
+
+		vif->sta_state = QTNF_STA_DISCONNECTED;
+		qtnf_scan_done(mac, true);
+	}
+}
+
+void qtnf_cfg80211_vif_reset(struct qtnf_vif *vif)
+{
+	if (vif->wdev.iftype == NL80211_IFTYPE_STATION) {
+		switch (vif->sta_state) {
+		case QTNF_STA_CONNECTING:
+			cfg80211_connect_result(vif->netdev,
+						vif->bss_cfg.bssid, NULL, 0,
+						NULL, 0,
+						WLAN_STATUS_UNSPECIFIED_FAILURE,
+						GFP_KERNEL);
+			break;
+		case QTNF_STA_CONNECTED:
+			cfg80211_disconnected(vif->netdev,
+					      WLAN_REASON_DEAUTH_LEAVING,
+					      NULL, 0, 1, GFP_KERNEL);
+			break;
+		case QTNF_STA_DISCONNECTED:
+			break;
+		}
+	}
+
+	cfg80211_shutdown_all_interfaces(vif->wdev.wiphy);
+	vif->sta_state = QTNF_STA_DISCONNECTED;
+}
+
+void qtnf_band_init_rates(struct ieee80211_supported_band *band)
+{
+	switch (band->band) {
+	case NL80211_BAND_2GHZ:
+		band->bitrates = qtnf_rates_2g;
+		band->n_bitrates = ARRAY_SIZE(qtnf_rates_2g);
+		break;
+	case NL80211_BAND_5GHZ:
+		band->bitrates = qtnf_rates_5g;
+		band->n_bitrates = ARRAY_SIZE(qtnf_rates_5g);
+		break;
+	default:
+		band->bitrates = NULL;
+		band->n_bitrates = 0;
+		break;
+	}
+}
diff --git a/drivers/net/wireless/quantenna/qtnfmac/cfg80211.h b/drivers/net/wireless/quantenna/qtnfmac/cfg80211.h
new file mode 100644
index 000000000000..5bd33124a7c8
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/cfg80211.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _QTN_FMAC_CFG80211_H_
+#define _QTN_FMAC_CFG80211_H_
+
+#include <net/cfg80211.h>
+
+#include "core.h"
+
+int qtnf_wiphy_register(struct qtnf_hw_info *hw_info, struct qtnf_wmac *mac);
+int qtnf_del_virtual_intf(struct wiphy *wiphy, struct wireless_dev *wdev);
+void qtnf_cfg80211_vif_reset(struct qtnf_vif *vif);
+void qtnf_band_init_rates(struct ieee80211_supported_band *band);
+void qtnf_band_setup_htvht_caps(struct qtnf_mac_info *macinfo,
+				struct ieee80211_supported_band *band);
+
+static inline void qtnf_scan_done(struct qtnf_wmac *mac, bool aborted)
+{
+	struct cfg80211_scan_info info = {
+		.aborted = aborted,
+	};
+
+	if (mac->scan_req) {
+		cfg80211_scan_done(mac->scan_req, &info);
+		mac->scan_req = NULL;
+	}
+}
+
+#endif /* _QTN_FMAC_CFG80211_H_ */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/commands.c b/drivers/net/wireless/quantenna/qtnfmac/commands.c
new file mode 100644
index 000000000000..b39dbc3d3c1f
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/commands.c
@@ -0,0 +1,1978 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by 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/types.h>
+#include <linux/skbuff.h>
+
+#include "cfg80211.h"
+#include "core.h"
+#include "qlink.h"
+#include "qlink_util.h"
+#include "bus.h"
+#include "commands.h"
+
+static int qtnf_cmd_check_reply_header(const struct qlink_resp *resp,
+				       u16 cmd_id, u8 mac_id, u8 vif_id,
+				       size_t resp_size)
+{
+	if (unlikely(le16_to_cpu(resp->cmd_id) != cmd_id)) {
+		pr_warn("VIF%u.%u CMD%x: bad cmd_id in response: 0x%.4X\n",
+			mac_id, vif_id, cmd_id, le16_to_cpu(resp->cmd_id));
+		return -EINVAL;
+	}
+
+	if (unlikely(resp->macid != mac_id)) {
+		pr_warn("VIF%u.%u CMD%x: bad MAC in response: %u\n",
+			mac_id, vif_id, cmd_id, resp->macid);
+		return -EINVAL;
+	}
+
+	if (unlikely(resp->vifid != vif_id)) {
+		pr_warn("VIF%u.%u CMD%x: bad VIF in response: %u\n",
+			mac_id, vif_id, cmd_id, resp->vifid);
+		return -EINVAL;
+	}
+
+	if (unlikely(le16_to_cpu(resp->mhdr.len) < resp_size)) {
+		pr_warn("VIF%u.%u CMD%x: bad response size %u < %zu\n",
+			mac_id, vif_id, cmd_id,
+			le16_to_cpu(resp->mhdr.len), resp_size);
+		return -ENOSPC;
+	}
+
+	return 0;
+}
+
+static int qtnf_cmd_send_with_reply(struct qtnf_bus *bus,
+				    struct sk_buff *cmd_skb,
+				    struct sk_buff **response_skb,
+				    u16 *result_code,
+				    size_t const_resp_size,
+				    size_t *var_resp_size)
+{
+	struct qlink_cmd *cmd;
+	const struct qlink_resp *resp;
+	struct sk_buff *resp_skb = NULL;
+	u16 cmd_id;
+	u8 mac_id, vif_id;
+	int ret;
+
+	cmd = (struct qlink_cmd *)cmd_skb->data;
+	cmd_id = le16_to_cpu(cmd->cmd_id);
+	mac_id = cmd->macid;
+	vif_id = cmd->vifid;
+	cmd->mhdr.len = cpu_to_le16(cmd_skb->len);
+
+	if (unlikely(bus->fw_state != QTNF_FW_STATE_ACTIVE &&
+		     le16_to_cpu(cmd->cmd_id) != QLINK_CMD_FW_INIT)) {
+		pr_warn("VIF%u.%u: drop cmd 0x%.4X in fw state %d\n",
+			mac_id, vif_id, le16_to_cpu(cmd->cmd_id),
+			bus->fw_state);
+		return -ENODEV;
+	}
+
+	pr_debug("VIF%u.%u cmd=0x%.4X\n", mac_id, vif_id,
+		 le16_to_cpu(cmd->cmd_id));
+
+	ret = qtnf_trans_send_cmd_with_resp(bus, cmd_skb, &resp_skb);
+
+	if (unlikely(ret))
+		goto out;
+
+	resp = (const struct qlink_resp *)resp_skb->data;
+	ret = qtnf_cmd_check_reply_header(resp, cmd_id, mac_id, vif_id,
+					  const_resp_size);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (likely(result_code))
+		*result_code = le16_to_cpu(resp->result);
+
+	/* Return length of variable part of response */
+	if (response_skb && var_resp_size)
+		*var_resp_size = le16_to_cpu(resp->mhdr.len) - const_resp_size;
+
+out:
+	if (response_skb)
+		*response_skb = resp_skb;
+	else
+		consume_skb(resp_skb);
+
+	return ret;
+}
+
+static inline int qtnf_cmd_send(struct qtnf_bus *bus,
+				struct sk_buff *cmd_skb,
+				u16 *result_code)
+{
+	return qtnf_cmd_send_with_reply(bus, cmd_skb, NULL, result_code,
+					sizeof(struct qlink_resp), NULL);
+}
+
+static struct sk_buff *qtnf_cmd_alloc_new_cmdskb(u8 macid, u8 vifid, u16 cmd_no,
+						 size_t cmd_size)
+{
+	struct qlink_cmd *cmd;
+	struct sk_buff *cmd_skb;
+
+	cmd_skb = __dev_alloc_skb(sizeof(*cmd) +
+				  QTNF_MAX_CMD_BUF_SIZE, GFP_KERNEL);
+	if (unlikely(!cmd_skb)) {
+		pr_err("VIF%u.%u CMD %u: alloc failed\n", macid, vifid, cmd_no);
+		return NULL;
+	}
+
+	skb_put_zero(cmd_skb, cmd_size);
+
+	cmd = (struct qlink_cmd *)cmd_skb->data;
+	cmd->mhdr.len = cpu_to_le16(cmd_skb->len);
+	cmd->mhdr.type = cpu_to_le16(QLINK_MSG_TYPE_CMD);
+	cmd->cmd_id = cpu_to_le16(cmd_no);
+	cmd->macid = macid;
+	cmd->vifid = vifid;
+
+	return cmd_skb;
+}
+
+int qtnf_cmd_send_start_ap(struct qtnf_vif *vif)
+{
+	struct sk_buff *cmd_skb;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_START_AP,
+					    sizeof(struct qlink_cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	vif->bss_status |= QTNF_STATE_AP_START;
+	netif_carrier_on(vif->netdev);
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_regulatory_config(struct qtnf_wmac *mac, const char *alpha2)
+{
+	struct sk_buff *cmd_skb;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, QLINK_VIFID_RSVD,
+					    QLINK_CMD_REG_REGION,
+					    sizeof(struct qlink_cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_cmd_skb_put_tlv_arr(cmd_skb, WLAN_EID_COUNTRY, alpha2,
+				 QTNF_MAX_ALPHA_LEN);
+
+	ret = qtnf_cmd_send(mac->bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("MAC%u: CMD failed: %u\n", mac->macid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	memcpy(mac->bus->hw_info.alpha2_code, alpha2,
+	       sizeof(mac->bus->hw_info.alpha2_code));
+out:
+	return ret;
+}
+
+int qtnf_cmd_send_config_ap(struct qtnf_vif *vif)
+{
+	struct sk_buff *cmd_skb;
+	struct qtnf_bss_config *bss_cfg = &vif->bss_cfg;
+	struct cfg80211_chan_def *chandef = &bss_cfg->chandef;
+	struct qlink_tlv_channel *qchan;
+	struct qlink_auth_encr aen;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret;
+	int i;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_CONFIG_AP,
+					    sizeof(struct qlink_cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	qtnf_cmd_skb_put_tlv_arr(cmd_skb, WLAN_EID_SSID, bss_cfg->ssid,
+				 bss_cfg->ssid_len);
+	qtnf_cmd_skb_put_tlv_u16(cmd_skb, QTN_TLV_ID_BCN_PERIOD,
+				 bss_cfg->bcn_period);
+	qtnf_cmd_skb_put_tlv_u8(cmd_skb, QTN_TLV_ID_DTIM, bss_cfg->dtim);
+
+	qchan = skb_put_zero(cmd_skb, sizeof(*qchan));
+	qchan->hdr.type = cpu_to_le16(QTN_TLV_ID_CHANNEL);
+	qchan->hdr.len = cpu_to_le16(sizeof(*qchan) -
+			sizeof(struct qlink_tlv_hdr));
+	qchan->hw_value = cpu_to_le16(
+		ieee80211_frequency_to_channel(chandef->chan->center_freq));
+
+	memset(&aen, 0, sizeof(aen));
+	aen.auth_type = bss_cfg->auth_type;
+	aen.privacy = !!bss_cfg->privacy;
+	aen.mfp = bss_cfg->mfp;
+	aen.wpa_versions = cpu_to_le32(bss_cfg->crypto.wpa_versions);
+	aen.cipher_group = cpu_to_le32(bss_cfg->crypto.cipher_group);
+	aen.n_ciphers_pairwise = cpu_to_le32(
+					bss_cfg->crypto.n_ciphers_pairwise);
+	for (i = 0; i < QLINK_MAX_NR_CIPHER_SUITES; i++)
+		aen.ciphers_pairwise[i] = cpu_to_le32(
+					bss_cfg->crypto.ciphers_pairwise[i]);
+	aen.n_akm_suites = cpu_to_le32(
+					bss_cfg->crypto.n_akm_suites);
+	for (i = 0; i < QLINK_MAX_NR_AKM_SUITES; i++)
+		aen.akm_suites[i] = cpu_to_le32(
+					bss_cfg->crypto.akm_suites[i]);
+	aen.control_port = bss_cfg->crypto.control_port;
+	aen.control_port_no_encrypt =
+			bss_cfg->crypto.control_port_no_encrypt;
+	aen.control_port_ethertype = cpu_to_le16(be16_to_cpu(
+				bss_cfg->crypto.control_port_ethertype));
+
+	qtnf_cmd_skb_put_tlv_arr(cmd_skb, QTN_TLV_ID_CRYPTO, (u8 *)&aen,
+				 sizeof(aen));
+
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	vif->bss_status |= QTNF_STATE_AP_CONFIG;
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_stop_ap(struct qtnf_vif *vif)
+{
+	struct sk_buff *cmd_skb;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_STOP_AP,
+					    sizeof(struct qlink_cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	vif->bss_status &= ~QTNF_STATE_AP_START;
+	vif->bss_status &= ~QTNF_STATE_AP_CONFIG;
+
+	netif_carrier_off(vif->netdev);
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_register_mgmt(struct qtnf_vif *vif, u16 frame_type, bool reg)
+{
+	struct sk_buff *cmd_skb;
+	struct qlink_cmd_mgmt_frame_register *cmd;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_REGISTER_MGMT,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_mgmt_frame_register *)cmd_skb->data;
+	cmd->frame_type = cpu_to_le16(frame_type);
+	cmd->do_register = reg;
+
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_mgmt_frame(struct qtnf_vif *vif, u32 cookie, u16 flags,
+			     u16 freq, const u8 *buf, size_t len)
+{
+	struct sk_buff *cmd_skb;
+	struct qlink_cmd_mgmt_frame_tx *cmd;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret;
+
+	if (sizeof(*cmd) + len > QTNF_MAX_CMD_BUF_SIZE) {
+		pr_warn("VIF%u.%u: frame is too big: %zu\n", vif->mac->macid,
+			vif->vifid, len);
+		return -E2BIG;
+	}
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_SEND_MGMT_FRAME,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_mgmt_frame_tx *)cmd_skb->data;
+	cmd->cookie = cpu_to_le32(cookie);
+	cmd->freq = cpu_to_le16(freq);
+	cmd->flags = cpu_to_le16(flags);
+
+	if (len && buf)
+		qtnf_cmd_skb_put_buffer(cmd_skb, buf, len);
+
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_mgmt_set_appie(struct qtnf_vif *vif, u8 frame_type,
+				 const u8 *buf, size_t len)
+{
+	struct sk_buff *cmd_skb;
+	struct qlink_cmd_mgmt_append_ie *cmd;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret;
+
+	if (sizeof(*cmd) + len > QTNF_MAX_CMD_BUF_SIZE) {
+		pr_warn("VIF%u.%u: %u frame is too big: %zu\n", vif->mac->macid,
+			vif->vifid, frame_type, len);
+		return -E2BIG;
+	}
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_MGMT_SET_APPIE,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_mgmt_append_ie *)cmd_skb->data;
+	cmd->type = frame_type;
+	cmd->flags = 0;
+
+	/* If len == 0 then IE buf for specified frame type
+	 * should be cleared on EP.
+	 */
+	if (len && buf)
+		qtnf_cmd_skb_put_buffer(cmd_skb, buf, len);
+
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u frame %u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, frame_type, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+static void
+qtnf_sta_info_parse_basic_counters(struct station_info *sinfo,
+		const struct qlink_sta_stat_basic_counters *counters)
+{
+	sinfo->filled |= BIT(NL80211_STA_INFO_RX_BYTES) |
+			 BIT(NL80211_STA_INFO_TX_BYTES);
+	sinfo->rx_bytes = get_unaligned_le64(&counters->rx_bytes);
+	sinfo->tx_bytes = get_unaligned_le64(&counters->tx_bytes);
+
+	sinfo->filled |= BIT(NL80211_STA_INFO_RX_PACKETS) |
+			 BIT(NL80211_STA_INFO_TX_PACKETS) |
+			 BIT(NL80211_STA_INFO_BEACON_RX);
+	sinfo->rx_packets = get_unaligned_le32(&counters->rx_packets);
+	sinfo->tx_packets = get_unaligned_le32(&counters->tx_packets);
+	sinfo->rx_beacon = get_unaligned_le64(&counters->rx_beacons);
+
+	sinfo->filled |= BIT(NL80211_STA_INFO_RX_DROP_MISC) |
+			 BIT(NL80211_STA_INFO_TX_FAILED);
+	sinfo->rx_dropped_misc = get_unaligned_le32(&counters->rx_dropped);
+	sinfo->tx_failed = get_unaligned_le32(&counters->tx_failed);
+}
+
+static void
+qtnf_sta_info_parse_rate(struct rate_info *rate_dst,
+			 const struct  qlink_sta_info_rate *rate_src)
+{
+	rate_dst->legacy = get_unaligned_le16(&rate_src->rate) * 10;
+
+	rate_dst->mcs = rate_src->mcs;
+	rate_dst->nss = rate_src->nss;
+	rate_dst->flags = 0;
+
+	switch (rate_src->bw) {
+	case QLINK_STA_INFO_RATE_BW_5:
+		rate_dst->bw = RATE_INFO_BW_5;
+		break;
+	case QLINK_STA_INFO_RATE_BW_10:
+		rate_dst->bw = RATE_INFO_BW_10;
+		break;
+	case QLINK_STA_INFO_RATE_BW_20:
+		rate_dst->bw = RATE_INFO_BW_20;
+		break;
+	case QLINK_STA_INFO_RATE_BW_40:
+		rate_dst->bw = RATE_INFO_BW_40;
+		break;
+	case QLINK_STA_INFO_RATE_BW_80:
+		rate_dst->bw = RATE_INFO_BW_80;
+		break;
+	case QLINK_STA_INFO_RATE_BW_160:
+		rate_dst->bw = RATE_INFO_BW_160;
+		break;
+	default:
+		rate_dst->bw = 0;
+		break;
+	}
+
+	if (rate_src->flags & QLINK_STA_INFO_RATE_FLAG_HT_MCS)
+		rate_dst->flags |= RATE_INFO_FLAGS_MCS;
+	else if (rate_src->flags & QLINK_STA_INFO_RATE_FLAG_VHT_MCS)
+		rate_dst->flags |= RATE_INFO_FLAGS_VHT_MCS;
+}
+
+static void
+qtnf_sta_info_parse_flags(struct nl80211_sta_flag_update *dst,
+			  const struct qlink_sta_info_state *src)
+{
+	u32 mask, value;
+
+	dst->mask = 0;
+	dst->set = 0;
+
+	mask = le32_to_cpu(src->mask);
+	value = le32_to_cpu(src->value);
+
+	if (mask & QLINK_STA_FLAG_AUTHORIZED) {
+		dst->mask |= BIT(NL80211_STA_FLAG_AUTHORIZED);
+		if (value & QLINK_STA_FLAG_AUTHORIZED)
+			dst->set |= BIT(NL80211_STA_FLAG_AUTHORIZED);
+	}
+
+	if (mask & QLINK_STA_FLAG_SHORT_PREAMBLE) {
+		dst->mask |= BIT(NL80211_STA_FLAG_SHORT_PREAMBLE);
+		if (value & QLINK_STA_FLAG_SHORT_PREAMBLE)
+			dst->set |= BIT(NL80211_STA_FLAG_SHORT_PREAMBLE);
+	}
+
+	if (mask & QLINK_STA_FLAG_WME) {
+		dst->mask |= BIT(NL80211_STA_FLAG_WME);
+		if (value & QLINK_STA_FLAG_WME)
+			dst->set |= BIT(NL80211_STA_FLAG_WME);
+	}
+
+	if (mask & QLINK_STA_FLAG_MFP) {
+		dst->mask |= BIT(NL80211_STA_FLAG_MFP);
+		if (value & QLINK_STA_FLAG_MFP)
+			dst->set |= BIT(NL80211_STA_FLAG_MFP);
+	}
+
+	if (mask & QLINK_STA_FLAG_AUTHENTICATED) {
+		dst->mask |= BIT(NL80211_STA_FLAG_AUTHENTICATED);
+		if (value & QLINK_STA_FLAG_AUTHENTICATED)
+			dst->set |= BIT(NL80211_STA_FLAG_AUTHENTICATED);
+	}
+
+	if (mask & QLINK_STA_FLAG_TDLS_PEER) {
+		dst->mask |= BIT(NL80211_STA_FLAG_TDLS_PEER);
+		if (value & QLINK_STA_FLAG_TDLS_PEER)
+			dst->set |= BIT(NL80211_STA_FLAG_TDLS_PEER);
+	}
+
+	if (mask & QLINK_STA_FLAG_ASSOCIATED) {
+		dst->mask |= BIT(NL80211_STA_FLAG_ASSOCIATED);
+		if (value & QLINK_STA_FLAG_ASSOCIATED)
+			dst->set |= BIT(NL80211_STA_FLAG_ASSOCIATED);
+	}
+}
+
+static void
+qtnf_sta_info_parse_generic_info(struct station_info *sinfo,
+				 const struct qlink_sta_info_generic *info)
+{
+	sinfo->filled |= BIT(NL80211_STA_INFO_CONNECTED_TIME) |
+			 BIT(NL80211_STA_INFO_INACTIVE_TIME);
+	sinfo->connected_time = get_unaligned_le32(&info->connected_time);
+	sinfo->inactive_time = get_unaligned_le32(&info->inactive_time);
+
+	sinfo->filled |= BIT(NL80211_STA_INFO_SIGNAL) |
+			 BIT(NL80211_STA_INFO_SIGNAL_AVG);
+	sinfo->signal = info->rssi - 120;
+	sinfo->signal_avg = info->rssi_avg - QLINK_RSSI_OFFSET;
+
+	if (info->rx_rate.rate) {
+		sinfo->filled |= BIT(NL80211_STA_INFO_RX_BITRATE);
+		qtnf_sta_info_parse_rate(&sinfo->rxrate, &info->rx_rate);
+	}
+
+	if (info->tx_rate.rate) {
+		sinfo->filled |= BIT(NL80211_STA_INFO_TX_BITRATE);
+		qtnf_sta_info_parse_rate(&sinfo->txrate, &info->tx_rate);
+	}
+
+	sinfo->filled |= BIT(NL80211_STA_INFO_STA_FLAGS);
+	qtnf_sta_info_parse_flags(&sinfo->sta_flags, &info->state);
+}
+
+static int qtnf_cmd_sta_info_parse(struct station_info *sinfo,
+				   const u8 *payload, size_t payload_size)
+{
+	const struct qlink_sta_stat_basic_counters *counters;
+	const struct qlink_sta_info_generic *sta_info;
+	u16 tlv_type;
+	u16 tlv_value_len;
+	size_t tlv_full_len;
+	const struct qlink_tlv_hdr *tlv;
+
+	sinfo->filled = 0;
+
+	tlv = (const struct qlink_tlv_hdr *)payload;
+	while (payload_size >= sizeof(struct qlink_tlv_hdr)) {
+		tlv_type = le16_to_cpu(tlv->type);
+		tlv_value_len = le16_to_cpu(tlv->len);
+		tlv_full_len = tlv_value_len + sizeof(struct qlink_tlv_hdr);
+		if (tlv_full_len > payload_size) {
+			pr_warn("malformed TLV 0x%.2X; LEN: %u\n",
+				tlv_type, tlv_value_len);
+			return -EINVAL;
+		}
+		switch (tlv_type) {
+		case QTN_TLV_ID_STA_BASIC_COUNTERS:
+			if (unlikely(tlv_value_len < sizeof(*counters))) {
+				pr_err("invalid TLV size %.4X: %u\n",
+				       tlv_type, tlv_value_len);
+				break;
+			}
+
+			counters = (void *)tlv->val;
+			qtnf_sta_info_parse_basic_counters(sinfo, counters);
+			break;
+		case QTN_TLV_ID_STA_GENERIC_INFO:
+			if (unlikely(tlv_value_len < sizeof(*sta_info)))
+				break;
+
+			sta_info = (void *)tlv->val;
+			qtnf_sta_info_parse_generic_info(sinfo, sta_info);
+			break;
+		default:
+			pr_warn("unexpected TLV type: %.4X\n", tlv_type);
+			break;
+		}
+		payload_size -= tlv_full_len;
+		tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_value_len);
+	}
+
+	if (payload_size) {
+		pr_warn("malformed TLV buf; bytes left: %zu\n", payload_size);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int qtnf_cmd_get_sta_info(struct qtnf_vif *vif, const u8 *sta_mac,
+			  struct station_info *sinfo)
+{
+	struct sk_buff *cmd_skb, *resp_skb = NULL;
+	struct qlink_cmd_get_sta_info *cmd;
+	const struct qlink_resp_get_sta_info *resp;
+	size_t var_resp_len;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_GET_STA_INFO,
+					    sizeof(*cmd));
+
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_get_sta_info *)cmd_skb->data;
+	ether_addr_copy(cmd->sta_addr, sta_mac);
+
+	ret = qtnf_cmd_send_with_reply(vif->mac->bus, cmd_skb, &resp_skb,
+				       &res_code, sizeof(*resp),
+				       &var_resp_len);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		switch (res_code) {
+		case QLINK_CMD_RESULT_ENOTFOUND:
+			pr_warn("VIF%u.%u: %pM STA not found\n",
+				vif->mac->macid, vif->vifid, sta_mac);
+			ret = -ENOENT;
+			break;
+		default:
+			pr_err("VIF%u.%u: can't get info for %pM: %u\n",
+			       vif->mac->macid, vif->vifid, sta_mac, res_code);
+			ret = -EFAULT;
+			break;
+		}
+		goto out;
+	}
+
+	resp = (const struct qlink_resp_get_sta_info *)resp_skb->data;
+
+	if (unlikely(!ether_addr_equal(sta_mac, resp->sta_addr))) {
+		pr_err("VIF%u.%u: wrong mac in reply: %pM != %pM\n",
+		       vif->mac->macid, vif->vifid, resp->sta_addr, sta_mac);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = qtnf_cmd_sta_info_parse(sinfo, resp->info, var_resp_len);
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	consume_skb(resp_skb);
+
+	return ret;
+}
+
+static int qtnf_cmd_send_add_change_intf(struct qtnf_vif *vif,
+					 enum nl80211_iftype iftype,
+					 u8 *mac_addr,
+					 enum qlink_cmd_type cmd_type)
+{
+	struct sk_buff *cmd_skb, *resp_skb = NULL;
+	struct qlink_cmd_manage_intf *cmd;
+	const struct qlink_resp_manage_intf *resp;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    cmd_type,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_manage_intf *)cmd_skb->data;
+
+	switch (iftype) {
+	case NL80211_IFTYPE_AP:
+		cmd->intf_info.if_type = cpu_to_le16(QLINK_IFTYPE_AP);
+		break;
+	case NL80211_IFTYPE_STATION:
+		cmd->intf_info.if_type = cpu_to_le16(QLINK_IFTYPE_STATION);
+		break;
+	default:
+		pr_err("VIF%u.%u: unsupported type %d\n", vif->mac->macid,
+		       vif->vifid, iftype);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (mac_addr)
+		ether_addr_copy(cmd->intf_info.mac_addr, mac_addr);
+	else
+		eth_zero_addr(cmd->intf_info.mac_addr);
+
+	ret = qtnf_cmd_send_with_reply(vif->mac->bus, cmd_skb, &resp_skb,
+				       &res_code, sizeof(*resp), NULL);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD %d failed: %u\n", vif->mac->macid,
+		       vif->vifid, cmd_type, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	resp = (const struct qlink_resp_manage_intf *)resp_skb->data;
+	ether_addr_copy(vif->mac_addr, resp->intf_info.mac_addr);
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	consume_skb(resp_skb);
+
+	return ret;
+}
+
+int qtnf_cmd_send_add_intf(struct qtnf_vif *vif,
+			   enum nl80211_iftype iftype, u8 *mac_addr)
+{
+	return qtnf_cmd_send_add_change_intf(vif, iftype, mac_addr,
+			QLINK_CMD_ADD_INTF);
+}
+
+int qtnf_cmd_send_change_intf_type(struct qtnf_vif *vif,
+				   enum nl80211_iftype iftype, u8 *mac_addr)
+{
+	return qtnf_cmd_send_add_change_intf(vif, iftype, mac_addr,
+					     QLINK_CMD_CHANGE_INTF);
+}
+
+int qtnf_cmd_send_del_intf(struct qtnf_vif *vif)
+{
+	struct sk_buff *cmd_skb;
+	struct qlink_cmd_manage_intf *cmd;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_DEL_INTF,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_manage_intf *)cmd_skb->data;
+
+	switch (vif->wdev.iftype) {
+	case NL80211_IFTYPE_AP:
+		cmd->intf_info.if_type = cpu_to_le16(QLINK_IFTYPE_AP);
+		break;
+	case NL80211_IFTYPE_STATION:
+		cmd->intf_info.if_type = cpu_to_le16(QLINK_IFTYPE_STATION);
+		break;
+	default:
+		pr_warn("VIF%u.%u: unsupported iftype %d\n", vif->mac->macid,
+			vif->vifid, vif->wdev.iftype);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	eth_zero_addr(cmd->intf_info.mac_addr);
+
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+static int
+qtnf_cmd_resp_proc_hw_info(struct qtnf_bus *bus,
+			   const struct qlink_resp_get_hw_info *resp)
+{
+	struct qtnf_hw_info *hwinfo = &bus->hw_info;
+
+	hwinfo->num_mac = resp->num_mac;
+	hwinfo->mac_bitmap = resp->mac_bitmap;
+	hwinfo->fw_ver = le32_to_cpu(resp->fw_ver);
+	hwinfo->ql_proto_ver = le16_to_cpu(resp->ql_proto_ver);
+	memcpy(hwinfo->alpha2_code, resp->alpha2_code,
+	       sizeof(hwinfo->alpha2_code));
+	hwinfo->total_tx_chain = resp->total_tx_chain;
+	hwinfo->total_rx_chain = resp->total_rx_chain;
+	hwinfo->hw_capab = le32_to_cpu(resp->hw_capab);
+
+	pr_info("fw_version=%d, MACs map %#x, alpha2=\"%c%c\", chains Tx=%u Rx=%u\n",
+		hwinfo->fw_ver, hwinfo->mac_bitmap,
+		hwinfo->alpha2_code[0], hwinfo->alpha2_code[1],
+		hwinfo->total_tx_chain, hwinfo->total_rx_chain);
+
+	return 0;
+}
+
+static int qtnf_parse_variable_mac_info(struct qtnf_wmac *mac,
+					const u8 *tlv_buf, size_t tlv_buf_size)
+{
+	struct ieee80211_iface_limit *limits = NULL;
+	const struct qlink_iface_limit *limit_record;
+	size_t record_count = 0, rec = 0;
+	u16 tlv_type, tlv_value_len, mask;
+	struct qlink_iface_comb_num *comb;
+	size_t tlv_full_len;
+	const struct qlink_tlv_hdr *tlv;
+
+	mac->macinfo.n_limits = 0;
+
+	tlv = (const struct qlink_tlv_hdr *)tlv_buf;
+	while (tlv_buf_size >= sizeof(struct qlink_tlv_hdr)) {
+		tlv_type = le16_to_cpu(tlv->type);
+		tlv_value_len = le16_to_cpu(tlv->len);
+		tlv_full_len = tlv_value_len + sizeof(struct qlink_tlv_hdr);
+		if (tlv_full_len > tlv_buf_size) {
+			pr_warn("MAC%u: malformed TLV 0x%.2X; LEN: %u\n",
+				mac->macid, tlv_type, tlv_value_len);
+			return -EINVAL;
+		}
+
+		switch (tlv_type) {
+		case QTN_TLV_ID_NUM_IFACE_COMB:
+			if (unlikely(tlv_value_len != sizeof(*comb)))
+				return -EINVAL;
+
+			comb = (void *)tlv->val;
+			record_count = le16_to_cpu(comb->iface_comb_num);
+
+			mac->macinfo.n_limits = record_count;
+			/* free earlier iface limits memory */
+			kfree(mac->macinfo.limits);
+			mac->macinfo.limits =
+				kzalloc(sizeof(*mac->macinfo.limits) *
+					record_count, GFP_KERNEL);
+
+			if (unlikely(!mac->macinfo.limits))
+				return -ENOMEM;
+
+			limits = mac->macinfo.limits;
+			break;
+		case QTN_TLV_ID_IFACE_LIMIT:
+			if (unlikely(!limits)) {
+				pr_warn("MAC%u: limits are not inited\n",
+					mac->macid);
+				return -EINVAL;
+			}
+
+			if (unlikely(tlv_value_len != sizeof(*limit_record))) {
+				pr_warn("MAC%u: record size mismatch\n",
+					mac->macid);
+				return -EINVAL;
+			}
+
+			limit_record = (void *)tlv->val;
+			limits[rec].max = le16_to_cpu(limit_record->max_num);
+			mask = le16_to_cpu(limit_record->type_mask);
+			limits[rec].types = qlink_iface_type_mask_to_nl(mask);
+			/* only AP and STA modes are supported */
+			limits[rec].types &= BIT(NL80211_IFTYPE_AP) |
+					     BIT(NL80211_IFTYPE_STATION);
+
+			pr_debug("MAC%u: MAX: %u; TYPES: %.4X\n", mac->macid,
+				 limits[rec].max, limits[rec].types);
+
+			if (limits[rec].types)
+				rec++;
+			break;
+		default:
+			break;
+		}
+		tlv_buf_size -= tlv_full_len;
+		tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_value_len);
+	}
+
+	if (tlv_buf_size) {
+		pr_warn("MAC%u: malformed TLV buf; bytes left: %zu\n",
+			mac->macid, tlv_buf_size);
+		return -EINVAL;
+	}
+
+	if (mac->macinfo.n_limits != rec) {
+		pr_err("MAC%u: combination mismatch: reported=%zu parsed=%zu\n",
+		       mac->macid, mac->macinfo.n_limits, rec);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void
+qtnf_cmd_resp_proc_mac_info(struct qtnf_wmac *mac,
+			    const struct qlink_resp_get_mac_info *resp_info)
+{
+	struct qtnf_mac_info *mac_info;
+	struct qtnf_vif *vif;
+
+	mac_info = &mac->macinfo;
+
+	mac_info->bands_cap = resp_info->bands_cap;
+	mac_info->phymode_cap = resp_info->phymode_cap;
+	memcpy(&mac_info->dev_mac, &resp_info->dev_mac,
+	       sizeof(mac_info->dev_mac));
+
+	ether_addr_copy(mac->macaddr, mac_info->dev_mac);
+
+	vif = qtnf_mac_get_base_vif(mac);
+	if (vif)
+		ether_addr_copy(vif->mac_addr, mac->macaddr);
+	else
+		pr_err("could not get valid base vif\n");
+
+	mac_info->num_tx_chain = resp_info->num_tx_chain;
+	mac_info->num_rx_chain = resp_info->num_rx_chain;
+
+	mac_info->max_ap_assoc_sta = le16_to_cpu(resp_info->max_ap_assoc_sta);
+	mac_info->radar_detect_widths =
+			qlink_chan_width_mask_to_nl(le16_to_cpu(
+					resp_info->radar_detect_widths));
+
+	memcpy(&mac_info->ht_cap, &resp_info->ht_cap, sizeof(mac_info->ht_cap));
+	memcpy(&mac_info->vht_cap, &resp_info->vht_cap,
+	       sizeof(mac_info->vht_cap));
+}
+
+static int
+qtnf_cmd_resp_fill_channels_info(struct ieee80211_supported_band *band,
+				 struct qlink_resp_get_chan_info *resp,
+				 size_t payload_len)
+{
+	u16 tlv_type;
+	size_t tlv_len;
+	const struct qlink_tlv_hdr *tlv;
+	const struct qlink_tlv_channel *qchan;
+	struct ieee80211_channel *chan;
+	unsigned int chidx = 0;
+	u32 qflags;
+
+	kfree(band->channels);
+	band->channels = NULL;
+
+	band->n_channels = resp->num_chans;
+	if (band->n_channels == 0)
+		return 0;
+
+	band->channels = kcalloc(band->n_channels, sizeof(*chan), GFP_KERNEL);
+	if (!band->channels) {
+		band->n_channels = 0;
+		return -ENOMEM;
+	}
+
+	tlv = (struct qlink_tlv_hdr *)resp->info;
+
+	while (payload_len >= sizeof(*tlv)) {
+		tlv_type = le16_to_cpu(tlv->type);
+		tlv_len = le16_to_cpu(tlv->len) + sizeof(*tlv);
+
+		if (tlv_len > payload_len) {
+			pr_warn("malformed TLV 0x%.2X; LEN: %zu\n",
+				tlv_type, tlv_len);
+			goto error_ret;
+		}
+
+		switch (tlv_type) {
+		case QTN_TLV_ID_CHANNEL:
+			if (unlikely(tlv_len != sizeof(*qchan))) {
+				pr_err("invalid channel TLV len %zu\n",
+				       tlv_len);
+				goto error_ret;
+			}
+
+			if (chidx == band->n_channels) {
+				pr_err("too many channel TLVs\n");
+				goto error_ret;
+			}
+
+			qchan = (const struct qlink_tlv_channel *)tlv;
+			chan = &band->channels[chidx++];
+			qflags = le32_to_cpu(qchan->flags);
+
+			chan->hw_value = le16_to_cpu(qchan->hw_value);
+			chan->band = band->band;
+			chan->center_freq = le16_to_cpu(qchan->center_freq);
+			chan->max_antenna_gain = (int)qchan->max_antenna_gain;
+			chan->max_power = (int)qchan->max_power;
+			chan->max_reg_power = (int)qchan->max_reg_power;
+			chan->beacon_found = qchan->beacon_found;
+			chan->dfs_cac_ms = le32_to_cpu(qchan->dfs_cac_ms);
+			chan->flags = 0;
+
+			if (qflags & QLINK_CHAN_DISABLED)
+				chan->flags |= IEEE80211_CHAN_DISABLED;
+
+			if (qflags & QLINK_CHAN_NO_IR)
+				chan->flags |= IEEE80211_CHAN_NO_IR;
+
+			if (qflags & QLINK_CHAN_NO_HT40PLUS)
+				chan->flags |= IEEE80211_CHAN_NO_HT40PLUS;
+
+			if (qflags & QLINK_CHAN_NO_HT40MINUS)
+				chan->flags |= IEEE80211_CHAN_NO_HT40MINUS;
+
+			if (qflags & QLINK_CHAN_NO_OFDM)
+				chan->flags |= IEEE80211_CHAN_NO_OFDM;
+
+			if (qflags & QLINK_CHAN_NO_80MHZ)
+				chan->flags |= IEEE80211_CHAN_NO_80MHZ;
+
+			if (qflags & QLINK_CHAN_NO_160MHZ)
+				chan->flags |= IEEE80211_CHAN_NO_160MHZ;
+
+			if (qflags & QLINK_CHAN_INDOOR_ONLY)
+				chan->flags |= IEEE80211_CHAN_INDOOR_ONLY;
+
+			if (qflags & QLINK_CHAN_IR_CONCURRENT)
+				chan->flags |= IEEE80211_CHAN_IR_CONCURRENT;
+
+			if (qflags & QLINK_CHAN_NO_20MHZ)
+				chan->flags |= IEEE80211_CHAN_NO_20MHZ;
+
+			if (qflags & QLINK_CHAN_NO_10MHZ)
+				chan->flags |= IEEE80211_CHAN_NO_10MHZ;
+
+			if (qflags & QLINK_CHAN_RADAR) {
+				chan->flags |= IEEE80211_CHAN_RADAR;
+				chan->dfs_state_entered = jiffies;
+
+				if (qchan->dfs_state == QLINK_DFS_USABLE)
+					chan->dfs_state = NL80211_DFS_USABLE;
+				else if (qchan->dfs_state ==
+					QLINK_DFS_AVAILABLE)
+					chan->dfs_state = NL80211_DFS_AVAILABLE;
+				else
+					chan->dfs_state =
+						NL80211_DFS_UNAVAILABLE;
+			}
+
+			pr_debug("chan=%d flags=%#x max_pow=%d max_reg_pow=%d\n",
+				 chan->hw_value, chan->flags, chan->max_power,
+				 chan->max_reg_power);
+			break;
+		default:
+			pr_warn("unknown TLV type: %#x\n", tlv_type);
+			break;
+		}
+
+		payload_len -= tlv_len;
+		tlv = (struct qlink_tlv_hdr *)((u8 *)tlv + tlv_len);
+	}
+
+	if (payload_len) {
+		pr_err("malformed TLV buf; bytes left: %zu\n", payload_len);
+		goto error_ret;
+	}
+
+	if (band->n_channels != chidx) {
+		pr_err("channel count mismatch: reported=%d, parsed=%d\n",
+		       band->n_channels, chidx);
+		goto error_ret;
+	}
+
+	return 0;
+
+error_ret:
+	kfree(band->channels);
+	band->channels = NULL;
+	band->n_channels = 0;
+
+	return -EINVAL;
+}
+
+static int qtnf_cmd_resp_proc_phy_params(struct qtnf_wmac *mac,
+					 const u8 *payload, size_t payload_len)
+{
+	struct qtnf_mac_info *mac_info;
+	struct qlink_tlv_frag_rts_thr *phy_thr;
+	struct qlink_tlv_rlimit *limit;
+	struct qlink_tlv_cclass *class;
+	u16 tlv_type;
+	u16 tlv_value_len;
+	size_t tlv_full_len;
+	const struct qlink_tlv_hdr *tlv;
+
+	mac_info = &mac->macinfo;
+
+	tlv = (struct qlink_tlv_hdr *)payload;
+	while (payload_len >= sizeof(struct qlink_tlv_hdr)) {
+		tlv_type = le16_to_cpu(tlv->type);
+		tlv_value_len = le16_to_cpu(tlv->len);
+		tlv_full_len = tlv_value_len + sizeof(struct qlink_tlv_hdr);
+
+		if (tlv_full_len > payload_len) {
+			pr_warn("MAC%u: malformed TLV 0x%.2X; LEN: %u\n",
+				mac->macid, tlv_type, tlv_value_len);
+			return -EINVAL;
+		}
+
+		switch (tlv_type) {
+		case QTN_TLV_ID_FRAG_THRESH:
+			phy_thr = (void *)tlv;
+			mac_info->frag_thr = (u32)le16_to_cpu(phy_thr->thr);
+			break;
+		case QTN_TLV_ID_RTS_THRESH:
+			phy_thr = (void *)tlv;
+			mac_info->rts_thr = (u32)le16_to_cpu(phy_thr->thr);
+			break;
+		case QTN_TLV_ID_SRETRY_LIMIT:
+			limit = (void *)tlv;
+			mac_info->sretry_limit = limit->rlimit;
+			break;
+		case QTN_TLV_ID_LRETRY_LIMIT:
+			limit = (void *)tlv;
+			mac_info->lretry_limit = limit->rlimit;
+			break;
+		case QTN_TLV_ID_COVERAGE_CLASS:
+			class = (void *)tlv;
+			mac_info->coverage_class = class->cclass;
+			break;
+		default:
+			pr_err("MAC%u: Unknown TLV type: %#x\n", mac->macid,
+			       le16_to_cpu(tlv->type));
+			break;
+		}
+
+		payload_len -= tlv_full_len;
+		tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_value_len);
+	}
+
+	if (payload_len) {
+		pr_warn("MAC%u: malformed TLV buf; bytes left: %zu\n",
+			mac->macid, payload_len);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int qtnf_cmd_get_mac_info(struct qtnf_wmac *mac)
+{
+	struct sk_buff *cmd_skb, *resp_skb = NULL;
+	const struct qlink_resp_get_mac_info *resp;
+	size_t var_data_len;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, QLINK_VIFID_RSVD,
+					    QLINK_CMD_MAC_INFO,
+					    sizeof(struct qlink_cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(mac->bus);
+
+	ret = qtnf_cmd_send_with_reply(mac->bus, cmd_skb, &resp_skb, &res_code,
+				       sizeof(*resp), &var_data_len);
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("MAC%u: CMD failed: %u\n", mac->macid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	resp = (const struct qlink_resp_get_mac_info *)resp_skb->data;
+	qtnf_cmd_resp_proc_mac_info(mac, resp);
+	ret = qtnf_parse_variable_mac_info(mac, resp->var_info, var_data_len);
+
+out:
+	qtnf_bus_unlock(mac->bus);
+	consume_skb(resp_skb);
+
+	return ret;
+}
+
+int qtnf_cmd_get_hw_info(struct qtnf_bus *bus)
+{
+	struct sk_buff *cmd_skb, *resp_skb = NULL;
+	const struct qlink_resp_get_hw_info *resp;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(QLINK_MACID_RSVD, QLINK_VIFID_RSVD,
+					    QLINK_CMD_GET_HW_INFO,
+					    sizeof(struct qlink_cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(bus);
+
+	ret = qtnf_cmd_send_with_reply(bus, cmd_skb, &resp_skb, &res_code,
+				       sizeof(*resp), NULL);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("cmd exec failed: 0x%.4X\n", res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	resp = (const struct qlink_resp_get_hw_info *)resp_skb->data;
+	ret = qtnf_cmd_resp_proc_hw_info(bus, resp);
+
+out:
+	qtnf_bus_unlock(bus);
+	consume_skb(resp_skb);
+
+	return ret;
+}
+
+int qtnf_cmd_get_mac_chan_info(struct qtnf_wmac *mac,
+			       struct ieee80211_supported_band *band)
+{
+	struct sk_buff *cmd_skb, *resp_skb = NULL;
+	size_t info_len;
+	struct qlink_cmd_chans_info_get *cmd;
+	struct qlink_resp_get_chan_info *resp;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+	u8 qband;
+
+	switch (band->band) {
+	case NL80211_BAND_2GHZ:
+		qband = QLINK_BAND_2GHZ;
+		break;
+	case NL80211_BAND_5GHZ:
+		qband = QLINK_BAND_5GHZ;
+		break;
+	case NL80211_BAND_60GHZ:
+		qband = QLINK_BAND_60GHZ;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, 0,
+					    QLINK_CMD_CHANS_INFO_GET,
+					    sizeof(*cmd));
+	if (!cmd_skb)
+		return -ENOMEM;
+
+	cmd = (struct qlink_cmd_chans_info_get *)cmd_skb->data;
+	cmd->band = qband;
+	ret = qtnf_cmd_send_with_reply(mac->bus, cmd_skb, &resp_skb, &res_code,
+				       sizeof(*resp), &info_len);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("MAC%u: CMD failed: %u\n", mac->macid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	resp = (struct qlink_resp_get_chan_info *)resp_skb->data;
+	if (resp->band != qband) {
+		pr_err("MAC%u: reply band %u != cmd band %u\n", mac->macid,
+		       resp->band, qband);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = qtnf_cmd_resp_fill_channels_info(band, resp, info_len);
+
+out:
+	consume_skb(resp_skb);
+
+	return ret;
+}
+
+int qtnf_cmd_send_get_phy_params(struct qtnf_wmac *mac)
+{
+	struct sk_buff *cmd_skb, *resp_skb = NULL;
+	size_t response_size;
+	struct qlink_resp_phy_params *resp;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, 0,
+					    QLINK_CMD_PHY_PARAMS_GET,
+					    sizeof(struct qlink_cmd));
+	if (!cmd_skb)
+		return -ENOMEM;
+
+	qtnf_bus_lock(mac->bus);
+
+	ret = qtnf_cmd_send_with_reply(mac->bus, cmd_skb, &resp_skb, &res_code,
+				       sizeof(*resp), &response_size);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("MAC%u: CMD failed: %u\n", mac->macid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	resp = (struct qlink_resp_phy_params *)resp_skb->data;
+	ret = qtnf_cmd_resp_proc_phy_params(mac, resp->info, response_size);
+
+out:
+	qtnf_bus_unlock(mac->bus);
+	consume_skb(resp_skb);
+
+	return ret;
+}
+
+int qtnf_cmd_send_update_phy_params(struct qtnf_wmac *mac, u32 changed)
+{
+	struct wiphy *wiphy = priv_to_wiphy(mac);
+	struct sk_buff *cmd_skb;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, 0,
+					    QLINK_CMD_PHY_PARAMS_SET,
+					    sizeof(struct qlink_cmd));
+	if (!cmd_skb)
+		return -ENOMEM;
+
+	qtnf_bus_lock(mac->bus);
+
+	if (changed & WIPHY_PARAM_FRAG_THRESHOLD)
+		qtnf_cmd_skb_put_tlv_u16(cmd_skb, QTN_TLV_ID_FRAG_THRESH,
+					 wiphy->frag_threshold);
+	if (changed & WIPHY_PARAM_RTS_THRESHOLD)
+		qtnf_cmd_skb_put_tlv_u16(cmd_skb, QTN_TLV_ID_RTS_THRESH,
+					 wiphy->rts_threshold);
+	if (changed & WIPHY_PARAM_COVERAGE_CLASS)
+		qtnf_cmd_skb_put_tlv_u8(cmd_skb, QTN_TLV_ID_COVERAGE_CLASS,
+					wiphy->coverage_class);
+
+	ret = qtnf_cmd_send(mac->bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("MAC%u: CMD failed: %u\n", mac->macid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+out:
+	qtnf_bus_unlock(mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_init_fw(struct qtnf_bus *bus)
+{
+	struct sk_buff *cmd_skb;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(QLINK_MACID_RSVD, QLINK_VIFID_RSVD,
+					    QLINK_CMD_FW_INIT,
+					    sizeof(struct qlink_cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(bus);
+
+	ret = qtnf_cmd_send(bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("cmd exec failed: 0x%.4X\n", res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+out:
+	qtnf_bus_unlock(bus);
+	return ret;
+}
+
+void qtnf_cmd_send_deinit_fw(struct qtnf_bus *bus)
+{
+	struct sk_buff *cmd_skb;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(QLINK_MACID_RSVD, QLINK_VIFID_RSVD,
+					    QLINK_CMD_FW_DEINIT,
+					    sizeof(struct qlink_cmd));
+	if (!cmd_skb)
+		return;
+
+	qtnf_bus_lock(bus);
+
+	qtnf_cmd_send(bus, cmd_skb, NULL);
+
+	qtnf_bus_unlock(bus);
+}
+
+int qtnf_cmd_send_add_key(struct qtnf_vif *vif, u8 key_index, bool pairwise,
+			  const u8 *mac_addr, struct key_params *params)
+{
+	struct sk_buff *cmd_skb;
+	struct qlink_cmd_add_key *cmd;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_ADD_KEY,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_add_key *)cmd_skb->data;
+
+	if (mac_addr)
+		ether_addr_copy(cmd->addr, mac_addr);
+	else
+		eth_broadcast_addr(cmd->addr);
+
+	cmd->cipher = cpu_to_le32(params->cipher);
+	cmd->key_index = key_index;
+	cmd->pairwise = pairwise;
+
+	if (params->key && params->key_len > 0)
+		qtnf_cmd_skb_put_tlv_arr(cmd_skb, QTN_TLV_ID_KEY,
+					 params->key,
+					 params->key_len);
+
+	if (params->seq && params->seq_len > 0)
+		qtnf_cmd_skb_put_tlv_arr(cmd_skb, QTN_TLV_ID_SEQ,
+					 params->seq,
+					 params->seq_len);
+
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n",
+		       vif->mac->macid, vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_del_key(struct qtnf_vif *vif, u8 key_index, bool pairwise,
+			  const u8 *mac_addr)
+{
+	struct sk_buff *cmd_skb;
+	struct qlink_cmd_del_key *cmd;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_DEL_KEY,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_del_key *)cmd_skb->data;
+
+	if (mac_addr)
+		ether_addr_copy(cmd->addr, mac_addr);
+	else
+		eth_broadcast_addr(cmd->addr);
+
+	cmd->key_index = key_index;
+	cmd->pairwise = pairwise;
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n",
+		       vif->mac->macid, vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_set_default_key(struct qtnf_vif *vif, u8 key_index,
+				  bool unicast, bool multicast)
+{
+	struct sk_buff *cmd_skb;
+	struct qlink_cmd_set_def_key *cmd;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_SET_DEFAULT_KEY,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_set_def_key *)cmd_skb->data;
+	cmd->key_index = key_index;
+	cmd->unicast = unicast;
+	cmd->multicast = multicast;
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_set_default_mgmt_key(struct qtnf_vif *vif, u8 key_index)
+{
+	struct sk_buff *cmd_skb;
+	struct qlink_cmd_set_def_mgmt_key *cmd;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_SET_DEFAULT_MGMT_KEY,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_set_def_mgmt_key *)cmd_skb->data;
+	cmd->key_index = key_index;
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+static u32 qtnf_encode_sta_flags(u32 flags)
+{
+	u32 code = 0;
+
+	if (flags & BIT(NL80211_STA_FLAG_AUTHORIZED))
+		code |= QLINK_STA_FLAG_AUTHORIZED;
+	if (flags & BIT(NL80211_STA_FLAG_SHORT_PREAMBLE))
+		code |= QLINK_STA_FLAG_SHORT_PREAMBLE;
+	if (flags & BIT(NL80211_STA_FLAG_WME))
+		code |= QLINK_STA_FLAG_WME;
+	if (flags & BIT(NL80211_STA_FLAG_MFP))
+		code |= QLINK_STA_FLAG_MFP;
+	if (flags & BIT(NL80211_STA_FLAG_AUTHENTICATED))
+		code |= QLINK_STA_FLAG_AUTHENTICATED;
+	if (flags & BIT(NL80211_STA_FLAG_TDLS_PEER))
+		code |= QLINK_STA_FLAG_TDLS_PEER;
+	if (flags & BIT(NL80211_STA_FLAG_ASSOCIATED))
+		code |= QLINK_STA_FLAG_ASSOCIATED;
+	return code;
+}
+
+int qtnf_cmd_send_change_sta(struct qtnf_vif *vif, const u8 *mac,
+			     struct station_parameters *params)
+{
+	struct sk_buff *cmd_skb;
+	struct qlink_cmd_change_sta *cmd;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_CHANGE_STA,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_change_sta *)cmd_skb->data;
+	ether_addr_copy(cmd->sta_addr, mac);
+	cmd->sta_flags_mask = cpu_to_le32(qtnf_encode_sta_flags(
+					  params->sta_flags_mask));
+	cmd->sta_flags_set = cpu_to_le32(qtnf_encode_sta_flags(
+					 params->sta_flags_set));
+
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_del_sta(struct qtnf_vif *vif,
+			  struct station_del_parameters *params)
+{
+	struct sk_buff *cmd_skb;
+	struct qlink_cmd_del_sta *cmd;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret = 0;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_DEL_STA,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_del_sta *)cmd_skb->data;
+
+	if (params->mac)
+		ether_addr_copy(cmd->sta_addr, params->mac);
+	else
+		eth_broadcast_addr(cmd->sta_addr);	/* flush all stations */
+
+	cmd->subtype = params->subtype;
+	cmd->reason_code = cpu_to_le16(params->reason_code);
+
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_scan(struct qtnf_wmac *mac)
+{
+	struct sk_buff *cmd_skb;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	struct ieee80211_channel *sc;
+	struct cfg80211_scan_request *scan_req = mac->scan_req;
+	struct qlink_tlv_channel *qchan;
+	int n_channels;
+	int count = 0;
+	int ret;
+	u32 flags;
+
+	if (scan_req->n_ssids > QTNF_MAX_SSID_LIST_LENGTH) {
+		pr_err("MAC%u: too many SSIDs in scan request\n", mac->macid);
+		return -EINVAL;
+	}
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, QLINK_VIFID_RSVD,
+					    QLINK_CMD_SCAN,
+					    sizeof(struct qlink_cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(mac->bus);
+
+	if (scan_req->n_ssids != 0) {
+		while (count < scan_req->n_ssids) {
+			qtnf_cmd_skb_put_tlv_arr(cmd_skb, WLAN_EID_SSID,
+				scan_req->ssids[count].ssid,
+				scan_req->ssids[count].ssid_len);
+			count++;
+		}
+	}
+
+	if (scan_req->ie_len != 0)
+		qtnf_cmd_skb_put_tlv_arr(cmd_skb, QTN_TLV_ID_IE_SET,
+					 scan_req->ie,
+					 scan_req->ie_len);
+
+	if (scan_req->n_channels) {
+		n_channels = scan_req->n_channels;
+		count = 0;
+
+		while (n_channels != 0) {
+			sc = scan_req->channels[count];
+			if (sc->flags & IEEE80211_CHAN_DISABLED) {
+				n_channels--;
+				continue;
+			}
+
+			pr_debug("MAC%u: scan chan=%d, freq=%d, flags=%#x\n",
+				 mac->macid, sc->hw_value, sc->center_freq,
+				 sc->flags);
+			qchan = skb_put_zero(cmd_skb, sizeof(*qchan));
+			flags = 0;
+
+			qchan->hdr.type = cpu_to_le16(QTN_TLV_ID_CHANNEL);
+			qchan->hdr.len = cpu_to_le16(sizeof(*qchan) -
+					sizeof(struct qlink_tlv_hdr));
+			qchan->center_freq = cpu_to_le16(sc->center_freq);
+			qchan->hw_value = cpu_to_le16(sc->hw_value);
+
+			if (sc->flags & IEEE80211_CHAN_NO_IR)
+				flags |= QLINK_CHAN_NO_IR;
+
+			if (sc->flags & IEEE80211_CHAN_RADAR)
+				flags |= QLINK_CHAN_RADAR;
+
+			qchan->flags = cpu_to_le32(flags);
+			n_channels--;
+			count++;
+		}
+	}
+
+	ret = qtnf_cmd_send(mac->bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	pr_debug("MAC%u: scan started\n", mac->macid);
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("MAC%u: CMD failed: %u\n", mac->macid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+out:
+	qtnf_bus_unlock(mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_connect(struct qtnf_vif *vif,
+			  struct cfg80211_connect_params *sme)
+{
+	struct sk_buff *cmd_skb;
+	struct qlink_cmd_connect *cmd;
+	struct qtnf_bss_config *bss_cfg = &vif->bss_cfg;
+	struct qlink_auth_encr aen;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret;
+	int i;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_CONNECT,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_connect *)cmd_skb->data;
+
+	ether_addr_copy(cmd->bssid, bss_cfg->bssid);
+
+	if (bss_cfg->chandef.chan)
+		cmd->freq = cpu_to_le16(bss_cfg->chandef.chan->center_freq);
+
+	cmd->bg_scan_period = cpu_to_le16(bss_cfg->bg_scan_period);
+
+	memset(&aen, 0, sizeof(aen));
+	aen.auth_type = bss_cfg->auth_type;
+	aen.privacy = !!bss_cfg->privacy;
+	aen.mfp = bss_cfg->mfp;
+	aen.wpa_versions = cpu_to_le32(bss_cfg->crypto.wpa_versions);
+	aen.cipher_group = cpu_to_le32(bss_cfg->crypto.cipher_group);
+	aen.n_ciphers_pairwise = cpu_to_le32(
+					bss_cfg->crypto.n_ciphers_pairwise);
+
+	for (i = 0; i < QLINK_MAX_NR_CIPHER_SUITES; i++)
+		aen.ciphers_pairwise[i] = cpu_to_le32(
+					bss_cfg->crypto.ciphers_pairwise[i]);
+
+	aen.n_akm_suites = cpu_to_le32(bss_cfg->crypto.n_akm_suites);
+
+	for (i = 0; i < QLINK_MAX_NR_AKM_SUITES; i++)
+		aen.akm_suites[i] = cpu_to_le32(
+					bss_cfg->crypto.akm_suites[i]);
+
+	aen.control_port = bss_cfg->crypto.control_port;
+	aen.control_port_no_encrypt =
+			bss_cfg->crypto.control_port_no_encrypt;
+	aen.control_port_ethertype = cpu_to_le16(be16_to_cpu(
+				bss_cfg->crypto.control_port_ethertype));
+
+	qtnf_cmd_skb_put_tlv_arr(cmd_skb, WLAN_EID_SSID, bss_cfg->ssid,
+				 bss_cfg->ssid_len);
+	qtnf_cmd_skb_put_tlv_arr(cmd_skb, QTN_TLV_ID_CRYPTO, (u8 *)&aen,
+				 sizeof(aen));
+
+	if (sme->ie_len != 0)
+		qtnf_cmd_skb_put_tlv_arr(cmd_skb, QTN_TLV_ID_IE_SET,
+					 sme->ie,
+					 sme->ie_len);
+
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_disconnect(struct qtnf_vif *vif, u16 reason_code)
+{
+	struct sk_buff *cmd_skb;
+	struct qlink_cmd_disconnect *cmd;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_DISCONNECT,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	cmd = (struct qlink_cmd_disconnect *)cmd_skb->data;
+	cmd->reason = cpu_to_le16(reason_code);
+
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
+
+int qtnf_cmd_send_updown_intf(struct qtnf_vif *vif, bool up)
+{
+	struct sk_buff *cmd_skb;
+	struct qlink_cmd_updown *cmd;
+	u16 res_code = QLINK_CMD_RESULT_OK;
+	int ret;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
+					    QLINK_CMD_UPDOWN_INTF,
+					    sizeof(*cmd));
+	if (unlikely(!cmd_skb))
+		return -ENOMEM;
+
+	cmd = (struct qlink_cmd_updown *)cmd_skb->data;
+	cmd->if_up = !!up;
+
+	qtnf_bus_lock(vif->mac->bus);
+
+	ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+
+	if (unlikely(ret))
+		goto out;
+
+	if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
+		pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
+		       vif->vifid, res_code);
+		ret = -EFAULT;
+		goto out;
+	}
+out:
+	qtnf_bus_unlock(vif->mac->bus);
+	return ret;
+}
diff --git a/drivers/net/wireless/quantenna/qtnfmac/commands.h b/drivers/net/wireless/quantenna/qtnfmac/commands.h
new file mode 100644
index 000000000000..6c51854ef5e7
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/commands.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2016 Quantenna Communications, Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef QLINK_COMMANDS_H_
+#define QLINK_COMMANDS_H_
+
+#include <linux/nl80211.h>
+
+#include "core.h"
+#include "bus.h"
+
+int qtnf_cmd_send_init_fw(struct qtnf_bus *bus);
+void qtnf_cmd_send_deinit_fw(struct qtnf_bus *bus);
+int qtnf_cmd_get_hw_info(struct qtnf_bus *bus);
+int qtnf_cmd_get_mac_info(struct qtnf_wmac *mac);
+int qtnf_cmd_send_add_intf(struct qtnf_vif *vif, enum nl80211_iftype iftype,
+			   u8 *mac_addr);
+int qtnf_cmd_send_change_intf_type(struct qtnf_vif *vif,
+				   enum nl80211_iftype iftype, u8 *mac_addr);
+int qtnf_cmd_send_del_intf(struct qtnf_vif *vif);
+int qtnf_cmd_get_mac_chan_info(struct qtnf_wmac *mac,
+			       struct ieee80211_supported_band *band);
+int qtnf_cmd_send_regulatory_config(struct qtnf_wmac *mac, const char *alpha2);
+int qtnf_cmd_send_config_ap(struct qtnf_vif *vif);
+int qtnf_cmd_send_start_ap(struct qtnf_vif *vif);
+int qtnf_cmd_send_stop_ap(struct qtnf_vif *vif);
+int qtnf_cmd_send_register_mgmt(struct qtnf_vif *vif, u16 frame_type, bool reg);
+int qtnf_cmd_send_mgmt_frame(struct qtnf_vif *vif, u32 cookie, u16 flags,
+			     u16 freq, const u8 *buf, size_t len);
+int qtnf_cmd_send_mgmt_set_appie(struct qtnf_vif *vif, u8 frame_type,
+				 const u8 *buf, size_t len);
+int qtnf_cmd_get_sta_info(struct qtnf_vif *vif, const u8 *sta_mac,
+			  struct station_info *sinfo);
+int qtnf_cmd_send_phy_params(struct qtnf_wmac *mac, u16 cmd_action,
+			     void *data_buf);
+int qtnf_cmd_send_add_key(struct qtnf_vif *vif, u8 key_index, bool pairwise,
+			  const u8 *mac_addr, struct key_params *params);
+int qtnf_cmd_send_del_key(struct qtnf_vif *vif, u8 key_index, bool pairwise,
+			  const u8 *mac_addr);
+int qtnf_cmd_send_set_default_key(struct qtnf_vif *vif, u8 key_index,
+				  bool unicast, bool multicast);
+int qtnf_cmd_send_set_default_mgmt_key(struct qtnf_vif *vif, u8 key_index);
+int qtnf_cmd_send_add_sta(struct qtnf_vif *vif, const u8 *mac,
+			  struct station_parameters *params);
+int qtnf_cmd_send_change_sta(struct qtnf_vif *vif, const u8 *mac,
+			     struct station_parameters *params);
+int qtnf_cmd_send_del_sta(struct qtnf_vif *vif,
+			  struct station_del_parameters *params);
+
+int qtnf_cmd_resp_parse(struct qtnf_bus *bus, struct sk_buff *resp_skb);
+int qtnf_cmd_resp_check(const struct qtnf_vif *vif,
+			const struct sk_buff *resp_skb, u16 cmd_id,
+			u16 *result, const u8 **payload, size_t *payload_size);
+int qtnf_cmd_send_scan(struct qtnf_wmac *mac);
+int qtnf_cmd_send_connect(struct qtnf_vif *vif,
+			  struct cfg80211_connect_params *sme);
+int qtnf_cmd_send_disconnect(struct qtnf_vif *vif,
+			     u16 reason_code);
+int qtnf_cmd_send_updown_intf(struct qtnf_vif *vif,
+			      bool up);
+
+#endif /* QLINK_COMMANDS_H_ */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/core.c b/drivers/net/wireless/quantenna/qtnfmac/core.c
new file mode 100644
index 000000000000..f053532c0e87
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/core.c
@@ -0,0 +1,618 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/if_ether.h>
+
+#include "core.h"
+#include "bus.h"
+#include "trans.h"
+#include "commands.h"
+#include "cfg80211.h"
+#include "event.h"
+#include "util.h"
+
+#define QTNF_DMP_MAX_LEN 48
+#define QTNF_PRIMARY_VIF_IDX	0
+
+struct qtnf_frame_meta_info {
+	u8 magic_s;
+	u8 ifidx;
+	u8 macid;
+	u8 magic_e;
+} __packed;
+
+struct qtnf_wmac *qtnf_core_get_mac(const struct qtnf_bus *bus, u8 macid)
+{
+	struct qtnf_wmac *mac = NULL;
+
+	if (unlikely(macid >= QTNF_MAX_MAC)) {
+		pr_err("invalid MAC index %u\n", macid);
+		return NULL;
+	}
+
+	mac = bus->mac[macid];
+
+	if (unlikely(!mac)) {
+		pr_err("MAC%u: not initialized\n", macid);
+		return NULL;
+	}
+
+	return mac;
+}
+
+/* Netdev handler for open.
+ */
+static int qtnf_netdev_open(struct net_device *ndev)
+{
+	netif_carrier_off(ndev);
+	qtnf_netdev_updown(ndev, 1);
+	return 0;
+}
+
+/* Netdev handler for close.
+ */
+static int qtnf_netdev_close(struct net_device *ndev)
+{
+	netif_carrier_off(ndev);
+	qtnf_virtual_intf_cleanup(ndev);
+	qtnf_netdev_updown(ndev, 0);
+	return 0;
+}
+
+/* Netdev handler for data transmission.
+ */
+static int
+qtnf_netdev_hard_start_xmit(struct sk_buff *skb, struct net_device *ndev)
+{
+	struct qtnf_vif *vif;
+	struct qtnf_wmac *mac;
+
+	vif = qtnf_netdev_get_priv(ndev);
+
+	if (unlikely(skb->dev != ndev)) {
+		pr_err_ratelimited("invalid skb->dev");
+		dev_kfree_skb_any(skb);
+		return 0;
+	}
+
+	if (unlikely(vif->wdev.iftype == NL80211_IFTYPE_UNSPECIFIED)) {
+		pr_err_ratelimited("%s: VIF not initialized\n", ndev->name);
+		dev_kfree_skb_any(skb);
+		return 0;
+	}
+
+	mac = vif->mac;
+	if (unlikely(!mac)) {
+		pr_err_ratelimited("%s: NULL mac pointer", ndev->name);
+		dev_kfree_skb_any(skb);
+		return 0;
+	}
+
+	if (!skb->len || (skb->len > ETH_FRAME_LEN)) {
+		pr_err_ratelimited("%s: invalid skb len %d\n", ndev->name,
+				   skb->len);
+		dev_kfree_skb_any(skb);
+		ndev->stats.tx_dropped++;
+		return 0;
+	}
+
+	/* tx path is enabled: reset vif timeout */
+	vif->cons_tx_timeout_cnt = 0;
+
+	return qtnf_bus_data_tx(mac->bus, skb);
+}
+
+/* Netdev handler for getting stats.
+ */
+static struct net_device_stats *qtnf_netdev_get_stats(struct net_device *dev)
+{
+	return &dev->stats;
+}
+
+/* Netdev handler for transmission timeout.
+ */
+static void qtnf_netdev_tx_timeout(struct net_device *ndev)
+{
+	struct qtnf_vif *vif = qtnf_netdev_get_priv(ndev);
+	struct qtnf_wmac *mac;
+	struct qtnf_bus *bus;
+
+	if (unlikely(!vif || !vif->mac || !vif->mac->bus))
+		return;
+
+	mac = vif->mac;
+	bus = mac->bus;
+
+	pr_warn("VIF%u.%u: Tx timeout- %lu\n", mac->macid, vif->vifid, jiffies);
+
+	qtnf_bus_data_tx_timeout(bus, ndev);
+	ndev->stats.tx_errors++;
+
+	if (++vif->cons_tx_timeout_cnt > QTNF_TX_TIMEOUT_TRSHLD) {
+		pr_err("Tx timeout threshold exceeded !\n");
+		pr_err("schedule interface %s reset !\n", netdev_name(ndev));
+		queue_work(bus->workqueue, &vif->reset_work);
+	}
+}
+
+/* Network device ops handlers */
+const struct net_device_ops qtnf_netdev_ops = {
+	.ndo_open = qtnf_netdev_open,
+	.ndo_stop = qtnf_netdev_close,
+	.ndo_start_xmit = qtnf_netdev_hard_start_xmit,
+	.ndo_tx_timeout = qtnf_netdev_tx_timeout,
+	.ndo_get_stats = qtnf_netdev_get_stats,
+};
+
+static int qtnf_mac_init_single_band(struct wiphy *wiphy,
+				     struct qtnf_wmac *mac,
+				     enum nl80211_band band)
+{
+	int ret;
+
+	wiphy->bands[band] = kzalloc(sizeof(*wiphy->bands[band]), GFP_KERNEL);
+	if (!wiphy->bands[band])
+		return -ENOMEM;
+
+	wiphy->bands[band]->band = band;
+
+	ret = qtnf_cmd_get_mac_chan_info(mac, wiphy->bands[band]);
+	if (ret) {
+		pr_err("MAC%u: band %u: failed to get chans info: %d\n",
+		       mac->macid, band, ret);
+		return ret;
+	}
+
+	qtnf_band_init_rates(wiphy->bands[band]);
+	qtnf_band_setup_htvht_caps(&mac->macinfo, wiphy->bands[band]);
+
+	return 0;
+}
+
+static int qtnf_mac_init_bands(struct qtnf_wmac *mac)
+{
+	struct wiphy *wiphy = priv_to_wiphy(mac);
+	int ret = 0;
+
+	if (mac->macinfo.bands_cap & QLINK_BAND_2GHZ) {
+		ret = qtnf_mac_init_single_band(wiphy, mac, NL80211_BAND_2GHZ);
+		if (ret)
+			goto out;
+	}
+
+	if (mac->macinfo.bands_cap & QLINK_BAND_5GHZ) {
+		ret = qtnf_mac_init_single_band(wiphy, mac, NL80211_BAND_5GHZ);
+		if (ret)
+			goto out;
+	}
+
+	if (mac->macinfo.bands_cap & QLINK_BAND_60GHZ)
+		ret = qtnf_mac_init_single_band(wiphy, mac, NL80211_BAND_60GHZ);
+
+out:
+	return ret;
+}
+
+struct qtnf_vif *qtnf_mac_get_free_vif(struct qtnf_wmac *mac)
+{
+	struct qtnf_vif *vif;
+	int i;
+
+	for (i = 0; i < QTNF_MAX_INTF; i++) {
+		vif = &mac->iflist[i];
+		if (vif->wdev.iftype == NL80211_IFTYPE_UNSPECIFIED)
+			return vif;
+	}
+
+	return NULL;
+}
+
+struct qtnf_vif *qtnf_mac_get_base_vif(struct qtnf_wmac *mac)
+{
+	struct qtnf_vif *vif;
+
+	vif = &mac->iflist[QTNF_PRIMARY_VIF_IDX];
+
+	if (vif->wdev.iftype == NL80211_IFTYPE_UNSPECIFIED)
+		return NULL;
+
+	return vif;
+}
+
+static void qtnf_vif_reset_handler(struct work_struct *work)
+{
+	struct qtnf_vif *vif = container_of(work, struct qtnf_vif, reset_work);
+
+	rtnl_lock();
+
+	if (vif->wdev.iftype == NL80211_IFTYPE_UNSPECIFIED) {
+		rtnl_unlock();
+		return;
+	}
+
+	/* stop tx completely */
+	netif_tx_stop_all_queues(vif->netdev);
+	if (netif_carrier_ok(vif->netdev))
+		netif_carrier_off(vif->netdev);
+
+	qtnf_cfg80211_vif_reset(vif);
+
+	rtnl_unlock();
+}
+
+static void qtnf_mac_init_primary_intf(struct qtnf_wmac *mac)
+{
+	struct qtnf_vif *vif = &mac->iflist[QTNF_PRIMARY_VIF_IDX];
+
+	vif->wdev.iftype = NL80211_IFTYPE_AP;
+	vif->bss_priority = QTNF_DEF_BSS_PRIORITY;
+	vif->wdev.wiphy = priv_to_wiphy(mac);
+	INIT_WORK(&vif->reset_work, qtnf_vif_reset_handler);
+	vif->cons_tx_timeout_cnt = 0;
+}
+
+static struct qtnf_wmac *qtnf_core_mac_alloc(struct qtnf_bus *bus,
+					     unsigned int macid)
+{
+	struct wiphy *wiphy;
+	struct qtnf_wmac *mac;
+	unsigned int i;
+
+	wiphy = qtnf_wiphy_allocate(bus);
+	if (!wiphy)
+		return ERR_PTR(-ENOMEM);
+
+	mac = wiphy_priv(wiphy);
+
+	mac->macid = macid;
+	mac->bus = bus;
+
+	for (i = 0; i < QTNF_MAX_INTF; i++) {
+		memset(&mac->iflist[i], 0, sizeof(struct qtnf_vif));
+		mac->iflist[i].wdev.iftype = NL80211_IFTYPE_UNSPECIFIED;
+		mac->iflist[i].mac = mac;
+		mac->iflist[i].vifid = i;
+		qtnf_sta_list_init(&mac->iflist[i].sta_list);
+	}
+
+	qtnf_mac_init_primary_intf(mac);
+	bus->mac[macid] = mac;
+
+	return mac;
+}
+
+int qtnf_core_net_attach(struct qtnf_wmac *mac, struct qtnf_vif *vif,
+			 const char *name, unsigned char name_assign_type,
+			 enum nl80211_iftype iftype)
+{
+	struct wiphy *wiphy = priv_to_wiphy(mac);
+	struct net_device *dev;
+	void *qdev_vif;
+	int ret;
+
+	dev = alloc_netdev_mqs(sizeof(struct qtnf_vif *), name,
+			       name_assign_type, ether_setup, 1, 1);
+	if (!dev) {
+		memset(&vif->wdev, 0, sizeof(vif->wdev));
+		vif->wdev.iftype = NL80211_IFTYPE_UNSPECIFIED;
+		return -ENOMEM;
+	}
+
+	vif->netdev = dev;
+
+	dev->netdev_ops = &qtnf_netdev_ops;
+	dev->needs_free_netdev = true;
+	dev_net_set(dev, wiphy_net(wiphy));
+	dev->ieee80211_ptr = &vif->wdev;
+	dev->ieee80211_ptr->iftype = iftype;
+	ether_addr_copy(dev->dev_addr, vif->mac_addr);
+	SET_NETDEV_DEV(dev, wiphy_dev(wiphy));
+	dev->flags |= IFF_BROADCAST | IFF_MULTICAST;
+	dev->watchdog_timeo = QTNF_DEF_WDOG_TIMEOUT;
+	dev->tx_queue_len = 100;
+
+	qdev_vif = netdev_priv(dev);
+	*((void **)qdev_vif) = vif;
+
+	SET_NETDEV_DEV(dev, mac->bus->dev);
+
+	ret = register_netdevice(dev);
+	if (ret) {
+		free_netdev(dev);
+		vif->wdev.iftype = NL80211_IFTYPE_UNSPECIFIED;
+	}
+
+	return ret;
+}
+
+static void qtnf_core_mac_detach(struct qtnf_bus *bus, unsigned int macid)
+{
+	struct qtnf_wmac *mac;
+	struct wiphy *wiphy;
+	struct qtnf_vif *vif;
+	unsigned int i;
+	enum nl80211_band band;
+
+	mac = bus->mac[macid];
+
+	if (!mac)
+		return;
+
+	wiphy = priv_to_wiphy(mac);
+
+	for (i = 0; i < QTNF_MAX_INTF; i++) {
+		vif = &mac->iflist[i];
+		rtnl_lock();
+		if (vif->netdev &&
+		    vif->wdev.iftype != NL80211_IFTYPE_UNSPECIFIED) {
+			qtnf_virtual_intf_cleanup(vif->netdev);
+			qtnf_del_virtual_intf(wiphy, &vif->wdev);
+		}
+		rtnl_unlock();
+		qtnf_sta_list_free(&vif->sta_list);
+	}
+
+	if (mac->wiphy_registered)
+		wiphy_unregister(wiphy);
+
+	for (band = NL80211_BAND_2GHZ; band < NUM_NL80211_BANDS; ++band) {
+		if (!wiphy->bands[band])
+			continue;
+
+		kfree(wiphy->bands[band]->channels);
+		wiphy->bands[band]->n_channels = 0;
+
+		kfree(wiphy->bands[band]);
+		wiphy->bands[band] = NULL;
+	}
+
+	kfree(mac->macinfo.limits);
+	kfree(wiphy->iface_combinations);
+	wiphy_free(wiphy);
+	bus->mac[macid] = NULL;
+}
+
+static int qtnf_core_mac_attach(struct qtnf_bus *bus, unsigned int macid)
+{
+	struct qtnf_wmac *mac;
+	struct qtnf_vif *vif;
+	int ret;
+
+	if (!(bus->hw_info.mac_bitmap & BIT(macid))) {
+		pr_info("MAC%u is not active in FW\n", macid);
+		return 0;
+	}
+
+	mac = qtnf_core_mac_alloc(bus, macid);
+	if (IS_ERR(mac)) {
+		pr_err("MAC%u allocation failed\n", macid);
+		return PTR_ERR(mac);
+	}
+
+	ret = qtnf_cmd_get_mac_info(mac);
+	if (ret) {
+		pr_err("MAC%u: failed to get info\n", macid);
+		goto error;
+	}
+
+	vif = qtnf_mac_get_base_vif(mac);
+	if (!vif) {
+		pr_err("MAC%u: primary VIF is not ready\n", macid);
+		ret = -EFAULT;
+		goto error;
+	}
+
+	ret = qtnf_cmd_send_add_intf(vif, NL80211_IFTYPE_AP, vif->mac_addr);
+	if (ret) {
+		pr_err("MAC%u: failed to add VIF\n", macid);
+		goto error;
+	}
+
+	ret = qtnf_cmd_send_get_phy_params(mac);
+	if (ret) {
+		pr_err("MAC%u: failed to get PHY settings\n", macid);
+		goto error;
+	}
+
+	ret = qtnf_mac_init_bands(mac);
+	if (ret) {
+		pr_err("MAC%u: failed to init bands\n", macid);
+		goto error;
+	}
+
+	ret = qtnf_wiphy_register(&bus->hw_info, mac);
+	if (ret) {
+		pr_err("MAC%u: wiphy registration failed\n", macid);
+		goto error;
+	}
+
+	mac->wiphy_registered = 1;
+
+	rtnl_lock();
+
+	ret = qtnf_core_net_attach(mac, vif, "wlan%d", NET_NAME_ENUM,
+				   NL80211_IFTYPE_AP);
+	rtnl_unlock();
+
+	if (ret) {
+		pr_err("MAC%u: failed to attach netdev\n", macid);
+		vif->wdev.iftype = NL80211_IFTYPE_UNSPECIFIED;
+		vif->netdev = NULL;
+		goto error;
+	}
+
+	pr_debug("MAC%u initialized\n", macid);
+
+	return 0;
+
+error:
+	qtnf_core_mac_detach(bus, macid);
+	return ret;
+}
+
+int qtnf_core_attach(struct qtnf_bus *bus)
+{
+	unsigned int i;
+	int ret;
+
+	qtnf_trans_init(bus);
+
+	bus->fw_state = QTNF_FW_STATE_BOOT_DONE;
+	qtnf_bus_data_rx_start(bus);
+
+	bus->workqueue = alloc_ordered_workqueue("QTNF_BUS", 0);
+	if (!bus->workqueue) {
+		pr_err("failed to alloc main workqueue\n");
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	INIT_WORK(&bus->event_work, qtnf_event_work_handler);
+
+	ret = qtnf_cmd_send_init_fw(bus);
+	if (ret) {
+		pr_err("failed to init FW: %d\n", ret);
+		goto error;
+	}
+
+	bus->fw_state = QTNF_FW_STATE_ACTIVE;
+
+	ret = qtnf_cmd_get_hw_info(bus);
+	if (ret) {
+		pr_err("failed to get HW info: %d\n", ret);
+		goto error;
+	}
+
+	if (bus->hw_info.ql_proto_ver != QLINK_PROTO_VER) {
+		pr_err("qlink version mismatch %u != %u\n",
+		       QLINK_PROTO_VER, bus->hw_info.ql_proto_ver);
+		ret = -EPROTONOSUPPORT;
+		goto error;
+	}
+
+	if (bus->hw_info.num_mac > QTNF_MAX_MAC) {
+		pr_err("no support for number of MACs=%u\n",
+		       bus->hw_info.num_mac);
+		ret = -ERANGE;
+		goto error;
+	}
+
+	for (i = 0; i < bus->hw_info.num_mac; i++) {
+		ret = qtnf_core_mac_attach(bus, i);
+
+		if (ret) {
+			pr_err("MAC%u: attach failed: %d\n", i, ret);
+			goto error;
+		}
+	}
+
+	return 0;
+
+error:
+	qtnf_core_detach(bus);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(qtnf_core_attach);
+
+void qtnf_core_detach(struct qtnf_bus *bus)
+{
+	unsigned int macid;
+
+	qtnf_bus_data_rx_stop(bus);
+
+	for (macid = 0; macid < QTNF_MAX_MAC; macid++)
+		qtnf_core_mac_detach(bus, macid);
+
+	if (bus->fw_state == QTNF_FW_STATE_ACTIVE)
+		qtnf_cmd_send_deinit_fw(bus);
+
+	bus->fw_state = QTNF_FW_STATE_DEAD;
+
+	if (bus->workqueue) {
+		flush_workqueue(bus->workqueue);
+		destroy_workqueue(bus->workqueue);
+	}
+
+	qtnf_trans_free(bus);
+}
+EXPORT_SYMBOL_GPL(qtnf_core_detach);
+
+static inline int qtnf_is_frame_meta_magic_valid(struct qtnf_frame_meta_info *m)
+{
+	return m->magic_s == 0xAB && m->magic_e == 0xBA;
+}
+
+struct net_device *qtnf_classify_skb(struct qtnf_bus *bus, struct sk_buff *skb)
+{
+	struct qtnf_frame_meta_info *meta;
+	struct net_device *ndev = NULL;
+	struct qtnf_wmac *mac;
+	struct qtnf_vif *vif;
+
+	meta = (struct qtnf_frame_meta_info *)
+		(skb_tail_pointer(skb) - sizeof(*meta));
+
+	if (unlikely(!qtnf_is_frame_meta_magic_valid(meta))) {
+		pr_err_ratelimited("invalid magic 0x%x:0x%x\n",
+				   meta->magic_s, meta->magic_e);
+		goto out;
+	}
+
+	if (unlikely(meta->macid >= QTNF_MAX_MAC)) {
+		pr_err_ratelimited("invalid mac(%u)\n", meta->macid);
+		goto out;
+	}
+
+	if (unlikely(meta->ifidx >= QTNF_MAX_INTF)) {
+		pr_err_ratelimited("invalid vif(%u)\n", meta->ifidx);
+		goto out;
+	}
+
+	mac = bus->mac[meta->macid];
+
+	if (unlikely(!mac)) {
+		pr_err_ratelimited("mac(%d) does not exist\n", meta->macid);
+		goto out;
+	}
+
+	vif = &mac->iflist[meta->ifidx];
+
+	if (unlikely(vif->wdev.iftype == NL80211_IFTYPE_UNSPECIFIED)) {
+		pr_err_ratelimited("vif(%u) does not exists\n", meta->ifidx);
+		goto out;
+	}
+
+	ndev = vif->netdev;
+
+	if (unlikely(!ndev)) {
+		pr_err_ratelimited("netdev for wlan%u.%u does not exists\n",
+				   meta->macid, meta->ifidx);
+		goto out;
+	}
+
+	__skb_trim(skb, skb->len - sizeof(*meta));
+
+out:
+	return ndev;
+}
+EXPORT_SYMBOL_GPL(qtnf_classify_skb);
+
+MODULE_AUTHOR("Quantenna Communications");
+MODULE_DESCRIPTION("Quantenna 802.11 wireless LAN FullMAC driver.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/wireless/quantenna/qtnfmac/core.h b/drivers/net/wireless/quantenna/qtnfmac/core.h
new file mode 100644
index 000000000000..a616434281cf
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/core.h
@@ -0,0 +1,173 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _QTN_FMAC_CORE_H_
+#define _QTN_FMAC_CORE_H_
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/semaphore.h>
+#include <linux/ip.h>
+#include <linux/skbuff.h>
+#include <linux/if_arp.h>
+#include <linux/etherdevice.h>
+#include <net/sock.h>
+#include <net/lib80211.h>
+#include <net/cfg80211.h>
+#include <linux/vmalloc.h>
+#include <linux/firmware.h>
+#include <linux/ctype.h>
+#include <linux/workqueue.h>
+#include <linux/slab.h>
+
+#include "qlink.h"
+#include "trans.h"
+
+#undef pr_fmt
+#define pr_fmt(fmt)	KBUILD_MODNAME ": %s: " fmt, __func__
+
+#define QTNF_MAX_SSID_LIST_LENGTH	2
+#define QTNF_MAX_VSIE_LEN		255
+#define QTNF_MAX_ALPHA_LEN		2
+#define QTNF_MAX_INTF			8
+#define QTNF_MAX_EVENT_QUEUE_LEN	255
+#define QTNF_DEFAULT_BG_SCAN_PERIOD	300
+#define QTNF_MAX_BG_SCAN_PERIOD		0xffff
+
+#define QTNF_DEF_BSS_PRIORITY		0
+#define QTNF_DEF_WDOG_TIMEOUT		5
+#define QTNF_TX_TIMEOUT_TRSHLD		100
+
+#define QTNF_STATE_AP_CONFIG		BIT(2)
+#define QTNF_STATE_AP_START		BIT(1)
+
+extern const struct net_device_ops qtnf_netdev_ops;
+struct qtnf_bus;
+struct qtnf_vif;
+
+struct qtnf_bss_config {
+	u8 ssid[IEEE80211_MAX_SSID_LEN];
+	u8 bssid[ETH_ALEN];
+	size_t ssid_len;
+	u8 dtim;
+	u16 bcn_period;
+	u16 auth_type;
+	bool privacy;
+	enum nl80211_mfp mfp;
+	struct cfg80211_chan_def chandef;
+	struct cfg80211_crypto_settings crypto;
+	u16 bg_scan_period;
+	u32 connect_flags;
+};
+
+struct qtnf_sta_node {
+	struct list_head list;
+	u8 mac_addr[ETH_ALEN];
+};
+
+struct qtnf_sta_list {
+	struct list_head head;
+	atomic_t size;
+};
+
+enum qtnf_sta_state {
+	QTNF_STA_DISCONNECTED,
+	QTNF_STA_CONNECTING,
+	QTNF_STA_CONNECTED
+};
+
+struct qtnf_vif {
+	struct wireless_dev wdev;
+	u8 vifid;
+	u8 bss_priority;
+	u8 bss_status;
+	enum qtnf_sta_state sta_state;
+	u16 mgmt_frames_bitmask;
+	struct net_device *netdev;
+	struct qtnf_wmac *mac;
+	u8 mac_addr[ETH_ALEN];
+	struct work_struct reset_work;
+	struct qtnf_bss_config bss_cfg;
+	struct qtnf_sta_list sta_list;
+	unsigned long cons_tx_timeout_cnt;
+};
+
+struct qtnf_mac_info {
+	u8 bands_cap;
+	u8 phymode_cap;
+	u8 dev_mac[ETH_ALEN];
+	u8 num_tx_chain;
+	u8 num_rx_chain;
+	u16 max_ap_assoc_sta;
+	u32 frag_thr;
+	u32 rts_thr;
+	u8 lretry_limit;
+	u8 sretry_limit;
+	u8 coverage_class;
+	u8 radar_detect_widths;
+	struct ieee80211_ht_cap ht_cap;
+	struct ieee80211_vht_cap vht_cap;
+	struct ieee80211_iface_limit *limits;
+	size_t n_limits;
+};
+
+struct qtnf_wmac {
+	u8 macid;
+	u8 wiphy_registered;
+	u8 macaddr[ETH_ALEN];
+	struct qtnf_bus *bus;
+	struct qtnf_mac_info macinfo;
+	struct qtnf_vif iflist[QTNF_MAX_INTF];
+	struct cfg80211_scan_request *scan_req;
+};
+
+struct qtnf_hw_info {
+	u8 num_mac;
+	u8 mac_bitmap;
+	u8 alpha2_code[QTNF_MAX_ALPHA_LEN];
+	u32 fw_ver;
+	u16 ql_proto_ver;
+	u8 total_tx_chain;
+	u8 total_rx_chain;
+	u32 hw_capab;
+};
+
+struct qtnf_vif *qtnf_mac_get_free_vif(struct qtnf_wmac *mac);
+struct qtnf_vif *qtnf_mac_get_base_vif(struct qtnf_wmac *mac);
+struct wiphy *qtnf_wiphy_allocate(struct qtnf_bus *bus);
+int qtnf_core_net_attach(struct qtnf_wmac *mac, struct qtnf_vif *priv,
+			 const char *name, unsigned char name_assign_type,
+			 enum nl80211_iftype iftype);
+void qtnf_main_work_queue(struct work_struct *work);
+int qtnf_cmd_send_update_phy_params(struct qtnf_wmac *mac, u32 changed);
+int qtnf_cmd_send_get_phy_params(struct qtnf_wmac *mac);
+
+struct qtnf_wmac *qtnf_core_get_mac(const struct qtnf_bus *bus, u8 macid);
+struct net_device *qtnf_classify_skb(struct qtnf_bus *bus, struct sk_buff *skb);
+struct net_device *qtnf_classify_skb_no_mbss(struct qtnf_bus *bus,
+					     struct sk_buff *skb);
+
+void qtnf_virtual_intf_cleanup(struct net_device *ndev);
+
+void qtnf_netdev_updown(struct net_device *ndev, bool up);
+
+static inline struct qtnf_vif *qtnf_netdev_get_priv(struct net_device *dev)
+{
+	return *((void **)netdev_priv(dev));
+}
+
+#endif /* _QTN_FMAC_CORE_H_ */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/debug.c b/drivers/net/wireless/quantenna/qtnfmac/debug.c
new file mode 100644
index 000000000000..9f826b9ef5d9
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/debug.c
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include "debug.h"
+
+#undef pr_fmt
+#define pr_fmt(fmt)	"qtnfmac dbg: %s: " fmt, __func__
+
+void qtnf_debugfs_init(struct qtnf_bus *bus, const char *name)
+{
+	bus->dbg_dir = debugfs_create_dir(name, NULL);
+
+	if (IS_ERR_OR_NULL(bus->dbg_dir)) {
+		pr_warn("failed to create debugfs root dir\n");
+		bus->dbg_dir = NULL;
+	}
+}
+
+void qtnf_debugfs_remove(struct qtnf_bus *bus)
+{
+	debugfs_remove_recursive(bus->dbg_dir);
+	bus->dbg_dir = NULL;
+}
+
+void qtnf_debugfs_add_entry(struct qtnf_bus *bus, const char *name,
+			    int (*fn)(struct seq_file *seq, void *data))
+{
+	struct dentry *entry;
+
+	entry = debugfs_create_devm_seqfile(bus->dev, name, bus->dbg_dir, fn);
+	if (IS_ERR_OR_NULL(entry))
+		pr_warn("failed to add entry (%s)\n", name);
+}
diff --git a/drivers/net/wireless/quantenna/qtnfmac/debug.h b/drivers/net/wireless/quantenna/qtnfmac/debug.h
new file mode 100644
index 000000000000..d6dd12b5d434
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/debug.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _QTN_FMAC_DEBUG_H_
+#define _QTN_FMAC_DEBUG_H_
+
+#include <linux/debugfs.h>
+
+#include "core.h"
+#include "bus.h"
+
+#ifdef CONFIG_DEBUG_FS
+
+void qtnf_debugfs_init(struct qtnf_bus *bus, const char *name);
+void qtnf_debugfs_remove(struct qtnf_bus *bus);
+void qtnf_debugfs_add_entry(struct qtnf_bus *bus, const char *name,
+			    int (*fn)(struct seq_file *seq, void *data));
+
+#else
+
+static inline void qtnf_debugfs_init(struct qtnf_bus *bus, const char *name)
+{
+}
+
+static inline void qtnf_debugfs_remove(struct qtnf_bus *bus)
+{
+}
+
+static inline void
+qtnf_debugfs_add_entry(struct qtnf_bus *bus, const char *name,
+		       int (*fn)(struct seq_file *seq, void *data))
+{
+}
+
+#endif /* CONFIG_DEBUG_FS */
+
+#endif /* _QTN_FMAC_DEBUG_H_ */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/event.c b/drivers/net/wireless/quantenna/qtnfmac/event.c
new file mode 100644
index 000000000000..9b61e9a83670
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/event.c
@@ -0,0 +1,452 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#include "cfg80211.h"
+#include "core.h"
+#include "qlink.h"
+#include "bus.h"
+#include "trans.h"
+#include "util.h"
+#include "event.h"
+
+static int
+qtnf_event_handle_sta_assoc(struct qtnf_wmac *mac, struct qtnf_vif *vif,
+			    const struct qlink_event_sta_assoc *sta_assoc,
+			    u16 len)
+{
+	const u8 *sta_addr;
+	u16 frame_control;
+	struct station_info sinfo = { 0 };
+	size_t payload_len;
+	u16 tlv_type;
+	u16 tlv_value_len;
+	size_t tlv_full_len;
+	const struct qlink_tlv_hdr *tlv;
+
+	if (unlikely(len < sizeof(*sta_assoc))) {
+		pr_err("VIF%u.%u: payload is too short (%u < %zu)\n",
+		       mac->macid, vif->vifid, len, sizeof(*sta_assoc));
+		return -EINVAL;
+	}
+
+	if (vif->wdev.iftype != NL80211_IFTYPE_AP) {
+		pr_err("VIF%u.%u: STA_ASSOC event when not in AP mode\n",
+		       mac->macid, vif->vifid);
+		return -EPROTO;
+	}
+
+	if (!(vif->bss_status & QTNF_STATE_AP_START)) {
+		pr_err("VIF%u.%u: STA_ASSOC event when AP is not started\n",
+		       mac->macid, vif->vifid);
+		return -EPROTO;
+	}
+
+	sta_addr = sta_assoc->sta_addr;
+	frame_control = le16_to_cpu(sta_assoc->frame_control);
+
+	pr_debug("VIF%u.%u: MAC:%pM FC:%x\n", mac->macid, vif->vifid, sta_addr,
+		 frame_control);
+
+	qtnf_sta_list_add(&vif->sta_list, sta_addr);
+
+	sinfo.assoc_req_ies = NULL;
+	sinfo.assoc_req_ies_len = 0;
+
+	payload_len = len - sizeof(*sta_assoc);
+	tlv = (struct qlink_tlv_hdr *)sta_assoc->ies;
+
+	while (payload_len >= sizeof(struct qlink_tlv_hdr)) {
+		tlv_type = le16_to_cpu(tlv->type);
+		tlv_value_len = le16_to_cpu(tlv->len);
+		tlv_full_len = tlv_value_len + sizeof(struct qlink_tlv_hdr);
+
+		if (tlv_full_len > payload_len) {
+			pr_warn("VIF%u.%u: malformed TLV 0x%.2X; LEN: %u\n",
+				mac->macid, vif->vifid, tlv_type,
+				tlv_value_len);
+			return -EINVAL;
+		}
+
+		if (tlv_type == QTN_TLV_ID_IE_SET) {
+			sinfo.assoc_req_ies = tlv->val;
+			sinfo.assoc_req_ies_len = tlv_value_len;
+		}
+
+		payload_len -= tlv_full_len;
+		tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_value_len);
+	}
+
+	if (payload_len) {
+		pr_warn("VIF%u.%u: malformed TLV buf; bytes left: %zu\n",
+			mac->macid, vif->vifid, payload_len);
+		return -EINVAL;
+	}
+
+	cfg80211_new_sta(vif->netdev, sta_assoc->sta_addr, &sinfo,
+			 GFP_KERNEL);
+
+	return 0;
+}
+
+static int
+qtnf_event_handle_sta_deauth(struct qtnf_wmac *mac, struct qtnf_vif *vif,
+			     const struct qlink_event_sta_deauth *sta_deauth,
+			     u16 len)
+{
+	const u8 *sta_addr;
+	u16 reason;
+
+	if (unlikely(len < sizeof(*sta_deauth))) {
+		pr_err("VIF%u.%u: payload is too short (%u < %zu)\n",
+		       mac->macid, vif->vifid, len,
+		       sizeof(struct qlink_event_sta_deauth));
+		return -EINVAL;
+	}
+
+	if (vif->wdev.iftype != NL80211_IFTYPE_AP) {
+		pr_err("VIF%u.%u: STA_DEAUTH event when not in AP mode\n",
+		       mac->macid, vif->vifid);
+		return -EPROTO;
+	}
+
+	if (!(vif->bss_status & QTNF_STATE_AP_START)) {
+		pr_err("VIF%u.%u: STA_DEAUTH event when AP is not started\n",
+		       mac->macid, vif->vifid);
+		return -EPROTO;
+	}
+
+	sta_addr = sta_deauth->sta_addr;
+	reason = le16_to_cpu(sta_deauth->reason);
+
+	pr_debug("VIF%u.%u: MAC:%pM reason:%x\n", mac->macid, vif->vifid,
+		 sta_addr, reason);
+
+	if (qtnf_sta_list_del(&vif->sta_list, sta_addr))
+		cfg80211_del_sta(vif->netdev, sta_deauth->sta_addr,
+				 GFP_KERNEL);
+
+	return 0;
+}
+
+static int
+qtnf_event_handle_bss_join(struct qtnf_vif *vif,
+			   const struct qlink_event_bss_join *join_info,
+			   u16 len)
+{
+	if (unlikely(len < sizeof(*join_info))) {
+		pr_err("VIF%u.%u: payload is too short (%u < %zu)\n",
+		       vif->mac->macid, vif->vifid, len,
+		       sizeof(struct qlink_event_bss_join));
+		return -EINVAL;
+	}
+
+	if (vif->wdev.iftype != NL80211_IFTYPE_STATION) {
+		pr_err("VIF%u.%u: BSS_JOIN event when not in STA mode\n",
+		       vif->mac->macid, vif->vifid);
+		return -EPROTO;
+	}
+
+	if (vif->sta_state != QTNF_STA_CONNECTING) {
+		pr_err("VIF%u.%u: BSS_JOIN event when STA is not connecting\n",
+		       vif->mac->macid, vif->vifid);
+		return -EPROTO;
+	}
+
+	pr_debug("VIF%u.%u: BSSID:%pM\n", vif->mac->macid, vif->vifid,
+		 join_info->bssid);
+
+	cfg80211_connect_result(vif->netdev, join_info->bssid, NULL, 0, NULL,
+				0, le16_to_cpu(join_info->status), GFP_KERNEL);
+
+	if (le16_to_cpu(join_info->status) == WLAN_STATUS_SUCCESS) {
+		vif->sta_state = QTNF_STA_CONNECTED;
+		netif_carrier_on(vif->netdev);
+	} else {
+		vif->sta_state = QTNF_STA_DISCONNECTED;
+	}
+
+	return 0;
+}
+
+static int
+qtnf_event_handle_bss_leave(struct qtnf_vif *vif,
+			    const struct qlink_event_bss_leave *leave_info,
+			    u16 len)
+{
+	if (unlikely(len < sizeof(*leave_info))) {
+		pr_err("VIF%u.%u: payload is too short (%u < %zu)\n",
+		       vif->mac->macid, vif->vifid, len,
+		       sizeof(struct qlink_event_bss_leave));
+		return -EINVAL;
+	}
+
+	if (vif->wdev.iftype != NL80211_IFTYPE_STATION) {
+		pr_err("VIF%u.%u: BSS_LEAVE event when not in STA mode\n",
+		       vif->mac->macid, vif->vifid);
+		return -EPROTO;
+	}
+
+	if (vif->sta_state != QTNF_STA_CONNECTED) {
+		pr_err("VIF%u.%u: BSS_LEAVE event when STA is not connected\n",
+		       vif->mac->macid, vif->vifid);
+		return -EPROTO;
+	}
+
+	pr_debug("VIF%u.%u: disconnected\n", vif->mac->macid, vif->vifid);
+
+	cfg80211_disconnected(vif->netdev, leave_info->reason, NULL, 0, 0,
+			      GFP_KERNEL);
+
+	vif->sta_state = QTNF_STA_DISCONNECTED;
+	netif_carrier_off(vif->netdev);
+
+	return 0;
+}
+
+static int
+qtnf_event_handle_mgmt_received(struct qtnf_vif *vif,
+				const struct qlink_event_rxmgmt *rxmgmt,
+				u16 len)
+{
+	const size_t min_len = sizeof(*rxmgmt) +
+			       sizeof(struct ieee80211_hdr_3addr);
+	const struct ieee80211_hdr_3addr *frame = (void *)rxmgmt->frame_data;
+	const u16 frame_len = len - sizeof(*rxmgmt);
+	enum nl80211_rxmgmt_flags flags = 0;
+
+	if (unlikely(len < min_len)) {
+		pr_err("VIF%u.%u: payload is too short (%u < %zu)\n",
+		       vif->mac->macid, vif->vifid, len, min_len);
+		return -EINVAL;
+	}
+
+	if (le32_to_cpu(rxmgmt->flags) & QLINK_RXMGMT_FLAG_ANSWERED)
+		flags |= NL80211_RXMGMT_FLAG_ANSWERED;
+
+	pr_debug("%s LEN:%u FC:%.4X SA:%pM\n", vif->netdev->name, frame_len,
+		 le16_to_cpu(frame->frame_control), frame->addr2);
+
+	cfg80211_rx_mgmt(&vif->wdev, le32_to_cpu(rxmgmt->freq),
+			 le32_to_cpu(rxmgmt->sig_dbm), rxmgmt->frame_data,
+			 frame_len, flags);
+
+	return 0;
+}
+
+static int
+qtnf_event_handle_scan_results(struct qtnf_vif *vif,
+			       const struct qlink_event_scan_result *sr,
+			       u16 len)
+{
+	struct cfg80211_bss *bss;
+	struct ieee80211_channel *channel;
+	struct wiphy *wiphy = priv_to_wiphy(vif->mac);
+	enum cfg80211_bss_frame_type frame_type;
+	size_t payload_len;
+	u16 tlv_type;
+	u16 tlv_value_len;
+	size_t tlv_full_len;
+	const struct qlink_tlv_hdr *tlv;
+
+	const u8 *ies = NULL;
+	size_t ies_len = 0;
+
+	if (len < sizeof(*sr)) {
+		pr_err("VIF%u.%u: payload is too short\n", vif->mac->macid,
+		       vif->vifid);
+		return -EINVAL;
+	}
+
+	channel = ieee80211_get_channel(wiphy, le16_to_cpu(sr->freq));
+	if (!channel) {
+		pr_err("VIF%u.%u: channel at %u MHz not found\n",
+		       vif->mac->macid, vif->vifid, le16_to_cpu(sr->freq));
+		return -EINVAL;
+	}
+
+	switch (sr->frame_type) {
+	case QLINK_BSS_FTYPE_BEACON:
+		frame_type = CFG80211_BSS_FTYPE_BEACON;
+		break;
+	case QLINK_BSS_FTYPE_PRESP:
+		frame_type = CFG80211_BSS_FTYPE_PRESP;
+		break;
+	default:
+		frame_type = CFG80211_BSS_FTYPE_UNKNOWN;
+	}
+
+	payload_len = len - sizeof(*sr);
+	tlv = (struct qlink_tlv_hdr *)sr->payload;
+
+	while (payload_len >= sizeof(struct qlink_tlv_hdr)) {
+		tlv_type = le16_to_cpu(tlv->type);
+		tlv_value_len = le16_to_cpu(tlv->len);
+		tlv_full_len = tlv_value_len + sizeof(struct qlink_tlv_hdr);
+
+		if (tlv_full_len > payload_len) {
+			pr_warn("VIF%u.%u: malformed TLV 0x%.2X; LEN: %u\n",
+				vif->mac->macid, vif->vifid, tlv_type,
+				tlv_value_len);
+			return -EINVAL;
+		}
+
+		if (tlv_type == QTN_TLV_ID_IE_SET) {
+			ies = tlv->val;
+			ies_len = tlv_value_len;
+		}
+
+		payload_len -= tlv_full_len;
+		tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_value_len);
+	}
+
+	if (payload_len) {
+		pr_warn("VIF%u.%u: malformed TLV buf; bytes left: %zu\n",
+			vif->mac->macid, vif->vifid, payload_len);
+		return -EINVAL;
+	}
+
+	bss = cfg80211_inform_bss(wiphy, channel, frame_type,
+				  sr->bssid, get_unaligned_le64(&sr->tsf),
+				  le16_to_cpu(sr->capab),
+				  le16_to_cpu(sr->bintval), ies, ies_len,
+				  sr->signal, GFP_KERNEL);
+	if (!bss)
+		return -ENOMEM;
+
+	cfg80211_put_bss(wiphy, bss);
+
+	return 0;
+}
+
+static int
+qtnf_event_handle_scan_complete(struct qtnf_wmac *mac,
+				const struct qlink_event_scan_complete *status,
+				u16 len)
+{
+	if (len < sizeof(*status)) {
+		pr_err("MAC%u: payload is too short\n", mac->macid);
+		return -EINVAL;
+	}
+
+	qtnf_scan_done(mac, le32_to_cpu(status->flags) & QLINK_SCAN_ABORTED);
+
+	return 0;
+}
+
+static int qtnf_event_parse(struct qtnf_wmac *mac,
+			    const struct sk_buff *event_skb)
+{
+	const struct qlink_event *event;
+	struct qtnf_vif *vif = NULL;
+	int ret = -1;
+	u16 event_id;
+	u16 event_len;
+
+	event = (const struct qlink_event *)event_skb->data;
+	event_id = le16_to_cpu(event->event_id);
+	event_len = le16_to_cpu(event->mhdr.len);
+
+	if (likely(event->vifid < QTNF_MAX_INTF)) {
+		vif = &mac->iflist[event->vifid];
+	} else {
+		pr_err("invalid vif(%u)\n", event->vifid);
+		return -EINVAL;
+	}
+
+	switch (event_id) {
+	case QLINK_EVENT_STA_ASSOCIATED:
+		ret = qtnf_event_handle_sta_assoc(mac, vif, (const void *)event,
+						  event_len);
+		break;
+	case QLINK_EVENT_STA_DEAUTH:
+		ret = qtnf_event_handle_sta_deauth(mac, vif,
+						   (const void *)event,
+						   event_len);
+		break;
+	case QLINK_EVENT_MGMT_RECEIVED:
+		ret = qtnf_event_handle_mgmt_received(vif, (const void *)event,
+						      event_len);
+		break;
+	case QLINK_EVENT_SCAN_RESULTS:
+		ret = qtnf_event_handle_scan_results(vif, (const void *)event,
+						     event_len);
+		break;
+	case QLINK_EVENT_SCAN_COMPLETE:
+		ret = qtnf_event_handle_scan_complete(mac, (const void *)event,
+						      event_len);
+		break;
+	case QLINK_EVENT_BSS_JOIN:
+		ret = qtnf_event_handle_bss_join(vif, (const void *)event,
+						 event_len);
+		break;
+	case QLINK_EVENT_BSS_LEAVE:
+		ret = qtnf_event_handle_bss_leave(vif, (const void *)event,
+						  event_len);
+		break;
+	default:
+		pr_warn("unknown event type: %x\n", event_id);
+		break;
+	}
+
+	return ret;
+}
+
+static int qtnf_event_process_skb(struct qtnf_bus *bus,
+				  const struct sk_buff *skb)
+{
+	const struct qlink_event *event;
+	struct qtnf_wmac *mac;
+	int res;
+
+	if (unlikely(!skb || skb->len < sizeof(*event))) {
+		pr_err("invalid event buffer\n");
+		return -EINVAL;
+	}
+
+	event = (struct qlink_event *)skb->data;
+
+	mac = qtnf_core_get_mac(bus, event->macid);
+
+	pr_debug("new event id:%x len:%u mac:%u vif:%u\n",
+		 le16_to_cpu(event->event_id), le16_to_cpu(event->mhdr.len),
+		 event->macid, event->vifid);
+
+	if (unlikely(!mac))
+		return -ENXIO;
+
+	qtnf_bus_lock(bus);
+	res = qtnf_event_parse(mac, skb);
+	qtnf_bus_unlock(bus);
+
+	return res;
+}
+
+void qtnf_event_work_handler(struct work_struct *work)
+{
+	struct qtnf_bus *bus = container_of(work, struct qtnf_bus, event_work);
+	struct sk_buff_head *event_queue = &bus->trans.event_queue;
+	struct sk_buff *current_event_skb = skb_dequeue(event_queue);
+
+	while (current_event_skb) {
+		qtnf_event_process_skb(bus, current_event_skb);
+		dev_kfree_skb_any(current_event_skb);
+		current_event_skb = skb_dequeue(event_queue);
+	}
+}
diff --git a/drivers/net/wireless/quantenna/qtnfmac/event.h b/drivers/net/wireless/quantenna/qtnfmac/event.h
new file mode 100644
index 000000000000..ae759b602c2a
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/event.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _QTN_FMAC_EVENT_H_
+#define _QTN_FMAC_EVENT_H_
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include "qlink.h"
+
+void qtnf_event_work_handler(struct work_struct *work);
+
+#endif /* _QTN_FMAC_EVENT_H_ */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/pearl/pcie.c b/drivers/net/wireless/quantenna/qtnfmac/pearl/pcie.c
new file mode 100644
index 000000000000..7fc4f0d6a9ad
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/pearl/pcie.c
@@ -0,0 +1,1378 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/firmware.h>
+#include <linux/pci.h>
+#include <linux/vmalloc.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/sched.h>
+#include <linux/completion.h>
+#include <linux/crc32.h>
+#include <linux/spinlock.h>
+
+#include "qtn_hw_ids.h"
+#include "pcie_bus_priv.h"
+#include "core.h"
+#include "bus.h"
+#include "debug.h"
+
+static bool use_msi = true;
+module_param(use_msi, bool, 0644);
+MODULE_PARM_DESC(use_msi, "set 0 to use legacy interrupt");
+
+static unsigned int tx_bd_size_param = 256;
+module_param(tx_bd_size_param, uint, 0644);
+MODULE_PARM_DESC(tx_bd_size_param, "Tx descriptors queue size");
+
+static unsigned int rx_bd_size_param = 256;
+module_param(rx_bd_size_param, uint, 0644);
+MODULE_PARM_DESC(rx_bd_size_param, "Rx descriptors queue size");
+
+static unsigned int rx_bd_reserved_param = 16;
+module_param(rx_bd_reserved_param, uint, 0644);
+MODULE_PARM_DESC(rx_bd_reserved_param, "Reserved RX descriptors");
+
+static u8 flashboot = 1;
+module_param(flashboot, byte, 0644);
+MODULE_PARM_DESC(flashboot, "set to 0 to use FW binary file on FS");
+
+#define DRV_NAME	"qtnfmac_pearl_pcie"
+
+static inline void qtnf_non_posted_write(u32 val, void __iomem *basereg)
+{
+	writel(val, basereg);
+
+	/* flush posted write */
+	readl(basereg);
+}
+
+static inline void qtnf_init_hdp_irqs(struct qtnf_pcie_bus_priv *priv)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->irq_lock, flags);
+	priv->pcie_irq_mask = (PCIE_HDP_INT_RX_BITS | PCIE_HDP_INT_TX_BITS);
+	spin_unlock_irqrestore(&priv->irq_lock, flags);
+}
+
+static inline void qtnf_enable_hdp_irqs(struct qtnf_pcie_bus_priv *priv)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->irq_lock, flags);
+	writel(priv->pcie_irq_mask, PCIE_HDP_INT_EN(priv->pcie_reg_base));
+	spin_unlock_irqrestore(&priv->irq_lock, flags);
+}
+
+static inline void qtnf_disable_hdp_irqs(struct qtnf_pcie_bus_priv *priv)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->irq_lock, flags);
+	writel(0x0, PCIE_HDP_INT_EN(priv->pcie_reg_base));
+	spin_unlock_irqrestore(&priv->irq_lock, flags);
+}
+
+static inline void qtnf_en_rxdone_irq(struct qtnf_pcie_bus_priv *priv)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->irq_lock, flags);
+	priv->pcie_irq_mask |= PCIE_HDP_INT_RX_BITS;
+	writel(priv->pcie_irq_mask, PCIE_HDP_INT_EN(priv->pcie_reg_base));
+	spin_unlock_irqrestore(&priv->irq_lock, flags);
+}
+
+static inline void qtnf_dis_rxdone_irq(struct qtnf_pcie_bus_priv *priv)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->irq_lock, flags);
+	priv->pcie_irq_mask &= ~PCIE_HDP_INT_RX_BITS;
+	writel(priv->pcie_irq_mask, PCIE_HDP_INT_EN(priv->pcie_reg_base));
+	spin_unlock_irqrestore(&priv->irq_lock, flags);
+}
+
+static inline void qtnf_en_txdone_irq(struct qtnf_pcie_bus_priv *priv)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->irq_lock, flags);
+	priv->pcie_irq_mask |= PCIE_HDP_INT_TX_BITS;
+	writel(priv->pcie_irq_mask, PCIE_HDP_INT_EN(priv->pcie_reg_base));
+	spin_unlock_irqrestore(&priv->irq_lock, flags);
+}
+
+static inline void qtnf_dis_txdone_irq(struct qtnf_pcie_bus_priv *priv)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->irq_lock, flags);
+	priv->pcie_irq_mask &= ~PCIE_HDP_INT_TX_BITS;
+	writel(priv->pcie_irq_mask, PCIE_HDP_INT_EN(priv->pcie_reg_base));
+	spin_unlock_irqrestore(&priv->irq_lock, flags);
+}
+
+static int qtnf_pcie_init_irq(struct qtnf_pcie_bus_priv *priv)
+{
+	struct pci_dev *pdev = priv->pdev;
+
+	/* fall back to legacy INTx interrupts by default */
+	priv->msi_enabled = 0;
+
+	/* check if MSI capability is available */
+	if (use_msi) {
+		if (!pci_enable_msi(pdev)) {
+			pr_debug("MSI interrupt enabled\n");
+			priv->msi_enabled = 1;
+		} else {
+			pr_warn("failed to enable MSI interrupts");
+		}
+	}
+
+	if (!priv->msi_enabled) {
+		pr_warn("legacy PCIE interrupts enabled\n");
+		pci_intx(pdev, 1);
+	}
+
+	return 0;
+}
+
+static void qtnf_deassert_intx(struct qtnf_pcie_bus_priv *priv)
+{
+	void __iomem *reg = priv->sysctl_bar + PEARL_PCIE_CFG0_OFFSET;
+	u32 cfg;
+
+	cfg = readl(reg);
+	cfg &= ~PEARL_ASSERT_INTX;
+	qtnf_non_posted_write(cfg, reg);
+}
+
+static void qtnf_ipc_gen_ep_int(void *arg)
+{
+	const struct qtnf_pcie_bus_priv *priv = arg;
+	const u32 data = QTN_PEARL_IPC_IRQ_WORD(QTN_PEARL_LHOST_IPC_IRQ);
+	void __iomem *reg = priv->sysctl_bar +
+			    QTN_PEARL_SYSCTL_LHOST_IRQ_OFFSET;
+
+	qtnf_non_posted_write(data, reg);
+}
+
+static void __iomem *qtnf_map_bar(struct qtnf_pcie_bus_priv *priv, u8 index)
+{
+	void __iomem *vaddr;
+	dma_addr_t busaddr;
+	size_t len;
+	int ret;
+
+	ret = pcim_iomap_regions(priv->pdev, 1 << index, DRV_NAME);
+	if (ret)
+		return IOMEM_ERR_PTR(ret);
+
+	busaddr = pci_resource_start(priv->pdev, index);
+	vaddr = pcim_iomap_table(priv->pdev)[index];
+	len = pci_resource_len(priv->pdev, index);
+
+	pr_debug("BAR%u vaddr=0x%p busaddr=%pad len=%u\n",
+		 index, vaddr, &busaddr, (int)len);
+
+	return vaddr;
+}
+
+static void qtnf_pcie_control_rx_callback(void *arg, const u8 *buf, size_t len)
+{
+	struct qtnf_pcie_bus_priv *priv = arg;
+	struct qtnf_bus *bus = pci_get_drvdata(priv->pdev);
+	struct sk_buff *skb;
+
+	if (unlikely(len == 0)) {
+		pr_warn("zero length packet received\n");
+		return;
+	}
+
+	skb = __dev_alloc_skb(len, GFP_KERNEL);
+
+	if (unlikely(!skb)) {
+		pr_err("failed to allocate skb\n");
+		return;
+	}
+
+	skb_put_data(skb, buf, len);
+
+	qtnf_trans_handle_rx_ctl_packet(bus, skb);
+}
+
+static int qtnf_pcie_init_shm_ipc(struct qtnf_pcie_bus_priv *priv)
+{
+	struct qtnf_shm_ipc_region __iomem *ipc_tx_reg;
+	struct qtnf_shm_ipc_region __iomem *ipc_rx_reg;
+	const struct qtnf_shm_ipc_int ipc_int = { qtnf_ipc_gen_ep_int, priv };
+	const struct qtnf_shm_ipc_rx_callback rx_callback = {
+					qtnf_pcie_control_rx_callback, priv };
+
+	ipc_tx_reg = &priv->bda->bda_shm_reg1;
+	ipc_rx_reg = &priv->bda->bda_shm_reg2;
+
+	qtnf_shm_ipc_init(&priv->shm_ipc_ep_in, QTNF_SHM_IPC_OUTBOUND,
+			  ipc_tx_reg, priv->workqueue,
+			  &ipc_int, &rx_callback);
+	qtnf_shm_ipc_init(&priv->shm_ipc_ep_out, QTNF_SHM_IPC_INBOUND,
+			  ipc_rx_reg, priv->workqueue,
+			  &ipc_int, &rx_callback);
+
+	return 0;
+}
+
+static void qtnf_pcie_free_shm_ipc(struct qtnf_pcie_bus_priv *priv)
+{
+	qtnf_shm_ipc_free(&priv->shm_ipc_ep_in);
+	qtnf_shm_ipc_free(&priv->shm_ipc_ep_out);
+}
+
+static int qtnf_pcie_init_memory(struct qtnf_pcie_bus_priv *priv)
+{
+	int ret = -ENOMEM;
+
+	priv->sysctl_bar = qtnf_map_bar(priv, QTN_SYSCTL_BAR);
+	if (IS_ERR_OR_NULL(priv->sysctl_bar)) {
+		pr_err("failed to map BAR%u\n", QTN_SYSCTL_BAR);
+		return ret;
+	}
+
+	priv->dmareg_bar = qtnf_map_bar(priv, QTN_DMA_BAR);
+	if (IS_ERR_OR_NULL(priv->dmareg_bar)) {
+		pr_err("failed to map BAR%u\n", QTN_DMA_BAR);
+		return ret;
+	}
+
+	priv->epmem_bar = qtnf_map_bar(priv, QTN_SHMEM_BAR);
+	if (IS_ERR_OR_NULL(priv->epmem_bar)) {
+		pr_err("failed to map BAR%u\n", QTN_SHMEM_BAR);
+		return ret;
+	}
+
+	priv->pcie_reg_base = priv->dmareg_bar;
+	priv->bda = priv->epmem_bar;
+	writel(priv->msi_enabled, &priv->bda->bda_rc_msi_enabled);
+
+	return 0;
+}
+
+static int
+qtnf_pcie_init_dma_mask(struct qtnf_pcie_bus_priv *priv, u64 dma_mask)
+{
+	int ret;
+
+	ret = dma_supported(&priv->pdev->dev, dma_mask);
+	if (!ret) {
+		pr_err("DMA mask %llu not supported\n", dma_mask);
+		return ret;
+	}
+
+	ret = pci_set_dma_mask(priv->pdev, dma_mask);
+	if (ret) {
+		pr_err("failed to set DMA mask %llu\n", dma_mask);
+		return ret;
+	}
+
+	ret = pci_set_consistent_dma_mask(priv->pdev, dma_mask);
+	if (ret) {
+		pr_err("failed to set consistent DMA mask %llu\n", dma_mask);
+		return ret;
+	}
+
+	return ret;
+}
+
+static void qtnf_tune_pcie_mps(struct qtnf_pcie_bus_priv *priv)
+{
+	struct pci_dev *pdev = priv->pdev;
+	struct pci_dev *parent;
+	int mps_p, mps_o, mps_m, mps;
+	int ret;
+
+	/* current mps */
+	mps_o = pcie_get_mps(pdev);
+
+	/* maximum supported mps */
+	mps_m = 128 << pdev->pcie_mpss;
+
+	/* suggested new mps value */
+	mps = mps_m;
+
+	if (pdev->bus && pdev->bus->self) {
+		/* parent (bus) mps */
+		parent = pdev->bus->self;
+
+		if (pci_is_pcie(parent)) {
+			mps_p = pcie_get_mps(parent);
+			mps = min(mps_m, mps_p);
+		}
+	}
+
+	ret = pcie_set_mps(pdev, mps);
+	if (ret) {
+		pr_err("failed to set mps to %d, keep using current %d\n",
+		       mps, mps_o);
+		priv->mps = mps_o;
+		return;
+	}
+
+	pr_debug("set mps to %d (was %d, max %d)\n", mps, mps_o, mps_m);
+	priv->mps = mps;
+}
+
+static int qtnf_is_state(__le32 __iomem *reg, u32 state)
+{
+	u32 s = readl(reg);
+
+	return s & state;
+}
+
+static void qtnf_set_state(__le32 __iomem *reg, u32 state)
+{
+	u32 s = readl(reg);
+
+	qtnf_non_posted_write(state | s, reg);
+}
+
+static void qtnf_clear_state(__le32 __iomem *reg, u32 state)
+{
+	u32 s = readl(reg);
+
+	qtnf_non_posted_write(s & ~state, reg);
+}
+
+static int qtnf_poll_state(__le32 __iomem *reg, u32 state, u32 delay_in_ms)
+{
+	u32 timeout = 0;
+
+	while ((qtnf_is_state(reg, state) == 0)) {
+		usleep_range(1000, 1200);
+		if (++timeout > delay_in_ms)
+			return -1;
+	}
+
+	return 0;
+}
+
+static int alloc_skb_array(struct qtnf_pcie_bus_priv *priv)
+{
+	struct sk_buff **vaddr;
+	int len;
+
+	len = priv->tx_bd_num * sizeof(*priv->tx_skb) +
+		priv->rx_bd_num * sizeof(*priv->rx_skb);
+	vaddr = devm_kzalloc(&priv->pdev->dev, len, GFP_KERNEL);
+
+	if (!vaddr)
+		return -ENOMEM;
+
+	priv->tx_skb = vaddr;
+
+	vaddr += priv->tx_bd_num;
+	priv->rx_skb = vaddr;
+
+	return 0;
+}
+
+static int alloc_bd_table(struct qtnf_pcie_bus_priv *priv)
+{
+	dma_addr_t paddr;
+	void *vaddr;
+	int len;
+
+	len = priv->tx_bd_num * sizeof(struct qtnf_tx_bd) +
+		priv->rx_bd_num * sizeof(struct qtnf_rx_bd);
+
+	vaddr = dmam_alloc_coherent(&priv->pdev->dev, len, &paddr, GFP_KERNEL);
+	if (!vaddr)
+		return -ENOMEM;
+
+	/* tx bd */
+
+	memset(vaddr, 0, len);
+
+	priv->bd_table_vaddr = vaddr;
+	priv->bd_table_paddr = paddr;
+	priv->bd_table_len = len;
+
+	priv->tx_bd_vbase = vaddr;
+	priv->tx_bd_pbase = paddr;
+
+	pr_debug("TX descriptor table: vaddr=0x%p paddr=%pad\n", vaddr, &paddr);
+
+	priv->tx_bd_reclaim_start = 0;
+	priv->tx_bd_index = 0;
+	priv->tx_queue_len = 0;
+
+	/* rx bd */
+
+	vaddr = ((struct qtnf_tx_bd *)vaddr) + priv->tx_bd_num;
+	paddr += priv->tx_bd_num * sizeof(struct qtnf_tx_bd);
+
+	priv->rx_bd_vbase = vaddr;
+	priv->rx_bd_pbase = paddr;
+
+	writel(QTN_HOST_LO32(paddr),
+	       PCIE_HDP_TX_HOST_Q_BASE_L(priv->pcie_reg_base));
+	writel(QTN_HOST_HI32(paddr),
+	       PCIE_HDP_TX_HOST_Q_BASE_H(priv->pcie_reg_base));
+	writel(priv->rx_bd_num | (sizeof(struct qtnf_rx_bd)) << 16,
+	       PCIE_HDP_TX_HOST_Q_SZ_CTRL(priv->pcie_reg_base));
+
+	priv->hw_txproc_wr_ptr = priv->rx_bd_num - rx_bd_reserved_param;
+
+	writel(priv->hw_txproc_wr_ptr,
+	       PCIE_HDP_TX_HOST_Q_WR_PTR(priv->pcie_reg_base));
+
+	pr_debug("RX descriptor table: vaddr=0x%p paddr=%pad\n", vaddr, &paddr);
+
+	priv->rx_bd_index = 0;
+
+	return 0;
+}
+
+static int skb2rbd_attach(struct qtnf_pcie_bus_priv *priv, u16 rx_bd_index)
+{
+	struct qtnf_rx_bd *rxbd;
+	struct sk_buff *skb;
+	dma_addr_t paddr;
+
+	skb = __dev_alloc_skb(SKB_BUF_SIZE + NET_IP_ALIGN,
+			      GFP_ATOMIC);
+	if (!skb) {
+		priv->rx_skb[rx_bd_index] = NULL;
+		return -ENOMEM;
+	}
+
+	priv->rx_skb[rx_bd_index] = skb;
+
+	skb_reserve(skb, NET_IP_ALIGN);
+
+	rxbd = &priv->rx_bd_vbase[rx_bd_index];
+
+	paddr = pci_map_single(priv->pdev, skb->data,
+			       SKB_BUF_SIZE, PCI_DMA_FROMDEVICE);
+	if (pci_dma_mapping_error(priv->pdev, paddr)) {
+		pr_err("skb DMA mapping error: %pad\n", &paddr);
+		return -ENOMEM;
+	}
+
+	writel(QTN_HOST_LO32(paddr),
+	       PCIE_HDP_HHBM_BUF_PTR(priv->pcie_reg_base));
+	writel(QTN_HOST_HI32(paddr),
+	       PCIE_HDP_HHBM_BUF_PTR_H(priv->pcie_reg_base));
+
+	/* keep rx skb paddrs in rx buffer descriptors for cleanup purposes */
+	rxbd->addr = cpu_to_le32(QTN_HOST_LO32(paddr));
+	rxbd->addr_h = cpu_to_le32(QTN_HOST_HI32(paddr));
+
+	rxbd->info = 0x0;
+
+	return 0;
+}
+
+static int alloc_rx_buffers(struct qtnf_pcie_bus_priv *priv)
+{
+	u16 i;
+	int ret = 0;
+
+	memset(priv->rx_bd_vbase, 0x0,
+	       priv->rx_bd_num * sizeof(struct qtnf_rx_bd));
+
+	for (i = 0; i < priv->rx_bd_num; i++) {
+		ret = skb2rbd_attach(priv, i);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+/* all rx/tx activity should have ceased before calling this function */
+static void free_xfer_buffers(void *data)
+{
+	struct qtnf_pcie_bus_priv *priv = (struct qtnf_pcie_bus_priv *)data;
+	struct qtnf_rx_bd *rxbd;
+	dma_addr_t paddr;
+	int i;
+
+	/* free rx buffers */
+	for (i = 0; i < priv->rx_bd_num; i++) {
+		if (priv->rx_skb[i]) {
+			rxbd = &priv->rx_bd_vbase[i];
+			paddr = QTN_HOST_ADDR(le32_to_cpu(rxbd->addr_h),
+					      le32_to_cpu(rxbd->addr));
+			pci_unmap_single(priv->pdev, paddr, SKB_BUF_SIZE,
+					 PCI_DMA_FROMDEVICE);
+
+			dev_kfree_skb_any(priv->rx_skb[i]);
+		}
+	}
+
+	/* free tx buffers */
+	for (i = 0; i < priv->tx_bd_num; i++) {
+		if (priv->tx_skb[i]) {
+			dev_kfree_skb_any(priv->tx_skb[i]);
+			priv->tx_skb[i] = NULL;
+		}
+	}
+}
+
+static int qtnf_pcie_init_xfer(struct qtnf_pcie_bus_priv *priv)
+{
+	int ret;
+
+	priv->tx_bd_num = tx_bd_size_param;
+	priv->rx_bd_num = rx_bd_size_param;
+
+	ret = alloc_skb_array(priv);
+	if (ret) {
+		pr_err("failed to allocate skb array\n");
+		return ret;
+	}
+
+	ret = alloc_bd_table(priv);
+	if (ret) {
+		pr_err("failed to allocate bd table\n");
+		return ret;
+	}
+
+	ret = alloc_rx_buffers(priv);
+	if (ret) {
+		pr_err("failed to allocate rx buffers\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static int qtnf_pcie_data_tx_reclaim(struct qtnf_pcie_bus_priv *priv)
+{
+	struct qtnf_tx_bd *txbd;
+	struct sk_buff *skb;
+	dma_addr_t paddr;
+	int last_sent;
+	int count;
+	int i;
+
+	last_sent = readl(PCIE_HDP_RX0DMA_CNT(priv->pcie_reg_base))
+			% priv->tx_bd_num;
+	i = priv->tx_bd_reclaim_start;
+	count = 0;
+
+	while (i != last_sent) {
+		skb = priv->tx_skb[i];
+		if (!skb)
+			break;
+
+		txbd = &priv->tx_bd_vbase[i];
+		paddr = QTN_HOST_ADDR(le32_to_cpu(txbd->addr_h),
+				      le32_to_cpu(txbd->addr));
+		pci_unmap_single(priv->pdev, paddr, skb->len, PCI_DMA_TODEVICE);
+
+		if (skb->dev) {
+			skb->dev->stats.tx_packets++;
+			skb->dev->stats.tx_bytes += skb->len;
+
+			if (netif_queue_stopped(skb->dev))
+				netif_wake_queue(skb->dev);
+		}
+
+		dev_kfree_skb_any(skb);
+		priv->tx_skb[i] = NULL;
+		priv->tx_queue_len--;
+		count++;
+
+		if (++i >= priv->tx_bd_num)
+			i = 0;
+	}
+
+	priv->tx_bd_reclaim_start = i;
+	priv->tx_reclaim_done += count;
+	priv->tx_reclaim_req++;
+
+	return count;
+}
+
+static bool qtnf_tx_queue_ready(struct qtnf_pcie_bus_priv *priv)
+{
+	if (priv->tx_queue_len >= priv->tx_bd_num - 1) {
+		pr_err_ratelimited("reclaim full Tx queue\n");
+		qtnf_pcie_data_tx_reclaim(priv);
+
+		if (priv->tx_queue_len >= priv->tx_bd_num - 1) {
+			priv->tx_full_count++;
+			return false;
+		}
+	}
+
+	return true;
+}
+
+static int qtnf_pcie_data_tx(struct qtnf_bus *bus, struct sk_buff *skb)
+{
+	struct qtnf_pcie_bus_priv *priv = (void *)get_bus_priv(bus);
+	dma_addr_t txbd_paddr, skb_paddr;
+	struct qtnf_tx_bd *txbd;
+	unsigned long flags;
+	int len, i;
+	u32 info;
+	int ret = 0;
+
+	spin_lock_irqsave(&priv->tx_lock, flags);
+
+	priv->tx_done_count++;
+
+	if (!qtnf_tx_queue_ready(priv)) {
+		if (skb->dev)
+			netif_stop_queue(skb->dev);
+
+		spin_unlock_irqrestore(&priv->tx_lock, flags);
+		return NETDEV_TX_BUSY;
+	}
+
+	i = priv->tx_bd_index;
+	priv->tx_skb[i] = skb;
+	len = skb->len;
+
+	skb_paddr = pci_map_single(priv->pdev, skb->data,
+				   skb->len, PCI_DMA_TODEVICE);
+	if (pci_dma_mapping_error(priv->pdev, skb_paddr)) {
+		pr_err("skb DMA mapping error: %pad\n", &skb_paddr);
+		ret = -ENOMEM;
+		goto tx_done;
+	}
+
+	txbd = &priv->tx_bd_vbase[i];
+	txbd->addr = cpu_to_le32(QTN_HOST_LO32(skb_paddr));
+	txbd->addr_h = cpu_to_le32(QTN_HOST_HI32(skb_paddr));
+
+	info = (len & QTN_PCIE_TX_DESC_LEN_MASK) << QTN_PCIE_TX_DESC_LEN_SHIFT;
+	txbd->info = cpu_to_le32(info);
+
+	/* sync up all descriptor updates before passing them to EP */
+	dma_wmb();
+
+	/* write new TX descriptor to PCIE_RX_FIFO on EP */
+	txbd_paddr = priv->tx_bd_pbase + i * sizeof(struct qtnf_tx_bd);
+	writel(QTN_HOST_LO32(txbd_paddr),
+	       PCIE_HDP_HOST_WR_DESC0(priv->pcie_reg_base));
+	writel(QTN_HOST_HI32(txbd_paddr),
+	       PCIE_HDP_HOST_WR_DESC0_H(priv->pcie_reg_base));
+
+	if (++i >= priv->tx_bd_num)
+		i = 0;
+
+	priv->tx_bd_index = i;
+	priv->tx_queue_len++;
+
+tx_done:
+	if (ret && skb) {
+		pr_err_ratelimited("drop skb\n");
+		if (skb->dev)
+			skb->dev->stats.tx_dropped++;
+		dev_kfree_skb_any(skb);
+	}
+
+	spin_unlock_irqrestore(&priv->tx_lock, flags);
+
+	return NETDEV_TX_OK;
+}
+
+static int qtnf_pcie_control_tx(struct qtnf_bus *bus, struct sk_buff *skb)
+{
+	struct qtnf_pcie_bus_priv *priv = (void *)get_bus_priv(bus);
+
+	return qtnf_shm_ipc_send(&priv->shm_ipc_ep_in, skb->data, skb->len);
+}
+
+static irqreturn_t qtnf_interrupt(int irq, void *data)
+{
+	struct qtnf_bus *bus = (struct qtnf_bus *)data;
+	struct qtnf_pcie_bus_priv *priv = (void *)get_bus_priv(bus);
+	u32 status;
+
+	priv->pcie_irq_count++;
+	status = readl(PCIE_HDP_INT_STATUS(priv->pcie_reg_base));
+
+	qtnf_shm_ipc_irq_handler(&priv->shm_ipc_ep_in);
+	qtnf_shm_ipc_irq_handler(&priv->shm_ipc_ep_out);
+
+	if (!(status & priv->pcie_irq_mask))
+		goto irq_done;
+
+	if (status & PCIE_HDP_INT_RX_BITS) {
+		priv->pcie_irq_rx_count++;
+		qtnf_dis_rxdone_irq(priv);
+		napi_schedule(&bus->mux_napi);
+	}
+
+	if (status & PCIE_HDP_INT_TX_BITS) {
+		priv->pcie_irq_tx_count++;
+		qtnf_dis_txdone_irq(priv);
+		tasklet_hi_schedule(&priv->reclaim_tq);
+	}
+
+irq_done:
+	/* H/W workaround: clean all bits, not only enabled */
+	qtnf_non_posted_write(~0U, PCIE_HDP_INT_STATUS(priv->pcie_reg_base));
+
+	if (!priv->msi_enabled)
+		qtnf_deassert_intx(priv);
+
+	return IRQ_HANDLED;
+}
+
+static inline void hw_txproc_wr_ptr_inc(struct qtnf_pcie_bus_priv *priv)
+{
+	u32 index;
+
+	index = priv->hw_txproc_wr_ptr;
+
+	if (++index >= priv->rx_bd_num)
+		index = 0;
+
+	priv->hw_txproc_wr_ptr = index;
+}
+
+static int qtnf_rx_poll(struct napi_struct *napi, int budget)
+{
+	struct qtnf_bus *bus = container_of(napi, struct qtnf_bus, mux_napi);
+	struct qtnf_pcie_bus_priv *priv = (void *)get_bus_priv(bus);
+	struct net_device *ndev = NULL;
+	struct sk_buff *skb = NULL;
+	int processed = 0;
+	struct qtnf_rx_bd *rxbd;
+	dma_addr_t skb_paddr;
+	u32 descw;
+	u16 index;
+	int ret;
+
+	index = priv->rx_bd_index;
+	rxbd = &priv->rx_bd_vbase[index];
+
+	descw = le32_to_cpu(rxbd->info);
+
+	while ((descw & QTN_TXDONE_MASK) && (processed < budget)) {
+		skb = priv->rx_skb[index];
+
+		if (likely(skb)) {
+			skb_put(skb, QTN_GET_LEN(descw));
+
+			skb_paddr = QTN_HOST_ADDR(le32_to_cpu(rxbd->addr_h),
+						  le32_to_cpu(rxbd->addr));
+			pci_unmap_single(priv->pdev, skb_paddr, SKB_BUF_SIZE,
+					 PCI_DMA_FROMDEVICE);
+
+			ndev = qtnf_classify_skb(bus, skb);
+			if (likely(ndev)) {
+				ndev->stats.rx_packets++;
+				ndev->stats.rx_bytes += skb->len;
+
+				skb->protocol = eth_type_trans(skb, ndev);
+				netif_receive_skb(skb);
+			} else {
+				pr_debug("drop untagged skb\n");
+				bus->mux_dev.stats.rx_dropped++;
+				dev_kfree_skb_any(skb);
+			}
+
+			processed++;
+		} else {
+			pr_err("missing rx_skb[%d]\n", index);
+		}
+
+		/* attached rx buffer is passed upstream: map a new one */
+		ret = skb2rbd_attach(priv, index);
+		if (likely(!ret)) {
+			if (++index >= priv->rx_bd_num)
+				index = 0;
+
+			priv->rx_bd_index = index;
+			hw_txproc_wr_ptr_inc(priv);
+
+			rxbd = &priv->rx_bd_vbase[index];
+			descw = le32_to_cpu(rxbd->info);
+		} else {
+			pr_err("failed to allocate new rx_skb[%d]\n", index);
+			break;
+		}
+
+		writel(priv->hw_txproc_wr_ptr,
+		       PCIE_HDP_TX_HOST_Q_WR_PTR(priv->pcie_reg_base));
+	}
+
+	if (processed < budget) {
+		napi_complete(napi);
+		qtnf_en_rxdone_irq(priv);
+	}
+
+	return processed;
+}
+
+static void
+qtnf_pcie_data_tx_timeout(struct qtnf_bus *bus, struct net_device *ndev)
+{
+	struct qtnf_pcie_bus_priv *priv = (void *)get_bus_priv(bus);
+
+	tasklet_hi_schedule(&priv->reclaim_tq);
+}
+
+static void qtnf_pcie_data_rx_start(struct qtnf_bus *bus)
+{
+	struct qtnf_pcie_bus_priv *priv = (void *)get_bus_priv(bus);
+
+	qtnf_enable_hdp_irqs(priv);
+	napi_enable(&bus->mux_napi);
+}
+
+static void qtnf_pcie_data_rx_stop(struct qtnf_bus *bus)
+{
+	struct qtnf_pcie_bus_priv *priv = (void *)get_bus_priv(bus);
+
+	napi_disable(&bus->mux_napi);
+	qtnf_disable_hdp_irqs(priv);
+}
+
+static const struct qtnf_bus_ops qtnf_pcie_bus_ops = {
+	/* control path methods */
+	.control_tx	= qtnf_pcie_control_tx,
+
+	/* data path methods */
+	.data_tx		= qtnf_pcie_data_tx,
+	.data_tx_timeout	= qtnf_pcie_data_tx_timeout,
+	.data_rx_start		= qtnf_pcie_data_rx_start,
+	.data_rx_stop		= qtnf_pcie_data_rx_stop,
+};
+
+static int qtnf_ep_fw_send(struct qtnf_pcie_bus_priv *priv, uint32_t size,
+			   int blk, const u8 *pblk, const u8 *fw)
+{
+	struct pci_dev *pdev = priv->pdev;
+	struct qtnf_bus *bus = pci_get_drvdata(pdev);
+
+	struct qtnf_pcie_fw_hdr *hdr;
+	u8 *pdata;
+
+	int hds = sizeof(*hdr);
+	struct sk_buff *skb = NULL;
+	int len = 0;
+	int ret;
+
+	skb = __dev_alloc_skb(QTN_PCIE_FW_BUFSZ, GFP_KERNEL);
+	if (!skb)
+		return -ENOMEM;
+
+	skb->len = QTN_PCIE_FW_BUFSZ;
+	skb->dev = NULL;
+
+	hdr = (struct qtnf_pcie_fw_hdr *)skb->data;
+	memcpy(hdr->boardflg, QTN_PCIE_BOARDFLG, strlen(QTN_PCIE_BOARDFLG));
+	hdr->fwsize = cpu_to_le32(size);
+	hdr->seqnum = cpu_to_le32(blk);
+
+	if (blk)
+		hdr->type = cpu_to_le32(QTN_FW_DSUB);
+	else
+		hdr->type = cpu_to_le32(QTN_FW_DBEGIN);
+
+	pdata = skb->data + hds;
+
+	len = QTN_PCIE_FW_BUFSZ - hds;
+	if (pblk >= (fw + size - len)) {
+		len = fw + size - pblk;
+		hdr->type = cpu_to_le32(QTN_FW_DEND);
+	}
+
+	hdr->pktlen = cpu_to_le32(len);
+	memcpy(pdata, pblk, len);
+	hdr->crc = cpu_to_le32(~crc32(0, pdata, len));
+
+	ret = qtnf_pcie_data_tx(bus, skb);
+
+	return (ret == NETDEV_TX_OK) ? len : 0;
+}
+
+static int
+qtnf_ep_fw_load(struct qtnf_pcie_bus_priv *priv, const u8 *fw, u32 fw_size)
+{
+	int blk_size = QTN_PCIE_FW_BUFSZ - sizeof(struct qtnf_pcie_fw_hdr);
+	int blk_count = fw_size / blk_size + ((fw_size % blk_size) ? 1 : 0);
+	const u8 *pblk = fw;
+	int threshold = 0;
+	int blk = 0;
+	int len;
+
+	pr_debug("FW upload started: fw_addr=0x%p size=%d\n", fw, fw_size);
+
+	while (blk < blk_count) {
+		if (++threshold > 10000) {
+			pr_err("FW upload failed: too many retries\n");
+			return -ETIMEDOUT;
+		}
+
+		len = qtnf_ep_fw_send(priv, fw_size, blk, pblk, fw);
+		if (len <= 0)
+			continue;
+
+		if (!((blk + 1) & QTN_PCIE_FW_DLMASK) ||
+		    (blk == (blk_count - 1))) {
+			qtnf_set_state(&priv->bda->bda_rc_state,
+				       QTN_RC_FW_SYNC);
+			if (qtnf_poll_state(&priv->bda->bda_ep_state,
+					    QTN_EP_FW_SYNC,
+					    QTN_FW_DL_TIMEOUT_MS)) {
+				pr_err("FW upload failed: SYNC timed out\n");
+				return -ETIMEDOUT;
+			}
+
+			qtnf_clear_state(&priv->bda->bda_ep_state,
+					 QTN_EP_FW_SYNC);
+
+			if (qtnf_is_state(&priv->bda->bda_ep_state,
+					  QTN_EP_FW_RETRY)) {
+				if (blk == (blk_count - 1)) {
+					int last_round =
+						blk_count & QTN_PCIE_FW_DLMASK;
+					blk -= last_round;
+					pblk -= ((last_round - 1) *
+						blk_size + len);
+				} else {
+					blk -= QTN_PCIE_FW_DLMASK;
+					pblk -= QTN_PCIE_FW_DLMASK * blk_size;
+				}
+
+				qtnf_clear_state(&priv->bda->bda_ep_state,
+						 QTN_EP_FW_RETRY);
+
+				pr_warn("FW upload retry: block #%d\n", blk);
+				continue;
+			}
+
+			qtnf_pcie_data_tx_reclaim(priv);
+		}
+
+		pblk += len;
+		blk++;
+	}
+
+	pr_debug("FW upload completed: totally sent %d blocks\n", blk);
+	return 0;
+}
+
+static void qtnf_firmware_load(const struct firmware *fw, void *context)
+{
+	struct qtnf_pcie_bus_priv *priv = (void *)context;
+	struct pci_dev *pdev = priv->pdev;
+	struct qtnf_bus *bus = pci_get_drvdata(pdev);
+	int ret;
+
+	if (!fw) {
+		pr_err("failed to get firmware %s\n", bus->fwname);
+		goto fw_load_err;
+	}
+
+	ret = qtnf_ep_fw_load(priv, fw->data, fw->size);
+	if (ret) {
+		pr_err("FW upload error\n");
+		goto fw_load_err;
+	}
+
+	if (qtnf_poll_state(&priv->bda->bda_ep_state, QTN_EP_FW_DONE,
+			    QTN_FW_DL_TIMEOUT_MS)) {
+		pr_err("FW bringup timed out\n");
+		goto fw_load_err;
+	}
+
+	bus->fw_state = QTNF_FW_STATE_FW_DNLD_DONE;
+	pr_info("firmware is up and running\n");
+
+fw_load_err:
+
+	if (fw)
+		release_firmware(fw);
+
+	complete(&bus->request_firmware_complete);
+}
+
+static int qtnf_bringup_fw(struct qtnf_bus *bus)
+{
+	struct qtnf_pcie_bus_priv *priv = (void *)get_bus_priv(bus);
+	struct pci_dev *pdev = priv->pdev;
+	int ret;
+	u32 state = QTN_RC_FW_LOADRDY | QTN_RC_FW_QLINK;
+
+	if (flashboot)
+		state |= QTN_RC_FW_FLASHBOOT;
+
+	qtnf_set_state(&priv->bda->bda_rc_state, state);
+
+	if (qtnf_poll_state(&priv->bda->bda_ep_state, QTN_EP_FW_LOADRDY,
+			    QTN_FW_DL_TIMEOUT_MS)) {
+		pr_err("card is not ready\n");
+		return -ETIMEDOUT;
+	}
+
+	qtnf_clear_state(&priv->bda->bda_ep_state, QTN_EP_FW_LOADRDY);
+
+	if (flashboot) {
+		pr_info("Booting FW from flash\n");
+
+		if (!qtnf_poll_state(&priv->bda->bda_ep_state, QTN_EP_FW_DONE,
+				     QTN_FW_DL_TIMEOUT_MS))
+			bus->fw_state = QTNF_FW_STATE_FW_DNLD_DONE;
+
+		return 0;
+	}
+
+	pr_info("starting firmware upload: %s\n", bus->fwname);
+
+	ret = request_firmware_nowait(THIS_MODULE, 1, bus->fwname, &pdev->dev,
+				      GFP_KERNEL, priv, qtnf_firmware_load);
+	if (ret < 0)
+		pr_err("request_firmware_nowait error %d\n", ret);
+	else
+		ret = 1;
+
+	return ret;
+}
+
+static void qtnf_reclaim_tasklet_fn(unsigned long data)
+{
+	struct qtnf_pcie_bus_priv *priv = (void *)data;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->tx_lock, flags);
+	qtnf_pcie_data_tx_reclaim(priv);
+	spin_unlock_irqrestore(&priv->tx_lock, flags);
+	qtnf_en_txdone_irq(priv);
+}
+
+static int qtnf_dbg_mps_show(struct seq_file *s, void *data)
+{
+	struct qtnf_bus *bus = dev_get_drvdata(s->private);
+	struct qtnf_pcie_bus_priv *priv = get_bus_priv(bus);
+
+	seq_printf(s, "%d\n", priv->mps);
+
+	return 0;
+}
+
+static int qtnf_dbg_msi_show(struct seq_file *s, void *data)
+{
+	struct qtnf_bus *bus = dev_get_drvdata(s->private);
+	struct qtnf_pcie_bus_priv *priv = get_bus_priv(bus);
+
+	seq_printf(s, "%u\n", priv->msi_enabled);
+
+	return 0;
+}
+
+static int qtnf_dbg_irq_stats(struct seq_file *s, void *data)
+{
+	struct qtnf_bus *bus = dev_get_drvdata(s->private);
+	struct qtnf_pcie_bus_priv *priv = get_bus_priv(bus);
+
+	seq_printf(s, "pcie_irq_count(%u)\n", priv->pcie_irq_count);
+	seq_printf(s, "pcie_irq_tx_count(%u)\n", priv->pcie_irq_tx_count);
+	seq_printf(s, "pcie_irq_rx_count(%u)\n", priv->pcie_irq_rx_count);
+
+	return 0;
+}
+
+static int qtnf_dbg_hdp_stats(struct seq_file *s, void *data)
+{
+	struct qtnf_bus *bus = dev_get_drvdata(s->private);
+	struct qtnf_pcie_bus_priv *priv = get_bus_priv(bus);
+
+	seq_printf(s, "tx_full_count(%u)\n", priv->tx_full_count);
+	seq_printf(s, "tx_done_count(%u)\n", priv->tx_done_count);
+	seq_printf(s, "tx_reclaim_done(%u)\n", priv->tx_reclaim_done);
+	seq_printf(s, "tx_reclaim_req(%u)\n", priv->tx_reclaim_req);
+	seq_printf(s, "tx_bd_reclaim_start(%u)\n", priv->tx_bd_reclaim_start);
+	seq_printf(s, "tx_bd_index(%u)\n", priv->tx_bd_index);
+	seq_printf(s, "rx_bd_index(%u)\n", priv->rx_bd_index);
+	seq_printf(s, "tx_queue_len(%u)\n", priv->tx_queue_len);
+
+	return 0;
+}
+
+static int qtnf_dbg_shm_stats(struct seq_file *s, void *data)
+{
+	struct qtnf_bus *bus = dev_get_drvdata(s->private);
+	struct qtnf_pcie_bus_priv *priv = get_bus_priv(bus);
+
+	seq_printf(s, "shm_ipc_ep_in.tx_packet_count(%zu)\n",
+		   priv->shm_ipc_ep_in.tx_packet_count);
+	seq_printf(s, "shm_ipc_ep_in.rx_packet_count(%zu)\n",
+		   priv->shm_ipc_ep_in.rx_packet_count);
+	seq_printf(s, "shm_ipc_ep_out.tx_packet_count(%zu)\n",
+		   priv->shm_ipc_ep_out.tx_timeout_count);
+	seq_printf(s, "shm_ipc_ep_out.rx_packet_count(%zu)\n",
+		   priv->shm_ipc_ep_out.rx_packet_count);
+
+	return 0;
+}
+
+static int qtnf_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
+{
+	struct qtnf_pcie_bus_priv *pcie_priv;
+	struct qtnf_bus *bus;
+	int ret;
+
+	bus = devm_kzalloc(&pdev->dev,
+			   sizeof(*bus) + sizeof(*pcie_priv), GFP_KERNEL);
+	if (!bus) {
+		ret = -ENOMEM;
+		goto err_init;
+	}
+
+	pcie_priv = get_bus_priv(bus);
+
+	pci_set_drvdata(pdev, bus);
+	bus->bus_ops = &qtnf_pcie_bus_ops;
+	bus->dev = &pdev->dev;
+	bus->fw_state = QTNF_FW_STATE_RESET;
+	pcie_priv->pdev = pdev;
+
+	strcpy(bus->fwname, QTN_PCI_PEARL_FW_NAME);
+	init_completion(&bus->request_firmware_complete);
+	mutex_init(&bus->bus_lock);
+	spin_lock_init(&pcie_priv->irq_lock);
+	spin_lock_init(&pcie_priv->tx_lock);
+
+	/* init stats */
+	pcie_priv->tx_full_count = 0;
+	pcie_priv->tx_done_count = 0;
+	pcie_priv->pcie_irq_count = 0;
+	pcie_priv->pcie_irq_rx_count = 0;
+	pcie_priv->pcie_irq_tx_count = 0;
+	pcie_priv->tx_reclaim_done = 0;
+	pcie_priv->tx_reclaim_req = 0;
+
+	pcie_priv->workqueue = create_singlethread_workqueue("QTNF_PEARL_PCIE");
+	if (!pcie_priv->workqueue) {
+		pr_err("failed to alloc bus workqueue\n");
+		ret = -ENODEV;
+		goto err_priv;
+	}
+
+	if (!pci_is_pcie(pdev)) {
+		pr_err("device %s is not PCI Express\n", pci_name(pdev));
+		ret = -EIO;
+		goto err_base;
+	}
+
+	qtnf_tune_pcie_mps(pcie_priv);
+
+	ret = pcim_enable_device(pdev);
+	if (ret) {
+		pr_err("failed to init PCI device %x\n", pdev->device);
+		goto err_base;
+	} else {
+		pr_debug("successful init of PCI device %x\n", pdev->device);
+	}
+
+	pcim_pin_device(pdev);
+	pci_set_master(pdev);
+
+	ret = qtnf_pcie_init_irq(pcie_priv);
+	if (ret < 0) {
+		pr_err("irq init failed\n");
+		goto err_base;
+	}
+
+	ret = qtnf_pcie_init_memory(pcie_priv);
+	if (ret < 0) {
+		pr_err("PCIE memory init failed\n");
+		goto err_base;
+	}
+
+	ret = qtnf_pcie_init_shm_ipc(pcie_priv);
+	if (ret < 0) {
+		pr_err("PCIE SHM IPC init failed\n");
+		goto err_base;
+	}
+
+	ret = qtnf_pcie_init_dma_mask(pcie_priv, DMA_BIT_MASK(32));
+	if (ret) {
+		pr_err("PCIE DMA mask init failed\n");
+		goto err_base;
+	}
+
+	ret = devm_add_action(&pdev->dev, free_xfer_buffers, (void *)pcie_priv);
+	if (ret) {
+		pr_err("custom release callback init failed\n");
+		goto err_base;
+	}
+
+	ret = qtnf_pcie_init_xfer(pcie_priv);
+	if (ret) {
+		pr_err("PCIE xfer init failed\n");
+		goto err_base;
+	}
+
+	/* init default irq settings */
+	qtnf_init_hdp_irqs(pcie_priv);
+
+	/* start with disabled irqs */
+	qtnf_disable_hdp_irqs(pcie_priv);
+
+	ret = devm_request_irq(&pdev->dev, pdev->irq, &qtnf_interrupt, 0,
+			       "qtnf_pcie_irq", (void *)bus);
+	if (ret) {
+		pr_err("failed to request pcie irq %d\n", pdev->irq);
+		goto err_base;
+	}
+
+	tasklet_init(&pcie_priv->reclaim_tq, qtnf_reclaim_tasklet_fn,
+		     (unsigned long)pcie_priv);
+	init_dummy_netdev(&bus->mux_dev);
+	netif_napi_add(&bus->mux_dev, &bus->mux_napi,
+		       qtnf_rx_poll, 10);
+
+	ret = qtnf_bringup_fw(bus);
+	if (ret < 0)
+		goto err_bringup_fw;
+	else if (ret)
+		wait_for_completion(&bus->request_firmware_complete);
+
+	if (bus->fw_state != QTNF_FW_STATE_FW_DNLD_DONE) {
+		pr_err("failed to start FW\n");
+		goto err_bringup_fw;
+	}
+
+	if (qtnf_poll_state(&pcie_priv->bda->bda_ep_state, QTN_EP_FW_QLINK_DONE,
+			    QTN_FW_QLINK_TIMEOUT_MS)) {
+		pr_err("FW runtime failure\n");
+		goto err_bringup_fw;
+	}
+
+	ret = qtnf_core_attach(bus);
+	if (ret) {
+		pr_err("failed to attach core\n");
+		goto err_bringup_fw;
+	}
+
+	qtnf_debugfs_init(bus, DRV_NAME);
+	qtnf_debugfs_add_entry(bus, "mps", qtnf_dbg_mps_show);
+	qtnf_debugfs_add_entry(bus, "msi_enabled", qtnf_dbg_msi_show);
+	qtnf_debugfs_add_entry(bus, "hdp_stats", qtnf_dbg_hdp_stats);
+	qtnf_debugfs_add_entry(bus, "irq_stats", qtnf_dbg_irq_stats);
+	qtnf_debugfs_add_entry(bus, "shm_stats", qtnf_dbg_shm_stats);
+
+	return 0;
+
+err_bringup_fw:
+	netif_napi_del(&bus->mux_napi);
+
+err_base:
+	flush_workqueue(pcie_priv->workqueue);
+	destroy_workqueue(pcie_priv->workqueue);
+
+err_priv:
+	pci_set_drvdata(pdev, NULL);
+
+err_init:
+	return ret;
+}
+
+static void qtnf_pcie_remove(struct pci_dev *pdev)
+{
+	struct qtnf_pcie_bus_priv *priv;
+	struct qtnf_bus *bus;
+
+	bus = pci_get_drvdata(pdev);
+	if (!bus)
+		return;
+
+	priv = get_bus_priv(bus);
+
+	qtnf_core_detach(bus);
+	netif_napi_del(&bus->mux_napi);
+
+	flush_workqueue(priv->workqueue);
+	destroy_workqueue(priv->workqueue);
+	tasklet_kill(&priv->reclaim_tq);
+
+	qtnf_debugfs_remove(bus);
+
+	qtnf_pcie_free_shm_ipc(priv);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int qtnf_pcie_suspend(struct device *dev)
+{
+	return -EOPNOTSUPP;
+}
+
+static int qtnf_pcie_resume(struct device *dev)
+{
+	return 0;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+#ifdef CONFIG_PM_SLEEP
+/* Power Management Hooks */
+static SIMPLE_DEV_PM_OPS(qtnf_pcie_pm_ops, qtnf_pcie_suspend,
+			 qtnf_pcie_resume);
+#endif
+
+static struct pci_device_id qtnf_pcie_devid_table[] = {
+	{
+		PCIE_VENDOR_ID_QUANTENNA, PCIE_DEVICE_ID_QTN_PEARL,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+	},
+	{ },
+};
+
+MODULE_DEVICE_TABLE(pci, qtnf_pcie_devid_table);
+
+static struct pci_driver qtnf_pcie_drv_data = {
+	.name = DRV_NAME,
+	.id_table = qtnf_pcie_devid_table,
+	.probe = qtnf_pcie_probe,
+	.remove = qtnf_pcie_remove,
+#ifdef CONFIG_PM_SLEEP
+	.driver = {
+		.pm = &qtnf_pcie_pm_ops,
+	},
+#endif
+};
+
+static int __init qtnf_pcie_register(void)
+{
+	pr_info("register Quantenna QSR10g FullMAC PCIE driver\n");
+	return pci_register_driver(&qtnf_pcie_drv_data);
+}
+
+static void __exit qtnf_pcie_exit(void)
+{
+	pr_info("unregister Quantenna QSR10g FullMAC PCIE driver\n");
+	pci_unregister_driver(&qtnf_pcie_drv_data);
+}
+
+module_init(qtnf_pcie_register);
+module_exit(qtnf_pcie_exit);
+
+MODULE_AUTHOR("Quantenna Communications");
+MODULE_DESCRIPTION("Quantenna QSR10g PCIe bus driver for 802.11 wireless LAN.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/wireless/quantenna/qtnfmac/pearl/pcie_bus_priv.h b/drivers/net/wireless/quantenna/qtnfmac/pearl/pcie_bus_priv.h
new file mode 100644
index 000000000000..2a897db2bd79
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/pearl/pcie_bus_priv.h
@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _QTN_FMAC_PCIE_H_
+#define _QTN_FMAC_PCIE_H_
+
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+
+#include "pcie_regs_pearl.h"
+#include "pcie_ipc.h"
+#include "shm_ipc.h"
+
+struct bus;
+
+struct qtnf_pcie_bus_priv {
+	struct pci_dev  *pdev;
+
+	/* lock for irq configuration changes */
+	spinlock_t irq_lock;
+
+	/* lock for tx operations */
+	spinlock_t tx_lock;
+	u8 msi_enabled;
+	int mps;
+
+	struct workqueue_struct *workqueue;
+	struct tasklet_struct reclaim_tq;
+
+	void __iomem *sysctl_bar;
+	void __iomem *epmem_bar;
+	void __iomem *dmareg_bar;
+
+	struct qtnf_shm_ipc shm_ipc_ep_in;
+	struct qtnf_shm_ipc shm_ipc_ep_out;
+
+	struct qtnf_pcie_bda __iomem *bda;
+	void __iomem *pcie_reg_base;
+
+	u16 tx_bd_num;
+	u16 rx_bd_num;
+
+	struct sk_buff **tx_skb;
+	struct sk_buff **rx_skb;
+
+	struct qtnf_tx_bd *tx_bd_vbase;
+	dma_addr_t tx_bd_pbase;
+
+	struct qtnf_rx_bd *rx_bd_vbase;
+	dma_addr_t rx_bd_pbase;
+
+	dma_addr_t bd_table_paddr;
+	void *bd_table_vaddr;
+	u32 bd_table_len;
+
+	u32 hw_txproc_wr_ptr;
+
+	u16 tx_bd_reclaim_start;
+	u16 tx_bd_index;
+	u32 tx_queue_len;
+
+	u16 rx_bd_index;
+
+	u32 pcie_irq_mask;
+
+	/* diagnostics stats */
+	u32 pcie_irq_count;
+	u32 pcie_irq_rx_count;
+	u32 pcie_irq_tx_count;
+	u32 tx_full_count;
+	u32 tx_done_count;
+	u32 tx_reclaim_done;
+	u32 tx_reclaim_req;
+};
+
+#endif /* _QTN_FMAC_PCIE_H_ */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/pearl/pcie_ipc.h b/drivers/net/wireless/quantenna/qtnfmac/pearl/pcie_ipc.h
new file mode 100644
index 000000000000..e00d508fbcf0
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/pearl/pcie_ipc.h
@@ -0,0 +1,158 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _QTN_FMAC_PCIE_IPC_H_
+#define _QTN_FMAC_PCIE_IPC_H_
+
+#include <linux/types.h>
+
+#include "shm_ipc_defs.h"
+
+/* bitmap for EP status and flags: updated by EP, read by RC */
+#define QTN_EP_HAS_UBOOT	BIT(0)
+#define QTN_EP_HAS_FIRMWARE	BIT(1)
+#define QTN_EP_REQ_UBOOT	BIT(2)
+#define QTN_EP_REQ_FIRMWARE	BIT(3)
+#define QTN_EP_ERROR_UBOOT	BIT(4)
+#define QTN_EP_ERROR_FIRMWARE	BIT(5)
+
+#define QTN_EP_FW_LOADRDY	BIT(8)
+#define QTN_EP_FW_SYNC		BIT(9)
+#define QTN_EP_FW_RETRY		BIT(10)
+#define QTN_EP_FW_QLINK_DONE	BIT(15)
+#define QTN_EP_FW_DONE		BIT(16)
+
+/* bitmap for RC status and flags: updated by RC, read by EP */
+#define QTN_RC_PCIE_LINK	BIT(0)
+#define QTN_RC_NET_LINK		BIT(1)
+#define QTN_RC_FW_FLASHBOOT	BIT(5)
+#define QTN_RC_FW_QLINK		BIT(7)
+#define QTN_RC_FW_LOADRDY	BIT(8)
+#define QTN_RC_FW_SYNC		BIT(9)
+
+/* state transition timeouts */
+#define QTN_FW_DL_TIMEOUT_MS	3000
+#define QTN_FW_QLINK_TIMEOUT_MS	30000
+
+#define PCIE_HDP_INT_RX_BITS (0		\
+	| PCIE_HDP_INT_EP_TXDMA		\
+	| PCIE_HDP_INT_EP_TXEMPTY	\
+	)
+
+#define PCIE_HDP_INT_TX_BITS (0		\
+	| PCIE_HDP_INT_EP_RXDMA		\
+	)
+
+#if BITS_PER_LONG == 64
+#define QTN_HOST_HI32(a)	((u32)(((u64)a) >> 32))
+#define QTN_HOST_LO32(a)	((u32)(((u64)a) & 0xffffffffUL))
+#define QTN_HOST_ADDR(h, l)	((((u64)h) << 32) | ((u64)l))
+#elif BITS_PER_LONG == 32
+#define QTN_HOST_HI32(a)	0
+#define QTN_HOST_LO32(a)	((u32)(((u32)a) & 0xffffffffUL))
+#define QTN_HOST_ADDR(h, l)	((u32)l)
+#else
+#error Unexpected BITS_PER_LONG value
+#endif
+
+#define QTN_SYSCTL_BAR	0
+#define QTN_SHMEM_BAR	2
+#define QTN_DMA_BAR	3
+
+#define QTN_PCIE_BDA_VERSION		0x1002
+
+#define PCIE_BDA_NAMELEN		32
+#define PCIE_HHBM_MAX_SIZE		512
+
+#define SKB_BUF_SIZE		2048
+
+#define QTN_PCIE_BOARDFLG	"PCIEQTN"
+#define QTN_PCIE_FW_DLMASK	0xF
+#define QTN_PCIE_FW_BUFSZ	2048
+
+#define QTN_ENET_ADDR_LENGTH	6
+
+#define QTN_TXDONE_MASK		((u32)0x80000000)
+#define QTN_GET_LEN(x)		((x) & 0xFFFF)
+
+#define QTN_PCIE_TX_DESC_LEN_MASK	0xFFFF
+#define QTN_PCIE_TX_DESC_LEN_SHIFT	0
+#define QTN_PCIE_TX_DESC_PORT_MASK	0xF
+#define QTN_PCIE_TX_DESC_PORT_SHIFT	16
+#define QTN_PCIE_TX_DESC_TQE_BIT	BIT(24)
+
+#define QTN_EP_LHOST_TQE_PORT	4
+
+enum qtnf_pcie_bda_ipc_flags {
+	QTN_PCIE_IPC_FLAG_HBM_MAGIC	= BIT(0),
+	QTN_PCIE_IPC_FLAG_SHM_PIO	= BIT(1),
+};
+
+struct qtnf_pcie_bda {
+	__le16 bda_len;
+	__le16 bda_version;
+	__le32 bda_pci_endian;
+	__le32 bda_ep_state;
+	__le32 bda_rc_state;
+	__le32 bda_dma_mask;
+	__le32 bda_msi_addr;
+	__le32 bda_flashsz;
+	u8 bda_boardname[PCIE_BDA_NAMELEN];
+	__le32 bda_rc_msi_enabled;
+	__le32 bda_hhbm_list[PCIE_HHBM_MAX_SIZE];
+	__le32 bda_dsbw_start_index;
+	__le32 bda_dsbw_end_index;
+	__le32 bda_dsbw_total_bytes;
+	__le32 bda_rc_tx_bd_base;
+	__le32 bda_rc_tx_bd_num;
+	u8 bda_pcie_mac[QTN_ENET_ADDR_LENGTH];
+	struct qtnf_shm_ipc_region bda_shm_reg1 __aligned(4096); /* host TX */
+	struct qtnf_shm_ipc_region bda_shm_reg2 __aligned(4096); /* host RX */
+} __packed;
+
+struct qtnf_tx_bd {
+	__le32 addr;
+	__le32 addr_h;
+	__le32 info;
+	__le32 info_h;
+} __packed;
+
+struct qtnf_rx_bd {
+	__le32 addr;
+	__le32 addr_h;
+	__le32 info;
+	__le32 info_h;
+	__le32 next_ptr;
+	__le32 next_ptr_h;
+} __packed;
+
+enum qtnf_fw_loadtype {
+	QTN_FW_DBEGIN,
+	QTN_FW_DSUB,
+	QTN_FW_DEND,
+	QTN_FW_CTRL
+};
+
+struct qtnf_pcie_fw_hdr {
+	u8 boardflg[8];
+	__le32 fwsize;
+	__le32 seqnum;
+	__le32 type;
+	__le32 pktlen;
+	__le32 crc;
+} __packed;
+
+#endif /* _QTN_FMAC_PCIE_IPC_H_ */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/pearl/pcie_regs_pearl.h b/drivers/net/wireless/quantenna/qtnfmac/pearl/pcie_regs_pearl.h
new file mode 100644
index 000000000000..78715b8a8ef9
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/pearl/pcie_regs_pearl.h
@@ -0,0 +1,353 @@
+/*
+ * Copyright (c) 2015 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef __PEARL_PCIE_H
+#define __PEARL_PCIE_H
+
+#define	PCIE_GEN2_BASE				(0xe9000000)
+#define	PCIE_GEN3_BASE				(0xe7000000)
+
+#define PEARL_CUR_PCIE_BASE			(PCIE_GEN2_BASE)
+#define PCIE_HDP_OFFSET				(0x2000)
+
+#define PCIE_HDP_CTRL(base)			((base) + 0x2c00)
+#define PCIE_HDP_AXI_CTRL(base)			((base) + 0x2c04)
+#define PCIE_HDP_HOST_WR_DESC0(base)		((base) + 0x2c10)
+#define PCIE_HDP_HOST_WR_DESC0_H(base)		((base) + 0x2c14)
+#define PCIE_HDP_HOST_WR_DESC1(base)		((base) + 0x2c18)
+#define PCIE_HDP_HOST_WR_DESC1_H(base)		((base) + 0x2c1c)
+#define PCIE_HDP_HOST_WR_DESC2(base)		((base) + 0x2c20)
+#define PCIE_HDP_HOST_WR_DESC2_H(base)		((base) + 0x2c24)
+#define PCIE_HDP_HOST_WR_DESC3(base)		((base) + 0x2c28)
+#define PCIE_HDP_HOST_WR_DESC4_H(base)		((base) + 0x2c2c)
+#define PCIE_HDP_RX_INT_CTRL(base)		((base) + 0x2c30)
+#define PCIE_HDP_TX_INT_CTRL(base)		((base) + 0x2c34)
+#define PCIE_HDP_INT_STATUS(base)		((base) + 0x2c38)
+#define PCIE_HDP_INT_EN(base)			((base) + 0x2c3c)
+#define PCIE_HDP_RX_DESC0_PTR(base)		((base) + 0x2c40)
+#define PCIE_HDP_RX_DESC0_NOE(base)		((base) + 0x2c44)
+#define PCIE_HDP_RX_DESC1_PTR(base)		((base) + 0x2c48)
+#define PCIE_HDP_RX_DESC1_NOE(base)		((base) + 0x2c4c)
+#define PCIE_HDP_RX_DESC2_PTR(base)		((base) + 0x2c50)
+#define PCIE_HDP_RX_DESC2_NOE(base)		((base) + 0x2c54)
+#define PCIE_HDP_RX_DESC3_PTR(base)		((base) + 0x2c58)
+#define PCIE_HDP_RX_DESC3_NOE(base)		((base) + 0x2c5c)
+
+#define PCIE_HDP_TX0_BASE_ADDR(base)		((base) + 0x2c60)
+#define PCIE_HDP_TX1_BASE_ADDR(base)		((base) + 0x2c64)
+#define PCIE_HDP_TX0_Q_CTRL(base)		((base) + 0x2c70)
+#define PCIE_HDP_TX1_Q_CTRL(base)		((base) + 0x2c74)
+#define PCIE_HDP_CFG0(base)			((base) + 0x2c80)
+#define PCIE_HDP_CFG1(base)			((base) + 0x2c84)
+#define PCIE_HDP_CFG2(base)			((base) + 0x2c88)
+#define PCIE_HDP_CFG3(base)			((base) + 0x2c8c)
+#define PCIE_HDP_CFG4(base)			((base) + 0x2c90)
+#define PCIE_HDP_CFG5(base)			((base) + 0x2c94)
+#define PCIE_HDP_CFG6(base)			((base) + 0x2c98)
+#define PCIE_HDP_CFG7(base)			((base) + 0x2c9c)
+#define PCIE_HDP_CFG8(base)			((base) + 0x2ca0)
+#define PCIE_HDP_CFG9(base)			((base) + 0x2ca4)
+#define PCIE_HDP_CFG10(base)			((base) + 0x2ca8)
+#define PCIE_HDP_CFG11(base)			((base) + 0x2cac)
+#define PCIE_INT(base)				((base) + 0x2cb0)
+#define PCIE_INT_MASK(base)			((base) + 0x2cb4)
+#define PCIE_MSI_MASK(base)			((base) + 0x2cb8)
+#define PCIE_MSI_PNDG(base)			((base) + 0x2cbc)
+#define PCIE_PRI_CFG(base)			((base) + 0x2cc0)
+#define PCIE_PHY_CR(base)			((base) + 0x2cc4)
+#define PCIE_HDP_CTAG_CTRL(base)		((base) + 0x2cf4)
+#define PCIE_HDP_HHBM_BUF_PTR(base)		((base) + 0x2d00)
+#define PCIE_HDP_HHBM_BUF_PTR_H(base)		((base) + 0x2d04)
+#define PCIE_HDP_HHBM_BUF_FIFO_NOE(base)	((base) + 0x2d04)
+#define PCIE_HDP_RX0DMA_CNT(base)		((base) + 0x2d10)
+#define PCIE_HDP_RX1DMA_CNT(base)		((base) + 0x2d14)
+#define PCIE_HDP_RX2DMA_CNT(base)		((base) + 0x2d18)
+#define PCIE_HDP_RX3DMA_CNT(base)		((base) + 0x2d1c)
+#define PCIE_HDP_TX0DMA_CNT(base)		((base) + 0x2d20)
+#define PCIE_HDP_TX1DMA_CNT(base)		((base) + 0x2d24)
+#define PCIE_HDP_RXDMA_CTRL(base)		((base) + 0x2d28)
+#define PCIE_HDP_TX_HOST_Q_SZ_CTRL(base)	((base) + 0x2d2c)
+#define PCIE_HDP_TX_HOST_Q_BASE_L(base)		((base) + 0x2d30)
+#define PCIE_HDP_TX_HOST_Q_BASE_H(base)		((base) + 0x2d34)
+#define PCIE_HDP_TX_HOST_Q_WR_PTR(base)		((base) + 0x2d38)
+#define PCIE_HDP_TX_HOST_Q_RD_PTR(base)		((base) + 0x2d3c)
+#define PCIE_HDP_TX_HOST_Q_STS(base)		((base) + 0x2d40)
+
+/* Host HBM pool registers */
+#define PCIE_HHBM_CSR_REG(base)			((base) + 0x2e00)
+#define PCIE_HHBM_Q_BASE_REG(base)		((base) + 0x2e04)
+#define PCIE_HHBM_Q_LIMIT_REG(base)		((base) + 0x2e08)
+#define PCIE_HHBM_Q_WR_REG(base)		((base) + 0x2e0c)
+#define PCIE_HHBM_Q_RD_REG(base)		((base) + 0x2e10)
+#define PCIE_HHBM_POOL_DATA_0_H(base)		((base) + 0x2e90)
+#define PCIE_HHBM_CONFIG(base)			((base) + 0x2f9c)
+#define PCIE_HHBM_POOL_REQ_0(base)		((base) + 0x2f10)
+#define PCIE_HHBM_POOL_DATA_0(base)		((base) + 0x2f40)
+#define PCIE_HHBM_WATERMARK_MASKED_INT(base)	((base) + 0x2f68)
+#define PCIE_HHBM_WATERMARK_INT(base)		((base) + 0x2f6c)
+#define PCIE_HHBM_POOL_WATERMARK(base)		((base) + 0x2f70)
+#define PCIE_HHBM_POOL_OVERFLOW_CNT(base)	((base) + 0x2f90)
+#define PCIE_HHBM_POOL_UNDERFLOW_CNT(base)	((base) + 0x2f94)
+#define HBM_INT_STATUS(base)			((base) + 0x2f9c)
+#define PCIE_HHBM_POOL_CNFIG(base)		((base) + 0x2f9c)
+
+/* host HBM bit field definition */
+#define HHBM_CONFIG_SOFT_RESET			(BIT(8))
+#define HHBM_WR_REQ				(BIT(0))
+#define HHBM_RD_REQ				(BIT(1))
+#define HHBM_DONE				(BIT(31))
+
+/* offsets for dual PCIE */
+#define PCIE_PORT_LINK_CTL(base)		((base) + 0x0710)
+#define PCIE_GEN2_CTL(base)			((base) + 0x080C)
+#define PCIE_GEN3_OFF(base)			((base) + 0x0890)
+#define PCIE_ATU_CTRL1(base)			((base) + 0x0904)
+#define PCIE_ATU_CTRL2(base)			((base) + 0x0908)
+#define PCIE_ATU_BASE_LOW(base)			((base) + 0x090C)
+#define PCIE_ATU_BASE_HIGH(base)		((base) + 0x0910)
+#define PCIE_ATU_BASE_LIMIT(base)		((base) + 0x0914)
+#define PCIE_ATU_TGT_LOW(base)			((base) + 0x0918)
+#define PCIE_ATU_TGT_HIGH(base)			((base) + 0x091C)
+#define PCIE_DMA_WR_ENABLE(base)		((base) + 0x097C)
+#define PCIE_DMA_WR_CHWTLOW(base)		((base) + 0x0988)
+#define PCIE_DMA_WR_CHWTHIG(base)		((base) + 0x098C)
+#define PCIE_DMA_WR_INTSTS(base)		((base) + 0x09BC)
+#define PCIE_DMA_WR_INTMASK(base)		((base) + 0x09C4)
+#define PCIE_DMA_WR_INTCLER(base)		((base) + 0x09C8)
+#define PCIE_DMA_WR_DONE_IMWR_ADDR_L(base)	((base) + 0x09D0)
+#define PCIE_DMA_WR_DONE_IMWR_ADDR_H(base)	((base) + 0x09D4)
+#define PCIE_DMA_WR_ABORT_IMWR_ADDR_L(base)	((base) + 0x09D8)
+#define PCIE_DMA_WR_ABORT_IMWR_ADDR_H(base)	((base) + 0x09DC)
+#define PCIE_DMA_WR_IMWR_DATA(base)		((base) + 0x09E0)
+#define PCIE_DMA_WR_LL_ERR_EN(base)		((base) + 0x0A00)
+#define PCIE_DMA_WR_DOORBELL(base)		((base) + 0x0980)
+#define PCIE_DMA_RD_ENABLE(base)		((base) + 0x099C)
+#define PCIE_DMA_RD_DOORBELL(base)		((base) + 0x09A0)
+#define PCIE_DMA_RD_CHWTLOW(base)		((base) + 0x09A8)
+#define PCIE_DMA_RD_CHWTHIG(base)		((base) + 0x09AC)
+#define PCIE_DMA_RD_INTSTS(base)		((base) + 0x0A10)
+#define PCIE_DMA_RD_INTMASK(base)		((base) + 0x0A18)
+#define PCIE_DMA_RD_INTCLER(base)		((base) + 0x0A1C)
+#define PCIE_DMA_RD_ERR_STS_L(base)		((base) + 0x0A24)
+#define PCIE_DMA_RD_ERR_STS_H(base)		((base) + 0x0A28)
+#define PCIE_DMA_RD_LL_ERR_EN(base)		((base) + 0x0A34)
+#define PCIE_DMA_RD_DONE_IMWR_ADDR_L(base)	((base) + 0x0A3C)
+#define PCIE_DMA_RD_DONE_IMWR_ADDR_H(base)	((base) + 0x0A40)
+#define PCIE_DMA_RD_ABORT_IMWR_ADDR_L(base)	((base) + 0x0A44)
+#define PCIE_DMA_RD_ABORT_IMWR_ADDR_H(base)	((base) + 0x0A48)
+#define PCIE_DMA_RD_IMWR_DATA(base)		((base) + 0x0A4C)
+#define PCIE_DMA_CHNL_CONTEXT(base)		((base) + 0x0A6C)
+#define PCIE_DMA_CHNL_CNTRL(base)		((base) + 0x0A70)
+#define PCIE_DMA_XFR_SIZE(base)			((base) + 0x0A78)
+#define PCIE_DMA_SAR_LOW(base)			((base) + 0x0A7C)
+#define PCIE_DMA_SAR_HIGH(base)			((base) + 0x0A80)
+#define PCIE_DMA_DAR_LOW(base)			((base) + 0x0A84)
+#define PCIE_DMA_DAR_HIGH(base)			((base) + 0x0A88)
+#define PCIE_DMA_LLPTR_LOW(base)		((base) + 0x0A8C)
+#define PCIE_DMA_LLPTR_HIGH(base)		((base) + 0x0A90)
+#define PCIE_DMA_WRLL_ERR_ENB(base)		((base) + 0x0A00)
+#define PCIE_DMA_RDLL_ERR_ENB(base)		((base) + 0x0A34)
+#define PCIE_DMABD_CHNL_CNTRL(base)		((base) + 0x8000)
+#define PCIE_DMABD_XFR_SIZE(base)		((base) + 0x8004)
+#define PCIE_DMABD_SAR_LOW(base)		((base) + 0x8008)
+#define PCIE_DMABD_SAR_HIGH(base)		((base) + 0x800c)
+#define PCIE_DMABD_DAR_LOW(base)		((base) + 0x8010)
+#define PCIE_DMABD_DAR_HIGH(base)		((base) + 0x8014)
+#define PCIE_DMABD_LLPTR_LOW(base)		((base) + 0x8018)
+#define PCIE_DMABD_LLPTR_HIGH(base)		((base) + 0x801c)
+#define PCIE_WRDMA0_CHNL_CNTRL(base)		((base) + 0x8000)
+#define PCIE_WRDMA0_XFR_SIZE(base)		((base) + 0x8004)
+#define PCIE_WRDMA0_SAR_LOW(base)		((base) + 0x8008)
+#define PCIE_WRDMA0_SAR_HIGH(base)		((base) + 0x800c)
+#define PCIE_WRDMA0_DAR_LOW(base)		((base) + 0x8010)
+#define PCIE_WRDMA0_DAR_HIGH(base)		((base) + 0x8014)
+#define PCIE_WRDMA0_LLPTR_LOW(base)		((base) + 0x8018)
+#define PCIE_WRDMA0_LLPTR_HIGH(base)		((base) + 0x801c)
+#define PCIE_WRDMA1_CHNL_CNTRL(base)		((base) + 0x8020)
+#define PCIE_WRDMA1_XFR_SIZE(base)		((base) + 0x8024)
+#define PCIE_WRDMA1_SAR_LOW(base)		((base) + 0x8028)
+#define PCIE_WRDMA1_SAR_HIGH(base)		((base) + 0x802c)
+#define PCIE_WRDMA1_DAR_LOW(base)		((base) + 0x8030)
+#define PCIE_WRDMA1_DAR_HIGH(base)		((base) + 0x8034)
+#define PCIE_WRDMA1_LLPTR_LOW(base)		((base) + 0x8038)
+#define PCIE_WRDMA1_LLPTR_HIGH(base)		((base) + 0x803c)
+#define PCIE_RDDMA0_CHNL_CNTRL(base)		((base) + 0x8040)
+#define PCIE_RDDMA0_XFR_SIZE(base)		((base) + 0x8044)
+#define PCIE_RDDMA0_SAR_LOW(base)		((base) + 0x8048)
+#define PCIE_RDDMA0_SAR_HIGH(base)		((base) + 0x804c)
+#define PCIE_RDDMA0_DAR_LOW(base)		((base) + 0x8050)
+#define PCIE_RDDMA0_DAR_HIGH(base)		((base) + 0x8054)
+#define PCIE_RDDMA0_LLPTR_LOW(base)		((base) + 0x8058)
+#define PCIE_RDDMA0_LLPTR_HIGH(base)		((base) + 0x805c)
+#define PCIE_RDDMA1_CHNL_CNTRL(base)		((base) + 0x8060)
+#define PCIE_RDDMA1_XFR_SIZE(base)		((base) + 0x8064)
+#define PCIE_RDDMA1_SAR_LOW(base)		((base) + 0x8068)
+#define PCIE_RDDMA1_SAR_HIGH(base)		((base) + 0x806c)
+#define PCIE_RDDMA1_DAR_LOW(base)		((base) + 0x8070)
+#define PCIE_RDDMA1_DAR_HIGH(base)		((base) + 0x8074)
+#define PCIE_RDDMA1_LLPTR_LOW(base)		((base) + 0x8078)
+#define PCIE_RDDMA1_LLPTR_HIGH(base)		((base) + 0x807c)
+
+#define PCIE_ID(base)				((base) + 0x0000)
+#define PCIE_CMD(base)				((base) + 0x0004)
+#define PCIE_BAR(base, n)			((base) + 0x0010 + ((n) << 2))
+#define PCIE_CAP_PTR(base)			((base) + 0x0034)
+#define PCIE_MSI_LBAR(base)			((base) + 0x0054)
+#define PCIE_MSI_CTRL(base)			((base) + 0x0050)
+#define PCIE_MSI_ADDR_L(base)			((base) + 0x0054)
+#define PCIE_MSI_ADDR_H(base)			((base) + 0x0058)
+#define PCIE_MSI_DATA(base)			((base) + 0x005C)
+#define PCIE_MSI_MASK_BIT(base)			((base) + 0x0060)
+#define PCIE_MSI_PEND_BIT(base)			((base) + 0x0064)
+#define PCIE_DEVCAP(base)			((base) + 0x0074)
+#define PCIE_DEVCTLSTS(base)			((base) + 0x0078)
+
+#define PCIE_CMDSTS(base)			((base) + 0x0004)
+#define PCIE_LINK_STAT(base)			((base) + 0x80)
+#define PCIE_LINK_CTL2(base)			((base) + 0xa0)
+#define PCIE_ASPM_L1_CTRL(base)			((base) + 0x70c)
+#define PCIE_ASPM_LINK_CTRL(base)		(PCIE_LINK_STAT)
+#define PCIE_ASPM_L1_SUBSTATE_TIMING(base)	((base) + 0xB44)
+#define PCIE_L1SUB_CTRL1(base)			((base) + 0x150)
+#define PCIE_PMCSR(base)			((base) + 0x44)
+#define PCIE_CFG_SPACE_LIMIT(base)		((base) + 0x100)
+
+/* PCIe link defines */
+#define PEARL_PCIE_LINKUP			(0x7)
+#define PEARL_PCIE_DATA_LINK			(BIT(0))
+#define PEARL_PCIE_PHY_LINK			(BIT(1))
+#define PEARL_PCIE_LINK_RST			(BIT(3))
+#define PEARL_PCIE_FATAL_ERR			(BIT(5))
+#define PEARL_PCIE_NONFATAL_ERR			(BIT(6))
+
+/* PCIe Lane defines */
+#define PCIE_G2_LANE_X1				((BIT(0)) << 16)
+#define PCIE_G2_LANE_X2				((BIT(0) | BIT(1)) << 16)
+
+/* PCIe DLL link enable */
+#define PCIE_DLL_LINK_EN			((BIT(0)) << 5)
+
+#define PCIE_LINK_GEN1				(BIT(0))
+#define PCIE_LINK_GEN2				(BIT(1))
+#define PCIE_LINK_GEN3				(BIT(2))
+#define PCIE_LINK_MODE(x)			(((x) >> 16) & 0x7)
+
+#define MSI_EN					(BIT(0))
+#define MSI_64_EN				(BIT(7))
+#define PCIE_MSI_ADDR_OFFSET(a)			((a) & 0xFFFF)
+#define PCIE_MSI_ADDR_ALIGN(a)			((a) & (~0xFFFF))
+
+#define PCIE_BAR_MASK(base, n)			((base) + 0x1010 + ((n) << 2))
+#define PCIE_MAX_BAR				(6)
+
+#define PCIE_ATU_VIEW(base)			((base) + 0x0900)
+#define PCIE_ATU_CTL1(base)			((base) + 0x0904)
+#define PCIE_ATU_CTL2(base)			((base) + 0x0908)
+#define PCIE_ATU_LBAR(base)			((base) + 0x090c)
+#define PCIE_ATU_UBAR(base)			((base) + 0x0910)
+#define PCIE_ATU_LAR(base)			((base) + 0x0914)
+#define PCIE_ATU_LTAR(base)			((base) + 0x0918)
+#define PCIE_ATU_UTAR(base)			((base) + 0x091c)
+
+#define PCIE_MSI_ADDR_LOWER(base)		((base) + 0x0820)
+#define PCIE_MSI_ADDR_UPPER(base)		((base) + 0x0824)
+#define PCIE_MSI_ENABLE(base)			((base) + 0x0828)
+#define PCIE_MSI_MASK_RC(base)			((base) + 0x082c)
+#define PCIE_MSI_STATUS(base)			((base) + 0x0830)
+#define PEARL_PCIE_MSI_REGION			(0xce000000)
+#define PEARL_PCIE_MSI_DATA			(0)
+#define PCIE_MSI_GPIO(base)			((base) + 0x0888)
+
+#define PCIE_HDP_HOST_QUEUE_FULL	(BIT(17))
+#define USE_BAR_MATCH_MODE
+#define PCIE_ATU_OB_REGION		(BIT(0))
+#define PCIE_ATU_EN_REGION		(BIT(31))
+#define PCIE_ATU_EN_MATCH		(BIT(30))
+#define PCIE_BASE_REGION		(0xb0000000)
+#define PCIE_MEM_MAP_SIZE		(512 * 1024)
+
+#define PCIE_OB_REG_REGION		(0xcf000000)
+#define PCIE_CONFIG_REGION		(0xcf000000)
+#define PCIE_CONFIG_SIZE		(4096)
+#define PCIE_CONFIG_CH			(1)
+
+/* inbound mapping */
+#define PCIE_IB_BAR0			(0x00000000)	/* ddr */
+#define PCIE_IB_BAR0_CH			(0)
+#define PCIE_IB_BAR3			(0xe0000000)	/* sys_reg */
+#define PCIE_IB_BAR3_CH			(1)
+
+/* outbound mapping */
+#define PCIE_MEM_CH			(0)
+#define PCIE_REG_CH			(1)
+#define PCIE_MEM_REGION			(0xc0000000)
+#define	PCIE_MEM_SIZE			(0x000fffff)
+#define PCIE_MEM_TAR			(0x80000000)
+
+#define PCIE_MSI_REGION			(0xce000000)
+#define PCIE_MSI_SIZE			(KBYTE(4) - 1)
+#define PCIE_MSI_CH			(1)
+
+/* size of config region */
+#define PCIE_CFG_SIZE			(0x0000ffff)
+
+#define PCIE_ATU_DIR_IB			(BIT(31))
+#define PCIE_ATU_DIR_OB			(0)
+#define PCIE_ATU_DIR_CFG		(2)
+#define PCIE_ATU_DIR_MATCH_IB		(BIT(31) | BIT(30))
+
+#define PCIE_DMA_WR_0			(0)
+#define PCIE_DMA_WR_1			(1)
+#define PCIE_DMA_RD_0			(2)
+#define PCIE_DMA_RD_1			(3)
+
+#define PCIE_DMA_CHNL_CNTRL_CB		(BIT(0))
+#define PCIE_DMA_CHNL_CNTRL_TCB		(BIT(1))
+#define PCIE_DMA_CHNL_CNTRL_LLP		(BIT(2))
+#define PCIE_DMA_CHNL_CNTRL_LIE		(BIT(3))
+#define PCIE_DMA_CHNL_CNTRL_RIE		(BIT(4))
+#define PCIE_DMA_CHNL_CNTRL_CSS		(BIT(8))
+#define PCIE_DMA_CHNL_CNTRL_LLE		(BIT(9))
+#define PCIE_DMA_CHNL_CNTRL_TLP		(BIT(26))
+
+#define PCIE_DMA_CHNL_CONTEXT_RD	(BIT(31))
+#define PCIE_DMA_CHNL_CONTEXT_WR	(0)
+#define PCIE_MAX_BAR			(6)
+
+/* PCIe HDP interrupt status definition */
+#define PCIE_HDP_INT_EP_RXDMA		(BIT(0))
+#define PCIE_HDP_INT_HBM_UF		(BIT(1))
+#define PCIE_HDP_INT_RX_LEN_ERR		(BIT(2))
+#define PCIE_HDP_INT_RX_HDR_LEN_ERR	(BIT(3))
+#define PCIE_HDP_INT_EP_TXDMA		(BIT(12))
+#define PCIE_HDP_INT_EP_TXEMPTY		(BIT(15))
+#define PCIE_HDP_INT_IPC		(BIT(29))
+
+/* PCIe interrupt status definition */
+#define PCIE_INT_MSI			(BIT(24))
+#define PCIE_INT_INTX			(BIT(23))
+
+/* PCIe legacy INTx */
+#define PEARL_PCIE_CFG0_OFFSET		(0x6C)
+#define PEARL_ASSERT_INTX		(BIT(9))
+
+/* SYS CTL regs */
+#define QTN_PEARL_SYSCTL_LHOST_IRQ_OFFSET	(0x001C)
+
+#define QTN_PEARL_IPC_IRQ_WORD(irq)	(BIT(irq) | BIT(irq + 16))
+#define QTN_PEARL_LHOST_IPC_IRQ		(6)
+
+#endif /* __PEARL_PCIE_H */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/qlink.h b/drivers/net/wireless/quantenna/qtnfmac/qlink.h
new file mode 100644
index 000000000000..6eafc15e0065
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/qlink.h
@@ -0,0 +1,901 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _QTN_QLINK_H_
+#define _QTN_QLINK_H_
+
+#include <linux/ieee80211.h>
+
+#define QLINK_PROTO_VER		3
+
+#define QLINK_MACID_RSVD		0xFF
+#define QLINK_VIFID_RSVD		0xFF
+
+/* Common QLINK protocol messages definitions.
+ */
+
+/**
+ * enum qlink_msg_type - QLINK message types
+ *
+ * Used to distinguish between message types of QLINK protocol.
+ *
+ * @QLINK_MSG_TYPE_CMD: Message is carrying data of a command sent from
+ *	driver to wireless hardware.
+ * @QLINK_MSG_TYPE_CMDRSP: Message is carrying data of a response to a command.
+ *	Sent from wireless HW to driver in reply to previously issued command.
+ * @QLINK_MSG_TYPE_EVENT: Data for an event originated in wireless hardware and
+ *	sent asynchronously to driver.
+ */
+enum qlink_msg_type {
+	QLINK_MSG_TYPE_CMD	= 1,
+	QLINK_MSG_TYPE_CMDRSP	= 2,
+	QLINK_MSG_TYPE_EVENT	= 3
+};
+
+/**
+ * struct qlink_msg_header - common QLINK protocol message header
+ *
+ * Portion of QLINK protocol header common for all message types.
+ *
+ * @type: Message type, one of &enum qlink_msg_type.
+ * @len: Total length of message including all headers.
+ */
+struct qlink_msg_header {
+	__le16 type;
+	__le16 len;
+} __packed;
+
+/* Generic definitions of data and information carried in QLINK messages
+ */
+
+enum qlink_hw_capab {
+	QLINK_HW_SUPPORTS_REG_UPDATE	= BIT(0),
+};
+
+enum qlink_phy_mode {
+	QLINK_PHYMODE_BGN	= BIT(0),
+	QLINK_PHYMODE_AN	= BIT(1),
+	QLINK_PHYMODE_AC	= BIT(2),
+};
+
+enum qlink_iface_type {
+	QLINK_IFTYPE_AP		= 1,
+	QLINK_IFTYPE_STATION	= 2,
+	QLINK_IFTYPE_ADHOC	= 3,
+	QLINK_IFTYPE_MONITOR	= 4,
+	QLINK_IFTYPE_WDS	= 5,
+};
+
+/**
+ * struct qlink_intf_info - information on virtual interface.
+ *
+ * Data describing a single virtual interface.
+ *
+ * @if_type: Mode of interface operation, one of &enum qlink_iface_type
+ * @flags: interface flagsmap.
+ * @mac_addr: MAC address of virtual interface.
+ */
+struct qlink_intf_info {
+	__le16 if_type;
+	__le16 flags;
+	u8 mac_addr[ETH_ALEN];
+	u8 rsvd[2];
+} __packed;
+
+enum qlink_sta_flags {
+	QLINK_STA_FLAG_INVALID		= 0,
+	QLINK_STA_FLAG_AUTHORIZED		= BIT(0),
+	QLINK_STA_FLAG_SHORT_PREAMBLE	= BIT(1),
+	QLINK_STA_FLAG_WME			= BIT(2),
+	QLINK_STA_FLAG_MFP			= BIT(3),
+	QLINK_STA_FLAG_AUTHENTICATED		= BIT(4),
+	QLINK_STA_FLAG_TDLS_PEER		= BIT(5),
+	QLINK_STA_FLAG_ASSOCIATED		= BIT(6),
+};
+
+enum qlink_channel_width {
+	QLINK_CHAN_WIDTH_5		= BIT(0),
+	QLINK_CHAN_WIDTH_10		= BIT(1),
+	QLINK_CHAN_WIDTH_20_NOHT	= BIT(2),
+	QLINK_CHAN_WIDTH_20		= BIT(3),
+	QLINK_CHAN_WIDTH_40		= BIT(4),
+	QLINK_CHAN_WIDTH_80		= BIT(5),
+	QLINK_CHAN_WIDTH_80P80		= BIT(6),
+	QLINK_CHAN_WIDTH_160		= BIT(7),
+};
+
+/* QLINK Command messages related definitions
+ */
+
+/**
+ * enum qlink_cmd_type - list of supported commands
+ *
+ * Commands are QLINK messages of type @QLINK_MSG_TYPE_CMD, sent by driver to
+ * wireless network device for processing. Device is expected to send back a
+ * reply message of type &QLINK_MSG_TYPE_CMDRSP, containing at least command
+ * execution status (one of &enum qlink_cmd_result) at least. Reply message
+ * may also contain data payload specific to the command type.
+ *
+ * @QLINK_CMD_CHANS_INFO_GET: for the specified MAC and specified band, get
+ *	number of operational channels and information on each of the channel.
+ *	This command is generic to a specified MAC, interface index must be set
+ *	to QLINK_VIFID_RSVD in command header.
+ */
+enum qlink_cmd_type {
+	QLINK_CMD_FW_INIT		= 0x0001,
+	QLINK_CMD_FW_DEINIT		= 0x0002,
+	QLINK_CMD_REGISTER_MGMT		= 0x0003,
+	QLINK_CMD_SEND_MGMT_FRAME	= 0x0004,
+	QLINK_CMD_MGMT_SET_APPIE	= 0x0005,
+	QLINK_CMD_PHY_PARAMS_GET	= 0x0011,
+	QLINK_CMD_PHY_PARAMS_SET	= 0x0012,
+	QLINK_CMD_GET_HW_INFO		= 0x0013,
+	QLINK_CMD_MAC_INFO		= 0x0014,
+	QLINK_CMD_ADD_INTF		= 0x0015,
+	QLINK_CMD_DEL_INTF		= 0x0016,
+	QLINK_CMD_CHANGE_INTF		= 0x0017,
+	QLINK_CMD_UPDOWN_INTF		= 0x0018,
+	QLINK_CMD_REG_REGION		= 0x0019,
+	QLINK_CMD_CHANS_INFO_GET	= 0x001A,
+	QLINK_CMD_CONFIG_AP		= 0x0020,
+	QLINK_CMD_START_AP		= 0x0021,
+	QLINK_CMD_STOP_AP		= 0x0022,
+	QLINK_CMD_GET_STA_INFO		= 0x0030,
+	QLINK_CMD_ADD_KEY		= 0x0040,
+	QLINK_CMD_DEL_KEY		= 0x0041,
+	QLINK_CMD_SET_DEFAULT_KEY	= 0x0042,
+	QLINK_CMD_SET_DEFAULT_MGMT_KEY	= 0x0043,
+	QLINK_CMD_CHANGE_STA		= 0x0051,
+	QLINK_CMD_DEL_STA		= 0x0052,
+	QLINK_CMD_SCAN			= 0x0053,
+	QLINK_CMD_CONNECT		= 0x0060,
+	QLINK_CMD_DISCONNECT		= 0x0061,
+};
+
+/**
+ * struct qlink_cmd - QLINK command message header
+ *
+ * Header used for QLINK messages of QLINK_MSG_TYPE_CMD type.
+ *
+ * @mhdr: Common QLINK message header.
+ * @cmd_id: command id, one of &enum qlink_cmd_type.
+ * @seq_num: sequence number of command message, used for matching with
+ *	response message.
+ * @macid: index of physical radio device the command is destined to or
+ *	QLINK_MACID_RSVD if not applicable.
+ * @vifid: index of virtual wireless interface on specified @macid the command
+ *	is destined to or QLINK_VIFID_RSVD if not applicable.
+ */
+struct qlink_cmd {
+	struct qlink_msg_header mhdr;
+	__le16 cmd_id;
+	__le16 seq_num;
+	u8 rsvd[2];
+	u8 macid;
+	u8 vifid;
+} __packed;
+
+/**
+ * struct qlink_cmd_manage_intf - interface management command
+ *
+ * Data for interface management commands QLINK_CMD_ADD_INTF, QLINK_CMD_DEL_INTF
+ * and QLINK_CMD_CHANGE_INTF.
+ *
+ * @intf_info: interface description.
+ */
+struct qlink_cmd_manage_intf {
+	struct qlink_cmd chdr;
+	struct qlink_intf_info intf_info;
+} __packed;
+
+enum qlink_mgmt_frame_type {
+	QLINK_MGMT_FRAME_ASSOC_REQ	= 0x00,
+	QLINK_MGMT_FRAME_ASSOC_RESP	= 0x01,
+	QLINK_MGMT_FRAME_REASSOC_REQ	= 0x02,
+	QLINK_MGMT_FRAME_REASSOC_RESP	= 0x03,
+	QLINK_MGMT_FRAME_PROBE_REQ	= 0x04,
+	QLINK_MGMT_FRAME_PROBE_RESP	= 0x05,
+	QLINK_MGMT_FRAME_BEACON		= 0x06,
+	QLINK_MGMT_FRAME_ATIM		= 0x07,
+	QLINK_MGMT_FRAME_DISASSOC	= 0x08,
+	QLINK_MGMT_FRAME_AUTH		= 0x09,
+	QLINK_MGMT_FRAME_DEAUTH		= 0x0A,
+	QLINK_MGMT_FRAME_ACTION		= 0x0B,
+
+	QLINK_MGMT_FRAME_TYPE_COUNT
+};
+
+/**
+ * struct qlink_cmd_mgmt_frame_register - data for QLINK_CMD_REGISTER_MGMT
+ *
+ * @frame_type: MGMT frame type the registration request describes, one of
+ *	&enum qlink_mgmt_frame_type.
+ * @do_register: 0 - unregister, otherwise register for reception of specified
+ *	MGMT frame type.
+ */
+struct qlink_cmd_mgmt_frame_register {
+	struct qlink_cmd chdr;
+	__le16 frame_type;
+	u8 do_register;
+} __packed;
+
+enum qlink_mgmt_frame_tx_flags {
+	QLINK_MGMT_FRAME_TX_FLAG_NONE		= 0,
+	QLINK_MGMT_FRAME_TX_FLAG_OFFCHAN	= BIT(0),
+	QLINK_MGMT_FRAME_TX_FLAG_NO_CCK		= BIT(1),
+	QLINK_MGMT_FRAME_TX_FLAG_ACK_NOWAIT	= BIT(2),
+};
+
+/**
+ * struct qlink_cmd_mgmt_frame_tx - data for QLINK_CMD_SEND_MGMT_FRAME command
+ *
+ * @cookie: opaque request identifier.
+ * @freq: Frequency to use for frame transmission.
+ * @flags: Transmission flags, one of &enum qlink_mgmt_frame_tx_flags.
+ * @frame_data: frame to transmit.
+ */
+struct qlink_cmd_mgmt_frame_tx {
+	struct qlink_cmd chdr;
+	__le32 cookie;
+	__le16 freq;
+	__le16 flags;
+	u8 frame_data[0];
+} __packed;
+
+/**
+ * struct qlink_cmd_mgmt_append_ie - data for QLINK_CMD_MGMT_SET_APPIE command
+ *
+ * @type: type of MGMT frame to appent requested IEs to, one of
+ *	&enum qlink_mgmt_frame_type.
+ * @flags: for future use.
+ * @ie_data: IEs data to append.
+ */
+struct qlink_cmd_mgmt_append_ie {
+	struct qlink_cmd chdr;
+	u8 type;
+	u8 flags;
+	u8 ie_data[0];
+} __packed;
+
+/**
+ * struct qlink_cmd_get_sta_info - data for QLINK_CMD_GET_STA_INFO command
+ *
+ * @sta_addr: MAC address of the STA statistics is requested for.
+ */
+struct qlink_cmd_get_sta_info {
+	struct qlink_cmd chdr;
+	u8 sta_addr[ETH_ALEN];
+} __packed;
+
+/**
+ * struct qlink_cmd_add_key - data for QLINK_CMD_ADD_KEY command.
+ *
+ * @key_index: index of the key being installed.
+ * @pairwise: whether to use pairwise key.
+ * @addr: MAC address of a STA key is being installed to.
+ * @cipher: cipher suite.
+ * @key_data: key data itself.
+ */
+struct qlink_cmd_add_key {
+	struct qlink_cmd chdr;
+	u8 key_index;
+	u8 pairwise;
+	u8 addr[ETH_ALEN];
+	__le32 cipher;
+	u8 key_data[0];
+} __packed;
+
+/**
+ * struct qlink_cmd_del_key_req - data for QLINK_CMD_DEL_KEY command
+ *
+ * @key_index: index of the key being removed.
+ * @pairwise: whether to use pairwise key.
+ * @addr: MAC address of a STA for which a key is removed.
+ */
+struct qlink_cmd_del_key {
+	struct qlink_cmd chdr;
+	u8 key_index;
+	u8 pairwise;
+	u8 addr[ETH_ALEN];
+} __packed;
+
+/**
+ * struct qlink_cmd_set_def_key - data for QLINK_CMD_SET_DEFAULT_KEY command
+ *
+ * @key_index: index of the key to be set as default one.
+ * @unicast: key is unicast.
+ * @multicast: key is multicast.
+ */
+struct qlink_cmd_set_def_key {
+	struct qlink_cmd chdr;
+	u8 key_index;
+	u8 unicast;
+	u8 multicast;
+} __packed;
+
+/**
+ * struct qlink_cmd_set_def_mgmt_key - data for QLINK_CMD_SET_DEFAULT_MGMT_KEY
+ *
+ * @key_index: index of the key to be set as default MGMT key.
+ */
+struct qlink_cmd_set_def_mgmt_key {
+	struct qlink_cmd chdr;
+	u8 key_index;
+} __packed;
+
+/**
+ * struct qlink_cmd_change_sta - data for QLINK_CMD_CHANGE_STA command
+ *
+ * @sta_flags_mask: STA flags mask, bitmap of &enum qlink_sta_flags
+ * @sta_flags_set: STA flags values, bitmap of &enum qlink_sta_flags
+ * @sta_addr: address of the STA for which parameters are set.
+ */
+struct qlink_cmd_change_sta {
+	struct qlink_cmd chdr;
+	__le32 sta_flags_mask;
+	__le32 sta_flags_set;
+	u8 sta_addr[ETH_ALEN];
+} __packed;
+
+/**
+ * struct qlink_cmd_del_sta - data for QLINK_CMD_DEL_STA command.
+ *
+ * See &struct station_del_parameters
+ */
+struct qlink_cmd_del_sta {
+	struct qlink_cmd chdr;
+	__le16 reason_code;
+	u8 subtype;
+	u8 sta_addr[ETH_ALEN];
+} __packed;
+
+enum qlink_sta_connect_flags {
+	QLINK_STA_CONNECT_DISABLE_HT	= BIT(0),
+	QLINK_STA_CONNECT_DISABLE_VHT	= BIT(1),
+	QLINK_STA_CONNECT_USE_RRM	= BIT(2),
+};
+
+/**
+ * struct qlink_cmd_connect - data for QLINK_CMD_CONNECT command
+ *
+ * @flags: for future use.
+ * @freq: center frequence of a channel which should be used to connect.
+ * @bg_scan_period: period of background scan.
+ * @bssid: BSSID of the BSS to connect to.
+ * @payload: variable portion of connection request.
+ */
+struct qlink_cmd_connect {
+	struct qlink_cmd chdr;
+	__le32 flags;
+	__le16 freq;
+	__le16 bg_scan_period;
+	u8 bssid[ETH_ALEN];
+	u8 payload[0];
+} __packed;
+
+/**
+ * struct qlink_cmd_disconnect - data for QLINK_CMD_DISCONNECT command
+ *
+ * @reason: code of the reason of disconnect, see &enum ieee80211_reasoncode.
+ */
+struct qlink_cmd_disconnect {
+	struct qlink_cmd chdr;
+	__le16 reason;
+} __packed;
+
+/**
+ * struct qlink_cmd_updown - data for QLINK_CMD_UPDOWN_INTF command
+ *
+ * @if_up: bring specified interface DOWN (if_up==0) or UP (otherwise).
+ *	Interface is specified in common command header @chdr.
+ */
+struct qlink_cmd_updown {
+	struct qlink_cmd chdr;
+	u8 if_up;
+} __packed;
+
+/**
+ * enum qlink_band - a list of frequency bands
+ *
+ * @QLINK_BAND_2GHZ: 2.4GHz band
+ * @QLINK_BAND_5GHZ: 5GHz band
+ * @QLINK_BAND_60GHZ: 60GHz band
+ */
+enum qlink_band {
+	QLINK_BAND_2GHZ = BIT(0),
+	QLINK_BAND_5GHZ = BIT(1),
+	QLINK_BAND_60GHZ = BIT(2),
+};
+
+/**
+ * struct qlink_cmd_chans_info_get - data for QLINK_CMD_CHANS_INFO_GET command
+ *
+ * @band: a PHY band for which channels info is needed, one of @enum qlink_band
+ */
+struct qlink_cmd_chans_info_get {
+	struct qlink_cmd chdr;
+	u8 band;
+} __packed;
+
+/* QLINK Command Responses messages related definitions
+ */
+
+enum qlink_cmd_result {
+	QLINK_CMD_RESULT_OK = 0,
+	QLINK_CMD_RESULT_INVALID,
+	QLINK_CMD_RESULT_ENOTSUPP,
+	QLINK_CMD_RESULT_ENOTFOUND,
+};
+
+/**
+ * struct qlink_resp - QLINK command response message header
+ *
+ * Header used for QLINK messages of QLINK_MSG_TYPE_CMDRSP type.
+ *
+ * @mhdr: see &struct qlink_msg_header.
+ * @cmd_id: command ID the response corresponds to, one of &enum qlink_cmd_type.
+ * @seq_num: sequence number of command message, used for matching with
+ *	response message.
+ * @result: result of the command execution, one of &enum qlink_cmd_result.
+ * @macid: index of physical radio device the response is sent from or
+ *	QLINK_MACID_RSVD if not applicable.
+ * @vifid: index of virtual wireless interface on specified @macid the response
+ *	is sent from or QLINK_VIFID_RSVD if not applicable.
+ */
+struct qlink_resp {
+	struct qlink_msg_header mhdr;
+	__le16 cmd_id;
+	__le16 seq_num;
+	__le16 result;
+	u8 macid;
+	u8 vifid;
+} __packed;
+
+/**
+ * struct qlink_resp_get_mac_info - response for QLINK_CMD_MAC_INFO command
+ *
+ * Data describing specific physical device providing wireless MAC
+ * functionality.
+ *
+ * @dev_mac: MAC address of physical WMAC device (used for first BSS on
+ *	specified WMAC).
+ * @num_tx_chain: Number of transmit chains used by WMAC.
+ * @num_rx_chain: Number of receive chains used by WMAC.
+ * @vht_cap: VHT capabilities.
+ * @ht_cap: HT capabilities.
+ * @bands_cap: wireless bands WMAC can operate in, bitmap of &enum qlink_band.
+ * @phymode_cap: PHY modes WMAC can operate in, bitmap of &enum qlink_phy_mode.
+ * @max_ap_assoc_sta: Maximum number of associations supported by WMAC.
+ * @radar_detect_widths: bitmask of channels BW for which WMAC can detect radar.
+ * @var_info: variable-length WMAC info data.
+ */
+struct qlink_resp_get_mac_info {
+	struct qlink_resp rhdr;
+	u8 dev_mac[ETH_ALEN];
+	u8 num_tx_chain;
+	u8 num_rx_chain;
+	struct ieee80211_vht_cap vht_cap;
+	struct ieee80211_ht_cap ht_cap;
+	u8 bands_cap;
+	u8 phymode_cap;
+	__le16 max_ap_assoc_sta;
+	__le16 radar_detect_widths;
+	u8 var_info[0];
+} __packed;
+
+/**
+ * struct qlink_resp_get_hw_info - response for QLINK_CMD_GET_HW_INFO command
+ *
+ * Description of wireless hardware capabilities and features.
+ *
+ * @fw_ver: wireless hardware firmware version.
+ * @hw_capab: Bitmap of capabilities supported by firmware.
+ * @ql_proto_ver: Version of QLINK protocol used by firmware.
+ * @country_code: country code ID firmware is configured to.
+ * @num_mac: Number of separate physical radio devices provided by hardware.
+ * @mac_bitmap: Bitmap of MAC IDs that are active and can be used in firmware.
+ * @total_tx_chains: total number of transmit chains used by device.
+ * @total_rx_chains: total number of receive chains.
+ */
+struct qlink_resp_get_hw_info {
+	struct qlink_resp rhdr;
+	__le32 fw_ver;
+	__le32 hw_capab;
+	__le16 ql_proto_ver;
+	u8 alpha2_code[2];
+	u8 num_mac;
+	u8 mac_bitmap;
+	u8 total_tx_chain;
+	u8 total_rx_chain;
+} __packed;
+
+/**
+ * struct qlink_resp_manage_intf - response for interface management commands
+ *
+ * Response data for QLINK_CMD_ADD_INTF and QLINK_CMD_CHANGE_INTF commands.
+ *
+ * @rhdr: Common Command Response message header.
+ * @intf_info: interface description.
+ */
+struct qlink_resp_manage_intf {
+	struct qlink_resp rhdr;
+	struct qlink_intf_info intf_info;
+} __packed;
+
+/**
+ * struct qlink_resp_get_sta_info - response for QLINK_CMD_GET_STA_INFO command
+ *
+ * Response data containing statistics for specified STA.
+ *
+ * @sta_addr: MAC address of STA the response carries statistic for.
+ * @info: statistics for specified STA.
+ */
+struct qlink_resp_get_sta_info {
+	struct qlink_resp rhdr;
+	u8 sta_addr[ETH_ALEN];
+	u8 info[0];
+} __packed;
+
+/**
+ * struct qlink_resp_get_chan_info - response for QLINK_CMD_CHANS_INFO_GET cmd
+ *
+ * @band: frequency band to which channels belong to, one of @enum qlink_band.
+ * @num_chans: total number of channels info data contained in reply data.
+ * @info: variable-length channels info.
+ */
+struct qlink_resp_get_chan_info {
+	struct qlink_resp rhdr;
+	u8 band;
+	u8 num_chans;
+	u8 rsvd[2];
+	u8 info[0];
+} __packed;
+
+/**
+ * struct qlink_resp_phy_params - response for QLINK_CMD_PHY_PARAMS_GET command
+ *
+ * @info: variable-length array of PHY params.
+ */
+struct qlink_resp_phy_params {
+	struct qlink_resp rhdr;
+	u8 info[0];
+} __packed;
+
+/* QLINK Events messages related definitions
+ */
+
+enum qlink_event_type {
+	QLINK_EVENT_STA_ASSOCIATED	= 0x0021,
+	QLINK_EVENT_STA_DEAUTH		= 0x0022,
+	QLINK_EVENT_MGMT_RECEIVED	= 0x0023,
+	QLINK_EVENT_SCAN_RESULTS	= 0x0024,
+	QLINK_EVENT_SCAN_COMPLETE	= 0x0025,
+	QLINK_EVENT_BSS_JOIN		= 0x0026,
+	QLINK_EVENT_BSS_LEAVE		= 0x0027,
+};
+
+/**
+ * struct qlink_event - QLINK event message header
+ *
+ * Header used for QLINK messages of QLINK_MSG_TYPE_EVENT type.
+ *
+ * @mhdr: Common QLINK message header.
+ * @event_id: Specifies specific event ID, one of &enum qlink_event_type.
+ * @macid: index of physical radio device the event was generated on or
+ *	QLINK_MACID_RSVD if not applicable.
+ * @vifid: index of virtual wireless interface on specified @macid the event
+ *	was generated on or QLINK_VIFID_RSVD if not applicable.
+ */
+struct qlink_event {
+	struct qlink_msg_header mhdr;
+	__le16 event_id;
+	u8 macid;
+	u8 vifid;
+} __packed;
+
+/**
+ * struct qlink_event_sta_assoc - data for QLINK_EVENT_STA_ASSOCIATED event
+ *
+ * @sta_addr: Address of a STA for which new association event was generated
+ * @frame_control: control bits from 802.11 ASSOC_REQUEST header.
+ * @payload: IEs from association request.
+ */
+struct qlink_event_sta_assoc {
+	struct qlink_event ehdr;
+	u8 sta_addr[ETH_ALEN];
+	__le16 frame_control;
+	u8 ies[0];
+} __packed;
+
+/**
+ * struct qlink_event_sta_deauth - data for QLINK_EVENT_STA_DEAUTH event
+ *
+ * @sta_addr: Address of a deauthenticated STA.
+ * @reason: reason for deauthentication.
+ */
+struct qlink_event_sta_deauth {
+	struct qlink_event ehdr;
+	u8 sta_addr[ETH_ALEN];
+	__le16 reason;
+} __packed;
+
+/**
+ * struct qlink_event_bss_join - data for QLINK_EVENT_BSS_JOIN event
+ *
+ * @bssid: BSSID of a BSS which interface tried to joined.
+ * @status: status of joining attempt, see &enum ieee80211_statuscode.
+ */
+struct qlink_event_bss_join {
+	struct qlink_event ehdr;
+	u8 bssid[ETH_ALEN];
+	__le16 status;
+} __packed;
+
+/**
+ * struct qlink_event_bss_leave - data for QLINK_EVENT_BSS_LEAVE event
+ *
+ * @reason: reason of disconnecting from BSS.
+ */
+struct qlink_event_bss_leave {
+	struct qlink_event ehdr;
+	u16 reason;
+} __packed;
+
+enum qlink_rxmgmt_flags {
+	QLINK_RXMGMT_FLAG_ANSWERED = 1 << 0,
+};
+
+/**
+ * struct qlink_event_rxmgmt - data for QLINK_EVENT_MGMT_RECEIVED event
+ *
+ * @freq: Frequency on which the frame was received in MHz.
+ * @sig_dbm: signal strength in dBm.
+ * @flags: bitmap of &enum qlink_rxmgmt_flags.
+ * @frame_data: data of Rx'd frame itself.
+ */
+struct qlink_event_rxmgmt {
+	struct qlink_event ehdr;
+	__le32 freq;
+	__le32 sig_dbm;
+	__le32 flags;
+	u8 frame_data[0];
+} __packed;
+
+enum qlink_frame_type {
+	QLINK_BSS_FTYPE_UNKNOWN,
+	QLINK_BSS_FTYPE_BEACON,
+	QLINK_BSS_FTYPE_PRESP,
+};
+
+/**
+ * struct qlink_event_scan_result - data for QLINK_EVENT_SCAN_RESULTS event
+ *
+ * @tsf: TSF timestamp indicating when scan results were generated.
+ * @freq: Center frequency of the channel where BSS for which the scan result
+ *	event was generated was discovered.
+ * @capab: capabilities field.
+ * @bintval: beacon interval announced by discovered BSS.
+ * @signal: signal strength.
+ * @frame_type: frame type used to get scan result, see &enum qlink_frame_type.
+ * @bssid: BSSID announced by discovered BSS.
+ * @ssid_len: length of SSID announced by BSS.
+ * @ssid: SSID announced by discovered BSS.
+ * @payload: IEs that are announced by discovered BSS in its MGMt frames.
+ */
+struct qlink_event_scan_result {
+	struct qlink_event ehdr;
+	__le64 tsf;
+	__le16 freq;
+	__le16 capab;
+	__le16 bintval;
+	s8 signal;
+	u8 frame_type;
+	u8 bssid[ETH_ALEN];
+	u8 ssid_len;
+	u8 ssid[IEEE80211_MAX_SSID_LEN];
+	u8 payload[0];
+} __packed;
+
+/**
+ * enum qlink_scan_complete_flags - indicates result of scan request.
+ *
+ * @QLINK_SCAN_NONE: Scan request was processed.
+ * @QLINK_SCAN_ABORTED: Scan was aborted.
+ */
+enum qlink_scan_complete_flags {
+	QLINK_SCAN_NONE		= 0,
+	QLINK_SCAN_ABORTED	= BIT(0),
+};
+
+/**
+ * struct qlink_event_scan_complete - data for QLINK_EVENT_SCAN_COMPLETE event
+ *
+ * @flags: flags indicating the status of pending scan request,
+ *	see &enum qlink_scan_complete_flags.
+ */
+struct qlink_event_scan_complete {
+	struct qlink_event ehdr;
+	__le32 flags;
+} __packed;
+
+/* QLINK TLVs (Type-Length Values) definitions
+ */
+
+enum qlink_tlv_id {
+	QTN_TLV_ID_FRAG_THRESH		= 0x0201,
+	QTN_TLV_ID_RTS_THRESH		= 0x0202,
+	QTN_TLV_ID_SRETRY_LIMIT		= 0x0203,
+	QTN_TLV_ID_LRETRY_LIMIT		= 0x0204,
+	QTN_TLV_ID_BCN_PERIOD		= 0x0205,
+	QTN_TLV_ID_DTIM			= 0x0206,
+	QTN_TLV_ID_CHANNEL		= 0x020F,
+	QTN_TLV_ID_COVERAGE_CLASS	= 0x0213,
+	QTN_TLV_ID_IFACE_LIMIT		= 0x0214,
+	QTN_TLV_ID_NUM_IFACE_COMB	= 0x0215,
+	QTN_TLV_ID_STA_BASIC_COUNTERS	= 0x0300,
+	QTN_TLV_ID_STA_GENERIC_INFO	= 0x0301,
+	QTN_TLV_ID_KEY			= 0x0302,
+	QTN_TLV_ID_SEQ			= 0x0303,
+	QTN_TLV_ID_CRYPTO		= 0x0304,
+	QTN_TLV_ID_IE_SET		= 0x0305,
+};
+
+struct qlink_tlv_hdr {
+	__le16 type;
+	__le16 len;
+	u8 val[0];
+} __packed;
+
+struct qlink_iface_limit {
+	__le16 max_num;
+	__le16 type_mask;
+} __packed;
+
+struct qlink_iface_comb_num {
+	__le16 iface_comb_num;
+} __packed;
+
+struct qlink_sta_stat_basic_counters {
+	__le64 rx_bytes;
+	__le64 tx_bytes;
+	__le64 rx_beacons;
+	__le32 rx_packets;
+	__le32 tx_packets;
+	__le32 rx_dropped;
+	__le32 tx_failed;
+} __packed;
+
+enum qlink_sta_info_rate_flags {
+	QLINK_STA_INFO_RATE_FLAG_INVALID	= 0,
+	QLINK_STA_INFO_RATE_FLAG_HT_MCS		= BIT(0),
+	QLINK_STA_INFO_RATE_FLAG_VHT_MCS	= BIT(1),
+	QLINK_STA_INFO_RATE_FLAG_SHORT_GI	= BIT(2),
+	QLINK_STA_INFO_RATE_FLAG_60G		= BIT(3),
+};
+
+enum qlink_sta_info_rate_bw {
+	QLINK_STA_INFO_RATE_BW_5		= 0,
+	QLINK_STA_INFO_RATE_BW_10		= 1,
+	QLINK_STA_INFO_RATE_BW_20		= 2,
+	QLINK_STA_INFO_RATE_BW_40		= 3,
+	QLINK_STA_INFO_RATE_BW_80		= 4,
+	QLINK_STA_INFO_RATE_BW_160		= 5,
+};
+
+/**
+ * struct qlink_sta_info_rate - STA rate statistics
+ *
+ * @rate: data rate in Mbps.
+ * @flags: bitmap of &enum qlink_sta_flags.
+ * @mcs: 802.11-defined MCS index.
+ * nss: Number of Spatial Streams.
+ * @bw: bandwidth, one of &enum qlink_sta_info_rate_bw.
+ */
+struct qlink_sta_info_rate {
+	__le16 rate;
+	u8 flags;
+	u8 mcs;
+	u8 nss;
+	u8 bw;
+} __packed;
+
+struct qlink_sta_info_state {
+	__le32 mask;
+	__le32 value;
+} __packed;
+
+#define QLINK_RSSI_OFFSET	120
+
+struct qlink_sta_info_generic {
+	struct qlink_sta_info_state state;
+	__le32 connected_time;
+	__le32 inactive_time;
+	struct qlink_sta_info_rate rx_rate;
+	struct qlink_sta_info_rate tx_rate;
+	u8 rssi;
+	u8 rssi_avg;
+} __packed;
+
+struct qlink_tlv_frag_rts_thr {
+	struct qlink_tlv_hdr hdr;
+	__le16 thr;
+} __packed;
+
+struct qlink_tlv_rlimit {
+	struct qlink_tlv_hdr hdr;
+	u8 rlimit;
+} __packed;
+
+struct qlink_tlv_cclass {
+	struct qlink_tlv_hdr hdr;
+	u8 cclass;
+} __packed;
+
+enum qlink_dfs_state {
+	QLINK_DFS_USABLE,
+	QLINK_DFS_UNAVAILABLE,
+	QLINK_DFS_AVAILABLE,
+};
+
+enum qlink_channel_flags {
+	QLINK_CHAN_DISABLED		= BIT(0),
+	QLINK_CHAN_NO_IR		= BIT(1),
+	QLINK_CHAN_RADAR		= BIT(3),
+	QLINK_CHAN_NO_HT40PLUS		= BIT(4),
+	QLINK_CHAN_NO_HT40MINUS		= BIT(5),
+	QLINK_CHAN_NO_OFDM		= BIT(6),
+	QLINK_CHAN_NO_80MHZ		= BIT(7),
+	QLINK_CHAN_NO_160MHZ		= BIT(8),
+	QLINK_CHAN_INDOOR_ONLY		= BIT(9),
+	QLINK_CHAN_IR_CONCURRENT	= BIT(10),
+	QLINK_CHAN_NO_20MHZ		= BIT(11),
+	QLINK_CHAN_NO_10MHZ		= BIT(12),
+};
+
+struct qlink_tlv_channel {
+	struct qlink_tlv_hdr hdr;
+	__le16 hw_value;
+	__le16 center_freq;
+	__le32 flags;
+	u8 band;
+	u8 max_antenna_gain;
+	u8 max_power;
+	u8 max_reg_power;
+	__le32 dfs_cac_ms;
+	u8 dfs_state;
+	u8 beacon_found;
+	u8 rsvd[2];
+} __packed;
+
+#define QLINK_MAX_NR_CIPHER_SUITES            5
+#define QLINK_MAX_NR_AKM_SUITES               2
+
+struct qlink_auth_encr {
+	__le32 wpa_versions;
+	__le32 cipher_group;
+	__le32 n_ciphers_pairwise;
+	__le32 ciphers_pairwise[QLINK_MAX_NR_CIPHER_SUITES];
+	__le32 n_akm_suites;
+	__le32 akm_suites[QLINK_MAX_NR_AKM_SUITES];
+	__le16 control_port_ethertype;
+	u8 auth_type;
+	u8 privacy;
+	u8 mfp;
+	u8 control_port;
+	u8 control_port_no_encrypt;
+} __packed;
+
+#endif /* _QTN_QLINK_H_ */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/qlink_util.c b/drivers/net/wireless/quantenna/qtnfmac/qlink_util.c
new file mode 100644
index 000000000000..49ae652ad9a3
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/qlink_util.c
@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by 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/nl80211.h>
+
+#include "qlink_util.h"
+
+u16 qlink_iface_type_mask_to_nl(u16 qlink_mask)
+{
+	u16 result = 0;
+
+	if (qlink_mask & QLINK_IFTYPE_AP)
+		result |= BIT(NL80211_IFTYPE_AP);
+
+	if (qlink_mask & QLINK_IFTYPE_STATION)
+		result |= BIT(NL80211_IFTYPE_STATION);
+
+	if (qlink_mask & QLINK_IFTYPE_ADHOC)
+		result |= BIT(NL80211_IFTYPE_ADHOC);
+
+	if (qlink_mask & QLINK_IFTYPE_MONITOR)
+		result |= BIT(NL80211_IFTYPE_MONITOR);
+
+	if (qlink_mask & QLINK_IFTYPE_WDS)
+		result |= BIT(NL80211_IFTYPE_WDS);
+
+	return result;
+}
+
+u8 qlink_chan_width_mask_to_nl(u16 qlink_mask)
+{
+	u8 result = 0;
+
+	if (qlink_mask & QLINK_CHAN_WIDTH_5)
+		result |= BIT(NL80211_CHAN_WIDTH_5);
+
+	if (qlink_mask & QLINK_CHAN_WIDTH_10)
+		result |= BIT(NL80211_CHAN_WIDTH_10);
+
+	if (qlink_mask & QLINK_CHAN_WIDTH_20_NOHT)
+		result |= BIT(NL80211_CHAN_WIDTH_20_NOHT);
+
+	if (qlink_mask & QLINK_CHAN_WIDTH_20)
+		result |= BIT(NL80211_CHAN_WIDTH_20);
+
+	if (qlink_mask & QLINK_CHAN_WIDTH_40)
+		result |= BIT(NL80211_CHAN_WIDTH_40);
+
+	if (qlink_mask & QLINK_CHAN_WIDTH_80)
+		result |= BIT(NL80211_CHAN_WIDTH_80);
+
+	if (qlink_mask & QLINK_CHAN_WIDTH_80P80)
+		result |= BIT(NL80211_CHAN_WIDTH_80P80);
+
+	if (qlink_mask & QLINK_CHAN_WIDTH_160)
+		result |= BIT(NL80211_CHAN_WIDTH_160);
+
+	return result;
+}
diff --git a/drivers/net/wireless/quantenna/qtnfmac/qlink_util.h b/drivers/net/wireless/quantenna/qtnfmac/qlink_util.h
new file mode 100644
index 000000000000..90d7d09a6c63
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/qlink_util.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _QTN_FMAC_QLINK_UTIL_H_
+#define _QTN_FMAC_QLINK_UTIL_H_
+
+#include <linux/types.h>
+#include <linux/skbuff.h>
+
+#include "qlink.h"
+
+static inline void qtnf_cmd_skb_put_action(struct sk_buff *skb, u16 action)
+{
+	__le16 *buf_ptr;
+
+	buf_ptr = skb_put(skb, sizeof(action));
+	*buf_ptr = cpu_to_le16(action);
+}
+
+static inline void
+qtnf_cmd_skb_put_buffer(struct sk_buff *skb, const u8 *buf_src, size_t len)
+{
+	skb_put_data(skb, buf_src, len);
+}
+
+static inline void qtnf_cmd_skb_put_tlv_arr(struct sk_buff *skb,
+					    u16 tlv_id, const u8 arr[],
+					    size_t arr_len)
+{
+	struct qlink_tlv_hdr *hdr = skb_put(skb, sizeof(*hdr) + arr_len);
+
+	hdr->type = cpu_to_le16(tlv_id);
+	hdr->len = cpu_to_le16(arr_len);
+	memcpy(hdr->val, arr, arr_len);
+}
+
+static inline void qtnf_cmd_skb_put_tlv_u8(struct sk_buff *skb, u16 tlv_id,
+					   u8 value)
+{
+	struct qlink_tlv_hdr *hdr = skb_put(skb, sizeof(*hdr) + sizeof(value));
+
+	hdr->type = cpu_to_le16(tlv_id);
+	hdr->len = cpu_to_le16(sizeof(value));
+	*hdr->val = value;
+}
+
+static inline void qtnf_cmd_skb_put_tlv_u16(struct sk_buff *skb,
+					    u16 tlv_id, u16 value)
+{
+	struct qlink_tlv_hdr *hdr = skb_put(skb, sizeof(*hdr) + sizeof(value));
+	__le16 tmp = cpu_to_le16(value);
+
+	hdr->type = cpu_to_le16(tlv_id);
+	hdr->len = cpu_to_le16(sizeof(value));
+	memcpy(hdr->val, &tmp, sizeof(tmp));
+}
+
+u16 qlink_iface_type_mask_to_nl(u16 qlink_mask);
+u8 qlink_chan_width_mask_to_nl(u16 qlink_mask);
+
+#endif /* _QTN_FMAC_QLINK_UTIL_H_ */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/qtn_hw_ids.h b/drivers/net/wireless/quantenna/qtnfmac/qtn_hw_ids.h
new file mode 100644
index 000000000000..c4ad40d59085
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/qtn_hw_ids.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef	_QTN_HW_IDS_H_
+#define	_QTN_HW_IDS_H_
+
+#include <linux/pci_ids.h>
+
+#define PCIE_VENDOR_ID_QUANTENNA	(0x1bb5)
+
+/* PCIE Device IDs */
+
+#define	PCIE_DEVICE_ID_QTN_PEARL	(0x0008)
+
+/* FW names */
+
+#define QTN_PCI_PEARL_FW_NAME		"qtn/fmac_qsr10g.img"
+
+#endif	/* _QTN_HW_IDS_H_ */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/shm_ipc.c b/drivers/net/wireless/quantenna/qtnfmac/shm_ipc.c
new file mode 100644
index 000000000000..aa106dd0a14b
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/shm_ipc.c
@@ -0,0 +1,176 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/types.h>
+#include <linux/io.h>
+
+#include "shm_ipc.h"
+
+#undef pr_fmt
+#define pr_fmt(fmt)	"qtnfmac shm_ipc: %s: " fmt, __func__
+
+static bool qtnf_shm_ipc_has_new_data(struct qtnf_shm_ipc *ipc)
+{
+	const u32 flags = readl(&ipc->shm_region->headroom.hdr.flags);
+
+	return (flags & QTNF_SHM_IPC_NEW_DATA);
+}
+
+static void qtnf_shm_handle_new_data(struct qtnf_shm_ipc *ipc)
+{
+	size_t size;
+	bool rx_buff_ok = true;
+	struct qtnf_shm_ipc_region_header __iomem *shm_reg_hdr;
+
+	shm_reg_hdr = &ipc->shm_region->headroom.hdr;
+
+	size = readw(&shm_reg_hdr->data_len);
+
+	if (unlikely(size == 0 || size > QTN_IPC_MAX_DATA_SZ)) {
+		pr_err("wrong rx packet size: %zu\n", size);
+		rx_buff_ok = false;
+	} else {
+		memcpy_fromio(ipc->rx_data, ipc->shm_region->data, size);
+	}
+
+	writel(QTNF_SHM_IPC_ACK, &shm_reg_hdr->flags);
+	readl(&shm_reg_hdr->flags); /* flush PCIe write */
+
+	ipc->interrupt.fn(ipc->interrupt.arg);
+
+	if (likely(rx_buff_ok)) {
+		ipc->rx_packet_count++;
+		ipc->rx_callback.fn(ipc->rx_callback.arg, ipc->rx_data, size);
+	}
+}
+
+static void qtnf_shm_ipc_irq_work(struct work_struct *work)
+{
+	struct qtnf_shm_ipc *ipc = container_of(work, struct qtnf_shm_ipc,
+						irq_work);
+
+	while (qtnf_shm_ipc_has_new_data(ipc))
+		qtnf_shm_handle_new_data(ipc);
+}
+
+static void qtnf_shm_ipc_irq_inbound_handler(struct qtnf_shm_ipc *ipc)
+{
+	u32 flags;
+
+	flags = readl(&ipc->shm_region->headroom.hdr.flags);
+
+	if (flags & QTNF_SHM_IPC_NEW_DATA)
+		queue_work(ipc->workqueue, &ipc->irq_work);
+}
+
+static void qtnf_shm_ipc_irq_outbound_handler(struct qtnf_shm_ipc *ipc)
+{
+	u32 flags;
+
+	if (!READ_ONCE(ipc->waiting_for_ack))
+		return;
+
+	flags = readl(&ipc->shm_region->headroom.hdr.flags);
+
+	if (flags & QTNF_SHM_IPC_ACK) {
+		WRITE_ONCE(ipc->waiting_for_ack, 0);
+		complete(&ipc->tx_completion);
+	}
+}
+
+int qtnf_shm_ipc_init(struct qtnf_shm_ipc *ipc,
+		      enum qtnf_shm_ipc_direction direction,
+		      struct qtnf_shm_ipc_region __iomem *shm_region,
+		      struct workqueue_struct *workqueue,
+		      const struct qtnf_shm_ipc_int *interrupt,
+		      const struct qtnf_shm_ipc_rx_callback *rx_callback)
+{
+	BUILD_BUG_ON(offsetof(struct qtnf_shm_ipc_region, data) !=
+		     QTN_IPC_REG_HDR_SZ);
+	BUILD_BUG_ON(sizeof(struct qtnf_shm_ipc_region) > QTN_IPC_REG_SZ);
+
+	ipc->shm_region = shm_region;
+	ipc->direction = direction;
+	ipc->interrupt = *interrupt;
+	ipc->rx_callback = *rx_callback;
+	ipc->tx_packet_count = 0;
+	ipc->rx_packet_count = 0;
+	ipc->workqueue = workqueue;
+	ipc->waiting_for_ack = 0;
+	ipc->tx_timeout_count = 0;
+
+	switch (direction) {
+	case QTNF_SHM_IPC_OUTBOUND:
+		ipc->irq_handler = qtnf_shm_ipc_irq_outbound_handler;
+		break;
+	case QTNF_SHM_IPC_INBOUND:
+		ipc->irq_handler = qtnf_shm_ipc_irq_inbound_handler;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	INIT_WORK(&ipc->irq_work, qtnf_shm_ipc_irq_work);
+	init_completion(&ipc->tx_completion);
+
+	return 0;
+}
+
+void qtnf_shm_ipc_free(struct qtnf_shm_ipc *ipc)
+{
+	complete_all(&ipc->tx_completion);
+}
+
+int qtnf_shm_ipc_send(struct qtnf_shm_ipc *ipc, const u8 *buf, size_t size)
+{
+	int ret = 0;
+	struct qtnf_shm_ipc_region_header __iomem *shm_reg_hdr;
+
+	shm_reg_hdr = &ipc->shm_region->headroom.hdr;
+
+	if (unlikely(size > QTN_IPC_MAX_DATA_SZ))
+		return -E2BIG;
+
+	ipc->tx_packet_count++;
+
+	writew(size, &shm_reg_hdr->data_len);
+	memcpy_toio(ipc->shm_region->data, buf, size);
+
+	/* sync previous writes before proceeding */
+	dma_wmb();
+
+	WRITE_ONCE(ipc->waiting_for_ack, 1);
+
+	/* sync previous memory write before announcing new data ready */
+	wmb();
+
+	writel(QTNF_SHM_IPC_NEW_DATA, &shm_reg_hdr->flags);
+	readl(&shm_reg_hdr->flags); /* flush PCIe write */
+
+	ipc->interrupt.fn(ipc->interrupt.arg);
+
+	if (!wait_for_completion_timeout(&ipc->tx_completion,
+					 QTN_SHM_IPC_ACK_TIMEOUT)) {
+		ret = -ETIMEDOUT;
+		ipc->tx_timeout_count++;
+		pr_err("TX ACK timeout\n");
+	}
+
+	/* now we're not waiting for ACK even in case of timeout */
+	WRITE_ONCE(ipc->waiting_for_ack, 0);
+
+	return ret;
+}
diff --git a/drivers/net/wireless/quantenna/qtnfmac/shm_ipc.h b/drivers/net/wireless/quantenna/qtnfmac/shm_ipc.h
new file mode 100644
index 000000000000..453dd6477b12
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/shm_ipc.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _QTN_FMAC_SHM_IPC_H_
+#define _QTN_FMAC_SHM_IPC_H_
+
+#include <linux/workqueue.h>
+#include <linux/completion.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+
+#include "shm_ipc_defs.h"
+
+#define QTN_SHM_IPC_ACK_TIMEOUT		(2 * HZ)
+
+struct qtnf_shm_ipc_int {
+	void (*fn)(void *arg);
+	void *arg;
+};
+
+struct qtnf_shm_ipc_rx_callback {
+	void (*fn)(void *arg, const u8 *buf, size_t len);
+	void *arg;
+};
+
+enum qtnf_shm_ipc_direction {
+	QTNF_SHM_IPC_OUTBOUND		= BIT(0),
+	QTNF_SHM_IPC_INBOUND		= BIT(1),
+};
+
+struct qtnf_shm_ipc {
+	struct qtnf_shm_ipc_region __iomem *shm_region;
+	enum qtnf_shm_ipc_direction direction;
+	size_t tx_packet_count;
+	size_t rx_packet_count;
+
+	size_t tx_timeout_count;
+
+	u8 waiting_for_ack;
+
+	u8 rx_data[QTN_IPC_MAX_DATA_SZ] __aligned(sizeof(u32));
+
+	struct qtnf_shm_ipc_int interrupt;
+	struct qtnf_shm_ipc_rx_callback rx_callback;
+
+	void (*irq_handler)(struct qtnf_shm_ipc *ipc);
+
+	struct workqueue_struct *workqueue;
+	struct work_struct irq_work;
+	struct completion tx_completion;
+};
+
+int qtnf_shm_ipc_init(struct qtnf_shm_ipc *ipc,
+		      enum qtnf_shm_ipc_direction direction,
+		      struct qtnf_shm_ipc_region __iomem *shm_region,
+		      struct workqueue_struct *workqueue,
+		      const struct qtnf_shm_ipc_int *interrupt,
+		      const struct qtnf_shm_ipc_rx_callback *rx_callback);
+void qtnf_shm_ipc_free(struct qtnf_shm_ipc *ipc);
+int qtnf_shm_ipc_send(struct qtnf_shm_ipc *ipc, const u8 *buf, size_t size);
+
+static inline void qtnf_shm_ipc_irq_handler(struct qtnf_shm_ipc *ipc)
+{
+	ipc->irq_handler(ipc);
+}
+
+#endif /* _QTN_FMAC_SHM_IPC_H_ */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/shm_ipc_defs.h b/drivers/net/wireless/quantenna/qtnfmac/shm_ipc_defs.h
new file mode 100644
index 000000000000..95a5f89a8b1a
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/shm_ipc_defs.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _QTN_FMAC_SHM_IPC_DEFS_H_
+#define _QTN_FMAC_SHM_IPC_DEFS_H_
+
+#include <linux/types.h>
+
+#define QTN_IPC_REG_HDR_SZ	(32)
+#define QTN_IPC_REG_SZ		(4096)
+#define QTN_IPC_MAX_DATA_SZ	(QTN_IPC_REG_SZ - QTN_IPC_REG_HDR_SZ)
+
+enum qtnf_shm_ipc_region_flags {
+	QTNF_SHM_IPC_NEW_DATA		= BIT(0),
+	QTNF_SHM_IPC_ACK		= BIT(1),
+};
+
+struct qtnf_shm_ipc_region_header {
+	__le32 flags;
+	__le16 data_len;
+} __packed;
+
+union qtnf_shm_ipc_region_headroom {
+	struct qtnf_shm_ipc_region_header hdr;
+	u8 headroom[QTN_IPC_REG_HDR_SZ];
+} __packed;
+
+struct qtnf_shm_ipc_region {
+	union qtnf_shm_ipc_region_headroom headroom;
+	u8 data[QTN_IPC_MAX_DATA_SZ];
+} __packed;
+
+#endif /* _QTN_FMAC_SHM_IPC_DEFS_H_ */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/trans.c b/drivers/net/wireless/quantenna/qtnfmac/trans.c
new file mode 100644
index 000000000000..ccddfebc508a
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/trans.c
@@ -0,0 +1,224 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/types.h>
+#include <linux/export.h>
+#include <linux/slab.h>
+
+#include "core.h"
+#include "commands.h"
+#include "event.h"
+#include "bus.h"
+
+#define QTNF_DEF_SYNC_CMD_TIMEOUT	(5 * HZ)
+
+int qtnf_trans_send_cmd_with_resp(struct qtnf_bus *bus, struct sk_buff *cmd_skb,
+				  struct sk_buff **response_skb)
+{
+	struct qtnf_cmd_ctl_node *ctl_node = &bus->trans.curr_cmd;
+	struct qlink_cmd *cmd = (void *)cmd_skb->data;
+	int ret = 0;
+	long status;
+	bool resp_not_handled = true;
+	struct sk_buff *resp_skb = NULL;
+
+	if (unlikely(!response_skb))
+		return -EFAULT;
+
+	spin_lock(&ctl_node->resp_lock);
+	ctl_node->seq_num++;
+	cmd->seq_num = cpu_to_le16(ctl_node->seq_num);
+	WARN(ctl_node->resp_skb, "qtnfmac: response skb not empty\n");
+	ctl_node->waiting_for_resp = true;
+	spin_unlock(&ctl_node->resp_lock);
+
+	ret = qtnf_bus_control_tx(bus, cmd_skb);
+	dev_kfree_skb(cmd_skb);
+
+	if (unlikely(ret))
+		goto out;
+
+	status = wait_for_completion_interruptible_timeout(
+						&ctl_node->cmd_resp_completion,
+						QTNF_DEF_SYNC_CMD_TIMEOUT);
+
+	spin_lock(&ctl_node->resp_lock);
+	resp_not_handled = ctl_node->waiting_for_resp;
+	resp_skb = ctl_node->resp_skb;
+	ctl_node->resp_skb = NULL;
+	ctl_node->waiting_for_resp = false;
+	spin_unlock(&ctl_node->resp_lock);
+
+	if (unlikely(status <= 0)) {
+		if (status == 0) {
+			ret = -ETIMEDOUT;
+			pr_err("response timeout\n");
+		} else {
+			ret = -EINTR;
+			pr_debug("interrupted\n");
+		}
+	}
+
+	if (unlikely(!resp_skb || resp_not_handled)) {
+		if (!ret)
+			ret = -EFAULT;
+
+		goto out;
+	}
+
+	ret = 0;
+	*response_skb = resp_skb;
+
+out:
+	if (unlikely(resp_skb && resp_not_handled))
+		dev_kfree_skb(resp_skb);
+
+	return ret;
+}
+
+static void qtnf_trans_signal_cmdresp(struct qtnf_bus *bus, struct sk_buff *skb)
+{
+	struct qtnf_cmd_ctl_node *ctl_node = &bus->trans.curr_cmd;
+	const struct qlink_resp *resp = (const struct qlink_resp *)skb->data;
+	const u16 recvd_seq_num = le16_to_cpu(resp->seq_num);
+
+	spin_lock(&ctl_node->resp_lock);
+
+	if (unlikely(!ctl_node->waiting_for_resp)) {
+		pr_err("unexpected response\n");
+		goto out_err;
+	}
+
+	if (unlikely(recvd_seq_num != ctl_node->seq_num)) {
+		pr_err("seq num mismatch\n");
+		goto out_err;
+	}
+
+	ctl_node->resp_skb = skb;
+	ctl_node->waiting_for_resp = false;
+
+	spin_unlock(&ctl_node->resp_lock);
+
+	complete(&ctl_node->cmd_resp_completion);
+	return;
+
+out_err:
+	spin_unlock(&ctl_node->resp_lock);
+	dev_kfree_skb(skb);
+}
+
+static int qtnf_trans_event_enqueue(struct qtnf_bus *bus, struct sk_buff *skb)
+{
+	struct qtnf_qlink_transport *trans = &bus->trans;
+
+	if (likely(skb_queue_len(&trans->event_queue) <
+		   trans->event_queue_max_len)) {
+		skb_queue_tail(&trans->event_queue, skb);
+		queue_work(bus->workqueue, &bus->event_work);
+	} else {
+		pr_warn("event dropped due to queue overflow\n");
+		dev_kfree_skb(skb);
+		return -1;
+	}
+
+	return 0;
+}
+
+void qtnf_trans_init(struct qtnf_bus *bus)
+{
+	struct qtnf_qlink_transport *trans = &bus->trans;
+
+	init_completion(&trans->curr_cmd.cmd_resp_completion);
+	spin_lock_init(&trans->curr_cmd.resp_lock);
+
+	spin_lock(&trans->curr_cmd.resp_lock);
+	trans->curr_cmd.seq_num = 0;
+	trans->curr_cmd.waiting_for_resp = false;
+	trans->curr_cmd.resp_skb = NULL;
+	spin_unlock(&trans->curr_cmd.resp_lock);
+
+	/* Init event handling related fields */
+	skb_queue_head_init(&trans->event_queue);
+	trans->event_queue_max_len = QTNF_MAX_EVENT_QUEUE_LEN;
+}
+
+static void qtnf_trans_free_events(struct qtnf_bus *bus)
+{
+	struct sk_buff_head *event_queue = &bus->trans.event_queue;
+	struct sk_buff *current_event_skb = skb_dequeue(event_queue);
+
+	while (current_event_skb) {
+		dev_kfree_skb_any(current_event_skb);
+		current_event_skb = skb_dequeue(event_queue);
+	}
+}
+
+void qtnf_trans_free(struct qtnf_bus *bus)
+{
+	if (!bus) {
+		pr_err("invalid bus pointer\n");
+		return;
+	}
+
+	qtnf_trans_free_events(bus);
+}
+
+int qtnf_trans_handle_rx_ctl_packet(struct qtnf_bus *bus, struct sk_buff *skb)
+{
+	const struct qlink_msg_header *header = (void *)skb->data;
+	int ret = -1;
+
+	if (unlikely(skb->len < sizeof(*header))) {
+		pr_warn("packet is too small: %u\n", skb->len);
+		dev_kfree_skb(skb);
+		return -EINVAL;
+	}
+
+	if (unlikely(skb->len != le16_to_cpu(header->len))) {
+		pr_warn("cmd reply length mismatch: %u != %u\n",
+			skb->len, le16_to_cpu(header->len));
+		dev_kfree_skb(skb);
+		return -EFAULT;
+	}
+
+	switch (le16_to_cpu(header->type)) {
+	case QLINK_MSG_TYPE_CMDRSP:
+		if (unlikely(skb->len < sizeof(struct qlink_cmd))) {
+			pr_warn("cmd reply too short: %u\n", skb->len);
+			dev_kfree_skb(skb);
+			break;
+		}
+
+		qtnf_trans_signal_cmdresp(bus, skb);
+		break;
+	case QLINK_MSG_TYPE_EVENT:
+		if (unlikely(skb->len < sizeof(struct qlink_event))) {
+			pr_warn("event too short: %u\n", skb->len);
+			dev_kfree_skb(skb);
+			break;
+		}
+
+		ret = qtnf_trans_event_enqueue(bus, skb);
+		break;
+	default:
+		pr_warn("unknown packet type: %x\n", le16_to_cpu(header->type));
+		dev_kfree_skb(skb);
+		break;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(qtnf_trans_handle_rx_ctl_packet);
diff --git a/drivers/net/wireless/quantenna/qtnfmac/trans.h b/drivers/net/wireless/quantenna/qtnfmac/trans.h
new file mode 100644
index 000000000000..9a473e07af0f
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/trans.h
@@ -0,0 +1,57 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _QTN_FMAC_TRANS_H_
+#define _QTN_FMAC_TRANS_H_
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/skbuff.h>
+#include <linux/mutex.h>
+
+#include "qlink.h"
+
+#define QTNF_CMD_FLAG_RESP_REQ		BIT(0)
+
+#define QTNF_MAX_CMD_BUF_SIZE	2048
+#define QTNF_DEF_CMD_HROOM	4
+
+struct qtnf_bus;
+
+struct qtnf_cmd_ctl_node {
+	struct completion cmd_resp_completion;
+	struct sk_buff *resp_skb;
+	u16 seq_num;
+	bool waiting_for_resp;
+	spinlock_t resp_lock; /* lock for resp_skb & waiting_for_resp changes */
+};
+
+struct qtnf_qlink_transport {
+	struct qtnf_cmd_ctl_node curr_cmd;
+	struct sk_buff_head event_queue;
+	size_t event_queue_max_len;
+};
+
+void qtnf_trans_init(struct qtnf_bus *bus);
+void qtnf_trans_free(struct qtnf_bus *bus);
+
+int qtnf_trans_send_next_cmd(struct qtnf_bus *bus);
+int qtnf_trans_handle_rx_ctl_packet(struct qtnf_bus *bus, struct sk_buff *skb);
+int qtnf_trans_send_cmd_with_resp(struct qtnf_bus *bus,
+				  struct sk_buff *cmd_skb,
+				  struct sk_buff **response_skb);
+
+#endif /* _QTN_FMAC_TRANS_H_ */
diff --git a/drivers/net/wireless/quantenna/qtnfmac/util.c b/drivers/net/wireless/quantenna/qtnfmac/util.c
new file mode 100644
index 000000000000..ed38e87471bf
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/util.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright (c) 2015-2016 Quantenna Communications, Inc.
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include "util.h"
+
+void qtnf_sta_list_init(struct qtnf_sta_list *list)
+{
+	if (unlikely(!list))
+		return;
+
+	INIT_LIST_HEAD(&list->head);
+	atomic_set(&list->size, 0);
+}
+
+struct qtnf_sta_node *qtnf_sta_list_lookup(struct qtnf_sta_list *list,
+					   const u8 *mac)
+{
+	struct qtnf_sta_node *node;
+
+	if (unlikely(!mac))
+		return NULL;
+
+	list_for_each_entry(node, &list->head, list) {
+		if (ether_addr_equal(node->mac_addr, mac))
+			return node;
+	}
+
+	return NULL;
+}
+
+struct qtnf_sta_node *qtnf_sta_list_lookup_index(struct qtnf_sta_list *list,
+						 size_t index)
+{
+	struct qtnf_sta_node *node;
+
+	if (qtnf_sta_list_size(list) <= index)
+		return NULL;
+
+	list_for_each_entry(node, &list->head, list) {
+		if (index-- == 0)
+			return node;
+	}
+
+	return NULL;
+}
+
+struct qtnf_sta_node *qtnf_sta_list_add(struct qtnf_sta_list *list,
+					const u8 *mac)
+{
+	struct qtnf_sta_node *node;
+
+	if (unlikely(!mac))
+		return NULL;
+
+	node = qtnf_sta_list_lookup(list, mac);
+
+	if (node)
+		goto done;
+
+	node = kzalloc(sizeof(*node), GFP_KERNEL);
+	if (unlikely(!node))
+		goto done;
+
+	ether_addr_copy(node->mac_addr, mac);
+	list_add_tail(&node->list, &list->head);
+	atomic_inc(&list->size);
+
+done:
+	return node;
+}
+
+bool qtnf_sta_list_del(struct qtnf_sta_list *list, const u8 *mac)
+{
+	struct qtnf_sta_node *node;
+	bool ret = false;
+
+	node = qtnf_sta_list_lookup(list, mac);
+
+	if (node) {
+		list_del(&node->list);
+		atomic_dec(&list->size);
+		kfree(node);
+		ret = true;
+	}
+
+	return ret;
+}
+
+void qtnf_sta_list_free(struct qtnf_sta_list *list)
+{
+	struct qtnf_sta_node *node, *tmp;
+
+	atomic_set(&list->size, 0);
+
+	list_for_each_entry_safe(node, tmp, &list->head, list) {
+		list_del(&node->list);
+		kfree(node);
+	}
+
+	INIT_LIST_HEAD(&list->head);
+}
diff --git a/drivers/net/wireless/quantenna/qtnfmac/util.h b/drivers/net/wireless/quantenna/qtnfmac/util.h
new file mode 100644
index 000000000000..0359eae8c24b
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/util.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2015 Quantenna Communications
+ *
+ * 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 QTNFMAC_UTIL_H
+#define QTNFMAC_UTIL_H
+
+#include <linux/kernel.h>
+#include "core.h"
+
+void qtnf_sta_list_init(struct qtnf_sta_list *list);
+
+struct qtnf_sta_node *qtnf_sta_list_lookup(struct qtnf_sta_list *list,
+					   const u8 *mac);
+struct qtnf_sta_node *qtnf_sta_list_lookup_index(struct qtnf_sta_list *list,
+						 size_t index);
+struct qtnf_sta_node *qtnf_sta_list_add(struct qtnf_sta_list *list,
+					const u8 *mac);
+bool qtnf_sta_list_del(struct qtnf_sta_list *list, const u8 *mac);
+
+void qtnf_sta_list_free(struct qtnf_sta_list *list);
+
+static inline size_t qtnf_sta_list_size(const struct qtnf_sta_list *list)
+{
+	return atomic_read(&list->size);
+}
+
+static inline bool qtnf_sta_list_empty(const struct qtnf_sta_list *list)
+{
+	return list_empty(&list->head);
+}
+
+#endif /* QTNFMAC_UTIL_H */
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2400pci.c b/drivers/net/wireless/ralink/rt2x00/rt2400pci.c
index 19874439ac40..0bc8b0249c57 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2400pci.c
@@ -77,10 +77,11 @@ static void rt2400pci_bbp_write(struct rt2x00_dev *rt2x00dev,
 	mutex_unlock(&rt2x00dev->csr_mutex);
 }
 
-static void rt2400pci_bbp_read(struct rt2x00_dev *rt2x00dev,
-			       const unsigned int word, u8 *value)
+static u8 rt2400pci_bbp_read(struct rt2x00_dev *rt2x00dev,
+			     const unsigned int word)
 {
 	u32 reg;
+	u8 value;
 
 	mutex_lock(&rt2x00dev->csr_mutex);
 
@@ -103,9 +104,11 @@ static void rt2400pci_bbp_read(struct rt2x00_dev *rt2x00dev,
 		WAIT_FOR_BBP(rt2x00dev, &reg);
 	}
 
-	*value = rt2x00_get_field32(reg, BBPCSR_VALUE);
+	value = rt2x00_get_field32(reg, BBPCSR_VALUE);
 
 	mutex_unlock(&rt2x00dev->csr_mutex);
+
+	return value;
 }
 
 static void rt2400pci_rf_write(struct rt2x00_dev *rt2x00dev,
@@ -138,7 +141,7 @@ static void rt2400pci_eepromregister_read(struct eeprom_93cx6 *eeprom)
 	struct rt2x00_dev *rt2x00dev = eeprom->data;
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, CSR21, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR21);
 
 	eeprom->reg_data_in = !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_IN);
 	eeprom->reg_data_out = !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_OUT);
@@ -202,7 +205,7 @@ static int rt2400pci_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, GPIOCSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, GPIOCSR);
 	return rt2x00_get_field32(reg, GPIOCSR_VAL0);
 }
 
@@ -215,7 +218,7 @@ static void rt2400pci_brightness_set(struct led_classdev *led_cdev,
 	unsigned int enabled = brightness != LED_OFF;
 	u32 reg;
 
-	rt2x00mmio_register_read(led->rt2x00dev, LEDCSR, &reg);
+	reg = rt2x00mmio_register_read(led->rt2x00dev, LEDCSR);
 
 	if (led->type == LED_TYPE_RADIO || led->type == LED_TYPE_ASSOC)
 		rt2x00_set_field32(&reg, LEDCSR_LINK, enabled);
@@ -233,7 +236,7 @@ static int rt2400pci_blink_set(struct led_classdev *led_cdev,
 	    container_of(led_cdev, struct rt2x00_led, led_dev);
 	u32 reg;
 
-	rt2x00mmio_register_read(led->rt2x00dev, LEDCSR, &reg);
+	reg = rt2x00mmio_register_read(led->rt2x00dev, LEDCSR);
 	rt2x00_set_field32(&reg, LEDCSR_ON_PERIOD, *delay_on);
 	rt2x00_set_field32(&reg, LEDCSR_OFF_PERIOD, *delay_off);
 	rt2x00mmio_register_write(led->rt2x00dev, LEDCSR, reg);
@@ -266,7 +269,7 @@ static void rt2400pci_config_filter(struct rt2x00_dev *rt2x00dev,
 	 * Note that the version error will always be dropped
 	 * since there is no filter for it at this time.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, RXCSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RXCSR0);
 	rt2x00_set_field32(&reg, RXCSR0_DROP_CRC,
 			   !(filter_flags & FIF_FCSFAIL));
 	rt2x00_set_field32(&reg, RXCSR0_DROP_PHYSICAL,
@@ -295,14 +298,14 @@ static void rt2400pci_config_intf(struct rt2x00_dev *rt2x00dev,
 		 * Enable beacon config
 		 */
 		bcn_preload = PREAMBLE + GET_DURATION(IEEE80211_HEADER, 20);
-		rt2x00mmio_register_read(rt2x00dev, BCNCSR1, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, BCNCSR1);
 		rt2x00_set_field32(&reg, BCNCSR1_PRELOAD, bcn_preload);
 		rt2x00mmio_register_write(rt2x00dev, BCNCSR1, reg);
 
 		/*
 		 * Enable synchronisation.
 		 */
-		rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR14);
 		rt2x00_set_field32(&reg, CSR14_TSF_SYNC, conf->sync);
 		rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 	}
@@ -330,35 +333,35 @@ static void rt2400pci_config_erp(struct rt2x00_dev *rt2x00dev,
 	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
 		preamble_mask = erp->short_preamble << 3;
 
-		rt2x00mmio_register_read(rt2x00dev, TXCSR1, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXCSR1);
 		rt2x00_set_field32(&reg, TXCSR1_ACK_TIMEOUT, 0x1ff);
 		rt2x00_set_field32(&reg, TXCSR1_ACK_CONSUME_TIME, 0x13a);
 		rt2x00_set_field32(&reg, TXCSR1_TSF_OFFSET, IEEE80211_HEADER);
 		rt2x00_set_field32(&reg, TXCSR1_AUTORESPONDER, 1);
 		rt2x00mmio_register_write(rt2x00dev, TXCSR1, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, ARCSR2, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, ARCSR2);
 		rt2x00_set_field32(&reg, ARCSR2_SIGNAL, 0x00);
 		rt2x00_set_field32(&reg, ARCSR2_SERVICE, 0x04);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 10));
 		rt2x00mmio_register_write(rt2x00dev, ARCSR2, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, ARCSR3, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, ARCSR3);
 		rt2x00_set_field32(&reg, ARCSR3_SIGNAL, 0x01 | preamble_mask);
 		rt2x00_set_field32(&reg, ARCSR3_SERVICE, 0x04);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 20));
 		rt2x00mmio_register_write(rt2x00dev, ARCSR3, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, ARCSR4, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, ARCSR4);
 		rt2x00_set_field32(&reg, ARCSR4_SIGNAL, 0x02 | preamble_mask);
 		rt2x00_set_field32(&reg, ARCSR4_SERVICE, 0x04);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 55));
 		rt2x00mmio_register_write(rt2x00dev, ARCSR4, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, ARCSR5, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, ARCSR5);
 		rt2x00_set_field32(&reg, ARCSR5_SIGNAL, 0x03 | preamble_mask);
 		rt2x00_set_field32(&reg, ARCSR5_SERVICE, 0x84);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
@@ -370,23 +373,23 @@ static void rt2400pci_config_erp(struct rt2x00_dev *rt2x00dev,
 		rt2x00mmio_register_write(rt2x00dev, ARCSR1, erp->basic_rates);
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
-		rt2x00mmio_register_read(rt2x00dev, CSR11, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR11);
 		rt2x00_set_field32(&reg, CSR11_SLOT_TIME, erp->slot_time);
 		rt2x00mmio_register_write(rt2x00dev, CSR11, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, CSR18, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR18);
 		rt2x00_set_field32(&reg, CSR18_SIFS, erp->sifs);
 		rt2x00_set_field32(&reg, CSR18_PIFS, erp->pifs);
 		rt2x00mmio_register_write(rt2x00dev, CSR18, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, CSR19, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR19);
 		rt2x00_set_field32(&reg, CSR19_DIFS, erp->difs);
 		rt2x00_set_field32(&reg, CSR19_EIFS, erp->eifs);
 		rt2x00mmio_register_write(rt2x00dev, CSR19, reg);
 	}
 
 	if (changed & BSS_CHANGED_BEACON_INT) {
-		rt2x00mmio_register_read(rt2x00dev, CSR12, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR12);
 		rt2x00_set_field32(&reg, CSR12_BEACON_INTERVAL,
 				   erp->beacon_int * 16);
 		rt2x00_set_field32(&reg, CSR12_CFP_MAX_DURATION,
@@ -408,8 +411,8 @@ static void rt2400pci_config_ant(struct rt2x00_dev *rt2x00dev,
 	BUG_ON(ant->rx == ANTENNA_SW_DIVERSITY ||
 	       ant->tx == ANTENNA_SW_DIVERSITY);
 
-	rt2400pci_bbp_read(rt2x00dev, 4, &r4);
-	rt2400pci_bbp_read(rt2x00dev, 1, &r1);
+	r4 = rt2400pci_bbp_read(rt2x00dev, 4);
+	r1 = rt2400pci_bbp_read(rt2x00dev, 1);
 
 	/*
 	 * Configure the TX antenna.
@@ -495,7 +498,7 @@ static void rt2400pci_config_channel(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Clear false CRC during channel switch.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, CNT0, &rf->rf1);
+	rf->rf1 = rt2x00mmio_register_read(rt2x00dev, CNT0);
 }
 
 static void rt2400pci_config_txpower(struct rt2x00_dev *rt2x00dev, int txpower)
@@ -508,7 +511,7 @@ static void rt2400pci_config_retry_limit(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, CSR11, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR11);
 	rt2x00_set_field32(&reg, CSR11_LONG_RETRY,
 			   libconf->conf->long_frame_max_tx_count);
 	rt2x00_set_field32(&reg, CSR11_SHORT_RETRY,
@@ -525,7 +528,7 @@ static void rt2400pci_config_ps(struct rt2x00_dev *rt2x00dev,
 	u32 reg;
 
 	if (state == STATE_SLEEP) {
-		rt2x00mmio_register_read(rt2x00dev, CSR20, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR20);
 		rt2x00_set_field32(&reg, CSR20_DELAY_AFTER_TBCN,
 				   (rt2x00dev->beacon_int - 20) * 16);
 		rt2x00_set_field32(&reg, CSR20_TBCN_BEFORE_WAKEUP,
@@ -538,7 +541,7 @@ static void rt2400pci_config_ps(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, CSR20_AUTOWAKE, 1);
 		rt2x00mmio_register_write(rt2x00dev, CSR20, reg);
 	} else {
-		rt2x00mmio_register_read(rt2x00dev, CSR20, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR20);
 		rt2x00_set_field32(&reg, CSR20_AUTOWAKE, 0);
 		rt2x00mmio_register_write(rt2x00dev, CSR20, reg);
 	}
@@ -566,7 +569,7 @@ static void rt2400pci_config_cw(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, CSR11, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR11);
 	rt2x00_set_field32(&reg, CSR11_CWMIN, cw_min);
 	rt2x00_set_field32(&reg, CSR11_CWMAX, cw_max);
 	rt2x00mmio_register_write(rt2x00dev, CSR11, reg);
@@ -584,13 +587,13 @@ static void rt2400pci_link_stats(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Update FCS error count from register.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, CNT0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CNT0);
 	qual->rx_failed = rt2x00_get_field32(reg, CNT0_FCS_ERROR);
 
 	/*
 	 * Update False CCA count from register.
 	 */
-	rt2400pci_bbp_read(rt2x00dev, 39, &bbp);
+	bbp = rt2400pci_bbp_read(rt2x00dev, 39);
 	qual->false_cca = bbp;
 }
 
@@ -639,12 +642,12 @@ static void rt2400pci_start_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00mmio_register_read(rt2x00dev, RXCSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, RXCSR0);
 		rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX, 0);
 		rt2x00mmio_register_write(rt2x00dev, RXCSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR14);
 		rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
 		rt2x00_set_field32(&reg, CSR14_TBCN, 1);
 		rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 1);
@@ -662,17 +665,17 @@ static void rt2400pci_kick_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_AC_VO:
-		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXCSR0);
 		rt2x00_set_field32(&reg, TXCSR0_KICK_PRIO, 1);
 		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	case QID_AC_VI:
-		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXCSR0);
 		rt2x00_set_field32(&reg, TXCSR0_KICK_TX, 1);
 		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	case QID_ATIM:
-		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXCSR0);
 		rt2x00_set_field32(&reg, TXCSR0_KICK_ATIM, 1);
 		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
@@ -690,17 +693,17 @@ static void rt2400pci_stop_queue(struct data_queue *queue)
 	case QID_AC_VO:
 	case QID_AC_VI:
 	case QID_ATIM:
-		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXCSR0);
 		rt2x00_set_field32(&reg, TXCSR0_ABORT, 1);
 		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	case QID_RX:
-		rt2x00mmio_register_read(rt2x00dev, RXCSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, RXCSR0);
 		rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX, 1);
 		rt2x00mmio_register_write(rt2x00dev, RXCSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR14);
 		rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
 		rt2x00_set_field32(&reg, CSR14_TBCN, 0);
 		rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
@@ -725,11 +728,11 @@ static bool rt2400pci_get_entry_state(struct queue_entry *entry)
 	u32 word;
 
 	if (entry->queue->qid == QID_RX) {
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 
 		return rt2x00_get_field32(word, RXD_W0_OWNER_NIC);
 	} else {
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 
 		return (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
 		        rt2x00_get_field32(word, TXD_W0_VALID));
@@ -743,19 +746,19 @@ static void rt2400pci_clear_entry(struct queue_entry *entry)
 	u32 word;
 
 	if (entry->queue->qid == QID_RX) {
-		rt2x00_desc_read(entry_priv->desc, 2, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 2);
 		rt2x00_set_field32(&word, RXD_W2_BUFFER_LENGTH, entry->skb->len);
 		rt2x00_desc_write(entry_priv->desc, 2, word);
 
-		rt2x00_desc_read(entry_priv->desc, 1, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 1);
 		rt2x00_set_field32(&word, RXD_W1_BUFFER_ADDRESS, skbdesc->skb_dma);
 		rt2x00_desc_write(entry_priv->desc, 1, word);
 
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 		rt2x00_set_field32(&word, RXD_W0_OWNER_NIC, 1);
 		rt2x00_desc_write(entry_priv->desc, 0, word);
 	} else {
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 		rt2x00_set_field32(&word, TXD_W0_VALID, 0);
 		rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 0);
 		rt2x00_desc_write(entry_priv->desc, 0, word);
@@ -770,7 +773,7 @@ static int rt2400pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize registers.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, TXCSR2, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXCSR2);
 	rt2x00_set_field32(&reg, TXCSR2_TXD_SIZE, rt2x00dev->tx[0].desc_size);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_TXD, rt2x00dev->tx[1].limit);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_ATIM, rt2x00dev->atim->limit);
@@ -778,36 +781,36 @@ static int rt2400pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	rt2x00mmio_register_write(rt2x00dev, TXCSR2, reg);
 
 	entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, TXCSR3, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXCSR3);
 	rt2x00_set_field32(&reg, TXCSR3_TX_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, TXCSR3, reg);
 
 	entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, TXCSR5, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXCSR5);
 	rt2x00_set_field32(&reg, TXCSR5_PRIO_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, TXCSR5, reg);
 
 	entry_priv = rt2x00dev->atim->entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, TXCSR4, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXCSR4);
 	rt2x00_set_field32(&reg, TXCSR4_ATIM_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, TXCSR4, reg);
 
 	entry_priv = rt2x00dev->bcn->entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, TXCSR6, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXCSR6);
 	rt2x00_set_field32(&reg, TXCSR6_BEACON_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, TXCSR6, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, RXCSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RXCSR1);
 	rt2x00_set_field32(&reg, RXCSR1_RXD_SIZE, rt2x00dev->rx->desc_size);
 	rt2x00_set_field32(&reg, RXCSR1_NUM_RXD, rt2x00dev->rx->limit);
 	rt2x00mmio_register_write(rt2x00dev, RXCSR1, reg);
 
 	entry_priv = rt2x00dev->rx->entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, RXCSR2, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RXCSR2);
 	rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, RXCSR2, reg);
@@ -824,18 +827,18 @@ static int rt2400pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00mmio_register_write(rt2x00dev, PSCSR2, 0x00023f20);
 	rt2x00mmio_register_write(rt2x00dev, PSCSR3, 0x00000002);
 
-	rt2x00mmio_register_read(rt2x00dev, TIMECSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TIMECSR);
 	rt2x00_set_field32(&reg, TIMECSR_US_COUNT, 33);
 	rt2x00_set_field32(&reg, TIMECSR_US_64_COUNT, 63);
 	rt2x00_set_field32(&reg, TIMECSR_BEACON_EXPECT, 0);
 	rt2x00mmio_register_write(rt2x00dev, TIMECSR, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR9, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR9);
 	rt2x00_set_field32(&reg, CSR9_MAX_FRAME_UNIT,
 			   (rt2x00dev->rx->data_size / 128));
 	rt2x00mmio_register_write(rt2x00dev, CSR9, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR14);
 	rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
 	rt2x00_set_field32(&reg, CSR14_TSF_SYNC, 0);
 	rt2x00_set_field32(&reg, CSR14_TBCN, 0);
@@ -848,14 +851,14 @@ static int rt2400pci_init_registers(struct rt2x00_dev *rt2x00dev)
 
 	rt2x00mmio_register_write(rt2x00dev, CNT3, 0x3f080000);
 
-	rt2x00mmio_register_read(rt2x00dev, ARCSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, ARCSR0);
 	rt2x00_set_field32(&reg, ARCSR0_AR_BBP_DATA0, 133);
 	rt2x00_set_field32(&reg, ARCSR0_AR_BBP_ID0, 134);
 	rt2x00_set_field32(&reg, ARCSR0_AR_BBP_DATA1, 136);
 	rt2x00_set_field32(&reg, ARCSR0_AR_BBP_ID1, 135);
 	rt2x00mmio_register_write(rt2x00dev, ARCSR0, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, RXCSR3, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RXCSR3);
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID0, 3); /* Tx power.*/
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID1, 32); /* Signal */
@@ -872,24 +875,24 @@ static int rt2400pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00mmio_register_write(rt2x00dev, MACCSR0, 0x00217223);
 	rt2x00mmio_register_write(rt2x00dev, MACCSR1, 0x00235518);
 
-	rt2x00mmio_register_read(rt2x00dev, MACCSR2, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MACCSR2);
 	rt2x00_set_field32(&reg, MACCSR2_DELAY, 64);
 	rt2x00mmio_register_write(rt2x00dev, MACCSR2, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, RALINKCSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RALINKCSR);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_DATA0, 17);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_ID0, 154);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_DATA1, 0);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_ID1, 154);
 	rt2x00mmio_register_write(rt2x00dev, RALINKCSR, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR1);
 	rt2x00_set_field32(&reg, CSR1_SOFT_RESET, 1);
 	rt2x00_set_field32(&reg, CSR1_BBP_RESET, 0);
 	rt2x00_set_field32(&reg, CSR1_HOST_READY, 0);
 	rt2x00mmio_register_write(rt2x00dev, CSR1, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR1);
 	rt2x00_set_field32(&reg, CSR1_SOFT_RESET, 0);
 	rt2x00_set_field32(&reg, CSR1_HOST_READY, 1);
 	rt2x00mmio_register_write(rt2x00dev, CSR1, reg);
@@ -899,8 +902,8 @@ static int rt2400pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	 * These registers are cleared on read,
 	 * so we may pass a useless variable to store the value.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, CNT0, &reg);
-	rt2x00mmio_register_read(rt2x00dev, CNT4, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CNT0);
+	reg = rt2x00mmio_register_read(rt2x00dev, CNT4);
 
 	return 0;
 }
@@ -911,7 +914,7 @@ static int rt2400pci_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 	u8 value;
 
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2400pci_bbp_read(rt2x00dev, 0, &value);
+		value = rt2400pci_bbp_read(rt2x00dev, 0);
 		if ((value != 0xff) && (value != 0x00))
 			return 0;
 		udelay(REGISTER_BUSY_DELAY);
@@ -947,7 +950,7 @@ static int rt2400pci_init_bbp(struct rt2x00_dev *rt2x00dev)
 	rt2400pci_bbp_write(rt2x00dev, 31, 0x00);
 
 	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
-		rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i, &eeprom);
+		eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i);
 
 		if (eeprom != 0xffff && eeprom != 0x0000) {
 			reg_id = rt2x00_get_field16(eeprom, EEPROM_BBP_REG_ID);
@@ -974,7 +977,7 @@ static void rt2400pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 * should clear the register to assure a clean state.
 	 */
 	if (state == STATE_RADIO_IRQ_ON) {
-		rt2x00mmio_register_read(rt2x00dev, CSR7, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR7);
 		rt2x00mmio_register_write(rt2x00dev, CSR7, reg);
 	}
 
@@ -984,7 +987,7 @@ static void rt2400pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irqsave(&rt2x00dev->irqmask_lock, flags);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR8);
 	rt2x00_set_field32(&reg, CSR8_TBCN_EXPIRE, mask);
 	rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, mask);
 	rt2x00_set_field32(&reg, CSR8_TXDONE_ATIMRING, mask);
@@ -1037,7 +1040,7 @@ static int rt2400pci_set_state(struct rt2x00_dev *rt2x00dev,
 
 	put_to_sleep = (state != STATE_AWAKE);
 
-	rt2x00mmio_register_read(rt2x00dev, PWRCSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, PWRCSR1);
 	rt2x00_set_field32(&reg, PWRCSR1_SET_STATE, 1);
 	rt2x00_set_field32(&reg, PWRCSR1_BBP_DESIRE_STATE, state);
 	rt2x00_set_field32(&reg, PWRCSR1_RF_DESIRE_STATE, state);
@@ -1050,7 +1053,7 @@ static int rt2400pci_set_state(struct rt2x00_dev *rt2x00dev,
 	 * device has entered the correct state.
 	 */
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2x00mmio_register_read(rt2x00dev, PWRCSR1, &reg2);
+		reg2 = rt2x00mmio_register_read(rt2x00dev, PWRCSR1);
 		bbp_state = rt2x00_get_field32(reg2, PWRCSR1_BBP_CURR_STATE);
 		rf_state = rt2x00_get_field32(reg2, PWRCSR1_RF_CURR_STATE);
 		if (bbp_state == state && rf_state == state)
@@ -1110,16 +1113,16 @@ static void rt2400pci_write_tx_desc(struct queue_entry *entry,
 	/*
 	 * Start writing the descriptor words.
 	 */
-	rt2x00_desc_read(txd, 1, &word);
+	word = rt2x00_desc_read(txd, 1);
 	rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS, skbdesc->skb_dma);
 	rt2x00_desc_write(txd, 1, word);
 
-	rt2x00_desc_read(txd, 2, &word);
+	word = rt2x00_desc_read(txd, 2);
 	rt2x00_set_field32(&word, TXD_W2_BUFFER_LENGTH, txdesc->length);
 	rt2x00_set_field32(&word, TXD_W2_DATABYTE_COUNT, txdesc->length);
 	rt2x00_desc_write(txd, 2, word);
 
-	rt2x00_desc_read(txd, 3, &word);
+	word = rt2x00_desc_read(txd, 3);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_SIGNAL, txdesc->u.plcp.signal);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_SIGNAL_REGNUM, 5);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_SIGNAL_BUSY, 1);
@@ -1128,7 +1131,7 @@ static void rt2400pci_write_tx_desc(struct queue_entry *entry,
 	rt2x00_set_field32(&word, TXD_W3_PLCP_SERVICE_BUSY, 1);
 	rt2x00_desc_write(txd, 3, word);
 
-	rt2x00_desc_read(txd, 4, &word);
+	word = rt2x00_desc_read(txd, 4);
 	rt2x00_set_field32(&word, TXD_W4_PLCP_LENGTH_LOW,
 			   txdesc->u.plcp.length_low);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_LENGTH_LOW_REGNUM, 8);
@@ -1144,7 +1147,7 @@ static void rt2400pci_write_tx_desc(struct queue_entry *entry,
 	 * the device, whereby the device may take hold of the TXD before we
 	 * finished updating it.
 	 */
-	rt2x00_desc_read(txd, 0, &word);
+	word = rt2x00_desc_read(txd, 0);
 	rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 1);
 	rt2x00_set_field32(&word, TXD_W0_VALID, 1);
 	rt2x00_set_field32(&word, TXD_W0_MORE_FRAG,
@@ -1180,7 +1183,7 @@ static void rt2400pci_write_beacon(struct queue_entry *entry,
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR14);
 	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
 	rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 
@@ -1225,10 +1228,10 @@ static void rt2400pci_fill_rxdone(struct queue_entry *entry,
 	u32 rx_low;
 	u32 rx_high;
 
-	rt2x00_desc_read(entry_priv->desc, 0, &word0);
-	rt2x00_desc_read(entry_priv->desc, 2, &word2);
-	rt2x00_desc_read(entry_priv->desc, 3, &word3);
-	rt2x00_desc_read(entry_priv->desc, 4, &word4);
+	word0 = rt2x00_desc_read(entry_priv->desc, 0);
+	word2 = rt2x00_desc_read(entry_priv->desc, 2);
+	word3 = rt2x00_desc_read(entry_priv->desc, 3);
+	word4 = rt2x00_desc_read(entry_priv->desc, 4);
 
 	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
 		rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
@@ -1282,7 +1285,7 @@ static void rt2400pci_txdone(struct rt2x00_dev *rt2x00dev,
 	while (!rt2x00queue_empty(queue)) {
 		entry = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
 		entry_priv = entry->priv_data;
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 
 		if (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
 		    !rt2x00_get_field32(word, TXD_W0_VALID))
@@ -1320,7 +1323,7 @@ static inline void rt2400pci_enable_interrupt(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR8);
 	rt2x00_set_field32(&reg, irq_field, 0);
 	rt2x00mmio_register_write(rt2x00dev, CSR8, reg);
 
@@ -1345,7 +1348,7 @@ static void rt2400pci_txstatus_tasklet(unsigned long data)
 	if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags)) {
 		spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-		rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR8);
 		rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, 0);
 		rt2x00_set_field32(&reg, CSR8_TXDONE_ATIMRING, 0);
 		rt2x00_set_field32(&reg, CSR8_TXDONE_PRIORING, 0);
@@ -1381,7 +1384,7 @@ static irqreturn_t rt2400pci_interrupt(int irq, void *dev_instance)
 	 * Get the interrupt sources & saved to local variable.
 	 * Write register value back to clear pending interrupts.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, CSR7, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR7);
 	rt2x00mmio_register_write(rt2x00dev, CSR7, reg);
 
 	if (!reg)
@@ -1419,7 +1422,7 @@ static irqreturn_t rt2400pci_interrupt(int irq, void *dev_instance)
 	 */
 	spin_lock(&rt2x00dev->irqmask_lock);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR8);
 	reg |= mask;
 	rt2x00mmio_register_write(rt2x00dev, CSR8, reg);
 
@@ -1440,7 +1443,7 @@ static int rt2400pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	u16 word;
 	u8 *mac;
 
-	rt2x00mmio_register_read(rt2x00dev, CSR21, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR21);
 
 	eeprom.data = rt2x00dev;
 	eeprom.register_read = rt2400pci_eepromregister_read;
@@ -1461,7 +1464,7 @@ static int rt2400pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	rt2x00lib_set_mac_address(rt2x00dev, mac);
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA);
 	if (word == 0xffff) {
 		rt2x00_err(rt2x00dev, "Invalid EEPROM data detected\n");
 		return -EINVAL;
@@ -1479,13 +1482,13 @@ static int rt2400pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Read EEPROM word for configuration.
 	 */
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA);
 
 	/*
 	 * Identify RF chipset.
 	 */
 	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
-	rt2x00mmio_register_read(rt2x00dev, CSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR0);
 	rt2x00_set_chip(rt2x00dev, RT2460, value,
 			rt2x00_get_field32(reg, CSR0_REVISION));
 
@@ -1630,7 +1633,7 @@ static int rt2400pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 	 * Enable rfkill polling by setting GPIO direction of the
 	 * rfkill switch GPIO pin correctly.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, GPIOCSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, GPIOCSR);
 	rt2x00_set_field32(&reg, GPIOCSR_DIR0, 1);
 	rt2x00mmio_register_write(rt2x00dev, GPIOCSR, reg);
 
@@ -1692,9 +1695,9 @@ static u64 rt2400pci_get_tsf(struct ieee80211_hw *hw,
 	u64 tsf;
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, CSR17, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR17);
 	tsf = (u64) rt2x00_get_field32(reg, CSR17_HIGH_TSFTIMER) << 32;
-	rt2x00mmio_register_read(rt2x00dev, CSR16, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR16);
 	tsf |= rt2x00_get_field32(reg, CSR16_LOW_TSFTIMER);
 
 	return tsf;
@@ -1705,7 +1708,7 @@ static int rt2400pci_tx_last_beacon(struct ieee80211_hw *hw)
 	struct rt2x00_dev *rt2x00dev = hw->priv;
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, CSR15, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR15);
 	return rt2x00_get_field32(reg, CSR15_BEACON_SENT);
 }
 
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2500pci.c b/drivers/net/wireless/ralink/rt2x00/rt2500pci.c
index 791434de8052..1ff5434798ec 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2500pci.c
@@ -77,10 +77,11 @@ static void rt2500pci_bbp_write(struct rt2x00_dev *rt2x00dev,
 	mutex_unlock(&rt2x00dev->csr_mutex);
 }
 
-static void rt2500pci_bbp_read(struct rt2x00_dev *rt2x00dev,
-			       const unsigned int word, u8 *value)
+static u8 rt2500pci_bbp_read(struct rt2x00_dev *rt2x00dev,
+			     const unsigned int word)
 {
 	u32 reg;
+	u8 value;
 
 	mutex_lock(&rt2x00dev->csr_mutex);
 
@@ -103,9 +104,11 @@ static void rt2500pci_bbp_read(struct rt2x00_dev *rt2x00dev,
 		WAIT_FOR_BBP(rt2x00dev, &reg);
 	}
 
-	*value = rt2x00_get_field32(reg, BBPCSR_VALUE);
+	value = rt2x00_get_field32(reg, BBPCSR_VALUE);
 
 	mutex_unlock(&rt2x00dev->csr_mutex);
+
+	return value;
 }
 
 static void rt2500pci_rf_write(struct rt2x00_dev *rt2x00dev,
@@ -138,7 +141,7 @@ static void rt2500pci_eepromregister_read(struct eeprom_93cx6 *eeprom)
 	struct rt2x00_dev *rt2x00dev = eeprom->data;
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, CSR21, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR21);
 
 	eeprom->reg_data_in = !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_IN);
 	eeprom->reg_data_out = !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_OUT);
@@ -202,7 +205,7 @@ static int rt2500pci_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, GPIOCSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, GPIOCSR);
 	return rt2x00_get_field32(reg, GPIOCSR_VAL0);
 }
 
@@ -215,7 +218,7 @@ static void rt2500pci_brightness_set(struct led_classdev *led_cdev,
 	unsigned int enabled = brightness != LED_OFF;
 	u32 reg;
 
-	rt2x00mmio_register_read(led->rt2x00dev, LEDCSR, &reg);
+	reg = rt2x00mmio_register_read(led->rt2x00dev, LEDCSR);
 
 	if (led->type == LED_TYPE_RADIO || led->type == LED_TYPE_ASSOC)
 		rt2x00_set_field32(&reg, LEDCSR_LINK, enabled);
@@ -233,7 +236,7 @@ static int rt2500pci_blink_set(struct led_classdev *led_cdev,
 	    container_of(led_cdev, struct rt2x00_led, led_dev);
 	u32 reg;
 
-	rt2x00mmio_register_read(led->rt2x00dev, LEDCSR, &reg);
+	reg = rt2x00mmio_register_read(led->rt2x00dev, LEDCSR);
 	rt2x00_set_field32(&reg, LEDCSR_ON_PERIOD, *delay_on);
 	rt2x00_set_field32(&reg, LEDCSR_OFF_PERIOD, *delay_off);
 	rt2x00mmio_register_write(led->rt2x00dev, LEDCSR, reg);
@@ -267,7 +270,7 @@ static void rt2500pci_config_filter(struct rt2x00_dev *rt2x00dev,
 	 * and broadcast frames will always be accepted since
 	 * there is no filter for it at this time.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, RXCSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RXCSR0);
 	rt2x00_set_field32(&reg, RXCSR0_DROP_CRC,
 			   !(filter_flags & FIF_FCSFAIL));
 	rt2x00_set_field32(&reg, RXCSR0_DROP_PHYSICAL,
@@ -300,7 +303,7 @@ static void rt2500pci_config_intf(struct rt2x00_dev *rt2x00dev,
 		 * Enable beacon config
 		 */
 		bcn_preload = PREAMBLE + GET_DURATION(IEEE80211_HEADER, 20);
-		rt2x00mmio_register_read(rt2x00dev, BCNCSR1, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, BCNCSR1);
 		rt2x00_set_field32(&reg, BCNCSR1_PRELOAD, bcn_preload);
 		rt2x00_set_field32(&reg, BCNCSR1_BEACON_CWMIN, queue->cw_min);
 		rt2x00mmio_register_write(rt2x00dev, BCNCSR1, reg);
@@ -308,7 +311,7 @@ static void rt2500pci_config_intf(struct rt2x00_dev *rt2x00dev,
 		/*
 		 * Enable synchronisation.
 		 */
-		rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR14);
 		rt2x00_set_field32(&reg, CSR14_TSF_SYNC, conf->sync);
 		rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 	}
@@ -335,35 +338,35 @@ static void rt2500pci_config_erp(struct rt2x00_dev *rt2x00dev,
 	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
 		preamble_mask = erp->short_preamble << 3;
 
-		rt2x00mmio_register_read(rt2x00dev, TXCSR1, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXCSR1);
 		rt2x00_set_field32(&reg, TXCSR1_ACK_TIMEOUT, 0x162);
 		rt2x00_set_field32(&reg, TXCSR1_ACK_CONSUME_TIME, 0xa2);
 		rt2x00_set_field32(&reg, TXCSR1_TSF_OFFSET, IEEE80211_HEADER);
 		rt2x00_set_field32(&reg, TXCSR1_AUTORESPONDER, 1);
 		rt2x00mmio_register_write(rt2x00dev, TXCSR1, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, ARCSR2, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, ARCSR2);
 		rt2x00_set_field32(&reg, ARCSR2_SIGNAL, 0x00);
 		rt2x00_set_field32(&reg, ARCSR2_SERVICE, 0x04);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 10));
 		rt2x00mmio_register_write(rt2x00dev, ARCSR2, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, ARCSR3, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, ARCSR3);
 		rt2x00_set_field32(&reg, ARCSR3_SIGNAL, 0x01 | preamble_mask);
 		rt2x00_set_field32(&reg, ARCSR3_SERVICE, 0x04);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 20));
 		rt2x00mmio_register_write(rt2x00dev, ARCSR3, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, ARCSR4, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, ARCSR4);
 		rt2x00_set_field32(&reg, ARCSR4_SIGNAL, 0x02 | preamble_mask);
 		rt2x00_set_field32(&reg, ARCSR4_SERVICE, 0x04);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
 				   GET_DURATION(ACK_SIZE, 55));
 		rt2x00mmio_register_write(rt2x00dev, ARCSR4, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, ARCSR5, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, ARCSR5);
 		rt2x00_set_field32(&reg, ARCSR5_SIGNAL, 0x03 | preamble_mask);
 		rt2x00_set_field32(&reg, ARCSR5_SERVICE, 0x84);
 		rt2x00_set_field32(&reg, ARCSR2_LENGTH,
@@ -375,23 +378,23 @@ static void rt2500pci_config_erp(struct rt2x00_dev *rt2x00dev,
 		rt2x00mmio_register_write(rt2x00dev, ARCSR1, erp->basic_rates);
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
-		rt2x00mmio_register_read(rt2x00dev, CSR11, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR11);
 		rt2x00_set_field32(&reg, CSR11_SLOT_TIME, erp->slot_time);
 		rt2x00mmio_register_write(rt2x00dev, CSR11, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, CSR18, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR18);
 		rt2x00_set_field32(&reg, CSR18_SIFS, erp->sifs);
 		rt2x00_set_field32(&reg, CSR18_PIFS, erp->pifs);
 		rt2x00mmio_register_write(rt2x00dev, CSR18, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, CSR19, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR19);
 		rt2x00_set_field32(&reg, CSR19_DIFS, erp->difs);
 		rt2x00_set_field32(&reg, CSR19_EIFS, erp->eifs);
 		rt2x00mmio_register_write(rt2x00dev, CSR19, reg);
 	}
 
 	if (changed & BSS_CHANGED_BEACON_INT) {
-		rt2x00mmio_register_read(rt2x00dev, CSR12, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR12);
 		rt2x00_set_field32(&reg, CSR12_BEACON_INTERVAL,
 				   erp->beacon_int * 16);
 		rt2x00_set_field32(&reg, CSR12_CFP_MAX_DURATION,
@@ -415,9 +418,9 @@ static void rt2500pci_config_ant(struct rt2x00_dev *rt2x00dev,
 	BUG_ON(ant->rx == ANTENNA_SW_DIVERSITY ||
 	       ant->tx == ANTENNA_SW_DIVERSITY);
 
-	rt2x00mmio_register_read(rt2x00dev, BBPCSR1, &reg);
-	rt2500pci_bbp_read(rt2x00dev, 14, &r14);
-	rt2500pci_bbp_read(rt2x00dev, 2, &r2);
+	reg = rt2x00mmio_register_read(rt2x00dev, BBPCSR1);
+	r14 = rt2500pci_bbp_read(rt2x00dev, 14);
+	r2 = rt2500pci_bbp_read(rt2x00dev, 2);
 
 	/*
 	 * Configure the TX antenna.
@@ -538,7 +541,7 @@ static void rt2500pci_config_channel(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Clear false CRC during channel switch.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, CNT0, &rf->rf1);
+	rf->rf1 = rt2x00mmio_register_read(rt2x00dev, CNT0);
 }
 
 static void rt2500pci_config_txpower(struct rt2x00_dev *rt2x00dev,
@@ -546,7 +549,7 @@ static void rt2500pci_config_txpower(struct rt2x00_dev *rt2x00dev,
 {
 	u32 rf3;
 
-	rt2x00_rf_read(rt2x00dev, 3, &rf3);
+	rf3 = rt2x00_rf_read(rt2x00dev, 3);
 	rt2x00_set_field32(&rf3, RF3_TXPOWER, TXPOWER_TO_DEV(txpower));
 	rt2500pci_rf_write(rt2x00dev, 3, rf3);
 }
@@ -556,7 +559,7 @@ static void rt2500pci_config_retry_limit(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, CSR11, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR11);
 	rt2x00_set_field32(&reg, CSR11_LONG_RETRY,
 			   libconf->conf->long_frame_max_tx_count);
 	rt2x00_set_field32(&reg, CSR11_SHORT_RETRY,
@@ -573,7 +576,7 @@ static void rt2500pci_config_ps(struct rt2x00_dev *rt2x00dev,
 	u32 reg;
 
 	if (state == STATE_SLEEP) {
-		rt2x00mmio_register_read(rt2x00dev, CSR20, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR20);
 		rt2x00_set_field32(&reg, CSR20_DELAY_AFTER_TBCN,
 				   (rt2x00dev->beacon_int - 20) * 16);
 		rt2x00_set_field32(&reg, CSR20_TBCN_BEFORE_WAKEUP,
@@ -586,7 +589,7 @@ static void rt2500pci_config_ps(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, CSR20_AUTOWAKE, 1);
 		rt2x00mmio_register_write(rt2x00dev, CSR20, reg);
 	} else {
-		rt2x00mmio_register_read(rt2x00dev, CSR20, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR20);
 		rt2x00_set_field32(&reg, CSR20_AUTOWAKE, 0);
 		rt2x00mmio_register_write(rt2x00dev, CSR20, reg);
 	}
@@ -622,13 +625,13 @@ static void rt2500pci_link_stats(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Update FCS error count from register.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, CNT0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CNT0);
 	qual->rx_failed = rt2x00_get_field32(reg, CNT0_FCS_ERROR);
 
 	/*
 	 * Update False CCA count from register.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, CNT3, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CNT3);
 	qual->false_cca = rt2x00_get_field32(reg, CNT3_FALSE_CCA);
 }
 
@@ -728,12 +731,12 @@ static void rt2500pci_start_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00mmio_register_read(rt2x00dev, RXCSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, RXCSR0);
 		rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX, 0);
 		rt2x00mmio_register_write(rt2x00dev, RXCSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR14);
 		rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
 		rt2x00_set_field32(&reg, CSR14_TBCN, 1);
 		rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 1);
@@ -751,17 +754,17 @@ static void rt2500pci_kick_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_AC_VO:
-		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXCSR0);
 		rt2x00_set_field32(&reg, TXCSR0_KICK_PRIO, 1);
 		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	case QID_AC_VI:
-		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXCSR0);
 		rt2x00_set_field32(&reg, TXCSR0_KICK_TX, 1);
 		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	case QID_ATIM:
-		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXCSR0);
 		rt2x00_set_field32(&reg, TXCSR0_KICK_ATIM, 1);
 		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
@@ -779,17 +782,17 @@ static void rt2500pci_stop_queue(struct data_queue *queue)
 	case QID_AC_VO:
 	case QID_AC_VI:
 	case QID_ATIM:
-		rt2x00mmio_register_read(rt2x00dev, TXCSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXCSR0);
 		rt2x00_set_field32(&reg, TXCSR0_ABORT, 1);
 		rt2x00mmio_register_write(rt2x00dev, TXCSR0, reg);
 		break;
 	case QID_RX:
-		rt2x00mmio_register_read(rt2x00dev, RXCSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, RXCSR0);
 		rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX, 1);
 		rt2x00mmio_register_write(rt2x00dev, RXCSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR14);
 		rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
 		rt2x00_set_field32(&reg, CSR14_TBCN, 0);
 		rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
@@ -814,11 +817,11 @@ static bool rt2500pci_get_entry_state(struct queue_entry *entry)
 	u32 word;
 
 	if (entry->queue->qid == QID_RX) {
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 
 		return rt2x00_get_field32(word, RXD_W0_OWNER_NIC);
 	} else {
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 
 		return (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
 		        rt2x00_get_field32(word, TXD_W0_VALID));
@@ -832,15 +835,15 @@ static void rt2500pci_clear_entry(struct queue_entry *entry)
 	u32 word;
 
 	if (entry->queue->qid == QID_RX) {
-		rt2x00_desc_read(entry_priv->desc, 1, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 1);
 		rt2x00_set_field32(&word, RXD_W1_BUFFER_ADDRESS, skbdesc->skb_dma);
 		rt2x00_desc_write(entry_priv->desc, 1, word);
 
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 		rt2x00_set_field32(&word, RXD_W0_OWNER_NIC, 1);
 		rt2x00_desc_write(entry_priv->desc, 0, word);
 	} else {
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 		rt2x00_set_field32(&word, TXD_W0_VALID, 0);
 		rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 0);
 		rt2x00_desc_write(entry_priv->desc, 0, word);
@@ -855,7 +858,7 @@ static int rt2500pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize registers.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, TXCSR2, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXCSR2);
 	rt2x00_set_field32(&reg, TXCSR2_TXD_SIZE, rt2x00dev->tx[0].desc_size);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_TXD, rt2x00dev->tx[1].limit);
 	rt2x00_set_field32(&reg, TXCSR2_NUM_ATIM, rt2x00dev->atim->limit);
@@ -863,36 +866,36 @@ static int rt2500pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	rt2x00mmio_register_write(rt2x00dev, TXCSR2, reg);
 
 	entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, TXCSR3, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXCSR3);
 	rt2x00_set_field32(&reg, TXCSR3_TX_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, TXCSR3, reg);
 
 	entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, TXCSR5, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXCSR5);
 	rt2x00_set_field32(&reg, TXCSR5_PRIO_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, TXCSR5, reg);
 
 	entry_priv = rt2x00dev->atim->entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, TXCSR4, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXCSR4);
 	rt2x00_set_field32(&reg, TXCSR4_ATIM_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, TXCSR4, reg);
 
 	entry_priv = rt2x00dev->bcn->entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, TXCSR6, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXCSR6);
 	rt2x00_set_field32(&reg, TXCSR6_BEACON_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, TXCSR6, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, RXCSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RXCSR1);
 	rt2x00_set_field32(&reg, RXCSR1_RXD_SIZE, rt2x00dev->rx->desc_size);
 	rt2x00_set_field32(&reg, RXCSR1_NUM_RXD, rt2x00dev->rx->limit);
 	rt2x00mmio_register_write(rt2x00dev, RXCSR1, reg);
 
 	entry_priv = rt2x00dev->rx->entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, RXCSR2, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RXCSR2);
 	rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, RXCSR2, reg);
@@ -909,13 +912,13 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00mmio_register_write(rt2x00dev, PSCSR2, 0x00020002);
 	rt2x00mmio_register_write(rt2x00dev, PSCSR3, 0x00000002);
 
-	rt2x00mmio_register_read(rt2x00dev, TIMECSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TIMECSR);
 	rt2x00_set_field32(&reg, TIMECSR_US_COUNT, 33);
 	rt2x00_set_field32(&reg, TIMECSR_US_64_COUNT, 63);
 	rt2x00_set_field32(&reg, TIMECSR_BEACON_EXPECT, 0);
 	rt2x00mmio_register_write(rt2x00dev, TIMECSR, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR9, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR9);
 	rt2x00_set_field32(&reg, CSR9_MAX_FRAME_UNIT,
 			   rt2x00dev->rx->data_size / 128);
 	rt2x00mmio_register_write(rt2x00dev, CSR9, reg);
@@ -923,11 +926,11 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Always use CWmin and CWmax set in descriptor.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, CSR11, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR11);
 	rt2x00_set_field32(&reg, CSR11_CW_SELECT, 0);
 	rt2x00mmio_register_write(rt2x00dev, CSR11, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR14);
 	rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
 	rt2x00_set_field32(&reg, CSR14_TSF_SYNC, 0);
 	rt2x00_set_field32(&reg, CSR14_TBCN, 0);
@@ -940,7 +943,7 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 
 	rt2x00mmio_register_write(rt2x00dev, CNT3, 0);
 
-	rt2x00mmio_register_read(rt2x00dev, TXCSR8, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXCSR8);
 	rt2x00_set_field32(&reg, TXCSR8_BBP_ID0, 10);
 	rt2x00_set_field32(&reg, TXCSR8_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, TXCSR8_BBP_ID1, 11);
@@ -951,28 +954,28 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, TXCSR8_BBP_ID3_VALID, 1);
 	rt2x00mmio_register_write(rt2x00dev, TXCSR8, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, ARTCSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, ARTCSR0);
 	rt2x00_set_field32(&reg, ARTCSR0_ACK_CTS_1MBS, 112);
 	rt2x00_set_field32(&reg, ARTCSR0_ACK_CTS_2MBS, 56);
 	rt2x00_set_field32(&reg, ARTCSR0_ACK_CTS_5_5MBS, 20);
 	rt2x00_set_field32(&reg, ARTCSR0_ACK_CTS_11MBS, 10);
 	rt2x00mmio_register_write(rt2x00dev, ARTCSR0, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, ARTCSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, ARTCSR1);
 	rt2x00_set_field32(&reg, ARTCSR1_ACK_CTS_6MBS, 45);
 	rt2x00_set_field32(&reg, ARTCSR1_ACK_CTS_9MBS, 37);
 	rt2x00_set_field32(&reg, ARTCSR1_ACK_CTS_12MBS, 33);
 	rt2x00_set_field32(&reg, ARTCSR1_ACK_CTS_18MBS, 29);
 	rt2x00mmio_register_write(rt2x00dev, ARTCSR1, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, ARTCSR2, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, ARTCSR2);
 	rt2x00_set_field32(&reg, ARTCSR2_ACK_CTS_24MBS, 29);
 	rt2x00_set_field32(&reg, ARTCSR2_ACK_CTS_36MBS, 25);
 	rt2x00_set_field32(&reg, ARTCSR2_ACK_CTS_48MBS, 25);
 	rt2x00_set_field32(&reg, ARTCSR2_ACK_CTS_54MBS, 25);
 	rt2x00mmio_register_write(rt2x00dev, ARTCSR2, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, RXCSR3, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RXCSR3);
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID0, 47); /* CCK Signal */
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID1, 51); /* Rssi */
@@ -983,7 +986,7 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, RXCSR3_BBP_ID3_VALID, 1);
 	rt2x00mmio_register_write(rt2x00dev, RXCSR3, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, PCICSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, PCICSR);
 	rt2x00_set_field32(&reg, PCICSR_BIG_ENDIAN, 0);
 	rt2x00_set_field32(&reg, PCICSR_RX_TRESHOLD, 0);
 	rt2x00_set_field32(&reg, PCICSR_TX_TRESHOLD, 3);
@@ -1004,11 +1007,11 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00mmio_register_write(rt2x00dev, MACCSR0, 0x00213223);
 	rt2x00mmio_register_write(rt2x00dev, MACCSR1, 0x00235518);
 
-	rt2x00mmio_register_read(rt2x00dev, MACCSR2, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MACCSR2);
 	rt2x00_set_field32(&reg, MACCSR2_DELAY, 64);
 	rt2x00mmio_register_write(rt2x00dev, MACCSR2, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, RALINKCSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RALINKCSR);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_DATA0, 17);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_ID0, 26);
 	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_VALID0, 1);
@@ -1021,13 +1024,13 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 
 	rt2x00mmio_register_write(rt2x00dev, TXACKCSR0, 0x00000020);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR1);
 	rt2x00_set_field32(&reg, CSR1_SOFT_RESET, 1);
 	rt2x00_set_field32(&reg, CSR1_BBP_RESET, 0);
 	rt2x00_set_field32(&reg, CSR1_HOST_READY, 0);
 	rt2x00mmio_register_write(rt2x00dev, CSR1, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR1);
 	rt2x00_set_field32(&reg, CSR1_SOFT_RESET, 0);
 	rt2x00_set_field32(&reg, CSR1_HOST_READY, 1);
 	rt2x00mmio_register_write(rt2x00dev, CSR1, reg);
@@ -1037,8 +1040,8 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	 * These registers are cleared on read,
 	 * so we may pass a useless variable to store the value.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, CNT0, &reg);
-	rt2x00mmio_register_read(rt2x00dev, CNT4, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CNT0);
+	reg = rt2x00mmio_register_read(rt2x00dev, CNT4);
 
 	return 0;
 }
@@ -1049,7 +1052,7 @@ static int rt2500pci_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 	u8 value;
 
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2500pci_bbp_read(rt2x00dev, 0, &value);
+		value = rt2500pci_bbp_read(rt2x00dev, 0);
 		if ((value != 0xff) && (value != 0x00))
 			return 0;
 		udelay(REGISTER_BUSY_DELAY);
@@ -1101,7 +1104,7 @@ static int rt2500pci_init_bbp(struct rt2x00_dev *rt2x00dev)
 	rt2500pci_bbp_write(rt2x00dev, 62, 0x10);
 
 	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
-		rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i, &eeprom);
+		eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i);
 
 		if (eeprom != 0xffff && eeprom != 0x0000) {
 			reg_id = rt2x00_get_field16(eeprom, EEPROM_BBP_REG_ID);
@@ -1128,7 +1131,7 @@ static void rt2500pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 * should clear the register to assure a clean state.
 	 */
 	if (state == STATE_RADIO_IRQ_ON) {
-		rt2x00mmio_register_read(rt2x00dev, CSR7, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR7);
 		rt2x00mmio_register_write(rt2x00dev, CSR7, reg);
 	}
 
@@ -1138,7 +1141,7 @@ static void rt2500pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irqsave(&rt2x00dev->irqmask_lock, flags);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR8);
 	rt2x00_set_field32(&reg, CSR8_TBCN_EXPIRE, mask);
 	rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, mask);
 	rt2x00_set_field32(&reg, CSR8_TXDONE_ATIMRING, mask);
@@ -1190,7 +1193,7 @@ static int rt2500pci_set_state(struct rt2x00_dev *rt2x00dev,
 
 	put_to_sleep = (state != STATE_AWAKE);
 
-	rt2x00mmio_register_read(rt2x00dev, PWRCSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, PWRCSR1);
 	rt2x00_set_field32(&reg, PWRCSR1_SET_STATE, 1);
 	rt2x00_set_field32(&reg, PWRCSR1_BBP_DESIRE_STATE, state);
 	rt2x00_set_field32(&reg, PWRCSR1_RF_DESIRE_STATE, state);
@@ -1203,7 +1206,7 @@ static int rt2500pci_set_state(struct rt2x00_dev *rt2x00dev,
 	 * device has entered the correct state.
 	 */
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2x00mmio_register_read(rt2x00dev, PWRCSR1, &reg2);
+		reg2 = rt2x00mmio_register_read(rt2x00dev, PWRCSR1);
 		bbp_state = rt2x00_get_field32(reg2, PWRCSR1_BBP_CURR_STATE);
 		rf_state = rt2x00_get_field32(reg2, PWRCSR1_RF_CURR_STATE);
 		if (bbp_state == state && rf_state == state)
@@ -1263,18 +1266,18 @@ static void rt2500pci_write_tx_desc(struct queue_entry *entry,
 	/*
 	 * Start writing the descriptor words.
 	 */
-	rt2x00_desc_read(txd, 1, &word);
+	word = rt2x00_desc_read(txd, 1);
 	rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS, skbdesc->skb_dma);
 	rt2x00_desc_write(txd, 1, word);
 
-	rt2x00_desc_read(txd, 2, &word);
+	word = rt2x00_desc_read(txd, 2);
 	rt2x00_set_field32(&word, TXD_W2_IV_OFFSET, IEEE80211_HEADER);
 	rt2x00_set_field32(&word, TXD_W2_AIFS, entry->queue->aifs);
 	rt2x00_set_field32(&word, TXD_W2_CWMIN, entry->queue->cw_min);
 	rt2x00_set_field32(&word, TXD_W2_CWMAX, entry->queue->cw_max);
 	rt2x00_desc_write(txd, 2, word);
 
-	rt2x00_desc_read(txd, 3, &word);
+	word = rt2x00_desc_read(txd, 3);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_SIGNAL, txdesc->u.plcp.signal);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_SERVICE, txdesc->u.plcp.service);
 	rt2x00_set_field32(&word, TXD_W3_PLCP_LENGTH_LOW,
@@ -1283,7 +1286,7 @@ static void rt2500pci_write_tx_desc(struct queue_entry *entry,
 			   txdesc->u.plcp.length_high);
 	rt2x00_desc_write(txd, 3, word);
 
-	rt2x00_desc_read(txd, 10, &word);
+	word = rt2x00_desc_read(txd, 10);
 	rt2x00_set_field32(&word, TXD_W10_RTS,
 			   test_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags));
 	rt2x00_desc_write(txd, 10, word);
@@ -1293,7 +1296,7 @@ static void rt2500pci_write_tx_desc(struct queue_entry *entry,
 	 * the device, whereby the device may take hold of the TXD before we
 	 * finished updating it.
 	 */
-	rt2x00_desc_read(txd, 0, &word);
+	word = rt2x00_desc_read(txd, 0);
 	rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 1);
 	rt2x00_set_field32(&word, TXD_W0_VALID, 1);
 	rt2x00_set_field32(&word, TXD_W0_MORE_FRAG,
@@ -1332,7 +1335,7 @@ static void rt2500pci_write_beacon(struct queue_entry *entry,
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, CSR14, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR14);
 	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
 	rt2x00mmio_register_write(rt2x00dev, CSR14, reg);
 
@@ -1368,8 +1371,8 @@ static void rt2500pci_fill_rxdone(struct queue_entry *entry,
 	u32 word0;
 	u32 word2;
 
-	rt2x00_desc_read(entry_priv->desc, 0, &word0);
-	rt2x00_desc_read(entry_priv->desc, 2, &word2);
+	word0 = rt2x00_desc_read(entry_priv->desc, 0);
+	word2 = rt2x00_desc_read(entry_priv->desc, 2);
 
 	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
 		rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
@@ -1410,7 +1413,7 @@ static void rt2500pci_txdone(struct rt2x00_dev *rt2x00dev,
 	while (!rt2x00queue_empty(queue)) {
 		entry = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
 		entry_priv = entry->priv_data;
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 
 		if (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
 		    !rt2x00_get_field32(word, TXD_W0_VALID))
@@ -1448,7 +1451,7 @@ static inline void rt2500pci_enable_interrupt(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR8);
 	rt2x00_set_field32(&reg, irq_field, 0);
 	rt2x00mmio_register_write(rt2x00dev, CSR8, reg);
 
@@ -1473,7 +1476,7 @@ static void rt2500pci_txstatus_tasklet(unsigned long data)
 	if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags)) {
 		spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-		rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, CSR8);
 		rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, 0);
 		rt2x00_set_field32(&reg, CSR8_TXDONE_ATIMRING, 0);
 		rt2x00_set_field32(&reg, CSR8_TXDONE_PRIORING, 0);
@@ -1509,7 +1512,7 @@ static irqreturn_t rt2500pci_interrupt(int irq, void *dev_instance)
 	 * Get the interrupt sources & saved to local variable.
 	 * Write register value back to clear pending interrupts.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, CSR7, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR7);
 	rt2x00mmio_register_write(rt2x00dev, CSR7, reg);
 
 	if (!reg)
@@ -1547,7 +1550,7 @@ static irqreturn_t rt2500pci_interrupt(int irq, void *dev_instance)
 	 */
 	spin_lock(&rt2x00dev->irqmask_lock);
 
-	rt2x00mmio_register_read(rt2x00dev, CSR8, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR8);
 	reg |= mask;
 	rt2x00mmio_register_write(rt2x00dev, CSR8, reg);
 
@@ -1566,7 +1569,7 @@ static int rt2500pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	u16 word;
 	u8 *mac;
 
-	rt2x00mmio_register_read(rt2x00dev, CSR21, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR21);
 
 	eeprom.data = rt2x00dev;
 	eeprom.register_read = rt2500pci_eepromregister_read;
@@ -1587,7 +1590,7 @@ static int rt2500pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	rt2x00lib_set_mac_address(rt2x00dev, mac);
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_NUM, 2);
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_TX_DEFAULT,
@@ -1603,7 +1606,7 @@ static int rt2500pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "Antenna: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_NIC_CARDBUS_ACCEL, 0);
 		rt2x00_set_field16(&word, EEPROM_NIC_DYN_BBP_TUNE, 0);
@@ -1612,7 +1615,7 @@ static int rt2500pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "NIC: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_CALIBRATE_OFFSET_RSSI,
 				   DEFAULT_RSSI_OFFSET);
@@ -1633,13 +1636,13 @@ static int rt2500pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Read EEPROM word for configuration.
 	 */
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA);
 
 	/*
 	 * Identify RF chipset.
 	 */
 	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
-	rt2x00mmio_register_read(rt2x00dev, CSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR0);
 	rt2x00_set_chip(rt2x00dev, RT2560, value,
 			rt2x00_get_field32(reg, CSR0_REVISION));
 
@@ -1689,14 +1692,14 @@ static int rt2500pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Check if the BBP tuning should be enabled.
 	 */
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC);
 	if (!rt2x00_get_field16(eeprom, EEPROM_NIC_DYN_BBP_TUNE))
 		__set_bit(CAPABILITY_LINK_TUNING, &rt2x00dev->cap_flags);
 
 	/*
 	 * Read the RSSI <-> dBm offset information.
 	 */
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET);
 	rt2x00dev->rssi_offset =
 	    rt2x00_get_field16(eeprom, EEPROM_CALIBRATE_OFFSET_RSSI);
 
@@ -1955,7 +1958,7 @@ static int rt2500pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 	 * Enable rfkill polling by setting GPIO direction of the
 	 * rfkill switch GPIO pin correctly.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, GPIOCSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, GPIOCSR);
 	rt2x00_set_field32(&reg, GPIOCSR_DIR0, 1);
 	rt2x00mmio_register_write(rt2x00dev, GPIOCSR, reg);
 
@@ -1991,9 +1994,9 @@ static u64 rt2500pci_get_tsf(struct ieee80211_hw *hw,
 	u64 tsf;
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, CSR17, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR17);
 	tsf = (u64) rt2x00_get_field32(reg, CSR17_HIGH_TSFTIMER) << 32;
-	rt2x00mmio_register_read(rt2x00dev, CSR16, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR16);
 	tsf |= rt2x00_get_field32(reg, CSR16_LOW_TSFTIMER);
 
 	return tsf;
@@ -2004,7 +2007,7 @@ static int rt2500pci_tx_last_beacon(struct ieee80211_hw *hw)
 	struct rt2x00_dev *rt2x00dev = hw->priv;
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, CSR15, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CSR15);
 	return rt2x00_get_field32(reg, CSR15_BEACON_SENT);
 }
 
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2500usb.c b/drivers/net/wireless/ralink/rt2x00/rt2500usb.c
index 0d2670a56c4c..529e05999abb 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2500usb.c
@@ -55,26 +55,24 @@ MODULE_PARM_DESC(nohwcrypt, "Disable hardware encryption.");
  * If the csr_mutex is already held then the _lock variants must
  * be used instead.
  */
-static void rt2500usb_register_read(struct rt2x00_dev *rt2x00dev,
-					   const unsigned int offset,
-					   u16 *value)
+static u16 rt2500usb_register_read(struct rt2x00_dev *rt2x00dev,
+				   const unsigned int offset)
 {
 	__le16 reg;
 	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_READ,
 				      USB_VENDOR_REQUEST_IN, offset,
 				      &reg, sizeof(reg));
-	*value = le16_to_cpu(reg);
+	return le16_to_cpu(reg);
 }
 
-static void rt2500usb_register_read_lock(struct rt2x00_dev *rt2x00dev,
-						const unsigned int offset,
-						u16 *value)
+static u16 rt2500usb_register_read_lock(struct rt2x00_dev *rt2x00dev,
+					const unsigned int offset)
 {
 	__le16 reg;
 	rt2x00usb_vendor_req_buff_lock(rt2x00dev, USB_MULTI_READ,
 				       USB_VENDOR_REQUEST_IN, offset,
 				       &reg, sizeof(reg), REGISTER_TIMEOUT);
-	*value = le16_to_cpu(reg);
+	return le16_to_cpu(reg);
 }
 
 static void rt2500usb_register_write(struct rt2x00_dev *rt2x00dev,
@@ -114,7 +112,7 @@ static int rt2500usb_regbusy_read(struct rt2x00_dev *rt2x00dev,
 	unsigned int i;
 
 	for (i = 0; i < REGISTER_USB_BUSY_COUNT; i++) {
-		rt2500usb_register_read_lock(rt2x00dev, offset, reg);
+		*reg = rt2500usb_register_read_lock(rt2x00dev, offset);
 		if (!rt2x00_get_field16(*reg, field))
 			return 1;
 		udelay(REGISTER_BUSY_DELAY);
@@ -155,10 +153,11 @@ static void rt2500usb_bbp_write(struct rt2x00_dev *rt2x00dev,
 	mutex_unlock(&rt2x00dev->csr_mutex);
 }
 
-static void rt2500usb_bbp_read(struct rt2x00_dev *rt2x00dev,
-			       const unsigned int word, u8 *value)
+static u8 rt2500usb_bbp_read(struct rt2x00_dev *rt2x00dev,
+			     const unsigned int word)
 {
 	u16 reg;
+	u8 value;
 
 	mutex_lock(&rt2x00dev->csr_mutex);
 
@@ -178,12 +177,14 @@ static void rt2500usb_bbp_read(struct rt2x00_dev *rt2x00dev,
 		rt2500usb_register_write_lock(rt2x00dev, PHY_CSR7, reg);
 
 		if (WAIT_FOR_BBP(rt2x00dev, &reg))
-			rt2500usb_register_read_lock(rt2x00dev, PHY_CSR7, &reg);
+			reg = rt2500usb_register_read_lock(rt2x00dev, PHY_CSR7);
 	}
 
-	*value = rt2x00_get_field16(reg, PHY_CSR7_DATA);
+	value = rt2x00_get_field16(reg, PHY_CSR7_DATA);
 
 	mutex_unlock(&rt2x00dev->csr_mutex);
+
+	return value;
 }
 
 static void rt2500usb_rf_write(struct rt2x00_dev *rt2x00dev,
@@ -216,14 +217,10 @@ static void rt2500usb_rf_write(struct rt2x00_dev *rt2x00dev,
 }
 
 #ifdef CONFIG_RT2X00_LIB_DEBUGFS
-static void _rt2500usb_register_read(struct rt2x00_dev *rt2x00dev,
-				     const unsigned int offset,
-				     u32 *value)
+static u32 _rt2500usb_register_read(struct rt2x00_dev *rt2x00dev,
+				     const unsigned int offset)
 {
-	u16 tmp;
-
-	rt2500usb_register_read(rt2x00dev, offset, &tmp);
-	*value = tmp;
+	return rt2500usb_register_read(rt2x00dev, offset);
 }
 
 static void _rt2500usb_register_write(struct rt2x00_dev *rt2x00dev,
@@ -271,7 +268,7 @@ static int rt2500usb_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 {
 	u16 reg;
 
-	rt2500usb_register_read(rt2x00dev, MAC_CSR19, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, MAC_CSR19);
 	return rt2x00_get_field16(reg, MAC_CSR19_VAL7);
 }
 
@@ -284,7 +281,7 @@ static void rt2500usb_brightness_set(struct led_classdev *led_cdev,
 	unsigned int enabled = brightness != LED_OFF;
 	u16 reg;
 
-	rt2500usb_register_read(led->rt2x00dev, MAC_CSR20, &reg);
+	reg = rt2500usb_register_read(led->rt2x00dev, MAC_CSR20);
 
 	if (led->type == LED_TYPE_RADIO || led->type == LED_TYPE_ASSOC)
 		rt2x00_set_field16(&reg, MAC_CSR20_LINK, enabled);
@@ -302,7 +299,7 @@ static int rt2500usb_blink_set(struct led_classdev *led_cdev,
 	    container_of(led_cdev, struct rt2x00_led, led_dev);
 	u16 reg;
 
-	rt2500usb_register_read(led->rt2x00dev, MAC_CSR21, &reg);
+	reg = rt2500usb_register_read(led->rt2x00dev, MAC_CSR21);
 	rt2x00_set_field16(&reg, MAC_CSR21_ON_PERIOD, *delay_on);
 	rt2x00_set_field16(&reg, MAC_CSR21_OFF_PERIOD, *delay_off);
 	rt2500usb_register_write(led->rt2x00dev, MAC_CSR21, reg);
@@ -356,7 +353,7 @@ static int rt2500usb_config_key(struct rt2x00_dev *rt2x00dev,
 		 */
 		mask = TXRX_CSR0_KEY_ID.bit_mask;
 
-		rt2500usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+		reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR0);
 		curr_cipher = rt2x00_get_field16(reg, TXRX_CSR0_ALGORITHM);
 		reg &= mask;
 
@@ -395,7 +392,7 @@ static int rt2500usb_config_key(struct rt2x00_dev *rt2x00dev,
 	 * TXRX_CSR0_KEY_ID contains only single-bit fields to indicate
 	 * a particular key is valid.
 	 */
-	rt2500usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR0);
 	rt2x00_set_field16(&reg, TXRX_CSR0_ALGORITHM, crypto->cipher);
 	rt2x00_set_field16(&reg, TXRX_CSR0_IV_OFFSET, IEEE80211_HEADER);
 
@@ -421,7 +418,7 @@ static void rt2500usb_config_filter(struct rt2x00_dev *rt2x00dev,
 	 * and broadcast frames will always be accepted since
 	 * there is no filter for it at this time.
 	 */
-	rt2500usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR2);
 	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_CRC,
 			   !(filter_flags & FIF_FCSFAIL));
 	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_PHYSICAL,
@@ -453,7 +450,7 @@ static void rt2500usb_config_intf(struct rt2x00_dev *rt2x00dev,
 		 * Enable beacon config
 		 */
 		bcn_preload = PREAMBLE + GET_DURATION(IEEE80211_HEADER, 20);
-		rt2500usb_register_read(rt2x00dev, TXRX_CSR20, &reg);
+		reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR20);
 		rt2x00_set_field16(&reg, TXRX_CSR20_OFFSET, bcn_preload >> 6);
 		rt2x00_set_field16(&reg, TXRX_CSR20_BCN_EXPECT_WINDOW,
 				   2 * (conf->type != NL80211_IFTYPE_STATION));
@@ -462,11 +459,11 @@ static void rt2500usb_config_intf(struct rt2x00_dev *rt2x00dev,
 		/*
 		 * Enable synchronisation.
 		 */
-		rt2500usb_register_read(rt2x00dev, TXRX_CSR18, &reg);
+		reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR18);
 		rt2x00_set_field16(&reg, TXRX_CSR18_OFFSET, 0);
 		rt2500usb_register_write(rt2x00dev, TXRX_CSR18, reg);
 
-		rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
+		reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR19);
 		rt2x00_set_field16(&reg, TXRX_CSR19_TSF_SYNC, conf->sync);
 		rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
 	}
@@ -487,7 +484,7 @@ static void rt2500usb_config_erp(struct rt2x00_dev *rt2x00dev,
 	u16 reg;
 
 	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
-		rt2500usb_register_read(rt2x00dev, TXRX_CSR10, &reg);
+		reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR10);
 		rt2x00_set_field16(&reg, TXRX_CSR10_AUTORESPOND_PREAMBLE,
 				   !!erp->short_preamble);
 		rt2500usb_register_write(rt2x00dev, TXRX_CSR10, reg);
@@ -498,7 +495,7 @@ static void rt2500usb_config_erp(struct rt2x00_dev *rt2x00dev,
 					 erp->basic_rates);
 
 	if (changed & BSS_CHANGED_BEACON_INT) {
-		rt2500usb_register_read(rt2x00dev, TXRX_CSR18, &reg);
+		reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR18);
 		rt2x00_set_field16(&reg, TXRX_CSR18_INTERVAL,
 				   erp->beacon_int * 4);
 		rt2500usb_register_write(rt2x00dev, TXRX_CSR18, reg);
@@ -526,10 +523,10 @@ static void rt2500usb_config_ant(struct rt2x00_dev *rt2x00dev,
 	BUG_ON(ant->rx == ANTENNA_SW_DIVERSITY ||
 	       ant->tx == ANTENNA_SW_DIVERSITY);
 
-	rt2500usb_bbp_read(rt2x00dev, 2, &r2);
-	rt2500usb_bbp_read(rt2x00dev, 14, &r14);
-	rt2500usb_register_read(rt2x00dev, PHY_CSR5, &csr5);
-	rt2500usb_register_read(rt2x00dev, PHY_CSR6, &csr6);
+	r2 = rt2500usb_bbp_read(rt2x00dev, 2);
+	r14 = rt2500usb_bbp_read(rt2x00dev, 14);
+	csr5 = rt2500usb_register_read(rt2x00dev, PHY_CSR5);
+	csr6 = rt2500usb_register_read(rt2x00dev, PHY_CSR6);
 
 	/*
 	 * Configure the TX antenna.
@@ -629,7 +626,7 @@ static void rt2500usb_config_txpower(struct rt2x00_dev *rt2x00dev,
 {
 	u32 rf3;
 
-	rt2x00_rf_read(rt2x00dev, 3, &rf3);
+	rf3 = rt2x00_rf_read(rt2x00dev, 3);
 	rt2x00_set_field32(&rf3, RF3_TXPOWER, TXPOWER_TO_DEV(txpower));
 	rt2500usb_rf_write(rt2x00dev, 3, rf3);
 }
@@ -643,7 +640,7 @@ static void rt2500usb_config_ps(struct rt2x00_dev *rt2x00dev,
 	u16 reg;
 
 	if (state == STATE_SLEEP) {
-		rt2500usb_register_read(rt2x00dev, MAC_CSR18, &reg);
+		reg = rt2500usb_register_read(rt2x00dev, MAC_CSR18);
 		rt2x00_set_field16(&reg, MAC_CSR18_DELAY_AFTER_BEACON,
 				   rt2x00dev->beacon_int - 20);
 		rt2x00_set_field16(&reg, MAC_CSR18_BEACONS_BEFORE_WAKEUP,
@@ -656,7 +653,7 @@ static void rt2500usb_config_ps(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field16(&reg, MAC_CSR18_AUTO_WAKE, 1);
 		rt2500usb_register_write(rt2x00dev, MAC_CSR18, reg);
 	} else {
-		rt2500usb_register_read(rt2x00dev, MAC_CSR18, &reg);
+		reg = rt2500usb_register_read(rt2x00dev, MAC_CSR18);
 		rt2x00_set_field16(&reg, MAC_CSR18_AUTO_WAKE, 0);
 		rt2500usb_register_write(rt2x00dev, MAC_CSR18, reg);
 	}
@@ -690,13 +687,13 @@ static void rt2500usb_link_stats(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Update FCS error count from register.
 	 */
-	rt2500usb_register_read(rt2x00dev, STA_CSR0, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, STA_CSR0);
 	qual->rx_failed = rt2x00_get_field16(reg, STA_CSR0_FCS_ERROR);
 
 	/*
 	 * Update False CCA count from register.
 	 */
-	rt2500usb_register_read(rt2x00dev, STA_CSR3, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, STA_CSR3);
 	qual->false_cca = rt2x00_get_field16(reg, STA_CSR3_FALSE_CCA_ERROR);
 }
 
@@ -706,19 +703,19 @@ static void rt2500usb_reset_tuner(struct rt2x00_dev *rt2x00dev,
 	u16 eeprom;
 	u16 value;
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R24, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R24);
 	value = rt2x00_get_field16(eeprom, EEPROM_BBPTUNE_R24_LOW);
 	rt2500usb_bbp_write(rt2x00dev, 24, value);
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R25, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R25);
 	value = rt2x00_get_field16(eeprom, EEPROM_BBPTUNE_R25_LOW);
 	rt2500usb_bbp_write(rt2x00dev, 25, value);
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R61, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R61);
 	value = rt2x00_get_field16(eeprom, EEPROM_BBPTUNE_R61_LOW);
 	rt2500usb_bbp_write(rt2x00dev, 61, value);
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_VGC, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_VGC);
 	value = rt2x00_get_field16(eeprom, EEPROM_BBPTUNE_VGCUPPER);
 	rt2500usb_bbp_write(rt2x00dev, 17, value);
 
@@ -735,12 +732,12 @@ static void rt2500usb_start_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2500usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
+		reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR2);
 		rt2x00_set_field16(&reg, TXRX_CSR2_DISABLE_RX, 0);
 		rt2500usb_register_write(rt2x00dev, TXRX_CSR2, reg);
 		break;
 	case QID_BEACON:
-		rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
+		reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR19);
 		rt2x00_set_field16(&reg, TXRX_CSR19_TSF_COUNT, 1);
 		rt2x00_set_field16(&reg, TXRX_CSR19_TBCN, 1);
 		rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 1);
@@ -758,12 +755,12 @@ static void rt2500usb_stop_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2500usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
+		reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR2);
 		rt2x00_set_field16(&reg, TXRX_CSR2_DISABLE_RX, 1);
 		rt2500usb_register_write(rt2x00dev, TXRX_CSR2, reg);
 		break;
 	case QID_BEACON:
-		rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
+		reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR19);
 		rt2x00_set_field16(&reg, TXRX_CSR19_TSF_COUNT, 0);
 		rt2x00_set_field16(&reg, TXRX_CSR19_TBCN, 0);
 		rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 0);
@@ -786,54 +783,54 @@ static int rt2500usb_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00usb_vendor_request_sw(rt2x00dev, USB_SINGLE_WRITE, 0x0308,
 				    0x00f0, REGISTER_TIMEOUT);
 
-	rt2500usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR2);
 	rt2x00_set_field16(&reg, TXRX_CSR2_DISABLE_RX, 1);
 	rt2500usb_register_write(rt2x00dev, TXRX_CSR2, reg);
 
 	rt2500usb_register_write(rt2x00dev, MAC_CSR13, 0x1111);
 	rt2500usb_register_write(rt2x00dev, MAC_CSR14, 0x1e11);
 
-	rt2500usb_register_read(rt2x00dev, MAC_CSR1, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, MAC_CSR1);
 	rt2x00_set_field16(&reg, MAC_CSR1_SOFT_RESET, 1);
 	rt2x00_set_field16(&reg, MAC_CSR1_BBP_RESET, 1);
 	rt2x00_set_field16(&reg, MAC_CSR1_HOST_READY, 0);
 	rt2500usb_register_write(rt2x00dev, MAC_CSR1, reg);
 
-	rt2500usb_register_read(rt2x00dev, MAC_CSR1, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, MAC_CSR1);
 	rt2x00_set_field16(&reg, MAC_CSR1_SOFT_RESET, 0);
 	rt2x00_set_field16(&reg, MAC_CSR1_BBP_RESET, 0);
 	rt2x00_set_field16(&reg, MAC_CSR1_HOST_READY, 0);
 	rt2500usb_register_write(rt2x00dev, MAC_CSR1, reg);
 
-	rt2500usb_register_read(rt2x00dev, TXRX_CSR5, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR5);
 	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID0, 13);
 	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID0_VALID, 1);
 	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID1, 12);
 	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID1_VALID, 1);
 	rt2500usb_register_write(rt2x00dev, TXRX_CSR5, reg);
 
-	rt2500usb_register_read(rt2x00dev, TXRX_CSR6, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR6);
 	rt2x00_set_field16(&reg, TXRX_CSR6_BBP_ID0, 10);
 	rt2x00_set_field16(&reg, TXRX_CSR6_BBP_ID0_VALID, 1);
 	rt2x00_set_field16(&reg, TXRX_CSR6_BBP_ID1, 11);
 	rt2x00_set_field16(&reg, TXRX_CSR6_BBP_ID1_VALID, 1);
 	rt2500usb_register_write(rt2x00dev, TXRX_CSR6, reg);
 
-	rt2500usb_register_read(rt2x00dev, TXRX_CSR7, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR7);
 	rt2x00_set_field16(&reg, TXRX_CSR7_BBP_ID0, 7);
 	rt2x00_set_field16(&reg, TXRX_CSR7_BBP_ID0_VALID, 1);
 	rt2x00_set_field16(&reg, TXRX_CSR7_BBP_ID1, 6);
 	rt2x00_set_field16(&reg, TXRX_CSR7_BBP_ID1_VALID, 1);
 	rt2500usb_register_write(rt2x00dev, TXRX_CSR7, reg);
 
-	rt2500usb_register_read(rt2x00dev, TXRX_CSR8, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR8);
 	rt2x00_set_field16(&reg, TXRX_CSR8_BBP_ID0, 5);
 	rt2x00_set_field16(&reg, TXRX_CSR8_BBP_ID0_VALID, 1);
 	rt2x00_set_field16(&reg, TXRX_CSR8_BBP_ID1, 0);
 	rt2x00_set_field16(&reg, TXRX_CSR8_BBP_ID1_VALID, 0);
 	rt2500usb_register_write(rt2x00dev, TXRX_CSR8, reg);
 
-	rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR19);
 	rt2x00_set_field16(&reg, TXRX_CSR19_TSF_COUNT, 0);
 	rt2x00_set_field16(&reg, TXRX_CSR19_TSF_SYNC, 0);
 	rt2x00_set_field16(&reg, TXRX_CSR19_TBCN, 0);
@@ -846,14 +843,14 @@ static int rt2500usb_init_registers(struct rt2x00_dev *rt2x00dev)
 	if (rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_AWAKE))
 		return -EBUSY;
 
-	rt2500usb_register_read(rt2x00dev, MAC_CSR1, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, MAC_CSR1);
 	rt2x00_set_field16(&reg, MAC_CSR1_SOFT_RESET, 0);
 	rt2x00_set_field16(&reg, MAC_CSR1_BBP_RESET, 0);
 	rt2x00_set_field16(&reg, MAC_CSR1_HOST_READY, 1);
 	rt2500usb_register_write(rt2x00dev, MAC_CSR1, reg);
 
 	if (rt2x00_rev(rt2x00dev) >= RT2570_VERSION_C) {
-		rt2500usb_register_read(rt2x00dev, PHY_CSR2, &reg);
+		reg = rt2500usb_register_read(rt2x00dev, PHY_CSR2);
 		rt2x00_set_field16(&reg, PHY_CSR2_LNA, 0);
 	} else {
 		reg = 0;
@@ -867,26 +864,26 @@ static int rt2500usb_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2500usb_register_write(rt2x00dev, MAC_CSR15, 0x01ee);
 	rt2500usb_register_write(rt2x00dev, MAC_CSR16, 0x0000);
 
-	rt2500usb_register_read(rt2x00dev, MAC_CSR8, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, MAC_CSR8);
 	rt2x00_set_field16(&reg, MAC_CSR8_MAX_FRAME_UNIT,
 			   rt2x00dev->rx->data_size);
 	rt2500usb_register_write(rt2x00dev, MAC_CSR8, reg);
 
-	rt2500usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR0);
 	rt2x00_set_field16(&reg, TXRX_CSR0_ALGORITHM, CIPHER_NONE);
 	rt2x00_set_field16(&reg, TXRX_CSR0_IV_OFFSET, IEEE80211_HEADER);
 	rt2x00_set_field16(&reg, TXRX_CSR0_KEY_ID, 0);
 	rt2500usb_register_write(rt2x00dev, TXRX_CSR0, reg);
 
-	rt2500usb_register_read(rt2x00dev, MAC_CSR18, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, MAC_CSR18);
 	rt2x00_set_field16(&reg, MAC_CSR18_DELAY_AFTER_BEACON, 90);
 	rt2500usb_register_write(rt2x00dev, MAC_CSR18, reg);
 
-	rt2500usb_register_read(rt2x00dev, PHY_CSR4, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, PHY_CSR4);
 	rt2x00_set_field16(&reg, PHY_CSR4_LOW_RF_LE, 1);
 	rt2500usb_register_write(rt2x00dev, PHY_CSR4, reg);
 
-	rt2500usb_register_read(rt2x00dev, TXRX_CSR1, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR1);
 	rt2x00_set_field16(&reg, TXRX_CSR1_AUTO_SEQUENCE, 1);
 	rt2500usb_register_write(rt2x00dev, TXRX_CSR1, reg);
 
@@ -899,7 +896,7 @@ static int rt2500usb_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 	u8 value;
 
 	for (i = 0; i < REGISTER_USB_BUSY_COUNT; i++) {
-		rt2500usb_bbp_read(rt2x00dev, 0, &value);
+		value = rt2500usb_bbp_read(rt2x00dev, 0);
 		if ((value != 0xff) && (value != 0x00))
 			return 0;
 		udelay(REGISTER_BUSY_DELAY);
@@ -952,7 +949,7 @@ static int rt2500usb_init_bbp(struct rt2x00_dev *rt2x00dev)
 	rt2500usb_bbp_write(rt2x00dev, 75, 0xff);
 
 	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
-		rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i, &eeprom);
+		eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i);
 
 		if (eeprom != 0xffff && eeprom != 0x0000) {
 			reg_id = rt2x00_get_field16(eeprom, EEPROM_BBP_REG_ID);
@@ -1018,7 +1015,7 @@ static int rt2500usb_set_state(struct rt2x00_dev *rt2x00dev,
 	 * device has entered the correct state.
 	 */
 	for (i = 0; i < REGISTER_USB_BUSY_COUNT; i++) {
-		rt2500usb_register_read(rt2x00dev, MAC_CSR17, &reg2);
+		reg2 = rt2500usb_register_read(rt2x00dev, MAC_CSR17);
 		bbp_state = rt2x00_get_field16(reg2, MAC_CSR17_BBP_CURR_STATE);
 		rf_state = rt2x00_get_field16(reg2, MAC_CSR17_RF_CURR_STATE);
 		if (bbp_state == state && rf_state == state)
@@ -1077,7 +1074,7 @@ static void rt2500usb_write_tx_desc(struct queue_entry *entry,
 	/*
 	 * Start writing the descriptor words.
 	 */
-	rt2x00_desc_read(txd, 0, &word);
+	word = rt2x00_desc_read(txd, 0);
 	rt2x00_set_field32(&word, TXD_W0_RETRY_LIMIT, txdesc->retry_limit);
 	rt2x00_set_field32(&word, TXD_W0_MORE_FRAG,
 			   test_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags));
@@ -1095,14 +1092,14 @@ static void rt2500usb_write_tx_desc(struct queue_entry *entry,
 	rt2x00_set_field32(&word, TXD_W0_KEY_ID, txdesc->key_idx);
 	rt2x00_desc_write(txd, 0, word);
 
-	rt2x00_desc_read(txd, 1, &word);
+	word = rt2x00_desc_read(txd, 1);
 	rt2x00_set_field32(&word, TXD_W1_IV_OFFSET, txdesc->iv_offset);
 	rt2x00_set_field32(&word, TXD_W1_AIFS, entry->queue->aifs);
 	rt2x00_set_field32(&word, TXD_W1_CWMIN, entry->queue->cw_min);
 	rt2x00_set_field32(&word, TXD_W1_CWMAX, entry->queue->cw_max);
 	rt2x00_desc_write(txd, 1, word);
 
-	rt2x00_desc_read(txd, 2, &word);
+	word = rt2x00_desc_read(txd, 2);
 	rt2x00_set_field32(&word, TXD_W2_PLCP_SIGNAL, txdesc->u.plcp.signal);
 	rt2x00_set_field32(&word, TXD_W2_PLCP_SERVICE, txdesc->u.plcp.service);
 	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_LOW,
@@ -1143,7 +1140,7 @@ static void rt2500usb_write_beacon(struct queue_entry *entry,
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
-	rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, TXRX_CSR19);
 	rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 0);
 	rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
 
@@ -1250,8 +1247,8 @@ static void rt2500usb_fill_rxdone(struct queue_entry *entry,
 	/*
 	 * It is now safe to read the descriptor on all architectures.
 	 */
-	rt2x00_desc_read(rxd, 0, &word0);
-	rt2x00_desc_read(rxd, 1, &word1);
+	word0 = rt2x00_desc_read(rxd, 0);
+	word1 = rt2x00_desc_read(rxd, 1);
 
 	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
 		rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
@@ -1263,8 +1260,8 @@ static void rt2500usb_fill_rxdone(struct queue_entry *entry,
 		rxdesc->cipher_status = RX_CRYPTO_FAIL_KEY;
 
 	if (rxdesc->cipher != CIPHER_NONE) {
-		_rt2x00_desc_read(rxd, 2, &rxdesc->iv[0]);
-		_rt2x00_desc_read(rxd, 3, &rxdesc->iv[1]);
+		rxdesc->iv[0] = _rt2x00_desc_read(rxd, 2);
+		rxdesc->iv[1] = _rt2x00_desc_read(rxd, 3);
 		rxdesc->dev_flags |= RXDONE_CRYPTO_IV;
 
 		/* ICV is located at the end of frame */
@@ -1342,7 +1339,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	rt2x00lib_set_mac_address(rt2x00dev, mac);
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_NUM, 2);
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_TX_DEFAULT,
@@ -1358,7 +1355,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "Antenna: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_NIC_CARDBUS_ACCEL, 0);
 		rt2x00_set_field16(&word, EEPROM_NIC_DYN_BBP_TUNE, 0);
@@ -1367,7 +1364,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "NIC: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_CALIBRATE_OFFSET_RSSI,
 				   DEFAULT_RSSI_OFFSET);
@@ -1376,7 +1373,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 				  word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_THRESHOLD, 45);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE, word);
@@ -1387,10 +1384,10 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	 * Switch lower vgc bound to current BBP R17 value,
 	 * lower the value a bit for better quality.
 	 */
-	rt2500usb_bbp_read(rt2x00dev, 17, &bbp);
+	bbp = rt2500usb_bbp_read(rt2x00dev, 17);
 	bbp -= 6;
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_VGC, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_VGC);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_VGCUPPER, 0x40);
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_VGCLOWER, bbp);
@@ -1401,7 +1398,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE_VGC, word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R17, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R17);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R17_LOW, 0x48);
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R17_HIGH, 0x41);
@@ -1409,7 +1406,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "BBPtune r17: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R24, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R24);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R24_LOW, 0x40);
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R24_HIGH, 0x80);
@@ -1417,7 +1414,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "BBPtune r24: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R25, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R25);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R25_LOW, 0x40);
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R25_HIGH, 0x50);
@@ -1425,7 +1422,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "BBPtune r25: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R61, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R61);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R61_LOW, 0x60);
 		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R61_HIGH, 0x6d);
@@ -1445,13 +1442,13 @@ static int rt2500usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Read EEPROM word for configuration.
 	 */
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA);
 
 	/*
 	 * Identify RF chipset.
 	 */
 	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
-	rt2500usb_register_read(rt2x00dev, MAC_CSR0, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, MAC_CSR0);
 	rt2x00_set_chip(rt2x00dev, RT2570, value, reg);
 
 	if (((reg & 0xfff0) != 0) || ((reg & 0x0000000f) == 0)) {
@@ -1511,7 +1508,7 @@ static int rt2500usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Read the RSSI <-> dBm offset information.
 	 */
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET);
 	rt2x00dev->rssi_offset =
 	    rt2x00_get_field16(eeprom, EEPROM_CALIBRATE_OFFSET_RSSI);
 
@@ -1776,7 +1773,7 @@ static int rt2500usb_probe_hw(struct rt2x00_dev *rt2x00dev)
 	 * Enable rfkill polling by setting GPIO direction of the
 	 * rfkill switch GPIO pin correctly.
 	 */
-	rt2500usb_register_read(rt2x00dev, MAC_CSR19, &reg);
+	reg = rt2500usb_register_read(rt2x00dev, MAC_CSR19);
 	rt2x00_set_field16(&reg, MAC_CSR19_DIR0, 0);
 	rt2500usb_register_write(rt2x00dev, MAC_CSR19, reg);
 
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2800lib.c b/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
index d11c7b210e81..6e2e760d98b1 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
@@ -110,10 +110,10 @@ static void rt2800_bbp_write(struct rt2x00_dev *rt2x00dev,
 	mutex_unlock(&rt2x00dev->csr_mutex);
 }
 
-static void rt2800_bbp_read(struct rt2x00_dev *rt2x00dev,
-			    const unsigned int word, u8 *value)
+static u8 rt2800_bbp_read(struct rt2x00_dev *rt2x00dev, const unsigned int word)
 {
 	u32 reg;
+	u8 value;
 
 	mutex_lock(&rt2x00dev->csr_mutex);
 
@@ -137,9 +137,11 @@ static void rt2800_bbp_read(struct rt2x00_dev *rt2x00dev,
 		WAIT_FOR_BBP(rt2x00dev, &reg);
 	}
 
-	*value = rt2x00_get_field32(reg, BBP_CSR_CFG_VALUE);
+	value = rt2x00_get_field32(reg, BBP_CSR_CFG_VALUE);
 
 	mutex_unlock(&rt2x00dev->csr_mutex);
+
+	return value;
 }
 
 static void rt2800_rfcsr_write(struct rt2x00_dev *rt2x00dev,
@@ -203,10 +205,11 @@ static void rt2800_rfcsr_write_dccal(struct rt2x00_dev *rt2x00dev,
 	rt2800_rfcsr_write_bank(rt2x00dev, 7, reg, value);
 }
 
-static void rt2800_rfcsr_read(struct rt2x00_dev *rt2x00dev,
-			      const unsigned int word, u8 *value)
+static u8 rt2800_rfcsr_read(struct rt2x00_dev *rt2x00dev,
+			    const unsigned int word)
 {
 	u32 reg;
+	u8 value;
 
 	mutex_lock(&rt2x00dev->csr_mutex);
 
@@ -232,7 +235,7 @@ static void rt2800_rfcsr_read(struct rt2x00_dev *rt2x00dev,
 			WAIT_FOR_RFCSR_MT7620(rt2x00dev, &reg);
 		}
 
-		*value = rt2x00_get_field32(reg, RF_CSR_CFG_DATA_MT7620);
+		value = rt2x00_get_field32(reg, RF_CSR_CFG_DATA_MT7620);
 		break;
 
 	default:
@@ -247,17 +250,19 @@ static void rt2800_rfcsr_read(struct rt2x00_dev *rt2x00dev,
 			WAIT_FOR_RFCSR(rt2x00dev, &reg);
 		}
 
-		*value = rt2x00_get_field32(reg, RF_CSR_CFG_DATA);
+		value = rt2x00_get_field32(reg, RF_CSR_CFG_DATA);
 		break;
 	}
 
 	mutex_unlock(&rt2x00dev->csr_mutex);
+
+	return value;
 }
 
-static void rt2800_rfcsr_read_bank(struct rt2x00_dev *rt2x00dev, const u8 bank,
-				   const unsigned int reg, u8 *value)
+static u8 rt2800_rfcsr_read_bank(struct rt2x00_dev *rt2x00dev, const u8 bank,
+				 const unsigned int reg)
 {
-	rt2800_rfcsr_read(rt2x00dev, (reg | (bank << 6)), value);
+	return rt2800_rfcsr_read(rt2x00dev, (reg | (bank << 6)));
 }
 
 static void rt2800_rf_write(struct rt2x00_dev *rt2x00dev,
@@ -405,13 +410,13 @@ static void *rt2800_eeprom_addr(struct rt2x00_dev *rt2x00dev,
 	return rt2x00_eeprom_addr(rt2x00dev, index);
 }
 
-static void rt2800_eeprom_read(struct rt2x00_dev *rt2x00dev,
-			       const enum rt2800_eeprom_word word, u16 *data)
+static u16 rt2800_eeprom_read(struct rt2x00_dev *rt2x00dev,
+			      const enum rt2800_eeprom_word word)
 {
 	unsigned int index;
 
 	index = rt2800_eeprom_word_index(rt2x00dev, word);
-	rt2x00_eeprom_read(rt2x00dev, index, data);
+	return rt2x00_eeprom_read(rt2x00dev, index);
 }
 
 static void rt2800_eeprom_write(struct rt2x00_dev *rt2x00dev,
@@ -423,15 +428,14 @@ static void rt2800_eeprom_write(struct rt2x00_dev *rt2x00dev,
 	rt2x00_eeprom_write(rt2x00dev, index, data);
 }
 
-static void rt2800_eeprom_read_from_array(struct rt2x00_dev *rt2x00dev,
-					  const enum rt2800_eeprom_word array,
-					  unsigned int offset,
-					  u16 *data)
+static u16 rt2800_eeprom_read_from_array(struct rt2x00_dev *rt2x00dev,
+					 const enum rt2800_eeprom_word array,
+					 unsigned int offset)
 {
 	unsigned int index;
 
 	index = rt2800_eeprom_word_index(rt2x00dev, array);
-	rt2x00_eeprom_read(rt2x00dev, index + offset, data);
+	return rt2x00_eeprom_read(rt2x00dev, index + offset);
 }
 
 static int rt2800_enable_wlan_rt3290(struct rt2x00_dev *rt2x00dev)
@@ -439,7 +443,7 @@ static int rt2800_enable_wlan_rt3290(struct rt2x00_dev *rt2x00dev)
 	u32 reg;
 	int i, count;
 
-	rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
+	reg = rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL);
 	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);
@@ -454,7 +458,7 @@ static int rt2800_enable_wlan_rt3290(struct rt2x00_dev *rt2x00dev)
 		 * Check PLL_LD & XTAL_RDY.
 		 */
 		for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-			rt2800_register_read(rt2x00dev, CMB_CTRL, &reg);
+			reg = rt2800_register_read(rt2x00dev, CMB_CTRL);
 			if (rt2x00_get_field32(reg, PLL_LD) &&
 			    rt2x00_get_field32(reg, XTAL_RDY))
 				break;
@@ -477,7 +481,7 @@ static int rt2800_enable_wlan_rt3290(struct rt2x00_dev *rt2x00dev)
 			count = 0;
 		}
 
-		rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
+		reg = rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL);
 		rt2x00_set_field32(&reg, PCIE_APP0_CLK_REQ, 0);
 		rt2x00_set_field32(&reg, WLAN_CLK_EN, 1);
 		rt2x00_set_field32(&reg, WLAN_RESET, 1);
@@ -532,7 +536,7 @@ int rt2800_wait_csr_ready(struct rt2x00_dev *rt2x00dev)
 	u32 reg;
 
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2800_register_read(rt2x00dev, MAC_CSR0, &reg);
+		reg = rt2800_register_read(rt2x00dev, MAC_CSR0);
 		if (reg && reg != ~0)
 			return 0;
 		msleep(1);
@@ -553,7 +557,7 @@ int rt2800_wait_wpdma_ready(struct rt2x00_dev *rt2x00dev)
 	 * before timing out.
 	 */
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2800_register_read(rt2x00dev, WPDMA_GLO_CFG, &reg);
+		reg = rt2800_register_read(rt2x00dev, WPDMA_GLO_CFG);
 		if (!rt2x00_get_field32(reg, WPDMA_GLO_CFG_TX_DMA_BUSY) &&
 		    !rt2x00_get_field32(reg, WPDMA_GLO_CFG_RX_DMA_BUSY))
 			return 0;
@@ -570,7 +574,7 @@ void rt2800_disable_wpdma(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2800_register_read(rt2x00dev, WPDMA_GLO_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, WPDMA_GLO_CFG);
 	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_TX_DMA, 0);
 	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_TX_DMA_BUSY, 0);
 	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_RX_DMA, 0);
@@ -720,7 +724,7 @@ int rt2800_load_firmware(struct rt2x00_dev *rt2x00dev,
 		    rt2x00_rt(rt2x00dev, RT3572) ||
 		    rt2x00_rt(rt2x00dev, RT5390) ||
 		    rt2x00_rt(rt2x00dev, RT5392)) {
-			rt2800_register_read(rt2x00dev, AUX_CTRL, &reg);
+			reg = rt2800_register_read(rt2x00dev, AUX_CTRL);
 			rt2x00_set_field32(&reg, AUX_CTRL_FORCE_PCIE_CLK, 1);
 			rt2x00_set_field32(&reg, AUX_CTRL_WAKE_PCIE_EN, 1);
 			rt2800_register_write(rt2x00dev, AUX_CTRL, reg);
@@ -739,7 +743,7 @@ int rt2800_load_firmware(struct rt2x00_dev *rt2x00dev,
 	 * Wait for device to stabilize.
 	 */
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2800_register_read(rt2x00dev, PBF_SYS_CTRL, &reg);
+		reg = rt2800_register_read(rt2x00dev, PBF_SYS_CTRL);
 		if (rt2x00_get_field32(reg, PBF_SYS_CTRL_READY))
 			break;
 		msleep(1);
@@ -781,7 +785,7 @@ void rt2800_write_tx_data(struct queue_entry *entry,
 	/*
 	 * Initialize TX Info descriptor
 	 */
-	rt2x00_desc_read(txwi, 0, &word);
+	word = rt2x00_desc_read(txwi, 0);
 	rt2x00_set_field32(&word, TXWI_W0_FRAG,
 			   test_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags));
 	rt2x00_set_field32(&word, TXWI_W0_MIMO_PS,
@@ -803,7 +807,7 @@ void rt2800_write_tx_data(struct queue_entry *entry,
 	rt2x00_set_field32(&word, TXWI_W0_PHYMODE, txdesc->rate_mode);
 	rt2x00_desc_write(txwi, 0, word);
 
-	rt2x00_desc_read(txwi, 1, &word);
+	word = rt2x00_desc_read(txwi, 1);
 	rt2x00_set_field32(&word, TXWI_W1_ACK,
 			   test_bit(ENTRY_TXD_ACK, &txdesc->flags));
 	rt2x00_set_field32(&word, TXWI_W1_NSEQ,
@@ -843,16 +847,16 @@ static int rt2800_agc_to_rssi(struct rt2x00_dev *rt2x00dev, u32 rxwi_w2)
 	u8 offset2;
 
 	if (rt2x00dev->curr_band == NL80211_BAND_2GHZ) {
-		rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_BG, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_BG);
 		offset0 = rt2x00_get_field16(eeprom, EEPROM_RSSI_BG_OFFSET0);
 		offset1 = rt2x00_get_field16(eeprom, EEPROM_RSSI_BG_OFFSET1);
-		rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_BG2, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_BG2);
 		offset2 = rt2x00_get_field16(eeprom, EEPROM_RSSI_BG2_OFFSET2);
 	} else {
-		rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_A, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_A);
 		offset0 = rt2x00_get_field16(eeprom, EEPROM_RSSI_A_OFFSET0);
 		offset1 = rt2x00_get_field16(eeprom, EEPROM_RSSI_A_OFFSET1);
-		rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_A2, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_A2);
 		offset2 = rt2x00_get_field16(eeprom, EEPROM_RSSI_A2_OFFSET2);
 	}
 
@@ -881,12 +885,12 @@ void rt2800_process_rxwi(struct queue_entry *entry,
 	__le32 *rxwi = (__le32 *) entry->skb->data;
 	u32 word;
 
-	rt2x00_desc_read(rxwi, 0, &word);
+	word = rt2x00_desc_read(rxwi, 0);
 
 	rxdesc->cipher = rt2x00_get_field32(word, RXWI_W0_UDF);
 	rxdesc->size = rt2x00_get_field32(word, RXWI_W0_MPDU_TOTAL_BYTE_COUNT);
 
-	rt2x00_desc_read(rxwi, 1, &word);
+	word = rt2x00_desc_read(rxwi, 1);
 
 	if (rt2x00_get_field32(word, RXWI_W1_SHORT_GI))
 		rxdesc->enc_flags |= RX_ENC_FLAG_SHORT_GI;
@@ -907,7 +911,7 @@ void rt2800_process_rxwi(struct queue_entry *entry,
 	if (rxdesc->rate_mode == RATE_MODE_CCK)
 		rxdesc->signal &= ~0x8;
 
-	rt2x00_desc_read(rxwi, 2, &word);
+	word = rt2x00_desc_read(rxwi, 2);
 
 	/*
 	 * Convert descriptor AGC value to RSSI value.
@@ -968,7 +972,7 @@ void rt2800_txdone_entry(struct queue_entry *entry, u32 status, __le32 *txwi,
 	 * Obtain the status about this packet.
 	 */
 	txdesc.flags = 0;
-	rt2x00_desc_read(txwi, 0, &word);
+	word = rt2x00_desc_read(txwi, 0);
 
 	mcs = rt2x00_get_field32(word, TXWI_W0_MCS);
 	ampdu = rt2x00_get_field32(word, TXWI_W0_AMPDU);
@@ -1093,7 +1097,7 @@ static void rt2800_update_beacons_setup(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * H/W sends up to MAC_BSSID_DW1_BSS_BCN_NUM + 1 consecutive beacons.
 	 */
-	rt2800_register_read(rt2x00dev, MAC_BSSID_DW1, &bssid_dw1);
+	bssid_dw1 = rt2800_register_read(rt2x00dev, MAC_BSSID_DW1);
 	rt2x00_set_field32(&bssid_dw1, MAC_BSSID_DW1_BSS_BCN_NUM,
 			   bcn_num > 0 ? bcn_num - 1 : 0);
 	rt2800_register_write(rt2x00dev, MAC_BSSID_DW1, bssid_dw1);
@@ -1112,7 +1116,7 @@ void rt2800_write_beacon(struct queue_entry *entry, struct txentry_desc *txdesc)
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
-	rt2800_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, BCN_TIME_CFG);
 	orig_reg = reg;
 	rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 0);
 	rt2800_register_write(rt2x00dev, BCN_TIME_CFG, reg);
@@ -1202,7 +1206,7 @@ void rt2800_clear_beacon(struct queue_entry *entry)
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
-	rt2800_register_read(rt2x00dev, BCN_TIME_CFG, &orig_reg);
+	orig_reg = rt2800_register_read(rt2x00dev, BCN_TIME_CFG);
 	reg = orig_reg;
 	rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 0);
 	rt2800_register_write(rt2x00dev, BCN_TIME_CFG, reg);
@@ -1275,10 +1279,10 @@ int rt2800_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 	u32 reg;
 
 	if (rt2x00_rt(rt2x00dev, RT3290)) {
-		rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
+		reg = rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL);
 		return rt2x00_get_field32(reg, WLAN_GPIO_IN_BIT0);
 	} else {
-		rt2800_register_read(rt2x00dev, GPIO_CTRL, &reg);
+		reg = rt2800_register_read(rt2x00dev, GPIO_CTRL);
 		return rt2x00_get_field32(reg, GPIO_CTRL_VAL2);
 	}
 }
@@ -1303,7 +1307,7 @@ static void rt2800_brightness_set(struct led_classdev *led_cdev,
 
 	/* Check for SoC (SOC devices don't support MCU requests) */
 	if (rt2x00_is_soc(led->rt2x00dev)) {
-		rt2800_register_read(led->rt2x00dev, LED_CFG, &reg);
+		reg = rt2800_register_read(led->rt2x00dev, LED_CFG);
 
 		/* Set LED Polarity */
 		rt2x00_set_field32(&reg, LED_CFG_LED_POLAR, polarity);
@@ -1392,7 +1396,7 @@ static void rt2800_config_wcid_attr_bssidx(struct rt2x00_dev *rt2x00dev,
 	 * The BSS Idx numbers is split in a main value of 3 bits,
 	 * and a extended field for adding one additional bit to the value.
 	 */
-	rt2800_register_read(rt2x00dev, offset, &reg);
+	reg = rt2800_register_read(rt2x00dev, offset);
 	rt2x00_set_field32(&reg, MAC_WCID_ATTRIBUTE_BSS_IDX, (bssidx & 0x7));
 	rt2x00_set_field32(&reg, MAC_WCID_ATTRIBUTE_BSS_IDX_EXT,
 			   (bssidx & 0x8) >> 3);
@@ -1410,7 +1414,7 @@ static void rt2800_config_wcid_attr_cipher(struct rt2x00_dev *rt2x00dev,
 	offset = MAC_WCID_ATTR_ENTRY(key->hw_key_idx);
 
 	if (crypto->cmd == SET_KEY) {
-		rt2800_register_read(rt2x00dev, offset, &reg);
+		reg = rt2800_register_read(rt2x00dev, offset);
 		rt2x00_set_field32(&reg, MAC_WCID_ATTRIBUTE_KEYTAB,
 				   !!(key->flags & IEEE80211_KEY_FLAG_PAIRWISE));
 		/*
@@ -1426,7 +1430,7 @@ static void rt2800_config_wcid_attr_cipher(struct rt2x00_dev *rt2x00dev,
 		rt2800_register_write(rt2x00dev, offset, reg);
 	} else {
 		/* Delete the cipher without touching the bssidx */
-		rt2800_register_read(rt2x00dev, offset, &reg);
+		reg = rt2800_register_read(rt2x00dev, offset);
 		rt2x00_set_field32(&reg, MAC_WCID_ATTRIBUTE_KEYTAB, 0);
 		rt2x00_set_field32(&reg, MAC_WCID_ATTRIBUTE_CIPHER, 0);
 		rt2x00_set_field32(&reg, MAC_WCID_ATTRIBUTE_CIPHER_EXT, 0);
@@ -1482,7 +1486,7 @@ int rt2800_config_shared_key(struct rt2x00_dev *rt2x00dev,
 
 	offset = SHARED_KEY_MODE_ENTRY(key->hw_key_idx / 8);
 
-	rt2800_register_read(rt2x00dev, offset, &reg);
+	reg = rt2800_register_read(rt2x00dev, offset);
 	rt2x00_set_field32(&reg, field,
 			   (crypto->cmd == SET_KEY) * crypto->cipher);
 	rt2800_register_write(rt2x00dev, offset, reg);
@@ -1548,7 +1552,7 @@ static void rt2800_set_max_psdu_len(struct rt2x00_dev *rt2x00dev)
 
 	max_psdu = min(drv_data->max_psdu, i);
 
-	rt2800_register_read(rt2x00dev, MAX_LEN_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, MAX_LEN_CFG);
 	rt2x00_set_field32(&reg, MAX_LEN_CFG_MAX_PSDU, max_psdu);
 	rt2800_register_write(rt2x00dev, MAX_LEN_CFG, reg);
 }
@@ -1640,7 +1644,7 @@ void rt2800_config_filter(struct rt2x00_dev *rt2x00dev,
 	 * and broadcast frames will always be accepted since
 	 * there is no filter for it at this time.
 	 */
-	rt2800_register_read(rt2x00dev, RX_FILTER_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, RX_FILTER_CFG);
 	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_CRC_ERROR,
 			   !(filter_flags & FIF_FCSFAIL));
 	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_PHY_ERROR,
@@ -1684,7 +1688,7 @@ void rt2800_config_intf(struct rt2x00_dev *rt2x00dev, struct rt2x00_intf *intf,
 		/*
 		 * Enable synchronisation.
 		 */
-		rt2800_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+		reg = rt2800_register_read(rt2x00dev, BCN_TIME_CFG);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_SYNC, conf->sync);
 		rt2800_register_write(rt2x00dev, BCN_TIME_CFG, reg);
 
@@ -1692,14 +1696,14 @@ void rt2800_config_intf(struct rt2x00_dev *rt2x00dev, struct rt2x00_intf *intf,
 			/*
 			 * Tune beacon queue transmit parameters for AP mode
 			 */
-			rt2800_register_read(rt2x00dev, TBTT_SYNC_CFG, &reg);
+			reg = rt2800_register_read(rt2x00dev, TBTT_SYNC_CFG);
 			rt2x00_set_field32(&reg, TBTT_SYNC_CFG_BCN_CWMIN, 0);
 			rt2x00_set_field32(&reg, TBTT_SYNC_CFG_BCN_AIFSN, 1);
 			rt2x00_set_field32(&reg, TBTT_SYNC_CFG_BCN_EXP_WIN, 32);
 			rt2x00_set_field32(&reg, TBTT_SYNC_CFG_TBTT_ADJUST, 0);
 			rt2800_register_write(rt2x00dev, TBTT_SYNC_CFG, reg);
 		} else {
-			rt2800_register_read(rt2x00dev, TBTT_SYNC_CFG, &reg);
+			reg = rt2800_register_read(rt2x00dev, TBTT_SYNC_CFG);
 			rt2x00_set_field32(&reg, TBTT_SYNC_CFG_BCN_CWMIN, 4);
 			rt2x00_set_field32(&reg, TBTT_SYNC_CFG_BCN_AIFSN, 2);
 			rt2x00_set_field32(&reg, TBTT_SYNC_CFG_BCN_EXP_WIN, 32);
@@ -1818,22 +1822,22 @@ static void rt2800_config_ht_opmode(struct rt2x00_dev *rt2x00dev,
 		gf20_mode = gf40_mode = 1;
 
 	/* Update HT protection config */
-	rt2800_register_read(rt2x00dev, MM20_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, MM20_PROT_CFG);
 	rt2x00_set_field32(&reg, MM20_PROT_CFG_PROTECT_RATE, mm20_rate);
 	rt2x00_set_field32(&reg, MM20_PROT_CFG_PROTECT_CTRL, mm20_mode);
 	rt2800_register_write(rt2x00dev, MM20_PROT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, MM40_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, MM40_PROT_CFG);
 	rt2x00_set_field32(&reg, MM40_PROT_CFG_PROTECT_RATE, mm40_rate);
 	rt2x00_set_field32(&reg, MM40_PROT_CFG_PROTECT_CTRL, mm40_mode);
 	rt2800_register_write(rt2x00dev, MM40_PROT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, GF20_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, GF20_PROT_CFG);
 	rt2x00_set_field32(&reg, GF20_PROT_CFG_PROTECT_RATE, gf20_rate);
 	rt2x00_set_field32(&reg, GF20_PROT_CFG_PROTECT_CTRL, gf20_mode);
 	rt2800_register_write(rt2x00dev, GF20_PROT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, GF40_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, GF40_PROT_CFG);
 	rt2x00_set_field32(&reg, GF40_PROT_CFG_PROTECT_RATE, gf40_rate);
 	rt2x00_set_field32(&reg, GF40_PROT_CFG_PROTECT_CTRL, gf40_mode);
 	rt2800_register_write(rt2x00dev, GF40_PROT_CFG, reg);
@@ -1845,14 +1849,14 @@ void rt2800_config_erp(struct rt2x00_dev *rt2x00dev, struct rt2x00lib_erp *erp,
 	u32 reg;
 
 	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
-		rt2800_register_read(rt2x00dev, AUTO_RSP_CFG, &reg);
+		reg = rt2800_register_read(rt2x00dev, AUTO_RSP_CFG);
 		rt2x00_set_field32(&reg, AUTO_RSP_CFG_AR_PREAMBLE,
 				   !!erp->short_preamble);
 		rt2800_register_write(rt2x00dev, AUTO_RSP_CFG, reg);
 	}
 
 	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
-		rt2800_register_read(rt2x00dev, OFDM_PROT_CFG, &reg);
+		reg = rt2800_register_read(rt2x00dev, OFDM_PROT_CFG);
 		rt2x00_set_field32(&reg, OFDM_PROT_CFG_PROTECT_CTRL,
 				   erp->cts_protection ? 2 : 0);
 		rt2800_register_write(rt2x00dev, OFDM_PROT_CFG, reg);
@@ -1865,18 +1869,18 @@ void rt2800_config_erp(struct rt2x00_dev *rt2x00dev, struct rt2x00lib_erp *erp,
 	}
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
-		rt2800_register_read(rt2x00dev, BKOFF_SLOT_CFG, &reg);
+		reg = rt2800_register_read(rt2x00dev, BKOFF_SLOT_CFG);
 		rt2x00_set_field32(&reg, BKOFF_SLOT_CFG_SLOT_TIME,
 				   erp->slot_time);
 		rt2800_register_write(rt2x00dev, BKOFF_SLOT_CFG, reg);
 
-		rt2800_register_read(rt2x00dev, XIFS_TIME_CFG, &reg);
+		reg = rt2800_register_read(rt2x00dev, XIFS_TIME_CFG);
 		rt2x00_set_field32(&reg, XIFS_TIME_CFG_EIFS, erp->eifs);
 		rt2800_register_write(rt2x00dev, XIFS_TIME_CFG, reg);
 	}
 
 	if (changed & BSS_CHANGED_BEACON_INT) {
-		rt2800_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+		reg = rt2800_register_read(rt2x00dev, BCN_TIME_CFG);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_INTERVAL,
 				   erp->beacon_int * 16);
 		rt2800_register_write(rt2x00dev, BCN_TIME_CFG, reg);
@@ -1893,7 +1897,7 @@ static void rt2800_config_3572bt_ant(struct rt2x00_dev *rt2x00dev)
 	u16 eeprom;
 	u8 led_ctrl, led_g_mode, led_r_mode;
 
-	rt2800_register_read(rt2x00dev, GPIO_SWITCH, &reg);
+	reg = rt2800_register_read(rt2x00dev, GPIO_SWITCH);
 	if (rt2x00dev->curr_band == NL80211_BAND_5GHZ) {
 		rt2x00_set_field32(&reg, GPIO_SWITCH_0, 1);
 		rt2x00_set_field32(&reg, GPIO_SWITCH_1, 1);
@@ -1903,12 +1907,12 @@ static void rt2800_config_3572bt_ant(struct rt2x00_dev *rt2x00dev)
 	}
 	rt2800_register_write(rt2x00dev, GPIO_SWITCH, reg);
 
-	rt2800_register_read(rt2x00dev, LED_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, LED_CFG);
 	led_g_mode = rt2x00_get_field32(reg, LED_CFG_LED_POLAR) ? 3 : 0;
 	led_r_mode = rt2x00_get_field32(reg, LED_CFG_LED_POLAR) ? 0 : 3;
 	if (led_g_mode != rt2x00_get_field32(reg, LED_CFG_G_LED_MODE) ||
 	    led_r_mode != rt2x00_get_field32(reg, LED_CFG_R_LED_MODE)) {
-		rt2800_eeprom_read(rt2x00dev, EEPROM_FREQ, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_FREQ);
 		led_ctrl = rt2x00_get_field16(eeprom, EEPROM_FREQ_LED_MODE);
 		if (led_ctrl == 0 || led_ctrl > 0x40) {
 			rt2x00_set_field32(&reg, LED_CFG_G_LED_MODE, led_g_mode);
@@ -1929,14 +1933,14 @@ static void rt2800_set_ant_diversity(struct rt2x00_dev *rt2x00dev,
 	u8 gpio_bit3 = (ant == ANTENNA_A) ? 0 : 1;
 
 	if (rt2x00_is_pci(rt2x00dev)) {
-		rt2800_register_read(rt2x00dev, E2PROM_CSR, &reg);
+		reg = rt2800_register_read(rt2x00dev, E2PROM_CSR);
 		rt2x00_set_field32(&reg, E2PROM_CSR_DATA_CLOCK, eesk_pin);
 		rt2800_register_write(rt2x00dev, E2PROM_CSR, reg);
 	} else if (rt2x00_is_usb(rt2x00dev))
 		rt2800_mcu_request(rt2x00dev, MCU_ANT_SELECT, 0xff,
 				   eesk_pin, 0);
 
-	rt2800_register_read(rt2x00dev, GPIO_CTRL, &reg);
+	reg = rt2800_register_read(rt2x00dev, GPIO_CTRL);
 	rt2x00_set_field32(&reg, GPIO_CTRL_DIR3, 0);
 	rt2x00_set_field32(&reg, GPIO_CTRL_VAL3, gpio_bit3);
 	rt2800_register_write(rt2x00dev, GPIO_CTRL, reg);
@@ -1948,8 +1952,8 @@ void rt2800_config_ant(struct rt2x00_dev *rt2x00dev, struct antenna_setup *ant)
 	u8 r3;
 	u16 eeprom;
 
-	rt2800_bbp_read(rt2x00dev, 1, &r1);
-	rt2800_bbp_read(rt2x00dev, 3, &r3);
+	r1 = rt2800_bbp_read(rt2x00dev, 1);
+	r3 = rt2800_bbp_read(rt2x00dev, 3);
 
 	if (rt2x00_rt(rt2x00dev, RT3572) &&
 	    rt2x00_has_cap_bt_coexist(rt2x00dev))
@@ -1983,8 +1987,8 @@ void rt2800_config_ant(struct rt2x00_dev *rt2x00dev, struct antenna_setup *ant)
 		    rt2x00_rt(rt2x00dev, RT3090) ||
 		    rt2x00_rt(rt2x00dev, RT3352) ||
 		    rt2x00_rt(rt2x00dev, RT3390)) {
-			rt2800_eeprom_read(rt2x00dev,
-					   EEPROM_NIC_CONF1, &eeprom);
+			eeprom = rt2800_eeprom_read(rt2x00dev,
+						    EEPROM_NIC_CONF1);
 			if (rt2x00_get_field16(eeprom,
 						EEPROM_NIC_CONF1_ANT_DIVERSITY))
 				rt2800_set_ant_diversity(rt2x00dev,
@@ -2027,28 +2031,28 @@ static void rt2800_config_lna_gain(struct rt2x00_dev *rt2x00dev,
 	short lna_gain;
 
 	if (libconf->rf.channel <= 14) {
-		rt2800_eeprom_read(rt2x00dev, EEPROM_LNA, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_LNA);
 		lna_gain = rt2x00_get_field16(eeprom, EEPROM_LNA_BG);
 	} else if (libconf->rf.channel <= 64) {
-		rt2800_eeprom_read(rt2x00dev, EEPROM_LNA, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_LNA);
 		lna_gain = rt2x00_get_field16(eeprom, EEPROM_LNA_A0);
 	} else if (libconf->rf.channel <= 128) {
 		if (rt2x00_rt(rt2x00dev, RT3593)) {
-			rt2800_eeprom_read(rt2x00dev, EEPROM_EXT_LNA2, &eeprom);
+			eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_EXT_LNA2);
 			lna_gain = rt2x00_get_field16(eeprom,
 						      EEPROM_EXT_LNA2_A1);
 		} else {
-			rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_BG2, &eeprom);
+			eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_BG2);
 			lna_gain = rt2x00_get_field16(eeprom,
 						      EEPROM_RSSI_BG2_LNA_A1);
 		}
 	} else {
 		if (rt2x00_rt(rt2x00dev, RT3593)) {
-			rt2800_eeprom_read(rt2x00dev, EEPROM_EXT_LNA2, &eeprom);
+			eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_EXT_LNA2);
 			lna_gain = rt2x00_get_field16(eeprom,
 						      EEPROM_EXT_LNA2_A2);
 		} else {
-			rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_A2, &eeprom);
+			eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_A2);
 			lna_gain = rt2x00_get_field16(eeprom,
 						      EEPROM_RSSI_A2_LNA_A2);
 		}
@@ -2072,7 +2076,7 @@ static void rt2800_freq_cal_mode1(struct rt2x00_dev *rt2x00dev)
 	freq_offset = rt2x00_get_field8(rt2x00dev->freq_offset, RFCSR17_CODE);
 	freq_offset = min_t(u8, freq_offset, FREQ_OFFSET_BOUND);
 
-	rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 17);
 	prev_rfcsr = rfcsr;
 
 	rt2x00_set_field8(&rfcsr, RFCSR17_CODE, freq_offset);
@@ -2174,23 +2178,23 @@ static void rt2800_config_channel_rf3xxx(struct rt2x00_dev *rt2x00dev,
 
 	rt2800_rfcsr_write(rt2x00dev, 2, rf->rf1);
 
-	rt2800_rfcsr_read(rt2x00dev, 3, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 3);
 	rt2x00_set_field8(&rfcsr, RFCSR3_K, rf->rf3);
 	rt2800_rfcsr_write(rt2x00dev, 3, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 6, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 6);
 	rt2x00_set_field8(&rfcsr, RFCSR6_R1, rf->rf2);
 	rt2800_rfcsr_write(rt2x00dev, 6, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 12, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 12);
 	rt2x00_set_field8(&rfcsr, RFCSR12_TX_POWER, info->default_power1);
 	rt2800_rfcsr_write(rt2x00dev, 12, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 13, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 13);
 	rt2x00_set_field8(&rfcsr, RFCSR13_TX_POWER, info->default_power2);
 	rt2800_rfcsr_write(rt2x00dev, 13, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 1);
 	rt2x00_set_field8(&rfcsr, RFCSR1_RX0_PD, 0);
 	rt2x00_set_field8(&rfcsr, RFCSR1_RX1_PD,
 			  rt2x00dev->default_ant.rx_chain_num <= 1);
@@ -2203,7 +2207,7 @@ static void rt2800_config_channel_rf3xxx(struct rt2x00_dev *rt2x00dev,
 			  rt2x00dev->default_ant.tx_chain_num <= 2);
 	rt2800_rfcsr_write(rt2x00dev, 1, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 23, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 23);
 	rt2x00_set_field8(&rfcsr, RFCSR23_FREQ_OFFSET, rt2x00dev->freq_offset);
 	rt2800_rfcsr_write(rt2x00dev, 23, rfcsr);
 
@@ -2220,19 +2224,19 @@ static void rt2800_config_channel_rf3xxx(struct rt2x00_dev *rt2x00dev,
 		}
 	}
 
-	rt2800_rfcsr_read(rt2x00dev, 24, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 24);
 	rt2x00_set_field8(&rfcsr, RFCSR24_TX_CALIB, calib_tx);
 	rt2800_rfcsr_write(rt2x00dev, 24, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 31, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 31);
 	rt2x00_set_field8(&rfcsr, RFCSR31_RX_CALIB, calib_rx);
 	rt2800_rfcsr_write(rt2x00dev, 31, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 7, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 7);
 	rt2x00_set_field8(&rfcsr, RFCSR7_RF_TUNING, 1);
 	rt2800_rfcsr_write(rt2x00dev, 7, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 30, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 30);
 	rt2x00_set_field8(&rfcsr, RFCSR30_RF_CALIBRATION, 1);
 	rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
 
@@ -2262,7 +2266,7 @@ static void rt2800_config_channel_rf3052(struct rt2x00_dev *rt2x00dev,
 	rt2800_rfcsr_write(rt2x00dev, 2, rf->rf1);
 	rt2800_rfcsr_write(rt2x00dev, 3, rf->rf3);
 
-	rt2800_rfcsr_read(rt2x00dev, 6, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 6);
 	rt2x00_set_field8(&rfcsr, RFCSR6_R1, rf->rf2);
 	if (rf->channel <= 14)
 		rt2x00_set_field8(&rfcsr, RFCSR6_TXDIV, 2);
@@ -2270,14 +2274,14 @@ static void rt2800_config_channel_rf3052(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field8(&rfcsr, RFCSR6_TXDIV, 1);
 	rt2800_rfcsr_write(rt2x00dev, 6, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 5, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 5);
 	if (rf->channel <= 14)
 		rt2x00_set_field8(&rfcsr, RFCSR5_R1, 1);
 	else
 		rt2x00_set_field8(&rfcsr, RFCSR5_R1, 2);
 	rt2800_rfcsr_write(rt2x00dev, 5, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 12, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 12);
 	if (rf->channel <= 14) {
 		rt2x00_set_field8(&rfcsr, RFCSR12_DR0, 3);
 		rt2x00_set_field8(&rfcsr, RFCSR12_TX_POWER,
@@ -2290,7 +2294,7 @@ static void rt2800_config_channel_rf3052(struct rt2x00_dev *rt2x00dev,
 	}
 	rt2800_rfcsr_write(rt2x00dev, 12, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 13, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 13);
 	if (rf->channel <= 14) {
 		rt2x00_set_field8(&rfcsr, RFCSR13_DR0, 3);
 		rt2x00_set_field8(&rfcsr, RFCSR13_TX_POWER,
@@ -2303,7 +2307,7 @@ static void rt2800_config_channel_rf3052(struct rt2x00_dev *rt2x00dev,
 	}
 	rt2800_rfcsr_write(rt2x00dev, 13, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 1);
 	rt2x00_set_field8(&rfcsr, RFCSR1_RX0_PD, 0);
 	rt2x00_set_field8(&rfcsr, RFCSR1_TX0_PD, 0);
 	rt2x00_set_field8(&rfcsr, RFCSR1_RX1_PD, 0);
@@ -2336,7 +2340,7 @@ static void rt2800_config_channel_rf3052(struct rt2x00_dev *rt2x00dev,
 	}
 	rt2800_rfcsr_write(rt2x00dev, 1, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 23, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 23);
 	rt2x00_set_field8(&rfcsr, RFCSR23_FREQ_OFFSET, rt2x00dev->freq_offset);
 	rt2800_rfcsr_write(rt2x00dev, 23, rfcsr);
 
@@ -2366,7 +2370,7 @@ static void rt2800_config_channel_rf3052(struct rt2x00_dev *rt2x00dev,
 		rt2800_rfcsr_write(rt2x00dev, 27, 0x00);
 		rt2800_rfcsr_write(rt2x00dev, 29, 0x9b);
 	} else {
-		rt2800_rfcsr_read(rt2x00dev, 7, &rfcsr);
+		rfcsr = rt2800_rfcsr_read(rt2x00dev, 7);
 		rt2x00_set_field8(&rfcsr, RFCSR7_BIT2, 1);
 		rt2x00_set_field8(&rfcsr, RFCSR7_BIT3, 0);
 		rt2x00_set_field8(&rfcsr, RFCSR7_BIT4, 1);
@@ -2399,7 +2403,7 @@ static void rt2800_config_channel_rf3052(struct rt2x00_dev *rt2x00dev,
 		rt2800_rfcsr_write(rt2x00dev, 29, 0x9f);
 	}
 
-	rt2800_register_read(rt2x00dev, GPIO_CTRL, &reg);
+	reg = rt2800_register_read(rt2x00dev, GPIO_CTRL);
 	rt2x00_set_field32(&reg, GPIO_CTRL_DIR7, 0);
 	if (rf->channel <= 14)
 		rt2x00_set_field32(&reg, GPIO_CTRL_VAL7, 1);
@@ -2407,7 +2411,7 @@ static void rt2800_config_channel_rf3052(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, GPIO_CTRL_VAL7, 0);
 	rt2800_register_write(rt2x00dev, GPIO_CTRL, reg);
 
-	rt2800_rfcsr_read(rt2x00dev, 7, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 7);
 	rt2x00_set_field8(&rfcsr, RFCSR7_RF_TUNING, 1);
 	rt2800_rfcsr_write(rt2x00dev, 7, rfcsr);
 }
@@ -2425,12 +2429,12 @@ static void rt2800_config_channel_rf3053(struct rt2x00_dev *rt2x00dev,
 	const bool txbf_enabled = false; /* TODO */
 
 	/* TODO: use TX{0,1,2}FinePowerControl values from EEPROM */
-	rt2800_bbp_read(rt2x00dev, 109, &bbp);
+	bbp = rt2800_bbp_read(rt2x00dev, 109);
 	rt2x00_set_field8(&bbp, BBP109_TX0_POWER, 0);
 	rt2x00_set_field8(&bbp, BBP109_TX1_POWER, 0);
 	rt2800_bbp_write(rt2x00dev, 109, bbp);
 
-	rt2800_bbp_read(rt2x00dev, 110, &bbp);
+	bbp = rt2800_bbp_read(rt2x00dev, 110);
 	rt2x00_set_field8(&bbp, BBP110_TX2_POWER, 0);
 	rt2800_bbp_write(rt2x00dev, 110, bbp);
 
@@ -2450,11 +2454,11 @@ static void rt2800_config_channel_rf3053(struct rt2x00_dev *rt2x00dev,
 	rt2800_rfcsr_write(rt2x00dev, 8, rf->rf1);
 	rt2800_rfcsr_write(rt2x00dev, 9, rf->rf3 & 0xf);
 
-	rt2800_rfcsr_read(rt2x00dev, 11, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 11);
 	rt2x00_set_field8(&rfcsr, RFCSR11_R, (rf->rf2 & 0x3));
 	rt2800_rfcsr_write(rt2x00dev, 11, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 11, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 11);
 	rt2x00_set_field8(&rfcsr, RFCSR11_PLL_IDOH, 1);
 	if (rf->channel <= 14)
 		rt2x00_set_field8(&rfcsr, RFCSR11_PLL_MOD, 1);
@@ -2462,7 +2466,7 @@ static void rt2800_config_channel_rf3053(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field8(&rfcsr, RFCSR11_PLL_MOD, 2);
 	rt2800_rfcsr_write(rt2x00dev, 11, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 53, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 53);
 	if (rf->channel <= 14) {
 		rfcsr = 0;
 		rt2x00_set_field8(&rfcsr, RFCSR53_TX_POWER,
@@ -2477,7 +2481,7 @@ static void rt2800_config_channel_rf3053(struct rt2x00_dev *rt2x00dev,
 	}
 	rt2800_rfcsr_write(rt2x00dev, 53, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 55, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 55);
 	if (rf->channel <= 14) {
 		rfcsr = 0;
 		rt2x00_set_field8(&rfcsr, RFCSR55_TX_POWER,
@@ -2492,7 +2496,7 @@ static void rt2800_config_channel_rf3053(struct rt2x00_dev *rt2x00dev,
 	}
 	rt2800_rfcsr_write(rt2x00dev, 55, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 54, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 54);
 	if (rf->channel <= 14) {
 		rfcsr = 0;
 		rt2x00_set_field8(&rfcsr, RFCSR54_TX_POWER,
@@ -2507,7 +2511,7 @@ static void rt2800_config_channel_rf3053(struct rt2x00_dev *rt2x00dev,
 	}
 	rt2800_rfcsr_write(rt2x00dev, 54, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 1);
 	rt2x00_set_field8(&rfcsr, RFCSR1_RX0_PD, 0);
 	rt2x00_set_field8(&rfcsr, RFCSR1_TX0_PD, 0);
 	rt2x00_set_field8(&rfcsr, RFCSR1_RX1_PD, 0);
@@ -2559,7 +2563,7 @@ static void rt2800_config_channel_rf3053(struct rt2x00_dev *rt2x00dev,
 	/* NOTE: the reference driver does not writes the new value
 	 * back to RFCSR 32
 	 */
-	rt2800_rfcsr_read(rt2x00dev, 32, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 32);
 	rt2x00_set_field8(&rfcsr, RFCSR32_TX_AGC_FC, txrx_agc_fc);
 
 	if (rf->channel <= 14)
@@ -2568,34 +2572,34 @@ static void rt2800_config_channel_rf3053(struct rt2x00_dev *rt2x00dev,
 		rfcsr = 0x80;
 	rt2800_rfcsr_write(rt2x00dev, 31, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 30, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 30);
 	rt2x00_set_field8(&rfcsr, RFCSR30_TX_H20M, txrx_h20m);
 	rt2x00_set_field8(&rfcsr, RFCSR30_RX_H20M, txrx_h20m);
 	rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
 
 	/* Band selection */
-	rt2800_rfcsr_read(rt2x00dev, 36, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 36);
 	if (rf->channel <= 14)
 		rt2x00_set_field8(&rfcsr, RFCSR36_RF_BS, 1);
 	else
 		rt2x00_set_field8(&rfcsr, RFCSR36_RF_BS, 0);
 	rt2800_rfcsr_write(rt2x00dev, 36, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 34, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 34);
 	if (rf->channel <= 14)
 		rfcsr = 0x3c;
 	else
 		rfcsr = 0x20;
 	rt2800_rfcsr_write(rt2x00dev, 34, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 12, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 12);
 	if (rf->channel <= 14)
 		rfcsr = 0x1a;
 	else
 		rfcsr = 0x12;
 	rt2800_rfcsr_write(rt2x00dev, 12, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 6, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 6);
 	if (rf->channel >= 1 && rf->channel <= 14)
 		rt2x00_set_field8(&rfcsr, RFCSR6_VCO_IC, 1);
 	else if (rf->channel >= 36 && rf->channel <= 64)
@@ -2606,7 +2610,7 @@ static void rt2800_config_channel_rf3053(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field8(&rfcsr, RFCSR6_VCO_IC, 1);
 	rt2800_rfcsr_write(rt2x00dev, 6, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 30, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 30);
 	rt2x00_set_field8(&rfcsr, RFCSR30_RX_VCM, 2);
 	rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
 
@@ -2620,11 +2624,11 @@ static void rt2800_config_channel_rf3053(struct rt2x00_dev *rt2x00dev,
 		rt2800_rfcsr_write(rt2x00dev, 13, 0x23);
 	}
 
-	rt2800_rfcsr_read(rt2x00dev, 51, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 51);
 	rt2x00_set_field8(&rfcsr, RFCSR51_BITS01, 1);
 	rt2800_rfcsr_write(rt2x00dev, 51, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 51, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 51);
 	if (rf->channel <= 14) {
 		rt2x00_set_field8(&rfcsr, RFCSR51_BITS24, 5);
 		rt2x00_set_field8(&rfcsr, RFCSR51_BITS57, 3);
@@ -2634,7 +2638,7 @@ static void rt2800_config_channel_rf3053(struct rt2x00_dev *rt2x00dev,
 	}
 	rt2800_rfcsr_write(rt2x00dev, 51, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 49, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 49);
 	if (rf->channel <= 14)
 		rt2x00_set_field8(&rfcsr, RFCSR49_TX_LO1_IC, 3);
 	else
@@ -2645,11 +2649,11 @@ static void rt2800_config_channel_rf3053(struct rt2x00_dev *rt2x00dev,
 
 	rt2800_rfcsr_write(rt2x00dev, 49, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 50, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 50);
 	rt2x00_set_field8(&rfcsr, RFCSR50_TX_LO1_EN, 0);
 	rt2800_rfcsr_write(rt2x00dev, 50, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 57, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 57);
 	if (rf->channel <= 14)
 		rt2x00_set_field8(&rfcsr, RFCSR57_DRV_CC, 0x1b);
 	else
@@ -2665,7 +2669,7 @@ static void rt2800_config_channel_rf3053(struct rt2x00_dev *rt2x00dev,
 	}
 
 	/* Initiate VCO calibration */
-	rt2800_rfcsr_read(rt2x00dev, 3, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 3);
 	if (rf->channel <= 14) {
 		rt2x00_set_field8(&rfcsr, RFCSR3_VCOCAL_EN, 1);
 	} else {
@@ -2721,11 +2725,11 @@ static void rt2800_config_channel_rf3290(struct rt2x00_dev *rt2x00dev,
 
 	rt2800_rfcsr_write(rt2x00dev, 8, rf->rf1);
 	rt2800_rfcsr_write(rt2x00dev, 9, rf->rf3);
-	rt2800_rfcsr_read(rt2x00dev, 11, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 11);
 	rt2x00_set_field8(&rfcsr, RFCSR11_R, rf->rf2);
 	rt2800_rfcsr_write(rt2x00dev, 11, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 49, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 49);
 	if (info->default_power1 > POWER_BOUND)
 		rt2x00_set_field8(&rfcsr, RFCSR49_TX, POWER_BOUND);
 	else
@@ -2775,7 +2779,7 @@ static void rt2800_config_channel_rf3322(struct rt2x00_dev *rt2x00dev,
 
 	rt2800_freq_cal_mode1(rt2x00dev);
 
-	rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 1);
 	rt2x00_set_field8(&rfcsr, RFCSR1_RX0_PD, 1);
 	rt2x00_set_field8(&rfcsr, RFCSR1_TX0_PD, 1);
 
@@ -2806,11 +2810,11 @@ static void rt2800_config_channel_rf53xx(struct rt2x00_dev *rt2x00dev,
 
 	rt2800_rfcsr_write(rt2x00dev, 8, rf->rf1);
 	rt2800_rfcsr_write(rt2x00dev, 9, rf->rf3);
-	rt2800_rfcsr_read(rt2x00dev, 11, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 11);
 	rt2x00_set_field8(&rfcsr, RFCSR11_R, rf->rf2);
 	rt2800_rfcsr_write(rt2x00dev, 11, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 49, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 49);
 	if (info->default_power1 > POWER_BOUND)
 		rt2x00_set_field8(&rfcsr, RFCSR49_TX, POWER_BOUND);
 	else
@@ -2818,7 +2822,7 @@ static void rt2800_config_channel_rf53xx(struct rt2x00_dev *rt2x00dev,
 	rt2800_rfcsr_write(rt2x00dev, 49, rfcsr);
 
 	if (rt2x00_rt(rt2x00dev, RT5392)) {
-		rt2800_rfcsr_read(rt2x00dev, 50, &rfcsr);
+		rfcsr = rt2800_rfcsr_read(rt2x00dev, 50);
 		if (info->default_power2 > POWER_BOUND)
 			rt2x00_set_field8(&rfcsr, RFCSR50_TX, POWER_BOUND);
 		else
@@ -2827,7 +2831,7 @@ static void rt2800_config_channel_rf53xx(struct rt2x00_dev *rt2x00dev,
 		rt2800_rfcsr_write(rt2x00dev, 50, rfcsr);
 	}
 
-	rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 1);
 	if (rt2x00_rt(rt2x00dev, RT5392)) {
 		rt2x00_set_field8(&rfcsr, RFCSR1_RX1_PD, 1);
 		rt2x00_set_field8(&rfcsr, RFCSR1_TX1_PD, 1);
@@ -2911,7 +2915,7 @@ static void rt2800_config_channel_rf55xx(struct rt2x00_dev *rt2x00dev,
 	const bool is_11b = false;
 	const bool is_type_ep = false;
 
-	rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
+	reg = rt2800_register_read(rt2x00dev, LDO_CFG0);
 	rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL,
 			   (rf->channel > 14 || conf_is_ht40(conf)) ? 5 : 0);
 	rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
@@ -2919,13 +2923,13 @@ static void rt2800_config_channel_rf55xx(struct rt2x00_dev *rt2x00dev,
 	/* Order of values on rf_channel entry: N, K, mod, R */
 	rt2800_rfcsr_write(rt2x00dev, 8, rf->rf1 & 0xff);
 
-	rt2800_rfcsr_read(rt2x00dev,  9, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev,  9);
 	rt2x00_set_field8(&rfcsr, RFCSR9_K, rf->rf2 & 0xf);
 	rt2x00_set_field8(&rfcsr, RFCSR9_N, (rf->rf1 & 0x100) >> 8);
 	rt2x00_set_field8(&rfcsr, RFCSR9_MOD, ((rf->rf3 - 8) & 0x4) >> 2);
 	rt2800_rfcsr_write(rt2x00dev, 9, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 11, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 11);
 	rt2x00_set_field8(&rfcsr, RFCSR11_R, rf->rf4 - 1);
 	rt2x00_set_field8(&rfcsr, RFCSR11_MOD, (rf->rf3 - 8) & 0x3);
 	rt2800_rfcsr_write(rt2x00dev, 11, rfcsr);
@@ -3093,7 +3097,7 @@ static void rt2800_config_channel_rf55xx(struct rt2x00_dev *rt2x00dev,
 		ep_reg = 0x3;
 	}
 
-	rt2800_rfcsr_read(rt2x00dev, 49, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 49);
 	if (info->default_power1 > power_bound)
 		rt2x00_set_field8(&rfcsr, RFCSR49_TX, power_bound);
 	else
@@ -3102,7 +3106,7 @@ static void rt2800_config_channel_rf55xx(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field8(&rfcsr, RFCSR49_EP, ep_reg);
 	rt2800_rfcsr_write(rt2x00dev, 49, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 50, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 50);
 	if (info->default_power2 > power_bound)
 		rt2x00_set_field8(&rfcsr, RFCSR50_TX, power_bound);
 	else
@@ -3111,7 +3115,7 @@ static void rt2800_config_channel_rf55xx(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field8(&rfcsr, RFCSR50_EP, ep_reg);
 	rt2800_rfcsr_write(rt2x00dev, 50, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 1);
 	rt2x00_set_field8(&rfcsr, RFCSR1_RF_BLOCK_EN, 1);
 	rt2x00_set_field8(&rfcsr, RFCSR1_PLL_PD, 1);
 
@@ -3144,7 +3148,7 @@ static void rt2800_config_channel_rf55xx(struct rt2x00_dev *rt2x00dev,
 	rt2800_freq_cal_mode1(rt2x00dev);
 
 	/* TODO merge with others */
-	rt2800_rfcsr_read(rt2x00dev, 3, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 3);
 	rt2x00_set_field8(&rfcsr, RFCSR3_VCOCAL_EN, 1);
 	rt2800_rfcsr_write(rt2x00dev, 3, rfcsr);
 
@@ -3186,7 +3190,7 @@ static void rt2800_config_channel_rf7620(struct rt2x00_dev *rt2x00dev,
 	/* Rdiv setting (set 0x03 if Xtal==20)
 	 * R13[1:0]
 	 */
-	rt2800_rfcsr_read(rt2x00dev, 13, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 13);
 	rt2x00_set_field8(&rfcsr, RFCSR13_RDIV_MT7620,
 			  rt2800_clk_is_20mhz(rt2x00dev) ? 3 : 0);
 	rt2800_rfcsr_write(rt2x00dev, 13, rfcsr);
@@ -3195,25 +3199,25 @@ static void rt2800_config_channel_rf7620(struct rt2x00_dev *rt2x00dev,
 	 * R20[7:0] in rf->rf1
 	 * R21[0] always 0
 	 */
-	rt2800_rfcsr_read(rt2x00dev, 20, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 20);
 	rfcsr = (rf->rf1 & 0x00ff);
 	rt2800_rfcsr_write(rt2x00dev, 20, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 21, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 21);
 	rt2x00_set_field8(&rfcsr, RFCSR21_BIT1, 0);
 	rt2800_rfcsr_write(rt2x00dev, 21, rfcsr);
 
 	/* K setting (always 0)
 	 * R16[3:0] (RF PLL freq selection)
 	 */
-	rt2800_rfcsr_read(rt2x00dev, 16, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 16);
 	rt2x00_set_field8(&rfcsr, RFCSR16_RF_PLL_FREQ_SEL_MT7620, 0);
 	rt2800_rfcsr_write(rt2x00dev, 16, rfcsr);
 
 	/* D setting (always 0)
 	 * R22[2:0] (D=15, R22[2:0]=<111>)
 	 */
-	rt2800_rfcsr_read(rt2x00dev, 22, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 22);
 	rt2x00_set_field8(&rfcsr, RFCSR22_FREQPLAN_D_MT7620, 0);
 	rt2800_rfcsr_write(rt2x00dev, 22, rfcsr);
 
@@ -3222,40 +3226,40 @@ static void rt2800_config_channel_rf7620(struct rt2x00_dev *rt2x00dev,
 	 *      R18<7:0> in rf->rf3
 	 *      R19<1:0> in rf->rf4
 	 */
-	rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 17);
 	rfcsr = rf->rf2;
 	rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 18, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 18);
 	rfcsr = rf->rf3;
 	rt2800_rfcsr_write(rt2x00dev, 18, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 19, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 19);
 	rt2x00_set_field8(&rfcsr, RFCSR19_K, rf->rf4);
 	rt2800_rfcsr_write(rt2x00dev, 19, rfcsr);
 
 	/* Default: XO=20MHz , SDM mode */
-	rt2800_rfcsr_read(rt2x00dev, 16, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 16);
 	rt2x00_set_field8(&rfcsr, RFCSR16_SDM_MODE_MT7620, 0x80);
 	rt2800_rfcsr_write(rt2x00dev, 16, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 21, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 21);
 	rt2x00_set_field8(&rfcsr, RFCSR21_BIT8, 1);
 	rt2800_rfcsr_write(rt2x00dev, 21, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 1);
 	rt2x00_set_field8(&rfcsr, RFCSR1_TX2_EN_MT7620,
 			  rt2x00dev->default_ant.tx_chain_num != 1);
 	rt2800_rfcsr_write(rt2x00dev, 1, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 2, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 2);
 	rt2x00_set_field8(&rfcsr, RFCSR2_TX2_EN_MT7620,
 			  rt2x00dev->default_ant.tx_chain_num != 1);
 	rt2x00_set_field8(&rfcsr, RFCSR2_RX2_EN_MT7620,
 			  rt2x00dev->default_ant.rx_chain_num != 1);
 	rt2800_rfcsr_write(rt2x00dev, 2, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 42, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 42);
 	rt2x00_set_field8(&rfcsr, RFCSR42_TX2_EN_MT7620,
 			  rt2x00dev->default_ant.tx_chain_num != 1);
 	rt2800_rfcsr_write(rt2x00dev, 42, rfcsr);
@@ -3283,7 +3287,7 @@ static void rt2800_config_channel_rf7620(struct rt2x00_dev *rt2x00dev,
 		rt2800_rfcsr_write_dccal(rt2x00dev, 59, 0x28);
 	}
 
-	rt2800_rfcsr_read(rt2x00dev, 28, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 28);
 	rt2x00_set_field8(&rfcsr, RFCSR28_CH11_HT40,
 			  conf_is_ht40(conf) && (rf->channel == 11));
 	rt2800_rfcsr_write(rt2x00dev, 28, rfcsr);
@@ -3296,36 +3300,36 @@ static void rt2800_config_channel_rf7620(struct rt2x00_dev *rt2x00dev,
 			rx_agc_fc = drv_data->rx_calibration_bw20;
 			tx_agc_fc = drv_data->tx_calibration_bw20;
 		}
-		rt2800_rfcsr_read_bank(rt2x00dev, 5, 6, &rfcsr);
+		rfcsr = rt2800_rfcsr_read_bank(rt2x00dev, 5, 6);
 		rfcsr &= (~0x3F);
 		rfcsr |= rx_agc_fc;
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 6, rfcsr);
-		rt2800_rfcsr_read_bank(rt2x00dev, 5, 7, &rfcsr);
+		rfcsr = rt2800_rfcsr_read_bank(rt2x00dev, 5, 7);
 		rfcsr &= (~0x3F);
 		rfcsr |= rx_agc_fc;
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 7, rfcsr);
-		rt2800_rfcsr_read_bank(rt2x00dev, 7, 6, &rfcsr);
+		rfcsr = rt2800_rfcsr_read_bank(rt2x00dev, 7, 6);
 		rfcsr &= (~0x3F);
 		rfcsr |= rx_agc_fc;
 		rt2800_rfcsr_write_bank(rt2x00dev, 7, 6, rfcsr);
-		rt2800_rfcsr_read_bank(rt2x00dev, 7, 7, &rfcsr);
+		rfcsr = rt2800_rfcsr_read_bank(rt2x00dev, 7, 7);
 		rfcsr &= (~0x3F);
 		rfcsr |= rx_agc_fc;
 		rt2800_rfcsr_write_bank(rt2x00dev, 7, 7, rfcsr);
 
-		rt2800_rfcsr_read_bank(rt2x00dev, 5, 58, &rfcsr);
+		rfcsr = rt2800_rfcsr_read_bank(rt2x00dev, 5, 58);
 		rfcsr &= (~0x3F);
 		rfcsr |= tx_agc_fc;
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 58, rfcsr);
-		rt2800_rfcsr_read_bank(rt2x00dev, 5, 59, &rfcsr);
+		rfcsr = rt2800_rfcsr_read_bank(rt2x00dev, 5, 59);
 		rfcsr &= (~0x3F);
 		rfcsr |= tx_agc_fc;
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 59, rfcsr);
-		rt2800_rfcsr_read_bank(rt2x00dev, 7, 58, &rfcsr);
+		rfcsr = rt2800_rfcsr_read_bank(rt2x00dev, 7, 58);
 		rfcsr &= (~0x3F);
 		rfcsr |= tx_agc_fc;
 		rt2800_rfcsr_write_bank(rt2x00dev, 7, 58, rfcsr);
-		rt2800_rfcsr_read_bank(rt2x00dev, 7, 59, &rfcsr);
+		rfcsr = rt2800_rfcsr_read_bank(rt2x00dev, 7, 59);
 		rfcsr &= (~0x3F);
 		rfcsr |= tx_agc_fc;
 		rt2800_rfcsr_write_bank(rt2x00dev, 7, 59, rfcsr);
@@ -3350,34 +3354,33 @@ static void rt2800_config_alc(struct rt2x00_dev *rt2x00dev,
 	if (max_power > 0x2f)
 		max_power = 0x2f;
 
-	rt2800_register_read(rt2x00dev, TX_ALC_CFG_0, &reg);
+	reg = rt2800_register_read(rt2x00dev, TX_ALC_CFG_0);
 	rt2x00_set_field32(&reg, TX_ALC_CFG_0_CH_INIT_0, power_level);
 	rt2x00_set_field32(&reg, TX_ALC_CFG_0_CH_INIT_1, power_level);
 	rt2x00_set_field32(&reg, TX_ALC_CFG_0_LIMIT_0, max_power);
 	rt2x00_set_field32(&reg, TX_ALC_CFG_0_LIMIT_1, max_power);
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &eeprom);
+	eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1);
 	if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF1_INTERNAL_TX_ALC)) {
 		/* init base power by eeprom target power */
-		rt2800_eeprom_read(rt2x00dev, EEPROM_TXPOWER_INIT,
-				   &target_power);
+		target_power = rt2800_eeprom_read(rt2x00dev,
+						  EEPROM_TXPOWER_INIT);
 		rt2x00_set_field32(&reg, TX_ALC_CFG_0_CH_INIT_0, target_power);
 		rt2x00_set_field32(&reg, TX_ALC_CFG_0_CH_INIT_1, target_power);
 	}
 	rt2800_register_write(rt2x00dev, TX_ALC_CFG_0, reg);
 
-	rt2800_register_read(rt2x00dev, TX_ALC_CFG_1, &reg);
+	reg = rt2800_register_read(rt2x00dev, TX_ALC_CFG_1);
 	rt2x00_set_field32(&reg, TX_ALC_CFG_1_TX_TEMP_COMP, 0);
 	rt2800_register_write(rt2x00dev, TX_ALC_CFG_1, reg);
 
 	/* Save MAC SYS CTRL registers */
-	rt2800_register_read(rt2x00dev, MAC_SYS_CTRL, &mac_sys_ctrl);
+	mac_sys_ctrl = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
 	/* Disable Tx/Rx */
 	rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, 0);
 	/* Check MAC Tx/Rx idle */
 	for (i = 0; i < 10000; i++) {
-		rt2800_register_read(rt2x00dev, MAC_STATUS_CFG,
-				     &mac_status);
+		mac_status = rt2800_register_read(rt2x00dev, MAC_STATUS_CFG);
 		if (mac_status & 0x3)
 			usleep_range(50, 200);
 		else
@@ -3388,7 +3391,7 @@ static void rt2800_config_alc(struct rt2x00_dev *rt2x00dev,
 		rt2x00_warn(rt2x00dev, "Wait MAC Status to MAX !!!\n");
 
 	if (chan->center_freq > 2457) {
-		rt2800_bbp_read(rt2x00dev, 30, &bbp);
+		bbp = rt2800_bbp_read(rt2x00dev, 30);
 		bbp = 0x40;
 		rt2800_bbp_write(rt2x00dev, 30, bbp);
 		rt2800_rfcsr_write(rt2x00dev, 39, 0);
@@ -3397,7 +3400,7 @@ static void rt2800_config_alc(struct rt2x00_dev *rt2x00dev,
 		else
 			rt2800_rfcsr_write(rt2x00dev, 42, 0x7b);
 	} else {
-		rt2800_bbp_read(rt2x00dev, 30, &bbp);
+		bbp = rt2800_bbp_read(rt2x00dev, 30);
 		bbp = 0x1f;
 		rt2800_bbp_write(rt2x00dev, 30, bbp);
 		rt2800_rfcsr_write(rt2x00dev, 39, 0x80);
@@ -3418,7 +3421,7 @@ static void rt2800_bbp_write_with_rx_chain(struct rt2x00_dev *rt2x00dev,
 	u8 chain, reg;
 
 	for (chain = 0; chain < rt2x00dev->default_ant.rx_chain_num; chain++) {
-		rt2800_bbp_read(rt2x00dev, 27, &reg);
+		reg = rt2800_bbp_read(rt2x00dev, 27);
 		rt2x00_set_field8(&reg,  BBP27_RX_CHAIN_SEL, chain);
 		rt2800_bbp_write(rt2x00dev, 27, reg);
 
@@ -3597,7 +3600,7 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	    rt2x00_rf(rt2x00dev, RF5372) ||
 	    rt2x00_rf(rt2x00dev, RF5390) ||
 	    rt2x00_rf(rt2x00dev, RF5392)) {
-		rt2800_rfcsr_read(rt2x00dev, 30, &rfcsr);
+		rfcsr = rt2800_rfcsr_read(rt2x00dev, 30);
 		if (rt2x00_rf(rt2x00dev, RF3322)) {
 			rt2x00_set_field8(&rfcsr, RF3322_RFCSR30_TX_H20M,
 					  conf_is_ht40(conf));
@@ -3611,7 +3614,7 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 		}
 		rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
 
-		rt2800_rfcsr_read(rt2x00dev, 3, &rfcsr);
+		rfcsr = rt2800_rfcsr_read(rt2x00dev, 3);
 		rt2x00_set_field8(&rfcsr, RFCSR3_VCOCAL_EN, 1);
 		rt2800_rfcsr_write(rt2x00dev, 3, rfcsr);
 	}
@@ -3690,7 +3693,7 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 			rt2800_bbp_write(rt2x00dev, 75, 0x50);
 	}
 
-	rt2800_register_read(rt2x00dev, TX_BAND_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, TX_BAND_CFG);
 	rt2x00_set_field32(&reg, TX_BAND_CFG_HT40_MINUS, conf_is_ht40_minus(conf));
 	rt2x00_set_field32(&reg, TX_BAND_CFG_A, rf->channel > 14);
 	rt2x00_set_field32(&reg, TX_BAND_CFG_BG, rf->channel <= 14);
@@ -3699,7 +3702,7 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	if (rt2x00_rt(rt2x00dev, RT3572))
 		rt2800_rfcsr_write(rt2x00dev, 8, 0);
 
-	rt2800_register_read(rt2x00dev, TX_PIN_CFG, &tx_pin);
+	tx_pin = rt2800_register_read(rt2x00dev, TX_PIN_CFG);
 
 	switch (rt2x00dev->default_ant.tx_chain_num) {
 	case 3:
@@ -3765,7 +3768,7 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	}
 
 	if (rt2x00_rt(rt2x00dev, RT3593)) {
-		rt2800_register_read(rt2x00dev, GPIO_CTRL, &reg);
+		reg = rt2800_register_read(rt2x00dev, GPIO_CTRL);
 
 		/* Band selection */
 		if (rt2x00_is_usb(rt2x00dev) ||
@@ -3832,11 +3835,11 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 		rt2800_iq_calibrate(rt2x00dev, rf->channel);
 	}
 
-	rt2800_bbp_read(rt2x00dev, 4, &bbp);
+	bbp = rt2800_bbp_read(rt2x00dev, 4);
 	rt2x00_set_field8(&bbp, BBP4_BANDWIDTH, 2 * conf_is_ht40(conf));
 	rt2800_bbp_write(rt2x00dev, 4, bbp);
 
-	rt2800_bbp_read(rt2x00dev, 3, &bbp);
+	bbp = rt2800_bbp_read(rt2x00dev, 3);
 	rt2x00_set_field8(&bbp, BBP3_HT40_MINUS, conf_is_ht40_minus(conf));
 	rt2800_bbp_write(rt2x00dev, 3, bbp);
 
@@ -3857,16 +3860,16 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Clear channel statistic counters
 	 */
-	rt2800_register_read(rt2x00dev, CH_IDLE_STA, &reg);
-	rt2800_register_read(rt2x00dev, CH_BUSY_STA, &reg);
-	rt2800_register_read(rt2x00dev, CH_BUSY_STA_SEC, &reg);
+	reg = rt2800_register_read(rt2x00dev, CH_IDLE_STA);
+	reg = rt2800_register_read(rt2x00dev, CH_BUSY_STA);
+	reg = rt2800_register_read(rt2x00dev, CH_BUSY_STA_SEC);
 
 	/*
 	 * Clear update flag
 	 */
 	if (rt2x00_rt(rt2x00dev, RT3352) ||
 	    rt2x00_rt(rt2x00dev, RT5350)) {
-		rt2800_bbp_read(rt2x00dev, 49, &bbp);
+		bbp = rt2800_bbp_read(rt2x00dev, 49);
 		rt2x00_set_field8(&bbp, BBP49_UPDATE_FLAG, 0);
 		rt2800_bbp_write(rt2x00dev, 49, bbp);
 	}
@@ -3883,7 +3886,7 @@ static int rt2800_get_gain_calibration_delta(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * First check if temperature compensation is supported.
 	 */
-	rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &eeprom);
+	eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1);
 	if (!rt2x00_get_field16(eeprom, EEPROM_NIC_CONF1_EXTERNAL_TX_ALC))
 		return 0;
 
@@ -3896,62 +3899,62 @@ static int rt2800_get_gain_calibration_delta(struct rt2x00_dev *rt2x00dev)
 	 * Example TSSI bounds  0xF0 0xD0 0xB5 0xA0 0x88 0x45 0x25 0x15 0x00
 	 */
 	if (rt2x00dev->curr_band == NL80211_BAND_2GHZ) {
-		rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_BG1, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_BG1);
 		tssi_bounds[0] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_BG1_MINUS4);
 		tssi_bounds[1] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_BG1_MINUS3);
 
-		rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_BG2, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_BG2);
 		tssi_bounds[2] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_BG2_MINUS2);
 		tssi_bounds[3] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_BG2_MINUS1);
 
-		rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_BG3, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_BG3);
 		tssi_bounds[4] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_BG3_REF);
 		tssi_bounds[5] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_BG3_PLUS1);
 
-		rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_BG4, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_BG4);
 		tssi_bounds[6] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_BG4_PLUS2);
 		tssi_bounds[7] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_BG4_PLUS3);
 
-		rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_BG5, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_BG5);
 		tssi_bounds[8] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_BG5_PLUS4);
 
 		step = rt2x00_get_field16(eeprom,
 					  EEPROM_TSSI_BOUND_BG5_AGC_STEP);
 	} else {
-		rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_A1, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_A1);
 		tssi_bounds[0] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_A1_MINUS4);
 		tssi_bounds[1] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_A1_MINUS3);
 
-		rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_A2, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_A2);
 		tssi_bounds[2] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_A2_MINUS2);
 		tssi_bounds[3] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_A2_MINUS1);
 
-		rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_A3, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_A3);
 		tssi_bounds[4] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_A3_REF);
 		tssi_bounds[5] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_A3_PLUS1);
 
-		rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_A4, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_A4);
 		tssi_bounds[6] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_A4_PLUS2);
 		tssi_bounds[7] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_A4_PLUS3);
 
-		rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_A5, &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_TSSI_BOUND_A5);
 		tssi_bounds[8] = rt2x00_get_field16(eeprom,
 					EEPROM_TSSI_BOUND_A5_PLUS4);
 
@@ -3968,7 +3971,7 @@ static int rt2800_get_gain_calibration_delta(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Read current TSSI (BBP 49).
 	 */
-	rt2800_bbp_read(rt2x00dev, 49, &current_tssi);
+	current_tssi = rt2800_bbp_read(rt2x00dev, 49);
 
 	/*
 	 * Compare TSSI value (BBP49) with the compensation boundaries
@@ -3997,7 +4000,7 @@ static int rt2800_get_txpower_bw_comp(struct rt2x00_dev *rt2x00dev,
 	u8 comp_type;
 	int comp_value = 0;
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_TXPOWER_DELTA, &eeprom);
+	eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_TXPOWER_DELTA);
 
 	/*
 	 * HT40 compensation not required.
@@ -4075,13 +4078,13 @@ static u8 rt2800_compensate_txpower(struct rt2x00_dev *rt2x00dev, int is_rate_b,
 		 * .11b data rate need add additional 4dbm
 		 * when calculating eirp txpower.
 		 */
-		rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
-					      1, &eeprom);
+		eeprom = rt2800_eeprom_read_from_array(rt2x00dev,
+						       EEPROM_TXPOWER_BYRATE,
+						       1);
 		criterion = rt2x00_get_field16(eeprom,
 					       EEPROM_TXPOWER_BYRATE_RATE0);
 
-		rt2800_eeprom_read(rt2x00dev, EEPROM_EIRP_MAX_TX_POWER,
-				   &eeprom);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_EIRP_MAX_TX_POWER);
 
 		if (band == NL80211_BAND_2GHZ)
 			eirp_txpower_criterion = rt2x00_get_field16(eeprom,
@@ -4150,8 +4153,8 @@ static void rt2800_config_txpower_rt3593(struct rt2x00_dev *rt2x00dev,
 		offset += 8;
 
 	/* read the next four txpower values */
-	rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
-				      offset, &eeprom);
+	eeprom = rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
+					       offset);
 
 	/* CCK 1MBS,2MBS */
 	txpower = rt2x00_get_field16(eeprom, EEPROM_TXPOWER_BYRATE_RATE0);
@@ -4198,8 +4201,8 @@ static void rt2800_config_txpower_rt3593(struct rt2x00_dev *rt2x00dev,
 			   TX_PWR_CFG_0_EXT_OFDM12_CH2, txpower);
 
 	/* read the next four txpower values */
-	rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
-				      offset + 1, &eeprom);
+	eeprom = rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
+					       offset + 1);
 
 	/* OFDM 24MBS,36MBS */
 	txpower = rt2x00_get_field16(eeprom, EEPROM_TXPOWER_BYRATE_RATE0);
@@ -4235,8 +4238,8 @@ static void rt2800_config_txpower_rt3593(struct rt2x00_dev *rt2x00dev,
 			   TX_PWR_CFG_7_OFDM54_CH2, txpower);
 
 	/* read the next four txpower values */
-	rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
-				      offset + 2, &eeprom);
+	eeprom = rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
+					       offset + 2);
 
 	/* MCS 0,1 */
 	txpower = rt2x00_get_field16(eeprom, EEPROM_TXPOWER_BYRATE_RATE0);
@@ -4283,8 +4286,8 @@ static void rt2800_config_txpower_rt3593(struct rt2x00_dev *rt2x00dev,
 			   TX_PWR_CFG_2_EXT_MCS6_CH2, txpower);
 
 	/* read the next four txpower values */
-	rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
-				      offset + 3, &eeprom);
+	eeprom = rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
+					       offset + 3);
 
 	/* MCS 7 */
 	txpower = rt2x00_get_field16(eeprom, EEPROM_TXPOWER_BYRATE_RATE0);
@@ -4331,8 +4334,8 @@ static void rt2800_config_txpower_rt3593(struct rt2x00_dev *rt2x00dev,
 			   TX_PWR_CFG_3_EXT_MCS12_CH2, txpower);
 
 	/* read the next four txpower values */
-	rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
-				      offset + 4, &eeprom);
+	eeprom = rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
+					       offset + 4);
 
 	/* MCS 14 */
 	txpower = rt2x00_get_field16(eeprom, EEPROM_TXPOWER_BYRATE_RATE0);
@@ -4379,8 +4382,8 @@ static void rt2800_config_txpower_rt3593(struct rt2x00_dev *rt2x00dev,
 			   TX_PWR_CFG_5_MCS18_CH2, txpower);
 
 	/* read the next four txpower values */
-	rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
-				      offset + 5, &eeprom);
+	eeprom = rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
+					       offset + 5);
 
 	/* MCS 20,21 */
 	txpower = rt2x00_get_field16(eeprom, EEPROM_TXPOWER_BYRATE_RATE0);
@@ -4416,8 +4419,8 @@ static void rt2800_config_txpower_rt3593(struct rt2x00_dev *rt2x00dev,
 			   TX_PWR_CFG_8_MCS23_CH2, txpower);
 
 	/* read the next four txpower values */
-	rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
-				      offset + 6, &eeprom);
+	eeprom = rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
+					       offset + 6);
 
 	/* STBC, MCS 0,1 */
 	txpower = rt2x00_get_field16(eeprom, EEPROM_TXPOWER_BYRATE_RATE0);
@@ -4460,8 +4463,8 @@ static void rt2800_config_txpower_rt3593(struct rt2x00_dev *rt2x00dev,
 			   txpower);
 
 	/* read the next four txpower values */
-	rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
-				      offset + 7, &eeprom);
+	eeprom = rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
+					       offset + 7);
 
 	/* STBC, MCS 7 */
 	txpower = rt2x00_get_field16(eeprom, EEPROM_TXPOWER_BYRATE_RATE0);
@@ -4541,8 +4544,9 @@ static void rt2800_config_txpower_rt6352(struct rt2x00_dev *rt2x00dev,
 	 * board vendors expected when they populated the EEPROM...
 	 */
 	for (i = 0; i < 5; i++) {
-		rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
-					      i * 2, &eeprom);
+		eeprom = rt2800_eeprom_read_from_array(rt2x00dev,
+						       EEPROM_TXPOWER_BYRATE,
+						       i * 2);
 
 		data = eeprom;
 
@@ -4558,8 +4562,9 @@ static void rt2800_config_txpower_rt6352(struct rt2x00_dev *rt2x00dev,
 
 		gdata |= (t << 8);
 
-		rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
-					      (i * 2) + 1, &eeprom);
+		eeprom = rt2800_eeprom_read_from_array(rt2x00dev,
+						       EEPROM_TXPOWER_BYRATE,
+						       (i * 2) + 1);
 
 		t = eeprom & 0x3f;
 		if (t == 32)
@@ -4601,26 +4606,26 @@ static void rt2800_config_txpower_rt6352(struct rt2x00_dev *rt2x00dev,
 
 	/* For OFDM 54MBS use value from OFDM 48MBS */
 	pwreg = 0;
-	rt2800_register_read(rt2x00dev, TX_PWR_CFG_1, &reg);
+	reg = rt2800_register_read(rt2x00dev, TX_PWR_CFG_1);
 	t = rt2x00_get_field32(reg, TX_PWR_CFG_1B_48MBS);
 	rt2x00_set_field32(&pwreg, TX_PWR_CFG_7B_54MBS, t);
 
 	/* For MCS 7 use value from MCS 6 */
-	rt2800_register_read(rt2x00dev, TX_PWR_CFG_2, &reg);
+	reg = rt2800_register_read(rt2x00dev, TX_PWR_CFG_2);
 	t = rt2x00_get_field32(reg, TX_PWR_CFG_2B_MCS6_MCS7);
 	rt2x00_set_field32(&pwreg, TX_PWR_CFG_7B_MCS7, t);
 	rt2800_register_write(rt2x00dev, TX_PWR_CFG_7, pwreg);
 
 	/* For MCS 15 use value from MCS 14 */
 	pwreg = 0;
-	rt2800_register_read(rt2x00dev, TX_PWR_CFG_3, &reg);
+	reg = rt2800_register_read(rt2x00dev, TX_PWR_CFG_3);
 	t = rt2x00_get_field32(reg, TX_PWR_CFG_3B_MCS14);
 	rt2x00_set_field32(&pwreg, TX_PWR_CFG_8B_MCS15, t);
 	rt2800_register_write(rt2x00dev, TX_PWR_CFG_8, pwreg);
 
 	/* For STBC MCS 7 use value from STBC MCS 6 */
 	pwreg = 0;
-	rt2800_register_read(rt2x00dev, TX_PWR_CFG_4, &reg);
+	reg = rt2800_register_read(rt2x00dev, TX_PWR_CFG_4);
 	t = rt2x00_get_field32(reg, TX_PWR_CFG_4B_STBC_MCS6);
 	rt2x00_set_field32(&pwreg, TX_PWR_CFG_9B_STBC_MCS7, t);
 	rt2800_register_write(rt2x00dev, TX_PWR_CFG_9, pwreg);
@@ -4702,7 +4707,7 @@ static void rt2800_config_txpower_rt28xx(struct rt2x00_dev *rt2x00dev,
 	} else {
 		power_ctrl = 0;
 	}
-	rt2800_bbp_read(rt2x00dev, 1, &r1);
+	r1 = rt2800_bbp_read(rt2x00dev, 1);
 	rt2x00_set_field8(&r1, BBP1_TX_POWER_CTRL, power_ctrl);
 	rt2800_bbp_write(rt2x00dev, 1, r1);
 
@@ -4713,11 +4718,12 @@ static void rt2800_config_txpower_rt28xx(struct rt2x00_dev *rt2x00dev,
 		if (offset > TX_PWR_CFG_4)
 			break;
 
-		rt2800_register_read(rt2x00dev, offset, &reg);
+		reg = rt2800_register_read(rt2x00dev, offset);
 
 		/* read the next four txpower values */
-		rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
-					      i, &eeprom);
+		eeprom = rt2800_eeprom_read_from_array(rt2x00dev,
+						       EEPROM_TXPOWER_BYRATE,
+						       i);
 
 		is_rate_b = i ? 0 : 1;
 		/*
@@ -4765,8 +4771,9 @@ static void rt2800_config_txpower_rt28xx(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE3, txpower);
 
 		/* read the next four txpower values */
-		rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_TXPOWER_BYRATE,
-					      i + 1, &eeprom);
+		eeprom = rt2800_eeprom_read_from_array(rt2x00dev,
+						       EEPROM_TXPOWER_BYRATE,
+						       i + 1);
 
 		is_rate_b = 0;
 		/*
@@ -4853,7 +4860,7 @@ void rt2800_vco_calibration(struct rt2x00_dev *rt2x00dev)
 	 * periodically to adjust the frequency to be precision.
 	*/
 
-	rt2800_register_read(rt2x00dev, TX_PIN_CFG, &tx_pin);
+	tx_pin = rt2800_register_read(rt2x00dev, TX_PIN_CFG);
 	tx_pin &= TX_PIN_CFG_PA_PE_DISABLE;
 	rt2800_register_write(rt2x00dev, TX_PIN_CFG, tx_pin);
 
@@ -4864,7 +4871,7 @@ void rt2800_vco_calibration(struct rt2x00_dev *rt2x00dev)
 	case RF3022:
 	case RF3320:
 	case RF3052:
-		rt2800_rfcsr_read(rt2x00dev, 7, &rfcsr);
+		rfcsr = rt2800_rfcsr_read(rt2x00dev, 7);
 		rt2x00_set_field8(&rfcsr, RFCSR7_RF_TUNING, 1);
 		rt2800_rfcsr_write(rt2x00dev, 7, rfcsr);
 		break;
@@ -4879,7 +4886,7 @@ void rt2800_vco_calibration(struct rt2x00_dev *rt2x00dev)
 	case RF5390:
 	case RF5392:
 	case RF5592:
-		rt2800_rfcsr_read(rt2x00dev, 3, &rfcsr);
+		rfcsr = rt2800_rfcsr_read(rt2x00dev, 3);
 		rt2x00_set_field8(&rfcsr, RFCSR3_VCOCAL_EN, 1);
 		rt2800_rfcsr_write(rt2x00dev, 3, rfcsr);
 		min_sleep = 1000;
@@ -4887,7 +4894,7 @@ void rt2800_vco_calibration(struct rt2x00_dev *rt2x00dev)
 	case RF7620:
 		rt2800_rfcsr_write(rt2x00dev, 5, 0x40);
 		rt2800_rfcsr_write(rt2x00dev, 4, 0x0C);
-		rt2800_rfcsr_read(rt2x00dev, 4, &rfcsr);
+		rfcsr = rt2800_rfcsr_read(rt2x00dev, 4);
 		rt2x00_set_field8(&rfcsr, RFCSR4_VCOCAL_EN, 1);
 		rt2800_rfcsr_write(rt2x00dev, 4, rfcsr);
 		min_sleep = 2000;
@@ -4901,7 +4908,7 @@ void rt2800_vco_calibration(struct rt2x00_dev *rt2x00dev)
 	if (min_sleep > 0)
 		usleep_range(min_sleep, min_sleep * 2);
 
-	rt2800_register_read(rt2x00dev, TX_PIN_CFG, &tx_pin);
+	tx_pin = rt2800_register_read(rt2x00dev, TX_PIN_CFG);
 	if (rt2x00dev->rf_channel <= 14) {
 		switch (rt2x00dev->default_ant.tx_chain_num) {
 		case 3:
@@ -4975,7 +4982,7 @@ static void rt2800_config_retry_limit(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2800_register_read(rt2x00dev, TX_RTY_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, TX_RTY_CFG);
 	rt2x00_set_field32(&reg, TX_RTY_CFG_SHORT_RTY_LIMIT,
 			   libconf->conf->short_frame_max_tx_count);
 	rt2x00_set_field32(&reg, TX_RTY_CFG_LONG_RTY_LIMIT,
@@ -4994,7 +5001,7 @@ static void rt2800_config_ps(struct rt2x00_dev *rt2x00dev,
 	if (state == STATE_SLEEP) {
 		rt2800_register_write(rt2x00dev, AUTOWAKEUP_CFG, 0);
 
-		rt2800_register_read(rt2x00dev, AUTOWAKEUP_CFG, &reg);
+		reg = rt2800_register_read(rt2x00dev, AUTOWAKEUP_CFG);
 		rt2x00_set_field32(&reg, AUTOWAKEUP_CFG_AUTO_LEAD_TIME, 5);
 		rt2x00_set_field32(&reg, AUTOWAKEUP_CFG_TBCN_BEFORE_WAKE,
 				   libconf->conf->listen_interval - 1);
@@ -5003,7 +5010,7 @@ static void rt2800_config_ps(struct rt2x00_dev *rt2x00dev,
 
 		rt2x00dev->ops->lib->set_device_state(rt2x00dev, state);
 	} else {
-		rt2800_register_read(rt2x00dev, AUTOWAKEUP_CFG, &reg);
+		reg = rt2800_register_read(rt2x00dev, AUTOWAKEUP_CFG);
 		rt2x00_set_field32(&reg, AUTOWAKEUP_CFG_AUTO_LEAD_TIME, 0);
 		rt2x00_set_field32(&reg, AUTOWAKEUP_CFG_TBCN_BEFORE_WAKE, 0);
 		rt2x00_set_field32(&reg, AUTOWAKEUP_CFG_AUTOWAKE, 0);
@@ -5046,7 +5053,7 @@ void rt2800_link_stats(struct rt2x00_dev *rt2x00dev, struct link_qual *qual)
 	/*
 	 * Update FCS error count from register.
 	 */
-	rt2800_register_read(rt2x00dev, RX_STA_CNT0, &reg);
+	reg = rt2800_register_read(rt2x00dev, RX_STA_CNT0);
 	qual->rx_failed = rt2x00_get_field32(reg, RX_STA_CNT0_CRC_ERR);
 }
 EXPORT_SYMBOL_GPL(rt2800_link_stats);
@@ -5175,7 +5182,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 
 	rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, 0x00000000);
 
-	rt2800_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, BCN_TIME_CFG);
 	rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_INTERVAL, 1600);
 	rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 0);
 	rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_SYNC, 0);
@@ -5186,43 +5193,43 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 
 	rt2800_config_filter(rt2x00dev, FIF_ALLMULTI);
 
-	rt2800_register_read(rt2x00dev, BKOFF_SLOT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, BKOFF_SLOT_CFG);
 	rt2x00_set_field32(&reg, BKOFF_SLOT_CFG_SLOT_TIME, 9);
 	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);
+		reg = rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL);
 		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);
+		reg = rt2800_register_read(rt2x00dev, CMB_CTRL);
 		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);
+		reg = rt2800_register_read(rt2x00dev, OSC_CTRL);
 		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);
+		reg = rt2800_register_read(rt2x00dev, COEX_CFG0);
 		rt2x00_set_field32(&reg, COEX_CFG_ANT, 0x5e);
 		rt2800_register_write(rt2x00dev, COEX_CFG0, reg);
 
-		rt2800_register_read(rt2x00dev, COEX_CFG2, &reg);
+		reg = rt2800_register_read(rt2x00dev, COEX_CFG2);
 		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);
+		reg = rt2800_register_read(rt2x00dev, PLL_CTRL);
 		rt2x00_set_field32(&reg, PLL_CONTROL, 1);
 		rt2800_register_write(rt2x00dev, PLL_CTRL, reg);
 	}
@@ -5243,8 +5250,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 		if (rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
 		    rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E) ||
 		    rt2x00_rt_rev_lt(rt2x00dev, RT3390, REV_RT3390E)) {
-			rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1,
-					   &eeprom);
+			eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1);
 			if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF1_DAC_TEST))
 				rt2800_register_write(rt2x00dev, TX_SW_CFG2,
 						      0x0000002c);
@@ -5279,8 +5285,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 		rt2800_register_write(rt2x00dev, TX_SW_CFG0, 0x00000402);
 		rt2800_register_write(rt2x00dev, TX_SW_CFG1, 0x00000000);
 		if (rt2x00_rt_rev_lt(rt2x00dev, RT3593, REV_RT3593E)) {
-			rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1,
-					   &eeprom);
+			eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1);
 			if (rt2x00_get_field16(eeprom,
 					       EEPROM_NIC_CONF1_DAC_TEST))
 				rt2800_register_write(rt2x00dev, TX_SW_CFG2,
@@ -5319,7 +5324,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 				      0x3630363A);
 		rt2800_register_write(rt2x00dev, TX1_RF_GAIN_CORRECT,
 				      0x3630363A);
-		rt2800_register_read(rt2x00dev, TX_ALC_CFG_1, &reg);
+		reg = rt2800_register_read(rt2x00dev, TX_ALC_CFG_1);
 		rt2x00_set_field32(&reg, TX_ALC_CFG_1_ROS_BUSY_EN, 0);
 		rt2800_register_write(rt2x00dev, TX_ALC_CFG_1, reg);
 	} else {
@@ -5327,7 +5332,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 		rt2800_register_write(rt2x00dev, TX_SW_CFG1, 0x00080606);
 	}
 
-	rt2800_register_read(rt2x00dev, TX_LINK_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, TX_LINK_CFG);
 	rt2x00_set_field32(&reg, TX_LINK_CFG_REMOTE_MFB_LIFETIME, 32);
 	rt2x00_set_field32(&reg, TX_LINK_CFG_MFB_ENABLE, 0);
 	rt2x00_set_field32(&reg, TX_LINK_CFG_REMOTE_UMFS_ENABLE, 0);
@@ -5338,13 +5343,13 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, TX_LINK_CFG_REMOTE_MFS, 0);
 	rt2800_register_write(rt2x00dev, TX_LINK_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, TX_TIMEOUT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, TX_TIMEOUT_CFG);
 	rt2x00_set_field32(&reg, TX_TIMEOUT_CFG_MPDU_LIFETIME, 9);
 	rt2x00_set_field32(&reg, TX_TIMEOUT_CFG_RX_ACK_TIMEOUT, 32);
 	rt2x00_set_field32(&reg, TX_TIMEOUT_CFG_TX_OP_TIMEOUT, 10);
 	rt2800_register_write(rt2x00dev, TX_TIMEOUT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, MAX_LEN_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, MAX_LEN_CFG);
 	rt2x00_set_field32(&reg, MAX_LEN_CFG_MAX_MPDU, AGGREGATION_SIZE);
 	if (rt2x00_is_usb(rt2x00dev)) {
 		drv_data->max_psdu = 3;
@@ -5360,7 +5365,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, MAX_LEN_CFG_MIN_MPDU, 10);
 	rt2800_register_write(rt2x00dev, MAX_LEN_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, LED_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, LED_CFG);
 	rt2x00_set_field32(&reg, LED_CFG_ON_PERIOD, 70);
 	rt2x00_set_field32(&reg, LED_CFG_OFF_PERIOD, 30);
 	rt2x00_set_field32(&reg, LED_CFG_SLOW_BLINK_PERIOD, 3);
@@ -5372,7 +5377,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 
 	rt2800_register_write(rt2x00dev, PBF_MAX_PCNT, 0x1f3fbf9f);
 
-	rt2800_register_read(rt2x00dev, TX_RTY_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, TX_RTY_CFG);
 	rt2x00_set_field32(&reg, TX_RTY_CFG_SHORT_RTY_LIMIT, 2);
 	rt2x00_set_field32(&reg, TX_RTY_CFG_LONG_RTY_LIMIT, 2);
 	rt2x00_set_field32(&reg, TX_RTY_CFG_LONG_RTY_THRE, 2000);
@@ -5381,7 +5386,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, TX_RTY_CFG_TX_AUTO_FB_ENABLE, 1);
 	rt2800_register_write(rt2x00dev, TX_RTY_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, AUTO_RSP_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, AUTO_RSP_CFG);
 	rt2x00_set_field32(&reg, AUTO_RSP_CFG_AUTORESPONDER, 1);
 	rt2x00_set_field32(&reg, AUTO_RSP_CFG_BAC_ACK_POLICY, 1);
 	rt2x00_set_field32(&reg, AUTO_RSP_CFG_CTS_40_MMODE, 1);
@@ -5391,7 +5396,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, AUTO_RSP_CFG_ACK_CTS_PSM_BIT, 0);
 	rt2800_register_write(rt2x00dev, AUTO_RSP_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, CCK_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, CCK_PROT_CFG);
 	rt2x00_set_field32(&reg, CCK_PROT_CFG_PROTECT_RATE, 3);
 	rt2x00_set_field32(&reg, CCK_PROT_CFG_PROTECT_CTRL, 0);
 	rt2x00_set_field32(&reg, CCK_PROT_CFG_PROTECT_NAV_SHORT, 1);
@@ -5404,7 +5409,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, CCK_PROT_CFG_RTS_TH_EN, 1);
 	rt2800_register_write(rt2x00dev, CCK_PROT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, OFDM_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, OFDM_PROT_CFG);
 	rt2x00_set_field32(&reg, OFDM_PROT_CFG_PROTECT_RATE, 3);
 	rt2x00_set_field32(&reg, OFDM_PROT_CFG_PROTECT_CTRL, 0);
 	rt2x00_set_field32(&reg, OFDM_PROT_CFG_PROTECT_NAV_SHORT, 1);
@@ -5417,7 +5422,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, OFDM_PROT_CFG_RTS_TH_EN, 1);
 	rt2800_register_write(rt2x00dev, OFDM_PROT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, MM20_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, MM20_PROT_CFG);
 	rt2x00_set_field32(&reg, MM20_PROT_CFG_PROTECT_RATE, 0x4004);
 	rt2x00_set_field32(&reg, MM20_PROT_CFG_PROTECT_CTRL, 1);
 	rt2x00_set_field32(&reg, MM20_PROT_CFG_PROTECT_NAV_SHORT, 1);
@@ -5430,7 +5435,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, MM20_PROT_CFG_RTS_TH_EN, 0);
 	rt2800_register_write(rt2x00dev, MM20_PROT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, MM40_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, MM40_PROT_CFG);
 	rt2x00_set_field32(&reg, MM40_PROT_CFG_PROTECT_RATE, 0x4084);
 	rt2x00_set_field32(&reg, MM40_PROT_CFG_PROTECT_CTRL, 1);
 	rt2x00_set_field32(&reg, MM40_PROT_CFG_PROTECT_NAV_SHORT, 1);
@@ -5443,7 +5448,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, MM40_PROT_CFG_RTS_TH_EN, 0);
 	rt2800_register_write(rt2x00dev, MM40_PROT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, GF20_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, GF20_PROT_CFG);
 	rt2x00_set_field32(&reg, GF20_PROT_CFG_PROTECT_RATE, 0x4004);
 	rt2x00_set_field32(&reg, GF20_PROT_CFG_PROTECT_CTRL, 1);
 	rt2x00_set_field32(&reg, GF20_PROT_CFG_PROTECT_NAV_SHORT, 1);
@@ -5456,7 +5461,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, GF20_PROT_CFG_RTS_TH_EN, 0);
 	rt2800_register_write(rt2x00dev, GF20_PROT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, GF40_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, GF40_PROT_CFG);
 	rt2x00_set_field32(&reg, GF40_PROT_CFG_PROTECT_RATE, 0x4084);
 	rt2x00_set_field32(&reg, GF40_PROT_CFG_PROTECT_CTRL, 1);
 	rt2x00_set_field32(&reg, GF40_PROT_CFG_PROTECT_NAV_SHORT, 1);
@@ -5472,7 +5477,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	if (rt2x00_is_usb(rt2x00dev)) {
 		rt2800_register_write(rt2x00dev, PBF_CFG, 0xf40006);
 
-		rt2800_register_read(rt2x00dev, WPDMA_GLO_CFG, &reg);
+		reg = rt2800_register_read(rt2x00dev, WPDMA_GLO_CFG);
 		rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_TX_DMA, 0);
 		rt2x00_set_field32(&reg, WPDMA_GLO_CFG_TX_DMA_BUSY, 0);
 		rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_RX_DMA, 0);
@@ -5489,7 +5494,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	 * The legacy driver also sets TXOP_CTRL_CFG_RESERVED_TRUN_EN to 1
 	 * although it is reserved.
 	 */
-	rt2800_register_read(rt2x00dev, TXOP_CTRL_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, TXOP_CTRL_CFG);
 	rt2x00_set_field32(&reg, TXOP_CTRL_CFG_TIMEOUT_TRUN_EN, 1);
 	rt2x00_set_field32(&reg, TXOP_CTRL_CFG_AC_TRUN_EN, 1);
 	rt2x00_set_field32(&reg, TXOP_CTRL_CFG_TXRATEGRP_TRUN_EN, 1);
@@ -5505,7 +5510,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	reg = rt2x00_rt(rt2x00dev, RT5592) ? 0x00000082 : 0x00000002;
 	rt2800_register_write(rt2x00dev, TXOP_HLDR_ET, reg);
 
-	rt2800_register_read(rt2x00dev, TX_RTS_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, TX_RTS_CFG);
 	rt2x00_set_field32(&reg, TX_RTS_CFG_AUTO_RTS_RETRY_LIMIT, 7);
 	rt2x00_set_field32(&reg, TX_RTS_CFG_RTS_THRES,
 			   IEEE80211_MAX_RTS_THRESHOLD);
@@ -5521,7 +5526,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	 * connection problems with 11g + CTS protection. Hence, use the same
 	 * defaults as the Ralink driver: 16 for both, CCK and OFDM SIFS.
 	 */
-	rt2800_register_read(rt2x00dev, XIFS_TIME_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, XIFS_TIME_CFG);
 	rt2x00_set_field32(&reg, XIFS_TIME_CFG_CCKM_SIFS_TIME, 16);
 	rt2x00_set_field32(&reg, XIFS_TIME_CFG_OFDM_SIFS_TIME, 16);
 	rt2x00_set_field32(&reg, XIFS_TIME_CFG_OFDM_XIFS_TIME, 4);
@@ -5551,16 +5556,16 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 		rt2800_clear_beacon_register(rt2x00dev, i);
 
 	if (rt2x00_is_usb(rt2x00dev)) {
-		rt2800_register_read(rt2x00dev, US_CYC_CNT, &reg);
+		reg = rt2800_register_read(rt2x00dev, US_CYC_CNT);
 		rt2x00_set_field32(&reg, US_CYC_CNT_CLOCK_CYCLE, 30);
 		rt2800_register_write(rt2x00dev, US_CYC_CNT, reg);
 	} else if (rt2x00_is_pcie(rt2x00dev)) {
-		rt2800_register_read(rt2x00dev, US_CYC_CNT, &reg);
+		reg = rt2800_register_read(rt2x00dev, US_CYC_CNT);
 		rt2x00_set_field32(&reg, US_CYC_CNT_CLOCK_CYCLE, 125);
 		rt2800_register_write(rt2x00dev, US_CYC_CNT, reg);
 	}
 
-	rt2800_register_read(rt2x00dev, HT_FBK_CFG0, &reg);
+	reg = rt2800_register_read(rt2x00dev, HT_FBK_CFG0);
 	rt2x00_set_field32(&reg, HT_FBK_CFG0_HTMCS0FBK, 0);
 	rt2x00_set_field32(&reg, HT_FBK_CFG0_HTMCS1FBK, 0);
 	rt2x00_set_field32(&reg, HT_FBK_CFG0_HTMCS2FBK, 1);
@@ -5571,7 +5576,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, HT_FBK_CFG0_HTMCS7FBK, 6);
 	rt2800_register_write(rt2x00dev, HT_FBK_CFG0, reg);
 
-	rt2800_register_read(rt2x00dev, HT_FBK_CFG1, &reg);
+	reg = rt2800_register_read(rt2x00dev, HT_FBK_CFG1);
 	rt2x00_set_field32(&reg, HT_FBK_CFG1_HTMCS8FBK, 8);
 	rt2x00_set_field32(&reg, HT_FBK_CFG1_HTMCS9FBK, 8);
 	rt2x00_set_field32(&reg, HT_FBK_CFG1_HTMCS10FBK, 9);
@@ -5582,7 +5587,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, HT_FBK_CFG1_HTMCS15FBK, 14);
 	rt2800_register_write(rt2x00dev, HT_FBK_CFG1, reg);
 
-	rt2800_register_read(rt2x00dev, LG_FBK_CFG0, &reg);
+	reg = rt2800_register_read(rt2x00dev, LG_FBK_CFG0);
 	rt2x00_set_field32(&reg, LG_FBK_CFG0_OFDMMCS0FBK, 8);
 	rt2x00_set_field32(&reg, LG_FBK_CFG0_OFDMMCS1FBK, 8);
 	rt2x00_set_field32(&reg, LG_FBK_CFG0_OFDMMCS2FBK, 9);
@@ -5593,7 +5598,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, LG_FBK_CFG0_OFDMMCS7FBK, 14);
 	rt2800_register_write(rt2x00dev, LG_FBK_CFG0, reg);
 
-	rt2800_register_read(rt2x00dev, LG_FBK_CFG1, &reg);
+	reg = rt2800_register_read(rt2x00dev, LG_FBK_CFG1);
 	rt2x00_set_field32(&reg, LG_FBK_CFG0_CCKMCS0FBK, 0);
 	rt2x00_set_field32(&reg, LG_FBK_CFG0_CCKMCS1FBK, 0);
 	rt2x00_set_field32(&reg, LG_FBK_CFG0_CCKMCS2FBK, 1);
@@ -5603,7 +5608,7 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Do not force the BA window size, we use the TXWI to set it
 	 */
-	rt2800_register_read(rt2x00dev, AMPDU_BA_WINSIZE, &reg);
+	reg = rt2800_register_read(rt2x00dev, AMPDU_BA_WINSIZE);
 	rt2x00_set_field32(&reg, AMPDU_BA_WINSIZE_FORCE_WINSIZE_ENABLE, 0);
 	rt2x00_set_field32(&reg, AMPDU_BA_WINSIZE_FORCE_WINSIZE, 0);
 	rt2800_register_write(rt2x00dev, AMPDU_BA_WINSIZE, reg);
@@ -5613,24 +5618,24 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	 * These registers are cleared on read,
 	 * so we may pass a useless variable to store the value.
 	 */
-	rt2800_register_read(rt2x00dev, RX_STA_CNT0, &reg);
-	rt2800_register_read(rt2x00dev, RX_STA_CNT1, &reg);
-	rt2800_register_read(rt2x00dev, RX_STA_CNT2, &reg);
-	rt2800_register_read(rt2x00dev, TX_STA_CNT0, &reg);
-	rt2800_register_read(rt2x00dev, TX_STA_CNT1, &reg);
-	rt2800_register_read(rt2x00dev, TX_STA_CNT2, &reg);
+	reg = rt2800_register_read(rt2x00dev, RX_STA_CNT0);
+	reg = rt2800_register_read(rt2x00dev, RX_STA_CNT1);
+	reg = rt2800_register_read(rt2x00dev, RX_STA_CNT2);
+	reg = rt2800_register_read(rt2x00dev, TX_STA_CNT0);
+	reg = rt2800_register_read(rt2x00dev, TX_STA_CNT1);
+	reg = rt2800_register_read(rt2x00dev, TX_STA_CNT2);
 
 	/*
 	 * Setup leadtime for pre tbtt interrupt to 6ms
 	 */
-	rt2800_register_read(rt2x00dev, INT_TIMER_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, INT_TIMER_CFG);
 	rt2x00_set_field32(&reg, INT_TIMER_CFG_PRE_TBTT_TIMER, 6 << 4);
 	rt2800_register_write(rt2x00dev, INT_TIMER_CFG, reg);
 
 	/*
 	 * Set up channel statistics timer
 	 */
-	rt2800_register_read(rt2x00dev, CH_TIME_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, CH_TIME_CFG);
 	rt2x00_set_field32(&reg, CH_TIME_CFG_EIFS_BUSY, 1);
 	rt2x00_set_field32(&reg, CH_TIME_CFG_NAV_BUSY, 1);
 	rt2x00_set_field32(&reg, CH_TIME_CFG_RX_BUSY, 1);
@@ -5647,7 +5652,7 @@ static int rt2800_wait_bbp_rf_ready(struct rt2x00_dev *rt2x00dev)
 	u32 reg;
 
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2800_register_read(rt2x00dev, MAC_STATUS_CFG, &reg);
+		reg = rt2800_register_read(rt2x00dev, MAC_STATUS_CFG);
 		if (!rt2x00_get_field32(reg, MAC_STATUS_CFG_BBP_RF_BUSY))
 			return 0;
 
@@ -5672,7 +5677,7 @@ static int rt2800_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 	msleep(1);
 
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2800_bbp_read(rt2x00dev, 0, &value);
+		value = rt2800_bbp_read(rt2x00dev, 0);
 		if ((value != 0xff) && (value != 0x00))
 			return 0;
 		udelay(REGISTER_BUSY_DELAY);
@@ -5686,7 +5691,7 @@ static void rt2800_bbp4_mac_if_ctrl(struct rt2x00_dev *rt2x00dev)
 {
 	u8 value;
 
-	rt2800_bbp_read(rt2x00dev, 4, &value);
+	value = rt2800_bbp_read(rt2x00dev, 4);
 	rt2x00_set_field8(&value, BBP4_MAC_IF_CTRL, 1);
 	rt2800_bbp_write(rt2x00dev, 4, value);
 }
@@ -5743,8 +5748,8 @@ static void rt2800_disable_unused_dac_adc(struct rt2x00_dev *rt2x00dev)
 	u16 eeprom;
 	u8 value;
 
-	rt2800_bbp_read(rt2x00dev, 138, &value);
-	rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &eeprom);
+	value = rt2800_bbp_read(rt2x00dev, 138);
+	eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0);
 	if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_TXPATH) == 1)
 		value |= 0x20;
 	if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RXPATH) == 1)
@@ -5927,12 +5932,12 @@ static void rt2800_init_bbp_3290(struct rt2x00_dev *rt2x00dev)
 	rt2800_bbp_write(rt2x00dev, 155, 0x3b);
 	rt2800_bbp_write(rt2x00dev, 253, 0x04);
 
-	rt2800_bbp_read(rt2x00dev, 47, &value);
+	value = rt2800_bbp_read(rt2x00dev, 47);
 	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);
+	value = rt2800_bbp_read(rt2x00dev, 3);
 	rt2x00_set_field8(&value, BBP3_ADC_MODE_SWITCH, 1);
 	rt2x00_set_field8(&value, BBP3_ADC_INIT_MODE, 1);
 	rt2800_bbp_write(rt2x00dev, 3, value);
@@ -6191,7 +6196,7 @@ static void rt2800_init_bbp_53xx(struct rt2x00_dev *rt2x00dev)
 
 	rt2800_disable_unused_dac_adc(rt2x00dev);
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &eeprom);
+	eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1);
 	div_mode = rt2x00_get_field16(eeprom,
 				      EEPROM_NIC_CONF1_ANT_DIVERSITY);
 	ant = (div_mode == 3) ? 1 : 0;
@@ -6200,7 +6205,7 @@ static void rt2800_init_bbp_53xx(struct rt2x00_dev *rt2x00dev)
 	if (rt2x00_has_cap_bt_coexist(rt2x00dev)) {
 		u32 reg;
 
-		rt2800_register_read(rt2x00dev, GPIO_CTRL, &reg);
+		reg = rt2800_register_read(rt2x00dev, GPIO_CTRL);
 		rt2x00_set_field32(&reg, GPIO_CTRL_DIR3, 0);
 		rt2x00_set_field32(&reg, GPIO_CTRL_DIR6, 0);
 		rt2x00_set_field32(&reg, GPIO_CTRL_VAL3, 0);
@@ -6219,7 +6224,7 @@ static void rt2800_init_bbp_53xx(struct rt2x00_dev *rt2x00dev)
 		rt2800_bbp_write(rt2x00dev, 154, 0); /* Clear previously selected antenna */
 	}
 
-	rt2800_bbp_read(rt2x00dev, 152, &value);
+	value = rt2800_bbp_read(rt2x00dev, 152);
 	if (ant == 0)
 		rt2x00_set_field8(&value, BBP152_RX_DEFAULT_ANT, 1);
 	else
@@ -6237,7 +6242,7 @@ static void rt2800_init_bbp_5592(struct rt2x00_dev *rt2x00dev)
 
 	rt2800_init_bbp_early(rt2x00dev);
 
-	rt2800_bbp_read(rt2x00dev, 105, &value);
+	value = rt2800_bbp_read(rt2x00dev, 105);
 	rt2x00_set_field8(&value, BBP105_MLD,
 			  rt2x00dev->default_ant.rx_chain_num == 2);
 	rt2800_bbp_write(rt2x00dev, 105, value);
@@ -6277,10 +6282,10 @@ static void rt2800_init_bbp_5592(struct rt2x00_dev *rt2x00dev)
 
 	rt2800_bbp4_mac_if_ctrl(rt2x00dev);
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &eeprom);
+	eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1);
 	div_mode = rt2x00_get_field16(eeprom, EEPROM_NIC_CONF1_ANT_DIVERSITY);
 	ant = (div_mode == 3) ? 1 : 0;
-	rt2800_bbp_read(rt2x00dev, 152, &value);
+	value = rt2800_bbp_read(rt2x00dev, 152);
 	if (ant == 0) {
 		/* Main antenna */
 		rt2x00_set_field8(&value, BBP152_RX_DEFAULT_ANT, 1);
@@ -6291,7 +6296,7 @@ static void rt2800_init_bbp_5592(struct rt2x00_dev *rt2x00dev)
 	rt2800_bbp_write(rt2x00dev, 152, value);
 
 	if (rt2x00_rt_rev_gte(rt2x00dev, RT5592, REV_RT5592C)) {
-		rt2800_bbp_read(rt2x00dev, 254, &value);
+		value = rt2800_bbp_read(rt2x00dev, 254);
 		rt2x00_set_field8(&value, BBP254_BIT7, 1);
 		rt2800_bbp_write(rt2x00dev, 254, value);
 	}
@@ -6317,11 +6322,10 @@ static void rt2800_bbp_dcoc_write(struct rt2x00_dev *rt2x00dev,
 	rt2800_bbp_write(rt2x00dev, 159, value);
 }
 
-static void rt2800_bbp_dcoc_read(struct rt2x00_dev *rt2x00dev,
-				 const u8 reg, u8 *value)
+static u8 rt2800_bbp_dcoc_read(struct rt2x00_dev *rt2x00dev, const u8 reg)
 {
 	rt2800_bbp_write(rt2x00dev, 158, reg);
-	rt2800_bbp_read(rt2x00dev, 159, value);
+	return rt2800_bbp_read(rt2x00dev, 159);
 }
 
 static void rt2800_init_bbp_6352(struct rt2x00_dev *rt2x00dev)
@@ -6329,7 +6333,7 @@ static void rt2800_init_bbp_6352(struct rt2x00_dev *rt2x00dev)
 	u8 bbp;
 
 	/* Apply Maximum Likelihood Detection (MLD) for 2 stream case */
-	rt2800_bbp_read(rt2x00dev, 105, &bbp);
+	bbp = rt2800_bbp_read(rt2x00dev, 105);
 	rt2x00_set_field8(&bbp, BBP105_MLD,
 			  rt2x00dev->default_ant.rx_chain_num == 2);
 	rt2800_bbp_write(rt2x00dev, 105, bbp);
@@ -6338,7 +6342,7 @@ static void rt2800_init_bbp_6352(struct rt2x00_dev *rt2x00dev)
 	rt2800_bbp4_mac_if_ctrl(rt2x00dev);
 
 	/* Fix I/Q swap issue */
-	rt2800_bbp_read(rt2x00dev, 1, &bbp);
+	bbp = rt2800_bbp_read(rt2x00dev, 1);
 	bbp |= 0x04;
 	rt2800_bbp_write(rt2x00dev, 1, bbp);
 
@@ -6578,8 +6582,8 @@ static void rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 	}
 
 	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
-		rt2800_eeprom_read_from_array(rt2x00dev, EEPROM_BBP_START, i,
-					      &eeprom);
+		eeprom = rt2800_eeprom_read_from_array(rt2x00dev,
+						       EEPROM_BBP_START, i);
 
 		if (eeprom != 0xffff && eeprom != 0x0000) {
 			reg_id = rt2x00_get_field16(eeprom, EEPROM_BBP_REG_ID);
@@ -6593,7 +6597,7 @@ static void rt2800_led_open_drain_enable(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2800_register_read(rt2x00dev, OPT_14_CSR, &reg);
+	reg = rt2800_register_read(rt2x00dev, OPT_14_CSR);
 	rt2x00_set_field32(&reg, OPT_14_CSR_BIT0, 1);
 	rt2800_register_write(rt2x00dev, OPT_14_CSR, reg);
 }
@@ -6611,15 +6615,15 @@ static u8 rt2800_init_rx_filter(struct rt2x00_dev *rt2x00dev, bool bw40,
 
 	rt2800_rfcsr_write(rt2x00dev, 24, rfcsr24);
 
-	rt2800_bbp_read(rt2x00dev, 4, &bbp);
+	bbp = rt2800_bbp_read(rt2x00dev, 4);
 	rt2x00_set_field8(&bbp, BBP4_BANDWIDTH, 2 * bw40);
 	rt2800_bbp_write(rt2x00dev, 4, bbp);
 
-	rt2800_rfcsr_read(rt2x00dev, 31, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 31);
 	rt2x00_set_field8(&rfcsr, RFCSR31_RX_H20M, bw40);
 	rt2800_rfcsr_write(rt2x00dev, 31, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 22, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 22);
 	rt2x00_set_field8(&rfcsr, RFCSR22_BASEBAND_LOOPBACK, 1);
 	rt2800_rfcsr_write(rt2x00dev, 22, rfcsr);
 
@@ -6632,7 +6636,7 @@ static u8 rt2800_init_rx_filter(struct rt2x00_dev *rt2x00dev, bool bw40,
 		rt2800_bbp_write(rt2x00dev, 25, 0x90);
 		msleep(1);
 
-		rt2800_bbp_read(rt2x00dev, 55, &passband);
+		passband = rt2800_bbp_read(rt2x00dev, 55);
 		if (passband)
 			break;
 	}
@@ -6646,7 +6650,7 @@ static u8 rt2800_init_rx_filter(struct rt2x00_dev *rt2x00dev, bool bw40,
 		rt2800_bbp_write(rt2x00dev, 25, 0x90);
 		msleep(1);
 
-		rt2800_bbp_read(rt2x00dev, 55, &stopband);
+		stopband = rt2800_bbp_read(rt2x00dev, 55);
 
 		if ((passband - stopband) <= filter_target) {
 			rfcsr24++;
@@ -6668,7 +6672,7 @@ static void rt2800_rf_init_calibration(struct rt2x00_dev *rt2x00dev,
 {
 	u8 rfcsr;
 
-	rt2800_rfcsr_read(rt2x00dev, rf_reg, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, rf_reg);
 	rt2x00_set_field8(&rfcsr, FIELD8(0x80), 1);
 	rt2800_rfcsr_write(rt2x00dev, rf_reg, rfcsr);
 	msleep(1);
@@ -6702,22 +6706,22 @@ static void rt2800_rx_filter_calibration(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Save BBP 25 & 26 values for later use in channel switching (for 3052)
 	 */
-	rt2800_bbp_read(rt2x00dev, 25, &drv_data->bbp25);
-	rt2800_bbp_read(rt2x00dev, 26, &drv_data->bbp26);
+	drv_data->bbp25 = rt2800_bbp_read(rt2x00dev, 25);
+	drv_data->bbp26 = rt2800_bbp_read(rt2x00dev, 26);
 
 	/*
 	 * Set back to initial state
 	 */
 	rt2800_bbp_write(rt2x00dev, 24, 0);
 
-	rt2800_rfcsr_read(rt2x00dev, 22, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 22);
 	rt2x00_set_field8(&rfcsr, RFCSR22_BASEBAND_LOOPBACK, 0);
 	rt2800_rfcsr_write(rt2x00dev, 22, rfcsr);
 
 	/*
 	 * Set BBP back to BW20
 	 */
-	rt2800_bbp_read(rt2x00dev, 4, &bbp);
+	bbp = rt2800_bbp_read(rt2x00dev, 4);
 	rt2x00_set_field8(&bbp, BBP4_BANDWIDTH, 0);
 	rt2800_bbp_write(rt2x00dev, 4, bbp);
 }
@@ -6728,7 +6732,7 @@ static void rt2800_normal_mode_setup_3xxx(struct rt2x00_dev *rt2x00dev)
 	u8 min_gain, rfcsr, bbp;
 	u16 eeprom;
 
-	rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 17);
 
 	rt2x00_set_field8(&rfcsr, RFCSR17_TX_LO1_EN, 0);
 	if (rt2x00_rt(rt2x00dev, RT3070) ||
@@ -6749,8 +6753,8 @@ static void rt2800_normal_mode_setup_3xxx(struct rt2x00_dev *rt2x00dev)
 
 	if (rt2x00_rt(rt2x00dev, RT3090)) {
 		/*  Turn off unused DAC1 and ADC1 to reduce power consumption */
-		rt2800_bbp_read(rt2x00dev, 138, &bbp);
-		rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &eeprom);
+		bbp = rt2800_bbp_read(rt2x00dev, 138);
+		eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0);
 		if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RXPATH) == 1)
 			rt2x00_set_field8(&bbp, BBP138_RX_ADC1, 0);
 		if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_TXPATH) == 1)
@@ -6759,7 +6763,7 @@ static void rt2800_normal_mode_setup_3xxx(struct rt2x00_dev *rt2x00dev)
 	}
 
 	if (rt2x00_rt(rt2x00dev, RT3070)) {
-		rt2800_rfcsr_read(rt2x00dev, 27, &rfcsr);
+		rfcsr = rt2800_rfcsr_read(rt2x00dev, 27);
 		if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F))
 			rt2x00_set_field8(&rfcsr, RFCSR27_R1, 3);
 		else
@@ -6771,7 +6775,7 @@ static void rt2800_normal_mode_setup_3xxx(struct rt2x00_dev *rt2x00dev)
 	} else if (rt2x00_rt(rt2x00dev, RT3071) ||
 		   rt2x00_rt(rt2x00dev, RT3090) ||
 		   rt2x00_rt(rt2x00dev, RT3390)) {
-		rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+		rfcsr = rt2800_rfcsr_read(rt2x00dev, 1);
 		rt2x00_set_field8(&rfcsr, RFCSR1_RF_BLOCK_EN, 1);
 		rt2x00_set_field8(&rfcsr, RFCSR1_RX0_PD, 0);
 		rt2x00_set_field8(&rfcsr, RFCSR1_TX0_PD, 0);
@@ -6779,15 +6783,15 @@ static void rt2800_normal_mode_setup_3xxx(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field8(&rfcsr, RFCSR1_TX1_PD, 1);
 		rt2800_rfcsr_write(rt2x00dev, 1, rfcsr);
 
-		rt2800_rfcsr_read(rt2x00dev, 15, &rfcsr);
+		rfcsr = rt2800_rfcsr_read(rt2x00dev, 15);
 		rt2x00_set_field8(&rfcsr, RFCSR15_TX_LO2_EN, 0);
 		rt2800_rfcsr_write(rt2x00dev, 15, rfcsr);
 
-		rt2800_rfcsr_read(rt2x00dev, 20, &rfcsr);
+		rfcsr = rt2800_rfcsr_read(rt2x00dev, 20);
 		rt2x00_set_field8(&rfcsr, RFCSR20_RX_LO1_EN, 0);
 		rt2800_rfcsr_write(rt2x00dev, 20, rfcsr);
 
-		rt2800_rfcsr_read(rt2x00dev, 21, &rfcsr);
+		rfcsr = rt2800_rfcsr_read(rt2x00dev, 21);
 		rt2x00_set_field8(&rfcsr, RFCSR21_RX_LO2_EN, 0);
 		rt2800_rfcsr_write(rt2x00dev, 21, rfcsr);
 	}
@@ -6799,30 +6803,30 @@ static void rt2800_normal_mode_setup_3593(struct rt2x00_dev *rt2x00dev)
 	u8 rfcsr;
 	u8 tx_gain;
 
-	rt2800_rfcsr_read(rt2x00dev, 50, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 50);
 	rt2x00_set_field8(&rfcsr, RFCSR50_TX_LO2_EN, 0);
 	rt2800_rfcsr_write(rt2x00dev, 50, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 51, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 51);
 	tx_gain = rt2x00_get_field8(drv_data->txmixer_gain_24g,
 				    RFCSR17_TXMIXER_GAIN);
 	rt2x00_set_field8(&rfcsr, RFCSR51_BITS24, tx_gain);
 	rt2800_rfcsr_write(rt2x00dev, 51, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 38, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 38);
 	rt2x00_set_field8(&rfcsr, RFCSR38_RX_LO1_EN, 0);
 	rt2800_rfcsr_write(rt2x00dev, 38, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 39, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 39);
 	rt2x00_set_field8(&rfcsr, RFCSR39_RX_LO2_EN, 0);
 	rt2800_rfcsr_write(rt2x00dev, 39, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 1);
 	rt2x00_set_field8(&rfcsr, RFCSR1_RF_BLOCK_EN, 1);
 	rt2x00_set_field8(&rfcsr, RFCSR1_PLL_PD, 1);
 	rt2800_rfcsr_write(rt2x00dev, 1, rfcsr);
 
-	rt2800_rfcsr_read(rt2x00dev, 30, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 30);
 	rt2x00_set_field8(&rfcsr, RFCSR30_RX_VCM, 2);
 	rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
 
@@ -6835,25 +6839,25 @@ static void rt2800_normal_mode_setup_5xxx(struct rt2x00_dev *rt2x00dev)
 	u16 eeprom;
 
 	/*  Turn off unused DAC1 and ADC1 to reduce power consumption */
-	rt2800_bbp_read(rt2x00dev, 138, &reg);
-	rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &eeprom);
+	reg = rt2800_bbp_read(rt2x00dev, 138);
+	eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0);
 	if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RXPATH) == 1)
 		rt2x00_set_field8(&reg, BBP138_RX_ADC1, 0);
 	if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_TXPATH) == 1)
 		rt2x00_set_field8(&reg, BBP138_TX_DAC1, 1);
 	rt2800_bbp_write(rt2x00dev, 138, reg);
 
-	rt2800_rfcsr_read(rt2x00dev, 38, &reg);
+	reg = rt2800_rfcsr_read(rt2x00dev, 38);
 	rt2x00_set_field8(&reg, RFCSR38_RX_LO1_EN, 0);
 	rt2800_rfcsr_write(rt2x00dev, 38, reg);
 
-	rt2800_rfcsr_read(rt2x00dev, 39, &reg);
+	reg = rt2800_rfcsr_read(rt2x00dev, 39);
 	rt2x00_set_field8(&reg, RFCSR39_RX_LO2_EN, 0);
 	rt2800_rfcsr_write(rt2x00dev, 39, reg);
 
 	rt2800_bbp4_mac_if_ctrl(rt2x00dev);
 
-	rt2800_rfcsr_read(rt2x00dev, 30, &reg);
+	reg = rt2800_rfcsr_read(rt2x00dev, 30);
 	rt2x00_set_field8(&reg, RFCSR30_RX_VCM, 2);
 	rt2800_rfcsr_write(rt2x00dev, 30, reg);
 }
@@ -6926,7 +6930,7 @@ static void rt2800_init_rfcsr_30xx(struct rt2x00_dev *rt2x00dev)
 	rt2800_rfcsr_write(rt2x00dev, 29, 0x1f);
 
 	if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F)) {
-		rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
+		reg = rt2800_register_read(rt2x00dev, LDO_CFG0);
 		rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
 		rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 3);
 		rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
@@ -6934,16 +6938,15 @@ static void rt2800_init_rfcsr_30xx(struct rt2x00_dev *rt2x00dev)
 		   rt2x00_rt(rt2x00dev, RT3090)) {
 		rt2800_rfcsr_write(rt2x00dev, 31, 0x14);
 
-		rt2800_rfcsr_read(rt2x00dev, 6, &rfcsr);
+		rfcsr = rt2800_rfcsr_read(rt2x00dev, 6);
 		rt2x00_set_field8(&rfcsr, RFCSR6_R2, 1);
 		rt2800_rfcsr_write(rt2x00dev, 6, rfcsr);
 
-		rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
+		reg = rt2800_register_read(rt2x00dev, LDO_CFG0);
 		rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
 		if (rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
 		    rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E)) {
-			rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1,
-					   &eeprom);
+			eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1);
 			if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF1_DAC_TEST))
 				rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 3);
 			else
@@ -6951,7 +6954,7 @@ static void rt2800_init_rfcsr_30xx(struct rt2x00_dev *rt2x00dev)
 		}
 		rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
 
-		rt2800_register_read(rt2x00dev, GPIO_SWITCH, &reg);
+		reg = rt2800_register_read(rt2x00dev, GPIO_SWITCH);
 		rt2x00_set_field32(&reg, GPIO_SWITCH_5, 0);
 		rt2800_register_write(rt2x00dev, GPIO_SWITCH, reg);
 	}
@@ -7020,7 +7023,7 @@ static void rt2800_init_rfcsr_3290(struct rt2x00_dev *rt2x00dev)
 	rt2800_rfcsr_write(rt2x00dev, 60, 0x45);
 	rt2800_rfcsr_write(rt2x00dev, 61, 0xc1);
 
-	rt2800_rfcsr_read(rt2x00dev, 29, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 29);
 	rt2x00_set_field8(&rfcsr, RFCSR29_RSSI_GAIN, 3);
 	rt2800_rfcsr_write(rt2x00dev, 29, rfcsr);
 
@@ -7166,7 +7169,7 @@ static void rt2800_init_rfcsr_3390(struct rt2x00_dev *rt2x00dev)
 	rt2800_rfcsr_write(rt2x00dev, 30, 0x20);
 	rt2800_rfcsr_write(rt2x00dev, 31, 0x0f);
 
-	rt2800_register_read(rt2x00dev, GPIO_SWITCH, &reg);
+	reg = rt2800_register_read(rt2x00dev, GPIO_SWITCH);
 	rt2x00_set_field32(&reg, GPIO_SWITCH_5, 0);
 	rt2800_register_write(rt2x00dev, GPIO_SWITCH, reg);
 
@@ -7218,16 +7221,16 @@ static void rt2800_init_rfcsr_3572(struct rt2x00_dev *rt2x00dev)
 	rt2800_rfcsr_write(rt2x00dev, 30, 0x09);
 	rt2800_rfcsr_write(rt2x00dev, 31, 0x10);
 
-	rt2800_rfcsr_read(rt2x00dev, 6, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 6);
 	rt2x00_set_field8(&rfcsr, RFCSR6_R2, 1);
 	rt2800_rfcsr_write(rt2x00dev, 6, rfcsr);
 
-	rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
+	reg = rt2800_register_read(rt2x00dev, LDO_CFG0);
 	rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 3);
 	rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
 	rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
 	msleep(1);
-	rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
+	reg = rt2800_register_read(rt2x00dev, LDO_CFG0);
 	rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 0);
 	rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
 	rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
@@ -7242,7 +7245,7 @@ static void rt3593_post_bbp_init(struct rt2x00_dev *rt2x00dev)
 	u8 bbp;
 	bool txbf_enabled = false; /* FIXME */
 
-	rt2800_bbp_read(rt2x00dev, 105, &bbp);
+	bbp = rt2800_bbp_read(rt2x00dev, 105);
 	if (rt2x00dev->default_ant.rx_chain_num == 1)
 		rt2x00_set_field8(&bbp, BBP105_MLD, 0);
 	else
@@ -7291,7 +7294,7 @@ static void rt2800_init_rfcsr_3593(struct rt2x00_dev *rt2x00dev)
 	u8 rfcsr;
 
 	/* Disable GPIO #4 and #7 function for LAN PE control */
-	rt2800_register_read(rt2x00dev, GPIO_SWITCH, &reg);
+	reg = rt2800_register_read(rt2x00dev, GPIO_SWITCH);
 	rt2x00_set_field32(&reg, GPIO_SWITCH_4, 0);
 	rt2x00_set_field32(&reg, GPIO_SWITCH_7, 0);
 	rt2800_register_write(rt2x00dev, GPIO_SWITCH, reg);
@@ -7332,22 +7335,22 @@ static void rt2800_init_rfcsr_3593(struct rt2x00_dev *rt2x00dev)
 
 	/* Initiate calibration */
 	/* TODO: use rt2800_rf_init_calibration ? */
-	rt2800_rfcsr_read(rt2x00dev, 2, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 2);
 	rt2x00_set_field8(&rfcsr, RFCSR2_RESCAL_EN, 1);
 	rt2800_rfcsr_write(rt2x00dev, 2, rfcsr);
 
 	rt2800_freq_cal_mode1(rt2x00dev);
 
-	rt2800_rfcsr_read(rt2x00dev, 18, &rfcsr);
+	rfcsr = rt2800_rfcsr_read(rt2x00dev, 18);
 	rt2x00_set_field8(&rfcsr, RFCSR18_XO_TUNE_BYPASS, 1);
 	rt2800_rfcsr_write(rt2x00dev, 18, rfcsr);
 
-	rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
+	reg = rt2800_register_read(rt2x00dev, LDO_CFG0);
 	rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 3);
 	rt2x00_set_field32(&reg, LDO_CFG0_BGSEL, 1);
 	rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
 	usleep_range(1000, 1500);
-	rt2800_register_read(rt2x00dev, LDO_CFG0, &reg);
+	reg = rt2800_register_read(rt2x00dev, LDO_CFG0);
 	rt2x00_set_field32(&reg, LDO_CFG0_LDO_CORE_VLEVEL, 0);
 	rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
 
@@ -7356,8 +7359,8 @@ static void rt2800_init_rfcsr_3593(struct rt2x00_dev *rt2x00dev)
 	drv_data->calibration_bw40 = 0x2f;
 
 	/* Save BBP 25 & 26 values for later use in channel switching */
-	rt2800_bbp_read(rt2x00dev, 25, &drv_data->bbp25);
-	rt2800_bbp_read(rt2x00dev, 26, &drv_data->bbp26);
+	drv_data->bbp25 = rt2800_bbp_read(rt2x00dev, 25);
+	drv_data->bbp26 = rt2800_bbp_read(rt2x00dev, 26);
 
 	rt2800_led_open_drain_enable(rt2x00dev);
 	rt2800_normal_mode_setup_3593(rt2x00dev);
@@ -7651,19 +7654,19 @@ static void rt2800_bbp_core_soft_reset(struct rt2x00_dev *rt2x00dev,
 {
 	u8 bbp_val;
 
-	rt2800_bbp_read(rt2x00dev, 21, &bbp_val);
+	bbp_val = rt2800_bbp_read(rt2x00dev, 21);
 	bbp_val |= 0x1;
 	rt2800_bbp_write(rt2x00dev, 21, bbp_val);
 	usleep_range(100, 200);
 
 	if (set_bw) {
-		rt2800_bbp_read(rt2x00dev, 4, &bbp_val);
+		bbp_val = rt2800_bbp_read(rt2x00dev, 4);
 		rt2x00_set_field8(&bbp_val, BBP4_BANDWIDTH, 2 * is_ht40);
 		rt2800_bbp_write(rt2x00dev, 4, bbp_val);
 		usleep_range(100, 200);
 	}
 
-	rt2800_bbp_read(rt2x00dev, 21, &bbp_val);
+	bbp_val = rt2800_bbp_read(rt2x00dev, 21);
 	bbp_val &= (~0x1);
 	rt2800_bbp_write(rt2x00dev, 21, bbp_val);
 	usleep_range(100, 200);
@@ -7680,7 +7683,7 @@ static int rt2800_rf_lp_config(struct rt2x00_dev *rt2x00dev, bool btxcal)
 
 	rt2800_register_write(rt2x00dev, RF_BYPASS0, 0x06);
 
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 17, &rf_val);
+	rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 17);
 	rf_val |= 0x80;
 	rt2800_rfcsr_write_bank(rt2x00dev, 5, 17, rf_val);
 
@@ -7688,11 +7691,11 @@ static int rt2800_rf_lp_config(struct rt2x00_dev *rt2x00dev, bool btxcal)
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 18, 0xC1);
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 19, 0x20);
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 20, 0x02);
-		rt2800_rfcsr_read_bank(rt2x00dev, 5, 3, &rf_val);
+		rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 3);
 		rf_val &= (~0x3F);
 		rf_val |= 0x3F;
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 3, rf_val);
-		rt2800_rfcsr_read_bank(rt2x00dev, 5, 4, &rf_val);
+		rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 4);
 		rf_val &= (~0x3F);
 		rf_val |= 0x3F;
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 4, rf_val);
@@ -7701,11 +7704,11 @@ static int rt2800_rf_lp_config(struct rt2x00_dev *rt2x00dev, bool btxcal)
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 18, 0xF1);
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 19, 0x18);
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 20, 0x02);
-		rt2800_rfcsr_read_bank(rt2x00dev, 5, 3, &rf_val);
+		rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 3);
 		rf_val &= (~0x3F);
 		rf_val |= 0x34;
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 3, rf_val);
-		rt2800_rfcsr_read_bank(rt2x00dev, 5, 4, &rf_val);
+		rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 4);
 		rf_val &= (~0x3F);
 		rf_val |= 0x34;
 		rt2800_rfcsr_write_bank(rt2x00dev, 5, 4, rf_val);
@@ -7725,14 +7728,14 @@ static char rt2800_lp_tx_filter_bw_cal(struct rt2x00_dev *rt2x00dev)
 	cnt = 0;
 	do {
 		usleep_range(500, 2000);
-		rt2800_bbp_read(rt2x00dev, 159, &bbp_val);
+		bbp_val = rt2800_bbp_read(rt2x00dev, 159);
 		if (bbp_val == 0x02 || cnt == 20)
 			break;
 
 		cnt++;
 	} while (cnt < 20);
 
-	rt2800_bbp_dcoc_read(rt2x00dev, 0x39, &bbp_val);
+	bbp_val = rt2800_bbp_dcoc_read(rt2x00dev, 0x39);
 	cal_val = bbp_val & 0x7F;
 	if (cal_val >= 0x40)
 		cal_val -= 128;
@@ -7761,67 +7764,67 @@ static void rt2800_bw_filter_calibration(struct rt2x00_dev *rt2x00dev,
 	u32 MAC_RF_CONTROL0, MAC_RF_BYPASS0;
 
 	/* Save MAC registers */
-	rt2800_register_read(rt2x00dev, RF_CONTROL0, &MAC_RF_CONTROL0);
-	rt2800_register_read(rt2x00dev, RF_BYPASS0, &MAC_RF_BYPASS0);
+	MAC_RF_CONTROL0 = rt2800_register_read(rt2x00dev, RF_CONTROL0);
+	MAC_RF_BYPASS0 = rt2800_register_read(rt2x00dev, RF_BYPASS0);
 
 	/* save BBP registers */
-	rt2800_bbp_read(rt2x00dev, 23, &savebbpr23);
+	savebbpr23 = rt2800_bbp_read(rt2x00dev, 23);
 
-	rt2800_bbp_dcoc_read(rt2x00dev, 0, &savebbp159r0);
-	rt2800_bbp_dcoc_read(rt2x00dev, 2, &savebbp159r2);
+	savebbp159r0 = rt2800_bbp_dcoc_read(rt2x00dev, 0);
+	savebbp159r2 = rt2800_bbp_dcoc_read(rt2x00dev, 2);
 
 	/* Save RF registers */
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 0, &saverfb5r00);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 1, &saverfb5r01);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 3, &saverfb5r03);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 4, &saverfb5r04);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 5, &saverfb5r05);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 6, &saverfb5r06);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 7, &saverfb5r07);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 8, &saverfb5r08);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 17, &saverfb5r17);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 18, &saverfb5r18);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 19, &saverfb5r19);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 20, &saverfb5r20);
-
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 37, &saverfb5r37);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 38, &saverfb5r38);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 39, &saverfb5r39);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 40, &saverfb5r40);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 41, &saverfb5r41);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 42, &saverfb5r42);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 43, &saverfb5r43);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 44, &saverfb5r44);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 45, &saverfb5r45);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 46, &saverfb5r46);
-
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 58, &saverfb5r58);
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 59, &saverfb5r59);
-
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 0, &rf_val);
+	saverfb5r00 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 0);
+	saverfb5r01 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 1);
+	saverfb5r03 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 3);
+	saverfb5r04 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 4);
+	saverfb5r05 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 5);
+	saverfb5r06 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 6);
+	saverfb5r07 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 7);
+	saverfb5r08 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 8);
+	saverfb5r17 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 17);
+	saverfb5r18 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 18);
+	saverfb5r19 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 19);
+	saverfb5r20 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 20);
+
+	saverfb5r37 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 37);
+	saverfb5r38 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 38);
+	saverfb5r39 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 39);
+	saverfb5r40 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 40);
+	saverfb5r41 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 41);
+	saverfb5r42 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 42);
+	saverfb5r43 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 43);
+	saverfb5r44 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 44);
+	saverfb5r45 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 45);
+	saverfb5r46 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 46);
+
+	saverfb5r58 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 58);
+	saverfb5r59 = rt2800_rfcsr_read_bank(rt2x00dev, 5, 59);
+
+	rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 0);
 	rf_val |= 0x3;
 	rt2800_rfcsr_write_bank(rt2x00dev, 5, 0, rf_val);
 
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 1, &rf_val);
+	rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 1);
 	rf_val |= 0x1;
 	rt2800_rfcsr_write_bank(rt2x00dev, 5, 1, rf_val);
 
 	cnt = 0;
 	do {
 		usleep_range(500, 2000);
-		rt2800_rfcsr_read_bank(rt2x00dev, 5, 1, &rf_val);
+		rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 1);
 		if (((rf_val & 0x1) == 0x00) || (cnt == 40))
 			break;
 		cnt++;
 	} while (cnt < 40);
 
-	rt2800_rfcsr_read_bank(rt2x00dev, 5, 0, &rf_val);
+	rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 0);
 	rf_val &= (~0x3);
 	rf_val |= 0x1;
 	rt2800_rfcsr_write_bank(rt2x00dev, 5, 0, rf_val);
 
 	/* I-3 */
-	rt2800_bbp_read(rt2x00dev, 23, &bbp_val);
+	bbp_val = rt2800_bbp_read(rt2x00dev, 23);
 	bbp_val &= (~0x1F);
 	bbp_val |= 0x10;
 	rt2800_bbp_write(rt2x00dev, 23, bbp_val);
@@ -7844,7 +7847,7 @@ static void rt2800_bw_filter_calibration(struct rt2x00_dev *rt2x00dev,
 				filter_target = rx_filter_target_40m;
 		}
 
-		rt2800_rfcsr_read_bank(rt2x00dev, 5, 8, &rf_val);
+		rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 8);
 		rf_val &= (~0x04);
 		if (loop == 1)
 			rf_val |= 0x4;
@@ -7856,25 +7859,25 @@ static void rt2800_bw_filter_calibration(struct rt2x00_dev *rt2x00dev,
 		rt2800_rf_lp_config(rt2x00dev, btxcal);
 		if (btxcal) {
 			tx_agc_fc = 0;
-			rt2800_rfcsr_read_bank(rt2x00dev, 5, 58, &rf_val);
+			rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 58);
 			rf_val &= (~0x7F);
 			rt2800_rfcsr_write_bank(rt2x00dev, 5, 58, rf_val);
-			rt2800_rfcsr_read_bank(rt2x00dev, 5, 59, &rf_val);
+			rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 59);
 			rf_val &= (~0x7F);
 			rt2800_rfcsr_write_bank(rt2x00dev, 5, 59, rf_val);
 		} else {
 			rx_agc_fc = 0;
-			rt2800_rfcsr_read_bank(rt2x00dev, 5, 6, &rf_val);
+			rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 6);
 			rf_val &= (~0x7F);
 			rt2800_rfcsr_write_bank(rt2x00dev, 5, 6, rf_val);
-			rt2800_rfcsr_read_bank(rt2x00dev, 5, 7, &rf_val);
+			rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 7);
 			rf_val &= (~0x7F);
 			rt2800_rfcsr_write_bank(rt2x00dev, 5, 7, rf_val);
 		}
 
 		usleep_range(1000, 2000);
 
-		rt2800_bbp_dcoc_read(rt2x00dev, 2, &bbp_val);
+		bbp_val = rt2800_bbp_dcoc_read(rt2x00dev, 2);
 		bbp_val &= (~0x6);
 		rt2800_bbp_dcoc_write(rt2x00dev, 2, bbp_val);
 
@@ -7882,25 +7885,25 @@ static void rt2800_bw_filter_calibration(struct rt2x00_dev *rt2x00dev,
 
 		cal_r32_init = rt2800_lp_tx_filter_bw_cal(rt2x00dev);
 
-		rt2800_bbp_dcoc_read(rt2x00dev, 2, &bbp_val);
+		bbp_val = rt2800_bbp_dcoc_read(rt2x00dev, 2);
 		bbp_val |= 0x6;
 		rt2800_bbp_dcoc_write(rt2x00dev, 2, bbp_val);
 do_cal:
 		if (btxcal) {
-			rt2800_rfcsr_read_bank(rt2x00dev, 5, 58, &rf_val);
+			rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 58);
 			rf_val &= (~0x7F);
 			rf_val |= tx_agc_fc;
 			rt2800_rfcsr_write_bank(rt2x00dev, 5, 58, rf_val);
-			rt2800_rfcsr_read_bank(rt2x00dev, 5, 59, &rf_val);
+			rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 59);
 			rf_val &= (~0x7F);
 			rf_val |= tx_agc_fc;
 			rt2800_rfcsr_write_bank(rt2x00dev, 5, 59, rf_val);
 		} else {
-			rt2800_rfcsr_read_bank(rt2x00dev, 5, 6, &rf_val);
+			rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 6);
 			rf_val &= (~0x7F);
 			rf_val |= rx_agc_fc;
 			rt2800_rfcsr_write_bank(rt2x00dev, 5, 6, rf_val);
-			rt2800_rfcsr_read_bank(rt2x00dev, 5, 7, &rf_val);
+			rf_val = rt2800_rfcsr_read_bank(rt2x00dev, 5, 7);
 			rf_val &= (~0x7F);
 			rf_val |= rx_agc_fc;
 			rt2800_rfcsr_write_bank(rt2x00dev, 5, 7, rf_val);
@@ -7980,7 +7983,7 @@ do_cal:
 	rt2800_bbp_dcoc_write(rt2x00dev, 0, savebbp159r0);
 	rt2800_bbp_dcoc_write(rt2x00dev, 2, savebbp159r2);
 
-	rt2800_bbp_read(rt2x00dev, 4, &bbp_val);
+	bbp_val = rt2800_bbp_read(rt2x00dev, 4);
 	rt2x00_set_field8(&bbp_val, BBP4_BANDWIDTH,
 			  2 * test_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags));
 	rt2800_bbp_write(rt2x00dev, 4, bbp_val);
@@ -8355,20 +8358,20 @@ int rt2800_enable_radio(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Enable RX.
 	 */
-	rt2800_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
+	reg = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
 	rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_TX, 1);
 	rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 0);
 	rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
 
 	udelay(50);
 
-	rt2800_register_read(rt2x00dev, WPDMA_GLO_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, WPDMA_GLO_CFG);
 	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_TX_DMA, 1);
 	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_ENABLE_RX_DMA, 1);
 	rt2x00_set_field32(&reg, WPDMA_GLO_CFG_TX_WRITEBACK_DONE, 1);
 	rt2800_register_write(rt2x00dev, WPDMA_GLO_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
+	reg = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
 	rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_TX, 1);
 	rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 1);
 	rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
@@ -8376,15 +8379,15 @@ int rt2800_enable_radio(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize LED control
 	 */
-	rt2800_eeprom_read(rt2x00dev, EEPROM_LED_AG_CONF, &word);
+	word = rt2800_eeprom_read(rt2x00dev, EEPROM_LED_AG_CONF);
 	rt2800_mcu_request(rt2x00dev, MCU_LED_AG_CONF, 0xff,
 			   word & 0xff, (word >> 8) & 0xff);
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_LED_ACT_CONF, &word);
+	word = rt2800_eeprom_read(rt2x00dev, EEPROM_LED_ACT_CONF);
 	rt2800_mcu_request(rt2x00dev, MCU_LED_ACT_CONF, 0xff,
 			   word & 0xff, (word >> 8) & 0xff);
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_LED_POLARITY, &word);
+	word = rt2800_eeprom_read(rt2x00dev, EEPROM_LED_POLARITY);
 	rt2800_mcu_request(rt2x00dev, MCU_LED_LED_POLARITY, 0xff,
 			   word & 0xff, (word >> 8) & 0xff);
 
@@ -8401,7 +8404,7 @@ void rt2800_disable_radio(struct rt2x00_dev *rt2x00dev)
 	/* Wait for DMA, ignore error */
 	rt2800_wait_wpdma_ready(rt2x00dev);
 
-	rt2800_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
+	reg = rt2800_register_read(rt2x00dev, MAC_SYS_CTRL);
 	rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_TX, 0);
 	rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 0);
 	rt2800_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
@@ -8418,7 +8421,7 @@ int rt2800_efuse_detect(struct rt2x00_dev *rt2x00dev)
 	else
 		efuse_ctrl_reg = EFUSE_CTRL;
 
-	rt2800_register_read(rt2x00dev, efuse_ctrl_reg, &reg);
+	reg = rt2800_register_read(rt2x00dev, efuse_ctrl_reg);
 	return rt2x00_get_field32(reg, EFUSE_CTRL_PRESENT);
 }
 EXPORT_SYMBOL_GPL(rt2800_efuse_detect);
@@ -8447,7 +8450,7 @@ static void rt2800_efuse_read(struct rt2x00_dev *rt2x00dev, unsigned int i)
 	}
 	mutex_lock(&rt2x00dev->csr_mutex);
 
-	rt2800_register_read_lock(rt2x00dev, efuse_ctrl_reg, &reg);
+	reg = rt2800_register_read_lock(rt2x00dev, efuse_ctrl_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);
@@ -8456,14 +8459,14 @@ static void rt2800_efuse_read(struct rt2x00_dev *rt2x00dev, unsigned int i)
 	/* Wait until the EEPROM has been loaded */
 	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, &reg);
+	reg = rt2800_register_read_lock(rt2x00dev, efuse_data3_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, &reg);
+	reg = rt2800_register_read_lock(rt2x00dev, efuse_data2_reg);
 	*(u32 *)&rt2x00dev->eeprom[i + 2] = cpu_to_le32(reg);
-	rt2800_register_read_lock(rt2x00dev, efuse_data1_reg, &reg);
+	reg = rt2800_register_read_lock(rt2x00dev, efuse_data1_reg);
 	*(u32 *)&rt2x00dev->eeprom[i + 4] = cpu_to_le32(reg);
-	rt2800_register_read_lock(rt2x00dev, efuse_data0_reg, &reg);
+	reg = rt2800_register_read_lock(rt2x00dev, efuse_data0_reg);
 	*(u32 *)&rt2x00dev->eeprom[i + 6] = cpu_to_le32(reg);
 
 	mutex_unlock(&rt2x00dev->csr_mutex);
@@ -8487,7 +8490,7 @@ static u8 rt2800_get_txmixer_gain_24g(struct rt2x00_dev *rt2x00dev)
 	if (rt2x00_rt(rt2x00dev, RT3593))
 		return 0;
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_TXMIXER_GAIN_BG, &word);
+	word = rt2800_eeprom_read(rt2x00dev, EEPROM_TXMIXER_GAIN_BG);
 	if ((word & 0x00ff) != 0x00ff)
 		return rt2x00_get_field16(word, EEPROM_TXMIXER_GAIN_BG_VAL);
 
@@ -8501,7 +8504,7 @@ static u8 rt2800_get_txmixer_gain_5g(struct rt2x00_dev *rt2x00dev)
 	if (rt2x00_rt(rt2x00dev, RT3593))
 		return 0;
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_TXMIXER_GAIN_A, &word);
+	word = rt2800_eeprom_read(rt2x00dev, EEPROM_TXMIXER_GAIN_A);
 	if ((word & 0x00ff) != 0x00ff)
 		return rt2x00_get_field16(word, EEPROM_TXMIXER_GAIN_A_VAL);
 
@@ -8529,7 +8532,7 @@ static int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	mac = rt2800_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	rt2x00lib_set_mac_address(rt2x00dev, mac);
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &word);
+	word = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_NIC_CONF0_RXPATH, 2);
 		rt2x00_set_field16(&word, EEPROM_NIC_CONF0_TXPATH, 1);
@@ -8546,7 +8549,7 @@ static int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2800_eeprom_write(rt2x00dev, EEPROM_NIC_CONF0, word);
 	}
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &word);
+	word = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_NIC_CONF1_HW_RADIO, 0);
 		rt2x00_set_field16(&word, EEPROM_NIC_CONF1_EXTERNAL_TX_ALC, 0);
@@ -8567,7 +8570,7 @@ static int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "NIC: 0x%04x\n", word);
 	}
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_FREQ, &word);
+	word = rt2800_eeprom_read(rt2x00dev, EEPROM_FREQ);
 	if ((word & 0x00ff) == 0x00ff) {
 		rt2x00_set_field16(&word, EEPROM_FREQ_OFFSET, 0);
 		rt2800_eeprom_write(rt2x00dev, EEPROM_FREQ, word);
@@ -8589,10 +8592,10 @@ static int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	 * lna0 as correct value. Note that EEPROM_LNA
 	 * is never validated.
 	 */
-	rt2800_eeprom_read(rt2x00dev, EEPROM_LNA, &word);
+	word = rt2800_eeprom_read(rt2x00dev, EEPROM_LNA);
 	default_lna_gain = rt2x00_get_field16(word, EEPROM_LNA_A0);
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_BG, &word);
+	word = rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_BG);
 	if (abs(rt2x00_get_field16(word, EEPROM_RSSI_BG_OFFSET0)) > 10)
 		rt2x00_set_field16(&word, EEPROM_RSSI_BG_OFFSET0, 0);
 	if (abs(rt2x00_get_field16(word, EEPROM_RSSI_BG_OFFSET1)) > 10)
@@ -8601,7 +8604,7 @@ static int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 
 	drv_data->txmixer_gain_24g = rt2800_get_txmixer_gain_24g(rt2x00dev);
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_BG2, &word);
+	word = rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_BG2);
 	if (abs(rt2x00_get_field16(word, EEPROM_RSSI_BG2_OFFSET2)) > 10)
 		rt2x00_set_field16(&word, EEPROM_RSSI_BG2_OFFSET2, 0);
 	if (!rt2x00_rt(rt2x00dev, RT3593)) {
@@ -8614,14 +8617,14 @@ static int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 
 	drv_data->txmixer_gain_5g = rt2800_get_txmixer_gain_5g(rt2x00dev);
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_A, &word);
+	word = rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_A);
 	if (abs(rt2x00_get_field16(word, EEPROM_RSSI_A_OFFSET0)) > 10)
 		rt2x00_set_field16(&word, EEPROM_RSSI_A_OFFSET0, 0);
 	if (abs(rt2x00_get_field16(word, EEPROM_RSSI_A_OFFSET1)) > 10)
 		rt2x00_set_field16(&word, EEPROM_RSSI_A_OFFSET1, 0);
 	rt2800_eeprom_write(rt2x00dev, EEPROM_RSSI_A, word);
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_A2, &word);
+	word = rt2800_eeprom_read(rt2x00dev, EEPROM_RSSI_A2);
 	if (abs(rt2x00_get_field16(word, EEPROM_RSSI_A2_OFFSET2)) > 10)
 		rt2x00_set_field16(&word, EEPROM_RSSI_A2_OFFSET2, 0);
 	if (!rt2x00_rt(rt2x00dev, RT3593)) {
@@ -8633,7 +8636,7 @@ static int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	rt2800_eeprom_write(rt2x00dev, EEPROM_RSSI_A2, word);
 
 	if (rt2x00_rt(rt2x00dev, RT3593)) {
-		rt2800_eeprom_read(rt2x00dev, EEPROM_EXT_LNA2, &word);
+		word = rt2800_eeprom_read(rt2x00dev, EEPROM_EXT_LNA2);
 		if (rt2x00_get_field16(word, EEPROM_EXT_LNA2_A1) == 0x00 ||
 		    rt2x00_get_field16(word, EEPROM_EXT_LNA2_A1) == 0xff)
 			rt2x00_set_field16(&word, EEPROM_EXT_LNA2_A1,
@@ -8657,7 +8660,7 @@ static int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Read EEPROM word for configuration.
 	 */
-	rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &eeprom);
+	eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0);
 
 	/*
 	 * Identify RF chipset by EEPROM value
@@ -8668,7 +8671,7 @@ static int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	    rt2x00_rt(rt2x00dev, RT5390) ||
 	    rt2x00_rt(rt2x00dev, RT5392) ||
 	    rt2x00_rt(rt2x00dev, RT6352))
-		rt2800_eeprom_read(rt2x00dev, EEPROM_CHIP_ID, &rf);
+		rf = rt2800_eeprom_read(rt2x00dev, EEPROM_CHIP_ID);
 	else if (rt2x00_rt(rt2x00dev, RT3352))
 		rf = RF3322;
 	else if (rt2x00_rt(rt2x00dev, RT5350))
@@ -8717,7 +8720,7 @@ static int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	rt2x00dev->default_ant.rx_chain_num =
 	    rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RXPATH);
 
-	rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &eeprom);
+	eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1);
 
 	if (rt2x00_rt(rt2x00dev, RT3070) ||
 	    rt2x00_rt(rt2x00dev, RT3090) ||
@@ -8771,7 +8774,7 @@ static int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Read frequency offset and RF programming sequence.
 	 */
-	rt2800_eeprom_read(rt2x00dev, EEPROM_FREQ, &eeprom);
+	eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_FREQ);
 	rt2x00dev->freq_offset = rt2x00_get_field16(eeprom, EEPROM_FREQ_OFFSET);
 
 	/*
@@ -8788,7 +8791,7 @@ static int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Check if support EIRP tx power limit feature.
 	 */
-	rt2800_eeprom_read(rt2x00dev, EEPROM_EIRP_MAX_TX_POWER, &eeprom);
+	eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_EIRP_MAX_TX_POWER);
 
 	if (rt2x00_get_field16(eeprom, EEPROM_EIRP_MAX_TX_POWER_2GHZ) <
 					EIRP_MAX_TX_POWER_LIMIT)
@@ -8797,7 +8800,7 @@ static int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Detect if device uses internal or external PA
 	 */
-	rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1, &eeprom);
+	eeprom = rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF1);
 
 	if (rt2x00_rt(rt2x00dev, RT3352)) {
 		if (rt2x00_get_field16(eeprom,
@@ -9239,7 +9242,7 @@ static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 		break;
 
 	case RF5592:
-		rt2800_register_read(rt2x00dev, MAC_DEBUG_INDEX, &reg);
+		reg = rt2800_register_read(rt2x00dev, MAC_DEBUG_INDEX);
 		if (rt2x00_get_field32(reg, MAC_DEBUG_INDEX_XTAL)) {
 			spec->num_channels = ARRAY_SIZE(rf_vals_5592_xtal40);
 			spec->channels = rf_vals_5592_xtal40;
@@ -9378,9 +9381,9 @@ static int rt2800_probe_rt(struct rt2x00_dev *rt2x00dev)
 	u32 rev;
 
 	if (rt2x00_rt(rt2x00dev, RT3290))
-		rt2800_register_read(rt2x00dev, MAC_CSR0_3290, &reg);
+		reg = rt2800_register_read(rt2x00dev, MAC_CSR0_3290);
 	else
-		rt2800_register_read(rt2x00dev, MAC_CSR0, &reg);
+		reg = rt2800_register_read(rt2x00dev, MAC_CSR0);
 
 	rt = rt2x00_get_field32(reg, MAC_CSR0_CHIPSET);
 	rev = rt2x00_get_field32(reg, MAC_CSR0_REVISION);
@@ -9440,7 +9443,7 @@ int rt2800_probe_hw(struct rt2x00_dev *rt2x00dev)
 	 * Enable rfkill polling by setting GPIO direction of the
 	 * rfkill switch GPIO pin correctly.
 	 */
-	rt2800_register_read(rt2x00dev, GPIO_CTRL, &reg);
+	reg = rt2800_register_read(rt2x00dev, GPIO_CTRL);
 	rt2x00_set_field32(&reg, GPIO_CTRL_DIR2, 1);
 	rt2800_register_write(rt2x00dev, GPIO_CTRL, reg);
 
@@ -9515,31 +9518,31 @@ int rt2800_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
 	u32 reg;
 	bool enabled = (value < IEEE80211_MAX_RTS_THRESHOLD);
 
-	rt2800_register_read(rt2x00dev, TX_RTS_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, TX_RTS_CFG);
 	rt2x00_set_field32(&reg, TX_RTS_CFG_RTS_THRES, value);
 	rt2800_register_write(rt2x00dev, TX_RTS_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, CCK_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, CCK_PROT_CFG);
 	rt2x00_set_field32(&reg, CCK_PROT_CFG_RTS_TH_EN, enabled);
 	rt2800_register_write(rt2x00dev, CCK_PROT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, OFDM_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, OFDM_PROT_CFG);
 	rt2x00_set_field32(&reg, OFDM_PROT_CFG_RTS_TH_EN, enabled);
 	rt2800_register_write(rt2x00dev, OFDM_PROT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, MM20_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, MM20_PROT_CFG);
 	rt2x00_set_field32(&reg, MM20_PROT_CFG_RTS_TH_EN, enabled);
 	rt2800_register_write(rt2x00dev, MM20_PROT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, MM40_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, MM40_PROT_CFG);
 	rt2x00_set_field32(&reg, MM40_PROT_CFG_RTS_TH_EN, enabled);
 	rt2800_register_write(rt2x00dev, MM40_PROT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, GF20_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, GF20_PROT_CFG);
 	rt2x00_set_field32(&reg, GF20_PROT_CFG_RTS_TH_EN, enabled);
 	rt2800_register_write(rt2x00dev, GF20_PROT_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, GF40_PROT_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, GF40_PROT_CFG);
 	rt2x00_set_field32(&reg, GF40_PROT_CFG_RTS_TH_EN, enabled);
 	rt2800_register_write(rt2x00dev, GF40_PROT_CFG, reg);
 
@@ -9582,7 +9585,7 @@ int rt2800_conf_tx(struct ieee80211_hw *hw,
 	field.bit_offset = (queue_idx & 1) * 16;
 	field.bit_mask = 0xffff << field.bit_offset;
 
-	rt2800_register_read(rt2x00dev, offset, &reg);
+	reg = rt2800_register_read(rt2x00dev, offset);
 	rt2x00_set_field32(&reg, field, queue->txop);
 	rt2800_register_write(rt2x00dev, offset, reg);
 
@@ -9590,22 +9593,22 @@ int rt2800_conf_tx(struct ieee80211_hw *hw,
 	field.bit_offset = queue_idx * 4;
 	field.bit_mask = 0xf << field.bit_offset;
 
-	rt2800_register_read(rt2x00dev, WMM_AIFSN_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, WMM_AIFSN_CFG);
 	rt2x00_set_field32(&reg, field, queue->aifs);
 	rt2800_register_write(rt2x00dev, WMM_AIFSN_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, WMM_CWMIN_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, WMM_CWMIN_CFG);
 	rt2x00_set_field32(&reg, field, queue->cw_min);
 	rt2800_register_write(rt2x00dev, WMM_CWMIN_CFG, reg);
 
-	rt2800_register_read(rt2x00dev, WMM_CWMAX_CFG, &reg);
+	reg = rt2800_register_read(rt2x00dev, WMM_CWMAX_CFG);
 	rt2x00_set_field32(&reg, field, queue->cw_max);
 	rt2800_register_write(rt2x00dev, WMM_CWMAX_CFG, reg);
 
 	/* Update EDCA registers */
 	offset = EDCA_AC0_CFG + (sizeof(u32) * queue_idx);
 
-	rt2800_register_read(rt2x00dev, offset, &reg);
+	reg = rt2800_register_read(rt2x00dev, offset);
 	rt2x00_set_field32(&reg, EDCA_AC0_CFG_TX_OP, queue->txop);
 	rt2x00_set_field32(&reg, EDCA_AC0_CFG_AIFSN, queue->aifs);
 	rt2x00_set_field32(&reg, EDCA_AC0_CFG_CWMIN, queue->cw_min);
@@ -9622,9 +9625,9 @@ u64 rt2800_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	u64 tsf;
 	u32 reg;
 
-	rt2800_register_read(rt2x00dev, TSF_TIMER_DW1, &reg);
+	reg = rt2800_register_read(rt2x00dev, TSF_TIMER_DW1);
 	tsf = (u64) rt2x00_get_field32(reg, TSF_TIMER_DW1_HIGH_WORD) << 32;
-	rt2800_register_read(rt2x00dev, TSF_TIMER_DW0, &reg);
+	reg = rt2800_register_read(rt2x00dev, TSF_TIMER_DW0);
 	tsf |= rt2x00_get_field32(reg, TSF_TIMER_DW0_LOW_WORD);
 
 	return tsf;
@@ -9691,9 +9694,9 @@ int rt2800_get_survey(struct ieee80211_hw *hw, int idx,
 
 	survey->channel = conf->chandef.chan;
 
-	rt2800_register_read(rt2x00dev, CH_IDLE_STA, &idle);
-	rt2800_register_read(rt2x00dev, CH_BUSY_STA, &busy);
-	rt2800_register_read(rt2x00dev, CH_BUSY_STA_SEC, &busy_ext);
+	idle = rt2800_register_read(rt2x00dev, CH_IDLE_STA);
+	busy = rt2800_register_read(rt2x00dev, CH_BUSY_STA);
+	busy_ext = rt2800_register_read(rt2x00dev, CH_BUSY_STA_SEC);
 
 	if (idle || busy) {
 		survey->filled = SURVEY_INFO_TIME |
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2800lib.h b/drivers/net/wireless/ralink/rt2x00/rt2800lib.h
index f357531d9488..275e3969abdd 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2800lib.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800lib.h
@@ -49,10 +49,10 @@ struct rt2800_drv_data {
 };
 
 struct rt2800_ops {
-	void (*register_read)(struct rt2x00_dev *rt2x00dev,
-			      const unsigned int offset, u32 *value);
-	void (*register_read_lock)(struct rt2x00_dev *rt2x00dev,
-				   const unsigned int offset, u32 *value);
+	u32 (*register_read)(struct rt2x00_dev *rt2x00dev,
+			      const unsigned int offset);
+	u32 (*register_read_lock)(struct rt2x00_dev *rt2x00dev,
+				   const unsigned int offset);
 	void (*register_write)(struct rt2x00_dev *rt2x00dev,
 			       const unsigned int offset, u32 value);
 	void (*register_write_lock)(struct rt2x00_dev *rt2x00dev,
@@ -78,22 +78,20 @@ struct rt2800_ops {
 	__le32 *(*drv_get_txwi)(struct queue_entry *entry);
 };
 
-static inline void rt2800_register_read(struct rt2x00_dev *rt2x00dev,
-					const unsigned int offset,
-					u32 *value)
+static inline u32 rt2800_register_read(struct rt2x00_dev *rt2x00dev,
+				       const unsigned int offset)
 {
 	const struct rt2800_ops *rt2800ops = rt2x00dev->ops->drv;
 
-	rt2800ops->register_read(rt2x00dev, offset, value);
+	return rt2800ops->register_read(rt2x00dev, offset);
 }
 
-static inline void rt2800_register_read_lock(struct rt2x00_dev *rt2x00dev,
-					     const unsigned int offset,
-					     u32 *value)
+static inline u32 rt2800_register_read_lock(struct rt2x00_dev *rt2x00dev,
+					    const unsigned int offset)
 {
 	const struct rt2800_ops *rt2800ops = rt2x00dev->ops->drv;
 
-	rt2800ops->register_read_lock(rt2x00dev, offset, value);
+	return rt2800ops->register_read_lock(rt2x00dev, offset);
 }
 
 static inline void rt2800_register_write(struct rt2x00_dev *rt2x00dev,
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2800mmio.c b/drivers/net/wireless/ralink/rt2x00/rt2800mmio.c
index 3ab3b5323897..ee5276e233fa 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2800mmio.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800mmio.c
@@ -109,7 +109,7 @@ void rt2800mmio_fill_rxdone(struct queue_entry *entry,
 	__le32 *rxd = entry_priv->desc;
 	u32 word;
 
-	rt2x00_desc_read(rxd, 3, &word);
+	word = rt2x00_desc_read(rxd, 3);
 
 	if (rt2x00_get_field32(word, RXD_W3_CRC_ERROR))
 		rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
@@ -175,7 +175,7 @@ static bool rt2800mmio_txdone_entry_check(struct queue_entry *entry, u32 status)
 	wcid = rt2x00_get_field32(status, TX_STA_FIFO_WCID);
 
 	txwi = rt2800_drv_get_txwi(entry);
-	rt2x00_desc_read(txwi, 1, &word);
+	word = rt2x00_desc_read(txwi, 1);
 	tx_wcid = rt2x00_get_field32(word, TXWI_W1_WIRELESS_CLI_ID);
 
 	return (tx_wcid == wcid);
@@ -331,7 +331,7 @@ static inline void rt2800mmio_enable_interrupt(struct rt2x00_dev *rt2x00dev,
 	 * access needs locking.
 	 */
 	spin_lock_irq(&rt2x00dev->irqmask_lock);
-	rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR);
 	rt2x00_set_field32(&reg, irq_field, 1);
 	rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
 	spin_unlock_irq(&rt2x00dev->irqmask_lock);
@@ -376,12 +376,12 @@ void rt2800mmio_tbtt_tasklet(unsigned long data)
 		 * interval every 64 beacons by 64us to mitigate this effect.
 		 */
 		if (drv_data->tbtt_tick == (BCN_TBTT_OFFSET - 2)) {
-			rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+			reg = rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG);
 			rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_INTERVAL,
 					   (rt2x00dev->beacon_int * 16) - 1);
 			rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
 		} else if (drv_data->tbtt_tick == (BCN_TBTT_OFFSET - 1)) {
-			rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+			reg = rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG);
 			rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_INTERVAL,
 					   (rt2x00dev->beacon_int * 16));
 			rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
@@ -439,7 +439,7 @@ static void rt2800mmio_txstatus_interrupt(struct rt2x00_dev *rt2x00dev)
 	 * need to lock the kfifo.
 	 */
 	for (i = 0; i < rt2x00dev->tx->limit; i++) {
-		rt2x00mmio_register_read(rt2x00dev, TX_STA_FIFO, &status);
+		status = rt2x00mmio_register_read(rt2x00dev, TX_STA_FIFO);
 
 		if (!rt2x00_get_field32(status, TX_STA_FIFO_VALID))
 			break;
@@ -460,7 +460,7 @@ irqreturn_t rt2800mmio_interrupt(int irq, void *dev_instance)
 	u32 reg, mask;
 
 	/* Read status and ACK all interrupts */
-	rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR);
 	rt2x00mmio_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
 
 	if (!reg)
@@ -501,7 +501,7 @@ irqreturn_t rt2800mmio_interrupt(int irq, void *dev_instance)
 	 * the tasklet will reenable the appropriate interrupts.
 	 */
 	spin_lock(&rt2x00dev->irqmask_lock);
-	rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR);
 	reg &= mask;
 	rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
 	spin_unlock(&rt2x00dev->irqmask_lock);
@@ -521,7 +521,7 @@ void rt2800mmio_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 * should clear the register to assure a clean state.
 	 */
 	if (state == STATE_RADIO_IRQ_ON) {
-		rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR);
 		rt2x00mmio_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
 	}
 
@@ -560,18 +560,18 @@ void rt2800mmio_start_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00mmio_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, MAC_SYS_CTRL);
 		rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 1);
 		rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
 		break;
 	case QID_BEACON:
-		rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 1);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE, 1);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 1);
 		rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, INT_TIMER_EN, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, INT_TIMER_EN);
 		rt2x00_set_field32(&reg, INT_TIMER_EN_PRE_TBTT_TIMER, 1);
 		rt2x00mmio_register_write(rt2x00dev, INT_TIMER_EN, reg);
 		break;
@@ -613,18 +613,18 @@ void rt2800mmio_stop_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00mmio_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, MAC_SYS_CTRL);
 		rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 0);
 		rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
 		break;
 	case QID_BEACON:
-		rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 0);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE, 0);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 0);
 		rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, INT_TIMER_EN, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, INT_TIMER_EN);
 		rt2x00_set_field32(&reg, INT_TIMER_EN_PRE_TBTT_TIMER, 0);
 		rt2x00mmio_register_write(rt2x00dev, INT_TIMER_EN, reg);
 
@@ -696,11 +696,11 @@ bool rt2800mmio_get_entry_state(struct queue_entry *entry)
 	u32 word;
 
 	if (entry->queue->qid == QID_RX) {
-		rt2x00_desc_read(entry_priv->desc, 1, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 1);
 
 		return (!rt2x00_get_field32(word, RXD_W1_DMA_DONE));
 	} else {
-		rt2x00_desc_read(entry_priv->desc, 1, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 1);
 
 		return (!rt2x00_get_field32(word, TXD_W1_DMA_DONE));
 	}
@@ -715,11 +715,11 @@ void rt2800mmio_clear_entry(struct queue_entry *entry)
 	u32 word;
 
 	if (entry->queue->qid == QID_RX) {
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 		rt2x00_set_field32(&word, RXD_W0_SDP0, skbdesc->skb_dma);
 		rt2x00_desc_write(entry_priv->desc, 0, word);
 
-		rt2x00_desc_read(entry_priv->desc, 1, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 1);
 		rt2x00_set_field32(&word, RXD_W1_DMA_DONE, 0);
 		rt2x00_desc_write(entry_priv->desc, 1, word);
 
@@ -730,7 +730,7 @@ void rt2800mmio_clear_entry(struct queue_entry *entry)
 		rt2x00mmio_register_write(rt2x00dev, RX_CRX_IDX,
 					  entry->entry_idx);
 	} else {
-		rt2x00_desc_read(entry_priv->desc, 1, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 1);
 		rt2x00_set_field32(&word, TXD_W1_DMA_DONE, 1);
 		rt2x00_desc_write(entry_priv->desc, 1, word);
 	}
@@ -810,7 +810,7 @@ int rt2800mmio_init_registers(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Reset DMA indexes
 	 */
-	rt2x00mmio_register_read(rt2x00dev, WPDMA_RST_IDX, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, WPDMA_RST_IDX);
 	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX0, 1);
 	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX1, 1);
 	rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX2, 1);
@@ -831,7 +831,7 @@ int rt2800mmio_init_registers(struct rt2x00_dev *rt2x00dev)
 	     rt2x00_rt(rt2x00dev, RT5390) ||
 	     rt2x00_rt(rt2x00dev, RT5392) ||
 	     rt2x00_rt(rt2x00dev, RT5592))) {
-		rt2x00mmio_register_read(rt2x00dev, AUX_CTRL, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, AUX_CTRL);
 		rt2x00_set_field32(&reg, AUX_CTRL_FORCE_PCIE_CLK, 1);
 		rt2x00_set_field32(&reg, AUX_CTRL_WAKE_PCIE_EN, 1);
 		rt2x00mmio_register_write(rt2x00dev, AUX_CTRL, reg);
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2800pci.c b/drivers/net/wireless/ralink/rt2x00/rt2800pci.c
index 0af22573a2eb..5cf655ff1430 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800pci.c
@@ -69,7 +69,7 @@ static void rt2800pci_mcu_status(struct rt2x00_dev *rt2x00dev, const u8 token)
 		return;
 
 	for (i = 0; i < 200; i++) {
-		rt2x00mmio_register_read(rt2x00dev, H2M_MAILBOX_CID, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, H2M_MAILBOX_CID);
 
 		if ((rt2x00_get_field32(reg, H2M_MAILBOX_CID_CMD0) == token) ||
 		    (rt2x00_get_field32(reg, H2M_MAILBOX_CID_CMD1) == token) ||
@@ -92,7 +92,7 @@ static void rt2800pci_eepromregister_read(struct eeprom_93cx6 *eeprom)
 	struct rt2x00_dev *rt2x00dev = eeprom->data;
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, E2PROM_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, E2PROM_CSR);
 
 	eeprom->reg_data_in = !!rt2x00_get_field32(reg, E2PROM_CSR_DATA_IN);
 	eeprom->reg_data_out = !!rt2x00_get_field32(reg, E2PROM_CSR_DATA_OUT);
@@ -122,7 +122,7 @@ static int rt2800pci_read_eeprom_pci(struct rt2x00_dev *rt2x00dev)
 	struct eeprom_93cx6 eeprom;
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, E2PROM_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, E2PROM_CSR);
 
 	eeprom.data = rt2x00dev;
 	eeprom.register_read = rt2800pci_eepromregister_read;
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2800usb.c b/drivers/net/wireless/ralink/rt2x00/rt2800usb.c
index f11e3f532a84..685b8e0cd67d 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800usb.c
@@ -61,12 +61,12 @@ static void rt2800usb_start_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00usb_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, MAC_SYS_CTRL);
 		rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 1);
 		rt2x00usb_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
 		break;
 	case QID_BEACON:
-		rt2x00usb_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, BCN_TIME_CFG);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 1);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE, 1);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 1);
@@ -84,12 +84,12 @@ static void rt2800usb_stop_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00usb_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, MAC_SYS_CTRL);
 		rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 0);
 		rt2x00usb_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
 		break;
 	case QID_BEACON:
-		rt2x00usb_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, BCN_TIME_CFG);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 0);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE, 0);
 		rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 0);
@@ -333,7 +333,7 @@ static int rt2800usb_init_registers(struct rt2x00_dev *rt2x00dev)
 	if (rt2800_wait_csr_ready(rt2x00dev))
 		return -EBUSY;
 
-	rt2x00usb_register_read(rt2x00dev, PBF_SYS_CTRL, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, PBF_SYS_CTRL);
 	rt2x00usb_register_write(rt2x00dev, PBF_SYS_CTRL, reg & ~0x00002000);
 
 	reg = 0;
@@ -456,7 +456,7 @@ static void rt2800usb_write_tx_desc(struct queue_entry *entry,
 	/*
 	 * Initialize TXINFO descriptor
 	 */
-	rt2x00_desc_read(txi, 0, &word);
+	word = rt2x00_desc_read(txi, 0);
 
 	/*
 	 * The size of TXINFO_W0_USB_DMA_TX_PKT_LEN is
@@ -527,7 +527,7 @@ static bool rt2800usb_txdone_entry_check(struct queue_entry *entry, u32 reg)
 	 */
 	txwi = rt2800usb_get_txwi(entry);
 
-	rt2x00_desc_read(txwi, 1, &word);
+	word = rt2x00_desc_read(txwi, 1);
 	tx_wcid = rt2x00_get_field32(word, TXWI_W1_WIRELESS_CLI_ID);
 	tx_ack  = rt2x00_get_field32(word, TXWI_W1_ACK);
 	tx_pid  = rt2x00_get_field32(word, TXWI_W1_PACKETID);
@@ -652,7 +652,7 @@ static void rt2800usb_fill_rxdone(struct queue_entry *entry,
 	 * | RXINFO | RXWI | header | L2 pad | payload | pad | RXD | USB pad |
 	 *          |<------------ rx_pkt_len -------------->|
 	 */
-	rt2x00_desc_read(rxi, 0, &word);
+	word = rt2x00_desc_read(rxi, 0);
 	rx_pkt_len = rt2x00_get_field32(word, RXINFO_W0_USB_DMA_RX_PKT_LEN);
 
 	/*
@@ -676,7 +676,7 @@ static void rt2800usb_fill_rxdone(struct queue_entry *entry,
 	/*
 	 * It is now safe to read the descriptor on all architectures.
 	 */
-	rt2x00_desc_read(rxd, 0, &word);
+	word = rt2x00_desc_read(rxd, 0);
 
 	if (rt2x00_get_field32(word, RXD_W0_CRC_ERROR))
 		rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
@@ -1156,6 +1156,8 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	{ USB_DEVICE(0x2001, 0x3c17) },
 	/* Panasonic */
 	{ USB_DEVICE(0x083a, 0xb511) },
+	/* Accton/Arcadyan/Epson */
+	{ USB_DEVICE(0x083a, 0xb512) },
 	/* Philips */
 	{ USB_DEVICE(0x0471, 0x20dd) },
 	/* Ralink */
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2x00.h b/drivers/net/wireless/ralink/rt2x00/rt2x00.h
index 1bc353eafe37..1f38c338ca7a 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00.h
@@ -1049,11 +1049,11 @@ struct rt2x00_bar_list_entry {
  * Generic RF access.
  * The RF is being accessed by word index.
  */
-static inline void rt2x00_rf_read(struct rt2x00_dev *rt2x00dev,
-				  const unsigned int word, u32 *data)
+static inline u32 rt2x00_rf_read(struct rt2x00_dev *rt2x00dev,
+				 const unsigned int word)
 {
 	BUG_ON(word < 1 || word > rt2x00dev->ops->rf_size / sizeof(u32));
-	*data = rt2x00dev->rf[word - 1];
+	return rt2x00dev->rf[word - 1];
 }
 
 static inline void rt2x00_rf_write(struct rt2x00_dev *rt2x00dev,
@@ -1072,10 +1072,10 @@ static inline void *rt2x00_eeprom_addr(struct rt2x00_dev *rt2x00dev,
 	return (void *)&rt2x00dev->eeprom[word];
 }
 
-static inline void rt2x00_eeprom_read(struct rt2x00_dev *rt2x00dev,
-				      const unsigned int word, u16 *data)
+static inline u16 rt2x00_eeprom_read(struct rt2x00_dev *rt2x00dev,
+				     const unsigned int word)
 {
-	*data = le16_to_cpu(rt2x00dev->eeprom[word]);
+	return le16_to_cpu(rt2x00dev->eeprom[word]);
 }
 
 static inline void rt2x00_eeprom_write(struct rt2x00_dev *rt2x00dev,
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2x00debug.c b/drivers/net/wireless/ralink/rt2x00/rt2x00debug.c
index 964aefdc11f0..51520a0e2138 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2x00debug.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00debug.c
@@ -188,7 +188,7 @@ void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
 		return;
 	}
 
-	dump_hdr = (struct rt2x00dump_hdr *)skb_put(skbcopy, sizeof(*dump_hdr));
+	dump_hdr = skb_put(skbcopy, sizeof(*dump_hdr));
 	dump_hdr->version = cpu_to_le32(DUMP_HEADER_VERSION);
 	dump_hdr->header_length = cpu_to_le32(sizeof(*dump_hdr));
 	dump_hdr->desc_length = cpu_to_le32(skbdesc->desc_len);
@@ -203,9 +203,8 @@ void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
 	dump_hdr->timestamp_usec = cpu_to_le32(timestamp.tv_usec);
 
 	if (!(skbdesc->flags & SKBDESC_DESC_IN_SKB))
-		memcpy(skb_put(skbcopy, skbdesc->desc_len), skbdesc->desc,
-		       skbdesc->desc_len);
-	memcpy(skb_put(skbcopy, skb->len), skb->data, skb->len);
+		skb_put_data(skbcopy, skbdesc->desc, skbdesc->desc_len);
+	skb_put_data(skbcopy, skb->data, skb->len);
 
 	skb_queue_tail(&intf->frame_dump_skbqueue, skbcopy);
 	wake_up_interruptible(&intf->frame_dump_waitqueue);
@@ -460,7 +459,7 @@ static ssize_t rt2x00debug_read_##__name(struct file *file,	\
 	if (debug->__name.flags & RT2X00DEBUGFS_OFFSET)		\
 		index *= debug->__name.word_size;		\
 								\
-	debug->__name.read(intf->rt2x00dev, index, &value);	\
+	value = debug->__name.read(intf->rt2x00dev, index);	\
 								\
 	size = sprintf(line, __format, value);			\
 								\
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2x00debug.h b/drivers/net/wireless/ralink/rt2x00/rt2x00debug.h
index e65712c235bd..a357a0727a0b 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2x00debug.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00debug.h
@@ -38,8 +38,8 @@ enum rt2x00debugfs_entry_flags {
 
 #define RT2X00DEBUGFS_REGISTER_ENTRY(__name, __type)		\
 struct reg##__name {						\
-	void (*read)(struct rt2x00_dev *rt2x00dev,		\
-		     const unsigned int word, __type *data);	\
+	__type (*read)(struct rt2x00_dev *rt2x00dev,		\
+		     const unsigned int word);			\
 	void (*write)(struct rt2x00_dev *rt2x00dev,		\
 		      const unsigned int word, __type data);	\
 								\
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2x00mmio.c b/drivers/net/wireless/ralink/rt2x00/rt2x00mmio.c
index da38d254c26f..528cb0401df1 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2x00mmio.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00mmio.c
@@ -43,7 +43,7 @@ int rt2x00mmio_regbusy_read(struct rt2x00_dev *rt2x00dev,
 		return 0;
 
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2x00mmio_register_read(rt2x00dev, offset, reg);
+		*reg = rt2x00mmio_register_read(rt2x00dev, offset);
 		if (!rt2x00_get_field32(*reg, field))
 			return 1;
 		udelay(REGISTER_BUSY_DELAY);
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2x00mmio.h b/drivers/net/wireless/ralink/rt2x00/rt2x00mmio.h
index 701c3127efb9..184a4148b2f8 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2x00mmio.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00mmio.h
@@ -29,11 +29,10 @@
 /*
  * Register access.
  */
-static inline void rt2x00mmio_register_read(struct rt2x00_dev *rt2x00dev,
-					    const unsigned int offset,
-					    u32 *value)
+static inline u32 rt2x00mmio_register_read(struct rt2x00_dev *rt2x00dev,
+					   const unsigned int offset)
 {
-	*value = readl(rt2x00dev->csr.base + offset);
+	return readl(rt2x00dev->csr.base + offset);
 }
 
 static inline void rt2x00mmio_register_multiread(struct rt2x00_dev *rt2x00dev,
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2x00queue.h b/drivers/net/wireless/ralink/rt2x00/rt2x00queue.h
index 6055f36211b9..a15bae29917b 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2x00queue.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00queue.h
@@ -642,11 +642,10 @@ static inline int rt2x00queue_dma_timeout(struct queue_entry *entry)
  * _rt2x00_desc_read - Read a word from the hardware descriptor.
  * @desc: Base descriptor address
  * @word: Word index from where the descriptor should be read.
- * @value: Address where the descriptor value should be written into.
  */
-static inline void _rt2x00_desc_read(__le32 *desc, const u8 word, __le32 *value)
+static inline __le32 _rt2x00_desc_read(__le32 *desc, const u8 word)
 {
-	*value = desc[word];
+	return desc[word];
 }
 
 /**
@@ -654,13 +653,10 @@ static inline void _rt2x00_desc_read(__le32 *desc, const u8 word, __le32 *value)
  * function will take care of the byte ordering.
  * @desc: Base descriptor address
  * @word: Word index from where the descriptor should be read.
- * @value: Address where the descriptor value should be written into.
  */
-static inline void rt2x00_desc_read(__le32 *desc, const u8 word, u32 *value)
+static inline u32 rt2x00_desc_read(__le32 *desc, const u8 word)
 {
-	__le32 tmp;
-	_rt2x00_desc_read(desc, word, &tmp);
-	*value = le32_to_cpu(tmp);
+	return le32_to_cpu(_rt2x00_desc_read(desc, word));
 }
 
 /**
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2x00usb.c b/drivers/net/wireless/ralink/rt2x00/rt2x00usb.c
index c696f0ad6a68..e2f4f5778267 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00usb.c
@@ -145,7 +145,7 @@ int rt2x00usb_regbusy_read(struct rt2x00_dev *rt2x00dev,
 		return -ENODEV;
 
 	for (i = 0; i < REGISTER_USB_BUSY_COUNT; i++) {
-		rt2x00usb_register_read_lock(rt2x00dev, offset, reg);
+		*reg = rt2x00usb_register_read_lock(rt2x00dev, offset);
 		if (!rt2x00_get_field32(*reg, field))
 			return 1;
 		udelay(REGISTER_BUSY_DELAY);
diff --git a/drivers/net/wireless/ralink/rt2x00/rt2x00usb.h b/drivers/net/wireless/ralink/rt2x00/rt2x00usb.h
index 569363da00a2..ff94c6944cfc 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2x00usb.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00usb.h
@@ -190,40 +190,36 @@ static inline int rt2x00usb_eeprom_read(struct rt2x00_dev *rt2x00dev,
  * rt2x00usb_register_read - Read 32bit register word
  * @rt2x00dev: Device pointer, see &struct rt2x00_dev.
  * @offset: Register offset
- * @value: Pointer to where register contents should be stored
  *
  * This function is a simple wrapper for 32bit register access
  * through rt2x00usb_vendor_request_buff().
  */
-static inline void rt2x00usb_register_read(struct rt2x00_dev *rt2x00dev,
-					   const unsigned int offset,
-					   u32 *value)
+static inline u32 rt2x00usb_register_read(struct rt2x00_dev *rt2x00dev,
+					  const unsigned int offset)
 {
 	__le32 reg = 0;
 	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_READ,
 				      USB_VENDOR_REQUEST_IN, offset,
 				      &reg, sizeof(reg));
-	*value = le32_to_cpu(reg);
+	return le32_to_cpu(reg);
 }
 
 /**
  * rt2x00usb_register_read_lock - Read 32bit register word
  * @rt2x00dev: Device pointer, see &struct rt2x00_dev.
  * @offset: Register offset
- * @value: Pointer to where register contents should be stored
  *
  * This function is a simple wrapper for 32bit register access
  * through rt2x00usb_vendor_req_buff_lock().
  */
-static inline void rt2x00usb_register_read_lock(struct rt2x00_dev *rt2x00dev,
-						const unsigned int offset,
-						u32 *value)
+static inline u32 rt2x00usb_register_read_lock(struct rt2x00_dev *rt2x00dev,
+					       const unsigned int offset)
 {
 	__le32 reg = 0;
 	rt2x00usb_vendor_req_buff_lock(rt2x00dev, USB_MULTI_READ,
 				       USB_VENDOR_REQUEST_IN, offset,
 				       &reg, sizeof(reg), REGISTER_TIMEOUT);
-	*value = le32_to_cpu(reg);
+	return le32_to_cpu(reg);
 }
 
 /**
diff --git a/drivers/net/wireless/ralink/rt2x00/rt61pci.c b/drivers/net/wireless/ralink/rt2x00/rt61pci.c
index 973d418b8113..234310200759 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt61pci.c
@@ -86,10 +86,11 @@ static void rt61pci_bbp_write(struct rt2x00_dev *rt2x00dev,
 	mutex_unlock(&rt2x00dev->csr_mutex);
 }
 
-static void rt61pci_bbp_read(struct rt2x00_dev *rt2x00dev,
-			     const unsigned int word, u8 *value)
+static u8 rt61pci_bbp_read(struct rt2x00_dev *rt2x00dev,
+			   const unsigned int word)
 {
 	u32 reg;
+	u8 value;
 
 	mutex_lock(&rt2x00dev->csr_mutex);
 
@@ -112,9 +113,11 @@ static void rt61pci_bbp_read(struct rt2x00_dev *rt2x00dev,
 		WAIT_FOR_BBP(rt2x00dev, &reg);
 	}
 
-	*value = rt2x00_get_field32(reg, PHY_CSR3_VALUE);
+	value = rt2x00_get_field32(reg, PHY_CSR3_VALUE);
 
 	mutex_unlock(&rt2x00dev->csr_mutex);
+
+	return value;
 }
 
 static void rt61pci_rf_write(struct rt2x00_dev *rt2x00dev,
@@ -161,7 +164,7 @@ static void rt61pci_mcu_request(struct rt2x00_dev *rt2x00dev,
 		rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_ARG1, arg1);
 		rt2x00mmio_register_write(rt2x00dev, H2M_MAILBOX_CSR, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, HOST_CMD_CSR, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, HOST_CMD_CSR);
 		rt2x00_set_field32(&reg, HOST_CMD_CSR_HOST_COMMAND, command);
 		rt2x00_set_field32(&reg, HOST_CMD_CSR_INTERRUPT_MCU, 1);
 		rt2x00mmio_register_write(rt2x00dev, HOST_CMD_CSR, reg);
@@ -176,7 +179,7 @@ static void rt61pci_eepromregister_read(struct eeprom_93cx6 *eeprom)
 	struct rt2x00_dev *rt2x00dev = eeprom->data;
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, E2PROM_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, E2PROM_CSR);
 
 	eeprom->reg_data_in = !!rt2x00_get_field32(reg, E2PROM_CSR_DATA_IN);
 	eeprom->reg_data_out = !!rt2x00_get_field32(reg, E2PROM_CSR_DATA_OUT);
@@ -240,7 +243,7 @@ static int rt61pci_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, MAC_CSR13, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR13);
 	return rt2x00_get_field32(reg, MAC_CSR13_VAL5);
 }
 
@@ -291,7 +294,7 @@ static int rt61pci_blink_set(struct led_classdev *led_cdev,
 	    container_of(led_cdev, struct rt2x00_led, led_dev);
 	u32 reg;
 
-	rt2x00mmio_register_read(led->rt2x00dev, MAC_CSR14, &reg);
+	reg = rt2x00mmio_register_read(led->rt2x00dev, MAC_CSR14);
 	rt2x00_set_field32(&reg, MAC_CSR14_ON_PERIOD, *delay_on);
 	rt2x00_set_field32(&reg, MAC_CSR14_OFF_PERIOD, *delay_off);
 	rt2x00mmio_register_write(led->rt2x00dev, MAC_CSR14, reg);
@@ -336,7 +339,7 @@ static int rt61pci_config_shared_key(struct rt2x00_dev *rt2x00dev,
 		 */
 		mask = (0xf << crypto->bssidx);
 
-		rt2x00mmio_register_read(rt2x00dev, SEC_CSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, SEC_CSR0);
 		reg &= mask;
 
 		if (reg && reg == mask)
@@ -369,14 +372,14 @@ static int rt61pci_config_shared_key(struct rt2x00_dev *rt2x00dev,
 			field.bit_offset = (3 * key->hw_key_idx);
 			field.bit_mask = 0x7 << field.bit_offset;
 
-			rt2x00mmio_register_read(rt2x00dev, SEC_CSR1, &reg);
+			reg = rt2x00mmio_register_read(rt2x00dev, SEC_CSR1);
 			rt2x00_set_field32(&reg, field, crypto->cipher);
 			rt2x00mmio_register_write(rt2x00dev, SEC_CSR1, reg);
 		} else {
 			field.bit_offset = (3 * (key->hw_key_idx - 8));
 			field.bit_mask = 0x7 << field.bit_offset;
 
-			rt2x00mmio_register_read(rt2x00dev, SEC_CSR5, &reg);
+			reg = rt2x00mmio_register_read(rt2x00dev, SEC_CSR5);
 			rt2x00_set_field32(&reg, field, crypto->cipher);
 			rt2x00mmio_register_write(rt2x00dev, SEC_CSR5, reg);
 		}
@@ -401,7 +404,7 @@ static int rt61pci_config_shared_key(struct rt2x00_dev *rt2x00dev,
 	 */
 	mask = 1 << key->hw_key_idx;
 
-	rt2x00mmio_register_read(rt2x00dev, SEC_CSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, SEC_CSR0);
 	if (crypto->cmd == SET_KEY)
 		reg |= mask;
 	else if (crypto->cmd == DISABLE_KEY)
@@ -430,10 +433,10 @@ static int rt61pci_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 		 * When both registers are full, we drop the key.
 		 * Otherwise, we use the first invalid entry.
 		 */
-		rt2x00mmio_register_read(rt2x00dev, SEC_CSR2, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, SEC_CSR2);
 		if (reg && reg == ~0) {
 			key->hw_key_idx = 32;
-			rt2x00mmio_register_read(rt2x00dev, SEC_CSR3, &reg);
+			reg = rt2x00mmio_register_read(rt2x00dev, SEC_CSR3);
 			if (reg && reg == ~0)
 				return -ENOSPC;
 		}
@@ -467,7 +470,7 @@ static int rt61pci_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 		 * Without this, received frames will not be decrypted
 		 * by the hardware.
 		 */
-		rt2x00mmio_register_read(rt2x00dev, SEC_CSR4, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, SEC_CSR4);
 		reg |= (1 << crypto->bssidx);
 		rt2x00mmio_register_write(rt2x00dev, SEC_CSR4, reg);
 
@@ -492,7 +495,7 @@ static int rt61pci_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 	if (key->hw_key_idx < 32) {
 		mask = 1 << key->hw_key_idx;
 
-		rt2x00mmio_register_read(rt2x00dev, SEC_CSR2, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, SEC_CSR2);
 		if (crypto->cmd == SET_KEY)
 			reg |= mask;
 		else if (crypto->cmd == DISABLE_KEY)
@@ -501,7 +504,7 @@ static int rt61pci_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 	} else {
 		mask = 1 << (key->hw_key_idx - 32);
 
-		rt2x00mmio_register_read(rt2x00dev, SEC_CSR3, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, SEC_CSR3);
 		if (crypto->cmd == SET_KEY)
 			reg |= mask;
 		else if (crypto->cmd == DISABLE_KEY)
@@ -523,7 +526,7 @@ static void rt61pci_config_filter(struct rt2x00_dev *rt2x00dev,
 	 * and broadcast frames will always be accepted since
 	 * there is no filter for it at this time.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_CRC,
 			   !(filter_flags & FIF_FCSFAIL));
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_PHYSICAL,
@@ -555,7 +558,7 @@ static void rt61pci_config_intf(struct rt2x00_dev *rt2x00dev,
 		/*
 		 * Enable synchronisation.
 		 */
-		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_SYNC, conf->sync);
 		rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, reg);
 	}
@@ -586,13 +589,13 @@ static void rt61pci_config_erp(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0);
 	rt2x00_set_field32(&reg, TXRX_CSR0_RX_ACK_TIMEOUT, 0x32);
 	rt2x00_set_field32(&reg, TXRX_CSR0_TSF_OFFSET, IEEE80211_HEADER);
 	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR0, reg);
 
 	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
-		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR4, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR4);
 		rt2x00_set_field32(&reg, TXRX_CSR4_AUTORESPOND_ENABLE, 1);
 		rt2x00_set_field32(&reg, TXRX_CSR4_AUTORESPOND_PREAMBLE,
 				   !!erp->short_preamble);
@@ -604,18 +607,18 @@ static void rt61pci_config_erp(struct rt2x00_dev *rt2x00dev,
 					  erp->basic_rates);
 
 	if (changed & BSS_CHANGED_BEACON_INT) {
-		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9);
 		rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_INTERVAL,
 				   erp->beacon_int * 16);
 		rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, reg);
 	}
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
-		rt2x00mmio_register_read(rt2x00dev, MAC_CSR9, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR9);
 		rt2x00_set_field32(&reg, MAC_CSR9_SLOT_TIME, erp->slot_time);
 		rt2x00mmio_register_write(rt2x00dev, MAC_CSR9, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, MAC_CSR8, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR8);
 		rt2x00_set_field32(&reg, MAC_CSR8_SIFS, erp->sifs);
 		rt2x00_set_field32(&reg, MAC_CSR8_SIFS_AFTER_RX_OFDM, 3);
 		rt2x00_set_field32(&reg, MAC_CSR8_EIFS, erp->eifs);
@@ -630,9 +633,9 @@ static void rt61pci_config_antenna_5x(struct rt2x00_dev *rt2x00dev,
 	u8 r4;
 	u8 r77;
 
-	rt61pci_bbp_read(rt2x00dev, 3, &r3);
-	rt61pci_bbp_read(rt2x00dev, 4, &r4);
-	rt61pci_bbp_read(rt2x00dev, 77, &r77);
+	r3 = rt61pci_bbp_read(rt2x00dev, 3);
+	r4 = rt61pci_bbp_read(rt2x00dev, 4);
+	r77 = rt61pci_bbp_read(rt2x00dev, 77);
 
 	rt2x00_set_field8(&r3, BBP_R3_SMART_MODE, rt2x00_rf(rt2x00dev, RF5325));
 
@@ -676,9 +679,9 @@ static void rt61pci_config_antenna_2x(struct rt2x00_dev *rt2x00dev,
 	u8 r4;
 	u8 r77;
 
-	rt61pci_bbp_read(rt2x00dev, 3, &r3);
-	rt61pci_bbp_read(rt2x00dev, 4, &r4);
-	rt61pci_bbp_read(rt2x00dev, 77, &r77);
+	r3 = rt61pci_bbp_read(rt2x00dev, 3);
+	r4 = rt61pci_bbp_read(rt2x00dev, 4);
+	r77 = rt61pci_bbp_read(rt2x00dev, 77);
 
 	rt2x00_set_field8(&r3, BBP_R3_SMART_MODE, rt2x00_rf(rt2x00dev, RF2529));
 	rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END,
@@ -712,7 +715,7 @@ static void rt61pci_config_antenna_2529_rx(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, MAC_CSR13, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR13);
 
 	rt2x00_set_field32(&reg, MAC_CSR13_DIR4, 0);
 	rt2x00_set_field32(&reg, MAC_CSR13_VAL4, p1);
@@ -730,9 +733,9 @@ static void rt61pci_config_antenna_2529(struct rt2x00_dev *rt2x00dev,
 	u8 r4;
 	u8 r77;
 
-	rt61pci_bbp_read(rt2x00dev, 3, &r3);
-	rt61pci_bbp_read(rt2x00dev, 4, &r4);
-	rt61pci_bbp_read(rt2x00dev, 77, &r77);
+	r3 = rt61pci_bbp_read(rt2x00dev, 3);
+	r4 = rt61pci_bbp_read(rt2x00dev, 4);
+	r77 = rt61pci_bbp_read(rt2x00dev, 77);
 
 	/*
 	 * Configure the RX antenna.
@@ -819,7 +822,7 @@ static void rt61pci_config_ant(struct rt2x00_dev *rt2x00dev,
 	for (i = 0; i < ARRAY_SIZE(antenna_sel_a); i++)
 		rt61pci_bbp_write(rt2x00dev, sel[i].word, sel[i].value[lna]);
 
-	rt2x00mmio_register_read(rt2x00dev, PHY_CSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, PHY_CSR0);
 
 	rt2x00_set_field32(&reg, PHY_CSR0_PA_PE_BG,
 			   rt2x00dev->curr_band == NL80211_BAND_2GHZ);
@@ -850,13 +853,13 @@ static void rt61pci_config_lna_gain(struct rt2x00_dev *rt2x00dev,
 		if (rt2x00_has_cap_external_lna_bg(rt2x00dev))
 			lna_gain += 14;
 
-		rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG, &eeprom);
+		eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG);
 		lna_gain -= rt2x00_get_field16(eeprom, EEPROM_RSSI_OFFSET_BG_1);
 	} else {
 		if (rt2x00_has_cap_external_lna_a(rt2x00dev))
 			lna_gain += 14;
 
-		rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_A, &eeprom);
+		eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_A);
 		lna_gain -= rt2x00_get_field16(eeprom, EEPROM_RSSI_OFFSET_A_1);
 	}
 
@@ -875,7 +878,7 @@ static void rt61pci_config_channel(struct rt2x00_dev *rt2x00dev,
 
 	smart = !(rt2x00_rf(rt2x00dev, RF5225) || rt2x00_rf(rt2x00dev, RF2527));
 
-	rt61pci_bbp_read(rt2x00dev, 3, &r3);
+	r3 = rt61pci_bbp_read(rt2x00dev, 3);
 	rt2x00_set_field8(&r3, BBP_R3_SMART_MODE, smart);
 	rt61pci_bbp_write(rt2x00dev, 3, r3);
 
@@ -913,10 +916,10 @@ static void rt61pci_config_txpower(struct rt2x00_dev *rt2x00dev,
 {
 	struct rf_channel rf;
 
-	rt2x00_rf_read(rt2x00dev, 1, &rf.rf1);
-	rt2x00_rf_read(rt2x00dev, 2, &rf.rf2);
-	rt2x00_rf_read(rt2x00dev, 3, &rf.rf3);
-	rt2x00_rf_read(rt2x00dev, 4, &rf.rf4);
+	rf.rf1 = rt2x00_rf_read(rt2x00dev, 1);
+	rf.rf2 = rt2x00_rf_read(rt2x00dev, 2);
+	rf.rf3 = rt2x00_rf_read(rt2x00dev, 3);
+	rf.rf4 = rt2x00_rf_read(rt2x00dev, 4);
 
 	rt61pci_config_channel(rt2x00dev, &rf, txpower);
 }
@@ -926,7 +929,7 @@ static void rt61pci_config_retry_limit(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR4, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR4);
 	rt2x00_set_field32(&reg, TXRX_CSR4_OFDM_TX_RATE_DOWN, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR4_OFDM_TX_RATE_STEP, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR4_OFDM_TX_FALLBACK_CCK, 0);
@@ -946,7 +949,7 @@ static void rt61pci_config_ps(struct rt2x00_dev *rt2x00dev,
 	u32 reg;
 
 	if (state == STATE_SLEEP) {
-		rt2x00mmio_register_read(rt2x00dev, MAC_CSR11, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR11);
 		rt2x00_set_field32(&reg, MAC_CSR11_DELAY_AFTER_TBCN,
 				   rt2x00dev->beacon_int - 10);
 		rt2x00_set_field32(&reg, MAC_CSR11_TBCN_BEFORE_WAKEUP,
@@ -967,7 +970,7 @@ static void rt61pci_config_ps(struct rt2x00_dev *rt2x00dev,
 
 		rt61pci_mcu_request(rt2x00dev, MCU_SLEEP, 0xff, 0, 0);
 	} else {
-		rt2x00mmio_register_read(rt2x00dev, MAC_CSR11, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR11);
 		rt2x00_set_field32(&reg, MAC_CSR11_DELAY_AFTER_TBCN, 0);
 		rt2x00_set_field32(&reg, MAC_CSR11_TBCN_BEFORE_WAKEUP, 0);
 		rt2x00_set_field32(&reg, MAC_CSR11_AUTOWAKE, 0);
@@ -1013,13 +1016,13 @@ static void rt61pci_link_stats(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Update FCS error count from register.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, STA_CSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, STA_CSR0);
 	qual->rx_failed = rt2x00_get_field32(reg, STA_CSR0_FCS_ERROR);
 
 	/*
 	 * Update False CCA count from register.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, STA_CSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, STA_CSR1);
 	qual->false_cca = rt2x00_get_field32(reg, STA_CSR1_FALSE_CCA_ERROR);
 }
 
@@ -1138,12 +1141,12 @@ static void rt61pci_start_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0);
 		rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX, 0);
 		rt2x00mmio_register_write(rt2x00dev, TXRX_CSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 1);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 1);
 		rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 1);
@@ -1161,22 +1164,22 @@ static void rt61pci_kick_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_AC_VO:
-		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC0, 1);
 		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_AC_VI:
-		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC1, 1);
 		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_AC_BE:
-		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC2, 1);
 		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_AC_BK:
-		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC3, 1);
 		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
@@ -1192,32 +1195,32 @@ static void rt61pci_stop_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_AC_VO:
-		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC0, 1);
 		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_AC_VI:
-		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC1, 1);
 		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_AC_BE:
-		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC2, 1);
 		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_AC_BK:
-		rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TX_CNTL_CSR);
 		rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC3, 1);
 		rt2x00mmio_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 		break;
 	case QID_RX:
-		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0);
 		rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX, 1);
 		rt2x00mmio_register_write(rt2x00dev, TXRX_CSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 0);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 0);
 		rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
@@ -1299,7 +1302,7 @@ static int rt61pci_load_firmware(struct rt2x00_dev *rt2x00dev,
 	 * Wait for stable hardware.
 	 */
 	for (i = 0; i < 100; i++) {
-		rt2x00mmio_register_read(rt2x00dev, MAC_CSR0, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR0);
 		if (reg)
 			break;
 		msleep(1);
@@ -1338,7 +1341,7 @@ static int rt61pci_load_firmware(struct rt2x00_dev *rt2x00dev,
 	rt2x00mmio_register_write(rt2x00dev, MCU_CNTL_CSR, reg);
 
 	for (i = 0; i < 100; i++) {
-		rt2x00mmio_register_read(rt2x00dev, MCU_CNTL_CSR, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, MCU_CNTL_CSR);
 		if (rt2x00_get_field32(reg, MCU_CNTL_CSR_READY))
 			break;
 		msleep(1);
@@ -1362,12 +1365,12 @@ static int rt61pci_load_firmware(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 1);
 	rt2x00mmio_register_write(rt2x00dev, MAC_CSR1, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, MAC_CSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR1);
 	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 0);
 	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 0);
 	rt2x00mmio_register_write(rt2x00dev, MAC_CSR1, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, MAC_CSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR1);
 	rt2x00_set_field32(&reg, MAC_CSR1_HOST_READY, 1);
 	rt2x00mmio_register_write(rt2x00dev, MAC_CSR1, reg);
 
@@ -1383,11 +1386,11 @@ static bool rt61pci_get_entry_state(struct queue_entry *entry)
 	u32 word;
 
 	if (entry->queue->qid == QID_RX) {
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 
 		return rt2x00_get_field32(word, RXD_W0_OWNER_NIC);
 	} else {
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 
 		return (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
 		        rt2x00_get_field32(word, TXD_W0_VALID));
@@ -1401,16 +1404,16 @@ static void rt61pci_clear_entry(struct queue_entry *entry)
 	u32 word;
 
 	if (entry->queue->qid == QID_RX) {
-		rt2x00_desc_read(entry_priv->desc, 5, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 5);
 		rt2x00_set_field32(&word, RXD_W5_BUFFER_PHYSICAL_ADDRESS,
 				   skbdesc->skb_dma);
 		rt2x00_desc_write(entry_priv->desc, 5, word);
 
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 		rt2x00_set_field32(&word, RXD_W0_OWNER_NIC, 1);
 		rt2x00_desc_write(entry_priv->desc, 0, word);
 	} else {
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 		rt2x00_set_field32(&word, TXD_W0_VALID, 0);
 		rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 0);
 		rt2x00_desc_write(entry_priv->desc, 0, word);
@@ -1425,7 +1428,7 @@ static int rt61pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize registers.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, TX_RING_CSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TX_RING_CSR0);
 	rt2x00_set_field32(&reg, TX_RING_CSR0_AC0_RING_SIZE,
 			   rt2x00dev->tx[0].limit);
 	rt2x00_set_field32(&reg, TX_RING_CSR0_AC1_RING_SIZE,
@@ -1436,36 +1439,36 @@ static int rt61pci_init_queues(struct rt2x00_dev *rt2x00dev)
 			   rt2x00dev->tx[3].limit);
 	rt2x00mmio_register_write(rt2x00dev, TX_RING_CSR0, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, TX_RING_CSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TX_RING_CSR1);
 	rt2x00_set_field32(&reg, TX_RING_CSR1_TXD_SIZE,
 			   rt2x00dev->tx[0].desc_size / 4);
 	rt2x00mmio_register_write(rt2x00dev, TX_RING_CSR1, reg);
 
 	entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, AC0_BASE_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, AC0_BASE_CSR);
 	rt2x00_set_field32(&reg, AC0_BASE_CSR_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, AC0_BASE_CSR, reg);
 
 	entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, AC1_BASE_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, AC1_BASE_CSR);
 	rt2x00_set_field32(&reg, AC1_BASE_CSR_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, AC1_BASE_CSR, reg);
 
 	entry_priv = rt2x00dev->tx[2].entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, AC2_BASE_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, AC2_BASE_CSR);
 	rt2x00_set_field32(&reg, AC2_BASE_CSR_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, AC2_BASE_CSR, reg);
 
 	entry_priv = rt2x00dev->tx[3].entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, AC3_BASE_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, AC3_BASE_CSR);
 	rt2x00_set_field32(&reg, AC3_BASE_CSR_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, AC3_BASE_CSR, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, RX_RING_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RX_RING_CSR);
 	rt2x00_set_field32(&reg, RX_RING_CSR_RING_SIZE, rt2x00dev->rx->limit);
 	rt2x00_set_field32(&reg, RX_RING_CSR_RXD_SIZE,
 			   rt2x00dev->rx->desc_size / 4);
@@ -1473,26 +1476,26 @@ static int rt61pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	rt2x00mmio_register_write(rt2x00dev, RX_RING_CSR, reg);
 
 	entry_priv = rt2x00dev->rx->entries[0].priv_data;
-	rt2x00mmio_register_read(rt2x00dev, RX_BASE_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RX_BASE_CSR);
 	rt2x00_set_field32(&reg, RX_BASE_CSR_RING_REGISTER,
 			   entry_priv->desc_dma);
 	rt2x00mmio_register_write(rt2x00dev, RX_BASE_CSR, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, TX_DMA_DST_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TX_DMA_DST_CSR);
 	rt2x00_set_field32(&reg, TX_DMA_DST_CSR_DEST_AC0, 2);
 	rt2x00_set_field32(&reg, TX_DMA_DST_CSR_DEST_AC1, 2);
 	rt2x00_set_field32(&reg, TX_DMA_DST_CSR_DEST_AC2, 2);
 	rt2x00_set_field32(&reg, TX_DMA_DST_CSR_DEST_AC3, 2);
 	rt2x00mmio_register_write(rt2x00dev, TX_DMA_DST_CSR, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, LOAD_TX_RING_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, LOAD_TX_RING_CSR);
 	rt2x00_set_field32(&reg, LOAD_TX_RING_CSR_LOAD_TXD_AC0, 1);
 	rt2x00_set_field32(&reg, LOAD_TX_RING_CSR_LOAD_TXD_AC1, 1);
 	rt2x00_set_field32(&reg, LOAD_TX_RING_CSR_LOAD_TXD_AC2, 1);
 	rt2x00_set_field32(&reg, LOAD_TX_RING_CSR_LOAD_TXD_AC3, 1);
 	rt2x00mmio_register_write(rt2x00dev, LOAD_TX_RING_CSR, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, RX_CNTL_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RX_CNTL_CSR);
 	rt2x00_set_field32(&reg, RX_CNTL_CSR_LOAD_RXD, 1);
 	rt2x00mmio_register_write(rt2x00dev, RX_CNTL_CSR, reg);
 
@@ -1503,13 +1506,13 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR0);
 	rt2x00_set_field32(&reg, TXRX_CSR0_AUTO_TX_SEQ, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR0_TX_WITHOUT_WAITING, 0);
 	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR0, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR1);
 	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID0, 47); /* CCK Signal */
 	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID1, 30); /* Rssi */
@@ -1523,7 +1526,7 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * CCK TXD BBP registers
 	 */
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR2, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR2);
 	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID0, 13);
 	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID1, 12);
@@ -1537,7 +1540,7 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * OFDM TXD BBP registers
 	 */
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR3, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR3);
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID0, 7);
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID1, 6);
@@ -1546,21 +1549,21 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID2_VALID, 1);
 	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR3, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR7, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR7);
 	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_6MBS, 59);
 	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_9MBS, 53);
 	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_12MBS, 49);
 	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_18MBS, 46);
 	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR7, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR8, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR8);
 	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_24MBS, 44);
 	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_36MBS, 42);
 	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_48MBS, 42);
 	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_54MBS, 42);
 	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR8, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9);
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_INTERVAL, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_SYNC, 0);
@@ -1573,7 +1576,7 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 
 	rt2x00mmio_register_write(rt2x00dev, MAC_CSR6, 0x00000fff);
 
-	rt2x00mmio_register_read(rt2x00dev, MAC_CSR9, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR9);
 	rt2x00_set_field32(&reg, MAC_CSR9_CW_SELECT, 0);
 	rt2x00mmio_register_write(rt2x00dev, MAC_CSR9, reg);
 
@@ -1619,24 +1622,24 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	 * These registers are cleared on read,
 	 * so we may pass a useless variable to store the value.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, STA_CSR0, &reg);
-	rt2x00mmio_register_read(rt2x00dev, STA_CSR1, &reg);
-	rt2x00mmio_register_read(rt2x00dev, STA_CSR2, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, STA_CSR0);
+	reg = rt2x00mmio_register_read(rt2x00dev, STA_CSR1);
+	reg = rt2x00mmio_register_read(rt2x00dev, STA_CSR2);
 
 	/*
 	 * Reset MAC and BBP registers.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, MAC_CSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR1);
 	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 1);
 	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 1);
 	rt2x00mmio_register_write(rt2x00dev, MAC_CSR1, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, MAC_CSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR1);
 	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 0);
 	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 0);
 	rt2x00mmio_register_write(rt2x00dev, MAC_CSR1, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, MAC_CSR1, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR1);
 	rt2x00_set_field32(&reg, MAC_CSR1_HOST_READY, 1);
 	rt2x00mmio_register_write(rt2x00dev, MAC_CSR1, reg);
 
@@ -1649,7 +1652,7 @@ static int rt61pci_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 	u8 value;
 
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt61pci_bbp_read(rt2x00dev, 0, &value);
+		value = rt61pci_bbp_read(rt2x00dev, 0);
 		if ((value != 0xff) && (value != 0x00))
 			return 0;
 		udelay(REGISTER_BUSY_DELAY);
@@ -1695,7 +1698,7 @@ static int rt61pci_init_bbp(struct rt2x00_dev *rt2x00dev)
 	rt61pci_bbp_write(rt2x00dev, 107, 0x04);
 
 	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
-		rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i, &eeprom);
+		eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i);
 
 		if (eeprom != 0xffff && eeprom != 0x0000) {
 			reg_id = rt2x00_get_field16(eeprom, EEPROM_BBP_REG_ID);
@@ -1722,10 +1725,10 @@ static void rt61pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 * should clear the register to assure a clean state.
 	 */
 	if (state == STATE_RADIO_IRQ_ON) {
-		rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR);
 		rt2x00mmio_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
 
-		rt2x00mmio_register_read(rt2x00dev, MCU_INT_SOURCE_CSR, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, MCU_INT_SOURCE_CSR);
 		rt2x00mmio_register_write(rt2x00dev, MCU_INT_SOURCE_CSR, reg);
 	}
 
@@ -1735,7 +1738,7 @@ static void rt61pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irqsave(&rt2x00dev->irqmask_lock, flags);
 
-	rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR);
 	rt2x00_set_field32(&reg, INT_MASK_CSR_TXDONE, mask);
 	rt2x00_set_field32(&reg, INT_MASK_CSR_RXDONE, mask);
 	rt2x00_set_field32(&reg, INT_MASK_CSR_BEACON_DONE, mask);
@@ -1743,7 +1746,7 @@ static void rt61pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&reg, INT_MASK_CSR_MITIGATION_PERIOD, 0xff);
 	rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, MCU_INT_MASK_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MCU_INT_MASK_CSR);
 	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_0, mask);
 	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_1, mask);
 	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_2, mask);
@@ -1783,7 +1786,7 @@ static int rt61pci_enable_radio(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Enable RX.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, RX_CNTL_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, RX_CNTL_CSR);
 	rt2x00_set_field32(&reg, RX_CNTL_CSR_ENABLE_RX_DMA, 1);
 	rt2x00mmio_register_write(rt2x00dev, RX_CNTL_CSR, reg);
 
@@ -1806,7 +1809,7 @@ static int rt61pci_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
 
 	put_to_sleep = (state != STATE_AWAKE);
 
-	rt2x00mmio_register_read(rt2x00dev, MAC_CSR12, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR12);
 	rt2x00_set_field32(&reg, MAC_CSR12_FORCE_WAKEUP, !put_to_sleep);
 	rt2x00_set_field32(&reg, MAC_CSR12_PUT_TO_SLEEP, put_to_sleep);
 	rt2x00mmio_register_write(rt2x00dev, MAC_CSR12, reg);
@@ -1817,7 +1820,7 @@ static int rt61pci_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
 	 * device has entered the correct state.
 	 */
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2x00mmio_register_read(rt2x00dev, MAC_CSR12, &reg2);
+		reg2 = rt2x00mmio_register_read(rt2x00dev, MAC_CSR12);
 		state = rt2x00_get_field32(reg2, MAC_CSR12_BBP_CURRENT_STATE);
 		if (state == !put_to_sleep)
 			return 0;
@@ -1876,7 +1879,7 @@ static void rt61pci_write_tx_desc(struct queue_entry *entry,
 	/*
 	 * Start writing the descriptor words.
 	 */
-	rt2x00_desc_read(txd, 1, &word);
+	word = rt2x00_desc_read(txd, 1);
 	rt2x00_set_field32(&word, TXD_W1_HOST_Q_ID, entry->queue->qid);
 	rt2x00_set_field32(&word, TXD_W1_AIFSN, entry->queue->aifs);
 	rt2x00_set_field32(&word, TXD_W1_CWMIN, entry->queue->cw_min);
@@ -1887,7 +1890,7 @@ static void rt61pci_write_tx_desc(struct queue_entry *entry,
 	rt2x00_set_field32(&word, TXD_W1_BUFFER_COUNT, 1);
 	rt2x00_desc_write(txd, 1, word);
 
-	rt2x00_desc_read(txd, 2, &word);
+	word = rt2x00_desc_read(txd, 2);
 	rt2x00_set_field32(&word, TXD_W2_PLCP_SIGNAL, txdesc->u.plcp.signal);
 	rt2x00_set_field32(&word, TXD_W2_PLCP_SERVICE, txdesc->u.plcp.service);
 	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_LOW,
@@ -1901,7 +1904,7 @@ static void rt61pci_write_tx_desc(struct queue_entry *entry,
 		_rt2x00_desc_write(txd, 4, skbdesc->iv[1]);
 	}
 
-	rt2x00_desc_read(txd, 5, &word);
+	word = rt2x00_desc_read(txd, 5);
 	rt2x00_set_field32(&word, TXD_W5_PID_TYPE, entry->queue->qid);
 	rt2x00_set_field32(&word, TXD_W5_PID_SUBTYPE, entry->entry_idx);
 	rt2x00_set_field32(&word, TXD_W5_TX_POWER,
@@ -1910,12 +1913,12 @@ static void rt61pci_write_tx_desc(struct queue_entry *entry,
 	rt2x00_desc_write(txd, 5, word);
 
 	if (entry->queue->qid != QID_BEACON) {
-		rt2x00_desc_read(txd, 6, &word);
+		word = rt2x00_desc_read(txd, 6);
 		rt2x00_set_field32(&word, TXD_W6_BUFFER_PHYSICAL_ADDRESS,
 				   skbdesc->skb_dma);
 		rt2x00_desc_write(txd, 6, word);
 
-		rt2x00_desc_read(txd, 11, &word);
+		word = rt2x00_desc_read(txd, 11);
 		rt2x00_set_field32(&word, TXD_W11_BUFFER_LENGTH0,
 				   txdesc->length);
 		rt2x00_desc_write(txd, 11, word);
@@ -1926,7 +1929,7 @@ static void rt61pci_write_tx_desc(struct queue_entry *entry,
 	 * the device, whereby the device may take hold of the TXD before we
 	 * finished updating it.
 	 */
-	rt2x00_desc_read(txd, 0, &word);
+	word = rt2x00_desc_read(txd, 0);
 	rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 1);
 	rt2x00_set_field32(&word, TXD_W0_VALID, 1);
 	rt2x00_set_field32(&word, TXD_W0_MORE_FRAG,
@@ -1975,7 +1978,7 @@ static void rt61pci_write_beacon(struct queue_entry *entry,
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9);
 	orig_reg = reg;
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
 	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, reg);
@@ -2036,7 +2039,7 @@ static void rt61pci_clear_beacon(struct queue_entry *entry)
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9, &orig_reg);
+	orig_reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR9);
 	reg = orig_reg;
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
 	rt2x00mmio_register_write(rt2x00dev, TXRX_CSR9, reg);
@@ -2092,8 +2095,8 @@ static void rt61pci_fill_rxdone(struct queue_entry *entry,
 	u32 word0;
 	u32 word1;
 
-	rt2x00_desc_read(entry_priv->desc, 0, &word0);
-	rt2x00_desc_read(entry_priv->desc, 1, &word1);
+	word0 = rt2x00_desc_read(entry_priv->desc, 0);
+	word1 = rt2x00_desc_read(entry_priv->desc, 1);
 
 	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
 		rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
@@ -2102,11 +2105,11 @@ static void rt61pci_fill_rxdone(struct queue_entry *entry,
 	rxdesc->cipher_status = rt2x00_get_field32(word0, RXD_W0_CIPHER_ERROR);
 
 	if (rxdesc->cipher != CIPHER_NONE) {
-		_rt2x00_desc_read(entry_priv->desc, 2, &rxdesc->iv[0]);
-		_rt2x00_desc_read(entry_priv->desc, 3, &rxdesc->iv[1]);
+		rxdesc->iv[0] = _rt2x00_desc_read(entry_priv->desc, 2);
+		rxdesc->iv[1] = _rt2x00_desc_read(entry_priv->desc, 3);
 		rxdesc->dev_flags |= RXDONE_CRYPTO_IV;
 
-		_rt2x00_desc_read(entry_priv->desc, 4, &rxdesc->icv);
+		rxdesc->icv = _rt2x00_desc_read(entry_priv->desc, 4);
 		rxdesc->dev_flags |= RXDONE_CRYPTO_ICV;
 
 		/*
@@ -2172,7 +2175,7 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
 	 * tx ring size for now.
 	 */
 	for (i = 0; i < rt2x00dev->tx->limit; i++) {
-		rt2x00mmio_register_read(rt2x00dev, STA_CSR4, &reg);
+		reg = rt2x00mmio_register_read(rt2x00dev, STA_CSR4);
 		if (!rt2x00_get_field32(reg, STA_CSR4_VALID))
 			break;
 
@@ -2195,7 +2198,7 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
 
 		entry = &queue->entries[index];
 		entry_priv = entry->priv_data;
-		rt2x00_desc_read(entry_priv->desc, 0, &word);
+		word = rt2x00_desc_read(entry_priv->desc, 0);
 
 		if (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
 		    !rt2x00_get_field32(word, TXD_W0_VALID))
@@ -2258,7 +2261,7 @@ static inline void rt61pci_enable_interrupt(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-	rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR);
 	rt2x00_set_field32(&reg, irq_field, 0);
 	rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
 
@@ -2276,7 +2279,7 @@ static void rt61pci_enable_mcu_interrupt(struct rt2x00_dev *rt2x00dev,
 	 */
 	spin_lock_irq(&rt2x00dev->irqmask_lock);
 
-	rt2x00mmio_register_read(rt2x00dev, MCU_INT_MASK_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MCU_INT_MASK_CSR);
 	rt2x00_set_field32(&reg, irq_field, 0);
 	rt2x00mmio_register_write(rt2x00dev, MCU_INT_MASK_CSR, reg);
 
@@ -2328,10 +2331,10 @@ static irqreturn_t rt61pci_interrupt(int irq, void *dev_instance)
 	 * Get the interrupt sources & saved to local variable.
 	 * Write register value back to clear pending interrupts.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, MCU_INT_SOURCE_CSR, &reg_mcu);
+	reg_mcu = rt2x00mmio_register_read(rt2x00dev, MCU_INT_SOURCE_CSR);
 	rt2x00mmio_register_write(rt2x00dev, MCU_INT_SOURCE_CSR, reg_mcu);
 
-	rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR);
 	rt2x00mmio_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
 
 	if (!reg && !reg_mcu)
@@ -2369,11 +2372,11 @@ static irqreturn_t rt61pci_interrupt(int irq, void *dev_instance)
 	 */
 	spin_lock(&rt2x00dev->irqmask_lock);
 
-	rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR);
 	reg |= mask;
 	rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, MCU_INT_MASK_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MCU_INT_MASK_CSR);
 	reg |= mask_mcu;
 	rt2x00mmio_register_write(rt2x00dev, MCU_INT_MASK_CSR, reg);
 
@@ -2393,7 +2396,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	u8 *mac;
 	s8 value;
 
-	rt2x00mmio_register_read(rt2x00dev, E2PROM_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, E2PROM_CSR);
 
 	eeprom.data = rt2x00dev;
 	eeprom.register_read = rt61pci_eepromregister_read;
@@ -2414,7 +2417,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	rt2x00lib_set_mac_address(rt2x00dev, mac);
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_NUM, 2);
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_TX_DEFAULT,
@@ -2429,7 +2432,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "Antenna: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_NIC_ENABLE_DIVERSITY, 0);
 		rt2x00_set_field16(&word, EEPROM_NIC_TX_DIVERSITY, 0);
@@ -2442,7 +2445,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "NIC: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_LED);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_LED_LED_MODE,
 				   LED_MODE_DEFAULT);
@@ -2450,7 +2453,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "Led: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_FREQ_OFFSET, 0);
 		rt2x00_set_field16(&word, EEPROM_FREQ_SEQ, 0);
@@ -2458,7 +2461,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "Freq: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_1, 0);
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_2, 0);
@@ -2474,7 +2477,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_BG, word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_A, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_A);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_1, 0);
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_2, 0);
@@ -2502,13 +2505,13 @@ static int rt61pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Read EEPROM word for configuration.
 	 */
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA);
 
 	/*
 	 * Identify RF chipset.
 	 */
 	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
-	rt2x00mmio_register_read(rt2x00dev, MAC_CSR0, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR0);
 	rt2x00_set_chip(rt2x00dev, rt2x00_get_field32(reg, MAC_CSR0_CHIPSET),
 			value, rt2x00_get_field32(reg, MAC_CSR0_REVISION));
 
@@ -2549,7 +2552,7 @@ static int rt61pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Read frequency offset and RF programming sequence.
 	 */
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ);
 	if (rt2x00_get_field16(eeprom, EEPROM_FREQ_SEQ))
 		__set_bit(CAPABILITY_RF_SEQUENCE, &rt2x00dev->cap_flags);
 
@@ -2558,7 +2561,7 @@ static int rt61pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Read external LNA informations.
 	 */
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC);
 
 	if (rt2x00_get_field16(eeprom, EEPROM_NIC_EXTERNAL_LNA_A))
 		__set_bit(CAPABILITY_EXTERNAL_LNA_A, &rt2x00dev->cap_flags);
@@ -2589,7 +2592,7 @@ static int rt61pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	 * switch to default led mode.
 	 */
 #ifdef CONFIG_RT2X00_LIB_LEDS
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_LED);
 	value = rt2x00_get_field16(eeprom, EEPROM_LED_LED_MODE);
 
 	rt61pci_init_led(rt2x00dev, &rt2x00dev->led_radio, LED_TYPE_RADIO);
@@ -2850,7 +2853,7 @@ static int rt61pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 	 * Enable rfkill polling by setting GPIO direction of the
 	 * rfkill switch GPIO pin correctly.
 	 */
-	rt2x00mmio_register_read(rt2x00dev, MAC_CSR13, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, MAC_CSR13);
 	rt2x00_set_field32(&reg, MAC_CSR13_DIR5, 1);
 	rt2x00mmio_register_write(rt2x00dev, MAC_CSR13, reg);
 
@@ -2922,7 +2925,7 @@ static int rt61pci_conf_tx(struct ieee80211_hw *hw,
 	field.bit_offset = (queue_idx & 1) * 16;
 	field.bit_mask = 0xffff << field.bit_offset;
 
-	rt2x00mmio_register_read(rt2x00dev, offset, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, offset);
 	rt2x00_set_field32(&reg, field, queue->txop);
 	rt2x00mmio_register_write(rt2x00dev, offset, reg);
 
@@ -2930,15 +2933,15 @@ static int rt61pci_conf_tx(struct ieee80211_hw *hw,
 	field.bit_offset = queue_idx * 4;
 	field.bit_mask = 0xf << field.bit_offset;
 
-	rt2x00mmio_register_read(rt2x00dev, AIFSN_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, AIFSN_CSR);
 	rt2x00_set_field32(&reg, field, queue->aifs);
 	rt2x00mmio_register_write(rt2x00dev, AIFSN_CSR, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, CWMIN_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CWMIN_CSR);
 	rt2x00_set_field32(&reg, field, queue->cw_min);
 	rt2x00mmio_register_write(rt2x00dev, CWMIN_CSR, reg);
 
-	rt2x00mmio_register_read(rt2x00dev, CWMAX_CSR, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, CWMAX_CSR);
 	rt2x00_set_field32(&reg, field, queue->cw_max);
 	rt2x00mmio_register_write(rt2x00dev, CWMAX_CSR, reg);
 
@@ -2951,9 +2954,9 @@ static u64 rt61pci_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	u64 tsf;
 	u32 reg;
 
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR13, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR13);
 	tsf = (u64) rt2x00_get_field32(reg, TXRX_CSR13_HIGH_TSFTIMER) << 32;
-	rt2x00mmio_register_read(rt2x00dev, TXRX_CSR12, &reg);
+	reg = rt2x00mmio_register_read(rt2x00dev, TXRX_CSR12);
 	tsf |= rt2x00_get_field32(reg, TXRX_CSR12_LOW_TSFTIMER);
 
 	return tsf;
diff --git a/drivers/net/wireless/ralink/rt2x00/rt73usb.c b/drivers/net/wireless/ralink/rt2x00/rt73usb.c
index bb8d307a789f..fd913222abd1 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt73usb.c
@@ -84,10 +84,11 @@ static void rt73usb_bbp_write(struct rt2x00_dev *rt2x00dev,
 	mutex_unlock(&rt2x00dev->csr_mutex);
 }
 
-static void rt73usb_bbp_read(struct rt2x00_dev *rt2x00dev,
-			     const unsigned int word, u8 *value)
+static u8 rt73usb_bbp_read(struct rt2x00_dev *rt2x00dev,
+			   const unsigned int word)
 {
 	u32 reg;
+	u8 value;
 
 	mutex_lock(&rt2x00dev->csr_mutex);
 
@@ -110,9 +111,11 @@ static void rt73usb_bbp_read(struct rt2x00_dev *rt2x00dev,
 		WAIT_FOR_BBP(rt2x00dev, &reg);
 	}
 
-	*value = rt2x00_get_field32(reg, PHY_CSR3_VALUE);
+	value = rt2x00_get_field32(reg, PHY_CSR3_VALUE);
 
 	mutex_unlock(&rt2x00dev->csr_mutex);
+
+	return value;
 }
 
 static void rt73usb_rf_write(struct rt2x00_dev *rt2x00dev,
@@ -185,7 +188,7 @@ static int rt73usb_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2x00usb_register_read(rt2x00dev, MAC_CSR13, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR13);
 	return rt2x00_get_field32(reg, MAC_CSR13_VAL7);
 }
 
@@ -238,7 +241,7 @@ static int rt73usb_blink_set(struct led_classdev *led_cdev,
 	    container_of(led_cdev, struct rt2x00_led, led_dev);
 	u32 reg;
 
-	rt2x00usb_register_read(led->rt2x00dev, MAC_CSR14, &reg);
+	reg = rt2x00usb_register_read(led->rt2x00dev, MAC_CSR14);
 	rt2x00_set_field32(&reg, MAC_CSR14_ON_PERIOD, *delay_on);
 	rt2x00_set_field32(&reg, MAC_CSR14_OFF_PERIOD, *delay_off);
 	rt2x00usb_register_write(led->rt2x00dev, MAC_CSR14, reg);
@@ -283,7 +286,7 @@ static int rt73usb_config_shared_key(struct rt2x00_dev *rt2x00dev,
 		 */
 		mask = (0xf << crypto->bssidx);
 
-		rt2x00usb_register_read(rt2x00dev, SEC_CSR0, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, SEC_CSR0);
 		reg &= mask;
 
 		if (reg && reg == mask)
@@ -316,14 +319,14 @@ static int rt73usb_config_shared_key(struct rt2x00_dev *rt2x00dev,
 			field.bit_offset = (3 * key->hw_key_idx);
 			field.bit_mask = 0x7 << field.bit_offset;
 
-			rt2x00usb_register_read(rt2x00dev, SEC_CSR1, &reg);
+			reg = rt2x00usb_register_read(rt2x00dev, SEC_CSR1);
 			rt2x00_set_field32(&reg, field, crypto->cipher);
 			rt2x00usb_register_write(rt2x00dev, SEC_CSR1, reg);
 		} else {
 			field.bit_offset = (3 * (key->hw_key_idx - 8));
 			field.bit_mask = 0x7 << field.bit_offset;
 
-			rt2x00usb_register_read(rt2x00dev, SEC_CSR5, &reg);
+			reg = rt2x00usb_register_read(rt2x00dev, SEC_CSR5);
 			rt2x00_set_field32(&reg, field, crypto->cipher);
 			rt2x00usb_register_write(rt2x00dev, SEC_CSR5, reg);
 		}
@@ -348,7 +351,7 @@ static int rt73usb_config_shared_key(struct rt2x00_dev *rt2x00dev,
 	 */
 	mask = 1 << key->hw_key_idx;
 
-	rt2x00usb_register_read(rt2x00dev, SEC_CSR0, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, SEC_CSR0);
 	if (crypto->cmd == SET_KEY)
 		reg |= mask;
 	else if (crypto->cmd == DISABLE_KEY)
@@ -377,10 +380,10 @@ static int rt73usb_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 		 * When both registers are full, we drop the key,
 		 * otherwise we use the first invalid entry.
 		 */
-		rt2x00usb_register_read(rt2x00dev, SEC_CSR2, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, SEC_CSR2);
 		if (reg && reg == ~0) {
 			key->hw_key_idx = 32;
-			rt2x00usb_register_read(rt2x00dev, SEC_CSR3, &reg);
+			reg = rt2x00usb_register_read(rt2x00dev, SEC_CSR3);
 			if (reg && reg == ~0)
 				return -ENOSPC;
 		}
@@ -417,7 +420,7 @@ static int rt73usb_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 		 * without this received frames will not be decrypted
 		 * by the hardware.
 		 */
-		rt2x00usb_register_read(rt2x00dev, SEC_CSR4, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, SEC_CSR4);
 		reg |= (1 << crypto->bssidx);
 		rt2x00usb_register_write(rt2x00dev, SEC_CSR4, reg);
 
@@ -442,7 +445,7 @@ static int rt73usb_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 	if (key->hw_key_idx < 32) {
 		mask = 1 << key->hw_key_idx;
 
-		rt2x00usb_register_read(rt2x00dev, SEC_CSR2, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, SEC_CSR2);
 		if (crypto->cmd == SET_KEY)
 			reg |= mask;
 		else if (crypto->cmd == DISABLE_KEY)
@@ -451,7 +454,7 @@ static int rt73usb_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 	} else {
 		mask = 1 << (key->hw_key_idx - 32);
 
-		rt2x00usb_register_read(rt2x00dev, SEC_CSR3, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, SEC_CSR3);
 		if (crypto->cmd == SET_KEY)
 			reg |= mask;
 		else if (crypto->cmd == DISABLE_KEY)
@@ -473,7 +476,7 @@ static void rt73usb_config_filter(struct rt2x00_dev *rt2x00dev,
 	 * and broadcast frames will always be accepted since
 	 * there is no filter for it at this time.
 	 */
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR0);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_CRC,
 			   !(filter_flags & FIF_FCSFAIL));
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_PHYSICAL,
@@ -505,7 +508,7 @@ static void rt73usb_config_intf(struct rt2x00_dev *rt2x00dev,
 		/*
 		 * Enable synchronisation.
 		 */
-		rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR9);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_SYNC, conf->sync);
 		rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
 	}
@@ -535,13 +538,13 @@ static void rt73usb_config_erp(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR0);
 	rt2x00_set_field32(&reg, TXRX_CSR0_RX_ACK_TIMEOUT, 0x32);
 	rt2x00_set_field32(&reg, TXRX_CSR0_TSF_OFFSET, IEEE80211_HEADER);
 	rt2x00usb_register_write(rt2x00dev, TXRX_CSR0, reg);
 
 	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
-		rt2x00usb_register_read(rt2x00dev, TXRX_CSR4, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR4);
 		rt2x00_set_field32(&reg, TXRX_CSR4_AUTORESPOND_ENABLE, 1);
 		rt2x00_set_field32(&reg, TXRX_CSR4_AUTORESPOND_PREAMBLE,
 				   !!erp->short_preamble);
@@ -553,18 +556,18 @@ static void rt73usb_config_erp(struct rt2x00_dev *rt2x00dev,
 					 erp->basic_rates);
 
 	if (changed & BSS_CHANGED_BEACON_INT) {
-		rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR9);
 		rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_INTERVAL,
 				   erp->beacon_int * 16);
 		rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
 	}
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
-		rt2x00usb_register_read(rt2x00dev, MAC_CSR9, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR9);
 		rt2x00_set_field32(&reg, MAC_CSR9_SLOT_TIME, erp->slot_time);
 		rt2x00usb_register_write(rt2x00dev, MAC_CSR9, reg);
 
-		rt2x00usb_register_read(rt2x00dev, MAC_CSR8, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR8);
 		rt2x00_set_field32(&reg, MAC_CSR8_SIFS, erp->sifs);
 		rt2x00_set_field32(&reg, MAC_CSR8_SIFS_AFTER_RX_OFDM, 3);
 		rt2x00_set_field32(&reg, MAC_CSR8_EIFS, erp->eifs);
@@ -580,9 +583,9 @@ static void rt73usb_config_antenna_5x(struct rt2x00_dev *rt2x00dev,
 	u8 r77;
 	u8 temp;
 
-	rt73usb_bbp_read(rt2x00dev, 3, &r3);
-	rt73usb_bbp_read(rt2x00dev, 4, &r4);
-	rt73usb_bbp_read(rt2x00dev, 77, &r77);
+	r3 = rt73usb_bbp_read(rt2x00dev, 3);
+	r4 = rt73usb_bbp_read(rt2x00dev, 4);
+	r77 = rt73usb_bbp_read(rt2x00dev, 77);
 
 	rt2x00_set_field8(&r3, BBP_R3_SMART_MODE, 0);
 
@@ -627,9 +630,9 @@ static void rt73usb_config_antenna_2x(struct rt2x00_dev *rt2x00dev,
 	u8 r4;
 	u8 r77;
 
-	rt73usb_bbp_read(rt2x00dev, 3, &r3);
-	rt73usb_bbp_read(rt2x00dev, 4, &r4);
-	rt73usb_bbp_read(rt2x00dev, 77, &r77);
+	r3 = rt73usb_bbp_read(rt2x00dev, 3);
+	r4 = rt73usb_bbp_read(rt2x00dev, 4);
+	r77 = rt73usb_bbp_read(rt2x00dev, 77);
 
 	rt2x00_set_field8(&r3, BBP_R3_SMART_MODE, 0);
 	rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END,
@@ -715,7 +718,7 @@ static void rt73usb_config_ant(struct rt2x00_dev *rt2x00dev,
 	for (i = 0; i < ARRAY_SIZE(antenna_sel_a); i++)
 		rt73usb_bbp_write(rt2x00dev, sel[i].word, sel[i].value[lna]);
 
-	rt2x00usb_register_read(rt2x00dev, PHY_CSR0, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, PHY_CSR0);
 
 	rt2x00_set_field32(&reg, PHY_CSR0_PA_PE_BG,
 			   (rt2x00dev->curr_band == NL80211_BAND_2GHZ));
@@ -740,10 +743,10 @@ static void rt73usb_config_lna_gain(struct rt2x00_dev *rt2x00dev,
 		if (rt2x00_has_cap_external_lna_bg(rt2x00dev))
 			lna_gain += 14;
 
-		rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG, &eeprom);
+		eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG);
 		lna_gain -= rt2x00_get_field16(eeprom, EEPROM_RSSI_OFFSET_BG_1);
 	} else {
-		rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_A, &eeprom);
+		eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_A);
 		lna_gain -= rt2x00_get_field16(eeprom, EEPROM_RSSI_OFFSET_A_1);
 	}
 
@@ -762,7 +765,7 @@ static void rt73usb_config_channel(struct rt2x00_dev *rt2x00dev,
 
 	smart = !(rt2x00_rf(rt2x00dev, RF5225) || rt2x00_rf(rt2x00dev, RF2527));
 
-	rt73usb_bbp_read(rt2x00dev, 3, &r3);
+	r3 = rt73usb_bbp_read(rt2x00dev, 3);
 	rt2x00_set_field8(&r3, BBP_R3_SMART_MODE, smart);
 	rt73usb_bbp_write(rt2x00dev, 3, r3);
 
@@ -796,10 +799,10 @@ static void rt73usb_config_txpower(struct rt2x00_dev *rt2x00dev,
 {
 	struct rf_channel rf;
 
-	rt2x00_rf_read(rt2x00dev, 1, &rf.rf1);
-	rt2x00_rf_read(rt2x00dev, 2, &rf.rf2);
-	rt2x00_rf_read(rt2x00dev, 3, &rf.rf3);
-	rt2x00_rf_read(rt2x00dev, 4, &rf.rf4);
+	rf.rf1 = rt2x00_rf_read(rt2x00dev, 1);
+	rf.rf2 = rt2x00_rf_read(rt2x00dev, 2);
+	rf.rf3 = rt2x00_rf_read(rt2x00dev, 3);
+	rf.rf4 = rt2x00_rf_read(rt2x00dev, 4);
 
 	rt73usb_config_channel(rt2x00dev, &rf, txpower);
 }
@@ -809,7 +812,7 @@ static void rt73usb_config_retry_limit(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR4, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR4);
 	rt2x00_set_field32(&reg, TXRX_CSR4_OFDM_TX_RATE_DOWN, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR4_OFDM_TX_RATE_STEP, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR4_OFDM_TX_FALLBACK_CCK, 0);
@@ -829,7 +832,7 @@ static void rt73usb_config_ps(struct rt2x00_dev *rt2x00dev,
 	u32 reg;
 
 	if (state == STATE_SLEEP) {
-		rt2x00usb_register_read(rt2x00dev, MAC_CSR11, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR11);
 		rt2x00_set_field32(&reg, MAC_CSR11_DELAY_AFTER_TBCN,
 				   rt2x00dev->beacon_int - 10);
 		rt2x00_set_field32(&reg, MAC_CSR11_TBCN_BEFORE_WAKEUP,
@@ -846,7 +849,7 @@ static void rt73usb_config_ps(struct rt2x00_dev *rt2x00dev,
 		rt2x00usb_vendor_request_sw(rt2x00dev, USB_DEVICE_MODE, 0,
 					    USB_MODE_SLEEP, REGISTER_TIMEOUT);
 	} else {
-		rt2x00usb_register_read(rt2x00dev, MAC_CSR11, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR11);
 		rt2x00_set_field32(&reg, MAC_CSR11_DELAY_AFTER_TBCN, 0);
 		rt2x00_set_field32(&reg, MAC_CSR11_TBCN_BEFORE_WAKEUP, 0);
 		rt2x00_set_field32(&reg, MAC_CSR11_AUTOWAKE, 0);
@@ -888,13 +891,13 @@ static void rt73usb_link_stats(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Update FCS error count from register.
 	 */
-	rt2x00usb_register_read(rt2x00dev, STA_CSR0, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, STA_CSR0);
 	qual->rx_failed = rt2x00_get_field32(reg, STA_CSR0_FCS_ERROR);
 
 	/*
 	 * Update False CCA count from register.
 	 */
-	rt2x00usb_register_read(rt2x00dev, STA_CSR1, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, STA_CSR1);
 	qual->false_cca = rt2x00_get_field32(reg, STA_CSR1_FALSE_CCA_ERROR);
 }
 
@@ -1025,12 +1028,12 @@ static void rt73usb_start_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR0);
 		rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX, 0);
 		rt2x00usb_register_write(rt2x00dev, TXRX_CSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR9);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 1);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 1);
 		rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 1);
@@ -1048,12 +1051,12 @@ static void rt73usb_stop_queue(struct data_queue *queue)
 
 	switch (queue->qid) {
 	case QID_RX:
-		rt2x00usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR0);
 		rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX, 1);
 		rt2x00usb_register_write(rt2x00dev, TXRX_CSR0, reg);
 		break;
 	case QID_BEACON:
-		rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR9);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 0);
 		rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 0);
 		rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
@@ -1112,7 +1115,7 @@ static int rt73usb_load_firmware(struct rt2x00_dev *rt2x00dev,
 	 * Wait for stable hardware.
 	 */
 	for (i = 0; i < 100; i++) {
-		rt2x00usb_register_read(rt2x00dev, MAC_CSR0, &reg);
+		reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR0);
 		if (reg)
 			break;
 		msleep(1);
@@ -1150,13 +1153,13 @@ static int rt73usb_init_registers(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR0);
 	rt2x00_set_field32(&reg, TXRX_CSR0_AUTO_TX_SEQ, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR0_TX_WITHOUT_WAITING, 0);
 	rt2x00usb_register_write(rt2x00dev, TXRX_CSR0, reg);
 
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR1, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR1);
 	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID0, 47); /* CCK Signal */
 	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID1, 30); /* Rssi */
@@ -1170,7 +1173,7 @@ static int rt73usb_init_registers(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * CCK TXD BBP registers
 	 */
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR2);
 	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID0, 13);
 	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID1, 12);
@@ -1184,7 +1187,7 @@ static int rt73usb_init_registers(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * OFDM TXD BBP registers
 	 */
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR3, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR3);
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID0, 7);
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID0_VALID, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID1, 6);
@@ -1193,21 +1196,21 @@ static int rt73usb_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID2_VALID, 1);
 	rt2x00usb_register_write(rt2x00dev, TXRX_CSR3, reg);
 
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR7, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR7);
 	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_6MBS, 59);
 	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_9MBS, 53);
 	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_12MBS, 49);
 	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_18MBS, 46);
 	rt2x00usb_register_write(rt2x00dev, TXRX_CSR7, reg);
 
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR8, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR8);
 	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_24MBS, 44);
 	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_36MBS, 42);
 	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_48MBS, 42);
 	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_54MBS, 42);
 	rt2x00usb_register_write(rt2x00dev, TXRX_CSR8, reg);
 
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR9);
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_INTERVAL, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 0);
 	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_SYNC, 0);
@@ -1218,7 +1221,7 @@ static int rt73usb_init_registers(struct rt2x00_dev *rt2x00dev)
 
 	rt2x00usb_register_write(rt2x00dev, TXRX_CSR15, 0x0000000f);
 
-	rt2x00usb_register_read(rt2x00dev, MAC_CSR6, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR6);
 	rt2x00_set_field32(&reg, MAC_CSR6_MAX_FRAME_UNIT, 0xfff);
 	rt2x00usb_register_write(rt2x00dev, MAC_CSR6, reg);
 
@@ -1246,7 +1249,7 @@ static int rt73usb_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00usb_register_write(rt2x00dev, PHY_CSR6, 0x00080606);
 	rt2x00usb_register_write(rt2x00dev, PHY_CSR7, 0x00000408);
 
-	rt2x00usb_register_read(rt2x00dev, MAC_CSR9, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR9);
 	rt2x00_set_field32(&reg, MAC_CSR9_CW_SELECT, 0);
 	rt2x00usb_register_write(rt2x00dev, MAC_CSR9, reg);
 
@@ -1266,24 +1269,24 @@ static int rt73usb_init_registers(struct rt2x00_dev *rt2x00dev)
 	 * These registers are cleared on read,
 	 * so we may pass a useless variable to store the value.
 	 */
-	rt2x00usb_register_read(rt2x00dev, STA_CSR0, &reg);
-	rt2x00usb_register_read(rt2x00dev, STA_CSR1, &reg);
-	rt2x00usb_register_read(rt2x00dev, STA_CSR2, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, STA_CSR0);
+	reg = rt2x00usb_register_read(rt2x00dev, STA_CSR1);
+	reg = rt2x00usb_register_read(rt2x00dev, STA_CSR2);
 
 	/*
 	 * Reset MAC and BBP registers.
 	 */
-	rt2x00usb_register_read(rt2x00dev, MAC_CSR1, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR1);
 	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 1);
 	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 1);
 	rt2x00usb_register_write(rt2x00dev, MAC_CSR1, reg);
 
-	rt2x00usb_register_read(rt2x00dev, MAC_CSR1, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR1);
 	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 0);
 	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 0);
 	rt2x00usb_register_write(rt2x00dev, MAC_CSR1, reg);
 
-	rt2x00usb_register_read(rt2x00dev, MAC_CSR1, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR1);
 	rt2x00_set_field32(&reg, MAC_CSR1_HOST_READY, 1);
 	rt2x00usb_register_write(rt2x00dev, MAC_CSR1, reg);
 
@@ -1296,7 +1299,7 @@ static int rt73usb_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 	u8 value;
 
 	for (i = 0; i < REGISTER_USB_BUSY_COUNT; i++) {
-		rt73usb_bbp_read(rt2x00dev, 0, &value);
+		value = rt73usb_bbp_read(rt2x00dev, 0);
 		if ((value != 0xff) && (value != 0x00))
 			return 0;
 		udelay(REGISTER_BUSY_DELAY);
@@ -1343,7 +1346,7 @@ static int rt73usb_init_bbp(struct rt2x00_dev *rt2x00dev)
 	rt73usb_bbp_write(rt2x00dev, 107, 0x04);
 
 	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
-		rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i, &eeprom);
+		eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i);
 
 		if (eeprom != 0xffff && eeprom != 0x0000) {
 			reg_id = rt2x00_get_field16(eeprom, EEPROM_BBP_REG_ID);
@@ -1390,7 +1393,7 @@ static int rt73usb_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
 
 	put_to_sleep = (state != STATE_AWAKE);
 
-	rt2x00usb_register_read(rt2x00dev, MAC_CSR12, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR12);
 	rt2x00_set_field32(&reg, MAC_CSR12_FORCE_WAKEUP, !put_to_sleep);
 	rt2x00_set_field32(&reg, MAC_CSR12_PUT_TO_SLEEP, put_to_sleep);
 	rt2x00usb_register_write(rt2x00dev, MAC_CSR12, reg);
@@ -1401,7 +1404,7 @@ static int rt73usb_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
 	 * device has entered the correct state.
 	 */
 	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-		rt2x00usb_register_read(rt2x00dev, MAC_CSR12, &reg2);
+		reg2 = rt2x00usb_register_read(rt2x00dev, MAC_CSR12);
 		state = rt2x00_get_field32(reg2, MAC_CSR12_BBP_CURRENT_STATE);
 		if (state == !put_to_sleep)
 			return 0;
@@ -1459,7 +1462,7 @@ static void rt73usb_write_tx_desc(struct queue_entry *entry,
 	/*
 	 * Start writing the descriptor words.
 	 */
-	rt2x00_desc_read(txd, 0, &word);
+	word = rt2x00_desc_read(txd, 0);
 	rt2x00_set_field32(&word, TXD_W0_BURST,
 			   test_bit(ENTRY_TXD_BURST, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_VALID, 1);
@@ -1485,7 +1488,7 @@ static void rt73usb_write_tx_desc(struct queue_entry *entry,
 	rt2x00_set_field32(&word, TXD_W0_CIPHER_ALG, txdesc->cipher);
 	rt2x00_desc_write(txd, 0, word);
 
-	rt2x00_desc_read(txd, 1, &word);
+	word = rt2x00_desc_read(txd, 1);
 	rt2x00_set_field32(&word, TXD_W1_HOST_Q_ID, entry->queue->qid);
 	rt2x00_set_field32(&word, TXD_W1_AIFSN, entry->queue->aifs);
 	rt2x00_set_field32(&word, TXD_W1_CWMIN, entry->queue->cw_min);
@@ -1495,7 +1498,7 @@ static void rt73usb_write_tx_desc(struct queue_entry *entry,
 			   test_bit(ENTRY_TXD_GENERATE_SEQ, &txdesc->flags));
 	rt2x00_desc_write(txd, 1, word);
 
-	rt2x00_desc_read(txd, 2, &word);
+	word = rt2x00_desc_read(txd, 2);
 	rt2x00_set_field32(&word, TXD_W2_PLCP_SIGNAL, txdesc->u.plcp.signal);
 	rt2x00_set_field32(&word, TXD_W2_PLCP_SERVICE, txdesc->u.plcp.service);
 	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_LOW,
@@ -1509,7 +1512,7 @@ static void rt73usb_write_tx_desc(struct queue_entry *entry,
 		_rt2x00_desc_write(txd, 4, skbdesc->iv[1]);
 	}
 
-	rt2x00_desc_read(txd, 5, &word);
+	word = rt2x00_desc_read(txd, 5);
 	rt2x00_set_field32(&word, TXD_W5_TX_POWER,
 			   TXPOWER_TO_DEV(entry->queue->rt2x00dev->tx_power));
 	rt2x00_set_field32(&word, TXD_W5_WAITING_DMA_DONE_INT, 1);
@@ -1538,7 +1541,7 @@ static void rt73usb_write_beacon(struct queue_entry *entry,
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR9);
 	orig_reg = reg;
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
 	rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
@@ -1603,7 +1606,7 @@ static void rt73usb_clear_beacon(struct queue_entry *entry)
 	 * Disable beaconing while we are reloading the beacon data,
 	 * otherwise we might be sending out invalid data.
 	 */
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR9, &orig_reg);
+	orig_reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR9);
 	reg = orig_reg;
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
 	rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
@@ -1691,8 +1694,8 @@ static void rt73usb_fill_rxdone(struct queue_entry *entry,
 	/*
 	 * It is now safe to read the descriptor on all architectures.
 	 */
-	rt2x00_desc_read(rxd, 0, &word0);
-	rt2x00_desc_read(rxd, 1, &word1);
+	word0 = rt2x00_desc_read(rxd, 0);
+	word1 = rt2x00_desc_read(rxd, 1);
 
 	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
 		rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
@@ -1701,11 +1704,11 @@ static void rt73usb_fill_rxdone(struct queue_entry *entry,
 	rxdesc->cipher_status = rt2x00_get_field32(word0, RXD_W0_CIPHER_ERROR);
 
 	if (rxdesc->cipher != CIPHER_NONE) {
-		_rt2x00_desc_read(rxd, 2, &rxdesc->iv[0]);
-		_rt2x00_desc_read(rxd, 3, &rxdesc->iv[1]);
+		rxdesc->iv[0] = _rt2x00_desc_read(rxd, 2);
+		rxdesc->iv[1] = _rt2x00_desc_read(rxd, 3);
 		rxdesc->dev_flags |= RXDONE_CRYPTO_IV;
 
-		_rt2x00_desc_read(rxd, 4, &rxdesc->icv);
+		rxdesc->icv = _rt2x00_desc_read(rxd, 4);
 		rxdesc->dev_flags |= RXDONE_CRYPTO_ICV;
 
 		/*
@@ -1768,7 +1771,7 @@ static int rt73usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	rt2x00lib_set_mac_address(rt2x00dev, mac);
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_NUM, 2);
 		rt2x00_set_field16(&word, EEPROM_ANTENNA_TX_DEFAULT,
@@ -1783,14 +1786,14 @@ static int rt73usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "Antenna: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_NIC_EXTERNAL_LNA, 0);
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_NIC, word);
 		rt2x00_eeprom_dbg(rt2x00dev, "NIC: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_LED);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_LED_POLARITY_RDY_G, 0);
 		rt2x00_set_field16(&word, EEPROM_LED_POLARITY_RDY_A, 0);
@@ -1806,7 +1809,7 @@ static int rt73usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "Led: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_FREQ_OFFSET, 0);
 		rt2x00_set_field16(&word, EEPROM_FREQ_SEQ, 0);
@@ -1814,7 +1817,7 @@ static int rt73usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_dbg(rt2x00dev, "Freq: 0x%04x\n", word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_1, 0);
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_2, 0);
@@ -1830,7 +1833,7 @@ static int rt73usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_BG, word);
 	}
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_A, &word);
+	word = rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_A);
 	if (word == 0xffff) {
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_1, 0);
 		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_2, 0);
@@ -1858,13 +1861,13 @@ static int rt73usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Read EEPROM word for configuration.
 	 */
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA);
 
 	/*
 	 * Identify RF chipset.
 	 */
 	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
-	rt2x00usb_register_read(rt2x00dev, MAC_CSR0, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR0);
 	rt2x00_set_chip(rt2x00dev, rt2x00_get_field32(reg, MAC_CSR0_CHIPSET),
 			value, rt2x00_get_field32(reg, MAC_CSR0_REVISION));
 
@@ -1904,13 +1907,13 @@ static int rt73usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Read frequency offset.
 	 */
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ);
 	rt2x00dev->freq_offset = rt2x00_get_field16(eeprom, EEPROM_FREQ_OFFSET);
 
 	/*
 	 * Read external LNA informations.
 	 */
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC);
 
 	if (rt2x00_get_field16(eeprom, EEPROM_NIC_EXTERNAL_LNA)) {
 		__set_bit(CAPABILITY_EXTERNAL_LNA_A, &rt2x00dev->cap_flags);
@@ -1921,7 +1924,7 @@ static int rt73usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	 * Store led settings, for correct led behaviour.
 	 */
 #ifdef CONFIG_RT2X00_LIB_LEDS
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &eeprom);
+	eeprom = rt2x00_eeprom_read(rt2x00dev, EEPROM_LED);
 
 	rt73usb_init_led(rt2x00dev, &rt2x00dev->led_radio, LED_TYPE_RADIO);
 	rt73usb_init_led(rt2x00dev, &rt2x00dev->led_assoc, LED_TYPE_ASSOC);
@@ -2188,7 +2191,7 @@ static int rt73usb_probe_hw(struct rt2x00_dev *rt2x00dev)
 	 * Enable rfkill polling by setting GPIO direction of the
 	 * rfkill switch GPIO pin correctly.
 	 */
-	rt2x00usb_register_read(rt2x00dev, MAC_CSR13, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, MAC_CSR13);
 	rt2x00_set_field32(&reg, MAC_CSR13_DIR7, 0);
 	rt2x00usb_register_write(rt2x00dev, MAC_CSR13, reg);
 
@@ -2260,7 +2263,7 @@ static int rt73usb_conf_tx(struct ieee80211_hw *hw,
 	field.bit_offset = (queue_idx & 1) * 16;
 	field.bit_mask = 0xffff << field.bit_offset;
 
-	rt2x00usb_register_read(rt2x00dev, offset, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, offset);
 	rt2x00_set_field32(&reg, field, queue->txop);
 	rt2x00usb_register_write(rt2x00dev, offset, reg);
 
@@ -2268,15 +2271,15 @@ static int rt73usb_conf_tx(struct ieee80211_hw *hw,
 	field.bit_offset = queue_idx * 4;
 	field.bit_mask = 0xf << field.bit_offset;
 
-	rt2x00usb_register_read(rt2x00dev, AIFSN_CSR, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, AIFSN_CSR);
 	rt2x00_set_field32(&reg, field, queue->aifs);
 	rt2x00usb_register_write(rt2x00dev, AIFSN_CSR, reg);
 
-	rt2x00usb_register_read(rt2x00dev, CWMIN_CSR, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, CWMIN_CSR);
 	rt2x00_set_field32(&reg, field, queue->cw_min);
 	rt2x00usb_register_write(rt2x00dev, CWMIN_CSR, reg);
 
-	rt2x00usb_register_read(rt2x00dev, CWMAX_CSR, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, CWMAX_CSR);
 	rt2x00_set_field32(&reg, field, queue->cw_max);
 	rt2x00usb_register_write(rt2x00dev, CWMAX_CSR, reg);
 
@@ -2289,9 +2292,9 @@ static u64 rt73usb_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	u64 tsf;
 	u32 reg;
 
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR13, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR13);
 	tsf = (u64) rt2x00_get_field32(reg, TXRX_CSR13_HIGH_TSFTIMER) << 32;
-	rt2x00usb_register_read(rt2x00dev, TXRX_CSR12, &reg);
+	reg = rt2x00usb_register_read(rt2x00dev, TXRX_CSR12);
 	tsf |= rt2x00_get_field32(reg, TXRX_CSR12_LOW_TSFTIMER);
 
 	return tsf;
diff --git a/drivers/net/wireless/ray_cs.c b/drivers/net/wireless/ray_cs.c
index b94479441b0c..170cd504e8ff 100644
--- a/drivers/net/wireless/ray_cs.c
+++ b/drivers/net/wireless/ray_cs.c
@@ -247,7 +247,10 @@ static const UCHAR b4_default_startup_parms[] = {
 	0x04, 0x08,		/* Noise gain, limit offset */
 	0x28, 0x28,		/* det rssi, med busy offsets */
 	7,			/* det sync thresh */
-	0, 2, 2			/* test mode, min, max */
+	0, 2, 2,		/* test mode, min, max */
+	0,			/* rx/tx delay */
+	0, 0, 0, 0, 0, 0,	/* current BSS id */
+	0			/* hop set */
 };
 
 /*===========================================================================*/
@@ -597,7 +600,7 @@ static void init_startup_params(ray_dev_t *local)
 	 *    a_beacon_period = hops    a_beacon_period = KuS
 	 *//* 64ms = 010000 */
 	if (local->fw_ver == 0x55) {
-		memcpy((UCHAR *) &local->sparm.b4, b4_default_startup_parms,
+		memcpy(&local->sparm.b4, b4_default_startup_parms,
 		       sizeof(struct b4_startup_params));
 		/* Translate sane kus input values to old build 4/5 format */
 		/* i = hop time in uS truncated to 3 bytes */
diff --git a/drivers/net/wireless/realtek/rtl818x/rtl8187/dev.c b/drivers/net/wireless/realtek/rtl818x/rtl8187/dev.c
index 35fe991dcc56..55198ac2b755 100644
--- a/drivers/net/wireless/realtek/rtl818x/rtl8187/dev.c
+++ b/drivers/net/wireless/realtek/rtl818x/rtl8187/dev.c
@@ -278,8 +278,7 @@ static void rtl8187_tx(struct ieee80211_hw *dev,
 	}
 
 	if (!priv->is_rtl8187b) {
-		struct rtl8187_tx_hdr *hdr =
-			(struct rtl8187_tx_hdr *)skb_push(skb, sizeof(*hdr));
+		struct rtl8187_tx_hdr *hdr = skb_push(skb, sizeof(*hdr));
 		hdr->flags = cpu_to_le32(flags);
 		hdr->len = 0;
 		hdr->rts_duration = rts_dur;
@@ -292,8 +291,7 @@ static void rtl8187_tx(struct ieee80211_hw *dev,
 		unsigned int epmap[4] = { 6, 7, 5, 4 };
 		u16 fc = le16_to_cpu(tx_hdr->frame_control);
 
-		struct rtl8187b_tx_hdr *hdr =
-			(struct rtl8187b_tx_hdr *)skb_push(skb, sizeof(*hdr));
+		struct rtl8187b_tx_hdr *hdr = skb_push(skb, sizeof(*hdr));
 		struct ieee80211_rate *txrate =
 			ieee80211_get_tx_rate(dev, info);
 		memset(hdr, 0, sizeof(*hdr));
diff --git a/drivers/net/wireless/realtek/rtl8xxxu/rtl8xxxu_core.c b/drivers/net/wireless/realtek/rtl8xxxu/rtl8xxxu_core.c
index 39d56313bc94..21e5ef021260 100644
--- a/drivers/net/wireless/realtek/rtl8xxxu/rtl8xxxu_core.c
+++ b/drivers/net/wireless/realtek/rtl8xxxu/rtl8xxxu_core.c
@@ -4952,7 +4952,7 @@ static void rtl8xxxu_tx(struct ieee80211_hw *hw,
 	if (control && control->sta)
 		sta = control->sta;
 
-	tx_desc = (struct rtl8xxxu_txdesc32 *)skb_push(skb, tx_desc_size);
+	tx_desc = skb_push(skb, tx_desc_size);
 
 	memset(tx_desc, 0, tx_desc_size);
 	tx_desc->pkt_size = cpu_to_le16(pktlen);
diff --git a/drivers/net/wireless/realtek/rtlwifi/base.c b/drivers/net/wireless/realtek/rtlwifi/base.c
index bdc379178e87..e36ee592c660 100644
--- a/drivers/net/wireless/realtek/rtlwifi/base.c
+++ b/drivers/net/wireless/realtek/rtlwifi/base.c
@@ -405,6 +405,10 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 		ieee80211_hw_set(hw, SUPPORTS_PS);
 		ieee80211_hw_set(hw, PS_NULLFUNC_STACK);
 	}
+	if (rtlpriv->psc.fwctrl_lps) {
+		ieee80211_hw_set(hw, SUPPORTS_PS);
+		ieee80211_hw_set(hw, SUPPORTS_DYNAMIC_PS);
+	}
 	hw->wiphy->interface_modes =
 	    BIT(NL80211_IFTYPE_AP) |
 	    BIT(NL80211_IFTYPE_STATION) |
@@ -560,6 +564,7 @@ int rtl_init_core(struct ieee80211_hw *hw)
 	spin_lock_init(&rtlpriv->locks.waitq_lock);
 	spin_lock_init(&rtlpriv->locks.entry_list_lock);
 	spin_lock_init(&rtlpriv->locks.c2hcmd_lock);
+	spin_lock_init(&rtlpriv->locks.scan_list_lock);
 	spin_lock_init(&rtlpriv->locks.cck_and_rw_pagea_lock);
 	spin_lock_init(&rtlpriv->locks.check_sendpkt_lock);
 	spin_lock_init(&rtlpriv->locks.fw_ps_lock);
@@ -568,6 +573,7 @@ int rtl_init_core(struct ieee80211_hw *hw)
 	/* <5> init list */
 	INIT_LIST_HEAD(&rtlpriv->entry_list);
 	INIT_LIST_HEAD(&rtlpriv->c2hcmd_list);
+	INIT_LIST_HEAD(&rtlpriv->scan_list.list);
 
 	rtlmac->link_state = MAC80211_NOLINK;
 
@@ -578,9 +584,12 @@ int rtl_init_core(struct ieee80211_hw *hw)
 }
 EXPORT_SYMBOL_GPL(rtl_init_core);
 
+static void rtl_free_entries_from_scan_list(struct ieee80211_hw *hw);
+
 void rtl_deinit_core(struct ieee80211_hw *hw)
 {
 	rtl_c2hcmd_launcher(hw, 0);
+	rtl_free_entries_from_scan_list(hw);
 }
 EXPORT_SYMBOL_GPL(rtl_deinit_core);
 
@@ -1110,6 +1119,9 @@ void rtl_get_tcb_desc(struct ieee80211_hw *hw,
 	if (txrate)
 		tcb_desc->hw_rate = txrate->hw_value;
 
+	if (rtl_is_tx_report_skb(hw, skb))
+		tcb_desc->use_spe_rpt = 1;
+
 	if (ieee80211_is_data(fc)) {
 		/*
 		 *we set data rate INX 0
@@ -1306,33 +1318,26 @@ bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 }
 EXPORT_SYMBOL_GPL(rtl_action_proc);
 
-static void setup_arp_tx(struct rtl_priv *rtlpriv, struct rtl_ps_ctl *ppsc)
+static void setup_special_tx(struct rtl_priv *rtlpriv, struct rtl_ps_ctl *ppsc,
+			     int type)
 {
 	struct ieee80211_hw *hw = rtlpriv->hw;
 
 	rtlpriv->ra.is_special_data = true;
 	if (rtlpriv->cfg->ops->get_btc_status())
 		rtlpriv->btcoexist.btc_ops->btc_special_packet_notify(
-					rtlpriv, 1);
+					rtlpriv, type);
 	rtl_lps_leave(hw);
 	ppsc->last_delaylps_stamp_jiffies = jiffies;
 }
 
-/*should call before software enc*/
-u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx,
-		       bool is_enc)
+static const u8 *rtl_skb_ether_type_ptr(struct ieee80211_hw *hw,
+					struct sk_buff *skb, bool is_enc)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	__le16 fc = rtl_get_fc(skb);
-	u16 ether_type;
 	u8 mac_hdr_len = ieee80211_get_hdrlen_from_skb(skb);
 	u8 encrypt_header_len = 0;
 	u8 offset;
-	const struct iphdr *ip;
-
-	if (!ieee80211_is_data(fc))
-		goto end;
 
 	switch (rtlpriv->sec.pairwise_enc_algorithm) {
 	case WEP40_ENCRYPTION:
@@ -1352,10 +1357,29 @@ u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx,
 	offset = mac_hdr_len + SNAP_SIZE;
 	if (is_enc)
 		offset += encrypt_header_len;
-	ether_type = be16_to_cpup((__be16 *)(skb->data + offset));
+
+	return skb->data + offset;
+}
+
+/*should call before software enc*/
+u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx,
+		       bool is_enc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	__le16 fc = rtl_get_fc(skb);
+	u16 ether_type;
+	const u8 *ether_type_ptr;
+	const struct iphdr *ip;
+
+	if (!ieee80211_is_data(fc))
+		goto end;
+
+	ether_type_ptr = rtl_skb_ether_type_ptr(hw, skb, is_enc);
+	ether_type = be16_to_cpup((__be16 *)ether_type_ptr);
 
 	if (ETH_P_IP == ether_type) {
-		ip = (struct iphdr *)((u8 *)skb->data + offset +
+		ip = (struct iphdr *)((u8 *)ether_type_ptr +
 		     PROTOC_TYPE_SIZE);
 		if (IPPROTO_UDP == ip->protocol) {
 			struct udphdr *udp = (struct udphdr *)((u8 *)ip +
@@ -1372,13 +1396,15 @@ u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx,
 					 (is_tx) ? "Tx" : "Rx");
 
 				if (is_tx)
-					setup_arp_tx(rtlpriv, ppsc);
+					setup_special_tx(rtlpriv, ppsc,
+							 PACKET_DHCP);
+
 				return true;
 			}
 		}
 	} else if (ETH_P_ARP == ether_type) {
 		if (is_tx)
-			setup_arp_tx(rtlpriv, ppsc);
+			setup_special_tx(rtlpriv, ppsc, PACKET_ARP);
 
 		return true;
 	} else if (ETH_P_PAE == ether_type) {
@@ -1389,6 +1415,8 @@ u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx,
 			rtlpriv->ra.is_special_data = true;
 			rtl_lps_leave(hw);
 			ppsc->last_delaylps_stamp_jiffies = jiffies;
+
+			setup_special_tx(rtlpriv, ppsc, PACKET_EAPOL);
 		}
 
 		return true;
@@ -1405,6 +1433,96 @@ end:
 }
 EXPORT_SYMBOL_GPL(rtl_is_special_data);
 
+bool rtl_is_tx_report_skb(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	u16 ether_type;
+	const u8 *ether_type_ptr;
+
+	ether_type_ptr = rtl_skb_ether_type_ptr(hw, skb, true);
+	ether_type = be16_to_cpup((__be16 *)ether_type_ptr);
+
+	/* EAPOL */
+	if (ether_type == ETH_P_PAE)
+		return true;
+
+	return false;
+}
+
+static u16 rtl_get_tx_report_sn(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tx_report *tx_report = &rtlpriv->tx_report;
+	u16 sn;
+
+	sn = atomic_inc_return(&tx_report->sn) & 0x0FFF;
+
+	tx_report->last_sent_sn = sn;
+	tx_report->last_sent_time = jiffies;
+
+	RT_TRACE(rtlpriv, COMP_TX_REPORT, DBG_DMESG,
+		 "Send TX-Report sn=0x%X\n", sn);
+
+	return sn;
+}
+
+void rtl_get_tx_report(struct rtl_tcb_desc *ptcb_desc, u8 *pdesc,
+		       struct ieee80211_hw *hw)
+{
+	if (ptcb_desc->use_spe_rpt) {
+		u16 sn = rtl_get_tx_report_sn(hw);
+
+		SET_TX_DESC_SPE_RPT(pdesc, 1);
+		SET_TX_DESC_SW_DEFINE(pdesc, sn);
+	}
+}
+EXPORT_SYMBOL_GPL(rtl_get_tx_report);
+
+void rtl_tx_report_handler(struct ieee80211_hw *hw, u8 *tmp_buf, u8 c2h_cmd_len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tx_report *tx_report = &rtlpriv->tx_report;
+	u16 sn;
+
+	sn = ((tmp_buf[7] & 0x0F) << 8) | tmp_buf[6];
+
+	tx_report->last_recv_sn = sn;
+
+	RT_TRACE(rtlpriv, COMP_TX_REPORT, DBG_DMESG,
+		 "Recv TX-Report st=0x%02X sn=0x%X retry=0x%X\n",
+		 tmp_buf[0], sn, tmp_buf[2]);
+}
+EXPORT_SYMBOL_GPL(rtl_tx_report_handler);
+
+bool rtl_check_tx_report_acked(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tx_report *tx_report = &rtlpriv->tx_report;
+
+	if (tx_report->last_sent_sn == tx_report->last_recv_sn)
+		return true;
+
+	if (time_before(tx_report->last_sent_time + 3 * HZ, jiffies)) {
+		RT_TRACE(rtlpriv, COMP_TX_REPORT, DBG_WARNING,
+			 "Check TX-Report timeout!!\n");
+		return true;	/* 3 sec. (timeout) seen as acked */
+	}
+
+	return false;
+}
+
+void rtl_wait_tx_report_acked(struct ieee80211_hw *hw, u32 wait_ms)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i;
+
+	for (i = 0; i < wait_ms; i++) {
+		if (rtl_check_tx_report_acked(hw))
+			break;
+		usleep_range(1000, 2000);
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+			 "Wait 1ms (%d/%d) to disable key.\n", i, wait_ms);
+	}
+}
 /*********************************************************
  *
  * functions called by core.c
@@ -1469,6 +1587,7 @@ int rtl_rx_agg_start(struct ieee80211_hw *hw,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_tid_data *tid_data;
 	struct rtl_sta_info *sta_entry = NULL;
+	u8 reject_agg;
 
 	if (sta == NULL)
 		return -EINVAL;
@@ -1476,6 +1595,14 @@ int rtl_rx_agg_start(struct ieee80211_hw *hw,
 	if (unlikely(tid >= MAX_TID_COUNT))
 		return -EINVAL;
 
+	if (rtlpriv->cfg->ops->get_btc_status()) {
+		rtlpriv->btcoexist.btc_ops->btc_get_ampdu_cfg(rtlpriv,
+							      &reject_agg,
+							      NULL, NULL);
+		if (reject_agg)
+			return -EINVAL;
+	}
+
 	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
 	if (!sta_entry)
 		return -ENXIO;
@@ -1530,6 +1657,24 @@ int rtl_tx_agg_oper(struct ieee80211_hw *hw,
 	return 0;
 }
 
+void rtl_rx_ampdu_apply(struct rtl_priv *rtlpriv)
+{
+	struct rtl_btc_ops *btc_ops = rtlpriv->btcoexist.btc_ops;
+	u8 reject_agg, ctrl_agg_size = 0, agg_size;
+
+	if (rtlpriv->cfg->ops->get_btc_status())
+		btc_ops->btc_get_ampdu_cfg(rtlpriv, &reject_agg,
+					   &ctrl_agg_size, &agg_size);
+
+	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
+		 "Set RX AMPDU: coex - reject=%d, ctrl_agg_size=%d, size=%d",
+		 reject_agg, ctrl_agg_size, agg_size);
+
+	rtlpriv->hw->max_rx_aggregation_subframes =
+		(ctrl_agg_size ? agg_size : IEEE80211_MAX_AMPDU_BUF);
+}
+EXPORT_SYMBOL(rtl_rx_ampdu_apply);
+
 /*********************************************************
  *
  * wq & timer callback functions
@@ -1564,6 +1709,100 @@ void rtl_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb)
 }
 EXPORT_SYMBOL_GPL(rtl_beacon_statistic);
 
+static void rtl_free_entries_from_scan_list(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_bssid_entry *entry, *next;
+
+	list_for_each_entry_safe(entry, next, &rtlpriv->scan_list.list, list) {
+		list_del(&entry->list);
+		kfree(entry);
+		rtlpriv->scan_list.num--;
+	}
+}
+
+void rtl_scan_list_expire(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_bssid_entry *entry, *next;
+	unsigned long flags;
+
+	spin_lock_irqsave(&rtlpriv->locks.scan_list_lock, flags);
+
+	list_for_each_entry_safe(entry, next, &rtlpriv->scan_list.list, list) {
+		/* 180 seconds */
+		if (jiffies_to_msecs(jiffies - entry->age) < 180000)
+			continue;
+
+		list_del(&entry->list);
+		kfree(entry);
+		rtlpriv->scan_list.num--;
+
+		RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD,
+			 "BSSID=%pM is expire in scan list (total=%d)\n",
+			 entry->bssid, rtlpriv->scan_list.num);
+	}
+
+	spin_unlock_irqrestore(&rtlpriv->locks.scan_list_lock, flags);
+
+	rtlpriv->btcoexist.btc_info.ap_num = rtlpriv->scan_list.num;
+}
+
+void rtl_collect_scan_list(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	unsigned long flags;
+
+	struct rtl_bssid_entry *entry;
+	bool entry_found = false;
+
+	/* check if it is scanning */
+	if (!mac->act_scanning)
+		return;
+
+	/* check if this really is a beacon */
+	if (!ieee80211_is_beacon(hdr->frame_control) &&
+	    !ieee80211_is_probe_resp(hdr->frame_control))
+		return;
+
+	spin_lock_irqsave(&rtlpriv->locks.scan_list_lock, flags);
+
+	list_for_each_entry(entry, &rtlpriv->scan_list.list, list) {
+		if (memcmp(entry->bssid, hdr->addr3, ETH_ALEN) == 0) {
+			list_del_init(&entry->list);
+			entry_found = true;
+			RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD,
+				 "Update BSSID=%pM to scan list (total=%d)\n",
+				 hdr->addr3, rtlpriv->scan_list.num);
+			break;
+		}
+	}
+
+	if (!entry_found) {
+		entry = kmalloc(sizeof(*entry), GFP_ATOMIC);
+
+		if (!entry)
+			goto label_err;
+
+		memcpy(entry->bssid, hdr->addr3, ETH_ALEN);
+		rtlpriv->scan_list.num++;
+
+		RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD,
+			 "Add BSSID=%pM to scan list (total=%d)\n",
+			 hdr->addr3, rtlpriv->scan_list.num);
+	}
+
+	entry->age = jiffies;
+
+	list_add_tail(&entry->list, &rtlpriv->scan_list.list);
+
+label_err:
+	spin_unlock_irqrestore(&rtlpriv->locks.scan_list_lock, flags);
+}
+EXPORT_SYMBOL(rtl_collect_scan_list);
+
 void rtl_watchdog_wq_callback(void *data)
 {
 	struct rtl_works *rtlworks = container_of_dwork_rtl(data,
@@ -1662,12 +1901,20 @@ void rtl_watchdog_wq_callback(void *data)
 									false;
 		}
 
+		/* PS is controlled by coex. */
+		if (rtlpriv->cfg->ops->get_btc_status() &&
+		    rtlpriv->btcoexist.btc_ops->btc_is_bt_ctrl_lps(rtlpriv))
+			goto label_lps_done;
+
 		if (((rtlpriv->link_info.num_rx_inperiod +
 		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
 		    (rtlpriv->link_info.num_rx_inperiod > 2))
 			rtl_lps_leave(hw);
 		else
 			rtl_lps_enter(hw);
+
+label_lps_done:
+		;
 	}
 
 	rtlpriv->link_info.num_rx_inperiod = 0;
@@ -1713,6 +1960,9 @@ void rtl_watchdog_wq_callback(void *data)
 		rtlpriv->btcoexist.btc_ops->btc_periodical(rtlpriv);
 
 	rtlpriv->link_info.bcn_rx_inperiod = 0;
+
+	/* <6> scan list */
+	rtl_scan_list_expire(hw);
 }
 
 void rtl_watch_dog_timer_callback(unsigned long data)
@@ -1875,8 +2125,7 @@ static struct sk_buff *rtl_make_smps_action(struct ieee80211_hw *hw,
 		return NULL;
 
 	skb_reserve(skb, hw->extra_tx_headroom);
-	action_frame = (void *)skb_put(skb, 27);
-	memset(action_frame, 0, 27);
+	action_frame = skb_put_zero(skb, 27);
 	memcpy(action_frame->da, da, ETH_ALEN);
 	memcpy(action_frame->sa, rtlefuse->dev_addr, ETH_ALEN);
 	memcpy(action_frame->bssid, bssid, ETH_ALEN);
@@ -2005,8 +2254,7 @@ struct sk_buff *rtl_make_del_ba(struct ieee80211_hw *hw,
 		return NULL;
 
 	skb_reserve(skb, hw->extra_tx_headroom);
-	action_frame = (void *)skb_put(skb, 34);
-	memset(action_frame, 0, 34);
+	action_frame = skb_put_zero(skb, 34);
 	memcpy(action_frame->sa, sa, ETH_ALEN);
 	memcpy(action_frame->da, rtlefuse->dev_addr, ETH_ALEN);
 	memcpy(action_frame->bssid, bssid, ETH_ALEN);
diff --git a/drivers/net/wireless/realtek/rtlwifi/base.h b/drivers/net/wireless/realtek/rtlwifi/base.h
index 02ff0c5624a7..ab7d81904d25 100644
--- a/drivers/net/wireless/realtek/rtlwifi/base.h
+++ b/drivers/net/wireless/realtek/rtlwifi/base.h
@@ -107,6 +107,11 @@ enum ap_peer {
 	SET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, \
 	(GET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr) & (~(__val))))
 
+#define SET_TX_DESC_SPE_RPT(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 8, 19, 1, __val)
+#define SET_TX_DESC_SW_DEFINE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 0, 12, __val)
+
 int rtl_init_core(struct ieee80211_hw *hw);
 void rtl_deinit_core(struct ieee80211_hw *hw);
 void rtl_init_rx_config(struct ieee80211_hw *hw);
@@ -123,7 +128,17 @@ bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb);
 u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx,
 		       bool is_enc);
 
+bool rtl_is_tx_report_skb(struct ieee80211_hw *hw, struct sk_buff *skb);
+void rtl_get_tx_report(struct rtl_tcb_desc *ptcb_desc, u8 *pdesc,
+		       struct ieee80211_hw *hw);
+void rtl_tx_report_handler(struct ieee80211_hw *hw, u8 *tmp_buf,
+			   u8 c2h_cmd_len);
+bool rtl_check_tx_report_acked(struct ieee80211_hw *hw);
+void rtl_wait_tx_report_acked(struct ieee80211_hw *hw, u32 wait_ms);
+
 void rtl_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb);
+void rtl_collect_scan_list(struct ieee80211_hw *hw, struct sk_buff *skb);
+void rtl_scan_list_expire(struct ieee80211_hw *hw);
 int rtl_tx_agg_start(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	struct ieee80211_sta *sta, u16 tid, u16 *ssn);
 int rtl_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
@@ -134,6 +149,7 @@ int rtl_rx_agg_start(struct ieee80211_hw *hw,
 		     struct ieee80211_sta *sta, u16 tid);
 int rtl_rx_agg_stop(struct ieee80211_hw *hw,
 		    struct ieee80211_sta *sta, u16 tid);
+void rtl_rx_ampdu_apply(struct rtl_priv *rtlpriv);
 void rtl_watchdog_wq_callback(void *data);
 void rtl_fwevt_wq_callback(void *data);
 void rtl_c2hcmd_wq_callback(void *data);
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbt_precomp.h b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbt_precomp.h
index 39b9a3309cfd..2ac989a4b2bb 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbt_precomp.h
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbt_precomp.h
@@ -37,6 +37,28 @@
 
 #include "halbtcoutsrc.h"
 
+/* Interface type */
+#define RT_PCI_INTERFACE	1
+#define RT_USB_INTERFACE	2
+#define RT_SDIO_INTERFACE	3
+#define DEV_BUS_TYPE		RT_PCI_INTERFACE
+
+/* IC type */
+#define RTL_HW_TYPE(adapter)	(rtl_hal((struct rtl_priv *)adapter)->hw_type)
+
+#define IS_NEW_GENERATION_IC(adapter)		\
+			(RTL_HW_TYPE(adapter) >= HARDWARE_TYPE_RTL8192EE)
+#define IS_HARDWARE_TYPE_8812(adapter)		\
+			(RTL_HW_TYPE(adapter) == HARDWARE_TYPE_RTL8812AE)
+#define IS_HARDWARE_TYPE_8821(adapter)		\
+			(RTL_HW_TYPE(adapter) == HARDWARE_TYPE_RTL8821AE)
+#define IS_HARDWARE_TYPE_8723A(adapter)	\
+			(RTL_HW_TYPE(adapter) == HARDWARE_TYPE_RTL8723AE)
+#define IS_HARDWARE_TYPE_8723B(adapter)	\
+			(RTL_HW_TYPE(adapter) == HARDWARE_TYPE_RTL8723BE)
+#define IS_HARDWARE_TYPE_8192E(adapter)	\
+			(RTL_HW_TYPE(adapter) == HARDWARE_TYPE_RTL8192EE)
+
 #include "halbtc8192e2ant.h"
 #include "halbtc8723b1ant.h"
 #include "halbtc8723b2ant.h"
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8192e2ant.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8192e2ant.c
index 57e633dbf9a9..44c25724529e 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8192e2ant.c
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8192e2ant.c
@@ -456,6 +456,39 @@ static void btc8192e2ant_query_bt_info(struct btc_coexist *btcoexist)
 	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
 }
 
+static
+bool btc8192e2ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
+{
+	static bool pre_wifi_busy = false, pre_under_4way = false,
+		    pre_bt_hs_on = false;
+	bool wifi_busy = false, under_4way = false, bt_hs_on = false;
+	bool wifi_connected = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+			   &under_4way);
+
+	if (wifi_connected) {
+		if (wifi_busy != pre_wifi_busy) {
+			pre_wifi_busy = wifi_busy;
+			return true;
+		}
+		if (under_4way != pre_under_4way) {
+			pre_under_4way = under_4way;
+			return true;
+		}
+		if (bt_hs_on != pre_bt_hs_on) {
+			pre_bt_hs_on = bt_hs_on;
+			return true;
+		}
+	}
+
+	return false;
+}
+
 static void btc8192e2ant_update_bt_link_info(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
@@ -2886,9 +2919,8 @@ void ex_btc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d",
 		 "0x774(lp rx[31:16]/tx[15:0])",
 		 coex_sta->low_priority_rx, coex_sta->low_priority_tx);
-#if (BT_AUTO_REPORT_ONLY_8192E_2ANT == 1)
-	btc8192e2ant_monitor_bt_ctr(btcoexist);
-#endif
+	if (btcoexist->auto_report_2ant)
+		btc8192e2ant_monitor_bt_ctr(btcoexist);
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
 }
 
@@ -3078,14 +3110,12 @@ void ex_btc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
 			 */
 		}
 
-#if (BT_AUTO_REPORT_ONLY_8192E_2ANT == 0)
-		if ((coex_sta->bt_info_ext & BIT4)) {
-			/* BT auto report already enabled, do nothing */
-		} else {
-			btc8192e2ant_bt_auto_report(btcoexist, FORCE_EXEC,
-						    true);
+		if (!btcoexist->auto_report_2ant) {
+			if (!(coex_sta->bt_info_ext & BIT4))
+				btc8192e2ant_bt_auto_report(btcoexist,
+							    FORCE_EXEC,
+							    true);
 		}
-#endif
 	}
 
 	/* check BIT2 first ==> check if bt is under inquiry or page scan */
@@ -3164,7 +3194,7 @@ void ex_btc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
 	btc8192e2ant_run_coexist_mechanism(btcoexist);
 }
 
-void ex_halbtc8192e2ant_halt_notify(struct btc_coexist *btcoexist)
+void ex_btc8192e2ant_halt_notify(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 
@@ -3207,13 +3237,13 @@ void ex_btc8192e2ant_periodical(struct btc_coexist *btcoexist)
 			 "************************************************\n");
 	}
 
-#if (BT_AUTO_REPORT_ONLY_8192E_2ANT == 0)
-	btc8192e2ant_query_bt_info(btcoexist);
-	btc8192e2ant_monitor_bt_ctr(btcoexist);
-	btc8192e2ant_monitor_bt_enable_disable(btcoexist);
-#else
-	if (btc8192e2ant_is_wifi_status_changed(btcoexist) ||
-	    coex_dm->auto_tdma_adjust)
-		btc8192e2ant_run_coexist_mechanism(btcoexist);
-#endif
+	if (!btcoexist->auto_report_2ant) {
+		btc8192e2ant_query_bt_info(btcoexist);
+		btc8192e2ant_monitor_bt_ctr(btcoexist);
+		btc8192e2ant_monitor_bt_enable_disable(btcoexist);
+	} else {
+		if (btc8192e2ant_is_wifi_status_changed(btcoexist) ||
+		    coex_dm->auto_tdma_adjust)
+			btc8192e2ant_run_coexist_mechanism(btcoexist);
+	}
 }
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8192e2ant.h b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8192e2ant.h
index fc0fa87ec404..65502acee52c 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8192e2ant.h
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8192e2ant.h
@@ -25,8 +25,6 @@
 /*****************************************************************
  *   The following is for 8192E 2Ant BT Co-exist definition
  *****************************************************************/
-#define	BT_AUTO_REPORT_ONLY_8192E_2ANT			0
-
 #define	BT_INFO_8192E_2ANT_B_FTP			BIT7
 #define	BT_INFO_8192E_2ANT_B_A2DP			BIT6
 #define	BT_INFO_8192E_2ANT_B_HID			BIT5
@@ -166,20 +164,20 @@ struct coex_sta_8192e_2ant {
 /****************************************************************
  *    The following is interface which will notify coex module.
  ****************************************************************/
-void ex_halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist);
-void ex_halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist);
-void ex_halbtc8192e2ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8192e2ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8192e2ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8192e2ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8192e2ant_media_status_notify(struct btc_coexist *btcoexist,
+void ex_btc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist);
+void ex_btc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist);
+void ex_btc8192e2ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8192e2ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8192e2ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8192e2ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8192e2ant_media_status_notify(struct btc_coexist *btcoexist,
+					 u8 type);
+void ex_btc8192e2ant_special_packet_notify(struct btc_coexist *btcoexist,
+					   u8 type);
+void ex_btc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
+				    u8 *tmpbuf, u8 length);
+void ex_btc8192e2ant_stack_operation_notify(struct btc_coexist *btcoexist,
 					    u8 type);
-void ex_halbtc8192e2ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type);
-void ex_halbtc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmpbuf, u8 length);
-void ex_halbtc8192e2ant_stack_operation_notify(struct btc_coexist *btcoexist,
-					       u8 type);
-void ex_halbtc8192e2ant_halt_notify(struct btc_coexist *btcoexist);
-void ex_halbtc8192e2ant_periodical(struct btc_coexist *btcoexist);
-void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist);
+void ex_btc8192e2ant_halt_notify(struct btc_coexist *btcoexist);
+void ex_btc8192e2ant_periodical(struct btc_coexist *btcoexist);
+void ex_btc8192e2ant_display_coex_info(struct btc_coexist *btcoexist);
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.c
index 2003c8c51dcc..03998d2e9eb8 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.c
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.c
@@ -210,11 +210,24 @@ static void halbtc8723b1ant_limited_rx(struct btc_coexist *btcoexist,
 	btcoexist->btc_set(btcoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
 }
 
+static void halbtc8723b1ant_query_bt_info(struct btc_coexist *btcoexist)
+{
+	u8 h2c_parameter[1] = {0};
+
+	coex_sta->c2h_bt_info_req_sent = true;
+
+	/* trigger */
+	h2c_parameter[0] |= BIT(0);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
+}
+
 static void halbtc8723b1ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 {
 	u32 reg_hp_txrx, reg_lp_txrx, u32tmp;
 	u32 reg_hp_tx = 0, reg_hp_rx = 0;
 	u32 reg_lp_tx = 0, reg_lp_rx = 0;
+	static u32 num_of_bt_counter_chk;
 
 	reg_hp_txrx = 0x770;
 	reg_lp_txrx = 0x774;
@@ -232,25 +245,122 @@ static void halbtc8723b1ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 	coex_sta->low_priority_tx = reg_lp_tx;
 	coex_sta->low_priority_rx = reg_lp_rx;
 
+	if ((coex_sta->low_priority_tx > 1050) &&
+	    (!coex_sta->c2h_bt_inquiry_page))
+		coex_sta->pop_event_cnt++;
+
 	/* reset counter */
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+
+	/* This part is for wifi FW and driver to update BT's status as
+	 * disabled.
+	 *
+	 * The flow is as the following
+	 * 1. disable BT
+	 * 2. if all BT Tx/Rx counter = 0, after 6 sec we query bt info
+	 * 3. Because BT will not rsp from mailbox, so wifi fw will know BT is
+	 * disabled
+	 *
+	 * 4. FW will rsp c2h for BT that driver will know BT is disabled.
+	 */
+	if ((reg_hp_tx == 0) && (reg_hp_rx == 0) && (reg_lp_tx == 0) &&
+	    (reg_lp_rx == 0)) {
+		num_of_bt_counter_chk++;
+		if (num_of_bt_counter_chk == 3)
+			halbtc8723b1ant_query_bt_info(btcoexist);
+	} else {
+		num_of_bt_counter_chk = 0;
+	}
 }
 
-static void halbtc8723b1ant_query_bt_info(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_monitor_wifi_ctr(struct btc_coexist *btcoexist)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	u8 h2c_parameter[1] = {0};
+	s32 wifi_rssi = 0;
+	bool wifi_busy = false, wifi_under_b_mode = false;
+	static u8 cck_lock_counter;
+	u32 total_cnt;
 
-	coex_sta->c2h_bt_info_req_sent = true;
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_B_MODE,
+			   &wifi_under_b_mode);
 
-	/* trigger */
-	h2c_parameter[0] |= BIT0;
+	if (coex_sta->under_ips) {
+		coex_sta->crc_ok_cck = 0;
+		coex_sta->crc_ok_11g = 0;
+		coex_sta->crc_ok_11n = 0;
+		coex_sta->crc_ok_11n_agg = 0;
+
+		coex_sta->crc_err_cck = 0;
+		coex_sta->crc_err_11g = 0;
+		coex_sta->crc_err_11n = 0;
+		coex_sta->crc_err_11n_agg = 0;
+	} else {
+		coex_sta->crc_ok_cck =
+			btcoexist->btc_read_4byte(btcoexist, 0xf88);
+		coex_sta->crc_ok_11g =
+			btcoexist->btc_read_2byte(btcoexist, 0xf94);
+		coex_sta->crc_ok_11n =
+			btcoexist->btc_read_2byte(btcoexist, 0xf90);
+		coex_sta->crc_ok_11n_agg =
+			btcoexist->btc_read_2byte(btcoexist, 0xfb8);
+
+		coex_sta->crc_err_cck =
+			btcoexist->btc_read_4byte(btcoexist, 0xf84);
+		coex_sta->crc_err_11g =
+			btcoexist->btc_read_2byte(btcoexist, 0xf96);
+		coex_sta->crc_err_11n =
+			btcoexist->btc_read_2byte(btcoexist, 0xf92);
+		coex_sta->crc_err_11n_agg =
+			btcoexist->btc_read_2byte(btcoexist, 0xfba);
+	}
 
-	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-		 "[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n",
-		 h2c_parameter[0]);
+	/* reset counter */
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0xf16, 0x1, 0x1);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0xf16, 0x1, 0x0);
+
+	if ((wifi_busy) && (wifi_rssi >= 30) && (!wifi_under_b_mode)) {
+		total_cnt = coex_sta->crc_ok_cck + coex_sta->crc_ok_11g +
+			    coex_sta->crc_ok_11n + coex_sta->crc_ok_11n_agg;
+
+		if ((coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_ACL_BUSY) ||
+		    (coex_dm->bt_status ==
+		     BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY) ||
+		    (coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_SCO_BUSY)) {
+			if (coex_sta->crc_ok_cck >
+			    (total_cnt - coex_sta->crc_ok_cck)) {
+				if (cck_lock_counter < 3)
+					cck_lock_counter++;
+			} else {
+				if (cck_lock_counter > 0)
+					cck_lock_counter--;
+			}
 
-	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
+		} else {
+			if (cck_lock_counter > 0)
+				cck_lock_counter--;
+		}
+	} else {
+		if (cck_lock_counter > 0)
+			cck_lock_counter--;
+	}
+
+	if (!coex_sta->pre_ccklock) {
+		if (cck_lock_counter >= 3)
+			coex_sta->cck_lock = true;
+		else
+			coex_sta->cck_lock = false;
+	} else {
+		if (cck_lock_counter == 0)
+			coex_sta->cck_lock = false;
+		else
+			coex_sta->cck_lock = true;
+	}
+
+	if (coex_sta->cck_lock)
+		coex_sta->cck_ever_lock = true;
+
+	coex_sta->pre_ccklock = coex_sta->cck_lock;
 }
 
 static bool btc8723b1ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
@@ -297,6 +407,7 @@ static void halbtc8723b1ant_update_bt_link_info(struct btc_coexist *btcoexist)
 	bt_link_info->a2dp_exist = coex_sta->a2dp_exist;
 	bt_link_info->pan_exist = coex_sta->pan_exist;
 	bt_link_info->hid_exist = coex_sta->hid_exist;
+	bt_link_info->bt_hi_pri_link_exist = coex_sta->bt_hi_pri_link_exist;
 
 	/* work around for HS mode. */
 	if (bt_hs_on) {
@@ -333,6 +444,35 @@ static void halbtc8723b1ant_update_bt_link_info(struct btc_coexist *btcoexist)
 		bt_link_info->hid_only = false;
 }
 
+static void halbtc8723b1ant_set_bt_auto_report(struct btc_coexist *btcoexist,
+					       bool enable_auto_report)
+{
+	u8 h2c_parameter[1] = {0};
+
+	h2c_parameter[0] = 0;
+
+	if (enable_auto_report)
+		h2c_parameter[0] |= BIT(0);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
+}
+
+static void halbtc8723b1ant_bt_auto_report(struct btc_coexist *btcoexist,
+					   bool force_exec,
+					   bool enable_auto_report)
+{
+	coex_dm->cur_bt_auto_report = enable_auto_report;
+
+	if (!force_exec) {
+		if (coex_dm->pre_bt_auto_report == coex_dm->cur_bt_auto_report)
+			return;
+	}
+	halbtc8723b1ant_set_bt_auto_report(btcoexist,
+					   coex_dm->cur_bt_auto_report);
+
+	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
+}
+
 static void btc8723b1ant_set_sw_pen_tx_rate_adapt(struct btc_coexist *btcoexist,
 						  bool low_penalty_ra)
 {
@@ -430,6 +570,8 @@ static void halbtc8723b1ant_coex_table(struct btc_coexist *btcoexist,
 static void halbtc8723b1ant_coex_table_with_type(struct btc_coexist *btcoexist,
 						 bool force_exec, u8 type)
 {
+	coex_sta->coex_table_type = type;
+
 	switch (type) {
 	case 0:
 		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
@@ -445,24 +587,68 @@ static void halbtc8723b1ant_coex_table_with_type(struct btc_coexist *btcoexist,
 		break;
 	case 3:
 		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0xaaaaaaaa, 0xffffff, 0x3);
+					   0x5a5a5a5a, 0xffffff, 0x3);
 		break;
 	case 4:
-		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0x5aaa5aaa, 0xffffff, 0x3);
+		if ((coex_sta->cck_ever_lock) && (coex_sta->scan_ap_num <= 5))
+			halbtc8723b1ant_coex_table(btcoexist, force_exec,
+						   0x55555555, 0xaaaa5a5a,
+						   0xffffff, 0x3);
+		else
+			halbtc8723b1ant_coex_table(btcoexist, force_exec,
+						   0x55555555, 0x5a5a5a5a,
+						   0xffffff, 0x3);
 		break;
 	case 5:
-		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
-					   0xaaaa5a5a, 0xffffff, 0x3);
+		if ((coex_sta->cck_ever_lock) && (coex_sta->scan_ap_num <= 5))
+			halbtc8723b1ant_coex_table(btcoexist, force_exec,
+						   0x5a5a5a5a, 0x5aaa5a5a,
+						   0xffffff, 0x3);
+		else
+			halbtc8723b1ant_coex_table(btcoexist, force_exec,
+						   0x5a5a5a5a, 0x5aaa5a5a,
+						   0xffffff, 0x3);
 		break;
 	case 6:
 		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0xaaaa5a5a, 0xffffff, 0x3);
+					   0xaaaaaaaa, 0xffffff, 0x3);
 		break;
 	case 7:
 		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0xaaaaaaaa,
 					   0xaaaaaaaa, 0xffffff, 0x3);
 		break;
+	case 8:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55dd55dd,
+					   0x5ada5ada, 0xffffff, 0x3);
+		break;
+	case 9:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55dd55dd,
+					   0x5ada5ada, 0xffffff, 0x3);
+		break;
+	case 10:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55dd55dd,
+					   0x5ada5ada, 0xffffff, 0x3);
+		break;
+	case 11:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55dd55dd,
+					   0x5ada5ada, 0xffffff, 0x3);
+		break;
+	case 12:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55dd55dd,
+					   0x5ada5ada, 0xffffff, 0x3);
+		break;
+	case 13:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x5fff5fff,
+					   0xaaaaaaaa, 0xffffff, 0x3);
+		break;
+	case 14:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x5fff5fff,
+					   0x5ada5ada, 0xffffff, 0x3);
+		break;
+	case 15:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55dd55dd,
+					   0xaaaaaaaa, 0xffffff, 0x3);
+		break;
 	default:
 		break;
 	}
@@ -611,14 +797,18 @@ static void halbtc8723b1ant_sw_mechanism(struct btc_coexist *btcoexist,
 }
 
 static void halbtc8723b1ant_set_ant_path(struct btc_coexist *btcoexist,
-					 u8 ant_pos_type, bool init_hw_cfg,
-					 bool wifi_off)
+					 u8 ant_pos_type, bool force_exec,
+					 bool init_hw_cfg, bool wifi_off)
 {
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	struct btc_board_info *board_info = &btcoexist->board_info;
-	u32 fw_ver = 0, u32tmp = 0;
+	u32 fw_ver = 0, u32tmp = 0, cnt_bt_cal_chk = 0;
 	bool pg_ext_switch = false;
 	bool use_ext_switch = false;
-	u8 h2c_parameter[2] = {0};
+	bool is_in_mp_mode = false;
+	u8 h2c_parameter[2] = {0}, u8tmp = 0;
+
+	coex_dm->cur_ant_pos_type = ant_pos_type;
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_EXT_SWITCH, &pg_ext_switch);
 	/* [31:16] = fw ver, [15:0] = fw sub ver */
@@ -628,24 +818,103 @@ static void halbtc8723b1ant_set_ant_path(struct btc_coexist *btcoexist,
 		use_ext_switch = true;
 
 	if (init_hw_cfg) {
-		/*BT select s0/s1 is controlled by WiFi */
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x1);
+		/* WiFi TRx Mask on */
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
+					  0x780);
+		/* remove due to interrupt is disabled that polling c2h will
+		 * fail and delay 100ms.
+		 */
 
-		/*Force GNT_BT to Normal */
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x0);
-	} else if (wifi_off) {
-		/*Force GNT_BT to High */
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x3);
-		/*BT select s0/s1 is controlled by BT */
+		if (fw_ver >= 0x180000) {
+			/* Use H2C to set GNT_BT to HIGH */
+			h2c_parameter[0] = 1;
+			btcoexist->btc_fill_h2c(btcoexist, 0x6E, 1,
+						h2c_parameter);
+		} else {
+			/* set grant_bt to high */
+			btcoexist->btc_write_1byte(btcoexist, 0x765, 0x18);
+		}
+		/* set wlan_act control by PTA */
+		btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
+
+		/* BT select s0/s1 is controlled by BT */
 		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x0);
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x39, 0x8, 0x1);
+		btcoexist->btc_write_1byte(btcoexist, 0x974, 0xff);
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x944, 0x3, 0x3);
+		btcoexist->btc_write_1byte(btcoexist, 0x930, 0x77);
+	} else if (wifi_off) {
+		if (fw_ver >= 0x180000) {
+			/* Use H2C to set GNT_BT to HIGH */
+			h2c_parameter[0] = 1;
+			btcoexist->btc_fill_h2c(btcoexist, 0x6E, 1,
+						h2c_parameter);
+		} else {
+			/* set grant_bt to high */
+			btcoexist->btc_write_1byte(btcoexist, 0x765, 0x18);
+		}
+		/* set wlan_act to always low */
+		btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
+
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_IS_IN_MP_MODE,
+				   &is_in_mp_mode);
+		if (!is_in_mp_mode)
+			/* BT select s0/s1 is controlled by BT */
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67,
+							   0x20, 0x0);
+		else
+			/* BT select s0/s1 is controlled by WiFi */
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67,
+							   0x20, 0x1);
 
-		/* 0x4c[24:23] = 00, Set Antenna control by BT_RFE_CTRL
-		 * BT Vendor 0xac = 0xf002
+		/* 0x4c[24:23]=00, Set Antenna control by BT_RFE_CTRL
+		 * BT Vendor 0xac=0xf002
 		 */
 		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
 		u32tmp &= ~BIT23;
 		u32tmp &= ~BIT24;
 		btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
+	} else {
+		/* Use H2C to set GNT_BT to LOW */
+		if (fw_ver >= 0x180000) {
+			if (btcoexist->btc_read_1byte(btcoexist, 0x765) != 0) {
+				h2c_parameter[0] = 0;
+				btcoexist->btc_fill_h2c(btcoexist, 0x6E, 1,
+							h2c_parameter);
+			}
+		} else {
+			/* BT calibration check */
+			while (cnt_bt_cal_chk <= 20) {
+				u8tmp = btcoexist->btc_read_1byte(btcoexist,
+								  0x49d);
+				cnt_bt_cal_chk++;
+				if (u8tmp & BIT(0)) {
+					RT_TRACE(rtlpriv, COMP_BT_COEXIST,
+						 DBG_LOUD,
+						 "[BTCoex], ########### BT is calibrating (wait cnt=%d) ###########\n",
+						 cnt_bt_cal_chk);
+					mdelay(50);
+				} else {
+					RT_TRACE(rtlpriv, COMP_BT_COEXIST,
+						 DBG_LOUD,
+						 "[BTCoex], ********** BT is NOT calibrating (wait cnt=%d)**********\n",
+						 cnt_bt_cal_chk);
+					break;
+				}
+			}
+
+			/* set grant_bt to PTA */
+			btcoexist->btc_write_1byte(btcoexist, 0x765, 0x0);
+		}
+
+		if (btcoexist->btc_read_1byte(btcoexist, 0x76e) != 0xc) {
+			/* set wlan_act control by PTA */
+			btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+		}
+
+		btcoexist->btc_write_1byte_bitmask(
+			btcoexist, 0x67, 0x20,
+			0x1); /* BT select s0/s1 is controlled by WiFi */
 	}
 
 	if (use_ext_switch) {
@@ -658,216 +927,278 @@ static void halbtc8723b1ant_set_ant_path(struct btc_coexist *btcoexist,
 			u32tmp |= BIT24;
 			btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
 
+			/* fixed internal switch S1->WiFi, S0->BT */
+			btcoexist->btc_write_4byte(btcoexist, 0x948, 0x0);
+
 			if (board_info->btdm_ant_pos ==
 			    BTC_ANTENNA_AT_MAIN_PORT) {
-				/* Main Ant to BT for IPS case 0x4c[23] = 1 */
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x64, 0x1,
-								   0x1);
-
 				/* tell firmware "no antenna inverse" */
 				h2c_parameter[0] = 0;
-				h2c_parameter[1] = 1;  /*ext switch type*/
+				/* ext switch type */
+				h2c_parameter[1] = 1;
 				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
 							h2c_parameter);
 			} else {
-				/* Aux Ant to  BT for IPS case 0x4c[23] = 1 */
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x64, 0x1,
-								   0x0);
-
 				/* tell firmware "antenna inverse" */
 				h2c_parameter[0] = 1;
-				h2c_parameter[1] = 1; /* ext switch type */
+				/* ext switch type */
+				h2c_parameter[1] = 1;
 				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
 							h2c_parameter);
 			}
 		}
 
-		/* fixed internal switch first
-		 * fixed internal switch S1->WiFi, S0->BT
-		 */
-		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
-			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
-		else	/* fixed internal switch S0->WiFi, S1->BT */
-			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x280);
-
-		/* ext switch setting */
-		switch (ant_pos_type) {
-		case BTC_ANT_PATH_WIFI:
-			if (board_info->btdm_ant_pos ==
-			    BTC_ANTENNA_AT_MAIN_PORT)
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x92c, 0x3,
-								   0x1);
-			else
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x92c, 0x3,
-								   0x2);
-			break;
-		case BTC_ANT_PATH_BT:
-			if (board_info->btdm_ant_pos ==
-			    BTC_ANTENNA_AT_MAIN_PORT)
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x92c, 0x3,
-								   0x2);
-			else
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x92c, 0x3,
-								   0x1);
-			break;
-		default:
-		case BTC_ANT_PATH_PTA:
-			if (board_info->btdm_ant_pos ==
-			    BTC_ANTENNA_AT_MAIN_PORT)
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x92c, 0x3,
-								   0x1);
-			else
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x92c, 0x3,
-								   0x2);
-			break;
+		if (force_exec ||
+		    (coex_dm->cur_ant_pos_type != coex_dm->pre_ant_pos_type)) {
+			/* ext switch setting */
+			switch (ant_pos_type) {
+			case BTC_ANT_PATH_WIFI:
+				if (board_info->btdm_ant_pos ==
+				    BTC_ANTENNA_AT_MAIN_PORT)
+					btcoexist->btc_write_1byte_bitmask(
+						btcoexist, 0x92c, 0x3, 0x1);
+				else
+					btcoexist->btc_write_1byte_bitmask(
+						btcoexist, 0x92c, 0x3, 0x2);
+				break;
+			case BTC_ANT_PATH_BT:
+				if (board_info->btdm_ant_pos ==
+				    BTC_ANTENNA_AT_MAIN_PORT)
+					btcoexist->btc_write_1byte_bitmask(
+						btcoexist, 0x92c, 0x3, 0x2);
+				else
+					btcoexist->btc_write_1byte_bitmask(
+						btcoexist, 0x92c, 0x3, 0x1);
+				break;
+			default:
+			case BTC_ANT_PATH_PTA:
+				if (board_info->btdm_ant_pos ==
+				    BTC_ANTENNA_AT_MAIN_PORT)
+					btcoexist->btc_write_1byte_bitmask(
+						btcoexist, 0x92c, 0x3, 0x1);
+				else
+					btcoexist->btc_write_1byte_bitmask(
+						btcoexist, 0x92c, 0x3, 0x2);
+				break;
+			}
 		}
-
 	} else {
 		if (init_hw_cfg) {
-			/* 0x4c[23] = 1, 0x4c[24] = 0 Antenna control by 0x64 */
+			/* 0x4c[23] = 1, 0x4c[24] = 0,
+			 * Antenna control by 0x64
+			 */
 			u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
 			u32tmp |= BIT23;
 			u32tmp &= ~BIT24;
 			btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
 
+			/* Fix Ext switch Main->S1, Aux->S0 */
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x64, 0x1,
+							   0x0);
+
 			if (board_info->btdm_ant_pos ==
 			    BTC_ANTENNA_AT_MAIN_PORT) {
-				/* Main Ant to WiFi for IPS case 0x4c[23] = 1 */
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x64, 0x1,
-								   0x0);
-
 				/* tell firmware "no antenna inverse" */
 				h2c_parameter[0] = 0;
-				h2c_parameter[1] = 0; /* internal switch type */
+				/* internal switch type */
+				h2c_parameter[1] = 0;
 				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
 							h2c_parameter);
 			} else {
-				/* Aux Ant to BT for IPS case 0x4c[23] = 1 */
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x64, 0x1,
-								   0x1);
-
 				/* tell firmware "antenna inverse" */
 				h2c_parameter[0] = 1;
-				h2c_parameter[1] = 0; /* internal switch type */
+				/* internal switch type */
+				h2c_parameter[1] = 0;
 				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
 							h2c_parameter);
 			}
 		}
 
-		/* fixed external switch first
-		 * Main->WiFi, Aux->BT
-		 */
-		if (board_info->btdm_ant_pos ==
-			BTC_ANTENNA_AT_MAIN_PORT)
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
-							   0x3, 0x1);
-		else	/* Main->BT, Aux->WiFi */
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
-							   0x3, 0x2);
-
-		/* internal switch setting */
-		switch (ant_pos_type) {
-		case BTC_ANT_PATH_WIFI:
-			if (board_info->btdm_ant_pos ==
-				BTC_ANTENNA_AT_MAIN_PORT)
-				btcoexist->btc_write_2byte(btcoexist, 0x948,
-							   0x0);
-			else
-				btcoexist->btc_write_2byte(btcoexist, 0x948,
-							   0x280);
-			break;
-		case BTC_ANT_PATH_BT:
-			if (board_info->btdm_ant_pos ==
-				BTC_ANTENNA_AT_MAIN_PORT)
-				btcoexist->btc_write_2byte(btcoexist, 0x948,
-							   0x280);
-			else
-				btcoexist->btc_write_2byte(btcoexist, 0x948,
-							   0x0);
-			break;
-		default:
-		case BTC_ANT_PATH_PTA:
-			if (board_info->btdm_ant_pos ==
-				BTC_ANTENNA_AT_MAIN_PORT)
-				btcoexist->btc_write_2byte(btcoexist, 0x948,
-							   0x200);
-			else
-				btcoexist->btc_write_2byte(btcoexist, 0x948,
-							   0x80);
-			break;
+		if (force_exec ||
+		    (coex_dm->cur_ant_pos_type != coex_dm->pre_ant_pos_type)) {
+			/* internal switch setting */
+			switch (ant_pos_type) {
+			case BTC_ANT_PATH_WIFI:
+				if (board_info->btdm_ant_pos ==
+				    BTC_ANTENNA_AT_MAIN_PORT)
+					btcoexist->btc_write_4byte(btcoexist,
+							0x948, 0x0);
+				else
+					btcoexist->btc_write_4byte(btcoexist,
+							0x948, 0x280);
+				break;
+			case BTC_ANT_PATH_BT:
+				if (board_info->btdm_ant_pos ==
+				    BTC_ANTENNA_AT_MAIN_PORT)
+					btcoexist->btc_write_4byte(btcoexist,
+							0x948, 0x280);
+				else
+					btcoexist->btc_write_4byte(btcoexist,
+							0x948, 0x0);
+				break;
+			default:
+			case BTC_ANT_PATH_PTA:
+				if (board_info->btdm_ant_pos ==
+				    BTC_ANTENNA_AT_MAIN_PORT)
+					btcoexist->btc_write_4byte(btcoexist,
+							0x948, 0x200);
+				else
+					btcoexist->btc_write_4byte(btcoexist,
+							0x948, 0x80);
+				break;
+			}
 		}
 	}
+
+	coex_dm->pre_ant_pos_type = coex_dm->cur_ant_pos_type;
 }
 
 static void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist,
 				    bool force_exec, bool turn_on, u8 type)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	bool wifi_busy = false;
 	u8 rssi_adjust_val = 0;
+	u8 ps_tdma_byte0_val = 0x51;
+	u8 ps_tdma_byte3_val = 0x10;
+	u8 ps_tdma_byte4_val = 0x50;
+	s8 wifi_duration_adjust = 0x0;
+	static bool pre_wifi_busy;
 
 	coex_dm->cur_ps_tdma_on = turn_on;
 	coex_dm->cur_ps_tdma = type;
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
 
-	if (!force_exec) {
-		if (coex_dm->cur_ps_tdma_on)
-			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-				 "[BTCoex], ******** TDMA(on, %d) *********\n",
-				 coex_dm->cur_ps_tdma);
-		else
-			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-				 "[BTCoex], ******** TDMA(off, %d) ********\n",
-				 coex_dm->cur_ps_tdma);
+	if (wifi_busy != pre_wifi_busy) {
+		force_exec = true;
+		pre_wifi_busy = wifi_busy;
+	}
 
+	if (!force_exec) {
 		if ((coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
 		    (coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
 			return;
 	}
+
+	if (coex_sta->scan_ap_num <= 5) {
+		wifi_duration_adjust = 5;
+
+		if (coex_sta->a2dp_bit_pool >= 35)
+			wifi_duration_adjust = -10;
+		else if (coex_sta->a2dp_bit_pool >= 45)
+			wifi_duration_adjust = -15;
+	} else if (coex_sta->scan_ap_num >= 40) {
+		wifi_duration_adjust = -15;
+
+		if (coex_sta->a2dp_bit_pool < 35)
+			wifi_duration_adjust = -5;
+		else if (coex_sta->a2dp_bit_pool < 45)
+			wifi_duration_adjust = -10;
+	} else if (coex_sta->scan_ap_num >= 20) {
+		wifi_duration_adjust = -10;
+
+		if (coex_sta->a2dp_bit_pool >= 45)
+			wifi_duration_adjust = -15;
+	} else {
+		wifi_duration_adjust = 0;
+
+		if (coex_sta->a2dp_bit_pool >= 35)
+			wifi_duration_adjust = -10;
+		else if (coex_sta->a2dp_bit_pool >= 45)
+			wifi_duration_adjust = -15;
+	}
+
+	if ((type == 1) || (type == 2) || (type == 9) || (type == 11) ||
+	    (type == 101) || (type == 102) || (type == 109) || (type == 101)) {
+		if (!coex_sta->force_lps_on) {
+			/* Native power save TDMA, only for A2DP-only case
+			 * 1/2/9/11 while wifi noisy threshold > 30
+			 */
+
+			/* no null-pkt */
+			ps_tdma_byte0_val = 0x61;
+			/* no tx-pause at BT-slot */
+			ps_tdma_byte3_val = 0x11;
+			/* 0x778 = d/1 toggle, no dynamic slot */
+			ps_tdma_byte4_val = 0x10;
+		} else {
+			/* null-pkt */
+			ps_tdma_byte0_val = 0x51;
+			/* tx-pause at BT-slot */
+			ps_tdma_byte3_val = 0x10;
+			/* 0x778 = d/1 toggle, dynamic slot */
+			ps_tdma_byte4_val = 0x50;
+		}
+	} else if ((type == 3) || (type == 13) || (type == 14) ||
+		   (type == 103) || (type == 113) || (type == 114)) {
+		/* null-pkt */
+		ps_tdma_byte0_val = 0x51;
+		/* tx-pause at BT-slot */
+		ps_tdma_byte3_val = 0x10;
+		/* 0x778 = d/1 toggle, no dynamic slot */
+		ps_tdma_byte4_val = 0x10;
+	} else { /* native power save case */
+		/* no null-pkt */
+		ps_tdma_byte0_val = 0x61;
+		/* no tx-pause at BT-slot */
+		ps_tdma_byte3_val = 0x11;
+		/* 0x778 = d/1 toggle, no dynamic slot */
+		ps_tdma_byte4_val = 0x11;
+		/* psTdmaByte4Va is not define for 0x778 = d/1, 1/1 case */
+	}
+
+	/* if (bt_link_info->slave_role) */
+	if ((bt_link_info->slave_role) && (bt_link_info->a2dp_exist))
+		/* 0x778 = 0x1 at wifi slot (no blocking BT Low-Pri pkts) */
+		ps_tdma_byte4_val = ps_tdma_byte4_val | 0x1;
+
+	if (type > 100) {
+		/* set antenna control by SW	 */
+		ps_tdma_byte0_val = ps_tdma_byte0_val | 0x82;
+		/* set antenna no toggle, control by antenna diversity */
+		ps_tdma_byte3_val = ps_tdma_byte3_val | 0x60;
+	}
+
 	if (turn_on) {
 		switch (type) {
 		default:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x1a,
-						       0x1a, 0x0, 0x50);
+						      0x1a, 0x0,
+						      ps_tdma_byte4_val);
 			break;
 		case 1:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x3a,
-						       0x03, 0x10, 0x50);
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val,
+				0x3a + wifi_duration_adjust, 0x03,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
 
 			rssi_adjust_val = 11;
 			break;
 		case 2:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x2b,
-						       0x03, 0x10, 0x50);
-			rssi_adjust_val = 14;
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val,
+				0x2d + wifi_duration_adjust, 0x03,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
 			break;
 		case 3:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x1d,
-						       0x1d, 0x0, 0x52);
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x30, 0x03,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
 			break;
 		case 4:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x15,
-						       0x3, 0x14, 0x0);
-			rssi_adjust_val = 17;
+						      0x3, 0x14, 0x0);
 			break;
 		case 5:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x15,
-						       0x3, 0x11, 0x10);
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x1f, 0x3,
+				ps_tdma_byte3_val, 0x11);
 			break;
 		case 6:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x20,
-						       0x3, 0x11, 0x13);
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x20, 0x3,
+				ps_tdma_byte3_val, 0x11);
 			break;
 		case 7:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xc,
@@ -875,33 +1206,44 @@ static void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist,
 			break;
 		case 8:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x25,
-						       0x3, 0x10, 0x0);
+						      0x3, 0x10, 0x0);
 			break;
 		case 9:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,  0x21,
-						       0x3, 0x10, 0x50);
-			rssi_adjust_val = 18;
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x21, 0x3,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
 			break;
 		case 10:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xa,
 						       0xa, 0x0, 0x40);
 			break;
 		case 11:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x15,
-						       0x03, 0x10, 0x50);
-			rssi_adjust_val = 20;
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x21, 0x03,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
 			break;
 		case 12:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x0a,
-						       0x0a, 0x0, 0x50);
+						      0x0a, 0x0, 0x50);
 			break;
 		case 13:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x15,
-						       0x15, 0x0, 0x50);
+			if (coex_sta->scan_ap_num <= 3)
+				halbtc8723b1ant_set_fw_ps_tdma(
+					btcoexist, ps_tdma_byte0_val, 0x40, 0x3,
+					ps_tdma_byte3_val, ps_tdma_byte4_val);
+			else
+				halbtc8723b1ant_set_fw_ps_tdma(
+					btcoexist, ps_tdma_byte0_val, 0x21, 0x3,
+					ps_tdma_byte3_val, ps_tdma_byte4_val);
 			break;
 		case 14:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x21,
-						       0x3, 0x10, 0x52);
+			if (coex_sta->scan_ap_num <= 3)
+				halbtc8723b1ant_set_fw_ps_tdma(
+					btcoexist, 0x51, 0x30, 0x3, 0x10, 0x50);
+			else
+				halbtc8723b1ant_set_fw_ps_tdma(
+					btcoexist, ps_tdma_byte0_val, 0x21, 0x3,
+					ps_tdma_byte3_val, ps_tdma_byte4_val);
 			break;
 		case 15:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xa,
@@ -909,103 +1251,173 @@ static void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist,
 			break;
 		case 16:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x15,
-						       0x3, 0x10, 0x0);
-			rssi_adjust_val = 18;
+						      0x3, 0x10, 0x0);
 			break;
 		case 18:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x25,
-						       0x3, 0x10, 0x0);
-			rssi_adjust_val = 14;
+						      0x3, 0x10, 0x0);
 			break;
 		case 20:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x35,
-						       0x03, 0x11, 0x10);
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x3f, 0x03,
+				ps_tdma_byte3_val, 0x10);
 			break;
 		case 21:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x25,
-						       0x03, 0x11, 0x11);
+						      0x03, 0x11, 0x11);
 			break;
 		case 22:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x25,
-						       0x03, 0x11, 0x10);
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x25, 0x03,
+				ps_tdma_byte3_val, 0x10);
 			break;
 		case 23:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
-						       0x3, 0x31, 0x18);
-			rssi_adjust_val = 22;
+						      0x3, 0x31, 0x18);
 			break;
 		case 24:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x15,
-						       0x3, 0x31, 0x18);
-			rssi_adjust_val = 22;
+						      0x3, 0x31, 0x18);
 			break;
 		case 25:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
 						       0x3, 0x31, 0x18);
-			rssi_adjust_val = 22;
 			break;
 		case 26:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
 						       0x3, 0x31, 0x18);
-			rssi_adjust_val = 22;
 			break;
 		case 27:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
-						       0x3, 0x31, 0x98);
-			rssi_adjust_val = 22;
+						      0x3, 0x31, 0x98);
 			break;
 		case 28:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x69, 0x25,
-						       0x3, 0x31, 0x0);
+						      0x3, 0x31, 0x0);
 			break;
 		case 29:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xab, 0x1a,
-						       0x1a, 0x1, 0x10);
+						      0x1a, 0x1, 0x10);
 			break;
 		case 30:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x14,
-						       0x3, 0x10, 0x50);
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x30,
+						       0x3, 0x10, 0x10);
 			break;
 		case 31:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xd3, 0x1a,
-						       0x1a, 0, 0x58);
+						      0x1a, 0, 0x58);
 			break;
 		case 32:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0xa,
-						       0x3, 0x10, 0x0);
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x35, 0x3,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
 			break;
 		case 33:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xa3, 0x25,
-						       0x3, 0x30, 0x90);
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x35, 0x3,
+				ps_tdma_byte3_val, 0x10);
 			break;
 		case 34:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x53, 0x1a,
-						       0x1a, 0x0, 0x10);
+						      0x1a, 0x0, 0x10);
 			break;
 		case 35:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x63, 0x1a,
-						       0x1a, 0x0, 0x10);
+						      0x1a, 0x0, 0x10);
 			break;
 		case 36:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xd3, 0x12,
-						       0x3, 0x14, 0x50);
+						      0x3, 0x14, 0x50);
 			break;
-		/* SoftAP only with no sta associated, BT disable,
-		 * TDMA mode for power saving
-		 * here softap mode screen off will cost 70-80mA for phone
-		 */
 		case 40:
+			/* SoftAP only with no sta associated,BT disable ,TDMA
+			 * mode for power saving
+			 *
+			 * here softap mode screen off will cost 70-80mA for
+			 * phone
+			 */
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x23, 0x18,
-						       0x00, 0x10, 0x24);
+						      0x00, 0x10, 0x24);
+			break;
+
+		case 101:
+			/* for 1-Ant translate to 2-Ant	 */
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val,
+				0x3a + wifi_duration_adjust, 0x03,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
+			break;
+		case 102:
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val,
+				0x2d + wifi_duration_adjust, 0x03,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
+			break;
+		case 103:
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x3a, 0x03,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
+			break;
+		case 105:
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x15, 0x3,
+				ps_tdma_byte3_val, 0x11);
+			break;
+		case 106:
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x20, 0x3,
+				ps_tdma_byte3_val, 0x11);
+			break;
+		case 109:
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x21, 0x3,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
+			break;
+		case 111:
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x21, 0x03,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
+			break;
+		case 113:
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x21, 0x3,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
+			break;
+		case 114:
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x21, 0x3,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
+			break;
+		case 120:
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x3f, 0x03,
+				ps_tdma_byte3_val, 0x10);
+			break;
+		case 122:
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x25, 0x03,
+				ps_tdma_byte3_val, 0x10);
+			break;
+		case 132:
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x25, 0x03,
+				ps_tdma_byte3_val, ps_tdma_byte4_val);
+			break;
+		case 133:
+			halbtc8723b1ant_set_fw_ps_tdma(
+				btcoexist, ps_tdma_byte0_val, 0x25, 0x03,
+				ps_tdma_byte3_val, 0x11);
 			break;
 		}
 	} else {
+		/* disable PS tdma */
 		switch (type) {
 		case 8: /* PTA Control */
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x8, 0x0,
 						       0x0, 0x0, 0x0);
 			halbtc8723b1ant_set_ant_path(btcoexist,
 						     BTC_ANT_PATH_PTA,
+						     FORCE_EXEC,
 						     false, false);
 			break;
 		case 0:
@@ -1013,17 +1425,10 @@ static void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist,
 			/* Software control, Antenna at BT side */
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0,
 						       0x0, 0x0, 0x0);
-			halbtc8723b1ant_set_ant_path(btcoexist,
-						     BTC_ANT_PATH_BT,
-						     false, false);
 			break;
-		case 9:
-			/* Software control, Antenna at WiFi side */
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0,
-						       0x0, 0x0, 0x0);
-			halbtc8723b1ant_set_ant_path(btcoexist,
-						     BTC_ANT_PATH_WIFI,
-						     false, false);
+		case 1: /* 2-Ant, 0x778=3, antenna control by ant diversity */
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0,
+						       0x48, 0x0);
 			break;
 		}
 	}
@@ -1037,8 +1442,191 @@ static void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist,
 	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
 }
 
+void btc8723b1ant_tdma_dur_adj_for_acl(struct btc_coexist *btcoexist,
+				       u8 wifi_status)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	static s32 up, dn, m, n, wait_count;
+	/*  0: no change, +1: increase WiFi duration,
+	 * -1: decrease WiFi duration
+	 */
+	s32 result;
+	u8 retry_count = 0, bt_info_ext;
+	bool wifi_busy = false;
+
+	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+		 "[BTCoex], TdmaDurationAdjustForAcl()\n");
+
+	if (wifi_status == BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY)
+		wifi_busy = true;
+	else
+		wifi_busy = false;
+
+	if ((wifi_status ==
+	     BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN) ||
+	    (wifi_status == BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN) ||
+	    (wifi_status == BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT)) {
+		if (coex_dm->cur_ps_tdma != 1 && coex_dm->cur_ps_tdma != 2 &&
+		    coex_dm->cur_ps_tdma != 3 && coex_dm->cur_ps_tdma != 9) {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 9);
+			coex_dm->ps_tdma_du_adj_type = 9;
+
+			up = 0;
+			dn = 0;
+			m = 1;
+			n = 3;
+			result = 0;
+			wait_count = 0;
+		}
+		return;
+	}
+
+	if (!coex_dm->auto_tdma_adjust) {
+		coex_dm->auto_tdma_adjust = true;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], first run TdmaDurationAdjust()!!\n");
+
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
+		coex_dm->ps_tdma_du_adj_type = 2;
+
+		up = 0;
+		dn = 0;
+		m = 1;
+		n = 3;
+		result = 0;
+		wait_count = 0;
+	} else {
+		/* acquire the BT TRx retry count from BT_Info byte2 */
+		retry_count = coex_sta->bt_retry_cnt;
+		bt_info_ext = coex_sta->bt_info_ext;
+
+		if ((coex_sta->low_priority_tx) > 1050 ||
+		    (coex_sta->low_priority_rx) > 1250)
+			retry_count++;
+
+		result = 0;
+		wait_count++;
+		/* no retry in the last 2-second duration */
+		if (retry_count == 0) {
+			up++;
+			dn--;
+
+			if (dn <= 0)
+				dn = 0;
+
+			if (up >= n) {
+				/* if retry count during continuous n*2 seconds
+				 * is 0, enlarge WiFi duration
+				 */
+				wait_count = 0;
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1;
+				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+					 "[BTCoex], Increase wifi duration!!\n");
+			}
+		} else if (retry_count <= 3) {
+			/* <=3 retry in the last 2-second duration */
+			up--;
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2) {
+				/* if continuous 2 retry count(every 2 seconds)
+				 * >0 and < 3, reduce WiFi duration
+				 */
+				if (wait_count <= 2)
+					/* avoid loop between the two levels */
+					m++;
+				else
+					m = 1;
+
+				if (m >= 20)
+					/* maximum of m = 20 ' will recheck if
+					 * need to adjust wifi duration in
+					 * maximum time interval 120 seconds
+					 */
+					m = 20;
+
+				n = 3 * m;
+				up = 0;
+				dn = 0;
+				wait_count = 0;
+				result = -1;
+				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+					 "[BTCoex], Decrease wifi duration for retryCounter<3!!\n");
+			}
+		} else {
+			/* retry count > 3, once retry count > 3, to reduce
+			 * WiFi duration
+			 */
+			if (wait_count == 1)
+				/* to avoid loop between the two levels */
+				m++;
+			else
+				m = 1;
+
+			if (m >= 20)
+				/* maximum of m = 20 ' will recheck if need to
+				 * adjust wifi duration in maximum time interval
+				 * 120 seconds
+				 */
+				m = 20;
+
+			n = 3 * m;
+			up = 0;
+			dn = 0;
+			wait_count = 0;
+			result = -1;
+			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+				 "[BTCoex], Decrease wifi duration for retryCounter>3!!\n");
+		}
+
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 1) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->ps_tdma_du_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->ps_tdma_du_adj_type = 11;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->ps_tdma_du_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 1);
+				coex_dm->ps_tdma_du_adj_type = 1;
+			}
+		}
+
+		if (coex_dm->cur_ps_tdma != 1 && coex_dm->cur_ps_tdma != 2 &&
+		    coex_dm->cur_ps_tdma != 9 && coex_dm->cur_ps_tdma != 11) {
+			/* recover to previous adjust type */
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
+						coex_dm->ps_tdma_du_adj_type);
+		}
+	}
+}
+
 static void halbtc8723b1ant_ps_tdma_chk_pwr_save(struct btc_coexist *btcoexist,
-						 bool new_ps_state)
+					  bool new_ps_state)
 {
 	u8 lps_mode = 0x0;
 
@@ -1078,6 +1666,7 @@ static void halbtc8723b1ant_power_save_state(struct btc_coexist *btcoexist,
 		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
 				   &low_pwr_disable);
 		btcoexist->btc_set(btcoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
+		coex_sta->force_lps_on = false;
 		break;
 	case BTC_PS_LPS_ON:
 		halbtc8723b1ant_ps_tdma_chk_pwr_save(btcoexist, true);
@@ -1089,27 +1678,95 @@ static void halbtc8723b1ant_power_save_state(struct btc_coexist *btcoexist,
 				   &low_pwr_disable);
 		/* power save must executed before psTdma */
 		btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+		coex_sta->force_lps_on = true;
 		break;
 	case BTC_PS_LPS_OFF:
 		halbtc8723b1ant_ps_tdma_chk_pwr_save(btcoexist, false);
 		btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+		coex_sta->force_lps_on = false;
 		break;
 	default:
 		break;
 	}
 }
 
+static void halbtc8723b1ant_action_wifi_only(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
+	halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA,
+				     FORCE_EXEC, false, false);
+}
+
+/* check if BT is disabled */
+static void halbtc8723b1ant_monitor_bt_enable_disable(struct btc_coexist
+						      *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	static u32 bt_disable_cnt;
+	bool bt_active = true, bt_disabled = false;
+
+	if (coex_sta->high_priority_tx == 0 &&
+	    coex_sta->high_priority_rx == 0 && coex_sta->low_priority_tx == 0 &&
+	    coex_sta->low_priority_rx == 0)
+		bt_active = false;
+	if (coex_sta->high_priority_tx == 0xffff &&
+	    coex_sta->high_priority_rx == 0xffff &&
+	    coex_sta->low_priority_tx == 0xffff &&
+	    coex_sta->low_priority_rx == 0xffff)
+		bt_active = false;
+	if (bt_active) {
+		bt_disable_cnt = 0;
+		bt_disabled = false;
+	} else {
+		bt_disable_cnt++;
+		if (bt_disable_cnt >= 2)
+			bt_disabled = true;
+	}
+	if (coex_sta->bt_disabled != bt_disabled) {
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], BT is from %s to %s!!\n",
+			 (coex_sta->bt_disabled ? "disabled" : "enabled"),
+			 (bt_disabled ? "disabled" : "enabled"));
+
+		coex_sta->bt_disabled = bt_disabled;
+		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
+				   &bt_disabled);
+		if (bt_disabled) {
+			halbtc8723b1ant_action_wifi_only(btcoexist);
+			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS,
+					   NULL);
+			btcoexist->btc_set(btcoexist, BTC_SET_ACT_NORMAL_LPS,
+					   NULL);
+		}
+	}
+}
+
 /*****************************************************
  *
  *	Non-Software Coex Mechanism start
  *
  *****************************************************/
+
+static void halbtc8723b1ant_action_bt_whck_test(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0,
+					 0x0);
+
+	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+	halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA, NORMAL_EXEC,
+				     false, false);
+	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+}
+
 static void halbtc8723b1ant_action_wifi_multiport(struct btc_coexist *btcoexist)
 {
 	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
 					 0x0, 0x0);
 
 	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+	halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA, NORMAL_EXEC,
+				     false, false);
 	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
 }
 
@@ -1123,35 +1780,56 @@ static void halbtc8723b1ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	bool wifi_connected = false, ap_enable = false;
+	bool wifi_busy = false, bt_busy = false;
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
 			   &ap_enable);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
 			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
 
-	if (!wifi_connected) {
-		halbtc8723b1ant_power_save_state(btcoexist,
-						 BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+	if (coex_sta->bt_abnormal_scan) {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 33);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+	} else if (!wifi_connected && !coex_sta->wifi_is_high_pri_task) {
+		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+						 0x0, 0x0);
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
-	} else if (bt_link_info->sco_exist || bt_link_info->hid_only) {
-		/* SCO/HID-only busy */
+		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA,
+					     NORMAL_EXEC, false, false);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+	} else if (bt_link_info->sco_exist || bt_link_info->hid_exist ||
+		   bt_link_info->a2dp_exist) {
+		/* SCO/HID/A2DP busy */
 		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
 						 0x0, 0x0);
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 32);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
-	} else {
-		if (ap_enable)
-			halbtc8723b1ant_power_save_state(btcoexist,
-							 BTC_PS_WIFI_NATIVE,
-							 0x0, 0x0);
+		if (coex_sta->c2h_bt_remote_name_req)
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
+						33);
 		else
-			halbtc8723b1ant_power_save_state(btcoexist,
-							 BTC_PS_LPS_ON,
-							 0x50, 0x4);
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
+						32);
+
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
+	} else if (bt_link_info->pan_exist || wifi_busy) {
+		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+						 0x0, 0x0);
+		if (coex_sta->c2h_bt_remote_name_req)
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
+						33);
+		else
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
+						32);
 
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 30);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
+	} else {
+		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+						 0x0, 0x0);
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA,
+					     NORMAL_EXEC, false, false);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
 	}
 }
 
@@ -1167,7 +1845,7 @@ static void btc8723b1ant_act_bt_sco_hid_only_busy(struct btc_coexist *btcoexist,
 	/* tdma and coex table */
 	if (bt_link_info->sco_exist) {
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 5);
 	} else {
 		/* HID */
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 6);
@@ -1181,6 +1859,10 @@ static void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 
+	if ((coex_sta->low_priority_rx >= 950) && (!coex_sta->under_ips))
+		bt_link_info->slave_role = true;
+	else
+		bt_link_info->slave_role = false;
 
 	if (bt_link_info->hid_only) { /* HID */
 		btc8723b1ant_act_bt_sco_hid_only_busy(btcoexist, wifi_status);
@@ -1189,39 +1871,40 @@ static void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
 	} else if (bt_link_info->a2dp_only) { /* A2DP */
 		if (wifi_status == BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE) {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						false, 8);
+						true, 32);
 			halbtc8723b1ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 2);
+							     NORMAL_EXEC, 4);
 			coex_dm->auto_tdma_adjust = false;
-		} else { /* for low BT RSSI */
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						true, 11);
+		} else {
+			btc8723b1ant_tdma_dur_adj_for_acl(btcoexist,
+							  wifi_status);
 			halbtc8723b1ant_coex_table_with_type(btcoexist,
 							     NORMAL_EXEC, 1);
-			coex_dm->auto_tdma_adjust = false;
+			coex_dm->auto_tdma_adjust = true;
 		}
-	} else if (bt_link_info->hid_exist &&
-		bt_link_info->a2dp_exist) { /* HID + A2DP */
+	} else if (((bt_link_info->a2dp_exist) && (bt_link_info->pan_exist)) ||
+		   (bt_link_info->hid_exist && bt_link_info->a2dp_exist &&
+		    bt_link_info->pan_exist)) {
+		/* A2DP + PAN(OPP,FTP), HID + A2DP + PAN(OPP,FTP) */
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
+		coex_dm->auto_tdma_adjust = false;
+	} else if (bt_link_info->hid_exist && bt_link_info->a2dp_exist) {
+		/* HID + A2DP */
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,	true, 14);
 		coex_dm->auto_tdma_adjust = false;
 
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 6);
-	 /* PAN(OPP,FTP), HID + PAN(OPP,FTP) */
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
 	} else if (bt_link_info->pan_only ||
-		   (bt_link_info->hid_exist && bt_link_info->pan_exist)) {
+			(bt_link_info->hid_exist && bt_link_info->pan_exist)) {
+		/* PAN(OPP,FTP), HID + PAN(OPP,FTP) */
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 6);
-		coex_dm->auto_tdma_adjust = false;
-	 /* A2DP + PAN(OPP,FTP), HID + A2DP + PAN(OPP,FTP) */
-	} else if ((bt_link_info->a2dp_exist && bt_link_info->pan_exist) ||
-		   (bt_link_info->hid_exist && bt_link_info->a2dp_exist &&
-		    bt_link_info->pan_exist)) {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
 		coex_dm->auto_tdma_adjust = false;
 	} else {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		/* BT no-profile busy (0x9) */
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 33);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
 		coex_dm->auto_tdma_adjust = false;
 	}
 }
@@ -1233,7 +1916,9 @@ static void btc8723b1ant_action_wifi_not_conn(struct btc_coexist *btcoexist)
 					 0x0, 0x0);
 
 	/* tdma and coex table */
-	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
+	halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA, NORMAL_EXEC,
+				     false, false);
 	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
 }
 
@@ -1246,30 +1931,31 @@ btc8723b1ant_action_wifi_not_conn_scan(struct btc_coexist *btcoexist)
 					 0x0, 0x0);
 
 	/* tdma and coex table */
-	if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
-		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
+	if (coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_ACL_BUSY) {
+		if (bt_link_info->a2dp_exist) {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						true, 22);
+						true, 32);
 			halbtc8723b1ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 1);
-		} else if (bt_link_info->pan_only) {
+							     NORMAL_EXEC, 4);
+		} else if (bt_link_info->a2dp_exist) {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						true, 20);
+						true, 22);
 			halbtc8723b1ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 2);
+							     NORMAL_EXEC, 4);
 		} else {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						true, 20);
 			halbtc8723b1ant_coex_table_with_type(btcoexist,
 							     NORMAL_EXEC, 1);
 		}
-	} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-		   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
-		    coex_dm->bt_status)){
+	} else if (coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_SCO_BUSY ||
+		   coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY){
 		btc8723b1ant_act_bt_sco_hid_only_busy(btcoexist,
 				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN);
 	} else {
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA,
+					     NORMAL_EXEC, false, false);
 		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
 	}
 }
@@ -1282,14 +1968,19 @@ btc8723b1ant_act_wifi_not_conn_asso_auth(struct btc_coexist *btcoexist)
 	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
 					 0x0, 0x0);
 
-	if ((BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) ||
-	    (bt_link_info->sco_exist) || (bt_link_info->hid_only) ||
-	    (bt_link_info->a2dp_only) || (bt_link_info->pan_only)) {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-	} else {
+	/* tdma and coex table */
+	if ((bt_link_info->sco_exist) || (bt_link_info->hid_exist) ||
+	    (bt_link_info->a2dp_exist)) {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 32);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 4);
+	} else if (bt_link_info->pan_exist) {
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 4);
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA,
+					     NORMAL_EXEC, false, false);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 2);
 	}
 }
 
@@ -1301,30 +1992,32 @@ static void btc8723b1ant_action_wifi_conn_scan(struct btc_coexist *btcoexist)
 					 0x0, 0x0);
 
 	/* tdma and coex table */
-	if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
-		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
+	if (coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_ACL_BUSY) {
+		if (bt_link_info->a2dp_exist) {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						true, 22);
+						true, 32);
 			halbtc8723b1ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 1);
-		} else if (bt_link_info->pan_only) {
+							     NORMAL_EXEC, 4);
+		} else if (bt_link_info->a2dp_exist &&
+			   bt_link_info->pan_exist) {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						true, 20);
+						true, 22);
 			halbtc8723b1ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 2);
+							     NORMAL_EXEC, 4);
 		} else {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						true, 20);
 			halbtc8723b1ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 1);
+							     NORMAL_EXEC, 4);
 		}
-	} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-		   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
-		    coex_dm->bt_status)) {
+	} else if (coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_SCO_BUSY ||
+		   coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY) {
 		btc8723b1ant_act_bt_sco_hid_only_busy(btcoexist,
 				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN);
 	} else {
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA,
+					     NORMAL_EXEC, false, false);
 		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
 	}
 }
@@ -1332,23 +2025,34 @@ static void btc8723b1ant_action_wifi_conn_scan(struct btc_coexist *btcoexist)
 static void halbtc8723b1ant_action_wifi_connected_special_packet(
 						struct btc_coexist *btcoexist)
 {
-	bool hs_connecting = false;
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool wifi_busy = false;
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_CONNECTING, &hs_connecting);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+
+	/* no special packet process for both WiFi and BT very busy */
+	if ((wifi_busy) &&
+	    ((bt_link_info->pan_exist) || (coex_sta->num_of_profile >= 2)))
+		return;
 
 	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
 					 0x0, 0x0);
 
 	/* tdma and coex table */
-	if ((BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) ||
-	    (bt_link_info->sco_exist) || (bt_link_info->hid_only) ||
-	    (bt_link_info->a2dp_only) || (bt_link_info->pan_only)) {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-	} else {
+	if ((bt_link_info->sco_exist) || (bt_link_info->hid_exist)) {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 32);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 5);
+	} else if (bt_link_info->a2dp_exist) {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 32);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
+	} else if (bt_link_info->pan_exist) {
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA,
+					     NORMAL_EXEC, false, false);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
 	}
 }
 
@@ -1391,12 +2095,31 @@ static void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
 	/* power save state */
 	if (!ap_enable &&
-	    BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status &&
+	    coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_ACL_BUSY &&
 	    !btcoexist->bt_link_info.hid_only) {
-		if (!wifi_busy && btcoexist->bt_link_info.a2dp_only)
-			halbtc8723b1ant_power_save_state(btcoexist,
+		if (btcoexist->bt_link_info.a2dp_only) {
+			if (!wifi_busy) {
+				halbtc8723b1ant_power_save_state(btcoexist,
 							 BTC_PS_WIFI_NATIVE,
 							 0x0, 0x0);
+			} else { /* busy */
+				if (coex_sta->scan_ap_num >=
+				    BT_8723B_1ANT_WIFI_NOISY_THRESH)
+					/* no force LPS, no PS-TDMA,
+					 * use pure TDMA
+					 */
+					halbtc8723b1ant_power_save_state(
+						btcoexist, BTC_PS_WIFI_NATIVE,
+						0x0, 0x0);
+				else
+					halbtc8723b1ant_power_save_state(
+						btcoexist, BTC_PS_LPS_ON, 0x50,
+						0x4);
+			}
+		} else if ((!coex_sta->pan_exist) && (!coex_sta->a2dp_exist) &&
+			   (!coex_sta->hid_exist))
+			halbtc8723b1ant_power_save_state(
+				btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
 		else
 			halbtc8723b1ant_power_save_state(btcoexist,
 							 BTC_PS_LPS_ON,
@@ -1407,36 +2130,44 @@ static void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 	}
 	/* tdma and coex table */
 	if (!wifi_busy) {
-		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
-			halbtc8723b1ant_action_wifi_connected_bt_acl_busy(btcoexist,
-				      BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE);
-		} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY ==
-						coex_dm->bt_status) ||
-			   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
-						coex_dm->bt_status)) {
+		if (coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_ACL_BUSY) {
+			halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
+				btcoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE);
+		} else if (coex_dm->bt_status ==
+				BT_8723B_1ANT_BT_STATUS_SCO_BUSY ||
+			   coex_dm->bt_status ==
+				BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY) {
 			btc8723b1ant_act_bt_sco_hid_only_busy(btcoexist,
 				     BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE);
 		} else {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						false, 8);
+			halbtc8723b1ant_set_ant_path(btcoexist,
+						     BTC_ANT_PATH_PTA,
+						     NORMAL_EXEC, false, false);
 			halbtc8723b1ant_coex_table_with_type(btcoexist,
 							     NORMAL_EXEC, 2);
 		}
 	} else {
-		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
-			halbtc8723b1ant_action_wifi_connected_bt_acl_busy(btcoexist,
-				    BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY);
-		} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY ==
-			   coex_dm->bt_status) ||
-			   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
-			    coex_dm->bt_status)) {
+		if (coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_ACL_BUSY) {
+			halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
+				btcoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY);
+		} else if (coex_dm->bt_status ==
+				BT_8723B_1ANT_BT_STATUS_SCO_BUSY ||
+			   coex_dm->bt_status ==
+				BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY) {
 			btc8723b1ant_act_bt_sco_hid_only_busy(btcoexist,
 				    BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY);
 		} else {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						false, 8);
+						true, 32);
+			halbtc8723b1ant_set_ant_path(btcoexist,
+						     BTC_ANT_PATH_PTA,
+						     NORMAL_EXEC, false, false);
 			halbtc8723b1ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 2);
+							     NORMAL_EXEC, 4);
 		}
 	}
 }
@@ -1445,12 +2176,15 @@ static void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool wifi_connected = false, bt_hs_on = false;
+	bool wifi_connected = false, bt_hs_on = false, wifi_busy = false;
 	bool increase_scan_dev_num = false;
 	bool bt_ctrl_agg_buf_size = false;
+	bool miracast_plus_bt = false;
 	u8 agg_buf_size = 5;
+	u8 iot_peer = BTC_IOT_PEER_UNKNOWN;
 	u32 wifi_link_status = 0;
 	u32 num_of_wifi_link = 0;
+	u32 wifi_bw;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 		 "[BTCoex], RunCoexistMechanism()===>\n");
@@ -1473,54 +2207,99 @@ static void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 		return;
 	}
 
-	if ((BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
-	    (BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-	    (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
-		increase_scan_dev_num = true;
+	if (coex_sta->bt_whck_test) {
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], wifi is under IPS !!!\n");
+		halbtc8723b1ant_action_bt_whck_test(btcoexist);
+		return;
 	}
 
+	if (coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_ACL_BUSY ||
+	    coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_SCO_BUSY ||
+	    coex_dm->bt_status == BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY)
+		increase_scan_dev_num = true;
+
 	btcoexist->btc_set(btcoexist, BTC_SET_BL_INC_SCAN_DEV_NUM,
 			   &increase_scan_dev_num);
-
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
 			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_LINK_STATUS,
 			   &wifi_link_status);
 	num_of_wifi_link = wifi_link_status >> 16;
-	if (num_of_wifi_link >= 2) {
-		halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
+
+	if (num_of_wifi_link >= 2 ||
+	    wifi_link_status & WIFI_P2P_GO_CONNECTED) {
+		if (bt_link_info->bt_link_exist) {
+			halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 1, 1,
+						   0, 1);
+			miracast_plus_bt = true;
+		} else {
+			halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0,
+						   0, 0);
+			miracast_plus_bt = false;
+		}
+		btcoexist->btc_set(btcoexist, BTC_SET_BL_MIRACAST_PLUS_BT,
+				   &miracast_plus_bt);
 		halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
-					   bt_ctrl_agg_buf_size,
-					   agg_buf_size);
-		halbtc8723b1ant_action_wifi_multiport(btcoexist);
+					   bt_ctrl_agg_buf_size, agg_buf_size);
+
+		if ((bt_link_info->a2dp_exist || wifi_busy) &&
+		    (coex_sta->c2h_bt_inquiry_page))
+			halbtc8723b1ant_action_bt_inquiry(btcoexist);
+		else
+			halbtc8723b1ant_action_wifi_multiport(btcoexist);
+
 		return;
 	}
 
-	if (!bt_link_info->sco_exist && !bt_link_info->hid_exist) {
-		halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
+	miracast_plus_bt = false;
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_MIRACAST_PLUS_BT,
+			   &miracast_plus_bt);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (bt_link_info->bt_link_exist && wifi_connected) {
+		halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 1, 1, 0, 1);
+
+		btcoexist->btc_get(btcoexist, BTC_GET_U1_IOT_PEER, &iot_peer);
+
+		if (iot_peer != BTC_IOT_PEER_CISCO &&
+		    iot_peer != BTC_IOT_PEER_BROADCOM) {
+			if (bt_link_info->sco_exist)
+				halbtc8723b1ant_limited_rx(btcoexist,
+							   NORMAL_EXEC, false,
+							   false, 0x5);
+			else
+				halbtc8723b1ant_limited_rx(btcoexist,
+							   NORMAL_EXEC, false,
+							   false, 0x5);
+		} else {
+			if (bt_link_info->sco_exist) {
+				halbtc8723b1ant_limited_rx(btcoexist,
+							   NORMAL_EXEC, true,
+							   false, 0x5);
+			} else {
+				if (wifi_bw == BTC_WIFI_BW_HT40)
+					halbtc8723b1ant_limited_rx(
+						btcoexist, NORMAL_EXEC, false,
+						true, 0x10);
+				else
+					halbtc8723b1ant_limited_rx(
+						btcoexist, NORMAL_EXEC, false,
+						true, 0x8);
+			}
+		}
+
+		halbtc8723b1ant_sw_mechanism(btcoexist, true);
 	} else {
-		if (wifi_connected)
-			halbtc8723b1ant_limited_tx(btcoexist,
-						   NORMAL_EXEC, 1, 1, 1, 1);
-		else
-			halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC,
-						   0, 0, 0, 0);
-	}
+		halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
 
-	if (bt_link_info->sco_exist) {
-		bt_ctrl_agg_buf_size = true;
-		agg_buf_size = 0x3;
-	} else if (bt_link_info->hid_exist) {
-		bt_ctrl_agg_buf_size = true;
-		agg_buf_size = 0x5;
-	} else if (bt_link_info->a2dp_exist || bt_link_info->pan_exist) {
-		bt_ctrl_agg_buf_size = true;
-		agg_buf_size = 0x8;
-	}
-	halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
-				   bt_ctrl_agg_buf_size, agg_buf_size);
+		halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false, false,
+					   0x5);
 
+		halbtc8723b1ant_sw_mechanism(btcoexist, false);
+	}
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
 
 	if (coex_sta->c2h_bt_inquiry_page) {
@@ -1556,91 +2335,141 @@ static void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 }
 
+/* force coex mechanism to reset */
 static void halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
 {
 	/* sw all off */
 	halbtc8723b1ant_sw_mechanism(btcoexist, false);
 
-	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
-	halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+	coex_sta->pop_event_cnt = 0;
 }
 
 static void halbtc8723b1ant_init_hw_config(struct btc_coexist *btcoexist,
-					   bool backup)
+					   bool backup, bool wifi_only)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	u32 u32tmp = 0;
-	u8 u8tmp = 0;
-	u32 cnt_bt_cal_chk = 0;
+	u8 u8tmpa = 0, u8tmpb = 0;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 		 "[BTCoex], 1Ant Init HW Config!!\n");
 
-	if (backup) {/* backup rf 0x1e value */
-		coex_dm->backup_arfr_cnt1 =
-			btcoexist->btc_read_4byte(btcoexist, 0x430);
-		coex_dm->backup_arfr_cnt2 =
-			btcoexist->btc_read_4byte(btcoexist, 0x434);
-		coex_dm->backup_retry_limit =
-			btcoexist->btc_read_2byte(btcoexist, 0x42a);
-		coex_dm->backup_ampdu_max_time =
-			btcoexist->btc_read_1byte(btcoexist, 0x456);
-	}
-
-	/* WiFi goto standby while GNT_BT 0-->1 */
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x780);
-	/* BT goto standby while GNT_BT 1-->0 */
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x2, 0xfffff, 0x500);
-
-	btcoexist->btc_write_1byte(btcoexist, 0x974, 0xff);
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x944, 0x3, 0x3);
-	btcoexist->btc_write_1byte(btcoexist, 0x930, 0x77);
-
-	/* BT calibration check */
-	while (cnt_bt_cal_chk <= 20) {
-		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x49d);
-		cnt_bt_cal_chk++;
-		if (u32tmp & BIT0) {
-			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-				 "[BTCoex], ########### BT calibration(cnt=%d) ###########\n",
-				      cnt_bt_cal_chk);
-			mdelay(50);
-		} else {
-			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-				 "[BTCoex], ********** BT NOT calibration (cnt=%d)**********\n",
-				      cnt_bt_cal_chk);
-			break;
-		}
-	}
+	/* 0xf0[15:12] --> Chip Cut information */
+	coex_sta->cut_version =
+		(btcoexist->btc_read_1byte(btcoexist, 0xf1) & 0xf0) >> 4;
+	/* enable TBTT interrupt */
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x550, 0x8, 0x1);
 
 	/* 0x790[5:0] = 0x5 */
-	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
-	u8tmp &= 0xc0;
-	u8tmp |= 0x5;
-	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
+	btcoexist->btc_write_1byte(btcoexist, 0x790, 0x5);
 
 	/* Enable counter statistics */
-	/*0x76e[3] = 1, WLAN_Act control by PTA */
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
 	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x1);
 	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
 
+	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
+
 	/* Antenna config */
-	halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA, true, false);
+	if (wifi_only)
+		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_WIFI,
+					     FORCE_EXEC, true, false);
+	else
+		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT,
+					     FORCE_EXEC, true, false);
+
 	/* PTA parameter */
 	halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+
+	u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x948);
+	u8tmpa = btcoexist->btc_read_1byte(btcoexist, 0x765);
+	u8tmpb = btcoexist->btc_read_1byte(btcoexist, 0x67);
+
+	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+		 "############# [BTCoex], 0x948=0x%x, 0x765=0x%x, 0x67=0x%x\n",
+		 u32tmp, u8tmpa, u8tmpb);
 }
 
 /**************************************************************
- * extern function start with ex_halbtc8723b1ant_
+ * extern function start with ex_btc8723b1ant_
  **************************************************************/
+void ex_btc8723b1ant_power_on_setting(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	u8 u8tmp = 0x0;
+	u16 u16tmp = 0x0;
+	u32 value;
+
+	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+		 "xxxxxxxxxxxxxxxx Execute 8723b 1-Ant PowerOn Setting xxxxxxxxxxxxxxxx!!\n");
+
+	btcoexist->stop_coex_dm = true;
+
+	btcoexist->btc_write_1byte(btcoexist, 0x67, 0x20);
+
+	/* enable BB, REG_SYS_FUNC_EN such that we can write 0x948 correctly. */
+	u16tmp = btcoexist->btc_read_2byte(btcoexist, 0x2);
+	btcoexist->btc_write_2byte(btcoexist, 0x2, u16tmp | BIT0 | BIT1);
+
+	/* set GRAN_BT = 1 */
+	btcoexist->btc_write_1byte(btcoexist, 0x765, 0x18);
+	/* set WLAN_ACT = 0 */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
+
+	/* S0 or S1 setting and Local register setting(By the setting fw can get
+	 * ant number, S0/S1, ... info)
+	 *
+	 * Local setting bit define
+	 *	BIT0: "0" for no antenna inverse; "1" for antenna inverse
+	 *	BIT1: "0" for internal switch; "1" for external switch
+	 *	BIT2: "0" for one antenna; "1" for two antenna
+	 * NOTE: here default all internal switch and 1-antenna ==> BIT1=0 and
+	 * BIT2 = 0
+	 */
+	if (btcoexist->chip_interface == BTC_INTF_USB) {
+		/* fixed at S0 for USB interface */
+		btcoexist->btc_write_4byte(btcoexist, 0x948, 0x0);
 
-void ex_halbtc8723b1ant_init_hwconfig(struct btc_coexist *btcoexist)
+		u8tmp |= 0x1; /* antenna inverse */
+		btcoexist->btc_write_local_reg_1byte(btcoexist, 0xfe08, u8tmp);
+
+		board_info->btdm_ant_pos = BTC_ANTENNA_AT_AUX_PORT;
+	} else {
+		/* for PCIE and SDIO interface, we check efuse 0xc3[6] */
+		if (board_info->single_ant_path == 0) {
+			/* set to S1 */
+			btcoexist->btc_write_4byte(btcoexist, 0x948, 0x280);
+			board_info->btdm_ant_pos = BTC_ANTENNA_AT_MAIN_PORT;
+			value = 1;
+		} else if (board_info->single_ant_path == 1) {
+			/* set to S0 */
+			btcoexist->btc_write_4byte(btcoexist, 0x948, 0x0);
+			u8tmp |= 0x1; /* antenna inverse */
+			board_info->btdm_ant_pos = BTC_ANTENNA_AT_AUX_PORT;
+			value = 0;
+		}
+
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_ANTPOSREGRISTRY_CTRL,
+				   &value);
+
+		if (btcoexist->chip_interface == BTC_INTF_PCI)
+			btcoexist->btc_write_local_reg_1byte(btcoexist, 0x384,
+							     u8tmp);
+		else if (btcoexist->chip_interface == BTC_INTF_SDIO)
+			btcoexist->btc_write_local_reg_1byte(btcoexist, 0x60,
+							     u8tmp);
+	}
+}
+
+
+void ex_btc8723b1ant_init_hwconfig(struct btc_coexist *btcoexist,
+				   bool wifi_only)
 {
-	halbtc8723b1ant_init_hw_config(btcoexist, true);
+	halbtc8723b1ant_init_hw_config(btcoexist, true, wifi_only);
+	btcoexist->stop_coex_dm = false;
 }
 
-void ex_halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
+void ex_btc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 
@@ -1654,7 +2483,7 @@ void ex_halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
 	halbtc8723b1ant_query_bt_info(btcoexist);
 }
 
-void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
+void ex_btc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
 {
 	struct btc_board_info *board_info = &btcoexist->board_info;
 	struct btc_stack_info *stack_info = &btcoexist->stack_info;
@@ -1687,11 +2516,6 @@ void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
 			 "\r\n ==========================================");
 	}
 
-	if (!board_info->bt_exist) {
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n BT not exists !!!");
-		return;
-	}
-
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d/ %d",
 		 "Ant PG Num/ Ant Mech/ Ant Pos:",
 		 board_info->pg_ant_num, board_info->btdm_ant_num,
@@ -1760,7 +2584,7 @@ void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = [%s/ %d/ %d] ",
 		 "BT [status/ rssi/ retryCnt]",
-		 ((btcoexist->bt_info.bt_disabled) ? ("disabled") :
+		 ((coex_sta->bt_disabled) ? ("disabled") :
 		  ((coex_sta->c2h_bt_inquiry_page) ? ("inquiry/page scan") :
 		   ((BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE ==
 		     coex_dm->bt_status) ?
@@ -1835,6 +2659,9 @@ void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
 			   coex_dm->error_condition);
 	}
 
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d",
+		 "Coex Table Type", coex_sta->coex_table_type);
+
 	/* Hw setting */
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s",
 		 "============[Hw setting]============");
@@ -1926,13 +2753,12 @@ void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d",
 		 "0x774(low-pri rx/tx)", coex_sta->low_priority_rx,
 		 coex_sta->low_priority_tx);
-#if (BT_AUTO_REPORT_ONLY_8723B_1ANT == 1)
-	halbtc8723b1ant_monitor_bt_ctr(btcoexist);
-#endif
+	if (btcoexist->auto_report_1ant)
+		halbtc8723b1ant_monitor_bt_ctr(btcoexist);
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
 }
 
-void ex_halbtc8723b1ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
+void ex_btc8723b1ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 
@@ -1945,7 +2771,7 @@ void ex_halbtc8723b1ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
 		coex_sta->under_ips = true;
 
 		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT,
-					     false, true);
+					     FORCE_EXEC, false, true);
 		/* set PTA control */
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
 		halbtc8723b1ant_coex_table_with_type(btcoexist,
@@ -1955,13 +2781,13 @@ void ex_halbtc8723b1ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
 			 "[BTCoex], IPS LEAVE notify\n");
 		coex_sta->under_ips = false;
 
-		halbtc8723b1ant_init_hw_config(btcoexist, false);
+		halbtc8723b1ant_init_hw_config(btcoexist, false, false);
 		halbtc8723b1ant_init_coex_dm(btcoexist);
 		halbtc8723b1ant_query_bt_info(btcoexist);
 	}
 }
 
-void ex_halbtc8723b1ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
+void ex_btc8723b1ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 
@@ -1979,17 +2805,45 @@ void ex_halbtc8723b1ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
 	}
 }
 
-void ex_halbtc8723b1ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
+void ex_btc8723b1ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	bool wifi_connected = false, bt_hs_on = false;
+	u8 u8tmpa, u8tmpb;
+	u32 u32tmp;
 	u32 wifi_link_status = 0;
 	u32 num_of_wifi_link = 0;
 	bool bt_ctrl_agg_buf_size = false;
 	u8 agg_buf_size = 5;
 
-	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
-	    btcoexist->bt_info.bt_disabled)
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm)
+		return;
+
+	if (type == BTC_SCAN_START) {
+		coex_sta->wifi_is_high_pri_task = true;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], SCAN START notify\n");
+		/* Force antenna setup for no scan result issue */
+		halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
+		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA,
+					     FORCE_EXEC, false, false);
+		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x948);
+		u8tmpa = btcoexist->btc_read_1byte(btcoexist, 0x765);
+		u8tmpb = btcoexist->btc_read_1byte(btcoexist, 0x67);
+
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], 0x948=0x%x, 0x765=0x%x, 0x67=0x%x\n",
+			 u32tmp, u8tmpa, u8tmpb);
+	} else {
+		coex_sta->wifi_is_high_pri_task = false;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], SCAN FINISH notify\n");
+
+		btcoexist->btc_get(btcoexist, BTC_GET_U1_AP_NUM,
+				   &coex_sta->scan_ap_num);
+	}
+
+	if (coex_sta->bt_disabled)
 		return;
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
@@ -2037,19 +2891,38 @@ void ex_halbtc8723b1ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
 	}
 }
 
-void ex_halbtc8723b1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
+void ex_btc8723b1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	bool wifi_connected = false, bt_hs_on = false;
 	u32 wifi_link_status = 0;
 	u32 num_of_wifi_link = 0;
-	bool bt_ctrl_agg_buf_size = false;
+	bool bt_ctrl_agg_buf_size = false, under_4way = false;
 	u8 agg_buf_size = 5;
 
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+			   &under_4way);
+
 	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
-	    btcoexist->bt_info.bt_disabled)
+	    coex_sta->bt_disabled)
 		return;
 
+	if (type == BTC_ASSOCIATE_START) {
+		coex_sta->wifi_is_high_pri_task = true;
+
+		/* Force antenna setup for no scan result issue */
+		halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
+		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA,
+					     FORCE_EXEC, false, false);
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], CONNECT START notify\n");
+		coex_dm->arp_cnt = 0;
+	} else {
+		coex_sta->wifi_is_high_pri_task = false;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], CONNECT FINISH notify\n");
+	}
+
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_LINK_STATUS,
 			   &wifi_link_status);
 	num_of_wifi_link = wifi_link_status>>16;
@@ -2088,33 +2961,68 @@ void ex_halbtc8723b1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 	}
 }
 
-void ex_halbtc8723b1ant_media_status_notify(struct btc_coexist *btcoexist,
-					    u8 type)
+void ex_btc8723b1ant_media_status_notify(struct btc_coexist *btcoexist,
+					 u8 type)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	u8 h2c_parameter[3] = {0};
 	u32 wifi_bw;
-	u8 wifiCentralChnl;
+	u8 wifi_central_chnl;
+	bool wifi_under_b_mode = false;
 
 	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
-	    btcoexist->bt_info.bt_disabled)
+	    coex_sta->bt_disabled)
 		return;
 
-	if (BTC_MEDIA_CONNECT == type)
+	if (type == BTC_MEDIA_CONNECT) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], MEDIA connect notify\n");
-	else
+		/* Force antenna setup for no scan result issue */
+		halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
+		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA,
+					     FORCE_EXEC, false, false);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_B_MODE,
+				   &wifi_under_b_mode);
+
+		/* Set CCK Tx/Rx high Pri except 11b mode */
+		if (wifi_under_b_mode) {
+			btcoexist->btc_write_1byte(btcoexist, 0x6cd,
+						   0x00); /* CCK Tx */
+			btcoexist->btc_write_1byte(btcoexist, 0x6cf,
+						   0x00); /* CCK Rx */
+		} else {
+			btcoexist->btc_write_1byte(btcoexist, 0x6cd,
+						   0x00); /* CCK Tx */
+			btcoexist->btc_write_1byte(btcoexist, 0x6cf,
+						   0x10); /* CCK Rx */
+		}
+
+		coex_dm->backup_arfr_cnt1 =
+			btcoexist->btc_read_4byte(btcoexist, 0x430);
+		coex_dm->backup_arfr_cnt2 =
+			btcoexist->btc_read_4byte(btcoexist, 0x434);
+		coex_dm->backup_retry_limit =
+			btcoexist->btc_read_2byte(btcoexist, 0x42a);
+		coex_dm->backup_ampdu_max_time =
+			btcoexist->btc_read_1byte(btcoexist, 0x456);
+	} else {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], MEDIA disconnect notify\n");
+		coex_dm->arp_cnt = 0;
+
+		btcoexist->btc_write_1byte(btcoexist, 0x6cd, 0x0); /* CCK Tx */
+		btcoexist->btc_write_1byte(btcoexist, 0x6cf, 0x0); /* CCK Rx */
+
+		coex_sta->cck_ever_lock = false;
+	}
 
 	/* only 2.4G we need to inform bt the chnl mask */
 	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL,
-			   &wifiCentralChnl);
+			   &wifi_central_chnl);
 
-	if ((BTC_MEDIA_CONNECT == type) &&
-	    (wifiCentralChnl <= 14)) {
+	if (type == BTC_MEDIA_CONNECT && wifi_central_chnl <= 14) {
 		h2c_parameter[0] = 0x0;
-		h2c_parameter[1] = wifiCentralChnl;
+		h2c_parameter[1] = wifi_central_chnl;
 		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 		if (BTC_WIFI_BW_HT40 == wifi_bw)
 			h2c_parameter[2] = 0x30;
@@ -2134,20 +3042,53 @@ void ex_halbtc8723b1ant_media_status_notify(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
 }
 
-void ex_halbtc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type)
+void ex_btc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
+					   u8 type)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	bool bt_hs_on = false;
 	u32 wifi_link_status = 0;
 	u32 num_of_wifi_link = 0;
-	bool bt_ctrl_agg_buf_size = false;
+	bool bt_ctrl_agg_buf_size = false, under_4way = false;
 	u8 agg_buf_size = 5;
 
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+			   &under_4way);
+
 	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
-	    btcoexist->bt_info.bt_disabled)
+	    coex_sta->bt_disabled)
 		return;
 
+	if (type == BTC_PACKET_DHCP || type == BTC_PACKET_EAPOL ||
+	    type == BTC_PACKET_ARP) {
+		if (type == BTC_PACKET_ARP) {
+			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+				 "[BTCoex], special Packet ARP notify\n");
+
+			coex_dm->arp_cnt++;
+			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+				 "[BTCoex], ARP Packet Count = %d\n",
+				 coex_dm->arp_cnt);
+
+			if ((coex_dm->arp_cnt >= 10) && (!under_4way))
+				/* if APR PKT > 10 after connect, do not go to
+				 * ActionWifiConnectedSpecificPacket(btcoexist)
+				 */
+				coex_sta->wifi_is_high_pri_task = false;
+			else
+				coex_sta->wifi_is_high_pri_task = true;
+		} else {
+			coex_sta->wifi_is_high_pri_task = true;
+			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+				 "[BTCoex], special Packet DHCP or EAPOL notify\n");
+		}
+	} else {
+		coex_sta->wifi_is_high_pri_task = false;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], special Packet [Type = %d] notify\n",
+			 type);
+	}
+
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_LINK_STATUS,
 		&wifi_link_status);
 	num_of_wifi_link = wifi_link_status >> 16;
@@ -2178,8 +3119,8 @@ void ex_halbtc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
 	}
 }
 
-void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmp_buf, u8 length)
+void ex_btc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
+				    u8 *tmp_buf, u8 length)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	u8 bt_info = 0;
@@ -2209,16 +3150,58 @@ void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 				 "0x%02x, ", tmp_buf[i]);
 	}
 
-	if (BT_INFO_SRC_8723B_1ANT_WIFI_FW != rsp_source) {
-		coex_sta->bt_retry_cnt =	/* [3:0] */
+	/* if 0xff, it means BT is under WHCK test */
+	if (bt_info == 0xff)
+		coex_sta->bt_whck_test = true;
+	else
+		coex_sta->bt_whck_test = false;
+
+	if (rsp_source != BT_INFO_SRC_8723B_1ANT_WIFI_FW) {
+		coex_sta->bt_retry_cnt = /* [3:0] */
 			coex_sta->bt_info_c2h[rsp_source][2] & 0xf;
 
+		if (coex_sta->bt_retry_cnt >= 1)
+			coex_sta->pop_event_cnt++;
+
+		if (coex_sta->bt_info_c2h[rsp_source][2] & 0x20)
+			coex_sta->c2h_bt_remote_name_req = true;
+		else
+			coex_sta->c2h_bt_remote_name_req = false;
+
 		coex_sta->bt_rssi =
-			coex_sta->bt_info_c2h[rsp_source][3] * 2 + 10;
+			coex_sta->bt_info_c2h[rsp_source][3] * 2 - 90;
 
 		coex_sta->bt_info_ext =
 			coex_sta->bt_info_c2h[rsp_source][4];
 
+		if (coex_sta->bt_info_c2h[rsp_source][1] == 0x49) {
+			coex_sta->a2dp_bit_pool =
+				coex_sta->bt_info_c2h[rsp_source][6];
+		} else {
+			coex_sta->a2dp_bit_pool = 0;
+		}
+
+		coex_sta->bt_tx_rx_mask =
+			(coex_sta->bt_info_c2h[rsp_source][2] & 0x40);
+		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TX_RX_MASK,
+				   &coex_sta->bt_tx_rx_mask);
+
+		if (!coex_sta->bt_tx_rx_mask) {
+			/* BT into is responded by BT FW and BT RF REG
+			 * 0x3C != 0x15 => Need to switch BT TRx Mask
+			 */
+			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+				 "[BTCoex], Switch BT TRx Mask since BT RF REG 0x3C != 0x15\n");
+			btcoexist->btc_set_bt_reg(btcoexist, BTC_BT_REG_RF,
+						  0x3c, 0x15);
+
+			/* BT TRx Mask lock 0x2c[0], 0x30[0] = 0 */
+			btcoexist->btc_set_bt_reg(btcoexist, BTC_BT_REG_RF,
+						  0x2c, 0x7c44);
+			btcoexist->btc_set_bt_reg(btcoexist, BTC_BT_REG_RF,
+						  0x30, 0x7c44);
+		}
+
 		/* Here we need to resend some wifi info to BT
 		 * because bt is reset and loss of the info.
 		 */
@@ -2228,11 +3211,11 @@ void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
 					   &wifi_connected);
 			if (wifi_connected)
-				ex_halbtc8723b1ant_media_status_notify(btcoexist,
-							     BTC_MEDIA_CONNECT);
+				ex_btc8723b1ant_media_status_notify(btcoexist,
+						BTC_MEDIA_CONNECT);
 			else
-				ex_halbtc8723b1ant_media_status_notify(btcoexist,
-							  BTC_MEDIA_DISCONNECT);
+				ex_btc8723b1ant_media_status_notify(btcoexist,
+						BTC_MEDIA_DISCONNECT);
 		}
 
 		if (coex_sta->bt_info_ext & BIT3) {
@@ -2247,14 +3230,15 @@ void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 		} else {
 			/* BT already NOT ignore Wlan active, do nothing here.*/
 		}
-#if (BT_AUTO_REPORT_ONLY_8723B_1ANT == 0)
-		if (coex_sta->bt_info_ext & BIT4) {
-			/* BT auto report already enabled, do nothing */
-		} else {
-			halbtc8723b1ant_bt_auto_report(btcoexist, FORCE_EXEC,
-						       true);
+		if (!btcoexist->auto_report_1ant) {
+			if (coex_sta->bt_info_ext & BIT4) {
+				/* BT auto report already enabled, do nothing */
+			} else {
+				halbtc8723b1ant_bt_auto_report(btcoexist,
+							       FORCE_EXEC,
+							       true);
+			}
 		}
-#endif
 	}
 
 	/* check BIT2 first ==> check if bt is under inquiry or page scan */
@@ -2263,6 +3247,8 @@ void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 	else
 		coex_sta->c2h_bt_inquiry_page = false;
 
+	coex_sta->num_of_profile = 0;
+
 	/* set link exist status */
 	if (!(bt_info & BT_INFO_8723B_1ANT_B_CONNECTION)) {
 		coex_sta->bt_link_exist = false;
@@ -2270,30 +3256,77 @@ void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 		coex_sta->a2dp_exist = false;
 		coex_sta->hid_exist = false;
 		coex_sta->sco_exist = false;
+
+		coex_sta->bt_hi_pri_link_exist = false;
 	} else {
 		/* connection exists */
 		coex_sta->bt_link_exist = true;
-		if (bt_info & BT_INFO_8723B_1ANT_B_FTP)
+		if (bt_info & BT_INFO_8723B_1ANT_B_FTP) {
 			coex_sta->pan_exist = true;
-		else
+			coex_sta->num_of_profile++;
+		} else {
 			coex_sta->pan_exist = false;
-		if (bt_info & BT_INFO_8723B_1ANT_B_A2DP)
+		}
+		if (bt_info & BT_INFO_8723B_1ANT_B_A2DP) {
 			coex_sta->a2dp_exist = true;
-		else
+			coex_sta->num_of_profile++;
+		} else {
 			coex_sta->a2dp_exist = false;
-		if (bt_info & BT_INFO_8723B_1ANT_B_HID)
+		}
+		if (bt_info & BT_INFO_8723B_1ANT_B_HID) {
 			coex_sta->hid_exist = true;
-		else
+			coex_sta->num_of_profile++;
+		} else {
 			coex_sta->hid_exist = false;
-		if (bt_info & BT_INFO_8723B_1ANT_B_SCO_ESCO)
+		}
+		if (bt_info & BT_INFO_8723B_1ANT_B_SCO_ESCO) {
 			coex_sta->sco_exist = true;
-		else
+			coex_sta->num_of_profile++;
+		} else {
 			coex_sta->sco_exist = false;
+		}
+
+		if ((!coex_sta->hid_exist) &&
+		    (!coex_sta->c2h_bt_inquiry_page) &&
+		    (!coex_sta->sco_exist)) {
+			if (coex_sta->high_priority_tx +
+				    coex_sta->high_priority_rx >=
+			    160) {
+				coex_sta->hid_exist = true;
+				coex_sta->wrong_profile_notification++;
+				coex_sta->num_of_profile++;
+				bt_info = bt_info | 0x28;
+			}
+		}
+
+		/* Add Hi-Pri Tx/Rx counter to avoid false detection */
+		if (((coex_sta->hid_exist) || (coex_sta->sco_exist)) &&
+		    (coex_sta->high_priority_tx + coex_sta->high_priority_rx >=
+		     160) &&
+		    (!coex_sta->c2h_bt_inquiry_page))
+			coex_sta->bt_hi_pri_link_exist = true;
+
+		if ((bt_info & BT_INFO_8723B_1ANT_B_ACL_BUSY) &&
+		    (coex_sta->num_of_profile == 0)) {
+			if (coex_sta->low_priority_tx +
+				    coex_sta->low_priority_rx >=
+			    160) {
+				coex_sta->pan_exist = true;
+				coex_sta->num_of_profile++;
+				coex_sta->wrong_profile_notification++;
+				bt_info = bt_info | 0x88;
+			}
+		}
 	}
 
 	halbtc8723b1ant_update_bt_link_info(btcoexist);
 
-	if (!(bt_info&BT_INFO_8723B_1ANT_B_CONNECTION)) {
+	/* mask profile bit for connect-ilde identification
+	 * ( for CSR case: A2DP idle --> 0x41)
+	 */
+	bt_info = bt_info & 0x1f;
+
+	if (!(bt_info & BT_INFO_8723B_1ANT_B_CONNECTION)) {
 		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE;
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], BtInfoNotify(), BT Non-Connected idle!\n");
@@ -2315,8 +3348,7 @@ void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], BtInfoNotify(), BT ACL busy!!!\n");
 	} else {
-		coex_dm->bt_status =
-			BT_8723B_1ANT_BT_STATUS_MAX;
+		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_MAX;
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], BtInfoNotify(), BT Non-Defined state!!\n");
 	}
@@ -2332,7 +3364,44 @@ void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 	halbtc8723b1ant_run_coexist_mechanism(btcoexist);
 }
 
-void ex_halbtc8723b1ant_halt_notify(struct btc_coexist *btcoexist)
+void ex_btc8723b1ant_rf_status_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	u32 u32tmp;
+	u8 u8tmpa, u8tmpb, u8tmpc;
+
+	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+		 "[BTCoex], RF Status notify\n");
+
+	if (type == BTC_RF_ON) {
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], RF is turned ON!!\n");
+		btcoexist->stop_coex_dm = false;
+	} else if (type == BTC_RF_OFF) {
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], RF is turned OFF!!\n");
+
+		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+						 0x0, 0x0);
+		halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
+		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT,
+					     FORCE_EXEC, false, true);
+
+		halbtc8723b1ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+		btcoexist->stop_coex_dm = true;
+
+		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x948);
+		u8tmpa = btcoexist->btc_read_1byte(btcoexist, 0x765);
+		u8tmpb = btcoexist->btc_read_1byte(btcoexist, 0x67);
+		u8tmpc = btcoexist->btc_read_1byte(btcoexist, 0x76e);
+
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "############# [BTCoex], 0x948=0x%x, 0x765=0x%x, 0x67=0x%x, 0x76e=0x%x\n",
+			 u32tmp, u8tmpa, u8tmpb, u8tmpc);
+	}
+}
+
+void ex_btc8723b1ant_halt_notify(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 
@@ -2340,7 +3409,8 @@ void ex_halbtc8723b1ant_halt_notify(struct btc_coexist *btcoexist)
 
 	btcoexist->stop_coex_dm = true;
 
-	halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT, false, true);
+	halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT, FORCE_EXEC,
+				     false, true);
 
 	halbtc8723b1ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
 
@@ -2348,10 +3418,12 @@ void ex_halbtc8723b1ant_halt_notify(struct btc_coexist *btcoexist)
 					 0x0, 0x0);
 	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
 
-	ex_halbtc8723b1ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+	ex_btc8723b1ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+
+	btcoexist->stop_coex_dm = true;
 }
 
-void ex_halbtc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
+void ex_btc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 
@@ -2360,81 +3432,66 @@ void ex_halbtc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
 	if (BTC_WIFI_PNP_SLEEP == pnp_state) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], Pnp notify to SLEEP\n");
-		btcoexist->stop_coex_dm = true;
-		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT, false,
-					     true);
+		halbtc8723b1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT,
+					     FORCE_EXEC, false, true);
 		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
 						 0x0, 0x0);
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
 		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+
+		/* Driver do not leave IPS/LPS when driver is going to sleep, so
+		 * BTCoexistence think wifi is still under IPS/LPS
+		 *
+		 * BT should clear UnderIPS/UnderLPS state to avoid mismatch
+		 * state after wakeup.
+		 */
+		coex_sta->under_ips = false;
+		coex_sta->under_lps = false;
+		btcoexist->stop_coex_dm = true;
 	} else if (BTC_WIFI_PNP_WAKE_UP == pnp_state) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], Pnp notify to WAKE UP\n");
 		btcoexist->stop_coex_dm = false;
-		halbtc8723b1ant_init_hw_config(btcoexist, false);
+		halbtc8723b1ant_init_hw_config(btcoexist, false, false);
 		halbtc8723b1ant_init_coex_dm(btcoexist);
 		halbtc8723b1ant_query_bt_info(btcoexist);
 	}
 }
 
-void ex_halbtc8723b1ant_coex_dm_reset(struct btc_coexist *btcoexist)
+void ex_btc8723b1ant_coex_dm_reset(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 		 "[BTCoex], *****************Coex DM Reset****************\n");
 
-	halbtc8723b1ant_init_hw_config(btcoexist, false);
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x2, 0xfffff, 0x0);
+	halbtc8723b1ant_init_hw_config(btcoexist, false, false);
 	halbtc8723b1ant_init_coex_dm(btcoexist);
 }
 
-void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist)
+void ex_btc8723b1ant_periodical(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-	static u8 dis_ver_info_cnt;
-	u32 fw_ver = 0, bt_patch_ver = 0;
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 		 "[BTCoex], ==========================Periodical===========================\n");
 
-	if (dis_ver_info_cnt <= 5) {
-		dis_ver_info_cnt += 1;
-		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-			 "[BTCoex], ****************************************************************\n");
-		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-			 "[BTCoex], Ant PG Num/ Ant Mech/ Ant Pos = %d/ %d/ %d\n",
-			 board_info->pg_ant_num, board_info->btdm_ant_num,
-			 board_info->btdm_ant_pos);
-		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-			 "[BTCoex], BT stack/ hci ext ver = %s / %d\n",
-			 stack_info->profile_notified ? "Yes" : "No",
-			 stack_info->hci_version);
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
-				   &bt_patch_ver);
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-			 "[BTCoex], CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n",
-			 glcoex_ver_date_8723b_1ant,
-			 glcoex_ver_8723b_1ant, fw_ver,
-			 bt_patch_ver, bt_patch_ver);
-		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-			 "[BTCoex], ****************************************************************\n");
-	}
+	if (!btcoexist->auto_report_1ant) {
+		halbtc8723b1ant_query_bt_info(btcoexist);
+		halbtc8723b1ant_monitor_bt_enable_disable(btcoexist);
+	} else {
+		halbtc8723b1ant_monitor_bt_ctr(btcoexist);
+		halbtc8723b1ant_monitor_wifi_ctr(btcoexist);
 
-#if (BT_AUTO_REPORT_ONLY_8723B_1ANT == 0)
-	halbtc8723b1ant_query_bt_info(btcoexist);
-	halbtc8723b1ant_monitor_bt_ctr(btcoexist);
-	halbtc8723b1ant_monitor_bt_enable_disable(btcoexist);
-#else
-	if (btc8723b1ant_is_wifi_status_changed(btcoexist) ||
-	    coex_dm->auto_tdma_adjust) {
-		halbtc8723b1ant_run_coexist_mechanism(btcoexist);
-	}
+		if ((coex_sta->high_priority_tx + coex_sta->high_priority_rx < 50) &&
+		    bt_link_info->hid_exist)
+			bt_link_info->hid_exist = false;
 
-	coex_sta->special_pkt_period_cnt++;
-#endif
+		if (btc8723b1ant_is_wifi_status_changed(btcoexist) ||
+		    coex_dm->auto_tdma_adjust) {
+			halbtc8723b1ant_run_coexist_mechanism(btcoexist);
+		}
+		coex_sta->special_pkt_period_cnt++;
+	}
 }
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.h b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.h
index 75f8094b7a34..8d4fde235e11 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.h
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.h
@@ -25,8 +25,6 @@
 /**********************************************************************
  * The following is for 8723B 1ANT BT Co-exist definition
  **********************************************************************/
-#define	BT_AUTO_REPORT_ONLY_8723B_1ANT			1
-
 #define	BT_INFO_8723B_1ANT_B_FTP			BIT7
 #define	BT_INFO_8723B_1ANT_B_A2DP			BIT6
 #define	BT_INFO_8723B_1ANT_B_HID			BIT5
@@ -41,6 +39,8 @@
 
 #define	BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT		2
 
+#define BT_8723B_1ANT_WIFI_NOISY_THRESH			50
+
 enum _BT_INFO_SRC_8723B_1ANT {
 	BT_INFO_SRC_8723B_1ANT_WIFI_FW			= 0x0,
 	BT_INFO_SRC_8723B_1ANT_BT_RSP			= 0x1,
@@ -84,13 +84,16 @@ enum _BT_8723B_1ANT_COEX_ALGO {
 };
 
 struct coex_dm_8723b_1ant {
+	/* hw setting */
+	u8 pre_ant_pos_type;
+	u8 cur_ant_pos_type;
 	/* fw mechanism */
 	bool cur_ignore_wlan_act;
 	bool pre_ignore_wlan_act;
 	u8 pre_ps_tdma;
 	u8 cur_ps_tdma;
 	u8 ps_tdma_para[5];
-	u8 tdma_adj_type;
+	u8 ps_tdma_du_adj_type;
 	bool auto_tdma_adjust;
 	bool pre_ps_tdma_on;
 	bool cur_ps_tdma_on;
@@ -133,16 +136,21 @@ struct coex_dm_8723b_1ant {
 	u8 cur_retry_limit_type;
 	u8 pre_ampdu_time_type;
 	u8 cur_ampdu_time_type;
+	u32	arp_cnt;
 
 	u8 error_condition;
 };
 
 struct coex_sta_8723b_1ant {
+	bool bt_disabled;
 	bool bt_link_exist;
 	bool sco_exist;
 	bool a2dp_exist;
 	bool hid_exist;
 	bool pan_exist;
+	bool bt_hi_pri_link_exist;
+	u8 num_of_profile;
+	bool bt_abnormal_scan;
 
 	bool under_lps;
 	bool under_ips;
@@ -154,31 +162,63 @@ struct coex_sta_8723b_1ant {
 	u8 bt_rssi;
 	u8 pre_bt_rssi_state;
 	u8 pre_wifi_rssi_state[4];
+	bool bt_tx_rx_mask;
 	bool c2h_bt_info_req_sent;
 	u8 bt_info_c2h[BT_INFO_SRC_8723B_1ANT_MAX][10];
 	u32 bt_info_c2h_cnt[BT_INFO_SRC_8723B_1ANT_MAX];
+	bool bt_whck_test;
 	bool c2h_bt_inquiry_page;
+	bool c2h_bt_remote_name_req;
+	bool wifi_is_high_pri_task;
 	u8 bt_retry_cnt;
 	u8 bt_info_ext;
+	u8 scan_ap_num;
+	bool cck_ever_lock;
+	u8 coex_table_type;
+	bool force_lps_on;
+	u32 pop_event_cnt;
+
+	u32 crc_ok_cck;
+	u32 crc_ok_11g;
+	u32 crc_ok_11n;
+	u32 crc_ok_11n_agg;
+
+	u32 crc_err_cck;
+	u32 crc_err_11g;
+	u32 crc_err_11n;
+	u32 crc_err_11n_agg;
+
+	bool cck_lock;
+	bool pre_ccklock;
+
+	u32 wrong_profile_notification;
+
+	u8 a2dp_bit_pool;
+	u8 cut_version;
 };
 
 /*************************************************************************
  * The following is interface which will notify coex module.
  *************************************************************************/
-void ex_halbtc8723b1ant_init_hwconfig(struct btc_coexist *btcoexist);
-void ex_halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist);
-void ex_halbtc8723b1ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b1ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b1ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b1ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b1ant_media_status_notify(struct btc_coexist *btcoexist,
-					    u8 type);
-void ex_halbtc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type);
-void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmpbuf, u8 length);
-void ex_halbtc8723b1ant_halt_notify(struct btc_coexist *btcoexist);
-void ex_halbtc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnpstate);
-void ex_halbtc8723b1ant_coex_dm_reset(struct btc_coexist *btcoexist);
-void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist);
-void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist);
+void ex_btc8723b1ant_power_on_setting(struct btc_coexist *btcoexist);
+void ex_btc8723b1ant_init_hwconfig(struct btc_coexist *btcoexist,
+				   bool wifi_only);
+void ex_btc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist);
+void ex_btc8723b1ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8723b1ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8723b1ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8723b1ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8723b1ant_media_status_notify(struct btc_coexist *btcoexist,
+					 u8 type);
+void ex_btc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
+					   u8 type);
+void ex_btc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
+				    u8 *tmpbuf, u8 length);
+void ex_btc8723b1ant_rf_status_notify(struct btc_coexist *btcoexist,
+				      u8 type);
+void ex_btc8723b1ant_halt_notify(struct btc_coexist *btcoexist);
+void ex_btc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnpstate);
+void ex_btc8723b1ant_coex_dm_reset(struct btc_coexist *btcoexist);
+void ex_btc8723b1ant_periodical(struct btc_coexist *btcoexist);
+void ex_btc8723b1ant_display_coex_info(struct btc_coexist *btcoexist);
+void ex_btc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state);
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b2ant.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b2ant.c
index 2f3946be4ce2..31965f0ef69d 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b2ant.c
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b2ant.c
@@ -707,6 +707,36 @@ static void btc8723b2ant_dec_bt_pwr(struct btc_coexist *btcoexist,
 	coex_dm->pre_dec_bt_pwr_lvl = coex_dm->cur_dec_bt_pwr_lvl;
 }
 
+static
+void halbtc8723b2ant_set_bt_auto_report(struct btc_coexist *btcoexist,
+					bool enable_auto_report)
+{
+	u8 h2c_parameter[1] = {0};
+
+	h2c_parameter[0] = 0;
+
+	if (enable_auto_report)
+		h2c_parameter[0] |= BIT(0);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
+}
+
+static
+void btc8723b2ant_bt_auto_report(struct btc_coexist *btcoexist,
+				 bool force_exec, bool enable_auto_report)
+{
+	coex_dm->cur_bt_auto_report = enable_auto_report;
+
+	if (!force_exec) {
+		if (coex_dm->pre_bt_auto_report == coex_dm->cur_bt_auto_report)
+			return;
+	}
+	halbtc8723b2ant_set_bt_auto_report(btcoexist,
+					   coex_dm->cur_bt_auto_report);
+
+	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
+}
+
 static void btc8723b2ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
 					  bool force_exec, u8 fw_dac_swing_lvl)
 {
@@ -3666,6 +3696,7 @@ void ex_btc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist)
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
 	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x3);
 	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
+	btcoexist->auto_report_2ant = true;
 }
 
 void ex_btc8723b2ant_power_on_setting(struct btc_coexist *btcoexist)
@@ -3966,9 +3997,8 @@ void ex_btc8723b2ant_display_coex_info(struct btc_coexist *btcoexist)
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d",
 		 "0x774(low-pri rx/tx)", coex_sta->low_priority_rx,
 		 coex_sta->low_priority_tx);
-#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 1)
-	btc8723b2ant_monitor_bt_ctr(btcoexist);
-#endif
+	if (btcoexist->auto_report_2ant)
+		btc8723b2ant_monitor_bt_ctr(btcoexist);
 	btcoexist->btc_disp_dbg_msg(btcoexist,
 	BTC_DBG_DISP_COEX_STATISTICS);
 }
@@ -4190,14 +4220,11 @@ void ex_btc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
 		} else {
 			/* BT already NOT ignore Wlan active, do nothing here.*/
 		}
-#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
-		if ((coex_sta->bt_info_ext & BIT4)) {
-			/* BT auto report already enabled, do nothing*/
-		} else {
-			btc8723b2ant_bt_auto_report(btcoexist, FORCE_EXEC,
-						    true);
+		if (!btcoexist->auto_report_2ant) {
+			if (!(coex_sta->bt_info_ext & BIT4))
+				btc8723b2ant_bt_auto_report(btcoexist,
+							    FORCE_EXEC, true);
 		}
-#endif
 	}
 
 	/* check BIT2 first ==> check if bt is under inquiry or page scan */
@@ -4347,21 +4374,22 @@ void ex_btc8723b2ant_periodical(struct btc_coexist *btcoexist)
 		}
 	}
 
-#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
-	btc8723b2ant_query_bt_info(btcoexist);
-#else
-	btc8723b2ant_monitor_bt_ctr(btcoexist);
-	btc8723b2ant_monitor_wifi_ctr(btcoexist);
+	if (!btcoexist->auto_report_2ant) {
+		btc8723b2ant_query_bt_info(btcoexist);
+	} else {
+		btc8723b2ant_monitor_bt_ctr(btcoexist);
+		btc8723b2ant_monitor_wifi_ctr(btcoexist);
 
-	/* for some BT speakers that High-Priority pkts appear before
-	 * playing, this will cause HID exist
-	 */
-	if ((coex_sta->high_priority_tx + coex_sta->high_priority_rx < 50) &&
-	    (bt_link_info->hid_exist))
-		bt_link_info->hid_exist = false;
-
-	if (btc8723b2ant_is_wifi_status_changed(btcoexist) ||
-	    coex_dm->auto_tdma_adjust)
-		btc8723b2ant_run_coexist_mechanism(btcoexist);
-#endif
+		/* for some BT speakers that High-Priority pkts appear before
+		 * playing, this will cause HID exist
+		 */
+		if ((coex_sta->high_priority_tx +
+		    coex_sta->high_priority_rx < 50) &&
+		    (bt_link_info->hid_exist))
+			bt_link_info->hid_exist = false;
+
+		if (btc8723b2ant_is_wifi_status_changed(btcoexist) ||
+		    coex_dm->auto_tdma_adjust)
+			btc8723b2ant_run_coexist_mechanism(btcoexist);
+	}
 }
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b2ant.h b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b2ant.h
index 18a35c7faba9..bc1e3042e271 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b2ant.h
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b2ant.h
@@ -28,8 +28,6 @@
 /************************************************************************
  * The following is for 8723B 2Ant BT Co-exist definition
  ************************************************************************/
-#define	BT_AUTO_REPORT_ONLY_8723B_2ANT			1
-
 #define	BT_INFO_8723B_2ANT_B_FTP			BIT7
 #define	BT_INFO_8723B_2ANT_B_A2DP			BIT6
 #define	BT_INFO_8723B_2ANT_B_HID			BIT5
@@ -198,5 +196,8 @@ void ex_btc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
 void ex_btc8723b2ant_halt_notify(struct btc_coexist *btcoexist);
 void ex_btc8723b2ant_periodical(struct btc_coexist *btcoexist);
 void ex_btc8723b2ant_display_coex_info(struct btc_coexist *btcoexist);
+void ex_btc8723b2ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state);
+void ex_btc8723b2ant_pre_load_firmware(struct btc_coexist *btcoexist);
+void ex_btc8723b2ant_power_on_setting(struct btc_coexist *btcoexist);
 
 #endif
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a1ant.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a1ant.c
index 5e9f3b0f7a25..4efac5fe9982 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a1ant.c
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a1ant.c
@@ -1107,8 +1107,8 @@ static void btc8821a1ant_ps_tdma(struct btc_coexist *btcoexist,
 						    0x3, 0x11, 0x10);
 			break;
 		case 6:
-			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xa,
-						    0x3, 0x0, 0x0);
+			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x20,
+						    0x3, 0x11, 0x13);
 			break;
 		case 7:
 			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xc,
@@ -1128,8 +1128,8 @@ static void btc8821a1ant_ps_tdma(struct btc_coexist *btcoexist,
 						    0xa, 0x0, 0x40);
 			break;
 		case 11:
-			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x14,
-						    0x03, 0x10, 0x10);
+			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x15,
+						    0x03, 0x10, 0x50);
 			rssi_adjust_val = 20;
 			break;
 		case 12:
@@ -1137,8 +1137,8 @@ static void btc8821a1ant_ps_tdma(struct btc_coexist *btcoexist,
 						    0x0a, 0x0, 0x50);
 			break;
 		case 13:
-			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x18,
-						    0x18, 0x0, 0x10);
+			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x12,
+						    0x12, 0x0, 0x50);
 			break;
 		case 14:
 			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x1e,
@@ -1163,8 +1163,8 @@ static void btc8821a1ant_ps_tdma(struct btc_coexist *btcoexist,
 						    0x03, 0x11, 0x10);
 			break;
 		case 21:
-			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x15,
-						    0x03, 0x11, 0x10);
+			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x25,
+						    0x03, 0x11, 0x11);
 			break;
 		case 22:
 			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x25,
@@ -1204,16 +1204,16 @@ static void btc8821a1ant_ps_tdma(struct btc_coexist *btcoexist,
 						    0x1a, 0x1, 0x10);
 			break;
 		case 30:
-			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x14,
-						    0x3, 0x10, 0x50);
+			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x30,
+						    0x3, 0x10, 0x10);
 			break;
 		case 31:
 			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0xd3, 0x1a,
 						    0x1a, 0, 0x58);
 			break;
 		case 32:
-			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x61, 0xa,
-						    0x3, 0x10, 0x0);
+			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x35,
+						    0x3, 0x11, 0x11);
 			break;
 		case 33:
 			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0xa3, 0x25,
@@ -1231,6 +1231,28 @@ static void btc8821a1ant_ps_tdma(struct btc_coexist *btcoexist,
 			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0xd3, 0x12,
 						    0x3, 0x14, 0x50);
 			break;
+		case 40:
+			/* SoftAP only with no sta associated, BT disable, TDMA
+			 * mode for power saving
+			 *
+			 * here softap mode screen off will cost 70-80mA for
+			 * phone
+			 */
+			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x23, 0x18,
+						    0x00, 0x10, 0x24);
+			break;
+		case 41:
+			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x15,
+						    0x3, 0x11, 0x11);
+			break;
+		case 42:
+			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x20,
+						    0x3, 0x11, 0x11);
+			break;
+		case 43:
+			btc8821a1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x30,
+						    0x3, 0x10, 0x11);
+			break;
 		}
 	} else {
 		/* disable PS tdma */
@@ -1619,15 +1641,23 @@ static void btc8821a1ant_act_wifi_con_bt_acl_busy(struct btc_coexist *btcoexist,
 		return;
 	} else if (bt_link_info->a2dp_only) {
 		/* A2DP */
-		if ((bt_rssi_state != BTC_RSSI_STATE_HIGH) &&
-		    (bt_rssi_state != BTC_RSSI_STATE_STAY_HIGH)) {
+		if (wifi_status == BT_8821A_1ANT_WIFI_STATUS_CONNECTED_IDLE) {
+			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 32);
+			btc8821a1ant_coex_table_with_type(btcoexist,
+							  NORMAL_EXEC, 1);
+			coex_dm->auto_tdma_adjust = false;
+		} else if ((bt_rssi_state != BTC_RSSI_STATE_HIGH) &&
+			   (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
+			btc8821a1ant_coex_table_with_type(btcoexist,
+							  NORMAL_EXEC, 1);
+		} else {
 			/* for low BT RSSI */
-			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-					     true, 11);
+			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
+			btc8821a1ant_coex_table_with_type(btcoexist,
+							  NORMAL_EXEC, 1);
 			coex_dm->auto_tdma_adjust = false;
 		}
-
-		btc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 	} else if (bt_link_info->hid_exist && bt_link_info->a2dp_exist) {
 		/* HID+A2DP */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
@@ -1638,7 +1668,7 @@ static void btc8821a1ant_act_wifi_con_bt_acl_busy(struct btc_coexist *btcoexist,
 		} else {
 			/*for low BT RSSI*/
 			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-					     true, 11);
+					     true, 14);
 			coex_dm->auto_tdma_adjust = false;
 		}
 
@@ -1647,13 +1677,13 @@ static void btc8821a1ant_act_wifi_con_bt_acl_busy(struct btc_coexist *btcoexist,
 		(bt_link_info->hid_exist && bt_link_info->pan_exist)) {
 		/* PAN(OPP, FTP), HID+PAN(OPP, FTP) */
 		btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
-		btc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		btc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 6);
 		coex_dm->auto_tdma_adjust = false;
 	} else if (((bt_link_info->a2dp_exist) && (bt_link_info->pan_exist)) ||
 		   (bt_link_info->hid_exist && bt_link_info->a2dp_exist &&
 		    bt_link_info->pan_exist)) {
 		/* A2DP+PAN(OPP, FTP), HID+A2DP+PAN(OPP, FTP) */
-		btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
+		btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 43);
 		btc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 		coex_dm->auto_tdma_adjust = false;
 	} else {
@@ -1718,52 +1748,49 @@ void btc8821a1ant_action_wifi_connected_scan(struct btc_coexist *btcoexist)
 
 	/* tdma and coex table */
 	if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
-		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
-			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 22);
+		if (bt_link_info->a2dp_exist) {
+			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
 			btc8821a1ant_coex_table_with_type(btcoexist,
 							  NORMAL_EXEC, 1);
 		} else {
-		btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
-		btc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
-	}
-	} else if ((BT_8821A_1ANT_BT_STATUS_SCO_BUSY ==
-		    coex_dm->bt_status) ||
-		   (BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY ==
-		    coex_dm->bt_status)) {
+			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+			btc8821a1ant_coex_table_with_type(btcoexist,
+							  NORMAL_EXEC, 4);
+		}
+	} else if ((coex_dm->bt_status == BT_8821A_1ANT_BT_STATUS_SCO_BUSY) ||
+		   (coex_dm->bt_status ==
+		    BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY)) {
 		btc8821a1ant_act_bt_sco_hid_only_busy(btcoexist,
 			BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SCAN);
 	} else {
-		btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
-		btc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		btc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
 	}
 }
 
 static void btc8821a1ant_act_wifi_conn_sp_pkt(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool hs_connecting = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_CONNECTING, &hs_connecting);
 
 	btc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
 				      0x0, 0x0);
 
 	/* tdma and coex table */
-	if (coex_dm->bt_status == BT_8821A_1ANT_BT_STATUS_ACL_BUSY) {
-		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
-			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-					     true, 22);
-			btc8821a1ant_coex_table_with_type(btcoexist,
-							  NORMAL_EXEC, 1);
-		} else {
-			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-					     true, 20);
-			btc8821a1ant_coex_table_with_type(btcoexist,
-							  NORMAL_EXEC, 1);
-		}
-	} else {
-		btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+	if ((bt_link_info->sco_exist) || (bt_link_info->hid_exist) ||
+	    (bt_link_info->a2dp_exist)) {
+		btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 32);
+		btc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
+	}
+
+	if ((bt_link_info->hid_exist) && (bt_link_info->a2dp_exist)) {
+		btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
 		btc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	} else if (bt_link_info->pan_exist) {
+		btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+		btc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
+	} else {
+		btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		btc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
 	}
 }
 
@@ -1773,6 +1800,7 @@ static void btc8821a1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 	bool wifi_busy = false;
 	bool scan = false, link = false, roam = false;
 	bool under_4way = false;
+	bool ap_enable = false;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 		 "[BTCoex], CoexForWifiConnect()===>\n");
@@ -1790,24 +1818,37 @@ static void btc8821a1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
 	if (scan || link || roam) {
-		btc8821a1ant_action_wifi_connected_scan(btcoexist);
+		if (scan)
+			btc8821a1ant_action_wifi_connected_scan(btcoexist);
+		else
+			btc8821a1ant_act_wifi_conn_sp_pkt(btcoexist);
+
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], CoexForWifiConnect(), return for wifi is under scan<===\n");
 		return;
 	}
 
 	/* power save state*/
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+			   &ap_enable);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
 	if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY ==
-			coex_dm->bt_status && !btcoexist->bt_link_info.hid_only)
-		btc8821a1ant_power_save_state(btcoexist,
-					      BTC_PS_LPS_ON, 0x50, 0x4);
-	else
+	    coex_dm->bt_status && !ap_enable &&
+	    !btcoexist->bt_link_info.hid_only) {
+		if (!wifi_busy && btcoexist->bt_link_info.a2dp_only)
+			/* A2DP */
+			btc8821a1ant_power_save_state(btcoexist,
+						BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		else
+			btc8821a1ant_power_save_state(btcoexist, BTC_PS_LPS_ON,
+						      0x50, 0x4);
+	} else {
 		btc8821a1ant_power_save_state(btcoexist,
 					      BTC_PS_WIFI_NATIVE,
 					      0x0, 0x0);
+	}
 
 	/* tdma and coex table */
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
 	if (!wifi_busy) {
 		if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
 			btc8821a1ant_act_wifi_con_bt_acl_busy(btcoexist,
@@ -1819,8 +1860,7 @@ static void btc8821a1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 			btc8821a1ant_act_bt_sco_hid_only_busy(btcoexist,
 				BT_8821A_1ANT_WIFI_STATUS_CONNECTED_IDLE);
 		} else {
-			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-					     true, 5);
+			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
 			btc8821a1ant_coex_table_with_type(btcoexist,
 							  NORMAL_EXEC, 2);
 		}
@@ -1835,7 +1875,7 @@ static void btc8821a1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 			btc8821a1ant_act_bt_sco_hid_only_busy(btcoexist,
 				BT_8821A_1ANT_WIFI_STATUS_CONNECTED_BUSY);
 		} else {
-			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+			btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
 			btc8821a1ant_coex_table_with_type(btcoexist,
 							  NORMAL_EXEC, 2);
 		}
@@ -1988,11 +2028,11 @@ static void btc8821a1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 			    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
 				btc8821a1ant_limited_tx(btcoexist,
 							NORMAL_EXEC, 1, 1,
-							1, 1);
+							0, 1);
 			} else {
 				btc8821a1ant_limited_tx(btcoexist,
 							NORMAL_EXEC, 1, 1,
-							1, 1);
+							0, 1);
 			}
 		} else {
 			btc8821a1ant_limited_tx(btcoexist, NORMAL_EXEC,
@@ -2056,7 +2096,6 @@ static void btc8821a1ant_init_coex_dm(struct btc_coexist *btcoexist)
 	 */
 	btc8821a1ant_sw_mechanism(btcoexist, false);
 
-	btc8821a1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
 	btc8821a1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
 }
 
@@ -2116,6 +2155,7 @@ static void btc8821a1ant_init_hw_config(struct btc_coexist *btcoexist,
 void ex_btc8821a1ant_init_hwconfig(struct btc_coexist *btcoexist, bool wifionly)
 {
 	btc8821a1ant_init_hw_config(btcoexist, true, wifionly);
+	btcoexist->auto_report_1ant = true;
 }
 
 void ex_btc8821a1ant_init_coex_dm(struct btc_coexist *btcoexist)
@@ -2406,9 +2446,8 @@ void ex_btc8821a1ant_display_coex_info(struct btc_coexist *btcoexist)
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		 "\r\n %-35s = %d/ %d", "0x774(low-pri rx/tx)",
 		 coex_sta->low_priority_rx, coex_sta->low_priority_tx);
-#if (BT_AUTO_REPORT_ONLY_8821A_1ANT == 1)
-	btc8821a1ant_monitor_bt_ctr(btcoexist);
-#endif
+	if (btcoexist->auto_report_1ant)
+		btc8821a1ant_monitor_bt_ctr(btcoexist);
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
 }
 
@@ -2434,7 +2473,7 @@ void ex_btc8821a1ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
 		btc8821a1ant_set_ant_path(btcoexist,
 					  BTC_ANT_PATH_BT, false, true);
 		/* set PTA control */
-		btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		btc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
 		btc8821a1ant_coex_table_with_type(btcoexist,
 						  NORMAL_EXEC, 0);
 	} else if (BTC_IPS_LEAVE == type) {
@@ -2442,7 +2481,9 @@ void ex_btc8821a1ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
 			 "[BTCoex], IPS LEAVE notify\n");
 		coex_sta->under_ips = false;
 
-		btc8821a1ant_run_coexist_mechanism(btcoexist);
+		btc8821a1ant_init_hw_config(btcoexist, false, false);
+		btc8821a1ant_init_coex_dm(btcoexist);
+		btc8821a1ant_query_bt_info(btcoexist);
 	}
 }
 
@@ -2484,6 +2525,19 @@ void ex_btc8821a1ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
 		return;
 	}
 
+	if (type == BTC_SCAN_START) {
+		coex_sta->wifi_is_high_pri_task = true;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], SCAN START notify\n");
+
+		/* Force antenna setup for no scan result issue */
+		btc8821a1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
+	} else {
+		coex_sta->wifi_is_high_pri_task = false;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], SCAN FINISH notify\n");
+	}
+
 	if (coex_sta->bt_disabled)
 		return;
 
@@ -2538,7 +2592,7 @@ void ex_btc8821a1ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
 void ex_btc8821a1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	bool	wifi_connected = false, bt_hs_on = false;
+	bool wifi_connected = false, bt_hs_on = false;
 	u32 wifi_link_status = 0;
 	u32 num_of_wifi_link = 0;
 	bool bt_ctrl_agg_buf_size = false;
@@ -2556,6 +2610,18 @@ void ex_btc8821a1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 		return;
 	}
 
+	if (type == BTC_ASSOCIATE_START) {
+		coex_sta->wifi_is_high_pri_task = true;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], CONNECT START notify\n");
+		coex_dm->arp_cnt = 0;
+	} else {
+		coex_sta->wifi_is_high_pri_task = false;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], CONNECT FINISH notify\n");
+		coex_dm->arp_cnt = 0;
+	}
+
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_LINK_STATUS,
 			   &wifi_link_status);
 	num_of_wifi_link = wifi_link_status >> 16;
@@ -2621,6 +2687,7 @@ void ex_btc8821a1ant_media_status_notify(struct btc_coexist *btcoexist,
 	} else {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], MEDIA disconnect notify\n");
+		coex_dm->arp_cnt = 0;
 	}
 
 	/* only 2.4G we need to inform bt the chnl mask */
@@ -2674,6 +2741,24 @@ void ex_btc8821a1ant_special_packet_notify(struct btc_coexist *btcoexist,
 		return;
 	}
 
+	if (type == BTC_PACKET_DHCP || type == BTC_PACKET_EAPOL ||
+	    type == BTC_PACKET_ARP) {
+		coex_sta->wifi_is_high_pri_task = true;
+
+		if (type == BTC_PACKET_ARP) {
+			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+				 "[BTCoex], specific Packet ARP notify\n");
+		} else {
+			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+				 "[BTCoex], specific Packet DHCP or EAPOL notify\n");
+		}
+	} else {
+		coex_sta->wifi_is_high_pri_task = false;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], specific Packet [Type = %d] notify\n",
+			 type);
+	}
+
 	coex_sta->special_pkt_period_cnt = 0;
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_LINK_STATUS,
@@ -2696,8 +2781,20 @@ void ex_btc8821a1ant_special_packet_notify(struct btc_coexist *btcoexist,
 		return;
 	}
 
-	if (BTC_PACKET_DHCP == type ||
-	    BTC_PACKET_EAPOL == type) {
+	if (type == BTC_PACKET_DHCP || type == BTC_PACKET_EAPOL ||
+	    type == BTC_PACKET_ARP) {
+		if (type == BTC_PACKET_ARP) {
+			coex_dm->arp_cnt++;
+			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+				 "[BTCoex], ARP Packet Count = %d\n",
+				 coex_dm->arp_cnt);
+			if (coex_dm->arp_cnt >= 10)
+				/* if APR PKT > 10 after connect, do not go to
+				 * btc8821a1ant_act_wifi_conn_sp_pkt
+				 */
+				return;
+		}
+
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], special Packet(%d) notify\n", type);
 		btc8821a1ant_act_wifi_conn_sp_pkt(btcoexist);
@@ -2742,14 +2839,28 @@ void ex_btc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
 	}
 
 	if (BT_INFO_SRC_8821A_1ANT_WIFI_FW != rsp_source) {
-		coex_sta->bt_retry_cnt =	/* [3:0]*/
-			coex_sta->bt_info_c2h[rsp_source][2]&0xf;
+		/* [3:0] */
+		coex_sta->bt_retry_cnt =
+			coex_sta->bt_info_c2h[rsp_source][2] & 0xf;
 
 		coex_sta->bt_rssi =
-			coex_sta->bt_info_c2h[rsp_source][3]*2+10;
+			coex_sta->bt_info_c2h[rsp_source][3] * 2 + 10;
 
-		coex_sta->bt_info_ext =
-			coex_sta->bt_info_c2h[rsp_source][4];
+		coex_sta->bt_info_ext = coex_sta->bt_info_c2h[rsp_source][4];
+
+		coex_sta->bt_tx_rx_mask =
+			(coex_sta->bt_info_c2h[rsp_source][2] & 0x40);
+		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TX_RX_MASK,
+				   &coex_sta->bt_tx_rx_mask);
+		if (!coex_sta->bt_tx_rx_mask) {
+			/* BT into is responded by BT FW and BT RF REG 0x3C !=
+			 * 0x15 => Need to switch BT TRx Mask
+			 */
+			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+				 "[BTCoex], Switch BT TRx Mask since BT RF REG 0x3C != 0x15\n");
+			btcoexist->btc_set_bt_reg(btcoexist, BTC_BT_REG_RF,
+						  0x3c, 0x15);
+		}
 
 		/* Here we need to resend some wifi info to BT
 		 * because bt is reset and lost the info
@@ -2831,11 +2942,11 @@ void ex_btc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n");
 	} else if ((bt_info&BT_INFO_8821A_1ANT_B_SCO_ESCO) ||
-		(bt_info&BT_INFO_8821A_1ANT_B_SCO_BUSY)) {
+		(bt_info & BT_INFO_8821A_1ANT_B_SCO_BUSY)) {
 		coex_dm->bt_status = BT_8821A_1ANT_BT_STATUS_SCO_BUSY;
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], BtInfoNotify(), BT SCO busy!!!\n");
-	} else if (bt_info&BT_INFO_8821A_1ANT_B_ACL_BUSY) {
+	} else if (bt_info & BT_INFO_8821A_1ANT_B_ACL_BUSY) {
 		if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY != coex_dm->bt_status)
 			coex_dm->auto_tdma_adjust = false;
 		coex_dm->bt_status = BT_8821A_1ANT_BT_STATUS_ACL_BUSY;
@@ -2964,10 +3075,10 @@ void ex_btc8821a1ant_periodical(struct btc_coexist *btcoexist)
 			 "[BTCoex], ****************************************************************\n");
 	}
 
-#if (BT_AUTO_REPORT_ONLY_8821A_1ANT == 0)
-	btc8821a1ant_query_bt_info(btcoexist);
-	btc8821a1ant_monitor_bt_ctr(btcoexist);
-#else
-	coex_sta->special_pkt_period_cnt++;
-#endif
+	if (!btcoexist->auto_report_1ant) {
+		btc8821a1ant_query_bt_info(btcoexist);
+		btc8821a1ant_monitor_bt_ctr(btcoexist);
+	} else {
+		coex_sta->special_pkt_period_cnt++;
+	}
 }
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a1ant.h b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a1ant.h
index 1bd1ebe3364e..b0a6626fbb66 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a1ant.h
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a1ant.h
@@ -27,8 +27,6 @@
  * The following is for 8821A 1ANT BT Co-exist definition
  *===========================================
  */
-#define	BT_AUTO_REPORT_ONLY_8821A_1ANT				0
-
 #define	BT_INFO_8821A_1ANT_B_FTP	BIT7
 #define	BT_INFO_8821A_1ANT_B_A2DP	BIT6
 #define	BT_INFO_8821A_1ANT_B_HID	BIT5
@@ -135,6 +133,7 @@ struct coex_dm_8821a_1ant {
 	u8	cur_retry_limit_type;
 	u8	pre_ampdu_time_type;
 	u8	cur_ampdu_time_type;
+	u32	arp_cnt;
 
 	u8	error_condition;
 };
@@ -155,6 +154,7 @@ struct coex_sta_8821a_1ant {
 	u32	low_priority_tx;
 	u32	low_priority_rx;
 	u8	bt_rssi;
+	bool	bt_tx_rx_mask;
 	u8	pre_bt_rssi_state;
 	u8	pre_wifi_rssi_state[4];
 	bool	c2h_bt_info_req_sent;
@@ -170,21 +170,23 @@ struct coex_sta_8821a_1ant {
  * The following is interface which will notify coex module.
  *===========================================
  */
-void ex_halbtc8821a1ant_init_hwconfig(struct btc_coexist *btcoexist);
-void ex_halbtc8821a1ant_init_coex_dm(struct btc_coexist *btcoexist);
-void ex_halbtc8821a1ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8821a1ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8821a1ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8821a1ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8821a1ant_media_status_notify(struct btc_coexist *btcoexist,
-					    u8 type);
-void ex_halbtc8821a1ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type);
-void ex_halbtc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmpbuf, u8 length);
-void ex_halbtc8821a1ant_halt_notify(struct btc_coexist *btcoexist);
-void ex_halbtc8821a1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnpstate);
-void ex_halbtc8821a1ant_periodical(struct btc_coexist *btcoexist);
-void ex_halbtc8821a1ant_display_coex_info(struct btc_coexist *btcoexist);
-void ex_halbtc8821a1ant_dbg_control(struct btc_coexist *btcoexist, u8 op_code,
-				    u8 op_len, u8 *data);
+void ex_btc8821a1ant_init_hwconfig(struct btc_coexist *btcoexist,
+				   bool wifi_only);
+void ex_btc8821a1ant_init_coex_dm(struct btc_coexist *btcoexist);
+void ex_btc8821a1ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8821a1ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8821a1ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8821a1ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8821a1ant_media_status_notify(struct btc_coexist *btcoexist,
+					 u8 type);
+void ex_btc8821a1ant_special_packet_notify(struct btc_coexist *btcoexist,
+					   u8 type);
+void ex_btc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
+				    u8 *tmpbuf, u8 length);
+void ex_btc8821a1ant_halt_notify(struct btc_coexist *btcoexist);
+void ex_btc8821a1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnpstate);
+void ex_btc8821a1ant_periodical(struct btc_coexist *btcoexist);
+void ex_btc8821a1ant_display_coex_info(struct btc_coexist *btcoexist);
+void ex_btc8821a1ant_dbg_control(struct btc_coexist *btcoexist, u8 op_code,
+				 u8 op_len, u8 *data);
+void ex_btc8821a1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state);
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a2ant.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a2ant.c
index 841b4a83ab70..41943c34edff 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a2ant.c
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a2ant.c
@@ -3220,12 +3220,16 @@ static void btc8821a2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 /* HID+A2DP+PAN(EDR) */
 static void btc8821a2ant_act_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 {
-	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u8 wifi_rssi_state, wifi_rssi_state1, bt_rssi_state;
 	u32 wifi_bw;
 
-	bt_info_ext = coex_sta->bt_info_ext;
 	wifi_rssi_state = btc8821a2ant_wifi_rssi_state(btcoexist, 0, 2, 15, 0);
-	bt_rssi_state = btc8821a2ant_bt_rssi_state(btcoexist, 2, 35, 0);
+	wifi_rssi_state1 = btc8821a2ant_wifi_rssi_state(btcoexist, 1, 2,
+				BT_8821A_2ANT_WIFI_RSSI_COEXSWITCH_THRES, 0);
+	bt_rssi_state = btc8821a2ant_bt_rssi_state(btcoexist,
+				2, BT_8821A_2ANT_BT_RSSI_COEXSWITCH_THRES, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
 
 	btc8821a2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
 	btc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
@@ -3235,44 +3239,32 @@ static void btc8821a2ant_act_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 	else
 		btc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, 0);
 
+	if (BTC_RSSI_HIGH(wifi_rssi_state1) && BTC_RSSI_HIGH(bt_rssi_state)) {
+		btc8821a2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+		btc8821a2ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					      0x0, 0x0);
+	} else {
+		btc8821a2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 14);
+		btc8821a2ant_power_save_state(btcoexist, BTC_PS_LPS_ON, 0x50,
+					      0x4);
+	}
+
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
-	if (wifi_bw == BTC_WIFI_BW_LEGACY) {
-		/* for HID at 11b/g mode */
-		btc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
-					0x5a5a5a5a, 0xffff, 0x3);
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		if (wifi_bw == BTC_WIFI_BW_HT40)
+			btc8821a2ant_tdma_duration_adjust(btcoexist, true,
+							  true, 3);
+		else
+			btc8821a2ant_tdma_duration_adjust(btcoexist, true,
+							  false, 3);
 	} else {
-		/* for HID quality & wifi performance balance at 11n mode */
-		btc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
-					0x5a5a5a5a, 0xffff, 0x3);
+		btc8821a2ant_tdma_duration_adjust(btcoexist, true, true, 3);
 	}
 
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/* fw mechanism */
-		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			if (bt_info_ext&BIT0) {
-				/* a2dp basic rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-							true, true, 3);
-			} else {
-				/* a2dp edr rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-							true, true, 3);
-			}
-		} else {
-			if (bt_info_ext&BIT0) {
-				/* a2dp basic rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-							true, true, 3);
-			} else {
-				/* a2dp edr rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-							true, true, 3);
-			}
-		}
-
-		/* sw mechanism */
+	/* sw mechanism */
+	if (wifi_bw == BTC_WIFI_BW_HT40) {
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
 			btc8821a2ant_sw_mechanism1(btcoexist, true, true,
@@ -3286,33 +3278,6 @@ static void btc8821a2ant_act_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 						   false, 0x18);
 		}
 	} else {
-		/* fw mechanism */
-		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			if (bt_info_ext&BIT0) {
-				/* a2dp basic rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-							true, false, 3);
-			} else {
-				/* a2dp edr rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-							true, false, 3);
-			}
-		} else {
-			if (bt_info_ext&BIT0) {
-				/* a2dp basic rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-								  true, true,
-								  3);
-			} else {
-				/* a2dp edr rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-								  true, true,
-								  3);
-			}
-		}
-
-		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
 			btc8821a2ant_sw_mechanism1(btcoexist, false, true,
@@ -3330,19 +3295,46 @@ static void btc8821a2ant_act_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 
 static void btc8821a2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 {
-	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
 	u32 wifi_bw;
+	u8 wifi_rssi_state, wifi_rssi_state1, bt_rssi_state;
+	u8 ap_num = 0;
 
-	bt_info_ext = coex_sta->bt_info_ext;
 	wifi_rssi_state = btc8821a2ant_wifi_rssi_state(btcoexist, 0, 2, 15, 0);
-	bt_rssi_state = btc8821a2ant_bt_rssi_state(btcoexist, 2, 35, 0);
+	wifi_rssi_state1 = btc8821a2ant_wifi_rssi_state(btcoexist, 1, 2,
+				BT_8821A_2ANT_WIFI_RSSI_COEXSWITCH_THRES, 0);
+	bt_rssi_state = btc8821a2ant_bt_rssi_state(btcoexist,
+				3, BT_8821A_2ANT_BT_RSSI_COEXSWITCH_THRES, 37);
 
-	if (BTC_RSSI_HIGH(bt_rssi_state))
-		btc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		btc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	btc8821a2ant_limited_rx(btcoexist, NORMAL_EXEC, false, true, 0x5);
+	btc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (wifi_bw == BTC_WIFI_BW_LEGACY) {
+		if (BTC_RSSI_HIGH(bt_rssi_state))
+			btc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, 2);
+		else if (BTC_RSSI_MEDIUM(bt_rssi_state))
+			btc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, 2);
+		else
+			btc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, 0);
+	} else {
+		/* only 802.11N mode we have to dec bt power to 4 degree */
+		if (BTC_RSSI_HIGH(bt_rssi_state)) {
+			btcoexist->btc_get(btcoexist, BTC_GET_U1_AP_NUM,
+					   &ap_num);
+			if (ap_num < 10)
+				btc8821a2ant_dec_bt_pwr(btcoexist,
+							NORMAL_EXEC, 4);
+			else
+				btc8821a2ant_dec_bt_pwr(btcoexist,
+							NORMAL_EXEC, 2);
+		} else if (BTC_RSSI_MEDIUM(bt_rssi_state)) {
+			btc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, 2);
+		} else {
+			btc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, 0);
+		}
+	}
 
 	if (wifi_bw == BTC_WIFI_BW_LEGACY) {
 		btc8821a2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
@@ -3354,36 +3346,15 @@ static void btc8821a2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 					      0x4);
 	}
 
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/* fw mechanism */
-		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			if (bt_info_ext & BIT0) {
-				/* a2dp basic rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-								  true, true,
-								  2);
-			} else {
-				/* a2dp edr rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-								  true, true,
-								  2);
-			}
-		} else {
-			if (bt_info_ext & BIT0) {
-				/* a2dp basic rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-								  true, true,
-								  2);
-			} else {
-				/* a2dp edr rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-								  true, true,
-								  2);
-			}
-		}
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		btc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 23);
+	} else {
+		btc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 23);
+	}
 
-		/* sw mechanism */
+	/* sw mechanism */
+	if (wifi_bw == BTC_WIFI_BW_HT40) {
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
 			btc8821a2ant_sw_mechanism1(btcoexist, true, true,
@@ -3397,36 +3368,6 @@ static void btc8821a2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 						   false, 0x18);
 		}
 	} else {
-		/* fw mechanism */
-		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			if (bt_info_ext & BIT0) {
-				/* a2dp basic rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-								  true, true,
-								  2);
-
-			} else {
-				/* a2dp edr rate */
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-								  true, true,
-								  2);
-			}
-		} else {
-			if (bt_info_ext & BIT0) {
-				/*a2dp basic rate*/
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-								  true, true,
-								  2);
-			} else {
-				/*a2dp edr rate*/
-				btc8821a2ant_tdma_duration_adjust(btcoexist,
-								  true, true,
-								  2);
-			}
-		}
-
-		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
 			btc8821a2ant_sw_mechanism1(btcoexist, false, true,
@@ -3544,14 +3485,14 @@ static void btc8821a2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	if (btc8821a2ant_is_common_action(btcoexist)) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], Action 2-Ant common\n");
-		coex_dm->reset_tdma_adjust = true;
+		coex_dm->auto_tdma_adjust = true;
 	} else {
 		if (coex_dm->cur_algorithm != coex_dm->pre_algorithm) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 				 "[BTCoex], pre_algorithm = %d, cur_algorithm = %d\n",
 				    coex_dm->pre_algorithm,
 				    coex_dm->cur_algorithm);
-			coex_dm->reset_tdma_adjust = true;
+			coex_dm->auto_tdma_adjust = false;
 		}
 		switch (coex_dm->cur_algorithm) {
 		case BT_8821A_2ANT_COEX_ALGO_SCO:
@@ -3614,6 +3555,26 @@ static void btc8821a2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 }
 
+static void btc8821a2ant_wifi_off_hw_cfg(struct btc_coexist *btcoexist)
+{
+	u8 h2c_parameter[2] = {0};
+	u32 fw_ver = 0;
+
+	/* set wlan_act to low */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
+
+	/* WiFi goto standby while GNT_BT 0-->1 */
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x780);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+	if (fw_ver >= 0x180000) {
+		/* Use H2C to set GNT_BT to HIGH */
+		h2c_parameter[0] = 1;
+		btcoexist->btc_fill_h2c(btcoexist, 0x6E, 1, h2c_parameter);
+	} else {
+		btcoexist->btc_write_1byte(btcoexist, 0x765, 0x18);
+	}
+}
+
 /**************************************************************
  * extern function start with ex_btc8821a2ant_
  **************************************************************/
@@ -3637,6 +3598,7 @@ void ex_btc8821a2ant_init_hwconfig(struct btc_coexist *btcoexist)
 
 	/* Antenna config */
 	btc8821a2ant_set_ant_path(btcoexist, BTC_ANT_WIFI_AT_MAIN, true, false);
+	coex_sta->dis_ver_info_cnt = 0;
 
 	/* PTA parameter */
 	btc8821a2ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
@@ -3648,6 +3610,43 @@ void ex_btc8821a2ant_init_hwconfig(struct btc_coexist *btcoexist)
 	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
 }
 
+void ex_btc8821a2ant_pre_load_firmware(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	u8 u8tmp = 0x4; /* Set BIT2 by default since it's 2ant case */
+
+	/**
+	 * S0 or S1 setting and Local register setting(By the setting fw can get
+	 * ant number, S0/S1, ... info)
+	 *
+	 * Local setting bit define
+	 *	BIT0: "0" for no antenna inverse; "1" for antenna inverse
+	 *	BIT1: "0" for internal switch; "1" for external switch
+	 *	BIT2: "0" for one antenna; "1" for two antenna
+	 * NOTE: here default all internal switch and 1-antenna ==> BIT1=0 and
+	 * BIT2=0
+	 */
+	if (btcoexist->chip_interface == BTC_INTF_USB) {
+		/* fixed at S0 for USB interface */
+		u8tmp |= 0x1; /* antenna inverse */
+		btcoexist->btc_write_local_reg_1byte(btcoexist, 0xfe08, u8tmp);
+	} else {
+		/* for PCIE and SDIO interface, we check efuse 0xc3[6] */
+		if (board_info->single_ant_path == 0) {
+		} else if (board_info->single_ant_path == 1) {
+			/* set to S0 */
+			u8tmp |= 0x1; /* antenna inverse */
+		}
+
+		if (btcoexist->chip_interface == BTC_INTF_PCI)
+			btcoexist->btc_write_local_reg_1byte(btcoexist, 0x384,
+							     u8tmp);
+		else if (btcoexist->chip_interface == BTC_INTF_SDIO)
+			btcoexist->btc_write_local_reg_1byte(btcoexist, 0x60,
+							     u8tmp);
+	}
+}
+
 void ex_btc8821a2ant_init_coex_dm(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
@@ -3748,14 +3747,6 @@ void ex_btc8821a2ant_display_coex_info(struct btc_coexist *btcoexist)
 		    ((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
 		     "uplink" : "downlink")));
 
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-		 "\r\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]",
-		   ((coex_sta->c2h_bt_inquiry_page) ? ("inquiry/page scan") :
-		    ((BT_8821A_2ANT_BT_STATUS_IDLE == coex_dm->bt_status)
-		     ? "idle" : ((BT_8821A_2ANT_BT_STATUS_CON_IDLE ==
-		     coex_dm->bt_status) ? "connected-idle" : "busy"))),
-		    coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
-
 	if (stack_info->profile_notified) {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 			 "\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP",
@@ -3791,11 +3782,6 @@ void ex_btc8821a2ant_display_coex_info(struct btc_coexist *btcoexist)
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s",
 		 "============[Sw mechanism]============");
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-		 "\r\n %-35s = %d/ %d/ %d/ %d ",
-		 "SM1[ShRf/ LpRA/ LimDig/ btLna]",
-		 coex_dm->cur_rf_rx_lpf_shrink, coex_dm->cur_low_penalty_ra,
-		 coex_dm->limited_dig, coex_dm->cur_bt_lna_constrain);
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		 "\r\n %-35s = %d/ %d/ %d(0x%x) ",
 		 "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]",
 		 coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off,
@@ -3900,11 +3886,16 @@ void ex_btc8821a2ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], IPS ENTER notify\n");
 		coex_sta->under_ips = true;
+		btc8821a2ant_wifi_off_hw_cfg(btcoexist);
+		btc8821a2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
 		btc8821a2ant_coex_all_off(btcoexist);
 	} else if (BTC_IPS_LEAVE == type) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[BTCoex], IPS LEAVE notify\n");
 		coex_sta->under_ips = false;
+		ex_btc8821a2ant_init_hwconfig(btcoexist);
+		btc8821a2ant_init_coex_dm(btcoexist);
+		btc8821a2ant_query_bt_info(btcoexist);
 	}
 }
 
@@ -4016,9 +4007,12 @@ void ex_btc8821a2ant_bt_info_notify(struct btc_coexist *btcoexist,
 	u8 bt_info = 0;
 	u8 i, rsp_source = 0;
 	bool bt_busy = false, limited_dig = false;
-	bool wifi_connected = false, bt_hs_on = false;
+	bool wifi_connected = false, wifi_under_5g = false;
 
 	coex_sta->c2h_bt_info_req_sent = false;
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
 
 	rsp_source = tmp_buf[0] & 0xf;
 	if (rsp_source >= BT_INFO_SRC_8821A_2ANT_MAX)
@@ -4041,16 +4035,35 @@ void ex_btc8821a2ant_bt_info_notify(struct btc_coexist *btcoexist,
 		}
 	}
 
+	if (btcoexist->manual_control) {
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], BtInfoNotify(), return for Manual CTRL<===\n");
+		return;
+	}
+
 	if (BT_INFO_SRC_8821A_2ANT_WIFI_FW != rsp_source) {
 		/* [3:0] */
 		coex_sta->bt_retry_cnt =
 			coex_sta->bt_info_c2h[rsp_source][2]&0xf;
 
 		coex_sta->bt_rssi =
-			coex_sta->bt_info_c2h[rsp_source][3]*2+10;
+			coex_sta->bt_info_c2h[rsp_source][3] * 2 + 10;
 
-		coex_sta->bt_info_ext =
-			coex_sta->bt_info_c2h[rsp_source][4];
+		coex_sta->bt_info_ext = coex_sta->bt_info_c2h[rsp_source][4];
+
+		coex_sta->bt_tx_rx_mask =
+			(coex_sta->bt_info_c2h[rsp_source][2] & 0x40);
+		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TX_RX_MASK,
+				   &coex_sta->bt_tx_rx_mask);
+		if (coex_sta->bt_tx_rx_mask) {
+			/* BT into is responded by BT FW and BT RF REG 0x3C !=
+			 * 0x01 => Need to switch BT TRx Mask
+			 */
+			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+				 "[BTCoex], Switch BT TRx Mask since BT RF REG 0x3C != 0x01\n");
+			btcoexist->btc_set_bt_reg(btcoexist, BTC_BT_REG_RF,
+						  0x3c, 0x01);
+		}
 
 		/* Here we need to resend some wifi info to BT
 		 * because bt is reset and loss of the info
@@ -4068,70 +4081,121 @@ void ex_btc8821a2ant_bt_info_notify(struct btc_coexist *btcoexist,
 
 		}
 
-		if ((coex_sta->bt_info_ext & BIT3)) {
-			btc8821a2ant_ignore_wlan_act(btcoexist,
-						     FORCE_EXEC, false);
-		} else {
-			/* BT already NOT ignore Wlan active, do nothing here.*/
+		if (!btcoexist->manual_control && !wifi_under_5g) {
+			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+				 "[BTCoex], BT ext info = 0x%x!!\n",
+				    coex_sta->bt_info_ext);
+			if ((coex_sta->bt_info_ext & BIT(3))) {
+				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+					 "[BTCoex], BT ext info bit3=1, wifi_connected=%d\n",
+					 wifi_connected);
+				if (wifi_connected) {
+					RT_TRACE(rtlpriv, COMP_BT_COEXIST,
+						 DBG_LOUD,
+						 "[BTCoex], BT ext info bit3 check, set BT NOT to ignore Wlan active!!\n");
+					btc8821a2ant_ignore_wlan_act(btcoexist,
+								     FORCE_EXEC,
+								     false);
+				}
+			} else {
+				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+					 "[BTCoex], BT ext info bit3=0, wifi_connected=%d\n",
+					 wifi_connected);
+				/* BT already NOT ignore Wlan active, do nothing
+				 * here.
+				 */
+				if (!wifi_connected) {
+					RT_TRACE(rtlpriv, COMP_BT_COEXIST,
+						 DBG_LOUD,
+						"[BTCoex], BT ext info bit3 check, set BT to ignore Wlan active!!\n");
+					btc8821a2ant_ignore_wlan_act(
+						btcoexist, FORCE_EXEC, true);
+				}
+			}
 		}
 	}
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
 	/* check BIT2 first ==> check if bt is under inquiry or page scan*/
 	if (bt_info & BT_INFO_8821A_2ANT_B_INQ_PAGE) {
 		coex_sta->c2h_bt_inquiry_page = true;
-		coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_NON_IDLE;
 	} else {
 		coex_sta->c2h_bt_inquiry_page = false;
-		if (bt_info == 0x1) {
-			/* connection exists but not busy*/
-			coex_sta->bt_link_exist = true;
-			coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_CON_IDLE;
-		} else if (bt_info & BT_INFO_8821A_2ANT_B_CONNECTION) {
-			/* connection exists and some link is busy*/
-			coex_sta->bt_link_exist = true;
-			if (bt_info & BT_INFO_8821A_2ANT_B_FTP)
-				coex_sta->pan_exist = true;
-			else
-				coex_sta->pan_exist = false;
-			if (bt_info & BT_INFO_8821A_2ANT_B_A2DP)
-				coex_sta->a2dp_exist = true;
-			else
-				coex_sta->a2dp_exist = false;
-			if (bt_info & BT_INFO_8821A_2ANT_B_HID)
-				coex_sta->hid_exist = true;
-			else
-				coex_sta->hid_exist = false;
-			if (bt_info & BT_INFO_8821A_2ANT_B_SCO_ESCO)
-				coex_sta->sco_exist = true;
-			else
-				coex_sta->sco_exist = false;
-			coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_NON_IDLE;
-		} else {
-			coex_sta->bt_link_exist = false;
+	}
+	/* set link exist status */
+	if (!(bt_info & BT_INFO_8821A_2ANT_B_CONNECTION)) {
+		coex_sta->bt_link_exist = false;
+		coex_sta->pan_exist = false;
+		coex_sta->a2dp_exist = false;
+		coex_sta->hid_exist = false;
+		coex_sta->sco_exist = false;
+	} else { /* connection exists */
+		coex_sta->bt_link_exist = true;
+		if (bt_info & BT_INFO_8821A_2ANT_B_FTP)
+			coex_sta->pan_exist = true;
+		else
 			coex_sta->pan_exist = false;
+		if (bt_info & BT_INFO_8821A_2ANT_B_A2DP)
+			coex_sta->a2dp_exist = true;
+		else
 			coex_sta->a2dp_exist = false;
+		if (bt_info & BT_INFO_8821A_2ANT_B_HID)
+			coex_sta->hid_exist = true;
+		else
 			coex_sta->hid_exist = false;
+		if (bt_info & BT_INFO_8821A_2ANT_B_SCO_ESCO)
+			coex_sta->sco_exist = true;
+		else
 			coex_sta->sco_exist = false;
-			coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_IDLE;
+
+		if ((!coex_sta->hid_exist) &&
+		    (!coex_sta->c2h_bt_inquiry_page) &&
+		    (!coex_sta->sco_exist)) {
+			if (coex_sta->high_priority_tx +
+				    coex_sta->high_priority_rx >= 160)
+				coex_sta->hid_exist = true;
 		}
+	}
 
-		btc8821a2ant_update_bt_link_info(btcoexist);
+	btc8821a2ant_update_bt_link_info(btcoexist);
+
+	if (!(bt_info & BT_INFO_8821A_2ANT_B_CONNECTION)) {
+		coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_IDLE;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], BtInfoNotify(), BT Non-Connected idle!!!\n");
+	} else if (bt_info == BT_INFO_8821A_2ANT_B_CONNECTION) {
+		/* connection exists but no busy */
+		coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_CON_IDLE;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n");
+	} else if ((bt_info & BT_INFO_8821A_2ANT_B_SCO_ESCO) ||
+		   (bt_info & BT_INFO_8821A_2ANT_B_SCO_BUSY)) {
+		coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_SCO_BUSY;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], BtInfoNotify(), BT SCO busy!!!\n");
+	} else if (bt_info & BT_INFO_8821A_2ANT_B_ACL_BUSY) {
+		coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_ACL_BUSY;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], BtInfoNotify(), BT ACL busy!!!\n");
+	} else {
+		coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_MAX;
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], BtInfoNotify(), BT Non-Defined state!!!\n");
 	}
 
-	if (BT_8821A_2ANT_BT_STATUS_NON_IDLE == coex_dm->bt_status)
+	if ((coex_dm->bt_status == BT_8821A_2ANT_BT_STATUS_ACL_BUSY) ||
+	    (coex_dm->bt_status == BT_8821A_2ANT_BT_STATUS_SCO_BUSY) ||
+	    (coex_dm->bt_status == BT_8821A_2ANT_BT_STATUS_ACL_SCO_BUSY)) {
 		bt_busy = true;
-	else
+		limited_dig = true;
+	} else {
 		bt_busy = false;
+		limited_dig = false;
+	}
+
 	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
 
-	if (BT_8821A_2ANT_BT_STATUS_IDLE != coex_dm->bt_status)
-		limited_dig = true;
-	else
-		limited_dig = false;
 	coex_dm->limited_dig = limited_dig;
-	btcoexist->btc_set(btcoexist,
-		BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
 
 	btc8821a2ant_run_coexist_mechanism(btcoexist);
 }
@@ -4143,46 +4207,57 @@ void ex_btc8821a2ant_halt_notify(struct btc_coexist *btcoexist)
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 		 "[BTCoex], Halt notify\n");
 
+	btc8821a2ant_wifi_off_hw_cfg(btcoexist);
 	btc8821a2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
 	ex_btc8821a2ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
 }
 
+void ex_btc8821a2ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD, "[BTCoex], Pnp notify\n");
+
+	if (pnp_state == BTC_WIFI_PNP_SLEEP) {
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], Pnp notify to SLEEP\n");
+	} else if (pnp_state == BTC_WIFI_PNP_WAKE_UP) {
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], Pnp notify to WAKE UP\n");
+		ex_btc8821a2ant_init_hwconfig(btcoexist);
+		btc8821a2ant_init_coex_dm(btcoexist);
+		btc8821a2ant_query_bt_info(btcoexist);
+	}
+}
+
 void ex_btc8821a2ant_periodical(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	static u8 dis_ver_info_cnt;
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-	u32 fw_ver = 0, bt_patch_ver = 0;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 		 "[BTCoex], ==========================Periodical===========================\n");
 
-	if (dis_ver_info_cnt <= 5) {
-		dis_ver_info_cnt += 1;
-		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-			 "[BTCoex], ****************************************************************\n");
-		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-			 "[BTCoex], Ant PG Num/ Ant Mech/ Ant Pos = %d/ %d/ %d\n",
-			 board_info->pg_ant_num,
-			 board_info->btdm_ant_num,
-			 board_info->btdm_ant_pos);
-		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-			 "[BTCoex], BT stack/ hci ext ver = %s / %d\n",
-			 stack_info->profile_notified ? "Yes" : "No",
-			 stack_info->hci_version);
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
-				   &bt_patch_ver);
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-			 "[BTCoex], CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n",
-			 glcoex_ver_date_8821a_2ant, glcoex_ver_8821a_2ant,
-			 fw_ver, bt_patch_ver, bt_patch_ver);
-		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-			 "[BTCoex], ****************************************************************\n");
+	if (coex_sta->dis_ver_info_cnt <= 5) {
+		coex_sta->dis_ver_info_cnt += 1;
+		if (coex_sta->dis_ver_info_cnt == 3) {
+			/* Antenna config to set 0x765 = 0x0 (GNT_BT control by
+			 * PTA) after initial
+			 */
+			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+				 "[BTCoex], Set GNT_BT control by PTA\n");
+			btc8821a2ant_set_ant_path(btcoexist,
+					BTC_ANT_WIFI_AT_MAIN, false, false);
+		}
 	}
 
-	btc8821a2ant_query_bt_info(btcoexist);
-	btc8821a2ant_monitor_bt_ctr(btcoexist);
-	btc8821a2ant_monitor_wifi_ctr(btcoexist);
+	if (btcoexist->auto_report_2ant) {
+		btc8821a2ant_query_bt_info(btcoexist);
+	} else {
+		btc8821a2ant_monitor_bt_ctr(btcoexist);
+		btc8821a2ant_monitor_wifi_ctr(btcoexist);
+
+		if (btc8821a2ant_is_wifi_status_changed(btcoexist) ||
+		    coex_dm->auto_tdma_adjust)
+			btc8821a2ant_run_coexist_mechanism(btcoexist);
+	}
 }
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a2ant.h b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a2ant.h
index 535ca10e910b..a839d5574422 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a2ant.h
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a2ant.h
@@ -54,6 +54,9 @@ enum _BT_8821A_2ANT_BT_STATUS {
 	BT_8821A_2ANT_BT_STATUS_IDLE		= 0x0,
 	BT_8821A_2ANT_BT_STATUS_CON_IDLE	= 0x1,
 	BT_8821A_2ANT_BT_STATUS_NON_IDLE	= 0x2,
+	BT_8821A_2ANT_BT_STATUS_ACL_BUSY	= 0x3,
+	BT_8821A_2ANT_BT_STATUS_SCO_BUSY	= 0x4,
+	BT_8821A_2ANT_BT_STATUS_ACL_SCO_BUSY	= 0x5,
 	BT_8821A_2ANT_BT_STATUS_MAX
 };
 
@@ -76,10 +79,6 @@ struct coex_dm_8821a_2ant {
 	/* fw mechanism */
 	bool		pre_dec_bt_pwr_lvl;
 	bool		cur_dec_bt_pwr_lvl;
-	bool		pre_bt_lna_constrain;
-	bool		cur_bt_lna_constrain;
-	u8		pre_bt_psd_mode;
-	u8		cur_bt_psd_mode;
 	u8		pre_fw_dac_swing_lvl;
 	u8		cur_fw_dac_swing_lvl;
 	bool		cur_ignore_wlan_act;
@@ -143,6 +142,7 @@ struct coex_sta_8821a_2ant {
 	u32	low_priority_tx;
 	u32	low_priority_rx;
 	u8	bt_rssi;
+	bool	bt_tx_rx_mask;
 	u8	pre_bt_rssi_state;
 	u8	pre_wifi_rssi_state[4];
 	bool	c2h_bt_info_req_sent;
@@ -164,6 +164,8 @@ struct coex_sta_8821a_2ant {
 
 	u8	coex_table_type;
 	bool	force_lps_on;
+
+	u8	dis_ver_info_cnt;
 };
 
 /*===========================================
@@ -171,58 +173,60 @@ struct coex_sta_8821a_2ant {
  *===========================================
  */
 void
-ex_halbtc8821a2ant_init_hwconfig(
+ex_btc8821a2ant_init_hwconfig(
 	struct btc_coexist *btcoexist
 	);
 void
-ex_halbtc8821a2ant_init_coex_dm(
+ex_btc8821a2ant_init_coex_dm(
 	struct btc_coexist *btcoexist
 	);
 void
-ex_halbtc8821a2ant_ips_notify(
+ex_btc8821a2ant_ips_notify(
 	struct btc_coexist *btcoexist,
 	u8 type
 	);
 void
-ex_halbtc8821a2ant_lps_notify(
+ex_btc8821a2ant_lps_notify(
 	struct btc_coexist *btcoexist,
 	u8 type
 	);
 void
-ex_halbtc8821a2ant_scan_notify(
+ex_btc8821a2ant_scan_notify(
 	struct btc_coexist *btcoexist,
 	u8 type
 	);
 void
-ex_halbtc8821a2ant_connect_notify(
+ex_btc8821a2ant_connect_notify(
 	struct btc_coexist *btcoexist,
 	u8 type
 	);
 void
-ex_halbtc8821a2ant_media_status_notify(
+ex_btc8821a2ant_media_status_notify(
 	struct btc_coexist *btcoexist,
 	u8 type
 	);
 void
-ex_halbtc8821a2ant_special_packet_notify(
+ex_btc8821a2ant_special_packet_notify(
 	struct btc_coexist *btcoexist,
 	u8 type
 	);
 void
-ex_halbtc8821a2ant_bt_info_notify(
+ex_btc8821a2ant_bt_info_notify(
 	struct btc_coexist *btcoexist,
 	u8 *tmp_buf,
 	u8 length
 	);
 void
-ex_halbtc8821a2ant_halt_notify(
+ex_btc8821a2ant_halt_notify(
 	struct btc_coexist *btcoexist
 	);
 void
-ex_halbtc8821a2ant_periodical(
+ex_btc8821a2ant_periodical(
 	struct btc_coexist *btcoexist
 	);
 void
-ex_halbtc8821a2ant_display_coex_info(
+ex_btc8821a2ant_display_coex_info(
 	struct btc_coexist *btcoexist
 	);
+void ex_btc8821a2ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state);
+void ex_btc8821a2ant_pre_load_firmware(struct btc_coexist *btcoexist);
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtcoutsrc.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtcoutsrc.c
index f13000612913..e6024b013ca5 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtcoutsrc.c
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtcoutsrc.c
@@ -36,6 +36,20 @@ u32 btc_dbg_type[BTC_MSG_MAX];
 /***************************************************
  *		Debug related function
  ***************************************************/
+
+const char *const gl_btc_wifi_bw_string[] = {
+	"11bg",
+	"HT20",
+	"HT40",
+	"HT80",
+	"HT160"
+};
+
+const char *const gl_btc_wifi_freq_string[] = {
+	"2.4G",
+	"5G"
+};
+
 static bool halbtc_is_bt_coexist_available(struct btc_coexist *btcoexist)
 {
 	if (!btcoexist->binded || NULL == btcoexist->adapter)
@@ -54,28 +68,39 @@ static bool halbtc_is_wifi_busy(struct rtl_priv *rtlpriv)
 
 static void halbtc_dbg_init(void)
 {
-	u8 i;
-
-	for (i = 0; i < BTC_MSG_MAX; i++)
-		btc_dbg_type[i] = 0;
-
-	btc_dbg_type[BTC_MSG_INTERFACE] =
-/*			INTF_INIT				| */
-/*			INTF_NOTIFY				| */
-			0;
+}
 
-	btc_dbg_type[BTC_MSG_ALGORITHM] =
-/*			ALGO_BT_RSSI_STATE			| */
-/*			ALGO_WIFI_RSSI_STATE			| */
-/*			ALGO_BT_MONITOR				| */
-/*			ALGO_TRACE				| */
-/*			ALGO_TRACE_FW				| */
-/*			ALGO_TRACE_FW_DETAIL			| */
-/*			ALGO_TRACE_FW_EXEC			| */
-/*			ALGO_TRACE_SW				| */
-/*			ALGO_TRACE_SW_DETAIL			| */
-/*			ALGO_TRACE_SW_EXEC			| */
-			0;
+/***************************************************
+ *		helper function
+ ***************************************************/
+static bool is_any_client_connect_to_ap(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	struct rtl_sta_info *drv_priv;
+	u8 cnt = 0;
+
+	if (mac->opmode == NL80211_IFTYPE_ADHOC ||
+	    mac->opmode == NL80211_IFTYPE_MESH_POINT ||
+	    mac->opmode == NL80211_IFTYPE_AP) {
+		if (in_interrupt() > 0) {
+			list_for_each_entry(drv_priv, &rtlpriv->entry_list,
+					    list) {
+				cnt++;
+			}
+		} else {
+			spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+			list_for_each_entry(drv_priv, &rtlpriv->entry_list,
+					    list) {
+				cnt++;
+			}
+			spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+		}
+	}
+	if (cnt > 0)
+		return true;
+	else
+		return false;
 }
 
 static bool halbtc_is_bt40(struct rtl_priv *adapter)
@@ -188,12 +213,14 @@ static void halbtc_leave_lps(struct btc_coexist *btcoexist)
 			   &ap_enable);
 
 	if (ap_enable) {
-		pr_info("halbtc_leave_lps()<--dont leave lps under AP mode\n");
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
+			 "%s()<--dont leave lps under AP mode\n", __func__);
 		return;
 	}
 
 	btcoexist->bt_info.bt_ctrl_lps = true;
 	btcoexist->bt_info.bt_lps_on = false;
+	rtl_lps_leave(rtlpriv->mac80211.hw);
 }
 
 static void halbtc_enter_lps(struct btc_coexist *btcoexist)
@@ -209,36 +236,93 @@ static void halbtc_enter_lps(struct btc_coexist *btcoexist)
 			   &ap_enable);
 
 	if (ap_enable) {
-		pr_info("halbtc_enter_lps()<--dont enter lps under AP mode\n");
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
+			 "%s()<--dont enter lps under AP mode\n", __func__);
 		return;
 	}
 
 	btcoexist->bt_info.bt_ctrl_lps = true;
-	btcoexist->bt_info.bt_lps_on = false;
+	btcoexist->bt_info.bt_lps_on = true;
+	rtl_lps_enter(rtlpriv->mac80211.hw);
 }
 
 static void halbtc_normal_lps(struct btc_coexist *btcoexist)
 {
+	struct rtl_priv *rtlpriv;
+
+	rtlpriv = btcoexist->adapter;
+
 	if (btcoexist->bt_info.bt_ctrl_lps) {
 		btcoexist->bt_info.bt_lps_on = false;
+		rtl_lps_leave(rtlpriv->mac80211.hw);
 		btcoexist->bt_info.bt_ctrl_lps = false;
 	}
 }
 
-static void halbtc_leave_low_power(void)
+static void halbtc_leave_low_power(struct btc_coexist *btcoexist)
 {
 }
 
-static void halbtc_nomal_low_power(void)
+static void halbtc_normal_low_power(struct btc_coexist *btcoexist)
 {
 }
 
-static void halbtc_disable_low_power(void)
+static void halbtc_disable_low_power(struct btc_coexist *btcoexist,
+				     bool low_pwr_disable)
 {
+	/* TODO: original/leave 32k low power */
+	btcoexist->bt_info.bt_disable_low_pwr = low_pwr_disable;
 }
 
-static void halbtc_aggregation_check(void)
+static void halbtc_aggregation_check(struct btc_coexist *btcoexist)
 {
+	bool need_to_act = false;
+	static unsigned long pre_time;
+	unsigned long cur_time = 0;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	/* To void continuous deleteBA=>addBA=>deleteBA=>addBA
+	 * This function is not allowed to continuous called
+	 * It can only be called after 8 seconds
+	 */
+
+	cur_time = jiffies;
+	if (jiffies_to_msecs(cur_time - pre_time) <= 8000) {
+		/* over 8 seconds you can execute this function again. */
+		return;
+	}
+	pre_time = cur_time;
+
+	if (btcoexist->bt_info.reject_agg_pkt) {
+		need_to_act = true;
+		btcoexist->bt_info.pre_reject_agg_pkt =
+			btcoexist->bt_info.reject_agg_pkt;
+	} else {
+		if (btcoexist->bt_info.pre_reject_agg_pkt) {
+			need_to_act = true;
+			btcoexist->bt_info.pre_reject_agg_pkt =
+				btcoexist->bt_info.reject_agg_pkt;
+		}
+
+		if (btcoexist->bt_info.pre_bt_ctrl_agg_buf_size !=
+		    btcoexist->bt_info.bt_ctrl_agg_buf_size) {
+			need_to_act = true;
+			btcoexist->bt_info.pre_bt_ctrl_agg_buf_size =
+				btcoexist->bt_info.bt_ctrl_agg_buf_size;
+		}
+
+		if (btcoexist->bt_info.bt_ctrl_agg_buf_size) {
+			if (btcoexist->bt_info.pre_agg_buf_size !=
+			    btcoexist->bt_info.agg_buf_size) {
+				need_to_act = true;
+			}
+			btcoexist->bt_info.pre_agg_buf_size =
+				btcoexist->bt_info.agg_buf_size;
+		}
+
+		if (need_to_act)
+			rtl_rx_ampdu_apply(rtlpriv);
+	}
 }
 
 static u32 halbtc_get_bt_patch_version(struct btc_coexist *btcoexist)
@@ -246,10 +330,37 @@ static u32 halbtc_get_bt_patch_version(struct btc_coexist *btcoexist)
 	return 0;
 }
 
-static s32 halbtc_get_wifi_rssi(struct rtl_priv *adapter)
+u32 halbtc_get_wifi_link_status(struct btc_coexist *btcoexist)
 {
-	struct rtl_priv *rtlpriv = adapter;
-	s32	undec_sm_pwdb = 0;
+	/* return value:
+	 * [31:16] => connected port number
+	 * [15:0]  => port connected bit define
+	 */
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	u32 ret_val = 0;
+	u32 port_connected_status = 0, num_of_connected_port = 0;
+
+	if (mac->opmode == NL80211_IFTYPE_STATION &&
+	    mac->link_state >= MAC80211_LINKED) {
+		port_connected_status |= WIFI_STA_CONNECTED;
+		num_of_connected_port++;
+	}
+	/* AP & ADHOC & MESH */
+	if (is_any_client_connect_to_ap(btcoexist)) {
+		port_connected_status |= WIFI_AP_CONNECTED;
+		num_of_connected_port++;
+	}
+	/* TODO: P2P Connected Status */
+
+	ret_val = (num_of_connected_port << 16) | port_connected_status;
+
+	return ret_val;
+}
+
+static s32 halbtc_get_wifi_rssi(struct rtl_priv *rtlpriv)
+{
+	int undec_sm_pwdb = 0;
 
 	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED)
 		undec_sm_pwdb = rtlpriv->dm.undec_sm_pwdb;
@@ -270,6 +381,7 @@ static bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
 	u32 *u32_tmp = (u32 *)out_buf;
 	u8 *u8_tmp = (u8 *)out_buf;
 	bool tmp = false;
+	bool ret = true;
 
 	if (!halbtc_is_bt_coexist_available(btcoexist))
 		return false;
@@ -277,12 +389,17 @@ static bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
 	switch (get_type) {
 	case BTC_GET_BL_HS_OPERATION:
 		*bool_tmp = false;
+		ret = false;
 		break;
 	case BTC_GET_BL_HS_CONNECTING:
 		*bool_tmp = false;
+		ret = false;
 		break;
 	case BTC_GET_BL_WIFI_CONNECTED:
-		if (rtlpriv->mac80211.link_state >= MAC80211_LINKED)
+		if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_STATION &&
+		    rtlpriv->mac80211.link_state >= MAC80211_LINKED)
+			tmp = true;
+		if (is_any_client_connect_to_ap(btcoexist))
 			tmp = true;
 		*bool_tmp = tmp;
 		break;
@@ -304,32 +421,26 @@ static bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
 		else
 			*bool_tmp = false;
 		break;
-	case BTC_GET_BL_WIFI_ROAM:	/*TODO*/
+	case BTC_GET_BL_WIFI_ROAM:
 		if (mac->link_state == MAC80211_LINKING)
 			*bool_tmp = true;
 		else
 			*bool_tmp = false;
 		break;
-	case BTC_GET_BL_WIFI_4_WAY_PROGRESS:	/*TODO*/
-			*bool_tmp = false;
-
+	case BTC_GET_BL_WIFI_4_WAY_PROGRESS:
+		*bool_tmp = rtlpriv->btcoexist.btc_info.in_4way;
 		break;
 	case BTC_GET_BL_WIFI_UNDER_5G:
-		*bool_tmp = false; /*TODO*/
-
-	case BTC_GET_BL_WIFI_DHCP:	/*TODO*/
-		break;
-	case BTC_GET_BL_WIFI_SOFTAP_IDLE:
-		*bool_tmp = true;
-		break;
-	case BTC_GET_BL_WIFI_SOFTAP_LINKING:
-		*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_IN_EARLY_SUSPEND:
-		*bool_tmp = false;
+		if (rtlhal->current_bandtype == BAND_ON_5G)
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
 		break;
 	case BTC_GET_BL_WIFI_AP_MODE_ENABLE:
-		*bool_tmp = false;
+		if (mac->opmode == NL80211_IFTYPE_AP)
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
 		break;
 	case BTC_GET_BL_WIFI_ENABLE_ENCRYPTION:
 		if (NO_ENCRYPTION == rtlpriv->sec.pairwise_enc_algorithm)
@@ -338,16 +449,26 @@ static bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
 			*bool_tmp = true;
 		break;
 	case BTC_GET_BL_WIFI_UNDER_B_MODE:
-		*bool_tmp = false; /*TODO*/
+		if (rtlpriv->mac80211.mode == WIRELESS_MODE_B)
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
 		break;
 	case BTC_GET_BL_EXT_SWITCH:
 		*bool_tmp = false;
 		break;
+	case BTC_GET_BL_WIFI_IS_IN_MP_MODE:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_IS_ASUS_8723B:
+		*bool_tmp = false;
+		break;
 	case BTC_GET_S4_WIFI_RSSI:
 		*s32_tmp = halbtc_get_wifi_rssi(rtlpriv);
 		break;
-	case BTC_GET_S4_HS_RSSI:	/*TODO*/
-		*s32_tmp = halbtc_get_wifi_rssi(rtlpriv);
+	case BTC_GET_S4_HS_RSSI:
+		*s32_tmp = 0;
+		ret = false;
 		break;
 	case BTC_GET_U4_WIFI_BW:
 		*u32_tmp = halbtc_get_wifi_bw(btcoexist);
@@ -359,7 +480,10 @@ static bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
 			*u32_tmp = BTC_WIFI_TRAFFIC_RX;
 		break;
 	case BTC_GET_U4_WIFI_FW_VER:
-		*u32_tmp = rtlhal->fw_version;
+		*u32_tmp = (rtlhal->fw_version << 16) | rtlhal->fw_subversion;
+		break;
+	case BTC_GET_U4_WIFI_LINK_STATUS:
+		*u32_tmp = halbtc_get_wifi_link_status(btcoexist);
 		break;
 	case BTC_GET_U4_BT_PATCH_VER:
 		*u32_tmp = halbtc_get_bt_patch_version(btcoexist);
@@ -374,10 +498,17 @@ static bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
 		*u8_tmp = halbtc_get_wifi_central_chnl(btcoexist);
 		break;
 	case BTC_GET_U1_WIFI_HS_CHNL:
-		*u8_tmp = 1;/*BT_OperateChnl(rtlpriv);*/
+		*u8_tmp = 0;
+		ret = false;
 		break;
-	case BTC_GET_U1_MAC_PHY_MODE:
-		*u8_tmp = BTC_MP_UNKNOWN;
+	case BTC_GET_U1_AP_NUM:
+		*u8_tmp = rtlpriv->btcoexist.btc_info.ap_num;
+		break;
+	case BTC_GET_U1_ANT_TYPE:
+		*u8_tmp = (u8)BTC_ANT_TYPE_0;
+		break;
+	case BTC_GET_U1_IOT_PEER:
+		*u8_tmp = 0;
 		break;
 
 		/************* 1Ant **************/
@@ -386,10 +517,11 @@ static bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
 		break;
 
 	default:
+		ret = false;
 		break;
 	}
 
-	return true;
+	return ret;
 }
 
 static bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
@@ -398,6 +530,7 @@ static bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
 	bool *bool_tmp = (bool *)in_buf;
 	u8 *u8_tmp = (u8 *)in_buf;
 	u32 *u32_tmp = (u32 *)in_buf;
+	bool ret = true;
 
 	if (!halbtc_is_bt_coexist_available(btcoexist))
 		return false;
@@ -420,11 +553,17 @@ static bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
 		btcoexist->bt_info.reject_agg_pkt = *bool_tmp;
 		break;
 	case BTC_SET_BL_BT_CTRL_AGG_SIZE:
-		btcoexist->bt_info.bt_ctrl_buf_size = *bool_tmp;
+		btcoexist->bt_info.bt_ctrl_agg_buf_size = *bool_tmp;
 		break;
 	case BTC_SET_BL_INC_SCAN_DEV_NUM:
 		btcoexist->bt_info.increase_scan_dev_num = *bool_tmp;
 		break;
+	case BTC_SET_BL_BT_TX_RX_MASK:
+		btcoexist->bt_info.bt_tx_rx_mask = *bool_tmp;
+		break;
+	case BTC_SET_BL_MIRACAST_PLUS_BT:
+		btcoexist->bt_info.miracast_plus_bt = *bool_tmp;
+		break;
 		/* set some u1Byte type variables. */
 	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON:
 		btcoexist->bt_info.rssi_adjust_for_agc_table_on = *u8_tmp;
@@ -432,25 +571,25 @@ static bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
 	case BTC_SET_U1_AGG_BUF_SIZE:
 		btcoexist->bt_info.agg_buf_size = *u8_tmp;
 		break;
-		/* the following are some action which will be triggered */
+
+	/* the following are some action which will be triggered */
 	case BTC_SET_ACT_GET_BT_RSSI:
-		/*BTHCI_SendGetBtRssiEvent(rtlpriv);*/
+		ret = false;
 		break;
 	case BTC_SET_ACT_AGGREGATE_CTRL:
-		halbtc_aggregation_check();
+		halbtc_aggregation_check(btcoexist);
 		break;
 
-		/* 1Ant */
+	/* 1Ant */
 	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE:
 		btcoexist->bt_info.rssi_adjust_for_1ant_coex_type = *u8_tmp;
 		break;
 	case BTC_SET_UI_SCAN_SIG_COMPENSATION:
-	/*	rtlpriv->mlmepriv.scan_compensation = *u8_tmp;  */
 		break;
-	case BTC_SET_U1_1ANT_LPS:
+	case BTC_SET_U1_LPS_VAL:
 		btcoexist->bt_info.lps_val = *u8_tmp;
 		break;
-	case BTC_SET_U1_1ANT_RPWM:
+	case BTC_SET_U1_RPWM_VAL:
 		btcoexist->bt_info.rpwm_val = *u8_tmp;
 		break;
 	/* the following are some action which will be triggered  */
@@ -464,41 +603,24 @@ static bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
 		halbtc_normal_lps(btcoexist);
 		break;
 	case BTC_SET_ACT_DISABLE_LOW_POWER:
-		halbtc_disable_low_power();
+		halbtc_disable_low_power(btcoexist, *bool_tmp);
 		break;
 	case BTC_SET_ACT_UPDATE_RAMASK:
 		btcoexist->bt_info.ra_mask = *u32_tmp;
 		break;
 	case BTC_SET_ACT_SEND_MIMO_PS:
 		break;
-	case BTC_SET_ACT_INC_FORCE_EXEC_PWR_CMD_CNT:
-		btcoexist->bt_info.force_exec_pwr_cmd_cnt++;
-		break;
 	case BTC_SET_ACT_CTRL_BT_INFO: /*wait for 8812/8821*/
 		break;
 	case BTC_SET_ACT_CTRL_BT_COEX:
 		break;
+	case BTC_SET_ACT_CTRL_8723B_ANT:
+		break;
 	default:
 		break;
 	}
 
-	return true;
-}
-
-static void halbtc_display_coex_statistics(struct btc_coexist *btcoexist)
-{
-}
-
-static void halbtc_display_bt_link_info(struct btc_coexist *btcoexist)
-{
-}
-
-static void halbtc_display_bt_fw_info(struct btc_coexist *btcoexist)
-{
-}
-
-static void halbtc_display_fw_pwr_mode_cmd(struct btc_coexist *btcoexist)
-{
+	return ret;
 }
 
 /************************************************************
@@ -574,6 +696,35 @@ static void halbtc_write_4byte(void *bt_context, u32 reg_addr, u32 data)
 	rtl_write_dword(rtlpriv, reg_addr, data);
 }
 
+void halbtc_write_local_reg_1byte(void *btc_context, u32 reg_addr, u8 data)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)btc_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	if (btcoexist->chip_interface == BTC_INTF_SDIO)
+		;
+	else if (btcoexist->chip_interface == BTC_INTF_PCI)
+		rtl_write_byte(rtlpriv, reg_addr, data);
+	else if (btcoexist->chip_interface == BTC_INTF_USB)
+		rtl_write_byte(rtlpriv, reg_addr, data);
+}
+
+void halbtc_set_macreg(void *btc_context, u32 reg_addr, u32 bit_mask, u32 data)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)btc_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	rtl_set_bbreg(rtlpriv->mac80211.hw, reg_addr, bit_mask, data);
+}
+
+u32 halbtc_get_macreg(void *btc_context, u32 reg_addr, u32 bit_mask)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)btc_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	return rtl_get_bbreg(rtlpriv->mac80211.hw, reg_addr, bit_mask);
+}
+
 static void halbtc_set_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask,
 			     u32 data)
 {
@@ -619,50 +770,64 @@ static void halbtc_fill_h2c_cmd(void *bt_context, u8 element_id,
 					cmd_len, cmd_buf);
 }
 
-static void halbtc_display_dbg_msg(void *bt_context, u8 disp_type)
+void halbtc_set_bt_reg(void *btc_context, u8 reg_type, u32 offset, u32 set_val)
 {
-	struct btc_coexist *btcoexist =	(struct btc_coexist *)bt_context;
-	switch (disp_type) {
-	case BTC_DBG_DISP_COEX_STATISTICS:
-		halbtc_display_coex_statistics(btcoexist);
-		break;
-	case BTC_DBG_DISP_BT_LINK_INFO:
-		halbtc_display_bt_link_info(btcoexist);
-		break;
-	case BTC_DBG_DISP_BT_FW_VER:
-		halbtc_display_bt_fw_info(btcoexist);
-		break;
-	case BTC_DBG_DISP_FW_PWR_MODE_CMD:
-		halbtc_display_fw_pwr_mode_cmd(btcoexist);
-		break;
-	default:
-		break;
+	struct btc_coexist *btcoexist = (struct btc_coexist *)btc_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	u8 cmd_buffer1[4] = {0};
+	u8 cmd_buffer2[4] = {0};
+	u8 *addr_to_set = (u8 *)&offset;
+	u8 *value_to_set = (u8 *)&set_val;
+	u8 oper_ver = 0;
+	u8 req_num = 0;
+
+	if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		cmd_buffer1[0] |= (oper_ver & 0x0f);	/* Set OperVer */
+		cmd_buffer1[0] |= ((req_num << 4) & 0xf0);	/* Set ReqNum */
+		cmd_buffer1[1] = 0x0d;	/* OpCode: BT_LO_OP_WRITE_REG_VALUE */
+		cmd_buffer1[2] = value_to_set[0];	/* Set WriteRegValue */
+		rtlpriv->cfg->ops->fill_h2c_cmd(rtlpriv->mac80211.hw, 0x67, 4,
+						&cmd_buffer1[0]);
+
+		msleep(200);
+		req_num++;
+
+		cmd_buffer2[0] |= (oper_ver & 0x0f);	/* Set OperVer */
+		cmd_buffer2[0] |= ((req_num << 4) & 0xf0);	/* Set ReqNum */
+		cmd_buffer2[1] = 0x0c;	/* OpCode: BT_LO_OP_WRITE_REG_ADDR */
+		cmd_buffer2[3] = addr_to_set[0];	/* Set WriteRegAddr */
+		rtlpriv->cfg->ops->fill_h2c_cmd(rtlpriv->mac80211.hw, 0x67, 4,
+						&cmd_buffer2[0]);
 	}
 }
 
+bool halbtc_under_ips(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
+	enum rf_pwrstate rtstate;
+
+	if (ppsc->inactiveps) {
+		rtstate = ppsc->rfpwr_state;
+
+		if (rtstate != ERFON &&
+		    ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
 /*****************************************************************
  *         Extern functions called by other module
  *****************************************************************/
-bool exhalbtc_initlize_variables(struct rtl_priv *adapter)
+bool exhalbtc_initlize_variables(void)
 {
 	struct btc_coexist *btcoexist = &gl_bt_coexist;
 
-	btcoexist->statistics.cnt_bind++;
-
 	halbtc_dbg_init();
 
-	if (btcoexist->binded)
-		return false;
-	else
-		btcoexist->binded = true;
-
-	btcoexist->chip_interface = BTC_INTF_UNKNOWN;
-
-	if (NULL == btcoexist->adapter)
-		btcoexist->adapter = adapter;
-
-	btcoexist->stack_info.profile_notified = false;
-
 	btcoexist->btc_read_1byte = halbtc_read_1byte;
 	btcoexist->btc_write_1byte = halbtc_write_1byte;
 	btcoexist->btc_write_1byte_bitmask = halbtc_bitmask_write_1byte;
@@ -670,6 +835,7 @@ bool exhalbtc_initlize_variables(struct rtl_priv *adapter)
 	btcoexist->btc_write_2byte = halbtc_write_2byte;
 	btcoexist->btc_read_4byte = halbtc_read_4byte;
 	btcoexist->btc_write_4byte = halbtc_write_4byte;
+	btcoexist->btc_write_local_reg_1byte = halbtc_write_local_reg_1byte;
 
 	btcoexist->btc_set_bb_reg = halbtc_set_bbreg;
 	btcoexist->btc_get_bb_reg = halbtc_get_bbreg;
@@ -678,10 +844,11 @@ bool exhalbtc_initlize_variables(struct rtl_priv *adapter)
 	btcoexist->btc_get_rf_reg = halbtc_get_rfreg;
 
 	btcoexist->btc_fill_h2c = halbtc_fill_h2c_cmd;
-	btcoexist->btc_disp_dbg_msg = halbtc_display_dbg_msg;
 
 	btcoexist->btc_get = halbtc_get;
 	btcoexist->btc_set = halbtc_set;
+	btcoexist->btc_set_bt_reg = halbtc_set_bt_reg;
+
 
 	btcoexist->bt_info.bt_ctrl_buf_size = false;
 	btcoexist->bt_info.agg_buf_size = 5;
@@ -690,40 +857,148 @@ bool exhalbtc_initlize_variables(struct rtl_priv *adapter)
 	return true;
 }
 
-void exhalbtc_init_hw_config(struct btc_coexist *btcoexist)
+bool exhalbtc_bind_bt_coex_withadapter(void *adapter)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct btc_coexist *btcoexist = &gl_bt_coexist;
+	struct rtl_priv *rtlpriv = adapter;
+	u8 ant_num = 2, chip_type, single_ant_path = 0;
+
+	if (btcoexist->binded)
+		return false;
+
+	switch (rtlpriv->rtlhal.interface) {
+	case INTF_PCI:
+		btcoexist->chip_interface = BTC_INTF_PCI;
+		break;
+	case INTF_USB:
+		btcoexist->chip_interface = BTC_INTF_USB;
+		break;
+	default:
+		btcoexist->chip_interface = BTC_INTF_UNKNOWN;
+		break;
+	}
+
+	btcoexist->binded = true;
+	btcoexist->statistics.cnt_bind++;
+
+	btcoexist->adapter = adapter;
+
+	btcoexist->stack_info.profile_notified = false;
+
+	btcoexist->bt_info.bt_ctrl_agg_buf_size = false;
+	btcoexist->bt_info.agg_buf_size = 5;
+
+	btcoexist->bt_info.increase_scan_dev_num = false;
+	btcoexist->bt_info.miracast_plus_bt = false;
+
+	chip_type = rtl_get_hwpg_bt_type(rtlpriv);
+	exhalbtc_set_chip_type(chip_type);
+	ant_num = rtl_get_hwpg_ant_num(rtlpriv);
+	exhalbtc_set_ant_num(rtlpriv, BT_COEX_ANT_TYPE_PG, ant_num);
 
+	/* set default antenna position to main  port */
+	btcoexist->board_info.btdm_ant_pos = BTC_ANTENNA_AT_MAIN_PORT;
+
+	single_ant_path = rtl_get_hwpg_single_ant_path(rtlpriv);
+	exhalbtc_set_single_ant_path(single_ant_path);
+
+	if (rtl_get_hwpg_package_type(rtlpriv) == 0)
+		btcoexist->board_info.tfbga_package = false;
+	else if (rtl_get_hwpg_package_type(rtlpriv) == 1)
+		btcoexist->board_info.tfbga_package = false;
+	else
+		btcoexist->board_info.tfbga_package = true;
+
+	if (btcoexist->board_info.tfbga_package)
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], Package Type = TFBGA\n");
+	else
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+			 "[BTCoex], Package Type = Non-TFBGA\n");
+
+	return true;
+}
+
+void exhalbtc_power_on_setting(struct btc_coexist *btcoexist)
+{
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->statistics.cnt_power_on++;
+
+	if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_power_on_setting(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_power_on_setting(btcoexist);
+	}
+}
+
+void exhalbtc_pre_load_firmware(struct btc_coexist *btcoexist)
+{
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->statistics.cnt_pre_load_firmware++;
+
+	if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_pre_load_firmware(btcoexist);
+	}
+}
+
+void exhalbtc_init_hw_config(struct btc_coexist *btcoexist, bool wifi_only)
+{
 	if (!halbtc_is_bt_coexist_available(btcoexist))
 		return;
 
 	btcoexist->statistics.cnt_init_hw_config++;
 
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_btc8723b2ant_init_hwconfig(btcoexist);
+	if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8821a2ant_init_hwconfig(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8821a1ant_init_hwconfig(btcoexist, wifi_only);
+	} else if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_init_hwconfig(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_init_hwconfig(btcoexist, wifi_only);
+	} else if (IS_HARDWARE_TYPE_8723A(btcoexist->adapter)) {
+		/* 8723A has no this function */
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8192e2ant_init_hwconfig(btcoexist);
+	}
 }
 
 void exhalbtc_init_coex_dm(struct btc_coexist *btcoexist)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-
 	if (!halbtc_is_bt_coexist_available(btcoexist))
 		return;
 
 	btcoexist->statistics.cnt_init_coex_dm++;
 
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_btc8723b2ant_init_coex_dm(btcoexist);
+	if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8821a2ant_init_coex_dm(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8821a1ant_init_coex_dm(btcoexist);
+	} else if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_init_coex_dm(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_init_coex_dm(btcoexist);
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8192e2ant_init_coex_dm(btcoexist);
+	}
 
 	btcoexist->initilized = true;
 }
 
 void exhalbtc_ips_notify(struct btc_coexist *btcoexist, u8 type)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	u8 ips_type;
 
 	if (!halbtc_is_bt_coexist_available(btcoexist))
@@ -737,18 +1012,28 @@ void exhalbtc_ips_notify(struct btc_coexist *btcoexist, u8 type)
 	else
 		ips_type = BTC_IPS_LEAVE;
 
-	halbtc_leave_low_power();
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_btc8723b2ant_ips_notify(btcoexist, ips_type);
+	halbtc_leave_low_power(btcoexist);
+
+	if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8821a2ant_ips_notify(btcoexist, ips_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8821a1ant_ips_notify(btcoexist, ips_type);
+	} else if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_ips_notify(btcoexist, ips_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_ips_notify(btcoexist, ips_type);
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8192e2ant_ips_notify(btcoexist, ips_type);
+	}
 
-	halbtc_nomal_low_power();
+	halbtc_normal_low_power(btcoexist);
 }
 
 void exhalbtc_lps_notify(struct btc_coexist *btcoexist, u8 type)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	u8 lps_type;
 
 	if (!halbtc_is_bt_coexist_available(btcoexist))
@@ -762,14 +1047,24 @@ void exhalbtc_lps_notify(struct btc_coexist *btcoexist, u8 type)
 	else
 		lps_type = BTC_LPS_ENABLE;
 
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_btc8723b2ant_lps_notify(btcoexist, lps_type);
+	if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8821a2ant_lps_notify(btcoexist, lps_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8821a1ant_lps_notify(btcoexist, lps_type);
+	} else if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_lps_notify(btcoexist, lps_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_lps_notify(btcoexist, lps_type);
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8192e2ant_lps_notify(btcoexist, lps_type);
+	}
 }
 
 void exhalbtc_scan_notify(struct btc_coexist *btcoexist, u8 type)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	u8 scan_type;
 
 	if (!halbtc_is_bt_coexist_available(btcoexist))
@@ -783,18 +1078,28 @@ void exhalbtc_scan_notify(struct btc_coexist *btcoexist, u8 type)
 	else
 		scan_type = BTC_SCAN_FINISH;
 
-	halbtc_leave_low_power();
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_btc8723b2ant_scan_notify(btcoexist, scan_type);
+	halbtc_leave_low_power(btcoexist);
+
+	if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8821a2ant_scan_notify(btcoexist, scan_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8821a1ant_scan_notify(btcoexist, scan_type);
+	} else if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_scan_notify(btcoexist, scan_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_scan_notify(btcoexist, scan_type);
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8192e2ant_scan_notify(btcoexist, scan_type);
+	}
 
-	halbtc_nomal_low_power();
+	halbtc_normal_low_power(btcoexist);
 }
 
 void exhalbtc_connect_notify(struct btc_coexist *btcoexist, u8 action)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	u8 asso_type;
 
 	if (!halbtc_is_bt_coexist_available(btcoexist))
@@ -808,10 +1113,24 @@ void exhalbtc_connect_notify(struct btc_coexist *btcoexist, u8 action)
 	else
 		asso_type = BTC_ASSOCIATE_FINISH;
 
-	halbtc_leave_low_power();
+	halbtc_leave_low_power(btcoexist);
+
+	if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8821a2ant_connect_notify(btcoexist, asso_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8821a1ant_connect_notify(btcoexist, asso_type);
+	} else if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_connect_notify(btcoexist, asso_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_connect_notify(btcoexist, asso_type);
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8192e2ant_connect_notify(btcoexist, asso_type);
+	}
 
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_btc8723b2ant_connect_notify(btcoexist, asso_type);
+	halbtc_normal_low_power(btcoexist);
 }
 
 void exhalbtc_mediastatus_notify(struct btc_coexist *btcoexist,
@@ -830,15 +1149,28 @@ void exhalbtc_mediastatus_notify(struct btc_coexist *btcoexist,
 	else
 		status = BTC_MEDIA_DISCONNECT;
 
-	halbtc_leave_low_power();
+	halbtc_leave_low_power(btcoexist);
+
+	if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8821a2ant_media_status_notify(btcoexist, status);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8821a1ant_media_status_notify(btcoexist, status);
+	} else if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_media_status_notify(btcoexist, status);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_media_status_notify(btcoexist, status);
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8192e2ant_media_status_notify(btcoexist, status);
+	}
 
-	halbtc_nomal_low_power();
+	halbtc_normal_low_power(btcoexist);
 }
 
 void exhalbtc_special_packet_notify(struct btc_coexist *btcoexist, u8 pkt_type)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	u8 packet_type;
 
 	if (!halbtc_is_bt_coexist_available(btcoexist))
@@ -847,28 +1179,85 @@ void exhalbtc_special_packet_notify(struct btc_coexist *btcoexist, u8 pkt_type)
 	if (btcoexist->manual_control)
 		return;
 
-	packet_type = BTC_PACKET_DHCP;
-
-	halbtc_leave_low_power();
+	if (pkt_type == PACKET_DHCP) {
+		packet_type = BTC_PACKET_DHCP;
+	} else if (pkt_type == PACKET_EAPOL) {
+		packet_type = BTC_PACKET_EAPOL;
+	} else if (pkt_type == PACKET_ARP) {
+		packet_type = BTC_PACKET_ARP;
+	} else {
+		packet_type = BTC_PACKET_UNKNOWN;
+		return;
+	}
 
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_btc8723b2ant_special_packet_notify(btcoexist,
-						      packet_type);
+	halbtc_leave_low_power(btcoexist);
+
+	if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8821a2ant_special_packet_notify(btcoexist,
+							      packet_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8821a1ant_special_packet_notify(btcoexist,
+							      packet_type);
+	} else if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_special_packet_notify(btcoexist,
+							      packet_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_special_packet_notify(btcoexist,
+							      packet_type);
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8192e2ant_special_packet_notify(btcoexist,
+							      packet_type);
+	}
 
-	halbtc_nomal_low_power();
+	halbtc_normal_low_power(btcoexist);
 }
 
 void exhalbtc_bt_info_notify(struct btc_coexist *btcoexist,
 			     u8 *tmp_buf, u8 length)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	if (!halbtc_is_bt_coexist_available(btcoexist))
 		return;
 	btcoexist->statistics.cnt_bt_info_notify++;
 
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_btc8723b2ant_bt_info_notify(btcoexist, tmp_buf, length);
+	halbtc_leave_low_power(btcoexist);
+
+	if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8821a2ant_bt_info_notify(btcoexist, tmp_buf,
+						       length);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8821a1ant_bt_info_notify(btcoexist, tmp_buf,
+						       length);
+	} else if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_bt_info_notify(btcoexist, tmp_buf,
+						       length);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_bt_info_notify(btcoexist, tmp_buf,
+						       length);
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8192e2ant_bt_info_notify(btcoexist, tmp_buf,
+						       length);
+	}
+
+	halbtc_normal_low_power(btcoexist);
+}
+
+void exhalbtc_rf_status_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+	} else if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_rf_status_notify(btcoexist, type);
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+	}
 }
 
 void exhalbtc_stack_operation_notify(struct btc_coexist *btcoexist, u8 type)
@@ -881,44 +1270,117 @@ void exhalbtc_stack_operation_notify(struct btc_coexist *btcoexist, u8 type)
 	if (btcoexist->manual_control)
 		return;
 
-	stack_op_type = BTC_STACK_OP_NONE;
-
-	halbtc_leave_low_power();
-
-	halbtc_nomal_low_power();
+	if ((type == HCI_BT_OP_INQUIRY_START) ||
+	    (type == HCI_BT_OP_PAGING_START) ||
+	    (type == HCI_BT_OP_PAIRING_START)) {
+		stack_op_type = BTC_STACK_OP_INQ_PAGE_PAIR_START;
+	} else if ((type == HCI_BT_OP_INQUIRY_FINISH) ||
+		   (type == HCI_BT_OP_PAGING_SUCCESS) ||
+		   (type == HCI_BT_OP_PAGING_UNSUCCESS) ||
+		   (type == HCI_BT_OP_PAIRING_FINISH)) {
+		stack_op_type = BTC_STACK_OP_INQ_PAGE_PAIR_FINISH;
+	} else {
+		stack_op_type = BTC_STACK_OP_NONE;
+	}
 }
 
 void exhalbtc_halt_notify(struct btc_coexist *btcoexist)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	if (!halbtc_is_bt_coexist_available(btcoexist))
 		return;
 
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_btc8723b2ant_halt_notify(btcoexist);
+	if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8821a2ant_halt_notify(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8821a1ant_halt_notify(btcoexist);
+	} else if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_halt_notify(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_halt_notify(btcoexist);
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8192e2ant_halt_notify(btcoexist);
+	}
+
+	btcoexist->binded = false;
 }
 
 void exhalbtc_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
 {
 	if (!halbtc_is_bt_coexist_available(btcoexist))
 		return;
+
+	/* currently only 1ant we have to do the notification,
+	 * once pnp is notified to sleep state, we have to leave LPS that
+	 * we can sleep normally.
+	 */
+
+	if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_pnp_notify(btcoexist, pnp_state);
+		else if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_pnp_notify(btcoexist, pnp_state);
+	} else if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8821a1ant_pnp_notify(btcoexist, pnp_state);
+		else if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8821a2ant_pnp_notify(btcoexist, pnp_state);
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+	}
 }
 
-void exhalbtc_periodical(struct btc_coexist *btcoexist)
+void exhalbtc_coex_dm_switch(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
 	if (!halbtc_is_bt_coexist_available(btcoexist))
 		return;
-	btcoexist->statistics.cnt_periodical++;
+	btcoexist->statistics.cnt_coex_dm_switch++;
+
+	halbtc_leave_low_power(btcoexist);
+
+	if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 1) {
+			btcoexist->stop_coex_dm = true;
+			ex_btc8723b1ant_coex_dm_reset(btcoexist);
+			exhalbtc_set_ant_num(rtlpriv,
+					     BT_COEX_ANT_TYPE_DETECTED, 2);
+			ex_btc8723b2ant_init_hwconfig(btcoexist);
+			ex_btc8723b2ant_init_coex_dm(btcoexist);
+			btcoexist->stop_coex_dm = false;
+		}
+	}
 
-	halbtc_leave_low_power();
+	halbtc_normal_low_power(btcoexist);
+}
 
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_btc8723b2ant_periodical(btcoexist);
+void exhalbtc_periodical(struct btc_coexist *btcoexist)
+{
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_periodical++;
+
+	halbtc_leave_low_power(btcoexist);
+
+	if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8821a2ant_periodical(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			if (!halbtc_under_ips(btcoexist))
+				ex_btc8821a1ant_periodical(btcoexist);
+	} else if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_periodical(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_periodical(btcoexist);
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8192e2ant_periodical(btcoexist);
+	}
 
-	halbtc_nomal_low_power();
+	halbtc_normal_low_power(btcoexist);
 }
 
 void exhalbtc_dbg_control(struct btc_coexist *btcoexist,
@@ -927,6 +1389,17 @@ void exhalbtc_dbg_control(struct btc_coexist *btcoexist,
 	if (!halbtc_is_bt_coexist_available(btcoexist))
 		return;
 	btcoexist->statistics.cnt_dbg_ctrl++;
+
+	halbtc_leave_low_power(btcoexist);
+
+	halbtc_normal_low_power(btcoexist);
+}
+
+void exhalbtc_antenna_detection(struct btc_coexist *btcoexist, u32 cent_freq,
+				u32 offset, u32 span, u32 seconds)
+{
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
 }
 
 void exhalbtc_stack_update_profile_info(void)
@@ -964,11 +1437,6 @@ void exhalbtc_set_bt_patch_version(u16 bt_hci_version, u16 bt_patch_version)
 	btcoexist->bt_info.bt_hci_ver = bt_hci_version;
 }
 
-void exhalbtc_set_bt_exist(bool bt_exist)
-{
-	gl_bt_coexist.board_info.bt_exist = bt_exist;
-}
-
 void exhalbtc_set_chip_type(u8 chip_type)
 {
 	switch (chip_type) {
@@ -1002,25 +1470,8 @@ void exhalbtc_set_ant_num(struct rtl_priv *rtlpriv, u8 type, u8 ant_num)
 	if (BT_COEX_ANT_TYPE_PG == type) {
 		gl_bt_coexist.board_info.pg_ant_num = ant_num;
 		gl_bt_coexist.board_info.btdm_ant_num = ant_num;
-		/* The antenna position:
-		 * Main (default) or Aux for pgAntNum=2 && btdmAntNum =1.
-		 * The antenna position should be determined by
-		 * auto-detect mechanism.
-		 * The following is assumed to main,
-		 * and those must be modified
-		 * if y auto-detect mechanism is ready
-		 */
-		if ((gl_bt_coexist.board_info.pg_ant_num == 2) &&
-		    (gl_bt_coexist.board_info.btdm_ant_num == 1))
-			gl_bt_coexist.board_info.btdm_ant_pos =
-						       BTC_ANTENNA_AT_MAIN_PORT;
-		else
-			gl_bt_coexist.board_info.btdm_ant_pos =
-						       BTC_ANTENNA_AT_MAIN_PORT;
 	} else if (BT_COEX_ANT_TYPE_ANTDIV == type) {
 		gl_bt_coexist.board_info.btdm_ant_num = ant_num;
-		gl_bt_coexist.board_info.btdm_ant_pos =
-						       BTC_ANTENNA_AT_MAIN_PORT;
 	} else if (type == BT_COEX_ANT_TYPE_DETECTED) {
 		gl_bt_coexist.board_info.btdm_ant_num = ant_num;
 		if (rtlpriv->cfg->mod_params->ant_sel == 1)
@@ -1032,13 +1483,33 @@ void exhalbtc_set_ant_num(struct rtl_priv *rtlpriv, u8 type, u8 ant_num)
 	}
 }
 
+/* Currently used by 8723b only, S0 or S1 */
+void exhalbtc_set_single_ant_path(u8 single_ant_path)
+{
+	gl_bt_coexist.board_info.single_ant_path = single_ant_path;
+}
+
 void exhalbtc_display_bt_coex_info(struct btc_coexist *btcoexist)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	if (!halbtc_is_bt_coexist_available(btcoexist))
 		return;
 
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_btc8723b2ant_display_coex_info(btcoexist);
+	halbtc_leave_low_power(btcoexist);
+
+	if (IS_HARDWARE_TYPE_8821(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8821a2ant_display_coex_info(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8821a1ant_display_coex_info(btcoexist);
+	} else if (IS_HARDWARE_TYPE_8723B(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8723b2ant_display_coex_info(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_btc8723b1ant_display_coex_info(btcoexist);
+	} else if (IS_HARDWARE_TYPE_8192E(btcoexist->adapter)) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_btc8192e2ant_display_coex_info(btcoexist);
+	}
+
+	halbtc_normal_low_power(btcoexist);
 }
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtcoutsrc.h b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtcoutsrc.h
index c8271135aaaa..f9b87c12db09 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtcoutsrc.h
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtcoutsrc.h
@@ -30,6 +30,9 @@
 #define		NORMAL_EXEC				false
 #define		FORCE_EXEC				true
 
+#define		BTC_RF_OFF				0x0
+#define		BTC_RF_ON				0x1
+
 #define		BTC_RF_A				RF90_PATH_A
 #define		BTC_RF_B				RF90_PATH_B
 #define		BTC_RF_C				RF90_PATH_C
@@ -150,6 +153,7 @@ struct btc_board_info {
 	u8 btdm_ant_pos;
 	u8 single_ant_path; /* current used for 8723b only, 1=>s0,  0=>s1 */
 	bool bt_exist;
+	bool tfbga_package;
 };
 
 enum btc_dbg_opcode {
@@ -196,6 +200,44 @@ enum btc_wifi_pnp {
 	BTC_WIFI_PNP_MAX
 };
 
+enum btc_iot_peer {
+	BTC_IOT_PEER_UNKNOWN = 0,
+	BTC_IOT_PEER_REALTEK = 1,
+	BTC_IOT_PEER_REALTEK_92SE = 2,
+	BTC_IOT_PEER_BROADCOM = 3,
+	BTC_IOT_PEER_RALINK = 4,
+	BTC_IOT_PEER_ATHEROS = 5,
+	BTC_IOT_PEER_CISCO = 6,
+	BTC_IOT_PEER_MERU = 7,
+	BTC_IOT_PEER_MARVELL = 8,
+	BTC_IOT_PEER_REALTEK_SOFTAP = 9,
+	BTC_IOT_PEER_SELF_SOFTAP = 10, /* Self is SoftAP */
+	BTC_IOT_PEER_AIRGO = 11,
+	BTC_IOT_PEER_REALTEK_JAGUAR_BCUTAP = 12,
+	BTC_IOT_PEER_REALTEK_JAGUAR_CCUTAP = 13,
+	BTC_IOT_PEER_MAX,
+};
+
+/* for 8723b-d cut large current issue */
+enum bt_wifi_coex_state {
+	BTC_WIFI_STAT_INIT,
+	BTC_WIFI_STAT_IQK,
+	BTC_WIFI_STAT_NORMAL_OFF,
+	BTC_WIFI_STAT_MP_OFF,
+	BTC_WIFI_STAT_NORMAL,
+	BTC_WIFI_STAT_ANT_DIV,
+	BTC_WIFI_STAT_MAX
+};
+
+enum bt_ant_type {
+	BTC_ANT_TYPE_0,
+	BTC_ANT_TYPE_1,
+	BTC_ANT_TYPE_2,
+	BTC_ANT_TYPE_3,
+	BTC_ANT_TYPE_4,
+	BTC_ANT_TYPE_MAX
+};
+
 enum btc_get_type {
 	/* type bool */
 	BTC_GET_BL_HS_OPERATION,
@@ -216,6 +258,9 @@ enum btc_get_type {
 	BTC_GET_BL_WIFI_UNDER_B_MODE,
 	BTC_GET_BL_EXT_SWITCH,
 	BTC_GET_BL_WIFI_IS_IN_MP_MODE,
+	BTC_GET_BL_IS_ASUS_8723B,
+	BTC_GET_BL_FW_READY,
+	BTC_GET_BL_RF4CE_CONNECTED,
 
 	/* type s4Byte */
 	BTC_GET_S4_WIFI_RSSI,
@@ -228,6 +273,11 @@ enum btc_get_type {
 	BTC_GET_U4_WIFI_LINK_STATUS,
 	BTC_GET_U4_BT_PATCH_VER,
 	BTC_GET_U4_VENDOR,
+	BTC_GET_U4_SUPPORTED_VERSION,
+	BTC_GET_U4_SUPPORTED_FEATURE,
+	BTC_GET_U4_WIFI_IQK_TOTAL,
+	BTC_GET_U4_WIFI_IQK_OK,
+	BTC_GET_U4_WIFI_IQK_FAIL,
 
 	/* type u1Byte */
 	BTC_GET_U1_WIFI_DOT11_CHNL,
@@ -235,6 +285,8 @@ enum btc_get_type {
 	BTC_GET_U1_WIFI_HS_CHNL,
 	BTC_GET_U1_MAC_PHY_MODE,
 	BTC_GET_U1_AP_NUM,
+	BTC_GET_U1_ANT_TYPE,
+	BTC_GET_U1_IOT_PEER,
 
 	/* for 1Ant */
 	BTC_GET_U1_LPS_MODE,
@@ -293,6 +345,7 @@ enum btc_set_type {
 	/* BT Coex related */
 	BTC_SET_ACT_CTRL_BT_INFO,
 	BTC_SET_ACT_CTRL_BT_COEX,
+	BTC_SET_ACT_CTRL_8723B_ANT,
 	/***************************/
 	BTC_SET_MAX
 };
@@ -411,12 +464,18 @@ struct btc_bt_info {
 	bool bt_disabled;
 	u8 rssi_adjust_for_agc_table_on;
 	u8 rssi_adjust_for_1ant_coex_type;
+	bool pre_bt_ctrl_agg_buf_size;
 	bool bt_busy;
+	u8 pre_agg_buf_size;
 	u8 agg_buf_size;
 	bool limited_dig;
+	bool pre_reject_agg_pkt;
 	bool reject_agg_pkt;
 	bool bt_ctrl_buf_size;
 	bool increase_scan_dev_num;
+	bool miracast_plus_bt;
+	bool bt_ctrl_agg_buf_size;
+	bool bt_tx_rx_mask;
 	u16 bt_hci_ver;
 	u16 bt_real_fw_ver;
 	u8 bt_fw_ver;
@@ -464,10 +523,13 @@ struct btc_statistics {
 	u32 cnt_coex_dm_switch;
 	u32 cnt_stack_operation_notify;
 	u32 cnt_dbg_ctrl;
+	u32 cnt_pre_load_firmware;
+	u32 cnt_power_on;
 };
 
 struct btc_bt_link_info {
 	bool bt_link_exist;
+	bool bt_hi_pri_link_exist;
 	bool sco_exist;
 	bool sco_only;
 	bool a2dp_exist;
@@ -496,6 +558,11 @@ struct btc_coexist {
 	enum btc_chip_interface	chip_interface;
 	struct btc_bt_link_info bt_link_info;
 
+	/* boolean variables to replace BT_AUTO_REPORT_ONLY_XXXXY_ZANT
+	 * configuration parameters
+	 */
+	bool auto_report_1ant;
+	bool auto_report_2ant;
 	bool initilized;
 	bool stop_coex_dm;
 	bool manual_control;
@@ -532,8 +599,9 @@ bool halbtc_is_wifi_uplink(struct rtl_priv *adapter);
 
 extern struct btc_coexist gl_bt_coexist;
 
-bool exhalbtc_initlize_variables(struct rtl_priv *adapter);
-void exhalbtc_init_hw_config(struct btc_coexist *btcoexist);
+bool exhalbtc_initlize_variables(void);
+bool exhalbtc_bind_bt_coex_withadapter(void *adapter);
+void exhalbtc_init_hw_config(struct btc_coexist *btcoexist, bool wifi_only);
 void exhalbtc_init_coex_dm(struct btc_coexist *btcoexist);
 void exhalbtc_ips_notify(struct btc_coexist *btcoexist, u8 type);
 void exhalbtc_lps_notify(struct btc_coexist *btcoexist, u8 type);
@@ -563,5 +631,6 @@ void exhalbtc_signal_compensation(struct btc_coexist *btcoexist,
 				  u8 *rssi_wifi, u8 *rssi_bt);
 void exhalbtc_lps_leave(struct btc_coexist *btcoexist);
 void exhalbtc_low_wifi_traffic_notify(struct btc_coexist *btcoexist);
+void exhalbtc_set_single_ant_path(u8 single_ant_path);
 
 #endif
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/rtl_btc.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/rtl_btc.c
index 46e0fa6be273..4366c9817e1e 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/rtl_btc.c
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/rtl_btc.c
@@ -45,42 +45,79 @@ static struct rtl_btc_ops rtl_btc_operation = {
 	.btc_is_disable_edca_turbo = rtl_btc_is_disable_edca_turbo,
 	.btc_is_bt_disabled = rtl_btc_is_bt_disabled,
 	.btc_special_packet_notify = rtl_btc_special_packet_notify,
+	.btc_record_pwr_mode = rtl_btc_record_pwr_mode,
+	.btc_get_lps_val = rtl_btc_get_lps_val,
+	.btc_get_rpwm_val = rtl_btc_get_rpwm_val,
+	.btc_is_bt_ctrl_lps = rtl_btc_is_bt_ctrl_lps,
+	.btc_is_bt_lps_on = rtl_btc_is_bt_lps_on,
+	.btc_get_ampdu_cfg = rtl_btc_get_ampdu_cfg,
 };
 
-void rtl_btc_init_variables(struct rtl_priv *rtlpriv)
+void rtl_btc_record_pwr_mode(struct rtl_priv *rtlpriv, u8 *buf, u8 len)
 {
-	exhalbtc_initlize_variables(rtlpriv);
+	u8 safe_len;
+
+	safe_len = sizeof(gl_bt_coexist.pwr_mode_val);
+
+	if (safe_len > len)
+		safe_len = len;
+
+	memcpy(gl_bt_coexist.pwr_mode_val, buf, safe_len);
 }
 
-void rtl_btc_init_hal_vars(struct rtl_priv *rtlpriv)
+u8 rtl_btc_get_lps_val(struct rtl_priv *rtlpriv)
 {
-	u8 ant_num;
-	u8 bt_exist;
-	u8 bt_type;
+	return gl_bt_coexist.bt_info.lps_val;
+}
 
-	ant_num = rtl_get_hwpg_ant_num(rtlpriv);
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-		 "%s, antNum is %d\n", __func__, ant_num);
+u8 rtl_btc_get_rpwm_val(struct rtl_priv *rtlpriv)
+{
+	return gl_bt_coexist.bt_info.rpwm_val;
+}
 
-	bt_exist = rtl_get_hwpg_bt_exist(rtlpriv);
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-		 "%s, bt_exist is %d\n", __func__, bt_exist);
-	exhalbtc_set_bt_exist(bt_exist);
+bool rtl_btc_is_bt_ctrl_lps(struct rtl_priv *rtlpriv)
+{
+	return gl_bt_coexist.bt_info.bt_ctrl_lps;
+}
+
+bool rtl_btc_is_bt_lps_on(struct rtl_priv *rtlpriv)
+{
+	return gl_bt_coexist.bt_info.bt_lps_on;
+}
 
-	bt_type = rtl_get_hwpg_bt_type(rtlpriv);
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "%s, bt_type is %d\n",
-		 __func__, bt_type);
-	exhalbtc_set_chip_type(bt_type);
+void rtl_btc_get_ampdu_cfg(struct rtl_priv *rtlpriv, u8 *reject_agg,
+			   u8 *ctrl_agg_size, u8 *agg_size)
+{
+	if (reject_agg)
+		*reject_agg = gl_bt_coexist.bt_info.reject_agg_pkt;
+	if (ctrl_agg_size)
+		*ctrl_agg_size = gl_bt_coexist.bt_info.bt_ctrl_agg_buf_size;
+	if (agg_size)
+		*agg_size = gl_bt_coexist.bt_info.agg_buf_size;
+}
 
-	if (rtlpriv->cfg->mod_params->ant_sel == 1)
-		exhalbtc_set_ant_num(rtlpriv, BT_COEX_ANT_TYPE_DETECTED, 1);
-	else
-		exhalbtc_set_ant_num(rtlpriv, BT_COEX_ANT_TYPE_PG, ant_num);
+void rtl_btc_init_variables(struct rtl_priv *rtlpriv)
+{
+	exhalbtc_initlize_variables();
+	exhalbtc_bind_bt_coex_withadapter(rtlpriv);
+}
+
+void rtl_btc_init_hal_vars(struct rtl_priv *rtlpriv)
+{
+	/* move ant_num, bt_type and single_ant_path to
+	 * exhalbtc_bind_bt_coex_withadapter()
+	 */
 }
 
 void rtl_btc_init_hw_config(struct rtl_priv *rtlpriv)
 {
-	exhalbtc_init_hw_config(&gl_bt_coexist);
+	u8 bt_exist;
+
+	bt_exist = rtl_get_hwpg_bt_exist(rtlpriv);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		"%s, bt_exist is %d\n", __func__, bt_exist);
+
+	exhalbtc_init_hw_config(&gl_bt_coexist, !bt_exist);
 	exhalbtc_init_coex_dm(&gl_bt_coexist);
 }
 
@@ -118,7 +155,9 @@ void rtl_btc_periodical(struct rtl_priv *rtlpriv)
 
 void rtl_btc_halt_notify(void)
 {
-	exhalbtc_halt_notify(&gl_bt_coexist);
+	struct btc_coexist *btcoexist = &gl_bt_coexist;
+
+	exhalbtc_halt_notify(btcoexist);
 }
 
 void rtl_btc_btinfo_notify(struct rtl_priv *rtlpriv, u8 *tmp_buf, u8 length)
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/rtl_btc.h b/drivers/net/wireless/realtek/rtlwifi/btcoexist/rtl_btc.h
index fff5117e1c4e..6fe521cbe7f0 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/rtl_btc.h
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/rtl_btc.h
@@ -43,6 +43,13 @@ bool rtl_btc_is_limited_dig(struct rtl_priv *rtlpriv);
 bool rtl_btc_is_disable_edca_turbo(struct rtl_priv *rtlpriv);
 bool rtl_btc_is_bt_disabled(struct rtl_priv *rtlpriv);
 void rtl_btc_special_packet_notify(struct rtl_priv *rtlpriv, u8 pkt_type);
+void rtl_btc_record_pwr_mode(struct rtl_priv *rtlpriv, u8 *buf, u8 len);
+u8   rtl_btc_get_lps_val(struct rtl_priv *rtlpriv);
+u8   rtl_btc_get_rpwm_val(struct rtl_priv *rtlpriv);
+bool rtl_btc_is_bt_ctrl_lps(struct rtl_priv *rtlpriv);
+bool rtl_btc_is_bt_lps_on(struct rtl_priv *rtlpriv);
+void rtl_btc_get_ampdu_cfg(struct rtl_priv *rtlpriv, u8 *reject_agg,
+			   u8 *ctrl_agg_size, u8 *agg_size);
 
 struct rtl_btc_ops *rtl_btc_get_ops_pointer(void);
 
diff --git a/drivers/net/wireless/realtek/rtlwifi/core.c b/drivers/net/wireless/realtek/rtlwifi/core.c
index a4f8e326a2bc..b0ad061048c5 100644
--- a/drivers/net/wireless/realtek/rtlwifi/core.c
+++ b/drivers/net/wireless/realtek/rtlwifi/core.c
@@ -629,7 +629,8 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 	}
 
 	/*For LPS */
-	if (changed & IEEE80211_CONF_CHANGE_PS) {
+	if ((changed & IEEE80211_CONF_CHANGE_PS) &&
+	    rtlpriv->psc.swctrl_lps && !rtlpriv->psc.fwctrl_lps) {
 		cancel_delayed_work(&rtlpriv->works.ps_work);
 		cancel_delayed_work(&rtlpriv->works.ps_rfon_wq);
 		if (conf->flags & IEEE80211_CONF_PS) {
@@ -1463,6 +1464,9 @@ static void rtl_op_sw_scan_complete(struct ieee80211_hw *hw,
 	RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD, "\n");
 	mac->act_scanning = false;
 	mac->skip_scan = false;
+
+	rtlpriv->btcoexist.btc_info.ap_num = rtlpriv->scan_list.num;
+
 	if (rtlpriv->link_info.higher_busytraffic)
 		return;
 
@@ -1670,6 +1674,8 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		 *so don't use rtl_cam_reset_all_entry
 		 *or clear all entry here.
 		 */
+		rtl_wait_tx_report_acked(hw, 500); /* wait 500ms for TX ack */
+
 		rtl_cam_delete_one_entry(hw, mac_addr, key_idx);
 		break;
 	default:
diff --git a/drivers/net/wireless/realtek/rtlwifi/debug.c b/drivers/net/wireless/realtek/rtlwifi/debug.c
index 7ecac6116d5d..38fef6dbb44b 100644
--- a/drivers/net/wireless/realtek/rtlwifi/debug.c
+++ b/drivers/net/wireless/realtek/rtlwifi/debug.c
@@ -27,7 +27,7 @@
 #include <linux/moduleparam.h>
 
 #ifdef CONFIG_RTLWIFI_DEBUG
-void _rtl_dbg_trace(struct rtl_priv *rtlpriv, int comp, int level,
+void _rtl_dbg_trace(struct rtl_priv *rtlpriv, u64 comp, int level,
 		    const char *fmt, ...)
 {
 	if (unlikely((comp & rtlpriv->cfg->mod_params->debug_mask) &&
diff --git a/drivers/net/wireless/realtek/rtlwifi/debug.h b/drivers/net/wireless/realtek/rtlwifi/debug.h
index bf5339f1c1bc..947718001457 100644
--- a/drivers/net/wireless/realtek/rtlwifi/debug.h
+++ b/drivers/net/wireless/realtek/rtlwifi/debug.h
@@ -105,6 +105,7 @@
 #define COMP_EASY_CONCURRENT	COMP_USB /* reuse of this bit is OK */
 #define COMP_BT_COEXIST			BIT(30)
 #define COMP_IQK			BIT(31)
+#define COMP_TX_REPORT			BIT_ULL(32)
 
 /*--------------------------------------------------------------
 		Define the rt_print components
@@ -169,7 +170,7 @@ enum dbgp_flag_e {
 struct rtl_priv;
 
 __printf(4, 5)
-void _rtl_dbg_trace(struct rtl_priv *rtlpriv, int comp, int level,
+void _rtl_dbg_trace(struct rtl_priv *rtlpriv, u64 comp, int level,
 		    const char *fmt, ...);
 
 __printf(4, 5)
@@ -198,7 +199,7 @@ struct rtl_priv;
 
 __printf(4, 5)
 static inline void RT_TRACE(struct rtl_priv *rtlpriv,
-			    int comp, int level,
+			    u64 comp, int level,
 			    const char *fmt, ...)
 {
 }
@@ -211,7 +212,7 @@ static inline void RTPRINT(struct rtl_priv *rtlpriv,
 }
 
 static inline void RT_PRINT_DATA(struct rtl_priv *rtlpriv,
-				 int comp, int level,
+				 u64 comp, int level,
 				 const char *titlestring,
 				 const void *hexdata, size_t hexdatalen)
 {
diff --git a/drivers/net/wireless/realtek/rtlwifi/pci.c b/drivers/net/wireless/realtek/rtlwifi/pci.c
index 2e6b888bd417..032b6317690d 100644
--- a/drivers/net/wireless/realtek/rtlwifi/pci.c
+++ b/drivers/net/wireless/realtek/rtlwifi/pci.c
@@ -729,14 +729,12 @@ static void _rtl_pci_rx_to_mac80211(struct ieee80211_hw *hw,
 		dev_kfree_skb_any(skb);
 	} else {
 		struct sk_buff *uskb = NULL;
-		u8 *pdata;
 
 		uskb = dev_alloc_skb(skb->len + 128);
 		if (likely(uskb)) {
 			memcpy(IEEE80211_SKB_RXCB(uskb), &rx_status,
 			       sizeof(rx_status));
-			pdata = (u8 *)skb_put(uskb, skb->len);
-			memcpy(pdata, skb->data, skb->len);
+			skb_put_data(uskb, skb->data, skb->len);
 			dev_kfree_skb_any(skb);
 			ieee80211_rx_irqsafe(hw, uskb);
 		} else {
@@ -881,6 +879,9 @@ static void _rtl_pci_rx_interrupt(struct ieee80211_hw *hw)
 				if (unicast)
 					rtlpriv->link_info.num_rx_inperiod++;
 			}
+
+			rtl_collect_scan_list(hw, skb);
+
 			/* static bcn for roaming */
 			rtl_beacon_statistic(hw, skb);
 			rtl_p2p_info(hw, (void *)skb->data, skb->len);
@@ -1826,6 +1827,7 @@ static int rtl_pci_start(struct ieee80211_hw *hw)
 	rtlpci->driver_is_goingto_unload = false;
 	if (rtlpriv->cfg->ops->get_btc_status &&
 	    rtlpriv->cfg->ops->get_btc_status()) {
+		rtlpriv->btcoexist.btc_info.ap_num = 36;
 		rtlpriv->btcoexist.btc_ops->btc_init_variables(rtlpriv);
 		rtlpriv->btcoexist.btc_ops->btc_init_hal_vars(rtlpriv);
 	}
diff --git a/drivers/net/wireless/realtek/rtlwifi/ps.c b/drivers/net/wireless/realtek/rtlwifi/ps.c
index 0d152877d969..07ee3096f50e 100644
--- a/drivers/net/wireless/realtek/rtlwifi/ps.c
+++ b/drivers/net/wireless/realtek/rtlwifi/ps.c
@@ -356,7 +356,7 @@ void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
 	if (mac->link_state != MAC80211_LINKED)
 		return;
 
-	if (ppsc->dot11_psmode == rt_psmode)
+	if (ppsc->dot11_psmode == rt_psmode && rt_psmode == EACTIVE)
 		return;
 
 	/* Update power save mode configured. */
@@ -438,11 +438,13 @@ static void rtl_lps_enter_core(struct ieee80211_hw *hw)
 
 	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
 
-	if (ppsc->dot11_psmode == EACTIVE) {
-		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
-			 "Enter 802.11 power save mode...\n");
-		rtl_lps_set_psmode(hw, EAUTOPS);
-	}
+	/* Don't need to check (ppsc->dot11_psmode == EACTIVE), because
+	 * bt_ccoexist may ask to enter lps.
+	 * In normal case, this constraint move to rtl_lps_set_psmode().
+	 */
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+		 "Enter 802.11 power save mode...\n");
+	rtl_lps_set_psmode(hw, EAUTOPS);
 
 	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
 }
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/fw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/fw.c
index 21ed9ad3be7a..a2eca669873b 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/fw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/fw.c
@@ -620,8 +620,7 @@ void rtl88e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 		      u1rsvdpageloc, 3);
 
 	skb = dev_alloc_skb(totalpacketlen);
-	memcpy(skb_put(skb, totalpacketlen),
-	       &reserved_page_packet, totalpacketlen);
+	skb_put_data(skb, &reserved_page_packet, totalpacketlen);
 
 	rtstatus = rtl_cmd_send_packet(hw, skb);
 
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/reg.h b/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/reg.h
index 15400ee6c04b..0c0d64aea651 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/reg.h
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/reg.h
@@ -248,7 +248,6 @@
 #define REG_RD_NAV_NXT				0x0544
 #define REG_NAV_PROT_LEN			0x0546
 #define REG_BCN_CTRL				0x0550
-#define REG_USTIME_TSF				0x0551
 #define REG_MBID_NUM				0x0552
 #define REG_DUAL_TSF_RST			0x0553
 #define REG_BCN_INTERVAL			0x0554
@@ -256,6 +255,7 @@
 #define REG_DRVERLYINT				0x0558
 #define REG_BCNDMATIM				0x0559
 #define REG_ATIMWND					0x055A
+#define REG_USTIME_TSF				0x055C
 #define REG_BCN_MAX_ERR				0x055D
 #define REG_RXTSF_OFFSET_CCK		0x055E
 #define REG_RXTSF_OFFSET_OFDM		0x055F
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/sw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/sw.c
index 7661cfa53032..774e72058d24 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/sw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/sw.c
@@ -270,7 +270,7 @@ static struct rtl_hal_ops rtl8188ee_hal_ops = {
 
 static struct rtl_mod_params rtl88ee_mod_params = {
 	.sw_crypto = false,
-	.inactiveps = false,
+	.inactiveps = true,
 	.swctrl_lps = false,
 	.fwctrl_lps = false,
 	.msi_support = true,
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192c/fw_common.c b/drivers/net/wireless/realtek/rtlwifi/rtl8192c/fw_common.c
index c7a77467b20e..015476e3f7e5 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192c/fw_common.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192c/fw_common.c
@@ -647,8 +647,7 @@ void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
 
 
 	skb = dev_alloc_skb(totalpacketlen);
-	memcpy((u8 *)skb_put(skb, totalpacketlen),
-	       &reserved_page_packet, totalpacketlen);
+	skb_put_data(skb, &reserved_page_packet, totalpacketlen);
 
 	if (cmd_send_packet)
 		rtstatus = cmd_send_packet(hw, skb);
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192ce/reg.h b/drivers/net/wireless/realtek/rtlwifi/rtl8192ce/reg.h
index 1bb7ed35812d..9e3b58a5d2bb 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192ce/reg.h
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192ce/reg.h
@@ -227,7 +227,6 @@
 #define REG_RD_NAV_NXT				0x0544
 #define REG_NAV_PROT_LEN			0x0546
 #define REG_BCN_CTRL				0x0550
-#define REG_USTIME_TSF				0x0551
 #define REG_MBID_NUM				0x0552
 #define REG_DUAL_TSF_RST			0x0553
 #define REG_BCN_INTERVAL			0x0554
@@ -235,6 +234,7 @@
 #define REG_DRVERLYINT				0x0558
 #define REG_BCNDMATIM				0x0559
 #define REG_ATIMWND				0x055A
+#define REG_USTIME_TSF				0x055C
 #define REG_BCN_MAX_ERR				0x055D
 #define REG_RXTSF_OFFSET_CCK			0x055E
 #define REG_RXTSF_OFFSET_OFDM			0x055F
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192cu/trx.c b/drivers/net/wireless/realtek/rtlwifi/rtl8192cu/trx.c
index 41422e4da8b7..de6c3428f7c6 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192cu/trx.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192cu/trx.c
@@ -512,7 +512,7 @@ void rtl92cu_tx_fill_desc(struct ieee80211_hw *hw,
 
 	seq_number = (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4;
 	rtl_get_tcb_desc(hw, info, sta, skb, tcb_desc);
-	txdesc = (u8 *)skb_push(skb, RTL_TX_HEADER_SIZE);
+	txdesc = skb_push(skb, RTL_TX_HEADER_SIZE);
 	memset(txdesc, 0, RTL_TX_HEADER_SIZE);
 	SET_TX_DESC_PKT_SIZE(txdesc, pktlen);
 	SET_TX_DESC_LINIP(txdesc, 0);
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192de/fw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8192de/fw.c
index 88faeab2574f..f4129cf96e7c 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192de/fw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192de/fw.c
@@ -668,8 +668,7 @@ void rtl92d_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool dl_finished)
 	if (!skb) {
 		dlok = false;
 	} else {
-		memcpy((u8 *) skb_put(skb, totalpacketlen),
-			&reserved_page_packet, totalpacketlen);
+		skb_put_data(skb, &reserved_page_packet, totalpacketlen);
 		rtstatus = _rtl92d_cmd_send_packet(hw, skb);
 
 		if (rtstatus)
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192de/reg.h b/drivers/net/wireless/realtek/rtlwifi/rtl8192de/reg.h
index b354b95936e2..d4c4e76a9244 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192de/reg.h
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192de/reg.h
@@ -255,7 +255,6 @@
 #define REG_RD_NAV_NXT			0x0544
 #define REG_NAV_PROT_LEN		0x0546
 #define REG_BCN_CTRL			0x0550
-#define REG_USTIME_TSF			0x0551
 #define REG_MBID_NUM			0x0552
 #define REG_DUAL_TSF_RST		0x0553
 #define REG_BCN_INTERVAL		0x0554
@@ -263,6 +262,7 @@
 #define REG_DRVERLYINT			0x0558
 #define REG_BCNDMATIM			0x0559
 #define REG_ATIMWND			0x055A
+#define REG_USTIME_TSF			0x055C
 #define REG_BCN_MAX_ERR			0x055D
 #define REG_RXTSF_OFFSET_CCK		0x055E
 #define REG_RXTSF_OFFSET_OFDM		0x055F
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/fw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/fw.c
index 1f42ce5f8f27..f5d4df985c37 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/fw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/fw.c
@@ -422,28 +422,86 @@ void rtl92ee_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 u1_h2c_set_pwrmode[H2C_92E_PWEMODE_LENGTH] = { 0 };
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	u8 rlbm , power_state = 0;
+	u8 rlbm, power_state = 0, byte5 = 0;
+	u8 awake_intvl;	/* DTIM = (awake_intvl - 1) */
+	struct rtl_btc_ops *btc_ops = rtlpriv->btcoexist.btc_ops;
+	bool bt_ctrl_lps = (rtlpriv->cfg->ops->get_btc_status() ?
+			    btc_ops->btc_is_bt_ctrl_lps(rtlpriv) : false);
+	bool bt_lps_on = (rtlpriv->cfg->ops->get_btc_status() ?
+			  btc_ops->btc_is_bt_lps_on(rtlpriv) : false);
+
+	if (bt_ctrl_lps)
+		mode = (bt_lps_on ? FW_PS_MIN_MODE : FW_PS_ACTIVE_MODE);
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG, "FW LPS mode = %d (coex:%d)\n",
+		 mode, bt_ctrl_lps);
+
+	switch (mode) {
+	case FW_PS_MIN_MODE:
+		rlbm = 0;
+		awake_intvl = 2;
+		break;
+	case FW_PS_MAX_MODE:
+		rlbm = 1;
+		awake_intvl = 2;
+		break;
+	case FW_PS_DTIM_MODE:
+		rlbm = 2;
+		awake_intvl = ppsc->reg_max_lps_awakeintvl;
+		/* hw->conf.ps_dtim_period or mac->vif->bss_conf.dtim_period
+		 * is only used in swlps.
+		 */
+		break;
+	default:
+		rlbm = 2;
+		awake_intvl = 4;
+		break;
+	}
 
-	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD , "FW LPS mode = %d\n", mode);
+	if (rtlpriv->mac80211.p2p) {
+		awake_intvl = 2;
+		rlbm = 1;
+	}
+
+	if (mode == FW_PS_ACTIVE_MODE) {
+		byte5 = 0x40;
+		power_state = FW_PWR_STATE_ACTIVE;
+	} else {
+		if (bt_ctrl_lps) {
+			byte5 = btc_ops->btc_get_lps_val(rtlpriv);
+			power_state = btc_ops->btc_get_rpwm_val(rtlpriv);
+
+			if ((rlbm == 2) && (byte5 & BIT(4))) {
+				/* Keep awake interval to 1 to prevent from
+				 * decreasing coex performance
+				 */
+				awake_intvl = 2;
+				rlbm = 2;
+			}
+		} else {
+			byte5 = 0x40;
+			power_state = FW_PWR_STATE_RF_OFF;
+		}
+	}
 
 	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
-	rlbm = 0;/*YJ,temp,120316. FW now not support RLBM=2.*/
 	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
 	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
-					 (rtlpriv->mac80211.p2p) ?
-					 ppsc->smart_ps : 1);
+					 bt_ctrl_lps ? 0 :
+					 ((rtlpriv->mac80211.p2p) ?
+					  ppsc->smart_ps : 1));
 	SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode,
-					       ppsc->reg_max_lps_awakeintvl);
+					       awake_intvl);
 	SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
-	if (mode == FW_PS_ACTIVE_MODE)
-		power_state |= FW_PWR_STATE_ACTIVE;
-	else
-		power_state |= FW_PWR_STATE_RF_OFF;
 	SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
+	SET_H2CCMD_PWRMODE_PARM_BYTE5(u1_h2c_set_pwrmode, byte5);
 
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 		      "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode\n",
 		      u1_h2c_set_pwrmode, H2C_92E_PWEMODE_LENGTH);
+	if (rtlpriv->cfg->ops->get_btc_status())
+		btc_ops->btc_record_pwr_mode(rtlpriv, u1_h2c_set_pwrmode,
+					     H2C_92E_PWEMODE_LENGTH);
 	rtl92ee_fill_h2c_cmd(hw, H2C_92E_SETPWRMODE, H2C_92E_PWEMODE_LENGTH,
 			     u1_h2c_set_pwrmode);
 }
@@ -708,8 +766,7 @@ void rtl92ee_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 		      u1rsvdpageloc, 3);
 
 	skb = dev_alloc_skb(totalpacketlen);
-	memcpy((u8 *)skb_put(skb, totalpacketlen),
-	       &reserved_page_packet, totalpacketlen);
+	skb_put_data(skb, &reserved_page_packet, totalpacketlen);
 
 	b_dlok = true;
 
@@ -843,6 +900,7 @@ void rtl92ee_c2h_content_parsing(struct ieee80211_hw *hw, u8 c2h_cmd_id,
 	case C2H_8192E_TX_REPORT:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE ,
 			 "[C2H], C2H_8723BE_TX_REPORT!\n");
+		rtl_tx_report_handler(hw, tmp_buf, c2h_cmd_len);
 		break;
 	case C2H_8192E_BT_INFO:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/fw.h b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/fw.h
index af8271967a88..b770f722daa6 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/fw.h
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/fw.h
@@ -37,7 +37,7 @@
 #define USE_OLD_WOWLAN_DEBUG_FW 0
 
 #define H2C_92E_RSVDPAGE_LOC_LEN		5
-#define H2C_92E_PWEMODE_LENGTH			5
+#define H2C_92E_PWEMODE_LENGTH			7
 #define H2C_92E_JOINBSSRPT_LENGTH		1
 #define H2C_92E_AP_OFFLOAD_LENGTH		3
 #define H2C_92E_WOWLAN_LENGTH			3
@@ -154,6 +154,8 @@ enum rtl8192e_c2h_evt {
 	SET_BITS_TO_LE_1BYTE((__cmd)+3, 0, 8, __val)
 #define SET_H2CCMD_PWRMODE_PARM_PWR_STATE(__cmd, __val)		\
 	SET_BITS_TO_LE_1BYTE((__cmd)+4, 0, 8, __val)
+#define SET_H2CCMD_PWRMODE_PARM_BYTE5(__cmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__cmd) + 5, 0, 8, __val)
 #define GET_92E_H2CCMD_PWRMODE_PARM_MODE(__cmd)			\
 	LE_BITS_TO_1BYTE(__cmd, 0, 8)
 
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/hw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/hw.c
index 6f5098a18655..d84ac7adfd82 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/hw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/hw.c
@@ -1670,7 +1670,8 @@ void rtl92ee_card_disable(struct ieee80211_hw *hw)
 	_rtl92ee_poweroff_adapter(hw);
 
 	/* after power off we should do iqk again */
-	rtlpriv->phy.iqk_initialized = false;
+	if (!rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->phy.iqk_initialized = false;
 }
 
 void rtl92ee_interrupt_recognized(struct ieee80211_hw *hw,
@@ -2506,7 +2507,7 @@ void rtl92ee_set_key(struct ieee80211_hw *hw, u32 key_index,
 				 "add one entry\n");
 			if (is_pairwise) {
 				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
-					 "set Pairwiase key\n");
+					 "set Pairwise key\n");
 
 				rtl_cam_add_one_entry(hw, macaddr, key_index,
 					       entry_id, enc_algo,
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/sw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/sw.c
index 48820bc497d8..eaa503b7c4b4 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/sw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/sw.c
@@ -253,7 +253,7 @@ static struct rtl_hal_ops rtl8192ee_hal_ops = {
 
 static struct rtl_mod_params rtl92ee_mod_params = {
 	.sw_crypto = false,
-	.inactiveps = false,
+	.inactiveps = true,
 	.swctrl_lps = false,
 	.fwctrl_lps = true,
 	.msi_support = true,
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/trx.c b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/trx.c
index b1864bb07c2c..55f238a2a310 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/trx.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/trx.c
@@ -731,6 +731,9 @@ void rtl92ee_tx_fill_desc(struct ieee80211_hw *hw,
 			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
 		}
 
+		/* tx report */
+		rtl_get_tx_report(ptcb_desc, pdesc, hw);
+
 		SET_TX_DESC_TX_RATE(pdesc, ptcb_desc->hw_rate);
 
 		if (ieee80211_is_mgmt(fc)) {
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/trx.h b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/trx.h
index 8053d1b12ec4..b0105c529010 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/trx.h
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/trx.h
@@ -159,7 +159,7 @@
 #define SET_TX_DESC_NULL_0(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 14, 1, __val)
 #define SET_TX_DESC_NULL_1(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 15, 1, __val)
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 8, 15, 1, __val)
 #define SET_TX_DESC_BK(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 16, 1, __val)
 #define SET_TX_DESC_MORE_FRAG(__pdesc, __val)		\
@@ -167,7 +167,7 @@
 #define SET_TX_DESC_RAW(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 18, 1, __val)
 #define SET_TX_DESC_SPE_RPT(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 19, 1, __val)
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 8, 19, 1, __val)
 #define SET_TX_DESC_AMPDU_DENSITY(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 20, 3, __val)
 #define SET_TX_DESC_BT_NULL(__pdesc, __val)		\
@@ -252,15 +252,15 @@
 
 /* Dword 6 */
 #define SET_TX_DESC_SW_DEFINE(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 12, __val)
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 0, 12, __val)
 #define SET_TX_DESC_ANTSEL_A(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 16, 3, __val)
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 16, 3, __val)
 #define SET_TX_DESC_ANTSEL_B(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 19, 3, __val)
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 19, 3, __val)
 #define SET_TX_DESC_ANTSEL_C(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 22, 3, __val)
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 22, 3, __val)
 #define SET_TX_DESC_ANTSEL_D(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+24, 25, 3, __val)
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 25, 3, __val)
 
 /* Dword 7 */
 #define SET_TX_DESC_TX_BUFFER_SIZE(__pdesc, __val)	\
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192se/fw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8192se/fw.c
index 89a0a28b8b20..e7b1d7c0f542 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192se/fw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192se/fw.c
@@ -188,10 +188,9 @@ static bool _rtl92s_firmware_downloadcode(struct ieee80211_hw *hw,
 		if (!skb)
 			return false;
 		skb_reserve(skb, extra_descoffset);
-		seg_ptr = (u8 *)skb_put(skb, (u32)(frag_length -
-					extra_descoffset));
-		memcpy(seg_ptr, code_virtual_address + frag_offset,
-		       (u32)(frag_length - extra_descoffset));
+		seg_ptr = skb_put_data(skb,
+				       code_virtual_address + frag_offset,
+				       (u32)(frag_length - extra_descoffset));
 
 		tcb_desc = (struct rtl_tcb_desc *)(skb->cb);
 		tcb_desc->queue_index = TXCMD_QUEUE;
@@ -463,7 +462,7 @@ static u32 _rtl92s_fill_h2c_cmd(struct sk_buff *skb, u32 h2cbufferlen,
 			break;
 
 		/* Clear content */
-		ph2c_buffer = (u8 *)skb_put(skb, (u32)len);
+		ph2c_buffer = skb_put(skb, (u32)len);
 		memset((ph2c_buffer + totallen + tx_desclen), 0, len);
 
 		/* CMD len */
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/fw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/fw.c
index a954a87b0ed9..bf9859f74b6f 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/fw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/fw.c
@@ -470,8 +470,7 @@ void rtl8723e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 		      u1rsvdpageloc, 3);
 
 	skb = dev_alloc_skb(totalpacketlen);
-	memcpy((u8 *)skb_put(skb, totalpacketlen),
-	       &reserved_page_packet, totalpacketlen);
+	skb_put_data(skb, &reserved_page_packet, totalpacketlen);
 
 	rtstatus = rtl_cmd_send_packet(hw, skb);
 
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/hw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/hw.c
index 859c045bd37c..5ac7b815648a 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/hw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/hw.c
@@ -2264,7 +2264,7 @@ void rtl8723e_set_key(struct ieee80211_hw *hw, u32 key_index,
 				 "add one entry\n");
 			if (is_pairwise) {
 				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
-					 "set Pairwiase key\n");
+					 "set Pairwise key\n");
 
 				rtl_cam_add_one_entry(hw, macaddr, key_index,
 						      entry_id, enc_algo,
@@ -2313,7 +2313,7 @@ static void rtl8723e_bt_var_init(struct ieee80211_hw *hw)
 		rtlpriv->btcoexist.eeprom_bt_radio_shared;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "BT Coexistance = 0x%x\n",
+		 "BT Coexistence = 0x%x\n",
 		 rtlpriv->btcoexist.bt_coexistence);
 
 	if (rtlpriv->btcoexist.bt_coexistence) {
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/reg.h b/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/reg.h
index 306059f9b9cc..30938cd9fce5 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/reg.h
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/reg.h
@@ -217,7 +217,6 @@
 #define REG_RD_NAV_NXT				0x0544
 #define REG_NAV_PROT_LEN			0x0546
 #define REG_BCN_CTRL				0x0550
-#define REG_USTIME_TSF				0x0551
 #define REG_MBID_NUM				0x0552
 #define REG_DUAL_TSF_RST			0x0553
 #define REG_BCN_INTERVAL			0x0554
@@ -225,6 +224,7 @@
 #define REG_DRVERLYINT				0x0558
 #define REG_BCNDMATIM				0x0559
 #define REG_ATIMWND					0x055A
+#define REG_USTIME_TSF				0x055C
 #define REG_BCN_MAX_ERR				0x055D
 #define REG_RXTSF_OFFSET_CCK		0x055E
 #define REG_RXTSF_OFFSET_OFDM		0x055F
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/fw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/fw.c
index 4fc839b1d601..dd6f95cfaec9 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/fw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/fw.c
@@ -240,27 +240,84 @@ void rtl8723be_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 u1_h2c_set_pwrmode[H2C_PWEMODE_LENGTH] = { 0 };
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	u8 rlbm, power_state = 0;
-	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
+	u8 rlbm, power_state = 0, byte5 = 0;
+	u8 awake_intvl;	/* DTIM = (awake_intvl - 1) */
+	struct rtl_btc_ops *btc_ops = rtlpriv->btcoexist.btc_ops;
+	bool bt_ctrl_lps = (rtlpriv->cfg->ops->get_btc_status() ?
+			    btc_ops->btc_is_bt_ctrl_lps(rtlpriv) : false);
+	bool bt_lps_on = (rtlpriv->cfg->ops->get_btc_status() ?
+			  btc_ops->btc_is_bt_lps_on(rtlpriv) : false);
+
+	if (bt_ctrl_lps)
+		mode = (bt_lps_on ? FW_PS_MIN_MODE : FW_PS_ACTIVE_MODE);
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG, "FW LPS mode = %d (coex:%d)\n",
+		 mode, bt_ctrl_lps);
+
+	switch (mode) {
+	case FW_PS_MIN_MODE:
+		rlbm = 0;
+		awake_intvl = 2;
+		break;
+	case FW_PS_MAX_MODE:
+		rlbm = 1;
+		awake_intvl = 2;
+		break;
+	case FW_PS_DTIM_MODE:
+		rlbm = 2;
+		awake_intvl = ppsc->reg_max_lps_awakeintvl;
+		/* hw->conf.ps_dtim_period or mac->vif->bss_conf.dtim_period
+		 * is only used in swlps.
+		 */
+		break;
+	default:
+		rlbm = 2;
+		awake_intvl = 4;
+		break;
+	}
+
+	if (rtlpriv->mac80211.p2p) {
+		awake_intvl = 2;
+		rlbm = 1;
+	}
+
+	if (mode == FW_PS_ACTIVE_MODE) {
+		byte5 = 0x40;
+		power_state = FW_PWR_STATE_ACTIVE;
+	} else {
+		if (bt_ctrl_lps) {
+			byte5 = btc_ops->btc_get_lps_val(rtlpriv);
+			power_state = btc_ops->btc_get_rpwm_val(rtlpriv);
+
+			if ((rlbm == 2) && (byte5 & BIT(4))) {
+				/* Keep awake interval to 1 to prevent from
+				 * decreasing coex performance
+				 */
+				awake_intvl = 2;
+				rlbm = 2;
+			}
+		} else {
+			byte5 = 0x40;
+			power_state = FW_PWR_STATE_RF_OFF;
+		}
+	}
 
 	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
-	rlbm = 0;/*YJ,temp,120316. FW now not support RLBM=2.*/
 	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
 	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
-					 (rtlpriv->mac80211.p2p) ?
-					  ppsc->smart_ps : 1);
+					 bt_ctrl_lps ? 0 : ppsc->smart_ps);
 	SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode,
-					       ppsc->reg_max_lps_awakeintvl);
+					       awake_intvl);
 	SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
-	if (mode == FW_PS_ACTIVE_MODE)
-		power_state |= FW_PWR_STATE_ACTIVE;
-	else
-		power_state |= FW_PWR_STATE_RF_OFF;
 	SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
+	SET_H2CCMD_PWRMODE_PARM_BYTE5(u1_h2c_set_pwrmode, byte5);
 
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 		      "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode\n",
 		      u1_h2c_set_pwrmode, H2C_PWEMODE_LENGTH);
+	if (rtlpriv->cfg->ops->get_btc_status())
+		btc_ops->btc_record_pwr_mode(rtlpriv, u1_h2c_set_pwrmode,
+					     H2C_PWEMODE_LENGTH);
 	rtl8723be_fill_h2c_cmd(hw, H2C_8723B_SETPWRMODE, H2C_PWEMODE_LENGTH,
 			       u1_h2c_set_pwrmode);
 }
@@ -527,8 +584,7 @@ void rtl8723be_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
 		      u1rsvdpageloc, sizeof(u1rsvdpageloc));
 
 	skb = dev_alloc_skb(totalpacketlen);
-	memcpy((u8 *)skb_put(skb, totalpacketlen),
-	       &reserved_page_packet, totalpacketlen);
+	skb_put_data(skb, &reserved_page_packet, totalpacketlen);
 
 	rtstatus = rtl_cmd_send_packet(hw, skb);
 
@@ -662,6 +718,7 @@ void rtl8723be_c2h_content_parsing(struct ieee80211_hw *hw,
 	case C2H_8723B_TX_REPORT:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
 			 "[C2H], C2H_8723BE_TX_REPORT!\n");
+		rtl_tx_report_handler(hw, tmp_buf, c2h_cmd_len);
 		break;
 	case C2H_8723B_BT_INFO:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/fw.h b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/fw.h
index 2482b3bc2bfa..2de4edb62bca 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/fw.h
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/fw.h
@@ -33,7 +33,7 @@
 
 #define USE_OLD_WOWLAN_DEBUG_FW			0
 
-#define H2C_PWEMODE_LENGTH			5
+#define H2C_PWEMODE_LENGTH			7
 
 /* Fw PS state for RPWM.
 *BIT[2:0] = HW state
@@ -125,6 +125,8 @@ enum rtl8723b_c2h_evt {
 	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+3, 0, 8, __val)
 #define SET_H2CCMD_PWRMODE_PARM_PWR_STATE(__ph2ccmd, __val)		\
 	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+4, 0, 8, __val)
+#define SET_H2CCMD_PWRMODE_PARM_BYTE5(__ph2ccmd, __val)			\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd) + 5, 0, 8, __val)
 #define GET_88E_H2CCMD_PWRMODE_PARM_MODE(__ph2ccmd)			\
 	LE_BITS_TO_1BYTE(__ph2ccmd, 0, 8)
 
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/hw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/hw.c
index 1acbfb86472c..2a7ad5ffe997 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/hw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/hw.c
@@ -846,6 +846,9 @@ static bool _rtl8723be_init_mac(struct ieee80211_hw *hw)
 		return false;
 	}
 
+	if (rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->btcoexist.btc_ops->btc_power_on_setting(rtlpriv);
+
 	bytetmp = rtl_read_byte(rtlpriv, REG_MULTI_FUNC_CTRL);
 	rtl_write_byte(rtlpriv, REG_MULTI_FUNC_CTRL, bytetmp | BIT(3));
 
@@ -1440,7 +1443,9 @@ int rtl8723be_hw_init(struct ieee80211_hw *hw)
 		 */
 		if (rtlpriv->btcoexist.btc_info.ant_num == ANT_X2 ||
 		    !rtlpriv->cfg->ops->get_btc_status()) {
-			rtl8723be_phy_iq_calibrate(hw, false);
+			rtl8723be_phy_iq_calibrate(hw,
+						   (rtlphy->iqk_initialized ?
+						    true : false));
 			rtlphy->iqk_initialized = true;
 		}
 		rtl8723be_dm_check_txpower_tracking(hw);
@@ -1674,7 +1679,8 @@ void rtl8723be_card_disable(struct ieee80211_hw *hw)
 	_rtl8723be_poweroff_adapter(hw);
 
 	/* after power off we should do iqk again */
-	rtlpriv->phy.iqk_initialized = false;
+	if (!rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->phy.iqk_initialized = false;
 }
 
 void rtl8723be_interrupt_recognized(struct ieee80211_hw *hw,
@@ -2637,7 +2643,7 @@ void rtl8723be_set_key(struct ieee80211_hw *hw, u32 key_index,
 				 "add one entry\n");
 			if (is_pairwise) {
 				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
-					 "set Pairwiase key\n");
+					 "set Pairwise key\n");
 
 				rtl_cam_add_one_entry(hw, macaddr, key_index,
 					       entry_id, enc_algo,
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/phy.c b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/phy.c
index ab0f39e46e1b..9752175cc466 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/phy.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/phy.c
@@ -2352,7 +2352,7 @@ void rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
 	if (b_recovery) {
 		rtl8723_phy_reload_adda_registers(hw, iqk_bb_reg,
 						  rtlphy->iqk_bb_backup, 9);
-		return;
+		goto label_done;
 	}
 	/* Save RF Path */
 	path_sel_bb = rtl_get_bbreg(hw, 0x948, MASKDWORD);
@@ -2460,6 +2460,7 @@ void rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
 	rtl_set_bbreg(hw, 0x948, MASKDWORD, path_sel_bb);
 	/* rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, 0xfffff, path_sel_rf); */
 
+label_done:
 	spin_lock(&rtlpriv->locks.iqk_lock);
 	rtlphy->lck_inprogress = false;
 	spin_unlock(&rtlpriv->locks.iqk_lock);
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/reg.h b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/reg.h
index 03581d2a5da0..95c4f8e206c7 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/reg.h
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/reg.h
@@ -261,7 +261,6 @@
 #define REG_RD_NAV_NXT				0x0544
 #define REG_NAV_PROT_LEN			0x0546
 #define REG_BCN_CTRL				0x0550
-#define REG_USTIME_TSF				0x0551
 #define REG_MBID_NUM				0x0552
 #define REG_DUAL_TSF_RST			0x0553
 #define REG_BCN_INTERVAL			0x0554
@@ -269,6 +268,7 @@
 #define REG_DRVERLYINT				0x0558
 #define REG_BCNDMATIM				0x0559
 #define REG_ATIMWND				0x055A
+#define REG_USTIME_TSF				0x055C
 #define REG_BCN_MAX_ERR				0x055D
 #define REG_RXTSF_OFFSET_CCK			0x055E
 #define REG_RXTSF_OFFSET_OFDM			0x055F
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/sw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/sw.c
index 8c0ac96b5430..f9d10f1e7cf8 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/sw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/sw.c
@@ -155,8 +155,8 @@ int rtl8723be_init_sw_vars(struct ieee80211_hw *hw)
 		 rtlpriv->cfg->mod_params->disable_watchdog;
 	if (rtlpriv->cfg->mod_params->disable_watchdog)
 		pr_info("watchdog disabled\n");
-	rtlpriv->psc.reg_fwctrl_lps = 3;
-	rtlpriv->psc.reg_max_lps_awakeintvl = 5;
+	rtlpriv->psc.reg_fwctrl_lps = 2;
+	rtlpriv->psc.reg_max_lps_awakeintvl = 2;
 	/* for ASPM, you can close aspm through
 	 * set const_support_pciaspm = 0
 	 */
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/trx.c b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/trx.c
index 3c6ce994c6aa..0e8944119652 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/trx.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/trx.c
@@ -488,6 +488,9 @@ void rtl8723be_tx_fill_desc(struct ieee80211_hw *hw,
 			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
 		}
 
+		/* tx report */
+		rtl_get_tx_report(ptcb_desc, pdesc, hw);
+
 		/* ptcb_desc->use_driver_rate = true; */
 		SET_TX_DESC_TX_RATE(pdesc, ptcb_desc->hw_rate);
 		if (ptcb_desc->hw_rate > DESC92C_RATEMCS0)
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/trx.h b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/trx.h
index 8a9fe41ac15b..0274659f48ed 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/trx.h
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/trx.h
@@ -107,7 +107,7 @@
 #define SET_TX_DESC_RDG_ENABLE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 13, 1, __val)
 #define SET_TX_DESC_BAR_RTY_TH(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 14, 2, __val)
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 8, 14, 2, __val)
 #define SET_TX_DESC_AGG_BREAK(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 16, 1, __val)
 #define SET_TX_DESC_MORE_FRAG(__pdesc, __val)		\
@@ -115,7 +115,7 @@
 #define SET_TX_DESC_RAW(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 18, 1, __val)
 #define SET_TX_DESC_SPE_RPT(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 19, 1, __val)
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 8, 19, 1, __val)
 #define SET_TX_DESC_AMPDU_DENSITY(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 20, 3, __val)
 #define SET_TX_DESC_BT_INT(__pdesc, __val)		\
@@ -187,6 +187,18 @@
 #define SET_TX_DESC_RTS_SC(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 13, 4, __val)
 
+#define SET_TX_DESC_SW_DEFINE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 0, 12, __val)
+#define SET_TX_DESC_MBSSID(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 12, 4, __val)
+#define SET_TX_DESC_ANTSEL_A(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 16, 3, __val)
+#define SET_TX_DESC_ANTSEL_B(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 19, 3, __val)
+#define SET_TX_DESC_ANTSEL_C(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 22, 3, __val)
+#define SET_TX_DESC_ANTSEL_D(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 25, 3, __val)
 
 #define SET_TX_DESC_TX_BUFFER_SIZE(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 16, __val)
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/fw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/fw.c
index 73350103b736..03259aa150fd 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/fw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/fw.c
@@ -489,29 +489,86 @@ void rtl8821ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 u1_h2c_set_pwrmode[H2C_8821AE_PWEMODE_LENGTH] = { 0 };
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	u8 rlbm, power_state = 0;
+	u8 rlbm, power_state = 0, byte5 = 0;
+	u8 awake_intvl;	/* DTIM = (awake_intvl - 1) */
+	struct rtl_btc_ops *btc_ops = rtlpriv->btcoexist.btc_ops;
+	bool bt_ctrl_lps = (rtlpriv->cfg->ops->get_btc_status() ?
+			    btc_ops->btc_is_bt_ctrl_lps(rtlpriv) : false);
+	bool bt_lps_on = (rtlpriv->cfg->ops->get_btc_status() ?
+			  btc_ops->btc_is_bt_lps_on(rtlpriv) : false);
+
+	if (bt_ctrl_lps)
+		mode = (bt_lps_on ? FW_PS_MIN_MODE : FW_PS_ACTIVE_MODE);
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG, "FW LPS mode = %d (coex:%d)\n",
+		 mode, bt_ctrl_lps);
+
+	switch (mode) {
+	case FW_PS_MIN_MODE:
+		rlbm = 0;
+		awake_intvl = 2;
+		break;
+	case FW_PS_MAX_MODE:
+		rlbm = 1;
+		awake_intvl = 2;
+		break;
+	case FW_PS_DTIM_MODE:
+		rlbm = 2;
+		awake_intvl = ppsc->reg_max_lps_awakeintvl;
+		/* hw->conf.ps_dtim_period or mac->vif->bss_conf.dtim_period
+		 * is only used in swlps.
+		 */
+		break;
+	default:
+		rlbm = 2;
+		awake_intvl = 4;
+		break;
+	}
 
-	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
+	if (rtlpriv->mac80211.p2p) {
+		awake_intvl = 2;
+		rlbm = 1;
+	}
+
+	if (mode == FW_PS_ACTIVE_MODE) {
+		byte5 = 0x40;
+		power_state = FW_PWR_STATE_ACTIVE;
+	} else {
+		if (bt_ctrl_lps) {
+			byte5 = btc_ops->btc_get_lps_val(rtlpriv);
+			power_state = btc_ops->btc_get_rpwm_val(rtlpriv);
+
+			if ((rlbm == 2) && (byte5 & BIT(4))) {
+				/* Keep awake interval to 1 to prevent from
+				 * decreasing coex performance
+				 */
+				awake_intvl = 2;
+				rlbm = 2;
+			}
+		} else {
+			byte5 = 0x40;
+			power_state = FW_PWR_STATE_RF_OFF;
+		}
+	}
 
 	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
-	rlbm = 0;/*YJ,temp,120316. FW now not support RLBM=2.*/
 	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
 	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
-					 (rtlpriv->mac80211.p2p) ?
-					 ppsc->smart_ps : 1);
+					 bt_ctrl_lps ? 0 :
+					 ((rtlpriv->mac80211.p2p) ?
+					  ppsc->smart_ps : 1));
 	SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode,
-					       ppsc->reg_max_lps_awakeintvl);
+					       awake_intvl);
 	SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
-	if (mode == FW_PS_ACTIVE_MODE)
-		power_state |= FW_PWR_STATE_ACTIVE;
-	else
-		power_state |= FW_PWR_STATE_RF_OFF;
-
 	SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
+	SET_H2CCMD_PWRMODE_PARM_BYTE5(u1_h2c_set_pwrmode, byte5);
 
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 		      "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode\n",
 		      u1_h2c_set_pwrmode, H2C_8821AE_PWEMODE_LENGTH);
+	if (rtlpriv->cfg->ops->get_btc_status())
+		btc_ops->btc_record_pwr_mode(rtlpriv, u1_h2c_set_pwrmode,
+					     H2C_8821AE_PWEMODE_LENGTH);
 	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_SETPWRMODE,
 			       H2C_8821AE_PWEMODE_LENGTH,
 			       u1_h2c_set_pwrmode);
@@ -1588,8 +1645,7 @@ out:
 		      &reserved_page_packet_8812[0], totalpacketlen);
 
 	skb = dev_alloc_skb(totalpacketlen);
-	memcpy((u8 *)skb_put(skb, totalpacketlen),
-	       &reserved_page_packet_8812, totalpacketlen);
+	skb_put_data(skb, &reserved_page_packet_8812, totalpacketlen);
 
 	rtstatus = rtl_cmd_send_packet(hw, skb);
 
@@ -1725,8 +1781,7 @@ out:
 		      &reserved_page_packet_8821[0], totalpacketlen);
 
 	skb = dev_alloc_skb(totalpacketlen);
-	memcpy((u8 *)skb_put(skb, totalpacketlen),
-	       &reserved_page_packet_8821, totalpacketlen);
+	skb_put_data(skb, &reserved_page_packet_8821, totalpacketlen);
 
 	rtstatus = rtl_cmd_send_packet(hw, skb);
 
@@ -1873,6 +1928,9 @@ void rtl8821ae_c2h_content_parsing(struct ieee80211_hw *hw,
 	case C2H_8812_DBG:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "[C2H], C2H_8812_DBG!!\n");
 		break;
+	case C2H_8812_TX_REPORT:
+		rtl_tx_report_handler(hw, tmp_buf, c2h_cmd_len);
+		break;
 	case C2H_8812_RA_RPT:
 		rtl8821ae_c2h_ra_report_handler(hw, tmp_buf, c2h_cmd_len);
 		break;
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/fw.h b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/fw.h
index 98d871afd92a..32d46d7128f5 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/fw.h
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/fw.h
@@ -42,7 +42,7 @@
 #define USE_OLD_WOWLAN_DEBUG_FW 0
 
 #define H2C_8821AE_RSVDPAGE_LOC_LEN		5
-#define H2C_8821AE_PWEMODE_LENGTH			5
+#define H2C_8821AE_PWEMODE_LENGTH			7
 #define H2C_8821AE_JOINBSSRPT_LENGTH		1
 #define H2C_8821AE_AP_OFFLOAD_LENGTH		3
 #define H2C_8821AE_WOWLAN_LENGTH			3
@@ -218,6 +218,8 @@ enum rtl8821a_h2c_cmd {
 	SET_BITS_TO_LE_1BYTE((__cmd)+3, 0, 8, __value)
 #define SET_H2CCMD_PWRMODE_PARM_PWR_STATE(__cmd, __value)	\
 	SET_BITS_TO_LE_1BYTE((__cmd)+4, 0, 8, __value)
+#define SET_H2CCMD_PWRMODE_PARM_BYTE5(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE((__cmd) + 5, 0, 8, __value)
 #define GET_8821AE_H2CCMD_PWRMODE_PARM_MODE(__cmd)		\
 	LE_BITS_TO_1BYTE(__cmd, 0, 8)
 
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/reg.h b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/reg.h
index ed69dbe178ff..db8bc8a2de61 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/reg.h
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/reg.h
@@ -267,7 +267,6 @@
 #define REG_RD_NAV_NXT				0x0544
 #define REG_NAV_PROT_LEN			0x0546
 #define REG_BCN_CTRL				0x0550
-#define REG_USTIME_TSF				0x0551
 #define REG_MBID_NUM				0x0552
 #define REG_DUAL_TSF_RST			0x0553
 #define REG_BCN_INTERVAL			0x0554
@@ -275,6 +274,7 @@
 #define REG_DRVERLYINT				0x0558
 #define REG_BCNDMATIM				0x0559
 #define REG_ATIMWND				0x055A
+#define REG_USTIME_TSF				0x055C
 #define REG_BCN_MAX_ERR				0x055D
 #define REG_RXTSF_OFFSET_CCK			0x055E
 #define REG_RXTSF_OFFSET_OFDM			0x055F
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/sw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/sw.c
index abaf34cb1433..d71d2776ca03 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/sw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/sw.c
@@ -172,8 +172,8 @@ int rtl8821ae_init_sw_vars(struct ieee80211_hw *hw)
 		rtlpriv->cfg->mod_params->disable_watchdog;
 	if (rtlpriv->cfg->mod_params->disable_watchdog)
 		pr_info("watchdog disabled\n");
-	rtlpriv->psc.reg_fwctrl_lps = 3;
-	rtlpriv->psc.reg_max_lps_awakeintvl = 5;
+	rtlpriv->psc.reg_fwctrl_lps = 2;
+	rtlpriv->psc.reg_max_lps_awakeintvl = 2;
 
 	/* for ASPM, you can close aspm through
 	 * set const_support_pciaspm = 0
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/trx.c b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/trx.c
index 03665e82065f..749818929e8f 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/trx.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/trx.c
@@ -740,6 +740,9 @@ void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
 			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
 		}
 
+		/* tx report */
+		rtl_get_tx_report(ptcb_desc, pdesc, hw);
+
 		/* ptcb_desc->use_driver_rate = true; */
 		SET_TX_DESC_TX_RATE(pdesc, ptcb_desc->hw_rate);
 		if (ptcb_desc->hw_rate > DESC_RATEMCS0)
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/trx.h b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/trx.h
index b6f3c564b8d1..9843a616dcec 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/trx.h
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/trx.h
@@ -114,7 +114,7 @@
 #define SET_TX_DESC_RAW(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 18, 1, __val)
 #define SET_TX_DESC_SPE_RPT(__pdesc, __val)			\
-	SET_BITS_TO_LE_4BYTE(__pdesc+8, 19, 1, __val)
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 8, 19, 1, __val)
 #define SET_TX_DESC_AMPDU_DENSITY(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 20, 3, __val)
 #define SET_TX_DESC_BT_INT(__pdesc, __val)	\
@@ -185,8 +185,21 @@
 #define SET_TX_DESC_RTS_SC(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 13, 4, __val)
 
+#define SET_TX_DESC_SW_DEFINE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 0, 12, __val)
+#define SET_TX_DESC_ANTSEL_A(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 16, 3, __val)
+#define SET_TX_DESC_ANTSEL_B(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 19, 3, __val)
+#define SET_TX_DESC_ANTSEL_C(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 22, 3, __val)
+#define SET_TX_DESC_ANTSEL_D(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 24, 25, 3, __val)
+#define SET_TX_DESC_MBSSID(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(i(__pdesc) + 24, 12, 4, __val)
+
 #define SET_TX_DESC_TX_BUFFER_SIZE(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 16, __val)
+	SET_BITS_TO_LE_4BYTE((__pdesc) + 28, 0, 16, __val)
 
 #define GET_TX_DESC_TX_BUFFER_SIZE(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+28, 0, 16)
diff --git a/drivers/net/wireless/realtek/rtlwifi/usb.c b/drivers/net/wireless/realtek/rtlwifi/usb.c
index 4d989b8ab185..5590d07d0918 100644
--- a/drivers/net/wireless/realtek/rtlwifi/usb.c
+++ b/drivers/net/wireless/realtek/rtlwifi/usb.c
@@ -653,7 +653,7 @@ static void _rtl_rx_completed(struct urb *_urb)
 		/* reserve some space for mac80211's radiotap */
 		skb_reserve(skb, __RADIO_TAP_SIZE_RSV);
 
-		memcpy(skb_put(skb, size), _urb->transfer_buffer, size);
+		skb_put_data(skb, _urb->transfer_buffer, size);
 
 		skb_queue_tail(&rtlusb->rx_queue, skb);
 		tasklet_schedule(&rtlusb->rx_work_tasklet);
diff --git a/drivers/net/wireless/realtek/rtlwifi/wifi.h b/drivers/net/wireless/realtek/rtlwifi/wifi.h
index c0d2601bc55f..fb1ebb01133f 100644
--- a/drivers/net/wireless/realtek/rtlwifi/wifi.h
+++ b/drivers/net/wireless/realtek/rtlwifi/wifi.h
@@ -1886,6 +1886,13 @@ struct rtl_efuse {
 	u8 channel_plan;
 };
 
+struct rtl_tx_report {
+	atomic_t sn;
+	u16 last_sent_sn;
+	unsigned long last_sent_time;
+	u16 last_recv_sn;
+};
+
 struct rtl_ps_ctl {
 	bool pwrdomain_protect;
 	bool in_powersavemode;
@@ -2075,6 +2082,8 @@ struct rtl_tcb_desc {
 	u8 use_driver_rate:1;
 	u8 disable_ratefallback:1;
 
+	u8 use_spe_rpt:1;
+
 	u8 ratr_index;
 	u8 mac_id;
 	u8 hw_rate;
@@ -2324,6 +2333,7 @@ struct rtl_locks {
 	spinlock_t entry_list_lock;
 	spinlock_t usb_lock;
 	spinlock_t c2hcmd_lock;
+	spinlock_t scan_list_lock; /* lock for the scan list */
 
 	/*FW clock change */
 	spinlock_t fw_ps_lock;
@@ -2472,6 +2482,9 @@ struct rtl_btc_info {
 	u8 btcoexist;
 	u8 ant_num;
 	u8 single_ant_path;
+
+	u8 ap_num;
+	bool in_4way;
 };
 
 struct bt_coexist_info {
@@ -2534,6 +2547,7 @@ struct bt_coexist_info {
 struct rtl_btc_ops {
 	void (*btc_init_variables) (struct rtl_priv *rtlpriv);
 	void (*btc_init_hal_vars) (struct rtl_priv *rtlpriv);
+	void (*btc_power_on_setting)(struct rtl_priv *rtlpriv);
 	void (*btc_init_hw_config) (struct rtl_priv *rtlpriv);
 	void (*btc_ips_notify) (struct rtl_priv *rtlpriv, u8 type);
 	void (*btc_lps_notify)(struct rtl_priv *rtlpriv, u8 type);
@@ -2550,6 +2564,13 @@ struct rtl_btc_ops {
 	bool (*btc_is_bt_disabled) (struct rtl_priv *rtlpriv);
 	void (*btc_special_packet_notify)(struct rtl_priv *rtlpriv,
 					  u8 pkt_type);
+	void (*btc_record_pwr_mode)(struct rtl_priv *rtlpriv, u8 *buf, u8 len);
+	u8   (*btc_get_lps_val)(struct rtl_priv *rtlpriv);
+	u8   (*btc_get_rpwm_val)(struct rtl_priv *rtlpriv);
+	bool (*btc_is_bt_ctrl_lps)(struct rtl_priv *rtlpriv);
+	void (*btc_get_ampdu_cfg)(struct rtl_priv *rtlpriv, u8 *reject_agg,
+				  u8 *ctrl_agg_size, u8 *agg_size);
+	bool (*btc_is_bt_lps_on)(struct rtl_priv *rtlpriv);
 };
 
 struct proxim {
@@ -2568,6 +2589,17 @@ struct rtl_c2hcmd {
 	u8 *val;
 };
 
+struct rtl_bssid_entry {
+	struct list_head list;
+	u8 bssid[ETH_ALEN];
+	u32 age;
+};
+
+struct rtl_scan_list {
+	int num;
+	struct list_head list;	/* sort by age */
+};
+
 struct rtl_priv {
 	struct ieee80211_hw *hw;
 	struct completion firmware_loading_complete;
@@ -2588,6 +2620,8 @@ struct rtl_priv {
 	struct rtl_security sec;
 	struct rtl_efuse efuse;
 	struct rtl_led_ctl ledctl;
+	struct rtl_tx_report tx_report;
+	struct rtl_scan_list scan_list;
 
 	struct rtl_ps_ctl psc;
 	struct rate_adaptive ra;
diff --git a/drivers/net/wireless/rsi/Makefile b/drivers/net/wireless/rsi/Makefile
index 25828b692756..a475c813674a 100644
--- a/drivers/net/wireless/rsi/Makefile
+++ b/drivers/net/wireless/rsi/Makefile
@@ -2,7 +2,7 @@ rsi_91x-y			+= rsi_91x_main.o
 rsi_91x-y			+= rsi_91x_core.o
 rsi_91x-y			+= rsi_91x_mac80211.o
 rsi_91x-y			+= rsi_91x_mgmt.o
-rsi_91x-y			+= rsi_91x_pkt.o
+rsi_91x-y			+= rsi_91x_hal.o
 rsi_91x-$(CONFIG_RSI_DEBUGFS)	+= rsi_91x_debugfs.o
 
 rsi_usb-y			+= rsi_91x_usb.o rsi_91x_usb_ops.o
diff --git a/drivers/net/wireless/rsi/rsi_91x_core.c b/drivers/net/wireless/rsi/rsi_91x_core.c
index f3d3995d8f6b..68f04a76769e 100644
--- a/drivers/net/wireless/rsi/rsi_91x_core.c
+++ b/drivers/net/wireless/rsi/rsi_91x_core.c
@@ -306,7 +306,7 @@ void rsi_core_qos_processor(struct rsi_common *common)
 		tstamp_2 = jiffies;
 		mutex_unlock(&common->tx_rxlock);
 
-		if (tstamp_2 > tstamp_1 + (300 * HZ / 1000))
+		if (time_after(tstamp_2, tstamp_1 + (300 * HZ) / 1000))
 			schedule();
 	}
 }
diff --git a/drivers/net/wireless/rsi/rsi_91x_debugfs.c b/drivers/net/wireless/rsi/rsi_91x_debugfs.c
index 828a042f903f..4c0a493bd44e 100644
--- a/drivers/net/wireless/rsi/rsi_91x_debugfs.c
+++ b/drivers/net/wireless/rsi/rsi_91x_debugfs.c
@@ -125,7 +125,9 @@ static int rsi_stats_read(struct seq_file *seq, void *data)
 	struct rsi_common *common = seq->private;
 
 	unsigned char fsm_state[][32] = {
+		"FSM_FW_NOT_LOADED",
 		"FSM_CARD_NOT_READY",
+		"FSM_COMMON_DEV_PARAMS_SENT",
 		"FSM_BOOT_PARAMS_SENT",
 		"FSM_EEPROM_READ_MAC_ADDR",
 		"FSM_RESET_MAC_SENT",
diff --git a/drivers/net/wireless/rsi/rsi_91x_hal.c b/drivers/net/wireless/rsi/rsi_91x_hal.c
new file mode 100644
index 000000000000..c2303599c12e
--- /dev/null
+++ b/drivers/net/wireless/rsi/rsi_91x_hal.c
@@ -0,0 +1,742 @@
+/**
+ * Copyright (c) 2014 Redpine Signals 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/firmware.h>
+#include "rsi_mgmt.h"
+#include "rsi_hal.h"
+#include "rsi_sdio.h"
+
+/* FLASH Firmware */
+static struct ta_metadata metadata_flash_content[] = {
+	{"flash_content", 0x00010000},
+	{"rsi/rs9113_wlan_qspi.rps", 0x00010000},
+};
+
+/**
+ * rsi_send_data_pkt() - This function sends the recieved data packet from
+ *			 driver to device.
+ * @common: Pointer to the driver private structure.
+ * @skb: Pointer to the socket buffer structure.
+ *
+ * Return: status: 0 on success, -1 on failure.
+ */
+int rsi_send_data_pkt(struct rsi_common *common, struct sk_buff *skb)
+{
+	struct rsi_hw *adapter = common->priv;
+	struct ieee80211_hdr *tmp_hdr;
+	struct ieee80211_tx_info *info;
+	struct skb_info *tx_params;
+	struct ieee80211_bss_conf *bss;
+	int status;
+	u8 ieee80211_size = MIN_802_11_HDR_LEN;
+	u8 extnd_size;
+	__le16 *frame_desc;
+	u16 seq_num;
+
+	info = IEEE80211_SKB_CB(skb);
+	bss = &info->control.vif->bss_conf;
+	tx_params = (struct skb_info *)info->driver_data;
+
+	if (!bss->assoc) {
+		status = -EINVAL;
+		goto err;
+	}
+
+	tmp_hdr = (struct ieee80211_hdr *)&skb->data[0];
+	seq_num = (le16_to_cpu(tmp_hdr->seq_ctrl) >> 4);
+
+	extnd_size = ((uintptr_t)skb->data & 0x3);
+
+	if ((FRAME_DESC_SZ + extnd_size) > skb_headroom(skb)) {
+		rsi_dbg(ERR_ZONE, "%s: Unable to send pkt\n", __func__);
+		status = -ENOSPC;
+		goto err;
+	}
+
+	skb_push(skb, (FRAME_DESC_SZ + extnd_size));
+	frame_desc = (__le16 *)&skb->data[0];
+	memset((u8 *)frame_desc, 0, FRAME_DESC_SZ);
+
+	if (ieee80211_is_data_qos(tmp_hdr->frame_control)) {
+		ieee80211_size += 2;
+		frame_desc[6] |= cpu_to_le16(BIT(12));
+	}
+
+	if ((!(info->flags & IEEE80211_TX_INTFL_DONT_ENCRYPT)) &&
+	    (common->secinfo.security_enable)) {
+		if (rsi_is_cipher_wep(common))
+			ieee80211_size += 4;
+		else
+			ieee80211_size += 8;
+		frame_desc[6] |= cpu_to_le16(BIT(15));
+	}
+
+	frame_desc[0] = cpu_to_le16((skb->len - FRAME_DESC_SZ) |
+				    (RSI_WIFI_DATA_Q << 12));
+	frame_desc[2] = cpu_to_le16((extnd_size) | (ieee80211_size) << 8);
+
+	if (common->min_rate != 0xffff) {
+		/* Send fixed rate */
+		frame_desc[3] = cpu_to_le16(RATE_INFO_ENABLE);
+		frame_desc[4] = cpu_to_le16(common->min_rate);
+
+		if (conf_is_ht40(&common->priv->hw->conf))
+			frame_desc[5] = cpu_to_le16(FULL40M_ENABLE);
+
+		if (common->vif_info[0].sgi) {
+			if (common->min_rate & 0x100) /* Only MCS rates */
+				frame_desc[4] |=
+					cpu_to_le16(ENABLE_SHORTGI_RATE);
+		}
+
+	}
+
+	frame_desc[6] |= cpu_to_le16(seq_num & 0xfff);
+	frame_desc[7] = cpu_to_le16(((tx_params->tid & 0xf) << 4) |
+				    (skb->priority & 0xf) |
+				    (tx_params->sta_id << 8));
+
+	status = adapter->host_intf_ops->write_pkt(common->priv, skb->data,
+						   skb->len);
+	if (status)
+		rsi_dbg(ERR_ZONE, "%s: Failed to write pkt\n",
+			__func__);
+
+err:
+	++common->tx_stats.total_tx_pkt_freed[skb->priority];
+	rsi_indicate_tx_status(common->priv, skb, status);
+	return status;
+}
+
+/**
+ * rsi_send_mgmt_pkt() - This functions sends the received management packet
+ *			 from driver to device.
+ * @common: Pointer to the driver private structure.
+ * @skb: Pointer to the socket buffer structure.
+ *
+ * Return: status: 0 on success, -1 on failure.
+ */
+int rsi_send_mgmt_pkt(struct rsi_common *common,
+		      struct sk_buff *skb)
+{
+	struct rsi_hw *adapter = common->priv;
+	struct ieee80211_hdr *wh;
+	struct ieee80211_tx_info *info;
+	struct ieee80211_bss_conf *bss;
+	struct ieee80211_hw *hw = adapter->hw;
+	struct ieee80211_conf *conf = &hw->conf;
+	struct skb_info *tx_params;
+	int status = -E2BIG;
+	__le16 *msg;
+	u8 extnd_size;
+	u8 vap_id = 0;
+
+	info = IEEE80211_SKB_CB(skb);
+	tx_params = (struct skb_info *)info->driver_data;
+	extnd_size = ((uintptr_t)skb->data & 0x3);
+
+	if (tx_params->flags & INTERNAL_MGMT_PKT) {
+		if ((extnd_size) > skb_headroom(skb)) {
+			rsi_dbg(ERR_ZONE, "%s: Unable to send pkt\n", __func__);
+			dev_kfree_skb(skb);
+			return -ENOSPC;
+		}
+		skb_push(skb, extnd_size);
+		skb->data[extnd_size + 4] = extnd_size;
+		status = adapter->host_intf_ops->write_pkt(common->priv,
+							   (u8 *)skb->data,
+							   skb->len);
+		if (status) {
+			rsi_dbg(ERR_ZONE,
+				"%s: Failed to write the packet\n", __func__);
+		}
+		dev_kfree_skb(skb);
+		return status;
+	}
+
+	bss = &info->control.vif->bss_conf;
+	wh = (struct ieee80211_hdr *)&skb->data[0];
+
+	if (FRAME_DESC_SZ > skb_headroom(skb))
+		goto err;
+
+	skb_push(skb, FRAME_DESC_SZ);
+	memset(skb->data, 0, FRAME_DESC_SZ);
+	msg = (__le16 *)skb->data;
+
+	if (skb->len > MAX_MGMT_PKT_SIZE) {
+		rsi_dbg(INFO_ZONE, "%s: Dropping mgmt pkt > 512\n", __func__);
+		goto err;
+	}
+
+	msg[0] = cpu_to_le16((skb->len - FRAME_DESC_SZ) |
+			    (RSI_WIFI_MGMT_Q << 12));
+	msg[1] = cpu_to_le16(TX_DOT11_MGMT);
+	msg[2] = cpu_to_le16(MIN_802_11_HDR_LEN << 8);
+	msg[3] = cpu_to_le16(RATE_INFO_ENABLE);
+	msg[6] = cpu_to_le16(le16_to_cpu(wh->seq_ctrl) >> 4);
+
+	if (wh->addr1[0] & BIT(0))
+		msg[3] |= cpu_to_le16(RSI_BROADCAST_PKT);
+
+	if (common->band == NL80211_BAND_2GHZ)
+		msg[4] = cpu_to_le16(RSI_11B_MODE);
+	else
+		msg[4] = cpu_to_le16((RSI_RATE_6 & 0x0f) | RSI_11G_MODE);
+
+	if (conf_is_ht40(conf)) {
+		msg[4] = cpu_to_le16(0xB | RSI_11G_MODE);
+		msg[5] = cpu_to_le16(0x6);
+	}
+
+	/* Indicate to firmware to give cfm */
+	if ((skb->data[16] == IEEE80211_STYPE_PROBE_REQ) && (!bss->assoc)) {
+		msg[1] |= cpu_to_le16(BIT(10));
+		msg[7] = cpu_to_le16(PROBEREQ_CONFIRM);
+		common->mgmt_q_block = true;
+	}
+
+	msg[7] |= cpu_to_le16(vap_id << 8);
+
+	status = adapter->host_intf_ops->write_pkt(common->priv, (u8 *)msg,
+						   skb->len);
+	if (status)
+		rsi_dbg(ERR_ZONE, "%s: Failed to write the packet\n", __func__);
+
+err:
+	rsi_indicate_tx_status(common->priv, skb, status);
+	return status;
+}
+
+static void bl_cmd_timeout(unsigned long priv)
+{
+	struct rsi_hw *adapter = (struct rsi_hw *)priv;
+
+	adapter->blcmd_timer_expired = true;
+	del_timer(&adapter->bl_cmd_timer);
+}
+
+static int bl_start_cmd_timer(struct rsi_hw *adapter, u32 timeout)
+{
+	init_timer(&adapter->bl_cmd_timer);
+	adapter->bl_cmd_timer.data = (unsigned long)adapter;
+	adapter->bl_cmd_timer.function = (void *)&bl_cmd_timeout;
+	adapter->bl_cmd_timer.expires = (msecs_to_jiffies(timeout) + jiffies);
+
+	adapter->blcmd_timer_expired = false;
+	add_timer(&adapter->bl_cmd_timer);
+
+	return 0;
+}
+
+static int bl_stop_cmd_timer(struct rsi_hw *adapter)
+{
+	adapter->blcmd_timer_expired = false;
+	if (timer_pending(&adapter->bl_cmd_timer))
+		del_timer(&adapter->bl_cmd_timer);
+
+	return 0;
+}
+
+static int bl_write_cmd(struct rsi_hw *adapter, u8 cmd, u8 exp_resp,
+			u16 *cmd_resp)
+{
+	struct rsi_host_intf_ops *hif_ops = adapter->host_intf_ops;
+	u32 regin_val = 0, regout_val = 0;
+	u32 regin_input = 0;
+	u8 output = 0;
+	int status;
+
+	regin_input = (REGIN_INPUT | adapter->priv->coex_mode);
+
+	while (!adapter->blcmd_timer_expired) {
+		regin_val = 0;
+		status = hif_ops->master_reg_read(adapter, SWBL_REGIN,
+						  &regin_val, 2);
+		if (status < 0) {
+			rsi_dbg(ERR_ZONE,
+				"%s: Command %0x REGIN reading failed..\n",
+				__func__, cmd);
+			return status;
+		}
+		mdelay(1);
+		if ((regin_val >> 12) != REGIN_VALID)
+			break;
+	}
+	if (adapter->blcmd_timer_expired) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Command %0x REGIN reading timed out..\n",
+			__func__, cmd);
+		return -ETIMEDOUT;
+	}
+
+	rsi_dbg(INFO_ZONE,
+		"Issuing write to Regin val:%0x sending cmd:%0x\n",
+		regin_val, (cmd | regin_input << 8));
+	status = hif_ops->master_reg_write(adapter, SWBL_REGIN,
+					   (cmd | regin_input << 8), 2);
+	if (status < 0)
+		return status;
+	mdelay(1);
+
+	if (cmd == LOAD_HOSTED_FW || cmd == JUMP_TO_ZERO_PC) {
+		/* JUMP_TO_ZERO_PC doesn't expect
+		 * any response. So return from here
+		 */
+		return 0;
+	}
+
+	while (!adapter->blcmd_timer_expired) {
+		regout_val = 0;
+		status = hif_ops->master_reg_read(adapter, SWBL_REGOUT,
+					     &regout_val, 2);
+		if (status < 0) {
+			rsi_dbg(ERR_ZONE,
+				"%s: Command %0x REGOUT reading failed..\n",
+				__func__, cmd);
+			return status;
+		}
+		mdelay(1);
+		if ((regout_val >> 8) == REGOUT_VALID)
+			break;
+	}
+	if (adapter->blcmd_timer_expired) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Command %0x REGOUT reading timed out..\n",
+			__func__, cmd);
+		return status;
+	}
+
+	*cmd_resp = ((u16 *)&regout_val)[0] & 0xffff;
+
+	output = ((u8 *)&regout_val)[0] & 0xff;
+
+	status = hif_ops->master_reg_write(adapter, SWBL_REGOUT,
+					   (cmd | REGOUT_INVALID << 8), 2);
+	if (status < 0) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Command %0x REGOUT writing failed..\n",
+			__func__, cmd);
+		return status;
+	}
+	mdelay(1);
+
+	if (output != exp_resp) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Recvd resp %x for cmd %0x\n",
+			__func__, output, cmd);
+		return -EINVAL;
+	}
+	rsi_dbg(INFO_ZONE,
+		"%s: Recvd Expected resp %x for cmd %0x\n",
+		__func__, output, cmd);
+
+	return 0;
+}
+
+static int bl_cmd(struct rsi_hw *adapter, u8 cmd, u8 exp_resp, char *str)
+{
+	u16 regout_val = 0;
+	u32 timeout;
+	int status;
+
+	if ((cmd == EOF_REACHED) || (cmd == PING_VALID) || (cmd == PONG_VALID))
+		timeout = BL_BURN_TIMEOUT;
+	else
+		timeout = BL_CMD_TIMEOUT;
+
+	bl_start_cmd_timer(adapter, timeout);
+	status = bl_write_cmd(adapter, cmd, exp_resp, &regout_val);
+	if (status < 0) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Command %s (%0x) writing failed..\n",
+			__func__, str, cmd);
+		return status;
+	}
+	bl_stop_cmd_timer(adapter);
+	return 0;
+}
+
+#define CHECK_SUM_OFFSET 20
+#define LEN_OFFSET 8
+#define ADDR_OFFSET 16
+static int bl_write_header(struct rsi_hw *adapter, u8 *flash_content,
+			   u32 content_size)
+{
+	struct rsi_host_intf_ops *hif_ops = adapter->host_intf_ops;
+	struct bl_header bl_hdr;
+	u32 write_addr, write_len;
+	int status;
+
+	bl_hdr.flags = 0;
+	bl_hdr.image_no = cpu_to_le32(adapter->priv->coex_mode);
+	bl_hdr.check_sum = cpu_to_le32(
+				*(u32 *)&flash_content[CHECK_SUM_OFFSET]);
+	bl_hdr.flash_start_address = cpu_to_le32(
+					*(u32 *)&flash_content[ADDR_OFFSET]);
+	bl_hdr.flash_len = cpu_to_le32(*(u32 *)&flash_content[LEN_OFFSET]);
+	write_len = sizeof(struct bl_header);
+
+	if (adapter->rsi_host_intf == RSI_HOST_INTF_USB) {
+		write_addr = PING_BUFFER_ADDRESS;
+		status = hif_ops->write_reg_multiple(adapter, write_addr,
+						 (u8 *)&bl_hdr, write_len);
+		if (status < 0) {
+			rsi_dbg(ERR_ZONE,
+				"%s: Failed to load Version/CRC structure\n",
+				__func__);
+			return status;
+		}
+	} else {
+		write_addr = PING_BUFFER_ADDRESS >> 16;
+		status = hif_ops->master_access_msword(adapter, write_addr);
+		if (status < 0) {
+			rsi_dbg(ERR_ZONE,
+				"%s: Unable to set ms word to common reg\n",
+				__func__);
+			return status;
+		}
+		write_addr = RSI_SD_REQUEST_MASTER |
+			     (PING_BUFFER_ADDRESS & 0xFFFF);
+		status = hif_ops->write_reg_multiple(adapter, write_addr,
+						 (u8 *)&bl_hdr, write_len);
+		if (status < 0) {
+			rsi_dbg(ERR_ZONE,
+				"%s: Failed to load Version/CRC structure\n",
+				__func__);
+			return status;
+		}
+	}
+	return 0;
+}
+
+static u32 read_flash_capacity(struct rsi_hw *adapter)
+{
+	u32 flash_sz = 0;
+
+	if ((adapter->host_intf_ops->master_reg_read(adapter, FLASH_SIZE_ADDR,
+						     &flash_sz, 2)) < 0) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Flash size reading failed..\n",
+			__func__);
+		return 0;
+	}
+	rsi_dbg(INIT_ZONE, "Flash capacity: %d KiloBytes\n", flash_sz);
+
+	return (flash_sz * 1024); /* Return size in kbytes */
+}
+
+static int ping_pong_write(struct rsi_hw *adapter, u8 cmd, u8 *addr, u32 size)
+{
+	struct rsi_host_intf_ops *hif_ops = adapter->host_intf_ops;
+	u32 block_size = adapter->block_size;
+	u32 cmd_addr;
+	u16 cmd_resp, cmd_req;
+	u8 *str;
+	int status;
+
+	if (cmd == PING_WRITE) {
+		cmd_addr = PING_BUFFER_ADDRESS;
+		cmd_resp = PONG_AVAIL;
+		cmd_req = PING_VALID;
+		str = "PING_VALID";
+	} else {
+		cmd_addr = PONG_BUFFER_ADDRESS;
+		cmd_resp = PING_AVAIL;
+		cmd_req = PONG_VALID;
+		str = "PONG_VALID";
+	}
+
+	status = hif_ops->load_data_master_write(adapter, cmd_addr, size,
+					    block_size, addr);
+	if (status) {
+		rsi_dbg(ERR_ZONE, "%s: Unable to write blk at addr %0x\n",
+			__func__, *addr);
+		return status;
+	}
+
+	status = bl_cmd(adapter, cmd_req, cmd_resp, str);
+	if (status) {
+		bl_stop_cmd_timer(adapter);
+		return status;
+	}
+	return 0;
+}
+
+static int auto_fw_upgrade(struct rsi_hw *adapter, u8 *flash_content,
+			   u32 content_size)
+{
+	u8 cmd, *temp_flash_content;
+	u32 temp_content_size, num_flash, index;
+	u32 flash_start_address;
+	int status;
+
+	temp_flash_content = flash_content;
+
+	if (content_size > MAX_FLASH_FILE_SIZE) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Flash Content size is more than 400K %u\n",
+			__func__, MAX_FLASH_FILE_SIZE);
+		return -EINVAL;
+	}
+
+	flash_start_address = *(u32 *)&flash_content[FLASH_START_ADDRESS];
+	rsi_dbg(INFO_ZONE, "flash start address: %08x\n", flash_start_address);
+
+	if (flash_start_address < FW_IMAGE_MIN_ADDRESS) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Fw image Flash Start Address is less than 64K\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if (flash_start_address % FLASH_SECTOR_SIZE) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Flash Start Address is not multiple of 4K\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if ((flash_start_address + content_size) > adapter->flash_capacity) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Flash Content will cross max flash size\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	temp_content_size  = content_size;
+	num_flash = content_size / FLASH_WRITE_CHUNK_SIZE;
+
+	rsi_dbg(INFO_ZONE, "content_size: %d, num_flash: %d\n",
+		content_size, num_flash);
+
+	for (index = 0; index <= num_flash; index++) {
+		rsi_dbg(INFO_ZONE, "flash index: %d\n", index);
+		if (index != num_flash) {
+			content_size = FLASH_WRITE_CHUNK_SIZE;
+			rsi_dbg(INFO_ZONE, "QSPI content_size:%d\n",
+				content_size);
+		} else {
+			content_size =
+				temp_content_size % FLASH_WRITE_CHUNK_SIZE;
+			rsi_dbg(INFO_ZONE,
+				"Writing last sector content_size:%d\n",
+				content_size);
+			if (!content_size) {
+				rsi_dbg(INFO_ZONE, "instruction size zero\n");
+				break;
+			}
+		}
+
+		if (index % 2)
+			cmd = PING_WRITE;
+		else
+			cmd = PONG_WRITE;
+
+		status = ping_pong_write(adapter, cmd, flash_content,
+					 content_size);
+		if (status) {
+			rsi_dbg(ERR_ZONE, "%s: Unable to load %d block\n",
+				__func__, index);
+			return status;
+		}
+
+		rsi_dbg(INFO_ZONE,
+			"%s: Successfully loaded %d instructions\n",
+			__func__, index);
+		flash_content += content_size;
+	}
+
+	status = bl_cmd(adapter, EOF_REACHED, FW_LOADING_SUCCESSFUL,
+			"EOF_REACHED");
+	if (status) {
+		bl_stop_cmd_timer(adapter);
+		return status;
+	}
+	rsi_dbg(INFO_ZONE, "FW loading is done and FW is running..\n");
+	return 0;
+}
+
+static int rsi_load_firmware(struct rsi_hw *adapter)
+{
+	struct rsi_host_intf_ops *hif_ops = adapter->host_intf_ops;
+	const struct firmware *fw_entry = NULL;
+	u32 regout_val = 0, content_size;
+	u16 tmp_regout_val = 0;
+	u8 *flash_content = NULL;
+	struct ta_metadata *metadata_p;
+	int status;
+
+	bl_start_cmd_timer(adapter, BL_CMD_TIMEOUT);
+
+	while (!adapter->blcmd_timer_expired) {
+		status = hif_ops->master_reg_read(adapter, SWBL_REGOUT,
+					      &regout_val, 2);
+		if (status < 0) {
+			rsi_dbg(ERR_ZONE,
+				"%s: REGOUT read failed\n", __func__);
+			return status;
+		}
+		mdelay(1);
+		if ((regout_val >> 8) == REGOUT_VALID)
+			break;
+	}
+	if (adapter->blcmd_timer_expired) {
+		rsi_dbg(ERR_ZONE, "%s: REGOUT read timedout\n", __func__);
+		rsi_dbg(ERR_ZONE,
+			"%s: Soft boot loader not present\n", __func__);
+		return -ETIMEDOUT;
+	}
+	bl_stop_cmd_timer(adapter);
+
+	rsi_dbg(INFO_ZONE, "Received Board Version Number: %x\n",
+		(regout_val & 0xff));
+
+	status = hif_ops->master_reg_write(adapter, SWBL_REGOUT,
+					(REGOUT_INVALID | REGOUT_INVALID << 8),
+					2);
+	if (status < 0) {
+		rsi_dbg(ERR_ZONE, "%s: REGOUT writing failed..\n", __func__);
+		return status;
+	}
+	mdelay(1);
+
+	status = bl_cmd(adapter, CONFIG_AUTO_READ_MODE, CMD_PASS,
+			"AUTO_READ_CMD");
+	if (status < 0)
+		return status;
+
+	adapter->flash_capacity = read_flash_capacity(adapter);
+	if (adapter->flash_capacity <= 0) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Unable to read flash size from EEPROM\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	metadata_p = &metadata_flash_content[adapter->priv->coex_mode];
+
+	rsi_dbg(INIT_ZONE, "%s: Loading file %s\n", __func__, metadata_p->name);
+	adapter->fw_file_name = metadata_p->name;
+
+	status = request_firmware(&fw_entry, metadata_p->name, adapter->device);
+	if (status < 0) {
+		rsi_dbg(ERR_ZONE, "%s: Failed to open file %s\n",
+			__func__, metadata_p->name);
+		return status;
+	}
+	flash_content = kmemdup(fw_entry->data, fw_entry->size, GFP_KERNEL);
+	if (!flash_content) {
+		rsi_dbg(ERR_ZONE, "%s: Failed to copy firmware\n", __func__);
+		status = -EIO;
+		goto fail;
+	}
+	content_size = fw_entry->size;
+	rsi_dbg(INFO_ZONE, "FW Length = %d bytes\n", content_size);
+
+	status = bl_write_header(adapter, flash_content, content_size);
+	if (status) {
+		rsi_dbg(ERR_ZONE,
+			"%s: RPS Image header loading failed\n",
+			__func__);
+		goto fail;
+	}
+
+	bl_start_cmd_timer(adapter, BL_CMD_TIMEOUT);
+	status = bl_write_cmd(adapter, CHECK_CRC, CMD_PASS, &tmp_regout_val);
+	if (status) {
+		bl_stop_cmd_timer(adapter);
+		rsi_dbg(ERR_ZONE,
+			"%s: CHECK_CRC Command writing failed..\n",
+			__func__);
+		if ((tmp_regout_val & 0xff) == CMD_FAIL) {
+			rsi_dbg(ERR_ZONE,
+				"CRC Fail.. Proceeding to Upgrade mode\n");
+			goto fw_upgrade;
+		}
+	}
+	bl_stop_cmd_timer(adapter);
+
+	status = bl_cmd(adapter, POLLING_MODE, CMD_PASS, "POLLING_MODE");
+	if (status)
+		goto fail;
+
+load_image_cmd:
+	status = bl_cmd(adapter, LOAD_HOSTED_FW, LOADING_INITIATED,
+			"LOAD_HOSTED_FW");
+	if (status)
+		goto fail;
+	rsi_dbg(INFO_ZONE, "Load Image command passed..\n");
+	goto success;
+
+fw_upgrade:
+	status = bl_cmd(adapter, BURN_HOSTED_FW, SEND_RPS_FILE, "FW_UPGRADE");
+	if (status)
+		goto fail;
+
+	rsi_dbg(INFO_ZONE, "Burn Command Pass.. Upgrading the firmware\n");
+
+	status = auto_fw_upgrade(adapter, flash_content, content_size);
+	if (status == 0) {
+		rsi_dbg(ERR_ZONE, "Firmware upgradation Done\n");
+		goto load_image_cmd;
+	}
+	rsi_dbg(ERR_ZONE, "Firmware upgrade failed\n");
+
+	status = bl_cmd(adapter, CONFIG_AUTO_READ_MODE, CMD_PASS,
+			"AUTO_READ_MODE");
+	if (status)
+		goto fail;
+
+success:
+	rsi_dbg(ERR_ZONE, "***** Firmware Loading successful *****\n");
+	kfree(flash_content);
+	release_firmware(fw_entry);
+	return 0;
+
+fail:
+	rsi_dbg(ERR_ZONE, "##### Firmware loading failed #####\n");
+	kfree(flash_content);
+	release_firmware(fw_entry);
+	return status;
+}
+
+int rsi_hal_device_init(struct rsi_hw *adapter)
+{
+	struct rsi_common *common = adapter->priv;
+
+	common->coex_mode = RSI_DEV_COEX_MODE_WIFI_ALONE;
+	common->oper_mode = RSI_DEV_OPMODE_WIFI_ALONE;
+	adapter->device_model = RSI_DEV_9113;
+
+	switch (adapter->device_model) {
+	case RSI_DEV_9113:
+		if (rsi_load_firmware(adapter)) {
+			rsi_dbg(ERR_ZONE,
+				"%s: Failed to load TA instructions\n",
+				__func__);
+			return -EINVAL;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+	common->fsm_state = FSM_CARD_NOT_READY;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rsi_hal_device_init);
+
diff --git a/drivers/net/wireless/rsi/rsi_91x_main.c b/drivers/net/wireless/rsi/rsi_91x_main.c
index 8810862ae826..f1cde0ca81f9 100644
--- a/drivers/net/wireless/rsi/rsi_91x_main.c
+++ b/drivers/net/wireless/rsi/rsi_91x_main.c
@@ -123,9 +123,16 @@ int rsi_read_pkt(struct rsi_common *common, s32 rcv_pkt_len)
 
 		queueno = rsi_get_queueno(frame_desc, offset);
 		length = rsi_get_length(frame_desc, offset);
-		extended_desc = rsi_get_extended_desc(frame_desc, offset);
+
+		/* Extended descriptor is valid for WLAN queues only */
+		if (queueno == RSI_WIFI_DATA_Q || queueno == RSI_WIFI_MGMT_Q)
+			extended_desc = rsi_get_extended_desc(frame_desc,
+							      offset);
 
 		switch (queueno) {
+		case RSI_COEX_Q:
+			rsi_mgmt_pkt_recv(common, (frame_desc + offset));
+			break;
 		case RSI_WIFI_DATA_Q:
 			skb = rsi_prepare_skb(common,
 					      (frame_desc + offset),
diff --git a/drivers/net/wireless/rsi/rsi_91x_mgmt.c b/drivers/net/wireless/rsi/rsi_91x_mgmt.c
index fac87c06357b..d4d365b5d2d6 100644
--- a/drivers/net/wireless/rsi/rsi_91x_mgmt.c
+++ b/drivers/net/wireless/rsi/rsi_91x_mgmt.c
@@ -45,10 +45,10 @@ static struct bootup_params boot_params_20 = {
 			}
 		},
 		.switch_clk_g = {
-			.switch_clk_info = cpu_to_le16(BIT(3)),
-			.bbp_lmac_clk_reg_val = cpu_to_le16(0x121),
-			.umac_clock_reg_config = 0x0,
-			.qspi_uart_clock_reg_config = 0x0
+			.switch_clk_info = cpu_to_le16(0xb),
+			.bbp_lmac_clk_reg_val = cpu_to_le16(0x111),
+			.umac_clock_reg_config = cpu_to_le16(0x48),
+			.qspi_uart_clock_reg_config = cpu_to_le16(0x1211)
 		}
 	},
 	{
@@ -106,7 +106,10 @@ static struct bootup_params boot_params_20 = {
 	.wdt_prog_value = 0x0,
 	.wdt_soc_rst_delay = 0x0,
 	.dcdc_operation_mode = 0x0,
-	.soc_reset_wait_cnt = 0x0
+	.soc_reset_wait_cnt = 0x0,
+	.waiting_time_at_fresh_sleep = 0x0,
+	.max_threshold_to_avoid_sleep = 0x0,
+	.beacon_resedue_alg_en = 0,
 };
 
 static struct bootup_params boot_params_40 = {
@@ -139,7 +142,7 @@ static struct bootup_params boot_params_40 = {
 			.switch_clk_info = cpu_to_le16(0x09),
 			.bbp_lmac_clk_reg_val = cpu_to_le16(0x1121),
 			.umac_clock_reg_config = cpu_to_le16(0x48),
-			.qspi_uart_clock_reg_config = 0x0
+			.qspi_uart_clock_reg_config = cpu_to_le16(0x1211)
 		}
 	},
 	{
@@ -197,7 +200,10 @@ static struct bootup_params boot_params_40 = {
 	.wdt_prog_value = 0x0,
 	.wdt_soc_rst_delay = 0x0,
 	.dcdc_operation_mode = 0x0,
-	.soc_reset_wait_cnt = 0x0
+	.soc_reset_wait_cnt = 0x0,
+	.waiting_time_at_fresh_sleep = 0x0,
+	.max_threshold_to_avoid_sleep = 0x0,
+	.beacon_resedue_alg_en = 0,
 };
 
 static u16 mcs[] = {13, 26, 39, 52, 78, 104, 117, 130};
@@ -218,6 +224,12 @@ static void rsi_set_default_parameters(struct rsi_common *common)
 	common->fsm_state = FSM_CARD_NOT_READY;
 	common->iface_down = true;
 	common->endpoint = EP_2GHZ_20MHZ;
+	common->driver_mode = 1; /* End to end mode */
+	common->lp_ps_handshake_mode = 0; /* Default no handShake mode*/
+	common->ulp_ps_handshake_mode = 2; /* Default PKT handShake mode*/
+	common->rf_power_val = 0; /* Default 1.9V */
+	common->wlan_rf_power_mode = 0;
+	common->obm_ant_sel_val = 2;
 }
 
 /**
@@ -389,9 +401,7 @@ static int rsi_mgmt_pkt_to_core(struct rsi_common *common,
 	struct ieee80211_tx_info *info;
 	struct skb_info *rx_params;
 	u8 pad_bytes = msg[4];
-	u8 pkt_recv;
 	struct sk_buff *skb;
-	char *buffer;
 
 	if (type == RX_DOT11_MGMT) {
 		if (!adapter->sc_nvifs)
@@ -412,13 +422,9 @@ static int rsi_mgmt_pkt_to_core(struct rsi_common *common,
 			return -ENOMEM;
 		}
 
-		buffer = skb_put(skb, msg_len);
-
-		memcpy(buffer,
-		       (u8 *)(msg +  FRAME_DESC_SZ + pad_bytes),
-		       msg_len);
-
-		pkt_recv = buffer[0];
+		skb_put_data(skb,
+			     (u8 *)(msg + FRAME_DESC_SZ + pad_bytes),
+			     msg_len);
 
 		info = IEEE80211_SKB_CB(skb);
 		rx_params = (struct skb_info *)info->driver_data;
@@ -756,6 +762,53 @@ int rsi_hal_load_key(struct rsi_common *common,
 }
 
 /*
+ * This function sends the common device configuration parameters to device.
+ * This frame includes the useful information to make device works on
+ * specific operating mode.
+ */
+static int rsi_send_common_dev_params(struct rsi_common *common)
+{
+	struct sk_buff *skb;
+	u16 frame_len;
+	struct rsi_config_vals *dev_cfgs;
+
+	frame_len = sizeof(struct rsi_config_vals);
+
+	rsi_dbg(MGMT_TX_ZONE, "Sending common device config params\n");
+	skb = dev_alloc_skb(frame_len);
+	if (!skb) {
+		rsi_dbg(ERR_ZONE, "%s: Unable to allocate skb\n", __func__);
+		return -ENOMEM;
+	}
+
+	memset(skb->data, 0, frame_len);
+
+	dev_cfgs = (struct rsi_config_vals *)skb->data;
+	memset(dev_cfgs, 0, (sizeof(struct rsi_config_vals)));
+
+	rsi_set_len_qno(&dev_cfgs->len_qno, (frame_len - FRAME_DESC_SZ),
+			RSI_COEX_Q);
+	dev_cfgs->pkt_type = COMMON_DEV_CONFIG;
+
+	dev_cfgs->lp_ps_handshake = common->lp_ps_handshake_mode;
+	dev_cfgs->ulp_ps_handshake = common->ulp_ps_handshake_mode;
+
+	dev_cfgs->unused_ulp_gpio = RSI_UNUSED_ULP_GPIO_BITMAP;
+	dev_cfgs->unused_soc_gpio_bitmap =
+				cpu_to_le32(RSI_UNUSED_SOC_GPIO_BITMAP);
+
+	dev_cfgs->opermode = common->oper_mode;
+	dev_cfgs->wlan_rf_pwr_mode = common->wlan_rf_power_mode;
+	dev_cfgs->driver_mode = common->driver_mode;
+	dev_cfgs->region_code = NL80211_DFS_FCC;
+	dev_cfgs->antenna_sel_val = common->obm_ant_sel_val;
+
+	skb_put(skb, frame_len);
+
+	return rsi_send_internal_mgmt_frame(common, skb);
+}
+
+/*
  * rsi_load_bootup_params() - This function send bootup params to the firmware.
  * @common: Pointer to the driver private structure.
  *
@@ -1487,6 +1540,40 @@ out:
 	return -EINVAL;
 }
 
+static int rsi_handle_card_ready(struct rsi_common *common, u8 *msg)
+{
+	switch (common->fsm_state) {
+	case FSM_CARD_NOT_READY:
+		rsi_dbg(INIT_ZONE, "Card ready indication from Common HAL\n");
+		rsi_set_default_parameters(common);
+		if (rsi_send_common_dev_params(common) < 0)
+			return -EINVAL;
+		common->fsm_state = FSM_COMMON_DEV_PARAMS_SENT;
+		break;
+	case FSM_COMMON_DEV_PARAMS_SENT:
+		rsi_dbg(INIT_ZONE, "Card ready indication from WLAN HAL\n");
+
+		/* Get usb buffer status register address */
+		common->priv->usb_buffer_status_reg = *(u32 *)&msg[8];
+		rsi_dbg(INFO_ZONE, "USB buffer status register = %x\n",
+			common->priv->usb_buffer_status_reg);
+
+		if (rsi_load_bootup_params(common)) {
+			common->fsm_state = FSM_CARD_NOT_READY;
+			return -EINVAL;
+		}
+		common->fsm_state = FSM_BOOT_PARAMS_SENT;
+		break;
+	default:
+		rsi_dbg(ERR_ZONE,
+			"%s: card ready indication in invalid state %d.\n",
+			__func__, common->fsm_state);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 /**
  * rsi_mgmt_pkt_recv() - This function processes the management packets
  *			 recieved from the hardware.
@@ -1499,7 +1586,6 @@ int rsi_mgmt_pkt_recv(struct rsi_common *common, u8 *msg)
 {
 	s32 msg_len = (le16_to_cpu(*(__le16 *)&msg[0]) & 0x0fff);
 	u16 msg_type = (msg[2]);
-	int ret;
 
 	rsi_dbg(FSM_ZONE, "%s: Msg Len: %d, Msg Type: %4x\n",
 		__func__, msg_len, msg_type);
@@ -1509,17 +1595,7 @@ int rsi_mgmt_pkt_recv(struct rsi_common *common, u8 *msg)
 	} else if (msg_type == CARD_READY_IND) {
 		rsi_dbg(FSM_ZONE, "%s: Card ready indication received\n",
 			__func__);
-		if (common->fsm_state == FSM_CARD_NOT_READY) {
-			rsi_set_default_parameters(common);
-
-			ret = rsi_load_bootup_params(common);
-			if (ret)
-				return ret;
-			else
-				common->fsm_state = FSM_BOOT_PARAMS_SENT;
-		} else {
-			return -EINVAL;
-		}
+		return rsi_handle_card_ready(common, msg);
 	} else if (msg_type == TX_STATUS_IND) {
 		if (msg[15] == PROBEREQ_CONFIRM) {
 			common->mgmt_q_block = false;
diff --git a/drivers/net/wireless/rsi/rsi_91x_pkt.c b/drivers/net/wireless/rsi/rsi_91x_pkt.c
deleted file mode 100644
index 02920c93e82d..000000000000
--- a/drivers/net/wireless/rsi/rsi_91x_pkt.c
+++ /dev/null
@@ -1,215 +0,0 @@
-/**
- * Copyright (c) 2014 Redpine Signals 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 "rsi_mgmt.h"
-
-/**
- * rsi_send_data_pkt() - This function sends the recieved data packet from
- *			 driver to device.
- * @common: Pointer to the driver private structure.
- * @skb: Pointer to the socket buffer structure.
- *
- * Return: status: 0 on success, -1 on failure.
- */
-int rsi_send_data_pkt(struct rsi_common *common, struct sk_buff *skb)
-{
-	struct rsi_hw *adapter = common->priv;
-	struct ieee80211_hdr *tmp_hdr;
-	struct ieee80211_tx_info *info;
-	struct skb_info *tx_params;
-	struct ieee80211_bss_conf *bss;
-	int status;
-	u8 ieee80211_size = MIN_802_11_HDR_LEN;
-	u8 extnd_size;
-	__le16 *frame_desc;
-	u16 seq_num;
-
-	info = IEEE80211_SKB_CB(skb);
-	bss = &info->control.vif->bss_conf;
-	tx_params = (struct skb_info *)info->driver_data;
-
-	if (!bss->assoc) {
-		status = -EINVAL;
-		goto err;
-	}
-
-	tmp_hdr = (struct ieee80211_hdr *)&skb->data[0];
-	seq_num = (le16_to_cpu(tmp_hdr->seq_ctrl) >> 4);
-
-	extnd_size = ((uintptr_t)skb->data & 0x3);
-
-	if ((FRAME_DESC_SZ + extnd_size) > skb_headroom(skb)) {
-		rsi_dbg(ERR_ZONE, "%s: Unable to send pkt\n", __func__);
-		status = -ENOSPC;
-		goto err;
-	}
-
-	skb_push(skb, (FRAME_DESC_SZ + extnd_size));
-	frame_desc = (__le16 *)&skb->data[0];
-	memset((u8 *)frame_desc, 0, FRAME_DESC_SZ);
-
-	if (ieee80211_is_data_qos(tmp_hdr->frame_control)) {
-		ieee80211_size += 2;
-		frame_desc[6] |= cpu_to_le16(BIT(12));
-	}
-
-	if ((!(info->flags & IEEE80211_TX_INTFL_DONT_ENCRYPT)) &&
-	    (common->secinfo.security_enable)) {
-		if (rsi_is_cipher_wep(common))
-			ieee80211_size += 4;
-		else
-			ieee80211_size += 8;
-		frame_desc[6] |= cpu_to_le16(BIT(15));
-	}
-
-	frame_desc[0] = cpu_to_le16((skb->len - FRAME_DESC_SZ) |
-				    (RSI_WIFI_DATA_Q << 12));
-	frame_desc[2] = cpu_to_le16((extnd_size) | (ieee80211_size) << 8);
-
-	if (common->min_rate != 0xffff) {
-		/* Send fixed rate */
-		frame_desc[3] = cpu_to_le16(RATE_INFO_ENABLE);
-		frame_desc[4] = cpu_to_le16(common->min_rate);
-
-		if (conf_is_ht40(&common->priv->hw->conf))
-			frame_desc[5] = cpu_to_le16(FULL40M_ENABLE);
-
-		if (common->vif_info[0].sgi) {
-			if (common->min_rate & 0x100) /* Only MCS rates */
-				frame_desc[4] |=
-					cpu_to_le16(ENABLE_SHORTGI_RATE);
-		}
-
-	}
-
-	frame_desc[6] |= cpu_to_le16(seq_num & 0xfff);
-	frame_desc[7] = cpu_to_le16(((tx_params->tid & 0xf) << 4) |
-				    (skb->priority & 0xf) |
-				    (tx_params->sta_id << 8));
-
-	status = adapter->host_intf_write_pkt(common->priv,
-					      skb->data,
-					      skb->len);
-	if (status)
-		rsi_dbg(ERR_ZONE, "%s: Failed to write pkt\n",
-			__func__);
-
-err:
-	++common->tx_stats.total_tx_pkt_freed[skb->priority];
-	rsi_indicate_tx_status(common->priv, skb, status);
-	return status;
-}
-
-/**
- * rsi_send_mgmt_pkt() - This functions sends the received management packet
- *			 from driver to device.
- * @common: Pointer to the driver private structure.
- * @skb: Pointer to the socket buffer structure.
- *
- * Return: status: 0 on success, -1 on failure.
- */
-int rsi_send_mgmt_pkt(struct rsi_common *common,
-		      struct sk_buff *skb)
-{
-	struct rsi_hw *adapter = common->priv;
-	struct ieee80211_hdr *wh;
-	struct ieee80211_tx_info *info;
-	struct ieee80211_bss_conf *bss;
-	struct ieee80211_hw *hw = adapter->hw;
-	struct ieee80211_conf *conf = &hw->conf;
-	struct skb_info *tx_params;
-	int status = -E2BIG;
-	__le16 *msg;
-	u8 extnd_size;
-	u8 vap_id = 0;
-
-	info = IEEE80211_SKB_CB(skb);
-	tx_params = (struct skb_info *)info->driver_data;
-	extnd_size = ((uintptr_t)skb->data & 0x3);
-
-	if (tx_params->flags & INTERNAL_MGMT_PKT) {
-		if ((extnd_size) > skb_headroom(skb)) {
-			rsi_dbg(ERR_ZONE, "%s: Unable to send pkt\n", __func__);
-			dev_kfree_skb(skb);
-			return -ENOSPC;
-		}
-		skb_push(skb, extnd_size);
-		skb->data[extnd_size + 4] = extnd_size;
-		status = adapter->host_intf_write_pkt(common->priv,
-						      (u8 *)skb->data,
-						      skb->len);
-		if (status) {
-			rsi_dbg(ERR_ZONE,
-				"%s: Failed to write the packet\n", __func__);
-		}
-		dev_kfree_skb(skb);
-		return status;
-	}
-
-	bss = &info->control.vif->bss_conf;
-	wh = (struct ieee80211_hdr *)&skb->data[0];
-
-	if (FRAME_DESC_SZ > skb_headroom(skb))
-		goto err;
-
-	skb_push(skb, FRAME_DESC_SZ);
-	memset(skb->data, 0, FRAME_DESC_SZ);
-	msg = (__le16 *)skb->data;
-
-	if (skb->len > MAX_MGMT_PKT_SIZE) {
-		rsi_dbg(INFO_ZONE, "%s: Dropping mgmt pkt > 512\n", __func__);
-		goto err;
-	}
-
-	msg[0] = cpu_to_le16((skb->len - FRAME_DESC_SZ) |
-			    (RSI_WIFI_MGMT_Q << 12));
-	msg[1] = cpu_to_le16(TX_DOT11_MGMT);
-	msg[2] = cpu_to_le16(MIN_802_11_HDR_LEN << 8);
-	msg[3] = cpu_to_le16(RATE_INFO_ENABLE);
-	msg[6] = cpu_to_le16(le16_to_cpu(wh->seq_ctrl) >> 4);
-
-	if (wh->addr1[0] & BIT(0))
-		msg[3] |= cpu_to_le16(RSI_BROADCAST_PKT);
-
-	if (common->band == NL80211_BAND_2GHZ)
-		msg[4] = cpu_to_le16(RSI_11B_MODE);
-	else
-		msg[4] = cpu_to_le16((RSI_RATE_6 & 0x0f) | RSI_11G_MODE);
-
-	if (conf_is_ht40(conf)) {
-		msg[4] = cpu_to_le16(0xB | RSI_11G_MODE);
-		msg[5] = cpu_to_le16(0x6);
-	}
-
-	/* Indicate to firmware to give cfm */
-	if ((skb->data[16] == IEEE80211_STYPE_PROBE_REQ) && (!bss->assoc)) {
-		msg[1] |= cpu_to_le16(BIT(10));
-		msg[7] = cpu_to_le16(PROBEREQ_CONFIRM);
-		common->mgmt_q_block = true;
-	}
-
-	msg[7] |= cpu_to_le16(vap_id << 8);
-
-	status = adapter->host_intf_write_pkt(common->priv,
-					      (u8 *)msg,
-					      skb->len);
-	if (status)
-		rsi_dbg(ERR_ZONE, "%s: Failed to write the packet\n", __func__);
-
-err:
-	rsi_indicate_tx_status(common->priv, skb, status);
-	return status;
-}
diff --git a/drivers/net/wireless/rsi/rsi_91x_sdio.c b/drivers/net/wireless/rsi/rsi_91x_sdio.c
index 8428858204a6..e5ea99bb2dd8 100644
--- a/drivers/net/wireless/rsi/rsi_91x_sdio.c
+++ b/drivers/net/wireless/rsi/rsi_91x_sdio.c
@@ -18,6 +18,7 @@
 #include <linux/module.h>
 #include "rsi_sdio.h"
 #include "rsi_common.h"
+#include "rsi_hal.h"
 
 /**
  * rsi_sdio_set_cmd52_arg() - This function prepares cmd 52 read/write arg.
@@ -138,6 +139,8 @@ static void rsi_handle_interrupt(struct sdio_func *function)
 {
 	struct rsi_hw *adapter = sdio_get_drvdata(function);
 
+	if (adapter->priv->fsm_state == FSM_FW_NOT_LOADED)
+		return;
 	sdio_release_host(function);
 	rsi_interrupt_handler(adapter);
 	sdio_claim_host(function);
@@ -365,6 +368,7 @@ static int rsi_setblocklength(struct rsi_hw *adapter, u32 length)
 
 	status = sdio_set_block_size(dev->pfunction, length);
 	dev->pfunction->max_blksize = 256;
+	adapter->block_size = dev->pfunction->max_blksize;
 
 	rsi_dbg(INFO_ZONE,
 		"%s: Operational blk length is %d\n", __func__, length);
@@ -487,8 +491,8 @@ void rsi_sdio_ack_intr(struct rsi_hw *adapter, u8 int_bit)
  */
 static int rsi_sdio_read_register_multiple(struct rsi_hw *adapter,
 					   u32 addr,
-					   u32 count,
-					   u8 *data)
+					   u8 *data,
+					   u16 count)
 {
 	struct rsi_91x_sdiodev *dev =
 		(struct rsi_91x_sdiodev *)adapter->rsi_dev;
@@ -518,7 +522,7 @@ static int rsi_sdio_read_register_multiple(struct rsi_hw *adapter,
 int rsi_sdio_write_register_multiple(struct rsi_hw *adapter,
 				     u32 addr,
 				     u8 *data,
-				     u32 count)
+				     u16 count)
 {
 	struct rsi_91x_sdiodev *dev =
 		(struct rsi_91x_sdiodev *)adapter->rsi_dev;
@@ -552,6 +556,182 @@ int rsi_sdio_write_register_multiple(struct rsi_hw *adapter,
 	return status;
 }
 
+static int rsi_sdio_load_data_master_write(struct rsi_hw *adapter,
+					   u32 base_address,
+					   u32 instructions_sz,
+					   u16 block_size,
+					   u8 *ta_firmware)
+{
+	u32 num_blocks, offset, i;
+	u16 msb_address, lsb_address;
+	u8 temp_buf[block_size];
+	int status;
+
+	num_blocks = instructions_sz / block_size;
+	msb_address = base_address >> 16;
+
+	rsi_dbg(INFO_ZONE, "ins_size: %d, num_blocks: %d\n",
+		instructions_sz, num_blocks);
+
+	/* Loading DM ms word in the sdio slave */
+	status = rsi_sdio_master_access_msword(adapter, msb_address);
+	if (status < 0) {
+		rsi_dbg(ERR_ZONE, "%s: Unable to set ms word reg\n", __func__);
+		return status;
+	}
+
+	for (offset = 0, i = 0; i < num_blocks; i++, offset += block_size) {
+		memset(temp_buf, 0, block_size);
+		memcpy(temp_buf, ta_firmware + offset, block_size);
+		lsb_address = (u16)base_address;
+		status = rsi_sdio_write_register_multiple
+					(adapter,
+					 lsb_address | RSI_SD_REQUEST_MASTER,
+					 temp_buf, block_size);
+		if (status < 0) {
+			rsi_dbg(ERR_ZONE, "%s: failed to write\n", __func__);
+			return status;
+		}
+		rsi_dbg(INFO_ZONE, "%s: loading block: %d\n", __func__, i);
+		base_address += block_size;
+
+		if ((base_address >> 16) != msb_address) {
+			msb_address += 1;
+
+			/* Loading DM ms word in the sdio slave */
+			status = rsi_sdio_master_access_msword(adapter,
+							       msb_address);
+			if (status < 0) {
+				rsi_dbg(ERR_ZONE,
+					"%s: Unable to set ms word reg\n",
+					__func__);
+				return status;
+			}
+		}
+	}
+
+	if (instructions_sz % block_size) {
+		memset(temp_buf, 0, block_size);
+		memcpy(temp_buf, ta_firmware + offset,
+		       instructions_sz % block_size);
+		lsb_address = (u16)base_address;
+		status = rsi_sdio_write_register_multiple
+					(adapter,
+					 lsb_address | RSI_SD_REQUEST_MASTER,
+					 temp_buf,
+					 instructions_sz % block_size);
+		if (status < 0)
+			return status;
+		rsi_dbg(INFO_ZONE,
+			"Written Last Block in Address 0x%x Successfully\n",
+			offset | RSI_SD_REQUEST_MASTER);
+	}
+	return 0;
+}
+
+#define FLASH_SIZE_ADDR                 0x04000016
+static int rsi_sdio_master_reg_read(struct rsi_hw *adapter, u32 addr,
+				    u32 *read_buf, u16 size)
+{
+	u32 addr_on_bus, *data;
+	u32 align[2] = {};
+	u16 ms_addr;
+	int status;
+
+	data = PTR_ALIGN(&align[0], 8);
+
+	ms_addr = (addr >> 16);
+	status = rsi_sdio_master_access_msword(adapter, ms_addr);
+	if (status < 0) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Unable to set ms word to common reg\n",
+			__func__);
+		return status;
+	}
+	addr &= 0xFFFF;
+
+	addr_on_bus = (addr & 0xFF000000);
+	if ((addr_on_bus == (FLASH_SIZE_ADDR & 0xFF000000)) ||
+	    (addr_on_bus == 0x0))
+		addr_on_bus = (addr & ~(0x3));
+	else
+		addr_on_bus = addr;
+
+	/* Bring TA out of reset */
+	status = rsi_sdio_read_register_multiple
+					(adapter,
+					 (addr_on_bus | RSI_SD_REQUEST_MASTER),
+					 (u8 *)data, 4);
+	if (status < 0) {
+		rsi_dbg(ERR_ZONE, "%s: AHB register read failed\n", __func__);
+		return status;
+	}
+	if (size == 2) {
+		if ((addr & 0x3) == 0)
+			*read_buf = *data;
+		else
+			*read_buf  = (*data >> 16);
+		*read_buf = (*read_buf & 0xFFFF);
+	} else if (size == 1) {
+		if ((addr & 0x3) == 0)
+			*read_buf = *data;
+		else if ((addr & 0x3) == 1)
+			*read_buf = (*data >> 8);
+		else if ((addr & 0x3) == 2)
+			*read_buf = (*data >> 16);
+		else
+			*read_buf = (*data >> 24);
+		*read_buf = (*read_buf & 0xFF);
+	} else {
+		*read_buf = *data;
+	}
+
+	return 0;
+}
+
+static int rsi_sdio_master_reg_write(struct rsi_hw *adapter,
+				     unsigned long addr,
+				     unsigned long data, u16 size)
+{
+	unsigned long data1[2], *data_aligned;
+	int status;
+
+	data_aligned = PTR_ALIGN(&data1[0], 8);
+
+	if (size == 2) {
+		*data_aligned = ((data << 16) | (data & 0xFFFF));
+	} else if (size == 1) {
+		u32 temp_data = data & 0xFF;
+
+		*data_aligned = ((temp_data << 24) | (temp_data << 16) |
+				 (temp_data << 8) | temp_data);
+	} else {
+		*data_aligned = data;
+	}
+	size = 4;
+
+	status = rsi_sdio_master_access_msword(adapter, (addr >> 16));
+	if (status < 0) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Unable to set ms word to common reg\n",
+			__func__);
+		return -EIO;
+	}
+	addr = addr & 0xFFFF;
+
+	/* Bring TA out of reset */
+	status = rsi_sdio_write_register_multiple
+					(adapter,
+					 (addr | RSI_SD_REQUEST_MASTER),
+					 (u8 *)data_aligned, size);
+	if (status < 0) {
+		rsi_dbg(ERR_ZONE,
+			"%s: Unable to do AHB reg write\n", __func__);
+		return status;
+	}
+	return 0;
+}
+
 /**
  * rsi_sdio_host_intf_write_pkt() - This function writes the packet to device.
  * @adapter: Pointer to the adapter structure.
@@ -614,8 +794,8 @@ int rsi_sdio_host_intf_read_pkt(struct rsi_hw *adapter,
 
 	status = rsi_sdio_read_register_multiple(adapter,
 						 length,
-						 length, /*num of bytes*/
-						 (u8 *)pkt);
+						 (u8 *)pkt,
+						 length); /*num of bytes*/
 
 	if (status)
 		rsi_dbg(ERR_ZONE, "%s: Failed to read frame: %d\n", __func__,
@@ -676,8 +856,6 @@ static int rsi_init_sdio_interface(struct rsi_hw *adapter,
 	}
 	sdio_release_host(pfunction);
 
-	adapter->host_intf_write_pkt = rsi_sdio_host_intf_write_pkt;
-	adapter->host_intf_read_pkt = rsi_sdio_host_intf_read_pkt;
 	adapter->determine_event_timeout = rsi_sdio_determine_event_timeout;
 	adapter->check_hw_queue_status = rsi_sdio_read_buffer_status_register;
 
@@ -691,6 +869,17 @@ fail:
 	return status;
 }
 
+static struct rsi_host_intf_ops sdio_host_intf_ops = {
+	.write_pkt		= rsi_sdio_host_intf_write_pkt,
+	.read_pkt		= rsi_sdio_host_intf_read_pkt,
+	.master_access_msword	= rsi_sdio_master_access_msword,
+	.read_reg_multiple	= rsi_sdio_read_register_multiple,
+	.write_reg_multiple	= rsi_sdio_write_register_multiple,
+	.master_reg_read	= rsi_sdio_master_reg_read,
+	.master_reg_write	= rsi_sdio_master_reg_write,
+	.load_data_master_write	= rsi_sdio_load_data_master_write,
+};
+
 /**
  * rsi_probe() - This function is called by kernel when the driver provided
  *		 Vendor and device IDs are matched. All the initialization
@@ -713,31 +902,38 @@ static int rsi_probe(struct sdio_func *pfunction,
 			__func__);
 		return 1;
 	}
+	adapter->rsi_host_intf = RSI_HOST_INTF_SDIO;
+	adapter->host_intf_ops = &sdio_host_intf_ops;
 
 	if (rsi_init_sdio_interface(adapter, pfunction)) {
 		rsi_dbg(ERR_ZONE, "%s: Failed to init sdio interface\n",
 			__func__);
 		goto fail;
 	}
+	sdio_claim_host(pfunction);
+	if (sdio_claim_irq(pfunction, rsi_handle_interrupt)) {
+		rsi_dbg(ERR_ZONE, "%s: Failed to request IRQ\n", __func__);
+		sdio_release_host(pfunction);
+		goto fail;
+	}
+	sdio_release_host(pfunction);
+	rsi_dbg(INIT_ZONE, "%s: Registered Interrupt handler\n", __func__);
 
-	if (rsi_sdio_device_init(adapter->priv)) {
+	if (rsi_hal_device_init(adapter)) {
 		rsi_dbg(ERR_ZONE, "%s: Failed in device init\n", __func__);
 		sdio_claim_host(pfunction);
+		sdio_release_irq(pfunction);
 		sdio_disable_func(pfunction);
 		sdio_release_host(pfunction);
 		goto fail;
 	}
+	rsi_dbg(INFO_ZONE, "===> RSI Device Init Done <===\n");
 
-	sdio_claim_host(pfunction);
-	if (sdio_claim_irq(pfunction, rsi_handle_interrupt)) {
-		rsi_dbg(ERR_ZONE, "%s: Failed to request IRQ\n", __func__);
-		sdio_release_host(pfunction);
-		goto fail;
+	if (rsi_sdio_master_access_msword(adapter, MISC_CFG_BASE_ADDR)) {
+		rsi_dbg(ERR_ZONE, "%s: Unable to set ms word reg\n", __func__);
+		return -EIO;
 	}
 
-	sdio_release_host(pfunction);
-	rsi_dbg(INIT_ZONE, "%s: Registered Interrupt handler\n", __func__);
-
 	return 0;
 fail:
 	rsi_91x_deinit(adapter);
diff --git a/drivers/net/wireless/rsi/rsi_91x_sdio_ops.c b/drivers/net/wireless/rsi/rsi_91x_sdio_ops.c
index 40d72312f3df..df2a63b1f15c 100644
--- a/drivers/net/wireless/rsi/rsi_91x_sdio_ops.c
+++ b/drivers/net/wireless/rsi/rsi_91x_sdio_ops.c
@@ -27,8 +27,7 @@
  *
  * Return: status: 0 on success, -1 on failure.
  */
-static int rsi_sdio_master_access_msword(struct rsi_hw *adapter,
-					 u16 ms_word)
+int rsi_sdio_master_access_msword(struct rsi_hw *adapter, u16 ms_word)
 {
 	u8 byte;
 	u8 function = 0;
@@ -61,171 +60,6 @@ static int rsi_sdio_master_access_msword(struct rsi_hw *adapter,
 }
 
 /**
- * rsi_copy_to_card() - This function includes the actual funtionality of
- *			copying the TA firmware to the card.Basically this
- *			function includes opening the TA file,reading the
- *			TA file and writing their values in blocks of data.
- * @common: Pointer to the driver private structure.
- * @fw: Pointer to the firmware value to be written.
- * @len: length of firmware file.
- * @num_blocks: Number of blocks to be written to the card.
- *
- * Return: 0 on success and -1 on failure.
- */
-static int rsi_copy_to_card(struct rsi_common *common,
-			    const u8 *fw,
-			    u32 len,
-			    u32 num_blocks)
-{
-	struct rsi_hw *adapter = common->priv;
-	struct rsi_91x_sdiodev *dev =
-		(struct rsi_91x_sdiodev *)adapter->rsi_dev;
-	u32 indx, ii;
-	u32 block_size = dev->tx_blk_size;
-	u32 lsb_address;
-	__le32 data[] = { TA_HOLD_THREAD_VALUE, TA_SOFT_RST_CLR,
-			  TA_PC_ZERO, TA_RELEASE_THREAD_VALUE };
-	u32 address[] = { TA_HOLD_THREAD_REG, TA_SOFT_RESET_REG,
-			  TA_TH0_PC_REG, TA_RELEASE_THREAD_REG };
-	u32 base_address;
-	u16 msb_address;
-
-	base_address = TA_LOAD_ADDRESS;
-	msb_address = base_address >> 16;
-
-	for (indx = 0, ii = 0; ii < num_blocks; ii++, indx += block_size) {
-		lsb_address = ((u16) base_address | RSI_SD_REQUEST_MASTER);
-		if (rsi_sdio_write_register_multiple(adapter,
-						     lsb_address,
-						     (u8 *)(fw + indx),
-						     block_size)) {
-			rsi_dbg(ERR_ZONE,
-				"%s: Unable to load %s blk\n", __func__,
-				FIRMWARE_RSI9113);
-			return -1;
-		}
-		rsi_dbg(INIT_ZONE, "%s: loading block: %d\n", __func__, ii);
-		base_address += block_size;
-		if ((base_address >> 16) != msb_address) {
-			msb_address += 1;
-			if (rsi_sdio_master_access_msword(adapter,
-							  msb_address)) {
-				rsi_dbg(ERR_ZONE,
-					"%s: Unable to set ms word reg\n",
-					__func__);
-				return -1;
-			}
-		}
-	}
-
-	if (len % block_size) {
-		lsb_address = ((u16) base_address | RSI_SD_REQUEST_MASTER);
-		if (rsi_sdio_write_register_multiple(adapter,
-						     lsb_address,
-						     (u8 *)(fw + indx),
-						     len % block_size)) {
-			rsi_dbg(ERR_ZONE,
-				"%s: Unable to load f/w\n", __func__);
-			return -1;
-		}
-	}
-	rsi_dbg(INIT_ZONE,
-		"%s: Succesfully loaded TA instructions\n", __func__);
-
-	if (rsi_sdio_master_access_msword(adapter, TA_BASE_ADDR)) {
-		rsi_dbg(ERR_ZONE,
-			"%s: Unable to set ms word to common reg\n",
-			__func__);
-		return -1;
-	}
-
-	for (ii = 0; ii < ARRAY_SIZE(data); ii++) {
-		/* Bringing TA out of reset */
-		if (rsi_sdio_write_register_multiple(adapter,
-						     (address[ii] |
-						     RSI_SD_REQUEST_MASTER),
-						     (u8 *)&data[ii],
-						     4)) {
-			rsi_dbg(ERR_ZONE,
-				"%s: Unable to hold TA threads\n", __func__);
-			return -1;
-		}
-	}
-
-	rsi_dbg(INIT_ZONE, "%s: loaded firmware\n", __func__);
-	return 0;
-}
-
-/**
- * rsi_load_ta_instructions() - This function includes the actual funtionality
- *				of loading the TA firmware.This function also
- *				includes opening the TA file,reading the TA
- *				file and writing their value in blocks of data.
- * @common: Pointer to the driver private structure.
- *
- * Return: status: 0 on success, -1 on failure.
- */
-static int rsi_load_ta_instructions(struct rsi_common *common)
-{
-	struct rsi_hw *adapter = common->priv;
-	struct rsi_91x_sdiodev *dev =
-		(struct rsi_91x_sdiodev *)adapter->rsi_dev;
-	u32 len;
-	u32 num_blocks;
-	const u8 *fw;
-	const struct firmware *fw_entry = NULL;
-	u32 block_size = dev->tx_blk_size;
-	int status = 0;
-	u32 base_address;
-	u16 msb_address;
-
-	if (rsi_sdio_master_access_msword(adapter, TA_BASE_ADDR)) {
-		rsi_dbg(ERR_ZONE,
-			"%s: Unable to set ms word to common reg\n",
-			__func__);
-		return -1;
-	}
-	base_address = TA_LOAD_ADDRESS;
-	msb_address = (base_address >> 16);
-
-	if (rsi_sdio_master_access_msword(adapter, msb_address)) {
-		rsi_dbg(ERR_ZONE,
-			"%s: Unable to set ms word reg\n", __func__);
-		return -1;
-	}
-
-	status = request_firmware(&fw_entry, FIRMWARE_RSI9113, adapter->device);
-	if (status < 0) {
-		rsi_dbg(ERR_ZONE, "%s Firmware file %s not found\n",
-			__func__, FIRMWARE_RSI9113);
-		return status;
-	}
-
-	/* Copy firmware into DMA-accessible memory */
-	fw = kmemdup(fw_entry->data, fw_entry->size, GFP_KERNEL);
-	if (!fw) {
-		status = -ENOMEM;
-		goto out;
-	}
-	len = fw_entry->size;
-
-	if (len % 4)
-		len += (4 - (len % 4));
-
-	num_blocks = (len / block_size);
-
-	rsi_dbg(INIT_ZONE, "%s: Instruction size:%d\n", __func__, len);
-	rsi_dbg(INIT_ZONE, "%s: num blocks: %d\n", __func__, num_blocks);
-
-	status = rsi_copy_to_card(common, fw, len, num_blocks);
-	kfree(fw);
-
-out:
-	release_firmware(fw_entry);
-	return status;
-}
-
-/**
  * rsi_process_pkt() - This Function reads rx_blocks register and figures out
  *		       the size of the rx pkt.
  * @common: Pointer to the driver private structure.
@@ -472,28 +306,6 @@ void rsi_interrupt_handler(struct rsi_hw *adapter)
 }
 
 /**
- * rsi_device_init() - This Function Initializes The HAL.
- * @common: Pointer to the driver private structure.
- *
- * Return: 0 on success, -1 on failure.
- */
-int rsi_sdio_device_init(struct rsi_common *common)
-{
-	if (rsi_load_ta_instructions(common))
-		return -1;
-
-	if (rsi_sdio_master_access_msword(common->priv, MISC_CFG_BASE_ADDR)) {
-		rsi_dbg(ERR_ZONE, "%s: Unable to set ms word reg\n",
-			__func__);
-		return -1;
-	}
-	rsi_dbg(INIT_ZONE,
-		"%s: Setting ms word to 0x41050000\n", __func__);
-
-	return 0;
-}
-
-/**
  * rsi_sdio_read_buffer_status_register() - This function is used to the read
  *					    buffer status register and set
  *					    relevant fields in
diff --git a/drivers/net/wireless/rsi/rsi_91x_usb.c b/drivers/net/wireless/rsi/rsi_91x_usb.c
index cc8deecea8cb..bcd7f454ef30 100644
--- a/drivers/net/wireless/rsi/rsi_91x_usb.c
+++ b/drivers/net/wireless/rsi/rsi_91x_usb.c
@@ -17,6 +17,7 @@
 
 #include <linux/module.h>
 #include "rsi_usb.h"
+#include "rsi_hal.h"
 
 /**
  * rsi_usb_card_write() - This function writes to the USB Card.
@@ -141,6 +142,9 @@ static int rsi_find_bulk_in_and_out_endpoints(struct usb_interface *interface,
 	return 0;
 }
 
+#define RSI_USB_REQ_OUT	(USB_TYPE_VENDOR | USB_DIR_OUT | USB_RECIP_DEVICE)
+#define RSI_USB_REQ_IN	(USB_TYPE_VENDOR | USB_DIR_IN | USB_RECIP_DEVICE)
+
 /* rsi_usb_reg_read() - This function reads data from given register address.
  * @usbdev: Pointer to the usb_device structure.
  * @reg: Address of the register to be read.
@@ -164,11 +168,11 @@ static int rsi_usb_reg_read(struct usb_device *usbdev,
 	status = usb_control_msg(usbdev,
 				 usb_rcvctrlpipe(usbdev, 0),
 				 USB_VENDOR_REGISTER_READ,
-				 USB_TYPE_VENDOR,
+				 RSI_USB_REQ_IN,
 				 ((reg & 0xffff0000) >> 16), (reg & 0xffff),
 				 (void *)buf,
 				 len,
-				 HZ * 5);
+				 USB_CTRL_GET_TIMEOUT);
 
 	*value = (buf[0] | (buf[1] << 8));
 	if (status < 0) {
@@ -211,12 +215,12 @@ static int rsi_usb_reg_write(struct usb_device *usbdev,
 	status = usb_control_msg(usbdev,
 				 usb_sndctrlpipe(usbdev, 0),
 				 USB_VENDOR_REGISTER_WRITE,
-				 USB_TYPE_VENDOR,
+				 RSI_USB_REQ_OUT,
 				 ((reg & 0xffff0000) >> 16),
 				 (reg & 0xffff),
 				 (void *)usb_reg_buf,
 				 len,
-				 HZ * 5);
+				 USB_CTRL_SET_TIMEOUT);
 	if (status < 0) {
 		rsi_dbg(ERR_ZONE,
 			"%s: Reg write failed with error code :%d\n",
@@ -273,6 +277,46 @@ static int rsi_rx_urb_submit(struct rsi_hw *adapter)
 	return status;
 }
 
+static int rsi_usb_read_register_multiple(struct rsi_hw *adapter, u32 addr,
+					  u8 *data, u16 count)
+{
+	struct rsi_91x_usbdev *dev = (struct rsi_91x_usbdev *)adapter->rsi_dev;
+	u8 *buf;
+	u16 transfer;
+	int status;
+
+	if (!addr)
+		return -EINVAL;
+
+	buf = kzalloc(RSI_USB_BUF_SIZE, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	while (count) {
+		transfer = min_t(u16, count, RSI_USB_BUF_SIZE);
+		status = usb_control_msg(dev->usbdev,
+					 usb_rcvctrlpipe(dev->usbdev, 0),
+					 USB_VENDOR_REGISTER_READ,
+					 RSI_USB_REQ_IN,
+					 ((addr & 0xffff0000) >> 16),
+					 (addr & 0xffff), (void *)buf,
+					 transfer, USB_CTRL_GET_TIMEOUT);
+		if (status < 0) {
+			rsi_dbg(ERR_ZONE,
+				"Reg read failed with error code :%d\n",
+				 status);
+			kfree(buf);
+			return status;
+		}
+		memcpy(data, buf, transfer);
+		count -= transfer;
+		data += transfer;
+		addr += transfer;
+	}
+	kfree(buf);
+	return 0;
+}
+
 /**
  * rsi_usb_write_register_multiple() - This function writes multiple bytes of
  *				       information to multiple registers.
@@ -283,41 +327,40 @@ static int rsi_rx_urb_submit(struct rsi_hw *adapter)
  *
  * Return: status: 0 on success, a negative error code on failure.
  */
-int rsi_usb_write_register_multiple(struct rsi_hw *adapter,
-				    u32 addr,
-				    u8 *data,
-				    u32 count)
+static int rsi_usb_write_register_multiple(struct rsi_hw *adapter, u32 addr,
+					   u8 *data, u16 count)
 {
 	struct rsi_91x_usbdev *dev = (struct rsi_91x_usbdev *)adapter->rsi_dev;
 	u8 *buf;
-	u8 transfer;
+	u16 transfer;
 	int status = 0;
 
-	buf = kzalloc(4096, GFP_KERNEL);
+	buf = kzalloc(RSI_USB_BUF_SIZE, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
 
 	while (count) {
-		transfer = (u8)(min_t(u32, count, 4096));
+		transfer = min_t(u16, count, RSI_USB_BUF_SIZE);
 		memcpy(buf, data, transfer);
 		status = usb_control_msg(dev->usbdev,
 					 usb_sndctrlpipe(dev->usbdev, 0),
 					 USB_VENDOR_REGISTER_WRITE,
-					 USB_TYPE_VENDOR,
+					 RSI_USB_REQ_OUT,
 					 ((addr & 0xffff0000) >> 16),
 					 (addr & 0xffff),
 					 (void *)buf,
 					 transfer,
-					 HZ * 5);
+					 USB_CTRL_SET_TIMEOUT);
 		if (status < 0) {
 			rsi_dbg(ERR_ZONE,
 				"Reg write failed with error code :%d\n",
 				status);
-		} else {
-			count -= transfer;
-			data += transfer;
-			addr += transfer;
+			kfree(buf);
+			return status;
 		}
+		count -= transfer;
+		data += transfer;
+		addr += transfer;
 	}
 
 	kfree(buf);
@@ -348,6 +391,77 @@ static int rsi_usb_host_intf_write_pkt(struct rsi_hw *adapter,
 				  len);
 }
 
+static int rsi_usb_master_reg_read(struct rsi_hw *adapter, u32 reg,
+				   u32 *value, u16 len)
+{
+	struct usb_device *usbdev =
+		((struct rsi_91x_usbdev *)adapter->rsi_dev)->usbdev;
+
+	return rsi_usb_reg_read(usbdev, reg, (u16 *)value, len);
+}
+
+static int rsi_usb_master_reg_write(struct rsi_hw *adapter,
+				    unsigned long reg,
+				    unsigned long value, u16 len)
+{
+	struct usb_device *usbdev =
+		((struct rsi_91x_usbdev *)adapter->rsi_dev)->usbdev;
+
+	return rsi_usb_reg_write(usbdev, reg, value, len);
+}
+
+static int rsi_usb_load_data_master_write(struct rsi_hw *adapter,
+					  u32 base_address,
+					  u32 instructions_sz, u16 block_size,
+					  u8 *ta_firmware)
+{
+	u16 num_blocks;
+	u32 cur_indx, i;
+	u8 temp_buf[256];
+	int status;
+
+	num_blocks = instructions_sz / block_size;
+	rsi_dbg(INFO_ZONE, "num_blocks: %d\n", num_blocks);
+
+	for (cur_indx = 0, i = 0; i < num_blocks; i++, cur_indx += block_size) {
+		memset(temp_buf, 0, block_size);
+		memcpy(temp_buf, ta_firmware + cur_indx, block_size);
+		status = rsi_usb_write_register_multiple(adapter, base_address,
+							 (u8 *)(temp_buf),
+							 block_size);
+		if (status < 0)
+			return status;
+
+		rsi_dbg(INFO_ZONE, "%s: loading block: %d\n", __func__, i);
+		base_address += block_size;
+	}
+
+	if (instructions_sz % block_size) {
+		memset(temp_buf, 0, block_size);
+		memcpy(temp_buf, ta_firmware + cur_indx,
+		       instructions_sz % block_size);
+		status = rsi_usb_write_register_multiple
+						(adapter, base_address,
+						 (u8 *)temp_buf,
+						 instructions_sz % block_size);
+		if (status < 0)
+			return status;
+		rsi_dbg(INFO_ZONE,
+			"Written Last Block in Address 0x%x Successfully\n",
+			cur_indx);
+	}
+	return 0;
+}
+
+static struct rsi_host_intf_ops usb_host_intf_ops = {
+	.write_pkt		= rsi_usb_host_intf_write_pkt,
+	.read_reg_multiple	= rsi_usb_read_register_multiple,
+	.write_reg_multiple	= rsi_usb_write_register_multiple,
+	.master_reg_read	= rsi_usb_master_reg_read,
+	.master_reg_write	= rsi_usb_master_reg_write,
+	.load_data_master_write	= rsi_usb_load_data_master_write,
+};
+
 /**
  * rsi_deinit_usb_interface() - This function deinitializes the usb interface.
  * @adapter: Pointer to the adapter structure.
@@ -410,12 +524,14 @@ static int rsi_init_usb_interface(struct rsi_hw *adapter,
 	}
 	rsi_dev->rx_usb_urb[0]->transfer_buffer = adapter->priv->rx_data_pkt;
 	rsi_dev->tx_blk_size = 252;
+	adapter->block_size = rsi_dev->tx_blk_size;
 
 	/* Initializing function callbacks */
 	adapter->rx_urb_submit = rsi_rx_urb_submit;
-	adapter->host_intf_write_pkt = rsi_usb_host_intf_write_pkt;
 	adapter->check_hw_queue_status = rsi_usb_check_queue_status;
 	adapter->determine_event_timeout = rsi_usb_event_timeout;
+	adapter->rsi_host_intf = RSI_HOST_INTF_USB;
+	adapter->host_intf_ops = &usb_host_intf_ops;
 
 	rsi_init_event(&rsi_dev->rx_thread.event);
 	status = rsi_create_kthread(common, &rsi_dev->rx_thread,
@@ -467,6 +583,7 @@ static int rsi_probe(struct usb_interface *pfunction,
 			__func__);
 		return -ENOMEM;
 	}
+	adapter->rsi_host_intf = RSI_HOST_INTF_USB;
 
 	status = rsi_init_usb_interface(adapter, pfunction);
 	if (status) {
@@ -480,25 +597,20 @@ static int rsi_probe(struct usb_interface *pfunction,
 	dev = (struct rsi_91x_usbdev *)adapter->rsi_dev;
 
 	status = rsi_usb_reg_read(dev->usbdev, FW_STATUS_REG, &fw_status, 2);
-	if (status)
+	if (status < 0)
 		goto err1;
 	else
 		fw_status &= 1;
 
 	if (!fw_status) {
-		status = rsi_usb_device_init(adapter->priv);
+		rsi_dbg(INIT_ZONE, "Loading firmware...\n");
+		status = rsi_hal_device_init(adapter);
 		if (status) {
 			rsi_dbg(ERR_ZONE, "%s: Failed in device init\n",
 				__func__);
 			goto err1;
 		}
-
-		status = rsi_usb_reg_write(dev->usbdev,
-					   USB_INTERNAL_REG_1,
-					   RSI_USB_READY_MAGIC_NUM, 1);
-		if (status)
-			goto err1;
-		rsi_dbg(INIT_ZONE, "%s: Performed device init\n", __func__);
+		rsi_dbg(INIT_ZONE, "%s: Device Init Done\n", __func__);
 	}
 
 	status = rsi_rx_urb_submit(adapter);
@@ -554,6 +666,7 @@ static const struct usb_device_id rsi_dev_table[] = {
 	{ USB_DEVICE(0x041B, 0x0301) },
 	{ USB_DEVICE(0x041B, 0x0201) },
 	{ USB_DEVICE(0x041B, 0x9330) },
+	{ USB_DEVICE(0x1618, 0x9113) },
 	{ /* Blank */},
 };
 
diff --git a/drivers/net/wireless/rsi/rsi_91x_usb_ops.c b/drivers/net/wireless/rsi/rsi_91x_usb_ops.c
index de4900862836..d3e0a07604a6 100644
--- a/drivers/net/wireless/rsi/rsi_91x_usb_ops.c
+++ b/drivers/net/wireless/rsi/rsi_91x_usb_ops.c
@@ -19,67 +19,6 @@
 #include "rsi_usb.h"
 
 /**
- * rsi_copy_to_card() - This function includes the actual funtionality of
- *			copying the TA firmware to the card.Basically this
- *			function includes opening the TA file,reading the TA
- *			file and writing their values in blocks of data.
- * @common: Pointer to the driver private structure.
- * @fw: Pointer to the firmware value to be written.
- * @len: length of firmware file.
- * @num_blocks: Number of blocks to be written to the card.
- *
- * Return: 0 on success and -1 on failure.
- */
-static int rsi_copy_to_card(struct rsi_common *common,
-			    const u8 *fw,
-			    u32 len,
-			    u32 num_blocks)
-{
-	struct rsi_hw *adapter = common->priv;
-	struct rsi_91x_usbdev *dev = (struct rsi_91x_usbdev *)adapter->rsi_dev;
-	u32 indx, ii;
-	u32 block_size = dev->tx_blk_size;
-	u32 lsb_address;
-	u32 base_address;
-
-	base_address = TA_LOAD_ADDRESS;
-
-	for (indx = 0, ii = 0; ii < num_blocks; ii++, indx += block_size) {
-		lsb_address = base_address;
-		if (rsi_usb_write_register_multiple(adapter,
-						    lsb_address,
-						    (u8 *)(fw + indx),
-						    block_size)) {
-			rsi_dbg(ERR_ZONE,
-				"%s: Unable to load %s blk\n", __func__,
-				FIRMWARE_RSI9113);
-			return -EIO;
-		}
-		rsi_dbg(INIT_ZONE, "%s: loading block: %d\n", __func__, ii);
-		base_address += block_size;
-	}
-
-	if (len % block_size) {
-		lsb_address = base_address;
-		if (rsi_usb_write_register_multiple(adapter,
-						    lsb_address,
-						    (u8 *)(fw + indx),
-						    len % block_size)) {
-			rsi_dbg(ERR_ZONE,
-				"%s: Unable to load %s blk\n", __func__,
-				FIRMWARE_RSI9113);
-			return -EIO;
-		}
-	}
-	rsi_dbg(INIT_ZONE,
-		"%s: Succesfully loaded %s instructions\n", __func__,
-		FIRMWARE_RSI9113);
-
-	rsi_dbg(INIT_ZONE, "%s: loaded firmware\n", __func__);
-	return 0;
-}
-
-/**
  * rsi_usb_rx_thread() - This is a kernel thread to receive the packets from
  *			 the USB device.
  * @common: Pointer to the driver private structure.
@@ -119,67 +58,3 @@ out:
 	complete_and_exit(&dev->rx_thread.completion, 0);
 }
 
-
-/**
- * rsi_load_ta_instructions() - This function includes the actual funtionality
- *				of loading the TA firmware.This function also
- *				includes opening the TA file,reading the TA
- *				file and writing their value in blocks of data.
- * @common: Pointer to the driver private structure.
- *
- * Return: status: 0 on success, -1 on failure.
- */
-static int rsi_load_ta_instructions(struct rsi_common *common)
-{
-	struct rsi_hw *adapter = common->priv;
-	struct rsi_91x_usbdev *dev = (struct rsi_91x_usbdev *)adapter->rsi_dev;
-	const struct firmware *fw_entry = NULL;
-	u32 block_size = dev->tx_blk_size;
-	const u8 *fw;
-	u32 num_blocks, len;
-	int status = 0;
-
-	status = request_firmware(&fw_entry, FIRMWARE_RSI9113, adapter->device);
-	if (status < 0) {
-		rsi_dbg(ERR_ZONE, "%s Firmware file %s not found\n",
-			__func__, FIRMWARE_RSI9113);
-		return status;
-	}
-
-	/* Copy firmware into DMA-accessible memory */
-	fw = kmemdup(fw_entry->data, fw_entry->size, GFP_KERNEL);
-	if (!fw) {
-		status = -ENOMEM;
-		goto out;
-	}
-	len = fw_entry->size;
-
-	if (len % 4)
-		len += (4 - (len % 4));
-
-	num_blocks = (len / block_size);
-
-	rsi_dbg(INIT_ZONE, "%s: Instruction size:%d\n", __func__, len);
-	rsi_dbg(INIT_ZONE, "%s: num blocks: %d\n", __func__, num_blocks);
-
-	status = rsi_copy_to_card(common, fw, len, num_blocks);
-	kfree(fw);
-
-out:
-	release_firmware(fw_entry);
-	return status;
-}
-
-/**
- * rsi_device_init() - This Function Initializes The HAL.
- * @common: Pointer to the driver private structure.
- *
- * Return: 0 on success, -1 on failure.
- */
-int rsi_usb_device_init(struct rsi_common *common)
-{
-	if (rsi_load_ta_instructions(common))
-		return -EIO;
-
-	return 0;
-		}
diff --git a/drivers/net/wireless/rsi/rsi_boot_params.h b/drivers/net/wireless/rsi/rsi_boot_params.h
index 5e2721f7909c..238ee96434ec 100644
--- a/drivers/net/wireless/rsi/rsi_boot_params.h
+++ b/drivers/net/wireless/rsi/rsi_boot_params.h
@@ -24,19 +24,19 @@
 #define WIFI_AFEPLL_CONFIGS              BIT(7)
 #define WIFI_SWITCH_CLK_CONFIGS          BIT(8)
 
-#define TA_PLL_M_VAL_20                  8
-#define TA_PLL_N_VAL_20                  1
+#define TA_PLL_M_VAL_20                  9
+#define TA_PLL_N_VAL_20                  0
 #define TA_PLL_P_VAL_20                  4
 
 #define PLL960_M_VAL_20                  0x14
 #define PLL960_N_VAL_20                  0
 #define PLL960_P_VAL_20                  5
 
-#define UMAC_CLK_40MHZ                   40
+#define UMAC_CLK_40MHZ                   80
 
-#define TA_PLL_M_VAL_40                  46
-#define TA_PLL_N_VAL_40                  3
-#define TA_PLL_P_VAL_40                  3
+#define TA_PLL_M_VAL_40                  9
+#define TA_PLL_N_VAL_40                  0
+#define TA_PLL_P_VAL_40                  4
 
 #define PLL960_M_VAL_40                  0x14
 #define PLL960_N_VAL_40                  0
@@ -122,5 +122,8 @@ struct bootup_params {
 	/* dcdc modes configs */
 	__le32 dcdc_operation_mode;
 	__le32 soc_reset_wait_cnt;
+	__le32 waiting_time_at_fresh_sleep;
+	__le32 max_threshold_to_avoid_sleep;
+	u8 beacon_resedue_alg_en;
 } __packed;
 #endif
diff --git a/drivers/net/wireless/rsi/rsi_common.h b/drivers/net/wireless/rsi/rsi_common.h
index d3fbe33d2324..44349696f5de 100644
--- a/drivers/net/wireless/rsi/rsi_common.h
+++ b/drivers/net/wireless/rsi/rsi_common.h
@@ -20,8 +20,7 @@
 #include <linux/kthread.h>
 
 #define EVENT_WAIT_FOREVER              0
-#define TA_LOAD_ADDRESS                 0x00
-#define FIRMWARE_RSI9113                "rsi_91x.fw"
+#define FIRMWARE_RSI9113                "rs9113_wlan_qspi.rps"
 #define QUEUE_NOT_FULL                  1
 #define QUEUE_FULL                      0
 
diff --git a/drivers/net/wireless/rsi/rsi_hal.h b/drivers/net/wireless/rsi/rsi_hal.h
new file mode 100644
index 000000000000..902dc540849c
--- /dev/null
+++ b/drivers/net/wireless/rsi/rsi_hal.h
@@ -0,0 +1,84 @@
+/**
+ * Copyright (c) 2017 Redpine Signals 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 __RSI_HAL_H__
+#define __RSI_HAL_H__
+
+#define FLASH_WRITE_CHUNK_SIZE		(4 * 1024)
+#define FLASH_SECTOR_SIZE		(4 * 1024)
+
+#define FLASH_SIZE_ADDR			0x04000016
+#define PING_BUFFER_ADDRESS		0x19000
+#define PONG_BUFFER_ADDRESS		0x1a000
+#define SWBL_REGIN			0x41050034
+#define SWBL_REGOUT			0x4105003c
+#define PING_WRITE			0x1
+#define PONG_WRITE			0x2
+
+#define BL_CMD_TIMEOUT			2000
+#define BL_BURN_TIMEOUT			(50 * 1000)
+
+#define REGIN_VALID			0xA
+#define REGIN_INPUT			0xA0
+#define REGOUT_VALID			0xAB
+#define REGOUT_INVALID			(~0xAB)
+#define CMD_PASS			0xAA
+#define CMD_FAIL			0xCC
+
+#define LOAD_HOSTED_FW			'A'
+#define BURN_HOSTED_FW			'B'
+#define PING_VALID			'I'
+#define PONG_VALID			'O'
+#define PING_AVAIL			'I'
+#define PONG_AVAIL			'O'
+#define EOF_REACHED			'E'
+#define CHECK_CRC			'K'
+#define POLLING_MODE			'P'
+#define CONFIG_AUTO_READ_MODE		'R'
+#define JUMP_TO_ZERO_PC			'J'
+#define FW_LOADING_SUCCESSFUL		'S'
+#define LOADING_INITIATED		'1'
+
+/* Boot loader commands */
+#define SEND_RPS_FILE			'2'
+
+#define FW_IMAGE_MIN_ADDRESS		(68 * 1024)
+#define MAX_FLASH_FILE_SIZE		(400 * 1024) //400K
+#define FLASH_START_ADDRESS		16
+
+#define COMMON_HAL_CARD_READY_IND	0x0
+
+#define COMMAN_HAL_WAIT_FOR_CARD_READY	1
+
+#define RSI_DEV_OPMODE_WIFI_ALONE	1
+#define RSI_DEV_COEX_MODE_WIFI_ALONE	1
+
+struct bl_header {
+	__le32 flags;
+	__le32 image_no;
+	__le32 check_sum;
+	__le32 flash_start_address;
+	__le32 flash_len;
+} __packed;
+
+struct ta_metadata {
+	char *name;
+	unsigned int address;
+};
+
+int rsi_hal_device_init(struct rsi_hw *adapter);
+
+#endif
diff --git a/drivers/net/wireless/rsi/rsi_main.h b/drivers/net/wireless/rsi/rsi_main.h
index 1d5904bc2c74..f3985250b593 100644
--- a/drivers/net/wireless/rsi/rsi_main.h
+++ b/drivers/net/wireless/rsi/rsi_main.h
@@ -31,13 +31,17 @@
 #define FSM_ZONE                        BIT(7)  /* For State Machine Msgs     */
 #define ISR_ZONE                        BIT(8)  /* For Interrupt Msgs         */
 
-#define FSM_CARD_NOT_READY              0
-#define FSM_BOOT_PARAMS_SENT            1
-#define FSM_EEPROM_READ_MAC_ADDR        2
-#define FSM_RESET_MAC_SENT              3
-#define FSM_RADIO_CAPS_SENT             4
-#define FSM_BB_RF_PROG_SENT             5
-#define FSM_MAC_INIT_DONE               6
+enum RSI_FSM_STATES {
+	FSM_FW_NOT_LOADED,
+	FSM_CARD_NOT_READY,
+	FSM_COMMON_DEV_PARAMS_SENT,
+	FSM_BOOT_PARAMS_SENT,
+	FSM_EEPROM_READ_MAC_ADDR,
+	FSM_RESET_MAC_SENT,
+	FSM_RADIO_CAPS_SENT,
+	FSM_BB_RF_PROG_SENT,
+	FSM_MAC_INIT_DONE
+};
 
 extern u32 rsi_zone_enabled;
 extern __printf(2, 3) void rsi_dbg(u32 zone, const char *fmt, ...);
@@ -60,6 +64,7 @@ extern __printf(2, 3) void rsi_dbg(u32 zone, const char *fmt, ...);
 #define MAX_CONTINUOUS_VI_PKTS          4
 
 /* Queue information */
+#define RSI_COEX_Q			0x0
 #define RSI_WIFI_MGMT_Q                 0x4
 #define RSI_WIFI_DATA_Q                 0x5
 #define IEEE80211_MGMT_FRAME            0x00
@@ -82,6 +87,8 @@ extern __printf(2, 3) void rsi_dbg(u32 zone, const char *fmt, ...);
 	((_q) == VI_Q) ? IEEE80211_AC_VI : \
 	IEEE80211_AC_VO)
 
+#define RSI_DEV_9113		1
+
 struct version_info {
 	u16 major;
 	u16 minor;
@@ -204,13 +211,26 @@ struct rsi_common {
 	struct cqm_info cqm_info;
 
 	bool hw_data_qs_blocked;
-	
+	u8 driver_mode;
+	u8 coex_mode;
+	u16 oper_mode;
+	u8 lp_ps_handshake_mode;
+	u8 ulp_ps_handshake_mode;
+	u8 rf_power_val;
+	u8 wlan_rf_power_mode;
+	u8 obm_ant_sel_val;
 	int tx_power;
 	u8 ant_in_use;
 };
 
+enum host_intf {
+	RSI_HOST_INTF_SDIO = 0,
+	RSI_HOST_INTF_USB
+};
+
 struct rsi_hw {
 	struct rsi_common *priv;
+	u8 device_model;
 	struct ieee80211_hw *hw;
 	struct ieee80211_vif *vifs[RSI_MAX_VIFS];
 	struct ieee80211_tx_queue_params edca_params[NUM_EDCA_QUEUES];
@@ -219,16 +239,40 @@ struct rsi_hw {
 	struct device *device;
 	u8 sc_nvifs;
 
+	enum host_intf rsi_host_intf;
+	u16 block_size;
+	u32 usb_buffer_status_reg;
 #ifdef CONFIG_RSI_DEBUGFS
 	struct rsi_debugfs *dfsentry;
 	u8 num_debugfs_entries;
 #endif
+	char *fw_file_name;
+	struct timer_list bl_cmd_timer;
+	bool blcmd_timer_expired;
+	u32 flash_capacity;
 	u8 dfs_region;
 	void *rsi_dev;
-	int (*host_intf_read_pkt)(struct rsi_hw *adapter, u8 *pkt, u32 len);
-	int (*host_intf_write_pkt)(struct rsi_hw *adapter, u8 *pkt, u32 len);
+	struct rsi_host_intf_ops *host_intf_ops;
 	int (*check_hw_queue_status)(struct rsi_hw *adapter, u8 q_num);
 	int (*rx_urb_submit)(struct rsi_hw *adapter);
 	int (*determine_event_timeout)(struct rsi_hw *adapter);
 };
+
+struct rsi_host_intf_ops {
+	int (*read_pkt)(struct rsi_hw *adapter, u8 *pkt, u32 len);
+	int (*write_pkt)(struct rsi_hw *adapter, u8 *pkt, u32 len);
+	int (*master_access_msword)(struct rsi_hw *adapter, u16 ms_word);
+	int (*read_reg_multiple)(struct rsi_hw *adapter, u32 addr,
+				 u8 *data, u16 count);
+	int (*write_reg_multiple)(struct rsi_hw *adapter, u32 addr,
+				  u8 *data, u16 count);
+	int (*master_reg_read)(struct rsi_hw *adapter, u32 addr,
+			       u32 *read_buf, u16 size);
+	int (*master_reg_write)(struct rsi_hw *adapter,
+				unsigned long addr, unsigned long data,
+				u16 size);
+	int (*load_data_master_write)(struct rsi_hw *adapter, u32 addr,
+				      u32 instructions_size, u16 block_size,
+				      u8 *fw);
+};
 #endif
diff --git a/drivers/net/wireless/rsi/rsi_mgmt.h b/drivers/net/wireless/rsi/rsi_mgmt.h
index dfbf7a50269b..dcb6db728cbd 100644
--- a/drivers/net/wireless/rsi/rsi_mgmt.h
+++ b/drivers/net/wireless/rsi/rsi_mgmt.h
@@ -205,6 +205,7 @@ enum cmd_frame_type {
 	CW_MODE_REQ,
 	PER_CMD_PKT,
 	ANT_SEL_FRAME = 0x20,
+	COMMON_DEV_CONFIG = 0x28,
 	RADIO_PARAMS_UPDATE = 0x29
 };
 
@@ -282,6 +283,76 @@ struct rsi_radio_caps {
 	__le16 preamble_type;
 } __packed;
 
+/* ULP GPIO flags */
+#define RSI_GPIO_MOTION_SENSOR_ULP_WAKEUP	BIT(0)
+#define RSI_GPIO_SLEEP_IND_FROM_DEVICE		BIT(1)
+#define RSI_GPIO_2_ULP				BIT(2)
+#define RSI_GPIO_PUSH_BUTTON_ULP_WAKEUP		BIT(3)
+
+/* SOC GPIO flags */
+#define RSI_GPIO_0_PSPI_CSN_0			BIT(0)
+#define RSI_GPIO_1_PSPI_CSN_1			BIT(1)
+#define RSI_GPIO_2_HOST_WAKEUP_INTR		BIT(2)
+#define RSI_GPIO_3_PSPI_DATA_0			BIT(3)
+#define RSI_GPIO_4_PSPI_DATA_1			BIT(4)
+#define RSI_GPIO_5_PSPI_DATA_2			BIT(5)
+#define RSI_GPIO_6_PSPI_DATA_3			BIT(6)
+#define RSI_GPIO_7_I2C_SCL			BIT(7)
+#define RSI_GPIO_8_I2C_SDA			BIT(8)
+#define RSI_GPIO_9_UART1_RX			BIT(9)
+#define RSI_GPIO_10_UART1_TX			BIT(10)
+#define RSI_GPIO_11_UART1_RTS_I2S_CLK		BIT(11)
+#define RSI_GPIO_12_UART1_CTS_I2S_WS		BIT(12)
+#define RSI_GPIO_13_DBG_UART_RX_I2S_DIN		BIT(13)
+#define RSI_GPIO_14_DBG_UART_RX_I2S_DOUT	BIT(14)
+#define RSI_GPIO_15_LP_WAKEUP_BOOT_BYPASS	BIT(15)
+#define RSI_GPIO_16_LED_0			BIT(16)
+#define RSI_GPIO_17_BTCOEX_WLAN_ACT_EXT_ANT_SEL	BIT(17)
+#define RSI_GPIO_18_BTCOEX_BT_PRIO_EXT_ANT_SEL	BIT(18)
+#define RSI_GPIO_19_BTCOEX_BT_ACT_EXT_ON_OFF	BIT(19)
+#define RSI_GPIO_20_RF_RESET			BIT(20)
+#define RSI_GPIO_21_SLEEP_IND_FROM_DEVICE	BIT(21)
+
+#define RSI_UNUSED_SOC_GPIO_BITMAP (RSI_GPIO_9_UART1_RX | \
+				    RSI_GPIO_10_UART1_TX | \
+				    RSI_GPIO_11_UART1_RTS_I2S_CLK | \
+				    RSI_GPIO_12_UART1_CTS_I2S_WS | \
+				    RSI_GPIO_13_DBG_UART_RX_I2S_DIN | \
+				    RSI_GPIO_14_DBG_UART_RX_I2S_DOUT | \
+				    RSI_GPIO_15_LP_WAKEUP_BOOT_BYPASS | \
+				    RSI_GPIO_17_BTCOEX_WLAN_ACT_EXT_ANT_SEL | \
+				    RSI_GPIO_18_BTCOEX_BT_PRIO_EXT_ANT_SEL | \
+				    RSI_GPIO_19_BTCOEX_BT_ACT_EXT_ON_OFF | \
+				    RSI_GPIO_21_SLEEP_IND_FROM_DEVICE)
+
+#define RSI_UNUSED_ULP_GPIO_BITMAP (RSI_GPIO_MOTION_SENSOR_ULP_WAKEUP | \
+				    RSI_GPIO_SLEEP_IND_FROM_DEVICE | \
+				    RSI_GPIO_2_ULP | \
+				    RSI_GPIO_PUSH_BUTTON_ULP_WAKEUP);
+struct rsi_config_vals {
+	__le16 len_qno;
+	u8 pkt_type;
+	u8 misc_flags;
+	__le16 reserved1[6];
+	u8 lp_ps_handshake;
+	u8 ulp_ps_handshake;
+	u8 sleep_config_params; /* 0 for no handshake,
+				 * 1 for GPIO based handshake,
+				 * 2 packet handshake
+				 */
+	u8 unused_ulp_gpio;
+	__le32 unused_soc_gpio_bitmap;
+	u8 ext_pa_or_bt_coex_en;
+	u8 opermode;
+	u8 wlan_rf_pwr_mode;
+	u8 bt_rf_pwr_mode;
+	u8 zigbee_rf_pwr_mode;
+	u8 driver_mode;
+	u8 region_code;
+	u8 antenna_sel_val;
+	u8 reserved2[16];
+} __packed;
+
 static inline u32 rsi_get_queueno(u8 *addr, u16 offset)
 {
 	return (le16_to_cpu(*(__le16 *)&addr[offset]) & 0x7000) >> 12;
@@ -307,6 +378,11 @@ static inline u8 rsi_get_channel(u8 *addr)
 	return *(char *)(addr + 15);
 }
 
+static inline void rsi_set_len_qno(__le16 *addr, u16 len, u8 qno)
+{
+	*addr = cpu_to_le16(len | ((qno & 7) << 12));
+}
+
 int rsi_mgmt_pkt_recv(struct rsi_common *common, u8 *msg);
 int rsi_set_vap_capabilities(struct rsi_common *common, enum opmode mode,
 			     u8 vap_status);
diff --git a/drivers/net/wireless/rsi/rsi_sdio.h b/drivers/net/wireless/rsi/rsi_sdio.h
index c7e8f2be7901..9fb73f68282a 100644
--- a/drivers/net/wireless/rsi/rsi_sdio.h
+++ b/drivers/net/wireless/rsi/rsi_sdio.h
@@ -110,19 +110,19 @@ struct rsi_91x_sdiodev {
 	u8 sdio_clock_speed;
 	u32 cardcapability;
 	u8 prev_desc[16];
-	u32 tx_blk_size;
+	u16 tx_blk_size;
 	u8 write_fail;
 };
 
 void rsi_interrupt_handler(struct rsi_hw *adapter);
 int rsi_init_sdio_slave_regs(struct rsi_hw *adapter);
-int rsi_sdio_device_init(struct rsi_common *common);
 int rsi_sdio_read_register(struct rsi_hw *adapter, u32 addr, u8 *data);
 int rsi_sdio_host_intf_read_pkt(struct rsi_hw *adapter, u8 *pkt, u32 length);
 int rsi_sdio_write_register(struct rsi_hw *adapter, u8 function,
 			    u32 addr, u8 *data);
 int rsi_sdio_write_register_multiple(struct rsi_hw *adapter, u32 addr,
-				     u8 *data, u32 count);
+				     u8 *data, u16 count);
+int rsi_sdio_master_access_msword(struct rsi_hw *adapter, u16 ms_word);
 void rsi_sdio_ack_intr(struct rsi_hw *adapter, u8 int_bit);
 int rsi_sdio_determine_event_timeout(struct rsi_hw *adapter);
 int rsi_sdio_read_buffer_status_register(struct rsi_hw *adapter, u8 q_num);
diff --git a/drivers/net/wireless/rsi/rsi_usb.h b/drivers/net/wireless/rsi/rsi_usb.h
index ebea0c411ead..59513ac61fb3 100644
--- a/drivers/net/wireless/rsi/rsi_usb.h
+++ b/drivers/net/wireless/rsi/rsi_usb.h
@@ -35,6 +35,8 @@
 #define MGMT_EP                      1
 #define DATA_EP                      2
 
+#define RSI_USB_BUF_SIZE	     4096
+
 struct rsi_91x_usbdev {
 	struct rsi_thread rx_thread;
 	u8 endpoint;
@@ -61,8 +63,5 @@ static inline int rsi_usb_event_timeout(struct rsi_hw *adapter)
 	return EVENT_WAIT_FOREVER;
 }
 
-int rsi_usb_device_init(struct rsi_common *common);
-int rsi_usb_write_register_multiple(struct rsi_hw *adapter, u32 addr,
-				    u8 *data, u32 count);
 void rsi_usb_rx_thread(struct rsi_common *common);
 #endif
diff --git a/drivers/net/wireless/st/cw1200/cw1200_sdio.c b/drivers/net/wireless/st/cw1200/cw1200_sdio.c
index 709f56e5ad87..1037ec62659d 100644
--- a/drivers/net/wireless/st/cw1200/cw1200_sdio.c
+++ b/drivers/net/wireless/st/cw1200/cw1200_sdio.c
@@ -266,7 +266,7 @@ static int cw1200_sdio_pm(struct hwbus_priv *self, bool suspend)
 	return ret;
 }
 
-static struct hwbus_ops cw1200_sdio_hwbus_ops = {
+static const struct hwbus_ops cw1200_sdio_hwbus_ops = {
 	.hwbus_memcpy_fromio	= cw1200_sdio_memcpy_fromio,
 	.hwbus_memcpy_toio	= cw1200_sdio_memcpy_toio,
 	.lock			= cw1200_sdio_lock,
diff --git a/drivers/net/wireless/st/cw1200/cw1200_spi.c b/drivers/net/wireless/st/cw1200/cw1200_spi.c
index 63f95e9c2992..412fb6e49aed 100644
--- a/drivers/net/wireless/st/cw1200/cw1200_spi.c
+++ b/drivers/net/wireless/st/cw1200/cw1200_spi.c
@@ -352,7 +352,7 @@ static int cw1200_spi_pm(struct hwbus_priv *self, bool suspend)
 	return irq_set_irq_wake(self->func->irq, suspend);
 }
 
-static struct hwbus_ops cw1200_spi_hwbus_ops = {
+static const struct hwbus_ops cw1200_spi_hwbus_ops = {
 	.hwbus_memcpy_fromio	= cw1200_spi_memcpy_fromio,
 	.hwbus_memcpy_toio	= cw1200_spi_memcpy_toio,
 	.lock			= cw1200_spi_lock,
diff --git a/drivers/net/wireless/st/cw1200/scan.c b/drivers/net/wireless/st/cw1200/scan.c
index 0a0ff7e31f5b..cc2ce60f4f09 100644
--- a/drivers/net/wireless/st/cw1200/scan.c
+++ b/drivers/net/wireless/st/cw1200/scan.c
@@ -84,7 +84,7 @@ int cw1200_hw_scan(struct ieee80211_hw *hw,
 		return -ENOMEM;
 
 	if (req->ie_len)
-		memcpy(skb_put(frame.skb, req->ie_len), req->ie, req->ie_len);
+		skb_put_data(frame.skb, req->ie, req->ie_len);
 
 	/* will be unlocked in cw1200_scan_work() */
 	down(&priv->scan.lock);
diff --git a/drivers/net/wireless/st/cw1200/txrx.c b/drivers/net/wireless/st/cw1200/txrx.c
index cd63ffef025a..e9050b41157a 100644
--- a/drivers/net/wireless/st/cw1200/txrx.c
+++ b/drivers/net/wireless/st/cw1200/txrx.c
@@ -574,7 +574,7 @@ cw1200_tx_h_wsm(struct cw1200_common *priv,
 		return NULL;
 	}
 
-	wsm = (struct wsm_tx *)skb_push(t->skb, sizeof(struct wsm_tx));
+	wsm = skb_push(t->skb, sizeof(struct wsm_tx));
 	t->txpriv.offset += sizeof(struct wsm_tx);
 	memset(wsm, 0, sizeof(*wsm));
 	wsm->hdr.len = __cpu_to_le16(t->skb->len);
diff --git a/drivers/net/wireless/ti/wl1251/main.c b/drivers/net/wireless/ti/wl1251/main.c
index bbf7604889b7..08f0477f78d9 100644
--- a/drivers/net/wireless/ti/wl1251/main.c
+++ b/drivers/net/wireless/ti/wl1251/main.c
@@ -1036,7 +1036,7 @@ static int wl1251_op_hw_scan(struct ieee80211_hw *hw,
 		goto out_idle;
 	}
 	if (req->ie_len)
-		memcpy(skb_put(skb, req->ie_len), req->ie, req->ie_len);
+		skb_put_data(skb, req->ie, req->ie_len);
 
 	ret = wl1251_cmd_template_set(wl, CMD_PROBE_REQ, skb->data,
 				      skb->len);
diff --git a/drivers/net/wireless/ti/wl1251/tx.c b/drivers/net/wireless/ti/wl1251/tx.c
index 81de83c6fcf6..de2fa6705574 100644
--- a/drivers/net/wireless/ti/wl1251/tx.c
+++ b/drivers/net/wireless/ti/wl1251/tx.c
@@ -161,8 +161,7 @@ static int wl1251_tx_fill_hdr(struct wl1251 *wl, struct sk_buff *skb,
 		return id;
 
 	fc = *(u16 *)skb->data;
-	tx_hdr = (struct tx_double_buffer_desc *) skb_push(skb,
-							   sizeof(*tx_hdr));
+	tx_hdr = skb_push(skb, sizeof(*tx_hdr));
 
 	tx_hdr->length = cpu_to_le16(skb->len - sizeof(*tx_hdr));
 	rate = ieee80211_get_tx_rate(wl->hw, control);
diff --git a/drivers/net/wireless/ti/wl18xx/main.c b/drivers/net/wireless/ti/wl18xx/main.c
index d1aa3eee0e81..0cf3b4013dd6 100644
--- a/drivers/net/wireless/ti/wl18xx/main.c
+++ b/drivers/net/wireless/ti/wl18xx/main.c
@@ -793,9 +793,13 @@ static int wl18xx_set_clk(struct wl1271 *wl)
 		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);
+		if (ret < 0)
+			goto out;
 	} else {
 		ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_SWALLOW_EN,
 					   PLLSH_WCS_PLL_SWALLOW_EN_VAL2);
+		if (ret < 0)
+			goto out;
 	}
 
 	/* choose WCS PLL */
@@ -819,8 +823,6 @@ static int wl18xx_set_clk(struct wl1271 *wl)
 	/* reset the swallowing logic */
 	ret = wl18xx_top_reg_write(wl, PLLSH_COEX_PLL_SWALLOW_EN,
 				   PLLSH_COEX_PLL_SWALLOW_EN_VAL2);
-	if (ret < 0)
-		goto out;
 
 out:
 	return ret;
diff --git a/drivers/net/wireless/ti/wlcore/cmd.c b/drivers/net/wireless/ti/wlcore/cmd.c
index 7f4da727bb7b..2bfc12fdc929 100644
--- a/drivers/net/wireless/ti/wlcore/cmd.c
+++ b/drivers/net/wireless/ti/wlcore/cmd.c
@@ -1156,9 +1156,9 @@ int wl12xx_cmd_build_probe_req(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 		goto out;
 	}
 	if (ie0_len)
-		memcpy(skb_put(skb, ie0_len), ie0, ie0_len);
+		skb_put_data(skb, ie0, ie0_len);
 	if (ie1_len)
-		memcpy(skb_put(skb, ie1_len), ie1, ie1_len);
+		skb_put_data(skb, ie1, ie1_len);
 
 	if (sched_scan &&
 	    (wl->quirks & WLCORE_QUIRK_DUAL_PROBE_TMPL)) {
@@ -1233,8 +1233,7 @@ int wl1271_cmd_build_arp_rsp(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 
 	skb_reserve(skb, sizeof(*hdr) + WL1271_EXTRA_SPACE_MAX);
 
-	tmpl = (struct wl12xx_arp_rsp_template *)skb_put(skb, sizeof(*tmpl));
-	memset(tmpl, 0, sizeof(*tmpl));
+	tmpl = skb_put_zero(skb, sizeof(*tmpl));
 
 	/* llc layer */
 	memcpy(tmpl->llc_hdr, rfc1042_header, sizeof(rfc1042_header));
@@ -1283,7 +1282,7 @@ int wl1271_cmd_build_arp_rsp(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 		memset(skb_push(skb, sizeof(__le16)), 0, sizeof(__le16));
 
 	/* mac80211 header */
-	hdr = (struct ieee80211_hdr_3addr *)skb_push(skb, sizeof(*hdr));
+	hdr = skb_push(skb, sizeof(*hdr));
 	memset(hdr, 0, sizeof(*hdr));
 	fc = IEEE80211_FTYPE_DATA | IEEE80211_FCTL_TODS;
 	if (wlvif->sta.qos)
diff --git a/drivers/net/wireless/ti/wlcore/debugfs.c b/drivers/net/wireless/ti/wlcore/debugfs.c
index de7e2a5fdffa..a2cb408be8aa 100644
--- a/drivers/net/wireless/ti/wlcore/debugfs.c
+++ b/drivers/net/wireless/ti/wlcore/debugfs.c
@@ -1149,15 +1149,9 @@ static ssize_t dev_mem_write(struct file *file, const char __user *user_buf,
 	part.mem.start = *ppos;
 	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;
-	}
+	buf = memdup_user(user_buf, bytes);
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
 	mutex_lock(&wl->mutex);
 
@@ -1197,7 +1191,6 @@ skip_write:
 	if (ret == 0)
 		*ppos += bytes;
 
-err_out:
 	kfree(buf);
 
 	return ((ret == 0) ? bytes : ret);
diff --git a/drivers/net/wireless/ti/wlcore/main.c b/drivers/net/wireless/ti/wlcore/main.c
index 382ec15ec1af..60aaa850fbd1 100644
--- a/drivers/net/wireless/ti/wlcore/main.c
+++ b/drivers/net/wireless/ti/wlcore/main.c
@@ -1308,13 +1308,12 @@ static struct sk_buff *wl12xx_alloc_dummy_packet(struct wl1271 *wl)
 
 	skb_reserve(skb, sizeof(struct wl1271_tx_hw_descr));
 
-	hdr = (struct ieee80211_hdr_3addr *) skb_put(skb, sizeof(*hdr));
-	memset(hdr, 0, sizeof(*hdr));
+	hdr = skb_put_zero(skb, sizeof(*hdr));
 	hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
 					 IEEE80211_STYPE_NULLFUNC |
 					 IEEE80211_FCTL_TODS);
 
-	memset(skb_put(skb, dummy_packet_size), 0, dummy_packet_size);
+	skb_put_zero(skb, dummy_packet_size);
 
 	/* Dummy packets require the TID to be management */
 	skb->priority = WL1271_TID_MGMT;
diff --git a/drivers/net/wireless/ti/wlcore/rx.c b/drivers/net/wireless/ti/wlcore/rx.c
index 52a55f9acd80..0f15696195f8 100644
--- a/drivers/net/wireless/ti/wlcore/rx.c
+++ b/drivers/net/wireless/ti/wlcore/rx.c
@@ -117,7 +117,6 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 	struct wl1271_rx_descriptor *desc;
 	struct sk_buff *skb;
 	struct ieee80211_hdr *hdr;
-	u8 *buf;
 	u8 beacon = 0;
 	u8 is_data = 0;
 	u8 reserved = 0, offset_to_data = 0;
@@ -174,15 +173,13 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 	/* reserve the unaligned payload(if any) */
 	skb_reserve(skb, reserved);
 
-	buf = skb_put(skb, pkt_data_len);
-
 	/*
 	 * Copy packets from aggregation buffer to the skbs without rx
 	 * descriptor and with packet payload aligned care. In case of unaligned
 	 * packets copy the packets in offset of 2 bytes guarantee IP header
 	 * payload aligned to 4 bytes.
 	 */
-	memcpy(buf, data + sizeof(*desc), pkt_data_len);
+	skb_put_data(skb, data + sizeof(*desc), pkt_data_len);
 	if (rx_align == WLCORE_RX_BUF_PADDED)
 		skb_pull(skb, RX_BUF_ALIGN);
 
diff --git a/drivers/net/wireless/ti/wlcore/sdio.c b/drivers/net/wireless/ti/wlcore/sdio.c
index 287023ef4a78..2fb38717346f 100644
--- a/drivers/net/wireless/ti/wlcore/sdio.c
+++ b/drivers/net/wireless/ti/wlcore/sdio.c
@@ -237,6 +237,7 @@ static const struct of_device_id wlcore_sdio_of_match_table[] = {
 	{ .compatible = "ti,wl1273", .data = &wl127x_data },
 	{ .compatible = "ti,wl1281", .data = &wl128x_data },
 	{ .compatible = "ti,wl1283", .data = &wl128x_data },
+	{ .compatible = "ti,wl1285", .data = &wl128x_data },
 	{ .compatible = "ti,wl1801", .data = &wl18xx_data },
 	{ .compatible = "ti,wl1805", .data = &wl18xx_data },
 	{ .compatible = "ti,wl1807", .data = &wl18xx_data },
diff --git a/drivers/net/wireless/ti/wlcore/spi.c b/drivers/net/wireless/ti/wlcore/spi.c
index f949ad2bd898..fdabb9242cca 100644
--- a/drivers/net/wireless/ti/wlcore/spi.c
+++ b/drivers/net/wireless/ti/wlcore/spi.c
@@ -70,10 +70,10 @@
 #define WSPI_MAX_CHUNK_SIZE    4092
 
 /*
- * wl18xx driver aggregation buffer size is (13 * PAGE_SIZE) compared to
- * (4 * PAGE_SIZE) for wl12xx, so use the larger buffer needed for wl18xx
+ * wl18xx driver aggregation buffer size is (13 * 4K) compared to
+ * (4 * 4K) for wl12xx, so use the larger buffer needed for wl18xx
  */
-#define SPI_AGGR_BUFFER_SIZE (13 * PAGE_SIZE)
+#define SPI_AGGR_BUFFER_SIZE (13 * SZ_4K)
 
 /* Maximum number of SPI write chunks */
 #define WSPI_MAX_NUM_OF_CHUNKS \
@@ -366,17 +366,14 @@ static int __wl12xx_spi_raw_write(struct device *child, int addr,
 static int __must_check wl12xx_spi_raw_write(struct device *child, int addr,
 					     void *buf, size_t len, bool fixed)
 {
-	int ret;
-
 	/* The ELP wakeup write may fail the first time due to internal
 	 * hardware latency. It is safer to send the wakeup command twice to
 	 * avoid unexpected failures.
 	 */
 	if (addr == HW_ACCESS_ELP_CTRL_REG)
-		ret = __wl12xx_spi_raw_write(child, addr, buf, len, fixed);
-	ret = __wl12xx_spi_raw_write(child, addr, buf, len, fixed);
+		__wl12xx_spi_raw_write(child, addr, buf, len, fixed);
 
-	return ret;
+	return __wl12xx_spi_raw_write(child, addr, buf, len, fixed);
 }
 
 /**
@@ -433,6 +430,7 @@ static const struct of_device_id wlcore_spi_of_match_table[] = {
 	{ .compatible = "ti,wl1273", .data = &wl127x_data},
 	{ .compatible = "ti,wl1281", .data = &wl128x_data},
 	{ .compatible = "ti,wl1283", .data = &wl128x_data},
+	{ .compatible = "ti,wl1285", .data = &wl128x_data},
 	{ .compatible = "ti,wl1801", .data = &wl18xx_data},
 	{ .compatible = "ti,wl1805", .data = &wl18xx_data},
 	{ .compatible = "ti,wl1807", .data = &wl18xx_data},
diff --git a/drivers/net/wireless/ti/wlcore/tx.c b/drivers/net/wireless/ti/wlcore/tx.c
index c1b8e4e9d70b..a3f5e9ca492a 100644
--- a/drivers/net/wireless/ti/wlcore/tx.c
+++ b/drivers/net/wireless/ti/wlcore/tx.c
@@ -223,8 +223,7 @@ static int wl1271_tx_allocate(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	total_blocks = wlcore_hw_calc_tx_blocks(wl, total_len, spare_blocks);
 
 	if (total_blocks <= wl->tx_blocks_available) {
-		desc = (struct wl1271_tx_hw_descr *)skb_push(
-			skb, total_len - skb->len);
+		desc = skb_push(skb, total_len - skb->len);
 
 		wlcore_hw_set_tx_desc_blocks(wl, desc, total_blocks,
 					     spare_blocks);
diff --git a/drivers/net/wireless/zydas/zd1201.c b/drivers/net/wireless/zydas/zd1201.c
index de7ff395977a..7f586d76cf17 100644
--- a/drivers/net/wireless/zydas/zd1201.c
+++ b/drivers/net/wireless/zydas/zd1201.c
@@ -326,13 +326,13 @@ static void zd1201_usbrx(struct urb *urb)
 			if (!(skb = dev_alloc_skb(datalen+24)))
 				goto resubmit;
 			
-			memcpy(skb_put(skb, 2), &data[datalen-16], 2);
-			memcpy(skb_put(skb, 2), &data[datalen-2], 2);
-			memcpy(skb_put(skb, 6), &data[datalen-14], 6);
-			memcpy(skb_put(skb, 6), &data[datalen-22], 6);
-			memcpy(skb_put(skb, 6), &data[datalen-8], 6);
-			memcpy(skb_put(skb, 2), &data[datalen-24], 2);
-			memcpy(skb_put(skb, len), data, len);
+			skb_put_data(skb, &data[datalen - 16], 2);
+			skb_put_data(skb, &data[datalen - 2], 2);
+			skb_put_data(skb, &data[datalen - 14], 6);
+			skb_put_data(skb, &data[datalen - 22], 6);
+			skb_put_data(skb, &data[datalen - 8], 6);
+			skb_put_data(skb, &data[datalen - 24], 2);
+			skb_put_data(skb, data, len);
 			skb->protocol = eth_type_trans(skb, zd->dev);
 			zd->dev->stats.rx_packets++;
 			zd->dev->stats.rx_bytes += skb->len;
@@ -359,9 +359,9 @@ static void zd1201_usbrx(struct urb *urb)
 				frag->skb = skb;
 				frag->seq = seq & IEEE80211_SCTL_SEQ;
 				skb_reserve(skb, 2);
-				memcpy(skb_put(skb, 12), &data[datalen-14], 12);
-				memcpy(skb_put(skb, 2), &data[6], 2);
-				memcpy(skb_put(skb, len), data+8, len);
+				skb_put_data(skb, &data[datalen - 14], 12);
+				skb_put_data(skb, &data[6], 2);
+				skb_put_data(skb, data + 8, len);
 				hlist_add_head(&frag->fnode, &zd->fraglist);
 				goto resubmit;
 			}
@@ -385,9 +385,9 @@ static void zd1201_usbrx(struct urb *urb)
 			if (!skb)
 				goto resubmit;
 			skb_reserve(skb, 2);
-			memcpy(skb_put(skb, 12), &data[datalen-14], 12);
-			memcpy(skb_put(skb, 2), &data[6], 2);
-			memcpy(skb_put(skb, len), data+8, len);
+			skb_put_data(skb, &data[datalen - 14], 12);
+			skb_put_data(skb, &data[6], 2);
+			skb_put_data(skb, data + 8, len);
 		}
 		skb->protocol = eth_type_trans(skb, zd->dev);
 		zd->dev->stats.rx_packets++;
diff --git a/drivers/net/wireless/zydas/zd1211rw/zd_mac.c b/drivers/net/wireless/zydas/zd1211rw/zd_mac.c
index fe6517a621b0..b785742bfd9e 100644
--- a/drivers/net/wireless/zydas/zd1211rw/zd_mac.c
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_mac.c
@@ -868,8 +868,7 @@ static int fill_ctrlset(struct zd_mac *mac,
 	unsigned int frag_len = skb->len + FCS_LEN;
 	unsigned int packet_length;
 	struct ieee80211_rate *txrate;
-	struct zd_ctrlset *cs = (struct zd_ctrlset *)
-		skb_push(skb, sizeof(struct zd_ctrlset));
+	struct zd_ctrlset *cs = skb_push(skb, sizeof(struct zd_ctrlset));
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
 	ZD_ASSERT(frag_len <= 0xffff);
@@ -1103,7 +1102,7 @@ int zd_mac_rx(struct ieee80211_hw *hw, const u8 *buffer, unsigned int length)
 	}
 
 	/* FIXME : could we avoid this big memcpy ? */
-	memcpy(skb_put(skb, length), buffer, length);
+	skb_put_data(skb, buffer, length);
 
 	memcpy(IEEE80211_SKB_RXCB(skb), &stats, sizeof(stats));
 	ieee80211_rx_irqsafe(hw, skb);
diff --git a/drivers/net/xen-netback/common.h b/drivers/net/xen-netback/common.h
index 530586be05b4..5b1d2e8402d9 100644
--- a/drivers/net/xen-netback/common.h
+++ b/drivers/net/xen-netback/common.h
@@ -199,6 +199,7 @@ struct xenvif_queue { /* Per-queue data for xenvif */
 	unsigned long   remaining_credit;
 	struct timer_list credit_timeout;
 	u64 credit_window_start;
+	bool rate_limited;
 
 	/* Statistics */
 	struct xenvif_stats stats;
diff --git a/drivers/net/xen-netback/interface.c b/drivers/net/xen-netback/interface.c
index 8397f6c92451..e322a862ddfe 100644
--- a/drivers/net/xen-netback/interface.c
+++ b/drivers/net/xen-netback/interface.c
@@ -106,7 +106,11 @@ static int xenvif_poll(struct napi_struct *napi, int budget)
 
 	if (work_done < budget) {
 		napi_complete_done(napi, work_done);
-		xenvif_napi_schedule_or_enable_events(queue);
+		/* If the queue is rate-limited, it shall be
+		 * rescheduled in the timer callback.
+		 */
+		if (likely(!queue->rate_limited))
+			xenvif_napi_schedule_or_enable_events(queue);
 	}
 
 	return work_done;
diff --git a/drivers/net/xen-netback/netback.c b/drivers/net/xen-netback/netback.c
index 602d408fa25e..5042ff8d449a 100644
--- a/drivers/net/xen-netback/netback.c
+++ b/drivers/net/xen-netback/netback.c
@@ -180,6 +180,7 @@ static void tx_add_credit(struct xenvif_queue *queue)
 		max_credit = ULONG_MAX; /* wrapped: clamp to ULONG_MAX */
 
 	queue->remaining_credit = min(max_credit, max_burst);
+	queue->rate_limited = false;
 }
 
 void xenvif_tx_credit_callback(unsigned long data)
@@ -686,8 +687,10 @@ static bool tx_credit_exceeded(struct xenvif_queue *queue, unsigned size)
 		msecs_to_jiffies(queue->credit_usec / 1000);
 
 	/* Timer could already be pending in rare cases. */
-	if (timer_pending(&queue->credit_timeout))
+	if (timer_pending(&queue->credit_timeout)) {
+		queue->rate_limited = true;
 		return true;
+	}
 
 	/* Passed the point where we can replenish credit? */
 	if (time_after_eq64(now, next_credit)) {
@@ -702,6 +705,7 @@ static bool tx_credit_exceeded(struct xenvif_queue *queue, unsigned size)
 		mod_timer(&queue->credit_timeout,
 			  next_credit);
 		queue->credit_window_start = next_credit;
+		queue->rate_limited = true;
 
 		return true;
 	}
diff --git a/drivers/nfc/Kconfig b/drivers/nfc/Kconfig
index c4208487fadc..b065eb605215 100644
--- a/drivers/nfc/Kconfig
+++ b/drivers/nfc/Kconfig
@@ -7,7 +7,7 @@ menu "Near Field Communication (NFC) devices"
 
 config NFC_TRF7970A
 	tristate "Texas Instruments TRF7970a NFC driver"
-	depends on SPI && NFC_DIGITAL
+	depends on SPI && NFC_DIGITAL && GPIOLIB
 	help
 	  This option enables the NFC driver for Texas Instruments' TRF7970a
 	  device. Such device supports 5 different protocols: ISO14443A,
diff --git a/drivers/nfc/fdp/fdp.c b/drivers/nfc/fdp/fdp.c
index 7c1eaea3b685..ec50027b0d8b 100644
--- a/drivers/nfc/fdp/fdp.c
+++ b/drivers/nfc/fdp/fdp.c
@@ -228,8 +228,7 @@ static int fdp_nci_send_patch(struct nci_dev *ndev, u8 conn_id, u8 type)
 
 		skb_reserve(skb, NCI_CTRL_HDR_SIZE);
 
-		memcpy(skb_put(skb, payload_size), fw->data + (fw->size - len),
-		       payload_size);
+		skb_put_data(skb, fw->data + (fw->size - len), payload_size);
 
 		rc = nci_send_data(ndev, conn_id, skb);
 
@@ -750,11 +749,9 @@ int fdp_nci_probe(struct fdp_i2c_phy *phy, struct nfc_phy_ops *phy_ops,
 	u32 protocols;
 	int r;
 
-	info = kzalloc(sizeof(struct fdp_nci_info), GFP_KERNEL);
-	if (!info) {
-		r = -ENOMEM;
-		goto err_info_alloc;
-	}
+	info = devm_kzalloc(dev, sizeof(struct fdp_nci_info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
 
 	info->phy = phy;
 	info->phy_ops = phy_ops;
@@ -776,8 +773,7 @@ int fdp_nci_probe(struct fdp_i2c_phy *phy, struct nfc_phy_ops *phy_ops,
 				   tx_tailroom);
 	if (!ndev) {
 		nfc_err(dev, "Cannot allocate nfc ndev\n");
-		r = -ENOMEM;
-		goto err_alloc_ndev;
+		return -ENOMEM;
 	}
 
 	r = nci_register_device(ndev);
@@ -793,9 +789,6 @@ int fdp_nci_probe(struct fdp_i2c_phy *phy, struct nfc_phy_ops *phy_ops,
 
 err_regdev:
 	nci_free_device(ndev);
-err_alloc_ndev:
-	kfree(info);
-err_info_alloc:
 	return r;
 }
 EXPORT_SYMBOL(fdp_nci_probe);
@@ -809,7 +802,6 @@ void fdp_nci_remove(struct nci_dev *ndev)
 
 	nci_unregister_device(ndev);
 	nci_free_device(ndev);
-	kfree(info);
 }
 EXPORT_SYMBOL(fdp_nci_remove);
 
diff --git a/drivers/nfc/fdp/i2c.c b/drivers/nfc/fdp/i2c.c
index 712936f5d2d6..c4da50e07bbc 100644
--- a/drivers/nfc/fdp/i2c.c
+++ b/drivers/nfc/fdp/i2c.c
@@ -27,7 +27,6 @@
 
 #define FDP_I2C_DRIVER_NAME	"fdp_nci_i2c"
 
-#define FDP_DP_POWER_GPIO_NAME	"power"
 #define FDP_DP_CLOCK_TYPE_NAME	"clock-type"
 #define FDP_DP_CLOCK_FREQ_NAME	"clock-freq"
 #define FDP_DP_FW_VSC_CFG_NAME	"fw-vsc-cfg"
@@ -79,14 +78,14 @@ static void fdp_nci_i2c_add_len_lrc(struct sk_buff *skb)
 
 	/* Add length header */
 	len = skb->len;
-	*skb_push(skb, 1) = len & 0xff;
-	*skb_push(skb, 1) = len >> 8;
+	*(u8 *)skb_push(skb, 1) = len & 0xff;
+	*(u8 *)skb_push(skb, 1) = len >> 8;
 
 	/* Compute and add lrc */
 	for (i = 0; i < len + 2; i++)
 		lrc ^= skb->data[i];
 
-	*skb_put(skb, 1) = lrc;
+	skb_put_u8(skb, lrc);
 }
 
 static void fdp_nci_i2c_remove_len_lrc(struct sk_buff *skb)
@@ -186,7 +185,7 @@ static int fdp_nci_i2c_read(struct fdp_i2c_phy *phy, struct sk_buff **skb)
 				goto flush;
 			}
 
-			memcpy(skb_put(*skb, len), tmp, len);
+			skb_put_data(*skb, tmp, len);
 			fdp_nci_i2c_dump_skb(&client->dev, "fdp_rd", *skb);
 
 			fdp_nci_i2c_remove_len_lrc(*skb);
@@ -281,8 +280,14 @@ vsc_read_err:
 		*clock_type, *clock_freq, *fw_vsc_cfg != NULL ? "yes" : "no");
 }
 
-static int fdp_nci_i2c_probe(struct i2c_client *client,
-			     const struct i2c_device_id *id)
+static const struct acpi_gpio_params power_gpios = { 0, 0, false };
+
+static const struct acpi_gpio_mapping acpi_fdp_gpios[] = {
+	{ "power-gpios", &power_gpios, 1 },
+	{},
+};
+
+static int fdp_nci_i2c_probe(struct i2c_client *client)
 {
 	struct fdp_i2c_phy *phy;
 	struct device *dev = &client->dev;
@@ -304,8 +309,7 @@ static int fdp_nci_i2c_probe(struct i2c_client *client,
 		return -ENODEV;
 	}
 
-	phy = devm_kzalloc(dev, sizeof(struct fdp_i2c_phy),
-			   GFP_KERNEL);
+	phy = devm_kzalloc(dev, sizeof(struct fdp_i2c_phy), GFP_KERNEL);
 	if (!phy)
 		return -ENOMEM;
 
@@ -313,19 +317,22 @@ static int fdp_nci_i2c_probe(struct i2c_client *client,
 	phy->next_read_size = FDP_NCI_I2C_MIN_PAYLOAD;
 	i2c_set_clientdata(client, phy);
 
-	r = request_threaded_irq(client->irq, NULL, fdp_nci_i2c_irq_thread_fn,
-				 IRQF_TRIGGER_RISING | IRQF_ONESHOT,
-				 FDP_I2C_DRIVER_NAME, phy);
+	r = devm_request_threaded_irq(dev, client->irq,
+				      NULL, fdp_nci_i2c_irq_thread_fn,
+				      IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+				      FDP_I2C_DRIVER_NAME, phy);
 
 	if (r < 0) {
 		nfc_err(&client->dev, "Unable to register IRQ handler\n");
 		return r;
 	}
 
-	/* Requesting the power gpio */
-	phy->power_gpio = devm_gpiod_get(dev, FDP_DP_POWER_GPIO_NAME,
-					 GPIOD_OUT_LOW);
+	r = devm_acpi_dev_add_driver_gpios(dev, acpi_fdp_gpios);
+	if (r)
+		dev_dbg(dev, "Unable to add GPIO mapping table\n");
 
+	/* Requesting the power gpio */
+	phy->power_gpio = devm_gpiod_get(dev, "power", GPIOD_OUT_LOW);
 	if (IS_ERR(phy->power_gpio)) {
 		nfc_err(dev, "Power GPIO request failed\n");
 		return PTR_ERR(phy->power_gpio);
@@ -360,12 +367,6 @@ static int fdp_nci_i2c_remove(struct i2c_client *client)
 	return 0;
 }
 
-static struct i2c_device_id fdp_nci_i2c_id_table[] = {
-	{"int339a", 0},
-	{}
-};
-MODULE_DEVICE_TABLE(i2c, fdp_nci_i2c_id_table);
-
 static const struct acpi_device_id fdp_nci_i2c_acpi_match[] = {
 	{"INT339A", 0},
 	{}
@@ -377,8 +378,7 @@ static struct i2c_driver fdp_nci_i2c_driver = {
 		   .name = FDP_I2C_DRIVER_NAME,
 		   .acpi_match_table = ACPI_PTR(fdp_nci_i2c_acpi_match),
 		  },
-	.id_table = fdp_nci_i2c_id_table,
-	.probe = fdp_nci_i2c_probe,
+	.probe_new = fdp_nci_i2c_probe,
 	.remove = fdp_nci_i2c_remove,
 };
 module_i2c_driver(fdp_nci_i2c_driver);
diff --git a/drivers/nfc/microread/i2c.c b/drivers/nfc/microread/i2c.c
index e0e8afd27849..b668b7b9a61e 100644
--- a/drivers/nfc/microread/i2c.c
+++ b/drivers/nfc/microread/i2c.c
@@ -70,12 +70,12 @@ static void microread_i2c_add_len_crc(struct sk_buff *skb)
 	int len;
 
 	len = skb->len;
-	*skb_push(skb, 1) = len;
+	*(u8 *)skb_push(skb, 1) = len;
 
 	for (i = 0; i < skb->len; i++)
 		crc = crc ^ skb->data[i];
 
-	*skb_put(skb, 1) = crc;
+	skb_put_u8(skb, crc);
 }
 
 static void microread_i2c_remove_len_crc(struct sk_buff *skb)
@@ -173,7 +173,7 @@ static int microread_i2c_read(struct microread_i2c_phy *phy,
 		goto flush;
 	}
 
-	*skb_put(*skb, 1) = len;
+	skb_put_u8(*skb, len);
 
 	r = i2c_master_recv(client, skb_put(*skb, len), len);
 	if (r != len) {
diff --git a/drivers/nfc/microread/microread.c b/drivers/nfc/microread/microread.c
index f454dc68cc03..e5d5d2d97409 100644
--- a/drivers/nfc/microread/microread.c
+++ b/drivers/nfc/microread/microread.c
@@ -419,7 +419,7 @@ static int microread_im_transceive(struct nfc_hci_dev *hdev,
 	pr_info("data exchange to gate 0x%x\n", target->hci_reader_gate);
 
 	if (target->hci_reader_gate == MICROREAD_GATE_ID_P2P_INITIATOR) {
-		*skb_push(skb, 1) = 0;
+		*(u8 *)skb_push(skb, 1) = 0;
 
 		return nfc_hci_send_event(hdev, target->hci_reader_gate,
 				     MICROREAD_EVT_P2P_INITIATOR_EXCHANGE_TO_RF,
@@ -441,8 +441,8 @@ static int microread_im_transceive(struct nfc_hci_dev *hdev,
 
 		crc = crc_ccitt(0xffff, skb->data, skb->len);
 		crc = ~crc;
-		*skb_put(skb, 1) = crc & 0xff;
-		*skb_put(skb, 1) = crc >> 8;
+		skb_put_u8(skb, crc & 0xff);
+		skb_put_u8(skb, crc >> 8);
 		break;
 	case MICROREAD_GATE_ID_MREAD_NFC_T3:
 		control_bits = 0xDB;
@@ -453,7 +453,7 @@ static int microread_im_transceive(struct nfc_hci_dev *hdev,
 		return 1;
 	}
 
-	*skb_push(skb, 1) = control_bits;
+	*(u8 *)skb_push(skb, 1) = control_bits;
 
 	info->async_cb_type = MICROREAD_CB_TYPE_READER_ALL;
 	info->async_cb = cb;
diff --git a/drivers/nfc/nfcmrvl/fw_dnld.c b/drivers/nfc/nfcmrvl/fw_dnld.c
index c38bdd6a5a82..7f8960a46aab 100644
--- a/drivers/nfc/nfcmrvl/fw_dnld.c
+++ b/drivers/nfc/nfcmrvl/fw_dnld.c
@@ -92,7 +92,7 @@ static struct sk_buff *alloc_lc_skb(struct nfcmrvl_private *priv, uint8_t plen)
 		return NULL;
 	}
 
-	hdr = (struct nci_data_hdr *) skb_put(skb, NCI_DATA_HDR_SIZE);
+	hdr = skb_put(skb, NCI_DATA_HDR_SIZE);
 	hdr->conn_id = NCI_CORE_LC_CONNID_PROP_FW_DL;
 	hdr->rfu = 0;
 	hdr->plen = plen;
@@ -292,7 +292,7 @@ static int process_state_fw_dnld(struct nfcmrvl_private *priv,
 			out_skb = alloc_lc_skb(priv, 1);
 			if (!out_skb)
 				return -ENOMEM;
-			*skb_put(out_skb, 1) = 0xBF;
+			skb_put_u8(out_skb, 0xBF);
 			nci_send_frame(priv->ndev, out_skb);
 			priv->fw_dnld.substate = SUBSTATE_WAIT_NACK_CREDIT;
 			return 0;
@@ -301,7 +301,7 @@ static int process_state_fw_dnld(struct nfcmrvl_private *priv,
 		out_skb = alloc_lc_skb(priv, 1);
 		if (!out_skb)
 			return -ENOMEM;
-		*skb_put(out_skb, 1) = HELPER_ACK_PACKET_FORMAT;
+		skb_put_u8(out_skb, HELPER_ACK_PACKET_FORMAT);
 		nci_send_frame(priv->ndev, out_skb);
 		priv->fw_dnld.substate = SUBSTATE_WAIT_ACK_CREDIT;
 		break;
@@ -324,10 +324,9 @@ static int process_state_fw_dnld(struct nfcmrvl_private *priv,
 			out_skb = alloc_lc_skb(priv, priv->fw_dnld.chunk_len);
 			if (!out_skb)
 				return -ENOMEM;
-			memcpy(skb_put(out_skb, priv->fw_dnld.chunk_len),
-			       ((uint8_t *)priv->fw_dnld.fw->data) +
-			       priv->fw_dnld.offset,
-			       priv->fw_dnld.chunk_len);
+			skb_put_data(out_skb,
+				     ((uint8_t *)priv->fw_dnld.fw->data) + priv->fw_dnld.offset,
+				     priv->fw_dnld.chunk_len);
 			nci_send_frame(priv->ndev, out_skb);
 			priv->fw_dnld.substate = SUBSTATE_WAIT_DATA_CREDIT;
 		}
@@ -458,7 +457,7 @@ int	nfcmrvl_fw_dnld_init(struct nfcmrvl_private *priv)
 
 	INIT_WORK(&priv->fw_dnld.rx_work, fw_dnld_rx_work);
 	snprintf(name, sizeof(name), "%s_nfcmrvl_fw_dnld_rx_wq",
-		 dev_name(priv->dev));
+		 dev_name(&priv->ndev->nfc_dev->dev));
 	priv->fw_dnld.rx_wq = create_singlethread_workqueue(name);
 	if (!priv->fw_dnld.rx_wq)
 		return -ENOMEM;
@@ -495,6 +494,7 @@ int nfcmrvl_fw_dnld_start(struct nci_dev *ndev, const char *firmware_name)
 {
 	struct nfcmrvl_private *priv = nci_get_drvdata(ndev);
 	struct nfcmrvl_fw_dnld *fw_dnld = &priv->fw_dnld;
+	int res;
 
 	if (!priv->support_fw_dnld)
 		return -ENOTSUPP;
@@ -510,7 +510,9 @@ int nfcmrvl_fw_dnld_start(struct nci_dev *ndev, const char *firmware_name)
 	 */
 
 	/* Retrieve FW binary */
-	if (request_firmware(&fw_dnld->fw, firmware_name, priv->dev) < 0) {
+	res = request_firmware(&fw_dnld->fw, firmware_name,
+			       &ndev->nfc_dev->dev);
+	if (res < 0) {
 		nfc_err(priv->dev, "failed to retrieve FW %s", firmware_name);
 		return -ENOENT;
 	}
diff --git a/drivers/nfc/nfcmrvl/i2c.c b/drivers/nfc/nfcmrvl/i2c.c
index 78b7aa835c81..ffec103702f1 100644
--- a/drivers/nfc/nfcmrvl/i2c.c
+++ b/drivers/nfc/nfcmrvl/i2c.c
@@ -60,7 +60,7 @@ static int nfcmrvl_i2c_read(struct nfcmrvl_i2c_drv_data *drv_data,
 		return -ENOMEM;
 
 	/* Copy NCI header into the SKB */
-	memcpy(skb_put(*skb, NCI_CTRL_HDR_SIZE), &nci_hdr, NCI_CTRL_HDR_SIZE);
+	skb_put_data(*skb, &nci_hdr, NCI_CTRL_HDR_SIZE);
 
 	if (nci_hdr.plen) {
 		/* Read the NCI payload */
diff --git a/drivers/nfc/nfcmrvl/main.c b/drivers/nfc/nfcmrvl/main.c
index 51c8240a1672..e65d027b91fa 100644
--- a/drivers/nfc/nfcmrvl/main.c
+++ b/drivers/nfc/nfcmrvl/main.c
@@ -68,7 +68,7 @@ static int nfcmrvl_nci_send(struct nci_dev *ndev, struct sk_buff *skb)
 		unsigned char *hdr;
 		unsigned char len = skb->len;
 
-		hdr = (char *) skb_push(skb, NFCMRVL_HCI_EVENT_HEADER_SIZE);
+		hdr = skb_push(skb, NFCMRVL_HCI_EVENT_HEADER_SIZE);
 		hdr[0] = NFCMRVL_HCI_COMMAND_CODE;
 		hdr[1] = NFCMRVL_HCI_OGF;
 		hdr[2] = NFCMRVL_HCI_OCF;
@@ -123,13 +123,14 @@ struct nfcmrvl_private *nfcmrvl_nci_register_dev(enum nfcmrvl_phy phy,
 
 	memcpy(&priv->config, pdata, sizeof(*pdata));
 
-	if (priv->config.reset_n_io) {
-		rc = devm_gpio_request_one(dev,
-					   priv->config.reset_n_io,
-					   GPIOF_OUT_INIT_LOW,
-					   "nfcmrvl_reset_n");
-		if (rc < 0)
+	if (gpio_is_valid(priv->config.reset_n_io)) {
+		rc = gpio_request_one(priv->config.reset_n_io,
+				      GPIOF_OUT_INIT_LOW,
+				      "nfcmrvl_reset_n");
+		if (rc < 0) {
+			priv->config.reset_n_io = -EINVAL;
 			nfc_err(dev, "failed to request reset_n io\n");
+		}
 	}
 
 	if (phy == NFCMRVL_PHY_SPI) {
@@ -154,7 +155,13 @@ struct nfcmrvl_private *nfcmrvl_nci_register_dev(enum nfcmrvl_phy phy,
 	if (!priv->ndev) {
 		nfc_err(dev, "nci_allocate_device failed\n");
 		rc = -ENOMEM;
-		goto error;
+		goto error_free_gpio;
+	}
+
+	rc = nfcmrvl_fw_dnld_init(priv);
+	if (rc) {
+		nfc_err(dev, "failed to initialize FW download %d\n", rc);
+		goto error_free_dev;
 	}
 
 	nci_set_drvdata(priv->ndev, priv);
@@ -162,24 +169,22 @@ struct nfcmrvl_private *nfcmrvl_nci_register_dev(enum nfcmrvl_phy phy,
 	rc = nci_register_device(priv->ndev);
 	if (rc) {
 		nfc_err(dev, "nci_register_device failed %d\n", rc);
-		goto error_free_dev;
+		goto error_fw_dnld_deinit;
 	}
 
 	/* Ensure that controller is powered off */
 	nfcmrvl_chip_halt(priv);
 
-	rc = nfcmrvl_fw_dnld_init(priv);
-	if (rc) {
-		nfc_err(dev, "failed to initialize FW download %d\n", rc);
-		goto error_free_dev;
-	}
-
 	nfc_info(dev, "registered with nci successfully\n");
 	return priv;
 
+error_fw_dnld_deinit:
+	nfcmrvl_fw_dnld_deinit(priv);
 error_free_dev:
 	nci_free_device(priv->ndev);
-error:
+error_free_gpio:
+	if (gpio_is_valid(priv->config.reset_n_io))
+		gpio_free(priv->config.reset_n_io);
 	kfree(priv);
 	return ERR_PTR(rc);
 }
@@ -194,8 +199,8 @@ void nfcmrvl_nci_unregister_dev(struct nfcmrvl_private *priv)
 
 	nfcmrvl_fw_dnld_deinit(priv);
 
-	if (priv->config.reset_n_io)
-		devm_gpio_free(priv->dev, priv->config.reset_n_io);
+	if (gpio_is_valid(priv->config.reset_n_io))
+		gpio_free(priv->config.reset_n_io);
 
 	nci_unregister_device(ndev);
 	nci_free_device(ndev);
@@ -262,7 +267,6 @@ int nfcmrvl_parse_dt(struct device_node *node,
 	reset_n_io = of_get_named_gpio(node, "reset-n-io", 0);
 	if (reset_n_io < 0) {
 		pr_info("no reset-n-io config\n");
-		reset_n_io = 0;
 	} else if (!gpio_is_valid(reset_n_io)) {
 		pr_err("invalid reset-n-io GPIO\n");
 		return reset_n_io;
diff --git a/drivers/nfc/nfcmrvl/uart.c b/drivers/nfc/nfcmrvl/uart.c
index 83a99e38e7bd..91162f8e0366 100644
--- a/drivers/nfc/nfcmrvl/uart.c
+++ b/drivers/nfc/nfcmrvl/uart.c
@@ -84,6 +84,7 @@ static int nfcmrvl_uart_parse_dt(struct device_node *node,
 	ret = nfcmrvl_parse_dt(matched_node, pdata);
 	if (ret < 0) {
 		pr_err("Failed to get generic entries\n");
+		of_node_put(matched_node);
 		return ret;
 	}
 
@@ -97,6 +98,8 @@ static int nfcmrvl_uart_parse_dt(struct device_node *node,
 	else
 		pdata->break_control = 0;
 
+	of_node_put(matched_node);
+
 	return 0;
 }
 
@@ -109,6 +112,7 @@ static int nfcmrvl_nci_uart_open(struct nci_uart *nu)
 	struct nfcmrvl_private *priv;
 	struct nfcmrvl_platform_data *pdata = NULL;
 	struct nfcmrvl_platform_data config;
+	struct device *dev = nu->tty->dev;
 
 	/*
 	 * Platform data cannot be used here since usually it is already used
@@ -116,9 +120,8 @@ static int nfcmrvl_nci_uart_open(struct nci_uart *nu)
 	 * and check if DT entries were added.
 	 */
 
-	if (nu->tty->dev->parent && nu->tty->dev->parent->of_node)
-		if (nfcmrvl_uart_parse_dt(nu->tty->dev->parent->of_node,
-					  &config) == 0)
+	if (dev && dev->parent && dev->parent->of_node)
+		if (nfcmrvl_uart_parse_dt(dev->parent->of_node, &config) == 0)
 			pdata = &config;
 
 	if (!pdata) {
@@ -131,7 +134,7 @@ static int nfcmrvl_nci_uart_open(struct nci_uart *nu)
 	}
 
 	priv = nfcmrvl_nci_register_dev(NFCMRVL_PHY_UART, nu, &uart_ops,
-					nu->tty->dev, pdata);
+					dev, pdata);
 	if (IS_ERR(priv))
 		return PTR_ERR(priv);
 
diff --git a/drivers/nfc/nfcmrvl/usb.c b/drivers/nfc/nfcmrvl/usb.c
index 585a0f20835b..bd35eab652be 100644
--- a/drivers/nfc/nfcmrvl/usb.c
+++ b/drivers/nfc/nfcmrvl/usb.c
@@ -83,8 +83,8 @@ static void nfcmrvl_bulk_complete(struct urb *urb)
 		if (!skb) {
 			nfc_err(&drv_data->udev->dev, "failed to alloc mem\n");
 		} else {
-			memcpy(skb_put(skb, urb->actual_length),
-			       urb->transfer_buffer, urb->actual_length);
+			skb_put_data(skb, urb->transfer_buffer,
+				     urb->actual_length);
 			if (nfcmrvl_nci_recv_frame(drv_data->priv, skb) < 0)
 				nfc_err(&drv_data->udev->dev,
 					"corrupted Rx packet\n");
@@ -341,15 +341,13 @@ static int nfcmrvl_probe(struct usb_interface *intf,
 	init_usb_anchor(&drv_data->deferred);
 
 	priv = nfcmrvl_nci_register_dev(NFCMRVL_PHY_USB, drv_data, &usb_ops,
-					&drv_data->udev->dev, &config);
+					&intf->dev, &config);
 	if (IS_ERR(priv))
 		return PTR_ERR(priv);
 
 	drv_data->priv = priv;
 	drv_data->priv->support_fw_dnld = false;
 
-	priv->dev = &drv_data->udev->dev;
-
 	usb_set_intfdata(intf, drv_data);
 
 	return 0;
diff --git a/drivers/nfc/nfcsim.c b/drivers/nfc/nfcsim.c
index a466e7978466..33449820e754 100644
--- a/drivers/nfc/nfcsim.c
+++ b/drivers/nfc/nfcsim.c
@@ -482,8 +482,10 @@ static int __init nfcsim_init(void)
 exit_err:
 	pr_err("Failed to initialize nfcsim driver (%d)\n", rc);
 
-	nfcsim_link_free(link0);
-	nfcsim_link_free(link1);
+	if (link0)
+		nfcsim_link_free(link0);
+	if (link1)
+		nfcsim_link_free(link1);
 
 	return rc;
 }
diff --git a/drivers/nfc/nxp-nci/firmware.c b/drivers/nfc/nxp-nci/firmware.c
index 553011f58339..e50c6f67bb39 100644
--- a/drivers/nfc/nxp-nci/firmware.c
+++ b/drivers/nfc/nxp-nci/firmware.c
@@ -124,8 +124,7 @@ static int nxp_nci_fw_send_chunk(struct nxp_nci_info *info)
 	header |= chunk_len & NXP_NCI_FW_FRAME_LEN_MASK;
 	put_unaligned_be16(header, skb_put(skb, NXP_NCI_FW_HDR_LEN));
 
-	memcpy(skb_put(skb, chunk_len), fw_info->data + fw_info->written,
-	       chunk_len);
+	skb_put_data(skb, fw_info->data + fw_info->written, chunk_len);
 
 	crc = nxp_nci_fw_crc(skb->data, chunk_len + NXP_NCI_FW_HDR_LEN);
 	put_unaligned_be16(crc, skb_put(skb, NXP_NCI_FW_CRC_LEN));
@@ -312,8 +311,7 @@ void nxp_nci_fw_recv_frame(struct nci_dev *ndev, struct sk_buff *skb)
 		if (nxp_nci_fw_check_crc(skb) != 0x00)
 			fw_info->cmd_result = -EBADMSG;
 		else
-			fw_info->cmd_result = nxp_nci_fw_read_status(
-					*skb_pull(skb, NXP_NCI_FW_HDR_LEN));
+			fw_info->cmd_result = nxp_nci_fw_read_status(*(u8 *)skb_pull(skb, NXP_NCI_FW_HDR_LEN));
 		kfree_skb(skb);
 	} else {
 		fw_info->cmd_result = -EIO;
diff --git a/drivers/nfc/nxp-nci/i2c.c b/drivers/nfc/nxp-nci/i2c.c
index ff22d761183c..198585bbc771 100644
--- a/drivers/nfc/nxp-nci/i2c.c
+++ b/drivers/nfc/nxp-nci/i2c.c
@@ -135,7 +135,7 @@ static int nxp_nci_i2c_fw_read(struct nxp_nci_i2c_phy *phy,
 		goto fw_read_exit;
 	}
 
-	memcpy(skb_put(*skb, NXP_NCI_FW_HDR_LEN), &header, NXP_NCI_FW_HDR_LEN);
+	skb_put_data(*skb, &header, NXP_NCI_FW_HDR_LEN);
 
 	r = i2c_master_recv(client, skb_put(*skb, frame_len), frame_len);
 	if (r != frame_len) {
@@ -176,8 +176,7 @@ static int nxp_nci_i2c_nci_read(struct nxp_nci_i2c_phy *phy,
 		goto nci_read_exit;
 	}
 
-	memcpy(skb_put(*skb, NCI_CTRL_HDR_SIZE), (void *) &header,
-	       NCI_CTRL_HDR_SIZE);
+	skb_put_data(*skb, (void *)&header, NCI_CTRL_HDR_SIZE);
 
 	r = i2c_master_recv(client, skb_put(*skb, header.plen), header.plen);
 	if (r != header.plen) {
diff --git a/drivers/nfc/pn533/pn533.c b/drivers/nfc/pn533/pn533.c
index 65bbaa5fcdda..c05cb637ba92 100644
--- a/drivers/nfc/pn533/pn533.c
+++ b/drivers/nfc/pn533/pn533.c
@@ -1006,7 +1006,7 @@ static int pn533_start_poll_complete(struct pn533 *dev, struct sk_buff *resp)
 static struct sk_buff *pn533_alloc_poll_tg_frame(struct pn533 *dev)
 {
 	struct sk_buff *skb;
-	u8 *felica, *nfcid3, *gb;
+	u8 *felica, *nfcid3;
 
 	u8 *gbytes = dev->gb;
 	size_t gbytes_len = dev->gb_len;
@@ -1032,29 +1032,26 @@ static struct sk_buff *pn533_alloc_poll_tg_frame(struct pn533 *dev)
 		return NULL;
 
 	/* DEP support only */
-	*skb_put(skb, 1) = PN533_INIT_TARGET_DEP;
+	skb_put_u8(skb, PN533_INIT_TARGET_DEP);
 
 	/* MIFARE params */
-	memcpy(skb_put(skb, 6), mifare_params, 6);
+	skb_put_data(skb, mifare_params, 6);
 
 	/* Felica params */
-	felica = skb_put(skb, 18);
-	memcpy(felica, felica_params, 18);
+	felica = skb_put_data(skb, felica_params, 18);
 	get_random_bytes(felica + 2, 6);
 
 	/* NFCID3 */
-	nfcid3 = skb_put(skb, 10);
-	memset(nfcid3, 0, 10);
+	nfcid3 = skb_put_zero(skb, 10);
 	memcpy(nfcid3, felica, 8);
 
 	/* General bytes */
-	*skb_put(skb, 1) = gbytes_len;
+	skb_put_u8(skb, gbytes_len);
 
-	gb = skb_put(skb, gbytes_len);
-	memcpy(gb, gbytes, gbytes_len);
+	skb_put_data(skb, gbytes, gbytes_len);
 
 	/* Len Tk */
-	*skb_put(skb, 1) = 0;
+	skb_put_u8(skb, 0);
 
 	return skb;
 }
@@ -1283,8 +1280,8 @@ static void pn533_wq_rf(struct work_struct *work)
 	if (!skb)
 		return;
 
-	*skb_put(skb, 1) = PN533_CFGITEM_RF_FIELD;
-	*skb_put(skb, 1) = PN533_CFGITEM_RF_FIELD_AUTO_RFCA;
+	skb_put_u8(skb, PN533_CFGITEM_RF_FIELD);
+	skb_put_u8(skb, PN533_CFGITEM_RF_FIELD_AUTO_RFCA);
 
 	rc = pn533_send_cmd_async(dev, PN533_CMD_RF_CONFIGURATION, skb,
 				  pn533_rf_complete, NULL);
@@ -1378,22 +1375,21 @@ static int pn533_poll_dep(struct nfc_dev *nfc_dev)
 	if (!skb)
 		return -ENOMEM;
 
-	*skb_put(skb, 1) = 0x01;  /* Active */
-	*skb_put(skb, 1) = 0x02;  /* 424 kbps */
+	skb_put_u8(skb, 0x01);  /* Active */
+	skb_put_u8(skb, 0x02);  /* 424 kbps */
 
 	next = skb_put(skb, 1);  /* Next */
 	*next = 0;
 
 	/* Copy passive data */
-	memcpy(skb_put(skb, PASSIVE_DATA_LEN), passive_data, PASSIVE_DATA_LEN);
+	skb_put_data(skb, passive_data, PASSIVE_DATA_LEN);
 	*next |= 1;
 
 	/* Copy NFCID3 (which is NFCID2 from SENSF_RES) */
-	memcpy(skb_put(skb, NFC_NFCID3_MAXSIZE), nfcid3,
-	       NFC_NFCID3_MAXSIZE);
+	skb_put_data(skb, nfcid3, NFC_NFCID3_MAXSIZE);
 	*next |= 2;
 
-	memcpy(skb_put(skb, dev->gb_len), dev->gb, dev->gb_len);
+	skb_put_data(skb, dev->gb, dev->gb_len);
 	*next |= 4; /* We have some Gi */
 
 	rc = pn533_send_cmd_async(dev, PN533_CMD_IN_JUMP_FOR_DEP, skb,
@@ -1473,7 +1469,7 @@ static struct sk_buff *pn533_alloc_poll_in_frame(struct pn533 *dev,
 	if (!skb)
 		return NULL;
 
-	memcpy(skb_put(skb, mod->len), &mod->data, mod->len);
+	skb_put_data(skb, &mod->data, mod->len);
 
 	return skb;
 }
@@ -1624,8 +1620,8 @@ static int pn533_activate_target_nfcdep(struct pn533 *dev)
 	if (!skb)
 		return -ENOMEM;
 
-	*skb_put(skb, sizeof(u8)) = 1; /* TG */
-	*skb_put(skb, sizeof(u8)) = 0; /* Next */
+	skb_put_u8(skb, 1); /* TG */
+	skb_put_u8(skb, 0); /* Next */
 
 	resp = pn533_send_cmd_sync(dev, PN533_CMD_IN_ATR, skb);
 	if (IS_ERR(resp))
@@ -1741,7 +1737,7 @@ static void pn533_deactivate_target(struct nfc_dev *nfc_dev,
 	if (!skb)
 		return;
 
-	*skb_put(skb, 1) = 1; /* TG*/
+	skb_put_u8(skb, 1); /* TG*/
 
 	rc = pn533_send_cmd_async(dev, PN533_CMD_IN_RELEASE, skb,
 				  pn533_deactivate_target_complete, NULL);
@@ -1852,14 +1848,14 @@ static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
 	if (!skb)
 		return -ENOMEM;
 
-	*skb_put(skb, 1) = !comm_mode;  /* ActPass */
-	*skb_put(skb, 1) = 0x02;  /* 424 kbps */
+	skb_put_u8(skb, !comm_mode);  /* ActPass */
+	skb_put_u8(skb, 0x02);  /* 424 kbps */
 
 	next = skb_put(skb, 1);  /* Next */
 	*next = 0;
 
 	/* Copy passive data */
-	memcpy(skb_put(skb, PASSIVE_DATA_LEN), passive_data, PASSIVE_DATA_LEN);
+	skb_put_data(skb, passive_data, PASSIVE_DATA_LEN);
 	*next |= 1;
 
 	/* Copy NFCID3 (which is NFCID2 from SENSF_RES) */
@@ -1867,12 +1863,11 @@ static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
 		memcpy(skb_put(skb, NFC_NFCID3_MAXSIZE), target->nfcid2,
 		       target->nfcid2_len);
 	else
-		memcpy(skb_put(skb, NFC_NFCID3_MAXSIZE), nfcid3,
-		       NFC_NFCID3_MAXSIZE);
+		skb_put_data(skb, nfcid3, NFC_NFCID3_MAXSIZE);
 	*next |= 2;
 
 	if (gb != NULL && gb_len > 0) {
-		memcpy(skb_put(skb, gb_len), gb, gb_len);
+		skb_put_data(skb, gb, gb_len);
 		*next |= 4; /* We have some Gi */
 	} else {
 		*next = 0;
@@ -2095,13 +2090,13 @@ static int pn533_fill_fragment_skbs(struct pn533 *dev, struct sk_buff *skb)
 
 			/* MI + TG */
 			if (frag_size  == PN533_CMD_DATAFRAME_MAXLEN)
-				*skb_push(frag, sizeof(u8)) =
-							(PN533_CMD_MI_MASK | 1);
+				*(u8 *)skb_push(frag, sizeof(u8)) =
+						(PN533_CMD_MI_MASK | 1);
 			else
-				*skb_push(frag, sizeof(u8)) =  1; /* TG */
+				*(u8 *)skb_push(frag, sizeof(u8)) =  1; /* TG */
 		}
 
-		memcpy(skb_put(frag, frag_size), skb->data, frag_size);
+		skb_put_data(frag, skb->data, frag_size);
 
 		/* Reduce the size of incoming buffer */
 		skb_pull(skb, frag_size);
@@ -2165,7 +2160,7 @@ static int pn533_transceive(struct nfc_dev *nfc_dev,
 				goto error;
 			}
 		} else {
-			*skb_push(skb, sizeof(u8)) =  1; /* TG */
+			*(u8 *)skb_push(skb, sizeof(u8)) =  1; /* TG */
 		}
 
 		rc = pn533_send_data_async(dev, PN533_CMD_IN_DATA_EXCHANGE,
@@ -2279,7 +2274,7 @@ static void pn533_wq_mi_recv(struct work_struct *work)
 			break;
 		}
 	default:
-		*skb_put(skb, sizeof(u8)) =  1; /*TG*/
+		skb_put_u8(skb, 1); /*TG*/
 
 		rc = pn533_send_cmd_direct_async(dev,
 						 PN533_CMD_IN_DATA_EXCHANGE,
@@ -2375,8 +2370,8 @@ static int pn533_set_configuration(struct pn533 *dev, u8 cfgitem, u8 *cfgdata,
 	if (!skb)
 		return -ENOMEM;
 
-	*skb_put(skb, sizeof(cfgitem)) = cfgitem;
-	memcpy(skb_put(skb, cfgdata_len), cfgdata, cfgdata_len);
+	skb_put_u8(skb, cfgitem);
+	skb_put_data(skb, cfgdata, cfgdata_len);
 
 	resp = pn533_send_cmd_sync(dev, PN533_CMD_RF_CONFIGURATION, skb);
 	if (IS_ERR(resp))
@@ -2420,7 +2415,7 @@ static int pn533_pasori_fw_reset(struct pn533 *dev)
 	if (!skb)
 		return -ENOMEM;
 
-	*skb_put(skb, sizeof(u8)) = 0x1;
+	skb_put_u8(skb, 0x1);
 
 	resp = pn533_send_cmd_sync(dev, 0x18, skb);
 	if (IS_ERR(resp))
@@ -2459,7 +2454,7 @@ static int pn532_sam_configuration(struct nfc_dev *nfc_dev)
 	if (!skb)
 		return -ENOMEM;
 
-	*skb_put(skb, 1) = 0x01;
+	skb_put_u8(skb, 0x01);
 
 	resp = pn533_send_cmd_sync(dev, PN533_CMD_SAM_CONFIGURATION, skb);
 	if (IS_ERR(resp))
diff --git a/drivers/nfc/pn533/usb.c b/drivers/nfc/pn533/usb.c
index 8ed203ea21ea..e153e8b64bb8 100644
--- a/drivers/nfc/pn533/usb.c
+++ b/drivers/nfc/pn533/usb.c
@@ -75,8 +75,8 @@ static void pn533_recv_response(struct urb *urb)
 		if (!skb) {
 			nfc_err(&phy->udev->dev, "failed to alloc memory\n");
 		} else {
-			memcpy(skb_put(skb, urb->actual_length),
-			       urb->transfer_buffer, urb->actual_length);
+			skb_put_data(skb, urb->transfer_buffer,
+				     urb->actual_length);
 		}
 	}
 
diff --git a/drivers/nfc/pn544/i2c.c b/drivers/nfc/pn544/i2c.c
index 71ac0836c9f4..4b14740edb67 100644
--- a/drivers/nfc/pn544/i2c.c
+++ b/drivers/nfc/pn544/i2c.c
@@ -283,12 +283,12 @@ static void pn544_hci_i2c_add_len_crc(struct sk_buff *skb)
 	int len;
 
 	len = skb->len + 2;
-	*skb_push(skb, 1) = len;
+	*(u8 *)skb_push(skb, 1) = len;
 
 	crc = crc_ccitt(0xffff, skb->data, skb->len);
 	crc = ~crc;
-	*skb_put(skb, 1) = crc & 0xff;
-	*skb_put(skb, 1) = crc >> 8;
+	skb_put_u8(skb, crc & 0xff);
+	skb_put_u8(skb, crc >> 8);
 }
 
 static void pn544_hci_i2c_remove_len_crc(struct sk_buff *skb)
@@ -391,7 +391,7 @@ static int pn544_hci_i2c_read(struct pn544_i2c_phy *phy, struct sk_buff **skb)
 		goto flush;
 	}
 
-	*skb_put(*skb, 1) = len;
+	skb_put_u8(*skb, len);
 
 	r = i2c_master_recv(client, skb_put(*skb, len), len);
 	if (r != len) {
@@ -904,7 +904,7 @@ static int pn544_hci_i2c_probe(struct i2c_client *client,
 	phy->i2c_dev = client;
 	i2c_set_clientdata(client, phy);
 
-	r = acpi_dev_add_driver_gpios(ACPI_COMPANION(dev), acpi_pn544_gpios);
+	r = devm_acpi_dev_add_driver_gpios(dev, acpi_pn544_gpios);
 	if (r)
 		dev_dbg(dev, "Unable to add GPIO mapping table\n");
 
@@ -958,7 +958,6 @@ static int pn544_hci_i2c_remove(struct i2c_client *client)
 	if (phy->powered)
 		pn544_hci_i2c_disable(phy);
 
-	acpi_dev_remove_driver_gpios(ACPI_COMPANION(&client->dev));
 	return 0;
 }
 
diff --git a/drivers/nfc/pn544/pn544.c b/drivers/nfc/pn544/pn544.c
index 12e819ddf17a..70e898e38b16 100644
--- a/drivers/nfc/pn544/pn544.c
+++ b/drivers/nfc/pn544/pn544.c
@@ -649,8 +649,8 @@ static int pn544_hci_im_transceive(struct nfc_hci_dev *hdev,
 		} else
 			return 1;
 	case PN544_RF_READER_F_GATE:
-		*skb_push(skb, 1) = 0;
-		*skb_push(skb, 1) = 0;
+		*(u8 *)skb_push(skb, 1) = 0;
+		*(u8 *)skb_push(skb, 1) = 0;
 
 		info->async_cb_type = PN544_CB_TYPE_READER_F;
 		info->async_cb = cb;
@@ -665,7 +665,7 @@ static int pn544_hci_im_transceive(struct nfc_hci_dev *hdev,
 					      PN544_JEWEL_RAW_CMD, skb->data,
 					      skb->len, cb, cb_context);
 	case PN544_RF_READER_NFCIP1_INITIATOR_GATE:
-		*skb_push(skb, 1) = 0;
+		*(u8 *)skb_push(skb, 1) = 0;
 
 		return nfc_hci_send_event(hdev, target->hci_reader_gate,
 					PN544_HCI_EVT_SND_DATA, skb->data,
@@ -680,7 +680,7 @@ static int pn544_hci_tm_send(struct nfc_hci_dev *hdev, struct sk_buff *skb)
 	int r;
 
 	/* Set default false for multiple information chaining */
-	*skb_push(skb, 1) = 0;
+	*(u8 *)skb_push(skb, 1) = 0;
 
 	r = nfc_hci_send_event(hdev, PN544_RF_READER_NFCIP1_TARGET_GATE,
 			       PN544_HCI_EVT_SND_DATA, skb->data, skb->len);
diff --git a/drivers/nfc/port100.c b/drivers/nfc/port100.c
index 19be93e177fe..bb43cebda9dc 100644
--- a/drivers/nfc/port100.c
+++ b/drivers/nfc/port100.c
@@ -991,7 +991,7 @@ static int port100_set_command_type(struct port100 *dev, u8 command_type)
 	if (!skb)
 		return -ENOMEM;
 
-	*skb_put(skb, sizeof(u8)) = command_type;
+	skb_put_u8(skb, command_type);
 
 	resp = port100_send_cmd_sync(dev, PORT100_CMD_SET_COMMAND_TYPE, skb);
 	if (IS_ERR(resp))
@@ -1059,7 +1059,7 @@ static int port100_switch_rf(struct nfc_digital_dev *ddev, bool on)
 	if (!skb)
 		return -ENOMEM;
 
-	*skb_put(skb, 1) = on ? 1 : 0;
+	skb_put_u8(skb, on ? 1 : 0);
 
 	/* Cancel the last command if the device is being switched off */
 	if (!on)
@@ -1089,9 +1089,8 @@ static int port100_in_set_rf(struct nfc_digital_dev *ddev, u8 rf)
 	if (!skb)
 		return -ENOMEM;
 
-	memcpy(skb_put(skb, sizeof(struct port100_in_rf_setting)),
-	       &in_rf_settings[rf],
-	       sizeof(struct port100_in_rf_setting));
+	skb_put_data(skb, &in_rf_settings[rf],
+		     sizeof(struct port100_in_rf_setting));
 
 	resp = port100_send_cmd_sync(dev, PORT100_CMD_IN_SET_RF, skb);
 
@@ -1133,7 +1132,7 @@ static int port100_in_set_framing(struct nfc_digital_dev *ddev, int param)
 	if (!skb)
 		return -ENOMEM;
 
-	memcpy(skb_put(skb, size), protocols, size);
+	skb_put_data(skb, protocols, size);
 
 	resp = port100_send_cmd_sync(dev, PORT100_CMD_IN_SET_PROTOCOL, skb);
 
@@ -1247,9 +1246,8 @@ static int port100_tg_set_rf(struct nfc_digital_dev *ddev, u8 rf)
 	if (!skb)
 		return -ENOMEM;
 
-	memcpy(skb_put(skb, sizeof(struct port100_tg_rf_setting)),
-	       &tg_rf_settings[rf],
-	       sizeof(struct port100_tg_rf_setting));
+	skb_put_data(skb, &tg_rf_settings[rf],
+		     sizeof(struct port100_tg_rf_setting));
 
 	resp = port100_send_cmd_sync(dev, PORT100_CMD_TG_SET_RF, skb);
 
@@ -1291,7 +1289,7 @@ static int port100_tg_set_framing(struct nfc_digital_dev *ddev, int param)
 	if (!skb)
 		return -ENOMEM;
 
-	memcpy(skb_put(skb, size), protocols, size);
+	skb_put_data(skb, protocols, size);
 
 	resp = port100_send_cmd_sync(dev, PORT100_CMD_TG_SET_PROTOCOL, skb);
 
diff --git a/drivers/nfc/s3fwrn5/firmware.c b/drivers/nfc/s3fwrn5/firmware.c
index 5f97da1947e3..38548bd970cd 100644
--- a/drivers/nfc/s3fwrn5/firmware.c
+++ b/drivers/nfc/s3fwrn5/firmware.c
@@ -76,9 +76,9 @@ static int s3fwrn5_fw_prep_msg(struct s3fwrn5_fw_info *fw_info,
 	if (!skb)
 		return -ENOMEM;
 
-	memcpy(skb_put(skb, S3FWRN5_FW_HDR_SIZE), &hdr, S3FWRN5_FW_HDR_SIZE);
+	skb_put_data(skb, &hdr, S3FWRN5_FW_HDR_SIZE);
 	if (len)
-		memcpy(skb_put(skb, len), data, len);
+		skb_put_data(skb, data, len);
 
 	*msg = skb;
 
diff --git a/drivers/nfc/s3fwrn5/i2c.c b/drivers/nfc/s3fwrn5/i2c.c
index 3ed0adf6479b..3f09d7fd2285 100644
--- a/drivers/nfc/s3fwrn5/i2c.c
+++ b/drivers/nfc/s3fwrn5/i2c.c
@@ -157,7 +157,7 @@ static int s3fwrn5_i2c_read(struct s3fwrn5_i2c_phy *phy)
 	if (!skb)
 		return -ENOMEM;
 
-	memcpy(skb_put(skb, hdr_size), hdr, hdr_size);
+	skb_put_data(skb, hdr, hdr_size);
 
 	if (data_len == 0)
 		goto out;
diff --git a/drivers/nfc/st-nci/i2c.c b/drivers/nfc/st-nci/i2c.c
index 9dfae0efa922..515f08d037fb 100644
--- a/drivers/nfc/st-nci/i2c.c
+++ b/drivers/nfc/st-nci/i2c.c
@@ -19,15 +19,12 @@
 
 #include <linux/module.h>
 #include <linux/i2c.h>
-#include <linux/gpio.h>
 #include <linux/gpio/consumer.h>
-#include <linux/of_irq.h>
-#include <linux/of_gpio.h>
 #include <linux/acpi.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/nfc.h>
-#include <linux/platform_data/st-nci.h>
+#include <linux/of.h>
 
 #include "st-nci.h"
 
@@ -40,18 +37,16 @@
 #define ST_NCI_I2C_MIN_SIZE 4   /* PCB(1) + NCI Packet header(3) */
 #define ST_NCI_I2C_MAX_SIZE 250 /* req 4.2.1 */
 
+#define ST_NCI_DRIVER_NAME "st_nci"
 #define ST_NCI_I2C_DRIVER_NAME "st_nci_i2c"
 
-#define ST_NCI_GPIO_NAME_RESET "reset"
-
 struct st_nci_i2c_phy {
 	struct i2c_client *i2c_dev;
 	struct llt_ndlc *ndlc;
 
 	bool irq_active;
 
-	unsigned int gpio_reset;
-	unsigned int irq_polarity;
+	struct gpio_desc *gpiod_reset;
 
 	struct st_nci_se_status se_status;
 };
@@ -60,9 +55,9 @@ static int st_nci_i2c_enable(void *phy_id)
 {
 	struct st_nci_i2c_phy *phy = phy_id;
 
-	gpio_set_value(phy->gpio_reset, 0);
+	gpiod_set_value(phy->gpiod_reset, 0);
 	usleep_range(10000, 15000);
-	gpio_set_value(phy->gpio_reset, 1);
+	gpiod_set_value(phy->gpiod_reset, 1);
 	usleep_range(80000, 85000);
 
 	if (phy->ndlc->powered == 0 && phy->irq_active == 0) {
@@ -208,114 +203,18 @@ static struct nfc_phy_ops i2c_phy_ops = {
 	.disable = st_nci_i2c_disable,
 };
 
-static int st_nci_i2c_acpi_request_resources(struct i2c_client *client)
-{
-	struct st_nci_i2c_phy *phy = i2c_get_clientdata(client);
-	struct gpio_desc *gpiod_reset;
-	struct device *dev = &client->dev;
-	u8 tmp;
-
-	/* Get RESET GPIO from ACPI */
-	gpiod_reset = devm_gpiod_get_index(dev, ST_NCI_GPIO_NAME_RESET, 1,
-					   GPIOD_OUT_HIGH);
-	if (IS_ERR(gpiod_reset)) {
-		nfc_err(dev, "Unable to get RESET GPIO\n");
-		return -ENODEV;
-	}
-
-	phy->gpio_reset = desc_to_gpio(gpiod_reset);
-
-	phy->irq_polarity = irq_get_trigger_type(client->irq);
-
-	phy->se_status.is_ese_present = false;
-	phy->se_status.is_uicc_present = false;
-
-	if (device_property_present(dev, "ese-present")) {
-		device_property_read_u8(dev, "ese-present", &tmp);
-		phy->se_status.is_ese_present = tmp;
-	}
-
-	if (device_property_present(dev, "uicc-present")) {
-		device_property_read_u8(dev, "uicc-present", &tmp);
-		phy->se_status.is_uicc_present = tmp;
-	}
-
-	return 0;
-}
-
-static int st_nci_i2c_of_request_resources(struct i2c_client *client)
-{
-	struct st_nci_i2c_phy *phy = i2c_get_clientdata(client);
-	struct device_node *pp;
-	int gpio;
-	int r;
-
-	pp = client->dev.of_node;
-	if (!pp)
-		return -ENODEV;
-
-	/* Get GPIO from device tree */
-	gpio = of_get_named_gpio(pp, "reset-gpios", 0);
-	if (gpio < 0) {
-		nfc_err(&client->dev,
-			"Failed to retrieve reset-gpios from device tree\n");
-		return gpio;
-	}
-
-	/* GPIO request and configuration */
-	r = devm_gpio_request_one(&client->dev, gpio,
-				GPIOF_OUT_INIT_HIGH, ST_NCI_GPIO_NAME_RESET);
-	if (r) {
-		nfc_err(&client->dev, "Failed to request reset pin\n");
-		return r;
-	}
-	phy->gpio_reset = gpio;
-
-	phy->irq_polarity = irq_get_trigger_type(client->irq);
-
-	phy->se_status.is_ese_present =
-				of_property_read_bool(pp, "ese-present");
-	phy->se_status.is_uicc_present =
-				of_property_read_bool(pp, "uicc-present");
-
-	return 0;
-}
-
-static int st_nci_i2c_request_resources(struct i2c_client *client)
-{
-	struct st_nci_nfc_platform_data *pdata;
-	struct st_nci_i2c_phy *phy = i2c_get_clientdata(client);
-	int r;
-
-	pdata = client->dev.platform_data;
-	if (pdata == NULL) {
-		nfc_err(&client->dev, "No platform data\n");
-		return -EINVAL;
-	}
+static const struct acpi_gpio_params reset_gpios = { 1, 0, false };
 
-	/* store for later use */
-	phy->gpio_reset = pdata->gpio_reset;
-	phy->irq_polarity = pdata->irq_polarity;
-
-	r = devm_gpio_request_one(&client->dev,
-			phy->gpio_reset, GPIOF_OUT_INIT_HIGH,
-			ST_NCI_GPIO_NAME_RESET);
-	if (r) {
-		pr_err("%s : reset gpio_request failed\n", __FILE__);
-		return r;
-	}
-
-	phy->se_status.is_ese_present = pdata->is_ese_present;
-	phy->se_status.is_uicc_present = pdata->is_uicc_present;
-
-	return 0;
-}
+static const struct acpi_gpio_mapping acpi_st_nci_gpios[] = {
+	{ "reset-gpios", &reset_gpios, 1 },
+	{},
+};
 
 static int st_nci_i2c_probe(struct i2c_client *client,
 				  const struct i2c_device_id *id)
 {
+	struct device *dev = &client->dev;
 	struct st_nci_i2c_phy *phy;
-	struct st_nci_nfc_platform_data *pdata;
 	int r;
 
 	dev_dbg(&client->dev, "%s\n", __func__);
@@ -326,8 +225,7 @@ static int st_nci_i2c_probe(struct i2c_client *client,
 		return -ENODEV;
 	}
 
-	phy = devm_kzalloc(&client->dev, sizeof(struct st_nci_i2c_phy),
-			   GFP_KERNEL);
+	phy = devm_kzalloc(dev, sizeof(struct st_nci_i2c_phy), GFP_KERNEL);
 	if (!phy)
 		return -ENOMEM;
 
@@ -335,32 +233,22 @@ static int st_nci_i2c_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, phy);
 
-	pdata = client->dev.platform_data;
-	if (!pdata && client->dev.of_node) {
-		r = st_nci_i2c_of_request_resources(client);
-		if (r) {
-			nfc_err(&client->dev, "No platform data\n");
-			return r;
-		}
-	} else if (pdata) {
-		r = st_nci_i2c_request_resources(client);
-		if (r) {
-			nfc_err(&client->dev,
-				"Cannot get platform resources\n");
-			return r;
-		}
-	} else if (ACPI_HANDLE(&client->dev)) {
-		r = st_nci_i2c_acpi_request_resources(client);
-		if (r) {
-			nfc_err(&client->dev, "Cannot get ACPI data\n");
-			return r;
-		}
-	} else {
-		nfc_err(&client->dev,
-			"st_nci platform resources not available\n");
+	r = devm_acpi_dev_add_driver_gpios(dev, acpi_st_nci_gpios);
+	if (r)
+		dev_dbg(dev, "Unable to add GPIO mapping table\n");
+
+	/* Get RESET GPIO */
+	phy->gpiod_reset = devm_gpiod_get(dev, "reset", GPIOD_OUT_HIGH);
+	if (IS_ERR(phy->gpiod_reset)) {
+		nfc_err(dev, "Unable to get RESET GPIO\n");
 		return -ENODEV;
 	}
 
+	phy->se_status.is_ese_present =
+				device_property_read_bool(dev, "ese-present");
+	phy->se_status.is_uicc_present =
+				device_property_read_bool(dev, "uicc-present");
+
 	r = ndlc_probe(phy, &i2c_phy_ops, &client->dev,
 			ST_NCI_FRAME_HEADROOM, ST_NCI_FRAME_TAILROOM,
 			&phy->ndlc, &phy->se_status);
@@ -372,7 +260,7 @@ static int st_nci_i2c_probe(struct i2c_client *client,
 	phy->irq_active = true;
 	r = devm_request_threaded_irq(&client->dev, client->irq, NULL,
 				st_nci_irq_thread_fn,
-				phy->irq_polarity | IRQF_ONESHOT,
+				IRQF_ONESHOT,
 				ST_NCI_DRIVER_NAME, phy);
 	if (r < 0)
 		nfc_err(&client->dev, "Unable to register IRQ handler\n");
diff --git a/drivers/nfc/st-nci/ndlc.c b/drivers/nfc/st-nci/ndlc.c
index 50880d747b02..9477994cf975 100644
--- a/drivers/nfc/st-nci/ndlc.c
+++ b/drivers/nfc/st-nci/ndlc.c
@@ -87,7 +87,7 @@ int ndlc_send(struct llt_ndlc *ndlc, struct sk_buff *skb)
 	u8 pcb = PCB_TYPE_DATAFRAME | PCB_DATAFRAME_RETRANSMIT_NO |
 		PCB_FRAME_CRC_INFO_NOTPRESENT;
 
-	*skb_push(skb, 1) = pcb;
+	*(u8 *)skb_push(skb, 1) = pcb;
 	skb_queue_tail(&ndlc->send_q, skb);
 
 	schedule_work(&ndlc->sm_work);
diff --git a/drivers/nfc/st-nci/spi.c b/drivers/nfc/st-nci/spi.c
index 89e341eba3eb..14705591b0fb 100644
--- a/drivers/nfc/st-nci/spi.c
+++ b/drivers/nfc/st-nci/spi.c
@@ -19,16 +19,13 @@
 
 #include <linux/module.h>
 #include <linux/spi/spi.h>
-#include <linux/gpio.h>
 #include <linux/gpio/consumer.h>
-#include <linux/of_irq.h>
-#include <linux/of_gpio.h>
 #include <linux/acpi.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/nfc.h>
+#include <linux/of.h>
 #include <net/nfc/nci.h>
-#include <linux/platform_data/st-nci.h>
 
 #include "st-nci.h"
 
@@ -41,18 +38,16 @@
 #define ST_NCI_SPI_MIN_SIZE 4   /* PCB(1) + NCI Packet header(3) */
 #define ST_NCI_SPI_MAX_SIZE 250 /* req 4.2.1 */
 
+#define ST_NCI_DRIVER_NAME "st_nci"
 #define ST_NCI_SPI_DRIVER_NAME "st_nci_spi"
 
-#define ST_NCI_GPIO_NAME_RESET "reset"
-
 struct st_nci_spi_phy {
 	struct spi_device *spi_dev;
 	struct llt_ndlc *ndlc;
 
 	bool irq_active;
 
-	unsigned int gpio_reset;
-	unsigned int irq_polarity;
+	struct gpio_desc *gpiod_reset;
 
 	struct st_nci_se_status se_status;
 };
@@ -61,9 +56,9 @@ static int st_nci_spi_enable(void *phy_id)
 {
 	struct st_nci_spi_phy *phy = phy_id;
 
-	gpio_set_value(phy->gpio_reset, 0);
+	gpiod_set_value(phy->gpiod_reset, 0);
 	usleep_range(10000, 15000);
-	gpio_set_value(phy->gpio_reset, 1);
+	gpiod_set_value(phy->gpiod_reset, 1);
 	usleep_range(80000, 85000);
 
 	if (phy->ndlc->powered == 0 && phy->irq_active == 0) {
@@ -223,113 +218,16 @@ static struct nfc_phy_ops spi_phy_ops = {
 	.disable = st_nci_spi_disable,
 };
 
-static int st_nci_spi_acpi_request_resources(struct spi_device *spi_dev)
-{
-	struct st_nci_spi_phy *phy = spi_get_drvdata(spi_dev);
-	struct gpio_desc *gpiod_reset;
-	struct device *dev = &spi_dev->dev;
-	u8 tmp;
-
-	/* Get RESET GPIO from ACPI */
-	gpiod_reset = devm_gpiod_get_index(dev, ST_NCI_GPIO_NAME_RESET, 1,
-					   GPIOD_OUT_HIGH);
-	if (IS_ERR(gpiod_reset)) {
-		nfc_err(dev, "Unable to get RESET GPIO\n");
-		return -ENODEV;
-	}
-
-	phy->gpio_reset = desc_to_gpio(gpiod_reset);
-
-	phy->irq_polarity = irq_get_trigger_type(spi_dev->irq);
-
-	phy->se_status.is_ese_present = false;
-	phy->se_status.is_uicc_present = false;
-
-	if (device_property_present(dev, "ese-present")) {
-		device_property_read_u8(dev, "ese-present", &tmp);
-		tmp = phy->se_status.is_ese_present;
-	}
-
-	if (device_property_present(dev, "uicc-present")) {
-		device_property_read_u8(dev, "uicc-present", &tmp);
-		tmp = phy->se_status.is_uicc_present;
-	}
-
-	return 0;
-}
-
-static int st_nci_spi_of_request_resources(struct spi_device *dev)
-{
-	struct st_nci_spi_phy *phy = spi_get_drvdata(dev);
-	struct device_node *pp;
-	int gpio;
-	int r;
-
-	pp = dev->dev.of_node;
-	if (!pp)
-		return -ENODEV;
-
-	/* Get GPIO from device tree */
-	gpio = of_get_named_gpio(pp, "reset-gpios", 0);
-	if (gpio < 0) {
-		nfc_err(&dev->dev,
-			"Failed to retrieve reset-gpios from device tree\n");
-		return gpio;
-	}
-
-	/* GPIO request and configuration */
-	r = devm_gpio_request_one(&dev->dev, gpio,
-				GPIOF_OUT_INIT_HIGH, ST_NCI_GPIO_NAME_RESET);
-	if (r) {
-		nfc_err(&dev->dev, "Failed to request reset pin\n");
-		return r;
-	}
-	phy->gpio_reset = gpio;
-
-	phy->irq_polarity = irq_get_trigger_type(dev->irq);
+static const struct acpi_gpio_params reset_gpios = { 1, 0, false };
 
-	phy->se_status.is_ese_present =
-				of_property_read_bool(pp, "ese-present");
-	phy->se_status.is_uicc_present =
-				of_property_read_bool(pp, "uicc-present");
-
-	return 0;
-}
-
-static int st_nci_spi_request_resources(struct spi_device *dev)
-{
-	struct st_nci_nfc_platform_data *pdata;
-	struct st_nci_spi_phy *phy = spi_get_drvdata(dev);
-	int r;
-
-	pdata = dev->dev.platform_data;
-	if (pdata == NULL) {
-		nfc_err(&dev->dev, "No platform data\n");
-		return -EINVAL;
-	}
-
-	/* store for later use */
-	phy->gpio_reset = pdata->gpio_reset;
-	phy->irq_polarity = pdata->irq_polarity;
-
-	r = devm_gpio_request_one(&dev->dev,
-			phy->gpio_reset, GPIOF_OUT_INIT_HIGH,
-			ST_NCI_GPIO_NAME_RESET);
-	if (r) {
-		pr_err("%s : reset gpio_request failed\n", __FILE__);
-		return r;
-	}
-
-	phy->se_status.is_ese_present = pdata->is_ese_present;
-	phy->se_status.is_uicc_present = pdata->is_uicc_present;
-
-	return 0;
-}
+static const struct acpi_gpio_mapping acpi_st_nci_gpios[] = {
+	{ "reset-gpios", &reset_gpios, 1 },
+	{},
+};
 
 static int st_nci_spi_probe(struct spi_device *dev)
 {
 	struct st_nci_spi_phy *phy;
-	struct st_nci_nfc_platform_data *pdata;
 	int r;
 
 	dev_dbg(&dev->dev, "%s\n", __func__);
@@ -351,32 +249,22 @@ static int st_nci_spi_probe(struct spi_device *dev)
 
 	spi_set_drvdata(dev, phy);
 
-	pdata = dev->dev.platform_data;
-	if (!pdata && dev->dev.of_node) {
-		r = st_nci_spi_of_request_resources(dev);
-		if (r) {
-			nfc_err(&dev->dev, "No platform data\n");
-			return r;
-		}
-	} else if (pdata) {
-		r = st_nci_spi_request_resources(dev);
-		if (r) {
-			nfc_err(&dev->dev,
-				"Cannot get platform resources\n");
-			return r;
-		}
-	} else if (ACPI_HANDLE(&dev->dev)) {
-		r = st_nci_spi_acpi_request_resources(dev);
-		if (r) {
-			nfc_err(&dev->dev, "Cannot get ACPI data\n");
-			return r;
-		}
-	} else {
-		nfc_err(&dev->dev,
-			"st_nci platform resources not available\n");
-		return -ENODEV;
+	r = devm_acpi_dev_add_driver_gpios(&dev->dev, acpi_st_nci_gpios);
+	if (r)
+		dev_dbg(&dev->dev, "Unable to add GPIO mapping table\n");
+
+	/* Get RESET GPIO */
+	phy->gpiod_reset = devm_gpiod_get(&dev->dev, "reset", GPIOD_OUT_HIGH);
+	if (IS_ERR(phy->gpiod_reset)) {
+		nfc_err(&dev->dev, "Unable to get RESET GPIO\n");
+		return PTR_ERR(phy->gpiod_reset);
 	}
 
+	phy->se_status.is_ese_present =
+			device_property_read_bool(&dev->dev, "ese-present");
+	phy->se_status.is_uicc_present =
+			device_property_read_bool(&dev->dev, "uicc-present");
+
 	r = ndlc_probe(phy, &spi_phy_ops, &dev->dev,
 			ST_NCI_FRAME_HEADROOM, ST_NCI_FRAME_TAILROOM,
 			&phy->ndlc, &phy->se_status);
@@ -388,7 +276,7 @@ static int st_nci_spi_probe(struct spi_device *dev)
 	phy->irq_active = true;
 	r = devm_request_threaded_irq(&dev->dev, dev->irq, NULL,
 				st_nci_irq_thread_fn,
-				phy->irq_polarity | IRQF_ONESHOT,
+				IRQF_ONESHOT,
 				ST_NCI_SPI_DRIVER_NAME, phy);
 	if (r < 0)
 		nfc_err(&dev->dev, "Unable to register IRQ handler\n");
diff --git a/drivers/nfc/st21nfca/core.c b/drivers/nfc/st21nfca/core.c
index 50be3b788f1c..e803fdfa9189 100644
--- a/drivers/nfc/st21nfca/core.c
+++ b/drivers/nfc/st21nfca/core.c
@@ -782,12 +782,12 @@ static int st21nfca_hci_im_transceive(struct nfc_hci_dev *hdev,
 		if (target->supported_protocols == NFC_PROTO_NFC_DEP_MASK)
 			return st21nfca_im_send_dep_req(hdev, skb);
 
-		*skb_push(skb, 1) = 0x1a;
+		*(u8 *)skb_push(skb, 1) = 0x1a;
 		return nfc_hci_send_cmd_async(hdev, target->hci_reader_gate,
 					      ST21NFCA_WR_XCHG_DATA, skb->data,
 					      skb->len, cb, cb_context);
 	case ST21NFCA_RF_READER_14443_3_A_GATE:
-		*skb_push(skb, 1) = 0x1a;	/* CTR, see spec:10.2.2.1 */
+		*(u8 *)skb_push(skb, 1) = 0x1a;	/* CTR, see spec:10.2.2.1 */
 
 		return nfc_hci_send_cmd_async(hdev, target->hci_reader_gate,
 					      ST21NFCA_WR_XCHG_DATA, skb->data,
@@ -797,7 +797,7 @@ static int st21nfca_hci_im_transceive(struct nfc_hci_dev *hdev,
 		info->async_cb = cb;
 		info->async_cb_context = cb_context;
 
-		*skb_push(skb, 1) = 0x17;
+		*(u8 *)skb_push(skb, 1) = 0x17;
 
 		return nfc_hci_send_cmd_async(hdev, target->hci_reader_gate,
 					      ST21NFCA_WR_XCHG_DATA, skb->data,
diff --git a/drivers/nfc/st21nfca/dep.c b/drivers/nfc/st21nfca/dep.c
index 798a32bbac5d..fd08be2917e6 100644
--- a/drivers/nfc/st21nfca/dep.c
+++ b/drivers/nfc/st21nfca/dep.c
@@ -315,10 +315,10 @@ int st21nfca_tm_send_dep_res(struct nfc_hci_dev *hdev, struct sk_buff *skb)
 	int r;
 	struct st21nfca_hci_info *info = nfc_hci_get_clientdata(hdev);
 
-	*skb_push(skb, 1) = info->dep_info.curr_nfc_dep_pni;
-	*skb_push(skb, 1) = ST21NFCA_NFCIP1_DEP_RES;
-	*skb_push(skb, 1) = ST21NFCA_NFCIP1_RES;
-	*skb_push(skb, 1) = skb->len;
+	*(u8 *)skb_push(skb, 1) = info->dep_info.curr_nfc_dep_pni;
+	*(u8 *)skb_push(skb, 1) = ST21NFCA_NFCIP1_DEP_RES;
+	*(u8 *)skb_push(skb, 1) = ST21NFCA_NFCIP1_RES;
+	*(u8 *)skb_push(skb, 1) = skb->len;
 
 	r = nfc_hci_send_event(hdev, ST21NFCA_RF_CARD_F_GATE,
 			ST21NFCA_EVT_SEND_DATA, skb->data, skb->len);
@@ -466,7 +466,7 @@ static void st21nfca_im_send_psl_req(struct nfc_hci_dev *hdev, u8 did, u8 bsi,
 	psl_req->brs = (0x30 & bsi << 4) | (bri & 0x03);
 	psl_req->fsl = lri;
 
-	*skb_push(skb, 1) = info->dep_info.to | 0x10;
+	*(u8 *)skb_push(skb, 1) = info->dep_info.to | 0x10;
 
 	st21nfca_im_send_pdu(info, skb);
 }
@@ -564,11 +564,11 @@ int st21nfca_im_send_atr_req(struct nfc_hci_dev *hdev, u8 *gb, size_t gb_len)
 	atr_req->ppi = ST21NFCA_LR_BITS_PAYLOAD_SIZE_254B;
 	if (gb_len) {
 		atr_req->ppi |= ST21NFCA_GB_BIT;
-		memcpy(skb_put(skb, gb_len), gb, gb_len);
+		skb_put_data(skb, gb, gb_len);
 	}
 	atr_req->length = sizeof(struct st21nfca_atr_req) + hdev->gb_len;
 
-	*skb_push(skb, 1) = info->dep_info.to | 0x10; /* timeout */
+	*(u8 *)skb_push(skb, 1) = info->dep_info.to | 0x10; /* timeout */
 
 	info->async_cb_type = ST21NFCA_CB_TYPE_READER_F;
 	info->async_cb_context = info;
@@ -629,10 +629,10 @@ static void st21nfca_im_recv_dep_res_cb(void *context, struct sk_buff *skb,
 		case ST21NFCA_NFC_DEP_PFB_SUPERVISOR_PDU:
 			pr_err("Received a SUPERVISOR PDU\n");
 			skb_pull(skb, size);
-			*skb_push(skb, 1) = ST21NFCA_NFCIP1_DEP_REQ;
-			*skb_push(skb, 1) = ST21NFCA_NFCIP1_REQ;
-			*skb_push(skb, 1) = skb->len;
-			*skb_push(skb, 1) = info->dep_info.to | 0x10;
+			*(u8 *)skb_push(skb, 1) = ST21NFCA_NFCIP1_DEP_REQ;
+			*(u8 *)skb_push(skb, 1) = ST21NFCA_NFCIP1_REQ;
+			*(u8 *)skb_push(skb, 1) = skb->len;
+			*(u8 *)skb_push(skb, 1) = info->dep_info.to | 0x10;
 
 			st21nfca_im_send_pdu(info, skb);
 			break;
@@ -655,12 +655,12 @@ int st21nfca_im_send_dep_req(struct nfc_hci_dev *hdev, struct sk_buff *skb)
 	info->async_cb_context = info;
 	info->async_cb = st21nfca_im_recv_dep_res_cb;
 
-	*skb_push(skb, 1) = info->dep_info.curr_nfc_dep_pni;
-	*skb_push(skb, 1) = ST21NFCA_NFCIP1_DEP_REQ;
-	*skb_push(skb, 1) = ST21NFCA_NFCIP1_REQ;
-	*skb_push(skb, 1) = skb->len;
+	*(u8 *)skb_push(skb, 1) = info->dep_info.curr_nfc_dep_pni;
+	*(u8 *)skb_push(skb, 1) = ST21NFCA_NFCIP1_DEP_REQ;
+	*(u8 *)skb_push(skb, 1) = ST21NFCA_NFCIP1_REQ;
+	*(u8 *)skb_push(skb, 1) = skb->len;
 
-	*skb_push(skb, 1) = info->dep_info.to | 0x10;
+	*(u8 *)skb_push(skb, 1) = info->dep_info.to | 0x10;
 
 	return nfc_hci_send_cmd_async(hdev, ST21NFCA_RF_READER_F_GATE,
 				      ST21NFCA_WR_XCHG_DATA,
diff --git a/drivers/nfc/st21nfca/i2c.c b/drivers/nfc/st21nfca/i2c.c
index 02a920ca07c8..cd1f7bfa75eb 100644
--- a/drivers/nfc/st21nfca/i2c.c
+++ b/drivers/nfc/st21nfca/i2c.c
@@ -61,8 +61,6 @@
 #define ST21NFCA_HCI_DRIVER_NAME "st21nfca_hci"
 #define ST21NFCA_HCI_I2C_DRIVER_NAME "st21nfca_hci_i2c"
 
-#define ST21NFCA_GPIO_NAME_EN "enable"
-
 struct st21nfca_i2c_phy {
 	struct i2c_client *i2c_dev;
 	struct nfc_hci_dev *hdev;
@@ -171,16 +169,16 @@ static void st21nfca_hci_add_len_crc(struct sk_buff *skb)
 	u16 crc;
 	u8 tmp;
 
-	*skb_push(skb, 1) = 0;
+	*(u8 *)skb_push(skb, 1) = 0;
 
 	crc = crc_ccitt(0xffff, skb->data, skb->len);
 	crc = ~crc;
 
 	tmp = crc & 0x00ff;
-	*skb_put(skb, 1) = tmp;
+	skb_put_u8(skb, tmp);
 
 	tmp = (crc >> 8) & 0x00ff;
-	*skb_put(skb, 1) = tmp;
+	skb_put_u8(skb, tmp);
 }
 
 static void st21nfca_hci_remove_len_crc(struct sk_buff *skb)
@@ -214,9 +212,9 @@ static int st21nfca_hci_i2c_write(void *phy_id, struct sk_buff *skb)
 	st21nfca_hci_add_len_crc(skb);
 
 	/* add ST21NFCA_SOF_EOF on tail */
-	*skb_put(skb, 1) = ST21NFCA_SOF_EOF;
+	skb_put_u8(skb, ST21NFCA_SOF_EOF);
 	/* add ST21NFCA_SOF_EOF on head */
-	*skb_push(skb, 1) = ST21NFCA_SOF_EOF;
+	*(u8 *)skb_push(skb, 1) = ST21NFCA_SOF_EOF;
 
 	/*
 	 * Compute byte stuffing
@@ -407,7 +405,7 @@ static int st21nfca_hci_i2c_read(struct st21nfca_i2c_phy *phy,
 			phy->current_read_len = 0;
 		}
 
-		memcpy(skb_put(skb, len), buf, len);
+		skb_put_data(skb, buf, len);
 
 		if (skb->data[skb->len - 1] == ST21NFCA_SOF_EOF) {
 			phy->current_read_len = 0;
@@ -501,41 +499,17 @@ static struct nfc_phy_ops i2c_phy_ops = {
 	.disable = st21nfca_hci_i2c_disable,
 };
 
-static int st21nfca_hci_i2c_acpi_request_resources(struct i2c_client *client)
-{
-	struct st21nfca_i2c_phy *phy = i2c_get_clientdata(client);
-	struct device *dev = &client->dev;
-
-	/* Get EN GPIO from ACPI */
-	phy->gpiod_ena = devm_gpiod_get_index(dev, ST21NFCA_GPIO_NAME_EN, 1,
-					      GPIOD_OUT_LOW);
-	if (IS_ERR(phy->gpiod_ena)) {
-		nfc_err(dev, "Unable to get ENABLE GPIO\n");
-		return PTR_ERR(phy->gpiod_ena);
-	}
-
-	return 0;
-}
-
-static int st21nfca_hci_i2c_of_request_resources(struct i2c_client *client)
-{
-	struct st21nfca_i2c_phy *phy = i2c_get_clientdata(client);
-	struct device *dev = &client->dev;
-
-	/* Get GPIO from device tree */
-	phy->gpiod_ena = devm_gpiod_get_index(dev, ST21NFCA_GPIO_NAME_EN, 0,
-					      GPIOD_OUT_HIGH);
-	if (IS_ERR(phy->gpiod_ena)) {
-		nfc_err(dev, "Failed to request enable pin\n");
-		return PTR_ERR(phy->gpiod_ena);
-	}
+static const struct acpi_gpio_params enable_gpios = { 1, 0, false };
 
-	return 0;
-}
+static const struct acpi_gpio_mapping acpi_st21nfca_gpios[] = {
+	{ "enable-gpios", &enable_gpios, 1 },
+	{},
+};
 
 static int st21nfca_hci_i2c_probe(struct i2c_client *client,
 				  const struct i2c_device_id *id)
 {
+	struct device *dev = &client->dev;
 	struct st21nfca_i2c_phy *phy;
 	int r;
 
@@ -562,21 +536,15 @@ static int st21nfca_hci_i2c_probe(struct i2c_client *client,
 	mutex_init(&phy->phy_lock);
 	i2c_set_clientdata(client, phy);
 
-	if (client->dev.of_node) {
-		r = st21nfca_hci_i2c_of_request_resources(client);
-		if (r) {
-			nfc_err(&client->dev, "No platform data\n");
-			return r;
-		}
-	} else if (ACPI_HANDLE(&client->dev)) {
-		r = st21nfca_hci_i2c_acpi_request_resources(client);
-		if (r) {
-			nfc_err(&client->dev, "Cannot get ACPI data\n");
-			return r;
-		}
-	} else {
-		nfc_err(&client->dev, "st21nfca platform resources not available\n");
-		return -ENODEV;
+	r = devm_acpi_dev_add_driver_gpios(dev, acpi_st21nfca_gpios);
+	if (r)
+		dev_dbg(dev, "Unable to add GPIO mapping table\n");
+
+	/* Get EN GPIO from resource provider */
+	phy->gpiod_ena = devm_gpiod_get(dev, "enable", GPIOD_OUT_LOW);
+	if (IS_ERR(phy->gpiod_ena)) {
+		nfc_err(dev, "Unable to get ENABLE GPIO\n");
+		return PTR_ERR(phy->gpiod_ena);
 	}
 
 	phy->se_status.is_ese_present =
diff --git a/drivers/nfc/st95hf/core.c b/drivers/nfc/st95hf/core.c
index c2840e412962..2b26f762fbc3 100644
--- a/drivers/nfc/st95hf/core.c
+++ b/drivers/nfc/st95hf/core.c
@@ -949,7 +949,7 @@ static int st95hf_in_send_cmd(struct nfc_digital_dev *ddev,
 	switch (stcontext->current_rf_tech) {
 	case NFC_DIGITAL_RF_TECH_106A:
 		len_data_to_tag = skb->len + 1;
-		*skb_put(skb, 1) = stcontext->sendrcv_trflag;
+		skb_put_u8(skb, stcontext->sendrcv_trflag);
 		break;
 	case NFC_DIGITAL_RF_TECH_106B:
 	case NFC_DIGITAL_RF_TECH_ISO15693:
diff --git a/drivers/nfc/trf7970a.c b/drivers/nfc/trf7970a.c
index 2d1c8ca6e679..eee5cc1a9220 100644
--- a/drivers/nfc/trf7970a.c
+++ b/drivers/nfc/trf7970a.c
@@ -20,9 +20,8 @@
 #include <linux/nfc.h>
 #include <linux/skbuff.h>
 #include <linux/delay.h>
-#include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
 #include <linux/of.h>
-#include <linux/of_gpio.h>
 #include <linux/spi/spi.h>
 #include <linux/regulator/consumer.h>
 
@@ -123,11 +122,10 @@
 		 NFC_PROTO_ISO14443_B_MASK | NFC_PROTO_FELICA_MASK | \
 		 NFC_PROTO_ISO15693_MASK | NFC_PROTO_NFC_DEP_MASK)
 
-#define TRF7970A_AUTOSUSPEND_DELAY		30000 /* 30 seconds */
+#define TRF7970A_AUTOSUSPEND_DELAY		30000	/* 30 seconds */
 #define TRF7970A_13MHZ_CLOCK_FREQUENCY		13560000
 #define TRF7970A_27MHZ_CLOCK_FREQUENCY		27120000
 
-
 #define TRF7970A_RX_SKB_ALLOC_SIZE		256
 
 #define TRF7970A_FIFO_SIZE			127
@@ -152,7 +150,6 @@
  */
 #define TRF7970A_QUIRK_IRQ_STATUS_READ		BIT(0)
 #define TRF7970A_QUIRK_EN2_MUST_STAY_LOW	BIT(1)
-#define TRF7970A_QUIRK_T5T_RMB_EXTRA_BYTE	BIT(2)
 
 /* Direct commands */
 #define TRF7970A_CMD_IDLE			0x00
@@ -295,7 +292,7 @@
 #define TRF7970A_REG_IO_CTRL_AUTO_REG		BIT(7)
 
 /* IRQ Status Register Bits */
-#define TRF7970A_IRQ_STATUS_NORESP		BIT(0) /* ISO15693 only */
+#define TRF7970A_IRQ_STATUS_NORESP		BIT(0)	/* ISO15693 only */
 #define TRF7970A_IRQ_STATUS_NFC_COL_ERROR	BIT(0)
 #define TRF7970A_IRQ_STATUS_COL			BIT(1)
 #define TRF7970A_IRQ_STATUS_FRAMING_EOF_ERROR	BIT(2)
@@ -451,16 +448,14 @@ struct trf7970a {
 	u8				md_rf_tech;
 	u8				tx_cmd;
 	bool				issue_eof;
-	bool				adjust_resp_len;
-	int				en2_gpio;
-	int				en_gpio;
+	struct gpio_desc		*en_gpiod;
+	struct gpio_desc		*en2_gpiod;
 	struct mutex			lock;
 	unsigned int			timeout;
 	bool				ignore_timeout;
 	struct delayed_work		timeout_work;
 };
 
-
 static int trf7970a_cmd(struct trf7970a *trf, u8 opcode)
 {
 	u8 cmd = TRF7970A_CMD_BIT_CTRL | TRF7970A_CMD_BIT_OPCODE(opcode);
@@ -471,7 +466,7 @@ static int trf7970a_cmd(struct trf7970a *trf, u8 opcode)
 	ret = spi_write(trf->spi, &cmd, 1);
 	if (ret)
 		dev_err(trf->dev, "%s - cmd: 0x%x, ret: %d\n", __func__, cmd,
-				ret);
+			ret);
 	return ret;
 }
 
@@ -483,14 +478,15 @@ static int trf7970a_read(struct trf7970a *trf, u8 reg, u8 *val)
 	ret = spi_write_then_read(trf->spi, &addr, 1, val, 1);
 	if (ret)
 		dev_err(trf->dev, "%s - addr: 0x%x, ret: %d\n", __func__, addr,
-				ret);
+			ret);
 
 	dev_dbg(trf->dev, "read(0x%x): 0x%x\n", addr, *val);
 
 	return ret;
 }
 
-static int trf7970a_read_cont(struct trf7970a *trf, u8 reg, u8 *buf, size_t len)
+static int trf7970a_read_cont(struct trf7970a *trf, u8 reg, u8 *buf,
+			      size_t len)
 {
 	u8 addr = reg | TRF7970A_CMD_BIT_RW | TRF7970A_CMD_BIT_CONTINUOUS;
 	struct spi_transfer t[2];
@@ -514,7 +510,7 @@ static int trf7970a_read_cont(struct trf7970a *trf, u8 reg, u8 *buf, size_t len)
 	ret = spi_sync(trf->spi, &m);
 	if (ret)
 		dev_err(trf->dev, "%s - addr: 0x%x, ret: %d\n", __func__, addr,
-				ret);
+			ret);
 	return ret;
 }
 
@@ -528,7 +524,7 @@ static int trf7970a_write(struct trf7970a *trf, u8 reg, u8 val)
 	ret = spi_write(trf->spi, buf, 2);
 	if (ret)
 		dev_err(trf->dev, "%s - write: 0x%x 0x%x, ret: %d\n", __func__,
-				buf[0], buf[1], ret);
+			buf[0], buf[1], ret);
 
 	return ret;
 }
@@ -550,7 +546,7 @@ static int trf7970a_read_irqstatus(struct trf7970a *trf, u8 *status)
 
 	if (ret)
 		dev_err(trf->dev, "%s - irqstatus: Status read failed: %d\n",
-				__func__, ret);
+			__func__, ret);
 	else
 		*status = buf[0];
 
@@ -564,12 +560,12 @@ static int trf7970a_read_target_proto(struct trf7970a *trf, u8 *target_proto)
 	u8 addr;
 
 	addr = TRF79070A_NFC_TARGET_PROTOCOL | TRF7970A_CMD_BIT_RW |
-		TRF7970A_CMD_BIT_CONTINUOUS;
+	       TRF7970A_CMD_BIT_CONTINUOUS;
 
 	ret = spi_write_then_read(trf->spi, &addr, 1, buf, 2);
 	if (ret)
 		dev_err(trf->dev, "%s - target_proto: Read failed: %d\n",
-				__func__, ret);
+			__func__, ret);
 	else
 		*target_proto = buf[0];
 
@@ -600,7 +596,7 @@ static int trf7970a_mode_detect(struct trf7970a *trf, u8 *rf_tech)
 		break;
 	default:
 		dev_dbg(trf->dev, "%s - mode_detect: target_proto: 0x%x\n",
-				__func__, target_proto);
+			__func__, target_proto);
 		return -EIO;
 	}
 
@@ -616,8 +612,8 @@ static void trf7970a_send_upstream(struct trf7970a *trf)
 
 	if (trf->rx_skb && !IS_ERR(trf->rx_skb) && !trf->aborting)
 		print_hex_dump_debug("trf7970a rx data: ", DUMP_PREFIX_NONE,
-				16, 1, trf->rx_skb->data, trf->rx_skb->len,
-				false);
+				     16, 1, trf->rx_skb->data, trf->rx_skb->len,
+				     false);
 
 	trf->state = TRF7970A_ST_IDLE;
 
@@ -632,13 +628,6 @@ static void trf7970a_send_upstream(struct trf7970a *trf)
 		trf->aborting = false;
 	}
 
-	if (trf->adjust_resp_len) {
-		if (trf->rx_skb)
-			skb_trim(trf->rx_skb, trf->rx_skb->len - 1);
-
-		trf->adjust_resp_len = false;
-	}
-
 	trf->cb(trf->ddev, trf->cb_arg, trf->rx_skb);
 
 	trf->rx_skb = NULL;
@@ -657,7 +646,8 @@ static void trf7970a_send_err_upstream(struct trf7970a *trf, int errno)
 }
 
 static int trf7970a_transmit(struct trf7970a *trf, struct sk_buff *skb,
-		unsigned int len, u8 *prefix, unsigned int prefix_len)
+			     unsigned int len, u8 *prefix,
+			     unsigned int prefix_len)
 {
 	struct spi_transfer t[2];
 	struct spi_message m;
@@ -665,7 +655,7 @@ static int trf7970a_transmit(struct trf7970a *trf, struct sk_buff *skb,
 	int ret;
 
 	print_hex_dump_debug("trf7970a tx data: ", DUMP_PREFIX_NONE,
-			16, 1, skb->data, len, false);
+			     16, 1, skb->data, len, false);
 
 	spi_message_init(&m);
 
@@ -682,7 +672,7 @@ static int trf7970a_transmit(struct trf7970a *trf, struct sk_buff *skb,
 	ret = spi_sync(trf->spi, &m);
 	if (ret) {
 		dev_err(trf->dev, "%s - Can't send tx data: %d\n", __func__,
-				ret);
+			ret);
 		return ret;
 	}
 
@@ -706,7 +696,7 @@ static int trf7970a_transmit(struct trf7970a *trf, struct sk_buff *skb,
 	}
 
 	dev_dbg(trf->dev, "Setting timeout for %d ms, state: %d\n", timeout,
-			trf->state);
+		trf->state);
 
 	schedule_delayed_work(&trf->timeout_work, msecs_to_jiffies(timeout));
 
@@ -774,9 +764,9 @@ static void trf7970a_drain_fifo(struct trf7970a *trf, u8 status)
 
 	if (fifo_bytes > skb_tailroom(skb)) {
 		skb = skb_copy_expand(skb, skb_headroom(skb),
-				max_t(int, fifo_bytes,
-					TRF7970A_RX_SKB_ALLOC_SIZE),
-				GFP_KERNEL);
+				      max_t(int, fifo_bytes,
+					    TRF7970A_RX_SKB_ALLOC_SIZE),
+				      GFP_KERNEL);
 		if (!skb) {
 			trf7970a_send_err_upstream(trf, -ENOMEM);
 			return;
@@ -787,7 +777,7 @@ static void trf7970a_drain_fifo(struct trf7970a *trf, u8 status)
 	}
 
 	ret = trf7970a_read_cont(trf, TRF7970A_FIFO_IO_REGISTER,
-			skb_put(skb, fifo_bytes), fifo_bytes);
+				 skb_put(skb, fifo_bytes), fifo_bytes);
 	if (ret) {
 		trf7970a_send_err_upstream(trf, ret);
 		return;
@@ -795,8 +785,7 @@ static void trf7970a_drain_fifo(struct trf7970a *trf, u8 status)
 
 	/* If received Type 2 ACK/NACK, shift right 4 bits and pass up */
 	if ((trf->framing == NFC_DIGITAL_FRAMING_NFCA_T2T) && (skb->len == 1) &&
-			(trf->special_fcn_reg1 ==
-				 TRF7970A_SPECIAL_FCN_REG1_4_BIT_RX)) {
+	    (trf->special_fcn_reg1 == TRF7970A_SPECIAL_FCN_REG1_4_BIT_RX)) {
 		skb->data[0] >>= 4;
 		status = TRF7970A_IRQ_STATUS_SRX;
 	} else {
@@ -819,16 +808,16 @@ static void trf7970a_drain_fifo(struct trf7970a *trf, u8 status)
 	}
 
 no_rx_data:
-	if (status == TRF7970A_IRQ_STATUS_SRX) { /* Receive complete */
+	if (status == TRF7970A_IRQ_STATUS_SRX) {	/* Receive complete */
 		trf7970a_send_upstream(trf);
 		return;
 	}
 
 	dev_dbg(trf->dev, "Setting timeout for %d ms\n",
-			TRF7970A_WAIT_FOR_RX_DATA_TIMEOUT);
+		TRF7970A_WAIT_FOR_RX_DATA_TIMEOUT);
 
 	schedule_delayed_work(&trf->timeout_work,
-			msecs_to_jiffies(TRF7970A_WAIT_FOR_RX_DATA_TIMEOUT));
+			   msecs_to_jiffies(TRF7970A_WAIT_FOR_RX_DATA_TIMEOUT));
 }
 
 static irqreturn_t trf7970a_irq(int irq, void *dev_id)
@@ -851,7 +840,7 @@ static irqreturn_t trf7970a_irq(int irq, void *dev_id)
 	}
 
 	dev_dbg(trf->dev, "IRQ - state: %d, status: 0x%x\n", trf->state,
-			status);
+		status);
 
 	if (!status) {
 		mutex_unlock(&trf->lock);
@@ -876,7 +865,7 @@ static irqreturn_t trf7970a_irq(int irq, void *dev_id)
 	case TRF7970A_ST_WAIT_FOR_TX_FIFO:
 		if (status & TRF7970A_IRQ_STATUS_TX) {
 			trf->ignore_timeout =
-				!cancel_delayed_work(&trf->timeout_work);
+			    !cancel_delayed_work(&trf->timeout_work);
 			trf7970a_fill_fifo(trf);
 		} else {
 			trf7970a_send_err_upstream(trf, -EIO);
@@ -886,11 +875,11 @@ static irqreturn_t trf7970a_irq(int irq, void *dev_id)
 	case TRF7970A_ST_WAIT_FOR_RX_DATA_CONT:
 		if (status & TRF7970A_IRQ_STATUS_SRX) {
 			trf->ignore_timeout =
-				!cancel_delayed_work(&trf->timeout_work);
+			    !cancel_delayed_work(&trf->timeout_work);
 			trf7970a_drain_fifo(trf, status);
 		} else if (status & TRF7970A_IRQ_STATUS_FIFO) {
 			ret = trf7970a_read(trf, TRF7970A_FIFO_STATUS,
-					&fifo_bytes);
+					    &fifo_bytes);
 
 			fifo_bytes &= ~TRF7970A_FIFO_STATUS_OVERFLOW;
 
@@ -899,14 +888,14 @@ static irqreturn_t trf7970a_irq(int irq, void *dev_id)
 			else if (!fifo_bytes)
 				trf7970a_cmd(trf, TRF7970A_CMD_FIFO_RESET);
 		} else if ((status == TRF7970A_IRQ_STATUS_TX) ||
-				(!trf->is_initiator &&
-				 (status == (TRF7970A_IRQ_STATUS_TX |
-					     TRF7970A_IRQ_STATUS_NFC_RF)))) {
+			   (!trf->is_initiator &&
+			    (status == (TRF7970A_IRQ_STATUS_TX |
+					TRF7970A_IRQ_STATUS_NFC_RF)))) {
 			trf7970a_cmd(trf, TRF7970A_CMD_FIFO_RESET);
 
 			if (!trf->timeout) {
-				trf->ignore_timeout = !cancel_delayed_work(
-						&trf->timeout_work);
+				trf->ignore_timeout =
+				    !cancel_delayed_work(&trf->timeout_work);
 				trf->rx_skb = ERR_PTR(0);
 				trf7970a_send_upstream(trf);
 				break;
@@ -930,13 +919,13 @@ static irqreturn_t trf7970a_irq(int irq, void *dev_id)
 				break;
 			case NFC_DIGITAL_FRAMING_NFCA_ANTICOL_COMPLETE:
 				ret = trf7970a_write(trf,
-					TRF7970A_SPECIAL_FCN_REG1,
-					TRF7970A_SPECIAL_FCN_REG1_14_ANTICOLL);
+					 TRF7970A_SPECIAL_FCN_REG1,
+					 TRF7970A_SPECIAL_FCN_REG1_14_ANTICOLL);
 				if (ret)
 					goto err_unlock_exit;
 
 				trf->special_fcn_reg1 =
-					TRF7970A_SPECIAL_FCN_REG1_14_ANTICOLL;
+				    TRF7970A_SPECIAL_FCN_REG1_14_ANTICOLL;
 				break;
 			default:
 				break;
@@ -944,7 +933,7 @@ static irqreturn_t trf7970a_irq(int irq, void *dev_id)
 
 			if (iso_ctrl != trf->iso_ctrl) {
 				ret = trf7970a_write(trf, TRF7970A_ISO_CTRL,
-						iso_ctrl);
+						     iso_ctrl);
 				if (ret)
 					goto err_unlock_exit;
 
@@ -961,7 +950,7 @@ static irqreturn_t trf7970a_irq(int irq, void *dev_id)
 	case TRF7970A_ST_LISTENING:
 		if (status & TRF7970A_IRQ_STATUS_SRX) {
 			trf->ignore_timeout =
-				!cancel_delayed_work(&trf->timeout_work);
+			    !cancel_delayed_work(&trf->timeout_work);
 			trf7970a_drain_fifo(trf, status);
 		} else if (!(status & TRF7970A_IRQ_STATUS_NFC_RF)) {
 			trf7970a_send_err_upstream(trf, -EIO);
@@ -970,7 +959,7 @@ static irqreturn_t trf7970a_irq(int irq, void *dev_id)
 	case TRF7970A_ST_LISTENING_MD:
 		if (status & TRF7970A_IRQ_STATUS_SRX) {
 			trf->ignore_timeout =
-				!cancel_delayed_work(&trf->timeout_work);
+			    !cancel_delayed_work(&trf->timeout_work);
 
 			ret = trf7970a_mode_detect(trf, &trf->md_rf_tech);
 			if (ret) {
@@ -985,7 +974,7 @@ static irqreturn_t trf7970a_irq(int irq, void *dev_id)
 		break;
 	default:
 		dev_err(trf->dev, "%s - Driver in invalid state: %d\n",
-				__func__, trf->state);
+			__func__, trf->state);
 	}
 
 err_unlock_exit:
@@ -1010,19 +999,19 @@ static void trf7970a_issue_eof(struct trf7970a *trf)
 	trf->state = TRF7970A_ST_WAIT_FOR_RX_DATA;
 
 	dev_dbg(trf->dev, "Setting timeout for %d ms, state: %d\n",
-			trf->timeout, trf->state);
+		trf->timeout, trf->state);
 
 	schedule_delayed_work(&trf->timeout_work,
-			msecs_to_jiffies(trf->timeout));
+			      msecs_to_jiffies(trf->timeout));
 }
 
 static void trf7970a_timeout_work_handler(struct work_struct *work)
 {
 	struct trf7970a *trf = container_of(work, struct trf7970a,
-			timeout_work.work);
+					    timeout_work.work);
 
 	dev_dbg(trf->dev, "Timeout - state: %d, ignore_timeout: %d\n",
-			trf->state, trf->ignore_timeout);
+		trf->state, trf->ignore_timeout);
 
 	mutex_lock(&trf->lock);
 
@@ -1053,7 +1042,7 @@ static int trf7970a_init(struct trf7970a *trf)
 		goto err_out;
 
 	ret = trf7970a_write(trf, TRF7970A_REG_IO_CTRL,
-			trf->io_ctrl | TRF7970A_REG_IO_CTRL_VRS(0x1));
+			     trf->io_ctrl | TRF7970A_REG_IO_CTRL_VRS(0x1));
 	if (ret)
 		goto err_out;
 
@@ -1066,13 +1055,13 @@ static int trf7970a_init(struct trf7970a *trf)
 	trf->chip_status_ctrl &= ~TRF7970A_CHIP_STATUS_RF_ON;
 
 	ret = trf7970a_write(trf, TRF7970A_MODULATOR_SYS_CLK_CTRL,
-			trf->modulator_sys_clk_ctrl);
+			     trf->modulator_sys_clk_ctrl);
 	if (ret)
 		goto err_out;
 
 	ret = trf7970a_write(trf, TRF7970A_ADJUTABLE_FIFO_IRQ_LEVELS,
-			TRF7970A_ADJUTABLE_FIFO_IRQ_LEVELS_WLH_96 |
-			TRF7970A_ADJUTABLE_FIFO_IRQ_LEVELS_WLL_32);
+			     TRF7970A_ADJUTABLE_FIFO_IRQ_LEVELS_WLH_96 |
+			     TRF7970A_ADJUTABLE_FIFO_IRQ_LEVELS_WLL_32);
 	if (ret)
 		goto err_out;
 
@@ -1093,7 +1082,7 @@ err_out:
 static void trf7970a_switch_rf_off(struct trf7970a *trf)
 {
 	if ((trf->state == TRF7970A_ST_PWR_OFF) ||
-			(trf->state == TRF7970A_ST_RF_OFF))
+	    (trf->state == TRF7970A_ST_RF_OFF))
 		return;
 
 	dev_dbg(trf->dev, "Switching rf off\n");
@@ -1117,9 +1106,9 @@ static int trf7970a_switch_rf_on(struct trf7970a *trf)
 
 	pm_runtime_get_sync(trf->dev);
 
-	if (trf->state != TRF7970A_ST_RF_OFF) { /* Power on, RF off */
+	if (trf->state != TRF7970A_ST_RF_OFF) {	/* Power on, RF off */
 		dev_err(trf->dev, "%s - Incorrect state: %d\n", __func__,
-				trf->state);
+			trf->state);
 		return -EINVAL;
 	}
 
@@ -1154,7 +1143,7 @@ static int trf7970a_switch_rf(struct nfc_digital_dev *ddev, bool on)
 			break;
 		default:
 			dev_err(trf->dev, "%s - Invalid request: %d %d\n",
-					__func__, trf->state, on);
+				__func__, trf->state, on);
 			trf7970a_switch_rf_off(trf);
 			ret = -EINVAL;
 		}
@@ -1165,7 +1154,7 @@ static int trf7970a_switch_rf(struct nfc_digital_dev *ddev, bool on)
 			break;
 		default:
 			dev_err(trf->dev, "%s - Invalid request: %d %d\n",
-					__func__, trf->state, on);
+				__func__, trf->state, on);
 			ret = -EINVAL;
 			/* FALLTHROUGH */
 		case TRF7970A_ST_IDLE:
@@ -1190,36 +1179,36 @@ static int trf7970a_in_config_rf_tech(struct trf7970a *trf, int tech)
 	case NFC_DIGITAL_RF_TECH_106A:
 		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_14443A_106;
 		trf->modulator_sys_clk_ctrl =
-			(trf->modulator_sys_clk_ctrl & 0xf8) |
-			TRF7970A_MODULATOR_DEPTH_OOK;
+		    (trf->modulator_sys_clk_ctrl & 0xf8) |
+		    TRF7970A_MODULATOR_DEPTH_OOK;
 		trf->guard_time = TRF7970A_GUARD_TIME_NFCA;
 		break;
 	case NFC_DIGITAL_RF_TECH_106B:
 		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_14443B_106;
 		trf->modulator_sys_clk_ctrl =
-			(trf->modulator_sys_clk_ctrl & 0xf8) |
-			TRF7970A_MODULATOR_DEPTH_ASK10;
+		    (trf->modulator_sys_clk_ctrl & 0xf8) |
+		    TRF7970A_MODULATOR_DEPTH_ASK10;
 		trf->guard_time = TRF7970A_GUARD_TIME_NFCB;
 		break;
 	case NFC_DIGITAL_RF_TECH_212F:
 		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_FELICA_212;
 		trf->modulator_sys_clk_ctrl =
-			(trf->modulator_sys_clk_ctrl & 0xf8) |
-			TRF7970A_MODULATOR_DEPTH_ASK10;
+		    (trf->modulator_sys_clk_ctrl & 0xf8) |
+		    TRF7970A_MODULATOR_DEPTH_ASK10;
 		trf->guard_time = TRF7970A_GUARD_TIME_NFCF;
 		break;
 	case NFC_DIGITAL_RF_TECH_424F:
 		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_FELICA_424;
 		trf->modulator_sys_clk_ctrl =
-			(trf->modulator_sys_clk_ctrl & 0xf8) |
-			TRF7970A_MODULATOR_DEPTH_ASK10;
+		    (trf->modulator_sys_clk_ctrl & 0xf8) |
+		    TRF7970A_MODULATOR_DEPTH_ASK10;
 		trf->guard_time = TRF7970A_GUARD_TIME_NFCF;
 		break;
 	case NFC_DIGITAL_RF_TECH_ISO15693:
 		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_15693_SGL_1OF4_2648;
 		trf->modulator_sys_clk_ctrl =
-			(trf->modulator_sys_clk_ctrl & 0xf8) |
-			TRF7970A_MODULATOR_DEPTH_OOK;
+		    (trf->modulator_sys_clk_ctrl & 0xf8) |
+		    TRF7970A_MODULATOR_DEPTH_OOK;
 		trf->guard_time = TRF7970A_GUARD_TIME_15693;
 		break;
 	default:
@@ -1246,7 +1235,8 @@ static int trf7970a_is_rf_field(struct trf7970a *trf, bool *is_rf_field)
 	u8 rssi;
 
 	ret = trf7970a_write(trf, TRF7970A_CHIP_STATUS_CTRL,
-			trf->chip_status_ctrl | TRF7970A_CHIP_STATUS_REC_ON);
+			     trf->chip_status_ctrl |
+			     TRF7970A_CHIP_STATUS_REC_ON);
 	if (ret)
 		return ret;
 
@@ -1261,7 +1251,7 @@ static int trf7970a_is_rf_field(struct trf7970a *trf, bool *is_rf_field)
 		return ret;
 
 	ret = trf7970a_write(trf, TRF7970A_CHIP_STATUS_CTRL,
-			trf->chip_status_ctrl);
+			     trf->chip_status_ctrl);
 	if (ret)
 		return ret;
 
@@ -1328,15 +1318,15 @@ static int trf7970a_in_config_framing(struct trf7970a *trf, int framing)
 		trf->iso_ctrl = iso_ctrl;
 
 		ret = trf7970a_write(trf, TRF7970A_MODULATOR_SYS_CLK_CTRL,
-				trf->modulator_sys_clk_ctrl);
+				     trf->modulator_sys_clk_ctrl);
 		if (ret)
 			return ret;
 	}
 
 	if (!(trf->chip_status_ctrl & TRF7970A_CHIP_STATUS_RF_ON)) {
 		ret = trf7970a_write(trf, TRF7970A_CHIP_STATUS_CTRL,
-				trf->chip_status_ctrl |
-					TRF7970A_CHIP_STATUS_RF_ON);
+				     trf->chip_status_ctrl |
+				     TRF7970A_CHIP_STATUS_RF_ON);
 		if (ret)
 			return ret;
 
@@ -1349,7 +1339,7 @@ static int trf7970a_in_config_framing(struct trf7970a *trf, int framing)
 }
 
 static int trf7970a_in_configure_hw(struct nfc_digital_dev *ddev, int type,
-		int param)
+				    int param)
 {
 	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
 	int ret;
@@ -1361,7 +1351,7 @@ static int trf7970a_in_configure_hw(struct nfc_digital_dev *ddev, int type,
 	trf->is_initiator = true;
 
 	if ((trf->state == TRF7970A_ST_PWR_OFF) ||
-			(trf->state == TRF7970A_ST_RF_OFF)) {
+	    (trf->state == TRF7970A_ST_RF_OFF)) {
 		ret = trf7970a_switch_rf_on(trf);
 		if (ret)
 			goto err_unlock;
@@ -1419,7 +1409,7 @@ static int trf7970a_per_cmd_config(struct trf7970a *trf, struct sk_buff *skb)
 	 * has to send an EOF in order to get a response.
 	 */
 	if ((trf->technology == NFC_DIGITAL_RF_TECH_106A) &&
-			(trf->framing == NFC_DIGITAL_FRAMING_NFCA_T2T)) {
+	    (trf->framing == NFC_DIGITAL_FRAMING_NFCA_T2T)) {
 		if (req[0] == NFC_T2T_CMD_READ)
 			special_fcn_reg1 = 0;
 		else
@@ -1427,7 +1417,7 @@ static int trf7970a_per_cmd_config(struct trf7970a *trf, struct sk_buff *skb)
 
 		if (special_fcn_reg1 != trf->special_fcn_reg1) {
 			ret = trf7970a_write(trf, TRF7970A_SPECIAL_FCN_REG1,
-					special_fcn_reg1);
+					     special_fcn_reg1);
 			if (ret)
 				return ret;
 
@@ -1447,7 +1437,7 @@ static int trf7970a_per_cmd_config(struct trf7970a *trf, struct sk_buff *skb)
 			iso_ctrl |= TRF7970A_ISO_CTRL_15693_SGL_1OF4_2648;
 			break;
 		case (ISO15693_REQ_FLAG_SUB_CARRIER |
-				ISO15693_REQ_FLAG_DATA_RATE):
+		      ISO15693_REQ_FLAG_DATA_RATE):
 			iso_ctrl |= TRF7970A_ISO_CTRL_15693_DBL_1OF4_2669;
 			break;
 		}
@@ -1460,23 +1450,18 @@ static int trf7970a_per_cmd_config(struct trf7970a *trf, struct sk_buff *skb)
 			trf->iso_ctrl = iso_ctrl;
 		}
 
-		if (trf->framing == NFC_DIGITAL_FRAMING_ISO15693_T5T) {
-			if (trf7970a_is_iso15693_write_or_lock(req[1]) &&
-					(req[0] & ISO15693_REQ_FLAG_OPTION))
-				trf->issue_eof = true;
-			else if ((trf->quirks &
-					TRF7970A_QUIRK_T5T_RMB_EXTRA_BYTE) &&
-				 (req[1] == ISO15693_CMD_READ_MULTIPLE_BLOCK))
-				trf->adjust_resp_len = true;
-		}
+		if ((trf->framing == NFC_DIGITAL_FRAMING_ISO15693_T5T) &&
+		    trf7970a_is_iso15693_write_or_lock(req[1]) &&
+		    (req[0] & ISO15693_REQ_FLAG_OPTION))
+			trf->issue_eof = true;
 	}
 
 	return 0;
 }
 
 static int trf7970a_send_cmd(struct nfc_digital_dev *ddev,
-		struct sk_buff *skb, u16 timeout,
-		nfc_digital_cmd_complete_t cb, void *arg)
+			     struct sk_buff *skb, u16 timeout,
+			     nfc_digital_cmd_complete_t cb, void *arg)
 {
 	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
 	u8 prefix[5];
@@ -1485,7 +1470,7 @@ static int trf7970a_send_cmd(struct nfc_digital_dev *ddev,
 	u8 status;
 
 	dev_dbg(trf->dev, "New request - state: %d, timeout: %d ms, len: %d\n",
-			trf->state, timeout, skb->len);
+		trf->state, timeout, skb->len);
 
 	if (skb->len > TRF7970A_TX_MAX)
 		return -EINVAL;
@@ -1493,9 +1478,9 @@ static int trf7970a_send_cmd(struct nfc_digital_dev *ddev,
 	mutex_lock(&trf->lock);
 
 	if ((trf->state != TRF7970A_ST_IDLE) &&
-			(trf->state != TRF7970A_ST_IDLE_RX_BLOCKED)) {
+	    (trf->state != TRF7970A_ST_IDLE_RX_BLOCKED)) {
 		dev_err(trf->dev, "%s - Bogus state: %d\n", __func__,
-				trf->state);
+			trf->state);
 		ret = -EIO;
 		goto out_err;
 	}
@@ -1509,7 +1494,7 @@ static int trf7970a_send_cmd(struct nfc_digital_dev *ddev,
 
 	if (timeout) {
 		trf->rx_skb = nfc_alloc_recv_skb(TRF7970A_RX_SKB_ALLOC_SIZE,
-				GFP_KERNEL);
+						 GFP_KERNEL);
 		if (!trf->rx_skb) {
 			dev_dbg(trf->dev, "Can't alloc rx_skb\n");
 			ret = -ENOMEM;
@@ -1546,14 +1531,14 @@ static int trf7970a_send_cmd(struct nfc_digital_dev *ddev,
 	 * That totals 5 bytes.
 	 */
 	prefix[0] = TRF7970A_CMD_BIT_CTRL |
-			TRF7970A_CMD_BIT_OPCODE(TRF7970A_CMD_FIFO_RESET);
+	    TRF7970A_CMD_BIT_OPCODE(TRF7970A_CMD_FIFO_RESET);
 	prefix[1] = TRF7970A_CMD_BIT_CTRL |
-			TRF7970A_CMD_BIT_OPCODE(trf->tx_cmd);
+	    TRF7970A_CMD_BIT_OPCODE(trf->tx_cmd);
 	prefix[2] = TRF7970A_CMD_BIT_CONTINUOUS | TRF7970A_TX_LENGTH_BYTE1;
 
 	if (trf->framing == NFC_DIGITAL_FRAMING_NFCA_SHORT) {
 		prefix[3] = 0x00;
-		prefix[4] = 0x0f; /* 7 bits */
+		prefix[4] = 0x0f;	/* 7 bits */
 	} else {
 		prefix[3] = (len & 0xf00) >> 4;
 		prefix[3] |= ((len & 0xf0) >> 4);
@@ -1587,25 +1572,24 @@ static int trf7970a_tg_config_rf_tech(struct trf7970a *trf, int tech)
 	switch (tech) {
 	case NFC_DIGITAL_RF_TECH_106A:
 		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_NFC_NFC_CE_MODE |
-			TRF7970A_ISO_CTRL_NFC_CE |
-			TRF7970A_ISO_CTRL_NFC_CE_14443A;
+		    TRF7970A_ISO_CTRL_NFC_CE | TRF7970A_ISO_CTRL_NFC_CE_14443A;
 		trf->modulator_sys_clk_ctrl =
-			(trf->modulator_sys_clk_ctrl & 0xf8) |
-			TRF7970A_MODULATOR_DEPTH_OOK;
+		    (trf->modulator_sys_clk_ctrl & 0xf8) |
+		    TRF7970A_MODULATOR_DEPTH_OOK;
 		break;
 	case NFC_DIGITAL_RF_TECH_212F:
 		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_NFC_NFC_CE_MODE |
-			TRF7970A_ISO_CTRL_NFC_NFCF_212;
+		    TRF7970A_ISO_CTRL_NFC_NFCF_212;
 		trf->modulator_sys_clk_ctrl =
-			(trf->modulator_sys_clk_ctrl & 0xf8) |
-			TRF7970A_MODULATOR_DEPTH_ASK10;
+		    (trf->modulator_sys_clk_ctrl & 0xf8) |
+		    TRF7970A_MODULATOR_DEPTH_ASK10;
 		break;
 	case NFC_DIGITAL_RF_TECH_424F:
 		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_NFC_NFC_CE_MODE |
-			TRF7970A_ISO_CTRL_NFC_NFCF_424;
+		    TRF7970A_ISO_CTRL_NFC_NFCF_424;
 		trf->modulator_sys_clk_ctrl =
-			(trf->modulator_sys_clk_ctrl & 0xf8) |
-			TRF7970A_MODULATOR_DEPTH_ASK10;
+		    (trf->modulator_sys_clk_ctrl & 0xf8) |
+		    TRF7970A_MODULATOR_DEPTH_ASK10;
 		break;
 	default:
 		dev_dbg(trf->dev, "Unsupported rf technology: %d\n", tech);
@@ -1622,9 +1606,9 @@ static int trf7970a_tg_config_rf_tech(struct trf7970a *trf, int tech)
 	 * here.
 	 */
 	if ((trf->framing == NFC_DIGITAL_FRAMING_NFC_DEP_ACTIVATED) &&
-			(trf->iso_ctrl_tech != trf->iso_ctrl)) {
+	    (trf->iso_ctrl_tech != trf->iso_ctrl)) {
 		ret = trf7970a_write(trf, TRF7970A_ISO_CTRL,
-				trf->iso_ctrl_tech);
+				     trf->iso_ctrl_tech);
 
 		trf->iso_ctrl = trf->iso_ctrl_tech;
 	}
@@ -1679,15 +1663,15 @@ static int trf7970a_tg_config_framing(struct trf7970a *trf, int framing)
 		trf->iso_ctrl = iso_ctrl;
 
 		ret = trf7970a_write(trf, TRF7970A_MODULATOR_SYS_CLK_CTRL,
-				trf->modulator_sys_clk_ctrl);
+				     trf->modulator_sys_clk_ctrl);
 		if (ret)
 			return ret;
 	}
 
 	if (!(trf->chip_status_ctrl & TRF7970A_CHIP_STATUS_RF_ON)) {
 		ret = trf7970a_write(trf, TRF7970A_CHIP_STATUS_CTRL,
-				trf->chip_status_ctrl |
-					TRF7970A_CHIP_STATUS_RF_ON);
+				     trf->chip_status_ctrl |
+				     TRF7970A_CHIP_STATUS_RF_ON);
 		if (ret)
 			return ret;
 
@@ -1698,7 +1682,7 @@ static int trf7970a_tg_config_framing(struct trf7970a *trf, int framing)
 }
 
 static int trf7970a_tg_configure_hw(struct nfc_digital_dev *ddev, int type,
-		int param)
+				    int param)
 {
 	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
 	int ret;
@@ -1710,7 +1694,7 @@ static int trf7970a_tg_configure_hw(struct nfc_digital_dev *ddev, int type,
 	trf->is_initiator = false;
 
 	if ((trf->state == TRF7970A_ST_PWR_OFF) ||
-			(trf->state == TRF7970A_ST_RF_OFF)) {
+	    (trf->state == TRF7970A_ST_RF_OFF)) {
 		ret = trf7970a_switch_rf_on(trf);
 		if (ret)
 			goto err_unlock;
@@ -1734,7 +1718,8 @@ err_unlock:
 }
 
 static int _trf7970a_tg_listen(struct nfc_digital_dev *ddev, u16 timeout,
-		nfc_digital_cmd_complete_t cb, void *arg, bool mode_detect)
+			       nfc_digital_cmd_complete_t cb, void *arg,
+			       bool mode_detect)
 {
 	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
 	int ret;
@@ -1742,9 +1727,9 @@ static int _trf7970a_tg_listen(struct nfc_digital_dev *ddev, u16 timeout,
 	mutex_lock(&trf->lock);
 
 	if ((trf->state != TRF7970A_ST_IDLE) &&
-			(trf->state != TRF7970A_ST_IDLE_RX_BLOCKED)) {
+	    (trf->state != TRF7970A_ST_IDLE_RX_BLOCKED)) {
 		dev_err(trf->dev, "%s - Bogus state: %d\n", __func__,
-				trf->state);
+			trf->state);
 		ret = -EIO;
 		goto out_err;
 	}
@@ -1757,7 +1742,7 @@ static int _trf7970a_tg_listen(struct nfc_digital_dev *ddev, u16 timeout,
 	}
 
 	trf->rx_skb = nfc_alloc_recv_skb(TRF7970A_RX_SKB_ALLOC_SIZE,
-			GFP_KERNEL);
+					 GFP_KERNEL);
 	if (!trf->rx_skb) {
 		dev_dbg(trf->dev, "Can't alloc rx_skb\n");
 		ret = -ENOMEM;
@@ -1765,25 +1750,25 @@ static int _trf7970a_tg_listen(struct nfc_digital_dev *ddev, u16 timeout,
 	}
 
 	ret = trf7970a_write(trf, TRF7970A_RX_SPECIAL_SETTINGS,
-			TRF7970A_RX_SPECIAL_SETTINGS_HBT |
-			TRF7970A_RX_SPECIAL_SETTINGS_M848 |
-			TRF7970A_RX_SPECIAL_SETTINGS_C424 |
-			TRF7970A_RX_SPECIAL_SETTINGS_C212);
+			     TRF7970A_RX_SPECIAL_SETTINGS_HBT |
+			     TRF7970A_RX_SPECIAL_SETTINGS_M848 |
+			     TRF7970A_RX_SPECIAL_SETTINGS_C424 |
+			     TRF7970A_RX_SPECIAL_SETTINGS_C212);
 	if (ret)
 		goto out_err;
 
 	ret = trf7970a_write(trf, TRF7970A_REG_IO_CTRL,
-			trf->io_ctrl | TRF7970A_REG_IO_CTRL_VRS(0x1));
+			     trf->io_ctrl | TRF7970A_REG_IO_CTRL_VRS(0x1));
 	if (ret)
 		goto out_err;
 
 	ret = trf7970a_write(trf, TRF7970A_NFC_LOW_FIELD_LEVEL,
-			TRF7970A_NFC_LOW_FIELD_LEVEL_RFDET(0x3));
+			     TRF7970A_NFC_LOW_FIELD_LEVEL_RFDET(0x3));
 	if (ret)
 		goto out_err;
 
 	ret = trf7970a_write(trf, TRF7970A_NFC_TARGET_LEVEL,
-			TRF7970A_NFC_TARGET_LEVEL_RFDET(0x7));
+			     TRF7970A_NFC_TARGET_LEVEL_RFDET(0x7));
 	if (ret)
 		goto out_err;
 
@@ -1808,32 +1793,33 @@ out_err:
 }
 
 static int trf7970a_tg_listen(struct nfc_digital_dev *ddev, u16 timeout,
-		nfc_digital_cmd_complete_t cb, void *arg)
+			      nfc_digital_cmd_complete_t cb, void *arg)
 {
 	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
 
 	dev_dbg(trf->dev, "Listen - state: %d, timeout: %d ms\n",
-			trf->state, timeout);
+		trf->state, timeout);
 
 	return _trf7970a_tg_listen(ddev, timeout, cb, arg, false);
 }
 
 static int trf7970a_tg_listen_md(struct nfc_digital_dev *ddev,
-		u16 timeout, nfc_digital_cmd_complete_t cb, void *arg)
+				 u16 timeout, nfc_digital_cmd_complete_t cb,
+				 void *arg)
 {
 	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
 	int ret;
 
 	dev_dbg(trf->dev, "Listen MD - state: %d, timeout: %d ms\n",
-			trf->state, timeout);
+		trf->state, timeout);
 
 	ret = trf7970a_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH,
-			NFC_DIGITAL_RF_TECH_106A);
+				       NFC_DIGITAL_RF_TECH_106A);
 	if (ret)
 		return ret;
 
 	ret = trf7970a_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
-			NFC_DIGITAL_FRAMING_NFCA_NFC_DEP);
+				       NFC_DIGITAL_FRAMING_NFCA_NFC_DEP);
 	if (ret)
 		return ret;
 
@@ -1845,7 +1831,7 @@ static int trf7970a_tg_get_rf_tech(struct nfc_digital_dev *ddev, u8 *rf_tech)
 	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
 
 	dev_dbg(trf->dev, "Get RF Tech - state: %d, rf_tech: %d\n",
-			trf->state, trf->md_rf_tech);
+		trf->state, trf->md_rf_tech);
 
 	*rf_tech = trf->md_rf_tech;
 
@@ -1908,14 +1894,13 @@ static int trf7970a_power_up(struct trf7970a *trf)
 
 	usleep_range(5000, 6000);
 
-	if (!(trf->quirks & TRF7970A_QUIRK_EN2_MUST_STAY_LOW)) {
-		if (gpio_is_valid(trf->en2_gpio)) {
-			gpio_set_value(trf->en2_gpio, 1);
-			usleep_range(1000, 2000);
-		}
+	if (trf->en2_gpiod &&
+	    !(trf->quirks & TRF7970A_QUIRK_EN2_MUST_STAY_LOW)) {
+		gpiod_set_value_cansleep(trf->en2_gpiod, 1);
+		usleep_range(1000, 2000);
 	}
 
-	gpio_set_value(trf->en_gpio, 1);
+	gpiod_set_value_cansleep(trf->en_gpiod, 1);
 
 	usleep_range(20000, 21000);
 
@@ -1935,18 +1920,19 @@ static int trf7970a_power_down(struct trf7970a *trf)
 
 	if (trf->state != TRF7970A_ST_RF_OFF) {
 		dev_dbg(trf->dev, "Can't power down - not RF_OFF state (%d)\n",
-				trf->state);
+			trf->state);
 		return -EBUSY;
 	}
 
-	gpio_set_value(trf->en_gpio, 0);
-	if (gpio_is_valid(trf->en2_gpio))
-		gpio_set_value(trf->en2_gpio, 0);
+	gpiod_set_value_cansleep(trf->en_gpiod, 0);
+
+	if (trf->en2_gpiod && !(trf->quirks & TRF7970A_QUIRK_EN2_MUST_STAY_LOW))
+		gpiod_set_value_cansleep(trf->en2_gpiod, 0);
 
 	ret = regulator_disable(trf->regulator);
 	if (ret)
 		dev_err(trf->dev, "%s - Can't disable VIN: %d\n", __func__,
-				ret);
+			ret);
 
 	trf->state = TRF7970A_ST_PWR_OFF;
 
@@ -2003,12 +1989,6 @@ static int trf7970a_get_autosuspend_delay(struct device_node *np)
 	return autosuspend_delay;
 }
 
-static int trf7970a_get_vin_voltage_override(struct device_node *np,
-		u32 *vin_uvolts)
-{
-	return of_property_read_u32(np, "vin-voltage-override", vin_uvolts);
-}
-
 static int trf7970a_probe(struct spi_device *spi)
 {
 	struct device_node *np = spi->dev.of_node;
@@ -2038,53 +2018,48 @@ static int trf7970a_probe(struct spi_device *spi)
 		return ret;
 	}
 
-	if (of_property_read_bool(np, "t5t-rmb-extra-byte-quirk"))
-		trf->quirks |= TRF7970A_QUIRK_T5T_RMB_EXTRA_BYTE;
-
 	if (of_property_read_bool(np, "irq-status-read-quirk"))
 		trf->quirks |= TRF7970A_QUIRK_IRQ_STATUS_READ;
 
-	/* There are two enable pins - both must be present */
-	trf->en_gpio = of_get_named_gpio(np, "ti,enable-gpios", 0);
-	if (!gpio_is_valid(trf->en_gpio)) {
+	/* There are two enable pins - only EN must be present in the DT */
+	trf->en_gpiod = devm_gpiod_get_index(trf->dev, "ti,enable", 0,
+					     GPIOD_OUT_LOW);
+	if (IS_ERR(trf->en_gpiod)) {
 		dev_err(trf->dev, "No EN GPIO property\n");
-		return trf->en_gpio;
+		return PTR_ERR(trf->en_gpiod);
 	}
 
-	ret = devm_gpio_request_one(trf->dev, trf->en_gpio,
-			GPIOF_DIR_OUT | GPIOF_INIT_LOW, "trf7970a EN");
-	if (ret) {
-		dev_err(trf->dev, "Can't request EN GPIO: %d\n", ret);
-		return ret;
-	}
-
-	trf->en2_gpio = of_get_named_gpio(np, "ti,enable-gpios", 1);
-	if (!gpio_is_valid(trf->en2_gpio)) {
+	trf->en2_gpiod = devm_gpiod_get_index_optional(trf->dev, "ti,enable", 1,
+						       GPIOD_OUT_LOW);
+	if (!trf->en2_gpiod) {
 		dev_info(trf->dev, "No EN2 GPIO property\n");
-	} else {
-		ret = devm_gpio_request_one(trf->dev, trf->en2_gpio,
-				GPIOF_DIR_OUT | GPIOF_INIT_LOW, "trf7970a EN2");
-		if (ret) {
-			dev_err(trf->dev, "Can't request EN2 GPIO: %d\n", ret);
-			return ret;
-		}
+	} else if (IS_ERR(trf->en2_gpiod)) {
+		dev_err(trf->dev, "Error getting EN2 GPIO property: %ld\n",
+			PTR_ERR(trf->en2_gpiod));
+		return PTR_ERR(trf->en2_gpiod);
+	} else if (of_property_read_bool(np, "en2-rf-quirk")) {
+		trf->quirks |= TRF7970A_QUIRK_EN2_MUST_STAY_LOW;
 	}
 
 	of_property_read_u32(np, "clock-frequency", &clk_freq);
-	if ((clk_freq != TRF7970A_27MHZ_CLOCK_FREQUENCY) ||
-		(clk_freq != TRF7970A_13MHZ_CLOCK_FREQUENCY)) {
+	if ((clk_freq != TRF7970A_27MHZ_CLOCK_FREQUENCY) &&
+	    (clk_freq != TRF7970A_13MHZ_CLOCK_FREQUENCY)) {
 		dev_err(trf->dev,
-			"clock-frequency (%u Hz) unsupported\n",
-			clk_freq);
+			"clock-frequency (%u Hz) unsupported\n", clk_freq);
 		return -EINVAL;
 	}
 
-	if (of_property_read_bool(np, "en2-rf-quirk"))
-		trf->quirks |= TRF7970A_QUIRK_EN2_MUST_STAY_LOW;
+	if (clk_freq == TRF7970A_27MHZ_CLOCK_FREQUENCY) {
+		trf->modulator_sys_clk_ctrl = TRF7970A_MODULATOR_27MHZ;
+		dev_dbg(trf->dev, "trf7970a configured for 27MHz crystal\n");
+	} else {
+		trf->modulator_sys_clk_ctrl = 0;
+	}
 
 	ret = devm_request_threaded_irq(trf->dev, spi->irq, NULL,
-			trf7970a_irq, IRQF_TRIGGER_RISING | IRQF_ONESHOT,
-			"trf7970a", trf);
+					trf7970a_irq,
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					"trf7970a", trf);
 	if (ret) {
 		dev_err(trf->dev, "Can't request IRQ#%d: %d\n", spi->irq, ret);
 		return ret;
@@ -2106,10 +2081,7 @@ static int trf7970a_probe(struct spi_device *spi)
 		goto err_destroy_lock;
 	}
 
-	ret = trf7970a_get_vin_voltage_override(np, &uvolts);
-	if (ret)
-		uvolts = regulator_get_voltage(trf->regulator);
-
+	uvolts = regulator_get_voltage(trf->regulator);
 	if (uvolts > 4000000)
 		trf->chip_status_ctrl = TRF7970A_CHIP_STATUS_VRS5_3;
 
@@ -2132,9 +2104,10 @@ static int trf7970a_probe(struct spi_device *spi)
 	}
 
 	trf->ddev = nfc_digital_allocate_device(&trf7970a_nfc_ops,
-			TRF7970A_SUPPORTED_PROTOCOLS,
-			NFC_DIGITAL_DRV_CAPS_IN_CRC |
-				NFC_DIGITAL_DRV_CAPS_TG_CRC, 0, 0);
+						TRF7970A_SUPPORTED_PROTOCOLS,
+						NFC_DIGITAL_DRV_CAPS_IN_CRC |
+						NFC_DIGITAL_DRV_CAPS_TG_CRC, 0,
+						0);
 	if (!trf->ddev) {
 		dev_err(trf->dev, "Can't allocate NFC digital device\n");
 		ret = -ENOMEM;
@@ -2157,7 +2130,7 @@ static int trf7970a_probe(struct spi_device *spi)
 	ret = nfc_digital_register_device(trf->ddev);
 	if (ret) {
 		dev_err(trf->dev, "Can't register NFC digital device: %d\n",
-				ret);
+			ret);
 		goto err_shutdown;
 	}
 
@@ -2266,29 +2239,31 @@ static int trf7970a_pm_runtime_resume(struct device *dev)
 static const struct dev_pm_ops trf7970a_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(trf7970a_suspend, trf7970a_resume)
 	SET_RUNTIME_PM_OPS(trf7970a_pm_runtime_suspend,
-			trf7970a_pm_runtime_resume, NULL)
+			   trf7970a_pm_runtime_resume, NULL)
 };
 
 static const struct of_device_id trf7970a_of_match[] = {
-	{ .compatible = "ti,trf7970a", },
-	{ /* sentinel */ },
+	{.compatible = "ti,trf7970a",},
+	{},
 };
+
 MODULE_DEVICE_TABLE(of, trf7970a_of_match);
 
 static const struct spi_device_id trf7970a_id_table[] = {
-	{ "trf7970a", 0 },
-	{ }
+	{"trf7970a", 0},
+	{}
 };
+
 MODULE_DEVICE_TABLE(spi, trf7970a_id_table);
 
 static struct spi_driver trf7970a_spi_driver = {
 	.probe		= trf7970a_probe,
 	.remove		= trf7970a_remove,
 	.id_table	= trf7970a_id_table,
-	.driver		= {
-		.name	= "trf7970a",
-		.of_match_table = of_match_ptr(trf7970a_of_match),
-		.pm	= &trf7970a_pm_ops,
+	.driver	= {
+		.name		= "trf7970a",
+		.of_match_table	= of_match_ptr(trf7970a_of_match),
+		.pm		= &trf7970a_pm_ops,
 	},
 };
 
diff --git a/drivers/ntb/hw/intel/ntb_hw_intel.c b/drivers/ntb/hw/intel/ntb_hw_intel.c
index c00238491673..7b3b6fd63d7d 100644
--- a/drivers/ntb/hw/intel/ntb_hw_intel.c
+++ b/drivers/ntb/hw/intel/ntb_hw_intel.c
@@ -2878,7 +2878,7 @@ static const struct intel_ntb_reg skx_reg = {
 	.link_is_up		= xeon_link_is_up,
 	.db_ioread		= skx_db_ioread,
 	.db_iowrite		= skx_db_iowrite,
-	.db_size		= sizeof(u64),
+	.db_size		= sizeof(u32),
 	.ntb_ctl		= SKX_NTBCNTL_OFFSET,
 	.mw_bar			= {2, 4},
 };
diff --git a/drivers/ntb/ntb_transport.c b/drivers/ntb/ntb_transport.c
index 02ca45fdd892..10e5bf460139 100644
--- a/drivers/ntb/ntb_transport.c
+++ b/drivers/ntb/ntb_transport.c
@@ -177,14 +177,12 @@ struct ntb_transport_qp {
 	u64 rx_err_ver;
 	u64 rx_memcpy;
 	u64 rx_async;
-	u64 dma_rx_prep_err;
 	u64 tx_bytes;
 	u64 tx_pkts;
 	u64 tx_ring_full;
 	u64 tx_err_no_buf;
 	u64 tx_memcpy;
 	u64 tx_async;
-	u64 dma_tx_prep_err;
 };
 
 struct ntb_transport_mw {
@@ -254,8 +252,6 @@ enum {
 #define QP_TO_MW(nt, qp)	((qp) % nt->mw_count)
 #define NTB_QP_DEF_NUM_ENTRIES	100
 #define NTB_LINK_DOWN_TIMEOUT	10
-#define DMA_RETRIES		20
-#define DMA_OUT_RESOURCE_TO	msecs_to_jiffies(50)
 
 static void ntb_transport_rxc_db(unsigned long data);
 static const struct ntb_ctx_ops ntb_transport_ops;
@@ -516,12 +512,6 @@ static ssize_t debugfs_read(struct file *filp, char __user *ubuf, size_t count,
 	out_offset += snprintf(buf + out_offset, out_count - out_offset,
 			       "free tx - \t%u\n",
 			       ntb_transport_tx_free_entry(qp));
-	out_offset += snprintf(buf + out_offset, out_count - out_offset,
-			       "DMA tx prep err - \t%llu\n",
-			       qp->dma_tx_prep_err);
-	out_offset += snprintf(buf + out_offset, out_count - out_offset,
-			       "DMA rx prep err - \t%llu\n",
-			       qp->dma_rx_prep_err);
 
 	out_offset += snprintf(buf + out_offset, out_count - out_offset,
 			       "\n");
@@ -623,7 +613,7 @@ static int ntb_transport_setup_qp_mw(struct ntb_transport_ctx *nt,
 	if (!mw->virt_addr)
 		return -ENOMEM;
 
-	if (qp_count % mw_count && mw_num + 1 < qp_count / mw_count)
+	if (mw_num < qp_count % mw_count)
 		num_qps_mw = qp_count / mw_count + 1;
 	else
 		num_qps_mw = qp_count / mw_count;
@@ -768,8 +758,6 @@ static void ntb_qp_link_down_reset(struct ntb_transport_qp *qp)
 	qp->tx_err_no_buf = 0;
 	qp->tx_memcpy = 0;
 	qp->tx_async = 0;
-	qp->dma_tx_prep_err = 0;
-	qp->dma_rx_prep_err = 0;
 }
 
 static void ntb_qp_link_cleanup(struct ntb_transport_qp *qp)
@@ -1000,7 +988,7 @@ static int ntb_transport_init_queue(struct ntb_transport_ctx *nt,
 	qp->event_handler = NULL;
 	ntb_qp_link_down_reset(qp);
 
-	if (qp_count % mw_count && mw_num + 1 < qp_count / mw_count)
+	if (mw_num < qp_count % mw_count)
 		num_qps_mw = qp_count / mw_count + 1;
 	else
 		num_qps_mw = qp_count / mw_count;
@@ -1128,8 +1116,8 @@ static int ntb_transport_probe(struct ntb_client *self, struct ntb_dev *ndev)
 	qp_count = ilog2(qp_bitmap);
 	if (max_num_clients && max_num_clients < qp_count)
 		qp_count = max_num_clients;
-	else if (mw_count < qp_count)
-		qp_count = mw_count;
+	else if (nt->mw_count < qp_count)
+		qp_count = nt->mw_count;
 
 	qp_bitmap &= BIT_ULL(qp_count) - 1;
 
@@ -1317,7 +1305,6 @@ static int ntb_async_rx_submit(struct ntb_queue_entry *entry, void *offset)
 	struct dmaengine_unmap_data *unmap;
 	dma_cookie_t cookie;
 	void *buf = entry->buf;
-	int retries = 0;
 
 	len = entry->len;
 	device = chan->device;
@@ -1346,22 +1333,11 @@ static int ntb_async_rx_submit(struct ntb_queue_entry *entry, void *offset)
 
 	unmap->from_cnt = 1;
 
-	for (retries = 0; retries < DMA_RETRIES; retries++) {
-		txd = device->device_prep_dma_memcpy(chan,
-						     unmap->addr[1],
-						     unmap->addr[0], len,
-						     DMA_PREP_INTERRUPT);
-		if (txd)
-			break;
-
-		set_current_state(TASK_INTERRUPTIBLE);
-		schedule_timeout(DMA_OUT_RESOURCE_TO);
-	}
-
-	if (!txd) {
-		qp->dma_rx_prep_err++;
+	txd = device->device_prep_dma_memcpy(chan, unmap->addr[1],
+					     unmap->addr[0], len,
+					     DMA_PREP_INTERRUPT);
+	if (!txd)
 		goto err_get_unmap;
-	}
 
 	txd->callback_result = ntb_rx_copy_callback;
 	txd->callback_param = entry;
@@ -1606,7 +1582,6 @@ static int ntb_async_tx_submit(struct ntb_transport_qp *qp,
 	struct dmaengine_unmap_data *unmap;
 	dma_addr_t dest;
 	dma_cookie_t cookie;
-	int retries = 0;
 
 	device = chan->device;
 	dest = qp->tx_mw_phys + qp->tx_max_frame * entry->tx_index;
@@ -1628,21 +1603,10 @@ static int ntb_async_tx_submit(struct ntb_transport_qp *qp,
 
 	unmap->to_cnt = 1;
 
-	for (retries = 0; retries < DMA_RETRIES; retries++) {
-		txd = device->device_prep_dma_memcpy(chan, dest,
-						     unmap->addr[0], len,
-						     DMA_PREP_INTERRUPT);
-		if (txd)
-			break;
-
-		set_current_state(TASK_INTERRUPTIBLE);
-		schedule_timeout(DMA_OUT_RESOURCE_TO);
-	}
-
-	if (!txd) {
-		qp->dma_tx_prep_err++;
+	txd = device->device_prep_dma_memcpy(chan, dest, unmap->addr[0], len,
+					     DMA_PREP_INTERRUPT);
+	if (!txd)
 		goto err_get_unmap;
-	}
 
 	txd->callback_result = ntb_tx_copy_callback;
 	txd->callback_param = entry;
diff --git a/drivers/ntb/test/ntb_perf.c b/drivers/ntb/test/ntb_perf.c
index 434e1d474f33..5cab2831ce99 100644
--- a/drivers/ntb/test/ntb_perf.c
+++ b/drivers/ntb/test/ntb_perf.c
@@ -90,11 +90,11 @@ MODULE_PARM_DESC(max_mw_size, "Limit size of large memory windows");
 
 static unsigned int seg_order = 19; /* 512K */
 module_param(seg_order, uint, 0644);
-MODULE_PARM_DESC(seg_order, "size order [n^2] of buffer segment for testing");
+MODULE_PARM_DESC(seg_order, "size order [2^n] of buffer segment for testing");
 
 static unsigned int run_order = 32; /* 4G */
 module_param(run_order, uint, 0644);
-MODULE_PARM_DESC(run_order, "size order [n^2] of total data to transfer");
+MODULE_PARM_DESC(run_order, "size order [2^n] of total data to transfer");
 
 static bool use_dma; /* default to 0 */
 module_param(use_dma, bool, 0644);
diff --git a/drivers/nubus/nubus.c b/drivers/nubus/nubus.c
index 77a48a5164ff..df431e8a0631 100644
--- a/drivers/nubus/nubus.c
+++ b/drivers/nubus/nubus.c
@@ -13,7 +13,6 @@
 #include <linux/nubus.h>
 #include <linux/errno.h>
 #include <linux/init.h>
-#include <linux/delay.h>
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <asm/setup.h>
@@ -34,14 +33,6 @@ extern void oss_nubus_init(void);
 
 #define NUBUS_TEST_PATTERN 0x5A932BC7
 
-/* Define this if you like to live dangerously - it is known not to
-   work on pretty much every machine except the Quadra 630 and the LC
-   III. */
-#undef I_WANT_TO_PROBE_SLOT_ZERO
-
-/* This sometimes helps combat failure to boot */
-#undef TRY_TO_DODGE_WSOD
-
 /* Globals */
 
 struct nubus_dev *nubus_devices;
@@ -101,9 +92,6 @@ static void nubus_rewind(unsigned char **ptr, int len, int map)
 {
 	unsigned char *p = *ptr;
 
-	/* Sanity check */
-	if (len > 65536)
-		pr_err("rewind of 0x%08x!\n", len);
 	while (len) {
 		do {
 			p--;
@@ -117,8 +105,6 @@ static void nubus_advance(unsigned char **ptr, int len, int map)
 {
 	unsigned char *p = *ptr;
 
-	if (len > 65536)
-		pr_err("advance of 0x%08x!\n", len);
 	while (len) {
 		while (not_useful(p, map))
 			p++;
@@ -130,10 +116,15 @@ static void nubus_advance(unsigned char **ptr, int len, int map)
 
 static void nubus_move(unsigned char **ptr, int len, int map)
 {
+	unsigned long slot_space = (unsigned long)*ptr & 0xFF000000;
+
 	if (len > 0)
 		nubus_advance(ptr, len, map);
 	else if (len < 0)
 		nubus_rewind(ptr, -len, map);
+
+	if (((unsigned long)*ptr & 0xFF000000) != slot_space)
+		pr_err("%s: moved out of slot address space!\n", __func__);
 }
 
 /* Now, functions to read the sResource tree */
@@ -454,10 +445,6 @@ nubus_get_functional_resource(struct nubus_board *board, int slot,
 	pr_info("  Function 0x%02x:\n", parent->type);
 	nubus_get_subdir(parent, &dir);
 
-	/* Apple seems to have botched the ROM on the IIx */
-	if (slot == 0 && (unsigned long)dir.base % 2)
-		dir.base += 1;
-
 	pr_debug("%s: parent is 0x%p, dir is 0x%p\n",
 	         __func__, parent->base, dir.base);
 
@@ -691,83 +678,6 @@ static int __init nubus_get_board_resource(struct nubus_board *board, int slot,
 	return 0;
 }
 
-/* Attempt to bypass the somewhat non-obvious arrangement of
-   sResources in the motherboard ROM */
-static void __init nubus_find_rom_dir(struct nubus_board* board)
-{
-	unsigned char *rp;
-	unsigned char *romdir;
-	struct nubus_dir dir;
-	struct nubus_dirent ent;
-
-	/* Check for the extra directory just under the format block */
-	rp = board->fblock;
-	nubus_rewind(&rp, 4, board->lanes);
-	if (nubus_get_rom(&rp, 4, board->lanes) != NUBUS_TEST_PATTERN) {
-		/* OK, the ROM was telling the truth */
-		board->directory = board->fblock;
-		nubus_move(&board->directory,
-			   nubus_expand32(board->doffset),
-			   board->lanes);
-		return;
-	}
-
-	/* On "slot zero", you have to walk down a few more
-	   directories to get to the equivalent of a real card's root
-	   directory.  We don't know what they were smoking when they
-	   came up with this. */
-	romdir = nubus_rom_addr(board->slot);
-	nubus_rewind(&romdir, ROM_DIR_OFFSET, board->lanes);
-	dir.base = dir.ptr = romdir;
-	dir.done = 0;
-	dir.mask = board->lanes;
-
-	/* This one points to an "Unknown Macintosh" directory */
-	if (nubus_readdir(&dir, &ent) == -1)
-		goto badrom;
-
-	if (console_loglevel >= CONSOLE_LOGLEVEL_DEBUG)
-		printk(KERN_INFO "nubus_get_rom_dir: entry %02x %06x\n", ent.type, ent.data);
-	/* This one takes us to where we want to go. */
-	if (nubus_readdir(&dir, &ent) == -1)
-		goto badrom;
-	if (console_loglevel >= CONSOLE_LOGLEVEL_DEBUG)
-		printk(KERN_DEBUG "nubus_get_rom_dir: entry %02x %06x\n", ent.type, ent.data);
-	nubus_get_subdir(&ent, &dir);
-
-	/* Resource ID 01, also an "Unknown Macintosh" */
-	if (nubus_readdir(&dir, &ent) == -1)
-		goto badrom;
-	if (console_loglevel >= CONSOLE_LOGLEVEL_DEBUG)
-		printk(KERN_DEBUG "nubus_get_rom_dir: entry %02x %06x\n", ent.type, ent.data);
-
-	/* FIXME: the first one is *not* always the right one.  We
-	   suspect this has something to do with the ROM revision.
-	   "The HORROR ROM" (LC-series) uses 0x7e, while "The HORROR
-	   Continues" (Q630) uses 0x7b.  The DAFB Macs evidently use
-	   something else.  Please run "Slots" on your Mac (see
-	   include/linux/nubus.h for where to get this program) and
-	   tell us where the 'SiDirPtr' for Slot 0 is.  If you feel
-	   brave, you should also use MacsBug to walk down the ROM
-	   directories like this function does and try to find the
-	   path to that address... */
-	if (nubus_readdir(&dir, &ent) == -1)
-		goto badrom;
-	if (console_loglevel >= CONSOLE_LOGLEVEL_DEBUG)
-		printk(KERN_DEBUG "nubus_get_rom_dir: entry %02x %06x\n", ent.type, ent.data);
-
-	/* Bwahahahaha... */
-	nubus_get_subdir(&ent, &dir);
-	board->directory = dir.base;
-	return;
-
-	/* Even more evil laughter... */
- badrom:
-	board->directory = board->fblock;
-	nubus_move(&board->directory, nubus_expand32(board->doffset), board->lanes);
-	printk(KERN_ERR "nubus_get_rom_dir: ROM weirdness!  Notify the developers...\n");
-}
-
 /* Add a board (might be many devices) to the list */
 static struct nubus_board * __init nubus_add_board(int slot, int bytelanes)
 {
@@ -828,8 +738,11 @@ static struct nubus_board * __init nubus_add_board(int slot, int bytelanes)
 	 * since the initial Macintosh ROM releases skipped the check.
 	 */
 
-	/* Attempt to work around slot zero weirdness */
-	nubus_find_rom_dir(board);
+	/* Set up the directory pointer */
+	board->directory = board->fblock;
+	nubus_move(&board->directory, nubus_expand32(board->doffset),
+	           board->lanes);
+
 	nubus_get_root_dir(board, &dir);
 
 	/* We're ready to rock */
@@ -849,9 +762,6 @@ static struct nubus_board * __init nubus_add_board(int slot, int bytelanes)
 		nubus_get_board_resource(board, slot, &ent);
 	}
 
-	/* Aaaarrrrgghh!  The LC III motherboard has *two* board
-	   resources.  I have no idea WTF to do about this. */
-
 	while (nubus_readdir(&dir, &ent) != -1) {
 		struct nubus_dev *dev;
 		struct nubus_dev **devp;
@@ -898,8 +808,6 @@ void __init nubus_probe_slot(int slot)
 			continue;
 
 		dp = *rp;
-		if(dp == 0)
-			continue;
 
 		/* The last byte of the format block consists of two
 		   nybbles which are "mirror images" of each other.
@@ -908,7 +816,7 @@ void __init nubus_probe_slot(int slot)
 			continue;
 		/* Check that this value is actually *on* one of the
 		   bytelanes it claims are valid! */
-		if ((dp & 0x0F) >= (1 << i))
+		if (not_useful(rp, dp))
 			continue;
 
 		/* Looks promising.  Let's put it on the list. */
@@ -922,10 +830,6 @@ void __init nubus_scan_bus(void)
 {
 	int slot;
 
-	/* This might not work on your machine */
-#ifdef I_WANT_TO_PROBE_SLOT_ZERO
-	nubus_probe_slot(0);
-#endif
 	for (slot = 9; slot < 15; slot++) {
 		nubus_probe_slot(slot);
 	}
@@ -943,13 +847,6 @@ static int __init nubus_init(void)
 		via_nubus_init();
 	}
 
-#ifdef TRY_TO_DODGE_WSOD
-	/* Rogue Ethernet interrupts can kill the machine if we don't
-	   do this.  Obviously this is bogus.  Hopefully the local VIA
-	   gurus can fix the real cause of the problem. */
-	mdelay(1000);
-#endif
-
 	/* And probe */
 	pr_info("NuBus: Scanning NuBus slots.\n");
 	nubus_devices = NULL;
diff --git a/drivers/nvdimm/blk.c b/drivers/nvdimm/blk.c
index 822198a75e96..f12d23c49771 100644
--- a/drivers/nvdimm/blk.c
+++ b/drivers/nvdimm/blk.c
@@ -186,7 +186,7 @@ static blk_qc_t nd_blk_make_request(struct request_queue *q, struct bio *bio)
 	 * another kernel subsystem, and we just pass it through.
 	 */
 	if (bio_integrity_enabled(bio) && bio_integrity_prep(bio)) {
-		bio->bi_error = -EIO;
+		bio->bi_status = BLK_STS_IOERR;
 		goto out;
 	}
 
@@ -205,7 +205,7 @@ static blk_qc_t nd_blk_make_request(struct request_queue *q, struct bio *bio)
 					"io error in %s sector %lld, len %d,\n",
 					(rw == READ) ? "READ" : "WRITE",
 					(unsigned long long) iter.bi_sector, len);
-			bio->bi_error = err;
+			bio->bi_status = errno_to_blk_status(err);
 			break;
 		}
 	}
@@ -273,7 +273,6 @@ static int nsblk_attach_disk(struct nd_namespace_blk *nsblk)
 
 	blk_queue_make_request(q, nd_blk_make_request);
 	blk_queue_max_hw_sectors(q, UINT_MAX);
-	blk_queue_bounce_limit(q, BLK_BOUNCE_ANY);
 	blk_queue_logical_block_size(q, nsblk_sector_size(nsblk));
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, q);
 	q->queuedata = nsblk;
diff --git a/drivers/nvdimm/btt.c b/drivers/nvdimm/btt.c
index 983718b8fd9b..b6ba0618ea46 100644
--- a/drivers/nvdimm/btt.c
+++ b/drivers/nvdimm/btt.c
@@ -1210,7 +1210,7 @@ static blk_qc_t btt_make_request(struct request_queue *q, struct bio *bio)
 	 * another kernel subsystem, and we just pass it through.
 	 */
 	if (bio_integrity_enabled(bio) && bio_integrity_prep(bio)) {
-		bio->bi_error = -EIO;
+		bio->bi_status = BLK_STS_IOERR;
 		goto out;
 	}
 
@@ -1232,7 +1232,7 @@ static blk_qc_t btt_make_request(struct request_queue *q, struct bio *bio)
 					(op_is_write(bio_op(bio))) ? "WRITE" :
 					"READ",
 					(unsigned long long) iter.bi_sector, len);
-			bio->bi_error = err;
+			bio->bi_status = errno_to_blk_status(err);
 			break;
 		}
 	}
@@ -1297,7 +1297,6 @@ static int btt_blk_init(struct btt *btt)
 	blk_queue_make_request(btt->btt_queue, btt_make_request);
 	blk_queue_logical_block_size(btt->btt_queue, btt->sector_size);
 	blk_queue_max_hw_sectors(btt->btt_queue, UINT_MAX);
-	blk_queue_bounce_limit(btt->btt_queue, BLK_BOUNCE_ANY);
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, btt->btt_queue);
 	btt->btt_queue->queuedata = btt;
 
diff --git a/drivers/nvdimm/btt_devs.c b/drivers/nvdimm/btt_devs.c
index ae00dc0d9791..4c989bb9a8a0 100644
--- a/drivers/nvdimm/btt_devs.c
+++ b/drivers/nvdimm/btt_devs.c
@@ -222,13 +222,6 @@ struct device *nd_btt_create(struct nd_region *nd_region)
 	return dev;
 }
 
-static bool uuid_is_null(u8 *uuid)
-{
-	static const u8 null_uuid[16];
-
-	return (memcmp(uuid, null_uuid, 16) == 0);
-}
-
 /**
  * nd_btt_arena_is_valid - check if the metadata layout is valid
  * @nd_btt:	device with BTT geometry and backing device info
@@ -249,7 +242,7 @@ bool nd_btt_arena_is_valid(struct nd_btt *nd_btt, struct btt_sb *super)
 	if (memcmp(super->signature, BTT_SIG, BTT_SIG_LEN) != 0)
 		return false;
 
-	if (!uuid_is_null(super->parent_uuid))
+	if (!guid_is_null((guid_t *)&super->parent_uuid))
 		if (memcmp(super->parent_uuid, parent_uuid, 16) != 0)
 			return false;
 
diff --git a/drivers/nvdimm/pmem.c b/drivers/nvdimm/pmem.c
index c544d466ea51..6b577afb1d44 100644
--- a/drivers/nvdimm/pmem.c
+++ b/drivers/nvdimm/pmem.c
@@ -49,19 +49,19 @@ static struct nd_region *to_region(struct pmem_device *pmem)
 	return to_nd_region(to_dev(pmem)->parent);
 }
 
-static int pmem_clear_poison(struct pmem_device *pmem, phys_addr_t offset,
-		unsigned int len)
+static blk_status_t pmem_clear_poison(struct pmem_device *pmem,
+		phys_addr_t offset, unsigned int len)
 {
 	struct device *dev = to_dev(pmem);
 	sector_t sector;
 	long cleared;
-	int rc = 0;
+	blk_status_t rc = BLK_STS_OK;
 
 	sector = (offset - pmem->data_offset) / 512;
 
 	cleared = nvdimm_clear_poison(dev, pmem->phys_addr + offset, len);
 	if (cleared < len)
-		rc = -EIO;
+		rc = BLK_STS_IOERR;
 	if (cleared > 0 && cleared / 512) {
 		cleared /= 512;
 		dev_dbg(dev, "%s: %#llx clear %ld sector%s\n", __func__,
@@ -84,7 +84,7 @@ static void write_pmem(void *pmem_addr, struct page *page,
 	kunmap_atomic(mem);
 }
 
-static int read_pmem(struct page *page, unsigned int off,
+static blk_status_t read_pmem(struct page *page, unsigned int off,
 		void *pmem_addr, unsigned int len)
 {
 	int rc;
@@ -93,15 +93,15 @@ static int read_pmem(struct page *page, unsigned int off,
 	rc = memcpy_mcsafe(mem + off, pmem_addr, len);
 	kunmap_atomic(mem);
 	if (rc)
-		return -EIO;
-	return 0;
+		return BLK_STS_IOERR;
+	return BLK_STS_OK;
 }
 
-static int pmem_do_bvec(struct pmem_device *pmem, struct page *page,
+static blk_status_t pmem_do_bvec(struct pmem_device *pmem, struct page *page,
 			unsigned int len, unsigned int off, bool is_write,
 			sector_t sector)
 {
-	int rc = 0;
+	blk_status_t rc = BLK_STS_OK;
 	bool bad_pmem = false;
 	phys_addr_t pmem_off = sector * 512 + pmem->data_offset;
 	void *pmem_addr = pmem->virt_addr + pmem_off;
@@ -111,7 +111,7 @@ static int pmem_do_bvec(struct pmem_device *pmem, struct page *page,
 
 	if (!is_write) {
 		if (unlikely(bad_pmem))
-			rc = -EIO;
+			rc = BLK_STS_IOERR;
 		else {
 			rc = read_pmem(page, off, pmem_addr, len);
 			flush_dcache_page(page);
@@ -149,7 +149,7 @@ static int pmem_do_bvec(struct pmem_device *pmem, struct page *page,
 
 static blk_qc_t pmem_make_request(struct request_queue *q, struct bio *bio)
 {
-	int rc = 0;
+	blk_status_t rc = 0;
 	bool do_acct;
 	unsigned long start;
 	struct bio_vec bvec;
@@ -166,7 +166,7 @@ static blk_qc_t pmem_make_request(struct request_queue *q, struct bio *bio)
 				bvec.bv_offset, op_is_write(bio_op(bio)),
 				iter.bi_sector);
 		if (rc) {
-			bio->bi_error = rc;
+			bio->bi_status = rc;
 			break;
 		}
 	}
@@ -184,7 +184,7 @@ static int pmem_rw_page(struct block_device *bdev, sector_t sector,
 		       struct page *page, bool is_write)
 {
 	struct pmem_device *pmem = bdev->bd_queue->queuedata;
-	int rc;
+	blk_status_t rc;
 
 	rc = pmem_do_bvec(pmem, page, PAGE_SIZE, 0, is_write, sector);
 
@@ -197,7 +197,7 @@ static int pmem_rw_page(struct block_device *bdev, sector_t sector,
 	if (rc == 0)
 		page_endio(page, is_write, 0);
 
-	return rc;
+	return blk_status_to_errno(rc);
 }
 
 /* see "strong" declaration in tools/testing/nvdimm/pmem-dax.c */
@@ -343,7 +343,6 @@ static int pmem_attach_disk(struct device *dev,
 	blk_queue_make_request(q, pmem_make_request);
 	blk_queue_physical_block_size(q, PAGE_SIZE);
 	blk_queue_max_hw_sectors(q, UINT_MAX);
-	blk_queue_bounce_limit(q, BLK_BOUNCE_ANY);
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, q);
 	queue_flag_set_unlocked(QUEUE_FLAG_DAX, q);
 	q->queuedata = pmem;
diff --git a/drivers/nvme/host/Kconfig b/drivers/nvme/host/Kconfig
index 90745a616df7..46d6cb1e03bd 100644
--- a/drivers/nvme/host/Kconfig
+++ b/drivers/nvme/host/Kconfig
@@ -13,18 +13,6 @@ config BLK_DEV_NVME
 	  To compile this driver as a module, choose M here: the
 	  module will be called nvme.
 
-config BLK_DEV_NVME_SCSI
-	bool "SCSI emulation for NVMe device nodes"
-	depends on NVME_CORE
-	---help---
-	  This adds support for the SG_IO ioctl on the NVMe character
-	  and block devices nodes, as well as a translation for a small
-	  number of selected SCSI commands to NVMe commands to the NVMe
-	  driver.  If you don't know what this means you probably want
-	  to say N here, unless you run a distro that abuses the SCSI
-	  emulation to provide stable device names for mount by id, like
-	  some OpenSuSE and SLES versions.
-
 config NVME_FABRICS
 	tristate
 
diff --git a/drivers/nvme/host/Makefile b/drivers/nvme/host/Makefile
index f1a7d945fbb6..cc0aacb4c8b4 100644
--- a/drivers/nvme/host/Makefile
+++ b/drivers/nvme/host/Makefile
@@ -5,7 +5,6 @@ obj-$(CONFIG_NVME_RDMA)			+= nvme-rdma.o
 obj-$(CONFIG_NVME_FC)			+= nvme-fc.o
 
 nvme-core-y				:= core.o
-nvme-core-$(CONFIG_BLK_DEV_NVME_SCSI)	+= scsi.o
 nvme-core-$(CONFIG_NVM)			+= lightnvm.o
 
 nvme-y					+= pci.o
diff --git a/drivers/nvme/host/core.c b/drivers/nvme/host/core.c
index 903d5813023a..d70df1d0072d 100644
--- a/drivers/nvme/host/core.c
+++ b/drivers/nvme/host/core.c
@@ -27,7 +27,6 @@
 #include <linux/nvme_ioctl.h>
 #include <linux/t10-pi.h>
 #include <linux/pm_qos.h>
-#include <scsi/sg.h>
 #include <asm/unaligned.h>
 
 #include "nvme.h"
@@ -45,7 +44,7 @@ module_param_named(io_timeout, nvme_io_timeout, byte, 0644);
 MODULE_PARM_DESC(io_timeout, "timeout in seconds for I/O");
 EXPORT_SYMBOL_GPL(nvme_io_timeout);
 
-unsigned char shutdown_timeout = 5;
+static unsigned char shutdown_timeout = 5;
 module_param(shutdown_timeout, byte, 0644);
 MODULE_PARM_DESC(shutdown_timeout, "timeout in seconds for controller shutdown");
 
@@ -65,34 +64,53 @@ static bool force_apst;
 module_param(force_apst, bool, 0644);
 MODULE_PARM_DESC(force_apst, "allow APST for newly enumerated devices even if quirked off");
 
+static bool streams;
+module_param(streams, bool, 0644);
+MODULE_PARM_DESC(streams, "turn on support for Streams write directives");
+
+struct workqueue_struct *nvme_wq;
+EXPORT_SYMBOL_GPL(nvme_wq);
+
 static LIST_HEAD(nvme_ctrl_list);
 static DEFINE_SPINLOCK(dev_list_lock);
 
 static struct class *nvme_class;
 
-static int nvme_error_status(struct request *req)
+int nvme_reset_ctrl(struct nvme_ctrl *ctrl)
+{
+	if (!nvme_change_ctrl_state(ctrl, NVME_CTRL_RESETTING))
+		return -EBUSY;
+	if (!queue_work(nvme_wq, &ctrl->reset_work))
+		return -EBUSY;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(nvme_reset_ctrl);
+
+static int nvme_reset_ctrl_sync(struct nvme_ctrl *ctrl)
+{
+	int ret;
+
+	ret = nvme_reset_ctrl(ctrl);
+	if (!ret)
+		flush_work(&ctrl->reset_work);
+	return ret;
+}
+
+static blk_status_t nvme_error_status(struct request *req)
 {
 	switch (nvme_req(req)->status & 0x7ff) {
 	case NVME_SC_SUCCESS:
-		return 0;
+		return BLK_STS_OK;
 	case NVME_SC_CAP_EXCEEDED:
-		return -ENOSPC;
-	default:
-		return -EIO;
-
-	/*
-	 * XXX: these errors are a nasty side-band protocol to
-	 * drivers/md/dm-mpath.c:noretry_error() that aren't documented
-	 * anywhere..
-	 */
-	case NVME_SC_CMD_SEQ_ERROR:
-		return -EILSEQ;
+		return BLK_STS_NOSPC;
 	case NVME_SC_ONCS_NOT_SUPPORTED:
-		return -EOPNOTSUPP;
+		return BLK_STS_NOTSUPP;
 	case NVME_SC_WRITE_FAULT:
 	case NVME_SC_READ_ERROR:
 	case NVME_SC_UNWRITTEN_BLOCK:
-		return -ENODATA;
+		return BLK_STS_MEDIUM;
+	default:
+		return BLK_STS_IOERR;
 	}
 }
 
@@ -165,7 +183,6 @@ bool nvme_change_ctrl_state(struct nvme_ctrl *ctrl,
 		switch (old_state) {
 		case NVME_CTRL_NEW:
 		case NVME_CTRL_LIVE:
-		case NVME_CTRL_RECONNECTING:
 			changed = true;
 			/* FALLTHRU */
 		default:
@@ -283,6 +300,105 @@ struct request *nvme_alloc_request(struct request_queue *q,
 }
 EXPORT_SYMBOL_GPL(nvme_alloc_request);
 
+static int nvme_toggle_streams(struct nvme_ctrl *ctrl, bool enable)
+{
+	struct nvme_command c;
+
+	memset(&c, 0, sizeof(c));
+
+	c.directive.opcode = nvme_admin_directive_send;
+	c.directive.nsid = cpu_to_le32(0xffffffff);
+	c.directive.doper = NVME_DIR_SND_ID_OP_ENABLE;
+	c.directive.dtype = NVME_DIR_IDENTIFY;
+	c.directive.tdtype = NVME_DIR_STREAMS;
+	c.directive.endir = enable ? NVME_DIR_ENDIR : 0;
+
+	return nvme_submit_sync_cmd(ctrl->admin_q, &c, NULL, 0);
+}
+
+static int nvme_disable_streams(struct nvme_ctrl *ctrl)
+{
+	return nvme_toggle_streams(ctrl, false);
+}
+
+static int nvme_enable_streams(struct nvme_ctrl *ctrl)
+{
+	return nvme_toggle_streams(ctrl, true);
+}
+
+static int nvme_get_stream_params(struct nvme_ctrl *ctrl,
+				  struct streams_directive_params *s, u32 nsid)
+{
+	struct nvme_command c;
+
+	memset(&c, 0, sizeof(c));
+	memset(s, 0, sizeof(*s));
+
+	c.directive.opcode = nvme_admin_directive_recv;
+	c.directive.nsid = cpu_to_le32(nsid);
+	c.directive.numd = sizeof(*s);
+	c.directive.doper = NVME_DIR_RCV_ST_OP_PARAM;
+	c.directive.dtype = NVME_DIR_STREAMS;
+
+	return nvme_submit_sync_cmd(ctrl->admin_q, &c, s, sizeof(*s));
+}
+
+static int nvme_configure_directives(struct nvme_ctrl *ctrl)
+{
+	struct streams_directive_params s;
+	int ret;
+
+	if (!(ctrl->oacs & NVME_CTRL_OACS_DIRECTIVES))
+		return 0;
+	if (!streams)
+		return 0;
+
+	ret = nvme_enable_streams(ctrl);
+	if (ret)
+		return ret;
+
+	ret = nvme_get_stream_params(ctrl, &s, 0xffffffff);
+	if (ret)
+		return ret;
+
+	ctrl->nssa = le16_to_cpu(s.nssa);
+	if (ctrl->nssa < BLK_MAX_WRITE_HINTS - 1) {
+		dev_info(ctrl->device, "too few streams (%u) available\n",
+					ctrl->nssa);
+		nvme_disable_streams(ctrl);
+		return 0;
+	}
+
+	ctrl->nr_streams = min_t(unsigned, ctrl->nssa, BLK_MAX_WRITE_HINTS - 1);
+	dev_info(ctrl->device, "Using %u streams\n", ctrl->nr_streams);
+	return 0;
+}
+
+/*
+ * Check if 'req' has a write hint associated with it. If it does, assign
+ * a valid namespace stream to the write.
+ */
+static void nvme_assign_write_stream(struct nvme_ctrl *ctrl,
+				     struct request *req, u16 *control,
+				     u32 *dsmgmt)
+{
+	enum rw_hint streamid = req->write_hint;
+
+	if (streamid == WRITE_LIFE_NOT_SET || streamid == WRITE_LIFE_NONE)
+		streamid = 0;
+	else {
+		streamid--;
+		if (WARN_ON_ONCE(streamid > ctrl->nr_streams))
+			return;
+
+		*control |= NVME_RW_DTYPE_STREAMS;
+		*dsmgmt |= streamid << 16;
+	}
+
+	if (streamid < ARRAY_SIZE(req->q->write_hints))
+		req->q->write_hints[streamid] += blk_rq_bytes(req) >> 9;
+}
+
 static inline void nvme_setup_flush(struct nvme_ns *ns,
 		struct nvme_command *cmnd)
 {
@@ -291,7 +407,7 @@ static inline void nvme_setup_flush(struct nvme_ns *ns,
 	cmnd->common.nsid = cpu_to_le32(ns->ns_id);
 }
 
-static inline int nvme_setup_discard(struct nvme_ns *ns, struct request *req,
+static blk_status_t nvme_setup_discard(struct nvme_ns *ns, struct request *req,
 		struct nvme_command *cmnd)
 {
 	unsigned short segments = blk_rq_nr_discard_segments(req), n = 0;
@@ -300,7 +416,7 @@ static inline int nvme_setup_discard(struct nvme_ns *ns, struct request *req,
 
 	range = kmalloc_array(segments, sizeof(*range), GFP_ATOMIC);
 	if (!range)
-		return BLK_MQ_RQ_QUEUE_BUSY;
+		return BLK_STS_RESOURCE;
 
 	__rq_for_each_bio(bio, req) {
 		u64 slba = nvme_block_nr(ns, bio->bi_iter.bi_sector);
@@ -314,7 +430,7 @@ static inline int nvme_setup_discard(struct nvme_ns *ns, struct request *req,
 
 	if (WARN_ON_ONCE(n != segments)) {
 		kfree(range);
-		return BLK_MQ_RQ_QUEUE_ERROR;
+		return BLK_STS_IOERR;
 	}
 
 	memset(cmnd, 0, sizeof(*cmnd));
@@ -328,15 +444,26 @@ static inline int nvme_setup_discard(struct nvme_ns *ns, struct request *req,
 	req->special_vec.bv_len = sizeof(*range) * segments;
 	req->rq_flags |= RQF_SPECIAL_PAYLOAD;
 
-	return BLK_MQ_RQ_QUEUE_OK;
+	return BLK_STS_OK;
 }
 
-static inline void nvme_setup_rw(struct nvme_ns *ns, struct request *req,
-		struct nvme_command *cmnd)
+static inline blk_status_t nvme_setup_rw(struct nvme_ns *ns,
+		struct request *req, struct nvme_command *cmnd)
 {
+	struct nvme_ctrl *ctrl = ns->ctrl;
 	u16 control = 0;
 	u32 dsmgmt = 0;
 
+	/*
+	 * If formated with metadata, require the block layer provide a buffer
+	 * unless this namespace is formated such that the metadata can be
+	 * stripped/generated by the controller with PRACT=1.
+	 */
+	if (ns && ns->ms &&
+	    (!ns->pi_type || ns->ms != sizeof(struct t10_pi_tuple)) &&
+	    !blk_integrity_rq(req) && !blk_rq_is_passthrough(req))
+		return BLK_STS_NOTSUPP;
+
 	if (req->cmd_flags & REQ_FUA)
 		control |= NVME_RW_FUA;
 	if (req->cmd_flags & (REQ_FAILFAST_DEV | REQ_RAHEAD))
@@ -351,6 +478,9 @@ static inline void nvme_setup_rw(struct nvme_ns *ns, struct request *req,
 	cmnd->rw.slba = cpu_to_le64(nvme_block_nr(ns, blk_rq_pos(req)));
 	cmnd->rw.length = cpu_to_le16((blk_rq_bytes(req) >> ns->lba_shift) - 1);
 
+	if (req_op(req) == REQ_OP_WRITE && ctrl->nr_streams)
+		nvme_assign_write_stream(ctrl, req, &control, &dsmgmt);
+
 	if (ns->ms) {
 		switch (ns->pi_type) {
 		case NVME_NS_DPS_PI_TYPE3:
@@ -370,12 +500,13 @@ static inline void nvme_setup_rw(struct nvme_ns *ns, struct request *req,
 
 	cmnd->rw.control = cpu_to_le16(control);
 	cmnd->rw.dsmgmt = cpu_to_le32(dsmgmt);
+	return 0;
 }
 
-int nvme_setup_cmd(struct nvme_ns *ns, struct request *req,
+blk_status_t nvme_setup_cmd(struct nvme_ns *ns, struct request *req,
 		struct nvme_command *cmd)
 {
-	int ret = BLK_MQ_RQ_QUEUE_OK;
+	blk_status_t ret = BLK_STS_OK;
 
 	if (!(req->rq_flags & RQF_DONTPREP)) {
 		nvme_req(req)->retries = 0;
@@ -398,11 +529,11 @@ int nvme_setup_cmd(struct nvme_ns *ns, struct request *req,
 		break;
 	case REQ_OP_READ:
 	case REQ_OP_WRITE:
-		nvme_setup_rw(ns, req, cmd);
+		ret = nvme_setup_rw(ns, req, cmd);
 		break;
 	default:
 		WARN_ON_ONCE(1);
-		return BLK_MQ_RQ_QUEUE_ERROR;
+		return BLK_STS_IOERR;
 	}
 
 	cmd->common.command_id = req->tag;
@@ -555,15 +686,16 @@ int nvme_submit_user_cmd(struct request_queue *q, struct nvme_command *cmd,
 			result, timeout);
 }
 
-static void nvme_keep_alive_end_io(struct request *rq, int error)
+static void nvme_keep_alive_end_io(struct request *rq, blk_status_t status)
 {
 	struct nvme_ctrl *ctrl = rq->end_io_data;
 
 	blk_mq_free_request(rq);
 
-	if (error) {
+	if (status) {
 		dev_err(ctrl->device,
-			"failed nvme_keep_alive_end_io error=%d\n", error);
+			"failed nvme_keep_alive_end_io error=%d\n",
+				status);
 		return;
 	}
 
@@ -599,7 +731,7 @@ static void nvme_keep_alive_work(struct work_struct *work)
 	if (nvme_keep_alive(ctrl)) {
 		/* allocation failure, reset the controller */
 		dev_err(ctrl->device, "keep-alive failed\n");
-		ctrl->ops->reset_ctrl(ctrl);
+		nvme_reset_ctrl(ctrl);
 		return;
 	}
 }
@@ -623,7 +755,7 @@ void nvme_stop_keep_alive(struct nvme_ctrl *ctrl)
 }
 EXPORT_SYMBOL_GPL(nvme_stop_keep_alive);
 
-int nvme_identify_ctrl(struct nvme_ctrl *dev, struct nvme_id_ctrl **id)
+static int nvme_identify_ctrl(struct nvme_ctrl *dev, struct nvme_id_ctrl **id)
 {
 	struct nvme_command c = { };
 	int error;
@@ -643,6 +775,77 @@ int nvme_identify_ctrl(struct nvme_ctrl *dev, struct nvme_id_ctrl **id)
 	return error;
 }
 
+static int nvme_identify_ns_descs(struct nvme_ns *ns, unsigned nsid)
+{
+	struct nvme_command c = { };
+	int status;
+	void *data;
+	int pos;
+	int len;
+
+	c.identify.opcode = nvme_admin_identify;
+	c.identify.nsid = cpu_to_le32(nsid);
+	c.identify.cns = NVME_ID_CNS_NS_DESC_LIST;
+
+	data = kzalloc(NVME_IDENTIFY_DATA_SIZE, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	status = nvme_submit_sync_cmd(ns->ctrl->admin_q, &c, data,
+				      NVME_IDENTIFY_DATA_SIZE);
+	if (status)
+		goto free_data;
+
+	for (pos = 0; pos < NVME_IDENTIFY_DATA_SIZE; pos += len) {
+		struct nvme_ns_id_desc *cur = data + pos;
+
+		if (cur->nidl == 0)
+			break;
+
+		switch (cur->nidt) {
+		case NVME_NIDT_EUI64:
+			if (cur->nidl != NVME_NIDT_EUI64_LEN) {
+				dev_warn(ns->ctrl->device,
+					 "ctrl returned bogus length: %d for NVME_NIDT_EUI64\n",
+					 cur->nidl);
+				goto free_data;
+			}
+			len = NVME_NIDT_EUI64_LEN;
+			memcpy(ns->eui, data + pos + sizeof(*cur), len);
+			break;
+		case NVME_NIDT_NGUID:
+			if (cur->nidl != NVME_NIDT_NGUID_LEN) {
+				dev_warn(ns->ctrl->device,
+					 "ctrl returned bogus length: %d for NVME_NIDT_NGUID\n",
+					 cur->nidl);
+				goto free_data;
+			}
+			len = NVME_NIDT_NGUID_LEN;
+			memcpy(ns->nguid, data + pos + sizeof(*cur), len);
+			break;
+		case NVME_NIDT_UUID:
+			if (cur->nidl != NVME_NIDT_UUID_LEN) {
+				dev_warn(ns->ctrl->device,
+					 "ctrl returned bogus length: %d for NVME_NIDT_UUID\n",
+					 cur->nidl);
+				goto free_data;
+			}
+			len = NVME_NIDT_UUID_LEN;
+			uuid_copy(&ns->uuid, data + pos + sizeof(*cur));
+			break;
+		default:
+			/* Skip unnkown types */
+			len = cur->nidl;
+			break;
+		}
+
+		len += sizeof(*cur);
+	}
+free_data:
+	kfree(data);
+	return status;
+}
+
 static int nvme_identify_ns_list(struct nvme_ctrl *dev, unsigned nsid, __le32 *ns_list)
 {
 	struct nvme_command c = { };
@@ -653,7 +856,7 @@ static int nvme_identify_ns_list(struct nvme_ctrl *dev, unsigned nsid, __le32 *n
 	return nvme_submit_sync_cmd(dev->admin_q, &c, ns_list, 0x1000);
 }
 
-int nvme_identify_ns(struct nvme_ctrl *dev, unsigned nsid,
+static int nvme_identify_ns(struct nvme_ctrl *dev, unsigned nsid,
 		struct nvme_id_ns **id)
 {
 	struct nvme_command c = { };
@@ -675,26 +878,7 @@ int nvme_identify_ns(struct nvme_ctrl *dev, unsigned nsid,
 	return error;
 }
 
-int nvme_get_features(struct nvme_ctrl *dev, unsigned fid, unsigned nsid,
-		      void *buffer, size_t buflen, u32 *result)
-{
-	struct nvme_command c;
-	union nvme_result res;
-	int ret;
-
-	memset(&c, 0, sizeof(c));
-	c.features.opcode = nvme_admin_get_features;
-	c.features.nsid = cpu_to_le32(nsid);
-	c.features.fid = cpu_to_le32(fid);
-
-	ret = __nvme_submit_sync_cmd(dev->admin_q, &c, &res, buffer, buflen, 0,
-			NVME_QID_ANY, 0, 0);
-	if (ret >= 0 && result)
-		*result = le32_to_cpu(res.u32);
-	return ret;
-}
-
-int nvme_set_features(struct nvme_ctrl *dev, unsigned fid, unsigned dword11,
+static int nvme_set_features(struct nvme_ctrl *dev, unsigned fid, unsigned dword11,
 		      void *buffer, size_t buflen, u32 *result)
 {
 	struct nvme_command c;
@@ -713,28 +897,6 @@ int nvme_set_features(struct nvme_ctrl *dev, unsigned fid, unsigned dword11,
 	return ret;
 }
 
-int nvme_get_log_page(struct nvme_ctrl *dev, struct nvme_smart_log **log)
-{
-	struct nvme_command c = { };
-	int error;
-
-	c.common.opcode = nvme_admin_get_log_page,
-	c.common.nsid = cpu_to_le32(0xFFFFFFFF),
-	c.common.cdw10[0] = cpu_to_le32(
-			(((sizeof(struct nvme_smart_log) / 4) - 1) << 16) |
-			 NVME_LOG_SMART),
-
-	*log = kmalloc(sizeof(struct nvme_smart_log), GFP_KERNEL);
-	if (!*log)
-		return -ENOMEM;
-
-	error = nvme_submit_sync_cmd(dev->admin_q, &c, *log,
-			sizeof(struct nvme_smart_log));
-	if (error)
-		kfree(*log);
-	return error;
-}
-
 int nvme_set_queue_count(struct nvme_ctrl *ctrl, int *count)
 {
 	u32 q_count = (*count - 1) | ((*count - 1) << 16);
@@ -752,7 +914,7 @@ int nvme_set_queue_count(struct nvme_ctrl *ctrl, int *count)
 	 * access to the admin queue, as that might be only way to fix them up.
 	 */
 	if (status > 0) {
-		dev_err(ctrl->dev, "Could not set queue count (%d)\n", status);
+		dev_err(ctrl->device, "Could not set queue count (%d)\n", status);
 		*count = 0;
 	} else {
 		nr_io_queues = min(result & 0xffff, result >> 16) + 1;
@@ -870,12 +1032,6 @@ static int nvme_ioctl(struct block_device *bdev, fmode_t mode,
 		return nvme_user_cmd(ns->ctrl, ns, (void __user *)arg);
 	case NVME_IOCTL_SUBMIT_IO:
 		return nvme_submit_io(ns, (void __user *)arg);
-#ifdef CONFIG_BLK_DEV_NVME_SCSI
-	case SG_GET_VERSION_NUM:
-		return nvme_sg_get_version_num((void __user *)arg);
-	case SG_IO:
-		return nvme_sg_io(ns, (void __user *)arg);
-#endif
 	default:
 #ifdef CONFIG_NVM
 		if (ns->ndev)
@@ -892,10 +1048,6 @@ static int nvme_ioctl(struct block_device *bdev, fmode_t mode,
 static int nvme_compat_ioctl(struct block_device *bdev, fmode_t mode,
 			unsigned int cmd, unsigned long arg)
 {
-	switch (cmd) {
-	case SG_IO:
-		return -ENOIOCTLCMD;
-	}
 	return nvme_ioctl(bdev, mode, cmd, arg);
 }
 #else
@@ -983,6 +1135,12 @@ static void nvme_init_integrity(struct nvme_ns *ns)
 }
 #endif /* CONFIG_BLK_DEV_INTEGRITY */
 
+static void nvme_set_chunk_size(struct nvme_ns *ns)
+{
+	u32 chunk_size = (((u32)ns->noiob) << (ns->lba_shift - 9));
+	blk_queue_chunk_sectors(ns->queue, rounddown_pow_of_two(chunk_size));
+}
+
 static void nvme_config_discard(struct nvme_ns *ns)
 {
 	struct nvme_ctrl *ctrl = ns->ctrl;
@@ -991,8 +1149,15 @@ static void nvme_config_discard(struct nvme_ns *ns)
 	BUILD_BUG_ON(PAGE_SIZE / sizeof(struct nvme_dsm_range) <
 			NVME_DSM_MAX_RANGES);
 
-	ns->queue->limits.discard_alignment = logical_block_size;
-	ns->queue->limits.discard_granularity = logical_block_size;
+	if (ctrl->nr_streams && ns->sws && ns->sgs) {
+		unsigned int sz = logical_block_size * ns->sws * ns->sgs;
+
+		ns->queue->limits.discard_alignment = sz;
+		ns->queue->limits.discard_granularity = sz;
+	} else {
+		ns->queue->limits.discard_alignment = logical_block_size;
+		ns->queue->limits.discard_granularity = logical_block_size;
+	}
 	blk_queue_max_discard_sectors(ns->queue, UINT_MAX);
 	blk_queue_max_discard_segments(ns->queue, NVME_DSM_MAX_RANGES);
 	queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, ns->queue);
@@ -1016,7 +1181,15 @@ static int nvme_revalidate_ns(struct nvme_ns *ns, struct nvme_id_ns **id)
 	if (ns->ctrl->vs >= NVME_VS(1, 1, 0))
 		memcpy(ns->eui, (*id)->eui64, sizeof(ns->eui));
 	if (ns->ctrl->vs >= NVME_VS(1, 2, 0))
-		memcpy(ns->uuid, (*id)->nguid, sizeof(ns->uuid));
+		memcpy(ns->nguid, (*id)->nguid, sizeof(ns->nguid));
+	if (ns->ctrl->vs >= NVME_VS(1, 3, 0)) {
+		 /* Don't treat error as fatal we potentially
+		  * already have a NGUID or EUI-64
+		  */
+		if (nvme_identify_ns_descs(ns, ns->ns_id))
+			dev_warn(ns->ctrl->device,
+				 "%s: Identify Descriptors failed\n", __func__);
+	}
 
 	return 0;
 }
@@ -1024,6 +1197,7 @@ static int nvme_revalidate_ns(struct nvme_ns *ns, struct nvme_id_ns **id)
 static void __nvme_revalidate_disk(struct gendisk *disk, struct nvme_id_ns *id)
 {
 	struct nvme_ns *ns = disk->private_data;
+	struct nvme_ctrl *ctrl = ns->ctrl;
 	u16 bs;
 
 	/*
@@ -1034,12 +1208,15 @@ static void __nvme_revalidate_disk(struct gendisk *disk, struct nvme_id_ns *id)
 	if (ns->lba_shift == 0)
 		ns->lba_shift = 9;
 	bs = 1 << ns->lba_shift;
+	ns->noiob = le16_to_cpu(id->noiob);
 
 	blk_mq_freeze_queue(disk->queue);
 
-	if (ns->ctrl->ops->flags & NVME_F_METADATA_SUPPORTED)
+	if (ctrl->ops->flags & NVME_F_METADATA_SUPPORTED)
 		nvme_prep_integrity(disk, id, bs);
 	blk_queue_logical_block_size(ns->queue, bs);
+	if (ns->noiob)
+		nvme_set_chunk_size(ns);
 	if (ns->ms && !blk_get_integrity(disk) && !ns->ext)
 		nvme_init_integrity(ns);
 	if (ns->ms && !(ns->ms == 8 && ns->pi_type) && !blk_get_integrity(disk))
@@ -1047,7 +1224,7 @@ static void __nvme_revalidate_disk(struct gendisk *disk, struct nvme_id_ns *id)
 	else
 		set_capacity(disk, le64_to_cpup(&id->nsze) << (ns->lba_shift - 9));
 
-	if (ns->ctrl->oncs & NVME_CTRL_ONCS_DSM)
+	if (ctrl->oncs & NVME_CTRL_ONCS_DSM)
 		nvme_config_discard(ns);
 	blk_mq_unfreeze_queue(disk->queue);
 }
@@ -1283,7 +1460,7 @@ EXPORT_SYMBOL_GPL(nvme_enable_ctrl);
 
 int nvme_shutdown_ctrl(struct nvme_ctrl *ctrl)
 {
-	unsigned long timeout = SHUTDOWN_TIMEOUT + jiffies;
+	unsigned long timeout = jiffies + (shutdown_timeout * HZ);
 	u32 csts;
 	int ret;
 
@@ -1372,7 +1549,7 @@ static void nvme_configure_apst(struct nvme_ctrl *ctrl)
 	if (!table)
 		return;
 
-	if (ctrl->ps_max_latency_us == 0) {
+	if (!ctrl->apst_enabled || ctrl->ps_max_latency_us == 0) {
 		/* Turn off APST. */
 		apste = 0;
 		dev_dbg(ctrl->device, "APST disabled\n");
@@ -1528,6 +1705,31 @@ static bool quirk_matches(const struct nvme_id_ctrl *id,
 		string_matches(id->fr, q->fr, sizeof(id->fr));
 }
 
+static void nvme_init_subnqn(struct nvme_ctrl *ctrl, struct nvme_id_ctrl *id)
+{
+	size_t nqnlen;
+	int off;
+
+	nqnlen = strnlen(id->subnqn, NVMF_NQN_SIZE);
+	if (nqnlen > 0 && nqnlen < NVMF_NQN_SIZE) {
+		strcpy(ctrl->subnqn, id->subnqn);
+		return;
+	}
+
+	if (ctrl->vs >= NVME_VS(1, 2, 1))
+		dev_warn(ctrl->device, "missing or invalid SUBNQN field.\n");
+
+	/* Generate a "fake" NQN per Figure 254 in NVMe 1.3 + ECN 001 */
+	off = snprintf(ctrl->subnqn, NVMF_NQN_SIZE,
+			"nqn.2014.08.org.nvmexpress:%4x%4x",
+			le16_to_cpu(id->vid), le16_to_cpu(id->ssvid));
+	memcpy(ctrl->subnqn + off, id->sn, sizeof(id->sn));
+	off += sizeof(id->sn);
+	memcpy(ctrl->subnqn + off, id->mn, sizeof(id->mn));
+	off += sizeof(id->mn);
+	memset(ctrl->subnqn + off, 0, sizeof(ctrl->subnqn) - off);
+}
+
 /*
  * Initialize the cached copies of the Identify data and various controller
  * register in our nvme_ctrl structure.  This should be called as soon as
@@ -1539,7 +1741,7 @@ int nvme_init_identify(struct nvme_ctrl *ctrl)
 	u64 cap;
 	int ret, page_shift;
 	u32 max_hw_sectors;
-	u8 prev_apsta;
+	bool prev_apst_enabled;
 
 	ret = ctrl->ops->reg_read32(ctrl, NVME_REG_VS, &ctrl->vs);
 	if (ret) {
@@ -1563,6 +1765,8 @@ int nvme_init_identify(struct nvme_ctrl *ctrl)
 		return -EIO;
 	}
 
+	nvme_init_subnqn(ctrl, id);
+
 	if (!ctrl->identified) {
 		/*
 		 * Check for quirks.  Quirk can depend on firmware version,
@@ -1582,7 +1786,7 @@ int nvme_init_identify(struct nvme_ctrl *ctrl)
 	}
 
 	if (force_apst && (ctrl->quirks & NVME_QUIRK_NO_DEEPEST_PS)) {
-		dev_warn(ctrl->dev, "forcibly allowing all power states due to nvme_core.force_apst -- use at your own risk\n");
+		dev_warn(ctrl->device, "forcibly allowing all power states due to nvme_core.force_apst -- use at your own risk\n");
 		ctrl->quirks &= ~NVME_QUIRK_NO_DEEPEST_PS;
 	}
 
@@ -1607,16 +1811,17 @@ int nvme_init_identify(struct nvme_ctrl *ctrl)
 	ctrl->kas = le16_to_cpu(id->kas);
 
 	ctrl->npss = id->npss;
-	prev_apsta = ctrl->apsta;
+	ctrl->apsta = id->apsta;
+	prev_apst_enabled = ctrl->apst_enabled;
 	if (ctrl->quirks & NVME_QUIRK_NO_APST) {
 		if (force_apst && id->apsta) {
-			dev_warn(ctrl->dev, "forcibly allowing APST due to nvme_core.force_apst -- use at your own risk\n");
-			ctrl->apsta = 1;
+			dev_warn(ctrl->device, "forcibly allowing APST due to nvme_core.force_apst -- use at your own risk\n");
+			ctrl->apst_enabled = true;
 		} else {
-			ctrl->apsta = 0;
+			ctrl->apst_enabled = false;
 		}
 	} else {
-		ctrl->apsta = id->apsta;
+		ctrl->apst_enabled = id->apsta;
 	}
 	memcpy(ctrl->psd, id->psd, sizeof(ctrl->psd));
 
@@ -1634,22 +1839,25 @@ int nvme_init_identify(struct nvme_ctrl *ctrl)
 			ret = -EINVAL;
 
 		if (!ctrl->opts->discovery_nqn && !ctrl->kas) {
-			dev_err(ctrl->dev,
+			dev_err(ctrl->device,
 				"keep-alive support is mandatory for fabrics\n");
 			ret = -EINVAL;
 		}
 	} else {
 		ctrl->cntlid = le16_to_cpu(id->cntlid);
+		ctrl->hmpre = le32_to_cpu(id->hmpre);
+		ctrl->hmmin = le32_to_cpu(id->hmmin);
 	}
 
 	kfree(id);
 
-	if (ctrl->apsta && !prev_apsta)
+	if (ctrl->apst_enabled && !prev_apst_enabled)
 		dev_pm_qos_expose_latency_tolerance(ctrl->device);
-	else if (!ctrl->apsta && prev_apsta)
+	else if (!ctrl->apst_enabled && prev_apst_enabled)
 		dev_pm_qos_hide_latency_tolerance(ctrl->device);
 
 	nvme_configure_apst(ctrl);
+	nvme_configure_directives(ctrl);
 
 	ctrl->identified = true;
 
@@ -1735,7 +1943,7 @@ static long nvme_dev_ioctl(struct file *file, unsigned int cmd,
 		return nvme_dev_user_cmd(ctrl, argp);
 	case NVME_IOCTL_RESET:
 		dev_warn(ctrl->device, "resetting controller\n");
-		return ctrl->ops->reset_ctrl(ctrl);
+		return nvme_reset_ctrl_sync(ctrl);
 	case NVME_IOCTL_SUBSYS_RESET:
 		return nvme_reset_subsystem(ctrl);
 	case NVME_IOCTL_RESCAN:
@@ -1761,7 +1969,7 @@ static ssize_t nvme_sysfs_reset(struct device *dev,
 	struct nvme_ctrl *ctrl = dev_get_drvdata(dev);
 	int ret;
 
-	ret = ctrl->ops->reset_ctrl(ctrl);
+	ret = nvme_reset_ctrl_sync(ctrl);
 	if (ret < 0)
 		return ret;
 	return count;
@@ -1787,8 +1995,8 @@ static ssize_t wwid_show(struct device *dev, struct device_attribute *attr,
 	int serial_len = sizeof(ctrl->serial);
 	int model_len = sizeof(ctrl->model);
 
-	if (memchr_inv(ns->uuid, 0, sizeof(ns->uuid)))
-		return sprintf(buf, "eui.%16phN\n", ns->uuid);
+	if (memchr_inv(ns->nguid, 0, sizeof(ns->nguid)))
+		return sprintf(buf, "eui.%16phN\n", ns->nguid);
 
 	if (memchr_inv(ns->eui, 0, sizeof(ns->eui)))
 		return sprintf(buf, "eui.%8phN\n", ns->eui);
@@ -1803,11 +2011,28 @@ static ssize_t wwid_show(struct device *dev, struct device_attribute *attr,
 }
 static DEVICE_ATTR(wwid, S_IRUGO, wwid_show, NULL);
 
+static ssize_t nguid_show(struct device *dev, struct device_attribute *attr,
+			  char *buf)
+{
+	struct nvme_ns *ns = nvme_get_ns_from_dev(dev);
+	return sprintf(buf, "%pU\n", ns->nguid);
+}
+static DEVICE_ATTR(nguid, S_IRUGO, nguid_show, NULL);
+
 static ssize_t uuid_show(struct device *dev, struct device_attribute *attr,
 								char *buf)
 {
 	struct nvme_ns *ns = nvme_get_ns_from_dev(dev);
-	return sprintf(buf, "%pU\n", ns->uuid);
+
+	/* For backward compatibility expose the NGUID to userspace if
+	 * we have no UUID set
+	 */
+	if (uuid_is_null(&ns->uuid)) {
+		printk_ratelimited(KERN_WARNING
+				   "No UUID available providing old NGUID\n");
+		return sprintf(buf, "%pU\n", ns->nguid);
+	}
+	return sprintf(buf, "%pU\n", &ns->uuid);
 }
 static DEVICE_ATTR(uuid, S_IRUGO, uuid_show, NULL);
 
@@ -1830,6 +2055,7 @@ static DEVICE_ATTR(nsid, S_IRUGO, nsid_show, NULL);
 static struct attribute *nvme_ns_attrs[] = {
 	&dev_attr_wwid.attr,
 	&dev_attr_uuid.attr,
+	&dev_attr_nguid.attr,
 	&dev_attr_eui.attr,
 	&dev_attr_nsid.attr,
 	NULL,
@@ -1842,7 +2068,12 @@ static umode_t nvme_ns_attrs_are_visible(struct kobject *kobj,
 	struct nvme_ns *ns = nvme_get_ns_from_dev(dev);
 
 	if (a == &dev_attr_uuid.attr) {
-		if (!memchr_inv(ns->uuid, 0, sizeof(ns->uuid)))
+		if (uuid_is_null(&ns->uuid) ||
+		    !memchr_inv(ns->nguid, 0, sizeof(ns->nguid)))
+			return 0;
+	}
+	if (a == &dev_attr_nguid.attr) {
+		if (!memchr_inv(ns->nguid, 0, sizeof(ns->nguid)))
 			return 0;
 	}
 	if (a == &dev_attr_eui.attr) {
@@ -1931,8 +2162,7 @@ static ssize_t nvme_sysfs_show_subsysnqn(struct device *dev,
 {
 	struct nvme_ctrl *ctrl = dev_get_drvdata(dev);
 
-	return snprintf(buf, PAGE_SIZE, "%s\n",
-			ctrl->ops->get_subsysnqn(ctrl));
+	return snprintf(buf, PAGE_SIZE, "%s\n", ctrl->subnqn);
 }
 static DEVICE_ATTR(subsysnqn, S_IRUGO, nvme_sysfs_show_subsysnqn, NULL);
 
@@ -1961,24 +2191,16 @@ static struct attribute *nvme_dev_attrs[] = {
 	NULL
 };
 
-#define CHECK_ATTR(ctrl, a, name)		\
-	if ((a) == &dev_attr_##name.attr &&	\
-	    !(ctrl)->ops->get_##name)		\
-		return 0
-
 static umode_t nvme_dev_attrs_are_visible(struct kobject *kobj,
 		struct attribute *a, int n)
 {
 	struct device *dev = container_of(kobj, struct device, kobj);
 	struct nvme_ctrl *ctrl = dev_get_drvdata(dev);
 
-	if (a == &dev_attr_delete_controller.attr) {
-		if (!ctrl->ops->delete_ctrl)
-			return 0;
-	}
-
-	CHECK_ATTR(ctrl, a, subsysnqn);
-	CHECK_ATTR(ctrl, a, address);
+	if (a == &dev_attr_delete_controller.attr && !ctrl->ops->delete_ctrl)
+		return 0;
+	if (a == &dev_attr_address.attr && !ctrl->ops->get_address)
+		return 0;
 
 	return a->mode;
 }
@@ -2019,6 +2241,32 @@ static struct nvme_ns *nvme_find_get_ns(struct nvme_ctrl *ctrl, unsigned nsid)
 	return ret;
 }
 
+static int nvme_setup_streams_ns(struct nvme_ctrl *ctrl, struct nvme_ns *ns)
+{
+	struct streams_directive_params s;
+	int ret;
+
+	if (!ctrl->nr_streams)
+		return 0;
+
+	ret = nvme_get_stream_params(ctrl, &s, ns->ns_id);
+	if (ret)
+		return ret;
+
+	ns->sws = le32_to_cpu(s.sws);
+	ns->sgs = le16_to_cpu(s.sgs);
+
+	if (ns->sws) {
+		unsigned int bs = 1 << ns->lba_shift;
+
+		blk_queue_io_min(ns->queue, bs * ns->sws);
+		if (ns->sgs)
+			blk_queue_io_opt(ns->queue, bs * ns->sws * ns->sgs);
+	}
+
+	return 0;
+}
+
 static void nvme_alloc_ns(struct nvme_ctrl *ctrl, unsigned nsid)
 {
 	struct nvme_ns *ns;
@@ -2048,6 +2296,7 @@ static void nvme_alloc_ns(struct nvme_ctrl *ctrl, unsigned nsid)
 
 	blk_queue_logical_block_size(ns->queue, 1 << ns->lba_shift);
 	nvme_set_queue_limits(ctrl, ns->queue);
+	nvme_setup_streams_ns(ctrl, ns);
 
 	sprintf(disk_name, "nvme%dn%d", ctrl->instance, ns->instance);
 
@@ -2056,7 +2305,7 @@ static void nvme_alloc_ns(struct nvme_ctrl *ctrl, unsigned nsid)
 
 	if (nvme_nvm_ns_supported(ns, id) &&
 				nvme_nvm_register(ns, disk_name, node)) {
-		dev_warn(ctrl->dev, "%s: LightNVM init failure\n", __func__);
+		dev_warn(ctrl->device, "%s: LightNVM init failure\n", __func__);
 		goto out_free_id;
 	}
 
@@ -2231,7 +2480,7 @@ void nvme_queue_scan(struct nvme_ctrl *ctrl)
 	 * removal.
 	 */
 	if (ctrl->state == NVME_CTRL_LIVE)
-		schedule_work(&ctrl->scan_work);
+		queue_work(nvme_wq, &ctrl->scan_work);
 }
 EXPORT_SYMBOL_GPL(nvme_queue_scan);
 
@@ -2286,7 +2535,7 @@ void nvme_complete_async_event(struct nvme_ctrl *ctrl, __le16 status,
 		/*FALLTHRU*/
 	case NVME_SC_ABORT_REQ:
 		++ctrl->event_limit;
-		schedule_work(&ctrl->async_event_work);
+		queue_work(nvme_wq, &ctrl->async_event_work);
 		break;
 	default:
 		break;
@@ -2309,7 +2558,7 @@ EXPORT_SYMBOL_GPL(nvme_complete_async_event);
 void nvme_queue_async_events(struct nvme_ctrl *ctrl)
 {
 	ctrl->event_limit = NVME_NR_AERS;
-	schedule_work(&ctrl->async_event_work);
+	queue_work(nvme_wq, &ctrl->async_event_work);
 }
 EXPORT_SYMBOL_GPL(nvme_queue_async_events);
 
@@ -2442,6 +2691,9 @@ void nvme_kill_queues(struct nvme_ctrl *ctrl)
 
 	mutex_lock(&ctrl->namespaces_mutex);
 
+	/* Forcibly unquiesce queues to avoid blocking dispatch */
+	blk_mq_unquiesce_queue(ctrl->admin_q);
+
 	/* Forcibly start all queues to avoid having stuck requests */
 	blk_mq_start_hw_queues(ctrl->admin_q);
 
@@ -2455,6 +2707,9 @@ void nvme_kill_queues(struct nvme_ctrl *ctrl)
 		revalidate_disk(ns->disk);
 		blk_set_queue_dying(ns->queue);
 
+		/* Forcibly unquiesce queues to avoid blocking dispatch */
+		blk_mq_unquiesce_queue(ns->queue);
+
 		/*
 		 * Forcibly start all queues to avoid having stuck requests.
 		 * Note that we must ensure the queues are not stopped
@@ -2533,7 +2788,7 @@ void nvme_start_queues(struct nvme_ctrl *ctrl)
 
 	mutex_lock(&ctrl->namespaces_mutex);
 	list_for_each_entry(ns, &ctrl->namespaces, list) {
-		blk_mq_start_stopped_hw_queues(ns->queue, true);
+		blk_mq_unquiesce_queue(ns->queue);
 		blk_mq_kick_requeue_list(ns->queue);
 	}
 	mutex_unlock(&ctrl->namespaces_mutex);
@@ -2544,10 +2799,15 @@ int __init nvme_core_init(void)
 {
 	int result;
 
+	nvme_wq = alloc_workqueue("nvme-wq",
+			WQ_UNBOUND | WQ_MEM_RECLAIM | WQ_SYSFS, 0);
+	if (!nvme_wq)
+		return -ENOMEM;
+
 	result = __register_chrdev(nvme_char_major, 0, NVME_MINORS, "nvme",
 							&nvme_dev_fops);
 	if (result < 0)
-		return result;
+		goto destroy_wq;
 	else if (result > 0)
 		nvme_char_major = result;
 
@@ -2559,8 +2819,10 @@ int __init nvme_core_init(void)
 
 	return 0;
 
- unregister_chrdev:
+unregister_chrdev:
 	__unregister_chrdev(nvme_char_major, 0, NVME_MINORS, "nvme");
+destroy_wq:
+	destroy_workqueue(nvme_wq);
 	return result;
 }
 
@@ -2568,6 +2830,7 @@ void nvme_core_exit(void)
 {
 	class_destroy(nvme_class);
 	__unregister_chrdev(nvme_char_major, 0, NVME_MINORS, "nvme");
+	destroy_workqueue(nvme_wq);
 }
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/nvme/host/fabrics.c b/drivers/nvme/host/fabrics.c
index 990e6fb32a63..2e582a240943 100644
--- a/drivers/nvme/host/fabrics.c
+++ b/drivers/nvme/host/fabrics.c
@@ -58,7 +58,6 @@ static struct nvmf_host *nvmf_host_add(const char *hostnqn)
 
 	kref_init(&host->ref);
 	memcpy(host->nqn, hostnqn, NVMF_NQN_SIZE);
-	uuid_be_gen(&host->id);
 
 	list_add_tail(&host->list, &nvmf_hosts);
 out_unlock:
@@ -75,7 +74,6 @@ static struct nvmf_host *nvmf_host_default(void)
 		return NULL;
 
 	kref_init(&host->ref);
-	uuid_be_gen(&host->id);
 	snprintf(host->nqn, NVMF_NQN_SIZE,
 		"nqn.2014-08.org.nvmexpress:NVMf:uuid:%pUb", &host->id);
 
@@ -128,16 +126,6 @@ int nvmf_get_address(struct nvme_ctrl *ctrl, char *buf, int size)
 EXPORT_SYMBOL_GPL(nvmf_get_address);
 
 /**
- * nvmf_get_subsysnqn() - Get subsystem NQN
- * @ctrl:	Host NVMe controller instance which we got the NQN
- */
-const char *nvmf_get_subsysnqn(struct nvme_ctrl *ctrl)
-{
-	return ctrl->opts->subsysnqn;
-}
-EXPORT_SYMBOL_GPL(nvmf_get_subsysnqn);
-
-/**
  * nvmf_reg_read32() -  NVMe Fabrics "Property Get" API function.
  * @ctrl:	Host NVMe controller instance maintaining the admin
  *		queue used to submit the property read command to
@@ -337,6 +325,24 @@ static void nvmf_log_connect_error(struct nvme_ctrl *ctrl,
 			}
 		}
 		break;
+
+	case NVME_SC_CONNECT_INVALID_HOST:
+		dev_err(ctrl->device,
+			"Connect for subsystem %s is not allowed, hostnqn: %s\n",
+			data->subsysnqn, data->hostnqn);
+		break;
+
+	case NVME_SC_CONNECT_CTRL_BUSY:
+		dev_err(ctrl->device,
+			"Connect command failed: controller is busy or not available\n");
+		break;
+
+	case NVME_SC_CONNECT_FORMAT:
+		dev_err(ctrl->device,
+			"Connect incompatible format: %d",
+			cmd->connect.recfmt);
+		break;
+
 	default:
 		dev_err(ctrl->device,
 			"Connect command failed, error wo/DNR bit: %d\n",
@@ -376,13 +382,7 @@ int nvmf_connect_admin_queue(struct nvme_ctrl *ctrl)
 	cmd.connect.opcode = nvme_fabrics_command;
 	cmd.connect.fctype = nvme_fabrics_type_connect;
 	cmd.connect.qid = 0;
-
-	/*
-	 * fabrics spec sets a minimum of depth 32 for admin queue,
-	 * so set the queue with this depth always until
-	 * justification otherwise.
-	 */
-	cmd.connect.sqsize = cpu_to_le16(NVMF_AQ_DEPTH - 1);
+	cmd.connect.sqsize = cpu_to_le16(NVME_AQ_DEPTH - 1);
 
 	/*
 	 * Set keep-alive timeout in seconds granularity (ms * 1000)
@@ -395,7 +395,7 @@ int nvmf_connect_admin_queue(struct nvme_ctrl *ctrl)
 	if (!data)
 		return -ENOMEM;
 
-	memcpy(&data->hostid, &ctrl->opts->host->id, sizeof(uuid_be));
+	uuid_copy(&data->hostid, &ctrl->opts->host->id);
 	data->cntlid = cpu_to_le16(0xffff);
 	strncpy(data->subsysnqn, ctrl->opts->subsysnqn, NVMF_NQN_SIZE);
 	strncpy(data->hostnqn, ctrl->opts->host->nqn, NVMF_NQN_SIZE);
@@ -454,7 +454,7 @@ int nvmf_connect_io_queue(struct nvme_ctrl *ctrl, u16 qid)
 	if (!data)
 		return -ENOMEM;
 
-	memcpy(&data->hostid, &ctrl->opts->host->id, sizeof(uuid_be));
+	uuid_copy(&data->hostid, &ctrl->opts->host->id);
 	data->cntlid = cpu_to_le16(ctrl->cntlid);
 	strncpy(data->subsysnqn, ctrl->opts->subsysnqn, NVMF_NQN_SIZE);
 	strncpy(data->hostnqn, ctrl->opts->host->nqn, NVMF_NQN_SIZE);
@@ -474,7 +474,7 @@ EXPORT_SYMBOL_GPL(nvmf_connect_io_queue);
 bool nvmf_should_reconnect(struct nvme_ctrl *ctrl)
 {
 	if (ctrl->opts->max_reconnects != -1 &&
-	    ctrl->opts->nr_reconnects < ctrl->opts->max_reconnects)
+	    ctrl->nr_reconnects < ctrl->opts->max_reconnects)
 		return true;
 
 	return false;
@@ -547,6 +547,7 @@ static const match_table_t opt_tokens = {
 	{ NVMF_OPT_KATO,		"keep_alive_tmo=%d"	},
 	{ NVMF_OPT_HOSTNQN,		"hostnqn=%s"		},
 	{ NVMF_OPT_HOST_TRADDR,		"host_traddr=%s"	},
+	{ NVMF_OPT_HOST_ID,		"hostid=%s"		},
 	{ NVMF_OPT_ERR,			NULL			}
 };
 
@@ -558,6 +559,7 @@ static int nvmf_parse_options(struct nvmf_ctrl_options *opts,
 	int token, ret = 0;
 	size_t nqnlen  = 0;
 	int ctrl_loss_tmo = NVMF_DEF_CTRL_LOSS_TMO;
+	uuid_t hostid;
 
 	/* Set defaults */
 	opts->queue_size = NVMF_DEF_QUEUE_SIZE;
@@ -568,6 +570,8 @@ static int nvmf_parse_options(struct nvmf_ctrl_options *opts,
 	if (!options)
 		return -ENOMEM;
 
+	uuid_gen(&hostid);
+
 	while ((p = strsep(&o, ",\n")) != NULL) {
 		if (!*p)
 			continue;
@@ -724,6 +728,17 @@ static int nvmf_parse_options(struct nvmf_ctrl_options *opts,
 			}
 			opts->host_traddr = p;
 			break;
+		case NVMF_OPT_HOST_ID:
+			p = match_strdup(args);
+			if (!p) {
+				ret = -ENOMEM;
+				goto out;
+			}
+			if (uuid_parse(p, &hostid)) {
+				ret = -EINVAL;
+				goto out;
+			}
+			break;
 		default:
 			pr_warn("unknown parameter or missing value '%s' in ctrl creation request\n",
 				p);
@@ -743,6 +758,8 @@ static int nvmf_parse_options(struct nvmf_ctrl_options *opts,
 		opts->host = nvmf_default_host;
 	}
 
+	uuid_copy(&opts->host->id, &hostid);
+
 out:
 	if (!opts->discovery_nqn && !opts->kato)
 		opts->kato = NVME_DEFAULT_KATO;
@@ -803,7 +820,8 @@ EXPORT_SYMBOL_GPL(nvmf_free_options);
 
 #define NVMF_REQUIRED_OPTS	(NVMF_OPT_TRANSPORT | NVMF_OPT_NQN)
 #define NVMF_ALLOWED_OPTS	(NVMF_OPT_QUEUE_SIZE | NVMF_OPT_NR_IO_QUEUES | \
-				 NVMF_OPT_KATO | NVMF_OPT_HOSTNQN)
+				 NVMF_OPT_KATO | NVMF_OPT_HOSTNQN | \
+				 NVMF_OPT_HOST_ID)
 
 static struct nvme_ctrl *
 nvmf_create_ctrl(struct device *dev, const char *buf, size_t count)
@@ -854,6 +872,15 @@ nvmf_create_ctrl(struct device *dev, const char *buf, size_t count)
 		goto out_unlock;
 	}
 
+	if (strcmp(ctrl->subnqn, opts->subsysnqn)) {
+		dev_warn(ctrl->device,
+			"controller returned incorrect NQN: \"%s\".\n",
+			ctrl->subnqn);
+		mutex_unlock(&nvmf_transports_mutex);
+		ctrl->ops->delete_ctrl(ctrl);
+		return ERR_PTR(-EINVAL);
+	}
+
 	mutex_unlock(&nvmf_transports_mutex);
 	return ctrl;
 
diff --git a/drivers/nvme/host/fabrics.h b/drivers/nvme/host/fabrics.h
index f5a9c1fb186f..bf33663218cd 100644
--- a/drivers/nvme/host/fabrics.h
+++ b/drivers/nvme/host/fabrics.h
@@ -36,7 +36,7 @@ struct nvmf_host {
 	struct kref		ref;
 	struct list_head	list;
 	char			nqn[NVMF_NQN_SIZE];
-	uuid_be			id;
+	uuid_t			id;
 };
 
 /**
@@ -56,6 +56,7 @@ enum {
 	NVMF_OPT_RECONNECT_DELAY = 1 << 9,
 	NVMF_OPT_HOST_TRADDR	= 1 << 10,
 	NVMF_OPT_CTRL_LOSS_TMO	= 1 << 11,
+	NVMF_OPT_HOST_ID	= 1 << 12,
 };
 
 /**
@@ -80,7 +81,6 @@ enum {
  * @discovery_nqn: indicates if the subsysnqn is the well-known discovery NQN.
  * @kato:	Keep-alive timeout.
  * @host:	Virtual NVMe host, contains the NQN and Host ID.
- * @nr_reconnects: number of reconnect attempted since the last ctrl failure
  * @max_reconnects: maximum number of allowed reconnect attempts before removing
  *              the controller, (-1) means reconnect forever, zero means remove
  *              immediately;
@@ -98,7 +98,6 @@ struct nvmf_ctrl_options {
 	bool			discovery_nqn;
 	unsigned int		kato;
 	struct nvmf_host	*host;
-	int			nr_reconnects;
 	int			max_reconnects;
 };
 
@@ -140,7 +139,6 @@ int nvmf_connect_io_queue(struct nvme_ctrl *ctrl, u16 qid);
 int nvmf_register_transport(struct nvmf_transport_ops *ops);
 void nvmf_unregister_transport(struct nvmf_transport_ops *ops);
 void nvmf_free_options(struct nvmf_ctrl_options *opts);
-const char *nvmf_get_subsysnqn(struct nvme_ctrl *ctrl);
 int nvmf_get_address(struct nvme_ctrl *ctrl, char *buf, int size);
 bool nvmf_should_reconnect(struct nvme_ctrl *ctrl);
 
diff --git a/drivers/nvme/host/fc.c b/drivers/nvme/host/fc.c
index 92964cef0f4b..ed87214fdc0e 100644
--- a/drivers/nvme/host/fc.c
+++ b/drivers/nvme/host/fc.c
@@ -36,7 +36,7 @@
  */
 #define NVME_FC_NR_AEN_COMMANDS	1
 #define NVME_FC_AQ_BLKMQ_DEPTH	\
-	(NVMF_AQ_DEPTH - NVME_FC_NR_AEN_COMMANDS)
+	(NVME_AQ_DEPTH - NVME_FC_NR_AEN_COMMANDS)
 #define AEN_CMDID_BASE		(NVME_FC_AQ_BLKMQ_DEPTH + 1)
 
 enum nvme_fc_queue_flags {
@@ -161,12 +161,12 @@ struct nvme_fc_ctrl {
 	struct blk_mq_tag_set	tag_set;
 
 	struct work_struct	delete_work;
-	struct work_struct	reset_work;
 	struct delayed_work	connect_work;
 
 	struct kref		ref;
 	u32			flags;
 	u32			iocnt;
+	wait_queue_head_t	ioabort_wait;
 
 	struct nvme_fc_fcp_op	aen_ops[NVME_FC_NR_AEN_COMMANDS];
 
@@ -214,7 +214,6 @@ static LIST_HEAD(nvme_fc_lport_list);
 static DEFINE_IDA(nvme_fc_local_port_cnt);
 static DEFINE_IDA(nvme_fc_ctrl_cnt);
 
-static struct workqueue_struct *nvme_fc_wq;
 
 
 
@@ -878,8 +877,7 @@ nvme_fc_connect_admin_queue(struct nvme_fc_ctrl *ctrl,
 	assoc_rqst->assoc_cmd.sqsize = cpu_to_be16(qsize);
 	/* Linux supports only Dynamic controllers */
 	assoc_rqst->assoc_cmd.cntlid = cpu_to_be16(0xffff);
-	memcpy(&assoc_rqst->assoc_cmd.hostid, &ctrl->ctrl.opts->host->id,
-		min_t(size_t, FCNVME_ASSOC_HOSTID_LEN, sizeof(uuid_be)));
+	uuid_copy(&assoc_rqst->assoc_cmd.hostid, &ctrl->ctrl.opts->host->id);
 	strncpy(assoc_rqst->assoc_cmd.hostnqn, ctrl->ctrl.opts->host->nqn,
 		min(FCNVME_ASSOC_HOSTNQN_LEN, NVMF_NQN_SIZE));
 	strncpy(assoc_rqst->assoc_cmd.subnqn, ctrl->ctrl.opts->subsysnqn,
@@ -1242,8 +1240,10 @@ __nvme_fc_fcpop_chk_teardowns(struct nvme_fc_ctrl *ctrl,
 
 	spin_lock_irqsave(&ctrl->lock, flags);
 	if (unlikely(op->flags & FCOP_FLAGS_TERMIO)) {
-		if (ctrl->flags & FCCTRL_TERMIO)
-			ctrl->iocnt--;
+		if (ctrl->flags & FCCTRL_TERMIO) {
+			if (!--ctrl->iocnt)
+				wake_up(&ctrl->ioabort_wait);
+		}
 	}
 	if (op->flags & FCOP_FLAGS_RELEASED)
 		complete_rq = true;
@@ -1450,18 +1450,8 @@ nvme_fc_init_request(struct blk_mq_tag_set *set, struct request *rq,
 {
 	struct nvme_fc_ctrl *ctrl = set->driver_data;
 	struct nvme_fc_fcp_op *op = blk_mq_rq_to_pdu(rq);
-	struct nvme_fc_queue *queue = &ctrl->queues[hctx_idx+1];
-
-	return __nvme_fc_init_request(ctrl, queue, op, rq, queue->rqcnt++);
-}
-
-static int
-nvme_fc_init_admin_request(struct blk_mq_tag_set *set, struct request *rq,
-		unsigned int hctx_idx, unsigned int numa_node)
-{
-	struct nvme_fc_ctrl *ctrl = set->driver_data;
-	struct nvme_fc_fcp_op *op = blk_mq_rq_to_pdu(rq);
-	struct nvme_fc_queue *queue = &ctrl->queues[0];
+	int queue_idx = (set == &ctrl->tag_set) ? hctx_idx + 1 : 0;
+	struct nvme_fc_queue *queue = &ctrl->queues[queue_idx];
 
 	return __nvme_fc_init_request(ctrl, queue, op, rq, queue->rqcnt++);
 }
@@ -1759,16 +1749,16 @@ nvme_fc_nvme_ctrl_freed(struct nvme_ctrl *nctrl)
 static void
 nvme_fc_error_recovery(struct nvme_fc_ctrl *ctrl, char *errmsg)
 {
+	/* only proceed if in LIVE state - e.g. on first error */
+	if (ctrl->ctrl.state != NVME_CTRL_LIVE)
+		return;
+
 	dev_warn(ctrl->ctrl.device,
 		"NVME-FC{%d}: transport association error detected: %s\n",
 		ctrl->cnum, errmsg);
 	dev_warn(ctrl->ctrl.device,
 		"NVME-FC{%d}: resetting controller\n", ctrl->cnum);
 
-	/* stop the queues on error, cleanup is in reset thread */
-	if (ctrl->queue_count > 1)
-		nvme_stop_queues(&ctrl->ctrl);
-
 	if (!nvme_change_ctrl_state(&ctrl->ctrl, NVME_CTRL_RECONNECTING)) {
 		dev_err(ctrl->ctrl.device,
 			"NVME-FC{%d}: error_recovery: Couldn't change state "
@@ -1776,10 +1766,7 @@ nvme_fc_error_recovery(struct nvme_fc_ctrl *ctrl, char *errmsg)
 		return;
 	}
 
-	if (!queue_work(nvme_fc_wq, &ctrl->reset_work))
-		dev_err(ctrl->ctrl.device,
-			"NVME-FC{%d}: error_recovery: Failed to schedule "
-			"reset work\n", ctrl->cnum);
+	nvme_reset_ctrl(&ctrl->ctrl);
 }
 
 static enum blk_eh_timer_return
@@ -1888,7 +1875,7 @@ nvme_fc_unmap_data(struct nvme_fc_ctrl *ctrl, struct request *rq,
  * level FC exchange resource that is also outstanding. This must be
  * considered in all cleanup operations.
  */
-static int
+static blk_status_t
 nvme_fc_start_fcp_op(struct nvme_fc_ctrl *ctrl, struct nvme_fc_queue *queue,
 	struct nvme_fc_fcp_op *op, u32 data_len,
 	enum nvmefc_fcp_datadir	io_dir)
@@ -1903,10 +1890,10 @@ nvme_fc_start_fcp_op(struct nvme_fc_ctrl *ctrl, struct nvme_fc_queue *queue,
 	 * the target device is present
 	 */
 	if (ctrl->rport->remoteport.port_state != FC_OBJSTATE_ONLINE)
-		return BLK_MQ_RQ_QUEUE_ERROR;
+		return BLK_STS_IOERR;
 
 	if (!nvme_fc_ctrl_get(ctrl))
-		return BLK_MQ_RQ_QUEUE_ERROR;
+		return BLK_STS_IOERR;
 
 	/* format the FC-NVME CMD IU and fcp_req */
 	cmdiu->connection_id = cpu_to_be64(queue->connection_id);
@@ -1954,8 +1941,9 @@ nvme_fc_start_fcp_op(struct nvme_fc_ctrl *ctrl, struct nvme_fc_queue *queue,
 		if (ret < 0) {
 			nvme_cleanup_cmd(op->rq);
 			nvme_fc_ctrl_put(ctrl);
-			return (ret == -ENOMEM || ret == -EAGAIN) ?
-				BLK_MQ_RQ_QUEUE_BUSY : BLK_MQ_RQ_QUEUE_ERROR;
+			if (ret == -ENOMEM || ret == -EAGAIN)
+				return BLK_STS_RESOURCE;
+			return BLK_STS_IOERR;
 		}
 	}
 
@@ -1972,28 +1960,26 @@ nvme_fc_start_fcp_op(struct nvme_fc_ctrl *ctrl, struct nvme_fc_queue *queue,
 					queue->lldd_handle, &op->fcp_req);
 
 	if (ret) {
-		if (op->rq) {			/* normal request */
+		if (op->rq)			/* normal request */
 			nvme_fc_unmap_data(ctrl, op->rq, op);
-			nvme_cleanup_cmd(op->rq);
-		}
 		/* else - aen. no cleanup needed */
 
 		nvme_fc_ctrl_put(ctrl);
 
 		if (ret != -EBUSY)
-			return BLK_MQ_RQ_QUEUE_ERROR;
+			return BLK_STS_IOERR;
 
 		if (op->rq) {
 			blk_mq_stop_hw_queues(op->rq->q);
 			blk_mq_delay_queue(queue->hctx, NVMEFC_QUEUE_DELAY);
 		}
-		return BLK_MQ_RQ_QUEUE_BUSY;
+		return BLK_STS_RESOURCE;
 	}
 
-	return BLK_MQ_RQ_QUEUE_OK;
+	return BLK_STS_OK;
 }
 
-static int
+static blk_status_t
 nvme_fc_queue_rq(struct blk_mq_hw_ctx *hctx,
 			const struct blk_mq_queue_data *bd)
 {
@@ -2006,7 +1992,7 @@ nvme_fc_queue_rq(struct blk_mq_hw_ctx *hctx,
 	struct nvme_command *sqe = &cmdiu->sqe;
 	enum nvmefc_fcp_datadir	io_dir;
 	u32 data_len;
-	int ret;
+	blk_status_t ret;
 
 	ret = nvme_setup_cmd(ns, rq, sqe);
 	if (ret)
@@ -2061,7 +2047,7 @@ nvme_fc_submit_async_event(struct nvme_ctrl *arg, int aer_idx)
 	struct nvme_fc_fcp_op *aen_op;
 	unsigned long flags;
 	bool terminating = false;
-	int ret;
+	blk_status_t ret;
 
 	if (aer_idx > NVME_FC_NR_AEN_COMMANDS)
 		return;
@@ -2093,7 +2079,6 @@ __nvme_fc_final_op_cleanup(struct request *rq)
 	op->flags &= ~(FCOP_FLAGS_TERMIO | FCOP_FLAGS_RELEASED |
 			FCOP_FLAGS_COMPLETE);
 
-	nvme_cleanup_cmd(rq);
 	nvme_fc_unmap_data(ctrl, rq, op);
 	nvme_complete_rq(rq);
 	nvme_fc_ctrl_put(ctrl);
@@ -2311,7 +2296,7 @@ nvme_fc_create_association(struct nvme_fc_ctrl *ctrl)
 	int ret;
 	bool changed;
 
-	++ctrl->ctrl.opts->nr_reconnects;
+	++ctrl->ctrl.nr_reconnects;
 
 	/*
 	 * Create the admin queue
@@ -2408,7 +2393,7 @@ nvme_fc_create_association(struct nvme_fc_ctrl *ctrl)
 	changed = nvme_change_ctrl_state(&ctrl->ctrl, NVME_CTRL_LIVE);
 	WARN_ON_ONCE(!changed);
 
-	ctrl->ctrl.opts->nr_reconnects = 0;
+	ctrl->ctrl.nr_reconnects = 0;
 
 	if (ctrl->queue_count > 1) {
 		nvme_start_queues(&ctrl->ctrl);
@@ -2494,11 +2479,7 @@ nvme_fc_delete_association(struct nvme_fc_ctrl *ctrl)
 
 	/* wait for all io that had to be aborted */
 	spin_lock_irqsave(&ctrl->lock, flags);
-	while (ctrl->iocnt) {
-		spin_unlock_irqrestore(&ctrl->lock, flags);
-		msleep(1000);
-		spin_lock_irqsave(&ctrl->lock, flags);
-	}
+	wait_event_lock_irq(ctrl->ioabort_wait, ctrl->iocnt == 0, ctrl->lock);
 	ctrl->flags &= ~FCCTRL_TERMIO;
 	spin_unlock_irqrestore(&ctrl->lock, flags);
 
@@ -2528,7 +2509,7 @@ nvme_fc_delete_ctrl_work(struct work_struct *work)
 	struct nvme_fc_ctrl *ctrl =
 		container_of(work, struct nvme_fc_ctrl, delete_work);
 
-	cancel_work_sync(&ctrl->reset_work);
+	cancel_work_sync(&ctrl->ctrl.reset_work);
 	cancel_delayed_work_sync(&ctrl->connect_work);
 
 	/*
@@ -2555,7 +2536,7 @@ __nvme_fc_schedule_delete_work(struct nvme_fc_ctrl *ctrl)
 	if (!nvme_change_ctrl_state(&ctrl->ctrl, NVME_CTRL_DELETING))
 		return true;
 
-	if (!queue_work(nvme_fc_wq, &ctrl->delete_work))
+	if (!queue_work(nvme_wq, &ctrl->delete_work))
 		return true;
 
 	return false;
@@ -2582,7 +2563,7 @@ nvme_fc_del_nvme_ctrl(struct nvme_ctrl *nctrl)
 	ret = __nvme_fc_del_ctrl(ctrl);
 
 	if (!ret)
-		flush_workqueue(nvme_fc_wq);
+		flush_workqueue(nvme_wq);
 
 	nvme_put_ctrl(&ctrl->ctrl);
 
@@ -2607,13 +2588,13 @@ nvme_fc_reconnect_or_delete(struct nvme_fc_ctrl *ctrl, int status)
 		dev_info(ctrl->ctrl.device,
 			"NVME-FC{%d}: Reconnect attempt in %d seconds.\n",
 			ctrl->cnum, ctrl->ctrl.opts->reconnect_delay);
-		queue_delayed_work(nvme_fc_wq, &ctrl->connect_work,
+		queue_delayed_work(nvme_wq, &ctrl->connect_work,
 				ctrl->ctrl.opts->reconnect_delay * HZ);
 	} else {
 		dev_warn(ctrl->ctrl.device,
 				"NVME-FC{%d}: Max reconnect attempts (%d) "
 				"reached. Removing controller\n",
-				ctrl->cnum, ctrl->ctrl.opts->nr_reconnects);
+				ctrl->cnum, ctrl->ctrl.nr_reconnects);
 		WARN_ON(__nvme_fc_schedule_delete_work(ctrl));
 	}
 }
@@ -2622,7 +2603,7 @@ static void
 nvme_fc_reset_ctrl_work(struct work_struct *work)
 {
 	struct nvme_fc_ctrl *ctrl =
-			container_of(work, struct nvme_fc_ctrl, reset_work);
+		container_of(work, struct nvme_fc_ctrl, ctrl.reset_work);
 	int ret;
 
 	/* will block will waiting for io to terminate */
@@ -2636,29 +2617,6 @@ nvme_fc_reset_ctrl_work(struct work_struct *work)
 			"NVME-FC{%d}: controller reset complete\n", ctrl->cnum);
 }
 
-/*
- * called by the nvme core layer, for sysfs interface that requests
- * a reset of the nvme controller
- */
-static int
-nvme_fc_reset_nvme_ctrl(struct nvme_ctrl *nctrl)
-{
-	struct nvme_fc_ctrl *ctrl = to_fc_ctrl(nctrl);
-
-	dev_info(ctrl->ctrl.device,
-		"NVME-FC{%d}: admin requested controller reset\n", ctrl->cnum);
-
-	if (!nvme_change_ctrl_state(&ctrl->ctrl, NVME_CTRL_RESETTING))
-		return -EBUSY;
-
-	if (!queue_work(nvme_fc_wq, &ctrl->reset_work))
-		return -EBUSY;
-
-	flush_work(&ctrl->reset_work);
-
-	return 0;
-}
-
 static const struct nvme_ctrl_ops nvme_fc_ctrl_ops = {
 	.name			= "fc",
 	.module			= THIS_MODULE,
@@ -2666,11 +2624,9 @@ static const struct nvme_ctrl_ops nvme_fc_ctrl_ops = {
 	.reg_read32		= nvmf_reg_read32,
 	.reg_read64		= nvmf_reg_read64,
 	.reg_write32		= nvmf_reg_write32,
-	.reset_ctrl		= nvme_fc_reset_nvme_ctrl,
 	.free_ctrl		= nvme_fc_nvme_ctrl_freed,
 	.submit_async_event	= nvme_fc_submit_async_event,
 	.delete_ctrl		= nvme_fc_del_nvme_ctrl,
-	.get_subsysnqn		= nvmf_get_subsysnqn,
 	.get_address		= nvmf_get_address,
 };
 
@@ -2696,7 +2652,7 @@ nvme_fc_connect_ctrl_work(struct work_struct *work)
 static const struct blk_mq_ops nvme_fc_admin_mq_ops = {
 	.queue_rq	= nvme_fc_queue_rq,
 	.complete	= nvme_fc_complete_rq,
-	.init_request	= nvme_fc_init_admin_request,
+	.init_request	= nvme_fc_init_request,
 	.exit_request	= nvme_fc_exit_request,
 	.reinit_request	= nvme_fc_reinit_request,
 	.init_hctx	= nvme_fc_init_admin_hctx,
@@ -2741,7 +2697,7 @@ nvme_fc_init_ctrl(struct device *dev, struct nvmf_ctrl_options *opts,
 	kref_init(&ctrl->ref);
 
 	INIT_WORK(&ctrl->delete_work, nvme_fc_delete_ctrl_work);
-	INIT_WORK(&ctrl->reset_work, nvme_fc_reset_ctrl_work);
+	INIT_WORK(&ctrl->ctrl.reset_work, nvme_fc_reset_ctrl_work);
 	INIT_DELAYED_WORK(&ctrl->connect_work, nvme_fc_connect_ctrl_work);
 	spin_lock_init(&ctrl->lock);
 
@@ -2808,6 +2764,9 @@ nvme_fc_init_ctrl(struct device *dev, struct nvmf_ctrl_options *opts,
 		nvme_uninit_ctrl(&ctrl->ctrl);
 		nvme_put_ctrl(&ctrl->ctrl);
 
+		/* Remove core ctrl ref. */
+		nvme_put_ctrl(&ctrl->ctrl);
+
 		/* as we're past the point where we transition to the ref
 		 * counting teardown path, if we return a bad pointer here,
 		 * the calling routine, thinking it's prior to the
@@ -2966,20 +2925,7 @@ static struct nvmf_transport_ops nvme_fc_transport = {
 
 static int __init nvme_fc_init_module(void)
 {
-	int ret;
-
-	nvme_fc_wq = create_workqueue("nvme_fc_wq");
-	if (!nvme_fc_wq)
-		return -ENOMEM;
-
-	ret = nvmf_register_transport(&nvme_fc_transport);
-	if (ret)
-		goto err;
-
-	return 0;
-err:
-	destroy_workqueue(nvme_fc_wq);
-	return ret;
+	return nvmf_register_transport(&nvme_fc_transport);
 }
 
 static void __exit nvme_fc_exit_module(void)
@@ -2990,8 +2936,6 @@ static void __exit nvme_fc_exit_module(void)
 
 	nvmf_unregister_transport(&nvme_fc_transport);
 
-	destroy_workqueue(nvme_fc_wq);
-
 	ida_destroy(&nvme_fc_local_port_cnt);
 	ida_destroy(&nvme_fc_ctrl_cnt);
 }
diff --git a/drivers/nvme/host/lightnvm.c b/drivers/nvme/host/lightnvm.c
index f5df78ed1e10..be8541335e31 100644
--- a/drivers/nvme/host/lightnvm.c
+++ b/drivers/nvme/host/lightnvm.c
@@ -242,7 +242,7 @@ static inline void _nvme_nvm_check_size(void)
 	BUILD_BUG_ON(sizeof(struct nvme_nvm_erase_blk) != 64);
 	BUILD_BUG_ON(sizeof(struct nvme_nvm_id_group) != 960);
 	BUILD_BUG_ON(sizeof(struct nvme_nvm_addr_format) != 16);
-	BUILD_BUG_ON(sizeof(struct nvme_nvm_id) != 4096);
+	BUILD_BUG_ON(sizeof(struct nvme_nvm_id) != NVME_IDENTIFY_DATA_SIZE);
 	BUILD_BUG_ON(sizeof(struct nvme_nvm_bb_tbl) != 64);
 }
 
@@ -480,7 +480,7 @@ static inline void nvme_nvm_rqtocmd(struct nvm_rq *rqd, struct nvme_ns *ns,
 					rqd->bio->bi_iter.bi_sector));
 }
 
-static void nvme_nvm_end_io(struct request *rq, int error)
+static void nvme_nvm_end_io(struct request *rq, blk_status_t status)
 {
 	struct nvm_rq *rqd = rq->end_io_data;
 
@@ -509,7 +509,7 @@ static int nvme_nvm_submit_io(struct nvm_dev *dev, struct nvm_rq *rqd)
 	rq = nvme_alloc_request(q, (struct nvme_command *)cmd, 0, NVME_QID_ANY);
 	if (IS_ERR(rq)) {
 		kfree(cmd);
-		return -ENOMEM;
+		return PTR_ERR(rq);
 	}
 	rq->cmd_flags &= ~REQ_FAILFAST_DRIVER;
 
@@ -571,13 +571,6 @@ static struct nvm_dev_ops nvme_nvm_dev_ops = {
 	.max_phys_sect		= 64,
 };
 
-static void nvme_nvm_end_user_vio(struct request *rq, int error)
-{
-	struct completion *waiting = rq->end_io_data;
-
-	complete(waiting);
-}
-
 static int nvme_nvm_submit_user_cmd(struct request_queue *q,
 				struct nvme_ns *ns,
 				struct nvme_nvm_command *vcmd,
@@ -608,7 +601,6 @@ static int nvme_nvm_submit_user_cmd(struct request_queue *q,
 	rq->timeout = timeout ? timeout : ADMIN_TIMEOUT;
 
 	rq->cmd_flags &= ~REQ_FAILFAST_DRIVER;
-	rq->end_io_data = &wait;
 
 	if (ppa_buf && ppa_len) {
 		ppa_list = dma_pool_alloc(dev->dma_pool, GFP_KERNEL, &ppa_dma);
@@ -662,9 +654,7 @@ static int nvme_nvm_submit_user_cmd(struct request_queue *q,
 	}
 
 submit:
-	blk_execute_rq_nowait(q, NULL, rq, 0, nvme_nvm_end_user_vio);
-
-	wait_for_completion_io(&wait);
+	blk_execute_rq(q, NULL, rq, 0);
 
 	if (nvme_req(rq)->flags & NVME_REQ_CANCELLED)
 		ret = -EINTR;
diff --git a/drivers/nvme/host/nvme.h b/drivers/nvme/host/nvme.h
index 9d6a070d4391..d70ff0fdd36b 100644
--- a/drivers/nvme/host/nvme.h
+++ b/drivers/nvme/host/nvme.h
@@ -27,12 +27,11 @@ extern unsigned char nvme_io_timeout;
 extern unsigned char admin_timeout;
 #define ADMIN_TIMEOUT	(admin_timeout * HZ)
 
-extern unsigned char shutdown_timeout;
-#define SHUTDOWN_TIMEOUT	(shutdown_timeout * HZ)
-
 #define NVME_DEFAULT_KATO	5
 #define NVME_KATO_GRACE		10
 
+extern struct workqueue_struct *nvme_wq;
+
 enum {
 	NVME_NS_LBA		= 0,
 	NVME_NS_LIGHTNVM	= 1,
@@ -131,6 +130,7 @@ struct nvme_ctrl {
 	struct device *device;	/* char device */
 	struct list_head node;
 	struct ida ns_ida;
+	struct work_struct reset_work;
 
 	struct opal_dev *opal_dev;
 
@@ -138,6 +138,7 @@ struct nvme_ctrl {
 	char serial[20];
 	char model[40];
 	char firmware_rev[8];
+	char subnqn[NVMF_NQN_SIZE];
 	u16 cntlid;
 
 	u32 ctrl_config;
@@ -147,6 +148,8 @@ struct nvme_ctrl {
 	u16 oncs;
 	u16 vid;
 	u16 oacs;
+	u16 nssa;
+	u16 nr_streams;
 	atomic_t abort_limit;
 	u8 event_limit;
 	u8 vwc;
@@ -165,6 +168,10 @@ struct nvme_ctrl {
 
 	/* Power saving configuration */
 	u64 ps_max_latency_us;
+	bool apst_enabled;
+
+	u32 hmpre;
+	u32 hmmin;
 
 	/* Fabrics only */
 	u16 sqsize;
@@ -172,12 +179,10 @@ struct nvme_ctrl {
 	u32 iorcsz;
 	u16 icdoff;
 	u16 maxcmd;
+	int nr_reconnects;
 	struct nvmf_ctrl_options *opts;
 };
 
-/*
- * An NVM Express namespace is equivalent to a SCSI LUN
- */
 struct nvme_ns {
 	struct list_head list;
 
@@ -189,14 +194,18 @@ struct nvme_ns {
 	int instance;
 
 	u8 eui[8];
-	u8 uuid[16];
+	u8 nguid[16];
+	uuid_t uuid;
 
 	unsigned ns_id;
 	int lba_shift;
 	u16 ms;
+	u16 sgs;
+	u32 sws;
 	bool ext;
 	u8 pi_type;
 	unsigned long flags;
+	u16 noiob;
 
 #define NVME_NS_REMOVING 0
 #define NVME_NS_DEAD     1
@@ -214,11 +223,9 @@ struct nvme_ctrl_ops {
 	int (*reg_read32)(struct nvme_ctrl *ctrl, u32 off, u32 *val);
 	int (*reg_write32)(struct nvme_ctrl *ctrl, u32 off, u32 val);
 	int (*reg_read64)(struct nvme_ctrl *ctrl, u32 off, u64 *val);
-	int (*reset_ctrl)(struct nvme_ctrl *ctrl);
 	void (*free_ctrl)(struct nvme_ctrl *ctrl);
 	void (*submit_async_event)(struct nvme_ctrl *ctrl, int aer_idx);
 	int (*delete_ctrl)(struct nvme_ctrl *ctrl);
-	const char *(*get_subsysnqn)(struct nvme_ctrl *ctrl);
 	int (*get_address)(struct nvme_ctrl *ctrl, char *buf, int size);
 };
 
@@ -296,7 +303,7 @@ void nvme_start_freeze(struct nvme_ctrl *ctrl);
 #define NVME_QID_ANY -1
 struct request *nvme_alloc_request(struct request_queue *q,
 		struct nvme_command *cmd, unsigned int flags, int qid);
-int nvme_setup_cmd(struct nvme_ns *ns, struct request *req,
+blk_status_t nvme_setup_cmd(struct nvme_ns *ns, struct request *req,
 		struct nvme_command *cmd);
 int nvme_submit_sync_cmd(struct request_queue *q, struct nvme_command *cmd,
 		void *buf, unsigned bufflen);
@@ -310,23 +317,10 @@ int __nvme_submit_user_cmd(struct request_queue *q, struct nvme_command *cmd,
 		void __user *ubuffer, unsigned bufflen,
 		void __user *meta_buffer, unsigned meta_len, u32 meta_seed,
 		u32 *result, unsigned timeout);
-int nvme_identify_ctrl(struct nvme_ctrl *dev, struct nvme_id_ctrl **id);
-int nvme_identify_ns(struct nvme_ctrl *dev, unsigned nsid,
-		struct nvme_id_ns **id);
-int nvme_get_log_page(struct nvme_ctrl *dev, struct nvme_smart_log **log);
-int nvme_get_features(struct nvme_ctrl *dev, unsigned fid, unsigned nsid,
-		      void *buffer, size_t buflen, u32 *result);
-int nvme_set_features(struct nvme_ctrl *dev, unsigned fid, unsigned dword11,
-		      void *buffer, size_t buflen, u32 *result);
 int nvme_set_queue_count(struct nvme_ctrl *ctrl, int *count);
 void nvme_start_keep_alive(struct nvme_ctrl *ctrl);
 void nvme_stop_keep_alive(struct nvme_ctrl *ctrl);
-
-struct sg_io_hdr;
-
-int nvme_sg_io(struct nvme_ns *ns, struct sg_io_hdr __user *u_hdr);
-int nvme_sg_io32(struct nvme_ns *ns, unsigned long arg);
-int nvme_sg_get_version_num(int __user *ip);
+int nvme_reset_ctrl(struct nvme_ctrl *ctrl);
 
 #ifdef CONFIG_NVM
 int nvme_nvm_ns_supported(struct nvme_ns *ns, struct nvme_id_ns *id);
diff --git a/drivers/nvme/host/pci.c b/drivers/nvme/host/pci.c
index 951042a375d6..5b1ac79fb607 100644
--- a/drivers/nvme/host/pci.c
+++ b/drivers/nvme/host/pci.c
@@ -17,28 +17,15 @@
 #include <linux/blkdev.h>
 #include <linux/blk-mq.h>
 #include <linux/blk-mq-pci.h>
-#include <linux/cpu.h>
-#include <linux/delay.h>
 #include <linux/dmi.h>
-#include <linux/errno.h>
-#include <linux/fs.h>
-#include <linux/genhd.h>
-#include <linux/hdreg.h>
-#include <linux/idr.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
-#include <linux/kdev_t.h>
-#include <linux/kernel.h>
 #include <linux/mm.h>
 #include <linux/module.h>
-#include <linux/moduleparam.h>
 #include <linux/mutex.h>
 #include <linux/pci.h>
 #include <linux/poison.h>
-#include <linux/ptrace.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
 #include <linux/t10-pi.h>
 #include <linux/timer.h>
 #include <linux/types.h>
@@ -49,7 +36,6 @@
 #include "nvme.h"
 
 #define NVME_Q_DEPTH		1024
-#define NVME_AQ_DEPTH		256
 #define SQ_SIZE(depth)		(depth * sizeof(struct nvme_command))
 #define CQ_SIZE(depth)		(depth * sizeof(struct nvme_completion))
 
@@ -66,12 +52,14 @@ static bool use_cmb_sqes = true;
 module_param(use_cmb_sqes, bool, 0644);
 MODULE_PARM_DESC(use_cmb_sqes, "use controller's memory buffer for I/O SQes");
 
-static struct workqueue_struct *nvme_workq;
+static unsigned int max_host_mem_size_mb = 128;
+module_param(max_host_mem_size_mb, uint, 0444);
+MODULE_PARM_DESC(max_host_mem_size_mb,
+	"Maximum Host Memory Buffer (HMB) size per controller (in MiB)");
 
 struct nvme_dev;
 struct nvme_queue;
 
-static int nvme_reset(struct nvme_dev *dev);
 static void nvme_process_cq(struct nvme_queue *nvmeq);
 static void nvme_dev_disable(struct nvme_dev *dev, bool shutdown);
 
@@ -92,9 +80,8 @@ struct nvme_dev {
 	int q_depth;
 	u32 db_stride;
 	void __iomem *bar;
-	struct work_struct reset_work;
+	unsigned long bar_mapped_size;
 	struct work_struct remove_work;
-	struct timer_list watchdog_timer;
 	struct mutex shutdown_lock;
 	bool subsystem;
 	void __iomem *cmb;
@@ -104,10 +91,18 @@ struct nvme_dev {
 	u32 cmbloc;
 	struct nvme_ctrl ctrl;
 	struct completion ioq_wait;
+
+	/* shadow doorbell buffer support: */
 	u32 *dbbuf_dbs;
 	dma_addr_t dbbuf_dbs_dma_addr;
 	u32 *dbbuf_eis;
 	dma_addr_t dbbuf_eis_dma_addr;
+
+	/* host memory buffer support: */
+	u64 host_mem_size;
+	u32 nr_host_mem_descs;
+	struct nvme_host_mem_buf_desc *host_mem_descs;
+	void **host_mem_desc_bufs;
 };
 
 static inline unsigned int sq_idx(unsigned int qid, u32 stride)
@@ -185,8 +180,8 @@ static inline void _nvme_check_size(void)
 	BUILD_BUG_ON(sizeof(struct nvme_format_cmd) != 64);
 	BUILD_BUG_ON(sizeof(struct nvme_abort_cmd) != 64);
 	BUILD_BUG_ON(sizeof(struct nvme_command) != 64);
-	BUILD_BUG_ON(sizeof(struct nvme_id_ctrl) != 4096);
-	BUILD_BUG_ON(sizeof(struct nvme_id_ns) != 4096);
+	BUILD_BUG_ON(sizeof(struct nvme_id_ctrl) != NVME_IDENTIFY_DATA_SIZE);
+	BUILD_BUG_ON(sizeof(struct nvme_id_ns) != NVME_IDENTIFY_DATA_SIZE);
 	BUILD_BUG_ON(sizeof(struct nvme_lba_range_type) != 64);
 	BUILD_BUG_ON(sizeof(struct nvme_smart_log) != 512);
 	BUILD_BUG_ON(sizeof(struct nvme_dbbuf) != 64);
@@ -350,19 +345,6 @@ static void nvme_admin_exit_hctx(struct blk_mq_hw_ctx *hctx, unsigned int hctx_i
 	nvmeq->tags = NULL;
 }
 
-static int nvme_admin_init_request(struct blk_mq_tag_set *set,
-		struct request *req, unsigned int hctx_idx,
-		unsigned int numa_node)
-{
-	struct nvme_dev *dev = set->driver_data;
-	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
-	struct nvme_queue *nvmeq = dev->queues[0];
-
-	BUG_ON(!nvmeq);
-	iod->nvmeq = nvmeq;
-	return 0;
-}
-
 static int nvme_init_hctx(struct blk_mq_hw_ctx *hctx, void *data,
 			  unsigned int hctx_idx)
 {
@@ -382,7 +364,8 @@ static int nvme_init_request(struct blk_mq_tag_set *set, struct request *req,
 {
 	struct nvme_dev *dev = set->driver_data;
 	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
-	struct nvme_queue *nvmeq = dev->queues[hctx_idx + 1];
+	int queue_idx = (set == &dev->tagset) ? hctx_idx + 1 : 0;
+	struct nvme_queue *nvmeq = dev->queues[queue_idx];
 
 	BUG_ON(!nvmeq);
 	iod->nvmeq = nvmeq;
@@ -427,7 +410,7 @@ static __le64 **iod_list(struct request *req)
 	return (__le64 **)(iod->sg + blk_rq_nr_phys_segments(req));
 }
 
-static int nvme_init_iod(struct request *rq, struct nvme_dev *dev)
+static blk_status_t nvme_init_iod(struct request *rq, struct nvme_dev *dev)
 {
 	struct nvme_iod *iod = blk_mq_rq_to_pdu(rq);
 	int nseg = blk_rq_nr_phys_segments(rq);
@@ -436,7 +419,7 @@ static int nvme_init_iod(struct request *rq, struct nvme_dev *dev)
 	if (nseg > NVME_INT_PAGES || size > NVME_INT_BYTES(dev)) {
 		iod->sg = kmalloc(nvme_iod_alloc_size(dev, size, nseg), GFP_ATOMIC);
 		if (!iod->sg)
-			return BLK_MQ_RQ_QUEUE_BUSY;
+			return BLK_STS_RESOURCE;
 	} else {
 		iod->sg = iod->inline_sg;
 	}
@@ -446,7 +429,7 @@ static int nvme_init_iod(struct request *rq, struct nvme_dev *dev)
 	iod->nents = 0;
 	iod->length = size;
 
-	return BLK_MQ_RQ_QUEUE_OK;
+	return BLK_STS_OK;
 }
 
 static void nvme_free_iod(struct nvme_dev *dev, struct request *req)
@@ -616,21 +599,21 @@ static bool nvme_setup_prps(struct nvme_dev *dev, struct request *req)
 	return true;
 }
 
-static int nvme_map_data(struct nvme_dev *dev, struct request *req,
+static blk_status_t nvme_map_data(struct nvme_dev *dev, struct request *req,
 		struct nvme_command *cmnd)
 {
 	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
 	struct request_queue *q = req->q;
 	enum dma_data_direction dma_dir = rq_data_dir(req) ?
 			DMA_TO_DEVICE : DMA_FROM_DEVICE;
-	int ret = BLK_MQ_RQ_QUEUE_ERROR;
+	blk_status_t ret = BLK_STS_IOERR;
 
 	sg_init_table(iod->sg, blk_rq_nr_phys_segments(req));
 	iod->nents = blk_rq_map_sg(q, req, iod->sg);
 	if (!iod->nents)
 		goto out;
 
-	ret = BLK_MQ_RQ_QUEUE_BUSY;
+	ret = BLK_STS_RESOURCE;
 	if (!dma_map_sg_attrs(dev->dev, iod->sg, iod->nents, dma_dir,
 				DMA_ATTR_NO_WARN))
 		goto out;
@@ -638,7 +621,7 @@ static int nvme_map_data(struct nvme_dev *dev, struct request *req,
 	if (!nvme_setup_prps(dev, req))
 		goto out_unmap;
 
-	ret = BLK_MQ_RQ_QUEUE_ERROR;
+	ret = BLK_STS_IOERR;
 	if (blk_integrity_rq(req)) {
 		if (blk_rq_count_integrity_sg(q, req->bio) != 1)
 			goto out_unmap;
@@ -658,7 +641,7 @@ static int nvme_map_data(struct nvme_dev *dev, struct request *req,
 	cmnd->rw.dptr.prp2 = cpu_to_le64(iod->first_dma);
 	if (blk_integrity_rq(req))
 		cmnd->rw.metadata = cpu_to_le64(sg_dma_address(&iod->meta_sg));
-	return BLK_MQ_RQ_QUEUE_OK;
+	return BLK_STS_OK;
 
 out_unmap:
 	dma_unmap_sg(dev->dev, iod->sg, iod->nents, dma_dir);
@@ -688,7 +671,7 @@ static void nvme_unmap_data(struct nvme_dev *dev, struct request *req)
 /*
  * NOTE: ns is NULL when called on the admin queue.
  */
-static int nvme_queue_rq(struct blk_mq_hw_ctx *hctx,
+static blk_status_t nvme_queue_rq(struct blk_mq_hw_ctx *hctx,
 			 const struct blk_mq_queue_data *bd)
 {
 	struct nvme_ns *ns = hctx->queue->queuedata;
@@ -696,47 +679,34 @@ static int nvme_queue_rq(struct blk_mq_hw_ctx *hctx,
 	struct nvme_dev *dev = nvmeq->dev;
 	struct request *req = bd->rq;
 	struct nvme_command cmnd;
-	int ret = BLK_MQ_RQ_QUEUE_OK;
-
-	/*
-	 * If formated with metadata, require the block layer provide a buffer
-	 * unless this namespace is formated such that the metadata can be
-	 * stripped/generated by the controller with PRACT=1.
-	 */
-	if (ns && ns->ms && !blk_integrity_rq(req)) {
-		if (!(ns->pi_type && ns->ms == 8) &&
-		    !blk_rq_is_passthrough(req)) {
-			blk_mq_end_request(req, -EFAULT);
-			return BLK_MQ_RQ_QUEUE_OK;
-		}
-	}
+	blk_status_t ret;
 
 	ret = nvme_setup_cmd(ns, req, &cmnd);
-	if (ret != BLK_MQ_RQ_QUEUE_OK)
+	if (ret)
 		return ret;
 
 	ret = nvme_init_iod(req, dev);
-	if (ret != BLK_MQ_RQ_QUEUE_OK)
+	if (ret)
 		goto out_free_cmd;
 
-	if (blk_rq_nr_phys_segments(req))
+	if (blk_rq_nr_phys_segments(req)) {
 		ret = nvme_map_data(dev, req, &cmnd);
-
-	if (ret != BLK_MQ_RQ_QUEUE_OK)
-		goto out_cleanup_iod;
+		if (ret)
+			goto out_cleanup_iod;
+	}
 
 	blk_mq_start_request(req);
 
 	spin_lock_irq(&nvmeq->q_lock);
 	if (unlikely(nvmeq->cq_vector < 0)) {
-		ret = BLK_MQ_RQ_QUEUE_ERROR;
+		ret = BLK_STS_IOERR;
 		spin_unlock_irq(&nvmeq->q_lock);
 		goto out_cleanup_iod;
 	}
 	__nvme_submit_cmd(nvmeq, &cmnd);
 	nvme_process_cq(nvmeq);
 	spin_unlock_irq(&nvmeq->q_lock);
-	return BLK_MQ_RQ_QUEUE_OK;
+	return BLK_STS_OK;
 out_cleanup_iod:
 	nvme_free_iod(dev, req);
 out_free_cmd:
@@ -759,65 +729,75 @@ static inline bool nvme_cqe_valid(struct nvme_queue *nvmeq, u16 head,
 	return (le16_to_cpu(nvmeq->cqes[head].status) & 1) == phase;
 }
 
-static void __nvme_process_cq(struct nvme_queue *nvmeq, unsigned int *tag)
+static inline void nvme_ring_cq_doorbell(struct nvme_queue *nvmeq)
 {
-	u16 head, phase;
-
-	head = nvmeq->cq_head;
-	phase = nvmeq->cq_phase;
-
-	while (nvme_cqe_valid(nvmeq, head, phase)) {
-		struct nvme_completion cqe = nvmeq->cqes[head];
-		struct request *req;
-
-		if (++head == nvmeq->q_depth) {
-			head = 0;
-			phase = !phase;
-		}
-
-		if (tag && *tag == cqe.command_id)
-			*tag = -1;
+	u16 head = nvmeq->cq_head;
 
-		if (unlikely(cqe.command_id >= nvmeq->q_depth)) {
-			dev_warn(nvmeq->dev->ctrl.device,
-				"invalid id %d completed on queue %d\n",
-				cqe.command_id, le16_to_cpu(cqe.sq_id));
-			continue;
-		}
+	if (likely(nvmeq->cq_vector >= 0)) {
+		if (nvme_dbbuf_update_and_check_event(head, nvmeq->dbbuf_cq_db,
+						      nvmeq->dbbuf_cq_ei))
+			writel(head, nvmeq->q_db + nvmeq->dev->db_stride);
+	}
+}
 
-		/*
-		 * AEN requests are special as they don't time out and can
-		 * survive any kind of queue freeze and often don't respond to
-		 * aborts.  We don't even bother to allocate a struct request
-		 * for them but rather special case them here.
-		 */
-		if (unlikely(nvmeq->qid == 0 &&
-				cqe.command_id >= NVME_AQ_BLKMQ_DEPTH)) {
-			nvme_complete_async_event(&nvmeq->dev->ctrl,
-					cqe.status, &cqe.result);
-			continue;
-		}
+static inline void nvme_handle_cqe(struct nvme_queue *nvmeq,
+		struct nvme_completion *cqe)
+{
+	struct request *req;
 
-		req = blk_mq_tag_to_rq(*nvmeq->tags, cqe.command_id);
-		nvme_end_request(req, cqe.status, cqe.result);
+	if (unlikely(cqe->command_id >= nvmeq->q_depth)) {
+		dev_warn(nvmeq->dev->ctrl.device,
+			"invalid id %d completed on queue %d\n",
+			cqe->command_id, le16_to_cpu(cqe->sq_id));
+		return;
 	}
 
-	if (head == nvmeq->cq_head && phase == nvmeq->cq_phase)
+	/*
+	 * AEN requests are special as they don't time out and can
+	 * survive any kind of queue freeze and often don't respond to
+	 * aborts.  We don't even bother to allocate a struct request
+	 * for them but rather special case them here.
+	 */
+	if (unlikely(nvmeq->qid == 0 &&
+			cqe->command_id >= NVME_AQ_BLKMQ_DEPTH)) {
+		nvme_complete_async_event(&nvmeq->dev->ctrl,
+				cqe->status, &cqe->result);
 		return;
+	}
 
-	if (likely(nvmeq->cq_vector >= 0))
-		if (nvme_dbbuf_update_and_check_event(head, nvmeq->dbbuf_cq_db,
-						      nvmeq->dbbuf_cq_ei))
-			writel(head, nvmeq->q_db + nvmeq->dev->db_stride);
-	nvmeq->cq_head = head;
-	nvmeq->cq_phase = phase;
+	req = blk_mq_tag_to_rq(*nvmeq->tags, cqe->command_id);
+	nvme_end_request(req, cqe->status, cqe->result);
+}
 
-	nvmeq->cqe_seen = 1;
+static inline bool nvme_read_cqe(struct nvme_queue *nvmeq,
+		struct nvme_completion *cqe)
+{
+	if (nvme_cqe_valid(nvmeq, nvmeq->cq_head, nvmeq->cq_phase)) {
+		*cqe = nvmeq->cqes[nvmeq->cq_head];
+
+		if (++nvmeq->cq_head == nvmeq->q_depth) {
+			nvmeq->cq_head = 0;
+			nvmeq->cq_phase = !nvmeq->cq_phase;
+		}
+		return true;
+	}
+	return false;
 }
 
 static void nvme_process_cq(struct nvme_queue *nvmeq)
 {
-	__nvme_process_cq(nvmeq, NULL);
+	struct nvme_completion cqe;
+	int consumed = 0;
+
+	while (nvme_read_cqe(nvmeq, &cqe)) {
+		nvme_handle_cqe(nvmeq, &cqe);
+		consumed++;
+	}
+
+	if (consumed) {
+		nvme_ring_cq_doorbell(nvmeq);
+		nvmeq->cqe_seen = 1;
+	}
 }
 
 static irqreturn_t nvme_irq(int irq, void *data)
@@ -842,16 +822,28 @@ static irqreturn_t nvme_irq_check(int irq, void *data)
 
 static int __nvme_poll(struct nvme_queue *nvmeq, unsigned int tag)
 {
-	if (nvme_cqe_valid(nvmeq, nvmeq->cq_head, nvmeq->cq_phase)) {
-		spin_lock_irq(&nvmeq->q_lock);
-		__nvme_process_cq(nvmeq, &tag);
-		spin_unlock_irq(&nvmeq->q_lock);
+	struct nvme_completion cqe;
+	int found = 0, consumed = 0;
 
-		if (tag == -1)
-			return 1;
-	}
+	if (!nvme_cqe_valid(nvmeq, nvmeq->cq_head, nvmeq->cq_phase))
+		return 0;
 
-	return 0;
+	spin_lock_irq(&nvmeq->q_lock);
+	while (nvme_read_cqe(nvmeq, &cqe)) {
+		nvme_handle_cqe(nvmeq, &cqe);
+		consumed++;
+
+		if (tag == cqe.command_id) {
+			found = 1;
+			break;
+		}
+       }
+
+	if (consumed)
+		nvme_ring_cq_doorbell(nvmeq);
+	spin_unlock_irq(&nvmeq->q_lock);
+
+	return found;
 }
 
 static int nvme_poll(struct blk_mq_hw_ctx *hctx, unsigned int tag)
@@ -939,7 +931,7 @@ static int adapter_delete_sq(struct nvme_dev *dev, u16 sqid)
 	return adapter_delete_queue(dev, nvme_admin_delete_sq, sqid);
 }
 
-static void abort_endio(struct request *req, int error)
+static void abort_endio(struct request *req, blk_status_t error)
 {
 	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
 	struct nvme_queue *nvmeq = iod->nvmeq;
@@ -950,6 +942,51 @@ static void abort_endio(struct request *req, int error)
 	blk_mq_free_request(req);
 }
 
+static bool nvme_should_reset(struct nvme_dev *dev, u32 csts)
+{
+
+	/* If true, indicates loss of adapter communication, possibly by a
+	 * NVMe Subsystem reset.
+	 */
+	bool nssro = dev->subsystem && (csts & NVME_CSTS_NSSRO);
+
+	/* If there is a reset ongoing, we shouldn't reset again. */
+	if (dev->ctrl.state == NVME_CTRL_RESETTING)
+		return false;
+
+	/* We shouldn't reset unless the controller is on fatal error state
+	 * _or_ if we lost the communication with it.
+	 */
+	if (!(csts & NVME_CSTS_CFS) && !nssro)
+		return false;
+
+	/* If PCI error recovery process is happening, we cannot reset or
+	 * the recovery mechanism will surely fail.
+	 */
+	if (pci_channel_offline(to_pci_dev(dev->dev)))
+		return false;
+
+	return true;
+}
+
+static void nvme_warn_reset(struct nvme_dev *dev, u32 csts)
+{
+	/* Read a config register to help see what died. */
+	u16 pci_status;
+	int result;
+
+	result = pci_read_config_word(to_pci_dev(dev->dev), PCI_STATUS,
+				      &pci_status);
+	if (result == PCIBIOS_SUCCESSFUL)
+		dev_warn(dev->ctrl.device,
+			 "controller is down; will reset: CSTS=0x%x, PCI_STATUS=0x%hx\n",
+			 csts, pci_status);
+	else
+		dev_warn(dev->ctrl.device,
+			 "controller is down; will reset: CSTS=0x%x, PCI_STATUS read failed (%d)\n",
+			 csts, result);
+}
+
 static enum blk_eh_timer_return nvme_timeout(struct request *req, bool reserved)
 {
 	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
@@ -957,6 +994,17 @@ static enum blk_eh_timer_return nvme_timeout(struct request *req, bool reserved)
 	struct nvme_dev *dev = nvmeq->dev;
 	struct request *abort_req;
 	struct nvme_command cmd;
+	u32 csts = readl(dev->bar + NVME_REG_CSTS);
+
+	/*
+	 * Reset immediately if the controller is failed
+	 */
+	if (nvme_should_reset(dev, csts)) {
+		nvme_warn_reset(dev, csts);
+		nvme_dev_disable(dev, false);
+		nvme_reset_ctrl(&dev->ctrl);
+		return BLK_EH_HANDLED;
+	}
 
 	/*
 	 * Did we miss an interrupt?
@@ -993,7 +1041,7 @@ static enum blk_eh_timer_return nvme_timeout(struct request *req, bool reserved)
 			 "I/O %d QID %d timeout, reset controller\n",
 			 req->tag, nvmeq->qid);
 		nvme_dev_disable(dev, false);
-		nvme_reset(dev);
+		nvme_reset_ctrl(&dev->ctrl);
 
 		/*
 		 * Mark the request as handled, since the inline shutdown
@@ -1247,7 +1295,7 @@ static const struct blk_mq_ops nvme_mq_admin_ops = {
 	.complete	= nvme_pci_complete_rq,
 	.init_hctx	= nvme_admin_init_hctx,
 	.exit_hctx      = nvme_admin_exit_hctx,
-	.init_request	= nvme_admin_init_request,
+	.init_request	= nvme_init_request,
 	.timeout	= nvme_timeout,
 };
 
@@ -1311,6 +1359,32 @@ static int nvme_alloc_admin_tags(struct nvme_dev *dev)
 	return 0;
 }
 
+static unsigned long db_bar_size(struct nvme_dev *dev, unsigned nr_io_queues)
+{
+	return NVME_REG_DBS + ((nr_io_queues + 1) * 8 * dev->db_stride);
+}
+
+static int nvme_remap_bar(struct nvme_dev *dev, unsigned long size)
+{
+	struct pci_dev *pdev = to_pci_dev(dev->dev);
+
+	if (size <= dev->bar_mapped_size)
+		return 0;
+	if (size > pci_resource_len(pdev, 0))
+		return -ENOMEM;
+	if (dev->bar)
+		iounmap(dev->bar);
+	dev->bar = ioremap(pci_resource_start(pdev, 0), size);
+	if (!dev->bar) {
+		dev->bar_mapped_size = 0;
+		return -ENOMEM;
+	}
+	dev->bar_mapped_size = size;
+	dev->dbs = dev->bar + NVME_REG_DBS;
+
+	return 0;
+}
+
 static int nvme_configure_admin_queue(struct nvme_dev *dev)
 {
 	int result;
@@ -1318,6 +1392,10 @@ static int nvme_configure_admin_queue(struct nvme_dev *dev)
 	u64 cap = lo_hi_readq(dev->bar + NVME_REG_CAP);
 	struct nvme_queue *nvmeq;
 
+	result = nvme_remap_bar(dev, db_bar_size(dev, 0));
+	if (result < 0)
+		return result;
+
 	dev->subsystem = readl(dev->bar + NVME_REG_VS) >= NVME_VS(1, 1, 0) ?
 						NVME_CAP_NSSRC(cap) : 0;
 
@@ -1358,66 +1436,6 @@ static int nvme_configure_admin_queue(struct nvme_dev *dev)
 	return result;
 }
 
-static bool nvme_should_reset(struct nvme_dev *dev, u32 csts)
-{
-
-	/* If true, indicates loss of adapter communication, possibly by a
-	 * NVMe Subsystem reset.
-	 */
-	bool nssro = dev->subsystem && (csts & NVME_CSTS_NSSRO);
-
-	/* If there is a reset ongoing, we shouldn't reset again. */
-	if (dev->ctrl.state == NVME_CTRL_RESETTING)
-		return false;
-
-	/* We shouldn't reset unless the controller is on fatal error state
-	 * _or_ if we lost the communication with it.
-	 */
-	if (!(csts & NVME_CSTS_CFS) && !nssro)
-		return false;
-
-	/* If PCI error recovery process is happening, we cannot reset or
-	 * the recovery mechanism will surely fail.
-	 */
-	if (pci_channel_offline(to_pci_dev(dev->dev)))
-		return false;
-
-	return true;
-}
-
-static void nvme_warn_reset(struct nvme_dev *dev, u32 csts)
-{
-	/* Read a config register to help see what died. */
-	u16 pci_status;
-	int result;
-
-	result = pci_read_config_word(to_pci_dev(dev->dev), PCI_STATUS,
-				      &pci_status);
-	if (result == PCIBIOS_SUCCESSFUL)
-		dev_warn(dev->ctrl.device,
-			 "controller is down; will reset: CSTS=0x%x, PCI_STATUS=0x%hx\n",
-			 csts, pci_status);
-	else
-		dev_warn(dev->ctrl.device,
-			 "controller is down; will reset: CSTS=0x%x, PCI_STATUS read failed (%d)\n",
-			 csts, result);
-}
-
-static void nvme_watchdog_timer(unsigned long data)
-{
-	struct nvme_dev *dev = (struct nvme_dev *)data;
-	u32 csts = readl(dev->bar + NVME_REG_CSTS);
-
-	/* Skip controllers under certain specific conditions. */
-	if (nvme_should_reset(dev, csts)) {
-		if (!nvme_reset(dev))
-			nvme_warn_reset(dev, csts);
-		return;
-	}
-
-	mod_timer(&dev->watchdog_timer, round_jiffies(jiffies + HZ));
-}
-
 static int nvme_create_io_queues(struct nvme_dev *dev)
 {
 	unsigned i, max;
@@ -1514,18 +1532,170 @@ static inline void nvme_release_cmb(struct nvme_dev *dev)
 	}
 }
 
-static size_t db_bar_size(struct nvme_dev *dev, unsigned nr_io_queues)
+static int nvme_set_host_mem(struct nvme_dev *dev, u32 bits)
+{
+	size_t len = dev->nr_host_mem_descs * sizeof(*dev->host_mem_descs);
+	struct nvme_command c;
+	u64 dma_addr;
+	int ret;
+
+	dma_addr = dma_map_single(dev->dev, dev->host_mem_descs, len,
+			DMA_TO_DEVICE);
+	if (dma_mapping_error(dev->dev, dma_addr))
+		return -ENOMEM;
+
+	memset(&c, 0, sizeof(c));
+	c.features.opcode	= nvme_admin_set_features;
+	c.features.fid		= cpu_to_le32(NVME_FEAT_HOST_MEM_BUF);
+	c.features.dword11	= cpu_to_le32(bits);
+	c.features.dword12	= cpu_to_le32(dev->host_mem_size >>
+					      ilog2(dev->ctrl.page_size));
+	c.features.dword13	= cpu_to_le32(lower_32_bits(dma_addr));
+	c.features.dword14	= cpu_to_le32(upper_32_bits(dma_addr));
+	c.features.dword15	= cpu_to_le32(dev->nr_host_mem_descs);
+
+	ret = nvme_submit_sync_cmd(dev->ctrl.admin_q, &c, NULL, 0);
+	if (ret) {
+		dev_warn(dev->ctrl.device,
+			 "failed to set host mem (err %d, flags %#x).\n",
+			 ret, bits);
+	}
+	dma_unmap_single(dev->dev, dma_addr, len, DMA_TO_DEVICE);
+	return ret;
+}
+
+static void nvme_free_host_mem(struct nvme_dev *dev)
+{
+	int i;
+
+	for (i = 0; i < dev->nr_host_mem_descs; i++) {
+		struct nvme_host_mem_buf_desc *desc = &dev->host_mem_descs[i];
+		size_t size = le32_to_cpu(desc->size) * dev->ctrl.page_size;
+
+		dma_free_coherent(dev->dev, size, dev->host_mem_desc_bufs[i],
+				le64_to_cpu(desc->addr));
+	}
+
+	kfree(dev->host_mem_desc_bufs);
+	dev->host_mem_desc_bufs = NULL;
+	kfree(dev->host_mem_descs);
+	dev->host_mem_descs = NULL;
+}
+
+static int nvme_alloc_host_mem(struct nvme_dev *dev, u64 min, u64 preferred)
 {
-	return 4096 + ((nr_io_queues + 1) * 8 * dev->db_stride);
+	struct nvme_host_mem_buf_desc *descs;
+	u32 chunk_size, max_entries, i = 0;
+	void **bufs;
+	u64 size, tmp;
+
+	/* start big and work our way down */
+	chunk_size = min(preferred, (u64)PAGE_SIZE << MAX_ORDER);
+retry:
+	tmp = (preferred + chunk_size - 1);
+	do_div(tmp, chunk_size);
+	max_entries = tmp;
+	descs = kcalloc(max_entries, sizeof(*descs), GFP_KERNEL);
+	if (!descs)
+		goto out;
+
+	bufs = kcalloc(max_entries, sizeof(*bufs), GFP_KERNEL);
+	if (!bufs)
+		goto out_free_descs;
+
+	for (size = 0; size < preferred; size += chunk_size) {
+		u32 len = min_t(u64, chunk_size, preferred - size);
+		dma_addr_t dma_addr;
+
+		bufs[i] = dma_alloc_attrs(dev->dev, len, &dma_addr, GFP_KERNEL,
+				DMA_ATTR_NO_KERNEL_MAPPING | DMA_ATTR_NO_WARN);
+		if (!bufs[i])
+			break;
+
+		descs[i].addr = cpu_to_le64(dma_addr);
+		descs[i].size = cpu_to_le32(len / dev->ctrl.page_size);
+		i++;
+	}
+
+	if (!size || (min && size < min)) {
+		dev_warn(dev->ctrl.device,
+			"failed to allocate host memory buffer.\n");
+		goto out_free_bufs;
+	}
+
+	dev_info(dev->ctrl.device,
+		"allocated %lld MiB host memory buffer.\n",
+		size >> ilog2(SZ_1M));
+	dev->nr_host_mem_descs = i;
+	dev->host_mem_size = size;
+	dev->host_mem_descs = descs;
+	dev->host_mem_desc_bufs = bufs;
+	return 0;
+
+out_free_bufs:
+	while (--i >= 0) {
+		size_t size = le32_to_cpu(descs[i].size) * dev->ctrl.page_size;
+
+		dma_free_coherent(dev->dev, size, bufs[i],
+				le64_to_cpu(descs[i].addr));
+	}
+
+	kfree(bufs);
+out_free_descs:
+	kfree(descs);
+out:
+	/* try a smaller chunk size if we failed early */
+	if (chunk_size >= PAGE_SIZE * 2 && (i == 0 || size < min)) {
+		chunk_size /= 2;
+		goto retry;
+	}
+	dev->host_mem_descs = NULL;
+	return -ENOMEM;
+}
+
+static void nvme_setup_host_mem(struct nvme_dev *dev)
+{
+	u64 max = (u64)max_host_mem_size_mb * SZ_1M;
+	u64 preferred = (u64)dev->ctrl.hmpre * 4096;
+	u64 min = (u64)dev->ctrl.hmmin * 4096;
+	u32 enable_bits = NVME_HOST_MEM_ENABLE;
+
+	preferred = min(preferred, max);
+	if (min > max) {
+		dev_warn(dev->ctrl.device,
+			"min host memory (%lld MiB) above limit (%d MiB).\n",
+			min >> ilog2(SZ_1M), max_host_mem_size_mb);
+		nvme_free_host_mem(dev);
+		return;
+	}
+
+	/*
+	 * If we already have a buffer allocated check if we can reuse it.
+	 */
+	if (dev->host_mem_descs) {
+		if (dev->host_mem_size >= min)
+			enable_bits |= NVME_HOST_MEM_RETURN;
+		else
+			nvme_free_host_mem(dev);
+	}
+
+	if (!dev->host_mem_descs) {
+		if (nvme_alloc_host_mem(dev, min, preferred))
+			return;
+	}
+
+	if (nvme_set_host_mem(dev, enable_bits))
+		nvme_free_host_mem(dev);
 }
 
 static int nvme_setup_io_queues(struct nvme_dev *dev)
 {
 	struct nvme_queue *adminq = dev->queues[0];
 	struct pci_dev *pdev = to_pci_dev(dev->dev);
-	int result, nr_io_queues, size;
+	int result, nr_io_queues;
+	unsigned long size;
 
-	nr_io_queues = num_online_cpus();
+	nr_io_queues = num_present_cpus();
 	result = nvme_set_queue_count(&dev->ctrl, &nr_io_queues);
 	if (result < 0)
 		return result;
@@ -1542,20 +1712,15 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
 			nvme_release_cmb(dev);
 	}
 
-	size = db_bar_size(dev, nr_io_queues);
-	if (size > 8192) {
-		iounmap(dev->bar);
-		do {
-			dev->bar = ioremap(pci_resource_start(pdev, 0), size);
-			if (dev->bar)
-				break;
-			if (!--nr_io_queues)
-				return -ENOMEM;
-			size = db_bar_size(dev, nr_io_queues);
-		} while (1);
-		dev->dbs = dev->bar + 4096;
-		adminq->q_db = dev->dbs;
-	}
+	do {
+		size = db_bar_size(dev, nr_io_queues);
+		result = nvme_remap_bar(dev, size);
+		if (!result)
+			break;
+		if (!--nr_io_queues)
+			return -ENOMEM;
+	} while (1);
+	adminq->q_db = dev->dbs;
 
 	/* Deregister the admin queue's interrupt */
 	pci_free_irq(pdev, 0, adminq);
@@ -1586,7 +1751,7 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
 	return nvme_create_io_queues(dev);
 }
 
-static void nvme_del_queue_end(struct request *req, int error)
+static void nvme_del_queue_end(struct request *req, blk_status_t error)
 {
 	struct nvme_queue *nvmeq = req->end_io_data;
 
@@ -1594,7 +1759,7 @@ static void nvme_del_queue_end(struct request *req, int error)
 	complete(&nvmeq->dev->ioq_wait);
 }
 
-static void nvme_del_cq_end(struct request *req, int error)
+static void nvme_del_cq_end(struct request *req, blk_status_t error)
 {
 	struct nvme_queue *nvmeq = req->end_io_data;
 
@@ -1799,13 +1964,12 @@ static void nvme_dev_disable(struct nvme_dev *dev, bool shutdown)
 	bool dead = true;
 	struct pci_dev *pdev = to_pci_dev(dev->dev);
 
-	del_timer_sync(&dev->watchdog_timer);
-
 	mutex_lock(&dev->shutdown_lock);
 	if (pci_is_enabled(pdev)) {
 		u32 csts = readl(dev->bar + NVME_REG_CSTS);
 
-		if (dev->ctrl.state == NVME_CTRL_LIVE)
+		if (dev->ctrl.state == NVME_CTRL_LIVE ||
+		    dev->ctrl.state == NVME_CTRL_RESETTING)
 			nvme_start_freeze(&dev->ctrl);
 		dead = !!((csts & NVME_CSTS_CFS) || !(csts & NVME_CSTS_RDY) ||
 			pdev->error_state  != pci_channel_io_normal);
@@ -1815,8 +1979,20 @@ static void nvme_dev_disable(struct nvme_dev *dev, bool shutdown)
 	 * Give the controller a chance to complete all entered requests if
 	 * doing a safe shutdown.
 	 */
-	if (!dead && shutdown)
-		nvme_wait_freeze_timeout(&dev->ctrl, NVME_IO_TIMEOUT);
+	if (!dead) {
+		if (shutdown)
+			nvme_wait_freeze_timeout(&dev->ctrl, NVME_IO_TIMEOUT);
+
+		/*
+		 * If the controller is still alive tell it to stop using the
+		 * host memory buffer.  In theory the shutdown / reset should
+		 * make sure that it doesn't access the host memoery anymore,
+		 * but I'd rather be safe than sorry..
+		 */
+		if (dev->host_mem_descs)
+			nvme_set_host_mem(dev, 0);
+
+	}
 	nvme_stop_queues(&dev->ctrl);
 
 	queues = dev->online_queues - 1;
@@ -1899,7 +2075,8 @@ static void nvme_remove_dead_ctrl(struct nvme_dev *dev, int status)
 
 static void nvme_reset_work(struct work_struct *work)
 {
-	struct nvme_dev *dev = container_of(work, struct nvme_dev, reset_work);
+	struct nvme_dev *dev =
+		container_of(work, struct nvme_dev, ctrl.reset_work);
 	bool was_suspend = !!(dev->ctrl.ctrl_config & NVME_CC_SHN_NORMAL);
 	int result = -ENODEV;
 
@@ -1948,6 +2125,9 @@ static void nvme_reset_work(struct work_struct *work)
 				 "unable to allocate dma for dbbuf\n");
 	}
 
+	if (dev->ctrl.hmpre)
+		nvme_setup_host_mem(dev);
+
 	result = nvme_setup_io_queues(dev);
 	if (result)
 		goto out;
@@ -1961,8 +2141,6 @@ static void nvme_reset_work(struct work_struct *work)
 	if (dev->online_queues > 1)
 		nvme_queue_async_events(&dev->ctrl);
 
-	mod_timer(&dev->watchdog_timer, round_jiffies(jiffies + HZ));
-
 	/*
 	 * Keep the controller around but remove all namespaces if we don't have
 	 * any working I/O queue.
@@ -2002,17 +2180,6 @@ static void nvme_remove_dead_ctrl_work(struct work_struct *work)
 	nvme_put_ctrl(&dev->ctrl);
 }
 
-static int nvme_reset(struct nvme_dev *dev)
-{
-	if (!dev->ctrl.admin_q || blk_queue_dying(dev->ctrl.admin_q))
-		return -ENODEV;
-	if (!nvme_change_ctrl_state(&dev->ctrl, NVME_CTRL_RESETTING))
-		return -EBUSY;
-	if (!queue_work(nvme_workq, &dev->reset_work))
-		return -EBUSY;
-	return 0;
-}
-
 static int nvme_pci_reg_read32(struct nvme_ctrl *ctrl, u32 off, u32 *val)
 {
 	*val = readl(to_nvme_dev(ctrl)->bar + off);
@@ -2031,16 +2198,6 @@ static int nvme_pci_reg_read64(struct nvme_ctrl *ctrl, u32 off, u64 *val)
 	return 0;
 }
 
-static int nvme_pci_reset_ctrl(struct nvme_ctrl *ctrl)
-{
-	struct nvme_dev *dev = to_nvme_dev(ctrl);
-	int ret = nvme_reset(dev);
-
-	if (!ret)
-		flush_work(&dev->reset_work);
-	return ret;
-}
-
 static const struct nvme_ctrl_ops nvme_pci_ctrl_ops = {
 	.name			= "pcie",
 	.module			= THIS_MODULE,
@@ -2048,7 +2205,6 @@ static const struct nvme_ctrl_ops nvme_pci_ctrl_ops = {
 	.reg_read32		= nvme_pci_reg_read32,
 	.reg_write32		= nvme_pci_reg_write32,
 	.reg_read64		= nvme_pci_reg_read64,
-	.reset_ctrl		= nvme_pci_reset_ctrl,
 	.free_ctrl		= nvme_pci_free_ctrl,
 	.submit_async_event	= nvme_pci_submit_async_event,
 };
@@ -2060,8 +2216,7 @@ static int nvme_dev_map(struct nvme_dev *dev)
 	if (pci_request_mem_regions(pdev, "nvme"))
 		return -ENODEV;
 
-	dev->bar = ioremap(pci_resource_start(pdev, 0), 8192);
-	if (!dev->bar)
+	if (nvme_remap_bar(dev, NVME_REG_DBS + 4096))
 		goto release;
 
 	return 0;
@@ -2115,10 +2270,8 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (result)
 		goto free;
 
-	INIT_WORK(&dev->reset_work, nvme_reset_work);
+	INIT_WORK(&dev->ctrl.reset_work, nvme_reset_work);
 	INIT_WORK(&dev->remove_work, nvme_remove_dead_ctrl_work);
-	setup_timer(&dev->watchdog_timer, nvme_watchdog_timer,
-		(unsigned long)dev);
 	mutex_init(&dev->shutdown_lock);
 	init_completion(&dev->ioq_wait);
 
@@ -2136,7 +2289,7 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	nvme_change_ctrl_state(&dev->ctrl, NVME_CTRL_RESETTING);
 	dev_info(dev->ctrl.device, "pci function %s\n", dev_name(&pdev->dev));
 
-	queue_work(nvme_workq, &dev->reset_work);
+	queue_work(nvme_wq, &dev->ctrl.reset_work);
 	return 0;
 
  release_pools:
@@ -2157,7 +2310,7 @@ static void nvme_reset_notify(struct pci_dev *pdev, bool prepare)
 	if (prepare)
 		nvme_dev_disable(dev, false);
 	else
-		nvme_reset(dev);
+		nvme_reset_ctrl(&dev->ctrl);
 }
 
 static void nvme_shutdown(struct pci_dev *pdev)
@@ -2177,7 +2330,7 @@ static void nvme_remove(struct pci_dev *pdev)
 
 	nvme_change_ctrl_state(&dev->ctrl, NVME_CTRL_DELETING);
 
-	cancel_work_sync(&dev->reset_work);
+	cancel_work_sync(&dev->ctrl.reset_work);
 	pci_set_drvdata(pdev, NULL);
 
 	if (!pci_device_is_present(pdev)) {
@@ -2185,9 +2338,10 @@ static void nvme_remove(struct pci_dev *pdev)
 		nvme_dev_disable(dev, false);
 	}
 
-	flush_work(&dev->reset_work);
+	flush_work(&dev->ctrl.reset_work);
 	nvme_uninit_ctrl(&dev->ctrl);
 	nvme_dev_disable(dev, true);
+	nvme_free_host_mem(dev);
 	nvme_dev_remove_admin(dev);
 	nvme_free_queues(dev, 0);
 	nvme_release_prp_pools(dev);
@@ -2228,7 +2382,7 @@ static int nvme_resume(struct device *dev)
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct nvme_dev *ndev = pci_get_drvdata(pdev);
 
-	nvme_reset(ndev);
+	nvme_reset_ctrl(&ndev->ctrl);
 	return 0;
 }
 #endif
@@ -2267,7 +2421,7 @@ static pci_ers_result_t nvme_slot_reset(struct pci_dev *pdev)
 
 	dev_info(dev->ctrl.device, "restart after slot reset\n");
 	pci_restore_state(pdev);
-	nvme_reset(dev);
+	nvme_reset_ctrl(&dev->ctrl);
 	return PCI_ERS_RESULT_RECOVERED;
 }
 
@@ -2323,22 +2477,12 @@ static struct pci_driver nvme_driver = {
 
 static int __init nvme_init(void)
 {
-	int result;
-
-	nvme_workq = alloc_workqueue("nvme", WQ_UNBOUND | WQ_MEM_RECLAIM, 0);
-	if (!nvme_workq)
-		return -ENOMEM;
-
-	result = pci_register_driver(&nvme_driver);
-	if (result)
-		destroy_workqueue(nvme_workq);
-	return result;
+	return pci_register_driver(&nvme_driver);
 }
 
 static void __exit nvme_exit(void)
 {
 	pci_unregister_driver(&nvme_driver);
-	destroy_workqueue(nvme_workq);
 	_nvme_check_size();
 }
 
diff --git a/drivers/nvme/host/rdma.c b/drivers/nvme/host/rdma.c
index 24397d306d53..6d4119dfbdaa 100644
--- a/drivers/nvme/host/rdma.c
+++ b/drivers/nvme/host/rdma.c
@@ -48,7 +48,7 @@
  */
 #define NVME_RDMA_NR_AEN_COMMANDS      1
 #define NVME_RDMA_AQ_BLKMQ_DEPTH       \
-	(NVMF_AQ_DEPTH - NVME_RDMA_NR_AEN_COMMANDS)
+	(NVME_AQ_DEPTH - NVME_RDMA_NR_AEN_COMMANDS)
 
 struct nvme_rdma_device {
 	struct ib_device       *dev;
@@ -80,10 +80,8 @@ struct nvme_rdma_request {
 };
 
 enum nvme_rdma_queue_flags {
-	NVME_RDMA_Q_CONNECTED = (1 << 0),
-	NVME_RDMA_IB_QUEUE_ALLOCATED = (1 << 1),
-	NVME_RDMA_Q_DELETING = (1 << 2),
-	NVME_RDMA_Q_LIVE = (1 << 3),
+	NVME_RDMA_Q_LIVE		= 0,
+	NVME_RDMA_Q_DELETING		= 1,
 };
 
 struct nvme_rdma_queue {
@@ -103,9 +101,6 @@ struct nvme_rdma_queue {
 };
 
 struct nvme_rdma_ctrl {
-	/* read and written in the hot path */
-	spinlock_t		lock;
-
 	/* read only in the hot path */
 	struct nvme_rdma_queue	*queues;
 	u32			queue_count;
@@ -113,7 +108,6 @@ struct nvme_rdma_ctrl {
 	/* other member variables */
 	struct blk_mq_tag_set	tag_set;
 	struct work_struct	delete_work;
-	struct work_struct	reset_work;
 	struct work_struct	err_work;
 
 	struct nvme_rdma_qe	async_event_sqe;
@@ -145,8 +139,6 @@ static DEFINE_MUTEX(device_list_mutex);
 static LIST_HEAD(nvme_rdma_ctrl_list);
 static DEFINE_MUTEX(nvme_rdma_ctrl_mutex);
 
-static struct workqueue_struct *nvme_rdma_wq;
-
 /*
  * Disabling this option makes small I/O goes faster, but is fundamentally
  * unsafe.  With it turned off we will have to register a global rkey that
@@ -301,10 +293,12 @@ out:
 	return ret;
 }
 
-static void __nvme_rdma_exit_request(struct nvme_rdma_ctrl *ctrl,
-		struct request *rq, unsigned int queue_idx)
+static void nvme_rdma_exit_request(struct blk_mq_tag_set *set,
+		struct request *rq, unsigned int hctx_idx)
 {
+	struct nvme_rdma_ctrl *ctrl = set->driver_data;
 	struct nvme_rdma_request *req = blk_mq_rq_to_pdu(rq);
+	int queue_idx = (set == &ctrl->tag_set) ? hctx_idx + 1 : 0;
 	struct nvme_rdma_queue *queue = &ctrl->queues[queue_idx];
 	struct nvme_rdma_device *dev = queue->device;
 
@@ -315,22 +309,13 @@ static void __nvme_rdma_exit_request(struct nvme_rdma_ctrl *ctrl,
 			DMA_TO_DEVICE);
 }
 
-static void nvme_rdma_exit_request(struct blk_mq_tag_set *set,
-		struct request *rq, unsigned int hctx_idx)
-{
-	return __nvme_rdma_exit_request(set->driver_data, rq, hctx_idx + 1);
-}
-
-static void nvme_rdma_exit_admin_request(struct blk_mq_tag_set *set,
-		struct request *rq, unsigned int hctx_idx)
-{
-	return __nvme_rdma_exit_request(set->driver_data, rq, 0);
-}
-
-static int __nvme_rdma_init_request(struct nvme_rdma_ctrl *ctrl,
-		struct request *rq, unsigned int queue_idx)
+static int nvme_rdma_init_request(struct blk_mq_tag_set *set,
+		struct request *rq, unsigned int hctx_idx,
+		unsigned int numa_node)
 {
+	struct nvme_rdma_ctrl *ctrl = set->driver_data;
 	struct nvme_rdma_request *req = blk_mq_rq_to_pdu(rq);
+	int queue_idx = (set == &ctrl->tag_set) ? hctx_idx + 1 : 0;
 	struct nvme_rdma_queue *queue = &ctrl->queues[queue_idx];
 	struct nvme_rdma_device *dev = queue->device;
 	struct ib_device *ibdev = dev->dev;
@@ -358,20 +343,6 @@ out_free_qe:
 	return -ENOMEM;
 }
 
-static int nvme_rdma_init_request(struct blk_mq_tag_set *set,
-		struct request *rq, unsigned int hctx_idx,
-		unsigned int numa_node)
-{
-	return __nvme_rdma_init_request(set->driver_data, rq, hctx_idx + 1);
-}
-
-static int nvme_rdma_init_admin_request(struct blk_mq_tag_set *set,
-		struct request *rq, unsigned int hctx_idx,
-		unsigned int numa_node)
-{
-	return __nvme_rdma_init_request(set->driver_data, rq, 0);
-}
-
 static int nvme_rdma_init_hctx(struct blk_mq_hw_ctx *hctx, void *data,
 		unsigned int hctx_idx)
 {
@@ -469,9 +440,6 @@ static void nvme_rdma_destroy_queue_ib(struct nvme_rdma_queue *queue)
 	struct nvme_rdma_device *dev;
 	struct ib_device *ibdev;
 
-	if (!test_and_clear_bit(NVME_RDMA_IB_QUEUE_ALLOCATED, &queue->flags))
-		return;
-
 	dev = queue->device;
 	ibdev = dev->dev;
 	rdma_destroy_qp(queue->cm_id);
@@ -483,17 +451,21 @@ static void nvme_rdma_destroy_queue_ib(struct nvme_rdma_queue *queue)
 	nvme_rdma_dev_put(dev);
 }
 
-static int nvme_rdma_create_queue_ib(struct nvme_rdma_queue *queue,
-		struct nvme_rdma_device *dev)
+static int nvme_rdma_create_queue_ib(struct nvme_rdma_queue *queue)
 {
-	struct ib_device *ibdev = dev->dev;
+	struct ib_device *ibdev;
 	const int send_wr_factor = 3;			/* MR, SEND, INV */
 	const int cq_factor = send_wr_factor + 1;	/* + RECV */
 	int comp_vector, idx = nvme_rdma_queue_idx(queue);
-
 	int ret;
 
-	queue->device = dev;
+	queue->device = nvme_rdma_find_get_device(queue->cm_id);
+	if (!queue->device) {
+		dev_err(queue->cm_id->device->dev.parent,
+			"no client data found!\n");
+		return -ECONNREFUSED;
+	}
+	ibdev = queue->device->dev;
 
 	/*
 	 * The admin queue is barely used once the controller is live, so don't
@@ -506,12 +478,12 @@ static int nvme_rdma_create_queue_ib(struct nvme_rdma_queue *queue,
 
 
 	/* +1 for ib_stop_cq */
-	queue->ib_cq = ib_alloc_cq(dev->dev, queue,
-				cq_factor * queue->queue_size + 1, comp_vector,
-				IB_POLL_SOFTIRQ);
+	queue->ib_cq = ib_alloc_cq(ibdev, queue,
+				cq_factor * queue->queue_size + 1,
+				comp_vector, IB_POLL_SOFTIRQ);
 	if (IS_ERR(queue->ib_cq)) {
 		ret = PTR_ERR(queue->ib_cq);
-		goto out;
+		goto out_put_dev;
 	}
 
 	ret = nvme_rdma_create_qp(queue, send_wr_factor);
@@ -524,7 +496,6 @@ static int nvme_rdma_create_queue_ib(struct nvme_rdma_queue *queue,
 		ret = -ENOMEM;
 		goto out_destroy_qp;
 	}
-	set_bit(NVME_RDMA_IB_QUEUE_ALLOCATED, &queue->flags);
 
 	return 0;
 
@@ -532,7 +503,8 @@ out_destroy_qp:
 	ib_destroy_qp(queue->qp);
 out_destroy_ib_cq:
 	ib_free_cq(queue->ib_cq);
-out:
+out_put_dev:
+	nvme_rdma_dev_put(queue->device);
 	return ret;
 }
 
@@ -583,12 +555,10 @@ static int nvme_rdma_init_queue(struct nvme_rdma_ctrl *ctrl,
 	}
 
 	clear_bit(NVME_RDMA_Q_DELETING, &queue->flags);
-	set_bit(NVME_RDMA_Q_CONNECTED, &queue->flags);
 
 	return 0;
 
 out_destroy_cm_id:
-	nvme_rdma_destroy_queue_ib(queue);
 	rdma_destroy_id(queue->cm_id);
 	return ret;
 }
@@ -718,11 +688,11 @@ static void nvme_rdma_reconnect_or_remove(struct nvme_rdma_ctrl *ctrl)
 	if (nvmf_should_reconnect(&ctrl->ctrl)) {
 		dev_info(ctrl->ctrl.device, "Reconnecting in %d seconds...\n",
 			ctrl->ctrl.opts->reconnect_delay);
-		queue_delayed_work(nvme_rdma_wq, &ctrl->reconnect_work,
+		queue_delayed_work(nvme_wq, &ctrl->reconnect_work,
 				ctrl->ctrl.opts->reconnect_delay * HZ);
 	} else {
 		dev_info(ctrl->ctrl.device, "Removing controller...\n");
-		queue_work(nvme_rdma_wq, &ctrl->delete_work);
+		queue_work(nvme_wq, &ctrl->delete_work);
 	}
 }
 
@@ -733,7 +703,7 @@ static void nvme_rdma_reconnect_ctrl_work(struct work_struct *work)
 	bool changed;
 	int ret;
 
-	++ctrl->ctrl.opts->nr_reconnects;
+	++ctrl->ctrl.nr_reconnects;
 
 	if (ctrl->queue_count > 1) {
 		nvme_rdma_free_io_queues(ctrl);
@@ -749,7 +719,7 @@ static void nvme_rdma_reconnect_ctrl_work(struct work_struct *work)
 	if (ret)
 		goto requeue;
 
-	ret = nvme_rdma_init_queue(ctrl, 0, NVMF_AQ_DEPTH);
+	ret = nvme_rdma_init_queue(ctrl, 0, NVME_AQ_DEPTH);
 	if (ret)
 		goto requeue;
 
@@ -777,7 +747,7 @@ static void nvme_rdma_reconnect_ctrl_work(struct work_struct *work)
 
 	changed = nvme_change_ctrl_state(&ctrl->ctrl, NVME_CTRL_LIVE);
 	WARN_ON_ONCE(!changed);
-	ctrl->ctrl.opts->nr_reconnects = 0;
+	ctrl->ctrl.nr_reconnects = 0;
 
 	if (ctrl->queue_count > 1) {
 		nvme_queue_scan(&ctrl->ctrl);
@@ -790,7 +760,7 @@ static void nvme_rdma_reconnect_ctrl_work(struct work_struct *work)
 
 requeue:
 	dev_info(ctrl->ctrl.device, "Failed reconnect attempt %d\n",
-			ctrl->ctrl.opts->nr_reconnects);
+			ctrl->ctrl.nr_reconnects);
 	nvme_rdma_reconnect_or_remove(ctrl);
 }
 
@@ -802,10 +772,8 @@ static void nvme_rdma_error_recovery_work(struct work_struct *work)
 
 	nvme_stop_keep_alive(&ctrl->ctrl);
 
-	for (i = 0; i < ctrl->queue_count; i++) {
-		clear_bit(NVME_RDMA_Q_CONNECTED, &ctrl->queues[i].flags);
+	for (i = 0; i < ctrl->queue_count; i++)
 		clear_bit(NVME_RDMA_Q_LIVE, &ctrl->queues[i].flags);
-	}
 
 	if (ctrl->queue_count > 1)
 		nvme_stop_queues(&ctrl->ctrl);
@@ -833,7 +801,7 @@ static void nvme_rdma_error_recovery(struct nvme_rdma_ctrl *ctrl)
 	if (!nvme_change_ctrl_state(&ctrl->ctrl, NVME_CTRL_RECONNECTING))
 		return;
 
-	queue_work(nvme_rdma_wq, &ctrl->err_work);
+	queue_work(nvme_wq, &ctrl->err_work);
 }
 
 static void nvme_rdma_wr_error(struct ib_cq *cq, struct ib_wc *wc,
@@ -1278,21 +1246,11 @@ static int nvme_rdma_conn_rejected(struct nvme_rdma_queue *queue,
 
 static int nvme_rdma_addr_resolved(struct nvme_rdma_queue *queue)
 {
-	struct nvme_rdma_device *dev;
 	int ret;
 
-	dev = nvme_rdma_find_get_device(queue->cm_id);
-	if (!dev) {
-		dev_err(queue->cm_id->device->dev.parent,
-			"no client data found!\n");
-		return -ECONNREFUSED;
-	}
-
-	ret = nvme_rdma_create_queue_ib(queue, dev);
-	if (ret) {
-		nvme_rdma_dev_put(dev);
-		goto out;
-	}
+	ret = nvme_rdma_create_queue_ib(queue);
+	if (ret)
+		return ret;
 
 	ret = rdma_resolve_route(queue->cm_id, NVME_RDMA_CONNECT_TIMEOUT_MS);
 	if (ret) {
@@ -1306,7 +1264,6 @@ static int nvme_rdma_addr_resolved(struct nvme_rdma_queue *queue)
 
 out_destroy_queue:
 	nvme_rdma_destroy_queue_ib(queue);
-out:
 	return ret;
 }
 
@@ -1334,8 +1291,8 @@ static int nvme_rdma_route_resolved(struct nvme_rdma_queue *queue)
 	 * specified by the Fabrics standard.
 	 */
 	if (priv.qid == 0) {
-		priv.hrqsize = cpu_to_le16(NVMF_AQ_DEPTH);
-		priv.hsqsize = cpu_to_le16(NVMF_AQ_DEPTH - 1);
+		priv.hrqsize = cpu_to_le16(NVME_AQ_DEPTH);
+		priv.hsqsize = cpu_to_le16(NVME_AQ_DEPTH - 1);
 	} else {
 		/*
 		 * current interpretation of the fabrics spec
@@ -1383,12 +1340,14 @@ static int nvme_rdma_cm_handler(struct rdma_cm_id *cm_id,
 		complete(&queue->cm_done);
 		return 0;
 	case RDMA_CM_EVENT_REJECTED:
+		nvme_rdma_destroy_queue_ib(queue);
 		cm_error = nvme_rdma_conn_rejected(queue, ev);
 		break;
-	case RDMA_CM_EVENT_ADDR_ERROR:
 	case RDMA_CM_EVENT_ROUTE_ERROR:
 	case RDMA_CM_EVENT_CONNECT_ERROR:
 	case RDMA_CM_EVENT_UNREACHABLE:
+		nvme_rdma_destroy_queue_ib(queue);
+	case RDMA_CM_EVENT_ADDR_ERROR:
 		dev_dbg(queue->ctrl->ctrl.device,
 			"CM error event %d\n", ev->event);
 		cm_error = -ECONNRESET;
@@ -1435,8 +1394,8 @@ nvme_rdma_timeout(struct request *rq, bool reserved)
 /*
  * We cannot accept any other command until the Connect command has completed.
  */
-static inline int nvme_rdma_queue_is_ready(struct nvme_rdma_queue *queue,
-		struct request *rq)
+static inline blk_status_t
+nvme_rdma_queue_is_ready(struct nvme_rdma_queue *queue, struct request *rq)
 {
 	if (unlikely(!test_bit(NVME_RDMA_Q_LIVE, &queue->flags))) {
 		struct nvme_command *cmd = nvme_req(rq)->cmd;
@@ -1452,16 +1411,15 @@ static inline int nvme_rdma_queue_is_ready(struct nvme_rdma_queue *queue,
 			 * failover.
 			 */
 			if (queue->ctrl->ctrl.state == NVME_CTRL_RECONNECTING)
-				return -EIO;
-			else
-				return -EAGAIN;
+				return BLK_STS_IOERR;
+			return BLK_STS_RESOURCE; /* try again later */
 		}
 	}
 
 	return 0;
 }
 
-static int nvme_rdma_queue_rq(struct blk_mq_hw_ctx *hctx,
+static blk_status_t nvme_rdma_queue_rq(struct blk_mq_hw_ctx *hctx,
 		const struct blk_mq_queue_data *bd)
 {
 	struct nvme_ns *ns = hctx->queue->queuedata;
@@ -1472,28 +1430,29 @@ static int nvme_rdma_queue_rq(struct blk_mq_hw_ctx *hctx,
 	struct nvme_command *c = sqe->data;
 	bool flush = false;
 	struct ib_device *dev;
-	int ret;
+	blk_status_t ret;
+	int err;
 
 	WARN_ON_ONCE(rq->tag < 0);
 
 	ret = nvme_rdma_queue_is_ready(queue, rq);
 	if (unlikely(ret))
-		goto err;
+		return ret;
 
 	dev = queue->device->dev;
 	ib_dma_sync_single_for_cpu(dev, sqe->dma,
 			sizeof(struct nvme_command), DMA_TO_DEVICE);
 
 	ret = nvme_setup_cmd(ns, rq, c);
-	if (ret != BLK_MQ_RQ_QUEUE_OK)
+	if (ret)
 		return ret;
 
 	blk_mq_start_request(rq);
 
-	ret = nvme_rdma_map_data(queue, rq, c);
-	if (ret < 0) {
+	err = nvme_rdma_map_data(queue, rq, c);
+	if (err < 0) {
 		dev_err(queue->ctrl->ctrl.device,
-			     "Failed to map data (%d)\n", ret);
+			     "Failed to map data (%d)\n", err);
 		nvme_cleanup_cmd(rq);
 		goto err;
 	}
@@ -1503,17 +1462,18 @@ static int nvme_rdma_queue_rq(struct blk_mq_hw_ctx *hctx,
 
 	if (req_op(rq) == REQ_OP_FLUSH)
 		flush = true;
-	ret = nvme_rdma_post_send(queue, sqe, req->sge, req->num_sge,
+	err = nvme_rdma_post_send(queue, sqe, req->sge, req->num_sge,
 			req->mr->need_inval ? &req->reg_wr.wr : NULL, flush);
-	if (ret) {
+	if (err) {
 		nvme_rdma_unmap_data(queue, rq);
 		goto err;
 	}
 
-	return BLK_MQ_RQ_QUEUE_OK;
+	return BLK_STS_OK;
 err:
-	return (ret == -ENOMEM || ret == -EAGAIN) ?
-		BLK_MQ_RQ_QUEUE_BUSY : BLK_MQ_RQ_QUEUE_ERROR;
+	if (err == -ENOMEM || err == -EAGAIN)
+		return BLK_STS_RESOURCE;
+	return BLK_STS_IOERR;
 }
 
 static int nvme_rdma_poll(struct blk_mq_hw_ctx *hctx, unsigned int tag)
@@ -1523,7 +1483,6 @@ static int nvme_rdma_poll(struct blk_mq_hw_ctx *hctx, unsigned int tag)
 	struct ib_wc wc;
 	int found = 0;
 
-	ib_req_notify_cq(cq, IB_CQ_NEXT_COMP);
 	while (ib_poll_cq(cq, 1, &wc) > 0) {
 		struct ib_cqe *cqe = wc.wr_cqe;
 
@@ -1560,8 +1519,8 @@ static const struct blk_mq_ops nvme_rdma_mq_ops = {
 static const struct blk_mq_ops nvme_rdma_admin_mq_ops = {
 	.queue_rq	= nvme_rdma_queue_rq,
 	.complete	= nvme_rdma_complete_rq,
-	.init_request	= nvme_rdma_init_admin_request,
-	.exit_request	= nvme_rdma_exit_admin_request,
+	.init_request	= nvme_rdma_init_request,
+	.exit_request	= nvme_rdma_exit_request,
 	.reinit_request	= nvme_rdma_reinit_request,
 	.init_hctx	= nvme_rdma_init_admin_hctx,
 	.timeout	= nvme_rdma_timeout,
@@ -1571,7 +1530,7 @@ static int nvme_rdma_configure_admin_queue(struct nvme_rdma_ctrl *ctrl)
 {
 	int error;
 
-	error = nvme_rdma_init_queue(ctrl, 0, NVMF_AQ_DEPTH);
+	error = nvme_rdma_init_queue(ctrl, 0, NVME_AQ_DEPTH);
 	if (error)
 		return error;
 
@@ -1672,7 +1631,7 @@ static void nvme_rdma_shutdown_ctrl(struct nvme_rdma_ctrl *ctrl)
 		nvme_rdma_free_io_queues(ctrl);
 	}
 
-	if (test_bit(NVME_RDMA_Q_CONNECTED, &ctrl->queues[0].flags))
+	if (test_bit(NVME_RDMA_Q_LIVE, &ctrl->queues[0].flags))
 		nvme_shutdown_ctrl(&ctrl->ctrl);
 
 	blk_mq_stop_hw_queues(ctrl->ctrl.admin_q);
@@ -1709,7 +1668,7 @@ static int __nvme_rdma_del_ctrl(struct nvme_rdma_ctrl *ctrl)
 	if (!nvme_change_ctrl_state(&ctrl->ctrl, NVME_CTRL_DELETING))
 		return -EBUSY;
 
-	if (!queue_work(nvme_rdma_wq, &ctrl->delete_work))
+	if (!queue_work(nvme_wq, &ctrl->delete_work))
 		return -EBUSY;
 
 	return 0;
@@ -1743,8 +1702,8 @@ static void nvme_rdma_remove_ctrl_work(struct work_struct *work)
 
 static void nvme_rdma_reset_ctrl_work(struct work_struct *work)
 {
-	struct nvme_rdma_ctrl *ctrl = container_of(work,
-					struct nvme_rdma_ctrl, reset_work);
+	struct nvme_rdma_ctrl *ctrl =
+		container_of(work, struct nvme_rdma_ctrl, ctrl.reset_work);
 	int ret;
 	bool changed;
 
@@ -1785,22 +1744,7 @@ static void nvme_rdma_reset_ctrl_work(struct work_struct *work)
 del_dead_ctrl:
 	/* Deleting this dead controller... */
 	dev_warn(ctrl->ctrl.device, "Removing after reset failure\n");
-	WARN_ON(!queue_work(nvme_rdma_wq, &ctrl->delete_work));
-}
-
-static int nvme_rdma_reset_ctrl(struct nvme_ctrl *nctrl)
-{
-	struct nvme_rdma_ctrl *ctrl = to_rdma_ctrl(nctrl);
-
-	if (!nvme_change_ctrl_state(&ctrl->ctrl, NVME_CTRL_RESETTING))
-		return -EBUSY;
-
-	if (!queue_work(nvme_rdma_wq, &ctrl->reset_work))
-		return -EBUSY;
-
-	flush_work(&ctrl->reset_work);
-
-	return 0;
+	WARN_ON(!queue_work(nvme_wq, &ctrl->delete_work));
 }
 
 static const struct nvme_ctrl_ops nvme_rdma_ctrl_ops = {
@@ -1810,11 +1754,9 @@ static const struct nvme_ctrl_ops nvme_rdma_ctrl_ops = {
 	.reg_read32		= nvmf_reg_read32,
 	.reg_read64		= nvmf_reg_read64,
 	.reg_write32		= nvmf_reg_write32,
-	.reset_ctrl		= nvme_rdma_reset_ctrl,
 	.free_ctrl		= nvme_rdma_free_ctrl,
 	.submit_async_event	= nvme_rdma_submit_async_event,
 	.delete_ctrl		= nvme_rdma_del_ctrl,
-	.get_subsysnqn		= nvmf_get_subsysnqn,
 	.get_address		= nvmf_get_address,
 };
 
@@ -1919,8 +1861,7 @@ static struct nvme_ctrl *nvme_rdma_create_ctrl(struct device *dev,
 			nvme_rdma_reconnect_ctrl_work);
 	INIT_WORK(&ctrl->err_work, nvme_rdma_error_recovery_work);
 	INIT_WORK(&ctrl->delete_work, nvme_rdma_del_ctrl_work);
-	INIT_WORK(&ctrl->reset_work, nvme_rdma_reset_ctrl_work);
-	spin_lock_init(&ctrl->lock);
+	INIT_WORK(&ctrl->ctrl.reset_work, nvme_rdma_reset_ctrl_work);
 
 	ctrl->queue_count = opts->nr_io_queues + 1; /* +1 for admin queue */
 	ctrl->ctrl.sqsize = opts->queue_size - 1;
@@ -1939,12 +1880,14 @@ static struct nvme_ctrl *nvme_rdma_create_ctrl(struct device *dev,
 	/* sanity check icdoff */
 	if (ctrl->ctrl.icdoff) {
 		dev_err(ctrl->ctrl.device, "icdoff is not supported!\n");
+		ret = -EINVAL;
 		goto out_remove_admin_queue;
 	}
 
 	/* sanity check keyed sgls */
 	if (!(ctrl->ctrl.sgls & (1 << 20))) {
 		dev_err(ctrl->ctrl.device, "Mandatory keyed sgls are not support\n");
+		ret = -EINVAL;
 		goto out_remove_admin_queue;
 	}
 
@@ -2033,7 +1976,7 @@ static void nvme_rdma_remove_one(struct ib_device *ib_device, void *client_data)
 	}
 	mutex_unlock(&nvme_rdma_ctrl_mutex);
 
-	flush_workqueue(nvme_rdma_wq);
+	flush_workqueue(nvme_wq);
 }
 
 static struct ib_client nvme_rdma_ib_client = {
@@ -2046,13 +1989,9 @@ static int __init nvme_rdma_init_module(void)
 {
 	int ret;
 
-	nvme_rdma_wq = create_workqueue("nvme_rdma_wq");
-	if (!nvme_rdma_wq)
-		return -ENOMEM;
-
 	ret = ib_register_client(&nvme_rdma_ib_client);
 	if (ret)
-		goto err_destroy_wq;
+		return ret;
 
 	ret = nvmf_register_transport(&nvme_rdma_transport);
 	if (ret)
@@ -2062,8 +2001,6 @@ static int __init nvme_rdma_init_module(void)
 
 err_unreg_client:
 	ib_unregister_client(&nvme_rdma_ib_client);
-err_destroy_wq:
-	destroy_workqueue(nvme_rdma_wq);
 	return ret;
 }
 
@@ -2071,7 +2008,6 @@ static void __exit nvme_rdma_cleanup_module(void)
 {
 	nvmf_unregister_transport(&nvme_rdma_transport);
 	ib_unregister_client(&nvme_rdma_ib_client);
-	destroy_workqueue(nvme_rdma_wq);
 }
 
 module_init(nvme_rdma_init_module);
diff --git a/drivers/nvme/host/scsi.c b/drivers/nvme/host/scsi.c
deleted file mode 100644
index 1f7671e631dd..000000000000
--- a/drivers/nvme/host/scsi.c
+++ /dev/null
@@ -1,2460 +0,0 @@
-/*
- * NVM Express device driver
- * Copyright (c) 2011-2014, Intel Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- */
-
-/*
- * Refer to the SCSI-NVMe Translation spec for details on how
- * each command is translated.
- */
-
-#include <linux/bio.h>
-#include <linux/bitops.h>
-#include <linux/blkdev.h>
-#include <linux/compat.h>
-#include <linux/delay.h>
-#include <linux/errno.h>
-#include <linux/fs.h>
-#include <linux/genhd.h>
-#include <linux/idr.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/io.h>
-#include <linux/kdev_t.h>
-#include <linux/kthread.h>
-#include <linux/kernel.h>
-#include <linux/mm.h>
-#include <linux/module.h>
-#include <linux/moduleparam.h>
-#include <linux/pci.h>
-#include <linux/poison.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/types.h>
-#include <asm/unaligned.h>
-#include <scsi/sg.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_request.h>
-
-#include "nvme.h"
-
-static int sg_version_num = 30534;	/* 2 digits for each component */
-
-/* VPD Page Codes */
-#define VPD_SUPPORTED_PAGES				0x00
-#define VPD_SERIAL_NUMBER				0x80
-#define VPD_DEVICE_IDENTIFIERS				0x83
-#define VPD_EXTENDED_INQUIRY				0x86
-#define VPD_BLOCK_LIMITS				0xB0
-#define VPD_BLOCK_DEV_CHARACTERISTICS			0xB1
-
-/* format unit paramter list offsets */
-#define FORMAT_UNIT_SHORT_PARM_LIST_LEN			4
-#define FORMAT_UNIT_LONG_PARM_LIST_LEN			8
-#define FORMAT_UNIT_PROT_INT_OFFSET			3
-#define FORMAT_UNIT_PROT_FIELD_USAGE_OFFSET		0
-#define FORMAT_UNIT_PROT_FIELD_USAGE_MASK		0x07
-
-/* Misc. defines */
-#define FIXED_SENSE_DATA				0x70
-#define DESC_FORMAT_SENSE_DATA				0x72
-#define FIXED_SENSE_DATA_ADD_LENGTH			10
-#define LUN_ENTRY_SIZE					8
-#define LUN_DATA_HEADER_SIZE				8
-#define ALL_LUNS_RETURNED				0x02
-#define ALL_WELL_KNOWN_LUNS_RETURNED			0x01
-#define RESTRICTED_LUNS_RETURNED			0x00
-#define DOWNLOAD_SAVE_ACTIVATE				0x05
-#define DOWNLOAD_SAVE_DEFER_ACTIVATE			0x0E
-#define ACTIVATE_DEFERRED_MICROCODE			0x0F
-#define FORMAT_UNIT_IMMED_MASK				0x2
-#define FORMAT_UNIT_IMMED_OFFSET			1
-#define KELVIN_TEMP_FACTOR				273
-#define FIXED_FMT_SENSE_DATA_SIZE			18
-#define DESC_FMT_SENSE_DATA_SIZE			8
-
-/* SCSI/NVMe defines and bit masks */
-#define INQ_STANDARD_INQUIRY_PAGE			0x00
-#define INQ_SUPPORTED_VPD_PAGES_PAGE			0x00
-#define INQ_UNIT_SERIAL_NUMBER_PAGE			0x80
-#define INQ_DEVICE_IDENTIFICATION_PAGE			0x83
-#define INQ_EXTENDED_INQUIRY_DATA_PAGE			0x86
-#define INQ_BDEV_LIMITS_PAGE				0xB0
-#define INQ_BDEV_CHARACTERISTICS_PAGE			0xB1
-#define INQ_SERIAL_NUMBER_LENGTH			0x14
-#define INQ_NUM_SUPPORTED_VPD_PAGES			6
-#define VERSION_SPC_4					0x06
-#define ACA_UNSUPPORTED					0
-#define STANDARD_INQUIRY_LENGTH				36
-#define ADDITIONAL_STD_INQ_LENGTH			31
-#define EXTENDED_INQUIRY_DATA_PAGE_LENGTH		0x3C
-#define RESERVED_FIELD					0
-
-/* Mode Sense/Select defines */
-#define MODE_PAGE_INFO_EXCEP				0x1C
-#define MODE_PAGE_CACHING				0x08
-#define MODE_PAGE_CONTROL				0x0A
-#define MODE_PAGE_POWER_CONDITION			0x1A
-#define MODE_PAGE_RETURN_ALL				0x3F
-#define MODE_PAGE_BLK_DES_LEN				0x08
-#define MODE_PAGE_LLBAA_BLK_DES_LEN			0x10
-#define MODE_PAGE_CACHING_LEN				0x14
-#define MODE_PAGE_CONTROL_LEN				0x0C
-#define MODE_PAGE_POW_CND_LEN				0x28
-#define MODE_PAGE_INF_EXC_LEN				0x0C
-#define MODE_PAGE_ALL_LEN				0x54
-#define MODE_SENSE6_MPH_SIZE				4
-#define MODE_SENSE_PAGE_CONTROL_MASK			0xC0
-#define MODE_SENSE_PAGE_CODE_OFFSET			2
-#define MODE_SENSE_PAGE_CODE_MASK			0x3F
-#define MODE_SENSE_LLBAA_MASK				0x10
-#define MODE_SENSE_LLBAA_SHIFT				4
-#define MODE_SENSE_DBD_MASK				8
-#define MODE_SENSE_DBD_SHIFT				3
-#define MODE_SENSE10_MPH_SIZE				8
-#define MODE_SELECT_CDB_PAGE_FORMAT_MASK		0x10
-#define MODE_SELECT_CDB_SAVE_PAGES_MASK			0x1
-#define MODE_SELECT_6_BD_OFFSET				3
-#define MODE_SELECT_10_BD_OFFSET			6
-#define MODE_SELECT_10_LLBAA_OFFSET			4
-#define MODE_SELECT_10_LLBAA_MASK			1
-#define MODE_SELECT_6_MPH_SIZE				4
-#define MODE_SELECT_10_MPH_SIZE				8
-#define CACHING_MODE_PAGE_WCE_MASK			0x04
-#define MODE_SENSE_BLK_DESC_ENABLED			0
-#define MODE_SENSE_BLK_DESC_COUNT			1
-#define MODE_SELECT_PAGE_CODE_MASK			0x3F
-#define SHORT_DESC_BLOCK				8
-#define LONG_DESC_BLOCK					16
-#define MODE_PAGE_POW_CND_LEN_FIELD			0x26
-#define MODE_PAGE_INF_EXC_LEN_FIELD			0x0A
-#define MODE_PAGE_CACHING_LEN_FIELD			0x12
-#define MODE_PAGE_CONTROL_LEN_FIELD			0x0A
-#define MODE_SENSE_PC_CURRENT_VALUES			0
-
-/* Log Sense defines */
-#define LOG_PAGE_SUPPORTED_LOG_PAGES_PAGE		0x00
-#define LOG_PAGE_SUPPORTED_LOG_PAGES_LENGTH		0x07
-#define LOG_PAGE_INFORMATIONAL_EXCEPTIONS_PAGE		0x2F
-#define LOG_PAGE_TEMPERATURE_PAGE			0x0D
-#define LOG_SENSE_CDB_SP_NOT_ENABLED			0
-#define LOG_SENSE_CDB_PC_MASK				0xC0
-#define LOG_SENSE_CDB_PC_SHIFT				6
-#define LOG_SENSE_CDB_PC_CUMULATIVE_VALUES		1
-#define LOG_SENSE_CDB_PAGE_CODE_MASK			0x3F
-#define REMAINING_INFO_EXCP_PAGE_LENGTH			0x8
-#define LOG_INFO_EXCP_PAGE_LENGTH			0xC
-#define REMAINING_TEMP_PAGE_LENGTH			0xC
-#define LOG_TEMP_PAGE_LENGTH				0x10
-#define LOG_TEMP_UNKNOWN				0xFF
-#define SUPPORTED_LOG_PAGES_PAGE_LENGTH			0x3
-
-/* Read Capacity defines */
-#define READ_CAP_10_RESP_SIZE				8
-#define READ_CAP_16_RESP_SIZE				32
-
-/* NVMe Namespace and Command Defines */
-#define BYTES_TO_DWORDS					4
-#define NVME_MAX_FIRMWARE_SLOT				7
-
-/* Report LUNs defines */
-#define REPORT_LUNS_FIRST_LUN_OFFSET			8
-
-/* SCSI ADDITIONAL SENSE Codes */
-
-#define SCSI_ASC_NO_SENSE				0x00
-#define SCSI_ASC_PERIPHERAL_DEV_WRITE_FAULT		0x03
-#define SCSI_ASC_LUN_NOT_READY				0x04
-#define SCSI_ASC_WARNING				0x0B
-#define SCSI_ASC_LOG_BLOCK_GUARD_CHECK_FAILED		0x10
-#define SCSI_ASC_LOG_BLOCK_APPTAG_CHECK_FAILED		0x10
-#define SCSI_ASC_LOG_BLOCK_REFTAG_CHECK_FAILED		0x10
-#define SCSI_ASC_UNRECOVERED_READ_ERROR			0x11
-#define SCSI_ASC_MISCOMPARE_DURING_VERIFY		0x1D
-#define SCSI_ASC_ACCESS_DENIED_INVALID_LUN_ID		0x20
-#define SCSI_ASC_ILLEGAL_COMMAND			0x20
-#define SCSI_ASC_ILLEGAL_BLOCK				0x21
-#define SCSI_ASC_INVALID_CDB				0x24
-#define SCSI_ASC_INVALID_LUN				0x25
-#define SCSI_ASC_INVALID_PARAMETER			0x26
-#define SCSI_ASC_FORMAT_COMMAND_FAILED			0x31
-#define SCSI_ASC_INTERNAL_TARGET_FAILURE		0x44
-
-/* SCSI ADDITIONAL SENSE Code Qualifiers */
-
-#define SCSI_ASCQ_CAUSE_NOT_REPORTABLE			0x00
-#define SCSI_ASCQ_FORMAT_COMMAND_FAILED			0x01
-#define SCSI_ASCQ_LOG_BLOCK_GUARD_CHECK_FAILED		0x01
-#define SCSI_ASCQ_LOG_BLOCK_APPTAG_CHECK_FAILED		0x02
-#define SCSI_ASCQ_LOG_BLOCK_REFTAG_CHECK_FAILED		0x03
-#define SCSI_ASCQ_FORMAT_IN_PROGRESS			0x04
-#define SCSI_ASCQ_POWER_LOSS_EXPECTED			0x08
-#define SCSI_ASCQ_INVALID_LUN_ID			0x09
-
-/* copied from drivers/usb/gadget/function/storage_common.h */
-static inline u32 get_unaligned_be24(u8 *buf)
-{
-	return 0xffffff & (u32) get_unaligned_be32(buf - 1);
-}
-
-/* Struct to gather data that needs to be extracted from a SCSI CDB.
-   Not conforming to any particular CDB variant, but compatible with all. */
-
-struct nvme_trans_io_cdb {
-	u8 fua;
-	u8 prot_info;
-	u64 lba;
-	u32 xfer_len;
-};
-
-
-/* Internal Helper Functions */
-
-
-/* Copy data to userspace memory */
-
-static int nvme_trans_copy_to_user(struct sg_io_hdr *hdr, void *from,
-								unsigned long n)
-{
-	int i;
-	void *index = from;
-	size_t remaining = n;
-	size_t xfer_len;
-
-	if (hdr->iovec_count > 0) {
-		struct sg_iovec sgl;
-
-		for (i = 0; i < hdr->iovec_count; i++) {
-			if (copy_from_user(&sgl, hdr->dxferp +
-						i * sizeof(struct sg_iovec),
-						sizeof(struct sg_iovec)))
-				return -EFAULT;
-			xfer_len = min(remaining, sgl.iov_len);
-			if (copy_to_user(sgl.iov_base, index, xfer_len))
-				return -EFAULT;
-
-			index += xfer_len;
-			remaining -= xfer_len;
-			if (remaining == 0)
-				break;
-		}
-		return 0;
-	}
-
-	if (copy_to_user(hdr->dxferp, from, n))
-		return -EFAULT;
-	return 0;
-}
-
-/* Copy data from userspace memory */
-
-static int nvme_trans_copy_from_user(struct sg_io_hdr *hdr, void *to,
-								unsigned long n)
-{
-	int i;
-	void *index = to;
-	size_t remaining = n;
-	size_t xfer_len;
-
-	if (hdr->iovec_count > 0) {
-		struct sg_iovec sgl;
-
-		for (i = 0; i < hdr->iovec_count; i++) {
-			if (copy_from_user(&sgl, hdr->dxferp +
-						i * sizeof(struct sg_iovec),
-						sizeof(struct sg_iovec)))
-				return -EFAULT;
-			xfer_len = min(remaining, sgl.iov_len);
-			if (copy_from_user(index, sgl.iov_base, xfer_len))
-				return -EFAULT;
-			index += xfer_len;
-			remaining -= xfer_len;
-			if (remaining == 0)
-				break;
-		}
-		return 0;
-	}
-
-	if (copy_from_user(to, hdr->dxferp, n))
-		return -EFAULT;
-	return 0;
-}
-
-/* Status/Sense Buffer Writeback */
-
-static int nvme_trans_completion(struct sg_io_hdr *hdr, u8 status, u8 sense_key,
-				 u8 asc, u8 ascq)
-{
-	u8 xfer_len;
-	u8 resp[DESC_FMT_SENSE_DATA_SIZE];
-
-	if (scsi_status_is_good(status)) {
-		hdr->status = SAM_STAT_GOOD;
-		hdr->masked_status = GOOD;
-		hdr->host_status = DID_OK;
-		hdr->driver_status = DRIVER_OK;
-		hdr->sb_len_wr = 0;
-	} else {
-		hdr->status = status;
-		hdr->masked_status = status >> 1;
-		hdr->host_status = DID_OK;
-		hdr->driver_status = DRIVER_OK;
-
-		memset(resp, 0, DESC_FMT_SENSE_DATA_SIZE);
-		resp[0] = DESC_FORMAT_SENSE_DATA;
-		resp[1] = sense_key;
-		resp[2] = asc;
-		resp[3] = ascq;
-
-		xfer_len = min_t(u8, hdr->mx_sb_len, DESC_FMT_SENSE_DATA_SIZE);
-		hdr->sb_len_wr = xfer_len;
-		if (copy_to_user(hdr->sbp, resp, xfer_len) > 0)
-			return -EFAULT;
-	}
-
-	return 0;
-}
-
-/*
- * Take a status code from a lowlevel routine, and if it was a positive NVMe
- * error code update the sense data based on it.  In either case the passed
- * in value is returned again, unless an -EFAULT from copy_to_user overrides
- * it.
- */
-static int nvme_trans_status_code(struct sg_io_hdr *hdr, int nvme_sc)
-{
-	u8 status, sense_key, asc, ascq;
-	int res;
-
-	/* For non-nvme (Linux) errors, simply return the error code */
-	if (nvme_sc < 0)
-		return nvme_sc;
-
-	/* Mask DNR, More, and reserved fields */
-	switch (nvme_sc & 0x7FF) {
-	/* Generic Command Status */
-	case NVME_SC_SUCCESS:
-		status = SAM_STAT_GOOD;
-		sense_key = NO_SENSE;
-		asc = SCSI_ASC_NO_SENSE;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_INVALID_OPCODE:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = ILLEGAL_REQUEST;
-		asc = SCSI_ASC_ILLEGAL_COMMAND;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_INVALID_FIELD:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = ILLEGAL_REQUEST;
-		asc = SCSI_ASC_INVALID_CDB;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_DATA_XFER_ERROR:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = MEDIUM_ERROR;
-		asc = SCSI_ASC_NO_SENSE;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_POWER_LOSS:
-		status = SAM_STAT_TASK_ABORTED;
-		sense_key = ABORTED_COMMAND;
-		asc = SCSI_ASC_WARNING;
-		ascq = SCSI_ASCQ_POWER_LOSS_EXPECTED;
-		break;
-	case NVME_SC_INTERNAL:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = HARDWARE_ERROR;
-		asc = SCSI_ASC_INTERNAL_TARGET_FAILURE;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_ABORT_REQ:
-		status = SAM_STAT_TASK_ABORTED;
-		sense_key = ABORTED_COMMAND;
-		asc = SCSI_ASC_NO_SENSE;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_ABORT_QUEUE:
-		status = SAM_STAT_TASK_ABORTED;
-		sense_key = ABORTED_COMMAND;
-		asc = SCSI_ASC_NO_SENSE;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_FUSED_FAIL:
-		status = SAM_STAT_TASK_ABORTED;
-		sense_key = ABORTED_COMMAND;
-		asc = SCSI_ASC_NO_SENSE;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_FUSED_MISSING:
-		status = SAM_STAT_TASK_ABORTED;
-		sense_key = ABORTED_COMMAND;
-		asc = SCSI_ASC_NO_SENSE;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_INVALID_NS:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = ILLEGAL_REQUEST;
-		asc = SCSI_ASC_ACCESS_DENIED_INVALID_LUN_ID;
-		ascq = SCSI_ASCQ_INVALID_LUN_ID;
-		break;
-	case NVME_SC_LBA_RANGE:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = ILLEGAL_REQUEST;
-		asc = SCSI_ASC_ILLEGAL_BLOCK;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_CAP_EXCEEDED:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = MEDIUM_ERROR;
-		asc = SCSI_ASC_NO_SENSE;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_NS_NOT_READY:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = NOT_READY;
-		asc = SCSI_ASC_LUN_NOT_READY;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-
-	/* Command Specific Status */
-	case NVME_SC_INVALID_FORMAT:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = ILLEGAL_REQUEST;
-		asc = SCSI_ASC_FORMAT_COMMAND_FAILED;
-		ascq = SCSI_ASCQ_FORMAT_COMMAND_FAILED;
-		break;
-	case NVME_SC_BAD_ATTRIBUTES:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = ILLEGAL_REQUEST;
-		asc = SCSI_ASC_INVALID_CDB;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-
-	/* Media Errors */
-	case NVME_SC_WRITE_FAULT:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = MEDIUM_ERROR;
-		asc = SCSI_ASC_PERIPHERAL_DEV_WRITE_FAULT;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_READ_ERROR:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = MEDIUM_ERROR;
-		asc = SCSI_ASC_UNRECOVERED_READ_ERROR;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_GUARD_CHECK:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = MEDIUM_ERROR;
-		asc = SCSI_ASC_LOG_BLOCK_GUARD_CHECK_FAILED;
-		ascq = SCSI_ASCQ_LOG_BLOCK_GUARD_CHECK_FAILED;
-		break;
-	case NVME_SC_APPTAG_CHECK:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = MEDIUM_ERROR;
-		asc = SCSI_ASC_LOG_BLOCK_APPTAG_CHECK_FAILED;
-		ascq = SCSI_ASCQ_LOG_BLOCK_APPTAG_CHECK_FAILED;
-		break;
-	case NVME_SC_REFTAG_CHECK:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = MEDIUM_ERROR;
-		asc = SCSI_ASC_LOG_BLOCK_REFTAG_CHECK_FAILED;
-		ascq = SCSI_ASCQ_LOG_BLOCK_REFTAG_CHECK_FAILED;
-		break;
-	case NVME_SC_COMPARE_FAILED:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = MISCOMPARE;
-		asc = SCSI_ASC_MISCOMPARE_DURING_VERIFY;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	case NVME_SC_ACCESS_DENIED:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = ILLEGAL_REQUEST;
-		asc = SCSI_ASC_ACCESS_DENIED_INVALID_LUN_ID;
-		ascq = SCSI_ASCQ_INVALID_LUN_ID;
-		break;
-
-	/* Unspecified/Default */
-	case NVME_SC_CMDID_CONFLICT:
-	case NVME_SC_CMD_SEQ_ERROR:
-	case NVME_SC_CQ_INVALID:
-	case NVME_SC_QID_INVALID:
-	case NVME_SC_QUEUE_SIZE:
-	case NVME_SC_ABORT_LIMIT:
-	case NVME_SC_ABORT_MISSING:
-	case NVME_SC_ASYNC_LIMIT:
-	case NVME_SC_FIRMWARE_SLOT:
-	case NVME_SC_FIRMWARE_IMAGE:
-	case NVME_SC_INVALID_VECTOR:
-	case NVME_SC_INVALID_LOG_PAGE:
-	default:
-		status = SAM_STAT_CHECK_CONDITION;
-		sense_key = ILLEGAL_REQUEST;
-		asc = SCSI_ASC_NO_SENSE;
-		ascq = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		break;
-	}
-
-	res = nvme_trans_completion(hdr, status, sense_key, asc, ascq);
-	return res ? res : nvme_sc;
-}
-
-/* INQUIRY Helper Functions */
-
-static int nvme_trans_standard_inquiry_page(struct nvme_ns *ns,
-					struct sg_io_hdr *hdr, u8 *inq_response,
-					int alloc_len)
-{
-	struct nvme_ctrl *ctrl = ns->ctrl;
-	struct nvme_id_ns *id_ns;
-	int res;
-	int nvme_sc;
-	int xfer_len;
-	u8 resp_data_format = 0x02;
-	u8 protect;
-	u8 cmdque = 0x01 << 1;
-	u8 fw_offset = sizeof(ctrl->firmware_rev);
-
-	/* nvme ns identify - use DPS value for PROTECT field */
-	nvme_sc = nvme_identify_ns(ctrl, ns->ns_id, &id_ns);
-	res = nvme_trans_status_code(hdr, nvme_sc);
-	if (res)
-		return res;
-
-	if (id_ns->dps)
-		protect = 0x01;
-	else
-		protect = 0;
-	kfree(id_ns);
-
-	memset(inq_response, 0, STANDARD_INQUIRY_LENGTH);
-	inq_response[2] = VERSION_SPC_4;
-	inq_response[3] = resp_data_format;	/*normaca=0 | hisup=0 */
-	inq_response[4] = ADDITIONAL_STD_INQ_LENGTH;
-	inq_response[5] = protect;	/* sccs=0 | acc=0 | tpgs=0 | pc3=0 */
-	inq_response[7] = cmdque;	/* wbus16=0 | sync=0 | vs=0 */
-	strncpy(&inq_response[8], "NVMe    ", 8);
-	strncpy(&inq_response[16], ctrl->model, 16);
-
-	while (ctrl->firmware_rev[fw_offset - 1] == ' ' && fw_offset > 4)
-		fw_offset--;
-	fw_offset -= 4;
-	strncpy(&inq_response[32], ctrl->firmware_rev + fw_offset, 4);
-
-	xfer_len = min(alloc_len, STANDARD_INQUIRY_LENGTH);
-	return nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
-}
-
-static int nvme_trans_supported_vpd_pages(struct nvme_ns *ns,
-					struct sg_io_hdr *hdr, u8 *inq_response,
-					int alloc_len)
-{
-	int xfer_len;
-
-	memset(inq_response, 0, STANDARD_INQUIRY_LENGTH);
-	inq_response[1] = INQ_SUPPORTED_VPD_PAGES_PAGE;   /* Page Code */
-	inq_response[3] = INQ_NUM_SUPPORTED_VPD_PAGES;    /* Page Length */
-	inq_response[4] = INQ_SUPPORTED_VPD_PAGES_PAGE;
-	inq_response[5] = INQ_UNIT_SERIAL_NUMBER_PAGE;
-	inq_response[6] = INQ_DEVICE_IDENTIFICATION_PAGE;
-	inq_response[7] = INQ_EXTENDED_INQUIRY_DATA_PAGE;
-	inq_response[8] = INQ_BDEV_CHARACTERISTICS_PAGE;
-	inq_response[9] = INQ_BDEV_LIMITS_PAGE;
-
-	xfer_len = min(alloc_len, STANDARD_INQUIRY_LENGTH);
-	return nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
-}
-
-static int nvme_trans_unit_serial_page(struct nvme_ns *ns,
-					struct sg_io_hdr *hdr, u8 *inq_response,
-					int alloc_len)
-{
-	int xfer_len;
-
-	memset(inq_response, 0, STANDARD_INQUIRY_LENGTH);
-	inq_response[1] = INQ_UNIT_SERIAL_NUMBER_PAGE; /* Page Code */
-	inq_response[3] = INQ_SERIAL_NUMBER_LENGTH;    /* Page Length */
-	strncpy(&inq_response[4], ns->ctrl->serial, INQ_SERIAL_NUMBER_LENGTH);
-
-	xfer_len = min(alloc_len, STANDARD_INQUIRY_LENGTH);
-	return nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
-}
-
-static int nvme_fill_device_id_eui64(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-		u8 *inq_response, int alloc_len)
-{
-	struct nvme_id_ns *id_ns;
-	int nvme_sc, res;
-	size_t len;
-	void *eui;
-
-	nvme_sc = nvme_identify_ns(ns->ctrl, ns->ns_id, &id_ns);
-	res = nvme_trans_status_code(hdr, nvme_sc);
-	if (res)
-		return res;
-
-	eui = id_ns->eui64;
-	len = sizeof(id_ns->eui64);
-
-	if (ns->ctrl->vs >= NVME_VS(1, 2, 0)) {
-		if (bitmap_empty(eui, len * 8)) {
-			eui = id_ns->nguid;
-			len = sizeof(id_ns->nguid);
-		}
-	}
-
-	if (bitmap_empty(eui, len * 8)) {
-		res = -EOPNOTSUPP;
-		goto out_free_id;
-	}
-
-	memset(inq_response, 0, alloc_len);
-	inq_response[1] = INQ_DEVICE_IDENTIFICATION_PAGE;
-	inq_response[3] = 4 + len; /* Page Length */
-
-	/* Designation Descriptor start */
-	inq_response[4] = 0x01;	/* Proto ID=0h | Code set=1h */
-	inq_response[5] = 0x02;	/* PIV=0b | Asso=00b | Designator Type=2h */
-	inq_response[6] = 0x00;	/* Rsvd */
-	inq_response[7] = len;	/* Designator Length */
-	memcpy(&inq_response[8], eui, len);
-
-	res = nvme_trans_copy_to_user(hdr, inq_response, alloc_len);
-out_free_id:
-	kfree(id_ns);
-	return res;
-}
-
-static int nvme_fill_device_id_scsi_string(struct nvme_ns *ns,
-		struct sg_io_hdr *hdr, u8 *inq_response, int alloc_len)
-{
-	struct nvme_ctrl *ctrl = ns->ctrl;
-	struct nvme_id_ctrl *id_ctrl;
-	int nvme_sc, res;
-
-	if (alloc_len < 72) {
-		return nvme_trans_completion(hdr,
-				SAM_STAT_CHECK_CONDITION,
-				ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-				SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-	}
-
-	nvme_sc = nvme_identify_ctrl(ctrl, &id_ctrl);
-	res = nvme_trans_status_code(hdr, nvme_sc);
-	if (res)
-		return res;
-
-	memset(inq_response, 0, alloc_len);
-	inq_response[1] = INQ_DEVICE_IDENTIFICATION_PAGE;
-	inq_response[3] = 0x48;	/* Page Length */
-
-	/* Designation Descriptor start */
-	inq_response[4] = 0x03;	/* Proto ID=0h | Code set=3h */
-	inq_response[5] = 0x08;	/* PIV=0b | Asso=00b | Designator Type=8h */
-	inq_response[6] = 0x00;	/* Rsvd */
-	inq_response[7] = 0x44;	/* Designator Length */
-
-	sprintf(&inq_response[8], "%04x", le16_to_cpu(id_ctrl->vid));
-	memcpy(&inq_response[12], ctrl->model, sizeof(ctrl->model));
-	sprintf(&inq_response[52], "%04x", cpu_to_be32(ns->ns_id));
-	memcpy(&inq_response[56], ctrl->serial, sizeof(ctrl->serial));
-
-	res = nvme_trans_copy_to_user(hdr, inq_response, alloc_len);
-	kfree(id_ctrl);
-	return res;
-}
-
-static int nvme_trans_device_id_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-					u8 *resp, int alloc_len)
-{
-	int res;
-
-	if (ns->ctrl->vs >= NVME_VS(1, 1, 0)) {
-		res = nvme_fill_device_id_eui64(ns, hdr, resp, alloc_len);
-		if (res != -EOPNOTSUPP)
-			return res;
-	}
-
-	return nvme_fill_device_id_scsi_string(ns, hdr, resp, alloc_len);
-}
-
-static int nvme_trans_ext_inq_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-					int alloc_len)
-{
-	u8 *inq_response;
-	int res;
-	int nvme_sc;
-	struct nvme_ctrl *ctrl = ns->ctrl;
-	struct nvme_id_ctrl *id_ctrl;
-	struct nvme_id_ns *id_ns;
-	int xfer_len;
-	u8 microcode = 0x80;
-	u8 spt;
-	u8 spt_lut[8] = {0, 0, 2, 1, 4, 6, 5, 7};
-	u8 grd_chk, app_chk, ref_chk, protect;
-	u8 uask_sup = 0x20;
-	u8 v_sup;
-	u8 luiclr = 0x01;
-
-	inq_response = kmalloc(EXTENDED_INQUIRY_DATA_PAGE_LENGTH, GFP_KERNEL);
-	if (inq_response == NULL)
-		return -ENOMEM;
-
-	nvme_sc = nvme_identify_ns(ctrl, ns->ns_id, &id_ns);
-	res = nvme_trans_status_code(hdr, nvme_sc);
-	if (res)
-		goto out_free_inq;
-
-	spt = spt_lut[id_ns->dpc & 0x07] << 3;
-	if (id_ns->dps)
-		protect = 0x01;
-	else
-		protect = 0;
-	kfree(id_ns);
-
-	grd_chk = protect << 2;
-	app_chk = protect << 1;
-	ref_chk = protect;
-
-	nvme_sc = nvme_identify_ctrl(ctrl, &id_ctrl);
-	res = nvme_trans_status_code(hdr, nvme_sc);
-	if (res)
-		goto out_free_inq;
-
-	v_sup = id_ctrl->vwc;
-	kfree(id_ctrl);
-
-	memset(inq_response, 0, EXTENDED_INQUIRY_DATA_PAGE_LENGTH);
-	inq_response[1] = INQ_EXTENDED_INQUIRY_DATA_PAGE;    /* Page Code */
-	inq_response[2] = 0x00;    /* Page Length MSB */
-	inq_response[3] = 0x3C;    /* Page Length LSB */
-	inq_response[4] = microcode | spt | grd_chk | app_chk | ref_chk;
-	inq_response[5] = uask_sup;
-	inq_response[6] = v_sup;
-	inq_response[7] = luiclr;
-	inq_response[8] = 0;
-	inq_response[9] = 0;
-
-	xfer_len = min(alloc_len, EXTENDED_INQUIRY_DATA_PAGE_LENGTH);
-	res = nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
-
- out_free_inq:
-	kfree(inq_response);
-	return res;
-}
-
-static int nvme_trans_bdev_limits_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-					u8 *inq_response, int alloc_len)
-{
-	__be32 max_sectors = cpu_to_be32(
-		nvme_block_nr(ns, queue_max_hw_sectors(ns->queue)));
-	__be32 max_discard = cpu_to_be32(ns->queue->limits.max_discard_sectors);
-	__be32 discard_desc_count = cpu_to_be32(0x100);
-
-	memset(inq_response, 0, STANDARD_INQUIRY_LENGTH);
-	inq_response[1] = VPD_BLOCK_LIMITS;
-	inq_response[3] = 0x3c; /* Page Length */
-	memcpy(&inq_response[8], &max_sectors, sizeof(u32));
-	memcpy(&inq_response[20], &max_discard, sizeof(u32));
-
-	if (max_discard)
-		memcpy(&inq_response[24], &discard_desc_count, sizeof(u32));
-
-	return nvme_trans_copy_to_user(hdr, inq_response, 0x3c);
-}
-
-static int nvme_trans_bdev_char_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-					int alloc_len)
-{
-	u8 *inq_response;
-	int res;
-	int xfer_len;
-
-	inq_response = kzalloc(EXTENDED_INQUIRY_DATA_PAGE_LENGTH, GFP_KERNEL);
-	if (inq_response == NULL) {
-		res = -ENOMEM;
-		goto out_mem;
-	}
-
-	inq_response[1] = INQ_BDEV_CHARACTERISTICS_PAGE;    /* Page Code */
-	inq_response[2] = 0x00;    /* Page Length MSB */
-	inq_response[3] = 0x3C;    /* Page Length LSB */
-	inq_response[4] = 0x00;    /* Medium Rotation Rate MSB */
-	inq_response[5] = 0x01;    /* Medium Rotation Rate LSB */
-	inq_response[6] = 0x00;    /* Form Factor */
-
-	xfer_len = min(alloc_len, EXTENDED_INQUIRY_DATA_PAGE_LENGTH);
-	res = nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
-
-	kfree(inq_response);
- out_mem:
-	return res;
-}
-
-/* LOG SENSE Helper Functions */
-
-static int nvme_trans_log_supp_pages(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-					int alloc_len)
-{
-	int res;
-	int xfer_len;
-	u8 *log_response;
-
-	log_response = kzalloc(LOG_PAGE_SUPPORTED_LOG_PAGES_LENGTH, GFP_KERNEL);
-	if (log_response == NULL) {
-		res = -ENOMEM;
-		goto out_mem;
-	}
-
-	log_response[0] = LOG_PAGE_SUPPORTED_LOG_PAGES_PAGE;
-	/* Subpage=0x00, Page Length MSB=0 */
-	log_response[3] = SUPPORTED_LOG_PAGES_PAGE_LENGTH;
-	log_response[4] = LOG_PAGE_SUPPORTED_LOG_PAGES_PAGE;
-	log_response[5] = LOG_PAGE_INFORMATIONAL_EXCEPTIONS_PAGE;
-	log_response[6] = LOG_PAGE_TEMPERATURE_PAGE;
-
-	xfer_len = min(alloc_len, LOG_PAGE_SUPPORTED_LOG_PAGES_LENGTH);
-	res = nvme_trans_copy_to_user(hdr, log_response, xfer_len);
-
-	kfree(log_response);
- out_mem:
-	return res;
-}
-
-static int nvme_trans_log_info_exceptions(struct nvme_ns *ns,
-					struct sg_io_hdr *hdr, int alloc_len)
-{
-	int res;
-	int xfer_len;
-	u8 *log_response;
-	struct nvme_smart_log *smart_log;
-	u8 temp_c;
-	u16 temp_k;
-
-	log_response = kzalloc(LOG_INFO_EXCP_PAGE_LENGTH, GFP_KERNEL);
-	if (log_response == NULL)
-		return -ENOMEM;
-
-	res = nvme_get_log_page(ns->ctrl, &smart_log);
-	if (res < 0)
-		goto out_free_response;
-
-	if (res != NVME_SC_SUCCESS) {
-		temp_c = LOG_TEMP_UNKNOWN;
-	} else {
-		temp_k = (smart_log->temperature[1] << 8) +
-				(smart_log->temperature[0]);
-		temp_c = temp_k - KELVIN_TEMP_FACTOR;
-	}
-	kfree(smart_log);
-
-	log_response[0] = LOG_PAGE_INFORMATIONAL_EXCEPTIONS_PAGE;
-	/* Subpage=0x00, Page Length MSB=0 */
-	log_response[3] = REMAINING_INFO_EXCP_PAGE_LENGTH;
-	/* Informational Exceptions Log Parameter 1 Start */
-	/* Parameter Code=0x0000 bytes 4,5 */
-	log_response[6] = 0x23; /* DU=0, TSD=1, ETC=0, TMC=0, FMT_AND_LNK=11b */
-	log_response[7] = 0x04; /* PARAMETER LENGTH */
-	/* Add sense Code and qualifier = 0x00 each */
-	/* Use Temperature from NVMe Get Log Page, convert to C from K */
-	log_response[10] = temp_c;
-
-	xfer_len = min(alloc_len, LOG_INFO_EXCP_PAGE_LENGTH);
-	res = nvme_trans_copy_to_user(hdr, log_response, xfer_len);
-
- out_free_response:
-	kfree(log_response);
-	return res;
-}
-
-static int nvme_trans_log_temperature(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-					int alloc_len)
-{
-	int res;
-	int xfer_len;
-	u8 *log_response;
-	struct nvme_smart_log *smart_log;
-	u32 feature_resp;
-	u8 temp_c_cur, temp_c_thresh;
-	u16 temp_k;
-
-	log_response = kzalloc(LOG_TEMP_PAGE_LENGTH, GFP_KERNEL);
-	if (log_response == NULL)
-		return -ENOMEM;
-
-	res = nvme_get_log_page(ns->ctrl, &smart_log);
-	if (res < 0)
-		goto out_free_response;
-
-	if (res != NVME_SC_SUCCESS) {
-		temp_c_cur = LOG_TEMP_UNKNOWN;
-	} else {
-		temp_k = (smart_log->temperature[1] << 8) +
-				(smart_log->temperature[0]);
-		temp_c_cur = temp_k - KELVIN_TEMP_FACTOR;
-	}
-	kfree(smart_log);
-
-	/* Get Features for Temp Threshold */
-	res = nvme_get_features(ns->ctrl, NVME_FEAT_TEMP_THRESH, 0, NULL, 0,
-								&feature_resp);
-	if (res != NVME_SC_SUCCESS)
-		temp_c_thresh = LOG_TEMP_UNKNOWN;
-	else
-		temp_c_thresh = (feature_resp & 0xFFFF) - KELVIN_TEMP_FACTOR;
-
-	log_response[0] = LOG_PAGE_TEMPERATURE_PAGE;
-	/* Subpage=0x00, Page Length MSB=0 */
-	log_response[3] = REMAINING_TEMP_PAGE_LENGTH;
-	/* Temperature Log Parameter 1 (Temperature) Start */
-	/* Parameter Code = 0x0000 */
-	log_response[6] = 0x01;		/* Format and Linking = 01b */
-	log_response[7] = 0x02;		/* Parameter Length */
-	/* Use Temperature from NVMe Get Log Page, convert to C from K */
-	log_response[9] = temp_c_cur;
-	/* Temperature Log Parameter 2 (Reference Temperature) Start */
-	log_response[11] = 0x01;	/* Parameter Code = 0x0001 */
-	log_response[12] = 0x01;	/* Format and Linking = 01b */
-	log_response[13] = 0x02;	/* Parameter Length */
-	/* Use Temperature Thresh from NVMe Get Log Page, convert to C from K */
-	log_response[15] = temp_c_thresh;
-
-	xfer_len = min(alloc_len, LOG_TEMP_PAGE_LENGTH);
-	res = nvme_trans_copy_to_user(hdr, log_response, xfer_len);
-
- out_free_response:
-	kfree(log_response);
-	return res;
-}
-
-/* MODE SENSE Helper Functions */
-
-static int nvme_trans_fill_mode_parm_hdr(u8 *resp, int len, u8 cdb10, u8 llbaa,
-					u16 mode_data_length, u16 blk_desc_len)
-{
-	/* Quick check to make sure I don't stomp on my own memory... */
-	if ((cdb10 && len < 8) || (!cdb10 && len < 4))
-		return -EINVAL;
-
-	if (cdb10) {
-		resp[0] = (mode_data_length & 0xFF00) >> 8;
-		resp[1] = (mode_data_length & 0x00FF);
-		resp[3] = 0x10 /* DPOFUA */;
-		resp[4] = llbaa;
-		resp[5] = RESERVED_FIELD;
-		resp[6] = (blk_desc_len & 0xFF00) >> 8;
-		resp[7] = (blk_desc_len & 0x00FF);
-	} else {
-		resp[0] = (mode_data_length & 0x00FF);
-		resp[2] = 0x10 /* DPOFUA */;
-		resp[3] = (blk_desc_len & 0x00FF);
-	}
-
-	return 0;
-}
-
-static int nvme_trans_fill_blk_desc(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-				    u8 *resp, int len, u8 llbaa)
-{
-	int res;
-	int nvme_sc;
-	struct nvme_id_ns *id_ns;
-	u8 flbas;
-	u32 lba_length;
-
-	if (llbaa == 0 && len < MODE_PAGE_BLK_DES_LEN)
-		return -EINVAL;
-	else if (llbaa > 0 && len < MODE_PAGE_LLBAA_BLK_DES_LEN)
-		return -EINVAL;
-
-	nvme_sc = nvme_identify_ns(ns->ctrl, ns->ns_id, &id_ns);
-	res = nvme_trans_status_code(hdr, nvme_sc);
-	if (res)
-		return res;
-
-	flbas = (id_ns->flbas) & 0x0F;
-	lba_length = (1 << (id_ns->lbaf[flbas].ds));
-
-	if (llbaa == 0) {
-		__be32 tmp_cap = cpu_to_be32(le64_to_cpu(id_ns->ncap));
-		/* Byte 4 is reserved */
-		__be32 tmp_len = cpu_to_be32(lba_length & 0x00FFFFFF);
-
-		memcpy(resp, &tmp_cap, sizeof(u32));
-		memcpy(&resp[4], &tmp_len, sizeof(u32));
-	} else {
-		__be64 tmp_cap = cpu_to_be64(le64_to_cpu(id_ns->ncap));
-		__be32 tmp_len = cpu_to_be32(lba_length);
-
-		memcpy(resp, &tmp_cap, sizeof(u64));
-		/* Bytes 8, 9, 10, 11 are reserved */
-		memcpy(&resp[12], &tmp_len, sizeof(u32));
-	}
-
-	kfree(id_ns);
-	return res;
-}
-
-static int nvme_trans_fill_control_page(struct nvme_ns *ns,
-					struct sg_io_hdr *hdr, u8 *resp,
-					int len)
-{
-	if (len < MODE_PAGE_CONTROL_LEN)
-		return -EINVAL;
-
-	resp[0] = MODE_PAGE_CONTROL;
-	resp[1] = MODE_PAGE_CONTROL_LEN_FIELD;
-	resp[2] = 0x0E;		/* TST=000b, TMF_ONLY=0, DPICZ=1,
-				 * D_SENSE=1, GLTSD=1, RLEC=0 */
-	resp[3] = 0x12;		/* Q_ALGO_MODIFIER=1h, NUAR=0, QERR=01b */
-	/* Byte 4:  VS=0, RAC=0, UA_INT=0, SWP=0 */
-	resp[5] = 0x40;		/* ATO=0, TAS=1, ATMPE=0, RWWP=0, AUTOLOAD=0 */
-	/* resp[6] and [7] are obsolete, thus zero */
-	resp[8] = 0xFF;		/* Busy timeout period = 0xffff */
-	resp[9] = 0xFF;
-	/* Bytes 10,11: Extended selftest completion time = 0x0000 */
-
-	return 0;
-}
-
-static int nvme_trans_fill_caching_page(struct nvme_ns *ns,
-					struct sg_io_hdr *hdr,
-					u8 *resp, int len)
-{
-	int res = 0;
-	int nvme_sc;
-	u32 feature_resp;
-	u8 vwc;
-
-	if (len < MODE_PAGE_CACHING_LEN)
-		return -EINVAL;
-
-	nvme_sc = nvme_get_features(ns->ctrl, NVME_FEAT_VOLATILE_WC, 0, NULL, 0,
-								&feature_resp);
-	res = nvme_trans_status_code(hdr, nvme_sc);
-	if (res)
-		return res;
-
-	vwc = feature_resp & 0x00000001;
-
-	resp[0] = MODE_PAGE_CACHING;
-	resp[1] = MODE_PAGE_CACHING_LEN_FIELD;
-	resp[2] = vwc << 2;
-	return 0;
-}
-
-static int nvme_trans_fill_pow_cnd_page(struct nvme_ns *ns,
-					struct sg_io_hdr *hdr, u8 *resp,
-					int len)
-{
-	if (len < MODE_PAGE_POW_CND_LEN)
-		return -EINVAL;
-
-	resp[0] = MODE_PAGE_POWER_CONDITION;
-	resp[1] = MODE_PAGE_POW_CND_LEN_FIELD;
-	/* All other bytes are zero */
-
-	return 0;
-}
-
-static int nvme_trans_fill_inf_exc_page(struct nvme_ns *ns,
-					struct sg_io_hdr *hdr, u8 *resp,
-					int len)
-{
-	if (len < MODE_PAGE_INF_EXC_LEN)
-		return -EINVAL;
-
-	resp[0] = MODE_PAGE_INFO_EXCEP;
-	resp[1] = MODE_PAGE_INF_EXC_LEN_FIELD;
-	resp[2] = 0x88;
-	/* All other bytes are zero */
-
-	return 0;
-}
-
-static int nvme_trans_fill_all_pages(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-				     u8 *resp, int len)
-{
-	int res;
-	u16 mode_pages_offset_1 = 0;
-	u16 mode_pages_offset_2, mode_pages_offset_3, mode_pages_offset_4;
-
-	mode_pages_offset_2 = mode_pages_offset_1 + MODE_PAGE_CACHING_LEN;
-	mode_pages_offset_3 = mode_pages_offset_2 + MODE_PAGE_CONTROL_LEN;
-	mode_pages_offset_4 = mode_pages_offset_3 + MODE_PAGE_POW_CND_LEN;
-
-	res = nvme_trans_fill_caching_page(ns, hdr, &resp[mode_pages_offset_1],
-					MODE_PAGE_CACHING_LEN);
-	if (res)
-		return res;
-	res = nvme_trans_fill_control_page(ns, hdr, &resp[mode_pages_offset_2],
-					MODE_PAGE_CONTROL_LEN);
-	if (res)
-		return res;
-	res = nvme_trans_fill_pow_cnd_page(ns, hdr, &resp[mode_pages_offset_3],
-					MODE_PAGE_POW_CND_LEN);
-	if (res)
-		return res;
-	return nvme_trans_fill_inf_exc_page(ns, hdr, &resp[mode_pages_offset_4],
-					MODE_PAGE_INF_EXC_LEN);
-}
-
-static inline int nvme_trans_get_blk_desc_len(u8 dbd, u8 llbaa)
-{
-	if (dbd == MODE_SENSE_BLK_DESC_ENABLED) {
-		/* SPC-4: len = 8 x Num_of_descriptors if llbaa = 0, 16x if 1 */
-		return 8 * (llbaa + 1) * MODE_SENSE_BLK_DESC_COUNT;
-	} else {
-		return 0;
-	}
-}
-
-static int nvme_trans_mode_page_create(struct nvme_ns *ns,
-					struct sg_io_hdr *hdr, u8 *cmd,
-					u16 alloc_len, u8 cdb10,
-					int (*mode_page_fill_func)
-					(struct nvme_ns *,
-					struct sg_io_hdr *hdr, u8 *, int),
-					u16 mode_pages_tot_len)
-{
-	int res;
-	int xfer_len;
-	u8 *response;
-	u8 dbd, llbaa;
-	u16 resp_size;
-	int mph_size;
-	u16 mode_pages_offset_1;
-	u16 blk_desc_len, blk_desc_offset, mode_data_length;
-
-	dbd = (cmd[1] & MODE_SENSE_DBD_MASK) >> MODE_SENSE_DBD_SHIFT;
-	llbaa = (cmd[1] & MODE_SENSE_LLBAA_MASK) >> MODE_SENSE_LLBAA_SHIFT;
-	mph_size = cdb10 ? MODE_SENSE10_MPH_SIZE : MODE_SENSE6_MPH_SIZE;
-
-	blk_desc_len = nvme_trans_get_blk_desc_len(dbd, llbaa);
-
-	resp_size = mph_size + blk_desc_len + mode_pages_tot_len;
-	/* Refer spc4r34 Table 440 for calculation of Mode data Length field */
-	mode_data_length = 3 + (3 * cdb10) + blk_desc_len + mode_pages_tot_len;
-
-	blk_desc_offset = mph_size;
-	mode_pages_offset_1 = blk_desc_offset + blk_desc_len;
-
-	response = kzalloc(resp_size, GFP_KERNEL);
-	if (response == NULL) {
-		res = -ENOMEM;
-		goto out_mem;
-	}
-
-	res = nvme_trans_fill_mode_parm_hdr(&response[0], mph_size, cdb10,
-					llbaa, mode_data_length, blk_desc_len);
-	if (res)
-		goto out_free;
-	if (blk_desc_len > 0) {
-		res = nvme_trans_fill_blk_desc(ns, hdr,
-					       &response[blk_desc_offset],
-					       blk_desc_len, llbaa);
-		if (res)
-			goto out_free;
-	}
-	res = mode_page_fill_func(ns, hdr, &response[mode_pages_offset_1],
-					mode_pages_tot_len);
-	if (res)
-		goto out_free;
-
-	xfer_len = min(alloc_len, resp_size);
-	res = nvme_trans_copy_to_user(hdr, response, xfer_len);
-
- out_free:
-	kfree(response);
- out_mem:
-	return res;
-}
-
-/* Read Capacity Helper Functions */
-
-static void nvme_trans_fill_read_cap(u8 *response, struct nvme_id_ns *id_ns,
-								u8 cdb16)
-{
-	u8 flbas;
-	u32 lba_length;
-	u64 rlba;
-	u8 prot_en;
-	u8 p_type_lut[4] = {0, 0, 1, 2};
-	__be64 tmp_rlba;
-	__be32 tmp_rlba_32;
-	__be32 tmp_len;
-
-	flbas = (id_ns->flbas) & 0x0F;
-	lba_length = (1 << (id_ns->lbaf[flbas].ds));
-	rlba = le64_to_cpup(&id_ns->nsze) - 1;
-	(id_ns->dps) ? (prot_en = 0x01) : (prot_en = 0);
-
-	if (!cdb16) {
-		if (rlba > 0xFFFFFFFF)
-			rlba = 0xFFFFFFFF;
-		tmp_rlba_32 = cpu_to_be32(rlba);
-		tmp_len = cpu_to_be32(lba_length);
-		memcpy(response, &tmp_rlba_32, sizeof(u32));
-		memcpy(&response[4], &tmp_len, sizeof(u32));
-	} else {
-		tmp_rlba = cpu_to_be64(rlba);
-		tmp_len = cpu_to_be32(lba_length);
-		memcpy(response, &tmp_rlba, sizeof(u64));
-		memcpy(&response[8], &tmp_len, sizeof(u32));
-		response[12] = (p_type_lut[id_ns->dps & 0x3] << 1) | prot_en;
-		/* P_I_Exponent = 0x0 | LBPPBE = 0x0 */
-		/* LBPME = 0 | LBPRZ = 0 | LALBA = 0x00 */
-		/* Bytes 16-31 - Reserved */
-	}
-}
-
-/* Start Stop Unit Helper Functions */
-
-static int nvme_trans_send_activate_fw_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-					u8 buffer_id)
-{
-	struct nvme_command c;
-	int nvme_sc;
-
-	memset(&c, 0, sizeof(c));
-	c.common.opcode = nvme_admin_activate_fw;
-	c.common.cdw10[0] = cpu_to_le32(buffer_id | NVME_FWACT_REPL_ACTV);
-
-	nvme_sc = nvme_submit_sync_cmd(ns->queue, &c, NULL, 0);
-	return nvme_trans_status_code(hdr, nvme_sc);
-}
-
-static int nvme_trans_send_download_fw_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-					u8 opcode, u32 tot_len, u32 offset,
-					u8 buffer_id)
-{
-	int nvme_sc;
-	struct nvme_command c;
-
-	if (hdr->iovec_count > 0) {
-		/* Assuming SGL is not allowed for this command */
-		return nvme_trans_completion(hdr,
-					SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST,
-					SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-	}
-
-	memset(&c, 0, sizeof(c));
-	c.common.opcode = nvme_admin_download_fw;
-	c.dlfw.numd = cpu_to_le32((tot_len/BYTES_TO_DWORDS) - 1);
-	c.dlfw.offset = cpu_to_le32(offset/BYTES_TO_DWORDS);
-
-	nvme_sc = nvme_submit_user_cmd(ns->ctrl->admin_q, &c,
-			hdr->dxferp, tot_len, NULL, 0);
-	return nvme_trans_status_code(hdr, nvme_sc);
-}
-
-/* Mode Select Helper Functions */
-
-static inline void nvme_trans_modesel_get_bd_len(u8 *parm_list, u8 cdb10,
-						u16 *bd_len, u8 *llbaa)
-{
-	if (cdb10) {
-		/* 10 Byte CDB */
-		*bd_len = (parm_list[MODE_SELECT_10_BD_OFFSET] << 8) +
-			parm_list[MODE_SELECT_10_BD_OFFSET + 1];
-		*llbaa = parm_list[MODE_SELECT_10_LLBAA_OFFSET] &
-				MODE_SELECT_10_LLBAA_MASK;
-	} else {
-		/* 6 Byte CDB */
-		*bd_len = parm_list[MODE_SELECT_6_BD_OFFSET];
-	}
-}
-
-static void nvme_trans_modesel_save_bd(struct nvme_ns *ns, u8 *parm_list,
-					u16 idx, u16 bd_len, u8 llbaa)
-{
-	/* Store block descriptor info if a FORMAT UNIT comes later */
-	/* TODO Saving 1st BD info; what to do if multiple BD received? */
-	if (llbaa == 0) {
-		/* Standard Block Descriptor - spc4r34 7.5.5.1 */
-		ns->mode_select_num_blocks =
-				(parm_list[idx + 1] << 16) +
-				(parm_list[idx + 2] << 8) +
-				(parm_list[idx + 3]);
-
-		ns->mode_select_block_len =
-				(parm_list[idx + 5] << 16) +
-				(parm_list[idx + 6] << 8) +
-				(parm_list[idx + 7]);
-	} else {
-		/* Long LBA Block Descriptor - sbc3r27 6.4.2.3 */
-		ns->mode_select_num_blocks =
-				(((u64)parm_list[idx + 0]) << 56) +
-				(((u64)parm_list[idx + 1]) << 48) +
-				(((u64)parm_list[idx + 2]) << 40) +
-				(((u64)parm_list[idx + 3]) << 32) +
-				(((u64)parm_list[idx + 4]) << 24) +
-				(((u64)parm_list[idx + 5]) << 16) +
-				(((u64)parm_list[idx + 6]) << 8) +
-				((u64)parm_list[idx + 7]);
-
-		ns->mode_select_block_len =
-				(parm_list[idx + 12] << 24) +
-				(parm_list[idx + 13] << 16) +
-				(parm_list[idx + 14] << 8) +
-				(parm_list[idx + 15]);
-	}
-}
-
-static int nvme_trans_modesel_get_mp(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-					u8 *mode_page, u8 page_code)
-{
-	int res = 0;
-	int nvme_sc;
-	unsigned dword11;
-
-	switch (page_code) {
-	case MODE_PAGE_CACHING:
-		dword11 = ((mode_page[2] & CACHING_MODE_PAGE_WCE_MASK) ? 1 : 0);
-		nvme_sc = nvme_set_features(ns->ctrl, NVME_FEAT_VOLATILE_WC,
-					    dword11, NULL, 0, NULL);
-		res = nvme_trans_status_code(hdr, nvme_sc);
-		break;
-	case MODE_PAGE_CONTROL:
-		break;
-	case MODE_PAGE_POWER_CONDITION:
-		/* Verify the OS is not trying to set timers */
-		if ((mode_page[2] & 0x01) != 0 || (mode_page[3] & 0x0F) != 0) {
-			res = nvme_trans_completion(hdr,
-						SAM_STAT_CHECK_CONDITION,
-						ILLEGAL_REQUEST,
-						SCSI_ASC_INVALID_PARAMETER,
-						SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-			break;
-		}
-		break;
-	default:
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		break;
-	}
-
-	return res;
-}
-
-static int nvme_trans_modesel_data(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-					u8 *cmd, u16 parm_list_len, u8 pf,
-					u8 sp, u8 cdb10)
-{
-	int res;
-	u8 *parm_list;
-	u16 bd_len;
-	u8 llbaa = 0;
-	u16 index, saved_index;
-	u8 page_code;
-	u16 mp_size;
-
-	/* Get parm list from data-in/out buffer */
-	parm_list = kmalloc(parm_list_len, GFP_KERNEL);
-	if (parm_list == NULL) {
-		res = -ENOMEM;
-		goto out;
-	}
-
-	res = nvme_trans_copy_from_user(hdr, parm_list, parm_list_len);
-	if (res)
-		goto out_mem;
-
-	nvme_trans_modesel_get_bd_len(parm_list, cdb10, &bd_len, &llbaa);
-	index = (cdb10) ? (MODE_SELECT_10_MPH_SIZE) : (MODE_SELECT_6_MPH_SIZE);
-
-	if (bd_len != 0) {
-		/* Block Descriptors present, parse */
-		nvme_trans_modesel_save_bd(ns, parm_list, index, bd_len, llbaa);
-		index += bd_len;
-	}
-	saved_index = index;
-
-	/* Multiple mode pages may be present; iterate through all */
-	/* In 1st Iteration, don't do NVME Command, only check for CDB errors */
-	do {
-		page_code = parm_list[index] & MODE_SELECT_PAGE_CODE_MASK;
-		mp_size = parm_list[index + 1] + 2;
-		if ((page_code != MODE_PAGE_CACHING) &&
-		    (page_code != MODE_PAGE_CONTROL) &&
-		    (page_code != MODE_PAGE_POWER_CONDITION)) {
-			res = nvme_trans_completion(hdr,
-						SAM_STAT_CHECK_CONDITION,
-						ILLEGAL_REQUEST,
-						SCSI_ASC_INVALID_CDB,
-						SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-			goto out_mem;
-		}
-		index += mp_size;
-	} while (index < parm_list_len);
-
-	/* In 2nd Iteration, do the NVME Commands */
-	index = saved_index;
-	do {
-		page_code = parm_list[index] & MODE_SELECT_PAGE_CODE_MASK;
-		mp_size = parm_list[index + 1] + 2;
-		res = nvme_trans_modesel_get_mp(ns, hdr, &parm_list[index],
-								page_code);
-		if (res)
-			break;
-		index += mp_size;
-	} while (index < parm_list_len);
-
- out_mem:
-	kfree(parm_list);
- out:
-	return res;
-}
-
-/* Format Unit Helper Functions */
-
-static int nvme_trans_fmt_set_blk_size_count(struct nvme_ns *ns,
-					     struct sg_io_hdr *hdr)
-{
-	int res = 0;
-	int nvme_sc;
-	u8 flbas;
-
-	/*
-	 * SCSI Expects a MODE SELECT would have been issued prior to
-	 * a FORMAT UNIT, and the block size and number would be used
-	 * from the block descriptor in it. If a MODE SELECT had not
-	 * been issued, FORMAT shall use the current values for both.
-	 */
-
-	if (ns->mode_select_num_blocks == 0 || ns->mode_select_block_len == 0) {
-		struct nvme_id_ns *id_ns;
-
-		nvme_sc = nvme_identify_ns(ns->ctrl, ns->ns_id, &id_ns);
-		res = nvme_trans_status_code(hdr, nvme_sc);
-		if (res)
-			return res;
-
-		if (ns->mode_select_num_blocks == 0)
-			ns->mode_select_num_blocks = le64_to_cpu(id_ns->ncap);
-		if (ns->mode_select_block_len == 0) {
-			flbas = (id_ns->flbas) & 0x0F;
-			ns->mode_select_block_len =
-						(1 << (id_ns->lbaf[flbas].ds));
-		}
-
-		kfree(id_ns);
-	}
-
-	return 0;
-}
-
-static int nvme_trans_fmt_get_parm_header(struct sg_io_hdr *hdr, u8 len,
-					u8 format_prot_info, u8 *nvme_pf_code)
-{
-	int res;
-	u8 *parm_list;
-	u8 pf_usage, pf_code;
-
-	parm_list = kmalloc(len, GFP_KERNEL);
-	if (parm_list == NULL) {
-		res = -ENOMEM;
-		goto out;
-	}
-	res = nvme_trans_copy_from_user(hdr, parm_list, len);
-	if (res)
-		goto out_mem;
-
-	if ((parm_list[FORMAT_UNIT_IMMED_OFFSET] &
-				FORMAT_UNIT_IMMED_MASK) != 0) {
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		goto out_mem;
-	}
-
-	if (len == FORMAT_UNIT_LONG_PARM_LIST_LEN &&
-	    (parm_list[FORMAT_UNIT_PROT_INT_OFFSET] & 0x0F) != 0) {
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		goto out_mem;
-	}
-	pf_usage = parm_list[FORMAT_UNIT_PROT_FIELD_USAGE_OFFSET] &
-			FORMAT_UNIT_PROT_FIELD_USAGE_MASK;
-	pf_code = (pf_usage << 2) | format_prot_info;
-	switch (pf_code) {
-	case 0:
-		*nvme_pf_code = 0;
-		break;
-	case 2:
-		*nvme_pf_code = 1;
-		break;
-	case 3:
-		*nvme_pf_code = 2;
-		break;
-	case 7:
-		*nvme_pf_code = 3;
-		break;
-	default:
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		break;
-	}
-
- out_mem:
-	kfree(parm_list);
- out:
-	return res;
-}
-
-static int nvme_trans_fmt_send_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-				   u8 prot_info)
-{
-	int res;
-	int nvme_sc;
-	struct nvme_id_ns *id_ns;
-	u8 i;
-	u8 nlbaf;
-	u8 selected_lbaf = 0xFF;
-	u32 cdw10 = 0;
-	struct nvme_command c;
-
-	/* Loop thru LBAF's in id_ns to match reqd lbaf, put in cdw10 */
-	nvme_sc = nvme_identify_ns(ns->ctrl, ns->ns_id, &id_ns);
-	res = nvme_trans_status_code(hdr, nvme_sc);
-	if (res)
-		return res;
-
-	nlbaf = id_ns->nlbaf;
-
-	for (i = 0; i < nlbaf; i++) {
-		if (ns->mode_select_block_len == (1 << (id_ns->lbaf[i].ds))) {
-			selected_lbaf = i;
-			break;
-		}
-	}
-	if (selected_lbaf > 0x0F) {
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-				ILLEGAL_REQUEST, SCSI_ASC_INVALID_PARAMETER,
-				SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-	}
-	if (ns->mode_select_num_blocks != le64_to_cpu(id_ns->ncap)) {
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-				ILLEGAL_REQUEST, SCSI_ASC_INVALID_PARAMETER,
-				SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-	}
-
-	cdw10 |= prot_info << 5;
-	cdw10 |= selected_lbaf & 0x0F;
-	memset(&c, 0, sizeof(c));
-	c.format.opcode = nvme_admin_format_nvm;
-	c.format.nsid = cpu_to_le32(ns->ns_id);
-	c.format.cdw10 = cpu_to_le32(cdw10);
-
-	nvme_sc = nvme_submit_sync_cmd(ns->ctrl->admin_q, &c, NULL, 0);
-	res = nvme_trans_status_code(hdr, nvme_sc);
-
-	kfree(id_ns);
-	return res;
-}
-
-static inline u32 nvme_trans_io_get_num_cmds(struct sg_io_hdr *hdr,
-					struct nvme_trans_io_cdb *cdb_info,
-					u32 max_blocks)
-{
-	/* If using iovecs, send one nvme command per vector */
-	if (hdr->iovec_count > 0)
-		return hdr->iovec_count;
-	else if (cdb_info->xfer_len > max_blocks)
-		return ((cdb_info->xfer_len - 1) / max_blocks) + 1;
-	else
-		return 1;
-}
-
-static u16 nvme_trans_io_get_control(struct nvme_ns *ns,
-					struct nvme_trans_io_cdb *cdb_info)
-{
-	u16 control = 0;
-
-	/* When Protection information support is added, implement here */
-
-	if (cdb_info->fua > 0)
-		control |= NVME_RW_FUA;
-
-	return control;
-}
-
-static int nvme_trans_do_nvme_io(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-				struct nvme_trans_io_cdb *cdb_info, u8 is_write)
-{
-	int nvme_sc = NVME_SC_SUCCESS;
-	u32 num_cmds;
-	u64 unit_len;
-	u64 unit_num_blocks;	/* Number of blocks to xfer in each nvme cmd */
-	u32 retcode;
-	u32 i = 0;
-	u64 nvme_offset = 0;
-	void __user *next_mapping_addr;
-	struct nvme_command c;
-	u8 opcode = (is_write ? nvme_cmd_write : nvme_cmd_read);
-	u16 control;
-	u32 max_blocks = queue_max_hw_sectors(ns->queue) >> (ns->lba_shift - 9);
-
-	num_cmds = nvme_trans_io_get_num_cmds(hdr, cdb_info, max_blocks);
-
-	/*
-	 * This loop handles two cases.
-	 * First, when an SGL is used in the form of an iovec list:
-	 *   - Use iov_base as the next mapping address for the nvme command_id
-	 *   - Use iov_len as the data transfer length for the command.
-	 * Second, when we have a single buffer
-	 *   - If larger than max_blocks, split into chunks, offset
-	 *        each nvme command accordingly.
-	 */
-	for (i = 0; i < num_cmds; i++) {
-		memset(&c, 0, sizeof(c));
-		if (hdr->iovec_count > 0) {
-			struct sg_iovec sgl;
-
-			retcode = copy_from_user(&sgl, hdr->dxferp +
-					i * sizeof(struct sg_iovec),
-					sizeof(struct sg_iovec));
-			if (retcode)
-				return -EFAULT;
-			unit_len = sgl.iov_len;
-			unit_num_blocks = unit_len >> ns->lba_shift;
-			next_mapping_addr = sgl.iov_base;
-		} else {
-			unit_num_blocks = min((u64)max_blocks,
-					(cdb_info->xfer_len - nvme_offset));
-			unit_len = unit_num_blocks << ns->lba_shift;
-			next_mapping_addr = hdr->dxferp +
-					((1 << ns->lba_shift) * nvme_offset);
-		}
-
-		c.rw.opcode = opcode;
-		c.rw.nsid = cpu_to_le32(ns->ns_id);
-		c.rw.slba = cpu_to_le64(cdb_info->lba + nvme_offset);
-		c.rw.length = cpu_to_le16(unit_num_blocks - 1);
-		control = nvme_trans_io_get_control(ns, cdb_info);
-		c.rw.control = cpu_to_le16(control);
-
-		if (get_capacity(ns->disk) - unit_num_blocks <
-				cdb_info->lba + nvme_offset) {
-			nvme_sc = NVME_SC_LBA_RANGE;
-			break;
-		}
-		nvme_sc = nvme_submit_user_cmd(ns->queue, &c,
-				next_mapping_addr, unit_len, NULL, 0);
-		if (nvme_sc)
-			break;
-
-		nvme_offset += unit_num_blocks;
-	}
-
-	return nvme_trans_status_code(hdr, nvme_sc);
-}
-
-
-/* SCSI Command Translation Functions */
-
-static int nvme_trans_io(struct nvme_ns *ns, struct sg_io_hdr *hdr, u8 is_write,
-							u8 *cmd)
-{
-	int res = 0;
-	struct nvme_trans_io_cdb cdb_info = { 0, };
-	u8 opcode = cmd[0];
-	u64 xfer_bytes;
-	u64 sum_iov_len = 0;
-	struct sg_iovec sgl;
-	int i;
-	size_t not_copied;
-
-	/*
-	 * The FUA and WPROTECT fields are not supported in 6-byte CDBs,
-	 * but always in the same place for all others.
-	 */
-	switch (opcode) {
-	case WRITE_6:
-	case READ_6:
-		break;
-	default:
-		cdb_info.fua = cmd[1] & 0x8;
-		cdb_info.prot_info = (cmd[1] & 0xe0) >> 5;
-		if (cdb_info.prot_info && !ns->pi_type) {
-			return nvme_trans_completion(hdr,
-					SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST,
-					SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		}
-	}
-
-	switch (opcode) {
-	case WRITE_6:
-	case READ_6:
-		cdb_info.lba = get_unaligned_be24(&cmd[1]);
-		cdb_info.xfer_len = cmd[4];
-		if (cdb_info.xfer_len == 0)
-			cdb_info.xfer_len = 256;
-		break;
-	case WRITE_10:
-	case READ_10:
-		cdb_info.lba = get_unaligned_be32(&cmd[2]);
-		cdb_info.xfer_len = get_unaligned_be16(&cmd[7]);
-		break;
-	case WRITE_12:
-	case READ_12:
-		cdb_info.lba = get_unaligned_be32(&cmd[2]);
-		cdb_info.xfer_len = get_unaligned_be32(&cmd[6]);
-		break;
-	case WRITE_16:
-	case READ_16:
-		cdb_info.lba = get_unaligned_be64(&cmd[2]);
-		cdb_info.xfer_len = get_unaligned_be32(&cmd[10]);
-		break;
-	default:
-		/* Will never really reach here */
-		res = -EIO;
-		goto out;
-	}
-
-	/* Calculate total length of transfer (in bytes) */
-	if (hdr->iovec_count > 0) {
-		for (i = 0; i < hdr->iovec_count; i++) {
-			not_copied = copy_from_user(&sgl, hdr->dxferp +
-						i * sizeof(struct sg_iovec),
-						sizeof(struct sg_iovec));
-			if (not_copied)
-				return -EFAULT;
-			sum_iov_len += sgl.iov_len;
-			/* IO vector sizes should be multiples of block size */
-			if (sgl.iov_len % (1 << ns->lba_shift) != 0) {
-				res = nvme_trans_completion(hdr,
-						SAM_STAT_CHECK_CONDITION,
-						ILLEGAL_REQUEST,
-						SCSI_ASC_INVALID_PARAMETER,
-						SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-				goto out;
-			}
-		}
-	} else {
-		sum_iov_len = hdr->dxfer_len;
-	}
-
-	/* As Per sg ioctl howto, if the lengths differ, use the lower one */
-	xfer_bytes = min(((u64)hdr->dxfer_len), sum_iov_len);
-
-	/* If block count and actual data buffer size dont match, error out */
-	if (xfer_bytes != (cdb_info.xfer_len << ns->lba_shift)) {
-		res = -EINVAL;
-		goto out;
-	}
-
-	/* Check for 0 length transfer - it is not illegal */
-	if (cdb_info.xfer_len == 0)
-		goto out;
-
-	/* Send NVMe IO Command(s) */
-	res = nvme_trans_do_nvme_io(ns, hdr, &cdb_info, is_write);
-	if (res)
-		goto out;
-
- out:
-	return res;
-}
-
-static int nvme_trans_inquiry(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-							u8 *cmd)
-{
-	int res = 0;
-	u8 evpd;
-	u8 page_code;
-	int alloc_len;
-	u8 *inq_response;
-
-	evpd = cmd[1] & 0x01;
-	page_code = cmd[2];
-	alloc_len = get_unaligned_be16(&cmd[3]);
-
-	inq_response = kmalloc(max(alloc_len, STANDARD_INQUIRY_LENGTH),
-				GFP_KERNEL);
-	if (inq_response == NULL) {
-		res = -ENOMEM;
-		goto out_mem;
-	}
-
-	if (evpd == 0) {
-		if (page_code == INQ_STANDARD_INQUIRY_PAGE) {
-			res = nvme_trans_standard_inquiry_page(ns, hdr,
-						inq_response, alloc_len);
-		} else {
-			res = nvme_trans_completion(hdr,
-						SAM_STAT_CHECK_CONDITION,
-						ILLEGAL_REQUEST,
-						SCSI_ASC_INVALID_CDB,
-						SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		}
-	} else {
-		switch (page_code) {
-		case VPD_SUPPORTED_PAGES:
-			res = nvme_trans_supported_vpd_pages(ns, hdr,
-						inq_response, alloc_len);
-			break;
-		case VPD_SERIAL_NUMBER:
-			res = nvme_trans_unit_serial_page(ns, hdr, inq_response,
-								alloc_len);
-			break;
-		case VPD_DEVICE_IDENTIFIERS:
-			res = nvme_trans_device_id_page(ns, hdr, inq_response,
-								alloc_len);
-			break;
-		case VPD_EXTENDED_INQUIRY:
-			res = nvme_trans_ext_inq_page(ns, hdr, alloc_len);
-			break;
-		case VPD_BLOCK_LIMITS:
-			res = nvme_trans_bdev_limits_page(ns, hdr, inq_response,
-								alloc_len);
-			break;
-		case VPD_BLOCK_DEV_CHARACTERISTICS:
-			res = nvme_trans_bdev_char_page(ns, hdr, alloc_len);
-			break;
-		default:
-			res = nvme_trans_completion(hdr,
-						SAM_STAT_CHECK_CONDITION,
-						ILLEGAL_REQUEST,
-						SCSI_ASC_INVALID_CDB,
-						SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-			break;
-		}
-	}
-	kfree(inq_response);
- out_mem:
-	return res;
-}
-
-static int nvme_trans_log_sense(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-							u8 *cmd)
-{
-	int res;
-	u16 alloc_len;
-	u8 pc;
-	u8 page_code;
-
-	if (cmd[1] != LOG_SENSE_CDB_SP_NOT_ENABLED) {
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		goto out;
-	}
-
-	page_code = cmd[2] & LOG_SENSE_CDB_PAGE_CODE_MASK;
-	pc = (cmd[2] & LOG_SENSE_CDB_PC_MASK) >> LOG_SENSE_CDB_PC_SHIFT;
-	if (pc != LOG_SENSE_CDB_PC_CUMULATIVE_VALUES) {
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		goto out;
-	}
-	alloc_len = get_unaligned_be16(&cmd[7]);
-	switch (page_code) {
-	case LOG_PAGE_SUPPORTED_LOG_PAGES_PAGE:
-		res = nvme_trans_log_supp_pages(ns, hdr, alloc_len);
-		break;
-	case LOG_PAGE_INFORMATIONAL_EXCEPTIONS_PAGE:
-		res = nvme_trans_log_info_exceptions(ns, hdr, alloc_len);
-		break;
-	case LOG_PAGE_TEMPERATURE_PAGE:
-		res = nvme_trans_log_temperature(ns, hdr, alloc_len);
-		break;
-	default:
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		break;
-	}
-
- out:
-	return res;
-}
-
-static int nvme_trans_mode_select(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-							u8 *cmd)
-{
-	u8 cdb10 = 0;
-	u16 parm_list_len;
-	u8 page_format;
-	u8 save_pages;
-
-	page_format = cmd[1] & MODE_SELECT_CDB_PAGE_FORMAT_MASK;
-	save_pages = cmd[1] & MODE_SELECT_CDB_SAVE_PAGES_MASK;
-
-	if (cmd[0] == MODE_SELECT) {
-		parm_list_len = cmd[4];
-	} else {
-		parm_list_len = cmd[7];
-		cdb10 = 1;
-	}
-
-	if (parm_list_len != 0) {
-		/*
-		 * According to SPC-4 r24, a paramter list length field of 0
-		 * shall not be considered an error
-		 */
-		return nvme_trans_modesel_data(ns, hdr, cmd, parm_list_len,
-						page_format, save_pages, cdb10);
-	}
-
-	return 0;
-}
-
-static int nvme_trans_mode_sense(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-							u8 *cmd)
-{
-	int res = 0;
-	u16 alloc_len;
-	u8 cdb10 = 0;
-
-	if (cmd[0] == MODE_SENSE) {
-		alloc_len = cmd[4];
-	} else {
-		alloc_len = get_unaligned_be16(&cmd[7]);
-		cdb10 = 1;
-	}
-
-	if ((cmd[2] & MODE_SENSE_PAGE_CONTROL_MASK) !=
-			MODE_SENSE_PC_CURRENT_VALUES) {
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		goto out;
-	}
-
-	switch (cmd[2] & MODE_SENSE_PAGE_CODE_MASK) {
-	case MODE_PAGE_CACHING:
-		res = nvme_trans_mode_page_create(ns, hdr, cmd, alloc_len,
-						cdb10,
-						&nvme_trans_fill_caching_page,
-						MODE_PAGE_CACHING_LEN);
-		break;
-	case MODE_PAGE_CONTROL:
-		res = nvme_trans_mode_page_create(ns, hdr, cmd, alloc_len,
-						cdb10,
-						&nvme_trans_fill_control_page,
-						MODE_PAGE_CONTROL_LEN);
-		break;
-	case MODE_PAGE_POWER_CONDITION:
-		res = nvme_trans_mode_page_create(ns, hdr, cmd, alloc_len,
-						cdb10,
-						&nvme_trans_fill_pow_cnd_page,
-						MODE_PAGE_POW_CND_LEN);
-		break;
-	case MODE_PAGE_INFO_EXCEP:
-		res = nvme_trans_mode_page_create(ns, hdr, cmd, alloc_len,
-						cdb10,
-						&nvme_trans_fill_inf_exc_page,
-						MODE_PAGE_INF_EXC_LEN);
-		break;
-	case MODE_PAGE_RETURN_ALL:
-		res = nvme_trans_mode_page_create(ns, hdr, cmd, alloc_len,
-						cdb10,
-						&nvme_trans_fill_all_pages,
-						MODE_PAGE_ALL_LEN);
-		break;
-	default:
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		break;
-	}
-
- out:
-	return res;
-}
-
-static int nvme_trans_read_capacity(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-							u8 *cmd, u8 cdb16)
-{
-	int res;
-	int nvme_sc;
-	u32 alloc_len;
-	u32 resp_size;
-	u32 xfer_len;
-	struct nvme_id_ns *id_ns;
-	u8 *response;
-
-	if (cdb16) {
-		alloc_len = get_unaligned_be32(&cmd[10]);
-		resp_size = READ_CAP_16_RESP_SIZE;
-	} else {
-		alloc_len = READ_CAP_10_RESP_SIZE;
-		resp_size = READ_CAP_10_RESP_SIZE;
-	}
-
-	nvme_sc = nvme_identify_ns(ns->ctrl, ns->ns_id, &id_ns);
-	res = nvme_trans_status_code(hdr, nvme_sc);
-	if (res)
-		return res;	
-
-	response = kzalloc(resp_size, GFP_KERNEL);
-	if (response == NULL) {
-		res = -ENOMEM;
-		goto out_free_id;
-	}
-	nvme_trans_fill_read_cap(response, id_ns, cdb16);
-
-	xfer_len = min(alloc_len, resp_size);
-	res = nvme_trans_copy_to_user(hdr, response, xfer_len);
-
-	kfree(response);
- out_free_id:
-	kfree(id_ns);
-	return res;
-}
-
-static int nvme_trans_report_luns(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-							u8 *cmd)
-{
-	int res;
-	int nvme_sc;
-	u32 alloc_len, xfer_len, resp_size;
-	u8 *response;
-	struct nvme_id_ctrl *id_ctrl;
-	u32 ll_length, lun_id;
-	u8 lun_id_offset = REPORT_LUNS_FIRST_LUN_OFFSET;
-	__be32 tmp_len;
-
-	switch (cmd[2]) {
-	default:
-		return nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-	case ALL_LUNS_RETURNED:
-	case ALL_WELL_KNOWN_LUNS_RETURNED:
-	case RESTRICTED_LUNS_RETURNED:
-		nvme_sc = nvme_identify_ctrl(ns->ctrl, &id_ctrl);
-		res = nvme_trans_status_code(hdr, nvme_sc);
-		if (res)
-			return res;
-
-		ll_length = le32_to_cpu(id_ctrl->nn) * LUN_ENTRY_SIZE;
-		resp_size = ll_length + LUN_DATA_HEADER_SIZE;
-
-		alloc_len = get_unaligned_be32(&cmd[6]);
-		if (alloc_len < resp_size) {
-			res = nvme_trans_completion(hdr,
-					SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-			goto out_free_id;
-		}
-
-		response = kzalloc(resp_size, GFP_KERNEL);
-		if (response == NULL) {
-			res = -ENOMEM;
-			goto out_free_id;
-		}
-
-		/* The first LUN ID will always be 0 per the SAM spec */
-		for (lun_id = 0; lun_id < le32_to_cpu(id_ctrl->nn); lun_id++) {
-			/*
-			 * Set the LUN Id and then increment to the next LUN
-			 * location in the parameter data.
-			 */
-			__be64 tmp_id = cpu_to_be64(lun_id);
-			memcpy(&response[lun_id_offset], &tmp_id, sizeof(u64));
-			lun_id_offset += LUN_ENTRY_SIZE;
-		}
-		tmp_len = cpu_to_be32(ll_length);
-		memcpy(response, &tmp_len, sizeof(u32));
-	}
-
-	xfer_len = min(alloc_len, resp_size);
-	res = nvme_trans_copy_to_user(hdr, response, xfer_len);
-
-	kfree(response);
- out_free_id:
-	kfree(id_ctrl);
-	return res;
-}
-
-static int nvme_trans_request_sense(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-							u8 *cmd)
-{
-	int res;
-	u8 alloc_len, xfer_len, resp_size;
-	u8 desc_format;
-	u8 *response;
-
-	desc_format = cmd[1] & 0x01;
-	alloc_len = cmd[4];
-
-	resp_size = ((desc_format) ? (DESC_FMT_SENSE_DATA_SIZE) :
-					(FIXED_FMT_SENSE_DATA_SIZE));
-	response = kzalloc(resp_size, GFP_KERNEL);
-	if (response == NULL) {
-		res = -ENOMEM;
-		goto out;
-	}
-
-	if (desc_format) {
-		/* Descriptor Format Sense Data */
-		response[0] = DESC_FORMAT_SENSE_DATA;
-		response[1] = NO_SENSE;
-		/* TODO How is LOW POWER CONDITION ON handled? (byte 2) */
-		response[2] = SCSI_ASC_NO_SENSE;
-		response[3] = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		/* SDAT_OVFL = 0 | Additional Sense Length = 0 */
-	} else {
-		/* Fixed Format Sense Data */
-		response[0] = FIXED_SENSE_DATA;
-		/* Byte 1 = Obsolete */
-		response[2] = NO_SENSE; /* FM, EOM, ILI, SDAT_OVFL = 0 */
-		/* Bytes 3-6 - Information - set to zero */
-		response[7] = FIXED_SENSE_DATA_ADD_LENGTH;
-		/* Bytes 8-11 - Cmd Specific Information - set to zero */
-		response[12] = SCSI_ASC_NO_SENSE;
-		response[13] = SCSI_ASCQ_CAUSE_NOT_REPORTABLE;
-		/* Byte 14 = Field Replaceable Unit Code = 0 */
-		/* Bytes 15-17 - SKSV=0; Sense Key Specific = 0 */
-	}
-
-	xfer_len = min(alloc_len, resp_size);
-	res = nvme_trans_copy_to_user(hdr, response, xfer_len);
-
-	kfree(response);
- out:
-	return res;
-}
-
-static int nvme_trans_synchronize_cache(struct nvme_ns *ns,
-					struct sg_io_hdr *hdr)
-{
-	int nvme_sc;
-	struct nvme_command c;
-
-	memset(&c, 0, sizeof(c));
-	c.common.opcode = nvme_cmd_flush;
-	c.common.nsid = cpu_to_le32(ns->ns_id);
-
-	nvme_sc = nvme_submit_sync_cmd(ns->queue, &c, NULL, 0);
-	return nvme_trans_status_code(hdr, nvme_sc);
-}
-
-static int nvme_trans_format_unit(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-							u8 *cmd)
-{
-	int res;
-	u8 parm_hdr_len = 0;
-	u8 nvme_pf_code = 0;
-	u8 format_prot_info, long_list, format_data;
-
-	format_prot_info = (cmd[1] & 0xc0) >> 6;
-	long_list = cmd[1] & 0x20;
-	format_data = cmd[1] & 0x10;
-
-	if (format_data != 0) {
-		if (format_prot_info != 0) {
-			if (long_list == 0)
-				parm_hdr_len = FORMAT_UNIT_SHORT_PARM_LIST_LEN;
-			else
-				parm_hdr_len = FORMAT_UNIT_LONG_PARM_LIST_LEN;
-		}
-	} else if (format_data == 0 && format_prot_info != 0) {
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		goto out;
-	}
-
-	/* Get parm header from data-in/out buffer */
-	/*
-	 * According to the translation spec, the only fields in the parameter
-	 * list we are concerned with are in the header. So allocate only that.
-	 */
-	if (parm_hdr_len > 0) {
-		res = nvme_trans_fmt_get_parm_header(hdr, parm_hdr_len,
-					format_prot_info, &nvme_pf_code);
-		if (res)
-			goto out;
-	}
-
-	/* Attempt to activate any previously downloaded firmware image */
-	res = nvme_trans_send_activate_fw_cmd(ns, hdr, 0);
-
-	/* Determine Block size and count and send format command */
-	res = nvme_trans_fmt_set_blk_size_count(ns, hdr);
-	if (res)
-		goto out;
-
-	res = nvme_trans_fmt_send_cmd(ns, hdr, nvme_pf_code);
-
- out:
-	return res;
-}
-
-static int nvme_trans_test_unit_ready(struct nvme_ns *ns,
-					struct sg_io_hdr *hdr,
-					u8 *cmd)
-{
-	if (nvme_ctrl_ready(ns->ctrl))
-		return nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					    NOT_READY, SCSI_ASC_LUN_NOT_READY,
-					    SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-	else
-		return nvme_trans_completion(hdr, SAM_STAT_GOOD, NO_SENSE, 0, 0);
-}
-
-static int nvme_trans_write_buffer(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-							u8 *cmd)
-{
-	int res = 0;
-	u32 buffer_offset, parm_list_length;
-	u8 buffer_id, mode;
-
-	parm_list_length = get_unaligned_be24(&cmd[6]);
-	if (parm_list_length % BYTES_TO_DWORDS != 0) {
-		/* NVMe expects Firmware file to be a whole number of DWORDS */
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		goto out;
-	}
-	buffer_id = cmd[2];
-	if (buffer_id > NVME_MAX_FIRMWARE_SLOT) {
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		goto out;
-	}
-	mode = cmd[1] & 0x1f;
-	buffer_offset = get_unaligned_be24(&cmd[3]);
-
-	switch (mode) {
-	case DOWNLOAD_SAVE_ACTIVATE:
-		res = nvme_trans_send_download_fw_cmd(ns, hdr, nvme_admin_download_fw,
-						parm_list_length, buffer_offset,
-						buffer_id);
-		if (res)
-			goto out;
-		res = nvme_trans_send_activate_fw_cmd(ns, hdr, buffer_id);
-		break;
-	case DOWNLOAD_SAVE_DEFER_ACTIVATE:
-		res = nvme_trans_send_download_fw_cmd(ns, hdr, nvme_admin_download_fw,
-						parm_list_length, buffer_offset,
-						buffer_id);
-		break;
-	case ACTIVATE_DEFERRED_MICROCODE:
-		res = nvme_trans_send_activate_fw_cmd(ns, hdr, buffer_id);
-		break;
-	default:
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		break;
-	}
-
- out:
-	return res;
-}
-
-struct scsi_unmap_blk_desc {
-	__be64	slba;
-	__be32	nlb;
-	u32	resv;
-};
-
-struct scsi_unmap_parm_list {
-	__be16	unmap_data_len;
-	__be16	unmap_blk_desc_data_len;
-	u32	resv;
-	struct scsi_unmap_blk_desc desc[0];
-};
-
-static int nvme_trans_unmap(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-							u8 *cmd)
-{
-	struct scsi_unmap_parm_list *plist;
-	struct nvme_dsm_range *range;
-	struct nvme_command c;
-	int i, nvme_sc, res;
-	u16 ndesc, list_len;
-
-	list_len = get_unaligned_be16(&cmd[7]);
-	if (!list_len)
-		return -EINVAL;
-
-	plist = kmalloc(list_len, GFP_KERNEL);
-	if (!plist)
-		return -ENOMEM;
-
-	res = nvme_trans_copy_from_user(hdr, plist, list_len);
-	if (res)
-		goto out;
-
-	ndesc = be16_to_cpu(plist->unmap_blk_desc_data_len) >> 4;
-	if (!ndesc || ndesc > 256) {
-		res = -EINVAL;
-		goto out;
-	}
-
-	range = kcalloc(ndesc, sizeof(*range), GFP_KERNEL);
-	if (!range) {
-		res = -ENOMEM;
-		goto out;
-	}
-
-	for (i = 0; i < ndesc; i++) {
-		range[i].nlb = cpu_to_le32(be32_to_cpu(plist->desc[i].nlb));
-		range[i].slba = cpu_to_le64(be64_to_cpu(plist->desc[i].slba));
-		range[i].cattr = 0;
-	}
-
-	memset(&c, 0, sizeof(c));
-	c.dsm.opcode = nvme_cmd_dsm;
-	c.dsm.nsid = cpu_to_le32(ns->ns_id);
-	c.dsm.nr = cpu_to_le32(ndesc - 1);
-	c.dsm.attributes = cpu_to_le32(NVME_DSMGMT_AD);
-
-	nvme_sc = nvme_submit_sync_cmd(ns->queue, &c, range,
-			ndesc * sizeof(*range));
-	res = nvme_trans_status_code(hdr, nvme_sc);
-
-	kfree(range);
- out:
-	kfree(plist);
-	return res;
-}
-
-static int nvme_scsi_translate(struct nvme_ns *ns, struct sg_io_hdr *hdr)
-{
-	u8 cmd[16];
-	int retcode;
-	unsigned int opcode;
-
-	if (hdr->cmdp == NULL)
-		return -EMSGSIZE;
-	if (hdr->cmd_len > sizeof(cmd))
-		return -EINVAL;
-	if (copy_from_user(cmd, hdr->cmdp, hdr->cmd_len))
-		return -EFAULT;
-
-	/*
-	 * Prime the hdr with good status for scsi commands that don't require
-	 * an nvme command for translation.
-	 */
-	retcode = nvme_trans_status_code(hdr, NVME_SC_SUCCESS);
-	if (retcode)
-		return retcode;
-
-	opcode = cmd[0];
-
-	switch (opcode) {
-	case READ_6:
-	case READ_10:
-	case READ_12:
-	case READ_16:
-		retcode = nvme_trans_io(ns, hdr, 0, cmd);
-		break;
-	case WRITE_6:
-	case WRITE_10:
-	case WRITE_12:
-	case WRITE_16:
-		retcode = nvme_trans_io(ns, hdr, 1, cmd);
-		break;
-	case INQUIRY:
-		retcode = nvme_trans_inquiry(ns, hdr, cmd);
-		break;
-	case LOG_SENSE:
-		retcode = nvme_trans_log_sense(ns, hdr, cmd);
-		break;
-	case MODE_SELECT:
-	case MODE_SELECT_10:
-		retcode = nvme_trans_mode_select(ns, hdr, cmd);
-		break;
-	case MODE_SENSE:
-	case MODE_SENSE_10:
-		retcode = nvme_trans_mode_sense(ns, hdr, cmd);
-		break;
-	case READ_CAPACITY:
-		retcode = nvme_trans_read_capacity(ns, hdr, cmd, 0);
-		break;
-	case SERVICE_ACTION_IN_16:
-		switch (cmd[1]) {
-		case SAI_READ_CAPACITY_16:
-			retcode = nvme_trans_read_capacity(ns, hdr, cmd, 1);
-			break;
-		default:
-			goto out;
-		}
-		break;
-	case REPORT_LUNS:
-		retcode = nvme_trans_report_luns(ns, hdr, cmd);
-		break;
-	case REQUEST_SENSE:
-		retcode = nvme_trans_request_sense(ns, hdr, cmd);
-		break;
-	case SYNCHRONIZE_CACHE:
-		retcode = nvme_trans_synchronize_cache(ns, hdr);
-		break;
-	case FORMAT_UNIT:
-		retcode = nvme_trans_format_unit(ns, hdr, cmd);
-		break;
-	case TEST_UNIT_READY:
-		retcode = nvme_trans_test_unit_ready(ns, hdr, cmd);
-		break;
-	case WRITE_BUFFER:
-		retcode = nvme_trans_write_buffer(ns, hdr, cmd);
-		break;
-	case UNMAP:
-		retcode = nvme_trans_unmap(ns, hdr, cmd);
-		break;
-	default:
- out:
-		retcode = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
-				ILLEGAL_REQUEST, SCSI_ASC_ILLEGAL_COMMAND,
-				SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		break;
-	}
-	return retcode;
-}
-
-int nvme_sg_io(struct nvme_ns *ns, struct sg_io_hdr __user *u_hdr)
-{
-	struct sg_io_hdr hdr;
-	int retcode;
-
-	if (!capable(CAP_SYS_ADMIN))
-		return -EACCES;
-	if (copy_from_user(&hdr, u_hdr, sizeof(hdr)))
-		return -EFAULT;
-	if (hdr.interface_id != 'S')
-		return -EINVAL;
-
-	/*
-	 * A positive return code means a NVMe status, which has been
-	 * translated to sense data.
-	 */
-	retcode = nvme_scsi_translate(ns, &hdr);
-	if (retcode < 0)
-		return retcode;
-	if (copy_to_user(u_hdr, &hdr, sizeof(sg_io_hdr_t)) > 0)
-		return -EFAULT;
-	return 0;
-}
-
-int nvme_sg_get_version_num(int __user *ip)
-{
-	return put_user(sg_version_num, ip);
-}
diff --git a/drivers/nvme/target/admin-cmd.c b/drivers/nvme/target/admin-cmd.c
index ff1f97006322..35f930db3c02 100644
--- a/drivers/nvme/target/admin-cmd.c
+++ b/drivers/nvme/target/admin-cmd.c
@@ -336,7 +336,7 @@ out:
 
 static void nvmet_execute_identify_nslist(struct nvmet_req *req)
 {
-	static const int buf_size = 4096;
+	static const int buf_size = NVME_IDENTIFY_DATA_SIZE;
 	struct nvmet_ctrl *ctrl = req->sq->ctrl;
 	struct nvmet_ns *ns;
 	u32 min_nsid = le32_to_cpu(req->cmd->identify.nsid);
@@ -367,6 +367,64 @@ out:
 	nvmet_req_complete(req, status);
 }
 
+static u16 nvmet_copy_ns_identifier(struct nvmet_req *req, u8 type, u8 len,
+				    void *id, off_t *off)
+{
+	struct nvme_ns_id_desc desc = {
+		.nidt = type,
+		.nidl = len,
+	};
+	u16 status;
+
+	status = nvmet_copy_to_sgl(req, *off, &desc, sizeof(desc));
+	if (status)
+		return status;
+	*off += sizeof(desc);
+
+	status = nvmet_copy_to_sgl(req, *off, id, len);
+	if (status)
+		return status;
+	*off += len;
+
+	return 0;
+}
+
+static void nvmet_execute_identify_desclist(struct nvmet_req *req)
+{
+	struct nvmet_ns *ns;
+	u16 status = 0;
+	off_t off = 0;
+
+	ns = nvmet_find_namespace(req->sq->ctrl, req->cmd->identify.nsid);
+	if (!ns) {
+		status = NVME_SC_INVALID_NS | NVME_SC_DNR;
+		goto out;
+	}
+
+	if (memchr_inv(&ns->uuid, 0, sizeof(ns->uuid))) {
+		status = nvmet_copy_ns_identifier(req, NVME_NIDT_UUID,
+						  NVME_NIDT_UUID_LEN,
+						  &ns->uuid, &off);
+		if (status)
+			goto out_put_ns;
+	}
+	if (memchr_inv(ns->nguid, 0, sizeof(ns->nguid))) {
+		status = nvmet_copy_ns_identifier(req, NVME_NIDT_NGUID,
+						  NVME_NIDT_NGUID_LEN,
+						  &ns->nguid, &off);
+		if (status)
+			goto out_put_ns;
+	}
+
+	if (sg_zero_buffer(req->sg, req->sg_cnt, NVME_IDENTIFY_DATA_SIZE - off,
+			off) != NVME_IDENTIFY_DATA_SIZE - off)
+		status = NVME_SC_INTERNAL | NVME_SC_DNR;
+out_put_ns:
+	nvmet_put_namespace(ns);
+out:
+	nvmet_req_complete(req, status);
+}
+
 /*
  * A "mimimum viable" abort implementation: the command is mandatory in the
  * spec, but we are not required to do any useful work.  We couldn't really
@@ -504,7 +562,7 @@ u16 nvmet_parse_admin_cmd(struct nvmet_req *req)
 		}
 		break;
 	case nvme_admin_identify:
-		req->data_len = 4096;
+		req->data_len = NVME_IDENTIFY_DATA_SIZE;
 		switch (cmd->identify.cns) {
 		case NVME_ID_CNS_NS:
 			req->execute = nvmet_execute_identify_ns;
@@ -515,6 +573,9 @@ u16 nvmet_parse_admin_cmd(struct nvmet_req *req)
 		case NVME_ID_CNS_NS_ACTIVE_LIST:
 			req->execute = nvmet_execute_identify_nslist;
 			return 0;
+		case NVME_ID_CNS_NS_DESC_LIST:
+			req->execute = nvmet_execute_identify_desclist;
+			return 0;
 		}
 		break;
 	case nvme_admin_abort_cmd:
diff --git a/drivers/nvme/target/configfs.c b/drivers/nvme/target/configfs.c
index be8c800078e2..a358ecd93e11 100644
--- a/drivers/nvme/target/configfs.c
+++ b/drivers/nvme/target/configfs.c
@@ -305,11 +305,41 @@ out_unlock:
 
 CONFIGFS_ATTR(nvmet_ns_, device_path);
 
+static ssize_t nvmet_ns_device_uuid_show(struct config_item *item, char *page)
+{
+	return sprintf(page, "%pUb\n", &to_nvmet_ns(item)->uuid);
+}
+
+static ssize_t nvmet_ns_device_uuid_store(struct config_item *item,
+					  const char *page, size_t count)
+{
+	struct nvmet_ns *ns = to_nvmet_ns(item);
+	struct nvmet_subsys *subsys = ns->subsys;
+	int ret = 0;
+
+
+	mutex_lock(&subsys->lock);
+	if (ns->enabled) {
+		ret = -EBUSY;
+		goto out_unlock;
+	}
+
+
+	if (uuid_parse(page, &ns->uuid))
+		ret = -EINVAL;
+
+out_unlock:
+	mutex_unlock(&subsys->lock);
+	return ret ? ret : count;
+}
+
 static ssize_t nvmet_ns_device_nguid_show(struct config_item *item, char *page)
 {
 	return sprintf(page, "%pUb\n", &to_nvmet_ns(item)->nguid);
 }
 
+CONFIGFS_ATTR(nvmet_ns_, device_uuid);
+
 static ssize_t nvmet_ns_device_nguid_store(struct config_item *item,
 		const char *page, size_t count)
 {
@@ -379,6 +409,7 @@ CONFIGFS_ATTR(nvmet_ns_, enable);
 static struct configfs_attribute *nvmet_ns_attrs[] = {
 	&nvmet_ns_attr_device_path,
 	&nvmet_ns_attr_device_nguid,
+	&nvmet_ns_attr_device_uuid,
 	&nvmet_ns_attr_enable,
 	NULL,
 };
@@ -619,8 +650,45 @@ out_unlock:
 
 CONFIGFS_ATTR(nvmet_subsys_, attr_allow_any_host);
 
+static ssize_t nvmet_subsys_version_show(struct config_item *item,
+					      char *page)
+{
+	struct nvmet_subsys *subsys = to_subsys(item);
+
+	if (NVME_TERTIARY(subsys->ver))
+		return snprintf(page, PAGE_SIZE, "%d.%d.%d\n",
+				(int)NVME_MAJOR(subsys->ver),
+				(int)NVME_MINOR(subsys->ver),
+				(int)NVME_TERTIARY(subsys->ver));
+	else
+		return snprintf(page, PAGE_SIZE, "%d.%d\n",
+				(int)NVME_MAJOR(subsys->ver),
+				(int)NVME_MINOR(subsys->ver));
+}
+
+static ssize_t nvmet_subsys_version_store(struct config_item *item,
+					       const char *page, size_t count)
+{
+	struct nvmet_subsys *subsys = to_subsys(item);
+	int major, minor, tertiary = 0;
+	int ret;
+
+
+	ret = sscanf(page, "%d.%d.%d\n", &major, &minor, &tertiary);
+	if (ret != 2 && ret != 3)
+		return -EINVAL;
+
+	down_write(&nvmet_config_sem);
+	subsys->ver = NVME_VS(major, minor, tertiary);
+	up_write(&nvmet_config_sem);
+
+	return count;
+}
+CONFIGFS_ATTR(nvmet_subsys_, version);
+
 static struct configfs_attribute *nvmet_subsys_attrs[] = {
 	&nvmet_subsys_attr_attr_allow_any_host,
+	&nvmet_subsys_attr_version,
 	NULL,
 };
 
diff --git a/drivers/nvme/target/core.c b/drivers/nvme/target/core.c
index eb9399ac97cf..b5b4ac103748 100644
--- a/drivers/nvme/target/core.c
+++ b/drivers/nvme/target/core.c
@@ -380,6 +380,7 @@ struct nvmet_ns *nvmet_ns_alloc(struct nvmet_subsys *subsys, u32 nsid)
 
 	ns->nsid = nsid;
 	ns->subsys = subsys;
+	uuid_gen(&ns->uuid);
 
 	return ns;
 }
@@ -926,7 +927,7 @@ struct nvmet_subsys *nvmet_subsys_alloc(const char *subsysnqn,
 	if (!subsys)
 		return NULL;
 
-	subsys->ver = NVME_VS(1, 2, 1); /* NVMe 1.2.1 */
+	subsys->ver = NVME_VS(1, 3, 0); /* NVMe 1.3.0 */
 
 	switch (type) {
 	case NVME_NQN_NVME:
diff --git a/drivers/nvme/target/discovery.c b/drivers/nvme/target/discovery.c
index 1aaf597e81fc..8f3b57b4c97b 100644
--- a/drivers/nvme/target/discovery.c
+++ b/drivers/nvme/target/discovery.c
@@ -53,7 +53,7 @@ static void nvmet_format_discovery_entry(struct nvmf_disc_rsp_page_hdr *hdr,
 	e->portid = port->disc_addr.portid;
 	/* we support only dynamic controllers */
 	e->cntlid = cpu_to_le16(NVME_CNTLID_DYNAMIC);
-	e->asqsz = cpu_to_le16(NVMF_AQ_DEPTH);
+	e->asqsz = cpu_to_le16(NVME_AQ_DEPTH);
 	e->subtype = type;
 	memcpy(e->trsvcid, port->disc_addr.trsvcid, NVMF_TRSVCID_SIZE);
 	memcpy(e->traddr, port->disc_addr.traddr, NVMF_TRADDR_SIZE);
@@ -185,7 +185,7 @@ u16 nvmet_parse_discovery_cmd(struct nvmet_req *req)
 		return NVME_SC_INVALID_OPCODE | NVME_SC_DNR;
 		}
 	case nvme_admin_identify:
-		req->data_len = 4096;
+		req->data_len = NVME_IDENTIFY_DATA_SIZE;
 		switch (cmd->identify.cns) {
 		case NVME_ID_CNS_CTRL:
 			req->execute =
diff --git a/drivers/nvme/target/fc.c b/drivers/nvme/target/fc.c
index 2006fae61980..7692a96c9065 100644
--- a/drivers/nvme/target/fc.c
+++ b/drivers/nvme/target/fc.c
@@ -2096,20 +2096,22 @@ nvmet_fc_handle_fcp_rqst(struct nvmet_fc_tgtport *tgtport,
 	/* clear any response payload */
 	memset(&fod->rspiubuf, 0, sizeof(fod->rspiubuf));
 
+	fod->data_sg = NULL;
+	fod->data_sg_cnt = 0;
+
 	ret = nvmet_req_init(&fod->req,
 				&fod->queue->nvme_cq,
 				&fod->queue->nvme_sq,
 				&nvmet_fc_tgt_fcp_ops);
-	if (!ret) {	/* bad SQE content or invalid ctrl state */
-		nvmet_fc_abort_op(tgtport, fod);
+	if (!ret) {
+		/* bad SQE content or invalid ctrl state */
+		/* nvmet layer has already called op done to send rsp. */
 		return;
 	}
 
 	/* keep a running counter of tail position */
 	atomic_inc(&fod->queue->sqtail);
 
-	fod->data_sg = NULL;
-	fod->data_sg_cnt = 0;
 	if (fod->total_length) {
 		ret = nvmet_fc_alloc_tgt_pgs(fod);
 		if (ret) {
diff --git a/drivers/nvme/target/fcloop.c b/drivers/nvme/target/fcloop.c
index 294a6611fb24..1bb9d5b311b1 100644
--- a/drivers/nvme/target/fcloop.c
+++ b/drivers/nvme/target/fcloop.c
@@ -569,7 +569,6 @@ fcloop_tgt_fcp_abort(struct nvmet_fc_target_port *tgtport,
 			struct nvmefc_tgt_fcp_req *tgt_fcpreq)
 {
 	struct fcloop_fcpreq *tfcp_req = tgt_fcp_req_to_fcpreq(tgt_fcpreq);
-	int active;
 
 	/*
 	 * mark aborted only in case there were 2 threads in transport
@@ -577,7 +576,6 @@ fcloop_tgt_fcp_abort(struct nvmet_fc_target_port *tgtport,
 	 * after the abort request
 	 */
 	spin_lock(&tfcp_req->reqlock);
-	active = tfcp_req->active;
 	tfcp_req->aborted = true;
 	spin_unlock(&tfcp_req->reqlock);
 
diff --git a/drivers/nvme/target/io-cmd.c b/drivers/nvme/target/io-cmd.c
index c77940d80fc8..40128793e613 100644
--- a/drivers/nvme/target/io-cmd.c
+++ b/drivers/nvme/target/io-cmd.c
@@ -21,7 +21,7 @@ static void nvmet_bio_done(struct bio *bio)
 	struct nvmet_req *req = bio->bi_private;
 
 	nvmet_req_complete(req,
-		bio->bi_error ? NVME_SC_INTERNAL | NVME_SC_DNR : 0);
+		bio->bi_status ? NVME_SC_INTERNAL | NVME_SC_DNR : 0);
 
 	if (bio != &req->inline_bio)
 		bio_put(bio);
@@ -145,7 +145,7 @@ static void nvmet_execute_discard(struct nvmet_req *req)
 		bio->bi_private = req;
 		bio->bi_end_io = nvmet_bio_done;
 		if (status) {
-			bio->bi_error = -EIO;
+			bio->bi_status = BLK_STS_IOERR;
 			bio_endio(bio);
 		} else {
 			submit_bio(bio);
diff --git a/drivers/nvme/target/loop.c b/drivers/nvme/target/loop.c
index e503cfff0337..5f55c683b338 100644
--- a/drivers/nvme/target/loop.c
+++ b/drivers/nvme/target/loop.c
@@ -21,8 +21,6 @@
 #include "../host/nvme.h"
 #include "../host/fabrics.h"
 
-#define NVME_LOOP_AQ_DEPTH		256
-
 #define NVME_LOOP_MAX_SEGMENTS		256
 
 /*
@@ -31,7 +29,7 @@
  */
 #define NVME_LOOP_NR_AEN_COMMANDS	1
 #define NVME_LOOP_AQ_BLKMQ_DEPTH	\
-	(NVME_LOOP_AQ_DEPTH - NVME_LOOP_NR_AEN_COMMANDS)
+	(NVME_AQ_DEPTH - NVME_LOOP_NR_AEN_COMMANDS)
 
 struct nvme_loop_iod {
 	struct nvme_request	nvme_req;
@@ -45,7 +43,6 @@ struct nvme_loop_iod {
 };
 
 struct nvme_loop_ctrl {
-	spinlock_t		lock;
 	struct nvme_loop_queue	*queues;
 	u32			queue_count;
 
@@ -59,7 +56,6 @@ struct nvme_loop_ctrl {
 
 	struct nvmet_ctrl	*target_ctrl;
 	struct work_struct	delete_work;
-	struct work_struct	reset_work;
 };
 
 static inline struct nvme_loop_ctrl *to_loop_ctrl(struct nvme_ctrl *ctrl)
@@ -151,7 +147,7 @@ nvme_loop_timeout(struct request *rq, bool reserved)
 	struct nvme_loop_iod *iod = blk_mq_rq_to_pdu(rq);
 
 	/* queue error recovery */
-	schedule_work(&iod->queue->ctrl->reset_work);
+	nvme_reset_ctrl(&iod->queue->ctrl->ctrl);
 
 	/* fail with DNR on admin cmd timeout */
 	nvme_req(rq)->status = NVME_SC_ABORT_REQ | NVME_SC_DNR;
@@ -159,17 +155,17 @@ nvme_loop_timeout(struct request *rq, bool reserved)
 	return BLK_EH_HANDLED;
 }
 
-static int nvme_loop_queue_rq(struct blk_mq_hw_ctx *hctx,
+static blk_status_t nvme_loop_queue_rq(struct blk_mq_hw_ctx *hctx,
 		const struct blk_mq_queue_data *bd)
 {
 	struct nvme_ns *ns = hctx->queue->queuedata;
 	struct nvme_loop_queue *queue = hctx->driver_data;
 	struct request *req = bd->rq;
 	struct nvme_loop_iod *iod = blk_mq_rq_to_pdu(req);
-	int ret;
+	blk_status_t ret;
 
 	ret = nvme_setup_cmd(ns, req, &iod->cmd);
-	if (ret != BLK_MQ_RQ_QUEUE_OK)
+	if (ret)
 		return ret;
 
 	iod->cmd.common.flags |= NVME_CMD_SGL_METABUF;
@@ -179,16 +175,15 @@ static int nvme_loop_queue_rq(struct blk_mq_hw_ctx *hctx,
 		nvme_cleanup_cmd(req);
 		blk_mq_start_request(req);
 		nvme_loop_queue_response(&iod->req);
-		return BLK_MQ_RQ_QUEUE_OK;
+		return BLK_STS_OK;
 	}
 
 	if (blk_rq_bytes(req)) {
 		iod->sg_table.sgl = iod->first_sgl;
-		ret = sg_alloc_table_chained(&iod->sg_table,
+		if (sg_alloc_table_chained(&iod->sg_table,
 				blk_rq_nr_phys_segments(req),
-				iod->sg_table.sgl);
-		if (ret)
-			return BLK_MQ_RQ_QUEUE_BUSY;
+				iod->sg_table.sgl))
+			return BLK_STS_RESOURCE;
 
 		iod->req.sg = iod->sg_table.sgl;
 		iod->req.sg_cnt = blk_rq_map_sg(req->q, req, iod->sg_table.sgl);
@@ -197,7 +192,7 @@ static int nvme_loop_queue_rq(struct blk_mq_hw_ctx *hctx,
 	blk_mq_start_request(req);
 
 	schedule_work(&iod->work);
-	return BLK_MQ_RQ_QUEUE_OK;
+	return BLK_STS_OK;
 }
 
 static void nvme_loop_submit_async_event(struct nvme_ctrl *arg, int aer_idx)
@@ -234,15 +229,10 @@ static int nvme_loop_init_request(struct blk_mq_tag_set *set,
 		struct request *req, unsigned int hctx_idx,
 		unsigned int numa_node)
 {
-	return nvme_loop_init_iod(set->driver_data, blk_mq_rq_to_pdu(req),
-			hctx_idx + 1);
-}
+	struct nvme_loop_ctrl *ctrl = set->driver_data;
 
-static int nvme_loop_init_admin_request(struct blk_mq_tag_set *set,
-		struct request *req, unsigned int hctx_idx,
-		unsigned int numa_node)
-{
-	return nvme_loop_init_iod(set->driver_data, blk_mq_rq_to_pdu(req), 0);
+	return nvme_loop_init_iod(ctrl, blk_mq_rq_to_pdu(req),
+			(set == &ctrl->tag_set) ? hctx_idx + 1 : 0);
 }
 
 static int nvme_loop_init_hctx(struct blk_mq_hw_ctx *hctx, void *data,
@@ -280,7 +270,7 @@ static const struct blk_mq_ops nvme_loop_mq_ops = {
 static const struct blk_mq_ops nvme_loop_admin_mq_ops = {
 	.queue_rq	= nvme_loop_queue_rq,
 	.complete	= nvme_loop_complete_rq,
-	.init_request	= nvme_loop_init_admin_request,
+	.init_request	= nvme_loop_init_request,
 	.init_hctx	= nvme_loop_init_admin_hctx,
 	.timeout	= nvme_loop_timeout,
 };
@@ -467,7 +457,7 @@ static int __nvme_loop_del_ctrl(struct nvme_loop_ctrl *ctrl)
 	if (!nvme_change_ctrl_state(&ctrl->ctrl, NVME_CTRL_DELETING))
 		return -EBUSY;
 
-	if (!schedule_work(&ctrl->delete_work))
+	if (!queue_work(nvme_wq, &ctrl->delete_work))
 		return -EBUSY;
 
 	return 0;
@@ -501,8 +491,8 @@ static void nvme_loop_delete_ctrl(struct nvmet_ctrl *nctrl)
 
 static void nvme_loop_reset_ctrl_work(struct work_struct *work)
 {
-	struct nvme_loop_ctrl *ctrl = container_of(work,
-					struct nvme_loop_ctrl, reset_work);
+	struct nvme_loop_ctrl *ctrl =
+		container_of(work, struct nvme_loop_ctrl, ctrl.reset_work);
 	bool changed;
 	int ret;
 
@@ -540,21 +530,6 @@ out_disable:
 	nvme_put_ctrl(&ctrl->ctrl);
 }
 
-static int nvme_loop_reset_ctrl(struct nvme_ctrl *nctrl)
-{
-	struct nvme_loop_ctrl *ctrl = to_loop_ctrl(nctrl);
-
-	if (!nvme_change_ctrl_state(&ctrl->ctrl, NVME_CTRL_RESETTING))
-		return -EBUSY;
-
-	if (!schedule_work(&ctrl->reset_work))
-		return -EBUSY;
-
-	flush_work(&ctrl->reset_work);
-
-	return 0;
-}
-
 static const struct nvme_ctrl_ops nvme_loop_ctrl_ops = {
 	.name			= "loop",
 	.module			= THIS_MODULE,
@@ -562,11 +537,9 @@ static const struct nvme_ctrl_ops nvme_loop_ctrl_ops = {
 	.reg_read32		= nvmf_reg_read32,
 	.reg_read64		= nvmf_reg_read64,
 	.reg_write32		= nvmf_reg_write32,
-	.reset_ctrl		= nvme_loop_reset_ctrl,
 	.free_ctrl		= nvme_loop_free_ctrl,
 	.submit_async_event	= nvme_loop_submit_async_event,
 	.delete_ctrl		= nvme_loop_del_ctrl,
-	.get_subsysnqn		= nvmf_get_subsysnqn,
 };
 
 static int nvme_loop_create_io_queues(struct nvme_loop_ctrl *ctrl)
@@ -629,15 +602,13 @@ static struct nvme_ctrl *nvme_loop_create_ctrl(struct device *dev,
 	INIT_LIST_HEAD(&ctrl->list);
 
 	INIT_WORK(&ctrl->delete_work, nvme_loop_del_ctrl_work);
-	INIT_WORK(&ctrl->reset_work, nvme_loop_reset_ctrl_work);
+	INIT_WORK(&ctrl->ctrl.reset_work, nvme_loop_reset_ctrl_work);
 
 	ret = nvme_init_ctrl(&ctrl->ctrl, dev, &nvme_loop_ctrl_ops,
 				0 /* no quirks, we're perfect! */);
 	if (ret)
 		goto out_put_ctrl;
 
-	spin_lock_init(&ctrl->lock);
-
 	ret = -ENOMEM;
 
 	ctrl->ctrl.sqsize = opts->queue_size - 1;
@@ -766,7 +737,7 @@ static void __exit nvme_loop_cleanup_module(void)
 		__nvme_loop_del_ctrl(ctrl);
 	mutex_unlock(&nvme_loop_ctrl_mutex);
 
-	flush_scheduled_work();
+	flush_workqueue(nvme_wq);
 }
 
 module_init(nvme_loop_init_module);
diff --git a/drivers/nvme/target/nvmet.h b/drivers/nvme/target/nvmet.h
index cfc5c7fb0ab7..747bbdb4f9c6 100644
--- a/drivers/nvme/target/nvmet.h
+++ b/drivers/nvme/target/nvmet.h
@@ -21,6 +21,7 @@
 #include <linux/percpu-refcount.h>
 #include <linux/list.h>
 #include <linux/mutex.h>
+#include <linux/uuid.h>
 #include <linux/nvme.h>
 #include <linux/configfs.h>
 #include <linux/rcupdate.h>
@@ -46,6 +47,7 @@ struct nvmet_ns {
 	u32			blksize_shift;
 	loff_t			size;
 	u8			nguid[16];
+	uuid_t			uuid;
 
 	bool			enabled;
 	struct nvmet_subsys	*subsys;
diff --git a/drivers/nvme/target/rdma.c b/drivers/nvme/target/rdma.c
index 9e45cde63376..56a4cba690b5 100644
--- a/drivers/nvme/target/rdma.c
+++ b/drivers/nvme/target/rdma.c
@@ -1027,7 +1027,7 @@ nvmet_rdma_parse_cm_connect_req(struct rdma_conn_param *conn,
 	queue->recv_queue_size = le16_to_cpu(req->hsqsize) + 1;
 	queue->send_queue_size = le16_to_cpu(req->hrqsize);
 
-	if (!queue->host_qid && queue->recv_queue_size > NVMF_AQ_DEPTH)
+	if (!queue->host_qid && queue->recv_queue_size > NVME_AQ_DEPTH)
 		return NVME_RDMA_CM_INVALID_HSQSIZE;
 
 	/* XXX: Should we enforce some kind of max for IO queues? */
@@ -1307,53 +1307,44 @@ static void nvmet_rdma_queue_connect_fail(struct rdma_cm_id *cm_id,
 
 /**
  * nvme_rdma_device_removal() - Handle RDMA device removal
+ * @cm_id:	rdma_cm id, used for nvmet port
  * @queue:      nvmet rdma queue (cm id qp_context)
- * @addr:	nvmet address (cm_id context)
  *
  * DEVICE_REMOVAL event notifies us that the RDMA device is about
- * to unplug so we should take care of destroying our RDMA resources.
- * This event will be generated for each allocated cm_id.
+ * to unplug. Note that this event can be generated on a normal
+ * queue cm_id and/or a device bound listener cm_id (where in this
+ * case queue will be null).
  *
- * Note that this event can be generated on a normal queue cm_id
- * and/or a device bound listener cm_id (where in this case
- * queue will be null).
- *
- * we claim ownership on destroying the cm_id. For queues we move
- * the queue state to NVMET_RDMA_IN_DEVICE_REMOVAL and for port
+ * We registered an ib_client to handle device removal for queues,
+ * so we only need to handle the listening port cm_ids. In this case
  * we nullify the priv to prevent double cm_id destruction and destroying
  * the cm_id implicitely by returning a non-zero rc to the callout.
  */
 static int nvmet_rdma_device_removal(struct rdma_cm_id *cm_id,
 		struct nvmet_rdma_queue *queue)
 {
-	unsigned long flags;
-
-	if (!queue) {
-		struct nvmet_port *port = cm_id->context;
+	struct nvmet_port *port;
 
+	if (queue) {
 		/*
-		 * This is a listener cm_id. Make sure that
-		 * future remove_port won't invoke a double
-		 * cm_id destroy. use atomic xchg to make sure
-		 * we don't compete with remove_port.
-		 */
-		if (xchg(&port->priv, NULL) != cm_id)
-			return 0;
-	} else {
-		/*
-		 * This is a queue cm_id. Make sure that
-		 * release queue will not destroy the cm_id
-		 * and schedule all ctrl queues removal (only
-		 * if the queue is not disconnecting already).
+		 * This is a queue cm_id. we have registered
+		 * an ib_client to handle queues removal
+		 * so don't interfear and just return.
 		 */
-		spin_lock_irqsave(&queue->state_lock, flags);
-		if (queue->state != NVMET_RDMA_Q_DISCONNECTING)
-			queue->state = NVMET_RDMA_IN_DEVICE_REMOVAL;
-		spin_unlock_irqrestore(&queue->state_lock, flags);
-		nvmet_rdma_queue_disconnect(queue);
-		flush_scheduled_work();
+		return 0;
 	}
 
+	port = cm_id->context;
+
+	/*
+	 * This is a listener cm_id. Make sure that
+	 * future remove_port won't invoke a double
+	 * cm_id destroy. use atomic xchg to make sure
+	 * we don't compete with remove_port.
+	 */
+	if (xchg(&port->priv, NULL) != cm_id)
+		return 0;
+
 	/*
 	 * We need to return 1 so that the core will destroy
 	 * it's own ID.  What a great API design..
@@ -1519,9 +1510,51 @@ static struct nvmet_fabrics_ops nvmet_rdma_ops = {
 	.delete_ctrl		= nvmet_rdma_delete_ctrl,
 };
 
+static void nvmet_rdma_add_one(struct ib_device *ib_device)
+{
+}
+
+static void nvmet_rdma_remove_one(struct ib_device *ib_device, void *client_data)
+{
+	struct nvmet_rdma_queue *queue;
+
+	/* Device is being removed, delete all queues using this device */
+	mutex_lock(&nvmet_rdma_queue_mutex);
+	list_for_each_entry(queue, &nvmet_rdma_queue_list, queue_list) {
+		if (queue->dev->device != ib_device)
+			continue;
+
+		pr_info("Removing queue %d\n", queue->idx);
+		__nvmet_rdma_queue_disconnect(queue);
+	}
+	mutex_unlock(&nvmet_rdma_queue_mutex);
+
+	flush_scheduled_work();
+}
+
+static struct ib_client nvmet_rdma_ib_client = {
+	.name   = "nvmet_rdma",
+	.add = nvmet_rdma_add_one,
+	.remove = nvmet_rdma_remove_one
+};
+
 static int __init nvmet_rdma_init(void)
 {
-	return nvmet_register_transport(&nvmet_rdma_ops);
+	int ret;
+
+	ret = ib_register_client(&nvmet_rdma_ib_client);
+	if (ret)
+		return ret;
+
+	ret = nvmet_register_transport(&nvmet_rdma_ops);
+	if (ret)
+		goto err_ib_client;
+
+	return 0;
+
+err_ib_client:
+	ib_unregister_client(&nvmet_rdma_ib_client);
+	return ret;
 }
 
 static void __exit nvmet_rdma_exit(void)
@@ -1544,6 +1577,7 @@ static void __exit nvmet_rdma_exit(void)
 	mutex_unlock(&nvmet_rdma_queue_mutex);
 
 	flush_scheduled_work();
+	ib_unregister_client(&nvmet_rdma_ib_client);
 	ida_destroy(&nvmet_rdma_queue_ida);
 }
 
diff --git a/drivers/nvmem/bcm-ocotp.c b/drivers/nvmem/bcm-ocotp.c
index 646cadbf1f93..3c56e3b2bd65 100644
--- a/drivers/nvmem/bcm-ocotp.c
+++ b/drivers/nvmem/bcm-ocotp.c
@@ -34,7 +34,7 @@
 #define OTPC_CMD_READ                0x0
 #define OTPC_CMD_OTP_PROG_ENABLE     0x2
 #define OTPC_CMD_OTP_PROG_DISABLE    0x3
-#define OTPC_CMD_PROGRAM             0xA
+#define OTPC_CMD_PROGRAM             0x8
 
 /* OTPC Status Bits */
 #define OTPC_STAT_CMD_DONE           BIT(1)
@@ -209,7 +209,7 @@ static int bcm_otpc_write(void *context, unsigned int offset, void *val,
 		set_command(priv->base, OTPC_CMD_PROGRAM);
 		set_cpu_address(priv->base, address++);
 		for (i = 0; i < priv->map->otpc_row_size; i++) {
-			writel(*buf, priv->base + priv->map->data_r_offset[i]);
+			writel(*buf, priv->base + priv->map->data_w_offset[i]);
 			buf++;
 			bytes_written += sizeof(*buf);
 		}
diff --git a/drivers/nvmem/core.c b/drivers/nvmem/core.c
index 8c830a80a648..4c49285168fb 100644
--- a/drivers/nvmem/core.c
+++ b/drivers/nvmem/core.c
@@ -287,9 +287,15 @@ static struct nvmem_cell *nvmem_find_cell(const char *cell_id)
 {
 	struct nvmem_cell *p;
 
+	mutex_lock(&nvmem_cells_mutex);
+
 	list_for_each_entry(p, &nvmem_cells, node)
-		if (p && !strcmp(p->name, cell_id))
+		if (p && !strcmp(p->name, cell_id)) {
+			mutex_unlock(&nvmem_cells_mutex);
 			return p;
+		}
+
+	mutex_unlock(&nvmem_cells_mutex);
 
 	return NULL;
 }
@@ -489,21 +495,24 @@ struct nvmem_device *nvmem_register(const struct nvmem_config *config)
 
 	rval = device_add(&nvmem->dev);
 	if (rval)
-		goto out;
+		goto err_put_device;
 
 	if (config->compat) {
 		rval = nvmem_setup_compat(nvmem, config);
 		if (rval)
-			goto out;
+			goto err_device_del;
 	}
 
 	if (config->cells)
 		nvmem_add_cells(nvmem, config);
 
 	return nvmem;
-out:
-	ida_simple_remove(&nvmem_ida, nvmem->id);
-	kfree(nvmem);
+
+err_device_del:
+	device_del(&nvmem->dev);
+err_put_device:
+	put_device(&nvmem->dev);
+
 	return ERR_PTR(rval);
 }
 EXPORT_SYMBOL_GPL(nvmem_register);
@@ -529,6 +538,7 @@ int nvmem_unregister(struct nvmem_device *nvmem)
 
 	nvmem_device_remove_all_cells(nvmem);
 	device_del(&nvmem->dev);
+	put_device(&nvmem->dev);
 
 	return 0;
 }
diff --git a/drivers/nvmem/rockchip-efuse.c b/drivers/nvmem/rockchip-efuse.c
index 423907bdd259..a0d4ede9b8fc 100644
--- a/drivers/nvmem/rockchip-efuse.c
+++ b/drivers/nvmem/rockchip-efuse.c
@@ -170,6 +170,10 @@ static const struct of_device_id rockchip_efuse_match[] = {
 		.data = (void *)&rockchip_rk3288_efuse_read,
 	},
 	{
+		.compatible = "rockchip,rk322x-efuse",
+		.data = (void *)&rockchip_rk3288_efuse_read,
+	},
+	{
 		.compatible = "rockchip,rk3288-efuse",
 		.data = (void *)&rockchip_rk3288_efuse_read,
 	},
diff --git a/drivers/of/base.c b/drivers/of/base.c
index 28d5f53bc631..cb1c49ae3b88 100644
--- a/drivers/of/base.c
+++ b/drivers/of/base.c
@@ -1601,6 +1601,7 @@ int of_phandle_iterator_init(struct of_phandle_iterator *it,
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(of_phandle_iterator_init);
 
 int of_phandle_iterator_next(struct of_phandle_iterator *it)
 {
@@ -1670,6 +1671,7 @@ err:
 
 	return -EINVAL;
 }
+EXPORT_SYMBOL_GPL(of_phandle_iterator_next);
 
 int of_phandle_iterator_args(struct of_phandle_iterator *it,
 			     uint32_t *args,
@@ -2485,6 +2487,41 @@ struct device_node *of_graph_get_endpoint_by_regs(
 EXPORT_SYMBOL(of_graph_get_endpoint_by_regs);
 
 /**
+ * of_graph_get_remote_endpoint() - get remote endpoint node
+ * @node: pointer to a local endpoint device_node
+ *
+ * Return: Remote endpoint node associated with remote endpoint node linked
+ *	   to @node. Use of_node_put() on it when done.
+ */
+struct device_node *of_graph_get_remote_endpoint(const struct device_node *node)
+{
+	/* Get remote endpoint node. */
+	return of_parse_phandle(node, "remote-endpoint", 0);
+}
+EXPORT_SYMBOL(of_graph_get_remote_endpoint);
+
+/**
+ * of_graph_get_port_parent() - get port's parent node
+ * @node: pointer to a local endpoint device_node
+ *
+ * Return: device node associated with endpoint node linked
+ *	   to @node. Use of_node_put() on it when done.
+ */
+struct device_node *of_graph_get_port_parent(struct device_node *node)
+{
+	unsigned int depth;
+
+	/* Walk 3 levels up only if there is 'ports' node. */
+	for (depth = 3; depth && node; depth--) {
+		node = of_get_next_parent(node);
+		if (depth == 2 && of_node_cmp(node->name, "ports"))
+			break;
+	}
+	return node;
+}
+EXPORT_SYMBOL(of_graph_get_port_parent);
+
+/**
  * of_graph_get_remote_port_parent() - get remote port's parent node
  * @node: pointer to a local endpoint device_node
  *
@@ -2495,18 +2532,11 @@ struct device_node *of_graph_get_remote_port_parent(
 			       const struct device_node *node)
 {
 	struct device_node *np;
-	unsigned int depth;
 
 	/* Get remote endpoint node. */
-	np = of_parse_phandle(node, "remote-endpoint", 0);
+	np = of_graph_get_remote_endpoint(node);
 
-	/* Walk 3 levels up only if there is 'ports' node. */
-	for (depth = 3; depth && np; depth--) {
-		np = of_get_next_parent(np);
-		if (depth == 2 && of_node_cmp(np->name, "ports"))
-			break;
-	}
-	return np;
+	return of_graph_get_port_parent(np);
 }
 EXPORT_SYMBOL(of_graph_get_remote_port_parent);
 
@@ -2522,13 +2552,25 @@ struct device_node *of_graph_get_remote_port(const struct device_node *node)
 	struct device_node *np;
 
 	/* Get remote endpoint node. */
-	np = of_parse_phandle(node, "remote-endpoint", 0);
+	np = of_graph_get_remote_endpoint(node);
 	if (!np)
 		return NULL;
 	return of_get_next_parent(np);
 }
 EXPORT_SYMBOL(of_graph_get_remote_port);
 
+int of_graph_get_endpoint_count(const struct device_node *np)
+{
+	struct device_node *endpoint;
+	int num = 0;
+
+	for_each_endpoint_of_node(np, endpoint)
+		num++;
+
+	return num;
+}
+EXPORT_SYMBOL(of_graph_get_endpoint_count);
+
 /**
  * of_graph_get_remote_node() - get remote parent device_node for given port/endpoint
  * @node: pointer to parent device_node containing graph port/endpoint
diff --git a/drivers/of/of_mdio.c b/drivers/of/of_mdio.c
index 7e4c80f9b6cd..e0dbd6e48a98 100644
--- a/drivers/of/of_mdio.c
+++ b/drivers/of/of_mdio.c
@@ -119,28 +119,6 @@ static void of_mdiobus_register_device(struct mii_bus *mdio,
 		child->name, addr);
 }
 
-int of_mdio_parse_addr(struct device *dev, const struct device_node *np)
-{
-	u32 addr;
-	int ret;
-
-	ret = of_property_read_u32(np, "reg", &addr);
-	if (ret < 0) {
-		dev_err(dev, "%s has invalid PHY address\n", np->full_name);
-		return ret;
-	}
-
-	/* A PHY must have a reg property in the range [0-31] */
-	if (addr >= PHY_MAX_ADDR) {
-		dev_err(dev, "%s PHY address %i is too large\n",
-			np->full_name, addr);
-		return -EINVAL;
-	}
-
-	return addr;
-}
-EXPORT_SYMBOL(of_mdio_parse_addr);
-
 /* The following is a list of PHY compatible strings which appear in
  * some DTBs. The compatible string is never matched against a PHY
  * driver, so is pointless. We only expect devices which are not PHYs
@@ -226,7 +204,6 @@ int of_mdiobus_register(struct mii_bus *mdio, struct device_node *np)
 	/* Get bus level PHY reset GPIO details */
 	mdio->reset_delay_us = DEFAULT_GPIO_RESET_DELAY;
 	of_property_read_u32(np, "reset-delay-us", &mdio->reset_delay_us);
-	mdio->num_reset_gpios = of_gpio_named_count(np, "reset-gpios");
 
 	/* Register the MDIO bus */
 	rc = mdiobus_register(mdio);
diff --git a/drivers/parisc/ccio-dma.c b/drivers/parisc/ccio-dma.c
index e32ca2ef9e54..6aa1e7f6672f 100644
--- a/drivers/parisc/ccio-dma.c
+++ b/drivers/parisc/ccio-dma.c
@@ -110,6 +110,8 @@
 #define CMD_TLB_DIRECT_WRITE 35         /* IO_COMMAND for I/O TLB Writes     */
 #define CMD_TLB_PURGE        33         /* IO_COMMAND to Purge I/O TLB entry */
 
+#define CCIO_MAPPING_ERROR    (~(dma_addr_t)0)
+
 struct ioa_registers {
         /* Runway Supervisory Set */
         int32_t    unused1[12];
@@ -741,6 +743,8 @@ ccio_map_single(struct device *dev, void *addr, size_t size,
 
 	BUG_ON(!dev);
 	ioc = GET_IOC(dev);
+	if (!ioc)
+		return CCIO_MAPPING_ERROR;
 
 	BUG_ON(size <= 0);
 
@@ -814,6 +818,10 @@ ccio_unmap_page(struct device *dev, dma_addr_t iova, size_t size,
 	
 	BUG_ON(!dev);
 	ioc = GET_IOC(dev);
+	if (!ioc) {
+		WARN_ON(!ioc);
+		return;
+	}
 
 	DBG_RUN("%s() iovp 0x%lx/%x\n",
 		__func__, (long)iova, size);
@@ -918,6 +926,8 @@ ccio_map_sg(struct device *dev, struct scatterlist *sglist, int nents,
 	
 	BUG_ON(!dev);
 	ioc = GET_IOC(dev);
+	if (!ioc)
+		return 0;
 	
 	DBG_RUN_SG("%s() START %d entries\n", __func__, nents);
 
@@ -990,6 +1000,10 @@ ccio_unmap_sg(struct device *dev, struct scatterlist *sglist, int nents,
 
 	BUG_ON(!dev);
 	ioc = GET_IOC(dev);
+	if (!ioc) {
+		WARN_ON(!ioc);
+		return;
+	}
 
 	DBG_RUN_SG("%s() START %d entries, %p,%x\n",
 		__func__, nents, sg_virt(sglist), sglist->length);
@@ -1011,6 +1025,11 @@ ccio_unmap_sg(struct device *dev, struct scatterlist *sglist, int nents,
 	DBG_RUN_SG("%s() DONE (nents %d)\n", __func__, nents);
 }
 
+static int ccio_mapping_error(struct device *dev, dma_addr_t dma_addr)
+{
+	return dma_addr == CCIO_MAPPING_ERROR;
+}
+
 static const struct dma_map_ops ccio_ops = {
 	.dma_supported =	ccio_dma_supported,
 	.alloc =		ccio_alloc,
@@ -1019,6 +1038,7 @@ static const struct dma_map_ops ccio_ops = {
 	.unmap_page =		ccio_unmap_page,
 	.map_sg = 		ccio_map_sg,
 	.unmap_sg = 		ccio_unmap_sg,
+	.mapping_error =	ccio_mapping_error,
 };
 
 #ifdef CONFIG_PROC_FS
diff --git a/drivers/parisc/dino.c b/drivers/parisc/dino.c
index 1133b5cc88ca..5c63b920b471 100644
--- a/drivers/parisc/dino.c
+++ b/drivers/parisc/dino.c
@@ -154,7 +154,10 @@ struct dino_device
 };
 
 /* Looks nice and keeps the compiler happy */
-#define DINO_DEV(d) ((struct dino_device *) d)
+#define DINO_DEV(d) ({				\
+	void *__pdata = d;			\
+	BUG_ON(!__pdata);			\
+	(struct dino_device *)__pdata; })
 
 
 /*
diff --git a/drivers/parisc/lba_pci.c b/drivers/parisc/lba_pci.c
index 2ec2aef4d211..bc286cbbbc9b 100644
--- a/drivers/parisc/lba_pci.c
+++ b/drivers/parisc/lba_pci.c
@@ -111,8 +111,10 @@ static u32 lba_t32;
 
 
 /* Looks nice and keeps the compiler happy */
-#define LBA_DEV(d) ((struct lba_device *) (d))
-
+#define LBA_DEV(d) ({				\
+	void *__pdata = d;			\
+	BUG_ON(!__pdata);			\
+	(struct lba_device *)__pdata; })
 
 /*
 ** Only allow 8 subsidiary busses per LBA
diff --git a/drivers/parisc/sba_iommu.c b/drivers/parisc/sba_iommu.c
index 33385e574433..4086f79d58d5 100644
--- a/drivers/parisc/sba_iommu.c
+++ b/drivers/parisc/sba_iommu.c
@@ -93,6 +93,8 @@
 
 #define DEFAULT_DMA_HINT_REG	0
 
+#define SBA_MAPPING_ERROR    (~(dma_addr_t)0)
+
 struct sba_device *sba_list;
 EXPORT_SYMBOL_GPL(sba_list);
 
@@ -691,6 +693,8 @@ static int sba_dma_supported( struct device *dev, u64 mask)
 		return 0;
 
 	ioc = GET_IOC(dev);
+	if (!ioc)
+		return 0;
 
 	/*
 	 * check if mask is >= than the current max IO Virt Address
@@ -722,6 +726,8 @@ sba_map_single(struct device *dev, void *addr, size_t size,
 	int pide;
 
 	ioc = GET_IOC(dev);
+	if (!ioc)
+		return SBA_MAPPING_ERROR;
 
 	/* save offset bits */
 	offset = ((dma_addr_t) (long) addr) & ~IOVP_MASK;
@@ -813,6 +819,10 @@ sba_unmap_page(struct device *dev, dma_addr_t iova, size_t size,
 	DBG_RUN("%s() iovp 0x%lx/%x\n", __func__, (long) iova, size);
 
 	ioc = GET_IOC(dev);
+	if (!ioc) {
+		WARN_ON(!ioc);
+		return;
+	}
 	offset = iova & ~IOVP_MASK;
 	iova ^= offset;        /* clear offset bits */
 	size += offset;
@@ -952,6 +962,8 @@ sba_map_sg(struct device *dev, struct scatterlist *sglist, int nents,
 	DBG_RUN_SG("%s() START %d entries\n", __func__, nents);
 
 	ioc = GET_IOC(dev);
+	if (!ioc)
+		return 0;
 
 	/* Fast path single entry scatterlists. */
 	if (nents == 1) {
@@ -1037,6 +1049,10 @@ sba_unmap_sg(struct device *dev, struct scatterlist *sglist, int nents,
 		__func__, nents, sg_virt(sglist), sglist->length);
 
 	ioc = GET_IOC(dev);
+	if (!ioc) {
+		WARN_ON(!ioc);
+		return;
+	}
 
 #ifdef SBA_COLLECT_STATS
 	ioc->usg_calls++;
@@ -1069,6 +1085,11 @@ sba_unmap_sg(struct device *dev, struct scatterlist *sglist, int nents,
 
 }
 
+static int sba_mapping_error(struct device *dev, dma_addr_t dma_addr)
+{
+	return dma_addr == SBA_MAPPING_ERROR;
+}
+
 static const struct dma_map_ops sba_ops = {
 	.dma_supported =	sba_dma_supported,
 	.alloc =		sba_alloc,
@@ -1077,6 +1098,7 @@ static const struct dma_map_ops sba_ops = {
 	.unmap_page =		sba_unmap_page,
 	.map_sg =		sba_map_sg,
 	.unmap_sg =		sba_unmap_sg,
+	.mapping_error =	sba_mapping_error,
 };
 
 
diff --git a/drivers/pci/Kconfig b/drivers/pci/Kconfig
index e0cacb7b8563..c32a77fc8b03 100644
--- a/drivers/pci/Kconfig
+++ b/drivers/pci/Kconfig
@@ -86,6 +86,9 @@ config PCI_ATS
 config PCI_ECAM
 	bool
 
+config PCI_LOCKLESS_CONFIG
+	bool
+
 config PCI_IOV
 	bool "PCI IOV support"
 	depends on PCI
diff --git a/drivers/pci/access.c b/drivers/pci/access.c
index 74cf5fffb1e1..913d6722ece9 100644
--- a/drivers/pci/access.c
+++ b/drivers/pci/access.c
@@ -25,6 +25,14 @@ DEFINE_RAW_SPINLOCK(pci_lock);
 #define PCI_word_BAD (pos & 1)
 #define PCI_dword_BAD (pos & 3)
 
+#ifdef CONFIG_PCI_LOCKLESS_CONFIG
+# define pci_lock_config(f)	do { (void)(f); } while (0)
+# define pci_unlock_config(f)	do { (void)(f); } while (0)
+#else
+# define pci_lock_config(f)	raw_spin_lock_irqsave(&pci_lock, f)
+# define pci_unlock_config(f)	raw_spin_unlock_irqrestore(&pci_lock, f)
+#endif
+
 #define PCI_OP_READ(size, type, len) \
 int pci_bus_read_config_##size \
 	(struct pci_bus *bus, unsigned int devfn, int pos, type *value)	\
@@ -33,10 +41,10 @@ int pci_bus_read_config_##size \
 	unsigned long flags;						\
 	u32 data = 0;							\
 	if (PCI_##size##_BAD) return PCIBIOS_BAD_REGISTER_NUMBER;	\
-	raw_spin_lock_irqsave(&pci_lock, flags);			\
+	pci_lock_config(flags);						\
 	res = bus->ops->read(bus, devfn, pos, len, &data);		\
 	*value = (type)data;						\
-	raw_spin_unlock_irqrestore(&pci_lock, flags);		\
+	pci_unlock_config(flags);					\
 	return res;							\
 }
 
@@ -47,9 +55,9 @@ int pci_bus_write_config_##size \
 	int res;							\
 	unsigned long flags;						\
 	if (PCI_##size##_BAD) return PCIBIOS_BAD_REGISTER_NUMBER;	\
-	raw_spin_lock_irqsave(&pci_lock, flags);			\
+	pci_lock_config(flags);						\
 	res = bus->ops->write(bus, devfn, pos, len, value);		\
-	raw_spin_unlock_irqrestore(&pci_lock, flags);		\
+	pci_unlock_config(flags);					\
 	return res;							\
 }
 
@@ -896,7 +904,7 @@ int pci_read_config_byte(const struct pci_dev *dev, int where, u8 *val)
 {
 	if (pci_dev_is_disconnected(dev)) {
 		*val = ~0;
-		return -ENODEV;
+		return PCIBIOS_DEVICE_NOT_FOUND;
 	}
 	return pci_bus_read_config_byte(dev->bus, dev->devfn, where, val);
 }
@@ -906,7 +914,7 @@ int pci_read_config_word(const struct pci_dev *dev, int where, u16 *val)
 {
 	if (pci_dev_is_disconnected(dev)) {
 		*val = ~0;
-		return -ENODEV;
+		return PCIBIOS_DEVICE_NOT_FOUND;
 	}
 	return pci_bus_read_config_word(dev->bus, dev->devfn, where, val);
 }
@@ -917,7 +925,7 @@ int pci_read_config_dword(const struct pci_dev *dev, int where,
 {
 	if (pci_dev_is_disconnected(dev)) {
 		*val = ~0;
-		return -ENODEV;
+		return PCIBIOS_DEVICE_NOT_FOUND;
 	}
 	return pci_bus_read_config_dword(dev->bus, dev->devfn, where, val);
 }
@@ -926,7 +934,7 @@ EXPORT_SYMBOL(pci_read_config_dword);
 int pci_write_config_byte(const struct pci_dev *dev, int where, u8 val)
 {
 	if (pci_dev_is_disconnected(dev))
-		return -ENODEV;
+		return PCIBIOS_DEVICE_NOT_FOUND;
 	return pci_bus_write_config_byte(dev->bus, dev->devfn, where, val);
 }
 EXPORT_SYMBOL(pci_write_config_byte);
@@ -934,7 +942,7 @@ EXPORT_SYMBOL(pci_write_config_byte);
 int pci_write_config_word(const struct pci_dev *dev, int where, u16 val)
 {
 	if (pci_dev_is_disconnected(dev))
-		return -ENODEV;
+		return PCIBIOS_DEVICE_NOT_FOUND;
 	return pci_bus_write_config_word(dev->bus, dev->devfn, where, val);
 }
 EXPORT_SYMBOL(pci_write_config_word);
@@ -943,7 +951,7 @@ int pci_write_config_dword(const struct pci_dev *dev, int where,
 					 u32 val)
 {
 	if (pci_dev_is_disconnected(dev))
-		return -ENODEV;
+		return PCIBIOS_DEVICE_NOT_FOUND;
 	return pci_bus_write_config_dword(dev->bus, dev->devfn, where, val);
 }
 EXPORT_SYMBOL(pci_write_config_dword);
diff --git a/drivers/pci/endpoint/functions/Kconfig b/drivers/pci/endpoint/functions/Kconfig
index 175edad42d2f..2942066607e0 100644
--- a/drivers/pci/endpoint/functions/Kconfig
+++ b/drivers/pci/endpoint/functions/Kconfig
@@ -5,6 +5,7 @@
 config PCI_EPF_TEST
 	tristate "PCI Endpoint Test driver"
 	depends on PCI_ENDPOINT
+	select CRC32
 	help
 	   Enable this configuration option to enable the test driver
 	   for PCI Endpoint.
diff --git a/drivers/pci/host/vmd.c b/drivers/pci/host/vmd.c
index e27ad2a3bd33..31203d69616d 100644
--- a/drivers/pci/host/vmd.c
+++ b/drivers/pci/host/vmd.c
@@ -554,6 +554,7 @@ static int vmd_find_free_domain(void)
 static int vmd_enable_domain(struct vmd_dev *vmd)
 {
 	struct pci_sysdata *sd = &vmd->sysdata;
+	struct fwnode_handle *fn;
 	struct resource *res;
 	u32 upper_bits;
 	unsigned long flags;
@@ -617,8 +618,13 @@ static int vmd_enable_domain(struct vmd_dev *vmd)
 
 	sd->node = pcibus_to_node(vmd->dev->bus);
 
-	vmd->irq_domain = pci_msi_create_irq_domain(NULL, &vmd_msi_domain_info,
+	fn = irq_domain_alloc_named_id_fwnode("VMD-MSI", vmd->sysdata.domain);
+	if (!fn)
+		return -ENODEV;
+
+	vmd->irq_domain = pci_msi_create_irq_domain(fn, &vmd_msi_domain_info,
 						    x86_vector_domain);
+	irq_domain_free_fwnode(fn);
 	if (!vmd->irq_domain)
 		return -ENODEV;
 
diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index ba44fdfda66b..fbad5dca3219 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -1463,7 +1463,7 @@ struct irq_domain *pci_msi_create_irq_domain(struct fwnode_handle *fwnode,
 	if (!domain)
 		return NULL;
 
-	domain->bus_token = DOMAIN_BUS_PCI_MSI;
+	irq_domain_update_bus_token(domain, DOMAIN_BUS_PCI_MSI);
 	return domain;
 }
 EXPORT_SYMBOL_GPL(pci_msi_create_irq_domain);
diff --git a/drivers/pci/pci-acpi.c b/drivers/pci/pci-acpi.c
index 001860361434..e70c1c7ba1bf 100644
--- a/drivers/pci/pci-acpi.c
+++ b/drivers/pci/pci-acpi.c
@@ -21,13 +21,12 @@
 #include "pci.h"
 
 /*
- * The UUID is defined in the PCI Firmware Specification available here:
+ * The GUID is defined in the PCI Firmware Specification available here:
  * https://www.pcisig.com/members/downloads/pcifw_r3_1_13Dec10.pdf
  */
-const u8 pci_acpi_dsm_uuid[] = {
-	0xd0, 0x37, 0xc9, 0xe5, 0x53, 0x35, 0x7a, 0x4d,
-	0x91, 0x17, 0xea, 0x4d, 0x19, 0xc3, 0x43, 0x4d
-};
+const guid_t pci_acpi_dsm_guid =
+	GUID_INIT(0xe5c937d0, 0x3553, 0x4d7a,
+		  0x91, 0x17, 0xea, 0x4d, 0x19, 0xc3, 0x43, 0x4d);
 
 #if defined(CONFIG_PCI_QUIRKS) && defined(CONFIG_ARM64)
 static int acpi_get_rc_addr(struct acpi_device *adev, struct resource *res)
@@ -395,29 +394,26 @@ bool pciehp_is_native(struct pci_dev *pdev)
 
 /**
  * pci_acpi_wake_bus - Root bus wakeup notification fork function.
- * @work: Work item to handle.
+ * @context: Device wakeup context.
  */
-static void pci_acpi_wake_bus(struct work_struct *work)
+static void pci_acpi_wake_bus(struct acpi_device_wakeup_context *context)
 {
 	struct acpi_device *adev;
 	struct acpi_pci_root *root;
 
-	adev = container_of(work, struct acpi_device, wakeup.context.work);
+	adev = container_of(context, struct acpi_device, wakeup.context);
 	root = acpi_driver_data(adev);
 	pci_pme_wakeup_bus(root->bus);
 }
 
 /**
  * pci_acpi_wake_dev - PCI device wakeup notification work function.
- * @handle: ACPI handle of a device the notification is for.
- * @work: Work item to handle.
+ * @context: Device wakeup context.
  */
-static void pci_acpi_wake_dev(struct work_struct *work)
+static void pci_acpi_wake_dev(struct acpi_device_wakeup_context *context)
 {
-	struct acpi_device_wakeup_context *context;
 	struct pci_dev *pci_dev;
 
-	context = container_of(work, struct acpi_device_wakeup_context, work);
 	pci_dev = to_pci_dev(context->dev);
 
 	if (pci_dev->pme_poll)
@@ -425,7 +421,7 @@ static void pci_acpi_wake_dev(struct work_struct *work)
 
 	if (pci_dev->current_state == PCI_D3cold) {
 		pci_wakeup_event(pci_dev);
-		pm_runtime_resume(&pci_dev->dev);
+		pm_request_resume(&pci_dev->dev);
 		return;
 	}
 
@@ -434,7 +430,7 @@ static void pci_acpi_wake_dev(struct work_struct *work)
 		pci_check_pme_status(pci_dev);
 
 	pci_wakeup_event(pci_dev);
-	pm_runtime_resume(&pci_dev->dev);
+	pm_request_resume(&pci_dev->dev);
 
 	pci_pme_wakeup_bus(pci_dev->subordinate);
 }
@@ -573,67 +569,29 @@ static pci_power_t acpi_pci_get_power_state(struct pci_dev *dev)
 	return state_conv[state];
 }
 
-static bool acpi_pci_can_wakeup(struct pci_dev *dev)
-{
-	struct acpi_device *adev = ACPI_COMPANION(&dev->dev);
-	return adev ? acpi_device_can_wakeup(adev) : false;
-}
-
-static void acpi_pci_propagate_wakeup_enable(struct pci_bus *bus, bool enable)
+static int acpi_pci_propagate_wakeup(struct pci_bus *bus, bool enable)
 {
 	while (bus->parent) {
-		if (!acpi_pm_device_sleep_wake(&bus->self->dev, enable))
-			return;
-		bus = bus->parent;
-	}
+		if (acpi_pm_device_can_wakeup(&bus->self->dev))
+			return acpi_pm_set_device_wakeup(&bus->self->dev, enable);
 
-	/* We have reached the root bus. */
-	if (bus->bridge)
-		acpi_pm_device_sleep_wake(bus->bridge, enable);
-}
-
-static int acpi_pci_sleep_wake(struct pci_dev *dev, bool enable)
-{
-	if (acpi_pci_can_wakeup(dev))
-		return acpi_pm_device_sleep_wake(&dev->dev, enable);
-
-	acpi_pci_propagate_wakeup_enable(dev->bus, enable);
-	return 0;
-}
-
-static void acpi_pci_propagate_run_wake(struct pci_bus *bus, bool enable)
-{
-	while (bus->parent) {
-		struct pci_dev *bridge = bus->self;
-
-		if (bridge->pme_interrupt)
-			return;
-		if (!acpi_pm_device_run_wake(&bridge->dev, enable))
-			return;
 		bus = bus->parent;
 	}
 
 	/* We have reached the root bus. */
-	if (bus->bridge)
-		acpi_pm_device_run_wake(bus->bridge, enable);
+	if (bus->bridge) {
+		if (acpi_pm_device_can_wakeup(bus->bridge))
+			return acpi_pm_set_device_wakeup(bus->bridge, enable);
+	}
+	return 0;
 }
 
-static int acpi_pci_run_wake(struct pci_dev *dev, bool enable)
+static int acpi_pci_wakeup(struct pci_dev *dev, bool enable)
 {
-	/*
-	 * 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))
-		return 0;
+	if (acpi_pm_device_can_wakeup(&dev->dev))
+		return acpi_pm_set_device_wakeup(&dev->dev, enable);
 
-	acpi_pci_propagate_run_wake(dev->bus, enable);
-	return 0;
+	return acpi_pci_propagate_wakeup(dev->bus, enable);
 }
 
 static bool acpi_pci_need_resume(struct pci_dev *dev)
@@ -657,8 +615,7 @@ static const struct pci_platform_pm_ops acpi_pci_platform_pm = {
 	.set_state = acpi_pci_set_power_state,
 	.get_state = acpi_pci_get_power_state,
 	.choose_state = acpi_pci_choose_state,
-	.sleep_wake = acpi_pci_sleep_wake,
-	.run_wake = acpi_pci_run_wake,
+	.set_wakeup = acpi_pci_wakeup,
 	.need_resume = acpi_pci_need_resume,
 };
 
@@ -680,7 +637,7 @@ void acpi_pci_add_bus(struct pci_bus *bus)
 	if (!pci_is_root_bus(bus))
 		return;
 
-	obj = acpi_evaluate_dsm(ACPI_HANDLE(bus->bridge), pci_acpi_dsm_uuid, 3,
+	obj = acpi_evaluate_dsm(ACPI_HANDLE(bus->bridge), &pci_acpi_dsm_guid, 3,
 				RESET_DELAY_DSM, NULL);
 	if (!obj)
 		return;
@@ -745,7 +702,7 @@ static void pci_acpi_optimize_delay(struct pci_dev *pdev,
 	if (bridge->ignore_reset_delay)
 		pdev->d3cold_delay = 0;
 
-	obj = acpi_evaluate_dsm(handle, pci_acpi_dsm_uuid, 3,
+	obj = acpi_evaluate_dsm(handle, &pci_acpi_dsm_guid, 3,
 				FUNCTION_DELAY_DSM, NULL);
 	if (!obj)
 		return;
@@ -781,9 +738,7 @@ static void pci_acpi_setup(struct device *dev)
 		return;
 
 	device_set_wakeup_capable(dev, true);
-	acpi_pci_sleep_wake(pci_dev, false);
-	if (adev->wakeup.flags.run_wake)
-		device_set_run_wake(dev, true);
+	acpi_pci_wakeup(pci_dev, false);
 }
 
 static void pci_acpi_cleanup(struct device *dev)
@@ -794,10 +749,8 @@ static void pci_acpi_cleanup(struct device *dev)
 		return;
 
 	pci_acpi_remove_pm_notifier(adev);
-	if (adev->wakeup.flags.valid) {
+	if (adev->wakeup.flags.valid)
 		device_set_wakeup_capable(dev, false);
-		device_set_run_wake(dev, false);
-	}
 }
 
 static bool pci_acpi_bus_match(struct device *dev)
diff --git a/drivers/pci/pci-driver.c b/drivers/pci/pci-driver.c
index 192e7b681b96..df4aead394f2 100644
--- a/drivers/pci/pci-driver.c
+++ b/drivers/pci/pci-driver.c
@@ -96,7 +96,7 @@ static void pci_free_dynids(struct pci_driver *drv)
  *
  * Allow PCI IDs to be added to an existing driver via sysfs.
  */
-static ssize_t store_new_id(struct device_driver *driver, const char *buf,
+static ssize_t new_id_store(struct device_driver *driver, const char *buf,
 			    size_t count)
 {
 	struct pci_driver *pdrv = to_pci_driver(driver);
@@ -154,7 +154,7 @@ static ssize_t store_new_id(struct device_driver *driver, const char *buf,
 		return retval;
 	return count;
 }
-static DRIVER_ATTR(new_id, S_IWUSR, NULL, store_new_id);
+static DRIVER_ATTR_WO(new_id);
 
 /**
  * store_remove_id - remove a PCI device ID from this driver
@@ -164,7 +164,7 @@ static DRIVER_ATTR(new_id, S_IWUSR, NULL, store_new_id);
  *
  * Removes a dynamic pci device ID to this driver.
  */
-static ssize_t store_remove_id(struct device_driver *driver, const char *buf,
+static ssize_t remove_id_store(struct device_driver *driver, const char *buf,
 			       size_t count)
 {
 	struct pci_dynid *dynid, *n;
@@ -198,7 +198,7 @@ static ssize_t store_remove_id(struct device_driver *driver, const char *buf,
 
 	return retval;
 }
-static DRIVER_ATTR(remove_id, S_IWUSR, NULL, store_remove_id);
+static DRIVER_ATTR_WO(remove_id);
 
 static struct attribute *pci_drv_attrs[] = {
 	&driver_attr_new_id.attr,
@@ -320,10 +320,19 @@ static long local_pci_probe(void *_ddi)
 	return 0;
 }
 
+static bool pci_physfn_is_probed(struct pci_dev *dev)
+{
+#ifdef CONFIG_PCI_IOV
+	return dev->is_virtfn && dev->physfn->is_probed;
+#else
+	return false;
+#endif
+}
+
 static int pci_call_probe(struct pci_driver *drv, struct pci_dev *dev,
 			  const struct pci_device_id *id)
 {
-	int error, node;
+	int error, node, cpu;
 	struct drv_dev_and_id ddi = { drv, dev, id };
 
 	/*
@@ -332,33 +341,27 @@ static int pci_call_probe(struct pci_driver *drv, struct pci_dev *dev,
 	 * on the right node.
 	 */
 	node = dev_to_node(&dev->dev);
+	dev->is_probed = 1;
+
+	cpu_hotplug_disable();
 
 	/*
-	 * On NUMA systems, we are likely to call a PF probe function using
-	 * work_on_cpu().  If that probe calls pci_enable_sriov() (which
-	 * adds the VF devices via pci_bus_add_device()), we may re-enter
-	 * this function to call the VF probe function.  Calling
-	 * work_on_cpu() again will cause a lockdep warning.  Since VFs are
-	 * always on the same node as the PF, we can work around this by
-	 * avoiding work_on_cpu() when we're already on the correct node.
-	 *
-	 * Preemption is enabled, so it's theoretically unsafe to use
-	 * numa_node_id(), but even if we run the probe function on the
-	 * wrong node, it should be functionally correct.
+	 * Prevent nesting work_on_cpu() for the case where a Virtual Function
+	 * device is probed from work_on_cpu() of the Physical device.
 	 */
-	if (node >= 0 && node != numa_node_id()) {
-		int cpu;
-
-		get_online_cpus();
+	if (node < 0 || node >= MAX_NUMNODES || !node_online(node) ||
+	    pci_physfn_is_probed(dev))
+		cpu = nr_cpu_ids;
+	else
 		cpu = cpumask_any_and(cpumask_of_node(node), cpu_online_mask);
-		if (cpu < nr_cpu_ids)
-			error = work_on_cpu(cpu, local_pci_probe, &ddi);
-		else
-			error = local_pci_probe(&ddi);
-		put_online_cpus();
-	} else
+
+	if (cpu < nr_cpu_ids)
+		error = work_on_cpu(cpu, local_pci_probe, &ddi);
+	else
 		error = local_pci_probe(&ddi);
 
+	dev->is_probed = 0;
+	cpu_hotplug_enable();
 	return error;
 }
 
@@ -1216,7 +1219,7 @@ static int pci_pm_runtime_resume(struct device *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_enable_wake(pci_dev, PCI_D0, false);
 	pci_fixup_device(pci_fixup_resume, pci_dev);
 
 	rc = pm->runtime_resume(dev);
diff --git a/drivers/pci/pci-label.c b/drivers/pci/pci-label.c
index 51357377efbc..2d8db3ead6e8 100644
--- a/drivers/pci/pci-label.c
+++ b/drivers/pci/pci-label.c
@@ -172,7 +172,7 @@ static int dsm_get_label(struct device *dev, char *buf,
 	if (!handle)
 		return -1;
 
-	obj = acpi_evaluate_dsm(handle, pci_acpi_dsm_uuid, 0x2,
+	obj = acpi_evaluate_dsm(handle, &pci_acpi_dsm_guid, 0x2,
 				DEVICE_LABEL_DSM, NULL);
 	if (!obj)
 		return -1;
@@ -212,7 +212,7 @@ static bool device_has_dsm(struct device *dev)
 	if (!handle)
 		return false;
 
-	return !!acpi_check_dsm(handle, pci_acpi_dsm_uuid, 0x2,
+	return !!acpi_check_dsm(handle, &pci_acpi_dsm_guid, 0x2,
 				1 << DEVICE_LABEL_DSM);
 }
 
diff --git a/drivers/pci/pci-mid.c b/drivers/pci/pci-mid.c
index 1c4af7227bca..a4ac940c7696 100644
--- a/drivers/pci/pci-mid.c
+++ b/drivers/pci/pci-mid.c
@@ -39,12 +39,7 @@ static pci_power_t mid_pci_choose_state(struct pci_dev *pdev)
 	return PCI_D3hot;
 }
 
-static int mid_pci_sleep_wake(struct pci_dev *dev, bool enable)
-{
-	return 0;
-}
-
-static int mid_pci_run_wake(struct pci_dev *dev, bool enable)
+static int mid_pci_wakeup(struct pci_dev *dev, bool enable)
 {
 	return 0;
 }
@@ -59,8 +54,7 @@ static const struct pci_platform_pm_ops mid_pci_platform_pm = {
 	.set_state	= mid_pci_set_power_state,
 	.get_state	= mid_pci_get_power_state,
 	.choose_state	= mid_pci_choose_state,
-	.sleep_wake	= mid_pci_sleep_wake,
-	.run_wake	= mid_pci_run_wake,
+	.set_wakeup	= mid_pci_wakeup,
 	.need_resume	= mid_pci_need_resume,
 };
 
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 563901cd9c06..0b5302a9fdae 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -574,8 +574,7 @@ static const struct pci_platform_pm_ops *pci_platform_pm;
 int pci_set_platform_pm(const struct pci_platform_pm_ops *ops)
 {
 	if (!ops->is_manageable || !ops->set_state  || !ops->get_state ||
-	    !ops->choose_state  || !ops->sleep_wake || !ops->run_wake  ||
-	    !ops->need_resume)
+	    !ops->choose_state  || !ops->set_wakeup || !ops->need_resume)
 		return -EINVAL;
 	pci_platform_pm = ops;
 	return 0;
@@ -603,16 +602,10 @@ static inline pci_power_t platform_pci_choose_state(struct pci_dev *dev)
 			pci_platform_pm->choose_state(dev) : PCI_POWER_ERROR;
 }
 
-static inline int platform_pci_sleep_wake(struct pci_dev *dev, bool enable)
+static inline int platform_pci_set_wakeup(struct pci_dev *dev, bool enable)
 {
 	return pci_platform_pm ?
-			pci_platform_pm->sleep_wake(dev, enable) : -ENODEV;
-}
-
-static inline int platform_pci_run_wake(struct pci_dev *dev, bool enable)
-{
-	return pci_platform_pm ?
-			pci_platform_pm->run_wake(dev, enable) : -ENODEV;
+			pci_platform_pm->set_wakeup(dev, enable) : -ENODEV;
 }
 
 static inline bool platform_pci_need_resume(struct pci_dev *dev)
@@ -1805,6 +1798,23 @@ static void __pci_pme_active(struct pci_dev *dev, bool enable)
 	pci_write_config_word(dev, dev->pm_cap + PCI_PM_CTRL, pmcsr);
 }
 
+static void pci_pme_restore(struct pci_dev *dev)
+{
+	u16 pmcsr;
+
+	if (!dev->pme_support)
+		return;
+
+	pci_read_config_word(dev, dev->pm_cap + PCI_PM_CTRL, &pmcsr);
+	if (dev->wakeup_prepared) {
+		pmcsr |= PCI_PM_CTRL_PME_ENABLE;
+	} else {
+		pmcsr &= ~PCI_PM_CTRL_PME_ENABLE;
+		pmcsr |= PCI_PM_CTRL_PME_STATUS;
+	}
+	pci_write_config_word(dev, dev->pm_cap + PCI_PM_CTRL, pmcsr);
+}
+
 /**
  * pci_pme_active - enable or disable PCI device's PME# function
  * @dev: PCI device to handle.
@@ -1872,10 +1882,9 @@ void pci_pme_active(struct pci_dev *dev, bool enable)
 EXPORT_SYMBOL(pci_pme_active);
 
 /**
- * __pci_enable_wake - enable PCI device as wakeup event source
+ * pci_enable_wake - enable PCI device as wakeup event source
  * @dev: PCI device affected
  * @state: PCI state from which device will issue wakeup events
- * @runtime: True if the events are to be generated at run time
  * @enable: True to enable event generation; false to disable
  *
  * This enables the device as a wakeup event source, or disables it.
@@ -1891,17 +1900,18 @@ EXPORT_SYMBOL(pci_pme_active);
  * Error code depending on the platform is returned if both the platform and
  * the native mechanism fail to enable the generation of wake-up events
  */
-int __pci_enable_wake(struct pci_dev *dev, pci_power_t state,
-		      bool runtime, bool enable)
+int pci_enable_wake(struct pci_dev *dev, pci_power_t state, bool enable)
 {
 	int ret = 0;
 
-	if (enable && !runtime && !device_may_wakeup(&dev->dev))
-		return -EINVAL;
-
-	/* Don't do the same thing twice in a row for one device. */
-	if (!!enable == !!dev->wakeup_prepared)
+	/*
+	 * Don't do the same thing twice in a row for one device, but restore
+	 * PME Enable in case it has been updated by config space restoration.
+	 */
+	if (!!enable == !!dev->wakeup_prepared) {
+		pci_pme_restore(dev);
 		return 0;
+	}
 
 	/*
 	 * According to "PCI System Architecture" 4th ed. by Tom Shanley & Don
@@ -1916,24 +1926,20 @@ int __pci_enable_wake(struct pci_dev *dev, pci_power_t state,
 			pci_pme_active(dev, true);
 		else
 			ret = 1;
-		error = runtime ? platform_pci_run_wake(dev, true) :
-					platform_pci_sleep_wake(dev, true);
+		error = platform_pci_set_wakeup(dev, true);
 		if (ret)
 			ret = error;
 		if (!ret)
 			dev->wakeup_prepared = true;
 	} else {
-		if (runtime)
-			platform_pci_run_wake(dev, false);
-		else
-			platform_pci_sleep_wake(dev, false);
+		platform_pci_set_wakeup(dev, false);
 		pci_pme_active(dev, false);
 		dev->wakeup_prepared = false;
 	}
 
 	return ret;
 }
-EXPORT_SYMBOL(__pci_enable_wake);
+EXPORT_SYMBOL(pci_enable_wake);
 
 /**
  * pci_wake_from_d3 - enable/disable device to wake up from D3_hot or D3_cold
@@ -2075,12 +2081,12 @@ int pci_finish_runtime_suspend(struct pci_dev *dev)
 
 	dev->runtime_d3cold = target_state == PCI_D3cold;
 
-	__pci_enable_wake(dev, target_state, true, pci_dev_run_wake(dev));
+	pci_enable_wake(dev, target_state, pci_dev_run_wake(dev));
 
 	error = pci_set_power_state(dev, target_state);
 
 	if (error) {
-		__pci_enable_wake(dev, target_state, true, false);
+		pci_enable_wake(dev, target_state, false);
 		dev->runtime_d3cold = false;
 	}
 
@@ -2099,7 +2105,7 @@ bool pci_dev_run_wake(struct pci_dev *dev)
 {
 	struct pci_bus *bus = dev->bus;
 
-	if (device_run_wake(&dev->dev))
+	if (device_can_wakeup(&dev->dev))
 		return true;
 
 	if (!dev->pme_support)
@@ -2112,7 +2118,7 @@ bool pci_dev_run_wake(struct pci_dev *dev)
 	while (bus->parent) {
 		struct pci_dev *bridge = bus->self;
 
-		if (device_run_wake(&bridge->dev))
+		if (device_can_wakeup(&bridge->dev))
 			return true;
 
 		bus = bus->parent;
@@ -2120,7 +2126,7 @@ bool pci_dev_run_wake(struct pci_dev *dev)
 
 	/* We have reached the root bus. */
 	if (bus->bridge)
-		return device_run_wake(bus->bridge);
+		return device_can_wakeup(bus->bridge);
 
 	return false;
 }
diff --git a/drivers/pci/pci.h b/drivers/pci/pci.h
index f8113e5b9812..240b2c0fed4b 100644
--- a/drivers/pci/pci.h
+++ b/drivers/pci/pci.h
@@ -47,11 +47,7 @@ int pci_probe_reset_function(struct pci_dev *dev);
  *                platform; to be used during system-wide transitions from a
  *                sleeping state to the working state and vice versa
  *
- * @sleep_wake: enables/disables the system wake up capability of given device
- *
- * @run_wake: enables/disables the platform to generate run-time wake-up events
- *		for given device (the device's wake-up capability has to be
- *		enabled by @sleep_wake for this feature to work)
+ * @set_wakeup: enables/disables wakeup capability for the device
  *
  * @need_resume: returns 'true' if the given device (which is currently
  *		suspended) needs to be resumed to be configured for system
@@ -65,8 +61,7 @@ struct pci_platform_pm_ops {
 	int (*set_state)(struct pci_dev *dev, pci_power_t state);
 	pci_power_t (*get_state)(struct pci_dev *dev);
 	pci_power_t (*choose_state)(struct pci_dev *dev);
-	int (*sleep_wake)(struct pci_dev *dev, bool enable);
-	int (*run_wake)(struct pci_dev *dev, bool enable);
+	int (*set_wakeup)(struct pci_dev *dev, bool enable);
 	bool (*need_resume)(struct pci_dev *dev);
 };
 
diff --git a/drivers/pci/pcie/pme.c b/drivers/pci/pcie/pme.c
index 2dd1c68e6de8..80e58d25006d 100644
--- a/drivers/pci/pcie/pme.c
+++ b/drivers/pci/pcie/pme.c
@@ -294,31 +294,29 @@ static irqreturn_t pcie_pme_irq(int irq, void *context)
 }
 
 /**
- * pcie_pme_set_native - Set the PME interrupt flag for given device.
+ * pcie_pme_can_wakeup - Set the wakeup capability flag.
  * @dev: PCI device to handle.
  * @ign: Ignored.
  */
-static int pcie_pme_set_native(struct pci_dev *dev, void *ign)
+static int pcie_pme_can_wakeup(struct pci_dev *dev, void *ign)
 {
-	device_set_run_wake(&dev->dev, true);
-	dev->pme_interrupt = true;
+	device_set_wakeup_capable(&dev->dev, true);
 	return 0;
 }
 
 /**
- * pcie_pme_mark_devices - Set the PME interrupt flag for devices below a port.
+ * pcie_pme_mark_devices - Set the wakeup flag for devices below a port.
  * @port: PCIe root port or event collector to handle.
  *
  * For each device below given root port, including the port itself (or for each
  * root complex integrated endpoint if @port is a root complex event collector)
- * set the flag indicating that it can signal run-time wake-up events via PCIe
- * PME interrupts.
+ * set the flag indicating that it can signal run-time wake-up events.
  */
 static void pcie_pme_mark_devices(struct pci_dev *port)
 {
-	pcie_pme_set_native(port, NULL);
+	pcie_pme_can_wakeup(port, NULL);
 	if (port->subordinate)
-		pci_walk_bus(port->subordinate, pcie_pme_set_native, NULL);
+		pci_walk_bus(port->subordinate, pcie_pme_can_wakeup, NULL);
 }
 
 /**
diff --git a/drivers/pcmcia/ds.c b/drivers/pcmcia/ds.c
index 69b5e811ea2b..a9258f641cee 100644
--- a/drivers/pcmcia/ds.c
+++ b/drivers/pcmcia/ds.c
@@ -95,7 +95,7 @@ struct pcmcia_dynid {
  * and causes the driver to probe for all devices again.
  */
 static ssize_t
-pcmcia_store_new_id(struct device_driver *driver, const char *buf, size_t count)
+new_id_store(struct device_driver *driver, const char *buf, size_t count)
 {
 	struct pcmcia_dynid *dynid;
 	struct pcmcia_driver *pdrv = to_pcmcia_drv(driver);
@@ -133,7 +133,7 @@ pcmcia_store_new_id(struct device_driver *driver, const char *buf, size_t count)
 		return retval;
 	return count;
 }
-static DRIVER_ATTR(new_id, S_IWUSR, NULL, pcmcia_store_new_id);
+static DRIVER_ATTR_WO(new_id);
 
 static void
 pcmcia_free_dynids(struct pcmcia_driver *drv)
diff --git a/drivers/perf/Kconfig b/drivers/perf/Kconfig
index aa587edaf9ea..e5197ffb7422 100644
--- a/drivers/perf/Kconfig
+++ b/drivers/perf/Kconfig
@@ -3,9 +3,10 @@
 #
 
 menu "Performance monitor support"
+	depends on PERF_EVENTS
 
 config ARM_PMU
-	depends on PERF_EVENTS && (ARM || ARM64)
+	depends on ARM || ARM64
 	bool "ARM PMU framework"
 	default y
 	help
@@ -18,7 +19,7 @@ config ARM_PMU_ACPI
 
 config QCOM_L2_PMU
 	bool "Qualcomm Technologies L2-cache PMU"
-	depends on ARCH_QCOM && ARM64 && PERF_EVENTS && ACPI
+	depends on ARCH_QCOM && ARM64 && ACPI
 	  help
 	  Provides support for the L2 cache performance monitor unit (PMU)
 	  in Qualcomm Technologies processors.
@@ -27,7 +28,7 @@ config QCOM_L2_PMU
 
 config QCOM_L3_PMU
 	bool "Qualcomm Technologies L3-cache PMU"
-	depends on ARCH_QCOM && ARM64 && PERF_EVENTS && ACPI
+	depends on ARCH_QCOM && ARM64 && ACPI
 	select QCOM_IRQ_COMBINER
 	help
 	   Provides support for the L3 cache performance monitor unit (PMU)
@@ -36,7 +37,7 @@ config QCOM_L3_PMU
 	   monitoring L3 cache events.
 
 config XGENE_PMU
-        depends on PERF_EVENTS && ARCH_XGENE
+        depends on ARCH_XGENE
         bool "APM X-Gene SoC PMU"
         default n
         help
diff --git a/drivers/perf/xgene_pmu.c b/drivers/perf/xgene_pmu.c
index 35b5289bc5da..e841282d690c 100644
--- a/drivers/perf/xgene_pmu.c
+++ b/drivers/perf/xgene_pmu.c
@@ -37,6 +37,8 @@
 
 #define CSW_CSWCR                       0x0000
 #define  CSW_CSWCR_DUALMCB_MASK         BIT(0)
+#define  CSW_CSWCR_MCB0_ROUTING(x)	(((x) & 0x0C) >> 2)
+#define  CSW_CSWCR_MCB1_ROUTING(x)	(((x) & 0x30) >> 4)
 #define MCBADDRMR                       0x0000
 #define  MCBADDRMR_DUALMCU_MODE_MASK    BIT(2)
 
@@ -50,8 +52,17 @@
 #define  PCPPMU_INT_L3C		BIT(2)
 #define  PCPPMU_INT_IOB		BIT(3)
 
+#define  PCPPMU_V3_INTMASK	0x00FF33FF
+#define  PCPPMU_V3_INTENMASK	0xFFFFFFFF
+#define  PCPPMU_V3_INTCLRMASK	0xFF00CC00
+#define  PCPPMU_V3_INT_MCU	0x000000FF
+#define  PCPPMU_V3_INT_MCB	0x00000300
+#define  PCPPMU_V3_INT_L3C	0x00FF0000
+#define  PCPPMU_V3_INT_IOB	0x00003000
+
 #define PMU_MAX_COUNTERS	4
-#define PMU_CNT_MAX_PERIOD	0x100000000ULL
+#define PMU_CNT_MAX_PERIOD	0xFFFFFFFFULL
+#define PMU_V3_CNT_MAX_PERIOD	0xFFFFFFFFFFFFFFFFULL
 #define PMU_OVERFLOW_MASK	0xF
 #define PMU_PMCR_E		BIT(0)
 #define PMU_PMCR_P		BIT(1)
@@ -73,6 +84,10 @@
 #define PMU_PMOVSR		0xC80
 #define PMU_PMCR		0xE04
 
+/* PMU registers for V3 */
+#define PMU_PMOVSCLR		0xC80
+#define PMU_PMOVSSET		0xCC0
+
 #define to_pmu_dev(p)     container_of(p, struct xgene_pmu_dev, pmu)
 #define GET_CNTR(ev)      (ev->hw.idx)
 #define GET_EVENTID(ev)   (ev->hw.config & 0xFFULL)
@@ -96,14 +111,33 @@ struct xgene_pmu_dev {
 	struct perf_event *pmu_counter_event[PMU_MAX_COUNTERS];
 };
 
+struct xgene_pmu_ops {
+	void (*mask_int)(struct xgene_pmu *pmu);
+	void (*unmask_int)(struct xgene_pmu *pmu);
+	u64 (*read_counter)(struct xgene_pmu_dev *pmu, int idx);
+	void (*write_counter)(struct xgene_pmu_dev *pmu, int idx, u64 val);
+	void (*write_evttype)(struct xgene_pmu_dev *pmu_dev, int idx, u32 val);
+	void (*write_agentmsk)(struct xgene_pmu_dev *pmu_dev, u32 val);
+	void (*write_agent1msk)(struct xgene_pmu_dev *pmu_dev, u32 val);
+	void (*enable_counter)(struct xgene_pmu_dev *pmu_dev, int idx);
+	void (*disable_counter)(struct xgene_pmu_dev *pmu_dev, int idx);
+	void (*enable_counter_int)(struct xgene_pmu_dev *pmu_dev, int idx);
+	void (*disable_counter_int)(struct xgene_pmu_dev *pmu_dev, int idx);
+	void (*reset_counters)(struct xgene_pmu_dev *pmu_dev);
+	void (*start_counters)(struct xgene_pmu_dev *pmu_dev);
+	void (*stop_counters)(struct xgene_pmu_dev *pmu_dev);
+};
+
 struct xgene_pmu {
 	struct device *dev;
 	int version;
 	void __iomem *pcppmu_csr;
 	u32 mcb_active_mask;
 	u32 mc_active_mask;
+	u32 l3c_active_mask;
 	cpumask_t cpu;
 	raw_spinlock_t lock;
+	const struct xgene_pmu_ops *ops;
 	struct list_head l3cpmus;
 	struct list_head iobpmus;
 	struct list_head mcbpmus;
@@ -125,11 +159,13 @@ struct xgene_pmu_data {
 enum xgene_pmu_version {
 	PCP_PMU_V1 = 1,
 	PCP_PMU_V2,
+	PCP_PMU_V3,
 };
 
 enum xgene_pmu_dev_type {
 	PMU_TYPE_L3C = 0,
 	PMU_TYPE_IOB,
+	PMU_TYPE_IOB_SLOW,
 	PMU_TYPE_MCB,
 	PMU_TYPE_MC,
 };
@@ -195,6 +231,56 @@ static const struct attribute_group mc_pmu_format_attr_group = {
 	.attrs = mc_pmu_format_attrs,
 };
 
+static struct attribute *l3c_pmu_v3_format_attrs[] = {
+	XGENE_PMU_FORMAT_ATTR(l3c_eventid, "config:0-39"),
+	NULL,
+};
+
+static struct attribute *iob_pmu_v3_format_attrs[] = {
+	XGENE_PMU_FORMAT_ATTR(iob_eventid, "config:0-47"),
+	NULL,
+};
+
+static struct attribute *iob_slow_pmu_v3_format_attrs[] = {
+	XGENE_PMU_FORMAT_ATTR(iob_slow_eventid, "config:0-16"),
+	NULL,
+};
+
+static struct attribute *mcb_pmu_v3_format_attrs[] = {
+	XGENE_PMU_FORMAT_ATTR(mcb_eventid, "config:0-35"),
+	NULL,
+};
+
+static struct attribute *mc_pmu_v3_format_attrs[] = {
+	XGENE_PMU_FORMAT_ATTR(mc_eventid, "config:0-44"),
+	NULL,
+};
+
+static const struct attribute_group l3c_pmu_v3_format_attr_group = {
+	.name = "format",
+	.attrs = l3c_pmu_v3_format_attrs,
+};
+
+static const struct attribute_group iob_pmu_v3_format_attr_group = {
+	.name = "format",
+	.attrs = iob_pmu_v3_format_attrs,
+};
+
+static const struct attribute_group iob_slow_pmu_v3_format_attr_group = {
+	.name = "format",
+	.attrs = iob_slow_pmu_v3_format_attrs,
+};
+
+static const struct attribute_group mcb_pmu_v3_format_attr_group = {
+	.name = "format",
+	.attrs = mcb_pmu_v3_format_attrs,
+};
+
+static const struct attribute_group mc_pmu_v3_format_attr_group = {
+	.name = "format",
+	.attrs = mc_pmu_v3_format_attrs,
+};
+
 /*
  * sysfs event attributes
  */
@@ -311,6 +397,219 @@ static const struct attribute_group mc_pmu_events_attr_group = {
 	.attrs = mc_pmu_events_attrs,
 };
 
+static struct attribute *l3c_pmu_v3_events_attrs[] = {
+	XGENE_PMU_EVENT_ATTR(cycle-count,			0x00),
+	XGENE_PMU_EVENT_ATTR(read-hit,				0x01),
+	XGENE_PMU_EVENT_ATTR(read-miss,				0x02),
+	XGENE_PMU_EVENT_ATTR(index-flush-eviction,		0x03),
+	XGENE_PMU_EVENT_ATTR(write-caused-replacement,		0x04),
+	XGENE_PMU_EVENT_ATTR(write-not-caused-replacement,	0x05),
+	XGENE_PMU_EVENT_ATTR(clean-eviction,			0x06),
+	XGENE_PMU_EVENT_ATTR(dirty-eviction,			0x07),
+	XGENE_PMU_EVENT_ATTR(read,				0x08),
+	XGENE_PMU_EVENT_ATTR(write,				0x09),
+	XGENE_PMU_EVENT_ATTR(request,				0x0a),
+	XGENE_PMU_EVENT_ATTR(tq-bank-conflict-issue-stall,	0x0b),
+	XGENE_PMU_EVENT_ATTR(tq-full,				0x0c),
+	XGENE_PMU_EVENT_ATTR(ackq-full,				0x0d),
+	XGENE_PMU_EVENT_ATTR(wdb-full,				0x0e),
+	XGENE_PMU_EVENT_ATTR(odb-full,				0x10),
+	XGENE_PMU_EVENT_ATTR(wbq-full,				0x11),
+	XGENE_PMU_EVENT_ATTR(input-req-async-fifo-stall,	0x12),
+	XGENE_PMU_EVENT_ATTR(output-req-async-fifo-stall,	0x13),
+	XGENE_PMU_EVENT_ATTR(output-data-async-fifo-stall,	0x14),
+	XGENE_PMU_EVENT_ATTR(total-insertion,			0x15),
+	XGENE_PMU_EVENT_ATTR(sip-insertions-r-set,		0x16),
+	XGENE_PMU_EVENT_ATTR(sip-insertions-r-clear,		0x17),
+	XGENE_PMU_EVENT_ATTR(dip-insertions-r-set,		0x18),
+	XGENE_PMU_EVENT_ATTR(dip-insertions-r-clear,		0x19),
+	XGENE_PMU_EVENT_ATTR(dip-insertions-force-r-set,	0x1a),
+	XGENE_PMU_EVENT_ATTR(egression,				0x1b),
+	XGENE_PMU_EVENT_ATTR(replacement,			0x1c),
+	XGENE_PMU_EVENT_ATTR(old-replacement,			0x1d),
+	XGENE_PMU_EVENT_ATTR(young-replacement,			0x1e),
+	XGENE_PMU_EVENT_ATTR(r-set-replacement,			0x1f),
+	XGENE_PMU_EVENT_ATTR(r-clear-replacement,		0x20),
+	XGENE_PMU_EVENT_ATTR(old-r-replacement,			0x21),
+	XGENE_PMU_EVENT_ATTR(old-nr-replacement,		0x22),
+	XGENE_PMU_EVENT_ATTR(young-r-replacement,		0x23),
+	XGENE_PMU_EVENT_ATTR(young-nr-replacement,		0x24),
+	XGENE_PMU_EVENT_ATTR(bloomfilter-clearing,		0x25),
+	XGENE_PMU_EVENT_ATTR(generation-flip,			0x26),
+	XGENE_PMU_EVENT_ATTR(vcc-droop-detected,		0x27),
+	NULL,
+};
+
+static struct attribute *iob_fast_pmu_v3_events_attrs[] = {
+	XGENE_PMU_EVENT_ATTR(cycle-count,			0x00),
+	XGENE_PMU_EVENT_ATTR(pa-req-buf-alloc-all,		0x01),
+	XGENE_PMU_EVENT_ATTR(pa-req-buf-alloc-rd,		0x02),
+	XGENE_PMU_EVENT_ATTR(pa-req-buf-alloc-wr,		0x03),
+	XGENE_PMU_EVENT_ATTR(pa-all-cp-req,			0x04),
+	XGENE_PMU_EVENT_ATTR(pa-cp-blk-req,			0x05),
+	XGENE_PMU_EVENT_ATTR(pa-cp-ptl-req,			0x06),
+	XGENE_PMU_EVENT_ATTR(pa-cp-rd-req,			0x07),
+	XGENE_PMU_EVENT_ATTR(pa-cp-wr-req,			0x08),
+	XGENE_PMU_EVENT_ATTR(ba-all-req,			0x09),
+	XGENE_PMU_EVENT_ATTR(ba-rd-req,				0x0a),
+	XGENE_PMU_EVENT_ATTR(ba-wr-req,				0x0b),
+	XGENE_PMU_EVENT_ATTR(pa-rd-shared-req-issued,		0x10),
+	XGENE_PMU_EVENT_ATTR(pa-rd-exclusive-req-issued,	0x11),
+	XGENE_PMU_EVENT_ATTR(pa-wr-invalidate-req-issued-stashable, 0x12),
+	XGENE_PMU_EVENT_ATTR(pa-wr-invalidate-req-issued-nonstashable, 0x13),
+	XGENE_PMU_EVENT_ATTR(pa-wr-back-req-issued-stashable,	0x14),
+	XGENE_PMU_EVENT_ATTR(pa-wr-back-req-issued-nonstashable, 0x15),
+	XGENE_PMU_EVENT_ATTR(pa-ptl-wr-req,			0x16),
+	XGENE_PMU_EVENT_ATTR(pa-ptl-rd-req,			0x17),
+	XGENE_PMU_EVENT_ATTR(pa-wr-back-clean-data,		0x18),
+	XGENE_PMU_EVENT_ATTR(pa-wr-back-cancelled-on-SS,	0x1b),
+	XGENE_PMU_EVENT_ATTR(pa-barrier-occurrence,		0x1c),
+	XGENE_PMU_EVENT_ATTR(pa-barrier-cycles,			0x1d),
+	XGENE_PMU_EVENT_ATTR(pa-total-cp-snoops,		0x20),
+	XGENE_PMU_EVENT_ATTR(pa-rd-shared-snoop,		0x21),
+	XGENE_PMU_EVENT_ATTR(pa-rd-shared-snoop-hit,		0x22),
+	XGENE_PMU_EVENT_ATTR(pa-rd-exclusive-snoop,		0x23),
+	XGENE_PMU_EVENT_ATTR(pa-rd-exclusive-snoop-hit,		0x24),
+	XGENE_PMU_EVENT_ATTR(pa-rd-wr-invalid-snoop,		0x25),
+	XGENE_PMU_EVENT_ATTR(pa-rd-wr-invalid-snoop-hit,	0x26),
+	XGENE_PMU_EVENT_ATTR(pa-req-buffer-full,		0x28),
+	XGENE_PMU_EVENT_ATTR(cswlf-outbound-req-fifo-full,	0x29),
+	XGENE_PMU_EVENT_ATTR(cswlf-inbound-snoop-fifo-backpressure, 0x2a),
+	XGENE_PMU_EVENT_ATTR(cswlf-outbound-lack-fifo-full,	0x2b),
+	XGENE_PMU_EVENT_ATTR(cswlf-inbound-gack-fifo-backpressure, 0x2c),
+	XGENE_PMU_EVENT_ATTR(cswlf-outbound-data-fifo-full,	0x2d),
+	XGENE_PMU_EVENT_ATTR(cswlf-inbound-data-fifo-backpressure, 0x2e),
+	XGENE_PMU_EVENT_ATTR(cswlf-inbound-req-backpressure,	0x2f),
+	NULL,
+};
+
+static struct attribute *iob_slow_pmu_v3_events_attrs[] = {
+	XGENE_PMU_EVENT_ATTR(cycle-count,			0x00),
+	XGENE_PMU_EVENT_ATTR(pa-axi0-rd-req,			0x01),
+	XGENE_PMU_EVENT_ATTR(pa-axi0-wr-req,			0x02),
+	XGENE_PMU_EVENT_ATTR(pa-axi1-rd-req,			0x03),
+	XGENE_PMU_EVENT_ATTR(pa-axi1-wr-req,			0x04),
+	XGENE_PMU_EVENT_ATTR(ba-all-axi-req,			0x07),
+	XGENE_PMU_EVENT_ATTR(ba-axi-rd-req,			0x08),
+	XGENE_PMU_EVENT_ATTR(ba-axi-wr-req,			0x09),
+	XGENE_PMU_EVENT_ATTR(ba-free-list-empty,		0x10),
+	NULL,
+};
+
+static struct attribute *mcb_pmu_v3_events_attrs[] = {
+	XGENE_PMU_EVENT_ATTR(cycle-count,			0x00),
+	XGENE_PMU_EVENT_ATTR(req-receive,			0x01),
+	XGENE_PMU_EVENT_ATTR(rd-req-recv,			0x02),
+	XGENE_PMU_EVENT_ATTR(rd-req-recv-2,			0x03),
+	XGENE_PMU_EVENT_ATTR(wr-req-recv,			0x04),
+	XGENE_PMU_EVENT_ATTR(wr-req-recv-2,			0x05),
+	XGENE_PMU_EVENT_ATTR(rd-req-sent-to-mcu,		0x06),
+	XGENE_PMU_EVENT_ATTR(rd-req-sent-to-mcu-2,		0x07),
+	XGENE_PMU_EVENT_ATTR(rd-req-sent-to-spec-mcu,		0x08),
+	XGENE_PMU_EVENT_ATTR(rd-req-sent-to-spec-mcu-2,		0x09),
+	XGENE_PMU_EVENT_ATTR(glbl-ack-recv-for-rd-sent-to-spec-mcu, 0x0a),
+	XGENE_PMU_EVENT_ATTR(glbl-ack-go-recv-for-rd-sent-to-spec-mcu, 0x0b),
+	XGENE_PMU_EVENT_ATTR(glbl-ack-nogo-recv-for-rd-sent-to-spec-mcu, 0x0c),
+	XGENE_PMU_EVENT_ATTR(glbl-ack-go-recv-any-rd-req,	0x0d),
+	XGENE_PMU_EVENT_ATTR(glbl-ack-go-recv-any-rd-req-2,	0x0e),
+	XGENE_PMU_EVENT_ATTR(wr-req-sent-to-mcu,		0x0f),
+	XGENE_PMU_EVENT_ATTR(gack-recv,				0x10),
+	XGENE_PMU_EVENT_ATTR(rd-gack-recv,			0x11),
+	XGENE_PMU_EVENT_ATTR(wr-gack-recv,			0x12),
+	XGENE_PMU_EVENT_ATTR(cancel-rd-gack,			0x13),
+	XGENE_PMU_EVENT_ATTR(cancel-wr-gack,			0x14),
+	XGENE_PMU_EVENT_ATTR(mcb-csw-req-stall,			0x15),
+	XGENE_PMU_EVENT_ATTR(mcu-req-intf-blocked,		0x16),
+	XGENE_PMU_EVENT_ATTR(mcb-mcu-rd-intf-stall,		0x17),
+	XGENE_PMU_EVENT_ATTR(csw-rd-intf-blocked,		0x18),
+	XGENE_PMU_EVENT_ATTR(csw-local-ack-intf-blocked,	0x19),
+	XGENE_PMU_EVENT_ATTR(mcu-req-table-full,		0x1a),
+	XGENE_PMU_EVENT_ATTR(mcu-stat-table-full,		0x1b),
+	XGENE_PMU_EVENT_ATTR(mcu-wr-table-full,			0x1c),
+	XGENE_PMU_EVENT_ATTR(mcu-rdreceipt-resp,		0x1d),
+	XGENE_PMU_EVENT_ATTR(mcu-wrcomplete-resp,		0x1e),
+	XGENE_PMU_EVENT_ATTR(mcu-retryack-resp,			0x1f),
+	XGENE_PMU_EVENT_ATTR(mcu-pcrdgrant-resp,		0x20),
+	XGENE_PMU_EVENT_ATTR(mcu-req-from-lastload,		0x21),
+	XGENE_PMU_EVENT_ATTR(mcu-req-from-bypass,		0x22),
+	XGENE_PMU_EVENT_ATTR(volt-droop-detect,			0x23),
+	NULL,
+};
+
+static struct attribute *mc_pmu_v3_events_attrs[] = {
+	XGENE_PMU_EVENT_ATTR(cycle-count,			0x00),
+	XGENE_PMU_EVENT_ATTR(act-sent,				0x01),
+	XGENE_PMU_EVENT_ATTR(pre-sent,				0x02),
+	XGENE_PMU_EVENT_ATTR(rd-sent,				0x03),
+	XGENE_PMU_EVENT_ATTR(rda-sent,				0x04),
+	XGENE_PMU_EVENT_ATTR(wr-sent,				0x05),
+	XGENE_PMU_EVENT_ATTR(wra-sent,				0x06),
+	XGENE_PMU_EVENT_ATTR(pd-entry-vld,			0x07),
+	XGENE_PMU_EVENT_ATTR(sref-entry-vld,			0x08),
+	XGENE_PMU_EVENT_ATTR(prea-sent,				0x09),
+	XGENE_PMU_EVENT_ATTR(ref-sent,				0x0a),
+	XGENE_PMU_EVENT_ATTR(rd-rda-sent,			0x0b),
+	XGENE_PMU_EVENT_ATTR(wr-wra-sent,			0x0c),
+	XGENE_PMU_EVENT_ATTR(raw-hazard,			0x0d),
+	XGENE_PMU_EVENT_ATTR(war-hazard,			0x0e),
+	XGENE_PMU_EVENT_ATTR(waw-hazard,			0x0f),
+	XGENE_PMU_EVENT_ATTR(rar-hazard,			0x10),
+	XGENE_PMU_EVENT_ATTR(raw-war-waw-hazard,		0x11),
+	XGENE_PMU_EVENT_ATTR(hprd-lprd-wr-req-vld,		0x12),
+	XGENE_PMU_EVENT_ATTR(lprd-req-vld,			0x13),
+	XGENE_PMU_EVENT_ATTR(hprd-req-vld,			0x14),
+	XGENE_PMU_EVENT_ATTR(hprd-lprd-req-vld,			0x15),
+	XGENE_PMU_EVENT_ATTR(wr-req-vld,			0x16),
+	XGENE_PMU_EVENT_ATTR(partial-wr-req-vld,		0x17),
+	XGENE_PMU_EVENT_ATTR(rd-retry,				0x18),
+	XGENE_PMU_EVENT_ATTR(wr-retry,				0x19),
+	XGENE_PMU_EVENT_ATTR(retry-gnt,				0x1a),
+	XGENE_PMU_EVENT_ATTR(rank-change,			0x1b),
+	XGENE_PMU_EVENT_ATTR(dir-change,			0x1c),
+	XGENE_PMU_EVENT_ATTR(rank-dir-change,			0x1d),
+	XGENE_PMU_EVENT_ATTR(rank-active,			0x1e),
+	XGENE_PMU_EVENT_ATTR(rank-idle,				0x1f),
+	XGENE_PMU_EVENT_ATTR(rank-pd,				0x20),
+	XGENE_PMU_EVENT_ATTR(rank-sref,				0x21),
+	XGENE_PMU_EVENT_ATTR(queue-fill-gt-thresh,		0x22),
+	XGENE_PMU_EVENT_ATTR(queue-rds-gt-thresh,		0x23),
+	XGENE_PMU_EVENT_ATTR(queue-wrs-gt-thresh,		0x24),
+	XGENE_PMU_EVENT_ATTR(phy-updt-complt,			0x25),
+	XGENE_PMU_EVENT_ATTR(tz-fail,				0x26),
+	XGENE_PMU_EVENT_ATTR(dram-errc,				0x27),
+	XGENE_PMU_EVENT_ATTR(dram-errd,				0x28),
+	XGENE_PMU_EVENT_ATTR(rd-enq,				0x29),
+	XGENE_PMU_EVENT_ATTR(wr-enq,				0x2a),
+	XGENE_PMU_EVENT_ATTR(tmac-limit-reached,		0x2b),
+	XGENE_PMU_EVENT_ATTR(tmaw-tracker-full,			0x2c),
+	NULL,
+};
+
+static const struct attribute_group l3c_pmu_v3_events_attr_group = {
+	.name = "events",
+	.attrs = l3c_pmu_v3_events_attrs,
+};
+
+static const struct attribute_group iob_fast_pmu_v3_events_attr_group = {
+	.name = "events",
+	.attrs = iob_fast_pmu_v3_events_attrs,
+};
+
+static const struct attribute_group iob_slow_pmu_v3_events_attr_group = {
+	.name = "events",
+	.attrs = iob_slow_pmu_v3_events_attrs,
+};
+
+static const struct attribute_group mcb_pmu_v3_events_attr_group = {
+	.name = "events",
+	.attrs = mcb_pmu_v3_events_attrs,
+};
+
+static const struct attribute_group mc_pmu_v3_events_attr_group = {
+	.name = "events",
+	.attrs = mc_pmu_v3_events_attrs,
+};
+
 /*
  * sysfs cpumask attributes
  */
@@ -334,7 +633,7 @@ static const struct attribute_group pmu_cpumask_attr_group = {
 };
 
 /*
- * Per PMU device attribute groups
+ * Per PMU device attribute groups of PMU v1 and v2
  */
 static const struct attribute_group *l3c_pmu_attr_groups[] = {
 	&l3c_pmu_format_attr_group,
@@ -364,6 +663,44 @@ static const struct attribute_group *mc_pmu_attr_groups[] = {
 	NULL
 };
 
+/*
+ * Per PMU device attribute groups of PMU v3
+ */
+static const struct attribute_group *l3c_pmu_v3_attr_groups[] = {
+	&l3c_pmu_v3_format_attr_group,
+	&pmu_cpumask_attr_group,
+	&l3c_pmu_v3_events_attr_group,
+	NULL
+};
+
+static const struct attribute_group *iob_fast_pmu_v3_attr_groups[] = {
+	&iob_pmu_v3_format_attr_group,
+	&pmu_cpumask_attr_group,
+	&iob_fast_pmu_v3_events_attr_group,
+	NULL
+};
+
+static const struct attribute_group *iob_slow_pmu_v3_attr_groups[] = {
+	&iob_slow_pmu_v3_format_attr_group,
+	&pmu_cpumask_attr_group,
+	&iob_slow_pmu_v3_events_attr_group,
+	NULL
+};
+
+static const struct attribute_group *mcb_pmu_v3_attr_groups[] = {
+	&mcb_pmu_v3_format_attr_group,
+	&pmu_cpumask_attr_group,
+	&mcb_pmu_v3_events_attr_group,
+	NULL
+};
+
+static const struct attribute_group *mc_pmu_v3_attr_groups[] = {
+	&mc_pmu_v3_format_attr_group,
+	&pmu_cpumask_attr_group,
+	&mc_pmu_v3_events_attr_group,
+	NULL
+};
+
 static int get_next_avail_cntr(struct xgene_pmu_dev *pmu_dev)
 {
 	int cntr;
@@ -387,23 +724,67 @@ static inline void xgene_pmu_mask_int(struct xgene_pmu *xgene_pmu)
 	writel(PCPPMU_INTENMASK, xgene_pmu->pcppmu_csr + PCPPMU_INTMASK_REG);
 }
 
+static inline void xgene_pmu_v3_mask_int(struct xgene_pmu *xgene_pmu)
+{
+	writel(PCPPMU_V3_INTENMASK, xgene_pmu->pcppmu_csr + PCPPMU_INTMASK_REG);
+}
+
 static inline void xgene_pmu_unmask_int(struct xgene_pmu *xgene_pmu)
 {
 	writel(PCPPMU_INTCLRMASK, xgene_pmu->pcppmu_csr + PCPPMU_INTMASK_REG);
 }
 
-static inline u32 xgene_pmu_read_counter(struct xgene_pmu_dev *pmu_dev, int idx)
+static inline void xgene_pmu_v3_unmask_int(struct xgene_pmu *xgene_pmu)
+{
+	writel(PCPPMU_V3_INTCLRMASK,
+	       xgene_pmu->pcppmu_csr + PCPPMU_INTMASK_REG);
+}
+
+static inline u64 xgene_pmu_read_counter32(struct xgene_pmu_dev *pmu_dev,
+					   int idx)
 {
 	return readl(pmu_dev->inf->csr + PMU_PMEVCNTR0 + (4 * idx));
 }
 
+static inline u64 xgene_pmu_read_counter64(struct xgene_pmu_dev *pmu_dev,
+					   int idx)
+{
+	u32 lo, hi;
+
+	/*
+	 * v3 has 64-bit counter registers composed by 2 32-bit registers
+	 * This can be a problem if the counter increases and carries
+	 * out of bit [31] between 2 reads. The extra reads would help
+	 * to prevent this issue.
+	 */
+	do {
+		hi = xgene_pmu_read_counter32(pmu_dev, 2 * idx + 1);
+		lo = xgene_pmu_read_counter32(pmu_dev, 2 * idx);
+	} while (hi != xgene_pmu_read_counter32(pmu_dev, 2 * idx + 1));
+
+	return (((u64)hi << 32) | lo);
+}
+
 static inline void
-xgene_pmu_write_counter(struct xgene_pmu_dev *pmu_dev, int idx, u32 val)
+xgene_pmu_write_counter32(struct xgene_pmu_dev *pmu_dev, int idx, u64 val)
 {
 	writel(val, pmu_dev->inf->csr + PMU_PMEVCNTR0 + (4 * idx));
 }
 
 static inline void
+xgene_pmu_write_counter64(struct xgene_pmu_dev *pmu_dev, int idx, u64 val)
+{
+	u32 cnt_lo, cnt_hi;
+
+	cnt_hi = upper_32_bits(val);
+	cnt_lo = lower_32_bits(val);
+
+	/* v3 has 64-bit counter registers composed by 2 32-bit registers */
+	xgene_pmu_write_counter32(pmu_dev, 2 * idx, cnt_lo);
+	xgene_pmu_write_counter32(pmu_dev, 2 * idx + 1, cnt_hi);
+}
+
+static inline void
 xgene_pmu_write_evttype(struct xgene_pmu_dev *pmu_dev, int idx, u32 val)
 {
 	writel(val, pmu_dev->inf->csr + PMU_PMEVTYPER0 + (4 * idx));
@@ -416,12 +797,18 @@ xgene_pmu_write_agentmsk(struct xgene_pmu_dev *pmu_dev, u32 val)
 }
 
 static inline void
+xgene_pmu_v3_write_agentmsk(struct xgene_pmu_dev *pmu_dev, u32 val) { }
+
+static inline void
 xgene_pmu_write_agent1msk(struct xgene_pmu_dev *pmu_dev, u32 val)
 {
 	writel(val, pmu_dev->inf->csr + PMU_PMAMR1);
 }
 
 static inline void
+xgene_pmu_v3_write_agent1msk(struct xgene_pmu_dev *pmu_dev, u32 val) { }
+
+static inline void
 xgene_pmu_enable_counter(struct xgene_pmu_dev *pmu_dev, int idx)
 {
 	u32 val;
@@ -491,20 +878,22 @@ static inline void xgene_pmu_stop_counters(struct xgene_pmu_dev *pmu_dev)
 static void xgene_perf_pmu_enable(struct pmu *pmu)
 {
 	struct xgene_pmu_dev *pmu_dev = to_pmu_dev(pmu);
+	struct xgene_pmu *xgene_pmu = pmu_dev->parent;
 	int enabled = bitmap_weight(pmu_dev->cntr_assign_mask,
 			pmu_dev->max_counters);
 
 	if (!enabled)
 		return;
 
-	xgene_pmu_start_counters(pmu_dev);
+	xgene_pmu->ops->start_counters(pmu_dev);
 }
 
 static void xgene_perf_pmu_disable(struct pmu *pmu)
 {
 	struct xgene_pmu_dev *pmu_dev = to_pmu_dev(pmu);
+	struct xgene_pmu *xgene_pmu = pmu_dev->parent;
 
-	xgene_pmu_stop_counters(pmu_dev);
+	xgene_pmu->ops->stop_counters(pmu_dev);
 }
 
 static int xgene_perf_event_init(struct perf_event *event)
@@ -572,49 +961,56 @@ static int xgene_perf_event_init(struct perf_event *event)
 static void xgene_perf_enable_event(struct perf_event *event)
 {
 	struct xgene_pmu_dev *pmu_dev = to_pmu_dev(event->pmu);
+	struct xgene_pmu *xgene_pmu = pmu_dev->parent;
 
-	xgene_pmu_write_evttype(pmu_dev, GET_CNTR(event), GET_EVENTID(event));
-	xgene_pmu_write_agentmsk(pmu_dev, ~((u32)GET_AGENTID(event)));
+	xgene_pmu->ops->write_evttype(pmu_dev, GET_CNTR(event),
+				      GET_EVENTID(event));
+	xgene_pmu->ops->write_agentmsk(pmu_dev, ~((u32)GET_AGENTID(event)));
 	if (pmu_dev->inf->type == PMU_TYPE_IOB)
-		xgene_pmu_write_agent1msk(pmu_dev, ~((u32)GET_AGENT1ID(event)));
+		xgene_pmu->ops->write_agent1msk(pmu_dev,
+						~((u32)GET_AGENT1ID(event)));
 
-	xgene_pmu_enable_counter(pmu_dev, GET_CNTR(event));
-	xgene_pmu_enable_counter_int(pmu_dev, GET_CNTR(event));
+	xgene_pmu->ops->enable_counter(pmu_dev, GET_CNTR(event));
+	xgene_pmu->ops->enable_counter_int(pmu_dev, GET_CNTR(event));
 }
 
 static void xgene_perf_disable_event(struct perf_event *event)
 {
 	struct xgene_pmu_dev *pmu_dev = to_pmu_dev(event->pmu);
+	struct xgene_pmu *xgene_pmu = pmu_dev->parent;
 
-	xgene_pmu_disable_counter(pmu_dev, GET_CNTR(event));
-	xgene_pmu_disable_counter_int(pmu_dev, GET_CNTR(event));
+	xgene_pmu->ops->disable_counter(pmu_dev, GET_CNTR(event));
+	xgene_pmu->ops->disable_counter_int(pmu_dev, GET_CNTR(event));
 }
 
 static void xgene_perf_event_set_period(struct perf_event *event)
 {
 	struct xgene_pmu_dev *pmu_dev = to_pmu_dev(event->pmu);
+	struct xgene_pmu *xgene_pmu = pmu_dev->parent;
 	struct hw_perf_event *hw = &event->hw;
 	/*
-	 * The X-Gene PMU counters have a period of 2^32. To account for the
-	 * possiblity of extreme interrupt latency we program for a period of
-	 * half that. Hopefully we can handle the interrupt before another 2^31
+	 * For 32 bit counter, it has a period of 2^32. To account for the
+	 * possibility of extreme interrupt latency we program for a period of
+	 * half that. Hopefully, we can handle the interrupt before another 2^31
 	 * events occur and the counter overtakes its previous value.
+	 * For 64 bit counter, we don't expect it overflow.
 	 */
 	u64 val = 1ULL << 31;
 
 	local64_set(&hw->prev_count, val);
-	xgene_pmu_write_counter(pmu_dev, hw->idx, (u32) val);
+	xgene_pmu->ops->write_counter(pmu_dev, hw->idx, val);
 }
 
 static void xgene_perf_event_update(struct perf_event *event)
 {
 	struct xgene_pmu_dev *pmu_dev = to_pmu_dev(event->pmu);
+	struct xgene_pmu *xgene_pmu = pmu_dev->parent;
 	struct hw_perf_event *hw = &event->hw;
 	u64 delta, prev_raw_count, new_raw_count;
 
 again:
 	prev_raw_count = local64_read(&hw->prev_count);
-	new_raw_count = xgene_pmu_read_counter(pmu_dev, GET_CNTR(event));
+	new_raw_count = xgene_pmu->ops->read_counter(pmu_dev, GET_CNTR(event));
 
 	if (local64_cmpxchg(&hw->prev_count, prev_raw_count,
 			    new_raw_count) != prev_raw_count)
@@ -633,6 +1029,7 @@ static void xgene_perf_read(struct perf_event *event)
 static void xgene_perf_start(struct perf_event *event, int flags)
 {
 	struct xgene_pmu_dev *pmu_dev = to_pmu_dev(event->pmu);
+	struct xgene_pmu *xgene_pmu = pmu_dev->parent;
 	struct hw_perf_event *hw = &event->hw;
 
 	if (WARN_ON_ONCE(!(hw->state & PERF_HES_STOPPED)))
@@ -646,8 +1043,8 @@ static void xgene_perf_start(struct perf_event *event, int flags)
 	if (flags & PERF_EF_RELOAD) {
 		u64 prev_raw_count =  local64_read(&hw->prev_count);
 
-		xgene_pmu_write_counter(pmu_dev, GET_CNTR(event),
-					(u32) prev_raw_count);
+		xgene_pmu->ops->write_counter(pmu_dev, GET_CNTR(event),
+					      prev_raw_count);
 	}
 
 	xgene_perf_enable_event(event);
@@ -713,7 +1110,10 @@ static int xgene_init_perf(struct xgene_pmu_dev *pmu_dev, char *name)
 {
 	struct xgene_pmu *xgene_pmu;
 
-	pmu_dev->max_period = PMU_CNT_MAX_PERIOD - 1;
+	if (pmu_dev->parent->version == PCP_PMU_V3)
+		pmu_dev->max_period = PMU_V3_CNT_MAX_PERIOD;
+	else
+		pmu_dev->max_period = PMU_CNT_MAX_PERIOD;
 	/* First version PMU supports only single event counter */
 	xgene_pmu = pmu_dev->parent;
 	if (xgene_pmu->version == PCP_PMU_V1)
@@ -736,8 +1136,8 @@ static int xgene_init_perf(struct xgene_pmu_dev *pmu_dev, char *name)
 	};
 
 	/* Hardware counter init */
-	xgene_pmu_stop_counters(pmu_dev);
-	xgene_pmu_reset_counters(pmu_dev);
+	xgene_pmu->ops->stop_counters(pmu_dev);
+	xgene_pmu->ops->reset_counters(pmu_dev);
 
 	return perf_pmu_register(&pmu_dev->pmu, name, -1);
 }
@@ -758,20 +1158,38 @@ xgene_pmu_dev_add(struct xgene_pmu *xgene_pmu, struct xgene_pmu_dev_ctx *ctx)
 
 	switch (pmu->inf->type) {
 	case PMU_TYPE_L3C:
-		pmu->attr_groups = l3c_pmu_attr_groups;
+		if (!(xgene_pmu->l3c_active_mask & pmu->inf->enable_mask))
+			goto dev_err;
+		if (xgene_pmu->version == PCP_PMU_V3)
+			pmu->attr_groups = l3c_pmu_v3_attr_groups;
+		else
+			pmu->attr_groups = l3c_pmu_attr_groups;
 		break;
 	case PMU_TYPE_IOB:
-		pmu->attr_groups = iob_pmu_attr_groups;
+		if (xgene_pmu->version == PCP_PMU_V3)
+			pmu->attr_groups = iob_fast_pmu_v3_attr_groups;
+		else
+			pmu->attr_groups = iob_pmu_attr_groups;
+		break;
+	case PMU_TYPE_IOB_SLOW:
+		if (xgene_pmu->version == PCP_PMU_V3)
+			pmu->attr_groups = iob_slow_pmu_v3_attr_groups;
 		break;
 	case PMU_TYPE_MCB:
 		if (!(xgene_pmu->mcb_active_mask & pmu->inf->enable_mask))
 			goto dev_err;
-		pmu->attr_groups = mcb_pmu_attr_groups;
+		if (xgene_pmu->version == PCP_PMU_V3)
+			pmu->attr_groups = mcb_pmu_v3_attr_groups;
+		else
+			pmu->attr_groups = mcb_pmu_attr_groups;
 		break;
 	case PMU_TYPE_MC:
 		if (!(xgene_pmu->mc_active_mask & pmu->inf->enable_mask))
 			goto dev_err;
-		pmu->attr_groups = mc_pmu_attr_groups;
+		if (xgene_pmu->version == PCP_PMU_V3)
+			pmu->attr_groups = mc_pmu_v3_attr_groups;
+		else
+			pmu->attr_groups = mc_pmu_attr_groups;
 		break;
 	default:
 		return -EINVAL;
@@ -795,18 +1213,27 @@ dev_err:
 static void _xgene_pmu_isr(int irq, struct xgene_pmu_dev *pmu_dev)
 {
 	struct xgene_pmu *xgene_pmu = pmu_dev->parent;
+	void __iomem *csr = pmu_dev->inf->csr;
 	u32 pmovsr;
 	int idx;
 
-	pmovsr = readl(pmu_dev->inf->csr + PMU_PMOVSR) & PMU_OVERFLOW_MASK;
+	xgene_pmu->ops->stop_counters(pmu_dev);
+
+	if (xgene_pmu->version == PCP_PMU_V3)
+		pmovsr = readl(csr + PMU_PMOVSSET) & PMU_OVERFLOW_MASK;
+	else
+		pmovsr = readl(csr + PMU_PMOVSR) & PMU_OVERFLOW_MASK;
+
 	if (!pmovsr)
-		return;
+		goto out;
 
 	/* Clear interrupt flag */
 	if (xgene_pmu->version == PCP_PMU_V1)
-		writel(0x0, pmu_dev->inf->csr + PMU_PMOVSR);
+		writel(0x0, csr + PMU_PMOVSR);
+	else if (xgene_pmu->version == PCP_PMU_V2)
+		writel(pmovsr, csr + PMU_PMOVSR);
 	else
-		writel(pmovsr, pmu_dev->inf->csr + PMU_PMOVSR);
+		writel(pmovsr, csr + PMU_PMOVSCLR);
 
 	for (idx = 0; idx < PMU_MAX_COUNTERS; idx++) {
 		struct perf_event *event = pmu_dev->pmu_counter_event[idx];
@@ -818,10 +1245,14 @@ static void _xgene_pmu_isr(int irq, struct xgene_pmu_dev *pmu_dev)
 		xgene_perf_event_update(event);
 		xgene_perf_event_set_period(event);
 	}
+
+out:
+	xgene_pmu->ops->start_counters(pmu_dev);
 }
 
 static irqreturn_t xgene_pmu_isr(int irq, void *dev_id)
 {
+	u32 intr_mcu, intr_mcb, intr_l3c, intr_iob;
 	struct xgene_pmu_dev_ctx *ctx;
 	struct xgene_pmu *xgene_pmu = dev_id;
 	unsigned long flags;
@@ -831,22 +1262,33 @@ static irqreturn_t xgene_pmu_isr(int irq, void *dev_id)
 
 	/* Get Interrupt PMU source */
 	val = readl(xgene_pmu->pcppmu_csr + PCPPMU_INTSTATUS_REG);
-	if (val & PCPPMU_INT_MCU) {
+	if (xgene_pmu->version == PCP_PMU_V3) {
+		intr_mcu = PCPPMU_V3_INT_MCU;
+		intr_mcb = PCPPMU_V3_INT_MCB;
+		intr_l3c = PCPPMU_V3_INT_L3C;
+		intr_iob = PCPPMU_V3_INT_IOB;
+	} else {
+		intr_mcu = PCPPMU_INT_MCU;
+		intr_mcb = PCPPMU_INT_MCB;
+		intr_l3c = PCPPMU_INT_L3C;
+		intr_iob = PCPPMU_INT_IOB;
+	}
+	if (val & intr_mcu) {
 		list_for_each_entry(ctx, &xgene_pmu->mcpmus, next) {
 			_xgene_pmu_isr(irq, ctx->pmu_dev);
 		}
 	}
-	if (val & PCPPMU_INT_MCB) {
+	if (val & intr_mcb) {
 		list_for_each_entry(ctx, &xgene_pmu->mcbpmus, next) {
 			_xgene_pmu_isr(irq, ctx->pmu_dev);
 		}
 	}
-	if (val & PCPPMU_INT_L3C) {
+	if (val & intr_l3c) {
 		list_for_each_entry(ctx, &xgene_pmu->l3cpmus, next) {
 			_xgene_pmu_isr(irq, ctx->pmu_dev);
 		}
 	}
-	if (val & PCPPMU_INT_IOB) {
+	if (val & intr_iob) {
 		list_for_each_entry(ctx, &xgene_pmu->iobpmus, next) {
 			_xgene_pmu_isr(irq, ctx->pmu_dev);
 		}
@@ -857,8 +1299,8 @@ static irqreturn_t xgene_pmu_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int acpi_pmu_probe_active_mcb_mcu(struct xgene_pmu *xgene_pmu,
-					 struct platform_device *pdev)
+static int acpi_pmu_probe_active_mcb_mcu_l3c(struct xgene_pmu *xgene_pmu,
+					     struct platform_device *pdev)
 {
 	void __iomem *csw_csr, *mcba_csr, *mcbb_csr;
 	struct resource *res;
@@ -885,6 +1327,8 @@ static int acpi_pmu_probe_active_mcb_mcu(struct xgene_pmu *xgene_pmu,
 		return PTR_ERR(mcbb_csr);
 	}
 
+	xgene_pmu->l3c_active_mask = 0x1;
+
 	reg = readl(csw_csr + CSW_CSWCR);
 	if (reg & CSW_CSWCR_DUALMCB_MASK) {
 		/* Dual MCB active */
@@ -905,8 +1349,56 @@ static int acpi_pmu_probe_active_mcb_mcu(struct xgene_pmu *xgene_pmu,
 	return 0;
 }
 
-static int fdt_pmu_probe_active_mcb_mcu(struct xgene_pmu *xgene_pmu,
-					struct platform_device *pdev)
+static int acpi_pmu_v3_probe_active_mcb_mcu_l3c(struct xgene_pmu *xgene_pmu,
+						struct platform_device *pdev)
+{
+	void __iomem *csw_csr;
+	struct resource *res;
+	unsigned int reg;
+	u32 mcb0routing;
+	u32 mcb1routing;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	csw_csr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(csw_csr)) {
+		dev_err(&pdev->dev, "ioremap failed for CSW CSR resource\n");
+		return PTR_ERR(csw_csr);
+	}
+
+	reg = readl(csw_csr + CSW_CSWCR);
+	mcb0routing = CSW_CSWCR_MCB0_ROUTING(reg);
+	mcb1routing = CSW_CSWCR_MCB1_ROUTING(reg);
+	if (reg & CSW_CSWCR_DUALMCB_MASK) {
+		/* Dual MCB active */
+		xgene_pmu->mcb_active_mask = 0x3;
+		/* Probe all active L3C(s), maximum is 8 */
+		xgene_pmu->l3c_active_mask = 0xFF;
+		/* Probe all active MC(s), maximum is 8 */
+		if ((mcb0routing == 0x2) && (mcb1routing == 0x2))
+			xgene_pmu->mc_active_mask = 0xFF;
+		else if ((mcb0routing == 0x1) && (mcb1routing == 0x1))
+			xgene_pmu->mc_active_mask =  0x33;
+		else
+			xgene_pmu->mc_active_mask =  0x11;
+	} else {
+		/* Single MCB active */
+		xgene_pmu->mcb_active_mask = 0x1;
+		/* Probe all active L3C(s), maximum is 4 */
+		xgene_pmu->l3c_active_mask = 0x0F;
+		/* Probe all active MC(s), maximum is 4 */
+		if (mcb0routing == 0x2)
+			xgene_pmu->mc_active_mask = 0x0F;
+		else if (mcb0routing == 0x1)
+			xgene_pmu->mc_active_mask =  0x03;
+		else
+			xgene_pmu->mc_active_mask =  0x01;
+	}
+
+	return 0;
+}
+
+static int fdt_pmu_probe_active_mcb_mcu_l3c(struct xgene_pmu *xgene_pmu,
+					    struct platform_device *pdev)
 {
 	struct regmap *csw_map, *mcba_map, *mcbb_map;
 	struct device_node *np = pdev->dev.of_node;
@@ -930,6 +1422,7 @@ static int fdt_pmu_probe_active_mcb_mcu(struct xgene_pmu *xgene_pmu,
 		return PTR_ERR(mcbb_map);
 	}
 
+	xgene_pmu->l3c_active_mask = 0x1;
 	if (regmap_read(csw_map, CSW_CSWCR, &reg))
 		return -EINVAL;
 
@@ -954,12 +1447,18 @@ static int fdt_pmu_probe_active_mcb_mcu(struct xgene_pmu *xgene_pmu,
 	return 0;
 }
 
-static int xgene_pmu_probe_active_mcb_mcu(struct xgene_pmu *xgene_pmu,
-					  struct platform_device *pdev)
+static int xgene_pmu_probe_active_mcb_mcu_l3c(struct xgene_pmu *xgene_pmu,
+					      struct platform_device *pdev)
 {
-	if (has_acpi_companion(&pdev->dev))
-		return acpi_pmu_probe_active_mcb_mcu(xgene_pmu, pdev);
-	return fdt_pmu_probe_active_mcb_mcu(xgene_pmu, pdev);
+	if (has_acpi_companion(&pdev->dev)) {
+		if (xgene_pmu->version == PCP_PMU_V3)
+			return acpi_pmu_v3_probe_active_mcb_mcu_l3c(xgene_pmu,
+								    pdev);
+		else
+			return acpi_pmu_probe_active_mcb_mcu_l3c(xgene_pmu,
+								 pdev);
+	}
+	return fdt_pmu_probe_active_mcb_mcu_l3c(xgene_pmu, pdev);
 }
 
 static char *xgene_pmu_dev_name(struct device *dev, u32 type, int id)
@@ -969,6 +1468,8 @@ static char *xgene_pmu_dev_name(struct device *dev, u32 type, int id)
 		return devm_kasprintf(dev, GFP_KERNEL, "l3c%d", id);
 	case PMU_TYPE_IOB:
 		return devm_kasprintf(dev, GFP_KERNEL, "iob%d", id);
+	case PMU_TYPE_IOB_SLOW:
+		return devm_kasprintf(dev, GFP_KERNEL, "iob-slow%d", id);
 	case PMU_TYPE_MCB:
 		return devm_kasprintf(dev, GFP_KERNEL, "mcb%d", id);
 	case PMU_TYPE_MC:
@@ -1047,9 +1548,40 @@ err:
 	return NULL;
 }
 
+static const struct acpi_device_id xgene_pmu_acpi_type_match[] = {
+	{"APMC0D5D", PMU_TYPE_L3C},
+	{"APMC0D5E", PMU_TYPE_IOB},
+	{"APMC0D5F", PMU_TYPE_MCB},
+	{"APMC0D60", PMU_TYPE_MC},
+	{"APMC0D84", PMU_TYPE_L3C},
+	{"APMC0D85", PMU_TYPE_IOB},
+	{"APMC0D86", PMU_TYPE_IOB_SLOW},
+	{"APMC0D87", PMU_TYPE_MCB},
+	{"APMC0D88", PMU_TYPE_MC},
+	{},
+};
+
+static const struct acpi_device_id *xgene_pmu_acpi_match_type(
+					const struct acpi_device_id *ids,
+					struct acpi_device *adev)
+{
+	const struct acpi_device_id *match_id = NULL;
+	const struct acpi_device_id *id;
+
+	for (id = ids; id->id[0] || id->cls; id++) {
+		if (!acpi_match_device_ids(adev, id))
+			match_id = id;
+		else if (match_id)
+			break;
+	}
+
+	return match_id;
+}
+
 static acpi_status acpi_pmu_dev_add(acpi_handle handle, u32 level,
 				    void *data, void **return_value)
 {
+	const struct acpi_device_id *acpi_id;
 	struct xgene_pmu *xgene_pmu = data;
 	struct xgene_pmu_dev_ctx *ctx;
 	struct acpi_device *adev;
@@ -1059,17 +1591,11 @@ static acpi_status acpi_pmu_dev_add(acpi_handle handle, u32 level,
 	if (acpi_bus_get_status(adev) || !adev->status.present)
 		return AE_OK;
 
-	if (!strcmp(acpi_device_hid(adev), "APMC0D5D"))
-		ctx = acpi_get_pmu_hw_inf(xgene_pmu, adev, PMU_TYPE_L3C);
-	else if (!strcmp(acpi_device_hid(adev), "APMC0D5E"))
-		ctx = acpi_get_pmu_hw_inf(xgene_pmu, adev, PMU_TYPE_IOB);
-	else if (!strcmp(acpi_device_hid(adev), "APMC0D5F"))
-		ctx = acpi_get_pmu_hw_inf(xgene_pmu, adev, PMU_TYPE_MCB);
-	else if (!strcmp(acpi_device_hid(adev), "APMC0D60"))
-		ctx = acpi_get_pmu_hw_inf(xgene_pmu, adev, PMU_TYPE_MC);
-	else
-		ctx = NULL;
+	acpi_id = xgene_pmu_acpi_match_type(xgene_pmu_acpi_type_match, adev);
+	if (!acpi_id)
+		return AE_OK;
 
+	ctx = acpi_get_pmu_hw_inf(xgene_pmu, adev, (u32)acpi_id->driver_data);
 	if (!ctx)
 		return AE_OK;
 
@@ -1086,6 +1612,9 @@ static acpi_status acpi_pmu_dev_add(acpi_handle handle, u32 level,
 	case PMU_TYPE_IOB:
 		list_add(&ctx->next, &xgene_pmu->iobpmus);
 		break;
+	case PMU_TYPE_IOB_SLOW:
+		list_add(&ctx->next, &xgene_pmu->iobpmus);
+		break;
 	case PMU_TYPE_MCB:
 		list_add(&ctx->next, &xgene_pmu->mcbpmus);
 		break;
@@ -1207,6 +1736,9 @@ static int fdt_pmu_probe_pmu_dev(struct xgene_pmu *xgene_pmu,
 		case PMU_TYPE_IOB:
 			list_add(&ctx->next, &xgene_pmu->iobpmus);
 			break;
+		case PMU_TYPE_IOB_SLOW:
+			list_add(&ctx->next, &xgene_pmu->iobpmus);
+			break;
 		case PMU_TYPE_MCB:
 			list_add(&ctx->next, &xgene_pmu->mcbpmus);
 			break;
@@ -1235,6 +1767,40 @@ static const struct xgene_pmu_data xgene_pmu_v2_data = {
 	.id   = PCP_PMU_V2,
 };
 
+static const struct xgene_pmu_ops xgene_pmu_ops = {
+	.mask_int = xgene_pmu_mask_int,
+	.unmask_int = xgene_pmu_unmask_int,
+	.read_counter = xgene_pmu_read_counter32,
+	.write_counter = xgene_pmu_write_counter32,
+	.write_evttype = xgene_pmu_write_evttype,
+	.write_agentmsk = xgene_pmu_write_agentmsk,
+	.write_agent1msk = xgene_pmu_write_agent1msk,
+	.enable_counter = xgene_pmu_enable_counter,
+	.disable_counter = xgene_pmu_disable_counter,
+	.enable_counter_int = xgene_pmu_enable_counter_int,
+	.disable_counter_int = xgene_pmu_disable_counter_int,
+	.reset_counters = xgene_pmu_reset_counters,
+	.start_counters = xgene_pmu_start_counters,
+	.stop_counters = xgene_pmu_stop_counters,
+};
+
+static const struct xgene_pmu_ops xgene_pmu_v3_ops = {
+	.mask_int = xgene_pmu_v3_mask_int,
+	.unmask_int = xgene_pmu_v3_unmask_int,
+	.read_counter = xgene_pmu_read_counter64,
+	.write_counter = xgene_pmu_write_counter64,
+	.write_evttype = xgene_pmu_write_evttype,
+	.write_agentmsk = xgene_pmu_v3_write_agentmsk,
+	.write_agent1msk = xgene_pmu_v3_write_agent1msk,
+	.enable_counter = xgene_pmu_enable_counter,
+	.disable_counter = xgene_pmu_disable_counter,
+	.enable_counter_int = xgene_pmu_enable_counter_int,
+	.disable_counter_int = xgene_pmu_disable_counter_int,
+	.reset_counters = xgene_pmu_reset_counters,
+	.start_counters = xgene_pmu_start_counters,
+	.stop_counters = xgene_pmu_stop_counters,
+};
+
 static const struct of_device_id xgene_pmu_of_match[] = {
 	{ .compatible	= "apm,xgene-pmu",	.data = &xgene_pmu_data },
 	{ .compatible	= "apm,xgene-pmu-v2",	.data = &xgene_pmu_v2_data },
@@ -1245,6 +1811,7 @@ MODULE_DEVICE_TABLE(of, xgene_pmu_of_match);
 static const struct acpi_device_id xgene_pmu_acpi_match[] = {
 	{"APMC0D5B", PCP_PMU_V1},
 	{"APMC0D5C", PCP_PMU_V2},
+	{"APMC0D83", PCP_PMU_V3},
 	{},
 };
 MODULE_DEVICE_TABLE(acpi, xgene_pmu_acpi_match);
@@ -1284,6 +1851,11 @@ static int xgene_pmu_probe(struct platform_device *pdev)
 	if (version < 0)
 		return -ENODEV;
 
+	if (version == PCP_PMU_V3)
+		xgene_pmu->ops = &xgene_pmu_v3_ops;
+	else
+		xgene_pmu->ops = &xgene_pmu_ops;
+
 	INIT_LIST_HEAD(&xgene_pmu->l3cpmus);
 	INIT_LIST_HEAD(&xgene_pmu->iobpmus);
 	INIT_LIST_HEAD(&xgene_pmu->mcbpmus);
@@ -1317,7 +1889,7 @@ static int xgene_pmu_probe(struct platform_device *pdev)
 	raw_spin_lock_init(&xgene_pmu->lock);
 
 	/* Check for active MCBs and MCUs */
-	rc = xgene_pmu_probe_active_mcb_mcu(xgene_pmu, pdev);
+	rc = xgene_pmu_probe_active_mcb_mcu_l3c(xgene_pmu, pdev);
 	if (rc) {
 		dev_warn(&pdev->dev, "Unknown MCB/MCU active status\n");
 		xgene_pmu->mcb_active_mask = 0x1;
@@ -1342,7 +1914,7 @@ static int xgene_pmu_probe(struct platform_device *pdev)
 	}
 
 	/* Enable interrupt */
-	xgene_pmu_unmask_int(xgene_pmu);
+	xgene_pmu->ops->unmask_int(xgene_pmu);
 
 	return 0;
 
diff --git a/drivers/phy/Kconfig b/drivers/phy/Kconfig
index afaf7b643eeb..c1807d4a0079 100644
--- a/drivers/phy/Kconfig
+++ b/drivers/phy/Kconfig
@@ -15,73 +15,6 @@ config GENERIC_PHY
 	  phy users can obtain reference to the PHY. All the users of this
 	  framework should select this config.
 
-config PHY_BCM_NS_USB2
-	tristate "Broadcom Northstar USB 2.0 PHY Driver"
-	depends on ARCH_BCM_IPROC || COMPILE_TEST
-	depends on HAS_IOMEM && OF
-	select GENERIC_PHY
-	help
-	  Enable this to support Broadcom USB 2.0 PHY connected to the USB
-	  controller on Northstar family.
-
-config PHY_BCM_NS_USB3
-	tristate "Broadcom Northstar USB 3.0 PHY Driver"
-	depends on ARCH_BCM_IPROC || COMPILE_TEST
-	depends on HAS_IOMEM && OF
-	select GENERIC_PHY
-	help
-	  Enable this to support Broadcom USB 3.0 PHY connected to the USB
-	  controller on Northstar family.
-
-config PHY_BERLIN_USB
-	tristate "Marvell Berlin USB PHY Driver"
-	depends on ARCH_BERLIN && RESET_CONTROLLER && HAS_IOMEM && OF
-	select GENERIC_PHY
-	help
-	  Enable this to support the USB PHY on Marvell Berlin SoCs.
-
-config PHY_BERLIN_SATA
-	tristate "Marvell Berlin SATA PHY driver"
-	depends on ARCH_BERLIN && HAS_IOMEM && OF
-	select GENERIC_PHY
-	help
-	  Enable this to support the SATA PHY on Marvell Berlin SoCs.
-
-config ARMADA375_USBCLUSTER_PHY
-	def_bool y
-	depends on MACH_ARMADA_375 || COMPILE_TEST
-	depends on OF && HAS_IOMEM
-	select GENERIC_PHY
-
-config PHY_DA8XX_USB
-	tristate "TI DA8xx USB PHY Driver"
-	depends on ARCH_DAVINCI_DA8XX
-	select GENERIC_PHY
-	select MFD_SYSCON
-	help
-	  Enable this to support the USB PHY on DA8xx SoCs.
-
-	  This driver controls both the USB 1.1 PHY and the USB 2.0 PHY.
-
-config PHY_DM816X_USB
-	tristate "TI dm816x USB PHY driver"
-	depends on ARCH_OMAP2PLUS
-	depends on USB_SUPPORT
-	select GENERIC_PHY
-	select USB_PHY
-	help
-	  Enable this for dm816x USB to work.
-
-config PHY_EXYNOS_MIPI_VIDEO
-	tristate "S5P/EXYNOS SoC series MIPI CSI-2/DSI PHY driver"
-	depends on HAS_IOMEM
-	depends on ARCH_S5PV210 || ARCH_EXYNOS || COMPILE_TEST
-	select GENERIC_PHY
-	default y if ARCH_S5PV210 || ARCH_EXYNOS
-	help
-	  Support for MIPI CSI-2 and MIPI DSI DPHY found on Samsung S5P
-	  and EXYNOS SoCs.
-
 config PHY_LPC18XX_USB_OTG
 	tristate "NXP LPC18xx/43xx SoC USB OTG PHY driver"
 	depends on OF && (ARCH_LPC18XX || COMPILE_TEST)
@@ -93,146 +26,6 @@ config PHY_LPC18XX_USB_OTG
 	  This driver is need for USB0 support on LPC18xx/43xx and takes
 	  care of enabling and clock setup.
 
-config PHY_PXA_28NM_HSIC
-	tristate "Marvell USB HSIC 28nm PHY Driver"
-	depends on HAS_IOMEM
-	select GENERIC_PHY
-	help
-	  Enable this to support Marvell USB HSIC PHY driver for Marvell
-	  SoC. This driver will do the PHY initialization and shutdown.
-	  The PHY driver will be used by Marvell ehci driver.
-
-	  To compile this driver as a module, choose M here.
-
-config PHY_PXA_28NM_USB2
-	tristate "Marvell USB 2.0 28nm PHY Driver"
-	depends on HAS_IOMEM
-	select GENERIC_PHY
-	help
-	  Enable this to support Marvell USB 2.0 PHY driver for Marvell
-	  SoC. This driver will do the PHY initialization and shutdown.
-	  The PHY driver will be used by Marvell udc/ehci/otg driver.
-
-	  To compile this driver as a module, choose M here.
-
-config PHY_MVEBU_SATA
-	def_bool y
-	depends on ARCH_DOVE || MACH_DOVE || MACH_KIRKWOOD
-	depends on OF
-	select GENERIC_PHY
-
-config PHY_MIPHY28LP
-	tristate "STMicroelectronics MIPHY28LP PHY driver for STiH407"
-	depends on ARCH_STI
-	select GENERIC_PHY
-	help
-	  Enable this to support the miphy transceiver (for SATA/PCIE/USB3)
-	  that is part of STMicroelectronics STiH407 SoC.
-
-config PHY_RCAR_GEN2
-	tristate "Renesas R-Car generation 2 USB PHY driver"
-	depends on ARCH_RENESAS
-	depends on GENERIC_PHY
-	help
-	  Support for USB PHY found on Renesas R-Car generation 2 SoCs.
-
-config PHY_RCAR_GEN3_USB2
-	tristate "Renesas R-Car generation 3 USB 2.0 PHY driver"
-	depends on ARCH_RENESAS
-	depends on EXTCON
-	select GENERIC_PHY
-	help
-	  Support for USB 2.0 PHY found on Renesas R-Car generation 3 SoCs.
-
-config OMAP_CONTROL_PHY
-	tristate "OMAP CONTROL PHY Driver"
-	depends on ARCH_OMAP2PLUS || COMPILE_TEST
-	help
-	  Enable this to add support for the PHY part present in the control
-	  module. This driver has API to power on the USB2 PHY and to write to
-	  the mailbox. The mailbox is present only in omap4 and the register to
-	  power on the USB2 PHY is present in OMAP4 and OMAP5. OMAP5 has an
-	  additional register to power on USB3 PHY/SATA PHY/PCIE PHY
-	  (PIPE3 PHY).
-
-config OMAP_USB2
-	tristate "OMAP USB2 PHY Driver"
-	depends on ARCH_OMAP2PLUS
-	depends on USB_SUPPORT
-	select GENERIC_PHY
-	select USB_PHY
-	select OMAP_CONTROL_PHY
-	depends on OMAP_OCP2SCP
-	help
-	  Enable this to support the transceiver that is part of SOC. This
-	  driver takes care of all the PHY functionality apart from comparator.
-	  The USB OTG controller communicates with the comparator using this
-	  driver.
-
-config TI_PIPE3
-	tristate "TI PIPE3 PHY Driver"
-	depends on ARCH_OMAP2PLUS || COMPILE_TEST
-	select GENERIC_PHY
-	select OMAP_CONTROL_PHY
-	depends on OMAP_OCP2SCP
-	help
-	  Enable this to support the PIPE3 PHY that is part of TI SOCs. This
-	  driver takes care of all the PHY functionality apart from comparator.
-	  This driver interacts with the "OMAP Control PHY Driver" to power
-	  on/off the PHY.
-
-config TWL4030_USB
-	tristate "TWL4030 USB Transceiver Driver"
-	depends on TWL4030_CORE && REGULATOR_TWL4030 && USB_MUSB_OMAP2PLUS
-	depends on USB_SUPPORT
-	depends on USB_GADGET || !USB_GADGET # if USB_GADGET=m, this can't 'y'
-	select GENERIC_PHY
-	select USB_PHY
-	help
-	  Enable this to support the USB OTG transceiver on TWL4030
-	  family chips (including the TWL5030 and TPS659x0 devices).
-	  This transceiver supports high and full speed devices plus,
-	  in host mode, low speed.
-
-config PHY_EXYNOS_DP_VIDEO
-	tristate "EXYNOS SoC series Display Port PHY driver"
-	depends on OF
-	depends on ARCH_EXYNOS || COMPILE_TEST
-	default ARCH_EXYNOS
-	select GENERIC_PHY
-	help
-	  Support for Display Port PHY found on Samsung EXYNOS SoCs.
-
-config BCM_KONA_USB2_PHY
-	tristate "Broadcom Kona USB2 PHY Driver"
-	depends on HAS_IOMEM
-	select GENERIC_PHY
-	help
-	  Enable this to support the Broadcom Kona USB 2.0 PHY.
-
-config PHY_EXYNOS5250_SATA
-	tristate "Exynos5250 Sata SerDes/PHY driver"
-	depends on SOC_EXYNOS5250
-	depends on HAS_IOMEM
-	depends on OF
-	select GENERIC_PHY
-	select I2C
-	select I2C_S3C2410
-	select MFD_SYSCON
-	help
-	  Enable this to support SATA SerDes/Phy found on Samsung's
-	  Exynos5250 based SoCs.This SerDes/Phy supports SATA 1.5 Gb/s,
-	  SATA 3.0 Gb/s, SATA 6.0 Gb/s speeds. It supports one SATA host
-	  port to accept one SATA device.
-
-config PHY_HIX5HD2_SATA
-	tristate "HIX5HD2 SATA PHY Driver"
-	depends on ARCH_HIX5HD2 && OF && HAS_IOMEM
-	select GENERIC_PHY
-	select MFD_SYSCON
-	help
-	  Support for SATA PHY on Hisilicon hix5hd2 Soc.
-
 config PHY_MT65XX_USB3
 	tristate "Mediatek USB3.0 PHY Driver"
 	depends on ARCH_MEDIATEK && OF
@@ -241,104 +34,6 @@ config PHY_MT65XX_USB3
 	  Say 'Y' here to add support for Mediatek USB3.0 PHY driver,
 	  it supports multiple usb2.0 and usb3.0 ports.
 
-config PHY_HI6220_USB
-	tristate "hi6220 USB PHY support"
-	depends on (ARCH_HISI && ARM64) || COMPILE_TEST
-	select GENERIC_PHY
-	select MFD_SYSCON
-	help
-	  Enable this to support the HISILICON HI6220 USB PHY.
-
-	  To compile this driver as a module, choose M here.
-
-config PHY_SUN4I_USB
-	tristate "Allwinner sunxi SoC USB PHY driver"
-	depends on ARCH_SUNXI && HAS_IOMEM && OF
-	depends on RESET_CONTROLLER
-	depends on EXTCON
-	depends on POWER_SUPPLY
-	depends on USB_SUPPORT
-	select GENERIC_PHY
-	select USB_COMMON
-	help
-	  Enable this to support the transceiver that is part of Allwinner
-	  sunxi SoCs.
-
-	  This driver controls the entire USB PHY block, both the USB OTG
-	  parts, as well as the 2 regular USB 2 host PHYs.
-
-config PHY_SUN9I_USB
-	tristate "Allwinner sun9i SoC USB PHY driver"
-	depends on ARCH_SUNXI && HAS_IOMEM && OF
-	depends on RESET_CONTROLLER
-	depends on USB_SUPPORT
-	select USB_COMMON
-	select GENERIC_PHY
-	help
-	  Enable this to support the transceiver that is part of Allwinner
-	  sun9i SoCs.
-
-	  This driver controls each individual USB 2 host PHY.
-
-config PHY_SAMSUNG_USB2
-	tristate "Samsung USB 2.0 PHY driver"
-	depends on HAS_IOMEM
-	depends on USB_EHCI_EXYNOS || USB_OHCI_EXYNOS || USB_DWC2
-	select GENERIC_PHY
-	select MFD_SYSCON
-	default ARCH_EXYNOS
-	help
-	  Enable this to support the Samsung USB 2.0 PHY driver for Samsung
-	  SoCs. This driver provides the interface for USB 2.0 PHY. Support
-	  for particular PHYs will be enabled based on the SoC type in addition
-	  to this driver.
-
-config PHY_S5PV210_USB2
-	bool "Support for S5PV210"
-	depends on PHY_SAMSUNG_USB2
-	depends on ARCH_S5PV210
-	help
-	  Enable USB PHY support for S5PV210. This option requires that Samsung
-	  USB 2.0 PHY driver is enabled and means that support for this
-	  particular SoC is compiled in the driver. In case of S5PV210 two phys
-	  are available - device and host.
-
-config PHY_EXYNOS4210_USB2
-	bool
-	depends on PHY_SAMSUNG_USB2
-	default CPU_EXYNOS4210
-
-config PHY_EXYNOS4X12_USB2
-	bool
-	depends on PHY_SAMSUNG_USB2
-	default SOC_EXYNOS3250 || SOC_EXYNOS4212 || SOC_EXYNOS4412
-
-config PHY_EXYNOS5250_USB2
-	bool
-	depends on PHY_SAMSUNG_USB2
-	default SOC_EXYNOS5250 || SOC_EXYNOS5420
-
-config PHY_EXYNOS5_USBDRD
-	tristate "Exynos5 SoC series USB DRD PHY driver"
-	depends on ARCH_EXYNOS && OF
-	depends on HAS_IOMEM
-	depends on USB_DWC3_EXYNOS
-	select GENERIC_PHY
-	select MFD_SYSCON
-	default y
-	help
-	  Enable USB DRD PHY support for Exynos 5 SoC series.
-	  This driver provides PHY interface for USB 3.0 DRD controller
-	  present on Exynos5 SoC series.
-
-config PHY_EXYNOS_PCIE
-	bool "Exynos PCIe PHY driver"
-	depends on OF && (ARCH_EXYNOS || COMPILE_TEST)
-	select GENERIC_PHY
-	help
-	  Enable PCIe PHY support for Exynos SoC series.
-	  This driver provides PHY interface for Exynos PCIe controller.
-
 config PHY_PISTACHIO_USB
 	tristate "IMG Pistachio USB2.0 PHY driver"
 	depends on MACH_PISTACHIO
@@ -346,83 +41,6 @@ config PHY_PISTACHIO_USB
 	help
 	  Enable this to support the USB2.0 PHY on the IMG Pistachio SoC.
 
-config PHY_QCOM_APQ8064_SATA
-	tristate "Qualcomm APQ8064 SATA SerDes/PHY driver"
-	depends on ARCH_QCOM
-	depends on HAS_IOMEM
-	depends on OF
-	select GENERIC_PHY
-
-config PHY_QCOM_IPQ806X_SATA
-	tristate "Qualcomm IPQ806x SATA SerDes/PHY driver"
-	depends on ARCH_QCOM
-	depends on HAS_IOMEM
-	depends on OF
-	select GENERIC_PHY
-
-config PHY_ROCKCHIP_USB
-	tristate "Rockchip USB2 PHY Driver"
-	depends on ARCH_ROCKCHIP && OF
-	select GENERIC_PHY
-	help
-	  Enable this to support the Rockchip USB 2.0 PHY.
-
-config PHY_ROCKCHIP_INNO_USB2
-	tristate "Rockchip INNO USB2PHY Driver"
-	depends on (ARCH_ROCKCHIP || COMPILE_TEST) && OF
-	depends on COMMON_CLK
-	depends on EXTCON
-	depends on USB_SUPPORT
-	select GENERIC_PHY
-	select USB_COMMON
-	help
-	  Support for Rockchip USB2.0 PHY with Innosilicon IP block.
-
-config PHY_ROCKCHIP_EMMC
-	tristate "Rockchip EMMC PHY Driver"
-	depends on ARCH_ROCKCHIP && OF
-	select GENERIC_PHY
-	help
-	  Enable this to support the Rockchip EMMC PHY.
-
-config PHY_ROCKCHIP_DP
-	tristate "Rockchip Display Port PHY Driver"
-	depends on ARCH_ROCKCHIP && OF
-	select GENERIC_PHY
-	help
-	  Enable this to support the Rockchip Display Port PHY.
-
-config PHY_ROCKCHIP_PCIE
-	tristate "Rockchip PCIe PHY Driver"
-	depends on (ARCH_ROCKCHIP && OF) || COMPILE_TEST
-	select GENERIC_PHY
-	select MFD_SYSCON
-	help
-	  Enable this to support the Rockchip PCIe PHY.
-
-config PHY_ROCKCHIP_TYPEC
-	tristate "Rockchip TYPEC PHY Driver"
-	depends on OF && (ARCH_ROCKCHIP || COMPILE_TEST)
-	select EXTCON
-	select GENERIC_PHY
-	select RESET_CONTROLLER
-	help
-	  Enable this to support the Rockchip USB TYPEC PHY.
-
-config PHY_ST_SPEAR1310_MIPHY
-	tristate "ST SPEAR1310-MIPHY driver"
-	select GENERIC_PHY
-	depends on MACH_SPEAR1310 || COMPILE_TEST
-	help
-	  Support for ST SPEAr1310 MIPHY which can be used for PCIe and SATA.
-
-config PHY_ST_SPEAR1340_MIPHY
-	tristate "ST SPEAR1340-MIPHY driver"
-	select GENERIC_PHY
-	depends on MACH_SPEAR1340 || COMPILE_TEST
-	help
-	  Support for ST SPEAr1340 MIPHY which can be used for PCIe and SATA.
-
 config PHY_XGENE
 	tristate "APM X-Gene 15Gbps PHY support"
 	depends on HAS_IOMEM && OF && (ARM64 || COMPILE_TEST)
@@ -430,104 +48,18 @@ config PHY_XGENE
 	help
 	  This option enables support for APM X-Gene SoC multi-purpose PHY.
 
-config PHY_STIH407_USB
-	tristate "STMicroelectronics USB2 picoPHY driver for STiH407 family"
-	depends on RESET_CONTROLLER
-	depends on ARCH_STI || COMPILE_TEST
-	select GENERIC_PHY
-	help
-	  Enable this support to enable the picoPHY device used by USB2
-	  and USB3 controllers on STMicroelectronics STiH407 SoC families.
-
-config PHY_QCOM_QMP
-	tristate "Qualcomm QMP PHY Driver"
-	depends on OF && COMMON_CLK && (ARCH_QCOM || COMPILE_TEST)
-	select GENERIC_PHY
-	help
-	  Enable this to support the QMP PHY transceiver that is used
-	  with controllers such as PCIe, UFS, and USB on Qualcomm chips.
-
-config PHY_QCOM_QUSB2
-	tristate "Qualcomm QUSB2 PHY Driver"
-	depends on OF && (ARCH_QCOM || COMPILE_TEST)
-	depends on NVMEM || !NVMEM
-	select GENERIC_PHY
-	help
-	  Enable this to support the HighSpeed QUSB2 PHY transceiver for USB
-	  controllers on Qualcomm chips. This driver supports the high-speed
-	  PHY which is usually paired with either the ChipIdea or Synopsys DWC3
-	  USB IPs on MSM SOCs.
-
-config PHY_QCOM_UFS
-	tristate "Qualcomm UFS PHY driver"
-	depends on OF && ARCH_QCOM
-	select GENERIC_PHY
-	help
-	  Support for UFS PHY on QCOM chipsets.
-
-config PHY_QCOM_USB_HS
-	tristate "Qualcomm USB HS PHY module"
-	depends on USB_ULPI_BUS
-	depends on EXTCON || !EXTCON # if EXTCON=m, this cannot be built-in
-	select GENERIC_PHY
-	help
-	  Support for the USB high-speed ULPI compliant phy on Qualcomm
-	  chipsets.
-
-config PHY_QCOM_USB_HSIC
-	tristate "Qualcomm USB HSIC ULPI PHY module"
-	depends on USB_ULPI_BUS
-	select GENERIC_PHY
-	help
-	  Support for the USB HSIC ULPI compliant PHY on QCOM chipsets.
-
-config PHY_TUSB1210
-	tristate "TI TUSB1210 ULPI PHY module"
-	depends on USB_ULPI_BUS
-	select GENERIC_PHY
-	help
-	  Support for TI TUSB1210 USB ULPI PHY.
-
-config PHY_BRCM_SATA
-	tristate "Broadcom SATA PHY driver"
-	depends on ARCH_BRCMSTB || ARCH_BCM_IPROC || BMIPS_GENERIC || COMPILE_TEST
-	depends on OF
-	select GENERIC_PHY
-	default ARCH_BCM_IPROC
-	help
-	  Enable this to support the Broadcom SATA PHY.
-	  If unsure, say N.
-
-config PHY_CYGNUS_PCIE
-	tristate "Broadcom Cygnus PCIe PHY driver"
-	depends on OF && (ARCH_BCM_CYGNUS || COMPILE_TEST)
-	select GENERIC_PHY
-	default ARCH_BCM_CYGNUS
-	help
-	  Enable this to support the Broadcom Cygnus PCIe PHY.
-	  If unsure, say N.
-
+source "drivers/phy/allwinner/Kconfig"
+source "drivers/phy/amlogic/Kconfig"
+source "drivers/phy/broadcom/Kconfig"
+source "drivers/phy/hisilicon/Kconfig"
+source "drivers/phy/marvell/Kconfig"
+source "drivers/phy/motorola/Kconfig"
+source "drivers/phy/qualcomm/Kconfig"
+source "drivers/phy/renesas/Kconfig"
+source "drivers/phy/rockchip/Kconfig"
+source "drivers/phy/samsung/Kconfig"
+source "drivers/phy/st/Kconfig"
 source "drivers/phy/tegra/Kconfig"
-
-config PHY_NS2_PCIE
-	tristate "Broadcom Northstar2 PCIe PHY driver"
-	depends on OF && MDIO_BUS_MUX_BCM_IPROC
-	select GENERIC_PHY
-	default ARCH_BCM_IPROC
-	help
-	  Enable this to support the Broadcom Northstar2 PCIe PHY.
-	  If unsure, say N.
-
-config PHY_MESON8B_USB2
-	tristate "Meson8b and GXBB USB2 PHY driver"
-	default ARCH_MESON
-	depends on OF && (ARCH_MESON || COMPILE_TEST)
-	depends on USB_SUPPORT
-	select USB_COMMON
-	select GENERIC_PHY
-	help
-	  Enable this to support the Meson USB2 PHYs found in Meson8b
-	  and GXBB SoCs.
-	  If unsure, say N.
+source "drivers/phy/ti/Kconfig"
 
 endmenu
diff --git a/drivers/phy/Makefile b/drivers/phy/Makefile
index f8047b4639fa..f252201e0ec9 100644
--- a/drivers/phy/Makefile
+++ b/drivers/phy/Makefile
@@ -3,64 +3,21 @@
 #
 
 obj-$(CONFIG_GENERIC_PHY)		+= phy-core.o
-obj-$(CONFIG_PHY_BCM_NS_USB2)		+= phy-bcm-ns-usb2.o
-obj-$(CONFIG_PHY_BCM_NS_USB3)		+= phy-bcm-ns-usb3.o
-obj-$(CONFIG_PHY_BERLIN_USB)		+= phy-berlin-usb.o
-obj-$(CONFIG_PHY_BERLIN_SATA)		+= phy-berlin-sata.o
-obj-$(CONFIG_PHY_DA8XX_USB)		+= phy-da8xx-usb.o
-obj-$(CONFIG_PHY_DM816X_USB)		+= phy-dm816x-usb.o
-obj-$(CONFIG_ARMADA375_USBCLUSTER_PHY)	+= phy-armada375-usb2.o
-obj-$(CONFIG_BCM_KONA_USB2_PHY)		+= phy-bcm-kona-usb2.o
-obj-$(CONFIG_PHY_EXYNOS_DP_VIDEO)	+= phy-exynos-dp-video.o
-obj-$(CONFIG_PHY_EXYNOS_MIPI_VIDEO)	+= phy-exynos-mipi-video.o
 obj-$(CONFIG_PHY_LPC18XX_USB_OTG)	+= phy-lpc18xx-usb-otg.o
-obj-$(CONFIG_PHY_PXA_28NM_USB2)		+= phy-pxa-28nm-usb2.o
-obj-$(CONFIG_PHY_PXA_28NM_HSIC)		+= phy-pxa-28nm-hsic.o
-obj-$(CONFIG_PHY_MVEBU_SATA)		+= phy-mvebu-sata.o
-obj-$(CONFIG_PHY_MIPHY28LP) 		+= phy-miphy28lp.o
-obj-$(CONFIG_PHY_RCAR_GEN2)		+= phy-rcar-gen2.o
-obj-$(CONFIG_PHY_RCAR_GEN3_USB2)	+= phy-rcar-gen3-usb2.o
-obj-$(CONFIG_OMAP_CONTROL_PHY)		+= phy-omap-control.o
-obj-$(CONFIG_OMAP_USB2)			+= phy-omap-usb2.o
-obj-$(CONFIG_TI_PIPE3)			+= phy-ti-pipe3.o
-obj-$(CONFIG_TWL4030_USB)		+= phy-twl4030-usb.o
-obj-$(CONFIG_PHY_EXYNOS5250_SATA)	+= phy-exynos5250-sata.o
-obj-$(CONFIG_PHY_HIX5HD2_SATA)		+= phy-hix5hd2-sata.o
-obj-$(CONFIG_PHY_HI6220_USB)		+= phy-hi6220-usb.o
 obj-$(CONFIG_PHY_MT65XX_USB3)		+= phy-mt65xx-usb3.o
-obj-$(CONFIG_PHY_SUN4I_USB)		+= phy-sun4i-usb.o
-obj-$(CONFIG_PHY_SUN9I_USB)		+= phy-sun9i-usb.o
-obj-$(CONFIG_PHY_SAMSUNG_USB2)		+= phy-exynos-usb2.o
-phy-exynos-usb2-y			+= phy-samsung-usb2.o
-phy-exynos-usb2-$(CONFIG_PHY_EXYNOS4210_USB2)	+= phy-exynos4210-usb2.o
-phy-exynos-usb2-$(CONFIG_PHY_EXYNOS4X12_USB2)	+= phy-exynos4x12-usb2.o
-phy-exynos-usb2-$(CONFIG_PHY_EXYNOS5250_USB2)	+= phy-exynos5250-usb2.o
-phy-exynos-usb2-$(CONFIG_PHY_S5PV210_USB2)	+= phy-s5pv210-usb2.o
-obj-$(CONFIG_PHY_EXYNOS5_USBDRD)	+= phy-exynos5-usbdrd.o
-obj-$(CONFIG_PHY_EXYNOS_PCIE)	+= phy-exynos-pcie.o
-obj-$(CONFIG_PHY_QCOM_APQ8064_SATA)	+= phy-qcom-apq8064-sata.o
-obj-$(CONFIG_PHY_ROCKCHIP_USB) += phy-rockchip-usb.o
-obj-$(CONFIG_PHY_ROCKCHIP_INNO_USB2)	+= phy-rockchip-inno-usb2.o
-obj-$(CONFIG_PHY_ROCKCHIP_EMMC) += phy-rockchip-emmc.o
-obj-$(CONFIG_PHY_ROCKCHIP_PCIE) += phy-rockchip-pcie.o
-obj-$(CONFIG_PHY_ROCKCHIP_DP)		+= phy-rockchip-dp.o
-obj-$(CONFIG_PHY_ROCKCHIP_TYPEC) += phy-rockchip-typec.o
-obj-$(CONFIG_PHY_QCOM_IPQ806X_SATA)	+= phy-qcom-ipq806x-sata.o
-obj-$(CONFIG_PHY_ST_SPEAR1310_MIPHY)	+= phy-spear1310-miphy.o
-obj-$(CONFIG_PHY_ST_SPEAR1340_MIPHY)	+= phy-spear1340-miphy.o
 obj-$(CONFIG_PHY_XGENE)			+= phy-xgene.o
-obj-$(CONFIG_PHY_STIH407_USB)		+= phy-stih407-usb.o
-obj-$(CONFIG_PHY_QCOM_QMP)		+= phy-qcom-qmp.o
-obj-$(CONFIG_PHY_QCOM_QUSB2)		+= phy-qcom-qusb2.o
-obj-$(CONFIG_PHY_QCOM_UFS) 	+= phy-qcom-ufs.o
-obj-$(CONFIG_PHY_QCOM_UFS) 	+= phy-qcom-ufs-qmp-20nm.o
-obj-$(CONFIG_PHY_QCOM_UFS) 	+= phy-qcom-ufs-qmp-14nm.o
-obj-$(CONFIG_PHY_QCOM_USB_HS) 		+= phy-qcom-usb-hs.o
-obj-$(CONFIG_PHY_QCOM_USB_HSIC) 	+= phy-qcom-usb-hsic.o
-obj-$(CONFIG_PHY_TUSB1210)		+= phy-tusb1210.o
-obj-$(CONFIG_PHY_BRCM_SATA)		+= phy-brcm-sata.o
 obj-$(CONFIG_PHY_PISTACHIO_USB)		+= phy-pistachio-usb.o
-obj-$(CONFIG_PHY_CYGNUS_PCIE)		+= phy-bcm-cygnus-pcie.o
-obj-$(CONFIG_ARCH_TEGRA) += tegra/
-obj-$(CONFIG_PHY_NS2_PCIE)		+= phy-bcm-ns2-pcie.o
-obj-$(CONFIG_PHY_MESON8B_USB2)		+= phy-meson8b-usb2.o
+
+obj-$(CONFIG_ARCH_SUNXI)		+= allwinner/
+obj-$(CONFIG_ARCH_MESON)		+= amlogic/
+obj-$(CONFIG_ARCH_RENESAS)		+= renesas/
+obj-$(CONFIG_ARCH_ROCKCHIP)		+= rockchip/
+obj-$(CONFIG_ARCH_TEGRA)		+= tegra/
+obj-y					+= broadcom/	\
+					   hisilicon/	\
+					   marvell/	\
+					   motorola/	\
+					   qualcomm/	\
+					   samsung/	\
+					   st/		\
+					   ti/
diff --git a/drivers/phy/allwinner/Kconfig b/drivers/phy/allwinner/Kconfig
new file mode 100644
index 000000000000..cdc1e745ba47
--- /dev/null
+++ b/drivers/phy/allwinner/Kconfig
@@ -0,0 +1,31 @@
+#
+# Phy drivers for Allwinner platforms
+#
+config PHY_SUN4I_USB
+	tristate "Allwinner sunxi SoC USB PHY driver"
+	depends on ARCH_SUNXI && HAS_IOMEM && OF
+	depends on RESET_CONTROLLER
+	depends on EXTCON
+	depends on POWER_SUPPLY
+	depends on USB_SUPPORT
+	select GENERIC_PHY
+	select USB_COMMON
+	help
+	  Enable this to support the transceiver that is part of Allwinner
+	  sunxi SoCs.
+
+	  This driver controls the entire USB PHY block, both the USB OTG
+	  parts, as well as the 2 regular USB 2 host PHYs.
+
+config PHY_SUN9I_USB
+	tristate "Allwinner sun9i SoC USB PHY driver"
+	depends on ARCH_SUNXI && HAS_IOMEM && OF
+	depends on RESET_CONTROLLER
+	depends on USB_SUPPORT
+	select USB_COMMON
+	select GENERIC_PHY
+	help
+	  Enable this to support the transceiver that is part of Allwinner
+	  sun9i SoCs.
+
+	  This driver controls each individual USB 2 host PHY.
diff --git a/drivers/phy/allwinner/Makefile b/drivers/phy/allwinner/Makefile
new file mode 100644
index 000000000000..8605529c01a1
--- /dev/null
+++ b/drivers/phy/allwinner/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_PHY_SUN4I_USB)		+= phy-sun4i-usb.o
+obj-$(CONFIG_PHY_SUN9I_USB)		+= phy-sun9i-usb.o
diff --git a/drivers/phy/phy-sun4i-usb.c b/drivers/phy/allwinner/phy-sun4i-usb.c
index bbf06cfe5898..bbf06cfe5898 100644
--- a/drivers/phy/phy-sun4i-usb.c
+++ b/drivers/phy/allwinner/phy-sun4i-usb.c
diff --git a/drivers/phy/phy-sun9i-usb.c b/drivers/phy/allwinner/phy-sun9i-usb.c
index 28fce4bce638..28fce4bce638 100644
--- a/drivers/phy/phy-sun9i-usb.c
+++ b/drivers/phy/allwinner/phy-sun9i-usb.c
diff --git a/drivers/phy/amlogic/Kconfig b/drivers/phy/amlogic/Kconfig
new file mode 100644
index 000000000000..cb8f4501652b
--- /dev/null
+++ b/drivers/phy/amlogic/Kconfig
@@ -0,0 +1,27 @@
+#
+# Phy drivers for Amlogic platforms
+#
+config PHY_MESON8B_USB2
+	tristate "Meson8, Meson8b and GXBB USB2 PHY driver"
+	default ARCH_MESON
+	depends on OF && (ARCH_MESON || COMPILE_TEST)
+	depends on USB_SUPPORT
+	select USB_COMMON
+	select GENERIC_PHY
+	help
+	  Enable this to support the Meson USB2 PHYs found in Meson8,
+	  Meson8b and GXBB SoCs.
+	  If unsure, say N.
+
+config PHY_MESON_GXL_USB2
+	tristate "Meson GXL and GXM USB2 PHY drivers"
+	default ARCH_MESON
+	depends on OF && (ARCH_MESON || COMPILE_TEST)
+	depends on USB_SUPPORT
+	select USB_COMMON
+	select GENERIC_PHY
+	select REGMAP_MMIO
+	help
+	  Enable this to support the Meson USB2 PHYs found in Meson
+	  GXL and GXM SoCs.
+	  If unsure, say N.
diff --git a/drivers/phy/amlogic/Makefile b/drivers/phy/amlogic/Makefile
new file mode 100644
index 000000000000..cfdc98715c30
--- /dev/null
+++ b/drivers/phy/amlogic/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_PHY_MESON8B_USB2)		+= phy-meson8b-usb2.o
+obj-$(CONFIG_PHY_MESON_GXL_USB2)	+= phy-meson-gxl-usb2.o
diff --git a/drivers/phy/amlogic/phy-meson-gxl-usb2.c b/drivers/phy/amlogic/phy-meson-gxl-usb2.c
new file mode 100644
index 000000000000..e90c4ee25dfe
--- /dev/null
+++ b/drivers/phy/amlogic/phy-meson-gxl-usb2.c
@@ -0,0 +1,273 @@
+/*
+ * Meson GXL and GXM USB2 PHY driver
+ *
+ * Copyright (C) 2017 Martin Blumenstingl <martin.blumenstingl@googlemail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/regmap.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/usb/of.h>
+
+/* bits [31:27] are read-only */
+#define U2P_R0							0x0
+	#define U2P_R0_BYPASS_SEL				BIT(0)
+	#define U2P_R0_BYPASS_DM_EN				BIT(1)
+	#define U2P_R0_BYPASS_DP_EN				BIT(2)
+	#define U2P_R0_TXBITSTUFF_ENH				BIT(3)
+	#define U2P_R0_TXBITSTUFF_EN				BIT(4)
+	#define U2P_R0_DM_PULLDOWN				BIT(5)
+	#define U2P_R0_DP_PULLDOWN				BIT(6)
+	#define U2P_R0_DP_VBUS_VLD_EXT_SEL			BIT(7)
+	#define U2P_R0_DP_VBUS_VLD_EXT				BIT(8)
+	#define U2P_R0_ADP_PRB_EN				BIT(9)
+	#define U2P_R0_ADP_DISCHARGE				BIT(10)
+	#define U2P_R0_ADP_CHARGE				BIT(11)
+	#define U2P_R0_DRV_VBUS					BIT(12)
+	#define U2P_R0_ID_PULLUP				BIT(13)
+	#define U2P_R0_LOOPBACK_EN_B				BIT(14)
+	#define U2P_R0_OTG_DISABLE				BIT(15)
+	#define U2P_R0_COMMON_ONN				BIT(16)
+	#define U2P_R0_FSEL_MASK				GENMASK(19, 17)
+	#define U2P_R0_REF_CLK_SEL_MASK				GENMASK(21, 20)
+	#define U2P_R0_POWER_ON_RESET				BIT(22)
+	#define U2P_R0_V_ATE_TEST_EN_B_MASK			GENMASK(24, 23)
+	#define U2P_R0_ID_SET_ID_DQ				BIT(25)
+	#define U2P_R0_ATE_RESET				BIT(26)
+	#define U2P_R0_FSV_MINUS				BIT(27)
+	#define U2P_R0_FSV_PLUS					BIT(28)
+	#define U2P_R0_BYPASS_DM_DATA				BIT(29)
+	#define U2P_R0_BYPASS_DP_DATA				BIT(30)
+
+#define U2P_R1							0x4
+	#define U2P_R1_BURN_IN_TEST				BIT(0)
+	#define U2P_R1_ACA_ENABLE				BIT(1)
+	#define U2P_R1_DCD_ENABLE				BIT(2)
+	#define U2P_R1_VDAT_SRC_EN_B				BIT(3)
+	#define U2P_R1_VDAT_DET_EN_B				BIT(4)
+	#define U2P_R1_CHARGES_SEL				BIT(5)
+	#define U2P_R1_TX_PREEMP_PULSE_TUNE			BIT(6)
+	#define U2P_R1_TX_PREEMP_AMP_TUNE_MASK			GENMASK(8, 7)
+	#define U2P_R1_TX_RES_TUNE_MASK				GENMASK(10, 9)
+	#define U2P_R1_TX_RISE_TUNE_MASK			GENMASK(12, 11)
+	#define U2P_R1_TX_VREF_TUNE_MASK			GENMASK(16, 13)
+	#define U2P_R1_TX_FSLS_TUNE_MASK			GENMASK(20, 17)
+	#define U2P_R1_TX_HSXV_TUNE_MASK			GENMASK(22, 21)
+	#define U2P_R1_OTG_TUNE_MASK				GENMASK(25, 23)
+	#define U2P_R1_SQRX_TUNE_MASK				GENMASK(28, 26)
+	#define U2P_R1_COMP_DIS_TUNE_MASK			GENMASK(31, 29)
+
+/* bits [31:14] are read-only */
+#define U2P_R2							0x8
+	#define U2P_R2_DATA_IN_MASK				GENMASK(3, 0)
+	#define U2P_R2_DATA_IN_EN_MASK				GENMASK(7, 4)
+	#define U2P_R2_ADDR_MASK				GENMASK(11, 8)
+	#define U2P_R2_DATA_OUT_SEL				BIT(12)
+	#define U2P_R2_CLK					BIT(13)
+	#define U2P_R2_DATA_OUT_MASK				GENMASK(17, 14)
+	#define U2P_R2_ACA_PIN_RANGE_C				BIT(18)
+	#define U2P_R2_ACA_PIN_RANGE_B				BIT(19)
+	#define U2P_R2_ACA_PIN_RANGE_A				BIT(20)
+	#define U2P_R2_ACA_PIN_GND				BIT(21)
+	#define U2P_R2_ACA_PIN_FLOAT				BIT(22)
+	#define U2P_R2_CHARGE_DETECT				BIT(23)
+	#define U2P_R2_DEVICE_SESSION_VALID			BIT(24)
+	#define U2P_R2_ADP_PROBE				BIT(25)
+	#define U2P_R2_ADP_SENSE				BIT(26)
+	#define U2P_R2_SESSION_END				BIT(27)
+	#define U2P_R2_VBUS_VALID				BIT(28)
+	#define U2P_R2_B_VALID					BIT(29)
+	#define U2P_R2_A_VALID					BIT(30)
+	#define U2P_R2_ID_DIG					BIT(31)
+
+#define U2P_R3							0xc
+
+#define RESET_COMPLETE_TIME				500
+
+struct phy_meson_gxl_usb2_priv {
+	struct regmap		*regmap;
+	enum phy_mode		mode;
+	int			is_enabled;
+};
+
+static const struct regmap_config phy_meson_gxl_usb2_regmap_conf = {
+	.reg_bits = 8,
+	.val_bits = 32,
+	.reg_stride = 4,
+	.max_register = U2P_R3,
+};
+
+static int phy_meson_gxl_usb2_reset(struct phy *phy)
+{
+	struct phy_meson_gxl_usb2_priv *priv = phy_get_drvdata(phy);
+
+	if (priv->is_enabled) {
+		/* reset the PHY and wait until settings are stabilized */
+		regmap_update_bits(priv->regmap, U2P_R0, U2P_R0_POWER_ON_RESET,
+				   U2P_R0_POWER_ON_RESET);
+		udelay(RESET_COMPLETE_TIME);
+		regmap_update_bits(priv->regmap, U2P_R0, U2P_R0_POWER_ON_RESET,
+				   0);
+		udelay(RESET_COMPLETE_TIME);
+	}
+
+	return 0;
+}
+
+static int phy_meson_gxl_usb2_set_mode(struct phy *phy, enum phy_mode mode)
+{
+	struct phy_meson_gxl_usb2_priv *priv = phy_get_drvdata(phy);
+
+	switch (mode) {
+	case PHY_MODE_USB_HOST:
+	case PHY_MODE_USB_OTG:
+		regmap_update_bits(priv->regmap, U2P_R0, U2P_R0_DM_PULLDOWN,
+				   U2P_R0_DM_PULLDOWN);
+		regmap_update_bits(priv->regmap, U2P_R0, U2P_R0_DP_PULLDOWN,
+				   U2P_R0_DP_PULLDOWN);
+		regmap_update_bits(priv->regmap, U2P_R0, U2P_R0_ID_PULLUP, 0);
+		break;
+
+	case PHY_MODE_USB_DEVICE:
+		regmap_update_bits(priv->regmap, U2P_R0, U2P_R0_DM_PULLDOWN,
+				   0);
+		regmap_update_bits(priv->regmap, U2P_R0, U2P_R0_DP_PULLDOWN,
+				   0);
+		regmap_update_bits(priv->regmap, U2P_R0, U2P_R0_ID_PULLUP,
+				   U2P_R0_ID_PULLUP);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	phy_meson_gxl_usb2_reset(phy);
+
+	priv->mode = mode;
+
+	return 0;
+}
+
+static int phy_meson_gxl_usb2_power_off(struct phy *phy)
+{
+	struct phy_meson_gxl_usb2_priv *priv = phy_get_drvdata(phy);
+
+	priv->is_enabled = 0;
+
+	/* power off the PHY by putting it into reset mode */
+	regmap_update_bits(priv->regmap, U2P_R0, U2P_R0_POWER_ON_RESET,
+			   U2P_R0_POWER_ON_RESET);
+
+	return 0;
+}
+
+static int phy_meson_gxl_usb2_power_on(struct phy *phy)
+{
+	struct phy_meson_gxl_usb2_priv *priv = phy_get_drvdata(phy);
+	int ret;
+
+	priv->is_enabled = 1;
+
+	/* power on the PHY by taking it out of reset mode */
+	regmap_update_bits(priv->regmap, U2P_R0, U2P_R0_POWER_ON_RESET, 0);
+
+	ret = phy_meson_gxl_usb2_set_mode(phy, priv->mode);
+	if (ret) {
+		phy_meson_gxl_usb2_power_off(phy);
+
+		dev_err(&phy->dev, "Failed to initialize PHY with mode %d\n",
+			priv->mode);
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct phy_ops phy_meson_gxl_usb2_ops = {
+	.power_on	= phy_meson_gxl_usb2_power_on,
+	.power_off	= phy_meson_gxl_usb2_power_off,
+	.set_mode	= phy_meson_gxl_usb2_set_mode,
+	.reset		= phy_meson_gxl_usb2_reset,
+	.owner		= THIS_MODULE,
+};
+
+static int phy_meson_gxl_usb2_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct phy_provider *phy_provider;
+	struct resource *res;
+	struct phy_meson_gxl_usb2_priv *priv;
+	struct phy *phy;
+	void __iomem *base;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, priv);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	switch (of_usb_get_dr_mode_by_phy(dev->of_node, -1)) {
+	case USB_DR_MODE_PERIPHERAL:
+		priv->mode = PHY_MODE_USB_DEVICE;
+		break;
+	case USB_DR_MODE_OTG:
+		priv->mode = PHY_MODE_USB_OTG;
+		break;
+	case USB_DR_MODE_HOST:
+	default:
+		priv->mode = PHY_MODE_USB_HOST;
+		break;
+	}
+
+	priv->regmap = devm_regmap_init_mmio(dev, base,
+					     &phy_meson_gxl_usb2_regmap_conf);
+	if (IS_ERR(priv->regmap))
+		return PTR_ERR(priv->regmap);
+
+	phy = devm_phy_create(dev, NULL, &phy_meson_gxl_usb2_ops);
+	if (IS_ERR(phy)) {
+		dev_err(dev, "failed to create PHY\n");
+		return PTR_ERR(phy);
+	}
+
+	phy_set_drvdata(phy, priv);
+
+	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+
+	return PTR_ERR_OR_ZERO(phy_provider);
+}
+
+static const struct of_device_id phy_meson_gxl_usb2_of_match[] = {
+	{ .compatible = "amlogic,meson-gxl-usb2-phy", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, phy_meson_gxl_usb2_of_match);
+
+static struct platform_driver phy_meson_gxl_usb2_driver = {
+	.probe	= phy_meson_gxl_usb2_probe,
+	.driver	= {
+		.name		= "phy-meson-gxl-usb2",
+		.of_match_table	= phy_meson_gxl_usb2_of_match,
+	},
+};
+module_platform_driver(phy_meson_gxl_usb2_driver);
+
+MODULE_AUTHOR("Martin Blumenstingl <martin.blumenstingl@googlemail.com>");
+MODULE_DESCRIPTION("Meson GXL and GXM USB2 PHY driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-meson8b-usb2.c b/drivers/phy/amlogic/phy-meson8b-usb2.c
index 30f56a6a411f..9c01b7e19b06 100644
--- a/drivers/phy/phy-meson8b-usb2.c
+++ b/drivers/phy/amlogic/phy-meson8b-usb2.c
@@ -1,5 +1,5 @@
 /*
- * Meson8b and GXBB USB2 PHY driver
+ * Meson8, Meson8b and GXBB USB2 PHY driver
  *
  * Copyright (C) 2016 Martin Blumenstingl <martin.blumenstingl@googlemail.com>
  *
@@ -266,6 +266,7 @@ static int phy_meson8b_usb2_probe(struct platform_device *pdev)
 }
 
 static const struct of_device_id phy_meson8b_usb2_of_match[] = {
+	{ .compatible = "amlogic,meson8-usb2-phy", },
 	{ .compatible = "amlogic,meson8b-usb2-phy", },
 	{ .compatible = "amlogic,meson-gxbb-usb2-phy", },
 	{ },
@@ -282,5 +283,5 @@ static struct platform_driver phy_meson8b_usb2_driver = {
 module_platform_driver(phy_meson8b_usb2_driver);
 
 MODULE_AUTHOR("Martin Blumenstingl <martin.blumenstingl@googlemail.com>");
-MODULE_DESCRIPTION("Meson8b and GXBB USB2 PHY driver");
+MODULE_DESCRIPTION("Meson8, Meson8b and GXBB USB2 PHY driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/phy/broadcom/Kconfig b/drivers/phy/broadcom/Kconfig
new file mode 100644
index 000000000000..37371b89b14f
--- /dev/null
+++ b/drivers/phy/broadcom/Kconfig
@@ -0,0 +1,69 @@
+#
+# Phy drivers for Broadcom platforms
+#
+config PHY_CYGNUS_PCIE
+	tristate "Broadcom Cygnus PCIe PHY driver"
+	depends on OF && (ARCH_BCM_CYGNUS || COMPILE_TEST)
+	select GENERIC_PHY
+	default ARCH_BCM_CYGNUS
+	help
+	  Enable this to support the Broadcom Cygnus PCIe PHY.
+	  If unsure, say N.
+
+config BCM_KONA_USB2_PHY
+	tristate "Broadcom Kona USB2 PHY Driver"
+	depends on HAS_IOMEM
+	select GENERIC_PHY
+	help
+	  Enable this to support the Broadcom Kona USB 2.0 PHY.
+
+config PHY_BCM_NS_USB2
+	tristate "Broadcom Northstar USB 2.0 PHY Driver"
+	depends on ARCH_BCM_IPROC || COMPILE_TEST
+	depends on HAS_IOMEM && OF
+	select GENERIC_PHY
+	help
+	  Enable this to support Broadcom USB 2.0 PHY connected to the USB
+	  controller on Northstar family.
+
+config PHY_BCM_NS_USB3
+	tristate "Broadcom Northstar USB 3.0 PHY Driver"
+	depends on ARCH_BCM_IPROC || COMPILE_TEST
+	depends on HAS_IOMEM && OF
+	select GENERIC_PHY
+	select MDIO_DEVICE
+	help
+	  Enable this to support Broadcom USB 3.0 PHY connected to the USB
+	  controller on Northstar family.
+
+config PHY_NS2_PCIE
+	tristate "Broadcom Northstar2 PCIe PHY driver"
+	depends on OF && MDIO_BUS_MUX_BCM_IPROC
+	select GENERIC_PHY
+	default ARCH_BCM_IPROC
+	help
+	  Enable this to support the Broadcom Northstar2 PCIe PHY.
+	  If unsure, say N.
+
+config PHY_NS2_USB_DRD
+	tristate "Broadcom Northstar2 USB DRD PHY support"
+	depends on OF && (ARCH_BCM_IPROC || COMPILE_TEST)
+	select GENERIC_PHY
+	select EXTCON
+	default ARCH_BCM_IPROC
+	help
+	  Enable this to support the Broadcom Northstar2 USB DRD PHY.
+	  This driver initializes the PHY in either HOST or DEVICE mode.
+	  The host or device configuration is read from device tree.
+
+	  If unsure, say N.
+
+config PHY_BRCM_SATA
+	tristate "Broadcom SATA PHY driver"
+	depends on ARCH_BRCMSTB || ARCH_BCM_IPROC || BMIPS_GENERIC || COMPILE_TEST
+	depends on OF
+	select GENERIC_PHY
+	default ARCH_BCM_IPROC
+	help
+	  Enable this to support the Broadcom SATA PHY.
+	  If unsure, say N.
diff --git a/drivers/phy/broadcom/Makefile b/drivers/phy/broadcom/Makefile
new file mode 100644
index 000000000000..4eb82ec8d491
--- /dev/null
+++ b/drivers/phy/broadcom/Makefile
@@ -0,0 +1,7 @@
+obj-$(CONFIG_PHY_CYGNUS_PCIE)		+= phy-bcm-cygnus-pcie.o
+obj-$(CONFIG_BCM_KONA_USB2_PHY)		+= phy-bcm-kona-usb2.o
+obj-$(CONFIG_PHY_BCM_NS_USB2)		+= phy-bcm-ns-usb2.o
+obj-$(CONFIG_PHY_BCM_NS_USB3)		+= phy-bcm-ns-usb3.o
+obj-$(CONFIG_PHY_NS2_PCIE)		+= phy-bcm-ns2-pcie.o
+obj-$(CONFIG_PHY_NS2_USB_DRD)		+= phy-bcm-ns2-usbdrd.o
+obj-$(CONFIG_PHY_BRCM_SATA)		+= phy-brcm-sata.o
diff --git a/drivers/phy/phy-bcm-cygnus-pcie.c b/drivers/phy/broadcom/phy-bcm-cygnus-pcie.c
index 0f4ac5d63cff..0f4ac5d63cff 100644
--- a/drivers/phy/phy-bcm-cygnus-pcie.c
+++ b/drivers/phy/broadcom/phy-bcm-cygnus-pcie.c
diff --git a/drivers/phy/phy-bcm-kona-usb2.c b/drivers/phy/broadcom/phy-bcm-kona-usb2.c
index 7b67fe49e30b..7b67fe49e30b 100644
--- a/drivers/phy/phy-bcm-kona-usb2.c
+++ b/drivers/phy/broadcom/phy-bcm-kona-usb2.c
diff --git a/drivers/phy/phy-bcm-ns-usb2.c b/drivers/phy/broadcom/phy-bcm-ns-usb2.c
index 58dff80e9386..58dff80e9386 100644
--- a/drivers/phy/phy-bcm-ns-usb2.c
+++ b/drivers/phy/broadcom/phy-bcm-ns-usb2.c
diff --git a/drivers/phy/phy-bcm-ns-usb3.c b/drivers/phy/broadcom/phy-bcm-ns-usb3.c
index 22b5e7047fa6..a53ae128eadf 100644
--- a/drivers/phy/phy-bcm-ns-usb3.c
+++ b/drivers/phy/broadcom/phy-bcm-ns-usb3.c
@@ -16,7 +16,9 @@
 #include <linux/bcma/bcma.h>
 #include <linux/delay.h>
 #include <linux/err.h>
+#include <linux/mdio.h>
 #include <linux/module.h>
+#include <linux/of_address.h>
 #include <linux/of_platform.h>
 #include <linux/platform_device.h>
 #include <linux/phy/phy.h>
@@ -52,7 +54,10 @@ struct bcm_ns_usb3 {
 	enum bcm_ns_family family;
 	void __iomem *dmp;
 	void __iomem *ccb_mii;
+	struct mdio_device *mdiodev;
 	struct phy *phy;
+
+	int (*phy_write)(struct bcm_ns_usb3 *usb3, u16 reg, u16 value);
 };
 
 static const struct of_device_id bcm_ns_usb3_id_table[] = {
@@ -68,63 +73,16 @@ static const struct of_device_id bcm_ns_usb3_id_table[] = {
 };
 MODULE_DEVICE_TABLE(of, bcm_ns_usb3_id_table);
 
-static int bcm_ns_usb3_wait_reg(struct bcm_ns_usb3 *usb3, void __iomem *addr,
-				u32 mask, u32 value, unsigned long timeout)
-{
-	unsigned long deadline = jiffies + timeout;
-	u32 val;
-
-	do {
-		val = readl(addr);
-		if ((val & mask) == value)
-			return 0;
-		cpu_relax();
-		udelay(10);
-	} while (!time_after_eq(jiffies, deadline));
-
-	dev_err(usb3->dev, "Timeout waiting for register %p\n", addr);
-
-	return -EBUSY;
-}
-
-static inline int bcm_ns_usb3_mii_mng_wait_idle(struct bcm_ns_usb3 *usb3)
-{
-	return bcm_ns_usb3_wait_reg(usb3, usb3->ccb_mii + BCMA_CCB_MII_MNG_CTL,
-				    0x0100, 0x0000,
-				    usecs_to_jiffies(BCM_NS_USB3_MII_MNG_TIMEOUT_US));
-}
-
 static int bcm_ns_usb3_mdio_phy_write(struct bcm_ns_usb3 *usb3, u16 reg,
 				      u16 value)
 {
-	u32 tmp = 0;
-	int err;
-
-	err = bcm_ns_usb3_mii_mng_wait_idle(usb3);
-	if (err < 0) {
-		dev_err(usb3->dev, "Couldn't write 0x%08x value\n", value);
-		return err;
-	}
-
-	/* TODO: Use a proper MDIO bus layer */
-	tmp |= 0x58020000; /* Magic value for MDIO PHY write */
-	tmp |= reg << 18;
-	tmp |= value;
-	writel(tmp, usb3->ccb_mii + BCMA_CCB_MII_MNG_CMD_DATA);
-
-	return 0;
+	return usb3->phy_write(usb3, reg, value);
 }
 
 static int bcm_ns_usb3_phy_init_ns_bx(struct bcm_ns_usb3 *usb3)
 {
 	int err;
 
-	/* Enable MDIO. Setting MDCDIV as 26  */
-	writel(0x0000009a, usb3->ccb_mii + BCMA_CCB_MII_MNG_CTL);
-
-	/* Wait for MDIO? */
-	udelay(2);
-
 	/* USB3 PLL Block */
 	err = bcm_ns_usb3_mdio_phy_write(usb3, BCM_NS_USB3_PHY_BASE_ADDR_REG,
 					 BCM_NS_USB3_PHY_PLL30_BLOCK);
@@ -143,9 +101,6 @@ static int bcm_ns_usb3_phy_init_ns_bx(struct bcm_ns_usb3 *usb3)
 	/* Deaaserting PLL Reset */
 	bcm_ns_usb3_mdio_phy_write(usb3, BCM_NS_USB3_PLLA_CONTROL1, 0x8000);
 
-	/* Waiting MII Mgt interface idle */
-	bcm_ns_usb3_mii_mng_wait_idle(usb3);
-
 	/* Deasserting USB3 system reset */
 	writel(0, usb3->dmp + BCMA_RESET_CTL);
 
@@ -169,9 +124,6 @@ static int bcm_ns_usb3_phy_init_ns_bx(struct bcm_ns_usb3 *usb3)
 	/* Enabling SSC */
 	bcm_ns_usb3_mdio_phy_write(usb3, BCM_NS_USB3_TX_PMD_CONTROL1, 0x1003);
 
-	/* Waiting MII Mgt interface idle */
-	bcm_ns_usb3_mii_mng_wait_idle(usb3);
-
 	return 0;
 }
 
@@ -179,12 +131,6 @@ static int bcm_ns_usb3_phy_init_ns_ax(struct bcm_ns_usb3 *usb3)
 {
 	int err;
 
-	/* Enable MDIO. Setting MDCDIV as 26  */
-	writel(0x0000009a, usb3->ccb_mii + BCMA_CCB_MII_MNG_CTL);
-
-	/* Wait for MDIO? */
-	udelay(2);
-
 	/* PLL30 block */
 	err = bcm_ns_usb3_mdio_phy_write(usb3, BCM_NS_USB3_PHY_BASE_ADDR_REG,
 					 BCM_NS_USB3_PHY_PLL30_BLOCK);
@@ -205,9 +151,6 @@ static int bcm_ns_usb3_phy_init_ns_ax(struct bcm_ns_usb3 *usb3)
 
 	bcm_ns_usb3_mdio_phy_write(usb3, BCM_NS_USB3_TX_PMD_CONTROL1, 0x1003);
 
-	/* Waiting MII Mgt interface idle */
-	bcm_ns_usb3_mii_mng_wait_idle(usb3);
-
 	/* Deasserting USB3 system reset */
 	writel(0, usb3->dmp + BCMA_RESET_CTL);
 
@@ -242,6 +185,128 @@ static const struct phy_ops ops = {
 	.owner		= THIS_MODULE,
 };
 
+/**************************************************
+ * MDIO driver code
+ **************************************************/
+
+static int bcm_ns_usb3_mdiodev_phy_write(struct bcm_ns_usb3 *usb3, u16 reg,
+					 u16 value)
+{
+	struct mdio_device *mdiodev = usb3->mdiodev;
+
+	return mdiobus_write(mdiodev->bus, mdiodev->addr, reg, value);
+}
+
+static int bcm_ns_usb3_mdio_probe(struct mdio_device *mdiodev)
+{
+	struct device *dev = &mdiodev->dev;
+	const struct of_device_id *of_id;
+	struct phy_provider *phy_provider;
+	struct device_node *syscon_np;
+	struct bcm_ns_usb3 *usb3;
+	struct resource res;
+	int err;
+
+	usb3 = devm_kzalloc(dev, sizeof(*usb3), GFP_KERNEL);
+	if (!usb3)
+		return -ENOMEM;
+
+	usb3->dev = dev;
+	usb3->mdiodev = mdiodev;
+
+	of_id = of_match_device(bcm_ns_usb3_id_table, dev);
+	if (!of_id)
+		return -EINVAL;
+	usb3->family = (enum bcm_ns_family)of_id->data;
+
+	syscon_np = of_parse_phandle(dev->of_node, "usb3-dmp-syscon", 0);
+	err = of_address_to_resource(syscon_np, 0, &res);
+	of_node_put(syscon_np);
+	if (err)
+		return err;
+
+	usb3->dmp = devm_ioremap_resource(dev, &res);
+	if (IS_ERR(usb3->dmp)) {
+		dev_err(dev, "Failed to map DMP regs\n");
+		return PTR_ERR(usb3->dmp);
+	}
+
+	usb3->phy_write = bcm_ns_usb3_mdiodev_phy_write;
+
+	usb3->phy = devm_phy_create(dev, NULL, &ops);
+	if (IS_ERR(usb3->phy)) {
+		dev_err(dev, "Failed to create PHY\n");
+		return PTR_ERR(usb3->phy);
+	}
+
+	phy_set_drvdata(usb3->phy, usb3);
+
+	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+
+	return PTR_ERR_OR_ZERO(phy_provider);
+}
+
+static struct mdio_driver bcm_ns_usb3_mdio_driver = {
+	.mdiodrv = {
+		.driver = {
+			.name = "bcm_ns_mdio_usb3",
+			.of_match_table = bcm_ns_usb3_id_table,
+		},
+	},
+	.probe = bcm_ns_usb3_mdio_probe,
+};
+
+/**************************************************
+ * Platform driver code
+ **************************************************/
+
+static int bcm_ns_usb3_wait_reg(struct bcm_ns_usb3 *usb3, void __iomem *addr,
+				u32 mask, u32 value, unsigned long timeout)
+{
+	unsigned long deadline = jiffies + timeout;
+	u32 val;
+
+	do {
+		val = readl(addr);
+		if ((val & mask) == value)
+			return 0;
+		cpu_relax();
+		udelay(10);
+	} while (!time_after_eq(jiffies, deadline));
+
+	dev_err(usb3->dev, "Timeout waiting for register %p\n", addr);
+
+	return -EBUSY;
+}
+
+static inline int bcm_ns_usb3_mii_mng_wait_idle(struct bcm_ns_usb3 *usb3)
+{
+	return bcm_ns_usb3_wait_reg(usb3, usb3->ccb_mii + BCMA_CCB_MII_MNG_CTL,
+				    0x0100, 0x0000,
+				    usecs_to_jiffies(BCM_NS_USB3_MII_MNG_TIMEOUT_US));
+}
+
+static int bcm_ns_usb3_platform_phy_write(struct bcm_ns_usb3 *usb3, u16 reg,
+					  u16 value)
+{
+	u32 tmp = 0;
+	int err;
+
+	err = bcm_ns_usb3_mii_mng_wait_idle(usb3);
+	if (err < 0) {
+		dev_err(usb3->dev, "Couldn't write 0x%08x value\n", value);
+		return err;
+	}
+
+	/* TODO: Use a proper MDIO bus layer */
+	tmp |= 0x58020000; /* Magic value for MDIO PHY write */
+	tmp |= reg << 18;
+	tmp |= value;
+	writel(tmp, usb3->ccb_mii + BCMA_CCB_MII_MNG_CMD_DATA);
+
+	return bcm_ns_usb3_mii_mng_wait_idle(usb3);
+}
+
 static int bcm_ns_usb3_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -275,6 +340,14 @@ static int bcm_ns_usb3_probe(struct platform_device *pdev)
 		return PTR_ERR(usb3->ccb_mii);
 	}
 
+	/* Enable MDIO. Setting MDCDIV as 26  */
+	writel(0x0000009a, usb3->ccb_mii + BCMA_CCB_MII_MNG_CTL);
+
+	/* Wait for MDIO? */
+	udelay(2);
+
+	usb3->phy_write = bcm_ns_usb3_platform_phy_write;
+
 	usb3->phy = devm_phy_create(dev, NULL, &ops);
 	if (IS_ERR(usb3->phy)) {
 		dev_err(dev, "Failed to create PHY\n");
@@ -298,6 +371,35 @@ static struct platform_driver bcm_ns_usb3_driver = {
 		.of_match_table = bcm_ns_usb3_id_table,
 	},
 };
-module_platform_driver(bcm_ns_usb3_driver);
+
+static int __init bcm_ns_usb3_module_init(void)
+{
+	int err;
+
+	/*
+	 * For backward compatibility we register as MDIO and platform driver.
+	 * After getting MDIO binding commonly used (e.g. switching all DT files
+	 * to use it) we should deprecate the old binding and eventually drop
+	 * support for it.
+	 */
+
+	err = mdio_driver_register(&bcm_ns_usb3_mdio_driver);
+	if (err)
+		return err;
+
+	err = platform_driver_register(&bcm_ns_usb3_driver);
+	if (err)
+		mdio_driver_unregister(&bcm_ns_usb3_mdio_driver);
+
+	return err;
+}
+module_init(bcm_ns_usb3_module_init);
+
+static void __exit bcm_ns_usb3_module_exit(void)
+{
+	platform_driver_unregister(&bcm_ns_usb3_driver);
+	mdio_driver_unregister(&bcm_ns_usb3_mdio_driver);
+}
+module_exit(bcm_ns_usb3_module_exit)
 
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-bcm-ns2-pcie.c b/drivers/phy/broadcom/phy-bcm-ns2-pcie.c
index 4c7d11d2b378..4c7d11d2b378 100644
--- a/drivers/phy/phy-bcm-ns2-pcie.c
+++ b/drivers/phy/broadcom/phy-bcm-ns2-pcie.c
diff --git a/drivers/phy/broadcom/phy-bcm-ns2-usbdrd.c b/drivers/phy/broadcom/phy-bcm-ns2-usbdrd.c
new file mode 100644
index 000000000000..9ae59e223131
--- /dev/null
+++ b/drivers/phy/broadcom/phy-bcm-ns2-usbdrd.c
@@ -0,0 +1,437 @@
+/*
+ * Copyright (C) 2017 Broadcom
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation 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/delay.h>
+#include <linux/extcon.h>
+#include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+
+#define ICFG_DRD_AFE		0x0
+#define ICFG_MISC_STAT		0x18
+#define ICFG_DRD_P0CTL		0x1C
+#define ICFG_STRAP_CTRL		0x20
+#define ICFG_FSM_CTRL		0x24
+
+#define ICFG_DEV_BIT		BIT(2)
+#define IDM_RST_BIT		BIT(0)
+#define AFE_CORERDY_VDDC	BIT(18)
+#define PHY_PLL_RESETB		BIT(15)
+#define PHY_RESETB		BIT(14)
+#define PHY_PLL_LOCK		BIT(0)
+
+#define DRD_DEV_MODE		BIT(20)
+#define OHCI_OVRCUR_POL		BIT(11)
+#define ICFG_OFF_MODE		BIT(6)
+#define PLL_LOCK_RETRY		1000
+
+#define EVT_DEVICE		0
+#define EVT_HOST		1
+
+#define DRD_HOST_MODE		(BIT(2) | BIT(3))
+#define DRD_DEVICE_MODE		(BIT(4) | BIT(5))
+#define DRD_HOST_VAL		0x803
+#define DRD_DEV_VAL		0x807
+#define GPIO_DELAY		20
+
+struct ns2_phy_data;
+struct ns2_phy_driver {
+	void __iomem *icfgdrd_regs;
+	void __iomem *idmdrd_rst_ctrl;
+	void __iomem *crmu_usb2_ctrl;
+	void __iomem *usb2h_strap_reg;
+	struct ns2_phy_data *data;
+	struct extcon_dev *edev;
+	struct gpio_desc *vbus_gpiod;
+	struct gpio_desc *id_gpiod;
+	int id_irq;
+	int vbus_irq;
+	unsigned long debounce_jiffies;
+	struct delayed_work wq_extcon;
+};
+
+struct ns2_phy_data {
+	struct ns2_phy_driver *driver;
+	struct phy *phy;
+	int new_state;
+};
+
+static const unsigned int usb_extcon_cable[] = {
+	EXTCON_USB,
+	EXTCON_USB_HOST,
+	EXTCON_NONE,
+};
+
+static inline int pll_lock_stat(u32 usb_reg, int reg_mask,
+				struct ns2_phy_driver *driver)
+{
+	int retry = PLL_LOCK_RETRY;
+	u32 val;
+
+	do {
+		udelay(1);
+		val = readl(driver->icfgdrd_regs + usb_reg);
+		if (val & reg_mask)
+			return 0;
+	} while (--retry > 0);
+
+	return -EBUSY;
+}
+
+static int ns2_drd_phy_init(struct phy *phy)
+{
+	struct ns2_phy_data *data = phy_get_drvdata(phy);
+	struct ns2_phy_driver *driver = data->driver;
+	u32 val;
+
+	val = readl(driver->icfgdrd_regs + ICFG_FSM_CTRL);
+
+	if (data->new_state == EVT_HOST) {
+		val &= ~DRD_DEVICE_MODE;
+		val |= DRD_HOST_MODE;
+	} else {
+		val &= ~DRD_HOST_MODE;
+		val |= DRD_DEVICE_MODE;
+	}
+	writel(val, driver->icfgdrd_regs + ICFG_FSM_CTRL);
+
+	return 0;
+}
+
+static int ns2_drd_phy_poweroff(struct phy *phy)
+{
+	struct ns2_phy_data *data = phy_get_drvdata(phy);
+	struct ns2_phy_driver *driver = data->driver;
+	u32 val;
+
+	val = readl(driver->crmu_usb2_ctrl);
+	val &= ~AFE_CORERDY_VDDC;
+	writel(val, driver->crmu_usb2_ctrl);
+
+	val = readl(driver->crmu_usb2_ctrl);
+	val &= ~DRD_DEV_MODE;
+	writel(val, driver->crmu_usb2_ctrl);
+
+	/* Disable Host and Device Mode */
+	val = readl(driver->icfgdrd_regs + ICFG_FSM_CTRL);
+	val &= ~(DRD_HOST_MODE | DRD_DEVICE_MODE | ICFG_OFF_MODE);
+	writel(val, driver->icfgdrd_regs + ICFG_FSM_CTRL);
+
+	return 0;
+}
+
+static int ns2_drd_phy_poweron(struct phy *phy)
+{
+	struct ns2_phy_data *data = phy_get_drvdata(phy);
+	struct ns2_phy_driver *driver = data->driver;
+	u32 extcon_event = data->new_state;
+	int ret;
+	u32 val;
+
+	if (extcon_event == EVT_DEVICE) {
+		writel(DRD_DEV_VAL, driver->icfgdrd_regs + ICFG_DRD_P0CTL);
+
+		val = readl(driver->idmdrd_rst_ctrl);
+		val &= ~IDM_RST_BIT;
+		writel(val, driver->idmdrd_rst_ctrl);
+
+		val = readl(driver->crmu_usb2_ctrl);
+		val |= (AFE_CORERDY_VDDC | DRD_DEV_MODE);
+		writel(val, driver->crmu_usb2_ctrl);
+
+		/* Bring PHY and PHY_PLL out of Reset */
+		val = readl(driver->crmu_usb2_ctrl);
+		val |= (PHY_PLL_RESETB | PHY_RESETB);
+		writel(val, driver->crmu_usb2_ctrl);
+
+		ret = pll_lock_stat(ICFG_MISC_STAT, PHY_PLL_LOCK, driver);
+		if (ret < 0) {
+			dev_err(&phy->dev, "Phy PLL lock failed\n");
+			return ret;
+		}
+	} else {
+		writel(DRD_HOST_VAL, driver->icfgdrd_regs + ICFG_DRD_P0CTL);
+
+		val = readl(driver->crmu_usb2_ctrl);
+		val |= AFE_CORERDY_VDDC;
+		writel(val, driver->crmu_usb2_ctrl);
+
+		ret = pll_lock_stat(ICFG_MISC_STAT, PHY_PLL_LOCK, driver);
+		if (ret < 0) {
+			dev_err(&phy->dev, "Phy PLL lock failed\n");
+			return ret;
+		}
+
+		val = readl(driver->idmdrd_rst_ctrl);
+		val &= ~IDM_RST_BIT;
+		writel(val, driver->idmdrd_rst_ctrl);
+
+		/* port over current Polarity */
+		val = readl(driver->usb2h_strap_reg);
+		val |= OHCI_OVRCUR_POL;
+		writel(val, driver->usb2h_strap_reg);
+	}
+
+	return 0;
+}
+
+static void connect_change(struct ns2_phy_driver *driver)
+{
+	u32 extcon_event;
+	u32 val;
+
+	extcon_event = driver->data->new_state;
+	val = readl(driver->icfgdrd_regs + ICFG_FSM_CTRL);
+
+	switch (extcon_event) {
+	case EVT_DEVICE:
+		val &= ~(DRD_HOST_MODE | DRD_DEVICE_MODE);
+		writel(val, driver->icfgdrd_regs + ICFG_FSM_CTRL);
+
+		val = (val & ~DRD_HOST_MODE) | DRD_DEVICE_MODE;
+		writel(val, driver->icfgdrd_regs + ICFG_FSM_CTRL);
+
+		val = readl(driver->icfgdrd_regs + ICFG_DRD_P0CTL);
+		val |= ICFG_DEV_BIT;
+		writel(val, driver->icfgdrd_regs + ICFG_DRD_P0CTL);
+		break;
+
+	case EVT_HOST:
+		val &= ~(DRD_HOST_MODE | DRD_DEVICE_MODE);
+		writel(val, driver->icfgdrd_regs + ICFG_FSM_CTRL);
+
+		val = (val & ~DRD_DEVICE_MODE) | DRD_HOST_MODE;
+		writel(val, driver->icfgdrd_regs + ICFG_FSM_CTRL);
+
+		val = readl(driver->usb2h_strap_reg);
+		val |= OHCI_OVRCUR_POL;
+		writel(val, driver->usb2h_strap_reg);
+
+		val = readl(driver->icfgdrd_regs + ICFG_DRD_P0CTL);
+		val &= ~ICFG_DEV_BIT;
+		writel(val, driver->icfgdrd_regs + ICFG_DRD_P0CTL);
+		break;
+
+	default:
+		pr_err("Invalid extcon event\n");
+		break;
+	}
+}
+
+static void extcon_work(struct work_struct *work)
+{
+	struct ns2_phy_driver *driver;
+	int vbus;
+	int id;
+
+	driver  = container_of(to_delayed_work(work),
+			       struct ns2_phy_driver, wq_extcon);
+
+	id = gpiod_get_value_cansleep(driver->id_gpiod);
+	vbus = gpiod_get_value_cansleep(driver->vbus_gpiod);
+
+	if (!id && vbus) { /* Host connected */
+		extcon_set_cable_state_(driver->edev, EXTCON_USB_HOST, true);
+		pr_debug("Host cable connected\n");
+		driver->data->new_state = EVT_HOST;
+		connect_change(driver);
+	} else if (id && !vbus) { /* Disconnected */
+		extcon_set_cable_state_(driver->edev, EXTCON_USB_HOST, false);
+		extcon_set_cable_state_(driver->edev, EXTCON_USB, false);
+		pr_debug("Cable disconnected\n");
+	} else if (id && vbus) { /* Device connected */
+		extcon_set_cable_state_(driver->edev, EXTCON_USB, true);
+		pr_debug("Device cable connected\n");
+		driver->data->new_state = EVT_DEVICE;
+		connect_change(driver);
+	}
+}
+
+static irqreturn_t gpio_irq_handler(int irq, void *dev_id)
+{
+	struct ns2_phy_driver *driver = dev_id;
+
+	queue_delayed_work(system_power_efficient_wq, &driver->wq_extcon,
+			   driver->debounce_jiffies);
+
+	return IRQ_HANDLED;
+}
+
+static struct phy_ops ops = {
+	.init		= ns2_drd_phy_init,
+	.power_on	= ns2_drd_phy_poweron,
+	.power_off	= ns2_drd_phy_poweroff,
+	.owner		= THIS_MODULE,
+};
+
+static const struct of_device_id ns2_drd_phy_dt_ids[] = {
+	{ .compatible = "brcm,ns2-drd-phy", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ns2_drd_phy_dt_ids);
+
+static int ns2_drd_phy_probe(struct platform_device *pdev)
+{
+	struct phy_provider *phy_provider;
+	struct device *dev = &pdev->dev;
+	struct ns2_phy_driver *driver;
+	struct ns2_phy_data *data;
+	struct resource *res;
+	int ret;
+	u32 val;
+
+	driver = devm_kzalloc(dev, sizeof(struct ns2_phy_driver),
+			      GFP_KERNEL);
+	if (!driver)
+		return -ENOMEM;
+
+	driver->data = devm_kzalloc(dev, sizeof(struct ns2_phy_data),
+				  GFP_KERNEL);
+	if (!driver->data)
+		return -ENOMEM;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "icfg");
+	driver->icfgdrd_regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(driver->icfgdrd_regs))
+		return PTR_ERR(driver->icfgdrd_regs);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "rst-ctrl");
+	driver->idmdrd_rst_ctrl = devm_ioremap_resource(dev, res);
+	if (IS_ERR(driver->idmdrd_rst_ctrl))
+		return PTR_ERR(driver->idmdrd_rst_ctrl);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "crmu-ctrl");
+	driver->crmu_usb2_ctrl = devm_ioremap_resource(dev, res);
+	if (IS_ERR(driver->crmu_usb2_ctrl))
+		return PTR_ERR(driver->crmu_usb2_ctrl);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "usb2-strap");
+	driver->usb2h_strap_reg = devm_ioremap_resource(dev, res);
+	if (IS_ERR(driver->usb2h_strap_reg))
+		return PTR_ERR(driver->usb2h_strap_reg);
+
+	 /* create extcon */
+	driver->id_gpiod = devm_gpiod_get(&pdev->dev, "id", GPIOD_IN);
+	if (IS_ERR(driver->id_gpiod)) {
+		dev_err(dev, "failed to get ID GPIO\n");
+		return PTR_ERR(driver->id_gpiod);
+	}
+	driver->vbus_gpiod = devm_gpiod_get(&pdev->dev, "vbus", GPIOD_IN);
+	if (IS_ERR(driver->vbus_gpiod)) {
+		dev_err(dev, "failed to get VBUS GPIO\n");
+		return PTR_ERR(driver->vbus_gpiod);
+	}
+
+	driver->edev = devm_extcon_dev_allocate(dev, usb_extcon_cable);
+	if (IS_ERR(driver->edev)) {
+		dev_err(dev, "failed to allocate extcon device\n");
+		return -ENOMEM;
+	}
+
+	ret = devm_extcon_dev_register(dev, driver->edev);
+	if (ret < 0) {
+		dev_err(dev, "failed to register extcon device\n");
+		return ret;
+	}
+
+	ret = gpiod_set_debounce(driver->id_gpiod, GPIO_DELAY * 1000);
+	if (ret < 0)
+		driver->debounce_jiffies = msecs_to_jiffies(GPIO_DELAY);
+
+	INIT_DELAYED_WORK(&driver->wq_extcon, extcon_work);
+
+	driver->id_irq = gpiod_to_irq(driver->id_gpiod);
+	if (driver->id_irq < 0) {
+		dev_err(dev, "failed to get ID IRQ\n");
+		return driver->id_irq;
+	}
+
+	driver->vbus_irq = gpiod_to_irq(driver->vbus_gpiod);
+	if (driver->vbus_irq < 0) {
+		dev_err(dev, "failed to get ID IRQ\n");
+		return driver->vbus_irq;
+	}
+
+	ret = devm_request_irq(dev, driver->id_irq, gpio_irq_handler,
+			       IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+			       "usb_id", driver);
+	if (ret < 0) {
+		dev_err(dev, "failed to request handler for ID IRQ\n");
+		return ret;
+	}
+
+	ret = devm_request_irq(dev, driver->vbus_irq, gpio_irq_handler,
+			       IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+			       "usb_vbus", driver);
+	if (ret < 0) {
+		dev_err(dev, "failed to request handler for VBUS IRQ\n");
+		return ret;
+	}
+
+	dev_set_drvdata(dev, driver);
+
+	/* Shutdown all ports. They can be powered up as required */
+	val = readl(driver->crmu_usb2_ctrl);
+	val &= ~(AFE_CORERDY_VDDC | PHY_RESETB);
+	writel(val, driver->crmu_usb2_ctrl);
+
+	data = driver->data;
+	data->phy = devm_phy_create(dev, dev->of_node, &ops);
+	if (IS_ERR(data->phy)) {
+		dev_err(dev, "Failed to create usb drd phy\n");
+		return PTR_ERR(data->phy);
+	}
+
+	data->driver = driver;
+	phy_set_drvdata(data->phy, data);
+
+	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+	if (IS_ERR(phy_provider)) {
+		dev_err(dev, "Failed to register as phy provider\n");
+		return PTR_ERR(phy_provider);
+	}
+
+	platform_set_drvdata(pdev, driver);
+
+	dev_info(dev, "Registered NS2 DRD Phy device\n");
+	queue_delayed_work(system_power_efficient_wq, &driver->wq_extcon,
+			   driver->debounce_jiffies);
+
+	return 0;
+}
+
+static struct platform_driver ns2_drd_phy_driver = {
+	.probe = ns2_drd_phy_probe,
+	.driver = {
+		.name = "bcm-ns2-usbphy",
+		.of_match_table = of_match_ptr(ns2_drd_phy_dt_ids),
+	},
+};
+module_platform_driver(ns2_drd_phy_driver);
+
+MODULE_ALIAS("platform:bcm-ns2-drd-phy");
+MODULE_AUTHOR("Broadcom");
+MODULE_DESCRIPTION("Broadcom NS2 USB2 PHY driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-brcm-sata.c b/drivers/phy/broadcom/phy-brcm-sata.c
index ccbc3d994998..e6544c8b1ace 100644
--- a/drivers/phy/phy-brcm-sata.c
+++ b/drivers/phy/broadcom/phy-brcm-sata.c
@@ -46,6 +46,7 @@ enum brcm_sata_phy_version {
 	BRCM_SATA_PHY_STB_40NM,
 	BRCM_SATA_PHY_IPROC_NS2,
 	BRCM_SATA_PHY_IPROC_NSP,
+	BRCM_SATA_PHY_IPROC_SR,
 };
 
 struct brcm_sata_port {
@@ -81,12 +82,17 @@ enum sata_phy_regs {
 	PLL_ACTRL2				= 0x8b,
 	PLL_ACTRL2_SELDIV_MASK			= 0x1f,
 	PLL_ACTRL2_SELDIV_SHIFT			= 9,
+	PLL_ACTRL6				= 0x86,
 
 	PLL1_REG_BANK				= 0x060,
 	PLL1_ACTRL2				= 0x82,
 	PLL1_ACTRL3				= 0x83,
 	PLL1_ACTRL4				= 0x84,
 
+	TX_REG_BANK				= 0x070,
+	TX_ACTRL0				= 0x80,
+	TX_ACTRL0_TXPOL_FLIP			= BIT(6),
+
 	OOB_REG_BANK				= 0x150,
 	OOB1_REG_BANK				= 0x160,
 	OOB_CTRL1				= 0x80,
@@ -347,6 +353,68 @@ static int brcm_nsp_sata_init(struct brcm_sata_port *port)
 	return 0;
 }
 
+/* SR PHY PLL0 registers */
+#define SR_PLL0_ACTRL6_MAGIC			0xa
+
+/* SR PHY PLL1 registers */
+#define SR_PLL1_ACTRL2_MAGIC			0x32
+#define SR_PLL1_ACTRL3_MAGIC			0x2
+#define SR_PLL1_ACTRL4_MAGIC			0x3e8
+
+static int brcm_sr_sata_init(struct brcm_sata_port *port)
+{
+	struct brcm_sata_phy *priv = port->phy_priv;
+	struct device *dev = port->phy_priv->dev;
+	void __iomem *base = priv->phy_base;
+	unsigned int val, try;
+
+	/* Configure PHY PLL register bank 1 */
+	val = SR_PLL1_ACTRL2_MAGIC;
+	brcm_sata_phy_wr(base, PLL1_REG_BANK, PLL1_ACTRL2, 0x0, val);
+	val = SR_PLL1_ACTRL3_MAGIC;
+	brcm_sata_phy_wr(base, PLL1_REG_BANK, PLL1_ACTRL3, 0x0, val);
+	val = SR_PLL1_ACTRL4_MAGIC;
+	brcm_sata_phy_wr(base, PLL1_REG_BANK, PLL1_ACTRL4, 0x0, val);
+
+	/* Configure PHY PLL register bank 0 */
+	val = SR_PLL0_ACTRL6_MAGIC;
+	brcm_sata_phy_wr(base, PLL_REG_BANK_0, PLL_ACTRL6, 0x0, val);
+
+	/* Wait for PHY PLL lock by polling pll_lock bit */
+	try = 50;
+	do {
+		val = brcm_sata_phy_rd(base, BLOCK0_REG_BANK,
+					BLOCK0_XGXSSTATUS);
+		if (val & BLOCK0_XGXSSTATUS_PLL_LOCK)
+			break;
+		msleep(20);
+		try--;
+	} while (try);
+
+	if ((val & BLOCK0_XGXSSTATUS_PLL_LOCK) == 0) {
+		/* PLL did not lock; give up */
+		dev_err(dev, "port%d PLL did not lock\n", port->portnum);
+		return -ETIMEDOUT;
+	}
+
+	/* Invert Tx polarity */
+	brcm_sata_phy_wr(base, TX_REG_BANK, TX_ACTRL0,
+			 ~TX_ACTRL0_TXPOL_FLIP, TX_ACTRL0_TXPOL_FLIP);
+
+	/* Configure OOB control to handle 100MHz reference clock */
+	val = ((0xc << OOB_CTRL1_BURST_MAX_SHIFT) |
+		(0x4 << OOB_CTRL1_BURST_MIN_SHIFT) |
+		(0x8 << OOB_CTRL1_WAKE_IDLE_MAX_SHIFT) |
+		(0x3 << OOB_CTRL1_WAKE_IDLE_MIN_SHIFT));
+	brcm_sata_phy_wr(base, OOB_REG_BANK, OOB_CTRL1, 0x0, val);
+	val = ((0x1b << OOB_CTRL2_RESET_IDLE_MAX_SHIFT) |
+		(0x2 << OOB_CTRL2_BURST_CNT_SHIFT) |
+		(0x9 << OOB_CTRL2_RESET_IDLE_MIN_SHIFT));
+	brcm_sata_phy_wr(base, OOB_REG_BANK, OOB_CTRL2, 0x0, val);
+
+	return 0;
+}
+
 static int brcm_sata_phy_init(struct phy *phy)
 {
 	int rc;
@@ -363,6 +431,9 @@ static int brcm_sata_phy_init(struct phy *phy)
 	case BRCM_SATA_PHY_IPROC_NSP:
 		rc = brcm_nsp_sata_init(port);
 		break;
+	case BRCM_SATA_PHY_IPROC_SR:
+		rc = brcm_sr_sata_init(port);
+		break;
 	default:
 		rc = -ENODEV;
 	}
@@ -384,6 +455,8 @@ static const struct of_device_id brcm_sata_phy_of_match[] = {
 	  .data = (void *)BRCM_SATA_PHY_IPROC_NS2 },
 	{ .compatible = "brcm,iproc-nsp-sata-phy",
 	  .data = (void *)BRCM_SATA_PHY_IPROC_NSP },
+	{ .compatible	= "brcm,iproc-sr-sata-phy",
+	  .data = (void *)BRCM_SATA_PHY_IPROC_SR },
 	{},
 };
 MODULE_DEVICE_TABLE(of, brcm_sata_phy_of_match);
diff --git a/drivers/phy/hisilicon/Kconfig b/drivers/phy/hisilicon/Kconfig
new file mode 100644
index 000000000000..6164c4cd0f65
--- /dev/null
+++ b/drivers/phy/hisilicon/Kconfig
@@ -0,0 +1,20 @@
+#
+# Phy drivers for Hisilicon platforms
+#
+config PHY_HI6220_USB
+	tristate "hi6220 USB PHY support"
+	depends on (ARCH_HISI && ARM64) || COMPILE_TEST
+	select GENERIC_PHY
+	select MFD_SYSCON
+	help
+	  Enable this to support the HISILICON HI6220 USB PHY.
+
+	  To compile this driver as a module, choose M here.
+
+config PHY_HIX5HD2_SATA
+	tristate "HIX5HD2 SATA PHY Driver"
+	depends on ARCH_HIX5HD2 && OF && HAS_IOMEM
+	select GENERIC_PHY
+	select MFD_SYSCON
+	help
+	  Support for SATA PHY on Hisilicon hix5hd2 Soc.
diff --git a/drivers/phy/hisilicon/Makefile b/drivers/phy/hisilicon/Makefile
new file mode 100644
index 000000000000..541b348187a8
--- /dev/null
+++ b/drivers/phy/hisilicon/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_PHY_HI6220_USB)		+= phy-hi6220-usb.o
+obj-$(CONFIG_PHY_HIX5HD2_SATA)		+= phy-hix5hd2-sata.o
diff --git a/drivers/phy/phy-hi6220-usb.c b/drivers/phy/hisilicon/phy-hi6220-usb.c
index 398c1021deec..398c1021deec 100644
--- a/drivers/phy/phy-hi6220-usb.c
+++ b/drivers/phy/hisilicon/phy-hi6220-usb.c
diff --git a/drivers/phy/phy-hix5hd2-sata.c b/drivers/phy/hisilicon/phy-hix5hd2-sata.c
index e5ab3aa78b9d..e5ab3aa78b9d 100644
--- a/drivers/phy/phy-hix5hd2-sata.c
+++ b/drivers/phy/hisilicon/phy-hix5hd2-sata.c
diff --git a/drivers/phy/marvell/Kconfig b/drivers/phy/marvell/Kconfig
new file mode 100644
index 000000000000..048d8893bc2e
--- /dev/null
+++ b/drivers/phy/marvell/Kconfig
@@ -0,0 +1,50 @@
+#
+# Phy drivers for Marvell platforms
+#
+config ARMADA375_USBCLUSTER_PHY
+	def_bool y
+	depends on MACH_ARMADA_375 || COMPILE_TEST
+	depends on OF && HAS_IOMEM
+	select GENERIC_PHY
+
+config PHY_BERLIN_SATA
+	tristate "Marvell Berlin SATA PHY driver"
+	depends on ARCH_BERLIN && HAS_IOMEM && OF
+	select GENERIC_PHY
+	help
+	  Enable this to support the SATA PHY on Marvell Berlin SoCs.
+
+config PHY_BERLIN_USB
+	tristate "Marvell Berlin USB PHY Driver"
+	depends on ARCH_BERLIN && RESET_CONTROLLER && HAS_IOMEM && OF
+	select GENERIC_PHY
+	help
+	  Enable this to support the USB PHY on Marvell Berlin SoCs.
+
+config PHY_MVEBU_SATA
+	def_bool y
+	depends on ARCH_DOVE || MACH_DOVE || MACH_KIRKWOOD
+	depends on OF
+	select GENERIC_PHY
+
+config PHY_PXA_28NM_HSIC
+	tristate "Marvell USB HSIC 28nm PHY Driver"
+	depends on HAS_IOMEM
+	select GENERIC_PHY
+	help
+	  Enable this to support Marvell USB HSIC PHY driver for Marvell
+	  SoC. This driver will do the PHY initialization and shutdown.
+	  The PHY driver will be used by Marvell ehci driver.
+
+	  To compile this driver as a module, choose M here.
+
+config PHY_PXA_28NM_USB2
+	tristate "Marvell USB 2.0 28nm PHY Driver"
+	depends on HAS_IOMEM
+	select GENERIC_PHY
+	help
+	  Enable this to support Marvell USB 2.0 PHY driver for Marvell
+	  SoC. This driver will do the PHY initialization and shutdown.
+	  The PHY driver will be used by Marvell udc/ehci/otg driver.
+
+	  To compile this driver as a module, choose M here.
diff --git a/drivers/phy/marvell/Makefile b/drivers/phy/marvell/Makefile
new file mode 100644
index 000000000000..3fc188f59118
--- /dev/null
+++ b/drivers/phy/marvell/Makefile
@@ -0,0 +1,6 @@
+obj-$(CONFIG_ARMADA375_USBCLUSTER_PHY)	+= phy-armada375-usb2.o
+obj-$(CONFIG_PHY_BERLIN_SATA)		+= phy-berlin-sata.o
+obj-$(CONFIG_PHY_BERLIN_USB)		+= phy-berlin-usb.o
+obj-$(CONFIG_PHY_MVEBU_SATA)		+= phy-mvebu-sata.o
+obj-$(CONFIG_PHY_PXA_28NM_HSIC)		+= phy-pxa-28nm-hsic.o
+obj-$(CONFIG_PHY_PXA_28NM_USB2)		+= phy-pxa-28nm-usb2.o
diff --git a/drivers/phy/phy-armada375-usb2.c b/drivers/phy/marvell/phy-armada375-usb2.c
index 1a3db288c0a9..1a3db288c0a9 100644
--- a/drivers/phy/phy-armada375-usb2.c
+++ b/drivers/phy/marvell/phy-armada375-usb2.c
diff --git a/drivers/phy/phy-berlin-sata.c b/drivers/phy/marvell/phy-berlin-sata.c
index 2c7a57f2d595..2c7a57f2d595 100644
--- a/drivers/phy/phy-berlin-sata.c
+++ b/drivers/phy/marvell/phy-berlin-sata.c
diff --git a/drivers/phy/phy-berlin-usb.c b/drivers/phy/marvell/phy-berlin-usb.c
index 2017751ede26..2017751ede26 100644
--- a/drivers/phy/phy-berlin-usb.c
+++ b/drivers/phy/marvell/phy-berlin-usb.c
diff --git a/drivers/phy/phy-mvebu-sata.c b/drivers/phy/marvell/phy-mvebu-sata.c
index 768ce92e81ce..768ce92e81ce 100644
--- a/drivers/phy/phy-mvebu-sata.c
+++ b/drivers/phy/marvell/phy-mvebu-sata.c
diff --git a/drivers/phy/phy-pxa-28nm-hsic.c b/drivers/phy/marvell/phy-pxa-28nm-hsic.c
index 234aacf4db20..234aacf4db20 100644
--- a/drivers/phy/phy-pxa-28nm-hsic.c
+++ b/drivers/phy/marvell/phy-pxa-28nm-hsic.c
diff --git a/drivers/phy/phy-pxa-28nm-usb2.c b/drivers/phy/marvell/phy-pxa-28nm-usb2.c
index 37e9c8ca4983..37e9c8ca4983 100644
--- a/drivers/phy/phy-pxa-28nm-usb2.c
+++ b/drivers/phy/marvell/phy-pxa-28nm-usb2.c
diff --git a/drivers/phy/motorola/Kconfig b/drivers/phy/motorola/Kconfig
new file mode 100644
index 000000000000..6bb7d6bdf1bf
--- /dev/null
+++ b/drivers/phy/motorola/Kconfig
@@ -0,0 +1,12 @@
+#
+# Phy drivers for Motorola devices
+#
+config PHY_CPCAP_USB
+	tristate "CPCAP PMIC USB PHY driver"
+	depends on USB_SUPPORT && IIO
+	depends on USB_MUSB_HDRC || USB_MUSB_HDRC=n
+	select GENERIC_PHY
+	select USB_PHY
+	help
+	  Enable this for USB to work on Motorola phones and tablets
+	  such as Droid 4.
diff --git a/drivers/phy/motorola/Makefile b/drivers/phy/motorola/Makefile
new file mode 100644
index 000000000000..b6cd618d671a
--- /dev/null
+++ b/drivers/phy/motorola/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for the phy drivers.
+#
+
+obj-$(CONFIG_PHY_CPCAP_USB)		+= phy-cpcap-usb.o
diff --git a/drivers/phy/motorola/phy-cpcap-usb.c b/drivers/phy/motorola/phy-cpcap-usb.c
new file mode 100644
index 000000000000..9b63efa5ae4d
--- /dev/null
+++ b/drivers/phy/motorola/phy-cpcap-usb.c
@@ -0,0 +1,676 @@
+/*
+ * Motorola CPCAP PMIC USB PHY driver
+ * Copyright (C) 2017 Tony Lindgren <tony@atomide.com>
+ *
+ * Some parts based on earlier Motorola Linux kernel tree code in
+ * board-mapphone-usb.c and cpcap-usb-det.c:
+ * Copyright (C) 2007 - 2011 Motorola, 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 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/atomic.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/iio/consumer.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+#include <linux/gpio/consumer.h>
+#include <linux/mfd/motorola-cpcap.h>
+#include <linux/phy/omap_usb.h>
+#include <linux/phy/phy.h>
+#include <linux/regulator/consumer.h>
+#include <linux/usb/musb.h>
+
+/* CPCAP_REG_USBC1 register bits */
+#define CPCAP_BIT_IDPULSE		BIT(15)
+#define CPCAP_BIT_ID100KPU		BIT(14)
+#define CPCAP_BIT_IDPUCNTRL		BIT(13)
+#define CPCAP_BIT_IDPU			BIT(12)
+#define CPCAP_BIT_IDPD			BIT(11)
+#define CPCAP_BIT_VBUSCHRGTMR3		BIT(10)
+#define CPCAP_BIT_VBUSCHRGTMR2		BIT(9)
+#define CPCAP_BIT_VBUSCHRGTMR1		BIT(8)
+#define CPCAP_BIT_VBUSCHRGTMR0		BIT(7)
+#define CPCAP_BIT_VBUSPU		BIT(6)
+#define CPCAP_BIT_VBUSPD		BIT(5)
+#define CPCAP_BIT_DMPD			BIT(4)
+#define CPCAP_BIT_DPPD			BIT(3)
+#define CPCAP_BIT_DM1K5PU		BIT(2)
+#define CPCAP_BIT_DP1K5PU		BIT(1)
+#define CPCAP_BIT_DP150KPU		BIT(0)
+
+/* CPCAP_REG_USBC2 register bits */
+#define CPCAP_BIT_ZHSDRV1		BIT(15)
+#define CPCAP_BIT_ZHSDRV0		BIT(14)
+#define CPCAP_BIT_DPLLCLKREQ		BIT(13)
+#define CPCAP_BIT_SE0CONN		BIT(12)
+#define CPCAP_BIT_UARTTXTRI		BIT(11)
+#define CPCAP_BIT_UARTSWAP		BIT(10)
+#define CPCAP_BIT_UARTMUX1		BIT(9)
+#define CPCAP_BIT_UARTMUX0		BIT(8)
+#define CPCAP_BIT_ULPISTPLOW		BIT(7)
+#define CPCAP_BIT_TXENPOL		BIT(6)
+#define CPCAP_BIT_USBXCVREN		BIT(5)
+#define CPCAP_BIT_USBCNTRL		BIT(4)
+#define CPCAP_BIT_USBSUSPEND		BIT(3)
+#define CPCAP_BIT_EMUMODE2		BIT(2)
+#define CPCAP_BIT_EMUMODE1		BIT(1)
+#define CPCAP_BIT_EMUMODE0		BIT(0)
+
+/* CPCAP_REG_USBC3 register bits */
+#define CPCAP_BIT_SPARE_898_15		BIT(15)
+#define CPCAP_BIT_IHSTX03		BIT(14)
+#define CPCAP_BIT_IHSTX02		BIT(13)
+#define CPCAP_BIT_IHSTX01		BIT(12)
+#define CPCAP_BIT_IHSTX0		BIT(11)
+#define CPCAP_BIT_IDPU_SPI		BIT(10)
+#define CPCAP_BIT_UNUSED_898_9		BIT(9)
+#define CPCAP_BIT_VBUSSTBY_EN		BIT(8)
+#define CPCAP_BIT_VBUSEN_SPI		BIT(7)
+#define CPCAP_BIT_VBUSPU_SPI		BIT(6)
+#define CPCAP_BIT_VBUSPD_SPI		BIT(5)
+#define CPCAP_BIT_DMPD_SPI		BIT(4)
+#define CPCAP_BIT_DPPD_SPI		BIT(3)
+#define CPCAP_BIT_SUSPEND_SPI		BIT(2)
+#define CPCAP_BIT_PU_SPI		BIT(1)
+#define CPCAP_BIT_ULPI_SPI_SEL		BIT(0)
+
+struct cpcap_usb_ints_state {
+	bool id_ground;
+	bool id_float;
+	bool chrg_det;
+	bool rvrs_chrg;
+	bool vbusov;
+
+	bool chrg_se1b;
+	bool se0conn;
+	bool rvrs_mode;
+	bool chrgcurr1;
+	bool vbusvld;
+	bool sessvld;
+	bool sessend;
+	bool se1;
+
+	bool battdetb;
+	bool dm;
+	bool dp;
+};
+
+enum cpcap_gpio_mode {
+	CPCAP_DM_DP,
+	CPCAP_MDM_RX_TX,
+	CPCAP_UNKNOWN,
+	CPCAP_OTG_DM_DP,
+};
+
+struct cpcap_phy_ddata {
+	struct regmap *reg;
+	struct device *dev;
+	struct clk *refclk;
+	struct usb_phy phy;
+	struct delayed_work detect_work;
+	struct pinctrl *pins;
+	struct pinctrl_state *pins_ulpi;
+	struct pinctrl_state *pins_utmi;
+	struct pinctrl_state *pins_uart;
+	struct gpio_desc *gpio[2];
+	struct iio_channel *vbus;
+	struct iio_channel *id;
+	struct regulator *vusb;
+	atomic_t active;
+};
+
+static bool cpcap_usb_vbus_valid(struct cpcap_phy_ddata *ddata)
+{
+	int error, value = 0;
+
+	error = iio_read_channel_processed(ddata->vbus, &value);
+	if (error >= 0)
+		return value > 3900 ? true : false;
+
+	dev_err(ddata->dev, "error reading VBUS: %i\n", error);
+
+	return false;
+}
+
+static int cpcap_usb_phy_set_host(struct usb_otg *otg, struct usb_bus *host)
+{
+	otg->host = host;
+	if (!host)
+		otg->state = OTG_STATE_UNDEFINED;
+
+	return 0;
+}
+
+static int cpcap_usb_phy_set_peripheral(struct usb_otg *otg,
+					struct usb_gadget *gadget)
+{
+	otg->gadget = gadget;
+	if (!gadget)
+		otg->state = OTG_STATE_UNDEFINED;
+
+	return 0;
+}
+
+static const struct phy_ops ops = {
+	.owner		= THIS_MODULE,
+};
+
+static int cpcap_phy_get_ints_state(struct cpcap_phy_ddata *ddata,
+				    struct cpcap_usb_ints_state *s)
+{
+	int val, error;
+
+	error = regmap_read(ddata->reg, CPCAP_REG_INTS1, &val);
+	if (error)
+		return error;
+
+	s->id_ground = val & BIT(15);
+	s->id_float = val & BIT(14);
+	s->vbusov = val & BIT(11);
+
+	error = regmap_read(ddata->reg, CPCAP_REG_INTS2, &val);
+	if (error)
+		return error;
+
+	s->vbusvld = val & BIT(3);
+	s->sessvld = val & BIT(2);
+	s->sessend = val & BIT(1);
+	s->se1 = val & BIT(0);
+
+	error = regmap_read(ddata->reg, CPCAP_REG_INTS4, &val);
+	if (error)
+		return error;
+
+	s->dm = val & BIT(1);
+	s->dp = val & BIT(0);
+
+	return 0;
+}
+
+static int cpcap_usb_set_uart_mode(struct cpcap_phy_ddata *ddata);
+static int cpcap_usb_set_usb_mode(struct cpcap_phy_ddata *ddata);
+
+static void cpcap_usb_detect(struct work_struct *work)
+{
+	struct cpcap_phy_ddata *ddata;
+	struct cpcap_usb_ints_state s;
+	bool vbus = false;
+	int error;
+
+	ddata = container_of(work, struct cpcap_phy_ddata, detect_work.work);
+
+	error = cpcap_phy_get_ints_state(ddata, &s);
+	if (error)
+		return;
+
+	if (s.id_ground) {
+		dev_dbg(ddata->dev, "id ground, USB host mode\n");
+		error = cpcap_usb_set_usb_mode(ddata);
+		if (error)
+			goto out_err;
+
+		error = musb_mailbox(MUSB_ID_GROUND);
+		if (error)
+			goto out_err;
+
+		error = regmap_update_bits(ddata->reg, CPCAP_REG_USBC3,
+					   CPCAP_BIT_VBUSSTBY_EN,
+					   CPCAP_BIT_VBUSSTBY_EN);
+		if (error)
+			goto out_err;
+
+		return;
+	}
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_USBC3,
+				   CPCAP_BIT_VBUSSTBY_EN, 0);
+	if (error)
+		goto out_err;
+
+	vbus = cpcap_usb_vbus_valid(ddata);
+
+	if (vbus) {
+		/* Are we connected to a docking station with vbus? */
+		if (s.id_ground) {
+			dev_dbg(ddata->dev, "connected to a dock\n");
+
+			/* No VBUS needed with docks */
+			error = cpcap_usb_set_usb_mode(ddata);
+			if (error)
+				goto out_err;
+			error = musb_mailbox(MUSB_ID_GROUND);
+			if (error)
+				goto out_err;
+
+			return;
+		}
+
+		/* Otherwise assume we're connected to a USB host */
+		dev_dbg(ddata->dev, "connected to USB host\n");
+		error = cpcap_usb_set_usb_mode(ddata);
+		if (error)
+			goto out_err;
+		error = musb_mailbox(MUSB_VBUS_VALID);
+		if (error)
+			goto out_err;
+
+		return;
+	}
+
+	/* Default to debug UART mode */
+	error = cpcap_usb_set_uart_mode(ddata);
+	if (error)
+		goto out_err;
+
+	error = musb_mailbox(MUSB_VBUS_OFF);
+	if (error)
+		goto out_err;
+
+	dev_dbg(ddata->dev, "set UART mode\n");
+
+	return;
+
+out_err:
+	dev_err(ddata->dev, "error setting cable state: %i\n", error);
+}
+
+static irqreturn_t cpcap_phy_irq_thread(int irq, void *data)
+{
+	struct cpcap_phy_ddata *ddata = data;
+
+	if (!atomic_read(&ddata->active))
+		return IRQ_NONE;
+
+	schedule_delayed_work(&ddata->detect_work, msecs_to_jiffies(1));
+
+	return IRQ_HANDLED;
+}
+
+static int cpcap_usb_init_irq(struct platform_device *pdev,
+			      struct cpcap_phy_ddata *ddata,
+			      const char *name)
+{
+	int irq, error;
+
+	irq = platform_get_irq_byname(pdev, name);
+	if (!irq)
+		return -ENODEV;
+
+	error = devm_request_threaded_irq(ddata->dev, irq, NULL,
+					  cpcap_phy_irq_thread,
+					  IRQF_SHARED,
+					  name, ddata);
+	if (error) {
+		dev_err(ddata->dev, "could not get irq %s: %i\n",
+			name, error);
+
+		return error;
+	}
+
+	return 0;
+}
+
+static const char * const cpcap_phy_irqs[] = {
+	/* REG_INT_0 */
+	"id_ground", "id_float",
+
+	/* REG_INT1 */
+	"se0conn", "vbusvld", "sessvld", "sessend", "se1",
+
+	/* REG_INT_3 */
+	"dm", "dp",
+};
+
+static int cpcap_usb_init_interrupts(struct platform_device *pdev,
+				     struct cpcap_phy_ddata *ddata)
+{
+	int i, error;
+
+	for (i = 0; i < ARRAY_SIZE(cpcap_phy_irqs); i++) {
+		error = cpcap_usb_init_irq(pdev, ddata, cpcap_phy_irqs[i]);
+		if (error)
+			return error;
+	}
+
+	return 0;
+}
+
+/*
+ * Optional pins and modes. At least Motorola mapphone devices
+ * are using two GPIOs and dynamic pinctrl to multiplex PHY pins
+ * to UART, ULPI or UTMI mode.
+ */
+
+static int cpcap_usb_gpio_set_mode(struct cpcap_phy_ddata *ddata,
+				   enum cpcap_gpio_mode mode)
+{
+	if (!ddata->gpio[0] || !ddata->gpio[1])
+		return 0;
+
+	gpiod_set_value(ddata->gpio[0], mode & 1);
+	gpiod_set_value(ddata->gpio[1], mode >> 1);
+
+	return 0;
+}
+
+static int cpcap_usb_set_uart_mode(struct cpcap_phy_ddata *ddata)
+{
+	int error;
+
+	error = cpcap_usb_gpio_set_mode(ddata, CPCAP_DM_DP);
+	if (error)
+		goto out_err;
+
+	if (ddata->pins_uart) {
+		error = pinctrl_select_state(ddata->pins, ddata->pins_uart);
+		if (error)
+			goto out_err;
+	}
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_USBC1,
+				   CPCAP_BIT_VBUSPD,
+				   CPCAP_BIT_VBUSPD);
+	if (error)
+		goto out_err;
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_USBC2,
+				   0xffff, CPCAP_BIT_UARTMUX0 |
+				   CPCAP_BIT_EMUMODE0);
+	if (error)
+		goto out_err;
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_USBC3, 0x7fff,
+				   CPCAP_BIT_IDPU_SPI);
+	if (error)
+		goto out_err;
+
+	return 0;
+
+out_err:
+	dev_err(ddata->dev, "%s failed with %i\n", __func__, error);
+
+	return error;
+}
+
+static int cpcap_usb_set_usb_mode(struct cpcap_phy_ddata *ddata)
+{
+	int error;
+
+	error = cpcap_usb_gpio_set_mode(ddata, CPCAP_OTG_DM_DP);
+	if (error)
+		return error;
+
+	if (ddata->pins_utmi) {
+		error = pinctrl_select_state(ddata->pins, ddata->pins_utmi);
+		if (error) {
+			dev_err(ddata->dev, "could not set usb mode: %i\n",
+				error);
+
+			return error;
+		}
+	}
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_USBC1,
+				   CPCAP_BIT_VBUSPD, 0);
+	if (error)
+		goto out_err;
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_USBC2,
+				   CPCAP_BIT_USBXCVREN,
+				   CPCAP_BIT_USBXCVREN);
+	if (error)
+		goto out_err;
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_USBC3,
+				   CPCAP_BIT_PU_SPI |
+				   CPCAP_BIT_DMPD_SPI |
+				   CPCAP_BIT_DPPD_SPI |
+				   CPCAP_BIT_SUSPEND_SPI |
+				   CPCAP_BIT_ULPI_SPI_SEL, 0);
+	if (error)
+		goto out_err;
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_USBC2,
+				   CPCAP_BIT_USBXCVREN,
+				   CPCAP_BIT_USBXCVREN);
+	if (error)
+		goto out_err;
+
+	return 0;
+
+out_err:
+	dev_err(ddata->dev, "%s failed with %i\n", __func__, error);
+
+	return error;
+}
+
+static int cpcap_usb_init_optional_pins(struct cpcap_phy_ddata *ddata)
+{
+	ddata->pins = devm_pinctrl_get(ddata->dev);
+	if (IS_ERR(ddata->pins)) {
+		dev_info(ddata->dev, "default pins not configured: %ld\n",
+			 PTR_ERR(ddata->pins));
+		ddata->pins = NULL;
+
+		return 0;
+	}
+
+	ddata->pins_ulpi = pinctrl_lookup_state(ddata->pins, "ulpi");
+	if (IS_ERR(ddata->pins_ulpi)) {
+		dev_info(ddata->dev, "ulpi pins not configured\n");
+		ddata->pins_ulpi = NULL;
+	}
+
+	ddata->pins_utmi = pinctrl_lookup_state(ddata->pins, "utmi");
+	if (IS_ERR(ddata->pins_utmi)) {
+		dev_info(ddata->dev, "utmi pins not configured\n");
+		ddata->pins_utmi = NULL;
+	}
+
+	ddata->pins_uart = pinctrl_lookup_state(ddata->pins, "uart");
+	if (IS_ERR(ddata->pins_uart)) {
+		dev_info(ddata->dev, "uart pins not configured\n");
+		ddata->pins_uart = NULL;
+	}
+
+	if (ddata->pins_uart)
+		return pinctrl_select_state(ddata->pins, ddata->pins_uart);
+
+	return 0;
+}
+
+static void cpcap_usb_init_optional_gpios(struct cpcap_phy_ddata *ddata)
+{
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		ddata->gpio[i] = devm_gpiod_get_index(ddata->dev, "mode",
+						      i, GPIOD_OUT_HIGH);
+		if (IS_ERR(ddata->gpio[i])) {
+			dev_info(ddata->dev, "no mode change GPIO%i: %li\n",
+				 i, PTR_ERR(ddata->gpio[i]));
+				 ddata->gpio[i] = NULL;
+		}
+	}
+}
+
+static int cpcap_usb_init_iio(struct cpcap_phy_ddata *ddata)
+{
+	enum iio_chan_type type;
+	int error;
+
+	ddata->vbus = devm_iio_channel_get(ddata->dev, "vbus");
+	if (IS_ERR(ddata->vbus)) {
+		error = PTR_ERR(ddata->vbus);
+		goto out_err;
+	}
+
+	if (!ddata->vbus->indio_dev) {
+		error = -ENXIO;
+		goto out_err;
+	}
+
+	error = iio_get_channel_type(ddata->vbus, &type);
+	if (error < 0)
+		goto out_err;
+
+	if (type != IIO_VOLTAGE) {
+		error = -EINVAL;
+		goto out_err;
+	}
+
+	return 0;
+
+out_err:
+	dev_err(ddata->dev, "could not initialize VBUS or ID IIO: %i\n",
+		error);
+
+	return error;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id cpcap_usb_phy_id_table[] = {
+	{
+		.compatible = "motorola,cpcap-usb-phy",
+	},
+	{
+		.compatible = "motorola,mapphone-cpcap-usb-phy",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, cpcap_usb_phy_id_table);
+#endif
+
+static int cpcap_usb_phy_probe(struct platform_device *pdev)
+{
+	struct cpcap_phy_ddata *ddata;
+	struct phy *generic_phy;
+	struct phy_provider *phy_provider;
+	struct usb_otg *otg;
+	const struct of_device_id *of_id;
+	int error;
+
+	of_id = of_match_device(of_match_ptr(cpcap_usb_phy_id_table),
+				&pdev->dev);
+	if (!of_id)
+		return -EINVAL;
+
+	ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+	if (!ddata)
+		return -ENOMEM;
+
+	ddata->reg = dev_get_regmap(pdev->dev.parent, NULL);
+	if (!ddata->reg)
+		return -ENODEV;
+
+	otg = devm_kzalloc(&pdev->dev, sizeof(*otg), GFP_KERNEL);
+	if (!otg)
+		return -ENOMEM;
+
+	ddata->dev = &pdev->dev;
+	ddata->phy.dev = ddata->dev;
+	ddata->phy.label = "cpcap_usb_phy";
+	ddata->phy.otg = otg;
+	ddata->phy.type = USB_PHY_TYPE_USB2;
+	otg->set_host = cpcap_usb_phy_set_host;
+	otg->set_peripheral = cpcap_usb_phy_set_peripheral;
+	otg->usb_phy = &ddata->phy;
+	INIT_DELAYED_WORK(&ddata->detect_work, cpcap_usb_detect);
+	platform_set_drvdata(pdev, ddata);
+
+	ddata->vusb = devm_regulator_get(&pdev->dev, "vusb");
+	if (IS_ERR(ddata->vusb))
+		return PTR_ERR(ddata->vusb);
+
+	error = regulator_enable(ddata->vusb);
+	if (error)
+		return error;
+
+	generic_phy = devm_phy_create(ddata->dev, NULL, &ops);
+	if (IS_ERR(generic_phy)) {
+		error = PTR_ERR(generic_phy);
+		return PTR_ERR(generic_phy);
+	}
+
+	phy_set_drvdata(generic_phy, ddata);
+
+	phy_provider = devm_of_phy_provider_register(ddata->dev,
+						     of_phy_simple_xlate);
+	if (IS_ERR(phy_provider))
+		return PTR_ERR(phy_provider);
+
+	error = cpcap_usb_init_optional_pins(ddata);
+	if (error)
+		return error;
+
+	cpcap_usb_init_optional_gpios(ddata);
+
+	error = cpcap_usb_init_iio(ddata);
+	if (error)
+		return error;
+
+	error = cpcap_usb_init_interrupts(pdev, ddata);
+	if (error)
+		return error;
+
+	usb_add_phy_dev(&ddata->phy);
+	atomic_set(&ddata->active, 1);
+	schedule_delayed_work(&ddata->detect_work, msecs_to_jiffies(1));
+
+	return 0;
+}
+
+static int cpcap_usb_phy_remove(struct platform_device *pdev)
+{
+	struct cpcap_phy_ddata *ddata = platform_get_drvdata(pdev);
+	int error;
+
+	atomic_set(&ddata->active, 0);
+	error = cpcap_usb_set_uart_mode(ddata);
+	if (error)
+		dev_err(ddata->dev, "could not set UART mode\n");
+
+	error = musb_mailbox(MUSB_VBUS_OFF);
+	if (error)
+		dev_err(ddata->dev, "could not set mailbox\n");
+
+	usb_remove_phy(&ddata->phy);
+	cancel_delayed_work_sync(&ddata->detect_work);
+	clk_unprepare(ddata->refclk);
+	regulator_disable(ddata->vusb);
+
+	return 0;
+}
+
+static struct platform_driver cpcap_usb_phy_driver = {
+	.probe		= cpcap_usb_phy_probe,
+	.remove		= cpcap_usb_phy_remove,
+	.driver		= {
+		.name	= "cpcap-usb-phy",
+		.of_match_table = of_match_ptr(cpcap_usb_phy_id_table),
+	},
+};
+
+module_platform_driver(cpcap_usb_phy_driver);
+
+MODULE_ALIAS("platform:cpcap_usb");
+MODULE_AUTHOR("Tony Lindgren <tony@atomide.com>");
+MODULE_DESCRIPTION("CPCAP usb phy driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/qualcomm/Kconfig b/drivers/phy/qualcomm/Kconfig
new file mode 100644
index 000000000000..7bfa64baf837
--- /dev/null
+++ b/drivers/phy/qualcomm/Kconfig
@@ -0,0 +1,58 @@
+#
+# Phy drivers for Qualcomm platforms
+#
+config PHY_QCOM_APQ8064_SATA
+	tristate "Qualcomm APQ8064 SATA SerDes/PHY driver"
+	depends on ARCH_QCOM
+	depends on HAS_IOMEM
+	depends on OF
+	select GENERIC_PHY
+
+config PHY_QCOM_IPQ806X_SATA
+	tristate "Qualcomm IPQ806x SATA SerDes/PHY driver"
+	depends on ARCH_QCOM
+	depends on HAS_IOMEM
+	depends on OF
+	select GENERIC_PHY
+
+config PHY_QCOM_QMP
+	tristate "Qualcomm QMP PHY Driver"
+	depends on OF && COMMON_CLK && (ARCH_QCOM || COMPILE_TEST)
+	select GENERIC_PHY
+	help
+	  Enable this to support the QMP PHY transceiver that is used
+	  with controllers such as PCIe, UFS, and USB on Qualcomm chips.
+
+config PHY_QCOM_QUSB2
+	tristate "Qualcomm QUSB2 PHY Driver"
+	depends on OF && (ARCH_QCOM || COMPILE_TEST)
+	depends on NVMEM || !NVMEM
+	select GENERIC_PHY
+	help
+	  Enable this to support the HighSpeed QUSB2 PHY transceiver for USB
+	  controllers on Qualcomm chips. This driver supports the high-speed
+	  PHY which is usually paired with either the ChipIdea or Synopsys DWC3
+	  USB IPs on MSM SOCs.
+
+config PHY_QCOM_UFS
+	tristate "Qualcomm UFS PHY driver"
+	depends on OF && ARCH_QCOM
+	select GENERIC_PHY
+	help
+	  Support for UFS PHY on QCOM chipsets.
+
+config PHY_QCOM_USB_HS
+	tristate "Qualcomm USB HS PHY module"
+	depends on USB_ULPI_BUS
+	depends on EXTCON || !EXTCON # if EXTCON=m, this cannot be built-in
+	select GENERIC_PHY
+	help
+	  Support for the USB high-speed ULPI compliant phy on Qualcomm
+	  chipsets.
+
+config PHY_QCOM_USB_HSIC
+	tristate "Qualcomm USB HSIC ULPI PHY module"
+	depends on USB_ULPI_BUS
+	select GENERIC_PHY
+	help
+	  Support for the USB HSIC ULPI compliant PHY on QCOM chipsets.
diff --git a/drivers/phy/qualcomm/Makefile b/drivers/phy/qualcomm/Makefile
new file mode 100644
index 000000000000..2e183d7695fd
--- /dev/null
+++ b/drivers/phy/qualcomm/Makefile
@@ -0,0 +1,9 @@
+obj-$(CONFIG_PHY_QCOM_APQ8064_SATA)	+= phy-qcom-apq8064-sata.o
+obj-$(CONFIG_PHY_QCOM_IPQ806X_SATA)	+= phy-qcom-ipq806x-sata.o
+obj-$(CONFIG_PHY_QCOM_QMP)		+= phy-qcom-qmp.o
+obj-$(CONFIG_PHY_QCOM_QUSB2)		+= phy-qcom-qusb2.o
+obj-$(CONFIG_PHY_QCOM_UFS)		+= phy-qcom-ufs.o
+obj-$(CONFIG_PHY_QCOM_UFS)		+= phy-qcom-ufs-qmp-14nm.o
+obj-$(CONFIG_PHY_QCOM_UFS)		+= phy-qcom-ufs-qmp-20nm.o
+obj-$(CONFIG_PHY_QCOM_USB_HS) 		+= phy-qcom-usb-hs.o
+obj-$(CONFIG_PHY_QCOM_USB_HSIC) 	+= phy-qcom-usb-hsic.o
diff --git a/drivers/phy/phy-qcom-apq8064-sata.c b/drivers/phy/qualcomm/phy-qcom-apq8064-sata.c
index 69ce2afac015..69ce2afac015 100644
--- a/drivers/phy/phy-qcom-apq8064-sata.c
+++ b/drivers/phy/qualcomm/phy-qcom-apq8064-sata.c
diff --git a/drivers/phy/phy-qcom-ipq806x-sata.c b/drivers/phy/qualcomm/phy-qcom-ipq806x-sata.c
index 0ad127cc9298..0ad127cc9298 100644
--- a/drivers/phy/phy-qcom-ipq806x-sata.c
+++ b/drivers/phy/qualcomm/phy-qcom-ipq806x-sata.c
diff --git a/drivers/phy/phy-qcom-qmp.c b/drivers/phy/qualcomm/phy-qcom-qmp.c
index 78ca62897784..78ca62897784 100644
--- a/drivers/phy/phy-qcom-qmp.c
+++ b/drivers/phy/qualcomm/phy-qcom-qmp.c
diff --git a/drivers/phy/phy-qcom-qusb2.c b/drivers/phy/qualcomm/phy-qcom-qusb2.c
index 6c575244c0fb..6c575244c0fb 100644
--- a/drivers/phy/phy-qcom-qusb2.c
+++ b/drivers/phy/qualcomm/phy-qcom-qusb2.c
diff --git a/drivers/phy/phy-qcom-ufs-i.h b/drivers/phy/qualcomm/phy-qcom-ufs-i.h
index 13b02b7de30b..13b02b7de30b 100644
--- a/drivers/phy/phy-qcom-ufs-i.h
+++ b/drivers/phy/qualcomm/phy-qcom-ufs-i.h
diff --git a/drivers/phy/phy-qcom-ufs-qmp-14nm.c b/drivers/phy/qualcomm/phy-qcom-ufs-qmp-14nm.c
index 12a1b498dc4b..12a1b498dc4b 100644
--- a/drivers/phy/phy-qcom-ufs-qmp-14nm.c
+++ b/drivers/phy/qualcomm/phy-qcom-ufs-qmp-14nm.c
diff --git a/drivers/phy/phy-qcom-ufs-qmp-14nm.h b/drivers/phy/qualcomm/phy-qcom-ufs-qmp-14nm.h
index 3aefdbacbcd0..3aefdbacbcd0 100644
--- a/drivers/phy/phy-qcom-ufs-qmp-14nm.h
+++ b/drivers/phy/qualcomm/phy-qcom-ufs-qmp-14nm.h
diff --git a/drivers/phy/phy-qcom-ufs-qmp-20nm.c b/drivers/phy/qualcomm/phy-qcom-ufs-qmp-20nm.c
index 4f68acb58b73..4f68acb58b73 100644
--- a/drivers/phy/phy-qcom-ufs-qmp-20nm.c
+++ b/drivers/phy/qualcomm/phy-qcom-ufs-qmp-20nm.c
diff --git a/drivers/phy/phy-qcom-ufs-qmp-20nm.h b/drivers/phy/qualcomm/phy-qcom-ufs-qmp-20nm.h
index 4f3076bb3d71..4f3076bb3d71 100644
--- a/drivers/phy/phy-qcom-ufs-qmp-20nm.h
+++ b/drivers/phy/qualcomm/phy-qcom-ufs-qmp-20nm.h
diff --git a/drivers/phy/phy-qcom-ufs.c b/drivers/phy/qualcomm/phy-qcom-ufs.c
index 43865ef340e2..43865ef340e2 100644
--- a/drivers/phy/phy-qcom-ufs.c
+++ b/drivers/phy/qualcomm/phy-qcom-ufs.c
diff --git a/drivers/phy/phy-qcom-usb-hs.c b/drivers/phy/qualcomm/phy-qcom-usb-hs.c
index 94dfbfd739c3..4b20abc3ae2f 100644
--- a/drivers/phy/phy-qcom-usb-hs.c
+++ b/drivers/phy/qualcomm/phy-qcom-usb-hs.c
@@ -11,12 +11,11 @@
 #include <linux/clk.h>
 #include <linux/regulator/consumer.h>
 #include <linux/of_device.h>
+#include <linux/phy/phy.h>
 #include <linux/reset.h>
 #include <linux/extcon.h>
 #include <linux/notifier.h>
 
-#include "ulpi_phy.h"
-
 #define ULPI_PWR_CLK_MNG_REG		0x88
 # define ULPI_PWR_OTG_COMP_DISABLE	BIT(0)
 
diff --git a/drivers/phy/phy-qcom-usb-hsic.c b/drivers/phy/qualcomm/phy-qcom-usb-hsic.c
index 47690f9945b9..c110563a73cb 100644
--- a/drivers/phy/phy-qcom-usb-hsic.c
+++ b/drivers/phy/qualcomm/phy-qcom-usb-hsic.c
@@ -8,13 +8,12 @@
 #include <linux/module.h>
 #include <linux/ulpi/driver.h>
 #include <linux/ulpi/regs.h>
+#include <linux/phy/phy.h>
 #include <linux/pinctrl/consumer.h>
 #include <linux/pinctrl/pinctrl-state.h>
 #include <linux/delay.h>
 #include <linux/clk.h>
 
-#include "ulpi_phy.h"
-
 #define ULPI_HSIC_CFG		0x30
 #define ULPI_HSIC_IO_CAL	0x33
 
diff --git a/drivers/phy/renesas/Kconfig b/drivers/phy/renesas/Kconfig
new file mode 100644
index 000000000000..cb09245e9b4c
--- /dev/null
+++ b/drivers/phy/renesas/Kconfig
@@ -0,0 +1,24 @@
+#
+# Phy drivers for Renesas platforms
+#
+config PHY_RCAR_GEN2
+	tristate "Renesas R-Car generation 2 USB PHY driver"
+	depends on ARCH_RENESAS
+	depends on GENERIC_PHY
+	help
+	  Support for USB PHY found on Renesas R-Car generation 2 SoCs.
+
+config PHY_RCAR_GEN3_USB2
+	tristate "Renesas R-Car generation 3 USB 2.0 PHY driver"
+	depends on ARCH_RENESAS
+	depends on EXTCON
+	select GENERIC_PHY
+	help
+	  Support for USB 2.0 PHY found on Renesas R-Car generation 3 SoCs.
+
+config PHY_RCAR_GEN3_USB3
+	tristate "Renesas R-Car generation 3 USB 3.0 PHY driver"
+	depends on ARCH_RENESAS || COMPILE_TEST
+	select GENERIC_PHY
+	help
+	  Support for USB 3.0 PHY found on Renesas R-Car generation 3 SoCs.
diff --git a/drivers/phy/renesas/Makefile b/drivers/phy/renesas/Makefile
new file mode 100644
index 000000000000..8b6025916a93
--- /dev/null
+++ b/drivers/phy/renesas/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_PHY_RCAR_GEN2)		+= phy-rcar-gen2.o
+obj-$(CONFIG_PHY_RCAR_GEN3_USB2)	+= phy-rcar-gen3-usb2.o
+obj-$(CONFIG_PHY_RCAR_GEN3_USB3)	+= phy-rcar-gen3-usb3.o
diff --git a/drivers/phy/phy-rcar-gen2.c b/drivers/phy/renesas/phy-rcar-gen2.c
index 97d4dd6ea924..97d4dd6ea924 100644
--- a/drivers/phy/phy-rcar-gen2.c
+++ b/drivers/phy/renesas/phy-rcar-gen2.c
diff --git a/drivers/phy/phy-rcar-gen3-usb2.c b/drivers/phy/renesas/phy-rcar-gen3-usb2.c
index 54c34298a000..54c34298a000 100644
--- a/drivers/phy/phy-rcar-gen3-usb2.c
+++ b/drivers/phy/renesas/phy-rcar-gen3-usb2.c
diff --git a/drivers/phy/renesas/phy-rcar-gen3-usb3.c b/drivers/phy/renesas/phy-rcar-gen3-usb3.c
new file mode 100644
index 000000000000..88c83c9b8ff9
--- /dev/null
+++ b/drivers/phy/renesas/phy-rcar-gen3-usb3.c
@@ -0,0 +1,226 @@
+/*
+ * Renesas R-Car Gen3 for USB3.0 PHY driver
+ *
+ * Copyright (C) 2017 Renesas Electronics Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+
+#define USB30_CLKSET0		0x034
+#define USB30_CLKSET1		0x036
+#define USB30_SSC_SET		0x038
+#define USB30_PHY_ENABLE	0x060
+#define USB30_VBUS_EN		0x064
+
+/* USB30_CLKSET0 */
+#define CLKSET0_PRIVATE			0x05c0
+#define CLKSET0_USB30_FSEL_USB_EXTAL	0x0002
+
+/* USB30_CLKSET1 */
+#define CLKSET1_USB30_PLL_MULTI_SHIFT		6
+#define CLKSET1_USB30_PLL_MULTI_USB_EXTAL	(0x64 << \
+						 CLKSET1_USB30_PLL_MULTI_SHIFT)
+#define CLKSET1_PHYRESET	BIT(4)	/* 1: reset */
+#define CLKSET1_REF_CLKDIV	BIT(3)	/* 1: USB_EXTAL */
+#define CLKSET1_PRIVATE_2_1	BIT(1)	/* Write B'01 */
+#define CLKSET1_REF_CLK_SEL	BIT(0)	/* 1: USB3S0_CLK_P */
+
+/* USB30_SSC_SET */
+#define SSC_SET_SSC_EN		BIT(12)
+#define SSC_SET_RANGE_SHIFT	9
+#define SSC_SET_RANGE_4980	(0x0 << SSC_SET_RANGE_SHIFT)
+#define SSC_SET_RANGE_4492	(0x1 << SSC_SET_RANGE_SHIFT)
+#define SSC_SET_RANGE_4003	(0x2 << SSC_SET_RANGE_SHIFT)
+
+/* USB30_PHY_ENABLE */
+#define PHY_ENABLE_RESET_EN	BIT(4)
+
+/* USB30_VBUS_EN */
+#define VBUS_EN_VBUS_EN		BIT(1)
+
+struct rcar_gen3_usb3 {
+	void __iomem *base;
+	struct phy *phy;
+	u32 ssc_range;
+	bool usb3s_clk;
+	bool usb_extal;
+};
+
+static void write_clkset1_for_usb_extal(struct rcar_gen3_usb3 *r, bool reset)
+{
+	u16 val = CLKSET1_USB30_PLL_MULTI_USB_EXTAL |
+		  CLKSET1_REF_CLKDIV | CLKSET1_PRIVATE_2_1;
+
+	if (reset)
+		val |= CLKSET1_PHYRESET;
+
+	writew(val, r->base + USB30_CLKSET1);
+}
+
+static void rcar_gen3_phy_usb3_enable_ssc(struct rcar_gen3_usb3 *r)
+{
+	u16 val = SSC_SET_SSC_EN;
+
+	switch (r->ssc_range) {
+	case 4980:
+		val |= SSC_SET_RANGE_4980;
+		break;
+	case 4492:
+		val |= SSC_SET_RANGE_4492;
+		break;
+	case 4003:
+		val |= SSC_SET_RANGE_4003;
+		break;
+	default:
+		dev_err(&r->phy->dev, "%s: unsupported range (%x)\n", __func__,
+			r->ssc_range);
+		return;
+	}
+
+	writew(val, r->base + USB30_SSC_SET);
+}
+
+static void rcar_gen3_phy_usb3_select_usb_extal(struct rcar_gen3_usb3 *r)
+{
+	write_clkset1_for_usb_extal(r, false);
+	if (r->ssc_range)
+		rcar_gen3_phy_usb3_enable_ssc(r);
+	writew(CLKSET0_PRIVATE | CLKSET0_USB30_FSEL_USB_EXTAL,
+	       r->base + USB30_CLKSET0);
+	writew(PHY_ENABLE_RESET_EN, r->base + USB30_PHY_ENABLE);
+	write_clkset1_for_usb_extal(r, true);
+	usleep_range(10, 20);
+	write_clkset1_for_usb_extal(r, false);
+}
+
+static int rcar_gen3_phy_usb3_init(struct phy *p)
+{
+	struct rcar_gen3_usb3 *r = phy_get_drvdata(p);
+
+	dev_vdbg(&r->phy->dev, "%s: enter (%d, %d, %d)\n", __func__,
+		 r->usb3s_clk, r->usb_extal, r->ssc_range);
+
+	if (!r->usb3s_clk && r->usb_extal)
+		rcar_gen3_phy_usb3_select_usb_extal(r);
+
+	/* Enables VBUS detection anyway */
+	writew(VBUS_EN_VBUS_EN, r->base + USB30_VBUS_EN);
+
+	return 0;
+}
+
+static const struct phy_ops rcar_gen3_phy_usb3_ops = {
+	.init		= rcar_gen3_phy_usb3_init,
+	.owner		= THIS_MODULE,
+};
+
+static const struct of_device_id rcar_gen3_phy_usb3_match_table[] = {
+	{ .compatible = "renesas,rcar-gen3-usb3-phy" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, rcar_gen3_phy_usb3_match_table);
+
+static int rcar_gen3_phy_usb3_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct rcar_gen3_usb3 *r;
+	struct phy_provider *provider;
+	struct resource *res;
+	int ret = 0;
+	struct clk *clk;
+
+	if (!dev->of_node) {
+		dev_err(dev, "This driver needs device tree\n");
+		return -EINVAL;
+	}
+
+	r = devm_kzalloc(dev, sizeof(*r), GFP_KERNEL);
+	if (!r)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	r->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(r->base))
+		return PTR_ERR(r->base);
+
+	clk = devm_clk_get(dev, "usb3s_clk");
+	if (!IS_ERR(clk) && !clk_prepare_enable(clk)) {
+		r->usb3s_clk = !!clk_get_rate(clk);
+		clk_disable_unprepare(clk);
+	}
+	clk = devm_clk_get(dev, "usb_extal");
+	if (!IS_ERR(clk) && !clk_prepare_enable(clk)) {
+		r->usb_extal = !!clk_get_rate(clk);
+		clk_disable_unprepare(clk);
+	}
+
+	if (!r->usb3s_clk && !r->usb_extal) {
+		dev_err(dev, "This driver needs usb3s_clk and/or usb_extal\n");
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/*
+	 * devm_phy_create() will call pm_runtime_enable(&phy->dev);
+	 * And then, phy-core will manage runtime pm for this device.
+	 */
+	pm_runtime_enable(dev);
+
+	r->phy = devm_phy_create(dev, NULL, &rcar_gen3_phy_usb3_ops);
+	if (IS_ERR(r->phy)) {
+		dev_err(dev, "Failed to create USB3 PHY\n");
+		ret = PTR_ERR(r->phy);
+		goto error;
+	}
+
+	of_property_read_u32(dev->of_node, "renesas,ssc-range", &r->ssc_range);
+
+	platform_set_drvdata(pdev, r);
+	phy_set_drvdata(r->phy, r);
+
+	provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+	if (IS_ERR(provider)) {
+		dev_err(dev, "Failed to register PHY provider\n");
+		ret = PTR_ERR(provider);
+		goto error;
+	}
+
+	return 0;
+
+error:
+	pm_runtime_disable(dev);
+
+	return ret;
+}
+
+static int rcar_gen3_phy_usb3_remove(struct platform_device *pdev)
+{
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+};
+
+static struct platform_driver rcar_gen3_phy_usb3_driver = {
+	.driver = {
+		.name		= "phy_rcar_gen3_usb3",
+		.of_match_table	= rcar_gen3_phy_usb3_match_table,
+	},
+	.probe	= rcar_gen3_phy_usb3_probe,
+	.remove = rcar_gen3_phy_usb3_remove,
+};
+module_platform_driver(rcar_gen3_phy_usb3_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Renesas R-Car Gen3 USB 3.0 PHY");
+MODULE_AUTHOR("Yoshihiro Shimoda <yoshihiro.shimoda.uh@renesas.com>");
diff --git a/drivers/phy/rockchip/Kconfig b/drivers/phy/rockchip/Kconfig
new file mode 100644
index 000000000000..f5325b2b679e
--- /dev/null
+++ b/drivers/phy/rockchip/Kconfig
@@ -0,0 +1,51 @@
+#
+# Phy drivers for Rockchip platforms
+#
+config PHY_ROCKCHIP_DP
+	tristate "Rockchip Display Port PHY Driver"
+	depends on ARCH_ROCKCHIP && OF
+	select GENERIC_PHY
+	help
+	  Enable this to support the Rockchip Display Port PHY.
+
+config PHY_ROCKCHIP_EMMC
+	tristate "Rockchip EMMC PHY Driver"
+	depends on ARCH_ROCKCHIP && OF
+	select GENERIC_PHY
+	help
+	  Enable this to support the Rockchip EMMC PHY.
+
+config PHY_ROCKCHIP_INNO_USB2
+	tristate "Rockchip INNO USB2PHY Driver"
+	depends on (ARCH_ROCKCHIP || COMPILE_TEST) && OF
+	depends on COMMON_CLK
+	depends on EXTCON
+	depends on USB_SUPPORT
+	select GENERIC_PHY
+	select USB_COMMON
+	help
+	  Support for Rockchip USB2.0 PHY with Innosilicon IP block.
+
+config PHY_ROCKCHIP_PCIE
+	tristate "Rockchip PCIe PHY Driver"
+	depends on (ARCH_ROCKCHIP && OF) || COMPILE_TEST
+	select GENERIC_PHY
+	select MFD_SYSCON
+	help
+	  Enable this to support the Rockchip PCIe PHY.
+
+config PHY_ROCKCHIP_TYPEC
+	tristate "Rockchip TYPEC PHY Driver"
+	depends on OF && (ARCH_ROCKCHIP || COMPILE_TEST)
+	select EXTCON
+	select GENERIC_PHY
+	select RESET_CONTROLLER
+	help
+	  Enable this to support the Rockchip USB TYPEC PHY.
+
+config PHY_ROCKCHIP_USB
+	tristate "Rockchip USB2 PHY Driver"
+	depends on ARCH_ROCKCHIP && OF
+	select GENERIC_PHY
+	help
+	  Enable this to support the Rockchip USB 2.0 PHY.
diff --git a/drivers/phy/rockchip/Makefile b/drivers/phy/rockchip/Makefile
new file mode 100644
index 000000000000..bd0acdf38e0f
--- /dev/null
+++ b/drivers/phy/rockchip/Makefile
@@ -0,0 +1,6 @@
+obj-$(CONFIG_PHY_ROCKCHIP_DP)		+= phy-rockchip-dp.o
+obj-$(CONFIG_PHY_ROCKCHIP_EMMC)		+= phy-rockchip-emmc.o
+obj-$(CONFIG_PHY_ROCKCHIP_INNO_USB2)	+= phy-rockchip-inno-usb2.o
+obj-$(CONFIG_PHY_ROCKCHIP_PCIE)		+= phy-rockchip-pcie.o
+obj-$(CONFIG_PHY_ROCKCHIP_TYPEC)	+= phy-rockchip-typec.o
+obj-$(CONFIG_PHY_ROCKCHIP_USB)		+= phy-rockchip-usb.o
diff --git a/drivers/phy/phy-rockchip-dp.c b/drivers/phy/rockchip/phy-rockchip-dp.c
index 8b267a746576..8b267a746576 100644
--- a/drivers/phy/phy-rockchip-dp.c
+++ b/drivers/phy/rockchip/phy-rockchip-dp.c
diff --git a/drivers/phy/phy-rockchip-emmc.c b/drivers/phy/rockchip/phy-rockchip-emmc.c
index f1b24f18e9b2..f1b24f18e9b2 100644
--- a/drivers/phy/phy-rockchip-emmc.c
+++ b/drivers/phy/rockchip/phy-rockchip-emmc.c
diff --git a/drivers/phy/phy-rockchip-inno-usb2.c b/drivers/phy/rockchip/phy-rockchip-inno-usb2.c
index 8efe78a49916..626883d9d176 100644
--- a/drivers/phy/phy-rockchip-inno-usb2.c
+++ b/drivers/phy/rockchip/phy-rockchip-inno-usb2.c
@@ -406,7 +406,8 @@ static int rockchip_usb2phy_init(struct phy *phy)
 	mutex_lock(&rport->mutex);
 
 	if (rport->port_id == USB2PHY_PORT_OTG) {
-		if (rport->mode != USB_DR_MODE_HOST) {
+		if (rport->mode != USB_DR_MODE_HOST &&
+		    rport->mode != USB_DR_MODE_UNKNOWN) {
 			/* clear bvalid status and enable bvalid detect irq */
 			ret = property_enable(rphy,
 					      &rport->port_cfg->bvalid_det_clr,
@@ -421,7 +422,7 @@ static int rockchip_usb2phy_init(struct phy *phy)
 				goto out;
 
 			schedule_delayed_work(&rport->otg_sm_work,
-					      OTG_SCHEDULE_DELAY);
+					      OTG_SCHEDULE_DELAY * 3);
 		} else {
 			/* If OTG works in host only mode, do nothing. */
 			dev_dbg(&rport->phy->dev, "mode %d\n", rport->mode);
@@ -463,6 +464,9 @@ static int rockchip_usb2phy_power_on(struct phy *phy)
 	if (ret)
 		return ret;
 
+	/* waiting for the utmi_clk to become stable */
+	usleep_range(1500, 2000);
+
 	rport->suspended = false;
 	return 0;
 }
@@ -493,7 +497,8 @@ static int rockchip_usb2phy_exit(struct phy *phy)
 	struct rockchip_usb2phy_port *rport = phy_get_drvdata(phy);
 
 	if (rport->port_id == USB2PHY_PORT_OTG &&
-	    rport->mode != USB_DR_MODE_HOST) {
+	    rport->mode != USB_DR_MODE_HOST &&
+	    rport->mode != USB_DR_MODE_UNKNOWN) {
 		cancel_delayed_work_sync(&rport->otg_sm_work);
 		cancel_delayed_work_sync(&rport->chg_work);
 	} else if (rport->port_id == USB2PHY_PORT_HOST)
@@ -970,7 +975,8 @@ static int rockchip_usb2phy_otg_port_init(struct rockchip_usb2phy *rphy,
 	mutex_init(&rport->mutex);
 
 	rport->mode = of_usb_get_dr_mode_by_phy(child_np, -1);
-	if (rport->mode == USB_DR_MODE_HOST) {
+	if (rport->mode == USB_DR_MODE_HOST ||
+	    rport->mode == USB_DR_MODE_UNKNOWN) {
 		ret = 0;
 		goto out;
 	}
@@ -1138,6 +1144,65 @@ disable_clks:
 	return ret;
 }
 
+static const struct rockchip_usb2phy_cfg rk3228_phy_cfgs[] = {
+	{
+		.reg = 0x760,
+		.num_ports	= 2,
+		.clkout_ctl	= { 0x0768, 4, 4, 1, 0 },
+		.port_cfgs	= {
+			[USB2PHY_PORT_OTG] = {
+				.phy_sus	= { 0x0760, 15, 0, 0, 0x1d1 },
+				.bvalid_det_en	= { 0x0680, 3, 3, 0, 1 },
+				.bvalid_det_st	= { 0x0690, 3, 3, 0, 1 },
+				.bvalid_det_clr	= { 0x06a0, 3, 3, 0, 1 },
+				.ls_det_en	= { 0x0680, 2, 2, 0, 1 },
+				.ls_det_st	= { 0x0690, 2, 2, 0, 1 },
+				.ls_det_clr	= { 0x06a0, 2, 2, 0, 1 },
+				.utmi_bvalid	= { 0x0480, 4, 4, 0, 1 },
+				.utmi_ls	= { 0x0480, 3, 2, 0, 1 },
+			},
+			[USB2PHY_PORT_HOST] = {
+				.phy_sus	= { 0x0764, 15, 0, 0, 0x1d1 },
+				.ls_det_en	= { 0x0680, 4, 4, 0, 1 },
+				.ls_det_st	= { 0x0690, 4, 4, 0, 1 },
+				.ls_det_clr	= { 0x06a0, 4, 4, 0, 1 }
+			}
+		},
+		.chg_det = {
+			.opmode		= { 0x0760, 3, 0, 5, 1 },
+			.cp_det		= { 0x0884, 4, 4, 0, 1 },
+			.dcp_det	= { 0x0884, 3, 3, 0, 1 },
+			.dp_det		= { 0x0884, 5, 5, 0, 1 },
+			.idm_sink_en	= { 0x0768, 8, 8, 0, 1 },
+			.idp_sink_en	= { 0x0768, 7, 7, 0, 1 },
+			.idp_src_en	= { 0x0768, 9, 9, 0, 1 },
+			.rdm_pdwn_en	= { 0x0768, 10, 10, 0, 1 },
+			.vdm_src_en	= { 0x0768, 12, 12, 0, 1 },
+			.vdp_src_en	= { 0x0768, 11, 11, 0, 1 },
+		},
+	},
+	{
+		.reg = 0x800,
+		.num_ports	= 2,
+		.clkout_ctl	= { 0x0808, 4, 4, 1, 0 },
+		.port_cfgs	= {
+			[USB2PHY_PORT_OTG] = {
+				.phy_sus	= { 0x800, 15, 0, 0, 0x1d1 },
+				.ls_det_en	= { 0x0684, 0, 0, 0, 1 },
+				.ls_det_st	= { 0x0694, 0, 0, 0, 1 },
+				.ls_det_clr	= { 0x06a4, 0, 0, 0, 1 }
+			},
+			[USB2PHY_PORT_HOST] = {
+				.phy_sus	= { 0x804, 15, 0, 0, 0x1d1 },
+				.ls_det_en	= { 0x0684, 1, 1, 0, 1 },
+				.ls_det_st	= { 0x0694, 1, 1, 0, 1 },
+				.ls_det_clr	= { 0x06a4, 1, 1, 0, 1 }
+			}
+		},
+	},
+	{ /* sentinel */ }
+};
+
 static const struct rockchip_usb2phy_cfg rk3328_phy_cfgs[] = {
 	{
 		.reg = 0x100,
@@ -1263,6 +1328,7 @@ static const struct rockchip_usb2phy_cfg rk3399_phy_cfgs[] = {
 };
 
 static const struct of_device_id rockchip_usb2phy_dt_match[] = {
+	{ .compatible = "rockchip,rk3228-usb2phy", .data = &rk3228_phy_cfgs },
 	{ .compatible = "rockchip,rk3328-usb2phy", .data = &rk3328_phy_cfgs },
 	{ .compatible = "rockchip,rk3366-usb2phy", .data = &rk3366_phy_cfgs },
 	{ .compatible = "rockchip,rk3399-usb2phy", .data = &rk3399_phy_cfgs },
diff --git a/drivers/phy/phy-rockchip-pcie.c b/drivers/phy/rockchip/phy-rockchip-pcie.c
index 6904633cad68..6904633cad68 100644
--- a/drivers/phy/phy-rockchip-pcie.c
+++ b/drivers/phy/rockchip/phy-rockchip-pcie.c
diff --git a/drivers/phy/phy-rockchip-typec.c b/drivers/phy/rockchip/phy-rockchip-typec.c
index 7cfb0f8995de..7cfb0f8995de 100644
--- a/drivers/phy/phy-rockchip-typec.c
+++ b/drivers/phy/rockchip/phy-rockchip-typec.c
diff --git a/drivers/phy/phy-rockchip-usb.c b/drivers/phy/rockchip/phy-rockchip-usb.c
index 3378eeb7a562..3378eeb7a562 100644
--- a/drivers/phy/phy-rockchip-usb.c
+++ b/drivers/phy/rockchip/phy-rockchip-usb.c
diff --git a/drivers/phy/samsung/Kconfig b/drivers/phy/samsung/Kconfig
new file mode 100644
index 000000000000..b7e0645a7bd9
--- /dev/null
+++ b/drivers/phy/samsung/Kconfig
@@ -0,0 +1,95 @@
+#
+# Phy drivers for Samsung platforms
+#
+config PHY_EXYNOS_DP_VIDEO
+	tristate "EXYNOS SoC series Display Port PHY driver"
+	depends on OF
+	depends on ARCH_EXYNOS || COMPILE_TEST
+	default ARCH_EXYNOS
+	select GENERIC_PHY
+	help
+	  Support for Display Port PHY found on Samsung EXYNOS SoCs.
+
+config PHY_EXYNOS_MIPI_VIDEO
+	tristate "S5P/EXYNOS SoC series MIPI CSI-2/DSI PHY driver"
+	depends on HAS_IOMEM
+	depends on ARCH_S5PV210 || ARCH_EXYNOS || COMPILE_TEST
+	select GENERIC_PHY
+	default y if ARCH_S5PV210 || ARCH_EXYNOS
+	help
+	  Support for MIPI CSI-2 and MIPI DSI DPHY found on Samsung S5P
+	  and EXYNOS SoCs.
+
+config PHY_EXYNOS_PCIE
+	bool "Exynos PCIe PHY driver"
+	depends on OF && (ARCH_EXYNOS || COMPILE_TEST)
+	select GENERIC_PHY
+	help
+	  Enable PCIe PHY support for Exynos SoC series.
+	  This driver provides PHY interface for Exynos PCIe controller.
+
+config PHY_SAMSUNG_USB2
+	tristate "Samsung USB 2.0 PHY driver"
+	depends on HAS_IOMEM
+	depends on USB_EHCI_EXYNOS || USB_OHCI_EXYNOS || USB_DWC2
+	select GENERIC_PHY
+	select MFD_SYSCON
+	default ARCH_EXYNOS
+	help
+	  Enable this to support the Samsung USB 2.0 PHY driver for Samsung
+	  SoCs. This driver provides the interface for USB 2.0 PHY. Support
+	  for particular PHYs will be enabled based on the SoC type in addition
+	  to this driver.
+
+config PHY_EXYNOS4210_USB2
+	bool
+	depends on PHY_SAMSUNG_USB2
+	default CPU_EXYNOS4210
+
+config PHY_EXYNOS4X12_USB2
+	bool
+	depends on PHY_SAMSUNG_USB2
+	default SOC_EXYNOS3250 || SOC_EXYNOS4212 || SOC_EXYNOS4412
+
+config PHY_EXYNOS5250_USB2
+	bool
+	depends on PHY_SAMSUNG_USB2
+	default SOC_EXYNOS5250 || SOC_EXYNOS5420
+
+config PHY_S5PV210_USB2
+	bool "Support for S5PV210"
+	depends on PHY_SAMSUNG_USB2
+	depends on ARCH_S5PV210
+	help
+	  Enable USB PHY support for S5PV210. This option requires that Samsung
+	  USB 2.0 PHY driver is enabled and means that support for this
+	  particular SoC is compiled in the driver. In case of S5PV210 two phys
+	  are available - device and host.
+
+config PHY_EXYNOS5_USBDRD
+	tristate "Exynos5 SoC series USB DRD PHY driver"
+	depends on ARCH_EXYNOS && OF
+	depends on HAS_IOMEM
+	depends on USB_DWC3_EXYNOS
+	select GENERIC_PHY
+	select MFD_SYSCON
+	default y
+	help
+	  Enable USB DRD PHY support for Exynos 5 SoC series.
+	  This driver provides PHY interface for USB 3.0 DRD controller
+	  present on Exynos5 SoC series.
+
+config PHY_EXYNOS5250_SATA
+	tristate "Exynos5250 Sata SerDes/PHY driver"
+	depends on SOC_EXYNOS5250
+	depends on HAS_IOMEM
+	depends on OF
+	select GENERIC_PHY
+	select I2C
+	select I2C_S3C2410
+	select MFD_SYSCON
+	help
+	  Enable this to support SATA SerDes/Phy found on Samsung's
+	  Exynos5250 based SoCs.This SerDes/Phy supports SATA 1.5 Gb/s,
+	  SATA 3.0 Gb/s, SATA 6.0 Gb/s speeds. It supports one SATA host
+	  port to accept one SATA device.
diff --git a/drivers/phy/samsung/Makefile b/drivers/phy/samsung/Makefile
new file mode 100644
index 000000000000..20d7f2424772
--- /dev/null
+++ b/drivers/phy/samsung/Makefile
@@ -0,0 +1,11 @@
+obj-$(CONFIG_PHY_EXYNOS_DP_VIDEO)	+= phy-exynos-dp-video.o
+obj-$(CONFIG_PHY_EXYNOS_MIPI_VIDEO)	+= phy-exynos-mipi-video.o
+obj-$(CONFIG_PHY_EXYNOS_PCIE)		+= phy-exynos-pcie.o
+obj-$(CONFIG_PHY_SAMSUNG_USB2)		+= phy-exynos-usb2.o
+phy-exynos-usb2-y			+= phy-samsung-usb2.o
+phy-exynos-usb2-$(CONFIG_PHY_EXYNOS4210_USB2)	+= phy-exynos4210-usb2.o
+phy-exynos-usb2-$(CONFIG_PHY_EXYNOS4X12_USB2)	+= phy-exynos4x12-usb2.o
+phy-exynos-usb2-$(CONFIG_PHY_EXYNOS5250_USB2)	+= phy-exynos5250-usb2.o
+phy-exynos-usb2-$(CONFIG_PHY_S5PV210_USB2)	+= phy-s5pv210-usb2.o
+obj-$(CONFIG_PHY_EXYNOS5_USBDRD)	+= phy-exynos5-usbdrd.o
+obj-$(CONFIG_PHY_EXYNOS5250_SATA)	+= phy-exynos5250-sata.o
diff --git a/drivers/phy/phy-exynos-dp-video.c b/drivers/phy/samsung/phy-exynos-dp-video.c
index bb3279dbf88c..bb3279dbf88c 100644
--- a/drivers/phy/phy-exynos-dp-video.c
+++ b/drivers/phy/samsung/phy-exynos-dp-video.c
diff --git a/drivers/phy/phy-exynos-mipi-video.c b/drivers/phy/samsung/phy-exynos-mipi-video.c
index c198886f80a3..c198886f80a3 100644
--- a/drivers/phy/phy-exynos-mipi-video.c
+++ b/drivers/phy/samsung/phy-exynos-mipi-video.c
diff --git a/drivers/phy/phy-exynos-pcie.c b/drivers/phy/samsung/phy-exynos-pcie.c
index a89c12faff39..a89c12faff39 100644
--- a/drivers/phy/phy-exynos-pcie.c
+++ b/drivers/phy/samsung/phy-exynos-pcie.c
diff --git a/drivers/phy/phy-exynos4210-usb2.c b/drivers/phy/samsung/phy-exynos4210-usb2.c
index 1f50e1004828..1f50e1004828 100644
--- a/drivers/phy/phy-exynos4210-usb2.c
+++ b/drivers/phy/samsung/phy-exynos4210-usb2.c
diff --git a/drivers/phy/phy-exynos4x12-usb2.c b/drivers/phy/samsung/phy-exynos4x12-usb2.c
index 7f27a91acf87..7f27a91acf87 100644
--- a/drivers/phy/phy-exynos4x12-usb2.c
+++ b/drivers/phy/samsung/phy-exynos4x12-usb2.c
diff --git a/drivers/phy/phy-exynos5-usbdrd.c b/drivers/phy/samsung/phy-exynos5-usbdrd.c
index 7c41daa2c625..7c41daa2c625 100644
--- a/drivers/phy/phy-exynos5-usbdrd.c
+++ b/drivers/phy/samsung/phy-exynos5-usbdrd.c
diff --git a/drivers/phy/phy-exynos5250-sata.c b/drivers/phy/samsung/phy-exynos5250-sata.c
index 60e13afcd9b8..60e13afcd9b8 100644
--- a/drivers/phy/phy-exynos5250-sata.c
+++ b/drivers/phy/samsung/phy-exynos5250-sata.c
diff --git a/drivers/phy/phy-exynos5250-usb2.c b/drivers/phy/samsung/phy-exynos5250-usb2.c
index aad806272305..aad806272305 100644
--- a/drivers/phy/phy-exynos5250-usb2.c
+++ b/drivers/phy/samsung/phy-exynos5250-usb2.c
diff --git a/drivers/phy/phy-s5pv210-usb2.c b/drivers/phy/samsung/phy-s5pv210-usb2.c
index f6f72339bbc3..f6f72339bbc3 100644
--- a/drivers/phy/phy-s5pv210-usb2.c
+++ b/drivers/phy/samsung/phy-s5pv210-usb2.c
diff --git a/drivers/phy/phy-samsung-usb2.c b/drivers/phy/samsung/phy-samsung-usb2.c
index 1d22d93b552d..1d22d93b552d 100644
--- a/drivers/phy/phy-samsung-usb2.c
+++ b/drivers/phy/samsung/phy-samsung-usb2.c
diff --git a/drivers/phy/phy-samsung-usb2.h b/drivers/phy/samsung/phy-samsung-usb2.h
index 6563e7ca0ac4..6563e7ca0ac4 100644
--- a/drivers/phy/phy-samsung-usb2.h
+++ b/drivers/phy/samsung/phy-samsung-usb2.h
diff --git a/drivers/phy/st/Kconfig b/drivers/phy/st/Kconfig
new file mode 100644
index 000000000000..0814d3f87ec6
--- /dev/null
+++ b/drivers/phy/st/Kconfig
@@ -0,0 +1,33 @@
+#
+# Phy drivers for STMicro platforms
+#
+config PHY_MIPHY28LP
+	tristate "STMicroelectronics MIPHY28LP PHY driver for STiH407"
+	depends on ARCH_STI
+	select GENERIC_PHY
+	help
+	  Enable this to support the miphy transceiver (for SATA/PCIE/USB3)
+	  that is part of STMicroelectronics STiH407 SoC.
+
+config PHY_ST_SPEAR1310_MIPHY
+	tristate "ST SPEAR1310-MIPHY driver"
+	select GENERIC_PHY
+	depends on MACH_SPEAR1310 || COMPILE_TEST
+	help
+	  Support for ST SPEAr1310 MIPHY which can be used for PCIe and SATA.
+
+config PHY_ST_SPEAR1340_MIPHY
+	tristate "ST SPEAR1340-MIPHY driver"
+	select GENERIC_PHY
+	depends on MACH_SPEAR1340 || COMPILE_TEST
+	help
+	  Support for ST SPEAr1340 MIPHY which can be used for PCIe and SATA.
+
+config PHY_STIH407_USB
+	tristate "STMicroelectronics USB2 picoPHY driver for STiH407 family"
+	depends on RESET_CONTROLLER
+	depends on ARCH_STI || COMPILE_TEST
+	select GENERIC_PHY
+	help
+	  Enable this support to enable the picoPHY device used by USB2
+	  and USB3 controllers on STMicroelectronics STiH407 SoC families.
diff --git a/drivers/phy/st/Makefile b/drivers/phy/st/Makefile
new file mode 100644
index 000000000000..e2adfe2166d2
--- /dev/null
+++ b/drivers/phy/st/Makefile
@@ -0,0 +1,4 @@
+obj-$(CONFIG_PHY_MIPHY28LP) 		+= phy-miphy28lp.o
+obj-$(CONFIG_PHY_ST_SPEAR1310_MIPHY)	+= phy-spear1310-miphy.o
+obj-$(CONFIG_PHY_ST_SPEAR1340_MIPHY)	+= phy-spear1340-miphy.o
+obj-$(CONFIG_PHY_STIH407_USB)		+= phy-stih407-usb.o
diff --git a/drivers/phy/phy-miphy28lp.c b/drivers/phy/st/phy-miphy28lp.c
index 213e2e15339c..213e2e15339c 100644
--- a/drivers/phy/phy-miphy28lp.c
+++ b/drivers/phy/st/phy-miphy28lp.c
diff --git a/drivers/phy/phy-spear1310-miphy.c b/drivers/phy/st/phy-spear1310-miphy.c
index ed67e98e54ca..ed67e98e54ca 100644
--- a/drivers/phy/phy-spear1310-miphy.c
+++ b/drivers/phy/st/phy-spear1310-miphy.c
diff --git a/drivers/phy/phy-spear1340-miphy.c b/drivers/phy/st/phy-spear1340-miphy.c
index 97280c0cf612..97280c0cf612 100644
--- a/drivers/phy/phy-spear1340-miphy.c
+++ b/drivers/phy/st/phy-spear1340-miphy.c
diff --git a/drivers/phy/phy-stih407-usb.c b/drivers/phy/st/phy-stih407-usb.c
index b1f44ab669fb..b1f44ab669fb 100644
--- a/drivers/phy/phy-stih407-usb.c
+++ b/drivers/phy/st/phy-stih407-usb.c
diff --git a/drivers/phy/ti/Kconfig b/drivers/phy/ti/Kconfig
new file mode 100644
index 000000000000..20503562666c
--- /dev/null
+++ b/drivers/phy/ti/Kconfig
@@ -0,0 +1,78 @@
+#
+# Phy drivers for TI platforms
+#
+config PHY_DA8XX_USB
+	tristate "TI DA8xx USB PHY Driver"
+	depends on ARCH_DAVINCI_DA8XX
+	select GENERIC_PHY
+	select MFD_SYSCON
+	help
+	  Enable this to support the USB PHY on DA8xx SoCs.
+
+	  This driver controls both the USB 1.1 PHY and the USB 2.0 PHY.
+
+config PHY_DM816X_USB
+	tristate "TI dm816x USB PHY driver"
+	depends on ARCH_OMAP2PLUS
+	depends on USB_SUPPORT
+	select GENERIC_PHY
+	select USB_PHY
+	help
+	  Enable this for dm816x USB to work.
+
+config OMAP_CONTROL_PHY
+	tristate "OMAP CONTROL PHY Driver"
+	depends on ARCH_OMAP2PLUS || COMPILE_TEST
+	help
+	  Enable this to add support for the PHY part present in the control
+	  module. This driver has API to power on the USB2 PHY and to write to
+	  the mailbox. The mailbox is present only in omap4 and the register to
+	  power on the USB2 PHY is present in OMAP4 and OMAP5. OMAP5 has an
+	  additional register to power on USB3 PHY/SATA PHY/PCIE PHY
+	  (PIPE3 PHY).
+
+config OMAP_USB2
+	tristate "OMAP USB2 PHY Driver"
+	depends on ARCH_OMAP2PLUS
+	depends on USB_SUPPORT
+	select GENERIC_PHY
+	select USB_PHY
+	select OMAP_CONTROL_PHY
+	depends on OMAP_OCP2SCP
+	help
+	  Enable this to support the transceiver that is part of SOC. This
+	  driver takes care of all the PHY functionality apart from comparator.
+	  The USB OTG controller communicates with the comparator using this
+	  driver.
+
+config TI_PIPE3
+	tristate "TI PIPE3 PHY Driver"
+	depends on ARCH_OMAP2PLUS || COMPILE_TEST
+	select GENERIC_PHY
+	select OMAP_CONTROL_PHY
+	depends on OMAP_OCP2SCP
+	help
+	  Enable this to support the PIPE3 PHY that is part of TI SOCs. This
+	  driver takes care of all the PHY functionality apart from comparator.
+	  This driver interacts with the "OMAP Control PHY Driver" to power
+	  on/off the PHY.
+
+config PHY_TUSB1210
+	tristate "TI TUSB1210 ULPI PHY module"
+	depends on USB_ULPI_BUS
+	select GENERIC_PHY
+	help
+	  Support for TI TUSB1210 USB ULPI PHY.
+
+config TWL4030_USB
+	tristate "TWL4030 USB Transceiver Driver"
+	depends on TWL4030_CORE && REGULATOR_TWL4030 && USB_MUSB_OMAP2PLUS
+	depends on USB_SUPPORT
+	depends on USB_GADGET || !USB_GADGET # if USB_GADGET=m, this can't 'y'
+	select GENERIC_PHY
+	select USB_PHY
+	help
+	  Enable this to support the USB OTG transceiver on TWL4030
+	  family chips (including the TWL5030 and TPS659x0 devices).
+	  This transceiver supports high and full speed devices plus,
+	  in host mode, low speed.
diff --git a/drivers/phy/ti/Makefile b/drivers/phy/ti/Makefile
new file mode 100644
index 000000000000..0cc3a1a557a3
--- /dev/null
+++ b/drivers/phy/ti/Makefile
@@ -0,0 +1,7 @@
+obj-$(CONFIG_PHY_DA8XX_USB)		+= phy-da8xx-usb.o
+obj-$(CONFIG_PHY_DM816X_USB)		+= phy-dm816x-usb.o
+obj-$(CONFIG_OMAP_CONTROL_PHY)		+= phy-omap-control.o
+obj-$(CONFIG_OMAP_USB2)			+= phy-omap-usb2.o
+obj-$(CONFIG_TI_PIPE3)			+= phy-ti-pipe3.o
+obj-$(CONFIG_PHY_TUSB1210)		+= phy-tusb1210.o
+obj-$(CONFIG_TWL4030_USB)		+= phy-twl4030-usb.o
diff --git a/drivers/phy/phy-da8xx-usb.c b/drivers/phy/ti/phy-da8xx-usb.c
index 1b82bff6330f..1b82bff6330f 100644
--- a/drivers/phy/phy-da8xx-usb.c
+++ b/drivers/phy/ti/phy-da8xx-usb.c
diff --git a/drivers/phy/phy-dm816x-usb.c b/drivers/phy/ti/phy-dm816x-usb.c
index cbcce7cf0028..cbcce7cf0028 100644
--- a/drivers/phy/phy-dm816x-usb.c
+++ b/drivers/phy/ti/phy-dm816x-usb.c
diff --git a/drivers/phy/phy-omap-control.c b/drivers/phy/ti/phy-omap-control.c
index e9c41b3fa0ee..e9c41b3fa0ee 100644
--- a/drivers/phy/phy-omap-control.c
+++ b/drivers/phy/ti/phy-omap-control.c
diff --git a/drivers/phy/phy-omap-usb2.c b/drivers/phy/ti/phy-omap-usb2.c
index fe909fd8144f..fe909fd8144f 100644
--- a/drivers/phy/phy-omap-usb2.c
+++ b/drivers/phy/ti/phy-omap-usb2.c
diff --git a/drivers/phy/phy-ti-pipe3.c b/drivers/phy/ti/phy-ti-pipe3.c
index 9c84d32c6f60..9c84d32c6f60 100644
--- a/drivers/phy/phy-ti-pipe3.c
+++ b/drivers/phy/ti/phy-ti-pipe3.c
diff --git a/drivers/phy/phy-tusb1210.c b/drivers/phy/ti/phy-tusb1210.c
index 4f6d5e71507d..b8ec39ac4dfc 100644
--- a/drivers/phy/phy-tusb1210.c
+++ b/drivers/phy/ti/phy-tusb1210.c
@@ -11,9 +11,9 @@
  */
 #include <linux/module.h>
 #include <linux/ulpi/driver.h>
+#include <linux/ulpi/regs.h>
 #include <linux/gpio/consumer.h>
-
-#include "ulpi_phy.h"
+#include <linux/phy/ulpi_phy.h>
 
 #define TUSB1210_VENDOR_SPECIFIC2		0x80
 #define TUSB1210_VENDOR_SPECIFIC2_IHSTX_SHIFT	0
@@ -53,9 +53,43 @@ static int tusb1210_power_off(struct phy *phy)
 	return 0;
 }
 
+static int tusb1210_set_mode(struct phy *phy, enum phy_mode mode)
+{
+	struct tusb1210 *tusb = phy_get_drvdata(phy);
+	int ret;
+
+	ret = ulpi_read(tusb->ulpi, ULPI_OTG_CTRL);
+	if (ret < 0)
+		return ret;
+
+	switch (mode) {
+	case PHY_MODE_USB_HOST:
+		ret |= (ULPI_OTG_CTRL_DRVVBUS_EXT
+			| ULPI_OTG_CTRL_ID_PULLUP
+			| ULPI_OTG_CTRL_DP_PULLDOWN
+			| ULPI_OTG_CTRL_DM_PULLDOWN);
+		ulpi_write(tusb->ulpi, ULPI_OTG_CTRL, ret);
+		ret |= ULPI_OTG_CTRL_DRVVBUS;
+		break;
+	case PHY_MODE_USB_DEVICE:
+		ret &= ~(ULPI_OTG_CTRL_DRVVBUS
+			 | ULPI_OTG_CTRL_DP_PULLDOWN
+			 | ULPI_OTG_CTRL_DM_PULLDOWN);
+		ulpi_write(tusb->ulpi, ULPI_OTG_CTRL, ret);
+		ret &= ~ULPI_OTG_CTRL_DRVVBUS_EXT;
+		break;
+	default:
+		/* nothing */
+		return 0;
+	}
+
+	return ulpi_write(tusb->ulpi, ULPI_OTG_CTRL, ret);
+}
+
 static const struct phy_ops phy_ops = {
 	.power_on = tusb1210_power_on,
 	.power_off = tusb1210_power_off,
+	.set_mode = tusb1210_set_mode,
 	.owner = THIS_MODULE,
 };
 
@@ -125,7 +159,8 @@ static void tusb1210_remove(struct ulpi *ulpi)
 #define TI_VENDOR_ID 0x0451
 
 static const struct ulpi_device_id tusb1210_ulpi_id[] = {
-	{ TI_VENDOR_ID, 0x1507, },
+	{ TI_VENDOR_ID, 0x1507, },  /* TUSB1210 */
+	{ TI_VENDOR_ID, 0x1508, },  /* TUSB1211 */
 	{ },
 };
 MODULE_DEVICE_TABLE(ulpi, tusb1210_ulpi_id);
diff --git a/drivers/phy/phy-twl4030-usb.c b/drivers/phy/ti/phy-twl4030-usb.c
index 2990b3965460..2990b3965460 100644
--- a/drivers/phy/phy-twl4030-usb.c
+++ b/drivers/phy/ti/phy-twl4030-usb.c
diff --git a/drivers/phy/ulpi_phy.h b/drivers/phy/ulpi_phy.h
deleted file mode 100644
index f2ebe490a4bc..000000000000
--- a/drivers/phy/ulpi_phy.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#include <linux/phy/phy.h>
-
-/**
- * Helper that registers PHY for a ULPI device and adds a lookup for binding it
- * and it's controller, which is always the parent.
- */
-static inline struct phy
-*ulpi_phy_create(struct ulpi *ulpi, const struct phy_ops *ops)
-{
-	struct phy *phy;
-	int ret;
-
-	phy = phy_create(&ulpi->dev, NULL, ops);
-	if (IS_ERR(phy))
-		return phy;
-
-	ret = phy_create_lookup(phy, "usb2-phy", dev_name(ulpi->dev.parent));
-	if (ret) {
-		phy_destroy(phy);
-		return ERR_PTR(ret);
-	}
-
-	return phy;
-}
-
-/* Remove a PHY that was created with ulpi_phy_create() and it's lookup. */
-static inline void ulpi_phy_destroy(struct ulpi *ulpi, struct phy *phy)
-{
-	phy_remove_lookup(phy, "usb2-phy", dev_name(ulpi->dev.parent));
-	phy_destroy(phy);
-}
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index 37af5e3029d5..e14b46c7b37f 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -146,6 +146,20 @@ config PINCTRL_FALCON
 	depends on SOC_FALCON
 	depends on PINCTRL_LANTIQ
 
+config PINCTRL_MCP23S08
+	tristate "Microchip MCP23xxx I/O expander"
+	depends on SPI_MASTER || I2C
+	depends on I2C || I2C=n
+	select GPIOLIB_IRQCHIP
+	select REGMAP_I2C if I2C
+	select REGMAP_SPI if SPI_MASTER
+	select GENERIC_PINCONF
+	help
+	  SPI/I2C driver for Microchip MCP23S08/MCP23S17/MCP23008/MCP23017
+	  I/O expanders.
+	  This provides a GPIO interface supporting inputs and outputs.
+	  The I2C versions of the chips can be used as interrupt-controller.
+
 config PINCTRL_MESON
 	bool
 	depends on OF
@@ -174,6 +188,17 @@ config PINCTRL_ROCKCHIP
 	select GENERIC_IRQ_CHIP
 	select MFD_SYSCON
 
+config PINCTRL_RZA1
+	bool "Renesas RZ/A1 gpio and pinctrl driver"
+	depends on OF
+	depends on ARCH_R7S72100 || COMPILE_TEST
+	select GPIOLIB
+	select GENERIC_PINCTRL_GROUPS
+	select GENERIC_PINMUX_FUNCTIONS
+	select GENERIC_PINCONF
+	help
+	  This selects pinctrl driver for Renesas RZ/A1 platforms.
+
 config PINCTRL_SINGLE
 	tristate "One-register-per-pin type device tree based pinctrl driver"
 	depends on OF
@@ -296,6 +321,16 @@ config PINCTRL_ZYNQ
 	help
 	  This selects the pinctrl driver for Xilinx Zynq.
 
+config PINCTRL_INGENIC
+	bool "Pinctrl driver for the Ingenic JZ47xx SoCs"
+	default y
+	depends on OF
+	depends on MACH_INGENIC || COMPILE_TEST
+	select GENERIC_PINCONF
+	select GENERIC_PINCTRL_GROUPS
+	select GENERIC_PINMUX_FUNCTIONS
+	select REGMAP_MMIO
+
 source "drivers/pinctrl/aspeed/Kconfig"
 source "drivers/pinctrl/bcm/Kconfig"
 source "drivers/pinctrl/berlin/Kconfig"
@@ -315,6 +350,7 @@ source "drivers/pinctrl/ti/Kconfig"
 source "drivers/pinctrl/uniphier/Kconfig"
 source "drivers/pinctrl/vt8500/Kconfig"
 source "drivers/pinctrl/mediatek/Kconfig"
+source "drivers/pinctrl/zte/Kconfig"
 
 config PINCTRL_XWAY
 	bool
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index 0e9b2226a7c2..2bc641d62400 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -19,12 +19,14 @@ obj-$(CONFIG_PINCTRL_DA850_PUPD) += pinctrl-da850-pupd.o
 obj-$(CONFIG_PINCTRL_DIGICOLOR)	+= pinctrl-digicolor.o
 obj-$(CONFIG_PINCTRL_FALCON)	+= pinctrl-falcon.o
 obj-$(CONFIG_PINCTRL_MAX77620)	+= pinctrl-max77620.o
+obj-$(CONFIG_PINCTRL_MCP23S08)	+= pinctrl-mcp23s08.o
 obj-$(CONFIG_PINCTRL_MESON)	+= meson/
 obj-$(CONFIG_PINCTRL_OXNAS)	+= pinctrl-oxnas.o
 obj-$(CONFIG_PINCTRL_PALMAS)	+= pinctrl-palmas.o
 obj-$(CONFIG_PINCTRL_PIC32)	+= pinctrl-pic32.o
 obj-$(CONFIG_PINCTRL_PISTACHIO)	+= pinctrl-pistachio.o
 obj-$(CONFIG_PINCTRL_ROCKCHIP)	+= pinctrl-rockchip.o
+obj-$(CONFIG_PINCTRL_RZA1)	+= pinctrl-rza1.o
 obj-$(CONFIG_PINCTRL_SINGLE)	+= pinctrl-single.o
 obj-$(CONFIG_PINCTRL_SIRF)	+= sirf/
 obj-$(CONFIG_PINCTRL_SX150X)	+= pinctrl-sx150x.o
@@ -39,6 +41,7 @@ obj-$(CONFIG_PINCTRL_LPC18XX)	+= pinctrl-lpc18xx.o
 obj-$(CONFIG_PINCTRL_TB10X)	+= pinctrl-tb10x.o
 obj-$(CONFIG_PINCTRL_ST) 	+= pinctrl-st.o
 obj-$(CONFIG_PINCTRL_ZYNQ)	+= pinctrl-zynq.o
+obj-$(CONFIG_PINCTRL_INGENIC)	+= pinctrl-ingenic.o
 
 obj-$(CONFIG_ARCH_ASPEED)	+= aspeed/
 obj-y				+= bcm/
@@ -58,3 +61,4 @@ obj-y				+= ti/
 obj-$(CONFIG_PINCTRL_UNIPHIER)	+= uniphier/
 obj-$(CONFIG_ARCH_VT8500)	+= vt8500/
 obj-$(CONFIG_PINCTRL_MTK)	+= mediatek/
+obj-$(CONFIG_PINCTRL_ZX)	+= zte/
diff --git a/drivers/pinctrl/bcm/pinctrl-bcm281xx.c b/drivers/pinctrl/bcm/pinctrl-bcm281xx.c
index 810a81786f62..a7cceffcedfa 100644
--- a/drivers/pinctrl/bcm/pinctrl-bcm281xx.c
+++ b/drivers/pinctrl/bcm/pinctrl-bcm281xx.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2013 Broadcom Corporation
+ * Copyright (C) 2013-2017 Broadcom
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License as
@@ -10,9 +10,10 @@
  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  */
+
 #include <linux/err.h>
 #include <linux/io.h>
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pinctrl/pinctrl.h>
@@ -1444,10 +1445,4 @@ static struct platform_driver bcm281xx_pinctrl_driver = {
 		.of_match_table = bcm281xx_pinctrl_of_match,
 	},
 };
-
-module_platform_driver_probe(bcm281xx_pinctrl_driver, bcm281xx_pinctrl_probe);
-
-MODULE_AUTHOR("Broadcom Corporation <bcm-kernel-feedback-list@broadcom.com>");
-MODULE_AUTHOR("Sherman Yin <syin@broadcom.com>");
-MODULE_DESCRIPTION("Broadcom BCM281xx pinctrl driver");
-MODULE_LICENSE("GPL v2");
+builtin_platform_driver_probe(bcm281xx_pinctrl_driver, bcm281xx_pinctrl_probe);
diff --git a/drivers/pinctrl/bcm/pinctrl-bcm2835.c b/drivers/pinctrl/bcm/pinctrl-bcm2835.c
index 85d009112864..230883168e99 100644
--- a/drivers/pinctrl/bcm/pinctrl-bcm2835.c
+++ b/drivers/pinctrl/bcm/pinctrl-bcm2835.c
@@ -27,7 +27,7 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/irqdesc.h>
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/of_address.h>
 #include <linux/of.h>
 #include <linux/of_irq.h>
@@ -1048,6 +1048,10 @@ static int bcm2835_pinctrl_probe(struct platform_device *pdev)
 	for (i = 0; i < BCM2835_NUM_IRQS; i++) {
 		pc->irq[i] = irq_of_parse_and_map(np, i);
 		pc->irq_group[i] = i;
+
+		if (pc->irq[i] == 0)
+			continue;
+
 		/*
 		 * Use the same handler for all groups: this is necessary
 		 * since we use one gpiochip to cover all lines - the
@@ -1075,31 +1079,17 @@ static int bcm2835_pinctrl_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int bcm2835_pinctrl_remove(struct platform_device *pdev)
-{
-	struct bcm2835_pinctrl *pc = platform_get_drvdata(pdev);
-
-	gpiochip_remove(&pc->gpio_chip);
-
-	return 0;
-}
-
 static const struct of_device_id bcm2835_pinctrl_match[] = {
 	{ .compatible = "brcm,bcm2835-gpio" },
 	{}
 };
-MODULE_DEVICE_TABLE(of, bcm2835_pinctrl_match);
 
 static struct platform_driver bcm2835_pinctrl_driver = {
 	.probe = bcm2835_pinctrl_probe,
-	.remove = bcm2835_pinctrl_remove,
 	.driver = {
 		.name = MODULE_NAME,
 		.of_match_table = bcm2835_pinctrl_match,
+		.suppress_bind_attrs = true,
 	},
 };
-module_platform_driver(bcm2835_pinctrl_driver);
-
-MODULE_AUTHOR("Chris Boot, Simon Arlott, Stephen Warren");
-MODULE_DESCRIPTION("BCM2835 Pin control driver");
-MODULE_LICENSE("GPL");
+builtin_platform_driver(bcm2835_pinctrl_driver);
diff --git a/drivers/pinctrl/bcm/pinctrl-cygnus-mux.c b/drivers/pinctrl/bcm/pinctrl-cygnus-mux.c
index d31c95701a92..44df35942a43 100644
--- a/drivers/pinctrl/bcm/pinctrl-cygnus-mux.c
+++ b/drivers/pinctrl/bcm/pinctrl-cygnus-mux.c
@@ -1,4 +1,5 @@
-/* Copyright (C) 2014-2015 Broadcom Corporation
+/*
+ * Copyright (C) 2014-2017 Broadcom
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License as
@@ -8,6 +9,10 @@
  * 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.
+ */
+
+/*
+ * Broadcom Cygnus IOMUX driver
  *
  * This file contains the Cygnus IOMUX driver that supports group based PINMUX
  * configuration. Although PINMUX configuration is mainly group based, the
@@ -17,7 +22,6 @@
 
 #include <linux/err.h>
 #include <linux/io.h>
-#include <linux/module.h>
 #include <linux/of.h>
 #include <linux/slab.h>
 #include <linux/platform_device.h>
@@ -1016,7 +1020,3 @@ static int __init cygnus_pinmux_init(void)
 	return platform_driver_register(&cygnus_pinmux_driver);
 }
 arch_initcall(cygnus_pinmux_init);
-
-MODULE_AUTHOR("Ray Jui <rjui@broadcom.com>");
-MODULE_DESCRIPTION("Broadcom Cygnus IOMUX driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/bcm/pinctrl-iproc-gpio.c b/drivers/pinctrl/bcm/pinctrl-iproc-gpio.c
index af5e904d4a1e..85a8c97d9dfe 100644
--- a/drivers/pinctrl/bcm/pinctrl-iproc-gpio.c
+++ b/drivers/pinctrl/bcm/pinctrl-iproc-gpio.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2014-2015 Broadcom Corporation
+ * Copyright (C) 2014-2017 Broadcom
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License as
@@ -9,7 +9,9 @@
  * kind, whether express or implied; without even the implied warranty
  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
+ */
+
+/*
  * This file contains the Broadcom Iproc GPIO driver that supports 3
  * GPIO controllers on Iproc including the ASIU GPIO controller, the
  * chipCommonG GPIO controller, and the always-on GPIO controller. Basic
diff --git a/drivers/pinctrl/bcm/pinctrl-nsp-gpio.c b/drivers/pinctrl/bcm/pinctrl-nsp-gpio.c
index 22442438275a..1cfe45fd391f 100644
--- a/drivers/pinctrl/bcm/pinctrl-nsp-gpio.c
+++ b/drivers/pinctrl/bcm/pinctrl-nsp-gpio.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2015 Broadcom Corporation
+ * Copyright (C) 2014-2017 Broadcom
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License as
@@ -9,7 +9,9 @@
  * kind, whether express or implied; without even the implied warranty
  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
+ */
+
+/*
  * This file contains the Broadcom Northstar Plus (NSP) GPIO driver that
  * supports the chipCommonA GPIO controller. Basic PINCONF such as bias,
  * pull up/down, slew and drive strength are also supported in this driver.
diff --git a/drivers/pinctrl/core.c b/drivers/pinctrl/core.c
index bd459a93b0e7..c5e2c5705058 100644
--- a/drivers/pinctrl/core.c
+++ b/drivers/pinctrl/core.c
@@ -170,7 +170,7 @@ const char *pin_get_name(struct pinctrl_dev *pctldev, const unsigned pin)
 	const struct pin_desc *desc;
 
 	desc = pin_desc_get(pctldev, pin);
-	if (desc == NULL) {
+	if (!desc) {
 		dev_err(pctldev->dev, "failed to get pin(%d) name\n",
 			pin);
 		return NULL;
@@ -214,7 +214,7 @@ static void pinctrl_free_pindescs(struct pinctrl_dev *pctldev,
 
 		pindesc = radix_tree_lookup(&pctldev->pin_desc_tree,
 					    pins[i].number);
-		if (pindesc != NULL) {
+		if (pindesc) {
 			radix_tree_delete(&pctldev->pin_desc_tree,
 					  pins[i].number);
 			if (pindesc->dynamic_name)
@@ -230,7 +230,7 @@ static int pinctrl_register_one_pin(struct pinctrl_dev *pctldev,
 	struct pin_desc *pindesc;
 
 	pindesc = pin_desc_get(pctldev, pin->number);
-	if (pindesc != NULL) {
+	if (pindesc) {
 		dev_err(pctldev->dev, "pin %d already registered\n",
 			pin->number);
 		return -EINVAL;
@@ -248,7 +248,7 @@ static int pinctrl_register_one_pin(struct pinctrl_dev *pctldev,
 		pindesc->name = pin->name;
 	} else {
 		pindesc->name = kasprintf(GFP_KERNEL, "PIN%u", pin->number);
-		if (pindesc->name == NULL) {
+		if (!pindesc->name) {
 			kfree(pindesc);
 			return -ENOMEM;
 		}
@@ -402,7 +402,7 @@ static int pinctrl_get_device_gpio_range(unsigned gpio,
 		struct pinctrl_gpio_range *range;
 
 		range = pinctrl_match_gpio_range(pctldev, gpio);
-		if (range != NULL) {
+		if (range) {
 			*outdev = pctldev;
 			*outrange = range;
 			mutex_unlock(&pinctrldev_list_mutex);
@@ -933,7 +933,7 @@ static int add_setting(struct pinctrl *p, struct pinctrl_dev *pctldev,
 	else
 		setting->pctldev =
 			get_pinctrl_dev_from_devname(map->ctrl_dev_name);
-	if (setting->pctldev == NULL) {
+	if (!setting->pctldev) {
 		kfree(setting);
 		/* Do not defer probing of hogs (circular loop) */
 		if (!strcmp(map->ctrl_dev_name, map->dev_name))
@@ -1024,6 +1024,16 @@ static struct pinctrl *create_pinctrl(struct device *dev,
 		/* Map must be for this device */
 		if (strcmp(map->dev_name, devname))
 			continue;
+		/*
+		 * If pctldev is not null, we are claiming hog for it,
+		 * that means, setting that is served by pctldev by itself.
+		 *
+		 * Thus we must skip map that is for this device but is served
+		 * by other device.
+		 */
+		if (pctldev &&
+		    strcmp(dev_name(pctldev->dev), map->ctrl_dev_name))
+			continue;
 
 		ret = add_setting(p, pctldev, map);
 		/*
@@ -1080,7 +1090,7 @@ struct pinctrl *pinctrl_get(struct device *dev)
 	 * return another pointer to it.
 	 */
 	p = find_pinctrl(dev);
-	if (p != NULL) {
+	if (p) {
 		dev_dbg(dev, "obtain a copy of previously claimed pinctrl\n");
 		kref_get(&p->users);
 		return p;
@@ -1551,7 +1561,7 @@ static int pinctrl_pins_show(struct seq_file *s, void *what)
 		pin = pctldev->desc->pins[i].number;
 		desc = pin_desc_get(pctldev, pin);
 		/* Pin space may be sparse */
-		if (desc == NULL)
+		if (!desc)
 			continue;
 
 		seq_printf(s, "pin %d (%s) ", pin, desc->name);
@@ -1718,7 +1728,7 @@ static int pinctrl_maps_show(struct seq_file *s, void *what)
 			break;
 		}
 
-		seq_printf(s, "\n");
+		seq_putc(s, '\n');
 	}
 	mutex_unlock(&pinctrl_maps_mutex);
 
@@ -2131,7 +2141,7 @@ void pinctrl_unregister(struct pinctrl_dev *pctldev)
 {
 	struct pinctrl_gpio_range *range, *n;
 
-	if (pctldev == NULL)
+	if (!pctldev)
 		return;
 
 	mutex_lock(&pctldev->mutex);
diff --git a/drivers/pinctrl/freescale/Kconfig b/drivers/pinctrl/freescale/Kconfig
index cae05e76c111..0b266b2aecd4 100644
--- a/drivers/pinctrl/freescale/Kconfig
+++ b/drivers/pinctrl/freescale/Kconfig
@@ -2,7 +2,7 @@ config PINCTRL_IMX
 	bool
 	select GENERIC_PINCTRL_GROUPS
 	select GENERIC_PINMUX_FUNCTIONS
-	select PINCONF
+	select GENERIC_PINCONF
 	select REGMAP
 
 config PINCTRL_IMX1_CORE
diff --git a/drivers/pinctrl/freescale/pinctrl-imx.c b/drivers/pinctrl/freescale/pinctrl-imx.c
index 74bd90dfd7b1..72aca758f4c6 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx.c
@@ -27,6 +27,7 @@
 #include <linux/regmap.h>
 
 #include "../core.h"
+#include "../pinconf.h"
 #include "../pinmux.h"
 #include "pinctrl-imx.h"
 
@@ -196,14 +197,16 @@ static int imx_pmx_set(struct pinctrl_dev *pctldev, unsigned selector,
 		if (info->flags & SHARE_MUX_CONF_REG) {
 			u32 reg;
 			reg = readl(ipctl->base + pin_reg->mux_reg);
-			reg &= ~(0x7 << 20);
-			reg |= (pin->mux_mode << 20);
+			reg &= ~info->mux_mask;
+			reg |= (pin->mux_mode << info->mux_shift);
 			writel(reg, ipctl->base + pin_reg->mux_reg);
+			dev_dbg(ipctl->dev, "write: offset 0x%x val 0x%x\n",
+				pin_reg->mux_reg, reg);
 		} else {
 			writel(pin->mux_mode, ipctl->base + pin_reg->mux_reg);
+			dev_dbg(ipctl->dev, "write: offset 0x%x val 0x%x\n",
+				pin_reg->mux_reg, pin->mux_mode);
 		}
-		dev_dbg(ipctl->dev, "write: offset 0x%x val 0x%x\n",
-			pin_reg->mux_reg, pin->mux_mode);
 
 		/*
 		 * If the select input value begins with 0xff, it's a quirky
@@ -287,7 +290,7 @@ static int imx_pmx_gpio_request_enable(struct pinctrl_dev *pctldev,
 
 mux_pin:
 	reg = readl(ipctl->base + pin_reg->mux_reg);
-	reg &= ~(0x7 << 20);
+	reg &= ~info->mux_mask;
 	reg |= imx_pin->config;
 	writel(reg, ipctl->base + pin_reg->mux_reg);
 
@@ -359,6 +362,62 @@ static const struct pinmux_ops imx_pmx_ops = {
 	.gpio_set_direction = imx_pmx_gpio_set_direction,
 };
 
+/* decode generic config into raw register values */
+static u32 imx_pinconf_decode_generic_config(struct imx_pinctrl *ipctl,
+					      unsigned long *configs,
+					      unsigned int num_configs)
+{
+	struct imx_pinctrl_soc_info *info = ipctl->info;
+	struct imx_cfg_params_decode *decode;
+	enum pin_config_param param;
+	u32 raw_config = 0;
+	u32 param_val;
+	int i, j;
+
+	WARN_ON(num_configs > info->num_decodes);
+
+	for (i = 0; i < num_configs; i++) {
+		param = pinconf_to_config_param(configs[i]);
+		param_val = pinconf_to_config_argument(configs[i]);
+		decode = info->decodes;
+		for (j = 0; j < info->num_decodes; j++) {
+			if (param == decode->param) {
+				if (decode->invert)
+					param_val = !param_val;
+				raw_config |= (param_val << decode->shift)
+					      & decode->mask;
+				break;
+			}
+			decode++;
+		}
+	}
+
+	if (info->fixup)
+		info->fixup(configs, num_configs, &raw_config);
+
+	return raw_config;
+}
+
+static u32 imx_pinconf_parse_generic_config(struct device_node *np,
+					    struct imx_pinctrl *ipctl)
+{
+	struct imx_pinctrl_soc_info *info = ipctl->info;
+	struct pinctrl_dev *pctl = ipctl->pctl;
+	unsigned int num_configs;
+	unsigned long *configs;
+	int ret;
+
+	if (!info->generic_pinconf)
+		return 0;
+
+	ret = pinconf_generic_parse_dt_config(np, pctl, &configs,
+					      &num_configs);
+	if (ret)
+		return 0;
+
+	return imx_pinconf_decode_generic_config(ipctl, configs, num_configs);
+}
+
 static int imx_pinconf_get(struct pinctrl_dev *pctldev,
 			     unsigned pin_id, unsigned long *config)
 {
@@ -375,7 +434,7 @@ static int imx_pinconf_get(struct pinctrl_dev *pctldev,
 	*config = readl(ipctl->base + pin_reg->conf_reg);
 
 	if (info->flags & SHARE_MUX_CONF_REG)
-		*config &= 0xffff;
+		*config &= ~info->mux_mask;
 
 	return 0;
 }
@@ -402,14 +461,16 @@ static int imx_pinconf_set(struct pinctrl_dev *pctldev,
 		if (info->flags & SHARE_MUX_CONF_REG) {
 			u32 reg;
 			reg = readl(ipctl->base + pin_reg->conf_reg);
-			reg &= ~0xffff;
+			reg &= info->mux_mask;
 			reg |= configs[i];
 			writel(reg, ipctl->base + pin_reg->conf_reg);
+			dev_dbg(ipctl->dev, "write: offset 0x%x val 0x%x\n",
+				pin_reg->conf_reg, reg);
 		} else {
 			writel(configs[i], ipctl->base + pin_reg->conf_reg);
+			dev_dbg(ipctl->dev, "write: offset 0x%x val 0x%lx\n",
+				pin_reg->conf_reg, configs[i]);
 		}
-		dev_dbg(ipctl->dev, "write: offset 0x%x val 0x%lx\n",
-			pin_reg->conf_reg, configs[i]);
 	} /* for each config */
 
 	return 0;
@@ -475,9 +536,10 @@ static const struct pinconf_ops imx_pinconf_ops = {
 
 static int imx_pinctrl_parse_groups(struct device_node *np,
 				    struct group_desc *grp,
-				    struct imx_pinctrl_soc_info *info,
+				    struct imx_pinctrl *ipctl,
 				    u32 index)
 {
+	struct imx_pinctrl_soc_info *info = ipctl->info;
 	int size, pin_size;
 	const __be32 *list;
 	int i;
@@ -489,25 +551,44 @@ static int imx_pinctrl_parse_groups(struct device_node *np,
 		pin_size = SHARE_FSL_PIN_SIZE;
 	else
 		pin_size = FSL_PIN_SIZE;
+
+	if (info->generic_pinconf)
+		pin_size -= 4;
+
 	/* Initialise group */
 	grp->name = np->name;
 
 	/*
 	 * the binding format is fsl,pins = <PIN_FUNC_ID CONFIG ...>,
 	 * do sanity check and calculate pins number
+	 *
+	 * First try legacy 'fsl,pins' property, then fall back to the
+	 * generic 'pins'.
+	 *
+	 * Note: for generic 'pins' case, there's no CONFIG part in
+	 * the binding format.
 	 */
 	list = of_get_property(np, "fsl,pins", &size);
 	if (!list) {
-		dev_err(info->dev, "no fsl,pins property in node %s\n", np->full_name);
-		return -EINVAL;
+		list = of_get_property(np, "pins", &size);
+		if (!list) {
+			dev_err(info->dev,
+				"no fsl,pins and pins property in node %s\n",
+				np->full_name);
+			return -EINVAL;
+		}
 	}
 
 	/* we do not check return since it's safe node passed down */
 	if (!size || size % pin_size) {
-		dev_err(info->dev, "Invalid fsl,pins property in node %s\n", np->full_name);
+		dev_err(info->dev, "Invalid fsl,pins or pins property in node %s\n",
+			np->full_name);
 		return -EINVAL;
 	}
 
+	/* first try to parse the generic pin config */
+	config = imx_pinconf_parse_generic_config(np, ipctl);
+
 	grp->num_pins = size / pin_size;
 	grp->data = devm_kzalloc(info->dev, grp->num_pins *
 				 sizeof(struct imx_pin), GFP_KERNEL);
@@ -544,11 +625,18 @@ static int imx_pinctrl_parse_groups(struct device_node *np,
 		pin->mux_mode = be32_to_cpu(*list++);
 		pin->input_val = be32_to_cpu(*list++);
 
-		/* SION bit is in mux register */
-		config = be32_to_cpu(*list++);
-		if (config & IMX_PAD_SION)
-			pin->mux_mode |= IOMUXC_CONFIG_SION;
-		pin->config = config & ~IMX_PAD_SION;
+		if (info->generic_pinconf) {
+			/* generic pin config decoded */
+			pin->config = config;
+		} else {
+			/* legacy pin config read from devicetree */
+			config = be32_to_cpu(*list++);
+
+			/* SION bit is in mux register */
+			if (config & IMX_PAD_SION)
+				pin->mux_mode |= IOMUXC_CONFIG_SION;
+			pin->config = config & ~IMX_PAD_SION;
+		}
 
 		dev_dbg(info->dev, "%s: 0x%x 0x%08lx", info->pins[pin_id].name,
 				pin->mux_mode, pin->config);
@@ -581,9 +669,10 @@ static int imx_pinctrl_parse_functions(struct device_node *np,
 		dev_err(info->dev, "no groups defined in %s\n", np->full_name);
 		return -EINVAL;
 	}
-	func->group_names = devm_kzalloc(info->dev,
-					 func->num_group_names *
+	func->group_names = devm_kcalloc(info->dev, func->num_group_names,
 					 sizeof(char *), GFP_KERNEL);
+	if (!func->group_names)
+		return -ENOMEM;
 
 	for_each_child_of_node(np, child) {
 		func->group_names[i] = child->name;
@@ -598,7 +687,7 @@ static int imx_pinctrl_parse_functions(struct device_node *np,
 				  info->group_index++, grp);
 		mutex_unlock(&info->mutex);
 
-		imx_pinctrl_parse_groups(child, grp, info, i++);
+		imx_pinctrl_parse_groups(child, grp, ipctl, i++);
 	}
 
 	return 0;
@@ -769,6 +858,10 @@ int imx_pinctrl_probe(struct platform_device *pdev,
 	imx_pinctrl_desc->confops = &imx_pinconf_ops;
 	imx_pinctrl_desc->owner = THIS_MODULE;
 
+	/* for generic pinconf */
+	imx_pinctrl_desc->custom_params = info->custom_params;
+	imx_pinctrl_desc->num_custom_params = info->num_custom_params;
+
 	mutex_init(&info->mutex);
 
 	ipctl->info = info;
diff --git a/drivers/pinctrl/freescale/pinctrl-imx.h b/drivers/pinctrl/freescale/pinctrl-imx.h
index ff2d3e56b7c5..880bba7fd1ab 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx.h
+++ b/drivers/pinctrl/freescale/pinctrl-imx.h
@@ -15,6 +15,8 @@
 #ifndef __DRIVERS_PINCTRL_IMX_H
 #define __DRIVERS_PINCTRL_IMX_H
 
+#include <linux/pinctrl/pinconf-generic.h>
+
 struct platform_device;
 
 /**
@@ -44,6 +46,14 @@ struct imx_pin_reg {
 	s16 conf_reg;
 };
 
+/* decode a generic config into raw register value */
+struct imx_cfg_params_decode {
+	enum pin_config_param param;
+	u32 mask;
+	u8 shift;
+	bool invert;
+};
+
 struct imx_pinctrl_soc_info {
 	struct device *dev;
 	const struct pinctrl_pin_desc *pins;
@@ -53,8 +63,27 @@ struct imx_pinctrl_soc_info {
 	unsigned int flags;
 	const char *gpr_compatible;
 	struct mutex mutex;
+
+	/* MUX_MODE shift and mask in case SHARE_MUX_CONF_REG */
+	unsigned int mux_mask;
+	u8 mux_shift;
+
+	/* generic pinconf */
+	bool generic_pinconf;
+	const struct pinconf_generic_params *custom_params;
+	unsigned int num_custom_params;
+	struct imx_cfg_params_decode *decodes;
+	unsigned int num_decodes;
+	void (*fixup)(unsigned long *configs, unsigned int num_configs,
+		      u32 *raw_config);
 };
 
+#define IMX_CFG_PARAMS_DECODE(p, m, o) \
+	{ .param = p, .mask = m, .shift = o, .invert = false, }
+
+#define IMX_CFG_PARAMS_DECODE_INVERT(p, m, o) \
+	{ .param = p, .mask = m, .shift = o, .invert = true, }
+
 #define SHARE_MUX_CONF_REG	0x1
 #define ZERO_OFFSET_VALID	0x2
 
diff --git a/drivers/pinctrl/freescale/pinctrl-imx7d.c b/drivers/pinctrl/freescale/pinctrl-imx7d.c
index a465a66c3ef4..754159ee7b1e 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx7d.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx7d.c
@@ -358,19 +358,19 @@ static const struct pinctrl_pin_desc imx7d_lpsr_pinctrl_pads[] = {
 	IMX_PINCTRL_PIN(MX7D_PAD_GPIO1_IO07),
 };
 
-static struct imx_pinctrl_soc_info imx7d_pinctrl_info = {
+static const struct imx_pinctrl_soc_info imx7d_pinctrl_info = {
 	.pins = imx7d_pinctrl_pads,
 	.npins = ARRAY_SIZE(imx7d_pinctrl_pads),
 	.gpr_compatible = "fsl,imx7d-iomuxc-gpr",
 };
 
-static struct imx_pinctrl_soc_info imx7d_lpsr_pinctrl_info = {
+static const struct imx_pinctrl_soc_info imx7d_lpsr_pinctrl_info = {
 	.pins = imx7d_lpsr_pinctrl_pads,
 	.npins = ARRAY_SIZE(imx7d_lpsr_pinctrl_pads),
 	.flags = ZERO_OFFSET_VALID,
 };
 
-static struct of_device_id imx7d_pinctrl_of_match[] = {
+static const struct of_device_id imx7d_pinctrl_of_match[] = {
 	{ .compatible = "fsl,imx7d-iomuxc", .data = &imx7d_pinctrl_info, },
 	{ .compatible = "fsl,imx7d-iomuxc-lpsr", .data = &imx7d_lpsr_pinctrl_info },
 	{ /* sentinel */ }
diff --git a/drivers/pinctrl/freescale/pinctrl-vf610.c b/drivers/pinctrl/freescale/pinctrl-vf610.c
index 2b1e198e3092..3bd85564d1e4 100644
--- a/drivers/pinctrl/freescale/pinctrl-vf610.c
+++ b/drivers/pinctrl/freescale/pinctrl-vf610.c
@@ -299,6 +299,8 @@ static struct imx_pinctrl_soc_info vf610_pinctrl_info = {
 	.pins = vf610_pinctrl_pads,
 	.npins = ARRAY_SIZE(vf610_pinctrl_pads),
 	.flags = SHARE_MUX_CONF_REG | ZERO_OFFSET_VALID,
+	.mux_mask = 0x700000,
+	.mux_shift = 20,
 };
 
 static const struct of_device_id vf610_pinctrl_of_match[] = {
diff --git a/drivers/pinctrl/intel/Kconfig b/drivers/pinctrl/intel/Kconfig
index 396830a41127..b82d6ff3116f 100644
--- a/drivers/pinctrl/intel/Kconfig
+++ b/drivers/pinctrl/intel/Kconfig
@@ -56,6 +56,14 @@ config PINCTRL_BROXTON
 	  Broxton pinctrl driver provides an interface that allows
 	  configuring of SoC pins and using them as GPIOs.
 
+config PINCTRL_CANNONLAKE
+	tristate "Intel Cannon Lake PCH pinctrl and GPIO driver"
+	depends on ACPI
+	select PINCTRL_INTEL
+	help
+	  This pinctrl driver provides an interface that allows configuring
+	  of Intel Cannon Lake PCH pins and using them as GPIOs.
+
 config PINCTRL_GEMINILAKE
 	tristate "Intel Gemini Lake SoC pinctrl and GPIO driver"
 	depends on ACPI
diff --git a/drivers/pinctrl/intel/Makefile b/drivers/pinctrl/intel/Makefile
index 12f3af5b2ca5..81df3cf408e3 100644
--- a/drivers/pinctrl/intel/Makefile
+++ b/drivers/pinctrl/intel/Makefile
@@ -5,5 +5,6 @@ obj-$(CONFIG_PINCTRL_CHERRYVIEW)	+= pinctrl-cherryview.o
 obj-$(CONFIG_PINCTRL_MERRIFIELD)	+= pinctrl-merrifield.o
 obj-$(CONFIG_PINCTRL_INTEL)		+= pinctrl-intel.o
 obj-$(CONFIG_PINCTRL_BROXTON)		+= pinctrl-broxton.o
+obj-$(CONFIG_PINCTRL_CANNONLAKE)	+= pinctrl-cannonlake.o
 obj-$(CONFIG_PINCTRL_GEMINILAKE)	+= pinctrl-geminilake.o
 obj-$(CONFIG_PINCTRL_SUNRISEPOINT)	+= pinctrl-sunrisepoint.o
diff --git a/drivers/pinctrl/intel/pinctrl-cannonlake.c b/drivers/pinctrl/intel/pinctrl-cannonlake.c
new file mode 100644
index 000000000000..3bc609b67dc2
--- /dev/null
+++ b/drivers/pinctrl/intel/pinctrl-cannonlake.c
@@ -0,0 +1,442 @@
+/*
+ * Intel Cannon Lake PCH pinctrl/GPIO driver
+ *
+ * Copyright (C) 2017, Intel Corporation
+ * Author: Mika Westerberg <mika.westerberg@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/acpi.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-intel.h"
+
+#define CNL_PAD_OWN	0x020
+#define CNL_PADCFGLOCK	0x080
+#define CNL_HOSTSW_OWN	0x0b0
+#define CNL_GPI_IE	0x120
+
+#define CNL_GPP(r, s, e)				\
+	{						\
+		.reg_num = (r),				\
+		.base = (s),				\
+		.size = ((e) - (s) + 1),		\
+	}
+
+#define CNL_COMMUNITY(b, s, e, g)			\
+	{						\
+		.barno = (b),				\
+		.padown_offset = CNL_PAD_OWN,		\
+		.padcfglock_offset = CNL_PADCFGLOCK,	\
+		.hostown_offset = CNL_HOSTSW_OWN,	\
+		.ie_offset = CNL_GPI_IE,		\
+		.pin_base = (s),			\
+		.npins = ((e) - (s) + 1),		\
+		.gpps = (g),				\
+		.ngpps = ARRAY_SIZE(g),			\
+	}
+
+/* Cannon Lake-LP */
+static const struct pinctrl_pin_desc cnllp_pins[] = {
+	/* GPP_A */
+	PINCTRL_PIN(0, "RCINB"),
+	PINCTRL_PIN(1, "LAD_0"),
+	PINCTRL_PIN(2, "LAD_1"),
+	PINCTRL_PIN(3, "LAD_2"),
+	PINCTRL_PIN(4, "LAD_3"),
+	PINCTRL_PIN(5, "LFRAMEB"),
+	PINCTRL_PIN(6, "SERIRQ"),
+	PINCTRL_PIN(7, "PIRQAB"),
+	PINCTRL_PIN(8, "CLKRUNB"),
+	PINCTRL_PIN(9, "CLKOUT_LPC_0"),
+	PINCTRL_PIN(10, "CLKOUT_LPC_1"),
+	PINCTRL_PIN(11, "PMEB"),
+	PINCTRL_PIN(12, "BM_BUSYB"),
+	PINCTRL_PIN(13, "SUSWARNB_SUSPWRDNACK"),
+	PINCTRL_PIN(14, "SUS_STATB"),
+	PINCTRL_PIN(15, "SUSACKB"),
+	PINCTRL_PIN(16, "SD_1P8_SEL"),
+	PINCTRL_PIN(17, "SD_PWR_EN_B"),
+	PINCTRL_PIN(18, "ISH_GP_0"),
+	PINCTRL_PIN(19, "ISH_GP_1"),
+	PINCTRL_PIN(20, "ISH_GP_2"),
+	PINCTRL_PIN(21, "ISH_GP_3"),
+	PINCTRL_PIN(22, "ISH_GP_4"),
+	PINCTRL_PIN(23, "ISH_GP_5"),
+	PINCTRL_PIN(24, "ESPI_CLK_LOOPBK"),
+	/* GPP_B */
+	PINCTRL_PIN(25, "CORE_VID_0"),
+	PINCTRL_PIN(26, "CORE_VID_1"),
+	PINCTRL_PIN(27, "VRALERTB"),
+	PINCTRL_PIN(28, "CPU_GP_2"),
+	PINCTRL_PIN(29, "CPU_GP_3"),
+	PINCTRL_PIN(30, "SRCCLKREQB_0"),
+	PINCTRL_PIN(31, "SRCCLKREQB_1"),
+	PINCTRL_PIN(32, "SRCCLKREQB_2"),
+	PINCTRL_PIN(33, "SRCCLKREQB_3"),
+	PINCTRL_PIN(34, "SRCCLKREQB_4"),
+	PINCTRL_PIN(35, "SRCCLKREQB_5"),
+	PINCTRL_PIN(36, "EXT_PWR_GATEB"),
+	PINCTRL_PIN(37, "SLP_S0B"),
+	PINCTRL_PIN(38, "PLTRSTB"),
+	PINCTRL_PIN(39, "SPKR"),
+	PINCTRL_PIN(40, "GSPI0_CS0B"),
+	PINCTRL_PIN(41, "GSPI0_CLK"),
+	PINCTRL_PIN(42, "GSPI0_MISO"),
+	PINCTRL_PIN(43, "GSPI0_MOSI"),
+	PINCTRL_PIN(44, "GSPI1_CS0B"),
+	PINCTRL_PIN(45, "GSPI1_CLK"),
+	PINCTRL_PIN(46, "GSPI1_MISO"),
+	PINCTRL_PIN(47, "GSPI1_MOSI"),
+	PINCTRL_PIN(48, "SML1ALERTB"),
+	PINCTRL_PIN(49, "GSPI0_CLK_LOOPBK"),
+	PINCTRL_PIN(50, "GSPI1_CLK_LOOPBK"),
+	/* GPP_G */
+	PINCTRL_PIN(51, "SD3_CMD"),
+	PINCTRL_PIN(52, "SD3_D0_SD4_RCLK_P"),
+	PINCTRL_PIN(53, "SD3_D1_SD4_RCLK_N"),
+	PINCTRL_PIN(54, "SD3_D2"),
+	PINCTRL_PIN(55, "SD3_D3"),
+	PINCTRL_PIN(56, "SD3_CDB"),
+	PINCTRL_PIN(57, "SD3_CLK"),
+	PINCTRL_PIN(58, "SD3_WP"),
+	/* SPI */
+	PINCTRL_PIN(59, "SPI0_IO_2"),
+	PINCTRL_PIN(60, "SPI0_IO_3"),
+	PINCTRL_PIN(61, "SPI0_MOSI_IO_0"),
+	PINCTRL_PIN(62, "SPI0_MISO_IO_1"),
+	PINCTRL_PIN(63, "SPI0_TPM_CSB"),
+	PINCTRL_PIN(64, "SPI0_FLASH_0_CSB"),
+	PINCTRL_PIN(65, "SPI0_FLASH_1_CSB"),
+	PINCTRL_PIN(66, "SPI0_CLK"),
+	PINCTRL_PIN(67, "SPI0_CLK_LOOPBK"),
+	/* GPP_D */
+	PINCTRL_PIN(68, "SPI1_CSB"),
+	PINCTRL_PIN(69, "SPI1_CLK"),
+	PINCTRL_PIN(70, "SPI1_MISO_IO_1"),
+	PINCTRL_PIN(71, "SPI1_MOSI_IO_0"),
+	PINCTRL_PIN(72, "IMGCLKOUT_0"),
+	PINCTRL_PIN(73, "ISH_I2C0_SDA"),
+	PINCTRL_PIN(74, "ISH_I2C0_SCL"),
+	PINCTRL_PIN(75, "ISH_I2C1_SDA"),
+	PINCTRL_PIN(76, "ISH_I2C1_SCL"),
+	PINCTRL_PIN(77, "ISH_SPI_CSB"),
+	PINCTRL_PIN(78, "ISH_SPI_CLK"),
+	PINCTRL_PIN(79, "ISH_SPI_MISO"),
+	PINCTRL_PIN(80, "ISH_SPI_MOSI"),
+	PINCTRL_PIN(81, "ISH_UART0_RXD"),
+	PINCTRL_PIN(82, "ISH_UART0_TXD"),
+	PINCTRL_PIN(83, "ISH_UART0_RTSB"),
+	PINCTRL_PIN(84, "ISH_UART0_CTSB"),
+	PINCTRL_PIN(85, "DMIC_CLK_1"),
+	PINCTRL_PIN(86, "DMIC_DATA_1"),
+	PINCTRL_PIN(87, "DMIC_CLK_0"),
+	PINCTRL_PIN(88, "DMIC_DATA_0"),
+	PINCTRL_PIN(89, "SPI1_IO_2"),
+	PINCTRL_PIN(90, "SPI1_IO_3"),
+	PINCTRL_PIN(91, "SSP_MCLK"),
+	PINCTRL_PIN(92, "GSPI2_CLK_LOOPBK"),
+	/* GPP_F */
+	PINCTRL_PIN(93, "CNV_GNSS_PA_BLANKING"),
+	PINCTRL_PIN(94, "CNV_GNSS_FTA"),
+	PINCTRL_PIN(95, "CNV_GNSS_SYSCK"),
+	PINCTRL_PIN(96, "EMMC_HIP_MON"),
+	PINCTRL_PIN(97, "CNV_BRI_DT"),
+	PINCTRL_PIN(98, "CNV_BRI_RSP"),
+	PINCTRL_PIN(99, "CNV_RGI_DT"),
+	PINCTRL_PIN(100, "CNV_RGI_RSP"),
+	PINCTRL_PIN(101, "CNV_MFUART2_RXD"),
+	PINCTRL_PIN(102, "CNV_MFUART2_TXD"),
+	PINCTRL_PIN(103, "GPP_F_10"),
+	PINCTRL_PIN(104, "EMMC_CMD"),
+	PINCTRL_PIN(105, "EMMC_DATA_0"),
+	PINCTRL_PIN(106, "EMMC_DATA_1"),
+	PINCTRL_PIN(107, "EMMC_DATA_2"),
+	PINCTRL_PIN(108, "EMMC_DATA_3"),
+	PINCTRL_PIN(109, "EMMC_DATA_4"),
+	PINCTRL_PIN(110, "EMMC_DATA_5"),
+	PINCTRL_PIN(111, "EMMC_DATA_6"),
+	PINCTRL_PIN(112, "EMMC_DATA_7"),
+	PINCTRL_PIN(113, "EMMC_RCLK"),
+	PINCTRL_PIN(114, "EMMC_CLK"),
+	PINCTRL_PIN(115, "EMMC_RESETB"),
+	PINCTRL_PIN(116, "A4WP_PRESENT"),
+	/* GPP_H */
+	PINCTRL_PIN(117, "SSP2_SCLK"),
+	PINCTRL_PIN(118, "SSP2_SFRM"),
+	PINCTRL_PIN(119, "SSP2_TXD"),
+	PINCTRL_PIN(120, "SSP2_RXD"),
+	PINCTRL_PIN(121, "I2C2_SDA"),
+	PINCTRL_PIN(122, "I2C2_SCL"),
+	PINCTRL_PIN(123, "I2C3_SDA"),
+	PINCTRL_PIN(124, "I2C3_SCL"),
+	PINCTRL_PIN(125, "I2C4_SDA"),
+	PINCTRL_PIN(126, "I2C4_SCL"),
+	PINCTRL_PIN(127, "I2C5_SDA"),
+	PINCTRL_PIN(128, "I2C5_SCL"),
+	PINCTRL_PIN(129, "M2_SKT2_CFG_0"),
+	PINCTRL_PIN(130, "M2_SKT2_CFG_1"),
+	PINCTRL_PIN(131, "M2_SKT2_CFG_2"),
+	PINCTRL_PIN(132, "M2_SKT2_CFG_3"),
+	PINCTRL_PIN(133, "DDPF_CTRLCLK"),
+	PINCTRL_PIN(134, "DDPF_CTRLDATA"),
+	PINCTRL_PIN(135, "CPU_VCCIO_PWR_GATEB"),
+	PINCTRL_PIN(136, "TIMESYNC_0"),
+	PINCTRL_PIN(137, "IMGCLKOUT_1"),
+	PINCTRL_PIN(138, "GPPC_H_21"),
+	PINCTRL_PIN(139, "GPPC_H_22"),
+	PINCTRL_PIN(140, "GPPC_H_23"),
+	/* vGPIO */
+	PINCTRL_PIN(141, "CNV_BTEN"),
+	PINCTRL_PIN(142, "CNV_GNEN"),
+	PINCTRL_PIN(143, "CNV_WFEN"),
+	PINCTRL_PIN(144, "CNV_WCEN"),
+	PINCTRL_PIN(145, "CNV_BT_HOST_WAKEB"),
+	PINCTRL_PIN(146, "CNV_BT_IF_SELECT"),
+	PINCTRL_PIN(147, "vCNV_BT_UART_TXD"),
+	PINCTRL_PIN(148, "vCNV_BT_UART_RXD"),
+	PINCTRL_PIN(149, "vCNV_BT_UART_CTS_B"),
+	PINCTRL_PIN(150, "vCNV_BT_UART_RTS_B"),
+	PINCTRL_PIN(151, "vCNV_MFUART1_TXD"),
+	PINCTRL_PIN(152, "vCNV_MFUART1_RXD"),
+	PINCTRL_PIN(153, "vCNV_MFUART1_CTS_B"),
+	PINCTRL_PIN(154, "vCNV_MFUART1_RTS_B"),
+	PINCTRL_PIN(155, "vCNV_GNSS_UART_TXD"),
+	PINCTRL_PIN(156, "vCNV_GNSS_UART_RXD"),
+	PINCTRL_PIN(157, "vCNV_GNSS_UART_CTS_B"),
+	PINCTRL_PIN(158, "vCNV_GNSS_UART_RTS_B"),
+	PINCTRL_PIN(159, "vUART0_TXD"),
+	PINCTRL_PIN(160, "vUART0_RXD"),
+	PINCTRL_PIN(161, "vUART0_CTS_B"),
+	PINCTRL_PIN(162, "vUART0_RTS_B"),
+	PINCTRL_PIN(163, "vISH_UART0_TXD"),
+	PINCTRL_PIN(164, "vISH_UART0_RXD"),
+	PINCTRL_PIN(165, "vISH_UART0_CTS_B"),
+	PINCTRL_PIN(166, "vISH_UART0_RTS_B"),
+	PINCTRL_PIN(167, "vISH_UART1_TXD"),
+	PINCTRL_PIN(168, "vISH_UART1_RXD"),
+	PINCTRL_PIN(169, "vISH_UART1_CTS_B"),
+	PINCTRL_PIN(170, "vISH_UART1_RTS_B"),
+	PINCTRL_PIN(171, "vCNV_BT_I2S_BCLK"),
+	PINCTRL_PIN(172, "vCNV_BT_I2S_WS_SYNC"),
+	PINCTRL_PIN(173, "vCNV_BT_I2S_SDO"),
+	PINCTRL_PIN(174, "vCNV_BT_I2S_SDI"),
+	PINCTRL_PIN(175, "vSSP2_SCLK"),
+	PINCTRL_PIN(176, "vSSP2_SFRM"),
+	PINCTRL_PIN(177, "vSSP2_TXD"),
+	PINCTRL_PIN(178, "vSSP2_RXD"),
+	PINCTRL_PIN(179, "vCNV_GNSS_HOST_WAKEB"),
+	PINCTRL_PIN(180, "vSD3_CD_B"),
+	/* GPP_C */
+	PINCTRL_PIN(181, "SMBCLK"),
+	PINCTRL_PIN(182, "SMBDATA"),
+	PINCTRL_PIN(183, "SMBALERTB"),
+	PINCTRL_PIN(184, "SML0CLK"),
+	PINCTRL_PIN(185, "SML0DATA"),
+	PINCTRL_PIN(186, "SML0ALERTB"),
+	PINCTRL_PIN(187, "SML1CLK"),
+	PINCTRL_PIN(188, "SML1DATA"),
+	PINCTRL_PIN(189, "UART0_RXD"),
+	PINCTRL_PIN(190, "UART0_TXD"),
+	PINCTRL_PIN(191, "UART0_RTSB"),
+	PINCTRL_PIN(192, "UART0_CTSB"),
+	PINCTRL_PIN(193, "UART1_RXD"),
+	PINCTRL_PIN(194, "UART1_TXD"),
+	PINCTRL_PIN(195, "UART1_RTSB"),
+	PINCTRL_PIN(196, "UART1_CTSB"),
+	PINCTRL_PIN(197, "I2C0_SDA"),
+	PINCTRL_PIN(198, "I2C0_SCL"),
+	PINCTRL_PIN(199, "I2C1_SDA"),
+	PINCTRL_PIN(200, "I2C1_SCL"),
+	PINCTRL_PIN(201, "UART2_RXD"),
+	PINCTRL_PIN(202, "UART2_TXD"),
+	PINCTRL_PIN(203, "UART2_RTSB"),
+	PINCTRL_PIN(204, "UART2_CTSB"),
+	/* GPP_E */
+	PINCTRL_PIN(205, "SATAXPCIE_0"),
+	PINCTRL_PIN(206, "SATAXPCIE_1"),
+	PINCTRL_PIN(207, "SATAXPCIE_2"),
+	PINCTRL_PIN(208, "CPU_GP_0"),
+	PINCTRL_PIN(209, "SATA_DEVSLP_0"),
+	PINCTRL_PIN(210, "SATA_DEVSLP_1"),
+	PINCTRL_PIN(211, "SATA_DEVSLP_2"),
+	PINCTRL_PIN(212, "CPU_GP_1"),
+	PINCTRL_PIN(213, "SATA_LEDB"),
+	PINCTRL_PIN(214, "USB2_OCB_0"),
+	PINCTRL_PIN(215, "USB2_OCB_1"),
+	PINCTRL_PIN(216, "USB2_OCB_2"),
+	PINCTRL_PIN(217, "USB2_OCB_3"),
+	PINCTRL_PIN(218, "DDSP_HPD_0"),
+	PINCTRL_PIN(219, "DDSP_HPD_1"),
+	PINCTRL_PIN(220, "DDSP_HPD_2"),
+	PINCTRL_PIN(221, "DDSP_HPD_3"),
+	PINCTRL_PIN(222, "EDP_HPD"),
+	PINCTRL_PIN(223, "DDPB_CTRLCLK"),
+	PINCTRL_PIN(224, "DDPB_CTRLDATA"),
+	PINCTRL_PIN(225, "DDPC_CTRLCLK"),
+	PINCTRL_PIN(226, "DDPC_CTRLDATA"),
+	PINCTRL_PIN(227, "DDPD_CTRLCLK"),
+	PINCTRL_PIN(228, "DDPD_CTRLDATA"),
+	/* JTAG */
+	PINCTRL_PIN(229, "JTAG_TDO"),
+	PINCTRL_PIN(230, "JTAGX"),
+	PINCTRL_PIN(231, "PRDYB"),
+	PINCTRL_PIN(232, "PREQB"),
+	PINCTRL_PIN(233, "CPU_TRSTB"),
+	PINCTRL_PIN(234, "JTAG_TDI"),
+	PINCTRL_PIN(235, "JTAG_TMS"),
+	PINCTRL_PIN(236, "JTAG_TCK"),
+	PINCTRL_PIN(237, "ITP_PMODE"),
+	/* HVCMOS */
+	PINCTRL_PIN(238, "L_BKLTEN"),
+	PINCTRL_PIN(239, "L_BKLTCTL"),
+	PINCTRL_PIN(240, "L_VDDEN"),
+	PINCTRL_PIN(241, "SYS_PWROK"),
+	PINCTRL_PIN(242, "SYS_RESETB"),
+	PINCTRL_PIN(243, "MLK_RSTB"),
+};
+
+static const unsigned int cnllp_spi0_pins[] = { 40, 41, 42, 43, 7 };
+static const unsigned int cnllp_spi0_modes[] = { 1, 1, 1, 1, 2 };
+static const unsigned int cnllp_spi1_pins[] = { 44, 45, 46, 47, 11 };
+static const unsigned int cnllp_spi1_modes[] = { 1, 1, 1, 1, 2 };
+static const unsigned int cnllp_spi2_pins[] = { 77, 78, 79, 80, 83 };
+static const unsigned int cnllp_spi2_modes[] = { 3, 3, 3, 3, 2 };
+
+static const unsigned int cnllp_i2c0_pins[] = { 197, 198 };
+static const unsigned int cnllp_i2c1_pins[] = { 199, 200 };
+static const unsigned int cnllp_i2c2_pins[] = { 121, 122 };
+static const unsigned int cnllp_i2c3_pins[] = { 123, 124 };
+static const unsigned int cnllp_i2c4_pins[] = { 125, 126 };
+static const unsigned int cnllp_i2c5_pins[] = { 127, 128 };
+
+static const unsigned int cnllp_uart0_pins[] = { 189, 190, 191, 192 };
+static const unsigned int cnllp_uart1_pins[] = { 193, 194, 195, 196 };
+static const unsigned int cnllp_uart2_pins[] = { 201, 202, 203, 204 };
+
+static const struct intel_pingroup cnllp_groups[] = {
+	PIN_GROUP("spi0_grp", cnllp_spi0_pins, cnllp_spi0_modes),
+	PIN_GROUP("spi1_grp", cnllp_spi1_pins, cnllp_spi1_modes),
+	PIN_GROUP("spi2_grp", cnllp_spi2_pins, cnllp_spi2_modes),
+	PIN_GROUP("i2c0_grp", cnllp_i2c0_pins, 1),
+	PIN_GROUP("i2c1_grp", cnllp_i2c1_pins, 1),
+	PIN_GROUP("i2c2_grp", cnllp_i2c2_pins, 1),
+	PIN_GROUP("i2c3_grp", cnllp_i2c3_pins, 1),
+	PIN_GROUP("i2c4_grp", cnllp_i2c4_pins, 1),
+	PIN_GROUP("i2c5_grp", cnllp_i2c5_pins, 1),
+	PIN_GROUP("uart0_grp", cnllp_uart0_pins, 1),
+	PIN_GROUP("uart1_grp", cnllp_uart1_pins, 1),
+	PIN_GROUP("uart2_grp", cnllp_uart2_pins, 1),
+};
+
+static const char * const cnllp_spi0_groups[] = { "spi0_grp" };
+static const char * const cnllp_spi1_groups[] = { "spi1_grp" };
+static const char * const cnllp_spi2_groups[] = { "spi2_grp" };
+static const char * const cnllp_i2c0_groups[] = { "i2c0_grp" };
+static const char * const cnllp_i2c1_groups[] = { "i2c1_grp" };
+static const char * const cnllp_i2c2_groups[] = { "i2c2_grp" };
+static const char * const cnllp_i2c3_groups[] = { "i2c3_grp" };
+static const char * const cnllp_i2c4_groups[] = { "i2c4_grp" };
+static const char * const cnllp_i2c5_groups[] = { "i2c5_grp" };
+static const char * const cnllp_uart0_groups[] = { "uart0_grp" };
+static const char * const cnllp_uart1_groups[] = { "uart1_grp" };
+static const char * const cnllp_uart2_groups[] = { "uart2_grp" };
+
+static const struct intel_function cnllp_functions[] = {
+	FUNCTION("spi0", cnllp_spi0_groups),
+	FUNCTION("spi1", cnllp_spi1_groups),
+	FUNCTION("spi2", cnllp_spi2_groups),
+	FUNCTION("i2c0", cnllp_i2c0_groups),
+	FUNCTION("i2c1", cnllp_i2c1_groups),
+	FUNCTION("i2c2", cnllp_i2c2_groups),
+	FUNCTION("i2c3", cnllp_i2c3_groups),
+	FUNCTION("i2c4", cnllp_i2c4_groups),
+	FUNCTION("i2c5", cnllp_i2c5_groups),
+	FUNCTION("uart0", cnllp_uart0_groups),
+	FUNCTION("uart1", cnllp_uart1_groups),
+	FUNCTION("uart2", cnllp_uart2_groups),
+};
+
+static const struct intel_padgroup cnllp_community0_gpps[] = {
+	CNL_GPP(0, 0, 24),	/* GPP_A */
+	CNL_GPP(1, 25, 50),	/* GPP_B */
+	CNL_GPP(2, 51, 58),	/* GPP_G */
+	CNL_GPP(3, 59, 67),	/* SPI */
+};
+
+static const struct intel_padgroup cnllp_community1_gpps[] = {
+	CNL_GPP(0, 68, 92),	/* GPP_D */
+	CNL_GPP(1, 93, 116),	/* GPP_F */
+	CNL_GPP(2, 117, 140),	/* GPP_H */
+	CNL_GPP(3, 141, 172),	/* vGPIO */
+	CNL_GPP(4, 173, 180),	/* vGPIO */
+};
+
+static const struct intel_padgroup cnllp_community4_gpps[] = {
+	CNL_GPP(0, 181, 204),	/* GPP_C */
+	CNL_GPP(1, 205, 228),	/* GPP_E */
+	CNL_GPP(2, 229, 237),	/* JTAG */
+	CNL_GPP(3, 238, 243),	/* HVCMOS */
+};
+
+static const struct intel_community cnllp_communities[] = {
+	CNL_COMMUNITY(0, 0, 67, cnllp_community0_gpps),
+	CNL_COMMUNITY(1, 68, 180, cnllp_community1_gpps),
+	CNL_COMMUNITY(2, 181, 243, cnllp_community4_gpps),
+};
+
+static const struct intel_pinctrl_soc_data cnllp_soc_data = {
+	.pins = cnllp_pins,
+	.npins = ARRAY_SIZE(cnllp_pins),
+	.groups = cnllp_groups,
+	.ngroups = ARRAY_SIZE(cnllp_groups),
+	.functions = cnllp_functions,
+	.nfunctions = ARRAY_SIZE(cnllp_functions),
+	.communities = cnllp_communities,
+	.ncommunities = ARRAY_SIZE(cnllp_communities),
+};
+
+static const struct acpi_device_id cnl_pinctrl_acpi_match[] = {
+	{ "INT34BB", (kernel_ulong_t)&cnllp_soc_data },
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, cnl_pinctrl_acpi_match);
+
+static int cnl_pinctrl_probe(struct platform_device *pdev)
+{
+	const struct intel_pinctrl_soc_data *soc_data;
+	const struct acpi_device_id *id;
+
+	id = acpi_match_device(cnl_pinctrl_acpi_match, &pdev->dev);
+	if (!id || !id->driver_data)
+		return -ENODEV;
+
+	soc_data = (const struct intel_pinctrl_soc_data *)id->driver_data;
+	return intel_pinctrl_probe(pdev, soc_data);
+}
+
+static const struct dev_pm_ops cnl_pinctrl_pm_ops = {
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(intel_pinctrl_suspend,
+				     intel_pinctrl_resume)
+};
+
+static struct platform_driver cnl_pinctrl_driver = {
+	.probe = cnl_pinctrl_probe,
+	.driver = {
+		.name = "cannonlake-pinctrl",
+		.acpi_match_table = cnl_pinctrl_acpi_match,
+		.pm = &cnl_pinctrl_pm_ops,
+	},
+};
+
+module_platform_driver(cnl_pinctrl_driver);
+
+MODULE_AUTHOR("Mika Westerberg <mika.westerberg@linux.intel.com>");
+MODULE_DESCRIPTION("Intel Cannon Lake PCH pinctrl/GPIO driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/intel/pinctrl-intel.c b/drivers/pinctrl/intel/pinctrl-intel.c
index 592b465e981e..6dc1096d3d34 100644
--- a/drivers/pinctrl/intel/pinctrl-intel.c
+++ b/drivers/pinctrl/intel/pinctrl-intel.c
@@ -117,6 +117,7 @@ struct intel_pinctrl {
 };
 
 #define pin_to_padno(c, p)	((p) - (c)->pin_base)
+#define padgroup_offset(g, p)	((p) - (g)->base)
 
 static struct intel_community *intel_get_community(struct intel_pinctrl *pctrl,
 						   unsigned pin)
@@ -135,6 +136,22 @@ static struct intel_community *intel_get_community(struct intel_pinctrl *pctrl,
 	return NULL;
 }
 
+static const struct intel_padgroup *
+intel_community_get_padgroup(const struct intel_community *community,
+			     unsigned pin)
+{
+	int i;
+
+	for (i = 0; i < community->ngpps; i++) {
+		const struct intel_padgroup *padgrp = &community->gpps[i];
+
+		if (pin >= padgrp->base && pin < padgrp->base + padgrp->size)
+			return padgrp;
+	}
+
+	return NULL;
+}
+
 static void __iomem *intel_get_padcfg(struct intel_pinctrl *pctrl, unsigned pin,
 				      unsigned reg)
 {
@@ -158,7 +175,8 @@ static void __iomem *intel_get_padcfg(struct intel_pinctrl *pctrl, unsigned pin,
 static bool intel_pad_owned_by_host(struct intel_pinctrl *pctrl, unsigned pin)
 {
 	const struct intel_community *community;
-	unsigned padno, gpp, offset, group;
+	const struct intel_padgroup *padgrp;
+	unsigned gpp, offset, gpp_offset;
 	void __iomem *padown;
 
 	community = intel_get_community(pctrl, pin);
@@ -167,19 +185,23 @@ static bool intel_pad_owned_by_host(struct intel_pinctrl *pctrl, unsigned pin)
 	if (!community->padown_offset)
 		return true;
 
-	padno = pin_to_padno(community, pin);
-	group = padno / community->gpp_size;
-	gpp = PADOWN_GPP(padno % community->gpp_size);
-	offset = community->padown_offset + 0x10 * group + gpp * 4;
+	padgrp = intel_community_get_padgroup(community, pin);
+	if (!padgrp)
+		return false;
+
+	gpp_offset = padgroup_offset(padgrp, pin);
+	gpp = PADOWN_GPP(gpp_offset);
+	offset = community->padown_offset + padgrp->padown_num * 4 + gpp * 4;
 	padown = community->regs + offset;
 
-	return !(readl(padown) & PADOWN_MASK(padno));
+	return !(readl(padown) & PADOWN_MASK(gpp_offset));
 }
 
 static bool intel_pad_acpi_mode(struct intel_pinctrl *pctrl, unsigned pin)
 {
 	const struct intel_community *community;
-	unsigned padno, gpp, offset;
+	const struct intel_padgroup *padgrp;
+	unsigned offset, gpp_offset;
 	void __iomem *hostown;
 
 	community = intel_get_community(pctrl, pin);
@@ -188,18 +210,22 @@ static bool intel_pad_acpi_mode(struct intel_pinctrl *pctrl, unsigned pin)
 	if (!community->hostown_offset)
 		return false;
 
-	padno = pin_to_padno(community, pin);
-	gpp = padno / community->gpp_size;
-	offset = community->hostown_offset + gpp * 4;
+	padgrp = intel_community_get_padgroup(community, pin);
+	if (!padgrp)
+		return true;
+
+	gpp_offset = padgroup_offset(padgrp, pin);
+	offset = community->hostown_offset + padgrp->reg_num * 4;
 	hostown = community->regs + offset;
 
-	return !(readl(hostown) & BIT(padno % community->gpp_size));
+	return !(readl(hostown) & BIT(gpp_offset));
 }
 
 static bool intel_pad_locked(struct intel_pinctrl *pctrl, unsigned pin)
 {
 	struct intel_community *community;
-	unsigned padno, gpp, offset;
+	const struct intel_padgroup *padgrp;
+	unsigned offset, gpp_offset;
 	u32 value;
 
 	community = intel_get_community(pctrl, pin);
@@ -208,22 +234,25 @@ static bool intel_pad_locked(struct intel_pinctrl *pctrl, unsigned pin)
 	if (!community->padcfglock_offset)
 		return false;
 
-	padno = pin_to_padno(community, pin);
-	gpp = padno / community->gpp_size;
+	padgrp = intel_community_get_padgroup(community, pin);
+	if (!padgrp)
+		return true;
+
+	gpp_offset = padgroup_offset(padgrp, pin);
 
 	/*
 	 * If PADCFGLOCK and PADCFGLOCKTX bits are both clear for this pad,
 	 * the pad is considered unlocked. Any other case means that it is
 	 * either fully or partially locked and we don't touch it.
 	 */
-	offset = community->padcfglock_offset + gpp * 8;
+	offset = community->padcfglock_offset + padgrp->reg_num * 8;
 	value = readl(community->regs + offset);
-	if (value & BIT(pin % community->gpp_size))
+	if (value & BIT(gpp_offset))
 		return true;
 
-	offset = community->padcfglock_offset + 4 + gpp * 8;
+	offset = community->padcfglock_offset + 4 + padgrp->reg_num * 8;
 	value = readl(community->regs + offset);
-	if (value & BIT(pin % community->gpp_size))
+	if (value & BIT(gpp_offset))
 		return true;
 
 	return false;
@@ -369,7 +398,11 @@ static int intel_pinmux_set_mux(struct pinctrl_dev *pctldev, unsigned function,
 		value = readl(padcfg0);
 
 		value &= ~PADCFG0_PMODE_MASK;
-		value |= grp->mode << PADCFG0_PMODE_SHIFT;
+
+		if (grp->modes)
+			value |= grp->modes[i] << PADCFG0_PMODE_SHIFT;
+		else
+			value |= grp->mode << PADCFG0_PMODE_SHIFT;
 
 		writel(value, padcfg0);
 	}
@@ -777,18 +810,22 @@ static void intel_gpio_irq_ack(struct irq_data *d)
 	const struct intel_community *community;
 	unsigned pin = irqd_to_hwirq(d);
 
-	raw_spin_lock(&pctrl->lock);
-
 	community = intel_get_community(pctrl, pin);
 	if (community) {
-		unsigned padno = pin_to_padno(community, pin);
-		unsigned gpp_offset = padno % community->gpp_size;
-		unsigned gpp = padno / community->gpp_size;
+		const struct intel_padgroup *padgrp;
+		unsigned gpp, gpp_offset;
+
+		padgrp = intel_community_get_padgroup(community, pin);
+		if (!padgrp)
+			return;
 
+		gpp = padgrp->reg_num;
+		gpp_offset = padgroup_offset(padgrp, pin);
+
+		raw_spin_lock(&pctrl->lock);
 		writel(BIT(gpp_offset), community->regs + GPI_IS + gpp * 4);
+		raw_spin_unlock(&pctrl->lock);
 	}
-
-	raw_spin_unlock(&pctrl->lock);
 }
 
 static void intel_gpio_irq_enable(struct irq_data *d)
@@ -797,27 +834,30 @@ static void intel_gpio_irq_enable(struct irq_data *d)
 	struct intel_pinctrl *pctrl = gpiochip_get_data(gc);
 	const struct intel_community *community;
 	unsigned pin = irqd_to_hwirq(d);
-	unsigned long flags;
-
-	raw_spin_lock_irqsave(&pctrl->lock, flags);
 
 	community = intel_get_community(pctrl, pin);
 	if (community) {
-		unsigned padno = pin_to_padno(community, pin);
-		unsigned gpp_size = community->gpp_size;
-		unsigned gpp_offset = padno % gpp_size;
-		unsigned gpp = padno / gpp_size;
+		const struct intel_padgroup *padgrp;
+		unsigned gpp, gpp_offset;
+		unsigned long flags;
 		u32 value;
 
+		padgrp = intel_community_get_padgroup(community, pin);
+		if (!padgrp)
+			return;
+
+		gpp = padgrp->reg_num;
+		gpp_offset = padgroup_offset(padgrp, pin);
+
+		raw_spin_lock_irqsave(&pctrl->lock, flags);
 		/* Clear interrupt status first to avoid unexpected interrupt */
 		writel(BIT(gpp_offset), community->regs + GPI_IS + gpp * 4);
 
 		value = readl(community->regs + community->ie_offset + gpp * 4);
 		value |= BIT(gpp_offset);
 		writel(value, community->regs + community->ie_offset + gpp * 4);
+		raw_spin_unlock_irqrestore(&pctrl->lock, flags);
 	}
-
-	raw_spin_unlock_irqrestore(&pctrl->lock, flags);
 }
 
 static void intel_gpio_irq_mask_unmask(struct irq_data *d, bool mask)
@@ -826,28 +866,33 @@ static void intel_gpio_irq_mask_unmask(struct irq_data *d, bool mask)
 	struct intel_pinctrl *pctrl = gpiochip_get_data(gc);
 	const struct intel_community *community;
 	unsigned pin = irqd_to_hwirq(d);
-	unsigned long flags;
-
-	raw_spin_lock_irqsave(&pctrl->lock, flags);
 
 	community = intel_get_community(pctrl, pin);
 	if (community) {
-		unsigned padno = pin_to_padno(community, pin);
-		unsigned gpp_offset = padno % community->gpp_size;
-		unsigned gpp = padno / community->gpp_size;
+		const struct intel_padgroup *padgrp;
+		unsigned gpp, gpp_offset;
+		unsigned long flags;
 		void __iomem *reg;
 		u32 value;
 
+		padgrp = intel_community_get_padgroup(community, pin);
+		if (!padgrp)
+			return;
+
+		gpp = padgrp->reg_num;
+		gpp_offset = padgroup_offset(padgrp, pin);
+
 		reg = community->regs + community->ie_offset + gpp * 4;
+
+		raw_spin_lock_irqsave(&pctrl->lock, flags);
 		value = readl(reg);
 		if (mask)
 			value &= ~BIT(gpp_offset);
 		else
 			value |= BIT(gpp_offset);
 		writel(value, reg);
+		raw_spin_unlock_irqrestore(&pctrl->lock, flags);
 	}
-
-	raw_spin_unlock_irqrestore(&pctrl->lock, flags);
 }
 
 static void intel_gpio_irq_mask(struct irq_data *d)
@@ -938,23 +983,20 @@ static irqreturn_t intel_gpio_community_irq_handler(struct intel_pinctrl *pctrl,
 	int gpp;
 
 	for (gpp = 0; gpp < community->ngpps; gpp++) {
+		const struct intel_padgroup *padgrp = &community->gpps[gpp];
 		unsigned long pending, enabled, gpp_offset;
 
-		pending = readl(community->regs + GPI_IS + gpp * 4);
+		pending = readl(community->regs + GPI_IS + padgrp->reg_num * 4);
 		enabled = readl(community->regs + community->ie_offset +
-				gpp * 4);
+				padgrp->reg_num * 4);
 
 		/* Only interrupts that are enabled */
 		pending &= enabled;
 
-		for_each_set_bit(gpp_offset, &pending, community->gpp_size) {
+		for_each_set_bit(gpp_offset, &pending, padgrp->size) {
 			unsigned padno, irq;
 
-			/*
-			 * The last group in community can have less pins
-			 * than NPADS_IN_GPP.
-			 */
-			padno = gpp_offset + gpp * community->gpp_size;
+			padno = padgrp->base - community->pin_base + gpp_offset;
 			if (padno >= community->npins)
 				break;
 
@@ -1045,6 +1087,56 @@ static int intel_gpio_probe(struct intel_pinctrl *pctrl, int irq)
 	return 0;
 }
 
+static int intel_pinctrl_add_padgroups(struct intel_pinctrl *pctrl,
+				       struct intel_community *community)
+{
+	struct intel_padgroup *gpps;
+	unsigned npins = community->npins;
+	unsigned padown_num = 0;
+	size_t ngpps, i;
+
+	if (community->gpps)
+		ngpps = community->ngpps;
+	else
+		ngpps = DIV_ROUND_UP(community->npins, community->gpp_size);
+
+	gpps = devm_kcalloc(pctrl->dev, ngpps, sizeof(*gpps), GFP_KERNEL);
+	if (!gpps)
+		return -ENOMEM;
+
+	for (i = 0; i < ngpps; i++) {
+		if (community->gpps) {
+			gpps[i] = community->gpps[i];
+		} else {
+			unsigned gpp_size = community->gpp_size;
+
+			gpps[i].reg_num = i;
+			gpps[i].base = community->pin_base + i * gpp_size;
+			gpps[i].size = min(gpp_size, npins);
+			npins -= gpps[i].size;
+		}
+
+		if (gpps[i].size > 32)
+			return -EINVAL;
+
+		gpps[i].padown_num = padown_num;
+
+		/*
+		 * In older hardware the number of padown registers per
+		 * group is fixed regardless of the group size.
+		 */
+		if (community->gpp_num_padown_regs)
+			padown_num += community->gpp_num_padown_regs;
+		else
+			padown_num += DIV_ROUND_UP(gpps[i].size * 4, 32);
+	}
+
+	community->ngpps = ngpps;
+	community->gpps = gpps;
+
+	return 0;
+}
+
 static int intel_pinctrl_pm_init(struct intel_pinctrl *pctrl)
 {
 #ifdef CONFIG_PM_SLEEP
@@ -1142,8 +1234,10 @@ int intel_pinctrl_probe(struct platform_device *pdev,
 
 		community->regs = regs;
 		community->pad_regs = regs + padbar;
-		community->ngpps = DIV_ROUND_UP(community->npins,
-						community->gpp_size);
+
+		ret = intel_pinctrl_add_padgroups(pctrl, community);
+		if (ret)
+			return ret;
 	}
 
 	irq = platform_get_irq(pdev, 0);
diff --git a/drivers/pinctrl/intel/pinctrl-intel.h b/drivers/pinctrl/intel/pinctrl-intel.h
index fe9521f345b5..7fdb07753c2d 100644
--- a/drivers/pinctrl/intel/pinctrl-intel.h
+++ b/drivers/pinctrl/intel/pinctrl-intel.h
@@ -22,13 +22,16 @@ struct device;
  * @name: Name of the groups
  * @pins: All pins in this group
  * @npins: Number of pins in this groups
- * @mode: Native mode in which the group is muxed out @pins
+ * @mode: Native mode in which the group is muxed out @pins. Used if @modes
+ *        is %NULL.
+ * @modes: If not %NULL this will hold mode for each pin in @pins
  */
 struct intel_pingroup {
 	const char *name;
 	const unsigned *pins;
 	size_t npins;
 	unsigned short mode;
+	const unsigned *modes;
 };
 
 /**
@@ -44,6 +47,23 @@ struct intel_function {
 };
 
 /**
+ * struct intel_padgroup - Hardware pad group information
+ * @reg_num: GPI_IS register number
+ * @base: Starting pin of this group
+ * @size: Size of this group (maximum is 32).
+ * @padown_num: PAD_OWN register number (assigned by the core driver)
+ *
+ * If pad groups of a community are not the same size, use this structure
+ * to specify them.
+ */
+struct intel_padgroup {
+	unsigned reg_num;
+	unsigned base;
+	unsigned size;
+	unsigned padown_num;
+};
+
+/**
  * struct intel_community - Intel pin community description
  * @barno: MMIO BAR number where registers for this community reside
  * @padown_offset: Register offset of PAD_OWN register from @regs. If %0
@@ -56,13 +76,22 @@ struct intel_function {
  * @ie_offset: Register offset of GPI_IE from @regs.
  * @pin_base: Starting pin of pins in this community
  * @gpp_size: Maximum number of pads in each group, such as PADCFGLOCK,
- *            HOSTSW_OWN,  GPI_IS, GPI_IE, etc.
+ *            HOSTSW_OWN,  GPI_IS, GPI_IE, etc. Used when @gpps is %NULL.
+ * @gpp_num_padown_regs: Number of pad registers each pad group consumes at
+ *			 minimum. Use %0 if the number of registers can be
+ *			 determined by the size of the group.
  * @npins: Number of pins in this community
  * @features: Additional features supported by the hardware
+ * @gpps: Pad groups if the controller has variable size pad groups
+ * @ngpps: Number of pad groups in this community
  * @regs: Community specific common registers (reserved for core driver)
  * @pad_regs: Community specific pad registers (reserved for core driver)
- * @ngpps: Number of groups (hw groups) in this community (reserved for
- *         core driver)
+ *
+ * Most Intel GPIO host controllers this driver supports each pad group is
+ * of equal size (except the last one). In that case the driver can just
+ * fill in @gpp_size field and let the core driver to handle the rest. If
+ * the controller has pad groups of variable size the client driver can
+ * pass custom @gpps and @ngpps instead.
  */
 struct intel_community {
 	unsigned barno;
@@ -72,23 +101,37 @@ struct intel_community {
 	unsigned ie_offset;
 	unsigned pin_base;
 	unsigned gpp_size;
+	unsigned gpp_num_padown_regs;
 	size_t npins;
 	unsigned features;
+	const struct intel_padgroup *gpps;
+	size_t ngpps;
+	/* Reserved for the core driver */
 	void __iomem *regs;
 	void __iomem *pad_regs;
-	size_t ngpps;
 };
 
 /* Additional features supported by the hardware */
 #define PINCTRL_FEATURE_DEBOUNCE	BIT(0)
 #define PINCTRL_FEATURE_1K_PD		BIT(1)
 
-#define PIN_GROUP(n, p, m)			\
-	{					\
-		.name = (n),			\
-		.pins = (p),			\
-		.npins = ARRAY_SIZE((p)),	\
-		.mode = (m),			\
+/**
+ * PIN_GROUP - Declare a pin group
+ * @n: Name of the group
+ * @p: An array of pins this group consists
+ * @m: Mode which the pins are put when this group is active. Can be either
+ *     a single integer or an array of integers in which case mode is per
+ *     pin.
+ */
+#define PIN_GROUP(n, p, m)					\
+	{							\
+		.name = (n),					\
+		.pins = (p),					\
+		.npins = ARRAY_SIZE((p)),			\
+		.mode = __builtin_choose_expr(			\
+			__builtin_constant_p((m)), (m), 0),	\
+		.modes = __builtin_choose_expr(			\
+			__builtin_constant_p((m)), NULL, (m)),	\
 	}
 
 #define FUNCTION(n, g)				\
diff --git a/drivers/pinctrl/intel/pinctrl-sunrisepoint.c b/drivers/pinctrl/intel/pinctrl-sunrisepoint.c
index 9877526c0807..8870a4100164 100644
--- a/drivers/pinctrl/intel/pinctrl-sunrisepoint.c
+++ b/drivers/pinctrl/intel/pinctrl-sunrisepoint.c
@@ -31,6 +31,7 @@
 		.hostown_offset = SPT_HOSTSW_OWN,	\
 		.ie_offset = SPT_GPI_IE,		\
 		.gpp_size = 24,				\
+		.gpp_num_padown_regs = 4,		\
 		.pin_base = (s),			\
 		.npins = ((e) - (s) + 1),		\
 	}
diff --git a/drivers/pinctrl/mediatek/Kconfig b/drivers/pinctrl/mediatek/Kconfig
index 80fe3b48796c..fac9866311f3 100644
--- a/drivers/pinctrl/mediatek/Kconfig
+++ b/drivers/pinctrl/mediatek/Kconfig
@@ -11,18 +11,11 @@ config PINCTRL_MTK
 # For ARMv7 SoCs
 config PINCTRL_MT2701
 	bool "Mediatek MT2701 pin control"
-	depends on MACH_MT2701 || COMPILE_TEST
+	depends on MACH_MT7623 || MACH_MT2701 || COMPILE_TEST
 	depends on OF
 	default MACH_MT2701
 	select PINCTRL_MTK
 
-config PINCTRL_MT7623
-	bool "Mediatek MT7623 pin control"
-	depends on MACH_MT7623 || COMPILE_TEST
-	depends on OF
-	default MACH_MT7623
-	select PINCTRL_MTK_COMMON
-
 config PINCTRL_MT8135
 	bool "Mediatek MT8135 pin control"
 	depends on MACH_MT8135 || COMPILE_TEST
diff --git a/drivers/pinctrl/mediatek/Makefile b/drivers/pinctrl/mediatek/Makefile
index 3e3390a14716..e59c613d4ddd 100644
--- a/drivers/pinctrl/mediatek/Makefile
+++ b/drivers/pinctrl/mediatek/Makefile
@@ -3,7 +3,6 @@ obj-y				+= pinctrl-mtk-common.o
 
 # SoC Drivers
 obj-$(CONFIG_PINCTRL_MT2701)	+= pinctrl-mt2701.o
-obj-$(CONFIG_PINCTRL_MT7623)	+= pinctrl-mt7623.o
 obj-$(CONFIG_PINCTRL_MT8135)	+= pinctrl-mt8135.o
 obj-$(CONFIG_PINCTRL_MT8127)	+= pinctrl-mt8127.o
 obj-$(CONFIG_PINCTRL_MT8173)	+= pinctrl-mt8173.o
diff --git a/drivers/pinctrl/mediatek/pinctrl-mt2701.c b/drivers/pinctrl/mediatek/pinctrl-mt2701.c
index 8d802fa7decd..f86f3b379607 100644
--- a/drivers/pinctrl/mediatek/pinctrl-mt2701.c
+++ b/drivers/pinctrl/mediatek/pinctrl-mt2701.c
@@ -565,6 +565,7 @@ static int mt2701_pinctrl_probe(struct platform_device *pdev)
 
 static const struct of_device_id mt2701_pctrl_match[] = {
 	{ .compatible = "mediatek,mt2701-pinctrl", },
+	{ .compatible = "mediatek,mt7623-pinctrl", },
 	{}
 };
 MODULE_DEVICE_TABLE(of, mt2701_pctrl_match);
diff --git a/drivers/pinctrl/mediatek/pinctrl-mt7623.c b/drivers/pinctrl/mediatek/pinctrl-mt7623.c
deleted file mode 100644
index fa28dd6b871b..000000000000
--- a/drivers/pinctrl/mediatek/pinctrl-mt7623.c
+++ /dev/null
@@ -1,379 +0,0 @@
-/*
- * Copyright (c) 2016 John Crispin <john@phrozen.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT 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 <dt-bindings/pinctrl/mt65xx.h>
-#include <linux/module.h>
-#include <linux/of.h>
-#include <linux/of_device.h>
-#include <linux/platform_device.h>
-#include <linux/pinctrl/pinctrl.h>
-#include <linux/regmap.h>
-
-#include "pinctrl-mtk-common.h"
-#include "pinctrl-mtk-mt7623.h"
-
-static const struct mtk_drv_group_desc mt7623_drv_grp[] =  {
-	/* 0E4E8SR 4/8/12/16 */
-	MTK_DRV_GRP(4, 16, 1, 2, 4),
-	/* 0E2E4SR  2/4/6/8 */
-	MTK_DRV_GRP(2, 8, 1, 2, 2),
-	/* E8E4E2  2/4/6/8/10/12/14/16 */
-	MTK_DRV_GRP(2, 16, 0, 2, 2)
-};
-
-#define DRV_SEL0	0xf50
-#define DRV_SEL1	0xf60
-#define DRV_SEL2	0xf70
-#define DRV_SEL3	0xf80
-#define DRV_SEL4	0xf90
-#define DRV_SEL5	0xfa0
-#define DRV_SEL6	0xfb0
-#define DRV_SEL7	0xfe0
-#define DRV_SEL8	0xfd0
-#define DRV_SEL9	0xff0
-#define DRV_SEL10	0xf00
-
-#define MSDC0_CTRL0	0xcc0
-#define MSDC0_CTRL1	0xcd0
-#define MSDC0_CTRL2	0xce0
-#define MSDC0_CTRL3	0xcf0
-#define MSDC0_CTRL4	0xd00
-#define MSDC0_CTRL5	0xd10
-#define MSDC0_CTRL6	0xd20
-#define MSDC1_CTRL0	0xd30
-#define MSDC1_CTRL1	0xd40
-#define MSDC1_CTRL2	0xd50
-#define MSDC1_CTRL3	0xd60
-#define MSDC1_CTRL4	0xd70
-#define MSDC1_CTRL5	0xd80
-#define MSDC1_CTRL6	0xd90
-
-#define IES_EN0		0xb20
-#define IES_EN1		0xb30
-#define IES_EN2		0xb40
-
-#define SMT_EN0		0xb50
-#define SMT_EN1		0xb60
-#define SMT_EN2		0xb70
-
-static const struct mtk_pin_drv_grp mt7623_pin_drv[] = {
-	MTK_PIN_DRV_GRP(0, DRV_SEL0, 0, 1),
-	MTK_PIN_DRV_GRP(1, DRV_SEL0, 0, 1),
-	MTK_PIN_DRV_GRP(2, DRV_SEL0, 0, 1),
-	MTK_PIN_DRV_GRP(3, DRV_SEL0, 0, 1),
-	MTK_PIN_DRV_GRP(4, DRV_SEL0, 0, 1),
-	MTK_PIN_DRV_GRP(5, DRV_SEL0, 0, 1),
-	MTK_PIN_DRV_GRP(6, DRV_SEL0, 0, 1),
-	MTK_PIN_DRV_GRP(7, DRV_SEL0, 4, 1),
-	MTK_PIN_DRV_GRP(8, DRV_SEL0, 4, 1),
-	MTK_PIN_DRV_GRP(9, DRV_SEL0, 4, 1),
-	MTK_PIN_DRV_GRP(10, DRV_SEL0, 8, 1),
-	MTK_PIN_DRV_GRP(11, DRV_SEL0, 8, 1),
-	MTK_PIN_DRV_GRP(12, DRV_SEL0, 8, 1),
-	MTK_PIN_DRV_GRP(13, DRV_SEL0, 8, 1),
-	MTK_PIN_DRV_GRP(14, DRV_SEL0, 12, 0),
-	MTK_PIN_DRV_GRP(15, DRV_SEL0, 12, 0),
-	MTK_PIN_DRV_GRP(18, DRV_SEL1, 4, 0),
-	MTK_PIN_DRV_GRP(19, DRV_SEL1, 4, 0),
-	MTK_PIN_DRV_GRP(20, DRV_SEL1, 4, 0),
-	MTK_PIN_DRV_GRP(21, DRV_SEL1, 4, 0),
-	MTK_PIN_DRV_GRP(22, DRV_SEL1, 8, 0),
-	MTK_PIN_DRV_GRP(23, DRV_SEL1, 8, 0),
-	MTK_PIN_DRV_GRP(24, DRV_SEL1, 8, 0),
-	MTK_PIN_DRV_GRP(25, DRV_SEL1, 8, 0),
-	MTK_PIN_DRV_GRP(26, DRV_SEL1, 8, 0),
-	MTK_PIN_DRV_GRP(27, DRV_SEL1, 12, 0),
-	MTK_PIN_DRV_GRP(28, DRV_SEL1, 12, 0),
-	MTK_PIN_DRV_GRP(29, DRV_SEL1, 12, 0),
-	MTK_PIN_DRV_GRP(33, DRV_SEL2, 0, 0),
-	MTK_PIN_DRV_GRP(34, DRV_SEL2, 0, 0),
-	MTK_PIN_DRV_GRP(35, DRV_SEL2, 0, 0),
-	MTK_PIN_DRV_GRP(36, DRV_SEL2, 0, 0),
-	MTK_PIN_DRV_GRP(37, DRV_SEL2, 0, 0),
-	MTK_PIN_DRV_GRP(39, DRV_SEL2, 8, 1),
-	MTK_PIN_DRV_GRP(40, DRV_SEL2, 8, 1),
-	MTK_PIN_DRV_GRP(41, DRV_SEL2, 8, 1),
-	MTK_PIN_DRV_GRP(42, DRV_SEL2, 8, 1),
-	MTK_PIN_DRV_GRP(43, DRV_SEL2, 12, 0),
-	MTK_PIN_DRV_GRP(44, DRV_SEL2, 12, 0),
-	MTK_PIN_DRV_GRP(45, DRV_SEL2, 12, 0),
-	MTK_PIN_DRV_GRP(47, DRV_SEL3, 0, 0),
-	MTK_PIN_DRV_GRP(48, DRV_SEL3, 0, 0),
-	MTK_PIN_DRV_GRP(49, DRV_SEL3, 4, 0),
-	MTK_PIN_DRV_GRP(53, DRV_SEL3, 12, 0),
-	MTK_PIN_DRV_GRP(54, DRV_SEL3, 12, 0),
-	MTK_PIN_DRV_GRP(55, DRV_SEL3, 12, 0),
-	MTK_PIN_DRV_GRP(56, DRV_SEL3, 12, 0),
-	MTK_PIN_DRV_GRP(60, DRV_SEL4, 8, 1),
-	MTK_PIN_DRV_GRP(61, DRV_SEL4, 8, 1),
-	MTK_PIN_DRV_GRP(62, DRV_SEL4, 8, 1),
-	MTK_PIN_DRV_GRP(63, DRV_SEL4, 12, 1),
-	MTK_PIN_DRV_GRP(64, DRV_SEL4, 12, 1),
-	MTK_PIN_DRV_GRP(65, DRV_SEL4, 12, 1),
-	MTK_PIN_DRV_GRP(66, DRV_SEL5, 0, 1),
-	MTK_PIN_DRV_GRP(67, DRV_SEL5, 0, 1),
-	MTK_PIN_DRV_GRP(68, DRV_SEL5, 0, 1),
-	MTK_PIN_DRV_GRP(69, DRV_SEL5, 0, 1),
-	MTK_PIN_DRV_GRP(70, DRV_SEL5, 0, 1),
-	MTK_PIN_DRV_GRP(71, DRV_SEL5, 0, 1),
-	MTK_PIN_DRV_GRP(72, DRV_SEL3, 4, 0),
-	MTK_PIN_DRV_GRP(73, DRV_SEL3, 4, 0),
-	MTK_PIN_DRV_GRP(74, DRV_SEL3, 4, 0),
-	MTK_PIN_DRV_GRP(83, DRV_SEL5, 0, 1),
-	MTK_PIN_DRV_GRP(84, DRV_SEL5, 0, 1),
-	MTK_PIN_DRV_GRP(105, MSDC1_CTRL1, 0, 1),
-	MTK_PIN_DRV_GRP(106, MSDC1_CTRL0, 0, 1),
-	MTK_PIN_DRV_GRP(107, MSDC1_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(108, MSDC1_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(109, MSDC1_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(110, MSDC1_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(111, MSDC0_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(112, MSDC0_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(113, MSDC0_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(114, MSDC0_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(115, MSDC0_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(116, MSDC0_CTRL1, 0, 1),
-	MTK_PIN_DRV_GRP(117, MSDC0_CTRL0, 0, 1),
-	MTK_PIN_DRV_GRP(118, MSDC0_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(119, MSDC0_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(120, MSDC0_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(121, MSDC0_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(126, DRV_SEL3, 4, 0),
-	MTK_PIN_DRV_GRP(199, DRV_SEL0, 4, 1),
-	MTK_PIN_DRV_GRP(200, DRV_SEL8, 0, 0),
-	MTK_PIN_DRV_GRP(201, DRV_SEL8, 0, 0),
-	MTK_PIN_DRV_GRP(203, DRV_SEL8, 4, 0),
-	MTK_PIN_DRV_GRP(204, DRV_SEL8, 4, 0),
-	MTK_PIN_DRV_GRP(205, DRV_SEL8, 4, 0),
-	MTK_PIN_DRV_GRP(206, DRV_SEL8, 4, 0),
-	MTK_PIN_DRV_GRP(207, DRV_SEL8, 4, 0),
-	MTK_PIN_DRV_GRP(208, DRV_SEL8, 8, 0),
-	MTK_PIN_DRV_GRP(209, DRV_SEL8, 8, 0),
-	MTK_PIN_DRV_GRP(236, DRV_SEL9, 4, 0),
-	MTK_PIN_DRV_GRP(237, DRV_SEL9, 4, 0),
-	MTK_PIN_DRV_GRP(238, DRV_SEL9, 4, 0),
-	MTK_PIN_DRV_GRP(239, DRV_SEL9, 4, 0),
-	MTK_PIN_DRV_GRP(240, DRV_SEL9, 4, 0),
-	MTK_PIN_DRV_GRP(241, DRV_SEL9, 4, 0),
-	MTK_PIN_DRV_GRP(242, DRV_SEL9, 8, 0),
-	MTK_PIN_DRV_GRP(243, DRV_SEL9, 8, 0),
-	MTK_PIN_DRV_GRP(257, MSDC0_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(261, MSDC1_CTRL2, 0, 1),
-	MTK_PIN_DRV_GRP(262, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(263, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(264, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(265, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(266, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(267, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(268, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(269, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(270, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(271, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(272, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(274, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(275, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(276, DRV_SEL10, 8, 0),
-	MTK_PIN_DRV_GRP(278, DRV_SEL2, 8, 1),
-};
-
-static const struct mtk_pin_spec_pupd_set_samereg mt7623_spec_pupd[] = {
-	MTK_PIN_PUPD_SPEC_SR(105, MSDC1_CTRL1, 8, 9, 10),
-	MTK_PIN_PUPD_SPEC_SR(106, MSDC1_CTRL0, 8, 9, 10),
-	MTK_PIN_PUPD_SPEC_SR(107, MSDC1_CTRL3, 0, 1, 2),
-	MTK_PIN_PUPD_SPEC_SR(108, MSDC1_CTRL3, 4, 5, 6),
-	MTK_PIN_PUPD_SPEC_SR(109, MSDC1_CTRL3, 8, 9, 10),
-	MTK_PIN_PUPD_SPEC_SR(110, MSDC1_CTRL3, 12, 13, 14),
-	MTK_PIN_PUPD_SPEC_SR(111, MSDC0_CTRL4, 12, 13, 14),
-	MTK_PIN_PUPD_SPEC_SR(112, MSDC0_CTRL4, 8, 9, 10),
-	MTK_PIN_PUPD_SPEC_SR(113, MSDC0_CTRL4, 4, 5, 6),
-	MTK_PIN_PUPD_SPEC_SR(114, MSDC0_CTRL4, 0, 1, 2),
-	MTK_PIN_PUPD_SPEC_SR(115, MSDC0_CTRL5, 0, 1, 2),
-	MTK_PIN_PUPD_SPEC_SR(116, MSDC0_CTRL1, 8, 9, 10),
-	MTK_PIN_PUPD_SPEC_SR(117, MSDC0_CTRL0, 8, 9, 10),
-	MTK_PIN_PUPD_SPEC_SR(118, MSDC0_CTRL3, 12, 13, 14),
-	MTK_PIN_PUPD_SPEC_SR(119, MSDC0_CTRL3, 8, 9, 10),
-	MTK_PIN_PUPD_SPEC_SR(120, MSDC0_CTRL3, 4, 5, 6),
-	MTK_PIN_PUPD_SPEC_SR(121, MSDC0_CTRL3, 0, 1, 2),
-};
-
-static int mt7623_spec_pull_set(struct regmap *regmap, unsigned int pin,
-		unsigned char align, bool isup, unsigned int r1r0)
-{
-	return mtk_pctrl_spec_pull_set_samereg(regmap, mt7623_spec_pupd,
-		ARRAY_SIZE(mt7623_spec_pupd), pin, align, isup, r1r0);
-}
-
-static const struct mtk_pin_ies_smt_set mt7623_ies_set[] = {
-	MTK_PIN_IES_SMT_SPEC(0, 6, IES_EN0, 0),
-	MTK_PIN_IES_SMT_SPEC(7, 9, IES_EN0, 1),
-	MTK_PIN_IES_SMT_SPEC(10, 13, IES_EN0, 2),
-	MTK_PIN_IES_SMT_SPEC(14, 15, IES_EN0, 3),
-	MTK_PIN_IES_SMT_SPEC(18, 21, IES_EN0, 5),
-	MTK_PIN_IES_SMT_SPEC(22, 26, IES_EN0, 6),
-	MTK_PIN_IES_SMT_SPEC(27, 29, IES_EN0, 7),
-	MTK_PIN_IES_SMT_SPEC(33, 37, IES_EN0, 8),
-	MTK_PIN_IES_SMT_SPEC(39, 42, IES_EN0, 9),
-	MTK_PIN_IES_SMT_SPEC(43, 45, IES_EN0, 10),
-	MTK_PIN_IES_SMT_SPEC(47, 48, IES_EN0, 11),
-	MTK_PIN_IES_SMT_SPEC(49, 49, IES_EN0, 12),
-	MTK_PIN_IES_SMT_SPEC(53, 56, IES_EN0, 14),
-	MTK_PIN_IES_SMT_SPEC(60, 62, IES_EN1, 0),
-	MTK_PIN_IES_SMT_SPEC(63, 65, IES_EN1, 1),
-	MTK_PIN_IES_SMT_SPEC(66, 71, IES_EN1, 2),
-	MTK_PIN_IES_SMT_SPEC(72, 74, IES_EN0, 12),
-	MTK_PIN_IES_SMT_SPEC(75, 76, IES_EN1, 3),
-	MTK_PIN_IES_SMT_SPEC(83, 84, IES_EN1, 2),
-	MTK_PIN_IES_SMT_SPEC(105, 121, MSDC1_CTRL1, 4),
-	MTK_PIN_IES_SMT_SPEC(122, 125, IES_EN1, 7),
-	MTK_PIN_IES_SMT_SPEC(126, 126, IES_EN0, 12),
-	MTK_PIN_IES_SMT_SPEC(199, 201, IES_EN0, 1),
-	MTK_PIN_IES_SMT_SPEC(203, 207, IES_EN2, 2),
-	MTK_PIN_IES_SMT_SPEC(208, 209, IES_EN2, 3),
-	MTK_PIN_IES_SMT_SPEC(236, 241, IES_EN2, 6),
-	MTK_PIN_IES_SMT_SPEC(242, 243, IES_EN2, 7),
-	MTK_PIN_IES_SMT_SPEC(261, 261, MSDC1_CTRL2, 4),
-	MTK_PIN_IES_SMT_SPEC(262, 272, IES_EN2, 12),
-	MTK_PIN_IES_SMT_SPEC(274, 276, IES_EN2, 12),
-	MTK_PIN_IES_SMT_SPEC(278, 278, IES_EN2, 13),
-};
-
-static const struct mtk_pin_ies_smt_set mt7623_smt_set[] = {
-	MTK_PIN_IES_SMT_SPEC(0, 6, SMT_EN0, 0),
-	MTK_PIN_IES_SMT_SPEC(7, 9, SMT_EN0, 1),
-	MTK_PIN_IES_SMT_SPEC(10, 13, SMT_EN0, 2),
-	MTK_PIN_IES_SMT_SPEC(14, 15, SMT_EN0, 3),
-	MTK_PIN_IES_SMT_SPEC(18, 21, SMT_EN0, 5),
-	MTK_PIN_IES_SMT_SPEC(22, 26, SMT_EN0, 6),
-	MTK_PIN_IES_SMT_SPEC(27, 29, SMT_EN0, 7),
-	MTK_PIN_IES_SMT_SPEC(33, 37, SMT_EN0, 8),
-	MTK_PIN_IES_SMT_SPEC(39, 42, SMT_EN0, 9),
-	MTK_PIN_IES_SMT_SPEC(43, 45, SMT_EN0, 10),
-	MTK_PIN_IES_SMT_SPEC(47, 48, SMT_EN0, 11),
-	MTK_PIN_IES_SMT_SPEC(49, 49, SMT_EN0, 12),
-	MTK_PIN_IES_SMT_SPEC(53, 56, SMT_EN0, 14),
-	MTK_PIN_IES_SMT_SPEC(60, 62, SMT_EN1, 0),
-	MTK_PIN_IES_SMT_SPEC(63, 65, SMT_EN1, 1),
-	MTK_PIN_IES_SMT_SPEC(66, 71, SMT_EN1, 2),
-	MTK_PIN_IES_SMT_SPEC(72, 74, SMT_EN0, 12),
-	MTK_PIN_IES_SMT_SPEC(75, 76, SMT_EN1, 3),
-	MTK_PIN_IES_SMT_SPEC(83, 84, SMT_EN1, 2),
-	MTK_PIN_IES_SMT_SPEC(105, 106, MSDC1_CTRL1, 11),
-	MTK_PIN_IES_SMT_SPEC(107, 107, MSDC1_CTRL3, 3),
-	MTK_PIN_IES_SMT_SPEC(108, 108, MSDC1_CTRL3, 7),
-	MTK_PIN_IES_SMT_SPEC(109, 109, MSDC1_CTRL3, 11),
-	MTK_PIN_IES_SMT_SPEC(110, 111, MSDC1_CTRL3, 15),
-	MTK_PIN_IES_SMT_SPEC(112, 112, MSDC0_CTRL4, 11),
-	MTK_PIN_IES_SMT_SPEC(113, 113, MSDC0_CTRL4, 7),
-	MTK_PIN_IES_SMT_SPEC(114, 115, MSDC0_CTRL4, 3),
-	MTK_PIN_IES_SMT_SPEC(116, 117, MSDC0_CTRL1, 11),
-	MTK_PIN_IES_SMT_SPEC(118, 118, MSDC0_CTRL3, 15),
-	MTK_PIN_IES_SMT_SPEC(119, 119, MSDC0_CTRL3, 11),
-	MTK_PIN_IES_SMT_SPEC(120, 120, MSDC0_CTRL3, 7),
-	MTK_PIN_IES_SMT_SPEC(121, 121, MSDC0_CTRL3, 3),
-	MTK_PIN_IES_SMT_SPEC(122, 125, SMT_EN1, 7),
-	MTK_PIN_IES_SMT_SPEC(126, 126, SMT_EN0, 12),
-	MTK_PIN_IES_SMT_SPEC(199, 201, SMT_EN0, 1),
-	MTK_PIN_IES_SMT_SPEC(203, 207, SMT_EN2, 2),
-	MTK_PIN_IES_SMT_SPEC(208, 209, SMT_EN2, 3),
-	MTK_PIN_IES_SMT_SPEC(236, 241, SMT_EN2, 6),
-	MTK_PIN_IES_SMT_SPEC(242, 243, SMT_EN2, 7),
-	MTK_PIN_IES_SMT_SPEC(261, 261, MSDC1_CTRL6, 3),
-	MTK_PIN_IES_SMT_SPEC(262, 272, SMT_EN2, 12),
-	MTK_PIN_IES_SMT_SPEC(274, 276, SMT_EN2, 12),
-	MTK_PIN_IES_SMT_SPEC(278, 278, SMT_EN2, 13),
-};
-
-static int mt7623_ies_smt_set(struct regmap *regmap, unsigned int pin,
-		unsigned char align, int value, enum pin_config_param arg)
-{
-	if (arg == PIN_CONFIG_INPUT_ENABLE)
-		return mtk_pconf_spec_set_ies_smt_range(regmap, mt7623_ies_set,
-			ARRAY_SIZE(mt7623_ies_set), pin, align, value);
-	else if (arg == PIN_CONFIG_INPUT_SCHMITT_ENABLE)
-		return mtk_pconf_spec_set_ies_smt_range(regmap, mt7623_smt_set,
-			ARRAY_SIZE(mt7623_smt_set), pin, align, value);
-	return -EINVAL;
-}
-
-static const struct mtk_pinctrl_devdata mt7623_pinctrl_data = {
-	.pins = mtk_pins_mt7623,
-	.npins = ARRAY_SIZE(mtk_pins_mt7623),
-	.grp_desc = mt7623_drv_grp,
-	.n_grp_cls = ARRAY_SIZE(mt7623_drv_grp),
-	.pin_drv_grp = mt7623_pin_drv,
-	.n_pin_drv_grps = ARRAY_SIZE(mt7623_pin_drv),
-	.spec_pull_set = mt7623_spec_pull_set,
-	.spec_ies_smt_set = mt7623_ies_smt_set,
-	.dir_offset = 0x0000,
-	.pullen_offset = 0x0150,
-	.pullsel_offset = 0x0280,
-	.dout_offset = 0x0500,
-	.din_offset = 0x0630,
-	.pinmux_offset = 0x0760,
-	.type1_start = 280,
-	.type1_end = 280,
-	.port_shf = 4,
-	.port_mask = 0x1f,
-	.port_align = 4,
-	.eint_offsets = {
-		.name = "mt7623_eint",
-		.stat      = 0x000,
-		.ack       = 0x040,
-		.mask      = 0x080,
-		.mask_set  = 0x0c0,
-		.mask_clr  = 0x100,
-		.sens      = 0x140,
-		.sens_set  = 0x180,
-		.sens_clr  = 0x1c0,
-		.soft      = 0x200,
-		.soft_set  = 0x240,
-		.soft_clr  = 0x280,
-		.pol       = 0x300,
-		.pol_set   = 0x340,
-		.pol_clr   = 0x380,
-		.dom_en    = 0x400,
-		.dbnc_ctrl = 0x500,
-		.dbnc_set  = 0x600,
-		.dbnc_clr  = 0x700,
-		.port_mask = 6,
-		.ports     = 6,
-	},
-	.ap_num = 169,
-	.db_cnt = 16,
-};
-
-static int mt7623_pinctrl_probe(struct platform_device *pdev)
-{
-	return mtk_pctrl_init(pdev, &mt7623_pinctrl_data, NULL);
-}
-
-static const struct of_device_id mt7623_pctrl_match[] = {
-	{ .compatible = "mediatek,mt7623-pinctrl", },
-	{}
-};
-MODULE_DEVICE_TABLE(of, mt7623_pctrl_match);
-
-static struct platform_driver mtk_pinctrl_driver = {
-	.probe = mt7623_pinctrl_probe,
-	.driver = {
-		.name = "mediatek-mt7623-pinctrl",
-		.of_match_table = mt7623_pctrl_match,
-	},
-};
-
-static int __init mtk_pinctrl_init(void)
-{
-	return platform_driver_register(&mtk_pinctrl_driver);
-}
-
-arch_initcall(mtk_pinctrl_init);
diff --git a/drivers/pinctrl/mediatek/pinctrl-mtk-mt7623.h b/drivers/pinctrl/mediatek/pinctrl-mtk-mt7623.h
deleted file mode 100644
index e06cfc40da0f..000000000000
--- a/drivers/pinctrl/mediatek/pinctrl-mtk-mt7623.h
+++ /dev/null
@@ -1,1936 +0,0 @@
-/*
- * Copyright (c) 2016 John Crispin <john@phrozen.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#ifndef __PINCTRL_MTK_MT7623_H
-#define __PINCTRL_MTK_MT7623_H
-
-#include <linux/pinctrl/pinctrl.h>
-#include "pinctrl-mtk-common.h"
-
-static const struct mtk_desc_pin mtk_pins_mt7623[] = {
-	MTK_PIN(
-		PINCTRL_PIN(0, "PWRAP_SPI0_MI"),
-		"J20", "mt7623",
-		MTK_EINT_FUNCTION(0, 148),
-		MTK_FUNCTION(0, "GPIO0"),
-		MTK_FUNCTION(1, "PWRAP_SPIDO"),
-		MTK_FUNCTION(2, "PWRAP_SPIDI")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(1, "PWRAP_SPI0_MO"),
-		"D10", "mt7623",
-		MTK_EINT_FUNCTION(0, 149),
-		MTK_FUNCTION(0, "GPIO1"),
-		MTK_FUNCTION(1, "PWRAP_SPIDI"),
-		MTK_FUNCTION(2, "PWRAP_SPIDO")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(2, "PWRAP_INT"),
-		"E11", "mt7623",
-		MTK_EINT_FUNCTION(0, 150),
-		MTK_FUNCTION(0, "GPIO2"),
-		MTK_FUNCTION(1, "PWRAP_INT")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(3, "PWRAP_SPI0_CK"),
-		"H12", "mt7623",
-		MTK_EINT_FUNCTION(0, 151),
-		MTK_FUNCTION(0, "GPIO3"),
-		MTK_FUNCTION(1, "PWRAP_SPICK_I")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(4, "PWRAP_SPI0_CSN"),
-		"E12", "mt7623",
-		MTK_EINT_FUNCTION(0, 152),
-		MTK_FUNCTION(0, "GPIO4"),
-		MTK_FUNCTION(1, "PWRAP_SPICS_B_I")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(5, "PWRAP_SPI0_CK2"),
-		"H11", "mt7623",
-		MTK_EINT_FUNCTION(0, 155),
-		MTK_FUNCTION(0, "GPIO5"),
-		MTK_FUNCTION(1, "PWRAP_SPICK2_I")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(6, "PWRAP_SPI0_CSN2"),
-		"G11", "mt7623",
-		MTK_EINT_FUNCTION(0, 156),
-		MTK_FUNCTION(0, "GPIO6"),
-		MTK_FUNCTION(1, "PWRAP_SPICS2_B_I")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(7, "SPI1_CSN"),
-		"G19", "mt7623",
-		MTK_EINT_FUNCTION(0, 153),
-		MTK_FUNCTION(0, "GPIO7"),
-		MTK_FUNCTION(1, "SPI1_CS")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(8, "SPI1_MI"),
-		"F19", "mt7623",
-		MTK_EINT_FUNCTION(0, 154),
-		MTK_FUNCTION(0, "GPIO8"),
-		MTK_FUNCTION(1, "SPI1_MI"),
-		MTK_FUNCTION(2, "SPI1_MO")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(9, "SPI1_MO"),
-		"G20", "mt7623",
-		MTK_EINT_FUNCTION(0, 157),
-		MTK_FUNCTION(0, "GPIO9"),
-		MTK_FUNCTION(1, "SPI1_MO"),
-		MTK_FUNCTION(2, "SPI1_MI")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(10, "RTC32K_CK"),
-		"A13", "mt7623",
-		MTK_EINT_FUNCTION(0, 158),
-		MTK_FUNCTION(0, "GPIO10"),
-		MTK_FUNCTION(1, "RTC32K_CK")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(11, "WATCHDOG"),
-		"D14", "mt7623",
-		MTK_EINT_FUNCTION(0, 159),
-		MTK_FUNCTION(0, "GPIO11"),
-		MTK_FUNCTION(1, "WATCHDOG")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(12, "SRCLKENA"),
-		"C13", "mt7623",
-		MTK_EINT_FUNCTION(0, 169),
-		MTK_FUNCTION(0, "GPIO12"),
-		MTK_FUNCTION(1, "SRCLKENA")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(13, "SRCLKENAI"),
-		"B13", "mt7623",
-		MTK_EINT_FUNCTION(0, 161),
-		MTK_FUNCTION(0, "GPIO13"),
-		MTK_FUNCTION(1, "SRCLKENAI")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(14, "GPIO14"),
-		"E18", "mt7623",
-		MTK_EINT_FUNCTION(0, 162),
-		MTK_FUNCTION(0, "GPIO14"),
-		MTK_FUNCTION(1, "URXD2"),
-		MTK_FUNCTION(2, "UTXD2")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(15, "GPIO15"),
-		"E17", "mt7623",
-		MTK_EINT_FUNCTION(0, 163),
-		MTK_FUNCTION(0, "GPIO15"),
-		MTK_FUNCTION(1, "UTXD2"),
-		MTK_FUNCTION(2, "URXD2")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(16, "GPIO16"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO16")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(17, "GPIO17"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO17")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(18, "PCM_CLK"),
-		"C19", "mt7623",
-		MTK_EINT_FUNCTION(0, 166),
-		MTK_FUNCTION(0, "GPIO18"),
-		MTK_FUNCTION(1, "PCM_CLK0"),
-		MTK_FUNCTION(6, "AP_PCM_CLKO")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(19, "PCM_SYNC"),
-		"D19", "mt7623",
-		MTK_EINT_FUNCTION(0, 167),
-		MTK_FUNCTION(0, "GPIO19"),
-		MTK_FUNCTION(1, "PCM_SYNC"),
-		MTK_FUNCTION(6, "AP_PCM_SYNC")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(20, "PCM_RX"),
-		"D18", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO20"),
-		MTK_FUNCTION(1, "PCM_RX"),
-		MTK_FUNCTION(4, "PCM_TX"),
-		MTK_FUNCTION(6, "AP_PCM_RX")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(21, "PCM_TX"),
-		"C18", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO21"),
-		MTK_FUNCTION(1, "PCM_TX"),
-		MTK_FUNCTION(4, "PCM_RX"),
-		MTK_FUNCTION(6, "AP_PCM_TX")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(22, "EINT0"),
-		"H15", "mt7623",
-		MTK_EINT_FUNCTION(0, 0),
-		MTK_FUNCTION(0, "GPIO22"),
-		MTK_FUNCTION(1, "UCTS0"),
-		MTK_FUNCTION(2, "PCIE0_PERST_N")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(23, "EINT1"),
-		"J16", "mt7623",
-		MTK_EINT_FUNCTION(0, 1),
-		MTK_FUNCTION(0, "GPIO23"),
-		MTK_FUNCTION(1, "URTS0"),
-		MTK_FUNCTION(2, "PCIE1_PERST_N")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(24, "EINT2"),
-		"H16", "mt7623",
-		MTK_EINT_FUNCTION(0, 2),
-		MTK_FUNCTION(0, "GPIO24"),
-		MTK_FUNCTION(1, "UCTS1"),
-		MTK_FUNCTION(2, "PCIE2_PERST_N")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(25, "EINT3"),
-		"K15", "mt7623",
-		MTK_EINT_FUNCTION(0, 3),
-		MTK_FUNCTION(0, "GPIO25"),
-		MTK_FUNCTION(1, "URTS1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(26, "EINT4"),
-		"G15", "mt7623",
-		MTK_EINT_FUNCTION(0, 4),
-		MTK_FUNCTION(0, "GPIO26"),
-		MTK_FUNCTION(1, "UCTS3"),
-		MTK_FUNCTION(6, "PCIE2_WAKE_N")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(27, "EINT5"),
-		"F15", "mt7623",
-		MTK_EINT_FUNCTION(0, 5),
-		MTK_FUNCTION(0, "GPIO27"),
-		MTK_FUNCTION(1, "URTS3"),
-		MTK_FUNCTION(6, "PCIE1_WAKE_N")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(28, "EINT6"),
-		"J15", "mt7623",
-		MTK_EINT_FUNCTION(0, 6),
-		MTK_FUNCTION(0, "GPIO28"),
-		MTK_FUNCTION(1, "DRV_VBUS"),
-		MTK_FUNCTION(6, "PCIE0_WAKE_N")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(29, "EINT7"),
-		"E15", "mt7623",
-		MTK_EINT_FUNCTION(0, 7),
-		MTK_FUNCTION(0, "GPIO29"),
-		MTK_FUNCTION(1, "IDDIG"),
-		MTK_FUNCTION(2, "MSDC1_WP"),
-		MTK_FUNCTION(6, "PCIE2_PERST_N")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(30, "GPIO30"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO30")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(31, "GPIO31"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO31")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(32, "GPIO32"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO32")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(33, "I2S1_DATA"),
-		"Y18", "mt7623",
-		MTK_EINT_FUNCTION(0, 15),
-		MTK_FUNCTION(0, "GPIO33"),
-		MTK_FUNCTION(1, "I2S1_DATA"),
-		MTK_FUNCTION(3, "PCM_TX"),
-		MTK_FUNCTION(6, "AP_PCM_TX")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(34, "I2S1_DATA_IN"),
-		"Y17", "mt7623",
-		MTK_EINT_FUNCTION(0, 16),
-		MTK_FUNCTION(0, "GPIO34"),
-		MTK_FUNCTION(1, "I2S1_DATA_IN"),
-		MTK_FUNCTION(3, "PCM_RX"),
-		MTK_FUNCTION(6, "AP_PCM_RX")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(35, "I2S1_BCK"),
-		"V17", "mt7623",
-		MTK_EINT_FUNCTION(0, 17),
-		MTK_FUNCTION(0, "GPIO35"),
-		MTK_FUNCTION(1, "I2S1_BCK"),
-		MTK_FUNCTION(3, "PCM_CLK0"),
-		MTK_FUNCTION(6, "AP_PCM_CLKO")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(36, "I2S1_LRCK"),
-		"W17", "mt7623",
-		MTK_EINT_FUNCTION(0, 18),
-		MTK_FUNCTION(0, "GPIO36"),
-		MTK_FUNCTION(1, "I2S1_LRCK"),
-		MTK_FUNCTION(3, "PCM_SYNC"),
-		MTK_FUNCTION(6, "AP_PCM_SYNC")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(37, "I2S1_MCLK"),
-		"AA18", "mt7623",
-		MTK_EINT_FUNCTION(0, 19),
-		MTK_FUNCTION(0, "GPIO37"),
-		MTK_FUNCTION(1, "I2S1_MCLK")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(38, "GPIO38"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO38")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(39, "JTMS"),
-		"G21", "mt7623",
-		MTK_EINT_FUNCTION(0, 21),
-		MTK_FUNCTION(0, "GPIO39"),
-		MTK_FUNCTION(1, "JTMS")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(40, "GPIO40"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO40")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(41, "JTDI"),
-		"H22", "mt7623",
-		MTK_EINT_FUNCTION(0, 23),
-		MTK_FUNCTION(0, "GPIO41"),
-		MTK_FUNCTION(1, "JTDI")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(42, "JTDO"),
-		"H21", "mt7623",
-		MTK_EINT_FUNCTION(0, 24),
-		MTK_FUNCTION(0, "GPIO42"),
-		MTK_FUNCTION(1, "JTDO")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(43, "NCLE"),
-		"C7", "mt7623",
-		MTK_EINT_FUNCTION(0, 25),
-		MTK_FUNCTION(0, "GPIO43"),
-		MTK_FUNCTION(1, "NCLE"),
-		MTK_FUNCTION(2, "EXT_XCS2")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(44, "NCEB1"),
-		"C6", "mt7623",
-		MTK_EINT_FUNCTION(0, 26),
-		MTK_FUNCTION(0, "GPIO44"),
-		MTK_FUNCTION(1, "NCEB1"),
-		MTK_FUNCTION(2, "IDDIG")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(45, "NCEB0"),
-		"D7", "mt7623",
-		MTK_EINT_FUNCTION(0, 27),
-		MTK_FUNCTION(0, "GPIO45"),
-		MTK_FUNCTION(1, "NCEB0"),
-		MTK_FUNCTION(2, "DRV_VBUS")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(46, "IR"),
-		"D15", "mt7623",
-		MTK_EINT_FUNCTION(0, 28),
-		MTK_FUNCTION(0, "GPIO46"),
-		MTK_FUNCTION(1, "IR")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(47, "NREB"),
-		"A6", "mt7623",
-		MTK_EINT_FUNCTION(0, 29),
-		MTK_FUNCTION(0, "GPIO47"),
-		MTK_FUNCTION(1, "NREB")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(48, "NRNB"),
-		"B6", "mt7623",
-		MTK_EINT_FUNCTION(0, 30),
-		MTK_FUNCTION(0, "GPIO48"),
-		MTK_FUNCTION(1, "NRNB")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(49, "I2S0_DATA"),
-		"AB18", "mt7623",
-		MTK_EINT_FUNCTION(0, 31),
-		MTK_FUNCTION(0, "GPIO49"),
-		MTK_FUNCTION(1, "I2S0_DATA"),
-		MTK_FUNCTION(3, "PCM_TX"),
-		MTK_FUNCTION(6, "AP_I2S_DO")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(50, "GPIO50"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO50")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(51, "GPIO51"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO51")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(52, "GPIO52"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO52")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(53, "SPI0_CSN"),
-		"E7", "mt7623",
-		MTK_EINT_FUNCTION(0, 35),
-		MTK_FUNCTION(0, "GPIO53"),
-		MTK_FUNCTION(1, "SPI0_CS"),
-		MTK_FUNCTION(5, "PWM1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(54, "SPI0_CK"),
-		"F7", "mt7623",
-		MTK_EINT_FUNCTION(0, 36),
-		MTK_FUNCTION(0, "GPIO54"),
-		MTK_FUNCTION(1, "SPI0_CK")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(55, "SPI0_MI"),
-		"E6", "mt7623",
-		MTK_EINT_FUNCTION(0, 37),
-		MTK_FUNCTION(0, "GPIO55"),
-		MTK_FUNCTION(1, "SPI0_MI"),
-		MTK_FUNCTION(2, "SPI0_MO"),
-		MTK_FUNCTION(3, "MSDC1_WP"),
-		MTK_FUNCTION(5, "PWM2")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(56, "SPI0_MO"),
-		"G7", "mt7623",
-		MTK_EINT_FUNCTION(0, 38),
-		MTK_FUNCTION(0, "GPIO56"),
-		MTK_FUNCTION(1, "SPI0_MO"),
-		MTK_FUNCTION(2, "SPI0_MI")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(57, "GPIO57"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO57")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(58, "GPIO58"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO58")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(59, "GPIO59"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO59")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(60, "WB_RSTB"),
-		"Y21", "mt7623",
-		MTK_EINT_FUNCTION(0, 41),
-		MTK_FUNCTION(0, "GPIO60"),
-		MTK_FUNCTION(1, "WB_RSTB")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(61, "GPIO61"),
-		"AA21", "mt7623",
-		MTK_EINT_FUNCTION(0, 42),
-		MTK_FUNCTION(0, "GPIO61"),
-		MTK_FUNCTION(1, "TEST_FD")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(62, "GPIO62"),
-		"AB22", "mt7623",
-		MTK_EINT_FUNCTION(0, 43),
-		MTK_FUNCTION(0, "GPIO62"),
-		MTK_FUNCTION(1, "TEST_FC")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(63, "WB_SCLK"),
-		"AC23", "mt7623",
-		MTK_EINT_FUNCTION(0, 44),
-		MTK_FUNCTION(0, "GPIO63"),
-		MTK_FUNCTION(1, "WB_SCLK")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(64, "WB_SDATA"),
-		"AB21", "mt7623",
-		MTK_EINT_FUNCTION(0, 45),
-		MTK_FUNCTION(0, "GPIO64"),
-		MTK_FUNCTION(1, "WB_SDATA")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(65, "WB_SEN"),
-		"AB24", "mt7623",
-		MTK_EINT_FUNCTION(0, 46),
-		MTK_FUNCTION(0, "GPIO65"),
-		MTK_FUNCTION(1, "WB_SEN")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(66, "WB_CRTL0"),
-		"AB20", "mt7623",
-		MTK_EINT_FUNCTION(0, 47),
-		MTK_FUNCTION(0, "GPIO66"),
-		MTK_FUNCTION(1, "WB_CRTL0")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(67, "WB_CRTL1"),
-		"AC20", "mt7623",
-		MTK_EINT_FUNCTION(0, 48),
-		MTK_FUNCTION(0, "GPIO67"),
-		MTK_FUNCTION(1, "WB_CRTL1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(68, "WB_CRTL2"),
-		"AB19", "mt7623",
-		MTK_EINT_FUNCTION(0, 49),
-		MTK_FUNCTION(0, "GPIO68"),
-		MTK_FUNCTION(1, "WB_CRTL2")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(69, "WB_CRTL3"),
-		"AC19", "mt7623",
-		MTK_EINT_FUNCTION(0, 50),
-		MTK_FUNCTION(0, "GPIO69"),
-		MTK_FUNCTION(1, "WB_CRTL3")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(70, "WB_CRTL4"),
-		"AD19", "mt7623",
-		MTK_EINT_FUNCTION(0, 51),
-		MTK_FUNCTION(0, "GPIO70"),
-		MTK_FUNCTION(1, "WB_CRTL4")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(71, "WB_CRTL5"),
-		"AE19", "mt7623",
-		MTK_EINT_FUNCTION(0, 52),
-		MTK_FUNCTION(0, "GPIO71"),
-		MTK_FUNCTION(1, "WB_CRTL5")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(72, "I2S0_DATA_IN"),
-		"AA20", "mt7623",
-		MTK_EINT_FUNCTION(0, 53),
-		MTK_FUNCTION(0, "GPIO72"),
-		MTK_FUNCTION(1, "I2S0_DATA_IN"),
-		MTK_FUNCTION(3, "PCM_RX"),
-		MTK_FUNCTION(4, "PWM0"),
-		MTK_FUNCTION(5, "DISP_PWM"),
-		MTK_FUNCTION(6, "AP_I2S_DI")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(73, "I2S0_LRCK"),
-		"Y20", "mt7623",
-		MTK_EINT_FUNCTION(0, 54),
-		MTK_FUNCTION(0, "GPIO73"),
-		MTK_FUNCTION(1, "I2S0_LRCK"),
-		MTK_FUNCTION(3, "PCM_SYNC"),
-		MTK_FUNCTION(6, "AP_I2S_LRCK")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(74, "I2S0_BCK"),
-		"Y19", "mt7623",
-		MTK_EINT_FUNCTION(0, 55),
-		MTK_FUNCTION(0, "GPIO74"),
-		MTK_FUNCTION(1, "I2S0_BCK"),
-		MTK_FUNCTION(3, "PCM_CLK0"),
-		MTK_FUNCTION(6, "AP_I2S_BCK")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(75, "SDA0"),
-		"K19", "mt7623",
-		MTK_EINT_FUNCTION(0, 56),
-		MTK_FUNCTION(0, "GPIO75"),
-		MTK_FUNCTION(1, "SDA0")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(76, "SCL0"),
-		"K20", "mt7623",
-		MTK_EINT_FUNCTION(0, 57),
-		MTK_FUNCTION(0, "GPIO76"),
-		MTK_FUNCTION(1, "SCL0")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(77, "GPIO77"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO77")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(78, "GPIO78"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO78")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(79, "GPIO79"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO79")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(80, "GPIO80"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO80")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(81, "GPIO81"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO81")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(82, "GPIO82"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO82")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(83, "LCM_RST"),
-		"V16", "mt7623",
-		MTK_EINT_FUNCTION(0, 64),
-		MTK_FUNCTION(0, "GPIO83"),
-		MTK_FUNCTION(1, "LCM_RST")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(84, "DSI_TE"),
-		"V14", "mt7623",
-		MTK_EINT_FUNCTION(0, 65),
-		MTK_FUNCTION(0, "GPIO84"),
-		MTK_FUNCTION(1, "DSI_TE")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(85, "GPIO85"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO85")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(86, "GPIO86"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO86")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(87, "GPIO87"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO87")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(88, "GPIO88"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO88")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(89, "GPIO89"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO89")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(90, "GPIO90"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO90")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(91, "GPIO91"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO91")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(92, "GPIO92"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO92")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(93, "GPIO93"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO93")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(94, "GPIO94"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO94")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(95, "MIPI_TCN"),
-		"AB14", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO95"),
-		MTK_FUNCTION(1, "TCN")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(96, "MIPI_TCP"),
-		"AC14", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO96"),
-		MTK_FUNCTION(1, "TCP")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(97, "MIPI_TDN1"),
-		"AE15", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO97"),
-		MTK_FUNCTION(1, "TDN1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(98, "MIPI_TDP1"),
-		"AD15", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO98"),
-		MTK_FUNCTION(1, "TDP1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(99, "MIPI_TDN0"),
-		"AB15", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO99"),
-		MTK_FUNCTION(1, "TDN0")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(100, "MIPI_TDP0"),
-		"AC15", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO100"),
-		MTK_FUNCTION(1, "TDP0")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(101, "GPIO101"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO101")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(102, "GPIO102"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO102")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(103, "GPIO103"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO103")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(104, "GPIO104"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO104")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(105, "MSDC1_CMD"),
-		"AD2", "mt7623",
-		MTK_EINT_FUNCTION(0, 78),
-		MTK_FUNCTION(0, "GPIO105"),
-		MTK_FUNCTION(1, "MSDC1_CMD"),
-		MTK_FUNCTION(3, "SDA1"),
-		MTK_FUNCTION(6, "I2SOUT_BCK")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(106, "MSDC1_CLK"),
-		"AD3", "mt7623",
-		MTK_EINT_FUNCTION(0, 79),
-		MTK_FUNCTION(0, "GPIO106"),
-		MTK_FUNCTION(1, "MSDC1_CLK"),
-		MTK_FUNCTION(3, "SCL1"),
-		MTK_FUNCTION(6, "I2SOUT_LRCK")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(107, "MSDC1_DAT0"),
-		"AE2", "mt7623",
-		MTK_EINT_FUNCTION(0, 80),
-		MTK_FUNCTION(0, "GPIO107"),
-		MTK_FUNCTION(1, "MSDC1_DAT0"),
-		MTK_FUNCTION(5, "UTXD0"),
-		MTK_FUNCTION(6, "I2SOUT_DATA_OUT")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(108, "MSDC1_DAT1"),
-		"AC1", "mt7623",
-		MTK_EINT_FUNCTION(0, 81),
-		MTK_FUNCTION(0, "GPIO108"),
-		MTK_FUNCTION(1, "MSDC1_DAT1"),
-		MTK_FUNCTION(3, "PWM0"),
-		MTK_FUNCTION(5, "URXD0"),
-		MTK_FUNCTION(6, "PWM1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(109, "MSDC1_DAT2"),
-		"AC3", "mt7623",
-		MTK_EINT_FUNCTION(0, 82),
-		MTK_FUNCTION(0, "GPIO109"),
-		MTK_FUNCTION(1, "MSDC1_DAT2"),
-		MTK_FUNCTION(3, "SDA2"),
-		MTK_FUNCTION(5, "UTXD1"),
-		MTK_FUNCTION(6, "PWM2")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(110, "MSDC1_DAT3"),
-		"AC4", "mt7623",
-		MTK_EINT_FUNCTION(0, 83),
-		MTK_FUNCTION(0, "GPIO110"),
-		MTK_FUNCTION(1, "MSDC1_DAT3"),
-		MTK_FUNCTION(3, "SCL2"),
-		MTK_FUNCTION(5, "URXD1"),
-		MTK_FUNCTION(6, "PWM3")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(111, "MSDC0_DAT7"),
-		"A2", "mt7623",
-		MTK_EINT_FUNCTION(0, 84),
-		MTK_FUNCTION(0, "GPIO111"),
-		MTK_FUNCTION(1, "MSDC0_DAT7"),
-		MTK_FUNCTION(4, "NLD7")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(112, "MSDC0_DAT6"),
-		"B3", "mt7623",
-		MTK_EINT_FUNCTION(0, 85),
-		MTK_FUNCTION(0, "GPIO112"),
-		MTK_FUNCTION(1, "MSDC0_DAT6"),
-		MTK_FUNCTION(4, "NLD6")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(113, "MSDC0_DAT5"),
-		"C4", "mt7623",
-		MTK_EINT_FUNCTION(0, 86),
-		MTK_FUNCTION(0, "GPIO113"),
-		MTK_FUNCTION(1, "MSDC0_DAT5"),
-		MTK_FUNCTION(4, "NLD5")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(114, "MSDC0_DAT4"),
-		"A4", "mt7623",
-		MTK_EINT_FUNCTION(0, 87),
-		MTK_FUNCTION(0, "GPIO114"),
-		MTK_FUNCTION(1, "MSDC0_DAT4"),
-		MTK_FUNCTION(4, "NLD4")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(115, "MSDC0_RSTB"),
-		"C5", "mt7623",
-		MTK_EINT_FUNCTION(0, 88),
-		MTK_FUNCTION(0, "GPIO115"),
-		MTK_FUNCTION(1, "MSDC0_RSTB"),
-		MTK_FUNCTION(4, "NLD8")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(116, "MSDC0_CMD"),
-		"D5", "mt7623",
-		MTK_EINT_FUNCTION(0, 89),
-		MTK_FUNCTION(0, "GPIO116"),
-		MTK_FUNCTION(1, "MSDC0_CMD"),
-		MTK_FUNCTION(4, "NALE")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(117, "MSDC0_CLK"),
-		"B1", "mt7623",
-		MTK_EINT_FUNCTION(0, 90),
-		MTK_FUNCTION(0, "GPIO117"),
-		MTK_FUNCTION(1, "MSDC0_CLK"),
-		MTK_FUNCTION(4, "NWEB")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(118, "MSDC0_DAT3"),
-		"D6", "mt7623",
-		MTK_EINT_FUNCTION(0, 91),
-		MTK_FUNCTION(0, "GPIO118"),
-		MTK_FUNCTION(1, "MSDC0_DAT3"),
-		MTK_FUNCTION(4, "NLD3")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(119, "MSDC0_DAT2"),
-		"B2", "mt7623",
-		MTK_EINT_FUNCTION(0, 92),
-		MTK_FUNCTION(0, "GPIO119"),
-		MTK_FUNCTION(1, "MSDC0_DAT2"),
-		MTK_FUNCTION(4, "NLD2")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(120, "MSDC0_DAT1"),
-		"A3", "mt7623",
-		MTK_EINT_FUNCTION(0, 93),
-		MTK_FUNCTION(0, "GPIO120"),
-		MTK_FUNCTION(1, "MSDC0_DAT1"),
-		MTK_FUNCTION(4, "NLD1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(121, "MSDC0_DAT0"),
-		"B4", "mt7623",
-		MTK_EINT_FUNCTION(0, 94),
-		MTK_FUNCTION(0, "GPIO121"),
-		MTK_FUNCTION(1, "MSDC0_DAT0"),
-		MTK_FUNCTION(4, "NLD0"),
-		MTK_FUNCTION(5, "WATCHDOG")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(122, "GPIO122"),
-		"H17", "mt7623",
-		MTK_EINT_FUNCTION(0, 95),
-		MTK_FUNCTION(0, "GPIO122"),
-		MTK_FUNCTION(1, "TEST"),
-		MTK_FUNCTION(4, "SDA2"),
-		MTK_FUNCTION(5, "URXD0")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(123, "GPIO123"),
-		"F17", "mt7623",
-		MTK_EINT_FUNCTION(0, 96),
-		MTK_FUNCTION(0, "GPIO123"),
-		MTK_FUNCTION(1, "TEST"),
-		MTK_FUNCTION(4, "SCL2"),
-		MTK_FUNCTION(5, "UTXD0")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(124, "GPIO124"),
-		"H18", "mt7623",
-		MTK_EINT_FUNCTION(0, 97),
-		MTK_FUNCTION(0, "GPIO124"),
-		MTK_FUNCTION(1, "TEST"),
-		MTK_FUNCTION(4, "SDA1"),
-		MTK_FUNCTION(5, "PWM3")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(125, "GPIO125"),
-		"G17", "mt7623",
-		MTK_EINT_FUNCTION(0, 98),
-		MTK_FUNCTION(0, "GPIO125"),
-		MTK_FUNCTION(1, "TEST"),
-		MTK_FUNCTION(4, "SCL1"),
-		MTK_FUNCTION(5, "PWM4")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(126, "I2S0_MCLK"),
-		"AA19", "mt7623",
-		MTK_EINT_FUNCTION(0, 99),
-		MTK_FUNCTION(0, "GPIO126"),
-		MTK_FUNCTION(1, "I2S0_MCLK"),
-		MTK_FUNCTION(6, "AP_I2S_MCLK")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(127, "GPIO127"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO127")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(128, "GPIO128"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO128")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(129, "GPIO129"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO129")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(130, "GPIO130"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO130")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(131, "GPIO131"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO131")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(132, "GPIO132"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO132")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(133, "GPIO133"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO133")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(134, "GPIO134"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO134")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(135, "GPIO135"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO135")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(136, "GPIO136"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO136")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(137, "GPIO137"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO137")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(138, "GPIO138"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO138")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(139, "GPIO139"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO139")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(140, "GPIO140"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO140")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(141, "GPIO141"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO141")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(142, "GPIO142"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO142")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(143, "GPIO143"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO143")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(144, "GPIO144"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO144")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(145, "GPIO145"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO145")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(146, "GPIO146"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO146")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(147, "GPIO147"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO147")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(148, "GPIO148"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO148")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(149, "GPIO149"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO149")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(150, "GPIO150"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO150")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(151, "GPIO151"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO151")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(152, "GPIO152"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO152")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(153, "GPIO153"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO153")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(154, "GPIO154"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO154")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(155, "GPIO155"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO155")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(156, "GPIO156"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO156")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(157, "GPIO157"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO157")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(158, "GPIO158"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO158")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(159, "GPIO159"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO159")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(160, "GPIO160"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO160")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(161, "GPIO161"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO161")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(162, "GPIO162"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO162")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(163, "GPIO163"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO163")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(164, "GPIO164"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO164")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(165, "GPIO165"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO165")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(166, "GPIO166"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO166")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(167, "GPIO167"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO167")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(168, "GPIO168"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO168")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(169, "GPIO169"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO169")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(170, "GPIO170"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO170")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(171, "GPIO171"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO171")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(172, "GPIO172"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO172")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(173, "GPIO173"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO173")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(174, "GPIO174"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO174")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(175, "GPIO175"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO175")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(176, "GPIO176"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO176")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(177, "GPIO177"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO177")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(178, "GPIO178"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO178")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(179, "GPIO179"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO179")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(180, "GPIO180"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO180")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(181, "GPIO181"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO181")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(182, "GPIO182"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO182")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(183, "GPIO183"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO183")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(184, "GPIO184"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO184")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(185, "GPIO185"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO185")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(186, "GPIO186"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO186")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(187, "GPIO187"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO187")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(188, "GPIO188"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO188")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(189, "GPIO189"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO189")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(190, "GPIO190"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO190")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(191, "GPIO191"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO191")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(192, "GPIO192"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO192")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(193, "GPIO193"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO193")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(194, "GPIO194"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO194")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(195, "GPIO195"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO195")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(196, "GPIO196"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO196")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(197, "GPIO197"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO197")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(198, "GPIO198"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO198")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(199, "SPI1_CK"),
-		"E19", "mt7623",
-		MTK_EINT_FUNCTION(0, 111),
-		MTK_FUNCTION(0, "GPIO199"),
-		MTK_FUNCTION(1, "SPI1_CK")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(200, "URXD2"),
-		"K18", "mt7623",
-		MTK_EINT_FUNCTION(0, 112),
-		MTK_FUNCTION(0, "GPIO200"),
-		MTK_FUNCTION(6, "URXD2")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(201, "UTXD2"),
-		"L18", "mt7623",
-		MTK_EINT_FUNCTION(0, 113),
-		MTK_FUNCTION(0, "GPIO201"),
-		MTK_FUNCTION(6, "UTXD2")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(202, "GPIO202"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO202")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(203, "PWM0"),
-		"AA16", "mt7623",
-		MTK_EINT_FUNCTION(0, 115),
-		MTK_FUNCTION(0, "GPIO203"),
-		MTK_FUNCTION(1, "PWM0"),
-		MTK_FUNCTION(2, "DISP_PWM")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(204, "PWM1"),
-		"Y16", "mt7623",
-		MTK_EINT_FUNCTION(0, 116),
-		MTK_FUNCTION(0, "GPIO204"),
-		MTK_FUNCTION(1, "PWM1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(205, "PWM2"),
-		"AA15", "mt7623",
-		MTK_EINT_FUNCTION(0, 117),
-		MTK_FUNCTION(0, "GPIO205"),
-		MTK_FUNCTION(1, "PWM2")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(206, "PWM3"),
-		"AA17", "mt7623",
-		MTK_EINT_FUNCTION(0, 118),
-		MTK_FUNCTION(0, "GPIO206"),
-		MTK_FUNCTION(1, "PWM3")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(207, "PWM4"),
-		"Y15", "mt7623",
-		MTK_EINT_FUNCTION(0, 119),
-		MTK_FUNCTION(0, "GPIO207"),
-		MTK_FUNCTION(1, "PWM4")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(208, "AUD_EXT_CK1"),
-		"W14", "mt7623",
-		MTK_EINT_FUNCTION(0, 120),
-		MTK_FUNCTION(0, "GPIO208"),
-		MTK_FUNCTION(1, "AUD_EXT_CK1"),
-		MTK_FUNCTION(2, "PWM0"),
-		MTK_FUNCTION(3, "PCIE0_PERST_N"),
-		MTK_FUNCTION(5, "DISP_PWM")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(209, "AUD_EXT_CK2"),
-		"V15", "mt7623",
-		MTK_EINT_FUNCTION(0, 121),
-		MTK_FUNCTION(0, "GPIO209"),
-		MTK_FUNCTION(1, "AUD_EXT_CK2"),
-		MTK_FUNCTION(2, "MSDC1_WP"),
-		MTK_FUNCTION(3, "PCIE1_PERST_N"),
-		MTK_FUNCTION(5, "PWM1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(210, "GPIO210"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO210")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(211, "GPIO211"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO211")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(212, "GPIO212"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO212")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(213, "GPIO213"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO213")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(214, "GPIO214"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO214")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(215, "GPIO215"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO215")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(216, "GPIO216"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO216")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(217, "GPIO217"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO217")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(218, "GPIO218"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO218")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(219, "GPIO219"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO219")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(220, "GPIO220"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO220")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(221, "GPIO221"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO221")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(222, "GPIO222"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO222")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(223, "GPIO223"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO223")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(224, "GPIO224"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO224")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(225, "GPIO225"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO225")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(226, "GPIO226"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO226")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(227, "GPIO227"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO227")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(228, "GPIO228"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO228")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(229, "GPIO229"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO229")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(230, "GPIO230"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO230")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(231, "GPIO231"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO231")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(232, "GPIO232"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO232")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(233, "GPIO233"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO233")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(234, "GPIO234"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO234")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(235, "GPIO235"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO235")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(236, "EXT_SDIO3"),
-		"A8", "mt7623",
-		MTK_EINT_FUNCTION(0, 122),
-		MTK_FUNCTION(0, "GPIO236"),
-		MTK_FUNCTION(1, "EXT_SDIO3"),
-		MTK_FUNCTION(2, "IDDIG")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(237, "EXT_SDIO2"),
-		"D8", "mt7623",
-		MTK_EINT_FUNCTION(0, 123),
-		MTK_FUNCTION(0, "GPIO237"),
-		MTK_FUNCTION(1, "EXT_SDIO2"),
-		MTK_FUNCTION(2, "DRV_VBUS")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(238, "EXT_SDIO1"),
-		"D9", "mt7623",
-		MTK_EINT_FUNCTION(0, 124),
-		MTK_FUNCTION(0, "GPIO238"),
-		MTK_FUNCTION(1, "EXT_SDIO1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(239, "EXT_SDIO0"),
-		"B8", "mt7623",
-		MTK_EINT_FUNCTION(0, 125),
-		MTK_FUNCTION(0, "GPIO239"),
-		MTK_FUNCTION(1, "EXT_SDIO0")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(240, "EXT_XCS"),
-		"C9", "mt7623",
-		MTK_EINT_FUNCTION(0, 126),
-		MTK_FUNCTION(0, "GPIO240"),
-		MTK_FUNCTION(1, "EXT_XCS")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(241, "EXT_SCK"),
-		"C8", "mt7623",
-		MTK_EINT_FUNCTION(0, 127),
-		MTK_FUNCTION(0, "GPIO241"),
-		MTK_FUNCTION(1, "EXT_SCK")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(242, "URTS2"),
-		"G18", "mt7623",
-		MTK_EINT_FUNCTION(0, 128),
-		MTK_FUNCTION(0, "GPIO242"),
-		MTK_FUNCTION(1, "URTS2"),
-		MTK_FUNCTION(2, "UTXD3"),
-		MTK_FUNCTION(3, "URXD3"),
-		MTK_FUNCTION(4, "SCL1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(243, "UCTS2"),
-		"H19", "mt7623",
-		MTK_EINT_FUNCTION(0, 129),
-		MTK_FUNCTION(0, "GPIO243"),
-		MTK_FUNCTION(1, "UCTS2"),
-		MTK_FUNCTION(2, "URXD3"),
-		MTK_FUNCTION(3, "UTXD3"),
-		MTK_FUNCTION(4, "SDA1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(244, "GPIO244"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO244")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(245, "GPIO245"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO245")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(246, "GPIO246"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO246")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(247, "GPIO247"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO247")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(248, "GPIO248"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO248")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(249, "GPIO249"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO249")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(250, "GPIO250"),
-		"A15", "mt7623",
-		MTK_EINT_FUNCTION(0, 135),
-		MTK_FUNCTION(0, "GPIO250"),
-		MTK_FUNCTION(1, "TEST_MD7"),
-		MTK_FUNCTION(6, "PCIE0_CLKREQ_N")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(251, "GPIO251"),
-		"B15", "mt7623",
-		MTK_EINT_FUNCTION(0, 136),
-		MTK_FUNCTION(0, "GPIO251"),
-		MTK_FUNCTION(1, "TEST_MD6"),
-		MTK_FUNCTION(6, "PCIE0_WAKE_N")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(252, "GPIO252"),
-		"C16", "mt7623",
-		MTK_EINT_FUNCTION(0, 137),
-		MTK_FUNCTION(0, "GPIO252"),
-		MTK_FUNCTION(1, "TEST_MD5"),
-		MTK_FUNCTION(6, "PCIE1_CLKREQ_N")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(253, "GPIO253"),
-		"D17", "mt7623",
-		MTK_EINT_FUNCTION(0, 138),
-		MTK_FUNCTION(0, "GPIO253"),
-		MTK_FUNCTION(1, "TEST_MD4"),
-		MTK_FUNCTION(6, "PCIE1_WAKE_N")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(254, "GPIO254"),
-		"D16", "mt7623",
-		MTK_EINT_FUNCTION(0, 139),
-		MTK_FUNCTION(0, "GPIO254"),
-		MTK_FUNCTION(1, "TEST_MD3"),
-		MTK_FUNCTION(6, "PCIE2_CLKREQ_N")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(255, "GPIO255"),
-		"C17", "mt7623",
-		MTK_EINT_FUNCTION(0, 140),
-		MTK_FUNCTION(0, "GPIO255"),
-		MTK_FUNCTION(1, "TEST_MD2"),
-		MTK_FUNCTION(6, "PCIE2_WAKE_N")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(256, "GPIO256"),
-		"B17", "mt7623",
-		MTK_EINT_FUNCTION(0, 141),
-		MTK_FUNCTION(0, "GPIO256"),
-		MTK_FUNCTION(1, "TEST_MD1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(257, "GPIO257"),
-		"C15", "mt7623",
-		MTK_EINT_FUNCTION(0, 142),
-		MTK_FUNCTION(0, "GPIO257"),
-		MTK_FUNCTION(1, "TEST_MD0")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(258, "GPIO258"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO258")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(259, "GPIO259"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO259")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(260, "GPIO260"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO260")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(261, "MSDC1_INS"),
-		"AD1", "mt7623",
-		MTK_EINT_FUNCTION(0, 146),
-		MTK_FUNCTION(0, "GPIO261"),
-		MTK_FUNCTION(1, "MSDC1_INS")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(262, "G2_TXEN"),
-		"A23", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO262"),
-		MTK_FUNCTION(1, "G2_TXEN")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(263, "G2_TXD3"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO263"),
-		MTK_FUNCTION(1, "G2_TXD3")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(264, "G2_TXD2"),
-		"C24", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO264"),
-		MTK_FUNCTION(1, "G2_TXD2")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(265, "G2_TXD1"),
-		"B25", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO265"),
-		MTK_FUNCTION(1, "G2_TXD1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(266, "G2_TXD0"),
-		"A24", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO266"),
-		MTK_FUNCTION(1, "G2_TXD0")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(267, "G2_TXCLK"),
-		"C23", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO267"),
-		MTK_FUNCTION(1, "G2_TXC")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(268, "G2_RXCLK"),
-		"B23", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO268"),
-		MTK_FUNCTION(1, "G2_RXC")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(269, "G2_RXD0"),
-		"D21", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO269"),
-		MTK_FUNCTION(1, "G2_RXD0")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(270, "G2_RXD1"),
-		"B22", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO270"),
-		MTK_FUNCTION(1, "G2_RXD1")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(271, "G2_RXD2"),
-		"A22", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO271"),
-		MTK_FUNCTION(1, "G2_RXD2")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(272, "G2_RXD3"),
-		"C22", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO272"),
-		MTK_FUNCTION(1, "G2_RXD3")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(273, "GPIO273"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO273")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(274, "G2_RXDV"),
-		"C21", "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO274"),
-		MTK_FUNCTION(1, "G2_RXDV")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(275, "G2_MDC"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO275"),
-		MTK_FUNCTION(1, "MDC")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(276, "G2_MDIO"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO276"),
-		MTK_FUNCTION(1, "MDIO")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(277, "GPIO277"),
-		NULL, "mt7623",
-		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
-		MTK_FUNCTION(0, "GPIO277")
-	),
-	MTK_PIN(
-		PINCTRL_PIN(278, "JTAG_RESET"),
-		"H20", "mt7623",
-		MTK_EINT_FUNCTION(0, 147),
-		MTK_FUNCTION(0, "GPIO278"),
-		MTK_FUNCTION(1, "JTAG_RESET")
-	),
-};
-
-#endif /* __PINCTRL_MTK_MT7623_H */
diff --git a/drivers/pinctrl/meson/pinctrl-meson-gxbb.c b/drivers/pinctrl/meson/pinctrl-meson-gxbb.c
index 9b00be15d258..7bbc0d3cddcf 100644
--- a/drivers/pinctrl/meson/pinctrl-meson-gxbb.c
+++ b/drivers/pinctrl/meson/pinctrl-meson-gxbb.c
@@ -85,6 +85,7 @@ static const struct pinctrl_pin_desc meson_gxbb_periphs_pins[] = {
 	MESON_PIN(GPIODV_15, EE_OFF),
 	MESON_PIN(GPIODV_16, EE_OFF),
 	MESON_PIN(GPIODV_17, EE_OFF),
+	MESON_PIN(GPIODV_18, EE_OFF),
 	MESON_PIN(GPIODV_19, EE_OFF),
 	MESON_PIN(GPIODV_20, EE_OFF),
 	MESON_PIN(GPIODV_21, EE_OFF),
@@ -137,7 +138,6 @@ static const struct pinctrl_pin_desc meson_gxbb_periphs_pins[] = {
 	MESON_PIN(GPIOX_19, EE_OFF),
 	MESON_PIN(GPIOX_20, EE_OFF),
 	MESON_PIN(GPIOX_21, EE_OFF),
-	MESON_PIN(GPIOX_22, EE_OFF),
 
 	MESON_PIN(GPIOCLK_0, EE_OFF),
 	MESON_PIN(GPIOCLK_1, EE_OFF),
@@ -161,6 +161,11 @@ static const unsigned int nor_q_pins[]		= { PIN(BOOT_12, EE_OFF) };
 static const unsigned int nor_c_pins[]		= { PIN(BOOT_13, EE_OFF) };
 static const unsigned int nor_cs_pins[]		= { PIN(BOOT_15, EE_OFF) };
 
+static const unsigned int spi_sclk_pins[]	= { PIN(GPIOZ_6, EE_OFF) };
+static const unsigned int spi_ss0_pins[]	= { PIN(GPIOZ_7, EE_OFF) };
+static const unsigned int spi_miso_pins[]	= { PIN(GPIOZ_12, EE_OFF) };
+static const unsigned int spi_mosi_pins[]	= { PIN(GPIOZ_13, EE_OFF) };
+
 static const unsigned int sdcard_d0_pins[] = { PIN(CARD_1, EE_OFF) };
 static const unsigned int sdcard_d1_pins[] = { PIN(CARD_0, EE_OFF) };
 static const unsigned int sdcard_d2_pins[] = { PIN(CARD_5, EE_OFF) };
@@ -290,6 +295,9 @@ static const unsigned int i2s_out_ch45_ao_pins[] = { PIN(GPIOAO_13, 0) };
 static const unsigned int spdif_out_ao_6_pins[]	= { PIN(GPIOAO_6, 0) };
 static const unsigned int spdif_out_ao_13_pins[] = { PIN(GPIOAO_13, 0) };
 
+static const unsigned int ao_cec_pins[]          = { PIN(GPIOAO_12, 0) };
+static const unsigned int ee_cec_pins[]          = { PIN(GPIOAO_12, 0) };
+
 static struct meson_pmx_group meson_gxbb_periphs_groups[] = {
 	GPIO_GROUP(GPIOZ_0, EE_OFF),
 	GPIO_GROUP(GPIOZ_1, EE_OFF),
@@ -462,6 +470,10 @@ static struct meson_pmx_group meson_gxbb_periphs_groups[] = {
 	GROUP(eth_txd1,		6,	4),
 	GROUP(eth_txd2,		6,	3),
 	GROUP(eth_txd3,		6,	2),
+	GROUP(spi_ss0,		5,	26),
+	GROUP(spi_sclk,		5,	27),
+	GROUP(spi_miso,		5,	28),
+	GROUP(spi_mosi,		5,	29),
 
 	/* Bank H */
 	GROUP(hdmi_hpd,		1,	26),
@@ -551,6 +563,8 @@ static struct meson_pmx_group meson_gxbb_aobus_groups[] = {
 	GROUP(i2s_out_ch45_ao,	1,	1),
 	GROUP(spdif_out_ao_6,	0,	16),
 	GROUP(spdif_out_ao_13,	0,	4),
+	GROUP(ao_cec,           0,      15),
+	GROUP(ee_cec,           0,      14),
 };
 
 static const char * const gpio_periphs_groups[] = {
@@ -598,6 +612,10 @@ static const char * const nor_groups[] = {
 	"nor_d", "nor_q", "nor_c", "nor_cs",
 };
 
+static const char * const spi_groups[] = {
+	"spi_mosi", "spi_miso", "spi_ss0", "spi_sclk",
+};
+
 static const char * const sdcard_groups[] = {
 	"sdcard_d0", "sdcard_d1", "sdcard_d2", "sdcard_d3",
 	"sdcard_cmd", "sdcard_clk",
@@ -739,10 +757,15 @@ static const char * const spdif_out_ao_groups[] = {
 	"spdif_out_ao_6", "spdif_out_ao_13",
 };
 
+static const char * const cec_ao_groups[] = {
+	"ao_cec", "ee_cec",
+};
+
 static struct meson_pmx_func meson_gxbb_periphs_functions[] = {
 	FUNCTION(gpio_periphs),
 	FUNCTION(emmc),
 	FUNCTION(nor),
+	FUNCTION(spi),
 	FUNCTION(sdcard),
 	FUNCTION(sdio),
 	FUNCTION(nand),
@@ -779,23 +802,24 @@ static struct meson_pmx_func meson_gxbb_aobus_functions[] = {
 	FUNCTION(pwm_ao_b),
 	FUNCTION(i2s_out_ao),
 	FUNCTION(spdif_out_ao),
+	FUNCTION(cec_ao),
 };
 
 static struct meson_bank meson_gxbb_periphs_banks[] = {
-	/*   name    first                      last                    pullen  pull    dir     out     in  */
-	BANK("X",    PIN(GPIOX_0, EE_OFF),	PIN(GPIOX_22, EE_OFF),  4,  0,  4,  0,  12, 0,  13, 0,  14, 0),
-	BANK("Y",    PIN(GPIOY_0, EE_OFF),	PIN(GPIOY_16, EE_OFF),  1,  0,  1,  0,  3,  0,  4,  0,  5,  0),
-	BANK("DV",   PIN(GPIODV_0, EE_OFF),	PIN(GPIODV_29, EE_OFF), 0,  0,  0,  0,  0,  0,  1,  0,  2,  0),
-	BANK("H",    PIN(GPIOH_0, EE_OFF),	PIN(GPIOH_3, EE_OFF),   1, 20,  1, 20,  3, 20,  4, 20,  5, 20),
-	BANK("Z",    PIN(GPIOZ_0, EE_OFF),	PIN(GPIOZ_15, EE_OFF),  3,  0,  3,  0,  9,  0,  10, 0, 11,  0),
-	BANK("CARD", PIN(CARD_0, EE_OFF),	PIN(CARD_6, EE_OFF),    2, 20,  2, 20,  6, 20,  7, 20,  8, 20),
-	BANK("BOOT", PIN(BOOT_0, EE_OFF),	PIN(BOOT_17, EE_OFF),   2,  0,  2,  0,  6,  0,  7,  0,  8,  0),
-	BANK("CLK",  PIN(GPIOCLK_0, EE_OFF),	PIN(GPIOCLK_3, EE_OFF), 3, 28,  3, 28,  9, 28, 10, 28, 11, 28),
+	/*   name    first                      last                    irq       pullen  pull    dir     out     in  */
+	BANK("X",    PIN(GPIOX_0, EE_OFF),	PIN(GPIOX_22, EE_OFF),  106, 128, 4,  0,  4,  0,  12, 0,  13, 0,  14, 0),
+	BANK("Y",    PIN(GPIOY_0, EE_OFF),	PIN(GPIOY_16, EE_OFF),   89, 105, 1,  0,  1,  0,  3,  0,  4,  0,  5,  0),
+	BANK("DV",   PIN(GPIODV_0, EE_OFF),	PIN(GPIODV_29, EE_OFF),  59,  88, 0,  0,  0,  0,  0,  0,  1,  0,  2,  0),
+	BANK("H",    PIN(GPIOH_0, EE_OFF),	PIN(GPIOH_3, EE_OFF),    30,  33, 1, 20,  1, 20,  3, 20,  4, 20,  5, 20),
+	BANK("Z",    PIN(GPIOZ_0, EE_OFF),	PIN(GPIOZ_15, EE_OFF),   14,  29, 3,  0,  3,  0,  9,  0,  10, 0, 11,  0),
+	BANK("CARD", PIN(CARD_0, EE_OFF),	PIN(CARD_6, EE_OFF),     52,  58, 2, 20,  2, 20,  6, 20,  7, 20,  8, 20),
+	BANK("BOOT", PIN(BOOT_0, EE_OFF),	PIN(BOOT_17, EE_OFF),    34,  51, 2,  0,  2,  0,  6,  0,  7,  0,  8,  0),
+	BANK("CLK",  PIN(GPIOCLK_0, EE_OFF),	PIN(GPIOCLK_3, EE_OFF), 129, 132, 3, 28,  3, 28,  9, 28, 10, 28, 11, 28),
 };
 
 static struct meson_bank meson_gxbb_aobus_banks[] = {
-	/*   name    first              last               pullen  pull    dir     out     in  */
-	BANK("AO",   PIN(GPIOAO_0, 0),  PIN(GPIOAO_13, 0), 0,  0,  0, 16,  0,  0,  0, 16,  1,  0),
+	/*   name    first              last               irq    pullen  pull    dir     out     in  */
+	BANK("AO",   PIN(GPIOAO_0, 0),  PIN(GPIOAO_13, 0), 0, 13, 0,  0,  0, 16,  0,  0,  0, 16,  1,  0),
 };
 
 struct meson_pinctrl_data meson_gxbb_periphs_pinctrl_data = {
diff --git a/drivers/pinctrl/meson/pinctrl-meson-gxl.c b/drivers/pinctrl/meson/pinctrl-meson-gxl.c
index 998210eacf37..36c14b85fc7c 100644
--- a/drivers/pinctrl/meson/pinctrl-meson-gxl.c
+++ b/drivers/pinctrl/meson/pinctrl-meson-gxl.c
@@ -89,6 +89,7 @@ static const struct pinctrl_pin_desc meson_gxl_periphs_pins[] = {
 	MESON_PIN(GPIODV_15, EE_OFF),
 	MESON_PIN(GPIODV_16, EE_OFF),
 	MESON_PIN(GPIODV_17, EE_OFF),
+	MESON_PIN(GPIODV_18, EE_OFF),
 	MESON_PIN(GPIODV_19, EE_OFF),
 	MESON_PIN(GPIODV_20, EE_OFF),
 	MESON_PIN(GPIODV_21, EE_OFF),
@@ -141,6 +142,11 @@ static const unsigned int nor_q_pins[]		= { PIN(BOOT_12, EE_OFF) };
 static const unsigned int nor_c_pins[]		= { PIN(BOOT_13, EE_OFF) };
 static const unsigned int nor_cs_pins[]		= { PIN(BOOT_15, EE_OFF) };
 
+static const unsigned int spi_mosi_pins[]	= { PIN(GPIOX_8, EE_OFF) };
+static const unsigned int spi_miso_pins[]	= { PIN(GPIOX_9, EE_OFF) };
+static const unsigned int spi_ss0_pins[]	= { PIN(GPIOX_10, EE_OFF) };
+static const unsigned int spi_sclk_pins[]	= { PIN(GPIOX_11, EE_OFF) };
+
 static const unsigned int sdcard_d0_pins[] = { PIN(CARD_1, EE_OFF) };
 static const unsigned int sdcard_d1_pins[] = { PIN(CARD_0, EE_OFF) };
 static const unsigned int sdcard_d2_pins[] = { PIN(CARD_5, EE_OFF) };
@@ -234,6 +240,28 @@ static const unsigned int i2s_out_ch67_z_pins[]	= { PIN(GPIOZ_7, EE_OFF) };
 
 static const unsigned int spdif_out_h_pins[]	= { PIN(GPIOH_4, EE_OFF) };
 
+static const unsigned int eth_link_led_pins[]	= { PIN(GPIOZ_14, EE_OFF) };
+static const unsigned int eth_act_led_pins[]	= { PIN(GPIOZ_15, EE_OFF) };
+
+static const unsigned int tsin_a_d0_pins[]	= { PIN(GPIODV_0, EE_OFF) };
+static const unsigned int tsin_a_d0_x_pins[]	= { PIN(GPIOX_10, EE_OFF) };
+static const unsigned int tsin_a_clk_pins[]	= { PIN(GPIODV_8, EE_OFF) };
+static const unsigned int tsin_a_clk_x_pins[]	= { PIN(GPIOX_11, EE_OFF) };
+static const unsigned int tsin_a_sop_pins[]	= { PIN(GPIODV_9, EE_OFF) };
+static const unsigned int tsin_a_sop_x_pins[]	= { PIN(GPIOX_8, EE_OFF) };
+static const unsigned int tsin_a_d_valid_pins[]	= { PIN(GPIODV_10, EE_OFF) };
+static const unsigned int tsin_a_d_valid_x_pins[] = { PIN(GPIOX_9, EE_OFF) };
+static const unsigned int tsin_a_fail_pins[]	= { PIN(GPIODV_11, EE_OFF) };
+static const unsigned int tsin_a_dp_pins[] = {
+	PIN(GPIODV_1, EE_OFF),
+	PIN(GPIODV_2, EE_OFF),
+	PIN(GPIODV_3, EE_OFF),
+	PIN(GPIODV_4, EE_OFF),
+	PIN(GPIODV_5, EE_OFF),
+	PIN(GPIODV_6, EE_OFF),
+	PIN(GPIODV_7, EE_OFF),
+};
+
 static const struct pinctrl_pin_desc meson_gxl_aobus_pins[] = {
 	MESON_PIN(GPIOAO_0, 0),
 	MESON_PIN(GPIOAO_1, 0),
@@ -271,11 +299,14 @@ static const unsigned int pwm_ao_a_8_pins[]	= { PIN(GPIOAO_8, 0) };
 static const unsigned int pwm_ao_b_pins[]	= { PIN(GPIOAO_9, 0) };
 static const unsigned int pwm_ao_b_6_pins[]	= { PIN(GPIOAO_6, 0) };
 
-static const unsigned int i2s_out_ch23_ao_pins[] = { PIN(GPIOAO_8, EE_OFF) };
-static const unsigned int i2s_out_ch45_ao_pins[] = { PIN(GPIOAO_9, EE_OFF) };
+static const unsigned int i2s_out_ch23_ao_pins[] = { PIN(GPIOAO_8, 0) };
+static const unsigned int i2s_out_ch45_ao_pins[] = { PIN(GPIOAO_9, 0) };
 
-static const unsigned int spdif_out_ao_6_pins[]	= { PIN(GPIOAO_6, EE_OFF) };
-static const unsigned int spdif_out_ao_9_pins[]	= { PIN(GPIOAO_9, EE_OFF) };
+static const unsigned int spdif_out_ao_6_pins[]	= { PIN(GPIOAO_6, 0) };
+static const unsigned int spdif_out_ao_9_pins[]	= { PIN(GPIOAO_9, 0) };
+
+static const unsigned int ao_cec_pins[]		= { PIN(GPIOAO_8, 0) };
+static const unsigned int ee_cec_pins[]		= { PIN(GPIOAO_8, 0) };
 
 static struct meson_pmx_group meson_gxl_periphs_groups[] = {
 	GPIO_GROUP(GPIOZ_0, EE_OFF),
@@ -405,6 +436,14 @@ static struct meson_pmx_group meson_gxl_periphs_groups[] = {
 	GROUP(pwm_a,		5,	25),
 	GROUP(pwm_e,		5,	15),
 	GROUP(pwm_f_x,		5,	14),
+	GROUP(spi_mosi,		5,	3),
+	GROUP(spi_miso,		5,	2),
+	GROUP(spi_ss0,		5,	1),
+	GROUP(spi_sclk,		5,	0),
+	GROUP(tsin_a_sop_x,	6,	3),
+	GROUP(tsin_a_d_valid_x,	6,	2),
+	GROUP(tsin_a_d0_x,	6,	1),
+	GROUP(tsin_a_clk_x,	6,	0),
 
 	/* Bank Z */
 	GROUP(eth_mdio,		4,	23),
@@ -425,6 +464,8 @@ static struct meson_pmx_group meson_gxl_periphs_groups[] = {
 	GROUP(i2s_out_ch23_z,	3,	26),
 	GROUP(i2s_out_ch45_z,	3,	25),
 	GROUP(i2s_out_ch67_z,	3,	24),
+	GROUP(eth_link_led,	4,	25),
+	GROUP(eth_act_led,	4,	24),
 
 	/* Bank H */
 	GROUP(hdmi_hpd,		6,	31),
@@ -451,6 +492,12 @@ static struct meson_pmx_group meson_gxl_periphs_groups[] = {
 	GROUP(i2c_sck_c,	1,	10),
 	GROUP(pwm_b,		2,	11),
 	GROUP(pwm_d,		2,	12),
+	GROUP(tsin_a_d0,	2,	4),
+	GROUP(tsin_a_dp,	2,	3),
+	GROUP(tsin_a_clk,	2,	2),
+	GROUP(tsin_a_sop,	2,	1),
+	GROUP(tsin_a_d_valid,	2,	0),
+	GROUP(tsin_a_fail,	1,	31),
 
 	/* Bank BOOT */
 	GROUP(emmc_nand_d07,	7,	31),
@@ -518,6 +565,8 @@ static struct meson_pmx_group meson_gxl_aobus_groups[] = {
 	GROUP(i2s_out_ch45_ao,	1,	1),
 	GROUP(spdif_out_ao_6,	0,	16),
 	GROUP(spdif_out_ao_9,	0,	4),
+	GROUP(ao_cec,		0,	15),
+	GROUP(ee_cec,		0,	14),
 };
 
 static const char * const gpio_periphs_groups[] = {
@@ -560,6 +609,10 @@ static const char * const nor_groups[] = {
 	"nor_d", "nor_q", "nor_c", "nor_cs",
 };
 
+static const char * const spi_groups[] = {
+	"spi_mosi", "spi_miso", "spi_ss0", "spi_sclk",
+};
+
 static const char * const sdcard_groups[] = {
 	"sdcard_d0", "sdcard_d1", "sdcard_d2", "sdcard_d3",
 	"sdcard_cmd", "sdcard_clk",
@@ -647,6 +700,16 @@ static const char * const spdif_out_groups[] = {
 	"spdif_out_h",
 };
 
+static const char * const eth_led_groups[] = {
+	"eth_link_led", "eth_act_led",
+};
+
+static const char * const tsin_a_groups[] = {
+	"tsin_a_clk", "tsin_a_clk_x", "tsin_a_sop", "tsin_a_sop_x",
+	"tsin_a_d_valid", "tsin_a_d_valid_x", "tsin_a_d0", "tsin_a_d0_x",
+	"tsin_a_dp", "tsin_a_fail",
+};
+
 static const char * const gpio_aobus_groups[] = {
 	"GPIOAO_0", "GPIOAO_1", "GPIOAO_2", "GPIOAO_3", "GPIOAO_4",
 	"GPIOAO_5", "GPIOAO_6", "GPIOAO_7", "GPIOAO_8", "GPIOAO_9",
@@ -689,10 +752,15 @@ static const char * const spdif_out_ao_groups[] = {
 	"spdif_out_ao_6", "spdif_out_ao_9",
 };
 
+static const char * const cec_ao_groups[] = {
+	"ao_cec", "ee_cec",
+};
+
 static struct meson_pmx_func meson_gxl_periphs_functions[] = {
 	FUNCTION(gpio_periphs),
 	FUNCTION(emmc),
 	FUNCTION(nor),
+	FUNCTION(spi),
 	FUNCTION(sdcard),
 	FUNCTION(sdio),
 	FUNCTION(nand),
@@ -713,6 +781,8 @@ static struct meson_pmx_func meson_gxl_periphs_functions[] = {
 	FUNCTION(hdmi_i2c),
 	FUNCTION(i2s_out),
 	FUNCTION(spdif_out),
+	FUNCTION(eth_led),
+	FUNCTION(tsin_a),
 };
 
 static struct meson_pmx_func meson_gxl_aobus_functions[] = {
@@ -726,22 +796,23 @@ static struct meson_pmx_func meson_gxl_aobus_functions[] = {
 	FUNCTION(pwm_ao_b),
 	FUNCTION(i2s_out_ao),
 	FUNCTION(spdif_out_ao),
+	FUNCTION(cec_ao),
 };
 
 static struct meson_bank meson_gxl_periphs_banks[] = {
-	/*   name    first                      last                    pullen  pull    dir     out     in  */
-	BANK("X",    PIN(GPIOX_0, EE_OFF),	PIN(GPIOX_18, EE_OFF),  4,  0,  4,  0,  12, 0,  13, 0,  14, 0),
-	BANK("DV",   PIN(GPIODV_0, EE_OFF),	PIN(GPIODV_29, EE_OFF), 0,  0,  0,  0,  0,  0,  1,  0,  2,  0),
-	BANK("H",    PIN(GPIOH_0, EE_OFF),	PIN(GPIOH_9, EE_OFF),   1, 20,  1, 20,  3, 20,  4, 20,  5, 20),
-	BANK("Z",    PIN(GPIOZ_0, EE_OFF),	PIN(GPIOZ_15, EE_OFF),  3,  0,  3,  0,  9,  0,  10, 0, 11,  0),
-	BANK("CARD", PIN(CARD_0, EE_OFF),	PIN(CARD_6, EE_OFF),    2, 20,  2, 20,  6, 20,  7, 20,  8, 20),
-	BANK("BOOT", PIN(BOOT_0, EE_OFF),	PIN(BOOT_15, EE_OFF),   2,  0,  2,  0,  6,  0,  7,  0,  8,  0),
-	BANK("CLK",  PIN(GPIOCLK_0, EE_OFF),	PIN(GPIOCLK_1, EE_OFF), 3, 28,  3, 28,  9, 28, 10, 28, 11, 28),
+	/*   name    first                      last                    irq	  pullen  pull    dir     out     in  */
+	BANK("X",    PIN(GPIOX_0, EE_OFF),	PIN(GPIOX_18, EE_OFF),   89, 107, 4,  0,  4,  0,  12, 0,  13, 0,  14, 0),
+	BANK("DV",   PIN(GPIODV_0, EE_OFF),	PIN(GPIODV_29, EE_OFF),  83,  88, 0,  0,  0,  0,  0,  0,  1,  0,  2,  0),
+	BANK("H",    PIN(GPIOH_0, EE_OFF),	PIN(GPIOH_9, EE_OFF),    26,  35, 1, 20,  1, 20,  3, 20,  4, 20,  5, 20),
+	BANK("Z",    PIN(GPIOZ_0, EE_OFF),	PIN(GPIOZ_15, EE_OFF),   10,  25, 3,  0,  3,  0,  9,  0,  10, 0, 11,  0),
+	BANK("CARD", PIN(CARD_0, EE_OFF),	PIN(CARD_6, EE_OFF),     52,  58, 2, 20,  2, 20,  6, 20,  7, 20,  8, 20),
+	BANK("BOOT", PIN(BOOT_0, EE_OFF),	PIN(BOOT_15, EE_OFF),    36,  51, 2,  0,  2,  0,  6,  0,  7,  0,  8,  0),
+	BANK("CLK",  PIN(GPIOCLK_0, EE_OFF),	PIN(GPIOCLK_1, EE_OFF), 108, 109, 3, 28,  3, 28,  9, 28, 10, 28, 11, 28),
 };
 
 static struct meson_bank meson_gxl_aobus_banks[] = {
-	/*   name    first              last              pullen  pull    dir     out     in  */
-	BANK("AO",   PIN(GPIOAO_0, 0),  PIN(GPIOAO_9, 0), 0,  0,  0, 16,  0,  0,  0, 16,  1,  0),
+	/*   name    first              last              irq	pullen  pull    dir     out     in  */
+	BANK("AO",   PIN(GPIOAO_0, 0),  PIN(GPIOAO_9, 0), 0, 9, 0,  0,  0, 16,  0,  0,  0, 16,  1,  0),
 };
 
 struct meson_pinctrl_data meson_gxl_periphs_pinctrl_data = {
diff --git a/drivers/pinctrl/meson/pinctrl-meson.h b/drivers/pinctrl/meson/pinctrl-meson.h
index 1aa871d5431e..890f296f5840 100644
--- a/drivers/pinctrl/meson/pinctrl-meson.h
+++ b/drivers/pinctrl/meson/pinctrl-meson.h
@@ -81,6 +81,7 @@ enum meson_reg_type {
  * @name:	bank name
  * @first:	first pin of the bank
  * @last:	last pin of the bank
+ * @irq:	hwirq base number of the bank
  * @regs:	array of register descriptors
  *
  * A bank represents a set of pins controlled by a contiguous set of
@@ -92,6 +93,8 @@ struct meson_bank {
 	const char *name;
 	unsigned int first;
 	unsigned int last;
+	int irq_first;
+	int irq_last;
 	struct meson_reg_desc regs[NUM_REG];
 };
 
@@ -147,12 +150,14 @@ struct meson_pinctrl {
 		.num_groups = ARRAY_SIZE(fn ## _groups),		\
 	}
 
-#define BANK(n, f, l, per, peb, pr, pb, dr, db, or, ob, ir, ib)		\
+#define BANK(n, f, l, fi, li, per, peb, pr, pb, dr, db, or, ob, ir, ib)	\
 	{								\
-		.name	= n,						\
-		.first	= f,						\
-		.last	= l,						\
-		.regs	= {						\
+		.name		= n,					\
+		.first		= f,					\
+		.last		= l,					\
+		.irq_first	= fi,					\
+		.irq_last	= li,					\
+		.regs = {						\
 			[REG_PULLEN]	= { per, peb },			\
 			[REG_PULL]	= { pr, pb },			\
 			[REG_DIR]	= { dr, db },			\
diff --git a/drivers/pinctrl/meson/pinctrl-meson8.c b/drivers/pinctrl/meson/pinctrl-meson8.c
index 07f1cb21c1b8..970f6f14502c 100644
--- a/drivers/pinctrl/meson/pinctrl-meson8.c
+++ b/drivers/pinctrl/meson/pinctrl-meson8.c
@@ -205,6 +205,9 @@ static const unsigned int i2c_sck_d0_pins[] = { PIN(GPIOX_17, 0) };
 static const unsigned int xtal_32k_out_pins[] = { PIN(GPIOX_10, 0) };
 static const unsigned int xtal_24m_out_pins[] = { PIN(GPIOX_11, 0) };
 
+static const unsigned int pwm_e_pins[] = { PIN(GPIOX_10, 0) };
+static const unsigned int pwm_b_x_pins[] = { PIN(GPIOX_11, 0) };
+
 /* bank Y */
 static const unsigned int uart_tx_c_pins[] = { PIN(GPIOY_0, 0) };
 static const unsigned int uart_rx_c_pins[] = { PIN(GPIOY_1, 0) };
@@ -219,6 +222,20 @@ static const unsigned int pcm_clk_b_pins[] = { PIN(GPIOY_7, 0) };
 static const unsigned int i2c_sda_c0_pins[] = { PIN(GPIOY_0, 0) };
 static const unsigned int i2c_sck_c0_pins[] = { PIN(GPIOY_1, 0) };
 
+static const unsigned int pwm_a_y_pins[] = { PIN(GPIOY_16, 0) };
+
+static const unsigned int i2s_out_ch45_pins[] = { PIN(GPIOY_0, 0) };
+static const unsigned int i2s_out_ch23_pins[] = { PIN(GPIOY_1, 0) };
+static const unsigned int i2s_out_ch01_pins[] = { PIN(GPIOY_4, 0) };
+static const unsigned int i2s_in_ch01_pins[] = { PIN(GPIOY_5, 0) };
+static const unsigned int i2s_lr_clk_in_pins[] = { PIN(GPIOY_6, 0) };
+static const unsigned int i2s_ao_clk_in_pins[] = { PIN(GPIOY_7, 0) };
+static const unsigned int i2s_am_clk_pins[] = { PIN(GPIOY_8, 0) };
+static const unsigned int i2s_out_ch78_pins[] = { PIN(GPIOY_9, 0) };
+
+static const unsigned int spdif_in_pins[] = { PIN(GPIOY_2, 0) };
+static const unsigned int spdif_out_pins[] = { PIN(GPIOY_3, 0) };
+
 /* bank DV */
 static const unsigned int dvin_rgb_pins[] = { PIN(GPIODV_0, 0), PIN(GPIODV_1, 0),
 					      PIN(GPIODV_2, 0), PIN(GPIODV_3, 0),
@@ -264,6 +281,10 @@ static const unsigned int uart_rts_b1_pins[] = { PIN(GPIODV_27, 0) };
 static const unsigned int vga_vs_pins[] = { PIN(GPIODV_24, 0) };
 static const unsigned int vga_hs_pins[] = { PIN(GPIODV_25, 0) };
 
+static const unsigned int pwm_c_dv9_pins[] = { PIN(GPIODV_9, 0) };
+static const unsigned int pwm_c_dv29_pins[] = { PIN(GPIODV_29, 0) };
+static const unsigned int pwm_d_pins[] = { PIN(GPIODV_28, 0) };
+
 /* bank H */
 static const unsigned int hdmi_hpd_pins[] = { PIN(GPIOH_0, 0) };
 static const unsigned int hdmi_sda_pins[] = { PIN(GPIOH_1, 0) };
@@ -312,6 +333,11 @@ static const unsigned int i2c_sck_a1_pins[] = { PIN(GPIOZ_1, 0) };
 static const unsigned int i2c_sda_a2_pins[] = { PIN(GPIOZ_0, 0) };
 static const unsigned int i2c_sck_a2_pins[] = { PIN(GPIOZ_1, 0) };
 
+static const unsigned int pwm_a_z0_pins[] = { PIN(GPIOZ_0, 0) };
+static const unsigned int pwm_a_z7_pins[] = { PIN(GPIOZ_7, 0) };
+static const unsigned int pwm_b_z_pins[] = { PIN(GPIOZ_1, 0) };
+static const unsigned int pwm_c_z_pins[] = { PIN(GPIOZ_8, 0) };
+
 /* bank BOOT */
 static const unsigned int sd_d0_c_pins[] = { PIN(BOOT_0, 0) };
 static const unsigned int sd_d1_c_pins[] = { PIN(BOOT_1, 0) };
@@ -369,6 +395,7 @@ static const unsigned int uart_cts_ao_a_pins[] = { PIN(GPIOAO_2, AO_OFF) };
 static const unsigned int uart_rts_ao_a_pins[] = { PIN(GPIOAO_3, AO_OFF) };
 
 static const unsigned int remote_input_pins[] = { PIN(GPIOAO_7, AO_OFF) };
+static const unsigned int remote_output_ao_pins[] = { PIN(GPIOAO_13, AO_OFF) };
 
 static const unsigned int i2c_slave_sck_ao_pins[] = { PIN(GPIOAO_4, AO_OFF) };
 static const unsigned int i2c_slave_sda_ao_pins[] = { PIN(GPIOAO_5, AO_OFF) };
@@ -382,6 +409,15 @@ static const unsigned int uart_rx_ao_b1_pins[] = { PIN(GPIOAO_5, AO_OFF) };
 static const unsigned int i2c_mst_sck_ao_pins[] = { PIN(GPIOAO_4, AO_OFF) };
 static const unsigned int i2c_mst_sda_ao_pins[] = { PIN(GPIOAO_5, AO_OFF) };
 
+static const unsigned int pwm_f_ao_pins[] = { PIN(GPIO_TEST_N, AO_OFF) };
+
+static const unsigned int i2s_am_clk_out_ao_pins[] = { PIN(GPIOAO_8, AO_OFF) };
+static const unsigned int i2s_ao_clk_out_ao_pins[] = { PIN(GPIOAO_9, AO_OFF) };
+static const unsigned int i2s_lr_clk_out_ao_pins[] = { PIN(GPIOAO_10, AO_OFF) };
+static const unsigned int i2s_out_ch01_ao_pins[] = { PIN(GPIOAO_11, AO_OFF) };
+
+static const unsigned int hdmi_cec_ao_pins[] = { PIN(GPIOAO_12, AO_OFF) };
+
 static struct meson_pmx_group meson8_cbus_groups[] = {
 	GPIO_GROUP(GPIOX_0, 0),
 	GPIO_GROUP(GPIOX_1, 0),
@@ -523,6 +559,9 @@ static struct meson_pmx_group meson8_cbus_groups[] = {
 	GROUP(xtal_32k_out,	3,	22),
 	GROUP(xtal_24m_out,	3,	23),
 
+	GROUP(pwm_e,		9,	19),
+	GROUP(pwm_b_x,		2,	3),
+
 	/* bank Y */
 	GROUP(uart_tx_c,	1,	19),
 	GROUP(uart_rx_c,	1,	18),
@@ -537,6 +576,20 @@ static struct meson_pmx_group meson8_cbus_groups[] = {
 	GROUP(i2c_sda_c0,	1,	15),
 	GROUP(i2c_sck_c0,	1,	14),
 
+	GROUP(pwm_a_y,		9,	14),
+
+	GROUP(i2s_out_ch45,	1,	10),
+	GROUP(i2s_out_ch23,	1,	19),
+	GROUP(i2s_out_ch01,	1,	6),
+	GROUP(i2s_in_ch01,	1,	5),
+	GROUP(i2s_lr_clk_in,	1,	4),
+	GROUP(i2s_ao_clk_in,	1,	2),
+	GROUP(i2s_am_clk,	1,	0),
+	GROUP(i2s_out_ch78,	1,	11),
+
+	GROUP(spdif_in,		1,	8),
+	GROUP(spdif_out,	1,	7),
+
 	/* bank DV */
 	GROUP(dvin_rgb,		0,	6),
 	GROUP(dvin_vs,		0,	9),
@@ -571,6 +624,10 @@ static struct meson_pmx_group meson8_cbus_groups[] = {
 	GROUP(vga_vs,		0,	21),
 	GROUP(vga_hs,		0,	20),
 
+	GROUP(pwm_c_dv9,	3,	24),
+	GROUP(pwm_c_dv29,	3,	25),
+	GROUP(pwm_d,		3,	26),
+
 	/* bank H */
 	GROUP(hdmi_hpd,		1,	26),
 	GROUP(hdmi_sda,		1,	25),
@@ -619,6 +676,11 @@ static struct meson_pmx_group meson8_cbus_groups[] = {
 	GROUP(i2c_sda_a2,	5,	7),
 	GROUP(i2c_sck_a2,	5,	6),
 
+	GROUP(pwm_a_z0,		9,	16),
+	GROUP(pwm_a_z7,		2,	0),
+	GROUP(pwm_b_z,		9,	15),
+	GROUP(pwm_c_z,		2,	1),
+
 	/* bank BOOT */
 	GROUP(sd_d0_c,		6,	29),
 	GROUP(sd_d1_c,		6,	28),
@@ -689,6 +751,7 @@ static struct meson_pmx_group meson8_aobus_groups[] = {
 	GROUP(uart_rts_ao_a,		0,	9),
 
 	GROUP(remote_input,		0,	0),
+	GROUP(remote_output_ao,		0,	31),
 
 	GROUP(i2c_slave_sck_ao,		0,	2),
 	GROUP(i2c_slave_sda_ao,		0,	1),
@@ -701,6 +764,15 @@ static struct meson_pmx_group meson8_aobus_groups[] = {
 
 	GROUP(i2c_mst_sck_ao,		0,	6),
 	GROUP(i2c_mst_sda_ao,		0,	5),
+
+	GROUP(pwm_f_ao,			0,	19),
+
+	GROUP(i2s_am_clk_out_ao,	0,	30),
+	GROUP(i2s_ao_clk_out_ao,	0,	29),
+	GROUP(i2s_lr_clk_out_ao,	0,	28),
+	GROUP(i2s_out_ch01_ao,		0,	27),
+
+	GROUP(hdmi_cec_ao,		0,	17),
 };
 
 static const char * const gpio_groups[] = {
@@ -828,6 +900,12 @@ static const char * const i2c_b_groups[] = {
 	"i2c_sda_b", "i2c_sck_b"
 };
 
+static const char * const i2s_groups[] = {
+	"i2s_out_ch45", "i2s_out_ch23_pins", "i2s_out_ch01_pins",
+	"i2s_in_ch01_pins", "i2s_lr_clk_in_pins", "i2s_ao_clk_in_pins",
+	"i2s_am_clk_pins", "i2s_out_ch78_pins"
+};
+
 static const char * const sd_c_groups[] = {
 	"sd_d0_c", "sd_d1_c", "sd_d2_c", "sd_d3_c",
 	"sd_cmd_c", "sd_clk_c"
@@ -849,6 +927,26 @@ static const char * const nor_groups[] = {
 	"nor_d", "nor_q", "nor_c", "nor_cs"
 };
 
+static const char * const pwm_a_groups[] = {
+	"pwm_a_y", "pwm_a_z0", "pwm_a_z7"
+};
+
+static const char * const pwm_b_groups[] = {
+	"pwm_b_x", "pwm_b_z"
+};
+
+static const char * const pwm_c_groups[] = {
+	"pwm_c_dv9", "pwm_c_dv29", "pwm_c_z"
+};
+
+static const char * const pwm_d_groups[] = {
+	"pwm_d"
+};
+
+static const char * const pwm_e_groups[] = {
+	"pwm_e"
+};
+
 static const char * const sd_b_groups[] = {
 	"sd_d1_b", "sd_d0_b", "sd_clk_b", "sd_cmd_b",
 	"sd_d3_b", "sd_d2_b"
@@ -858,12 +956,16 @@ static const char * const sdxc_b_groups[] = {
 	"sdxc_d13_b", "sdxc_d0_b", "sdxc_clk_b", "sdxc_cmd_b"
 };
 
+static const char * const spdif_groups[] = {
+	"spdif_in", "spdif_out"
+};
+
 static const char * const uart_ao_groups[] = {
 	"uart_tx_ao_a", "uart_rx_ao_a", "uart_cts_ao_a", "uart_rts_ao_a"
 };
 
 static const char * const remote_groups[] = {
-	"remote_input"
+	"remote_input", "remote_output_ao"
 };
 
 static const char * const i2c_slave_ao_groups[] = {
@@ -878,6 +980,19 @@ static const char * const i2c_mst_ao_groups[] = {
 	"i2c_mst_sck_ao", "i2c_mst_sda_ao"
 };
 
+static const char * const pwm_f_ao_groups[] = {
+	"pwm_f_ao"
+};
+
+static const char * const i2s_ao_groups[] = {
+	"i2s_am_clk_out_ao", "i2s_ao_clk_out_ao", "i2s_lr_clk_out_ao",
+	"i2s_out_ch01_ao"
+};
+
+static const char * const hdmi_cec_ao_groups[] = {
+	"hdmi_cec_ao"
+};
+
 static struct meson_pmx_func meson8_cbus_functions[] = {
 	FUNCTION(gpio),
 	FUNCTION(sd_a),
@@ -905,6 +1020,13 @@ static struct meson_pmx_func meson8_cbus_functions[] = {
 	FUNCTION(nor),
 	FUNCTION(sd_b),
 	FUNCTION(sdxc_b),
+	FUNCTION(pwm_a),
+	FUNCTION(pwm_b),
+	FUNCTION(pwm_c),
+	FUNCTION(pwm_d),
+	FUNCTION(pwm_e),
+	FUNCTION(i2s),
+	FUNCTION(spdif),
 };
 
 static struct meson_pmx_func meson8_aobus_functions[] = {
@@ -913,22 +1035,25 @@ static struct meson_pmx_func meson8_aobus_functions[] = {
 	FUNCTION(i2c_slave_ao),
 	FUNCTION(uart_ao_b),
 	FUNCTION(i2c_mst_ao),
+	FUNCTION(pwm_f_ao),
+	FUNCTION(i2s_ao),
+	FUNCTION(hdmi_cec_ao),
 };
 
 static struct meson_bank meson8_cbus_banks[] = {
-	/*   name    first             last                 pullen  pull    dir     out     in  */
-	BANK("X",    PIN(GPIOX_0, 0),  PIN(GPIOX_21, 0),    4,  0,  4,  0,  0,  0,  1,  0,  2,  0),
-	BANK("Y",    PIN(GPIOY_0, 0),  PIN(GPIOY_16, 0),    3,  0,  3,  0,  3,  0,  4,  0,  5,  0),
-	BANK("DV",   PIN(GPIODV_0, 0), PIN(GPIODV_29, 0),   0,  0,  0,  0,  7,  0,  8,  0,  9,  0),
-	BANK("H",    PIN(GPIOH_0, 0),  PIN(GPIOH_9, 0),     1, 16,  1, 16,  9, 19, 10, 19, 11, 19),
-	BANK("Z",    PIN(GPIOZ_0, 0),  PIN(GPIOZ_14, 0),    1,  0,  1,  0,  3, 17,  4, 17,  5, 17),
-	BANK("CARD", PIN(CARD_0, 0),   PIN(CARD_6, 0),      2, 20,  2, 20,  0, 22,  1, 22,  2, 22),
-	BANK("BOOT", PIN(BOOT_0, 0),   PIN(BOOT_18, 0),     2,  0,  2,  0,  9,  0, 10,  0, 11,  0),
+	/*   name    first             last                 irq       pullen  pull    dir     out     in  */
+	BANK("X",    PIN(GPIOX_0, 0),  PIN(GPIOX_21, 0),    112, 133, 4,  0,  4,  0,  0,  0,  1,  0,  2,  0),
+	BANK("Y",    PIN(GPIOY_0, 0),  PIN(GPIOY_16, 0),    95,  111, 3,  0,  3,  0,  3,  0,  4,  0,  5,  0),
+	BANK("DV",   PIN(GPIODV_0, 0), PIN(GPIODV_29, 0),   65,   94, 0,  0,  0,  0,  7,  0,  8,  0,  9,  0),
+	BANK("H",    PIN(GPIOH_0, 0),  PIN(GPIOH_9, 0),     29,   38, 1, 16,  1, 16,  9, 19, 10, 19, 11, 19),
+	BANK("Z",    PIN(GPIOZ_0, 0),  PIN(GPIOZ_14, 0),    14,   28, 1,  0,  1,  0,  3, 17,  4, 17,  5, 17),
+	BANK("CARD", PIN(CARD_0, 0),   PIN(CARD_6, 0),      58,   64, 2, 20,  2, 20,  0, 22,  1, 22,  2, 22),
+	BANK("BOOT", PIN(BOOT_0, 0),   PIN(BOOT_18, 0),     39,   57, 2,  0,  2,  0,  9,  0, 10,  0, 11,  0),
 };
 
 static struct meson_bank meson8_aobus_banks[] = {
-	/*   name    first                  last                      pullen  pull    dir     out     in  */
-	BANK("AO",   PIN(GPIOAO_0, AO_OFF), PIN(GPIO_TEST_N, AO_OFF), 0,  0,  0, 16,  0,  0,  0, 16,  1,  0),
+	/*   name    first                  last                      irq    pullen  pull    dir     out     in  */
+	BANK("AO",   PIN(GPIOAO_0, AO_OFF), PIN(GPIO_TEST_N, AO_OFF), 0, 13, 0,  0,  0, 16,  0,  0,  0, 16,  1,  0),
 };
 
 struct meson_pinctrl_data meson8_cbus_pinctrl_data = {
diff --git a/drivers/pinctrl/meson/pinctrl-meson8b.c b/drivers/pinctrl/meson/pinctrl-meson8b.c
index bf747eb1f3f4..71f216b5b0b9 100644
--- a/drivers/pinctrl/meson/pinctrl-meson8b.c
+++ b/drivers/pinctrl/meson/pinctrl-meson8b.c
@@ -124,6 +124,12 @@ static const struct pinctrl_pin_desc meson8b_aobus_pins[] = {
 	MESON_PIN(GPIOAO_11, AO_OFF),
 	MESON_PIN(GPIOAO_12, AO_OFF),
 	MESON_PIN(GPIOAO_13, AO_OFF),
+
+	/*
+	 * The following 2 pins are not mentionned in the public datasheet
+	 * According to this datasheet, they can't be used with the gpio
+	 * interrupt controller
+	 */
 	MESON_PIN(GPIO_BSD_EN, AO_OFF),
 	MESON_PIN(GPIO_TEST_N, AO_OFF),
 };
@@ -881,19 +887,25 @@ static struct meson_pmx_func meson8b_aobus_functions[] = {
 };
 
 static struct meson_bank meson8b_cbus_banks[] = {
-	/*   name    first                      last                   pullen  pull    dir     out     in  */
-	BANK("X",    PIN(GPIOX_0, 0),		PIN(GPIOX_21, 0),      4,  0,  4,  0,  0,  0,  1,  0,  2,  0),
-	BANK("Y",    PIN(GPIOY_0, 0),		PIN(GPIOY_14, 0),      3,  0,  3,  0,  3,  0,  4,  0,  5,  0),
-	BANK("DV",   PIN(GPIODV_9, 0),		PIN(GPIODV_29, 0),     0,  0,  0,  0,  7,  0,  8,  0,  9,  0),
-	BANK("H",    PIN(GPIOH_0, 0),		PIN(GPIOH_9, 0),       1, 16,  1, 16,  9, 19, 10, 19, 11, 19),
-	BANK("CARD", PIN(CARD_0, 0),		PIN(CARD_6, 0),        2, 20,  2, 20,  0, 22,  1, 22,  2, 22),
-	BANK("BOOT", PIN(BOOT_0, 0),		PIN(BOOT_18, 0),       2,  0,  2,  0,  9,  0, 10,  0, 11,  0),
-	BANK("DIF",  PIN(DIF_0_P, 0),		PIN(DIF_4_N, 0),       5,  8,  5,  8, 12, 12, 13, 12, 14, 12),
+	/*   name    first                      last                irq      pullen  pull    dir     out     in  */
+	BANK("X",    PIN(GPIOX_0, 0),		PIN(GPIOX_21, 0),   97, 118, 4,  0,  4,  0,  0,  0,  1,  0,  2,  0),
+	BANK("Y",    PIN(GPIOY_0, 0),		PIN(GPIOY_14, 0),   80,  96, 3,  0,  3,  0,  3,  0,  4,  0,  5,  0),
+	BANK("DV",   PIN(GPIODV_9, 0),		PIN(GPIODV_29, 0),  59,  79, 0,  0,  0,  0,  7,  0,  8,  0,  9,  0),
+	BANK("H",    PIN(GPIOH_0, 0),		PIN(GPIOH_9, 0),    14,  23, 1, 16,  1, 16,  9, 19, 10, 19, 11, 19),
+	BANK("CARD", PIN(CARD_0, 0),		PIN(CARD_6, 0),     43,  49, 2, 20,  2, 20,  0, 22,  1, 22,  2, 22),
+	BANK("BOOT", PIN(BOOT_0, 0),		PIN(BOOT_18, 0),    24,  42, 2,  0,  2,  0,  9,  0, 10,  0, 11,  0),
+
+	/*
+	 * The following bank is not mentionned in the public datasheet
+	 * There is no information whether it can be used with the gpio
+	 * interrupt controller
+	 */
+	BANK("DIF",  PIN(DIF_0_P, 0),		PIN(DIF_4_N, 0),    -1,  -1, 5,  8,  5,  8, 12, 12, 13, 12, 14, 12),
 };
 
 static struct meson_bank meson8b_aobus_banks[] = {
-	/*   name    first                  last                      pullen  pull    dir     out     in  */
-	BANK("AO",   PIN(GPIOAO_0, AO_OFF), PIN(GPIO_TEST_N, AO_OFF), 0,  0,  0, 16,  0,  0,  0, 16,  1,  0),
+	/*   name    first                  last                      irq    pullen  pull    dir     out     in  */
+	BANK("AO",   PIN(GPIOAO_0, AO_OFF), PIN(GPIO_TEST_N, AO_OFF), 0, 13, 0,  0,  0, 16,  0,  0,  0, 16,  1,  0),
 };
 
 struct meson_pinctrl_data meson8b_cbus_pinctrl_data = {
diff --git a/drivers/pinctrl/mvebu/Kconfig b/drivers/pinctrl/mvebu/Kconfig
index 5bade32d3089..d9773b77ff9f 100644
--- a/drivers/pinctrl/mvebu/Kconfig
+++ b/drivers/pinctrl/mvebu/Kconfig
@@ -1,5 +1,3 @@
-if PLAT_ORION
-
 config PINCTRL_MVEBU
 	bool
 	select PINMUX
@@ -30,6 +28,14 @@ config PINCTRL_ARMADA_39X
 	bool
 	select PINCTRL_MVEBU
 
+config PINCTRL_ARMADA_AP806
+	bool
+	select PINCTRL_MVEBU
+
+config PINCTRL_ARMADA_CP110
+	bool
+	select PINCTRL_MVEBU
+
 config PINCTRL_ARMADA_XP
 	bool
 	select PINCTRL_MVEBU
@@ -38,8 +44,6 @@ config PINCTRL_ORION
 	bool
 	select PINCTRL_MVEBU
 
-endif
-
 config PINCTRL_ARMADA_37XX
        bool
        select GENERIC_PINCONF
diff --git a/drivers/pinctrl/mvebu/Makefile b/drivers/pinctrl/mvebu/Makefile
index 60c245a60f39..5b03fd55e28d 100644
--- a/drivers/pinctrl/mvebu/Makefile
+++ b/drivers/pinctrl/mvebu/Makefile
@@ -5,6 +5,8 @@ obj-$(CONFIG_PINCTRL_ARMADA_370) += pinctrl-armada-370.o
 obj-$(CONFIG_PINCTRL_ARMADA_375) += pinctrl-armada-375.o
 obj-$(CONFIG_PINCTRL_ARMADA_38X) += pinctrl-armada-38x.o
 obj-$(CONFIG_PINCTRL_ARMADA_39X) += pinctrl-armada-39x.o
+obj-$(CONFIG_PINCTRL_ARMADA_AP806) += pinctrl-armada-ap806.o
+obj-$(CONFIG_PINCTRL_ARMADA_CP110) += pinctrl-armada-cp110.o
 obj-$(CONFIG_PINCTRL_ARMADA_XP)  += pinctrl-armada-xp.o
 obj-$(CONFIG_PINCTRL_ARMADA_37XX)  += pinctrl-armada-37xx.o
 obj-$(CONFIG_PINCTRL_ORION)  += pinctrl-orion.o
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-37xx.c b/drivers/pinctrl/mvebu/pinctrl-armada-37xx.c
index 5c96f5558310..f024e25787fc 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-37xx.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-37xx.c
@@ -13,7 +13,9 @@
 #include <linux/gpio/driver.h>
 #include <linux/mfd/syscon.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 #include <linux/of_device.h>
+#include <linux/of_irq.h>
 #include <linux/pinctrl/pinconf-generic.h>
 #include <linux/pinctrl/pinconf.h>
 #include <linux/pinctrl/pinctrl.h>
@@ -30,6 +32,11 @@
 #define OUTPUT_CTL	0x20
 #define SELECTION	0x30
 
+#define IRQ_EN		0x0
+#define IRQ_POL		0x08
+#define IRQ_STATUS	0x10
+#define IRQ_WKUP	0x18
+
 #define NB_FUNCS 2
 #define GPIO_PER_REG	32
 
@@ -75,9 +82,12 @@ struct armada_37xx_pmx_func {
 
 struct armada_37xx_pinctrl {
 	struct regmap			*regmap;
+	void __iomem			*base;
 	const struct armada_37xx_pin_data	*data;
 	struct device			*dev;
 	struct gpio_chip		gpio_chip;
+	struct irq_chip			irq_chip;
+	spinlock_t			irq_lock;
 	struct pinctrl_desc		pctl;
 	struct pinctrl_dev		*pctl_dev;
 	struct armada_37xx_pin_group	*groups;
@@ -147,8 +157,9 @@ static struct armada_37xx_pin_group armada_37xx_nb_groups[] = {
 	PIN_GRP_GPIO("onewire", 4, 1, BIT(16), "onewire"),
 	PIN_GRP_GPIO("uart1", 25, 2, BIT(17), "uart"),
 	PIN_GRP_GPIO("spi_quad", 15, 2, BIT(18), "spi"),
-	PIN_GRP_EXTRA("uart2", 9, 2, BIT(13) | BIT(14) | BIT(19),
-		      BIT(13) | BIT(14), BIT(19), 18, 2, "gpio", "uart"),
+	PIN_GRP_EXTRA("uart2", 9, 2, BIT(1) | BIT(13) | BIT(14) | BIT(19),
+		      BIT(1) | BIT(13) | BIT(14), BIT(1) | BIT(19),
+		      18, 2, "gpio", "uart"),
 	PIN_GRP_GPIO("led0_od", 11, 1, BIT(20), "led"),
 	PIN_GRP_GPIO("led1_od", 12, 1, BIT(21), "led"),
 	PIN_GRP_GPIO("led2_od", 13, 1, BIT(22), "led"),
@@ -159,8 +170,8 @@ static struct armada_37xx_pin_group armada_37xx_nb_groups[] = {
 static struct armada_37xx_pin_group armada_37xx_sb_groups[] = {
 	PIN_GRP_GPIO("usb32_drvvbus0", 0, 1, BIT(0), "drvbus"),
 	PIN_GRP_GPIO("usb2_drvvbus1", 1, 1, BIT(1), "drvbus"),
-	PIN_GRP_GPIO("sdio_sb", 24, 5, BIT(2), "sdio"),
-	PIN_GRP_EXTRA("rgmii", 6, 14, BIT(3), 0, BIT(3), 23, 1, "mii", "gpio"),
+	PIN_GRP_GPIO("sdio_sb", 24, 6, BIT(2), "sdio"),
+	PIN_GRP_EXTRA("rgmii", 6, 12, BIT(3), 0, BIT(3), 23, 1, "mii", "gpio"),
 	PIN_GRP_GPIO("pcie1", 3, 2, BIT(4), "pcie"),
 	PIN_GRP_GPIO("ptp", 20, 3, BIT(5), "ptp"),
 	PIN_GRP("ptp_clk", 21, 1, BIT(6), "ptp", "mii"),
@@ -346,6 +357,14 @@ static int armada_37xx_pmx_set(struct pinctrl_dev *pctldev,
 	return armada_37xx_pmx_set_by_name(pctldev, name, grp);
 }
 
+static inline void armada_37xx_irq_update_reg(unsigned int *reg,
+					  struct irq_data *d)
+{
+	int offset = irqd_to_hwirq(d);
+
+	armada_37xx_update_reg(reg, offset);
+}
+
 static int armada_37xx_gpio_direction_input(struct gpio_chip *chip,
 					    unsigned int offset)
 {
@@ -468,6 +487,214 @@ static const struct gpio_chip armada_37xx_gpiolib_chip = {
 	.owner = THIS_MODULE,
 };
 
+static void armada_37xx_irq_ack(struct irq_data *d)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
+	struct armada_37xx_pinctrl *info = gpiochip_get_data(chip);
+	u32 reg = IRQ_STATUS;
+	unsigned long flags;
+
+	armada_37xx_irq_update_reg(&reg, d);
+	spin_lock_irqsave(&info->irq_lock, flags);
+	writel(d->mask, info->base + reg);
+	spin_unlock_irqrestore(&info->irq_lock, flags);
+}
+
+static void armada_37xx_irq_mask(struct irq_data *d)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
+	struct armada_37xx_pinctrl *info = gpiochip_get_data(chip);
+	u32 val, reg = IRQ_EN;
+	unsigned long flags;
+
+	armada_37xx_irq_update_reg(&reg, d);
+	spin_lock_irqsave(&info->irq_lock, flags);
+	val = readl(info->base + reg);
+	writel(val & ~d->mask, info->base + reg);
+	spin_unlock_irqrestore(&info->irq_lock, flags);
+}
+
+static void armada_37xx_irq_unmask(struct irq_data *d)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
+	struct armada_37xx_pinctrl *info = gpiochip_get_data(chip);
+	u32 val, reg = IRQ_EN;
+	unsigned long flags;
+
+	armada_37xx_irq_update_reg(&reg, d);
+	spin_lock_irqsave(&info->irq_lock, flags);
+	val = readl(info->base + reg);
+	writel(val | d->mask, info->base + reg);
+	spin_unlock_irqrestore(&info->irq_lock, flags);
+}
+
+static int armada_37xx_irq_set_wake(struct irq_data *d, unsigned int on)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
+	struct armada_37xx_pinctrl *info = gpiochip_get_data(chip);
+	u32 val, reg = IRQ_WKUP;
+	unsigned long flags;
+
+	armada_37xx_irq_update_reg(&reg, d);
+	spin_lock_irqsave(&info->irq_lock, flags);
+	val = readl(info->base + reg);
+	if (on)
+		val |= d->mask;
+	else
+		val &= ~d->mask;
+	writel(val, info->base + reg);
+	spin_unlock_irqrestore(&info->irq_lock, flags);
+
+	return 0;
+}
+
+static int armada_37xx_irq_set_type(struct irq_data *d, unsigned int type)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
+	struct armada_37xx_pinctrl *info = gpiochip_get_data(chip);
+	u32 val, reg = IRQ_POL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&info->irq_lock, flags);
+	armada_37xx_irq_update_reg(&reg, d);
+	val = readl(info->base + reg);
+	switch (type) {
+	case IRQ_TYPE_EDGE_RISING:
+		val &= ~d->mask;
+		break;
+	case IRQ_TYPE_EDGE_FALLING:
+		val |= d->mask;
+		break;
+	default:
+		spin_unlock_irqrestore(&info->irq_lock, flags);
+		return -EINVAL;
+	}
+	writel(val, info->base + reg);
+	spin_unlock_irqrestore(&info->irq_lock, flags);
+
+	return 0;
+}
+
+
+static void armada_37xx_irq_handler(struct irq_desc *desc)
+{
+	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+	struct armada_37xx_pinctrl *info = gpiochip_get_data(gc);
+	struct irq_domain *d = gc->irqdomain;
+	int i;
+
+	chained_irq_enter(chip, desc);
+	for (i = 0; i <= d->revmap_size / GPIO_PER_REG; i++) {
+		u32 status;
+		unsigned long flags;
+
+		spin_lock_irqsave(&info->irq_lock, flags);
+		status = readl_relaxed(info->base + IRQ_STATUS + 4 * i);
+		/* Manage only the interrupt that was enabled */
+		status &= readl_relaxed(info->base + IRQ_EN + 4 * i);
+		spin_unlock_irqrestore(&info->irq_lock, flags);
+		while (status) {
+			u32 hwirq = ffs(status) - 1;
+			u32 virq = irq_find_mapping(d, hwirq +
+						     i * GPIO_PER_REG);
+
+			generic_handle_irq(virq);
+
+			/* Update status in case a new IRQ appears */
+			spin_lock_irqsave(&info->irq_lock, flags);
+			status = readl_relaxed(info->base +
+					       IRQ_STATUS + 4 * i);
+			/* Manage only the interrupt that was enabled */
+			status &= readl_relaxed(info->base + IRQ_EN + 4 * i);
+			spin_unlock_irqrestore(&info->irq_lock, flags);
+		}
+	}
+	chained_irq_exit(chip, desc);
+}
+
+static int armada_37xx_irqchip_register(struct platform_device *pdev,
+					struct armada_37xx_pinctrl *info)
+{
+	struct device_node *np = info->dev->of_node;
+	int nrirqs = info->data->nr_pins;
+	struct gpio_chip *gc = &info->gpio_chip;
+	struct irq_chip *irqchip = &info->irq_chip;
+	struct resource res;
+	int ret = -ENODEV, i, nr_irq_parent;
+
+	/* Check if we have at least one gpio-controller child node */
+	for_each_child_of_node(info->dev->of_node, np) {
+		if (of_property_read_bool(np, "gpio-controller")) {
+			ret = 0;
+			break;
+		}
+	};
+	if (ret)
+		return ret;
+
+	nr_irq_parent = of_irq_count(np);
+	spin_lock_init(&info->irq_lock);
+
+	if (!nr_irq_parent) {
+		dev_err(&pdev->dev, "Invalid or no IRQ\n");
+		return 0;
+	}
+
+	if (of_address_to_resource(info->dev->of_node, 1, &res)) {
+		dev_err(info->dev, "cannot find IO resource\n");
+		return -ENOENT;
+	}
+
+	info->base = devm_ioremap_resource(info->dev, &res);
+	if (IS_ERR(info->base))
+		return PTR_ERR(info->base);
+
+	irqchip->irq_ack = armada_37xx_irq_ack;
+	irqchip->irq_mask = armada_37xx_irq_mask;
+	irqchip->irq_unmask = armada_37xx_irq_unmask;
+	irqchip->irq_set_wake = armada_37xx_irq_set_wake;
+	irqchip->irq_set_type = armada_37xx_irq_set_type;
+	irqchip->name = info->data->name;
+
+	ret = gpiochip_irqchip_add(gc, irqchip, 0,
+				   handle_edge_irq, IRQ_TYPE_NONE);
+	if (ret) {
+		dev_info(&pdev->dev, "could not add irqchip\n");
+		return ret;
+	}
+
+	/*
+	 * Many interrupts are connected to the parent interrupt
+	 * controller. But we do not take advantage of this and use
+	 * the chained irq with all of them.
+	 */
+	for (i = 0; i < nrirqs; i++) {
+		struct irq_data *d = irq_get_irq_data(gc->irq_base + i);
+
+		/*
+		 * The mask field is a "precomputed bitmask for
+		 * accessing the chip registers" which was introduced
+		 * for the generic irqchip framework. As we don't use
+		 * this framework, we can reuse this field for our own
+		 * usage.
+		 */
+		d->mask = BIT(i % GPIO_PER_REG);
+	}
+
+	for (i = 0; i < nr_irq_parent; i++) {
+		int irq = irq_of_parse_and_map(np, i);
+
+		if (irq < 0)
+			continue;
+
+		gpiochip_set_chained_irqchip(gc, irqchip, irq,
+					     armada_37xx_irq_handler);
+	}
+
+	return 0;
+}
+
 static int armada_37xx_gpiochip_register(struct platform_device *pdev,
 					struct armada_37xx_pinctrl *info)
 {
@@ -496,6 +723,9 @@ static int armada_37xx_gpiochip_register(struct platform_device *pdev,
 	ret = devm_gpiochip_add_data(&pdev->dev, gc, info);
 	if (ret)
 		return ret;
+	ret = armada_37xx_irqchip_register(pdev, info);
+	if (ret)
+		return ret;
 
 	return 0;
 }
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-ap806.c b/drivers/pinctrl/mvebu/pinctrl-armada-ap806.c
new file mode 100644
index 000000000000..66e442260a4e
--- /dev/null
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-ap806.c
@@ -0,0 +1,140 @@
+/*
+ * Marvell Armada ap806 pinctrl driver based on mvebu pinctrl core
+ *
+ * Copyright (C) 2017 Marvell
+ *
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ * Hanna Hawa <hannah@marvell.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/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-mvebu.h"
+
+static struct mvebu_mpp_mode armada_ap806_mpp_modes[] = {
+	MPP_MODE(0,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(1, "sdio",    "clk"),
+		 MPP_FUNCTION(3, "spi0",    "clk")),
+	MPP_MODE(1,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(1, "sdio",    "cmd"),
+		 MPP_FUNCTION(3, "spi0",    "miso")),
+	MPP_MODE(2,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(1, "sdio",    "d0"),
+		 MPP_FUNCTION(3, "spi0",    "mosi")),
+	MPP_MODE(3,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(1, "sdio",    "d1"),
+		 MPP_FUNCTION(3, "spi0",    "cs0n")),
+	MPP_MODE(4,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(1, "sdio",    "d2"),
+		 MPP_FUNCTION(3, "i2c0",    "sda")),
+	MPP_MODE(5,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(1, "sdio",    "d3"),
+		 MPP_FUNCTION(3, "i2c0",    "sdk")),
+	MPP_MODE(6,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(1, "sdio",    "ds")),
+	MPP_MODE(7,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(1, "sdio",    "d4"),
+		 MPP_FUNCTION(3, "uart1",   "rxd")),
+	MPP_MODE(8,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(1, "sdio",    "d5"),
+		 MPP_FUNCTION(3, "uart1",   "txd")),
+	MPP_MODE(9,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(1, "sdio",    "d6"),
+		 MPP_FUNCTION(3, "spi0",    "cs1n")),
+	MPP_MODE(10,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(1, "sdio",    "d7")),
+	MPP_MODE(11,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(3, "uart0",   "txd")),
+	MPP_MODE(12,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(1, "sdio",    "pw_off"),
+		 MPP_FUNCTION(2, "sdio",    "hw_rst")),
+	MPP_MODE(13,
+		 MPP_FUNCTION(0, "gpio",    NULL)),
+	MPP_MODE(14,
+		 MPP_FUNCTION(0, "gpio",    NULL)),
+	MPP_MODE(15,
+		 MPP_FUNCTION(0, "gpio",    NULL)),
+	MPP_MODE(16,
+		 MPP_FUNCTION(0, "gpio",    NULL)),
+	MPP_MODE(17,
+		 MPP_FUNCTION(0, "gpio",    NULL)),
+	MPP_MODE(18,
+		 MPP_FUNCTION(0, "gpio",    NULL)),
+	MPP_MODE(19,
+		 MPP_FUNCTION(0, "gpio",    NULL),
+		 MPP_FUNCTION(3, "uart0",   "rxd"),
+		 MPP_FUNCTION(4, "sdio",    "pw_off")),
+};
+
+static struct mvebu_pinctrl_soc_info armada_ap806_pinctrl_info;
+
+static const struct of_device_id armada_ap806_pinctrl_of_match[] = {
+	{
+		.compatible = "marvell,ap806-pinctrl",
+	},
+	{ },
+};
+
+static const struct mvebu_mpp_ctrl armada_ap806_mpp_controls[] = {
+	MPP_FUNC_CTRL(0, 19, NULL, mvebu_regmap_mpp_ctrl),
+};
+
+static struct pinctrl_gpio_range armada_ap806_mpp_gpio_ranges[] = {
+	MPP_GPIO_RANGE(0,   0,  0, 20),
+};
+
+static int armada_ap806_pinctrl_probe(struct platform_device *pdev)
+{
+	struct mvebu_pinctrl_soc_info *soc = &armada_ap806_pinctrl_info;
+	const struct of_device_id *match =
+		of_match_device(armada_ap806_pinctrl_of_match, &pdev->dev);
+
+	if (!match || !pdev->dev.parent)
+		return -ENODEV;
+
+	soc->variant = 0; /* no variants for Armada AP806 */
+	soc->controls = armada_ap806_mpp_controls;
+	soc->ncontrols = ARRAY_SIZE(armada_ap806_mpp_controls);
+	soc->gpioranges = armada_ap806_mpp_gpio_ranges;
+	soc->ngpioranges = ARRAY_SIZE(armada_ap806_mpp_gpio_ranges);
+	soc->modes = armada_ap806_mpp_modes;
+	soc->nmodes = armada_ap806_mpp_controls[0].npins;
+
+	pdev->dev.platform_data = soc;
+
+	return mvebu_pinctrl_simple_regmap_probe(pdev, pdev->dev.parent, 0);
+}
+
+static struct platform_driver armada_ap806_pinctrl_driver = {
+	.driver = {
+		.name = "armada-ap806-pinctrl",
+		.of_match_table = of_match_ptr(armada_ap806_pinctrl_of_match),
+	},
+	.probe = armada_ap806_pinctrl_probe,
+};
+
+builtin_platform_driver(armada_ap806_pinctrl_driver);
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-cp110.c b/drivers/pinctrl/mvebu/pinctrl-armada-cp110.c
new file mode 100644
index 000000000000..7f85beb45482
--- /dev/null
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-cp110.c
@@ -0,0 +1,687 @@
+/*
+ * Marvell Armada CP110 pinctrl driver based on mvebu pinctrl core
+ *
+ * Copyright (C) 2017 Marvell
+ *
+ * Hanna Hawa <hannah@marvell.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/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/platform_device.h>
+
+#include "pinctrl-mvebu.h"
+
+/*
+ * Even if the pin controller is the same the MMP available depend on the SoC
+ * integration.
+ *  - In Armada7K (single CP) almost all the MPPs are available (except the
+ *    MMP 39 to 43)
+ *  - In Armada8K (dual CP) the MPPs are split into 2 parts, MPPs 0-31 from
+ *    CPS, and MPPs 32-62 from CPM, the below flags (V_ARMADA_8K_CPM,
+ *    V_ARMADA_8K_CPS) set which MPP is available to the CPx.
+ * The x_PLUS enum mean that the MPP available for CPx and for Armada70x0
+ */
+enum {
+	V_ARMADA_7K = BIT(0),
+	V_ARMADA_8K_CPM = BIT(1),
+	V_ARMADA_8K_CPS = BIT(2),
+	V_ARMADA_7K_8K_CPM = (V_ARMADA_7K | V_ARMADA_8K_CPM),
+	V_ARMADA_7K_8K_CPS = (V_ARMADA_7K | V_ARMADA_8K_CPS),
+};
+
+static struct mvebu_mpp_mode armada_cp110_mpp_modes[] = {
+	MPP_MODE(0,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ale1"),
+		 MPP_FUNCTION(2,	"au",		"i2smclk"),
+		 MPP_FUNCTION(3,	"ge0",		"rxd3"),
+		 MPP_FUNCTION(4,	"tdm",		"pclk"),
+		 MPP_FUNCTION(6,	"ptp",		"pulse"),
+		 MPP_FUNCTION(7,	"mss_i2c",	"sda"),
+		 MPP_FUNCTION(8,	"uart0",	"rxd"),
+		 MPP_FUNCTION(9,	"sata0",	"present_act"),
+		 MPP_FUNCTION(10,	"ge",		"mdio")),
+	MPP_MODE(1,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ale0"),
+		 MPP_FUNCTION(2,	"au",		"i2sdo_spdifo"),
+		 MPP_FUNCTION(3,	"ge0",		"rxd2"),
+		 MPP_FUNCTION(4,	"tdm",		"drx"),
+		 MPP_FUNCTION(6,	"ptp",		"clk"),
+		 MPP_FUNCTION(7,	"mss_i2c",	"sck"),
+		 MPP_FUNCTION(8,	"uart0",	"txd"),
+		 MPP_FUNCTION(9,	"sata1",	"present_act"),
+		 MPP_FUNCTION(10,	"ge",		"mdc")),
+	MPP_MODE(2,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad15"),
+		 MPP_FUNCTION(2,	"au",		"i2sextclk"),
+		 MPP_FUNCTION(3,	"ge0",		"rxd1"),
+		 MPP_FUNCTION(4,	"tdm",		"dtx"),
+		 MPP_FUNCTION(5,	"mss_uart",	"rxd"),
+		 MPP_FUNCTION(6,	"ptp",		"pclk_out"),
+		 MPP_FUNCTION(7,	"i2c1",		"sck"),
+		 MPP_FUNCTION(8,	"uart1",	"rxd"),
+		 MPP_FUNCTION(9,	"sata0",	"present_act"),
+		 MPP_FUNCTION(10,	"xg",		"mdc")),
+	MPP_MODE(3,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad14"),
+		 MPP_FUNCTION(2,	"au",		"i2slrclk"),
+		 MPP_FUNCTION(3,	"ge0",		"rxd0"),
+		 MPP_FUNCTION(4,	"tdm",		"fsync"),
+		 MPP_FUNCTION(5,	"mss_uart",	"txd"),
+		 MPP_FUNCTION(6,	"pcie",		"rstoutn"),
+		 MPP_FUNCTION(7,	"i2c1",		"sda"),
+		 MPP_FUNCTION(8,	"uart1",	"txd"),
+		 MPP_FUNCTION(9,	"sata1",	"present_act"),
+		 MPP_FUNCTION(10,	"xg",		"mdio")),
+	MPP_MODE(4,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad13"),
+		 MPP_FUNCTION(2,	"au",		"i2sbclk"),
+		 MPP_FUNCTION(3,	"ge0",		"rxctl"),
+		 MPP_FUNCTION(4,	"tdm",		"rstn"),
+		 MPP_FUNCTION(5,	"mss_uart",	"rxd"),
+		 MPP_FUNCTION(6,	"uart1",	"cts"),
+		 MPP_FUNCTION(7,	"pcie0",	"clkreq"),
+		 MPP_FUNCTION(8,	"uart3",	"rxd"),
+		 MPP_FUNCTION(10,	"ge",		"mdc")),
+	MPP_MODE(5,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad12"),
+		 MPP_FUNCTION(2,	"au",		"i2sdi"),
+		 MPP_FUNCTION(3,	"ge0",		"rxclk"),
+		 MPP_FUNCTION(4,	"tdm",		"intn"),
+		 MPP_FUNCTION(5,	"mss_uart",	"txd"),
+		 MPP_FUNCTION(6,	"uart1",	"rts"),
+		 MPP_FUNCTION(7,	"pcie1",	"clkreq"),
+		 MPP_FUNCTION(8,	"uart3",	"txd"),
+		 MPP_FUNCTION(10,	"ge",		"mdio")),
+	MPP_MODE(6,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad11"),
+		 MPP_FUNCTION(3,	"ge0",		"txd3"),
+		 MPP_FUNCTION(4,	"spi0",		"csn2"),
+		 MPP_FUNCTION(5,	"au",		"i2sextclk"),
+		 MPP_FUNCTION(6,	"sata1",	"present_act"),
+		 MPP_FUNCTION(7,	"pcie2",	"clkreq"),
+		 MPP_FUNCTION(8,	"uart0",	"rxd"),
+		 MPP_FUNCTION(9,	"ptp",		"pulse")),
+	MPP_MODE(7,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad10"),
+		 MPP_FUNCTION(3,	"ge0",		"txd2"),
+		 MPP_FUNCTION(4,	"spi0",		"csn1"),
+		 MPP_FUNCTION(5,	"spi1",		"csn1"),
+		 MPP_FUNCTION(6,	"sata0",	"present_act"),
+		 MPP_FUNCTION(7,	"led",		"data"),
+		 MPP_FUNCTION(8,	"uart0",	"txd"),
+		 MPP_FUNCTION(9,	"ptp",		"clk")),
+	MPP_MODE(8,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad9"),
+		 MPP_FUNCTION(3,	"ge0",		"txd1"),
+		 MPP_FUNCTION(4,	"spi0",		"csn0"),
+		 MPP_FUNCTION(5,	"spi1",		"csn0"),
+		 MPP_FUNCTION(6,	"uart0",	"cts"),
+		 MPP_FUNCTION(7,	"led",		"stb"),
+		 MPP_FUNCTION(8,	"uart2",	"rxd"),
+		 MPP_FUNCTION(9,	"ptp",		"pclk_out"),
+		 MPP_FUNCTION(10,	"synce1",	"clk")),
+	MPP_MODE(9,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad8"),
+		 MPP_FUNCTION(3,	"ge0",		"txd0"),
+		 MPP_FUNCTION(4,	"spi0",		"mosi"),
+		 MPP_FUNCTION(5,	"spi1",		"mosi"),
+		 MPP_FUNCTION(7,	"pcie",		"rstoutn"),
+		 MPP_FUNCTION(10,	"synce2",	"clk")),
+	MPP_MODE(10,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"readyn"),
+		 MPP_FUNCTION(3,	"ge0",		"txctl"),
+		 MPP_FUNCTION(4,	"spi0",		"miso"),
+		 MPP_FUNCTION(5,	"spi1",		"miso"),
+		 MPP_FUNCTION(6,	"uart0",	"cts"),
+		 MPP_FUNCTION(7,	"sata1",	"present_act")),
+	MPP_MODE(11,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"wen1"),
+		 MPP_FUNCTION(3,	"ge0",		"txclkout"),
+		 MPP_FUNCTION(4,	"spi0",		"clk"),
+		 MPP_FUNCTION(5,	"spi1",		"clk"),
+		 MPP_FUNCTION(6,	"uart0",	"rts"),
+		 MPP_FUNCTION(7,	"led",		"clk"),
+		 MPP_FUNCTION(8,	"uart2",	"txd"),
+		 MPP_FUNCTION(9,	"sata0",	"present_act")),
+	MPP_MODE(12,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"clk_out"),
+		 MPP_FUNCTION(2,	"nf",		"rbn1"),
+		 MPP_FUNCTION(3,	"spi1",		"csn1"),
+		 MPP_FUNCTION(4,	"ge0",		"rxclk")),
+	MPP_MODE(13,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"burstn"),
+		 MPP_FUNCTION(2,	"nf",		"rbn0"),
+		 MPP_FUNCTION(3,	"spi1",		"miso"),
+		 MPP_FUNCTION(4,	"ge0",		"rxctl"),
+		 MPP_FUNCTION(8,	"mss_spi",	"miso")),
+	MPP_MODE(14,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"bootcsn"),
+		 MPP_FUNCTION(2,	"dev",		"csn0"),
+		 MPP_FUNCTION(3,	"spi1",		"csn0"),
+		 MPP_FUNCTION(4,	"spi0",		"csn3"),
+		 MPP_FUNCTION(5,	"au",		"i2sextclk"),
+		 MPP_FUNCTION(6,	"spi0",		"miso"),
+		 MPP_FUNCTION(7,	"sata0",	"present_act"),
+		 MPP_FUNCTION(8,	"mss_spi",	"csn")),
+	MPP_MODE(15,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad7"),
+		 MPP_FUNCTION(3,	"spi1",		"mosi"),
+		 MPP_FUNCTION(6,	"spi0",		"mosi"),
+		 MPP_FUNCTION(8,	"mss_spi",	"mosi"),
+		 MPP_FUNCTION(11,	"ptp",		"pulse_cp2cp")),
+	MPP_MODE(16,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad6"),
+		 MPP_FUNCTION(3,	"spi1",		"clk"),
+		 MPP_FUNCTION(8,	"mss_spi",	"clk")),
+	MPP_MODE(17,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad5"),
+		 MPP_FUNCTION(4,	"ge0",		"txd3")),
+	MPP_MODE(18,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad4"),
+		 MPP_FUNCTION(4,	"ge0",		"txd2"),
+		 MPP_FUNCTION(11,	"ptp",		"clk_cp2cp")),
+	MPP_MODE(19,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad3"),
+		 MPP_FUNCTION(4,	"ge0",		"txd1"),
+		 MPP_FUNCTION(11,	"wakeup",	"out_cp2cp")),
+	MPP_MODE(20,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad2"),
+		 MPP_FUNCTION(4,	"ge0",		"txd0")),
+	MPP_MODE(21,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad1"),
+		 MPP_FUNCTION(4,	"ge0",		"txctl"),
+		 MPP_FUNCTION(11,	"sei",		"in_cp2cp")),
+	MPP_MODE(22,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"ad0"),
+		 MPP_FUNCTION(4,	"ge0",		"txclkout"),
+		 MPP_FUNCTION(11,	"wakeup",	"in_cp2cp")),
+	MPP_MODE(23,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"a1"),
+		 MPP_FUNCTION(5,	"au",		"i2smclk"),
+		 MPP_FUNCTION(11,	"link",		"rd_in_cp2cp")),
+	MPP_MODE(24,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"a0"),
+		 MPP_FUNCTION(5,	"au",		"i2slrclk")),
+	MPP_MODE(25,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"oen"),
+		 MPP_FUNCTION(5,	"au",		"i2sdo_spdifo")),
+	MPP_MODE(26,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"wen0"),
+		 MPP_FUNCTION(5,	"au",		"i2sbclk")),
+	MPP_MODE(27,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"csn0"),
+		 MPP_FUNCTION(2,	"spi1",		"miso"),
+		 MPP_FUNCTION(3,	"mss_gpio4",	NULL),
+		 MPP_FUNCTION(4,	"ge0",		"rxd3"),
+		 MPP_FUNCTION(5,	"spi0",		"csn4"),
+		 MPP_FUNCTION(8,	"ge",		"mdio"),
+		 MPP_FUNCTION(9,	"sata0",	"present_act"),
+		 MPP_FUNCTION(10,	"uart0",	"rts"),
+		 MPP_FUNCTION(11,	"rei",		"in_cp2cp")),
+	MPP_MODE(28,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"csn1"),
+		 MPP_FUNCTION(2,	"spi1",		"csn0"),
+		 MPP_FUNCTION(3,	"mss_gpio5",	NULL),
+		 MPP_FUNCTION(4,	"ge0",		"rxd2"),
+		 MPP_FUNCTION(5,	"spi0",		"csn5"),
+		 MPP_FUNCTION(6,	"pcie2",	"clkreq"),
+		 MPP_FUNCTION(7,	"ptp",		"pulse"),
+		 MPP_FUNCTION(8,	"ge",		"mdc"),
+		 MPP_FUNCTION(9,	"sata1",	"present_act"),
+		 MPP_FUNCTION(10,	"uart0",	"cts"),
+		 MPP_FUNCTION(11,	"led",		"data")),
+	MPP_MODE(29,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"csn2"),
+		 MPP_FUNCTION(2,	"spi1",		"mosi"),
+		 MPP_FUNCTION(3,	"mss_gpio6",	NULL),
+		 MPP_FUNCTION(4,	"ge0",		"rxd1"),
+		 MPP_FUNCTION(5,	"spi0",		"csn6"),
+		 MPP_FUNCTION(6,	"pcie1",	"clkreq"),
+		 MPP_FUNCTION(7,	"ptp",		"clk"),
+		 MPP_FUNCTION(8,	"mss_i2c",	"sda"),
+		 MPP_FUNCTION(9,	"sata0",	"present_act"),
+		 MPP_FUNCTION(10,	"uart0",	"rxd"),
+		 MPP_FUNCTION(11,	"led",		"stb")),
+	MPP_MODE(30,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"csn3"),
+		 MPP_FUNCTION(2,	"spi1",		"clk"),
+		 MPP_FUNCTION(3,	"mss_gpio7",	NULL),
+		 MPP_FUNCTION(4,	"ge0",		"rxd0"),
+		 MPP_FUNCTION(5,	"spi0",		"csn7"),
+		 MPP_FUNCTION(6,	"pcie0",	"clkreq"),
+		 MPP_FUNCTION(7,	"ptp",		"pclk_out"),
+		 MPP_FUNCTION(8,	"mss_i2c",	"sck"),
+		 MPP_FUNCTION(9,	"sata1",	"present_act"),
+		 MPP_FUNCTION(10,	"uart0",	"txd"),
+		 MPP_FUNCTION(11,	"led",		"clk")),
+	MPP_MODE(31,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"dev",		"a2"),
+		 MPP_FUNCTION(3,	"mss_gpio4",	NULL),
+		 MPP_FUNCTION(6,	"pcie",		"rstoutn"),
+		 MPP_FUNCTION(8,	"ge",		"mdc")),
+	MPP_MODE(32,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"mii",		"col"),
+		 MPP_FUNCTION(2,	"mii",		"txerr"),
+		 MPP_FUNCTION(3,	"mss_spi",	"miso"),
+		 MPP_FUNCTION(4,	"tdm",		"drx"),
+		 MPP_FUNCTION(5,	"au",		"i2sextclk"),
+		 MPP_FUNCTION(6,	"au",		"i2sdi"),
+		 MPP_FUNCTION(7,	"ge",		"mdio"),
+		 MPP_FUNCTION(8,	"sdio",		"v18_en"),
+		 MPP_FUNCTION(9,	"pcie1",	"clkreq"),
+		 MPP_FUNCTION(10,	"mss_gpio0",	NULL)),
+	MPP_MODE(33,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"mii",		"txclk"),
+		 MPP_FUNCTION(2,	"sdio",		"pwr10"),
+		 MPP_FUNCTION(3,	"mss_spi",	"csn"),
+		 MPP_FUNCTION(4,	"tdm",		"fsync"),
+		 MPP_FUNCTION(5,	"au",		"i2smclk"),
+		 MPP_FUNCTION(6,	"sdio",		"bus_pwr"),
+		 MPP_FUNCTION(8,	"xg",		"mdio"),
+		 MPP_FUNCTION(9,	"pcie2",	"clkreq"),
+		 MPP_FUNCTION(10,	"mss_gpio1",	NULL)),
+	MPP_MODE(34,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"mii",		"rxerr"),
+		 MPP_FUNCTION(2,	"sdio",		"pwr11"),
+		 MPP_FUNCTION(3,	"mss_spi",	"mosi"),
+		 MPP_FUNCTION(4,	"tdm",		"dtx"),
+		 MPP_FUNCTION(5,	"au",		"i2slrclk"),
+		 MPP_FUNCTION(6,	"sdio",		"wr_protect"),
+		 MPP_FUNCTION(7,	"ge",		"mdc"),
+		 MPP_FUNCTION(9,	"pcie0",	"clkreq"),
+		 MPP_FUNCTION(10,	"mss_gpio2",	NULL)),
+	MPP_MODE(35,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"sata1",	"present_act"),
+		 MPP_FUNCTION(2,	"i2c1",		"sda"),
+		 MPP_FUNCTION(3,	"mss_spi",	"clk"),
+		 MPP_FUNCTION(4,	"tdm",		"pclk"),
+		 MPP_FUNCTION(5,	"au",		"i2sdo_spdifo"),
+		 MPP_FUNCTION(6,	"sdio",		"card_detect"),
+		 MPP_FUNCTION(7,	"xg",		"mdio"),
+		 MPP_FUNCTION(8,	"ge",		"mdio"),
+		 MPP_FUNCTION(9,	"pcie",		"rstoutn"),
+		 MPP_FUNCTION(10,	"mss_gpio3",	NULL)),
+	MPP_MODE(36,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"synce2",	"clk"),
+		 MPP_FUNCTION(2,	"i2c1",		"sck"),
+		 MPP_FUNCTION(3,	"ptp",		"clk"),
+		 MPP_FUNCTION(4,	"synce1",	"clk"),
+		 MPP_FUNCTION(5,	"au",		"i2sbclk"),
+		 MPP_FUNCTION(6,	"sata0",	"present_act"),
+		 MPP_FUNCTION(7,	"xg",		"mdc"),
+		 MPP_FUNCTION(8,	"ge",		"mdc"),
+		 MPP_FUNCTION(9,	"pcie2",	"clkreq"),
+		 MPP_FUNCTION(10,	"mss_gpio5",	NULL)),
+	MPP_MODE(37,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"uart2",	"rxd"),
+		 MPP_FUNCTION(2,	"i2c0",		"sck"),
+		 MPP_FUNCTION(3,	"ptp",		"pclk_out"),
+		 MPP_FUNCTION(4,	"tdm",		"intn"),
+		 MPP_FUNCTION(5,	"mss_i2c",	"sck"),
+		 MPP_FUNCTION(6,	"sata1",	"present_act"),
+		 MPP_FUNCTION(7,	"ge",		"mdc"),
+		 MPP_FUNCTION(8,	"xg",		"mdc"),
+		 MPP_FUNCTION(9,	"pcie1",	"clkreq"),
+		 MPP_FUNCTION(10,	"mss_gpio6",	NULL),
+		 MPP_FUNCTION(11,	"link",		"rd_out_cp2cp")),
+	MPP_MODE(38,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"uart2",	"txd"),
+		 MPP_FUNCTION(2,	"i2c0",		"sda"),
+		 MPP_FUNCTION(3,	"ptp",		"pulse"),
+		 MPP_FUNCTION(4,	"tdm",		"rstn"),
+		 MPP_FUNCTION(5,	"mss_i2c",	"sda"),
+		 MPP_FUNCTION(6,	"sata0",	"present_act"),
+		 MPP_FUNCTION(7,	"ge",		"mdio"),
+		 MPP_FUNCTION(8,	"xg",		"mdio"),
+		 MPP_FUNCTION(9,	"au",		"i2sextclk"),
+		 MPP_FUNCTION(10,	"mss_gpio7",	NULL),
+		 MPP_FUNCTION(11,	"ptp",		"pulse_cp2cp")),
+	MPP_MODE(39,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"sdio",		"wr_protect"),
+		 MPP_FUNCTION(4,	"au",		"i2sbclk"),
+		 MPP_FUNCTION(5,	"ptp",		"clk"),
+		 MPP_FUNCTION(6,	"spi0",		"csn1"),
+		 MPP_FUNCTION(9,	"sata1",	"present_act"),
+		 MPP_FUNCTION(10,	"mss_gpio0",	NULL)),
+	MPP_MODE(40,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"sdio",		"pwr11"),
+		 MPP_FUNCTION(2,	"synce1",	"clk"),
+		 MPP_FUNCTION(3,	"mss_i2c",	"sda"),
+		 MPP_FUNCTION(4,	"au",		"i2sdo_spdifo"),
+		 MPP_FUNCTION(5,	"ptp",		"pclk_out"),
+		 MPP_FUNCTION(6,	"spi0",		"clk"),
+		 MPP_FUNCTION(7,	"uart1",	"txd"),
+		 MPP_FUNCTION(8,	"ge",		"mdio"),
+		 MPP_FUNCTION(9,	"sata0",	"present_act"),
+		 MPP_FUNCTION(10,	"mss_gpio1",	NULL)),
+	MPP_MODE(41,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"sdio",		"pwr10"),
+		 MPP_FUNCTION(2,	"sdio",		"bus_pwr"),
+		 MPP_FUNCTION(3,	"mss_i2c",	"sck"),
+		 MPP_FUNCTION(4,	"au",		"i2slrclk"),
+		 MPP_FUNCTION(5,	"ptp",		"pulse"),
+		 MPP_FUNCTION(6,	"spi0",		"mosi"),
+		 MPP_FUNCTION(7,	"uart1",	"rxd"),
+		 MPP_FUNCTION(8,	"ge",		"mdc"),
+		 MPP_FUNCTION(9,	"sata1",	"present_act"),
+		 MPP_FUNCTION(10,	"mss_gpio2",	NULL),
+		 MPP_FUNCTION(11,	"rei",		"out_cp2cp")),
+	MPP_MODE(42,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"sdio",		"v18_en"),
+		 MPP_FUNCTION(2,	"sdio",		"wr_protect"),
+		 MPP_FUNCTION(3,	"synce2",	"clk"),
+		 MPP_FUNCTION(4,	"au",		"i2smclk"),
+		 MPP_FUNCTION(5,	"mss_uart",	"txd"),
+		 MPP_FUNCTION(6,	"spi0",		"miso"),
+		 MPP_FUNCTION(7,	"uart1",	"cts"),
+		 MPP_FUNCTION(8,	"xg",		"mdc"),
+		 MPP_FUNCTION(9,	"sata0",	"present_act"),
+		 MPP_FUNCTION(10,	"mss_gpio4",	NULL)),
+	MPP_MODE(43,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"sdio",		"card_detect"),
+		 MPP_FUNCTION(3,	"synce1",	"clk"),
+		 MPP_FUNCTION(4,	"au",		"i2sextclk"),
+		 MPP_FUNCTION(5,	"mss_uart",	"rxd"),
+		 MPP_FUNCTION(6,	"spi0",		"csn0"),
+		 MPP_FUNCTION(7,	"uart1",	"rts"),
+		 MPP_FUNCTION(8,	"xg",		"mdio"),
+		 MPP_FUNCTION(9,	"sata1",	"present_act"),
+		 MPP_FUNCTION(10,	"mss_gpio5",	NULL),
+		 MPP_FUNCTION(11,	"wakeup",	"out_cp2cp")),
+	MPP_MODE(44,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"ge1",		"txd2"),
+		 MPP_FUNCTION(7,	"uart0",	"rts"),
+		 MPP_FUNCTION(11,	"ptp",		"clk_cp2cp")),
+	MPP_MODE(45,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"ge1",		"txd3"),
+		 MPP_FUNCTION(7,	"uart0",	"txd"),
+		 MPP_FUNCTION(9,	"pcie",		"rstoutn")),
+	MPP_MODE(46,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"ge1",		"txd1"),
+		 MPP_FUNCTION(7,	"uart1",	"rts")),
+	MPP_MODE(47,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"ge1",		"txd0"),
+		 MPP_FUNCTION(5,	"spi1",		"clk"),
+		 MPP_FUNCTION(7,	"uart1",	"txd"),
+		 MPP_FUNCTION(8,	"ge",		"mdc")),
+	MPP_MODE(48,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"ge1",		"txctl_txen"),
+		 MPP_FUNCTION(5,	"spi1",		"mosi"),
+		 MPP_FUNCTION(8,	"xg",		"mdc"),
+		 MPP_FUNCTION(11,	"wakeup",	"in_cp2cp")),
+	MPP_MODE(49,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"ge1",		"txclkout"),
+		 MPP_FUNCTION(2,	"mii",		"crs"),
+		 MPP_FUNCTION(5,	"spi1",		"miso"),
+		 MPP_FUNCTION(7,	"uart1",	"rxd"),
+		 MPP_FUNCTION(8,	"ge",		"mdio"),
+		 MPP_FUNCTION(9,	"pcie0",	"clkreq"),
+		 MPP_FUNCTION(10,	"sdio",		"v18_en"),
+		 MPP_FUNCTION(11,	"sei",		"out_cp2cp")),
+	MPP_MODE(50,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"ge1",		"rxclk"),
+		 MPP_FUNCTION(2,	"mss_i2c",	"sda"),
+		 MPP_FUNCTION(5,	"spi1",		"csn0"),
+		 MPP_FUNCTION(6,	"uart2",	"txd"),
+		 MPP_FUNCTION(7,	"uart0",	"rxd"),
+		 MPP_FUNCTION(8,	"xg",		"mdio"),
+		 MPP_FUNCTION(10,	"sdio",		"pwr11")),
+	MPP_MODE(51,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"ge1",		"rxd0"),
+		 MPP_FUNCTION(2,	"mss_i2c",	"sck"),
+		 MPP_FUNCTION(5,	"spi1",		"csn1"),
+		 MPP_FUNCTION(6,	"uart2",	"rxd"),
+		 MPP_FUNCTION(7,	"uart0",	"cts"),
+		 MPP_FUNCTION(10,	"sdio",		"pwr10")),
+	MPP_MODE(52,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"ge1",		"rxd1"),
+		 MPP_FUNCTION(2,	"synce1",	"clk"),
+		 MPP_FUNCTION(4,	"synce2",	"clk"),
+		 MPP_FUNCTION(5,	"spi1",		"csn2"),
+		 MPP_FUNCTION(7,	"uart1",	"cts"),
+		 MPP_FUNCTION(8,	"led",		"clk"),
+		 MPP_FUNCTION(9,	"pcie",		"rstoutn"),
+		 MPP_FUNCTION(10,	"pcie0",	"clkreq")),
+	MPP_MODE(53,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"ge1",		"rxd2"),
+		 MPP_FUNCTION(3,	"ptp",		"clk"),
+		 MPP_FUNCTION(5,	"spi1",		"csn3"),
+		 MPP_FUNCTION(7,	"uart1",	"rxd"),
+		 MPP_FUNCTION(8,	"led",		"stb"),
+		 MPP_FUNCTION(11,	"sdio",		"led")),
+	MPP_MODE(54,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"ge1",		"rxd3"),
+		 MPP_FUNCTION(2,	"synce2",	"clk"),
+		 MPP_FUNCTION(3,	"ptp",		"pclk_out"),
+		 MPP_FUNCTION(4,	"synce1",	"clk"),
+		 MPP_FUNCTION(8,	"led",		"data"),
+		 MPP_FUNCTION(10,	"sdio",		"hw_rst"),
+		 MPP_FUNCTION(11,	"sdio",		"wr_protect")),
+	MPP_MODE(55,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"ge1",		"rxctl_rxdv"),
+		 MPP_FUNCTION(3,	"ptp",		"pulse"),
+		 MPP_FUNCTION(10,	"sdio",		"led"),
+		 MPP_FUNCTION(11,	"sdio",		"card_detect")),
+	MPP_MODE(56,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(4,	"tdm",		"drx"),
+		 MPP_FUNCTION(5,	"au",		"i2sdo_spdifo"),
+		 MPP_FUNCTION(6,	"spi0",		"clk"),
+		 MPP_FUNCTION(7,	"uart1",	"rxd"),
+		 MPP_FUNCTION(9,	"sata1",	"present_act"),
+		 MPP_FUNCTION(14,	"sdio",		"clk")),
+	MPP_MODE(57,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(2,	"mss_i2c",	"sda"),
+		 MPP_FUNCTION(3,	"ptp",		"pclk_out"),
+		 MPP_FUNCTION(4,	"tdm",		"intn"),
+		 MPP_FUNCTION(5,	"au",		"i2sbclk"),
+		 MPP_FUNCTION(6,	"spi0",		"mosi"),
+		 MPP_FUNCTION(7,	"uart1",	"txd"),
+		 MPP_FUNCTION(9,	"sata0",	"present_act"),
+		 MPP_FUNCTION(14,	"sdio",		"cmd")),
+	MPP_MODE(58,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(2,	"mss_i2c",	"sck"),
+		 MPP_FUNCTION(3,	"ptp",		"clk"),
+		 MPP_FUNCTION(4,	"tdm",		"rstn"),
+		 MPP_FUNCTION(5,	"au",		"i2sdi"),
+		 MPP_FUNCTION(6,	"spi0",		"miso"),
+		 MPP_FUNCTION(7,	"uart1",	"cts"),
+		 MPP_FUNCTION(8,	"led",		"clk"),
+		 MPP_FUNCTION(14,	"sdio",		"d0")),
+	MPP_MODE(59,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"mss_gpio7",	NULL),
+		 MPP_FUNCTION(2,	"synce2",	"clk"),
+		 MPP_FUNCTION(4,	"tdm",		"fsync"),
+		 MPP_FUNCTION(5,	"au",		"i2slrclk"),
+		 MPP_FUNCTION(6,	"spi0",		"csn0"),
+		 MPP_FUNCTION(7,	"uart0",	"cts"),
+		 MPP_FUNCTION(8,	"led",		"stb"),
+		 MPP_FUNCTION(9,	"uart1",	"txd"),
+		 MPP_FUNCTION(14,	"sdio",		"d1")),
+	MPP_MODE(60,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"mss_gpio6",	NULL),
+		 MPP_FUNCTION(3,	"ptp",		"pulse"),
+		 MPP_FUNCTION(4,	"tdm",		"dtx"),
+		 MPP_FUNCTION(5,	"au",		"i2smclk"),
+		 MPP_FUNCTION(6,	"spi0",		"csn1"),
+		 MPP_FUNCTION(7,	"uart0",	"rts"),
+		 MPP_FUNCTION(8,	"led",		"data"),
+		 MPP_FUNCTION(9,	"uart1",	"rxd"),
+		 MPP_FUNCTION(14,	"sdio",		"d2")),
+	MPP_MODE(61,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"mss_gpio5",	NULL),
+		 MPP_FUNCTION(3,	"ptp",		"clk"),
+		 MPP_FUNCTION(4,	"tdm",		"pclk"),
+		 MPP_FUNCTION(5,	"au",		"i2sextclk"),
+		 MPP_FUNCTION(6,	"spi0",		"csn2"),
+		 MPP_FUNCTION(7,	"uart0",	"txd"),
+		 MPP_FUNCTION(8,	"uart2",	"txd"),
+		 MPP_FUNCTION(9,	"sata1",	"present_act"),
+		 MPP_FUNCTION(10,	"ge",		"mdio"),
+		 MPP_FUNCTION(14,	"sdio",		"d3")),
+	MPP_MODE(62,
+		 MPP_FUNCTION(0,	"gpio",		NULL),
+		 MPP_FUNCTION(1,	"mss_gpio4",	NULL),
+		 MPP_FUNCTION(2,	"synce1",	"clk"),
+		 MPP_FUNCTION(3,	"ptp",		"pclk_out"),
+		 MPP_FUNCTION(5,	"sata1",	"present_act"),
+		 MPP_FUNCTION(6,	"spi0",		"csn3"),
+		 MPP_FUNCTION(7,	"uart0",	"rxd"),
+		 MPP_FUNCTION(8,	"uart2",	"rxd"),
+		 MPP_FUNCTION(9,	"sata0",	"present_act"),
+		 MPP_FUNCTION(10,	"ge",		"mdc")),
+};
+
+static const struct of_device_id armada_cp110_pinctrl_of_match[] = {
+	{
+		.compatible	= "marvell,armada-7k-pinctrl",
+		.data		= (void *) V_ARMADA_7K,
+	},
+	{
+		.compatible	= "marvell,armada-8k-cpm-pinctrl",
+		.data		= (void *) V_ARMADA_8K_CPM,
+	},
+	{
+		.compatible	= "marvell,armada-8k-cps-pinctrl",
+		.data		= (void *) V_ARMADA_8K_CPS,
+	},
+	{ },
+};
+
+static const struct mvebu_mpp_ctrl armada_cp110_mpp_controls[] = {
+	MPP_FUNC_CTRL(0, 62, NULL, mvebu_regmap_mpp_ctrl),
+};
+
+static void mvebu_pinctrl_assign_variant(struct mvebu_mpp_mode *m,
+					 u8 variant)
+{
+	struct mvebu_mpp_ctrl_setting *s;
+
+	for (s = m->settings ; s->name ; s++)
+		s->variant = variant;
+}
+
+static int armada_cp110_pinctrl_probe(struct platform_device *pdev)
+{
+	struct mvebu_pinctrl_soc_info *soc;
+	const struct of_device_id *match =
+		of_match_device(armada_cp110_pinctrl_of_match, &pdev->dev);
+	int i;
+
+	if (!pdev->dev.parent)
+		return -ENODEV;
+
+	soc = devm_kzalloc(&pdev->dev,
+			   sizeof(struct mvebu_pinctrl_soc_info), GFP_KERNEL);
+	if (!soc)
+		return -ENOMEM;
+
+	soc->variant = (unsigned long) match->data & 0xff;
+	soc->controls = armada_cp110_mpp_controls;
+	soc->ncontrols = ARRAY_SIZE(armada_cp110_mpp_controls);
+	soc->modes = armada_cp110_mpp_modes;
+	soc->nmodes = ARRAY_SIZE(armada_cp110_mpp_modes);
+	for (i = 0; i < ARRAY_SIZE(armada_cp110_mpp_modes); i++) {
+		struct mvebu_mpp_mode *m = &armada_cp110_mpp_modes[i];
+
+		switch (i) {
+		case 0 ... 31:
+			mvebu_pinctrl_assign_variant(m, V_ARMADA_7K_8K_CPS);
+			break;
+		case 32 ... 38:
+			mvebu_pinctrl_assign_variant(m, V_ARMADA_7K_8K_CPM);
+			break;
+		case 39 ... 43:
+			mvebu_pinctrl_assign_variant(m, V_ARMADA_8K_CPM);
+			break;
+		case 44 ... 62:
+			mvebu_pinctrl_assign_variant(m, V_ARMADA_7K_8K_CPM);
+			break;
+		}
+	}
+	pdev->dev.platform_data = soc;
+
+	return mvebu_pinctrl_simple_regmap_probe(pdev, pdev->dev.parent, 0);
+}
+
+static struct platform_driver armada_cp110_pinctrl_driver = {
+	.driver = {
+		.name = "armada-cp110-pinctrl",
+		.of_match_table = of_match_ptr(armada_cp110_pinctrl_of_match),
+	},
+	.probe = armada_cp110_pinctrl_probe,
+};
+
+builtin_platform_driver(armada_cp110_pinctrl_driver);
diff --git a/drivers/pinctrl/mvebu/pinctrl-mvebu.c b/drivers/pinctrl/mvebu/pinctrl-mvebu.c
index e4dda12d371a..163d4614b0f8 100644
--- a/drivers/pinctrl/mvebu/pinctrl-mvebu.c
+++ b/drivers/pinctrl/mvebu/pinctrl-mvebu.c
@@ -810,21 +810,17 @@ int mvebu_regmap_mpp_ctrl_set(struct mvebu_mpp_ctrl_data *data,
 }
 
 int mvebu_pinctrl_simple_regmap_probe(struct platform_device *pdev,
-				      struct device *syscon_dev)
+				      struct device *syscon_dev, u32 offset)
 {
 	struct mvebu_pinctrl_soc_info *soc = dev_get_platdata(&pdev->dev);
 	struct mvebu_mpp_ctrl_data *mpp_data;
 	struct regmap *regmap;
-	u32 offset;
 	int i;
 
 	regmap = syscon_node_to_regmap(syscon_dev->of_node);
 	if (IS_ERR(regmap))
 		return PTR_ERR(regmap);
 
-	if (of_property_read_u32(pdev->dev.of_node, "offset", &offset))
-		return -EINVAL;
-
 	mpp_data = devm_kcalloc(&pdev->dev, soc->ncontrols, sizeof(*mpp_data),
 				GFP_KERNEL);
 	if (!mpp_data)
diff --git a/drivers/pinctrl/mvebu/pinctrl-mvebu.h b/drivers/pinctrl/mvebu/pinctrl-mvebu.h
index c90704e74884..75bba436bf59 100644
--- a/drivers/pinctrl/mvebu/pinctrl-mvebu.h
+++ b/drivers/pinctrl/mvebu/pinctrl-mvebu.h
@@ -210,6 +210,6 @@ int mvebu_regmap_mpp_ctrl_set(struct mvebu_mpp_ctrl_data *data, unsigned pid,
 int mvebu_pinctrl_probe(struct platform_device *pdev);
 int mvebu_pinctrl_simple_mmio_probe(struct platform_device *pdev);
 int mvebu_pinctrl_simple_regmap_probe(struct platform_device *pdev,
-				      struct device *syscon_dev);
+				      struct device *syscon_dev, u32 offset);
 
 #endif
diff --git a/drivers/pinctrl/pinconf-generic.c b/drivers/pinctrl/pinconf-generic.c
index 720a19fd38d2..fc0c230aa11f 100644
--- a/drivers/pinctrl/pinconf-generic.c
+++ b/drivers/pinctrl/pinconf-generic.c
@@ -44,6 +44,7 @@ static const struct pin_config_item conf_items[] = {
 	PCONFDUMP(PIN_CONFIG_INPUT_SCHMITT, "input schmitt trigger", NULL, false),
 	PCONFDUMP(PIN_CONFIG_INPUT_SCHMITT_ENABLE, "input schmitt enabled", NULL, false),
 	PCONFDUMP(PIN_CONFIG_LOW_POWER_MODE, "pin low power", "mode", true),
+	PCONFDUMP(PIN_CONFIG_OUTPUT_ENABLE, "output enabled", NULL, false),
 	PCONFDUMP(PIN_CONFIG_OUTPUT, "pin output", "level", true),
 	PCONFDUMP(PIN_CONFIG_POWER_SOURCE, "pin power source", "selector", true),
 	PCONFDUMP(PIN_CONFIG_SLEW_RATE, "slew rate", NULL, true),
@@ -172,6 +173,8 @@ static const struct pinconf_generic_params dt_params[] = {
 	{ "input-schmitt-enable", PIN_CONFIG_INPUT_SCHMITT_ENABLE, 1 },
 	{ "low-power-disable", PIN_CONFIG_LOW_POWER_MODE, 0 },
 	{ "low-power-enable", PIN_CONFIG_LOW_POWER_MODE, 1 },
+	{ "output-disable", PIN_CONFIG_OUTPUT_ENABLE, 0 },
+	{ "output-enable", PIN_CONFIG_OUTPUT_ENABLE, 1 },
 	{ "output-high", PIN_CONFIG_OUTPUT, 1, },
 	{ "output-low", PIN_CONFIG_OUTPUT, 0, },
 	{ "power-source", PIN_CONFIG_POWER_SOURCE, 0 },
diff --git a/drivers/pinctrl/pinconf.c b/drivers/pinctrl/pinconf.c
index a02dba35fcf3..7fc417e4ae96 100644
--- a/drivers/pinctrl/pinconf.c
+++ b/drivers/pinctrl/pinconf.c
@@ -87,9 +87,8 @@ int pin_config_group_get(const char *dev_name, const char *pin_group,
 	ops = pctldev->desc->confops;
 
 	if (!ops || !ops->pin_config_group_get) {
-		dev_dbg(pctldev->dev, "cannot get configuration for pin "
-			"group, missing group config get function in "
-			"driver\n");
+		dev_dbg(pctldev->dev,
+			"cannot get configuration for pin group, missing group config get function in driver\n");
 		ret = -ENOTSUPP;
 		goto unlock;
 	}
@@ -232,7 +231,7 @@ static void pinconf_show_config(struct seq_file *s, struct pinctrl_dev *pctldev,
 							    configs[i]);
 		else
 			seq_printf(s, "%08lx", configs[i]);
-		seq_puts(s, "\n");
+		seq_putc(s, '\n');
 	}
 }
 
@@ -244,10 +243,10 @@ void pinconf_show_map(struct seq_file *s, struct pinctrl_map const *map)
 
 	switch (map->type) {
 	case PIN_MAP_TYPE_CONFIGS_PIN:
-		seq_printf(s, "pin ");
+		seq_puts(s, "pin ");
 		break;
 	case PIN_MAP_TYPE_CONFIGS_GROUP:
-		seq_printf(s, "group ");
+		seq_puts(s, "group ");
 		break;
 	default:
 		break;
@@ -319,14 +318,13 @@ static int pinconf_pins_show(struct seq_file *s, void *what)
 		pin = pctldev->desc->pins[i].number;
 		desc = pin_desc_get(pctldev, pin);
 		/* Skip if we cannot search the pin */
-		if (desc == NULL)
+		if (!desc)
 			continue;
 
 		seq_printf(s, "pin %d (%s): ", pin, desc->name);
 
 		pinconf_dump_pin(pctldev, s, pin);
-
-		seq_printf(s, "\n");
+		seq_putc(s, '\n');
 	}
 
 	mutex_unlock(&pctldev->mutex);
@@ -361,8 +359,7 @@ static int pinconf_groups_show(struct seq_file *s, void *what)
 
 		seq_printf(s, "%u (%s): ", selector, gname);
 		pinconf_dump_group(pctldev, s, selector, gname);
-		seq_printf(s, "\n");
-
+		seq_putc(s, '\n');
 		selector++;
 	}
 
@@ -397,9 +394,9 @@ static const struct file_operations pinconf_groups_ops = {
 
 struct dbg_cfg {
 	enum pinctrl_map_type map_type;
-	char dev_name[MAX_NAME_LEN+1];
-	char state_name[MAX_NAME_LEN+1];
-	char pin_name[MAX_NAME_LEN+1];
+	char dev_name[MAX_NAME_LEN + 1];
+	char state_name[MAX_NAME_LEN + 1];
+	char pin_name[MAX_NAME_LEN + 1];
 };
 
 /*
@@ -485,7 +482,7 @@ static ssize_t pinconf_dbg_config_write(struct file *file,
 	const struct pinconf_ops *confops = NULL;
 	struct dbg_cfg *dbg = &pinconf_dbg_conf;
 	const struct pinctrl_map_configs *configs;
-	char config[MAX_NAME_LEN+1];
+	char config[MAX_NAME_LEN + 1];
 	char buf[128];
 	char *b = &buf[0];
 	int buf_size;
@@ -526,7 +523,7 @@ static ssize_t pinconf_dbg_config_write(struct file *file,
 
 	/* get arg 'device_name' */
 	token = strsep(&b, " ");
-	if (token == NULL)
+	if (!token)
 		return -EINVAL;
 	if (strlen(token) >= MAX_NAME_LEN)
 		return -EINVAL;
@@ -534,7 +531,7 @@ static ssize_t pinconf_dbg_config_write(struct file *file,
 
 	/* get arg 'state_name' */
 	token = strsep(&b, " ");
-	if (token == NULL)
+	if (!token)
 		return -EINVAL;
 	if (strlen(token) >= MAX_NAME_LEN)
 		return -EINVAL;
@@ -542,7 +539,7 @@ static ssize_t pinconf_dbg_config_write(struct file *file,
 
 	/* get arg 'pin_name' */
 	token = strsep(&b, " ");
-	if (token == NULL)
+	if (!token)
 		return -EINVAL;
 	if (strlen(token) >= MAX_NAME_LEN)
 		return -EINVAL;
@@ -550,7 +547,7 @@ static ssize_t pinconf_dbg_config_write(struct file *file,
 
 	/* get new_value of config' */
 	token = strsep(&b, " ");
-	if (token == NULL)
+	if (!token)
 		return -EINVAL;
 	if (strlen(token) >= MAX_NAME_LEN)
 		return -EINVAL;
diff --git a/drivers/pinctrl/pinctrl-amd.c b/drivers/pinctrl/pinctrl-amd.c
index 1482d132fbb8..e6779d4352a2 100644
--- a/drivers/pinctrl/pinctrl-amd.c
+++ b/drivers/pinctrl/pinctrl-amd.c
@@ -8,6 +8,10 @@
  * 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.
+ *
+ * Contact Information: Nehal Shah <Nehal-bakulchandra.Shah@amd.com>
+ *			Shyam Sundar S K <Shyam-sundar.S-k@amd.com>
+ *
  */
 
 #include <linux/err.h>
@@ -495,64 +499,54 @@ static struct irq_chip amd_gpio_irqchip = {
 	.flags        = IRQCHIP_SKIP_SET_WAKE,
 };
 
-static void amd_gpio_irq_handler(struct irq_desc *desc)
+#define PIN_IRQ_PENDING	(BIT(INTERRUPT_STS_OFF) | BIT(WAKE_STS_OFF))
+
+static irqreturn_t amd_gpio_irq_handler(int irq, void *dev_id)
 {
-	u32 i;
-	u32 off;
-	u32 reg;
-	u32 pin_reg;
-	u64 reg64;
-	int handled = 0;
-	unsigned int irq;
+	struct amd_gpio *gpio_dev = dev_id;
+	struct gpio_chip *gc = &gpio_dev->gc;
+	irqreturn_t ret = IRQ_NONE;
+	unsigned int i, irqnr;
 	unsigned long flags;
-	struct irq_chip *chip = irq_desc_get_chip(desc);
-	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
-	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
+	u32 *regs, regval;
+	u64 status, mask;
 
-	chained_irq_enter(chip, desc);
-	/*enable GPIO interrupt again*/
+	/* Read the wake status */
 	raw_spin_lock_irqsave(&gpio_dev->lock, flags);
-	reg = readl(gpio_dev->base + WAKE_INT_STATUS_REG1);
-	reg64 = reg;
-	reg64 = reg64 << 32;
-
-	reg = readl(gpio_dev->base + WAKE_INT_STATUS_REG0);
-	reg64 |= reg;
+	status = readl(gpio_dev->base + WAKE_INT_STATUS_REG1);
+	status <<= 32;
+	status |= readl(gpio_dev->base + WAKE_INT_STATUS_REG0);
 	raw_spin_unlock_irqrestore(&gpio_dev->lock, flags);
 
-	/*
-	 * first 46 bits indicates interrupt status.
-	 * one bit represents four interrupt sources.
-	*/
-	for (off = 0; off < 46 ; off++) {
-		if (reg64 & BIT(off)) {
-			for (i = 0; i < 4; i++) {
-				pin_reg = readl(gpio_dev->base +
-						(off * 4 + i) * 4);
-				if ((pin_reg & BIT(INTERRUPT_STS_OFF)) ||
-					(pin_reg & BIT(WAKE_STS_OFF))) {
-					irq = irq_find_mapping(gc->irqdomain,
-								off * 4 + i);
-					generic_handle_irq(irq);
-					writel(pin_reg,
-						gpio_dev->base
-						+ (off * 4 + i) * 4);
-					handled++;
-				}
-			}
+	/* Bit 0-45 contain the relevant status bits */
+	status &= (1ULL << 46) - 1;
+	regs = gpio_dev->base;
+	for (mask = 1, irqnr = 0; status; mask <<= 1, regs += 4, irqnr += 4) {
+		if (!(status & mask))
+			continue;
+		status &= ~mask;
+
+		/* Each status bit covers four pins */
+		for (i = 0; i < 4; i++) {
+			regval = readl(regs + i);
+			if (!(regval & PIN_IRQ_PENDING))
+				continue;
+			irq = irq_find_mapping(gc->irqdomain, irqnr + i);
+			generic_handle_irq(irq);
+			/* Clear interrupt */
+			writel(regval, regs + i);
+			ret = IRQ_HANDLED;
 		}
 	}
 
-	if (handled == 0)
-		handle_bad_irq(desc);
-
+	/* Signal EOI to the GPIO unit */
 	raw_spin_lock_irqsave(&gpio_dev->lock, flags);
-	reg = readl(gpio_dev->base + WAKE_INT_MASTER_REG);
-	reg |= EOI_MASK;
-	writel(reg, gpio_dev->base + WAKE_INT_MASTER_REG);
+	regval = readl(gpio_dev->base + WAKE_INT_MASTER_REG);
+	regval |= EOI_MASK;
+	writel(regval, gpio_dev->base + WAKE_INT_MASTER_REG);
 	raw_spin_unlock_irqrestore(&gpio_dev->lock, flags);
 
-	chained_irq_exit(chip, desc);
+	return ret;
 }
 
 static int amd_get_groups_count(struct pinctrl_dev *pctldev)
@@ -821,10 +815,11 @@ static int amd_gpio_probe(struct platform_device *pdev)
 		goto out2;
 	}
 
-	gpiochip_set_chained_irqchip(&gpio_dev->gc,
-				 &amd_gpio_irqchip,
-				 irq_base,
-				 amd_gpio_irq_handler);
+	ret = devm_request_irq(&pdev->dev, irq_base, amd_gpio_irq_handler, 0,
+			       KBUILD_MODNAME, gpio_dev);
+	if (ret)
+		goto out2;
+
 	platform_set_drvdata(pdev, gpio_dev);
 
 	dev_dbg(&pdev->dev, "amd gpio driver loaded\n");
diff --git a/drivers/pinctrl/pinctrl-ingenic.c b/drivers/pinctrl/pinctrl-ingenic.c
new file mode 100644
index 000000000000..d8e8842967d6
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-ingenic.c
@@ -0,0 +1,852 @@
+/*
+ * Ingenic SoCs pinctrl driver
+ *
+ * Copyright (c) 2017 Paul Cercueil <paul@crapouillou.net>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/compiler.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/of_device.h>
+#include <linux/of_platform.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinconf-generic.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+#include "core.h"
+#include "pinconf.h"
+#include "pinmux.h"
+
+#define JZ4740_GPIO_DATA	0x10
+#define JZ4740_GPIO_PULL_DIS	0x30
+#define JZ4740_GPIO_FUNC	0x40
+#define JZ4740_GPIO_SELECT	0x50
+#define JZ4740_GPIO_DIR		0x60
+#define JZ4740_GPIO_TRIG	0x70
+#define JZ4740_GPIO_FLAG	0x80
+
+#define JZ4770_GPIO_INT		0x10
+#define JZ4770_GPIO_MSK		0x20
+#define JZ4770_GPIO_PAT1	0x30
+#define JZ4770_GPIO_PAT0	0x40
+#define JZ4770_GPIO_FLAG	0x50
+#define JZ4770_GPIO_PEN		0x70
+
+#define REG_SET(x) ((x) + 0x4)
+#define REG_CLEAR(x) ((x) + 0x8)
+
+#define PINS_PER_GPIO_CHIP 32
+
+enum jz_version {
+	ID_JZ4740,
+	ID_JZ4770,
+	ID_JZ4780,
+};
+
+struct ingenic_chip_info {
+	unsigned int num_chips;
+
+	const struct group_desc *groups;
+	unsigned int num_groups;
+
+	const struct function_desc *functions;
+	unsigned int num_functions;
+
+	const u32 *pull_ups, *pull_downs;
+};
+
+struct ingenic_pinctrl {
+	struct device *dev;
+	struct regmap *map;
+	struct pinctrl_dev *pctl;
+	struct pinctrl_pin_desc *pdesc;
+	enum jz_version version;
+
+	const struct ingenic_chip_info *info;
+};
+
+static const u32 jz4740_pull_ups[4] = {
+	0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff,
+};
+
+static const u32 jz4740_pull_downs[4] = {
+	0x00000000, 0x00000000, 0x00000000, 0x00000000,
+};
+
+static int jz4740_mmc_1bit_pins[] = { 0x69, 0x68, 0x6a, };
+static int jz4740_mmc_4bit_pins[] = { 0x6b, 0x6c, 0x6d, };
+static int jz4740_uart0_data_pins[] = { 0x7a, 0x79, };
+static int jz4740_uart0_hwflow_pins[] = { 0x7e, 0x7f, };
+static int jz4740_uart1_data_pins[] = { 0x7e, 0x7f, };
+static int jz4740_lcd_8bit_pins[] = {
+	0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x52, 0x53, 0x54,
+};
+static int jz4740_lcd_16bit_pins[] = {
+	0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f, 0x55,
+};
+static int jz4740_lcd_18bit_pins[] = { 0x50, 0x51, };
+static int jz4740_lcd_18bit_tft_pins[] = { 0x56, 0x57, 0x31, 0x32, };
+static int jz4740_nand_cs1_pins[] = { 0x39, };
+static int jz4740_nand_cs2_pins[] = { 0x3a, };
+static int jz4740_nand_cs3_pins[] = { 0x3b, };
+static int jz4740_nand_cs4_pins[] = { 0x3c, };
+static int jz4740_pwm_pwm0_pins[] = { 0x77, };
+static int jz4740_pwm_pwm1_pins[] = { 0x78, };
+static int jz4740_pwm_pwm2_pins[] = { 0x79, };
+static int jz4740_pwm_pwm3_pins[] = { 0x7a, };
+static int jz4740_pwm_pwm4_pins[] = { 0x7b, };
+static int jz4740_pwm_pwm5_pins[] = { 0x7c, };
+static int jz4740_pwm_pwm6_pins[] = { 0x7e, };
+static int jz4740_pwm_pwm7_pins[] = { 0x7f, };
+
+static int jz4740_mmc_1bit_funcs[] = { 0, 0, 0, };
+static int jz4740_mmc_4bit_funcs[] = { 0, 0, 0, };
+static int jz4740_uart0_data_funcs[] = { 1, 1, };
+static int jz4740_uart0_hwflow_funcs[] = { 1, 1, };
+static int jz4740_uart1_data_funcs[] = { 2, 2, };
+static int jz4740_lcd_8bit_funcs[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, };
+static int jz4740_lcd_16bit_funcs[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, };
+static int jz4740_lcd_18bit_funcs[] = { 0, 0, };
+static int jz4740_lcd_18bit_tft_funcs[] = { 0, 0, 0, 0, };
+static int jz4740_nand_cs1_funcs[] = { 0, };
+static int jz4740_nand_cs2_funcs[] = { 0, };
+static int jz4740_nand_cs3_funcs[] = { 0, };
+static int jz4740_nand_cs4_funcs[] = { 0, };
+static int jz4740_pwm_pwm0_funcs[] = { 0, };
+static int jz4740_pwm_pwm1_funcs[] = { 0, };
+static int jz4740_pwm_pwm2_funcs[] = { 0, };
+static int jz4740_pwm_pwm3_funcs[] = { 0, };
+static int jz4740_pwm_pwm4_funcs[] = { 0, };
+static int jz4740_pwm_pwm5_funcs[] = { 0, };
+static int jz4740_pwm_pwm6_funcs[] = { 0, };
+static int jz4740_pwm_pwm7_funcs[] = { 0, };
+
+#define INGENIC_PIN_GROUP(name, id)			\
+	{						\
+		name,					\
+		id##_pins,				\
+		ARRAY_SIZE(id##_pins),			\
+		id##_funcs,				\
+	}
+
+static const struct group_desc jz4740_groups[] = {
+	INGENIC_PIN_GROUP("mmc-1bit", jz4740_mmc_1bit),
+	INGENIC_PIN_GROUP("mmc-4bit", jz4740_mmc_4bit),
+	INGENIC_PIN_GROUP("uart0-data", jz4740_uart0_data),
+	INGENIC_PIN_GROUP("uart0-hwflow", jz4740_uart0_hwflow),
+	INGENIC_PIN_GROUP("uart1-data", jz4740_uart1_data),
+	INGENIC_PIN_GROUP("lcd-8bit", jz4740_lcd_8bit),
+	INGENIC_PIN_GROUP("lcd-16bit", jz4740_lcd_16bit),
+	INGENIC_PIN_GROUP("lcd-18bit", jz4740_lcd_18bit),
+	INGENIC_PIN_GROUP("lcd-18bit-tft", jz4740_lcd_18bit_tft),
+	{ "lcd-no-pins", },
+	INGENIC_PIN_GROUP("nand-cs1", jz4740_nand_cs1),
+	INGENIC_PIN_GROUP("nand-cs2", jz4740_nand_cs2),
+	INGENIC_PIN_GROUP("nand-cs3", jz4740_nand_cs3),
+	INGENIC_PIN_GROUP("nand-cs4", jz4740_nand_cs4),
+	INGENIC_PIN_GROUP("pwm0", jz4740_pwm_pwm0),
+	INGENIC_PIN_GROUP("pwm1", jz4740_pwm_pwm1),
+	INGENIC_PIN_GROUP("pwm2", jz4740_pwm_pwm2),
+	INGENIC_PIN_GROUP("pwm3", jz4740_pwm_pwm3),
+	INGENIC_PIN_GROUP("pwm4", jz4740_pwm_pwm4),
+	INGENIC_PIN_GROUP("pwm5", jz4740_pwm_pwm5),
+	INGENIC_PIN_GROUP("pwm6", jz4740_pwm_pwm6),
+	INGENIC_PIN_GROUP("pwm7", jz4740_pwm_pwm7),
+};
+
+static const char *jz4740_mmc_groups[] = { "mmc-1bit", "mmc-4bit", };
+static const char *jz4740_uart0_groups[] = { "uart0-data", "uart0-hwflow", };
+static const char *jz4740_uart1_groups[] = { "uart1-data", };
+static const char *jz4740_lcd_groups[] = {
+	"lcd-8bit", "lcd-16bit", "lcd-18bit", "lcd-18bit-tft", "lcd-no-pins",
+};
+static const char *jz4740_nand_groups[] = {
+	"nand-cs1", "nand-cs2", "nand-cs3", "nand-cs4",
+};
+static const char *jz4740_pwm0_groups[] = { "pwm0", };
+static const char *jz4740_pwm1_groups[] = { "pwm1", };
+static const char *jz4740_pwm2_groups[] = { "pwm2", };
+static const char *jz4740_pwm3_groups[] = { "pwm3", };
+static const char *jz4740_pwm4_groups[] = { "pwm4", };
+static const char *jz4740_pwm5_groups[] = { "pwm5", };
+static const char *jz4740_pwm6_groups[] = { "pwm6", };
+static const char *jz4740_pwm7_groups[] = { "pwm7", };
+
+static const struct function_desc jz4740_functions[] = {
+	{ "mmc", jz4740_mmc_groups, ARRAY_SIZE(jz4740_mmc_groups), },
+	{ "uart0", jz4740_uart0_groups, ARRAY_SIZE(jz4740_uart0_groups), },
+	{ "uart1", jz4740_uart1_groups, ARRAY_SIZE(jz4740_uart1_groups), },
+	{ "lcd", jz4740_lcd_groups, ARRAY_SIZE(jz4740_lcd_groups), },
+	{ "nand", jz4740_nand_groups, ARRAY_SIZE(jz4740_nand_groups), },
+	{ "pwm0", jz4740_pwm0_groups, ARRAY_SIZE(jz4740_pwm0_groups), },
+	{ "pwm1", jz4740_pwm1_groups, ARRAY_SIZE(jz4740_pwm1_groups), },
+	{ "pwm2", jz4740_pwm2_groups, ARRAY_SIZE(jz4740_pwm2_groups), },
+	{ "pwm3", jz4740_pwm3_groups, ARRAY_SIZE(jz4740_pwm3_groups), },
+	{ "pwm4", jz4740_pwm4_groups, ARRAY_SIZE(jz4740_pwm4_groups), },
+	{ "pwm5", jz4740_pwm5_groups, ARRAY_SIZE(jz4740_pwm5_groups), },
+	{ "pwm6", jz4740_pwm6_groups, ARRAY_SIZE(jz4740_pwm6_groups), },
+	{ "pwm7", jz4740_pwm7_groups, ARRAY_SIZE(jz4740_pwm7_groups), },
+};
+
+static const struct ingenic_chip_info jz4740_chip_info = {
+	.num_chips = 4,
+	.groups = jz4740_groups,
+	.num_groups = ARRAY_SIZE(jz4740_groups),
+	.functions = jz4740_functions,
+	.num_functions = ARRAY_SIZE(jz4740_functions),
+	.pull_ups = jz4740_pull_ups,
+	.pull_downs = jz4740_pull_downs,
+};
+
+static const u32 jz4770_pull_ups[6] = {
+	0x3fffffff, 0xfff0030c, 0xffffffff, 0xffff4fff, 0xfffffb7c, 0xffa7f00f,
+};
+
+static const u32 jz4770_pull_downs[6] = {
+	0x00000000, 0x000f0c03, 0x00000000, 0x0000b000, 0x00000483, 0x00580ff0,
+};
+
+static int jz4770_uart0_data_pins[] = { 0xa0, 0xa3, };
+static int jz4770_uart0_hwflow_pins[] = { 0xa1, 0xa2, };
+static int jz4770_uart1_data_pins[] = { 0x7a, 0x7c, };
+static int jz4770_uart1_hwflow_pins[] = { 0x7b, 0x7d, };
+static int jz4770_uart2_data_pins[] = { 0x66, 0x67, };
+static int jz4770_uart2_hwflow_pins[] = { 0x65, 0x64, };
+static int jz4770_uart3_data_pins[] = { 0x6c, 0x85, };
+static int jz4770_uart3_hwflow_pins[] = { 0x88, 0x89, };
+static int jz4770_uart4_data_pins[] = { 0x54, 0x4a, };
+static int jz4770_mmc0_8bit_a_pins[] = { 0x04, 0x05, 0x06, 0x07, 0x18, };
+static int jz4770_mmc0_4bit_a_pins[] = { 0x15, 0x16, 0x17, };
+static int jz4770_mmc0_1bit_a_pins[] = { 0x12, 0x13, 0x14, };
+static int jz4770_mmc0_4bit_e_pins[] = { 0x95, 0x96, 0x97, };
+static int jz4770_mmc0_1bit_e_pins[] = { 0x9c, 0x9d, 0x94, };
+static int jz4770_mmc1_4bit_d_pins[] = { 0x75, 0x76, 0x77, };
+static int jz4770_mmc1_1bit_d_pins[] = { 0x78, 0x79, 0x74, };
+static int jz4770_mmc1_4bit_e_pins[] = { 0x95, 0x96, 0x97, };
+static int jz4770_mmc1_1bit_e_pins[] = { 0x9c, 0x9d, 0x94, };
+static int jz4770_nemc_data_pins[] = {
+	0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+};
+static int jz4770_nemc_cle_ale_pins[] = { 0x20, 0x21, };
+static int jz4770_nemc_addr_pins[] = { 0x22, 0x23, 0x24, 0x25, };
+static int jz4770_nemc_rd_we_pins[] = { 0x10, 0x11, };
+static int jz4770_nemc_frd_fwe_pins[] = { 0x12, 0x13, };
+static int jz4770_nemc_cs1_pins[] = { 0x15, };
+static int jz4770_nemc_cs2_pins[] = { 0x16, };
+static int jz4770_nemc_cs3_pins[] = { 0x17, };
+static int jz4770_nemc_cs4_pins[] = { 0x18, };
+static int jz4770_nemc_cs5_pins[] = { 0x19, };
+static int jz4770_nemc_cs6_pins[] = { 0x1a, };
+static int jz4770_i2c0_pins[] = { 0x6e, 0x6f, };
+static int jz4770_i2c1_pins[] = { 0x8e, 0x8f, };
+static int jz4770_i2c2_pins[] = { 0xb0, 0xb1, };
+static int jz4770_i2c3_pins[] = { 0x6a, 0x6b, };
+static int jz4770_i2c4_e_pins[] = { 0x8c, 0x8d, };
+static int jz4770_i2c4_f_pins[] = { 0xb9, 0xb8, };
+static int jz4770_cim_pins[] = {
+	0x26, 0x27, 0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31,
+};
+static int jz4770_lcd_32bit_pins[] = {
+	0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47,
+	0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f,
+	0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57,
+	0x58, 0x59, 0x51,
+};
+static int jz4770_pwm_pwm0_pins[] = { 0x80, };
+static int jz4770_pwm_pwm1_pins[] = { 0x81, };
+static int jz4770_pwm_pwm2_pins[] = { 0x82, };
+static int jz4770_pwm_pwm3_pins[] = { 0x83, };
+static int jz4770_pwm_pwm4_pins[] = { 0x84, };
+static int jz4770_pwm_pwm5_pins[] = { 0x85, };
+static int jz4770_pwm_pwm6_pins[] = { 0x6a, };
+static int jz4770_pwm_pwm7_pins[] = { 0x6b, };
+
+static int jz4770_uart0_data_funcs[] = { 0, 0, };
+static int jz4770_uart0_hwflow_funcs[] = { 0, 0, };
+static int jz4770_uart1_data_funcs[] = { 0, 0, };
+static int jz4770_uart1_hwflow_funcs[] = { 0, 0, };
+static int jz4770_uart2_data_funcs[] = { 1, 1, };
+static int jz4770_uart2_hwflow_funcs[] = { 1, 1, };
+static int jz4770_uart3_data_funcs[] = { 0, 1, };
+static int jz4770_uart3_hwflow_funcs[] = { 0, 0, };
+static int jz4770_uart4_data_funcs[] = { 2, 2, };
+static int jz4770_mmc0_8bit_a_funcs[] = { 1, 1, 1, 1, 1, };
+static int jz4770_mmc0_4bit_a_funcs[] = { 1, 1, 1, };
+static int jz4770_mmc0_1bit_a_funcs[] = { 1, 1, 0, };
+static int jz4770_mmc0_4bit_e_funcs[] = { 0, 0, 0, };
+static int jz4770_mmc0_1bit_e_funcs[] = { 0, 0, 0, };
+static int jz4770_mmc1_4bit_d_funcs[] = { 0, 0, 0, };
+static int jz4770_mmc1_1bit_d_funcs[] = { 0, 0, 0, };
+static int jz4770_mmc1_4bit_e_funcs[] = { 1, 1, 1, };
+static int jz4770_mmc1_1bit_e_funcs[] = { 1, 1, 1, };
+static int jz4770_nemc_data_funcs[] = { 0, 0, 0, 0, 0, 0, 0, 0, };
+static int jz4770_nemc_cle_ale_funcs[] = { 0, 0, };
+static int jz4770_nemc_addr_funcs[] = { 0, 0, 0, 0, };
+static int jz4770_nemc_rd_we_funcs[] = { 0, 0, };
+static int jz4770_nemc_frd_fwe_funcs[] = { 0, 0, };
+static int jz4770_nemc_cs1_funcs[] = { 0, };
+static int jz4770_nemc_cs2_funcs[] = { 0, };
+static int jz4770_nemc_cs3_funcs[] = { 0, };
+static int jz4770_nemc_cs4_funcs[] = { 0, };
+static int jz4770_nemc_cs5_funcs[] = { 0, };
+static int jz4770_nemc_cs6_funcs[] = { 0, };
+static int jz4770_i2c0_funcs[] = { 0, 0, };
+static int jz4770_i2c1_funcs[] = { 0, 0, };
+static int jz4770_i2c2_funcs[] = { 2, 2, };
+static int jz4770_i2c3_funcs[] = { 1, 1, };
+static int jz4770_i2c4_e_funcs[] = { 1, 1, };
+static int jz4770_i2c4_f_funcs[] = { 1, 1, };
+static int jz4770_cim_funcs[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, };
+static int jz4770_lcd_32bit_funcs[] = {
+	0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0,
+};
+static int jz4770_pwm_pwm0_funcs[] = { 0, };
+static int jz4770_pwm_pwm1_funcs[] = { 0, };
+static int jz4770_pwm_pwm2_funcs[] = { 0, };
+static int jz4770_pwm_pwm3_funcs[] = { 0, };
+static int jz4770_pwm_pwm4_funcs[] = { 0, };
+static int jz4770_pwm_pwm5_funcs[] = { 0, };
+static int jz4770_pwm_pwm6_funcs[] = { 0, };
+static int jz4770_pwm_pwm7_funcs[] = { 0, };
+
+static const struct group_desc jz4770_groups[] = {
+	INGENIC_PIN_GROUP("uart0-data", jz4770_uart0_data),
+	INGENIC_PIN_GROUP("uart0-hwflow", jz4770_uart0_hwflow),
+	INGENIC_PIN_GROUP("uart1-data", jz4770_uart1_data),
+	INGENIC_PIN_GROUP("uart1-hwflow", jz4770_uart1_hwflow),
+	INGENIC_PIN_GROUP("uart2-data", jz4770_uart2_data),
+	INGENIC_PIN_GROUP("uart2-hwflow", jz4770_uart2_hwflow),
+	INGENIC_PIN_GROUP("uart3-data", jz4770_uart3_data),
+	INGENIC_PIN_GROUP("uart3-hwflow", jz4770_uart3_hwflow),
+	INGENIC_PIN_GROUP("uart4-data", jz4770_uart4_data),
+	INGENIC_PIN_GROUP("mmc0-8bit-a", jz4770_mmc0_8bit_a),
+	INGENIC_PIN_GROUP("mmc0-4bit-a", jz4770_mmc0_4bit_a),
+	INGENIC_PIN_GROUP("mmc0-1bit-a", jz4770_mmc0_1bit_a),
+	INGENIC_PIN_GROUP("mmc0-4bit-e", jz4770_mmc0_4bit_e),
+	INGENIC_PIN_GROUP("mmc0-1bit-e", jz4770_mmc0_1bit_e),
+	INGENIC_PIN_GROUP("mmc1-4bit-d", jz4770_mmc1_4bit_d),
+	INGENIC_PIN_GROUP("mmc1-1bit-d", jz4770_mmc1_1bit_d),
+	INGENIC_PIN_GROUP("mmc1-4bit-e", jz4770_mmc1_4bit_e),
+	INGENIC_PIN_GROUP("mmc1-1bit-e", jz4770_mmc1_1bit_e),
+	INGENIC_PIN_GROUP("nemc-data", jz4770_nemc_data),
+	INGENIC_PIN_GROUP("nemc-cle-ale", jz4770_nemc_cle_ale),
+	INGENIC_PIN_GROUP("nemc-addr", jz4770_nemc_addr),
+	INGENIC_PIN_GROUP("nemc-rd-we", jz4770_nemc_rd_we),
+	INGENIC_PIN_GROUP("nemc-frd-fwe", jz4770_nemc_frd_fwe),
+	INGENIC_PIN_GROUP("nemc-cs1", jz4770_nemc_cs1),
+	INGENIC_PIN_GROUP("nemc-cs2", jz4770_nemc_cs2),
+	INGENIC_PIN_GROUP("nemc-cs3", jz4770_nemc_cs3),
+	INGENIC_PIN_GROUP("nemc-cs4", jz4770_nemc_cs4),
+	INGENIC_PIN_GROUP("nemc-cs5", jz4770_nemc_cs5),
+	INGENIC_PIN_GROUP("nemc-cs6", jz4770_nemc_cs6),
+	INGENIC_PIN_GROUP("i2c0-data", jz4770_i2c0),
+	INGENIC_PIN_GROUP("i2c1-data", jz4770_i2c1),
+	INGENIC_PIN_GROUP("i2c2-data", jz4770_i2c2),
+	INGENIC_PIN_GROUP("i2c3-data", jz4770_i2c3),
+	INGENIC_PIN_GROUP("i2c4-data-e", jz4770_i2c4_e),
+	INGENIC_PIN_GROUP("i2c4-data-f", jz4770_i2c4_f),
+	INGENIC_PIN_GROUP("cim-data", jz4770_cim),
+	INGENIC_PIN_GROUP("lcd-32bit", jz4770_lcd_32bit),
+	{ "lcd-no-pins", },
+	INGENIC_PIN_GROUP("pwm0", jz4770_pwm_pwm0),
+	INGENIC_PIN_GROUP("pwm1", jz4770_pwm_pwm1),
+	INGENIC_PIN_GROUP("pwm2", jz4770_pwm_pwm2),
+	INGENIC_PIN_GROUP("pwm3", jz4770_pwm_pwm3),
+	INGENIC_PIN_GROUP("pwm4", jz4770_pwm_pwm4),
+	INGENIC_PIN_GROUP("pwm5", jz4770_pwm_pwm5),
+	INGENIC_PIN_GROUP("pwm6", jz4770_pwm_pwm6),
+	INGENIC_PIN_GROUP("pwm7", jz4770_pwm_pwm7),
+};
+
+static const char *jz4770_uart0_groups[] = { "uart0-data", "uart0-hwflow", };
+static const char *jz4770_uart1_groups[] = { "uart1-data", "uart1-hwflow", };
+static const char *jz4770_uart2_groups[] = { "uart2-data", "uart2-hwflow", };
+static const char *jz4770_uart3_groups[] = { "uart3-data", "uart3-hwflow", };
+static const char *jz4770_uart4_groups[] = { "uart4-data", };
+static const char *jz4770_mmc0_groups[] = {
+	"mmc0-8bit-a", "mmc0-4bit-a", "mmc0-1bit-a",
+	"mmc0-1bit-e", "mmc0-4bit-e",
+};
+static const char *jz4770_mmc1_groups[] = {
+	"mmc1-1bit-d", "mmc1-4bit-d", "mmc1-1bit-e", "mmc1-4bit-e",
+};
+static const char *jz4770_nemc_groups[] = {
+	"nemc-data", "nemc-cle-ale", "nemc-addr", "nemc-rd-we", "nemc-frd-fwe",
+};
+static const char *jz4770_cs1_groups[] = { "nemc-cs1", };
+static const char *jz4770_cs6_groups[] = { "nemc-cs6", };
+static const char *jz4770_i2c0_groups[] = { "i2c0-data", };
+static const char *jz4770_i2c1_groups[] = { "i2c1-data", };
+static const char *jz4770_i2c2_groups[] = { "i2c2-data", };
+static const char *jz4770_i2c3_groups[] = { "i2c3-data", };
+static const char *jz4770_i2c4_groups[] = { "i2c4-data-e", "i2c4-data-f", };
+static const char *jz4770_cim_groups[] = { "cim-data", };
+static const char *jz4770_lcd_groups[] = { "lcd-32bit", "lcd-no-pins", };
+static const char *jz4770_pwm0_groups[] = { "pwm0", };
+static const char *jz4770_pwm1_groups[] = { "pwm1", };
+static const char *jz4770_pwm2_groups[] = { "pwm2", };
+static const char *jz4770_pwm3_groups[] = { "pwm3", };
+static const char *jz4770_pwm4_groups[] = { "pwm4", };
+static const char *jz4770_pwm5_groups[] = { "pwm5", };
+static const char *jz4770_pwm6_groups[] = { "pwm6", };
+static const char *jz4770_pwm7_groups[] = { "pwm7", };
+
+static const struct function_desc jz4770_functions[] = {
+	{ "uart0", jz4770_uart0_groups, ARRAY_SIZE(jz4770_uart0_groups), },
+	{ "uart1", jz4770_uart1_groups, ARRAY_SIZE(jz4770_uart1_groups), },
+	{ "uart2", jz4770_uart2_groups, ARRAY_SIZE(jz4770_uart2_groups), },
+	{ "uart3", jz4770_uart3_groups, ARRAY_SIZE(jz4770_uart3_groups), },
+	{ "uart4", jz4770_uart4_groups, ARRAY_SIZE(jz4770_uart4_groups), },
+	{ "mmc0", jz4770_mmc0_groups, ARRAY_SIZE(jz4770_mmc0_groups), },
+	{ "mmc1", jz4770_mmc1_groups, ARRAY_SIZE(jz4770_mmc1_groups), },
+	{ "nemc", jz4770_nemc_groups, ARRAY_SIZE(jz4770_nemc_groups), },
+	{ "nemc-cs1", jz4770_cs1_groups, ARRAY_SIZE(jz4770_cs1_groups), },
+	{ "nemc-cs6", jz4770_cs6_groups, ARRAY_SIZE(jz4770_cs6_groups), },
+	{ "i2c0", jz4770_i2c0_groups, ARRAY_SIZE(jz4770_i2c0_groups), },
+	{ "i2c1", jz4770_i2c1_groups, ARRAY_SIZE(jz4770_i2c1_groups), },
+	{ "i2c2", jz4770_i2c2_groups, ARRAY_SIZE(jz4770_i2c2_groups), },
+	{ "i2c3", jz4770_i2c3_groups, ARRAY_SIZE(jz4770_i2c3_groups), },
+	{ "i2c4", jz4770_i2c4_groups, ARRAY_SIZE(jz4770_i2c4_groups), },
+	{ "cim", jz4770_cim_groups, ARRAY_SIZE(jz4770_cim_groups), },
+	{ "lcd", jz4770_lcd_groups, ARRAY_SIZE(jz4770_lcd_groups), },
+	{ "pwm0", jz4770_pwm0_groups, ARRAY_SIZE(jz4770_pwm0_groups), },
+	{ "pwm1", jz4770_pwm1_groups, ARRAY_SIZE(jz4770_pwm1_groups), },
+	{ "pwm2", jz4770_pwm2_groups, ARRAY_SIZE(jz4770_pwm2_groups), },
+	{ "pwm3", jz4770_pwm3_groups, ARRAY_SIZE(jz4770_pwm3_groups), },
+	{ "pwm4", jz4770_pwm4_groups, ARRAY_SIZE(jz4770_pwm4_groups), },
+	{ "pwm5", jz4770_pwm5_groups, ARRAY_SIZE(jz4770_pwm5_groups), },
+	{ "pwm6", jz4770_pwm6_groups, ARRAY_SIZE(jz4770_pwm6_groups), },
+	{ "pwm7", jz4770_pwm7_groups, ARRAY_SIZE(jz4770_pwm7_groups), },
+};
+
+static const struct ingenic_chip_info jz4770_chip_info = {
+	.num_chips = 6,
+	.groups = jz4770_groups,
+	.num_groups = ARRAY_SIZE(jz4770_groups),
+	.functions = jz4770_functions,
+	.num_functions = ARRAY_SIZE(jz4770_functions),
+	.pull_ups = jz4770_pull_ups,
+	.pull_downs = jz4770_pull_downs,
+};
+
+static inline void ingenic_config_pin(struct ingenic_pinctrl *jzpc,
+		unsigned int pin, u8 reg, bool set)
+{
+	unsigned int idx = pin % PINS_PER_GPIO_CHIP;
+	unsigned int offt = pin / PINS_PER_GPIO_CHIP;
+
+	regmap_write(jzpc->map, offt * 0x100 +
+			(set ? REG_SET(reg) : REG_CLEAR(reg)), BIT(idx));
+}
+
+static inline bool ingenic_get_pin_config(struct ingenic_pinctrl *jzpc,
+		unsigned int pin, u8 reg)
+{
+	unsigned int idx = pin % PINS_PER_GPIO_CHIP;
+	unsigned int offt = pin / PINS_PER_GPIO_CHIP;
+	unsigned int val;
+
+	regmap_read(jzpc->map, offt * 0x100 + reg, &val);
+
+	return val & BIT(idx);
+}
+
+static struct pinctrl_ops ingenic_pctlops = {
+	.get_groups_count = pinctrl_generic_get_group_count,
+	.get_group_name = pinctrl_generic_get_group_name,
+	.get_group_pins = pinctrl_generic_get_group_pins,
+	.dt_node_to_map = pinconf_generic_dt_node_to_map_all,
+	.dt_free_map = pinconf_generic_dt_free_map,
+};
+
+static int ingenic_pinmux_set_pin_fn(struct ingenic_pinctrl *jzpc,
+		int pin, int func)
+{
+	unsigned int idx = pin % PINS_PER_GPIO_CHIP;
+	unsigned int offt = pin / PINS_PER_GPIO_CHIP;
+
+	dev_dbg(jzpc->dev, "set pin P%c%u to function %u\n",
+			'A' + offt, idx, func);
+
+	if (jzpc->version >= ID_JZ4770) {
+		ingenic_config_pin(jzpc, pin, JZ4770_GPIO_INT, false);
+		ingenic_config_pin(jzpc, pin, JZ4770_GPIO_MSK, false);
+		ingenic_config_pin(jzpc, pin, JZ4770_GPIO_PAT1, func & 0x2);
+		ingenic_config_pin(jzpc, pin, JZ4770_GPIO_PAT0, func & 0x1);
+	} else {
+		ingenic_config_pin(jzpc, pin, JZ4740_GPIO_FUNC, true);
+		ingenic_config_pin(jzpc, pin, JZ4740_GPIO_TRIG, func & 0x2);
+		ingenic_config_pin(jzpc, pin, JZ4740_GPIO_SELECT, func > 0);
+	}
+
+	return 0;
+}
+
+static int ingenic_pinmux_set_mux(struct pinctrl_dev *pctldev,
+		unsigned int selector, unsigned int group)
+{
+	struct ingenic_pinctrl *jzpc = pinctrl_dev_get_drvdata(pctldev);
+	struct function_desc *func;
+	struct group_desc *grp;
+	unsigned int i;
+
+	func = pinmux_generic_get_function(pctldev, selector);
+	if (!func)
+		return -EINVAL;
+
+	grp = pinctrl_generic_get_group(pctldev, group);
+	if (!grp)
+		return -EINVAL;
+
+	dev_dbg(pctldev->dev, "enable function %s group %s\n",
+		func->name, grp->name);
+
+	for (i = 0; i < grp->num_pins; i++) {
+		int *pin_modes = grp->data;
+
+		ingenic_pinmux_set_pin_fn(jzpc, grp->pins[i], pin_modes[i]);
+	}
+
+	return 0;
+}
+
+static int ingenic_pinmux_gpio_set_direction(struct pinctrl_dev *pctldev,
+		struct pinctrl_gpio_range *range,
+		unsigned int pin, bool input)
+{
+	struct ingenic_pinctrl *jzpc = pinctrl_dev_get_drvdata(pctldev);
+	unsigned int idx = pin % PINS_PER_GPIO_CHIP;
+	unsigned int offt = pin / PINS_PER_GPIO_CHIP;
+
+	dev_dbg(pctldev->dev, "set pin P%c%u to %sput\n",
+			'A' + offt, idx, input ? "in" : "out");
+
+	if (jzpc->version >= ID_JZ4770) {
+		ingenic_config_pin(jzpc, pin, JZ4770_GPIO_INT, false);
+		ingenic_config_pin(jzpc, pin, JZ4770_GPIO_MSK, true);
+		ingenic_config_pin(jzpc, pin, JZ4770_GPIO_PAT1, input);
+	} else {
+		ingenic_config_pin(jzpc, pin, JZ4740_GPIO_SELECT, false);
+		ingenic_config_pin(jzpc, pin, JZ4740_GPIO_DIR, input);
+		ingenic_config_pin(jzpc, pin, JZ4740_GPIO_FUNC, false);
+	}
+
+	return 0;
+}
+
+static struct pinmux_ops ingenic_pmxops = {
+	.get_functions_count = pinmux_generic_get_function_count,
+	.get_function_name = pinmux_generic_get_function_name,
+	.get_function_groups = pinmux_generic_get_function_groups,
+	.set_mux = ingenic_pinmux_set_mux,
+	.gpio_set_direction = ingenic_pinmux_gpio_set_direction,
+};
+
+static int ingenic_pinconf_get(struct pinctrl_dev *pctldev,
+		unsigned int pin, unsigned long *config)
+{
+	struct ingenic_pinctrl *jzpc = pinctrl_dev_get_drvdata(pctldev);
+	enum pin_config_param param = pinconf_to_config_param(*config);
+	unsigned int idx = pin % PINS_PER_GPIO_CHIP;
+	unsigned int offt = pin / PINS_PER_GPIO_CHIP;
+	bool pull;
+
+	if (jzpc->version >= ID_JZ4770)
+		pull = !ingenic_get_pin_config(jzpc, pin, JZ4770_GPIO_PEN);
+	else
+		pull = !ingenic_get_pin_config(jzpc, pin, JZ4740_GPIO_PULL_DIS);
+
+	switch (param) {
+	case PIN_CONFIG_BIAS_DISABLE:
+		if (pull)
+			return -EINVAL;
+		break;
+
+	case PIN_CONFIG_BIAS_PULL_UP:
+		if (!pull || !(jzpc->info->pull_ups[offt] & BIT(idx)))
+			return -EINVAL;
+		break;
+
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		if (!pull || !(jzpc->info->pull_downs[offt] & BIT(idx)))
+			return -EINVAL;
+		break;
+
+	default:
+		return -ENOTSUPP;
+	}
+
+	*config = pinconf_to_config_packed(param, 1);
+	return 0;
+}
+
+static void ingenic_set_bias(struct ingenic_pinctrl *jzpc,
+		unsigned int pin, bool enabled)
+{
+	if (jzpc->version >= ID_JZ4770)
+		ingenic_config_pin(jzpc, pin, JZ4770_GPIO_PEN, !enabled);
+	else
+		ingenic_config_pin(jzpc, pin, JZ4740_GPIO_PULL_DIS, !enabled);
+}
+
+static int ingenic_pinconf_set(struct pinctrl_dev *pctldev, unsigned int pin,
+		unsigned long *configs, unsigned int num_configs)
+{
+	struct ingenic_pinctrl *jzpc = pinctrl_dev_get_drvdata(pctldev);
+	unsigned int idx = pin % PINS_PER_GPIO_CHIP;
+	unsigned int offt = pin / PINS_PER_GPIO_CHIP;
+	unsigned int cfg;
+
+	for (cfg = 0; cfg < num_configs; cfg++) {
+		switch (pinconf_to_config_param(configs[cfg])) {
+		case PIN_CONFIG_BIAS_DISABLE:
+		case PIN_CONFIG_BIAS_PULL_UP:
+		case PIN_CONFIG_BIAS_PULL_DOWN:
+			continue;
+		default:
+			return -ENOTSUPP;
+		}
+	}
+
+	for (cfg = 0; cfg < num_configs; cfg++) {
+		switch (pinconf_to_config_param(configs[cfg])) {
+		case PIN_CONFIG_BIAS_DISABLE:
+			dev_dbg(jzpc->dev, "disable pull-over for pin P%c%u\n",
+					'A' + offt, idx);
+			ingenic_set_bias(jzpc, pin, false);
+			break;
+
+		case PIN_CONFIG_BIAS_PULL_UP:
+			if (!(jzpc->info->pull_ups[offt] & BIT(idx)))
+				return -EINVAL;
+			dev_dbg(jzpc->dev, "set pull-up for pin P%c%u\n",
+					'A' + offt, idx);
+			ingenic_set_bias(jzpc, pin, true);
+			break;
+
+		case PIN_CONFIG_BIAS_PULL_DOWN:
+			if (!(jzpc->info->pull_downs[offt] & BIT(idx)))
+				return -EINVAL;
+			dev_dbg(jzpc->dev, "set pull-down for pin P%c%u\n",
+					'A' + offt, idx);
+			ingenic_set_bias(jzpc, pin, true);
+			break;
+
+		default:
+			unreachable();
+		}
+	}
+
+	return 0;
+}
+
+static int ingenic_pinconf_group_get(struct pinctrl_dev *pctldev,
+		unsigned int group, unsigned long *config)
+{
+	const unsigned int *pins;
+	unsigned int i, npins, old = 0;
+	int ret;
+
+	ret = pinctrl_generic_get_group_pins(pctldev, group, &pins, &npins);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < npins; i++) {
+		if (ingenic_pinconf_get(pctldev, pins[i], config))
+			return -ENOTSUPP;
+
+		/* configs do not match between two pins */
+		if (i && (old != *config))
+			return -ENOTSUPP;
+
+		old = *config;
+	}
+
+	return 0;
+}
+
+static int ingenic_pinconf_group_set(struct pinctrl_dev *pctldev,
+		unsigned int group, unsigned long *configs,
+		unsigned int num_configs)
+{
+	const unsigned int *pins;
+	unsigned int i, npins;
+	int ret;
+
+	ret = pinctrl_generic_get_group_pins(pctldev, group, &pins, &npins);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < npins; i++) {
+		ret = ingenic_pinconf_set(pctldev,
+				pins[i], configs, num_configs);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static struct pinconf_ops ingenic_confops = {
+	.is_generic = true,
+	.pin_config_get = ingenic_pinconf_get,
+	.pin_config_set = ingenic_pinconf_set,
+	.pin_config_group_get = ingenic_pinconf_group_get,
+	.pin_config_group_set = ingenic_pinconf_group_set,
+};
+
+static const struct regmap_config ingenic_pinctrl_regmap_config = {
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_stride = 4,
+};
+
+static const struct of_device_id ingenic_pinctrl_of_match[] = {
+	{ .compatible = "ingenic,jz4740-pinctrl", .data = (void *) ID_JZ4740 },
+	{ .compatible = "ingenic,jz4770-pinctrl", .data = (void *) ID_JZ4770 },
+	{ .compatible = "ingenic,jz4780-pinctrl", .data = (void *) ID_JZ4780 },
+	{},
+};
+
+int ingenic_pinctrl_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct ingenic_pinctrl *jzpc;
+	struct pinctrl_desc *pctl_desc;
+	void __iomem *base;
+	const struct platform_device_id *id = platform_get_device_id(pdev);
+	const struct of_device_id *of_id = of_match_device(
+			ingenic_pinctrl_of_match, dev);
+	const struct ingenic_chip_info *chip_info;
+	unsigned int i;
+	int err;
+
+	jzpc = devm_kzalloc(dev, sizeof(*jzpc), GFP_KERNEL);
+	if (!jzpc)
+		return -ENOMEM;
+
+	base = devm_ioremap_resource(dev,
+			platform_get_resource(pdev, IORESOURCE_MEM, 0));
+	if (IS_ERR(base)) {
+		dev_err(dev, "Failed to ioremap registers\n");
+		return PTR_ERR(base);
+	}
+
+	jzpc->map = devm_regmap_init_mmio(dev, base,
+			&ingenic_pinctrl_regmap_config);
+	if (IS_ERR(jzpc->map)) {
+		dev_err(dev, "Failed to create regmap\n");
+		return PTR_ERR(jzpc->map);
+	}
+
+	jzpc->dev = dev;
+
+	if (of_id)
+		jzpc->version = (enum jz_version)of_id->data;
+	else
+		jzpc->version = (enum jz_version)id->driver_data;
+
+	if (jzpc->version >= ID_JZ4770)
+		chip_info = &jz4770_chip_info;
+	else
+		chip_info = &jz4740_chip_info;
+	jzpc->info = chip_info;
+
+	pctl_desc = devm_kzalloc(&pdev->dev, sizeof(*pctl_desc), GFP_KERNEL);
+	if (!pctl_desc)
+		return -ENOMEM;
+
+	/* fill in pinctrl_desc structure */
+	pctl_desc->name = dev_name(dev);
+	pctl_desc->owner = THIS_MODULE;
+	pctl_desc->pctlops = &ingenic_pctlops;
+	pctl_desc->pmxops = &ingenic_pmxops;
+	pctl_desc->confops = &ingenic_confops;
+	pctl_desc->npins = chip_info->num_chips * PINS_PER_GPIO_CHIP;
+	pctl_desc->pins = jzpc->pdesc = devm_kzalloc(&pdev->dev,
+			sizeof(*jzpc->pdesc) * pctl_desc->npins, GFP_KERNEL);
+	if (!jzpc->pdesc)
+		return -ENOMEM;
+
+	for (i = 0; i < pctl_desc->npins; i++) {
+		jzpc->pdesc[i].number = i;
+		jzpc->pdesc[i].name = kasprintf(GFP_KERNEL, "P%c%d",
+						'A' + (i / PINS_PER_GPIO_CHIP),
+						i % PINS_PER_GPIO_CHIP);
+	}
+
+	jzpc->pctl = devm_pinctrl_register(dev, pctl_desc, jzpc);
+	if (IS_ERR(jzpc->pctl)) {
+		dev_err(dev, "Failed to register pinctrl\n");
+		return PTR_ERR(jzpc->pctl);
+	}
+
+	for (i = 0; i < chip_info->num_groups; i++) {
+		const struct group_desc *group = &chip_info->groups[i];
+
+		err = pinctrl_generic_add_group(jzpc->pctl, group->name,
+				group->pins, group->num_pins, group->data);
+		if (err) {
+			dev_err(dev, "Failed to register group %s\n",
+					group->name);
+			return err;
+		}
+	}
+
+	for (i = 0; i < chip_info->num_functions; i++) {
+		const struct function_desc *func = &chip_info->functions[i];
+
+		err = pinmux_generic_add_function(jzpc->pctl, func->name,
+				func->group_names, func->num_group_names,
+				func->data);
+		if (err) {
+			dev_err(dev, "Failed to register function %s\n",
+					func->name);
+			return err;
+		}
+	}
+
+	dev_set_drvdata(dev, jzpc->map);
+
+	if (dev->of_node) {
+		err = of_platform_populate(dev->of_node, NULL, NULL, dev);
+		if (err) {
+			dev_err(dev, "Failed to probe GPIO devices\n");
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+static const struct platform_device_id ingenic_pinctrl_ids[] = {
+	{ "jz4740-pinctrl", ID_JZ4740 },
+	{ "jz4770-pinctrl", ID_JZ4770 },
+	{ "jz4780-pinctrl", ID_JZ4780 },
+	{},
+};
+
+static struct platform_driver ingenic_pinctrl_driver = {
+	.driver = {
+		.name = "pinctrl-ingenic",
+		.of_match_table = of_match_ptr(ingenic_pinctrl_of_match),
+		.suppress_bind_attrs = true,
+	},
+	.probe = ingenic_pinctrl_probe,
+	.id_table = ingenic_pinctrl_ids,
+};
+
+static int __init ingenic_pinctrl_drv_register(void)
+{
+	return platform_driver_register(&ingenic_pinctrl_driver);
+}
+postcore_initcall(ingenic_pinctrl_drv_register);
diff --git a/drivers/gpio/gpio-mcp23s08.c b/drivers/pinctrl/pinctrl-mcp23s08.c
index 2a57d024481d..3e40d4245512 100644
--- a/drivers/gpio/gpio-mcp23s08.c
+++ b/drivers/pinctrl/pinctrl-mcp23s08.c
@@ -1,14 +1,4 @@
-/*
- * MCP23S08 SPI/I2C GPIO gpio expander driver
- *
- * The inputs and outputs of the mcp23s08, mcp23s17, mcp23008 and mcp23017 are
- * supported.
- * For the I2C versions of the chips (mcp23008 and mcp23017) generation of
- * interrupts is also supported.
- * The hardware of the SPI versions of the chips (mcp23s08 and mcp23s17) is
- * also capable of generating interrupts, but the linux driver does not
- * support that yet.
- */
+/* MCP23S08 SPI/I2C GPIO driver */
 
 #include <linux/kernel.h>
 #include <linux/device.h>
@@ -21,11 +11,13 @@
 #include <linux/slab.h>
 #include <asm/byteorder.h>
 #include <linux/interrupt.h>
-#include <linux/of_irq.h>
 #include <linux/of_device.h>
 #include <linux/regmap.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinconf-generic.h>
 
-/**
+/*
  * MCP types supported by driver
  */
 #define MCP_TYPE_S08	0
@@ -34,6 +26,8 @@
 #define MCP_TYPE_017	3
 #define MCP_TYPE_S18    4
 
+#define MCP_MAX_DEV_PER_CS	8
+
 /* Registers are all 8 bits wide.
  *
  * The mcp23s17 has twice as many bits, and can be configured to work
@@ -64,19 +58,52 @@ struct mcp23s08 {
 	bool			irq_active_high;
 	bool			reg_shift;
 
-	u16			cache[11];
 	u16			irq_rise;
 	u16			irq_fall;
 	int			irq;
 	bool			irq_controller;
-	/* lock protects the cached values */
+	int			cached_gpio;
+	/* lock protects regmap access with bypass/cache flags */
 	struct mutex		lock;
-	struct mutex		irq_lock;
 
 	struct gpio_chip	chip;
 
 	struct regmap		*regmap;
 	struct device		*dev;
+
+	struct pinctrl_dev	*pctldev;
+	struct pinctrl_desc	pinctrl_desc;
+};
+
+static const struct reg_default mcp23x08_defaults[] = {
+	{.reg = MCP_IODIR,		.def = 0xff},
+	{.reg = MCP_IPOL,		.def = 0x00},
+	{.reg = MCP_GPINTEN,		.def = 0x00},
+	{.reg = MCP_DEFVAL,		.def = 0x00},
+	{.reg = MCP_INTCON,		.def = 0x00},
+	{.reg = MCP_IOCON,		.def = 0x00},
+	{.reg = MCP_GPPU,		.def = 0x00},
+	{.reg = MCP_OLAT,		.def = 0x00},
+};
+
+static const struct regmap_range mcp23x08_volatile_range = {
+	.range_min = MCP_INTF,
+	.range_max = MCP_GPIO,
+};
+
+static const struct regmap_access_table mcp23x08_volatile_table = {
+	.yes_ranges = &mcp23x08_volatile_range,
+	.n_yes_ranges = 1,
+};
+
+static const struct regmap_range mcp23x08_precious_range = {
+	.range_min = MCP_GPIO,
+	.range_max = MCP_GPIO,
+};
+
+static const struct regmap_access_table mcp23x08_precious_table = {
+	.yes_ranges = &mcp23x08_precious_range,
+	.n_yes_ranges = 1,
 };
 
 static const struct regmap_config mcp23x08_regmap = {
@@ -84,18 +111,203 @@ static const struct regmap_config mcp23x08_regmap = {
 	.val_bits = 8,
 
 	.reg_stride = 1,
+	.volatile_table = &mcp23x08_volatile_table,
+	.precious_table = &mcp23x08_precious_table,
+	.reg_defaults = mcp23x08_defaults,
+	.num_reg_defaults = ARRAY_SIZE(mcp23x08_defaults),
+	.cache_type = REGCACHE_FLAT,
 	.max_register = MCP_OLAT,
 };
 
+static const struct reg_default mcp23x16_defaults[] = {
+	{.reg = MCP_IODIR << 1,		.def = 0xffff},
+	{.reg = MCP_IPOL << 1,		.def = 0x0000},
+	{.reg = MCP_GPINTEN << 1,	.def = 0x0000},
+	{.reg = MCP_DEFVAL << 1,	.def = 0x0000},
+	{.reg = MCP_INTCON << 1,	.def = 0x0000},
+	{.reg = MCP_IOCON << 1,		.def = 0x0000},
+	{.reg = MCP_GPPU << 1,		.def = 0x0000},
+	{.reg = MCP_OLAT << 1,		.def = 0x0000},
+};
+
+static const struct regmap_range mcp23x16_volatile_range = {
+	.range_min = MCP_INTF << 1,
+	.range_max = MCP_GPIO << 1,
+};
+
+static const struct regmap_access_table mcp23x16_volatile_table = {
+	.yes_ranges = &mcp23x16_volatile_range,
+	.n_yes_ranges = 1,
+};
+
+static const struct regmap_range mcp23x16_precious_range = {
+	.range_min = MCP_GPIO << 1,
+	.range_max = MCP_GPIO << 1,
+};
+
+static const struct regmap_access_table mcp23x16_precious_table = {
+	.yes_ranges = &mcp23x16_precious_range,
+	.n_yes_ranges = 1,
+};
+
 static const struct regmap_config mcp23x17_regmap = {
 	.reg_bits = 8,
 	.val_bits = 16,
 
 	.reg_stride = 2,
 	.max_register = MCP_OLAT << 1,
+	.volatile_table = &mcp23x16_volatile_table,
+	.precious_table = &mcp23x16_precious_table,
+	.reg_defaults = mcp23x16_defaults,
+	.num_reg_defaults = ARRAY_SIZE(mcp23x16_defaults),
+	.cache_type = REGCACHE_FLAT,
 	.val_format_endian = REGMAP_ENDIAN_LITTLE,
 };
 
+static int mcp_read(struct mcp23s08 *mcp, unsigned int reg, unsigned int *val)
+{
+	return regmap_read(mcp->regmap, reg << mcp->reg_shift, val);
+}
+
+static int mcp_write(struct mcp23s08 *mcp, unsigned int reg, unsigned int val)
+{
+	return regmap_write(mcp->regmap, reg << mcp->reg_shift, val);
+}
+
+static int mcp_set_mask(struct mcp23s08 *mcp, unsigned int reg,
+		       unsigned int mask, bool enabled)
+{
+	u16 val  = enabled ? 0xffff : 0x0000;
+	return regmap_update_bits(mcp->regmap, reg << mcp->reg_shift,
+				  mask, val);
+}
+
+static int mcp_set_bit(struct mcp23s08 *mcp, unsigned int reg,
+		       unsigned int pin, bool enabled)
+{
+	u16 mask = BIT(pin);
+	return mcp_set_mask(mcp, reg, mask, enabled);
+}
+
+static const struct pinctrl_pin_desc mcp23x08_pins[] = {
+	PINCTRL_PIN(0, "gpio0"),
+	PINCTRL_PIN(1, "gpio1"),
+	PINCTRL_PIN(2, "gpio2"),
+	PINCTRL_PIN(3, "gpio3"),
+	PINCTRL_PIN(4, "gpio4"),
+	PINCTRL_PIN(5, "gpio5"),
+	PINCTRL_PIN(6, "gpio6"),
+	PINCTRL_PIN(7, "gpio7"),
+};
+
+static const struct pinctrl_pin_desc mcp23x17_pins[] = {
+	PINCTRL_PIN(0, "gpio0"),
+	PINCTRL_PIN(1, "gpio1"),
+	PINCTRL_PIN(2, "gpio2"),
+	PINCTRL_PIN(3, "gpio3"),
+	PINCTRL_PIN(4, "gpio4"),
+	PINCTRL_PIN(5, "gpio5"),
+	PINCTRL_PIN(6, "gpio6"),
+	PINCTRL_PIN(7, "gpio7"),
+	PINCTRL_PIN(8, "gpio8"),
+	PINCTRL_PIN(9, "gpio9"),
+	PINCTRL_PIN(10, "gpio10"),
+	PINCTRL_PIN(11, "gpio11"),
+	PINCTRL_PIN(12, "gpio12"),
+	PINCTRL_PIN(13, "gpio13"),
+	PINCTRL_PIN(14, "gpio14"),
+	PINCTRL_PIN(15, "gpio15"),
+};
+
+static int mcp_pinctrl_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	return 0;
+}
+
+static const char *mcp_pinctrl_get_group_name(struct pinctrl_dev *pctldev,
+						unsigned int group)
+{
+	return NULL;
+}
+
+static int mcp_pinctrl_get_group_pins(struct pinctrl_dev *pctldev,
+					unsigned int group,
+					const unsigned int **pins,
+					unsigned int *num_pins)
+{
+	return -ENOTSUPP;
+}
+
+static const struct pinctrl_ops mcp_pinctrl_ops = {
+	.get_groups_count = mcp_pinctrl_get_groups_count,
+	.get_group_name = mcp_pinctrl_get_group_name,
+	.get_group_pins = mcp_pinctrl_get_group_pins,
+#ifdef CONFIG_OF
+	.dt_node_to_map = pinconf_generic_dt_node_to_map_pin,
+	.dt_free_map = pinconf_generic_dt_free_map,
+#endif
+};
+
+static int mcp_pinconf_get(struct pinctrl_dev *pctldev, unsigned int pin,
+			      unsigned long *config)
+{
+	struct mcp23s08 *mcp = pinctrl_dev_get_drvdata(pctldev);
+	enum pin_config_param param = pinconf_to_config_param(*config);
+	unsigned int data, status;
+	int ret;
+
+	switch (param) {
+	case PIN_CONFIG_BIAS_PULL_UP:
+		ret = mcp_read(mcp, MCP_GPPU, &data);
+		if (ret < 0)
+			return ret;
+		status = (data & BIT(pin)) ? 1 : 0;
+		break;
+	default:
+		dev_err(mcp->dev, "Invalid config param %04x\n", param);
+		return -ENOTSUPP;
+	}
+
+	*config = 0;
+
+	return status ? 0 : -EINVAL;
+}
+
+static int mcp_pinconf_set(struct pinctrl_dev *pctldev, unsigned int pin,
+			      unsigned long *configs, unsigned int num_configs)
+{
+	struct mcp23s08 *mcp = pinctrl_dev_get_drvdata(pctldev);
+	enum pin_config_param param;
+	u32 arg, mask;
+	u16 val;
+	int ret = 0;
+	int i;
+
+	for (i = 0; i < num_configs; i++) {
+		param = pinconf_to_config_param(configs[i]);
+		arg = pinconf_to_config_argument(configs[i]);
+
+		switch (param) {
+		case PIN_CONFIG_BIAS_PULL_UP:
+			val = arg ? 0xFFFF : 0x0000;
+			mask = BIT(pin);
+			ret = mcp_set_bit(mcp, MCP_GPPU, pin, arg);
+			break;
+		default:
+			dev_err(mcp->dev, "Invalid config param %04x\n", param);
+			return -ENOTSUPP;
+		}
+	}
+
+	return ret;
+}
+
+static const struct pinconf_ops mcp_pinconf_ops = {
+	.pin_config_get = mcp_pinconf_get,
+	.pin_config_set = mcp_pinconf_set,
+	.is_generic = true,
+};
+
 /*----------------------------------------------------------------------*/
 
 #ifdef CONFIG_SPI_MASTER
@@ -158,30 +370,6 @@ static const struct regmap_bus mcp23sxx_spi_regmap = {
 
 #endif /* CONFIG_SPI_MASTER */
 
-static int mcp_read(struct mcp23s08 *mcp, unsigned int reg, unsigned int *val)
-{
-	return regmap_read(mcp->regmap, reg << mcp->reg_shift, val);
-}
-
-static int mcp_write(struct mcp23s08 *mcp, unsigned int reg, unsigned int val)
-{
-	return regmap_write(mcp->regmap, reg << mcp->reg_shift, val);
-}
-
-static int mcp_update_cache(struct mcp23s08 *mcp)
-{
-	int ret, reg, i;
-
-	for (i = 0; i < ARRAY_SIZE(mcp->cache); i++) {
-		ret = mcp_read(mcp, i, &reg);
-		if (ret < 0)
-			return ret;
-		mcp->cache[i] = reg;
-	}
-
-	return 0;
-}
-
 /*----------------------------------------------------------------------*/
 
 /* A given spi_device can represent up to eight mcp23sxx chips
@@ -202,9 +390,9 @@ static int mcp23s08_direction_input(struct gpio_chip *chip, unsigned offset)
 	int status;
 
 	mutex_lock(&mcp->lock);
-	mcp->cache[MCP_IODIR] |= (1 << offset);
-	status = mcp_write(mcp, MCP_IODIR, mcp->cache[MCP_IODIR]);
+	status = mcp_set_bit(mcp, MCP_IODIR, offset, true);
 	mutex_unlock(&mcp->lock);
+
 	return status;
 }
 
@@ -219,33 +407,27 @@ static int mcp23s08_get(struct gpio_chip *chip, unsigned offset)
 	ret = mcp_read(mcp, MCP_GPIO, &status);
 	if (ret < 0)
 		status = 0;
-	else {
-		mcp->cache[MCP_GPIO] = status;
+	else
 		status = !!(status & (1 << offset));
-	}
+
+	mcp->cached_gpio = status;
+
 	mutex_unlock(&mcp->lock);
 	return status;
 }
 
-static int __mcp23s08_set(struct mcp23s08 *mcp, unsigned mask, int value)
+static int __mcp23s08_set(struct mcp23s08 *mcp, unsigned mask, bool value)
 {
-	unsigned olat = mcp->cache[MCP_OLAT];
-
-	if (value)
-		olat |= mask;
-	else
-		olat &= ~mask;
-	mcp->cache[MCP_OLAT] = olat;
-	return mcp_write(mcp, MCP_OLAT, olat);
+	return mcp_set_mask(mcp, MCP_OLAT, mask, value);
 }
 
 static void mcp23s08_set(struct gpio_chip *chip, unsigned offset, int value)
 {
 	struct mcp23s08	*mcp = gpiochip_get_data(chip);
-	unsigned mask = 1 << offset;
+	unsigned mask = BIT(offset);
 
 	mutex_lock(&mcp->lock);
-	__mcp23s08_set(mcp, mask, value);
+	__mcp23s08_set(mcp, mask, !!value);
 	mutex_unlock(&mcp->lock);
 }
 
@@ -253,14 +435,13 @@ static int
 mcp23s08_direction_output(struct gpio_chip *chip, unsigned offset, int value)
 {
 	struct mcp23s08	*mcp = gpiochip_get_data(chip);
-	unsigned mask = 1 << offset;
+	unsigned mask = BIT(offset);
 	int status;
 
 	mutex_lock(&mcp->lock);
 	status = __mcp23s08_set(mcp, mask, value);
 	if (status == 0) {
-		mcp->cache[MCP_IODIR] &= ~mask;
-		status = mcp_write(mcp, MCP_IODIR, mcp->cache[MCP_IODIR]);
+		status = mcp_set_mask(mcp, MCP_IODIR, mask, false);
 	}
 	mutex_unlock(&mcp->lock);
 	return status;
@@ -270,7 +451,7 @@ mcp23s08_direction_output(struct gpio_chip *chip, unsigned offset, int value)
 static irqreturn_t mcp23s08_irq(int irq, void *data)
 {
 	struct mcp23s08 *mcp = data;
-	int intcap, intf, i, gpio, gpio_orig, intcap_mask;
+	int intcap, intcon, intf, i, gpio, gpio_orig, intcap_mask, defval;
 	unsigned int child_irq;
 	bool intf_set, intcap_changed, gpio_bit_changed,
 		defval_changed, gpio_set;
@@ -281,25 +462,31 @@ static irqreturn_t mcp23s08_irq(int irq, void *data)
 		return IRQ_HANDLED;
 	}
 
-	mcp->cache[MCP_INTF] = intf;
-
 	if (mcp_read(mcp, MCP_INTCAP, &intcap) < 0) {
 		mutex_unlock(&mcp->lock);
 		return IRQ_HANDLED;
 	}
 
-	mcp->cache[MCP_INTCAP] = intcap;
+	if (mcp_read(mcp, MCP_INTCON, &intcon) < 0) {
+		mutex_unlock(&mcp->lock);
+		return IRQ_HANDLED;
+	}
+
+	if (mcp_read(mcp, MCP_DEFVAL, &defval) < 0) {
+		mutex_unlock(&mcp->lock);
+		return IRQ_HANDLED;
+	}
 
 	/* This clears the interrupt(configurable on S18) */
 	if (mcp_read(mcp, MCP_GPIO, &gpio) < 0) {
 		mutex_unlock(&mcp->lock);
 		return IRQ_HANDLED;
 	}
-	gpio_orig = mcp->cache[MCP_GPIO];
-	mcp->cache[MCP_GPIO] = gpio;
+	gpio_orig = mcp->cached_gpio;
+	mcp->cached_gpio = gpio;
 	mutex_unlock(&mcp->lock);
 
-	if (mcp->cache[MCP_INTF] == 0) {
+	if (intf == 0) {
 		/* There is no interrupt pending */
 		return IRQ_HANDLED;
 	}
@@ -327,7 +514,7 @@ static irqreturn_t mcp23s08_irq(int irq, void *data)
 		 * to see if the input has changed.
 		 */
 
-		intf_set = BIT(i) & mcp->cache[MCP_INTF];
+		intf_set = intf & BIT(i);
 		if (i < 8 && intf_set)
 			intcap_mask = 0x00FF;
 		else if (i >= 8 && intf_set)
@@ -336,14 +523,14 @@ static irqreturn_t mcp23s08_irq(int irq, void *data)
 			intcap_mask = 0x00;
 
 		intcap_changed = (intcap_mask &
-			(BIT(i) & mcp->cache[MCP_INTCAP])) !=
+			(intcap & BIT(i))) !=
 			(intcap_mask & (BIT(i) & gpio_orig));
-		gpio_set = BIT(i) & mcp->cache[MCP_GPIO];
+		gpio_set = BIT(i) & gpio;
 		gpio_bit_changed = (BIT(i) & gpio_orig) !=
-			(BIT(i) & mcp->cache[MCP_GPIO]);
-		defval_changed = (BIT(i) & mcp->cache[MCP_INTCON]) &&
-			((BIT(i) & mcp->cache[MCP_GPIO]) !=
-			(BIT(i) & mcp->cache[MCP_DEFVAL]));
+			(BIT(i) & gpio);
+		defval_changed = (BIT(i) & intcon) &&
+			((BIT(i) & gpio) !=
+			(BIT(i) & defval));
 
 		if (((gpio_bit_changed || intcap_changed) &&
 			(BIT(i) & mcp->irq_rise) && gpio_set) ||
@@ -364,7 +551,7 @@ static void mcp23s08_irq_mask(struct irq_data *data)
 	struct mcp23s08 *mcp = gpiochip_get_data(gc);
 	unsigned int pos = data->hwirq;
 
-	mcp->cache[MCP_GPINTEN] &= ~BIT(pos);
+	mcp_set_bit(mcp, MCP_GPINTEN, pos, false);
 }
 
 static void mcp23s08_irq_unmask(struct irq_data *data)
@@ -373,7 +560,7 @@ static void mcp23s08_irq_unmask(struct irq_data *data)
 	struct mcp23s08 *mcp = gpiochip_get_data(gc);
 	unsigned int pos = data->hwirq;
 
-	mcp->cache[MCP_GPINTEN] |= BIT(pos);
+	mcp_set_bit(mcp, MCP_GPINTEN, pos, true);
 }
 
 static int mcp23s08_irq_set_type(struct irq_data *data, unsigned int type)
@@ -384,23 +571,23 @@ static int mcp23s08_irq_set_type(struct irq_data *data, unsigned int type)
 	int status = 0;
 
 	if ((type & IRQ_TYPE_EDGE_BOTH) == IRQ_TYPE_EDGE_BOTH) {
-		mcp->cache[MCP_INTCON] &= ~BIT(pos);
+		mcp_set_bit(mcp, MCP_INTCON, pos, false);
 		mcp->irq_rise |= BIT(pos);
 		mcp->irq_fall |= BIT(pos);
 	} else if (type & IRQ_TYPE_EDGE_RISING) {
-		mcp->cache[MCP_INTCON] &= ~BIT(pos);
+		mcp_set_bit(mcp, MCP_INTCON, pos, false);
 		mcp->irq_rise |= BIT(pos);
 		mcp->irq_fall &= ~BIT(pos);
 	} else if (type & IRQ_TYPE_EDGE_FALLING) {
-		mcp->cache[MCP_INTCON] &= ~BIT(pos);
+		mcp_set_bit(mcp, MCP_INTCON, pos, false);
 		mcp->irq_rise &= ~BIT(pos);
 		mcp->irq_fall |= BIT(pos);
 	} else if (type & IRQ_TYPE_LEVEL_HIGH) {
-		mcp->cache[MCP_INTCON] |= BIT(pos);
-		mcp->cache[MCP_DEFVAL] &= ~BIT(pos);
+		mcp_set_bit(mcp, MCP_INTCON, pos, true);
+		mcp_set_bit(mcp, MCP_DEFVAL, pos, false);
 	} else if (type & IRQ_TYPE_LEVEL_LOW) {
-		mcp->cache[MCP_INTCON] |= BIT(pos);
-		mcp->cache[MCP_DEFVAL] |= BIT(pos);
+		mcp_set_bit(mcp, MCP_INTCON, pos, true);
+		mcp_set_bit(mcp, MCP_DEFVAL, pos, true);
 	} else
 		return -EINVAL;
 
@@ -412,7 +599,8 @@ static void mcp23s08_irq_bus_lock(struct irq_data *data)
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(data);
 	struct mcp23s08 *mcp = gpiochip_get_data(gc);
 
-	mutex_lock(&mcp->irq_lock);
+	mutex_lock(&mcp->lock);
+	regcache_cache_only(mcp->regmap, true);
 }
 
 static void mcp23s08_irq_bus_unlock(struct irq_data *data)
@@ -420,12 +608,10 @@ static void mcp23s08_irq_bus_unlock(struct irq_data *data)
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(data);
 	struct mcp23s08 *mcp = gpiochip_get_data(gc);
 
-	mutex_lock(&mcp->lock);
-	mcp_write(mcp, MCP_GPINTEN, mcp->cache[MCP_GPINTEN]);
-	mcp_write(mcp, MCP_DEFVAL, mcp->cache[MCP_DEFVAL]);
-	mcp_write(mcp, MCP_INTCON, mcp->cache[MCP_INTCON]);
+	regcache_cache_only(mcp->regmap, false);
+	regcache_sync(mcp->regmap);
+
 	mutex_unlock(&mcp->lock);
-	mutex_unlock(&mcp->irq_lock);
 }
 
 static struct irq_chip mcp23s08_irq_chip = {
@@ -443,8 +629,6 @@ static int mcp23s08_irq_setup(struct mcp23s08 *mcp)
 	int err;
 	unsigned long irqflags = IRQF_ONESHOT | IRQF_SHARED;
 
-	mutex_init(&mcp->irq_lock);
-
 	if (mcp->irq_active_high)
 		irqflags |= IRQF_TRIGGER_HIGH;
 	else
@@ -484,6 +668,47 @@ static int mcp23s08_irq_setup(struct mcp23s08 *mcp)
 #include <linux/seq_file.h>
 
 /*
+ * This compares the chip's registers with the register
+ * cache and corrects any incorrectly set register. This
+ * can be used to fix state for MCP23xxx, that temporary
+ * lost its power supply.
+ */
+#define MCP23S08_CONFIG_REGS 8
+static int __check_mcp23s08_reg_cache(struct mcp23s08 *mcp)
+{
+	int cached[MCP23S08_CONFIG_REGS];
+	int err = 0, i;
+
+	/* read cached config registers */
+	for (i = 0; i < MCP23S08_CONFIG_REGS; i++) {
+		err = mcp_read(mcp, i, &cached[i]);
+		if (err)
+			goto out;
+	}
+
+	regcache_cache_bypass(mcp->regmap, true);
+
+	for (i = 0; i < MCP23S08_CONFIG_REGS; i++) {
+		int uncached;
+		err = mcp_read(mcp, i, &uncached);
+		if (err)
+			goto out;
+
+		if (uncached != cached[i]) {
+			dev_err(mcp->dev, "restoring reg 0x%02x from 0x%04x to 0x%04x (power-loss?)\n",
+				i, uncached, cached[i]);
+			mcp_write(mcp, i, cached[i]);
+		}
+	}
+
+out:
+	if (err)
+		dev_err(mcp->dev, "read error: reg=%02x, err=%d", i, err);
+	regcache_cache_bypass(mcp->regmap, false);
+	return err;
+}
+
+/*
  * This shows more info than the generic gpio dump code:
  * pullups, deglitching, open drain drive.
  */
@@ -493,6 +718,7 @@ static void mcp23s08_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 	char		bank;
 	int		t;
 	unsigned	mask;
+	int iodir, gpio, gppu;
 
 	mcp = gpiochip_get_data(chip);
 
@@ -500,14 +726,30 @@ static void mcp23s08_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 	bank = '0' + ((mcp->addr >> 1) & 0x7);
 
 	mutex_lock(&mcp->lock);
-	t = mcp_update_cache(mcp);
-	if (t < 0) {
-		seq_printf(s, " I/O ERROR %d\n", t);
+
+	t = __check_mcp23s08_reg_cache(mcp);
+	if (t) {
+		seq_printf(s, " I/O Error\n");
+		goto done;
+	}
+	t = mcp_read(mcp, MCP_IODIR, &iodir);
+	if (t) {
+		seq_printf(s, " I/O Error\n");
+		goto done;
+	}
+	t = mcp_read(mcp, MCP_GPIO, &gpio);
+	if (t) {
+		seq_printf(s, " I/O Error\n");
+		goto done;
+	}
+	t = mcp_read(mcp, MCP_GPPU, &gppu);
+	if (t) {
+		seq_printf(s, " I/O Error\n");
 		goto done;
 	}
 
-	for (t = 0, mask = 1; t < chip->ngpio; t++, mask <<= 1) {
-		const char	*label;
+	for (t = 0, mask = BIT(0); t < chip->ngpio; t++, mask <<= 1) {
+		const char *label;
 
 		label = gpiochip_is_requested(chip, t);
 		if (!label)
@@ -515,9 +757,9 @@ static void mcp23s08_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 
 		seq_printf(s, " gpio-%-3d P%c.%d (%-12s) %s %s %s",
 			chip->base + t, bank, t, label,
-			(mcp->cache[MCP_IODIR] & mask) ? "in " : "out",
-			(mcp->cache[MCP_GPIO] & mask) ? "hi" : "lo",
-			(mcp->cache[MCP_GPPU] & mask) ? "up" : "  ");
+			(iodir & mask) ? "in " : "out",
+			(gpio & mask) ? "hi" : "lo",
+			(gppu & mask) ? "up" : "  ");
 		/* NOTE:  ignoring the irq-related registers */
 		seq_puts(s, "\n");
 	}
@@ -533,7 +775,7 @@ done:
 
 static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 			      void *data, unsigned addr, unsigned type,
-			      struct mcp23s08_platform_data *pdata, int cs)
+			      unsigned int base, int cs)
 {
 	int status, ret;
 	bool mirror = false;
@@ -605,7 +847,7 @@ static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 	if (IS_ERR(mcp->regmap))
 		return PTR_ERR(mcp->regmap);
 
-	mcp->chip.base = pdata->base;
+	mcp->chip.base = base;
 	mcp->chip.can_sleep = true;
 	mcp->chip.parent = dev;
 	mcp->chip.owner = THIS_MODULE;
@@ -618,13 +860,14 @@ static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 	if (ret < 0)
 		goto fail;
 
-	mcp->irq_controller = pdata->irq_controller;
+	mcp->irq_controller =
+		device_property_read_bool(dev, "interrupt-controller");
 	if (mcp->irq && mcp->irq_controller) {
 		mcp->irq_active_high =
-			of_property_read_bool(mcp->chip.parent->of_node,
+			device_property_read_bool(dev,
 					      "microchip,irq-active-high");
 
-		mirror = pdata->mirror;
+		mirror = device_property_read_bool(dev, "microchip,irq-mirror");
 	}
 
 	if ((status & IOCON_SEQOP) || !(status & IOCON_HAEN) || mirror ||
@@ -648,32 +891,7 @@ static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 			goto fail;
 	}
 
-	/* configure ~100K pullups */
-	ret = mcp_write(mcp, MCP_GPPU, pdata->chip[cs].pullups);
-	if (ret < 0)
-		goto fail;
-
-	ret = mcp_update_cache(mcp);
-	if (ret < 0)
-		goto fail;
-
-	/* disable inverter on input */
-	if (mcp->cache[MCP_IPOL] != 0) {
-		mcp->cache[MCP_IPOL] = 0;
-		ret = mcp_write(mcp, MCP_IPOL, 0);
-		if (ret < 0)
-			goto fail;
-	}
-
-	/* disable irqs */
-	if (mcp->cache[MCP_GPINTEN] != 0) {
-		mcp->cache[MCP_GPINTEN] = 0;
-		ret = mcp_write(mcp, MCP_GPINTEN, 0);
-		if (ret < 0)
-			goto fail;
-	}
-
-	ret = gpiochip_add_data(&mcp->chip, mcp);
+	ret = devm_gpiochip_add_data(dev, &mcp->chip, mcp);
 	if (ret < 0)
 		goto fail;
 
@@ -682,6 +900,23 @@ static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 		if (ret)
 			goto fail;
 	}
+
+	mcp->pinctrl_desc.name = "mcp23xxx-pinctrl";
+	mcp->pinctrl_desc.pctlops = &mcp_pinctrl_ops;
+	mcp->pinctrl_desc.confops = &mcp_pinconf_ops;
+	mcp->pinctrl_desc.npins = mcp->chip.ngpio;
+	if (mcp->pinctrl_desc.npins == 8)
+		mcp->pinctrl_desc.pins = mcp23x08_pins;
+	else if (mcp->pinctrl_desc.npins == 16)
+		mcp->pinctrl_desc.pins = mcp23x17_pins;
+	mcp->pinctrl_desc.owner = THIS_MODULE;
+
+	mcp->pctldev = devm_pinctrl_register(dev, &mcp->pinctrl_desc, mcp);
+	if (IS_ERR(mcp->pctldev)) {
+		ret = PTR_ERR(mcp->pctldev);
+		goto fail;
+	}
+
 fail:
 	if (ret < 0)
 		dev_dbg(dev, "can't setup chip %d, --> %d\n", addr, ret);
@@ -753,60 +988,26 @@ static int mcp230xx_probe(struct i2c_client *client,
 	struct mcp23s08_platform_data *pdata, local_pdata;
 	struct mcp23s08 *mcp;
 	int status;
-	const struct of_device_id *match;
 
-	match = of_match_device(of_match_ptr(mcp23s08_i2c_of_match),
-					&client->dev);
-	if (match) {
+	pdata = dev_get_platdata(&client->dev);
+	if (!pdata) {
 		pdata = &local_pdata;
 		pdata->base = -1;
-		pdata->chip[0].pullups = 0;
-		pdata->irq_controller =	of_property_read_bool(
-					client->dev.of_node,
-					"interrupt-controller");
-		pdata->mirror = of_property_read_bool(client->dev.of_node,
-						      "microchip,irq-mirror");
-		client->irq = irq_of_parse_and_map(client->dev.of_node, 0);
-	} else {
-		pdata = dev_get_platdata(&client->dev);
-		if (!pdata) {
-			pdata = devm_kzalloc(&client->dev,
-					sizeof(struct mcp23s08_platform_data),
-					GFP_KERNEL);
-			if (!pdata)
-				return -ENOMEM;
-			pdata->base = -1;
-		}
 	}
 
-	mcp = kzalloc(sizeof(*mcp), GFP_KERNEL);
+	mcp = devm_kzalloc(&client->dev, sizeof(*mcp), GFP_KERNEL);
 	if (!mcp)
 		return -ENOMEM;
 
 	mcp->irq = client->irq;
 	status = mcp23s08_probe_one(mcp, &client->dev, client, client->addr,
-				    id->driver_data, pdata, 0);
+				    id->driver_data, pdata->base, 0);
 	if (status)
-		goto fail;
+		return status;
 
 	i2c_set_clientdata(client, mcp);
 
 	return 0;
-
-fail:
-	kfree(mcp);
-
-	return status;
-}
-
-static int mcp230xx_remove(struct i2c_client *client)
-{
-	struct mcp23s08 *mcp = i2c_get_clientdata(client);
-
-	gpiochip_remove(&mcp->chip);
-	kfree(mcp);
-
-	return 0;
 }
 
 static const struct i2c_device_id mcp230xx_id[] = {
@@ -822,7 +1023,6 @@ static struct i2c_driver mcp230xx_driver = {
 		.of_match_table = of_match_ptr(mcp23s08_i2c_of_match),
 	},
 	.probe		= mcp230xx_probe,
-	.remove		= mcp230xx_remove,
 	.id_table	= mcp230xx_id,
 };
 
@@ -856,60 +1056,40 @@ static int mcp23s08_probe(struct spi_device *spi)
 	int				status, type;
 	unsigned			ngpio = 0;
 	const struct			of_device_id *match;
-	u32				spi_present_mask = 0;
 
 	match = of_match_device(of_match_ptr(mcp23s08_spi_of_match), &spi->dev);
-	if (match) {
+	if (match)
 		type = (int)(uintptr_t)match->data;
-		status = of_property_read_u32(spi->dev.of_node,
-			    "microchip,spi-present-mask", &spi_present_mask);
+	else
+		type = spi_get_device_id(spi)->driver_data;
+
+	pdata = dev_get_platdata(&spi->dev);
+	if (!pdata) {
+		pdata = &local_pdata;
+		pdata->base = -1;
+
+		status = device_property_read_u32(&spi->dev,
+			"microchip,spi-present-mask", &pdata->spi_present_mask);
 		if (status) {
-			status = of_property_read_u32(spi->dev.of_node,
-				    "mcp,spi-present-mask", &spi_present_mask);
+			status = device_property_read_u32(&spi->dev,
+				"mcp,spi-present-mask",
+				&pdata->spi_present_mask);
+
 			if (status) {
-				dev_err(&spi->dev,
-					"DT has no spi-present-mask\n");
+				dev_err(&spi->dev, "missing spi-present-mask");
 				return -ENODEV;
 			}
 		}
-		if ((spi_present_mask <= 0) || (spi_present_mask >= 256)) {
-			dev_err(&spi->dev, "invalid spi-present-mask\n");
-			return -ENODEV;
-		}
+	}
 
-		pdata = &local_pdata;
-		pdata->base = -1;
-		for (addr = 0; addr < ARRAY_SIZE(pdata->chip); addr++) {
-			pdata->chip[addr].pullups = 0;
-			if (spi_present_mask & (1 << addr))
-				chips++;
-		}
-		pdata->irq_controller =	of_property_read_bool(
-					spi->dev.of_node,
-					"interrupt-controller");
-		pdata->mirror = of_property_read_bool(spi->dev.of_node,
-						      "microchip,irq-mirror");
-	} else {
-		type = spi_get_device_id(spi)->driver_data;
-		pdata = dev_get_platdata(&spi->dev);
-		if (!pdata) {
-			pdata = devm_kzalloc(&spi->dev,
-					sizeof(struct mcp23s08_platform_data),
-					GFP_KERNEL);
-			pdata->base = -1;
-		}
+	if (!pdata->spi_present_mask || pdata->spi_present_mask > 0xff) {
+		dev_err(&spi->dev, "invalid spi-present-mask");
+		return -ENODEV;
+	}
 
-		for (addr = 0; addr < ARRAY_SIZE(pdata->chip); addr++) {
-			if (!pdata->chip[addr].is_present)
-				continue;
+	for (addr = 0; addr < MCP_MAX_DEV_PER_CS; addr++) {
+		if (pdata->spi_present_mask & BIT(addr))
 			chips++;
-			if ((type == MCP_TYPE_S08) && (addr > 3)) {
-				dev_err(&spi->dev,
-					"mcp23s08 only supports address 0..3\n");
-				return -EINVAL;
-			}
-			spi_present_mask |= 1 << addr;
-		}
 	}
 
 	if (!chips)
@@ -923,19 +1103,17 @@ static int mcp23s08_probe(struct spi_device *spi)
 
 	spi_set_drvdata(spi, data);
 
-	spi->irq = irq_of_parse_and_map(spi->dev.of_node, 0);
-
-	for (addr = 0; addr < ARRAY_SIZE(pdata->chip); addr++) {
-		if (!(spi_present_mask & (1 << addr)))
+	for (addr = 0; addr < MCP_MAX_DEV_PER_CS; addr++) {
+		if (!(pdata->spi_present_mask & BIT(addr)))
 			continue;
 		chips--;
 		data->mcp[addr] = &data->chip[chips];
 		data->mcp[addr]->irq = spi->irq;
 		status = mcp23s08_probe_one(data->mcp[addr], &spi->dev, spi,
-					    0x40 | (addr << 1), type, pdata,
-					    addr);
+					    0x40 | (addr << 1), type,
+					    pdata->base, addr);
 		if (status < 0)
-			goto fail;
+			return status;
 
 		if (pdata->base != -1)
 			pdata->base += data->mcp[addr]->chip.ngpio;
@@ -943,36 +1121,6 @@ static int mcp23s08_probe(struct spi_device *spi)
 	}
 	data->ngpio = ngpio;
 
-	/* NOTE:  these chips have a relatively sane IRQ framework, with
-	 * per-signal masking and level/edge triggering.  It's not yet
-	 * handled here...
-	 */
-
-	return 0;
-
-fail:
-	for (addr = 0; addr < ARRAY_SIZE(data->mcp); addr++) {
-
-		if (!data->mcp[addr])
-			continue;
-		gpiochip_remove(&data->mcp[addr]->chip);
-	}
-	return status;
-}
-
-static int mcp23s08_remove(struct spi_device *spi)
-{
-	struct mcp23s08_driver_data	*data = spi_get_drvdata(spi);
-	unsigned			addr;
-
-	for (addr = 0; addr < ARRAY_SIZE(data->mcp); addr++) {
-
-		if (!data->mcp[addr])
-			continue;
-
-		gpiochip_remove(&data->mcp[addr]->chip);
-	}
-
 	return 0;
 }
 
@@ -986,7 +1134,6 @@ MODULE_DEVICE_TABLE(spi, mcp23s08_ids);
 
 static struct spi_driver mcp23s08_driver = {
 	.probe		= mcp23s08_probe,
-	.remove		= mcp23s08_remove,
 	.id_table	= mcp23s08_ids,
 	.driver = {
 		.name	= "mcp23s08",
diff --git a/drivers/pinctrl/pinctrl-rockchip.c b/drivers/pinctrl/pinctrl-rockchip.c
index f141aa0430b1..e831647c56a6 100644
--- a/drivers/pinctrl/pinctrl-rockchip.c
+++ b/drivers/pinctrl/pinctrl-rockchip.c
@@ -143,9 +143,7 @@ struct rockchip_drv {
  * @gpio_chip: gpiolib chip
  * @grange: gpio range
  * @slock: spinlock for the gpio bank
- * @irq_lock: bus lock for irq chip
- * @new_irqs: newly configured irqs which must be muxed as GPIOs in
- *	irq_bus_sync_unlock()
+ * @route_mask: bits describing the routing pins of per bank
  */
 struct rockchip_pin_bank {
 	void __iomem			*reg_base;
@@ -168,8 +166,7 @@ struct rockchip_pin_bank {
 	struct pinctrl_gpio_range	grange;
 	raw_spinlock_t			slock;
 	u32				toggle_edge_mode;
-	struct mutex			irq_lock;
-	u32				new_irqs;
+	u32				route_mask;
 };
 
 #define PIN_BANK(id, pins, label)			\
@@ -293,6 +290,22 @@ struct rockchip_pin_bank {
 	}
 
 /**
+ * struct rockchip_mux_recalced_data: represent a pin iomux data.
+ * @bank_num: bank number.
+ * @pin: index at register or used to calc index.
+ * @func: the min pin.
+ * @route_offset: the max pin.
+ * @route_val: the register offset.
+ */
+struct rockchip_mux_route_data {
+	u8 bank_num;
+	u8 pin;
+	u8 func;
+	u32 route_offset;
+	u32 route_val;
+};
+
+/**
  */
 struct rockchip_pin_ctrl {
 	struct rockchip_pin_bank	*pin_banks;
@@ -304,6 +317,8 @@ struct rockchip_pin_ctrl {
 	int				pmu_mux_offset;
 	int				grf_drv_offset;
 	int				pmu_drv_offset;
+	struct rockchip_mux_route_data *iomux_routes;
+	u32				niomux_routes;
 
 	void	(*pull_calc_reg)(struct rockchip_pin_bank *bank,
 				    int pin_num, struct regmap **regmap,
@@ -585,6 +600,280 @@ static void rk3328_recalc_mux(u8 bank_num, int pin, int *reg,
 	*bit = data->bit;
 }
 
+static struct rockchip_mux_route_data rk3228_mux_route_data[] = {
+	{
+		/* pwm0-0 */
+		.bank_num = 0,
+		.pin = 26,
+		.func = 1,
+		.route_offset = 0x50,
+		.route_val = BIT(16),
+	}, {
+		/* pwm0-1 */
+		.bank_num = 3,
+		.pin = 21,
+		.func = 1,
+		.route_offset = 0x50,
+		.route_val = BIT(16) | BIT(0),
+	}, {
+		/* pwm1-0 */
+		.bank_num = 0,
+		.pin = 27,
+		.func = 1,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 1),
+	}, {
+		/* pwm1-1 */
+		.bank_num = 0,
+		.pin = 30,
+		.func = 2,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 1) | BIT(1),
+	}, {
+		/* pwm2-0 */
+		.bank_num = 0,
+		.pin = 28,
+		.func = 1,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 2),
+	}, {
+		/* pwm2-1 */
+		.bank_num = 1,
+		.pin = 12,
+		.func = 2,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 2) | BIT(2),
+	}, {
+		/* pwm3-0 */
+		.bank_num = 3,
+		.pin = 26,
+		.func = 1,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 3),
+	}, {
+		/* pwm3-1 */
+		.bank_num = 1,
+		.pin = 11,
+		.func = 2,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 3) | BIT(3),
+	}, {
+		/* sdio-0_d0 */
+		.bank_num = 1,
+		.pin = 1,
+		.func = 1,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 4),
+	}, {
+		/* sdio-1_d0 */
+		.bank_num = 3,
+		.pin = 2,
+		.func = 1,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 4) | BIT(4),
+	}, {
+		/* spi-0_rx */
+		.bank_num = 0,
+		.pin = 13,
+		.func = 2,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 5),
+	}, {
+		/* spi-1_rx */
+		.bank_num = 2,
+		.pin = 0,
+		.func = 2,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 5) | BIT(5),
+	}, {
+		/* emmc-0_cmd */
+		.bank_num = 1,
+		.pin = 22,
+		.func = 2,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 7),
+	}, {
+		/* emmc-1_cmd */
+		.bank_num = 2,
+		.pin = 4,
+		.func = 2,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 7) | BIT(7),
+	}, {
+		/* uart2-0_rx */
+		.bank_num = 1,
+		.pin = 19,
+		.func = 2,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 8),
+	}, {
+		/* uart2-1_rx */
+		.bank_num = 1,
+		.pin = 10,
+		.func = 2,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 8) | BIT(8),
+	}, {
+		/* uart1-0_rx */
+		.bank_num = 1,
+		.pin = 10,
+		.func = 1,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 11),
+	}, {
+		/* uart1-1_rx */
+		.bank_num = 3,
+		.pin = 13,
+		.func = 1,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 11) | BIT(11),
+	},
+};
+
+static struct rockchip_mux_route_data rk3328_mux_route_data[] = {
+	{
+		/* uart2dbg_rxm0 */
+		.bank_num = 1,
+		.pin = 1,
+		.func = 2,
+		.route_offset = 0x50,
+		.route_val = BIT(16) | BIT(16 + 1),
+	}, {
+		/* uart2dbg_rxm1 */
+		.bank_num = 2,
+		.pin = 1,
+		.func = 1,
+		.route_offset = 0x50,
+		.route_val = BIT(16) | BIT(16 + 1) | BIT(0),
+	}, {
+		/* gmac-m1-optimized_rxd0 */
+		.bank_num = 1,
+		.pin = 11,
+		.func = 2,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 2) | BIT(16 + 10) | BIT(2) | BIT(10),
+	}, {
+		/* pdm_sdi0m0 */
+		.bank_num = 2,
+		.pin = 19,
+		.func = 2,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 3),
+	}, {
+		/* pdm_sdi0m1 */
+		.bank_num = 1,
+		.pin = 23,
+		.func = 3,
+		.route_offset = 0x50,
+		.route_val =  BIT(16 + 3) | BIT(3),
+	}, {
+		/* spi_rxdm2 */
+		.bank_num = 3,
+		.pin = 2,
+		.func = 4,
+		.route_offset = 0x50,
+		.route_val =  BIT(16 + 4) | BIT(16 + 5) | BIT(5),
+	}, {
+		/* i2s2_sdim0 */
+		.bank_num = 1,
+		.pin = 24,
+		.func = 1,
+		.route_offset = 0x50,
+		.route_val = BIT(16 + 6),
+	}, {
+		/* i2s2_sdim1 */
+		.bank_num = 3,
+		.pin = 2,
+		.func = 6,
+		.route_offset = 0x50,
+		.route_val =  BIT(16 + 6) | BIT(6),
+	}, {
+		/* card_iom1 */
+		.bank_num = 2,
+		.pin = 22,
+		.func = 3,
+		.route_offset = 0x50,
+		.route_val =  BIT(16 + 7) | BIT(7),
+	}, {
+		/* tsp_d5m1 */
+		.bank_num = 2,
+		.pin = 16,
+		.func = 3,
+		.route_offset = 0x50,
+		.route_val =  BIT(16 + 8) | BIT(8),
+	}, {
+		/* cif_data5m1 */
+		.bank_num = 2,
+		.pin = 16,
+		.func = 4,
+		.route_offset = 0x50,
+		.route_val =  BIT(16 + 9) | BIT(9),
+	},
+};
+
+static struct rockchip_mux_route_data rk3399_mux_route_data[] = {
+	{
+		/* uart2dbga_rx */
+		.bank_num = 4,
+		.pin = 8,
+		.func = 2,
+		.route_offset = 0xe21c,
+		.route_val = BIT(16 + 10) | BIT(16 + 11),
+	}, {
+		/* uart2dbgb_rx */
+		.bank_num = 4,
+		.pin = 16,
+		.func = 2,
+		.route_offset = 0xe21c,
+		.route_val = BIT(16 + 10) | BIT(16 + 11) | BIT(10),
+	}, {
+		/* uart2dbgc_rx */
+		.bank_num = 4,
+		.pin = 19,
+		.func = 1,
+		.route_offset = 0xe21c,
+		.route_val = BIT(16 + 10) | BIT(16 + 11) | BIT(11),
+	}, {
+		/* pcie_clkreqn */
+		.bank_num = 2,
+		.pin = 26,
+		.func = 2,
+		.route_offset = 0xe21c,
+		.route_val = BIT(16 + 14),
+	}, {
+		/* pcie_clkreqnb */
+		.bank_num = 4,
+		.pin = 24,
+		.func = 1,
+		.route_offset = 0xe21c,
+		.route_val = BIT(16 + 14) | BIT(14),
+	},
+};
+
+static bool rockchip_get_mux_route(struct rockchip_pin_bank *bank, int pin,
+				   int mux, u32 *reg, u32 *value)
+{
+	struct rockchip_pinctrl *info = bank->drvdata;
+	struct rockchip_pin_ctrl *ctrl = info->ctrl;
+	struct rockchip_mux_route_data *data;
+	int i;
+
+	for (i = 0; i < ctrl->niomux_routes; i++) {
+		data = &ctrl->iomux_routes[i];
+		if ((data->bank_num == bank->bank_num) &&
+		    (data->pin == pin) && (data->func == mux))
+			break;
+	}
+
+	if (i >= ctrl->niomux_routes)
+		return false;
+
+	*reg = data->route_offset;
+	*value = data->route_val;
+
+	return true;
+}
+
 static int rockchip_get_mux(struct rockchip_pin_bank *bank, int pin)
 {
 	struct rockchip_pinctrl *info = bank->drvdata;
@@ -683,7 +972,7 @@ static int rockchip_set_mux(struct rockchip_pin_bank *bank, int pin, int mux)
 	struct regmap *regmap;
 	int reg, ret, mask, mux_type;
 	u8 bit;
-	u32 data, rmask;
+	u32 data, rmask, route_reg, route_val;
 
 	ret = rockchip_verify_mux(bank, pin, mux);
 	if (ret < 0)
@@ -719,6 +1008,15 @@ static int rockchip_set_mux(struct rockchip_pin_bank *bank, int pin, int mux)
 	if (ctrl->iomux_recalc && (mux_type & IOMUX_RECALCED))
 		ctrl->iomux_recalc(bank->bank_num, pin, &reg, &bit, &mask);
 
+	if (bank->route_mask & BIT(pin)) {
+		if (rockchip_get_mux_route(bank, pin, mux, &route_reg,
+					   &route_val)) {
+			ret = regmap_write(regmap, route_reg, route_val);
+			if (ret)
+				return ret;
+		}
+	}
+
 	data = (mask << (bit + 16));
 	rmask = data | (data >> 16);
 	data |= (mux & mask) << bit;
@@ -2134,12 +2432,11 @@ static int rockchip_irq_set_type(struct irq_data *d, unsigned int type)
 	int ret;
 
 	/* make sure the pin is configured as gpio input */
-	ret = rockchip_verify_mux(bank, d->hwirq, RK_FUNC_GPIO);
+	ret = rockchip_set_mux(bank, d->hwirq, RK_FUNC_GPIO);
 	if (ret < 0)
 		return ret;
 
-	bank->new_irqs |= mask;
-
+	clk_enable(bank->clk);
 	raw_spin_lock_irqsave(&bank->slock, flags);
 
 	data = readl_relaxed(bank->reg_base + GPIO_SWPORT_DDR);
@@ -2197,6 +2494,7 @@ static int rockchip_irq_set_type(struct irq_data *d, unsigned int type)
 	default:
 		irq_gc_unlock(gc);
 		raw_spin_unlock_irqrestore(&bank->slock, flags);
+		clk_disable(bank->clk);
 		return -EINVAL;
 	}
 
@@ -2205,6 +2503,7 @@ static int rockchip_irq_set_type(struct irq_data *d, unsigned int type)
 
 	irq_gc_unlock(gc);
 	raw_spin_unlock_irqrestore(&bank->slock, flags);
+	clk_disable(bank->clk);
 
 	return 0;
 }
@@ -2248,34 +2547,6 @@ static void rockchip_irq_disable(struct irq_data *d)
 	clk_disable(bank->clk);
 }
 
-static void rockchip_irq_bus_lock(struct irq_data *d)
-{
-	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
-	struct rockchip_pin_bank *bank = gc->private;
-
-	clk_enable(bank->clk);
-	mutex_lock(&bank->irq_lock);
-}
-
-static void rockchip_irq_bus_sync_unlock(struct irq_data *d)
-{
-	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
-	struct rockchip_pin_bank *bank = gc->private;
-
-	while (bank->new_irqs) {
-		unsigned int irq = __ffs(bank->new_irqs);
-		int ret;
-
-		ret = rockchip_set_mux(bank, irq, RK_FUNC_GPIO);
-		WARN_ON(ret < 0);
-
-		bank->new_irqs &= ~BIT(irq);
-	}
-
-	mutex_unlock(&bank->irq_lock);
-	clk_disable(bank->clk);
-}
-
 static int rockchip_interrupts_register(struct platform_device *pdev,
 						struct rockchip_pinctrl *info)
 {
@@ -2342,9 +2613,6 @@ static int rockchip_interrupts_register(struct platform_device *pdev,
 		gc->chip_types[0].chip.irq_suspend = rockchip_irq_suspend;
 		gc->chip_types[0].chip.irq_resume = rockchip_irq_resume;
 		gc->chip_types[0].chip.irq_set_type = rockchip_irq_set_type;
-		gc->chip_types[0].chip.irq_bus_lock = rockchip_irq_bus_lock;
-		gc->chip_types[0].chip.irq_bus_sync_unlock =
-						rockchip_irq_bus_sync_unlock;
 		gc->wake_enabled = IRQ_MSK(bank->nr_pins);
 
 		irq_set_chained_handler_and_data(bank->irq,
@@ -2518,7 +2786,6 @@ static struct rockchip_pin_ctrl *rockchip_pinctrl_get_soc_data(
 		int bank_pins = 0;
 
 		raw_spin_lock_init(&bank->slock);
-		mutex_init(&bank->irq_lock);
 		bank->drvdata = d;
 		bank->pin_base = ctrl->nr_pins;
 		ctrl->nr_pins += bank->nr_pins;
@@ -2585,6 +2852,16 @@ static struct rockchip_pin_ctrl *rockchip_pinctrl_get_soc_data(
 
 			bank_pins += 8;
 		}
+
+		/* calculate the per-bank route_mask */
+		for (j = 0; j < ctrl->niomux_routes; j++) {
+			int pin = 0;
+
+			if (ctrl->iomux_routes[j].bank_num == bank->bank_num) {
+				pin = ctrl->iomux_routes[j].pin;
+				bank->route_mask |= BIT(pin);
+			}
+		}
 	}
 
 	return ctrl;
@@ -2835,6 +3112,8 @@ static struct rockchip_pin_ctrl rk3228_pin_ctrl = {
 		.label			= "RK3228-GPIO",
 		.type			= RK3288,
 		.grf_mux_offset		= 0x0,
+		.iomux_routes		= rk3228_mux_route_data,
+		.niomux_routes		= ARRAY_SIZE(rk3228_mux_route_data),
 		.pull_calc_reg		= rk3228_calc_pull_reg_and_bit,
 		.drv_calc_reg		= rk3228_calc_drv_reg_and_bit,
 };
@@ -2902,6 +3181,8 @@ static struct rockchip_pin_ctrl rk3328_pin_ctrl = {
 		.label			= "RK3328-GPIO",
 		.type			= RK3288,
 		.grf_mux_offset		= 0x0,
+		.iomux_routes		= rk3328_mux_route_data,
+		.niomux_routes		= ARRAY_SIZE(rk3328_mux_route_data),
 		.pull_calc_reg		= rk3228_calc_pull_reg_and_bit,
 		.drv_calc_reg		= rk3228_calc_drv_reg_and_bit,
 		.iomux_recalc		= rk3328_recalc_mux,
@@ -2992,33 +3273,35 @@ static struct rockchip_pin_ctrl rk3399_pin_ctrl = {
 		.pmu_mux_offset		= 0x0,
 		.grf_drv_offset		= 0xe100,
 		.pmu_drv_offset		= 0x80,
+		.iomux_routes		= rk3399_mux_route_data,
+		.niomux_routes		= ARRAY_SIZE(rk3399_mux_route_data),
 		.pull_calc_reg		= rk3399_calc_pull_reg_and_bit,
 		.drv_calc_reg		= rk3399_calc_drv_reg_and_bit,
 };
 
 static const struct of_device_id rockchip_pinctrl_dt_match[] = {
 	{ .compatible = "rockchip,rv1108-pinctrl",
-		.data = (void *)&rv1108_pin_ctrl },
+		.data = &rv1108_pin_ctrl },
 	{ .compatible = "rockchip,rk2928-pinctrl",
-		.data = (void *)&rk2928_pin_ctrl },
+		.data = &rk2928_pin_ctrl },
 	{ .compatible = "rockchip,rk3036-pinctrl",
-		.data = (void *)&rk3036_pin_ctrl },
+		.data = &rk3036_pin_ctrl },
 	{ .compatible = "rockchip,rk3066a-pinctrl",
-		.data = (void *)&rk3066a_pin_ctrl },
+		.data = &rk3066a_pin_ctrl },
 	{ .compatible = "rockchip,rk3066b-pinctrl",
-		.data = (void *)&rk3066b_pin_ctrl },
+		.data = &rk3066b_pin_ctrl },
 	{ .compatible = "rockchip,rk3188-pinctrl",
-		.data = (void *)&rk3188_pin_ctrl },
+		.data = &rk3188_pin_ctrl },
 	{ .compatible = "rockchip,rk3228-pinctrl",
-		.data = (void *)&rk3228_pin_ctrl },
+		.data = &rk3228_pin_ctrl },
 	{ .compatible = "rockchip,rk3288-pinctrl",
-		.data = (void *)&rk3288_pin_ctrl },
+		.data = &rk3288_pin_ctrl },
 	{ .compatible = "rockchip,rk3328-pinctrl",
-		.data = (void *)&rk3328_pin_ctrl },
+		.data = &rk3328_pin_ctrl },
 	{ .compatible = "rockchip,rk3368-pinctrl",
-		.data = (void *)&rk3368_pin_ctrl },
+		.data = &rk3368_pin_ctrl },
 	{ .compatible = "rockchip,rk3399-pinctrl",
-		.data = (void *)&rk3399_pin_ctrl },
+		.data = &rk3399_pin_ctrl },
 	{},
 };
 
diff --git a/drivers/pinctrl/pinctrl-rza1.c b/drivers/pinctrl/pinctrl-rza1.c
new file mode 100644
index 000000000000..dc164da10446
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-rza1.c
@@ -0,0 +1,1308 @@
+/*
+ * Combined GPIO and pin controller support for Renesas RZ/A1 (r7s72100) SoC
+ *
+ * Copyright (C) 2017 Jacopo Mondi
+ *
+ * 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.
+ */
+
+/*
+ * This pin controller/gpio combined driver supports Renesas devices of RZ/A1
+ * family.
+ * This includes SoCs which are sub- or super- sets of this particular line,
+ * as RZ/A1H (r7s721000), RZ/A1M (r7s721010) and RZ/A1L (r7s721020).
+ */
+
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <linux/gpio/driver.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinconf-generic.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/slab.h>
+
+#include "core.h"
+#include "devicetree.h"
+#include "pinconf.h"
+#include "pinmux.h"
+
+#define DRIVER_NAME			"pinctrl-rza1"
+
+#define RZA1_P_REG			0x0000
+#define RZA1_PPR_REG			0x0200
+#define RZA1_PM_REG			0x0300
+#define RZA1_PMC_REG			0x0400
+#define RZA1_PFC_REG			0x0500
+#define RZA1_PFCE_REG			0x0600
+#define RZA1_PFCEA_REG			0x0a00
+#define RZA1_PIBC_REG			0x4000
+#define RZA1_PBDC_REG			0x4100
+#define RZA1_PIPC_REG			0x4200
+
+#define RZA1_ADDR(mem, reg, port)	((mem) + (reg) + ((port) * 4))
+
+#define RZA1_NPORTS			12
+#define RZA1_PINS_PER_PORT		16
+#define RZA1_NPINS			(RZA1_PINS_PER_PORT * RZA1_NPORTS)
+#define RZA1_PIN_ID_TO_PORT(id)		((id) / RZA1_PINS_PER_PORT)
+#define RZA1_PIN_ID_TO_PIN(id)		((id) % RZA1_PINS_PER_PORT)
+
+/*
+ * Use 16 lower bits [15:0] for pin identifier
+ * Use 16 higher bits [31:16] for pin mux function
+ */
+#define MUX_PIN_ID_MASK			GENMASK(15, 0)
+#define MUX_FUNC_MASK			GENMASK(31, 16)
+
+#define MUX_FUNC_OFFS			16
+#define MUX_FUNC(pinconf)		\
+	((pinconf & MUX_FUNC_MASK) >> MUX_FUNC_OFFS)
+#define MUX_FUNC_PFC_MASK		BIT(0)
+#define MUX_FUNC_PFCE_MASK		BIT(1)
+#define MUX_FUNC_PFCEA_MASK		BIT(2)
+
+/* Pin mux flags */
+#define MUX_FLAGS_BIDIR			BIT(0)
+#define MUX_FLAGS_SWIO_INPUT		BIT(1)
+#define MUX_FLAGS_SWIO_OUTPUT		BIT(2)
+
+/* ----------------------------------------------------------------------------
+ * RZ/A1 pinmux flags
+ */
+
+/**
+ * rza1_bidir_pin - describe a single pin that needs bidir flag applied.
+ */
+struct rza1_bidir_pin {
+	u8 pin: 4;
+	u8 func: 4;
+};
+
+/**
+ * rza1_bidir_entry - describe a list of pins that needs bidir flag applied.
+ *		      Each struct rza1_bidir_entry describes a port.
+ */
+struct rza1_bidir_entry {
+	const unsigned int npins;
+	const struct rza1_bidir_pin *pins;
+};
+
+/**
+ * rza1_swio_pin - describe a single pin that needs bidir flag applied.
+ */
+struct rza1_swio_pin {
+	u16 pin: 4;
+	u16 port: 4;
+	u16 func: 4;
+	u16 input: 1;
+};
+
+/**
+ * rza1_swio_entry - describe a list of pins that needs swio flag applied
+ */
+struct rza1_swio_entry {
+	const unsigned int npins;
+	const struct rza1_swio_pin *pins;
+};
+
+/**
+ * rza1_pinmux_conf - group together bidir and swio pinmux flag tables
+ */
+struct rza1_pinmux_conf {
+	const struct rza1_bidir_entry *bidir_entries;
+	const struct rza1_swio_entry *swio_entries;
+};
+
+/* ----------------------------------------------------------------------------
+ * RZ/A1H (r7s72100) pinmux flags
+ */
+
+static const struct rza1_bidir_pin rza1h_bidir_pins_p1[] = {
+	{ .pin = 0, .func = 1 },
+	{ .pin = 1, .func = 1 },
+	{ .pin = 2, .func = 1 },
+	{ .pin = 3, .func = 1 },
+	{ .pin = 4, .func = 1 },
+	{ .pin = 5, .func = 1 },
+	{ .pin = 6, .func = 1 },
+	{ .pin = 7, .func = 1 },
+};
+
+static const struct rza1_bidir_pin rza1h_bidir_pins_p2[] = {
+	{ .pin = 0, .func = 1 },
+	{ .pin = 1, .func = 1 },
+	{ .pin = 2, .func = 1 },
+	{ .pin = 3, .func = 1 },
+	{ .pin = 4, .func = 1 },
+	{ .pin = 0, .func = 4 },
+	{ .pin = 1, .func = 4 },
+	{ .pin = 2, .func = 4 },
+	{ .pin = 3, .func = 4 },
+	{ .pin = 5, .func = 1 },
+	{ .pin = 6, .func = 1 },
+	{ .pin = 7, .func = 1 },
+	{ .pin = 8, .func = 1 },
+	{ .pin = 9, .func = 1 },
+	{ .pin = 10, .func = 1 },
+	{ .pin = 11, .func = 1 },
+	{ .pin = 12, .func = 1 },
+	{ .pin = 13, .func = 1 },
+	{ .pin = 14, .func = 1 },
+	{ .pin = 15, .func = 1 },
+	{ .pin = 12, .func = 4 },
+	{ .pin = 13, .func = 4 },
+	{ .pin = 14, .func = 4 },
+	{ .pin = 15, .func = 4 },
+};
+
+static const struct rza1_bidir_pin rza1h_bidir_pins_p3[] = {
+	{ .pin = 3, .func = 2 },
+	{ .pin = 10, .func = 7 },
+	{ .pin = 11, .func = 7 },
+	{ .pin = 13, .func = 7 },
+	{ .pin = 14, .func = 7 },
+	{ .pin = 15, .func = 7 },
+	{ .pin = 10, .func = 8 },
+	{ .pin = 11, .func = 8 },
+	{ .pin = 13, .func = 8 },
+	{ .pin = 14, .func = 8 },
+	{ .pin = 15, .func = 8 },
+};
+
+static const struct rza1_bidir_pin rza1h_bidir_pins_p4[] = {
+	{ .pin = 0, .func = 8 },
+	{ .pin = 1, .func = 8 },
+	{ .pin = 2, .func = 8 },
+	{ .pin = 3, .func = 8 },
+	{ .pin = 10, .func = 3 },
+	{ .pin = 11, .func = 3 },
+	{ .pin = 13, .func = 3 },
+	{ .pin = 14, .func = 3 },
+	{ .pin = 15, .func = 3 },
+	{ .pin = 10, .func = 4 },
+	{ .pin = 11, .func = 4 },
+	{ .pin = 13, .func = 4 },
+	{ .pin = 14, .func = 4 },
+	{ .pin = 15, .func = 4 },
+	{ .pin = 12, .func = 5 },
+	{ .pin = 13, .func = 5 },
+	{ .pin = 14, .func = 5 },
+	{ .pin = 15, .func = 5 },
+};
+
+static const struct rza1_bidir_pin rza1h_bidir_pins_p6[] = {
+	{ .pin = 0, .func = 1 },
+	{ .pin = 1, .func = 1 },
+	{ .pin = 2, .func = 1 },
+	{ .pin = 3, .func = 1 },
+	{ .pin = 4, .func = 1 },
+	{ .pin = 5, .func = 1 },
+	{ .pin = 6, .func = 1 },
+	{ .pin = 7, .func = 1 },
+	{ .pin = 8, .func = 1 },
+	{ .pin = 9, .func = 1 },
+	{ .pin = 10, .func = 1 },
+	{ .pin = 11, .func = 1 },
+	{ .pin = 12, .func = 1 },
+	{ .pin = 13, .func = 1 },
+	{ .pin = 14, .func = 1 },
+	{ .pin = 15, .func = 1 },
+};
+
+static const struct rza1_bidir_pin rza1h_bidir_pins_p7[] = {
+	{ .pin = 13, .func = 3 },
+};
+
+static const struct rza1_bidir_pin rza1h_bidir_pins_p8[] = {
+	{ .pin = 8, .func = 3 },
+	{ .pin = 9, .func = 3 },
+	{ .pin = 10, .func = 3 },
+	{ .pin = 11, .func = 3 },
+	{ .pin = 14, .func = 2 },
+	{ .pin = 15, .func = 2 },
+	{ .pin = 14, .func = 3 },
+	{ .pin = 15, .func = 3 },
+};
+
+static const struct rza1_bidir_pin rza1h_bidir_pins_p9[] = {
+	{ .pin = 0, .func = 2 },
+	{ .pin = 1, .func = 2 },
+	{ .pin = 4, .func = 2 },
+	{ .pin = 5, .func = 2 },
+	{ .pin = 6, .func = 2 },
+	{ .pin = 7, .func = 2 },
+};
+
+static const struct rza1_bidir_pin rza1h_bidir_pins_p11[] = {
+	{ .pin = 6, .func = 2 },
+	{ .pin = 7, .func = 2 },
+	{ .pin = 9, .func = 2 },
+	{ .pin = 6, .func = 4 },
+	{ .pin = 7, .func = 4 },
+	{ .pin = 9, .func = 4 },
+	{ .pin = 10, .func = 2 },
+	{ .pin = 11, .func = 2 },
+	{ .pin = 10, .func = 4 },
+	{ .pin = 11, .func = 4 },
+	{ .pin = 12, .func = 4 },
+	{ .pin = 13, .func = 4 },
+	{ .pin = 14, .func = 4 },
+	{ .pin = 15, .func = 4 },
+};
+
+static const struct rza1_swio_pin rza1h_swio_pins[] = {
+	{ .port = 2, .pin = 7, .func = 4, .input = 0 },
+	{ .port = 2, .pin = 11, .func = 4, .input = 0 },
+	{ .port = 3, .pin = 7, .func = 3, .input = 0 },
+	{ .port = 3, .pin = 7, .func = 8, .input = 0 },
+	{ .port = 4, .pin = 7, .func = 5, .input = 0 },
+	{ .port = 4, .pin = 7, .func = 11, .input = 0 },
+	{ .port = 4, .pin = 15, .func = 6, .input = 0 },
+	{ .port = 5, .pin = 0, .func = 1, .input = 1 },
+	{ .port = 5, .pin = 1, .func = 1, .input = 1 },
+	{ .port = 5, .pin = 2, .func = 1, .input = 1 },
+	{ .port = 5, .pin = 3, .func = 1, .input = 1 },
+	{ .port = 5, .pin = 4, .func = 1, .input = 1 },
+	{ .port = 5, .pin = 5, .func = 1, .input = 1 },
+	{ .port = 5, .pin = 6, .func = 1, .input = 1 },
+	{ .port = 5, .pin = 7, .func = 1, .input = 1 },
+	{ .port = 7, .pin = 4, .func = 6, .input = 0 },
+	{ .port = 7, .pin = 11, .func = 2, .input = 0 },
+	{ .port = 8, .pin = 10, .func = 8, .input = 0 },
+	{ .port = 10, .pin = 15, .func = 2, .input = 0 },
+};
+
+static const struct rza1_bidir_entry rza1h_bidir_entries[RZA1_NPORTS] = {
+	[1] = { ARRAY_SIZE(rza1h_bidir_pins_p1), rza1h_bidir_pins_p1 },
+	[2] = { ARRAY_SIZE(rza1h_bidir_pins_p2), rza1h_bidir_pins_p2 },
+	[3] = { ARRAY_SIZE(rza1h_bidir_pins_p3), rza1h_bidir_pins_p3 },
+	[4] = { ARRAY_SIZE(rza1h_bidir_pins_p4), rza1h_bidir_pins_p4 },
+	[6] = { ARRAY_SIZE(rza1h_bidir_pins_p6), rza1h_bidir_pins_p6 },
+	[7] = { ARRAY_SIZE(rza1h_bidir_pins_p7), rza1h_bidir_pins_p7 },
+	[8] = { ARRAY_SIZE(rza1h_bidir_pins_p8), rza1h_bidir_pins_p8 },
+	[9] = { ARRAY_SIZE(rza1h_bidir_pins_p9), rza1h_bidir_pins_p9 },
+	[11] = { ARRAY_SIZE(rza1h_bidir_pins_p11), rza1h_bidir_pins_p11 },
+};
+
+static const struct rza1_swio_entry rza1h_swio_entries[] = {
+	[0] = { ARRAY_SIZE(rza1h_swio_pins), rza1h_swio_pins },
+};
+
+/* RZ/A1H (r7s72100x) pinmux flags table */
+static const struct rza1_pinmux_conf rza1h_pmx_conf = {
+	.bidir_entries	= rza1h_bidir_entries,
+	.swio_entries	= rza1h_swio_entries,
+};
+
+/* ----------------------------------------------------------------------------
+ * RZ/A1 types
+ */
+/**
+ * rza1_mux_conf - describes a pin multiplexing operation
+ *
+ * @id: the pin identifier from 0 to RZA1_NPINS
+ * @port: the port where pin sits on
+ * @pin: pin id
+ * @mux_func: alternate function id number
+ * @mux_flags: alternate function flags
+ * @value: output value to set the pin to
+ */
+struct rza1_mux_conf {
+	u16 id;
+	u8 port;
+	u8 pin;
+	u8 mux_func;
+	u8 mux_flags;
+	u8 value;
+};
+
+/**
+ * rza1_port - describes a pin port
+ *
+ * This is mostly useful to lock register writes per-bank and not globally.
+ *
+ * @lock: protect access to HW registers
+ * @id: port number
+ * @base: logical address base
+ * @pins: pins sitting on this port
+ */
+struct rza1_port {
+	spinlock_t lock;
+	unsigned int id;
+	void __iomem *base;
+	struct pinctrl_pin_desc *pins;
+};
+
+/**
+ * rza1_pinctrl - RZ pincontroller device
+ *
+ * @dev: parent device structure
+ * @mutex: protect [pinctrl|pinmux]_generic functions
+ * @base: logical address base
+ * @nports: number of pin controller ports
+ * @ports: pin controller banks
+ * @pins: pin array for pinctrl core
+ * @desc: pincontroller desc for pinctrl core
+ * @pctl: pinctrl device
+ * @data: device specific data
+ */
+struct rza1_pinctrl {
+	struct device *dev;
+
+	struct mutex mutex;
+
+	void __iomem *base;
+
+	unsigned int nport;
+	struct rza1_port *ports;
+
+	struct pinctrl_pin_desc *pins;
+	struct pinctrl_desc desc;
+	struct pinctrl_dev *pctl;
+
+	const void *data;
+};
+
+/* ----------------------------------------------------------------------------
+ * RZ/A1 pinmux flags
+ */
+static inline bool rza1_pinmux_get_bidir(unsigned int port,
+					 unsigned int pin,
+					 unsigned int func,
+					 const struct rza1_bidir_entry *table)
+{
+	const struct rza1_bidir_entry *entry = &table[port];
+	const struct rza1_bidir_pin *bidir_pin;
+	unsigned int i;
+
+	for (i = 0; i < entry->npins; ++i) {
+		bidir_pin = &entry->pins[i];
+		if (bidir_pin->pin == pin && bidir_pin->func == func)
+			return true;
+	}
+
+	return false;
+}
+
+static inline int rza1_pinmux_get_swio(unsigned int port,
+				       unsigned int pin,
+				       unsigned int func,
+				       const struct rza1_swio_entry *table)
+{
+	const struct rza1_swio_pin *swio_pin;
+	unsigned int i;
+
+
+	for (i = 0; i < table->npins; ++i) {
+		swio_pin = &table->pins[i];
+		if (swio_pin->port == port && swio_pin->pin == pin &&
+		    swio_pin->func == func)
+			return swio_pin->input;
+	}
+
+	return -ENOENT;
+}
+
+/**
+ * rza1_pinmux_get_flags() - return pinmux flags associated to a pin
+ */
+static unsigned int rza1_pinmux_get_flags(unsigned int port, unsigned int pin,
+					  unsigned int func,
+					  struct rza1_pinctrl *rza1_pctl)
+
+{
+	const struct rza1_pinmux_conf *pmx_conf = rza1_pctl->data;
+	const struct rza1_bidir_entry *bidir_entries = pmx_conf->bidir_entries;
+	const struct rza1_swio_entry *swio_entries = pmx_conf->swio_entries;
+	unsigned int pmx_flags = 0;
+	int ret;
+
+	if (rza1_pinmux_get_bidir(port, pin, func, bidir_entries))
+		pmx_flags |= MUX_FLAGS_BIDIR;
+
+	ret = rza1_pinmux_get_swio(port, pin, func, swio_entries);
+	if (ret == 0)
+		pmx_flags |= MUX_FLAGS_SWIO_OUTPUT;
+	else if (ret > 0)
+		pmx_flags |= MUX_FLAGS_SWIO_INPUT;
+
+	return pmx_flags;
+}
+
+/* ----------------------------------------------------------------------------
+ * RZ/A1 SoC operations
+ */
+
+/**
+ * rza1_set_bit() - un-locked set/clear a single bit in pin configuration
+ *		    registers
+ */
+static inline void rza1_set_bit(struct rza1_port *port, unsigned int reg,
+				unsigned int bit, bool set)
+{
+	void __iomem *mem = RZA1_ADDR(port->base, reg, port->id);
+	u16 val = ioread16(mem);
+
+	if (set)
+		val |= BIT(bit);
+	else
+		val &= ~BIT(bit);
+
+	iowrite16(val, mem);
+}
+
+static inline unsigned int rza1_get_bit(struct rza1_port *port,
+					unsigned int reg, unsigned int bit)
+{
+	void __iomem *mem = RZA1_ADDR(port->base, reg, port->id);
+
+	return ioread16(mem) & BIT(bit);
+}
+
+/**
+ * rza1_pin_reset() - reset a pin to default initial state
+ *
+ * Reset pin state disabling input buffer and bi-directional control,
+ * and configure it as input port.
+ * Note that pin is now configured with direction as input but with input
+ * buffer disabled. This implies the pin value cannot be read in this state.
+ *
+ * @port: port where pin sits on
+ * @pin: pin offset
+ */
+static void rza1_pin_reset(struct rza1_port *port, unsigned int pin)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&port->lock, irqflags);
+	rza1_set_bit(port, RZA1_PIBC_REG, pin, 0);
+	rza1_set_bit(port, RZA1_PBDC_REG, pin, 0);
+
+	rza1_set_bit(port, RZA1_PM_REG, pin, 1);
+	rza1_set_bit(port, RZA1_PMC_REG, pin, 0);
+	rza1_set_bit(port, RZA1_PIPC_REG, pin, 0);
+	spin_unlock_irqrestore(&port->lock, irqflags);
+}
+
+static inline int rza1_pin_get_direction(struct rza1_port *port,
+					 unsigned int pin)
+{
+	unsigned long irqflags;
+	int input;
+
+	spin_lock_irqsave(&port->lock, irqflags);
+	input = rza1_get_bit(port, RZA1_PM_REG, pin);
+	spin_unlock_irqrestore(&port->lock, irqflags);
+
+	return !!input;
+}
+
+/**
+ * rza1_pin_set_direction() - set I/O direction on a pin in port mode
+ *
+ * When running in output port mode keep PBDC enabled to allow reading the
+ * pin value from PPR.
+ *
+ * @port: port where pin sits on
+ * @pin: pin offset
+ * @input: input enable/disable flag
+ */
+static inline void rza1_pin_set_direction(struct rza1_port *port,
+					  unsigned int pin, bool input)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&port->lock, irqflags);
+
+	rza1_set_bit(port, RZA1_PIBC_REG, pin, 1);
+	if (input) {
+		rza1_set_bit(port, RZA1_PM_REG, pin, 1);
+		rza1_set_bit(port, RZA1_PBDC_REG, pin, 0);
+	} else {
+		rza1_set_bit(port, RZA1_PM_REG, pin, 0);
+		rza1_set_bit(port, RZA1_PBDC_REG, pin, 1);
+	}
+
+	spin_unlock_irqrestore(&port->lock, irqflags);
+}
+
+static inline void rza1_pin_set(struct rza1_port *port, unsigned int pin,
+				unsigned int value)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&port->lock, irqflags);
+	rza1_set_bit(port, RZA1_P_REG, pin, !!value);
+	spin_unlock_irqrestore(&port->lock, irqflags);
+}
+
+static inline int rza1_pin_get(struct rza1_port *port, unsigned int pin)
+{
+	unsigned long irqflags;
+	int val;
+
+	spin_lock_irqsave(&port->lock, irqflags);
+	val = rza1_get_bit(port, RZA1_PPR_REG, pin);
+	spin_unlock_irqrestore(&port->lock, irqflags);
+
+	return val;
+}
+
+/**
+ * rza1_pin_mux_single() - configure pin multiplexing on a single pin
+ *
+ * @pinctrl: RZ/A1 pin controller device
+ * @mux_conf: pin multiplexing descriptor
+ */
+static int rza1_pin_mux_single(struct rza1_pinctrl *rza1_pctl,
+			       struct rza1_mux_conf *mux_conf)
+{
+	struct rza1_port *port = &rza1_pctl->ports[mux_conf->port];
+	unsigned int pin = mux_conf->pin;
+	u8 mux_func = mux_conf->mux_func;
+	u8 mux_flags = mux_conf->mux_flags;
+	u8 mux_flags_from_table;
+
+	rza1_pin_reset(port, pin);
+
+	/* SWIO pinmux flags coming from DT are high precedence */
+	mux_flags_from_table = rza1_pinmux_get_flags(port->id, pin, mux_func,
+						     rza1_pctl);
+	if (mux_flags)
+		mux_flags |= (mux_flags_from_table & MUX_FLAGS_BIDIR);
+	else
+		mux_flags = mux_flags_from_table;
+
+	if (mux_flags & MUX_FLAGS_BIDIR)
+		rza1_set_bit(port, RZA1_PBDC_REG, pin, 1);
+
+	/*
+	 * Enable alternate function mode and select it.
+	 *
+	 * Be careful here: the pin mux sub-nodes in device tree
+	 * enumerate alternate functions from 1 to 8;
+	 * subtract 1 before using macros to match registers configuration
+	 * which expects numbers from 0 to 7 instead.
+	 *
+	 * ----------------------------------------------------
+	 * Alternate mode selection table:
+	 *
+	 * PMC	PFC	PFCE	PFCAE	(mux_func - 1)
+	 * 1	0	0	0	0
+	 * 1	1	0	0	1
+	 * 1	0	1	0	2
+	 * 1	1	1	0	3
+	 * 1	0	0	1	4
+	 * 1	1	0	1	5
+	 * 1	0	1	1	6
+	 * 1	1	1	1	7
+	 * ----------------------------------------------------
+	 */
+	mux_func -= 1;
+	rza1_set_bit(port, RZA1_PFC_REG, pin, mux_func & MUX_FUNC_PFC_MASK);
+	rza1_set_bit(port, RZA1_PFCE_REG, pin, mux_func & MUX_FUNC_PFCE_MASK);
+	rza1_set_bit(port, RZA1_PFCEA_REG, pin, mux_func & MUX_FUNC_PFCEA_MASK);
+
+	/*
+	 * All alternate functions except a few need PIPCn = 1.
+	 * If PIPCn has to stay disabled (SW IO mode), configure PMn according
+	 * to I/O direction specified by pin configuration -after- PMC has been
+	 * set to one.
+	 */
+	if (mux_flags & (MUX_FLAGS_SWIO_INPUT | MUX_FLAGS_SWIO_OUTPUT))
+		rza1_set_bit(port, RZA1_PM_REG, pin,
+			     mux_flags & MUX_FLAGS_SWIO_INPUT);
+	else
+		rza1_set_bit(port, RZA1_PIPC_REG, pin, 1);
+
+	rza1_set_bit(port, RZA1_PMC_REG, pin, 1);
+
+	return 0;
+}
+
+/* ----------------------------------------------------------------------------
+ * gpio operations
+ */
+
+/**
+ * rza1_gpio_request() - configure pin in port mode
+ *
+ * Configure a pin as gpio (port mode).
+ * After reset, the pin is in input mode with input buffer disabled.
+ * To use the pin as input or output, set_direction shall be called first
+ *
+ * @chip: gpio chip where the gpio sits on
+ * @gpio: gpio offset
+ */
+static int rza1_gpio_request(struct gpio_chip *chip, unsigned int gpio)
+{
+	struct rza1_port *port = gpiochip_get_data(chip);
+
+	rza1_pin_reset(port, gpio);
+
+	return 0;
+}
+
+/**
+ * rza1_gpio_disable_free() - reset a pin
+ *
+ * Surprisingly, disable_free a gpio, is equivalent to request it.
+ * Reset pin to port mode, with input buffer disabled. This overwrites all
+ * port direction settings applied with set_direction
+ *
+ * @chip: gpio chip where the gpio sits on
+ * @gpio: gpio offset
+ */
+static void rza1_gpio_free(struct gpio_chip *chip, unsigned int gpio)
+{
+	struct rza1_port *port = gpiochip_get_data(chip);
+
+	rza1_pin_reset(port, gpio);
+}
+
+static int rza1_gpio_get_direction(struct gpio_chip *chip, unsigned int gpio)
+{
+	struct rza1_port *port = gpiochip_get_data(chip);
+
+	return rza1_pin_get_direction(port, gpio);
+}
+
+static int rza1_gpio_direction_input(struct gpio_chip *chip,
+				     unsigned int gpio)
+{
+	struct rza1_port *port = gpiochip_get_data(chip);
+
+	rza1_pin_set_direction(port, gpio, true);
+
+	return 0;
+}
+
+static int rza1_gpio_direction_output(struct gpio_chip *chip,
+				      unsigned int gpio,
+				      int value)
+{
+	struct rza1_port *port = gpiochip_get_data(chip);
+
+	/* Set value before driving pin direction */
+	rza1_pin_set(port, gpio, value);
+	rza1_pin_set_direction(port, gpio, false);
+
+	return 0;
+}
+
+/**
+ * rza1_gpio_get() - read a gpio pin value
+ *
+ * Read gpio pin value through PPR register.
+ * Requires bi-directional mode to work when reading the value of a pin
+ * in output mode
+ *
+ * @chip: gpio chip where the gpio sits on
+ * @gpio: gpio offset
+ */
+static int rza1_gpio_get(struct gpio_chip *chip, unsigned int gpio)
+{
+	struct rza1_port *port = gpiochip_get_data(chip);
+
+	return rza1_pin_get(port, gpio);
+}
+
+static void rza1_gpio_set(struct gpio_chip *chip, unsigned int gpio,
+			  int value)
+{
+	struct rza1_port *port = gpiochip_get_data(chip);
+
+	rza1_pin_set(port, gpio, value);
+}
+
+static struct gpio_chip rza1_gpiochip_template = {
+	.request		= rza1_gpio_request,
+	.free			= rza1_gpio_free,
+	.get_direction		= rza1_gpio_get_direction,
+	.direction_input	= rza1_gpio_direction_input,
+	.direction_output	= rza1_gpio_direction_output,
+	.get			= rza1_gpio_get,
+	.set			= rza1_gpio_set,
+};
+/* ----------------------------------------------------------------------------
+ * pinctrl operations
+ */
+
+/**
+ * rza1_dt_node_pin_count() - Count number of pins in a dt node or in all its
+ *			      children sub-nodes
+ *
+ * @np: device tree node to parse
+ */
+static int rza1_dt_node_pin_count(struct device_node *np)
+{
+	struct device_node *child;
+	struct property *of_pins;
+	unsigned int npins;
+
+	of_pins = of_find_property(np, "pinmux", NULL);
+	if (of_pins)
+		return of_pins->length / sizeof(u32);
+
+	npins = 0;
+	for_each_child_of_node(np, child) {
+		of_pins = of_find_property(child, "pinmux", NULL);
+		if (!of_pins)
+			return -EINVAL;
+
+		npins += of_pins->length / sizeof(u32);
+	}
+
+	return npins;
+}
+
+/**
+ * rza1_parse_pmx_function() - parse a pin mux sub-node
+ *
+ * @rza1_pctl: RZ/A1 pin controller device
+ * @np: of pmx sub-node
+ * @mux_confs: array of pin mux configurations to fill with parsed info
+ * @grpins: array of pin ids to mux
+ */
+static int rza1_parse_pinmux_node(struct rza1_pinctrl *rza1_pctl,
+				  struct device_node *np,
+				  struct rza1_mux_conf *mux_confs,
+				  unsigned int *grpins)
+{
+	struct pinctrl_dev *pctldev = rza1_pctl->pctl;
+	char const *prop_name = "pinmux";
+	unsigned long *pin_configs;
+	unsigned int npin_configs;
+	struct property *of_pins;
+	unsigned int npins;
+	u8 pinmux_flags;
+	unsigned int i;
+	int ret;
+
+	of_pins = of_find_property(np, prop_name, NULL);
+	if (!of_pins) {
+		dev_dbg(rza1_pctl->dev, "Missing %s property\n", prop_name);
+		return -ENOENT;
+	}
+	npins = of_pins->length / sizeof(u32);
+
+	/*
+	 * Collect pin configuration properties: they apply to all pins in
+	 * this sub-node
+	 */
+	ret = pinconf_generic_parse_dt_config(np, pctldev, &pin_configs,
+					      &npin_configs);
+	if (ret) {
+		dev_err(rza1_pctl->dev,
+			"Unable to parse pin configuration options for %s\n",
+			np->name);
+		return ret;
+	}
+
+	/*
+	 * Create a mask with pinmux flags from pin configuration;
+	 * very few pins (TIOC[0-4][A|B|C|D] require SWIO direction
+	 * specified in device tree.
+	 */
+	pinmux_flags = 0;
+	for (i = 0; i < npin_configs && pinmux_flags == 0; i++)
+		switch (pinconf_to_config_param(pin_configs[i])) {
+		case PIN_CONFIG_INPUT_ENABLE:
+			pinmux_flags |= MUX_FLAGS_SWIO_INPUT;
+			break;
+		case PIN_CONFIG_OUTPUT:
+			pinmux_flags |= MUX_FLAGS_SWIO_OUTPUT;
+		default:
+			break;
+
+		}
+
+	kfree(pin_configs);
+
+	/* Collect pin positions and their mux settings. */
+	for (i = 0; i < npins; ++i) {
+		u32 of_pinconf;
+		struct rza1_mux_conf *mux_conf = &mux_confs[i];
+
+		ret = of_property_read_u32_index(np, prop_name, i, &of_pinconf);
+		if (ret)
+			return ret;
+
+		mux_conf->id		= of_pinconf & MUX_PIN_ID_MASK;
+		mux_conf->port		= RZA1_PIN_ID_TO_PORT(mux_conf->id);
+		mux_conf->pin		= RZA1_PIN_ID_TO_PIN(mux_conf->id);
+		mux_conf->mux_func	= MUX_FUNC(of_pinconf);
+		mux_conf->mux_flags	= pinmux_flags;
+
+		if (mux_conf->port >= RZA1_NPORTS ||
+		    mux_conf->pin >= RZA1_PINS_PER_PORT) {
+			dev_err(rza1_pctl->dev,
+				"Wrong port %u pin %u for %s property\n",
+				mux_conf->port, mux_conf->pin, prop_name);
+			return -EINVAL;
+		}
+
+		grpins[i] = mux_conf->id;
+	}
+
+	return npins;
+}
+
+/**
+ * rza1_dt_node_to_map() - map a pin mux node to a function/group
+ *
+ * Parse and register a pin mux function.
+ *
+ * @pctldev: pin controller device
+ * @np: device tree node to parse
+ * @map: pointer to pin map (output)
+ * @num_maps: number of collected maps (output)
+ */
+static int rza1_dt_node_to_map(struct pinctrl_dev *pctldev,
+			       struct device_node *np,
+			       struct pinctrl_map **map,
+			       unsigned int *num_maps)
+{
+	struct rza1_pinctrl *rza1_pctl = pinctrl_dev_get_drvdata(pctldev);
+	struct rza1_mux_conf *mux_confs, *mux_conf;
+	unsigned int *grpins, *grpin;
+	struct device_node *child;
+	const char *grpname;
+	const char **fngrps;
+	int ret, npins;
+
+	npins = rza1_dt_node_pin_count(np);
+	if (npins < 0) {
+		dev_err(rza1_pctl->dev, "invalid pinmux node structure\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Functions are made of 1 group only;
+	 * in fact, functions and groups are identical for this pin controller
+	 * except that functions carry an array of per-pin mux configuration
+	 * settings.
+	 */
+	mux_confs = devm_kcalloc(rza1_pctl->dev, npins, sizeof(*mux_confs),
+				 GFP_KERNEL);
+	grpins = devm_kcalloc(rza1_pctl->dev, npins, sizeof(*grpins),
+			      GFP_KERNEL);
+	fngrps = devm_kzalloc(rza1_pctl->dev, sizeof(*fngrps), GFP_KERNEL);
+
+	if (!mux_confs || !grpins || !fngrps)
+		return -ENOMEM;
+
+	/*
+	 * Parse the pinmux node.
+	 * If the node does not contain "pinmux" property (-ENOENT)
+	 * that property shall be specified in all its children sub-nodes.
+	 */
+	mux_conf = &mux_confs[0];
+	grpin = &grpins[0];
+
+	ret = rza1_parse_pinmux_node(rza1_pctl, np, mux_conf, grpin);
+	if (ret == -ENOENT)
+		for_each_child_of_node(np, child) {
+			ret = rza1_parse_pinmux_node(rza1_pctl, child, mux_conf,
+						     grpin);
+			if (ret < 0)
+				return ret;
+
+			grpin += ret;
+			mux_conf += ret;
+		}
+	else if (ret < 0)
+		return ret;
+
+	/* Register pin group and function name to pinctrl_generic */
+	grpname	= np->name;
+	fngrps[0] = grpname;
+
+	mutex_lock(&rza1_pctl->mutex);
+	ret = pinctrl_generic_add_group(pctldev, grpname, grpins, npins,
+					NULL);
+	if (ret) {
+		mutex_unlock(&rza1_pctl->mutex);
+		return ret;
+	}
+
+	ret = pinmux_generic_add_function(pctldev, grpname, fngrps, 1,
+					  mux_confs);
+	if (ret)
+		goto remove_group;
+	mutex_unlock(&rza1_pctl->mutex);
+
+	dev_info(rza1_pctl->dev, "Parsed function and group %s with %d pins\n",
+				 grpname, npins);
+
+	/* Create map where to retrieve function and mux settings from */
+	*num_maps = 0;
+	*map = kzalloc(sizeof(**map), GFP_KERNEL);
+	if (!*map) {
+		ret = -ENOMEM;
+		goto remove_function;
+	}
+
+	(*map)->type = PIN_MAP_TYPE_MUX_GROUP;
+	(*map)->data.mux.group = np->name;
+	(*map)->data.mux.function = np->name;
+	*num_maps = 1;
+
+	return 0;
+
+remove_function:
+	mutex_lock(&rza1_pctl->mutex);
+	pinmux_generic_remove_last_function(pctldev);
+
+remove_group:
+	pinctrl_generic_remove_last_group(pctldev);
+	mutex_unlock(&rza1_pctl->mutex);
+
+	dev_info(rza1_pctl->dev, "Unable to parse function and group %s\n",
+				 grpname);
+
+	return ret;
+}
+
+static void rza1_dt_free_map(struct pinctrl_dev *pctldev,
+			     struct pinctrl_map *map, unsigned int num_maps)
+{
+	kfree(map);
+}
+
+static const struct pinctrl_ops rza1_pinctrl_ops = {
+	.get_groups_count	= pinctrl_generic_get_group_count,
+	.get_group_name		= pinctrl_generic_get_group_name,
+	.get_group_pins		= pinctrl_generic_get_group_pins,
+	.dt_node_to_map		= rza1_dt_node_to_map,
+	.dt_free_map		= rza1_dt_free_map,
+};
+
+/* ----------------------------------------------------------------------------
+ * pinmux operations
+ */
+
+/**
+ * rza1_set_mux() - retrieve pins from a group and apply their mux settings
+ *
+ * @pctldev: pin controller device
+ * @selector: function selector
+ * @group: group selector
+ */
+static int rza1_set_mux(struct pinctrl_dev *pctldev, unsigned int selector,
+			   unsigned int group)
+{
+	struct rza1_pinctrl *rza1_pctl = pinctrl_dev_get_drvdata(pctldev);
+	struct rza1_mux_conf *mux_confs;
+	struct function_desc *func;
+	struct group_desc *grp;
+	int i;
+
+	grp = pinctrl_generic_get_group(pctldev, group);
+	if (!grp)
+		return -EINVAL;
+
+	func = pinmux_generic_get_function(pctldev, selector);
+	if (!func)
+		return -EINVAL;
+
+	mux_confs = (struct rza1_mux_conf *)func->data;
+	for (i = 0; i < grp->num_pins; ++i) {
+		int ret;
+
+		ret = rza1_pin_mux_single(rza1_pctl, &mux_confs[i]);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static struct pinmux_ops rza1_pinmux_ops = {
+	.get_functions_count	= pinmux_generic_get_function_count,
+	.get_function_name	= pinmux_generic_get_function_name,
+	.get_function_groups	= pinmux_generic_get_function_groups,
+	.set_mux		= rza1_set_mux,
+	.strict			= true,
+};
+
+/* ----------------------------------------------------------------------------
+ * RZ/A1 pin controller driver operations
+ */
+
+static unsigned int rza1_count_gpio_chips(struct device_node *np)
+{
+	struct device_node *child;
+	unsigned int count = 0;
+
+	for_each_child_of_node(np, child) {
+		if (!of_property_read_bool(child, "gpio-controller"))
+			continue;
+
+		count++;
+	}
+
+	return count;
+}
+
+/**
+ * rza1_parse_gpiochip() - parse and register a gpio chip and pin range
+ *
+ * The gpio controller subnode shall provide a "gpio-ranges" list property as
+ * defined by gpio device tree binding documentation.
+ *
+ * @rza1_pctl: RZ/A1 pin controller device
+ * @np: of gpio-controller node
+ * @chip: gpio chip to register to gpiolib
+ * @range: pin range to register to pinctrl core
+ */
+static int rza1_parse_gpiochip(struct rza1_pinctrl *rza1_pctl,
+			       struct device_node *np,
+			       struct gpio_chip *chip,
+			       struct pinctrl_gpio_range *range)
+{
+	const char *list_name = "gpio-ranges";
+	struct of_phandle_args of_args;
+	unsigned int gpioport;
+	u32 pinctrl_base;
+	int ret;
+
+	ret = of_parse_phandle_with_fixed_args(np, list_name, 3, 0, &of_args);
+	if (ret) {
+		dev_err(rza1_pctl->dev, "Unable to parse %s list property\n",
+			list_name);
+		return ret;
+	}
+
+	/*
+	 * Find out on which port this gpio-chip maps to by inspecting the
+	 * second argument of the "gpio-ranges" property.
+	 */
+	pinctrl_base = of_args.args[1];
+	gpioport = RZA1_PIN_ID_TO_PORT(pinctrl_base);
+	if (gpioport > RZA1_NPORTS) {
+		dev_err(rza1_pctl->dev,
+			"Invalid values in property %s\n", list_name);
+		return -EINVAL;
+	}
+
+	*chip		= rza1_gpiochip_template;
+	chip->base	= -1;
+	chip->label	= devm_kasprintf(rza1_pctl->dev, GFP_KERNEL, "%s-%u",
+					 np->name, gpioport);
+	chip->ngpio	= of_args.args[2];
+	chip->of_node	= np;
+	chip->parent	= rza1_pctl->dev;
+
+	range->id	= gpioport;
+	range->name	= chip->label;
+	range->pin_base	= range->base = pinctrl_base;
+	range->npins	= of_args.args[2];
+	range->gc	= chip;
+
+	ret = devm_gpiochip_add_data(rza1_pctl->dev, chip,
+				     &rza1_pctl->ports[gpioport]);
+	if (ret)
+		return ret;
+
+	pinctrl_add_gpio_range(rza1_pctl->pctl, range);
+
+	dev_info(rza1_pctl->dev, "Parsed gpiochip %s with %d pins\n",
+		 chip->label, chip->ngpio);
+
+	return 0;
+}
+
+/**
+ * rza1_gpio_register() - parse DT to collect gpio-chips and gpio-ranges
+ *
+ * @rza1_pctl: RZ/A1 pin controller device
+ */
+static int rza1_gpio_register(struct rza1_pinctrl *rza1_pctl)
+{
+	struct device_node *np = rza1_pctl->dev->of_node;
+	struct pinctrl_gpio_range *gpio_ranges;
+	struct gpio_chip *gpio_chips;
+	struct device_node *child;
+	unsigned int ngpiochips;
+	unsigned int i;
+	int ret;
+
+	ngpiochips = rza1_count_gpio_chips(np);
+	if (ngpiochips == 0) {
+		dev_dbg(rza1_pctl->dev, "No gpiochip registered\n");
+		return 0;
+	}
+
+	gpio_chips = devm_kcalloc(rza1_pctl->dev, ngpiochips,
+				  sizeof(*gpio_chips), GFP_KERNEL);
+	gpio_ranges = devm_kcalloc(rza1_pctl->dev, ngpiochips,
+				   sizeof(*gpio_ranges), GFP_KERNEL);
+	if (!gpio_chips || !gpio_ranges)
+		return -ENOMEM;
+
+	i = 0;
+	for_each_child_of_node(np, child) {
+		if (!of_property_read_bool(child, "gpio-controller"))
+			continue;
+
+		ret = rza1_parse_gpiochip(rza1_pctl, child, &gpio_chips[i],
+					  &gpio_ranges[i]);
+		if (ret)
+			goto gpiochip_remove;
+
+		++i;
+	}
+
+	dev_info(rza1_pctl->dev, "Registered %u gpio controllers\n", i);
+
+	return 0;
+
+gpiochip_remove:
+	for (; i > 0; i--)
+		devm_gpiochip_remove(rza1_pctl->dev, &gpio_chips[i - 1]);
+
+	return ret;
+}
+
+/**
+ * rza1_pinctrl_register() - Enumerate pins, ports and gpiochips; register
+ *			     them to pinctrl and gpio cores.
+ *
+ * @rza1_pctl: RZ/A1 pin controller device
+ */
+static int rza1_pinctrl_register(struct rza1_pinctrl *rza1_pctl)
+{
+	struct pinctrl_pin_desc *pins;
+	struct rza1_port *ports;
+	unsigned int i;
+	int ret;
+
+	pins = devm_kcalloc(rza1_pctl->dev, RZA1_NPINS, sizeof(*pins),
+			    GFP_KERNEL);
+	ports = devm_kcalloc(rza1_pctl->dev, RZA1_NPORTS, sizeof(*ports),
+			     GFP_KERNEL);
+	if (!pins || !ports)
+		return -ENOMEM;
+
+	rza1_pctl->pins		= pins;
+	rza1_pctl->desc.pins	= pins;
+	rza1_pctl->desc.npins	= RZA1_NPINS;
+	rza1_pctl->ports	= ports;
+
+	for (i = 0; i < RZA1_NPINS; ++i) {
+		unsigned int pin = RZA1_PIN_ID_TO_PIN(i);
+		unsigned int port = RZA1_PIN_ID_TO_PORT(i);
+
+		pins[i].number = i;
+		pins[i].name = devm_kasprintf(rza1_pctl->dev, GFP_KERNEL,
+					      "P%u-%u", port, pin);
+
+		if (i % RZA1_PINS_PER_PORT == 0) {
+			/*
+			 * Setup ports;
+			 * they provide per-port lock and logical base address.
+			 */
+			unsigned int port_id = RZA1_PIN_ID_TO_PORT(i);
+
+			ports[port_id].id	= port_id;
+			ports[port_id].base	= rza1_pctl->base;
+			ports[port_id].pins	= &pins[i];
+			spin_lock_init(&ports[port_id].lock);
+		}
+	}
+
+	ret = devm_pinctrl_register_and_init(rza1_pctl->dev, &rza1_pctl->desc,
+					     rza1_pctl, &rza1_pctl->pctl);
+	if (ret) {
+		dev_err(rza1_pctl->dev,
+			"RZ/A1 pin controller registration failed\n");
+		return ret;
+	}
+
+	ret = pinctrl_enable(rza1_pctl->pctl);
+	if (ret) {
+		dev_err(rza1_pctl->dev,
+			"RZ/A1 pin controller failed to start\n");
+		return ret;
+	}
+
+	ret = rza1_gpio_register(rza1_pctl);
+	if (ret) {
+		dev_err(rza1_pctl->dev, "RZ/A1 GPIO registration failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int rza1_pinctrl_probe(struct platform_device *pdev)
+{
+	struct rza1_pinctrl *rza1_pctl;
+	struct resource *res;
+	int ret;
+
+	rza1_pctl = devm_kzalloc(&pdev->dev, sizeof(*rza1_pctl), GFP_KERNEL);
+	if (!rza1_pctl)
+		return -ENOMEM;
+
+	rza1_pctl->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	rza1_pctl->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(rza1_pctl->base))
+		return PTR_ERR(rza1_pctl->base);
+
+	mutex_init(&rza1_pctl->mutex);
+
+	platform_set_drvdata(pdev, rza1_pctl);
+
+	rza1_pctl->desc.name	= DRIVER_NAME;
+	rza1_pctl->desc.pctlops	= &rza1_pinctrl_ops;
+	rza1_pctl->desc.pmxops	= &rza1_pinmux_ops;
+	rza1_pctl->desc.owner	= THIS_MODULE;
+	rza1_pctl->data		= of_device_get_match_data(&pdev->dev);
+
+	ret = rza1_pinctrl_register(rza1_pctl);
+	if (ret)
+		return ret;
+
+	dev_info(&pdev->dev,
+		 "RZ/A1 pin controller and gpio successfully registered\n");
+
+	return 0;
+}
+
+static const struct of_device_id rza1_pinctrl_of_match[] = {
+	{
+		.compatible	= "renesas,r7s72100-ports",
+		.data		= &rza1h_pmx_conf,
+	},
+	{ }
+};
+
+static struct platform_driver rza1_pinctrl_driver = {
+	.driver = {
+		.name = DRIVER_NAME,
+		.of_match_table = rza1_pinctrl_of_match,
+	},
+	.probe = rza1_pinctrl_probe,
+};
+
+static int __init rza1_pinctrl_init(void)
+{
+	return platform_driver_register(&rza1_pinctrl_driver);
+}
+core_initcall(rza1_pinctrl_init);
+
+MODULE_AUTHOR("Jacopo Mondi <jacopo+renesas@jmondi.org");
+MODULE_DESCRIPTION("Pin and gpio controller driver for Reneas RZ/A1 SoC");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-single.c b/drivers/pinctrl/pinctrl-single.c
index 9c267dcda094..b8b3d932cd73 100644
--- a/drivers/pinctrl/pinctrl-single.c
+++ b/drivers/pinctrl/pinctrl-single.c
@@ -1270,8 +1270,6 @@ static void pcs_free_resources(struct pcs_device *pcs)
 #endif
 }
 
-static const struct of_device_id pcs_of_match[];
-
 static int pcs_add_gpio_func(struct device_node *node, struct pcs_device *pcs)
 {
 	const char *propname = "pinctrl-single,gpio-range";
@@ -1637,15 +1635,14 @@ static int pcs_quirk_missing_pinctrl_cells(struct pcs_device *pcs,
 static int pcs_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
-	const struct of_device_id *match;
 	struct pcs_pdata *pdata;
 	struct resource *res;
 	struct pcs_device *pcs;
 	const struct pcs_soc_data *soc;
 	int ret;
 
-	match = of_match_device(pcs_of_match, &pdev->dev);
-	if (!match)
+	soc = of_device_get_match_data(&pdev->dev);
+	if (WARN_ON(!soc))
 		return -EINVAL;
 
 	pcs = devm_kzalloc(&pdev->dev, sizeof(*pcs), GFP_KERNEL);
@@ -1658,7 +1655,6 @@ static int pcs_probe(struct platform_device *pdev)
 	raw_spin_lock_init(&pcs->lock);
 	mutex_init(&pcs->mutex);
 	INIT_LIST_HEAD(&pcs->gpiofuncs);
-	soc = match->data;
 	pcs->flags = soc->flags;
 	memcpy(&pcs->socdata, soc, sizeof(*soc));
 
diff --git a/drivers/pinctrl/pinctrl-xway.c b/drivers/pinctrl/pinctrl-xway.c
index d4167e2c173a..f9e98a7d4f0c 100644
--- a/drivers/pinctrl/pinctrl-xway.c
+++ b/drivers/pinctrl/pinctrl-xway.c
@@ -1028,7 +1028,7 @@ static const struct ltq_pin_group xrx200_grps[] = {
 	GRP_MUX("spi_cs5", SPI, xrx200_pins_spi_cs5),
 	GRP_MUX("spi_cs6", SPI, xrx200_pins_spi_cs6),
 	GRP_MUX("usif uart_rx", USIF, xrx200_pins_usif_uart_rx),
-	GRP_MUX("usif uart_rx", USIF, xrx200_pins_usif_uart_tx),
+	GRP_MUX("usif uart_tx", USIF, xrx200_pins_usif_uart_tx),
 	GRP_MUX("usif uart_rts", USIF, xrx200_pins_usif_uart_rts),
 	GRP_MUX("usif uart_cts", USIF, xrx200_pins_usif_uart_cts),
 	GRP_MUX("usif uart_dtr", USIF, xrx200_pins_usif_uart_dtr),
diff --git a/drivers/pinctrl/qcom/Kconfig b/drivers/pinctrl/qcom/Kconfig
index 3ebdc01f53c0..9e504dbc7fb5 100644
--- a/drivers/pinctrl/qcom/Kconfig
+++ b/drivers/pinctrl/qcom/Kconfig
@@ -39,6 +39,16 @@ config PINCTRL_IPQ8064
 	  This is the pinctrl, pinmux, pinconf and gpiolib driver for the
 	  Qualcomm TLMM block found in the Qualcomm IPQ8064 platform.
 
+config PINCTRL_IPQ8074
+	tristate "Qualcomm Technologies, Inc. IPQ8074 pin controller driver"
+	depends on GPIOLIB && OF
+	select PINCTRL_MSM
+	help
+	  This is the pinctrl, pinmux, pinconf and gpiolib driver for
+	  the Qualcomm Technologies Inc. TLMM block found on the
+	  Qualcomm Technologies Inc. IPQ8074 platform. Select this for
+	  IPQ8074.
+
 config PINCTRL_MSM8660
 	tristate "Qualcomm 8660 pin controller driver"
 	depends on GPIOLIB && OF
diff --git a/drivers/pinctrl/qcom/Makefile b/drivers/pinctrl/qcom/Makefile
index ab47764dbc5c..06c8b2ace05f 100644
--- a/drivers/pinctrl/qcom/Makefile
+++ b/drivers/pinctrl/qcom/Makefile
@@ -4,6 +4,7 @@ obj-$(CONFIG_PINCTRL_APQ8064)	+= pinctrl-apq8064.o
 obj-$(CONFIG_PINCTRL_APQ8084)	+= pinctrl-apq8084.o
 obj-$(CONFIG_PINCTRL_IPQ4019)	+= pinctrl-ipq4019.o
 obj-$(CONFIG_PINCTRL_IPQ8064)	+= pinctrl-ipq8064.o
+obj-$(CONFIG_PINCTRL_IPQ8074)	+= pinctrl-ipq8074.o
 obj-$(CONFIG_PINCTRL_MSM8660)	+= pinctrl-msm8660.o
 obj-$(CONFIG_PINCTRL_MSM8960)	+= pinctrl-msm8960.o
 obj-$(CONFIG_PINCTRL_MSM8X74)	+= pinctrl-msm8x74.o
diff --git a/drivers/pinctrl/qcom/pinctrl-ipq8074.c b/drivers/pinctrl/qcom/pinctrl-ipq8074.c
new file mode 100644
index 000000000000..10fb076e2456
--- /dev/null
+++ b/drivers/pinctrl/qcom/pinctrl-ipq8074.c
@@ -0,0 +1,1076 @@
+/*
+ * Copyright (c) 2017, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-msm.h"
+
+#define FUNCTION(fname)			                \
+	[msm_mux_##fname] = {		                \
+		.name = #fname,				\
+		.groups = fname##_groups,               \
+		.ngroups = ARRAY_SIZE(fname##_groups),	\
+	}
+
+#define REG_SIZE 0x1000
+#define PINGROUP(id, f1, f2, f3, f4, f5, f6, f7, f8, f9)	\
+	{					        \
+		.name = "gpio" #id,			\
+		.pins = gpio##id##_pins,		\
+		.npins = (unsigned int)ARRAY_SIZE(gpio##id##_pins),	\
+		.funcs = (int[]){			\
+			msm_mux_gpio, /* gpio mode */	\
+			msm_mux_##f1,			\
+			msm_mux_##f2,			\
+			msm_mux_##f3,			\
+			msm_mux_##f4,			\
+			msm_mux_##f5,			\
+			msm_mux_##f6,			\
+			msm_mux_##f7,			\
+			msm_mux_##f8,			\
+			msm_mux_##f9			\
+		},				        \
+		.nfuncs = 10,				\
+		.ctl_reg = REG_SIZE * id,		\
+		.io_reg = 0x4 + REG_SIZE * id,		\
+		.intr_cfg_reg = 0x8 + REG_SIZE * id,		\
+		.intr_status_reg = 0xc + REG_SIZE * id,	\
+		.intr_target_reg = 0x8 + REG_SIZE * id,	\
+		.mux_bit = 2,			\
+		.pull_bit = 0,			\
+		.drv_bit = 6,			\
+		.oe_bit = 9,			\
+		.in_bit = 0,			\
+		.out_bit = 1,			\
+		.intr_enable_bit = 0,		\
+		.intr_status_bit = 0,		\
+		.intr_target_bit = 5,		\
+		.intr_raw_status_bit = 4,	\
+		.intr_polarity_bit = 1,		\
+		.intr_detection_bit = 2,	\
+		.intr_detection_width = 2,	\
+	}
+
+static const struct pinctrl_pin_desc ipq8074_pins[] = {
+	PINCTRL_PIN(0, "GPIO_0"),
+	PINCTRL_PIN(1, "GPIO_1"),
+	PINCTRL_PIN(2, "GPIO_2"),
+	PINCTRL_PIN(3, "GPIO_3"),
+	PINCTRL_PIN(4, "GPIO_4"),
+	PINCTRL_PIN(5, "GPIO_5"),
+	PINCTRL_PIN(6, "GPIO_6"),
+	PINCTRL_PIN(7, "GPIO_7"),
+	PINCTRL_PIN(8, "GPIO_8"),
+	PINCTRL_PIN(9, "GPIO_9"),
+	PINCTRL_PIN(10, "GPIO_10"),
+	PINCTRL_PIN(11, "GPIO_11"),
+	PINCTRL_PIN(12, "GPIO_12"),
+	PINCTRL_PIN(13, "GPIO_13"),
+	PINCTRL_PIN(14, "GPIO_14"),
+	PINCTRL_PIN(15, "GPIO_15"),
+	PINCTRL_PIN(16, "GPIO_16"),
+	PINCTRL_PIN(17, "GPIO_17"),
+	PINCTRL_PIN(18, "GPIO_18"),
+	PINCTRL_PIN(19, "GPIO_19"),
+	PINCTRL_PIN(20, "GPIO_20"),
+	PINCTRL_PIN(21, "GPIO_21"),
+	PINCTRL_PIN(22, "GPIO_22"),
+	PINCTRL_PIN(23, "GPIO_23"),
+	PINCTRL_PIN(24, "GPIO_24"),
+	PINCTRL_PIN(25, "GPIO_25"),
+	PINCTRL_PIN(26, "GPIO_26"),
+	PINCTRL_PIN(27, "GPIO_27"),
+	PINCTRL_PIN(28, "GPIO_28"),
+	PINCTRL_PIN(29, "GPIO_29"),
+	PINCTRL_PIN(30, "GPIO_30"),
+	PINCTRL_PIN(31, "GPIO_31"),
+	PINCTRL_PIN(32, "GPIO_32"),
+	PINCTRL_PIN(33, "GPIO_33"),
+	PINCTRL_PIN(34, "GPIO_34"),
+	PINCTRL_PIN(35, "GPIO_35"),
+	PINCTRL_PIN(36, "GPIO_36"),
+	PINCTRL_PIN(37, "GPIO_37"),
+	PINCTRL_PIN(38, "GPIO_38"),
+	PINCTRL_PIN(39, "GPIO_39"),
+	PINCTRL_PIN(40, "GPIO_40"),
+	PINCTRL_PIN(41, "GPIO_41"),
+	PINCTRL_PIN(42, "GPIO_42"),
+	PINCTRL_PIN(43, "GPIO_43"),
+	PINCTRL_PIN(44, "GPIO_44"),
+	PINCTRL_PIN(45, "GPIO_45"),
+	PINCTRL_PIN(46, "GPIO_46"),
+	PINCTRL_PIN(47, "GPIO_47"),
+	PINCTRL_PIN(48, "GPIO_48"),
+	PINCTRL_PIN(49, "GPIO_49"),
+	PINCTRL_PIN(50, "GPIO_50"),
+	PINCTRL_PIN(51, "GPIO_51"),
+	PINCTRL_PIN(52, "GPIO_52"),
+	PINCTRL_PIN(53, "GPIO_53"),
+	PINCTRL_PIN(54, "GPIO_54"),
+	PINCTRL_PIN(55, "GPIO_55"),
+	PINCTRL_PIN(56, "GPIO_56"),
+	PINCTRL_PIN(57, "GPIO_57"),
+	PINCTRL_PIN(58, "GPIO_58"),
+	PINCTRL_PIN(59, "GPIO_59"),
+	PINCTRL_PIN(60, "GPIO_60"),
+	PINCTRL_PIN(61, "GPIO_61"),
+	PINCTRL_PIN(62, "GPIO_62"),
+	PINCTRL_PIN(63, "GPIO_63"),
+	PINCTRL_PIN(64, "GPIO_64"),
+	PINCTRL_PIN(65, "GPIO_65"),
+	PINCTRL_PIN(66, "GPIO_66"),
+	PINCTRL_PIN(67, "GPIO_67"),
+	PINCTRL_PIN(68, "GPIO_68"),
+	PINCTRL_PIN(69, "GPIO_69"),
+};
+
+#define DECLARE_MSM_GPIO_PINS(pin) \
+	static const unsigned int gpio##pin##_pins[] = { pin }
+DECLARE_MSM_GPIO_PINS(0);
+DECLARE_MSM_GPIO_PINS(1);
+DECLARE_MSM_GPIO_PINS(2);
+DECLARE_MSM_GPIO_PINS(3);
+DECLARE_MSM_GPIO_PINS(4);
+DECLARE_MSM_GPIO_PINS(5);
+DECLARE_MSM_GPIO_PINS(6);
+DECLARE_MSM_GPIO_PINS(7);
+DECLARE_MSM_GPIO_PINS(8);
+DECLARE_MSM_GPIO_PINS(9);
+DECLARE_MSM_GPIO_PINS(10);
+DECLARE_MSM_GPIO_PINS(11);
+DECLARE_MSM_GPIO_PINS(12);
+DECLARE_MSM_GPIO_PINS(13);
+DECLARE_MSM_GPIO_PINS(14);
+DECLARE_MSM_GPIO_PINS(15);
+DECLARE_MSM_GPIO_PINS(16);
+DECLARE_MSM_GPIO_PINS(17);
+DECLARE_MSM_GPIO_PINS(18);
+DECLARE_MSM_GPIO_PINS(19);
+DECLARE_MSM_GPIO_PINS(20);
+DECLARE_MSM_GPIO_PINS(21);
+DECLARE_MSM_GPIO_PINS(22);
+DECLARE_MSM_GPIO_PINS(23);
+DECLARE_MSM_GPIO_PINS(24);
+DECLARE_MSM_GPIO_PINS(25);
+DECLARE_MSM_GPIO_PINS(26);
+DECLARE_MSM_GPIO_PINS(27);
+DECLARE_MSM_GPIO_PINS(28);
+DECLARE_MSM_GPIO_PINS(29);
+DECLARE_MSM_GPIO_PINS(30);
+DECLARE_MSM_GPIO_PINS(31);
+DECLARE_MSM_GPIO_PINS(32);
+DECLARE_MSM_GPIO_PINS(33);
+DECLARE_MSM_GPIO_PINS(34);
+DECLARE_MSM_GPIO_PINS(35);
+DECLARE_MSM_GPIO_PINS(36);
+DECLARE_MSM_GPIO_PINS(37);
+DECLARE_MSM_GPIO_PINS(38);
+DECLARE_MSM_GPIO_PINS(39);
+DECLARE_MSM_GPIO_PINS(40);
+DECLARE_MSM_GPIO_PINS(41);
+DECLARE_MSM_GPIO_PINS(42);
+DECLARE_MSM_GPIO_PINS(43);
+DECLARE_MSM_GPIO_PINS(44);
+DECLARE_MSM_GPIO_PINS(45);
+DECLARE_MSM_GPIO_PINS(46);
+DECLARE_MSM_GPIO_PINS(47);
+DECLARE_MSM_GPIO_PINS(48);
+DECLARE_MSM_GPIO_PINS(49);
+DECLARE_MSM_GPIO_PINS(50);
+DECLARE_MSM_GPIO_PINS(51);
+DECLARE_MSM_GPIO_PINS(52);
+DECLARE_MSM_GPIO_PINS(53);
+DECLARE_MSM_GPIO_PINS(54);
+DECLARE_MSM_GPIO_PINS(55);
+DECLARE_MSM_GPIO_PINS(56);
+DECLARE_MSM_GPIO_PINS(57);
+DECLARE_MSM_GPIO_PINS(58);
+DECLARE_MSM_GPIO_PINS(59);
+DECLARE_MSM_GPIO_PINS(60);
+DECLARE_MSM_GPIO_PINS(61);
+DECLARE_MSM_GPIO_PINS(62);
+DECLARE_MSM_GPIO_PINS(63);
+DECLARE_MSM_GPIO_PINS(64);
+DECLARE_MSM_GPIO_PINS(65);
+DECLARE_MSM_GPIO_PINS(66);
+DECLARE_MSM_GPIO_PINS(67);
+DECLARE_MSM_GPIO_PINS(68);
+DECLARE_MSM_GPIO_PINS(69);
+
+enum ipq8074_functions {
+	msm_mux_atest_char,
+	msm_mux_atest_char0,
+	msm_mux_atest_char1,
+	msm_mux_atest_char2,
+	msm_mux_atest_char3,
+	msm_mux_audio_rxbclk,
+	msm_mux_audio_rxd,
+	msm_mux_audio_rxfsync,
+	msm_mux_audio_rxmclk,
+	msm_mux_audio_txbclk,
+	msm_mux_audio_txd,
+	msm_mux_audio_txfsync,
+	msm_mux_audio_txmclk,
+	msm_mux_blsp0_i2c,
+	msm_mux_blsp0_spi,
+	msm_mux_blsp0_uart,
+	msm_mux_blsp1_i2c,
+	msm_mux_blsp1_spi,
+	msm_mux_blsp1_uart,
+	msm_mux_blsp2_i2c,
+	msm_mux_blsp2_spi,
+	msm_mux_blsp2_uart,
+	msm_mux_blsp3_i2c,
+	msm_mux_blsp3_spi,
+	msm_mux_blsp3_spi0,
+	msm_mux_blsp3_spi1,
+	msm_mux_blsp3_spi2,
+	msm_mux_blsp3_spi3,
+	msm_mux_blsp3_uart,
+	msm_mux_blsp4_i2c0,
+	msm_mux_blsp4_i2c1,
+	msm_mux_blsp4_spi0,
+	msm_mux_blsp4_spi1,
+	msm_mux_blsp4_uart0,
+	msm_mux_blsp4_uart1,
+	msm_mux_blsp5_i2c,
+	msm_mux_blsp5_spi,
+	msm_mux_blsp5_uart,
+	msm_mux_burn0,
+	msm_mux_burn1,
+	msm_mux_cri_trng,
+	msm_mux_cri_trng0,
+	msm_mux_cri_trng1,
+	msm_mux_cxc0,
+	msm_mux_cxc1,
+	msm_mux_dbg_out,
+	msm_mux_gcc_plltest,
+	msm_mux_gcc_tlmm,
+	msm_mux_gpio,
+	msm_mux_ldo_en,
+	msm_mux_ldo_update,
+	msm_mux_led0,
+	msm_mux_led1,
+	msm_mux_led2,
+	msm_mux_mac0_sa0,
+	msm_mux_mac0_sa1,
+	msm_mux_mac1_sa0,
+	msm_mux_mac1_sa1,
+	msm_mux_mac1_sa2,
+	msm_mux_mac1_sa3,
+	msm_mux_mac2_sa0,
+	msm_mux_mac2_sa1,
+	msm_mux_mdc,
+	msm_mux_mdio,
+	msm_mux_pcie0_clk,
+	msm_mux_pcie0_rst,
+	msm_mux_pcie0_wake,
+	msm_mux_pcie1_clk,
+	msm_mux_pcie1_rst,
+	msm_mux_pcie1_wake,
+	msm_mux_pcm_drx,
+	msm_mux_pcm_dtx,
+	msm_mux_pcm_fsync,
+	msm_mux_pcm_pclk,
+	msm_mux_pcm_zsi0,
+	msm_mux_pcm_zsi1,
+	msm_mux_prng_rosc,
+	msm_mux_pta1_0,
+	msm_mux_pta1_1,
+	msm_mux_pta1_2,
+	msm_mux_pta2_0,
+	msm_mux_pta2_1,
+	msm_mux_pta2_2,
+	msm_mux_pwm0,
+	msm_mux_pwm1,
+	msm_mux_pwm2,
+	msm_mux_pwm3,
+	msm_mux_qdss_cti_trig_in_a0,
+	msm_mux_qdss_cti_trig_in_a1,
+	msm_mux_qdss_cti_trig_in_b0,
+	msm_mux_qdss_cti_trig_in_b1,
+	msm_mux_qdss_cti_trig_out_a0,
+	msm_mux_qdss_cti_trig_out_a1,
+	msm_mux_qdss_cti_trig_out_b0,
+	msm_mux_qdss_cti_trig_out_b1,
+	msm_mux_qdss_traceclk_a,
+	msm_mux_qdss_traceclk_b,
+	msm_mux_qdss_tracectl_a,
+	msm_mux_qdss_tracectl_b,
+	msm_mux_qdss_tracedata_a,
+	msm_mux_qdss_tracedata_b,
+	msm_mux_qpic,
+	msm_mux_rx0,
+	msm_mux_rx1,
+	msm_mux_rx2,
+	msm_mux_sd_card,
+	msm_mux_sd_write,
+	msm_mux_tsens_max,
+	msm_mux_wci2a,
+	msm_mux_wci2b,
+	msm_mux_wci2c,
+	msm_mux_wci2d,
+	msm_mux_NA,
+};
+
+static const char * const qpic_groups[] = {
+	"gpio0",	/* LCD_TE */
+	"gpio1",	/* BUSY_N */
+	"gpio2",	/* LCD_RS_N */
+	"gpio3",	/* WE_N */
+	"gpio4",	/* OE_N */
+	"gpio5",	/* DATA[0] */
+	"gpio6",	/* DATA[1] */
+	"gpio7",	/* DATA[2] */
+	"gpio8",	/* DATA[3] */
+	"gpio9",	/* CS_CSR_LCD */
+	"gpio10",	/* CLE */
+	"gpio11",	/* NAND_CS_N */
+	"gpio12",	/* DATA[4] */
+	"gpio13",	/* DATA[5] */
+	"gpio14",	/* DATA[6] */
+	"gpio15",	/* DATA[7] */
+	"gpio16",	/* DATA[8] */
+	"gpio17",	/* ALE */
+};
+
+static const char * const blsp5_i2c_groups[] = {
+	"gpio0", "gpio2",
+};
+
+static const char * const blsp5_spi_groups[] = {
+	"gpio0", "gpio2", "gpio9", "gpio16",
+};
+
+static const char * const wci2a_groups[] = {
+	"gpio0", "gpio2",
+};
+
+static const char * const blsp3_spi3_groups[] = {
+	"gpio0", "gpio2", "gpio9",
+};
+
+static const char * const burn0_groups[] = {
+	"gpio0",
+};
+
+static const char * const pcm_zsi0_groups[] = {
+	"gpio1",
+};
+
+static const char * const blsp5_uart_groups[] = {
+	"gpio0", "gpio2", "gpio9", "gpio16",
+};
+
+static const char * const mac1_sa2_groups[] = {
+	"gpio1", "gpio11",
+};
+
+static const char * const blsp3_spi0_groups[] = {
+	"gpio1", "gpio3", "gpio4",
+};
+
+static const char * const burn1_groups[] = {
+	"gpio1",
+};
+
+static const char * const mac0_sa1_groups[] = {
+	"gpio3", "gpio4",
+};
+
+static const char * const qdss_cti_trig_out_b0_groups[] = {
+	"gpio3",
+};
+
+static const char * const qdss_cti_trig_in_b0_groups[] = {
+	"gpio4",
+};
+
+static const char * const blsp4_uart0_groups[] = {
+	"gpio5", "gpio6", "gpio7", "gpio8",
+};
+
+static const char * const blsp4_i2c0_groups[] = {
+	"gpio5", "gpio6",
+};
+
+static const char * const blsp4_spi0_groups[] = {
+	"gpio5", "gpio6", "gpio7", "gpio8",
+};
+
+static const char * const mac2_sa1_groups[] = {
+	"gpio5", "gpio6",
+};
+
+static const char * const qdss_cti_trig_out_b1_groups[] = {
+	"gpio5",
+};
+
+static const char * const qdss_cti_trig_in_b1_groups[] = {
+	"gpio6",
+};
+
+static const char * const cxc0_groups[] = {
+	"gpio9", "gpio16",
+};
+
+static const char * const mac1_sa3_groups[] = {
+	"gpio9", "gpio16",
+};
+
+static const char * const qdss_cti_trig_in_a1_groups[] = {
+	"gpio9",
+};
+
+static const char * const qdss_cti_trig_out_a1_groups[] = {
+	"gpio10",
+};
+
+static const char * const wci2c_groups[] = {
+	"gpio11", "gpio17",
+};
+
+static const char * const qdss_cti_trig_in_a0_groups[] = {
+	"gpio11",
+};
+
+static const char * const qdss_cti_trig_out_a0_groups[] = {
+	"gpio12",
+};
+
+static const char * const qdss_traceclk_b_groups[] = {
+	"gpio14",
+};
+
+static const char * const qdss_tracectl_b_groups[] = {
+	"gpio15",
+};
+
+static const char * const pcm_zsi1_groups[] = {
+	"gpio16",
+};
+
+static const char * const qdss_tracedata_b_groups[] = {
+	"gpio16", "gpio17", "gpio18", "gpio19", "gpio20", "gpio21", "gpio22",
+	"gpio23", "gpio24", "gpio25", "gpio26", "gpio27", "gpio28", "gpio29",
+	"gpio30", "gpio31",
+};
+
+static const char * const led0_groups[] = {
+	"gpio18",
+};
+
+static const char * const pwm0_groups[] = {
+	"gpio18", "gpio21", "gpio25", "gpio29", "gpio63",
+};
+
+static const char * const led1_groups[] = {
+	"gpio19",
+};
+
+static const char * const pwm1_groups[] = {
+	"gpio19", "gpio22", "gpio26", "gpio30", "gpio64",
+};
+
+static const char * const led2_groups[] = {
+	"gpio20",
+};
+
+static const char * const pwm2_groups[] = {
+	"gpio20", "gpio23", "gpio27", "gpio31", "gpio66",
+};
+
+static const char * const blsp4_uart1_groups[] = {
+	"gpio21", "gpio22", "gpio23", "gpio24",
+};
+
+static const char * const blsp4_i2c1_groups[] = {
+	"gpio21", "gpio22",
+};
+
+static const char * const blsp4_spi1_groups[] = {
+	"gpio21", "gpio22", "gpio23", "gpio24",
+};
+
+static const char * const wci2d_groups[] = {
+	"gpio21", "gpio22",
+};
+
+static const char * const mac1_sa1_groups[] = {
+	"gpio21", "gpio22",
+};
+
+static const char * const blsp3_spi2_groups[] = {
+	"gpio21", "gpio22", "gpio23",
+};
+
+static const char * const pwm3_groups[] = {
+	"gpio24", "gpio28", "gpio32", "gpio67",
+};
+
+static const char * const audio_txmclk_groups[] = {
+	"gpio25",
+};
+
+static const char * const audio_txbclk_groups[] = {
+	"gpio26",
+};
+
+static const char * const audio_txfsync_groups[] = {
+	"gpio27",
+};
+
+static const char * const audio_txd_groups[] = {
+	"gpio28",
+};
+
+static const char * const audio_rxmclk_groups[] = {
+	"gpio29",
+};
+
+static const char * const atest_char0_groups[] = {
+	"gpio29",
+};
+
+static const char * const audio_rxbclk_groups[] = {
+	"gpio30",
+};
+
+static const char * const atest_char1_groups[] = {
+	"gpio30",
+};
+
+static const char * const audio_rxfsync_groups[] = {
+	"gpio31",
+};
+
+static const char * const atest_char2_groups[] = {
+	"gpio31",
+};
+
+static const char * const audio_rxd_groups[] = {
+	"gpio32",
+};
+
+static const char * const atest_char3_groups[] = {
+	"gpio32",
+};
+
+static const char * const pcm_drx_groups[] = {
+	"gpio33",
+};
+
+static const char * const mac1_sa0_groups[] = {
+	"gpio33", "gpio34",
+};
+
+static const char * const mac0_sa0_groups[] = {
+	"gpio33", "gpio34",
+};
+
+static const char * const pcm_dtx_groups[] = {
+	"gpio34",
+};
+
+static const char * const pcm_fsync_groups[] = {
+	"gpio35",
+};
+
+static const char * const mac2_sa0_groups[] = {
+	"gpio35", "gpio36",
+};
+
+static const char * const qdss_traceclk_a_groups[] = {
+	"gpio35",
+};
+
+static const char * const pcm_pclk_groups[] = {
+	"gpio36",
+};
+
+static const char * const qdss_tracectl_a_groups[] = {
+	"gpio36",
+};
+
+static const char * const atest_char_groups[] = {
+	"gpio37",
+};
+
+static const char * const qdss_tracedata_a_groups[] = {
+	"gpio37", "gpio38", "gpio39", "gpio40", "gpio41", "gpio42", "gpio43",
+	"gpio44", "gpio45", "gpio46", "gpio47", "gpio48", "gpio49", "gpio50",
+	"gpio51", "gpio52",
+};
+
+static const char * const blsp0_uart_groups[] = {
+	"gpio38", "gpio39", "gpio40", "gpio41",
+};
+
+static const char * const blsp0_i2c_groups[] = {
+	"gpio38", "gpio39",
+};
+
+static const char * const blsp0_spi_groups[] = {
+	"gpio38", "gpio39", "gpio40", "gpio41",
+};
+
+static const char * const blsp1_uart_groups[] = {
+	"gpio42", "gpio43", "gpio44", "gpio45",
+};
+
+static const char * const blsp1_i2c_groups[] = {
+	"gpio42", "gpio43",
+};
+
+static const char * const blsp1_spi_groups[] = {
+	"gpio42", "gpio43", "gpio44", "gpio45",
+};
+
+static const char * const blsp2_uart_groups[] = {
+	"gpio46", "gpio47", "gpio48", "gpio49",
+};
+
+static const char * const blsp2_i2c_groups[] = {
+	"gpio46", "gpio47",
+};
+
+static const char * const blsp2_spi_groups[] = {
+	"gpio46", "gpio47", "gpio48", "gpio49",
+};
+
+static const char * const blsp3_uart_groups[] = {
+	"gpio50", "gpio51", "gpio52", "gpio53",
+};
+
+static const char * const blsp3_i2c_groups[] = {
+	"gpio50", "gpio51",
+};
+
+static const char * const blsp3_spi_groups[] = {
+	"gpio50", "gpio51", "gpio52", "gpio53",
+};
+
+static const char * const pta2_0_groups[] = {
+	"gpio54",
+};
+
+static const char * const wci2b_groups[] = {
+	"gpio54", "gpio56",
+};
+
+static const char * const cxc1_groups[] = {
+	"gpio54", "gpio56",
+};
+
+static const char * const blsp3_spi1_groups[] = {
+	"gpio54", "gpio55", "gpio56",
+};
+
+static const char * const pta2_1_groups[] = {
+	"gpio55",
+};
+
+static const char * const pta2_2_groups[] = {
+	"gpio56",
+};
+
+static const char * const pcie0_clk_groups[] = {
+	"gpio57",
+};
+
+static const char * const dbg_out_groups[] = {
+	"gpio57",
+};
+
+static const char * const cri_trng0_groups[] = {
+	"gpio57",
+};
+
+static const char * const pcie0_rst_groups[] = {
+	"gpio58",
+};
+
+static const char * const cri_trng1_groups[] = {
+	"gpio58",
+};
+
+static const char * const pcie0_wake_groups[] = {
+	"gpio59",
+};
+
+static const char * const cri_trng_groups[] = {
+	"gpio59",
+};
+
+static const char * const pcie1_clk_groups[] = {
+	"gpio60",
+};
+
+static const char * const rx2_groups[] = {
+	"gpio60",
+};
+
+static const char * const ldo_update_groups[] = {
+	"gpio60",
+};
+
+static const char * const pcie1_rst_groups[] = {
+	"gpio61",
+};
+
+static const char * const ldo_en_groups[] = {
+	"gpio61",
+};
+
+static const char * const pcie1_wake_groups[] = {
+	"gpio62",
+};
+
+static const char * const gcc_plltest_groups[] = {
+	"gpio62", "gpio63",
+};
+
+static const char * const sd_card_groups[] = {
+	"gpio63",
+};
+
+static const char * const pta1_1_groups[] = {
+	"gpio64",
+};
+
+static const char * const rx1_groups[] = {
+	"gpio64",
+};
+
+static const char * const pta1_2_groups[] = {
+	"gpio65",
+};
+
+static const char * const gcc_tlmm_groups[] = {
+	"gpio65",
+};
+
+static const char * const pta1_0_groups[] = {
+	"gpio66",
+};
+
+static const char * const prng_rosc_groups[] = {
+	"gpio66",
+};
+
+static const char * const sd_write_groups[] = {
+	"gpio67",
+};
+
+static const char * const rx0_groups[] = {
+	"gpio67",
+};
+
+static const char * const tsens_max_groups[] = {
+	"gpio67",
+};
+
+static const char * const mdc_groups[] = {
+	"gpio68",
+};
+
+static const char * const mdio_groups[] = {
+	"gpio69",
+};
+
+static const char * const gpio_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3", "gpio4", "gpio5", "gpio6", "gpio7",
+	"gpio8", "gpio9", "gpio10", "gpio11", "gpio12", "gpio13", "gpio14",
+	"gpio15", "gpio16", "gpio17", "gpio18", "gpio19", "gpio20", "gpio21",
+	"gpio22", "gpio23", "gpio24", "gpio25", "gpio26", "gpio27", "gpio28",
+	"gpio29", "gpio30", "gpio31", "gpio32", "gpio33", "gpio34", "gpio35",
+	"gpio36", "gpio37", "gpio38", "gpio39", "gpio40", "gpio41", "gpio42",
+	"gpio43", "gpio44", "gpio45", "gpio46", "gpio47", "gpio48", "gpio49",
+	"gpio50", "gpio51", "gpio52", "gpio53", "gpio54", "gpio55", "gpio56",
+	"gpio57", "gpio58", "gpio59", "gpio60", "gpio61", "gpio62", "gpio63",
+	"gpio64", "gpio65", "gpio66", "gpio67", "gpio68", "gpio69",
+};
+
+static const struct msm_function ipq8074_functions[] = {
+	FUNCTION(atest_char),
+	FUNCTION(atest_char0),
+	FUNCTION(atest_char1),
+	FUNCTION(atest_char2),
+	FUNCTION(atest_char3),
+	FUNCTION(audio_rxbclk),
+	FUNCTION(audio_rxd),
+	FUNCTION(audio_rxfsync),
+	FUNCTION(audio_rxmclk),
+	FUNCTION(audio_txbclk),
+	FUNCTION(audio_txd),
+	FUNCTION(audio_txfsync),
+	FUNCTION(audio_txmclk),
+	FUNCTION(blsp0_i2c),
+	FUNCTION(blsp0_spi),
+	FUNCTION(blsp0_uart),
+	FUNCTION(blsp1_i2c),
+	FUNCTION(blsp1_spi),
+	FUNCTION(blsp1_uart),
+	FUNCTION(blsp2_i2c),
+	FUNCTION(blsp2_spi),
+	FUNCTION(blsp2_uart),
+	FUNCTION(blsp3_i2c),
+	FUNCTION(blsp3_spi),
+	FUNCTION(blsp3_spi0),
+	FUNCTION(blsp3_spi1),
+	FUNCTION(blsp3_spi2),
+	FUNCTION(blsp3_spi3),
+	FUNCTION(blsp3_uart),
+	FUNCTION(blsp4_i2c0),
+	FUNCTION(blsp4_i2c1),
+	FUNCTION(blsp4_spi0),
+	FUNCTION(blsp4_spi1),
+	FUNCTION(blsp4_uart0),
+	FUNCTION(blsp4_uart1),
+	FUNCTION(blsp5_i2c),
+	FUNCTION(blsp5_spi),
+	FUNCTION(blsp5_uart),
+	FUNCTION(burn0),
+	FUNCTION(burn1),
+	FUNCTION(cri_trng),
+	FUNCTION(cri_trng0),
+	FUNCTION(cri_trng1),
+	FUNCTION(cxc0),
+	FUNCTION(cxc1),
+	FUNCTION(dbg_out),
+	FUNCTION(gcc_plltest),
+	FUNCTION(gcc_tlmm),
+	FUNCTION(gpio),
+	FUNCTION(ldo_en),
+	FUNCTION(ldo_update),
+	FUNCTION(led0),
+	FUNCTION(led1),
+	FUNCTION(led2),
+	FUNCTION(mac0_sa0),
+	FUNCTION(mac0_sa1),
+	FUNCTION(mac1_sa0),
+	FUNCTION(mac1_sa1),
+	FUNCTION(mac1_sa2),
+	FUNCTION(mac1_sa3),
+	FUNCTION(mac2_sa0),
+	FUNCTION(mac2_sa1),
+	FUNCTION(mdc),
+	FUNCTION(mdio),
+	FUNCTION(pcie0_clk),
+	FUNCTION(pcie0_rst),
+	FUNCTION(pcie0_wake),
+	FUNCTION(pcie1_clk),
+	FUNCTION(pcie1_rst),
+	FUNCTION(pcie1_wake),
+	FUNCTION(pcm_drx),
+	FUNCTION(pcm_dtx),
+	FUNCTION(pcm_fsync),
+	FUNCTION(pcm_pclk),
+	FUNCTION(pcm_zsi0),
+	FUNCTION(pcm_zsi1),
+	FUNCTION(prng_rosc),
+	FUNCTION(pta1_0),
+	FUNCTION(pta1_1),
+	FUNCTION(pta1_2),
+	FUNCTION(pta2_0),
+	FUNCTION(pta2_1),
+	FUNCTION(pta2_2),
+	FUNCTION(pwm0),
+	FUNCTION(pwm1),
+	FUNCTION(pwm2),
+	FUNCTION(pwm3),
+	FUNCTION(qdss_cti_trig_in_a0),
+	FUNCTION(qdss_cti_trig_in_a1),
+	FUNCTION(qdss_cti_trig_in_b0),
+	FUNCTION(qdss_cti_trig_in_b1),
+	FUNCTION(qdss_cti_trig_out_a0),
+	FUNCTION(qdss_cti_trig_out_a1),
+	FUNCTION(qdss_cti_trig_out_b0),
+	FUNCTION(qdss_cti_trig_out_b1),
+	FUNCTION(qdss_traceclk_a),
+	FUNCTION(qdss_traceclk_b),
+	FUNCTION(qdss_tracectl_a),
+	FUNCTION(qdss_tracectl_b),
+	FUNCTION(qdss_tracedata_a),
+	FUNCTION(qdss_tracedata_b),
+	FUNCTION(qpic),
+	FUNCTION(rx0),
+	FUNCTION(rx1),
+	FUNCTION(rx2),
+	FUNCTION(sd_card),
+	FUNCTION(sd_write),
+	FUNCTION(tsens_max),
+	FUNCTION(wci2a),
+	FUNCTION(wci2b),
+	FUNCTION(wci2c),
+	FUNCTION(wci2d),
+};
+
+static const struct msm_pingroup ipq8074_groups[] = {
+	PINGROUP(0, qpic, blsp5_uart, blsp5_i2c, blsp5_spi, wci2a,
+		 blsp3_spi3, NA, burn0, NA),
+	PINGROUP(1, qpic, pcm_zsi0, mac1_sa2, blsp3_spi0, NA, burn1, NA, NA,
+		 NA),
+	PINGROUP(2, qpic, blsp5_uart, blsp5_i2c, blsp5_spi, wci2a,
+		 blsp3_spi3, NA, NA, NA),
+	PINGROUP(3, qpic, mac0_sa1, blsp3_spi0, qdss_cti_trig_out_b0, NA, NA,
+		 NA, NA, NA),
+	PINGROUP(4, qpic, mac0_sa1, blsp3_spi0, qdss_cti_trig_in_b0, NA, NA,
+		 NA, NA, NA),
+	PINGROUP(5, qpic, blsp4_uart0, blsp4_i2c0, blsp4_spi0, mac2_sa1,
+		 qdss_cti_trig_out_b1, NA, NA, NA),
+	PINGROUP(6, qpic, blsp4_uart0, blsp4_i2c0, blsp4_spi0, mac2_sa1,
+		 qdss_cti_trig_in_b1, NA, NA, NA),
+	PINGROUP(7, qpic, blsp4_uart0, blsp4_spi0, NA, NA, NA, NA, NA, NA),
+	PINGROUP(8, qpic, blsp4_uart0, blsp4_spi0, NA, NA, NA, NA, NA, NA),
+	PINGROUP(9, qpic, blsp5_uart, blsp5_spi, cxc0, mac1_sa3, blsp3_spi3,
+		 qdss_cti_trig_in_a1, NA, NA),
+	PINGROUP(10, qpic, qdss_cti_trig_out_a1, NA, NA, NA, NA, NA, NA,
+		 NA),
+	PINGROUP(11, qpic, wci2c, mac1_sa2, qdss_cti_trig_in_a0, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(12, qpic, qdss_cti_trig_out_a0, NA, NA, NA, NA, NA, NA,
+		 NA),
+	PINGROUP(13, qpic, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(14, qpic, qdss_traceclk_b, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(15, qpic, qdss_tracectl_b, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(16, qpic, blsp5_uart, pcm_zsi1, blsp5_spi, cxc0, mac1_sa3,
+		 qdss_tracedata_b, NA, NA),
+	PINGROUP(17, qpic, wci2c, qdss_tracedata_b, NA, NA, NA, NA, NA, NA),
+	PINGROUP(18, led0, pwm0, qdss_tracedata_b, NA, NA, NA, NA, NA, NA),
+	PINGROUP(19, led1, pwm1, NA, qdss_tracedata_b, NA, NA, NA, NA, NA),
+	PINGROUP(20, led2, pwm2, NA, qdss_tracedata_b, NA, NA, NA, NA, NA),
+	PINGROUP(21, pwm0, blsp4_uart1, blsp4_i2c1, blsp4_spi1, wci2d, mac1_sa1,
+		 blsp3_spi2, NA, qdss_tracedata_b),
+	PINGROUP(22, pwm1, blsp4_uart1, blsp4_i2c1, blsp4_spi1, wci2d, mac1_sa1,
+		 blsp3_spi2, NA, qdss_tracedata_b),
+	PINGROUP(23, pwm2, blsp4_uart1, blsp4_spi1, blsp3_spi2, NA,
+		 qdss_tracedata_b, NA, NA, NA),
+	PINGROUP(24, pwm3, blsp4_uart1, blsp4_spi1, NA, qdss_tracedata_b, NA,
+		 NA, NA, NA),
+	PINGROUP(25, audio_txmclk, pwm0, NA, qdss_tracedata_b, NA, NA, NA, NA,
+		 NA),
+	PINGROUP(26, audio_txbclk, pwm1, NA, qdss_tracedata_b, NA, NA, NA, NA,
+		 NA),
+	PINGROUP(27, audio_txfsync, pwm2, NA, qdss_tracedata_b, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(28, audio_txd, pwm3, NA, qdss_tracedata_b, NA, NA, NA, NA,
+		 NA),
+	PINGROUP(29, audio_rxmclk, pwm0, atest_char0, NA, qdss_tracedata_b,
+		 NA, NA, NA, NA),
+	PINGROUP(30, audio_rxbclk, pwm1, atest_char1, NA, qdss_tracedata_b,
+		 NA, NA, NA, NA),
+	PINGROUP(31, audio_rxfsync, pwm2, atest_char2, NA, qdss_tracedata_b,
+		 NA, NA, NA, NA),
+	PINGROUP(32, audio_rxd, pwm3, atest_char3, NA, NA, NA, NA, NA, NA),
+	PINGROUP(33, pcm_drx, mac1_sa0, mac0_sa0, NA, NA, NA, NA, NA, NA),
+	PINGROUP(34, pcm_dtx, mac1_sa0, mac0_sa0, NA, NA, NA, NA, NA, NA),
+	PINGROUP(35, pcm_fsync, mac2_sa0, qdss_traceclk_a, NA, NA, NA, NA, NA, NA),
+	PINGROUP(36, pcm_pclk, mac2_sa0, NA, qdss_tracectl_a, NA, NA, NA, NA, NA),
+	PINGROUP(37, atest_char, NA, qdss_tracedata_a, NA, NA, NA, NA, NA, NA),
+	PINGROUP(38, blsp0_uart, blsp0_i2c, blsp0_spi, NA, qdss_tracedata_a,
+		 NA, NA, NA, NA),
+	PINGROUP(39, blsp0_uart, blsp0_i2c, blsp0_spi, NA, qdss_tracedata_a,
+		 NA, NA, NA, NA),
+	PINGROUP(40, blsp0_uart, blsp0_spi, NA, qdss_tracedata_a, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(41, blsp0_uart, blsp0_spi, NA, qdss_tracedata_a, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(42, blsp1_uart, blsp1_i2c, blsp1_spi, NA, qdss_tracedata_a,
+		 NA, NA, NA, NA),
+	PINGROUP(43, blsp1_uart, blsp1_i2c, blsp1_spi, NA, qdss_tracedata_a,
+		 NA, NA, NA, NA),
+	PINGROUP(44, blsp1_uart, blsp1_spi, NA, qdss_tracedata_a, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(45, blsp1_uart, blsp1_spi, qdss_tracedata_a, NA, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(46, blsp2_uart, blsp2_i2c, blsp2_spi, qdss_tracedata_a, NA,
+		 NA, NA, NA, NA),
+	PINGROUP(47, blsp2_uart, blsp2_i2c, blsp2_spi, NA, qdss_tracedata_a,
+		 NA, NA, NA, NA),
+	PINGROUP(48, blsp2_uart, blsp2_spi, NA, qdss_tracedata_a, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(49, blsp2_uart, blsp2_spi, NA, qdss_tracedata_a, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(50, blsp3_uart, blsp3_i2c, blsp3_spi, NA, qdss_tracedata_a,
+		 NA, NA, NA, NA),
+	PINGROUP(51, blsp3_uart, blsp3_i2c, blsp3_spi, NA, qdss_tracedata_a,
+		 NA, NA, NA, NA),
+	PINGROUP(52, blsp3_uart, blsp3_spi, NA, qdss_tracedata_a, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(53, blsp3_uart, blsp3_spi, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(54, pta2_0, wci2b, cxc1, blsp3_spi1, NA, NA, NA, NA, NA),
+	PINGROUP(55, pta2_1, blsp3_spi1, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(56, pta2_2, wci2b, cxc1, blsp3_spi1, NA, NA, NA, NA, NA),
+	PINGROUP(57, pcie0_clk, NA, dbg_out, cri_trng0, NA, NA, NA, NA, NA),
+	PINGROUP(58, pcie0_rst, NA, cri_trng1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(59, pcie0_wake, NA, cri_trng, NA, NA, NA, NA, NA, NA),
+	PINGROUP(60, pcie1_clk, rx2, ldo_update, NA, NA, NA, NA, NA, NA),
+	PINGROUP(61, pcie1_rst, ldo_en, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(62, pcie1_wake, gcc_plltest, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(63, sd_card, pwm0, NA, gcc_plltest, NA, NA, NA, NA, NA),
+	PINGROUP(64, pta1_1, pwm1, NA, rx1, NA, NA, NA, NA, NA),
+	PINGROUP(65, pta1_2, NA, gcc_tlmm, NA, NA, NA, NA, NA, NA),
+	PINGROUP(66, pta1_0, pwm2, prng_rosc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(67, sd_write, pwm3, rx0, tsens_max, NA, NA, NA, NA, NA),
+	PINGROUP(68, mdc, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(69, mdio, NA, NA, NA, NA, NA, NA, NA, NA),
+};
+
+static const struct msm_pinctrl_soc_data ipq8074_pinctrl = {
+	.pins = ipq8074_pins,
+	.npins = ARRAY_SIZE(ipq8074_pins),
+	.functions = ipq8074_functions,
+	.nfunctions = ARRAY_SIZE(ipq8074_functions),
+	.groups = ipq8074_groups,
+	.ngroups = ARRAY_SIZE(ipq8074_groups),
+	.ngpios = 70,
+};
+
+static int ipq8074_pinctrl_probe(struct platform_device *pdev)
+{
+	return msm_pinctrl_probe(pdev, &ipq8074_pinctrl);
+}
+
+static const struct of_device_id ipq8074_pinctrl_of_match[] = {
+	{ .compatible = "qcom,ipq8074-pinctrl", },
+	{ },
+};
+
+static struct platform_driver ipq8074_pinctrl_driver = {
+	.driver = {
+		.name = "ipq8074-pinctrl",
+		.of_match_table = ipq8074_pinctrl_of_match,
+	},
+	.probe = ipq8074_pinctrl_probe,
+	.remove = msm_pinctrl_remove,
+};
+
+static int __init ipq8074_pinctrl_init(void)
+{
+	return platform_driver_register(&ipq8074_pinctrl_driver);
+}
+arch_initcall(ipq8074_pinctrl_init);
+
+static void __exit ipq8074_pinctrl_exit(void)
+{
+	platform_driver_unregister(&ipq8074_pinctrl_driver);
+}
+module_exit(ipq8074_pinctrl_exit);
+
+MODULE_DESCRIPTION("Qualcomm ipq8074 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, ipq8074_pinctrl_of_match);
diff --git a/drivers/pinctrl/samsung/Kconfig b/drivers/pinctrl/samsung/Kconfig
index d0461cd5d707..0357f9701eb9 100644
--- a/drivers/pinctrl/samsung/Kconfig
+++ b/drivers/pinctrl/samsung/Kconfig
@@ -10,6 +10,16 @@ config PINCTRL_EXYNOS
 	bool "Pinctrl driver data for Samsung EXYNOS SoCs other than 5440"
 	depends on OF && GPIOLIB && (ARCH_EXYNOS || ARCH_S5PV210)
 	select PINCTRL_SAMSUNG
+	select PINCTRL_EXYNOS_ARM if ARM && (ARCH_EXYNOS || ARCH_S5PV210)
+	select PINCTRL_EXYNOS_ARM64 if ARM64 && ARCH_EXYNOS
+
+config PINCTRL_EXYNOS_ARM
+	bool "ARMv7-specific pinctrl driver data for Exynos (except Exynos5440)" if COMPILE_TEST
+	depends on PINCTRL_EXYNOS
+
+config PINCTRL_EXYNOS_ARM64
+	bool "ARMv8-specific pinctrl driver data for Exynos" if COMPILE_TEST
+	depends on PINCTRL_EXYNOS
 
 config PINCTRL_EXYNOS5440
 	bool "Samsung EXYNOS5440 SoC pinctrl driver"
diff --git a/drivers/pinctrl/samsung/Makefile b/drivers/pinctrl/samsung/Makefile
index 70160c059edd..595995851ea5 100644
--- a/drivers/pinctrl/samsung/Makefile
+++ b/drivers/pinctrl/samsung/Makefile
@@ -2,6 +2,8 @@
 
 obj-$(CONFIG_PINCTRL_SAMSUNG)	+= pinctrl-samsung.o
 obj-$(CONFIG_PINCTRL_EXYNOS)	+= pinctrl-exynos.o
+obj-$(CONFIG_PINCTRL_EXYNOS_ARM)	+= pinctrl-exynos-arm.o
+obj-$(CONFIG_PINCTRL_EXYNOS_ARM64)	+= pinctrl-exynos-arm64.o
 obj-$(CONFIG_PINCTRL_EXYNOS5440)	+= pinctrl-exynos5440.o
 obj-$(CONFIG_PINCTRL_S3C24XX)	+= pinctrl-s3c24xx.o
 obj-$(CONFIG_PINCTRL_S3C64XX)	+= pinctrl-s3c64xx.o
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos-arm.c b/drivers/pinctrl/samsung/pinctrl-exynos-arm.c
new file mode 100644
index 000000000000..071084d3ee9c
--- /dev/null
+++ b/drivers/pinctrl/samsung/pinctrl-exynos-arm.c
@@ -0,0 +1,815 @@
+/*
+ * Exynos specific support for Samsung pinctrl/gpiolib driver with eint support.
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ * Copyright (c) 2012 Linaro Ltd
+ *		http://www.linaro.org
+ *
+ * Author: Thomas Abraham <thomas.ab@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 file contains the Samsung Exynos specific information required by the
+ * the Samsung pinctrl/gpiolib driver. It also includes the implementation of
+ * external gpio and wakeup interrupt support.
+ */
+
+#include <linux/device.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/soc/samsung/exynos-regs-pmu.h>
+
+#include "pinctrl-samsung.h"
+#include "pinctrl-exynos.h"
+
+static const struct samsung_pin_bank_type bank_type_off = {
+	.fld_width = { 4, 1, 2, 2, 2, 2, },
+	.reg_offset = { 0x00, 0x04, 0x08, 0x0c, 0x10, 0x14, },
+};
+
+static const struct samsung_pin_bank_type bank_type_alive = {
+	.fld_width = { 4, 1, 2, 2, },
+	.reg_offset = { 0x00, 0x04, 0x08, 0x0c, },
+};
+
+/* Retention control for S5PV210 are located at the end of clock controller */
+#define S5P_OTHERS 0xE000
+
+#define S5P_OTHERS_RET_IO		(1 << 31)
+#define S5P_OTHERS_RET_CF		(1 << 30)
+#define S5P_OTHERS_RET_MMC		(1 << 29)
+#define S5P_OTHERS_RET_UART		(1 << 28)
+
+static void s5pv210_retention_disable(struct samsung_pinctrl_drv_data *drvdata)
+{
+	void __iomem *clk_base = (void __iomem *)drvdata->retention_ctrl->priv;
+	u32 tmp;
+
+	tmp = __raw_readl(clk_base + S5P_OTHERS);
+	tmp |= (S5P_OTHERS_RET_IO | S5P_OTHERS_RET_CF | S5P_OTHERS_RET_MMC |
+		S5P_OTHERS_RET_UART);
+	__raw_writel(tmp, clk_base + S5P_OTHERS);
+}
+
+static struct samsung_retention_ctrl *
+s5pv210_retention_init(struct samsung_pinctrl_drv_data *drvdata,
+		       const struct samsung_retention_data *data)
+{
+	struct samsung_retention_ctrl *ctrl;
+	struct device_node *np;
+	void __iomem *clk_base;
+
+	ctrl = devm_kzalloc(drvdata->dev, sizeof(*ctrl), GFP_KERNEL);
+	if (!ctrl)
+		return ERR_PTR(-ENOMEM);
+
+	np = of_find_compatible_node(NULL, NULL, "samsung,s5pv210-clock");
+	if (!np) {
+		pr_err("%s: failed to find clock controller DT node\n",
+			__func__);
+		return ERR_PTR(-ENODEV);
+	}
+
+	clk_base = of_iomap(np, 0);
+	if (!clk_base) {
+		pr_err("%s: failed to map clock registers\n", __func__);
+		return ERR_PTR(-EINVAL);
+	}
+
+	ctrl->priv = (void __force *)clk_base;
+	ctrl->disable = s5pv210_retention_disable;
+
+	return ctrl;
+}
+
+static const struct samsung_retention_data s5pv210_retention_data __initconst = {
+	.init	 = s5pv210_retention_init,
+};
+
+/* pin banks of s5pv210 pin-controller */
+static const struct samsung_pin_bank_data s5pv210_pin_bank[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(4, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpb", 0x08),
+	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpc0", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpc1", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0a0, "gpd0", 0x14),
+	EXYNOS_PIN_BANK_EINTG(6, 0x0c0, "gpd1", 0x18),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0e0, "gpe0", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(5, 0x100, "gpe1", 0x20),
+	EXYNOS_PIN_BANK_EINTG(8, 0x120, "gpf0", 0x24),
+	EXYNOS_PIN_BANK_EINTG(8, 0x140, "gpf1", 0x28),
+	EXYNOS_PIN_BANK_EINTG(8, 0x160, "gpf2", 0x2c),
+	EXYNOS_PIN_BANK_EINTG(6, 0x180, "gpf3", 0x30),
+	EXYNOS_PIN_BANK_EINTG(7, 0x1a0, "gpg0", 0x34),
+	EXYNOS_PIN_BANK_EINTG(7, 0x1c0, "gpg1", 0x38),
+	EXYNOS_PIN_BANK_EINTG(7, 0x1e0, "gpg2", 0x3c),
+	EXYNOS_PIN_BANK_EINTG(7, 0x200, "gpg3", 0x40),
+	EXYNOS_PIN_BANK_EINTN(7, 0x220, "gpi"),
+	EXYNOS_PIN_BANK_EINTG(8, 0x240, "gpj0", 0x44),
+	EXYNOS_PIN_BANK_EINTG(6, 0x260, "gpj1", 0x48),
+	EXYNOS_PIN_BANK_EINTG(8, 0x280, "gpj2", 0x4c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x2a0, "gpj3", 0x50),
+	EXYNOS_PIN_BANK_EINTG(5, 0x2c0, "gpj4", 0x54),
+	EXYNOS_PIN_BANK_EINTN(8, 0x2e0, "mp01"),
+	EXYNOS_PIN_BANK_EINTN(4, 0x300, "mp02"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x320, "mp03"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x340, "mp04"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x360, "mp05"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x380, "mp06"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x3a0, "mp07"),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc00, "gph0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc20, "gph1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc40, "gph2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc60, "gph3", 0x0c),
+};
+
+const struct samsung_pin_ctrl s5pv210_pin_ctrl[] __initconst = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= s5pv210_pin_bank,
+		.nr_banks	= ARRAY_SIZE(s5pv210_pin_bank),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.eint_wkup_init = exynos_eint_wkup_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &s5pv210_retention_data,
+	},
+};
+
+/* Pad retention control code for accessing PMU regmap */
+static atomic_t exynos_shared_retention_refcnt;
+
+/* pin banks of exynos3250 pin-controller 0 */
+static const struct samsung_pin_bank_data exynos3250_pin_banks0[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpb",  0x08),
+	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpc0", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpc1", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0a0, "gpd0", 0x14),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0c0, "gpd1", 0x18),
+};
+
+/* pin banks of exynos3250 pin-controller 1 */
+static const struct samsung_pin_bank_data exynos3250_pin_banks1[] __initconst = {
+	EXYNOS_PIN_BANK_EINTN(8, 0x120, "gpe0"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x140, "gpe1"),
+	EXYNOS_PIN_BANK_EINTN(3, 0x180, "gpe2"),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpk0", 0x08),
+	EXYNOS_PIN_BANK_EINTG(7, 0x060, "gpk1", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(7, 0x080, "gpk2", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0c0, "gpl0", 0x18),
+	EXYNOS_PIN_BANK_EINTG(8, 0x260, "gpm0", 0x24),
+	EXYNOS_PIN_BANK_EINTG(7, 0x280, "gpm1", 0x28),
+	EXYNOS_PIN_BANK_EINTG(5, 0x2a0, "gpm2", 0x2c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x2c0, "gpm3", 0x30),
+	EXYNOS_PIN_BANK_EINTG(8, 0x2e0, "gpm4", 0x34),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc00, "gpx0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc20, "gpx1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc40, "gpx2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc60, "gpx3", 0x0c),
+};
+
+/*
+ * PMU pad retention groups for Exynos3250 doesn't match pin banks, so handle
+ * them all together
+ */
+static const u32 exynos3250_retention_regs[] = {
+	S5P_PAD_RET_MAUDIO_OPTION,
+	S5P_PAD_RET_GPIO_OPTION,
+	S5P_PAD_RET_UART_OPTION,
+	S5P_PAD_RET_MMCA_OPTION,
+	S5P_PAD_RET_MMCB_OPTION,
+	S5P_PAD_RET_EBIA_OPTION,
+	S5P_PAD_RET_EBIB_OPTION,
+	S5P_PAD_RET_MMC2_OPTION,
+	S5P_PAD_RET_SPI_OPTION,
+};
+
+static const struct samsung_retention_data exynos3250_retention_data __initconst = {
+	.regs	 = exynos3250_retention_regs,
+	.nr_regs = ARRAY_SIZE(exynos3250_retention_regs),
+	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
+	.refcnt	 = &exynos_shared_retention_refcnt,
+	.init	 = exynos_retention_init,
+};
+
+/*
+ * Samsung pinctrl driver data for Exynos3250 SoC. Exynos3250 SoC includes
+ * two gpio/pin-mux/pinconfig controllers.
+ */
+const struct samsung_pin_ctrl exynos3250_pin_ctrl[] __initconst = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= exynos3250_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos3250_pin_banks0),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos3250_retention_data,
+	}, {
+		/* pin-controller instance 1 data */
+		.pin_banks	= exynos3250_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos3250_pin_banks1),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.eint_wkup_init = exynos_eint_wkup_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos3250_retention_data,
+	},
+};
+
+/* pin banks of exynos4210 pin-controller 0 */
+static const struct samsung_pin_bank_data exynos4210_pin_banks0[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpb", 0x08),
+	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpc0", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpc1", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0A0, "gpd0", 0x14),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0C0, "gpd1", 0x18),
+	EXYNOS_PIN_BANK_EINTG(5, 0x0E0, "gpe0", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x100, "gpe1", 0x20),
+	EXYNOS_PIN_BANK_EINTG(6, 0x120, "gpe2", 0x24),
+	EXYNOS_PIN_BANK_EINTG(8, 0x140, "gpe3", 0x28),
+	EXYNOS_PIN_BANK_EINTG(8, 0x160, "gpe4", 0x2c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x180, "gpf0", 0x30),
+	EXYNOS_PIN_BANK_EINTG(8, 0x1A0, "gpf1", 0x34),
+	EXYNOS_PIN_BANK_EINTG(8, 0x1C0, "gpf2", 0x38),
+	EXYNOS_PIN_BANK_EINTG(6, 0x1E0, "gpf3", 0x3c),
+};
+
+/* pin banks of exynos4210 pin-controller 1 */
+static const struct samsung_pin_bank_data exynos4210_pin_banks1[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpj0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(5, 0x020, "gpj1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(7, 0x040, "gpk0", 0x08),
+	EXYNOS_PIN_BANK_EINTG(7, 0x060, "gpk1", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(7, 0x080, "gpk2", 0x10),
+	EXYNOS_PIN_BANK_EINTG(7, 0x0A0, "gpk3", 0x14),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0C0, "gpl0", 0x18),
+	EXYNOS_PIN_BANK_EINTG(3, 0x0E0, "gpl1", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x100, "gpl2", 0x20),
+	EXYNOS_PIN_BANK_EINTN(6, 0x120, "gpy0"),
+	EXYNOS_PIN_BANK_EINTN(4, 0x140, "gpy1"),
+	EXYNOS_PIN_BANK_EINTN(6, 0x160, "gpy2"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x180, "gpy3"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x1A0, "gpy4"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x1C0, "gpy5"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x1E0, "gpy6"),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC00, "gpx0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC20, "gpx1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC40, "gpx2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC60, "gpx3", 0x0c),
+};
+
+/* pin banks of exynos4210 pin-controller 2 */
+static const struct samsung_pin_bank_data exynos4210_pin_banks2[] __initconst = {
+	EXYNOS_PIN_BANK_EINTN(7, 0x000, "gpz"),
+};
+
+/* PMU pad retention groups registers for Exynos4 (without audio) */
+static const u32 exynos4_retention_regs[] = {
+	S5P_PAD_RET_GPIO_OPTION,
+	S5P_PAD_RET_UART_OPTION,
+	S5P_PAD_RET_MMCA_OPTION,
+	S5P_PAD_RET_MMCB_OPTION,
+	S5P_PAD_RET_EBIA_OPTION,
+	S5P_PAD_RET_EBIB_OPTION,
+};
+
+static const struct samsung_retention_data exynos4_retention_data __initconst = {
+	.regs	 = exynos4_retention_regs,
+	.nr_regs = ARRAY_SIZE(exynos4_retention_regs),
+	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
+	.refcnt	 = &exynos_shared_retention_refcnt,
+	.init	 = exynos_retention_init,
+};
+
+/* PMU retention control for audio pins can be tied to audio pin bank */
+static const u32 exynos4_audio_retention_regs[] = {
+	S5P_PAD_RET_MAUDIO_OPTION,
+};
+
+static const struct samsung_retention_data exynos4_audio_retention_data __initconst = {
+	.regs	 = exynos4_audio_retention_regs,
+	.nr_regs = ARRAY_SIZE(exynos4_audio_retention_regs),
+	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
+	.init	 = exynos_retention_init,
+};
+
+/*
+ * Samsung pinctrl driver data for Exynos4210 SoC. Exynos4210 SoC includes
+ * three gpio/pin-mux/pinconfig controllers.
+ */
+const struct samsung_pin_ctrl exynos4210_pin_ctrl[] __initconst = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= exynos4210_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos4210_pin_banks0),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos4_retention_data,
+	}, {
+		/* pin-controller instance 1 data */
+		.pin_banks	= exynos4210_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos4210_pin_banks1),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.eint_wkup_init = exynos_eint_wkup_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos4_retention_data,
+	}, {
+		/* pin-controller instance 2 data */
+		.pin_banks	= exynos4210_pin_banks2,
+		.nr_banks	= ARRAY_SIZE(exynos4210_pin_banks2),
+		.retention_data	= &exynos4_audio_retention_data,
+	},
+};
+
+/* pin banks of exynos4x12 pin-controller 0 */
+static const struct samsung_pin_bank_data exynos4x12_pin_banks0[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpb", 0x08),
+	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpc0", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpc1", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0A0, "gpd0", 0x14),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0C0, "gpd1", 0x18),
+	EXYNOS_PIN_BANK_EINTG(8, 0x180, "gpf0", 0x30),
+	EXYNOS_PIN_BANK_EINTG(8, 0x1A0, "gpf1", 0x34),
+	EXYNOS_PIN_BANK_EINTG(8, 0x1C0, "gpf2", 0x38),
+	EXYNOS_PIN_BANK_EINTG(6, 0x1E0, "gpf3", 0x3c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x240, "gpj0", 0x40),
+	EXYNOS_PIN_BANK_EINTG(5, 0x260, "gpj1", 0x44),
+};
+
+/* pin banks of exynos4x12 pin-controller 1 */
+static const struct samsung_pin_bank_data exynos4x12_pin_banks1[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(7, 0x040, "gpk0", 0x08),
+	EXYNOS_PIN_BANK_EINTG(7, 0x060, "gpk1", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(7, 0x080, "gpk2", 0x10),
+	EXYNOS_PIN_BANK_EINTG(7, 0x0A0, "gpk3", 0x14),
+	EXYNOS_PIN_BANK_EINTG(7, 0x0C0, "gpl0", 0x18),
+	EXYNOS_PIN_BANK_EINTG(2, 0x0E0, "gpl1", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x100, "gpl2", 0x20),
+	EXYNOS_PIN_BANK_EINTG(8, 0x260, "gpm0", 0x24),
+	EXYNOS_PIN_BANK_EINTG(7, 0x280, "gpm1", 0x28),
+	EXYNOS_PIN_BANK_EINTG(5, 0x2A0, "gpm2", 0x2c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x2C0, "gpm3", 0x30),
+	EXYNOS_PIN_BANK_EINTG(8, 0x2E0, "gpm4", 0x34),
+	EXYNOS_PIN_BANK_EINTN(6, 0x120, "gpy0"),
+	EXYNOS_PIN_BANK_EINTN(4, 0x140, "gpy1"),
+	EXYNOS_PIN_BANK_EINTN(6, 0x160, "gpy2"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x180, "gpy3"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x1A0, "gpy4"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x1C0, "gpy5"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x1E0, "gpy6"),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC00, "gpx0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC20, "gpx1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC40, "gpx2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC60, "gpx3", 0x0c),
+};
+
+/* pin banks of exynos4x12 pin-controller 2 */
+static const struct samsung_pin_bank_data exynos4x12_pin_banks2[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz", 0x00),
+};
+
+/* pin banks of exynos4x12 pin-controller 3 */
+static const struct samsung_pin_bank_data exynos4x12_pin_banks3[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpv0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpv1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpv2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpv3", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(2, 0x080, "gpv4", 0x10),
+};
+
+/*
+ * Samsung pinctrl driver data for Exynos4x12 SoC. Exynos4x12 SoC includes
+ * four gpio/pin-mux/pinconfig controllers.
+ */
+const struct samsung_pin_ctrl exynos4x12_pin_ctrl[] __initconst = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= exynos4x12_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos4x12_pin_banks0),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos4_retention_data,
+	}, {
+		/* pin-controller instance 1 data */
+		.pin_banks	= exynos4x12_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos4x12_pin_banks1),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.eint_wkup_init = exynos_eint_wkup_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos4_retention_data,
+	}, {
+		/* pin-controller instance 2 data */
+		.pin_banks	= exynos4x12_pin_banks2,
+		.nr_banks	= ARRAY_SIZE(exynos4x12_pin_banks2),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos4_audio_retention_data,
+	}, {
+		/* pin-controller instance 3 data */
+		.pin_banks	= exynos4x12_pin_banks3,
+		.nr_banks	= ARRAY_SIZE(exynos4x12_pin_banks3),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	},
+};
+
+/* pin banks of exynos5250 pin-controller 0 */
+static const struct samsung_pin_bank_data exynos5250_pin_banks0[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpa2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpb0", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpb1", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0A0, "gpb2", 0x14),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0C0, "gpb3", 0x18),
+	EXYNOS_PIN_BANK_EINTG(7, 0x0E0, "gpc0", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(4, 0x100, "gpc1", 0x20),
+	EXYNOS_PIN_BANK_EINTG(7, 0x120, "gpc2", 0x24),
+	EXYNOS_PIN_BANK_EINTG(7, 0x140, "gpc3", 0x28),
+	EXYNOS_PIN_BANK_EINTG(4, 0x160, "gpd0", 0x2c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x180, "gpd1", 0x30),
+	EXYNOS_PIN_BANK_EINTG(7, 0x2E0, "gpc4", 0x34),
+	EXYNOS_PIN_BANK_EINTN(6, 0x1A0, "gpy0"),
+	EXYNOS_PIN_BANK_EINTN(4, 0x1C0, "gpy1"),
+	EXYNOS_PIN_BANK_EINTN(6, 0x1E0, "gpy2"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x200, "gpy3"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x220, "gpy4"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x240, "gpy5"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x260, "gpy6"),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC00, "gpx0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC20, "gpx1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC40, "gpx2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC60, "gpx3", 0x0c),
+};
+
+/* pin banks of exynos5250 pin-controller 1 */
+static const struct samsung_pin_bank_data exynos5250_pin_banks1[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpe0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(2, 0x020, "gpe1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(4, 0x040, "gpf0", 0x08),
+	EXYNOS_PIN_BANK_EINTG(4, 0x060, "gpf1", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x080, "gpg0", 0x10),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0A0, "gpg1", 0x14),
+	EXYNOS_PIN_BANK_EINTG(2, 0x0C0, "gpg2", 0x18),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0E0, "gph0", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x100, "gph1", 0x20),
+};
+
+/* pin banks of exynos5250 pin-controller 2 */
+static const struct samsung_pin_bank_data exynos5250_pin_banks2[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpv0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpv1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpv2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(8, 0x080, "gpv3", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(2, 0x0C0, "gpv4", 0x10),
+};
+
+/* pin banks of exynos5250 pin-controller 3 */
+static const struct samsung_pin_bank_data exynos5250_pin_banks3[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz", 0x00),
+};
+
+/*
+ * Samsung pinctrl driver data for Exynos5250 SoC. Exynos5250 SoC includes
+ * four gpio/pin-mux/pinconfig controllers.
+ */
+const struct samsung_pin_ctrl exynos5250_pin_ctrl[] __initconst = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= exynos5250_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos5250_pin_banks0),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.eint_wkup_init = exynos_eint_wkup_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos4_retention_data,
+	}, {
+		/* pin-controller instance 1 data */
+		.pin_banks	= exynos5250_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos5250_pin_banks1),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos4_retention_data,
+	}, {
+		/* pin-controller instance 2 data */
+		.pin_banks	= exynos5250_pin_banks2,
+		.nr_banks	= ARRAY_SIZE(exynos5250_pin_banks2),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 3 data */
+		.pin_banks	= exynos5250_pin_banks3,
+		.nr_banks	= ARRAY_SIZE(exynos5250_pin_banks3),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos4_audio_retention_data,
+	},
+};
+
+/* pin banks of exynos5260 pin-controller 0 */
+static const struct samsung_pin_bank_data exynos5260_pin_banks0[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(4, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(7, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpa2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpb0", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(4, 0x080, "gpb1", 0x10),
+	EXYNOS_PIN_BANK_EINTG(5, 0x0a0, "gpb2", 0x14),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0c0, "gpb3", 0x18),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0e0, "gpb4", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x100, "gpb5", 0x20),
+	EXYNOS_PIN_BANK_EINTG(8, 0x120, "gpd0", 0x24),
+	EXYNOS_PIN_BANK_EINTG(7, 0x140, "gpd1", 0x28),
+	EXYNOS_PIN_BANK_EINTG(5, 0x160, "gpd2", 0x2c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x180, "gpe0", 0x30),
+	EXYNOS_PIN_BANK_EINTG(5, 0x1a0, "gpe1", 0x34),
+	EXYNOS_PIN_BANK_EINTG(4, 0x1c0, "gpf0", 0x38),
+	EXYNOS_PIN_BANK_EINTG(8, 0x1e0, "gpf1", 0x3c),
+	EXYNOS_PIN_BANK_EINTG(2, 0x200, "gpk0", 0x40),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc00, "gpx0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc20, "gpx1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc40, "gpx2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc60, "gpx3", 0x0c),
+};
+
+/* pin banks of exynos5260 pin-controller 1 */
+static const struct samsung_pin_bank_data exynos5260_pin_banks1[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpc0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpc1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(7, 0x040, "gpc2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(4, 0x060, "gpc3", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(4, 0x080, "gpc4", 0x10),
+};
+
+/* pin banks of exynos5260 pin-controller 2 */
+static const struct samsung_pin_bank_data exynos5260_pin_banks2[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(4, 0x020, "gpz1", 0x04),
+};
+
+/*
+ * Samsung pinctrl driver data for Exynos5260 SoC. Exynos5260 SoC includes
+ * three gpio/pin-mux/pinconfig controllers.
+ */
+const struct samsung_pin_ctrl exynos5260_pin_ctrl[] __initconst = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= exynos5260_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos5260_pin_banks0),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.eint_wkup_init = exynos_eint_wkup_init,
+	}, {
+		/* pin-controller instance 1 data */
+		.pin_banks	= exynos5260_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos5260_pin_banks1),
+		.eint_gpio_init = exynos_eint_gpio_init,
+	}, {
+		/* pin-controller instance 2 data */
+		.pin_banks	= exynos5260_pin_banks2,
+		.nr_banks	= ARRAY_SIZE(exynos5260_pin_banks2),
+		.eint_gpio_init = exynos_eint_gpio_init,
+	},
+};
+
+/* pin banks of exynos5410 pin-controller 0 */
+static const struct samsung_pin_bank_data exynos5410_pin_banks0[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpa2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpb0", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpb1", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0A0, "gpb2", 0x14),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0C0, "gpb3", 0x18),
+	EXYNOS_PIN_BANK_EINTG(7, 0x0E0, "gpc0", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(4, 0x100, "gpc3", 0x20),
+	EXYNOS_PIN_BANK_EINTG(7, 0x120, "gpc1", 0x24),
+	EXYNOS_PIN_BANK_EINTG(7, 0x140, "gpc2", 0x28),
+	EXYNOS_PIN_BANK_EINTN(2, 0x160, "gpm5"),
+	EXYNOS_PIN_BANK_EINTG(8, 0x180, "gpd1", 0x2c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x1A0, "gpe0", 0x30),
+	EXYNOS_PIN_BANK_EINTG(2, 0x1C0, "gpe1", 0x34),
+	EXYNOS_PIN_BANK_EINTG(6, 0x1E0, "gpf0", 0x38),
+	EXYNOS_PIN_BANK_EINTG(8, 0x200, "gpf1", 0x3c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x220, "gpg0", 0x40),
+	EXYNOS_PIN_BANK_EINTG(8, 0x240, "gpg1", 0x44),
+	EXYNOS_PIN_BANK_EINTG(2, 0x260, "gpg2", 0x48),
+	EXYNOS_PIN_BANK_EINTG(4, 0x280, "gph0", 0x4c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x2A0, "gph1", 0x50),
+	EXYNOS_PIN_BANK_EINTN(8, 0x2C0, "gpm7"),
+	EXYNOS_PIN_BANK_EINTN(6, 0x2E0, "gpy0"),
+	EXYNOS_PIN_BANK_EINTN(4, 0x300, "gpy1"),
+	EXYNOS_PIN_BANK_EINTN(6, 0x320, "gpy2"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x340, "gpy3"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x360, "gpy4"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x380, "gpy5"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x3A0, "gpy6"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x3C0, "gpy7"),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC00, "gpx0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC20, "gpx1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC40, "gpx2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC60, "gpx3", 0x0c),
+};
+
+/* pin banks of exynos5410 pin-controller 1 */
+static const struct samsung_pin_bank_data exynos5410_pin_banks1[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(5, 0x000, "gpj0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpj1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpj2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpj3", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(2, 0x080, "gpj4", 0x10),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0A0, "gpk0", 0x14),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0C0, "gpk1", 0x18),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0E0, "gpk2", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(7, 0x100, "gpk3", 0x20),
+};
+
+/* pin banks of exynos5410 pin-controller 2 */
+static const struct samsung_pin_bank_data exynos5410_pin_banks2[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpv0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpv1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpv2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(8, 0x080, "gpv3", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(2, 0x0C0, "gpv4", 0x10),
+};
+
+/* pin banks of exynos5410 pin-controller 3 */
+static const struct samsung_pin_bank_data exynos5410_pin_banks3[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz", 0x00),
+};
+
+/*
+ * Samsung pinctrl driver data for Exynos5410 SoC. Exynos5410 SoC includes
+ * four gpio/pin-mux/pinconfig controllers.
+ */
+const struct samsung_pin_ctrl exynos5410_pin_ctrl[] __initconst = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= exynos5410_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos5410_pin_banks0),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.eint_wkup_init = exynos_eint_wkup_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 1 data */
+		.pin_banks	= exynos5410_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos5410_pin_banks1),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 2 data */
+		.pin_banks	= exynos5410_pin_banks2,
+		.nr_banks	= ARRAY_SIZE(exynos5410_pin_banks2),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 3 data */
+		.pin_banks	= exynos5410_pin_banks3,
+		.nr_banks	= ARRAY_SIZE(exynos5410_pin_banks3),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	},
+};
+
+/* pin banks of exynos5420 pin-controller 0 */
+static const struct samsung_pin_bank_data exynos5420_pin_banks0[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpy7", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC00, "gpx0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC20, "gpx1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC40, "gpx2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC60, "gpx3", 0x0c),
+};
+
+/* pin banks of exynos5420 pin-controller 1 */
+static const struct samsung_pin_bank_data exynos5420_pin_banks1[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpc0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpc1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(7, 0x040, "gpc2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(4, 0x060, "gpc3", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(2, 0x080, "gpc4", 0x10),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0A0, "gpd1", 0x14),
+	EXYNOS_PIN_BANK_EINTN(6, 0x0C0, "gpy0"),
+	EXYNOS_PIN_BANK_EINTN(4, 0x0E0, "gpy1"),
+	EXYNOS_PIN_BANK_EINTN(6, 0x100, "gpy2"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x120, "gpy3"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x140, "gpy4"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x160, "gpy5"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x180, "gpy6"),
+};
+
+/* pin banks of exynos5420 pin-controller 2 */
+static const struct samsung_pin_bank_data exynos5420_pin_banks2[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpe0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(2, 0x020, "gpe1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(6, 0x040, "gpf0", 0x08),
+	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpf1", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x080, "gpg0", 0x10),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0A0, "gpg1", 0x14),
+	EXYNOS_PIN_BANK_EINTG(2, 0x0C0, "gpg2", 0x18),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0E0, "gpj4", 0x1c),
+};
+
+/* pin banks of exynos5420 pin-controller 3 */
+static const struct samsung_pin_bank_data exynos5420_pin_banks3[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpa2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpb0", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpb1", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0A0, "gpb2", 0x14),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0C0, "gpb3", 0x18),
+	EXYNOS_PIN_BANK_EINTG(2, 0x0E0, "gpb4", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x100, "gph0", 0x20),
+};
+
+/* pin banks of exynos5420 pin-controller 4 */
+static const struct samsung_pin_bank_data exynos5420_pin_banks4[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz", 0x00),
+};
+
+/* PMU pad retention groups registers for Exynos5420 (without audio) */
+static const u32 exynos5420_retention_regs[] = {
+	EXYNOS_PAD_RET_DRAM_OPTION,
+	EXYNOS_PAD_RET_JTAG_OPTION,
+	EXYNOS5420_PAD_RET_GPIO_OPTION,
+	EXYNOS5420_PAD_RET_UART_OPTION,
+	EXYNOS5420_PAD_RET_MMCA_OPTION,
+	EXYNOS5420_PAD_RET_MMCB_OPTION,
+	EXYNOS5420_PAD_RET_MMCC_OPTION,
+	EXYNOS5420_PAD_RET_HSI_OPTION,
+	EXYNOS_PAD_RET_EBIA_OPTION,
+	EXYNOS_PAD_RET_EBIB_OPTION,
+	EXYNOS5420_PAD_RET_SPI_OPTION,
+	EXYNOS5420_PAD_RET_DRAM_COREBLK_OPTION,
+};
+
+static const struct samsung_retention_data exynos5420_retention_data __initconst = {
+	.regs	 = exynos5420_retention_regs,
+	.nr_regs = ARRAY_SIZE(exynos5420_retention_regs),
+	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
+	.refcnt	 = &exynos_shared_retention_refcnt,
+	.init	 = exynos_retention_init,
+};
+
+/*
+ * Samsung pinctrl driver data for Exynos5420 SoC. Exynos5420 SoC includes
+ * four gpio/pin-mux/pinconfig controllers.
+ */
+const struct samsung_pin_ctrl exynos5420_pin_ctrl[] __initconst = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= exynos5420_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos5420_pin_banks0),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.eint_wkup_init = exynos_eint_wkup_init,
+		.retention_data	= &exynos5420_retention_data,
+	}, {
+		/* pin-controller instance 1 data */
+		.pin_banks	= exynos5420_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos5420_pin_banks1),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.retention_data	= &exynos5420_retention_data,
+	}, {
+		/* pin-controller instance 2 data */
+		.pin_banks	= exynos5420_pin_banks2,
+		.nr_banks	= ARRAY_SIZE(exynos5420_pin_banks2),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.retention_data	= &exynos5420_retention_data,
+	}, {
+		/* pin-controller instance 3 data */
+		.pin_banks	= exynos5420_pin_banks3,
+		.nr_banks	= ARRAY_SIZE(exynos5420_pin_banks3),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.retention_data	= &exynos5420_retention_data,
+	}, {
+		/* pin-controller instance 4 data */
+		.pin_banks	= exynos5420_pin_banks4,
+		.nr_banks	= ARRAY_SIZE(exynos5420_pin_banks4),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.retention_data	= &exynos4_audio_retention_data,
+	},
+};
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos-arm64.c b/drivers/pinctrl/samsung/pinctrl-exynos-arm64.c
new file mode 100644
index 000000000000..08e9fdb58fd2
--- /dev/null
+++ b/drivers/pinctrl/samsung/pinctrl-exynos-arm64.c
@@ -0,0 +1,399 @@
+/*
+ * Exynos ARMv8 specific support for Samsung pinctrl/gpiolib driver
+ * with eint support.
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ * Copyright (c) 2012 Linaro Ltd
+ *		http://www.linaro.org
+ * Copyright (c) 2017 Krzysztof Kozlowski <krzk@kernel.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This file contains the Samsung Exynos specific information required by the
+ * the Samsung pinctrl/gpiolib driver. It also includes the implementation of
+ * external gpio and wakeup interrupt support.
+ */
+
+#include <linux/slab.h>
+#include <linux/soc/samsung/exynos-regs-pmu.h>
+
+#include "pinctrl-samsung.h"
+#include "pinctrl-exynos.h"
+
+static const struct samsung_pin_bank_type bank_type_off = {
+	.fld_width = { 4, 1, 2, 2, 2, 2, },
+	.reg_offset = { 0x00, 0x04, 0x08, 0x0c, 0x10, 0x14, },
+};
+
+static const struct samsung_pin_bank_type bank_type_alive = {
+	.fld_width = { 4, 1, 2, 2, },
+	.reg_offset = { 0x00, 0x04, 0x08, 0x0c, },
+};
+
+/* Exynos5433 has the 4bit widths for PINCFG_TYPE_DRV bitfields. */
+static const struct samsung_pin_bank_type exynos5433_bank_type_off = {
+	.fld_width = { 4, 1, 2, 4, 2, 2, },
+	.reg_offset = { 0x00, 0x04, 0x08, 0x0c, 0x10, 0x14, },
+};
+
+static const struct samsung_pin_bank_type exynos5433_bank_type_alive = {
+	.fld_width = { 4, 1, 2, 4, },
+	.reg_offset = { 0x00, 0x04, 0x08, 0x0c, },
+};
+
+/* Pad retention control code for accessing PMU regmap */
+static atomic_t exynos_shared_retention_refcnt;
+
+/* pin banks of exynos5433 pin-controller - ALIVE */
+static const struct samsung_pin_bank_data exynos5433_pin_banks0[] __initconst = {
+	EXYNOS5433_PIN_BANK_EINTW(8, 0x000, "gpa0", 0x00),
+	EXYNOS5433_PIN_BANK_EINTW(8, 0x020, "gpa1", 0x04),
+	EXYNOS5433_PIN_BANK_EINTW(8, 0x040, "gpa2", 0x08),
+	EXYNOS5433_PIN_BANK_EINTW(8, 0x060, "gpa3", 0x0c),
+	EXYNOS5433_PIN_BANK_EINTW_EXT(8, 0x020, "gpf1", 0x1004, 1),
+	EXYNOS5433_PIN_BANK_EINTW_EXT(4, 0x040, "gpf2", 0x1008, 1),
+	EXYNOS5433_PIN_BANK_EINTW_EXT(4, 0x060, "gpf3", 0x100c, 1),
+	EXYNOS5433_PIN_BANK_EINTW_EXT(8, 0x080, "gpf4", 0x1010, 1),
+	EXYNOS5433_PIN_BANK_EINTW_EXT(8, 0x0a0, "gpf5", 0x1014, 1),
+};
+
+/* pin banks of exynos5433 pin-controller - AUD */
+static const struct samsung_pin_bank_data exynos5433_pin_banks1[] __initconst = {
+	EXYNOS5433_PIN_BANK_EINTG(7, 0x000, "gpz0", 0x00),
+	EXYNOS5433_PIN_BANK_EINTG(4, 0x020, "gpz1", 0x04),
+};
+
+/* pin banks of exynos5433 pin-controller - CPIF */
+static const struct samsung_pin_bank_data exynos5433_pin_banks2[] __initconst = {
+	EXYNOS5433_PIN_BANK_EINTG(2, 0x000, "gpv6", 0x00),
+};
+
+/* pin banks of exynos5433 pin-controller - eSE */
+static const struct samsung_pin_bank_data exynos5433_pin_banks3[] __initconst = {
+	EXYNOS5433_PIN_BANK_EINTG(3, 0x000, "gpj2", 0x00),
+};
+
+/* pin banks of exynos5433 pin-controller - FINGER */
+static const struct samsung_pin_bank_data exynos5433_pin_banks4[] __initconst = {
+	EXYNOS5433_PIN_BANK_EINTG(4, 0x000, "gpd5", 0x00),
+};
+
+/* pin banks of exynos5433 pin-controller - FSYS */
+static const struct samsung_pin_bank_data exynos5433_pin_banks5[] __initconst = {
+	EXYNOS5433_PIN_BANK_EINTG(6, 0x000, "gph1", 0x00),
+	EXYNOS5433_PIN_BANK_EINTG(7, 0x020, "gpr4", 0x04),
+	EXYNOS5433_PIN_BANK_EINTG(5, 0x040, "gpr0", 0x08),
+	EXYNOS5433_PIN_BANK_EINTG(8, 0x060, "gpr1", 0x0c),
+	EXYNOS5433_PIN_BANK_EINTG(2, 0x080, "gpr2", 0x10),
+	EXYNOS5433_PIN_BANK_EINTG(8, 0x0a0, "gpr3", 0x14),
+};
+
+/* pin banks of exynos5433 pin-controller - IMEM */
+static const struct samsung_pin_bank_data exynos5433_pin_banks6[] __initconst = {
+	EXYNOS5433_PIN_BANK_EINTG(8, 0x000, "gpf0", 0x00),
+};
+
+/* pin banks of exynos5433 pin-controller - NFC */
+static const struct samsung_pin_bank_data exynos5433_pin_banks7[] __initconst = {
+	EXYNOS5433_PIN_BANK_EINTG(3, 0x000, "gpj0", 0x00),
+};
+
+/* pin banks of exynos5433 pin-controller - PERIC */
+static const struct samsung_pin_bank_data exynos5433_pin_banks8[] __initconst = {
+	EXYNOS5433_PIN_BANK_EINTG(6, 0x000, "gpv7", 0x00),
+	EXYNOS5433_PIN_BANK_EINTG(5, 0x020, "gpb0", 0x04),
+	EXYNOS5433_PIN_BANK_EINTG(8, 0x040, "gpc0", 0x08),
+	EXYNOS5433_PIN_BANK_EINTG(2, 0x060, "gpc1", 0x0c),
+	EXYNOS5433_PIN_BANK_EINTG(6, 0x080, "gpc2", 0x10),
+	EXYNOS5433_PIN_BANK_EINTG(8, 0x0a0, "gpc3", 0x14),
+	EXYNOS5433_PIN_BANK_EINTG(2, 0x0c0, "gpg0", 0x18),
+	EXYNOS5433_PIN_BANK_EINTG(4, 0x0e0, "gpd0", 0x1c),
+	EXYNOS5433_PIN_BANK_EINTG(6, 0x100, "gpd1", 0x20),
+	EXYNOS5433_PIN_BANK_EINTG(8, 0x120, "gpd2", 0x24),
+	EXYNOS5433_PIN_BANK_EINTG(5, 0x140, "gpd4", 0x28),
+	EXYNOS5433_PIN_BANK_EINTG(2, 0x160, "gpd8", 0x2c),
+	EXYNOS5433_PIN_BANK_EINTG(7, 0x180, "gpd6", 0x30),
+	EXYNOS5433_PIN_BANK_EINTG(3, 0x1a0, "gpd7", 0x34),
+	EXYNOS5433_PIN_BANK_EINTG(5, 0x1c0, "gpg1", 0x38),
+	EXYNOS5433_PIN_BANK_EINTG(2, 0x1e0, "gpg2", 0x3c),
+	EXYNOS5433_PIN_BANK_EINTG(8, 0x200, "gpg3", 0x40),
+};
+
+/* pin banks of exynos5433 pin-controller - TOUCH */
+static const struct samsung_pin_bank_data exynos5433_pin_banks9[] __initconst = {
+	EXYNOS5433_PIN_BANK_EINTG(3, 0x000, "gpj1", 0x00),
+};
+
+/* PMU pin retention groups registers for Exynos5433 (without audio & fsys) */
+static const u32 exynos5433_retention_regs[] = {
+	EXYNOS5433_PAD_RETENTION_TOP_OPTION,
+	EXYNOS5433_PAD_RETENTION_UART_OPTION,
+	EXYNOS5433_PAD_RETENTION_EBIA_OPTION,
+	EXYNOS5433_PAD_RETENTION_EBIB_OPTION,
+	EXYNOS5433_PAD_RETENTION_SPI_OPTION,
+	EXYNOS5433_PAD_RETENTION_MIF_OPTION,
+	EXYNOS5433_PAD_RETENTION_USBXTI_OPTION,
+	EXYNOS5433_PAD_RETENTION_BOOTLDO_OPTION,
+	EXYNOS5433_PAD_RETENTION_UFS_OPTION,
+	EXYNOS5433_PAD_RETENTION_FSYSGENIO_OPTION,
+};
+
+static const struct samsung_retention_data exynos5433_retention_data __initconst = {
+	.regs	 = exynos5433_retention_regs,
+	.nr_regs = ARRAY_SIZE(exynos5433_retention_regs),
+	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
+	.refcnt	 = &exynos_shared_retention_refcnt,
+	.init	 = exynos_retention_init,
+};
+
+/* PMU retention control for audio pins can be tied to audio pin bank */
+static const u32 exynos5433_audio_retention_regs[] = {
+	EXYNOS5433_PAD_RETENTION_AUD_OPTION,
+};
+
+static const struct samsung_retention_data exynos5433_audio_retention_data __initconst = {
+	.regs	 = exynos5433_audio_retention_regs,
+	.nr_regs = ARRAY_SIZE(exynos5433_audio_retention_regs),
+	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
+	.init	 = exynos_retention_init,
+};
+
+/* PMU retention control for mmc pins can be tied to fsys pin bank */
+static const u32 exynos5433_fsys_retention_regs[] = {
+	EXYNOS5433_PAD_RETENTION_MMC0_OPTION,
+	EXYNOS5433_PAD_RETENTION_MMC1_OPTION,
+	EXYNOS5433_PAD_RETENTION_MMC2_OPTION,
+};
+
+static const struct samsung_retention_data exynos5433_fsys_retention_data __initconst = {
+	.regs	 = exynos5433_fsys_retention_regs,
+	.nr_regs = ARRAY_SIZE(exynos5433_fsys_retention_regs),
+	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
+	.init	 = exynos_retention_init,
+};
+
+/*
+ * Samsung pinctrl driver data for Exynos5433 SoC. Exynos5433 SoC includes
+ * ten gpio/pin-mux/pinconfig controllers.
+ */
+const struct samsung_pin_ctrl exynos5433_pin_ctrl[] __initconst = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= exynos5433_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks0),
+		.eint_wkup_init = exynos_eint_wkup_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.nr_ext_resources = 1,
+		.retention_data	= &exynos5433_retention_data,
+	}, {
+		/* pin-controller instance 1 data */
+		.pin_banks	= exynos5433_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks1),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos5433_audio_retention_data,
+	}, {
+		/* pin-controller instance 2 data */
+		.pin_banks	= exynos5433_pin_banks2,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks2),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos5433_retention_data,
+	}, {
+		/* pin-controller instance 3 data */
+		.pin_banks	= exynos5433_pin_banks3,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks3),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos5433_retention_data,
+	}, {
+		/* pin-controller instance 4 data */
+		.pin_banks	= exynos5433_pin_banks4,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks4),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos5433_retention_data,
+	}, {
+		/* pin-controller instance 5 data */
+		.pin_banks	= exynos5433_pin_banks5,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks5),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos5433_fsys_retention_data,
+	}, {
+		/* pin-controller instance 6 data */
+		.pin_banks	= exynos5433_pin_banks6,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks6),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos5433_retention_data,
+	}, {
+		/* pin-controller instance 7 data */
+		.pin_banks	= exynos5433_pin_banks7,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks7),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos5433_retention_data,
+	}, {
+		/* pin-controller instance 8 data */
+		.pin_banks	= exynos5433_pin_banks8,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks8),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos5433_retention_data,
+	}, {
+		/* pin-controller instance 9 data */
+		.pin_banks	= exynos5433_pin_banks9,
+		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks9),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.retention_data	= &exynos5433_retention_data,
+	},
+};
+
+/* pin banks of exynos7 pin-controller - ALIVE */
+static const struct samsung_pin_bank_data exynos7_pin_banks0[] __initconst = {
+	EXYNOS_PIN_BANK_EINTW(8, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0x040, "gpa2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0x060, "gpa3", 0x0c),
+};
+
+/* pin banks of exynos7 pin-controller - BUS0 */
+static const struct samsung_pin_bank_data exynos7_pin_banks1[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(5, 0x000, "gpb0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpc0", 0x04),
+	EXYNOS_PIN_BANK_EINTG(2, 0x040, "gpc1", 0x08),
+	EXYNOS_PIN_BANK_EINTG(6, 0x060, "gpc2", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x080, "gpc3", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0a0, "gpd0", 0x14),
+	EXYNOS_PIN_BANK_EINTG(6, 0x0c0, "gpd1", 0x18),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0e0, "gpd2", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(5, 0x100, "gpd4", 0x20),
+	EXYNOS_PIN_BANK_EINTG(4, 0x120, "gpd5", 0x24),
+	EXYNOS_PIN_BANK_EINTG(6, 0x140, "gpd6", 0x28),
+	EXYNOS_PIN_BANK_EINTG(3, 0x160, "gpd7", 0x2c),
+	EXYNOS_PIN_BANK_EINTG(2, 0x180, "gpd8", 0x30),
+	EXYNOS_PIN_BANK_EINTG(2, 0x1a0, "gpg0", 0x34),
+	EXYNOS_PIN_BANK_EINTG(4, 0x1c0, "gpg3", 0x38),
+};
+
+/* pin banks of exynos7 pin-controller - NFC */
+static const struct samsung_pin_bank_data exynos7_pin_banks2[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(3, 0x000, "gpj0", 0x00),
+};
+
+/* pin banks of exynos7 pin-controller - TOUCH */
+static const struct samsung_pin_bank_data exynos7_pin_banks3[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(3, 0x000, "gpj1", 0x00),
+};
+
+/* pin banks of exynos7 pin-controller - FF */
+static const struct samsung_pin_bank_data exynos7_pin_banks4[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(4, 0x000, "gpg4", 0x00),
+};
+
+/* pin banks of exynos7 pin-controller - ESE */
+static const struct samsung_pin_bank_data exynos7_pin_banks5[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(5, 0x000, "gpv7", 0x00),
+};
+
+/* pin banks of exynos7 pin-controller - FSYS0 */
+static const struct samsung_pin_bank_data exynos7_pin_banks6[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpr4", 0x00),
+};
+
+/* pin banks of exynos7 pin-controller - FSYS1 */
+static const struct samsung_pin_bank_data exynos7_pin_banks7[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(4, 0x000, "gpr0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpr1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(5, 0x040, "gpr2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpr3", 0x0c),
+};
+
+/* pin banks of exynos7 pin-controller - BUS1 */
+static const struct samsung_pin_bank_data exynos7_pin_banks8[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpf0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpf1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(4, 0x060, "gpf2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpf3", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0a0, "gpf4", 0x10),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0c0, "gpf5", 0x14),
+	EXYNOS_PIN_BANK_EINTG(5, 0x0e0, "gpg1", 0x18),
+	EXYNOS_PIN_BANK_EINTG(5, 0x100, "gpg2", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(6, 0x120, "gph1", 0x20),
+	EXYNOS_PIN_BANK_EINTG(3, 0x140, "gpv6", 0x24),
+};
+
+static const struct samsung_pin_bank_data exynos7_pin_banks9[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(4, 0x020, "gpz1", 0x04),
+};
+
+const struct samsung_pin_ctrl exynos7_pin_ctrl[] __initconst = {
+	{
+		/* pin-controller instance 0 Alive data */
+		.pin_banks	= exynos7_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks0),
+		.eint_wkup_init = exynos_eint_wkup_init,
+	}, {
+		/* pin-controller instance 1 BUS0 data */
+		.pin_banks	= exynos7_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks1),
+		.eint_gpio_init = exynos_eint_gpio_init,
+	}, {
+		/* pin-controller instance 2 NFC data */
+		.pin_banks	= exynos7_pin_banks2,
+		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks2),
+		.eint_gpio_init = exynos_eint_gpio_init,
+	}, {
+		/* pin-controller instance 3 TOUCH data */
+		.pin_banks	= exynos7_pin_banks3,
+		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks3),
+		.eint_gpio_init = exynos_eint_gpio_init,
+	}, {
+		/* pin-controller instance 4 FF data */
+		.pin_banks	= exynos7_pin_banks4,
+		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks4),
+		.eint_gpio_init = exynos_eint_gpio_init,
+	}, {
+		/* pin-controller instance 5 ESE data */
+		.pin_banks	= exynos7_pin_banks5,
+		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks5),
+		.eint_gpio_init = exynos_eint_gpio_init,
+	}, {
+		/* pin-controller instance 6 FSYS0 data */
+		.pin_banks	= exynos7_pin_banks6,
+		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks6),
+		.eint_gpio_init = exynos_eint_gpio_init,
+	}, {
+		/* pin-controller instance 7 FSYS1 data */
+		.pin_banks	= exynos7_pin_banks7,
+		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks7),
+		.eint_gpio_init = exynos_eint_gpio_init,
+	}, {
+		/* pin-controller instance 8 BUS1 data */
+		.pin_banks	= exynos7_pin_banks8,
+		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks8),
+		.eint_gpio_init = exynos_eint_gpio_init,
+	}, {
+		/* pin-controller instance 9 AUD data */
+		.pin_banks	= exynos7_pin_banks9,
+		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks9),
+		.eint_gpio_init = exynos_eint_gpio_init,
+	},
+};
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos.c b/drivers/pinctrl/samsung/pinctrl-exynos.c
index 7b0e6cc35e04..731530a9ce38 100644
--- a/drivers/pinctrl/samsung/pinctrl-exynos.c
+++ b/drivers/pinctrl/samsung/pinctrl-exynos.c
@@ -18,21 +18,18 @@
  * external gpio and wakeup interrupt support.
  */
 
-#include <linux/module.h>
 #include <linux/device.h>
 #include <linux/interrupt.h>
 #include <linux/irqdomain.h>
 #include <linux/irq.h>
 #include <linux/irqchip/chained_irq.h>
-#include <linux/of_address.h>
+#include <linux/of.h>
 #include <linux/of_irq.h>
-#include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/regmap.h>
 #include <linux/err.h>
 #include <linux/soc/samsung/exynos-pmu.h>
-#include <linux/soc/samsung/exynos-regs-pmu.h>
 
 #include "pinctrl-samsung.h"
 #include "pinctrl-exynos.h"
@@ -50,27 +47,6 @@ static inline struct exynos_irq_chip *to_exynos_irq_chip(struct irq_chip *chip)
 	return container_of(chip, struct exynos_irq_chip, chip);
 }
 
-static const struct samsung_pin_bank_type bank_type_off = {
-	.fld_width = { 4, 1, 2, 2, 2, 2, },
-	.reg_offset = { 0x00, 0x04, 0x08, 0x0c, 0x10, 0x14, },
-};
-
-static const struct samsung_pin_bank_type bank_type_alive = {
-	.fld_width = { 4, 1, 2, 2, },
-	.reg_offset = { 0x00, 0x04, 0x08, 0x0c, },
-};
-
-/* Exynos5433 has the 4bit widths for PINCFG_TYPE_DRV bitfields. */
-static const struct samsung_pin_bank_type exynos5433_bank_type_off = {
-	.fld_width = { 4, 1, 2, 4, 2, 2, },
-	.reg_offset = { 0x00, 0x04, 0x08, 0x0c, 0x10, 0x14, },
-};
-
-static const struct samsung_pin_bank_type exynos5433_bank_type_alive = {
-	.fld_width = { 4, 1, 2, 4, },
-	.reg_offset = { 0x00, 0x04, 0x08, 0x0c, },
-};
-
 static void exynos_irq_mask(struct irq_data *irqd)
 {
 	struct irq_chip *chip = irq_data_get_irq_chip(irqd);
@@ -205,8 +181,6 @@ static int exynos_irq_request_resources(struct irq_data *irqd)
 
 	spin_unlock_irqrestore(&bank->slock, flags);
 
-	exynos_irq_unmask(irqd);
-
 	return 0;
 }
 
@@ -226,8 +200,6 @@ static void exynos_irq_release_resources(struct irq_data *irqd)
 	shift = irqd->hwirq * bank_type->fld_width[PINCFG_TYPE_FUNC];
 	mask = (1 << bank_type->fld_width[PINCFG_TYPE_FUNC]) - 1;
 
-	exynos_irq_mask(irqd);
-
 	spin_lock_irqsave(&bank->slock, flags);
 
 	con = readl(bank->eint_base + reg_con);
@@ -308,7 +280,7 @@ struct exynos_eint_gpio_save {
  * exynos_eint_gpio_init() - setup handling of external gpio interrupts.
  * @d: driver data of samsung pinctrl driver.
  */
-static int exynos_eint_gpio_init(struct samsung_pinctrl_drv_data *d)
+int exynos_eint_gpio_init(struct samsung_pinctrl_drv_data *d)
 {
 	struct samsung_pin_bank *bank;
 	struct device *dev = d->dev;
@@ -387,7 +359,7 @@ static int exynos_wkup_irq_set_wake(struct irq_data *irqd, unsigned int on)
 /*
  * irq_chip for wakeup interrupts
  */
-static struct exynos_irq_chip exynos4210_wkup_irq_chip __initdata = {
+static const struct exynos_irq_chip exynos4210_wkup_irq_chip __initconst = {
 	.chip = {
 		.name = "exynos4210_wkup_irq_chip",
 		.irq_unmask = exynos_irq_unmask,
@@ -403,7 +375,7 @@ static struct exynos_irq_chip exynos4210_wkup_irq_chip __initdata = {
 	.eint_pend = EXYNOS_WKUP_EPEND_OFFSET,
 };
 
-static struct exynos_irq_chip exynos7_wkup_irq_chip __initdata = {
+static const struct exynos_irq_chip exynos7_wkup_irq_chip __initconst = {
 	.chip = {
 		.name = "exynos7_wkup_irq_chip",
 		.irq_unmask = exynos_irq_unmask,
@@ -483,7 +455,7 @@ static void exynos_irq_demux_eint16_31(struct irq_desc *desc)
  * exynos_eint_wkup_init() - setup handling of external wakeup interrupts.
  * @d: driver data of samsung pinctrl driver.
  */
-static int exynos_eint_wkup_init(struct samsung_pinctrl_drv_data *d)
+int exynos_eint_wkup_init(struct samsung_pinctrl_drv_data *d)
 {
 	struct device *dev = d->dev;
 	struct device_node *wkup_np = NULL;
@@ -503,6 +475,8 @@ static int exynos_eint_wkup_init(struct samsung_pinctrl_drv_data *d)
 		if (match) {
 			irq_chip = kmemdup(match->data,
 				sizeof(*irq_chip), GFP_KERNEL);
+			if (!irq_chip)
+				return -ENOMEM;
 			wkup_np = np;
 			break;
 		}
@@ -599,7 +573,7 @@ static void exynos_pinctrl_suspend_bank(
 	pr_debug("%s: save fltcon1 %#010x\n", bank->name, save->eint_fltcon1);
 }
 
-static void exynos_pinctrl_suspend(struct samsung_pinctrl_drv_data *drvdata)
+void exynos_pinctrl_suspend(struct samsung_pinctrl_drv_data *drvdata)
 {
 	struct samsung_pin_bank *bank = drvdata->pin_banks;
 	int i;
@@ -634,7 +608,7 @@ static void exynos_pinctrl_resume_bank(
 						+ 2 * bank->eint_offset + 4);
 }
 
-static void exynos_pinctrl_resume(struct samsung_pinctrl_drv_data *drvdata)
+void exynos_pinctrl_resume(struct samsung_pinctrl_drv_data *drvdata)
 {
 	struct samsung_pin_bank *bank = drvdata->pin_banks;
 	int i;
@@ -644,114 +618,6 @@ static void exynos_pinctrl_resume(struct samsung_pinctrl_drv_data *drvdata)
 			exynos_pinctrl_resume_bank(drvdata, bank);
 }
 
-/* Retention control for S5PV210 are located at the end of clock controller */
-#define S5P_OTHERS 0xE000
-
-#define S5P_OTHERS_RET_IO		(1 << 31)
-#define S5P_OTHERS_RET_CF		(1 << 30)
-#define S5P_OTHERS_RET_MMC		(1 << 29)
-#define S5P_OTHERS_RET_UART		(1 << 28)
-
-static void s5pv210_retention_disable(struct samsung_pinctrl_drv_data *drvdata)
-{
-	void *clk_base = drvdata->retention_ctrl->priv;
-	u32 tmp;
-
-	tmp = __raw_readl(clk_base + S5P_OTHERS);
-	tmp |= (S5P_OTHERS_RET_IO | S5P_OTHERS_RET_CF | S5P_OTHERS_RET_MMC |
-		S5P_OTHERS_RET_UART);
-	__raw_writel(tmp, clk_base + S5P_OTHERS);
-}
-
-static struct samsung_retention_ctrl *
-s5pv210_retention_init(struct samsung_pinctrl_drv_data *drvdata,
-		       const struct samsung_retention_data *data)
-{
-	struct samsung_retention_ctrl *ctrl;
-	struct device_node *np;
-	void *clk_base;
-
-	ctrl = devm_kzalloc(drvdata->dev, sizeof(*ctrl), GFP_KERNEL);
-	if (!ctrl)
-		return ERR_PTR(-ENOMEM);
-
-	np = of_find_compatible_node(NULL, NULL, "samsung,s5pv210-clock");
-	if (!np) {
-		pr_err("%s: failed to find clock controller DT node\n",
-			__func__);
-		return ERR_PTR(-ENODEV);
-	}
-
-	clk_base = of_iomap(np, 0);
-	if (!clk_base) {
-		pr_err("%s: failed to map clock registers\n", __func__);
-		return ERR_PTR(-EINVAL);
-	}
-
-	ctrl->priv = clk_base;
-	ctrl->disable = s5pv210_retention_disable;
-
-	return ctrl;
-}
-
-static const struct samsung_retention_data s5pv210_retention_data __initconst = {
-	.init	 = s5pv210_retention_init,
-};
-
-/* pin banks of s5pv210 pin-controller */
-static const struct samsung_pin_bank_data s5pv210_pin_bank[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(4, 0x020, "gpa1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpb", 0x08),
-	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpc0", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpc1", 0x10),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0a0, "gpd0", 0x14),
-	EXYNOS_PIN_BANK_EINTG(6, 0x0c0, "gpd1", 0x18),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0e0, "gpe0", 0x1c),
-	EXYNOS_PIN_BANK_EINTG(5, 0x100, "gpe1", 0x20),
-	EXYNOS_PIN_BANK_EINTG(8, 0x120, "gpf0", 0x24),
-	EXYNOS_PIN_BANK_EINTG(8, 0x140, "gpf1", 0x28),
-	EXYNOS_PIN_BANK_EINTG(8, 0x160, "gpf2", 0x2c),
-	EXYNOS_PIN_BANK_EINTG(6, 0x180, "gpf3", 0x30),
-	EXYNOS_PIN_BANK_EINTG(7, 0x1a0, "gpg0", 0x34),
-	EXYNOS_PIN_BANK_EINTG(7, 0x1c0, "gpg1", 0x38),
-	EXYNOS_PIN_BANK_EINTG(7, 0x1e0, "gpg2", 0x3c),
-	EXYNOS_PIN_BANK_EINTG(7, 0x200, "gpg3", 0x40),
-	EXYNOS_PIN_BANK_EINTN(7, 0x220, "gpi"),
-	EXYNOS_PIN_BANK_EINTG(8, 0x240, "gpj0", 0x44),
-	EXYNOS_PIN_BANK_EINTG(6, 0x260, "gpj1", 0x48),
-	EXYNOS_PIN_BANK_EINTG(8, 0x280, "gpj2", 0x4c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x2a0, "gpj3", 0x50),
-	EXYNOS_PIN_BANK_EINTG(5, 0x2c0, "gpj4", 0x54),
-	EXYNOS_PIN_BANK_EINTN(8, 0x2e0, "mp01"),
-	EXYNOS_PIN_BANK_EINTN(4, 0x300, "mp02"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x320, "mp03"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x340, "mp04"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x360, "mp05"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x380, "mp06"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x3a0, "mp07"),
-	EXYNOS_PIN_BANK_EINTW(8, 0xc00, "gph0", 0x00),
-	EXYNOS_PIN_BANK_EINTW(8, 0xc20, "gph1", 0x04),
-	EXYNOS_PIN_BANK_EINTW(8, 0xc40, "gph2", 0x08),
-	EXYNOS_PIN_BANK_EINTW(8, 0xc60, "gph3", 0x0c),
-};
-
-const struct samsung_pin_ctrl s5pv210_pin_ctrl[] __initconst = {
-	{
-		/* pin-controller instance 0 data */
-		.pin_banks	= s5pv210_pin_bank,
-		.nr_banks	= ARRAY_SIZE(s5pv210_pin_bank),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.eint_wkup_init = exynos_eint_wkup_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &s5pv210_retention_data,
-	},
-};
-
-/* Pad retention control code for accessing PMU regmap */
-static atomic_t exynos_shared_retention_refcnt;
-
 static void exynos_retention_enable(struct samsung_pinctrl_drv_data *drvdata)
 {
 	if (drvdata->retention_ctrl->refcnt)
@@ -771,7 +637,7 @@ static void exynos_retention_disable(struct samsung_pinctrl_drv_data *drvdata)
 		regmap_write(pmu_regs, ctrl->regs[i], ctrl->value);
 }
 
-static struct samsung_retention_ctrl *
+struct samsung_retention_ctrl *
 exynos_retention_init(struct samsung_pinctrl_drv_data *drvdata,
 		      const struct samsung_retention_data *data)
 {
@@ -801,1022 +667,3 @@ exynos_retention_init(struct samsung_pinctrl_drv_data *drvdata,
 
 	return ctrl;
 }
-
-/* pin banks of exynos3250 pin-controller 0 */
-static const struct samsung_pin_bank_data exynos3250_pin_banks0[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpb",  0x08),
-	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpc0", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpc1", 0x10),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0a0, "gpd0", 0x14),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0c0, "gpd1", 0x18),
-};
-
-/* pin banks of exynos3250 pin-controller 1 */
-static const struct samsung_pin_bank_data exynos3250_pin_banks1[] __initconst = {
-	EXYNOS_PIN_BANK_EINTN(8, 0x120, "gpe0"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x140, "gpe1"),
-	EXYNOS_PIN_BANK_EINTN(3, 0x180, "gpe2"),
-	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpk0", 0x08),
-	EXYNOS_PIN_BANK_EINTG(7, 0x060, "gpk1", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(7, 0x080, "gpk2", 0x10),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0c0, "gpl0", 0x18),
-	EXYNOS_PIN_BANK_EINTG(8, 0x260, "gpm0", 0x24),
-	EXYNOS_PIN_BANK_EINTG(7, 0x280, "gpm1", 0x28),
-	EXYNOS_PIN_BANK_EINTG(5, 0x2a0, "gpm2", 0x2c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x2c0, "gpm3", 0x30),
-	EXYNOS_PIN_BANK_EINTG(8, 0x2e0, "gpm4", 0x34),
-	EXYNOS_PIN_BANK_EINTW(8, 0xc00, "gpx0", 0x00),
-	EXYNOS_PIN_BANK_EINTW(8, 0xc20, "gpx1", 0x04),
-	EXYNOS_PIN_BANK_EINTW(8, 0xc40, "gpx2", 0x08),
-	EXYNOS_PIN_BANK_EINTW(8, 0xc60, "gpx3", 0x0c),
-};
-
-/*
- * PMU pad retention groups for Exynos3250 doesn't match pin banks, so handle
- * them all together
- */
-static const u32 exynos3250_retention_regs[] = {
-	S5P_PAD_RET_MAUDIO_OPTION,
-	S5P_PAD_RET_GPIO_OPTION,
-	S5P_PAD_RET_UART_OPTION,
-	S5P_PAD_RET_MMCA_OPTION,
-	S5P_PAD_RET_MMCB_OPTION,
-	S5P_PAD_RET_EBIA_OPTION,
-	S5P_PAD_RET_EBIB_OPTION,
-	S5P_PAD_RET_MMC2_OPTION,
-	S5P_PAD_RET_SPI_OPTION,
-};
-
-static const struct samsung_retention_data exynos3250_retention_data __initconst = {
-	.regs	 = exynos3250_retention_regs,
-	.nr_regs = ARRAY_SIZE(exynos3250_retention_regs),
-	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
-	.refcnt	 = &exynos_shared_retention_refcnt,
-	.init	 = exynos_retention_init,
-};
-
-/*
- * Samsung pinctrl driver data for Exynos3250 SoC. Exynos3250 SoC includes
- * two gpio/pin-mux/pinconfig controllers.
- */
-const struct samsung_pin_ctrl exynos3250_pin_ctrl[] __initconst = {
-	{
-		/* pin-controller instance 0 data */
-		.pin_banks	= exynos3250_pin_banks0,
-		.nr_banks	= ARRAY_SIZE(exynos3250_pin_banks0),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos3250_retention_data,
-	}, {
-		/* pin-controller instance 1 data */
-		.pin_banks	= exynos3250_pin_banks1,
-		.nr_banks	= ARRAY_SIZE(exynos3250_pin_banks1),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.eint_wkup_init = exynos_eint_wkup_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos3250_retention_data,
-	},
-};
-
-/* pin banks of exynos4210 pin-controller 0 */
-static const struct samsung_pin_bank_data exynos4210_pin_banks0[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpb", 0x08),
-	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpc0", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpc1", 0x10),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0A0, "gpd0", 0x14),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0C0, "gpd1", 0x18),
-	EXYNOS_PIN_BANK_EINTG(5, 0x0E0, "gpe0", 0x1c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x100, "gpe1", 0x20),
-	EXYNOS_PIN_BANK_EINTG(6, 0x120, "gpe2", 0x24),
-	EXYNOS_PIN_BANK_EINTG(8, 0x140, "gpe3", 0x28),
-	EXYNOS_PIN_BANK_EINTG(8, 0x160, "gpe4", 0x2c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x180, "gpf0", 0x30),
-	EXYNOS_PIN_BANK_EINTG(8, 0x1A0, "gpf1", 0x34),
-	EXYNOS_PIN_BANK_EINTG(8, 0x1C0, "gpf2", 0x38),
-	EXYNOS_PIN_BANK_EINTG(6, 0x1E0, "gpf3", 0x3c),
-};
-
-/* pin banks of exynos4210 pin-controller 1 */
-static const struct samsung_pin_bank_data exynos4210_pin_banks1[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpj0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(5, 0x020, "gpj1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(7, 0x040, "gpk0", 0x08),
-	EXYNOS_PIN_BANK_EINTG(7, 0x060, "gpk1", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(7, 0x080, "gpk2", 0x10),
-	EXYNOS_PIN_BANK_EINTG(7, 0x0A0, "gpk3", 0x14),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0C0, "gpl0", 0x18),
-	EXYNOS_PIN_BANK_EINTG(3, 0x0E0, "gpl1", 0x1c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x100, "gpl2", 0x20),
-	EXYNOS_PIN_BANK_EINTN(6, 0x120, "gpy0"),
-	EXYNOS_PIN_BANK_EINTN(4, 0x140, "gpy1"),
-	EXYNOS_PIN_BANK_EINTN(6, 0x160, "gpy2"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x180, "gpy3"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x1A0, "gpy4"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x1C0, "gpy5"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x1E0, "gpy6"),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC00, "gpx0", 0x00),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC20, "gpx1", 0x04),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC40, "gpx2", 0x08),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC60, "gpx3", 0x0c),
-};
-
-/* pin banks of exynos4210 pin-controller 2 */
-static const struct samsung_pin_bank_data exynos4210_pin_banks2[] __initconst = {
-	EXYNOS_PIN_BANK_EINTN(7, 0x000, "gpz"),
-};
-
-/* PMU pad retention groups registers for Exynos4 (without audio) */
-static const u32 exynos4_retention_regs[] = {
-	S5P_PAD_RET_GPIO_OPTION,
-	S5P_PAD_RET_UART_OPTION,
-	S5P_PAD_RET_MMCA_OPTION,
-	S5P_PAD_RET_MMCB_OPTION,
-	S5P_PAD_RET_EBIA_OPTION,
-	S5P_PAD_RET_EBIB_OPTION,
-};
-
-static const struct samsung_retention_data exynos4_retention_data __initconst = {
-	.regs	 = exynos4_retention_regs,
-	.nr_regs = ARRAY_SIZE(exynos4_retention_regs),
-	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
-	.refcnt	 = &exynos_shared_retention_refcnt,
-	.init	 = exynos_retention_init,
-};
-
-/* PMU retention control for audio pins can be tied to audio pin bank */
-static const u32 exynos4_audio_retention_regs[] = {
-	S5P_PAD_RET_MAUDIO_OPTION,
-};
-
-static const struct samsung_retention_data exynos4_audio_retention_data __initconst = {
-	.regs	 = exynos4_audio_retention_regs,
-	.nr_regs = ARRAY_SIZE(exynos4_audio_retention_regs),
-	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
-	.init	 = exynos_retention_init,
-};
-
-/*
- * Samsung pinctrl driver data for Exynos4210 SoC. Exynos4210 SoC includes
- * three gpio/pin-mux/pinconfig controllers.
- */
-const struct samsung_pin_ctrl exynos4210_pin_ctrl[] __initconst = {
-	{
-		/* pin-controller instance 0 data */
-		.pin_banks	= exynos4210_pin_banks0,
-		.nr_banks	= ARRAY_SIZE(exynos4210_pin_banks0),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos4_retention_data,
-	}, {
-		/* pin-controller instance 1 data */
-		.pin_banks	= exynos4210_pin_banks1,
-		.nr_banks	= ARRAY_SIZE(exynos4210_pin_banks1),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.eint_wkup_init = exynos_eint_wkup_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos4_retention_data,
-	}, {
-		/* pin-controller instance 2 data */
-		.pin_banks	= exynos4210_pin_banks2,
-		.nr_banks	= ARRAY_SIZE(exynos4210_pin_banks2),
-		.retention_data	= &exynos4_audio_retention_data,
-	},
-};
-
-/* pin banks of exynos4x12 pin-controller 0 */
-static const struct samsung_pin_bank_data exynos4x12_pin_banks0[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpb", 0x08),
-	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpc0", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpc1", 0x10),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0A0, "gpd0", 0x14),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0C0, "gpd1", 0x18),
-	EXYNOS_PIN_BANK_EINTG(8, 0x180, "gpf0", 0x30),
-	EXYNOS_PIN_BANK_EINTG(8, 0x1A0, "gpf1", 0x34),
-	EXYNOS_PIN_BANK_EINTG(8, 0x1C0, "gpf2", 0x38),
-	EXYNOS_PIN_BANK_EINTG(6, 0x1E0, "gpf3", 0x3c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x240, "gpj0", 0x40),
-	EXYNOS_PIN_BANK_EINTG(5, 0x260, "gpj1", 0x44),
-};
-
-/* pin banks of exynos4x12 pin-controller 1 */
-static const struct samsung_pin_bank_data exynos4x12_pin_banks1[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(7, 0x040, "gpk0", 0x08),
-	EXYNOS_PIN_BANK_EINTG(7, 0x060, "gpk1", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(7, 0x080, "gpk2", 0x10),
-	EXYNOS_PIN_BANK_EINTG(7, 0x0A0, "gpk3", 0x14),
-	EXYNOS_PIN_BANK_EINTG(7, 0x0C0, "gpl0", 0x18),
-	EXYNOS_PIN_BANK_EINTG(2, 0x0E0, "gpl1", 0x1c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x100, "gpl2", 0x20),
-	EXYNOS_PIN_BANK_EINTG(8, 0x260, "gpm0", 0x24),
-	EXYNOS_PIN_BANK_EINTG(7, 0x280, "gpm1", 0x28),
-	EXYNOS_PIN_BANK_EINTG(5, 0x2A0, "gpm2", 0x2c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x2C0, "gpm3", 0x30),
-	EXYNOS_PIN_BANK_EINTG(8, 0x2E0, "gpm4", 0x34),
-	EXYNOS_PIN_BANK_EINTN(6, 0x120, "gpy0"),
-	EXYNOS_PIN_BANK_EINTN(4, 0x140, "gpy1"),
-	EXYNOS_PIN_BANK_EINTN(6, 0x160, "gpy2"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x180, "gpy3"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x1A0, "gpy4"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x1C0, "gpy5"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x1E0, "gpy6"),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC00, "gpx0", 0x00),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC20, "gpx1", 0x04),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC40, "gpx2", 0x08),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC60, "gpx3", 0x0c),
-};
-
-/* pin banks of exynos4x12 pin-controller 2 */
-static const struct samsung_pin_bank_data exynos4x12_pin_banks2[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz", 0x00),
-};
-
-/* pin banks of exynos4x12 pin-controller 3 */
-static const struct samsung_pin_bank_data exynos4x12_pin_banks3[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpv0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpv1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpv2", 0x08),
-	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpv3", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(2, 0x080, "gpv4", 0x10),
-};
-
-/*
- * Samsung pinctrl driver data for Exynos4x12 SoC. Exynos4x12 SoC includes
- * four gpio/pin-mux/pinconfig controllers.
- */
-const struct samsung_pin_ctrl exynos4x12_pin_ctrl[] __initconst = {
-	{
-		/* pin-controller instance 0 data */
-		.pin_banks	= exynos4x12_pin_banks0,
-		.nr_banks	= ARRAY_SIZE(exynos4x12_pin_banks0),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos4_retention_data,
-	}, {
-		/* pin-controller instance 1 data */
-		.pin_banks	= exynos4x12_pin_banks1,
-		.nr_banks	= ARRAY_SIZE(exynos4x12_pin_banks1),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.eint_wkup_init = exynos_eint_wkup_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos4_retention_data,
-	}, {
-		/* pin-controller instance 2 data */
-		.pin_banks	= exynos4x12_pin_banks2,
-		.nr_banks	= ARRAY_SIZE(exynos4x12_pin_banks2),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos4_audio_retention_data,
-	}, {
-		/* pin-controller instance 3 data */
-		.pin_banks	= exynos4x12_pin_banks3,
-		.nr_banks	= ARRAY_SIZE(exynos4x12_pin_banks3),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-	},
-};
-
-/* pin banks of exynos5250 pin-controller 0 */
-static const struct samsung_pin_bank_data exynos5250_pin_banks0[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpa2", 0x08),
-	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpb0", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpb1", 0x10),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0A0, "gpb2", 0x14),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0C0, "gpb3", 0x18),
-	EXYNOS_PIN_BANK_EINTG(7, 0x0E0, "gpc0", 0x1c),
-	EXYNOS_PIN_BANK_EINTG(4, 0x100, "gpc1", 0x20),
-	EXYNOS_PIN_BANK_EINTG(7, 0x120, "gpc2", 0x24),
-	EXYNOS_PIN_BANK_EINTG(7, 0x140, "gpc3", 0x28),
-	EXYNOS_PIN_BANK_EINTG(4, 0x160, "gpd0", 0x2c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x180, "gpd1", 0x30),
-	EXYNOS_PIN_BANK_EINTG(7, 0x2E0, "gpc4", 0x34),
-	EXYNOS_PIN_BANK_EINTN(6, 0x1A0, "gpy0"),
-	EXYNOS_PIN_BANK_EINTN(4, 0x1C0, "gpy1"),
-	EXYNOS_PIN_BANK_EINTN(6, 0x1E0, "gpy2"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x200, "gpy3"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x220, "gpy4"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x240, "gpy5"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x260, "gpy6"),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC00, "gpx0", 0x00),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC20, "gpx1", 0x04),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC40, "gpx2", 0x08),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC60, "gpx3", 0x0c),
-};
-
-/* pin banks of exynos5250 pin-controller 1 */
-static const struct samsung_pin_bank_data exynos5250_pin_banks1[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpe0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(2, 0x020, "gpe1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(4, 0x040, "gpf0", 0x08),
-	EXYNOS_PIN_BANK_EINTG(4, 0x060, "gpf1", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x080, "gpg0", 0x10),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0A0, "gpg1", 0x14),
-	EXYNOS_PIN_BANK_EINTG(2, 0x0C0, "gpg2", 0x18),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0E0, "gph0", 0x1c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x100, "gph1", 0x20),
-};
-
-/* pin banks of exynos5250 pin-controller 2 */
-static const struct samsung_pin_bank_data exynos5250_pin_banks2[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpv0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpv1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpv2", 0x08),
-	EXYNOS_PIN_BANK_EINTG(8, 0x080, "gpv3", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(2, 0x0C0, "gpv4", 0x10),
-};
-
-/* pin banks of exynos5250 pin-controller 3 */
-static const struct samsung_pin_bank_data exynos5250_pin_banks3[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz", 0x00),
-};
-
-/*
- * Samsung pinctrl driver data for Exynos5250 SoC. Exynos5250 SoC includes
- * four gpio/pin-mux/pinconfig controllers.
- */
-const struct samsung_pin_ctrl exynos5250_pin_ctrl[] __initconst = {
-	{
-		/* pin-controller instance 0 data */
-		.pin_banks	= exynos5250_pin_banks0,
-		.nr_banks	= ARRAY_SIZE(exynos5250_pin_banks0),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.eint_wkup_init = exynos_eint_wkup_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos4_retention_data,
-	}, {
-		/* pin-controller instance 1 data */
-		.pin_banks	= exynos5250_pin_banks1,
-		.nr_banks	= ARRAY_SIZE(exynos5250_pin_banks1),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos4_retention_data,
-	}, {
-		/* pin-controller instance 2 data */
-		.pin_banks	= exynos5250_pin_banks2,
-		.nr_banks	= ARRAY_SIZE(exynos5250_pin_banks2),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-	}, {
-		/* pin-controller instance 3 data */
-		.pin_banks	= exynos5250_pin_banks3,
-		.nr_banks	= ARRAY_SIZE(exynos5250_pin_banks3),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos4_audio_retention_data,
-	},
-};
-
-/* pin banks of exynos5260 pin-controller 0 */
-static const struct samsung_pin_bank_data exynos5260_pin_banks0[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(4, 0x000, "gpa0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(7, 0x020, "gpa1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpa2", 0x08),
-	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpb0", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(4, 0x080, "gpb1", 0x10),
-	EXYNOS_PIN_BANK_EINTG(5, 0x0a0, "gpb2", 0x14),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0c0, "gpb3", 0x18),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0e0, "gpb4", 0x1c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x100, "gpb5", 0x20),
-	EXYNOS_PIN_BANK_EINTG(8, 0x120, "gpd0", 0x24),
-	EXYNOS_PIN_BANK_EINTG(7, 0x140, "gpd1", 0x28),
-	EXYNOS_PIN_BANK_EINTG(5, 0x160, "gpd2", 0x2c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x180, "gpe0", 0x30),
-	EXYNOS_PIN_BANK_EINTG(5, 0x1a0, "gpe1", 0x34),
-	EXYNOS_PIN_BANK_EINTG(4, 0x1c0, "gpf0", 0x38),
-	EXYNOS_PIN_BANK_EINTG(8, 0x1e0, "gpf1", 0x3c),
-	EXYNOS_PIN_BANK_EINTG(2, 0x200, "gpk0", 0x40),
-	EXYNOS_PIN_BANK_EINTW(8, 0xc00, "gpx0", 0x00),
-	EXYNOS_PIN_BANK_EINTW(8, 0xc20, "gpx1", 0x04),
-	EXYNOS_PIN_BANK_EINTW(8, 0xc40, "gpx2", 0x08),
-	EXYNOS_PIN_BANK_EINTW(8, 0xc60, "gpx3", 0x0c),
-};
-
-/* pin banks of exynos5260 pin-controller 1 */
-static const struct samsung_pin_bank_data exynos5260_pin_banks1[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpc0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpc1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(7, 0x040, "gpc2", 0x08),
-	EXYNOS_PIN_BANK_EINTG(4, 0x060, "gpc3", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(4, 0x080, "gpc4", 0x10),
-};
-
-/* pin banks of exynos5260 pin-controller 2 */
-static const struct samsung_pin_bank_data exynos5260_pin_banks2[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(4, 0x020, "gpz1", 0x04),
-};
-
-/*
- * Samsung pinctrl driver data for Exynos5260 SoC. Exynos5260 SoC includes
- * three gpio/pin-mux/pinconfig controllers.
- */
-const struct samsung_pin_ctrl exynos5260_pin_ctrl[] __initconst = {
-	{
-		/* pin-controller instance 0 data */
-		.pin_banks	= exynos5260_pin_banks0,
-		.nr_banks	= ARRAY_SIZE(exynos5260_pin_banks0),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.eint_wkup_init = exynos_eint_wkup_init,
-	}, {
-		/* pin-controller instance 1 data */
-		.pin_banks	= exynos5260_pin_banks1,
-		.nr_banks	= ARRAY_SIZE(exynos5260_pin_banks1),
-		.eint_gpio_init = exynos_eint_gpio_init,
-	}, {
-		/* pin-controller instance 2 data */
-		.pin_banks	= exynos5260_pin_banks2,
-		.nr_banks	= ARRAY_SIZE(exynos5260_pin_banks2),
-		.eint_gpio_init = exynos_eint_gpio_init,
-	},
-};
-
-/* pin banks of exynos5410 pin-controller 0 */
-static const struct samsung_pin_bank_data exynos5410_pin_banks0[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpa2", 0x08),
-	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpb0", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpb1", 0x10),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0A0, "gpb2", 0x14),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0C0, "gpb3", 0x18),
-	EXYNOS_PIN_BANK_EINTG(7, 0x0E0, "gpc0", 0x1c),
-	EXYNOS_PIN_BANK_EINTG(4, 0x100, "gpc3", 0x20),
-	EXYNOS_PIN_BANK_EINTG(7, 0x120, "gpc1", 0x24),
-	EXYNOS_PIN_BANK_EINTG(7, 0x140, "gpc2", 0x28),
-	EXYNOS_PIN_BANK_EINTN(2, 0x160, "gpm5"),
-	EXYNOS_PIN_BANK_EINTG(8, 0x180, "gpd1", 0x2c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x1A0, "gpe0", 0x30),
-	EXYNOS_PIN_BANK_EINTG(2, 0x1C0, "gpe1", 0x34),
-	EXYNOS_PIN_BANK_EINTG(6, 0x1E0, "gpf0", 0x38),
-	EXYNOS_PIN_BANK_EINTG(8, 0x200, "gpf1", 0x3c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x220, "gpg0", 0x40),
-	EXYNOS_PIN_BANK_EINTG(8, 0x240, "gpg1", 0x44),
-	EXYNOS_PIN_BANK_EINTG(2, 0x260, "gpg2", 0x48),
-	EXYNOS_PIN_BANK_EINTG(4, 0x280, "gph0", 0x4c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x2A0, "gph1", 0x50),
-	EXYNOS_PIN_BANK_EINTN(8, 0x2C0, "gpm7"),
-	EXYNOS_PIN_BANK_EINTN(6, 0x2E0, "gpy0"),
-	EXYNOS_PIN_BANK_EINTN(4, 0x300, "gpy1"),
-	EXYNOS_PIN_BANK_EINTN(6, 0x320, "gpy2"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x340, "gpy3"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x360, "gpy4"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x380, "gpy5"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x3A0, "gpy6"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x3C0, "gpy7"),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC00, "gpx0", 0x00),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC20, "gpx1", 0x04),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC40, "gpx2", 0x08),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC60, "gpx3", 0x0c),
-};
-
-/* pin banks of exynos5410 pin-controller 1 */
-static const struct samsung_pin_bank_data exynos5410_pin_banks1[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(5, 0x000, "gpj0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpj1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpj2", 0x08),
-	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpj3", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(2, 0x080, "gpj4", 0x10),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0A0, "gpk0", 0x14),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0C0, "gpk1", 0x18),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0E0, "gpk2", 0x1c),
-	EXYNOS_PIN_BANK_EINTG(7, 0x100, "gpk3", 0x20),
-};
-
-/* pin banks of exynos5410 pin-controller 2 */
-static const struct samsung_pin_bank_data exynos5410_pin_banks2[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpv0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpv1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpv2", 0x08),
-	EXYNOS_PIN_BANK_EINTG(8, 0x080, "gpv3", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(2, 0x0C0, "gpv4", 0x10),
-};
-
-/* pin banks of exynos5410 pin-controller 3 */
-static const struct samsung_pin_bank_data exynos5410_pin_banks3[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz", 0x00),
-};
-
-/*
- * Samsung pinctrl driver data for Exynos5410 SoC. Exynos5410 SoC includes
- * four gpio/pin-mux/pinconfig controllers.
- */
-const struct samsung_pin_ctrl exynos5410_pin_ctrl[] __initconst = {
-	{
-		/* pin-controller instance 0 data */
-		.pin_banks	= exynos5410_pin_banks0,
-		.nr_banks	= ARRAY_SIZE(exynos5410_pin_banks0),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.eint_wkup_init = exynos_eint_wkup_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-	}, {
-		/* pin-controller instance 1 data */
-		.pin_banks	= exynos5410_pin_banks1,
-		.nr_banks	= ARRAY_SIZE(exynos5410_pin_banks1),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-	}, {
-		/* pin-controller instance 2 data */
-		.pin_banks	= exynos5410_pin_banks2,
-		.nr_banks	= ARRAY_SIZE(exynos5410_pin_banks2),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-	}, {
-		/* pin-controller instance 3 data */
-		.pin_banks	= exynos5410_pin_banks3,
-		.nr_banks	= ARRAY_SIZE(exynos5410_pin_banks3),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-	},
-};
-
-/* pin banks of exynos5420 pin-controller 0 */
-static const struct samsung_pin_bank_data exynos5420_pin_banks0[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpy7", 0x00),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC00, "gpx0", 0x00),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC20, "gpx1", 0x04),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC40, "gpx2", 0x08),
-	EXYNOS_PIN_BANK_EINTW(8, 0xC60, "gpx3", 0x0c),
-};
-
-/* pin banks of exynos5420 pin-controller 1 */
-static const struct samsung_pin_bank_data exynos5420_pin_banks1[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpc0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpc1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(7, 0x040, "gpc2", 0x08),
-	EXYNOS_PIN_BANK_EINTG(4, 0x060, "gpc3", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(2, 0x080, "gpc4", 0x10),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0A0, "gpd1", 0x14),
-	EXYNOS_PIN_BANK_EINTN(6, 0x0C0, "gpy0"),
-	EXYNOS_PIN_BANK_EINTN(4, 0x0E0, "gpy1"),
-	EXYNOS_PIN_BANK_EINTN(6, 0x100, "gpy2"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x120, "gpy3"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x140, "gpy4"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x160, "gpy5"),
-	EXYNOS_PIN_BANK_EINTN(8, 0x180, "gpy6"),
-};
-
-/* pin banks of exynos5420 pin-controller 2 */
-static const struct samsung_pin_bank_data exynos5420_pin_banks2[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpe0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(2, 0x020, "gpe1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(6, 0x040, "gpf0", 0x08),
-	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpf1", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x080, "gpg0", 0x10),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0A0, "gpg1", 0x14),
-	EXYNOS_PIN_BANK_EINTG(2, 0x0C0, "gpg2", 0x18),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0E0, "gpj4", 0x1c),
-};
-
-/* pin banks of exynos5420 pin-controller 3 */
-static const struct samsung_pin_bank_data exynos5420_pin_banks3[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpa2", 0x08),
-	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpb0", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpb1", 0x10),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0A0, "gpb2", 0x14),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0C0, "gpb3", 0x18),
-	EXYNOS_PIN_BANK_EINTG(2, 0x0E0, "gpb4", 0x1c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x100, "gph0", 0x20),
-};
-
-/* pin banks of exynos5420 pin-controller 4 */
-static const struct samsung_pin_bank_data exynos5420_pin_banks4[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz", 0x00),
-};
-
-/* PMU pad retention groups registers for Exynos5420 (without audio) */
-static const u32 exynos5420_retention_regs[] = {
-	EXYNOS_PAD_RET_DRAM_OPTION,
-	EXYNOS_PAD_RET_JTAG_OPTION,
-	EXYNOS5420_PAD_RET_GPIO_OPTION,
-	EXYNOS5420_PAD_RET_UART_OPTION,
-	EXYNOS5420_PAD_RET_MMCA_OPTION,
-	EXYNOS5420_PAD_RET_MMCB_OPTION,
-	EXYNOS5420_PAD_RET_MMCC_OPTION,
-	EXYNOS5420_PAD_RET_HSI_OPTION,
-	EXYNOS_PAD_RET_EBIA_OPTION,
-	EXYNOS_PAD_RET_EBIB_OPTION,
-	EXYNOS5420_PAD_RET_SPI_OPTION,
-	EXYNOS5420_PAD_RET_DRAM_COREBLK_OPTION,
-};
-
-static const struct samsung_retention_data exynos5420_retention_data __initconst = {
-	.regs	 = exynos5420_retention_regs,
-	.nr_regs = ARRAY_SIZE(exynos5420_retention_regs),
-	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
-	.refcnt	 = &exynos_shared_retention_refcnt,
-	.init	 = exynos_retention_init,
-};
-
-/*
- * Samsung pinctrl driver data for Exynos5420 SoC. Exynos5420 SoC includes
- * four gpio/pin-mux/pinconfig controllers.
- */
-const struct samsung_pin_ctrl exynos5420_pin_ctrl[] __initconst = {
-	{
-		/* pin-controller instance 0 data */
-		.pin_banks	= exynos5420_pin_banks0,
-		.nr_banks	= ARRAY_SIZE(exynos5420_pin_banks0),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.eint_wkup_init = exynos_eint_wkup_init,
-		.retention_data	= &exynos5420_retention_data,
-	}, {
-		/* pin-controller instance 1 data */
-		.pin_banks	= exynos5420_pin_banks1,
-		.nr_banks	= ARRAY_SIZE(exynos5420_pin_banks1),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.retention_data	= &exynos5420_retention_data,
-	}, {
-		/* pin-controller instance 2 data */
-		.pin_banks	= exynos5420_pin_banks2,
-		.nr_banks	= ARRAY_SIZE(exynos5420_pin_banks2),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.retention_data	= &exynos5420_retention_data,
-	}, {
-		/* pin-controller instance 3 data */
-		.pin_banks	= exynos5420_pin_banks3,
-		.nr_banks	= ARRAY_SIZE(exynos5420_pin_banks3),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.retention_data	= &exynos5420_retention_data,
-	}, {
-		/* pin-controller instance 4 data */
-		.pin_banks	= exynos5420_pin_banks4,
-		.nr_banks	= ARRAY_SIZE(exynos5420_pin_banks4),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.retention_data	= &exynos4_audio_retention_data,
-	},
-};
-
-/* pin banks of exynos5433 pin-controller - ALIVE */
-static const struct samsung_pin_bank_data exynos5433_pin_banks0[] __initconst = {
-	EXYNOS5433_PIN_BANK_EINTW(8, 0x000, "gpa0", 0x00),
-	EXYNOS5433_PIN_BANK_EINTW(8, 0x020, "gpa1", 0x04),
-	EXYNOS5433_PIN_BANK_EINTW(8, 0x040, "gpa2", 0x08),
-	EXYNOS5433_PIN_BANK_EINTW(8, 0x060, "gpa3", 0x0c),
-	EXYNOS5433_PIN_BANK_EINTW_EXT(8, 0x020, "gpf1", 0x1004, 1),
-	EXYNOS5433_PIN_BANK_EINTW_EXT(4, 0x040, "gpf2", 0x1008, 1),
-	EXYNOS5433_PIN_BANK_EINTW_EXT(4, 0x060, "gpf3", 0x100c, 1),
-	EXYNOS5433_PIN_BANK_EINTW_EXT(8, 0x080, "gpf4", 0x1010, 1),
-	EXYNOS5433_PIN_BANK_EINTW_EXT(8, 0x0a0, "gpf5", 0x1014, 1),
-};
-
-/* pin banks of exynos5433 pin-controller - AUD */
-static const struct samsung_pin_bank_data exynos5433_pin_banks1[] __initconst = {
-	EXYNOS5433_PIN_BANK_EINTG(7, 0x000, "gpz0", 0x00),
-	EXYNOS5433_PIN_BANK_EINTG(4, 0x020, "gpz1", 0x04),
-};
-
-/* pin banks of exynos5433 pin-controller - CPIF */
-static const struct samsung_pin_bank_data exynos5433_pin_banks2[] __initconst = {
-	EXYNOS5433_PIN_BANK_EINTG(2, 0x000, "gpv6", 0x00),
-};
-
-/* pin banks of exynos5433 pin-controller - eSE */
-static const struct samsung_pin_bank_data exynos5433_pin_banks3[] __initconst = {
-	EXYNOS5433_PIN_BANK_EINTG(3, 0x000, "gpj2", 0x00),
-};
-
-/* pin banks of exynos5433 pin-controller - FINGER */
-static const struct samsung_pin_bank_data exynos5433_pin_banks4[] __initconst = {
-	EXYNOS5433_PIN_BANK_EINTG(4, 0x000, "gpd5", 0x00),
-};
-
-/* pin banks of exynos5433 pin-controller - FSYS */
-static const struct samsung_pin_bank_data exynos5433_pin_banks5[] __initconst = {
-	EXYNOS5433_PIN_BANK_EINTG(6, 0x000, "gph1", 0x00),
-	EXYNOS5433_PIN_BANK_EINTG(7, 0x020, "gpr4", 0x04),
-	EXYNOS5433_PIN_BANK_EINTG(5, 0x040, "gpr0", 0x08),
-	EXYNOS5433_PIN_BANK_EINTG(8, 0x060, "gpr1", 0x0c),
-	EXYNOS5433_PIN_BANK_EINTG(2, 0x080, "gpr2", 0x10),
-	EXYNOS5433_PIN_BANK_EINTG(8, 0x0a0, "gpr3", 0x14),
-};
-
-/* pin banks of exynos5433 pin-controller - IMEM */
-static const struct samsung_pin_bank_data exynos5433_pin_banks6[] __initconst = {
-	EXYNOS5433_PIN_BANK_EINTG(8, 0x000, "gpf0", 0x00),
-};
-
-/* pin banks of exynos5433 pin-controller - NFC */
-static const struct samsung_pin_bank_data exynos5433_pin_banks7[] __initconst = {
-	EXYNOS5433_PIN_BANK_EINTG(3, 0x000, "gpj0", 0x00),
-};
-
-/* pin banks of exynos5433 pin-controller - PERIC */
-static const struct samsung_pin_bank_data exynos5433_pin_banks8[] __initconst = {
-	EXYNOS5433_PIN_BANK_EINTG(6, 0x000, "gpv7", 0x00),
-	EXYNOS5433_PIN_BANK_EINTG(5, 0x020, "gpb0", 0x04),
-	EXYNOS5433_PIN_BANK_EINTG(8, 0x040, "gpc0", 0x08),
-	EXYNOS5433_PIN_BANK_EINTG(2, 0x060, "gpc1", 0x0c),
-	EXYNOS5433_PIN_BANK_EINTG(6, 0x080, "gpc2", 0x10),
-	EXYNOS5433_PIN_BANK_EINTG(8, 0x0a0, "gpc3", 0x14),
-	EXYNOS5433_PIN_BANK_EINTG(2, 0x0c0, "gpg0", 0x18),
-	EXYNOS5433_PIN_BANK_EINTG(4, 0x0e0, "gpd0", 0x1c),
-	EXYNOS5433_PIN_BANK_EINTG(6, 0x100, "gpd1", 0x20),
-	EXYNOS5433_PIN_BANK_EINTG(8, 0x120, "gpd2", 0x24),
-	EXYNOS5433_PIN_BANK_EINTG(5, 0x140, "gpd4", 0x28),
-	EXYNOS5433_PIN_BANK_EINTG(2, 0x160, "gpd8", 0x2c),
-	EXYNOS5433_PIN_BANK_EINTG(7, 0x180, "gpd6", 0x30),
-	EXYNOS5433_PIN_BANK_EINTG(3, 0x1a0, "gpd7", 0x34),
-	EXYNOS5433_PIN_BANK_EINTG(5, 0x1c0, "gpg1", 0x38),
-	EXYNOS5433_PIN_BANK_EINTG(2, 0x1e0, "gpg2", 0x3c),
-	EXYNOS5433_PIN_BANK_EINTG(8, 0x200, "gpg3", 0x40),
-};
-
-/* pin banks of exynos5433 pin-controller - TOUCH */
-static const struct samsung_pin_bank_data exynos5433_pin_banks9[] __initconst = {
-	EXYNOS5433_PIN_BANK_EINTG(3, 0x000, "gpj1", 0x00),
-};
-
-/* PMU pin retention groups registers for Exynos5433 (without audio & fsys) */
-static const u32 exynos5433_retention_regs[] = {
-	EXYNOS5433_PAD_RETENTION_TOP_OPTION,
-	EXYNOS5433_PAD_RETENTION_UART_OPTION,
-	EXYNOS5433_PAD_RETENTION_EBIA_OPTION,
-	EXYNOS5433_PAD_RETENTION_EBIB_OPTION,
-	EXYNOS5433_PAD_RETENTION_SPI_OPTION,
-	EXYNOS5433_PAD_RETENTION_MIF_OPTION,
-	EXYNOS5433_PAD_RETENTION_USBXTI_OPTION,
-	EXYNOS5433_PAD_RETENTION_BOOTLDO_OPTION,
-	EXYNOS5433_PAD_RETENTION_UFS_OPTION,
-	EXYNOS5433_PAD_RETENTION_FSYSGENIO_OPTION,
-};
-
-static const struct samsung_retention_data exynos5433_retention_data __initconst = {
-	.regs	 = exynos5433_retention_regs,
-	.nr_regs = ARRAY_SIZE(exynos5433_retention_regs),
-	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
-	.refcnt	 = &exynos_shared_retention_refcnt,
-	.init	 = exynos_retention_init,
-};
-
-/* PMU retention control for audio pins can be tied to audio pin bank */
-static const u32 exynos5433_audio_retention_regs[] = {
-	EXYNOS5433_PAD_RETENTION_AUD_OPTION,
-};
-
-static const struct samsung_retention_data exynos5433_audio_retention_data __initconst = {
-	.regs	 = exynos5433_audio_retention_regs,
-	.nr_regs = ARRAY_SIZE(exynos5433_audio_retention_regs),
-	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
-	.init	 = exynos_retention_init,
-};
-
-/* PMU retention control for mmc pins can be tied to fsys pin bank */
-static const u32 exynos5433_fsys_retention_regs[] = {
-	EXYNOS5433_PAD_RETENTION_MMC0_OPTION,
-	EXYNOS5433_PAD_RETENTION_MMC1_OPTION,
-	EXYNOS5433_PAD_RETENTION_MMC2_OPTION,
-};
-
-static const struct samsung_retention_data exynos5433_fsys_retention_data __initconst = {
-	.regs	 = exynos5433_fsys_retention_regs,
-	.nr_regs = ARRAY_SIZE(exynos5433_fsys_retention_regs),
-	.value	 = EXYNOS_WAKEUP_FROM_LOWPWR,
-	.init	 = exynos_retention_init,
-};
-
-/*
- * Samsung pinctrl driver data for Exynos5433 SoC. Exynos5433 SoC includes
- * ten gpio/pin-mux/pinconfig controllers.
- */
-const struct samsung_pin_ctrl exynos5433_pin_ctrl[] __initconst = {
-	{
-		/* pin-controller instance 0 data */
-		.pin_banks	= exynos5433_pin_banks0,
-		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks0),
-		.eint_wkup_init = exynos_eint_wkup_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.nr_ext_resources = 1,
-		.retention_data	= &exynos5433_retention_data,
-	}, {
-		/* pin-controller instance 1 data */
-		.pin_banks	= exynos5433_pin_banks1,
-		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks1),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos5433_audio_retention_data,
-	}, {
-		/* pin-controller instance 2 data */
-		.pin_banks	= exynos5433_pin_banks2,
-		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks2),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos5433_retention_data,
-	}, {
-		/* pin-controller instance 3 data */
-		.pin_banks	= exynos5433_pin_banks3,
-		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks3),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos5433_retention_data,
-	}, {
-		/* pin-controller instance 4 data */
-		.pin_banks	= exynos5433_pin_banks4,
-		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks4),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos5433_retention_data,
-	}, {
-		/* pin-controller instance 5 data */
-		.pin_banks	= exynos5433_pin_banks5,
-		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks5),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos5433_fsys_retention_data,
-	}, {
-		/* pin-controller instance 6 data */
-		.pin_banks	= exynos5433_pin_banks6,
-		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks6),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos5433_retention_data,
-	}, {
-		/* pin-controller instance 7 data */
-		.pin_banks	= exynos5433_pin_banks7,
-		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks7),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos5433_retention_data,
-	}, {
-		/* pin-controller instance 8 data */
-		.pin_banks	= exynos5433_pin_banks8,
-		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks8),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos5433_retention_data,
-	}, {
-		/* pin-controller instance 9 data */
-		.pin_banks	= exynos5433_pin_banks9,
-		.nr_banks	= ARRAY_SIZE(exynos5433_pin_banks9),
-		.eint_gpio_init = exynos_eint_gpio_init,
-		.suspend	= exynos_pinctrl_suspend,
-		.resume		= exynos_pinctrl_resume,
-		.retention_data	= &exynos5433_retention_data,
-	},
-};
-
-/* pin banks of exynos7 pin-controller - ALIVE */
-static const struct samsung_pin_bank_data exynos7_pin_banks0[] __initconst = {
-	EXYNOS_PIN_BANK_EINTW(8, 0x000, "gpa0", 0x00),
-	EXYNOS_PIN_BANK_EINTW(8, 0x020, "gpa1", 0x04),
-	EXYNOS_PIN_BANK_EINTW(8, 0x040, "gpa2", 0x08),
-	EXYNOS_PIN_BANK_EINTW(8, 0x060, "gpa3", 0x0c),
-};
-
-/* pin banks of exynos7 pin-controller - BUS0 */
-static const struct samsung_pin_bank_data exynos7_pin_banks1[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(5, 0x000, "gpb0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpc0", 0x04),
-	EXYNOS_PIN_BANK_EINTG(2, 0x040, "gpc1", 0x08),
-	EXYNOS_PIN_BANK_EINTG(6, 0x060, "gpc2", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x080, "gpc3", 0x10),
-	EXYNOS_PIN_BANK_EINTG(4, 0x0a0, "gpd0", 0x14),
-	EXYNOS_PIN_BANK_EINTG(6, 0x0c0, "gpd1", 0x18),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0e0, "gpd2", 0x1c),
-	EXYNOS_PIN_BANK_EINTG(5, 0x100, "gpd4", 0x20),
-	EXYNOS_PIN_BANK_EINTG(4, 0x120, "gpd5", 0x24),
-	EXYNOS_PIN_BANK_EINTG(6, 0x140, "gpd6", 0x28),
-	EXYNOS_PIN_BANK_EINTG(3, 0x160, "gpd7", 0x2c),
-	EXYNOS_PIN_BANK_EINTG(2, 0x180, "gpd8", 0x30),
-	EXYNOS_PIN_BANK_EINTG(2, 0x1a0, "gpg0", 0x34),
-	EXYNOS_PIN_BANK_EINTG(4, 0x1c0, "gpg3", 0x38),
-};
-
-/* pin banks of exynos7 pin-controller - NFC */
-static const struct samsung_pin_bank_data exynos7_pin_banks2[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(3, 0x000, "gpj0", 0x00),
-};
-
-/* pin banks of exynos7 pin-controller - TOUCH */
-static const struct samsung_pin_bank_data exynos7_pin_banks3[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(3, 0x000, "gpj1", 0x00),
-};
-
-/* pin banks of exynos7 pin-controller - FF */
-static const struct samsung_pin_bank_data exynos7_pin_banks4[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(4, 0x000, "gpg4", 0x00),
-};
-
-/* pin banks of exynos7 pin-controller - ESE */
-static const struct samsung_pin_bank_data exynos7_pin_banks5[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(5, 0x000, "gpv7", 0x00),
-};
-
-/* pin banks of exynos7 pin-controller - FSYS0 */
-static const struct samsung_pin_bank_data exynos7_pin_banks6[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpr4", 0x00),
-};
-
-/* pin banks of exynos7 pin-controller - FSYS1 */
-static const struct samsung_pin_bank_data exynos7_pin_banks7[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(4, 0x000, "gpr0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpr1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(5, 0x040, "gpr2", 0x08),
-	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpr3", 0x0c),
-};
-
-/* pin banks of exynos7 pin-controller - BUS1 */
-static const struct samsung_pin_bank_data exynos7_pin_banks8[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpf0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpf1", 0x04),
-	EXYNOS_PIN_BANK_EINTG(4, 0x060, "gpf2", 0x08),
-	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpf3", 0x0c),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0a0, "gpf4", 0x10),
-	EXYNOS_PIN_BANK_EINTG(8, 0x0c0, "gpf5", 0x14),
-	EXYNOS_PIN_BANK_EINTG(5, 0x0e0, "gpg1", 0x18),
-	EXYNOS_PIN_BANK_EINTG(5, 0x100, "gpg2", 0x1c),
-	EXYNOS_PIN_BANK_EINTG(6, 0x120, "gph1", 0x20),
-	EXYNOS_PIN_BANK_EINTG(3, 0x140, "gpv6", 0x24),
-};
-
-static const struct samsung_pin_bank_data exynos7_pin_banks9[] __initconst = {
-	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz0", 0x00),
-	EXYNOS_PIN_BANK_EINTG(4, 0x020, "gpz1", 0x04),
-};
-
-const struct samsung_pin_ctrl exynos7_pin_ctrl[] __initconst = {
-	{
-		/* pin-controller instance 0 Alive data */
-		.pin_banks	= exynos7_pin_banks0,
-		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks0),
-		.eint_wkup_init = exynos_eint_wkup_init,
-	}, {
-		/* pin-controller instance 1 BUS0 data */
-		.pin_banks	= exynos7_pin_banks1,
-		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks1),
-		.eint_gpio_init = exynos_eint_gpio_init,
-	}, {
-		/* pin-controller instance 2 NFC data */
-		.pin_banks	= exynos7_pin_banks2,
-		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks2),
-		.eint_gpio_init = exynos_eint_gpio_init,
-	}, {
-		/* pin-controller instance 3 TOUCH data */
-		.pin_banks	= exynos7_pin_banks3,
-		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks3),
-		.eint_gpio_init = exynos_eint_gpio_init,
-	}, {
-		/* pin-controller instance 4 FF data */
-		.pin_banks	= exynos7_pin_banks4,
-		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks4),
-		.eint_gpio_init = exynos_eint_gpio_init,
-	}, {
-		/* pin-controller instance 5 ESE data */
-		.pin_banks	= exynos7_pin_banks5,
-		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks5),
-		.eint_gpio_init = exynos_eint_gpio_init,
-	}, {
-		/* pin-controller instance 6 FSYS0 data */
-		.pin_banks	= exynos7_pin_banks6,
-		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks6),
-		.eint_gpio_init = exynos_eint_gpio_init,
-	}, {
-		/* pin-controller instance 7 FSYS1 data */
-		.pin_banks	= exynos7_pin_banks7,
-		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks7),
-		.eint_gpio_init = exynos_eint_gpio_init,
-	}, {
-		/* pin-controller instance 8 BUS1 data */
-		.pin_banks	= exynos7_pin_banks8,
-		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks8),
-		.eint_gpio_init = exynos_eint_gpio_init,
-	}, {
-		/* pin-controller instance 9 AUD data */
-		.pin_banks	= exynos7_pin_banks9,
-		.nr_banks	= ARRAY_SIZE(exynos7_pin_banks9),
-		.eint_gpio_init = exynos_eint_gpio_init,
-	},
-};
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos.h b/drivers/pinctrl/samsung/pinctrl-exynos.h
index cd046eb7d705..b90139715c8f 100644
--- a/drivers/pinctrl/samsung/pinctrl-exynos.h
+++ b/drivers/pinctrl/samsung/pinctrl-exynos.h
@@ -17,6 +17,9 @@
  * (at your option) any later version.
  */
 
+#ifndef __PINCTRL_SAMSUNG_EXYNOS_H
+#define __PINCTRL_SAMSUNG_EXYNOS_H
+
 /* External GPIO and wakeup interrupt related definitions */
 #define EXYNOS_GPIO_ECON_OFFSET		0x700
 #define EXYNOS_GPIO_EFLTCON_OFFSET	0x800
@@ -131,3 +134,13 @@ struct exynos_muxed_weint_data {
 	unsigned int nr_banks;
 	struct samsung_pin_bank *banks[];
 };
+
+int exynos_eint_gpio_init(struct samsung_pinctrl_drv_data *d);
+int exynos_eint_wkup_init(struct samsung_pinctrl_drv_data *d);
+void exynos_pinctrl_suspend(struct samsung_pinctrl_drv_data *drvdata);
+void exynos_pinctrl_resume(struct samsung_pinctrl_drv_data *drvdata);
+struct samsung_retention_ctrl *
+exynos_retention_init(struct samsung_pinctrl_drv_data *drvdata,
+		      const struct samsung_retention_data *data);
+
+#endif /* __PINCTRL_SAMSUNG_EXYNOS_H */
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos5440.c b/drivers/pinctrl/samsung/pinctrl-exynos5440.c
index 3000df80709f..32a3a9fd65c4 100644
--- a/drivers/pinctrl/samsung/pinctrl-exynos5440.c
+++ b/drivers/pinctrl/samsung/pinctrl-exynos5440.c
@@ -1,6 +1,8 @@
 /*
  * pin-controller/pin-mux/pin-config/gpio-driver for Samsung's EXYNOS5440 SoC.
  *
+ * Author: Thomas Abraham <thomas.ab@samsung.com>
+ *
  * Copyright (c) 2012 Samsung Electronics Co., Ltd.
  *		http://www.samsung.com
  *
@@ -10,7 +12,7 @@
  * (at your option) any later version.
  */
 
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/io.h>
 #include <linux/slab.h>
@@ -991,7 +993,6 @@ static const struct of_device_id exynos5440_pinctrl_dt_match[] = {
 	{ .compatible = "samsung,exynos5440-pinctrl" },
 	{},
 };
-MODULE_DEVICE_TABLE(of, exynos5440_pinctrl_dt_match);
 
 static struct platform_driver exynos5440_pinctrl_driver = {
 	.probe		= exynos5440_pinctrl_probe,
@@ -1007,13 +1008,3 @@ static int __init exynos5440_pinctrl_drv_register(void)
 	return platform_driver_register(&exynos5440_pinctrl_driver);
 }
 postcore_initcall(exynos5440_pinctrl_drv_register);
-
-static void __exit exynos5440_pinctrl_drv_unregister(void)
-{
-	platform_driver_unregister(&exynos5440_pinctrl_driver);
-}
-module_exit(exynos5440_pinctrl_drv_unregister);
-
-MODULE_AUTHOR("Thomas Abraham <thomas.ab@samsung.com>");
-MODULE_DESCRIPTION("Samsung EXYNOS5440 SoC pinctrl driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/samsung/pinctrl-s3c24xx.c b/drivers/pinctrl/samsung/pinctrl-s3c24xx.c
index b82a003546ae..49774851e84a 100644
--- a/drivers/pinctrl/samsung/pinctrl-s3c24xx.c
+++ b/drivers/pinctrl/samsung/pinctrl-s3c24xx.c
@@ -13,7 +13,7 @@
  * external gpio and wakeup interrupt support.
  */
 
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/device.h>
 #include <linux/interrupt.h>
 #include <linux/irqdomain.h>
diff --git a/drivers/pinctrl/samsung/pinctrl-s3c64xx.c b/drivers/pinctrl/samsung/pinctrl-s3c64xx.c
index f17890aa6e25..4a88d7446e87 100644
--- a/drivers/pinctrl/samsung/pinctrl-s3c64xx.c
+++ b/drivers/pinctrl/samsung/pinctrl-s3c64xx.c
@@ -15,7 +15,7 @@
  * external gpio and wakeup interrupt support.
  */
 
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/device.h>
 #include <linux/interrupt.h>
 #include <linux/irqdomain.h>
diff --git a/drivers/pinctrl/samsung/pinctrl-samsung.c b/drivers/pinctrl/samsung/pinctrl-samsung.c
index a4a0da5d2a32..f542642eed8d 100644
--- a/drivers/pinctrl/samsung/pinctrl-samsung.c
+++ b/drivers/pinctrl/samsung/pinctrl-samsung.c
@@ -20,7 +20,7 @@
  * and wakeup interrupts can be hooked to.
  */
 
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/io.h>
 #include <linux/slab.h>
@@ -1183,27 +1183,29 @@ static int __maybe_unused samsung_pinctrl_resume(struct device *dev)
 }
 
 static const struct of_device_id samsung_pinctrl_dt_match[] = {
-#ifdef CONFIG_PINCTRL_EXYNOS
+#ifdef CONFIG_PINCTRL_EXYNOS_ARM
 	{ .compatible = "samsung,exynos3250-pinctrl",
-		.data = (void *)exynos3250_pin_ctrl },
+		.data = exynos3250_pin_ctrl },
 	{ .compatible = "samsung,exynos4210-pinctrl",
-		.data = (void *)exynos4210_pin_ctrl },
+		.data = exynos4210_pin_ctrl },
 	{ .compatible = "samsung,exynos4x12-pinctrl",
-		.data = (void *)exynos4x12_pin_ctrl },
+		.data = exynos4x12_pin_ctrl },
 	{ .compatible = "samsung,exynos5250-pinctrl",
-		.data = (void *)exynos5250_pin_ctrl },
+		.data = exynos5250_pin_ctrl },
 	{ .compatible = "samsung,exynos5260-pinctrl",
-		.data = (void *)exynos5260_pin_ctrl },
+		.data = exynos5260_pin_ctrl },
 	{ .compatible = "samsung,exynos5410-pinctrl",
-		.data = (void *)exynos5410_pin_ctrl },
+		.data = exynos5410_pin_ctrl },
 	{ .compatible = "samsung,exynos5420-pinctrl",
-		.data = (void *)exynos5420_pin_ctrl },
-	{ .compatible = "samsung,exynos5433-pinctrl",
-		.data = (void *)exynos5433_pin_ctrl },
+		.data = exynos5420_pin_ctrl },
 	{ .compatible = "samsung,s5pv210-pinctrl",
-		.data = (void *)s5pv210_pin_ctrl },
+		.data = s5pv210_pin_ctrl },
+#endif
+#ifdef CONFIG_PINCTRL_EXYNOS_ARM64
+	{ .compatible = "samsung,exynos5433-pinctrl",
+		.data = exynos5433_pin_ctrl },
 	{ .compatible = "samsung,exynos7-pinctrl",
-		.data = (void *)exynos7_pin_ctrl },
+		.data = exynos7_pin_ctrl },
 #endif
 #ifdef CONFIG_PINCTRL_S3C64XX
 	{ .compatible = "samsung,s3c64xx-pinctrl",
@@ -1221,7 +1223,6 @@ static const struct of_device_id samsung_pinctrl_dt_match[] = {
 #endif
 	{},
 };
-MODULE_DEVICE_TABLE(of, samsung_pinctrl_dt_match);
 
 static const struct dev_pm_ops samsung_pinctrl_pm_ops = {
 	SET_LATE_SYSTEM_SLEEP_PM_OPS(samsung_pinctrl_suspend,
@@ -1243,13 +1244,3 @@ static int __init samsung_pinctrl_drv_register(void)
 	return platform_driver_register(&samsung_pinctrl_driver);
 }
 postcore_initcall(samsung_pinctrl_drv_register);
-
-static void __exit samsung_pinctrl_drv_unregister(void)
-{
-	platform_driver_unregister(&samsung_pinctrl_driver);
-}
-module_exit(samsung_pinctrl_drv_unregister);
-
-MODULE_AUTHOR("Thomas Abraham <thomas.ab@samsung.com>");
-MODULE_DESCRIPTION("Samsung pinctrl driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/sh-pfc/Kconfig b/drivers/pinctrl/sh-pfc/Kconfig
index 07eca54bdc1c..24f76a05a5a9 100644
--- a/drivers/pinctrl/sh-pfc/Kconfig
+++ b/drivers/pinctrl/sh-pfc/Kconfig
@@ -34,6 +34,16 @@ config PINCTRL_PFC_R8A7740
 	depends on ARCH_R8A7740
 	select PINCTRL_SH_PFC_GPIO
 
+config PINCTRL_PFC_R8A7743
+	def_bool y
+	depends on ARCH_R8A7743
+	select PINCTRL_SH_PFC
+
+config PINCTRL_PFC_R8A7745
+        def_bool y
+        depends on ARCH_R8A7745
+        select PINCTRL_SH_PFC
+
 config PINCTRL_PFC_R8A7778
 	def_bool y
 	depends on ARCH_R8A7778
diff --git a/drivers/pinctrl/sh-pfc/Makefile b/drivers/pinctrl/sh-pfc/Makefile
index 8e08684774af..33d28eed9ba3 100644
--- a/drivers/pinctrl/sh-pfc/Makefile
+++ b/drivers/pinctrl/sh-pfc/Makefile
@@ -3,6 +3,8 @@ obj-$(CONFIG_PINCTRL_SH_PFC_GPIO)	+= gpio.o
 obj-$(CONFIG_PINCTRL_PFC_EMEV2)	+= pfc-emev2.o
 obj-$(CONFIG_PINCTRL_PFC_R8A73A4)	+= pfc-r8a73a4.o
 obj-$(CONFIG_PINCTRL_PFC_R8A7740)	+= pfc-r8a7740.o
+obj-$(CONFIG_PINCTRL_PFC_R8A7743)	+= pfc-r8a7791.o
+obj-$(CONFIG_PINCTRL_PFC_R8A7745)	+= pfc-r8a7794.o
 obj-$(CONFIG_PINCTRL_PFC_R8A7778)	+= pfc-r8a7778.o
 obj-$(CONFIG_PINCTRL_PFC_R8A7779)	+= pfc-r8a7779.o
 obj-$(CONFIG_PINCTRL_PFC_R8A7790)	+= pfc-r8a7790.o
diff --git a/drivers/pinctrl/sh-pfc/core.c b/drivers/pinctrl/sh-pfc/core.c
index 4a5a0feb931b..e72391d5e57d 100644
--- a/drivers/pinctrl/sh-pfc/core.c
+++ b/drivers/pinctrl/sh-pfc/core.c
@@ -485,6 +485,18 @@ static const struct of_device_id sh_pfc_of_table[] = {
 		.data = &r8a7740_pinmux_info,
 	},
 #endif
+#ifdef CONFIG_PINCTRL_PFC_R8A7743
+	{
+		.compatible = "renesas,pfc-r8a7743",
+		.data = &r8a7743_pinmux_info,
+	},
+#endif
+#ifdef CONFIG_PINCTRL_PFC_R8A7745
+	{
+		.compatible = "renesas,pfc-r8a7745",
+		.data = &r8a7745_pinmux_info,
+	},
+#endif
 #ifdef CONFIG_PINCTRL_PFC_R8A7778
 	{
 		.compatible = "renesas,pfc-r8a7778",
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7791.c b/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
index 2ed7eeb50aac..4c5ffbd75be7 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
@@ -1,8 +1,8 @@
 /*
- * r8a7791 processor support - PFC hardware block.
+ * r8a7791/r8a7743 processor support - PFC hardware block.
  *
  * Copyright (C) 2013 Renesas Electronics Corporation
- * Copyright (C) 2014-2015 Cogent Embedded, Inc.
+ * Copyright (C) 2014-2017 Cogent Embedded, Inc.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2
@@ -119,22 +119,22 @@ enum {
 	/* IPSR0 */
 	FN_D0, FN_D1, FN_D2, FN_D3, FN_D4, FN_D5, FN_D6, FN_D7, FN_D8,
 	FN_D9, FN_D10, FN_D11, FN_D12, FN_D13, FN_D14, FN_D15,
-	FN_A0, FN_ATAWR0_N_C, FN_MSIOF0_SCK_B, FN_SCL0_C, FN_PWM2_B,
+	FN_A0, FN_ATAWR0_N_C, FN_MSIOF0_SCK_B, FN_I2C0_SCL_C, FN_PWM2_B,
 	FN_A1, FN_MSIOF0_SYNC_B, FN_A2, FN_MSIOF0_SS1_B,
 	FN_A3, FN_MSIOF0_SS2_B, FN_A4, FN_MSIOF0_TXD_B,
 	FN_A5, FN_MSIOF0_RXD_B, FN_A6, FN_MSIOF1_SCK,
 
 	/* IPSR1 */
-	FN_A7, FN_MSIOF1_SYNC, FN_A8, FN_MSIOF1_SS1, FN_SCL0,
-	FN_A9, FN_MSIOF1_SS2, FN_SDA0,
+	FN_A7, FN_MSIOF1_SYNC, FN_A8, FN_MSIOF1_SS1, FN_I2C0_SCL,
+	FN_A9, FN_MSIOF1_SS2, FN_I2C0_SDA,
 	FN_A10, FN_MSIOF1_TXD, FN_MSIOF1_TXD_D,
-	FN_A11, FN_MSIOF1_RXD, FN_SCL3_D, FN_MSIOF1_RXD_D,
-	FN_A12, FN_FMCLK, FN_SDA3_D, FN_MSIOF1_SCK_D,
+	FN_A11, FN_MSIOF1_RXD, FN_I2C3_SCL_D, FN_MSIOF1_RXD_D,
+	FN_A12, FN_FMCLK, FN_I2C3_SDA_D, FN_MSIOF1_SCK_D,
 	FN_A13, FN_ATAG0_N_C, FN_BPFCLK, FN_MSIOF1_SS1_D,
 	FN_A14, FN_ATADIR0_N_C, FN_FMIN, FN_FMIN_C, FN_MSIOF1_SYNC_D,
 	FN_A15, FN_BPFCLK_C,
 	FN_A16, FN_DREQ2_B, FN_FMCLK_C, FN_SCIFA1_SCK_B,
-	FN_A17, FN_DACK2_B, FN_SDA0_C,
+	FN_A17, FN_DACK2_B, FN_I2C0_SDA_C,
 	FN_A18, FN_DREQ1, FN_SCIFA1_RXD_C, FN_SCIFB1_RXD_C,
 
 	/* IPSR2 */
@@ -145,8 +145,8 @@ enum {
 	FN_A23, FN_IO2, FN_BPFCLK_B, FN_RX0, FN_SCIFA0_RXD,
 	FN_A24, FN_DREQ2, FN_IO3, FN_TX1, FN_SCIFA1_TXD,
 	FN_A25, FN_DACK2, FN_SSL, FN_DREQ1_C, FN_RX1, FN_SCIFA1_RXD,
-	FN_CS0_N, FN_ATAG0_N_B, FN_SCL1,
-	FN_CS1_N_A26, FN_ATADIR0_N_B, FN_SDA1,
+	FN_CS0_N, FN_ATAG0_N_B, FN_I2C1_SCL,
+	FN_CS1_N_A26, FN_ATADIR0_N_B, FN_I2C1_SDA,
 	FN_EX_CS1_N, FN_MSIOF2_SCK,
 	FN_EX_CS2_N, FN_ATAWR0_N, FN_MSIOF2_SYNC,
 	FN_EX_CS3_N, FN_ATADIR0_N, FN_MSIOF2_TXD, FN_ATAG0_N, FN_EX_WAIT1,
@@ -169,12 +169,13 @@ enum {
 	FN_SSI_WS0129, FN_HTX0_C, FN_HTX2_C, FN_SCIFB0_TXD_C, FN_SCIFB2_TXD_C,
 
 	/* IPSR4 */
-	FN_SSI_SDATA0, FN_SCL0_B, FN_SCL7_B, FN_MSIOF2_SCK_C,
-	FN_SSI_SCK1, FN_SDA0_B, FN_SDA7_B, FN_MSIOF2_SYNC_C, FN_GLO_I0_D,
-	FN_SSI_WS1, FN_SCL1_B, FN_SCL8_B, FN_MSIOF2_TXD_C, FN_GLO_I1_D,
-	FN_SSI_SDATA1, FN_SDA1_B, FN_SDA8_B, FN_MSIOF2_RXD_C,
-	FN_SSI_SCK2, FN_SCL2, FN_GPS_CLK_B, FN_GLO_Q0_D, FN_HSCK1_E,
-	FN_SSI_WS2, FN_SDA2, FN_GPS_SIGN_B, FN_RX2_E,
+	FN_SSI_SDATA0, FN_I2C0_SCL_B, FN_IIC0_SCL_B, FN_MSIOF2_SCK_C,
+	FN_SSI_SCK1, FN_I2C0_SDA_B, FN_IIC0_SDA_B, FN_MSIOF2_SYNC_C,
+	FN_GLO_I0_D,
+	FN_SSI_WS1, FN_I2C1_SCL_B, FN_IIC1_SCL_B, FN_MSIOF2_TXD_C, FN_GLO_I1_D,
+	FN_SSI_SDATA1, FN_I2C1_SDA_B, FN_IIC1_SDA_B, FN_MSIOF2_RXD_C,
+	FN_SSI_SCK2, FN_I2C2_SCL, FN_GPS_CLK_B, FN_GLO_Q0_D, FN_HSCK1_E,
+	FN_SSI_WS2, FN_I2C2_SDA, FN_GPS_SIGN_B, FN_RX2_E,
 	FN_GLO_Q1_D, FN_HCTS1_N_E,
 	FN_SSI_SDATA2, FN_GPS_MAG_B, FN_TX2_E, FN_HRTS1_N_E,
 	FN_SSI_SCK34, FN_SSI_WS34, FN_SSI_SDATA3,
@@ -210,10 +211,10 @@ enum {
 	FN_IRQ0, FN_SCIFB1_RXD_D, FN_INTC_IRQ0_N,
 	FN_IRQ1, FN_SCIFB1_SCK_C, FN_INTC_IRQ1_N,
 	FN_IRQ2, FN_SCIFB1_TXD_D, FN_INTC_IRQ2_N,
-	FN_IRQ3, FN_SCL4_C, FN_MSIOF2_TXD_E, FN_INTC_IRQ3_N,
-	FN_IRQ4, FN_HRX1_C, FN_SDA4_C, FN_MSIOF2_RXD_E, FN_INTC_IRQ4_N,
-	FN_IRQ5, FN_HTX1_C, FN_SCL1_E, FN_MSIOF2_SCK_E,
-	FN_IRQ6, FN_HSCK1_C, FN_MSIOF1_SS2_B, FN_SDA1_E, FN_MSIOF2_SYNC_E,
+	FN_IRQ3, FN_I2C4_SCL_C, FN_MSIOF2_TXD_E, FN_INTC_IRQ3_N,
+	FN_IRQ4, FN_HRX1_C, FN_I2C4_SDA_C, FN_MSIOF2_RXD_E, FN_INTC_IRQ4_N,
+	FN_IRQ5, FN_HTX1_C, FN_I2C1_SCL_E, FN_MSIOF2_SCK_E,
+	FN_IRQ6, FN_HSCK1_C, FN_MSIOF1_SS2_B, FN_I2C1_SDA_E, FN_MSIOF2_SYNC_E,
 	FN_IRQ7, FN_HCTS1_N_C, FN_MSIOF1_TXD_B, FN_GPS_CLK_C, FN_GPS_CLK_D,
 	FN_IRQ8, FN_HRTS1_N_C, FN_MSIOF1_RXD_B, FN_GPS_SIGN_C, FN_GPS_SIGN_D,
 
@@ -257,16 +258,16 @@ enum {
 	FN_DU1_DB5, FN_LCDOUT21, FN_TX3, FN_SCIFA3_TXD, FN_CAN1_TX,
 
 	/* IPSR9 */
-	FN_DU1_DB6, FN_LCDOUT22, FN_SCL3_C, FN_RX3, FN_SCIFA3_RXD,
-	FN_DU1_DB7, FN_LCDOUT23, FN_SDA3_C, FN_SCIF3_SCK, FN_SCIFA3_SCK,
+	FN_DU1_DB6, FN_LCDOUT22, FN_I2C3_SCL_C, FN_RX3, FN_SCIFA3_RXD,
+	FN_DU1_DB7, FN_LCDOUT23, FN_I2C3_SDA_C, FN_SCIF3_SCK, FN_SCIFA3_SCK,
 	FN_DU1_DOTCLKIN, FN_QSTVA_QVS,
 	FN_DU1_DOTCLKOUT0, FN_QCLK,
 	FN_DU1_DOTCLKOUT1, FN_QSTVB_QVE, FN_CAN0_TX,
-	FN_TX3_B, FN_SCL2_B, FN_PWM4,
+	FN_TX3_B, FN_I2C2_SCL_B, FN_PWM4,
 	FN_DU1_EXHSYNC_DU1_HSYNC, FN_QSTH_QHS,
 	FN_DU1_EXVSYNC_DU1_VSYNC, FN_QSTB_QHE,
 	FN_DU1_EXODDF_DU1_ODDF_DISP_CDE, FN_QCPV_QDE,
-	FN_CAN0_RX, FN_RX3_B, FN_SDA2_B,
+	FN_CAN0_RX, FN_RX3_B, FN_I2C2_SDA_B,
 	FN_DU1_DISP, FN_QPOLA,
 	FN_DU1_CDE, FN_QPOLB, FN_PWM4_B,
 	FN_VI0_CLKENB, FN_TX4, FN_SCIFA4_TXD, FN_TS_SDATA0_D,
@@ -274,15 +275,15 @@ enum {
 	FN_VI0_HSYNC_N, FN_TX5, FN_SCIFA5_TXD, FN_TS_SDEN0_D,
 	FN_VI0_VSYNC_N, FN_RX5, FN_SCIFA5_RXD, FN_TS_SPSYNC0_D,
 	FN_VI0_DATA3_VI0_B3, FN_SCIF3_SCK_B, FN_SCIFA3_SCK_B,
-	FN_VI0_G0, FN_SCL8, FN_STP_IVCXO27_0_C, FN_SCL4,
+	FN_VI0_G0, FN_IIC1_SCL, FN_STP_IVCXO27_0_C, FN_I2C4_SCL,
 	FN_HCTS2_N, FN_SCIFB2_CTS_N, FN_ATAWR1_N,
 
 	/* IPSR10 */
-	FN_VI0_G1, FN_SDA8, FN_STP_ISCLK_0_C, FN_SDA4,
+	FN_VI0_G1, FN_IIC1_SDA, FN_STP_ISCLK_0_C, FN_I2C4_SDA,
 	FN_HRTS2_N, FN_SCIFB2_RTS_N, FN_ATADIR1_N,
-	FN_VI0_G2, FN_VI2_HSYNC_N, FN_STP_ISD_0_C, FN_SCL3_B,
+	FN_VI0_G2, FN_VI2_HSYNC_N, FN_STP_ISD_0_C, FN_I2C3_SCL_B,
 	FN_HSCK2, FN_SCIFB2_SCK, FN_ATARD1_N,
-	FN_VI0_G3, FN_VI2_VSYNC_N, FN_STP_ISEN_0_C, FN_SDA3_B,
+	FN_VI0_G3, FN_VI2_VSYNC_N, FN_STP_ISEN_0_C, FN_I2C3_SDA_B,
 	FN_HRX2, FN_SCIFB2_RXD, FN_ATACS01_N,
 	FN_VI0_G4, FN_VI2_CLKENB, FN_STP_ISSYNC_0_C,
 	FN_HTX2, FN_SCIFB2_TXD, FN_SCIFB0_SCK_D,
@@ -296,13 +297,13 @@ enum {
 	FN_TS_SCK0_C, FN_ATAG1_N,
 	FN_VI0_R2, FN_VI2_DATA3, FN_GLO_Q0_B, FN_TS_SDEN0_C,
 	FN_VI0_R3, FN_VI2_DATA4, FN_GLO_Q1_B, FN_TS_SPSYNC0_C,
-	FN_VI0_R4, FN_VI2_DATA5, FN_GLO_SCLK_B, FN_TX0_C, FN_SCL1_D,
+	FN_VI0_R4, FN_VI2_DATA5, FN_GLO_SCLK_B, FN_TX0_C, FN_I2C1_SCL_D,
 
 	/* IPSR11 */
-	FN_VI0_R5, FN_VI2_DATA6, FN_GLO_SDATA_B, FN_RX0_C, FN_SDA1_D,
-	FN_VI0_R6, FN_VI2_DATA7, FN_GLO_SS_B, FN_TX1_C, FN_SCL4_B,
+	FN_VI0_R5, FN_VI2_DATA6, FN_GLO_SDATA_B, FN_RX0_C, FN_I2C1_SDA_D,
+	FN_VI0_R6, FN_VI2_DATA7, FN_GLO_SS_B, FN_TX1_C, FN_I2C4_SCL_B,
 	FN_VI0_R7, FN_GLO_RFON_B, FN_RX1_C, FN_CAN0_RX_E,
-	FN_SDA4_B, FN_HRX1_D, FN_SCIFB0_RXD_D,
+	FN_I2C4_SDA_B, FN_HRX1_D, FN_SCIFB0_RXD_D,
 	FN_VI1_HSYNC_N, FN_AVB_RXD0, FN_TS_SDATA0_B, FN_TX4_B, FN_SCIFA4_TXD_B,
 	FN_VI1_VSYNC_N, FN_AVB_RXD1, FN_TS_SCK0_B, FN_RX4_B, FN_SCIFA4_RXD_B,
 	FN_VI1_CLKENB, FN_AVB_RXD2, FN_TS_SDEN0_B,
@@ -312,15 +313,15 @@ enum {
 	FN_VI1_DATA3, FN_AVB_RX_ER, FN_VI1_DATA4, FN_AVB_MDIO,
 	FN_VI1_DATA5, FN_AVB_RX_DV, FN_VI1_DATA6, FN_AVB_MAGIC,
 	FN_VI1_DATA7, FN_AVB_MDC,
-	FN_ETH_MDIO, FN_AVB_RX_CLK, FN_SCL2_C,
-	FN_ETH_CRS_DV, FN_AVB_LINK, FN_SDA2_C,
+	FN_ETH_MDIO, FN_AVB_RX_CLK, FN_I2C2_SCL_C,
+	FN_ETH_CRS_DV, FN_AVB_LINK, FN_I2C2_SDA_C,
 
 	/* IPSR12 */
-	FN_ETH_RX_ER, FN_AVB_CRS, FN_SCL3, FN_SCL7,
-	FN_ETH_RXD0, FN_AVB_PHY_INT, FN_SDA3, FN_SDA7,
+	FN_ETH_RX_ER, FN_AVB_CRS, FN_I2C3_SCL, FN_IIC0_SCL,
+	FN_ETH_RXD0, FN_AVB_PHY_INT, FN_I2C3_SDA, FN_IIC0_SDA,
 	FN_ETH_RXD1, FN_AVB_GTXREFCLK, FN_CAN0_TX_C,
-	FN_SCL2_D, FN_MSIOF1_RXD_E,
-	FN_ETH_LINK, FN_AVB_TXD0, FN_CAN0_RX_C, FN_SDA2_D, FN_MSIOF1_SCK_E,
+	FN_I2C2_SCL_D, FN_MSIOF1_RXD_E,
+	FN_ETH_LINK, FN_AVB_TXD0, FN_CAN0_RX_C, FN_I2C2_SDA_D, FN_MSIOF1_SCK_E,
 	FN_ETH_REFCLK, FN_AVB_TXD1, FN_SCIFA3_RXD_B,
 	FN_CAN1_RX_C, FN_MSIOF1_SYNC_E,
 	FN_ETH_TXD1, FN_AVB_TXD2, FN_SCIFA3_TXD_B,
@@ -351,23 +352,23 @@ enum {
 	FN_SD1_CMD, FN_REMOCON_B, FN_SD1_DATA0, FN_SPEEDIN_B,
 	FN_SD1_DATA1, FN_IETX_B, FN_SD1_DATA2, FN_IECLK_B,
 	FN_SD1_DATA3, FN_IERX_B,
-	FN_SD1_CD, FN_PWM0, FN_TPU_TO0, FN_SCL1_C,
+	FN_SD1_CD, FN_PWM0, FN_TPU_TO0, FN_I2C1_SCL_C,
 
 	/* IPSR14 */
-	FN_SD1_WP, FN_PWM1_B, FN_SDA1_C,
+	FN_SD1_WP, FN_PWM1_B, FN_I2C1_SDA_C,
 	FN_SD2_CLK, FN_MMC_CLK, FN_SD2_CMD, FN_MMC_CMD,
 	FN_SD2_DATA0, FN_MMC_D0, FN_SD2_DATA1, FN_MMC_D1,
 	FN_SD2_DATA2, FN_MMC_D2, FN_SD2_DATA3, FN_MMC_D3,
-	FN_SD2_CD, FN_MMC_D4, FN_SCL8_C, FN_TX5_B, FN_SCIFA5_TXD_C,
-	FN_SD2_WP, FN_MMC_D5, FN_SDA8_C, FN_RX5_B, FN_SCIFA5_RXD_C,
+	FN_SD2_CD, FN_MMC_D4, FN_IIC1_SCL_C, FN_TX5_B, FN_SCIFA5_TXD_C,
+	FN_SD2_WP, FN_MMC_D5, FN_IIC1_SDA_C, FN_RX5_B, FN_SCIFA5_RXD_C,
 	FN_MSIOF0_SCK, FN_RX2_C, FN_ADIDATA, FN_VI1_CLK_C, FN_VI1_G0_B,
 	FN_MSIOF0_SYNC, FN_TX2_C, FN_ADICS_SAMP, FN_VI1_CLKENB_C, FN_VI1_G1_B,
 	FN_MSIOF0_TXD, FN_ADICLK, FN_VI1_FIELD_C, FN_VI1_G2_B,
 	FN_MSIOF0_RXD, FN_ADICHS0, FN_VI1_DATA0_C, FN_VI1_G3_B,
 	FN_MSIOF0_SS1, FN_MMC_D6, FN_ADICHS1, FN_TX0_E,
-	FN_VI1_HSYNC_N_C, FN_SCL7_C, FN_VI1_G4_B,
+	FN_VI1_HSYNC_N_C, FN_IIC0_SCL_C, FN_VI1_G4_B,
 	FN_MSIOF0_SS2, FN_MMC_D7, FN_ADICHS2, FN_RX0_E,
-	FN_VI1_VSYNC_N_C, FN_SDA7_C, FN_VI1_G5_B,
+	FN_VI1_VSYNC_N_C, FN_IIC0_SDA_C, FN_VI1_G5_B,
 
 	/* IPSR15 */
 	FN_SIM0_RST, FN_IETX, FN_CAN1_TX_D,
@@ -432,18 +433,18 @@ enum {
 	/* MOD_SEL3 */
 	FN_SEL_HSCIF2_0, FN_SEL_HSCIF2_1, FN_SEL_HSCIF2_2, FN_SEL_HSCIF2_3,
 	FN_SEL_CANCLK_0, FN_SEL_CANCLK_1, FN_SEL_CANCLK_2, FN_SEL_CANCLK_3,
-	FN_SEL_IIC8_0, FN_SEL_IIC8_1, FN_SEL_IIC8_2,
-	FN_SEL_IIC7_0, FN_SEL_IIC7_1, FN_SEL_IIC7_2,
-	FN_SEL_IIC4_0, FN_SEL_IIC4_1, FN_SEL_IIC4_2,
-	FN_SEL_IIC3_0, FN_SEL_IIC3_1, FN_SEL_IIC3_2, FN_SEL_IIC3_3,
+	FN_SEL_IIC1_0, FN_SEL_IIC1_1, FN_SEL_IIC1_2,
+	FN_SEL_IIC0_0, FN_SEL_IIC0_1, FN_SEL_IIC0_2,
+	FN_SEL_I2C4_0, FN_SEL_I2C4_1, FN_SEL_I2C4_2,
+	FN_SEL_I2C3_0, FN_SEL_I2C3_1, FN_SEL_I2C3_2, FN_SEL_I2C3_3,
 	FN_SEL_SCIF3_0, FN_SEL_SCIF3_1, FN_SEL_SCIF3_2, FN_SEL_SCIF3_3,
 	FN_SEL_IEB_0, FN_SEL_IEB_1, FN_SEL_IEB_2,
 	FN_SEL_MMC_0, FN_SEL_MMC_1,
 	FN_SEL_SCIF5_0, FN_SEL_SCIF5_1,
-	FN_SEL_IIC2_0, FN_SEL_IIC2_1, FN_SEL_IIC2_2, FN_SEL_IIC2_3,
-	FN_SEL_IIC1_0, FN_SEL_IIC1_1, FN_SEL_IIC1_2, FN_SEL_IIC1_3,
-	FN_SEL_IIC1_4,
-	FN_SEL_IIC0_0, FN_SEL_IIC0_1, FN_SEL_IIC0_2,
+	FN_SEL_I2C2_0, FN_SEL_I2C2_1, FN_SEL_I2C2_2, FN_SEL_I2C2_3,
+	FN_SEL_I2C1_0, FN_SEL_I2C1_1, FN_SEL_I2C1_2, FN_SEL_I2C1_3,
+	FN_SEL_I2C1_4,
+	FN_SEL_I2C0_0, FN_SEL_I2C0_1, FN_SEL_I2C0_2,
 
 	/* MOD_SEL4 */
 	FN_SEL_SOF1_0, FN_SEL_SOF1_1, FN_SEL_SOF1_2, FN_SEL_SOF1_3,
@@ -481,22 +482,23 @@ enum {
 	D0_MARK, D1_MARK, D2_MARK, D3_MARK, D4_MARK, D5_MARK,
 	D6_MARK, D7_MARK, D8_MARK,
 	D9_MARK, D10_MARK, D11_MARK, D12_MARK, D13_MARK, D14_MARK, D15_MARK,
-	A0_MARK, ATAWR0_N_C_MARK, MSIOF0_SCK_B_MARK, SCL0_C_MARK, PWM2_B_MARK,
+	A0_MARK, ATAWR0_N_C_MARK, MSIOF0_SCK_B_MARK, I2C0_SCL_C_MARK,
+	PWM2_B_MARK,
 	A1_MARK, MSIOF0_SYNC_B_MARK, A2_MARK, MSIOF0_SS1_B_MARK,
 	A3_MARK, MSIOF0_SS2_B_MARK, A4_MARK, MSIOF0_TXD_B_MARK,
 	A5_MARK, MSIOF0_RXD_B_MARK, A6_MARK, MSIOF1_SCK_MARK,
 
 	/* IPSR1 */
-	A7_MARK, MSIOF1_SYNC_MARK, A8_MARK, MSIOF1_SS1_MARK, SCL0_MARK,
-	A9_MARK, MSIOF1_SS2_MARK, SDA0_MARK,
+	A7_MARK, MSIOF1_SYNC_MARK, A8_MARK, MSIOF1_SS1_MARK, I2C0_SCL_MARK,
+	A9_MARK, MSIOF1_SS2_MARK, I2C0_SDA_MARK,
 	A10_MARK, MSIOF1_TXD_MARK, MSIOF1_TXD_D_MARK,
-	A11_MARK, MSIOF1_RXD_MARK, SCL3_D_MARK, MSIOF1_RXD_D_MARK,
-	A12_MARK, FMCLK_MARK, SDA3_D_MARK, MSIOF1_SCK_D_MARK,
+	A11_MARK, MSIOF1_RXD_MARK, I2C3_SCL_D_MARK, MSIOF1_RXD_D_MARK,
+	A12_MARK, FMCLK_MARK, I2C3_SDA_D_MARK, MSIOF1_SCK_D_MARK,
 	A13_MARK, ATAG0_N_C_MARK, BPFCLK_MARK, MSIOF1_SS1_D_MARK,
 	A14_MARK, ATADIR0_N_C_MARK, FMIN_MARK, FMIN_C_MARK, MSIOF1_SYNC_D_MARK,
 	A15_MARK, BPFCLK_C_MARK,
 	A16_MARK, DREQ2_B_MARK, FMCLK_C_MARK, SCIFA1_SCK_B_MARK,
-	A17_MARK, DACK2_B_MARK, SDA0_C_MARK,
+	A17_MARK, DACK2_B_MARK, I2C0_SDA_C_MARK,
 	A18_MARK, DREQ1_MARK, SCIFA1_RXD_C_MARK, SCIFB1_RXD_C_MARK,
 
 	/* IPSR2 */
@@ -509,8 +511,8 @@ enum {
 	A24_MARK, DREQ2_MARK, IO3_MARK, TX1_MARK, SCIFA1_TXD_MARK,
 	A25_MARK, DACK2_MARK, SSL_MARK, DREQ1_C_MARK,
 	RX1_MARK, SCIFA1_RXD_MARK,
-	CS0_N_MARK, ATAG0_N_B_MARK, SCL1_MARK,
-	CS1_N_A26_MARK, ATADIR0_N_B_MARK, SDA1_MARK,
+	CS0_N_MARK, ATAG0_N_B_MARK, I2C1_SCL_MARK,
+	CS1_N_A26_MARK, ATADIR0_N_B_MARK, I2C1_SDA_MARK,
 	EX_CS1_N_MARK, MSIOF2_SCK_MARK,
 	EX_CS2_N_MARK, ATAWR0_N_MARK, MSIOF2_SYNC_MARK,
 	EX_CS3_N_MARK, ATADIR0_N_MARK, MSIOF2_TXD_MARK,
@@ -537,14 +539,15 @@ enum {
 	SCIFB0_TXD_C_MARK, SCIFB2_TXD_C_MARK,
 
 	/* IPSR4 */
-	SSI_SDATA0_MARK, SCL0_B_MARK, SCL7_B_MARK, MSIOF2_SCK_C_MARK,
-	SSI_SCK1_MARK, SDA0_B_MARK, SDA7_B_MARK,
+	SSI_SDATA0_MARK, I2C0_SCL_B_MARK, IIC0_SCL_B_MARK, MSIOF2_SCK_C_MARK,
+	SSI_SCK1_MARK, I2C0_SDA_B_MARK, IIC0_SDA_B_MARK,
 	MSIOF2_SYNC_C_MARK, GLO_I0_D_MARK,
-	SSI_WS1_MARK, SCL1_B_MARK, SCL8_B_MARK,
+	SSI_WS1_MARK, I2C1_SCL_B_MARK, IIC1_SCL_B_MARK,
 	MSIOF2_TXD_C_MARK, GLO_I1_D_MARK,
-	SSI_SDATA1_MARK, SDA1_B_MARK, SDA8_B_MARK, MSIOF2_RXD_C_MARK,
-	SSI_SCK2_MARK, SCL2_MARK, GPS_CLK_B_MARK, GLO_Q0_D_MARK, HSCK1_E_MARK,
-	SSI_WS2_MARK, SDA2_MARK, GPS_SIGN_B_MARK, RX2_E_MARK,
+	SSI_SDATA1_MARK, I2C1_SDA_B_MARK, IIC1_SDA_B_MARK, MSIOF2_RXD_C_MARK,
+	SSI_SCK2_MARK, I2C2_SCL_MARK, GPS_CLK_B_MARK, GLO_Q0_D_MARK,
+	HSCK1_E_MARK,
+	SSI_WS2_MARK, I2C2_SDA_MARK, GPS_SIGN_B_MARK, RX2_E_MARK,
 	GLO_Q1_D_MARK, HCTS1_N_E_MARK,
 	SSI_SDATA2_MARK, GPS_MAG_B_MARK, TX2_E_MARK, HRTS1_N_E_MARK,
 	SSI_SCK34_MARK, SSI_WS34_MARK, SSI_SDATA3_MARK,
@@ -580,12 +583,12 @@ enum {
 	IRQ0_MARK, SCIFB1_RXD_D_MARK, INTC_IRQ0_N_MARK,
 	IRQ1_MARK, SCIFB1_SCK_C_MARK, INTC_IRQ1_N_MARK,
 	IRQ2_MARK, SCIFB1_TXD_D_MARK, INTC_IRQ2_N_MARK,
-	IRQ3_MARK, SCL4_C_MARK, MSIOF2_TXD_E_MARK, INTC_IRQ3_N_MARK,
-	IRQ4_MARK, HRX1_C_MARK, SDA4_C_MARK,
+	IRQ3_MARK, I2C4_SCL_C_MARK, MSIOF2_TXD_E_MARK, INTC_IRQ3_N_MARK,
+	IRQ4_MARK, HRX1_C_MARK, I2C4_SDA_C_MARK,
 	MSIOF2_RXD_E_MARK, INTC_IRQ4_N_MARK,
-	IRQ5_MARK, HTX1_C_MARK, SCL1_E_MARK, MSIOF2_SCK_E_MARK,
+	IRQ5_MARK, HTX1_C_MARK, I2C1_SCL_E_MARK, MSIOF2_SCK_E_MARK,
 	IRQ6_MARK, HSCK1_C_MARK, MSIOF1_SS2_B_MARK,
-	SDA1_E_MARK, MSIOF2_SYNC_E_MARK,
+	I2C1_SDA_E_MARK, MSIOF2_SYNC_E_MARK,
 	IRQ7_MARK, HCTS1_N_C_MARK, MSIOF1_TXD_B_MARK,
 	GPS_CLK_C_MARK, GPS_CLK_D_MARK,
 	IRQ8_MARK, HRTS1_N_C_MARK, MSIOF1_RXD_B_MARK,
@@ -632,17 +635,17 @@ enum {
 	DU1_DB5_MARK, LCDOUT21_MARK, TX3_MARK, SCIFA3_TXD_MARK, CAN1_TX_MARK,
 
 	/* IPSR9 */
-	DU1_DB6_MARK, LCDOUT22_MARK, SCL3_C_MARK, RX3_MARK, SCIFA3_RXD_MARK,
-	DU1_DB7_MARK, LCDOUT23_MARK, SDA3_C_MARK,
+	DU1_DB6_MARK, LCDOUT22_MARK, I2C3_SCL_C_MARK, RX3_MARK, SCIFA3_RXD_MARK,
+	DU1_DB7_MARK, LCDOUT23_MARK, I2C3_SDA_C_MARK,
 	SCIF3_SCK_MARK, SCIFA3_SCK_MARK,
 	DU1_DOTCLKIN_MARK, QSTVA_QVS_MARK,
 	DU1_DOTCLKOUT0_MARK, QCLK_MARK,
 	DU1_DOTCLKOUT1_MARK, QSTVB_QVE_MARK, CAN0_TX_MARK,
-	TX3_B_MARK, SCL2_B_MARK, PWM4_MARK,
+	TX3_B_MARK, I2C2_SCL_B_MARK, PWM4_MARK,
 	DU1_EXHSYNC_DU1_HSYNC_MARK, QSTH_QHS_MARK,
 	DU1_EXVSYNC_DU1_VSYNC_MARK, QSTB_QHE_MARK,
 	DU1_EXODDF_DU1_ODDF_DISP_CDE_MARK, QCPV_QDE_MARK,
-	CAN0_RX_MARK, RX3_B_MARK, SDA2_B_MARK,
+	CAN0_RX_MARK, RX3_B_MARK, I2C2_SDA_B_MARK,
 	DU1_DISP_MARK, QPOLA_MARK,
 	DU1_CDE_MARK, QPOLB_MARK, PWM4_B_MARK,
 	VI0_CLKENB_MARK, TX4_MARK, SCIFA4_TXD_MARK, TS_SDATA0_D_MARK,
@@ -650,15 +653,15 @@ enum {
 	VI0_HSYNC_N_MARK, TX5_MARK, SCIFA5_TXD_MARK, TS_SDEN0_D_MARK,
 	VI0_VSYNC_N_MARK, RX5_MARK, SCIFA5_RXD_MARK, TS_SPSYNC0_D_MARK,
 	VI0_DATA3_VI0_B3_MARK, SCIF3_SCK_B_MARK, SCIFA3_SCK_B_MARK,
-	VI0_G0_MARK, SCL8_MARK, STP_IVCXO27_0_C_MARK, SCL4_MARK,
+	VI0_G0_MARK, IIC1_SCL_MARK, STP_IVCXO27_0_C_MARK, I2C4_SCL_MARK,
 	HCTS2_N_MARK, SCIFB2_CTS_N_MARK, ATAWR1_N_MARK,
 
 	/* IPSR10 */
-	VI0_G1_MARK, SDA8_MARK, STP_ISCLK_0_C_MARK, SDA4_MARK,
+	VI0_G1_MARK, IIC1_SDA_MARK, STP_ISCLK_0_C_MARK, I2C4_SDA_MARK,
 	HRTS2_N_MARK, SCIFB2_RTS_N_MARK, ATADIR1_N_MARK,
-	VI0_G2_MARK, VI2_HSYNC_N_MARK, STP_ISD_0_C_MARK, SCL3_B_MARK,
+	VI0_G2_MARK, VI2_HSYNC_N_MARK, STP_ISD_0_C_MARK, I2C3_SCL_B_MARK,
 	HSCK2_MARK, SCIFB2_SCK_MARK, ATARD1_N_MARK,
-	VI0_G3_MARK, VI2_VSYNC_N_MARK, STP_ISEN_0_C_MARK, SDA3_B_MARK,
+	VI0_G3_MARK, VI2_VSYNC_N_MARK, STP_ISEN_0_C_MARK, I2C3_SDA_B_MARK,
 	HRX2_MARK, SCIFB2_RXD_MARK, ATACS01_N_MARK,
 	VI0_G4_MARK, VI2_CLKENB_MARK, STP_ISSYNC_0_C_MARK,
 	HTX2_MARK, SCIFB2_TXD_MARK, SCIFB0_SCK_D_MARK,
@@ -672,13 +675,15 @@ enum {
 	TS_SCK0_C_MARK, ATAG1_N_MARK,
 	VI0_R2_MARK, VI2_DATA3_MARK, GLO_Q0_B_MARK, TS_SDEN0_C_MARK,
 	VI0_R3_MARK, VI2_DATA4_MARK, GLO_Q1_B_MARK, TS_SPSYNC0_C_MARK,
-	VI0_R4_MARK, VI2_DATA5_MARK, GLO_SCLK_B_MARK, TX0_C_MARK, SCL1_D_MARK,
+	VI0_R4_MARK, VI2_DATA5_MARK, GLO_SCLK_B_MARK, TX0_C_MARK,
+	I2C1_SCL_D_MARK,
 
 	/* IPSR11 */
-	VI0_R5_MARK, VI2_DATA6_MARK, GLO_SDATA_B_MARK, RX0_C_MARK, SDA1_D_MARK,
-	VI0_R6_MARK, VI2_DATA7_MARK, GLO_SS_B_MARK, TX1_C_MARK, SCL4_B_MARK,
+	VI0_R5_MARK, VI2_DATA6_MARK, GLO_SDATA_B_MARK, RX0_C_MARK,
+	I2C1_SDA_D_MARK,
+	VI0_R6_MARK, VI2_DATA7_MARK, GLO_SS_B_MARK, TX1_C_MARK, I2C4_SCL_B_MARK,
 	VI0_R7_MARK, GLO_RFON_B_MARK, RX1_C_MARK, CAN0_RX_E_MARK,
-	SDA4_B_MARK, HRX1_D_MARK, SCIFB0_RXD_D_MARK,
+	I2C4_SDA_B_MARK, HRX1_D_MARK, SCIFB0_RXD_D_MARK,
 	VI1_HSYNC_N_MARK, AVB_RXD0_MARK, TS_SDATA0_B_MARK,
 	TX4_B_MARK, SCIFA4_TXD_B_MARK,
 	VI1_VSYNC_N_MARK, AVB_RXD1_MARK, TS_SCK0_B_MARK,
@@ -690,16 +695,16 @@ enum {
 	VI1_DATA3_MARK, AVB_RX_ER_MARK, VI1_DATA4_MARK, AVB_MDIO_MARK,
 	VI1_DATA5_MARK, AVB_RX_DV_MARK, VI1_DATA6_MARK, AVB_MAGIC_MARK,
 	VI1_DATA7_MARK, AVB_MDC_MARK,
-	ETH_MDIO_MARK, AVB_RX_CLK_MARK, SCL2_C_MARK,
-	ETH_CRS_DV_MARK, AVB_LINK_MARK, SDA2_C_MARK,
+	ETH_MDIO_MARK, AVB_RX_CLK_MARK, I2C2_SCL_C_MARK,
+	ETH_CRS_DV_MARK, AVB_LINK_MARK, I2C2_SDA_C_MARK,
 
 	/* IPSR12 */
-	ETH_RX_ER_MARK, AVB_CRS_MARK, SCL3_MARK, SCL7_MARK,
-	ETH_RXD0_MARK, AVB_PHY_INT_MARK, SDA3_MARK, SDA7_MARK,
+	ETH_RX_ER_MARK, AVB_CRS_MARK, I2C3_SCL_MARK, IIC0_SCL_MARK,
+	ETH_RXD0_MARK, AVB_PHY_INT_MARK, I2C3_SDA_MARK, IIC0_SDA_MARK,
 	ETH_RXD1_MARK, AVB_GTXREFCLK_MARK, CAN0_TX_C_MARK,
-	SCL2_D_MARK, MSIOF1_RXD_E_MARK,
+	I2C2_SCL_D_MARK, MSIOF1_RXD_E_MARK,
 	ETH_LINK_MARK, AVB_TXD0_MARK, CAN0_RX_C_MARK,
-	SDA2_D_MARK, MSIOF1_SCK_E_MARK,
+	I2C2_SDA_D_MARK, MSIOF1_SCK_E_MARK,
 	ETH_REFCLK_MARK, AVB_TXD1_MARK, SCIFA3_RXD_B_MARK,
 	CAN1_RX_C_MARK, MSIOF1_SYNC_E_MARK,
 	ETH_TXD1_MARK, AVB_TXD2_MARK, SCIFA3_TXD_B_MARK,
@@ -730,15 +735,17 @@ enum {
 	SD1_CMD_MARK, REMOCON_B_MARK, SD1_DATA0_MARK, SPEEDIN_B_MARK,
 	SD1_DATA1_MARK, IETX_B_MARK, SD1_DATA2_MARK, IECLK_B_MARK,
 	SD1_DATA3_MARK, IERX_B_MARK,
-	SD1_CD_MARK, PWM0_MARK, TPU_TO0_MARK, SCL1_C_MARK,
+	SD1_CD_MARK, PWM0_MARK, TPU_TO0_MARK, I2C1_SCL_C_MARK,
 
 	/* IPSR14 */
-	SD1_WP_MARK, PWM1_B_MARK, SDA1_C_MARK,
+	SD1_WP_MARK, PWM1_B_MARK, I2C1_SDA_C_MARK,
 	SD2_CLK_MARK, MMC_CLK_MARK, SD2_CMD_MARK, MMC_CMD_MARK,
 	SD2_DATA0_MARK, MMC_D0_MARK, SD2_DATA1_MARK, MMC_D1_MARK,
 	SD2_DATA2_MARK, MMC_D2_MARK, SD2_DATA3_MARK, MMC_D3_MARK,
-	SD2_CD_MARK, MMC_D4_MARK, SCL8_C_MARK, TX5_B_MARK, SCIFA5_TXD_C_MARK,
-	SD2_WP_MARK, MMC_D5_MARK, SDA8_C_MARK, RX5_B_MARK, SCIFA5_RXD_C_MARK,
+	SD2_CD_MARK, MMC_D4_MARK, IIC1_SCL_C_MARK, TX5_B_MARK,
+	SCIFA5_TXD_C_MARK,
+	SD2_WP_MARK, MMC_D5_MARK, IIC1_SDA_C_MARK, RX5_B_MARK,
+	SCIFA5_RXD_C_MARK,
 	MSIOF0_SCK_MARK, RX2_C_MARK, ADIDATA_MARK,
 	VI1_CLK_C_MARK, VI1_G0_B_MARK,
 	MSIOF0_SYNC_MARK, TX2_C_MARK, ADICS_SAMP_MARK,
@@ -746,9 +753,9 @@ enum {
 	MSIOF0_TXD_MARK, ADICLK_MARK, VI1_FIELD_C_MARK, VI1_G2_B_MARK,
 	MSIOF0_RXD_MARK, ADICHS0_MARK, VI1_DATA0_C_MARK, VI1_G3_B_MARK,
 	MSIOF0_SS1_MARK, MMC_D6_MARK, ADICHS1_MARK, TX0_E_MARK,
-	VI1_HSYNC_N_C_MARK, SCL7_C_MARK, VI1_G4_B_MARK,
+	VI1_HSYNC_N_C_MARK, IIC0_SCL_C_MARK, VI1_G4_B_MARK,
 	MSIOF0_SS2_MARK, MMC_D7_MARK, ADICHS2_MARK, RX0_E_MARK,
-	VI1_VSYNC_N_C_MARK, SDA7_C_MARK, VI1_G5_B_MARK,
+	VI1_VSYNC_N_C_MARK, IIC0_SDA_C_MARK, VI1_G5_B_MARK,
 
 	/* IPSR15 */
 	SIM0_RST_MARK, IETX_MARK, CAN1_TX_D_MARK,
@@ -822,7 +829,7 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_GPSR(IP0_18_16, A0),
 	PINMUX_IPSR_MSEL(IP0_18_16, ATAWR0_N_C, SEL_LBS_2),
 	PINMUX_IPSR_MSEL(IP0_18_16, MSIOF0_SCK_B, SEL_SOF0_1),
-	PINMUX_IPSR_MSEL(IP0_18_16, SCL0_C, SEL_IIC0_2),
+	PINMUX_IPSR_MSEL(IP0_18_16, I2C0_SCL_C, SEL_I2C0_2),
 	PINMUX_IPSR_GPSR(IP0_18_16, PWM2_B),
 	PINMUX_IPSR_GPSR(IP0_20_19, A1),
 	PINMUX_IPSR_MSEL(IP0_20_19, MSIOF0_SYNC_B, SEL_SOF0_1),
@@ -842,20 +849,20 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP1_1_0, MSIOF1_SYNC, SEL_SOF1_0),
 	PINMUX_IPSR_GPSR(IP1_3_2, A8),
 	PINMUX_IPSR_MSEL(IP1_3_2, MSIOF1_SS1, SEL_SOF1_0),
-	PINMUX_IPSR_MSEL(IP1_3_2, SCL0, SEL_IIC0_0),
+	PINMUX_IPSR_MSEL(IP1_3_2, I2C0_SCL, SEL_I2C0_0),
 	PINMUX_IPSR_GPSR(IP1_5_4, A9),
 	PINMUX_IPSR_MSEL(IP1_5_4, MSIOF1_SS2, SEL_SOF1_0),
-	PINMUX_IPSR_MSEL(IP1_5_4, SDA0, SEL_IIC0_0),
+	PINMUX_IPSR_MSEL(IP1_5_4, I2C0_SDA, SEL_I2C0_0),
 	PINMUX_IPSR_GPSR(IP1_7_6, A10),
 	PINMUX_IPSR_MSEL(IP1_7_6, MSIOF1_TXD, SEL_SOF1_0),
 	PINMUX_IPSR_MSEL(IP1_7_6, MSIOF1_TXD_D, SEL_SOF1_3),
 	PINMUX_IPSR_GPSR(IP1_10_8, A11),
 	PINMUX_IPSR_MSEL(IP1_10_8, MSIOF1_RXD, SEL_SOF1_0),
-	PINMUX_IPSR_MSEL(IP1_10_8, SCL3_D, SEL_IIC3_3),
+	PINMUX_IPSR_MSEL(IP1_10_8, I2C3_SCL_D, SEL_I2C3_3),
 	PINMUX_IPSR_MSEL(IP1_10_8, MSIOF1_RXD_D, SEL_SOF1_3),
 	PINMUX_IPSR_GPSR(IP1_13_11, A12),
 	PINMUX_IPSR_MSEL(IP1_13_11, FMCLK, SEL_FM_0),
-	PINMUX_IPSR_MSEL(IP1_13_11, SDA3_D, SEL_IIC3_3),
+	PINMUX_IPSR_MSEL(IP1_13_11, I2C3_SDA_D, SEL_I2C3_3),
 	PINMUX_IPSR_MSEL(IP1_13_11, MSIOF1_SCK_D, SEL_SOF1_3),
 	PINMUX_IPSR_GPSR(IP1_16_14, A13),
 	PINMUX_IPSR_MSEL(IP1_16_14, ATAG0_N_C, SEL_LBS_2),
@@ -874,7 +881,7 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP1_25_23, SCIFA1_SCK_B, SEL_SCIFA1_1),
 	PINMUX_IPSR_GPSR(IP1_28_26, A17),
 	PINMUX_IPSR_MSEL(IP1_28_26, DACK2_B, SEL_LBS_1),
-	PINMUX_IPSR_MSEL(IP1_28_26, SDA0_C, SEL_IIC0_2),
+	PINMUX_IPSR_MSEL(IP1_28_26, I2C0_SDA_C, SEL_I2C0_2),
 	PINMUX_IPSR_GPSR(IP1_31_29, A18),
 	PINMUX_IPSR_MSEL(IP1_31_29, DREQ1, SEL_LBS_0),
 	PINMUX_IPSR_MSEL(IP1_31_29, SCIFA1_RXD_C, SEL_SCIFA1_2),
@@ -914,10 +921,10 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP2_18_16, SCIFA1_RXD, SEL_SCIFA1_0),
 	PINMUX_IPSR_GPSR(IP2_20_19, CS0_N),
 	PINMUX_IPSR_MSEL(IP2_20_19, ATAG0_N_B, SEL_LBS_1),
-	PINMUX_IPSR_MSEL(IP2_20_19, SCL1, SEL_IIC1_0),
+	PINMUX_IPSR_MSEL(IP2_20_19, I2C1_SCL, SEL_I2C1_0),
 	PINMUX_IPSR_GPSR(IP2_22_21, CS1_N_A26),
 	PINMUX_IPSR_MSEL(IP2_22_21, ATADIR0_N_B, SEL_LBS_1),
-	PINMUX_IPSR_MSEL(IP2_22_21, SDA1, SEL_IIC1_0),
+	PINMUX_IPSR_MSEL(IP2_22_21, I2C1_SDA, SEL_I2C1_0),
 	PINMUX_IPSR_GPSR(IP2_24_23, EX_CS1_N),
 	PINMUX_IPSR_MSEL(IP2_24_23, MSIOF2_SCK, SEL_SOF2_0),
 	PINMUX_IPSR_GPSR(IP2_26_25, EX_CS2_N),
@@ -989,30 +996,30 @@ static const u16 pinmux_data[] = {
 
 	/* IPSR4 */
 	PINMUX_IPSR_MSEL(IP4_1_0, SSI_SDATA0, SEL_SSI0_0),
-	PINMUX_IPSR_MSEL(IP4_1_0, SCL0_B, SEL_IIC0_1),
-	PINMUX_IPSR_MSEL(IP4_1_0, SCL7_B, SEL_IIC7_1),
+	PINMUX_IPSR_MSEL(IP4_1_0, I2C0_SCL_B, SEL_I2C0_1),
+	PINMUX_IPSR_MSEL(IP4_1_0, IIC0_SCL_B, SEL_IIC0_1),
 	PINMUX_IPSR_MSEL(IP4_1_0, MSIOF2_SCK_C, SEL_SOF2_2),
 	PINMUX_IPSR_MSEL(IP4_4_2, SSI_SCK1, SEL_SSI1_0),
-	PINMUX_IPSR_MSEL(IP4_4_2, SDA0_B, SEL_IIC0_1),
-	PINMUX_IPSR_MSEL(IP4_4_2, SDA7_B, SEL_IIC7_1),
+	PINMUX_IPSR_MSEL(IP4_4_2, I2C0_SDA_B, SEL_I2C0_1),
+	PINMUX_IPSR_MSEL(IP4_4_2, IIC0_SDA_B, SEL_IIC0_1),
 	PINMUX_IPSR_MSEL(IP4_4_2, MSIOF2_SYNC_C, SEL_SOF2_2),
 	PINMUX_IPSR_MSEL(IP4_4_2, GLO_I0_D, SEL_GPS_3),
 	PINMUX_IPSR_MSEL(IP4_7_5, SSI_WS1, SEL_SSI1_0),
-	PINMUX_IPSR_MSEL(IP4_7_5, SCL1_B, SEL_IIC1_1),
-	PINMUX_IPSR_MSEL(IP4_7_5, SCL8_B, SEL_IIC8_1),
+	PINMUX_IPSR_MSEL(IP4_7_5, I2C1_SCL_B, SEL_I2C1_1),
+	PINMUX_IPSR_MSEL(IP4_7_5, IIC1_SCL_B, SEL_IIC1_1),
 	PINMUX_IPSR_MSEL(IP4_7_5, MSIOF2_TXD_C, SEL_SOF2_2),
 	PINMUX_IPSR_MSEL(IP4_7_5, GLO_I1_D, SEL_GPS_3),
 	PINMUX_IPSR_MSEL(IP4_9_8, SSI_SDATA1, SEL_SSI1_0),
-	PINMUX_IPSR_MSEL(IP4_9_8, SDA1_B, SEL_IIC1_1),
-	PINMUX_IPSR_MSEL(IP4_9_8, SDA8_B, SEL_IIC8_1),
+	PINMUX_IPSR_MSEL(IP4_9_8, I2C1_SDA_B, SEL_I2C1_1),
+	PINMUX_IPSR_MSEL(IP4_9_8, IIC1_SDA_B, SEL_IIC1_1),
 	PINMUX_IPSR_MSEL(IP4_9_8, MSIOF2_RXD_C, SEL_SOF2_2),
 	PINMUX_IPSR_GPSR(IP4_12_10, SSI_SCK2),
-	PINMUX_IPSR_MSEL(IP4_12_10, SCL2, SEL_IIC2_0),
+	PINMUX_IPSR_MSEL(IP4_12_10, I2C2_SCL, SEL_I2C2_0),
 	PINMUX_IPSR_MSEL(IP4_12_10, GPS_CLK_B, SEL_GPS_1),
 	PINMUX_IPSR_MSEL(IP4_12_10, GLO_Q0_D, SEL_GPS_3),
 	PINMUX_IPSR_MSEL(IP4_12_10, HSCK1_E, SEL_HSCIF1_4),
 	PINMUX_IPSR_GPSR(IP4_15_13, SSI_WS2),
-	PINMUX_IPSR_MSEL(IP4_15_13, SDA2, SEL_IIC2_0),
+	PINMUX_IPSR_MSEL(IP4_15_13, I2C2_SDA, SEL_I2C2_0),
 	PINMUX_IPSR_MSEL(IP4_15_13, GPS_SIGN_B, SEL_GPS_1),
 	PINMUX_IPSR_MSEL(IP4_15_13, RX2_E, SEL_SCIF2_4),
 	PINMUX_IPSR_MSEL(IP4_15_13, GLO_Q1_D, SEL_GPS_3),
@@ -1115,22 +1122,22 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP6_13_12, SCIFB1_TXD_D, SEL_SCIFB1_3),
 	PINMUX_IPSR_GPSR(IP6_13_12, INTC_IRQ2_N),
 	PINMUX_IPSR_GPSR(IP6_15_14, IRQ3),
-	PINMUX_IPSR_MSEL(IP6_15_14, SCL4_C, SEL_IIC4_2),
+	PINMUX_IPSR_MSEL(IP6_15_14, I2C4_SCL_C, SEL_I2C4_2),
 	PINMUX_IPSR_MSEL(IP6_15_14, MSIOF2_TXD_E, SEL_SOF2_4),
 	PINMUX_IPSR_GPSR(IP6_15_14, INTC_IRQ4_N),
 	PINMUX_IPSR_GPSR(IP6_18_16, IRQ4),
 	PINMUX_IPSR_MSEL(IP6_18_16, HRX1_C, SEL_HSCIF1_2),
-	PINMUX_IPSR_MSEL(IP6_18_16, SDA4_C, SEL_IIC4_2),
+	PINMUX_IPSR_MSEL(IP6_18_16, I2C4_SDA_C, SEL_I2C4_2),
 	PINMUX_IPSR_MSEL(IP6_18_16, MSIOF2_RXD_E, SEL_SOF2_4),
 	PINMUX_IPSR_GPSR(IP6_18_16, INTC_IRQ4_N),
 	PINMUX_IPSR_GPSR(IP6_20_19, IRQ5),
 	PINMUX_IPSR_MSEL(IP6_20_19, HTX1_C, SEL_HSCIF1_2),
-	PINMUX_IPSR_MSEL(IP6_20_19, SCL1_E, SEL_IIC1_4),
+	PINMUX_IPSR_MSEL(IP6_20_19, I2C1_SCL_E, SEL_I2C1_4),
 	PINMUX_IPSR_MSEL(IP6_20_19, MSIOF2_SCK_E, SEL_SOF2_4),
 	PINMUX_IPSR_GPSR(IP6_23_21, IRQ6),
 	PINMUX_IPSR_MSEL(IP6_23_21, HSCK1_C, SEL_HSCIF1_2),
 	PINMUX_IPSR_MSEL(IP6_23_21, MSIOF1_SS2_B, SEL_SOF1_1),
-	PINMUX_IPSR_MSEL(IP6_23_21, SDA1_E, SEL_IIC1_4),
+	PINMUX_IPSR_MSEL(IP6_23_21, I2C1_SDA_E, SEL_I2C1_4),
 	PINMUX_IPSR_MSEL(IP6_23_21, MSIOF2_SYNC_E, SEL_SOF2_4),
 	PINMUX_IPSR_GPSR(IP6_26_24, IRQ7),
 	PINMUX_IPSR_MSEL(IP6_26_24, HCTS1_N_C, SEL_HSCIF1_2),
@@ -1260,12 +1267,12 @@ static const u16 pinmux_data[] = {
 	/* IPSR9 */
 	PINMUX_IPSR_GPSR(IP9_2_0, DU1_DB6),
 	PINMUX_IPSR_GPSR(IP9_2_0, LCDOUT22),
-	PINMUX_IPSR_MSEL(IP9_2_0, SCL3_C, SEL_IIC3_2),
+	PINMUX_IPSR_MSEL(IP9_2_0, I2C3_SCL_C, SEL_I2C3_2),
 	PINMUX_IPSR_MSEL(IP9_2_0, RX3, SEL_SCIF3_0),
 	PINMUX_IPSR_MSEL(IP9_2_0, SCIFA3_RXD, SEL_SCIFA3_0),
 	PINMUX_IPSR_GPSR(IP9_5_3, DU1_DB7),
 	PINMUX_IPSR_GPSR(IP9_5_3, LCDOUT23),
-	PINMUX_IPSR_MSEL(IP9_5_3, SDA3_C, SEL_IIC3_2),
+	PINMUX_IPSR_MSEL(IP9_5_3, I2C3_SDA_C, SEL_I2C3_2),
 	PINMUX_IPSR_MSEL(IP9_5_3, SCIF3_SCK, SEL_SCIF3_0),
 	PINMUX_IPSR_MSEL(IP9_5_3, SCIFA3_SCK, SEL_SCIFA3_0),
 	PINMUX_IPSR_MSEL(IP9_6, DU1_DOTCLKIN, SEL_DIS_0),
@@ -1276,7 +1283,7 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_GPSR(IP9_10_8, QSTVB_QVE),
 	PINMUX_IPSR_MSEL(IP9_10_8, CAN0_TX, SEL_CAN0_0),
 	PINMUX_IPSR_MSEL(IP9_10_8, TX3_B, SEL_SCIF3_1),
-	PINMUX_IPSR_MSEL(IP9_10_8, SCL2_B, SEL_IIC2_1),
+	PINMUX_IPSR_MSEL(IP9_10_8, I2C2_SCL_B, SEL_I2C2_1),
 	PINMUX_IPSR_GPSR(IP9_10_8, PWM4),
 	PINMUX_IPSR_GPSR(IP9_11, DU1_EXHSYNC_DU1_HSYNC),
 	PINMUX_IPSR_GPSR(IP9_11, QSTH_QHS),
@@ -1286,7 +1293,7 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_GPSR(IP9_15_13, QCPV_QDE),
 	PINMUX_IPSR_MSEL(IP9_15_13, CAN0_RX, SEL_CAN0_0),
 	PINMUX_IPSR_MSEL(IP9_15_13, RX3_B, SEL_SCIF3_1),
-	PINMUX_IPSR_MSEL(IP9_15_13, SDA2_B, SEL_IIC2_1),
+	PINMUX_IPSR_MSEL(IP9_15_13, I2C2_SDA_B, SEL_I2C2_1),
 	PINMUX_IPSR_GPSR(IP9_16, DU1_DISP),
 	PINMUX_IPSR_GPSR(IP9_16, QPOLA),
 	PINMUX_IPSR_GPSR(IP9_18_17, DU1_CDE),
@@ -1312,32 +1319,32 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP9_28_27, SCIF3_SCK_B, SEL_SCIF3_1),
 	PINMUX_IPSR_MSEL(IP9_28_27, SCIFA3_SCK_B, SEL_SCIFA3_1),
 	PINMUX_IPSR_GPSR(IP9_31_29, VI0_G0),
-	PINMUX_IPSR_MSEL(IP9_31_29, SCL8, SEL_IIC8_0),
+	PINMUX_IPSR_MSEL(IP9_31_29, IIC1_SCL, SEL_IIC1_0),
 	PINMUX_IPSR_MSEL(IP9_31_29, STP_IVCXO27_0_C, SEL_SSP_2),
-	PINMUX_IPSR_MSEL(IP9_31_29, SCL4, SEL_IIC4_0),
+	PINMUX_IPSR_MSEL(IP9_31_29, I2C4_SCL, SEL_I2C4_0),
 	PINMUX_IPSR_MSEL(IP9_31_29, HCTS2_N, SEL_HSCIF2_0),
 	PINMUX_IPSR_MSEL(IP9_31_29, SCIFB2_CTS_N, SEL_SCIFB2_0),
 	PINMUX_IPSR_GPSR(IP9_31_29, ATAWR1_N),
 
 	/* IPSR10 */
 	PINMUX_IPSR_GPSR(IP10_2_0, VI0_G1),
-	PINMUX_IPSR_MSEL(IP10_2_0, SDA8, SEL_IIC8_0),
+	PINMUX_IPSR_MSEL(IP10_2_0, IIC1_SDA, SEL_IIC1_0),
 	PINMUX_IPSR_MSEL(IP10_2_0, STP_ISCLK_0_C, SEL_SSP_2),
-	PINMUX_IPSR_MSEL(IP10_2_0, SDA4, SEL_IIC4_0),
+	PINMUX_IPSR_MSEL(IP10_2_0, I2C4_SDA, SEL_I2C4_0),
 	PINMUX_IPSR_MSEL(IP10_2_0, HRTS2_N, SEL_HSCIF2_0),
 	PINMUX_IPSR_MSEL(IP10_2_0, SCIFB2_RTS_N, SEL_SCIFB2_0),
 	PINMUX_IPSR_GPSR(IP10_2_0, ATADIR1_N),
 	PINMUX_IPSR_GPSR(IP10_5_3, VI0_G2),
 	PINMUX_IPSR_GPSR(IP10_5_3, VI2_HSYNC_N),
 	PINMUX_IPSR_MSEL(IP10_5_3, STP_ISD_0_C, SEL_SSP_2),
-	PINMUX_IPSR_MSEL(IP10_5_3, SCL3_B, SEL_IIC3_1),
+	PINMUX_IPSR_MSEL(IP10_5_3, I2C3_SCL_B, SEL_I2C3_1),
 	PINMUX_IPSR_MSEL(IP10_5_3, HSCK2, SEL_HSCIF2_0),
 	PINMUX_IPSR_MSEL(IP10_5_3, SCIFB2_SCK, SEL_SCIFB2_0),
 	PINMUX_IPSR_GPSR(IP10_5_3, ATARD1_N),
 	PINMUX_IPSR_GPSR(IP10_8_6, VI0_G3),
 	PINMUX_IPSR_GPSR(IP10_8_6, VI2_VSYNC_N),
 	PINMUX_IPSR_MSEL(IP10_8_6, STP_ISEN_0_C, SEL_SSP_2),
-	PINMUX_IPSR_MSEL(IP10_8_6, SDA3_B, SEL_IIC3_1),
+	PINMUX_IPSR_MSEL(IP10_8_6, I2C3_SDA_B, SEL_I2C3_1),
 	PINMUX_IPSR_MSEL(IP10_8_6, HRX2, SEL_HSCIF2_0),
 	PINMUX_IPSR_MSEL(IP10_8_6, SCIFB2_RXD, SEL_SCIFB2_0),
 	PINMUX_IPSR_GPSR(IP10_8_6, ATACS01_N),
@@ -1382,24 +1389,24 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_GPSR(IP10_31_29, VI2_DATA5),
 	PINMUX_IPSR_MSEL(IP10_31_29, GLO_SCLK_B, SEL_GPS_1),
 	PINMUX_IPSR_MSEL(IP10_31_29, TX0_C, SEL_SCIF0_2),
-	PINMUX_IPSR_MSEL(IP10_31_29, SCL1_D, SEL_IIC1_3),
+	PINMUX_IPSR_MSEL(IP10_31_29, I2C1_SCL_D, SEL_I2C1_3),
 
 	/* IPSR11 */
 	PINMUX_IPSR_GPSR(IP11_2_0, VI0_R5),
 	PINMUX_IPSR_GPSR(IP11_2_0, VI2_DATA6),
 	PINMUX_IPSR_MSEL(IP11_2_0, GLO_SDATA_B, SEL_GPS_1),
 	PINMUX_IPSR_MSEL(IP11_2_0, RX0_C, SEL_SCIF0_2),
-	PINMUX_IPSR_MSEL(IP11_2_0, SDA1_D, SEL_IIC1_3),
+	PINMUX_IPSR_MSEL(IP11_2_0, I2C1_SDA_D, SEL_I2C1_3),
 	PINMUX_IPSR_GPSR(IP11_5_3, VI0_R6),
 	PINMUX_IPSR_GPSR(IP11_5_3, VI2_DATA7),
 	PINMUX_IPSR_MSEL(IP11_5_3, GLO_SS_B, SEL_GPS_1),
 	PINMUX_IPSR_MSEL(IP11_5_3, TX1_C, SEL_SCIF1_2),
-	PINMUX_IPSR_MSEL(IP11_5_3, SCL4_B, SEL_IIC4_1),
+	PINMUX_IPSR_MSEL(IP11_5_3, I2C4_SCL_B, SEL_I2C4_1),
 	PINMUX_IPSR_GPSR(IP11_8_6, VI0_R7),
 	PINMUX_IPSR_MSEL(IP11_8_6, GLO_RFON_B, SEL_GPS_1),
 	PINMUX_IPSR_MSEL(IP11_8_6, RX1_C, SEL_SCIF1_2),
 	PINMUX_IPSR_MSEL(IP11_8_6, CAN0_RX_E, SEL_CAN0_4),
-	PINMUX_IPSR_MSEL(IP11_8_6, SDA4_B, SEL_IIC4_1),
+	PINMUX_IPSR_MSEL(IP11_8_6, I2C4_SDA_B, SEL_I2C4_1),
 	PINMUX_IPSR_MSEL(IP11_8_6, HRX1_D, SEL_HSCIF1_3),
 	PINMUX_IPSR_MSEL(IP11_8_6, SCIFB0_RXD_D, SEL_SCIFB_3),
 	PINMUX_IPSR_MSEL(IP11_11_9, VI1_HSYNC_N, SEL_VI1_0),
@@ -1438,29 +1445,29 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_GPSR(IP11_27, AVB_MDC),
 	PINMUX_IPSR_GPSR(IP11_29_28, ETH_MDIO),
 	PINMUX_IPSR_GPSR(IP11_29_28, AVB_RX_CLK),
-	PINMUX_IPSR_MSEL(IP11_29_28, SCL2_C, SEL_IIC2_2),
+	PINMUX_IPSR_MSEL(IP11_29_28, I2C2_SCL_C, SEL_I2C2_2),
 	PINMUX_IPSR_GPSR(IP11_31_30, ETH_CRS_DV),
 	PINMUX_IPSR_GPSR(IP11_31_30, AVB_LINK),
-	PINMUX_IPSR_MSEL(IP11_31_30, SDA2_C, SEL_IIC2_2),
+	PINMUX_IPSR_MSEL(IP11_31_30, I2C2_SDA_C, SEL_I2C2_2),
 
 	/* IPSR12 */
 	PINMUX_IPSR_GPSR(IP12_1_0, ETH_RX_ER),
 	PINMUX_IPSR_GPSR(IP12_1_0, AVB_CRS),
-	PINMUX_IPSR_MSEL(IP12_1_0, SCL3, SEL_IIC3_0),
-	PINMUX_IPSR_MSEL(IP12_1_0, SCL7, SEL_IIC7_0),
+	PINMUX_IPSR_MSEL(IP12_1_0, I2C3_SCL, SEL_I2C3_0),
+	PINMUX_IPSR_MSEL(IP12_1_0, IIC0_SCL, SEL_IIC0_0),
 	PINMUX_IPSR_GPSR(IP12_3_2, ETH_RXD0),
 	PINMUX_IPSR_GPSR(IP12_3_2, AVB_PHY_INT),
-	PINMUX_IPSR_MSEL(IP12_3_2, SDA3, SEL_IIC3_0),
-	PINMUX_IPSR_MSEL(IP12_3_2, SDA7, SEL_IIC7_0),
+	PINMUX_IPSR_MSEL(IP12_3_2, I2C3_SDA, SEL_I2C3_0),
+	PINMUX_IPSR_MSEL(IP12_3_2, IIC0_SDA, SEL_IIC0_0),
 	PINMUX_IPSR_GPSR(IP12_6_4, ETH_RXD1),
 	PINMUX_IPSR_GPSR(IP12_6_4, AVB_GTXREFCLK),
 	PINMUX_IPSR_MSEL(IP12_6_4, CAN0_TX_C, SEL_CAN0_2),
-	PINMUX_IPSR_MSEL(IP12_6_4, SCL2_D, SEL_IIC2_3),
+	PINMUX_IPSR_MSEL(IP12_6_4, I2C2_SCL_D, SEL_I2C2_3),
 	PINMUX_IPSR_MSEL(IP12_6_4, MSIOF1_RXD_E, SEL_SOF1_4),
 	PINMUX_IPSR_GPSR(IP12_9_7, ETH_LINK),
 	PINMUX_IPSR_GPSR(IP12_9_7, AVB_TXD0),
 	PINMUX_IPSR_MSEL(IP12_9_7, CAN0_RX_C, SEL_CAN0_2),
-	PINMUX_IPSR_MSEL(IP12_9_7, SDA2_D, SEL_IIC2_3),
+	PINMUX_IPSR_MSEL(IP12_9_7, I2C2_SDA_D, SEL_I2C2_3),
 	PINMUX_IPSR_MSEL(IP12_9_7, MSIOF1_SCK_E, SEL_SOF1_4),
 	PINMUX_IPSR_GPSR(IP12_12_10, ETH_REFCLK),
 	PINMUX_IPSR_GPSR(IP12_12_10, AVB_TXD1),
@@ -1552,12 +1559,12 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_GPSR(IP13_30_28, SD1_CD),
 	PINMUX_IPSR_GPSR(IP13_30_28, PWM0),
 	PINMUX_IPSR_GPSR(IP13_30_28, TPU_TO0),
-	PINMUX_IPSR_MSEL(IP13_30_28, SCL1_C, SEL_IIC1_2),
+	PINMUX_IPSR_MSEL(IP13_30_28, I2C1_SCL_C, SEL_I2C1_2),
 
 	/* IPSR14 */
 	PINMUX_IPSR_GPSR(IP14_1_0, SD1_WP),
 	PINMUX_IPSR_GPSR(IP14_1_0, PWM1_B),
-	PINMUX_IPSR_MSEL(IP14_1_0, SDA1_C, SEL_IIC1_2),
+	PINMUX_IPSR_MSEL(IP14_1_0, I2C1_SDA_C, SEL_I2C1_2),
 	PINMUX_IPSR_GPSR(IP14_2, SD2_CLK),
 	PINMUX_IPSR_GPSR(IP14_2, MMC_CLK),
 	PINMUX_IPSR_GPSR(IP14_3, SD2_CMD),
@@ -1572,12 +1579,12 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_GPSR(IP14_7, MMC_D3),
 	PINMUX_IPSR_GPSR(IP14_10_8, SD2_CD),
 	PINMUX_IPSR_GPSR(IP14_10_8, MMC_D4),
-	PINMUX_IPSR_MSEL(IP14_10_8, SCL8_C, SEL_IIC8_2),
+	PINMUX_IPSR_MSEL(IP14_10_8, IIC1_SCL_C, SEL_IIC1_2),
 	PINMUX_IPSR_MSEL(IP14_10_8, TX5_B, SEL_SCIF5_1),
 	PINMUX_IPSR_MSEL(IP14_10_8, SCIFA5_TXD_C, SEL_SCIFA5_2),
 	PINMUX_IPSR_GPSR(IP14_13_11, SD2_WP),
 	PINMUX_IPSR_GPSR(IP14_13_11, MMC_D5),
-	PINMUX_IPSR_MSEL(IP14_13_11, SDA8_C, SEL_IIC8_2),
+	PINMUX_IPSR_MSEL(IP14_13_11, IIC1_SDA_C, SEL_IIC1_2),
 	PINMUX_IPSR_MSEL(IP14_13_11, RX5_B, SEL_SCIF5_1),
 	PINMUX_IPSR_MSEL(IP14_13_11, SCIFA5_RXD_C, SEL_SCIFA5_2),
 	PINMUX_IPSR_MSEL(IP14_16_14, MSIOF0_SCK, SEL_SOF0_0),
@@ -1603,14 +1610,14 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP14_28_26, ADICHS1, SEL_RAD_0),
 	PINMUX_IPSR_MSEL(IP14_28_26, TX0_E, SEL_SCIF0_4),
 	PINMUX_IPSR_MSEL(IP14_28_26, VI1_HSYNC_N_C, SEL_VI1_2),
-	PINMUX_IPSR_MSEL(IP14_28_26, SCL7_C, SEL_IIC7_2),
+	PINMUX_IPSR_MSEL(IP14_28_26, IIC0_SCL_C, SEL_IIC0_2),
 	PINMUX_IPSR_GPSR(IP14_28_26, VI1_G4_B),
 	PINMUX_IPSR_MSEL(IP14_31_29, MSIOF0_SS2, SEL_SOF0_0),
 	PINMUX_IPSR_MSEL(IP14_31_29, MMC_D7, SEL_MMC_0),
 	PINMUX_IPSR_MSEL(IP14_31_29, ADICHS2, SEL_RAD_0),
 	PINMUX_IPSR_MSEL(IP14_31_29, RX0_E, SEL_SCIF0_4),
 	PINMUX_IPSR_MSEL(IP14_31_29, VI1_VSYNC_N_C, SEL_VI1_2),
-	PINMUX_IPSR_MSEL(IP14_31_29, SDA7_C, SEL_IIC7_2),
+	PINMUX_IPSR_MSEL(IP14_31_29, IIC0_SDA_C, SEL_IIC0_2),
 	PINMUX_IPSR_GPSR(IP14_31_29, VI1_G5_B),
 
 	/* IPSR15 */
@@ -2343,21 +2350,21 @@ static const unsigned int i2c0_pins[] = {
 	RCAR_GP_PIN(0, 24), RCAR_GP_PIN(0, 25),
 };
 static const unsigned int i2c0_mux[] = {
-	SCL0_MARK, SDA0_MARK,
+	I2C0_SCL_MARK, I2C0_SDA_MARK,
 };
 static const unsigned int i2c0_b_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(2, 2), RCAR_GP_PIN(2, 3),
 };
 static const unsigned int i2c0_b_mux[] = {
-	SCL0_B_MARK, SDA0_B_MARK,
+	I2C0_SCL_B_MARK, I2C0_SDA_B_MARK,
 };
 static const unsigned int i2c0_c_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(0, 16), RCAR_GP_PIN(1, 1),
 };
 static const unsigned int i2c0_c_mux[] = {
-	SCL0_C_MARK, SDA0_C_MARK,
+	I2C0_SCL_C_MARK, I2C0_SDA_C_MARK,
 };
 /* - I2C1 ------------------------------------------------------------------- */
 static const unsigned int i2c1_pins[] = {
@@ -2365,35 +2372,35 @@ static const unsigned int i2c1_pins[] = {
 	RCAR_GP_PIN(1, 10), RCAR_GP_PIN(1, 11),
 };
 static const unsigned int i2c1_mux[] = {
-	SCL1_MARK, SDA1_MARK,
+	I2C1_SCL_MARK, I2C1_SDA_MARK,
 };
 static const unsigned int i2c1_b_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(2, 4), RCAR_GP_PIN(2, 5),
 };
 static const unsigned int i2c1_b_mux[] = {
-	SCL1_B_MARK, SDA1_B_MARK,
+	I2C1_SCL_B_MARK, I2C1_SDA_B_MARK,
 };
 static const unsigned int i2c1_c_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(6, 14), RCAR_GP_PIN(6, 15),
 };
 static const unsigned int i2c1_c_mux[] = {
-	SCL1_C_MARK, SDA1_C_MARK,
+	I2C1_SCL_C_MARK, I2C1_SDA_C_MARK,
 };
 static const unsigned int i2c1_d_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(4, 25), RCAR_GP_PIN(4, 26),
 };
 static const unsigned int i2c1_d_mux[] = {
-	SCL1_D_MARK, SDA1_D_MARK,
+	I2C1_SCL_D_MARK, I2C1_SDA_D_MARK,
 };
 static const unsigned int i2c1_e_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(7, 15), RCAR_GP_PIN(7, 16),
 };
 static const unsigned int i2c1_e_mux[] = {
-	SCL1_E_MARK, SDA1_E_MARK,
+	I2C1_SCL_E_MARK, I2C1_SDA_E_MARK,
 };
 /* - I2C2 ------------------------------------------------------------------- */
 static const unsigned int i2c2_pins[] = {
@@ -2401,28 +2408,28 @@ static const unsigned int i2c2_pins[] = {
 	RCAR_GP_PIN(2, 6), RCAR_GP_PIN(2, 7),
 };
 static const unsigned int i2c2_mux[] = {
-	SCL2_MARK, SDA2_MARK,
+	I2C2_SCL_MARK, I2C2_SDA_MARK,
 };
 static const unsigned int i2c2_b_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(3, 26), RCAR_GP_PIN(3, 29),
 };
 static const unsigned int i2c2_b_mux[] = {
-	SCL2_B_MARK, SDA2_B_MARK,
+	I2C2_SCL_B_MARK, I2C2_SDA_B_MARK,
 };
 static const unsigned int i2c2_c_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(5, 13), RCAR_GP_PIN(5, 14),
 };
 static const unsigned int i2c2_c_mux[] = {
-	SCL2_C_MARK, SDA2_C_MARK,
+	I2C2_SCL_C_MARK, I2C2_SDA_C_MARK,
 };
 static const unsigned int i2c2_d_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(5, 17), RCAR_GP_PIN(5, 18),
 };
 static const unsigned int i2c2_d_mux[] = {
-	SCL2_D_MARK, SDA2_D_MARK,
+	I2C2_SCL_D_MARK, I2C2_SDA_D_MARK,
 };
 /* - I2C3 ------------------------------------------------------------------- */
 static const unsigned int i2c3_pins[] = {
@@ -2430,28 +2437,28 @@ static const unsigned int i2c3_pins[] = {
 	RCAR_GP_PIN(5, 15), RCAR_GP_PIN(5, 16),
 };
 static const unsigned int i2c3_mux[] = {
-	SCL3_MARK, SDA3_MARK,
+	I2C3_SCL_MARK, I2C3_SDA_MARK,
 };
 static const unsigned int i2c3_b_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(4, 15), RCAR_GP_PIN(4, 16),
 };
 static const unsigned int i2c3_b_mux[] = {
-	SCL3_B_MARK, SDA3_B_MARK,
+	I2C3_SCL_B_MARK, I2C3_SDA_B_MARK,
 };
 static const unsigned int i2c3_c_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(3, 22), RCAR_GP_PIN(3, 23),
 };
 static const unsigned int i2c3_c_mux[] = {
-	SCL3_C_MARK, SDA3_C_MARK,
+	I2C3_SCL_C_MARK, I2C3_SDA_C_MARK,
 };
 static const unsigned int i2c3_d_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(0, 27), RCAR_GP_PIN(0, 28),
 };
 static const unsigned int i2c3_d_mux[] = {
-	SCL3_D_MARK, SDA3_D_MARK,
+	I2C3_SCL_D_MARK, I2C3_SDA_D_MARK,
 };
 /* - I2C4 ------------------------------------------------------------------- */
 static const unsigned int i2c4_pins[] = {
@@ -2459,21 +2466,21 @@ static const unsigned int i2c4_pins[] = {
 	RCAR_GP_PIN(4, 13), RCAR_GP_PIN(4, 14),
 };
 static const unsigned int i2c4_mux[] = {
-	SCL4_MARK, SDA4_MARK,
+	I2C4_SCL_MARK, I2C4_SDA_MARK,
 };
 static const unsigned int i2c4_b_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(4, 27), RCAR_GP_PIN(4, 28),
 };
 static const unsigned int i2c4_b_mux[] = {
-	SCL4_B_MARK, SDA4_B_MARK,
+	I2C4_SCL_B_MARK, I2C4_SDA_B_MARK,
 };
 static const unsigned int i2c4_c_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(7, 13), RCAR_GP_PIN(7, 14),
 };
 static const unsigned int i2c4_c_mux[] = {
-	SCL4_C_MARK, SDA4_C_MARK,
+	I2C4_SCL_C_MARK, I2C4_SDA_C_MARK,
 };
 /* - I2C7 ------------------------------------------------------------------- */
 static const unsigned int i2c7_pins[] = {
@@ -2481,21 +2488,21 @@ static const unsigned int i2c7_pins[] = {
 	RCAR_GP_PIN(5, 15), RCAR_GP_PIN(5, 16),
 };
 static const unsigned int i2c7_mux[] = {
-	SCL7_MARK, SDA7_MARK,
+	IIC0_SCL_MARK, IIC0_SDA_MARK,
 };
 static const unsigned int i2c7_b_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(2, 2), RCAR_GP_PIN(2, 3),
 };
 static const unsigned int i2c7_b_mux[] = {
-	SCL7_B_MARK, SDA7_B_MARK,
+	IIC0_SCL_B_MARK, IIC0_SDA_B_MARK,
 };
 static const unsigned int i2c7_c_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(6, 28), RCAR_GP_PIN(6, 29),
 };
 static const unsigned int i2c7_c_mux[] = {
-	SCL7_C_MARK, SDA7_C_MARK,
+	IIC0_SCL_C_MARK, IIC0_SDA_C_MARK,
 };
 /* - I2C8 ------------------------------------------------------------------- */
 static const unsigned int i2c8_pins[] = {
@@ -2503,21 +2510,21 @@ static const unsigned int i2c8_pins[] = {
 	RCAR_GP_PIN(4, 13), RCAR_GP_PIN(4, 14),
 };
 static const unsigned int i2c8_mux[] = {
-	SCL8_MARK, SDA8_MARK,
+	IIC1_SCL_MARK, IIC1_SDA_MARK,
 };
 static const unsigned int i2c8_b_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(2, 4), RCAR_GP_PIN(2, 5),
 };
 static const unsigned int i2c8_b_mux[] = {
-	SCL8_B_MARK, SDA8_B_MARK,
+	IIC1_SCL_B_MARK, IIC1_SDA_B_MARK,
 };
 static const unsigned int i2c8_c_pins[] = {
 	/* SCL, SDA */
 	RCAR_GP_PIN(6, 22), RCAR_GP_PIN(6, 23),
 };
 static const unsigned int i2c8_c_mux[] = {
-	SCL8_C_MARK, SDA8_C_MARK,
+	IIC1_SCL_C_MARK, IIC1_SDA_C_MARK,
 };
 /* - INTC ------------------------------------------------------------------- */
 static const unsigned int intc_irq0_pins[] = {
@@ -4412,357 +4419,364 @@ static const unsigned int vin2_clk_mux[] = {
 	VI2_CLK_MARK,
 };
 
-static const struct sh_pfc_pin_group pinmux_groups[] = {
-	SH_PFC_PIN_GROUP(adi_common),
-	SH_PFC_PIN_GROUP(adi_chsel0),
-	SH_PFC_PIN_GROUP(adi_chsel1),
-	SH_PFC_PIN_GROUP(adi_chsel2),
-	SH_PFC_PIN_GROUP(adi_common_b),
-	SH_PFC_PIN_GROUP(adi_chsel0_b),
-	SH_PFC_PIN_GROUP(adi_chsel1_b),
-	SH_PFC_PIN_GROUP(adi_chsel2_b),
-	SH_PFC_PIN_GROUP(audio_clk_a),
-	SH_PFC_PIN_GROUP(audio_clk_b),
-	SH_PFC_PIN_GROUP(audio_clk_b_b),
-	SH_PFC_PIN_GROUP(audio_clk_c),
-	SH_PFC_PIN_GROUP(audio_clkout),
-	SH_PFC_PIN_GROUP(avb_link),
-	SH_PFC_PIN_GROUP(avb_magic),
-	SH_PFC_PIN_GROUP(avb_phy_int),
-	SH_PFC_PIN_GROUP(avb_mdio),
-	SH_PFC_PIN_GROUP(avb_mii),
-	SH_PFC_PIN_GROUP(avb_gmii),
-	SH_PFC_PIN_GROUP(can0_data),
-	SH_PFC_PIN_GROUP(can0_data_b),
-	SH_PFC_PIN_GROUP(can0_data_c),
-	SH_PFC_PIN_GROUP(can0_data_d),
-	SH_PFC_PIN_GROUP(can0_data_e),
-	SH_PFC_PIN_GROUP(can0_data_f),
-	SH_PFC_PIN_GROUP(can1_data),
-	SH_PFC_PIN_GROUP(can1_data_b),
-	SH_PFC_PIN_GROUP(can1_data_c),
-	SH_PFC_PIN_GROUP(can1_data_d),
-	SH_PFC_PIN_GROUP(can_clk),
-	SH_PFC_PIN_GROUP(can_clk_b),
-	SH_PFC_PIN_GROUP(can_clk_c),
-	SH_PFC_PIN_GROUP(can_clk_d),
-	SH_PFC_PIN_GROUP(du_rgb666),
-	SH_PFC_PIN_GROUP(du_rgb888),
-	SH_PFC_PIN_GROUP(du_clk_out_0),
-	SH_PFC_PIN_GROUP(du_clk_out_1),
-	SH_PFC_PIN_GROUP(du_sync),
-	SH_PFC_PIN_GROUP(du_oddf),
-	SH_PFC_PIN_GROUP(du_cde),
-	SH_PFC_PIN_GROUP(du_disp),
-	SH_PFC_PIN_GROUP(du0_clk_in),
-	SH_PFC_PIN_GROUP(du1_clk_in),
-	SH_PFC_PIN_GROUP(du1_clk_in_b),
-	SH_PFC_PIN_GROUP(du1_clk_in_c),
-	SH_PFC_PIN_GROUP(eth_link),
-	SH_PFC_PIN_GROUP(eth_magic),
-	SH_PFC_PIN_GROUP(eth_mdio),
-	SH_PFC_PIN_GROUP(eth_rmii),
-	SH_PFC_PIN_GROUP(hscif0_data),
-	SH_PFC_PIN_GROUP(hscif0_clk),
-	SH_PFC_PIN_GROUP(hscif0_ctrl),
-	SH_PFC_PIN_GROUP(hscif0_data_b),
-	SH_PFC_PIN_GROUP(hscif0_ctrl_b),
-	SH_PFC_PIN_GROUP(hscif0_data_c),
-	SH_PFC_PIN_GROUP(hscif0_clk_c),
-	SH_PFC_PIN_GROUP(hscif1_data),
-	SH_PFC_PIN_GROUP(hscif1_clk),
-	SH_PFC_PIN_GROUP(hscif1_ctrl),
-	SH_PFC_PIN_GROUP(hscif1_data_b),
-	SH_PFC_PIN_GROUP(hscif1_data_c),
-	SH_PFC_PIN_GROUP(hscif1_clk_c),
-	SH_PFC_PIN_GROUP(hscif1_ctrl_c),
-	SH_PFC_PIN_GROUP(hscif1_data_d),
-	SH_PFC_PIN_GROUP(hscif1_data_e),
-	SH_PFC_PIN_GROUP(hscif1_clk_e),
-	SH_PFC_PIN_GROUP(hscif1_ctrl_e),
-	SH_PFC_PIN_GROUP(hscif2_data),
-	SH_PFC_PIN_GROUP(hscif2_clk),
-	SH_PFC_PIN_GROUP(hscif2_ctrl),
-	SH_PFC_PIN_GROUP(hscif2_data_b),
-	SH_PFC_PIN_GROUP(hscif2_ctrl_b),
-	SH_PFC_PIN_GROUP(hscif2_data_c),
-	SH_PFC_PIN_GROUP(hscif2_clk_c),
-	SH_PFC_PIN_GROUP(hscif2_data_d),
-	SH_PFC_PIN_GROUP(i2c0),
-	SH_PFC_PIN_GROUP(i2c0_b),
-	SH_PFC_PIN_GROUP(i2c0_c),
-	SH_PFC_PIN_GROUP(i2c1),
-	SH_PFC_PIN_GROUP(i2c1_b),
-	SH_PFC_PIN_GROUP(i2c1_c),
-	SH_PFC_PIN_GROUP(i2c1_d),
-	SH_PFC_PIN_GROUP(i2c1_e),
-	SH_PFC_PIN_GROUP(i2c2),
-	SH_PFC_PIN_GROUP(i2c2_b),
-	SH_PFC_PIN_GROUP(i2c2_c),
-	SH_PFC_PIN_GROUP(i2c2_d),
-	SH_PFC_PIN_GROUP(i2c3),
-	SH_PFC_PIN_GROUP(i2c3_b),
-	SH_PFC_PIN_GROUP(i2c3_c),
-	SH_PFC_PIN_GROUP(i2c3_d),
-	SH_PFC_PIN_GROUP(i2c4),
-	SH_PFC_PIN_GROUP(i2c4_b),
-	SH_PFC_PIN_GROUP(i2c4_c),
-	SH_PFC_PIN_GROUP(i2c7),
-	SH_PFC_PIN_GROUP(i2c7_b),
-	SH_PFC_PIN_GROUP(i2c7_c),
-	SH_PFC_PIN_GROUP(i2c8),
-	SH_PFC_PIN_GROUP(i2c8_b),
-	SH_PFC_PIN_GROUP(i2c8_c),
-	SH_PFC_PIN_GROUP(intc_irq0),
-	SH_PFC_PIN_GROUP(intc_irq1),
-	SH_PFC_PIN_GROUP(intc_irq2),
-	SH_PFC_PIN_GROUP(intc_irq3),
-	SH_PFC_PIN_GROUP(mlb_3pin),
-	SH_PFC_PIN_GROUP(mmc_data1),
-	SH_PFC_PIN_GROUP(mmc_data4),
-	SH_PFC_PIN_GROUP(mmc_data8),
-	SH_PFC_PIN_GROUP(mmc_ctrl),
-	SH_PFC_PIN_GROUP(msiof0_clk),
-	SH_PFC_PIN_GROUP(msiof0_sync),
-	SH_PFC_PIN_GROUP(msiof0_ss1),
-	SH_PFC_PIN_GROUP(msiof0_ss2),
-	SH_PFC_PIN_GROUP(msiof0_rx),
-	SH_PFC_PIN_GROUP(msiof0_tx),
-	SH_PFC_PIN_GROUP(msiof0_clk_b),
-	SH_PFC_PIN_GROUP(msiof0_sync_b),
-	SH_PFC_PIN_GROUP(msiof0_ss1_b),
-	SH_PFC_PIN_GROUP(msiof0_ss2_b),
-	SH_PFC_PIN_GROUP(msiof0_rx_b),
-	SH_PFC_PIN_GROUP(msiof0_tx_b),
-	SH_PFC_PIN_GROUP(msiof0_clk_c),
-	SH_PFC_PIN_GROUP(msiof0_sync_c),
-	SH_PFC_PIN_GROUP(msiof0_ss1_c),
-	SH_PFC_PIN_GROUP(msiof0_ss2_c),
-	SH_PFC_PIN_GROUP(msiof0_rx_c),
-	SH_PFC_PIN_GROUP(msiof0_tx_c),
-	SH_PFC_PIN_GROUP(msiof1_clk),
-	SH_PFC_PIN_GROUP(msiof1_sync),
-	SH_PFC_PIN_GROUP(msiof1_ss1),
-	SH_PFC_PIN_GROUP(msiof1_ss2),
-	SH_PFC_PIN_GROUP(msiof1_rx),
-	SH_PFC_PIN_GROUP(msiof1_tx),
-	SH_PFC_PIN_GROUP(msiof1_clk_b),
-	SH_PFC_PIN_GROUP(msiof1_sync_b),
-	SH_PFC_PIN_GROUP(msiof1_ss1_b),
-	SH_PFC_PIN_GROUP(msiof1_ss2_b),
-	SH_PFC_PIN_GROUP(msiof1_rx_b),
-	SH_PFC_PIN_GROUP(msiof1_tx_b),
-	SH_PFC_PIN_GROUP(msiof1_clk_c),
-	SH_PFC_PIN_GROUP(msiof1_sync_c),
-	SH_PFC_PIN_GROUP(msiof1_rx_c),
-	SH_PFC_PIN_GROUP(msiof1_tx_c),
-	SH_PFC_PIN_GROUP(msiof1_clk_d),
-	SH_PFC_PIN_GROUP(msiof1_sync_d),
-	SH_PFC_PIN_GROUP(msiof1_ss1_d),
-	SH_PFC_PIN_GROUP(msiof1_rx_d),
-	SH_PFC_PIN_GROUP(msiof1_tx_d),
-	SH_PFC_PIN_GROUP(msiof1_clk_e),
-	SH_PFC_PIN_GROUP(msiof1_sync_e),
-	SH_PFC_PIN_GROUP(msiof1_rx_e),
-	SH_PFC_PIN_GROUP(msiof1_tx_e),
-	SH_PFC_PIN_GROUP(msiof2_clk),
-	SH_PFC_PIN_GROUP(msiof2_sync),
-	SH_PFC_PIN_GROUP(msiof2_ss1),
-	SH_PFC_PIN_GROUP(msiof2_ss2),
-	SH_PFC_PIN_GROUP(msiof2_rx),
-	SH_PFC_PIN_GROUP(msiof2_tx),
-	SH_PFC_PIN_GROUP(msiof2_clk_b),
-	SH_PFC_PIN_GROUP(msiof2_sync_b),
-	SH_PFC_PIN_GROUP(msiof2_ss1_b),
-	SH_PFC_PIN_GROUP(msiof2_ss2_b),
-	SH_PFC_PIN_GROUP(msiof2_rx_b),
-	SH_PFC_PIN_GROUP(msiof2_tx_b),
-	SH_PFC_PIN_GROUP(msiof2_clk_c),
-	SH_PFC_PIN_GROUP(msiof2_sync_c),
-	SH_PFC_PIN_GROUP(msiof2_rx_c),
-	SH_PFC_PIN_GROUP(msiof2_tx_c),
-	SH_PFC_PIN_GROUP(msiof2_clk_d),
-	SH_PFC_PIN_GROUP(msiof2_sync_d),
-	SH_PFC_PIN_GROUP(msiof2_ss1_d),
-	SH_PFC_PIN_GROUP(msiof2_ss2_d),
-	SH_PFC_PIN_GROUP(msiof2_rx_d),
-	SH_PFC_PIN_GROUP(msiof2_tx_d),
-	SH_PFC_PIN_GROUP(msiof2_clk_e),
-	SH_PFC_PIN_GROUP(msiof2_sync_e),
-	SH_PFC_PIN_GROUP(msiof2_rx_e),
-	SH_PFC_PIN_GROUP(msiof2_tx_e),
-	SH_PFC_PIN_GROUP(pwm0),
-	SH_PFC_PIN_GROUP(pwm0_b),
-	SH_PFC_PIN_GROUP(pwm1),
-	SH_PFC_PIN_GROUP(pwm1_b),
-	SH_PFC_PIN_GROUP(pwm2),
-	SH_PFC_PIN_GROUP(pwm2_b),
-	SH_PFC_PIN_GROUP(pwm3),
-	SH_PFC_PIN_GROUP(pwm4),
-	SH_PFC_PIN_GROUP(pwm4_b),
-	SH_PFC_PIN_GROUP(pwm5),
-	SH_PFC_PIN_GROUP(pwm5_b),
-	SH_PFC_PIN_GROUP(pwm6),
-	SH_PFC_PIN_GROUP(qspi_ctrl),
-	SH_PFC_PIN_GROUP(qspi_data2),
-	SH_PFC_PIN_GROUP(qspi_data4),
-	SH_PFC_PIN_GROUP(qspi_ctrl_b),
-	SH_PFC_PIN_GROUP(qspi_data2_b),
-	SH_PFC_PIN_GROUP(qspi_data4_b),
-	SH_PFC_PIN_GROUP(scif0_data),
-	SH_PFC_PIN_GROUP(scif0_data_b),
-	SH_PFC_PIN_GROUP(scif0_data_c),
-	SH_PFC_PIN_GROUP(scif0_data_d),
-	SH_PFC_PIN_GROUP(scif0_data_e),
-	SH_PFC_PIN_GROUP(scif1_data),
-	SH_PFC_PIN_GROUP(scif1_data_b),
-	SH_PFC_PIN_GROUP(scif1_clk_b),
-	SH_PFC_PIN_GROUP(scif1_data_c),
-	SH_PFC_PIN_GROUP(scif1_data_d),
-	SH_PFC_PIN_GROUP(scif2_data),
-	SH_PFC_PIN_GROUP(scif2_data_b),
-	SH_PFC_PIN_GROUP(scif2_clk_b),
-	SH_PFC_PIN_GROUP(scif2_data_c),
-	SH_PFC_PIN_GROUP(scif2_data_e),
-	SH_PFC_PIN_GROUP(scif3_data),
-	SH_PFC_PIN_GROUP(scif3_clk),
-	SH_PFC_PIN_GROUP(scif3_data_b),
-	SH_PFC_PIN_GROUP(scif3_clk_b),
-	SH_PFC_PIN_GROUP(scif3_data_c),
-	SH_PFC_PIN_GROUP(scif3_data_d),
-	SH_PFC_PIN_GROUP(scif4_data),
-	SH_PFC_PIN_GROUP(scif4_data_b),
-	SH_PFC_PIN_GROUP(scif4_data_c),
-	SH_PFC_PIN_GROUP(scif5_data),
-	SH_PFC_PIN_GROUP(scif5_data_b),
-	SH_PFC_PIN_GROUP(scifa0_data),
-	SH_PFC_PIN_GROUP(scifa0_data_b),
-	SH_PFC_PIN_GROUP(scifa1_data),
-	SH_PFC_PIN_GROUP(scifa1_clk),
-	SH_PFC_PIN_GROUP(scifa1_data_b),
-	SH_PFC_PIN_GROUP(scifa1_clk_b),
-	SH_PFC_PIN_GROUP(scifa1_data_c),
-	SH_PFC_PIN_GROUP(scifa2_data),
-	SH_PFC_PIN_GROUP(scifa2_clk),
-	SH_PFC_PIN_GROUP(scifa2_data_b),
-	SH_PFC_PIN_GROUP(scifa3_data),
-	SH_PFC_PIN_GROUP(scifa3_clk),
-	SH_PFC_PIN_GROUP(scifa3_data_b),
-	SH_PFC_PIN_GROUP(scifa3_clk_b),
-	SH_PFC_PIN_GROUP(scifa3_data_c),
-	SH_PFC_PIN_GROUP(scifa3_clk_c),
-	SH_PFC_PIN_GROUP(scifa4_data),
-	SH_PFC_PIN_GROUP(scifa4_data_b),
-	SH_PFC_PIN_GROUP(scifa4_data_c),
-	SH_PFC_PIN_GROUP(scifa5_data),
-	SH_PFC_PIN_GROUP(scifa5_data_b),
-	SH_PFC_PIN_GROUP(scifa5_data_c),
-	SH_PFC_PIN_GROUP(scifb0_data),
-	SH_PFC_PIN_GROUP(scifb0_clk),
-	SH_PFC_PIN_GROUP(scifb0_ctrl),
-	SH_PFC_PIN_GROUP(scifb0_data_b),
-	SH_PFC_PIN_GROUP(scifb0_clk_b),
-	SH_PFC_PIN_GROUP(scifb0_ctrl_b),
-	SH_PFC_PIN_GROUP(scifb0_data_c),
-	SH_PFC_PIN_GROUP(scifb0_clk_c),
-	SH_PFC_PIN_GROUP(scifb0_data_d),
-	SH_PFC_PIN_GROUP(scifb0_clk_d),
-	SH_PFC_PIN_GROUP(scifb1_data),
-	SH_PFC_PIN_GROUP(scifb1_clk),
-	SH_PFC_PIN_GROUP(scifb1_ctrl),
-	SH_PFC_PIN_GROUP(scifb1_data_b),
-	SH_PFC_PIN_GROUP(scifb1_clk_b),
-	SH_PFC_PIN_GROUP(scifb1_data_c),
-	SH_PFC_PIN_GROUP(scifb1_clk_c),
-	SH_PFC_PIN_GROUP(scifb1_data_d),
-	SH_PFC_PIN_GROUP(scifb2_data),
-	SH_PFC_PIN_GROUP(scifb2_clk),
-	SH_PFC_PIN_GROUP(scifb2_ctrl),
-	SH_PFC_PIN_GROUP(scifb2_data_b),
-	SH_PFC_PIN_GROUP(scifb2_clk_b),
-	SH_PFC_PIN_GROUP(scifb2_ctrl_b),
-	SH_PFC_PIN_GROUP(scifb2_data_c),
-	SH_PFC_PIN_GROUP(scifb2_clk_c),
-	SH_PFC_PIN_GROUP(scifb2_data_d),
-	SH_PFC_PIN_GROUP(scif_clk),
-	SH_PFC_PIN_GROUP(scif_clk_b),
-	SH_PFC_PIN_GROUP(sdhi0_data1),
-	SH_PFC_PIN_GROUP(sdhi0_data4),
-	SH_PFC_PIN_GROUP(sdhi0_ctrl),
-	SH_PFC_PIN_GROUP(sdhi0_cd),
-	SH_PFC_PIN_GROUP(sdhi0_wp),
-	SH_PFC_PIN_GROUP(sdhi1_data1),
-	SH_PFC_PIN_GROUP(sdhi1_data4),
-	SH_PFC_PIN_GROUP(sdhi1_ctrl),
-	SH_PFC_PIN_GROUP(sdhi1_cd),
-	SH_PFC_PIN_GROUP(sdhi1_wp),
-	SH_PFC_PIN_GROUP(sdhi2_data1),
-	SH_PFC_PIN_GROUP(sdhi2_data4),
-	SH_PFC_PIN_GROUP(sdhi2_ctrl),
-	SH_PFC_PIN_GROUP(sdhi2_cd),
-	SH_PFC_PIN_GROUP(sdhi2_wp),
-	SH_PFC_PIN_GROUP(ssi0_data),
-	SH_PFC_PIN_GROUP(ssi0_data_b),
-	SH_PFC_PIN_GROUP(ssi0129_ctrl),
-	SH_PFC_PIN_GROUP(ssi0129_ctrl_b),
-	SH_PFC_PIN_GROUP(ssi1_data),
-	SH_PFC_PIN_GROUP(ssi1_data_b),
-	SH_PFC_PIN_GROUP(ssi1_ctrl),
-	SH_PFC_PIN_GROUP(ssi1_ctrl_b),
-	SH_PFC_PIN_GROUP(ssi2_data),
-	SH_PFC_PIN_GROUP(ssi2_ctrl),
-	SH_PFC_PIN_GROUP(ssi3_data),
-	SH_PFC_PIN_GROUP(ssi34_ctrl),
-	SH_PFC_PIN_GROUP(ssi4_data),
-	SH_PFC_PIN_GROUP(ssi4_ctrl),
-	SH_PFC_PIN_GROUP(ssi5_data),
-	SH_PFC_PIN_GROUP(ssi5_ctrl),
-	SH_PFC_PIN_GROUP(ssi6_data),
-	SH_PFC_PIN_GROUP(ssi6_ctrl),
-	SH_PFC_PIN_GROUP(ssi7_data),
-	SH_PFC_PIN_GROUP(ssi7_data_b),
-	SH_PFC_PIN_GROUP(ssi78_ctrl),
-	SH_PFC_PIN_GROUP(ssi78_ctrl_b),
-	SH_PFC_PIN_GROUP(ssi8_data),
-	SH_PFC_PIN_GROUP(ssi8_data_b),
-	SH_PFC_PIN_GROUP(ssi9_data),
-	SH_PFC_PIN_GROUP(ssi9_data_b),
-	SH_PFC_PIN_GROUP(ssi9_ctrl),
-	SH_PFC_PIN_GROUP(ssi9_ctrl_b),
-	SH_PFC_PIN_GROUP(usb0),
-	SH_PFC_PIN_GROUP(usb1),
-	VIN_DATA_PIN_GROUP(vin0_data, 24),
-	VIN_DATA_PIN_GROUP(vin0_data, 20),
-	SH_PFC_PIN_GROUP(vin0_data18),
-	VIN_DATA_PIN_GROUP(vin0_data, 16),
-	VIN_DATA_PIN_GROUP(vin0_data, 12),
-	VIN_DATA_PIN_GROUP(vin0_data, 10),
-	VIN_DATA_PIN_GROUP(vin0_data, 8),
-	SH_PFC_PIN_GROUP(vin0_sync),
-	SH_PFC_PIN_GROUP(vin0_field),
-	SH_PFC_PIN_GROUP(vin0_clkenb),
-	SH_PFC_PIN_GROUP(vin0_clk),
-	SH_PFC_PIN_GROUP(vin1_data8),
-	SH_PFC_PIN_GROUP(vin1_sync),
-	SH_PFC_PIN_GROUP(vin1_field),
-	SH_PFC_PIN_GROUP(vin1_clkenb),
-	SH_PFC_PIN_GROUP(vin1_clk),
-	VIN_DATA_PIN_GROUP(vin1_b_data, 24),
-	VIN_DATA_PIN_GROUP(vin1_b_data, 20),
-	SH_PFC_PIN_GROUP(vin1_b_data18),
-	VIN_DATA_PIN_GROUP(vin1_b_data, 16),
-	VIN_DATA_PIN_GROUP(vin1_b_data, 12),
-	VIN_DATA_PIN_GROUP(vin1_b_data, 10),
-	VIN_DATA_PIN_GROUP(vin1_b_data, 8),
-	SH_PFC_PIN_GROUP(vin1_b_sync),
-	SH_PFC_PIN_GROUP(vin1_b_field),
-	SH_PFC_PIN_GROUP(vin1_b_clkenb),
-	SH_PFC_PIN_GROUP(vin1_b_clk),
-	SH_PFC_PIN_GROUP(vin2_data8),
-	SH_PFC_PIN_GROUP(vin2_sync),
-	SH_PFC_PIN_GROUP(vin2_field),
-	SH_PFC_PIN_GROUP(vin2_clkenb),
-	SH_PFC_PIN_GROUP(vin2_clk),
+static const struct {
+	struct sh_pfc_pin_group common[341];
+	struct sh_pfc_pin_group r8a779x[9];
+} pinmux_groups = {
+	.common = {
+		SH_PFC_PIN_GROUP(audio_clk_a),
+		SH_PFC_PIN_GROUP(audio_clk_b),
+		SH_PFC_PIN_GROUP(audio_clk_b_b),
+		SH_PFC_PIN_GROUP(audio_clk_c),
+		SH_PFC_PIN_GROUP(audio_clkout),
+		SH_PFC_PIN_GROUP(avb_link),
+		SH_PFC_PIN_GROUP(avb_magic),
+		SH_PFC_PIN_GROUP(avb_phy_int),
+		SH_PFC_PIN_GROUP(avb_mdio),
+		SH_PFC_PIN_GROUP(avb_mii),
+		SH_PFC_PIN_GROUP(avb_gmii),
+		SH_PFC_PIN_GROUP(can0_data),
+		SH_PFC_PIN_GROUP(can0_data_b),
+		SH_PFC_PIN_GROUP(can0_data_c),
+		SH_PFC_PIN_GROUP(can0_data_d),
+		SH_PFC_PIN_GROUP(can0_data_e),
+		SH_PFC_PIN_GROUP(can0_data_f),
+		SH_PFC_PIN_GROUP(can1_data),
+		SH_PFC_PIN_GROUP(can1_data_b),
+		SH_PFC_PIN_GROUP(can1_data_c),
+		SH_PFC_PIN_GROUP(can1_data_d),
+		SH_PFC_PIN_GROUP(can_clk),
+		SH_PFC_PIN_GROUP(can_clk_b),
+		SH_PFC_PIN_GROUP(can_clk_c),
+		SH_PFC_PIN_GROUP(can_clk_d),
+		SH_PFC_PIN_GROUP(du_rgb666),
+		SH_PFC_PIN_GROUP(du_rgb888),
+		SH_PFC_PIN_GROUP(du_clk_out_0),
+		SH_PFC_PIN_GROUP(du_clk_out_1),
+		SH_PFC_PIN_GROUP(du_sync),
+		SH_PFC_PIN_GROUP(du_oddf),
+		SH_PFC_PIN_GROUP(du_cde),
+		SH_PFC_PIN_GROUP(du_disp),
+		SH_PFC_PIN_GROUP(du0_clk_in),
+		SH_PFC_PIN_GROUP(du1_clk_in),
+		SH_PFC_PIN_GROUP(du1_clk_in_b),
+		SH_PFC_PIN_GROUP(du1_clk_in_c),
+		SH_PFC_PIN_GROUP(eth_link),
+		SH_PFC_PIN_GROUP(eth_magic),
+		SH_PFC_PIN_GROUP(eth_mdio),
+		SH_PFC_PIN_GROUP(eth_rmii),
+		SH_PFC_PIN_GROUP(hscif0_data),
+		SH_PFC_PIN_GROUP(hscif0_clk),
+		SH_PFC_PIN_GROUP(hscif0_ctrl),
+		SH_PFC_PIN_GROUP(hscif0_data_b),
+		SH_PFC_PIN_GROUP(hscif0_ctrl_b),
+		SH_PFC_PIN_GROUP(hscif0_data_c),
+		SH_PFC_PIN_GROUP(hscif0_clk_c),
+		SH_PFC_PIN_GROUP(hscif1_data),
+		SH_PFC_PIN_GROUP(hscif1_clk),
+		SH_PFC_PIN_GROUP(hscif1_ctrl),
+		SH_PFC_PIN_GROUP(hscif1_data_b),
+		SH_PFC_PIN_GROUP(hscif1_data_c),
+		SH_PFC_PIN_GROUP(hscif1_clk_c),
+		SH_PFC_PIN_GROUP(hscif1_ctrl_c),
+		SH_PFC_PIN_GROUP(hscif1_data_d),
+		SH_PFC_PIN_GROUP(hscif1_data_e),
+		SH_PFC_PIN_GROUP(hscif1_clk_e),
+		SH_PFC_PIN_GROUP(hscif1_ctrl_e),
+		SH_PFC_PIN_GROUP(hscif2_data),
+		SH_PFC_PIN_GROUP(hscif2_clk),
+		SH_PFC_PIN_GROUP(hscif2_ctrl),
+		SH_PFC_PIN_GROUP(hscif2_data_b),
+		SH_PFC_PIN_GROUP(hscif2_ctrl_b),
+		SH_PFC_PIN_GROUP(hscif2_data_c),
+		SH_PFC_PIN_GROUP(hscif2_clk_c),
+		SH_PFC_PIN_GROUP(hscif2_data_d),
+		SH_PFC_PIN_GROUP(i2c0),
+		SH_PFC_PIN_GROUP(i2c0_b),
+		SH_PFC_PIN_GROUP(i2c0_c),
+		SH_PFC_PIN_GROUP(i2c1),
+		SH_PFC_PIN_GROUP(i2c1_b),
+		SH_PFC_PIN_GROUP(i2c1_c),
+		SH_PFC_PIN_GROUP(i2c1_d),
+		SH_PFC_PIN_GROUP(i2c1_e),
+		SH_PFC_PIN_GROUP(i2c2),
+		SH_PFC_PIN_GROUP(i2c2_b),
+		SH_PFC_PIN_GROUP(i2c2_c),
+		SH_PFC_PIN_GROUP(i2c2_d),
+		SH_PFC_PIN_GROUP(i2c3),
+		SH_PFC_PIN_GROUP(i2c3_b),
+		SH_PFC_PIN_GROUP(i2c3_c),
+		SH_PFC_PIN_GROUP(i2c3_d),
+		SH_PFC_PIN_GROUP(i2c4),
+		SH_PFC_PIN_GROUP(i2c4_b),
+		SH_PFC_PIN_GROUP(i2c4_c),
+		SH_PFC_PIN_GROUP(i2c7),
+		SH_PFC_PIN_GROUP(i2c7_b),
+		SH_PFC_PIN_GROUP(i2c7_c),
+		SH_PFC_PIN_GROUP(i2c8),
+		SH_PFC_PIN_GROUP(i2c8_b),
+		SH_PFC_PIN_GROUP(i2c8_c),
+		SH_PFC_PIN_GROUP(intc_irq0),
+		SH_PFC_PIN_GROUP(intc_irq1),
+		SH_PFC_PIN_GROUP(intc_irq2),
+		SH_PFC_PIN_GROUP(intc_irq3),
+		SH_PFC_PIN_GROUP(mmc_data1),
+		SH_PFC_PIN_GROUP(mmc_data4),
+		SH_PFC_PIN_GROUP(mmc_data8),
+		SH_PFC_PIN_GROUP(mmc_ctrl),
+		SH_PFC_PIN_GROUP(msiof0_clk),
+		SH_PFC_PIN_GROUP(msiof0_sync),
+		SH_PFC_PIN_GROUP(msiof0_ss1),
+		SH_PFC_PIN_GROUP(msiof0_ss2),
+		SH_PFC_PIN_GROUP(msiof0_rx),
+		SH_PFC_PIN_GROUP(msiof0_tx),
+		SH_PFC_PIN_GROUP(msiof0_clk_b),
+		SH_PFC_PIN_GROUP(msiof0_sync_b),
+		SH_PFC_PIN_GROUP(msiof0_ss1_b),
+		SH_PFC_PIN_GROUP(msiof0_ss2_b),
+		SH_PFC_PIN_GROUP(msiof0_rx_b),
+		SH_PFC_PIN_GROUP(msiof0_tx_b),
+		SH_PFC_PIN_GROUP(msiof0_clk_c),
+		SH_PFC_PIN_GROUP(msiof0_sync_c),
+		SH_PFC_PIN_GROUP(msiof0_ss1_c),
+		SH_PFC_PIN_GROUP(msiof0_ss2_c),
+		SH_PFC_PIN_GROUP(msiof0_rx_c),
+		SH_PFC_PIN_GROUP(msiof0_tx_c),
+		SH_PFC_PIN_GROUP(msiof1_clk),
+		SH_PFC_PIN_GROUP(msiof1_sync),
+		SH_PFC_PIN_GROUP(msiof1_ss1),
+		SH_PFC_PIN_GROUP(msiof1_ss2),
+		SH_PFC_PIN_GROUP(msiof1_rx),
+		SH_PFC_PIN_GROUP(msiof1_tx),
+		SH_PFC_PIN_GROUP(msiof1_clk_b),
+		SH_PFC_PIN_GROUP(msiof1_sync_b),
+		SH_PFC_PIN_GROUP(msiof1_ss1_b),
+		SH_PFC_PIN_GROUP(msiof1_ss2_b),
+		SH_PFC_PIN_GROUP(msiof1_rx_b),
+		SH_PFC_PIN_GROUP(msiof1_tx_b),
+		SH_PFC_PIN_GROUP(msiof1_clk_c),
+		SH_PFC_PIN_GROUP(msiof1_sync_c),
+		SH_PFC_PIN_GROUP(msiof1_rx_c),
+		SH_PFC_PIN_GROUP(msiof1_tx_c),
+		SH_PFC_PIN_GROUP(msiof1_clk_d),
+		SH_PFC_PIN_GROUP(msiof1_sync_d),
+		SH_PFC_PIN_GROUP(msiof1_ss1_d),
+		SH_PFC_PIN_GROUP(msiof1_rx_d),
+		SH_PFC_PIN_GROUP(msiof1_tx_d),
+		SH_PFC_PIN_GROUP(msiof1_clk_e),
+		SH_PFC_PIN_GROUP(msiof1_sync_e),
+		SH_PFC_PIN_GROUP(msiof1_rx_e),
+		SH_PFC_PIN_GROUP(msiof1_tx_e),
+		SH_PFC_PIN_GROUP(msiof2_clk),
+		SH_PFC_PIN_GROUP(msiof2_sync),
+		SH_PFC_PIN_GROUP(msiof2_ss1),
+		SH_PFC_PIN_GROUP(msiof2_ss2),
+		SH_PFC_PIN_GROUP(msiof2_rx),
+		SH_PFC_PIN_GROUP(msiof2_tx),
+		SH_PFC_PIN_GROUP(msiof2_clk_b),
+		SH_PFC_PIN_GROUP(msiof2_sync_b),
+		SH_PFC_PIN_GROUP(msiof2_ss1_b),
+		SH_PFC_PIN_GROUP(msiof2_ss2_b),
+		SH_PFC_PIN_GROUP(msiof2_rx_b),
+		SH_PFC_PIN_GROUP(msiof2_tx_b),
+		SH_PFC_PIN_GROUP(msiof2_clk_c),
+		SH_PFC_PIN_GROUP(msiof2_sync_c),
+		SH_PFC_PIN_GROUP(msiof2_rx_c),
+		SH_PFC_PIN_GROUP(msiof2_tx_c),
+		SH_PFC_PIN_GROUP(msiof2_clk_d),
+		SH_PFC_PIN_GROUP(msiof2_sync_d),
+		SH_PFC_PIN_GROUP(msiof2_ss1_d),
+		SH_PFC_PIN_GROUP(msiof2_ss2_d),
+		SH_PFC_PIN_GROUP(msiof2_rx_d),
+		SH_PFC_PIN_GROUP(msiof2_tx_d),
+		SH_PFC_PIN_GROUP(msiof2_clk_e),
+		SH_PFC_PIN_GROUP(msiof2_sync_e),
+		SH_PFC_PIN_GROUP(msiof2_rx_e),
+		SH_PFC_PIN_GROUP(msiof2_tx_e),
+		SH_PFC_PIN_GROUP(pwm0),
+		SH_PFC_PIN_GROUP(pwm0_b),
+		SH_PFC_PIN_GROUP(pwm1),
+		SH_PFC_PIN_GROUP(pwm1_b),
+		SH_PFC_PIN_GROUP(pwm2),
+		SH_PFC_PIN_GROUP(pwm2_b),
+		SH_PFC_PIN_GROUP(pwm3),
+		SH_PFC_PIN_GROUP(pwm4),
+		SH_PFC_PIN_GROUP(pwm4_b),
+		SH_PFC_PIN_GROUP(pwm5),
+		SH_PFC_PIN_GROUP(pwm5_b),
+		SH_PFC_PIN_GROUP(pwm6),
+		SH_PFC_PIN_GROUP(qspi_ctrl),
+		SH_PFC_PIN_GROUP(qspi_data2),
+		SH_PFC_PIN_GROUP(qspi_data4),
+		SH_PFC_PIN_GROUP(qspi_ctrl_b),
+		SH_PFC_PIN_GROUP(qspi_data2_b),
+		SH_PFC_PIN_GROUP(qspi_data4_b),
+		SH_PFC_PIN_GROUP(scif0_data),
+		SH_PFC_PIN_GROUP(scif0_data_b),
+		SH_PFC_PIN_GROUP(scif0_data_c),
+		SH_PFC_PIN_GROUP(scif0_data_d),
+		SH_PFC_PIN_GROUP(scif0_data_e),
+		SH_PFC_PIN_GROUP(scif1_data),
+		SH_PFC_PIN_GROUP(scif1_data_b),
+		SH_PFC_PIN_GROUP(scif1_clk_b),
+		SH_PFC_PIN_GROUP(scif1_data_c),
+		SH_PFC_PIN_GROUP(scif1_data_d),
+		SH_PFC_PIN_GROUP(scif2_data),
+		SH_PFC_PIN_GROUP(scif2_data_b),
+		SH_PFC_PIN_GROUP(scif2_clk_b),
+		SH_PFC_PIN_GROUP(scif2_data_c),
+		SH_PFC_PIN_GROUP(scif2_data_e),
+		SH_PFC_PIN_GROUP(scif3_data),
+		SH_PFC_PIN_GROUP(scif3_clk),
+		SH_PFC_PIN_GROUP(scif3_data_b),
+		SH_PFC_PIN_GROUP(scif3_clk_b),
+		SH_PFC_PIN_GROUP(scif3_data_c),
+		SH_PFC_PIN_GROUP(scif3_data_d),
+		SH_PFC_PIN_GROUP(scif4_data),
+		SH_PFC_PIN_GROUP(scif4_data_b),
+		SH_PFC_PIN_GROUP(scif4_data_c),
+		SH_PFC_PIN_GROUP(scif5_data),
+		SH_PFC_PIN_GROUP(scif5_data_b),
+		SH_PFC_PIN_GROUP(scifa0_data),
+		SH_PFC_PIN_GROUP(scifa0_data_b),
+		SH_PFC_PIN_GROUP(scifa1_data),
+		SH_PFC_PIN_GROUP(scifa1_clk),
+		SH_PFC_PIN_GROUP(scifa1_data_b),
+		SH_PFC_PIN_GROUP(scifa1_clk_b),
+		SH_PFC_PIN_GROUP(scifa1_data_c),
+		SH_PFC_PIN_GROUP(scifa2_data),
+		SH_PFC_PIN_GROUP(scifa2_clk),
+		SH_PFC_PIN_GROUP(scifa2_data_b),
+		SH_PFC_PIN_GROUP(scifa3_data),
+		SH_PFC_PIN_GROUP(scifa3_clk),
+		SH_PFC_PIN_GROUP(scifa3_data_b),
+		SH_PFC_PIN_GROUP(scifa3_clk_b),
+		SH_PFC_PIN_GROUP(scifa3_data_c),
+		SH_PFC_PIN_GROUP(scifa3_clk_c),
+		SH_PFC_PIN_GROUP(scifa4_data),
+		SH_PFC_PIN_GROUP(scifa4_data_b),
+		SH_PFC_PIN_GROUP(scifa4_data_c),
+		SH_PFC_PIN_GROUP(scifa5_data),
+		SH_PFC_PIN_GROUP(scifa5_data_b),
+		SH_PFC_PIN_GROUP(scifa5_data_c),
+		SH_PFC_PIN_GROUP(scifb0_data),
+		SH_PFC_PIN_GROUP(scifb0_clk),
+		SH_PFC_PIN_GROUP(scifb0_ctrl),
+		SH_PFC_PIN_GROUP(scifb0_data_b),
+		SH_PFC_PIN_GROUP(scifb0_clk_b),
+		SH_PFC_PIN_GROUP(scifb0_ctrl_b),
+		SH_PFC_PIN_GROUP(scifb0_data_c),
+		SH_PFC_PIN_GROUP(scifb0_clk_c),
+		SH_PFC_PIN_GROUP(scifb0_data_d),
+		SH_PFC_PIN_GROUP(scifb0_clk_d),
+		SH_PFC_PIN_GROUP(scifb1_data),
+		SH_PFC_PIN_GROUP(scifb1_clk),
+		SH_PFC_PIN_GROUP(scifb1_ctrl),
+		SH_PFC_PIN_GROUP(scifb1_data_b),
+		SH_PFC_PIN_GROUP(scifb1_clk_b),
+		SH_PFC_PIN_GROUP(scifb1_data_c),
+		SH_PFC_PIN_GROUP(scifb1_clk_c),
+		SH_PFC_PIN_GROUP(scifb1_data_d),
+		SH_PFC_PIN_GROUP(scifb2_data),
+		SH_PFC_PIN_GROUP(scifb2_clk),
+		SH_PFC_PIN_GROUP(scifb2_ctrl),
+		SH_PFC_PIN_GROUP(scifb2_data_b),
+		SH_PFC_PIN_GROUP(scifb2_clk_b),
+		SH_PFC_PIN_GROUP(scifb2_ctrl_b),
+		SH_PFC_PIN_GROUP(scifb2_data_c),
+		SH_PFC_PIN_GROUP(scifb2_clk_c),
+		SH_PFC_PIN_GROUP(scifb2_data_d),
+		SH_PFC_PIN_GROUP(scif_clk),
+		SH_PFC_PIN_GROUP(scif_clk_b),
+		SH_PFC_PIN_GROUP(sdhi0_data1),
+		SH_PFC_PIN_GROUP(sdhi0_data4),
+		SH_PFC_PIN_GROUP(sdhi0_ctrl),
+		SH_PFC_PIN_GROUP(sdhi0_cd),
+		SH_PFC_PIN_GROUP(sdhi0_wp),
+		SH_PFC_PIN_GROUP(sdhi1_data1),
+		SH_PFC_PIN_GROUP(sdhi1_data4),
+		SH_PFC_PIN_GROUP(sdhi1_ctrl),
+		SH_PFC_PIN_GROUP(sdhi1_cd),
+		SH_PFC_PIN_GROUP(sdhi1_wp),
+		SH_PFC_PIN_GROUP(sdhi2_data1),
+		SH_PFC_PIN_GROUP(sdhi2_data4),
+		SH_PFC_PIN_GROUP(sdhi2_ctrl),
+		SH_PFC_PIN_GROUP(sdhi2_cd),
+		SH_PFC_PIN_GROUP(sdhi2_wp),
+		SH_PFC_PIN_GROUP(ssi0_data),
+		SH_PFC_PIN_GROUP(ssi0_data_b),
+		SH_PFC_PIN_GROUP(ssi0129_ctrl),
+		SH_PFC_PIN_GROUP(ssi0129_ctrl_b),
+		SH_PFC_PIN_GROUP(ssi1_data),
+		SH_PFC_PIN_GROUP(ssi1_data_b),
+		SH_PFC_PIN_GROUP(ssi1_ctrl),
+		SH_PFC_PIN_GROUP(ssi1_ctrl_b),
+		SH_PFC_PIN_GROUP(ssi2_data),
+		SH_PFC_PIN_GROUP(ssi2_ctrl),
+		SH_PFC_PIN_GROUP(ssi3_data),
+		SH_PFC_PIN_GROUP(ssi34_ctrl),
+		SH_PFC_PIN_GROUP(ssi4_data),
+		SH_PFC_PIN_GROUP(ssi4_ctrl),
+		SH_PFC_PIN_GROUP(ssi5_data),
+		SH_PFC_PIN_GROUP(ssi5_ctrl),
+		SH_PFC_PIN_GROUP(ssi6_data),
+		SH_PFC_PIN_GROUP(ssi6_ctrl),
+		SH_PFC_PIN_GROUP(ssi7_data),
+		SH_PFC_PIN_GROUP(ssi7_data_b),
+		SH_PFC_PIN_GROUP(ssi78_ctrl),
+		SH_PFC_PIN_GROUP(ssi78_ctrl_b),
+		SH_PFC_PIN_GROUP(ssi8_data),
+		SH_PFC_PIN_GROUP(ssi8_data_b),
+		SH_PFC_PIN_GROUP(ssi9_data),
+		SH_PFC_PIN_GROUP(ssi9_data_b),
+		SH_PFC_PIN_GROUP(ssi9_ctrl),
+		SH_PFC_PIN_GROUP(ssi9_ctrl_b),
+		SH_PFC_PIN_GROUP(usb0),
+		SH_PFC_PIN_GROUP(usb1),
+		VIN_DATA_PIN_GROUP(vin0_data, 24),
+		VIN_DATA_PIN_GROUP(vin0_data, 20),
+		SH_PFC_PIN_GROUP(vin0_data18),
+		VIN_DATA_PIN_GROUP(vin0_data, 16),
+		VIN_DATA_PIN_GROUP(vin0_data, 12),
+		VIN_DATA_PIN_GROUP(vin0_data, 10),
+		VIN_DATA_PIN_GROUP(vin0_data, 8),
+		SH_PFC_PIN_GROUP(vin0_sync),
+		SH_PFC_PIN_GROUP(vin0_field),
+		SH_PFC_PIN_GROUP(vin0_clkenb),
+		SH_PFC_PIN_GROUP(vin0_clk),
+		SH_PFC_PIN_GROUP(vin1_data8),
+		SH_PFC_PIN_GROUP(vin1_sync),
+		SH_PFC_PIN_GROUP(vin1_field),
+		SH_PFC_PIN_GROUP(vin1_clkenb),
+		SH_PFC_PIN_GROUP(vin1_clk),
+		VIN_DATA_PIN_GROUP(vin1_b_data, 24),
+		VIN_DATA_PIN_GROUP(vin1_b_data, 20),
+		SH_PFC_PIN_GROUP(vin1_b_data18),
+		VIN_DATA_PIN_GROUP(vin1_b_data, 16),
+		VIN_DATA_PIN_GROUP(vin1_b_data, 12),
+		VIN_DATA_PIN_GROUP(vin1_b_data, 10),
+		VIN_DATA_PIN_GROUP(vin1_b_data, 8),
+		SH_PFC_PIN_GROUP(vin1_b_sync),
+		SH_PFC_PIN_GROUP(vin1_b_field),
+		SH_PFC_PIN_GROUP(vin1_b_clkenb),
+		SH_PFC_PIN_GROUP(vin1_b_clk),
+		SH_PFC_PIN_GROUP(vin2_data8),
+		SH_PFC_PIN_GROUP(vin2_sync),
+		SH_PFC_PIN_GROUP(vin2_field),
+		SH_PFC_PIN_GROUP(vin2_clkenb),
+		SH_PFC_PIN_GROUP(vin2_clk),
+	},
+	.r8a779x = {
+		SH_PFC_PIN_GROUP(adi_common),
+		SH_PFC_PIN_GROUP(adi_chsel0),
+		SH_PFC_PIN_GROUP(adi_chsel1),
+		SH_PFC_PIN_GROUP(adi_chsel2),
+		SH_PFC_PIN_GROUP(adi_common_b),
+		SH_PFC_PIN_GROUP(adi_chsel0_b),
+		SH_PFC_PIN_GROUP(adi_chsel1_b),
+		SH_PFC_PIN_GROUP(adi_chsel2_b),
+		SH_PFC_PIN_GROUP(mlb_3pin),
+	}
 };
 
 static const char * const adi_groups[] = {
@@ -5280,65 +5294,72 @@ static const char * const vin2_groups[] = {
 	"vin2_clk",
 };
 
-static const struct sh_pfc_function pinmux_functions[] = {
-	SH_PFC_FUNCTION(adi),
-	SH_PFC_FUNCTION(audio_clk),
-	SH_PFC_FUNCTION(avb),
-	SH_PFC_FUNCTION(can0),
-	SH_PFC_FUNCTION(can1),
-	SH_PFC_FUNCTION(du),
-	SH_PFC_FUNCTION(du0),
-	SH_PFC_FUNCTION(du1),
-	SH_PFC_FUNCTION(eth),
-	SH_PFC_FUNCTION(hscif0),
-	SH_PFC_FUNCTION(hscif1),
-	SH_PFC_FUNCTION(hscif2),
-	SH_PFC_FUNCTION(i2c0),
-	SH_PFC_FUNCTION(i2c1),
-	SH_PFC_FUNCTION(i2c2),
-	SH_PFC_FUNCTION(i2c3),
-	SH_PFC_FUNCTION(i2c4),
-	SH_PFC_FUNCTION(i2c7),
-	SH_PFC_FUNCTION(i2c8),
-	SH_PFC_FUNCTION(intc),
-	SH_PFC_FUNCTION(mlb),
-	SH_PFC_FUNCTION(mmc),
-	SH_PFC_FUNCTION(msiof0),
-	SH_PFC_FUNCTION(msiof1),
-	SH_PFC_FUNCTION(msiof2),
-	SH_PFC_FUNCTION(pwm0),
-	SH_PFC_FUNCTION(pwm1),
-	SH_PFC_FUNCTION(pwm2),
-	SH_PFC_FUNCTION(pwm3),
-	SH_PFC_FUNCTION(pwm4),
-	SH_PFC_FUNCTION(pwm5),
-	SH_PFC_FUNCTION(pwm6),
-	SH_PFC_FUNCTION(qspi),
-	SH_PFC_FUNCTION(scif0),
-	SH_PFC_FUNCTION(scif1),
-	SH_PFC_FUNCTION(scif2),
-	SH_PFC_FUNCTION(scif3),
-	SH_PFC_FUNCTION(scif4),
-	SH_PFC_FUNCTION(scif5),
-	SH_PFC_FUNCTION(scifa0),
-	SH_PFC_FUNCTION(scifa1),
-	SH_PFC_FUNCTION(scifa2),
-	SH_PFC_FUNCTION(scifa3),
-	SH_PFC_FUNCTION(scifa4),
-	SH_PFC_FUNCTION(scifa5),
-	SH_PFC_FUNCTION(scifb0),
-	SH_PFC_FUNCTION(scifb1),
-	SH_PFC_FUNCTION(scifb2),
-	SH_PFC_FUNCTION(scif_clk),
-	SH_PFC_FUNCTION(sdhi0),
-	SH_PFC_FUNCTION(sdhi1),
-	SH_PFC_FUNCTION(sdhi2),
-	SH_PFC_FUNCTION(ssi),
-	SH_PFC_FUNCTION(usb0),
-	SH_PFC_FUNCTION(usb1),
-	SH_PFC_FUNCTION(vin0),
-	SH_PFC_FUNCTION(vin1),
-	SH_PFC_FUNCTION(vin2),
+static const struct {
+	struct sh_pfc_function common[56];
+	struct sh_pfc_function r8a779x[2];
+} pinmux_functions = {
+	.common = {
+		SH_PFC_FUNCTION(audio_clk),
+		SH_PFC_FUNCTION(avb),
+		SH_PFC_FUNCTION(can0),
+		SH_PFC_FUNCTION(can1),
+		SH_PFC_FUNCTION(du),
+		SH_PFC_FUNCTION(du0),
+		SH_PFC_FUNCTION(du1),
+		SH_PFC_FUNCTION(eth),
+		SH_PFC_FUNCTION(hscif0),
+		SH_PFC_FUNCTION(hscif1),
+		SH_PFC_FUNCTION(hscif2),
+		SH_PFC_FUNCTION(i2c0),
+		SH_PFC_FUNCTION(i2c1),
+		SH_PFC_FUNCTION(i2c2),
+		SH_PFC_FUNCTION(i2c3),
+		SH_PFC_FUNCTION(i2c4),
+		SH_PFC_FUNCTION(i2c7),
+		SH_PFC_FUNCTION(i2c8),
+		SH_PFC_FUNCTION(intc),
+		SH_PFC_FUNCTION(mmc),
+		SH_PFC_FUNCTION(msiof0),
+		SH_PFC_FUNCTION(msiof1),
+		SH_PFC_FUNCTION(msiof2),
+		SH_PFC_FUNCTION(pwm0),
+		SH_PFC_FUNCTION(pwm1),
+		SH_PFC_FUNCTION(pwm2),
+		SH_PFC_FUNCTION(pwm3),
+		SH_PFC_FUNCTION(pwm4),
+		SH_PFC_FUNCTION(pwm5),
+		SH_PFC_FUNCTION(pwm6),
+		SH_PFC_FUNCTION(qspi),
+		SH_PFC_FUNCTION(scif0),
+		SH_PFC_FUNCTION(scif1),
+		SH_PFC_FUNCTION(scif2),
+		SH_PFC_FUNCTION(scif3),
+		SH_PFC_FUNCTION(scif4),
+		SH_PFC_FUNCTION(scif5),
+		SH_PFC_FUNCTION(scifa0),
+		SH_PFC_FUNCTION(scifa1),
+		SH_PFC_FUNCTION(scifa2),
+		SH_PFC_FUNCTION(scifa3),
+		SH_PFC_FUNCTION(scifa4),
+		SH_PFC_FUNCTION(scifa5),
+		SH_PFC_FUNCTION(scifb0),
+		SH_PFC_FUNCTION(scifb1),
+		SH_PFC_FUNCTION(scifb2),
+		SH_PFC_FUNCTION(scif_clk),
+		SH_PFC_FUNCTION(sdhi0),
+		SH_PFC_FUNCTION(sdhi1),
+		SH_PFC_FUNCTION(sdhi2),
+		SH_PFC_FUNCTION(ssi),
+		SH_PFC_FUNCTION(usb0),
+		SH_PFC_FUNCTION(usb1),
+		SH_PFC_FUNCTION(vin0),
+		SH_PFC_FUNCTION(vin1),
+		SH_PFC_FUNCTION(vin2),
+	},
+	.r8a779x = {
+		SH_PFC_FUNCTION(adi),
+		SH_PFC_FUNCTION(mlb),
+	}
 };
 
 static const struct pinmux_cfg_reg pinmux_config_regs[] = {
@@ -5638,7 +5659,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_A1, FN_MSIOF0_SYNC_B,
 		0, 0,
 		/* IP0_18_16 [3] */
-		FN_A0, FN_ATAWR0_N_C, FN_MSIOF0_SCK_B, FN_SCL0_C, FN_PWM2_B,
+		FN_A0, FN_ATAWR0_N_C, FN_MSIOF0_SCK_B, FN_I2C0_SCL_C, FN_PWM2_B,
 		0, 0, 0,
 		/* IP0_15 [1] */
 		FN_D15, 0,
@@ -5679,7 +5700,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_A18, FN_DREQ1, FN_SCIFA1_RXD_C, 0, FN_SCIFB1_RXD_C,
 		0, 0, 0,
 		/* IP1_28_26 [3] */
-		FN_A17, FN_DACK2_B, 0, FN_SDA0_C,
+		FN_A17, FN_DACK2_B, 0, FN_I2C0_SDA_C,
 		0, 0, 0, 0,
 		/* IP1_25_23 [3] */
 		FN_A16, FN_DREQ2_B, FN_FMCLK_C, 0, FN_SCIFA1_SCK_B,
@@ -5694,17 +5715,17 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_A13, FN_ATAG0_N_C, FN_BPFCLK, FN_MSIOF1_SS1_D,
 		0, 0, 0, 0,
 		/* IP1_13_11 [3] */
-		FN_A12, FN_FMCLK, FN_SDA3_D, FN_MSIOF1_SCK_D,
+		FN_A12, FN_FMCLK, FN_I2C3_SDA_D, FN_MSIOF1_SCK_D,
 		0, 0, 0, 0,
 		/* IP1_10_8 [3] */
-		FN_A11, FN_MSIOF1_RXD, FN_SCL3_D, FN_MSIOF1_RXD_D,
+		FN_A11, FN_MSIOF1_RXD, FN_I2C3_SCL_D, FN_MSIOF1_RXD_D,
 		0, 0, 0, 0,
 		/* IP1_7_6 [2] */
 		FN_A10, FN_MSIOF1_TXD, 0, FN_MSIOF1_TXD_D,
 		/* IP1_5_4 [2] */
-		FN_A9, FN_MSIOF1_SS2, FN_SDA0, 0,
+		FN_A9, FN_MSIOF1_SS2, FN_I2C0_SDA, 0,
 		/* IP1_3_2 [2] */
-		FN_A8, FN_MSIOF1_SS1, FN_SCL0, 0,
+		FN_A8, FN_MSIOF1_SS1, FN_I2C0_SCL, 0,
 		/* IP1_1_0 [2] */
 		FN_A7, FN_MSIOF1_SYNC,
 		0, 0, }
@@ -5722,9 +5743,9 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		/* IP2_24_23 [2] */
 		FN_EX_CS1_N, FN_MSIOF2_SCK, 0, 0,
 		/* IP2_22_21 [2] */
-		FN_CS1_N_A26, FN_ATADIR0_N_B, FN_SDA1, 0,
+		FN_CS1_N_A26, FN_ATADIR0_N_B, FN_I2C1_SDA, 0,
 		/* IP2_20_19 [2] */
-		FN_CS0_N, FN_ATAG0_N_B, FN_SCL1, 0,
+		FN_CS0_N, FN_ATAG0_N_B, FN_I2C1_SCL, 0,
 		/* IP2_18_16 [3] */
 		FN_A25, FN_DACK2, FN_SSL, FN_DREQ1_C, FN_RX1, FN_SCIFA1_RXD,
 		0, 0,
@@ -5807,23 +5828,23 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_SSI_SDATA2, FN_GPS_MAG_B, FN_TX2_E, FN_HRTS1_N_E,
 		0, 0, 0, 0,
 		/* IP4_15_13 [3] */
-		FN_SSI_WS2, FN_SDA2, FN_GPS_SIGN_B, FN_RX2_E,
+		FN_SSI_WS2, FN_I2C2_SDA, FN_GPS_SIGN_B, FN_RX2_E,
 		FN_GLO_Q1_D, FN_HCTS1_N_E,
 		0, 0,
 		/* IP4_12_10 [3] */
-		FN_SSI_SCK2, FN_SCL2, FN_GPS_CLK_B, FN_GLO_Q0_D, FN_HSCK1_E,
+		FN_SSI_SCK2, FN_I2C2_SCL, FN_GPS_CLK_B, FN_GLO_Q0_D, FN_HSCK1_E,
 		0, 0, 0,
 		/* IP4_9_8 [2] */
-		FN_SSI_SDATA1, FN_SDA1_B, FN_SDA8_B, FN_MSIOF2_RXD_C,
+		FN_SSI_SDATA1, FN_I2C1_SDA_B, FN_IIC1_SDA_B, FN_MSIOF2_RXD_C,
 		/* IP4_7_5 [3] */
-		FN_SSI_WS1, FN_SCL1_B, FN_SCL8_B, FN_MSIOF2_TXD_C, FN_GLO_I1_D,
-		0, 0, 0,
+		FN_SSI_WS1, FN_I2C1_SCL_B, FN_IIC1_SCL_B, FN_MSIOF2_TXD_C,
+		FN_GLO_I1_D, 0, 0, 0,
 		/* IP4_4_2 [3] */
-		FN_SSI_SCK1, FN_SDA0_B, FN_SDA7_B,
+		FN_SSI_SCK1, FN_I2C0_SDA_B, FN_IIC0_SDA_B,
 		FN_MSIOF2_SYNC_C, FN_GLO_I0_D,
 		0, 0, 0,
 		/* IP4_1_0 [2] */
-		FN_SSI_SDATA0, FN_SCL0_B, FN_SCL7_B, FN_MSIOF2_SCK_C, }
+		FN_SSI_SDATA0, FN_I2C0_SCL_B, FN_IIC0_SCL_B, FN_MSIOF2_SCK_C, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR5", 0xE6060034, 32,
 			     3, 3, 2, 2, 2, 3, 2, 3, 3, 3, 3, 3) {
@@ -5877,15 +5898,15 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		0, 0, 0,
 		/* IP6_23_21 [3] */
 		FN_IRQ6, FN_HSCK1_C, FN_MSIOF1_SS2_B,
-		FN_SDA1_E, FN_MSIOF2_SYNC_E,
+		FN_I2C1_SDA_E, FN_MSIOF2_SYNC_E,
 		0, 0, 0,
 		/* IP6_20_19 [2] */
-		FN_IRQ5, FN_HTX1_C, FN_SCL1_E, FN_MSIOF2_SCK_E,
+		FN_IRQ5, FN_HTX1_C, FN_I2C1_SCL_E, FN_MSIOF2_SCK_E,
 		/* IP6_18_16 [3] */
-		FN_IRQ4, FN_HRX1_C, FN_SDA4_C, FN_MSIOF2_RXD_E, FN_INTC_IRQ4_N,
-		0, 0, 0,
+		FN_IRQ4, FN_HRX1_C, FN_I2C4_SDA_C, FN_MSIOF2_RXD_E,
+		FN_INTC_IRQ4_N,	0, 0, 0,
 		/* IP6_15_14 [2] */
-		FN_IRQ3, FN_SCL4_C, FN_MSIOF2_TXD_E, FN_INTC_IRQ3_N,
+		FN_IRQ3, FN_I2C4_SCL_C, FN_MSIOF2_TXD_E, FN_INTC_IRQ3_N,
 		/* IP6_13_12 [2] */
 		FN_IRQ2, FN_SCIFB1_TXD_D, FN_INTC_IRQ2_N, 0,
 		/* IP6_11_10 [2] */
@@ -5990,7 +6011,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG_VAR("IPSR9", 0xE6060044, 32,
 			     3, 2, 2, 2, 2, 2, 2, 1, 3, 1, 1, 3, 1, 1, 3, 3) {
 		/* IP9_31_29 [3] */
-		FN_VI0_G0, FN_SCL8, FN_STP_IVCXO27_0_C, FN_SCL4,
+		FN_VI0_G0, FN_IIC1_SCL, FN_STP_IVCXO27_0_C, FN_I2C4_SCL,
 		FN_HCTS2_N, FN_SCIFB2_CTS_N, FN_ATAWR1_N, 0,
 		/* IP9_28_27 [2] */
 		FN_VI0_DATA3_VI0_B3, FN_SCIF3_SCK_B, FN_SCIFA3_SCK_B, 0,
@@ -6008,7 +6029,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_DU1_DISP, FN_QPOLA,
 		/* IP9_15_13 [3] */
 		FN_DU1_EXODDF_DU1_ODDF_DISP_CDE, FN_QCPV_QDE,
-		FN_CAN0_RX, FN_RX3_B, FN_SDA2_B,
+		FN_CAN0_RX, FN_RX3_B, FN_I2C2_SDA_B,
 		0, 0, 0,
 		/* IP9_12 [1] */
 		FN_DU1_EXVSYNC_DU1_VSYNC, FN_QSTB_QHE,
@@ -6016,24 +6037,24 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_DU1_EXHSYNC_DU1_HSYNC, FN_QSTH_QHS,
 		/* IP9_10_8 [3] */
 		FN_DU1_DOTCLKOUT1, FN_QSTVB_QVE, FN_CAN0_TX,
-		FN_TX3_B, FN_SCL2_B, FN_PWM4,
+		FN_TX3_B, FN_I2C2_SCL_B, FN_PWM4,
 		0, 0,
 		/* IP9_7 [1] */
 		FN_DU1_DOTCLKOUT0, FN_QCLK,
 		/* IP9_6 [1] */
 		FN_DU1_DOTCLKIN, FN_QSTVA_QVS,
 		/* IP9_5_3 [3] */
-		FN_DU1_DB7, FN_LCDOUT23, FN_SDA3_C,
+		FN_DU1_DB7, FN_LCDOUT23, FN_I2C3_SDA_C,
 		FN_SCIF3_SCK, FN_SCIFA3_SCK,
 		0, 0, 0,
 		/* IP9_2_0 [3] */
-		FN_DU1_DB6, FN_LCDOUT22, FN_SCL3_C, FN_RX3, FN_SCIFA3_RXD,
+		FN_DU1_DB6, FN_LCDOUT22, FN_I2C3_SCL_C, FN_RX3, FN_SCIFA3_RXD,
 		0, 0, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR10", 0xE6060048, 32,
 			     3, 2, 2, 3, 3, 2, 2, 3, 3, 3, 3, 3) {
 		/* IP10_31_29 [3] */
-		FN_VI0_R4, FN_VI2_DATA5, FN_GLO_SCLK_B, FN_TX0_C, FN_SCL1_D,
+		FN_VI0_R4, FN_VI2_DATA5, FN_GLO_SCLK_B, FN_TX0_C, FN_I2C1_SCL_D,
 		0, 0, 0,
 		/* IP10_28_27 [2] */
 		FN_VI0_R3, FN_VI2_DATA4, FN_GLO_Q1_B, FN_TS_SPSYNC0_C,
@@ -6058,22 +6079,22 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_HTX2, FN_SCIFB2_TXD, FN_SCIFB0_SCK_D,
 		0, 0,
 		/* IP10_8_6 [3] */
-		FN_VI0_G3, FN_VI2_VSYNC_N, FN_STP_ISEN_0_C, FN_SDA3_B,
+		FN_VI0_G3, FN_VI2_VSYNC_N, FN_STP_ISEN_0_C, FN_I2C3_SDA_B,
 		FN_HRX2, FN_SCIFB2_RXD, FN_ATACS01_N, 0,
 		/* IP10_5_3 [3] */
-		FN_VI0_G2, FN_VI2_HSYNC_N, FN_STP_ISD_0_C, FN_SCL3_B,
+		FN_VI0_G2, FN_VI2_HSYNC_N, FN_STP_ISD_0_C, FN_I2C3_SCL_B,
 		FN_HSCK2, FN_SCIFB2_SCK, FN_ATARD1_N, 0,
 		/* IP10_2_0 [3] */
-		FN_VI0_G1, FN_SDA8, FN_STP_ISCLK_0_C, FN_SDA4,
+		FN_VI0_G1, FN_IIC1_SDA, FN_STP_ISCLK_0_C, FN_I2C4_SDA,
 		FN_HRTS2_N, FN_SCIFB2_RTS_N, FN_ATADIR1_N, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR11", 0xE606004C, 32,
 			     2, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2,
 			     3, 3, 3, 3, 3) {
 		/* IP11_31_30 [2] */
-		FN_ETH_CRS_DV, FN_AVB_LINK, FN_SDA2_C, 0,
+		FN_ETH_CRS_DV, FN_AVB_LINK, FN_I2C2_SDA_C, 0,
 		/* IP11_29_28 [2] */
-		FN_ETH_MDIO, FN_AVB_RX_CLK, FN_SCL2_C, 0,
+		FN_ETH_MDIO, FN_AVB_RX_CLK, FN_I2C2_SCL_C, 0,
 		/* IP11_27 [1] */
 		FN_VI1_DATA7, FN_AVB_MDC,
 		/* IP11_26 [1] */
@@ -6106,13 +6127,13 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		0, 0, 0,
 		/* IP11_8_6 [3] */
 		FN_VI0_R7, FN_GLO_RFON_B, FN_RX1_C, FN_CAN0_RX_E,
-		FN_SDA4_B, FN_HRX1_D, FN_SCIFB0_RXD_D, 0,
+		FN_I2C4_SDA_B, FN_HRX1_D, FN_SCIFB0_RXD_D, 0,
 		/* IP11_5_3 [3] */
-		FN_VI0_R6, FN_VI2_DATA7, FN_GLO_SS_B, FN_TX1_C, FN_SCL4_B,
+		FN_VI0_R6, FN_VI2_DATA7, FN_GLO_SS_B, FN_TX1_C, FN_I2C4_SCL_B,
 		0, 0, 0,
 		/* IP11_2_0 [3] */
-		FN_VI0_R5, FN_VI2_DATA6, FN_GLO_SDATA_B, FN_RX0_C, FN_SDA1_D,
-		0, 0, 0, }
+		FN_VI0_R5, FN_VI2_DATA6, FN_GLO_SDATA_B, FN_RX0_C,
+		FN_I2C1_SDA_D, 0, 0, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR12", 0xE6060050, 32,
 			     2, 3, 3, 2, 2, 2, 2, 3, 3, 3, 3, 2, 2) {
@@ -6144,16 +6165,16 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		0, 0, 0,
 		/* IP12_9_7 [3] */
 		FN_ETH_LINK, FN_AVB_TXD0, FN_CAN0_RX_C,
-		FN_SDA2_D, FN_MSIOF1_SCK_E,
+		FN_I2C2_SDA_D, FN_MSIOF1_SCK_E,
 		0, 0, 0,
 		/* IP12_6_4 [3] */
 		FN_ETH_RXD1, FN_AVB_GTXREFCLK, FN_CAN0_TX_C,
-		FN_SCL2_D, FN_MSIOF1_RXD_E,
+		FN_I2C2_SCL_D, FN_MSIOF1_RXD_E,
 		0, 0, 0,
 		/* IP12_3_2 [2] */
-		FN_ETH_RXD0, FN_AVB_PHY_INT, FN_SDA3, FN_SDA7,
+		FN_ETH_RXD0, FN_AVB_PHY_INT, FN_I2C3_SDA, FN_IIC0_SDA,
 		/* IP12_1_0 [2] */
-		FN_ETH_RX_ER, FN_AVB_CRS, FN_SCL3, FN_SCL7, }
+		FN_ETH_RX_ER, FN_AVB_CRS, FN_I2C3_SCL, FN_IIC0_SCL, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR13", 0xE6060054, 32,
 			     1, 3, 1, 1, 1, 2, 1, 3, 3, 1, 1, 1, 1, 1, 1,
@@ -6161,7 +6182,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		/* IP13_31 [1] */
 		0, 0,
 		/* IP13_30_28 [3] */
-		FN_SD1_CD, FN_PWM0, FN_TPU_TO0, FN_SCL1_C,
+		FN_SD1_CD, FN_PWM0, FN_TPU_TO0, FN_I2C1_SCL_C,
 		0, 0, 0, 0,
 		/* IP13_27 [1] */
 		FN_SD1_DATA3, FN_IERX_B,
@@ -6210,10 +6231,10 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 			     3, 3, 3, 3, 3, 3, 3, 3, 1, 1, 1, 1, 1, 1, 2) {
 		/* IP14_31_29 [3] */
 		FN_MSIOF0_SS2, FN_MMC_D7, FN_ADICHS2, FN_RX0_E,
-		FN_VI1_VSYNC_N_C, FN_SDA7_C, FN_VI1_G5_B, 0,
+		FN_VI1_VSYNC_N_C, FN_IIC0_SDA_C, FN_VI1_G5_B, 0,
 		/* IP14_28_26 [3] */
 		FN_MSIOF0_SS1, FN_MMC_D6, FN_ADICHS1, FN_TX0_E,
-		FN_VI1_HSYNC_N_C, FN_SCL7_C, FN_VI1_G4_B, 0,
+		FN_VI1_HSYNC_N_C, FN_IIC0_SCL_C, FN_VI1_G4_B, 0,
 		/* IP14_25_23 [3] */
 		FN_MSIOF0_RXD, FN_ADICHS0, 0, FN_VI1_DATA0_C, FN_VI1_G3_B,
 		0, 0, 0,
@@ -6229,10 +6250,10 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_VI1_CLK_C, FN_VI1_G0_B,
 		0, 0,
 		/* IP14_13_11 [3] */
-		FN_SD2_WP, FN_MMC_D5, FN_SDA8_C, FN_RX5_B, FN_SCIFA5_RXD_C,
+		FN_SD2_WP, FN_MMC_D5, FN_IIC1_SDA_C, FN_RX5_B, FN_SCIFA5_RXD_C,
 		0, 0, 0,
 		/* IP14_10_8 [3] */
-		FN_SD2_CD, FN_MMC_D4, FN_SCL8_C, FN_TX5_B, FN_SCIFA5_TXD_C,
+		FN_SD2_CD, FN_MMC_D4, FN_IIC1_SCL_C, FN_TX5_B, FN_SCIFA5_TXD_C,
 		0, 0, 0,
 		/* IP14_7 [1] */
 		FN_SD2_DATA3, FN_MMC_D3,
@@ -6247,7 +6268,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		/* IP14_2 [1] */
 		FN_SD2_CLK, FN_MMC_CLK,
 		/* IP14_1_0 [2] */
-		FN_SD1_WP, FN_PWM1_B, FN_SDA1_C, 0, }
+		FN_SD1_WP, FN_PWM1_B, FN_I2C1_SDA_C, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR15", 0xE606005C, 32,
 			     2, 3, 3, 3, 3, 3, 3, 3, 3, 2, 2, 2) {
@@ -6424,14 +6445,14 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		/* SEL_CANCLK [2] */
 		FN_SEL_CANCLK_0, FN_SEL_CANCLK_1,
 		FN_SEL_CANCLK_2, FN_SEL_CANCLK_3,
-		/* SEL_IIC8 [2] */
-		FN_SEL_IIC8_0, FN_SEL_IIC8_1, FN_SEL_IIC8_2, 0,
-		/* SEL_IIC7 [2] */
-		FN_SEL_IIC7_0, FN_SEL_IIC7_1, FN_SEL_IIC7_2, 0,
-		/* SEL_IIC4 [2] */
-		FN_SEL_IIC4_0, FN_SEL_IIC4_1, FN_SEL_IIC4_2, 0,
-		/* SEL_IIC3 [2] */
-		FN_SEL_IIC3_0, FN_SEL_IIC3_1, FN_SEL_IIC3_2, FN_SEL_IIC3_3,
+		/* SEL_IIC1 [2] */
+		FN_SEL_IIC1_0, FN_SEL_IIC1_1, FN_SEL_IIC1_2, 0,
+		/* SEL_IIC0 [2] */
+		FN_SEL_IIC0_0, FN_SEL_IIC0_1, FN_SEL_IIC0_2, 0,
+		/* SEL_I2C4 [2] */
+		FN_SEL_I2C4_0, FN_SEL_I2C4_1, FN_SEL_I2C4_2, 0,
+		/* SEL_I2C3 [2] */
+		FN_SEL_I2C3_0, FN_SEL_I2C3_1, FN_SEL_I2C3_2, FN_SEL_I2C3_3,
 		/* SEL_SCIF3 [2] */
 		FN_SEL_SCIF3_0, FN_SEL_SCIF3_1, FN_SEL_SCIF3_2, FN_SEL_SCIF3_3,
 		/* SEL_IEB [2] */
@@ -6442,14 +6463,14 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_SEL_SCIF5_0, FN_SEL_SCIF5_1,
 		/* RESERVED [2] */
 		0, 0, 0, 0,
-		/* SEL_IIC2 [2] */
-		FN_SEL_IIC2_0, FN_SEL_IIC2_1, FN_SEL_IIC2_2, FN_SEL_IIC2_3,
-		/* SEL_IIC1 [3] */
-		FN_SEL_IIC1_0, FN_SEL_IIC1_1, FN_SEL_IIC1_2, FN_SEL_IIC1_3,
-		FN_SEL_IIC1_4,
+		/* SEL_I2C2 [2] */
+		FN_SEL_I2C2_0, FN_SEL_I2C2_1, FN_SEL_I2C2_2, FN_SEL_I2C2_3,
+		/* SEL_I2C1 [3] */
+		FN_SEL_I2C1_0, FN_SEL_I2C1_1, FN_SEL_I2C1_2, FN_SEL_I2C1_3,
+		FN_SEL_I2C1_4,
 		0, 0, 0,
-		/* SEL_IIC0 [2] */
-		FN_SEL_IIC0_0, FN_SEL_IIC0_1, FN_SEL_IIC0_2, 0,
+		/* SEL_I2C0 [2] */
+		FN_SEL_I2C0_0, FN_SEL_I2C0_1, FN_SEL_I2C0_2, 0,
 		/* RESERVED [2] */
 		0, 0, 0, 0,
 		/* RESERVED [2] */
@@ -6520,6 +6541,28 @@ static const struct sh_pfc_soc_operations r8a7791_pinmux_ops = {
 	.pin_to_pocctrl = r8a7791_pin_to_pocctrl,
 };
 
+#ifdef CONFIG_PINCTRL_PFC_R8A7743
+const struct sh_pfc_soc_info r8a7743_pinmux_info = {
+	.name = "r8a77430_pfc",
+	.ops = &r8a7791_pinmux_ops,
+	.unlock_reg = 0xe6060000, /* PMMR */
+
+	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
+
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.groups = pinmux_groups.common,
+	.nr_groups = ARRAY_SIZE(pinmux_groups.common),
+	.functions = pinmux_functions.common,
+	.nr_functions = ARRAY_SIZE(pinmux_functions.common),
+
+	.cfg_regs = pinmux_config_regs,
+
+	.pinmux_data = pinmux_data,
+	.pinmux_data_size = ARRAY_SIZE(pinmux_data),
+};
+#endif
+
 #ifdef CONFIG_PINCTRL_PFC_R8A7791
 const struct sh_pfc_soc_info r8a7791_pinmux_info = {
 	.name = "r8a77910_pfc",
@@ -6530,10 +6573,12 @@ const struct sh_pfc_soc_info r8a7791_pinmux_info = {
 
 	.pins = pinmux_pins,
 	.nr_pins = ARRAY_SIZE(pinmux_pins),
-	.groups = pinmux_groups,
-	.nr_groups = ARRAY_SIZE(pinmux_groups),
-	.functions = pinmux_functions,
-	.nr_functions = ARRAY_SIZE(pinmux_functions),
+	.groups = pinmux_groups.common,
+	.nr_groups = ARRAY_SIZE(pinmux_groups.common) +
+		     ARRAY_SIZE(pinmux_groups.r8a779x),
+	.functions = pinmux_functions.common,
+	.nr_functions = ARRAY_SIZE(pinmux_functions.common) +
+			ARRAY_SIZE(pinmux_functions.r8a779x),
 
 	.cfg_regs = pinmux_config_regs,
 
@@ -6552,10 +6597,12 @@ const struct sh_pfc_soc_info r8a7793_pinmux_info = {
 
 	.pins = pinmux_pins,
 	.nr_pins = ARRAY_SIZE(pinmux_pins),
-	.groups = pinmux_groups,
-	.nr_groups = ARRAY_SIZE(pinmux_groups),
-	.functions = pinmux_functions,
-	.nr_functions = ARRAY_SIZE(pinmux_functions),
+	.groups = pinmux_groups.common,
+	.nr_groups = ARRAY_SIZE(pinmux_groups.common) +
+		     ARRAY_SIZE(pinmux_groups.r8a779x),
+	.functions = pinmux_functions.common,
+	.nr_functions = ARRAY_SIZE(pinmux_functions.common) +
+			ARRAY_SIZE(pinmux_functions.r8a779x),
 
 	.cfg_regs = pinmux_config_regs,
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7792.c b/drivers/pinctrl/sh-pfc/pfc-r8a7792.c
index 21badb6166b9..cc3597f66605 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7792.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7792.c
@@ -1137,6 +1137,43 @@ static const unsigned int scif0_ctrl_pins[] = {
 static const unsigned int scif0_ctrl_mux[] = {
 	RTS0_N_MARK, CTS0_N_MARK,
 };
+/* - SCIF1 ------------------------------------------------------------------ */
+static const unsigned int scif1_data_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(10, 19), RCAR_GP_PIN(10, 18),
+};
+static const unsigned int scif1_data_mux[] = {
+	RX1_MARK, TX1_MARK,
+};
+static const unsigned int scif1_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(10, 15),
+};
+static const unsigned int scif1_clk_mux[] = {
+	SCK1_MARK,
+};
+static const unsigned int scif1_ctrl_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(10, 17), RCAR_GP_PIN(10, 16),
+};
+static const unsigned int scif1_ctrl_mux[] = {
+	RTS1_N_MARK, CTS1_N_MARK,
+};
+/* - SCIF2 ------------------------------------------------------------------ */
+static const unsigned int scif2_data_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(10, 22), RCAR_GP_PIN(10, 21),
+};
+static const unsigned int scif2_data_mux[] = {
+	RX2_MARK, TX2_MARK,
+};
+static const unsigned int scif2_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(10, 20),
+};
+static const unsigned int scif2_clk_mux[] = {
+	SCK2_MARK,
+};
 /* - SCIF3 ------------------------------------------------------------------ */
 static const unsigned int scif3_data_pins[] = {
 	/* RX, TX */
@@ -1680,6 +1717,11 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(scif0_data),
 	SH_PFC_PIN_GROUP(scif0_clk),
 	SH_PFC_PIN_GROUP(scif0_ctrl),
+	SH_PFC_PIN_GROUP(scif1_data),
+	SH_PFC_PIN_GROUP(scif1_clk),
+	SH_PFC_PIN_GROUP(scif1_ctrl),
+	SH_PFC_PIN_GROUP(scif2_data),
+	SH_PFC_PIN_GROUP(scif2_clk),
 	SH_PFC_PIN_GROUP(scif3_data),
 	SH_PFC_PIN_GROUP(scif3_clk),
 	SH_PFC_PIN_GROUP(sdhi0_data1),
@@ -1826,6 +1868,17 @@ static const char * const scif0_groups[] = {
 	"scif0_ctrl",
 };
 
+static const char * const scif1_groups[] = {
+	"scif1_data",
+	"scif1_clk",
+	"scif1_ctrl",
+};
+
+static const char * const scif2_groups[] = {
+	"scif2_data",
+	"scif2_clk",
+};
+
 static const char * const scif3_groups[] = {
 	"scif3_data",
 	"scif3_clk",
@@ -1924,6 +1977,8 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(msiof1),
 	SH_PFC_FUNCTION(qspi),
 	SH_PFC_FUNCTION(scif0),
+	SH_PFC_FUNCTION(scif1),
+	SH_PFC_FUNCTION(scif2),
 	SH_PFC_FUNCTION(scif3),
 	SH_PFC_FUNCTION(sdhi0),
 	SH_PFC_FUNCTION(vin0),
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7794.c b/drivers/pinctrl/sh-pfc/pfc-r8a7794.c
index ef093ac0cf2f..a0ed220071f5 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7794.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7794.c
@@ -1,9 +1,9 @@
 /*
- * r8a7794 processor support - PFC hardware block.
+ * r8a7794/r8a7745 processor support - PFC hardware block.
  *
  * Copyright (C) 2014-2015 Renesas Electronics Corporation
  * Copyright (C) 2015 Renesas Solutions Corp.
- * Copyright (C) 2015-2016 Cogent Embedded, Inc., <source@cogentembedded.com>
+ * Copyright (C) 2015-2017 Cogent Embedded, Inc. <source@cogentembedded.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2
@@ -105,235 +105,279 @@ enum {
 	FN_I2C3_SDA_B, FN_SCIF5_TXD_B, FN_D5, FN_SCIF4_RXD_B, FN_I2C0_SCL_D,
 
 	/* IPSR1 */
-	FN_D6, FN_SCIF4_TXD_B, FN_I2C0_SDA_D, FN_D7, FN_IRQ3, FN_TCLK1,
-	FN_PWM6_B, FN_D8, FN_HSCIF2_HRX, FN_I2C1_SCL_B, FN_D9, FN_HSCIF2_HTX,
-	FN_I2C1_SDA_B, FN_D10, FN_HSCIF2_HSCK, FN_SCIF1_SCK_C, FN_IRQ6,
-	FN_PWM5_C, FN_D11, FN_HSCIF2_HCTS_N, FN_SCIF1_RXD_C, FN_I2C1_SCL_D,
-	FN_D12, FN_HSCIF2_HRTS_N, FN_SCIF1_TXD_C, FN_I2C1_SDA_D, FN_D13,
-	FN_SCIFA1_SCK, FN_TANS1, FN_PWM2_C, FN_TCLK2_B, FN_D14, FN_SCIFA1_RXD,
-	FN_IIC0_SCL_B, FN_D15, FN_SCIFA1_TXD, FN_IIC0_SDA_B, FN_A0,
-	FN_SCIFB1_SCK, FN_PWM3_B, FN_A1, FN_SCIFB1_TXD, FN_A3, FN_SCIFB0_SCK,
-	FN_A4, FN_SCIFB0_TXD, FN_A5, FN_SCIFB0_RXD, FN_PWM4_B, FN_TPUTO3_C,
+	FN_D6, FN_SCIF4_TXD_B, FN_I2C0_SDA_D,
+	FN_D7, FN_IRQ3, FN_TCLK1, FN_PWM6_B,
+	FN_D8, FN_HSCIF2_HRX, FN_I2C1_SCL_B,
+	FN_D9, FN_HSCIF2_HTX, FN_I2C1_SDA_B,
+	FN_D10, FN_HSCIF2_HSCK, FN_SCIF1_SCK_C, FN_IRQ6, FN_PWM5_C,
+	FN_D11, FN_HSCIF2_HCTS_N, FN_SCIF1_RXD_C, FN_I2C1_SCL_D,
+	FN_D12, FN_HSCIF2_HRTS_N, FN_SCIF1_TXD_C, FN_I2C1_SDA_D,
+	FN_D13, FN_SCIFA1_SCK, FN_PWM2_C, FN_TCLK2_B,
+	FN_D14, FN_SCIFA1_RXD, FN_I2C5_SCL_B,
+	FN_D15, FN_SCIFA1_TXD, FN_I2C5_SDA_B,
+	FN_A0, FN_SCIFB1_SCK, FN_PWM3_B,
+	FN_A1, FN_SCIFB1_TXD,
+	FN_A3, FN_SCIFB0_SCK,
+	FN_A4, FN_SCIFB0_TXD,
+	FN_A5, FN_SCIFB0_RXD, FN_PWM4_B, FN_TPUTO3_C,
 	FN_A6, FN_SCIFB0_CTS_N, FN_SCIFA4_RXD_B, FN_TPUTO2_C,
 
 	/* IPSR2 */
-	FN_A7, FN_SCIFB0_RTS_N, FN_SCIFA4_TXD_B, FN_A8, FN_MSIOF1_RXD,
-	FN_SCIFA0_RXD_B, FN_A9, FN_MSIOF1_TXD, FN_SCIFA0_TXD_B, FN_A10,
-	FN_MSIOF1_SCK, FN_IIC1_SCL_B, FN_A11, FN_MSIOF1_SYNC, FN_IIC1_SDA_B,
-	FN_A12, FN_MSIOF1_SS1, FN_SCIFA5_RXD_B, FN_A13, FN_MSIOF1_SS2,
-	FN_SCIFA5_TXD_B, FN_A14, FN_MSIOF2_RXD, FN_HSCIF0_HRX_B, FN_DREQ1_N,
-	FN_A15, FN_MSIOF2_TXD, FN_HSCIF0_HTX_B, FN_DACK1, FN_A16,
-	FN_MSIOF2_SCK, FN_HSCIF0_HSCK_B, FN_SPEEDIN, FN_VSP, FN_CAN_CLK_C,
-	FN_TPUTO2_B, FN_A17, FN_MSIOF2_SYNC, FN_SCIF4_RXD_E, FN_CAN1_RX_B,
-	FN_AVB_AVTP_CAPTURE_B, FN_A18, FN_MSIOF2_SS1, FN_SCIF4_TXD_E,
-	FN_CAN1_TX_B, FN_AVB_AVTP_MATCH_B, FN_A19, FN_MSIOF2_SS2, FN_PWM4,
-	FN_TPUTO2, FN_MOUT0, FN_A20, FN_SPCLK, FN_MOUT1,
+	FN_A7, FN_SCIFB0_RTS_N, FN_SCIFA4_TXD_B,
+	FN_A8, FN_MSIOF1_RXD, FN_SCIFA0_RXD_B,
+	FN_A9, FN_MSIOF1_TXD, FN_SCIFA0_TXD_B,
+	FN_A10, FN_MSIOF1_SCK, FN_IIC0_SCL_B,
+	FN_A11, FN_MSIOF1_SYNC, FN_IIC0_SDA_B,
+	FN_A12, FN_MSIOF1_SS1, FN_SCIFA5_RXD_B,
+	FN_A13, FN_MSIOF1_SS2, FN_SCIFA5_TXD_B,
+	FN_A14, FN_MSIOF2_RXD, FN_HSCIF0_HRX_B, FN_DREQ1_N,
+	FN_A15, FN_MSIOF2_TXD, FN_HSCIF0_HTX_B, FN_DACK1,
+	FN_A16, FN_MSIOF2_SCK, FN_HSCIF0_HSCK_B, FN_SPEEDIN, FN_CAN_CLK_C,
+	FN_TPUTO2_B,
+	FN_A17, FN_MSIOF2_SYNC, FN_SCIF4_RXD_E, FN_CAN1_RX_B,
+	FN_A18, FN_MSIOF2_SS1, FN_SCIF4_TXD_E, FN_CAN1_TX_B,
+	FN_A19, FN_MSIOF2_SS2, FN_PWM4, FN_TPUTO2,
+	FN_A20, FN_SPCLK,
 
 	/* IPSR3 */
-	FN_A21, FN_MOSI_IO0, FN_MOUT2, FN_A22, FN_MISO_IO1, FN_MOUT5,
-	FN_ATADIR1_N, FN_A23, FN_IO2, FN_MOUT6, FN_ATAWR1_N, FN_A24, FN_IO3,
-	FN_EX_WAIT2, FN_A25, FN_SSL, FN_ATARD1_N, FN_CS0_N, FN_VI1_DATA8,
-	FN_CS1_N_A26, FN_VI1_DATA9, FN_EX_CS0_N, FN_VI1_DATA10, FN_EX_CS1_N,
-	FN_TPUTO3_B, FN_SCIFB2_RXD, FN_VI1_DATA11, FN_EX_CS2_N, FN_PWM0,
-	FN_SCIF4_RXD_C, FN_TS_SDATA_B, FN_RIF0_SYNC, FN_TPUTO3, FN_SCIFB2_TXD,
-	FN_SDATA_B, FN_EX_CS3_N, FN_SCIFA2_SCK, FN_SCIF4_TXD_C, FN_TS_SCK_B,
-	FN_RIF0_CLK, FN_BPFCLK, FN_SCIFB2_SCK, FN_MDATA_B, FN_EX_CS4_N,
-	FN_SCIFA2_RXD, FN_I2C2_SCL_E, FN_TS_SDEN_B, FN_RIF0_D0, FN_FMCLK,
-	FN_SCIFB2_CTS_N, FN_SCKZ_B, FN_EX_CS5_N, FN_SCIFA2_TXD, FN_I2C2_SDA_E,
-	FN_TS_SPSYNC_B, FN_RIF0_D1, FN_FMIN, FN_SCIFB2_RTS_N, FN_STM_N_B,
-	FN_BS_N, FN_DRACK0, FN_PWM1_C, FN_TPUTO0_C, FN_ATACS01_N, FN_MTS_N_B,
-	FN_RD_N, FN_ATACS11_N, FN_RD_WR_N, FN_ATAG1_N,
+	FN_A21, FN_MOSI_IO0,
+	FN_A22, FN_MISO_IO1, FN_ATADIR1_N,
+	FN_A23, FN_IO2, FN_ATAWR1_N,
+	FN_A24, FN_IO3, FN_EX_WAIT2,
+	FN_A25, FN_SSL, FN_ATARD1_N,
+	FN_CS0_N, FN_VI1_DATA8,
+	FN_CS1_N_A26, FN_VI1_DATA9,
+	FN_EX_CS0_N, FN_VI1_DATA10,
+	FN_EX_CS1_N, FN_TPUTO3_B, FN_SCIFB2_RXD, FN_VI1_DATA11,
+	FN_EX_CS2_N, FN_PWM0, FN_SCIF4_RXD_C, FN_TS_SDATA_B, FN_TPUTO3,
+	FN_SCIFB2_TXD,
+	FN_EX_CS3_N, FN_SCIFA2_SCK, FN_SCIF4_TXD_C, FN_TS_SCK_B, FN_BPFCLK,
+	FN_SCIFB2_SCK,
+	FN_EX_CS4_N, FN_SCIFA2_RXD, FN_I2C2_SCL_E, FN_TS_SDEN_B, FN_FMCLK,
+	FN_SCIFB2_CTS_N,
+	FN_EX_CS5_N, FN_SCIFA2_TXD, FN_I2C2_SDA_E, FN_TS_SPSYNC_B, FN_FMIN,
+	FN_SCIFB2_RTS_N,
+	FN_BS_N, FN_DRACK0, FN_PWM1_C, FN_TPUTO0_C, FN_ATACS01_N,
+	FN_RD_N, FN_ATACS11_N,
+	FN_RD_WR_N, FN_ATAG1_N,
 
 	/* IPSR4 */
-	FN_EX_WAIT0, FN_CAN_CLK_B, FN_SCIF_CLK, FN_PWMFSW0, FN_DU0_DR0,
-	FN_LCDOUT16, FN_SCIF5_RXD_C, FN_I2C2_SCL_D, FN_CC50_STATE0,
-	FN_DU0_DR1, FN_LCDOUT17, FN_SCIF5_TXD_C, FN_I2C2_SDA_D, FN_CC50_STATE1,
-	FN_DU0_DR2, FN_LCDOUT18, FN_CC50_STATE2, FN_DU0_DR3, FN_LCDOUT19,
-	FN_CC50_STATE3, FN_DU0_DR4, FN_LCDOUT20, FN_CC50_STATE4, FN_DU0_DR5,
-	FN_LCDOUT21, FN_CC50_STATE5, FN_DU0_DR6, FN_LCDOUT22, FN_CC50_STATE6,
-	FN_DU0_DR7, FN_LCDOUT23, FN_CC50_STATE7, FN_DU0_DG0, FN_LCDOUT8,
-	FN_SCIFA0_RXD_C, FN_I2C3_SCL_D, FN_CC50_STATE8, FN_DU0_DG1, FN_LCDOUT9,
-	FN_SCIFA0_TXD_C, FN_I2C3_SDA_D, FN_CC50_STATE9, FN_DU0_DG2, FN_LCDOUT10,
-	FN_CC50_STATE10, FN_DU0_DG3, FN_LCDOUT11, FN_CC50_STATE11, FN_DU0_DG4,
-	FN_LCDOUT12, FN_CC50_STATE12,
+	FN_EX_WAIT0, FN_CAN_CLK_B, FN_SCIF_CLK,
+	FN_DU0_DR0, FN_LCDOUT16, FN_SCIF5_RXD_C, FN_I2C2_SCL_D,
+	FN_DU0_DR1, FN_LCDOUT17, FN_SCIF5_TXD_C, FN_I2C2_SDA_D,
+	FN_DU0_DR2, FN_LCDOUT18,
+	FN_DU0_DR3, FN_LCDOUT19,
+	FN_DU0_DR4, FN_LCDOUT20,
+	FN_DU0_DR5, FN_LCDOUT21,
+	FN_DU0_DR6, FN_LCDOUT22,
+	FN_DU0_DR7, FN_LCDOUT23,
+	FN_DU0_DG0, FN_LCDOUT8, FN_SCIFA0_RXD_C, FN_I2C3_SCL_D,
+	FN_DU0_DG1, FN_LCDOUT9, FN_SCIFA0_TXD_C, FN_I2C3_SDA_D,
+	FN_DU0_DG2, FN_LCDOUT10,
+	FN_DU0_DG3, FN_LCDOUT11,
+	FN_DU0_DG4, FN_LCDOUT12,
 
 	/* IPSR5 */
-	FN_DU0_DG5, FN_LCDOUT13, FN_CC50_STATE13, FN_DU0_DG6, FN_LCDOUT14,
-	FN_CC50_STATE14, FN_DU0_DG7, FN_LCDOUT15, FN_CC50_STATE15, FN_DU0_DB0,
-	FN_LCDOUT0, FN_SCIFA4_RXD_C, FN_I2C4_SCL_D, FN_CAN0_RX_C,
-	FN_CC50_STATE16, FN_DU0_DB1, FN_LCDOUT1, FN_SCIFA4_TXD_C, FN_I2C4_SDA_D,
-	FN_CAN0_TX_C, FN_CC50_STATE17, FN_DU0_DB2, FN_LCDOUT2, FN_CC50_STATE18,
-	FN_DU0_DB3, FN_LCDOUT3, FN_CC50_STATE19, FN_DU0_DB4, FN_LCDOUT4,
-	FN_CC50_STATE20, FN_DU0_DB5, FN_LCDOUT5, FN_CC50_STATE21, FN_DU0_DB6,
-	FN_LCDOUT6, FN_CC50_STATE22, FN_DU0_DB7, FN_LCDOUT7, FN_CC50_STATE23,
-	FN_DU0_DOTCLKIN, FN_QSTVA_QVS, FN_CC50_STATE24, FN_DU0_DOTCLKOUT0,
-	FN_QCLK, FN_CC50_STATE25, FN_DU0_DOTCLKOUT1, FN_QSTVB_QVE,
-	FN_CC50_STATE26, FN_DU0_EXHSYNC_DU0_HSYNC, FN_QSTH_QHS, FN_CC50_STATE27,
+	FN_DU0_DG5, FN_LCDOUT13,
+	FN_DU0_DG6, FN_LCDOUT14,
+	FN_DU0_DG7, FN_LCDOUT15,
+	FN_DU0_DB0, FN_LCDOUT0, FN_SCIFA4_RXD_C, FN_I2C4_SCL_D, FN_CAN0_RX_C,
+	FN_DU0_DB1, FN_LCDOUT1, FN_SCIFA4_TXD_C, FN_I2C4_SDA_D, FN_CAN0_TX_C,
+	FN_DU0_DB2, FN_LCDOUT2,
+	FN_DU0_DB3, FN_LCDOUT3,
+	FN_DU0_DB4, FN_LCDOUT4,
+	FN_DU0_DB5, FN_LCDOUT5,
+	FN_DU0_DB6, FN_LCDOUT6,
+	FN_DU0_DB7, FN_LCDOUT7,
+	FN_DU0_DOTCLKIN, FN_QSTVA_QVS,
+	FN_DU0_DOTCLKOUT0, FN_QCLK,
+	FN_DU0_DOTCLKOUT1, FN_QSTVB_QVE,
+	FN_DU0_EXHSYNC_DU0_HSYNC, FN_QSTH_QHS,
 
 	/* IPSR6 */
-	FN_DU0_EXVSYNC_DU0_VSYNC, FN_QSTB_QHE, FN_CC50_STATE28,
-	FN_DU0_EXODDF_DU0_ODDF_DISP_CDE, FN_QCPV_QDE, FN_CC50_STATE29,
-	FN_DU0_DISP, FN_QPOLA, FN_CC50_STATE30, FN_DU0_CDE, FN_QPOLB,
-	FN_CC50_STATE31, FN_VI0_CLK, FN_AVB_RX_CLK, FN_VI0_DATA0_VI0_B0,
-	FN_AVB_RX_DV, FN_VI0_DATA1_VI0_B1, FN_AVB_RXD0, FN_VI0_DATA2_VI0_B2,
-	FN_AVB_RXD1, FN_VI0_DATA3_VI0_B3, FN_AVB_RXD2, FN_VI0_DATA4_VI0_B4,
-	FN_AVB_RXD3, FN_VI0_DATA5_VI0_B5, FN_AVB_RXD4, FN_VI0_DATA6_VI0_B6,
-	FN_AVB_RXD5, FN_VI0_DATA7_VI0_B7, FN_AVB_RXD6, FN_VI0_CLKENB,
-	FN_I2C3_SCL, FN_SCIFA5_RXD_C, FN_IETX_C, FN_AVB_RXD7, FN_VI0_FIELD,
-	FN_I2C3_SDA, FN_SCIFA5_TXD_C, FN_IECLK_C, FN_AVB_RX_ER, FN_VI0_HSYNC_N,
-	FN_SCIF0_RXD_B, FN_I2C0_SCL_C, FN_IERX_C, FN_AVB_COL, FN_VI0_VSYNC_N,
-	FN_SCIF0_TXD_B, FN_I2C0_SDA_C, FN_AUDIO_CLKOUT_B, FN_AVB_TX_EN,
-	FN_ETH_MDIO, FN_VI0_G0, FN_MSIOF2_RXD_B, FN_IIC0_SCL_D, FN_AVB_TX_CLK,
-	FN_ADIDATA, FN_AD_DI,
+	FN_DU0_EXVSYNC_DU0_VSYNC, FN_QSTB_QHE,
+	FN_DU0_EXODDF_DU0_ODDF_DISP_CDE, FN_QCPV_QDE,
+	FN_DU0_DISP, FN_QPOLA,
+	FN_DU0_CDE, FN_QPOLB,
+	FN_VI0_CLK, FN_AVB_RX_CLK,
+	FN_VI0_DATA0_VI0_B0, FN_AVB_RX_DV,
+	FN_VI0_DATA1_VI0_B1, FN_AVB_RXD0,
+	FN_VI0_DATA2_VI0_B2, FN_AVB_RXD1,
+	FN_VI0_DATA3_VI0_B3, FN_AVB_RXD2,
+	FN_VI0_DATA4_VI0_B4, FN_AVB_RXD3,
+	FN_VI0_DATA5_VI0_B5, FN_AVB_RXD4,
+	FN_VI0_DATA6_VI0_B6, FN_AVB_RXD5,
+	FN_VI0_DATA7_VI0_B7, FN_AVB_RXD6,
+	FN_VI0_CLKENB, FN_I2C3_SCL, FN_SCIFA5_RXD_C, FN_IETX_C, FN_AVB_RXD7,
+	FN_VI0_FIELD, FN_I2C3_SDA, FN_SCIFA5_TXD_C, FN_IECLK_C, FN_AVB_RX_ER,
+	FN_VI0_HSYNC_N, FN_SCIF0_RXD_B, FN_I2C0_SCL_C, FN_IERX_C, FN_AVB_COL,
+	FN_VI0_VSYNC_N, FN_SCIF0_TXD_B, FN_I2C0_SDA_C, FN_AUDIO_CLKOUT_B,
+	FN_AVB_TX_EN,
+	FN_ETH_MDIO, FN_VI0_G0, FN_MSIOF2_RXD_B, FN_I2C5_SCL_D, FN_AVB_TX_CLK,
+	FN_ADIDATA,
 
 	/* IPSR7 */
-	FN_ETH_CRS_DV, FN_VI0_G1, FN_MSIOF2_TXD_B, FN_IIC0_SDA_D, FN_AVB_TXD0,
-	FN_ADICS_SAMP, FN_AD_DO, FN_ETH_RX_ER, FN_VI0_G2, FN_MSIOF2_SCK_B,
-	FN_CAN0_RX_B, FN_AVB_TXD1, FN_ADICLK, FN_AD_CLK, FN_ETH_RXD0, FN_VI0_G3,
-	FN_MSIOF2_SYNC_B, FN_CAN0_TX_B, FN_AVB_TXD2, FN_ADICHS0, FN_AD_NCS_N,
+	FN_ETH_CRS_DV, FN_VI0_G1, FN_MSIOF2_TXD_B, FN_I2C5_SDA_D, FN_AVB_TXD0,
+	FN_ADICS_SAMP,
+	FN_ETH_RX_ER, FN_VI0_G2, FN_MSIOF2_SCK_B, FN_CAN0_RX_B, FN_AVB_TXD1,
+	FN_ADICLK,
+	FN_ETH_RXD0, FN_VI0_G3,	FN_MSIOF2_SYNC_B, FN_CAN0_TX_B, FN_AVB_TXD2,
+	FN_ADICHS0,
 	FN_ETH_RXD1, FN_VI0_G4, FN_MSIOF2_SS1_B, FN_SCIF4_RXD_D, FN_AVB_TXD3,
-	FN_ADICHS1, FN_ETH_LINK, FN_VI0_G5, FN_MSIOF2_SS2_B, FN_SCIF4_TXD_D,
-	FN_AVB_TXD4, FN_ADICHS2, FN_ETH_REFCLK, FN_VI0_G6, FN_SCIF2_SCK_C,
-	FN_AVB_TXD5, FN_SSI_SCK5_B, FN_ETH_TXD1, FN_VI0_G7, FN_SCIF2_RXD_C,
-	FN_IIC1_SCL_D, FN_AVB_TXD6, FN_SSI_WS5_B, FN_ETH_TX_EN, FN_VI0_R0,
-	FN_SCIF2_TXD_C, FN_IIC1_SDA_D, FN_AVB_TXD7, FN_SSI_SDATA5_B,
+	FN_ADICHS1,
+	FN_ETH_LINK, FN_VI0_G5, FN_MSIOF2_SS2_B, FN_SCIF4_TXD_D, FN_AVB_TXD4,
+	FN_ADICHS2,
+	FN_ETH_REFCLK, FN_VI0_G6, FN_SCIF2_SCK_C, FN_AVB_TXD5, FN_SSI_SCK5_B,
+	FN_ETH_TXD1, FN_VI0_G7, FN_SCIF2_RXD_C, FN_IIC0_SCL_D, FN_AVB_TXD6,
+	FN_SSI_WS5_B,
+	FN_ETH_TX_EN, FN_VI0_R0, FN_SCIF2_TXD_C, FN_IIC0_SDA_D, FN_AVB_TXD7,
+	FN_SSI_SDATA5_B,
 	FN_ETH_MAGIC, FN_VI0_R1, FN_SCIF3_SCK_B, FN_AVB_TX_ER, FN_SSI_SCK6_B,
 	FN_ETH_TXD0, FN_VI0_R2, FN_SCIF3_RXD_B, FN_I2C4_SCL_E, FN_AVB_GTX_CLK,
-	FN_SSI_WS6_B, FN_DREQ0_N, FN_SCIFB1_RXD,
+	FN_SSI_WS6_B,
+	FN_DREQ0_N, FN_SCIFB1_RXD,
 
 	/* IPSR8 */
 	FN_ETH_MDC, FN_VI0_R3, FN_SCIF3_TXD_B, FN_I2C4_SDA_E, FN_AVB_MDC,
-	FN_SSI_SDATA6_B, FN_HSCIF0_HRX, FN_VI0_R4, FN_I2C1_SCL_C,
-	FN_AUDIO_CLKA_B, FN_AVB_MDIO, FN_SSI_SCK78_B, FN_HSCIF0_HTX,
-	FN_VI0_R5, FN_I2C1_SDA_C, FN_AUDIO_CLKB_B, FN_AVB_LINK, FN_SSI_WS78_B,
+	FN_SSI_SDATA6_B,
+	FN_HSCIF0_HRX, FN_VI0_R4, FN_I2C1_SCL_C, FN_AUDIO_CLKA_B, FN_AVB_MDIO,
+	FN_SSI_SCK78_B,
+	FN_HSCIF0_HTX, FN_VI0_R5, FN_I2C1_SDA_C, FN_AUDIO_CLKB_B, FN_AVB_LINK,
+	FN_SSI_WS78_B,
 	FN_HSCIF0_HCTS_N, FN_VI0_R6, FN_SCIF0_RXD_D, FN_I2C0_SCL_E,
-	FN_AVB_MAGIC, FN_SSI_SDATA7_B, FN_HSCIF0_HRTS_N, FN_VI0_R7,
-	FN_SCIF0_TXD_D, FN_I2C0_SDA_E, FN_AVB_PHY_INT, FN_SSI_SDATA8_B,
+	FN_AVB_MAGIC, FN_SSI_SDATA7_B,
+	FN_HSCIF0_HRTS_N, FN_VI0_R7, FN_SCIF0_TXD_D, FN_I2C0_SDA_E,
+	FN_AVB_PHY_INT, FN_SSI_SDATA8_B,
 	FN_HSCIF0_HSCK, FN_SCIF_CLK_B, FN_AVB_CRS, FN_AUDIO_CLKC_B,
 	FN_I2C0_SCL, FN_SCIF0_RXD_C, FN_PWM5, FN_TCLK1_B, FN_AVB_GTXREFCLK,
-	FN_CAN1_RX_D, FN_TPUTO0_B, FN_I2C0_SDA, FN_SCIF0_TXD_C, FN_TPUTO0,
-	FN_CAN_CLK, FN_DVC_MUTE, FN_CAN1_TX_D, FN_I2C1_SCL, FN_SCIF4_RXD,
-	FN_PWM5_B, FN_DU1_DR0, FN_RIF1_SYNC_B, FN_TS_SDATA_D, FN_TPUTO1_B,
-	FN_I2C1_SDA, FN_SCIF4_TXD, FN_IRQ5, FN_DU1_DR1, FN_RIF1_CLK_B,
-	FN_TS_SCK_D, FN_BPFCLK_C, FN_MSIOF0_RXD, FN_SCIF5_RXD, FN_I2C2_SCL_C,
-	FN_DU1_DR2, FN_RIF1_D0_B, FN_TS_SDEN_D, FN_FMCLK_C, FN_RDS_CLK,
+	FN_CAN1_RX_D, FN_TPUTO0_B,
+	FN_I2C0_SDA, FN_SCIF0_TXD_C, FN_TPUTO0, FN_CAN_CLK, FN_DVC_MUTE,
+	FN_CAN1_TX_D,
+	FN_I2C1_SCL, FN_SCIF4_RXD, FN_PWM5_B, FN_DU1_DR0, FN_TS_SDATA_D,
+	FN_TPUTO1_B,
+	FN_I2C1_SDA, FN_SCIF4_TXD, FN_IRQ5, FN_DU1_DR1, FN_TS_SCK_D,
+	FN_BPFCLK_C,
+	FN_MSIOF0_RXD, FN_SCIF5_RXD, FN_I2C2_SCL_C, FN_DU1_DR2, FN_TS_SDEN_D,
+	FN_FMCLK_C,
 
 	/* IPSR9 */
-	FN_MSIOF0_TXD, FN_SCIF5_TXD, FN_I2C2_SDA_C, FN_DU1_DR3, FN_RIF1_D1_B,
-	FN_TS_SPSYNC_D, FN_FMIN_C, FN_RDS_DATA, FN_MSIOF0_SCK, FN_IRQ0,
-	FN_TS_SDATA, FN_DU1_DR4, FN_RIF1_SYNC, FN_TPUTO1_C, FN_MSIOF0_SYNC,
-	FN_PWM1, FN_TS_SCK, FN_DU1_DR5, FN_RIF1_CLK, FN_BPFCLK_B, FN_MSIOF0_SS1,
-	FN_SCIFA0_RXD, FN_TS_SDEN, FN_DU1_DR6, FN_RIF1_D0, FN_FMCLK_B,
-	FN_RDS_CLK_B, FN_MSIOF0_SS2, FN_SCIFA0_TXD, FN_TS_SPSYNC, FN_DU1_DR7,
-	FN_RIF1_D1, FN_FMIN_B, FN_RDS_DATA_B, FN_HSCIF1_HRX, FN_I2C4_SCL,
-	FN_PWM6, FN_DU1_DG0, FN_HSCIF1_HTX, FN_I2C4_SDA, FN_TPUTO1, FN_DU1_DG1,
+	FN_MSIOF0_TXD, FN_SCIF5_TXD, FN_I2C2_SDA_C, FN_DU1_DR3, FN_TS_SPSYNC_D,
+	FN_FMIN_C,
+	FN_MSIOF0_SCK, FN_IRQ0, FN_TS_SDATA, FN_DU1_DR4, FN_TPUTO1_C,
+	FN_MSIOF0_SYNC, FN_PWM1, FN_TS_SCK, FN_DU1_DR5, FN_BPFCLK_B,
+	FN_MSIOF0_SS1, FN_SCIFA0_RXD, FN_TS_SDEN, FN_DU1_DR6, FN_FMCLK_B,
+	FN_MSIOF0_SS2, FN_SCIFA0_TXD, FN_TS_SPSYNC, FN_DU1_DR7, FN_FMIN_B,
+	FN_HSCIF1_HRX, FN_I2C4_SCL, FN_PWM6, FN_DU1_DG0,
+	FN_HSCIF1_HTX, FN_I2C4_SDA, FN_TPUTO1, FN_DU1_DG1,
 	FN_HSCIF1_HSCK, FN_PWM2, FN_IETX, FN_DU1_DG2, FN_REMOCON_B,
-	FN_SPEEDIN_B, FN_VSP_B, FN_HSCIF1_HCTS_N, FN_SCIFA4_RXD, FN_IECLK,
-	FN_DU1_DG3, FN_SSI_SCK1_B, FN_CAN_DEBUG_HW_TRIGGER, FN_CC50_STATE32,
+	FN_SPEEDIN_B,
+	FN_HSCIF1_HCTS_N, FN_SCIFA4_RXD, FN_IECLK, FN_DU1_DG3, FN_SSI_SCK1_B,
 	FN_HSCIF1_HRTS_N, FN_SCIFA4_TXD, FN_IERX, FN_DU1_DG4, FN_SSI_WS1_B,
-	FN_CAN_STEP0, FN_CC50_STATE33, FN_SCIF1_SCK, FN_PWM3, FN_TCLK2,
-	FN_DU1_DG5, FN_SSI_SDATA1_B, FN_CAN_TXCLK, FN_CC50_STATE34,
+	FN_SCIF1_SCK, FN_PWM3, FN_TCLK2, FN_DU1_DG5, FN_SSI_SDATA1_B,
 
 	/* IPSR10 */
-	FN_SCIF1_RXD, FN_IIC0_SCL, FN_DU1_DG6, FN_SSI_SCK2_B, FN_CAN_DEBUGOUT0,
-	FN_CC50_STATE35, FN_SCIF1_TXD, FN_IIC0_SDA, FN_DU1_DG7, FN_SSI_WS2_B,
-	FN_CAN_DEBUGOUT1, FN_CC50_STATE36, FN_SCIF2_RXD, FN_IIC1_SCL,
-	FN_DU1_DB0, FN_SSI_SDATA2_B, FN_USB0_EXTLP, FN_CAN_DEBUGOUT2,
-	FN_CC50_STATE37, FN_SCIF2_TXD, FN_IIC1_SDA, FN_DU1_DB1, FN_SSI_SCK9_B,
-	FN_USB0_OVC1, FN_CAN_DEBUGOUT3, FN_CC50_STATE38, FN_SCIF2_SCK, FN_IRQ1,
-	FN_DU1_DB2, FN_SSI_WS9_B, FN_USB0_IDIN, FN_CAN_DEBUGOUT4,
-	FN_CC50_STATE39, FN_SCIF3_SCK, FN_IRQ2, FN_BPFCLK_D, FN_DU1_DB3,
-	FN_SSI_SDATA9_B, FN_TANS2, FN_CAN_DEBUGOUT5, FN_CC50_OSCOUT,
+	FN_SCIF1_RXD, FN_I2C5_SCL, FN_DU1_DG6, FN_SSI_SCK2_B,
+	FN_SCIF1_TXD, FN_I2C5_SDA, FN_DU1_DG7, FN_SSI_WS2_B,
+	FN_SCIF2_RXD, FN_IIC0_SCL, FN_DU1_DB0, FN_SSI_SDATA2_B,
+	FN_SCIF2_TXD, FN_IIC0_SDA, FN_DU1_DB1, FN_SSI_SCK9_B,
+	FN_SCIF2_SCK, FN_IRQ1, FN_DU1_DB2, FN_SSI_WS9_B,
+	FN_SCIF3_SCK, FN_IRQ2, FN_BPFCLK_D, FN_DU1_DB3, FN_SSI_SDATA9_B,
 	FN_SCIF3_RXD, FN_I2C1_SCL_E, FN_FMCLK_D, FN_DU1_DB4, FN_AUDIO_CLKA_C,
-	FN_SSI_SCK4_B, FN_CAN_DEBUGOUT6, FN_RDS_CLK_C, FN_SCIF3_TXD,
-	FN_I2C1_SDA_E, FN_FMIN_D, FN_DU1_DB5, FN_AUDIO_CLKB_C, FN_SSI_WS4_B,
-	FN_CAN_DEBUGOUT7, FN_RDS_DATA_C, FN_I2C2_SCL, FN_SCIFA5_RXD, FN_DU1_DB6,
-	FN_AUDIO_CLKC_C, FN_SSI_SDATA4_B, FN_CAN_DEBUGOUT8, FN_I2C2_SDA,
-	FN_SCIFA5_TXD, FN_DU1_DB7, FN_AUDIO_CLKOUT_C, FN_CAN_DEBUGOUT9,
-	FN_SSI_SCK5, FN_SCIFA3_SCK, FN_DU1_DOTCLKIN, FN_CAN_DEBUGOUT10,
+	FN_SSI_SCK4_B,
+	FN_SCIF3_TXD, FN_I2C1_SDA_E, FN_FMIN_D, FN_DU1_DB5, FN_AUDIO_CLKB_C,
+	FN_SSI_WS4_B,
+	FN_I2C2_SCL, FN_SCIFA5_RXD, FN_DU1_DB6, FN_AUDIO_CLKC_C,
+	FN_SSI_SDATA4_B,
+	FN_I2C2_SDA, FN_SCIFA5_TXD, FN_DU1_DB7, FN_AUDIO_CLKOUT_C,
+	FN_SSI_SCK5, FN_SCIFA3_SCK, FN_DU1_DOTCLKIN,
 
 	/* IPSR11 */
 	FN_SSI_WS5, FN_SCIFA3_RXD, FN_I2C3_SCL_C, FN_DU1_DOTCLKOUT0,
-	FN_CAN_DEBUGOUT11, FN_SSI_SDATA5, FN_SCIFA3_TXD, FN_I2C3_SDA_C,
-	FN_DU1_DOTCLKOUT1, FN_CAN_DEBUGOUT12, FN_SSI_SCK6, FN_SCIFA1_SCK_B,
-	FN_DU1_EXHSYNC_DU1_HSYNC, FN_CAN_DEBUGOUT13, FN_SSI_WS6,
-	FN_SCIFA1_RXD_B, FN_I2C4_SCL_C, FN_DU1_EXVSYNC_DU1_VSYNC,
-	FN_CAN_DEBUGOUT14, FN_SSI_SDATA6, FN_SCIFA1_TXD_B, FN_I2C4_SDA_C,
-	FN_DU1_EXODDF_DU1_ODDF_DISP_CDE, FN_CAN_DEBUGOUT15, FN_SSI_SCK78,
-	FN_SCIFA2_SCK_B, FN_IIC0_SDA_C, FN_DU1_DISP, FN_SSI_WS78,
-	FN_SCIFA2_RXD_B, FN_IIC0_SCL_C, FN_DU1_CDE, FN_SSI_SDATA7,
-	FN_SCIFA2_TXD_B, FN_IRQ8, FN_AUDIO_CLKA_D, FN_CAN_CLK_D, FN_PCMOE_N,
+	FN_SSI_SDATA5, FN_SCIFA3_TXD, FN_I2C3_SDA_C, FN_DU1_DOTCLKOUT1,
+	FN_SSI_SCK6, FN_SCIFA1_SCK_B, FN_DU1_EXHSYNC_DU1_HSYNC,
+	FN_SSI_WS6, FN_SCIFA1_RXD_B, FN_I2C4_SCL_C, FN_DU1_EXVSYNC_DU1_VSYNC,
+	FN_SSI_SDATA6, FN_SCIFA1_TXD_B, FN_I2C4_SDA_C,
+	FN_DU1_EXODDF_DU1_ODDF_DISP_CDE,
+	FN_SSI_SCK78, FN_SCIFA2_SCK_B, FN_I2C5_SDA_C, FN_DU1_DISP,
+	FN_SSI_WS78, FN_SCIFA2_RXD_B, FN_I2C5_SCL_C, FN_DU1_CDE,
+	FN_SSI_SDATA7, FN_SCIFA2_TXD_B, FN_IRQ8, FN_AUDIO_CLKA_D, FN_CAN_CLK_D,
 	FN_SSI_SCK0129, FN_MSIOF1_RXD_B, FN_SCIF5_RXD_D, FN_ADIDATA_B,
-	FN_AD_DI_B, FN_PCMWE_N, FN_SSI_WS0129, FN_MSIOF1_TXD_B, FN_SCIF5_TXD_D,
-	FN_ADICS_SAMP_B, FN_AD_DO_B, FN_SSI_SDATA0, FN_MSIOF1_SCK_B, FN_PWM0_B,
-	FN_ADICLK_B, FN_AD_CLK_B,
+	FN_SSI_WS0129, FN_MSIOF1_TXD_B, FN_SCIF5_TXD_D, FN_ADICS_SAMP_B,
+	FN_SSI_SDATA0, FN_MSIOF1_SCK_B, FN_PWM0_B, FN_ADICLK_B,
 
 	/* IPSR12 */
 	FN_SSI_SCK34, FN_MSIOF1_SYNC_B, FN_SCIFA1_SCK_C, FN_ADICHS0_B,
-	FN_AD_NCS_N_B, FN_DREQ1_N_B, FN_SSI_WS34, FN_MSIOF1_SS1_B,
-	FN_SCIFA1_RXD_C, FN_ADICHS1_B, FN_CAN1_RX_C, FN_DACK1_B, FN_SSI_SDATA3,
-	FN_MSIOF1_SS2_B, FN_SCIFA1_TXD_C, FN_ADICHS2_B, FN_CAN1_TX_C,
-	FN_DREQ2_N, FN_SSI_SCK4, FN_MLB_CLK, FN_IETX_B, FN_IRD_TX, FN_SSI_WS4,
-	FN_MLB_SIG, FN_IECLK_B, FN_IRD_RX, FN_SSI_SDATA4, FN_MLB_DAT,
-	FN_IERX_B, FN_IRD_SCK, FN_SSI_SDATA8, FN_SCIF1_SCK_B,
-	FN_PWM1_B, FN_IRQ9, FN_REMOCON, FN_DACK2, FN_ETH_MDIO_B, FN_SSI_SCK1,
-	FN_SCIF1_RXD_B, FN_IIC1_SCL_C, FN_VI1_CLK, FN_CAN0_RX_D,
-	FN_AVB_AVTP_CAPTURE, FN_ETH_CRS_DV_B, FN_SSI_WS1, FN_SCIF1_TXD_B,
-	FN_IIC1_SDA_C, FN_VI1_DATA0, FN_CAN0_TX_D, FN_AVB_AVTP_MATCH,
-	FN_ETH_RX_ER_B, FN_SSI_SDATA1, FN_HSCIF1_HRX_B, FN_SDATA, FN_VI1_DATA1,
-	FN_ATAWR0_N, FN_ETH_RXD0_B, FN_SSI_SCK2, FN_HSCIF1_HTX_B, FN_VI1_DATA2,
-	FN_MDATA, FN_ATAG0_N, FN_ETH_RXD1_B,
+	FN_DREQ1_N_B,
+	FN_SSI_WS34, FN_MSIOF1_SS1_B, FN_SCIFA1_RXD_C, FN_ADICHS1_B,
+	FN_CAN1_RX_C, FN_DACK1_B,
+	FN_SSI_SDATA3, FN_MSIOF1_SS2_B, FN_SCIFA1_TXD_C, FN_ADICHS2_B,
+	FN_CAN1_TX_C, FN_DREQ2_N,
+	FN_SSI_SCK4, FN_MLB_CLK, FN_IETX_B, FN_SSI_WS4, FN_MLB_SIG, FN_IECLK_B,
+	FN_SSI_SDATA4, FN_MLB_DAT, FN_IERX_B,
+	FN_SSI_SDATA8, FN_SCIF1_SCK_B, FN_PWM1_B, FN_IRQ9, FN_REMOCON,
+	FN_DACK2, FN_ETH_MDIO_B,
+	FN_SSI_SCK1, FN_SCIF1_RXD_B, FN_IIC0_SCL_C, FN_VI1_CLK, FN_CAN0_RX_D,
+	FN_ETH_CRS_DV_B,
+	FN_SSI_WS1, FN_SCIF1_TXD_B, FN_IIC0_SDA_C, FN_VI1_DATA0, FN_CAN0_TX_D,
+	FN_ETH_RX_ER_B,
+	FN_SSI_SDATA1, FN_HSCIF1_HRX_B, FN_VI1_DATA1, FN_ATAWR0_N,
+	FN_ETH_RXD0_B,
+	FN_SSI_SCK2, FN_HSCIF1_HTX_B, FN_VI1_DATA2, FN_ATAG0_N, FN_ETH_RXD1_B,
 
 	/* IPSR13 */
-	FN_SSI_WS2, FN_HSCIF1_HCTS_N_B, FN_SCIFA0_RXD_D, FN_VI1_DATA3, FN_SCKZ,
-	FN_ATACS00_N, FN_ETH_LINK_B, FN_SSI_SDATA2, FN_HSCIF1_HRTS_N_B,
-	FN_SCIFA0_TXD_D, FN_VI1_DATA4, FN_STM_N, FN_ATACS10_N, FN_ETH_REFCLK_B,
-	FN_SSI_SCK9, FN_SCIF2_SCK_B, FN_PWM2_B, FN_VI1_DATA5, FN_MTS_N,
-	FN_EX_WAIT1, FN_ETH_TXD1_B, FN_SSI_WS9, FN_SCIF2_RXD_B, FN_I2C3_SCL_E,
-	FN_VI1_DATA6, FN_ATARD0_N, FN_ETH_TX_EN_B, FN_SSI_SDATA9,
-	FN_SCIF2_TXD_B, FN_I2C3_SDA_E, FN_VI1_DATA7, FN_ATADIR0_N,
-	FN_ETH_MAGIC_B, FN_AUDIO_CLKA, FN_I2C0_SCL_B, FN_SCIFA4_RXD_D,
-	FN_VI1_CLKENB, FN_TS_SDATA_C, FN_RIF0_SYNC_B, FN_ETH_TXD0_B,
+	FN_SSI_WS2, FN_HSCIF1_HCTS_N_B, FN_SCIFA0_RXD_D, FN_VI1_DATA3,
+	FN_ATACS00_N, FN_ETH_LINK_B,
+	FN_SSI_SDATA2, FN_HSCIF1_HRTS_N_B, FN_SCIFA0_TXD_D, FN_VI1_DATA4,
+	FN_ATACS10_N, FN_ETH_REFCLK_B,
+	FN_SSI_SCK9, FN_SCIF2_SCK_B, FN_PWM2_B, FN_VI1_DATA5, FN_EX_WAIT1,
+	FN_ETH_TXD1_B,
+	FN_SSI_WS9, FN_SCIF2_RXD_B, FN_I2C3_SCL_E, FN_VI1_DATA6, FN_ATARD0_N,
+	FN_ETH_TX_EN_B,
+	FN_SSI_SDATA9, FN_SCIF2_TXD_B, FN_I2C3_SDA_E, FN_VI1_DATA7,
+	FN_ATADIR0_N, FN_ETH_MAGIC_B,
+	FN_AUDIO_CLKA, FN_I2C0_SCL_B, FN_SCIFA4_RXD_D, FN_VI1_CLKENB,
+	FN_TS_SDATA_C, FN_ETH_TXD0_B,
 	FN_AUDIO_CLKB, FN_I2C0_SDA_B, FN_SCIFA4_TXD_D, FN_VI1_FIELD,
-	FN_TS_SCK_C, FN_RIF0_CLK_B, FN_BPFCLK_E, FN_ETH_MDC_B, FN_AUDIO_CLKC,
-	FN_I2C4_SCL_B, FN_SCIFA5_RXD_D, FN_VI1_HSYNC_N, FN_TS_SDEN_C,
-	FN_RIF0_D0_B, FN_FMCLK_E, FN_RDS_CLK_D, FN_AUDIO_CLKOUT, FN_I2C4_SDA_B,
-	FN_SCIFA5_TXD_D, FN_VI1_VSYNC_N, FN_TS_SPSYNC_C, FN_RIF0_D1_B,
-	FN_FMIN_E, FN_RDS_DATA_D,
+	FN_TS_SCK_C, FN_BPFCLK_E, FN_ETH_MDC_B,
+	FN_AUDIO_CLKC, FN_I2C4_SCL_B, FN_SCIFA5_RXD_D, FN_VI1_HSYNC_N,
+	FN_TS_SDEN_C, FN_FMCLK_E,
+	FN_AUDIO_CLKOUT, FN_I2C4_SDA_B, FN_SCIFA5_TXD_D, FN_VI1_VSYNC_N,
+	FN_TS_SPSYNC_C, FN_FMIN_E,
 
 	/* MOD_SEL */
 	FN_SEL_ADG_0, FN_SEL_ADG_1, FN_SEL_ADG_2, FN_SEL_ADG_3,
-	FN_SEL_ADI_0, FN_SEL_ADI_1, FN_SEL_CAN_0, FN_SEL_CAN_1,
-	FN_SEL_CAN_2, FN_SEL_CAN_3, FN_SEL_DARC_0, FN_SEL_DARC_1,
-	FN_SEL_DARC_2, FN_SEL_DARC_3, FN_SEL_DARC_4, FN_SEL_DR0_0,
-	FN_SEL_DR0_1, FN_SEL_DR1_0, FN_SEL_DR1_1, FN_SEL_DR2_0, FN_SEL_DR2_1,
-	FN_SEL_DR3_0, FN_SEL_DR3_1, FN_SEL_ETH_0, FN_SEL_ETH_1, FN_SEL_FSN_0,
-	FN_SEL_FSN_1, FN_SEL_I2C00_0, FN_SEL_I2C00_1, FN_SEL_I2C00_2,
-	FN_SEL_I2C00_3, FN_SEL_I2C00_4, FN_SEL_I2C01_0, FN_SEL_I2C01_1,
-	FN_SEL_I2C01_2, FN_SEL_I2C01_3, FN_SEL_I2C01_4, FN_SEL_I2C02_0,
-	FN_SEL_I2C02_1, FN_SEL_I2C02_2, FN_SEL_I2C02_3, FN_SEL_I2C02_4,
+	FN_SEL_CAN_0, FN_SEL_CAN_1, FN_SEL_CAN_2, FN_SEL_CAN_3,
+	FN_SEL_DARC_0, FN_SEL_DARC_1, FN_SEL_DARC_2, FN_SEL_DARC_3,
+	FN_SEL_DARC_4,
+	FN_SEL_ETH_0, FN_SEL_ETH_1,
+	FN_SEL_I2C00_0, FN_SEL_I2C00_1, FN_SEL_I2C00_2,	FN_SEL_I2C00_3,
+	FN_SEL_I2C00_4,
+	FN_SEL_I2C01_0, FN_SEL_I2C01_1,	FN_SEL_I2C01_2, FN_SEL_I2C01_3,
+	FN_SEL_I2C01_4,
+	FN_SEL_I2C02_0, FN_SEL_I2C02_1, FN_SEL_I2C02_2, FN_SEL_I2C02_3,
+	FN_SEL_I2C02_4,
 	FN_SEL_I2C03_0, FN_SEL_I2C03_1, FN_SEL_I2C03_2, FN_SEL_I2C03_3,
-	FN_SEL_I2C03_4, FN_SEL_I2C04_0, FN_SEL_I2C04_1, FN_SEL_I2C04_2,
-	FN_SEL_I2C04_3, FN_SEL_I2C04_4, FN_SEL_IIC00_0, FN_SEL_IIC00_1,
-	FN_SEL_IIC00_2, FN_SEL_IIC00_3, FN_SEL_AVB_0, FN_SEL_AVB_1,
+	FN_SEL_I2C03_4,
+	FN_SEL_I2C04_0, FN_SEL_I2C04_1, FN_SEL_I2C04_2,	FN_SEL_I2C04_3,
+	FN_SEL_I2C04_4,
+	FN_SEL_I2C05_0, FN_SEL_I2C05_1,	FN_SEL_I2C05_2, FN_SEL_I2C05_3,
 
 	/* MOD_SEL2 */
-	FN_SEL_IEB_0, FN_SEL_IEB_1, FN_SEL_IEB_2, FN_SEL_IIC01_0,
-	FN_SEL_IIC01_1, FN_SEL_IIC01_2, FN_SEL_IIC01_3, FN_SEL_LBS_0,
-	FN_SEL_LBS_1, FN_SEL_MSI1_0, FN_SEL_MSI1_1, FN_SEL_MSI2_0,
-	FN_SEL_MSI2_1, FN_SEL_RAD_0, FN_SEL_RAD_1, FN_SEL_RCN_0,
-	FN_SEL_RCN_1, FN_SEL_RSP_0, FN_SEL_RSP_1, FN_SEL_SCIFA0_0,
-	FN_SEL_SCIFA0_1, FN_SEL_SCIFA0_2, FN_SEL_SCIFA0_3, FN_SEL_SCIFA1_0,
-	FN_SEL_SCIFA1_1, FN_SEL_SCIFA1_2, FN_SEL_SCIFA2_0, FN_SEL_SCIFA2_1,
-	FN_SEL_SCIFA3_0, FN_SEL_SCIFA3_1, FN_SEL_SCIFA4_0, FN_SEL_SCIFA4_1,
-	FN_SEL_SCIFA4_2, FN_SEL_SCIFA4_3, FN_SEL_SCIFA5_0, FN_SEL_SCIFA5_1,
-	FN_SEL_SCIFA5_2, FN_SEL_SCIFA5_3, FN_SEL_SPDM_0, FN_SEL_SPDM_1,
-	FN_SEL_TMU_0, FN_SEL_TMU_1, FN_SEL_TSIF0_0, FN_SEL_TSIF0_1,
-	FN_SEL_TSIF0_2, FN_SEL_TSIF0_3, FN_SEL_CAN0_0, FN_SEL_CAN0_1,
-	FN_SEL_CAN0_2, FN_SEL_CAN0_3, FN_SEL_CAN1_0, FN_SEL_CAN1_1,
-	FN_SEL_CAN1_2, FN_SEL_CAN1_3, FN_SEL_HSCIF0_0, FN_SEL_HSCIF0_1,
-	FN_SEL_HSCIF1_0, FN_SEL_HSCIF1_1, FN_SEL_RDS_0, FN_SEL_RDS_1,
-	FN_SEL_RDS_2, FN_SEL_RDS_3,
+	FN_SEL_IEB_0, FN_SEL_IEB_1, FN_SEL_IEB_2,
+	FN_SEL_IIC0_0, FN_SEL_IIC0_1, FN_SEL_IIC0_2, FN_SEL_IIC0_3,
+	FN_SEL_LBS_0, FN_SEL_LBS_1, FN_SEL_MSI1_0, FN_SEL_MSI1_1,
+	FN_SEL_MSI2_0, FN_SEL_MSI2_1, FN_SEL_RAD_0, FN_SEL_RAD_1,
+	FN_SEL_RCN_0, FN_SEL_RCN_1, FN_SEL_RSP_0, FN_SEL_RSP_1,
+	FN_SEL_SCIFA0_0, FN_SEL_SCIFA0_1, FN_SEL_SCIFA0_2, FN_SEL_SCIFA0_3,
+	FN_SEL_SCIFA1_0, FN_SEL_SCIFA1_1, FN_SEL_SCIFA1_2,
+	FN_SEL_SCIFA2_0, FN_SEL_SCIFA2_1, FN_SEL_SCIFA3_0, FN_SEL_SCIFA3_1,
+	FN_SEL_SCIFA4_0, FN_SEL_SCIFA4_1, FN_SEL_SCIFA4_2, FN_SEL_SCIFA4_3,
+	FN_SEL_SCIFA5_0, FN_SEL_SCIFA5_1, FN_SEL_SCIFA5_2, FN_SEL_SCIFA5_3,
+	FN_SEL_TMU_0, FN_SEL_TMU_1,
+	FN_SEL_TSIF0_0, FN_SEL_TSIF0_1, FN_SEL_TSIF0_2, FN_SEL_TSIF0_3,
+	FN_SEL_CAN0_0, FN_SEL_CAN0_1, FN_SEL_CAN0_2, FN_SEL_CAN0_3,
+	FN_SEL_CAN1_0, FN_SEL_CAN1_1, FN_SEL_CAN1_2, FN_SEL_CAN1_3,
+	FN_SEL_HSCIF0_0, FN_SEL_HSCIF0_1, FN_SEL_HSCIF1_0, FN_SEL_HSCIF1_1,
 
 	/* MOD_SEL3 */
 	FN_SEL_SCIF0_0, FN_SEL_SCIF0_1, FN_SEL_SCIF0_2, FN_SEL_SCIF0_3,
@@ -372,117 +416,141 @@ enum {
 	SCIF4_RXD_B_MARK, I2C0_SCL_D_MARK,
 
 	/* IPSR1 */
-	D6_MARK, SCIF4_TXD_B_MARK, I2C0_SDA_D_MARK, D7_MARK, IRQ3_MARK,
-	TCLK1_MARK, PWM6_B_MARK, D8_MARK, HSCIF2_HRX_MARK, I2C1_SCL_B_MARK,
-	D9_MARK, HSCIF2_HTX_MARK, I2C1_SDA_B_MARK, D10_MARK,
-	HSCIF2_HSCK_MARK, SCIF1_SCK_C_MARK, IRQ6_MARK, PWM5_C_MARK,
+	D6_MARK, SCIF4_TXD_B_MARK, I2C0_SDA_D_MARK,
+	D7_MARK, IRQ3_MARK, TCLK1_MARK, PWM6_B_MARK,
+	D8_MARK, HSCIF2_HRX_MARK, I2C1_SCL_B_MARK,
+	D9_MARK, HSCIF2_HTX_MARK, I2C1_SDA_B_MARK,
+	D10_MARK, HSCIF2_HSCK_MARK, SCIF1_SCK_C_MARK, IRQ6_MARK, PWM5_C_MARK,
 	D11_MARK, HSCIF2_HCTS_N_MARK, SCIF1_RXD_C_MARK, I2C1_SCL_D_MARK,
 	D12_MARK, HSCIF2_HRTS_N_MARK, SCIF1_TXD_C_MARK, I2C1_SDA_D_MARK,
-	D13_MARK, SCIFA1_SCK_MARK, TANS1_MARK, PWM2_C_MARK, TCLK2_B_MARK,
-	D14_MARK, SCIFA1_RXD_MARK, IIC0_SCL_B_MARK, D15_MARK, SCIFA1_TXD_MARK,
-	IIC0_SDA_B_MARK, A0_MARK, SCIFB1_SCK_MARK, PWM3_B_MARK, A1_MARK,
-	SCIFB1_TXD_MARK, A3_MARK, SCIFB0_SCK_MARK, A4_MARK, SCIFB0_TXD_MARK,
-	A5_MARK, SCIFB0_RXD_MARK, PWM4_B_MARK, TPUTO3_C_MARK, A6_MARK,
-	SCIFB0_CTS_N_MARK, SCIFA4_RXD_B_MARK, TPUTO2_C_MARK,
+	D13_MARK, SCIFA1_SCK_MARK, PWM2_C_MARK, TCLK2_B_MARK,
+	D14_MARK, SCIFA1_RXD_MARK, I2C5_SCL_B_MARK,
+	D15_MARK, SCIFA1_TXD_MARK, I2C5_SDA_B_MARK,
+	A0_MARK, SCIFB1_SCK_MARK, PWM3_B_MARK,
+	A1_MARK, SCIFB1_TXD_MARK,
+	A3_MARK, SCIFB0_SCK_MARK,
+	A4_MARK, SCIFB0_TXD_MARK,
+	A5_MARK, SCIFB0_RXD_MARK, PWM4_B_MARK, TPUTO3_C_MARK,
+	A6_MARK, SCIFB0_CTS_N_MARK, SCIFA4_RXD_B_MARK, TPUTO2_C_MARK,
 
 	/* IPSR2 */
-	A7_MARK, SCIFB0_RTS_N_MARK, SCIFA4_TXD_B_MARK, A8_MARK, MSIOF1_RXD_MARK,
-	SCIFA0_RXD_B_MARK, A9_MARK, MSIOF1_TXD_MARK, SCIFA0_TXD_B_MARK,
-	A10_MARK, MSIOF1_SCK_MARK, IIC1_SCL_B_MARK, A11_MARK, MSIOF1_SYNC_MARK,
-	IIC1_SDA_B_MARK, A12_MARK, MSIOF1_SS1_MARK, SCIFA5_RXD_B_MARK,
-	A13_MARK, MSIOF1_SS2_MARK, SCIFA5_TXD_B_MARK, A14_MARK, MSIOF2_RXD_MARK,
-	HSCIF0_HRX_B_MARK, DREQ1_N_MARK, A15_MARK, MSIOF2_TXD_MARK,
-	HSCIF0_HTX_B_MARK, DACK1_MARK, A16_MARK, MSIOF2_SCK_MARK,
-	HSCIF0_HSCK_B_MARK, SPEEDIN_MARK, VSP_MARK, CAN_CLK_C_MARK,
-	TPUTO2_B_MARK, A17_MARK, MSIOF2_SYNC_MARK, SCIF4_RXD_E_MARK,
-	CAN1_RX_B_MARK, AVB_AVTP_CAPTURE_B_MARK, A18_MARK, MSIOF2_SS1_MARK,
-	SCIF4_TXD_E_MARK, CAN1_TX_B_MARK, AVB_AVTP_MATCH_B_MARK, A19_MARK,
-	MSIOF2_SS2_MARK, PWM4_MARK, TPUTO2_MARK, MOUT0_MARK, A20_MARK,
-	SPCLK_MARK, MOUT1_MARK,
+	A7_MARK, SCIFB0_RTS_N_MARK, SCIFA4_TXD_B_MARK,
+	A8_MARK, MSIOF1_RXD_MARK, SCIFA0_RXD_B_MARK,
+	A9_MARK, MSIOF1_TXD_MARK, SCIFA0_TXD_B_MARK,
+	A10_MARK, MSIOF1_SCK_MARK, IIC0_SCL_B_MARK,
+	A11_MARK, MSIOF1_SYNC_MARK, IIC0_SDA_B_MARK,
+	A12_MARK, MSIOF1_SS1_MARK, SCIFA5_RXD_B_MARK,
+	A13_MARK, MSIOF1_SS2_MARK, SCIFA5_TXD_B_MARK,
+	A14_MARK, MSIOF2_RXD_MARK, HSCIF0_HRX_B_MARK, DREQ1_N_MARK,
+	A15_MARK, MSIOF2_TXD_MARK, HSCIF0_HTX_B_MARK, DACK1_MARK,
+	A16_MARK, MSIOF2_SCK_MARK, HSCIF0_HSCK_B_MARK, SPEEDIN_MARK,
+	CAN_CLK_C_MARK, TPUTO2_B_MARK,
+	A17_MARK, MSIOF2_SYNC_MARK, SCIF4_RXD_E_MARK, CAN1_RX_B_MARK,
+	A18_MARK, MSIOF2_SS1_MARK, SCIF4_TXD_E_MARK, CAN1_TX_B_MARK,
+	A19_MARK, MSIOF2_SS2_MARK, PWM4_MARK, TPUTO2_MARK,
+	A20_MARK, SPCLK_MARK,
 
 	/* IPSR3 */
-	A21_MARK, MOSI_IO0_MARK, MOUT2_MARK, A22_MARK, MISO_IO1_MARK,
-	MOUT5_MARK, ATADIR1_N_MARK, A23_MARK, IO2_MARK, MOUT6_MARK,
-	ATAWR1_N_MARK, A24_MARK, IO3_MARK, EX_WAIT2_MARK, A25_MARK, SSL_MARK,
-	ATARD1_N_MARK, CS0_N_MARK, VI1_DATA8_MARK, CS1_N_A26_MARK,
-	VI1_DATA9_MARK, EX_CS0_N_MARK, VI1_DATA10_MARK, EX_CS1_N_MARK,
-	TPUTO3_B_MARK, SCIFB2_RXD_MARK, VI1_DATA11_MARK, EX_CS2_N_MARK,
-	PWM0_MARK, SCIF4_RXD_C_MARK, TS_SDATA_B_MARK, RIF0_SYNC_MARK,
-	TPUTO3_MARK, SCIFB2_TXD_MARK, SDATA_B_MARK, EX_CS3_N_MARK,
-	SCIFA2_SCK_MARK, SCIF4_TXD_C_MARK, TS_SCK_B_MARK, RIF0_CLK_MARK,
-	BPFCLK_MARK, SCIFB2_SCK_MARK, MDATA_B_MARK, EX_CS4_N_MARK,
-	SCIFA2_RXD_MARK, I2C2_SCL_E_MARK, TS_SDEN_B_MARK, RIF0_D0_MARK,
-	FMCLK_MARK, SCIFB2_CTS_N_MARK, SCKZ_B_MARK, EX_CS5_N_MARK,
-	SCIFA2_TXD_MARK, I2C2_SDA_E_MARK, TS_SPSYNC_B_MARK, RIF0_D1_MARK,
-	FMIN_MARK, SCIFB2_RTS_N_MARK, STM_N_B_MARK, BS_N_MARK, DRACK0_MARK,
-	PWM1_C_MARK, TPUTO0_C_MARK, ATACS01_N_MARK, MTS_N_B_MARK, RD_N_MARK,
-	ATACS11_N_MARK, RD_WR_N_MARK, ATAG1_N_MARK,
+	A21_MARK, MOSI_IO0_MARK,
+	A22_MARK, MISO_IO1_MARK, ATADIR1_N_MARK,
+	A23_MARK, IO2_MARK, ATAWR1_N_MARK,
+	A24_MARK, IO3_MARK, EX_WAIT2_MARK,
+	A25_MARK, SSL_MARK, ATARD1_N_MARK,
+	CS0_N_MARK, VI1_DATA8_MARK,
+	CS1_N_A26_MARK, VI1_DATA9_MARK,
+	EX_CS0_N_MARK, VI1_DATA10_MARK,
+	EX_CS1_N_MARK, TPUTO3_B_MARK, SCIFB2_RXD_MARK, VI1_DATA11_MARK,
+	EX_CS2_N_MARK, PWM0_MARK, SCIF4_RXD_C_MARK, TS_SDATA_B_MARK,
+	TPUTO3_MARK, SCIFB2_TXD_MARK,
+	EX_CS3_N_MARK, SCIFA2_SCK_MARK, SCIF4_TXD_C_MARK, TS_SCK_B_MARK,
+	BPFCLK_MARK, SCIFB2_SCK_MARK,
+	EX_CS4_N_MARK, SCIFA2_RXD_MARK, I2C2_SCL_E_MARK, TS_SDEN_B_MARK,
+	FMCLK_MARK, SCIFB2_CTS_N_MARK,
+	EX_CS5_N_MARK, SCIFA2_TXD_MARK, I2C2_SDA_E_MARK, TS_SPSYNC_B_MARK,
+	FMIN_MARK, SCIFB2_RTS_N_MARK,
+	BS_N_MARK, DRACK0_MARK, PWM1_C_MARK, TPUTO0_C_MARK, ATACS01_N_MARK,
+	RD_N_MARK, ATACS11_N_MARK,
+	RD_WR_N_MARK, ATAG1_N_MARK,
 
 	/* IPSR4 */
-	EX_WAIT0_MARK, CAN_CLK_B_MARK, SCIF_CLK_MARK, PWMFSW0_MARK,
+	EX_WAIT0_MARK, CAN_CLK_B_MARK, SCIF_CLK_MARK,
 	DU0_DR0_MARK, LCDOUT16_MARK, SCIF5_RXD_C_MARK, I2C2_SCL_D_MARK,
-	CC50_STATE0_MARK, DU0_DR1_MARK, LCDOUT17_MARK, SCIF5_TXD_C_MARK,
-	I2C2_SDA_D_MARK, CC50_STATE1_MARK, DU0_DR2_MARK, LCDOUT18_MARK,
-	CC50_STATE2_MARK, DU0_DR3_MARK, LCDOUT19_MARK, CC50_STATE3_MARK,
-	DU0_DR4_MARK, LCDOUT20_MARK, CC50_STATE4_MARK, DU0_DR5_MARK,
-	LCDOUT21_MARK, CC50_STATE5_MARK, DU0_DR6_MARK, LCDOUT22_MARK,
-	CC50_STATE6_MARK, DU0_DR7_MARK, LCDOUT23_MARK, CC50_STATE7_MARK,
+	DU0_DR1_MARK, LCDOUT17_MARK, SCIF5_TXD_C_MARK, I2C2_SDA_D_MARK,
+	DU0_DR2_MARK, LCDOUT18_MARK,
+	DU0_DR3_MARK, LCDOUT19_MARK,
+	DU0_DR4_MARK, LCDOUT20_MARK,
+	DU0_DR5_MARK, LCDOUT21_MARK,
+	DU0_DR6_MARK, LCDOUT22_MARK,
+	DU0_DR7_MARK, LCDOUT23_MARK,
 	DU0_DG0_MARK, LCDOUT8_MARK, SCIFA0_RXD_C_MARK, I2C3_SCL_D_MARK,
-	CC50_STATE8_MARK, DU0_DG1_MARK, LCDOUT9_MARK, SCIFA0_TXD_C_MARK,
-	I2C3_SDA_D_MARK, CC50_STATE9_MARK, DU0_DG2_MARK, LCDOUT10_MARK,
-	CC50_STATE10_MARK, DU0_DG3_MARK, LCDOUT11_MARK, CC50_STATE11_MARK,
-	DU0_DG4_MARK, LCDOUT12_MARK, CC50_STATE12_MARK,
+	DU0_DG1_MARK, LCDOUT9_MARK, SCIFA0_TXD_C_MARK, I2C3_SDA_D_MARK,
+	DU0_DG2_MARK, LCDOUT10_MARK,
+	DU0_DG3_MARK, LCDOUT11_MARK,
+	DU0_DG4_MARK, LCDOUT12_MARK,
 
 	/* IPSR5 */
-	DU0_DG5_MARK, LCDOUT13_MARK, CC50_STATE13_MARK, DU0_DG6_MARK,
-	LCDOUT14_MARK, CC50_STATE14_MARK, DU0_DG7_MARK, LCDOUT15_MARK,
-	CC50_STATE15_MARK, DU0_DB0_MARK, LCDOUT0_MARK, SCIFA4_RXD_C_MARK,
-	I2C4_SCL_D_MARK, CAN0_RX_C_MARK, CC50_STATE16_MARK, DU0_DB1_MARK,
-	LCDOUT1_MARK, SCIFA4_TXD_C_MARK, I2C4_SDA_D_MARK, CAN0_TX_C_MARK,
-	CC50_STATE17_MARK, DU0_DB2_MARK, LCDOUT2_MARK, CC50_STATE18_MARK,
-	DU0_DB3_MARK, LCDOUT3_MARK, CC50_STATE19_MARK, DU0_DB4_MARK,
-	LCDOUT4_MARK, CC50_STATE20_MARK, DU0_DB5_MARK, LCDOUT5_MARK,
-	CC50_STATE21_MARK, DU0_DB6_MARK, LCDOUT6_MARK, CC50_STATE22_MARK,
-	DU0_DB7_MARK, LCDOUT7_MARK, CC50_STATE23_MARK, DU0_DOTCLKIN_MARK,
-	QSTVA_QVS_MARK, CC50_STATE24_MARK, DU0_DOTCLKOUT0_MARK,
-	QCLK_MARK, CC50_STATE25_MARK, DU0_DOTCLKOUT1_MARK, QSTVB_QVE_MARK,
-	CC50_STATE26_MARK, DU0_EXHSYNC_DU0_HSYNC_MARK, QSTH_QHS_MARK,
-	CC50_STATE27_MARK,
+	DU0_DG5_MARK, LCDOUT13_MARK,
+	DU0_DG6_MARK, LCDOUT14_MARK,
+	DU0_DG7_MARK, LCDOUT15_MARK,
+	DU0_DB0_MARK, LCDOUT0_MARK, SCIFA4_RXD_C_MARK, I2C4_SCL_D_MARK,
+	CAN0_RX_C_MARK,
+	DU0_DB1_MARK, LCDOUT1_MARK, SCIFA4_TXD_C_MARK, I2C4_SDA_D_MARK,
+	CAN0_TX_C_MARK,
+	DU0_DB2_MARK, LCDOUT2_MARK,
+	DU0_DB3_MARK, LCDOUT3_MARK,
+	DU0_DB4_MARK, LCDOUT4_MARK,
+	DU0_DB5_MARK, LCDOUT5_MARK,
+	DU0_DB6_MARK, LCDOUT6_MARK,
+	DU0_DB7_MARK, LCDOUT7_MARK,
+	DU0_DOTCLKIN_MARK, QSTVA_QVS_MARK,
+	DU0_DOTCLKOUT0_MARK, QCLK_MARK,
+	DU0_DOTCLKOUT1_MARK, QSTVB_QVE_MARK,
+	DU0_EXHSYNC_DU0_HSYNC_MARK, QSTH_QHS_MARK,
 
 	/* IPSR6 */
-	DU0_EXVSYNC_DU0_VSYNC_MARK, QSTB_QHE_MARK, CC50_STATE28_MARK,
-	DU0_EXODDF_DU0_ODDF_DISP_CDE_MARK, QCPV_QDE_MARK, CC50_STATE29_MARK,
-	DU0_DISP_MARK, QPOLA_MARK, CC50_STATE30_MARK, DU0_CDE_MARK, QPOLB_MARK,
-	CC50_STATE31_MARK, VI0_CLK_MARK, AVB_RX_CLK_MARK, VI0_DATA0_VI0_B0_MARK,
-	AVB_RX_DV_MARK, VI0_DATA1_VI0_B1_MARK, AVB_RXD0_MARK,
-	VI0_DATA2_VI0_B2_MARK, AVB_RXD1_MARK, VI0_DATA3_VI0_B3_MARK,
-	AVB_RXD2_MARK, VI0_DATA4_VI0_B4_MARK, AVB_RXD3_MARK,
-	VI0_DATA5_VI0_B5_MARK, AVB_RXD4_MARK, VI0_DATA6_VI0_B6_MARK,
-	AVB_RXD5_MARK, VI0_DATA7_VI0_B7_MARK, AVB_RXD6_MARK, VI0_CLKENB_MARK,
-	I2C3_SCL_MARK, SCIFA5_RXD_C_MARK, IETX_C_MARK, AVB_RXD7_MARK,
+	DU0_EXVSYNC_DU0_VSYNC_MARK, QSTB_QHE_MARK,
+	DU0_EXODDF_DU0_ODDF_DISP_CDE_MARK, QCPV_QDE_MARK,
+	DU0_DISP_MARK, QPOLA_MARK, DU0_CDE_MARK, QPOLB_MARK,
+	VI0_CLK_MARK, AVB_RX_CLK_MARK, VI0_DATA0_VI0_B0_MARK, AVB_RX_DV_MARK,
+	VI0_DATA1_VI0_B1_MARK, AVB_RXD0_MARK,
+	VI0_DATA2_VI0_B2_MARK, AVB_RXD1_MARK,
+	VI0_DATA3_VI0_B3_MARK, AVB_RXD2_MARK,
+	VI0_DATA4_VI0_B4_MARK, AVB_RXD3_MARK,
+	VI0_DATA5_VI0_B5_MARK, AVB_RXD4_MARK,
+	VI0_DATA6_VI0_B6_MARK, AVB_RXD5_MARK,
+	VI0_DATA7_VI0_B7_MARK, AVB_RXD6_MARK,
+	VI0_CLKENB_MARK, I2C3_SCL_MARK, SCIFA5_RXD_C_MARK, IETX_C_MARK,
+	AVB_RXD7_MARK,
 	VI0_FIELD_MARK, I2C3_SDA_MARK, SCIFA5_TXD_C_MARK, IECLK_C_MARK,
-	AVB_RX_ER_MARK, VI0_HSYNC_N_MARK, SCIF0_RXD_B_MARK, I2C0_SCL_C_MARK,
-	IERX_C_MARK, AVB_COL_MARK, VI0_VSYNC_N_MARK, SCIF0_TXD_B_MARK,
-	I2C0_SDA_C_MARK, AUDIO_CLKOUT_B_MARK, AVB_TX_EN_MARK, ETH_MDIO_MARK,
-	VI0_G0_MARK, MSIOF2_RXD_B_MARK, IIC0_SCL_D_MARK, AVB_TX_CLK_MARK,
-	ADIDATA_MARK, AD_DI_MARK,
+	AVB_RX_ER_MARK,
+	VI0_HSYNC_N_MARK, SCIF0_RXD_B_MARK, I2C0_SCL_C_MARK, IERX_C_MARK,
+	AVB_COL_MARK,
+	VI0_VSYNC_N_MARK, SCIF0_TXD_B_MARK, I2C0_SDA_C_MARK,
+	AUDIO_CLKOUT_B_MARK, AVB_TX_EN_MARK,
+	ETH_MDIO_MARK, VI0_G0_MARK, MSIOF2_RXD_B_MARK, I2C5_SCL_D_MARK,
+	AVB_TX_CLK_MARK, ADIDATA_MARK,
 
 	/* IPSR7 */
-	ETH_CRS_DV_MARK, VI0_G1_MARK, MSIOF2_TXD_B_MARK, IIC0_SDA_D_MARK,
-	AVB_TXD0_MARK, ADICS_SAMP_MARK, AD_DO_MARK, ETH_RX_ER_MARK, VI0_G2_MARK,
-	MSIOF2_SCK_B_MARK, CAN0_RX_B_MARK, AVB_TXD1_MARK, ADICLK_MARK,
-	AD_CLK_MARK, ETH_RXD0_MARK, VI0_G3_MARK, MSIOF2_SYNC_B_MARK,
-	CAN0_TX_B_MARK, AVB_TXD2_MARK, ADICHS0_MARK, AD_NCS_N_MARK,
+	ETH_CRS_DV_MARK, VI0_G1_MARK, MSIOF2_TXD_B_MARK, I2C5_SDA_D_MARK,
+	AVB_TXD0_MARK, ADICS_SAMP_MARK,
+	ETH_RX_ER_MARK, VI0_G2_MARK, MSIOF2_SCK_B_MARK, CAN0_RX_B_MARK,
+	AVB_TXD1_MARK, ADICLK_MARK,
+	ETH_RXD0_MARK, VI0_G3_MARK, MSIOF2_SYNC_B_MARK, CAN0_TX_B_MARK,
+	AVB_TXD2_MARK, ADICHS0_MARK,
 	ETH_RXD1_MARK, VI0_G4_MARK, MSIOF2_SS1_B_MARK, SCIF4_RXD_D_MARK,
-	AVB_TXD3_MARK, ADICHS1_MARK, ETH_LINK_MARK, VI0_G5_MARK,
-	MSIOF2_SS2_B_MARK, SCIF4_TXD_D_MARK, AVB_TXD4_MARK, ADICHS2_MARK,
+	AVB_TXD3_MARK, ADICHS1_MARK,
+	ETH_LINK_MARK, VI0_G5_MARK, MSIOF2_SS2_B_MARK, SCIF4_TXD_D_MARK,
+	AVB_TXD4_MARK, ADICHS2_MARK,
 	ETH_REFCLK_MARK, VI0_G6_MARK, SCIF2_SCK_C_MARK, AVB_TXD5_MARK,
-	SSI_SCK5_B_MARK, ETH_TXD1_MARK, VI0_G7_MARK, SCIF2_RXD_C_MARK,
-	IIC1_SCL_D_MARK, AVB_TXD6_MARK, SSI_WS5_B_MARK, ETH_TX_EN_MARK,
-	VI0_R0_MARK, SCIF2_TXD_C_MARK, IIC1_SDA_D_MARK, AVB_TXD7_MARK,
-	SSI_SDATA5_B_MARK, ETH_MAGIC_MARK, VI0_R1_MARK, SCIF3_SCK_B_MARK,
-	AVB_TX_ER_MARK, SSI_SCK6_B_MARK, ETH_TXD0_MARK, VI0_R2_MARK,
-	SCIF3_RXD_B_MARK, I2C4_SCL_E_MARK, AVB_GTX_CLK_MARK, SSI_WS6_B_MARK,
+	SSI_SCK5_B_MARK,
+	ETH_TXD1_MARK, VI0_G7_MARK, SCIF2_RXD_C_MARK, IIC0_SCL_D_MARK,
+	AVB_TXD6_MARK, SSI_WS5_B_MARK,
+	ETH_TX_EN_MARK, VI0_R0_MARK, SCIF2_TXD_C_MARK, IIC0_SDA_D_MARK,
+	AVB_TXD7_MARK, SSI_SDATA5_B_MARK,
+	ETH_MAGIC_MARK, VI0_R1_MARK, SCIF3_SCK_B_MARK, AVB_TX_ER_MARK,
+	SSI_SCK6_B_MARK,
+	ETH_TXD0_MARK, VI0_R2_MARK, SCIF3_RXD_B_MARK, I2C4_SCL_E_MARK,
+	AVB_GTX_CLK_MARK, SSI_WS6_B_MARK,
 	DREQ0_N_MARK, SCIFB1_RXD_MARK,
 
 	/* IPSR8 */
@@ -498,103 +566,107 @@ enum {
 	I2C0_SCL_MARK, SCIF0_RXD_C_MARK, PWM5_MARK, TCLK1_B_MARK,
 	AVB_GTXREFCLK_MARK, CAN1_RX_D_MARK, TPUTO0_B_MARK, I2C0_SDA_MARK,
 	SCIF0_TXD_C_MARK, TPUTO0_MARK, CAN_CLK_MARK, DVC_MUTE_MARK,
-	CAN1_TX_D_MARK, I2C1_SCL_MARK, SCIF4_RXD_MARK, PWM5_B_MARK,
-	DU1_DR0_MARK, RIF1_SYNC_B_MARK, TS_SDATA_D_MARK, TPUTO1_B_MARK,
-	I2C1_SDA_MARK, SCIF4_TXD_MARK, IRQ5_MARK, DU1_DR1_MARK, RIF1_CLK_B_MARK,
-	TS_SCK_D_MARK, BPFCLK_C_MARK, MSIOF0_RXD_MARK, SCIF5_RXD_MARK,
-	I2C2_SCL_C_MARK, DU1_DR2_MARK, RIF1_D0_B_MARK, TS_SDEN_D_MARK,
-	FMCLK_C_MARK, RDS_CLK_MARK,
+	CAN1_TX_D_MARK,
+	I2C1_SCL_MARK, SCIF4_RXD_MARK, PWM5_B_MARK, DU1_DR0_MARK,
+	TS_SDATA_D_MARK, TPUTO1_B_MARK,
+	I2C1_SDA_MARK, SCIF4_TXD_MARK, IRQ5_MARK, DU1_DR1_MARK,	TS_SCK_D_MARK,
+	BPFCLK_C_MARK,
+	MSIOF0_RXD_MARK, SCIF5_RXD_MARK, I2C2_SCL_C_MARK, DU1_DR2_MARK,
+	TS_SDEN_D_MARK, FMCLK_C_MARK,
 
 	/* IPSR9 */
 	MSIOF0_TXD_MARK, SCIF5_TXD_MARK, I2C2_SDA_C_MARK, DU1_DR3_MARK,
-	RIF1_D1_B_MARK, TS_SPSYNC_D_MARK, FMIN_C_MARK, RDS_DATA_MARK,
-	MSIOF0_SCK_MARK, IRQ0_MARK, TS_SDATA_MARK, DU1_DR4_MARK, RIF1_SYNC_MARK,
-	TPUTO1_C_MARK, MSIOF0_SYNC_MARK, PWM1_MARK, TS_SCK_MARK, DU1_DR5_MARK,
-	RIF1_CLK_MARK, BPFCLK_B_MARK, MSIOF0_SS1_MARK, SCIFA0_RXD_MARK,
-	TS_SDEN_MARK, DU1_DR6_MARK, RIF1_D0_MARK, FMCLK_B_MARK, RDS_CLK_B_MARK,
+	TS_SPSYNC_D_MARK, FMIN_C_MARK,
+	MSIOF0_SCK_MARK, IRQ0_MARK, TS_SDATA_MARK, DU1_DR4_MARK, TPUTO1_C_MARK,
+	MSIOF0_SYNC_MARK, PWM1_MARK, TS_SCK_MARK, DU1_DR5_MARK, BPFCLK_B_MARK,
+	MSIOF0_SS1_MARK, SCIFA0_RXD_MARK, TS_SDEN_MARK, DU1_DR6_MARK,
+	FMCLK_B_MARK,
 	MSIOF0_SS2_MARK, SCIFA0_TXD_MARK, TS_SPSYNC_MARK, DU1_DR7_MARK,
-	RIF1_D1_MARK, FMIN_B_MARK, RDS_DATA_B_MARK, HSCIF1_HRX_MARK,
-	I2C4_SCL_MARK, PWM6_MARK, DU1_DG0_MARK, HSCIF1_HTX_MARK,
-	I2C4_SDA_MARK, TPUTO1_MARK, DU1_DG1_MARK, HSCIF1_HSCK_MARK,
-	PWM2_MARK, IETX_MARK, DU1_DG2_MARK, REMOCON_B_MARK, SPEEDIN_B_MARK,
-	VSP_B_MARK, HSCIF1_HCTS_N_MARK, SCIFA4_RXD_MARK, IECLK_MARK,
-	DU1_DG3_MARK, SSI_SCK1_B_MARK, CAN_DEBUG_HW_TRIGGER_MARK,
-	CC50_STATE32_MARK, HSCIF1_HRTS_N_MARK, SCIFA4_TXD_MARK, IERX_MARK,
-	DU1_DG4_MARK, SSI_WS1_B_MARK, CAN_STEP0_MARK, CC50_STATE33_MARK,
+	FMIN_B_MARK,
+	HSCIF1_HRX_MARK, I2C4_SCL_MARK, PWM6_MARK, DU1_DG0_MARK,
+	HSCIF1_HTX_MARK, I2C4_SDA_MARK, TPUTO1_MARK, DU1_DG1_MARK,
+	HSCIF1_HSCK_MARK, PWM2_MARK, IETX_MARK, DU1_DG2_MARK, REMOCON_B_MARK,
+	SPEEDIN_B_MARK,
+	HSCIF1_HCTS_N_MARK, SCIFA4_RXD_MARK, IECLK_MARK, DU1_DG3_MARK,
+	SSI_SCK1_B_MARK,
+	HSCIF1_HRTS_N_MARK, SCIFA4_TXD_MARK, IERX_MARK, DU1_DG4_MARK,
+	SSI_WS1_B_MARK,
 	SCIF1_SCK_MARK, PWM3_MARK, TCLK2_MARK, DU1_DG5_MARK, SSI_SDATA1_B_MARK,
-	CAN_TXCLK_MARK, CC50_STATE34_MARK,
+	CAN_TXCLK_MARK,
 
 	/* IPSR10 */
-	SCIF1_RXD_MARK, IIC0_SCL_MARK, DU1_DG6_MARK, SSI_SCK2_B_MARK,
-	CAN_DEBUGOUT0_MARK, CC50_STATE35_MARK, SCIF1_TXD_MARK, IIC0_SDA_MARK,
-	DU1_DG7_MARK, SSI_WS2_B_MARK, CAN_DEBUGOUT1_MARK, CC50_STATE36_MARK,
-	SCIF2_RXD_MARK, IIC1_SCL_MARK, DU1_DB0_MARK, SSI_SDATA2_B_MARK,
-	USB0_EXTLP_MARK, CAN_DEBUGOUT2_MARK, CC50_STATE37_MARK, SCIF2_TXD_MARK,
-	IIC1_SDA_MARK, DU1_DB1_MARK, SSI_SCK9_B_MARK, USB0_OVC1_MARK,
-	CAN_DEBUGOUT3_MARK, CC50_STATE38_MARK, SCIF2_SCK_MARK, IRQ1_MARK,
-	DU1_DB2_MARK, SSI_WS9_B_MARK, USB0_IDIN_MARK, CAN_DEBUGOUT4_MARK,
-	CC50_STATE39_MARK, SCIF3_SCK_MARK, IRQ2_MARK, BPFCLK_D_MARK,
-	DU1_DB3_MARK, SSI_SDATA9_B_MARK, TANS2_MARK, CAN_DEBUGOUT5_MARK,
-	CC50_OSCOUT_MARK, SCIF3_RXD_MARK, I2C1_SCL_E_MARK, FMCLK_D_MARK,
-	DU1_DB4_MARK, AUDIO_CLKA_C_MARK, SSI_SCK4_B_MARK, CAN_DEBUGOUT6_MARK,
-	RDS_CLK_C_MARK, SCIF3_TXD_MARK, I2C1_SDA_E_MARK, FMIN_D_MARK,
-	DU1_DB5_MARK, AUDIO_CLKB_C_MARK, SSI_WS4_B_MARK, CAN_DEBUGOUT7_MARK,
-	RDS_DATA_C_MARK, I2C2_SCL_MARK, SCIFA5_RXD_MARK, DU1_DB6_MARK,
-	AUDIO_CLKC_C_MARK, SSI_SDATA4_B_MARK, CAN_DEBUGOUT8_MARK, I2C2_SDA_MARK,
-	SCIFA5_TXD_MARK, DU1_DB7_MARK, AUDIO_CLKOUT_C_MARK, CAN_DEBUGOUT9_MARK,
-	SSI_SCK5_MARK, SCIFA3_SCK_MARK, DU1_DOTCLKIN_MARK, CAN_DEBUGOUT10_MARK,
+	SCIF1_RXD_MARK, I2C5_SCL_MARK, DU1_DG6_MARK, SSI_SCK2_B_MARK,
+	SCIF1_TXD_MARK, I2C5_SDA_MARK, DU1_DG7_MARK, SSI_WS2_B_MARK,
+	SCIF2_RXD_MARK, IIC0_SCL_MARK, DU1_DB0_MARK, SSI_SDATA2_B_MARK,
+	SCIF2_TXD_MARK, IIC0_SDA_MARK, DU1_DB1_MARK, SSI_SCK9_B_MARK,
+	SCIF2_SCK_MARK, IRQ1_MARK, DU1_DB2_MARK, SSI_WS9_B_MARK,
+	SCIF3_SCK_MARK, IRQ2_MARK, BPFCLK_D_MARK, DU1_DB3_MARK,
+	SSI_SDATA9_B_MARK,
+	SCIF3_RXD_MARK, I2C1_SCL_E_MARK, FMCLK_D_MARK, DU1_DB4_MARK,
+	AUDIO_CLKA_C_MARK, SSI_SCK4_B_MARK,
+	SCIF3_TXD_MARK, I2C1_SDA_E_MARK, FMIN_D_MARK, DU1_DB5_MARK,
+	AUDIO_CLKB_C_MARK, SSI_WS4_B_MARK,
+	I2C2_SCL_MARK, SCIFA5_RXD_MARK, DU1_DB6_MARK, AUDIO_CLKC_C_MARK,
+	SSI_SDATA4_B_MARK,
+	I2C2_SDA_MARK, SCIFA5_TXD_MARK, DU1_DB7_MARK, AUDIO_CLKOUT_C_MARK,
+	SSI_SCK5_MARK, SCIFA3_SCK_MARK, DU1_DOTCLKIN_MARK,
 
 	/* IPSR11 */
 	SSI_WS5_MARK, SCIFA3_RXD_MARK, I2C3_SCL_C_MARK, DU1_DOTCLKOUT0_MARK,
-	CAN_DEBUGOUT11_MARK, SSI_SDATA5_MARK, SCIFA3_TXD_MARK, I2C3_SDA_C_MARK,
-	DU1_DOTCLKOUT1_MARK, CAN_DEBUGOUT12_MARK, SSI_SCK6_MARK,
-	SCIFA1_SCK_B_MARK, DU1_EXHSYNC_DU1_HSYNC_MARK, CAN_DEBUGOUT13_MARK,
+	SSI_SDATA5_MARK, SCIFA3_TXD_MARK, I2C3_SDA_C_MARK, DU1_DOTCLKOUT1_MARK,
+	SSI_SCK6_MARK, SCIFA1_SCK_B_MARK, DU1_EXHSYNC_DU1_HSYNC_MARK,
 	SSI_WS6_MARK, SCIFA1_RXD_B_MARK, I2C4_SCL_C_MARK,
-	DU1_EXVSYNC_DU1_VSYNC_MARK, CAN_DEBUGOUT14_MARK, SSI_SDATA6_MARK,
-	SCIFA1_TXD_B_MARK, I2C4_SDA_C_MARK, DU1_EXODDF_DU1_ODDF_DISP_CDE_MARK,
-	CAN_DEBUGOUT15_MARK, SSI_SCK78_MARK, SCIFA2_SCK_B_MARK, IIC0_SDA_C_MARK,
-	DU1_DISP_MARK, SSI_WS78_MARK, SCIFA2_RXD_B_MARK, IIC0_SCL_C_MARK,
-	DU1_CDE_MARK, SSI_SDATA7_MARK, SCIFA2_TXD_B_MARK, IRQ8_MARK,
-	AUDIO_CLKA_D_MARK, CAN_CLK_D_MARK, PCMOE_N_MARK, SSI_SCK0129_MARK,
-	MSIOF1_RXD_B_MARK, SCIF5_RXD_D_MARK, ADIDATA_B_MARK, AD_DI_B_MARK,
-	PCMWE_N_MARK, SSI_WS0129_MARK, MSIOF1_TXD_B_MARK, SCIF5_TXD_D_MARK,
-	ADICS_SAMP_B_MARK, AD_DO_B_MARK, SSI_SDATA0_MARK, MSIOF1_SCK_B_MARK,
-	PWM0_B_MARK, ADICLK_B_MARK, AD_CLK_B_MARK,
+	DU1_EXVSYNC_DU1_VSYNC_MARK,
+	SSI_SDATA6_MARK, SCIFA1_TXD_B_MARK, I2C4_SDA_C_MARK,
+	DU1_EXODDF_DU1_ODDF_DISP_CDE_MARK,
+	SSI_SCK78_MARK, SCIFA2_SCK_B_MARK, I2C5_SDA_C_MARK, DU1_DISP_MARK,
+	SSI_WS78_MARK, SCIFA2_RXD_B_MARK, I2C5_SCL_C_MARK, DU1_CDE_MARK,
+	SSI_SDATA7_MARK, SCIFA2_TXD_B_MARK, IRQ8_MARK, AUDIO_CLKA_D_MARK,
+	CAN_CLK_D_MARK,
+	SSI_SCK0129_MARK, MSIOF1_RXD_B_MARK, SCIF5_RXD_D_MARK, ADIDATA_B_MARK,
+	SSI_WS0129_MARK, MSIOF1_TXD_B_MARK, SCIF5_TXD_D_MARK, ADICS_SAMP_B_MARK,
+	SSI_SDATA0_MARK, MSIOF1_SCK_B_MARK, PWM0_B_MARK, ADICLK_B_MARK,
 
 	/* IPSR12 */
 	SSI_SCK34_MARK, MSIOF1_SYNC_B_MARK, SCIFA1_SCK_C_MARK, ADICHS0_B_MARK,
-	AD_NCS_N_B_MARK, DREQ1_N_B_MARK, SSI_WS34_MARK, MSIOF1_SS1_B_MARK,
-	SCIFA1_RXD_C_MARK, ADICHS1_B_MARK, CAN1_RX_C_MARK, DACK1_B_MARK,
+	DREQ1_N_B_MARK,
+	SSI_WS34_MARK, MSIOF1_SS1_B_MARK, SCIFA1_RXD_C_MARK, ADICHS1_B_MARK,
+	CAN1_RX_C_MARK, DACK1_B_MARK,
 	SSI_SDATA3_MARK, MSIOF1_SS2_B_MARK, SCIFA1_TXD_C_MARK, ADICHS2_B_MARK,
-	CAN1_TX_C_MARK, DREQ2_N_MARK, SSI_SCK4_MARK, MLB_CLK_MARK, IETX_B_MARK,
-	IRD_TX_MARK, SSI_WS4_MARK, MLB_SIG_MARK, IECLK_B_MARK, IRD_RX_MARK,
-	SSI_SDATA4_MARK, MLB_DAT_MARK, IERX_B_MARK, IRD_SCK_MARK,
+	CAN1_TX_C_MARK, DREQ2_N_MARK,
+	SSI_SCK4_MARK, MLB_CLK_MARK, IETX_B_MARK,
+	SSI_WS4_MARK, MLB_SIG_MARK, IECLK_B_MARK,
+	SSI_SDATA4_MARK, MLB_DAT_MARK, IERX_B_MARK,
 	SSI_SDATA8_MARK, SCIF1_SCK_B_MARK, PWM1_B_MARK, IRQ9_MARK, REMOCON_MARK,
-	DACK2_MARK, ETH_MDIO_B_MARK, SSI_SCK1_MARK, SCIF1_RXD_B_MARK,
-	IIC1_SCL_C_MARK, VI1_CLK_MARK, CAN0_RX_D_MARK, AVB_AVTP_CAPTURE_MARK,
-	ETH_CRS_DV_B_MARK, SSI_WS1_MARK, SCIF1_TXD_B_MARK, IIC1_SDA_C_MARK,
-	VI1_DATA0_MARK, CAN0_TX_D_MARK, AVB_AVTP_MATCH_MARK, ETH_RX_ER_B_MARK,
-	SSI_SDATA1_MARK, HSCIF1_HRX_B_MARK, VI1_DATA1_MARK, SDATA_MARK,
-	ATAWR0_N_MARK, ETH_RXD0_B_MARK, SSI_SCK2_MARK, HSCIF1_HTX_B_MARK,
-	VI1_DATA2_MARK, MDATA_MARK, ATAG0_N_MARK, ETH_RXD1_B_MARK,
+	DACK2_MARK, ETH_MDIO_B_MARK,
+	SSI_SCK1_MARK, SCIF1_RXD_B_MARK, IIC0_SCL_C_MARK, VI1_CLK_MARK,
+	CAN0_RX_D_MARK, ETH_CRS_DV_B_MARK,
+	SSI_WS1_MARK, SCIF1_TXD_B_MARK, IIC0_SDA_C_MARK, VI1_DATA0_MARK,
+	CAN0_TX_D_MARK, ETH_RX_ER_B_MARK,
+	SSI_SDATA1_MARK, HSCIF1_HRX_B_MARK, VI1_DATA1_MARK, ATAWR0_N_MARK,
+	ETH_RXD0_B_MARK,
+	SSI_SCK2_MARK, HSCIF1_HTX_B_MARK, VI1_DATA2_MARK, ATAG0_N_MARK,
+	ETH_RXD1_B_MARK,
 
 	/* IPSR13 */
 	SSI_WS2_MARK, HSCIF1_HCTS_N_B_MARK, SCIFA0_RXD_D_MARK, VI1_DATA3_MARK,
-	SCKZ_MARK, ATACS00_N_MARK, ETH_LINK_B_MARK, SSI_SDATA2_MARK,
-	HSCIF1_HRTS_N_B_MARK, SCIFA0_TXD_D_MARK, VI1_DATA4_MARK, STM_N_MARK,
-	ATACS10_N_MARK, ETH_REFCLK_B_MARK, SSI_SCK9_MARK, SCIF2_SCK_B_MARK,
-	PWM2_B_MARK, VI1_DATA5_MARK, MTS_N_MARK, EX_WAIT1_MARK,
-	ETH_TXD1_B_MARK, SSI_WS9_MARK, SCIF2_RXD_B_MARK, I2C3_SCL_E_MARK,
-	VI1_DATA6_MARK, ATARD0_N_MARK, ETH_TX_EN_B_MARK, SSI_SDATA9_MARK,
-	SCIF2_TXD_B_MARK, I2C3_SDA_E_MARK, VI1_DATA7_MARK, ATADIR0_N_MARK,
-	ETH_MAGIC_B_MARK, AUDIO_CLKA_MARK, I2C0_SCL_B_MARK, SCIFA4_RXD_D_MARK,
-	VI1_CLKENB_MARK, TS_SDATA_C_MARK, RIF0_SYNC_B_MARK, ETH_TXD0_B_MARK,
+	ATACS00_N_MARK, ETH_LINK_B_MARK,
+	SSI_SDATA2_MARK, HSCIF1_HRTS_N_B_MARK, SCIFA0_TXD_D_MARK,
+	VI1_DATA4_MARK, ATACS10_N_MARK, ETH_REFCLK_B_MARK,
+	SSI_SCK9_MARK, SCIF2_SCK_B_MARK, PWM2_B_MARK, VI1_DATA5_MARK,
+	EX_WAIT1_MARK, ETH_TXD1_B_MARK,
+	SSI_WS9_MARK, SCIF2_RXD_B_MARK, I2C3_SCL_E_MARK, VI1_DATA6_MARK,
+	ATARD0_N_MARK, ETH_TX_EN_B_MARK,
+	SSI_SDATA9_MARK, SCIF2_TXD_B_MARK, I2C3_SDA_E_MARK, VI1_DATA7_MARK,
+	ATADIR0_N_MARK, ETH_MAGIC_B_MARK,
+	AUDIO_CLKA_MARK, I2C0_SCL_B_MARK, SCIFA4_RXD_D_MARK, VI1_CLKENB_MARK,
+	TS_SDATA_C_MARK, ETH_TXD0_B_MARK,
 	AUDIO_CLKB_MARK, I2C0_SDA_B_MARK, SCIFA4_TXD_D_MARK, VI1_FIELD_MARK,
-	TS_SCK_C_MARK, RIF0_CLK_B_MARK, BPFCLK_E_MARK, ETH_MDC_B_MARK,
+	TS_SCK_C_MARK, BPFCLK_E_MARK, ETH_MDC_B_MARK,
 	AUDIO_CLKC_MARK, I2C4_SCL_B_MARK, SCIFA5_RXD_D_MARK, VI1_HSYNC_N_MARK,
-	TS_SDEN_C_MARK, RIF0_D0_B_MARK, FMCLK_E_MARK, RDS_CLK_D_MARK,
+	TS_SDEN_C_MARK, FMCLK_E_MARK,
 	AUDIO_CLKOUT_MARK, I2C4_SDA_B_MARK, SCIFA5_TXD_D_MARK, VI1_VSYNC_N_MARK,
-	TS_SPSYNC_C_MARK, RIF0_D1_B_MARK, FMIN_E_MARK, RDS_DATA_D_MARK,
+	TS_SPSYNC_C_MARK, FMIN_E_MARK,
 	PINMUX_MARK_END,
 };
 
@@ -700,15 +772,14 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP1_14_13, I2C1_SDA_D, SEL_I2C01_3),
 	PINMUX_IPSR_GPSR(IP1_17_15, D13),
 	PINMUX_IPSR_MSEL(IP1_17_15, SCIFA1_SCK, SEL_SCIFA1_0),
-	PINMUX_IPSR_GPSR(IP1_17_15, TANS1),
 	PINMUX_IPSR_GPSR(IP1_17_15, PWM2_C),
 	PINMUX_IPSR_MSEL(IP1_17_15, TCLK2_B, SEL_TMU_1),
 	PINMUX_IPSR_GPSR(IP1_19_18, D14),
 	PINMUX_IPSR_MSEL(IP1_19_18, SCIFA1_RXD, SEL_SCIFA1_0),
-	PINMUX_IPSR_MSEL(IP1_19_18, IIC0_SCL_B, SEL_IIC00_1),
+	PINMUX_IPSR_MSEL(IP1_19_18, I2C5_SCL_B, SEL_I2C05_1),
 	PINMUX_IPSR_GPSR(IP1_21_20, D15),
 	PINMUX_IPSR_MSEL(IP1_21_20, SCIFA1_TXD, SEL_SCIFA1_0),
-	PINMUX_IPSR_MSEL(IP1_21_20, IIC0_SDA_B, SEL_IIC00_1),
+	PINMUX_IPSR_MSEL(IP1_21_20, I2C5_SDA_B, SEL_I2C05_1),
 	PINMUX_IPSR_GPSR(IP1_23_22, A0),
 	PINMUX_IPSR_GPSR(IP1_23_22, SCIFB1_SCK),
 	PINMUX_IPSR_GPSR(IP1_23_22, PWM3_B),
@@ -739,10 +810,10 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP2_5_4, SCIFA0_TXD_B, SEL_SCIFA0_1),
 	PINMUX_IPSR_GPSR(IP2_7_6, A10),
 	PINMUX_IPSR_MSEL(IP2_7_6, MSIOF1_SCK, SEL_MSI1_0),
-	PINMUX_IPSR_MSEL(IP2_7_6, IIC1_SCL_B, SEL_IIC01_1),
+	PINMUX_IPSR_MSEL(IP2_7_6, IIC0_SCL_B, SEL_IIC0_1),
 	PINMUX_IPSR_GPSR(IP2_9_8, A11),
 	PINMUX_IPSR_MSEL(IP2_9_8, MSIOF1_SYNC, SEL_MSI1_0),
-	PINMUX_IPSR_MSEL(IP2_9_8, IIC1_SDA_B, SEL_IIC01_1),
+	PINMUX_IPSR_MSEL(IP2_9_8, IIC0_SDA_B, SEL_IIC0_1),
 	PINMUX_IPSR_GPSR(IP2_11_10, A12),
 	PINMUX_IPSR_MSEL(IP2_11_10, MSIOF1_SS1, SEL_MSI1_0),
 	PINMUX_IPSR_MSEL(IP2_11_10, SCIFA5_RXD_B, SEL_SCIFA5_1),
@@ -761,39 +832,31 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP2_20_18, MSIOF2_SCK, SEL_MSI2_0),
 	PINMUX_IPSR_MSEL(IP2_20_18, HSCIF0_HSCK_B, SEL_HSCIF0_1),
 	PINMUX_IPSR_MSEL(IP2_20_18, SPEEDIN, SEL_RSP_0),
-	PINMUX_IPSR_MSEL(IP2_20_18, VSP, SEL_SPDM_0),
 	PINMUX_IPSR_MSEL(IP2_20_18, CAN_CLK_C, SEL_CAN_2),
 	PINMUX_IPSR_GPSR(IP2_20_18, TPUTO2_B),
 	PINMUX_IPSR_GPSR(IP2_23_21, A17),
 	PINMUX_IPSR_MSEL(IP2_23_21, MSIOF2_SYNC, SEL_MSI2_0),
 	PINMUX_IPSR_MSEL(IP2_23_21, SCIF4_RXD_E, SEL_SCIF4_4),
 	PINMUX_IPSR_MSEL(IP2_23_21, CAN1_RX_B, SEL_CAN1_1),
-	PINMUX_IPSR_MSEL(IP2_23_21, AVB_AVTP_CAPTURE_B, SEL_AVB_1),
 	PINMUX_IPSR_GPSR(IP2_26_24, A18),
 	PINMUX_IPSR_MSEL(IP2_26_24, MSIOF2_SS1, SEL_MSI2_0),
 	PINMUX_IPSR_MSEL(IP2_26_24, SCIF4_TXD_E, SEL_SCIF4_4),
 	PINMUX_IPSR_MSEL(IP2_26_24, CAN1_TX_B, SEL_CAN1_1),
-	PINMUX_IPSR_MSEL(IP2_26_24, AVB_AVTP_MATCH_B, SEL_AVB_1),
 	PINMUX_IPSR_GPSR(IP2_29_27, A19),
 	PINMUX_IPSR_MSEL(IP2_29_27, MSIOF2_SS2, SEL_MSI2_0),
 	PINMUX_IPSR_GPSR(IP2_29_27, PWM4),
 	PINMUX_IPSR_GPSR(IP2_29_27, TPUTO2),
-	PINMUX_IPSR_GPSR(IP2_29_27, MOUT0),
 	PINMUX_IPSR_GPSR(IP2_31_30, A20),
 	PINMUX_IPSR_GPSR(IP2_31_30, SPCLK),
-	PINMUX_IPSR_GPSR(IP2_29_27, MOUT1),
 
 	/* IPSR3 */
 	PINMUX_IPSR_GPSR(IP3_1_0, A21),
 	PINMUX_IPSR_GPSR(IP3_1_0, MOSI_IO0),
-	PINMUX_IPSR_GPSR(IP3_1_0, MOUT2),
 	PINMUX_IPSR_GPSR(IP3_3_2, A22),
 	PINMUX_IPSR_GPSR(IP3_3_2, MISO_IO1),
-	PINMUX_IPSR_GPSR(IP3_3_2, MOUT5),
 	PINMUX_IPSR_GPSR(IP3_3_2, ATADIR1_N),
 	PINMUX_IPSR_GPSR(IP3_5_4, A23),
 	PINMUX_IPSR_GPSR(IP3_5_4, IO2),
-	PINMUX_IPSR_GPSR(IP3_5_4, MOUT6),
 	PINMUX_IPSR_GPSR(IP3_5_4, ATAWR1_N),
 	PINMUX_IPSR_GPSR(IP3_7_6, A24),
 	PINMUX_IPSR_GPSR(IP3_7_6, IO3),
@@ -815,40 +878,31 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_GPSR(IP3_17_15, PWM0),
 	PINMUX_IPSR_MSEL(IP3_17_15, SCIF4_RXD_C, SEL_SCIF4_2),
 	PINMUX_IPSR_MSEL(IP3_17_15, TS_SDATA_B, SEL_TSIF0_1),
-	PINMUX_IPSR_MSEL(IP3_17_15, RIF0_SYNC, SEL_DR0_0),
 	PINMUX_IPSR_GPSR(IP3_17_15, TPUTO3),
 	PINMUX_IPSR_GPSR(IP3_17_15, SCIFB2_TXD),
-	PINMUX_IPSR_MSEL(IP3_17_15, SDATA_B, SEL_FSN_1),
 	PINMUX_IPSR_GPSR(IP3_20_18, EX_CS3_N),
 	PINMUX_IPSR_MSEL(IP3_20_18, SCIFA2_SCK, SEL_SCIFA2_0),
 	PINMUX_IPSR_MSEL(IP3_20_18, SCIF4_TXD_C, SEL_SCIF4_2),
 	PINMUX_IPSR_MSEL(IP3_20_18, TS_SCK_B, SEL_TSIF0_1),
-	PINMUX_IPSR_MSEL(IP3_20_18, RIF0_CLK, SEL_DR0_0),
 	PINMUX_IPSR_MSEL(IP3_20_18, BPFCLK, SEL_DARC_0),
 	PINMUX_IPSR_GPSR(IP3_20_18, SCIFB2_SCK),
-	PINMUX_IPSR_MSEL(IP3_20_18, MDATA_B, SEL_FSN_1),
 	PINMUX_IPSR_GPSR(IP3_23_21, EX_CS4_N),
 	PINMUX_IPSR_MSEL(IP3_23_21, SCIFA2_RXD, SEL_SCIFA2_0),
 	PINMUX_IPSR_MSEL(IP3_23_21, I2C2_SCL_E, SEL_I2C02_4),
 	PINMUX_IPSR_MSEL(IP3_23_21, TS_SDEN_B, SEL_TSIF0_1),
-	PINMUX_IPSR_MSEL(IP3_23_21, RIF0_D0, SEL_DR0_0),
 	PINMUX_IPSR_MSEL(IP3_23_21, FMCLK, SEL_DARC_0),
 	PINMUX_IPSR_GPSR(IP3_23_21, SCIFB2_CTS_N),
-	PINMUX_IPSR_MSEL(IP3_23_21, SCKZ_B, SEL_FSN_1),
 	PINMUX_IPSR_GPSR(IP3_26_24, EX_CS5_N),
 	PINMUX_IPSR_MSEL(IP3_26_24, SCIFA2_TXD, SEL_SCIFA2_0),
 	PINMUX_IPSR_MSEL(IP3_26_24, I2C2_SDA_E, SEL_I2C02_4),
 	PINMUX_IPSR_MSEL(IP3_26_24, TS_SPSYNC_B, SEL_TSIF0_1),
-	PINMUX_IPSR_MSEL(IP3_26_24, RIF0_D1, SEL_DR1_0),
 	PINMUX_IPSR_MSEL(IP3_26_24, FMIN, SEL_DARC_0),
 	PINMUX_IPSR_GPSR(IP3_26_24, SCIFB2_RTS_N),
-	PINMUX_IPSR_MSEL(IP3_26_24, STM_N_B, SEL_FSN_1),
 	PINMUX_IPSR_GPSR(IP3_29_27, BS_N),
 	PINMUX_IPSR_GPSR(IP3_29_27, DRACK0),
 	PINMUX_IPSR_GPSR(IP3_29_27, PWM1_C),
 	PINMUX_IPSR_GPSR(IP3_29_27, TPUTO0_C),
 	PINMUX_IPSR_GPSR(IP3_29_27, ATACS01_N),
-	PINMUX_IPSR_MSEL(IP3_29_27, MTS_N_B, SEL_FSN_1),
 	PINMUX_IPSR_GPSR(IP3_30, RD_N),
 	PINMUX_IPSR_GPSR(IP3_30, ATACS11_N),
 	PINMUX_IPSR_GPSR(IP3_31, RD_WR_N),
@@ -858,121 +912,88 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_GPSR(IP4_1_0, EX_WAIT0),
 	PINMUX_IPSR_MSEL(IP4_1_0, CAN_CLK_B, SEL_CAN_1),
 	PINMUX_IPSR_MSEL(IP4_1_0, SCIF_CLK, SEL_SCIF0_0),
-	PINMUX_IPSR_GPSR(IP4_1_0, PWMFSW0),
 	PINMUX_IPSR_GPSR(IP4_4_2, DU0_DR0),
 	PINMUX_IPSR_GPSR(IP4_4_2, LCDOUT16),
 	PINMUX_IPSR_MSEL(IP4_4_2, SCIF5_RXD_C, SEL_SCIF5_2),
 	PINMUX_IPSR_MSEL(IP4_4_2, I2C2_SCL_D, SEL_I2C02_3),
-	PINMUX_IPSR_GPSR(IP4_4_2, CC50_STATE0),
 	PINMUX_IPSR_GPSR(IP4_7_5, DU0_DR1),
 	PINMUX_IPSR_GPSR(IP4_7_5, LCDOUT17),
 	PINMUX_IPSR_MSEL(IP4_7_5, SCIF5_TXD_C, SEL_SCIF5_2),
 	PINMUX_IPSR_MSEL(IP4_7_5, I2C2_SDA_D, SEL_I2C02_3),
-	PINMUX_IPSR_GPSR(IP4_9_8, CC50_STATE1),
 	PINMUX_IPSR_GPSR(IP4_9_8, DU0_DR2),
 	PINMUX_IPSR_GPSR(IP4_9_8, LCDOUT18),
-	PINMUX_IPSR_GPSR(IP4_9_8, CC50_STATE2),
 	PINMUX_IPSR_GPSR(IP4_11_10, DU0_DR3),
 	PINMUX_IPSR_GPSR(IP4_11_10, LCDOUT19),
-	PINMUX_IPSR_GPSR(IP4_11_10, CC50_STATE3),
 	PINMUX_IPSR_GPSR(IP4_13_12, DU0_DR4),
 	PINMUX_IPSR_GPSR(IP4_13_12, LCDOUT20),
-	PINMUX_IPSR_GPSR(IP4_13_12, CC50_STATE4),
 	PINMUX_IPSR_GPSR(IP4_15_14, DU0_DR5),
 	PINMUX_IPSR_GPSR(IP4_15_14, LCDOUT21),
-	PINMUX_IPSR_GPSR(IP4_15_14, CC50_STATE5),
 	PINMUX_IPSR_GPSR(IP4_17_16, DU0_DR6),
 	PINMUX_IPSR_GPSR(IP4_17_16, LCDOUT22),
-	PINMUX_IPSR_GPSR(IP4_17_16, CC50_STATE6),
 	PINMUX_IPSR_GPSR(IP4_19_18, DU0_DR7),
 	PINMUX_IPSR_GPSR(IP4_19_18, LCDOUT23),
-	PINMUX_IPSR_GPSR(IP4_19_18, CC50_STATE7),
 	PINMUX_IPSR_GPSR(IP4_22_20, DU0_DG0),
 	PINMUX_IPSR_GPSR(IP4_22_20, LCDOUT8),
 	PINMUX_IPSR_MSEL(IP4_22_20, SCIFA0_RXD_C, SEL_SCIFA0_2),
 	PINMUX_IPSR_MSEL(IP4_22_20, I2C3_SCL_D, SEL_I2C03_3),
-	PINMUX_IPSR_GPSR(IP4_22_20, CC50_STATE8),
 	PINMUX_IPSR_GPSR(IP4_25_23, DU0_DG1),
 	PINMUX_IPSR_GPSR(IP4_25_23, LCDOUT9),
 	PINMUX_IPSR_MSEL(IP4_25_23, SCIFA0_TXD_C, SEL_SCIFA0_2),
 	PINMUX_IPSR_MSEL(IP4_25_23, I2C3_SDA_D, SEL_I2C03_3),
-	PINMUX_IPSR_GPSR(IP4_25_23, CC50_STATE9),
 	PINMUX_IPSR_GPSR(IP4_27_26, DU0_DG2),
 	PINMUX_IPSR_GPSR(IP4_27_26, LCDOUT10),
-	PINMUX_IPSR_GPSR(IP4_27_26, CC50_STATE10),
 	PINMUX_IPSR_GPSR(IP4_29_28, DU0_DG3),
 	PINMUX_IPSR_GPSR(IP4_29_28, LCDOUT11),
-	PINMUX_IPSR_GPSR(IP4_29_28, CC50_STATE11),
 	PINMUX_IPSR_GPSR(IP4_31_30, DU0_DG4),
 	PINMUX_IPSR_GPSR(IP4_31_30, LCDOUT12),
-	PINMUX_IPSR_GPSR(IP4_31_30, CC50_STATE12),
 
 	/* IPSR5 */
 	PINMUX_IPSR_GPSR(IP5_1_0, DU0_DG5),
 	PINMUX_IPSR_GPSR(IP5_1_0, LCDOUT13),
-	PINMUX_IPSR_GPSR(IP5_1_0, CC50_STATE13),
 	PINMUX_IPSR_GPSR(IP5_3_2, DU0_DG6),
 	PINMUX_IPSR_GPSR(IP5_3_2, LCDOUT14),
-	PINMUX_IPSR_GPSR(IP5_3_2, CC50_STATE14),
 	PINMUX_IPSR_GPSR(IP5_5_4, DU0_DG7),
 	PINMUX_IPSR_GPSR(IP5_5_4, LCDOUT15),
-	PINMUX_IPSR_GPSR(IP5_5_4, CC50_STATE15),
 	PINMUX_IPSR_GPSR(IP5_8_6, DU0_DB0),
 	PINMUX_IPSR_GPSR(IP5_8_6, LCDOUT0),
 	PINMUX_IPSR_MSEL(IP5_8_6, SCIFA4_RXD_C, SEL_SCIFA4_2),
 	PINMUX_IPSR_MSEL(IP5_8_6, I2C4_SCL_D, SEL_I2C04_3),
 	PINMUX_IPSR_MSEL(IP7_8_6, CAN0_RX_C, SEL_CAN0_2),
-	PINMUX_IPSR_GPSR(IP5_8_6, CC50_STATE16),
 	PINMUX_IPSR_GPSR(IP5_11_9, DU0_DB1),
 	PINMUX_IPSR_GPSR(IP5_11_9, LCDOUT1),
 	PINMUX_IPSR_MSEL(IP5_11_9, SCIFA4_TXD_C, SEL_SCIFA4_2),
 	PINMUX_IPSR_MSEL(IP5_11_9, I2C4_SDA_D, SEL_I2C04_3),
 	PINMUX_IPSR_MSEL(IP5_11_9, CAN0_TX_C, SEL_CAN0_2),
-	PINMUX_IPSR_GPSR(IP5_11_9, CC50_STATE17),
 	PINMUX_IPSR_GPSR(IP5_13_12, DU0_DB2),
 	PINMUX_IPSR_GPSR(IP5_13_12, LCDOUT2),
-	PINMUX_IPSR_GPSR(IP5_13_12, CC50_STATE18),
 	PINMUX_IPSR_GPSR(IP5_15_14, DU0_DB3),
 	PINMUX_IPSR_GPSR(IP5_15_14, LCDOUT3),
-	PINMUX_IPSR_GPSR(IP5_15_14, CC50_STATE19),
 	PINMUX_IPSR_GPSR(IP5_17_16, DU0_DB4),
 	PINMUX_IPSR_GPSR(IP5_17_16, LCDOUT4),
-	PINMUX_IPSR_GPSR(IP5_17_16, CC50_STATE20),
 	PINMUX_IPSR_GPSR(IP5_19_18, DU0_DB5),
 	PINMUX_IPSR_GPSR(IP5_19_18, LCDOUT5),
-	PINMUX_IPSR_GPSR(IP5_19_18, CC50_STATE21),
 	PINMUX_IPSR_GPSR(IP5_21_20, DU0_DB6),
 	PINMUX_IPSR_GPSR(IP5_21_20, LCDOUT6),
-	PINMUX_IPSR_GPSR(IP5_21_20, CC50_STATE22),
 	PINMUX_IPSR_GPSR(IP5_23_22, DU0_DB7),
 	PINMUX_IPSR_GPSR(IP5_23_22, LCDOUT7),
-	PINMUX_IPSR_GPSR(IP5_23_22, CC50_STATE23),
 	PINMUX_IPSR_GPSR(IP5_25_24, DU0_DOTCLKIN),
 	PINMUX_IPSR_GPSR(IP5_25_24, QSTVA_QVS),
-	PINMUX_IPSR_GPSR(IP5_25_24, CC50_STATE24),
 	PINMUX_IPSR_GPSR(IP5_27_26, DU0_DOTCLKOUT0),
 	PINMUX_IPSR_GPSR(IP5_27_26, QCLK),
-	PINMUX_IPSR_GPSR(IP5_27_26, CC50_STATE25),
 	PINMUX_IPSR_GPSR(IP5_29_28, DU0_DOTCLKOUT1),
 	PINMUX_IPSR_GPSR(IP5_29_28, QSTVB_QVE),
-	PINMUX_IPSR_GPSR(IP5_29_28, CC50_STATE26),
 	PINMUX_IPSR_GPSR(IP5_31_30, DU0_EXHSYNC_DU0_HSYNC),
 	PINMUX_IPSR_GPSR(IP5_31_30, QSTH_QHS),
-	PINMUX_IPSR_GPSR(IP5_31_30, CC50_STATE27),
 
 	/* IPSR6 */
 	PINMUX_IPSR_GPSR(IP6_1_0, DU0_EXVSYNC_DU0_VSYNC),
 	PINMUX_IPSR_GPSR(IP6_1_0, QSTB_QHE),
-	PINMUX_IPSR_GPSR(IP6_1_0, CC50_STATE28),
 	PINMUX_IPSR_GPSR(IP6_3_2, DU0_EXODDF_DU0_ODDF_DISP_CDE),
 	PINMUX_IPSR_GPSR(IP6_3_2, QCPV_QDE),
-	PINMUX_IPSR_GPSR(IP6_3_2, CC50_STATE29),
 	PINMUX_IPSR_GPSR(IP6_5_4, DU0_DISP),
 	PINMUX_IPSR_GPSR(IP6_5_4, QPOLA),
-	PINMUX_IPSR_GPSR(IP6_5_4, CC50_STATE30),
 	PINMUX_IPSR_GPSR(IP6_7_6, DU0_CDE),
 	PINMUX_IPSR_GPSR(IP6_7_6, QPOLB),
-	PINMUX_IPSR_GPSR(IP6_7_6, CC50_STATE31),
 	PINMUX_IPSR_GPSR(IP6_8, VI0_CLK),
 	PINMUX_IPSR_GPSR(IP6_8, AVB_RX_CLK),
 	PINMUX_IPSR_GPSR(IP6_9, VI0_DATA0_VI0_B0),
@@ -1014,33 +1035,29 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP6_31_29, ETH_MDIO, SEL_ETH_0),
 	PINMUX_IPSR_GPSR(IP6_31_29, VI0_G0),
 	PINMUX_IPSR_MSEL(IP6_31_29, MSIOF2_RXD_B, SEL_MSI2_1),
-	PINMUX_IPSR_MSEL(IP6_31_29, IIC0_SCL_D, SEL_IIC00_3),
+	PINMUX_IPSR_MSEL(IP6_31_29, I2C5_SCL_D, SEL_I2C05_3),
 	PINMUX_IPSR_GPSR(IP6_31_29, AVB_TX_CLK),
 	PINMUX_IPSR_MSEL(IP6_31_29, ADIDATA, SEL_RAD_0),
-	PINMUX_IPSR_MSEL(IP6_31_29, AD_DI, SEL_ADI_0),
 
 	/* IPSR7 */
 	PINMUX_IPSR_MSEL(IP7_2_0, ETH_CRS_DV, SEL_ETH_0),
 	PINMUX_IPSR_GPSR(IP7_2_0, VI0_G1),
 	PINMUX_IPSR_MSEL(IP7_2_0, MSIOF2_TXD_B, SEL_MSI2_1),
-	PINMUX_IPSR_MSEL(IP7_2_0, IIC0_SDA_D, SEL_IIC00_3),
+	PINMUX_IPSR_MSEL(IP7_2_0, I2C5_SDA_D, SEL_I2C05_3),
 	PINMUX_IPSR_GPSR(IP7_2_0, AVB_TXD0),
 	PINMUX_IPSR_MSEL(IP7_2_0, ADICS_SAMP, SEL_RAD_0),
-	PINMUX_IPSR_MSEL(IP7_2_0, AD_DO, SEL_ADI_0),
 	PINMUX_IPSR_MSEL(IP7_5_3, ETH_RX_ER, SEL_ETH_0),
 	PINMUX_IPSR_GPSR(IP7_5_3, VI0_G2),
 	PINMUX_IPSR_MSEL(IP7_5_3, MSIOF2_SCK_B, SEL_MSI2_1),
 	PINMUX_IPSR_MSEL(IP7_5_3, CAN0_RX_B, SEL_CAN0_1),
 	PINMUX_IPSR_GPSR(IP7_5_3, AVB_TXD1),
 	PINMUX_IPSR_MSEL(IP7_5_3, ADICLK, SEL_RAD_0),
-	PINMUX_IPSR_MSEL(IP7_5_3, AD_CLK, SEL_ADI_0),
 	PINMUX_IPSR_MSEL(IP7_8_6, ETH_RXD0, SEL_ETH_0),
 	PINMUX_IPSR_GPSR(IP7_8_6, VI0_G3),
 	PINMUX_IPSR_MSEL(IP7_8_6, MSIOF2_SYNC_B, SEL_MSI2_1),
 	PINMUX_IPSR_MSEL(IP7_8_6, CAN0_TX_B, SEL_CAN0_1),
 	PINMUX_IPSR_GPSR(IP7_8_6, AVB_TXD2),
 	PINMUX_IPSR_MSEL(IP7_8_6, ADICHS0, SEL_RAD_0),
-	PINMUX_IPSR_MSEL(IP7_8_6, AD_NCS_N, SEL_ADI_0),
 	PINMUX_IPSR_MSEL(IP7_11_9, ETH_RXD1, SEL_ETH_0),
 	PINMUX_IPSR_GPSR(IP7_11_9, VI0_G4),
 	PINMUX_IPSR_MSEL(IP7_11_9, MSIOF2_SS1_B, SEL_MSI2_1),
@@ -1061,13 +1078,13 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP7_20_18, ETH_TXD1, SEL_ETH_0),
 	PINMUX_IPSR_GPSR(IP7_20_18, VI0_G7),
 	PINMUX_IPSR_MSEL(IP7_20_18, SCIF2_RXD_C, SEL_SCIF2_2),
-	PINMUX_IPSR_MSEL(IP7_20_18, IIC1_SCL_D, SEL_IIC01_3),
+	PINMUX_IPSR_MSEL(IP7_20_18, IIC0_SCL_D, SEL_IIC0_3),
 	PINMUX_IPSR_GPSR(IP7_20_18, AVB_TXD6),
 	PINMUX_IPSR_MSEL(IP7_20_18, SSI_WS5_B, SEL_SSI5_1),
 	PINMUX_IPSR_MSEL(IP7_23_21, ETH_TX_EN, SEL_ETH_0),
 	PINMUX_IPSR_GPSR(IP7_23_21, VI0_R0),
 	PINMUX_IPSR_MSEL(IP7_23_21, SCIF2_TXD_C, SEL_SCIF2_2),
-	PINMUX_IPSR_MSEL(IP7_23_21, IIC1_SDA_D, SEL_IIC01_3),
+	PINMUX_IPSR_MSEL(IP7_23_21, IIC0_SDA_D, SEL_IIC0_3),
 	PINMUX_IPSR_GPSR(IP7_23_21, AVB_TXD7),
 	PINMUX_IPSR_MSEL(IP7_23_21, SSI_SDATA5_B, SEL_SSI5_1),
 	PINMUX_IPSR_MSEL(IP7_26_24, ETH_MAGIC, SEL_ETH_0),
@@ -1136,60 +1153,48 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP8_25_23, SCIF4_RXD, SEL_SCIF4_0),
 	PINMUX_IPSR_GPSR(IP8_25_23, PWM5_B),
 	PINMUX_IPSR_GPSR(IP8_25_23, DU1_DR0),
-	PINMUX_IPSR_MSEL(IP8_25_23, RIF1_SYNC_B, SEL_DR2_1),
 	PINMUX_IPSR_MSEL(IP8_25_23, TS_SDATA_D, SEL_TSIF0_3),
 	PINMUX_IPSR_GPSR(IP8_25_23, TPUTO1_B),
 	PINMUX_IPSR_MSEL(IP8_28_26, I2C1_SDA, SEL_I2C01_0),
 	PINMUX_IPSR_MSEL(IP8_28_26, SCIF4_TXD, SEL_SCIF4_0),
 	PINMUX_IPSR_GPSR(IP8_28_26, IRQ5),
 	PINMUX_IPSR_GPSR(IP8_28_26, DU1_DR1),
-	PINMUX_IPSR_MSEL(IP8_28_26, RIF1_CLK_B, SEL_DR2_1),
 	PINMUX_IPSR_MSEL(IP8_28_26, TS_SCK_D, SEL_TSIF0_3),
 	PINMUX_IPSR_MSEL(IP8_28_26, BPFCLK_C, SEL_DARC_2),
 	PINMUX_IPSR_GPSR(IP8_31_29, MSIOF0_RXD),
 	PINMUX_IPSR_MSEL(IP8_31_29, SCIF5_RXD, SEL_SCIF5_0),
 	PINMUX_IPSR_MSEL(IP8_31_29, I2C2_SCL_C, SEL_I2C02_2),
 	PINMUX_IPSR_GPSR(IP8_31_29, DU1_DR2),
-	PINMUX_IPSR_MSEL(IP8_31_29, RIF1_D0_B, SEL_DR2_1),
 	PINMUX_IPSR_MSEL(IP8_31_29, TS_SDEN_D, SEL_TSIF0_3),
 	PINMUX_IPSR_MSEL(IP8_31_29, FMCLK_C, SEL_DARC_2),
-	PINMUX_IPSR_MSEL(IP8_31_29, RDS_CLK, SEL_RDS_0),
 
 	/* IPSR9 */
 	PINMUX_IPSR_GPSR(IP9_2_0, MSIOF0_TXD),
 	PINMUX_IPSR_MSEL(IP9_2_0, SCIF5_TXD, SEL_SCIF5_0),
 	PINMUX_IPSR_MSEL(IP9_2_0, I2C2_SDA_C, SEL_I2C02_2),
 	PINMUX_IPSR_GPSR(IP9_2_0, DU1_DR3),
-	PINMUX_IPSR_MSEL(IP9_2_0, RIF1_D1_B, SEL_DR3_1),
 	PINMUX_IPSR_MSEL(IP9_2_0, TS_SPSYNC_D, SEL_TSIF0_3),
 	PINMUX_IPSR_MSEL(IP9_2_0, FMIN_C, SEL_DARC_2),
-	PINMUX_IPSR_MSEL(IP9_2_0, RDS_DATA, SEL_RDS_0),
 	PINMUX_IPSR_GPSR(IP9_5_3, MSIOF0_SCK),
 	PINMUX_IPSR_GPSR(IP9_5_3, IRQ0),
 	PINMUX_IPSR_MSEL(IP9_5_3, TS_SDATA, SEL_TSIF0_0),
 	PINMUX_IPSR_GPSR(IP9_5_3, DU1_DR4),
-	PINMUX_IPSR_MSEL(IP9_5_3, RIF1_SYNC, SEL_DR2_0),
 	PINMUX_IPSR_GPSR(IP9_5_3, TPUTO1_C),
 	PINMUX_IPSR_GPSR(IP9_8_6, MSIOF0_SYNC),
 	PINMUX_IPSR_GPSR(IP9_8_6, PWM1),
 	PINMUX_IPSR_MSEL(IP9_8_6, TS_SCK, SEL_TSIF0_0),
 	PINMUX_IPSR_GPSR(IP9_8_6, DU1_DR5),
-	PINMUX_IPSR_MSEL(IP9_8_6, RIF1_CLK, SEL_DR2_0),
 	PINMUX_IPSR_MSEL(IP9_8_6, BPFCLK_B, SEL_DARC_1),
 	PINMUX_IPSR_GPSR(IP9_11_9, MSIOF0_SS1),
 	PINMUX_IPSR_MSEL(IP9_11_9, SCIFA0_RXD, SEL_SCIFA0_0),
 	PINMUX_IPSR_MSEL(IP9_11_9, TS_SDEN, SEL_TSIF0_0),
 	PINMUX_IPSR_GPSR(IP9_11_9, DU1_DR6),
-	PINMUX_IPSR_MSEL(IP9_11_9, RIF1_D0, SEL_DR2_0),
 	PINMUX_IPSR_MSEL(IP9_11_9, FMCLK_B, SEL_DARC_1),
-	PINMUX_IPSR_MSEL(IP9_11_9, RDS_CLK_B, SEL_RDS_1),
 	PINMUX_IPSR_GPSR(IP9_14_12, MSIOF0_SS2),
 	PINMUX_IPSR_MSEL(IP9_14_12, SCIFA0_TXD, SEL_SCIFA0_0),
 	PINMUX_IPSR_MSEL(IP9_14_12, TS_SPSYNC, SEL_TSIF0_0),
 	PINMUX_IPSR_GPSR(IP9_14_12, DU1_DR7),
-	PINMUX_IPSR_MSEL(IP9_14_12, RIF1_D1, SEL_DR3_0),
 	PINMUX_IPSR_MSEL(IP9_14_12, FMIN_B, SEL_DARC_1),
-	PINMUX_IPSR_MSEL(IP9_14_12, RDS_DATA_B, SEL_RDS_1),
 	PINMUX_IPSR_MSEL(IP9_16_15, HSCIF1_HRX, SEL_HSCIF1_0),
 	PINMUX_IPSR_MSEL(IP9_16_15, I2C4_SCL, SEL_I2C04_0),
 	PINMUX_IPSR_GPSR(IP9_16_15, PWM6),
@@ -1204,165 +1209,124 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_GPSR(IP9_21_19, DU1_DG2),
 	PINMUX_IPSR_MSEL(IP9_21_19, REMOCON_B, SEL_RCN_1),
 	PINMUX_IPSR_MSEL(IP9_21_19, SPEEDIN_B, SEL_RSP_1),
-	PINMUX_IPSR_MSEL(IP9_21_19, VSP_B, SEL_SPDM_1),
 	PINMUX_IPSR_MSEL(IP9_24_22, HSCIF1_HCTS_N, SEL_HSCIF1_0),
 	PINMUX_IPSR_MSEL(IP9_24_22, SCIFA4_RXD, SEL_SCIFA4_0),
 	PINMUX_IPSR_MSEL(IP9_24_22, IECLK, SEL_IEB_0),
 	PINMUX_IPSR_GPSR(IP9_24_22, DU1_DG3),
 	PINMUX_IPSR_MSEL(IP9_24_22, SSI_SCK1_B, SEL_SSI1_1),
-	PINMUX_IPSR_GPSR(IP9_24_22, CAN_DEBUG_HW_TRIGGER),
-	PINMUX_IPSR_GPSR(IP9_24_22, CC50_STATE32),
 	PINMUX_IPSR_MSEL(IP9_27_25, HSCIF1_HRTS_N, SEL_HSCIF1_0),
 	PINMUX_IPSR_MSEL(IP9_27_25, SCIFA4_TXD, SEL_SCIFA4_0),
 	PINMUX_IPSR_MSEL(IP9_27_25, IERX, SEL_IEB_0),
 	PINMUX_IPSR_GPSR(IP9_27_25, DU1_DG4),
 	PINMUX_IPSR_MSEL(IP9_27_25, SSI_WS1_B, SEL_SSI1_1),
-	PINMUX_IPSR_GPSR(IP9_27_25, CAN_STEP0),
-	PINMUX_IPSR_GPSR(IP9_27_25, CC50_STATE33),
 	PINMUX_IPSR_MSEL(IP9_30_28, SCIF1_SCK, SEL_SCIF1_0),
 	PINMUX_IPSR_GPSR(IP9_30_28, PWM3),
 	PINMUX_IPSR_MSEL(IP9_30_28, TCLK2, SEL_TMU_0),
 	PINMUX_IPSR_GPSR(IP9_30_28, DU1_DG5),
 	PINMUX_IPSR_MSEL(IP9_30_28, SSI_SDATA1_B, SEL_SSI1_1),
-	PINMUX_IPSR_GPSR(IP9_30_28, CAN_TXCLK),
-	PINMUX_IPSR_GPSR(IP9_30_28, CC50_STATE34),
 
 	/* IPSR10 */
 	PINMUX_IPSR_MSEL(IP10_2_0, SCIF1_RXD, SEL_SCIF1_0),
-	PINMUX_IPSR_MSEL(IP10_2_0, IIC0_SCL, SEL_IIC00_0),
+	PINMUX_IPSR_MSEL(IP10_2_0, I2C5_SCL, SEL_I2C05_0),
 	PINMUX_IPSR_GPSR(IP10_2_0, DU1_DG6),
 	PINMUX_IPSR_MSEL(IP10_2_0, SSI_SCK2_B, SEL_SSI2_1),
-	PINMUX_IPSR_GPSR(IP10_2_0, CAN_DEBUGOUT0),
-	PINMUX_IPSR_GPSR(IP10_2_0, CC50_STATE35),
 	PINMUX_IPSR_MSEL(IP10_5_3, SCIF1_TXD, SEL_SCIF1_0),
-	PINMUX_IPSR_MSEL(IP10_5_3, IIC0_SDA, SEL_IIC00_0),
+	PINMUX_IPSR_MSEL(IP10_5_3, I2C5_SDA, SEL_I2C05_0),
 	PINMUX_IPSR_GPSR(IP10_5_3, DU1_DG7),
 	PINMUX_IPSR_MSEL(IP10_5_3, SSI_WS2_B, SEL_SSI2_1),
-	PINMUX_IPSR_GPSR(IP10_5_3, CAN_DEBUGOUT1),
-	PINMUX_IPSR_GPSR(IP10_5_3, CC50_STATE36),
 	PINMUX_IPSR_MSEL(IP10_8_6, SCIF2_RXD, SEL_SCIF2_0),
-	PINMUX_IPSR_MSEL(IP10_8_6, IIC1_SCL, SEL_IIC01_0),
+	PINMUX_IPSR_MSEL(IP10_8_6, IIC0_SCL, SEL_IIC0_0),
 	PINMUX_IPSR_GPSR(IP10_8_6, DU1_DB0),
 	PINMUX_IPSR_MSEL(IP10_8_6, SSI_SDATA2_B, SEL_SSI2_1),
-	PINMUX_IPSR_GPSR(IP10_8_6, USB0_EXTLP),
-	PINMUX_IPSR_GPSR(IP10_8_6, CAN_DEBUGOUT2),
-	PINMUX_IPSR_GPSR(IP10_8_6, CC50_STATE37),
 	PINMUX_IPSR_MSEL(IP10_11_9, SCIF2_TXD, SEL_SCIF2_0),
-	PINMUX_IPSR_MSEL(IP10_11_9, IIC1_SDA, SEL_IIC01_0),
+	PINMUX_IPSR_MSEL(IP10_11_9, IIC0_SDA, SEL_IIC0_0),
 	PINMUX_IPSR_GPSR(IP10_11_9, DU1_DB1),
 	PINMUX_IPSR_MSEL(IP10_11_9, SSI_SCK9_B, SEL_SSI9_1),
-	PINMUX_IPSR_GPSR(IP10_11_9, USB0_OVC1),
-	PINMUX_IPSR_GPSR(IP10_11_9, CAN_DEBUGOUT3),
-	PINMUX_IPSR_GPSR(IP10_11_9, CC50_STATE38),
 	PINMUX_IPSR_MSEL(IP10_14_12, SCIF2_SCK, SEL_SCIF2_0),
 	PINMUX_IPSR_GPSR(IP10_14_12, IRQ1),
 	PINMUX_IPSR_GPSR(IP10_14_12, DU1_DB2),
 	PINMUX_IPSR_MSEL(IP10_14_12, SSI_WS9_B, SEL_SSI9_1),
-	PINMUX_IPSR_GPSR(IP10_14_12, USB0_IDIN),
-	PINMUX_IPSR_GPSR(IP10_14_12, CAN_DEBUGOUT4),
-	PINMUX_IPSR_GPSR(IP10_14_12, CC50_STATE39),
 	PINMUX_IPSR_MSEL(IP10_17_15, SCIF3_SCK, SEL_SCIF3_0),
 	PINMUX_IPSR_GPSR(IP10_17_15, IRQ2),
 	PINMUX_IPSR_MSEL(IP10_17_15, BPFCLK_D, SEL_DARC_3),
 	PINMUX_IPSR_GPSR(IP10_17_15, DU1_DB3),
 	PINMUX_IPSR_MSEL(IP10_17_15, SSI_SDATA9_B, SEL_SSI9_1),
-	PINMUX_IPSR_GPSR(IP10_17_15, TANS2),
-	PINMUX_IPSR_GPSR(IP10_17_15, CAN_DEBUGOUT5),
-	PINMUX_IPSR_GPSR(IP10_17_15, CC50_OSCOUT),
 	PINMUX_IPSR_MSEL(IP10_20_18, SCIF3_RXD, SEL_SCIF3_0),
 	PINMUX_IPSR_MSEL(IP10_20_18, I2C1_SCL_E, SEL_I2C01_4),
 	PINMUX_IPSR_MSEL(IP10_20_18, FMCLK_D, SEL_DARC_3),
 	PINMUX_IPSR_GPSR(IP10_20_18, DU1_DB4),
 	PINMUX_IPSR_MSEL(IP10_20_18, AUDIO_CLKA_C, SEL_ADG_2),
 	PINMUX_IPSR_MSEL(IP10_20_18, SSI_SCK4_B, SEL_SSI4_1),
-	PINMUX_IPSR_GPSR(IP10_20_18, CAN_DEBUGOUT6),
-	PINMUX_IPSR_MSEL(IP10_20_18, RDS_CLK_C, SEL_RDS_2),
 	PINMUX_IPSR_MSEL(IP10_23_21, SCIF3_TXD, SEL_SCIF3_0),
 	PINMUX_IPSR_MSEL(IP10_23_21, I2C1_SDA_E, SEL_I2C01_4),
 	PINMUX_IPSR_MSEL(IP10_23_21, FMIN_D, SEL_DARC_3),
 	PINMUX_IPSR_GPSR(IP10_23_21, DU1_DB5),
 	PINMUX_IPSR_MSEL(IP10_23_21, AUDIO_CLKB_C, SEL_ADG_2),
 	PINMUX_IPSR_MSEL(IP10_23_21, SSI_WS4_B, SEL_SSI4_1),
-	PINMUX_IPSR_GPSR(IP10_23_21, CAN_DEBUGOUT7),
-	PINMUX_IPSR_MSEL(IP10_23_21, RDS_DATA_C, SEL_RDS_2),
 	PINMUX_IPSR_MSEL(IP10_26_24, I2C2_SCL, SEL_I2C02_0),
 	PINMUX_IPSR_MSEL(IP10_26_24, SCIFA5_RXD, SEL_SCIFA5_0),
 	PINMUX_IPSR_GPSR(IP10_26_24, DU1_DB6),
 	PINMUX_IPSR_MSEL(IP10_26_24, AUDIO_CLKC_C, SEL_ADG_2),
 	PINMUX_IPSR_MSEL(IP10_26_24, SSI_SDATA4_B, SEL_SSI4_1),
-	PINMUX_IPSR_GPSR(IP10_26_24, CAN_DEBUGOUT8),
 	PINMUX_IPSR_MSEL(IP10_29_27, I2C2_SDA, SEL_I2C02_0),
 	PINMUX_IPSR_MSEL(IP10_29_27, SCIFA5_TXD, SEL_SCIFA5_0),
 	PINMUX_IPSR_GPSR(IP10_29_27, DU1_DB7),
 	PINMUX_IPSR_MSEL(IP10_29_27, AUDIO_CLKOUT_C, SEL_ADG_2),
-	PINMUX_IPSR_GPSR(IP10_29_27, CAN_DEBUGOUT9),
 	PINMUX_IPSR_MSEL(IP10_31_30, SSI_SCK5, SEL_SSI5_0),
 	PINMUX_IPSR_MSEL(IP10_31_30, SCIFA3_SCK, SEL_SCIFA3_0),
 	PINMUX_IPSR_GPSR(IP10_31_30, DU1_DOTCLKIN),
-	PINMUX_IPSR_GPSR(IP10_31_30, CAN_DEBUGOUT10),
 
 	/* IPSR11 */
 	PINMUX_IPSR_MSEL(IP11_2_0, SSI_WS5, SEL_SSI5_0),
 	PINMUX_IPSR_MSEL(IP11_2_0, SCIFA3_RXD, SEL_SCIFA3_0),
 	PINMUX_IPSR_MSEL(IP11_2_0, I2C3_SCL_C, SEL_I2C03_2),
 	PINMUX_IPSR_GPSR(IP11_2_0, DU1_DOTCLKOUT0),
-	PINMUX_IPSR_GPSR(IP11_2_0, CAN_DEBUGOUT11),
 	PINMUX_IPSR_MSEL(IP11_5_3, SSI_SDATA5, SEL_SSI5_0),
 	PINMUX_IPSR_MSEL(IP11_5_3, SCIFA3_TXD, SEL_SCIFA3_0),
 	PINMUX_IPSR_MSEL(IP11_5_3, I2C3_SDA_C, SEL_I2C03_2),
 	PINMUX_IPSR_GPSR(IP11_5_3, DU1_DOTCLKOUT1),
-	PINMUX_IPSR_GPSR(IP11_5_3, CAN_DEBUGOUT12),
 	PINMUX_IPSR_MSEL(IP11_7_6, SSI_SCK6, SEL_SSI6_0),
 	PINMUX_IPSR_MSEL(IP11_7_6, SCIFA1_SCK_B, SEL_SCIFA1_1),
 	PINMUX_IPSR_GPSR(IP11_7_6, DU1_EXHSYNC_DU1_HSYNC),
-	PINMUX_IPSR_GPSR(IP11_7_6, CAN_DEBUGOUT13),
 	PINMUX_IPSR_MSEL(IP11_10_8, SSI_WS6, SEL_SSI6_0),
 	PINMUX_IPSR_MSEL(IP11_10_8, SCIFA1_RXD_B, SEL_SCIFA1_1),
 	PINMUX_IPSR_MSEL(IP11_10_8, I2C4_SCL_C, SEL_I2C04_2),
 	PINMUX_IPSR_GPSR(IP11_10_8, DU1_EXVSYNC_DU1_VSYNC),
-	PINMUX_IPSR_GPSR(IP11_10_8, CAN_DEBUGOUT14),
 	PINMUX_IPSR_MSEL(IP11_13_11, SSI_SDATA6, SEL_SSI6_0),
 	PINMUX_IPSR_MSEL(IP11_13_11, SCIFA1_TXD_B, SEL_SCIFA1_1),
 	PINMUX_IPSR_MSEL(IP11_13_11, I2C4_SDA_C, SEL_I2C04_2),
 	PINMUX_IPSR_GPSR(IP11_13_11, DU1_EXODDF_DU1_ODDF_DISP_CDE),
-	PINMUX_IPSR_GPSR(IP11_13_11, CAN_DEBUGOUT15),
 	PINMUX_IPSR_MSEL(IP11_15_14, SSI_SCK78, SEL_SSI7_0),
 	PINMUX_IPSR_MSEL(IP11_15_14, SCIFA2_SCK_B, SEL_SCIFA2_1),
-	PINMUX_IPSR_MSEL(IP11_15_14, IIC0_SDA_C, SEL_IIC00_2),
+	PINMUX_IPSR_MSEL(IP11_15_14, I2C5_SDA_C, SEL_I2C05_2),
 	PINMUX_IPSR_GPSR(IP11_15_14, DU1_DISP),
 	PINMUX_IPSR_MSEL(IP11_17_16, SSI_WS78, SEL_SSI7_0),
 	PINMUX_IPSR_MSEL(IP11_17_16, SCIFA2_RXD_B, SEL_SCIFA2_1),
-	PINMUX_IPSR_MSEL(IP11_17_16, IIC0_SCL_C, SEL_IIC00_2),
+	PINMUX_IPSR_MSEL(IP11_17_16, I2C5_SCL_C, SEL_I2C05_2),
 	PINMUX_IPSR_GPSR(IP11_17_16, DU1_CDE),
 	PINMUX_IPSR_MSEL(IP11_20_18, SSI_SDATA7, SEL_SSI7_0),
 	PINMUX_IPSR_MSEL(IP11_20_18, SCIFA2_TXD_B, SEL_SCIFA2_1),
 	PINMUX_IPSR_GPSR(IP11_20_18, IRQ8),
 	PINMUX_IPSR_MSEL(IP11_20_18, AUDIO_CLKA_D, SEL_ADG_3),
 	PINMUX_IPSR_MSEL(IP11_20_18, CAN_CLK_D, SEL_CAN_3),
-	PINMUX_IPSR_GPSR(IP11_20_18, PCMOE_N),
 	PINMUX_IPSR_GPSR(IP11_23_21, SSI_SCK0129),
 	PINMUX_IPSR_MSEL(IP11_23_21, MSIOF1_RXD_B, SEL_MSI1_1),
 	PINMUX_IPSR_MSEL(IP11_23_21, SCIF5_RXD_D, SEL_SCIF5_3),
 	PINMUX_IPSR_MSEL(IP11_23_21, ADIDATA_B, SEL_RAD_1),
-	PINMUX_IPSR_MSEL(IP11_23_21, AD_DI_B, SEL_ADI_1),
-	PINMUX_IPSR_GPSR(IP11_23_21, PCMWE_N),
 	PINMUX_IPSR_GPSR(IP11_26_24, SSI_WS0129),
 	PINMUX_IPSR_MSEL(IP11_26_24, MSIOF1_TXD_B, SEL_MSI1_1),
 	PINMUX_IPSR_MSEL(IP11_26_24, SCIF5_TXD_D, SEL_SCIF5_3),
 	PINMUX_IPSR_MSEL(IP11_26_24, ADICS_SAMP_B, SEL_RAD_1),
-	PINMUX_IPSR_MSEL(IP11_26_24, AD_DO_B, SEL_ADI_1),
 	PINMUX_IPSR_GPSR(IP11_29_27, SSI_SDATA0),
 	PINMUX_IPSR_MSEL(IP11_29_27, MSIOF1_SCK_B, SEL_MSI1_1),
 	PINMUX_IPSR_GPSR(IP11_29_27, PWM0_B),
 	PINMUX_IPSR_MSEL(IP11_29_27, ADICLK_B, SEL_RAD_1),
-	PINMUX_IPSR_MSEL(IP11_29_27, AD_CLK_B, SEL_ADI_1),
 
 	/* IPSR12 */
 	PINMUX_IPSR_GPSR(IP12_2_0, SSI_SCK34),
 	PINMUX_IPSR_MSEL(IP12_2_0, MSIOF1_SYNC_B, SEL_MSI1_1),
 	PINMUX_IPSR_MSEL(IP12_2_0, SCIFA1_SCK_C, SEL_SCIFA1_2),
 	PINMUX_IPSR_MSEL(IP12_2_0, ADICHS0_B, SEL_RAD_1),
-	PINMUX_IPSR_MSEL(IP12_2_0, AD_NCS_N_B, SEL_ADI_1),
 	PINMUX_IPSR_MSEL(IP12_2_0, DREQ1_N_B, SEL_LBS_1),
 	PINMUX_IPSR_GPSR(IP12_5_3, SSI_WS34),
 	PINMUX_IPSR_MSEL(IP12_5_3, MSIOF1_SS1_B, SEL_MSI1_1),
@@ -1379,15 +1343,12 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP12_10_9, SSI_SCK4, SEL_SSI4_0),
 	PINMUX_IPSR_GPSR(IP12_10_9, MLB_CLK),
 	PINMUX_IPSR_MSEL(IP12_10_9, IETX_B, SEL_IEB_1),
-	PINMUX_IPSR_GPSR(IP12_10_9, IRD_TX),
 	PINMUX_IPSR_MSEL(IP12_12_11, SSI_WS4, SEL_SSI4_0),
 	PINMUX_IPSR_GPSR(IP12_12_11, MLB_SIG),
 	PINMUX_IPSR_MSEL(IP12_12_11, IECLK_B, SEL_IEB_1),
-	PINMUX_IPSR_GPSR(IP12_12_11, IRD_RX),
 	PINMUX_IPSR_MSEL(IP12_14_13, SSI_SDATA4, SEL_SSI4_0),
 	PINMUX_IPSR_GPSR(IP12_14_13, MLB_DAT),
 	PINMUX_IPSR_MSEL(IP12_14_13, IERX_B, SEL_IEB_1),
-	PINMUX_IPSR_GPSR(IP12_14_13, IRD_SCK),
 	PINMUX_IPSR_MSEL(IP12_17_15, SSI_SDATA8, SEL_SSI8_0),
 	PINMUX_IPSR_MSEL(IP12_17_15, SCIF1_SCK_B, SEL_SCIF1_1),
 	PINMUX_IPSR_GPSR(IP12_17_15, PWM1_B),
@@ -1397,28 +1358,24 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP12_17_15, ETH_MDIO_B, SEL_ETH_1),
 	PINMUX_IPSR_MSEL(IP12_20_18, SSI_SCK1, SEL_SSI1_0),
 	PINMUX_IPSR_MSEL(IP12_20_18, SCIF1_RXD_B, SEL_SCIF1_1),
-	PINMUX_IPSR_MSEL(IP12_20_18, IIC1_SCL_C, SEL_IIC01_2),
+	PINMUX_IPSR_MSEL(IP12_20_18, IIC0_SCL_C, SEL_IIC0_2),
 	PINMUX_IPSR_GPSR(IP12_20_18, VI1_CLK),
 	PINMUX_IPSR_MSEL(IP12_20_18, CAN0_RX_D, SEL_CAN0_3),
-	PINMUX_IPSR_MSEL(IP12_20_18, AVB_AVTP_CAPTURE, SEL_AVB_0),
 	PINMUX_IPSR_MSEL(IP12_20_18, ETH_CRS_DV_B, SEL_ETH_1),
 	PINMUX_IPSR_MSEL(IP12_23_21, SSI_WS1, SEL_SSI1_0),
 	PINMUX_IPSR_MSEL(IP12_23_21, SCIF1_TXD_B, SEL_SCIF1_1),
-	PINMUX_IPSR_MSEL(IP12_23_21, IIC1_SDA_C, SEL_IIC01_2),
+	PINMUX_IPSR_MSEL(IP12_23_21, IIC0_SDA_C, SEL_IIC0_2),
 	PINMUX_IPSR_GPSR(IP12_23_21, VI1_DATA0),
 	PINMUX_IPSR_MSEL(IP12_23_21, CAN0_TX_D, SEL_CAN0_3),
-	PINMUX_IPSR_MSEL(IP12_23_21, AVB_AVTP_MATCH, SEL_AVB_0),
 	PINMUX_IPSR_MSEL(IP12_23_21, ETH_RX_ER_B, SEL_ETH_1),
 	PINMUX_IPSR_MSEL(IP12_26_24, SSI_SDATA1, SEL_SSI1_0),
 	PINMUX_IPSR_MSEL(IP12_26_24, HSCIF1_HRX_B, SEL_HSCIF1_1),
 	PINMUX_IPSR_GPSR(IP12_26_24, VI1_DATA1),
-	PINMUX_IPSR_MSEL(IP12_26_24, SDATA, SEL_FSN_0),
 	PINMUX_IPSR_GPSR(IP12_26_24, ATAWR0_N),
 	PINMUX_IPSR_MSEL(IP12_26_24, ETH_RXD0_B, SEL_ETH_1),
 	PINMUX_IPSR_MSEL(IP12_29_27, SSI_SCK2, SEL_SSI2_0),
 	PINMUX_IPSR_MSEL(IP12_29_27, HSCIF1_HTX_B, SEL_HSCIF1_1),
 	PINMUX_IPSR_GPSR(IP12_29_27, VI1_DATA2),
-	PINMUX_IPSR_MSEL(IP12_29_27, MDATA, SEL_FSN_0),
 	PINMUX_IPSR_GPSR(IP12_29_27, ATAG0_N),
 	PINMUX_IPSR_MSEL(IP12_29_27, ETH_RXD1_B, SEL_ETH_1),
 
@@ -1427,21 +1384,18 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP13_2_0, HSCIF1_HCTS_N_B, SEL_HSCIF1_1),
 	PINMUX_IPSR_MSEL(IP13_2_0, SCIFA0_RXD_D, SEL_SCIFA0_3),
 	PINMUX_IPSR_GPSR(IP13_2_0, VI1_DATA3),
-	PINMUX_IPSR_MSEL(IP13_2_0, SCKZ, SEL_FSN_0),
 	PINMUX_IPSR_GPSR(IP13_2_0, ATACS00_N),
 	PINMUX_IPSR_MSEL(IP13_2_0, ETH_LINK_B, SEL_ETH_1),
 	PINMUX_IPSR_MSEL(IP13_5_3, SSI_SDATA2, SEL_SSI2_0),
 	PINMUX_IPSR_MSEL(IP13_5_3, HSCIF1_HRTS_N_B, SEL_HSCIF1_1),
 	PINMUX_IPSR_MSEL(IP13_5_3, SCIFA0_TXD_D, SEL_SCIFA0_3),
 	PINMUX_IPSR_GPSR(IP13_5_3, VI1_DATA4),
-	PINMUX_IPSR_MSEL(IP13_5_3, STM_N, SEL_FSN_0),
 	PINMUX_IPSR_GPSR(IP13_5_3, ATACS10_N),
 	PINMUX_IPSR_MSEL(IP13_5_3, ETH_REFCLK_B, SEL_ETH_1),
 	PINMUX_IPSR_MSEL(IP13_8_6, SSI_SCK9, SEL_SSI9_0),
 	PINMUX_IPSR_MSEL(IP13_8_6, SCIF2_SCK_B, SEL_SCIF2_1),
 	PINMUX_IPSR_GPSR(IP13_8_6, PWM2_B),
 	PINMUX_IPSR_GPSR(IP13_8_6, VI1_DATA5),
-	PINMUX_IPSR_MSEL(IP13_8_6, MTS_N, SEL_FSN_0),
 	PINMUX_IPSR_GPSR(IP13_8_6, EX_WAIT1),
 	PINMUX_IPSR_MSEL(IP13_8_6, ETH_TXD1_B, SEL_ETH_1),
 	PINMUX_IPSR_MSEL(IP13_11_9, SSI_WS9, SEL_SSI9_0),
@@ -1461,14 +1415,12 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP13_17_15, SCIFA4_RXD_D, SEL_SCIFA4_3),
 	PINMUX_IPSR_GPSR(IP13_17_15, VI1_CLKENB),
 	PINMUX_IPSR_MSEL(IP13_17_15, TS_SDATA_C, SEL_TSIF0_2),
-	PINMUX_IPSR_MSEL(IP13_17_15, RIF0_SYNC_B, SEL_DR0_1),
 	PINMUX_IPSR_MSEL(IP13_17_15, ETH_TXD0_B, SEL_ETH_1),
 	PINMUX_IPSR_MSEL(IP13_20_18, AUDIO_CLKB, SEL_ADG_0),
 	PINMUX_IPSR_MSEL(IP13_20_18, I2C0_SDA_B, SEL_I2C00_1),
 	PINMUX_IPSR_MSEL(IP13_20_18, SCIFA4_TXD_D, SEL_SCIFA4_3),
 	PINMUX_IPSR_GPSR(IP13_20_18, VI1_FIELD),
 	PINMUX_IPSR_MSEL(IP13_20_18, TS_SCK_C, SEL_TSIF0_2),
-	PINMUX_IPSR_MSEL(IP13_20_18, RIF0_CLK_B, SEL_DR0_1),
 	PINMUX_IPSR_MSEL(IP13_20_18, BPFCLK_E, SEL_DARC_4),
 	PINMUX_IPSR_MSEL(IP13_20_18, ETH_MDC_B, SEL_ETH_1),
 	PINMUX_IPSR_MSEL(IP13_23_21, AUDIO_CLKC, SEL_ADG_0),
@@ -1476,17 +1428,13 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP13_23_21, SCIFA5_RXD_D, SEL_SCIFA5_3),
 	PINMUX_IPSR_GPSR(IP13_23_21, VI1_HSYNC_N),
 	PINMUX_IPSR_MSEL(IP13_23_21, TS_SDEN_C, SEL_TSIF0_2),
-	PINMUX_IPSR_MSEL(IP13_23_21, RIF0_D0_B, SEL_DR0_1),
 	PINMUX_IPSR_MSEL(IP13_23_21, FMCLK_E, SEL_DARC_4),
-	PINMUX_IPSR_MSEL(IP13_23_21, RDS_CLK_D, SEL_RDS_3),
 	PINMUX_IPSR_MSEL(IP13_26_24, AUDIO_CLKOUT, SEL_ADG_0),
 	PINMUX_IPSR_MSEL(IP13_26_24, I2C4_SDA_B, SEL_I2C04_1),
 	PINMUX_IPSR_MSEL(IP13_26_24, SCIFA5_TXD_D, SEL_SCIFA5_3),
 	PINMUX_IPSR_GPSR(IP13_26_24, VI1_VSYNC_N),
 	PINMUX_IPSR_MSEL(IP13_26_24, TS_SPSYNC_C, SEL_TSIF0_2),
-	PINMUX_IPSR_MSEL(IP13_26_24, RIF0_D1_B, SEL_DR1_1),
 	PINMUX_IPSR_MSEL(IP13_26_24, FMIN_E, SEL_DARC_4),
-	PINMUX_IPSR_MSEL(IP13_26_24, RDS_DATA_D, SEL_RDS_3),
 };
 
 static const struct sh_pfc_pin pinmux_pins[] = {
@@ -1660,30 +1608,6 @@ static const unsigned int avb_gmii_mux[] = {
 	AVB_TX_EN_MARK, AVB_TX_ER_MARK, AVB_TX_CLK_MARK,
 	AVB_COL_MARK,
 };
-static const unsigned int avb_avtp_capture_pins[] = {
-	RCAR_GP_PIN(5, 11),
-};
-static const unsigned int avb_avtp_capture_mux[] = {
-	AVB_AVTP_CAPTURE_MARK,
-};
-static const unsigned int avb_avtp_match_pins[] = {
-	RCAR_GP_PIN(5, 12),
-};
-static const unsigned int avb_avtp_match_mux[] = {
-	AVB_AVTP_MATCH_MARK,
-};
-static const unsigned int avb_avtp_capture_b_pins[] = {
-	RCAR_GP_PIN(1, 1),
-};
-static const unsigned int avb_avtp_capture_b_mux[] = {
-	AVB_AVTP_CAPTURE_B_MARK,
-};
-static const unsigned int avb_avtp_match_b_pins[] = {
-	RCAR_GP_PIN(1, 2),
-};
-static const unsigned int avb_avtp_match_b_mux[] = {
-	AVB_AVTP_MATCH_B_MARK,
-};
 /* - DU --------------------------------------------------------------------- */
 static const unsigned int du0_rgb666_pins[] = {
 	/* R[7:2], G[7:2], B[7:2] */
@@ -3535,10 +3459,6 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(avb_mdio),
 	SH_PFC_PIN_GROUP(avb_mii),
 	SH_PFC_PIN_GROUP(avb_gmii),
-	SH_PFC_PIN_GROUP(avb_avtp_capture),
-	SH_PFC_PIN_GROUP(avb_avtp_match),
-	SH_PFC_PIN_GROUP(avb_avtp_capture_b),
-	SH_PFC_PIN_GROUP(avb_avtp_match_b),
 	SH_PFC_PIN_GROUP(du0_rgb666),
 	SH_PFC_PIN_GROUP(du0_rgb888),
 	SH_PFC_PIN_GROUP(du0_clk0_out),
@@ -3809,10 +3729,6 @@ static const char * const avb_groups[] = {
 	"avb_mdio",
 	"avb_mii",
 	"avb_gmii",
-	"avb_avtp_capture",
-	"avb_avtp_match",
-	"avb_avtp_capture_b",
-	"avb_avtp_match_b",
 };
 
 static const char * const du0_groups[] = {
@@ -4540,11 +4456,11 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		/* IP1_23_22 [2] */
 		FN_A0, FN_SCIFB1_SCK, FN_PWM3_B, 0,
 		/* IP1_21_20 [2] */
-		FN_D15, FN_SCIFA1_TXD, FN_IIC0_SDA_B, 0,
+		FN_D15, FN_SCIFA1_TXD, FN_I2C5_SDA_B, 0,
 		/* IP1_19_18 [2] */
-		FN_D14, FN_SCIFA1_RXD, FN_IIC0_SCL_B, 0,
+		FN_D14, FN_SCIFA1_RXD, FN_I2C5_SCL_B, 0,
 		/* IP1_17_15 [3] */
-		FN_D13, FN_SCIFA1_SCK, FN_TANS1, FN_PWM2_C, FN_TCLK2_B,
+		FN_D13, FN_SCIFA1_SCK, 0, FN_PWM2_C, FN_TCLK2_B,
 		0, 0, 0,
 		/* IP1_14_13 [2] */
 		FN_D12, FN_HSCIF2_HRTS_N, FN_SCIF1_TXD_C, FN_I2C1_SDA_D,
@@ -4565,19 +4481,19 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG_VAR("IPSR2", 0xE6060028, 32,
 			     2, 3, 3, 3, 3, 2, 2, 2, 2, 2, 2, 2, 2, 2) {
 		/* IP2_31_30 [2] */
-		FN_A20, FN_SPCLK, FN_MOUT1, 0,
+		FN_A20, FN_SPCLK, 0, 0,
 		/* IP2_29_27 [3] */
 		FN_A19, FN_MSIOF2_SS2, FN_PWM4, FN_TPUTO2,
-		FN_MOUT0, 0, 0, 0,
+		0, 0, 0, 0,
 		/* IP2_26_24 [3] */
 		FN_A18, FN_MSIOF2_SS1, FN_SCIF4_TXD_E, FN_CAN1_TX_B,
-		FN_AVB_AVTP_MATCH_B, 0, 0, 0,
+		0, 0, 0, 0,
 		/* IP2_23_21 [3] */
 		FN_A17, FN_MSIOF2_SYNC, FN_SCIF4_RXD_E, FN_CAN1_RX_B,
-		FN_AVB_AVTP_CAPTURE_B, 0, 0, 0,
+		0, 0, 0, 0,
 		/* IP2_20_18 [3] */
 		FN_A16, FN_MSIOF2_SCK, FN_HSCIF0_HSCK_B, FN_SPEEDIN,
-		FN_VSP, FN_CAN_CLK_C, FN_TPUTO2_B, 0,
+		0, FN_CAN_CLK_C, FN_TPUTO2_B, 0,
 		/* IP2_17_16 [2] */
 		FN_A15, FN_MSIOF2_TXD, FN_HSCIF0_HTX_B, FN_DACK1,
 		/* IP2_15_14 [2] */
@@ -4587,9 +4503,9 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		/* IP2_11_10 [2] */
 		FN_A12, FN_MSIOF1_SS1, FN_SCIFA5_RXD_B, 0,
 		/* IP2_9_8 [2] */
-		FN_A11, FN_MSIOF1_SYNC, FN_IIC1_SDA_B, 0,
+		FN_A11, FN_MSIOF1_SYNC, FN_IIC0_SDA_B, 0,
 		/* IP2_7_6 [2] */
-		FN_A10, FN_MSIOF1_SCK, FN_IIC1_SCL_B, 0,
+		FN_A10, FN_MSIOF1_SCK, FN_IIC0_SCL_B, 0,
 		/* IP2_5_4 [2] */
 		FN_A9, FN_MSIOF1_TXD, FN_SCIFA0_TXD_B, 0,
 		/* IP2_3_2 [2] */
@@ -4605,19 +4521,19 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_RD_N, FN_ATACS11_N,
 		/* IP3_29_27 [3] */
 		FN_BS_N, FN_DRACK0, FN_PWM1_C, FN_TPUTO0_C, FN_ATACS01_N,
-		FN_MTS_N_B, 0, 0,
+		0, 0, 0,
 		/* IP3_26_24 [3] */
 		FN_EX_CS5_N, FN_SCIFA2_TXD, FN_I2C2_SDA_E, FN_TS_SPSYNC_B,
-		FN_RIF0_D1, FN_FMIN, FN_SCIFB2_RTS_N, FN_STM_N_B,
+		0, FN_FMIN, FN_SCIFB2_RTS_N, 0,
 		/* IP3_23_21 [3] */
 		FN_EX_CS4_N, FN_SCIFA2_RXD, FN_I2C2_SCL_E, FN_TS_SDEN_B,
-		FN_RIF0_D0, FN_FMCLK, FN_SCIFB2_CTS_N, FN_SCKZ_B,
+		0, FN_FMCLK, FN_SCIFB2_CTS_N, 0,
 		/* IP3_20_18 [3] */
 		FN_EX_CS3_N, FN_SCIFA2_SCK, FN_SCIF4_TXD_C, FN_TS_SCK_B,
-		FN_RIF0_CLK, FN_BPFCLK, FN_SCIFB2_SCK, FN_MDATA_B,
+		0, FN_BPFCLK, FN_SCIFB2_SCK, 0,
 		/* IP3_17_15 [3] */
 		FN_EX_CS2_N, FN_PWM0, FN_SCIF4_RXD_C, FN_TS_SDATA_B,
-		FN_RIF0_SYNC, FN_TPUTO3, FN_SCIFB2_TXD, FN_SDATA_B,
+		0, FN_TPUTO3, FN_SCIFB2_TXD, 0,
 		/* IP3_14_13 [2] */
 		FN_EX_CS1_N, FN_TPUTO3_B, FN_SCIFB2_RXD, FN_VI1_DATA11,
 		/* IP3_12 [1] */
@@ -4631,88 +4547,88 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		/* IP3_7_6 [2] */
 		FN_A24, FN_IO3, FN_EX_WAIT2, 0,
 		/* IP3_5_4 [2] */
-		FN_A23, FN_IO2, FN_MOUT6, FN_ATAWR1_N,
+		FN_A23, FN_IO2, 0, FN_ATAWR1_N,
 		/* IP3_3_2 [2] */
-		FN_A22, FN_MISO_IO1, FN_MOUT5, FN_ATADIR1_N,
+		FN_A22, FN_MISO_IO1, 0, FN_ATADIR1_N,
 		/* IP3_1_0 [2] */
-		FN_A21, FN_MOSI_IO0, FN_MOUT2, 0, }
+		FN_A21, FN_MOSI_IO0, 0, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR4", 0xE6060030, 32,
 			     2, 2, 2, 3, 3, 2, 2, 2, 2, 2, 2, 3, 3, 2) {
 		/* IP4_31_30 [2] */
-		FN_DU0_DG4, FN_LCDOUT12, FN_CC50_STATE12, 0,
+		FN_DU0_DG4, FN_LCDOUT12, 0, 0,
 		/* IP4_29_28 [2] */
-		FN_DU0_DG3, FN_LCDOUT11, FN_CC50_STATE11, 0,
+		FN_DU0_DG3, FN_LCDOUT11, 0, 0,
 		/* IP4_27_26 [2] */
-		FN_DU0_DG2, FN_LCDOUT10, FN_CC50_STATE10, 0,
+		FN_DU0_DG2, FN_LCDOUT10, 0, 0,
 		/* IP4_25_23 [3] */
 		FN_DU0_DG1, FN_LCDOUT9, FN_SCIFA0_TXD_C, FN_I2C3_SDA_D,
-		FN_CC50_STATE9, 0, 0, 0,
+		0, 0, 0, 0,
 		/* IP4_22_20 [3] */
 		FN_DU0_DG0, FN_LCDOUT8, FN_SCIFA0_RXD_C, FN_I2C3_SCL_D,
-		FN_CC50_STATE8, 0, 0, 0,
+		0, 0, 0, 0,
 		/* IP4_19_18 [2] */
-		FN_DU0_DR7, FN_LCDOUT23, FN_CC50_STATE7, 0,
+		FN_DU0_DR7, FN_LCDOUT23, 0, 0,
 		/* IP4_17_16 [2] */
-		FN_DU0_DR6, FN_LCDOUT22, FN_CC50_STATE6, 0,
+		FN_DU0_DR6, FN_LCDOUT22, 0, 0,
 		/* IP4_15_14 [2] */
-		FN_DU0_DR5, FN_LCDOUT21, FN_CC50_STATE5, 0,
+		FN_DU0_DR5, FN_LCDOUT21, 0, 0,
 		/* IP4_13_12 [2] */
-		FN_DU0_DR4, FN_LCDOUT20, FN_CC50_STATE4, 0,
+		FN_DU0_DR4, FN_LCDOUT20, 0, 0,
 		/* IP4_11_10 [2] */
-		FN_DU0_DR3, FN_LCDOUT19, FN_CC50_STATE3, 0,
+		FN_DU0_DR3, FN_LCDOUT19, 0, 0,
 		/* IP4_9_8 [2] */
-		FN_DU0_DR2, FN_LCDOUT18, FN_CC50_STATE2, 0,
+		FN_DU0_DR2, FN_LCDOUT18, 0, 0,
 		/* IP4_7_5 [3] */
 		FN_DU0_DR1, FN_LCDOUT17, FN_SCIF5_TXD_C, FN_I2C2_SDA_D,
-		FN_CC50_STATE1, 0, 0, 0,
+		0, 0, 0, 0,
 		/* IP4_4_2 [3] */
 		FN_DU0_DR0, FN_LCDOUT16, FN_SCIF5_RXD_C, FN_I2C2_SCL_D,
-		FN_CC50_STATE0, 0, 0, 0,
+		0, 0, 0, 0,
 		/* IP4_1_0 [2] */
-		FN_EX_WAIT0, FN_CAN_CLK_B, FN_SCIF_CLK, FN_PWMFSW0, }
+		FN_EX_WAIT0, FN_CAN_CLK_B, FN_SCIF_CLK, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR5", 0xE6060034, 32,
 			     2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 3, 3, 2, 2, 2) {
 		/* IP5_31_30 [2] */
-		FN_DU0_EXHSYNC_DU0_HSYNC, FN_QSTH_QHS, FN_CC50_STATE27, 0,
+		FN_DU0_EXHSYNC_DU0_HSYNC, FN_QSTH_QHS, 0, 0,
 		/* IP5_29_28 [2] */
-		FN_DU0_DOTCLKOUT1, FN_QSTVB_QVE, FN_CC50_STATE26, 0,
+		FN_DU0_DOTCLKOUT1, FN_QSTVB_QVE, 0, 0,
 		/* IP5_27_26 [2] */
-		FN_DU0_DOTCLKOUT0, FN_QCLK, FN_CC50_STATE25, 0,
+		FN_DU0_DOTCLKOUT0, FN_QCLK, 0, 0,
 		/* IP5_25_24 [2] */
-		FN_DU0_DOTCLKIN, FN_QSTVA_QVS, FN_CC50_STATE24, 0,
+		FN_DU0_DOTCLKIN, FN_QSTVA_QVS, 0, 0,
 		/* IP5_23_22 [2] */
-		FN_DU0_DB7, FN_LCDOUT7, FN_CC50_STATE23, 0,
+		FN_DU0_DB7, FN_LCDOUT7, 0, 0,
 		/* IP5_21_20 [2] */
-		FN_DU0_DB6, FN_LCDOUT6, FN_CC50_STATE22, 0,
+		FN_DU0_DB6, FN_LCDOUT6, 0, 0,
 		/* IP5_19_18 [2] */
-		FN_DU0_DB5, FN_LCDOUT5, FN_CC50_STATE21, 0,
+		FN_DU0_DB5, FN_LCDOUT5, 0, 0,
 		/* IP5_17_16 [2] */
-		FN_DU0_DB4, FN_LCDOUT4, FN_CC50_STATE20, 0,
+		FN_DU0_DB4, FN_LCDOUT4, 0, 0,
 		/* IP5_15_14 [2] */
-		FN_DU0_DB3, FN_LCDOUT3, FN_CC50_STATE19, 0,
+		FN_DU0_DB3, FN_LCDOUT3, 0, 0,
 		/* IP5_13_12 [2] */
-		FN_DU0_DB2, FN_LCDOUT2, FN_CC50_STATE18, 0,
+		FN_DU0_DB2, FN_LCDOUT2, 0, 0,
 		/* IP5_11_9 [3] */
 		FN_DU0_DB1, FN_LCDOUT1, FN_SCIFA4_TXD_C, FN_I2C4_SDA_D,
-		FN_CAN0_TX_C, FN_CC50_STATE17, 0, 0,
+		FN_CAN0_TX_C, 0, 0, 0,
 		/* IP5_8_6 [3] */
 		FN_DU0_DB0, FN_LCDOUT0, FN_SCIFA4_RXD_C, FN_I2C4_SCL_D,
-		FN_CAN0_RX_C, FN_CC50_STATE16, 0, 0,
+		FN_CAN0_RX_C, 0, 0, 0,
 		/* IP5_5_4 [2] */
-		FN_DU0_DG7, FN_LCDOUT15, FN_CC50_STATE15, 0,
+		FN_DU0_DG7, FN_LCDOUT15, 0, 0,
 		/* IP5_3_2 [2] */
-		FN_DU0_DG6, FN_LCDOUT14, FN_CC50_STATE14, 0,
+		FN_DU0_DG6, FN_LCDOUT14, 0, 0,
 		/* IP5_1_0 [2] */
-		FN_DU0_DG5, FN_LCDOUT13, FN_CC50_STATE13, 0, }
+		FN_DU0_DG5, FN_LCDOUT13, 0, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR6", 0xE6060038, 32,
 			     3, 3, 3, 3, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2,
 			     2, 2) {
 		/* IP6_31_29 [3] */
-		FN_ETH_MDIO, FN_VI0_G0, FN_MSIOF2_RXD_B, FN_IIC0_SCL_D,
-		FN_AVB_TX_CLK, FN_ADIDATA, FN_AD_DI, 0,
+		FN_ETH_MDIO, FN_VI0_G0, FN_MSIOF2_RXD_B, FN_I2C5_SCL_D,
+		FN_AVB_TX_CLK, FN_ADIDATA, 0, 0,
 		/* IP6_28_26 [3] */
 		FN_VI0_VSYNC_N, FN_SCIF0_TXD_B, FN_I2C0_SDA_C,
 		FN_AUDIO_CLKOUT_B, FN_AVB_TX_EN, 0, 0, 0,
@@ -4744,14 +4660,14 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		/* IP6_8 [1] */
 		FN_VI0_CLK, FN_AVB_RX_CLK,
 		/* IP6_7_6 [2] */
-		FN_DU0_CDE, FN_QPOLB, FN_CC50_STATE31, 0,
+		FN_DU0_CDE, FN_QPOLB, 0, 0,
 		/* IP6_5_4 [2] */
-		FN_DU0_DISP, FN_QPOLA, FN_CC50_STATE30, 0,
+		FN_DU0_DISP, FN_QPOLA, 0, 0,
 		/* IP6_3_2 [2] */
-		FN_DU0_EXODDF_DU0_ODDF_DISP_CDE, FN_QCPV_QDE, FN_CC50_STATE29,
+		FN_DU0_EXODDF_DU0_ODDF_DISP_CDE, FN_QCPV_QDE, 0,
 		0,
 		/* IP6_1_0 [2] */
-		FN_DU0_EXVSYNC_DU0_VSYNC, FN_QSTB_QHE, FN_CC50_STATE28, 0, }
+		FN_DU0_EXVSYNC_DU0_VSYNC, FN_QSTB_QHE, 0, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR7", 0xE606003C, 32,
 			     1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3) {
@@ -4766,10 +4682,10 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_ETH_MAGIC, FN_VI0_R1, FN_SCIF3_SCK_B, FN_AVB_TX_ER,
 		FN_SSI_SCK6_B, 0, 0, 0,
 		/* IP7_23_21 [3] */
-		FN_ETH_TX_EN, FN_VI0_R0, FN_SCIF2_TXD_C, FN_IIC1_SDA_D,
+		FN_ETH_TX_EN, FN_VI0_R0, FN_SCIF2_TXD_C, FN_IIC0_SDA_D,
 		FN_AVB_TXD7, FN_SSI_SDATA5_B, 0, 0,
 		/* IP7_20_18 [3] */
-		FN_ETH_TXD1, FN_VI0_G7, FN_SCIF2_RXD_C, FN_IIC1_SCL_D,
+		FN_ETH_TXD1, FN_VI0_G7, FN_SCIF2_RXD_C, FN_IIC0_SCL_D,
 		FN_AVB_TXD6, FN_SSI_WS5_B, 0, 0,
 		/* IP7_17_15 [3] */
 		FN_ETH_REFCLK, FN_VI0_G6, FN_SCIF2_SCK_C, FN_AVB_TXD5,
@@ -4782,25 +4698,25 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_AVB_TXD3, FN_ADICHS1, 0, 0,
 		/* IP7_8_6 [3] */
 		FN_ETH_RXD0, FN_VI0_G3, FN_MSIOF2_SYNC_B, FN_CAN0_TX_B,
-		FN_AVB_TXD2, FN_ADICHS0, FN_AD_NCS_N, 0,
+		FN_AVB_TXD2, FN_ADICHS0, 0, 0,
 		/* IP7_5_3 [3] */
 		FN_ETH_RX_ER, FN_VI0_G2, FN_MSIOF2_SCK_B, FN_CAN0_RX_B,
-		FN_AVB_TXD1, FN_ADICLK, FN_AD_CLK, 0,
+		FN_AVB_TXD1, FN_ADICLK, 0, 0,
 		/* IP7_2_0 [3] */
-		FN_ETH_CRS_DV, FN_VI0_G1, FN_MSIOF2_TXD_B, FN_IIC0_SDA_D,
-		FN_AVB_TXD0, FN_ADICS_SAMP, FN_AD_DO, 0, }
+		FN_ETH_CRS_DV, FN_VI0_G1, FN_MSIOF2_TXD_B, FN_I2C5_SDA_D,
+		FN_AVB_TXD0, FN_ADICS_SAMP, 0, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR8", 0xE6060040, 32,
 			     3, 3, 3, 3, 3, 2, 3, 3, 3, 3, 3) {
 		/* IP8_31_29 [3] */
 		FN_MSIOF0_RXD, FN_SCIF5_RXD, FN_I2C2_SCL_C, FN_DU1_DR2,
-		FN_RIF1_D0_B, FN_TS_SDEN_D, FN_FMCLK_C, FN_RDS_CLK,
+		0, FN_TS_SDEN_D, FN_FMCLK_C, 0,
 		/* IP8_28_26 [3] */
 		FN_I2C1_SDA, FN_SCIF4_TXD, FN_IRQ5, FN_DU1_DR1,
-		FN_RIF1_CLK_B, FN_TS_SCK_D, FN_BPFCLK_C, 0,
+		0, FN_TS_SCK_D, FN_BPFCLK_C, 0,
 		/* IP8_25_23 [3] */
 		FN_I2C1_SCL, FN_SCIF4_RXD, FN_PWM5_B, FN_DU1_DR0,
-		FN_RIF1_SYNC_B, FN_TS_SDATA_D, FN_TPUTO1_B, 0,
+		0, FN_TS_SDATA_D, FN_TPUTO1_B, 0,
 		/* IP8_22_20 [3] */
 		FN_I2C0_SDA, FN_SCIF0_TXD_C, FN_TPUTO0, FN_CAN_CLK,
 		FN_DVC_MUTE, FN_CAN1_TX_D, 0, 0,
@@ -4831,70 +4747,70 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		0, 0,
 		/* IP9_30_28 [3] */
 		FN_SCIF1_SCK, FN_PWM3, FN_TCLK2, FN_DU1_DG5,
-		FN_SSI_SDATA1_B, FN_CAN_TXCLK, FN_CC50_STATE34, 0,
+		FN_SSI_SDATA1_B, 0, 0, 0,
 		/* IP9_27_25 [3] */
 		FN_HSCIF1_HRTS_N, FN_SCIFA4_TXD, FN_IERX, FN_DU1_DG4,
-		FN_SSI_WS1_B, FN_CAN_STEP0, FN_CC50_STATE33, 0,
+		FN_SSI_WS1_B, 0, 0, 0,
 		/* IP9_24_22 [3] */
 		FN_HSCIF1_HCTS_N, FN_SCIFA4_RXD, FN_IECLK, FN_DU1_DG3,
-		FN_SSI_SCK1_B, FN_CAN_DEBUG_HW_TRIGGER, FN_CC50_STATE32, 0,
+		FN_SSI_SCK1_B, 0, 0, 0,
 		/* IP9_21_19 [3] */
 		FN_HSCIF1_HSCK, FN_PWM2, FN_IETX, FN_DU1_DG2,
-		FN_REMOCON_B, FN_SPEEDIN_B, FN_VSP_B, 0,
+		FN_REMOCON_B, FN_SPEEDIN_B, 0, 0,
 		/* IP9_18_17 [2] */
 		FN_HSCIF1_HTX, FN_I2C4_SDA, FN_TPUTO1, FN_DU1_DG1,
 		/* IP9_16_15 [2] */
 		FN_HSCIF1_HRX, FN_I2C4_SCL, FN_PWM6, FN_DU1_DG0,
 		/* IP9_14_12 [3] */
 		FN_MSIOF0_SS2, FN_SCIFA0_TXD, FN_TS_SPSYNC, FN_DU1_DR7,
-		FN_RIF1_D1, FN_FMIN_B, FN_RDS_DATA_B, 0,
+		0, FN_FMIN_B, 0, 0,
 		/* IP9_11_9 [3] */
 		FN_MSIOF0_SS1, FN_SCIFA0_RXD, FN_TS_SDEN, FN_DU1_DR6,
-		FN_RIF1_D0, FN_FMCLK_B, FN_RDS_CLK_B, 0,
+		0, FN_FMCLK_B, 0, 0,
 		/* IP9_8_6 [3] */
 		FN_MSIOF0_SYNC, FN_PWM1, FN_TS_SCK, FN_DU1_DR5,
-		FN_RIF1_CLK, FN_BPFCLK_B, 0, 0,
+		0, FN_BPFCLK_B, 0, 0,
 		/* IP9_5_3 [3] */
 		FN_MSIOF0_SCK, FN_IRQ0, FN_TS_SDATA, FN_DU1_DR4,
-		FN_RIF1_SYNC, FN_TPUTO1_C, 0, 0,
+		0, FN_TPUTO1_C, 0, 0,
 		/* IP9_2_0 [3] */
 		FN_MSIOF0_TXD, FN_SCIF5_TXD, FN_I2C2_SDA_C, FN_DU1_DR3,
-		FN_RIF1_D1_B, FN_TS_SPSYNC_D, FN_FMIN_C, FN_RDS_DATA, }
+		0, FN_TS_SPSYNC_D, FN_FMIN_C, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR10", 0xE6060048, 32,
 			     2, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3) {
 		/* IP10_31_30 [2] */
-		FN_SSI_SCK5, FN_SCIFA3_SCK, FN_DU1_DOTCLKIN, FN_CAN_DEBUGOUT10,
+		FN_SSI_SCK5, FN_SCIFA3_SCK, FN_DU1_DOTCLKIN, 0,
 		/* IP10_29_27 [3] */
 		FN_I2C2_SDA, FN_SCIFA5_TXD, FN_DU1_DB7, FN_AUDIO_CLKOUT_C,
-		FN_CAN_DEBUGOUT9, 0, 0, 0,
+		0, 0, 0, 0,
 		/* IP10_26_24 [3] */
 		FN_I2C2_SCL, FN_SCIFA5_RXD, FN_DU1_DB6, FN_AUDIO_CLKC_C,
-		FN_SSI_SDATA4_B, FN_CAN_DEBUGOUT8, 0, 0,
+		FN_SSI_SDATA4_B, 0, 0, 0,
 		/* IP10_23_21 [3] */
 		FN_SCIF3_TXD, FN_I2C1_SDA_E, FN_FMIN_D, FN_DU1_DB5,
-		FN_AUDIO_CLKB_C, FN_SSI_WS4_B, FN_CAN_DEBUGOUT7, FN_RDS_DATA_C,
+		FN_AUDIO_CLKB_C, FN_SSI_WS4_B, 0, 0,
 		/* IP10_20_18 [3] */
 		FN_SCIF3_RXD, FN_I2C1_SCL_E, FN_FMCLK_D, FN_DU1_DB4,
-		FN_AUDIO_CLKA_C, FN_SSI_SCK4_B, FN_CAN_DEBUGOUT6, FN_RDS_CLK_C,
+		FN_AUDIO_CLKA_C, FN_SSI_SCK4_B, 0, 0,
 		/* IP10_17_15 [3] */
 		FN_SCIF3_SCK, FN_IRQ2, FN_BPFCLK_D, FN_DU1_DB3,
-		FN_SSI_SDATA9_B, FN_TANS2, FN_CAN_DEBUGOUT5, FN_CC50_OSCOUT,
+		FN_SSI_SDATA9_B, 0, 0, 0,
 		/* IP10_14_12 [3] */
 		FN_SCIF2_SCK, FN_IRQ1, FN_DU1_DB2, FN_SSI_WS9_B,
-		FN_USB0_IDIN, FN_CAN_DEBUGOUT4, FN_CC50_STATE39, 0,
+		0, 0, 0, 0,
 		/* IP10_11_9 [3] */
-		FN_SCIF2_TXD, FN_IIC1_SDA, FN_DU1_DB1, FN_SSI_SCK9_B,
-		FN_USB0_OVC1, FN_CAN_DEBUGOUT3, FN_CC50_STATE38, 0,
+		FN_SCIF2_TXD, FN_IIC0_SDA, FN_DU1_DB1, FN_SSI_SCK9_B,
+		0, 0, 0, 0,
 		/* IP10_8_6 [3] */
-		FN_SCIF2_RXD, FN_IIC1_SCL, FN_DU1_DB0, FN_SSI_SDATA2_B,
-		FN_USB0_EXTLP, FN_CAN_DEBUGOUT2, FN_CC50_STATE37, 0,
+		FN_SCIF2_RXD, FN_IIC0_SCL, FN_DU1_DB0, FN_SSI_SDATA2_B,
+		0, 0, 0, 0,
 		/* IP10_5_3 [3] */
-		FN_SCIF1_TXD, FN_IIC0_SDA, FN_DU1_DG7, FN_SSI_WS2_B,
-		FN_CAN_DEBUGOUT1, FN_CC50_STATE36, 0, 0,
+		FN_SCIF1_TXD, FN_I2C5_SDA, FN_DU1_DG7, FN_SSI_WS2_B,
+		0, 0, 0, 0,
 		/* IP10_2_0 [3] */
-		FN_SCIF1_RXD, FN_IIC0_SCL, FN_DU1_DG6, FN_SSI_SCK2_B,
-		FN_CAN_DEBUGOUT0, FN_CC50_STATE35, 0, 0, }
+		FN_SCIF1_RXD, FN_I2C5_SCL, FN_DU1_DG6, FN_SSI_SCK2_B,
+		0, 0, 0, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR11", 0xE606004C, 32,
 			     2, 3, 3, 3, 3, 2, 2, 3, 3, 2, 3, 3) {
@@ -4902,61 +4818,60 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		0, 0, 0, 0,
 		/* IP11_29_27 [3] */
 		FN_SSI_SDATA0, FN_MSIOF1_SCK_B, FN_PWM0_B, FN_ADICLK_B,
-		FN_AD_CLK_B, 0, 0, 0,
+		0, 0, 0, 0,
 		/* IP11_26_24 [3] */
 		FN_SSI_WS0129, FN_MSIOF1_TXD_B, FN_SCIF5_TXD_D, FN_ADICS_SAMP_B,
-		FN_AD_DO_B, 0, 0, 0,
+		0, 0, 0, 0,
 		/* IP11_23_21 [3] */
 		FN_SSI_SCK0129, FN_MSIOF1_RXD_B, FN_SCIF5_RXD_D, FN_ADIDATA_B,
-		FN_AD_DI_B, FN_PCMWE_N, 0, 0,
+		0, 0, 0, 0,
 		/* IP11_20_18 [3] */
 		FN_SSI_SDATA7, FN_SCIFA2_TXD_B, FN_IRQ8, FN_AUDIO_CLKA_D,
-		FN_CAN_CLK_D, FN_PCMOE_N, 0, 0,
+		FN_CAN_CLK_D, 0, 0, 0,
 		/* IP11_17_16 [2] */
-		FN_SSI_WS78, FN_SCIFA2_RXD_B, FN_IIC0_SCL_C, FN_DU1_CDE,
+		FN_SSI_WS78, FN_SCIFA2_RXD_B, FN_I2C5_SCL_C, FN_DU1_CDE,
 		/* IP11_15_14 [2] */
-		FN_SSI_SCK78, FN_SCIFA2_SCK_B, FN_IIC0_SDA_C, FN_DU1_DISP,
+		FN_SSI_SCK78, FN_SCIFA2_SCK_B, FN_I2C5_SDA_C, FN_DU1_DISP,
 		/* IP11_13_11 [3] */
 		FN_SSI_SDATA6, FN_SCIFA1_TXD_B, FN_I2C4_SDA_C,
-		FN_DU1_EXODDF_DU1_ODDF_DISP_CDE, FN_CAN_DEBUGOUT15, 0, 0, 0,
+		FN_DU1_EXODDF_DU1_ODDF_DISP_CDE, 0, 0, 0, 0,
 		/* IP11_10_8 [3] */
 		FN_SSI_WS6, FN_SCIFA1_RXD_B, FN_I2C4_SCL_C,
-		FN_DU1_EXVSYNC_DU1_VSYNC, FN_CAN_DEBUGOUT14, 0, 0, 0,
+		FN_DU1_EXVSYNC_DU1_VSYNC, 0, 0, 0, 0,
 		/* IP11_7_6 [2] */
-		FN_SSI_SCK6, FN_SCIFA1_SCK_B, FN_DU1_EXHSYNC_DU1_HSYNC,
-		FN_CAN_DEBUGOUT13,
+		FN_SSI_SCK6, FN_SCIFA1_SCK_B, FN_DU1_EXHSYNC_DU1_HSYNC, 0,
 		/* IP11_5_3 [3] */
 		FN_SSI_SDATA5, FN_SCIFA3_TXD, FN_I2C3_SDA_C, FN_DU1_DOTCLKOUT1,
-		FN_CAN_DEBUGOUT12, 0, 0, 0,
+		0, 0, 0, 0,
 		/* IP11_2_0 [3] */
 		FN_SSI_WS5, FN_SCIFA3_RXD, FN_I2C3_SCL_C, FN_DU1_DOTCLKOUT0,
-		FN_CAN_DEBUGOUT11, 0, 0, 0, }
+		0, 0, 0, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR12", 0xE6060050, 32,
 			     2, 3, 3, 3, 3, 3, 2, 2, 2, 3, 3, 3) {
 		/* IP12_31_30 [2] */
 		0, 0, 0, 0,
 		/* IP12_29_27 [3] */
-		FN_SSI_SCK2, FN_HSCIF1_HTX_B, FN_VI1_DATA2, FN_MDATA,
+		FN_SSI_SCK2, FN_HSCIF1_HTX_B, FN_VI1_DATA2, 0,
 		FN_ATAG0_N, FN_ETH_RXD1_B, 0, 0,
 		/* IP12_26_24 [3] */
-		FN_SSI_SDATA1, FN_HSCIF1_HRX_B, FN_VI1_DATA1, FN_SDATA,
+		FN_SSI_SDATA1, FN_HSCIF1_HRX_B, FN_VI1_DATA1, 0,
 		FN_ATAWR0_N, FN_ETH_RXD0_B, 0, 0,
 		/* IP12_23_21 [3] */
-		FN_SSI_WS1, FN_SCIF1_TXD_B, FN_IIC1_SDA_C, FN_VI1_DATA0,
-		FN_CAN0_TX_D, FN_AVB_AVTP_MATCH, FN_ETH_RX_ER_B, 0,
+		FN_SSI_WS1, FN_SCIF1_TXD_B, FN_IIC0_SDA_C, FN_VI1_DATA0,
+		FN_CAN0_TX_D, 0, FN_ETH_RX_ER_B, 0,
 		/* IP12_20_18 [3] */
-		FN_SSI_SCK1, FN_SCIF1_RXD_B, FN_IIC1_SCL_C, FN_VI1_CLK,
-		FN_CAN0_RX_D, FN_AVB_AVTP_CAPTURE, FN_ETH_CRS_DV_B, 0,
+		FN_SSI_SCK1, FN_SCIF1_RXD_B, FN_IIC0_SCL_C, FN_VI1_CLK,
+		FN_CAN0_RX_D, 0, FN_ETH_CRS_DV_B, 0,
 		/* IP12_17_15 [3] */
 		FN_SSI_SDATA8, FN_SCIF1_SCK_B, FN_PWM1_B, FN_IRQ9,
 		FN_REMOCON, FN_DACK2, FN_ETH_MDIO_B, 0,
 		/* IP12_14_13 [2] */
-		FN_SSI_SDATA4, FN_MLB_DAT, FN_IERX_B, FN_IRD_SCK,
+		FN_SSI_SDATA4, FN_MLB_DAT, FN_IERX_B, 0,
 		/* IP12_12_11 [2] */
-		FN_SSI_WS4, FN_MLB_SIG, FN_IECLK_B, FN_IRD_RX,
+		FN_SSI_WS4, FN_MLB_SIG, FN_IECLK_B, 0,
 		/* IP12_10_9 [2] */
-		FN_SSI_SCK4, FN_MLB_CLK, FN_IETX_B, FN_IRD_TX,
+		FN_SSI_SCK4, FN_MLB_CLK, FN_IETX_B, 0,
 		/* IP12_8_6 [3] */
 		FN_SSI_SDATA3, FN_MSIOF1_SS2_B, FN_SCIFA1_TXD_C, FN_ADICHS2_B,
 		FN_CAN1_TX_C, FN_DREQ2_N, 0, 0,
@@ -4965,7 +4880,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_CAN1_RX_C, FN_DACK1_B, 0, 0,
 		/* IP12_2_0 [3] */
 		FN_SSI_SCK34, FN_MSIOF1_SYNC_B, FN_SCIFA1_SCK_C, FN_ADICHS0_B,
-		FN_AD_NCS_N_B, FN_DREQ1_N_B, 0, 0, }
+		0, FN_DREQ1_N_B, 0, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("IPSR13", 0xE6060054, 32,
 			     1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3) {
@@ -4981,16 +4896,16 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		0, 0,
 		/* IP13_26_24 [3] */
 		FN_AUDIO_CLKOUT, FN_I2C4_SDA_B, FN_SCIFA5_TXD_D, FN_VI1_VSYNC_N,
-		FN_TS_SPSYNC_C, FN_RIF0_D1_B, FN_FMIN_E, FN_RDS_DATA_D,
+		FN_TS_SPSYNC_C, 0, FN_FMIN_E, 0,
 		/* IP13_23_21 [3] */
 		FN_AUDIO_CLKC, FN_I2C4_SCL_B, FN_SCIFA5_RXD_D, FN_VI1_HSYNC_N,
-		FN_TS_SDEN_C, FN_RIF0_D0_B, FN_FMCLK_E, FN_RDS_CLK_D,
+		FN_TS_SDEN_C, 0, FN_FMCLK_E, 0,
 		/* IP13_20_18 [3] */
 		FN_AUDIO_CLKB, FN_I2C0_SDA_B, FN_SCIFA4_TXD_D, FN_VI1_FIELD,
-		FN_TS_SCK_C, FN_RIF0_CLK_B, FN_BPFCLK_E, FN_ETH_MDC_B,
+		FN_TS_SCK_C, 0, FN_BPFCLK_E, FN_ETH_MDC_B,
 		/* IP13_17_15 [3] */
 		FN_AUDIO_CLKA, FN_I2C0_SCL_B, FN_SCIFA4_RXD_D, FN_VI1_CLKENB,
-		FN_TS_SDATA_C, FN_RIF0_SYNC_B, FN_ETH_TXD0_B, 0,
+		FN_TS_SDATA_C, 0, FN_ETH_TXD0_B, 0,
 		/* IP13_14_12 [3] */
 		FN_SSI_SDATA9, FN_SCIF2_TXD_B, FN_I2C3_SDA_E, FN_VI1_DATA7,
 		FN_ATADIR0_N, FN_ETH_MAGIC_B, 0, 0,
@@ -4999,38 +4914,32 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_ATARD0_N, FN_ETH_TX_EN_B, 0, 0,
 		/* IP13_8_6 [3] */
 		FN_SSI_SCK9, FN_SCIF2_SCK_B, FN_PWM2_B, FN_VI1_DATA5,
-		FN_MTS_N, FN_EX_WAIT1, FN_ETH_TXD1_B, 0,
+		0, FN_EX_WAIT1, FN_ETH_TXD1_B, 0,
 		/* IP13_5_3 [2] */
 		FN_SSI_SDATA2, FN_HSCIF1_HRTS_N_B, FN_SCIFA0_TXD_D,
-		FN_VI1_DATA4, FN_STM_N, FN_ATACS10_N, FN_ETH_REFCLK_B, 0,
+		FN_VI1_DATA4, 0, FN_ATACS10_N, FN_ETH_REFCLK_B, 0,
 		/* IP13_2_0 [3] */
 		FN_SSI_WS2, FN_HSCIF1_HCTS_N_B, FN_SCIFA0_RXD_D, FN_VI1_DATA3,
-		FN_SCKZ, FN_ATACS00_N, FN_ETH_LINK_B, 0, }
+		0, FN_ATACS00_N, FN_ETH_LINK_B, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("MOD_SEL", 0xE6060090, 32,
-			     2, 1, 2, 3, 1, 1, 1, 1, 1, 1, 3, 3, 3, 3, 3,
+			     2, 1, 2, 3, 4, 1, 1, 3, 3, 3, 3, 3,
 			     2, 1) {
 		/* SEL_ADG [2] */
 		FN_SEL_ADG_0, FN_SEL_ADG_1, FN_SEL_ADG_2, FN_SEL_ADG_3,
-		/* SEL_ADI [1] */
-		FN_SEL_ADI_0, FN_SEL_ADI_1,
+		/* RESERVED [1] */
+		0, 0,
 		/* SEL_CAN [2] */
 		FN_SEL_CAN_0, FN_SEL_CAN_1, FN_SEL_CAN_2, FN_SEL_CAN_3,
 		/* SEL_DARC [3] */
 		FN_SEL_DARC_0, FN_SEL_DARC_1, FN_SEL_DARC_2, FN_SEL_DARC_3,
 		FN_SEL_DARC_4, 0, 0, 0,
-		/* SEL_DR0 [1] */
-		FN_SEL_DR0_0, FN_SEL_DR0_1,
-		/* SEL_DR1 [1] */
-		FN_SEL_DR1_0, FN_SEL_DR1_1,
-		/* SEL_DR2 [1] */
-		FN_SEL_DR2_0, FN_SEL_DR2_1,
-		/* SEL_DR3 [1] */
-		FN_SEL_DR3_0, FN_SEL_DR3_1,
+		/* RESERVED [4] */
+		0, 0, 0, 0, 0, 0, 0, 0,	0, 0, 0, 0, 0, 0, 0, 0,
 		/* SEL_ETH [1] */
 		FN_SEL_ETH_0, FN_SEL_ETH_1,
-		/* SLE_FSN [1] */
-		FN_SEL_FSN_0, FN_SEL_FSN_1,
+		/* RESERVED [1] */
+		0, 0,
 		/* SEL_IC200 [3] */
 		FN_SEL_I2C00_0, FN_SEL_I2C00_1, FN_SEL_I2C00_2, FN_SEL_I2C00_3,
 		FN_SEL_I2C00_4, 0, 0, 0,
@@ -5046,10 +4955,10 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		/* SEL_I2C04 [3] */
 		FN_SEL_I2C04_0, FN_SEL_I2C04_1, FN_SEL_I2C04_2, FN_SEL_I2C04_3,
 		FN_SEL_I2C04_4, 0, 0, 0,
-		/* SEL_IIC00 [2] */
-		FN_SEL_IIC00_0, FN_SEL_IIC00_1, FN_SEL_IIC00_2, FN_SEL_IIC00_3,
-		/* SEL_AVB [1] */
-		FN_SEL_AVB_0, FN_SEL_AVB_1, }
+		/* SEL_I2C05 [2] */
+		FN_SEL_I2C05_0, FN_SEL_I2C05_1, FN_SEL_I2C05_2, FN_SEL_I2C05_3,
+		/* RESERVED [1] */
+		0, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("MOD_SEL2", 0xE6060094, 32,
 			     2, 2, 1, 1, 1, 1, 1, 1, 2, 2, 1, 1, 2, 2, 1, 1,
@@ -5057,7 +4966,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		/* SEL_IEB [2] */
 		FN_SEL_IEB_0, FN_SEL_IEB_1, FN_SEL_IEB_2, 0,
 		/* SEL_IIC0 [2] */
-		FN_SEL_IIC01_0, FN_SEL_IIC01_1, FN_SEL_IIC01_2, FN_SEL_IIC01_3,
+		FN_SEL_IIC0_0, FN_SEL_IIC0_1, FN_SEL_IIC0_2, FN_SEL_IIC0_3,
 		/* SEL_LBS [1] */
 		FN_SEL_LBS_0, FN_SEL_LBS_1,
 		/* SEL_MSI1 [1] */
@@ -5085,8 +4994,8 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		/* SEL_SCIFA5 [2] */
 		FN_SEL_SCIFA5_0, FN_SEL_SCIFA5_1, FN_SEL_SCIFA5_2,
 		FN_SEL_SCIFA5_3,
-		/* SEL_SPDM [1] */
-		FN_SEL_SPDM_0, FN_SEL_SPDM_1,
+		/* RESERVED [1] */
+		0, 0,
 		/* SEL_TMU [1] */
 		FN_SEL_TMU_0, FN_SEL_TMU_1,
 		/* SEL_TSIF0 [2] */
@@ -5099,8 +5008,8 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_SEL_HSCIF0_0, FN_SEL_HSCIF0_1,
 		/* SEL_HSCIF1 [1] */
 		FN_SEL_HSCIF1_0, FN_SEL_HSCIF1_1,
-		/* SEL_RDS [2] */
-		FN_SEL_RDS_0, FN_SEL_RDS_1, FN_SEL_RDS_2, FN_SEL_RDS_3, }
+		/* RESERVED [2] */
+		0, 0, 0, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("MOD_SEL3", 0xE6060098, 32,
 			     2, 2, 2, 1, 3, 2, 1, 1, 1, 1, 1, 1, 1, 1,
@@ -5185,6 +5094,28 @@ static const struct sh_pfc_soc_operations r8a7794_pinmux_ops = {
 	.pin_to_pocctrl = r8a7794_pin_to_pocctrl,
 };
 
+#ifdef CONFIG_PINCTRL_PFC_R8A7745
+const struct sh_pfc_soc_info r8a7745_pinmux_info = {
+	.name = "r8a77450_pfc",
+	.unlock_reg = 0xe6060000, /* PMMR */
+
+	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
+
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.groups = pinmux_groups,
+	.nr_groups = ARRAY_SIZE(pinmux_groups),
+	.functions = pinmux_functions,
+	.nr_functions = ARRAY_SIZE(pinmux_functions),
+
+	.cfg_regs = pinmux_config_regs,
+
+	.pinmux_data = pinmux_data,
+	.pinmux_data_size = ARRAY_SIZE(pinmux_data),
+};
+#endif
+
+#ifdef CONFIG_PINCTRL_PFC_R8A7794
 const struct sh_pfc_soc_info r8a7794_pinmux_info = {
 	.name = "r8a77940_pfc",
 	.ops = &r8a7794_pinmux_ops,
@@ -5204,3 +5135,4 @@ const struct sh_pfc_soc_info r8a7794_pinmux_info = {
 	.pinmux_data = pinmux_data,
 	.pinmux_data_size = ARRAY_SIZE(pinmux_data),
 };
+#endif
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7795-es1.c b/drivers/pinctrl/sh-pfc/pfc-r8a7795-es1.c
index 081efda9a280..95fd0994893a 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7795-es1.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7795-es1.c
@@ -192,8 +192,8 @@
 #define GPSR6_9		F_(SSI_WS4,		IP14_27_24)
 #define GPSR6_8		F_(SSI_SCK4,		IP14_23_20)
 #define GPSR6_7		F_(SSI_SDATA3,		IP14_19_16)
-#define GPSR6_6		F_(SSI_WS34,		IP14_15_12)
-#define GPSR6_5		F_(SSI_SCK34,		IP14_11_8)
+#define GPSR6_6		F_(SSI_WS349,		IP14_15_12)
+#define GPSR6_5		F_(SSI_SCK349,		IP14_11_8)
 #define GPSR6_4		F_(SSI_SDATA2_A,	IP14_7_4)
 #define GPSR6_3		F_(SSI_SDATA1_A,	IP14_3_0)
 #define GPSR6_2		F_(SSI_SDATA0,		IP13_31_28)
@@ -328,8 +328,8 @@
 #define IP13_31_28	FM(SSI_SDATA0)		F_(0, 0)	FM(MSIOF1_SS2_F)	F_(0, 0)			F_(0, 0)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP14_3_0	FM(SSI_SDATA1_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)			F_(0, 0)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP14_7_4	FM(SSI_SDATA2_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)			FM(SSI_SCK1_B)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
-#define IP14_11_8	FM(SSI_SCK34)		F_(0, 0)	FM(MSIOF1_SS1_A)	F_(0, 0)			F_(0, 0)	F_(0, 0)	FM(STP_OPWM_0_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
-#define IP14_15_12	FM(SSI_WS34)		FM(HCTS2_N_A)	FM(MSIOF1_SS2_A)	F_(0, 0)			F_(0, 0)	F_(0, 0)	FM(STP_IVCXO27_0_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
+#define IP14_11_8	FM(SSI_SCK349)		F_(0, 0)	FM(MSIOF1_SS1_A)	F_(0, 0)			F_(0, 0)	F_(0, 0)	FM(STP_OPWM_0_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
+#define IP14_15_12	FM(SSI_WS349)		FM(HCTS2_N_A)	FM(MSIOF1_SS2_A)	F_(0, 0)			F_(0, 0)	F_(0, 0)	FM(STP_IVCXO27_0_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP14_19_16	FM(SSI_SDATA3)		FM(HRTS2_N_A)	FM(MSIOF1_TXD_A)	F_(0, 0)			F_(0, 0)	FM(TS_SCK0_A)	FM(STP_ISCLK_0_A)	FM(RIF0_D1_A)	FM(RIF2_D0_A)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP14_23_20	FM(SSI_SCK4)		FM(HRX2_A)	FM(MSIOF1_SCK_A)	F_(0, 0)			F_(0, 0)	FM(TS_SDAT0_A)	FM(STP_ISD_0_A)		FM(RIF0_CLK_A)	FM(RIF2_CLK_A)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP14_27_24	FM(SSI_WS4)		FM(HTX2_A)	FM(MSIOF1_SYNC_A)	F_(0, 0)			F_(0, 0)	FM(TS_SDEN0_A)	FM(STP_ISEN_0_A)	FM(RIF0_SYNC_A)	FM(RIF2_SYNC_A)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
@@ -1256,11 +1256,11 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP14_7_4,	SSI_SDATA2_A,		SEL_SSI_0),
 	PINMUX_IPSR_MSEL(IP14_7_4,	SSI_SCK1_B,		SEL_SSI_1),
 
-	PINMUX_IPSR_GPSR(IP14_11_8,	SSI_SCK34),
+	PINMUX_IPSR_GPSR(IP14_11_8,	SSI_SCK349),
 	PINMUX_IPSR_MSEL(IP14_11_8,	MSIOF1_SS1_A,		SEL_MSIOF1_0),
 	PINMUX_IPSR_MSEL(IP14_11_8,	STP_OPWM_0_A,		SEL_SSP1_0_0),
 
-	PINMUX_IPSR_GPSR(IP14_15_12,	SSI_WS34),
+	PINMUX_IPSR_GPSR(IP14_15_12,	SSI_WS349),
 	PINMUX_IPSR_MSEL(IP14_15_12,	HCTS2_N_A,		SEL_HSCIF2_0),
 	PINMUX_IPSR_MSEL(IP14_15_12,	MSIOF1_SS2_A,		SEL_MSIOF1_0),
 	PINMUX_IPSR_MSEL(IP14_15_12,	STP_IVCXO27_0_A,	SEL_SSP1_0_0),
@@ -3650,12 +3650,12 @@ static const unsigned int ssi3_data_pins[] = {
 static const unsigned int ssi3_data_mux[] = {
 	SSI_SDATA3_MARK,
 };
-static const unsigned int ssi34_ctrl_pins[] = {
+static const unsigned int ssi349_ctrl_pins[] = {
 	/* SCK, WS */
 	RCAR_GP_PIN(6, 5), RCAR_GP_PIN(6, 6),
 };
-static const unsigned int ssi34_ctrl_mux[] = {
-	SSI_SCK34_MARK, SSI_WS34_MARK,
+static const unsigned int ssi349_ctrl_mux[] = {
+	SSI_SCK349_MARK, SSI_WS349_MARK,
 };
 static const unsigned int ssi4_data_pins[] = {
 	/* SDATA */
@@ -4063,7 +4063,7 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(ssi2_ctrl_a),
 	SH_PFC_PIN_GROUP(ssi2_ctrl_b),
 	SH_PFC_PIN_GROUP(ssi3_data),
-	SH_PFC_PIN_GROUP(ssi34_ctrl),
+	SH_PFC_PIN_GROUP(ssi349_ctrl),
 	SH_PFC_PIN_GROUP(ssi4_data),
 	SH_PFC_PIN_GROUP(ssi4_ctrl),
 	SH_PFC_PIN_GROUP(ssi5_data),
@@ -4509,7 +4509,7 @@ static const char * const ssi_groups[] = {
 	"ssi2_ctrl_a",
 	"ssi2_ctrl_b",
 	"ssi3_data",
-	"ssi34_ctrl",
+	"ssi349_ctrl",
 	"ssi4_data",
 	"ssi4_ctrl",
 	"ssi5_data",
@@ -5356,8 +5356,8 @@ static const struct pinmux_drive_reg pinmux_drive_regs[] = {
 		{ RCAR_GP_PIN(6,  2), 24, 3 },	/* SSI_SDATA0 */
 		{ RCAR_GP_PIN(6,  3), 20, 3 },	/* SSI_SDATA1 */
 		{ RCAR_GP_PIN(6,  4), 16, 3 },	/* SSI_SDATA2 */
-		{ RCAR_GP_PIN(6,  5), 12, 3 },	/* SSI_SCK34 */
-		{ RCAR_GP_PIN(6,  6),  8, 3 },	/* SSI_WS34 */
+		{ RCAR_GP_PIN(6,  5), 12, 3 },	/* SSI_SCK349 */
+		{ RCAR_GP_PIN(6,  6),  8, 3 },	/* SSI_WS349 */
 		{ RCAR_GP_PIN(6,  7),  4, 3 },	/* SSI_SDATA3 */
 		{ RCAR_GP_PIN(6,  8),  0, 3 },	/* SSI_SCK4 */
 	} },
@@ -5604,8 +5604,8 @@ static const struct sh_pfc_bias_info bias_info[] = {
 	{ RCAR_GP_PIN(6,  9),    PU5, 16 },	/* SSI_WS4 */
 	{ RCAR_GP_PIN(6,  8),    PU5, 15 },	/* SSI_SCK4 */
 	{ RCAR_GP_PIN(6,  7),    PU5, 14 },	/* SSI_SDATA3 */
-	{ RCAR_GP_PIN(6,  6),    PU5, 13 },	/* SSI_WS34 */
-	{ RCAR_GP_PIN(6,  5),    PU5, 12 },	/* SSI_SCK34 */
+	{ RCAR_GP_PIN(6,  6),    PU5, 13 },	/* SSI_WS349 */
+	{ RCAR_GP_PIN(6,  5),    PU5, 12 },	/* SSI_SCK349 */
 	{ RCAR_GP_PIN(6,  4),    PU5, 11 },	/* SSI_SDATA2_A */
 	{ RCAR_GP_PIN(6,  3),    PU5, 10 },	/* SSI_SDATA1_A */
 	{ RCAR_GP_PIN(6,  2),    PU5,  9 },	/* SSI_SDATA0 */
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7795.c b/drivers/pinctrl/sh-pfc/pfc-r8a7795.c
index 0454f31c0831..1656295af2b0 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7795.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7795.c
@@ -193,8 +193,8 @@
 #define GPSR6_9		F_(SSI_WS4,		IP15_27_24)
 #define GPSR6_8		F_(SSI_SCK4,		IP15_23_20)
 #define GPSR6_7		F_(SSI_SDATA3,		IP15_19_16)
-#define GPSR6_6		F_(SSI_WS34,		IP15_15_12)
-#define GPSR6_5		F_(SSI_SCK34,		IP15_11_8)
+#define GPSR6_6		F_(SSI_WS349,		IP15_15_12)
+#define GPSR6_5		F_(SSI_SCK349,		IP15_11_8)
 #define GPSR6_4		F_(SSI_SDATA2_A,	IP15_7_4)
 #define GPSR6_3		F_(SSI_SDATA1_A,	IP15_3_0)
 #define GPSR6_2		F_(SSI_SDATA0,		IP14_31_28)
@@ -339,8 +339,8 @@
 #define IP14_31_28	FM(SSI_SDATA0)		F_(0, 0)	FM(MSIOF1_SS2_F)	F_(0, 0)			F_(0, 0)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP15_3_0	FM(SSI_SDATA1_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)			F_(0, 0)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP15_7_4	FM(SSI_SDATA2_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)			FM(SSI_SCK1_B)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
-#define IP15_11_8	FM(SSI_SCK34)		F_(0, 0)	FM(MSIOF1_SS1_A)	F_(0, 0)			F_(0, 0)	F_(0, 0)	FM(STP_OPWM_0_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
-#define IP15_15_12	FM(SSI_WS34)		FM(HCTS2_N_A)	FM(MSIOF1_SS2_A)	F_(0, 0)			F_(0, 0)	F_(0, 0)	FM(STP_IVCXO27_0_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
+#define IP15_11_8	FM(SSI_SCK349)		F_(0, 0)	FM(MSIOF1_SS1_A)	F_(0, 0)			F_(0, 0)	F_(0, 0)	FM(STP_OPWM_0_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
+#define IP15_15_12	FM(SSI_WS349)		FM(HCTS2_N_A)	FM(MSIOF1_SS2_A)	F_(0, 0)			F_(0, 0)	F_(0, 0)	FM(STP_IVCXO27_0_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP15_19_16	FM(SSI_SDATA3)		FM(HRTS2_N_A)	FM(MSIOF1_TXD_A)	F_(0, 0)			F_(0, 0)	FM(TS_SCK0_A)	FM(STP_ISCLK_0_A)	FM(RIF0_D1_A)	FM(RIF2_D0_A)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP15_23_20	FM(SSI_SCK4)		FM(HRX2_A)	FM(MSIOF1_SCK_A)	F_(0, 0)			F_(0, 0)	FM(TS_SDAT0_A)	FM(STP_ISD_0_A)		FM(RIF0_CLK_A)	FM(RIF2_CLK_A)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP15_27_24	FM(SSI_WS4)		FM(HTX2_A)	FM(MSIOF1_SYNC_A)	F_(0, 0)			F_(0, 0)	FM(TS_SDEN0_A)	FM(STP_ISEN_0_A)	FM(RIF0_SYNC_A)	FM(RIF2_SYNC_A)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
@@ -1315,11 +1315,11 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP15_7_4,	SSI_SDATA2_A,		SEL_SSI_0),
 	PINMUX_IPSR_MSEL(IP15_7_4,	SSI_SCK1_B,		SEL_SSI_1),
 
-	PINMUX_IPSR_GPSR(IP15_11_8,	SSI_SCK34),
+	PINMUX_IPSR_GPSR(IP15_11_8,	SSI_SCK349),
 	PINMUX_IPSR_MSEL(IP15_11_8,	MSIOF1_SS1_A,		SEL_MSIOF1_0),
 	PINMUX_IPSR_MSEL(IP15_11_8,	STP_OPWM_0_A,		SEL_SSP1_0_0),
 
-	PINMUX_IPSR_GPSR(IP15_15_12,	SSI_WS34),
+	PINMUX_IPSR_GPSR(IP15_15_12,	SSI_WS349),
 	PINMUX_IPSR_MSEL(IP15_15_12,	HCTS2_N_A,		SEL_HSCIF2_0),
 	PINMUX_IPSR_MSEL(IP15_15_12,	MSIOF1_SS2_A,		SEL_MSIOF1_0),
 	PINMUX_IPSR_MSEL(IP15_15_12,	STP_IVCXO27_0_A,	SEL_SSP1_0_0),
@@ -1576,6 +1576,273 @@ static const struct sh_pfc_pin pinmux_pins[] = {
 	SH_PFC_PIN_NAMED_CFG(ROW_GROUP_A('T'), 30, ASEBRK, CFG_FLAGS),
 };
 
+/* - EtherAVB --------------------------------------------------------------- */
+static const unsigned int avb_link_pins[] = {
+	/* AVB_LINK */
+	RCAR_GP_PIN(2, 12),
+};
+static const unsigned int avb_link_mux[] = {
+	AVB_LINK_MARK,
+};
+static const unsigned int avb_magic_pins[] = {
+	/* AVB_MAGIC_ */
+	RCAR_GP_PIN(2, 10),
+};
+static const unsigned int avb_magic_mux[] = {
+	AVB_MAGIC_MARK,
+};
+static const unsigned int avb_phy_int_pins[] = {
+	/* AVB_PHY_INT */
+	RCAR_GP_PIN(2, 11),
+};
+static const unsigned int avb_phy_int_mux[] = {
+	AVB_PHY_INT_MARK,
+};
+static const unsigned int avb_mdc_pins[] = {
+	/* AVB_MDC, AVB_MDIO */
+	RCAR_GP_PIN(2, 9), PIN_NUMBER('A', 9),
+};
+static const unsigned int avb_mdc_mux[] = {
+	AVB_MDC_MARK, AVB_MDIO_MARK,
+};
+static const unsigned int avb_mii_pins[] = {
+	/*
+	 * AVB_TX_CTL, AVB_TXC, AVB_TD0,
+	 * AVB_TD1, AVB_TD2, AVB_TD3,
+	 * AVB_RX_CTL, AVB_RXC, AVB_RD0,
+	 * AVB_RD1, AVB_RD2, AVB_RD3,
+	 * AVB_TXCREFCLK
+	 */
+	PIN_NUMBER('A', 8), PIN_NUMBER('A', 19), PIN_NUMBER('A', 18),
+	PIN_NUMBER('B', 18), PIN_NUMBER('A', 17), PIN_NUMBER('B', 17),
+	PIN_NUMBER('A', 16), PIN_NUMBER('B', 19), PIN_NUMBER('A', 13),
+	PIN_NUMBER('B', 13), PIN_NUMBER('A', 14), PIN_NUMBER('B', 14),
+	PIN_NUMBER('A', 12),
+
+};
+static const unsigned int avb_mii_mux[] = {
+	AVB_TX_CTL_MARK, AVB_TXC_MARK, AVB_TD0_MARK,
+	AVB_TD1_MARK, AVB_TD2_MARK, AVB_TD3_MARK,
+	AVB_RX_CTL_MARK, AVB_RXC_MARK, AVB_RD0_MARK,
+	AVB_RD1_MARK, AVB_RD2_MARK, AVB_RD3_MARK,
+	AVB_TXCREFCLK_MARK,
+};
+static const unsigned int avb_avtp_pps_pins[] = {
+	/* AVB_AVTP_PPS */
+	RCAR_GP_PIN(2, 6),
+};
+static const unsigned int avb_avtp_pps_mux[] = {
+	AVB_AVTP_PPS_MARK,
+};
+static const unsigned int avb_avtp_match_a_pins[] = {
+	/* AVB_AVTP_MATCH_A */
+	RCAR_GP_PIN(2, 13),
+};
+static const unsigned int avb_avtp_match_a_mux[] = {
+	AVB_AVTP_MATCH_A_MARK,
+};
+static const unsigned int avb_avtp_capture_a_pins[] = {
+	/* AVB_AVTP_CAPTURE_A */
+	RCAR_GP_PIN(2, 14),
+};
+static const unsigned int avb_avtp_capture_a_mux[] = {
+	AVB_AVTP_CAPTURE_A_MARK,
+};
+static const unsigned int avb_avtp_match_b_pins[] = {
+	/*  AVB_AVTP_MATCH_B */
+	RCAR_GP_PIN(1, 8),
+};
+static const unsigned int avb_avtp_match_b_mux[] = {
+	AVB_AVTP_MATCH_B_MARK,
+};
+static const unsigned int avb_avtp_capture_b_pins[] = {
+	/* AVB_AVTP_CAPTURE_B */
+	RCAR_GP_PIN(1, 11),
+};
+static const unsigned int avb_avtp_capture_b_mux[] = {
+	AVB_AVTP_CAPTURE_B_MARK,
+};
+
+/* - DU --------------------------------------------------------------------- */
+static const unsigned int du_rgb666_pins[] = {
+	/* R[7:2], G[7:2], B[7:2] */
+	RCAR_GP_PIN(0, 15), RCAR_GP_PIN(0, 14), RCAR_GP_PIN(0, 13),
+	RCAR_GP_PIN(0, 12), RCAR_GP_PIN(0, 11), RCAR_GP_PIN(0, 10),
+	RCAR_GP_PIN(1, 15), RCAR_GP_PIN(1, 14), RCAR_GP_PIN(1, 13),
+	RCAR_GP_PIN(1, 12), RCAR_GP_PIN(1, 19), RCAR_GP_PIN(1, 18),
+	RCAR_GP_PIN(1, 7),  RCAR_GP_PIN(1, 6),  RCAR_GP_PIN(1, 5),
+	RCAR_GP_PIN(1, 4),  RCAR_GP_PIN(1, 3),  RCAR_GP_PIN(1, 2),
+};
+static const unsigned int du_rgb666_mux[] = {
+	DU_DR7_MARK, DU_DR6_MARK, DU_DR5_MARK, DU_DR4_MARK,
+	DU_DR3_MARK, DU_DR2_MARK,
+	DU_DG7_MARK, DU_DG6_MARK, DU_DG5_MARK, DU_DG4_MARK,
+	DU_DG3_MARK, DU_DG2_MARK,
+	DU_DB7_MARK, DU_DB6_MARK, DU_DB5_MARK, DU_DB4_MARK,
+	DU_DB3_MARK, DU_DB2_MARK,
+};
+static const unsigned int du_rgb888_pins[] = {
+	/* R[7:0], G[7:0], B[7:0] */
+	RCAR_GP_PIN(0, 15), RCAR_GP_PIN(0, 14), RCAR_GP_PIN(0, 13),
+	RCAR_GP_PIN(0, 12), RCAR_GP_PIN(0, 11), RCAR_GP_PIN(0, 10),
+	RCAR_GP_PIN(0, 9),  RCAR_GP_PIN(0, 8),
+	RCAR_GP_PIN(1, 15), RCAR_GP_PIN(1, 14), RCAR_GP_PIN(1, 13),
+	RCAR_GP_PIN(1, 12), RCAR_GP_PIN(1, 19), RCAR_GP_PIN(1, 18),
+	RCAR_GP_PIN(1, 17), RCAR_GP_PIN(1, 16),
+	RCAR_GP_PIN(1, 7),  RCAR_GP_PIN(1, 6),  RCAR_GP_PIN(1, 5),
+	RCAR_GP_PIN(1, 4),  RCAR_GP_PIN(1, 3),  RCAR_GP_PIN(1, 2),
+	RCAR_GP_PIN(1, 1),  RCAR_GP_PIN(1, 0),
+};
+static const unsigned int du_rgb888_mux[] = {
+	DU_DR7_MARK, DU_DR6_MARK, DU_DR5_MARK, DU_DR4_MARK,
+	DU_DR3_MARK, DU_DR2_MARK, DU_DR1_MARK, DU_DR0_MARK,
+	DU_DG7_MARK, DU_DG6_MARK, DU_DG5_MARK, DU_DG4_MARK,
+	DU_DG3_MARK, DU_DG2_MARK, DU_DG1_MARK, DU_DG0_MARK,
+	DU_DB7_MARK, DU_DB6_MARK, DU_DB5_MARK, DU_DB4_MARK,
+	DU_DB3_MARK, DU_DB2_MARK, DU_DB1_MARK, DU_DB0_MARK,
+};
+static const unsigned int du_clk_out_0_pins[] = {
+	/* CLKOUT */
+	RCAR_GP_PIN(1, 27),
+};
+static const unsigned int du_clk_out_0_mux[] = {
+	DU_DOTCLKOUT0_MARK
+};
+static const unsigned int du_clk_out_1_pins[] = {
+	/* CLKOUT */
+	RCAR_GP_PIN(2, 3),
+};
+static const unsigned int du_clk_out_1_mux[] = {
+	DU_DOTCLKOUT1_MARK
+};
+static const unsigned int du_sync_pins[] = {
+	/* EXVSYNC/VSYNC, EXHSYNC/HSYNC */
+	RCAR_GP_PIN(2, 5), RCAR_GP_PIN(2, 4),
+};
+static const unsigned int du_sync_mux[] = {
+	DU_EXVSYNC_DU_VSYNC_MARK, DU_EXHSYNC_DU_HSYNC_MARK
+};
+static const unsigned int du_oddf_pins[] = {
+	/* EXDISP/EXODDF/EXCDE */
+	RCAR_GP_PIN(2, 2),
+};
+static const unsigned int du_oddf_mux[] = {
+	DU_EXODDF_DU_ODDF_DISP_CDE_MARK,
+};
+static const unsigned int du_cde_pins[] = {
+	/* CDE */
+	RCAR_GP_PIN(2, 0),
+};
+static const unsigned int du_cde_mux[] = {
+	DU_CDE_MARK,
+};
+static const unsigned int du_disp_pins[] = {
+	/* DISP */
+	RCAR_GP_PIN(2, 1),
+};
+static const unsigned int du_disp_mux[] = {
+	DU_DISP_MARK,
+};
+
+/* - PWM0 --------------------------------------------------------------------*/
+static const unsigned int pwm0_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 6),
+};
+static const unsigned int pwm0_mux[] = {
+	PWM0_MARK,
+};
+/* - PWM1 --------------------------------------------------------------------*/
+static const unsigned int pwm1_a_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 7),
+};
+static const unsigned int pwm1_a_mux[] = {
+	PWM1_A_MARK,
+};
+static const unsigned int pwm1_b_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(1, 8),
+};
+static const unsigned int pwm1_b_mux[] = {
+	PWM1_B_MARK,
+};
+/* - PWM2 --------------------------------------------------------------------*/
+static const unsigned int pwm2_a_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 8),
+};
+static const unsigned int pwm2_a_mux[] = {
+	PWM2_A_MARK,
+};
+static const unsigned int pwm2_b_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(1, 11),
+};
+static const unsigned int pwm2_b_mux[] = {
+	PWM2_B_MARK,
+};
+/* - PWM3 --------------------------------------------------------------------*/
+static const unsigned int pwm3_a_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(1, 0),
+};
+static const unsigned int pwm3_a_mux[] = {
+	PWM3_A_MARK,
+};
+static const unsigned int pwm3_b_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 2),
+};
+static const unsigned int pwm3_b_mux[] = {
+	PWM3_B_MARK,
+};
+/* - PWM4 --------------------------------------------------------------------*/
+static const unsigned int pwm4_a_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(1, 1),
+};
+static const unsigned int pwm4_a_mux[] = {
+	PWM4_A_MARK,
+};
+static const unsigned int pwm4_b_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 3),
+};
+static const unsigned int pwm4_b_mux[] = {
+	PWM4_B_MARK,
+};
+/* - PWM5 --------------------------------------------------------------------*/
+static const unsigned int pwm5_a_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(1, 2),
+};
+static const unsigned int pwm5_a_mux[] = {
+	PWM5_A_MARK,
+};
+static const unsigned int pwm5_b_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 4),
+};
+static const unsigned int pwm5_b_mux[] = {
+	PWM5_B_MARK,
+};
+/* - PWM6 --------------------------------------------------------------------*/
+static const unsigned int pwm6_a_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(1, 3),
+};
+static const unsigned int pwm6_a_mux[] = {
+	PWM6_A_MARK,
+};
+static const unsigned int pwm6_b_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 5),
+};
+static const unsigned int pwm6_b_mux[] = {
+	PWM6_B_MARK,
+};
+
 /* - SCIF0 ------------------------------------------------------------------ */
 static const unsigned int scif0_data_pins[] = {
 	/* RX, TX */
@@ -1790,6 +2057,37 @@ static const unsigned int scif_clk_b_mux[] = {
 };
 
 static const struct sh_pfc_pin_group pinmux_groups[] = {
+	SH_PFC_PIN_GROUP(avb_link),
+	SH_PFC_PIN_GROUP(avb_magic),
+	SH_PFC_PIN_GROUP(avb_phy_int),
+	SH_PFC_PIN_GROUP(avb_mdc),
+	SH_PFC_PIN_GROUP(avb_mii),
+	SH_PFC_PIN_GROUP(avb_avtp_pps),
+	SH_PFC_PIN_GROUP(avb_avtp_match_a),
+	SH_PFC_PIN_GROUP(avb_avtp_capture_a),
+	SH_PFC_PIN_GROUP(avb_avtp_match_b),
+	SH_PFC_PIN_GROUP(avb_avtp_capture_b),
+	SH_PFC_PIN_GROUP(du_rgb666),
+	SH_PFC_PIN_GROUP(du_rgb888),
+	SH_PFC_PIN_GROUP(du_clk_out_0),
+	SH_PFC_PIN_GROUP(du_clk_out_1),
+	SH_PFC_PIN_GROUP(du_sync),
+	SH_PFC_PIN_GROUP(du_oddf),
+	SH_PFC_PIN_GROUP(du_cde),
+	SH_PFC_PIN_GROUP(du_disp),
+	SH_PFC_PIN_GROUP(pwm0),
+	SH_PFC_PIN_GROUP(pwm1_a),
+	SH_PFC_PIN_GROUP(pwm1_b),
+	SH_PFC_PIN_GROUP(pwm2_a),
+	SH_PFC_PIN_GROUP(pwm2_b),
+	SH_PFC_PIN_GROUP(pwm3_a),
+	SH_PFC_PIN_GROUP(pwm3_b),
+	SH_PFC_PIN_GROUP(pwm4_a),
+	SH_PFC_PIN_GROUP(pwm4_b),
+	SH_PFC_PIN_GROUP(pwm5_a),
+	SH_PFC_PIN_GROUP(pwm5_b),
+	SH_PFC_PIN_GROUP(pwm6_a),
+	SH_PFC_PIN_GROUP(pwm6_b),
 	SH_PFC_PIN_GROUP(scif0_data),
 	SH_PFC_PIN_GROUP(scif0_clk),
 	SH_PFC_PIN_GROUP(scif0_ctrl),
@@ -1821,6 +2119,64 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(scif_clk_b),
 };
 
+static const char * const avb_groups[] = {
+	"avb_link",
+	"avb_magic",
+	"avb_phy_int",
+	"avb_mdc",
+	"avb_mii",
+	"avb_avtp_pps",
+	"avb_avtp_match_a",
+	"avb_avtp_capture_a",
+	"avb_avtp_match_b",
+	"avb_avtp_capture_b",
+};
+
+static const char * const du_groups[] = {
+	"du_rgb666",
+	"du_rgb888",
+	"du_clk_out_0",
+	"du_clk_out_1",
+	"du_sync",
+	"du_oddf",
+	"du_cde",
+	"du_disp",
+};
+
+static const char * const pwm0_groups[] = {
+	"pwm0",
+};
+
+static const char * const pwm1_groups[] = {
+	"pwm1_a",
+	"pwm1_b",
+};
+
+static const char * const pwm2_groups[] = {
+	"pwm2_a",
+	"pwm2_b",
+};
+
+static const char * const pwm3_groups[] = {
+	"pwm3_a",
+	"pwm3_b",
+};
+
+static const char * const pwm4_groups[] = {
+	"pwm4_a",
+	"pwm4_b",
+};
+
+static const char * const pwm5_groups[] = {
+	"pwm5_a",
+	"pwm5_b",
+};
+
+static const char * const pwm6_groups[] = {
+	"pwm6_a",
+	"pwm6_b",
+};
+
 static const char * const scif0_groups[] = {
 	"scif0_data",
 	"scif0_clk",
@@ -1872,6 +2228,15 @@ static const char * const scif_clk_groups[] = {
 };
 
 static const struct sh_pfc_function pinmux_functions[] = {
+	SH_PFC_FUNCTION(avb),
+	SH_PFC_FUNCTION(du),
+	SH_PFC_FUNCTION(pwm0),
+	SH_PFC_FUNCTION(pwm1),
+	SH_PFC_FUNCTION(pwm2),
+	SH_PFC_FUNCTION(pwm3),
+	SH_PFC_FUNCTION(pwm4),
+	SH_PFC_FUNCTION(pwm5),
+	SH_PFC_FUNCTION(pwm6),
 	SH_PFC_FUNCTION(scif0),
 	SH_PFC_FUNCTION(scif1),
 	SH_PFC_FUNCTION(scif2),
@@ -2653,8 +3018,8 @@ static const struct pinmux_drive_reg pinmux_drive_regs[] = {
 		{ RCAR_GP_PIN(6,  2), 24, 3 },	/* SSI_SDATA0 */
 		{ RCAR_GP_PIN(6,  3), 20, 3 },	/* SSI_SDATA1 */
 		{ RCAR_GP_PIN(6,  4), 16, 3 },	/* SSI_SDATA2 */
-		{ RCAR_GP_PIN(6,  5), 12, 3 },	/* SSI_SCK34 */
-		{ RCAR_GP_PIN(6,  6),  8, 3 },	/* SSI_WS34 */
+		{ RCAR_GP_PIN(6,  5), 12, 3 },	/* SSI_SCK349 */
+		{ RCAR_GP_PIN(6,  6),  8, 3 },	/* SSI_WS349 */
 		{ RCAR_GP_PIN(6,  7),  4, 3 },	/* SSI_SDATA3 */
 		{ RCAR_GP_PIN(6,  8),  0, 3 },	/* SSI_SCK4 */
 	} },
@@ -2900,8 +3265,8 @@ static const struct sh_pfc_bias_info bias_info[] = {
 	{ RCAR_GP_PIN(6,  9),    PU5, 16 },	/* SSI_WS4 */
 	{ RCAR_GP_PIN(6,  8),    PU5, 15 },	/* SSI_SCK4 */
 	{ RCAR_GP_PIN(6,  7),    PU5, 14 },	/* SSI_SDATA3 */
-	{ RCAR_GP_PIN(6,  6),    PU5, 13 },	/* SSI_WS34 */
-	{ RCAR_GP_PIN(6,  5),    PU5, 12 },	/* SSI_SCK34 */
+	{ RCAR_GP_PIN(6,  6),    PU5, 13 },	/* SSI_WS349 */
+	{ RCAR_GP_PIN(6,  5),    PU5, 12 },	/* SSI_SCK349 */
 	{ RCAR_GP_PIN(6,  4),    PU5, 11 },	/* SSI_SDATA2_A */
 	{ RCAR_GP_PIN(6,  3),    PU5, 10 },	/* SSI_SDATA1_A */
 	{ RCAR_GP_PIN(6,  2),    PU5,  9 },	/* SSI_SDATA0 */
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7796.c b/drivers/pinctrl/sh-pfc/pfc-r8a7796.c
index b0362ae707e2..98bf5d0e078e 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7796.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7796.c
@@ -199,13 +199,13 @@
 #define GPSR6_9		F_(SSI_WS4,		IP15_27_24)
 #define GPSR6_8		F_(SSI_SCK4,		IP15_23_20)
 #define GPSR6_7		F_(SSI_SDATA3,		IP15_19_16)
-#define GPSR6_6		F_(SSI_WS34,		IP15_15_12)
-#define GPSR6_5		F_(SSI_SCK34,		IP15_11_8)
+#define GPSR6_6		F_(SSI_WS349,		IP15_15_12)
+#define GPSR6_5		F_(SSI_SCK349,		IP15_11_8)
 #define GPSR6_4		F_(SSI_SDATA2_A,	IP15_7_4)
 #define GPSR6_3		F_(SSI_SDATA1_A,	IP15_3_0)
 #define GPSR6_2		F_(SSI_SDATA0,		IP14_31_28)
-#define GPSR6_1		F_(SSI_WS0129,		IP14_27_24)
-#define GPSR6_0		F_(SSI_SCK0129,		IP14_23_20)
+#define GPSR6_1		F_(SSI_WS01239,		IP14_27_24)
+#define GPSR6_0		F_(SSI_SCK01239,	IP14_23_20)
 
 /* GPSR7 */
 #define GPSR7_3		FM(GP7_03)
@@ -338,15 +338,15 @@
 #define IP14_11_8	FM(MLB_CLK)		F_(0, 0)	FM(MSIOF1_SCK_F)	F_(0, 0)			FM(SCL1_B)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP14_15_12	FM(MLB_SIG)		FM(RX1_B)	FM(MSIOF1_SYNC_F)	F_(0, 0)			FM(SDA1_B)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP14_19_16	FM(MLB_DAT)		FM(TX1_B)	FM(MSIOF1_RXD_F)	F_(0, 0)			F_(0, 0)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
-#define IP14_23_20	FM(SSI_SCK0129)		F_(0, 0)	FM(MSIOF1_TXD_F)	F_(0, 0)			F_(0, 0)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
-#define IP14_27_24	FM(SSI_WS0129)		F_(0, 0)	FM(MSIOF1_SS1_F)	F_(0, 0)			F_(0, 0)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
+#define IP14_23_20	FM(SSI_SCK01239)	F_(0, 0)	FM(MSIOF1_TXD_F)	F_(0, 0)			F_(0, 0)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
+#define IP14_27_24	FM(SSI_WS01239)		F_(0, 0)	FM(MSIOF1_SS1_F)	F_(0, 0)			F_(0, 0)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 
 /* IPSRx */		/* 0 */			/* 1 */		/* 2 */			/* 3 */				/* 4 */		/* 5 */		/* 6 */			/* 7 */		/* 8 */			/* 9 */		/* A */		/* B */		/* C - F */
 #define IP14_31_28	FM(SSI_SDATA0)		F_(0, 0)	FM(MSIOF1_SS2_F)	F_(0, 0)			F_(0, 0)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP15_3_0	FM(SSI_SDATA1_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)			F_(0, 0)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP15_7_4	FM(SSI_SDATA2_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)			FM(SSI_SCK1_B)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
-#define IP15_11_8	FM(SSI_SCK34)		F_(0, 0)	FM(MSIOF1_SS1_A)	F_(0, 0)			F_(0, 0)	F_(0, 0)	FM(STP_OPWM_0_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
-#define IP15_15_12	FM(SSI_WS34)		FM(HCTS2_N_A)	FM(MSIOF1_SS2_A)	F_(0, 0)			F_(0, 0)	F_(0, 0)	FM(STP_IVCXO27_0_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
+#define IP15_11_8	FM(SSI_SCK349)		F_(0, 0)	FM(MSIOF1_SS1_A)	F_(0, 0)			F_(0, 0)	F_(0, 0)	FM(STP_OPWM_0_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
+#define IP15_15_12	FM(SSI_WS349)		FM(HCTS2_N_A)	FM(MSIOF1_SS2_A)	F_(0, 0)			F_(0, 0)	F_(0, 0)	FM(STP_IVCXO27_0_A)	F_(0, 0)	F_(0, 0)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP15_19_16	FM(SSI_SDATA3)		FM(HRTS2_N_A)	FM(MSIOF1_TXD_A)	F_(0, 0)			F_(0, 0)	FM(TS_SCK0_A)	FM(STP_ISCLK_0_A)	FM(RIF0_D1_A)	FM(RIF2_D0_A)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP15_23_20	FM(SSI_SCK4)		FM(HRX2_A)	FM(MSIOF1_SCK_A)	F_(0, 0)			F_(0, 0)	FM(TS_SDAT0_A)	FM(STP_ISD_0_A)		FM(RIF0_CLK_A)	FM(RIF2_CLK_A)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
 #define IP15_27_24	FM(SSI_WS4)		FM(HTX2_A)	FM(MSIOF1_SYNC_A)	F_(0, 0)			F_(0, 0)	FM(TS_SDEN0_A)	FM(STP_ISEN_0_A)	FM(RIF0_SYNC_A)	FM(RIF2_SYNC_A)		F_(0, 0)	F_(0, 0)	F_(0, 0)	F_(0, 0) F_(0, 0) F_(0, 0) F_(0, 0)
@@ -1304,10 +1304,10 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP14_19_16,	TX1_B,			SEL_SCIF1_1),
 	PINMUX_IPSR_MSEL(IP14_19_16,	MSIOF1_RXD_F,		SEL_MSIOF1_5),
 
-	PINMUX_IPSR_GPSR(IP14_23_20,	SSI_SCK0129),
+	PINMUX_IPSR_GPSR(IP14_23_20,	SSI_SCK01239),
 	PINMUX_IPSR_MSEL(IP14_23_20,	MSIOF1_TXD_F,		SEL_MSIOF1_5),
 
-	PINMUX_IPSR_GPSR(IP14_27_24,	SSI_WS0129),
+	PINMUX_IPSR_GPSR(IP14_27_24,	SSI_WS01239),
 	PINMUX_IPSR_MSEL(IP14_27_24,	MSIOF1_SS1_F,		SEL_MSIOF1_5),
 
 	PINMUX_IPSR_GPSR(IP14_31_28,	SSI_SDATA0),
@@ -1319,11 +1319,11 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_MSEL(IP15_7_4,	SSI_SDATA2_A,		SEL_SSI_0),
 	PINMUX_IPSR_MSEL(IP15_7_4,	SSI_SCK1_B,		SEL_SSI_1),
 
-	PINMUX_IPSR_GPSR(IP15_11_8,	SSI_SCK34),
+	PINMUX_IPSR_GPSR(IP15_11_8,	SSI_SCK349),
 	PINMUX_IPSR_MSEL(IP15_11_8,	MSIOF1_SS1_A,		SEL_MSIOF1_0),
 	PINMUX_IPSR_MSEL(IP15_11_8,	STP_OPWM_0_A,		SEL_SSP1_0_0),
 
-	PINMUX_IPSR_GPSR(IP15_15_12,	SSI_WS34),
+	PINMUX_IPSR_GPSR(IP15_15_12,	SSI_WS349),
 	PINMUX_IPSR_MSEL(IP15_15_12,	HCTS2_N_A,		SEL_HSCIF2_0),
 	PINMUX_IPSR_MSEL(IP15_15_12,	MSIOF1_SS2_A,		SEL_MSIOF1_0),
 	PINMUX_IPSR_MSEL(IP15_15_12,	STP_IVCXO27_0_A,	SEL_SSP1_0_0),
@@ -1582,6 +1582,128 @@ static const struct sh_pfc_pin pinmux_pins[] = {
 	SH_PFC_PIN_NAMED_CFG(ROW_GROUP_A('T'), 30, ASEBRK, CFG_FLAGS),
 };
 
+/* - AUDIO CLOCK ------------------------------------------------------------ */
+static const unsigned int audio_clk_a_a_pins[] = {
+	/* CLK A */
+	RCAR_GP_PIN(6, 22),
+};
+static const unsigned int audio_clk_a_a_mux[] = {
+	AUDIO_CLKA_A_MARK,
+};
+static const unsigned int audio_clk_a_b_pins[] = {
+	/* CLK A */
+	RCAR_GP_PIN(5, 4),
+};
+static const unsigned int audio_clk_a_b_mux[] = {
+	AUDIO_CLKA_B_MARK,
+};
+static const unsigned int audio_clk_a_c_pins[] = {
+	/* CLK A */
+	RCAR_GP_PIN(5, 19),
+};
+static const unsigned int audio_clk_a_c_mux[] = {
+	AUDIO_CLKA_C_MARK,
+};
+static const unsigned int audio_clk_b_a_pins[] = {
+	/* CLK B */
+	RCAR_GP_PIN(5, 12),
+};
+static const unsigned int audio_clk_b_a_mux[] = {
+	AUDIO_CLKB_A_MARK,
+};
+static const unsigned int audio_clk_b_b_pins[] = {
+	/* CLK B */
+	RCAR_GP_PIN(6, 23),
+};
+static const unsigned int audio_clk_b_b_mux[] = {
+	AUDIO_CLKB_B_MARK,
+};
+static const unsigned int audio_clk_c_a_pins[] = {
+	/* CLK C */
+	RCAR_GP_PIN(5, 21),
+};
+static const unsigned int audio_clk_c_a_mux[] = {
+	AUDIO_CLKC_A_MARK,
+};
+static const unsigned int audio_clk_c_b_pins[] = {
+	/* CLK C */
+	RCAR_GP_PIN(5, 0),
+};
+static const unsigned int audio_clk_c_b_mux[] = {
+	AUDIO_CLKC_B_MARK,
+};
+static const unsigned int audio_clkout_a_pins[] = {
+	/* CLKOUT */
+	RCAR_GP_PIN(5, 18),
+};
+static const unsigned int audio_clkout_a_mux[] = {
+	AUDIO_CLKOUT_A_MARK,
+};
+static const unsigned int audio_clkout_b_pins[] = {
+	/* CLKOUT */
+	RCAR_GP_PIN(6, 28),
+};
+static const unsigned int audio_clkout_b_mux[] = {
+	AUDIO_CLKOUT_B_MARK,
+};
+static const unsigned int audio_clkout_c_pins[] = {
+	/* CLKOUT */
+	RCAR_GP_PIN(5, 3),
+};
+static const unsigned int audio_clkout_c_mux[] = {
+	AUDIO_CLKOUT_C_MARK,
+};
+static const unsigned int audio_clkout_d_pins[] = {
+	/* CLKOUT */
+	RCAR_GP_PIN(5, 21),
+};
+static const unsigned int audio_clkout_d_mux[] = {
+	AUDIO_CLKOUT_D_MARK,
+};
+static const unsigned int audio_clkout1_a_pins[] = {
+	/* CLKOUT1 */
+	RCAR_GP_PIN(5, 15),
+};
+static const unsigned int audio_clkout1_a_mux[] = {
+	AUDIO_CLKOUT1_A_MARK,
+};
+static const unsigned int audio_clkout1_b_pins[] = {
+	/* CLKOUT1 */
+	RCAR_GP_PIN(6, 29),
+};
+static const unsigned int audio_clkout1_b_mux[] = {
+	AUDIO_CLKOUT1_B_MARK,
+};
+static const unsigned int audio_clkout2_a_pins[] = {
+	/* CLKOUT2 */
+	RCAR_GP_PIN(5, 16),
+};
+static const unsigned int audio_clkout2_a_mux[] = {
+	AUDIO_CLKOUT2_A_MARK,
+};
+static const unsigned int audio_clkout2_b_pins[] = {
+	/* CLKOUT2 */
+	RCAR_GP_PIN(6, 30),
+};
+static const unsigned int audio_clkout2_b_mux[] = {
+	AUDIO_CLKOUT2_B_MARK,
+};
+
+static const unsigned int audio_clkout3_a_pins[] = {
+	/* CLKOUT3 */
+	RCAR_GP_PIN(5, 19),
+};
+static const unsigned int audio_clkout3_a_mux[] = {
+	AUDIO_CLKOUT3_A_MARK,
+};
+static const unsigned int audio_clkout3_b_pins[] = {
+	/* CLKOUT3 */
+	RCAR_GP_PIN(6, 31),
+};
+static const unsigned int audio_clkout3_b_mux[] = {
+	AUDIO_CLKOUT3_B_MARK,
+};
+
 /* - EtherAVB --------------------------------------------------------------- */
 static const unsigned int avb_link_pins[] = {
 	/* AVB_LINK */
@@ -1605,11 +1727,33 @@ static const unsigned int avb_phy_int_mux[] = {
 	AVB_PHY_INT_MARK,
 };
 static const unsigned int avb_mdc_pins[] = {
-	/* AVB_MDC */
-	RCAR_GP_PIN(2, 9),
+	/* AVB_MDC, AVB_MDIO */
+	RCAR_GP_PIN(2, 9), PIN_NUMBER('A', 9),
 };
 static const unsigned int avb_mdc_mux[] = {
-	AVB_MDC_MARK,
+	AVB_MDC_MARK, AVB_MDIO_MARK,
+};
+static const unsigned int avb_mii_pins[] = {
+	/*
+	 * AVB_TX_CTL, AVB_TXC, AVB_TD0,
+	 * AVB_TD1, AVB_TD2, AVB_TD3,
+	 * AVB_RX_CTL, AVB_RXC, AVB_RD0,
+	 * AVB_RD1, AVB_RD2, AVB_RD3,
+	 * AVB_TXCREFCLK
+	 */
+	PIN_NUMBER('A', 8), PIN_NUMBER('A', 19), PIN_NUMBER('A', 18),
+	PIN_NUMBER('B', 18), PIN_NUMBER('A', 17), PIN_NUMBER('B', 17),
+	PIN_NUMBER('A', 16), PIN_NUMBER('B', 19), PIN_NUMBER('A', 13),
+	PIN_NUMBER('B', 13), PIN_NUMBER('A', 14), PIN_NUMBER('B', 14),
+	PIN_NUMBER('A', 12),
+
+};
+static const unsigned int avb_mii_mux[] = {
+	AVB_TX_CTL_MARK, AVB_TXC_MARK, AVB_TD0_MARK,
+	AVB_TD1_MARK, AVB_TD2_MARK, AVB_TD3_MARK,
+	AVB_RX_CTL_MARK, AVB_RXC_MARK, AVB_RD0_MARK,
+	AVB_RD1_MARK, AVB_RD2_MARK, AVB_RD3_MARK,
+	AVB_TXCREFCLK_MARK,
 };
 static const unsigned int avb_avtp_pps_pins[] = {
 	/* AVB_AVTP_PPS */
@@ -2955,6 +3099,105 @@ static const unsigned int msiof3_rxd_e_mux[] = {
 	MSIOF3_RXD_E_MARK,
 };
 
+/* - PWM0 --------------------------------------------------------------------*/
+static const unsigned int pwm0_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 6),
+};
+static const unsigned int pwm0_mux[] = {
+	PWM0_MARK,
+};
+/* - PWM1 --------------------------------------------------------------------*/
+static const unsigned int pwm1_a_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 7),
+};
+static const unsigned int pwm1_a_mux[] = {
+	PWM1_A_MARK,
+};
+static const unsigned int pwm1_b_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(1, 8),
+};
+static const unsigned int pwm1_b_mux[] = {
+	PWM1_B_MARK,
+};
+/* - PWM2 --------------------------------------------------------------------*/
+static const unsigned int pwm2_a_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 8),
+};
+static const unsigned int pwm2_a_mux[] = {
+	PWM2_A_MARK,
+};
+static const unsigned int pwm2_b_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(1, 11),
+};
+static const unsigned int pwm2_b_mux[] = {
+	PWM2_B_MARK,
+};
+/* - PWM3 --------------------------------------------------------------------*/
+static const unsigned int pwm3_a_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(1, 0),
+};
+static const unsigned int pwm3_a_mux[] = {
+	PWM3_A_MARK,
+};
+static const unsigned int pwm3_b_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 2),
+};
+static const unsigned int pwm3_b_mux[] = {
+	PWM3_B_MARK,
+};
+/* - PWM4 --------------------------------------------------------------------*/
+static const unsigned int pwm4_a_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(1, 1),
+};
+static const unsigned int pwm4_a_mux[] = {
+	PWM4_A_MARK,
+};
+static const unsigned int pwm4_b_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 3),
+};
+static const unsigned int pwm4_b_mux[] = {
+	PWM4_B_MARK,
+};
+/* - PWM5 --------------------------------------------------------------------*/
+static const unsigned int pwm5_a_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(1, 2),
+};
+static const unsigned int pwm5_a_mux[] = {
+	PWM5_A_MARK,
+};
+static const unsigned int pwm5_b_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 4),
+};
+static const unsigned int pwm5_b_mux[] = {
+	PWM5_B_MARK,
+};
+/* - PWM6 --------------------------------------------------------------------*/
+static const unsigned int pwm6_a_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(1, 3),
+};
+static const unsigned int pwm6_a_mux[] = {
+	PWM6_A_MARK,
+};
+static const unsigned int pwm6_b_pins[] = {
+	/* PWM */
+	RCAR_GP_PIN(2, 5),
+};
+static const unsigned int pwm6_b_mux[] = {
+	PWM6_B_MARK,
+};
+
 /* - SCIF0 ------------------------------------------------------------------ */
 static const unsigned int scif0_data_pins[] = {
 	/* RX, TX */
@@ -3376,11 +3619,206 @@ static const unsigned int sdhi3_ds_mux[] = {
 	SD3_DS_MARK,
 };
 
+/* - SSI -------------------------------------------------------------------- */
+static const unsigned int ssi0_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(6, 2),
+};
+static const unsigned int ssi0_data_mux[] = {
+	SSI_SDATA0_MARK,
+};
+static const unsigned int ssi01239_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(6, 0), RCAR_GP_PIN(6, 1),
+};
+static const unsigned int ssi01239_ctrl_mux[] = {
+	SSI_SCK01239_MARK, SSI_WS01239_MARK,
+};
+static const unsigned int ssi1_data_a_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(6, 3),
+};
+static const unsigned int ssi1_data_a_mux[] = {
+	SSI_SDATA1_A_MARK,
+};
+static const unsigned int ssi1_data_b_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(5, 12),
+};
+static const unsigned int ssi1_data_b_mux[] = {
+	SSI_SDATA1_B_MARK,
+};
+static const unsigned int ssi1_ctrl_a_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(6, 26), RCAR_GP_PIN(6, 27),
+};
+static const unsigned int ssi1_ctrl_a_mux[] = {
+	SSI_SCK1_A_MARK, SSI_WS1_A_MARK,
+};
+static const unsigned int ssi1_ctrl_b_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(6, 4), RCAR_GP_PIN(6, 21),
+};
+static const unsigned int ssi1_ctrl_b_mux[] = {
+	SSI_SCK1_B_MARK, SSI_WS1_B_MARK,
+};
+static const unsigned int ssi2_data_a_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(6, 4),
+};
+static const unsigned int ssi2_data_a_mux[] = {
+	SSI_SDATA2_A_MARK,
+};
+static const unsigned int ssi2_data_b_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(5, 13),
+};
+static const unsigned int ssi2_data_b_mux[] = {
+	SSI_SDATA2_B_MARK,
+};
+static const unsigned int ssi2_ctrl_a_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(5, 19), RCAR_GP_PIN(5, 21),
+};
+static const unsigned int ssi2_ctrl_a_mux[] = {
+	SSI_SCK2_A_MARK, SSI_WS2_A_MARK,
+};
+static const unsigned int ssi2_ctrl_b_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(6, 28), RCAR_GP_PIN(6, 29),
+};
+static const unsigned int ssi2_ctrl_b_mux[] = {
+	SSI_SCK2_B_MARK, SSI_WS2_B_MARK,
+};
+static const unsigned int ssi3_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(6, 7),
+};
+static const unsigned int ssi3_data_mux[] = {
+	SSI_SDATA3_MARK,
+};
+static const unsigned int ssi349_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(6, 5), RCAR_GP_PIN(6, 6),
+};
+static const unsigned int ssi349_ctrl_mux[] = {
+	SSI_SCK349_MARK, SSI_WS349_MARK,
+};
+static const unsigned int ssi4_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(6, 10),
+};
+static const unsigned int ssi4_data_mux[] = {
+	SSI_SDATA4_MARK,
+};
+static const unsigned int ssi4_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(6, 8), RCAR_GP_PIN(6, 9),
+};
+static const unsigned int ssi4_ctrl_mux[] = {
+	SSI_SCK4_MARK, SSI_WS4_MARK,
+};
+static const unsigned int ssi5_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(6, 13),
+};
+static const unsigned int ssi5_data_mux[] = {
+	SSI_SDATA5_MARK,
+};
+static const unsigned int ssi5_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(6, 11), RCAR_GP_PIN(6, 12),
+};
+static const unsigned int ssi5_ctrl_mux[] = {
+	SSI_SCK5_MARK, SSI_WS5_MARK,
+};
+static const unsigned int ssi6_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(6, 16),
+};
+static const unsigned int ssi6_data_mux[] = {
+	SSI_SDATA6_MARK,
+};
+static const unsigned int ssi6_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(6, 14), RCAR_GP_PIN(6, 15),
+};
+static const unsigned int ssi6_ctrl_mux[] = {
+	SSI_SCK6_MARK, SSI_WS6_MARK,
+};
+static const unsigned int ssi7_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(6, 19),
+};
+static const unsigned int ssi7_data_mux[] = {
+	SSI_SDATA7_MARK,
+};
+static const unsigned int ssi78_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(6, 17), RCAR_GP_PIN(6, 18),
+};
+static const unsigned int ssi78_ctrl_mux[] = {
+	SSI_SCK78_MARK, SSI_WS78_MARK,
+};
+static const unsigned int ssi8_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(6, 20),
+};
+static const unsigned int ssi8_data_mux[] = {
+	SSI_SDATA8_MARK,
+};
+static const unsigned int ssi9_data_a_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(6, 21),
+};
+static const unsigned int ssi9_data_a_mux[] = {
+	SSI_SDATA9_A_MARK,
+};
+static const unsigned int ssi9_data_b_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(5, 14),
+};
+static const unsigned int ssi9_data_b_mux[] = {
+	SSI_SDATA9_B_MARK,
+};
+static const unsigned int ssi9_ctrl_a_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(5, 15), RCAR_GP_PIN(5, 16),
+};
+static const unsigned int ssi9_ctrl_a_mux[] = {
+	SSI_SCK9_A_MARK, SSI_WS9_A_MARK,
+};
+static const unsigned int ssi9_ctrl_b_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(6, 30), RCAR_GP_PIN(6, 31),
+};
+static const unsigned int ssi9_ctrl_b_mux[] = {
+	SSI_SCK9_B_MARK, SSI_WS9_B_MARK,
+};
+
 static const struct sh_pfc_pin_group pinmux_groups[] = {
+	SH_PFC_PIN_GROUP(audio_clk_a_a),
+	SH_PFC_PIN_GROUP(audio_clk_a_b),
+	SH_PFC_PIN_GROUP(audio_clk_a_c),
+	SH_PFC_PIN_GROUP(audio_clk_b_a),
+	SH_PFC_PIN_GROUP(audio_clk_b_b),
+	SH_PFC_PIN_GROUP(audio_clk_c_a),
+	SH_PFC_PIN_GROUP(audio_clk_c_b),
+	SH_PFC_PIN_GROUP(audio_clkout_a),
+	SH_PFC_PIN_GROUP(audio_clkout_b),
+	SH_PFC_PIN_GROUP(audio_clkout_c),
+	SH_PFC_PIN_GROUP(audio_clkout_d),
+	SH_PFC_PIN_GROUP(audio_clkout1_a),
+	SH_PFC_PIN_GROUP(audio_clkout1_b),
+	SH_PFC_PIN_GROUP(audio_clkout2_a),
+	SH_PFC_PIN_GROUP(audio_clkout2_b),
+	SH_PFC_PIN_GROUP(audio_clkout3_a),
+	SH_PFC_PIN_GROUP(audio_clkout3_b),
 	SH_PFC_PIN_GROUP(avb_link),
 	SH_PFC_PIN_GROUP(avb_magic),
 	SH_PFC_PIN_GROUP(avb_phy_int),
 	SH_PFC_PIN_GROUP(avb_mdc),
+	SH_PFC_PIN_GROUP(avb_mii),
 	SH_PFC_PIN_GROUP(avb_avtp_pps),
 	SH_PFC_PIN_GROUP(avb_avtp_match_a),
 	SH_PFC_PIN_GROUP(avb_avtp_capture_a),
@@ -3565,6 +4003,19 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(msiof3_ss2_e),
 	SH_PFC_PIN_GROUP(msiof3_txd_e),
 	SH_PFC_PIN_GROUP(msiof3_rxd_e),
+	SH_PFC_PIN_GROUP(pwm0),
+	SH_PFC_PIN_GROUP(pwm1_a),
+	SH_PFC_PIN_GROUP(pwm1_b),
+	SH_PFC_PIN_GROUP(pwm2_a),
+	SH_PFC_PIN_GROUP(pwm2_b),
+	SH_PFC_PIN_GROUP(pwm3_a),
+	SH_PFC_PIN_GROUP(pwm3_b),
+	SH_PFC_PIN_GROUP(pwm4_a),
+	SH_PFC_PIN_GROUP(pwm4_b),
+	SH_PFC_PIN_GROUP(pwm5_a),
+	SH_PFC_PIN_GROUP(pwm5_b),
+	SH_PFC_PIN_GROUP(pwm6_a),
+	SH_PFC_PIN_GROUP(pwm6_b),
 	SH_PFC_PIN_GROUP(scif0_data),
 	SH_PFC_PIN_GROUP(scif0_clk),
 	SH_PFC_PIN_GROUP(scif0_ctrl),
@@ -3620,6 +4071,51 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(sdhi3_cd),
 	SH_PFC_PIN_GROUP(sdhi3_wp),
 	SH_PFC_PIN_GROUP(sdhi3_ds),
+	SH_PFC_PIN_GROUP(ssi0_data),
+	SH_PFC_PIN_GROUP(ssi01239_ctrl),
+	SH_PFC_PIN_GROUP(ssi1_data_a),
+	SH_PFC_PIN_GROUP(ssi1_data_b),
+	SH_PFC_PIN_GROUP(ssi1_ctrl_a),
+	SH_PFC_PIN_GROUP(ssi1_ctrl_b),
+	SH_PFC_PIN_GROUP(ssi2_data_a),
+	SH_PFC_PIN_GROUP(ssi2_data_b),
+	SH_PFC_PIN_GROUP(ssi2_ctrl_a),
+	SH_PFC_PIN_GROUP(ssi2_ctrl_b),
+	SH_PFC_PIN_GROUP(ssi3_data),
+	SH_PFC_PIN_GROUP(ssi349_ctrl),
+	SH_PFC_PIN_GROUP(ssi4_data),
+	SH_PFC_PIN_GROUP(ssi4_ctrl),
+	SH_PFC_PIN_GROUP(ssi5_data),
+	SH_PFC_PIN_GROUP(ssi5_ctrl),
+	SH_PFC_PIN_GROUP(ssi6_data),
+	SH_PFC_PIN_GROUP(ssi6_ctrl),
+	SH_PFC_PIN_GROUP(ssi7_data),
+	SH_PFC_PIN_GROUP(ssi78_ctrl),
+	SH_PFC_PIN_GROUP(ssi8_data),
+	SH_PFC_PIN_GROUP(ssi9_data_a),
+	SH_PFC_PIN_GROUP(ssi9_data_b),
+	SH_PFC_PIN_GROUP(ssi9_ctrl_a),
+	SH_PFC_PIN_GROUP(ssi9_ctrl_b),
+};
+
+static const char * const audio_clk_groups[] = {
+	"audio_clk_a_a",
+	"audio_clk_a_b",
+	"audio_clk_a_c",
+	"audio_clk_b_a",
+	"audio_clk_b_b",
+	"audio_clk_c_a",
+	"audio_clk_c_b",
+	"audio_clkout_a",
+	"audio_clkout_b",
+	"audio_clkout_c",
+	"audio_clkout_d",
+	"audio_clkout1_a",
+	"audio_clkout1_b",
+	"audio_clkout2_a",
+	"audio_clkout2_b",
+	"audio_clkout3_a",
+	"audio_clkout3_b",
 };
 
 static const char * const avb_groups[] = {
@@ -3627,6 +4123,7 @@ static const char * const avb_groups[] = {
 	"avb_magic",
 	"avb_phy_int",
 	"avb_mdc",
+	"avb_mii",
 	"avb_avtp_pps",
 	"avb_avtp_match_a",
 	"avb_avtp_capture_a",
@@ -3879,6 +4376,40 @@ static const char * const msiof3_groups[] = {
 	"msiof3_rxd_e",
 };
 
+static const char * const pwm0_groups[] = {
+	"pwm0",
+};
+
+static const char * const pwm1_groups[] = {
+	"pwm1_a",
+	"pwm1_b",
+};
+
+static const char * const pwm2_groups[] = {
+	"pwm2_a",
+	"pwm2_b",
+};
+
+static const char * const pwm3_groups[] = {
+	"pwm3_a",
+	"pwm3_b",
+};
+
+static const char * const pwm4_groups[] = {
+	"pwm4_a",
+	"pwm4_b",
+};
+
+static const char * const pwm5_groups[] = {
+	"pwm5_a",
+	"pwm5_b",
+};
+
+static const char * const pwm6_groups[] = {
+	"pwm6_a",
+	"pwm6_b",
+};
+
 static const char * const scif0_groups[] = {
 	"scif0_data",
 	"scif0_clk",
@@ -3967,7 +4498,36 @@ static const char * const sdhi3_groups[] = {
 	"sdhi3_ds",
 };
 
+static const char * const ssi_groups[] = {
+	"ssi0_data",
+	"ssi01239_ctrl",
+	"ssi1_data_a",
+	"ssi1_data_b",
+	"ssi1_ctrl_a",
+	"ssi1_ctrl_b",
+	"ssi2_data_a",
+	"ssi2_data_b",
+	"ssi2_ctrl_a",
+	"ssi2_ctrl_b",
+	"ssi3_data",
+	"ssi349_ctrl",
+	"ssi4_data",
+	"ssi4_ctrl",
+	"ssi5_data",
+	"ssi5_ctrl",
+	"ssi6_data",
+	"ssi6_ctrl",
+	"ssi7_data",
+	"ssi78_ctrl",
+	"ssi8_data",
+	"ssi9_data_a",
+	"ssi9_data_b",
+	"ssi9_ctrl_a",
+	"ssi9_ctrl_b",
+};
+
 static const struct sh_pfc_function pinmux_functions[] = {
+	SH_PFC_FUNCTION(audio_clk),
 	SH_PFC_FUNCTION(avb),
 	SH_PFC_FUNCTION(can0),
 	SH_PFC_FUNCTION(can1),
@@ -3991,6 +4551,13 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(msiof1),
 	SH_PFC_FUNCTION(msiof2),
 	SH_PFC_FUNCTION(msiof3),
+	SH_PFC_FUNCTION(pwm0),
+	SH_PFC_FUNCTION(pwm1),
+	SH_PFC_FUNCTION(pwm2),
+	SH_PFC_FUNCTION(pwm3),
+	SH_PFC_FUNCTION(pwm4),
+	SH_PFC_FUNCTION(pwm5),
+	SH_PFC_FUNCTION(pwm6),
 	SH_PFC_FUNCTION(scif0),
 	SH_PFC_FUNCTION(scif1),
 	SH_PFC_FUNCTION(scif2),
@@ -4002,6 +4569,7 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(sdhi1),
 	SH_PFC_FUNCTION(sdhi2),
 	SH_PFC_FUNCTION(sdhi3),
+	SH_PFC_FUNCTION(ssi),
 };
 
 static const struct pinmux_cfg_reg pinmux_config_regs[] = {
@@ -4775,8 +5343,8 @@ static const struct pinmux_drive_reg pinmux_drive_regs[] = {
 		{ RCAR_GP_PIN(6,  2), 24, 3 },	/* SSI_SDATA0 */
 		{ RCAR_GP_PIN(6,  3), 20, 3 },	/* SSI_SDATA1 */
 		{ RCAR_GP_PIN(6,  4), 16, 3 },	/* SSI_SDATA2 */
-		{ RCAR_GP_PIN(6,  5), 12, 3 },	/* SSI_SCK34 */
-		{ RCAR_GP_PIN(6,  6),  8, 3 },	/* SSI_WS34 */
+		{ RCAR_GP_PIN(6,  5), 12, 3 },	/* SSI_SCK349 */
+		{ RCAR_GP_PIN(6,  6),  8, 3 },	/* SSI_WS349 */
 		{ RCAR_GP_PIN(6,  7),  4, 3 },	/* SSI_SDATA3 */
 		{ RCAR_GP_PIN(6,  8),  0, 3 },	/* SSI_SCK4 */
 	} },
@@ -5022,8 +5590,8 @@ static const struct sh_pfc_bias_info bias_info[] = {
 	{ RCAR_GP_PIN(6,  9),    PU5, 16 },	/* SSI_WS4 */
 	{ RCAR_GP_PIN(6,  8),    PU5, 15 },	/* SSI_SCK4 */
 	{ RCAR_GP_PIN(6,  7),    PU5, 14 },	/* SSI_SDATA3 */
-	{ RCAR_GP_PIN(6,  6),    PU5, 13 },	/* SSI_WS34 */
-	{ RCAR_GP_PIN(6,  5),    PU5, 12 },	/* SSI_SCK34 */
+	{ RCAR_GP_PIN(6,  6),    PU5, 13 },	/* SSI_WS349 */
+	{ RCAR_GP_PIN(6,  5),    PU5, 12 },	/* SSI_SCK349 */
 	{ RCAR_GP_PIN(6,  4),    PU5, 11 },	/* SSI_SDATA2_A */
 	{ RCAR_GP_PIN(6,  3),    PU5, 10 },	/* SSI_SDATA1_A */
 	{ RCAR_GP_PIN(6,  2),    PU5,  9 },	/* SSI_SDATA0 */
diff --git a/drivers/pinctrl/sh-pfc/sh_pfc.h b/drivers/pinctrl/sh-pfc/sh_pfc.h
index f31eb6c1e87d..4376397123de 100644
--- a/drivers/pinctrl/sh-pfc/sh_pfc.h
+++ b/drivers/pinctrl/sh-pfc/sh_pfc.h
@@ -259,6 +259,8 @@ struct sh_pfc_soc_info {
 extern const struct sh_pfc_soc_info emev2_pinmux_info;
 extern const struct sh_pfc_soc_info r8a73a4_pinmux_info;
 extern const struct sh_pfc_soc_info r8a7740_pinmux_info;
+extern const struct sh_pfc_soc_info r8a7743_pinmux_info;
+extern const struct sh_pfc_soc_info r8a7745_pinmux_info;
 extern const struct sh_pfc_soc_info r8a7778_pinmux_info;
 extern const struct sh_pfc_soc_info r8a7779_pinmux_info;
 extern const struct sh_pfc_soc_info r8a7790_pinmux_info;
diff --git a/drivers/pinctrl/stm32/pinctrl-stm32.c b/drivers/pinctrl/stm32/pinctrl-stm32.c
index d3c5f5dfbbd7..06431ff49ffb 100644
--- a/drivers/pinctrl/stm32/pinctrl-stm32.c
+++ b/drivers/pinctrl/stm32/pinctrl-stm32.c
@@ -209,6 +209,24 @@ static int stm32_gpio_to_irq(struct gpio_chip *chip, unsigned int offset)
 	return irq_create_fwspec_mapping(&fwspec);
 }
 
+static int stm32_gpio_get_direction(struct gpio_chip *chip, unsigned int offset)
+{
+	struct stm32_gpio_bank *bank = gpiochip_get_data(chip);
+	int pin = stm32_gpio_pin(offset);
+	int ret;
+	u32 mode, alt;
+
+	stm32_pmx_get_mode(bank, pin, &mode, &alt);
+	if ((alt == 0) && (mode == 0))
+		ret = 1;
+	else if ((alt == 0) && (mode == 1))
+		ret = 0;
+	else
+		ret = -EINVAL;
+
+	return ret;
+}
+
 static const struct gpio_chip stm32_gpio_template = {
 	.request		= stm32_gpio_request,
 	.free			= stm32_gpio_free,
@@ -217,14 +235,44 @@ static const struct gpio_chip stm32_gpio_template = {
 	.direction_input	= stm32_gpio_direction_input,
 	.direction_output	= stm32_gpio_direction_output,
 	.to_irq			= stm32_gpio_to_irq,
+	.get_direction		= stm32_gpio_get_direction,
 };
 
+static int stm32_gpio_irq_request_resources(struct irq_data *irq_data)
+{
+	struct stm32_gpio_bank *bank = irq_data->domain->host_data;
+	struct stm32_pinctrl *pctl = dev_get_drvdata(bank->gpio_chip.parent);
+	int ret;
+
+	ret = stm32_gpio_direction_input(&bank->gpio_chip, irq_data->hwirq);
+	if (ret)
+		return ret;
+
+	ret = gpiochip_lock_as_irq(&bank->gpio_chip, irq_data->hwirq);
+	if (ret) {
+		dev_err(pctl->dev, "unable to lock HW IRQ %lu for IRQ\n",
+			irq_data->hwirq);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void stm32_gpio_irq_release_resources(struct irq_data *irq_data)
+{
+	struct stm32_gpio_bank *bank = irq_data->domain->host_data;
+
+	gpiochip_unlock_as_irq(&bank->gpio_chip, irq_data->hwirq);
+}
+
 static struct irq_chip stm32_gpio_irq_chip = {
 	.name           = "stm32gpio",
 	.irq_eoi	= irq_chip_eoi_parent,
 	.irq_mask       = irq_chip_mask_parent,
 	.irq_unmask     = irq_chip_unmask_parent,
 	.irq_set_type   = irq_chip_set_type_parent,
+	.irq_request_resources = stm32_gpio_irq_request_resources,
+	.irq_release_resources = stm32_gpio_irq_release_resources,
 };
 
 static int stm32_gpio_domain_translate(struct irq_domain *d,
@@ -248,15 +296,6 @@ static void stm32_gpio_domain_activate(struct irq_domain *d,
 	struct stm32_pinctrl *pctl = dev_get_drvdata(bank->gpio_chip.parent);
 
 	regmap_field_write(pctl->irqmux[irq_data->hwirq], bank->bank_nr);
-	gpiochip_lock_as_irq(&bank->gpio_chip, irq_data->hwirq);
-}
-
-static void stm32_gpio_domain_deactivate(struct irq_domain *d,
-				       struct irq_data *irq_data)
-{
-	struct stm32_gpio_bank *bank = d->host_data;
-
-	gpiochip_unlock_as_irq(&bank->gpio_chip, irq_data->hwirq);
 }
 
 static int stm32_gpio_domain_alloc(struct irq_domain *d,
@@ -285,7 +324,6 @@ static const struct irq_domain_ops stm32_gpio_domain_ops = {
 	.alloc          = stm32_gpio_domain_alloc,
 	.free           = irq_domain_free_irqs_common,
 	.activate	= stm32_gpio_domain_activate,
-	.deactivate	= stm32_gpio_domain_deactivate,
 };
 
 /* Pinctrl functions */
@@ -411,11 +449,6 @@ static int stm32_pctrl_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 		pin = STM32_GET_PIN_NO(pinfunc);
 		func = STM32_GET_PIN_FUNC(pinfunc);
 
-		if (pin >= pctl->match_data->npins) {
-			dev_err(pctl->dev, "invalid pin number.\n");
-			return -EINVAL;
-		}
-
 		if (!stm32_pctrl_is_function_valid(pctl, pin, func)) {
 			dev_err(pctl->dev, "invalid function.\n");
 			return -EINVAL;
@@ -558,8 +591,8 @@ static void stm32_pmx_set_mode(struct stm32_gpio_bank *bank,
 	clk_disable(bank->clk);
 }
 
-static void stm32_pmx_get_mode(struct stm32_gpio_bank *bank,
-		int pin, u32 *mode, u32 *alt)
+void stm32_pmx_get_mode(struct stm32_gpio_bank *bank, int pin, u32 *mode,
+			u32 *alt)
 {
 	u32 val;
 	int alt_shift = (pin % 8) * 4;
@@ -798,7 +831,7 @@ static int stm32_pconf_parse_conf(struct pinctrl_dev *pctldev,
 		break;
 	case PIN_CONFIG_OUTPUT:
 		__stm32_gpio_set(bank, offset, arg);
-		ret = stm32_pmx_gpio_set_direction(pctldev, NULL, pin, false);
+		ret = stm32_pmx_gpio_set_direction(pctldev, range, pin, false);
 		break;
 	default:
 		ret = -EINVAL;
diff --git a/drivers/pinctrl/stm32/pinctrl-stm32.h b/drivers/pinctrl/stm32/pinctrl-stm32.h
index 35ebc94c01e4..8702a9992ce5 100644
--- a/drivers/pinctrl/stm32/pinctrl-stm32.h
+++ b/drivers/pinctrl/stm32/pinctrl-stm32.h
@@ -45,7 +45,10 @@ struct stm32_pinctrl_match_data {
 	const unsigned int npins;
 };
 
-int stm32_pctl_probe(struct platform_device *pdev);
+struct stm32_gpio_bank;
 
+int stm32_pctl_probe(struct platform_device *pdev);
+void stm32_pmx_get_mode(struct stm32_gpio_bank *bank,
+			int pin, u32 *mode, u32 *alt);
 #endif /* __PINCTRL_STM32_H */
 
diff --git a/drivers/pinctrl/sunxi/Kconfig b/drivers/pinctrl/sunxi/Kconfig
index 793e6f94fa0b..31f85ca92669 100644
--- a/drivers/pinctrl/sunxi/Kconfig
+++ b/drivers/pinctrl/sunxi/Kconfig
@@ -7,7 +7,7 @@ config PINCTRL_SUNXI
 	select GPIOLIB
 
 config PINCTRL_SUN4I_A10
-	def_bool MACH_SUN4I
+	def_bool MACH_SUN4I || MACH_SUN7I
 	select PINCTRL_SUNXI
 
 config PINCTRL_SUN5I
@@ -23,10 +23,6 @@ config PINCTRL_SUN6I_A31_R
 	depends on RESET_CONTROLLER
 	select PINCTRL_SUNXI
 
-config PINCTRL_SUN7I_A20
-	def_bool MACH_SUN7I
-	select PINCTRL_SUNXI
-
 config PINCTRL_SUN8I_A23
 	def_bool MACH_SUN8I
 	select PINCTRL_SUNXI
@@ -39,6 +35,10 @@ config PINCTRL_SUN8I_A83T
 	def_bool MACH_SUN8I
 	select PINCTRL_SUNXI
 
+config PINCTRL_SUN8I_A83T_R
+	def_bool MACH_SUN8I
+	select PINCTRL_SUNXI
+
 config PINCTRL_SUN8I_A23_R
 	def_bool MACH_SUN8I
 	depends on RESET_CONTROLLER
diff --git a/drivers/pinctrl/sunxi/Makefile b/drivers/pinctrl/sunxi/Makefile
index df4ccd6cd44c..dc6c9619e41c 100644
--- a/drivers/pinctrl/sunxi/Makefile
+++ b/drivers/pinctrl/sunxi/Makefile
@@ -6,13 +6,13 @@ obj-$(CONFIG_PINCTRL_SUN4I_A10)		+= pinctrl-sun4i-a10.o
 obj-$(CONFIG_PINCTRL_SUN5I)		+= pinctrl-sun5i.o
 obj-$(CONFIG_PINCTRL_SUN6I_A31)		+= pinctrl-sun6i-a31.o
 obj-$(CONFIG_PINCTRL_SUN6I_A31_R)	+= pinctrl-sun6i-a31-r.o
-obj-$(CONFIG_PINCTRL_SUN7I_A20)		+= pinctrl-sun7i-a20.o
 obj-$(CONFIG_PINCTRL_SUN8I_A23)		+= pinctrl-sun8i-a23.o
 obj-$(CONFIG_PINCTRL_SUN8I_A23_R)	+= pinctrl-sun8i-a23-r.o
 obj-$(CONFIG_PINCTRL_SUN8I_A33)		+= pinctrl-sun8i-a33.o
 obj-$(CONFIG_PINCTRL_SUN50I_A64)	+= pinctrl-sun50i-a64.o
 obj-$(CONFIG_PINCTRL_SUN50I_A64_R)	+= pinctrl-sun50i-a64-r.o
 obj-$(CONFIG_PINCTRL_SUN8I_A83T)	+= pinctrl-sun8i-a83t.o
+obj-$(CONFIG_PINCTRL_SUN8I_A83T_R)	+= pinctrl-sun8i-a83t-r.o
 obj-$(CONFIG_PINCTRL_SUN8I_H3)		+= pinctrl-sun8i-h3.o
 obj-$(CONFIG_PINCTRL_SUN8I_H3_R)	+= pinctrl-sun8i-h3-r.o
 obj-$(CONFIG_PINCTRL_SUN8I_V3S)		+= pinctrl-sun8i-v3s.o
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c b/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
index fb30b86a97ee..159580c04b14 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
@@ -24,101 +24,147 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD3 */
 		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x4, "uart2")),	/* RTS */
+		  SUNXI_FUNCTION(0x4, "uart2"),		/* RTS */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GRXD3 */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 1),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD2 */
 		  SUNXI_FUNCTION(0x3, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "uart2")),	/* CTS */
+		  SUNXI_FUNCTION(0x4, "uart2"),		/* CTS */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GRXD2 */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 2),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD1 */
 		  SUNXI_FUNCTION(0x3, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x4, "uart2")),	/* TX */
+		  SUNXI_FUNCTION(0x4, "uart2"),		/* TX */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GRXD1 */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 3),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD0 */
 		  SUNXI_FUNCTION(0x3, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x4, "uart2")),	/* RX */
+		  SUNXI_FUNCTION(0x4, "uart2"),		/* RX */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GRXD0 */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 4),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
-		  SUNXI_FUNCTION(0x3, "spi1")),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS1 */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GTXD3 */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 5),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
-		  SUNXI_FUNCTION(0x3, "spi3")),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* CS0 */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GTXD2 */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 6),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
-		  SUNXI_FUNCTION(0x3, "spi3")),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* CLK */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GTXD1 */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 7),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
-		  SUNXI_FUNCTION(0x3, "spi3")),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* MOSI */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GTXD0 */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 8),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
-		  SUNXI_FUNCTION(0x3, "spi3")),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* MISO */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GRXCK */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 9),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
-		  SUNXI_FUNCTION(0x3, "spi3")),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* CS1 */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GNULL / ERXERR */
+					 PINCTRL_SUN7I_A20),
+		  SUNXI_FUNCTION_VARIANT(0x6, "i2s1",	/* MCLK */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 10),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXDV */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* TX */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* TX */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GRXDV */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 11),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDC */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RX */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RX */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* EMDC */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 12),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDIO */
 		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RTS */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RTS */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* EMDIO */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 13),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXEN */
 		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* CTS */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* CTS */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GTXCTL / ETXEN */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 14),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXCK */
 		  SUNXI_FUNCTION(0x3, "uart7"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* DTR */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* DTR */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GNULL / ETXCK */
+					 PINCTRL_SUN7I_A20),
+		  SUNXI_FUNCTION_VARIANT(0x6, "i2s1",	/* BCLK */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 15),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ECRS */
 		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* DSR */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* DSR */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GTXCK / ECRS */
+					 PINCTRL_SUN7I_A20),
+		  SUNXI_FUNCTION_VARIANT(0x6, "i2s1",	/* LRCK */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 16),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ECOL */
 		  SUNXI_FUNCTION(0x3, "can"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* DCD */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* DCD */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GCLKIN / ECOL */
+					 PINCTRL_SUN7I_A20),
+		  SUNXI_FUNCTION_VARIANT(0x6, "i2s1",	/* DO */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 17),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXERR */
 		  SUNXI_FUNCTION(0x3, "can"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RING */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RING */
+		  SUNXI_FUNCTION_VARIANT(0x5, "gmac",	/* GNULL / ETXERR */
+					 PINCTRL_SUN7I_A20),
+		  SUNXI_FUNCTION_VARIANT(0x6, "i2s1",	/* DI */
+					 PINCTRL_SUN7I_A20)),
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 0),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
@@ -150,47 +196,77 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 5),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* MCLK */
+		  /*
+		   * On A10 there's only one I2S controller and the pin group
+		   * is simply named "i2s". On A20 there's two and thus it's
+		   * renamed to "i2s0". Deal with these name here, in order
+		   * to satisfy existing device trees.
+		   */
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s",	/* MCLK */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s0",	/* MCLK */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x3, "ac97")),		/* MCLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 6),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* BCLK */
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s",	/* BCLK */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s0",	/* BCLK */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x3, "ac97")),		/* BCLK */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 7),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* LRCK */
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s",	/* LRCK */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s0",	/* LRCK */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x3, "ac97")),		/* SYNC */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 8),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* DO0 */
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s",	/* DO0 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s0",	/* DO0 */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x3, "ac97")),		/* DO */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 9),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s")),		/* DO1 */
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s",	/* DO1 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s0",	/* DO1 */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 10),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s")),		/* DO2 */
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s",	/* DO2 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s0",	/* DO2 */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 11),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s")),		/* DO3 */
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s",	/* DO3 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s0",	/* DO3 */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 12),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* DI */
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s",	/* DI */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x2, "i2s0",	/* DI */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x3, "ac97"),		/* DI */
-		/* Undocumented mux function - See SPDIF MCLK above */
+		/* Undocumented mux function on A10 - See SPDIF MCLK above */
 		  SUNXI_FUNCTION(0x4, "spdif")),        /* SPDIF IN */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 13),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
-		/* Undocumented mux function - See SPDIF MCLK above */
+		/* Undocumented mux function on A10 - See SPDIF MCLK above */
 		  SUNXI_FUNCTION(0x4, "spdif")),        /* SPDIF OUT */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 14),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
@@ -672,7 +748,8 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA0 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAA0 */
+					 PINCTRL_SUN4I_A10),
 		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
 		  SUNXI_FUNCTION_IRQ(0x6, 0),		/* EINT0 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* D0 */
@@ -680,7 +757,8 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA1 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAA1 */
+					 PINCTRL_SUN4I_A10),
 		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
 		  SUNXI_FUNCTION_IRQ(0x6, 1),		/* EINT1 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* D1 */
@@ -688,7 +766,8 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA2 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAA2 */
+					 PINCTRL_SUN4I_A10),
 		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
 		  SUNXI_FUNCTION_IRQ(0x6, 2),		/* EINT2 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* D2 */
@@ -696,7 +775,8 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIRQ */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAIRQ */
+					 PINCTRL_SUN4I_A10),
 		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
 		  SUNXI_FUNCTION_IRQ(0x6, 3),		/* EINT3 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* D3 */
@@ -704,7 +784,8 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD0 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD0 */
+					 PINCTRL_SUN4I_A10),
 		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
 		  SUNXI_FUNCTION_IRQ(0x6, 4),		/* EINT4 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* D4 */
@@ -712,7 +793,8 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD1 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD1 */
+					 PINCTRL_SUN4I_A10),
 		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
 		  SUNXI_FUNCTION_IRQ(0x6, 5),		/* EINT5 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* D5 */
@@ -720,7 +802,8 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD2 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD2 */
+					 PINCTRL_SUN4I_A10),
 		  SUNXI_FUNCTION(0x4, "uart5"),		/* TX */
 		  SUNXI_FUNCTION(0x5, "ms"),		/* BS */
 		  SUNXI_FUNCTION_IRQ(0x6, 6),		/* EINT6 */
@@ -729,7 +812,8 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD3 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD3 */
+					 PINCTRL_SUN4I_A10),
 		  SUNXI_FUNCTION(0x4, "uart5"),		/* RX */
 		  SUNXI_FUNCTION(0x5, "ms"),		/* CLK */
 		  SUNXI_FUNCTION_IRQ(0x6, 7),		/* EINT7 */
@@ -738,7 +822,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D8 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD4 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD4 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ERXD3 */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN0 */
 		  SUNXI_FUNCTION(0x5, "ms"),		/* D0 */
 		  SUNXI_FUNCTION_IRQ(0x6, 8),		/* EINT8 */
@@ -747,7 +834,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D9 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD5 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD5 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ERXD2 */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN1 */
 		  SUNXI_FUNCTION(0x5, "ms"),		/* D1 */
 		  SUNXI_FUNCTION_IRQ(0x6, 9),		/* EINT9 */
@@ -756,7 +846,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D10 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD6 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD6 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ERXD1 */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN2 */
 		  SUNXI_FUNCTION(0x5, "ms"),		/* D2 */
 		  SUNXI_FUNCTION_IRQ(0x6, 10),		/* EINT10 */
@@ -765,7 +858,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D11 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD7 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD7 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ERXD0 */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN3 */
 		  SUNXI_FUNCTION(0x5, "ms"),		/* D3 */
 		  SUNXI_FUNCTION_IRQ(0x6, 11),		/* EINT11 */
@@ -774,7 +870,8 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D12 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD8 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD8 */
+					 PINCTRL_SUN4I_A10),
 		  SUNXI_FUNCTION(0x4, "ps2"),		/* SCK1 */
 		  SUNXI_FUNCTION_IRQ(0x6, 12),		/* EINT12 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* D12 */
@@ -782,7 +879,8 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D13 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD9 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD9 */
+					 PINCTRL_SUN4I_A10),
 		  SUNXI_FUNCTION(0x4, "ps2"),		/* SDA1 */
 		  SUNXI_FUNCTION(0x5, "sim"),		/* RST */
 		  SUNXI_FUNCTION_IRQ(0x6, 13),		/* EINT13 */
@@ -791,7 +889,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D14 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD10 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD10 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ETXD3 */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN4 */
 		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPEN */
 		  SUNXI_FUNCTION_IRQ(0x6, 14),		/* EINT14 */
@@ -800,7 +901,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D15 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD11 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD11 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ETXD2 */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN5 */
 		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPPP */
 		  SUNXI_FUNCTION_IRQ(0x6, 15),		/* EINT15 */
@@ -809,7 +913,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D16 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD12 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD12 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ETXD1 */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN6 */
 		  SUNXI_FUNCTION_IRQ(0x6, 16),		/* EINT16 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* D16 */
@@ -817,7 +924,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D17 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD13 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD13 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ETXD0 */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN7 */
 		  SUNXI_FUNCTION(0x5, "sim"),		/* VCCEN */
 		  SUNXI_FUNCTION_IRQ(0x6, 17),		/* EINT17 */
@@ -826,7 +936,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D18 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD14 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD14 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ERXCK */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT0 */
 		  SUNXI_FUNCTION(0x5, "sim"),		/* SCK */
 		  SUNXI_FUNCTION_IRQ(0x6, 18),		/* EINT18 */
@@ -835,7 +948,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D19 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD15 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAD15 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ERXERR */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT1 */
 		  SUNXI_FUNCTION(0x5, "sim"),		/* SDA */
 		  SUNXI_FUNCTION_IRQ(0x6, 19),		/* EINT19 */
@@ -844,7 +960,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D20 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAOE */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAOE */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ERXDV */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "can"),		/* TX */
 		  SUNXI_FUNCTION_IRQ(0x6, 20),		/* EINT20 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* D20 */
@@ -852,7 +971,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D21 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATADREQ */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATADREQ */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* EMDC */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "can"),		/* RX */
 		  SUNXI_FUNCTION_IRQ(0x6, 21),		/* EINT21 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* D21 */
@@ -860,7 +982,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D22 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATADACK */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATADACK */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* EMDIO */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT2 */
 		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CMD */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* D22 */
@@ -868,7 +993,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D23 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATACS0 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATACS0 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ETXEN */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT3 */
 		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CLK */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* D23 */
@@ -876,7 +1004,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATACS1 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATACS1 */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ETXCK */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT4 */
 		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D0 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* PCLK */
@@ -884,7 +1015,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* DE */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIORDY */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAIORDY */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ECRS */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT5 */
 		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D1 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* FIELD */
@@ -892,7 +1026,10 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIOR */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAIOR */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ECOL */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT6 */
 		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D2 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* HSYNC */
@@ -900,24 +1037,35 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "lcd1"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIOW */
+		  SUNXI_FUNCTION_VARIANT(0x3, "pata",	/* ATAIOW */
+					 PINCTRL_SUN4I_A10),
+		  SUNXI_FUNCTION_VARIANT(0x3, "emac",	/* ETXERR */
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT7 */
 		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D3 */
 		  SUNXI_FUNCTION(0x7, "csi1")),		/* VSYNC */
 	/* Hole */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 0),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_VARIANT(0x3, "i2c3",	/* SCK */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 1),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_VARIANT(0x3, "i2c3",	/* SDA */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 2),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_VARIANT(0x3, "i2c4",	/* SCK */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 3),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "pwm")),		/* PWM1 */
+		  SUNXI_FUNCTION(0x2, "pwm"),		/* PWM1 */
+		  SUNXI_FUNCTION_VARIANT(0x3, "i2c3",	/* SDA */
+					 PINCTRL_SUN7I_A20)),
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 4),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
@@ -959,12 +1107,16 @@ static const struct sunxi_desc_pin sun4i_a10_pins[] = {
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "spi0"),		/* MOSI */
 		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
+		  SUNXI_FUNCTION_VARIANT(0x4, "clk_out_a",
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 13),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
 		  SUNXI_FUNCTION(0x2, "spi0"),		/* MISO */
 		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
+		  SUNXI_FUNCTION_VARIANT(0x4, "clk_out_b",
+					 PINCTRL_SUN7I_A20),
 		  SUNXI_FUNCTION_IRQ(0x6, 25)),		/* EINT25 */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 14),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
@@ -1027,12 +1179,21 @@ static const struct sunxi_pinctrl_desc sun4i_a10_pinctrl_data = {
 
 static int sun4i_a10_pinctrl_probe(struct platform_device *pdev)
 {
-	return sunxi_pinctrl_init(pdev,
-				  &sun4i_a10_pinctrl_data);
+	unsigned long variant = (unsigned long)of_device_get_match_data(&pdev->dev);
+
+	return sunxi_pinctrl_init_with_variant(pdev, &sun4i_a10_pinctrl_data,
+					       variant);
 }
 
 static const struct of_device_id sun4i_a10_pinctrl_match[] = {
-	{ .compatible = "allwinner,sun4i-a10-pinctrl", },
+	{
+		.compatible = "allwinner,sun4i-a10-pinctrl",
+		.data = (void *)PINCTRL_SUN4I_A10
+	},
+	{
+		.compatible = "allwinner,sun7i-a20-pinctrl",
+		.data = (void *)PINCTRL_SUN7I_A20
+	},
 	{}
 };
 
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c b/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c
deleted file mode 100644
index b6f4c68ffb39..000000000000
--- a/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c
+++ /dev/null
@@ -1,1056 +0,0 @@
-/*
- * Allwinner A20 SoCs pinctrl driver.
- *
- * Copyright (C) 2014 Maxime Ripard
- *
- * Maxime Ripard <maxime.ripard@free-electrons.com>
- *
- * This file is licensed under the terms of the GNU General Public
- * License version 2.  This program is licensed "as is" without any
- * warranty of any kind, whether express or implied.
- */
-
-#include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/of.h>
-#include <linux/of_device.h>
-#include <linux/pinctrl/pinctrl.h>
-
-#include "pinctrl-sunxi.h"
-
-static const struct sunxi_desc_pin sun7i_a20_pins[] = {
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 0),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD3 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x4, "uart2"),		/* RTS */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 1),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD2 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "uart2"),		/* CTS */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 2),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD1 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x4, "uart2"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 3),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD0 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x4, "uart2"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 4),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS1 */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 5),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* CS0 */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 6),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* CLK */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 7),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* MOSI */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 8),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* MISO */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 9),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* CS1 */
-		  SUNXI_FUNCTION(0x5, "gmac"),		/* GNULL / ERXERR */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 10),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXDV */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXCTL / ERXDV */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 11),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDC */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* EMDC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 12),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDIO */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* RTS */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* EMDIO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 13),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXEN */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* CTS */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXCTL / ETXEN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 14),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXCK */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* DTR */
-		  SUNXI_FUNCTION(0x5, "gmac"),		/* GNULL / ETXCK */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* BCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 15),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ECRS */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* DSR */
-		  SUNXI_FUNCTION(0x5, "gmac"),		/* GTXCK / ECRS */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* LRCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 16),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ECOL */
-		  SUNXI_FUNCTION(0x3, "can"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* DCD */
-		  SUNXI_FUNCTION(0x5, "gmac"),		/* GCLKIN / ECOL */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* DO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 17),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXERR */
-		  SUNXI_FUNCTION(0x3, "can"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* RING */
-		  SUNXI_FUNCTION(0x5, "gmac"),		/* GNULL / ETXERR */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* LRCK */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 0),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 1),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 2),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "pwm")),		/* PWM0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 3),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "spdif")),	/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 4),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0")),		/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 5),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* MCLK */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 6),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* BCLK */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* BCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 7),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* LRCK */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* SYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 8),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO0 */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* DO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 9),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 10),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 11),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 12),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DI */
-		  SUNXI_FUNCTION(0x3, "ac97"),		/* DI */
-		  SUNXI_FUNCTION(0x4, "spdif")),	/* DI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 13),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
-		  SUNXI_FUNCTION(0x4, "spdif")),	/* DO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 14),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "jtag")),		/* MS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 15),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "jtag")),		/* CK0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 16),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "jtag")),		/* DO0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 17),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "jtag")),		/* DI0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 18),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 19),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 20),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 21),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 22),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0"),		/* TX */
-		  SUNXI_FUNCTION(0x3, "ir1")),		/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 23),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0"),		/* RX */
-		  SUNXI_FUNCTION(0x3, "ir1")),		/* RX */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 0),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 1),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 2),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 3),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 4),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 5),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NRE# */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 6),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 7),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 8),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 9),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 10),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 11),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 12),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 13),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 14),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 15),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 16),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NWP */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 17),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 18),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 19),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE4 */
-		  SUNXI_FUNCTION(0x3, "spi2")),		/* CS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 20),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE5 */
-		  SUNXI_FUNCTION(0x3, "spi2")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 21),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE6 */
-		  SUNXI_FUNCTION(0x3, "spi2")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 22),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE7 */
-		  SUNXI_FUNCTION(0x3, "spi2")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 23),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 24),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQS */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 0),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 1),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 2),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 3),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 4),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 5),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 6),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VPC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 7),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 8),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D8 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 9),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D9 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VM3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 10),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 11),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 12),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 13),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 14),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 15),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 16),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D16 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VPC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 17),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D17 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 18),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 19),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
-		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 20),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D20 */
-		  SUNXI_FUNCTION(0x3, "csi1")),		/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 21),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D21 */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPEN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 22),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D22 */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPPP */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 23),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D23 */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* DET */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 24),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* VCCEN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 25),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* DE */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* RST */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 26),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 27),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x3, "sim")),		/* SDA */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 0),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* PCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 1),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* ERR */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* CK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 2),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* SYNC */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* HSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 3),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* DVLD */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* VSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 4),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 5),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "sim")),		/* VPPEN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 6),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 7),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 8),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 9),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 10),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 11),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "csi0")),		/* D7 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 0),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* MSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 1),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 2),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "uart0")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 3),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 4),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "uart0")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 5),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 0),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* PCK */
-		  SUNXI_FUNCTION(0x4, "mmc1")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 1),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* ERR */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* CK */
-		  SUNXI_FUNCTION(0x4, "mmc1")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 2),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* SYNC */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x4, "mmc1")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 3),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* DVLD */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x4, "mmc1")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 4),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "mmc1"),		/* D2 */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 5),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "mmc1"),		/* D3 */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 6),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 7),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D11 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 8),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 9),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D13 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 10),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 11),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ts1"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "csi1"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "csi0")),		/* D15 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 0),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 0),		/* EINT0 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 1),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 1),		/* EINT1 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 2),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 2),		/* EINT2 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 3),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 3),		/* EINT3 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 4),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 4),		/* EINT4 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 5),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 5),		/* EINT5 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 6),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "uart5"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* BS */
-		  SUNXI_FUNCTION_IRQ(0x6, 6),		/* EINT6 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 7),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "uart5"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* CLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 7),		/* EINT7 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 8),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D8 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD3 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN0 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 8),		/* EINT8 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 9),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D9 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD2 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN1 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 9),		/* EINT9 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 10),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D10 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD1 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN2 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D2 */
-		  SUNXI_FUNCTION_IRQ(0x6, 10),		/* EINT10 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 11),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D11 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD0 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN3 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D3 */
-		  SUNXI_FUNCTION_IRQ(0x6, 11),		/* EINT11 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D11 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 12),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D12 */
-		  SUNXI_FUNCTION(0x4, "ps2"),		/* SCK1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 12),		/* EINT12 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 13),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D13 */
-		  SUNXI_FUNCTION(0x4, "ps2"),		/* SDA1 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* RST */
-		  SUNXI_FUNCTION_IRQ(0x6, 13),		/* EINT13 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D13 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 14),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D14 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD3 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN4 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPEN */
-		  SUNXI_FUNCTION_IRQ(0x6, 14),		/* EINT14 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 15),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D15 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD3 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN5 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPPP */
-		  SUNXI_FUNCTION_IRQ(0x6, 15),		/* EINT15 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D15 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 16),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D16 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD2 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN6 */
-		  SUNXI_FUNCTION_IRQ(0x6, 16),		/* EINT16 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D16 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 17),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D17 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD1 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN7 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* VCCEN */
-		  SUNXI_FUNCTION_IRQ(0x6, 17),		/* EINT17 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D17 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 18),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D18 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD0 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT0 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* SCK */
-		  SUNXI_FUNCTION_IRQ(0x6, 18),		/* EINT18 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D18 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 19),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D19 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXERR */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT1 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* SDA */
-		  SUNXI_FUNCTION_IRQ(0x6, 19),		/* EINT19 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D19 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 20),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D20 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXDV */
-		  SUNXI_FUNCTION(0x4, "can"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 20),		/* EINT20 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D20 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 21),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D21 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* EMDC */
-		  SUNXI_FUNCTION(0x4, "can"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 21),		/* EINT21 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D21 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 22),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D22 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* EMDIO */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT2 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CMD */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 23),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D23 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXEN */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT3 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CLK */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D23 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 24),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXCK */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT4 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D0 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* PCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 25),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* DE */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ECRS */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT5 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D1 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* FIELD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 26),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ECOL */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT6 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D2 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* HSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 27),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXERR */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT7 */
-		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D3 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* VSYNC */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 0),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "i2c3")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 1),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "i2c3")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 2),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "i2c4")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 3),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "pwm"),		/* PWM1 */
-		  SUNXI_FUNCTION(0x3, "i2c4")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 4),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 5),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 6),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 7),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 8),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 9),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 10),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart5"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 22)),		/* EINT22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 11),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart5"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 23)),		/* EINT23 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 12),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "clk_out_a"),	/* CLK_OUT_A */
-		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 13),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "clk_out_b"),	/* CLK_OUT_B */
-		  SUNXI_FUNCTION_IRQ(0x6, 25)),		/* EINT25 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 14),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "ps2"),		/* SCK1 */
-		  SUNXI_FUNCTION(0x4, "timer4"),	/* TCLKIN0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 26)),		/* EINT26 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 15),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "ps2"),		/* SDA1 */
-		  SUNXI_FUNCTION(0x4, "timer5"),	/* TCLKIN1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 27)),		/* EINT27 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 16),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 28)),		/* EINT28 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 17),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 29)),		/* EINT29 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 18),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 30)),		/* EINT30 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 19),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 31)),		/* EINT31 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 20),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ps2"),		/* SCK0 */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSCL */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 21),
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ps2"),		/* SDA0 */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSDA */
-};
-
-static const struct sunxi_pinctrl_desc sun7i_a20_pinctrl_data = {
-	.pins = sun7i_a20_pins,
-	.npins = ARRAY_SIZE(sun7i_a20_pins),
-	.irq_banks = 1,
-};
-
-static int sun7i_a20_pinctrl_probe(struct platform_device *pdev)
-{
-	return sunxi_pinctrl_init(pdev,
-				  &sun7i_a20_pinctrl_data);
-}
-
-static const struct of_device_id sun7i_a20_pinctrl_match[] = {
-	{ .compatible = "allwinner,sun7i-a20-pinctrl", },
-	{}
-};
-
-static struct platform_driver sun7i_a20_pinctrl_driver = {
-	.probe	= sun7i_a20_pinctrl_probe,
-	.driver	= {
-		.name		= "sun7i-a20-pinctrl",
-		.of_match_table	= sun7i_a20_pinctrl_match,
-	},
-};
-builtin_platform_driver(sun7i_a20_pinctrl_driver);
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun8i-a83t-r.c b/drivers/pinctrl/sunxi/pinctrl-sun8i-a83t-r.c
new file mode 100644
index 000000000000..6531cf67958e
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun8i-a83t-r.c
@@ -0,0 +1,128 @@
+/*
+ * Allwinner A83T SoCs special pins pinctrl driver.
+ *
+ * Copyright (C) 2017 Chen-Yu Tsai
+ * Chen-Yu Tsai <wens@csie.org>
+ *
+ * Based on pinctrl-sun50i-a64-r.c
+ *
+ * Copyright (C) 2016 Icenowy Zheng
+ * Icenowy Zheng <icenowy@aosc.xyz>
+ *
+ * Copyright (C) 2014 Chen-Yu Tsai
+ * Chen-Yu Tsai <wens@csie.org>
+ *
+ * Copyright (C) 2014 Boris Brezillon
+ * Boris Brezillon <boris.brezillon@free-electrons.com>
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/platform_device.h>
+#include <linux/reset.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun8i_a83t_r_pins[] = {
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_rsb"),		/* SCK */
+		  SUNXI_FUNCTION(0x3, "s_i2c"),		/* SCK */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 0)),	/* PL_EINT0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_rsb"),		/* SDA */
+		  SUNXI_FUNCTION(0x3, "s_i2c"),		/* SDA */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 1)),	/* PL_EINT1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_uart"),	/* TX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 2)),	/* PL_EINT2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_uart"),	/* RX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 3)),	/* PL_EINT3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_jtag"),	/* MS */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 4)),	/* PL_EINT4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_jtag"),	/* CK */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 5)),	/* PL_EINT5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_jtag"),	/* DO */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 6)),	/* PL_EINT6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_jtag"),	/* DI */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 7)),	/* PL_EINT7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_i2c"),		/* SCK */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 8)),	/* PL_EINT8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_i2c"),		/* SDA */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 9)),	/* PL_EINT9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_pwm"),
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 10)),	/* PL_EINT10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 11)),	/* PL_EINT11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_cir_rx"),
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 12)),	/* PL_EINT12 */
+};
+
+static const struct sunxi_pinctrl_desc sun8i_a83t_r_pinctrl_data = {
+	.pins = sun8i_a83t_r_pins,
+	.npins = ARRAY_SIZE(sun8i_a83t_r_pins),
+	.pin_base = PL_BASE,
+	.irq_banks = 1,
+};
+
+static int sun8i_a83t_r_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun8i_a83t_r_pinctrl_data);
+}
+
+static const struct of_device_id sun8i_a83t_r_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun8i-a83t-r-pinctrl", },
+	{}
+};
+
+static struct platform_driver sun8i_a83t_r_pinctrl_driver = {
+	.probe	= sun8i_a83t_r_pinctrl_probe,
+	.driver	= {
+		.name		= "sun8i-a83t-r-pinctrl",
+		.of_match_table	= sun8i_a83t_r_pinctrl_match,
+	},
+};
+builtin_platform_driver(sun8i_a83t_r_pinctrl_driver);
diff --git a/drivers/pinctrl/sunxi/pinctrl-sunxi.c b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
index 58774acfc814..0dfd7fa66c48 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sunxi.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
@@ -979,7 +979,7 @@ static int sunxi_pinctrl_irq_of_xlate(struct irq_domain *d,
 	return 0;
 }
 
-static struct irq_domain_ops sunxi_pinctrl_irq_domain_ops = {
+static const struct irq_domain_ops sunxi_pinctrl_irq_domain_ops = {
 	.xlate		= sunxi_pinctrl_irq_of_xlate,
 };
 
diff --git a/drivers/pinctrl/sunxi/pinctrl-sunxi.h b/drivers/pinctrl/sunxi/pinctrl-sunxi.h
index a9d315a1256c..1bfc0d8a55df 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sunxi.h
+++ b/drivers/pinctrl/sunxi/pinctrl-sunxi.h
@@ -87,6 +87,9 @@
 #define PINCTRL_SUN5I_GR8	BIT(3)
 #define PINCTRL_SUN6I_A31	BIT(4)
 #define PINCTRL_SUN6I_A31S	BIT(5)
+#define PINCTRL_SUN4I_A10	BIT(6)
+#define PINCTRL_SUN7I_A20	BIT(7)
+#define PINCTRL_SUN8I_R40	BIT(8)
 
 struct sunxi_desc_function {
 	unsigned long	variant;
diff --git a/drivers/pinctrl/tegra/pinctrl-tegra.c b/drivers/pinctrl/tegra/pinctrl-tegra.c
index 277622b4b6fb..51716819129d 100644
--- a/drivers/pinctrl/tegra/pinctrl-tegra.c
+++ b/drivers/pinctrl/tegra/pinctrl-tegra.c
@@ -21,7 +21,6 @@
 #include <linux/err.h>
 #include <linux/init.h>
 #include <linux/io.h>
-#include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pinctrl/machine.h>
diff --git a/drivers/pinctrl/tegra/pinctrl-tegra114.c b/drivers/pinctrl/tegra/pinctrl-tegra114.c
index 952132ce5ea0..56b33fca1bfc 100644
--- a/drivers/pinctrl/tegra/pinctrl-tegra114.c
+++ b/drivers/pinctrl/tegra/pinctrl-tegra114.c
@@ -1,6 +1,8 @@
 /*
  * Pinctrl data for the NVIDIA Tegra114 pinmux
  *
+ * Author: Pritesh Raithatha <praithatha@nvidia.com>
+ *
  * Copyright (c) 2012-2013, NVIDIA CORPORATION.  All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify it
@@ -13,7 +15,7 @@
  * more details.
  */
 
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pinctrl/pinctrl.h>
@@ -1857,7 +1859,6 @@ static const struct of_device_id tegra114_pinctrl_of_match[] = {
 	{ .compatible = "nvidia,tegra114-pinmux", },
 	{ },
 };
-MODULE_DEVICE_TABLE(of, tegra114_pinctrl_of_match);
 
 static struct platform_driver tegra114_pinctrl_driver = {
 	.driver = {
@@ -1866,8 +1867,4 @@ static struct platform_driver tegra114_pinctrl_driver = {
 	},
 	.probe = tegra114_pinctrl_probe,
 };
-module_platform_driver(tegra114_pinctrl_driver);
-
-MODULE_AUTHOR("Pritesh Raithatha <praithatha@nvidia.com>");
-MODULE_DESCRIPTION("NVIDIA Tegra114 pinctrl driver");
-MODULE_LICENSE("GPL v2");
+builtin_platform_driver(tegra114_pinctrl_driver);
diff --git a/drivers/pinctrl/tegra/pinctrl-tegra124.c b/drivers/pinctrl/tegra/pinctrl-tegra124.c
index bca239e3ae50..7bc998ace0d5 100644
--- a/drivers/pinctrl/tegra/pinctrl-tegra124.c
+++ b/drivers/pinctrl/tegra/pinctrl-tegra124.c
@@ -1,6 +1,8 @@
 /*
  * Pinctrl data for the NVIDIA Tegra124 pinmux
  *
+ * Author: Ashwini Ghuge <aghuge@nvidia.com>
+ *
  * Copyright (c) 2013-2014, NVIDIA CORPORATION.  All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify it
@@ -13,7 +15,7 @@
  * more details.
  */
 
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pinctrl/pinctrl.h>
@@ -2069,7 +2071,6 @@ static const struct of_device_id tegra124_pinctrl_of_match[] = {
 	{ .compatible = "nvidia,tegra124-pinmux", },
 	{ },
 };
-MODULE_DEVICE_TABLE(of, tegra124_pinctrl_of_match);
 
 static struct platform_driver tegra124_pinctrl_driver = {
 	.driver = {
@@ -2078,8 +2079,4 @@ static struct platform_driver tegra124_pinctrl_driver = {
 	},
 	.probe = tegra124_pinctrl_probe,
 };
-module_platform_driver(tegra124_pinctrl_driver);
-
-MODULE_AUTHOR("Ashwini Ghuge <aghuge@nvidia.com>");
-MODULE_DESCRIPTION("NVIDIA Tegra124 pinctrl driver");
-MODULE_LICENSE("GPL v2");
+builtin_platform_driver(tegra124_pinctrl_driver);
diff --git a/drivers/pinctrl/tegra/pinctrl-tegra20.c b/drivers/pinctrl/tegra/pinctrl-tegra20.c
index ad62451a5a9b..7e38ee9bae78 100644
--- a/drivers/pinctrl/tegra/pinctrl-tegra20.c
+++ b/drivers/pinctrl/tegra/pinctrl-tegra20.c
@@ -1,6 +1,8 @@
 /*
  * Pinctrl data for the NVIDIA Tegra20 pinmux
  *
+ * Author: Stephen Warren <swarren@nvidia.com>
+ *
  * Copyright (c) 2011-2012, NVIDIA CORPORATION.  All rights reserved.
  *
  * Derived from code:
@@ -17,7 +19,7 @@
  * more details.
  */
 
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pinctrl/pinctrl.h>
@@ -2246,9 +2248,4 @@ static struct platform_driver tegra20_pinctrl_driver = {
 	},
 	.probe = tegra20_pinctrl_probe,
 };
-module_platform_driver(tegra20_pinctrl_driver);
-
-MODULE_AUTHOR("Stephen Warren <swarren@nvidia.com>");
-MODULE_DESCRIPTION("NVIDIA Tegra20 pinctrl driver");
-MODULE_LICENSE("GPL v2");
-MODULE_DEVICE_TABLE(of, tegra20_pinctrl_of_match);
+builtin_platform_driver(tegra20_pinctrl_driver);
diff --git a/drivers/pinctrl/tegra/pinctrl-tegra210.c b/drivers/pinctrl/tegra/pinctrl-tegra210.c
index 2b70e93da9db..c244e5b17bd6 100644
--- a/drivers/pinctrl/tegra/pinctrl-tegra210.c
+++ b/drivers/pinctrl/tegra/pinctrl-tegra210.c
@@ -13,7 +13,7 @@
  * more details.
  */
 
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pinctrl/pinctrl.h>
@@ -1573,7 +1573,6 @@ static const struct of_device_id tegra210_pinctrl_of_match[] = {
 	{ .compatible = "nvidia,tegra210-pinmux", },
 	{ },
 };
-MODULE_DEVICE_TABLE(of, tegra210_pinctrl_of_match);
 
 static struct platform_driver tegra210_pinctrl_driver = {
 	.driver = {
@@ -1582,8 +1581,4 @@ static struct platform_driver tegra210_pinctrl_driver = {
 	},
 	.probe = tegra210_pinctrl_probe,
 };
-module_platform_driver(tegra210_pinctrl_driver);
-
-MODULE_AUTHOR("NVIDIA");
-MODULE_DESCRIPTION("NVIDIA Tegra210 pinctrl driver");
-MODULE_LICENSE("GPL v2");
+builtin_platform_driver(tegra210_pinctrl_driver);
diff --git a/drivers/pinctrl/tegra/pinctrl-tegra30.c b/drivers/pinctrl/tegra/pinctrl-tegra30.c
index 474ac6daf513..1f180a20f2ab 100644
--- a/drivers/pinctrl/tegra/pinctrl-tegra30.c
+++ b/drivers/pinctrl/tegra/pinctrl-tegra30.c
@@ -1,6 +1,8 @@
 /*
  * Pinctrl data for the NVIDIA Tegra30 pinmux
  *
+ * Author: Stephen Warren <swarren@nvidia.com>
+ *
  * Copyright (c) 2011-2012, NVIDIA CORPORATION.  All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify it
@@ -13,7 +15,7 @@
  * more details.
  */
 
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pinctrl/pinctrl.h>
@@ -2492,7 +2494,6 @@ static const struct of_device_id tegra30_pinctrl_of_match[] = {
 	{ .compatible = "nvidia,tegra30-pinmux", },
 	{ },
 };
-MODULE_DEVICE_TABLE(of, tegra30_pinctrl_of_match);
 
 static struct platform_driver tegra30_pinctrl_driver = {
 	.driver = {
@@ -2501,8 +2502,4 @@ static struct platform_driver tegra30_pinctrl_driver = {
 	},
 	.probe = tegra30_pinctrl_probe,
 };
-module_platform_driver(tegra30_pinctrl_driver);
-
-MODULE_AUTHOR("Stephen Warren <swarren@nvidia.com>");
-MODULE_DESCRIPTION("NVIDIA Tegra30 pinctrl driver");
-MODULE_LICENSE("GPL v2");
+builtin_platform_driver(tegra30_pinctrl_driver);
diff --git a/drivers/pinctrl/uniphier/pinctrl-uniphier-ld11.c b/drivers/pinctrl/uniphier/pinctrl-uniphier-ld11.c
index 706effe0a492..ad73db8d067b 100644
--- a/drivers/pinctrl/uniphier/pinctrl-uniphier-ld11.c
+++ b/drivers/pinctrl/uniphier/pinctrl-uniphier-ld11.c
@@ -508,57 +508,71 @@ static const unsigned usb1_pins[] = {48, 49};
 static const int usb1_muxvals[] = {0, 0};
 static const unsigned usb2_pins[] = {50, 51};
 static const int usb2_muxvals[] = {0, 0};
-static const unsigned port_range_pins[] = {
+static const unsigned port_range0_pins[] = {
 	159, 160, 161, 162, 163, 164, 165, 166,		/* PORT0x */
 	0, 1, 2, 3, 4, 5, 6, 7,				/* PORT1x */
 	8, 9, 10, 11, 12, 13, 14, 15,			/* PORT2x */
-	16, 17, 18, -1, -1, -1, -1, -1,			/* PORT3x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT4x */
-	-1, -1, -1, 46, 47, 48, 49, 50,			/* PORT5x */
-	51, -1, -1, 54, 55, 56, 57, 58,			/* PORT6x */
+	16, 17, 18,					/* PORT30-32 */
+};
+static const int port_range0_muxvals[] = {
+	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT0x */
+	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT1x */
+	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT2x */
+	15, 15, 15,					/* PORT30-32 */
+};
+static const unsigned port_range1_pins[] = {
+	46, 47, 48, 49, 50,				/* PORT53-57 */
+	51,						/* PORT60 */
+};
+static const int port_range1_muxvals[] = {
+	15, 15, 15, 15, 15,				/* PORT53-57 */
+	15,						/* PORT60 */
+};
+static const unsigned port_range2_pins[] = {
+	54, 55, 56, 57, 58,				/* PORT63-67 */
 	59, 60, 69, 70, 71, 72, 73, 74,			/* PORT7x */
 	75, 76, 77, 78, 79, 80, 81, 82,			/* PORT8x */
 	83, 84, 85, 86, 87, 88, 89, 90,			/* PORT9x */
 	91, 92, 93, 94, 95, 96, 97, 98,			/* PORT10x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT11x */
-	99, 100, 101, 102, 103, 104, 105, 106,		/* PORT12x */
-	107, 108, 109, 110, 111, 112, 113, 114,		/* PORT13x */
-	115, 116, 117, 118, 119, 120, 121, 122,		/* PORT14x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT15x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT16x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT17x */
-	61, 62, 63, 64, 65, 66, 67, 68,			/* PORT18x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT19x */
-	123, 124, 125, 126, 127, 128, 129, 130,		/* PORT20x */
-	131, 132, 133, 134, 135, 136, 137, 138,		/* PORT21x */
-	139, 140, 141, 142, -1, -1, -1, -1,		/* PORT22x */
-	147, 148, 149, 150, 151, 152, 153, 154,		/* PORT23x */
-	155, 156, 157, 143, 144, 145, 146, 158,		/* PORT24x */
 };
-static const int port_range_muxvals[] = {
-	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT0x */
-	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT1x */
-	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT2x */
-	15, 15, 15, -1, -1, -1, -1, -1,			/* PORT3x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT4x */
-	-1, -1, -1, 15, 15, 15, 15, 15,			/* PORT5x */
-	15, -1, -1, 15, 15, 15, 15, 15,			/* PORT6x */
+static const int port_range2_muxvals[] = {
+	15, 15, 15, 15, 15,				/* PORT63-67 */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT7x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT8x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT9x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT10x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT11x */
+};
+static const unsigned port_range3_pins[] = {
+	99, 100, 101, 102, 103, 104, 105, 106,		/* PORT12x */
+	107, 108, 109, 110, 111, 112, 113, 114,		/* PORT13x */
+	115, 116, 117, 118, 119, 120, 121, 122,		/* PORT14x */
+};
+static const int port_range3_muxvals[] = {
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT12x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT13x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT14x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT15x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT16x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT17x */
+};
+static const unsigned port_range4_pins[] = {
+	61, 62, 63, 64, 65, 66, 67, 68,			/* PORT18x */
+};
+static const int port_range4_muxvals[] = {
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT18x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT19x */
+};
+static const unsigned port_range5_pins[] = {
+	123, 124, 125, 126, 127, 128, 129, 130,		/* PORT20x */
+	131, 132, 133, 134, 135, 136, 137, 138,		/* PORT21x */
+	139, 140, 141, 142,				/* PORT220-223 */
+};
+static const int port_range5_muxvals[] = {
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT20x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT21x */
-	15, 15, 15, 15, -1, -1, -1, -1,			/* PORT22x */
+	15, 15, 15, 15,					/* PORT220-223 */
+};
+static const unsigned port_range6_pins[] = {
+	147, 148, 149, 150, 151, 152, 153, 154,		/* PORT23x */
+	155, 156, 157, 143, 144, 145, 146, 158,		/* PORT24x */
+};
+static const int port_range6_muxvals[] = {
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT23x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT24x */
 };
@@ -607,147 +621,153 @@ static const struct uniphier_pinctrl_group uniphier_ld11_groups[] = {
 	UNIPHIER_PINCTRL_GROUP(usb0),
 	UNIPHIER_PINCTRL_GROUP(usb1),
 	UNIPHIER_PINCTRL_GROUP(usb2),
-	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_PORT(port_range),
+	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_PORT(port_range0),
+	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_PORT(port_range1),
+	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_PORT(port_range2),
+	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_PORT(port_range3),
+	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_PORT(port_range4),
+	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_PORT(port_range5),
+	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_PORT(port_range6),
 	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_IRQ(xirq),
 	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_IRQ(xirq_alternatives),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port00, port_range, 0),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port01, port_range, 1),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port02, port_range, 2),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port03, port_range, 3),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port04, port_range, 4),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port05, port_range, 5),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port06, port_range, 6),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port07, port_range, 7),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port10, port_range, 8),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port11, port_range, 9),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port12, port_range, 10),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port13, port_range, 11),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port14, port_range, 12),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port15, port_range, 13),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port16, port_range, 14),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port17, port_range, 15),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port20, port_range, 16),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port21, port_range, 17),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port22, port_range, 18),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port23, port_range, 19),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port24, port_range, 20),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port25, port_range, 21),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port26, port_range, 22),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port27, port_range, 23),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port30, port_range, 24),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port31, port_range, 25),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port32, port_range, 26),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port53, port_range, 43),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port54, port_range, 44),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port55, port_range, 45),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port56, port_range, 46),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port57, port_range, 47),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port60, port_range, 48),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port63, port_range, 51),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port64, port_range, 52),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port65, port_range, 53),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port66, port_range, 54),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port67, port_range, 55),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port70, port_range, 56),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port71, port_range, 57),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port72, port_range, 58),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port73, port_range, 59),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port74, port_range, 60),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port75, port_range, 61),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port76, port_range, 62),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port77, port_range, 63),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port80, port_range, 64),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port81, port_range, 65),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port82, port_range, 66),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port83, port_range, 67),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port84, port_range, 68),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port85, port_range, 69),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port86, port_range, 70),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port87, port_range, 71),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port90, port_range, 72),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port91, port_range, 73),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port92, port_range, 74),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port93, port_range, 75),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port94, port_range, 76),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port95, port_range, 77),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port96, port_range, 78),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port97, port_range, 79),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port100, port_range, 80),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port101, port_range, 81),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port102, port_range, 82),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port103, port_range, 83),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port104, port_range, 84),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port105, port_range, 85),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port106, port_range, 86),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port107, port_range, 87),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port120, port_range, 96),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port121, port_range, 97),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port122, port_range, 98),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port123, port_range, 99),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port124, port_range, 100),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port125, port_range, 101),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port126, port_range, 102),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port127, port_range, 103),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port130, port_range, 104),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port131, port_range, 105),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port132, port_range, 106),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port133, port_range, 107),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port134, port_range, 108),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port135, port_range, 109),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port136, port_range, 110),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port137, port_range, 111),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port140, port_range, 112),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port141, port_range, 113),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port142, port_range, 114),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port143, port_range, 115),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port144, port_range, 116),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port145, port_range, 117),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port146, port_range, 118),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port147, port_range, 119),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port180, port_range, 144),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port181, port_range, 145),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port182, port_range, 146),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port183, port_range, 147),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port184, port_range, 148),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port185, port_range, 149),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port186, port_range, 150),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port187, port_range, 151),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port200, port_range, 160),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port201, port_range, 161),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port202, port_range, 162),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port203, port_range, 163),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port204, port_range, 164),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port205, port_range, 165),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port206, port_range, 166),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port207, port_range, 167),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port210, port_range, 168),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port211, port_range, 169),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port212, port_range, 170),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port213, port_range, 171),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port214, port_range, 172),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port215, port_range, 173),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port216, port_range, 174),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port217, port_range, 175),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port220, port_range, 176),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port221, port_range, 177),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port222, port_range, 178),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port223, port_range, 179),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port230, port_range, 184),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port231, port_range, 185),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port232, port_range, 186),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port233, port_range, 187),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port234, port_range, 188),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port235, port_range, 189),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port236, port_range, 190),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port237, port_range, 191),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port240, port_range, 192),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port241, port_range, 193),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port242, port_range, 194),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port243, port_range, 195),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port244, port_range, 196),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port245, port_range, 197),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port246, port_range, 198),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port247, port_range, 199),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port00, port_range0, 0),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port01, port_range0, 1),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port02, port_range0, 2),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port03, port_range0, 3),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port04, port_range0, 4),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port05, port_range0, 5),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port06, port_range0, 6),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port07, port_range0, 7),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port10, port_range0, 8),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port11, port_range0, 9),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port12, port_range0, 10),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port13, port_range0, 11),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port14, port_range0, 12),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port15, port_range0, 13),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port16, port_range0, 14),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port17, port_range0, 15),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port20, port_range0, 16),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port21, port_range0, 17),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port22, port_range0, 18),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port23, port_range0, 19),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port24, port_range0, 20),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port25, port_range0, 21),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port26, port_range0, 22),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port27, port_range0, 23),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port30, port_range0, 24),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port31, port_range0, 25),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port32, port_range0, 26),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port53, port_range1, 0),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port54, port_range1, 1),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port55, port_range1, 2),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port56, port_range1, 3),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port57, port_range1, 4),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port60, port_range1, 5),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port63, port_range2, 0),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port64, port_range2, 1),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port65, port_range2, 2),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port66, port_range2, 3),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port67, port_range2, 4),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port70, port_range2, 5),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port71, port_range2, 6),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port72, port_range2, 7),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port73, port_range2, 8),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port74, port_range2, 9),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port75, port_range2, 10),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port76, port_range2, 11),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port77, port_range2, 12),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port80, port_range2, 13),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port81, port_range2, 14),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port82, port_range2, 15),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port83, port_range2, 16),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port84, port_range2, 17),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port85, port_range2, 18),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port86, port_range2, 19),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port87, port_range2, 20),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port90, port_range2, 21),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port91, port_range2, 22),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port92, port_range2, 23),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port93, port_range2, 24),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port94, port_range2, 25),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port95, port_range2, 26),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port96, port_range2, 27),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port97, port_range2, 28),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port100, port_range2, 29),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port101, port_range2, 30),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port102, port_range2, 31),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port103, port_range2, 32),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port104, port_range2, 33),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port105, port_range2, 34),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port106, port_range2, 35),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port107, port_range2, 36),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port120, port_range3, 0),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port121, port_range3, 1),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port122, port_range3, 2),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port123, port_range3, 3),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port124, port_range3, 4),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port125, port_range3, 5),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port126, port_range3, 6),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port127, port_range3, 7),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port130, port_range3, 8),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port131, port_range3, 9),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port132, port_range3, 10),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port133, port_range3, 11),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port134, port_range3, 12),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port135, port_range3, 13),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port136, port_range3, 14),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port137, port_range3, 15),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port140, port_range3, 16),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port141, port_range3, 17),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port142, port_range3, 18),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port143, port_range3, 19),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port144, port_range3, 20),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port145, port_range3, 21),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port146, port_range3, 22),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port147, port_range3, 23),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port180, port_range4, 0),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port181, port_range4, 1),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port182, port_range4, 2),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port183, port_range4, 3),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port184, port_range4, 4),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port185, port_range4, 5),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port186, port_range4, 6),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port187, port_range4, 7),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port200, port_range5, 0),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port201, port_range5, 1),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port202, port_range5, 2),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port203, port_range5, 3),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port204, port_range5, 4),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port205, port_range5, 5),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port206, port_range5, 6),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port207, port_range5, 7),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port210, port_range5, 8),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port211, port_range5, 9),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port212, port_range5, 10),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port213, port_range5, 11),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port214, port_range5, 12),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port215, port_range5, 13),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port216, port_range5, 14),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port217, port_range5, 15),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port220, port_range5, 16),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port221, port_range5, 17),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port222, port_range5, 18),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port223, port_range5, 19),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port230, port_range6, 0),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port231, port_range6, 1),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port232, port_range6, 2),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port233, port_range6, 3),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port234, port_range6, 4),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port235, port_range6, 5),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port236, port_range6, 6),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port237, port_range6, 7),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port240, port_range6, 8),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port241, port_range6, 9),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port242, port_range6, 10),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port243, port_range6, 11),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port244, port_range6, 12),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port245, port_range6, 13),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port246, port_range6, 14),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port247, port_range6, 15),
 	UNIPHIER_PINCTRL_GROUP_SINGLE(xirq0, xirq, 0),
 	UNIPHIER_PINCTRL_GROUP_SINGLE(xirq1, xirq, 1),
 	UNIPHIER_PINCTRL_GROUP_SINGLE(xirq2, xirq, 2),
diff --git a/drivers/pinctrl/uniphier/pinctrl-uniphier-ld20.c b/drivers/pinctrl/uniphier/pinctrl-uniphier-ld20.c
index c8d18a2d3a88..93006626028d 100644
--- a/drivers/pinctrl/uniphier/pinctrl-uniphier-ld20.c
+++ b/drivers/pinctrl/uniphier/pinctrl-uniphier-ld20.c
@@ -597,7 +597,7 @@ static const unsigned usb2_pins[] = {50, 51};
 static const int usb2_muxvals[] = {0, 0};
 static const unsigned usb3_pins[] = {52, 53};
 static const int usb3_muxvals[] = {0, 0};
-static const unsigned port_range_pins[] = {
+static const unsigned port_range0_pins[] = {
 	168, 169, 170, 171, 172, 173, 174, 175,		/* PORT0x */
 	0, 1, 2, 3, 4, 5, 6, 7,				/* PORT1x */
 	8, 9, 10, 11, 12, 13, 14, 15,			/* PORT2x */
@@ -609,23 +609,8 @@ static const unsigned port_range_pins[] = {
 	75, 76, 77, 78, 79, 80, 81, 82,			/* PORT8x */
 	83, 84, 85, 86, 87, 88, 89, 90,			/* PORT9x */
 	91, 92, 93, 94, 95, 96, 97, 98,			/* PORT10x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT11x */
-	99, 100, 101, 102, 103, 104, 105, 106,		/* PORT12x */
-	107, 108, 109, 110, 111, 112, 113, 114,		/* PORT13x */
-	115, 116, 117, 118, 119, 120, 121, 122,		/* PORT14x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT15x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT16x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT17x */
-	61, 62, 63, 64, 65, 66, 67, 68,			/* PORT18x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT19x */
-	123, 124, 125, 126, 127, 128, 129, 130,		/* PORT20x */
-	131, 132, 133, 134, 135, 136, 137, 138,		/* PORT21x */
-	139, 140, 141, 142, 143, 144, 145, 146,		/* PORT22x */
-	147, 148, 149, 150, 151, 152, 153, 154,		/* PORT23x */
-	155, 156, 157, 158, 159, 160, 161, 162,		/* PORT24x */
-	163, 164, 165, 166, 167,			/* PORT25x */
 };
-static const int port_range_muxvals[] = {
+static const int port_range0_muxvals[] = {
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT0x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT1x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT2x */
@@ -637,21 +622,38 @@ static const int port_range_muxvals[] = {
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT8x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT9x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT10x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT11x */
+};
+static const unsigned port_range1_pins[] = {
+	99, 100, 101, 102, 103, 104, 105, 106,		/* PORT12x */
+	107, 108, 109, 110, 111, 112, 113, 114,		/* PORT13x */
+	115, 116, 117, 118, 119, 120, 121, 122,		/* PORT14x */
+};
+static const int port_range1_muxvals[] = {
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT12x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT13x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT14x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT15x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT16x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT17x */
+};
+static const unsigned port_range2_pins[] = {
+	61, 62, 63, 64, 65, 66, 67, 68,			/* PORT18x */
+};
+static const int port_range2_muxvals[] = {
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT18x */
-	-1, -1, -1, -1, -1, -1, -1, -1,			/* PORT19x */
+};
+static const unsigned port_range3_pins[] = {
+	123, 124, 125, 126, 127, 128, 129, 130,		/* PORT20x */
+	131, 132, 133, 134, 135, 136, 137, 138,		/* PORT21x */
+	139, 140, 141, 142, 143, 144, 145, 146,		/* PORT22x */
+	147, 148, 149, 150, 151, 152, 153, 154,		/* PORT23x */
+	155, 156, 157, 158, 159, 160, 161, 162,		/* PORT24x */
+	163, 164, 165, 166, 167,			/* PORT250-254 */
+};
+static const int port_range3_muxvals[] = {
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT20x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT21x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT22x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT23x */
 	15, 15, 15, 15, 15, 15, 15, 15,			/* PORT24x */
-	15, 15, 15, 15, 15,				/* PORT25x */
+	15, 15, 15, 15, 15,				/* PORT250-254 */
 };
 static const unsigned xirq_pins[] = {
 	149, 150, 151, 152, 153, 154, 155, 156,		/* XIRQ0-7 */
@@ -695,174 +697,177 @@ static const struct uniphier_pinctrl_group uniphier_ld20_groups[] = {
 	UNIPHIER_PINCTRL_GROUP(usb1),
 	UNIPHIER_PINCTRL_GROUP(usb2),
 	UNIPHIER_PINCTRL_GROUP(usb3),
-	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_PORT(port_range),
+	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_PORT(port_range0),
+	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_PORT(port_range1),
+	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_PORT(port_range2),
+	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_PORT(port_range3),
 	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_IRQ(xirq),
 	UNIPHIER_PINCTRL_GROUP_GPIO_RANGE_IRQ(xirq_alternatives),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port00, port_range, 0),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port01, port_range, 1),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port02, port_range, 2),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port03, port_range, 3),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port04, port_range, 4),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port05, port_range, 5),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port06, port_range, 6),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port07, port_range, 7),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port10, port_range, 8),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port11, port_range, 9),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port12, port_range, 10),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port13, port_range, 11),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port14, port_range, 12),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port15, port_range, 13),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port16, port_range, 14),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port17, port_range, 15),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port20, port_range, 16),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port21, port_range, 17),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port22, port_range, 18),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port23, port_range, 19),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port24, port_range, 20),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port25, port_range, 21),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port26, port_range, 22),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port27, port_range, 23),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port30, port_range, 24),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port31, port_range, 25),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port32, port_range, 26),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port33, port_range, 27),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port34, port_range, 28),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port35, port_range, 29),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port36, port_range, 30),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port37, port_range, 31),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port40, port_range, 32),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port41, port_range, 33),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port42, port_range, 34),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port43, port_range, 35),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port44, port_range, 36),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port45, port_range, 37),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port46, port_range, 38),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port47, port_range, 39),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port50, port_range, 40),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port51, port_range, 41),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port52, port_range, 42),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port53, port_range, 43),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port54, port_range, 44),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port55, port_range, 45),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port56, port_range, 46),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port57, port_range, 47),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port60, port_range, 48),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port61, port_range, 49),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port62, port_range, 50),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port63, port_range, 51),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port64, port_range, 52),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port65, port_range, 53),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port66, port_range, 54),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port67, port_range, 55),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port70, port_range, 56),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port71, port_range, 57),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port72, port_range, 58),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port73, port_range, 59),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port74, port_range, 60),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port75, port_range, 61),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port76, port_range, 62),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port77, port_range, 63),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port80, port_range, 64),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port81, port_range, 65),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port82, port_range, 66),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port83, port_range, 67),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port84, port_range, 68),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port85, port_range, 69),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port86, port_range, 70),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port87, port_range, 71),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port90, port_range, 72),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port91, port_range, 73),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port92, port_range, 74),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port93, port_range, 75),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port94, port_range, 76),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port95, port_range, 77),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port96, port_range, 78),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port97, port_range, 79),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port100, port_range, 80),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port101, port_range, 81),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port102, port_range, 82),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port103, port_range, 83),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port104, port_range, 84),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port105, port_range, 85),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port106, port_range, 86),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port107, port_range, 87),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port120, port_range, 96),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port121, port_range, 97),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port122, port_range, 98),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port123, port_range, 99),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port124, port_range, 100),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port125, port_range, 101),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port126, port_range, 102),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port127, port_range, 103),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port130, port_range, 104),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port131, port_range, 105),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port132, port_range, 106),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port133, port_range, 107),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port134, port_range, 108),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port135, port_range, 109),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port136, port_range, 110),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port137, port_range, 111),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port140, port_range, 112),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port141, port_range, 113),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port142, port_range, 114),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port143, port_range, 115),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port144, port_range, 116),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port145, port_range, 117),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port146, port_range, 118),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port147, port_range, 119),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port180, port_range, 144),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port181, port_range, 145),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port182, port_range, 146),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port183, port_range, 147),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port184, port_range, 148),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port185, port_range, 149),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port186, port_range, 150),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port187, port_range, 151),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port200, port_range, 160),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port201, port_range, 161),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port202, port_range, 162),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port203, port_range, 163),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port204, port_range, 164),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port205, port_range, 165),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port206, port_range, 166),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port207, port_range, 167),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port210, port_range, 168),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port211, port_range, 169),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port212, port_range, 170),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port213, port_range, 171),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port214, port_range, 172),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port215, port_range, 173),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port216, port_range, 174),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port217, port_range, 175),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port220, port_range, 176),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port221, port_range, 177),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port222, port_range, 178),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port223, port_range, 179),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port224, port_range, 180),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port225, port_range, 181),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port226, port_range, 182),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port227, port_range, 183),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port230, port_range, 184),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port231, port_range, 185),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port232, port_range, 186),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port233, port_range, 187),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port234, port_range, 188),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port235, port_range, 189),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port236, port_range, 190),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port237, port_range, 191),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port240, port_range, 192),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port241, port_range, 193),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port242, port_range, 194),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port243, port_range, 195),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port244, port_range, 196),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port245, port_range, 197),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port246, port_range, 198),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port247, port_range, 199),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port250, port_range, 200),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port251, port_range, 201),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port252, port_range, 202),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port253, port_range, 203),
-	UNIPHIER_PINCTRL_GROUP_SINGLE(port254, port_range, 204),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port00, port_range0, 0),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port01, port_range0, 1),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port02, port_range0, 2),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port03, port_range0, 3),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port04, port_range0, 4),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port05, port_range0, 5),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port06, port_range0, 6),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port07, port_range0, 7),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port10, port_range0, 8),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port11, port_range0, 9),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port12, port_range0, 10),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port13, port_range0, 11),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port14, port_range0, 12),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port15, port_range0, 13),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port16, port_range0, 14),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port17, port_range0, 15),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port20, port_range0, 16),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port21, port_range0, 17),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port22, port_range0, 18),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port23, port_range0, 19),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port24, port_range0, 20),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port25, port_range0, 21),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port26, port_range0, 22),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port27, port_range0, 23),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port30, port_range0, 24),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port31, port_range0, 25),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port32, port_range0, 26),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port33, port_range0, 27),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port34, port_range0, 28),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port35, port_range0, 29),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port36, port_range0, 30),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port37, port_range0, 31),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port40, port_range0, 32),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port41, port_range0, 33),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port42, port_range0, 34),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port43, port_range0, 35),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port44, port_range0, 36),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port45, port_range0, 37),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port46, port_range0, 38),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port47, port_range0, 39),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port50, port_range0, 40),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port51, port_range0, 41),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port52, port_range0, 42),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port53, port_range0, 43),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port54, port_range0, 44),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port55, port_range0, 45),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port56, port_range0, 46),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port57, port_range0, 47),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port60, port_range0, 48),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port61, port_range0, 49),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port62, port_range0, 50),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port63, port_range0, 51),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port64, port_range0, 52),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port65, port_range0, 53),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port66, port_range0, 54),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port67, port_range0, 55),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port70, port_range0, 56),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port71, port_range0, 57),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port72, port_range0, 58),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port73, port_range0, 59),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port74, port_range0, 60),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port75, port_range0, 61),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port76, port_range0, 62),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port77, port_range0, 63),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port80, port_range0, 64),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port81, port_range0, 65),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port82, port_range0, 66),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port83, port_range0, 67),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port84, port_range0, 68),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port85, port_range0, 69),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port86, port_range0, 70),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port87, port_range0, 71),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port90, port_range0, 72),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port91, port_range0, 73),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port92, port_range0, 74),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port93, port_range0, 75),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port94, port_range0, 76),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port95, port_range0, 77),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port96, port_range0, 78),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port97, port_range0, 79),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port100, port_range0, 80),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port101, port_range0, 81),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port102, port_range0, 82),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port103, port_range0, 83),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port104, port_range0, 84),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port105, port_range0, 85),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port106, port_range0, 86),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port107, port_range0, 87),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port120, port_range1, 0),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port121, port_range1, 1),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port122, port_range1, 2),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port123, port_range1, 3),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port124, port_range1, 4),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port125, port_range1, 5),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port126, port_range1, 6),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port127, port_range1, 7),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port130, port_range1, 8),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port131, port_range1, 9),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port132, port_range1, 10),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port133, port_range1, 11),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port134, port_range1, 12),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port135, port_range1, 13),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port136, port_range1, 14),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port137, port_range1, 15),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port140, port_range1, 16),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port141, port_range1, 17),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port142, port_range1, 18),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port143, port_range1, 19),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port144, port_range1, 20),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port145, port_range1, 21),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port146, port_range1, 22),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port147, port_range1, 23),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port180, port_range2, 0),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port181, port_range2, 1),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port182, port_range2, 2),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port183, port_range2, 3),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port184, port_range2, 4),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port185, port_range2, 5),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port186, port_range2, 6),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port187, port_range2, 7),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port200, port_range3, 0),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port201, port_range3, 1),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port202, port_range3, 2),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port203, port_range3, 3),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port204, port_range3, 4),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port205, port_range3, 5),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port206, port_range3, 6),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port207, port_range3, 7),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port210, port_range3, 8),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port211, port_range3, 9),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port212, port_range3, 10),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port213, port_range3, 11),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port214, port_range3, 12),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port215, port_range3, 13),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port216, port_range3, 14),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port217, port_range3, 15),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port220, port_range3, 16),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port221, port_range3, 17),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port222, port_range3, 18),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port223, port_range3, 19),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port224, port_range3, 20),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port225, port_range3, 21),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port226, port_range3, 22),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port227, port_range3, 23),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port230, port_range3, 24),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port231, port_range3, 25),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port232, port_range3, 26),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port233, port_range3, 27),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port234, port_range3, 28),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port235, port_range3, 29),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port236, port_range3, 30),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port237, port_range3, 31),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port240, port_range3, 32),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port241, port_range3, 33),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port242, port_range3, 34),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port243, port_range3, 35),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port244, port_range3, 36),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port245, port_range3, 37),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port246, port_range3, 38),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port247, port_range3, 39),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port250, port_range3, 40),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port251, port_range3, 41),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port252, port_range3, 42),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port253, port_range3, 43),
+	UNIPHIER_PINCTRL_GROUP_SINGLE(port254, port_range3, 44),
 	UNIPHIER_PINCTRL_GROUP_SINGLE(xirq0, xirq, 0),
 	UNIPHIER_PINCTRL_GROUP_SINGLE(xirq1, xirq, 1),
 	UNIPHIER_PINCTRL_GROUP_SINGLE(xirq2, xirq, 2),
diff --git a/drivers/pinctrl/zte/Kconfig b/drivers/pinctrl/zte/Kconfig
new file mode 100644
index 000000000000..0d97352a24ec
--- /dev/null
+++ b/drivers/pinctrl/zte/Kconfig
@@ -0,0 +1,13 @@
+config PINCTRL_ZX
+	bool
+	select PINMUX
+	select GENERIC_PINCONF
+	select GENERIC_PINCTRL_GROUPS
+	select GENERIC_PINMUX_FUNCTIONS
+
+config PINCTRL_ZX296718
+	bool "ZTE ZX296718 pinctrl driver"
+	depends on OF && ARCH_ZX
+	select PINCTRL_ZX
+	help
+	  Say Y here to enable the ZX296718 pinctrl driver
diff --git a/drivers/pinctrl/zte/Makefile b/drivers/pinctrl/zte/Makefile
new file mode 100644
index 000000000000..c42e651d7a73
--- /dev/null
+++ b/drivers/pinctrl/zte/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_PINCTRL_ZX)	+= pinctrl-zx.o
+obj-$(CONFIG_PINCTRL_ZX296718)	+= pinctrl-zx296718.o
diff --git a/drivers/pinctrl/zte/pinctrl-zx.c b/drivers/pinctrl/zte/pinctrl-zx.c
new file mode 100644
index 000000000000..787e3967bd5c
--- /dev/null
+++ b/drivers/pinctrl/zte/pinctrl-zx.c
@@ -0,0 +1,445 @@
+/*
+ * Copyright (C) 2017 Sanechips Technology Co., Ltd.
+ * Copyright 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinconf-generic.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "../core.h"
+#include "../pinctrl-utils.h"
+#include "../pinmux.h"
+#include "pinctrl-zx.h"
+
+#define ZX_PULL_DOWN		BIT(0)
+#define ZX_PULL_UP		BIT(1)
+#define ZX_INPUT_ENABLE		BIT(3)
+#define ZX_DS_SHIFT		4
+#define ZX_DS_MASK		(0x7 << ZX_DS_SHIFT)
+#define ZX_DS_VALUE(x)		(((x) << ZX_DS_SHIFT) & ZX_DS_MASK)
+#define ZX_SLEW			BIT(8)
+
+struct zx_pinctrl {
+	struct pinctrl_dev *pctldev;
+	struct device *dev;
+	void __iomem *base;
+	void __iomem *aux_base;
+	spinlock_t lock;
+	struct zx_pinctrl_soc_info *info;
+};
+
+static int zx_dt_node_to_map(struct pinctrl_dev *pctldev,
+			     struct device_node *np_config,
+			     struct pinctrl_map **map, u32 *num_maps)
+{
+	return pinconf_generic_dt_node_to_map(pctldev, np_config, map,
+					      num_maps, PIN_MAP_TYPE_INVALID);
+}
+
+static const struct pinctrl_ops zx_pinctrl_ops = {
+	.dt_node_to_map = zx_dt_node_to_map,
+	.dt_free_map = pinctrl_utils_free_map,
+	.get_groups_count = pinctrl_generic_get_group_count,
+	.get_group_name = pinctrl_generic_get_group_name,
+	.get_group_pins = pinctrl_generic_get_group_pins,
+};
+
+#define NONAON_MVAL 2
+
+static int zx_set_mux(struct pinctrl_dev *pctldev, unsigned int func_selector,
+		      unsigned int group_selector)
+{
+	struct zx_pinctrl *zpctl = pinctrl_dev_get_drvdata(pctldev);
+	struct zx_pinctrl_soc_info *info = zpctl->info;
+	const struct pinctrl_pin_desc *pindesc = info->pins + group_selector;
+	struct zx_pin_data *data = pindesc->drv_data;
+	struct zx_mux_desc *mux = data->muxes;
+	u32 mask = (1 << data->width) - 1;
+	u32 offset = data->offset;
+	u32 bitpos = data->bitpos;
+	struct function_desc *func;
+	unsigned long flags;
+	u32 val, mval;
+
+	/* Skip reserved pin */
+	if (!data)
+		return -EINVAL;
+
+	func = pinmux_generic_get_function(pctldev, func_selector);
+	if (!func)
+		return -EINVAL;
+
+	while (mux->name) {
+		if (strcmp(mux->name, func->name) == 0)
+			break;
+		mux++;
+	}
+
+	/* Found mux value to be written */
+	mval = mux->muxval;
+
+	spin_lock_irqsave(&zpctl->lock, flags);
+
+	if (data->aon_pin) {
+		/*
+		 * It's an AON pin, whose mux register offset and bit position
+		 * can be caluculated from pin number.  Each register covers 16
+		 * pins, and each pin occupies 2 bits.
+		 */
+		u16 aoffset = pindesc->number / 16 * 4;
+		u16 abitpos = (pindesc->number % 16) * 2;
+
+		if (mval & AON_MUX_FLAG) {
+			/*
+			 * This is a mux value that needs to be written into
+			 * AON pinmux register.  Write it and then we're done.
+			 */
+			val = readl(zpctl->aux_base + aoffset);
+			val &= ~(0x3 << abitpos);
+			val |= (mval & 0x3) << abitpos;
+			writel(val, zpctl->aux_base + aoffset);
+		} else {
+			/*
+			 * It's a mux value that needs to be written into TOP
+			 * pinmux register.
+			 */
+			val = readl(zpctl->base + offset);
+			val &= ~(mask << bitpos);
+			val |= (mval & mask) << bitpos;
+			writel(val, zpctl->base + offset);
+
+			/*
+			 * In this case, the AON pinmux register needs to be
+			 * set up to select non-AON function.
+			 */
+			val = readl(zpctl->aux_base + aoffset);
+			val &= ~(0x3 << abitpos);
+			val |= NONAON_MVAL << abitpos;
+			writel(val, zpctl->aux_base + aoffset);
+		}
+
+	} else {
+		/*
+		 * This is a TOP pin, and we only need to set up TOP pinmux
+		 * register and then we're done with it.
+		 */
+		val = readl(zpctl->base + offset);
+		val &= ~(mask << bitpos);
+		val |= (mval & mask) << bitpos;
+		writel(val, zpctl->base + offset);
+	}
+
+	spin_unlock_irqrestore(&zpctl->lock, flags);
+
+	return 0;
+}
+
+static const struct pinmux_ops zx_pinmux_ops = {
+	.get_functions_count = pinmux_generic_get_function_count,
+	.get_function_name = pinmux_generic_get_function_name,
+	.get_function_groups = pinmux_generic_get_function_groups,
+	.set_mux = zx_set_mux,
+};
+
+static int zx_pin_config_get(struct pinctrl_dev *pctldev, unsigned int pin,
+			     unsigned long *config)
+{
+	struct zx_pinctrl *zpctl = pinctrl_dev_get_drvdata(pctldev);
+	struct zx_pinctrl_soc_info *info = zpctl->info;
+	const struct pinctrl_pin_desc *pindesc = info->pins + pin;
+	struct zx_pin_data *data = pindesc->drv_data;
+	enum pin_config_param param = pinconf_to_config_param(*config);
+	u32 val;
+
+	/* Skip reserved pin */
+	if (!data)
+		return -EINVAL;
+
+	val = readl(zpctl->aux_base + data->coffset);
+	val = val >> data->cbitpos;
+
+	switch (param) {
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		val &= ZX_PULL_DOWN;
+		val = !!val;
+		if (val == 0)
+			return -EINVAL;
+		break;
+	case PIN_CONFIG_BIAS_PULL_UP:
+		val &= ZX_PULL_UP;
+		val = !!val;
+		if (val == 0)
+			return -EINVAL;
+		break;
+	case PIN_CONFIG_INPUT_ENABLE:
+		val &= ZX_INPUT_ENABLE;
+		val = !!val;
+		if (val == 0)
+			return -EINVAL;
+		break;
+	case PIN_CONFIG_DRIVE_STRENGTH:
+		val &= ZX_DS_MASK;
+		val = val >> ZX_DS_SHIFT;
+		break;
+	case PIN_CONFIG_SLEW_RATE:
+		val &= ZX_SLEW;
+		val = !!val;
+		break;
+	default:
+		return -ENOTSUPP;
+	}
+
+	*config = pinconf_to_config_packed(param, val);
+
+	return 0;
+}
+
+static int zx_pin_config_set(struct pinctrl_dev *pctldev, unsigned int pin,
+			     unsigned long *configs, unsigned int num_configs)
+{
+	struct zx_pinctrl *zpctl = pinctrl_dev_get_drvdata(pctldev);
+	struct zx_pinctrl_soc_info *info = zpctl->info;
+	const struct pinctrl_pin_desc *pindesc = info->pins + pin;
+	struct zx_pin_data *data = pindesc->drv_data;
+	enum pin_config_param param;
+	u32 val, arg;
+	int i;
+
+	/* Skip reserved pin */
+	if (!data)
+		return -EINVAL;
+
+	val = readl(zpctl->aux_base + data->coffset);
+
+	for (i = 0; i < num_configs; i++) {
+		param = pinconf_to_config_param(configs[i]);
+		arg = pinconf_to_config_argument(configs[i]);
+
+		switch (param) {
+		case PIN_CONFIG_BIAS_PULL_DOWN:
+			val |= ZX_PULL_DOWN << data->cbitpos;
+			break;
+		case PIN_CONFIG_BIAS_PULL_UP:
+			val |= ZX_PULL_UP << data->cbitpos;
+			break;
+		case PIN_CONFIG_INPUT_ENABLE:
+			val |= ZX_INPUT_ENABLE << data->cbitpos;
+			break;
+		case PIN_CONFIG_DRIVE_STRENGTH:
+			val &= ~(ZX_DS_MASK << data->cbitpos);
+			val |= ZX_DS_VALUE(arg) << data->cbitpos;
+			break;
+		case PIN_CONFIG_SLEW_RATE:
+			if (arg)
+				val |= ZX_SLEW << data->cbitpos;
+			else
+				val &= ~ZX_SLEW << data->cbitpos;
+			break;
+		default:
+			return -ENOTSUPP;
+		}
+	}
+
+	writel(val, zpctl->aux_base + data->coffset);
+	return 0;
+}
+
+static const struct pinconf_ops zx_pinconf_ops = {
+	.pin_config_set = zx_pin_config_set,
+	.pin_config_get = zx_pin_config_get,
+	.is_generic = true,
+};
+
+static int zx_pinctrl_build_state(struct platform_device *pdev)
+{
+	struct zx_pinctrl *zpctl = platform_get_drvdata(pdev);
+	struct zx_pinctrl_soc_info *info = zpctl->info;
+	struct pinctrl_dev *pctldev = zpctl->pctldev;
+	struct function_desc *functions;
+	int nfunctions;
+	struct group_desc *groups;
+	int ngroups;
+	int i;
+
+	/* Every single pin composes a group */
+	ngroups = info->npins;
+	groups = devm_kzalloc(&pdev->dev, ngroups * sizeof(*groups),
+			      GFP_KERNEL);
+	if (!groups)
+		return -ENOMEM;
+
+	for (i = 0; i < ngroups; i++) {
+		const struct pinctrl_pin_desc *pindesc = info->pins + i;
+		struct group_desc *group = groups + i;
+
+		group->name = pindesc->name;
+		group->pins = (int *) &pindesc->number;
+		group->num_pins = 1;
+		radix_tree_insert(&pctldev->pin_group_tree, i, group);
+	}
+
+	pctldev->num_groups = ngroups;
+
+	/* Build function list from pin mux functions */
+	functions = devm_kzalloc(&pdev->dev, info->npins * sizeof(*functions),
+				 GFP_KERNEL);
+	if (!functions)
+		return -ENOMEM;
+
+	nfunctions = 0;
+	for (i = 0; i < info->npins; i++) {
+		const struct pinctrl_pin_desc *pindesc = info->pins + i;
+		struct zx_pin_data *data = pindesc->drv_data;
+		struct zx_mux_desc *mux;
+
+		/* Reserved pins do not have a drv_data at all */
+		if (!data)
+			continue;
+
+		/* Loop over all muxes for the pin */
+		mux = data->muxes;
+		while (mux->name) {
+			struct function_desc *func = functions;
+
+			/* Search function list for given mux */
+			while (func->name) {
+				if (strcmp(mux->name, func->name) == 0) {
+					/* Function exists */
+					func->num_group_names++;
+					break;
+				}
+				func++;
+			}
+
+			if (!func->name) {
+				/* New function */
+				func->name = mux->name;
+				func->num_group_names = 1;
+				radix_tree_insert(&pctldev->pin_function_tree,
+						  nfunctions++, func);
+			}
+
+			mux++;
+		}
+	}
+
+	pctldev->num_functions = nfunctions;
+	functions = krealloc(functions, nfunctions * sizeof(*functions),
+			     GFP_KERNEL);
+
+	/* Find pin groups for every single function */
+	for (i = 0; i < info->npins; i++) {
+		const struct pinctrl_pin_desc *pindesc = info->pins + i;
+		struct zx_pin_data *data = pindesc->drv_data;
+		struct zx_mux_desc *mux;
+
+		if (!data)
+			continue;
+
+		mux = data->muxes;
+		while (mux->name) {
+			struct function_desc *func;
+			const char **group;
+			int j;
+
+			/* Find function for given mux */
+			for (j = 0; j < nfunctions; j++)
+				if (strcmp(functions[j].name, mux->name) == 0)
+					break;
+
+			func = functions + j;
+			if (!func->group_names) {
+				func->group_names = devm_kzalloc(&pdev->dev,
+						func->num_group_names *
+						sizeof(*func->group_names),
+						GFP_KERNEL);
+				if (!func->group_names)
+					return -ENOMEM;
+			}
+
+			group = func->group_names;
+			while (*group)
+				group++;
+			*group = pindesc->name;
+
+			mux++;
+		}
+	}
+
+	return 0;
+}
+
+int zx_pinctrl_init(struct platform_device *pdev,
+		    struct zx_pinctrl_soc_info *info)
+{
+	struct pinctrl_desc *pctldesc;
+	struct zx_pinctrl *zpctl;
+	struct device_node *np;
+	struct resource *res;
+	int ret;
+
+	zpctl = devm_kzalloc(&pdev->dev, sizeof(*zpctl), GFP_KERNEL);
+	if (!zpctl)
+		return -ENOMEM;
+
+	spin_lock_init(&zpctl->lock);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	zpctl->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(zpctl->base))
+		return PTR_ERR(zpctl->base);
+
+	np = of_parse_phandle(pdev->dev.of_node, "zte,auxiliary-controller", 0);
+	if (!np) {
+		dev_err(&pdev->dev, "failed to find auxiliary controller\n");
+		return -ENODEV;
+	}
+
+	zpctl->aux_base = of_iomap(np, 0);
+	if (!zpctl->aux_base)
+		return -ENOMEM;
+
+	zpctl->dev = &pdev->dev;
+	zpctl->info = info;
+
+	pctldesc = devm_kzalloc(&pdev->dev, sizeof(*pctldesc), GFP_KERNEL);
+	if (!pctldesc)
+		return -ENOMEM;
+
+	pctldesc->name = dev_name(&pdev->dev);
+	pctldesc->owner = THIS_MODULE;
+	pctldesc->pins = info->pins;
+	pctldesc->npins = info->npins;
+	pctldesc->pctlops = &zx_pinctrl_ops;
+	pctldesc->pmxops = &zx_pinmux_ops;
+	pctldesc->confops = &zx_pinconf_ops;
+
+	zpctl->pctldev = devm_pinctrl_register(&pdev->dev, pctldesc, zpctl);
+	if (IS_ERR(zpctl->pctldev)) {
+		ret = PTR_ERR(zpctl->pctldev);
+		dev_err(&pdev->dev, "failed to register pinctrl: %d\n", ret);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, zpctl);
+
+	ret = zx_pinctrl_build_state(pdev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to build state: %d\n", ret);
+		return ret;
+	}
+
+	dev_info(&pdev->dev, "initialized pinctrl driver\n");
+	return 0;
+}
diff --git a/drivers/pinctrl/zte/pinctrl-zx.h b/drivers/pinctrl/zte/pinctrl-zx.h
new file mode 100644
index 000000000000..bc67e2be0503
--- /dev/null
+++ b/drivers/pinctrl/zte/pinctrl-zx.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright (C) 2017 Sanechips Technology Co., Ltd.
+ * Copyright 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __PINCTRL_ZX_H
+#define __PINCTRL_ZX_H
+
+/**
+ * struct zx_mux_desc - hardware mux descriptor
+ * @name: mux function name
+ * @muxval: mux register bit value
+ */
+struct zx_mux_desc {
+	const char *name;
+	u8 muxval;
+};
+
+/**
+ * struct zx_pin_data - hardware per-pin data
+ * @aon_pin: whether it's an AON pin
+ * @offset: register offset within TOP pinmux controller
+ * @bitpos: bit position within TOP pinmux register
+ * @width: bit width within TOP pinmux register
+ * @coffset: pinconf register offset within AON controller
+ * @cbitpos: pinconf bit position within AON register
+ * @muxes: available mux function names and corresponding register values
+ *
+ * Unlike TOP pinmux and AON pinconf registers which are arranged pretty
+ * arbitrarily, AON pinmux register bits are well organized per pin id, and
+ * each pin occupies two bits, so that we can calculate the AON register offset
+ * and bit position from pin id.  Thus, we only need to define TOP pinmux and
+ * AON pinconf register data for the pin.
+ */
+struct zx_pin_data {
+	bool aon_pin;
+	u16 offset;
+	u16 bitpos;
+	u16 width;
+	u16 coffset;
+	u16 cbitpos;
+	struct zx_mux_desc *muxes;
+};
+
+struct zx_pinctrl_soc_info {
+	const struct pinctrl_pin_desc *pins;
+	unsigned int npins;
+};
+
+#define TOP_PIN(pin, off, bp, wd, coff, cbp, ...) {		\
+	.number = pin,						\
+	.name = #pin,						\
+	.drv_data = &(struct zx_pin_data) {			\
+		.aon_pin = false,				\
+		.offset = off,					\
+		.bitpos = bp,					\
+		.width = wd,					\
+		.coffset = coff,				\
+		.cbitpos = cbp,					\
+		.muxes = (struct zx_mux_desc[]) {		\
+			 __VA_ARGS__, { } },			\
+	},							\
+}
+
+#define AON_PIN(pin, off, bp, wd, coff, cbp, ...) {		\
+	.number = pin,						\
+	.name = #pin,						\
+	.drv_data = &(struct zx_pin_data) {			\
+		.aon_pin = true,				\
+		.offset = off,					\
+		.bitpos = bp,					\
+		.width = wd,					\
+		.coffset = coff,				\
+		.cbitpos = cbp,					\
+		.muxes = (struct zx_mux_desc[]) {		\
+			 __VA_ARGS__, { } },			\
+	},							\
+}
+
+#define ZX_RESERVED(pin) PINCTRL_PIN(pin, #pin)
+
+#define TOP_MUX(_val, _name) {					\
+	.name = _name,						\
+	.muxval = _val,						\
+}
+
+/*
+ * When the flag is set, it's a mux configuration for an AON pin that sits in
+ * AON register.  Otherwise, it's one for AON pin but sitting in TOP register.
+ */
+#define AON_MUX_FLAG BIT(7)
+
+#define AON_MUX(_val, _name) {					\
+	.name = _name,						\
+	.muxval = _val | AON_MUX_FLAG,				\
+}
+
+int zx_pinctrl_init(struct platform_device *pdev,
+		    struct zx_pinctrl_soc_info *info);
+
+#endif /* __PINCTRL_ZX_H */
diff --git a/drivers/pinctrl/zte/pinctrl-zx296718.c b/drivers/pinctrl/zte/pinctrl-zx296718.c
new file mode 100644
index 000000000000..71efec17ee7e
--- /dev/null
+++ b/drivers/pinctrl/zte/pinctrl-zx296718.c
@@ -0,0 +1,1027 @@
+/*
+ * Copyright (C) 2017 Sanechips Technology Co., Ltd.
+ * Copyright 2017 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/platform_device.h>
+
+#include "pinctrl-zx.h"
+
+#define TOP_REG0	0x00
+#define TOP_REG1	0x04
+#define TOP_REG2	0x08
+#define TOP_REG3	0x0c
+#define TOP_REG4	0x10
+#define TOP_REG5	0x14
+#define TOP_REG6	0x18
+#define TOP_REG7	0x1c
+#define TOP_REG8	0x20
+
+/*
+ * The pin numbering starts from AON pins with reserved ones included,
+ * so that register data like offset and bit position for AON pins can
+ * be calculated from pin number.
+ */
+enum zx296718_pin {
+	/* aon_pmm_reg_0 */
+	I2C3_SCL = 0,
+	I2C3_SDA = 1,
+	AON_RESERVED0 = 2,
+	AON_RESERVED1 = 3,
+	SEC_EN = 4,
+	UART0_RXD = 5,
+	UART0_TXD = 6,
+	IR_IN = 7,
+	SPI0_CLK = 8,
+	SPI0_CS = 9,
+	SPI0_TXD = 10,
+	SPI0_RXD = 11,
+	KEY_COL0 = 12,
+	KEY_COL1 = 13,
+	KEY_COL2 = 14,
+	KEY_ROW0 = 15,
+
+	/* aon_pmm_reg_1 */
+	KEY_ROW1 = 16,
+	KEY_ROW2 = 17,
+	HDMI_SCL = 18,
+	HDMI_SDA = 19,
+	JTAG_TCK = 20,
+	JTAG_TRSTN = 21,
+	JTAG_TMS = 22,
+	JTAG_TDI = 23,
+	JTAG_TDO = 24,
+	I2C0_SCL = 25,
+	I2C0_SDA = 26,
+	I2C1_SCL = 27,
+	I2C1_SDA = 28,
+	AON_RESERVED2 = 29,
+	AON_RESERVED3 = 30,
+	AON_RESERVED4 = 31,
+
+	/* aon_pmm_reg_2 */
+	SPI1_CLK = 32,
+	SPI1_CS = 33,
+	SPI1_TXD = 34,
+	SPI1_RXD = 35,
+	AON_RESERVED5 = 36,
+	AON_RESERVED6 = 37,
+	AUDIO_DET = 38,
+	SPDIF_OUT = 39,
+	HDMI_CEC = 40,
+	HDMI_HPD = 41,
+	GMAC_25M_OUT = 42,
+	BOOT_SEL0 = 43,
+	BOOT_SEL1 = 44,
+	BOOT_SEL2 = 45,
+	DEEP_SLEEP_OUT_N = 46,
+	AON_RESERVED7 = 47,
+
+	/* top_pmm_reg_0 */
+	GMII_GTX_CLK = 48,
+	GMII_TX_CLK = 49,
+	GMII_TXD0 = 50,
+	GMII_TXD1 = 51,
+	GMII_TXD2 = 52,
+	GMII_TXD3 = 53,
+	GMII_TXD4 = 54,
+	GMII_TXD5 = 55,
+	GMII_TXD6 = 56,
+	GMII_TXD7 = 57,
+	GMII_TX_ER = 58,
+	GMII_TX_EN = 59,
+	GMII_RX_CLK = 60,
+	GMII_RXD0 = 61,
+	GMII_RXD1 = 62,
+	GMII_RXD2 = 63,
+
+	/* top_pmm_reg_1 */
+	GMII_RXD3 = 64,
+	GMII_RXD4 = 65,
+	GMII_RXD5 = 66,
+	GMII_RXD6 = 67,
+	GMII_RXD7 = 68,
+	GMII_RX_ER = 69,
+	GMII_RX_DV = 70,
+	GMII_COL = 71,
+	GMII_CRS = 72,
+	GMII_MDC = 73,
+	GMII_MDIO = 74,
+	SDIO1_CLK = 75,
+	SDIO1_CMD = 76,
+	SDIO1_DATA0 = 77,
+	SDIO1_DATA1 = 78,
+	SDIO1_DATA2 = 79,
+
+	/* top_pmm_reg_2 */
+	SDIO1_DATA3 = 80,
+	SDIO1_CD = 81,
+	SDIO1_WP = 82,
+	USIM1_CD = 83,
+	USIM1_CLK = 84,
+	USIM1_RST = 85,
+
+	/* top_pmm_reg_3 */
+	USIM1_DATA = 86,
+	SDIO0_CLK = 87,
+	SDIO0_CMD = 88,
+	SDIO0_DATA0 = 89,
+	SDIO0_DATA1 = 90,
+	SDIO0_DATA2 = 91,
+	SDIO0_DATA3 = 92,
+	SDIO0_CD = 93,
+	SDIO0_WP = 94,
+
+	/* top_pmm_reg_4 */
+	TSI0_DATA0 = 95,
+	SPINOR_CLK = 96,
+	TSI2_DATA = 97,
+	TSI2_CLK = 98,
+	TSI2_SYNC = 99,
+	TSI2_VALID = 100,
+	SPINOR_CS = 101,
+	SPINOR_DQ0 = 102,
+	SPINOR_DQ1 = 103,
+	SPINOR_DQ2 = 104,
+	SPINOR_DQ3 = 105,
+	VGA_HS = 106,
+	VGA_VS = 107,
+	TSI3_DATA = 108,
+
+	/* top_pmm_reg_5 */
+	TSI3_CLK = 109,
+	TSI3_SYNC = 110,
+	TSI3_VALID = 111,
+	I2S1_WS = 112,
+	I2S1_BCLK = 113,
+	I2S1_MCLK = 114,
+	I2S1_DIN0 = 115,
+	I2S1_DOUT0 = 116,
+	SPI3_CLK = 117,
+	SPI3_CS = 118,
+	SPI3_TXD = 119,
+	NAND_LDO_MS18_SEL = 120,
+
+	/* top_pmm_reg_6 */
+	SPI3_RXD = 121,
+	I2S0_MCLK = 122,
+	I2S0_BCLK = 123,
+	I2S0_WS = 124,
+	I2S0_DIN0 = 125,
+	I2S0_DOUT0 = 126,
+	I2C5_SCL = 127,
+	I2C5_SDA = 128,
+	SPI2_CLK = 129,
+	SPI2_CS = 130,
+	SPI2_TXD = 131,
+
+	/* top_pmm_reg_7 */
+	SPI2_RXD = 132,
+	NAND_WP_N = 133,
+	NAND_PAGE_SIZE0 = 134,
+	NAND_PAGE_SIZE1 = 135,
+	NAND_ADDR_CYCLE = 136,
+	NAND_RB0 = 137,
+	NAND_RB1 = 138,
+	NAND_RB2 = 139,
+	NAND_RB3 = 140,
+
+	/* top_pmm_reg_8 */
+	GMAC_125M_IN = 141,
+	GMAC_50M_OUT = 142,
+	SPINOR_SSCLK_LOOPBACK = 143,
+	SPINOR_SDIO1CLK_LOOPBACK = 144,
+};
+
+static const struct pinctrl_pin_desc zx296718_pins[] = {
+	/* aon_pmm_reg_0 */
+	AON_PIN(I2C3_SCL, TOP_REG2, 18, 2, 0x48, 0,
+		AON_MUX(0x0, "ANMI"),		/* anmi */
+		AON_MUX(0x1, "AGPIO"),		/* agpio29 */
+		AON_MUX(0x2, "nonAON"),		/* pin0 */
+		AON_MUX(0x3, "EXT_INT"),	/* int4 */
+		TOP_MUX(0x0, "I2C3"),		/* scl */
+		TOP_MUX(0x1, "SPI2"),		/* txd */
+		TOP_MUX(0x2, "I2S1")),		/* din0 */
+	AON_PIN(I2C3_SDA, TOP_REG2, 20, 2, 0x48, 9,
+		AON_MUX(0x0, "WD"),		/* rst_b */
+		AON_MUX(0x1, "AGPIO"),		/* agpio30 */
+		AON_MUX(0x2, "nonAON"),		/* pin1 */
+		AON_MUX(0x3, "EXT_INT"),	/* int5 */
+		TOP_MUX(0x0, "I2C3"),		/* sda */
+		TOP_MUX(0x1, "SPI2"),		/* rxd */
+		TOP_MUX(0x2, "I2S0")),		/* mclk */
+	ZX_RESERVED(AON_RESERVED0),
+	ZX_RESERVED(AON_RESERVED1),
+	AON_PIN(SEC_EN, TOP_REG3, 5, 1, 0x50, 0,
+		AON_MUX(0x0, "SEC"),		/* en */
+		AON_MUX(0x1, "AGPIO"),		/* agpio28 */
+		AON_MUX(0x2, "nonAON"),		/* pin3 */
+		AON_MUX(0x3, "EXT_INT"),	/* int7 */
+		TOP_MUX(0x0, "I2C2"),		/* sda */
+		TOP_MUX(0x1, "SPI2")),		/* cs */
+	AON_PIN(UART0_RXD, 0, 0, 0, 0x50, 9,
+		AON_MUX(0x0, "UART0"),		/* rxd */
+		AON_MUX(0x1, "AGPIO"),		/* agpio20 */
+		AON_MUX(0x2, "nonAON")),	/* pin34 */
+	AON_PIN(UART0_TXD, 0, 0, 0, 0x50, 18,
+		AON_MUX(0x0, "UART0"),		/* txd */
+		AON_MUX(0x1, "AGPIO"),		/* agpio21 */
+		AON_MUX(0x2, "nonAON")),	/* pin32 */
+	AON_PIN(IR_IN, 0, 0, 0, 0x64, 0,
+		AON_MUX(0x0, "IR"),		/* in */
+		AON_MUX(0x1, "AGPIO"),		/* agpio0 */
+		AON_MUX(0x2, "nonAON")),	/* pin27 */
+	AON_PIN(SPI0_CLK, TOP_REG3, 16, 1, 0x64, 9,
+		AON_MUX(0x0, "EXT_INT"),	/* int0 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio23 */
+		AON_MUX(0x2, "nonAON"),		/* pin5 */
+		AON_MUX(0x3, "PCU"),		/* test6 */
+		TOP_MUX(0x0, "SPI0"),		/* clk */
+		TOP_MUX(0x1, "ISP")),		/* flash_trig */
+	AON_PIN(SPI0_CS, TOP_REG3, 17, 1, 0x64, 18,
+		AON_MUX(0x0, "EXT_INT"),	/* int1 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio24 */
+		AON_MUX(0x2, "nonAON"),		/* pin6 */
+		AON_MUX(0x3, "PCU"),		/* test0 */
+		TOP_MUX(0x0, "SPI0"),		/* cs */
+		TOP_MUX(0x1, "ISP")),		/* prelight_trig */
+	AON_PIN(SPI0_TXD, TOP_REG3, 18, 1, 0x68, 0,
+		AON_MUX(0x0, "EXT_INT"),	/* int2 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio25 */
+		AON_MUX(0x2, "nonAON"),		/* pin7 */
+		AON_MUX(0x3, "PCU"),		/* test1 */
+		TOP_MUX(0x0, "SPI0"),		/* txd */
+		TOP_MUX(0x1, "ISP")),		/* shutter_trig */
+	AON_PIN(SPI0_RXD, TOP_REG3, 19, 1, 0x68, 9,
+		AON_MUX(0x0, "EXT_INT"),	/* int3 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio26 */
+		AON_MUX(0x2, "nonAON"),		/* pin8 */
+		AON_MUX(0x3, "PCU"),		/* test2 */
+		TOP_MUX(0x0, "SPI0"),		/* rxd */
+		TOP_MUX(0x1, "ISP")),		/* shutter_open */
+	AON_PIN(KEY_COL0, TOP_REG3, 20, 1, 0x68, 18,
+		AON_MUX(0x0, "KEY"),		/* col0 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio5 */
+		AON_MUX(0x2, "nonAON"),		/* pin9 */
+		AON_MUX(0x3, "PCU"),		/* test3 */
+		TOP_MUX(0x0, "UART3"),		/* rxd */
+		TOP_MUX(0x1, "I2S0")),		/* din1 */
+	AON_PIN(KEY_COL1, TOP_REG3, 21, 2, 0x6c, 0,
+		AON_MUX(0x0, "KEY"),		/* col1 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio6 */
+		AON_MUX(0x2, "nonAON"),		/* pin10 */
+		TOP_MUX(0x0, "UART3"),		/* txd */
+		TOP_MUX(0x1, "I2S0"),		/* din2 */
+		TOP_MUX(0x2, "VGA")),		/* scl */
+	AON_PIN(KEY_COL2, TOP_REG3, 23, 2, 0x6c, 9,
+		AON_MUX(0x0, "KEY"),		/* col2 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio7 */
+		AON_MUX(0x2, "nonAON"),		/* pin11 */
+		TOP_MUX(0x0, "PWM"),		/* out1 */
+		TOP_MUX(0x1, "I2S0"),		/* din3 */
+		TOP_MUX(0x2, "VGA")),		/* sda */
+	AON_PIN(KEY_ROW0, 0, 0, 0, 0x6c, 18,
+		AON_MUX(0x0, "KEY"),		/* row0 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio8 */
+		AON_MUX(0x2, "nonAON"),		/* pin33 */
+		AON_MUX(0x3, "WD")),		/* rst_b */
+
+	/* aon_pmm_reg_1 */
+	AON_PIN(KEY_ROW1, TOP_REG3, 25, 2, 0x70, 0,
+		AON_MUX(0x0, "KEY"),		/* row1 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio9 */
+		AON_MUX(0x2, "nonAON"),		/* pin12 */
+		TOP_MUX(0x0, "LCD"),		/* port0 lcd_te */
+		TOP_MUX(0x1, "I2S0"),		/* dout2 */
+		TOP_MUX(0x2, "PWM"),		/* out2 */
+		TOP_MUX(0x3, "VGA")),		/* hs1 */
+	AON_PIN(KEY_ROW2, TOP_REG3, 27, 2, 0x70, 9,
+		AON_MUX(0x0, "KEY"),		/* row2 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio10 */
+		AON_MUX(0x2, "nonAON"),		/* pin13 */
+		TOP_MUX(0x0, "LCD"),		/* port1 lcd_te */
+		TOP_MUX(0x1, "I2S0"),		/* dout3 */
+		TOP_MUX(0x2, "PWM"),		/* out3 */
+		TOP_MUX(0x3, "VGA")),		/* vs1 */
+	AON_PIN(HDMI_SCL, TOP_REG3, 29, 1, 0x70, 18,
+		AON_MUX(0x0, "PCU"),		/* test7 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio3 */
+		AON_MUX(0x2, "nonAON"),		/* pin14 */
+		TOP_MUX(0x0, "HDMI"),		/* scl */
+		TOP_MUX(0x1, "UART3")),		/* rxd */
+	AON_PIN(HDMI_SDA, TOP_REG3, 30, 1, 0x74, 0,
+		AON_MUX(0x0, "PCU"),		/* test8 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio4 */
+		AON_MUX(0x2, "nonAON"),		/* pin15 */
+		TOP_MUX(0x0, "HDMI"),		/* sda */
+		TOP_MUX(0x1, "UART3")),		/* txd */
+	AON_PIN(JTAG_TCK, TOP_REG7, 3, 1, 0x78, 18,
+		AON_MUX(0x0, "JTAG"),		/* tck */
+		AON_MUX(0x1, "AGPIO"),		/* agpio11 */
+		AON_MUX(0x2, "nonAON"),		/* pin22 */
+		AON_MUX(0x3, "EXT_INT"),	/* int4 */
+		TOP_MUX(0x0, "SPI4"),		/* clk */
+		TOP_MUX(0x1, "UART1")),		/* rxd */
+	AON_PIN(JTAG_TRSTN, TOP_REG7, 4, 1, 0xac, 0,
+		AON_MUX(0x0, "JTAG"),		/* trstn */
+		AON_MUX(0x1, "AGPIO"),		/* agpio12 */
+		AON_MUX(0x2, "nonAON"),		/* pin23 */
+		AON_MUX(0x3, "EXT_INT"),	/* int5 */
+		TOP_MUX(0x0, "SPI4"),		/* cs */
+		TOP_MUX(0x1, "UART1")),		/* txd */
+	AON_PIN(JTAG_TMS, TOP_REG7, 5, 1, 0xac, 9,
+		AON_MUX(0x0, "JTAG"),		/* tms */
+		AON_MUX(0x1, "AGPIO"),		/* agpio13 */
+		AON_MUX(0x2, "nonAON"),		/* pin24 */
+		AON_MUX(0x3, "EXT_INT"),	/* int6 */
+		TOP_MUX(0x0, "SPI4"),		/* txd */
+		TOP_MUX(0x1, "UART2")),		/* rxd */
+	AON_PIN(JTAG_TDI, TOP_REG7, 6, 1, 0xac, 18,
+		AON_MUX(0x0, "JTAG"),		/* tdi */
+		AON_MUX(0x1, "AGPIO"),		/* agpio14 */
+		AON_MUX(0x2, "nonAON"),		/* pin25 */
+		AON_MUX(0x3, "EXT_INT"),	/* int7 */
+		TOP_MUX(0x0, "SPI4"),		/* rxd */
+		TOP_MUX(0x1, "UART2")),		/* txd */
+	AON_PIN(JTAG_TDO, 0, 0, 0, 0xb0, 0,
+		AON_MUX(0x0, "JTAG"),		/* tdo */
+		AON_MUX(0x1, "AGPIO"),		/* agpio15 */
+		AON_MUX(0x2, "nonAON")),	/* pin26 */
+	AON_PIN(I2C0_SCL, 0, 0, 0, 0xb0, 9,
+		AON_MUX(0x0, "I2C0"),		/* scl */
+		AON_MUX(0x1, "AGPIO"),		/* agpio16 */
+		AON_MUX(0x2, "nonAON")),	/* pin28 */
+	AON_PIN(I2C0_SDA, 0, 0, 0, 0xb0, 18,
+		AON_MUX(0x0, "I2C0"),		/* sda */
+		AON_MUX(0x1, "AGPIO"),		/* agpio17 */
+		AON_MUX(0x2, "nonAON")),	/* pin29 */
+	AON_PIN(I2C1_SCL, TOP_REG8, 4, 1, 0xb4, 0,
+		AON_MUX(0x0, "I2C1"),		/* scl */
+		AON_MUX(0x1, "AGPIO"),		/* agpio18 */
+		AON_MUX(0x2, "nonAON"),		/* pin30 */
+		TOP_MUX(0x0, "LCD")),		/* port0 lcd_te */
+	AON_PIN(I2C1_SDA, TOP_REG8, 5, 1, 0xb4, 9,
+		AON_MUX(0x0, "I2C1"),		/* sda */
+		AON_MUX(0x1, "AGPIO"),		/* agpio19 */
+		AON_MUX(0x2, "nonAON"),		/* pin31 */
+		TOP_MUX(0x0, "LCD")),		/* port1 lcd_te */
+	ZX_RESERVED(AON_RESERVED2),
+	ZX_RESERVED(AON_RESERVED3),
+	ZX_RESERVED(AON_RESERVED4),
+
+	/* aon_pmm_reg_2 */
+	AON_PIN(SPI1_CLK, TOP_REG2, 6, 3, 0x40, 9,
+		AON_MUX(0x0, "EXT_INT"),	/* int0 */
+		AON_MUX(0x1, "PCU"),		/* test12 */
+		AON_MUX(0x2, "nonAON"),		/* pin39 */
+		TOP_MUX(0x0, "SPI1"),		/* clk */
+		TOP_MUX(0x1, "PCM"),		/* clk */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio35 */
+		TOP_MUX(0x3, "I2C4"),		/* scl */
+		TOP_MUX(0x4, "I2S1"),		/* mclk */
+		TOP_MUX(0x5, "ISP")),		/* flash_trig */
+	AON_PIN(SPI1_CS, TOP_REG2, 9, 3, 0x40, 18,
+		AON_MUX(0x0, "EXT_INT"),	/* int1 */
+		AON_MUX(0x1, "PCU"),		/* test13 */
+		AON_MUX(0x2, "nonAON"),		/* pin40 */
+		TOP_MUX(0x0, "SPI1"),		/* cs */
+		TOP_MUX(0x1, "PCM"),		/* fs */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio36 */
+		TOP_MUX(0x3, "I2C4"),		/* sda */
+		TOP_MUX(0x4, "I2S1"),		/* bclk */
+		TOP_MUX(0x5, "ISP")),		/* prelight_trig */
+	AON_PIN(SPI1_TXD, TOP_REG2, 12, 3, 0x44, 0,
+		AON_MUX(0x0, "EXT_INT"),	/* int2 */
+		AON_MUX(0x1, "PCU"),		/* test14 */
+		AON_MUX(0x2, "nonAON"),		/* pin41 */
+		TOP_MUX(0x0, "SPI1"),		/* txd */
+		TOP_MUX(0x1, "PCM"),		/* txd */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio37 */
+		TOP_MUX(0x3, "UART5"),		/* rxd */
+		TOP_MUX(0x4, "I2S1"),		/* ws */
+		TOP_MUX(0x5, "ISP")),		/* shutter_trig */
+	AON_PIN(SPI1_RXD, TOP_REG2, 15, 3, 0x44, 9,
+		AON_MUX(0x0, "EXT_INT"),	/* int3 */
+		AON_MUX(0x1, "PCU"),		/* test15 */
+		AON_MUX(0x2, "nonAON"),		/* pin42 */
+		TOP_MUX(0x0, "SPI1"),		/* rxd */
+		TOP_MUX(0x1, "PCM"),		/* rxd */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio38 */
+		TOP_MUX(0x3, "UART5"),		/* txd */
+		TOP_MUX(0x4, "I2S1"),		/* dout0 */
+		TOP_MUX(0x5, "ISP")),		/* shutter_open */
+	ZX_RESERVED(AON_RESERVED5),
+	ZX_RESERVED(AON_RESERVED6),
+	AON_PIN(AUDIO_DET, TOP_REG3, 3, 2, 0x48, 18,
+		AON_MUX(0x0, "PCU"),		/* test4 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio27 */
+		AON_MUX(0x2, "nonAON"),		/* pin2 */
+		AON_MUX(0x3, "EXT_INT"),	/* int16 */
+		TOP_MUX(0x0, "AUDIO"),		/* detect */
+		TOP_MUX(0x1, "I2C2"),		/* scl */
+		TOP_MUX(0x2, "SPI2")),		/* clk */
+	AON_PIN(SPDIF_OUT, TOP_REG3, 14, 2, 0x78, 9,
+		AON_MUX(0x0, "PCU"),		/* test5 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio22 */
+		AON_MUX(0x2, "nonAON"),		/* pin4 */
+		TOP_MUX(0x0, "SPDIF"),		/* out */
+		TOP_MUX(0x1, "PWM"),		/* out0 */
+		TOP_MUX(0x2, "ISP")),		/* fl_trig */
+	AON_PIN(HDMI_CEC, 0, 0, 0, 0x74, 9,
+		AON_MUX(0x0, "PCU"),		/* test9 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio1 */
+		AON_MUX(0x2, "nonAON")),	/* pin16 */
+	AON_PIN(HDMI_HPD, 0, 0, 0, 0x74, 18,
+		AON_MUX(0x0, "PCU"),		/* test10 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio2 */
+		AON_MUX(0x2, "nonAON")),	/* pin17 */
+	AON_PIN(GMAC_25M_OUT, 0, 0, 0, 0x78, 0,
+		AON_MUX(0x0, "PCU"),		/* test11 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio31 */
+		AON_MUX(0x2, "nonAON")),	/* pin43 */
+	AON_PIN(BOOT_SEL0, 0, 0, 0, 0xc0, 9,
+		AON_MUX(0x0, "BOOT"),		/* sel0 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio18 */
+		AON_MUX(0x2, "nonAON")),	/* pin18 */
+	AON_PIN(BOOT_SEL1, 0, 0, 0, 0xc0, 18,
+		AON_MUX(0x0, "BOOT"),		/* sel1 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio19 */
+		AON_MUX(0x2, "nonAON")),	/* pin19 */
+	AON_PIN(BOOT_SEL2, 0, 0, 0, 0xc4, 0,
+		AON_MUX(0x0, "BOOT"),		/* sel2 */
+		AON_MUX(0x1, "AGPIO"),		/* agpio20 */
+		AON_MUX(0x2, "nonAON")),	/* pin20 */
+	AON_PIN(DEEP_SLEEP_OUT_N, 0, 0, 0, 0xc4, 9,
+		AON_MUX(0x0, "DEEPSLP"),	/* deep sleep out_n */
+		AON_MUX(0x1, "AGPIO"),		/* agpio21 */
+		AON_MUX(0x2, "nonAON")),	/* pin21 */
+	ZX_RESERVED(AON_RESERVED7),
+
+	/* top_pmm_reg_0 */
+	TOP_PIN(GMII_GTX_CLK, TOP_REG0, 0, 2, 0x10, 0,
+		TOP_MUX(0x0, "GMII"),		/* gtx_clk */
+		TOP_MUX(0x1, "DVI0"),		/* clk */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio0 */
+	TOP_PIN(GMII_TX_CLK, TOP_REG0, 2, 2, 0x10, 9,
+		TOP_MUX(0x0, "GMII"),		/* tx_clk */
+		TOP_MUX(0x1, "DVI0"),		/* vs */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio1 */
+	TOP_PIN(GMII_TXD0, TOP_REG0, 4, 2, 0x10, 18,
+		TOP_MUX(0x0, "GMII"),		/* txd0 */
+		TOP_MUX(0x1, "DVI0"),		/* hs */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio2 */
+	TOP_PIN(GMII_TXD1, TOP_REG0, 6, 2, 0x14, 0,
+		TOP_MUX(0x0, "GMII"),		/* txd1 */
+		TOP_MUX(0x1, "DVI0"),		/* d0 */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio3 */
+	TOP_PIN(GMII_TXD2, TOP_REG0, 8, 2, 0x14, 9,
+		TOP_MUX(0x0, "GMII"),		/* txd2 */
+		TOP_MUX(0x1, "DVI0"),		/* d1 */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio4 */
+	TOP_PIN(GMII_TXD3, TOP_REG0, 10, 2, 0x14, 18,
+		TOP_MUX(0x0, "GMII"),		/* txd3 */
+		TOP_MUX(0x1, "DVI0"),		/* d2 */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio5 */
+	TOP_PIN(GMII_TXD4, TOP_REG0, 12, 2, 0x18, 0,
+		TOP_MUX(0x0, "GMII"),		/* txd4 */
+		TOP_MUX(0x1, "DVI0"),		/* d3 */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio6 */
+	TOP_PIN(GMII_TXD5, TOP_REG0, 14, 2, 0x18, 9,
+		TOP_MUX(0x0, "GMII"),		/* txd5 */
+		TOP_MUX(0x1, "DVI0"),		/* d4 */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio7 */
+	TOP_PIN(GMII_TXD6, TOP_REG0, 16, 2, 0x18, 18,
+		TOP_MUX(0x0, "GMII"),		/* txd6 */
+		TOP_MUX(0x1, "DVI0"),		/* d5 */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio8 */
+	TOP_PIN(GMII_TXD7, TOP_REG0, 18, 2, 0x1c, 0,
+		TOP_MUX(0x0, "GMII"),		/* txd7 */
+		TOP_MUX(0x1, "DVI0"),		/* d6 */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio9 */
+	TOP_PIN(GMII_TX_ER, TOP_REG0, 20, 2, 0x1c, 9,
+		TOP_MUX(0x0, "GMII"),		/* tx_er */
+		TOP_MUX(0x1, "DVI0"),		/* d7 */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio10 */
+	TOP_PIN(GMII_TX_EN, TOP_REG0, 22, 2, 0x1c, 18,
+		TOP_MUX(0x0, "GMII"),		/* tx_en */
+		TOP_MUX(0x1, "DVI0"),		/* d8 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio11 */
+	TOP_PIN(GMII_RX_CLK, TOP_REG0, 24, 2, 0x20, 0,
+		TOP_MUX(0x0, "GMII"),		/* rx_clk */
+		TOP_MUX(0x1, "DVI0"),		/* d9 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio12 */
+	TOP_PIN(GMII_RXD0, TOP_REG0, 26, 2, 0x20, 9,
+		TOP_MUX(0x0, "GMII"),		/* rxd0 */
+		TOP_MUX(0x1, "DVI0"),		/* d10 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio13 */
+	TOP_PIN(GMII_RXD1, TOP_REG0, 28, 2, 0x20, 18,
+		TOP_MUX(0x0, "GMII"),		/* rxd1 */
+		TOP_MUX(0x1, "DVI0"),		/* d11 */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio14 */
+	TOP_PIN(GMII_RXD2, TOP_REG0, 30, 2, 0x24, 0,
+		TOP_MUX(0x0, "GMII"),		/* rxd2 */
+		TOP_MUX(0x1, "DVI1"),		/* clk */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio15 */
+
+	/* top_pmm_reg_1 */
+	TOP_PIN(GMII_RXD3, TOP_REG1, 0, 2, 0x24, 9,
+		TOP_MUX(0x0, "GMII"),		/* rxd3 */
+		TOP_MUX(0x1, "DVI1"),		/* hs */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio16 */
+	TOP_PIN(GMII_RXD4, TOP_REG1, 2, 2, 0x24, 18,
+		TOP_MUX(0x0, "GMII"),		/* rxd4 */
+		TOP_MUX(0x1, "DVI1"),		/* vs */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio17 */
+	TOP_PIN(GMII_RXD5, TOP_REG1, 4, 2, 0x28, 0,
+		TOP_MUX(0x0, "GMII"),		/* rxd5 */
+		TOP_MUX(0x1, "DVI1"),		/* d0 */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio18 */
+		TOP_MUX(0x3, "TSI0")),		/* dat0 */
+	TOP_PIN(GMII_RXD6, TOP_REG1, 6, 2, 0x28, 9,
+		TOP_MUX(0x0, "GMII"),		/* rxd6 */
+		TOP_MUX(0x1, "DVI1"),		/* d1 */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio19 */
+		TOP_MUX(0x3, "TSI0")),		/* clk */
+	TOP_PIN(GMII_RXD7, TOP_REG1, 8, 2, 0x28, 18,
+		TOP_MUX(0x0, "GMII"),		/* rxd7 */
+		TOP_MUX(0x1, "DVI1"),		/* d2 */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio20 */
+		TOP_MUX(0x3, "TSI0")),		/* sync */
+	TOP_PIN(GMII_RX_ER, TOP_REG1, 10, 2, 0x2c, 0,
+		TOP_MUX(0x0, "GMII"),		/* rx_er */
+		TOP_MUX(0x1, "DVI1"),		/* d3 */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio21 */
+		TOP_MUX(0x3, "TSI0")),		/* valid */
+	TOP_PIN(GMII_RX_DV, TOP_REG1, 12, 2, 0x2c, 9,
+		TOP_MUX(0x0, "GMII"),		/* rx_dv */
+		TOP_MUX(0x1, "DVI1"),		/* d4 */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio22 */
+		TOP_MUX(0x3, "TSI1")),		/* dat0 */
+	TOP_PIN(GMII_COL, TOP_REG1, 14, 2, 0x2c, 18,
+		TOP_MUX(0x0, "GMII"),		/* col */
+		TOP_MUX(0x1, "DVI1"),		/* d5 */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio23 */
+		TOP_MUX(0x3, "TSI1")),		/* clk */
+	TOP_PIN(GMII_CRS, TOP_REG1, 16, 2, 0x30, 0,
+		TOP_MUX(0x0, "GMII"),		/* crs */
+		TOP_MUX(0x1, "DVI1"),		/* d6 */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio24 */
+		TOP_MUX(0x3, "TSI1")),		/* sync */
+	TOP_PIN(GMII_MDC, TOP_REG1, 18, 2, 0x30, 9,
+		TOP_MUX(0x0, "GMII"),		/* mdc */
+		TOP_MUX(0x1, "DVI1"),		/* d7 */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio25 */
+		TOP_MUX(0x3, "TSI1")),		/* valid */
+	TOP_PIN(GMII_MDIO, TOP_REG1, 20, 1, 0x30, 18,
+		TOP_MUX(0x0, "GMII"),		/* mdio */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio26 */
+	TOP_PIN(SDIO1_CLK, TOP_REG1, 21, 2, 0x34, 18,
+		TOP_MUX(0x0, "SDIO1"),		/* clk */
+		TOP_MUX(0x1, "USIM0"),		/* clk */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio27 */
+		TOP_MUX(0x3, "SPINOR")),	/* clk */
+	TOP_PIN(SDIO1_CMD, TOP_REG1, 23, 2, 0x38, 0,
+		TOP_MUX(0x0, "SDIO1"),		/* cmd */
+		TOP_MUX(0x1, "USIM0"),		/* cd */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio28 */
+		TOP_MUX(0x3, "SPINOR")),	/* cs */
+	TOP_PIN(SDIO1_DATA0, TOP_REG1, 25, 2, 0x38, 9,
+		TOP_MUX(0x0, "SDIO1"),		/* dat0 */
+		TOP_MUX(0x1, "USIM0"),		/* rst */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio29 */
+		TOP_MUX(0x3, "SPINOR")),	/* dq0 */
+	TOP_PIN(SDIO1_DATA1, TOP_REG1, 27, 2, 0x38, 18,
+		TOP_MUX(0x0, "SDIO1"),		/* dat1 */
+		TOP_MUX(0x1, "USIM0"),		/* data */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio30 */
+		TOP_MUX(0x3, "SPINOR")),	/* dq1 */
+	TOP_PIN(SDIO1_DATA2, TOP_REG1, 29, 2, 0x3c, 0,
+		TOP_MUX(0x0, "SDIO1"),		/* dat2 */
+		TOP_MUX(0x1, "BGPIO"),		/* gpio31 */
+		TOP_MUX(0x2, "SPINOR")),	/* dq2 */
+
+	/* top_pmm_reg_2 */
+	TOP_PIN(SDIO1_DATA3, TOP_REG2, 0, 2, 0x3c, 9,
+		TOP_MUX(0x0, "SDIO1"),		/* dat3 */
+		TOP_MUX(0x1, "BGPIO"),		/* gpio32 */
+		TOP_MUX(0x2, "SPINOR")),	/* dq3 */
+	TOP_PIN(SDIO1_CD, TOP_REG2, 2, 2, 0x3c, 18,
+		TOP_MUX(0x0, "SDIO1"),		/* cd */
+		TOP_MUX(0x1, "BGPIO"),		/* gpio33 */
+		TOP_MUX(0x2, "ISP")),		/* fl_trig */
+	TOP_PIN(SDIO1_WP, TOP_REG2, 4, 2, 0x40, 0,
+		TOP_MUX(0x0, "SDIO1"),		/* wp */
+		TOP_MUX(0x1, "BGPIO"),		/* gpio34 */
+		TOP_MUX(0x2, "ISP")),		/* ref_clk */
+	TOP_PIN(USIM1_CD, TOP_REG2, 22, 3, 0x44, 18,
+		TOP_MUX(0x0, "USIM1"),		/* cd */
+		TOP_MUX(0x1, "UART4"),		/* rxd */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio39 */
+		TOP_MUX(0x3, "SPI3"),		/* clk */
+		TOP_MUX(0x4, "I2S0"),		/* bclk */
+		TOP_MUX(0x5, "B_DVI0")),	/* d8 */
+	TOP_PIN(USIM1_CLK, TOP_REG2, 25, 3, 0x4c, 18,
+		TOP_MUX(0x0, "USIM1"),		/* clk */
+		TOP_MUX(0x1, "UART4"),		/* txd */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio40 */
+		TOP_MUX(0x3, "SPI3"),		/* cs */
+		TOP_MUX(0x4, "I2S0"),		/* ws */
+		TOP_MUX(0x5, "B_DVI0")),	/* d9 */
+	TOP_PIN(USIM1_RST, TOP_REG2, 28, 3, 0x4c, 0,
+		TOP_MUX(0x0, "USIM1"),		/* rst */
+		TOP_MUX(0x1, "UART4"),		/* cts */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio41 */
+		TOP_MUX(0x3, "SPI3"),		/* txd */
+		TOP_MUX(0x4, "I2S0"),		/* dout0 */
+		TOP_MUX(0x5, "B_DVI0")),	/* d10 */
+
+	/* top_pmm_reg_3 */
+	TOP_PIN(USIM1_DATA, TOP_REG3, 0, 3, 0x4c, 9,
+		TOP_MUX(0x0, "USIM1"),		/* dat */
+		TOP_MUX(0x1, "UART4"),		/* rst */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio42 */
+		TOP_MUX(0x3, "SPI3"),		/* rxd */
+		TOP_MUX(0x4, "I2S0"),		/* din0 */
+		TOP_MUX(0x5, "B_DVI0")),	/* d11 */
+	TOP_PIN(SDIO0_CLK, TOP_REG3, 6, 1, 0x58, 0,
+		TOP_MUX(0x0, "SDIO0"),		/* clk */
+		TOP_MUX(0x1, "GPIO")),		/* gpio43 */
+	TOP_PIN(SDIO0_CMD, TOP_REG3, 7, 1, 0x58, 9,
+		TOP_MUX(0x0, "SDIO0"),		/* cmd */
+		TOP_MUX(0x1, "GPIO")),		/* gpio44 */
+	TOP_PIN(SDIO0_DATA0, TOP_REG3, 8, 1, 0x58, 18,
+		TOP_MUX(0x0, "SDIO0"),		/* dat0 */
+		TOP_MUX(0x1, "GPIO")),		/* gpio45 */
+	TOP_PIN(SDIO0_DATA1, TOP_REG3, 9, 1, 0x5c, 0,
+		TOP_MUX(0x0, "SDIO0"),		/* dat1 */
+		TOP_MUX(0x1, "GPIO")),		/* gpio46 */
+	TOP_PIN(SDIO0_DATA2, TOP_REG3, 10, 1, 0x5c, 9,
+		TOP_MUX(0x0, "SDIO0"),		/* dat2 */
+		TOP_MUX(0x1, "GPIO")),		/* gpio47 */
+	TOP_PIN(SDIO0_DATA3, TOP_REG3, 11, 1, 0x5c, 18,
+		TOP_MUX(0x0, "SDIO0"),		/* dat3 */
+		TOP_MUX(0x1, "GPIO")),		/* gpio48 */
+	TOP_PIN(SDIO0_CD, TOP_REG3, 12, 1, 0x60, 0,
+		TOP_MUX(0x0, "SDIO0"),		/* cd */
+		TOP_MUX(0x1, "GPIO")),		/* gpio49 */
+	TOP_PIN(SDIO0_WP, TOP_REG3, 13, 1, 0x60, 9,
+		TOP_MUX(0x0, "SDIO0"),		/* wp */
+		TOP_MUX(0x1, "GPIO")),		/* gpio50 */
+
+	/* top_pmm_reg_4 */
+	TOP_PIN(TSI0_DATA0, TOP_REG4, 0, 2, 0x60, 18,
+		TOP_MUX(0x0, "TSI0"),		/* dat0 */
+		TOP_MUX(0x1, "LCD"),		/* clk */
+		TOP_MUX(0x2, "BGPIO")),		/* gpio51 */
+	TOP_PIN(SPINOR_CLK, TOP_REG4, 2, 2, 0xa8, 18,
+		TOP_MUX(0x0, "SPINOR"),		/* clk */
+		TOP_MUX(0x1, "TSI0"),		/* dat1 */
+		TOP_MUX(0x2, "LCD"),		/* dat0 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio52 */
+	TOP_PIN(TSI2_DATA, TOP_REG4, 4, 2, 0x7c, 0,
+		TOP_MUX(0x0, "TSI2"),		/* dat */
+		TOP_MUX(0x1, "TSI0"),		/* dat2 */
+		TOP_MUX(0x2, "LCD"),		/* dat1 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio53 */
+	TOP_PIN(TSI2_CLK, TOP_REG4, 6, 2, 0x7c, 9,
+		TOP_MUX(0x0, "TSI2"),		/* clk */
+		TOP_MUX(0x1, "TSI0"),		/* dat3 */
+		TOP_MUX(0x2, "LCD"),		/* dat2 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio54 */
+	TOP_PIN(TSI2_SYNC, TOP_REG4, 8, 2, 0x7c, 18,
+		TOP_MUX(0x0, "TSI2"),		/* sync */
+		TOP_MUX(0x1, "TSI0"),		/* dat4 */
+		TOP_MUX(0x2, "LCD"),		/* dat3 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio55 */
+	TOP_PIN(TSI2_VALID, TOP_REG4, 10, 2, 0x80, 0,
+		TOP_MUX(0x0, "TSI2"),		/* valid */
+		TOP_MUX(0x1, "TSI0"),		/* dat5 */
+		TOP_MUX(0x2, "LCD"),		/* dat4 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio56 */
+	TOP_PIN(SPINOR_CS, TOP_REG4, 12, 2, 0x80, 9,
+		TOP_MUX(0x0, "SPINOR"),		/* cs */
+		TOP_MUX(0x1, "TSI0"),		/* dat6 */
+		TOP_MUX(0x2, "LCD"),		/* dat5 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio57 */
+	TOP_PIN(SPINOR_DQ0, TOP_REG4, 14, 2, 0x80, 18,
+		TOP_MUX(0x0, "SPINOR"),		/* dq0 */
+		TOP_MUX(0x1, "TSI0"),		/* dat7 */
+		TOP_MUX(0x2, "LCD"),		/* dat6 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio58 */
+	TOP_PIN(SPINOR_DQ1, TOP_REG4, 16, 2, 0x84, 0,
+		TOP_MUX(0x0, "SPINOR"),		/* dq1 */
+		TOP_MUX(0x1, "TSI0"),		/* clk */
+		TOP_MUX(0x2, "LCD"),		/* dat7 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio59 */
+	TOP_PIN(SPINOR_DQ2, TOP_REG4, 18, 2, 0x84, 9,
+		TOP_MUX(0x0, "SPINOR"),		/* dq2 */
+		TOP_MUX(0x1, "TSI0"),		/* sync */
+		TOP_MUX(0x2, "LCD"),		/* dat8 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio60 */
+	TOP_PIN(SPINOR_DQ3, TOP_REG4, 20, 2, 0x84, 18,
+		TOP_MUX(0x0, "SPINOR"),		/* dq3 */
+		TOP_MUX(0x1, "TSI0"),		/* valid */
+		TOP_MUX(0x2, "LCD"),		/* dat9 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio61 */
+	TOP_PIN(VGA_HS, TOP_REG4, 22, 3, 0x88, 0,
+		TOP_MUX(0x0, "VGA"),		/* hs */
+		TOP_MUX(0x1, "TSI1"),		/* dat0 */
+		TOP_MUX(0x2, "LCD"),		/* dat10 */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio62 */
+		TOP_MUX(0x4, "I2S1"),		/* din1 */
+		TOP_MUX(0x5, "B_DVI0")),	/* clk */
+	TOP_PIN(VGA_VS, TOP_REG4, 25, 3, 0x88, 9,
+		TOP_MUX(0x0, "VGA"),		/* vs0 */
+		TOP_MUX(0x1, "TSI1"),		/* dat1 */
+		TOP_MUX(0x2, "LCD"),		/* dat11 */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio63 */
+		TOP_MUX(0x4, "I2S1"),		/* din2 */
+		TOP_MUX(0x5, "B_DVI0")),	/* vs */
+	TOP_PIN(TSI3_DATA, TOP_REG4, 28, 3, 0x88, 18,
+		TOP_MUX(0x0, "TSI3"),		/* dat */
+		TOP_MUX(0x1, "TSI1"),		/* dat2 */
+		TOP_MUX(0x2, "LCD"),		/* dat12 */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio64 */
+		TOP_MUX(0x4, "I2S1"),		/* din3 */
+		TOP_MUX(0x5, "B_DVI0")),	/* hs */
+
+	/* top_pmm_reg_5 */
+	TOP_PIN(TSI3_CLK, TOP_REG5, 0, 3, 0x8c, 0,
+		TOP_MUX(0x0, "TSI3"),		/* clk */
+		TOP_MUX(0x1, "TSI1"),		/* dat3 */
+		TOP_MUX(0x2, "LCD"),		/* dat13 */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio65 */
+		TOP_MUX(0x4, "I2S1"),		/* dout1 */
+		TOP_MUX(0x5, "B_DVI0")),	/* d0 */
+	TOP_PIN(TSI3_SYNC, TOP_REG5, 3, 3, 0x8c, 9,
+		TOP_MUX(0x0, "TSI3"),		/* sync */
+		TOP_MUX(0x1, "TSI1"),		/* dat4 */
+		TOP_MUX(0x2, "LCD"),		/* dat14 */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio66 */
+		TOP_MUX(0x4, "I2S1"),		/* dout2 */
+		TOP_MUX(0x5, "B_DVI0")),	/* d1 */
+	TOP_PIN(TSI3_VALID, TOP_REG5, 6, 3, 0x8c, 18,
+		TOP_MUX(0x0, "TSI3"),		/* valid */
+		TOP_MUX(0x1, "TSI1"),		/* dat5 */
+		TOP_MUX(0x2, "LCD"),		/* dat15 */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio67 */
+		TOP_MUX(0x4, "I2S1"),		/* dout3 */
+		TOP_MUX(0x5, "B_DVI0")),	/* d2 */
+	TOP_PIN(I2S1_WS, TOP_REG5, 9, 3, 0x90, 0,
+		TOP_MUX(0x0, "I2S1"),		/* ws */
+		TOP_MUX(0x1, "TSI1"),		/* dat6 */
+		TOP_MUX(0x2, "LCD"),		/* dat16 */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio68 */
+		TOP_MUX(0x4, "VGA"),		/* scl */
+		TOP_MUX(0x5, "B_DVI0")),	/* d3 */
+	TOP_PIN(I2S1_BCLK, TOP_REG5, 12, 3, 0x90, 9,
+		TOP_MUX(0x0, "I2S1"),		/* bclk */
+		TOP_MUX(0x1, "TSI1"),		/* dat7 */
+		TOP_MUX(0x2, "LCD"),		/* dat17 */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio69 */
+		TOP_MUX(0x4, "VGA"),		/* sda */
+		TOP_MUX(0x5, "B_DVI0")),	/* d4 */
+	TOP_PIN(I2S1_MCLK, TOP_REG5, 15, 2, 0x90, 18,
+		TOP_MUX(0x0, "I2S1"),		/* mclk */
+		TOP_MUX(0x1, "TSI1"),		/* clk */
+		TOP_MUX(0x2, "LCD"),		/* dat18 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio70 */
+	TOP_PIN(I2S1_DIN0, TOP_REG5, 17, 2, 0x94, 0,
+		TOP_MUX(0x0, "I2S1"),		/* din0 */
+		TOP_MUX(0x1, "TSI1"),		/* sync */
+		TOP_MUX(0x2, "LCD"),		/* dat19 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio71 */
+	TOP_PIN(I2S1_DOUT0, TOP_REG5, 19, 2, 0x94, 9,
+		TOP_MUX(0x0, "I2S1"),		/* dout0 */
+		TOP_MUX(0x1, "TSI1"),		/* valid */
+		TOP_MUX(0x2, "LCD"),		/* dat20 */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio72 */
+	TOP_PIN(SPI3_CLK, TOP_REG5, 21, 3, 0x94, 18,
+		TOP_MUX(0x0, "SPI3"),		/* clk */
+		TOP_MUX(0x1, "TSO1"),		/* clk */
+		TOP_MUX(0x2, "LCD"),		/* dat21 */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio73 */
+		TOP_MUX(0x4, "UART5"),		/* rxd */
+		TOP_MUX(0x5, "PCM"),		/* fs */
+		TOP_MUX(0x6, "I2S0"),		/* din1 */
+		TOP_MUX(0x7, "B_DVI0")),	/* d5 */
+	TOP_PIN(SPI3_CS, TOP_REG5, 24, 3, 0x98, 0,
+		TOP_MUX(0x0, "SPI3"),		/* cs */
+		TOP_MUX(0x1, "TSO1"),		/* dat0 */
+		TOP_MUX(0x2, "LCD"),		/* dat22 */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio74 */
+		TOP_MUX(0x4, "UART5"),		/* txd */
+		TOP_MUX(0x5, "PCM"),		/* clk */
+		TOP_MUX(0x6, "I2S0"),		/* din2 */
+		TOP_MUX(0x7, "B_DVI0")),	/* d6 */
+	TOP_PIN(SPI3_TXD, TOP_REG5, 27, 3, 0x98, 9,
+		TOP_MUX(0x0, "SPI3"),		/* txd */
+		TOP_MUX(0x1, "TSO1"),		/* dat1 */
+		TOP_MUX(0x2, "LCD"),		/* dat23 */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio75 */
+		TOP_MUX(0x4, "UART5"),		/* cts */
+		TOP_MUX(0x5, "PCM"),		/* txd */
+		TOP_MUX(0x6, "I2S0"),		/* din3 */
+		TOP_MUX(0x7, "B_DVI0")),	/* d7 */
+	TOP_PIN(NAND_LDO_MS18_SEL, TOP_REG5, 30, 1, 0xe4, 0,
+		TOP_MUX(0x0, "NAND"),		/* ldo_ms18_sel */
+		TOP_MUX(0x1, "BGPIO")),		/* gpio99 */
+
+	/* top_pmm_reg_6 */
+	TOP_PIN(SPI3_RXD, TOP_REG6, 0, 3, 0x98, 18,
+		TOP_MUX(0x0, "SPI3"),		/* rxd */
+		TOP_MUX(0x1, "TSO1"),		/* dat2 */
+		TOP_MUX(0x2, "LCD"),		/* stvu_vsync */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio76 */
+		TOP_MUX(0x4, "UART5"),		/* rts */
+		TOP_MUX(0x5, "PCM"),		/* rxd */
+		TOP_MUX(0x6, "I2S0"),		/* dout1 */
+		TOP_MUX(0x7, "B_DVI1")),	/* clk */
+	TOP_PIN(I2S0_MCLK, TOP_REG6, 3, 3, 0x9c, 0,
+		TOP_MUX(0x0, "I2S0"),		/* mclk */
+		TOP_MUX(0x1, "TSO1"),		/* dat3 */
+		TOP_MUX(0x2, "LCD"),		/* stvd */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio77 */
+		TOP_MUX(0x4, "USIM0"),		/* cd */
+		TOP_MUX(0x5, "B_DVI1")),	/* vs */
+	TOP_PIN(I2S0_BCLK, TOP_REG6, 6, 3, 0x9c, 9,
+		TOP_MUX(0x0, "I2S0"),		/* bclk */
+		TOP_MUX(0x1, "TSO1"),		/* dat4 */
+		TOP_MUX(0x2, "LCD"),		/* sthl_hsync */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio78 */
+		TOP_MUX(0x4, "USIM0"),		/* clk */
+		TOP_MUX(0x5, "B_DVI1")),	/* hs */
+	TOP_PIN(I2S0_WS, TOP_REG6, 9, 3, 0x9c, 18,
+		TOP_MUX(0x0, "I2S0"),		/* ws */
+		TOP_MUX(0x1, "TSO1"),		/* dat5 */
+		TOP_MUX(0x2, "LCD"),		/* sthr */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio79 */
+		TOP_MUX(0x4, "USIM0"),		/* rst */
+		TOP_MUX(0x5, "B_DVI1")),	/* d0 */
+	TOP_PIN(I2S0_DIN0, TOP_REG6, 12, 3, 0xa0, 0,
+		TOP_MUX(0x0, "I2S0"),		/* din0 */
+		TOP_MUX(0x1, "TSO1"),		/* dat6 */
+		TOP_MUX(0x2, "LCD"),		/* oev_dataen */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio80 */
+		TOP_MUX(0x4, "USIM0"),		/* dat */
+		TOP_MUX(0x5, "B_DVI1")),	/* d1 */
+	TOP_PIN(I2S0_DOUT0, TOP_REG6, 15, 2, 0xa0, 9,
+		TOP_MUX(0x0, "I2S0"),		/* dout0 */
+		TOP_MUX(0x1, "TSO1"),		/* dat7 */
+		TOP_MUX(0x2, "LCD"),		/* ckv */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio81 */
+	TOP_PIN(I2C5_SCL, TOP_REG6, 17, 3, 0xa0, 18,
+		TOP_MUX(0x0, "I2C5"),		/* scl */
+		TOP_MUX(0x1, "TSO1"),		/* sync */
+		TOP_MUX(0x2, "LCD"),		/* ld */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio82 */
+		TOP_MUX(0x4, "PWM"),		/* out2 */
+		TOP_MUX(0x5, "I2S0"),		/* dout2 */
+		TOP_MUX(0x6, "B_DVI1")),	/* d2 */
+	TOP_PIN(I2C5_SDA, TOP_REG6, 20, 3, 0xa4, 0,
+		TOP_MUX(0x0, "I2C5"),		/* sda */
+		TOP_MUX(0x1, "TSO1"),		/* vld */
+		TOP_MUX(0x2, "LCD"),		/* pol */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio83 */
+		TOP_MUX(0x4, "PWM"),		/* out3 */
+		TOP_MUX(0x5, "I2S0"),		/* dout3 */
+		TOP_MUX(0x6, "B_DVI1")),	/* d3 */
+	TOP_PIN(SPI2_CLK, TOP_REG6, 23, 3, 0xa4, 9,
+		TOP_MUX(0x0, "SPI2"),		/* clk */
+		TOP_MUX(0x1, "TSO0"),		/* clk */
+		TOP_MUX(0x2, "LCD"),		/* degsl */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio84 */
+		TOP_MUX(0x4, "I2C4"),		/* scl */
+		TOP_MUX(0x5, "B_DVI1")),	/* d4 */
+	TOP_PIN(SPI2_CS, TOP_REG6, 26, 3, 0xa4, 18,
+		TOP_MUX(0x0, "SPI2"),		/* cs */
+		TOP_MUX(0x1, "TSO0"),		/* data */
+		TOP_MUX(0x2, "LCD"),		/* rev */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio85 */
+		TOP_MUX(0x4, "I2C4"),		/* sda */
+		TOP_MUX(0x5, "B_DVI1")),	/* d5 */
+	TOP_PIN(SPI2_TXD, TOP_REG6, 29, 3, 0xa8, 0,
+		TOP_MUX(0x0, "SPI2"),		/* txd */
+		TOP_MUX(0x1, "TSO0"),		/* sync */
+		TOP_MUX(0x2, "LCD"),		/* u_d */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio86 */
+		TOP_MUX(0x4, "I2C4"),		/* scl */
+		TOP_MUX(0x5, "B_DVI1")),	/* d6 */
+
+	/* top_pmm_reg_7 */
+	TOP_PIN(SPI2_RXD, TOP_REG7, 0, 3, 0xa8, 9,
+		TOP_MUX(0x0, "SPI2"),		/* rxd */
+		TOP_MUX(0x1, "TSO0"),		/* vld */
+		TOP_MUX(0x2, "LCD"),		/* r_l */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio87 */
+		TOP_MUX(0x4, "I2C3"),		/* sda */
+		TOP_MUX(0x5, "B_DVI1")),	/* d7 */
+	TOP_PIN(NAND_WP_N, TOP_REG7, 7, 3, 0x54, 9,
+		TOP_MUX(0x0, "NAND"),		/* wp */
+		TOP_MUX(0x1, "PWM"),		/* out2 */
+		TOP_MUX(0x2, "SPI2"),		/* clk */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio88 */
+		TOP_MUX(0x4, "TSI0"),		/* dat0 */
+		TOP_MUX(0x5, "I2S1")),		/* din1 */
+	TOP_PIN(NAND_PAGE_SIZE0, TOP_REG7, 10, 3, 0xb8, 0,
+		TOP_MUX(0x0, "NAND"),		/* boot_pagesize0 */
+		TOP_MUX(0x1, "PWM"),		/* out3 */
+		TOP_MUX(0x2, "SPI2"),		/* cs */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio89 */
+		TOP_MUX(0x4, "TSI0"),		/* clk */
+		TOP_MUX(0x5, "I2S1")),		/* din2 */
+	TOP_PIN(NAND_PAGE_SIZE1, TOP_REG7, 13, 3, 0xb8, 9,
+		TOP_MUX(0x0, "NAND"),		/* boot_pagesize1 */
+		TOP_MUX(0x1, "I2C4"),		/* scl */
+		TOP_MUX(0x2, "SPI2"),		/* txd */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio90 */
+		TOP_MUX(0x4, "TSI0"),		/* sync */
+		TOP_MUX(0x5, "I2S1")),		/* din3 */
+	TOP_PIN(NAND_ADDR_CYCLE, TOP_REG7, 16, 3, 0xb8, 18,
+		TOP_MUX(0x0, "NAND"),		/* boot_addr_cycles */
+		TOP_MUX(0x1, "I2C4"),		/* sda */
+		TOP_MUX(0x2, "SPI2"),		/* rxd */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio91 */
+		TOP_MUX(0x4, "TSI0"),		/* valid */
+		TOP_MUX(0x5, "I2S1")),		/* dout1 */
+	TOP_PIN(NAND_RB0, TOP_REG7, 19, 3, 0xbc, 0,
+		TOP_MUX(0x0, "NAND"),		/* rdy_busy0 */
+		TOP_MUX(0x1, "I2C2"),		/* scl */
+		TOP_MUX(0x2, "USIM0"),		/* cd */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio92 */
+		TOP_MUX(0x4, "TSI1")),		/* data0 */
+	TOP_PIN(NAND_RB1, TOP_REG7, 22, 3, 0xbc, 9,
+		TOP_MUX(0x0, "NAND"),		/* rdy_busy1 */
+		TOP_MUX(0x1, "I2C2"),		/* sda */
+		TOP_MUX(0x2, "USIM0"),		/* clk */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio93 */
+		TOP_MUX(0x4, "TSI1")),		/* clk */
+	TOP_PIN(NAND_RB2, TOP_REG7, 25, 3, 0xbc, 18,
+		TOP_MUX(0x0, "NAND"),		/* rdy_busy2 */
+		TOP_MUX(0x1, "UART5"),		/* rxd */
+		TOP_MUX(0x2, "USIM0"),		/* rst */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio94 */
+		TOP_MUX(0x4, "TSI1"),		/* sync */
+		TOP_MUX(0x4, "I2S1")),		/* dout2 */
+	TOP_PIN(NAND_RB3, TOP_REG7, 28, 3, 0x54, 18,
+		TOP_MUX(0x0, "NAND"),		/* rdy_busy3 */
+		TOP_MUX(0x1, "UART5"),		/* txd */
+		TOP_MUX(0x2, "USIM0"),		/* dat */
+		TOP_MUX(0x3, "BGPIO"),		/* gpio95 */
+		TOP_MUX(0x4, "TSI1"),		/* valid */
+		TOP_MUX(0x4, "I2S1")),		/* dout3 */
+
+	/* top_pmm_reg_8 */
+	TOP_PIN(GMAC_125M_IN, TOP_REG8, 0, 2, 0x34, 0,
+		TOP_MUX(0x0, "GMII"),		/* 125m_in */
+		TOP_MUX(0x1, "USB2"),		/* 0_drvvbus */
+		TOP_MUX(0x2, "ISP"),		/* ref_clk */
+		TOP_MUX(0x3, "BGPIO")),		/* gpio96 */
+	TOP_PIN(GMAC_50M_OUT, TOP_REG8, 2, 2, 0x34, 9,
+		TOP_MUX(0x0, "GMII"),		/* 50m_out */
+		TOP_MUX(0x1, "USB2"),		/* 1_drvvbus */
+		TOP_MUX(0x2, "BGPIO"),		/* gpio97 */
+		TOP_MUX(0x3, "USB2")),		/* 0_drvvbus */
+	TOP_PIN(SPINOR_SSCLK_LOOPBACK, TOP_REG8, 6, 1, 0xc8, 9,
+		TOP_MUX(0x0, "SPINOR")),	/* sdio1_clk_i */
+	TOP_PIN(SPINOR_SDIO1CLK_LOOPBACK, TOP_REG8, 7, 1, 0xc8, 18,
+		TOP_MUX(0x0, "SPINOR")),	/* ssclk_i */
+};
+
+static struct zx_pinctrl_soc_info zx296718_pinctrl_info = {
+	.pins = zx296718_pins,
+	.npins = ARRAY_SIZE(zx296718_pins),
+};
+
+static int zx296718_pinctrl_probe(struct platform_device *pdev)
+{
+	return zx_pinctrl_init(pdev, &zx296718_pinctrl_info);
+}
+
+static const struct of_device_id zx296718_pinctrl_match[] = {
+	{ .compatible = "zte,zx296718-pmm", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, zx296718_pinctrl_match);
+
+static struct platform_driver zx296718_pinctrl_driver = {
+	.probe  = zx296718_pinctrl_probe,
+	.driver = {
+		.name = "zx296718-pinctrl",
+		.of_match_table = zx296718_pinctrl_match,
+	},
+};
+builtin_platform_driver(zx296718_pinctrl_driver);
+
+MODULE_DESCRIPTION("ZTE ZX296718 pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/platform/goldfish/goldfish_pipe.c b/drivers/platform/goldfish/goldfish_pipe.c
index 5f3672153b12..0578d34eec3f 100644
--- a/drivers/platform/goldfish/goldfish_pipe.c
+++ b/drivers/platform/goldfish/goldfish_pipe.c
@@ -266,7 +266,7 @@ struct goldfish_pipe_dev {
 	unsigned char __iomem *base;
 };
 
-struct goldfish_pipe_dev pipe_dev[1] = {};
+static struct goldfish_pipe_dev pipe_dev[1] = {};
 
 static int goldfish_cmd_locked(struct goldfish_pipe *pipe, enum PipeCmdCode cmd)
 {
diff --git a/drivers/platform/x86/Kconfig b/drivers/platform/x86/Kconfig
index 8489020ecf44..a3ccc3c795a5 100644
--- a/drivers/platform/x86/Kconfig
+++ b/drivers/platform/x86/Kconfig
@@ -794,6 +794,25 @@ config INTEL_CHT_INT33FE
 	  This driver instantiates i2c-clients for these, so that standard
 	  i2c drivers for these chips can bind to the them.
 
+config INTEL_INT0002_VGPIO
+	tristate "Intel ACPI INT0002 Virtual GPIO driver"
+	depends on GPIOLIB && ACPI
+	select GPIOLIB_IRQCHIP
+	---help---
+	  Some peripherals on Bay Trail and Cherry Trail platforms signal a
+	  Power Management Event (PME) to the Power Management Controller (PMC)
+	  to wakeup the system. When this happens software needs to explicitly
+	  clear the PME bus 0 status bit in the GPE0a_STS register to avoid an
+	  IRQ storm on IRQ 9.
+
+	  This is modelled in ACPI through the INT0002 ACPI device, which is
+	  called a "Virtual GPIO controller" in ACPI because it defines the
+	  event handler to call when the PME triggers through _AEI and _L02
+	  methods as would be done for a real GPIO interrupt in ACPI.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called intel_int0002_vgpio.
+
 config INTEL_HID_EVENT
 	tristate "INTEL HID Event"
 	depends on ACPI
diff --git a/drivers/platform/x86/Makefile b/drivers/platform/x86/Makefile
index 182a3ed6605a..ab22ce77fb66 100644
--- a/drivers/platform/x86/Makefile
+++ b/drivers/platform/x86/Makefile
@@ -46,6 +46,7 @@ obj-$(CONFIG_TOSHIBA_BT_RFKILL)	+= toshiba_bluetooth.o
 obj-$(CONFIG_TOSHIBA_HAPS)	+= toshiba_haps.o
 obj-$(CONFIG_TOSHIBA_WMI)	+= toshiba-wmi.o
 obj-$(CONFIG_INTEL_CHT_INT33FE)	+= intel_cht_int33fe.o
+obj-$(CONFIG_INTEL_INT0002_VGPIO) += intel_int0002_vgpio.o
 obj-$(CONFIG_INTEL_HID_EVENT)	+= intel-hid.o
 obj-$(CONFIG_INTEL_VBTN)	+= intel-vbtn.o
 obj-$(CONFIG_INTEL_SCU_IPC)	+= intel_scu_ipc.o
diff --git a/drivers/platform/x86/intel-hid.c b/drivers/platform/x86/intel-hid.c
index 63ba2cbd04c2..8519e0f97bdd 100644
--- a/drivers/platform/x86/intel-hid.c
+++ b/drivers/platform/x86/intel-hid.c
@@ -23,6 +23,7 @@
 #include <linux/platform_device.h>
 #include <linux/input/sparse-keymap.h>
 #include <linux/acpi.h>
+#include <linux/suspend.h>
 #include <acpi/acpi_bus.h>
 
 MODULE_LICENSE("GPL");
@@ -75,6 +76,7 @@ static const struct key_entry intel_array_keymap[] = {
 struct intel_hid_priv {
 	struct input_dev *input_dev;
 	struct input_dev *array;
+	bool wakeup_mode;
 };
 
 static int intel_hid_set_enable(struct device *device, bool enable)
@@ -116,23 +118,37 @@ static void intel_button_array_enable(struct device *device, bool enable)
 		dev_warn(device, "failed to set button capability\n");
 }
 
-static int intel_hid_pl_suspend_handler(struct device *device)
+static int intel_hid_pm_prepare(struct device *device)
 {
-	intel_hid_set_enable(device, false);
-	intel_button_array_enable(device, false);
+	struct intel_hid_priv *priv = dev_get_drvdata(device);
+
+	priv->wakeup_mode = true;
+	return 0;
+}
 
+static int intel_hid_pl_suspend_handler(struct device *device)
+{
+	if (pm_suspend_via_firmware()) {
+		intel_hid_set_enable(device, false);
+		intel_button_array_enable(device, false);
+	}
 	return 0;
 }
 
 static int intel_hid_pl_resume_handler(struct device *device)
 {
-	intel_hid_set_enable(device, true);
-	intel_button_array_enable(device, true);
+	struct intel_hid_priv *priv = dev_get_drvdata(device);
 
+	priv->wakeup_mode = false;
+	if (pm_resume_via_firmware()) {
+		intel_hid_set_enable(device, true);
+		intel_button_array_enable(device, true);
+	}
 	return 0;
 }
 
 static const struct dev_pm_ops intel_hid_pl_pm_ops = {
+	.prepare = intel_hid_pm_prepare,
 	.freeze  = intel_hid_pl_suspend_handler,
 	.thaw  = intel_hid_pl_resume_handler,
 	.restore  = intel_hid_pl_resume_handler,
@@ -186,6 +202,19 @@ static void notify_handler(acpi_handle handle, u32 event, void *context)
 	unsigned long long ev_index;
 	acpi_status status;
 
+	if (priv->wakeup_mode) {
+		/* Wake up on 5-button array events only. */
+		if (event == 0xc0 || !priv->array)
+			return;
+
+		if (sparse_keymap_entry_from_scancode(priv->array, event))
+			pm_wakeup_hard_event(&device->dev);
+		else
+			dev_info(&device->dev, "unknown event 0x%x\n", event);
+
+		return;
+	}
+
 	/* 0xC0 is for HID events, other values are for 5 button array */
 	if (event != 0xc0) {
 		if (!priv->array ||
@@ -270,6 +299,7 @@ static int intel_hid_probe(struct platform_device *device)
 				 "failed to enable HID power button\n");
 	}
 
+	device_init_wakeup(&device->dev, true);
 	return 0;
 
 err_remove_notify:
diff --git a/drivers/platform/x86/intel-vbtn.c b/drivers/platform/x86/intel-vbtn.c
index c2035e121ac2..61f106377661 100644
--- a/drivers/platform/x86/intel-vbtn.c
+++ b/drivers/platform/x86/intel-vbtn.c
@@ -23,6 +23,7 @@
 #include <linux/platform_device.h>
 #include <linux/input/sparse-keymap.h>
 #include <linux/acpi.h>
+#include <linux/suspend.h>
 #include <acpi/acpi_bus.h>
 
 MODULE_LICENSE("GPL");
@@ -46,6 +47,7 @@ static const struct key_entry intel_vbtn_keymap[] = {
 
 struct intel_vbtn_priv {
 	struct input_dev *input_dev;
+	bool wakeup_mode;
 };
 
 static int intel_vbtn_input_setup(struct platform_device *device)
@@ -73,9 +75,15 @@ static void notify_handler(acpi_handle handle, u32 event, void *context)
 	struct platform_device *device = context;
 	struct intel_vbtn_priv *priv = dev_get_drvdata(&device->dev);
 
-	if (!sparse_keymap_report_event(priv->input_dev, event, 1, true))
-		dev_info(&device->dev, "unknown event index 0x%x\n",
-			 event);
+	if (priv->wakeup_mode) {
+		if (sparse_keymap_entry_from_scancode(priv->input_dev, event)) {
+			pm_wakeup_hard_event(&device->dev);
+			return;
+		}
+	} else if (sparse_keymap_report_event(priv->input_dev, event, 1, true)) {
+		return;
+	}
+	dev_info(&device->dev, "unknown event index 0x%x\n", event);
 }
 
 static int intel_vbtn_probe(struct platform_device *device)
@@ -109,6 +117,7 @@ static int intel_vbtn_probe(struct platform_device *device)
 	if (ACPI_FAILURE(status))
 		return -EBUSY;
 
+	device_init_wakeup(&device->dev, true);
 	return 0;
 }
 
@@ -125,10 +134,34 @@ static int intel_vbtn_remove(struct platform_device *device)
 	return 0;
 }
 
+static int intel_vbtn_pm_prepare(struct device *dev)
+{
+	struct intel_vbtn_priv *priv = dev_get_drvdata(dev);
+
+	priv->wakeup_mode = true;
+	return 0;
+}
+
+static int intel_vbtn_pm_resume(struct device *dev)
+{
+	struct intel_vbtn_priv *priv = dev_get_drvdata(dev);
+
+	priv->wakeup_mode = false;
+	return 0;
+}
+
+static const struct dev_pm_ops intel_vbtn_pm_ops = {
+	.prepare = intel_vbtn_pm_prepare,
+	.resume = intel_vbtn_pm_resume,
+	.restore = intel_vbtn_pm_resume,
+	.thaw = intel_vbtn_pm_resume,
+};
+
 static struct platform_driver intel_vbtn_pl_driver = {
 	.driver = {
 		.name = "intel-vbtn",
 		.acpi_match_table = intel_vbtn_ids,
+		.pm = &intel_vbtn_pm_ops,
 	},
 	.probe = intel_vbtn_probe,
 	.remove = intel_vbtn_remove,
diff --git a/drivers/platform/x86/intel_int0002_vgpio.c b/drivers/platform/x86/intel_int0002_vgpio.c
new file mode 100644
index 000000000000..92dc230ef5b2
--- /dev/null
+++ b/drivers/platform/x86/intel_int0002_vgpio.c
@@ -0,0 +1,219 @@
+/*
+ * Intel INT0002 "Virtual GPIO" driver
+ *
+ * Copyright (C) 2017 Hans de Goede <hdegoede@redhat.com>
+ *
+ * Loosely based on android x86 kernel code which is:
+ *
+ * Copyright (c) 2014, Intel Corporation.
+ *
+ * Author: Dyut Kumar Sil <dyut.k.sil@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.
+ *
+ * Some peripherals on Bay Trail and Cherry Trail platforms signal a Power
+ * Management Event (PME) to the Power Management Controller (PMC) to wakeup
+ * the system. When this happens software needs to clear the PME bus 0 status
+ * bit in the GPE0a_STS register to avoid an IRQ storm on IRQ 9.
+ *
+ * This is modelled in ACPI through the INT0002 ACPI device, which is
+ * called a "Virtual GPIO controller" in ACPI because it defines the event
+ * handler to call when the PME triggers through _AEI and _L02 / _E02
+ * methods as would be done for a real GPIO interrupt in ACPI. Note this
+ * is a hack to define an AML event handler for the PME while using existing
+ * ACPI mechanisms, this is not a real GPIO at all.
+ *
+ * This driver will bind to the INT0002 device, and register as a GPIO
+ * controller, letting gpiolib-acpi.c call the _L02 handler as it would
+ * for a real GPIO controller.
+ */
+
+#include <linux/acpi.h>
+#include <linux/bitmap.h>
+#include <linux/gpio/driver.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/suspend.h>
+
+#include <asm/cpu_device_id.h>
+#include <asm/intel-family.h>
+
+#define DRV_NAME			"INT0002 Virtual GPIO"
+
+/* For some reason the virtual GPIO pin tied to the GPE is numbered pin 2 */
+#define GPE0A_PME_B0_VIRT_GPIO_PIN	2
+
+#define GPE0A_PME_B0_STS_BIT		BIT(13)
+#define GPE0A_PME_B0_EN_BIT		BIT(13)
+#define GPE0A_STS_PORT			0x420
+#define GPE0A_EN_PORT			0x428
+
+#define ICPU(model)	{ X86_VENDOR_INTEL, 6, model, X86_FEATURE_ANY, }
+
+static const struct x86_cpu_id int0002_cpu_ids[] = {
+/*
+ * Limit ourselves to Cherry Trail for now, until testing shows we
+ * need to handle the INT0002 device on Baytrail too.
+ *	ICPU(INTEL_FAM6_ATOM_SILVERMONT1),	 * Valleyview, Bay Trail *
+ */
+	ICPU(INTEL_FAM6_ATOM_AIRMONT),		/* Braswell, Cherry Trail */
+	{}
+};
+
+/*
+ * As this is not a real GPIO at all, but just a hack to model an event in
+ * ACPI the get / set functions are dummy functions.
+ */
+
+static int int0002_gpio_get(struct gpio_chip *chip, unsigned int offset)
+{
+	return 0;
+}
+
+static void int0002_gpio_set(struct gpio_chip *chip, unsigned int offset,
+			     int value)
+{
+}
+
+static int int0002_gpio_direction_output(struct gpio_chip *chip,
+					 unsigned int offset, int value)
+{
+	return 0;
+}
+
+static void int0002_irq_ack(struct irq_data *data)
+{
+	outl(GPE0A_PME_B0_STS_BIT, GPE0A_STS_PORT);
+}
+
+static void int0002_irq_unmask(struct irq_data *data)
+{
+	u32 gpe_en_reg;
+
+	gpe_en_reg = inl(GPE0A_EN_PORT);
+	gpe_en_reg |= GPE0A_PME_B0_EN_BIT;
+	outl(gpe_en_reg, GPE0A_EN_PORT);
+}
+
+static void int0002_irq_mask(struct irq_data *data)
+{
+	u32 gpe_en_reg;
+
+	gpe_en_reg = inl(GPE0A_EN_PORT);
+	gpe_en_reg &= ~GPE0A_PME_B0_EN_BIT;
+	outl(gpe_en_reg, GPE0A_EN_PORT);
+}
+
+static irqreturn_t int0002_irq(int irq, void *data)
+{
+	struct gpio_chip *chip = data;
+	u32 gpe_sts_reg;
+
+	gpe_sts_reg = inl(GPE0A_STS_PORT);
+	if (!(gpe_sts_reg & GPE0A_PME_B0_STS_BIT))
+		return IRQ_NONE;
+
+	generic_handle_irq(irq_find_mapping(chip->irqdomain,
+					    GPE0A_PME_B0_VIRT_GPIO_PIN));
+
+	pm_system_wakeup();
+
+	return IRQ_HANDLED;
+}
+
+static struct irq_chip int0002_irqchip = {
+	.name			= DRV_NAME,
+	.irq_ack		= int0002_irq_ack,
+	.irq_mask		= int0002_irq_mask,
+	.irq_unmask		= int0002_irq_unmask,
+};
+
+static int int0002_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	const struct x86_cpu_id *cpu_id;
+	struct gpio_chip *chip;
+	int irq, ret;
+
+	/* Menlow has a different INT0002 device? <sigh> */
+	cpu_id = x86_match_cpu(int0002_cpu_ids);
+	if (!cpu_id)
+		return -ENODEV;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(dev, "Error getting IRQ: %d\n", irq);
+		return irq;
+	}
+
+	chip = devm_kzalloc(dev, sizeof(*chip), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	chip->label = DRV_NAME;
+	chip->parent = dev;
+	chip->owner = THIS_MODULE;
+	chip->get = int0002_gpio_get;
+	chip->set = int0002_gpio_set;
+	chip->direction_input = int0002_gpio_get;
+	chip->direction_output = int0002_gpio_direction_output;
+	chip->base = -1;
+	chip->ngpio = GPE0A_PME_B0_VIRT_GPIO_PIN + 1;
+	chip->irq_need_valid_mask = true;
+
+	ret = devm_gpiochip_add_data(&pdev->dev, chip, NULL);
+	if (ret) {
+		dev_err(dev, "Error adding gpio chip: %d\n", ret);
+		return ret;
+	}
+
+	bitmap_clear(chip->irq_valid_mask, 0, GPE0A_PME_B0_VIRT_GPIO_PIN);
+
+	/*
+	 * We manually request the irq here instead of passing a flow-handler
+	 * to gpiochip_set_chained_irqchip, because the irq is shared.
+	 */
+	ret = devm_request_irq(dev, irq, int0002_irq,
+			       IRQF_SHARED | IRQF_NO_THREAD, "INT0002", chip);
+	if (ret) {
+		dev_err(dev, "Error requesting IRQ %d: %d\n", irq, ret);
+		return ret;
+	}
+
+	ret = gpiochip_irqchip_add(chip, &int0002_irqchip, 0, handle_edge_irq,
+				   IRQ_TYPE_NONE);
+	if (ret) {
+		dev_err(dev, "Error adding irqchip: %d\n", ret);
+		return ret;
+	}
+
+	gpiochip_set_chained_irqchip(chip, &int0002_irqchip, irq, NULL);
+
+	return 0;
+}
+
+static const struct acpi_device_id int0002_acpi_ids[] = {
+	{ "INT0002", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, int0002_acpi_ids);
+
+static struct platform_driver int0002_driver = {
+	.driver = {
+		.name			= DRV_NAME,
+		.acpi_match_table	= int0002_acpi_ids,
+	},
+	.probe	= int0002_probe,
+};
+
+module_platform_driver(int0002_driver);
+
+MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
+MODULE_DESCRIPTION("Intel INT0002 Virtual GPIO driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/platform/x86/intel_telemetry_debugfs.c b/drivers/platform/x86/intel_telemetry_debugfs.c
index ef29f18b1951..4cc2f4ea0a25 100644
--- a/drivers/platform/x86/intel_telemetry_debugfs.c
+++ b/drivers/platform/x86/intel_telemetry_debugfs.c
@@ -97,11 +97,9 @@
 	} \
 }
 
-#ifdef CONFIG_PM_SLEEP
 static u8 suspend_prep_ok;
 static u32 suspend_shlw_ctr_temp, suspend_deep_ctr_temp;
 static u64 suspend_shlw_res_temp, suspend_deep_res_temp;
-#endif
 
 struct telemetry_susp_stats {
 	u32 shlw_swake_ctr;
@@ -807,7 +805,6 @@ static const struct file_operations telem_ioss_trc_verb_ops = {
 	.release	= single_release,
 };
 
-#ifdef CONFIG_PM_SLEEP
 static int pm_suspend_prep_cb(void)
 {
 	struct telemetry_evtlog evtlog[TELEM_MAX_OS_ALLOCATED_EVENTS];
@@ -937,7 +934,6 @@ static int pm_notification(struct notifier_block *this,
 static struct notifier_block pm_notifier = {
 	.notifier_call = pm_notification,
 };
-#endif /* CONFIG_PM_SLEEP */
 
 static int __init telemetry_debugfs_init(void)
 {
@@ -960,14 +956,13 @@ static int __init telemetry_debugfs_init(void)
 	if (err < 0)
 		return -EINVAL;
 
-
-#ifdef CONFIG_PM_SLEEP
 	register_pm_notifier(&pm_notifier);
-#endif /* CONFIG_PM_SLEEP */
 
 	debugfs_conf->telemetry_dbg_dir = debugfs_create_dir("telemetry", NULL);
-	if (!debugfs_conf->telemetry_dbg_dir)
-		return -ENOMEM;
+	if (!debugfs_conf->telemetry_dbg_dir) {
+		err = -ENOMEM;
+		goto out_pm;
+	}
 
 	f = debugfs_create_file("pss_info", S_IFREG | S_IRUGO,
 				debugfs_conf->telemetry_dbg_dir, NULL,
@@ -1014,6 +1009,8 @@ static int __init telemetry_debugfs_init(void)
 out:
 	debugfs_remove_recursive(debugfs_conf->telemetry_dbg_dir);
 	debugfs_conf->telemetry_dbg_dir = NULL;
+out_pm:
+	unregister_pm_notifier(&pm_notifier);
 
 	return err;
 }
@@ -1022,6 +1019,7 @@ static void __exit telemetry_debugfs_exit(void)
 {
 	debugfs_remove_recursive(debugfs_conf->telemetry_dbg_dir);
 	debugfs_conf->telemetry_dbg_dir = NULL;
+	unregister_pm_notifier(&pm_notifier);
 }
 
 late_initcall(telemetry_debugfs_init);
diff --git a/drivers/platform/x86/thinkpad_acpi.c b/drivers/platform/x86/thinkpad_acpi.c
index 7b6cb0c69b02..f6861b551178 100644
--- a/drivers/platform/x86/thinkpad_acpi.c
+++ b/drivers/platform/x86/thinkpad_acpi.c
@@ -1438,25 +1438,20 @@ static int tpacpi_rfk_procfs_write(const enum tpacpi_rfk_id id, char *buf)
  */
 
 /* interface_version --------------------------------------------------- */
-static ssize_t tpacpi_driver_interface_version_show(
-				struct device_driver *drv,
-				char *buf)
+static ssize_t interface_version_show(struct device_driver *drv, char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "0x%08x\n", TPACPI_SYSFS_VERSION);
 }
-
-static DRIVER_ATTR(interface_version, S_IRUGO,
-		tpacpi_driver_interface_version_show, NULL);
+static DRIVER_ATTR_RO(interface_version);
 
 /* debug_level --------------------------------------------------------- */
-static ssize_t tpacpi_driver_debug_show(struct device_driver *drv,
-						char *buf)
+static ssize_t debug_level_show(struct device_driver *drv, char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "0x%04x\n", dbg_level);
 }
 
-static ssize_t tpacpi_driver_debug_store(struct device_driver *drv,
-						const char *buf, size_t count)
+static ssize_t debug_level_store(struct device_driver *drv, const char *buf,
+				 size_t count)
 {
 	unsigned long t;
 
@@ -1467,34 +1462,28 @@ static ssize_t tpacpi_driver_debug_store(struct device_driver *drv,
 
 	return count;
 }
-
-static DRIVER_ATTR(debug_level, S_IWUSR | S_IRUGO,
-		tpacpi_driver_debug_show, tpacpi_driver_debug_store);
+static DRIVER_ATTR_RW(debug_level);
 
 /* version ------------------------------------------------------------- */
-static ssize_t tpacpi_driver_version_show(struct device_driver *drv,
-						char *buf)
+static ssize_t version_show(struct device_driver *drv, char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "%s v%s\n",
 			TPACPI_DESC, TPACPI_VERSION);
 }
-
-static DRIVER_ATTR(version, S_IRUGO,
-		tpacpi_driver_version_show, NULL);
+static DRIVER_ATTR_RO(version);
 
 /* --------------------------------------------------------------------- */
 
 #ifdef CONFIG_THINKPAD_ACPI_DEBUGFACILITIES
 
 /* wlsw_emulstate ------------------------------------------------------ */
-static ssize_t tpacpi_driver_wlsw_emulstate_show(struct device_driver *drv,
-						char *buf)
+static ssize_t wlsw_emulstate_show(struct device_driver *drv, char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "%d\n", !!tpacpi_wlsw_emulstate);
 }
 
-static ssize_t tpacpi_driver_wlsw_emulstate_store(struct device_driver *drv,
-						const char *buf, size_t count)
+static ssize_t wlsw_emulstate_store(struct device_driver *drv, const char *buf,
+				    size_t count)
 {
 	unsigned long t;
 
@@ -1508,22 +1497,16 @@ static ssize_t tpacpi_driver_wlsw_emulstate_store(struct device_driver *drv,
 
 	return count;
 }
-
-static DRIVER_ATTR(wlsw_emulstate, S_IWUSR | S_IRUGO,
-		tpacpi_driver_wlsw_emulstate_show,
-		tpacpi_driver_wlsw_emulstate_store);
+static DRIVER_ATTR_RW(wlsw_emulstate);
 
 /* bluetooth_emulstate ------------------------------------------------- */
-static ssize_t tpacpi_driver_bluetooth_emulstate_show(
-					struct device_driver *drv,
-					char *buf)
+static ssize_t bluetooth_emulstate_show(struct device_driver *drv, char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "%d\n", !!tpacpi_bluetooth_emulstate);
 }
 
-static ssize_t tpacpi_driver_bluetooth_emulstate_store(
-					struct device_driver *drv,
-					const char *buf, size_t count)
+static ssize_t bluetooth_emulstate_store(struct device_driver *drv,
+					 const char *buf, size_t count)
 {
 	unsigned long t;
 
@@ -1534,22 +1517,16 @@ static ssize_t tpacpi_driver_bluetooth_emulstate_store(
 
 	return count;
 }
-
-static DRIVER_ATTR(bluetooth_emulstate, S_IWUSR | S_IRUGO,
-		tpacpi_driver_bluetooth_emulstate_show,
-		tpacpi_driver_bluetooth_emulstate_store);
+static DRIVER_ATTR_RW(bluetooth_emulstate);
 
 /* wwan_emulstate ------------------------------------------------- */
-static ssize_t tpacpi_driver_wwan_emulstate_show(
-					struct device_driver *drv,
-					char *buf)
+static ssize_t wwan_emulstate_show(struct device_driver *drv, char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "%d\n", !!tpacpi_wwan_emulstate);
 }
 
-static ssize_t tpacpi_driver_wwan_emulstate_store(
-					struct device_driver *drv,
-					const char *buf, size_t count)
+static ssize_t wwan_emulstate_store(struct device_driver *drv, const char *buf,
+				    size_t count)
 {
 	unsigned long t;
 
@@ -1560,22 +1537,16 @@ static ssize_t tpacpi_driver_wwan_emulstate_store(
 
 	return count;
 }
-
-static DRIVER_ATTR(wwan_emulstate, S_IWUSR | S_IRUGO,
-		tpacpi_driver_wwan_emulstate_show,
-		tpacpi_driver_wwan_emulstate_store);
+static DRIVER_ATTR_RW(wwan_emulstate);
 
 /* uwb_emulstate ------------------------------------------------- */
-static ssize_t tpacpi_driver_uwb_emulstate_show(
-					struct device_driver *drv,
-					char *buf)
+static ssize_t uwb_emulstate_show(struct device_driver *drv, char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "%d\n", !!tpacpi_uwb_emulstate);
 }
 
-static ssize_t tpacpi_driver_uwb_emulstate_store(
-					struct device_driver *drv,
-					const char *buf, size_t count)
+static ssize_t uwb_emulstate_store(struct device_driver *drv, const char *buf,
+				   size_t count)
 {
 	unsigned long t;
 
@@ -1586,10 +1557,7 @@ static ssize_t tpacpi_driver_uwb_emulstate_store(
 
 	return count;
 }
-
-static DRIVER_ATTR(uwb_emulstate, S_IWUSR | S_IRUGO,
-		tpacpi_driver_uwb_emulstate_show,
-		tpacpi_driver_uwb_emulstate_store);
+static DRIVER_ATTR_RW(uwb_emulstate);
 #endif
 
 /* --------------------------------------------------------------------- */
@@ -8606,14 +8574,13 @@ static ssize_t fan_fan2_input_show(struct device *dev,
 static DEVICE_ATTR(fan2_input, S_IRUGO, fan_fan2_input_show, NULL);
 
 /* sysfs fan fan_watchdog (hwmon driver) ------------------------------- */
-static ssize_t fan_fan_watchdog_show(struct device_driver *drv,
-				     char *buf)
+static ssize_t fan_watchdog_show(struct device_driver *drv, char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "%u\n", fan_watchdog_maxinterval);
 }
 
-static ssize_t fan_fan_watchdog_store(struct device_driver *drv,
-				      const char *buf, size_t count)
+static ssize_t fan_watchdog_store(struct device_driver *drv, const char *buf,
+				  size_t count)
 {
 	unsigned long t;
 
@@ -8630,9 +8597,7 @@ static ssize_t fan_fan_watchdog_store(struct device_driver *drv,
 
 	return count;
 }
-
-static DRIVER_ATTR(fan_watchdog, S_IWUSR | S_IRUGO,
-		fan_fan_watchdog_show, fan_fan_watchdog_store);
+static DRIVER_ATTR_RW(fan_watchdog);
 
 /* --------------------------------------------------------------------- */
 static struct attribute *fan_attributes[] = {
diff --git a/drivers/pnp/pnpacpi/core.c b/drivers/pnp/pnpacpi/core.c
index 9113876487ed..3a4c1aa0201e 100644
--- a/drivers/pnp/pnpacpi/core.c
+++ b/drivers/pnp/pnpacpi/core.c
@@ -149,8 +149,8 @@ static int pnpacpi_suspend(struct pnp_dev *dev, pm_message_t state)
 	}
 
 	if (device_can_wakeup(&dev->dev)) {
-		error = acpi_pm_device_sleep_wake(&dev->dev,
-				device_may_wakeup(&dev->dev));
+		error = acpi_pm_set_device_wakeup(&dev->dev,
+					      device_may_wakeup(&dev->dev));
 		if (error)
 			return error;
 	}
@@ -185,7 +185,7 @@ static int pnpacpi_resume(struct pnp_dev *dev)
 	}
 
 	if (device_may_wakeup(&dev->dev))
-		acpi_pm_device_sleep_wake(&dev->dev, false);
+		acpi_pm_set_device_wakeup(&dev->dev, false);
 
 	if (acpi_device_power_manageable(acpi_dev))
 		error = acpi_device_set_power(acpi_dev, ACPI_STATE_D0);
diff --git a/drivers/power/avs/rockchip-io-domain.c b/drivers/power/avs/rockchip-io-domain.c
index 85812521b6ba..031a34372191 100644
--- a/drivers/power/avs/rockchip-io-domain.c
+++ b/drivers/power/avs/rockchip-io-domain.c
@@ -253,6 +253,16 @@ static const struct rockchip_iodomain_soc_data soc_data_rk3188 = {
 	},
 };
 
+static const struct rockchip_iodomain_soc_data soc_data_rk3228 = {
+	.grf_offset = 0x418,
+	.supply_names = {
+		"vccio1",
+		"vccio2",
+		"vccio3",
+		"vccio4",
+	},
+};
+
 static const struct rockchip_iodomain_soc_data soc_data_rk3288 = {
 	.grf_offset = 0x380,
 	.supply_names = {
@@ -345,6 +355,10 @@ static const struct of_device_id rockchip_iodomain_match[] = {
 		.data = (void *)&soc_data_rk3188
 	},
 	{
+		.compatible = "rockchip,rk3228-io-voltage-domain",
+		.data = (void *)&soc_data_rk3228
+	},
+	{
 		.compatible = "rockchip,rk3288-io-voltage-domain",
 		.data = (void *)&soc_data_rk3288
 	},
diff --git a/drivers/power/reset/Kconfig b/drivers/power/reset/Kconfig
index 13f1714cf6f7..ca0de1a78e85 100644
--- a/drivers/power/reset/Kconfig
+++ b/drivers/power/reset/Kconfig
@@ -58,9 +58,9 @@ config POWER_RESET_BRCMKONA
 
 config POWER_RESET_BRCMSTB
 	bool "Broadcom STB reset driver"
-	depends on ARM || MIPS || COMPILE_TEST
+	depends on ARM || ARM64 || MIPS || COMPILE_TEST
 	depends on MFD_SYSCON
-	default ARCH_BRCMSTB
+	default ARCH_BRCMSTB || BMIPS_GENERIC
 	help
 	  This driver provides restart support for Broadcom STB boards.
 
diff --git a/drivers/power/reset/at91-poweroff.c b/drivers/power/reset/at91-poweroff.c
index c6c3beea72f9..c30c40193aaa 100644
--- a/drivers/power/reset/at91-poweroff.c
+++ b/drivers/power/reset/at91-poweroff.c
@@ -97,7 +97,7 @@ static void at91_lpddr_poweroff(void)
 		  "r" cpu_to_le32(AT91_DDRSDRC_LPDDR2_PWOFF),
 		  "r" (at91_shdwc_base),
 		  "r" cpu_to_le32(AT91_SHDW_KEY | AT91_SHDW_SHDW)
-		: "r0");
+		: "r6");
 }
 
 static int at91_poweroff_get_wakeup_mode(struct device_node *np)
diff --git a/drivers/power/reset/at91-sama5d2_shdwc.c b/drivers/power/reset/at91-sama5d2_shdwc.c
index 90b0b5a70ce5..55fce8b75245 100644
--- a/drivers/power/reset/at91-sama5d2_shdwc.c
+++ b/drivers/power/reset/at91-sama5d2_shdwc.c
@@ -132,7 +132,7 @@ static void at91_lpddr_poweroff(void)
 		  "r" cpu_to_le32(AT91_DDRSDRC_LPDDR2_PWOFF),
 		  "r" (at91_shdwc->at91_shdwc_base),
 		  "r" cpu_to_le32(AT91_SHDW_KEY | AT91_SHDW_SHDW)
-		: "r0");
+		: "r6");
 }
 
 static u32 at91_shdwc_debouncer_value(struct platform_device *pdev,
diff --git a/drivers/power/reset/reboot-mode.c b/drivers/power/reset/reboot-mode.c
index fb512183ace3..8f975ca0a8c4 100644
--- a/drivers/power/reset/reboot-mode.c
+++ b/drivers/power/reset/reboot-mode.c
@@ -13,7 +13,7 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/reboot.h>
-#include "reboot-mode.h"
+#include <linux/reboot-mode.h>
 
 #define PREFIX "mode-"
 
diff --git a/drivers/power/reset/reboot-mode.h b/drivers/power/reset/reboot-mode.h
deleted file mode 100644
index 75f7fe5c881f..000000000000
--- a/drivers/power/reset/reboot-mode.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#ifndef __REBOOT_MODE_H__
-#define __REBOOT_MODE_H__
-
-struct reboot_mode_driver {
-	struct device *dev;
-	struct list_head head;
-	int (*write)(struct reboot_mode_driver *reboot, unsigned int magic);
-	struct notifier_block reboot_notifier;
-};
-
-int reboot_mode_register(struct reboot_mode_driver *reboot);
-int reboot_mode_unregister(struct reboot_mode_driver *reboot);
-int devm_reboot_mode_register(struct device *dev,
-			      struct reboot_mode_driver *reboot);
-void devm_reboot_mode_unregister(struct device *dev,
-				 struct reboot_mode_driver *reboot);
-
-#endif
diff --git a/drivers/power/reset/syscon-reboot-mode.c b/drivers/power/reset/syscon-reboot-mode.c
index c8c371b285b1..563a97d7f73e 100644
--- a/drivers/power/reset/syscon-reboot-mode.c
+++ b/drivers/power/reset/syscon-reboot-mode.c
@@ -15,7 +15,7 @@
 #include <linux/reboot.h>
 #include <linux/regmap.h>
 #include <linux/mfd/syscon.h>
-#include "reboot-mode.h"
+#include <linux/reboot-mode.h>
 
 struct syscon_reboot_mode {
 	struct regmap *map;
diff --git a/drivers/power/supply/Kconfig b/drivers/power/supply/Kconfig
index 86f40bf37c34..969f5005669c 100644
--- a/drivers/power/supply/Kconfig
+++ b/drivers/power/supply/Kconfig
@@ -82,6 +82,14 @@ config BATTERY_ACT8945A
 	  Say Y here to enable support for power supply provided by
 	  Active-semi ActivePath ACT8945A charger.
 
+config BATTERY_CPCAP
+	tristate "Motorola CPCAP PMIC battery driver"
+	depends on MFD_CPCAP && IIO
+	default MFD_CPCAP
+	help
+	  Say Y here to enable support for battery on Motorola
+	  phones and tablets such as droid 4.
+
 config BATTERY_DS2760
 	tristate "DS2760 battery driver (HP iPAQ & others)"
 	depends on W1 && W1_SLAVE_DS2760
@@ -190,6 +198,17 @@ config BATTERY_BQ27XXX_I2C
 	  Say Y here to enable support for batteries with BQ27xxx chips
 	  connected over an I2C bus.
 
+config BATTERY_BQ27XXX_DT_UPDATES_NVM
+	bool "BQ27xxx support for update of NVM/flash data memory"
+	depends on BATTERY_BQ27XXX_I2C
+	help
+	  Say Y here to enable devicetree monitored-battery config to update
+	  NVM/flash data memory. Only enable this option for devices with a
+	  fuel gauge mounted on the circuit board, and a battery that cannot
+	  easily be replaced with one of a different type. Not for
+	  general-purpose kernels, as this can cause misconfiguration of a
+	  smart battery with embedded NVM/flash.
+
 config BATTERY_DA9030
 	tristate "DA9030 battery driver"
 	depends on PMIC_DA903X
@@ -408,6 +427,13 @@ config CHARGER_MANAGER
           runtime and in suspend-to-RAM by waking up the system periodically
           with help of suspend_again support.
 
+config CHARGER_LTC3651
+	tristate "LTC3651 charger"
+	depends on GPIOLIB
+	help
+	  Say Y to include support for the LTC3651 battery charger which reports
+	  its status via GPIO lines.
+
 config CHARGER_MAX14577
 	tristate "Maxim MAX14577/77836 battery charger driver"
 	depends on MFD_MAX14577
diff --git a/drivers/power/supply/Makefile b/drivers/power/supply/Makefile
index a39126d7a6ce..a41f40957847 100644
--- a/drivers/power/supply/Makefile
+++ b/drivers/power/supply/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_BATTERY_88PM860X)	+= 88pm860x_battery.o
 obj-$(CONFIG_BATTERY_ACT8945A)	+= act8945a_charger.o
 obj-$(CONFIG_BATTERY_AXP20X)	+= axp20x_battery.o
 obj-$(CONFIG_CHARGER_AXP20X)	+= axp20x_ac_power.o
+obj-$(CONFIG_BATTERY_CPCAP)	+= cpcap-battery.o
 obj-$(CONFIG_BATTERY_DS2760)	+= ds2760_battery.o
 obj-$(CONFIG_BATTERY_DS2780)	+= ds2780_battery.o
 obj-$(CONFIG_BATTERY_DS2781)	+= ds2781_battery.o
@@ -61,6 +62,7 @@ obj-$(CONFIG_CHARGER_LP8727)	+= lp8727_charger.o
 obj-$(CONFIG_CHARGER_LP8788)	+= lp8788-charger.o
 obj-$(CONFIG_CHARGER_GPIO)	+= gpio-charger.o
 obj-$(CONFIG_CHARGER_MANAGER)	+= charger-manager.o
+obj-$(CONFIG_CHARGER_LTC3651)	+= ltc3651-charger.o
 obj-$(CONFIG_CHARGER_MAX14577)	+= max14577_charger.o
 obj-$(CONFIG_CHARGER_DETECTOR_MAX14656)	+= max14656_charger_detector.o
 obj-$(CONFIG_CHARGER_MAX77693)	+= max77693_charger.o
diff --git a/drivers/power/supply/axp20x_battery.c b/drivers/power/supply/axp20x_battery.c
index 5d29b2eab8fc..7494f0f0eadb 100644
--- a/drivers/power/supply/axp20x_battery.c
+++ b/drivers/power/supply/axp20x_battery.c
@@ -60,6 +60,8 @@ struct axp20x_batt_ps {
 	struct iio_channel *batt_chrg_i;
 	struct iio_channel *batt_dischrg_i;
 	struct iio_channel *batt_v;
+	/* Maximum constant charge current */
+	unsigned int max_ccc;
 	u8 axp_id;
 };
 
@@ -129,6 +131,14 @@ static void raw_to_constant_charge_current(struct axp20x_batt_ps *axp, int *val)
 		*val = *val * 150000 + 300000;
 }
 
+static void constant_charge_current_to_raw(struct axp20x_batt_ps *axp, int *val)
+{
+	if (axp->axp_id == AXP209_ID)
+		*val = (*val - 300000) / 100000;
+	else
+		*val = (*val - 300000) / 150000;
+}
+
 static int axp20x_get_constant_charge_current(struct axp20x_batt_ps *axp,
 					      int *val)
 {
@@ -221,9 +231,7 @@ static int axp20x_battery_get_prop(struct power_supply *psy,
 		break;
 
 	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX:
-		val->intval = AXP20X_CHRG_CTRL1_TGT_CURR;
-		raw_to_constant_charge_current(axp20x_batt, &val->intval);
-
+		val->intval = axp20x_batt->max_ccc;
 		break;
 
 	case POWER_SUPPLY_PROP_CURRENT_NOW:
@@ -340,10 +348,10 @@ static int axp20x_battery_set_max_voltage(struct axp20x_batt_ps *axp20x_batt,
 static int axp20x_set_constant_charge_current(struct axp20x_batt_ps *axp_batt,
 					      int charge_current)
 {
-	if (axp_batt->axp_id == AXP209_ID)
-		charge_current = (charge_current - 300000) / 100000;
-	else
-		charge_current = (charge_current - 300000) / 150000;
+	if (charge_current > axp_batt->max_ccc)
+		return -EINVAL;
+
+	constant_charge_current_to_raw(axp_batt, &charge_current);
 
 	if (charge_current > AXP20X_CHRG_CTRL1_TGT_CURR || charge_current < 0)
 		return -EINVAL;
@@ -352,6 +360,36 @@ static int axp20x_set_constant_charge_current(struct axp20x_batt_ps *axp_batt,
 				  AXP20X_CHRG_CTRL1_TGT_CURR, charge_current);
 }
 
+static int axp20x_set_max_constant_charge_current(struct axp20x_batt_ps *axp,
+						  int charge_current)
+{
+	bool lower_max = false;
+
+	constant_charge_current_to_raw(axp, &charge_current);
+
+	if (charge_current > AXP20X_CHRG_CTRL1_TGT_CURR || charge_current < 0)
+		return -EINVAL;
+
+	raw_to_constant_charge_current(axp, &charge_current);
+
+	if (charge_current > axp->max_ccc)
+		dev_warn(axp->dev,
+			 "Setting max constant charge current higher than previously defined. Note that increasing the constant charge current may damage your battery.\n");
+	else
+		lower_max = true;
+
+	axp->max_ccc = charge_current;
+
+	if (lower_max) {
+		int current_cc;
+
+		axp20x_get_constant_charge_current(axp, &current_cc);
+		if (current_cc > charge_current)
+			axp20x_set_constant_charge_current(axp, charge_current);
+	}
+
+	return 0;
+}
 static int axp20x_set_voltage_min_design(struct axp20x_batt_ps *axp_batt,
 					 int min_voltage)
 {
@@ -380,6 +418,9 @@ static int axp20x_battery_set_prop(struct power_supply *psy,
 	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT:
 		return axp20x_set_constant_charge_current(axp20x_batt,
 							  val->intval);
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX:
+		return axp20x_set_max_constant_charge_current(axp20x_batt,
+							      val->intval);
 
 	default:
 		return -EINVAL;
@@ -405,7 +446,8 @@ static int axp20x_battery_prop_writeable(struct power_supply *psy,
 {
 	return psp == POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN ||
 	       psp == POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN ||
-	       psp == POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT;
+	       psp == POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT ||
+	       psp == POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX;
 }
 
 static const struct power_supply_desc axp20x_batt_ps_desc = {
@@ -433,6 +475,7 @@ static int axp20x_power_probe(struct platform_device *pdev)
 {
 	struct axp20x_batt_ps *axp20x_batt;
 	struct power_supply_config psy_cfg = {};
+	struct power_supply_battery_info info;
 
 	if (!of_device_is_available(pdev->dev.of_node))
 		return -ENODEV;
@@ -484,6 +527,35 @@ static int axp20x_power_probe(struct platform_device *pdev)
 		return PTR_ERR(axp20x_batt->batt);
 	}
 
+	if (!power_supply_get_battery_info(axp20x_batt->batt, &info)) {
+		int vmin = info.voltage_min_design_uv;
+		int ccc = info.constant_charge_current_max_ua;
+
+		if (vmin > 0 && axp20x_set_voltage_min_design(axp20x_batt,
+							      vmin))
+			dev_err(&pdev->dev,
+				"couldn't set voltage_min_design\n");
+
+		/* Set max to unverified value to be able to set CCC */
+		axp20x_batt->max_ccc = ccc;
+
+		if (ccc <= 0 || axp20x_set_constant_charge_current(axp20x_batt,
+								   ccc)) {
+			dev_err(&pdev->dev,
+				"couldn't set constant charge current from DT: fallback to minimum value\n");
+			ccc = 300000;
+			axp20x_batt->max_ccc = ccc;
+			axp20x_set_constant_charge_current(axp20x_batt, ccc);
+		}
+	}
+
+	/*
+	 * Update max CCC to a valid value if battery info is present or set it
+	 * to current register value by default.
+	 */
+	axp20x_get_constant_charge_current(axp20x_batt,
+					   &axp20x_batt->max_ccc);
+
 	return 0;
 }
 
diff --git a/drivers/power/supply/axp20x_usb_power.c b/drivers/power/supply/axp20x_usb_power.c
index 2397c482656e..44f70dcea61e 100644
--- a/drivers/power/supply/axp20x_usb_power.c
+++ b/drivers/power/supply/axp20x_usb_power.c
@@ -339,7 +339,7 @@ static int axp20x_usb_power_probe(struct platform_device *pdev)
 		"VBUS_REMOVAL", "VBUS_VALID", "VBUS_NOT_VALID", NULL };
 	static const char * const axp22x_irq_names[] = {
 		"VBUS_PLUGIN", "VBUS_REMOVAL", NULL };
-	static const char * const *irq_names;
+	const char * const *irq_names;
 	const struct power_supply_desc *usb_power_desc;
 	int i, irq, ret;
 
diff --git a/drivers/power/supply/bq24735-charger.c b/drivers/power/supply/bq24735-charger.c
index eb0145380def..6931e1d826f5 100644
--- a/drivers/power/supply/bq24735-charger.c
+++ b/drivers/power/supply/bq24735-charger.c
@@ -81,14 +81,12 @@ static int bq24735_charger_property_is_writeable(struct power_supply *psy,
 static inline int bq24735_write_word(struct i2c_client *client, u8 reg,
 				     u16 value)
 {
-	return i2c_smbus_write_word_data(client, reg, le16_to_cpu(value));
+	return i2c_smbus_write_word_data(client, reg, value);
 }
 
 static inline int bq24735_read_word(struct i2c_client *client, u8 reg)
 {
-	s32 ret = i2c_smbus_read_word_data(client, reg);
-
-	return ret < 0 ? ret : le16_to_cpu(ret);
+	return i2c_smbus_read_word_data(client, reg);
 }
 
 static int bq24735_update_word(struct i2c_client *client, u8 reg,
diff --git a/drivers/power/supply/bq27xxx_battery.c b/drivers/power/supply/bq27xxx_battery.c
index 398801a21b86..ed44439d0112 100644
--- a/drivers/power/supply/bq27xxx_battery.c
+++ b/drivers/power/supply/bq27xxx_battery.c
@@ -5,6 +5,7 @@
  * Copyright (C) 2008 Eurotech S.p.A. <info@eurotech.it>
  * Copyright (C) 2010-2011 Lars-Peter Clausen <lars@metafoo.de>
  * Copyright (C) 2011 Pali Rohár <pali.rohar@gmail.com>
+ * Copyright (C) 2017 Liam Breck <kernel@networkimprov.net>
  *
  * Based on a previous work by Copyright (C) 2008 Texas Instruments, Inc.
  *
@@ -65,6 +66,7 @@
 #define BQ27XXX_FLAG_DSC	BIT(0)
 #define BQ27XXX_FLAG_SOCF	BIT(1) /* State-of-Charge threshold final */
 #define BQ27XXX_FLAG_SOC1	BIT(2) /* State-of-Charge threshold 1 */
+#define BQ27XXX_FLAG_CFGUP	BIT(4)
 #define BQ27XXX_FLAG_FC		BIT(9)
 #define BQ27XXX_FLAG_OTD	BIT(14)
 #define BQ27XXX_FLAG_OTC	BIT(15)
@@ -78,6 +80,12 @@
 #define BQ27000_FLAG_FC		BIT(5)
 #define BQ27000_FLAG_CHGS	BIT(7) /* Charge state flag */
 
+/* control register params */
+#define BQ27XXX_SEALED			0x20
+#define BQ27XXX_SET_CFGUPDATE		0x13
+#define BQ27XXX_SOFT_RESET		0x42
+#define BQ27XXX_RESET			0x41
+
 #define BQ27XXX_RS			(20) /* Resistor sense mOhm */
 #define BQ27XXX_POWER_CONSTANT		(29200) /* 29.2 µV^2 * 1000 */
 #define BQ27XXX_CURRENT_CONSTANT	(3570) /* 3.57 µV * 1000 */
@@ -108,9 +116,21 @@ enum bq27xxx_reg_index {
 	BQ27XXX_REG_SOC,	/* State-of-Charge */
 	BQ27XXX_REG_DCAP,	/* Design Capacity */
 	BQ27XXX_REG_AP,		/* Average Power */
+	BQ27XXX_DM_CTRL,	/* Block Data Control */
+	BQ27XXX_DM_CLASS,	/* Data Class */
+	BQ27XXX_DM_BLOCK,	/* Data Block */
+	BQ27XXX_DM_DATA,	/* Block Data */
+	BQ27XXX_DM_CKSUM,	/* Block Data Checksum */
 	BQ27XXX_REG_MAX,	/* sentinel */
 };
 
+#define BQ27XXX_DM_REG_ROWS \
+	[BQ27XXX_DM_CTRL] = 0x61,  \
+	[BQ27XXX_DM_CLASS] = 0x3e, \
+	[BQ27XXX_DM_BLOCK] = 0x3f, \
+	[BQ27XXX_DM_DATA] = 0x40,  \
+	[BQ27XXX_DM_CKSUM] = 0x60
+
 /* Register mappings */
 static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 	[BQ27000] = {
@@ -131,6 +151,11 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x0b,
 		[BQ27XXX_REG_DCAP] = 0x76,
 		[BQ27XXX_REG_AP] = 0x24,
+		[BQ27XXX_DM_CTRL] = INVALID_REG_ADDR,
+		[BQ27XXX_DM_CLASS] = INVALID_REG_ADDR,
+		[BQ27XXX_DM_BLOCK] = INVALID_REG_ADDR,
+		[BQ27XXX_DM_DATA] = INVALID_REG_ADDR,
+		[BQ27XXX_DM_CKSUM] = INVALID_REG_ADDR,
 	},
 	[BQ27010] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -150,6 +175,11 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x0b,
 		[BQ27XXX_REG_DCAP] = 0x76,
 		[BQ27XXX_REG_AP] = INVALID_REG_ADDR,
+		[BQ27XXX_DM_CTRL] = INVALID_REG_ADDR,
+		[BQ27XXX_DM_CLASS] = INVALID_REG_ADDR,
+		[BQ27XXX_DM_BLOCK] = INVALID_REG_ADDR,
+		[BQ27XXX_DM_DATA] = INVALID_REG_ADDR,
+		[BQ27XXX_DM_CKSUM] = INVALID_REG_ADDR,
 	},
 	[BQ2750X] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -169,6 +199,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x2c,
 		[BQ27XXX_REG_DCAP] = 0x3c,
 		[BQ27XXX_REG_AP] = INVALID_REG_ADDR,
+		BQ27XXX_DM_REG_ROWS,
 	},
 	[BQ2751X] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -188,6 +219,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x20,
 		[BQ27XXX_REG_DCAP] = 0x2e,
 		[BQ27XXX_REG_AP] = INVALID_REG_ADDR,
+		BQ27XXX_DM_REG_ROWS,
 	},
 	[BQ27500] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -207,6 +239,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x2c,
 		[BQ27XXX_REG_DCAP] = 0x3c,
 		[BQ27XXX_REG_AP] = 0x24,
+		BQ27XXX_DM_REG_ROWS,
 	},
 	[BQ27510G1] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -226,6 +259,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x2c,
 		[BQ27XXX_REG_DCAP] = 0x3c,
 		[BQ27XXX_REG_AP] = 0x24,
+		BQ27XXX_DM_REG_ROWS,
 	},
 	[BQ27510G2] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -245,6 +279,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x2c,
 		[BQ27XXX_REG_DCAP] = 0x3c,
 		[BQ27XXX_REG_AP] = 0x24,
+		BQ27XXX_DM_REG_ROWS,
 	},
 	[BQ27510G3] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -264,6 +299,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x20,
 		[BQ27XXX_REG_DCAP] = 0x2e,
 		[BQ27XXX_REG_AP] = INVALID_REG_ADDR,
+		BQ27XXX_DM_REG_ROWS,
 	},
 	[BQ27520G1] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -283,6 +319,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x2c,
 		[BQ27XXX_REG_DCAP] = 0x3c,
 		[BQ27XXX_REG_AP] = 0x24,
+		BQ27XXX_DM_REG_ROWS,
 	},
 	[BQ27520G2] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -302,6 +339,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x2c,
 		[BQ27XXX_REG_DCAP] = 0x3c,
 		[BQ27XXX_REG_AP] = 0x24,
+		BQ27XXX_DM_REG_ROWS,
 	},
 	[BQ27520G3] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -321,6 +359,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x2c,
 		[BQ27XXX_REG_DCAP] = 0x3c,
 		[BQ27XXX_REG_AP] = 0x24,
+		BQ27XXX_DM_REG_ROWS,
 	},
 	[BQ27520G4] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -340,6 +379,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x20,
 		[BQ27XXX_REG_DCAP] = INVALID_REG_ADDR,
 		[BQ27XXX_REG_AP] = INVALID_REG_ADDR,
+		BQ27XXX_DM_REG_ROWS,
 	},
 	[BQ27530] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -359,6 +399,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x2c,
 		[BQ27XXX_REG_DCAP] = INVALID_REG_ADDR,
 		[BQ27XXX_REG_AP] = 0x24,
+		BQ27XXX_DM_REG_ROWS,
 	},
 	[BQ27541] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -378,6 +419,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x2c,
 		[BQ27XXX_REG_DCAP] = 0x3c,
 		[BQ27XXX_REG_AP] = 0x24,
+		BQ27XXX_DM_REG_ROWS,
 	},
 	[BQ27545] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -397,6 +439,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x2c,
 		[BQ27XXX_REG_DCAP] = INVALID_REG_ADDR,
 		[BQ27XXX_REG_AP] = 0x24,
+		BQ27XXX_DM_REG_ROWS,
 	},
 	[BQ27421] = {
 		[BQ27XXX_REG_CTRL] = 0x00,
@@ -416,6 +459,7 @@ static u8 bq27xxx_regs[][BQ27XXX_REG_MAX] = {
 		[BQ27XXX_REG_SOC] = 0x1c,
 		[BQ27XXX_REG_DCAP] = 0x3c,
 		[BQ27XXX_REG_AP] = 0x18,
+		BQ27XXX_DM_REG_ROWS,
 	},
 };
 
@@ -757,6 +801,73 @@ static struct {
 static DEFINE_MUTEX(bq27xxx_list_lock);
 static LIST_HEAD(bq27xxx_battery_devices);
 
+#define BQ27XXX_MSLEEP(i) usleep_range((i)*1000, (i)*1000+500)
+
+#define BQ27XXX_DM_SZ	32
+
+struct bq27xxx_dm_reg {
+	u8 subclass_id;
+	u8 offset;
+	u8 bytes;
+	u16 min, max;
+};
+
+/**
+ * struct bq27xxx_dm_buf - chip data memory buffer
+ * @class: data memory subclass_id
+ * @block: data memory block number
+ * @data: data from/for the block
+ * @has_data: true if data has been filled by read
+ * @dirty: true if data has changed since last read/write
+ *
+ * Encapsulates info required to manage chip data memory blocks.
+ */
+struct bq27xxx_dm_buf {
+	u8 class;
+	u8 block;
+	u8 data[BQ27XXX_DM_SZ];
+	bool has_data, dirty;
+};
+
+#define BQ27XXX_DM_BUF(di, i) { \
+	.class = (di)->dm_regs[i].subclass_id, \
+	.block = (di)->dm_regs[i].offset / BQ27XXX_DM_SZ, \
+}
+
+static inline u16 *bq27xxx_dm_reg_ptr(struct bq27xxx_dm_buf *buf,
+				      struct bq27xxx_dm_reg *reg)
+{
+	if (buf->class == reg->subclass_id &&
+	    buf->block == reg->offset / BQ27XXX_DM_SZ)
+		return (u16 *) (buf->data + reg->offset % BQ27XXX_DM_SZ);
+
+	return NULL;
+}
+
+enum bq27xxx_dm_reg_id {
+	BQ27XXX_DM_DESIGN_CAPACITY = 0,
+	BQ27XXX_DM_DESIGN_ENERGY,
+	BQ27XXX_DM_TERMINATE_VOLTAGE,
+};
+
+static const char * const bq27xxx_dm_reg_name[] = {
+	[BQ27XXX_DM_DESIGN_CAPACITY] = "design-capacity",
+	[BQ27XXX_DM_DESIGN_ENERGY] = "design-energy",
+	[BQ27XXX_DM_TERMINATE_VOLTAGE] = "terminate-voltage",
+};
+
+
+static bool bq27xxx_dt_to_nvm = true;
+module_param_named(dt_monitored_battery_updates_nvm, bq27xxx_dt_to_nvm, bool, 0444);
+MODULE_PARM_DESC(dt_monitored_battery_updates_nvm,
+	"Devicetree monitored-battery config updates data memory on NVM/flash chips.\n"
+	"Users must set this =0 when installing a different type of battery!\n"
+	"Default is =1."
+#ifndef CONFIG_BATTERY_BQ27XXX_DT_UPDATES_NVM
+	"\nSetting this affects future kernel updates, not the current configuration."
+#endif
+);
+
 static int poll_interval_param_set(const char *val, const struct kernel_param *kp)
 {
 	struct bq27xxx_device_info *di;
@@ -794,11 +905,419 @@ MODULE_PARM_DESC(poll_interval,
 static inline int bq27xxx_read(struct bq27xxx_device_info *di, int reg_index,
 			       bool single)
 {
-	/* Reports EINVAL for invalid/missing registers */
+	int ret;
+
+	if (!di || di->regs[reg_index] == INVALID_REG_ADDR)
+		return -EINVAL;
+
+	ret = di->bus.read(di, di->regs[reg_index], single);
+	if (ret < 0)
+		dev_dbg(di->dev, "failed to read register 0x%02x (index %d)\n",
+			di->regs[reg_index], reg_index);
+
+	return ret;
+}
+
+static inline int bq27xxx_write(struct bq27xxx_device_info *di, int reg_index,
+				u16 value, bool single)
+{
+	int ret;
+
+	if (!di || di->regs[reg_index] == INVALID_REG_ADDR)
+		return -EINVAL;
+
+	if (!di->bus.write)
+		return -EPERM;
+
+	ret = di->bus.write(di, di->regs[reg_index], value, single);
+	if (ret < 0)
+		dev_dbg(di->dev, "failed to write register 0x%02x (index %d)\n",
+			di->regs[reg_index], reg_index);
+
+	return ret;
+}
+
+static inline int bq27xxx_read_block(struct bq27xxx_device_info *di, int reg_index,
+				     u8 *data, int len)
+{
+	int ret;
+
+	if (!di || di->regs[reg_index] == INVALID_REG_ADDR)
+		return -EINVAL;
+
+	if (!di->bus.read_bulk)
+		return -EPERM;
+
+	ret = di->bus.read_bulk(di, di->regs[reg_index], data, len);
+	if (ret < 0)
+		dev_dbg(di->dev, "failed to read_bulk register 0x%02x (index %d)\n",
+			di->regs[reg_index], reg_index);
+
+	return ret;
+}
+
+static inline int bq27xxx_write_block(struct bq27xxx_device_info *di, int reg_index,
+				      u8 *data, int len)
+{
+	int ret;
+
 	if (!di || di->regs[reg_index] == INVALID_REG_ADDR)
 		return -EINVAL;
 
-	return di->bus.read(di, di->regs[reg_index], single);
+	if (!di->bus.write_bulk)
+		return -EPERM;
+
+	ret = di->bus.write_bulk(di, di->regs[reg_index], data, len);
+	if (ret < 0)
+		dev_dbg(di->dev, "failed to write_bulk register 0x%02x (index %d)\n",
+			di->regs[reg_index], reg_index);
+
+	return ret;
+}
+
+static int bq27xxx_battery_seal(struct bq27xxx_device_info *di)
+{
+	int ret;
+
+	ret = bq27xxx_write(di, BQ27XXX_REG_CTRL, BQ27XXX_SEALED, false);
+	if (ret < 0) {
+		dev_err(di->dev, "bus error on seal: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int bq27xxx_battery_unseal(struct bq27xxx_device_info *di)
+{
+	int ret;
+
+	if (di->unseal_key == 0) {
+		dev_err(di->dev, "unseal failed due to missing key\n");
+		return -EINVAL;
+	}
+
+	ret = bq27xxx_write(di, BQ27XXX_REG_CTRL, (u16)(di->unseal_key >> 16), false);
+	if (ret < 0)
+		goto out;
+
+	ret = bq27xxx_write(di, BQ27XXX_REG_CTRL, (u16)di->unseal_key, false);
+	if (ret < 0)
+		goto out;
+
+	return 0;
+
+out:
+	dev_err(di->dev, "bus error on unseal: %d\n", ret);
+	return ret;
+}
+
+static u8 bq27xxx_battery_checksum_dm_block(struct bq27xxx_dm_buf *buf)
+{
+	u16 sum = 0;
+	int i;
+
+	for (i = 0; i < BQ27XXX_DM_SZ; i++)
+		sum += buf->data[i];
+	sum &= 0xff;
+
+	return 0xff - sum;
+}
+
+static int bq27xxx_battery_read_dm_block(struct bq27xxx_device_info *di,
+					 struct bq27xxx_dm_buf *buf)
+{
+	int ret;
+
+	buf->has_data = false;
+
+	ret = bq27xxx_write(di, BQ27XXX_DM_CLASS, buf->class, true);
+	if (ret < 0)
+		goto out;
+
+	ret = bq27xxx_write(di, BQ27XXX_DM_BLOCK, buf->block, true);
+	if (ret < 0)
+		goto out;
+
+	BQ27XXX_MSLEEP(1);
+
+	ret = bq27xxx_read_block(di, BQ27XXX_DM_DATA, buf->data, BQ27XXX_DM_SZ);
+	if (ret < 0)
+		goto out;
+
+	ret = bq27xxx_read(di, BQ27XXX_DM_CKSUM, true);
+	if (ret < 0)
+		goto out;
+
+	if ((u8)ret != bq27xxx_battery_checksum_dm_block(buf)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	buf->has_data = true;
+	buf->dirty = false;
+
+	return 0;
+
+out:
+	dev_err(di->dev, "bus error reading chip memory: %d\n", ret);
+	return ret;
+}
+
+static void bq27xxx_battery_update_dm_block(struct bq27xxx_device_info *di,
+					    struct bq27xxx_dm_buf *buf,
+					    enum bq27xxx_dm_reg_id reg_id,
+					    unsigned int val)
+{
+	struct bq27xxx_dm_reg *reg = &di->dm_regs[reg_id];
+	const char *str = bq27xxx_dm_reg_name[reg_id];
+	u16 *prev = bq27xxx_dm_reg_ptr(buf, reg);
+
+	if (prev == NULL) {
+		dev_warn(di->dev, "buffer does not match %s dm spec\n", str);
+		return;
+	}
+
+	if (reg->bytes != 2) {
+		dev_warn(di->dev, "%s dm spec has unsupported byte size\n", str);
+		return;
+	}
+
+	if (!buf->has_data)
+		return;
+
+	if (be16_to_cpup(prev) == val) {
+		dev_info(di->dev, "%s has %u\n", str, val);
+		return;
+	}
+
+#ifdef CONFIG_BATTERY_BQ27XXX_DT_UPDATES_NVM
+	if (!di->ram_chip && !bq27xxx_dt_to_nvm) {
+#else
+	if (!di->ram_chip) {
+#endif
+		/* devicetree and NVM differ; defer to NVM */
+		dev_warn(di->dev, "%s has %u; update to %u disallowed "
+#ifdef CONFIG_BATTERY_BQ27XXX_DT_UPDATES_NVM
+			 "by dt_monitored_battery_updates_nvm=0"
+#else
+			 "for flash/NVM data memory"
+#endif
+			 "\n", str, be16_to_cpup(prev), val);
+		return;
+	}
+
+	dev_info(di->dev, "update %s to %u\n", str, val);
+
+	*prev = cpu_to_be16(val);
+	buf->dirty = true;
+}
+
+static int bq27xxx_battery_cfgupdate_priv(struct bq27xxx_device_info *di, bool active)
+{
+	const int limit = 100;
+	u16 cmd = active ? BQ27XXX_SET_CFGUPDATE : BQ27XXX_SOFT_RESET;
+	int ret, try = limit;
+
+	ret = bq27xxx_write(di, BQ27XXX_REG_CTRL, cmd, false);
+	if (ret < 0)
+		return ret;
+
+	do {
+		BQ27XXX_MSLEEP(25);
+		ret = bq27xxx_read(di, BQ27XXX_REG_FLAGS, false);
+		if (ret < 0)
+			return ret;
+	} while (!!(ret & BQ27XXX_FLAG_CFGUP) != active && --try);
+
+	if (!try) {
+		dev_err(di->dev, "timed out waiting for cfgupdate flag %d\n", active);
+		return -EINVAL;
+	}
+
+	if (limit - try > 3)
+		dev_warn(di->dev, "cfgupdate %d, retries %d\n", active, limit - try);
+
+	return 0;
+}
+
+static inline int bq27xxx_battery_set_cfgupdate(struct bq27xxx_device_info *di)
+{
+	int ret = bq27xxx_battery_cfgupdate_priv(di, true);
+	if (ret < 0 && ret != -EINVAL)
+		dev_err(di->dev, "bus error on set_cfgupdate: %d\n", ret);
+
+	return ret;
+}
+
+static inline int bq27xxx_battery_soft_reset(struct bq27xxx_device_info *di)
+{
+	int ret = bq27xxx_battery_cfgupdate_priv(di, false);
+	if (ret < 0 && ret != -EINVAL)
+		dev_err(di->dev, "bus error on soft_reset: %d\n", ret);
+
+	return ret;
+}
+
+static int bq27xxx_battery_write_dm_block(struct bq27xxx_device_info *di,
+					  struct bq27xxx_dm_buf *buf)
+{
+	bool cfgup = di->chip == BQ27421; /* assume related chips need cfgupdate */
+	int ret;
+
+	if (!buf->dirty)
+		return 0;
+
+	if (cfgup) {
+		ret = bq27xxx_battery_set_cfgupdate(di);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = bq27xxx_write(di, BQ27XXX_DM_CTRL, 0, true);
+	if (ret < 0)
+		goto out;
+
+	ret = bq27xxx_write(di, BQ27XXX_DM_CLASS, buf->class, true);
+	if (ret < 0)
+		goto out;
+
+	ret = bq27xxx_write(di, BQ27XXX_DM_BLOCK, buf->block, true);
+	if (ret < 0)
+		goto out;
+
+	BQ27XXX_MSLEEP(1);
+
+	ret = bq27xxx_write_block(di, BQ27XXX_DM_DATA, buf->data, BQ27XXX_DM_SZ);
+	if (ret < 0)
+		goto out;
+
+	ret = bq27xxx_write(di, BQ27XXX_DM_CKSUM,
+			    bq27xxx_battery_checksum_dm_block(buf), true);
+	if (ret < 0)
+		goto out;
+
+	/* DO NOT read BQ27XXX_DM_CKSUM here to verify it! That may cause NVM
+	 * corruption on the '425 chip (and perhaps others), which can damage
+	 * the chip.
+	 */
+
+	if (cfgup) {
+		BQ27XXX_MSLEEP(1);
+		ret = bq27xxx_battery_soft_reset(di);
+		if (ret < 0)
+			return ret;
+	} else {
+		BQ27XXX_MSLEEP(100); /* flash DM updates in <100ms */
+	}
+
+	buf->dirty = false;
+
+	return 0;
+
+out:
+	if (cfgup)
+		bq27xxx_battery_soft_reset(di);
+
+	dev_err(di->dev, "bus error writing chip memory: %d\n", ret);
+	return ret;
+}
+
+static void bq27xxx_battery_set_config(struct bq27xxx_device_info *di,
+				       struct power_supply_battery_info *info)
+{
+	struct bq27xxx_dm_buf bd = BQ27XXX_DM_BUF(di, BQ27XXX_DM_DESIGN_CAPACITY);
+	struct bq27xxx_dm_buf bt = BQ27XXX_DM_BUF(di, BQ27XXX_DM_TERMINATE_VOLTAGE);
+	bool updated;
+
+	if (bq27xxx_battery_unseal(di) < 0)
+		return;
+
+	if (info->charge_full_design_uah != -EINVAL &&
+	    info->energy_full_design_uwh != -EINVAL) {
+		bq27xxx_battery_read_dm_block(di, &bd);
+		/* assume design energy & capacity are in same block */
+		bq27xxx_battery_update_dm_block(di, &bd,
+					BQ27XXX_DM_DESIGN_CAPACITY,
+					info->charge_full_design_uah / 1000);
+		bq27xxx_battery_update_dm_block(di, &bd,
+					BQ27XXX_DM_DESIGN_ENERGY,
+					info->energy_full_design_uwh / 1000);
+	}
+
+	if (info->voltage_min_design_uv != -EINVAL) {
+		bool same = bd.class == bt.class && bd.block == bt.block;
+		if (!same)
+			bq27xxx_battery_read_dm_block(di, &bt);
+		bq27xxx_battery_update_dm_block(di, same ? &bd : &bt,
+					BQ27XXX_DM_TERMINATE_VOLTAGE,
+					info->voltage_min_design_uv / 1000);
+	}
+
+	updated = bd.dirty || bt.dirty;
+
+	bq27xxx_battery_write_dm_block(di, &bd);
+	bq27xxx_battery_write_dm_block(di, &bt);
+
+	bq27xxx_battery_seal(di);
+
+	if (updated && di->chip != BQ27421) { /* not a cfgupdate chip, so reset */
+		bq27xxx_write(di, BQ27XXX_REG_CTRL, BQ27XXX_RESET, false);
+		BQ27XXX_MSLEEP(300); /* reset time is not documented */
+	}
+	/* assume bq27xxx_battery_update() is called hereafter */
+}
+
+static void bq27xxx_battery_settings(struct bq27xxx_device_info *di)
+{
+	struct power_supply_battery_info info = {};
+	unsigned int min, max;
+
+	if (power_supply_get_battery_info(di->bat, &info) < 0)
+		return;
+
+	if (!di->dm_regs) {
+		dev_warn(di->dev, "data memory update not supported for chip\n");
+		return;
+	}
+
+	if (info.energy_full_design_uwh != info.charge_full_design_uah) {
+		if (info.energy_full_design_uwh == -EINVAL)
+			dev_warn(di->dev, "missing battery:energy-full-design-microwatt-hours\n");
+		else if (info.charge_full_design_uah == -EINVAL)
+			dev_warn(di->dev, "missing battery:charge-full-design-microamp-hours\n");
+	}
+
+	/* assume min == 0 */
+	max = di->dm_regs[BQ27XXX_DM_DESIGN_ENERGY].max;
+	if (info.energy_full_design_uwh > max * 1000) {
+		dev_err(di->dev, "invalid battery:energy-full-design-microwatt-hours %d\n",
+			info.energy_full_design_uwh);
+		info.energy_full_design_uwh = -EINVAL;
+	}
+
+	/* assume min == 0 */
+	max = di->dm_regs[BQ27XXX_DM_DESIGN_CAPACITY].max;
+	if (info.charge_full_design_uah > max * 1000) {
+		dev_err(di->dev, "invalid battery:charge-full-design-microamp-hours %d\n",
+			info.charge_full_design_uah);
+		info.charge_full_design_uah = -EINVAL;
+	}
+
+	min = di->dm_regs[BQ27XXX_DM_TERMINATE_VOLTAGE].min;
+	max = di->dm_regs[BQ27XXX_DM_TERMINATE_VOLTAGE].max;
+	if ((info.voltage_min_design_uv < min * 1000 ||
+	     info.voltage_min_design_uv > max * 1000) &&
+	     info.voltage_min_design_uv != -EINVAL) {
+		dev_err(di->dev, "invalid battery:voltage-min-design-microvolt %d\n",
+			info.voltage_min_design_uv);
+		info.voltage_min_design_uv = -EINVAL;
+	}
+
+	if ((info.energy_full_design_uwh != -EINVAL &&
+	     info.charge_full_design_uah != -EINVAL) ||
+	     info.voltage_min_design_uv  != -EINVAL)
+		bq27xxx_battery_set_config(di, &info);
 }
 
 /*
@@ -1318,6 +1837,13 @@ static int bq27xxx_battery_get_property(struct power_supply *psy,
 	case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
 		ret = bq27xxx_simple_value(di->charge_design_full, val);
 		break;
+	/*
+	 * TODO: Implement these to make registers set from
+	 * power_supply_battery_info visible in sysfs.
+	 */
+	case POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN:
+	case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
+		return -EINVAL;
 	case POWER_SUPPLY_PROP_CYCLE_COUNT:
 		ret = bq27xxx_simple_value(di->cache.cycle_count, val);
 		break;
@@ -1351,7 +1877,10 @@ static void bq27xxx_external_power_changed(struct power_supply *psy)
 int bq27xxx_battery_setup(struct bq27xxx_device_info *di)
 {
 	struct power_supply_desc *psy_desc;
-	struct power_supply_config psy_cfg = { .drv_data = di, };
+	struct power_supply_config psy_cfg = {
+		.of_node = di->dev->of_node,
+		.drv_data = di,
+	};
 
 	INIT_DELAYED_WORK(&di->work, bq27xxx_battery_poll);
 	mutex_init(&di->lock);
@@ -1376,6 +1905,7 @@ int bq27xxx_battery_setup(struct bq27xxx_device_info *di)
 
 	dev_info(di->dev, "support ver. %s enabled\n", DRIVER_VERSION);
 
+	bq27xxx_battery_settings(di);
 	bq27xxx_battery_update(di);
 
 	mutex_lock(&bq27xxx_list_lock);
diff --git a/drivers/power/supply/bq27xxx_battery_i2c.c b/drivers/power/supply/bq27xxx_battery_i2c.c
index c68fbc3fe50a..a5972214f074 100644
--- a/drivers/power/supply/bq27xxx_battery_i2c.c
+++ b/drivers/power/supply/bq27xxx_battery_i2c.c
@@ -38,7 +38,7 @@ static int bq27xxx_battery_i2c_read(struct bq27xxx_device_info *di, u8 reg,
 {
 	struct i2c_client *client = to_i2c_client(di->dev);
 	struct i2c_msg msg[2];
-	unsigned char data[2];
+	u8 data[2];
 	int ret;
 
 	if (!client->adapter)
@@ -68,6 +68,82 @@ static int bq27xxx_battery_i2c_read(struct bq27xxx_device_info *di, u8 reg,
 	return ret;
 }
 
+static int bq27xxx_battery_i2c_write(struct bq27xxx_device_info *di, u8 reg,
+				     int value, bool single)
+{
+	struct i2c_client *client = to_i2c_client(di->dev);
+	struct i2c_msg msg;
+	u8 data[4];
+	int ret;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = reg;
+	if (single) {
+		data[1] = (u8) value;
+		msg.len = 2;
+	} else {
+		put_unaligned_le16(value, &data[1]);
+		msg.len = 3;
+	}
+
+	msg.buf = data;
+	msg.addr = client->addr;
+	msg.flags = 0;
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0)
+		return ret;
+	if (ret != 1)
+		return -EINVAL;
+	return 0;
+}
+
+static int bq27xxx_battery_i2c_bulk_read(struct bq27xxx_device_info *di, u8 reg,
+					 u8 *data, int len)
+{
+	struct i2c_client *client = to_i2c_client(di->dev);
+	int ret;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	ret = i2c_smbus_read_i2c_block_data(client, reg, len, data);
+	if (ret < 0)
+		return ret;
+	if (ret != len)
+		return -EINVAL;
+	return 0;
+}
+
+static int bq27xxx_battery_i2c_bulk_write(struct bq27xxx_device_info *di,
+					  u8 reg, u8 *data, int len)
+{
+	struct i2c_client *client = to_i2c_client(di->dev);
+	struct i2c_msg msg;
+	u8 buf[33];
+	int ret;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	buf[0] = reg;
+	memcpy(&buf[1], data, len);
+
+	msg.buf = buf;
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = len + 1;
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0)
+		return ret;
+	if (ret != 1)
+		return -EINVAL;
+	return 0;
+}
+
 static int bq27xxx_battery_i2c_probe(struct i2c_client *client,
 				     const struct i2c_device_id *id)
 {
@@ -95,7 +171,11 @@ static int bq27xxx_battery_i2c_probe(struct i2c_client *client,
 	di->dev = &client->dev;
 	di->chip = id->driver_data;
 	di->name = name;
+
 	di->bus.read = bq27xxx_battery_i2c_read;
+	di->bus.write = bq27xxx_battery_i2c_write;
+	di->bus.read_bulk = bq27xxx_battery_i2c_bulk_read;
+	di->bus.write_bulk = bq27xxx_battery_i2c_bulk_write;
 
 	ret = bq27xxx_battery_setup(di);
 	if (ret)
diff --git a/drivers/power/supply/cpcap-battery.c b/drivers/power/supply/cpcap-battery.c
new file mode 100644
index 000000000000..ee71a2b37b12
--- /dev/null
+++ b/drivers/power/supply/cpcap-battery.c
@@ -0,0 +1,808 @@
+/*
+ * Battery driver for CPCAP PMIC
+ *
+ * Copyright (C) 2017 Tony Lindgren <tony@atomide.com>
+ *
+ * Some parts of the code based on earlie Motorola mapphone Linux kernel
+ * drivers:
+ *
+ * Copyright (C) 2009-2010 Motorola, 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 "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/delay.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/reboot.h>
+#include <linux/regmap.h>
+
+#include <linux/iio/consumer.h>
+#include <linux/iio/types.h>
+#include <linux/mfd/motorola-cpcap.h>
+
+#include <asm/div64.h>
+
+/*
+ * Register bit defines for CPCAP_REG_BPEOL. Some of these seem to
+ * map to MC13783UG.pdf "Table 5-19. Register 13, Power Control 0"
+ * to enable BATTDETEN, LOBAT and EOL features. We currently use
+ * LOBAT interrupts instead of EOL.
+ */
+#define CPCAP_REG_BPEOL_BIT_EOL9	BIT(9)	/* Set for EOL irq */
+#define CPCAP_REG_BPEOL_BIT_EOL8	BIT(8)	/* Set for EOL irq */
+#define CPCAP_REG_BPEOL_BIT_UNKNOWN7	BIT(7)
+#define CPCAP_REG_BPEOL_BIT_UNKNOWN6	BIT(6)
+#define CPCAP_REG_BPEOL_BIT_UNKNOWN5	BIT(5)
+#define CPCAP_REG_BPEOL_BIT_EOL_MULTI	BIT(4)	/* Set for multiple EOL irqs */
+#define CPCAP_REG_BPEOL_BIT_UNKNOWN3	BIT(3)
+#define CPCAP_REG_BPEOL_BIT_UNKNOWN2	BIT(2)
+#define CPCAP_REG_BPEOL_BIT_BATTDETEN	BIT(1)	/* Enable battery detect */
+#define CPCAP_REG_BPEOL_BIT_EOLSEL	BIT(0)	/* BPDET = 0, EOL = 1 */
+
+#define CPCAP_BATTERY_CC_SAMPLE_PERIOD_MS	250
+
+enum {
+	CPCAP_BATTERY_IIO_BATTDET,
+	CPCAP_BATTERY_IIO_VOLTAGE,
+	CPCAP_BATTERY_IIO_CHRG_CURRENT,
+	CPCAP_BATTERY_IIO_BATT_CURRENT,
+	CPCAP_BATTERY_IIO_NR,
+};
+
+enum cpcap_battery_irq_action {
+	CPCAP_BATTERY_IRQ_ACTION_NONE,
+	CPCAP_BATTERY_IRQ_ACTION_BATTERY_LOW,
+	CPCAP_BATTERY_IRQ_ACTION_POWEROFF,
+};
+
+struct cpcap_interrupt_desc {
+	const char *name;
+	struct list_head node;
+	int irq;
+	enum cpcap_battery_irq_action action;
+};
+
+struct cpcap_battery_config {
+	int ccm;
+	int cd_factor;
+	struct power_supply_info info;
+};
+
+struct cpcap_coulomb_counter_data {
+	s32 sample;		/* 24-bits */
+	s32 accumulator;
+	s16 offset;		/* 10-bits */
+};
+
+enum cpcap_battery_state {
+	CPCAP_BATTERY_STATE_PREVIOUS,
+	CPCAP_BATTERY_STATE_LATEST,
+	CPCAP_BATTERY_STATE_NR,
+};
+
+struct cpcap_battery_state_data {
+	int voltage;
+	int current_ua;
+	int counter_uah;
+	int temperature;
+	ktime_t time;
+	struct cpcap_coulomb_counter_data cc;
+};
+
+struct cpcap_battery_ddata {
+	struct device *dev;
+	struct regmap *reg;
+	struct list_head irq_list;
+	struct iio_channel *channels[CPCAP_BATTERY_IIO_NR];
+	struct power_supply *psy;
+	struct cpcap_battery_config config;
+	struct cpcap_battery_state_data state[CPCAP_BATTERY_STATE_NR];
+	atomic_t active;
+	int status;
+	u16 vendor;
+};
+
+#define CPCAP_NO_BATTERY	-400
+
+static struct cpcap_battery_state_data *
+cpcap_battery_get_state(struct cpcap_battery_ddata *ddata,
+			enum cpcap_battery_state state)
+{
+	if (state >= CPCAP_BATTERY_STATE_NR)
+		return NULL;
+
+	return &ddata->state[state];
+}
+
+static struct cpcap_battery_state_data *
+cpcap_battery_latest(struct cpcap_battery_ddata *ddata)
+{
+	return cpcap_battery_get_state(ddata, CPCAP_BATTERY_STATE_LATEST);
+}
+
+static struct cpcap_battery_state_data *
+cpcap_battery_previous(struct cpcap_battery_ddata *ddata)
+{
+	return cpcap_battery_get_state(ddata, CPCAP_BATTERY_STATE_PREVIOUS);
+}
+
+static int cpcap_charger_battery_temperature(struct cpcap_battery_ddata *ddata,
+					     int *value)
+{
+	struct iio_channel *channel;
+	int error;
+
+	channel = ddata->channels[CPCAP_BATTERY_IIO_BATTDET];
+	error = iio_read_channel_processed(channel, value);
+	if (error < 0) {
+		dev_warn(ddata->dev, "%s failed: %i\n", __func__, error);
+		*value = CPCAP_NO_BATTERY;
+
+		return error;
+	}
+
+	*value /= 100;
+
+	return 0;
+}
+
+static int cpcap_battery_get_voltage(struct cpcap_battery_ddata *ddata)
+{
+	struct iio_channel *channel;
+	int error, value = 0;
+
+	channel = ddata->channels[CPCAP_BATTERY_IIO_VOLTAGE];
+	error = iio_read_channel_processed(channel, &value);
+	if (error < 0) {
+		dev_warn(ddata->dev, "%s failed: %i\n", __func__, error);
+
+		return 0;
+	}
+
+	return value * 1000;
+}
+
+static int cpcap_battery_get_current(struct cpcap_battery_ddata *ddata)
+{
+	struct iio_channel *channel;
+	int error, value = 0;
+
+	channel = ddata->channels[CPCAP_BATTERY_IIO_BATT_CURRENT];
+	error = iio_read_channel_processed(channel, &value);
+	if (error < 0) {
+		dev_warn(ddata->dev, "%s failed: %i\n", __func__, error);
+
+		return 0;
+	}
+
+	return value * 1000;
+}
+
+/**
+ * cpcap_battery_cc_raw_div - calculate and divide coulomb counter μAms values
+ * @ddata: device driver data
+ * @sample: coulomb counter sample value
+ * @accumulator: coulomb counter integrator value
+ * @offset: coulomb counter offset value
+ * @divider: conversion divider
+ *
+ * Note that cc_lsb and cc_dur values are from Motorola Linux kernel
+ * function data_get_avg_curr_ua() and seem to be based on measured test
+ * results. It also has the following comment:
+ *
+ * Adjustment factors are applied here as a temp solution per the test
+ * results. Need to work out a formal solution for this adjustment.
+ *
+ * A coulomb counter for similar hardware seems to be documented in
+ * "TWL6030 Gas Gauging Basics (Rev. A)" swca095a.pdf in chapter
+ * "10 Calculating Accumulated Current". We however follow what the
+ * Motorola mapphone Linux kernel is doing as there may be either a
+ * TI or ST coulomb counter in the PMIC.
+ */
+static int cpcap_battery_cc_raw_div(struct cpcap_battery_ddata *ddata,
+				    u32 sample, s32 accumulator,
+				    s16 offset, u32 divider)
+{
+	s64 acc;
+	u64 tmp;
+	int avg_current;
+	u32 cc_lsb;
+
+	sample &= 0xffffff;		/* 24-bits, unsigned */
+	offset &= 0x7ff;		/* 10-bits, signed */
+
+	switch (ddata->vendor) {
+	case CPCAP_VENDOR_ST:
+		cc_lsb = 95374;		/* μAms per LSB */
+		break;
+	case CPCAP_VENDOR_TI:
+		cc_lsb = 91501;		/* μAms per LSB */
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	acc = accumulator;
+	acc = acc - ((s64)sample * offset);
+	cc_lsb = (cc_lsb * ddata->config.cd_factor) / 1000;
+
+	if (acc >=  0)
+		tmp = acc;
+	else
+		tmp = acc * -1;
+
+	tmp = tmp * cc_lsb;
+	do_div(tmp, divider);
+	avg_current = tmp;
+
+	if (acc >= 0)
+		return -avg_current;
+	else
+		return avg_current;
+}
+
+/* 3600000μAms = 1μAh */
+static int cpcap_battery_cc_to_uah(struct cpcap_battery_ddata *ddata,
+				   u32 sample, s32 accumulator,
+				   s16 offset)
+{
+	return cpcap_battery_cc_raw_div(ddata, sample,
+					accumulator, offset,
+					3600000);
+}
+
+static int cpcap_battery_cc_to_ua(struct cpcap_battery_ddata *ddata,
+				  u32 sample, s32 accumulator,
+				  s16 offset)
+{
+	return cpcap_battery_cc_raw_div(ddata, sample,
+					accumulator, offset,
+					sample *
+					CPCAP_BATTERY_CC_SAMPLE_PERIOD_MS);
+}
+
+/**
+ * cpcap_battery_read_accumulated - reads cpcap coulomb counter
+ * @ddata: device driver data
+ * @regs: coulomb counter values
+ *
+ * Based on Motorola mapphone kernel function data_read_regs().
+ * Looking at the registers, the coulomb counter seems similar to
+ * the coulomb counter in TWL6030. See "TWL6030 Gas Gauging Basics
+ * (Rev. A) swca095a.pdf for "10 Calculating Accumulated Current".
+ *
+ * Note that swca095a.pdf instructs to stop the coulomb counter
+ * before reading to avoid values changing. Motorola mapphone
+ * Linux kernel does not do it, so let's assume they've verified
+ * the data produced is correct.
+ */
+static int
+cpcap_battery_read_accumulated(struct cpcap_battery_ddata *ddata,
+			       struct cpcap_coulomb_counter_data *ccd)
+{
+	u16 buf[7];	/* CPCAP_REG_CC1 to CCI */
+	int error;
+
+	ccd->sample = 0;
+	ccd->accumulator = 0;
+	ccd->offset = 0;
+
+	/* Read coulomb counter register range */
+	error = regmap_bulk_read(ddata->reg, CPCAP_REG_CCS1,
+				 buf, ARRAY_SIZE(buf));
+	if (error)
+		return 0;
+
+	/* Sample value CPCAP_REG_CCS1 & 2 */
+	ccd->sample = (buf[1] & 0x0fff) << 16;
+	ccd->sample |= buf[0];
+
+	/* Accumulator value CPCAP_REG_CCA1 & 2 */
+	ccd->accumulator = ((s16)buf[3]) << 16;
+	ccd->accumulator |= buf[2];
+
+	/* Offset value CPCAP_REG_CCO */
+	ccd->offset = buf[5];
+
+	/* Adjust offset based on mode value CPCAP_REG_CCM? */
+	if (buf[4] >= 0x200)
+		ccd->offset |= 0xfc00;
+
+	return cpcap_battery_cc_to_uah(ddata,
+				       ccd->sample,
+				       ccd->accumulator,
+				       ccd->offset);
+}
+
+/**
+ * cpcap_battery_cc_get_avg_current - read cpcap coulumb counter
+ * @ddata: cpcap battery driver device data
+ */
+static int cpcap_battery_cc_get_avg_current(struct cpcap_battery_ddata *ddata)
+{
+	int value, acc, error;
+	s32 sample = 1;
+	s16 offset;
+
+	if (ddata->vendor == CPCAP_VENDOR_ST)
+		sample = 4;
+
+	/* Coulomb counter integrator */
+	error = regmap_read(ddata->reg, CPCAP_REG_CCI, &value);
+	if (error)
+		return error;
+
+	if ((ddata->vendor == CPCAP_VENDOR_TI) && (value > 0x2000))
+		value = value | 0xc000;
+
+	acc = (s16)value;
+
+	/* Coulomb counter sample time */
+	error = regmap_read(ddata->reg, CPCAP_REG_CCM, &value);
+	if (error)
+		return error;
+
+	if (value < 0x200)
+		offset = value;
+	else
+		offset = value | 0xfc00;
+
+	return cpcap_battery_cc_to_ua(ddata, sample, acc, offset);
+}
+
+static bool cpcap_battery_full(struct cpcap_battery_ddata *ddata)
+{
+	struct cpcap_battery_state_data *state = cpcap_battery_latest(ddata);
+
+	/* Basically anything that measures above 4347000 is full */
+	if (state->voltage >= (ddata->config.info.voltage_max_design - 4000))
+		return true;
+
+	return false;
+}
+
+static int cpcap_battery_update_status(struct cpcap_battery_ddata *ddata)
+{
+	struct cpcap_battery_state_data state, *latest, *previous;
+	ktime_t now;
+	int error;
+
+	memset(&state, 0, sizeof(state));
+	now = ktime_get();
+
+	latest = cpcap_battery_latest(ddata);
+	if (latest) {
+		s64 delta_ms = ktime_to_ms(ktime_sub(now, latest->time));
+
+		if (delta_ms < CPCAP_BATTERY_CC_SAMPLE_PERIOD_MS)
+			return delta_ms;
+	}
+
+	state.time = now;
+	state.voltage = cpcap_battery_get_voltage(ddata);
+	state.current_ua = cpcap_battery_get_current(ddata);
+	state.counter_uah = cpcap_battery_read_accumulated(ddata, &state.cc);
+
+	error = cpcap_charger_battery_temperature(ddata,
+						  &state.temperature);
+	if (error)
+		return error;
+
+	previous = cpcap_battery_previous(ddata);
+	memcpy(previous, latest, sizeof(*previous));
+	memcpy(latest, &state, sizeof(*latest));
+
+	return 0;
+}
+
+static enum power_supply_property cpcap_battery_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_TECHNOLOGY,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN,
+	POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN,
+	POWER_SUPPLY_PROP_CURRENT_AVG,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
+	POWER_SUPPLY_PROP_CHARGE_COUNTER,
+	POWER_SUPPLY_PROP_POWER_NOW,
+	POWER_SUPPLY_PROP_POWER_AVG,
+	POWER_SUPPLY_PROP_CAPACITY_LEVEL,
+	POWER_SUPPLY_PROP_SCOPE,
+	POWER_SUPPLY_PROP_TEMP,
+};
+
+static int cpcap_battery_get_property(struct power_supply *psy,
+				      enum power_supply_property psp,
+				      union power_supply_propval *val)
+{
+	struct cpcap_battery_ddata *ddata = power_supply_get_drvdata(psy);
+	struct cpcap_battery_state_data *latest, *previous;
+	u32 sample;
+	s32 accumulator;
+	int cached;
+	s64 tmp;
+
+	cached = cpcap_battery_update_status(ddata);
+	if (cached < 0)
+		return cached;
+
+	latest = cpcap_battery_latest(ddata);
+	previous = cpcap_battery_previous(ddata);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_PRESENT:
+		if (latest->temperature > CPCAP_NO_BATTERY)
+			val->intval = 1;
+		else
+			val->intval = 0;
+		break;
+	case POWER_SUPPLY_PROP_STATUS:
+		if (cpcap_battery_full(ddata)) {
+			val->intval = POWER_SUPPLY_STATUS_FULL;
+			break;
+		}
+		if (cpcap_battery_cc_get_avg_current(ddata) < 0)
+			val->intval = POWER_SUPPLY_STATUS_CHARGING;
+		else
+			val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
+		break;
+	case POWER_SUPPLY_PROP_TECHNOLOGY:
+		val->intval = ddata->config.info.technology;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		val->intval = cpcap_battery_get_voltage(ddata);
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
+		val->intval = ddata->config.info.voltage_max_design;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
+		val->intval = ddata->config.info.voltage_min_design;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_AVG:
+		if (cached) {
+			val->intval = cpcap_battery_cc_get_avg_current(ddata);
+			break;
+		}
+		sample = latest->cc.sample - previous->cc.sample;
+		accumulator = latest->cc.accumulator - previous->cc.accumulator;
+		val->intval = cpcap_battery_cc_to_ua(ddata, sample,
+						     accumulator,
+						     latest->cc.offset);
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		val->intval = latest->current_ua;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_COUNTER:
+		val->intval = latest->counter_uah;
+		break;
+	case POWER_SUPPLY_PROP_POWER_NOW:
+		tmp = (latest->voltage / 10000) * latest->current_ua;
+		val->intval = div64_s64(tmp, 100);
+		break;
+	case POWER_SUPPLY_PROP_POWER_AVG:
+		if (cached) {
+			tmp = cpcap_battery_cc_get_avg_current(ddata);
+			tmp *= (latest->voltage / 10000);
+			val->intval = div64_s64(tmp, 100);
+			break;
+		}
+		sample = latest->cc.sample - previous->cc.sample;
+		accumulator = latest->cc.accumulator - previous->cc.accumulator;
+		tmp = cpcap_battery_cc_to_ua(ddata, sample, accumulator,
+					     latest->cc.offset);
+		tmp *= ((latest->voltage + previous->voltage) / 20000);
+		val->intval = div64_s64(tmp, 100);
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY_LEVEL:
+		if (cpcap_battery_full(ddata))
+			val->intval = POWER_SUPPLY_CAPACITY_LEVEL_FULL;
+		else if (latest->voltage >= 3750000)
+			val->intval = POWER_SUPPLY_CAPACITY_LEVEL_HIGH;
+		else if (latest->voltage >= 3300000)
+			val->intval = POWER_SUPPLY_CAPACITY_LEVEL_NORMAL;
+		else if (latest->voltage > 3100000)
+			val->intval = POWER_SUPPLY_CAPACITY_LEVEL_LOW;
+		else if (latest->voltage <= 3100000)
+			val->intval = POWER_SUPPLY_CAPACITY_LEVEL_CRITICAL;
+		else
+			val->intval = POWER_SUPPLY_CAPACITY_LEVEL_UNKNOWN;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
+		val->intval = ddata->config.info.charge_full_design;
+		break;
+	case POWER_SUPPLY_PROP_SCOPE:
+		val->intval = POWER_SUPPLY_SCOPE_SYSTEM;
+		break;
+	case POWER_SUPPLY_PROP_TEMP:
+		val->intval = latest->temperature;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static irqreturn_t cpcap_battery_irq_thread(int irq, void *data)
+{
+	struct cpcap_battery_ddata *ddata = data;
+	struct cpcap_battery_state_data *latest;
+	struct cpcap_interrupt_desc *d;
+
+	if (!atomic_read(&ddata->active))
+		return IRQ_NONE;
+
+	list_for_each_entry(d, &ddata->irq_list, node) {
+		if (irq == d->irq)
+			break;
+	}
+
+	if (!d)
+		return IRQ_NONE;
+
+	latest = cpcap_battery_latest(ddata);
+
+	switch (d->action) {
+	case CPCAP_BATTERY_IRQ_ACTION_BATTERY_LOW:
+		if (latest->counter_uah >= 0)
+			dev_warn(ddata->dev, "Battery low at 3.3V!\n");
+		break;
+	case CPCAP_BATTERY_IRQ_ACTION_POWEROFF:
+		if (latest->counter_uah >= 0) {
+			dev_emerg(ddata->dev,
+				  "Battery empty at 3.1V, powering off\n");
+			orderly_poweroff(true);
+		}
+		break;
+	default:
+		break;
+	}
+
+	power_supply_changed(ddata->psy);
+
+	return IRQ_HANDLED;
+}
+
+static int cpcap_battery_init_irq(struct platform_device *pdev,
+				  struct cpcap_battery_ddata *ddata,
+				  const char *name)
+{
+	struct cpcap_interrupt_desc *d;
+	int irq, error;
+
+	irq = platform_get_irq_byname(pdev, name);
+	if (!irq)
+		return -ENODEV;
+
+	error = devm_request_threaded_irq(ddata->dev, irq, NULL,
+					  cpcap_battery_irq_thread,
+					  IRQF_SHARED,
+					  name, ddata);
+	if (error) {
+		dev_err(ddata->dev, "could not get irq %s: %i\n",
+			name, error);
+
+		return error;
+	}
+
+	d = devm_kzalloc(ddata->dev, sizeof(*d), GFP_KERNEL);
+	if (!d)
+		return -ENOMEM;
+
+	d->name = name;
+	d->irq = irq;
+
+	if (!strncmp(name, "lowbph", 6))
+		d->action = CPCAP_BATTERY_IRQ_ACTION_BATTERY_LOW;
+	else if (!strncmp(name, "lowbpl", 6))
+		d->action = CPCAP_BATTERY_IRQ_ACTION_POWEROFF;
+
+	list_add(&d->node, &ddata->irq_list);
+
+	return 0;
+}
+
+static int cpcap_battery_init_interrupts(struct platform_device *pdev,
+					 struct cpcap_battery_ddata *ddata)
+{
+	const char * const cpcap_battery_irqs[] = {
+		"eol", "lowbph", "lowbpl",
+		"chrgcurr1", "battdetb"
+	};
+	int i, error;
+
+	for (i = 0; i < ARRAY_SIZE(cpcap_battery_irqs); i++) {
+		error = cpcap_battery_init_irq(pdev, ddata,
+					       cpcap_battery_irqs[i]);
+		if (error)
+			return error;
+	}
+
+	/* Enable low battery interrupts for 3.3V high and 3.1V low */
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_BPEOL,
+				   0xffff,
+				   CPCAP_REG_BPEOL_BIT_BATTDETEN);
+	if (error)
+		return error;
+
+	return 0;
+}
+
+static int cpcap_battery_init_iio(struct cpcap_battery_ddata *ddata)
+{
+	const char * const names[CPCAP_BATTERY_IIO_NR] = {
+		"battdetb", "battp", "chg_isense", "batti",
+	};
+	int error, i;
+
+	for (i = 0; i < CPCAP_BATTERY_IIO_NR; i++) {
+		ddata->channels[i] = devm_iio_channel_get(ddata->dev,
+							  names[i]);
+		if (IS_ERR(ddata->channels[i])) {
+			error = PTR_ERR(ddata->channels[i]);
+			goto out_err;
+		}
+
+		if (!ddata->channels[i]->indio_dev) {
+			error = -ENXIO;
+			goto out_err;
+		}
+	}
+
+	return 0;
+
+out_err:
+	dev_err(ddata->dev, "could not initialize VBUS or ID IIO: %i\n",
+		error);
+
+	return error;
+}
+
+/*
+ * Based on the values from Motorola mapphone Linux kernel. In the
+ * the Motorola mapphone Linux kernel tree the value for pm_cd_factor
+ * is passed to the kernel via device tree. If it turns out to be
+ * something device specific we can consider that too later.
+ *
+ * And looking at the battery full and shutdown values for the stock
+ * kernel on droid 4, full is 4351000 and software initiates shutdown
+ * at 3078000. The device will die around 2743000.
+ */
+static const struct cpcap_battery_config cpcap_battery_default_data = {
+	.ccm = 0x3ff,
+	.cd_factor = 0x3cc,
+	.info.technology = POWER_SUPPLY_TECHNOLOGY_LION,
+	.info.voltage_max_design = 4351000,
+	.info.voltage_min_design = 3100000,
+	.info.charge_full_design = 1740000,
+};
+
+#ifdef CONFIG_OF
+static const struct of_device_id cpcap_battery_id_table[] = {
+	{
+		.compatible = "motorola,cpcap-battery",
+		.data = &cpcap_battery_default_data,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, cpcap_battery_id_table);
+#endif
+
+static int cpcap_battery_probe(struct platform_device *pdev)
+{
+	struct power_supply_desc *psy_desc;
+	struct cpcap_battery_ddata *ddata;
+	const struct of_device_id *match;
+	struct power_supply_config psy_cfg = {};
+	int error;
+
+	match = of_match_device(of_match_ptr(cpcap_battery_id_table),
+				&pdev->dev);
+	if (!match)
+		return -EINVAL;
+
+	if (!match->data) {
+		dev_err(&pdev->dev, "no configuration data found\n");
+
+		return -ENODEV;
+	}
+
+	ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+	if (!ddata)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&ddata->irq_list);
+	ddata->dev = &pdev->dev;
+	memcpy(&ddata->config, match->data, sizeof(ddata->config));
+
+	ddata->reg = dev_get_regmap(ddata->dev->parent, NULL);
+	if (!ddata->reg)
+		return -ENODEV;
+
+	error = cpcap_get_vendor(ddata->dev, ddata->reg, &ddata->vendor);
+	if (error)
+		return error;
+
+	platform_set_drvdata(pdev, ddata);
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_CCM,
+				   0xffff, ddata->config.ccm);
+	if (error)
+		return error;
+
+	error = cpcap_battery_init_interrupts(pdev, ddata);
+	if (error)
+		return error;
+
+	error = cpcap_battery_init_iio(ddata);
+	if (error)
+		return error;
+
+	psy_desc = devm_kzalloc(ddata->dev, sizeof(*psy_desc), GFP_KERNEL);
+	if (!psy_desc)
+		return -ENOMEM;
+
+	psy_desc->name = "battery",
+	psy_desc->type = POWER_SUPPLY_TYPE_BATTERY,
+	psy_desc->properties = cpcap_battery_props,
+	psy_desc->num_properties = ARRAY_SIZE(cpcap_battery_props),
+	psy_desc->get_property = cpcap_battery_get_property,
+
+	psy_cfg.of_node = pdev->dev.of_node;
+	psy_cfg.drv_data = ddata;
+
+	ddata->psy = devm_power_supply_register(ddata->dev, psy_desc,
+						&psy_cfg);
+	error = PTR_ERR_OR_ZERO(ddata->psy);
+	if (error) {
+		dev_err(ddata->dev, "failed to register power supply\n");
+		return error;
+	}
+
+	atomic_set(&ddata->active, 1);
+
+	return 0;
+}
+
+static int cpcap_battery_remove(struct platform_device *pdev)
+{
+	struct cpcap_battery_ddata *ddata = platform_get_drvdata(pdev);
+	int error;
+
+	atomic_set(&ddata->active, 0);
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_BPEOL,
+				   0xffff, 0);
+	if (error)
+		dev_err(&pdev->dev, "could not disable: %i\n", error);
+
+	return 0;
+}
+
+static struct platform_driver cpcap_battery_driver = {
+	.driver	= {
+		.name		= "cpcap_battery",
+		.of_match_table = of_match_ptr(cpcap_battery_id_table),
+	},
+	.probe	= cpcap_battery_probe,
+	.remove = cpcap_battery_remove,
+};
+module_platform_driver(cpcap_battery_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Tony Lindgren <tony@atomide.com>");
+MODULE_DESCRIPTION("CPCAP PMIC Battery Driver");
diff --git a/drivers/power/supply/cpcap-charger.c b/drivers/power/supply/cpcap-charger.c
index 26a2dc7ac9a2..11a07633de6c 100644
--- a/drivers/power/supply/cpcap-charger.c
+++ b/drivers/power/supply/cpcap-charger.c
@@ -38,20 +38,27 @@
 #include <linux/iio/consumer.h>
 #include <linux/mfd/motorola-cpcap.h>
 
-/* CPCAP_REG_CRM register bits */
+/*
+ * CPCAP_REG_CRM register bits. For documentation of somewhat similar hardware,
+ * see NXP "MC13783 Power Management and Audio Circuit Users's Guide"
+ * MC13783UG.pdf chapter "8.5 Battery Interface Register Summary". The registers
+ * and values for CPCAP are different, but some of the internal components seem
+ * similar. Also see the Motorola Linux kernel cpcap-regbits.h. CPCAP_REG_CHRGR_1
+ * bits that seem to describe the CRM register.
+ */
 #define CPCAP_REG_CRM_UNUSED_641_15	BIT(15)	/* 641 = register number */
 #define CPCAP_REG_CRM_UNUSED_641_14	BIT(14)	/* 641 = register number */
-#define CPCAP_REG_CRM_CHRG_LED_EN	BIT(13)
-#define CPCAP_REG_CRM_RVRSMODE		BIT(12)
-#define CPCAP_REG_CRM_ICHRG_TR1		BIT(11)
+#define CPCAP_REG_CRM_CHRG_LED_EN	BIT(13)	/* Charger LED */
+#define CPCAP_REG_CRM_RVRSMODE		BIT(12)	/* USB VBUS output enable */
+#define CPCAP_REG_CRM_ICHRG_TR1		BIT(11)	/* Trickle charge current */
 #define CPCAP_REG_CRM_ICHRG_TR0		BIT(10)
-#define CPCAP_REG_CRM_FET_OVRD		BIT(9)
-#define CPCAP_REG_CRM_FET_CTRL		BIT(8)
-#define CPCAP_REG_CRM_VCHRG3		BIT(7)
+#define CPCAP_REG_CRM_FET_OVRD		BIT(9)	/* 0 = hardware, 1 = FET_CTRL */
+#define CPCAP_REG_CRM_FET_CTRL		BIT(8)	/* BPFET 1 if FET_OVRD set */
+#define CPCAP_REG_CRM_VCHRG3		BIT(7)	/* Charge voltage bits */
 #define CPCAP_REG_CRM_VCHRG2		BIT(6)
 #define CPCAP_REG_CRM_VCHRG1		BIT(5)
 #define CPCAP_REG_CRM_VCHRG0		BIT(4)
-#define CPCAP_REG_CRM_ICHRG3		BIT(3)
+#define CPCAP_REG_CRM_ICHRG3		BIT(3)	/* Charge current bits */
 #define CPCAP_REG_CRM_ICHRG2		BIT(2)
 #define CPCAP_REG_CRM_ICHRG1		BIT(1)
 #define CPCAP_REG_CRM_ICHRG0		BIT(0)
@@ -63,42 +70,50 @@
 #define CPCAP_REG_CRM_TR_0A48		CPCAP_REG_CRM_TR(0x2)
 #define CPCAP_REG_CRM_TR_0A72		CPCAP_REG_CRM_TR(0x4)
 
-/* CPCAP_REG_CRM charge voltages */
+/*
+ * CPCAP_REG_CRM charge voltages based on the ADC channel 1 values.
+ * Note that these register bits don't match MC13783UG.pdf VCHRG
+ * register bits.
+ */
 #define CPCAP_REG_CRM_VCHRG(val)	(((val) & 0xf) << 4)
 #define CPCAP_REG_CRM_VCHRG_3V80	CPCAP_REG_CRM_VCHRG(0x0)
 #define CPCAP_REG_CRM_VCHRG_4V10	CPCAP_REG_CRM_VCHRG(0x1)
-#define CPCAP_REG_CRM_VCHRG_4V15	CPCAP_REG_CRM_VCHRG(0x2)
-#define CPCAP_REG_CRM_VCHRG_4V20	CPCAP_REG_CRM_VCHRG(0x3)
-#define CPCAP_REG_CRM_VCHRG_4V22	CPCAP_REG_CRM_VCHRG(0x4)
-#define CPCAP_REG_CRM_VCHRG_4V24	CPCAP_REG_CRM_VCHRG(0x5)
-#define CPCAP_REG_CRM_VCHRG_4V26	CPCAP_REG_CRM_VCHRG(0x6)
-#define CPCAP_REG_CRM_VCHRG_4V28	CPCAP_REG_CRM_VCHRG(0x7)
-#define CPCAP_REG_CRM_VCHRG_4V30	CPCAP_REG_CRM_VCHRG(0x8)
-#define CPCAP_REG_CRM_VCHRG_4V32	CPCAP_REG_CRM_VCHRG(0x9)
-#define CPCAP_REG_CRM_VCHRG_4V34	CPCAP_REG_CRM_VCHRG(0xa)
+#define CPCAP_REG_CRM_VCHRG_4V12	CPCAP_REG_CRM_VCHRG(0x2)
+#define CPCAP_REG_CRM_VCHRG_4V15	CPCAP_REG_CRM_VCHRG(0x3)
+#define CPCAP_REG_CRM_VCHRG_4V17	CPCAP_REG_CRM_VCHRG(0x4)
+#define CPCAP_REG_CRM_VCHRG_4V20	CPCAP_REG_CRM_VCHRG(0x5)
+#define CPCAP_REG_CRM_VCHRG_4V23	CPCAP_REG_CRM_VCHRG(0x6)
+#define CPCAP_REG_CRM_VCHRG_4V25	CPCAP_REG_CRM_VCHRG(0x7)
+#define CPCAP_REG_CRM_VCHRG_4V27	CPCAP_REG_CRM_VCHRG(0x8)
+#define CPCAP_REG_CRM_VCHRG_4V30	CPCAP_REG_CRM_VCHRG(0x9)
+#define CPCAP_REG_CRM_VCHRG_4V33	CPCAP_REG_CRM_VCHRG(0xa)
 #define CPCAP_REG_CRM_VCHRG_4V35	CPCAP_REG_CRM_VCHRG(0xb)
 #define CPCAP_REG_CRM_VCHRG_4V38	CPCAP_REG_CRM_VCHRG(0xc)
 #define CPCAP_REG_CRM_VCHRG_4V40	CPCAP_REG_CRM_VCHRG(0xd)
 #define CPCAP_REG_CRM_VCHRG_4V42	CPCAP_REG_CRM_VCHRG(0xe)
 #define CPCAP_REG_CRM_VCHRG_4V44	CPCAP_REG_CRM_VCHRG(0xf)
 
-/* CPCAP_REG_CRM charge currents */
+/*
+ * CPCAP_REG_CRM charge currents. These seem to match MC13783UG.pdf
+ * values in "Table 8-3. Charge Path Regulator Current Limit
+ * Characteristics" for the nominal values.
+ */
 #define CPCAP_REG_CRM_ICHRG(val)	(((val) & 0xf) << 0)
 #define CPCAP_REG_CRM_ICHRG_0A000	CPCAP_REG_CRM_ICHRG(0x0)
 #define CPCAP_REG_CRM_ICHRG_0A070	CPCAP_REG_CRM_ICHRG(0x1)
-#define CPCAP_REG_CRM_ICHRG_0A176	CPCAP_REG_CRM_ICHRG(0x2)
-#define CPCAP_REG_CRM_ICHRG_0A264	CPCAP_REG_CRM_ICHRG(0x3)
-#define CPCAP_REG_CRM_ICHRG_0A352	CPCAP_REG_CRM_ICHRG(0x4)
-#define CPCAP_REG_CRM_ICHRG_0A440	CPCAP_REG_CRM_ICHRG(0x5)
-#define CPCAP_REG_CRM_ICHRG_0A528	CPCAP_REG_CRM_ICHRG(0x6)
-#define CPCAP_REG_CRM_ICHRG_0A616	CPCAP_REG_CRM_ICHRG(0x7)
-#define CPCAP_REG_CRM_ICHRG_0A704	CPCAP_REG_CRM_ICHRG(0x8)
-#define CPCAP_REG_CRM_ICHRG_0A792	CPCAP_REG_CRM_ICHRG(0x9)
-#define CPCAP_REG_CRM_ICHRG_0A880	CPCAP_REG_CRM_ICHRG(0xa)
-#define CPCAP_REG_CRM_ICHRG_0A968	CPCAP_REG_CRM_ICHRG(0xb)
-#define CPCAP_REG_CRM_ICHRG_1A056	CPCAP_REG_CRM_ICHRG(0xc)
-#define CPCAP_REG_CRM_ICHRG_1A144	CPCAP_REG_CRM_ICHRG(0xd)
-#define CPCAP_REG_CRM_ICHRG_1A584	CPCAP_REG_CRM_ICHRG(0xe)
+#define CPCAP_REG_CRM_ICHRG_0A177	CPCAP_REG_CRM_ICHRG(0x2)
+#define CPCAP_REG_CRM_ICHRG_0A266	CPCAP_REG_CRM_ICHRG(0x3)
+#define CPCAP_REG_CRM_ICHRG_0A355	CPCAP_REG_CRM_ICHRG(0x4)
+#define CPCAP_REG_CRM_ICHRG_0A443	CPCAP_REG_CRM_ICHRG(0x5)
+#define CPCAP_REG_CRM_ICHRG_0A532	CPCAP_REG_CRM_ICHRG(0x6)
+#define CPCAP_REG_CRM_ICHRG_0A621	CPCAP_REG_CRM_ICHRG(0x7)
+#define CPCAP_REG_CRM_ICHRG_0A709	CPCAP_REG_CRM_ICHRG(0x8)
+#define CPCAP_REG_CRM_ICHRG_0A798	CPCAP_REG_CRM_ICHRG(0x9)
+#define CPCAP_REG_CRM_ICHRG_0A886	CPCAP_REG_CRM_ICHRG(0xa)
+#define CPCAP_REG_CRM_ICHRG_0A975	CPCAP_REG_CRM_ICHRG(0xb)
+#define CPCAP_REG_CRM_ICHRG_1A064	CPCAP_REG_CRM_ICHRG(0xc)
+#define CPCAP_REG_CRM_ICHRG_1A152	CPCAP_REG_CRM_ICHRG(0xd)
+#define CPCAP_REG_CRM_ICHRG_1A596	CPCAP_REG_CRM_ICHRG(0xe)
 #define CPCAP_REG_CRM_ICHRG_NO_LIMIT	CPCAP_REG_CRM_ICHRG(0xf)
 
 enum {
@@ -428,9 +443,9 @@ static void cpcap_usb_detect(struct work_struct *work)
 		int max_current;
 
 		if (cpcap_charger_battery_found(ddata))
-			max_current = CPCAP_REG_CRM_ICHRG_1A584;
+			max_current = CPCAP_REG_CRM_ICHRG_1A596;
 		else
-			max_current = CPCAP_REG_CRM_ICHRG_0A528;
+			max_current = CPCAP_REG_CRM_ICHRG_0A532;
 
 		error = cpcap_charger_set_state(ddata,
 						CPCAP_REG_CRM_VCHRG_4V35,
@@ -586,6 +601,7 @@ static int cpcap_charger_probe(struct platform_device *pdev)
 {
 	struct cpcap_charger_ddata *ddata;
 	const struct of_device_id *of_id;
+	struct power_supply_config psy_cfg = {};
 	int error;
 
 	of_id = of_match_device(of_match_ptr(cpcap_charger_id_table),
@@ -614,9 +630,12 @@ static int cpcap_charger_probe(struct platform_device *pdev)
 
 	atomic_set(&ddata->active, 1);
 
+	psy_cfg.of_node = pdev->dev.of_node;
+	psy_cfg.drv_data = ddata;
+
 	ddata->usb = devm_power_supply_register(ddata->dev,
 						&cpcap_charger_usb_desc,
-						NULL);
+						&psy_cfg);
 	if (IS_ERR(ddata->usb)) {
 		error = PTR_ERR(ddata->usb);
 		dev_err(ddata->dev, "failed to register USB charger: %i\n",
diff --git a/drivers/power/supply/ds2760_battery.c b/drivers/power/supply/ds2760_battery.c
index 17225689e3f6..ae180dc929c9 100644
--- a/drivers/power/supply/ds2760_battery.c
+++ b/drivers/power/supply/ds2760_battery.c
@@ -28,7 +28,7 @@
 #include <linux/platform_device.h>
 #include <linux/power_supply.h>
 
-#include "../../w1/w1.h"
+#include <linux/w1.h>
 #include "../../w1/slaves/w1_ds2760.h"
 
 struct ds2760_device_info {
diff --git a/drivers/power/supply/ds2780_battery.c b/drivers/power/supply/ds2780_battery.c
index 1b3b6fa89c28..8edd4aa5f475 100644
--- a/drivers/power/supply/ds2780_battery.c
+++ b/drivers/power/supply/ds2780_battery.c
@@ -21,7 +21,7 @@
 #include <linux/power_supply.h>
 #include <linux/idr.h>
 
-#include "../../w1/w1.h"
+#include <linux/w1.h>
 #include "../../w1/slaves/w1_ds2780.h"
 
 /* Current unit measurement in uA for a 1 milli-ohm sense resistor */
diff --git a/drivers/power/supply/ds2781_battery.c b/drivers/power/supply/ds2781_battery.c
index cc0149131f89..4400402f9ec5 100644
--- a/drivers/power/supply/ds2781_battery.c
+++ b/drivers/power/supply/ds2781_battery.c
@@ -19,7 +19,7 @@
 #include <linux/power_supply.h>
 #include <linux/idr.h>
 
-#include "../../w1/w1.h"
+#include <linux/w1.h>
 #include "../../w1/slaves/w1_ds2781.h"
 
 /* Current unit measurement in uA for a 1 milli-ohm sense resistor */
diff --git a/drivers/power/supply/ltc3651-charger.c b/drivers/power/supply/ltc3651-charger.c
new file mode 100644
index 000000000000..eea63ff211c4
--- /dev/null
+++ b/drivers/power/supply/ltc3651-charger.c
@@ -0,0 +1,210 @@
+/*
+ *  Copyright (C) 2017, Topic Embedded Products
+ *  Driver for LTC3651 charger IC.
+ *
+ *  This program is free software; you can redistribute it and/or modify it
+ *  under  the terms of the 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/gpio/consumer.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+
+struct ltc3651_charger {
+	struct power_supply *charger;
+	struct power_supply_desc charger_desc;
+	struct gpio_desc *acpr_gpio;
+	struct gpio_desc *fault_gpio;
+	struct gpio_desc *chrg_gpio;
+};
+
+static irqreturn_t ltc3651_charger_irq(int irq, void *devid)
+{
+	struct power_supply *charger = devid;
+
+	power_supply_changed(charger);
+
+	return IRQ_HANDLED;
+}
+
+static inline struct ltc3651_charger *psy_to_ltc3651_charger(
+	struct power_supply *psy)
+{
+	return power_supply_get_drvdata(psy);
+}
+
+static int ltc3651_charger_get_property(struct power_supply *psy,
+		enum power_supply_property psp, union power_supply_propval *val)
+{
+	struct ltc3651_charger *ltc3651_charger = psy_to_ltc3651_charger(psy);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		if (!ltc3651_charger->chrg_gpio) {
+			val->intval = POWER_SUPPLY_STATUS_UNKNOWN;
+			break;
+		}
+		if (gpiod_get_value(ltc3651_charger->chrg_gpio))
+			val->intval = POWER_SUPPLY_STATUS_CHARGING;
+		else
+			val->intval = POWER_SUPPLY_STATUS_NOT_CHARGING;
+		break;
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = gpiod_get_value(ltc3651_charger->acpr_gpio);
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		if (!ltc3651_charger->fault_gpio) {
+			val->intval = POWER_SUPPLY_HEALTH_UNKNOWN;
+			break;
+		}
+		if (!gpiod_get_value(ltc3651_charger->fault_gpio)) {
+			val->intval = POWER_SUPPLY_HEALTH_GOOD;
+			break;
+		}
+		/*
+		 * If the fault pin is active, the chrg pin explains the type
+		 * of failure.
+		 */
+		if (!ltc3651_charger->chrg_gpio) {
+			val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+			break;
+		}
+		val->intval = gpiod_get_value(ltc3651_charger->chrg_gpio) ?
+				POWER_SUPPLY_HEALTH_OVERHEAT :
+				POWER_SUPPLY_HEALTH_DEAD;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static enum power_supply_property ltc3651_charger_properties[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_HEALTH,
+};
+
+static int ltc3651_charger_probe(struct platform_device *pdev)
+{
+	struct power_supply_config psy_cfg = {};
+	struct ltc3651_charger *ltc3651_charger;
+	struct power_supply_desc *charger_desc;
+	int ret;
+
+	ltc3651_charger = devm_kzalloc(&pdev->dev, sizeof(*ltc3651_charger),
+					GFP_KERNEL);
+	if (!ltc3651_charger)
+		return -ENOMEM;
+
+	ltc3651_charger->acpr_gpio = devm_gpiod_get(&pdev->dev,
+					"lltc,acpr", GPIOD_IN);
+	if (IS_ERR(ltc3651_charger->acpr_gpio)) {
+		ret = PTR_ERR(ltc3651_charger->acpr_gpio);
+		dev_err(&pdev->dev, "Failed to acquire acpr GPIO: %d\n", ret);
+		return ret;
+	}
+	ltc3651_charger->fault_gpio = devm_gpiod_get_optional(&pdev->dev,
+					"lltc,fault", GPIOD_IN);
+	if (IS_ERR(ltc3651_charger->fault_gpio)) {
+		ret = PTR_ERR(ltc3651_charger->fault_gpio);
+		dev_err(&pdev->dev, "Failed to acquire fault GPIO: %d\n", ret);
+		return ret;
+	}
+	ltc3651_charger->chrg_gpio = devm_gpiod_get_optional(&pdev->dev,
+					"lltc,chrg", GPIOD_IN);
+	if (IS_ERR(ltc3651_charger->chrg_gpio)) {
+		ret = PTR_ERR(ltc3651_charger->chrg_gpio);
+		dev_err(&pdev->dev, "Failed to acquire chrg GPIO: %d\n", ret);
+		return ret;
+	}
+
+	charger_desc = &ltc3651_charger->charger_desc;
+	charger_desc->name = pdev->dev.of_node->name;
+	charger_desc->type = POWER_SUPPLY_TYPE_MAINS;
+	charger_desc->properties = ltc3651_charger_properties;
+	charger_desc->num_properties = ARRAY_SIZE(ltc3651_charger_properties);
+	charger_desc->get_property = ltc3651_charger_get_property;
+	psy_cfg.of_node = pdev->dev.of_node;
+	psy_cfg.drv_data = ltc3651_charger;
+
+	ltc3651_charger->charger = devm_power_supply_register(&pdev->dev,
+						      charger_desc, &psy_cfg);
+	if (IS_ERR(ltc3651_charger->charger)) {
+		ret = PTR_ERR(ltc3651_charger->charger);
+		dev_err(&pdev->dev, "Failed to register power supply: %d\n",
+			ret);
+		return ret;
+	}
+
+	/*
+	 * Acquire IRQs for the GPIO pins if possible. If the system does not
+	 * support IRQs on these pins, userspace will have to poll the sysfs
+	 * files manually.
+	 */
+	if (ltc3651_charger->acpr_gpio) {
+		ret = gpiod_to_irq(ltc3651_charger->acpr_gpio);
+		if (ret >= 0)
+			ret = devm_request_any_context_irq(&pdev->dev, ret,
+				ltc3651_charger_irq,
+				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+				dev_name(&pdev->dev), ltc3651_charger->charger);
+		if (ret < 0)
+			dev_warn(&pdev->dev, "Failed to request acpr irq\n");
+	}
+	if (ltc3651_charger->fault_gpio) {
+		ret = gpiod_to_irq(ltc3651_charger->fault_gpio);
+		if (ret >= 0)
+			ret = devm_request_any_context_irq(&pdev->dev, ret,
+				ltc3651_charger_irq,
+				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+				dev_name(&pdev->dev), ltc3651_charger->charger);
+		if (ret < 0)
+			dev_warn(&pdev->dev, "Failed to request fault irq\n");
+	}
+	if (ltc3651_charger->chrg_gpio) {
+		ret = gpiod_to_irq(ltc3651_charger->chrg_gpio);
+		if (ret >= 0)
+			ret = devm_request_any_context_irq(&pdev->dev, ret,
+				ltc3651_charger_irq,
+				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+				dev_name(&pdev->dev), ltc3651_charger->charger);
+		if (ret < 0)
+			dev_warn(&pdev->dev, "Failed to request chrg irq\n");
+	}
+
+	platform_set_drvdata(pdev, ltc3651_charger);
+
+	return 0;
+}
+
+static const struct of_device_id ltc3651_charger_match[] = {
+	{ .compatible = "lltc,ltc3651-charger" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ltc3651_charger_match);
+
+static struct platform_driver ltc3651_charger_driver = {
+	.probe = ltc3651_charger_probe,
+	.driver = {
+		.name = "ltc3651-charger",
+		.of_match_table = ltc3651_charger_match,
+	},
+};
+
+module_platform_driver(ltc3651_charger_driver);
+
+MODULE_AUTHOR("Mike Looijmans <mike.looijmans@topic.nl>");
+MODULE_DESCRIPTION("Driver for LTC3651 charger");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:ltc3651-charger");
diff --git a/drivers/power/supply/power_supply_core.c b/drivers/power/supply/power_supply_core.c
index 7ec7c7c202bd..540d3e0aa011 100644
--- a/drivers/power/supply/power_supply_core.c
+++ b/drivers/power/supply/power_supply_core.c
@@ -17,6 +17,7 @@
 #include <linux/device.h>
 #include <linux/notifier.h>
 #include <linux/err.h>
+#include <linux/of.h>
 #include <linux/power_supply.h>
 #include <linux/thermal.h>
 #include "power_supply.h"
@@ -274,8 +275,30 @@ static int power_supply_check_supplies(struct power_supply *psy)
 	return power_supply_populate_supplied_from(psy);
 }
 #else
-static inline int power_supply_check_supplies(struct power_supply *psy)
+static int power_supply_check_supplies(struct power_supply *psy)
 {
+	int nval, ret;
+
+	if (!psy->dev.parent)
+		return 0;
+
+	nval = device_property_read_string_array(psy->dev.parent,
+						 "supplied-from", NULL, 0);
+	if (nval <= 0)
+		return 0;
+
+	psy->supplied_from = devm_kmalloc_array(&psy->dev, nval,
+						sizeof(char *), GFP_KERNEL);
+	if (!psy->supplied_from)
+		return -ENOMEM;
+
+	ret = device_property_read_string_array(psy->dev.parent,
+		"supplied-from", (const char **)psy->supplied_from, nval);
+	if (ret < 0)
+		return ret;
+
+	psy->num_supplies = nval;
+
 	return 0;
 }
 #endif
@@ -497,6 +520,62 @@ struct power_supply *devm_power_supply_get_by_phandle(struct device *dev,
 EXPORT_SYMBOL_GPL(devm_power_supply_get_by_phandle);
 #endif /* CONFIG_OF */
 
+int power_supply_get_battery_info(struct power_supply *psy,
+				  struct power_supply_battery_info *info)
+{
+	struct device_node *battery_np;
+	const char *value;
+	int err;
+
+	info->energy_full_design_uwh         = -EINVAL;
+	info->charge_full_design_uah         = -EINVAL;
+	info->voltage_min_design_uv          = -EINVAL;
+	info->precharge_current_ua           = -EINVAL;
+	info->charge_term_current_ua         = -EINVAL;
+	info->constant_charge_current_max_ua = -EINVAL;
+	info->constant_charge_voltage_max_uv = -EINVAL;
+
+	if (!psy->of_node) {
+		dev_warn(&psy->dev, "%s currently only supports devicetree\n",
+			 __func__);
+		return -ENXIO;
+	}
+
+	battery_np = of_parse_phandle(psy->of_node, "monitored-battery", 0);
+	if (!battery_np)
+		return -ENODEV;
+
+	err = of_property_read_string(battery_np, "compatible", &value);
+	if (err)
+		return err;
+
+	if (strcmp("simple-battery", value))
+		return -ENODEV;
+
+	/* The property and field names below must correspond to elements
+	 * in enum power_supply_property. For reasoning, see
+	 * Documentation/power/power_supply_class.txt.
+	 */
+
+	of_property_read_u32(battery_np, "energy-full-design-microwatt-hours",
+			     &info->energy_full_design_uwh);
+	of_property_read_u32(battery_np, "charge-full-design-microamp-hours",
+			     &info->charge_full_design_uah);
+	of_property_read_u32(battery_np, "voltage-min-design-microvolt",
+			     &info->voltage_min_design_uv);
+	of_property_read_u32(battery_np, "precharge-current-microamp",
+			     &info->precharge_current_ua);
+	of_property_read_u32(battery_np, "charge-term-current-microamp",
+			     &info->charge_term_current_ua);
+	of_property_read_u32(battery_np, "constant_charge_current_max_microamp",
+			     &info->constant_charge_current_max_ua);
+	of_property_read_u32(battery_np, "constant_charge_voltage_max_microvolt",
+			     &info->constant_charge_voltage_max_uv);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(power_supply_get_battery_info);
+
 int power_supply_get_property(struct power_supply *psy,
 			    enum power_supply_property psp,
 			    union power_supply_propval *val)
@@ -669,7 +748,7 @@ static int ps_set_cur_charge_cntl_limit(struct thermal_cooling_device *tcd,
 	return ret;
 }
 
-static struct thermal_cooling_device_ops psy_tcd_ops = {
+static const struct thermal_cooling_device_ops psy_tcd_ops = {
 	.get_max_state = ps_get_max_charge_cntl_limit,
 	.get_cur_state = ps_get_cur_chrage_cntl_limit,
 	.set_cur_state = ps_set_cur_charge_cntl_limit,
diff --git a/drivers/power/supply/power_supply_sysfs.c b/drivers/power/supply/power_supply_sysfs.c
index bcde8d13476a..5204f115970f 100644
--- a/drivers/power/supply/power_supply_sysfs.c
+++ b/drivers/power/supply/power_supply_sysfs.c
@@ -40,35 +40,42 @@
 
 static struct device_attribute power_supply_attrs[];
 
+static const char * const power_supply_type_text[] = {
+	"Unknown", "Battery", "UPS", "Mains", "USB",
+	"USB_DCP", "USB_CDP", "USB_ACA", "USB_C",
+	"USB_PD", "USB_PD_DRP", "BrickID"
+};
+
+static const char * const power_supply_status_text[] = {
+	"Unknown", "Charging", "Discharging", "Not charging", "Full"
+};
+
+static const char * const power_supply_charge_type_text[] = {
+	"Unknown", "N/A", "Trickle", "Fast"
+};
+
+static const char * const power_supply_health_text[] = {
+	"Unknown", "Good", "Overheat", "Dead", "Over voltage",
+	"Unspecified failure", "Cold", "Watchdog timer expire",
+	"Safety timer expire"
+};
+
+static const char * const power_supply_technology_text[] = {
+	"Unknown", "NiMH", "Li-ion", "Li-poly", "LiFe", "NiCd",
+	"LiMn"
+};
+
+static const char * const power_supply_capacity_level_text[] = {
+	"Unknown", "Critical", "Low", "Normal", "High", "Full"
+};
+
+static const char * const power_supply_scope_text[] = {
+	"Unknown", "System", "Device"
+};
+
 static ssize_t power_supply_show_property(struct device *dev,
 					  struct device_attribute *attr,
 					  char *buf) {
-	static char *type_text[] = {
-		"Unknown", "Battery", "UPS", "Mains", "USB",
-		"USB_DCP", "USB_CDP", "USB_ACA", "USB_C",
-		"USB_PD", "USB_PD_DRP"
-	};
-	static char *status_text[] = {
-		"Unknown", "Charging", "Discharging", "Not charging", "Full"
-	};
-	static char *charge_type[] = {
-		"Unknown", "N/A", "Trickle", "Fast"
-	};
-	static char *health_text[] = {
-		"Unknown", "Good", "Overheat", "Dead", "Over voltage",
-		"Unspecified failure", "Cold", "Watchdog timer expire",
-		"Safety timer expire"
-	};
-	static char *technology_text[] = {
-		"Unknown", "NiMH", "Li-ion", "Li-poly", "LiFe", "NiCd",
-		"LiMn"
-	};
-	static char *capacity_level_text[] = {
-		"Unknown", "Critical", "Low", "Normal", "High", "Full"
-	};
-	static char *scope_text[] = {
-		"Unknown", "System", "Device"
-	};
 	ssize_t ret = 0;
 	struct power_supply *psy = dev_get_drvdata(dev);
 	const ptrdiff_t off = attr - power_supply_attrs;
@@ -91,19 +98,26 @@ static ssize_t power_supply_show_property(struct device *dev,
 	}
 
 	if (off == POWER_SUPPLY_PROP_STATUS)
-		return sprintf(buf, "%s\n", status_text[value.intval]);
+		return sprintf(buf, "%s\n",
+			       power_supply_status_text[value.intval]);
 	else if (off == POWER_SUPPLY_PROP_CHARGE_TYPE)
-		return sprintf(buf, "%s\n", charge_type[value.intval]);
+		return sprintf(buf, "%s\n",
+			       power_supply_charge_type_text[value.intval]);
 	else if (off == POWER_SUPPLY_PROP_HEALTH)
-		return sprintf(buf, "%s\n", health_text[value.intval]);
+		return sprintf(buf, "%s\n",
+			       power_supply_health_text[value.intval]);
 	else if (off == POWER_SUPPLY_PROP_TECHNOLOGY)
-		return sprintf(buf, "%s\n", technology_text[value.intval]);
+		return sprintf(buf, "%s\n",
+			       power_supply_technology_text[value.intval]);
 	else if (off == POWER_SUPPLY_PROP_CAPACITY_LEVEL)
-		return sprintf(buf, "%s\n", capacity_level_text[value.intval]);
+		return sprintf(buf, "%s\n",
+			       power_supply_capacity_level_text[value.intval]);
 	else if (off == POWER_SUPPLY_PROP_TYPE)
-		return sprintf(buf, "%s\n", type_text[value.intval]);
+		return sprintf(buf, "%s\n",
+			       power_supply_type_text[value.intval]);
 	else if (off == POWER_SUPPLY_PROP_SCOPE)
-		return sprintf(buf, "%s\n", scope_text[value.intval]);
+		return sprintf(buf, "%s\n",
+			       power_supply_scope_text[value.intval]);
 	else if (off >= POWER_SUPPLY_PROP_MODEL_NAME)
 		return sprintf(buf, "%s\n", value.strval);
 
@@ -117,14 +131,46 @@ static ssize_t power_supply_store_property(struct device *dev,
 	struct power_supply *psy = dev_get_drvdata(dev);
 	const ptrdiff_t off = attr - power_supply_attrs;
 	union power_supply_propval value;
-	long long_val;
 
-	/* TODO: support other types than int */
-	ret = kstrtol(buf, 10, &long_val);
-	if (ret < 0)
-		return ret;
+	/* maybe it is a enum property? */
+	switch (off) {
+	case POWER_SUPPLY_PROP_STATUS:
+		ret = sysfs_match_string(power_supply_status_text, buf);
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_TYPE:
+		ret = sysfs_match_string(power_supply_charge_type_text, buf);
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		ret = sysfs_match_string(power_supply_health_text, buf);
+		break;
+	case POWER_SUPPLY_PROP_TECHNOLOGY:
+		ret = sysfs_match_string(power_supply_technology_text, buf);
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY_LEVEL:
+		ret = sysfs_match_string(power_supply_capacity_level_text, buf);
+		break;
+	case POWER_SUPPLY_PROP_SCOPE:
+		ret = sysfs_match_string(power_supply_scope_text, buf);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	/*
+	 * If no match was found, then check to see if it is an integer.
+	 * Integer values are valid for enums in addition to the text value.
+	 */
+	if (ret < 0) {
+		long long_val;
+
+		ret = kstrtol(buf, 10, &long_val);
+		if (ret < 0)
+			return ret;
+
+		ret = long_val;
+	}
 
-	value.intval = long_val;
+	value.intval = ret;
 
 	ret = power_supply_set_property(psy, off, &value);
 	if (ret < 0)
@@ -196,6 +242,7 @@ static struct device_attribute power_supply_attrs[] = {
 	POWER_SUPPLY_ATTR(time_to_full_avg),
 	POWER_SUPPLY_ATTR(type),
 	POWER_SUPPLY_ATTR(scope),
+	POWER_SUPPLY_ATTR(precharge_current),
 	POWER_SUPPLY_ATTR(charge_term_current),
 	POWER_SUPPLY_ATTR(calibrate),
 	/* Properties of type `const char *' */
diff --git a/drivers/power/supply/sbs-battery.c b/drivers/power/supply/sbs-battery.c
index e3a114e60f1a..f7059459f0fb 100644
--- a/drivers/power/supply/sbs-battery.c
+++ b/drivers/power/supply/sbs-battery.c
@@ -171,6 +171,7 @@ struct sbs_info {
 	u32				i2c_retry_count;
 	u32				poll_retry_count;
 	struct delayed_work		work;
+	struct mutex			mode_lock;
 };
 
 static char model_name[I2C_SMBUS_BLOCK_MAX + 1];
@@ -199,7 +200,7 @@ static int sbs_read_word_data(struct i2c_client *client, u8 address)
 		return ret;
 	}
 
-	return le16_to_cpu(ret);
+	return ret;
 }
 
 static int sbs_read_string_data(struct i2c_client *client, u8 address,
@@ -265,7 +266,7 @@ static int sbs_read_string_data(struct i2c_client *client, u8 address,
 	memcpy(values, block_buffer + 1, block_length);
 	values[block_length] = '\0';
 
-	return le16_to_cpu(ret);
+	return ret;
 }
 
 static int sbs_write_word_data(struct i2c_client *client, u8 address,
@@ -278,8 +279,7 @@ static int sbs_write_word_data(struct i2c_client *client, u8 address,
 	retries = chip->i2c_retry_count;
 
 	while (retries > 0) {
-		ret = i2c_smbus_write_word_data(client, address,
-			le16_to_cpu(value));
+		ret = i2c_smbus_write_word_data(client, address, value);
 		if (ret >= 0)
 			break;
 		retries--;
@@ -438,6 +438,11 @@ static int sbs_get_battery_property(struct i2c_client *client,
 	} else {
 		if (psp == POWER_SUPPLY_PROP_STATUS)
 			val->intval = POWER_SUPPLY_STATUS_UNKNOWN;
+		else if (psp == POWER_SUPPLY_PROP_CAPACITY)
+			/* sbs spec says that this can be >100 %
+			 * even if max value is 100 %
+			 */
+			val->intval = min(ret, 100);
 		else
 			val->intval = 0;
 	}
@@ -548,12 +553,7 @@ static int sbs_get_battery_capacity(struct i2c_client *client,
 	if (ret < 0)
 		return ret;
 
-	if (psp == POWER_SUPPLY_PROP_CAPACITY) {
-		/* sbs spec says that this can be >100 %
-		* even if max value is 100 % */
-		val->intval = min(ret, 100);
-	} else
-		val->intval = ret;
+	val->intval = ret;
 
 	ret = sbs_set_battery_mode(client, mode);
 	if (ret < 0)
@@ -618,12 +618,17 @@ static int sbs_get_property(struct power_supply *psy,
 	case POWER_SUPPLY_PROP_CHARGE_NOW:
 	case POWER_SUPPLY_PROP_CHARGE_FULL:
 	case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
-	case POWER_SUPPLY_PROP_CAPACITY:
 		ret = sbs_get_property_index(client, psp);
 		if (ret < 0)
 			break;
 
+		/* sbs_get_battery_capacity() will change the battery mode
+		 * temporarily to read the requested attribute. Ensure we stay
+		 * in the desired mode for the duration of the attribute read.
+		 */
+		mutex_lock(&chip->mode_lock);
 		ret = sbs_get_battery_capacity(client, ret, psp, val);
+		mutex_unlock(&chip->mode_lock);
 		break;
 
 	case POWER_SUPPLY_PROP_SERIAL_NUMBER:
@@ -640,6 +645,7 @@ static int sbs_get_property(struct power_supply *psy,
 	case POWER_SUPPLY_PROP_TIME_TO_FULL_AVG:
 	case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
 	case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
+	case POWER_SUPPLY_PROP_CAPACITY:
 		ret = sbs_get_property_index(client, psp);
 		if (ret < 0)
 			break;
@@ -808,6 +814,7 @@ static int sbs_probe(struct i2c_client *client,
 	psy_cfg.of_node = client->dev.of_node;
 	psy_cfg.drv_data = chip;
 	chip->last_state = POWER_SUPPLY_STATUS_UNKNOWN;
+	mutex_init(&chip->mode_lock);
 
 	/* use pdata if available, fall back to DT properties,
 	 * or hardcoded defaults if not
diff --git a/drivers/power/supply/twl4030_charger.c b/drivers/power/supply/twl4030_charger.c
index 2f82d0e9ec1b..3de802f169a1 100644
--- a/drivers/power/supply/twl4030_charger.c
+++ b/drivers/power/supply/twl4030_charger.c
@@ -153,7 +153,7 @@ struct twl4030_bci {
 };
 
 /* strings for 'usb_mode' values */
-static char *modes[] = { "off", "auto", "continuous" };
+static const char *modes[] = { "off", "auto", "continuous" };
 
 /*
  * clear and set bits on an given register on a given module
@@ -624,63 +624,6 @@ static irqreturn_t twl4030_bci_interrupt(int irq, void *arg)
 	return IRQ_HANDLED;
 }
 
-/*
- * Provide "max_current" attribute in sysfs.
- */
-static ssize_t
-twl4030_bci_max_current_store(struct device *dev, struct device_attribute *attr,
-	const char *buf, size_t n)
-{
-	struct twl4030_bci *bci = dev_get_drvdata(dev->parent);
-	int cur = 0;
-	int status = 0;
-	status = kstrtoint(buf, 10, &cur);
-	if (status)
-		return status;
-	if (cur < 0)
-		return -EINVAL;
-	if (dev == &bci->ac->dev)
-		bci->ac_cur = cur;
-	else
-		bci->usb_cur_target = cur;
-
-	twl4030_charger_update_current(bci);
-	return n;
-}
-
-/*
- * sysfs max_current show
- */
-static ssize_t twl4030_bci_max_current_show(struct device *dev,
-	struct device_attribute *attr, char *buf)
-{
-	int status = 0;
-	int cur = -1;
-	u8 bcictl1;
-	struct twl4030_bci *bci = dev_get_drvdata(dev->parent);
-
-	if (dev == &bci->ac->dev) {
-		if (!bci->ac_is_active)
-			cur = bci->ac_cur;
-	} else {
-		if (bci->ac_is_active)
-			cur = bci->usb_cur_target;
-	}
-	if (cur < 0) {
-		cur = twl4030bci_read_adc_val(TWL4030_BCIIREF1);
-		if (cur < 0)
-			return cur;
-		status = twl4030_bci_read(TWL4030_BCICTL1, &bcictl1);
-		if (status < 0)
-			return status;
-		cur = regval2ua(cur, bcictl1 & TWL4030_CGAIN);
-	}
-	return scnprintf(buf, PAGE_SIZE, "%u\n", cur);
-}
-
-static DEVICE_ATTR(max_current, 0644, twl4030_bci_max_current_show,
-			twl4030_bci_max_current_store);
-
 static void twl4030_bci_usb_work(struct work_struct *data)
 {
 	struct twl4030_bci *bci = container_of(data, struct twl4030_bci, work);
@@ -726,14 +669,10 @@ twl4030_bci_mode_store(struct device *dev, struct device_attribute *attr,
 	int mode;
 	int status;
 
-	if (sysfs_streq(buf, modes[0]))
-		mode = 0;
-	else if (sysfs_streq(buf, modes[1]))
-		mode = 1;
-	else if (sysfs_streq(buf, modes[2]))
-		mode = 2;
-	else
-		return -EINVAL;
+	mode = sysfs_match_string(modes, buf);
+	if (mode < 0)
+		return mode;
+
 	if (dev == &bci->ac->dev) {
 		if (mode == 2)
 			return -EINVAL;
@@ -1041,6 +980,12 @@ static int twl4030_bci_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, bci);
 
+	bci->channel_vac = devm_iio_channel_get(&pdev->dev, "vac");
+	if (IS_ERR(bci->channel_vac)) {
+		bci->channel_vac = NULL;
+		dev_warn(&pdev->dev, "could not request vac iio channel");
+	}
+
 	bci->ac = devm_power_supply_register(&pdev->dev, &twl4030_bci_ac_desc,
 					     NULL);
 	if (IS_ERR(bci->ac)) {
@@ -1074,12 +1019,6 @@ static int twl4030_bci_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	bci->channel_vac = iio_channel_get(&pdev->dev, "vac");
-	if (IS_ERR(bci->channel_vac)) {
-		bci->channel_vac = NULL;
-		dev_warn(&pdev->dev, "could not request vac iio channel");
-	}
-
 	INIT_WORK(&bci->work, twl4030_bci_usb_work);
 	INIT_DELAYED_WORK(&bci->current_worker, twl4030_current_worker);
 
@@ -1101,7 +1040,7 @@ static int twl4030_bci_probe(struct platform_device *pdev)
 			       TWL4030_INTERRUPTS_BCIIMR1A);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to unmask interrupts: %d\n", ret);
-		goto fail;
+		return ret;
 	}
 
 	reg = ~(u32)(TWL4030_VBATOV | TWL4030_VBUSOV | TWL4030_ACCHGOV);
@@ -1111,14 +1050,10 @@ static int twl4030_bci_probe(struct platform_device *pdev)
 		dev_warn(&pdev->dev, "failed to unmask interrupts: %d\n", ret);
 
 	twl4030_charger_update_current(bci);
-	if (device_create_file(&bci->usb->dev, &dev_attr_max_current))
-		dev_warn(&pdev->dev, "could not create sysfs file\n");
 	if (device_create_file(&bci->usb->dev, &dev_attr_mode))
 		dev_warn(&pdev->dev, "could not create sysfs file\n");
 	if (device_create_file(&bci->ac->dev, &dev_attr_mode))
 		dev_warn(&pdev->dev, "could not create sysfs file\n");
-	if (device_create_file(&bci->ac->dev, &dev_attr_max_current))
-		dev_warn(&pdev->dev, "could not create sysfs file\n");
 
 	twl4030_charger_enable_ac(bci, true);
 	if (!IS_ERR_OR_NULL(bci->transceiver))
@@ -1134,10 +1069,6 @@ static int twl4030_bci_probe(struct platform_device *pdev)
 		twl4030_charger_enable_backup(0, 0);
 
 	return 0;
-fail:
-	iio_channel_release(bci->channel_vac);
-
-	return ret;
 }
 
 static int twl4030_bci_remove(struct platform_device *pdev)
@@ -1148,11 +1079,7 @@ static int twl4030_bci_remove(struct platform_device *pdev)
 	twl4030_charger_enable_usb(bci, false);
 	twl4030_charger_enable_backup(0, 0);
 
-	iio_channel_release(bci->channel_vac);
-
-	device_remove_file(&bci->usb->dev, &dev_attr_max_current);
 	device_remove_file(&bci->usb->dev, &dev_attr_mode);
-	device_remove_file(&bci->ac->dev, &dev_attr_max_current);
 	device_remove_file(&bci->ac->dev, &dev_attr_mode);
 	/* mask interrupts */
 	twl_i2c_write_u8(TWL4030_MODULE_INTERRUPTS, 0xff,
diff --git a/drivers/powercap/intel_rapl.c b/drivers/powercap/intel_rapl.c
index 9ddad0815ba9..d1694f1def72 100644
--- a/drivers/powercap/intel_rapl.c
+++ b/drivers/powercap/intel_rapl.c
@@ -874,7 +874,9 @@ static int rapl_write_data_raw(struct rapl_domain *rd,
 
 	cpu = rd->rp->lead_cpu;
 	bits = rapl_unit_xlate(rd, rp->unit, value, 1);
-	bits |= bits << rp->shift;
+	bits <<= rp->shift;
+	bits &= rp->mask;
+
 	memset(&ma, 0, sizeof(ma));
 
 	ma.msr_no = rd->msrs[rp->id];
diff --git a/drivers/pps/Kconfig b/drivers/pps/Kconfig
index 564a51abeece..4b29a7182d7b 100644
--- a/drivers/pps/Kconfig
+++ b/drivers/pps/Kconfig
@@ -2,9 +2,7 @@
 # PPS support configuration
 #
 
-menu "PPS support"
-
-config PPS
+menuconfig PPS
 	tristate "PPS support"
 	---help---
 	  PPS (Pulse Per Second) is a special pulse provided by some GPS
@@ -20,10 +18,10 @@ config PPS
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called pps_core.ko.
-if PPS
 
 config PPS_DEBUG
 	bool "PPS debugging messages"
+	depends on PPS
 	help
 	  Say Y here if you want the PPS support to produce a bunch of debug
 	  messages to the system log.  Select this if you are having a
@@ -31,17 +29,13 @@ config PPS_DEBUG
 
 config NTP_PPS
 	bool "PPS kernel consumer support"
-	depends on !NO_HZ_COMMON
+	depends on PPS && !NO_HZ_COMMON
 	help
 	  This option adds support for direct in-kernel time
 	  synchronization using an external PPS signal.
 
 	  It doesn't work on tickless systems at the moment.
 
-endif
-
 source drivers/pps/clients/Kconfig
 
 source drivers/pps/generators/Kconfig
-
-endmenu
diff --git a/drivers/pps/clients/Kconfig b/drivers/pps/clients/Kconfig
index 0c9f2805d076..efec021ce662 100644
--- a/drivers/pps/clients/Kconfig
+++ b/drivers/pps/clients/Kconfig
@@ -2,12 +2,12 @@
 # PPS clients configuration
 #
 
-if PPS
-
 comment "PPS clients support"
+	depends on PPS
 
 config PPS_CLIENT_KTIMER
 	tristate "Kernel timer client (Testing client, use for debug)"
+	depends on PPS
 	help
 	  If you say yes here you get support for a PPS debugging client
 	  which uses a kernel timer to generate the PPS signal.
@@ -37,5 +37,3 @@ config PPS_CLIENT_GPIO
 	  GPIO. To be useful you must also register a platform device
 	  specifying the GPIO pin and other options, usually in your board
 	  setup.
-
-endif
diff --git a/drivers/pps/generators/Kconfig b/drivers/pps/generators/Kconfig
index e4c4f3dc0728..86b59378e71f 100644
--- a/drivers/pps/generators/Kconfig
+++ b/drivers/pps/generators/Kconfig
@@ -3,10 +3,11 @@
 #
 
 comment "PPS generators support"
+	depends on PPS
 
 config PPS_GENERATOR_PARPORT
 	tristate "Parallel port PPS signal generator"
-	depends on PARPORT && BROKEN
+	depends on PPS && PARPORT && BROKEN
 	help
 	  If you say yes here you get support for a PPS signal generator which
 	  utilizes STROBE pin of a parallel port to send PPS signals. It uses
diff --git a/drivers/ptp/Kconfig b/drivers/ptp/Kconfig
index 384f661a6496..a21ad10d613c 100644
--- a/drivers/ptp/Kconfig
+++ b/drivers/ptp/Kconfig
@@ -25,6 +25,22 @@ config PTP_1588_CLOCK
 	  To compile this driver as a module, choose M here: the module
 	  will be called ptp.
 
+config PTP_1588_CLOCK_DTE
+	tristate "Broadcom DTE as PTP clock"
+	depends on PTP_1588_CLOCK
+	depends on NET && HAS_IOMEM
+	depends on ARCH_BCM_MOBILE || (ARCH_BCM_IPROC && !(ARCH_BCM_NSP || ARCH_BCM_5301X)) || COMPILE_TEST
+	default y
+	help
+	  This driver adds support for using the Digital timing engine
+	  (DTE) in the Broadcom SoC's as a PTP clock.
+
+	  The clock can be used in both wired and wireless networks
+	  for PTP purposes.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ptp_dte.
+
 config PTP_1588_CLOCK_GIANFAR
 	tristate "Freescale eTSEC as PTP clock"
 	depends on GIANFAR
diff --git a/drivers/ptp/Makefile b/drivers/ptp/Makefile
index 530736161a8b..d1f2fb19c980 100644
--- a/drivers/ptp/Makefile
+++ b/drivers/ptp/Makefile
@@ -4,6 +4,7 @@
 
 ptp-y					:= ptp_clock.o ptp_chardev.o ptp_sysfs.o
 obj-$(CONFIG_PTP_1588_CLOCK)		+= ptp.o
+obj-$(CONFIG_PTP_1588_CLOCK_DTE)	+= ptp_dte.o
 obj-$(CONFIG_PTP_1588_CLOCK_IXP46X)	+= ptp_ixp46x.o
 obj-$(CONFIG_PTP_1588_CLOCK_PCH)	+= ptp_pch.o
 obj-$(CONFIG_PTP_1588_CLOCK_KVM)	+= ptp_kvm.o
diff --git a/drivers/ptp/ptp_dte.c b/drivers/ptp/ptp_dte.c
new file mode 100644
index 000000000000..00145a3f1e70
--- /dev/null
+++ b/drivers/ptp/ptp_dte.c
@@ -0,0 +1,353 @@
+/*
+ * Copyright 2017 Broadcom
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation 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/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/types.h>
+
+#define DTE_NCO_LOW_TIME_REG	0x00
+#define DTE_NCO_TIME_REG	0x04
+#define DTE_NCO_OVERFLOW_REG	0x08
+#define DTE_NCO_INC_REG		0x0c
+
+#define DTE_NCO_SUM2_MASK	0xffffffff
+#define DTE_NCO_SUM2_SHIFT	4ULL
+
+#define DTE_NCO_SUM3_MASK	0xff
+#define DTE_NCO_SUM3_SHIFT	36ULL
+#define DTE_NCO_SUM3_WR_SHIFT	8
+
+#define DTE_NCO_TS_WRAP_MASK	0xfff
+#define DTE_NCO_TS_WRAP_LSHIFT	32
+
+#define DTE_NCO_INC_DEFAULT	0x80000000
+#define DTE_NUM_REGS_TO_RESTORE	4
+
+/* Full wrap around is 44bits in ns (~4.887 hrs) */
+#define DTE_WRAP_AROUND_NSEC_SHIFT 44
+
+/* 44 bits NCO */
+#define DTE_NCO_MAX_NS	0xFFFFFFFFFFF
+
+/* 125MHz with 3.29 reg cfg */
+#define DTE_PPB_ADJ(ppb) (u32)(div64_u64((((u64)abs(ppb) * BIT(28)) +\
+				      62500000ULL), 125000000ULL))
+
+/* ptp dte priv structure */
+struct ptp_dte {
+	void __iomem *regs;
+	struct ptp_clock *ptp_clk;
+	struct ptp_clock_info caps;
+	struct device *dev;
+	u32 ts_ovf_last;
+	u32 ts_wrap_cnt;
+	spinlock_t lock;
+	u32 reg_val[DTE_NUM_REGS_TO_RESTORE];
+};
+
+static void dte_write_nco(void __iomem *regs, s64 ns)
+{
+	u32 sum2, sum3;
+
+	sum2 = (u32)((ns >> DTE_NCO_SUM2_SHIFT) & DTE_NCO_SUM2_MASK);
+	/* compensate for ignoring sum1 */
+	if (sum2 != DTE_NCO_SUM2_MASK)
+		sum2++;
+
+	/* to write sum3, bits [15:8] needs to be written */
+	sum3 = (u32)(((ns >> DTE_NCO_SUM3_SHIFT) & DTE_NCO_SUM3_MASK) <<
+		     DTE_NCO_SUM3_WR_SHIFT);
+
+	writel(0, (regs + DTE_NCO_LOW_TIME_REG));
+	writel(sum2, (regs + DTE_NCO_TIME_REG));
+	writel(sum3, (regs + DTE_NCO_OVERFLOW_REG));
+}
+
+static s64 dte_read_nco(void __iomem *regs)
+{
+	u32 sum2, sum3;
+	s64 ns;
+
+	/*
+	 * ignoring sum1 (4 bits) gives a 16ns resolution, which
+	 * works due to the async register read.
+	 */
+	sum3 = readl(regs + DTE_NCO_OVERFLOW_REG) & DTE_NCO_SUM3_MASK;
+	sum2 = readl(regs + DTE_NCO_TIME_REG);
+	ns = ((s64)sum3 << DTE_NCO_SUM3_SHIFT) |
+		 ((s64)sum2 << DTE_NCO_SUM2_SHIFT);
+
+	return ns;
+}
+
+static void dte_write_nco_delta(struct ptp_dte *ptp_dte, s64 delta)
+{
+	s64 ns;
+
+	ns = dte_read_nco(ptp_dte->regs);
+
+	/* handle wraparound conditions */
+	if ((delta < 0) && (abs(delta) > ns)) {
+		if (ptp_dte->ts_wrap_cnt) {
+			ns += DTE_NCO_MAX_NS + delta;
+			ptp_dte->ts_wrap_cnt--;
+		} else {
+			ns = 0;
+		}
+	} else {
+		ns += delta;
+		if (ns > DTE_NCO_MAX_NS) {
+			ptp_dte->ts_wrap_cnt++;
+			ns -= DTE_NCO_MAX_NS;
+		}
+	}
+
+	dte_write_nco(ptp_dte->regs, ns);
+
+	ptp_dte->ts_ovf_last = (ns >> DTE_NCO_TS_WRAP_LSHIFT) &
+			DTE_NCO_TS_WRAP_MASK;
+}
+
+static s64 dte_read_nco_with_ovf(struct ptp_dte *ptp_dte)
+{
+	u32 ts_ovf;
+	s64 ns = 0;
+
+	ns = dte_read_nco(ptp_dte->regs);
+
+	/*Timestamp overflow: 8 LSB bits of sum3, 4 MSB bits of sum2 */
+	ts_ovf = (ns >> DTE_NCO_TS_WRAP_LSHIFT) & DTE_NCO_TS_WRAP_MASK;
+
+	/* Check for wrap around */
+	if (ts_ovf < ptp_dte->ts_ovf_last)
+		ptp_dte->ts_wrap_cnt++;
+
+	ptp_dte->ts_ovf_last = ts_ovf;
+
+	/* adjust for wraparounds */
+	ns += (s64)(BIT_ULL(DTE_WRAP_AROUND_NSEC_SHIFT) * ptp_dte->ts_wrap_cnt);
+
+	return ns;
+}
+
+static int ptp_dte_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+{
+	u32 nco_incr;
+	unsigned long flags;
+	struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);
+
+	if (abs(ppb) > ptp_dte->caps.max_adj) {
+		dev_err(ptp_dte->dev, "ppb adj too big\n");
+		return -EINVAL;
+	}
+
+	if (ppb < 0)
+		nco_incr = DTE_NCO_INC_DEFAULT - DTE_PPB_ADJ(ppb);
+	else
+		nco_incr = DTE_NCO_INC_DEFAULT + DTE_PPB_ADJ(ppb);
+
+	spin_lock_irqsave(&ptp_dte->lock, flags);
+	writel(nco_incr, ptp_dte->regs + DTE_NCO_INC_REG);
+	spin_unlock_irqrestore(&ptp_dte->lock, flags);
+
+	return 0;
+}
+
+static int ptp_dte_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	unsigned long flags;
+	struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);
+
+	spin_lock_irqsave(&ptp_dte->lock, flags);
+	dte_write_nco_delta(ptp_dte, delta);
+	spin_unlock_irqrestore(&ptp_dte->lock, flags);
+
+	return 0;
+}
+
+static int ptp_dte_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+	unsigned long flags;
+	struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);
+
+	spin_lock_irqsave(&ptp_dte->lock, flags);
+	*ts = ns_to_timespec64(dte_read_nco_with_ovf(ptp_dte));
+	spin_unlock_irqrestore(&ptp_dte->lock, flags);
+
+	return 0;
+}
+
+static int ptp_dte_settime(struct ptp_clock_info *ptp,
+			     const struct timespec64 *ts)
+{
+	unsigned long flags;
+	struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);
+
+	spin_lock_irqsave(&ptp_dte->lock, flags);
+
+	/* Disable nco increment */
+	writel(0, ptp_dte->regs + DTE_NCO_INC_REG);
+
+	dte_write_nco(ptp_dte->regs, timespec64_to_ns(ts));
+
+	/* reset overflow and wrap counter */
+	ptp_dte->ts_ovf_last = 0;
+	ptp_dte->ts_wrap_cnt = 0;
+
+	/* Enable nco increment */
+	writel(DTE_NCO_INC_DEFAULT, ptp_dte->regs + DTE_NCO_INC_REG);
+
+	spin_unlock_irqrestore(&ptp_dte->lock, flags);
+
+	return 0;
+}
+
+static int ptp_dte_enable(struct ptp_clock_info *ptp,
+			    struct ptp_clock_request *rq, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+static struct ptp_clock_info ptp_dte_caps = {
+	.owner		= THIS_MODULE,
+	.name		= "DTE PTP timer",
+	.max_adj	= 50000000,
+	.n_ext_ts	= 0,
+	.n_pins		= 0,
+	.pps		= 0,
+	.adjfreq	= ptp_dte_adjfreq,
+	.adjtime	= ptp_dte_adjtime,
+	.gettime64	= ptp_dte_gettime,
+	.settime64	= ptp_dte_settime,
+	.enable		= ptp_dte_enable,
+};
+
+static int ptp_dte_probe(struct platform_device *pdev)
+{
+	struct ptp_dte *ptp_dte;
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+
+	ptp_dte = devm_kzalloc(dev, sizeof(struct ptp_dte), GFP_KERNEL);
+	if (!ptp_dte)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	ptp_dte->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(ptp_dte->regs)) {
+		dev_err(dev,
+			"%s: io remap failed\n", __func__);
+		return PTR_ERR(ptp_dte->regs);
+	}
+
+	spin_lock_init(&ptp_dte->lock);
+
+	ptp_dte->dev = dev;
+	ptp_dte->caps = ptp_dte_caps;
+	ptp_dte->ptp_clk = ptp_clock_register(&ptp_dte->caps, &pdev->dev);
+	if (IS_ERR(ptp_dte->ptp_clk)) {
+		dev_err(dev,
+			"%s: Failed to register ptp clock\n", __func__);
+		return PTR_ERR(ptp_dte->ptp_clk);
+	}
+
+	platform_set_drvdata(pdev, ptp_dte);
+
+	dev_info(dev, "ptp clk probe done\n");
+
+	return 0;
+}
+
+static int ptp_dte_remove(struct platform_device *pdev)
+{
+	struct ptp_dte *ptp_dte = platform_get_drvdata(pdev);
+	u8 i;
+
+	ptp_clock_unregister(ptp_dte->ptp_clk);
+
+	for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++)
+		writel(0, ptp_dte->regs + (i * sizeof(u32)));
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int ptp_dte_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct ptp_dte *ptp_dte = platform_get_drvdata(pdev);
+	u8 i;
+
+	for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++) {
+		ptp_dte->reg_val[i] =
+			readl(ptp_dte->regs + (i * sizeof(u32)));
+	}
+
+	/* disable the nco */
+	writel(0, ptp_dte->regs + DTE_NCO_INC_REG);
+
+	return 0;
+}
+
+static int ptp_dte_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct ptp_dte *ptp_dte = platform_get_drvdata(pdev);
+	u8 i;
+
+	for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++) {
+		if ((i * sizeof(u32)) != DTE_NCO_OVERFLOW_REG)
+			writel(ptp_dte->reg_val[i],
+				(ptp_dte->regs + (i * sizeof(u32))));
+		else
+			writel(((ptp_dte->reg_val[i] &
+				DTE_NCO_SUM3_MASK) << DTE_NCO_SUM3_WR_SHIFT),
+				(ptp_dte->regs + (i * sizeof(u32))));
+	}
+
+	return 0;
+}
+
+static const struct dev_pm_ops ptp_dte_pm_ops = {
+	.suspend = ptp_dte_suspend,
+	.resume = ptp_dte_resume
+};
+
+#define PTP_DTE_PM_OPS	(&ptp_dte_pm_ops)
+#else
+#define PTP_DTE_PM_OPS	NULL
+#endif
+
+static const struct of_device_id ptp_dte_of_match[] = {
+	{ .compatible = "brcm,ptp-dte", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ptp_dte_of_match);
+
+static struct platform_driver ptp_dte_driver = {
+	.driver = {
+		.name = "ptp-dte",
+		.pm = PTP_DTE_PM_OPS,
+		.of_match_table = ptp_dte_of_match,
+	},
+	.probe    = ptp_dte_probe,
+	.remove   = ptp_dte_remove,
+};
+module_platform_driver(ptp_dte_driver);
+
+MODULE_AUTHOR("Broadcom");
+MODULE_DESCRIPTION("Broadcom DTE PTP Clock driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pwm/pwm-jz4740.c b/drivers/pwm/pwm-jz4740.c
index 76d13150283f..a75ff3622450 100644
--- a/drivers/pwm/pwm-jz4740.c
+++ b/drivers/pwm/pwm-jz4740.c
@@ -21,22 +21,10 @@
 #include <linux/platform_device.h>
 #include <linux/pwm.h>
 
-#include <asm/mach-jz4740/gpio.h>
 #include <asm/mach-jz4740/timer.h>
 
 #define NUM_PWM 8
 
-static const unsigned int jz4740_pwm_gpio_list[NUM_PWM] = {
-	JZ_GPIO_PWM0,
-	JZ_GPIO_PWM1,
-	JZ_GPIO_PWM2,
-	JZ_GPIO_PWM3,
-	JZ_GPIO_PWM4,
-	JZ_GPIO_PWM5,
-	JZ_GPIO_PWM6,
-	JZ_GPIO_PWM7,
-};
-
 struct jz4740_pwm_chip {
 	struct pwm_chip chip;
 	struct clk *clk;
@@ -49,9 +37,6 @@ static inline struct jz4740_pwm_chip *to_jz4740(struct pwm_chip *chip)
 
 static int jz4740_pwm_request(struct pwm_chip *chip, struct pwm_device *pwm)
 {
-	unsigned int gpio = jz4740_pwm_gpio_list[pwm->hwpwm];
-	int ret;
-
 	/*
 	 * Timers 0 and 1 are used for system tasks, so they are unavailable
 	 * for use as PWMs.
@@ -59,15 +44,6 @@ static int jz4740_pwm_request(struct pwm_chip *chip, struct pwm_device *pwm)
 	if (pwm->hwpwm < 2)
 		return -EBUSY;
 
-	ret = gpio_request(gpio, pwm->label);
-	if (ret) {
-		dev_err(chip->dev, "Failed to request GPIO#%u for PWM: %d\n",
-			gpio, ret);
-		return ret;
-	}
-
-	jz_gpio_set_function(gpio, JZ_GPIO_FUNC_PWM);
-
 	jz4740_timer_start(pwm->hwpwm);
 
 	return 0;
@@ -75,13 +51,8 @@ static int jz4740_pwm_request(struct pwm_chip *chip, struct pwm_device *pwm)
 
 static void jz4740_pwm_free(struct pwm_chip *chip, struct pwm_device *pwm)
 {
-	unsigned int gpio = jz4740_pwm_gpio_list[pwm->hwpwm];
-
 	jz4740_timer_set_ctrl(pwm->hwpwm, 0);
 
-	jz_gpio_set_function(gpio, JZ_GPIO_FUNC_NONE);
-	gpio_free(gpio);
-
 	jz4740_timer_stop(pwm->hwpwm);
 }
 
diff --git a/drivers/ras/cec.c b/drivers/ras/cec.c
index 6aab46d91d33..d0e5d6ee882c 100644
--- a/drivers/ras/cec.c
+++ b/drivers/ras/cec.c
@@ -481,7 +481,7 @@ static int __init create_debugfs_nodes(void)
 
 	count = debugfs_create_file("count_threshold", S_IRUSR | S_IWUSR, d,
 				    &count_threshold, &count_threshold_ops);
-	if (!decay) {
+	if (!count) {
 		pr_warn("Error creating count_threshold debugfs node!\n");
 		goto err;
 	}
diff --git a/drivers/ras/ras.c b/drivers/ras/ras.c
index 94f8038864b4..5429d3795732 100644
--- a/drivers/ras/ras.c
+++ b/drivers/ras/ras.c
@@ -7,11 +7,24 @@
 
 #include <linux/init.h>
 #include <linux/ras.h>
+#include <linux/uuid.h>
 
 #define CREATE_TRACE_POINTS
 #define TRACE_INCLUDE_PATH ../../include/ras
 #include <ras/ras_event.h>
 
+void log_non_standard_event(const uuid_le *sec_type, const uuid_le *fru_id,
+			    const char *fru_text, const u8 sev, const u8 *err,
+			    const u32 len)
+{
+	trace_non_standard_event(sec_type, fru_id, fru_text, sev, err, len);
+}
+
+void log_arm_hw_error(struct cper_sec_proc_arm *err)
+{
+	trace_arm_event(err);
+}
+
 static int __init ras_init(void)
 {
 	int rc = 0;
@@ -27,9 +40,10 @@ subsys_initcall(ras_init);
 EXPORT_TRACEPOINT_SYMBOL_GPL(extlog_mem_event);
 #endif
 EXPORT_TRACEPOINT_SYMBOL_GPL(mc_event);
+EXPORT_TRACEPOINT_SYMBOL_GPL(non_standard_event);
+EXPORT_TRACEPOINT_SYMBOL_GPL(arm_event);
 
-
-int __init parse_ras_param(char *str)
+static int __init parse_ras_param(char *str)
 {
 #ifdef CONFIG_RAS_CEC
 	parse_cec_param(str);
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index 48db87d6dfef..99b9362331b5 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -214,11 +214,11 @@ config REGULATOR_DA9055
 	  will be called da9055-regulator.
 
 config REGULATOR_DA9062
-	tristate "Dialog Semiconductor DA9062 regulators"
+	tristate "Dialog Semiconductor DA9061/62 regulators"
 	depends on MFD_DA9062
 	help
 	  Say y here to support the BUCKs and LDOs regulators found on
-	  DA9062 PMICs.
+	  DA9061 and DA9062 PMICs.
 
 	  This driver can also be built as a module. If so, the module
 	  will be called da9062-regulator.
@@ -296,6 +296,16 @@ config REGULATOR_HI6421
 	  21 general purpose LDOs, 3 dedicated LDOs, and 5 BUCKs. All
 	  of them come with support to either ECO (idle) or sleep mode.
 
+config REGULATOR_HI6421V530
+	tristate "HiSilicon Hi6421v530 PMIC voltage regulator support"
+	depends on MFD_HI6421_PMIC && OF
+	help
+	  This driver provides support for the voltage regulators on
+	  HiSilicon Hi6421v530 PMU / Codec IC.
+	  Hi6421v530 is a multi-function device which, on regulator part,
+	  provides 5 general purpose LDOs, and all of them come with support
+	  to either ECO (idle) or sleep mode.
+
 config REGULATOR_HI655X
 	tristate "Hisilicon HI655X PMIC regulators support"
 	depends on ARCH_HISI || COMPILE_TEST
@@ -365,6 +375,14 @@ config REGULATOR_LP8755
 	  chip contains six step-down DC/DC converters which can support
 	  9 mode multiphase configuration.
 
+config REGULATOR_LP87565
+	tristate "TI LP87565 Power regulators"
+	depends on MFD_TI_LP87565 && OF
+	help
+	  This driver supports LP87565 voltage regulator chips. LP87565
+	  provides four step-down converters. It supports software based
+	  voltage control for different voltage domains
+
 config REGULATOR_LP8788
 	tristate "TI LP8788 Power Regulators"
 	depends on MFD_LP8788
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index dc3503fb3e30..95b1e86ae692 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -38,6 +38,7 @@ obj-$(CONFIG_REGULATOR_DB8500_PRCMU) += db8500-prcmu.o
 obj-$(CONFIG_REGULATOR_FAN53555) += fan53555.o
 obj-$(CONFIG_REGULATOR_GPIO) += gpio-regulator.o
 obj-$(CONFIG_REGULATOR_HI6421) += hi6421-regulator.o
+obj-$(CONFIG_REGULATOR_HI6421V530) += hi6421v530-regulator.o
 obj-$(CONFIG_REGULATOR_HI655X) += hi655x-regulator.o
 obj-$(CONFIG_REGULATOR_ISL6271A) += isl6271a-regulator.o
 obj-$(CONFIG_REGULATOR_ISL9305) += isl9305.o
@@ -46,6 +47,7 @@ obj-$(CONFIG_REGULATOR_LP3971) += lp3971.o
 obj-$(CONFIG_REGULATOR_LP3972) += lp3972.o
 obj-$(CONFIG_REGULATOR_LP872X) += lp872x.o
 obj-$(CONFIG_REGULATOR_LP873X) += lp873x-regulator.o
+obj-$(CONFIG_REGULATOR_LP87565) += lp87565-regulator.o
 obj-$(CONFIG_REGULATOR_LP8788) += lp8788-buck.o
 obj-$(CONFIG_REGULATOR_LP8788) += lp8788-ldo.o
 obj-$(CONFIG_REGULATOR_LP8755) += lp8755.o
diff --git a/drivers/regulator/axp20x-regulator.c b/drivers/regulator/axp20x-regulator.c
index 0b9d4e3e52c7..e2608fe770b9 100644
--- a/drivers/regulator/axp20x-regulator.c
+++ b/drivers/regulator/axp20x-regulator.c
@@ -244,6 +244,82 @@ static const struct regulator_desc axp22x_drivevbus_regulator = {
 	.ops		= &axp20x_ops_sw,
 };
 
+static const struct regulator_linear_range axp803_dcdc234_ranges[] = {
+	REGULATOR_LINEAR_RANGE(500000, 0x0, 0x46, 10000),
+	REGULATOR_LINEAR_RANGE(1220000, 0x47, 0x4b, 20000),
+};
+
+static const struct regulator_linear_range axp803_dcdc5_ranges[] = {
+	REGULATOR_LINEAR_RANGE(800000, 0x0, 0x20, 10000),
+	REGULATOR_LINEAR_RANGE(1140000, 0x21, 0x44, 20000),
+};
+
+static const struct regulator_linear_range axp803_dcdc6_ranges[] = {
+	REGULATOR_LINEAR_RANGE(600000, 0x0, 0x32, 10000),
+	REGULATOR_LINEAR_RANGE(1120000, 0x33, 0x47, 20000),
+};
+
+/* AXP806's CLDO2 and AXP809's DLDO1 shares the same range */
+static const struct regulator_linear_range axp803_dldo2_ranges[] = {
+	REGULATOR_LINEAR_RANGE(700000, 0x0, 0x1a, 100000),
+	REGULATOR_LINEAR_RANGE(3400000, 0x1b, 0x1f, 200000),
+};
+
+static const struct regulator_desc axp803_regulators[] = {
+	AXP_DESC(AXP803, DCDC1, "dcdc1", "vin1", 1600, 3400, 100,
+		 AXP803_DCDC1_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL1, BIT(0)),
+	AXP_DESC_RANGES(AXP803, DCDC2, "dcdc2", "vin2", axp803_dcdc234_ranges,
+			76, AXP803_DCDC2_V_OUT, 0x7f, AXP22X_PWR_OUT_CTRL1,
+			BIT(1)),
+	AXP_DESC_RANGES(AXP803, DCDC3, "dcdc3", "vin3", axp803_dcdc234_ranges,
+			76, AXP803_DCDC3_V_OUT, 0x7f, AXP22X_PWR_OUT_CTRL1,
+			BIT(2)),
+	AXP_DESC_RANGES(AXP803, DCDC4, "dcdc4", "vin4", axp803_dcdc234_ranges,
+			76, AXP803_DCDC4_V_OUT, 0x7f, AXP22X_PWR_OUT_CTRL1,
+			BIT(3)),
+	AXP_DESC_RANGES(AXP803, DCDC5, "dcdc5", "vin5", axp803_dcdc5_ranges,
+			68, AXP803_DCDC5_V_OUT, 0x7f, AXP22X_PWR_OUT_CTRL1,
+			BIT(4)),
+	AXP_DESC_RANGES(AXP803, DCDC6, "dcdc6", "vin6", axp803_dcdc6_ranges,
+			72, AXP803_DCDC6_V_OUT, 0x7f, AXP22X_PWR_OUT_CTRL1,
+			BIT(5)),
+	/* secondary switchable output of DCDC1 */
+	AXP_DESC_SW(AXP803, DC1SW, "dc1sw", NULL, AXP22X_PWR_OUT_CTRL2,
+		    BIT(7)),
+	AXP_DESC(AXP803, ALDO1, "aldo1", "aldoin", 700, 3300, 100,
+		 AXP22X_ALDO1_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL3, BIT(5)),
+	AXP_DESC(AXP803, ALDO2, "aldo2", "aldoin", 700, 3300, 100,
+		 AXP22X_ALDO2_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL3, BIT(6)),
+	AXP_DESC(AXP803, ALDO3, "aldo3", "aldoin", 700, 3300, 100,
+		 AXP22X_ALDO3_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL3, BIT(7)),
+	AXP_DESC(AXP803, DLDO1, "dldo1", "dldoin", 700, 3300, 100,
+		 AXP22X_DLDO1_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(3)),
+	AXP_DESC_RANGES(AXP803, DLDO2, "dldo2", "dldoin", axp803_dldo2_ranges,
+			32, AXP22X_DLDO2_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2,
+			BIT(4)),
+	AXP_DESC(AXP803, DLDO3, "dldo3", "dldoin", 700, 3300, 100,
+		 AXP22X_DLDO3_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(5)),
+	AXP_DESC(AXP803, DLDO4, "dldo4", "dldoin", 700, 3300, 100,
+		 AXP22X_DLDO4_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(6)),
+	AXP_DESC(AXP803, ELDO1, "eldo1", "eldoin", 700, 1900, 50,
+		 AXP22X_ELDO1_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(0)),
+	AXP_DESC(AXP803, ELDO2, "eldo2", "eldoin", 700, 1900, 50,
+		 AXP22X_ELDO2_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(1)),
+	AXP_DESC(AXP803, ELDO3, "eldo3", "eldoin", 700, 1900, 50,
+		 AXP22X_ELDO3_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(2)),
+	AXP_DESC(AXP803, FLDO1, "fldo1", "fldoin", 700, 1450, 50,
+		 AXP803_FLDO1_V_OUT, 0x0f, AXP22X_PWR_OUT_CTRL3, BIT(2)),
+	AXP_DESC(AXP803, FLDO2, "fldo2", "fldoin", 700, 1450, 50,
+		 AXP803_FLDO2_V_OUT, 0x0f, AXP22X_PWR_OUT_CTRL3, BIT(3)),
+	AXP_DESC_IO(AXP803, LDO_IO0, "ldo-io0", "ips", 700, 3300, 100,
+		    AXP22X_LDO_IO0_V_OUT, 0x1f, AXP20X_GPIO0_CTRL, 0x07,
+		    AXP22X_IO_ENABLED, AXP22X_IO_DISABLED),
+	AXP_DESC_IO(AXP803, LDO_IO1, "ldo-io1", "ips", 700, 3300, 100,
+		    AXP22X_LDO_IO1_V_OUT, 0x1f, AXP20X_GPIO1_CTRL, 0x07,
+		    AXP22X_IO_ENABLED, AXP22X_IO_DISABLED),
+	AXP_DESC_FIXED(AXP803, RTC_LDO, "rtc-ldo", "ips", 3000),
+};
+
 static const struct regulator_linear_range axp806_dcdca_ranges[] = {
 	REGULATOR_LINEAR_RANGE(600000, 0x0, 0x32, 10000),
 	REGULATOR_LINEAR_RANGE(1120000, 0x33, 0x47, 20000),
@@ -254,11 +330,6 @@ static const struct regulator_linear_range axp806_dcdcd_ranges[] = {
 	REGULATOR_LINEAR_RANGE(1600000, 0x2e, 0x3f, 100000),
 };
 
-static const struct regulator_linear_range axp806_cldo2_ranges[] = {
-	REGULATOR_LINEAR_RANGE(700000, 0x0, 0x1a, 100000),
-	REGULATOR_LINEAR_RANGE(3400000, 0x1b, 0x1f, 200000),
-};
-
 static const struct regulator_desc axp806_regulators[] = {
 	AXP_DESC_RANGES(AXP806, DCDCA, "dcdca", "vina", axp806_dcdca_ranges,
 			72, AXP806_DCDCA_V_CTRL, 0x7f, AXP806_PWR_OUT_CTRL1,
@@ -289,7 +360,7 @@ static const struct regulator_desc axp806_regulators[] = {
 		 AXP806_BLDO4_V_CTRL, 0x0f, AXP806_PWR_OUT_CTRL2, BIT(3)),
 	AXP_DESC(AXP806, CLDO1, "cldo1", "cldoin", 700, 3300, 100,
 		 AXP806_CLDO1_V_CTRL, 0x1f, AXP806_PWR_OUT_CTRL2, BIT(4)),
-	AXP_DESC_RANGES(AXP806, CLDO2, "cldo2", "cldoin", axp806_cldo2_ranges,
+	AXP_DESC_RANGES(AXP806, CLDO2, "cldo2", "cldoin", axp803_dldo2_ranges,
 			32, AXP806_CLDO2_V_CTRL, 0x1f, AXP806_PWR_OUT_CTRL2,
 			BIT(5)),
 	AXP_DESC(AXP806, CLDO3, "cldo3", "cldoin", 700, 3300, 100,
@@ -326,7 +397,7 @@ static const struct regulator_desc axp809_regulators[] = {
 		 AXP22X_ALDO2_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL1, BIT(7)),
 	AXP_DESC(AXP809, ALDO3, "aldo3", "aldoin", 700, 3300, 100,
 		 AXP22X_ALDO3_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(5)),
-	AXP_DESC_RANGES(AXP809, DLDO1, "dldo1", "dldoin", axp806_cldo2_ranges,
+	AXP_DESC_RANGES(AXP809, DLDO1, "dldo1", "dldoin", axp803_dldo2_ranges,
 			32, AXP22X_DLDO1_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2,
 			BIT(3)),
 	AXP_DESC(AXP809, DLDO2, "dldo2", "dldoin", 700, 3300, 100,
@@ -369,14 +440,21 @@ static int axp20x_set_dcdc_freq(struct platform_device *pdev, u32 dcdcfreq)
 		def = 1500;
 		step = 75;
 		break;
-	case AXP806_ID:
+	case AXP803_ID:
 		/*
-		 * AXP806 DCDC work frequency setting has the same range and
+		 * AXP803 DCDC work frequency setting has the same range and
 		 * step as AXP22X, but at a different register.
 		 * Fall through to the check below.
 		 * (See include/linux/mfd/axp20x.h)
 		 */
-		reg = AXP806_DCDC_FREQ_CTRL;
+		reg = AXP803_DCDC_FREQ_CTRL;
+	case AXP806_ID:
+		/*
+		 * AXP806 also have DCDC work frequency setting register at a
+		 * different position.
+		 */
+		if (axp20x->variant == AXP806_ID)
+			reg = AXP806_DCDC_FREQ_CTRL;
 	case AXP221_ID:
 	case AXP223_ID:
 	case AXP809_ID:
@@ -475,6 +553,14 @@ static int axp20x_set_dcdc_workmode(struct regulator_dev *rdev, int id, u32 work
 		workmode <<= id - AXP22X_DCDC1;
 		break;
 
+	case AXP803_ID:
+		if (id < AXP803_DCDC1 || id > AXP803_DCDC6)
+			return -EINVAL;
+
+		mask = AXP22X_WORKMODE_DCDCX_MASK(id - AXP803_DCDC1);
+		workmode <<= id - AXP803_DCDC1;
+		break;
+
 	default:
 		/* should not happen */
 		WARN_ON(1);
@@ -492,20 +578,38 @@ static bool axp20x_is_polyphase_slave(struct axp20x_dev *axp20x, int id)
 {
 	u32 reg = 0;
 
-	/* Only AXP806 has poly-phase outputs */
-	if (axp20x->variant != AXP806_ID)
-		return false;
+	/*
+	 * Currently in our supported AXP variants, only AXP803 and AXP806
+	 * have polyphase regulators.
+	 */
+	switch (axp20x->variant) {
+	case AXP803_ID:
+		regmap_read(axp20x->regmap, AXP803_POLYPHASE_CTRL, &reg);
+
+		switch (id) {
+		case AXP803_DCDC3:
+			return !!(reg & BIT(6));
+		case AXP803_DCDC6:
+			return !!(reg & BIT(7));
+		}
+		break;
 
-	regmap_read(axp20x->regmap, AXP806_DCDC_MODE_CTRL2, &reg);
+	case AXP806_ID:
+		regmap_read(axp20x->regmap, AXP806_DCDC_MODE_CTRL2, &reg);
+
+		switch (id) {
+		case AXP806_DCDCB:
+			return (((reg & GENMASK(7, 6)) == BIT(6)) ||
+				((reg & GENMASK(7, 6)) == BIT(7)));
+		case AXP806_DCDCC:
+			return ((reg & GENMASK(7, 6)) == BIT(7));
+		case AXP806_DCDCE:
+			return !!(reg & BIT(5));
+		}
+		break;
 
-	switch (id) {
-	case AXP806_DCDCB:
-		return (((reg & GENMASK(7, 6)) == BIT(6)) ||
-			((reg & GENMASK(7, 6)) == BIT(7)));
-	case AXP806_DCDCC:
-		return ((reg & GENMASK(7, 6)) == BIT(7));
-	case AXP806_DCDCE:
-		return !!(reg & BIT(5));
+	default:
+		return false;
 	}
 
 	return false;
@@ -540,6 +644,10 @@ static int axp20x_regulator_probe(struct platform_device *pdev)
 		drivevbus = of_property_read_bool(pdev->dev.parent->of_node,
 						  "x-powers,drive-vbus-en");
 		break;
+	case AXP803_ID:
+		regulators = axp803_regulators;
+		nregulators = AXP803_REG_ID_MAX;
+		break;
 	case AXP806_ID:
 		regulators = axp806_regulators;
 		nregulators = AXP806_REG_ID_MAX;
@@ -579,6 +687,7 @@ static int axp20x_regulator_probe(struct platform_device *pdev)
 		 * name.
 		 */
 		if ((regulators == axp22x_regulators && i == AXP22X_DC1SW) ||
+		    (regulators == axp803_regulators && i == AXP803_DC1SW) ||
 		    (regulators == axp809_regulators && i == AXP809_DC1SW)) {
 			new_desc = devm_kzalloc(&pdev->dev, sizeof(*desc),
 						GFP_KERNEL);
diff --git a/drivers/regulator/bd9571mwv-regulator.c b/drivers/regulator/bd9571mwv-regulator.c
index 8ba206fec31e..c67a83d53c4c 100644
--- a/drivers/regulator/bd9571mwv-regulator.c
+++ b/drivers/regulator/bd9571mwv-regulator.c
@@ -43,7 +43,7 @@ enum bd9571mwv_regulators { VD09, VD18, VD25, VD33, DVFS };
 		.linear_min_sel		= _lmin,		\
 	}
 
-int bd9571mwv_avs_get_moni_state(struct regulator_dev *rdev)
+static int bd9571mwv_avs_get_moni_state(struct regulator_dev *rdev)
 {
 	unsigned int val;
 	int ret;
@@ -55,8 +55,8 @@ int bd9571mwv_avs_get_moni_state(struct regulator_dev *rdev)
 	return val & BD9571MWV_AVS_SET_MONI_MASK;
 }
 
-int bd9571mwv_avs_set_voltage_sel_regmap(struct regulator_dev *rdev,
-					 unsigned int sel)
+static int bd9571mwv_avs_set_voltage_sel_regmap(struct regulator_dev *rdev,
+						unsigned int sel)
 {
 	int ret;
 
@@ -68,7 +68,7 @@ int bd9571mwv_avs_set_voltage_sel_regmap(struct regulator_dev *rdev,
 				 rdev->desc->vsel_mask, sel);
 }
 
-int bd9571mwv_avs_get_voltage_sel_regmap(struct regulator_dev *rdev)
+static int bd9571mwv_avs_get_voltage_sel_regmap(struct regulator_dev *rdev)
 {
 	unsigned int val;
 	int ret;
@@ -87,8 +87,8 @@ int bd9571mwv_avs_get_voltage_sel_regmap(struct regulator_dev *rdev)
 	return val;
 }
 
-int bd9571mwv_reg_set_voltage_sel_regmap(struct regulator_dev *rdev,
-					 unsigned int sel)
+static int bd9571mwv_reg_set_voltage_sel_regmap(struct regulator_dev *rdev,
+						unsigned int sel)
 {
 	return regmap_write_bits(rdev->regmap, BD9571MWV_DVFS_SETVID,
 				 rdev->desc->vsel_mask, sel);
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index c0d9ae8d0860..e567fa54980b 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -1462,7 +1462,7 @@ static struct regulator_dev *regulator_lookup_by_name(const char *name)
 static struct regulator_dev *regulator_dev_lookup(struct device *dev,
 						  const char *supply)
 {
-	struct regulator_dev *r;
+	struct regulator_dev *r = NULL;
 	struct device_node *node;
 	struct regulator_map *map;
 	const char *devname = NULL;
@@ -1489,10 +1489,6 @@ static struct regulator_dev *regulator_dev_lookup(struct device *dev,
 	if (dev)
 		devname = dev_name(dev);
 
-	r = regulator_lookup_by_name(supply);
-	if (r)
-		return r;
-
 	mutex_lock(&regulator_list_mutex);
 	list_for_each_entry(map, &regulator_map_list, list) {
 		/* If the mapping has a device set up it must match */
@@ -1511,6 +1507,10 @@ static struct regulator_dev *regulator_dev_lookup(struct device *dev,
 	if (r)
 		return r;
 
+	r = regulator_lookup_by_name(supply);
+	if (r)
+		return r;
+
 	return ERR_PTR(-ENODEV);
 }
 
@@ -2767,6 +2767,12 @@ static int _regulator_set_voltage_time(struct regulator_dev *rdev,
 		ramp_delay = rdev->desc->ramp_delay;
 	else if (rdev->constraints->settling_time)
 		return rdev->constraints->settling_time;
+	else if (rdev->constraints->settling_time_up &&
+		 (new_uV > old_uV))
+		return rdev->constraints->settling_time_up;
+	else if (rdev->constraints->settling_time_down &&
+		 (new_uV < old_uV))
+		return rdev->constraints->settling_time_down;
 
 	if (ramp_delay == 0) {
 		rdev_dbg(rdev, "ramp_delay not set\n");
@@ -2938,7 +2944,8 @@ static int regulator_set_voltage_unlocked(struct regulator *regulator,
 	if (rdev->supply &&
 	    regulator_ops_is_valid(rdev->supply->rdev,
 				   REGULATOR_CHANGE_VOLTAGE) &&
-	    (rdev->desc->min_dropout_uV || !rdev->desc->ops->get_voltage)) {
+	    (rdev->desc->min_dropout_uV || !(rdev->desc->ops->get_voltage ||
+					   rdev->desc->ops->get_voltage_sel))) {
 		int current_supply_uV;
 		int selector;
 
@@ -4311,41 +4318,31 @@ void *regulator_get_init_drvdata(struct regulator_init_data *reg_init_data)
 EXPORT_SYMBOL_GPL(regulator_get_init_drvdata);
 
 #ifdef CONFIG_DEBUG_FS
-static ssize_t supply_map_read_file(struct file *file, char __user *user_buf,
-				    size_t count, loff_t *ppos)
+static int supply_map_show(struct seq_file *sf, void *data)
 {
-	char *buf = kmalloc(PAGE_SIZE, GFP_KERNEL);
-	ssize_t len, ret = 0;
 	struct regulator_map *map;
 
-	if (!buf)
-		return -ENOMEM;
-
 	list_for_each_entry(map, &regulator_map_list, list) {
-		len = snprintf(buf + ret, PAGE_SIZE - ret,
-			       "%s -> %s.%s\n",
-			       rdev_get_name(map->regulator), map->dev_name,
-			       map->supply);
-		if (len >= 0)
-			ret += len;
-		if (ret > PAGE_SIZE) {
-			ret = PAGE_SIZE;
-			break;
-		}
+		seq_printf(sf, "%s -> %s.%s\n",
+				rdev_get_name(map->regulator), map->dev_name,
+				map->supply);
 	}
 
-	ret = simple_read_from_buffer(user_buf, count, ppos, buf, ret);
-
-	kfree(buf);
+	return 0;
+}
 
-	return ret;
+static int supply_map_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, supply_map_show, inode->i_private);
 }
 #endif
 
 static const struct file_operations supply_map_fops = {
 #ifdef CONFIG_DEBUG_FS
-	.read = supply_map_read_file,
-	.llseek = default_llseek,
+	.open = supply_map_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
 #endif
 };
 
diff --git a/drivers/regulator/da9062-regulator.c b/drivers/regulator/da9062-regulator.c
index 0638c8b40521..34a70d9dc450 100644
--- a/drivers/regulator/da9062-regulator.c
+++ b/drivers/regulator/da9062-regulator.c
@@ -1,6 +1,6 @@
 /*
- * da9062-regulator.c - REGULATOR device driver for DA9062
- * Copyright (C) 2015  Dialog Semiconductor Ltd.
+ * Regulator device driver for DA9061 and DA9062.
+ * Copyright (C) 2015-2017  Dialog Semiconductor
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
@@ -28,6 +28,17 @@
 
 /* Regulator IDs */
 enum {
+	DA9061_ID_BUCK1,
+	DA9061_ID_BUCK2,
+	DA9061_ID_BUCK3,
+	DA9061_ID_LDO1,
+	DA9061_ID_LDO2,
+	DA9061_ID_LDO3,
+	DA9061_ID_LDO4,
+	DA9061_MAX_REGULATORS,
+};
+
+enum {
 	DA9062_ID_BUCK1,
 	DA9062_ID_BUCK2,
 	DA9062_ID_BUCK3,
@@ -88,15 +99,21 @@ enum {
 
 /* Regulator operations */
 
-/* Current limits array (in uA) BUCK1 and BUCK3.
-   Entry indexes corresponds to register values. */
+/* Current limits array (in uA)
+ * - DA9061_ID_[BUCK1|BUCK3]
+ * - DA9062_ID_[BUCK1|BUCK2|BUCK4]
+ * Entry indexes corresponds to register values.
+ */
 static const int da9062_buck_a_limits[] = {
 	 500000,  600000,  700000,  800000,  900000, 1000000, 1100000, 1200000,
 	1300000, 1400000, 1500000, 1600000, 1700000, 1800000, 1900000, 2000000
 };
 
-/* Current limits array (in uA) for BUCK2.
-   Entry indexes corresponds to register values. */
+/* Current limits array (in uA)
+ * - DA9061_ID_BUCK2
+ * - DA9062_ID_BUCK3
+ * Entry indexes corresponds to register values.
+ */
 static const int da9062_buck_b_limits[] = {
 	1500000, 1600000, 1700000, 1800000, 1900000, 2000000, 2100000, 2200000,
 	2300000, 2400000, 2500000, 2600000, 2700000, 2800000, 2900000, 3000000
@@ -405,8 +422,254 @@ static const struct regulator_ops da9062_ldo_ops = {
 	.set_suspend_mode	= da9062_ldo_set_suspend_mode,
 };
 
-/* Regulator information */
-static const struct da9062_regulator_info local_regulator_info[] = {
+/* DA9061 Regulator information */
+static const struct da9062_regulator_info local_da9061_regulator_info[] = {
+	{
+		.desc.id = DA9061_ID_BUCK1,
+		.desc.name = "DA9061 BUCK1",
+		.desc.of_match = of_match_ptr("buck1"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_buck_ops,
+		.desc.min_uV = (300) * 1000,
+		.desc.uV_step = (10) * 1000,
+		.desc.n_voltages = ((1570) - (300))/(10) + 1,
+		.current_limits = da9062_buck_a_limits,
+		.n_current_limits = ARRAY_SIZE(da9062_buck_a_limits),
+		.desc.enable_reg = DA9062AA_BUCK1_CONT,
+		.desc.enable_mask = DA9062AA_BUCK1_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VBUCK1_A,
+		.desc.vsel_mask = DA9062AA_VBUCK1_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VBUCK1_A,
+			__builtin_ffs((int)DA9062AA_BUCK1_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK1_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VBUCK1_B,
+			__builtin_ffs((int)DA9062AA_BUCK1_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK1_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VBUCK1_B,
+		.mode = REG_FIELD(DA9062AA_BUCK1_CFG,
+			__builtin_ffs((int)DA9062AA_BUCK1_MODE_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK1_MODE_MASK)) - 1),
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VBUCK1_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VBUCK1_SEL_MASK)) - 1),
+		.ilimit = REG_FIELD(DA9062AA_BUCK_ILIM_C,
+			__builtin_ffs((int)DA9062AA_BUCK1_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK1_ILIM_MASK)) - 1),
+	},
+	{
+		.desc.id = DA9061_ID_BUCK2,
+		.desc.name = "DA9061 BUCK2",
+		.desc.of_match = of_match_ptr("buck2"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_buck_ops,
+		.desc.min_uV = (800) * 1000,
+		.desc.uV_step = (20) * 1000,
+		.desc.n_voltages = ((3340) - (800))/(20) + 1,
+		.current_limits = da9062_buck_b_limits,
+		.n_current_limits = ARRAY_SIZE(da9062_buck_b_limits),
+		.desc.enable_reg = DA9062AA_BUCK3_CONT,
+		.desc.enable_mask = DA9062AA_BUCK3_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VBUCK3_A,
+		.desc.vsel_mask = DA9062AA_VBUCK3_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VBUCK3_A,
+			__builtin_ffs((int)DA9062AA_BUCK3_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK3_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VBUCK3_B,
+			__builtin_ffs((int)DA9062AA_BUCK3_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK3_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VBUCK3_B,
+		.mode = REG_FIELD(DA9062AA_BUCK3_CFG,
+			__builtin_ffs((int)DA9062AA_BUCK3_MODE_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK3_MODE_MASK)) - 1),
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VBUCK3_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VBUCK3_SEL_MASK)) - 1),
+		.ilimit = REG_FIELD(DA9062AA_BUCK_ILIM_A,
+			__builtin_ffs((int)DA9062AA_BUCK3_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK3_ILIM_MASK)) - 1),
+	},
+	{
+		.desc.id = DA9061_ID_BUCK3,
+		.desc.name = "DA9061 BUCK3",
+		.desc.of_match = of_match_ptr("buck3"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_buck_ops,
+		.desc.min_uV = (530) * 1000,
+		.desc.uV_step = (10) * 1000,
+		.desc.n_voltages = ((1800) - (530))/(10) + 1,
+		.current_limits = da9062_buck_a_limits,
+		.n_current_limits = ARRAY_SIZE(da9062_buck_a_limits),
+		.desc.enable_reg = DA9062AA_BUCK4_CONT,
+		.desc.enable_mask = DA9062AA_BUCK4_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VBUCK4_A,
+		.desc.vsel_mask = DA9062AA_VBUCK4_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VBUCK4_A,
+			__builtin_ffs((int)DA9062AA_BUCK4_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK4_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VBUCK4_B,
+			__builtin_ffs((int)DA9062AA_BUCK4_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK4_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VBUCK4_B,
+		.mode = REG_FIELD(DA9062AA_BUCK4_CFG,
+			__builtin_ffs((int)DA9062AA_BUCK4_MODE_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK4_MODE_MASK)) - 1),
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VBUCK4_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VBUCK4_SEL_MASK)) - 1),
+		.ilimit = REG_FIELD(DA9062AA_BUCK_ILIM_B,
+			__builtin_ffs((int)DA9062AA_BUCK4_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK4_ILIM_MASK)) - 1),
+	},
+	{
+		.desc.id = DA9061_ID_LDO1,
+		.desc.name = "DA9061 LDO1",
+		.desc.of_match = of_match_ptr("ldo1"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_ldo_ops,
+		.desc.min_uV = (900) * 1000,
+		.desc.uV_step = (50) * 1000,
+		.desc.n_voltages = ((3600) - (900))/(50) + 1,
+		.desc.enable_reg = DA9062AA_LDO1_CONT,
+		.desc.enable_mask = DA9062AA_LDO1_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VLDO1_A,
+		.desc.vsel_mask = DA9062AA_VLDO1_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VLDO1_A,
+			__builtin_ffs((int)DA9062AA_LDO1_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO1_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VLDO1_B,
+			__builtin_ffs((int)DA9062AA_LDO1_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO1_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VLDO1_B,
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VLDO1_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VLDO1_SEL_MASK)) - 1),
+		.oc_event = REG_FIELD(DA9062AA_STATUS_D,
+			__builtin_ffs((int)DA9062AA_LDO1_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO1_ILIM_MASK)) - 1),
+	},
+	{
+		.desc.id = DA9061_ID_LDO2,
+		.desc.name = "DA9061 LDO2",
+		.desc.of_match = of_match_ptr("ldo2"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_ldo_ops,
+		.desc.min_uV = (900) * 1000,
+		.desc.uV_step = (50) * 1000,
+		.desc.n_voltages = ((3600) - (600))/(50) + 1,
+		.desc.enable_reg = DA9062AA_LDO2_CONT,
+		.desc.enable_mask = DA9062AA_LDO2_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VLDO2_A,
+		.desc.vsel_mask = DA9062AA_VLDO2_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VLDO2_A,
+			__builtin_ffs((int)DA9062AA_LDO2_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO2_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VLDO2_B,
+			__builtin_ffs((int)DA9062AA_LDO2_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO2_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VLDO2_B,
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VLDO2_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VLDO2_SEL_MASK)) - 1),
+		.oc_event = REG_FIELD(DA9062AA_STATUS_D,
+			__builtin_ffs((int)DA9062AA_LDO2_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO2_ILIM_MASK)) - 1),
+	},
+	{
+		.desc.id = DA9061_ID_LDO3,
+		.desc.name = "DA9061 LDO3",
+		.desc.of_match = of_match_ptr("ldo3"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_ldo_ops,
+		.desc.min_uV = (900) * 1000,
+		.desc.uV_step = (50) * 1000,
+		.desc.n_voltages = ((3600) - (900))/(50) + 1,
+		.desc.enable_reg = DA9062AA_LDO3_CONT,
+		.desc.enable_mask = DA9062AA_LDO3_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VLDO3_A,
+		.desc.vsel_mask = DA9062AA_VLDO3_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VLDO3_A,
+			__builtin_ffs((int)DA9062AA_LDO3_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO3_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VLDO3_B,
+			__builtin_ffs((int)DA9062AA_LDO3_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO3_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VLDO3_B,
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VLDO3_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VLDO3_SEL_MASK)) - 1),
+		.oc_event = REG_FIELD(DA9062AA_STATUS_D,
+			__builtin_ffs((int)DA9062AA_LDO3_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO3_ILIM_MASK)) - 1),
+	},
+	{
+		.desc.id = DA9061_ID_LDO4,
+		.desc.name = "DA9061 LDO4",
+		.desc.of_match = of_match_ptr("ldo4"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_ldo_ops,
+		.desc.min_uV = (900) * 1000,
+		.desc.uV_step = (50) * 1000,
+		.desc.n_voltages = ((3600) - (900))/(50) + 1,
+		.desc.enable_reg = DA9062AA_LDO4_CONT,
+		.desc.enable_mask = DA9062AA_LDO4_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VLDO4_A,
+		.desc.vsel_mask = DA9062AA_VLDO4_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VLDO4_A,
+			__builtin_ffs((int)DA9062AA_LDO4_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO4_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VLDO4_B,
+			__builtin_ffs((int)DA9062AA_LDO4_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO4_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VLDO4_B,
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VLDO4_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VLDO4_SEL_MASK)) - 1),
+		.oc_event = REG_FIELD(DA9062AA_STATUS_D,
+			__builtin_ffs((int)DA9062AA_LDO4_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO4_ILIM_MASK)) - 1),
+	},
+};
+
+/* DA9062 Regulator information */
+static const struct da9062_regulator_info local_da9062_regulator_info[] = {
 	{
 		.desc.id = DA9062_ID_BUCK1,
 		.desc.name = "DA9062 BUCK1",
@@ -727,17 +990,33 @@ static int da9062_regulator_probe(struct platform_device *pdev)
 	struct da9062_regulators *regulators;
 	struct da9062_regulator *regl;
 	struct regulator_config config = { };
+	const struct da9062_regulator_info *rinfo;
 	int irq, n, ret;
 	size_t size;
+	int max_regulators;
+
+	switch (chip->chip_type) {
+	case COMPAT_TYPE_DA9061:
+		max_regulators = DA9061_MAX_REGULATORS;
+		rinfo = local_da9061_regulator_info;
+		break;
+	case COMPAT_TYPE_DA9062:
+		max_regulators = DA9062_MAX_REGULATORS;
+		rinfo = local_da9062_regulator_info;
+		break;
+	default:
+		dev_err(chip->dev, "Unrecognised chip type\n");
+		return -ENODEV;
+	}
 
 	/* Allocate memory required by usable regulators */
 	size = sizeof(struct da9062_regulators) +
-		DA9062_MAX_REGULATORS * sizeof(struct da9062_regulator);
+		max_regulators * sizeof(struct da9062_regulator);
 	regulators = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
 	if (!regulators)
 		return -ENOMEM;
 
-	regulators->n_regulators = DA9062_MAX_REGULATORS;
+	regulators->n_regulators = max_regulators;
 	platform_set_drvdata(pdev, regulators);
 
 	n = 0;
@@ -745,7 +1024,7 @@ static int da9062_regulator_probe(struct platform_device *pdev)
 		/* Initialise regulator structure */
 		regl = &regulators->regulator[n];
 		regl->hw = chip;
-		regl->info = &local_regulator_info[n];
+		regl->info = &rinfo[n];
 		regl->desc = regl->info->desc;
 		regl->desc.type = REGULATOR_VOLTAGE;
 		regl->desc.owner = THIS_MODULE;
@@ -836,6 +1115,6 @@ module_exit(da9062_regulator_cleanup);
 
 /* Module information */
 MODULE_AUTHOR("S Twiss <stwiss.opensource@diasemi.com>");
-MODULE_DESCRIPTION("REGULATOR device driver for Dialog DA9062");
+MODULE_DESCRIPTION("REGULATOR device driver for Dialog DA9062 and DA9061");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:da9062-regulators");
diff --git a/drivers/regulator/hi6421-regulator.c b/drivers/regulator/hi6421-regulator.c
index 62c5f5445d44..259c3a865ac6 100644
--- a/drivers/regulator/hi6421-regulator.c
+++ b/drivers/regulator/hi6421-regulator.c
@@ -621,7 +621,14 @@ static int hi6421_regulator_probe(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct platform_device_id hi6421_regulator_table[] = {
+	{ .name = "hi6421-regulator" },
+	{},
+};
+MODULE_DEVICE_TABLE(platform, hi6421_regulator_table);
+
 static struct platform_driver hi6421_regulator_driver = {
+	.id_table = hi6421_regulator_table,
 	.driver = {
 		.name	= "hi6421-regulator",
 	},
diff --git a/drivers/regulator/hi6421v530-regulator.c b/drivers/regulator/hi6421v530-regulator.c
new file mode 100644
index 000000000000..c09bc71538a5
--- /dev/null
+++ b/drivers/regulator/hi6421v530-regulator.c
@@ -0,0 +1,214 @@
+/*
+ * Device driver for regulators in Hi6421V530 IC
+ *
+ * Copyright (c) <2017> HiSilicon Technologies Co., Ltd.
+ *              http://www.hisilicon.com
+ * Copyright (c) <2017> Linaro Ltd.
+ *              http://www.linaro.org
+ *
+ * Author: Wang Xiaoyin <hw.wangxiaoyin@hisilicon.com>
+ *         Guodong Xu <guodong.xu@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/mfd/hi6421-pmic.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/regulator/driver.h>
+
+/*
+ * struct hi6421v530_regulator_info - hi6421v530 regulator information
+ * @desc: regulator description
+ * @mode_mask: ECO mode bitmask of LDOs; for BUCKs, this masks sleep
+ * @eco_microamp: eco mode load upper limit (in uA), valid for LDOs only
+ */
+struct hi6421v530_regulator_info {
+	struct regulator_desc rdesc;
+	u8 mode_mask;
+	u32 eco_microamp;
+};
+
+/* HI6421v530 regulators */
+enum hi6421v530_regulator_id {
+	HI6421V530_LDO3,
+	HI6421V530_LDO9,
+	HI6421V530_LDO11,
+	HI6421V530_LDO15,
+	HI6421V530_LDO16,
+};
+
+static const unsigned int ldo_3_voltages[] = {
+	1800000, 1825000, 1850000, 1875000,
+	1900000, 1925000, 1950000, 1975000,
+	2000000, 2025000, 2050000, 2075000,
+	2100000, 2125000, 2150000, 2200000,
+};
+
+static const unsigned int ldo_9_11_voltages[] = {
+	1750000, 1800000, 1825000, 2800000,
+	2850000, 2950000, 3000000, 3300000,
+};
+
+static const unsigned int ldo_15_16_voltages[] = {
+	1750000, 1800000, 2400000, 2600000,
+	2700000, 2850000, 2950000, 3000000,
+};
+
+static const struct regulator_ops hi6421v530_ldo_ops;
+
+#define HI6421V530_LDO_ENABLE_TIME (350)
+
+/*
+ * _id - LDO id name string
+ * v_table - voltage table
+ * vreg - voltage select register
+ * vmask - voltage select mask
+ * ereg - enable register
+ * emask - enable mask
+ * odelay - off/on delay time in uS
+ * ecomask - eco mode mask
+ * ecoamp - eco mode load uppler limit in uA
+ */
+#define HI6421V530_LDO(_ID, v_table, vreg, vmask, ereg, emask,		\
+		   odelay, ecomask, ecoamp) {				\
+	.rdesc = {							\
+		.name		 = #_ID,				\
+		.of_match        = of_match_ptr(#_ID),			\
+		.regulators_node = of_match_ptr("regulators"),		\
+		.ops		 = &hi6421v530_ldo_ops,			\
+		.type		 = REGULATOR_VOLTAGE,			\
+		.id		 = HI6421V530_##_ID,			\
+		.owner		 = THIS_MODULE,				\
+		.n_voltages	 = ARRAY_SIZE(v_table),			\
+		.volt_table	 = v_table,				\
+		.vsel_reg	 = HI6421_REG_TO_BUS_ADDR(vreg),	\
+		.vsel_mask	 = vmask,				\
+		.enable_reg	 = HI6421_REG_TO_BUS_ADDR(ereg),	\
+		.enable_mask	 = emask,				\
+		.enable_time	 = HI6421V530_LDO_ENABLE_TIME,		\
+		.off_on_delay	 = odelay,				\
+	},								\
+	.mode_mask	= ecomask,					\
+	.eco_microamp	= ecoamp,					\
+}
+
+/* HI6421V530 regulator information */
+
+static struct hi6421v530_regulator_info hi6421v530_regulator_info[] = {
+	HI6421V530_LDO(LDO3, ldo_3_voltages, 0x061, 0xf, 0x060, 0x2,
+		   20000, 0x6, 8000),
+	HI6421V530_LDO(LDO9, ldo_9_11_voltages, 0x06b, 0x7, 0x06a, 0x2,
+		   40000, 0x6, 8000),
+	HI6421V530_LDO(LDO11, ldo_9_11_voltages, 0x06f, 0x7, 0x06e, 0x2,
+		   40000, 0x6, 8000),
+	HI6421V530_LDO(LDO15, ldo_15_16_voltages, 0x077, 0x7, 0x076, 0x2,
+		   40000, 0x6, 8000),
+	HI6421V530_LDO(LDO16, ldo_15_16_voltages, 0x079, 0x7, 0x078, 0x2,
+		   40000, 0x6, 8000),
+};
+
+static unsigned int hi6421v530_regulator_ldo_get_mode(
+					struct regulator_dev *rdev)
+{
+	struct hi6421v530_regulator_info *info;
+	unsigned int reg_val;
+
+	info = rdev_get_drvdata(rdev);
+	regmap_read(rdev->regmap, rdev->desc->enable_reg, &reg_val);
+
+	if (reg_val & (info->mode_mask))
+		return REGULATOR_MODE_IDLE;
+
+	return REGULATOR_MODE_NORMAL;
+}
+
+static int hi6421v530_regulator_ldo_set_mode(struct regulator_dev *rdev,
+						unsigned int mode)
+{
+	struct hi6421v530_regulator_info *info;
+	unsigned int new_mode;
+
+	info = rdev_get_drvdata(rdev);
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		new_mode = 0;
+		break;
+	case REGULATOR_MODE_IDLE:
+		new_mode = info->mode_mask;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+			   info->mode_mask, new_mode);
+
+	return 0;
+}
+
+
+static const struct regulator_ops hi6421v530_ldo_ops = {
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_mode = hi6421v530_regulator_ldo_get_mode,
+	.set_mode = hi6421v530_regulator_ldo_set_mode,
+};
+
+static int hi6421v530_regulator_probe(struct platform_device *pdev)
+{
+	struct hi6421_pmic *pmic;
+	struct regulator_dev *rdev;
+	struct regulator_config config = { };
+	unsigned int i;
+
+	pmic = dev_get_drvdata(pdev->dev.parent);
+	if (!pmic) {
+		dev_err(&pdev->dev, "no pmic in the regulator parent node\n");
+		return -ENODEV;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(hi6421v530_regulator_info); i++) {
+		config.dev = pdev->dev.parent;
+		config.regmap = pmic->regmap;
+		config.driver_data = &hi6421v530_regulator_info[i];
+
+		rdev = devm_regulator_register(&pdev->dev,
+				&hi6421v530_regulator_info[i].rdesc,
+				&config);
+		if (IS_ERR(rdev)) {
+			dev_err(&pdev->dev, "failed to register regulator %s\n",
+				hi6421v530_regulator_info[i].rdesc.name);
+			return PTR_ERR(rdev);
+		}
+	}
+	return 0;
+}
+
+static const struct platform_device_id hi6421v530_regulator_table[] = {
+	{ .name = "hi6421v530-regulator" },
+	{},
+};
+MODULE_DEVICE_TABLE(platform, hi6421v530_regulator_table);
+
+static struct platform_driver hi6421v530_regulator_driver = {
+	.id_table = hi6421v530_regulator_table,
+	.driver = {
+		.name	= "hi6421v530-regulator",
+	},
+	.probe	= hi6421v530_regulator_probe,
+};
+module_platform_driver(hi6421v530_regulator_driver);
+
+MODULE_AUTHOR("Wang Xiaoyin <hw.wangxiaoyin@hisilicon.com>");
+MODULE_DESCRIPTION("Hi6421v530 regulator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/lp8755.c b/drivers/regulator/lp8755.c
index db34e1da75ef..244822bb63cd 100644
--- a/drivers/regulator/lp8755.c
+++ b/drivers/regulator/lp8755.c
@@ -99,7 +99,7 @@ static int lp8755_buck_enable_time(struct regulator_dev *rdev)
 
 	ret = lp8755_read(pchip, 0x12 + id, &regval);
 	if (ret < 0) {
-		dev_err(pchip->dev, "i2c acceess error %s\n", __func__);
+		dev_err(pchip->dev, "i2c access error %s\n", __func__);
 		return ret;
 	}
 	return (regval & 0xff) * 100;
@@ -144,7 +144,7 @@ static int lp8755_buck_set_mode(struct regulator_dev *rdev, unsigned int mode)
 		goto err_i2c;
 	return ret;
 err_i2c:
-	dev_err(pchip->dev, "i2c acceess error %s\n", __func__);
+	dev_err(pchip->dev, "i2c access error %s\n", __func__);
 	return ret;
 }
 
@@ -175,7 +175,7 @@ static unsigned int lp8755_buck_get_mode(struct regulator_dev *rdev)
 	return REGULATOR_MODE_NORMAL;
 
 err_i2c:
-	dev_err(pchip->dev, "i2c acceess error %s\n", __func__);
+	dev_err(pchip->dev, "i2c access error %s\n", __func__);
 	return 0;
 }
 
@@ -223,7 +223,7 @@ static int lp8755_buck_set_ramp(struct regulator_dev *rdev, int ramp)
 		goto err_i2c;
 	return ret;
 err_i2c:
-	dev_err(pchip->dev, "i2c acceess error %s\n", __func__);
+	dev_err(pchip->dev, "i2c access error %s\n", __func__);
 	return ret;
 }
 
@@ -295,7 +295,7 @@ static int lp8755_init_data(struct lp8755_chip *pchip)
 	return ret;
 
 out_i2c_error:
-	dev_err(pchip->dev, "i2c acceess error %s\n", __func__);
+	dev_err(pchip->dev, "i2c access error %s\n", __func__);
 	return ret;
 }
 
@@ -404,7 +404,7 @@ static irqreturn_t lp8755_irq_handler(int irq, void *data)
 	return IRQ_HANDLED;
 
 err_i2c:
-	dev_err(pchip->dev, "i2c acceess error %s\n", __func__);
+	dev_err(pchip->dev, "i2c access error %s\n", __func__);
 	return IRQ_NONE;
 }
 
@@ -420,7 +420,7 @@ static int lp8755_int_config(struct lp8755_chip *pchip)
 
 	ret = lp8755_read(pchip, 0x0F, &regval);
 	if (ret < 0) {
-		dev_err(pchip->dev, "i2c acceess error %s\n", __func__);
+		dev_err(pchip->dev, "i2c access error %s\n", __func__);
 		return ret;
 	}
 
diff --git a/drivers/regulator/lp87565-regulator.c b/drivers/regulator/lp87565-regulator.c
new file mode 100644
index 000000000000..cfdbe294fb6a
--- /dev/null
+++ b/drivers/regulator/lp87565-regulator.c
@@ -0,0 +1,236 @@
+/*
+ * Regulator driver for LP87565 PMIC
+ *
+ * Copyright (C) 2017 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. 
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#include <linux/mfd/lp87565.h>
+
+#define LP87565_REGULATOR(_name, _id, _of, _ops, _n, _vr, _vm, _er, _em, \
+			 _delay, _lr, _cr)				\
+	[_id] = {							\
+		.desc = {						\
+			.name			= _name,		\
+			.supply_name		= _of "-in",		\
+			.id			= _id,			\
+			.of_match		= of_match_ptr(_of),	\
+			.regulators_node	= of_match_ptr("regulators"),\
+			.ops			= &_ops,		\
+			.n_voltages		= _n,			\
+			.type			= REGULATOR_VOLTAGE,	\
+			.owner			= THIS_MODULE,		\
+			.vsel_reg		= _vr,			\
+			.vsel_mask		= _vm,			\
+			.enable_reg		= _er,			\
+			.enable_mask		= _em,			\
+			.ramp_delay		= _delay,		\
+			.linear_ranges		= _lr,			\
+			.n_linear_ranges	= ARRAY_SIZE(_lr),	\
+		},							\
+		.ctrl2_reg = _cr,					\
+	}
+
+struct lp87565_regulator {
+	struct regulator_desc desc;
+	unsigned int ctrl2_reg;
+};
+
+static const struct lp87565_regulator regulators[];
+
+static const struct regulator_linear_range buck0_1_2_3_ranges[] = {
+	REGULATOR_LINEAR_RANGE(600000, 0xA, 0x17, 10000),
+	REGULATOR_LINEAR_RANGE(735000, 0x18, 0x9d, 5000),
+	REGULATOR_LINEAR_RANGE(1420000, 0x9e, 0xff, 20000),
+};
+
+static unsigned int lp87565_buck_ramp_delay[] = {
+	30000, 15000, 10000, 7500, 3800, 1900, 940, 470
+};
+
+/* LP87565 BUCK current limit */
+static const unsigned int lp87565_buck_uA[] = {
+	1500000, 2000000, 2500000, 3000000, 3500000, 4000000, 4500000, 5000000,
+};
+
+static int lp87565_buck_set_ramp_delay(struct regulator_dev *rdev,
+				       int ramp_delay)
+{
+	int id = rdev_get_id(rdev);
+	struct lp87565 *lp87565 = rdev_get_drvdata(rdev);
+	unsigned int reg;
+	int ret;
+
+	if (ramp_delay <= 470)
+		reg = 7;
+	else if (ramp_delay <= 940)
+		reg = 6;
+	else if (ramp_delay <= 1900)
+		reg = 5;
+	else if (ramp_delay <= 3800)
+		reg = 4;
+	else if (ramp_delay <= 7500)
+		reg = 3;
+	else if (ramp_delay <= 10000)
+		reg = 2;
+	else if (ramp_delay <= 15000)
+		reg = 1;
+	else
+		reg = 0;
+
+	ret = regmap_update_bits(lp87565->regmap, regulators[id].ctrl2_reg,
+				 LP87565_BUCK_CTRL_2_SLEW_RATE,
+				 reg << __ffs(LP87565_BUCK_CTRL_2_SLEW_RATE));
+	if (ret) {
+		dev_err(lp87565->dev, "SLEW RATE write failed: %d\n", ret);
+		return ret;
+	}
+
+	rdev->constraints->ramp_delay = lp87565_buck_ramp_delay[reg];
+
+	return 0;
+}
+
+static int lp87565_buck_set_current_limit(struct regulator_dev *rdev,
+					  int min_uA, int max_uA)
+{
+	int id = rdev_get_id(rdev);
+	struct lp87565 *lp87565 = rdev_get_drvdata(rdev);
+	int i;
+
+	for (i = ARRAY_SIZE(lp87565_buck_uA) - 1; i >= 0; i--) {
+		if (lp87565_buck_uA[i] >= min_uA &&
+		    lp87565_buck_uA[i] <= max_uA)
+			return regmap_update_bits(lp87565->regmap,
+						  regulators[id].ctrl2_reg,
+						  LP87565_BUCK_CTRL_2_ILIM,
+						  i << __ffs(LP87565_BUCK_CTRL_2_ILIM));
+	}
+
+	return -EINVAL;
+}
+
+static int lp87565_buck_get_current_limit(struct regulator_dev *rdev)
+{
+	int id = rdev_get_id(rdev);
+	struct lp87565 *lp87565 = rdev_get_drvdata(rdev);
+	int ret;
+	unsigned int val;
+
+	ret = regmap_read(lp87565->regmap, regulators[id].ctrl2_reg, &val);
+	if (ret)
+		return ret;
+
+	val = (val & LP87565_BUCK_CTRL_2_ILIM) >>
+	       __ffs(LP87565_BUCK_CTRL_2_ILIM);
+
+	return (val < ARRAY_SIZE(lp87565_buck_uA)) ?
+			lp87565_buck_uA[val] : -EINVAL;
+}
+
+/* Operations permitted on BUCK0, BUCK1 */
+static struct regulator_ops lp87565_buck_ops = {
+	.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		= regulator_list_voltage_linear_range,
+	.map_voltage		= regulator_map_voltage_linear_range,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
+	.set_ramp_delay		= lp87565_buck_set_ramp_delay,
+	.set_current_limit	= lp87565_buck_set_current_limit,
+	.get_current_limit	= lp87565_buck_get_current_limit,
+};
+
+static const struct lp87565_regulator regulators[] = {
+	LP87565_REGULATOR("BUCK0", LP87565_BUCK_0, "buck0", lp87565_buck_ops,
+			  256, LP87565_REG_BUCK0_VOUT, LP87565_BUCK_VSET,
+			  LP87565_REG_BUCK0_CTRL_1,
+			  LP87565_BUCK_CTRL_1_EN, 3800,
+			  buck0_1_2_3_ranges, LP87565_REG_BUCK0_CTRL_2),
+	LP87565_REGULATOR("BUCK1", LP87565_BUCK_1, "buck1", lp87565_buck_ops,
+			  256, LP87565_REG_BUCK1_VOUT, LP87565_BUCK_VSET,
+			  LP87565_REG_BUCK1_CTRL_1,
+			  LP87565_BUCK_CTRL_1_EN, 3800,
+			  buck0_1_2_3_ranges, LP87565_REG_BUCK1_CTRL_2),
+	LP87565_REGULATOR("BUCK2", LP87565_BUCK_2, "buck2", lp87565_buck_ops,
+			  256, LP87565_REG_BUCK2_VOUT, LP87565_BUCK_VSET,
+			  LP87565_REG_BUCK2_CTRL_1,
+			  LP87565_BUCK_CTRL_1_EN, 3800,
+			  buck0_1_2_3_ranges, LP87565_REG_BUCK2_CTRL_2),
+	LP87565_REGULATOR("BUCK3", LP87565_BUCK_3, "buck3", lp87565_buck_ops,
+			  256, LP87565_REG_BUCK3_VOUT, LP87565_BUCK_VSET,
+			  LP87565_REG_BUCK3_CTRL_1,
+			  LP87565_BUCK_CTRL_1_EN, 3800,
+			  buck0_1_2_3_ranges, LP87565_REG_BUCK3_CTRL_2),
+	LP87565_REGULATOR("BUCK10", LP87565_BUCK_10, "buck10", lp87565_buck_ops,
+			  256, LP87565_REG_BUCK0_VOUT, LP87565_BUCK_VSET,
+			  LP87565_REG_BUCK0_CTRL_1,
+			  LP87565_BUCK_CTRL_1_EN, 3800,
+			  buck0_1_2_3_ranges, LP87565_REG_BUCK0_CTRL_2),
+	LP87565_REGULATOR("BUCK23", LP87565_BUCK_23, "buck23", lp87565_buck_ops,
+			  256, LP87565_REG_BUCK2_VOUT, LP87565_BUCK_VSET,
+			  LP87565_REG_BUCK2_CTRL_1,
+			  LP87565_BUCK_CTRL_1_EN, 3800,
+			  buck0_1_2_3_ranges, LP87565_REG_BUCK2_CTRL_2),
+};
+
+static int lp87565_regulator_probe(struct platform_device *pdev)
+{
+	struct lp87565 *lp87565 = dev_get_drvdata(pdev->dev.parent);
+	struct regulator_config config = { };
+	struct regulator_dev *rdev;
+	int i, min_idx = LP87565_BUCK_1, max_idx = LP87565_BUCK_3;
+
+	platform_set_drvdata(pdev, lp87565);
+
+	config.dev = &pdev->dev;
+	config.dev->of_node = lp87565->dev->of_node;
+	config.driver_data = lp87565;
+	config.regmap = lp87565->regmap;
+
+	if (lp87565->dev_type == LP87565_DEVICE_TYPE_LP87565_Q1) {
+		min_idx = LP87565_BUCK_10;
+		max_idx = LP87565_BUCK_23;
+	}
+
+	for (i = min_idx; i <= max_idx; i++) {
+		rdev = devm_regulator_register(&pdev->dev, &regulators[i].desc,
+					       &config);
+		if (IS_ERR(rdev)) {
+			dev_err(lp87565->dev, "failed to register %s regulator\n",
+				pdev->name);
+			return PTR_ERR(rdev);
+		}
+	}
+
+	return 0;
+}
+
+static const struct platform_device_id lp87565_regulator_id_table[] = {
+	{ "lp87565-regulator", },
+	{ "lp87565-q1-regulator", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, lp87565_regulator_id_table);
+
+static struct platform_driver lp87565_regulator_driver = {
+	.driver = {
+		.name = "lp87565-pmic",
+	},
+	.probe = lp87565_regulator_probe,
+	.id_table = lp87565_regulator_id_table,
+};
+module_platform_driver(lp87565_regulator_driver);
+
+MODULE_AUTHOR("J Keerthy <j-keerthy@ti.com>");
+MODULE_DESCRIPTION("LP87565 voltage regulator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/max8997-regulator.c b/drivers/regulator/max8997-regulator.c
index efabc0ea0e96..559b9ac45404 100644
--- a/drivers/regulator/max8997-regulator.c
+++ b/drivers/regulator/max8997-regulator.c
@@ -428,12 +428,9 @@ static int max8997_set_voltage_charger_cv(struct regulator_dev *rdev,
 	if (max_uV < 4000000 || min_uV > 4350000)
 		return -EINVAL;
 
-	if (min_uV <= 4000000) {
-		if (max_uV >= 4000000)
-			return -EINVAL;
-		else
-			val = 0x1;
-	} else if (min_uV <= 4200000 && max_uV >= 4200000)
+	if (min_uV <= 4000000)
+		val = 0x1;
+	else if (min_uV <= 4200000 && max_uV >= 4200000)
 		val = 0x0;
 	else {
 		lb = (min_uV - 4000001) / 20000 + 2;
diff --git a/drivers/regulator/of_regulator.c b/drivers/regulator/of_regulator.c
index 09d677d5d3f0..96bf75458da5 100644
--- a/drivers/regulator/of_regulator.c
+++ b/drivers/regulator/of_regulator.c
@@ -90,6 +90,25 @@ static void of_get_regulation_constraints(struct device_node *np,
 	if (!ret)
 		constraints->settling_time = pval;
 
+	ret = of_property_read_u32(np, "regulator-settling-time-up-us", &pval);
+	if (!ret)
+		constraints->settling_time_up = pval;
+	if (constraints->settling_time_up && constraints->settling_time) {
+		pr_warn("%s: ambiguous configuration for settling time, ignoring 'regulator-settling-time-up-us'\n",
+			np->name);
+		constraints->settling_time_up = 0;
+	}
+
+	ret = of_property_read_u32(np, "regulator-settling-time-down-us",
+				   &pval);
+	if (!ret)
+		constraints->settling_time_down = pval;
+	if (constraints->settling_time_down && constraints->settling_time) {
+		pr_warn("%s: ambiguous configuration for settling time, ignoring 'regulator-settling-time-down-us'\n",
+			np->name);
+		constraints->settling_time_down = 0;
+	}
+
 	ret = of_property_read_u32(np, "regulator-enable-ramp-delay", &pval);
 	if (!ret)
 		constraints->enable_time = pval;
diff --git a/drivers/regulator/palmas-regulator.c b/drivers/regulator/palmas-regulator.c
index 31ae5ee3a80d..bb5ab7d78895 100644
--- a/drivers/regulator/palmas-regulator.c
+++ b/drivers/regulator/palmas-regulator.c
@@ -264,6 +264,13 @@ static struct palmas_regs_info tps65917_regs_info[] = {
 		.sleep_id	= TPS65917_EXTERNAL_REQSTR_ID_SMPS5,
 	},
 	{
+		.name		= "SMPS12",
+		.sname		= "smps1-in",
+		.vsel_addr	= TPS65917_SMPS1_VOLTAGE,
+		.ctrl_addr	= TPS65917_SMPS1_CTRL,
+		.sleep_id	= TPS65917_EXTERNAL_REQSTR_ID_SMPS12,
+	},
+	{
 		.name		= "LDO1",
 		.sname		= "ldo1-in",
 		.vsel_addr	= TPS65917_LDO1_VOLTAGE,
@@ -367,6 +374,7 @@ static struct palmas_sleep_requestor_info tps65917_sleep_req_info[] = {
 	EXTERNAL_REQUESTOR_TPS65917(SMPS3, 1, 2),
 	EXTERNAL_REQUESTOR_TPS65917(SMPS4, 1, 3),
 	EXTERNAL_REQUESTOR_TPS65917(SMPS5, 1, 4),
+	EXTERNAL_REQUESTOR_TPS65917(SMPS12, 1, 5),
 	EXTERNAL_REQUESTOR_TPS65917(LDO1, 2, 0),
 	EXTERNAL_REQUESTOR_TPS65917(LDO2, 2, 1),
 	EXTERNAL_REQUESTOR_TPS65917(LDO3, 2, 2),
@@ -1305,7 +1313,8 @@ static int tps65917_smps_registration(struct palmas_pmic *pmic,
 		 */
 		desc = &pmic->desc[id];
 		desc->n_linear_ranges = 3;
-		if ((id == TPS65917_REG_SMPS2) && pmic->smps12)
+		if ((id == TPS65917_REG_SMPS2 || id == TPS65917_REG_SMPS1) &&
+		    pmic->smps12)
 			continue;
 
 		/* Initialise sleep/init values from platform data */
@@ -1427,6 +1436,7 @@ static struct of_regulator_match tps65917_matches[] = {
 	{ .name = "smps3", },
 	{ .name = "smps4", },
 	{ .name = "smps5", },
+	{ .name = "smps12",},
 	{ .name = "ldo1", },
 	{ .name = "ldo2", },
 	{ .name = "ldo3", },
@@ -1455,7 +1465,7 @@ static struct palmas_pmic_driver_data palmas_ddata = {
 
 static struct palmas_pmic_driver_data tps65917_ddata = {
 	.smps_start = TPS65917_REG_SMPS1,
-	.smps_end = TPS65917_REG_SMPS5,
+	.smps_end = TPS65917_REG_SMPS12,
 	.ldo_begin = TPS65917_REG_LDO1,
 	.ldo_end = TPS65917_REG_LDO5,
 	.max_reg = TPS65917_NUM_REGS,
@@ -1491,7 +1501,7 @@ static int palmas_dt_to_pdata(struct device *dev,
 	}
 
 	for (idx = 0; idx < ddata->max_reg; idx++) {
-		static struct of_regulator_match *match;
+		struct of_regulator_match *match;
 		struct palmas_reg_init *rinit;
 		struct device_node *np;
 
@@ -1643,8 +1653,10 @@ static int palmas_regulators_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	if (reg & PALMAS_SMPS_CTRL_SMPS12_SMPS123_EN)
+	if (reg & PALMAS_SMPS_CTRL_SMPS12_SMPS123_EN) {
 		pmic->smps123 = 1;
+		pmic->smps12 = 1;
+	}
 
 	if (reg & PALMAS_SMPS_CTRL_SMPS45_SMPS457_EN)
 		pmic->smps457 = 1;
diff --git a/drivers/regulator/tps65910-regulator.c b/drivers/regulator/tps65910-regulator.c
index 696116ebdf50..81672a58fcc2 100644
--- a/drivers/regulator/tps65910-regulator.c
+++ b/drivers/regulator/tps65910-regulator.c
@@ -1107,6 +1107,7 @@ static int tps65910_probe(struct platform_device *pdev)
 
 	switch (tps65910_chip_id(tps65910)) {
 	case TPS65910:
+		BUILD_BUG_ON(TPS65910_NUM_REGS < ARRAY_SIZE(tps65910_regs));
 		pmic->get_ctrl_reg = &tps65910_get_ctrl_register;
 		pmic->num_regulators = ARRAY_SIZE(tps65910_regs);
 		pmic->ext_sleep_control = tps65910_ext_sleep_control;
@@ -1119,6 +1120,7 @@ static int tps65910_probe(struct platform_device *pdev)
 					DCDCCTRL_DCDCCKSYNC_MASK);
 		break;
 	case TPS65911:
+		BUILD_BUG_ON(TPS65910_NUM_REGS < ARRAY_SIZE(tps65911_regs));
 		pmic->get_ctrl_reg = &tps65911_get_ctrl_register;
 		pmic->num_regulators = ARRAY_SIZE(tps65911_regs);
 		pmic->ext_sleep_control = tps65911_ext_sleep_control;
@@ -1144,8 +1146,7 @@ static int tps65910_probe(struct platform_device *pdev)
 	if (!pmic->rdev)
 		return -ENOMEM;
 
-	for (i = 0; i < pmic->num_regulators && i < TPS65910_NUM_REGS;
-			i++, info++) {
+	for (i = 0; i < pmic->num_regulators; i++, info++) {
 		/* Register the regulators */
 		pmic->info[i] = info;
 
diff --git a/drivers/reset/Kconfig b/drivers/reset/Kconfig
index d21c07ccc94e..608c071e4bbf 100644
--- a/drivers/reset/Kconfig
+++ b/drivers/reset/Kconfig
@@ -34,6 +34,13 @@ config RESET_BERLIN
 	help
 	  This enables the reset controller driver for Marvell Berlin SoCs.
 
+config RESET_GEMINI
+	bool "Gemini Reset Driver" if COMPILE_TEST
+	default ARCH_GEMINI
+	select MFD_SYSCON
+	help
+	  This enables the reset controller driver for Cortina Systems Gemini.
+
 config RESET_IMX7
 	bool "i.MX7 Reset Driver" if COMPILE_TEST
 	default SOC_IMX7D
@@ -80,7 +87,15 @@ config RESET_SUNXI
 	help
 	  This enables the reset driver for Allwinner SoCs.
 
-config TI_SYSCON_RESET
+config RESET_TI_SCI
+	tristate "TI System Control Interface (TI-SCI) reset driver"
+	depends on TI_SCI_PROTOCOL
+	help
+	  This enables the reset driver support over TI System Control Interface
+	  available on some new TI's SoCs. If you wish to use reset resources
+	  managed by the TI System Controller, say Y here. Otherwise, say N.
+
+config RESET_TI_SYSCON
 	tristate "TI SYSCON Reset Driver"
 	depends on HAS_IOMEM
 	select MFD_SYSCON
diff --git a/drivers/reset/Makefile b/drivers/reset/Makefile
index 02a74db94339..7081f9da2599 100644
--- a/drivers/reset/Makefile
+++ b/drivers/reset/Makefile
@@ -5,6 +5,7 @@ obj-$(CONFIG_ARCH_TEGRA) += tegra/
 obj-$(CONFIG_RESET_A10SR) += reset-a10sr.o
 obj-$(CONFIG_RESET_ATH79) += reset-ath79.o
 obj-$(CONFIG_RESET_BERLIN) += reset-berlin.o
+obj-$(CONFIG_RESET_GEMINI) += reset-gemini.o
 obj-$(CONFIG_RESET_IMX7) += reset-imx7.o
 obj-$(CONFIG_RESET_LPC18XX) += reset-lpc18xx.o
 obj-$(CONFIG_RESET_MESON) += reset-meson.o
@@ -13,7 +14,8 @@ obj-$(CONFIG_RESET_PISTACHIO) += reset-pistachio.o
 obj-$(CONFIG_RESET_SOCFPGA) += reset-socfpga.o
 obj-$(CONFIG_RESET_STM32) += reset-stm32.o
 obj-$(CONFIG_RESET_SUNXI) += reset-sunxi.o
-obj-$(CONFIG_TI_SYSCON_RESET) += reset-ti-syscon.o
+obj-$(CONFIG_RESET_TI_SCI) += reset-ti-sci.o
+obj-$(CONFIG_RESET_TI_SYSCON) += reset-ti-syscon.o
 obj-$(CONFIG_RESET_UNIPHIER) += reset-uniphier.o
 obj-$(CONFIG_RESET_ZX2967) += reset-zx2967.o
 obj-$(CONFIG_RESET_ZYNQ) += reset-zynq.o
diff --git a/drivers/reset/core.c b/drivers/reset/core.c
index cd739d2fa160..0090784ff410 100644
--- a/drivers/reset/core.c
+++ b/drivers/reset/core.c
@@ -13,6 +13,7 @@
 #include <linux/err.h>
 #include <linux/export.h>
 #include <linux/kernel.h>
+#include <linux/kref.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/reset.h>
@@ -40,7 +41,7 @@ struct reset_control {
 	struct reset_controller_dev *rcdev;
 	struct list_head list;
 	unsigned int id;
-	unsigned int refcnt;
+	struct kref refcnt;
 	bool shared;
 	atomic_t deassert_count;
 	atomic_t triggered_count;
@@ -288,7 +289,7 @@ static struct reset_control *__reset_control_get_internal(
 			if (WARN_ON(!rstc->shared || !shared))
 				return ERR_PTR(-EBUSY);
 
-			rstc->refcnt++;
+			kref_get(&rstc->refcnt);
 			return rstc;
 		}
 	}
@@ -302,18 +303,18 @@ static struct reset_control *__reset_control_get_internal(
 	rstc->rcdev = rcdev;
 	list_add(&rstc->list, &rcdev->reset_control_head);
 	rstc->id = index;
-	rstc->refcnt = 1;
+	kref_init(&rstc->refcnt);
 	rstc->shared = shared;
 
 	return rstc;
 }
 
-static void __reset_control_put_internal(struct reset_control *rstc)
+static void __reset_control_release(struct kref *kref)
 {
-	lockdep_assert_held(&reset_list_mutex);
+	struct reset_control *rstc = container_of(kref, struct reset_control,
+						  refcnt);
 
-	if (--rstc->refcnt)
-		return;
+	lockdep_assert_held(&reset_list_mutex);
 
 	module_put(rstc->rcdev->owner);
 
@@ -321,6 +322,13 @@ static void __reset_control_put_internal(struct reset_control *rstc)
 	kfree(rstc);
 }
 
+static void __reset_control_put_internal(struct reset_control *rstc)
+{
+	lockdep_assert_held(&reset_list_mutex);
+
+	kref_put(&rstc->refcnt, __reset_control_release);
+}
+
 struct reset_control *__of_reset_control_get(struct device_node *node,
 				     const char *id, int index, bool shared,
 				     bool optional)
@@ -400,7 +408,6 @@ EXPORT_SYMBOL_GPL(__reset_control_get);
  * reset_control_put - free the reset controller
  * @rstc: reset controller
  */
-
 void reset_control_put(struct reset_control *rstc)
 {
 	if (IS_ERR_OR_NULL(rstc))
diff --git a/drivers/reset/reset-gemini.c b/drivers/reset/reset-gemini.c
new file mode 100644
index 000000000000..a2478997c75b
--- /dev/null
+++ b/drivers/reset/reset-gemini.c
@@ -0,0 +1,110 @@
+/*
+ * Cortina Gemini Reset controller driver
+ * Copyright (C) 2017 Linus Walleij <linus.walleij@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/reset-controller.h>
+#include <dt-bindings/reset/cortina,gemini-reset.h>
+
+/**
+ * struct gemini_reset - gemini reset controller
+ * @map: regmap to access the containing system controller
+ * @rcdev: reset controller device
+ */
+struct gemini_reset {
+	struct regmap *map;
+	struct reset_controller_dev rcdev;
+};
+
+#define GEMINI_GLOBAL_SOFT_RESET 0x0c
+
+#define to_gemini_reset(p) \
+	container_of((p), struct gemini_reset, rcdev)
+
+/*
+ * This is a self-deasserting reset controller.
+ */
+static int gemini_reset(struct reset_controller_dev *rcdev,
+			unsigned long id)
+{
+	struct gemini_reset *gr = to_gemini_reset(rcdev);
+
+	/* Manual says to always set BIT 30 (CPU1) to 1 */
+	return regmap_write(gr->map,
+			    GEMINI_GLOBAL_SOFT_RESET,
+			    BIT(GEMINI_RESET_CPU1) | BIT(id));
+}
+
+static int gemini_reset_status(struct reset_controller_dev *rcdev,
+			     unsigned long id)
+{
+	struct gemini_reset *gr = to_gemini_reset(rcdev);
+	u32 val;
+	int ret;
+
+	ret = regmap_read(gr->map, GEMINI_GLOBAL_SOFT_RESET, &val);
+	if (ret)
+		return ret;
+
+	return !!(val & BIT(id));
+}
+
+static const struct reset_control_ops gemini_reset_ops = {
+	.reset = gemini_reset,
+	.status = gemini_reset_status,
+};
+
+static int gemini_reset_probe(struct platform_device *pdev)
+{
+	struct gemini_reset *gr;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	int ret;
+
+	gr = devm_kzalloc(dev, sizeof(*gr), GFP_KERNEL);
+	if (!gr)
+		return -ENOMEM;
+
+	gr->map = syscon_node_to_regmap(np);
+	if (IS_ERR(gr->map)) {
+		ret = PTR_ERR(gr->map);
+		dev_err(dev, "unable to get regmap (%d)", ret);
+		return ret;
+	}
+	gr->rcdev.owner = THIS_MODULE;
+	gr->rcdev.nr_resets = 32;
+	gr->rcdev.ops = &gemini_reset_ops;
+	gr->rcdev.of_node = pdev->dev.of_node;
+
+	ret = devm_reset_controller_register(&pdev->dev, &gr->rcdev);
+	if (ret)
+		return ret;
+
+	dev_info(dev, "registered Gemini reset controller\n");
+	return 0;
+}
+
+static const struct of_device_id gemini_reset_dt_ids[] = {
+	{ .compatible = "cortina,gemini-syscon", },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver gemini_reset_driver = {
+	.probe	= gemini_reset_probe,
+	.driver = {
+		.name		= "gemini-reset",
+		.of_match_table	= gemini_reset_dt_ids,
+		.suppress_bind_attrs = true,
+	},
+};
+builtin_platform_driver(gemini_reset_driver);
diff --git a/drivers/reset/reset-ti-sci.c b/drivers/reset/reset-ti-sci.c
new file mode 100644
index 000000000000..bf68729ab729
--- /dev/null
+++ b/drivers/reset/reset-ti-sci.c
@@ -0,0 +1,269 @@
+/*
+ * Texas Instrument's System Control Interface (TI-SCI) reset driver
+ *
+ * Copyright (C) 2015-2017 Texas Instruments Incorporated - http://www.ti.com/
+ *	Andrew F. Davis <afd@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/idr.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/reset-controller.h>
+#include <linux/soc/ti/ti_sci_protocol.h>
+
+/**
+ * struct ti_sci_reset_control - reset control structure
+ * @dev_id: SoC-specific device identifier
+ * @reset_mask: reset mask to use for toggling reset
+ * @lock: synchronize reset_mask read-modify-writes
+ */
+struct ti_sci_reset_control {
+	u32 dev_id;
+	u32 reset_mask;
+	struct mutex lock;
+};
+
+/**
+ * struct ti_sci_reset_data - reset controller information structure
+ * @rcdev: reset controller entity
+ * @dev: reset controller device pointer
+ * @sci: TI SCI handle used for communication with system controller
+ * @idr: idr structure for mapping ids to reset control structures
+ */
+struct ti_sci_reset_data {
+	struct reset_controller_dev rcdev;
+	struct device *dev;
+	const struct ti_sci_handle *sci;
+	struct idr idr;
+};
+
+#define to_ti_sci_reset_data(p)	\
+	container_of((p), struct ti_sci_reset_data, rcdev)
+
+/**
+ * ti_sci_reset_set() - program a device's reset
+ * @rcdev: reset controller entity
+ * @id: ID of the reset to toggle
+ * @assert: boolean flag to indicate assert or deassert
+ *
+ * This is a common internal function used to assert or deassert a device's
+ * reset using the TI SCI protocol. The device's reset is asserted if the
+ * @assert argument is true, or deasserted if @assert argument is false.
+ * The mechanism itself is a read-modify-write procedure, the current device
+ * reset register is read using a TI SCI device operation, the new value is
+ * set or un-set using the reset's mask, and the new reset value written by
+ * using another TI SCI device operation.
+ *
+ * Return: 0 for successful request, else a corresponding error value
+ */
+static int ti_sci_reset_set(struct reset_controller_dev *rcdev,
+			    unsigned long id, bool assert)
+{
+	struct ti_sci_reset_data *data = to_ti_sci_reset_data(rcdev);
+	const struct ti_sci_handle *sci = data->sci;
+	const struct ti_sci_dev_ops *dev_ops = &sci->ops.dev_ops;
+	struct ti_sci_reset_control *control;
+	u32 reset_state;
+	int ret;
+
+	control = idr_find(&data->idr, id);
+	if (!control)
+		return -EINVAL;
+
+	mutex_lock(&control->lock);
+
+	ret = dev_ops->get_device_resets(sci, control->dev_id, &reset_state);
+	if (ret)
+		goto out;
+
+	if (assert)
+		reset_state |= control->reset_mask;
+	else
+		reset_state &= ~control->reset_mask;
+
+	ret = dev_ops->set_device_resets(sci, control->dev_id, reset_state);
+out:
+	mutex_unlock(&control->lock);
+
+	return ret;
+}
+
+/**
+ * ti_sci_reset_assert() - assert device reset
+ * @rcdev: reset controller entity
+ * @id: ID of the reset to be asserted
+ *
+ * This function implements the reset driver op to assert a device's reset
+ * using the TI SCI protocol. This invokes the function ti_sci_reset_set()
+ * with the corresponding parameters as passed in, but with the @assert
+ * argument set to true for asserting the reset.
+ *
+ * Return: 0 for successful request, else a corresponding error value
+ */
+static int ti_sci_reset_assert(struct reset_controller_dev *rcdev,
+			       unsigned long id)
+{
+	return ti_sci_reset_set(rcdev, id, true);
+}
+
+/**
+ * ti_sci_reset_deassert() - deassert device reset
+ * @rcdev: reset controller entity
+ * @id: ID of the reset to be deasserted
+ *
+ * This function implements the reset driver op to deassert a device's reset
+ * using the TI SCI protocol. This invokes the function ti_sci_reset_set()
+ * with the corresponding parameters as passed in, but with the @assert
+ * argument set to false for deasserting the reset.
+ *
+ * Return: 0 for successful request, else a corresponding error value
+ */
+static int ti_sci_reset_deassert(struct reset_controller_dev *rcdev,
+				 unsigned long id)
+{
+	return ti_sci_reset_set(rcdev, id, false);
+}
+
+/**
+ * ti_sci_reset_status() - check device reset status
+ * @rcdev: reset controller entity
+ * @id: ID of reset to be checked
+ *
+ * This function implements the reset driver op to return the status of a
+ * device's reset using the TI SCI protocol. The reset register value is read
+ * by invoking the TI SCI device operation .get_device_resets(), and the
+ * status of the specific reset is extracted and returned using this reset's
+ * reset mask.
+ *
+ * Return: 0 if reset is deasserted, or a non-zero value if reset is asserted
+ */
+static int ti_sci_reset_status(struct reset_controller_dev *rcdev,
+			       unsigned long id)
+{
+	struct ti_sci_reset_data *data = to_ti_sci_reset_data(rcdev);
+	const struct ti_sci_handle *sci = data->sci;
+	const struct ti_sci_dev_ops *dev_ops = &sci->ops.dev_ops;
+	struct ti_sci_reset_control *control;
+	u32 reset_state;
+	int ret;
+
+	control = idr_find(&data->idr, id);
+	if (!control)
+		return -EINVAL;
+
+	ret = dev_ops->get_device_resets(sci, control->dev_id, &reset_state);
+	if (ret)
+		return ret;
+
+	return reset_state & control->reset_mask;
+}
+
+static const struct reset_control_ops ti_sci_reset_ops = {
+	.assert		= ti_sci_reset_assert,
+	.deassert	= ti_sci_reset_deassert,
+	.status		= ti_sci_reset_status,
+};
+
+/**
+ * ti_sci_reset_of_xlate() - translate a set of OF arguments to a reset ID
+ * @rcdev: reset controller entity
+ * @reset_spec: OF reset argument specifier
+ *
+ * This function performs the translation of the reset argument specifier
+ * values defined in a reset consumer device node. The function allocates a
+ * reset control structure for that device reset, and will be used by the
+ * driver for performing any reset functions on that reset. An idr structure
+ * is allocated and used to map to the reset control structure. This idr
+ * is used by the driver to do reset lookups.
+ *
+ * Return: 0 for successful request, else a corresponding error value
+ */
+static int ti_sci_reset_of_xlate(struct reset_controller_dev *rcdev,
+				 const struct of_phandle_args *reset_spec)
+{
+	struct ti_sci_reset_data *data = to_ti_sci_reset_data(rcdev);
+	struct ti_sci_reset_control *control;
+
+	if (WARN_ON(reset_spec->args_count != rcdev->of_reset_n_cells))
+		return -EINVAL;
+
+	control = devm_kzalloc(data->dev, sizeof(*control), GFP_KERNEL);
+	if (!control)
+		return -ENOMEM;
+
+	control->dev_id = reset_spec->args[0];
+	control->reset_mask = reset_spec->args[1];
+	mutex_init(&control->lock);
+
+	return idr_alloc(&data->idr, control, 0, 0, GFP_KERNEL);
+}
+
+static const struct of_device_id ti_sci_reset_of_match[] = {
+	{ .compatible = "ti,sci-reset", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, ti_sci_reset_of_match);
+
+static int ti_sci_reset_probe(struct platform_device *pdev)
+{
+	struct ti_sci_reset_data *data;
+
+	if (!pdev->dev.of_node)
+		return -ENODEV;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->sci = devm_ti_sci_get_handle(&pdev->dev);
+	if (IS_ERR(data->sci))
+		return PTR_ERR(data->sci);
+
+	data->rcdev.ops = &ti_sci_reset_ops;
+	data->rcdev.owner = THIS_MODULE;
+	data->rcdev.of_node = pdev->dev.of_node;
+	data->rcdev.of_reset_n_cells = 2;
+	data->rcdev.of_xlate = ti_sci_reset_of_xlate;
+	data->dev = &pdev->dev;
+	idr_init(&data->idr);
+
+	platform_set_drvdata(pdev, data);
+
+	return reset_controller_register(&data->rcdev);
+}
+
+static int ti_sci_reset_remove(struct platform_device *pdev)
+{
+	struct ti_sci_reset_data *data = platform_get_drvdata(pdev);
+
+	reset_controller_unregister(&data->rcdev);
+
+	idr_destroy(&data->idr);
+
+	return 0;
+}
+
+static struct platform_driver ti_sci_reset_driver = {
+	.probe = ti_sci_reset_probe,
+	.remove = ti_sci_reset_remove,
+	.driver = {
+		.name = "ti-sci-reset",
+		.of_match_table = ti_sci_reset_of_match,
+	},
+};
+module_platform_driver(ti_sci_reset_driver);
+
+MODULE_AUTHOR("Andrew F. Davis <afd@ti.com>");
+MODULE_DESCRIPTION("TI System Control Interface (TI SCI) Reset driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/reset/sti/reset-syscfg.c b/drivers/reset/sti/reset-syscfg.c
index 9bd57a5eee72..7e0f2aa55ba7 100644
--- a/drivers/reset/sti/reset-syscfg.c
+++ b/drivers/reset/sti/reset-syscfg.c
@@ -145,16 +145,14 @@ static int syscfg_reset_controller_register(struct device *dev,
 				const struct syscfg_reset_controller_data *data)
 {
 	struct syscfg_reset_controller *rc;
-	size_t size;
 	int i, err;
 
 	rc = devm_kzalloc(dev, sizeof(*rc), GFP_KERNEL);
 	if (!rc)
 		return -ENOMEM;
 
-	size = sizeof(struct syscfg_reset_channel) * data->nr_channels;
-
-	rc->channels = devm_kzalloc(dev, size, GFP_KERNEL);
+	rc->channels = devm_kcalloc(dev, data->nr_channels,
+				    sizeof(*rc->channels), GFP_KERNEL);
 	if (!rc->channels)
 		return -ENOMEM;
 
diff --git a/drivers/rpmsg/rpmsg_char.c b/drivers/rpmsg/rpmsg_char.c
index 0ca2ccc09ca6..2576284f99a7 100644
--- a/drivers/rpmsg/rpmsg_char.c
+++ b/drivers/rpmsg/rpmsg_char.c
@@ -116,7 +116,7 @@ static int rpmsg_ept_cb(struct rpmsg_device *rpdev, void *buf, int len,
 	if (!skb)
 		return -ENOMEM;
 
-	memcpy(skb_put(skb, len), buf, len);
+	skb_put_data(skb, buf, len);
 
 	spin_lock(&eptdev->queue_lock);
 	skb_queue_tail(&eptdev->queue, skb);
diff --git a/drivers/rpmsg/rpmsg_core.c b/drivers/rpmsg/rpmsg_core.c
index 600f5f9f7431..ad3d2a9df287 100644
--- a/drivers/rpmsg/rpmsg_core.c
+++ b/drivers/rpmsg/rpmsg_core.c
@@ -330,7 +330,8 @@ field##_show(struct device *dev,					\
 	struct rpmsg_device *rpdev = to_rpmsg_device(dev);		\
 									\
 	return sprintf(buf, format_string, rpdev->path);		\
-}
+}									\
+static DEVICE_ATTR_RO(field);
 
 /* for more info, see Documentation/ABI/testing/sysfs-bus-rpmsg */
 rpmsg_show_attr(name, id.name, "%s\n");
@@ -345,15 +346,17 @@ static ssize_t modalias_show(struct device *dev,
 
 	return sprintf(buf, RPMSG_DEVICE_MODALIAS_FMT "\n", rpdev->id.name);
 }
-
-static struct device_attribute rpmsg_dev_attrs[] = {
-	__ATTR_RO(name),
-	__ATTR_RO(modalias),
-	__ATTR_RO(dst),
-	__ATTR_RO(src),
-	__ATTR_RO(announce),
-	__ATTR_NULL
+static DEVICE_ATTR_RO(modalias);
+
+static struct attribute *rpmsg_dev_attrs[] = {
+	&dev_attr_name.attr,
+	&dev_attr_modalias.attr,
+	&dev_attr_dst.attr,
+	&dev_attr_src.attr,
+	&dev_attr_announce.attr,
+	NULL,
 };
+ATTRIBUTE_GROUPS(rpmsg_dev);
 
 /* rpmsg devices and drivers are matched using the service name */
 static inline int rpmsg_id_match(const struct rpmsg_device *rpdev,
@@ -455,7 +458,7 @@ static int rpmsg_dev_remove(struct device *dev)
 static struct bus_type rpmsg_bus = {
 	.name		= "rpmsg",
 	.match		= rpmsg_dev_match,
-	.dev_attrs	= rpmsg_dev_attrs,
+	.dev_groups	= rpmsg_dev_groups,
 	.uevent		= rpmsg_uevent,
 	.probe		= rpmsg_dev_probe,
 	.remove		= rpmsg_dev_remove,
diff --git a/drivers/rtc/rtc-imxdi.c b/drivers/rtc/rtc-imxdi.c
index 6b54f6c24c5f..80931114c899 100644
--- a/drivers/rtc/rtc-imxdi.c
+++ b/drivers/rtc/rtc-imxdi.c
@@ -709,7 +709,7 @@ static irqreturn_t dryice_irq(int irq, void *dev_id)
 		/*If the write wait queue is empty then there is no pending
 		  operations. It means the interrupt is for DryIce -Security.
 		  IRQ must be returned as none.*/
-		if (list_empty_careful(&imxdi->write_wait.task_list))
+		if (list_empty_careful(&imxdi->write_wait.head))
 			return rc;
 
 		/* DSR_WCF clears itself on DSR read */
diff --git a/drivers/s390/block/Kconfig b/drivers/s390/block/Kconfig
index 0acb8c2f9475..31f014b57bfc 100644
--- a/drivers/s390/block/Kconfig
+++ b/drivers/s390/block/Kconfig
@@ -82,10 +82,3 @@ config SCM_BLOCK
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called scm_block.
-
-config SCM_BLOCK_CLUSTER_WRITE
-	def_bool y
-	prompt "SCM force cluster writes"
-	depends on SCM_BLOCK
-	help
-	  Force writes to Storage Class Memory (SCM) to be in done in clusters.
diff --git a/drivers/s390/block/Makefile b/drivers/s390/block/Makefile
index c2f4e673e031..b64e2b32c753 100644
--- a/drivers/s390/block/Makefile
+++ b/drivers/s390/block/Makefile
@@ -19,7 +19,4 @@ obj-$(CONFIG_BLK_DEV_XPRAM) += xpram.o
 obj-$(CONFIG_DCSSBLK) += dcssblk.o
 
 scm_block-objs := scm_drv.o scm_blk.o
-ifdef CONFIG_SCM_BLOCK_CLUSTER_WRITE
-scm_block-objs += scm_blk_cluster.o
-endif
 obj-$(CONFIG_SCM_BLOCK) += scm_block.o
diff --git a/drivers/s390/block/dasd.c b/drivers/s390/block/dasd.c
index 6fb3fd5efc11..0f1fe4ff7f51 100644
--- a/drivers/s390/block/dasd.c
+++ b/drivers/s390/block/dasd.c
@@ -1965,8 +1965,12 @@ static int __dasd_device_is_unusable(struct dasd_device *device,
 {
 	int mask = ~(DASD_STOPPED_DC_WAIT | DASD_UNRESUMED_PM);
 
-	if (test_bit(DASD_FLAG_OFFLINE, &device->flags)) {
-		/* dasd is being set offline. */
+	if (test_bit(DASD_FLAG_OFFLINE, &device->flags) &&
+	    !test_bit(DASD_FLAG_SAFE_OFFLINE_RUNNING, &device->flags)) {
+		/*
+		 * dasd is being set offline
+		 * but it is no safe offline where we have to allow I/O
+		 */
 		return 1;
 	}
 	if (device->stopped) {
@@ -2672,7 +2676,7 @@ static void __dasd_process_request_queue(struct dasd_block *block)
 	 */
 	if (basedev->state < DASD_STATE_READY) {
 		while ((req = blk_fetch_request(block->request_queue)))
-			__blk_end_request_all(req, -EIO);
+			__blk_end_request_all(req, BLK_STS_IOERR);
 		return;
 	}
 
@@ -2692,7 +2696,7 @@ static void __dasd_process_request_queue(struct dasd_block *block)
 				      "Rejecting write request %p",
 				      req);
 			blk_start_request(req);
-			__blk_end_request_all(req, -EIO);
+			__blk_end_request_all(req, BLK_STS_IOERR);
 			continue;
 		}
 		if (test_bit(DASD_FLAG_ABORTALL, &basedev->flags) &&
@@ -2702,7 +2706,7 @@ static void __dasd_process_request_queue(struct dasd_block *block)
 				      "Rejecting failfast request %p",
 				      req);
 			blk_start_request(req);
-			__blk_end_request_all(req, -ETIMEDOUT);
+			__blk_end_request_all(req, BLK_STS_TIMEOUT);
 			continue;
 		}
 		cqr = basedev->discipline->build_cp(basedev, block, req);
@@ -2734,7 +2738,7 @@ static void __dasd_process_request_queue(struct dasd_block *block)
 				      "on request %p",
 				      PTR_ERR(cqr), req);
 			blk_start_request(req);
-			__blk_end_request_all(req, -EIO);
+			__blk_end_request_all(req, BLK_STS_IOERR);
 			continue;
 		}
 		/*
@@ -2755,21 +2759,29 @@ static void __dasd_cleanup_cqr(struct dasd_ccw_req *cqr)
 {
 	struct request *req;
 	int status;
-	int error = 0;
+	blk_status_t error = BLK_STS_OK;
 
 	req = (struct request *) cqr->callback_data;
 	dasd_profile_end(cqr->block, cqr, req);
+
 	status = cqr->block->base->discipline->free_cp(cqr, req);
 	if (status < 0)
-		error = status;
+		error = errno_to_blk_status(status);
 	else if (status == 0) {
-		if (cqr->intrc == -EPERM)
-			error = -EBADE;
-		else if (cqr->intrc == -ENOLINK ||
-			 cqr->intrc == -ETIMEDOUT)
-			error = cqr->intrc;
-		else
-			error = -EIO;
+		switch (cqr->intrc) {
+		case -EPERM:
+			error = BLK_STS_NEXUS;
+			break;
+		case -ENOLINK:
+			error = BLK_STS_TRANSPORT;
+			break;
+		case -ETIMEDOUT:
+			error = BLK_STS_TIMEOUT;
+			break;
+		default:
+			error = BLK_STS_IOERR;
+			break;
+		}
 	}
 	__blk_end_request_all(req, error);
 }
@@ -3190,7 +3202,7 @@ static void dasd_flush_request_queue(struct dasd_block *block)
 
 	spin_lock_irq(&block->request_queue_lock);
 	while ((req = blk_fetch_request(block->request_queue)))
-		__blk_end_request_all(req, -EIO);
+		__blk_end_request_all(req, BLK_STS_IOERR);
 	spin_unlock_irq(&block->request_queue_lock);
 }
 
@@ -3562,57 +3574,69 @@ int dasd_generic_set_offline(struct ccw_device *cdev)
 			else
 				pr_warn("%s: The DASD cannot be set offline while it is in use\n",
 					dev_name(&cdev->dev));
-			clear_bit(DASD_FLAG_OFFLINE, &device->flags);
-			goto out_busy;
+			rc = -EBUSY;
+			goto out_err;
 		}
 	}
 
-	if (test_bit(DASD_FLAG_SAFE_OFFLINE_RUNNING, &device->flags)) {
-		/*
-		 * safe offline already running
-		 * could only be called by normal offline so safe_offline flag
-		 * needs to be removed to run normal offline and kill all I/O
-		 */
-		if (test_and_set_bit(DASD_FLAG_OFFLINE, &device->flags))
-			/* Already doing normal offline processing */
-			goto out_busy;
-		else
-			clear_bit(DASD_FLAG_SAFE_OFFLINE, &device->flags);
-	} else {
-		if (test_bit(DASD_FLAG_OFFLINE, &device->flags))
-			/* Already doing offline processing */
-			goto out_busy;
+	/*
+	 * Test if the offline processing is already running and exit if so.
+	 * If a safe offline is being processed this could only be a normal
+	 * offline that should be able to overtake the safe offline and
+	 * cancel any I/O we do not want to wait for any longer
+	 */
+	if (test_bit(DASD_FLAG_OFFLINE, &device->flags)) {
+		if (test_bit(DASD_FLAG_SAFE_OFFLINE_RUNNING, &device->flags)) {
+			clear_bit(DASD_FLAG_SAFE_OFFLINE_RUNNING,
+				  &device->flags);
+		} else {
+			rc = -EBUSY;
+			goto out_err;
+		}
 	}
-
 	set_bit(DASD_FLAG_OFFLINE, &device->flags);
-	spin_unlock_irqrestore(get_ccwdev_lock(cdev), flags);
 
 	/*
-	 * if safe_offline called set safe_offline_running flag and
+	 * if safe_offline is called set safe_offline_running flag and
 	 * clear safe_offline so that a call to normal offline
 	 * can overrun safe_offline processing
 	 */
 	if (test_and_clear_bit(DASD_FLAG_SAFE_OFFLINE, &device->flags) &&
 	    !test_and_set_bit(DASD_FLAG_SAFE_OFFLINE_RUNNING, &device->flags)) {
+		/* need to unlock here to wait for outstanding I/O */
+		spin_unlock_irqrestore(get_ccwdev_lock(cdev), flags);
 		/*
 		 * If we want to set the device safe offline all IO operations
 		 * should be finished before continuing the offline process
 		 * so sync bdev first and then wait for our queues to become
 		 * empty
 		 */
-		/* sync blockdev and partitions */
 		if (device->block) {
 			rc = fsync_bdev(device->block->bdev);
 			if (rc != 0)
 				goto interrupted;
 		}
-		/* schedule device tasklet and wait for completion */
 		dasd_schedule_device_bh(device);
 		rc = wait_event_interruptible(shutdown_waitq,
 					      _wait_for_empty_queues(device));
 		if (rc != 0)
 			goto interrupted;
+
+		/*
+		 * check if a normal offline process overtook the offline
+		 * processing in this case simply do nothing beside returning
+		 * that we got interrupted
+		 * otherwise mark safe offline as not running any longer and
+		 * continue with normal offline
+		 */
+		spin_lock_irqsave(get_ccwdev_lock(cdev), flags);
+		if (!test_bit(DASD_FLAG_SAFE_OFFLINE_RUNNING, &device->flags)) {
+			rc = -ERESTARTSYS;
+			goto out_err;
+		}
+		clear_bit(DASD_FLAG_SAFE_OFFLINE_RUNNING, &device->flags);
 	}
+	spin_unlock_irqrestore(get_ccwdev_lock(cdev), flags);
 
 	dasd_set_target_state(device, DASD_STATE_NEW);
 	/* dasd_delete_device destroys the device reference. */
@@ -3624,22 +3648,18 @@ int dasd_generic_set_offline(struct ccw_device *cdev)
 	 */
 	if (block)
 		dasd_free_block(block);
+
 	return 0;
 
 interrupted:
 	/* interrupted by signal */
-	clear_bit(DASD_FLAG_SAFE_OFFLINE, &device->flags);
+	spin_lock_irqsave(get_ccwdev_lock(cdev), flags);
 	clear_bit(DASD_FLAG_SAFE_OFFLINE_RUNNING, &device->flags);
 	clear_bit(DASD_FLAG_OFFLINE, &device->flags);
-	dasd_put_device(device);
-
-	return rc;
-
-out_busy:
+out_err:
 	dasd_put_device(device);
 	spin_unlock_irqrestore(get_ccwdev_lock(cdev), flags);
-
-	return -EBUSY;
+	return rc;
 }
 EXPORT_SYMBOL_GPL(dasd_generic_set_offline);
 
diff --git a/drivers/s390/block/dasd_devmap.c b/drivers/s390/block/dasd_devmap.c
index 1164b51d09f3..7c7351276d2e 100644
--- a/drivers/s390/block/dasd_devmap.c
+++ b/drivers/s390/block/dasd_devmap.c
@@ -315,45 +315,58 @@ static int __init dasd_parse_range(const char *range)
 	char *features_str = NULL;
 	char *from_str = NULL;
 	char *to_str = NULL;
-	size_t len = strlen(range) + 1;
-	char tmp[len];
+	int rc = 0;
+	char *tmp;
 
-	strlcpy(tmp, range, len);
+	tmp = kstrdup(range, GFP_KERNEL);
+	if (!tmp)
+		return -ENOMEM;
 
-	if (dasd_evaluate_range_param(tmp, &from_str, &to_str, &features_str))
-		goto out_err;
+	if (dasd_evaluate_range_param(tmp, &from_str, &to_str, &features_str)) {
+		rc = -EINVAL;
+		goto out;
+	}
 
-	if (dasd_busid(from_str, &from_id0, &from_id1, &from))
-		goto out_err;
+	if (dasd_busid(from_str, &from_id0, &from_id1, &from)) {
+		rc = -EINVAL;
+		goto out;
+	}
 
 	to = from;
 	to_id0 = from_id0;
 	to_id1 = from_id1;
 	if (to_str) {
-		if (dasd_busid(to_str, &to_id0, &to_id1, &to))
-			goto out_err;
+		if (dasd_busid(to_str, &to_id0, &to_id1, &to)) {
+			rc = -EINVAL;
+			goto out;
+		}
 		if (from_id0 != to_id0 || from_id1 != to_id1 || from > to) {
 			pr_err("%s is not a valid device range\n", range);
-			goto out_err;
+			rc = -EINVAL;
+			goto out;
 		}
 	}
 
 	features = dasd_feature_list(features_str);
-	if (features < 0)
-		goto out_err;
+	if (features < 0) {
+		rc = -EINVAL;
+		goto out;
+	}
 	/* each device in dasd= parameter should be set initially online */
 	features |= DASD_FEATURE_INITIAL_ONLINE;
 	while (from <= to) {
 		sprintf(bus_id, "%01x.%01x.%04x", from_id0, from_id1, from++);
 		devmap = dasd_add_busid(bus_id, features);
-		if (IS_ERR(devmap))
-			return PTR_ERR(devmap);
+		if (IS_ERR(devmap)) {
+			rc = PTR_ERR(devmap);
+			goto out;
+		}
 	}
 
-	return 0;
+out:
+	kfree(tmp);
 
-out_err:
-	return -EINVAL;
+	return rc;
 }
 
 /*
@@ -735,13 +748,22 @@ static ssize_t
 dasd_ro_show(struct device *dev, struct device_attribute *attr, char *buf)
 {
 	struct dasd_devmap *devmap;
-	int ro_flag;
+	struct dasd_device *device;
+	int ro_flag = 0;
 
 	devmap = dasd_find_busid(dev_name(dev));
-	if (!IS_ERR(devmap))
-		ro_flag = (devmap->features & DASD_FEATURE_READONLY) != 0;
-	else
-		ro_flag = (DASD_FEATURE_DEFAULT & DASD_FEATURE_READONLY) != 0;
+	if (IS_ERR(devmap))
+		goto out;
+
+	ro_flag = !!(devmap->features & DASD_FEATURE_READONLY);
+
+	spin_lock(&dasd_devmap_lock);
+	device = devmap->device;
+	if (device)
+		ro_flag |= test_bit(DASD_FLAG_DEVICE_RO, &device->flags);
+	spin_unlock(&dasd_devmap_lock);
+
+out:
 	return snprintf(buf, PAGE_SIZE, ro_flag ? "1\n" : "0\n");
 }
 
@@ -764,7 +786,7 @@ dasd_ro_store(struct device *dev, struct device_attribute *attr,
 
 	device = dasd_device_from_cdev(cdev);
 	if (IS_ERR(device))
-		return PTR_ERR(device);
+		return count;
 
 	spin_lock_irqsave(get_ccwdev_lock(cdev), flags);
 	val = val || test_bit(DASD_FLAG_DEVICE_RO, &device->flags);
@@ -928,11 +950,14 @@ dasd_safe_offline_store(struct device *dev, struct device_attribute *attr,
 {
 	struct ccw_device *cdev = to_ccwdev(dev);
 	struct dasd_device *device;
+	unsigned long flags;
 	int rc;
 
-	device = dasd_device_from_cdev(cdev);
+	spin_lock_irqsave(get_ccwdev_lock(cdev), flags);
+	device = dasd_device_from_cdev_locked(cdev);
 	if (IS_ERR(device)) {
 		rc = PTR_ERR(device);
+		spin_unlock_irqrestore(get_ccwdev_lock(cdev), flags);
 		goto out;
 	}
 
@@ -940,12 +965,14 @@ dasd_safe_offline_store(struct device *dev, struct device_attribute *attr,
 	    test_bit(DASD_FLAG_SAFE_OFFLINE_RUNNING, &device->flags)) {
 		/* Already doing offline processing */
 		dasd_put_device(device);
+		spin_unlock_irqrestore(get_ccwdev_lock(cdev), flags);
 		rc = -EBUSY;
 		goto out;
 	}
 
 	set_bit(DASD_FLAG_SAFE_OFFLINE, &device->flags);
 	dasd_put_device(device);
+	spin_unlock_irqrestore(get_ccwdev_lock(cdev), flags);
 
 	rc = ccw_device_set_offline(cdev);
 
diff --git a/drivers/s390/block/dcssblk.c b/drivers/s390/block/dcssblk.c
index 36e5280af3e4..06eb1de52d1c 100644
--- a/drivers/s390/block/dcssblk.c
+++ b/drivers/s390/block/dcssblk.c
@@ -845,7 +845,7 @@ dcssblk_make_request(struct request_queue *q, struct bio *bio)
 	unsigned long source_addr;
 	unsigned long bytes_done;
 
-	blk_queue_split(q, &bio, q->bio_split);
+	blk_queue_split(q, &bio);
 
 	bytes_done = 0;
 	dev_info = bio->bi_bdev->bd_disk->private_data;
diff --git a/drivers/s390/block/scm_blk.c b/drivers/s390/block/scm_blk.c
index 152de6817875..42018a20f2b7 100644
--- a/drivers/s390/block/scm_blk.c
+++ b/drivers/s390/block/scm_blk.c
@@ -13,6 +13,7 @@
 #include <linux/mempool.h>
 #include <linux/module.h>
 #include <linux/blkdev.h>
+#include <linux/blk-mq.h>
 #include <linux/genhd.h>
 #include <linux/slab.h>
 #include <linux/list.h>
@@ -42,7 +43,6 @@ static void __scm_free_rq(struct scm_request *scmrq)
 	struct aob_rq_header *aobrq = to_aobrq(scmrq);
 
 	free_page((unsigned long) scmrq->aob);
-	__scm_free_rq_cluster(scmrq);
 	kfree(scmrq->request);
 	kfree(aobrq);
 }
@@ -82,9 +82,6 @@ static int __scm_alloc_rq(void)
 	if (!scmrq->request)
 		goto free;
 
-	if (__scm_alloc_rq_cluster(scmrq))
-		goto free;
-
 	INIT_LIST_HEAD(&scmrq->list);
 	spin_lock_irq(&list_lock);
 	list_add(&scmrq->list, &inactive_requests);
@@ -114,13 +111,13 @@ static struct scm_request *scm_request_fetch(void)
 {
 	struct scm_request *scmrq = NULL;
 
-	spin_lock(&list_lock);
+	spin_lock_irq(&list_lock);
 	if (list_empty(&inactive_requests))
 		goto out;
 	scmrq = list_first_entry(&inactive_requests, struct scm_request, list);
 	list_del(&scmrq->list);
 out:
-	spin_unlock(&list_lock);
+	spin_unlock_irq(&list_lock);
 	return scmrq;
 }
 
@@ -231,140 +228,133 @@ static inline void scm_request_init(struct scm_blk_dev *bdev,
 	aob->request.data = (u64) aobrq;
 	scmrq->bdev = bdev;
 	scmrq->retries = 4;
-	scmrq->error = 0;
+	scmrq->error = BLK_STS_OK;
 	/* We don't use all msbs - place aidaws at the end of the aob page. */
 	scmrq->next_aidaw = (void *) &aob->msb[nr_requests_per_io];
-	scm_request_cluster_init(scmrq);
 }
 
-static void scm_ensure_queue_restart(struct scm_blk_dev *bdev)
-{
-	if (atomic_read(&bdev->queued_reqs)) {
-		/* Queue restart is triggered by the next interrupt. */
-		return;
-	}
-	blk_delay_queue(bdev->rq, SCM_QUEUE_DELAY);
-}
-
-void scm_request_requeue(struct scm_request *scmrq)
+static void scm_request_requeue(struct scm_request *scmrq)
 {
 	struct scm_blk_dev *bdev = scmrq->bdev;
 	int i;
 
-	scm_release_cluster(scmrq);
 	for (i = 0; i < nr_requests_per_io && scmrq->request[i]; i++)
-		blk_requeue_request(bdev->rq, scmrq->request[i]);
+		blk_mq_requeue_request(scmrq->request[i], false);
 
 	atomic_dec(&bdev->queued_reqs);
 	scm_request_done(scmrq);
-	scm_ensure_queue_restart(bdev);
+	blk_mq_kick_requeue_list(bdev->rq);
 }
 
-void scm_request_finish(struct scm_request *scmrq)
+static void scm_request_finish(struct scm_request *scmrq)
 {
 	struct scm_blk_dev *bdev = scmrq->bdev;
 	int i;
 
-	scm_release_cluster(scmrq);
-	for (i = 0; i < nr_requests_per_io && scmrq->request[i]; i++)
-		blk_end_request_all(scmrq->request[i], scmrq->error);
+	for (i = 0; i < nr_requests_per_io && scmrq->request[i]; i++) {
+		if (scmrq->error)
+			blk_mq_end_request(scmrq->request[i], scmrq->error);
+		else
+			blk_mq_complete_request(scmrq->request[i]);
+	}
 
 	atomic_dec(&bdev->queued_reqs);
 	scm_request_done(scmrq);
 }
 
-static int scm_request_start(struct scm_request *scmrq)
+static void scm_request_start(struct scm_request *scmrq)
 {
 	struct scm_blk_dev *bdev = scmrq->bdev;
-	int ret;
 
 	atomic_inc(&bdev->queued_reqs);
-	if (!scmrq->aob->request.msb_count) {
-		scm_request_requeue(scmrq);
-		return -EINVAL;
-	}
-
-	ret = eadm_start_aob(scmrq->aob);
-	if (ret) {
+	if (eadm_start_aob(scmrq->aob)) {
 		SCM_LOG(5, "no subchannel");
 		scm_request_requeue(scmrq);
 	}
-	return ret;
 }
 
-static void scm_blk_request(struct request_queue *rq)
+struct scm_queue {
+	struct scm_request *scmrq;
+	spinlock_t lock;
+};
+
+static int scm_blk_request(struct blk_mq_hw_ctx *hctx,
+			   const struct blk_mq_queue_data *qd)
 {
-	struct scm_device *scmdev = rq->queuedata;
+	struct scm_device *scmdev = hctx->queue->queuedata;
 	struct scm_blk_dev *bdev = dev_get_drvdata(&scmdev->dev);
-	struct scm_request *scmrq = NULL;
-	struct request *req;
+	struct scm_queue *sq = hctx->driver_data;
+	struct request *req = qd->rq;
+	struct scm_request *scmrq;
 
-	while ((req = blk_peek_request(rq))) {
-		if (!scm_permit_request(bdev, req))
-			goto out;
+	spin_lock(&sq->lock);
+	if (!scm_permit_request(bdev, req)) {
+		spin_unlock(&sq->lock);
+		return BLK_MQ_RQ_QUEUE_BUSY;
+	}
 
+	scmrq = sq->scmrq;
+	if (!scmrq) {
+		scmrq = scm_request_fetch();
 		if (!scmrq) {
-			scmrq = scm_request_fetch();
-			if (!scmrq) {
-				SCM_LOG(5, "no request");
-				goto out;
-			}
-			scm_request_init(bdev, scmrq);
+			SCM_LOG(5, "no request");
+			spin_unlock(&sq->lock);
+			return BLK_MQ_RQ_QUEUE_BUSY;
 		}
-		scm_request_set(scmrq, req);
+		scm_request_init(bdev, scmrq);
+		sq->scmrq = scmrq;
+	}
+	scm_request_set(scmrq, req);
 
-		if (!scm_reserve_cluster(scmrq)) {
-			SCM_LOG(5, "cluster busy");
-			scm_request_set(scmrq, NULL);
-			if (scmrq->aob->request.msb_count)
-				goto out;
+	if (scm_request_prepare(scmrq)) {
+		SCM_LOG(5, "aidaw alloc failed");
+		scm_request_set(scmrq, NULL);
 
-			scm_request_done(scmrq);
-			return;
-		}
+		if (scmrq->aob->request.msb_count)
+			scm_request_start(scmrq);
 
-		if (scm_need_cluster_request(scmrq)) {
-			if (scmrq->aob->request.msb_count) {
-				/* Start cluster requests separately. */
-				scm_request_set(scmrq, NULL);
-				if (scm_request_start(scmrq))
-					return;
-			} else {
-				atomic_inc(&bdev->queued_reqs);
-				blk_start_request(req);
-				scm_initiate_cluster_request(scmrq);
-			}
-			scmrq = NULL;
-			continue;
-		}
+		sq->scmrq = NULL;
+		spin_unlock(&sq->lock);
+		return BLK_MQ_RQ_QUEUE_BUSY;
+	}
+	blk_mq_start_request(req);
 
-		if (scm_request_prepare(scmrq)) {
-			SCM_LOG(5, "aidaw alloc failed");
-			scm_request_set(scmrq, NULL);
-			goto out;
-		}
-		blk_start_request(req);
+	if (qd->last || scmrq->aob->request.msb_count == nr_requests_per_io) {
+		scm_request_start(scmrq);
+		sq->scmrq = NULL;
+	}
+	spin_unlock(&sq->lock);
+	return BLK_MQ_RQ_QUEUE_OK;
+}
 
-		if (scmrq->aob->request.msb_count < nr_requests_per_io)
-			continue;
+static int scm_blk_init_hctx(struct blk_mq_hw_ctx *hctx, void *data,
+			     unsigned int idx)
+{
+	struct scm_queue *qd = kzalloc(sizeof(*qd), GFP_KERNEL);
 
-		if (scm_request_start(scmrq))
-			return;
+	if (!qd)
+		return -ENOMEM;
 
-		scmrq = NULL;
-	}
-out:
-	if (scmrq)
-		scm_request_start(scmrq);
-	else
-		scm_ensure_queue_restart(bdev);
+	spin_lock_init(&qd->lock);
+	hctx->driver_data = qd;
+
+	return 0;
+}
+
+static void scm_blk_exit_hctx(struct blk_mq_hw_ctx *hctx, unsigned int idx)
+{
+	struct scm_queue *qd = hctx->driver_data;
+
+	WARN_ON(qd->scmrq);
+	kfree(hctx->driver_data);
+	hctx->driver_data = NULL;
 }
 
 static void __scmrq_log_error(struct scm_request *scmrq)
 {
 	struct aob *aob = scmrq->aob;
 
-	if (scmrq->error == -ETIMEDOUT)
+	if (scmrq->error == BLK_STS_TIMEOUT)
 		SCM_LOG(1, "Request timeout");
 	else {
 		SCM_LOG(1, "Request error");
@@ -377,27 +367,12 @@ static void __scmrq_log_error(struct scm_request *scmrq)
 		       scmrq->error);
 }
 
-void scm_blk_irq(struct scm_device *scmdev, void *data, int error)
-{
-	struct scm_request *scmrq = data;
-	struct scm_blk_dev *bdev = scmrq->bdev;
-
-	scmrq->error = error;
-	if (error)
-		__scmrq_log_error(scmrq);
-
-	spin_lock(&bdev->lock);
-	list_add_tail(&scmrq->list, &bdev->finished_requests);
-	spin_unlock(&bdev->lock);
-	tasklet_hi_schedule(&bdev->tasklet);
-}
-
 static void scm_blk_handle_error(struct scm_request *scmrq)
 {
 	struct scm_blk_dev *bdev = scmrq->bdev;
 	unsigned long flags;
 
-	if (scmrq->error != -EIO)
+	if (scmrq->error != BLK_STS_IOERR)
 		goto restart;
 
 	/* For -EIO the response block is valid. */
@@ -419,54 +394,46 @@ restart:
 		return;
 
 requeue:
-	spin_lock_irqsave(&bdev->rq_lock, flags);
 	scm_request_requeue(scmrq);
-	spin_unlock_irqrestore(&bdev->rq_lock, flags);
 }
 
-static void scm_blk_tasklet(struct scm_blk_dev *bdev)
+void scm_blk_irq(struct scm_device *scmdev, void *data, blk_status_t error)
 {
-	struct scm_request *scmrq;
-	unsigned long flags;
-
-	spin_lock_irqsave(&bdev->lock, flags);
-	while (!list_empty(&bdev->finished_requests)) {
-		scmrq = list_first_entry(&bdev->finished_requests,
-					 struct scm_request, list);
-		list_del(&scmrq->list);
-		spin_unlock_irqrestore(&bdev->lock, flags);
+	struct scm_request *scmrq = data;
 
-		if (scmrq->error && scmrq->retries-- > 0) {
+	scmrq->error = error;
+	if (error) {
+		__scmrq_log_error(scmrq);
+		if (scmrq->retries-- > 0) {
 			scm_blk_handle_error(scmrq);
-
-			/* Request restarted or requeued, handle next. */
-			spin_lock_irqsave(&bdev->lock, flags);
-			continue;
+			return;
 		}
+	}
 
-		if (scm_test_cluster_request(scmrq)) {
-			scm_cluster_request_irq(scmrq);
-			spin_lock_irqsave(&bdev->lock, flags);
-			continue;
-		}
+	scm_request_finish(scmrq);
+}
 
-		scm_request_finish(scmrq);
-		spin_lock_irqsave(&bdev->lock, flags);
-	}
-	spin_unlock_irqrestore(&bdev->lock, flags);
-	/* Look out for more requests. */
-	blk_run_queue(bdev->rq);
+static void scm_blk_request_done(struct request *req)
+{
+	blk_mq_end_request(req, 0);
 }
 
 static const struct block_device_operations scm_blk_devops = {
 	.owner = THIS_MODULE,
 };
 
+static const struct blk_mq_ops scm_mq_ops = {
+	.queue_rq = scm_blk_request,
+	.complete = scm_blk_request_done,
+	.init_hctx = scm_blk_init_hctx,
+	.exit_hctx = scm_blk_exit_hctx,
+};
+
 int scm_blk_dev_setup(struct scm_blk_dev *bdev, struct scm_device *scmdev)
 {
-	struct request_queue *rq;
-	int len, ret = -ENOMEM;
 	unsigned int devindex, nr_max_blk;
+	struct request_queue *rq;
+	int len, ret;
 
 	devindex = atomic_inc_return(&nr_devices) - 1;
 	/* scma..scmz + scmaa..scmzz */
@@ -477,18 +444,23 @@ int scm_blk_dev_setup(struct scm_blk_dev *bdev, struct scm_device *scmdev)
 
 	bdev->scmdev = scmdev;
 	bdev->state = SCM_OPER;
-	spin_lock_init(&bdev->rq_lock);
 	spin_lock_init(&bdev->lock);
-	INIT_LIST_HEAD(&bdev->finished_requests);
 	atomic_set(&bdev->queued_reqs, 0);
-	tasklet_init(&bdev->tasklet,
-		     (void (*)(unsigned long)) scm_blk_tasklet,
-		     (unsigned long) bdev);
 
-	rq = blk_init_queue(scm_blk_request, &bdev->rq_lock);
-	if (!rq)
+	bdev->tag_set.ops = &scm_mq_ops;
+	bdev->tag_set.nr_hw_queues = nr_requests;
+	bdev->tag_set.queue_depth = nr_requests_per_io * nr_requests;
+	bdev->tag_set.flags = BLK_MQ_F_SHOULD_MERGE;
+
+	ret = blk_mq_alloc_tag_set(&bdev->tag_set);
+	if (ret)
 		goto out;
 
+	rq = blk_mq_init_queue(&bdev->tag_set);
+	if (IS_ERR(rq)) {
+		ret = PTR_ERR(rq);
+		goto out_tag;
+	}
 	bdev->rq = rq;
 	nr_max_blk = min(scmdev->nr_max_block,
 			 (unsigned int) (PAGE_SIZE / sizeof(struct aidaw)));
@@ -498,12 +470,12 @@ int scm_blk_dev_setup(struct scm_blk_dev *bdev, struct scm_device *scmdev)
 	blk_queue_max_segments(rq, nr_max_blk);
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, rq);
 	queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, rq);
-	scm_blk_dev_cluster_setup(bdev);
 
 	bdev->gendisk = alloc_disk(SCM_NR_PARTS);
-	if (!bdev->gendisk)
+	if (!bdev->gendisk) {
+		ret = -ENOMEM;
 		goto out_queue;
-
+	}
 	rq->queuedata = scmdev;
 	bdev->gendisk->private_data = scmdev;
 	bdev->gendisk->fops = &scm_blk_devops;
@@ -528,6 +500,8 @@ int scm_blk_dev_setup(struct scm_blk_dev *bdev, struct scm_device *scmdev)
 
 out_queue:
 	blk_cleanup_queue(rq);
+out_tag:
+	blk_mq_free_tag_set(&bdev->tag_set);
 out:
 	atomic_dec(&nr_devices);
 	return ret;
@@ -535,9 +509,9 @@ out:
 
 void scm_blk_dev_cleanup(struct scm_blk_dev *bdev)
 {
-	tasklet_kill(&bdev->tasklet);
 	del_gendisk(bdev->gendisk);
 	blk_cleanup_queue(bdev->gendisk->queue);
+	blk_mq_free_tag_set(&bdev->tag_set);
 	put_disk(bdev->gendisk);
 }
 
@@ -558,7 +532,7 @@ static bool __init scm_blk_params_valid(void)
 	if (!nr_requests_per_io || nr_requests_per_io > 64)
 		return false;
 
-	return scm_cluster_size_valid();
+	return true;
 }
 
 static int __init scm_blk_init(void)
diff --git a/drivers/s390/block/scm_blk.h b/drivers/s390/block/scm_blk.h
index 09218cdc5129..71288dd9dd7f 100644
--- a/drivers/s390/block/scm_blk.h
+++ b/drivers/s390/block/scm_blk.h
@@ -4,6 +4,7 @@
 #include <linux/interrupt.h>
 #include <linux/spinlock.h>
 #include <linux/blkdev.h>
+#include <linux/blk-mq.h>
 #include <linux/genhd.h>
 #include <linux/list.h>
 
@@ -14,18 +15,14 @@
 #define SCM_QUEUE_DELAY 5
 
 struct scm_blk_dev {
-	struct tasklet_struct tasklet;
 	struct request_queue *rq;
 	struct gendisk *gendisk;
+	struct blk_mq_tag_set tag_set;
 	struct scm_device *scmdev;
-	spinlock_t rq_lock;	/* guard the request queue */
-	spinlock_t lock;	/* guard the rest of the blockdev */
+	spinlock_t lock;
 	atomic_t queued_reqs;
 	enum {SCM_OPER, SCM_WR_PROHIBIT} state;
 	struct list_head finished_requests;
-#ifdef CONFIG_SCM_BLOCK_CLUSTER_WRITE
-	struct list_head cluster_list;
-#endif
 };
 
 struct scm_request {
@@ -35,14 +32,7 @@ struct scm_request {
 	struct aob *aob;
 	struct list_head list;
 	u8 retries;
-	int error;
-#ifdef CONFIG_SCM_BLOCK_CLUSTER_WRITE
-	struct {
-		enum {CLUSTER_NONE, CLUSTER_READ, CLUSTER_WRITE} state;
-		struct list_head list;
-		void **buf;
-	} cluster;
-#endif
+	blk_status_t error;
 };
 
 #define to_aobrq(rq) container_of((void *) rq, struct aob_rq_header, data)
@@ -50,57 +40,13 @@ struct scm_request {
 int scm_blk_dev_setup(struct scm_blk_dev *, struct scm_device *);
 void scm_blk_dev_cleanup(struct scm_blk_dev *);
 void scm_blk_set_available(struct scm_blk_dev *);
-void scm_blk_irq(struct scm_device *, void *, int);
-
-void scm_request_finish(struct scm_request *);
-void scm_request_requeue(struct scm_request *);
+void scm_blk_irq(struct scm_device *, void *, blk_status_t);
 
 struct aidaw *scm_aidaw_fetch(struct scm_request *scmrq, unsigned int bytes);
 
 int scm_drv_init(void);
 void scm_drv_cleanup(void);
 
-#ifdef CONFIG_SCM_BLOCK_CLUSTER_WRITE
-void __scm_free_rq_cluster(struct scm_request *);
-int __scm_alloc_rq_cluster(struct scm_request *);
-void scm_request_cluster_init(struct scm_request *);
-bool scm_reserve_cluster(struct scm_request *);
-void scm_release_cluster(struct scm_request *);
-void scm_blk_dev_cluster_setup(struct scm_blk_dev *);
-bool scm_need_cluster_request(struct scm_request *);
-void scm_initiate_cluster_request(struct scm_request *);
-void scm_cluster_request_irq(struct scm_request *);
-bool scm_test_cluster_request(struct scm_request *);
-bool scm_cluster_size_valid(void);
-#else /* CONFIG_SCM_BLOCK_CLUSTER_WRITE */
-static inline void __scm_free_rq_cluster(struct scm_request *scmrq) {}
-static inline int __scm_alloc_rq_cluster(struct scm_request *scmrq)
-{
-	return 0;
-}
-static inline void scm_request_cluster_init(struct scm_request *scmrq) {}
-static inline bool scm_reserve_cluster(struct scm_request *scmrq)
-{
-	return true;
-}
-static inline void scm_release_cluster(struct scm_request *scmrq) {}
-static inline void scm_blk_dev_cluster_setup(struct scm_blk_dev *bdev) {}
-static inline bool scm_need_cluster_request(struct scm_request *scmrq)
-{
-	return false;
-}
-static inline void scm_initiate_cluster_request(struct scm_request *scmrq) {}
-static inline void scm_cluster_request_irq(struct scm_request *scmrq) {}
-static inline bool scm_test_cluster_request(struct scm_request *scmrq)
-{
-	return false;
-}
-static inline bool scm_cluster_size_valid(void)
-{
-	return true;
-}
-#endif /* CONFIG_SCM_BLOCK_CLUSTER_WRITE */
-
 extern debug_info_t *scm_debug;
 
 #define SCM_LOG(imp, txt) do {					\
diff --git a/drivers/s390/block/scm_blk_cluster.c b/drivers/s390/block/scm_blk_cluster.c
deleted file mode 100644
index 7497ddde2dd6..000000000000
--- a/drivers/s390/block/scm_blk_cluster.c
+++ /dev/null
@@ -1,255 +0,0 @@
-/*
- * Block driver for s390 storage class memory.
- *
- * Copyright IBM Corp. 2012
- * Author(s): Sebastian Ott <sebott@linux.vnet.ibm.com>
- */
-
-#include <linux/spinlock.h>
-#include <linux/module.h>
-#include <linux/blkdev.h>
-#include <linux/genhd.h>
-#include <linux/slab.h>
-#include <linux/list.h>
-#include <asm/eadm.h>
-#include "scm_blk.h"
-
-static unsigned int write_cluster_size = 64;
-module_param(write_cluster_size, uint, S_IRUGO);
-MODULE_PARM_DESC(write_cluster_size,
-		 "Number of pages used for contiguous writes.");
-
-#define CLUSTER_SIZE (write_cluster_size * PAGE_SIZE)
-
-void __scm_free_rq_cluster(struct scm_request *scmrq)
-{
-	int i;
-
-	if (!scmrq->cluster.buf)
-		return;
-
-	for (i = 0; i < 2 * write_cluster_size; i++)
-		free_page((unsigned long) scmrq->cluster.buf[i]);
-
-	kfree(scmrq->cluster.buf);
-}
-
-int __scm_alloc_rq_cluster(struct scm_request *scmrq)
-{
-	int i;
-
-	scmrq->cluster.buf = kzalloc(sizeof(void *) * 2 * write_cluster_size,
-				 GFP_KERNEL);
-	if (!scmrq->cluster.buf)
-		return -ENOMEM;
-
-	for (i = 0; i < 2 * write_cluster_size; i++) {
-		scmrq->cluster.buf[i] = (void *) get_zeroed_page(GFP_DMA);
-		if (!scmrq->cluster.buf[i])
-			return -ENOMEM;
-	}
-	INIT_LIST_HEAD(&scmrq->cluster.list);
-	return 0;
-}
-
-void scm_request_cluster_init(struct scm_request *scmrq)
-{
-	scmrq->cluster.state = CLUSTER_NONE;
-}
-
-static bool clusters_intersect(struct request *A, struct request *B)
-{
-	unsigned long firstA, lastA, firstB, lastB;
-
-	firstA = ((u64) blk_rq_pos(A) << 9) / CLUSTER_SIZE;
-	lastA = (((u64) blk_rq_pos(A) << 9) +
-		    blk_rq_bytes(A) - 1) / CLUSTER_SIZE;
-
-	firstB = ((u64) blk_rq_pos(B) << 9) / CLUSTER_SIZE;
-	lastB = (((u64) blk_rq_pos(B) << 9) +
-		    blk_rq_bytes(B) - 1) / CLUSTER_SIZE;
-
-	return (firstB <= lastA && firstA <= lastB);
-}
-
-bool scm_reserve_cluster(struct scm_request *scmrq)
-{
-	struct request *req = scmrq->request[scmrq->aob->request.msb_count];
-	struct scm_blk_dev *bdev = scmrq->bdev;
-	struct scm_request *iter;
-	int pos, add = 1;
-
-	if (write_cluster_size == 0)
-		return true;
-
-	spin_lock(&bdev->lock);
-	list_for_each_entry(iter, &bdev->cluster_list, cluster.list) {
-		if (iter == scmrq) {
-			/*
-			 * We don't have to use clusters_intersect here, since
-			 * cluster requests are always started separately.
-			 */
-			add = 0;
-			continue;
-		}
-		for (pos = 0; pos < iter->aob->request.msb_count; pos++) {
-			if (clusters_intersect(req, iter->request[pos]) &&
-			    (rq_data_dir(req) == WRITE ||
-			     rq_data_dir(iter->request[pos]) == WRITE)) {
-				spin_unlock(&bdev->lock);
-				return false;
-			}
-		}
-	}
-	if (add)
-		list_add(&scmrq->cluster.list, &bdev->cluster_list);
-	spin_unlock(&bdev->lock);
-
-	return true;
-}
-
-void scm_release_cluster(struct scm_request *scmrq)
-{
-	struct scm_blk_dev *bdev = scmrq->bdev;
-	unsigned long flags;
-
-	if (write_cluster_size == 0)
-		return;
-
-	spin_lock_irqsave(&bdev->lock, flags);
-	list_del(&scmrq->cluster.list);
-	spin_unlock_irqrestore(&bdev->lock, flags);
-}
-
-void scm_blk_dev_cluster_setup(struct scm_blk_dev *bdev)
-{
-	INIT_LIST_HEAD(&bdev->cluster_list);
-	blk_queue_io_opt(bdev->rq, CLUSTER_SIZE);
-}
-
-static int scm_prepare_cluster_request(struct scm_request *scmrq)
-{
-	struct scm_blk_dev *bdev = scmrq->bdev;
-	struct scm_device *scmdev = bdev->gendisk->private_data;
-	struct request *req = scmrq->request[0];
-	struct msb *msb = &scmrq->aob->msb[0];
-	struct req_iterator iter;
-	struct aidaw *aidaw;
-	struct bio_vec bv;
-	int i = 0;
-	u64 addr;
-
-	switch (scmrq->cluster.state) {
-	case CLUSTER_NONE:
-		scmrq->cluster.state = CLUSTER_READ;
-		/* fall through */
-	case CLUSTER_READ:
-		msb->bs = MSB_BS_4K;
-		msb->oc = MSB_OC_READ;
-		msb->flags = MSB_FLAG_IDA;
-		msb->blk_count = write_cluster_size;
-
-		addr = scmdev->address + ((u64) blk_rq_pos(req) << 9);
-		msb->scm_addr = round_down(addr, CLUSTER_SIZE);
-
-		if (msb->scm_addr !=
-		    round_down(addr + (u64) blk_rq_bytes(req) - 1,
-			       CLUSTER_SIZE))
-			msb->blk_count = 2 * write_cluster_size;
-
-		aidaw = scm_aidaw_fetch(scmrq, msb->blk_count * PAGE_SIZE);
-		if (!aidaw)
-			return -ENOMEM;
-
-		scmrq->aob->request.msb_count = 1;
-		msb->data_addr = (u64) aidaw;
-		for (i = 0; i < msb->blk_count; i++) {
-			aidaw->data_addr = (u64) scmrq->cluster.buf[i];
-			aidaw++;
-		}
-
-		break;
-	case CLUSTER_WRITE:
-		aidaw = (void *) msb->data_addr;
-		msb->oc = MSB_OC_WRITE;
-
-		for (addr = msb->scm_addr;
-		     addr < scmdev->address + ((u64) blk_rq_pos(req) << 9);
-		     addr += PAGE_SIZE) {
-			aidaw->data_addr = (u64) scmrq->cluster.buf[i];
-			aidaw++;
-			i++;
-		}
-		rq_for_each_segment(bv, req, iter) {
-			aidaw->data_addr = (u64) page_address(bv.bv_page);
-			aidaw++;
-			i++;
-		}
-		for (; i < msb->blk_count; i++) {
-			aidaw->data_addr = (u64) scmrq->cluster.buf[i];
-			aidaw++;
-		}
-		break;
-	}
-	return 0;
-}
-
-bool scm_need_cluster_request(struct scm_request *scmrq)
-{
-	int pos = scmrq->aob->request.msb_count;
-
-	if (rq_data_dir(scmrq->request[pos]) == READ)
-		return false;
-
-	return blk_rq_bytes(scmrq->request[pos]) < CLUSTER_SIZE;
-}
-
-/* Called with queue lock held. */
-void scm_initiate_cluster_request(struct scm_request *scmrq)
-{
-	if (scm_prepare_cluster_request(scmrq))
-		goto requeue;
-	if (eadm_start_aob(scmrq->aob))
-		goto requeue;
-	return;
-requeue:
-	scm_request_requeue(scmrq);
-}
-
-bool scm_test_cluster_request(struct scm_request *scmrq)
-{
-	return scmrq->cluster.state != CLUSTER_NONE;
-}
-
-void scm_cluster_request_irq(struct scm_request *scmrq)
-{
-	struct scm_blk_dev *bdev = scmrq->bdev;
-	unsigned long flags;
-
-	switch (scmrq->cluster.state) {
-	case CLUSTER_NONE:
-		BUG();
-		break;
-	case CLUSTER_READ:
-		if (scmrq->error) {
-			scm_request_finish(scmrq);
-			break;
-		}
-		scmrq->cluster.state = CLUSTER_WRITE;
-		spin_lock_irqsave(&bdev->rq_lock, flags);
-		scm_initiate_cluster_request(scmrq);
-		spin_unlock_irqrestore(&bdev->rq_lock, flags);
-		break;
-	case CLUSTER_WRITE:
-		scm_request_finish(scmrq);
-		break;
-	}
-}
-
-bool scm_cluster_size_valid(void)
-{
-	if (write_cluster_size == 1 || write_cluster_size > 128)
-		return false;
-
-	return !(write_cluster_size & (write_cluster_size - 1));
-}
diff --git a/drivers/s390/block/xpram.c b/drivers/s390/block/xpram.c
index b9d7e755c8a3..a48f0d40c1d2 100644
--- a/drivers/s390/block/xpram.c
+++ b/drivers/s390/block/xpram.c
@@ -190,7 +190,7 @@ static blk_qc_t xpram_make_request(struct request_queue *q, struct bio *bio)
 	unsigned long page_addr;
 	unsigned long bytes;
 
-	blk_queue_split(q, &bio, q->bio_split);
+	blk_queue_split(q, &bio);
 
 	if ((bio->bi_iter.bi_sector & 7) != 0 ||
 	    (bio->bi_iter.bi_size & 4095) != 0)
diff --git a/drivers/s390/char/sclp.c b/drivers/s390/char/sclp.c
index 9c471ea1b99c..6111c1fa2d1e 100644
--- a/drivers/s390/char/sclp.c
+++ b/drivers/s390/char/sclp.c
@@ -1096,26 +1096,26 @@ static const struct dev_pm_ops sclp_pm_ops = {
 	.restore	= sclp_restore,
 };
 
-static ssize_t sclp_show_console_pages(struct device_driver *dev, char *buf)
+static ssize_t con_pages_show(struct device_driver *dev, char *buf)
 {
 	return sprintf(buf, "%i\n", sclp_console_pages);
 }
 
-static DRIVER_ATTR(con_pages, S_IRUSR, sclp_show_console_pages, NULL);
+static DRIVER_ATTR_RO(con_pages);
 
-static ssize_t sclp_show_con_drop(struct device_driver *dev, char *buf)
+static ssize_t con_drop_show(struct device_driver *dev, char *buf)
 {
 	return sprintf(buf, "%i\n", sclp_console_drop);
 }
 
-static DRIVER_ATTR(con_drop, S_IRUSR, sclp_show_con_drop, NULL);
+static DRIVER_ATTR_RO(con_drop);
 
-static ssize_t sclp_show_console_full(struct device_driver *dev, char *buf)
+static ssize_t con_full_show(struct device_driver *dev, char *buf)
 {
 	return sprintf(buf, "%lu\n", sclp_console_full);
 }
 
-static DRIVER_ATTR(con_full, S_IRUSR, sclp_show_console_full, NULL);
+static DRIVER_ATTR_RO(con_full);
 
 static struct attribute *sclp_drv_attrs[] = {
 	&driver_attr_con_pages.attr,
diff --git a/drivers/s390/char/vmlogrdr.c b/drivers/s390/char/vmlogrdr.c
index 57974a1e0e03..b19020b9efff 100644
--- a/drivers/s390/char/vmlogrdr.c
+++ b/drivers/s390/char/vmlogrdr.c
@@ -641,10 +641,8 @@ static ssize_t vmlogrdr_recording_store(struct device * dev,
 static DEVICE_ATTR(recording, 0200, NULL, vmlogrdr_recording_store);
 
 
-static ssize_t vmlogrdr_recording_status_show(struct device_driver *driver,
-					      char *buf)
+static ssize_t recording_status_show(struct device_driver *driver, char *buf)
 {
-
 	static const char cp_command[] = "QUERY RECORDING ";
 	int len;
 
@@ -652,8 +650,7 @@ 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 DRIVER_ATTR_RO(recording_status);
 static struct attribute *vmlogrdr_drv_attrs[] = {
 	&driver_attr_recording_status.attr,
 	NULL,
diff --git a/drivers/s390/cio/css.c b/drivers/s390/cio/css.c
index e2aa944eb566..d3e504c3c362 100644
--- a/drivers/s390/cio/css.c
+++ b/drivers/s390/cio/css.c
@@ -296,6 +296,51 @@ static const struct attribute_group *default_subch_attr_groups[] = {
 	NULL,
 };
 
+static ssize_t chpids_show(struct device *dev,
+			   struct device_attribute *attr,
+			   char *buf)
+{
+	struct subchannel *sch = to_subchannel(dev);
+	struct chsc_ssd_info *ssd = &sch->ssd_info;
+	ssize_t ret = 0;
+	int mask;
+	int chp;
+
+	for (chp = 0; chp < 8; chp++) {
+		mask = 0x80 >> chp;
+		if (ssd->path_mask & mask)
+			ret += sprintf(buf + ret, "%02x ", ssd->chpid[chp].id);
+		else
+			ret += sprintf(buf + ret, "00 ");
+	}
+	ret += sprintf(buf + ret, "\n");
+	return ret;
+}
+static DEVICE_ATTR(chpids, 0444, chpids_show, NULL);
+
+static ssize_t pimpampom_show(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	struct subchannel *sch = to_subchannel(dev);
+	struct pmcw *pmcw = &sch->schib.pmcw;
+
+	return sprintf(buf, "%02x %02x %02x\n",
+		       pmcw->pim, pmcw->pam, pmcw->pom);
+}
+static DEVICE_ATTR(pimpampom, 0444, pimpampom_show, NULL);
+
+static struct attribute *io_subchannel_type_attrs[] = {
+	&dev_attr_chpids.attr,
+	&dev_attr_pimpampom.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(io_subchannel_type);
+
+static const struct device_type io_subchannel_type = {
+	.groups = io_subchannel_type_groups,
+};
+
 int css_register_subchannel(struct subchannel *sch)
 {
 	int ret;
@@ -304,6 +349,10 @@ int css_register_subchannel(struct subchannel *sch)
 	sch->dev.parent = &channel_subsystems[0]->device;
 	sch->dev.bus = &css_bus_type;
 	sch->dev.groups = default_subch_attr_groups;
+
+	if (sch->st == SUBCHANNEL_TYPE_IO)
+		sch->dev.type = &io_subchannel_type;
+
 	/*
 	 * We don't want to generate uevents for I/O subchannels that don't
 	 * have a working ccw device behind them since they will be
diff --git a/drivers/s390/cio/device.c b/drivers/s390/cio/device.c
index b8006ea9099c..7be01a58b44f 100644
--- a/drivers/s390/cio/device.c
+++ b/drivers/s390/cio/device.c
@@ -208,44 +208,6 @@ int __init io_subchannel_init(void)
 
 /************************ device handling **************************/
 
-/*
- * A ccw_device has some interfaces in sysfs in addition to the
- * standard ones.
- * The following entries are designed to export the information which
- * resided in 2.4 in /proc/subchannels. Subchannel and device number
- * are obvious, so they don't have an entry :)
- * TODO: Split chpids and pimpampom up? Where is "in use" in the tree?
- */
-static ssize_t
-chpids_show (struct device * dev, struct device_attribute *attr, char * buf)
-{
-	struct subchannel *sch = to_subchannel(dev);
-	struct chsc_ssd_info *ssd = &sch->ssd_info;
-	ssize_t ret = 0;
-	int chp;
-	int mask;
-
-	for (chp = 0; chp < 8; chp++) {
-		mask = 0x80 >> chp;
-		if (ssd->path_mask & mask)
-			ret += sprintf(buf + ret, "%02x ", ssd->chpid[chp].id);
-		else
-			ret += sprintf(buf + ret, "00 ");
-	}
-	ret += sprintf (buf+ret, "\n");
-	return min((ssize_t)PAGE_SIZE, ret);
-}
-
-static ssize_t
-pimpampom_show (struct device * dev, struct device_attribute *attr, char * buf)
-{
-	struct subchannel *sch = to_subchannel(dev);
-	struct pmcw *pmcw = &sch->schib.pmcw;
-
-	return sprintf (buf, "%02x %02x %02x\n",
-			pmcw->pim, pmcw->pam, pmcw->pom);
-}
-
 static ssize_t
 devtype_show (struct device *dev, struct device_attribute *attr, char *buf)
 {
@@ -636,8 +598,6 @@ static ssize_t vpm_show(struct device *dev, struct device_attribute *attr,
 	return sprintf(buf, "%02x\n", sch->vpm);
 }
 
-static DEVICE_ATTR(chpids, 0444, chpids_show, NULL);
-static DEVICE_ATTR(pimpampom, 0444, pimpampom_show, NULL);
 static DEVICE_ATTR(devtype, 0444, devtype_show, NULL);
 static DEVICE_ATTR(cutype, 0444, cutype_show, NULL);
 static DEVICE_ATTR(modalias, 0444, modalias_show, NULL);
@@ -647,8 +607,6 @@ static DEVICE_ATTR(logging, 0200, NULL, initiate_logging);
 static DEVICE_ATTR(vpm, 0444, vpm_show, NULL);
 
 static struct attribute *io_subchannel_attrs[] = {
-	&dev_attr_chpids.attr,
-	&dev_attr_pimpampom.attr,
 	&dev_attr_logging.attr,
 	&dev_attr_vpm.attr,
 	NULL,
diff --git a/drivers/s390/cio/eadm_sch.c b/drivers/s390/cio/eadm_sch.c
index b3f44bc7f644..0f11f3bcac82 100644
--- a/drivers/s390/cio/eadm_sch.c
+++ b/drivers/s390/cio/eadm_sch.c
@@ -135,7 +135,7 @@ static void eadm_subchannel_irq(struct subchannel *sch)
 	struct eadm_private *private = get_eadm_private(sch);
 	struct eadm_scsw *scsw = &sch->schib.scsw.eadm;
 	struct irb *irb = this_cpu_ptr(&cio_irb);
-	int error = 0;
+	blk_status_t error = BLK_STS_OK;
 
 	EADM_LOG(6, "irq");
 	EADM_LOG_HEX(6, irb, sizeof(*irb));
@@ -144,10 +144,10 @@ static void eadm_subchannel_irq(struct subchannel *sch)
 
 	if ((scsw->stctl & (SCSW_STCTL_ALERT_STATUS | SCSW_STCTL_STATUS_PEND))
 	    && scsw->eswf == 1 && irb->esw.eadm.erw.r)
-		error = -EIO;
+		error = BLK_STS_IOERR;
 
 	if (scsw->fctl & SCSW_FCTL_CLEAR_FUNC)
-		error = -ETIMEDOUT;
+		error = BLK_STS_TIMEOUT;
 
 	eadm_subchannel_set_timeout(sch, 0);
 
diff --git a/drivers/s390/cio/scm.c b/drivers/s390/cio/scm.c
index 15268edc54ae..1fa53ecdc2aa 100644
--- a/drivers/s390/cio/scm.c
+++ b/drivers/s390/cio/scm.c
@@ -71,7 +71,7 @@ void scm_driver_unregister(struct scm_driver *scmdrv)
 }
 EXPORT_SYMBOL_GPL(scm_driver_unregister);
 
-void scm_irq_handler(struct aob *aob, int error)
+void scm_irq_handler(struct aob *aob, blk_status_t error)
 {
 	struct aob_rq_header *aobrq = (void *) aob->request.data;
 	struct scm_device *scmdev = aobrq->scmdev;
diff --git a/drivers/s390/cio/vfio_ccw_drv.c b/drivers/s390/cio/vfio_ccw_drv.c
index e90dd43d2a55..a25367ebaa89 100644
--- a/drivers/s390/cio/vfio_ccw_drv.c
+++ b/drivers/s390/cio/vfio_ccw_drv.c
@@ -90,54 +90,6 @@ static void vfio_ccw_sch_io_todo(struct work_struct *work)
 }
 
 /*
- * Sysfs interfaces
- */
-static ssize_t chpids_show(struct device *dev,
-			   struct device_attribute *attr,
-			   char *buf)
-{
-	struct subchannel *sch = to_subchannel(dev);
-	struct chsc_ssd_info *ssd = &sch->ssd_info;
-	ssize_t ret = 0;
-	int chp;
-	int mask;
-
-	for (chp = 0; chp < 8; chp++) {
-		mask = 0x80 >> chp;
-		if (ssd->path_mask & mask)
-			ret += sprintf(buf + ret, "%02x ", ssd->chpid[chp].id);
-		else
-			ret += sprintf(buf + ret, "00 ");
-	}
-	ret += sprintf(buf+ret, "\n");
-	return ret;
-}
-
-static ssize_t pimpampom_show(struct device *dev,
-			      struct device_attribute *attr,
-			      char *buf)
-{
-	struct subchannel *sch = to_subchannel(dev);
-	struct pmcw *pmcw = &sch->schib.pmcw;
-
-	return sprintf(buf, "%02x %02x %02x\n",
-		       pmcw->pim, pmcw->pam, pmcw->pom);
-}
-
-static DEVICE_ATTR(chpids, 0444, chpids_show, NULL);
-static DEVICE_ATTR(pimpampom, 0444, pimpampom_show, NULL);
-
-static struct attribute *vfio_subchannel_attrs[] = {
-	&dev_attr_chpids.attr,
-	&dev_attr_pimpampom.attr,
-	NULL,
-};
-
-static struct attribute_group vfio_subchannel_attr_group = {
-	.attrs = vfio_subchannel_attrs,
-};
-
-/*
  * Css driver callbacks
  */
 static void vfio_ccw_sch_irq(struct subchannel *sch)
@@ -174,13 +126,9 @@ static int vfio_ccw_sch_probe(struct subchannel *sch)
 	if (ret)
 		goto out_free;
 
-	ret = sysfs_create_group(&sch->dev.kobj, &vfio_subchannel_attr_group);
-	if (ret)
-		goto out_disable;
-
 	ret = vfio_ccw_mdev_reg(sch);
 	if (ret)
-		goto out_rm_group;
+		goto out_disable;
 
 	INIT_WORK(&private->io_work, vfio_ccw_sch_io_todo);
 	atomic_set(&private->avail, 1);
@@ -188,8 +136,6 @@ static int vfio_ccw_sch_probe(struct subchannel *sch)
 
 	return 0;
 
-out_rm_group:
-	sysfs_remove_group(&sch->dev.kobj, &vfio_subchannel_attr_group);
 out_disable:
 	cio_disable_subchannel(sch);
 out_free:
@@ -206,8 +152,6 @@ static int vfio_ccw_sch_remove(struct subchannel *sch)
 
 	vfio_ccw_mdev_unreg(sch);
 
-	sysfs_remove_group(&sch->dev.kobj, &vfio_subchannel_attr_group);
-
 	dev_set_drvdata(&sch->dev, NULL);
 
 	kfree(private);
diff --git a/drivers/s390/cio/vfio_ccw_ops.c b/drivers/s390/cio/vfio_ccw_ops.c
index e72abbc18ee3..a66a317f3e4f 100644
--- a/drivers/s390/cio/vfio_ccw_ops.c
+++ b/drivers/s390/cio/vfio_ccw_ops.c
@@ -70,14 +70,14 @@ static ssize_t name_show(struct kobject *kobj, struct device *dev, char *buf)
 {
 	return sprintf(buf, "I/O subchannel (Non-QDIO)\n");
 }
-MDEV_TYPE_ATTR_RO(name);
+static MDEV_TYPE_ATTR_RO(name);
 
 static ssize_t device_api_show(struct kobject *kobj, struct device *dev,
 			       char *buf)
 {
 	return sprintf(buf, "%s\n", VFIO_DEVICE_API_CCW_STRING);
 }
-MDEV_TYPE_ATTR_RO(device_api);
+static MDEV_TYPE_ATTR_RO(device_api);
 
 static ssize_t available_instances_show(struct kobject *kobj,
 					struct device *dev, char *buf)
@@ -86,7 +86,7 @@ static ssize_t available_instances_show(struct kobject *kobj,
 
 	return sprintf(buf, "%d\n", atomic_read(&private->avail));
 }
-MDEV_TYPE_ATTR_RO(available_instances);
+static MDEV_TYPE_ATTR_RO(available_instances);
 
 static struct attribute *mdev_types_attrs[] = {
 	&mdev_type_attr_name.attr,
@@ -100,7 +100,7 @@ static struct attribute_group mdev_type_group = {
 	.attrs = mdev_types_attrs,
 };
 
-struct attribute_group *mdev_type_groups[] = {
+static struct attribute_group *mdev_type_groups[] = {
 	&mdev_type_group,
 	NULL,
 };
@@ -152,7 +152,7 @@ static int vfio_ccw_mdev_open(struct mdev_device *mdev)
 				      &events, &private->nb);
 }
 
-void vfio_ccw_mdev_release(struct mdev_device *mdev)
+static void vfio_ccw_mdev_release(struct mdev_device *mdev)
 {
 	struct vfio_ccw_private *private =
 		dev_get_drvdata(mdev_parent_dev(mdev));
@@ -233,7 +233,7 @@ static int vfio_ccw_mdev_get_region_info(struct vfio_region_info *info,
 	}
 }
 
-int vfio_ccw_mdev_get_irq_info(struct vfio_irq_info *info)
+static int vfio_ccw_mdev_get_irq_info(struct vfio_irq_info *info)
 {
 	if (info->index != VFIO_CCW_IO_IRQ_INDEX)
 		return -EINVAL;
diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index 9be4596d8a08..6dee598979e7 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -668,10 +668,28 @@ static int ap_device_probe(struct device *dev)
 	struct ap_driver *ap_drv = to_ap_drv(dev->driver);
 	int rc;
 
+	/* Add queue/card to list of active queues/cards */
+	spin_lock_bh(&ap_list_lock);
+	if (is_card_dev(dev))
+		list_add(&to_ap_card(dev)->list, &ap_card_list);
+	else
+		list_add(&to_ap_queue(dev)->list,
+			 &to_ap_queue(dev)->card->queues);
+	spin_unlock_bh(&ap_list_lock);
+
 	ap_dev->drv = ap_drv;
 	rc = ap_drv->probe ? ap_drv->probe(ap_dev) : -ENODEV;
-	if (rc)
+
+	if (rc) {
+		spin_lock_bh(&ap_list_lock);
+		if (is_card_dev(dev))
+			list_del_init(&to_ap_card(dev)->list);
+		else
+			list_del_init(&to_ap_queue(dev)->list);
+		spin_unlock_bh(&ap_list_lock);
 		ap_dev->drv = NULL;
+	}
+
 	return rc;
 }
 
@@ -680,14 +698,17 @@ static int ap_device_remove(struct device *dev)
 	struct ap_device *ap_dev = to_ap_dev(dev);
 	struct ap_driver *ap_drv = ap_dev->drv;
 
+	if (ap_drv->remove)
+		ap_drv->remove(ap_dev);
+
+	/* Remove queue/card from list of active queues/cards */
 	spin_lock_bh(&ap_list_lock);
 	if (is_card_dev(dev))
 		list_del_init(&to_ap_card(dev)->list);
 	else
 		list_del_init(&to_ap_queue(dev)->list);
 	spin_unlock_bh(&ap_list_lock);
-	if (ap_drv->remove)
-		ap_drv->remove(ap_dev);
+
 	return 0;
 }
 
@@ -745,7 +766,7 @@ static ssize_t ap_domain_store(struct bus_type *bus,
 	ap_domain_index = domain;
 	spin_unlock_bh(&ap_domain_lock);
 
-	AP_DBF(DBF_DEBUG, "store new default domain=%d\n", domain);
+	AP_DBF(DBF_DEBUG, "stored new default domain=%d\n", domain);
 
 	return count;
 }
@@ -931,6 +952,7 @@ static int ap_select_domain(void)
 	}
 	if (best_domain >= 0){
 		ap_domain_index = best_domain;
+		AP_DBF(DBF_DEBUG, "new ap_domain_index=%d\n", ap_domain_index);
 		spin_unlock_bh(&ap_domain_lock);
 		return 0;
 	}
@@ -967,7 +989,7 @@ static void ap_scan_bus(struct work_struct *unused)
 	ap_qid_t qid;
 	int depth = 0, type = 0;
 	unsigned int functions = 0;
-	int rc, id, dom, borked, domains;
+	int rc, id, dom, borked, domains, defdomdevs = 0;
 
 	AP_DBF(DBF_DEBUG, "ap_scan_bus running\n");
 
@@ -1031,6 +1053,8 @@ static void ap_scan_bus(struct work_struct *unused)
 				put_device(dev);
 				if (!borked) {
 					domains++;
+					if (dom == ap_domain_index)
+						defdomdevs++;
 					continue;
 				}
 			}
@@ -1056,10 +1080,6 @@ static void ap_scan_bus(struct work_struct *unused)
 				}
 				/* get it and thus adjust reference counter */
 				get_device(&ac->ap_dev.device);
-				/* Add card device to card list */
-				spin_lock_bh(&ap_list_lock);
-				list_add(&ac->list, &ap_card_list);
-				spin_unlock_bh(&ap_list_lock);
 			}
 			/* now create the new queue device */
 			aq = ap_queue_create(qid, type);
@@ -1070,10 +1090,6 @@ static void ap_scan_bus(struct work_struct *unused)
 			aq->ap_dev.device.parent = &ac->ap_dev.device;
 			dev_set_name(&aq->ap_dev.device,
 				     "%02x.%04x", id, dom);
-			/* Add queue device to card queue list */
-			spin_lock_bh(&ap_list_lock);
-			list_add(&aq->list, &ac->queues);
-			spin_unlock_bh(&ap_list_lock);
 			/* Start with a device reset */
 			spin_lock_bh(&aq->lock);
 			ap_wait(ap_sm_event(aq, AP_EVENT_POLL));
@@ -1081,13 +1097,12 @@ static void ap_scan_bus(struct work_struct *unused)
 			/* Register device */
 			rc = device_register(&aq->ap_dev.device);
 			if (rc) {
-				spin_lock_bh(&ap_list_lock);
-				list_del_init(&aq->list);
-				spin_unlock_bh(&ap_list_lock);
 				put_device(&aq->ap_dev.device);
 				continue;
 			}
 			domains++;
+			if (dom == ap_domain_index)
+				defdomdevs++;
 		} /* end domain loop */
 		if (ac) {
 			/* remove card dev if there are no queue devices */
@@ -1096,6 +1111,11 @@ static void ap_scan_bus(struct work_struct *unused)
 			put_device(&ac->ap_dev.device);
 		}
 	} /* end device loop */
+
+	if (defdomdevs < 1)
+		AP_DBF(DBF_INFO, "no queue device with default domain %d available\n",
+		       ap_domain_index);
+
 out:
 	mod_timer(&ap_config_timer, jiffies + ap_config_time * HZ);
 }
@@ -1164,14 +1184,14 @@ int __init ap_module_init(void)
 	ap_init_configuration();
 
 	if (ap_configuration)
-		max_domain_id = ap_max_domain_id ? : (AP_DOMAINS - 1);
+		max_domain_id =
+			ap_max_domain_id ? ap_max_domain_id : AP_DOMAINS - 1;
 	else
 		max_domain_id = 15;
 	if (ap_domain_index < -1 || ap_domain_index > max_domain_id) {
 		pr_warn("%d is not a valid cryptographic domain\n",
 			ap_domain_index);
-		rc = -EINVAL;
-		goto out_free;
+		ap_domain_index = -1;
 	}
 	/* In resume callback we need to know if the user had set the domain.
 	 * If so, we can not just reset it.
@@ -1244,7 +1264,6 @@ out:
 	unregister_reset_call(&ap_reset_call);
 	if (ap_using_interrupts())
 		unregister_adapter_interrupt(&ap_airq);
-out_free:
 	kfree(ap_configuration);
 	return rc;
 }
diff --git a/drivers/s390/crypto/ap_card.c b/drivers/s390/crypto/ap_card.c
index cfa161ccc74e..836efac96813 100644
--- a/drivers/s390/crypto/ap_card.c
+++ b/drivers/s390/crypto/ap_card.c
@@ -160,7 +160,14 @@ static struct device_type ap_card_type = {
 
 static void ap_card_device_release(struct device *dev)
 {
-	kfree(to_ap_card(dev));
+	struct ap_card *ac = to_ap_card(dev);
+
+	if (!list_empty(&ac->list)) {
+		spin_lock_bh(&ap_list_lock);
+		list_del_init(&ac->list);
+		spin_unlock_bh(&ap_list_lock);
+	}
+	kfree(ac);
 }
 
 struct ap_card *ap_card_create(int id, int queue_depth, int device_type,
diff --git a/drivers/s390/crypto/ap_queue.c b/drivers/s390/crypto/ap_queue.c
index 480c58a63769..0f1a5d02acb0 100644
--- a/drivers/s390/crypto/ap_queue.c
+++ b/drivers/s390/crypto/ap_queue.c
@@ -584,7 +584,14 @@ static struct device_type ap_queue_type = {
 
 static void ap_queue_device_release(struct device *dev)
 {
-	kfree(to_ap_queue(dev));
+	struct ap_queue *aq = to_ap_queue(dev);
+
+	if (!list_empty(&aq->list)) {
+		spin_lock_bh(&ap_list_lock);
+		list_del_init(&aq->list);
+		spin_unlock_bh(&ap_list_lock);
+	}
+	kfree(aq);
 }
 
 struct ap_queue *ap_queue_create(ap_qid_t qid, int device_type)
diff --git a/drivers/s390/crypto/pkey_api.c b/drivers/s390/crypto/pkey_api.c
index ea86da8c75f9..f61fa47135a6 100644
--- a/drivers/s390/crypto/pkey_api.c
+++ b/drivers/s390/crypto/pkey_api.c
@@ -178,9 +178,9 @@ static inline void prep_xcrb(struct ica_xcRB *pxcrb,
 	pxcrb->user_defined = (cardnr == 0xFFFF ? AUTOSELECT : cardnr);
 	pxcrb->request_control_blk_length =
 		preqcblk->cprb_len + preqcblk->req_parml;
-	pxcrb->request_control_blk_addr = (void *) preqcblk;
+	pxcrb->request_control_blk_addr = (void __user *) preqcblk;
 	pxcrb->reply_control_blk_length = preqcblk->rpl_msgbl;
-	pxcrb->reply_control_blk_addr = (void *) prepcblk;
+	pxcrb->reply_control_blk_addr = (void __user *) prepcblk;
 }
 
 /*
@@ -1194,7 +1194,7 @@ static struct miscdevice pkey_dev = {
 /*
  * Module init
  */
-int __init pkey_init(void)
+static int __init pkey_init(void)
 {
 	cpacf_mask_t pckmo_functions;
 
diff --git a/drivers/s390/crypto/zcrypt_api.c b/drivers/s390/crypto/zcrypt_api.c
index 93015f85d4a6..b1c27e28859b 100644
--- a/drivers/s390/crypto/zcrypt_api.c
+++ b/drivers/s390/crypto/zcrypt_api.c
@@ -821,8 +821,10 @@ static long zcrypt_unlocked_ioctl(struct file *filp, unsigned int cmd,
 			do {
 				rc = zcrypt_rsa_modexpo(&mex);
 			} while (rc == -EAGAIN);
-		if (rc)
+		if (rc) {
+			ZCRYPT_DBF(DBF_DEBUG, "ioctl ICARSAMODEXPO rc=%d", rc);
 			return rc;
+		}
 		return put_user(mex.outputdatalength, &umex->outputdatalength);
 	}
 	case ICARSACRT: {
@@ -838,8 +840,10 @@ static long zcrypt_unlocked_ioctl(struct file *filp, unsigned int cmd,
 			do {
 				rc = zcrypt_rsa_crt(&crt);
 			} while (rc == -EAGAIN);
-		if (rc)
+		if (rc) {
+			ZCRYPT_DBF(DBF_DEBUG, "ioctl ICARSACRT rc=%d", rc);
 			return rc;
+		}
 		return put_user(crt.outputdatalength, &ucrt->outputdatalength);
 	}
 	case ZSECSENDCPRB: {
@@ -855,6 +859,8 @@ static long zcrypt_unlocked_ioctl(struct file *filp, unsigned int cmd,
 			do {
 				rc = zcrypt_send_cprb(&xcRB);
 			} while (rc == -EAGAIN);
+		if (rc)
+			ZCRYPT_DBF(DBF_DEBUG, "ioctl ZSENDCPRB rc=%d", rc);
 		if (copy_to_user(uxcRB, &xcRB, sizeof(xcRB)))
 			return -EFAULT;
 		return rc;
@@ -872,6 +878,8 @@ static long zcrypt_unlocked_ioctl(struct file *filp, unsigned int cmd,
 			do {
 				rc = zcrypt_send_ep11_cprb(&xcrb);
 			} while (rc == -EAGAIN);
+		if (rc)
+			ZCRYPT_DBF(DBF_DEBUG, "ioctl ZSENDEP11CPRB rc=%d", rc);
 		if (copy_to_user(uxcrb, &xcrb, sizeof(xcrb)))
 			return -EFAULT;
 		return rc;
diff --git a/drivers/s390/crypto/zcrypt_cca_key.h b/drivers/s390/crypto/zcrypt_cca_key.h
index ca0cdbe46368..12cff6262566 100644
--- a/drivers/s390/crypto/zcrypt_cca_key.h
+++ b/drivers/s390/crypto/zcrypt_cca_key.h
@@ -48,26 +48,6 @@ struct cca_token_hdr {
 
 #define CCA_TKN_HDR_ID_EXT 0x1E
 
-/**
- * mapping for the cca private ME section
- */
-struct cca_private_ext_ME_sec {
-	unsigned char  section_identifier;
-	unsigned char  version;
-	unsigned short section_length;
-	unsigned char  private_key_hash[20];
-	unsigned char  reserved1[4];
-	unsigned char  key_format;
-	unsigned char  reserved2;
-	unsigned char  key_name_hash[20];
-	unsigned char  key_use_flags[4];
-	unsigned char  reserved3[6];
-	unsigned char  reserved4[24];
-	unsigned char  confounder[24];
-	unsigned char  exponent[128];
-	unsigned char  modulus[128];
-} __attribute__((packed));
-
 #define CCA_PVT_USAGE_ALL 0x80
 
 /**
@@ -124,77 +104,6 @@ struct cca_pvt_ext_CRT_sec {
 #define CCA_PVT_EXT_CRT_SEC_FMT_CL 0x40
 
 /**
- * Set up private key fields of a type6 MEX message.
- * Note that all numerics in the key token are big-endian,
- * while the entries in the key block header are little-endian.
- *
- * @mex: pointer to user input data
- * @p: pointer to memory area for the key
- *
- * Returns the size of the key area or -EFAULT
- */
-static inline int zcrypt_type6_mex_key_de(struct ica_rsa_modexpo *mex,
-					  void *p, int big_endian)
-{
-	static struct cca_token_hdr static_pvt_me_hdr = {
-		.token_identifier	=  0x1E,
-		.token_length		=  0x0183,
-	};
-	static struct cca_private_ext_ME_sec static_pvt_me_sec = {
-		.section_identifier	=  0x02,
-		.section_length		=  0x016C,
-		.key_use_flags		= {0x80,0x00,0x00,0x00},
-	};
-	static struct cca_public_sec static_pub_me_sec = {
-		.section_identifier	=  0x04,
-		.section_length		=  0x000F,
-		.exponent_len		=  0x0003,
-	};
-	static char pk_exponent[3] = { 0x01, 0x00, 0x01 };
-	struct {
-		struct T6_keyBlock_hdr t6_hdr;
-		struct cca_token_hdr pvtMeHdr;
-		struct cca_private_ext_ME_sec pvtMeSec;
-		struct cca_public_sec pubMeSec;
-		char exponent[3];
-	} __attribute__((packed)) *key = p;
-	unsigned char *temp;
-
-	memset(key, 0, sizeof(*key));
-
-	if (big_endian) {
-		key->t6_hdr.blen = cpu_to_be16(0x189);
-		key->t6_hdr.ulen = cpu_to_be16(0x189 - 2);
-	} else {
-		key->t6_hdr.blen = cpu_to_le16(0x189);
-		key->t6_hdr.ulen = cpu_to_le16(0x189 - 2);
-	}
-	key->pvtMeHdr = static_pvt_me_hdr;
-	key->pvtMeSec = static_pvt_me_sec;
-	key->pubMeSec = static_pub_me_sec;
-	/*
-	 * In a private key, the modulus doesn't appear in the public
-	 * section. So, an arbitrary public exponent of 0x010001 will be
-	 * used.
-	 */
-	memcpy(key->exponent, pk_exponent, 3);
-
-	/* key parameter block */
-	temp = key->pvtMeSec.exponent +
-		sizeof(key->pvtMeSec.exponent) - mex->inputdatalength;
-	if (copy_from_user(temp, mex->b_key, mex->inputdatalength))
-		return -EFAULT;
-
-	/* modulus */
-	temp = key->pvtMeSec.modulus +
-		sizeof(key->pvtMeSec.modulus) - mex->inputdatalength;
-	if (copy_from_user(temp, mex->n_modulus, mex->inputdatalength))
-		return -EFAULT;
-	key->pubMeSec.modulus_bit_len = 8 * mex->inputdatalength;
-	return sizeof(*key);
-}
-
-/**
  * Set up private key fields of a type6 MEX message. The _pad variant
  * strips leading zeroes from the b_key.
  * Note that all numerics in the key token are big-endian,
@@ -205,8 +114,7 @@ static inline int zcrypt_type6_mex_key_de(struct ica_rsa_modexpo *mex,
  *
  * Returns the size of the key area or -EFAULT
  */
-static inline int zcrypt_type6_mex_key_en(struct ica_rsa_modexpo *mex,
-					  void *p, int big_endian)
+static inline int zcrypt_type6_mex_key_en(struct ica_rsa_modexpo *mex, void *p)
 {
 	static struct cca_token_hdr static_pub_hdr = {
 		.token_identifier	=  0x1E,
@@ -251,13 +159,8 @@ static inline int zcrypt_type6_mex_key_en(struct ica_rsa_modexpo *mex,
 					2*mex->inputdatalength - i;
 	key->pubHdr.token_length =
 		key->pubSec.section_length + sizeof(key->pubHdr);
-	if (big_endian) {
-		key->t6_hdr.ulen = cpu_to_be16(key->pubHdr.token_length + 4);
-		key->t6_hdr.blen = cpu_to_be16(key->pubHdr.token_length + 6);
-	} else {
-		key->t6_hdr.ulen = cpu_to_le16(key->pubHdr.token_length + 4);
-		key->t6_hdr.blen = cpu_to_le16(key->pubHdr.token_length + 6);
-	}
+	key->t6_hdr.ulen = key->pubHdr.token_length + 4;
+	key->t6_hdr.blen = key->pubHdr.token_length + 6;
 	return sizeof(*key) + 2*mex->inputdatalength - i;
 }
 
@@ -271,8 +174,7 @@ static inline int zcrypt_type6_mex_key_en(struct ica_rsa_modexpo *mex,
  *
  * Returns the size of the key area or -EFAULT
  */
-static inline int zcrypt_type6_crt_key(struct ica_rsa_modexpo_crt *crt,
-				       void *p, int big_endian)
+static inline int zcrypt_type6_crt_key(struct ica_rsa_modexpo_crt *crt, void *p)
 {
 	static struct cca_public_sec static_cca_pub_sec = {
 		.section_identifier = 4,
@@ -298,13 +200,8 @@ static inline int zcrypt_type6_crt_key(struct ica_rsa_modexpo_crt *crt,
 	size = sizeof(*key) + key_len + sizeof(*pub) + 3;
 
 	/* parameter block.key block */
-	if (big_endian) {
-		key->t6_hdr.blen = cpu_to_be16(size);
-		key->t6_hdr.ulen = cpu_to_be16(size - 2);
-	} else {
-		key->t6_hdr.blen = cpu_to_le16(size);
-		key->t6_hdr.ulen = cpu_to_le16(size - 2);
-	}
+	key->t6_hdr.blen = size;
+	key->t6_hdr.ulen = size - 2;
 
 	/* key token header */
 	key->token.token_identifier = CCA_TKN_HDR_ID_EXT;
diff --git a/drivers/s390/crypto/zcrypt_msgtype6.c b/drivers/s390/crypto/zcrypt_msgtype6.c
index e5563ffeb839..4fddb4319481 100644
--- a/drivers/s390/crypto/zcrypt_msgtype6.c
+++ b/drivers/s390/crypto/zcrypt_msgtype6.c
@@ -291,7 +291,7 @@ static int ICAMEX_msg_to_type6MEX_msgX(struct zcrypt_queue *zq,
 		return -EFAULT;
 
 	/* Set up key which is located after the variable length text. */
-	size = zcrypt_type6_mex_key_en(mex, msg->text+mex->inputdatalength, 1);
+	size = zcrypt_type6_mex_key_en(mex, msg->text+mex->inputdatalength);
 	if (size < 0)
 		return size;
 	size += sizeof(*msg) + mex->inputdatalength;
@@ -353,7 +353,7 @@ static int ICACRT_msg_to_type6CRT_msgX(struct zcrypt_queue *zq,
 		return -EFAULT;
 
 	/* Set up key which is located after the variable length text. */
-	size = zcrypt_type6_crt_key(crt, msg->text + crt->inputdatalength, 1);
+	size = zcrypt_type6_crt_key(crt, msg->text + crt->inputdatalength);
 	if (size < 0)
 		return size;
 	size += sizeof(*msg) + crt->inputdatalength;	/* total size of msg */
diff --git a/drivers/s390/net/ctcm_fsms.c b/drivers/s390/net/ctcm_fsms.c
index 730d9619400e..570ae3b7adf6 100644
--- a/drivers/s390/net/ctcm_fsms.c
+++ b/drivers/s390/net/ctcm_fsms.c
@@ -217,7 +217,7 @@ void ctcm_purge_skb_queue(struct sk_buff_head *q)
 	CTCM_DBF_TEXT(TRACE, CTC_DBF_DEBUG, __func__);
 
 	while ((skb = skb_dequeue(q))) {
-		atomic_dec(&skb->users);
+		refcount_dec(&skb->users);
 		dev_kfree_skb_any(skb);
 	}
 }
@@ -271,7 +271,7 @@ static void chx_txdone(fsm_instance *fi, int event, void *arg)
 			priv->stats.tx_bytes += 2;
 			first = 0;
 		}
-		atomic_dec(&skb->users);
+		refcount_dec(&skb->users);
 		dev_kfree_skb_irq(skb);
 	}
 	spin_lock(&ch->collect_lock);
@@ -297,7 +297,7 @@ static void chx_txdone(fsm_instance *fi, int event, void *arg)
 				skb_put(ch->trans_skb, skb->len), skb->len);
 			priv->stats.tx_packets++;
 			priv->stats.tx_bytes += skb->len - LL_HEADER_LENGTH;
-			atomic_dec(&skb->users);
+			refcount_dec(&skb->users);
 			dev_kfree_skb_irq(skb);
 			i++;
 		}
@@ -1248,7 +1248,7 @@ static void ctcmpc_chx_txdone(fsm_instance *fi, int event, void *arg)
 			priv->stats.tx_bytes += 2;
 			first = 0;
 		}
-		atomic_dec(&skb->users);
+		refcount_dec(&skb->users);
 		dev_kfree_skb_irq(skb);
 	}
 	spin_lock(&ch->collect_lock);
@@ -1279,7 +1279,7 @@ static void ctcmpc_chx_txdone(fsm_instance *fi, int event, void *arg)
 		       __func__, data_space);
 
 	while ((skb = skb_dequeue(&ch->collect_queue))) {
-		memcpy(skb_put(ch->trans_skb, skb->len), skb->data, skb->len);
+		skb_put_data(ch->trans_skb, skb->data, skb->len);
 		p_header = (struct pdu *)
 			(skb_tail_pointer(ch->trans_skb) - skb->len);
 		p_header->pdu_flag = 0x00;
@@ -1298,7 +1298,7 @@ static void ctcmpc_chx_txdone(fsm_instance *fi, int event, void *arg)
 		data_space -= skb->len;
 		priv->stats.tx_packets++;
 		priv->stats.tx_bytes += skb->len;
-		atomic_dec(&skb->users);
+		refcount_dec(&skb->users);
 		dev_kfree_skb_any(skb);
 		peekskb = skb_peek(&ch->collect_queue);
 		if (peekskb->len > data_space)
@@ -1431,13 +1431,12 @@ static void ctcmpc_chx_rx(fsm_instance *fi, int event, void *arg)
 			break;
 		case MPCG_STATE_FLOWC:
 		case MPCG_STATE_READY:
-			memcpy(skb_put(new_skb, block_len),
-					       skb->data, block_len);
+			skb_put_data(new_skb, skb->data, block_len);
 			skb_queue_tail(&ch->io_queue, new_skb);
 			tasklet_schedule(&ch->ch_tasklet);
 			break;
 		default:
-			memcpy(skb_put(new_skb, len), skb->data, len);
+			skb_put_data(new_skb, skb->data, len);
 			skb_queue_tail(&ch->io_queue, new_skb);
 			tasklet_hi_schedule(&ch->ch_tasklet);
 			break;
@@ -1796,7 +1795,7 @@ static void ctcmpc_chx_send_sweep(fsm_instance *fsm, int event, void *arg)
 		fsm_event(grp->fsm, MPCG_EVENT_INOP, dev);
 				goto done;
 	} else {
-		atomic_inc(&skb->users);
+		refcount_inc(&skb->users);
 		skb_queue_tail(&wch->io_queue, skb);
 	}
 
diff --git a/drivers/s390/net/ctcm_main.c b/drivers/s390/net/ctcm_main.c
index 198842ce6876..1563b1458e44 100644
--- a/drivers/s390/net/ctcm_main.c
+++ b/drivers/s390/net/ctcm_main.c
@@ -483,7 +483,7 @@ static int ctcm_transmit_skb(struct channel *ch, struct sk_buff *skb)
 			spin_unlock_irqrestore(&ch->collect_lock, saveflags);
 			return -EBUSY;
 		} else {
-			atomic_inc(&skb->users);
+			refcount_inc(&skb->users);
 			header.length = l;
 			header.type = be16_to_cpu(skb->protocol);
 			header.unused = 0;
@@ -500,7 +500,7 @@ static int ctcm_transmit_skb(struct channel *ch, struct sk_buff *skb)
 	 * Protect skb against beeing free'd by upper
 	 * layers.
 	 */
-	atomic_inc(&skb->users);
+	refcount_inc(&skb->users);
 	ch->prof.txlen += skb->len;
 	header.length = skb->len + LL_HEADER_LENGTH;
 	header.type = be16_to_cpu(skb->protocol);
@@ -517,14 +517,14 @@ static int ctcm_transmit_skb(struct channel *ch, struct sk_buff *skb)
 	if (hi) {
 		nskb = alloc_skb(skb->len, GFP_ATOMIC | GFP_DMA);
 		if (!nskb) {
-			atomic_dec(&skb->users);
+			refcount_dec(&skb->users);
 			skb_pull(skb, LL_HEADER_LENGTH + 2);
 			ctcm_clear_busy(ch->netdev);
 			return -ENOMEM;
 		} else {
-			memcpy(skb_put(nskb, skb->len), skb->data, skb->len);
-			atomic_inc(&nskb->users);
-			atomic_dec(&skb->users);
+			skb_put_data(nskb, skb->data, skb->len);
+			refcount_inc(&nskb->users);
+			refcount_dec(&skb->users);
 			dev_kfree_skb_irq(skb);
 			skb = nskb;
 		}
@@ -542,7 +542,7 @@ static int ctcm_transmit_skb(struct channel *ch, struct sk_buff *skb)
 			 * Remove our header. It gets added
 			 * again on retransmit.
 			 */
-			atomic_dec(&skb->users);
+			refcount_dec(&skb->users);
 			skb_pull(skb, LL_HEADER_LENGTH + 2);
 			ctcm_clear_busy(ch->netdev);
 			return -ENOMEM;
@@ -553,7 +553,7 @@ static int ctcm_transmit_skb(struct channel *ch, struct sk_buff *skb)
 		ch->ccw[1].count = skb->len;
 		skb_copy_from_linear_data(skb,
 				skb_put(ch->trans_skb, skb->len), skb->len);
-		atomic_dec(&skb->users);
+		refcount_dec(&skb->users);
 		dev_kfree_skb_irq(skb);
 		ccw_idx = 0;
 	} else {
@@ -638,7 +638,7 @@ static void ctcmpc_send_sweep_req(struct channel *rch)
 	header->th.th_seq_num	= 0x00;
 	header->sw.th_last_seq	= ch->th_seq_num;
 
-	memcpy(skb_put(sweep_skb, TH_SWEEP_LENGTH), header, TH_SWEEP_LENGTH);
+	skb_put_data(sweep_skb, header, TH_SWEEP_LENGTH);
 
 	kfree(header);
 
@@ -679,7 +679,7 @@ static int ctcmpc_transmit_skb(struct channel *ch, struct sk_buff *skb)
 
 	if ((fsm_getstate(ch->fsm) != CTC_STATE_TXIDLE) || grp->in_sweep) {
 		spin_lock_irqsave(&ch->collect_lock, saveflags);
-		atomic_inc(&skb->users);
+		refcount_inc(&skb->users);
 		p_header = kmalloc(PDU_HEADER_LENGTH, gfp_type());
 
 		if (!p_header) {
@@ -716,7 +716,7 @@ static int ctcmpc_transmit_skb(struct channel *ch, struct sk_buff *skb)
 	 * Protect skb against beeing free'd by upper
 	 * layers.
 	 */
-	atomic_inc(&skb->users);
+	refcount_inc(&skb->users);
 
 	/*
 	 * IDAL support in CTCM is broken, so we have to
@@ -728,9 +728,9 @@ static int ctcmpc_transmit_skb(struct channel *ch, struct sk_buff *skb)
 		if (!nskb) {
 			goto nomem_exit;
 		} else {
-			memcpy(skb_put(nskb, skb->len), skb->data, skb->len);
-			atomic_inc(&nskb->users);
-			atomic_dec(&skb->users);
+			skb_put_data(nskb, skb->data, skb->len);
+			refcount_inc(&nskb->users);
+			refcount_dec(&skb->users);
 			dev_kfree_skb_irq(skb);
 			skb = nskb;
 		}
@@ -809,8 +809,8 @@ static int ctcmpc_transmit_skb(struct channel *ch, struct sk_buff *skb)
 		skb_reset_tail_pointer(ch->trans_skb);
 		ch->trans_skb->len = 0;
 		ch->ccw[1].count = skb->len;
-		memcpy(skb_put(ch->trans_skb, skb->len), skb->data, skb->len);
-		atomic_dec(&skb->users);
+		skb_put_data(ch->trans_skb, skb->data, skb->len);
+		refcount_dec(&skb->users);
 		dev_kfree_skb_irq(skb);
 		ccw_idx = 0;
 		CTCM_PR_DBGDATA("%s(%s): trans_skb len: %04x\n"
@@ -855,7 +855,7 @@ nomem_exit:
 			"%s(%s): MEMORY allocation ERROR\n",
 			CTCM_FUNTAIL, ch->id);
 	rc = -ENOMEM;
-	atomic_dec(&skb->users);
+	refcount_dec(&skb->users);
 	dev_kfree_skb_any(skb);
 	fsm_event(priv->mpcg->fsm, MPCG_EVENT_INOP, dev);
 done:
@@ -960,7 +960,7 @@ static int ctcmpc_tx(struct sk_buff *skb, struct net_device *dev)
 		}
 		newskb->protocol = skb->protocol;
 		skb_reserve(newskb, TH_HEADER_LENGTH + PDU_HEADER_LENGTH);
-		memcpy(skb_put(newskb, skb->len), skb->data, skb->len);
+		skb_put_data(newskb, skb->data, skb->len);
 		dev_kfree_skb_any(skb);
 		skb = newskb;
 	}
@@ -1770,15 +1770,15 @@ static struct ccwgroup_driver ctcm_group_driver = {
 	.restore     = ctcm_pm_resume,
 };
 
-static ssize_t ctcm_driver_group_store(struct device_driver *ddrv,
-				       const char *buf,	size_t count)
+static ssize_t group_store(struct device_driver *ddrv, const char *buf,
+			   size_t count)
 {
 	int err;
 
 	err = ccwgroup_create_dev(ctcm_root_dev, &ctcm_group_driver, 2, buf);
 	return err ? err : count;
 }
-static DRIVER_ATTR(group, 0200, NULL, ctcm_driver_group_store);
+static DRIVER_ATTR_WO(group);
 
 static struct attribute *ctcm_drv_attrs[] = {
 	&driver_attr_group.attr,
diff --git a/drivers/s390/net/ctcm_mpc.c b/drivers/s390/net/ctcm_mpc.c
index c103fc7efe9f..f8be39634f03 100644
--- a/drivers/s390/net/ctcm_mpc.c
+++ b/drivers/s390/net/ctcm_mpc.c
@@ -667,7 +667,7 @@ static void ctcmpc_send_sweep_resp(struct channel *rch)
 	header->th.th_seq_num	= 0x00;
 	header->sw.th_last_seq	= ch->th_seq_num;
 
-	memcpy(skb_put(sweep_skb, TH_SWEEP_LENGTH), header, TH_SWEEP_LENGTH);
+	skb_put_data(sweep_skb, header, TH_SWEEP_LENGTH);
 
 	kfree(header);
 
@@ -974,9 +974,8 @@ void mpc_channel_action(struct channel *ch, int direction, int action)
 		skb_reset_tail_pointer(ch->xid_skb);
 		ch->xid_skb->len = 0;
 
-		memcpy(skb_put(ch->xid_skb, grp->xid_skb->len),
-				grp->xid_skb->data,
-				grp->xid_skb->len);
+		skb_put_data(ch->xid_skb, grp->xid_skb->data,
+			     grp->xid_skb->len);
 
 		ch->xid->xid2_dlc_type =
 			((CHANNEL_DIRECTION(ch->flags) == CTCM_READ)
@@ -1149,7 +1148,7 @@ static void ctcmpc_unpack_skb(struct channel *ch, struct sk_buff *pskb)
 				fsm_event(grp->fsm, MPCG_EVENT_INOP, dev);
 						goto done;
 			}
-			memcpy(skb_put(skb, new_len), pskb->data, new_len);
+			skb_put_data(skb, pskb->data, new_len);
 
 			skb_reset_mac_header(skb);
 			skb->dev = pskb->dev;
@@ -1297,16 +1296,15 @@ struct mpc_group *ctcmpc_init_mpc_group(struct ctcm_priv *priv)
 	/*  base xid for all channels in group  */
 	grp->xid_skb_data = grp->xid_skb->data;
 	grp->xid_th = (struct th_header *)grp->xid_skb->data;
-	memcpy(skb_put(grp->xid_skb, TH_HEADER_LENGTH),
-			&thnorm, TH_HEADER_LENGTH);
+	skb_put_data(grp->xid_skb, &thnorm, TH_HEADER_LENGTH);
 
 	grp->xid = (struct xid2 *)skb_tail_pointer(grp->xid_skb);
-	memcpy(skb_put(grp->xid_skb, XID2_LENGTH), &init_xid, XID2_LENGTH);
+	skb_put_data(grp->xid_skb, &init_xid, XID2_LENGTH);
 	grp->xid->xid2_adj_id = jiffies | 0xfff00000;
 	grp->xid->xid2_sender_id = jiffies;
 
 	grp->xid_id = skb_tail_pointer(grp->xid_skb);
-	memcpy(skb_put(grp->xid_skb, 4), "VTAM", 4);
+	skb_put_data(grp->xid_skb, "VTAM", 4);
 
 	grp->rcvd_xid_skb =
 		__dev_alloc_skb(MPC_BUFSIZE_DEFAULT, GFP_ATOMIC|GFP_DMA);
@@ -1318,8 +1316,7 @@ struct mpc_group *ctcmpc_init_mpc_group(struct ctcm_priv *priv)
 	}
 	grp->rcvd_xid_data = grp->rcvd_xid_skb->data;
 	grp->rcvd_xid_th = (struct th_header *)grp->rcvd_xid_skb->data;
-	memcpy(skb_put(grp->rcvd_xid_skb, TH_HEADER_LENGTH),
-			&thnorm, TH_HEADER_LENGTH);
+	skb_put_data(grp->rcvd_xid_skb, &thnorm, TH_HEADER_LENGTH);
 	grp->saved_xid2 = NULL;
 	priv->xid = grp->xid;
 	priv->mpcg = grp;
@@ -1410,8 +1407,7 @@ static void mpc_action_go_inop(fsm_instance *fi, int event, void *arg)
 	skb_reset_tail_pointer(grp->rcvd_xid_skb);
 	grp->rcvd_xid_skb->len = 0;
 	grp->rcvd_xid_th = (struct th_header *)grp->rcvd_xid_skb->data;
-	memcpy(skb_put(grp->rcvd_xid_skb, TH_HEADER_LENGTH), &thnorm,
-	       TH_HEADER_LENGTH);
+	skb_put_data(grp->rcvd_xid_skb, &thnorm, TH_HEADER_LENGTH);
 
 	if (grp->send_qllc_disc == 1) {
 		grp->send_qllc_disc = 0;
@@ -1590,8 +1586,7 @@ static int mpc_validate_xid(struct mpcg_info *mpcginfo)
 		grp->saved_xid2 =
 			(struct xid2 *)skb_tail_pointer(grp->rcvd_xid_skb);
 
-		memcpy(skb_put(grp->rcvd_xid_skb,
-					XID2_LENGTH), xid, XID2_LENGTH);
+		skb_put_data(grp->rcvd_xid_skb, xid, XID2_LENGTH);
 		grp->rcvd_xid_skb->data = grp->rcvd_xid_data;
 
 		skb_reset_tail_pointer(grp->rcvd_xid_skb);
@@ -1908,17 +1903,15 @@ static void mpc_action_doxid7(fsm_instance *fsm, int event, void *arg)
 				if (fsm_getstate(ch->fsm) == CH_XID7_PENDING1) {
 					fsm_newstate(ch->fsm, CH_XID7_PENDING2);
 					ch->ccw[8].cmd_code = CCW_CMD_SENSE_CMD;
-					memcpy(skb_put(ch->xid_skb,
-							TH_HEADER_LENGTH),
-					       &thdummy, TH_HEADER_LENGTH);
+					skb_put_data(ch->xid_skb, &thdummy,
+						     TH_HEADER_LENGTH);
 					send = 1;
 				}
 			} else if (fsm_getstate(ch->fsm) < CH_XID7_PENDING2) {
 					fsm_newstate(ch->fsm, CH_XID7_PENDING2);
 					ch->ccw[8].cmd_code = CCW_CMD_WRITE_CTL;
-					memcpy(skb_put(ch->xid_skb,
-						       TH_HEADER_LENGTH),
-					       &thnorm, TH_HEADER_LENGTH);
+					skb_put_data(ch->xid_skb, &thnorm,
+						     TH_HEADER_LENGTH);
 					send = 1;
 			}
 		} else {
@@ -1926,17 +1919,16 @@ static void mpc_action_doxid7(fsm_instance *fsm, int event, void *arg)
 			if (grp->roll == YSIDE) {
 				if (fsm_getstate(ch->fsm) < CH_XID7_PENDING4) {
 					fsm_newstate(ch->fsm, CH_XID7_PENDING4);
-					memcpy(skb_put(ch->xid_skb,
-						       TH_HEADER_LENGTH),
-					       &thnorm, TH_HEADER_LENGTH);
+					skb_put_data(ch->xid_skb, &thnorm,
+						     TH_HEADER_LENGTH);
 					ch->ccw[8].cmd_code = CCW_CMD_WRITE_CTL;
 					send = 1;
 				}
 			} else if (fsm_getstate(ch->fsm) == CH_XID7_PENDING3) {
 				fsm_newstate(ch->fsm, CH_XID7_PENDING4);
 				ch->ccw[8].cmd_code = CCW_CMD_SENSE_CMD;
-				memcpy(skb_put(ch->xid_skb, TH_HEADER_LENGTH),
-						&thdummy, TH_HEADER_LENGTH);
+				skb_put_data(ch->xid_skb, &thdummy,
+					     TH_HEADER_LENGTH);
 				send = 1;
 			}
 		}
@@ -2122,7 +2114,7 @@ static int mpc_send_qllc_discontact(struct net_device *dev)
 			return -ENOMEM;
 		}
 
-		memcpy(skb_put(skb, new_len), qllcptr, new_len);
+		skb_put_data(skb, qllcptr, new_len);
 		kfree(qllcptr);
 
 		if (skb_headroom(skb) < 4) {
diff --git a/drivers/s390/net/lcs.c b/drivers/s390/net/lcs.c
index 211b31d9f157..619da81dca70 100644
--- a/drivers/s390/net/lcs.c
+++ b/drivers/s390/net/lcs.c
@@ -1796,7 +1796,7 @@ lcs_get_skb(struct lcs_card *card, char *skb_data, unsigned int skb_len)
 		card->stats.rx_dropped++;
 		return;
 	}
-	memcpy(skb_put(skb, skb_len), skb_data, skb_len);
+	skb_put_data(skb, skb_data, skb_len);
 	skb->protocol =	card->lan_type_trans(skb, card->dev);
 	card->stats.rx_bytes += skb_len;
 	card->stats.rx_packets++;
@@ -2411,14 +2411,14 @@ static struct ccwgroup_driver lcs_group_driver = {
 	.restore     = lcs_restore,
 };
 
-static ssize_t lcs_driver_group_store(struct device_driver *ddrv,
-				      const char *buf, size_t count)
+static ssize_t group_store(struct device_driver *ddrv, const char *buf,
+			   size_t count)
 {
 	int err;
 	err = ccwgroup_create_dev(lcs_root_dev, &lcs_group_driver, 2, buf);
 	return err ? err : count;
 }
-static DRIVER_ATTR(group, 0200, NULL, lcs_driver_group_store);
+static DRIVER_ATTR_WO(group);
 
 static struct attribute *lcs_drv_attrs[] = {
 	&driver_attr_group.attr,
diff --git a/drivers/s390/net/netiucv.c b/drivers/s390/net/netiucv.c
index dba94b486f05..7e0e6a4019f3 100644
--- a/drivers/s390/net/netiucv.c
+++ b/drivers/s390/net/netiucv.c
@@ -743,7 +743,7 @@ static void conn_action_txdone(fsm_instance *fi, int event, void *arg)
 	conn->prof.tx_pending--;
 	if (single_flag) {
 		if ((skb = skb_dequeue(&conn->commit_queue))) {
-			atomic_dec(&skb->users);
+			refcount_dec(&skb->users);
 			if (privptr) {
 				privptr->stats.tx_packets++;
 				privptr->stats.tx_bytes +=
@@ -759,15 +759,14 @@ static void conn_action_txdone(fsm_instance *fi, int event, void *arg)
 	spin_lock_irqsave(&conn->collect_lock, saveflags);
 	while ((skb = skb_dequeue(&conn->collect_queue))) {
 		header.next = conn->tx_buff->len + skb->len + NETIUCV_HDRLEN;
-		memcpy(skb_put(conn->tx_buff, NETIUCV_HDRLEN), &header,
-		       NETIUCV_HDRLEN);
+		skb_put_data(conn->tx_buff, &header, NETIUCV_HDRLEN);
 		skb_copy_from_linear_data(skb,
 					  skb_put(conn->tx_buff, skb->len),
 					  skb->len);
 		txbytes += skb->len;
 		txpackets++;
 		stat_maxcq++;
-		atomic_dec(&skb->users);
+		refcount_dec(&skb->users);
 		dev_kfree_skb_any(skb);
 	}
 	if (conn->collect_len > conn->prof.maxmulti)
@@ -780,7 +779,7 @@ static void conn_action_txdone(fsm_instance *fi, int event, void *arg)
 	}
 
 	header.next = 0;
-	memcpy(skb_put(conn->tx_buff, NETIUCV_HDRLEN), &header, NETIUCV_HDRLEN);
+	skb_put_data(conn->tx_buff, &header, NETIUCV_HDRLEN);
 	conn->prof.send_stamp = jiffies;
 	txmsg.class = 0;
 	txmsg.tag = 0;
@@ -959,7 +958,7 @@ static void netiucv_purge_skb_queue(struct sk_buff_head *q)
 	struct sk_buff *skb;
 
 	while ((skb = skb_dequeue(q))) {
-		atomic_dec(&skb->users);
+		refcount_dec(&skb->users);
 		dev_kfree_skb_any(skb);
 	}
 }
@@ -1177,7 +1176,7 @@ static int netiucv_transmit_skb(struct iucv_connection *conn,
 			IUCV_DBF_TEXT(data, 2,
 				      "EBUSY from netiucv_transmit_skb\n");
 		} else {
-			atomic_inc(&skb->users);
+			refcount_inc(&skb->users);
 			skb_queue_tail(&conn->collect_queue, skb);
 			conn->collect_len += l;
 			rc = 0;
@@ -1201,8 +1200,7 @@ static int netiucv_transmit_skb(struct iucv_connection *conn,
 				return rc;
 			} else {
 				skb_reserve(nskb, NETIUCV_HDRLEN);
-				memcpy(skb_put(nskb, skb->len),
-				       skb->data, skb->len);
+				skb_put_data(nskb, skb->data, skb->len);
 			}
 			copied = 1;
 		}
@@ -1212,7 +1210,7 @@ static int netiucv_transmit_skb(struct iucv_connection *conn,
 		header.next = nskb->len + NETIUCV_HDRLEN;
 		memcpy(skb_push(nskb, NETIUCV_HDRLEN), &header, NETIUCV_HDRLEN);
 		header.next = 0;
-		memcpy(skb_put(nskb, NETIUCV_HDRLEN), &header,  NETIUCV_HDRLEN);
+		skb_put_data(nskb, &header, NETIUCV_HDRLEN);
 
 		fsm_newstate(conn->fsm, CONN_STATE_TX);
 		conn->prof.send_stamp = jiffies;
@@ -1247,7 +1245,7 @@ static int netiucv_transmit_skb(struct iucv_connection *conn,
 		} else {
 			if (copied)
 				dev_kfree_skb(skb);
-			atomic_inc(&nskb->users);
+			refcount_inc(&nskb->users);
 			skb_queue_tail(&conn->commit_queue, nskb);
 		}
 	}
@@ -1954,7 +1952,6 @@ static void netiucv_free_netdevice(struct net_device *dev)
 		privptr->conn = NULL; privptr->fsm = NULL;
 		/* privptr gets freed by free_netdev() */
 	}
-	free_netdev(dev);
 }
 
 /**
@@ -1972,7 +1969,8 @@ static void netiucv_setup_netdevice(struct net_device *dev)
 	dev->mtu	         = NETIUCV_MTU_DEFAULT;
 	dev->min_mtu		 = 576;
 	dev->max_mtu		 = NETIUCV_MTU_MAX;
-	dev->destructor          = netiucv_free_netdevice;
+	dev->needs_free_netdev   = true;
+	dev->priv_destructor     = netiucv_free_netdevice;
 	dev->hard_header_len     = NETIUCV_HDRLEN;
 	dev->addr_len            = 0;
 	dev->type                = ARPHRD_SLIP;
@@ -2020,8 +2018,8 @@ out_netdev:
 	return NULL;
 }
 
-static ssize_t conn_write(struct device_driver *drv,
-			  const char *buf, size_t count)
+static ssize_t connection_store(struct device_driver *drv, const char *buf,
+				size_t count)
 {
 	char username[9];
 	char userdata[17];
@@ -2082,11 +2080,10 @@ out_free_ndev:
 	netiucv_free_netdevice(dev);
 	return rc;
 }
+static DRIVER_ATTR_WO(connection);
 
-static DRIVER_ATTR(connection, 0200, NULL, conn_write);
-
-static ssize_t remove_write (struct device_driver *drv,
-			     const char *buf, size_t count)
+static ssize_t remove_store(struct device_driver *drv, const char *buf,
+			    size_t count)
 {
 	struct iucv_connection *cp;
         struct net_device *ndev;
@@ -2132,8 +2129,7 @@ static ssize_t remove_write (struct device_driver *drv,
 	IUCV_DBF_TEXT(data, 2, "remove_write: unknown device\n");
         return -EINVAL;
 }
-
-static DRIVER_ATTR(remove, 0200, NULL, remove_write);
+static DRIVER_ATTR_WO(remove);
 
 static struct attribute * netiucv_drv_attrs[] = {
 	&driver_attr_connection.attr,
diff --git a/drivers/s390/net/qeth_core.h b/drivers/s390/net/qeth_core.h
index 30bc6105aac3..7a0ffc71b25d 100644
--- a/drivers/s390/net/qeth_core.h
+++ b/drivers/s390/net/qeth_core.h
@@ -659,6 +659,7 @@ struct qeth_card_info {
 	int max_mtu;
 	int broadcast_capable;
 	int unique_id;
+	bool layer_enforced;
 	struct qeth_card_blkt blkt;
 	enum qeth_ipa_promisc_modes promisc_mode;
 	__u32 diagass_support;
@@ -696,6 +697,7 @@ struct qeth_osn_info {
 };
 
 enum qeth_discipline_id {
+	QETH_DISCIPLINE_UNDETERMINED = -1,
 	QETH_DISCIPLINE_LAYER3 = 0,
 	QETH_DISCIPLINE_LAYER2 = 1,
 };
@@ -984,6 +986,7 @@ struct qeth_cmd_buffer *qeth_get_setassparms_cmd(struct qeth_card *,
 int qeth_set_features(struct net_device *, netdev_features_t);
 int qeth_recover_features(struct net_device *);
 netdev_features_t qeth_fix_features(struct net_device *, netdev_features_t);
+int qeth_vm_request_mac(struct qeth_card *card);
 
 /* exports for OSN */
 int qeth_osn_assist(struct net_device *, void *, int);
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index fc6d85f2b38d..4792cabb862e 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -27,6 +27,9 @@
 #include <asm/io.h>
 #include <asm/sysinfo.h>
 #include <asm/compat.h>
+#include <asm/diag.h>
+#include <asm/cio.h>
+#include <asm/ccwdev.h>
 
 #include "qeth_core.h"
 
@@ -1239,7 +1242,7 @@ static void qeth_release_skbs(struct qeth_qdio_out_buffer *buf)
 				iucv->sk_txnotify(skb, TX_NOTIFY_GENERALERROR);
 			}
 		}
-		atomic_dec(&skb->users);
+		refcount_dec(&skb->users);
 		dev_kfree_skb_any(skb);
 		skb = skb_dequeue(&buf->skb_list);
 	}
@@ -1723,6 +1726,25 @@ static void qeth_configure_unitaddr(struct qeth_card *card, char *prcd)
 			       (prcd[0x11] == _ascebc['M']));
 }
 
+/* Determine whether the device requires a specific layer discipline */
+static enum qeth_discipline_id qeth_enforce_discipline(struct qeth_card *card)
+{
+	if (card->info.type == QETH_CARD_TYPE_OSM ||
+	    card->info.type == QETH_CARD_TYPE_OSN) {
+		QETH_DBF_TEXT(SETUP, 3, "force l2");
+		return QETH_DISCIPLINE_LAYER2;
+	}
+
+	/* virtual HiperSocket is L3 only: */
+	if (card->info.guestlan && card->info.type == QETH_CARD_TYPE_IQD) {
+		QETH_DBF_TEXT(SETUP, 3, "force l3");
+		return QETH_DISCIPLINE_LAYER3;
+	}
+
+	QETH_DBF_TEXT(SETUP, 3, "force no");
+	return QETH_DISCIPLINE_UNDETERMINED;
+}
+
 static void qeth_configure_blkt_default(struct qeth_card *card, char *prcd)
 {
 	QETH_DBF_TEXT(SETUP, 2, "cfgblkt");
@@ -3347,6 +3369,28 @@ static void qeth_handle_send_error(struct qeth_card *card,
 		       (u16)qdio_err, (u8)sbalf15);
 }
 
+/**
+ * qeth_prep_flush_pack_buffer - Prepares flushing of a packing buffer.
+ * @queue: queue to check for packing buffer
+ *
+ * Returns number of buffers that were prepared for flush.
+ */
+static int qeth_prep_flush_pack_buffer(struct qeth_qdio_out_q *queue)
+{
+	struct qeth_qdio_out_buffer *buffer;
+
+	buffer = queue->bufs[queue->next_buf_to_fill];
+	if ((atomic_read(&buffer->state) == QETH_QDIO_BUF_EMPTY) &&
+	    (buffer->next_element_to_fill > 0)) {
+		/* it's a packing buffer */
+		atomic_set(&buffer->state, QETH_QDIO_BUF_PRIMED);
+		queue->next_buf_to_fill =
+			(queue->next_buf_to_fill + 1) % QDIO_MAX_BUFFERS_PER_Q;
+		return 1;
+	}
+	return 0;
+}
+
 /*
  * Switched to packing state if the number of used buffers on a queue
  * reaches a certain limit.
@@ -3373,9 +3417,6 @@ static void qeth_switch_to_packing_if_needed(struct qeth_qdio_out_q *queue)
  */
 static int qeth_switch_to_nonpacking_if_needed(struct qeth_qdio_out_q *queue)
 {
-	struct qeth_qdio_out_buffer *buffer;
-	int flush_count = 0;
-
 	if (queue->do_pack) {
 		if (atomic_read(&queue->used_buffers)
 		    <= QETH_LOW_WATERMARK_PACK) {
@@ -3384,42 +3425,9 @@ static int qeth_switch_to_nonpacking_if_needed(struct qeth_qdio_out_q *queue)
 			if (queue->card->options.performance_stats)
 				queue->card->perf_stats.sc_p_dp++;
 			queue->do_pack = 0;
-			/* flush packing buffers */
-			buffer = queue->bufs[queue->next_buf_to_fill];
-			if ((atomic_read(&buffer->state) ==
-						QETH_QDIO_BUF_EMPTY) &&
-			    (buffer->next_element_to_fill > 0)) {
-				atomic_set(&buffer->state,
-					   QETH_QDIO_BUF_PRIMED);
-				flush_count++;
-				queue->next_buf_to_fill =
-					(queue->next_buf_to_fill + 1) %
-					QDIO_MAX_BUFFERS_PER_Q;
-			}
+			return qeth_prep_flush_pack_buffer(queue);
 		}
 	}
-	return flush_count;
-}
-
-
-/*
- * Called to flush a packing buffer if no more pci flags are on the queue.
- * Checks if there is a packing buffer and prepares it to be flushed.
- * In that case returns 1, otherwise zero.
- */
-static int qeth_flush_buffers_on_no_pci(struct qeth_qdio_out_q *queue)
-{
-	struct qeth_qdio_out_buffer *buffer;
-
-	buffer = queue->bufs[queue->next_buf_to_fill];
-	if ((atomic_read(&buffer->state) == QETH_QDIO_BUF_EMPTY) &&
-	   (buffer->next_element_to_fill > 0)) {
-		/* it's a packing buffer */
-		atomic_set(&buffer->state, QETH_QDIO_BUF_PRIMED);
-		queue->next_buf_to_fill =
-			(queue->next_buf_to_fill + 1) % QDIO_MAX_BUFFERS_PER_Q;
-		return 1;
-	}
 	return 0;
 }
 
@@ -3532,8 +3540,7 @@ static void qeth_check_outbound_queue(struct qeth_qdio_out_q *queue)
 			flush_cnt += qeth_switch_to_nonpacking_if_needed(queue);
 			if (!flush_cnt &&
 			    !atomic_read(&queue->set_pci_flags_count))
-				flush_cnt +=
-					qeth_flush_buffers_on_no_pci(queue);
+				flush_cnt += qeth_prep_flush_pack_buffer(queue);
 			if (queue->card->options.performance_stats &&
 			    q_was_packing)
 				queue->card->perf_stats.bufs_sent_pack +=
@@ -3968,7 +3975,7 @@ static inline int qeth_fill_buffer(struct qeth_qdio_out_q *queue,
 	int flush_cnt = 0, hdr_len, large_send = 0;
 
 	buffer = buf->buffer;
-	atomic_inc(&skb->users);
+	refcount_inc(&skb->users);
 	skb_queue_tail(&buf->skb_list, skb);
 
 	/*check first on TSO ....*/
@@ -4099,7 +4106,8 @@ int qeth_do_send_packet(struct qeth_card *card, struct qeth_qdio_out_q *queue,
 							   flush_count);
 				atomic_set(&queue->state,
 						QETH_OUT_Q_UNLOCKED);
-				return -EBUSY;
+				rc = -EBUSY;
+				goto out;
 			}
 		}
 	}
@@ -4118,19 +4126,21 @@ int qeth_do_send_packet(struct qeth_card *card, struct qeth_qdio_out_q *queue,
 	 * In that case we will enter this loop
 	 */
 	while (atomic_dec_return(&queue->state)) {
-		flush_count = 0;
 		start_index = queue->next_buf_to_fill;
 		/* check if we can go back to non-packing state */
-		flush_count += qeth_switch_to_nonpacking_if_needed(queue);
+		tmp = qeth_switch_to_nonpacking_if_needed(queue);
 		/*
 		 * check if we need to flush a packing buffer to get a pci
 		 * flag out on the queue
 		 */
-		if (!flush_count && !atomic_read(&queue->set_pci_flags_count))
-			flush_count += qeth_flush_buffers_on_no_pci(queue);
-		if (flush_count)
-			qeth_flush_buffers(queue, start_index, flush_count);
+		if (!tmp && !atomic_read(&queue->set_pci_flags_count))
+			tmp = qeth_prep_flush_pack_buffer(queue);
+		if (tmp) {
+			qeth_flush_buffers(queue, start_index, tmp);
+			flush_count += tmp;
+		}
 	}
+out:
 	/* at this point the queue is UNLOCKED again */
 	if (queue->card->options.performance_stats && do_pack)
 		queue->card->perf_stats.bufs_sent_pack += flush_count;
@@ -4199,8 +4209,7 @@ int qeth_change_mtu(struct net_device *dev, int new_mtu)
 	sprintf(dbf_text, "%8x", new_mtu);
 	QETH_CARD_TEXT(card, 4, dbf_text);
 
-	if ((!qeth_is_supported(card, IPA_IP_FRAGMENTATION)) &&
-	    (!qeth_mtu_is_valid(card, new_mtu)))
+	if (!qeth_mtu_is_valid(card, new_mtu))
 		return -EINVAL;
 	dev->mtu = new_mtu;
 	return 0;
@@ -4767,6 +4776,64 @@ static int qeth_query_card_info(struct qeth_card *card,
 					(void *)carrier_info);
 }
 
+/**
+ * qeth_vm_request_mac() - Request a hypervisor-managed MAC address
+ * @card: pointer to a qeth_card
+ *
+ * Returns
+ *	0, if a MAC address has been set for the card's netdevice
+ *	a return code, for various error conditions
+ */
+int qeth_vm_request_mac(struct qeth_card *card)
+{
+	struct diag26c_mac_resp *response;
+	struct diag26c_mac_req *request;
+	struct ccw_dev_id id;
+	int rc;
+
+	QETH_DBF_TEXT(SETUP, 2, "vmreqmac");
+
+	if (!card->dev)
+		return -ENODEV;
+
+	request = kzalloc(sizeof(*request), GFP_KERNEL | GFP_DMA);
+	response = kzalloc(sizeof(*response), GFP_KERNEL | GFP_DMA);
+	if (!request || !response) {
+		rc = -ENOMEM;
+		goto out;
+	}
+
+	ccw_device_get_id(CARD_DDEV(card), &id);
+	request->resp_buf_len = sizeof(*response);
+	request->resp_version = DIAG26C_VERSION2;
+	request->op_code = DIAG26C_GET_MAC;
+	request->devno = id.devno;
+
+	rc = diag26c(request, response, DIAG26C_MAC_SERVICES);
+	if (rc)
+		goto out;
+
+	if (request->resp_buf_len < sizeof(*response) ||
+	    response->version != request->resp_version) {
+		rc = -EIO;
+		QETH_DBF_TEXT(SETUP, 2, "badresp");
+		QETH_DBF_HEX(SETUP, 2, &request->resp_buf_len,
+			     sizeof(request->resp_buf_len));
+	} else if (!is_valid_ether_addr(response->mac)) {
+		rc = -EINVAL;
+		QETH_DBF_TEXT(SETUP, 2, "badmac");
+		QETH_DBF_HEX(SETUP, 2, response->mac, ETH_ALEN);
+	} else {
+		ether_addr_copy(card->dev->dev_addr, response->mac);
+	}
+
+out:
+	kfree(response);
+	kfree(request);
+	return rc;
+}
+EXPORT_SYMBOL_GPL(qeth_vm_request_mac);
+
 static inline int qeth_get_qdio_q_format(struct qeth_card *card)
 {
 	if (card->info.type == QETH_CARD_TYPE_IQD)
@@ -5144,12 +5211,11 @@ static inline int qeth_create_skb_frag(struct qeth_qdio_buffer *qethbuffer,
 
 		skb_reserve(*pskb, ETH_HLEN);
 		if (data_len <= QETH_RX_PULL_LEN) {
-			memcpy(skb_put(*pskb, data_len), element->addr + offset,
-				data_len);
+			skb_put_data(*pskb, element->addr + offset, data_len);
 		} else {
 			get_page(page);
-			memcpy(skb_put(*pskb, QETH_RX_PULL_LEN),
-			       element->addr + offset, QETH_RX_PULL_LEN);
+			skb_put_data(*pskb, element->addr + offset,
+				     QETH_RX_PULL_LEN);
 			skb_fill_page_desc(*pskb, *pfrag, page,
 				offset + QETH_RX_PULL_LEN,
 				data_len - QETH_RX_PULL_LEN);
@@ -5245,8 +5311,7 @@ struct sk_buff *qeth_core_get_next_skb(struct qeth_card *card,
 				    &skb, offset, &frag, data_len))
 					goto no_mem;
 			} else {
-				memcpy(skb_put(skb, data_len), data_ptr,
-					data_len);
+				skb_put_data(skb, data_ptr, data_len);
 			}
 		}
 		skb_len -= data_len;
@@ -5501,6 +5566,7 @@ int qeth_core_load_discipline(struct qeth_card *card,
 		enum qeth_discipline_id discipline)
 {
 	int rc = 0;
+
 	mutex_lock(&qeth_mod_mutex);
 	switch (discipline) {
 	case QETH_DISCIPLINE_LAYER3:
@@ -5511,7 +5577,10 @@ int qeth_core_load_discipline(struct qeth_card *card,
 		card->discipline = try_then_request_module(
 			symbol_get(qeth_l2_discipline), "qeth_l2");
 		break;
+	default:
+		break;
 	}
+
 	if (!card->discipline) {
 		dev_err(&card->gdev->dev, "There is no kernel module to "
 			"support discipline %d\n", discipline);
@@ -5614,6 +5683,7 @@ static int qeth_core_probe_device(struct ccwgroup_device *gdev)
 	struct qeth_card *card;
 	struct device *dev;
 	int rc;
+	enum qeth_discipline_id enforced_disc;
 	unsigned long flags;
 	char dbf_name[DBF_NAME_LEN];
 
@@ -5661,10 +5731,15 @@ static int qeth_core_probe_device(struct ccwgroup_device *gdev)
 		goto err_card;
 	}
 
-	switch (card->info.type) {
-	case QETH_CARD_TYPE_OSN:
-	case QETH_CARD_TYPE_OSM:
-		rc = qeth_core_load_discipline(card, QETH_DISCIPLINE_LAYER2);
+	qeth_determine_capabilities(card);
+	enforced_disc = qeth_enforce_discipline(card);
+	switch (enforced_disc) {
+	case QETH_DISCIPLINE_UNDETERMINED:
+		gdev->dev.type = &qeth_generic_devtype;
+		break;
+	default:
+		card->info.layer_enforced = true;
+		rc = qeth_core_load_discipline(card, enforced_disc);
 		if (rc)
 			goto err_card;
 
@@ -5675,16 +5750,11 @@ static int qeth_core_probe_device(struct ccwgroup_device *gdev)
 		if (rc)
 			goto err_disc;
 		break;
-	default:
-		gdev->dev.type = &qeth_generic_devtype;
-		break;
 	}
 
 	write_lock_irqsave(&qeth_core_card_list.rwlock, flags);
 	list_add_tail(&card->list, &qeth_core_card_list.list);
 	write_unlock_irqrestore(&qeth_core_card_list.rwlock, flags);
-
-	qeth_determine_capabilities(card);
 	return 0;
 
 err_disc:
@@ -5721,7 +5791,7 @@ static int qeth_core_set_online(struct ccwgroup_device *gdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&gdev->dev);
 	int rc = 0;
-	int def_discipline;
+	enum qeth_discipline_id def_discipline;
 
 	if (!card->discipline) {
 		if (card->info.type == QETH_CARD_TYPE_IQD)
@@ -5800,8 +5870,8 @@ static struct ccwgroup_driver qeth_core_ccwgroup_driver = {
 	.restore = qeth_core_restore,
 };
 
-static ssize_t qeth_core_driver_group_store(struct device_driver *ddrv,
-					    const char *buf, size_t count)
+static ssize_t group_store(struct device_driver *ddrv, const char *buf,
+			   size_t count)
 {
 	int err;
 
@@ -5810,7 +5880,7 @@ static ssize_t qeth_core_driver_group_store(struct device_driver *ddrv,
 
 	return err ? err : count;
 }
-static DRIVER_ATTR(group, 0200, NULL, qeth_core_driver_group_store);
+static DRIVER_ATTR_WO(group);
 
 static struct attribute *qeth_drv_attrs[] = {
 	&driver_attr_group.attr,
@@ -6406,11 +6476,8 @@ netdev_features_t qeth_fix_features(struct net_device *dev,
 		features &= ~NETIF_F_IP_CSUM;
 	if (!qeth_is_supported(card, IPA_INBOUND_CHECKSUM))
 		features &= ~NETIF_F_RXCSUM;
-	if (!qeth_is_supported(card, IPA_OUTBOUND_TSO)) {
+	if (!qeth_is_supported(card, IPA_OUTBOUND_TSO))
 		features &= ~NETIF_F_TSO;
-		dev_info(&card->gdev->dev, "Outbound TSO not supported on %s\n",
-			 QETH_CARD_IFNAME(card));
-	}
 	/* if the card isn't up, remove features that require hw changes */
 	if (card->state == CARD_STATE_DOWN ||
 	    card->state == CARD_STATE_RECOVER)
diff --git a/drivers/s390/net/qeth_core_mpc.c b/drivers/s390/net/qeth_core_mpc.c
index beb4bdc26de5..6dd7d05e5693 100644
--- a/drivers/s390/net/qeth_core_mpc.c
+++ b/drivers/s390/net/qeth_core_mpc.c
@@ -167,13 +167,21 @@ static struct ipa_rc_msg qeth_ipa_rc_msg[] = {
 	{IPA_RC_IP_TABLE_FULL,		"Add Addr IP Table Full - ipv6"},
 	{IPA_RC_UNKNOWN_ERROR,		"IPA command failed - reason unknown"},
 	{IPA_RC_UNSUPPORTED_COMMAND,	"Command not supported"},
+	{IPA_RC_TRACE_ALREADY_ACTIVE,	"trace already active"},
+	{IPA_RC_INVALID_FORMAT,		"invalid format or length"},
 	{IPA_RC_DUP_IPV6_REMOTE, "ipv6 address already registered remote"},
+	{IPA_RC_SBP_IQD_NOT_CONFIGURED,	"Not configured for bridgeport"},
 	{IPA_RC_DUP_IPV6_HOME,		"ipv6 address already registered"},
 	{IPA_RC_UNREGISTERED_ADDR,	"Address not registered"},
 	{IPA_RC_NO_ID_AVAILABLE,	"No identifiers available"},
 	{IPA_RC_ID_NOT_FOUND,		"Identifier not found"},
+	{IPA_RC_SBP_IQD_ANO_DEV_PRIMARY, "Primary bridgeport exists already"},
+	{IPA_RC_SBP_IQD_CURRENT_SECOND,	"Bridgeport is currently secondary"},
+	{IPA_RC_SBP_IQD_LIMIT_SECOND, "Limit of secondary bridgeports reached"},
 	{IPA_RC_INVALID_IP_VERSION,	"IP version incorrect"},
+	{IPA_RC_SBP_IQD_CURRENT_PRIMARY, "Bridgeport is currently primary"},
 	{IPA_RC_LAN_FRAME_MISMATCH,	"LAN and frame mismatch"},
+	{IPA_RC_SBP_IQD_NO_QDIO_QUEUES,	"QDIO queues not established"},
 	{IPA_RC_L2_UNSUPPORTED_CMD,	"Unsupported layer 2 command"},
 	{IPA_RC_L2_DUP_MAC,		"Duplicate MAC address"},
 	{IPA_RC_L2_ADDR_TABLE_FULL,	"Layer2 address table full"},
@@ -185,6 +193,14 @@ static struct ipa_rc_msg qeth_ipa_rc_msg[] = {
 	{IPA_RC_L2_INVALID_VLAN_ID,	"L2 invalid vlan id"},
 	{IPA_RC_L2_DUP_VLAN_ID,		"L2 duplicate vlan id"},
 	{IPA_RC_L2_VLAN_ID_NOT_FOUND,	"L2 vlan id not found"},
+	{IPA_RC_SBP_OSA_NOT_CONFIGURED,	"Not configured for bridgeport"},
+	{IPA_RC_SBP_OSA_OS_MISMATCH,	"OS mismatch"},
+	{IPA_RC_SBP_OSA_ANO_DEV_PRIMARY, "Primary bridgeport exists already"},
+	{IPA_RC_SBP_OSA_CURRENT_SECOND,	"Bridgeport is currently secondary"},
+	{IPA_RC_SBP_OSA_LIMIT_SECOND, "Limit of secondary bridgeports reached"},
+	{IPA_RC_SBP_OSA_NOT_AUTHD_BY_ZMAN, "Not authorized by zManager"},
+	{IPA_RC_SBP_OSA_CURRENT_PRIMARY, "Bridgeport is currently primary"},
+	{IPA_RC_SBP_OSA_NO_QDIO_QUEUES,	"QDIO queues not established"},
 	{IPA_RC_DATA_MISMATCH,		"Data field mismatch (v4/v6 mixed)"},
 	{IPA_RC_INVALID_MTU_SIZE,	"Invalid MTU size"},
 	{IPA_RC_INVALID_LANTYPE,	"Invalid LAN type"},
diff --git a/drivers/s390/net/qeth_core_mpc.h b/drivers/s390/net/qeth_core_mpc.h
index 4accb0a61ce0..912e0107de8f 100644
--- a/drivers/s390/net/qeth_core_mpc.h
+++ b/drivers/s390/net/qeth_core_mpc.h
@@ -142,12 +142,18 @@ enum qeth_ipa_return_codes {
 	IPA_RC_TRACE_ALREADY_ACTIVE	= 0x0005,
 	IPA_RC_INVALID_FORMAT		= 0x0006,
 	IPA_RC_DUP_IPV6_REMOTE		= 0x0008,
+	IPA_RC_SBP_IQD_NOT_CONFIGURED	= 0x000C,
 	IPA_RC_DUP_IPV6_HOME		= 0x0010,
 	IPA_RC_UNREGISTERED_ADDR	= 0x0011,
 	IPA_RC_NO_ID_AVAILABLE		= 0x0012,
 	IPA_RC_ID_NOT_FOUND		= 0x0013,
+	IPA_RC_SBP_IQD_ANO_DEV_PRIMARY	= 0x0014,
+	IPA_RC_SBP_IQD_CURRENT_SECOND	= 0x0018,
+	IPA_RC_SBP_IQD_LIMIT_SECOND	= 0x001C,
 	IPA_RC_INVALID_IP_VERSION	= 0x0020,
+	IPA_RC_SBP_IQD_CURRENT_PRIMARY	= 0x0024,
 	IPA_RC_LAN_FRAME_MISMATCH	= 0x0040,
+	IPA_RC_SBP_IQD_NO_QDIO_QUEUES	= 0x00EB,
 	IPA_RC_L2_UNSUPPORTED_CMD	= 0x2003,
 	IPA_RC_L2_DUP_MAC		= 0x2005,
 	IPA_RC_L2_ADDR_TABLE_FULL	= 0x2006,
@@ -159,6 +165,14 @@ enum qeth_ipa_return_codes {
 	IPA_RC_L2_INVALID_VLAN_ID	= 0x2015,
 	IPA_RC_L2_DUP_VLAN_ID		= 0x2016,
 	IPA_RC_L2_VLAN_ID_NOT_FOUND	= 0x2017,
+	IPA_RC_SBP_OSA_NOT_CONFIGURED	= 0x2B0C,
+	IPA_RC_SBP_OSA_OS_MISMATCH	= 0x2B10,
+	IPA_RC_SBP_OSA_ANO_DEV_PRIMARY	= 0x2B14,
+	IPA_RC_SBP_OSA_CURRENT_SECOND	= 0x2B18,
+	IPA_RC_SBP_OSA_LIMIT_SECOND	= 0x2B1C,
+	IPA_RC_SBP_OSA_NOT_AUTHD_BY_ZMAN = 0x2B20,
+	IPA_RC_SBP_OSA_CURRENT_PRIMARY	= 0x2B24,
+	IPA_RC_SBP_OSA_NO_QDIO_QUEUES	= 0x2BEB,
 	IPA_RC_DATA_MISMATCH		= 0xe001,
 	IPA_RC_INVALID_MTU_SIZE		= 0xe002,
 	IPA_RC_INVALID_LANTYPE		= 0xe003,
@@ -187,12 +201,16 @@ enum qeth_ipa_return_codes {
 #define IPA_RC_INVALID_SUBCMD		IPA_RC_IP_TABLE_FULL
 #define IPA_RC_HARDWARE_AUTH_ERROR	IPA_RC_UNKNOWN_ERROR
 
+/* for SETBRIDGEPORT (double occupancies) */
+#define IPA_RC_SBP_IQD_OS_MISMATCH	 IPA_RC_DUP_IPV6_HOME
+#define IPA_RC_SBP_IQD_NOT_AUTHD_BY_ZMAN IPA_RC_INVALID_IP_VERSION
+
 /* IPA function flags; each flag marks availability of respective function */
 enum qeth_ipa_funcs {
 	IPA_ARP_PROCESSING      = 0x00000001L,
 	IPA_INBOUND_CHECKSUM    = 0x00000002L,
 	IPA_OUTBOUND_CHECKSUM   = 0x00000004L,
-	IPA_IP_FRAGMENTATION    = 0x00000008L,
+	/* RESERVED		= 0x00000008L,*/
 	IPA_FILTERING           = 0x00000010L,
 	IPA_IPV6                = 0x00000020L,
 	IPA_MULTICASTING        = 0x00000040L,
diff --git a/drivers/s390/net/qeth_core_sys.c b/drivers/s390/net/qeth_core_sys.c
index db6a285d41e0..6d255c22656d 100644
--- a/drivers/s390/net/qeth_core_sys.c
+++ b/drivers/s390/net/qeth_core_sys.c
@@ -413,7 +413,7 @@ static ssize_t qeth_dev_layer2_store(struct device *dev,
 
 	if (card->options.layer2 == newdis)
 		goto out;
-	if (card->info.type == QETH_CARD_TYPE_OSM) {
+	if (card->info.layer_enforced) {
 		/* fixed layer, can't switch */
 		rc = -EOPNOTSUPP;
 		goto out;
diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index bd2df62a5cdf..ad110abfdd47 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -21,6 +21,7 @@
 #include <linux/hash.h>
 #include <linux/hashtable.h>
 #include <linux/string.h>
+#include <asm/setup.h>
 #include "qeth_core.h"
 #include "qeth_l2.h"
 
@@ -505,9 +506,19 @@ static int qeth_l2_request_initial_mac(struct qeth_card *card)
 	int rc = 0;
 	char vendor_pre[] = {0x02, 0x00, 0x00};
 
-	QETH_DBF_TEXT(SETUP, 2, "doL2init");
+	QETH_DBF_TEXT(SETUP, 2, "l2reqmac");
 	QETH_DBF_TEXT_(SETUP, 2, "doL2%s", CARD_BUS_ID(card));
 
+	if (MACHINE_IS_VM) {
+		rc = qeth_vm_request_mac(card);
+		if (!rc)
+			goto out;
+		QETH_DBF_MESSAGE(2, "z/VM MAC Service failed on device %s: x%x\n",
+				 CARD_BUS_ID(card), rc);
+		QETH_DBF_TEXT_(SETUP, 2, "err%04x", rc);
+		/* fall back to alternative mechanism: */
+	}
+
 	if (qeth_is_supported(card, IPA_SETADAPTERPARMS)) {
 		rc = qeth_query_setadapterparms(card);
 		if (rc) {
@@ -528,11 +539,12 @@ static int qeth_l2_request_initial_mac(struct qeth_card *card)
 			QETH_DBF_TEXT_(SETUP, 2, "1err%04x", rc);
 			return rc;
 		}
-		QETH_DBF_HEX(SETUP, 2, card->dev->dev_addr, OSA_ADDR_LEN);
 	} else {
 		eth_random_addr(card->dev->dev_addr);
 		memcpy(card->dev->dev_addr, vendor_pre, 3);
 	}
+out:
+	QETH_DBF_HEX(SETUP, 2, card->dev->dev_addr, card->dev->addr_len);
 	return 0;
 }
 
@@ -759,8 +771,7 @@ static netdev_tx_t qeth_l2_hard_start_xmit(struct sk_buff *skb,
 						sizeof(struct qeth_hdr));
 			if (!new_skb)
 				goto tx_drop;
-			hdr = (struct qeth_hdr *)skb_push(new_skb,
-						sizeof(struct qeth_hdr));
+			hdr = skb_push(new_skb, sizeof(struct qeth_hdr));
 			skb_set_mac_header(new_skb, sizeof(struct qeth_hdr));
 			qeth_l2_fill_header(card, hdr, new_skb, cast_type);
 			if (new_skb->ip_summed == CHECKSUM_PARTIAL)
@@ -1017,6 +1028,13 @@ static int qeth_l2_start_ipassists(struct qeth_card *card)
 	return 0;
 }
 
+static void qeth_l2_trace_features(struct qeth_card *card)
+{
+	QETH_CARD_TEXT(card, 2, "l2featur");
+	QETH_CARD_HEX(card, 2, &card->options.sbp.supported_funcs,
+		      sizeof(card->options.sbp.supported_funcs));
+}
+
 static int __qeth_l2_set_online(struct ccwgroup_device *gdev, int recovery_mode)
 {
 	struct qeth_card *card = dev_get_drvdata(&gdev->dev);
@@ -1040,6 +1058,7 @@ static int __qeth_l2_set_online(struct ccwgroup_device *gdev, int recovery_mode)
 		dev_info(&card->gdev->dev,
 		"The device represents a Bridge Capable Port\n");
 	qeth_trace_features(card);
+	qeth_l2_trace_features(card);
 
 	if (!card->dev && qeth_l2_setup_netdev(card)) {
 		rc = -ENODEV;
@@ -1643,27 +1662,27 @@ static int qeth_bridgeport_makerc(struct qeth_card *card,
 	if ((is_iqd && (cbctl->ipa_rc == IPA_RC_SUCCESS)) ||
 	    (!is_iqd && (cbctl->ipa_rc == cbctl->cmd_rc)))
 		switch (cbctl->cmd_rc) {
-		case 0x0000:
+		case IPA_RC_SUCCESS:
 			rc = 0;
 			break;
-		case 0x2B04:
-		case 0x0004:
+		case IPA_RC_L2_UNSUPPORTED_CMD:
+		case IPA_RC_UNSUPPORTED_COMMAND:
 			rc = -EOPNOTSUPP;
 			break;
-		case 0x2B0C:
-		case 0x000C: /* Not configured as bridge Port */
+		case IPA_RC_SBP_OSA_NOT_CONFIGURED:
+		case IPA_RC_SBP_IQD_NOT_CONFIGURED:
 			rc = -ENODEV; /* maybe not the best code here? */
 			dev_err(&card->gdev->dev,
 	"The device is not configured as a Bridge Port\n");
 			break;
-		case 0x2B10:
-		case 0x0010: /* OS mismatch */
+		case IPA_RC_SBP_OSA_OS_MISMATCH:
+		case IPA_RC_SBP_IQD_OS_MISMATCH:
 			rc = -EPERM;
 			dev_err(&card->gdev->dev,
 	"A Bridge Port is already configured by a different operating system\n");
 			break;
-		case 0x2B14:
-		case 0x0014: /* Another device is Primary */
+		case IPA_RC_SBP_OSA_ANO_DEV_PRIMARY:
+		case IPA_RC_SBP_IQD_ANO_DEV_PRIMARY:
 			switch (setcmd) {
 			case IPA_SBP_SET_PRIMARY_BRIDGE_PORT:
 				rc = -EEXIST;
@@ -1679,26 +1698,26 @@ static int qeth_bridgeport_makerc(struct qeth_card *card,
 				rc = -EIO;
 			}
 			break;
-		case 0x2B18:
-		case 0x0018: /* This device is currently Secondary */
+		case IPA_RC_SBP_OSA_CURRENT_SECOND:
+		case IPA_RC_SBP_IQD_CURRENT_SECOND:
 			rc = -EBUSY;
 			dev_err(&card->gdev->dev,
 	"The device is already a secondary Bridge Port\n");
 			break;
-		case 0x2B1C:
-		case 0x001C: /* Limit for Secondary devices reached */
+		case IPA_RC_SBP_OSA_LIMIT_SECOND:
+		case IPA_RC_SBP_IQD_LIMIT_SECOND:
 			rc = -EEXIST;
 			dev_err(&card->gdev->dev,
 	"The LAN cannot have more secondary Bridge Ports\n");
 			break;
-		case 0x2B24:
-		case 0x0024: /* This device is currently Primary */
+		case IPA_RC_SBP_OSA_CURRENT_PRIMARY:
+		case IPA_RC_SBP_IQD_CURRENT_PRIMARY:
 			rc = -EBUSY;
 			dev_err(&card->gdev->dev,
 	"The device is already a primary Bridge Port\n");
 			break;
-		case 0x2B20:
-		case 0x0020: /* Not authorized by zManager */
+		case IPA_RC_SBP_OSA_NOT_AUTHD_BY_ZMAN:
+		case IPA_RC_SBP_IQD_NOT_AUTHD_BY_ZMAN:
 			rc = -EACCES;
 			dev_err(&card->gdev->dev,
 	"The device is not authorized to be a Bridge Port\n");
diff --git a/drivers/s390/net/qeth_l3.h b/drivers/s390/net/qeth_l3.h
index 26f79533e62e..9b5e439f18cf 100644
--- a/drivers/s390/net/qeth_l3.h
+++ b/drivers/s390/net/qeth_l3.h
@@ -65,6 +65,7 @@ struct qeth_ipato_entry {
 	int mask_bits;
 };
 
+extern const struct attribute_group *qeth_l3_attr_groups[];
 
 void qeth_l3_ipaddr_to_string(enum qeth_prot_versions, const __u8 *, char *);
 int qeth_l3_string_to_ipaddr(const char *, enum qeth_prot_versions, __u8 *);
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index d8df1e635163..3062cde33a3d 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -956,31 +956,6 @@ static int qeth_l3_start_ipa_arp_processing(struct qeth_card *card)
 	return rc;
 }
 
-static int qeth_l3_start_ipa_ip_fragmentation(struct qeth_card *card)
-{
-	int rc;
-
-	QETH_CARD_TEXT(card, 3, "ipaipfrg");
-
-	if (!qeth_is_supported(card, IPA_IP_FRAGMENTATION)) {
-		dev_info(&card->gdev->dev,
-			"Hardware IP fragmentation not supported on %s\n",
-			QETH_CARD_IFNAME(card));
-		return  -EOPNOTSUPP;
-	}
-
-	rc = qeth_send_simple_setassparms(card, IPA_IP_FRAGMENTATION,
-					  IPA_CMD_ASS_START, 0);
-	if (rc) {
-		dev_warn(&card->gdev->dev,
-			"Starting IP fragmentation support for %s failed\n",
-			QETH_CARD_IFNAME(card));
-	} else
-		dev_info(&card->gdev->dev,
-			"Hardware IP fragmentation enabled \n");
-	return rc;
-}
-
 static int qeth_l3_start_ipa_source_mac(struct qeth_card *card)
 {
 	int rc;
@@ -1060,9 +1035,6 @@ static int qeth_l3_softsetup_ipv6(struct qeth_card *card)
 
 	QETH_CARD_TEXT(card, 3, "softipv6");
 
-	if (card->info.type == QETH_CARD_TYPE_IQD)
-		goto out;
-
 	rc = qeth_query_ipassists(card, QETH_PROT_IPV6);
 	if (rc) {
 		dev_err(&card->gdev->dev,
@@ -1070,6 +1042,10 @@ static int qeth_l3_softsetup_ipv6(struct qeth_card *card)
 			QETH_CARD_IFNAME(card));
 		return rc;
 	}
+
+	if (card->info.type == QETH_CARD_TYPE_IQD)
+		goto out;
+
 	rc = qeth_send_simple_setassparms(card, IPA_IPV6,
 					  IPA_CMD_ASS_START, 3);
 	if (rc) {
@@ -1171,7 +1147,6 @@ static int qeth_l3_start_ipassists(struct qeth_card *card)
 	if (qeth_set_access_ctrl_online(card, 0))
 		return -EIO;
 	qeth_l3_start_ipa_arp_processing(card);	/* go on*/
-	qeth_l3_start_ipa_ip_fragmentation(card);	/* go on*/
 	qeth_l3_start_ipa_source_mac(card);	/* go on*/
 	qeth_l3_start_ipa_vlan(card);		/* go on*/
 	qeth_l3_start_ipa_multicast(card);		/* go on*/
@@ -2702,8 +2677,7 @@ static netdev_tx_t qeth_l3_hard_start_xmit(struct sk_buff *skb,
 	use_tso = skb_is_gso(skb) &&
 		  (qeth_get_ip_protocol(skb) == IPPROTO_TCP) && (ipv == 4);
 
-	if ((card->info.type == QETH_CARD_TYPE_IQD) &&
-	    !skb_is_nonlinear(skb)) {
+	if (card->info.type == QETH_CARD_TYPE_IQD) {
 		new_skb = skb;
 		data_offset = ETH_HLEN;
 		hdr = kmem_cache_alloc(qeth_core_header_cache, GFP_ATOMIC);
@@ -2716,12 +2690,7 @@ static netdev_tx_t qeth_l3_hard_start_xmit(struct sk_buff *skb,
 					+ VLAN_HLEN);
 		if (!new_skb)
 			goto tx_drop;
-	}
 
-	if (card->info.type == QETH_CARD_TYPE_IQD) {
-		if (data_offset < 0)
-			skb_pull(new_skb, ETH_HLEN);
-	} else {
 		if (ipv == 4) {
 			skb_pull(new_skb, ETH_HLEN);
 		}
@@ -2760,16 +2729,14 @@ static netdev_tx_t qeth_l3_hard_start_xmit(struct sk_buff *skb,
 	}
 
 	if (use_tso) {
-		hdr = (struct qeth_hdr *)skb_push(new_skb,
-						sizeof(struct qeth_hdr_tso));
+		hdr = skb_push(new_skb, sizeof(struct qeth_hdr_tso));
 		memset(hdr, 0, sizeof(struct qeth_hdr_tso));
 		qeth_l3_fill_header(card, hdr, new_skb, ipv, cast_type);
 		qeth_tso_fill_header(card, hdr, new_skb);
 		hdr_elements++;
 	} else {
 		if (data_offset < 0) {
-			hdr = (struct qeth_hdr *)skb_push(new_skb,
-						sizeof(struct qeth_hdr));
+			hdr = skb_push(new_skb, sizeof(struct qeth_hdr));
 			qeth_l3_fill_header(card, hdr, new_skb, ipv,
 						cast_type);
 		} else {
@@ -3036,14 +3003,21 @@ static int qeth_l3_setup_netdev(struct qeth_card *card)
 	return register_netdev(card->dev);
 }
 
+static const struct device_type qeth_l3_devtype = {
+	.name = "qeth_layer3",
+	.groups = qeth_l3_attr_groups,
+};
+
 static int qeth_l3_probe_device(struct ccwgroup_device *gdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&gdev->dev);
 	int rc;
 
-	rc = qeth_l3_create_device_attributes(&gdev->dev);
-	if (rc)
-		return rc;
+	if (gdev->dev.type == &qeth_generic_devtype) {
+		rc = qeth_l3_create_device_attributes(&gdev->dev);
+		if (rc)
+			return rc;
+	}
 	hash_init(card->ip_htable);
 	hash_init(card->ip_mc_htable);
 	card->options.layer2 = 0;
@@ -3055,7 +3029,8 @@ static void qeth_l3_remove_device(struct ccwgroup_device *cgdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&cgdev->dev);
 
-	qeth_l3_remove_device_attributes(&cgdev->dev);
+	if (cgdev->dev.type == &qeth_generic_devtype)
+		qeth_l3_remove_device_attributes(&cgdev->dev);
 
 	qeth_set_allowed_threads(card, 0, 1);
 	wait_event(card->wait_q, qeth_threads_running(card, 0xffffffff) == 0);
@@ -3311,7 +3286,7 @@ static int qeth_l3_control_event(struct qeth_card *card,
 }
 
 struct qeth_discipline qeth_l3_discipline = {
-	.devtype = &qeth_generic_devtype,
+	.devtype = &qeth_l3_devtype,
 	.start_poll = qeth_qdio_start_poll,
 	.input_handler = (qdio_handler_t *) qeth_qdio_input_handler,
 	.output_handler = (qdio_handler_t *) qeth_qdio_output_handler,
diff --git a/drivers/s390/net/qeth_l3_sys.c b/drivers/s390/net/qeth_l3_sys.c
index ff29a4b416b4..f2f94f59e0fa 100644
--- a/drivers/s390/net/qeth_l3_sys.c
+++ b/drivers/s390/net/qeth_l3_sys.c
@@ -1049,3 +1049,14 @@ void qeth_l3_remove_device_attributes(struct device *dev)
 	sysfs_remove_group(&dev->kobj, &qeth_device_vipa_group);
 	sysfs_remove_group(&dev->kobj, &qeth_device_rxip_group);
 }
+
+const struct attribute_group *qeth_l3_attr_groups[] = {
+	&qeth_device_attr_group,
+	&qeth_device_blkt_group,
+	/* l3 specific, see l3_{create,remove}_device_attributes(): */
+	&qeth_l3_device_attr_group,
+	&qeth_device_ipato_group,
+	&qeth_device_vipa_group,
+	&qeth_device_rxip_group,
+NULL,
+};
diff --git a/drivers/sbus/char/jsflash.c b/drivers/sbus/char/jsflash.c
index 62fed9dc893e..14f377ac1280 100644
--- a/drivers/sbus/char/jsflash.c
+++ b/drivers/sbus/char/jsflash.c
@@ -214,7 +214,7 @@ static void jsfd_request(void)
 		struct jsfd_part *jdp = req->rq_disk->private_data;
 		unsigned long offset = blk_rq_pos(req) << 9;
 		size_t len = blk_rq_cur_bytes(req);
-		int err = -EIO;
+		blk_status_t err = BLK_STS_IOERR;
 
 		if ((offset + len) > jdp->dsize)
 			goto end;
@@ -230,7 +230,7 @@ static void jsfd_request(void)
 		}
 
 		jsfd_read(bio_data(req->bio), jdp->dbase + offset, len);
-		err = 0;
+		err = BLK_STS_OK;
 	end:
 		if (!__blk_end_request_cur(req, err))
 			req = jsfd_next_request();
@@ -592,6 +592,7 @@ static int jsfd_init(void)
 			put_disk(disk);
 			goto out;
 		}
+		blk_queue_bounce_limit(disk->queue, BLK_BOUNCE_HIGH);
 		jsfd_disk[i] = disk;
 	}
 
diff --git a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
index b1c105f4130d..7dfe709a7138 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
@@ -352,7 +352,7 @@ static int bnx2fc_xmit(struct fc_lport *lport, struct fc_frame *fp)
 		frag = &skb_shinfo(skb)->frags[skb_shinfo(skb)->nr_frags - 1];
 		cp = kmap_atomic(skb_frag_page(frag)) + frag->page_offset;
 	} else {
-		cp = (struct fcoe_crc_eof *)skb_put(skb, tlen);
+		cp = skb_put(skb, tlen);
 	}
 
 	memset(cp, 0, sizeof(*cp));
diff --git a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
index 1880eb6c68f7..7b09e7ddf35e 100644
--- a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
+++ b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
@@ -354,7 +354,7 @@ static inline void make_tx_data_wr(struct cxgbi_sock *csk, struct sk_buff *skb,
 	struct l2t_entry *l2t = csk->l2t;
 
 	skb_reset_transport_header(skb);
-	req = (struct tx_data_wr *)__skb_push(skb, sizeof(*req));
+	req = __skb_push(skb, sizeof(*req));
 	req->wr_hi = htonl(V_WR_OP(FW_WROPCODE_OFLD_TX_DATA) |
 			(req_completion ? F_WR_COMPL : 0));
 	req->wr_lo = htonl(V_WR_TID(csk->tid));
diff --git a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
index fa994713e718..a69a9ac836f5 100644
--- a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
+++ b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
@@ -644,7 +644,7 @@ static inline void make_tx_data_wr(struct cxgbi_sock *csk, struct sk_buff *skb,
 	unsigned int wr_ulp_mode = 0, val;
 	bool imm = is_ofld_imm(skb);
 
-	req = (struct fw_ofld_tx_data_wr *)__skb_push(skb, sizeof(*req));
+	req = __skb_push(skb, sizeof(*req));
 
 	if (imm) {
 		req->op_to_immdlen = htonl(FW_WR_OP_V(FW_OFLD_TX_DATA_WR) |
@@ -806,7 +806,7 @@ static void do_act_establish(struct cxgbi_device *cdev, struct sk_buff *skb)
 
 	cxgbi_sock_get(csk);
 	csk->tid = tid;
-	cxgb4_insert_tid(lldi->tids, csk, tid);
+	cxgb4_insert_tid(lldi->tids, csk, tid, csk->csk_family);
 	cxgbi_sock_set_flag(csk, CTPF_HAS_TID);
 
 	free_atid(csk);
@@ -956,7 +956,8 @@ static void do_act_open_rpl(struct cxgbi_device *cdev, struct sk_buff *skb)
 	if (status && status != CPL_ERR_TCAM_FULL &&
 	    status != CPL_ERR_CONN_EXIST &&
 	    status != CPL_ERR_ARP_MISS)
-		cxgb4_remove_tid(lldi->tids, csk->port_id, GET_TID(rpl));
+		cxgb4_remove_tid(lldi->tids, csk->port_id, GET_TID(rpl),
+				 csk->csk_family);
 
 	cxgbi_sock_get(csk);
 	spin_lock_bh(&csk->lock);
@@ -1590,7 +1591,8 @@ static void release_offload_resources(struct cxgbi_sock *csk)
 		free_atid(csk);
 	else if (cxgbi_sock_flag(csk, CTPF_HAS_TID)) {
 		lldi = cxgbi_cdev_priv(csk->cdev);
-		cxgb4_remove_tid(lldi->tids, 0, csk->tid);
+		cxgb4_remove_tid(lldi->tids, 0, csk->tid,
+				 csk->csk_family);
 		cxgbi_sock_clear_flag(csk, CTPF_HAS_TID);
 		cxgbi_sock_put(csk);
 	}
diff --git a/drivers/scsi/dpt/dpti_i2o.h b/drivers/scsi/dpt/dpti_i2o.h
index bd9e31e16249..16fc380b5512 100644
--- a/drivers/scsi/dpt/dpti_i2o.h
+++ b/drivers/scsi/dpt/dpti_i2o.h
@@ -48,7 +48,7 @@
 #include <linux/wait.h>
 typedef wait_queue_head_t adpt_wait_queue_head_t;
 #define ADPT_DECLARE_WAIT_QUEUE_HEAD(wait) DECLARE_WAIT_QUEUE_HEAD_ONSTACK(wait)
-typedef wait_queue_t adpt_wait_queue_t;
+typedef wait_queue_entry_t adpt_wait_queue_entry_t;
 
 /*
  * message structures
diff --git a/drivers/scsi/fcoe/fcoe.c b/drivers/scsi/fcoe/fcoe.c
index ea21e7bb7ef5..85f9a3eba387 100644
--- a/drivers/scsi/fcoe/fcoe.c
+++ b/drivers/scsi/fcoe/fcoe.c
@@ -1543,7 +1543,7 @@ static int fcoe_xmit(struct fc_lport *lport, struct fc_frame *fp)
 		cp = kmap_atomic(skb_frag_page(frag))
 			+ frag->page_offset;
 	} else {
-		cp = (struct fcoe_crc_eof *)skb_put(skb, tlen);
+		cp = skb_put(skb, tlen);
 	}
 
 	memset(cp, 0, sizeof(*cp));
diff --git a/drivers/scsi/fcoe/fcoe_ctlr.c b/drivers/scsi/fcoe/fcoe_ctlr.c
index 656463ff9ccb..fff6f1851dc1 100644
--- a/drivers/scsi/fcoe/fcoe_ctlr.c
+++ b/drivers/scsi/fcoe/fcoe_ctlr.c
@@ -626,7 +626,7 @@ static int fcoe_ctlr_encaps(struct fcoe_ctlr *fip, struct fc_lport *lport,
 	fh = (struct fc_frame_header *)skb->data;
 	op = *(u8 *)(fh + 1);
 	dlen = sizeof(struct fip_encaps) + skb->len;	/* len before push */
-	cap = (struct fip_encaps_head *)skb_push(skb, sizeof(*cap));
+	cap = skb_push(skb, sizeof(*cap));
 	memset(cap, 0, sizeof(*cap));
 
 	if (lport->point_to_multipoint) {
@@ -660,8 +660,7 @@ static int fcoe_ctlr_encaps(struct fcoe_ctlr *fip, struct fc_lport *lport,
 
 	if (op != ELS_LS_RJT) {
 		dlen += sizeof(*mac);
-		mac = (struct fip_mac_desc *)skb_put(skb, sizeof(*mac));
-		memset(mac, 0, sizeof(*mac));
+		mac = skb_put_zero(skb, sizeof(*mac));
 		mac->fd_desc.fip_dtype = FIP_DT_MAC;
 		mac->fd_desc.fip_dlen = sizeof(*mac) / FIP_BPW;
 		if (dtype != FIP_DT_FLOGI && dtype != FIP_DT_FDISC) {
diff --git a/drivers/scsi/fnic/fnic_fcs.c b/drivers/scsi/fnic/fnic_fcs.c
index d212eb574c94..999fc7547560 100644
--- a/drivers/scsi/fnic/fnic_fcs.c
+++ b/drivers/scsi/fnic/fnic_fcs.c
@@ -664,7 +664,7 @@ static inline int fnic_import_rq_eth_pkt(struct fnic *fnic, struct sk_buff *skb)
 	eh = (struct ethhdr *)skb->data;
 	if (eh->h_proto == htons(ETH_P_8021Q)) {
 		memmove((u8 *)eh + VLAN_HLEN, eh, ETH_ALEN * 2);
-		eh = (struct ethhdr *)skb_pull(skb, VLAN_HLEN);
+		eh = skb_pull(skb, VLAN_HLEN);
 		skb_reset_mac_header(skb);
 	}
 	if (eh->h_proto == htons(ETH_P_FIP)) {
@@ -1024,8 +1024,7 @@ void fnic_eth_send(struct fcoe_ctlr *fip, struct sk_buff *skb)
 
 	if (!fnic->vlan_hw_insert) {
 		eth_hdr = (struct ethhdr *)skb_mac_header(skb);
-		vlan_hdr = (struct vlan_ethhdr *)skb_push(skb,
-				sizeof(*vlan_hdr) - sizeof(*eth_hdr));
+		vlan_hdr = skb_push(skb, sizeof(*vlan_hdr) - sizeof(*eth_hdr));
 		memcpy(vlan_hdr, eth_hdr, 2 * ETH_ALEN);
 		vlan_hdr->h_vlan_proto = htons(ETH_P_8021Q);
 		vlan_hdr->h_vlan_encapsulated_proto = eth_hdr->h_proto;
@@ -1091,7 +1090,7 @@ static int fnic_send_frame(struct fnic *fnic, struct fc_frame *fp)
 
 	if (!fnic->vlan_hw_insert) {
 		eth_hdr_len = sizeof(*vlan_hdr) + sizeof(*fcoe_hdr);
-		vlan_hdr = (struct vlan_ethhdr *)skb_push(skb, eth_hdr_len);
+		vlan_hdr = skb_push(skb, eth_hdr_len);
 		eth_hdr = (struct ethhdr *)vlan_hdr;
 		vlan_hdr->h_vlan_proto = htons(ETH_P_8021Q);
 		vlan_hdr->h_vlan_encapsulated_proto = htons(ETH_P_FCOE);
@@ -1099,7 +1098,7 @@ static int fnic_send_frame(struct fnic *fnic, struct fc_frame *fp)
 		fcoe_hdr = (struct fcoe_hdr *)(vlan_hdr + 1);
 	} else {
 		eth_hdr_len = sizeof(*eth_hdr) + sizeof(*fcoe_hdr);
-		eth_hdr = (struct ethhdr *)skb_push(skb, eth_hdr_len);
+		eth_hdr = skb_push(skb, eth_hdr_len);
 		eth_hdr->h_proto = htons(ETH_P_FCOE);
 		fcoe_hdr = (struct fcoe_hdr *)(eth_hdr + 1);
 	}
diff --git a/drivers/scsi/ibmvscsi_tgt/ibmvscsi_tgt.c b/drivers/scsi/ibmvscsi_tgt/ibmvscsi_tgt.c
index abf6026645dd..659ab483d716 100644
--- a/drivers/scsi/ibmvscsi_tgt/ibmvscsi_tgt.c
+++ b/drivers/scsi/ibmvscsi_tgt/ibmvscsi_tgt.c
@@ -3934,10 +3934,6 @@ static const struct target_core_fabric_ops ibmvscsis_ops = {
 
 static void ibmvscsis_dev_release(struct device *dev) {};
 
-static struct class_attribute ibmvscsis_class_attrs[] = {
-	__ATTR_NULL,
-};
-
 static struct device_attribute dev_attr_system_id =
 	__ATTR(system_id, S_IRUGO, system_id_show, NULL);
 
@@ -3957,7 +3953,6 @@ ATTRIBUTE_GROUPS(ibmvscsis_dev);
 static struct class ibmvscsis_class = {
 	.name		= "ibmvscsis",
 	.dev_release	= ibmvscsis_dev_release,
-	.class_attrs	= ibmvscsis_class_attrs,
 	.dev_groups	= ibmvscsis_dev_groups,
 };
 
diff --git a/drivers/scsi/ips.c b/drivers/scsi/ips.c
index 3419e1bcdff6..67621308eb9c 100644
--- a/drivers/scsi/ips.c
+++ b/drivers/scsi/ips.c
@@ -301,13 +301,13 @@ static uint32_t ips_statupd_copperhead_memio(ips_ha_t *);
 static uint32_t ips_statupd_morpheus(ips_ha_t *);
 static ips_scb_t *ips_getscb(ips_ha_t *);
 static void ips_putq_scb_head(ips_scb_queue_t *, ips_scb_t *);
-static void ips_putq_wait_tail(ips_wait_queue_t *, struct scsi_cmnd *);
+static void ips_putq_wait_tail(ips_wait_queue_entry_t *, struct scsi_cmnd *);
 static void ips_putq_copp_tail(ips_copp_queue_t *,
 				      ips_copp_wait_item_t *);
 static ips_scb_t *ips_removeq_scb_head(ips_scb_queue_t *);
 static ips_scb_t *ips_removeq_scb(ips_scb_queue_t *, ips_scb_t *);
-static struct scsi_cmnd *ips_removeq_wait_head(ips_wait_queue_t *);
-static struct scsi_cmnd *ips_removeq_wait(ips_wait_queue_t *,
+static struct scsi_cmnd *ips_removeq_wait_head(ips_wait_queue_entry_t *);
+static struct scsi_cmnd *ips_removeq_wait(ips_wait_queue_entry_t *,
 					  struct scsi_cmnd *);
 static ips_copp_wait_item_t *ips_removeq_copp(ips_copp_queue_t *,
 						     ips_copp_wait_item_t *);
@@ -2871,7 +2871,7 @@ ips_removeq_scb(ips_scb_queue_t * queue, ips_scb_t * item)
 /* ASSUMED to be called from within the HA lock                             */
 /*                                                                          */
 /****************************************************************************/
-static void ips_putq_wait_tail(ips_wait_queue_t *queue, struct scsi_cmnd *item)
+static void ips_putq_wait_tail(ips_wait_queue_entry_t *queue, struct scsi_cmnd *item)
 {
 	METHOD_TRACE("ips_putq_wait_tail", 1);
 
@@ -2902,7 +2902,7 @@ static void ips_putq_wait_tail(ips_wait_queue_t *queue, struct scsi_cmnd *item)
 /* ASSUMED to be called from within the HA lock                             */
 /*                                                                          */
 /****************************************************************************/
-static struct scsi_cmnd *ips_removeq_wait_head(ips_wait_queue_t *queue)
+static struct scsi_cmnd *ips_removeq_wait_head(ips_wait_queue_entry_t *queue)
 {
 	struct scsi_cmnd *item;
 
@@ -2936,7 +2936,7 @@ static struct scsi_cmnd *ips_removeq_wait_head(ips_wait_queue_t *queue)
 /* ASSUMED to be called from within the HA lock                             */
 /*                                                                          */
 /****************************************************************************/
-static struct scsi_cmnd *ips_removeq_wait(ips_wait_queue_t *queue,
+static struct scsi_cmnd *ips_removeq_wait(ips_wait_queue_entry_t *queue,
 					  struct scsi_cmnd *item)
 {
 	struct scsi_cmnd *p;
diff --git a/drivers/scsi/ips.h b/drivers/scsi/ips.h
index b782bb60baf0..366be3b2f9b4 100644
--- a/drivers/scsi/ips.h
+++ b/drivers/scsi/ips.h
@@ -989,7 +989,7 @@ typedef struct ips_wait_queue {
 	struct scsi_cmnd *head;
 	struct scsi_cmnd *tail;
 	int count;
-} ips_wait_queue_t;
+} ips_wait_queue_entry_t;
 
 typedef struct ips_copp_wait_item {
 	struct scsi_cmnd *scsi_cmd;
@@ -1035,7 +1035,7 @@ typedef struct ips_ha {
    ips_stat_t         sp;                 /* Status packer pointer      */
    struct ips_scb    *scbs;               /* Array of all CCBS          */
    struct ips_scb    *scb_freelist;       /* SCB free list              */
-   ips_wait_queue_t   scb_waitlist;       /* Pending SCB list           */
+   ips_wait_queue_entry_t   scb_waitlist;       /* Pending SCB list           */
    ips_copp_queue_t   copp_waitlist;      /* Pending PT list            */
    ips_scb_queue_t    scb_activelist;     /* Active SCB list            */
    IPS_IO_CMD        *dummy;              /* dummy command              */
diff --git a/drivers/scsi/libfc/fc_libfc.c b/drivers/scsi/libfc/fc_libfc.c
index d623d084b7ec..dbadbc81b24b 100644
--- a/drivers/scsi/libfc/fc_libfc.c
+++ b/drivers/scsi/libfc/fc_libfc.c
@@ -178,7 +178,7 @@ void fc_fill_hdr(struct fc_frame *fp, const struct fc_frame *in_fp,
 		fill = -fr_len(fp) & 3;
 		if (fill) {
 			/* TODO, this may be a problem with fragmented skb */
-			memset(skb_put(fp_skb(fp), fill), 0, fill);
+			skb_put_zero(fp_skb(fp), fill);
 			f_ctl |= fill;
 		}
 		fr_eof(fp) = FC_EOF_T;
diff --git a/drivers/scsi/lpfc/lpfc_debugfs.c b/drivers/scsi/lpfc/lpfc_debugfs.c
index ed2850645e70..5cc8b0f7d885 100644
--- a/drivers/scsi/lpfc/lpfc_debugfs.c
+++ b/drivers/scsi/lpfc/lpfc_debugfs.c
@@ -1968,10 +1968,6 @@ lpfc_debugfs_nvmestat_write(struct file *file, const char __user *buf,
 	if (nbytes > 64)
 		nbytes = 64;
 
-	/* Protect copy from user */
-	if (!access_ok(VERIFY_READ, buf, nbytes))
-		return -EFAULT;
-
 	memset(mybuf, 0, sizeof(mybuf));
 
 	if (copy_from_user(mybuf, buf, nbytes))
@@ -2056,10 +2052,6 @@ lpfc_debugfs_nvmektime_write(struct file *file, const char __user *buf,
 	if (nbytes > 64)
 		nbytes = 64;
 
-	/* Protect copy from user */
-	if (!access_ok(VERIFY_READ, buf, nbytes))
-		return -EFAULT;
-
 	memset(mybuf, 0, sizeof(mybuf));
 
 	if (copy_from_user(mybuf, buf, nbytes))
@@ -2188,10 +2180,6 @@ lpfc_debugfs_nvmeio_trc_write(struct file *file, const char __user *buf,
 	if (nbytes > 64)
 		nbytes = 64;
 
-	/* Protect copy from user */
-	if (!access_ok(VERIFY_READ, buf, nbytes))
-		return -EFAULT;
-
 	memset(mybuf, 0, sizeof(mybuf));
 
 	if (copy_from_user(mybuf, buf, nbytes))
@@ -2299,10 +2287,6 @@ lpfc_debugfs_cpucheck_write(struct file *file, const char __user *buf,
 	if (nbytes > 64)
 		nbytes = 64;
 
-	/* Protect copy from user */
-	if (!access_ok(VERIFY_READ, buf, nbytes))
-		return -EFAULT;
-
 	memset(mybuf, 0, sizeof(mybuf));
 
 	if (copy_from_user(mybuf, buf, nbytes))
@@ -2373,10 +2357,6 @@ static int lpfc_idiag_cmd_get(const char __user *buf, size_t nbytes,
 	int i;
 	size_t bsize;
 
-	/* Protect copy from user */
-	if (!access_ok(VERIFY_READ, buf, nbytes))
-		return -EFAULT;
-
 	memset(mybuf, 0, sizeof(mybuf));
 	memset(idiag_cmd, 0, sizeof(*idiag_cmd));
 	bsize = min(nbytes, (sizeof(mybuf)-1));
diff --git a/drivers/scsi/osd/osd_initiator.c b/drivers/scsi/osd/osd_initiator.c
index 8a1b94816419..a4f28b7e4c65 100644
--- a/drivers/scsi/osd/osd_initiator.c
+++ b/drivers/scsi/osd/osd_initiator.c
@@ -446,7 +446,7 @@ static void _put_request(struct request *rq)
 	 *       code paths.
 	 */
 	if (unlikely(rq->bio))
-		blk_end_request(rq, -ENOMEM, blk_rq_bytes(rq));
+		blk_end_request(rq, BLK_STS_IOERR, blk_rq_bytes(rq));
 	else
 		blk_put_request(rq);
 }
@@ -474,10 +474,10 @@ void osd_end_request(struct osd_request *or)
 EXPORT_SYMBOL(osd_end_request);
 
 static void _set_error_resid(struct osd_request *or, struct request *req,
-			     int error)
+			     blk_status_t error)
 {
 	or->async_error = error;
-	or->req_errors = scsi_req(req)->result ? : error;
+	or->req_errors = scsi_req(req)->result;
 	or->sense_len = scsi_req(req)->sense_len;
 	if (or->sense_len)
 		memcpy(or->sense, scsi_req(req)->sense, or->sense_len);
@@ -489,17 +489,19 @@ static void _set_error_resid(struct osd_request *or, struct request *req,
 
 int osd_execute_request(struct osd_request *or)
 {
-	int error;
-
 	blk_execute_rq(or->request->q, NULL, or->request, 0);
-	error = scsi_req(or->request)->result ? -EIO : 0;
 
-	_set_error_resid(or, or->request, error);
-	return error;
+	if (scsi_req(or->request)->result) {
+		_set_error_resid(or, or->request, BLK_STS_IOERR);
+		return -EIO;
+	}
+
+	_set_error_resid(or, or->request, BLK_STS_OK);
+	return 0;
 }
 EXPORT_SYMBOL(osd_execute_request);
 
-static void osd_request_async_done(struct request *req, int error)
+static void osd_request_async_done(struct request *req, blk_status_t error)
 {
 	struct osd_request *or = req->end_io_data;
 
@@ -1572,13 +1574,9 @@ static struct request *_make_request(struct request_queue *q, bool has_write,
 			flags);
 	if (IS_ERR(req))
 		return req;
-	scsi_req_init(req);
 
 	for_each_bio(bio) {
-		struct bio *bounce_bio = bio;
-
-		blk_queue_bounce(req->q, &bounce_bio);
-		ret = blk_rq_append_bio(req, bounce_bio);
+		ret = blk_rq_append_bio(req, bio);
 		if (ret)
 			return ERR_PTR(ret);
 	}
@@ -1617,7 +1615,6 @@ static int _init_blk_request(struct osd_request *or,
 				ret = PTR_ERR(req);
 				goto out;
 			}
-			scsi_req_init(req);
 			or->in.req = or->request->next_rq = req;
 		}
 	} else if (has_in)
@@ -1914,7 +1911,7 @@ analyze:
 		/* scsi sense is Empty, the request was never issued to target
 		 * linux return code might tell us what happened.
 		 */
-		if (or->async_error == -ENOMEM)
+		if (or->async_error == BLK_STS_RESOURCE)
 			osi->osd_err_pri = OSD_ERR_PRI_RESOURCE;
 		else
 			osi->osd_err_pri = OSD_ERR_PRI_UNREACHABLE;
diff --git a/drivers/scsi/osst.c b/drivers/scsi/osst.c
index 67cbed92f07d..929ee7e88120 100644
--- a/drivers/scsi/osst.c
+++ b/drivers/scsi/osst.c
@@ -320,7 +320,7 @@ static int osst_chk_result(struct osst_tape * STp, struct osst_request * SRpnt)
 
 
 /* Wakeup from interrupt */
-static void osst_end_async(struct request *req, int update)
+static void osst_end_async(struct request *req, blk_status_t status)
 {
 	struct scsi_request *rq = scsi_req(req);
 	struct osst_request *SRpnt = req->end_io_data;
@@ -373,7 +373,6 @@ static int osst_execute(struct osst_request *SRpnt, const unsigned char *cmd,
 		return DRIVER_ERROR << 24;
 
 	rq = scsi_req(req);
-	scsi_req_init(req);
 	req->rq_flags |= RQF_QUIET;
 
 	SRpnt->bio = NULL;
diff --git a/drivers/scsi/qedf/qedf_fip.c b/drivers/scsi/qedf/qedf_fip.c
index 64b04f245e15..aefd24ca9604 100644
--- a/drivers/scsi/qedf/qedf_fip.c
+++ b/drivers/scsi/qedf/qedf_fip.c
@@ -125,8 +125,7 @@ void qedf_fip_send(struct fcoe_ctlr *fip, struct sk_buff *skb)
 	sub = fiph->fip_subcode;
 
 	if (!qedf->vlan_hw_insert) {
-		vlan_hdr = (struct vlan_ethhdr *)skb_push(skb, sizeof(*vlan_hdr)
-		    - sizeof(*eth_hdr));
+		vlan_hdr = skb_push(skb, sizeof(*vlan_hdr) - sizeof(*eth_hdr));
 		memcpy(vlan_hdr, eth_hdr, 2 * ETH_ALEN);
 		vlan_hdr->h_vlan_proto = htons(ETH_P_8021Q);
 		vlan_hdr->h_vlan_encapsulated_proto = eth_hdr->h_proto;
diff --git a/drivers/scsi/qedf/qedf_main.c b/drivers/scsi/qedf/qedf_main.c
index 6149ea004fa1..b58bba4604e8 100644
--- a/drivers/scsi/qedf/qedf_main.c
+++ b/drivers/scsi/qedf/qedf_main.c
@@ -896,7 +896,7 @@ static int qedf_xmit(struct fc_lport *lport, struct fc_frame *fp)
 		frag = &skb_shinfo(skb)->frags[skb_shinfo(skb)->nr_frags - 1];
 		cp = kmap_atomic(skb_frag_page(frag)) + frag->page_offset;
 	} else {
-		cp = (struct fcoe_crc_eof *)skb_put(skb, tlen);
+		cp = skb_put(skb, tlen);
 	}
 
 	memset(cp, 0, sizeof(*cp));
@@ -2236,7 +2236,7 @@ static void qedf_ll2_process_skb(struct work_struct *work)
 	/* Undo VLAN encapsulation */
 	if (eh->h_proto == htons(ETH_P_8021Q)) {
 		memmove((u8 *)eh + VLAN_HLEN, eh, ETH_ALEN * 2);
-		eh = (struct ethhdr *)skb_pull(skb, VLAN_HLEN);
+		eh = skb_pull(skb, VLAN_HLEN);
 		skb_reset_mac_header(skb);
 	}
 
@@ -3065,7 +3065,7 @@ static int __qedf_probe(struct pci_dev *pdev, int mode)
 		   "WWPN=%016llx.\n", qedf->wwnn, qedf->wwpn);
 
 	sprintf(host_buf, "host_%d", host->host_no);
-	qed_ops->common->set_id(qedf->cdev, host_buf, QEDF_VERSION);
+	qed_ops->common->set_name(qedf->cdev, host_buf);
 
 
 	/* Set xid max values */
diff --git a/drivers/scsi/qedi/qedi_fw.c b/drivers/scsi/qedi/qedi_fw.c
index d0dd92f6a365..19254bd739d9 100644
--- a/drivers/scsi/qedi/qedi_fw.c
+++ b/drivers/scsi/qedi/qedi_fw.c
@@ -870,7 +870,6 @@ static void qedi_process_cmd_cleanup_resp(struct qedi_ctx *qedi,
 		QEDI_ERR(&qedi->dbg_ctx,
 			 "Delayed or untracked cleanup response, itt=0x%x, tid=0x%x, cid=0x%x, task=%p\n",
 			 protoitt, cqe->itid, qedi_conn->iscsi_conn_id, task);
-		WARN_ON(1);
 	}
 }
 
@@ -2101,14 +2100,16 @@ int qedi_iscsi_send_ioreq(struct iscsi_task *task)
 	/* Update header info */
 	SET_FIELD(cmd_pdu_header.flags_attr, ISCSI_CMD_HDR_ATTR,
 		  ISCSI_ATTR_SIMPLE);
-	if (sc->sc_data_direction == DMA_TO_DEVICE) {
-		SET_FIELD(cmd_pdu_header.flags_attr,
-			  ISCSI_CMD_HDR_WRITE, 1);
-		task_type = ISCSI_TASK_TYPE_INITIATOR_WRITE;
-	} else {
-		SET_FIELD(cmd_pdu_header.flags_attr,
-			  ISCSI_CMD_HDR_READ, 1);
-		task_type = ISCSI_TASK_TYPE_INITIATOR_READ;
+	if (hdr->cdb[0] != TEST_UNIT_READY) {
+		if (sc->sc_data_direction == DMA_TO_DEVICE) {
+			SET_FIELD(cmd_pdu_header.flags_attr,
+				  ISCSI_CMD_HDR_WRITE, 1);
+			task_type = ISCSI_TASK_TYPE_INITIATOR_WRITE;
+		} else {
+			SET_FIELD(cmd_pdu_header.flags_attr,
+				  ISCSI_CMD_HDR_READ, 1);
+			task_type = ISCSI_TASK_TYPE_INITIATOR_READ;
+		}
 	}
 
 	cmd_pdu_header.lun.lo = be32_to_cpu(scsi_lun[0]);
@@ -2119,7 +2120,7 @@ int qedi_iscsi_send_ioreq(struct iscsi_task *task)
 	cmd_pdu_header.expected_transfer_length = cpu_to_be32(hdr->data_length);
 	cmd_pdu_header.hdr_second_dword = ntoh24(hdr->dlength);
 	cmd_pdu_header.cmd_sn = be32_to_cpu(hdr->cmdsn);
-	cmd_pdu_header.opcode = hdr->opcode;
+	cmd_pdu_header.hdr_first_byte = hdr->opcode;
 	qedi_cpy_scsi_cdb(sc, (u32 *)cmd_pdu_header.cdb);
 
 	/* Fill tx AHS and rx buffer */
diff --git a/drivers/scsi/qedi/qedi_fw_api.c b/drivers/scsi/qedi/qedi_fw_api.c
index fd354d4e03eb..7df32a68bd54 100644
--- a/drivers/scsi/qedi/qedi_fw_api.c
+++ b/drivers/scsi/qedi/qedi_fw_api.c
@@ -578,7 +578,8 @@ int init_initiator_rw_iscsi_task(struct iscsi_task_params *task_params,
 					  (struct iscsi_common_hdr *)cmd_header,
 					  tx_sgl_params, cmd_params,
 					  dif_task_params);
-	else if (GET_FIELD(cmd_header->flags_attr, ISCSI_CMD_HDR_READ))
+	else if (GET_FIELD(cmd_header->flags_attr, ISCSI_CMD_HDR_READ) ||
+		 (task_params->rx_io_size == 0 && task_params->tx_io_size == 0))
 		return init_rw_iscsi_task(task_params,
 					  ISCSI_TASK_TYPE_INITIATOR_READ,
 					  conn_params,
diff --git a/drivers/scsi/qedi/qedi_iscsi.c b/drivers/scsi/qedi/qedi_iscsi.c
index 87f0af358b33..80edd28b635f 100644
--- a/drivers/scsi/qedi/qedi_iscsi.c
+++ b/drivers/scsi/qedi/qedi_iscsi.c
@@ -1466,9 +1466,6 @@ static const struct {
 	{ ISCSI_CONN_ERROR_OUT_OF_SGES_ERROR,
 	  "out of sge error"
 	},
-	{ ISCSI_CONN_ERROR_TCP_SEG_PROC_IP_OPTIONS_ERROR,
-	  "tcp seg ip options error"
-	},
 	{ ISCSI_CONN_ERROR_TCP_IP_FRAGMENT_ERROR,
 	  "tcp ip fragment error"
 	},
diff --git a/drivers/scsi/qedi/qedi_main.c b/drivers/scsi/qedi/qedi_main.c
index 09a294634bc7..5f5a4ef2e529 100644
--- a/drivers/scsi/qedi/qedi_main.c
+++ b/drivers/scsi/qedi/qedi_main.c
@@ -1499,11 +1499,9 @@ err_idx:
 
 void qedi_clear_task_idx(struct qedi_ctx *qedi, int idx)
 {
-	if (!test_and_clear_bit(idx, qedi->task_idx_map)) {
+	if (!test_and_clear_bit(idx, qedi->task_idx_map))
 		QEDI_ERR(&qedi->dbg_ctx,
 			 "FW task context, already cleared, tid=0x%x\n", idx);
-		WARN_ON(1);
-	}
 }
 
 void qedi_update_itt_map(struct qedi_ctx *qedi, u32 tid, u32 proto_itt,
@@ -1844,7 +1842,7 @@ static int __qedi_probe(struct pci_dev *pdev, int mode)
 		  qedi->mac);
 
 	sprintf(host_buf, "host_%d", qedi->shost->host_no);
-	qedi_ops->common->set_id(qedi->cdev, host_buf, QEDI_MODULE_VERSION);
+	qedi_ops->common->set_name(qedi->cdev, host_buf);
 
 	qedi_ops->register_ops(qedi->cdev, &qedi_cb_ops, qedi);
 
diff --git a/drivers/scsi/qla1280.c b/drivers/scsi/qla1280.c
index 634254a52301..8a29fb09db14 100644
--- a/drivers/scsi/qla1280.c
+++ b/drivers/scsi/qla1280.c
@@ -3390,7 +3390,7 @@ qla1280_isp_cmd(struct scsi_qla_host *ha)
 	 *    On PCI bus, order reverses and write of 6 posts, then index 5,
 	 *       causing chip to issue full queue of stale commands
 	 * The mmiowb() prevents future writes from crossing the barrier.
-	 * See Documentation/DocBook/deviceiobook.tmpl for more information.
+	 * See Documentation/driver-api/device-io.rst for more information.
 	 */
 	WRT_REG_WORD(&reg->mailbox4, ha->req_ring_index);
 	mmiowb();
diff --git a/drivers/scsi/scsi_debug.c b/drivers/scsi/scsi_debug.c
index dc095a292c61..3be980d47268 100644
--- a/drivers/scsi/scsi_debug.c
+++ b/drivers/scsi/scsi_debug.c
@@ -245,7 +245,7 @@ struct sdebug_dev_info {
 	unsigned int channel;
 	unsigned int target;
 	u64 lun;
-	uuid_be lu_name;
+	uuid_t lu_name;
 	struct sdebug_host_info *sdbg_host;
 	unsigned long uas_bm[1];
 	atomic_t num_in_q;
@@ -965,7 +965,7 @@ static const u64 naa3_comp_c = 0x3111111000000000ULL;
 static int inquiry_vpd_83(unsigned char *arr, int port_group_id,
 			  int target_dev_id, int dev_id_num,
 			  const char *dev_id_str, int dev_id_str_len,
-			  const uuid_be *lu_name)
+			  const uuid_t *lu_name)
 {
 	int num, port_a;
 	char b[32];
@@ -3568,7 +3568,7 @@ static void sdebug_q_cmd_wq_complete(struct work_struct *work)
 }
 
 static bool got_shared_uuid;
-static uuid_be shared_uuid;
+static uuid_t shared_uuid;
 
 static struct sdebug_dev_info *sdebug_device_create(
 			struct sdebug_host_info *sdbg_host, gfp_t flags)
@@ -3578,12 +3578,12 @@ static struct sdebug_dev_info *sdebug_device_create(
 	devip = kzalloc(sizeof(*devip), flags);
 	if (devip) {
 		if (sdebug_uuid_ctl == 1)
-			uuid_be_gen(&devip->lu_name);
+			uuid_gen(&devip->lu_name);
 		else if (sdebug_uuid_ctl == 2) {
 			if (got_shared_uuid)
 				devip->lu_name = shared_uuid;
 			else {
-				uuid_be_gen(&shared_uuid);
+				uuid_gen(&shared_uuid);
 				got_shared_uuid = true;
 				devip->lu_name = shared_uuid;
 			}
diff --git a/drivers/scsi/scsi_error.c b/drivers/scsi/scsi_error.c
index ac3196420435..ea9f40e51f68 100644
--- a/drivers/scsi/scsi_error.c
+++ b/drivers/scsi/scsi_error.c
@@ -1880,7 +1880,7 @@ int scsi_decide_disposition(struct scsi_cmnd *scmd)
 	}
 }
 
-static void eh_lock_door_done(struct request *req, int uptodate)
+static void eh_lock_door_done(struct request *req, blk_status_t status)
 {
 	__blk_put_request(req->q, req);
 }
@@ -1909,7 +1909,6 @@ static void scsi_eh_lock_door(struct scsi_device *sdev)
 	if (IS_ERR(req))
 		return;
 	rq = scsi_req(req);
-	scsi_req_init(req);
 
 	rq->cmd[0] = ALLOW_MEDIUM_REMOVAL;
 	rq->cmd[1] = 0;
diff --git a/drivers/scsi/scsi_lib.c b/drivers/scsi/scsi_lib.c
index 41c19c75dab4..f6097b89d5d3 100644
--- a/drivers/scsi/scsi_lib.c
+++ b/drivers/scsi/scsi_lib.c
@@ -250,7 +250,6 @@ int scsi_execute(struct scsi_device *sdev, const unsigned char *cmd,
 	if (IS_ERR(req))
 		return ret;
 	rq = scsi_req(req);
-	scsi_req_init(req);
 
 	if (bufflen &&	blk_rq_map_kern(sdev->request_queue, req,
 					buffer, bufflen, __GFP_RECLAIM))
@@ -625,7 +624,7 @@ static void scsi_release_bidi_buffers(struct scsi_cmnd *cmd)
 	cmd->request->next_rq->special = NULL;
 }
 
-static bool scsi_end_request(struct request *req, int error,
+static bool scsi_end_request(struct request *req, blk_status_t error,
 		unsigned int bytes, unsigned int bidi_bytes)
 {
 	struct scsi_cmnd *cmd = req->special;
@@ -684,45 +683,28 @@ static bool scsi_end_request(struct request *req, int error,
  * @cmd:	SCSI command (unused)
  * @result:	scsi error code
  *
- * Translate SCSI error code into standard UNIX errno.
- * Return values:
- * -ENOLINK	temporary transport failure
- * -EREMOTEIO	permanent target failure, do not retry
- * -EBADE	permanent nexus failure, retry on other path
- * -ENOSPC	No write space available
- * -ENODATA	Medium error
- * -EIO		unspecified I/O error
+ * Translate SCSI error code into block errors.
  */
-static int __scsi_error_from_host_byte(struct scsi_cmnd *cmd, int result)
+static blk_status_t __scsi_error_from_host_byte(struct scsi_cmnd *cmd,
+		int result)
 {
-	int error = 0;
-
-	switch(host_byte(result)) {
+	switch (host_byte(result)) {
 	case DID_TRANSPORT_FAILFAST:
-		error = -ENOLINK;
-		break;
+		return BLK_STS_TRANSPORT;
 	case DID_TARGET_FAILURE:
 		set_host_byte(cmd, DID_OK);
-		error = -EREMOTEIO;
-		break;
+		return BLK_STS_TARGET;
 	case DID_NEXUS_FAILURE:
-		set_host_byte(cmd, DID_OK);
-		error = -EBADE;
-		break;
+		return BLK_STS_NEXUS;
 	case DID_ALLOC_FAILURE:
 		set_host_byte(cmd, DID_OK);
-		error = -ENOSPC;
-		break;
+		return BLK_STS_NOSPC;
 	case DID_MEDIUM_ERROR:
 		set_host_byte(cmd, DID_OK);
-		error = -ENODATA;
-		break;
+		return BLK_STS_MEDIUM;
 	default:
-		error = -EIO;
-		break;
+		return BLK_STS_IOERR;
 	}
-
-	return error;
 }
 
 /*
@@ -759,7 +741,7 @@ void scsi_io_completion(struct scsi_cmnd *cmd, unsigned int good_bytes)
 	int result = cmd->result;
 	struct request_queue *q = cmd->device->request_queue;
 	struct request *req = cmd->request;
-	int error = 0;
+	blk_status_t error = BLK_STS_OK;
 	struct scsi_sense_hdr sshdr;
 	bool sense_valid = false;
 	int sense_deferred = 0, level = 0;
@@ -798,7 +780,7 @@ void scsi_io_completion(struct scsi_cmnd *cmd, unsigned int good_bytes)
 			 * both sides at once.
 			 */
 			scsi_req(req->next_rq)->resid_len = scsi_in(cmd)->resid;
-			if (scsi_end_request(req, 0, blk_rq_bytes(req),
+			if (scsi_end_request(req, BLK_STS_OK, blk_rq_bytes(req),
 					blk_rq_bytes(req->next_rq)))
 				BUG();
 			return;
@@ -840,7 +822,7 @@ void scsi_io_completion(struct scsi_cmnd *cmd, unsigned int good_bytes)
 			scsi_print_sense(cmd);
 		result = 0;
 		/* for passthrough error may be set */
-		error = 0;
+		error = BLK_STS_OK;
 	}
 
 	/*
@@ -912,18 +894,18 @@ void scsi_io_completion(struct scsi_cmnd *cmd, unsigned int good_bytes)
 				action = ACTION_REPREP;
 			} else if (sshdr.asc == 0x10) /* DIX */ {
 				action = ACTION_FAIL;
-				error = -EILSEQ;
+				error = BLK_STS_PROTECTION;
 			/* INVALID COMMAND OPCODE or INVALID FIELD IN CDB */
 			} else if (sshdr.asc == 0x20 || sshdr.asc == 0x24) {
 				action = ACTION_FAIL;
-				error = -EREMOTEIO;
+				error = BLK_STS_TARGET;
 			} else
 				action = ACTION_FAIL;
 			break;
 		case ABORTED_COMMAND:
 			action = ACTION_FAIL;
 			if (sshdr.asc == 0x10) /* DIF */
-				error = -EILSEQ;
+				error = BLK_STS_PROTECTION;
 			break;
 		case NOT_READY:
 			/* If the device is in the process of becoming
@@ -1124,6 +1106,19 @@ err_exit:
 }
 EXPORT_SYMBOL(scsi_init_io);
 
+/**
+ * scsi_initialize_rq - initialize struct scsi_cmnd.req
+ *
+ * Called from inside blk_get_request().
+ */
+void scsi_initialize_rq(struct request *rq)
+{
+	struct scsi_cmnd *cmd = blk_mq_rq_to_pdu(rq);
+
+	scsi_req_init(&cmd->req);
+}
+EXPORT_SYMBOL(scsi_initialize_rq);
+
 /* Add a command to the list used by the aacraid and dpt_i2o drivers */
 void scsi_add_cmd_to_list(struct scsi_cmnd *cmd)
 {
@@ -1153,6 +1148,7 @@ void scsi_del_cmd_from_list(struct scsi_cmnd *cmd)
 	}
 }
 
+/* Called after a request has been started. */
 void scsi_init_command(struct scsi_device *dev, struct scsi_cmnd *cmd)
 {
 	void *buf = cmd->sense_buffer;
@@ -1847,15 +1843,15 @@ out_delay:
 		blk_delay_queue(q, SCSI_QUEUE_DELAY);
 }
 
-static inline int prep_to_mq(int ret)
+static inline blk_status_t prep_to_mq(int ret)
 {
 	switch (ret) {
 	case BLKPREP_OK:
-		return BLK_MQ_RQ_QUEUE_OK;
+		return BLK_STS_OK;
 	case BLKPREP_DEFER:
-		return BLK_MQ_RQ_QUEUE_BUSY;
+		return BLK_STS_RESOURCE;
 	default:
-		return BLK_MQ_RQ_QUEUE_ERROR;
+		return BLK_STS_IOERR;
 	}
 }
 
@@ -1914,7 +1910,7 @@ static void scsi_mq_done(struct scsi_cmnd *cmd)
 	blk_mq_complete_request(cmd->request);
 }
 
-static int scsi_queue_rq(struct blk_mq_hw_ctx *hctx,
+static blk_status_t scsi_queue_rq(struct blk_mq_hw_ctx *hctx,
 			 const struct blk_mq_queue_data *bd)
 {
 	struct request *req = bd->rq;
@@ -1922,14 +1918,14 @@ static int scsi_queue_rq(struct blk_mq_hw_ctx *hctx,
 	struct scsi_device *sdev = q->queuedata;
 	struct Scsi_Host *shost = sdev->host;
 	struct scsi_cmnd *cmd = blk_mq_rq_to_pdu(req);
-	int ret;
+	blk_status_t ret;
 	int reason;
 
 	ret = prep_to_mq(scsi_prep_state_check(sdev, req));
-	if (ret != BLK_MQ_RQ_QUEUE_OK)
+	if (ret != BLK_STS_OK)
 		goto out;
 
-	ret = BLK_MQ_RQ_QUEUE_BUSY;
+	ret = BLK_STS_RESOURCE;
 	if (!get_device(&sdev->sdev_gendev))
 		goto out;
 
@@ -1942,7 +1938,7 @@ static int scsi_queue_rq(struct blk_mq_hw_ctx *hctx,
 
 	if (!(req->rq_flags & RQF_DONTPREP)) {
 		ret = prep_to_mq(scsi_mq_prep_fn(req));
-		if (ret != BLK_MQ_RQ_QUEUE_OK)
+		if (ret != BLK_STS_OK)
 			goto out_dec_host_busy;
 		req->rq_flags |= RQF_DONTPREP;
 	} else {
@@ -1960,11 +1956,11 @@ static int scsi_queue_rq(struct blk_mq_hw_ctx *hctx,
 	reason = scsi_dispatch_cmd(cmd);
 	if (reason) {
 		scsi_set_blocked(cmd, reason);
-		ret = BLK_MQ_RQ_QUEUE_BUSY;
+		ret = BLK_STS_RESOURCE;
 		goto out_dec_host_busy;
 	}
 
-	return BLK_MQ_RQ_QUEUE_OK;
+	return BLK_STS_OK;
 
 out_dec_host_busy:
 	atomic_dec(&shost->host_busy);
@@ -1977,12 +1973,14 @@ out_put_device:
 	put_device(&sdev->sdev_gendev);
 out:
 	switch (ret) {
-	case BLK_MQ_RQ_QUEUE_BUSY:
+	case BLK_STS_OK:
+		break;
+	case BLK_STS_RESOURCE:
 		if (atomic_read(&sdev->device_busy) == 0 &&
 		    !scsi_device_blocked(sdev))
 			blk_mq_delay_run_hw_queue(hctx, SCSI_QUEUE_DELAY);
 		break;
-	case BLK_MQ_RQ_QUEUE_ERROR:
+	default:
 		/*
 		 * Make sure to release all allocated ressources when
 		 * we hit an error, as we will never see this command
@@ -1991,8 +1989,6 @@ out:
 		if (req->rq_flags & RQF_DONTPREP)
 			scsi_mq_uninit_cmd(cmd);
 		break;
-	default:
-		break;
 	}
 	return ret;
 }
@@ -2073,6 +2069,8 @@ void __scsi_init_queue(struct Scsi_Host *shost, struct request_queue *q)
 {
 	struct device *dev = shost->dma_dev;
 
+	queue_flag_set_unlocked(QUEUE_FLAG_SCSI_PASSTHROUGH, q);
+
 	/*
 	 * this limit is imposed by hardware restrictions
 	 */
@@ -2159,6 +2157,7 @@ struct request_queue *scsi_alloc_queue(struct scsi_device *sdev)
 	q->request_fn = scsi_request_fn;
 	q->init_rq_fn = scsi_init_rq;
 	q->exit_rq_fn = scsi_exit_rq;
+	q->initialize_rq_fn = scsi_initialize_rq;
 
 	if (blk_init_allocated_queue(q) < 0) {
 		blk_cleanup_queue(q);
@@ -2183,6 +2182,7 @@ static const struct blk_mq_ops scsi_mq_ops = {
 #endif
 	.init_request	= scsi_init_request,
 	.exit_request	= scsi_exit_request,
+	.initialize_rq_fn = scsi_initialize_rq,
 	.map_queues	= scsi_map_queues,
 };
 
@@ -2989,7 +2989,7 @@ int scsi_internal_device_block_nowait(struct scsi_device *sdev)
 	 * request queue. 
 	 */
 	if (q->mq_ops) {
-		blk_mq_stop_hw_queues(q);
+		blk_mq_quiesce_queue_nowait(q);
 	} else {
 		spin_lock_irqsave(q->queue_lock, flags);
 		blk_stop_queue(q);
@@ -3043,7 +3043,7 @@ void scsi_start_queue(struct scsi_device *sdev)
 	unsigned long flags;
 
 	if (q->mq_ops) {
-		blk_mq_start_stopped_hw_queues(q, false);
+		blk_mq_unquiesce_queue(q);
 	} else {
 		spin_lock_irqsave(q->queue_lock, flags);
 		blk_start_queue(q);
diff --git a/drivers/scsi/scsi_scan.c b/drivers/scsi/scsi_scan.c
index 3832ba57151b..fd88dabd599d 100644
--- a/drivers/scsi/scsi_scan.c
+++ b/drivers/scsi/scsi_scan.c
@@ -1037,10 +1037,11 @@ static unsigned char *scsi_inq_str(unsigned char *buf, unsigned char *inq,
  *     allocate and set it up by calling scsi_add_lun.
  *
  * Return:
- *     SCSI_SCAN_NO_RESPONSE: could not allocate or setup a scsi_device
- *     SCSI_SCAN_TARGET_PRESENT: target responded, but no device is
+ *
+ *   - SCSI_SCAN_NO_RESPONSE: could not allocate or setup a scsi_device
+ *   - SCSI_SCAN_TARGET_PRESENT: target responded, but no device is
  *         attached at the LUN
- *     SCSI_SCAN_LUN_PRESENT: a new scsi_device was allocated and initialized
+ *   - SCSI_SCAN_LUN_PRESENT: a new scsi_device was allocated and initialized
  **/
 static int scsi_probe_and_add_lun(struct scsi_target *starget,
 				  u64 lun, int *bflagsp,
diff --git a/drivers/scsi/scsi_transport_fc.c b/drivers/scsi/scsi_transport_fc.c
index 6dd0922a499d..7e24aa30c3b0 100644
--- a/drivers/scsi/scsi_transport_fc.c
+++ b/drivers/scsi/scsi_transport_fc.c
@@ -2914,16 +2914,18 @@ EXPORT_SYMBOL(fc_remote_port_add);
  * port is no longer part of the topology. Note: Although a port
  * may no longer be part of the topology, it may persist in the remote
  * ports displayed by the fc_host. We do this under 2 conditions:
+ *
  * 1) If the port was a scsi target, we delay its deletion by "blocking" it.
- *   This allows the port to temporarily disappear, then reappear without
- *   disrupting the SCSI device tree attached to it. During the "blocked"
- *   period the port will still exist.
+ *    This allows the port to temporarily disappear, then reappear without
+ *    disrupting the SCSI device tree attached to it. During the "blocked"
+ *    period the port will still exist.
+ *
  * 2) If the port was a scsi target and disappears for longer than we
- *   expect, we'll delete the port and the tear down the SCSI device tree
- *   attached to it. However, we want to semi-persist the target id assigned
- *   to that port if it eventually does exist. The port structure will
- *   remain (although with minimal information) so that the target id
- *   bindings also remain.
+ *    expect, we'll delete the port and the tear down the SCSI device tree
+ *    attached to it. However, we want to semi-persist the target id assigned
+ *    to that port if it eventually does exist. The port structure will
+ *    remain (although with minimal information) so that the target id
+ *    bindings also remain.
  *
  * If the remote port is not an FCP Target, it will be fully torn down
  * and deallocated, including the fc_remote_port class device.
diff --git a/drivers/scsi/scsi_transport_sas.c b/drivers/scsi/scsi_transport_sas.c
index 0ebe2f1bb908..5006a656e16a 100644
--- a/drivers/scsi/scsi_transport_sas.c
+++ b/drivers/scsi/scsi_transport_sas.c
@@ -33,6 +33,7 @@
 #include <linux/bsg.h>
 
 #include <scsi/scsi.h>
+#include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_request.h>
 #include <scsi/scsi_device.h>
 #include <scsi/scsi_host.h>
@@ -172,7 +173,7 @@ static void sas_smp_request(struct request_queue *q, struct Scsi_Host *shost,
 			    struct sas_rphy *rphy)
 {
 	struct request *req;
-	int ret;
+	blk_status_t ret;
 	int (*handler)(struct Scsi_Host *, struct sas_rphy *, struct request *);
 
 	while ((req = blk_fetch_request(q)) != NULL) {
@@ -230,6 +231,7 @@ static int sas_bsg_initialize(struct Scsi_Host *shost, struct sas_rphy *rphy)
 	q = blk_alloc_queue(GFP_KERNEL);
 	if (!q)
 		return -ENOMEM;
+	q->initialize_rq_fn = scsi_initialize_rq;
 	q->cmd_size = sizeof(struct scsi_request);
 
 	if (rphy) {
@@ -249,6 +251,11 @@ static int sas_bsg_initialize(struct Scsi_Host *shost, struct sas_rphy *rphy)
 	if (error)
 		goto out_cleanup_queue;
 
+	/*
+	 * by default assume old behaviour and bounce for any highmem page
+	 */
+	blk_queue_bounce_limit(q, BLK_BOUNCE_HIGH);
+
 	error = bsg_register_queue(q, dev, name, release);
 	if (error)
 		goto out_cleanup_queue;
@@ -264,6 +271,7 @@ static int sas_bsg_initialize(struct Scsi_Host *shost, struct sas_rphy *rphy)
 		q->queuedata = shost;
 
 	queue_flag_set_unlocked(QUEUE_FLAG_BIDI, q);
+	queue_flag_set_unlocked(QUEUE_FLAG_SCSI_PASSTHROUGH, q);
 	return 0;
 
 out_cleanup_queue:
diff --git a/drivers/scsi/scsicam.c b/drivers/scsi/scsicam.c
index 910f4a7a3924..31273468589c 100644
--- a/drivers/scsi/scsicam.c
+++ b/drivers/scsi/scsicam.c
@@ -116,8 +116,8 @@ EXPORT_SYMBOL(scsicam_bios_param);
  * @hds: put heads here
  * @secs: put sectors here
  *
- * Description: determine the BIOS mapping/geometry used to create the partition
- *      table, storing the results in *cyls, *hds, and *secs 
+ * Determine the BIOS mapping/geometry used to create the partition
+ * table, storing the results in @cyls, @hds, and @secs
  *
  * Returns: -1 on failure, 0 on success.
  */
diff --git a/drivers/scsi/sd.c b/drivers/scsi/sd.c
index 7dc8cb8f1818..bea36adeee17 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -50,6 +50,7 @@
 #include <linux/string_helpers.h>
 #include <linux/async.h>
 #include <linux/slab.h>
+#include <linux/sed-opal.h>
 #include <linux/pm_runtime.h>
 #include <linux/pr.h>
 #include <linux/t10-pi.h>
@@ -629,6 +630,26 @@ static void scsi_disk_put(struct scsi_disk *sdkp)
 	mutex_unlock(&sd_ref_mutex);
 }
 
+#ifdef CONFIG_BLK_SED_OPAL
+static int sd_sec_submit(void *data, u16 spsp, u8 secp, void *buffer,
+		size_t len, bool send)
+{
+	struct scsi_device *sdev = data;
+	u8 cdb[12] = { 0, };
+	int ret;
+
+	cdb[0] = send ? SECURITY_PROTOCOL_OUT : SECURITY_PROTOCOL_IN;
+	cdb[1] = secp;
+	put_unaligned_be16(spsp, &cdb[2]);
+	put_unaligned_be32(len, &cdb[6]);
+
+	ret = scsi_execute_req(sdev, cdb,
+			send ? DMA_TO_DEVICE : DMA_FROM_DEVICE,
+			buffer, len, NULL, SD_TIMEOUT, SD_MAX_RETRIES, NULL);
+	return ret <= 0 ? ret : -EIO;
+}
+#endif /* CONFIG_BLK_SED_OPAL */
+
 static unsigned char sd_setup_protect_cmnd(struct scsi_cmnd *scmd,
 					   unsigned int dix, unsigned int dif)
 {
@@ -1441,6 +1462,9 @@ static int sd_ioctl(struct block_device *bdev, fmode_t mode,
 	if (error)
 		goto out;
 
+	if (is_sed_ioctl(cmd))
+		return sed_ioctl(sdkp->opal_dev, cmd, p);
+
 	/*
 	 * Send SCSI addressing ioctls directly to mid level, send other
 	 * ioctls to block level and then onto mid level if they can't be
@@ -3004,6 +3028,20 @@ static void sd_read_write_same(struct scsi_disk *sdkp, unsigned char *buffer)
 		sdkp->ws10 = 1;
 }
 
+static void sd_read_security(struct scsi_disk *sdkp, unsigned char *buffer)
+{
+	struct scsi_device *sdev = sdkp->device;
+
+	if (!sdev->security_supported)
+		return;
+
+	if (scsi_report_opcode(sdev, buffer, SD_BUF_SIZE,
+			SECURITY_PROTOCOL_IN) == 1 &&
+	    scsi_report_opcode(sdev, buffer, SD_BUF_SIZE,
+			SECURITY_PROTOCOL_OUT) == 1)
+		sdkp->security = 1;
+}
+
 /**
  *	sd_revalidate_disk - called the first time a new disk is seen,
  *	performs disk spin up, read_capacity, etc.
@@ -3057,6 +3095,7 @@ static int sd_revalidate_disk(struct gendisk *disk)
 		sd_read_cache_type(sdkp, buffer);
 		sd_read_app_tag_own(sdkp, buffer);
 		sd_read_write_same(sdkp, buffer);
+		sd_read_security(sdkp, buffer);
 	}
 
 	sdkp->first_scan = 0;
@@ -3217,6 +3256,12 @@ static void sd_probe_async(void *data, async_cookie_t cookie)
 
 	sd_revalidate_disk(gd);
 
+	if (sdkp->security) {
+		sdkp->opal_dev = init_opal_dev(sdp, &sd_sec_submit);
+		if (sdkp->opal_dev)
+			sd_printk(KERN_NOTICE, sdkp, "supports TCG Opal\n");
+	}
+
 	sd_printk(KERN_NOTICE, sdkp, "Attached SCSI %sdisk\n",
 		  sdp->removable ? "removable " : "");
 	scsi_autopm_put_device(sdp);
@@ -3366,6 +3411,8 @@ static int sd_remove(struct device *dev)
 
 	sd_zbc_remove(sdkp);
 
+	free_opal_dev(sdkp->opal_dev);
+
 	blk_register_region(devt, SD_MINORS, NULL,
 			    sd_default_probe, NULL, NULL);
 
@@ -3518,6 +3565,7 @@ static int sd_suspend_runtime(struct device *dev)
 static int sd_resume(struct device *dev)
 {
 	struct scsi_disk *sdkp = dev_get_drvdata(dev);
+	int ret;
 
 	if (!sdkp)	/* E.g.: runtime resume at the start of sd_probe() */
 		return 0;
@@ -3526,7 +3574,10 @@ static int sd_resume(struct device *dev)
 		return 0;
 
 	sd_printk(KERN_NOTICE, sdkp, "Starting disk\n");
-	return sd_start_stop_device(sdkp, 1);
+	ret = sd_start_stop_device(sdkp, 1);
+	if (!ret)
+		opal_unlock_from_suspend(sdkp->opal_dev);
+	return ret;
 }
 
 /**
diff --git a/drivers/scsi/sd.h b/drivers/scsi/sd.h
index 61d02efd366c..99c4dde9b6bf 100644
--- a/drivers/scsi/sd.h
+++ b/drivers/scsi/sd.h
@@ -71,6 +71,7 @@ struct scsi_disk {
 	struct scsi_device *device;
 	struct device	dev;
 	struct gendisk	*disk;
+	struct opal_dev *opal_dev;
 #ifdef CONFIG_BLK_DEV_ZONED
 	unsigned int	nr_zones;
 	unsigned int	zone_blocks;
@@ -114,6 +115,7 @@ struct scsi_disk {
 	unsigned	rc_basis: 2;
 	unsigned	zoned: 2;
 	unsigned	urswrz : 1;
+	unsigned	security : 1;
 	unsigned	ignore_medium_access_errors : 1;
 };
 #define to_scsi_disk(obj) container_of(obj,struct scsi_disk,dev)
diff --git a/drivers/scsi/sg.c b/drivers/scsi/sg.c
index 82c33a6edbea..21225d62b0c1 100644
--- a/drivers/scsi/sg.c
+++ b/drivers/scsi/sg.c
@@ -177,7 +177,7 @@ typedef struct sg_device { /* holds the state of each scsi generic device */
 } Sg_device;
 
 /* tasklet or soft irq callback */
-static void sg_rq_end_io(struct request *rq, int uptodate);
+static void sg_rq_end_io(struct request *rq, blk_status_t status);
 static int sg_start_req(Sg_request *srp, unsigned char *cmd);
 static int sg_finish_rem_req(Sg_request * srp);
 static int sg_build_indirect(Sg_scatter_hold * schp, Sg_fd * sfp, int buff_size);
@@ -808,7 +808,7 @@ sg_common_write(Sg_fd * sfp, Sg_request * srp,
 	if (atomic_read(&sdp->detaching)) {
 		if (srp->bio) {
 			scsi_req_free_cmd(scsi_req(srp->rq));
-			blk_end_request_all(srp->rq, -EIO);
+			blk_end_request_all(srp->rq, BLK_STS_IOERR);
 			srp->rq = NULL;
 		}
 
@@ -1300,7 +1300,7 @@ sg_rq_end_io_usercontext(struct work_struct *work)
  * level when a command is completed (or has failed).
  */
 static void
-sg_rq_end_io(struct request *rq, int uptodate)
+sg_rq_end_io(struct request *rq, blk_status_t status)
 {
 	struct sg_request *srp = rq->end_io_data;
 	struct scsi_request *req = scsi_req(rq);
@@ -1732,8 +1732,6 @@ sg_start_req(Sg_request *srp, unsigned char *cmd)
 	}
 	req = scsi_req(rq);
 
-	scsi_req_init(rq);
-
 	if (hp->cmd_len > BLK_MAX_CDB)
 		req->cmd = long_cmdp;
 	memcpy(req->cmd, cmd, hp->cmd_len);
diff --git a/drivers/scsi/st.c b/drivers/scsi/st.c
index 1ea34d6f5437..8e5013d9cad4 100644
--- a/drivers/scsi/st.c
+++ b/drivers/scsi/st.c
@@ -511,7 +511,7 @@ static void st_do_stats(struct scsi_tape *STp, struct request *req)
 	atomic64_dec(&STp->stats->in_flight);
 }
 
-static void st_scsi_execute_end(struct request *req, int uptodate)
+static void st_scsi_execute_end(struct request *req, blk_status_t status)
 {
 	struct st_request *SRpnt = req->end_io_data;
 	struct scsi_request *rq = scsi_req(req);
@@ -549,7 +549,6 @@ static int st_scsi_execute(struct st_request *SRpnt, const unsigned char *cmd,
 	if (IS_ERR(req))
 		return DRIVER_ERROR << 24;
 	rq = scsi_req(req);
-	scsi_req_init(req);
 	req->rq_flags |= RQF_QUIET;
 
 	mdata->null_mapped = 1;
diff --git a/drivers/sh/superhyway/superhyway-sysfs.c b/drivers/sh/superhyway/superhyway-sysfs.c
index 55434330867b..774f31b564f8 100644
--- a/drivers/sh/superhyway/superhyway-sysfs.c
+++ b/drivers/sh/superhyway/superhyway-sysfs.c
@@ -19,7 +19,8 @@ static ssize_t name##_show(struct device *dev, struct device_attribute *attr, ch
 {									\
 	struct superhyway_device *s = to_superhyway_device(dev);	\
 	return sprintf(buf, fmt, s->field);				\
-}
+}									\
+static DEVICE_ATTR_RO(name);
 
 /* VCR flags */
 superhyway_ro_attr(perr_flags, "0x%02x\n", vcr.perr_flags);
@@ -32,14 +33,22 @@ superhyway_ro_attr(top_mb, "0x%02x\n", vcr.top_mb);
 /* Misc */
 superhyway_ro_attr(resource, "0x%08lx\n", resource[0].start);
 
-struct device_attribute superhyway_dev_attrs[] = {
-	__ATTR_RO(perr_flags),
-	__ATTR_RO(merr_flags),
-	__ATTR_RO(mod_vers),
-	__ATTR_RO(mod_id),
-	__ATTR_RO(bot_mb),
-	__ATTR_RO(top_mb),
-	__ATTR_RO(resource),
-	__ATTR_NULL,
+static struct attribute *superhyway_dev_attrs[] = {
+	&dev_attr_perr_flags.attr,
+	&dev_attr_merr_flags.attr,
+	&dev_attr_mod_vers.attr,
+	&dev_attr_mod_id.attr,
+	&dev_attr_bot_mb.attr,
+	&dev_attr_top_mb.attr,
+	&dev_attr_resource.attr,
+	NULL,
 };
 
+static const struct attribute_group superhyway_dev_group = {
+	.attrs = superhyway_dev_attrs,
+};
+
+const struct attribute_group *superhyway_dev_groups[] = {
+	&superhyway_dev_group,
+	NULL,
+};
diff --git a/drivers/sh/superhyway/superhyway.c b/drivers/sh/superhyway/superhyway.c
index bb1fb7712134..348836b90605 100644
--- a/drivers/sh/superhyway/superhyway.c
+++ b/drivers/sh/superhyway/superhyway.c
@@ -209,7 +209,7 @@ struct bus_type superhyway_bus_type = {
 	.name		= "superhyway",
 	.match		= superhyway_bus_match,
 #ifdef CONFIG_SYSFS
-	.dev_attrs	= superhyway_dev_attrs,
+	.dev_groups	= superhyway_dev_groups,
 #endif
 	.probe		= superhyway_device_probe,
 	.remove		= superhyway_device_remove,
diff --git a/drivers/soc/Kconfig b/drivers/soc/Kconfig
index 309643fe35f9..07fc0ac51c52 100644
--- a/drivers/soc/Kconfig
+++ b/drivers/soc/Kconfig
@@ -1,11 +1,13 @@
 menu "SOC (System On Chip) specific Drivers"
 
+source "drivers/soc/actions/Kconfig"
 source "drivers/soc/atmel/Kconfig"
 source "drivers/soc/bcm/Kconfig"
 source "drivers/soc/fsl/Kconfig"
 source "drivers/soc/imx/Kconfig"
 source "drivers/soc/mediatek/Kconfig"
 source "drivers/soc/qcom/Kconfig"
+source "drivers/soc/renesas/Kconfig"
 source "drivers/soc/rockchip/Kconfig"
 source "drivers/soc/samsung/Kconfig"
 source "drivers/soc/sunxi/Kconfig"
diff --git a/drivers/soc/Makefile b/drivers/soc/Makefile
index 824b44281efa..9241125416ba 100644
--- a/drivers/soc/Makefile
+++ b/drivers/soc/Makefile
@@ -2,6 +2,7 @@
 # Makefile for the Linux Kernel SOC specific device drivers.
 #
 
+obj-$(CONFIG_ARCH_ACTIONS)	+= actions/
 obj-$(CONFIG_ARCH_AT91)		+= atmel/
 obj-y				+= bcm/
 obj-$(CONFIG_ARCH_DOVE)		+= dove/
@@ -10,7 +11,7 @@ obj-y				+= fsl/
 obj-$(CONFIG_ARCH_MXC)		+= imx/
 obj-$(CONFIG_ARCH_MEDIATEK)	+= mediatek/
 obj-$(CONFIG_ARCH_QCOM)		+= qcom/
-obj-$(CONFIG_ARCH_RENESAS)	+= renesas/
+obj-y				+= renesas/
 obj-$(CONFIG_ARCH_ROCKCHIP)	+= rockchip/
 obj-$(CONFIG_SOC_SAMSUNG)	+= samsung/
 obj-$(CONFIG_ARCH_SUNXI)	+= sunxi/
diff --git a/drivers/soc/actions/Kconfig b/drivers/soc/actions/Kconfig
new file mode 100644
index 000000000000..9d68b5a771c3
--- /dev/null
+++ b/drivers/soc/actions/Kconfig
@@ -0,0 +1,16 @@
+if ARCH_ACTIONS || COMPILE_TEST
+
+config OWL_PM_DOMAINS_HELPER
+	bool
+
+config OWL_PM_DOMAINS
+	bool "Actions Semi SPS power domains"
+	depends on PM
+	select OWL_PM_DOMAINS_HELPER
+	select PM_GENERIC_DOMAINS
+	help
+	  Say 'y' here to enable support for Smart Power System (SPS)
+	  power-gating on Actions Semiconductor S500 SoC.
+	  If unsure, say 'n'.
+
+endif
diff --git a/drivers/soc/actions/Makefile b/drivers/soc/actions/Makefile
new file mode 100644
index 000000000000..1e101b06bab1
--- /dev/null
+++ b/drivers/soc/actions/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_OWL_PM_DOMAINS_HELPER) += owl-sps-helper.o
+obj-$(CONFIG_OWL_PM_DOMAINS) += owl-sps.o
diff --git a/drivers/soc/actions/owl-sps-helper.c b/drivers/soc/actions/owl-sps-helper.c
new file mode 100644
index 000000000000..9d7a2c2b44ec
--- /dev/null
+++ b/drivers/soc/actions/owl-sps-helper.c
@@ -0,0 +1,51 @@
+/*
+ * Actions Semi Owl Smart Power System (SPS) shared helpers
+ *
+ * Copyright 2012 Actions Semi Inc.
+ * Author: Actions Semi, Inc.
+ *
+ * Copyright (c) 2017 Andreas Färber
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/delay.h>
+#include <linux/io.h>
+
+#define OWL_SPS_PG_CTL	0x0
+
+int owl_sps_set_pg(void __iomem *base, u32 pwr_mask, u32 ack_mask, bool enable)
+{
+	u32 val;
+	bool ack;
+	int timeout;
+
+	val = readl(base + OWL_SPS_PG_CTL);
+	ack = val & ack_mask;
+	if (ack == enable)
+		return 0;
+
+	if (enable)
+		val |= pwr_mask;
+	else
+		val &= ~pwr_mask;
+
+	writel(val, base + OWL_SPS_PG_CTL);
+
+	for (timeout = 5000; timeout > 0; timeout -= 50) {
+		val = readl(base + OWL_SPS_PG_CTL);
+		if ((val & ack_mask) == (enable ? ack_mask : 0))
+			break;
+		udelay(50);
+	}
+	if (timeout <= 0)
+		return -ETIMEDOUT;
+
+	udelay(10);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(owl_sps_set_pg);
diff --git a/drivers/soc/actions/owl-sps.c b/drivers/soc/actions/owl-sps.c
new file mode 100644
index 000000000000..875225bfa21c
--- /dev/null
+++ b/drivers/soc/actions/owl-sps.c
@@ -0,0 +1,224 @@
+/*
+ * Actions Semi Owl Smart Power System (SPS)
+ *
+ * Copyright 2012 Actions Semi Inc.
+ * Author: Actions Semi, Inc.
+ *
+ * Copyright (c) 2017 Andreas Färber
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/pm_domain.h>
+#include <linux/soc/actions/owl-sps.h>
+#include <dt-bindings/power/owl-s500-powergate.h>
+
+struct owl_sps_domain_info {
+	const char *name;
+	int pwr_bit;
+	int ack_bit;
+	unsigned int genpd_flags;
+};
+
+struct owl_sps_info {
+	unsigned num_domains;
+	const struct owl_sps_domain_info *domains;
+};
+
+struct owl_sps {
+	struct device *dev;
+	const struct owl_sps_info *info;
+	void __iomem *base;
+	struct genpd_onecell_data genpd_data;
+	struct generic_pm_domain *domains[];
+};
+
+#define to_owl_pd(gpd) container_of(gpd, struct owl_sps_domain, genpd)
+
+struct owl_sps_domain {
+	struct generic_pm_domain genpd;
+	const struct owl_sps_domain_info *info;
+	struct owl_sps *sps;
+};
+
+static int owl_sps_set_power(struct owl_sps_domain *pd, bool enable)
+{
+	u32 pwr_mask, ack_mask;
+
+	ack_mask = BIT(pd->info->ack_bit);
+	pwr_mask = BIT(pd->info->pwr_bit);
+
+	return owl_sps_set_pg(pd->sps->base, pwr_mask, ack_mask, enable);
+}
+
+static int owl_sps_power_on(struct generic_pm_domain *domain)
+{
+	struct owl_sps_domain *pd = to_owl_pd(domain);
+
+	dev_dbg(pd->sps->dev, "%s power on", pd->info->name);
+
+	return owl_sps_set_power(pd, true);
+}
+
+static int owl_sps_power_off(struct generic_pm_domain *domain)
+{
+	struct owl_sps_domain *pd = to_owl_pd(domain);
+
+	dev_dbg(pd->sps->dev, "%s power off", pd->info->name);
+
+	return owl_sps_set_power(pd, false);
+}
+
+static int owl_sps_init_domain(struct owl_sps *sps, int index)
+{
+	struct owl_sps_domain *pd;
+
+	pd = devm_kzalloc(sps->dev, sizeof(*pd), GFP_KERNEL);
+	if (!pd)
+		return -ENOMEM;
+
+	pd->info = &sps->info->domains[index];
+	pd->sps = sps;
+
+	pd->genpd.name = pd->info->name;
+	pd->genpd.power_on = owl_sps_power_on;
+	pd->genpd.power_off = owl_sps_power_off;
+	pd->genpd.flags = pd->info->genpd_flags;
+	pm_genpd_init(&pd->genpd, NULL, false);
+
+	sps->genpd_data.domains[index] = &pd->genpd;
+
+	return 0;
+}
+
+static int owl_sps_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match;
+	const struct owl_sps_info *sps_info;
+	struct owl_sps *sps;
+	int i, ret;
+
+	if (!pdev->dev.of_node) {
+		dev_err(&pdev->dev, "no device node\n");
+		return -ENODEV;
+	}
+
+	match = of_match_device(pdev->dev.driver->of_match_table, &pdev->dev);
+	if (!match || !match->data) {
+		dev_err(&pdev->dev, "unknown compatible or missing data\n");
+		return -EINVAL;
+	}
+
+	sps_info = match->data;
+
+	sps = devm_kzalloc(&pdev->dev, sizeof(*sps) +
+			   sps_info->num_domains * sizeof(sps->domains[0]),
+			   GFP_KERNEL);
+	if (!sps)
+		return -ENOMEM;
+
+	sps->base = of_io_request_and_map(pdev->dev.of_node, 0, "owl-sps");
+	if (IS_ERR(sps->base)) {
+		dev_err(&pdev->dev, "failed to map sps registers\n");
+		return PTR_ERR(sps->base);
+	}
+
+	sps->dev = &pdev->dev;
+	sps->info = sps_info;
+	sps->genpd_data.domains = sps->domains;
+	sps->genpd_data.num_domains = sps_info->num_domains;
+
+	for (i = 0; i < sps_info->num_domains; i++) {
+		ret = owl_sps_init_domain(sps, i);
+		if (ret)
+			return ret;
+	}
+
+	ret = of_genpd_add_provider_onecell(pdev->dev.of_node, &sps->genpd_data);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to add provider (%d)", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct owl_sps_domain_info s500_sps_domains[] = {
+	[S500_PD_VDE] = {
+		.name = "VDE",
+		.pwr_bit = 0,
+		.ack_bit = 16,
+	},
+	[S500_PD_VCE_SI] = {
+		.name = "VCE_SI",
+		.pwr_bit = 1,
+		.ack_bit = 17,
+	},
+	[S500_PD_USB2_1] = {
+		.name = "USB2_1",
+		.pwr_bit = 2,
+		.ack_bit = 18,
+	},
+	[S500_PD_CPU2] = {
+		.name = "CPU2",
+		.pwr_bit = 5,
+		.ack_bit = 21,
+		.genpd_flags = GENPD_FLAG_ALWAYS_ON,
+	},
+	[S500_PD_CPU3] = {
+		.name = "CPU3",
+		.pwr_bit = 6,
+		.ack_bit = 22,
+		.genpd_flags = GENPD_FLAG_ALWAYS_ON,
+	},
+	[S500_PD_DMA] = {
+		.name = "DMA",
+		.pwr_bit = 8,
+		.ack_bit = 12,
+	},
+	[S500_PD_DS] = {
+		.name = "DS",
+		.pwr_bit = 9,
+		.ack_bit = 13,
+	},
+	[S500_PD_USB3] = {
+		.name = "USB3",
+		.pwr_bit = 10,
+		.ack_bit = 14,
+	},
+	[S500_PD_USB2_0] = {
+		.name = "USB2_0",
+		.pwr_bit = 11,
+		.ack_bit = 15,
+	},
+};
+
+static const struct owl_sps_info s500_sps_info = {
+	.num_domains = ARRAY_SIZE(s500_sps_domains),
+	.domains = s500_sps_domains,
+};
+
+static const struct of_device_id owl_sps_of_matches[] = {
+	{ .compatible = "actions,s500-sps", .data = &s500_sps_info },
+	{ }
+};
+
+static struct platform_driver owl_sps_platform_driver = {
+	.probe = owl_sps_probe,
+	.driver = {
+		.name = "owl-sps",
+		.of_match_table = owl_sps_of_matches,
+		.suppress_bind_attrs = true,
+	},
+};
+
+static int __init owl_sps_init(void)
+{
+	return platform_driver_register(&owl_sps_platform_driver);
+}
+postcore_initcall(owl_sps_init);
diff --git a/drivers/soc/atmel/soc.c b/drivers/soc/atmel/soc.c
index 4790094b498e..c1363c83c352 100644
--- a/drivers/soc/atmel/soc.c
+++ b/drivers/soc/atmel/soc.c
@@ -107,6 +107,30 @@ static const struct at91_soc __initconst socs[] = {
 	AT91_SOC(SAMA5D4_CIDR_MATCH, SAMA5D44_EXID_MATCH,
 		 "sama5d44", "sama5d4"),
 #endif
+#ifdef CONFIG_SOC_SAMV7
+	AT91_SOC(SAME70Q21_CIDR_MATCH, SAME70Q21_EXID_MATCH,
+		 "same70q21", "same7"),
+	AT91_SOC(SAME70Q20_CIDR_MATCH, SAME70Q20_EXID_MATCH,
+		 "same70q20", "same7"),
+	AT91_SOC(SAME70Q19_CIDR_MATCH, SAME70Q19_EXID_MATCH,
+		 "same70q19", "same7"),
+	AT91_SOC(SAMS70Q21_CIDR_MATCH, SAMS70Q21_EXID_MATCH,
+		 "sams70q21", "sams7"),
+	AT91_SOC(SAMS70Q20_CIDR_MATCH, SAMS70Q20_EXID_MATCH,
+		 "sams70q20", "sams7"),
+	AT91_SOC(SAMS70Q19_CIDR_MATCH, SAMS70Q19_EXID_MATCH,
+		 "sams70q19", "sams7"),
+	AT91_SOC(SAMV71Q21_CIDR_MATCH, SAMV71Q21_EXID_MATCH,
+		 "samv71q21", "samv7"),
+	AT91_SOC(SAMV71Q20_CIDR_MATCH, SAMV71Q20_EXID_MATCH,
+		 "samv71q20", "samv7"),
+	AT91_SOC(SAMV71Q19_CIDR_MATCH, SAMV71Q19_EXID_MATCH,
+		 "samv71q19", "samv7"),
+	AT91_SOC(SAMV70Q20_CIDR_MATCH, SAMV70Q20_EXID_MATCH,
+		 "samv70q20", "samv7"),
+	AT91_SOC(SAMV70Q19_CIDR_MATCH, SAMV70Q19_EXID_MATCH,
+		 "samv70q19", "samv7"),
+#endif
 	{ /* sentinel */ },
 };
 
diff --git a/drivers/soc/atmel/soc.h b/drivers/soc/atmel/soc.h
index 228efded5085..a90bd5b0ef8f 100644
--- a/drivers/soc/atmel/soc.h
+++ b/drivers/soc/atmel/soc.h
@@ -88,4 +88,30 @@ at91_soc_init(const struct at91_soc *socs);
 #define SAMA5D43_EXID_MATCH		0x00000003
 #define SAMA5D44_EXID_MATCH		0x00000004
 
+#define SAME70Q21_CIDR_MATCH		0x21020e00
+#define SAME70Q21_EXID_MATCH		0x00000002
+#define SAME70Q20_CIDR_MATCH		0x21020c00
+#define SAME70Q20_EXID_MATCH		0x00000002
+#define SAME70Q19_CIDR_MATCH		0x210d0a00
+#define SAME70Q19_EXID_MATCH		0x00000002
+
+#define SAMS70Q21_CIDR_MATCH		0x21120e00
+#define SAMS70Q21_EXID_MATCH		0x00000002
+#define SAMS70Q20_CIDR_MATCH		0x21120c00
+#define SAMS70Q20_EXID_MATCH		0x00000002
+#define SAMS70Q19_CIDR_MATCH		0x211d0a00
+#define SAMS70Q19_EXID_MATCH		0x00000002
+
+#define SAMV71Q21_CIDR_MATCH		0x21220e00
+#define SAMV71Q21_EXID_MATCH		0x00000002
+#define SAMV71Q20_CIDR_MATCH		0x21220c00
+#define SAMV71Q20_EXID_MATCH		0x00000002
+#define SAMV71Q19_CIDR_MATCH		0x212d0a00
+#define SAMV71Q19_EXID_MATCH		0x00000002
+
+#define SAMV70Q20_CIDR_MATCH		0x21320c00
+#define SAMV70Q20_EXID_MATCH		0x00000002
+#define SAMV70Q19_CIDR_MATCH		0x213d0a00
+#define SAMV70Q19_EXID_MATCH		0x00000002
+
 #endif /* __AT91_SOC_H */
diff --git a/drivers/soc/bcm/Kconfig b/drivers/soc/bcm/Kconfig
index a39b0d58ddd0..49f1e2a75d61 100644
--- a/drivers/soc/bcm/Kconfig
+++ b/drivers/soc/bcm/Kconfig
@@ -11,7 +11,7 @@ config RASPBERRYPI_POWER
 
 config SOC_BRCMSTB
 	bool "Broadcom STB SoC drivers"
-	depends on ARM
+	depends on ARM || ARM64 || BMIPS_GENERIC || COMPILE_TEST
 	select SOC_BUS
 	help
 	  Enables drivers for the Broadcom Set-Top Box (STB) series of chips.
diff --git a/drivers/soc/imx/Makefile b/drivers/soc/imx/Makefile
index 5b6e396c1121..aab41a5cc317 100644
--- a/drivers/soc/imx/Makefile
+++ b/drivers/soc/imx/Makefile
@@ -1,2 +1,2 @@
-obj-y += gpc.o
+obj-$(CONFIG_HAVE_IMX_GPC) += gpc.o
 obj-$(CONFIG_IMX7_PM_DOMAINS) += gpcv2.o
diff --git a/drivers/soc/mediatek/mtk-pmic-wrap.c b/drivers/soc/mediatek/mtk-pmic-wrap.c
index a5f10936fb9c..c80a04e1b2b1 100644
--- a/drivers/soc/mediatek/mtk-pmic-wrap.c
+++ b/drivers/soc/mediatek/mtk-pmic-wrap.c
@@ -1117,6 +1117,11 @@ static int pwrap_probe(struct platform_device *pdev)
 	const struct of_device_id *of_slave_id = NULL;
 	struct resource *res;
 
+	if (!of_id) {
+		dev_err(&pdev->dev, "Error: No device match found\n");
+		return -ENODEV;
+	}
+
 	if (pdev->dev.of_node->child)
 		of_slave_id = of_match_node(of_slave_match_tbl,
 					    pdev->dev.of_node->child);
@@ -1200,7 +1205,8 @@ static int pwrap_probe(struct platform_device *pdev)
 
 	if (!(pwrap_readl(wrp, PWRAP_WACS2_RDATA) & PWRAP_STATE_INIT_DONE0)) {
 		dev_dbg(wrp->dev, "initialization isn't finished\n");
-		return -ENODEV;
+		ret = -ENODEV;
+		goto err_out2;
 	}
 
 	/* Initialize watchdog, may not be done by the bootloader */
@@ -1220,8 +1226,10 @@ static int pwrap_probe(struct platform_device *pdev)
 		goto err_out2;
 
 	wrp->regmap = devm_regmap_init(wrp->dev, NULL, wrp, &pwrap_regmap_config);
-	if (IS_ERR(wrp->regmap))
-		return PTR_ERR(wrp->regmap);
+	if (IS_ERR(wrp->regmap)) {
+		ret = PTR_ERR(wrp->regmap);
+		goto err_out2;
+	}
 
 	ret = of_platform_populate(np, NULL, NULL, wrp->dev);
 	if (ret) {
diff --git a/drivers/soc/mediatek/mtk-scpsys.c b/drivers/soc/mediatek/mtk-scpsys.c
index beb79162369a..ceb2cc495cd0 100644
--- a/drivers/soc/mediatek/mtk-scpsys.c
+++ b/drivers/soc/mediatek/mtk-scpsys.c
@@ -21,6 +21,7 @@
 #include <linux/soc/mediatek/infracfg.h>
 
 #include <dt-bindings/power/mt2701-power.h>
+#include <dt-bindings/power/mt6797-power.h>
 #include <dt-bindings/power/mt8173-power.h>
 
 #define SPM_VDE_PWR_CON			0x0210
@@ -71,6 +72,7 @@ enum clk_id {
 	CLK_VENC,
 	CLK_VENC_LT,
 	CLK_ETHIF,
+	CLK_VDEC,
 	CLK_MAX,
 };
 
@@ -81,6 +83,7 @@ static const char * const clk_names[] = {
 	"venc",
 	"venc_lt",
 	"ethif",
+	"vdec",
 	NULL,
 };
 
@@ -107,21 +110,28 @@ struct scp_domain {
 	struct regulator *supply;
 };
 
+struct scp_ctrl_reg {
+	int pwr_sta_offs;
+	int pwr_sta2nd_offs;
+};
+
 struct scp {
 	struct scp_domain *domains;
 	struct genpd_onecell_data pd_data;
 	struct device *dev;
 	void __iomem *base;
 	struct regmap *infracfg;
+	struct scp_ctrl_reg ctrl_reg;
 };
 
 static int scpsys_domain_is_on(struct scp_domain *scpd)
 {
 	struct scp *scp = scpd->scp;
 
-	u32 status = readl(scp->base + SPM_PWR_STATUS) & scpd->data->sta_mask;
-	u32 status2 = readl(scp->base + SPM_PWR_STATUS_2ND) &
-				scpd->data->sta_mask;
+	u32 status = readl(scp->base + scp->ctrl_reg.pwr_sta_offs) &
+						scpd->data->sta_mask;
+	u32 status2 = readl(scp->base + scp->ctrl_reg.pwr_sta2nd_offs) &
+						scpd->data->sta_mask;
 
 	/*
 	 * A domain is on when both status bits are set. If only one is set
@@ -346,7 +356,8 @@ static void init_clks(struct platform_device *pdev, struct clk **clk)
 }
 
 static struct scp *init_scp(struct platform_device *pdev,
-			const struct scp_domain_data *scp_domain_data, int num)
+			const struct scp_domain_data *scp_domain_data, int num,
+			struct scp_ctrl_reg *scp_ctrl_reg)
 {
 	struct genpd_onecell_data *pd_data;
 	struct resource *res;
@@ -358,6 +369,9 @@ static struct scp *init_scp(struct platform_device *pdev,
 	if (!scp)
 		return ERR_PTR(-ENOMEM);
 
+	scp->ctrl_reg.pwr_sta_offs = scp_ctrl_reg->pwr_sta_offs;
+	scp->ctrl_reg.pwr_sta2nd_offs = scp_ctrl_reg->pwr_sta2nd_offs;
+
 	scp->dev = &pdev->dev;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -556,8 +570,13 @@ static const struct scp_domain_data scp_domain_data_mt2701[] = {
 static int __init scpsys_probe_mt2701(struct platform_device *pdev)
 {
 	struct scp *scp;
+	struct scp_ctrl_reg scp_reg;
 
-	scp = init_scp(pdev, scp_domain_data_mt2701, NUM_DOMAINS_MT2701);
+	scp_reg.pwr_sta_offs = SPM_PWR_STATUS;
+	scp_reg.pwr_sta2nd_offs = SPM_PWR_STATUS_2ND;
+
+	scp = init_scp(pdev, scp_domain_data_mt2701, NUM_DOMAINS_MT2701,
+		       &scp_reg);
 	if (IS_ERR(scp))
 		return PTR_ERR(scp);
 
@@ -567,6 +586,116 @@ static int __init scpsys_probe_mt2701(struct platform_device *pdev)
 }
 
 /*
+ * MT6797 power domain support
+ */
+
+static const struct scp_domain_data scp_domain_data_mt6797[] = {
+	[MT6797_POWER_DOMAIN_VDEC] = {
+		.name = "vdec",
+		.sta_mask = BIT(7),
+		.ctl_offs = 0x300,
+		.sram_pdn_bits = GENMASK(8, 8),
+		.sram_pdn_ack_bits = GENMASK(12, 12),
+		.clk_id = {CLK_VDEC},
+	},
+	[MT6797_POWER_DOMAIN_VENC] = {
+		.name = "venc",
+		.sta_mask = BIT(21),
+		.ctl_offs = 0x304,
+		.sram_pdn_bits = GENMASK(11, 8),
+		.sram_pdn_ack_bits = GENMASK(15, 12),
+		.clk_id = {CLK_NONE},
+	},
+	[MT6797_POWER_DOMAIN_ISP] = {
+		.name = "isp",
+		.sta_mask = BIT(5),
+		.ctl_offs = 0x308,
+		.sram_pdn_bits = GENMASK(9, 8),
+		.sram_pdn_ack_bits = GENMASK(13, 12),
+		.clk_id = {CLK_NONE},
+	},
+	[MT6797_POWER_DOMAIN_MM] = {
+		.name = "mm",
+		.sta_mask = BIT(3),
+		.ctl_offs = 0x30C,
+		.sram_pdn_bits = GENMASK(8, 8),
+		.sram_pdn_ack_bits = GENMASK(12, 12),
+		.clk_id = {CLK_MM},
+		.bus_prot_mask = (BIT(1) | BIT(2)),
+	},
+	[MT6797_POWER_DOMAIN_AUDIO] = {
+		.name = "audio",
+		.sta_mask = BIT(24),
+		.ctl_offs = 0x314,
+		.sram_pdn_bits = GENMASK(11, 8),
+		.sram_pdn_ack_bits = GENMASK(15, 12),
+		.clk_id = {CLK_NONE},
+	},
+	[MT6797_POWER_DOMAIN_MFG_ASYNC] = {
+		.name = "mfg_async",
+		.sta_mask = BIT(13),
+		.ctl_offs = 0x334,
+		.sram_pdn_bits = 0,
+		.sram_pdn_ack_bits = 0,
+		.clk_id = {CLK_MFG},
+	},
+	[MT6797_POWER_DOMAIN_MJC] = {
+		.name = "mjc",
+		.sta_mask = BIT(20),
+		.ctl_offs = 0x310,
+		.sram_pdn_bits = GENMASK(8, 8),
+		.sram_pdn_ack_bits = GENMASK(12, 12),
+		.clk_id = {CLK_NONE},
+	},
+};
+
+#define NUM_DOMAINS_MT6797	ARRAY_SIZE(scp_domain_data_mt6797)
+#define SPM_PWR_STATUS_MT6797		0x0180
+#define SPM_PWR_STATUS_2ND_MT6797	0x0184
+
+static int __init scpsys_probe_mt6797(struct platform_device *pdev)
+{
+	struct scp *scp;
+	struct genpd_onecell_data *pd_data;
+	int ret;
+	struct scp_ctrl_reg scp_reg;
+
+	scp_reg.pwr_sta_offs = SPM_PWR_STATUS_MT6797;
+	scp_reg.pwr_sta2nd_offs = SPM_PWR_STATUS_2ND_MT6797;
+
+	scp = init_scp(pdev, scp_domain_data_mt6797, NUM_DOMAINS_MT6797,
+		       &scp_reg);
+	if (IS_ERR(scp))
+		return PTR_ERR(scp);
+
+	mtk_register_power_domains(pdev, scp, NUM_DOMAINS_MT6797);
+
+	pd_data = &scp->pd_data;
+
+	ret = pm_genpd_add_subdomain(pd_data->domains[MT6797_POWER_DOMAIN_MM],
+				     pd_data->domains[MT6797_POWER_DOMAIN_VDEC]);
+	if (ret && IS_ENABLED(CONFIG_PM))
+		dev_err(&pdev->dev, "Failed to add subdomain: %d\n", ret);
+
+	ret = pm_genpd_add_subdomain(pd_data->domains[MT6797_POWER_DOMAIN_MM],
+				     pd_data->domains[MT6797_POWER_DOMAIN_ISP]);
+	if (ret && IS_ENABLED(CONFIG_PM))
+		dev_err(&pdev->dev, "Failed to add subdomain: %d\n", ret);
+
+	ret = pm_genpd_add_subdomain(pd_data->domains[MT6797_POWER_DOMAIN_MM],
+				     pd_data->domains[MT6797_POWER_DOMAIN_VENC]);
+	if (ret && IS_ENABLED(CONFIG_PM))
+		dev_err(&pdev->dev, "Failed to add subdomain: %d\n", ret);
+
+	ret = pm_genpd_add_subdomain(pd_data->domains[MT6797_POWER_DOMAIN_MM],
+				     pd_data->domains[MT6797_POWER_DOMAIN_MJC]);
+	if (ret && IS_ENABLED(CONFIG_PM))
+		dev_err(&pdev->dev, "Failed to add subdomain: %d\n", ret);
+
+	return 0;
+}
+
+/*
  * MT8173 power domain support
  */
 
@@ -667,8 +796,13 @@ static int __init scpsys_probe_mt8173(struct platform_device *pdev)
 	struct scp *scp;
 	struct genpd_onecell_data *pd_data;
 	int ret;
+	struct scp_ctrl_reg scp_reg;
 
-	scp = init_scp(pdev, scp_domain_data_mt8173, NUM_DOMAINS_MT8173);
+	scp_reg.pwr_sta_offs = SPM_PWR_STATUS;
+	scp_reg.pwr_sta2nd_offs = SPM_PWR_STATUS_2ND;
+
+	scp = init_scp(pdev, scp_domain_data_mt8173, NUM_DOMAINS_MT8173,
+		       &scp_reg);
 	if (IS_ERR(scp))
 		return PTR_ERR(scp);
 
@@ -698,6 +832,9 @@ static const struct of_device_id of_scpsys_match_tbl[] = {
 		.compatible = "mediatek,mt2701-scpsys",
 		.data = scpsys_probe_mt2701,
 	}, {
+		.compatible = "mediatek,mt6797-scpsys",
+		.data = scpsys_probe_mt6797,
+	}, {
 		.compatible = "mediatek,mt8173-scpsys",
 		.data = scpsys_probe_mt8173,
 	}, {
diff --git a/drivers/soc/qcom/smsm.c b/drivers/soc/qcom/smsm.c
index d0337b2a71c8..dc540ea92e9d 100644
--- a/drivers/soc/qcom/smsm.c
+++ b/drivers/soc/qcom/smsm.c
@@ -439,14 +439,15 @@ static int smsm_get_size_info(struct qcom_smsm *smsm)
 	} *info;
 
 	info = qcom_smem_get(QCOM_SMEM_HOST_ANY, SMEM_SMSM_SIZE_INFO, &size);
-	if (PTR_ERR(info) == -ENOENT || size != sizeof(*info)) {
+	if (IS_ERR(info) && PTR_ERR(info) != -ENOENT) {
+		if (PTR_ERR(info) != -EPROBE_DEFER)
+			dev_err(smsm->dev, "unable to retrieve smsm size info\n");
+		return PTR_ERR(info);
+	} else if (IS_ERR(info) || size != sizeof(*info)) {
 		dev_warn(smsm->dev, "no smsm size info, using defaults\n");
 		smsm->num_entries = SMSM_DEFAULT_NUM_ENTRIES;
 		smsm->num_hosts = SMSM_DEFAULT_NUM_HOSTS;
 		return 0;
-	} else if (IS_ERR(info)) {
-		dev_err(smsm->dev, "unable to retrieve smsm size info\n");
-		return PTR_ERR(info);
 	}
 
 	smsm->num_entries = info->num_entries;
diff --git a/drivers/soc/renesas/Kconfig b/drivers/soc/renesas/Kconfig
new file mode 100644
index 000000000000..87a4be46bd98
--- /dev/null
+++ b/drivers/soc/renesas/Kconfig
@@ -0,0 +1,63 @@
+config SOC_RENESAS
+	bool "Renesas SoC driver support" if COMPILE_TEST && !ARCH_RENESAS
+	default y if ARCH_RENESAS
+	select SOC_BUS
+	select RST_RCAR if ARCH_RCAR_GEN1 || ARCH_RCAR_GEN2 || \
+			   ARCH_R8A7795 || ARCH_R8A7796
+	select SYSC_R8A7743 if ARCH_R8A7743
+	select SYSC_R8A7745 if ARCH_R8A7745
+	select SYSC_R8A7779 if ARCH_R8A7779
+	select SYSC_R8A7790 if ARCH_R8A7790
+	select SYSC_R8A7791 if ARCH_R8A7791 || ARCH_R8A7793
+	select SYSC_R8A7792 if ARCH_R8A7792
+	select SYSC_R8A7794 if ARCH_R8A7794
+	select SYSC_R8A7795 if ARCH_R8A7795
+	select SYSC_R8A7796 if ARCH_R8A7796
+
+if SOC_RENESAS
+
+# SoC
+config SYSC_R8A7743
+	bool "RZ/G1M System Controller support" if COMPILE_TEST
+	select SYSC_RCAR
+
+config SYSC_R8A7745
+	bool "RZ/G1E System Controller support" if COMPILE_TEST
+	select SYSC_RCAR
+
+config SYSC_R8A7779
+	bool "R-Car H1 System Controller support" if COMPILE_TEST
+	select SYSC_RCAR
+
+config SYSC_R8A7790
+	bool "R-Car H2 System Controller support" if COMPILE_TEST
+	select SYSC_RCAR
+
+config SYSC_R8A7791
+	bool "R-Car M2-W/N System Controller support" if COMPILE_TEST
+	select SYSC_RCAR
+
+config SYSC_R8A7792
+	bool "R-Car V2H System Controller support" if COMPILE_TEST
+	select SYSC_RCAR
+
+config SYSC_R8A7794
+	bool "R-Car E2 System Controller support" if COMPILE_TEST
+	select SYSC_RCAR
+
+config SYSC_R8A7795
+	bool "R-Car H3 System Controller support" if COMPILE_TEST
+	select SYSC_RCAR
+
+config SYSC_R8A7796
+	bool "R-Car M3-W System Controller support" if COMPILE_TEST
+	select SYSC_RCAR
+
+# Family
+config RST_RCAR
+	bool "R-Car Reset Controller support" if COMPILE_TEST
+
+config SYSC_RCAR
+	bool "R-Car System Controller support" if COMPILE_TEST
+
+endif # SOC_RENESAS
diff --git a/drivers/soc/renesas/Makefile b/drivers/soc/renesas/Makefile
index d9115cb5ed9d..1a1a297b26a7 100644
--- a/drivers/soc/renesas/Makefile
+++ b/drivers/soc/renesas/Makefile
@@ -1,18 +1,17 @@
-obj-$(CONFIG_SOC_BUS)		+= renesas-soc.o
+# Generic, must be first because of soc_device_register()
+obj-$(CONFIG_SOC_RENESAS)	+= renesas-soc.o
 
-obj-$(CONFIG_ARCH_RCAR_GEN1)	+= rcar-rst.o
-obj-$(CONFIG_ARCH_RCAR_GEN2)	+= rcar-rst.o
-obj-$(CONFIG_ARCH_R8A7795)	+= rcar-rst.o
-obj-$(CONFIG_ARCH_R8A7796)	+= rcar-rst.o
+# SoC
+obj-$(CONFIG_SYSC_R8A7743)	+= r8a7743-sysc.o
+obj-$(CONFIG_SYSC_R8A7745)	+= r8a7745-sysc.o
+obj-$(CONFIG_SYSC_R8A7779)	+= r8a7779-sysc.o
+obj-$(CONFIG_SYSC_R8A7790)	+= r8a7790-sysc.o
+obj-$(CONFIG_SYSC_R8A7791)	+= r8a7791-sysc.o
+obj-$(CONFIG_SYSC_R8A7792)	+= r8a7792-sysc.o
+obj-$(CONFIG_SYSC_R8A7794)	+= r8a7794-sysc.o
+obj-$(CONFIG_SYSC_R8A7795)	+= r8a7795-sysc.o
+obj-$(CONFIG_SYSC_R8A7796)	+= r8a7796-sysc.o
 
-obj-$(CONFIG_ARCH_R8A7743)	+= rcar-sysc.o r8a7743-sysc.o
-obj-$(CONFIG_ARCH_R8A7745)	+= rcar-sysc.o r8a7745-sysc.o
-obj-$(CONFIG_ARCH_R8A7779)	+= rcar-sysc.o r8a7779-sysc.o
-obj-$(CONFIG_ARCH_R8A7790)	+= rcar-sysc.o r8a7790-sysc.o
-obj-$(CONFIG_ARCH_R8A7791)	+= rcar-sysc.o r8a7791-sysc.o
-obj-$(CONFIG_ARCH_R8A7792)	+= rcar-sysc.o r8a7792-sysc.o
-# R-Car M2-N is identical to R-Car M2-W w.r.t. power domains.
-obj-$(CONFIG_ARCH_R8A7793)	+= rcar-sysc.o r8a7791-sysc.o
-obj-$(CONFIG_ARCH_R8A7794)	+= rcar-sysc.o r8a7794-sysc.o
-obj-$(CONFIG_ARCH_R8A7795)	+= rcar-sysc.o r8a7795-sysc.o
-obj-$(CONFIG_ARCH_R8A7796)	+= rcar-sysc.o r8a7796-sysc.o
+# Family
+obj-$(CONFIG_RST_RCAR)		+= rcar-rst.o
+obj-$(CONFIG_SYSC_RCAR)		+= rcar-sysc.o
diff --git a/drivers/soc/renesas/rcar-sysc.c b/drivers/soc/renesas/rcar-sysc.c
index 528a13742aeb..7c8da3c90011 100644
--- a/drivers/soc/renesas/rcar-sysc.c
+++ b/drivers/soc/renesas/rcar-sysc.c
@@ -181,17 +181,6 @@ static int rcar_sysc_pd_power_off(struct generic_pm_domain *genpd)
 	struct rcar_sysc_pd *pd = to_rcar_pd(genpd);
 
 	pr_debug("%s: %s\n", __func__, genpd->name);
-
-	if (pd->flags & PD_NO_CR) {
-		pr_debug("%s: Cannot control %s\n", __func__, genpd->name);
-		return -EBUSY;
-	}
-
-	if (pd->flags & PD_BUSY) {
-		pr_debug("%s: %s busy\n", __func__, genpd->name);
-		return -EBUSY;
-	}
-
 	return rcar_sysc_power_down(&pd->ch);
 }
 
@@ -200,12 +189,6 @@ static int rcar_sysc_pd_power_on(struct generic_pm_domain *genpd)
 	struct rcar_sysc_pd *pd = to_rcar_pd(genpd);
 
 	pr_debug("%s: %s\n", __func__, genpd->name);
-
-	if (pd->flags & PD_NO_CR) {
-		pr_debug("%s: Cannot control %s\n", __func__, genpd->name);
-		return 0;
-	}
-
 	return rcar_sysc_power_up(&pd->ch);
 }
 
@@ -223,8 +206,7 @@ static void __init rcar_sysc_pd_setup(struct rcar_sysc_pd *pd)
 		 * only be turned off if the CPU is not in use.
 		 */
 		pr_debug("PM domain %s contains %s\n", name, "CPU");
-		pd->flags |= PD_BUSY;
-		gov = &pm_domain_always_on_gov;
+		genpd->flags |= GENPD_FLAG_ALWAYS_ON;
 	} else if (pd->flags & PD_SCU) {
 		/*
 		 * This domain contains an SCU and cache-controller, and
@@ -232,19 +214,17 @@ static void __init rcar_sysc_pd_setup(struct rcar_sysc_pd *pd)
 		 * not in use.
 		 */
 		pr_debug("PM domain %s contains %s\n", name, "SCU");
-		pd->flags |= PD_BUSY;
-		gov = &pm_domain_always_on_gov;
+		genpd->flags |= GENPD_FLAG_ALWAYS_ON;
 	} else if (pd->flags & PD_NO_CR) {
 		/*
 		 * This domain cannot be turned off.
 		 */
-		pd->flags |= PD_BUSY;
-		gov = &pm_domain_always_on_gov;
+		genpd->flags |= GENPD_FLAG_ALWAYS_ON;
 	}
 
 	if (!(pd->flags & (PD_CPU | PD_SCU))) {
 		/* Enable Clock Domain for I/O devices */
-		genpd->flags = GENPD_FLAG_PM_CLK;
+		genpd->flags |= GENPD_FLAG_PM_CLK;
 		if (has_cpg_mstp) {
 			genpd->attach_dev = cpg_mstp_attach_dev;
 			genpd->detach_dev = cpg_mstp_detach_dev;
@@ -275,35 +255,33 @@ finalize:
 }
 
 static const struct of_device_id rcar_sysc_matches[] = {
-#ifdef CONFIG_ARCH_R8A7743
+#ifdef CONFIG_SYSC_R8A7743
 	{ .compatible = "renesas,r8a7743-sysc", .data = &r8a7743_sysc_info },
 #endif
-#ifdef CONFIG_ARCH_R8A7745
+#ifdef CONFIG_SYSC_R8A7745
 	{ .compatible = "renesas,r8a7745-sysc", .data = &r8a7745_sysc_info },
 #endif
-#ifdef CONFIG_ARCH_R8A7779
+#ifdef CONFIG_SYSC_R8A7779
 	{ .compatible = "renesas,r8a7779-sysc", .data = &r8a7779_sysc_info },
 #endif
-#ifdef CONFIG_ARCH_R8A7790
+#ifdef CONFIG_SYSC_R8A7790
 	{ .compatible = "renesas,r8a7790-sysc", .data = &r8a7790_sysc_info },
 #endif
-#ifdef CONFIG_ARCH_R8A7791
+#ifdef CONFIG_SYSC_R8A7791
 	{ .compatible = "renesas,r8a7791-sysc", .data = &r8a7791_sysc_info },
-#endif
-#ifdef CONFIG_ARCH_R8A7792
-	{ .compatible = "renesas,r8a7792-sysc", .data = &r8a7792_sysc_info },
-#endif
-#ifdef CONFIG_ARCH_R8A7793
 	/* R-Car M2-N is identical to R-Car M2-W w.r.t. power domains. */
 	{ .compatible = "renesas,r8a7793-sysc", .data = &r8a7791_sysc_info },
 #endif
-#ifdef CONFIG_ARCH_R8A7794
+#ifdef CONFIG_SYSC_R8A7792
+	{ .compatible = "renesas,r8a7792-sysc", .data = &r8a7792_sysc_info },
+#endif
+#ifdef CONFIG_SYSC_R8A7794
 	{ .compatible = "renesas,r8a7794-sysc", .data = &r8a7794_sysc_info },
 #endif
-#ifdef CONFIG_ARCH_R8A7795
+#ifdef CONFIG_SYSC_R8A7795
 	{ .compatible = "renesas,r8a7795-sysc", .data = &r8a7795_sysc_info },
 #endif
-#ifdef CONFIG_ARCH_R8A7796
+#ifdef CONFIG_SYSC_R8A7796
 	{ .compatible = "renesas,r8a7796-sysc", .data = &r8a7796_sysc_info },
 #endif
 	{ /* sentinel */ }
diff --git a/drivers/soc/renesas/rcar-sysc.h b/drivers/soc/renesas/rcar-sysc.h
index 07edb049a401..1a5bebaf54ba 100644
--- a/drivers/soc/renesas/rcar-sysc.h
+++ b/drivers/soc/renesas/rcar-sysc.h
@@ -20,8 +20,6 @@
 #define PD_SCU		BIT(1)	/* Area contains SCU and L2 cache */
 #define PD_NO_CR	BIT(2)	/* Area lacks PWR{ON,OFF}CR registers */
 
-#define PD_BUSY		BIT(3)	/* Busy, for internal use only */
-
 #define PD_CPU_CR	PD_CPU		  /* CPU area has CR (R-Car H1) */
 #define PD_CPU_NOCR	PD_CPU | PD_NO_CR /* CPU area lacks CR (R-Car Gen2/3) */
 #define PD_ALWAYS_ON	PD_NO_CR	  /* Always-on area */
diff --git a/drivers/soc/tegra/Kconfig b/drivers/soc/tegra/Kconfig
index dcf088db40b6..1beb7c347344 100644
--- a/drivers/soc/tegra/Kconfig
+++ b/drivers/soc/tegra/Kconfig
@@ -115,3 +115,8 @@ config SOC_TEGRA_PMC
 
 config SOC_TEGRA_PMC_TEGRA186
 	bool
+
+config SOC_TEGRA_POWERGATE_BPMP
+	def_bool y
+	depends on PM_GENERIC_DOMAINS
+	depends on TEGRA_BPMP
diff --git a/drivers/soc/tegra/Makefile b/drivers/soc/tegra/Makefile
index 4f81dd55e5d1..0e52b45721ac 100644
--- a/drivers/soc/tegra/Makefile
+++ b/drivers/soc/tegra/Makefile
@@ -4,3 +4,4 @@ obj-y += common.o
 obj-$(CONFIG_SOC_TEGRA_FLOWCTRL) += flowctrl.o
 obj-$(CONFIG_SOC_TEGRA_PMC) += pmc.o
 obj-$(CONFIG_SOC_TEGRA_PMC_TEGRA186) += pmc-tegra186.o
+obj-$(CONFIG_SOC_TEGRA_POWERGATE_BPMP) += powergate-bpmp.o
diff --git a/drivers/soc/tegra/flowctrl.c b/drivers/soc/tegra/flowctrl.c
index 0e345c05fc65..5433cc7a043e 100644
--- a/drivers/soc/tegra/flowctrl.c
+++ b/drivers/soc/tegra/flowctrl.c
@@ -157,7 +157,7 @@ static int tegra_flowctrl_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	tegra_flowctrl_base = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(tegra_flowctrl_base))
-		return PTR_ERR(base);
+		return PTR_ERR(tegra_flowctrl_base);
 
 	iounmap(base);
 
diff --git a/drivers/soc/tegra/powergate-bpmp.c b/drivers/soc/tegra/powergate-bpmp.c
new file mode 100644
index 000000000000..8fc356039401
--- /dev/null
+++ b/drivers/soc/tegra/powergate-bpmp.c
@@ -0,0 +1,359 @@
+/*
+ * Copyright (c) 2016-2017, 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.
+ */
+
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pm_domain.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+
+#include <soc/tegra/bpmp.h>
+#include <soc/tegra/bpmp-abi.h>
+
+struct tegra_powergate_info {
+	unsigned int id;
+	char *name;
+};
+
+struct tegra_powergate {
+	struct generic_pm_domain genpd;
+	struct tegra_bpmp *bpmp;
+	unsigned int id;
+};
+
+static inline struct tegra_powergate *
+to_tegra_powergate(struct generic_pm_domain *genpd)
+{
+	return container_of(genpd, struct tegra_powergate, genpd);
+}
+
+static int tegra_bpmp_powergate_set_state(struct tegra_bpmp *bpmp,
+					  unsigned int id, u32 state)
+{
+	struct mrq_pg_request request;
+	struct tegra_bpmp_message msg;
+
+	memset(&request, 0, sizeof(request));
+	request.cmd = CMD_PG_SET_STATE;
+	request.id = id;
+	request.set_state.state = state;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.mrq = MRQ_PG;
+	msg.tx.data = &request;
+	msg.tx.size = sizeof(request);
+
+	return tegra_bpmp_transfer(bpmp, &msg);
+}
+
+static int tegra_bpmp_powergate_get_state(struct tegra_bpmp *bpmp,
+					  unsigned int id)
+{
+	struct mrq_pg_response response;
+	struct mrq_pg_request request;
+	struct tegra_bpmp_message msg;
+	int err;
+
+	memset(&request, 0, sizeof(request));
+	request.cmd = CMD_PG_GET_STATE;
+	request.id = id;
+
+	memset(&response, 0, sizeof(response));
+
+	memset(&msg, 0, sizeof(msg));
+	msg.mrq = MRQ_PG;
+	msg.tx.data = &request;
+	msg.tx.size = sizeof(request);
+	msg.rx.data = &response;
+	msg.rx.size = sizeof(response);
+
+	err = tegra_bpmp_transfer(bpmp, &msg);
+	if (err < 0)
+		return PG_STATE_OFF;
+
+	return response.get_state.state;
+}
+
+static int tegra_bpmp_powergate_get_max_id(struct tegra_bpmp *bpmp)
+{
+	struct mrq_pg_response response;
+	struct mrq_pg_request request;
+	struct tegra_bpmp_message msg;
+	int err;
+
+	memset(&request, 0, sizeof(request));
+	request.cmd = CMD_PG_GET_MAX_ID;
+
+	memset(&response, 0, sizeof(response));
+
+	memset(&msg, 0, sizeof(msg));
+	msg.mrq = MRQ_PG;
+	msg.tx.data = &request;
+	msg.tx.size = sizeof(request);
+	msg.rx.data = &response;
+	msg.rx.size = sizeof(response);
+
+	err = tegra_bpmp_transfer(bpmp, &msg);
+	if (err < 0)
+		return err;
+
+	return response.get_max_id.max_id;
+}
+
+static char *tegra_bpmp_powergate_get_name(struct tegra_bpmp *bpmp,
+					   unsigned int id)
+{
+	struct mrq_pg_response response;
+	struct mrq_pg_request request;
+	struct tegra_bpmp_message msg;
+	int err;
+
+	memset(&request, 0, sizeof(request));
+	request.cmd = CMD_PG_GET_NAME;
+	request.id = id;
+
+	memset(&response, 0, sizeof(response));
+
+	memset(&msg, 0, sizeof(msg));
+	msg.mrq = MRQ_PG;
+	msg.tx.data = &request;
+	msg.tx.size = sizeof(request);
+	msg.rx.data = &response;
+	msg.rx.size = sizeof(response);
+
+	err = tegra_bpmp_transfer(bpmp, &msg);
+	if (err < 0)
+		return NULL;
+
+	return kstrdup(response.get_name.name, GFP_KERNEL);
+}
+
+static inline bool tegra_bpmp_powergate_is_powered(struct tegra_bpmp *bpmp,
+						   unsigned int id)
+{
+	return tegra_bpmp_powergate_get_state(bpmp, id) != PG_STATE_OFF;
+}
+
+static int tegra_powergate_power_on(struct generic_pm_domain *domain)
+{
+	struct tegra_powergate *powergate = to_tegra_powergate(domain);
+	struct tegra_bpmp *bpmp = powergate->bpmp;
+
+	return tegra_bpmp_powergate_set_state(bpmp, powergate->id,
+					      PG_STATE_ON);
+}
+
+static int tegra_powergate_power_off(struct generic_pm_domain *domain)
+{
+	struct tegra_powergate *powergate = to_tegra_powergate(domain);
+	struct tegra_bpmp *bpmp = powergate->bpmp;
+
+	return tegra_bpmp_powergate_set_state(bpmp, powergate->id,
+					      PG_STATE_OFF);
+}
+
+static struct tegra_powergate *
+tegra_powergate_add(struct tegra_bpmp *bpmp,
+		    const struct tegra_powergate_info *info)
+{
+	struct tegra_powergate *powergate;
+	bool off;
+	int err;
+
+	off = !tegra_bpmp_powergate_is_powered(bpmp, info->id);
+
+	powergate = devm_kzalloc(bpmp->dev, sizeof(*powergate), GFP_KERNEL);
+	if (!powergate)
+		return ERR_PTR(-ENOMEM);
+
+	powergate->id = info->id;
+	powergate->bpmp = bpmp;
+
+	powergate->genpd.name = kstrdup(info->name, GFP_KERNEL);
+	powergate->genpd.power_on = tegra_powergate_power_on;
+	powergate->genpd.power_off = tegra_powergate_power_off;
+
+	err = pm_genpd_init(&powergate->genpd, NULL, off);
+	if (err < 0) {
+		kfree(powergate->genpd.name);
+		return ERR_PTR(err);
+	}
+
+	return powergate;
+}
+
+static void tegra_powergate_remove(struct tegra_powergate *powergate)
+{
+	struct generic_pm_domain *genpd = &powergate->genpd;
+	struct tegra_bpmp *bpmp = powergate->bpmp;
+	int err;
+
+	err = pm_genpd_remove(genpd);
+	if (err < 0)
+		dev_err(bpmp->dev, "failed to remove power domain %s: %d\n",
+			genpd->name, err);
+
+	kfree(genpd->name);
+}
+
+static int
+tegra_bpmp_probe_powergates(struct tegra_bpmp *bpmp,
+			    struct tegra_powergate_info **powergatesp)
+{
+	struct tegra_powergate_info *powergates;
+	unsigned int max_id, id, count = 0;
+	unsigned int num_holes = 0;
+	int err;
+
+	err = tegra_bpmp_powergate_get_max_id(bpmp);
+	if (err < 0)
+		return err;
+
+	max_id = err;
+
+	dev_dbg(bpmp->dev, "maximum powergate ID: %u\n", max_id);
+
+	powergates = kcalloc(max_id + 1, sizeof(*powergates), GFP_KERNEL);
+	if (!powergates)
+		return -ENOMEM;
+
+	for (id = 0; id <= max_id; id++) {
+		struct tegra_powergate_info *info = &powergates[count];
+
+		info->name = tegra_bpmp_powergate_get_name(bpmp, id);
+		if (!info->name || info->name[0] == '\0') {
+			num_holes++;
+			continue;
+		}
+
+		info->id = id;
+		count++;
+	}
+
+	dev_dbg(bpmp->dev, "holes: %u\n", num_holes);
+
+	*powergatesp = powergates;
+
+	return count;
+}
+
+static int tegra_bpmp_add_powergates(struct tegra_bpmp *bpmp,
+				     struct tegra_powergate_info *powergates,
+				     unsigned int count)
+{
+	struct genpd_onecell_data *genpd = &bpmp->genpd;
+	struct generic_pm_domain **domains;
+	struct tegra_powergate *powergate;
+	unsigned int i;
+	int err;
+
+	domains = kcalloc(count, sizeof(*domains), GFP_KERNEL);
+	if (!domains)
+		return -ENOMEM;
+
+	for (i = 0; i < count; i++) {
+		powergate = tegra_powergate_add(bpmp, &powergates[i]);
+		if (IS_ERR(powergate)) {
+			err = PTR_ERR(powergate);
+			goto remove;
+		}
+
+		dev_dbg(bpmp->dev, "added power domain %s\n",
+			powergate->genpd.name);
+		domains[i] = &powergate->genpd;
+	}
+
+	genpd->num_domains = count;
+	genpd->domains = domains;
+
+	return 0;
+
+remove:
+	while (i--) {
+		powergate = to_tegra_powergate(domains[i]);
+		tegra_powergate_remove(powergate);
+	}
+
+	kfree(genpd->domains);
+	return err;
+}
+
+static void tegra_bpmp_remove_powergates(struct tegra_bpmp *bpmp)
+{
+	struct genpd_onecell_data *genpd = &bpmp->genpd;
+	unsigned int i = genpd->num_domains;
+	struct tegra_powergate *powergate;
+
+	while (i--) {
+		dev_dbg(bpmp->dev, "removing power domain %s\n",
+			genpd->domains[i]->name);
+		powergate = to_tegra_powergate(genpd->domains[i]);
+		tegra_powergate_remove(powergate);
+	}
+}
+
+static struct generic_pm_domain *
+tegra_powergate_xlate(struct of_phandle_args *spec, void *data)
+{
+	struct generic_pm_domain *domain = ERR_PTR(-ENOENT);
+	struct genpd_onecell_data *genpd = data;
+	unsigned int i;
+
+	for (i = 0; i < genpd->num_domains; i++) {
+		struct tegra_powergate *powergate;
+
+		powergate = to_tegra_powergate(genpd->domains[i]);
+		if (powergate->id == spec->args[0]) {
+			domain = &powergate->genpd;
+			break;
+		}
+	}
+
+	return domain;
+}
+
+int tegra_bpmp_init_powergates(struct tegra_bpmp *bpmp)
+{
+	struct device_node *np = bpmp->dev->of_node;
+	struct tegra_powergate_info *powergates;
+	struct device *dev = bpmp->dev;
+	unsigned int count, i;
+	int err;
+
+	err = tegra_bpmp_probe_powergates(bpmp, &powergates);
+	if (err < 0)
+		return err;
+
+	count = err;
+
+	dev_dbg(dev, "%u power domains probed\n", count);
+
+	err = tegra_bpmp_add_powergates(bpmp, powergates, count);
+	if (err < 0)
+		goto free;
+
+	bpmp->genpd.xlate = tegra_powergate_xlate;
+
+	err = of_genpd_add_provider_onecell(np, &bpmp->genpd);
+	if (err < 0) {
+		dev_err(dev, "failed to add power domain provider: %d\n", err);
+		tegra_bpmp_remove_powergates(bpmp);
+	}
+
+free:
+	for (i = 0; i < count; i++)
+		kfree(powergates[i].name);
+
+	kfree(powergates);
+	return err;
+}
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 1761c9004fc1..9b31351fe429 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -393,6 +393,13 @@ config SPI_FSL_ESPI
 	  From MPC8536, 85xx platform uses the controller, and all P10xx,
 	  P20xx, P30xx,P40xx, P50xx uses this controller.
 
+config SPI_MESON_SPICC
+	tristate "Amlogic Meson SPICC controller"
+	depends on ARCH_MESON || COMPILE_TEST
+	help
+	  This enables master mode support for the SPICC (SPI communication
+	  controller) available in Amlogic Meson SoCs.
+
 config SPI_MESON_SPIFC
 	tristate "Amlogic Meson SPIFC controller"
 	depends on ARCH_MESON || COMPILE_TEST
@@ -457,6 +464,7 @@ config SPI_OMAP24XX
 
 config SPI_TI_QSPI
 	tristate "DRA7xxx QSPI controller support"
+	depends on HAS_DMA
 	depends on ARCH_OMAP2PLUS || COMPILE_TEST
 	help
 	  QSPI master controller for DRA7xxx used for flash devices.
@@ -619,6 +627,16 @@ config SPI_SIRF
 	help
 	  SPI driver for CSR SiRFprimaII SoCs
 
+config SPI_STM32
+	tristate "STMicroelectronics STM32 SPI controller"
+	depends on ARCH_STM32 || COMPILE_TEST
+	help
+	  SPI driver for STMicroelectonics STM32 SoCs.
+
+	  STM32 SPI controller supports DMA and PIO modes. When DMA
+	  is not available, the driver automatically falls back to
+	  PIO mode.
+
 config SPI_ST_SSC4
 	tristate "STMicroelectronics SPI SSC-based driver"
 	depends on ARCH_STI || COMPILE_TEST
@@ -784,6 +802,30 @@ config SPI_TLE62X0
 
 endif # SPI_MASTER
 
-# (slave support would go here)
+#
+# SLAVE side ... listening to other SPI masters
+#
+
+config SPI_SLAVE
+	bool "SPI slave protocol handlers"
+	help
+	  If your system has a slave-capable SPI controller, you can enable
+	  slave protocol handlers.
+
+if SPI_SLAVE
+
+config SPI_SLAVE_TIME
+	tristate "SPI slave handler reporting boot up time"
+	help
+	  SPI slave handler responding with the time of reception of the last
+	  SPI message.
+
+config SPI_SLAVE_SYSTEM_CONTROL
+	tristate "SPI slave handler controlling system state"
+	help
+	  SPI slave handler to allow remote control of system reboot, power
+	  off, halt, and suspend.
+
+endif # SPI_SLAVE
 
 endif # SPI
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index b375a7a89216..a3ae2b70cdc3 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -53,6 +53,7 @@ obj-$(CONFIG_SPI_LANTIQ_SSC)		+= spi-lantiq-ssc.o
 obj-$(CONFIG_SPI_JCORE)			+= spi-jcore.o
 obj-$(CONFIG_SPI_LM70_LLP)		+= spi-lm70llp.o
 obj-$(CONFIG_SPI_LP8841_RTC)		+= spi-lp8841-rtc.o
+obj-$(CONFIG_SPI_MESON_SPICC)		+= spi-meson-spicc.o
 obj-$(CONFIG_SPI_MESON_SPIFC)		+= spi-meson-spifc.o
 obj-$(CONFIG_SPI_MPC512x_PSC)		+= spi-mpc512x-psc.o
 obj-$(CONFIG_SPI_MPC52xx_PSC)		+= spi-mpc52xx-psc.o
@@ -89,6 +90,7 @@ obj-$(CONFIG_SPI_SH_HSPI)		+= spi-sh-hspi.o
 obj-$(CONFIG_SPI_SH_MSIOF)		+= spi-sh-msiof.o
 obj-$(CONFIG_SPI_SH_SCI)		+= spi-sh-sci.o
 obj-$(CONFIG_SPI_SIRF)		+= spi-sirf.o
+obj-$(CONFIG_SPI_STM32) 		+= spi-stm32.o
 obj-$(CONFIG_SPI_ST_SSC4)		+= spi-st-ssc4.o
 obj-$(CONFIG_SPI_SUN4I)			+= spi-sun4i.o
 obj-$(CONFIG_SPI_SUN6I)			+= spi-sun6i.o
@@ -105,3 +107,7 @@ obj-$(CONFIG_SPI_XILINX)		+= spi-xilinx.o
 obj-$(CONFIG_SPI_XLP)			+= spi-xlp.o
 obj-$(CONFIG_SPI_XTENSA_XTFPGA)		+= spi-xtensa-xtfpga.o
 obj-$(CONFIG_SPI_ZYNQMP_GQSPI)		+= spi-zynqmp-gqspi.o
+
+# SPI slave protocol handlers
+obj-$(CONFIG_SPI_SLAVE_TIME)		+= spi-slave-time.o
+obj-$(CONFIG_SPI_SLAVE_SYSTEM_CONTROL)	+= spi-slave-system-control.o
diff --git a/drivers/spi/spi-atmel.c b/drivers/spi/spi-atmel.c
index 1eb83c9613d5..f95da364c283 100644
--- a/drivers/spi/spi-atmel.c
+++ b/drivers/spi/spi-atmel.c
@@ -269,6 +269,7 @@ struct atmel_spi_caps {
 	bool	is_spi2;
 	bool	has_wdrbt;
 	bool	has_dma_support;
+	bool	has_pdc_support;
 };
 
 /*
@@ -1422,11 +1423,31 @@ static void atmel_get_caps(struct atmel_spi *as)
 	unsigned int version;
 
 	version = atmel_get_version(as);
-	dev_info(&as->pdev->dev, "version: 0x%x\n", version);
 
 	as->caps.is_spi2 = version > 0x121;
 	as->caps.has_wdrbt = version >= 0x210;
+#ifdef CONFIG_SOC_SAM_V4_V5
+	/*
+	 * Atmel SoCs based on ARM9 (SAM9x) cores should not use spi_map_buf()
+	 * since this later function tries to map buffers with dma_map_sg()
+	 * even if they have not been allocated inside DMA-safe areas.
+	 * On SoCs based on Cortex A5 (SAMA5Dx), it works anyway because for
+	 * those ARM cores, the data cache follows the PIPT model.
+	 * Also the L2 cache controller of SAMA5D2 uses the PIPT model too.
+	 * In case of PIPT caches, there cannot be cache aliases.
+	 * However on ARM9 cores, the data cache follows the VIVT model, hence
+	 * the cache aliases issue can occur when buffers are allocated from
+	 * DMA-unsafe areas, by vmalloc() for instance, where cache coherency is
+	 * not taken into account or at least not handled completely (cache
+	 * lines of aliases are not invalidated).
+	 * This is not a theorical issue: it was reproduced when trying to mount
+	 * a UBI file-system on a at91sam9g35ek board.
+	 */
+	as->caps.has_dma_support = false;
+#else
 	as->caps.has_dma_support = version >= 0x212;
+#endif
+	as->caps.has_pdc_support = version < 0x212;
 }
 
 /*-------------------------------------------------------------------------*/
@@ -1567,7 +1588,7 @@ static int atmel_spi_probe(struct platform_device *pdev)
 		} else if (ret == -EPROBE_DEFER) {
 			return ret;
 		}
-	} else {
+	} else if (as->caps.has_pdc_support) {
 		as->use_pdc = true;
 	}
 
@@ -1609,8 +1630,9 @@ static int atmel_spi_probe(struct platform_device *pdev)
 		goto out_free_dma;
 
 	/* go! */
-	dev_info(&pdev->dev, "Atmel SPI Controller at 0x%08lx (irq %d)\n",
-			(unsigned long)regs->start, irq);
+	dev_info(&pdev->dev, "Atmel SPI Controller version 0x%x at 0x%08lx (irq %d)\n",
+			atmel_get_version(as), (unsigned long)regs->start,
+			irq);
 
 	return 0;
 
diff --git a/drivers/spi/spi-bcm63xx-hsspi.c b/drivers/spi/spi-bcm63xx-hsspi.c
index 5514cd02e93a..4da2d4a524ca 100644
--- a/drivers/spi/spi-bcm63xx-hsspi.c
+++ b/drivers/spi/spi-bcm63xx-hsspi.c
@@ -484,6 +484,7 @@ static const struct of_device_id bcm63xx_hsspi_of_match[] = {
 	{ .compatible = "brcm,bcm6328-hsspi", },
 	{ },
 };
+MODULE_DEVICE_TABLE(of, bcm63xx_hsspi_of_match);
 
 static struct platform_driver bcm63xx_hsspi_driver = {
 	.driver = {
diff --git a/drivers/spi/spi-bcm63xx.c b/drivers/spi/spi-bcm63xx.c
index 247f71b02235..84c7356ce5b4 100644
--- a/drivers/spi/spi-bcm63xx.c
+++ b/drivers/spi/spi-bcm63xx.c
@@ -147,7 +147,7 @@ struct bcm63xx_spi {
 
 	/* Platform data */
 	const unsigned long	*reg_offsets;
-	unsigned		fifo_size;
+	unsigned int		fifo_size;
 	unsigned int		msg_type_shift;
 	unsigned int		msg_ctl_width;
 
@@ -191,7 +191,7 @@ static inline void bcm_spi_writew(struct bcm63xx_spi *bs,
 #endif
 }
 
-static const unsigned bcm63xx_spi_freq_table[SPI_CLK_MASK][2] = {
+static const unsigned int bcm63xx_spi_freq_table[SPI_CLK_MASK][2] = {
 	{ 20000000, SPI_CLK_20MHZ },
 	{ 12500000, SPI_CLK_12_50MHZ },
 	{  6250000, SPI_CLK_6_250MHZ },
diff --git a/drivers/spi/spi-davinci.c b/drivers/spi/spi-davinci.c
index 595acdcfc7d0..6ddb6ef1fda4 100644
--- a/drivers/spi/spi-davinci.c
+++ b/drivers/spi/spi-davinci.c
@@ -873,9 +873,8 @@ static int spi_davinci_get_pdata(struct platform_device *pdev,
 	return 0;
 }
 #else
-static struct davinci_spi_platform_data
-	*spi_davinci_get_pdata(struct platform_device *pdev,
-		struct davinci_spi *dspi)
+static int spi_davinci_get_pdata(struct platform_device *pdev,
+			struct davinci_spi *dspi)
 {
 	return -ENODEV;
 }
@@ -965,7 +964,9 @@ static int davinci_spi_probe(struct platform_device *pdev)
 		ret = -ENODEV;
 		goto free_master;
 	}
-	clk_prepare_enable(dspi->clk);
+	ret = clk_prepare_enable(dspi->clk);
+	if (ret)
+		goto free_master;
 
 	master->dev.of_node = pdev->dev.of_node;
 	master->bus_num = pdev->id;
diff --git a/drivers/spi/spi-fsl-dspi.c b/drivers/spi/spi-fsl-dspi.c
index 15201645bdc4..d89127f4a46d 100644
--- a/drivers/spi/spi-fsl-dspi.c
+++ b/drivers/spi/spi-fsl-dspi.c
@@ -1032,7 +1032,8 @@ static int dspi_probe(struct platform_device *pdev)
 		goto out_master_put;
 
 	if (dspi->devtype_data->trans_mode == DSPI_DMA_MODE) {
-		if (dspi_request_dma(dspi, res->start)) {
+		ret = dspi_request_dma(dspi, res->start);
+		if (ret < 0) {
 			dev_err(&pdev->dev, "can't get dma channels\n");
 			goto out_clk_put;
 		}
diff --git a/drivers/spi/spi-imx.c b/drivers/spi/spi-imx.c
index b402530a7a9a..f9698b7aeb3b 100644
--- a/drivers/spi/spi-imx.c
+++ b/drivers/spi/spi-imx.c
@@ -56,10 +56,6 @@
 
 /* The maximum  bytes that a sdma BD can transfer.*/
 #define MAX_SDMA_BD_BYTES  (1 << 15)
-struct spi_imx_config {
-	unsigned int speed_hz;
-	unsigned int bpw;
-};
 
 enum spi_imx_devtype {
 	IMX1_CSPI,
@@ -74,7 +70,7 @@ struct spi_imx_data;
 
 struct spi_imx_devtype_data {
 	void (*intctrl)(struct spi_imx_data *, int);
-	int (*config)(struct spi_device *, struct spi_imx_config *);
+	int (*config)(struct spi_device *);
 	void (*trigger)(struct spi_imx_data *);
 	int (*rx_available)(struct spi_imx_data *);
 	void (*reset)(struct spi_imx_data *);
@@ -94,7 +90,8 @@ struct spi_imx_data {
 	unsigned long spi_clk;
 	unsigned int spi_bus_clk;
 
-	unsigned int bytes_per_word;
+	unsigned int speed_hz;
+	unsigned int bits_per_word;
 	unsigned int spi_drctl;
 
 	unsigned int count;
@@ -203,34 +200,27 @@ out:
 	return i;
 }
 
-static int spi_imx_bytes_per_word(const int bpw)
+static int spi_imx_bytes_per_word(const int bits_per_word)
 {
-	return DIV_ROUND_UP(bpw, BITS_PER_BYTE);
+	return DIV_ROUND_UP(bits_per_word, BITS_PER_BYTE);
 }
 
 static bool spi_imx_can_dma(struct spi_master *master, struct spi_device *spi,
 			 struct spi_transfer *transfer)
 {
 	struct spi_imx_data *spi_imx = spi_master_get_devdata(master);
-	unsigned int bpw, i;
+	unsigned int bytes_per_word, i;
 
 	if (!master->dma_rx)
 		return false;
 
-	if (!transfer)
-		return false;
-
-	bpw = transfer->bits_per_word;
-	if (!bpw)
-		bpw = spi->bits_per_word;
-
-	bpw = spi_imx_bytes_per_word(bpw);
+	bytes_per_word = spi_imx_bytes_per_word(transfer->bits_per_word);
 
-	if (bpw != 1 && bpw != 2 && bpw != 4)
+	if (bytes_per_word != 1 && bytes_per_word != 2 && bytes_per_word != 4)
 		return false;
 
 	for (i = spi_imx_get_fifosize(spi_imx) / 2; i > 0; i--) {
-		if (!(transfer->len % (i * bpw)))
+		if (!(transfer->len % (i * bytes_per_word)))
 			break;
 	}
 
@@ -340,12 +330,11 @@ static void mx51_ecspi_trigger(struct spi_imx_data *spi_imx)
 	writel(reg, spi_imx->base + MX51_ECSPI_CTRL);
 }
 
-static int mx51_ecspi_config(struct spi_device *spi,
-			     struct spi_imx_config *config)
+static int mx51_ecspi_config(struct spi_device *spi)
 {
 	struct spi_imx_data *spi_imx = spi_master_get_devdata(spi->master);
 	u32 ctrl = MX51_ECSPI_CTRL_ENABLE;
-	u32 clk = config->speed_hz, delay, reg;
+	u32 clk = spi_imx->speed_hz, delay, reg;
 	u32 cfg = readl(spi_imx->base + MX51_ECSPI_CONFIG);
 
 	/*
@@ -364,13 +353,13 @@ static int mx51_ecspi_config(struct spi_device *spi,
 		ctrl |= MX51_ECSPI_CTRL_DRCTL(spi_imx->spi_drctl);
 
 	/* set clock speed */
-	ctrl |= mx51_ecspi_clkdiv(spi_imx, config->speed_hz, &clk);
+	ctrl |= mx51_ecspi_clkdiv(spi_imx, spi_imx->speed_hz, &clk);
 	spi_imx->spi_bus_clk = clk;
 
 	/* set chip select to use */
 	ctrl |= MX51_ECSPI_CTRL_CS(spi->chip_select);
 
-	ctrl |= (config->bpw - 1) << MX51_ECSPI_CTRL_BL_OFFSET;
+	ctrl |= (spi_imx->bits_per_word - 1) << MX51_ECSPI_CTRL_BL_OFFSET;
 
 	cfg |= MX51_ECSPI_CONFIG_SBBCTRL(spi->chip_select);
 
@@ -501,21 +490,21 @@ static void mx31_trigger(struct spi_imx_data *spi_imx)
 	writel(reg, spi_imx->base + MXC_CSPICTRL);
 }
 
-static int mx31_config(struct spi_device *spi, struct spi_imx_config *config)
+static int mx31_config(struct spi_device *spi)
 {
 	struct spi_imx_data *spi_imx = spi_master_get_devdata(spi->master);
 	unsigned int reg = MX31_CSPICTRL_ENABLE | MX31_CSPICTRL_MASTER;
 	unsigned int clk;
 
-	reg |= spi_imx_clkdiv_2(spi_imx->spi_clk, config->speed_hz, &clk) <<
+	reg |= spi_imx_clkdiv_2(spi_imx->spi_clk, spi_imx->speed_hz, &clk) <<
 		MX31_CSPICTRL_DR_SHIFT;
 	spi_imx->spi_bus_clk = clk;
 
 	if (is_imx35_cspi(spi_imx)) {
-		reg |= (config->bpw - 1) << MX35_CSPICTRL_BL_SHIFT;
+		reg |= (spi_imx->bits_per_word - 1) << MX35_CSPICTRL_BL_SHIFT;
 		reg |= MX31_CSPICTRL_SSCTL;
 	} else {
-		reg |= (config->bpw - 1) << MX31_CSPICTRL_BC_SHIFT;
+		reg |= (spi_imx->bits_per_word - 1) << MX31_CSPICTRL_BC_SHIFT;
 	}
 
 	if (spi->mode & SPI_CPHA)
@@ -597,18 +586,18 @@ static void mx21_trigger(struct spi_imx_data *spi_imx)
 	writel(reg, spi_imx->base + MXC_CSPICTRL);
 }
 
-static int mx21_config(struct spi_device *spi, struct spi_imx_config *config)
+static int mx21_config(struct spi_device *spi)
 {
 	struct spi_imx_data *spi_imx = spi_master_get_devdata(spi->master);
 	unsigned int reg = MX21_CSPICTRL_ENABLE | MX21_CSPICTRL_MASTER;
 	unsigned int max = is_imx27_cspi(spi_imx) ? 16 : 18;
 	unsigned int clk;
 
-	reg |= spi_imx_clkdiv_1(spi_imx->spi_clk, config->speed_hz, max, &clk)
+	reg |= spi_imx_clkdiv_1(spi_imx->spi_clk, spi_imx->speed_hz, max, &clk)
 		<< MX21_CSPICTRL_DR_SHIFT;
 	spi_imx->spi_bus_clk = clk;
 
-	reg |= config->bpw - 1;
+	reg |= spi_imx->bits_per_word - 1;
 
 	if (spi->mode & SPI_CPHA)
 		reg |= MX21_CSPICTRL_PHA;
@@ -666,17 +655,17 @@ static void mx1_trigger(struct spi_imx_data *spi_imx)
 	writel(reg, spi_imx->base + MXC_CSPICTRL);
 }
 
-static int mx1_config(struct spi_device *spi, struct spi_imx_config *config)
+static int mx1_config(struct spi_device *spi)
 {
 	struct spi_imx_data *spi_imx = spi_master_get_devdata(spi->master);
 	unsigned int reg = MX1_CSPICTRL_ENABLE | MX1_CSPICTRL_MASTER;
 	unsigned int clk;
 
-	reg |= spi_imx_clkdiv_2(spi_imx->spi_clk, config->speed_hz, &clk) <<
+	reg |= spi_imx_clkdiv_2(spi_imx->spi_clk, spi_imx->speed_hz, &clk) <<
 		MX1_CSPICTRL_DR_SHIFT;
 	spi_imx->spi_bus_clk = clk;
 
-	reg |= config->bpw - 1;
+	reg |= spi_imx->bits_per_word - 1;
 
 	if (spi->mode & SPI_CPHA)
 		reg |= MX1_CSPICTRL_PHA;
@@ -841,15 +830,14 @@ static irqreturn_t spi_imx_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int spi_imx_dma_configure(struct spi_master *master,
-				 int bytes_per_word)
+static int spi_imx_dma_configure(struct spi_master *master)
 {
 	int ret;
 	enum dma_slave_buswidth buswidth;
 	struct dma_slave_config rx = {}, tx = {};
 	struct spi_imx_data *spi_imx = spi_master_get_devdata(master);
 
-	switch (bytes_per_word) {
+	switch (spi_imx_bytes_per_word(spi_imx->bits_per_word)) {
 	case 4:
 		buswidth = DMA_SLAVE_BUSWIDTH_4_BYTES;
 		break;
@@ -883,8 +871,6 @@ static int spi_imx_dma_configure(struct spi_master *master,
 		return ret;
 	}
 
-	spi_imx->bytes_per_word = bytes_per_word;
-
 	return 0;
 }
 
@@ -892,22 +878,19 @@ static int spi_imx_setupxfer(struct spi_device *spi,
 				 struct spi_transfer *t)
 {
 	struct spi_imx_data *spi_imx = spi_master_get_devdata(spi->master);
-	struct spi_imx_config config;
 	int ret;
 
-	config.bpw = t ? t->bits_per_word : spi->bits_per_word;
-	config.speed_hz  = t ? t->speed_hz : spi->max_speed_hz;
+	if (!t)
+		return 0;
 
-	if (!config.speed_hz)
-		config.speed_hz = spi->max_speed_hz;
-	if (!config.bpw)
-		config.bpw = spi->bits_per_word;
+	spi_imx->bits_per_word = t->bits_per_word;
+	spi_imx->speed_hz  = t->speed_hz;
 
 	/* Initialize the functions for transfer */
-	if (config.bpw <= 8) {
+	if (spi_imx->bits_per_word <= 8) {
 		spi_imx->rx = spi_imx_buf_rx_u8;
 		spi_imx->tx = spi_imx_buf_tx_u8;
-	} else if (config.bpw <= 16) {
+	} else if (spi_imx->bits_per_word <= 16) {
 		spi_imx->rx = spi_imx_buf_rx_u16;
 		spi_imx->tx = spi_imx_buf_tx_u16;
 	} else {
@@ -921,13 +904,12 @@ static int spi_imx_setupxfer(struct spi_device *spi,
 		spi_imx->usedma = 0;
 
 	if (spi_imx->usedma) {
-		ret = spi_imx_dma_configure(spi->master,
-					    spi_imx_bytes_per_word(config.bpw));
+		ret = spi_imx_dma_configure(spi->master);
 		if (ret)
 			return ret;
 	}
 
-	spi_imx->devtype_data->config(spi, &config);
+	spi_imx->devtype_data->config(spi);
 
 	return 0;
 }
@@ -976,8 +958,6 @@ static int spi_imx_sdma_init(struct device *dev, struct spi_imx_data *spi_imx,
 		goto err;
 	}
 
-	spi_imx_dma_configure(master, 1);
-
 	init_completion(&spi_imx->dma_rx_completion);
 	init_completion(&spi_imx->dma_tx_completion);
 	master->can_dma = spi_imx_can_dma;
@@ -1189,15 +1169,15 @@ static int spi_imx_probe(struct platform_device *pdev)
 	}
 
 	master = spi_alloc_master(&pdev->dev, sizeof(struct spi_imx_data));
+	if (!master)
+		return -ENOMEM;
+
 	ret = of_property_read_u32(np, "fsl,spi-rdy-drctl", &spi_drctl);
 	if ((ret < 0) || (spi_drctl >= 0x3)) {
 		/* '11' is reserved */
 		spi_drctl = 0;
 	}
 
-	if (!master)
-		return -ENOMEM;
-
 	platform_set_drvdata(pdev, master);
 
 	master->bits_per_word_mask = SPI_BPW_RANGE_MASK(1, 32);
diff --git a/drivers/spi/spi-loopback-test.c b/drivers/spi/spi-loopback-test.c
index f4875f177df0..3459965004f8 100644
--- a/drivers/spi/spi-loopback-test.c
+++ b/drivers/spi/spi-loopback-test.c
@@ -894,7 +894,7 @@ int spi_test_execute_msg(struct spi_device *spi, struct spi_test *test,
 		test->elapsed_time = ktime_to_ns(ktime_sub(ktime_get(), start));
 		if (ret == -ETIMEDOUT) {
 			dev_info(&spi->dev,
-				 "spi-message timed out - reruning...\n");
+				 "spi-message timed out - rerunning...\n");
 			/* rerun after a few explicit schedules */
 			for (i = 0; i < 16; i++)
 				schedule();
@@ -1021,10 +1021,9 @@ int spi_test_run_tests(struct spi_device *spi,
 		rx = vmalloc(SPI_TEST_MAX_SIZE_PLUS);
 	else
 		rx = kzalloc(SPI_TEST_MAX_SIZE_PLUS, GFP_KERNEL);
-	if (!rx) {
-		ret = -ENOMEM;
-		goto out;
-	}
+	if (!rx)
+		return -ENOMEM;
+
 
 	if (use_vmalloc)
 		tx = vmalloc(SPI_TEST_MAX_SIZE_PLUS);
@@ -1032,7 +1031,7 @@ int spi_test_run_tests(struct spi_device *spi,
 		tx = kzalloc(SPI_TEST_MAX_SIZE_PLUS, GFP_KERNEL);
 	if (!tx) {
 		ret = -ENOMEM;
-		goto out;
+		goto err_tx;
 	}
 
 	/* now run the individual tests in the table */
@@ -1057,8 +1056,9 @@ int spi_test_run_tests(struct spi_device *spi,
 	}
 
 out:
-	kvfree(rx);
 	kvfree(tx);
+err_tx:
+	kvfree(rx);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(spi_test_run_tests);
diff --git a/drivers/spi/spi-meson-spicc.c b/drivers/spi/spi-meson-spicc.c
new file mode 100644
index 000000000000..7f8429635502
--- /dev/null
+++ b/drivers/spi/spi-meson-spicc.c
@@ -0,0 +1,619 @@
+/*
+ * Driver for Amlogic Meson SPI communication controller (SPICC)
+ *
+ * Copyright (C) BayLibre, SAS
+ * Author: Neil Armstrong <narmstrong@baylibre.com>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+
+#include <linux/bitfield.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/reset.h>
+#include <linux/gpio.h>
+
+/*
+ * The Meson SPICC controller could support DMA based transfers, but is not
+ * implemented by the vendor code, and while having the registers documentation
+ * it has never worked on the GXL Hardware.
+ * The PIO mode is the only mode implemented, and due to badly designed HW :
+ * - all transfers are cutted in 16 words burst because the FIFO hangs on
+ *   TX underflow, and there is no TX "Half-Empty" interrupt, so we go by
+ *   FIFO max size chunk only
+ * - CS management is dumb, and goes UP between every burst, so is really a
+ *   "Data Valid" signal than a Chip Select, GPIO link should be used instead
+ *   to have a CS go down over the full transfer
+ */
+
+#define SPICC_MAX_FREQ	30000000
+#define SPICC_MAX_BURST	128
+
+/* Register Map */
+#define SPICC_RXDATA	0x00
+
+#define SPICC_TXDATA	0x04
+
+#define SPICC_CONREG	0x08
+#define SPICC_ENABLE		BIT(0)
+#define SPICC_MODE_MASTER	BIT(1)
+#define SPICC_XCH		BIT(2)
+#define SPICC_SMC		BIT(3)
+#define SPICC_POL		BIT(4)
+#define SPICC_PHA		BIT(5)
+#define SPICC_SSCTL		BIT(6)
+#define SPICC_SSPOL		BIT(7)
+#define SPICC_DRCTL_MASK	GENMASK(9, 8)
+#define SPICC_DRCTL_IGNORE	0
+#define SPICC_DRCTL_FALLING	1
+#define SPICC_DRCTL_LOWLEVEL	2
+#define SPICC_CS_MASK		GENMASK(13, 12)
+#define SPICC_DATARATE_MASK	GENMASK(18, 16)
+#define SPICC_DATARATE_DIV4	0
+#define SPICC_DATARATE_DIV8	1
+#define SPICC_DATARATE_DIV16	2
+#define SPICC_DATARATE_DIV32	3
+#define SPICC_BITLENGTH_MASK	GENMASK(24, 19)
+#define SPICC_BURSTLENGTH_MASK	GENMASK(31, 25)
+
+#define SPICC_INTREG	0x0c
+#define SPICC_TE_EN	BIT(0) /* TX FIFO Empty Interrupt */
+#define SPICC_TH_EN	BIT(1) /* TX FIFO Half-Full Interrupt */
+#define SPICC_TF_EN	BIT(2) /* TX FIFO Full Interrupt */
+#define SPICC_RR_EN	BIT(3) /* RX FIFO Ready Interrupt */
+#define SPICC_RH_EN	BIT(4) /* RX FIFO Half-Full Interrupt */
+#define SPICC_RF_EN	BIT(5) /* RX FIFO Full Interrupt */
+#define SPICC_RO_EN	BIT(6) /* RX FIFO Overflow Interrupt */
+#define SPICC_TC_EN	BIT(7) /* Transfert Complete Interrupt */
+
+#define SPICC_DMAREG	0x10
+#define SPICC_DMA_ENABLE		BIT(0)
+#define SPICC_TXFIFO_THRESHOLD_MASK	GENMASK(5, 1)
+#define SPICC_RXFIFO_THRESHOLD_MASK	GENMASK(10, 6)
+#define SPICC_READ_BURST_MASK		GENMASK(14, 11)
+#define SPICC_WRITE_BURST_MASK		GENMASK(18, 15)
+#define SPICC_DMA_URGENT		BIT(19)
+#define SPICC_DMA_THREADID_MASK		GENMASK(25, 20)
+#define SPICC_DMA_BURSTNUM_MASK		GENMASK(31, 26)
+
+#define SPICC_STATREG	0x14
+#define SPICC_TE	BIT(0) /* TX FIFO Empty Interrupt */
+#define SPICC_TH	BIT(1) /* TX FIFO Half-Full Interrupt */
+#define SPICC_TF	BIT(2) /* TX FIFO Full Interrupt */
+#define SPICC_RR	BIT(3) /* RX FIFO Ready Interrupt */
+#define SPICC_RH	BIT(4) /* RX FIFO Half-Full Interrupt */
+#define SPICC_RF	BIT(5) /* RX FIFO Full Interrupt */
+#define SPICC_RO	BIT(6) /* RX FIFO Overflow Interrupt */
+#define SPICC_TC	BIT(7) /* Transfert Complete Interrupt */
+
+#define SPICC_PERIODREG	0x18
+#define SPICC_PERIOD	GENMASK(14, 0)	/* Wait cycles */
+
+#define SPICC_TESTREG	0x1c
+#define SPICC_TXCNT_MASK	GENMASK(4, 0)	/* TX FIFO Counter */
+#define SPICC_RXCNT_MASK	GENMASK(9, 5)	/* RX FIFO Counter */
+#define SPICC_SMSTATUS_MASK	GENMASK(12, 10)	/* State Machine Status */
+#define SPICC_LBC_RO		BIT(13)	/* Loop Back Control Read-Only */
+#define SPICC_LBC_W1		BIT(14) /* Loop Back Control Write-Only */
+#define SPICC_SWAP_RO		BIT(14) /* RX FIFO Data Swap Read-Only */
+#define SPICC_SWAP_W1		BIT(15) /* RX FIFO Data Swap Write-Only */
+#define SPICC_DLYCTL_RO_MASK	GENMASK(20, 15) /* Delay Control Read-Only */
+#define SPICC_DLYCTL_W1_MASK	GENMASK(21, 16) /* Delay Control Write-Only */
+#define SPICC_FIFORST_RO_MASK	GENMASK(22, 21) /* FIFO Softreset Read-Only */
+#define SPICC_FIFORST_W1_MASK	GENMASK(23, 22) /* FIFO Softreset Write-Only */
+
+#define SPICC_DRADDR	0x20	/* Read Address of DMA */
+
+#define SPICC_DWADDR	0x24	/* Write Address of DMA */
+
+#define writel_bits_relaxed(mask, val, addr) \
+	writel_relaxed((readl_relaxed(addr) & ~(mask)) | (val), addr)
+
+#define SPICC_BURST_MAX	16
+#define SPICC_FIFO_HALF 10
+
+struct meson_spicc_device {
+	struct spi_master		*master;
+	struct platform_device		*pdev;
+	void __iomem			*base;
+	struct clk			*core;
+	struct spi_message		*message;
+	struct spi_transfer		*xfer;
+	u8				*tx_buf;
+	u8				*rx_buf;
+	unsigned int			bytes_per_word;
+	unsigned long			tx_remain;
+	unsigned long			txb_remain;
+	unsigned long			rx_remain;
+	unsigned long			rxb_remain;
+	unsigned long			xfer_remain;
+	bool				is_burst_end;
+	bool				is_last_burst;
+};
+
+static inline bool meson_spicc_txfull(struct meson_spicc_device *spicc)
+{
+	return !!FIELD_GET(SPICC_TF,
+			   readl_relaxed(spicc->base + SPICC_STATREG));
+}
+
+static inline bool meson_spicc_rxready(struct meson_spicc_device *spicc)
+{
+	return FIELD_GET(SPICC_RH | SPICC_RR | SPICC_RF_EN,
+			 readl_relaxed(spicc->base + SPICC_STATREG));
+}
+
+static inline u32 meson_spicc_pull_data(struct meson_spicc_device *spicc)
+{
+	unsigned int bytes = spicc->bytes_per_word;
+	unsigned int byte_shift = 0;
+	u32 data = 0;
+	u8 byte;
+
+	while (bytes--) {
+		byte = *spicc->tx_buf++;
+		data |= (byte & 0xff) << byte_shift;
+		byte_shift += 8;
+	}
+
+	spicc->tx_remain--;
+	return data;
+}
+
+static inline void meson_spicc_push_data(struct meson_spicc_device *spicc,
+					 u32 data)
+{
+	unsigned int bytes = spicc->bytes_per_word;
+	unsigned int byte_shift = 0;
+	u8 byte;
+
+	while (bytes--) {
+		byte = (data >> byte_shift) & 0xff;
+		*spicc->rx_buf++ = byte;
+		byte_shift += 8;
+	}
+
+	spicc->rx_remain--;
+}
+
+static inline void meson_spicc_rx(struct meson_spicc_device *spicc)
+{
+	/* Empty RX FIFO */
+	while (spicc->rx_remain &&
+	       meson_spicc_rxready(spicc))
+		meson_spicc_push_data(spicc,
+				readl_relaxed(spicc->base + SPICC_RXDATA));
+}
+
+static inline void meson_spicc_tx(struct meson_spicc_device *spicc)
+{
+	/* Fill Up TX FIFO */
+	while (spicc->tx_remain &&
+	       !meson_spicc_txfull(spicc))
+		writel_relaxed(meson_spicc_pull_data(spicc),
+			       spicc->base + SPICC_TXDATA);
+}
+
+static inline u32 meson_spicc_setup_rx_irq(struct meson_spicc_device *spicc,
+					   u32 irq_ctrl)
+{
+	if (spicc->rx_remain > SPICC_FIFO_HALF)
+		irq_ctrl |= SPICC_RH_EN;
+	else
+		irq_ctrl |= SPICC_RR_EN;
+
+	return irq_ctrl;
+}
+
+static inline void meson_spicc_setup_burst(struct meson_spicc_device *spicc,
+					   unsigned int burst_len)
+{
+	/* Setup Xfer variables */
+	spicc->tx_remain = burst_len;
+	spicc->rx_remain = burst_len;
+	spicc->xfer_remain -= burst_len * spicc->bytes_per_word;
+	spicc->is_burst_end = false;
+	if (burst_len < SPICC_BURST_MAX || !spicc->xfer_remain)
+		spicc->is_last_burst = true;
+	else
+		spicc->is_last_burst = false;
+
+	/* Setup burst length */
+	writel_bits_relaxed(SPICC_BURSTLENGTH_MASK,
+			FIELD_PREP(SPICC_BURSTLENGTH_MASK,
+				burst_len),
+			spicc->base + SPICC_CONREG);
+
+	/* Fill TX FIFO */
+	meson_spicc_tx(spicc);
+}
+
+static irqreturn_t meson_spicc_irq(int irq, void *data)
+{
+	struct meson_spicc_device *spicc = (void *) data;
+	u32 ctrl = readl_relaxed(spicc->base + SPICC_INTREG);
+	u32 stat = readl_relaxed(spicc->base + SPICC_STATREG) & ctrl;
+
+	ctrl &= ~(SPICC_RH_EN | SPICC_RR_EN);
+
+	/* Empty RX FIFO */
+	meson_spicc_rx(spicc);
+
+	/* Enable TC interrupt since we transferred everything */
+	if (!spicc->tx_remain && !spicc->rx_remain) {
+		spicc->is_burst_end = true;
+
+		/* Enable TC interrupt */
+		ctrl |= SPICC_TC_EN;
+
+		/* Reload IRQ status */
+		stat = readl_relaxed(spicc->base + SPICC_STATREG) & ctrl;
+	}
+
+	/* Check transfer complete */
+	if ((stat & SPICC_TC) && spicc->is_burst_end) {
+		unsigned int burst_len;
+
+		/* Clear TC bit */
+		writel_relaxed(SPICC_TC, spicc->base + SPICC_STATREG);
+
+		/* Disable TC interrupt */
+		ctrl &= ~SPICC_TC_EN;
+
+		if (spicc->is_last_burst) {
+			/* Disable all IRQs */
+			writel(0, spicc->base + SPICC_INTREG);
+
+			spi_finalize_current_transfer(spicc->master);
+
+			return IRQ_HANDLED;
+		}
+
+		burst_len = min_t(unsigned int,
+				  spicc->xfer_remain / spicc->bytes_per_word,
+				  SPICC_BURST_MAX);
+
+		/* Setup burst */
+		meson_spicc_setup_burst(spicc, burst_len);
+
+		/* Restart burst */
+		writel_bits_relaxed(SPICC_XCH, SPICC_XCH,
+				    spicc->base + SPICC_CONREG);
+	}
+
+	/* Setup RX interrupt trigger */
+	ctrl = meson_spicc_setup_rx_irq(spicc, ctrl);
+
+	/* Reconfigure interrupts */
+	writel(ctrl, spicc->base + SPICC_INTREG);
+
+	return IRQ_HANDLED;
+}
+
+static u32 meson_spicc_setup_speed(struct meson_spicc_device *spicc, u32 conf,
+				   u32 speed)
+{
+	unsigned long parent, value;
+	unsigned int i, div;
+
+	parent = clk_get_rate(spicc->core);
+
+	/* Find closest inferior/equal possible speed */
+	for (i = 0 ; i < 7 ; ++i) {
+		/* 2^(data_rate+2) */
+		value = parent >> (i + 2);
+
+		if (value <= speed)
+			break;
+	}
+
+	/* If provided speed it lower than max divider, use max divider */
+	if (i > 7) {
+		div = 7;
+		dev_warn_once(&spicc->pdev->dev, "unable to get close to speed %u\n",
+			      speed);
+	} else
+		div = i;
+
+	dev_dbg(&spicc->pdev->dev, "parent %lu, speed %u -> %lu (%u)\n",
+		parent, speed, value, div);
+
+	conf &= ~SPICC_DATARATE_MASK;
+	conf |= FIELD_PREP(SPICC_DATARATE_MASK, div);
+
+	return conf;
+}
+
+static void meson_spicc_setup_xfer(struct meson_spicc_device *spicc,
+				   struct spi_transfer *xfer)
+{
+	u32 conf, conf_orig;
+
+	/* Read original configuration */
+	conf = conf_orig = readl_relaxed(spicc->base + SPICC_CONREG);
+
+	/* Select closest divider */
+	conf = meson_spicc_setup_speed(spicc, conf, xfer->speed_hz);
+
+	/* Setup word width */
+	conf &= ~SPICC_BITLENGTH_MASK;
+	conf |= FIELD_PREP(SPICC_BITLENGTH_MASK,
+			   (spicc->bytes_per_word << 3) - 1);
+
+	/* Ignore if unchanged */
+	if (conf != conf_orig)
+		writel_relaxed(conf, spicc->base + SPICC_CONREG);
+}
+
+static int meson_spicc_transfer_one(struct spi_master *master,
+				    struct spi_device *spi,
+				    struct spi_transfer *xfer)
+{
+	struct meson_spicc_device *spicc = spi_master_get_devdata(master);
+	unsigned int burst_len;
+	u32 irq = 0;
+
+	/* Store current transfer */
+	spicc->xfer = xfer;
+
+	/* Setup transfer parameters */
+	spicc->tx_buf = (u8 *)xfer->tx_buf;
+	spicc->rx_buf = (u8 *)xfer->rx_buf;
+	spicc->xfer_remain = xfer->len;
+
+	/* Pre-calculate word size */
+	spicc->bytes_per_word =
+	   DIV_ROUND_UP(spicc->xfer->bits_per_word, 8);
+
+	/* Setup transfer parameters */
+	meson_spicc_setup_xfer(spicc, xfer);
+
+	burst_len = min_t(unsigned int,
+			  spicc->xfer_remain / spicc->bytes_per_word,
+			  SPICC_BURST_MAX);
+
+	meson_spicc_setup_burst(spicc, burst_len);
+
+	irq = meson_spicc_setup_rx_irq(spicc, irq);
+
+	/* Start burst */
+	writel_bits_relaxed(SPICC_XCH, SPICC_XCH, spicc->base + SPICC_CONREG);
+
+	/* Enable interrupts */
+	writel_relaxed(irq, spicc->base + SPICC_INTREG);
+
+	return 1;
+}
+
+static int meson_spicc_prepare_message(struct spi_master *master,
+				       struct spi_message *message)
+{
+	struct meson_spicc_device *spicc = spi_master_get_devdata(master);
+	struct spi_device *spi = message->spi;
+	u32 conf = 0;
+
+	/* Store current message */
+	spicc->message = message;
+
+	/* Enable Master */
+	conf |= SPICC_ENABLE;
+	conf |= SPICC_MODE_MASTER;
+
+	/* SMC = 0 */
+
+	/* Setup transfer mode */
+	if (spi->mode & SPI_CPOL)
+		conf |= SPICC_POL;
+	else
+		conf &= ~SPICC_POL;
+
+	if (spi->mode & SPI_CPHA)
+		conf |= SPICC_PHA;
+	else
+		conf &= ~SPICC_PHA;
+
+	/* SSCTL = 0 */
+
+	if (spi->mode & SPI_CS_HIGH)
+		conf |= SPICC_SSPOL;
+	else
+		conf &= ~SPICC_SSPOL;
+
+	if (spi->mode & SPI_READY)
+		conf |= FIELD_PREP(SPICC_DRCTL_MASK, SPICC_DRCTL_LOWLEVEL);
+	else
+		conf |= FIELD_PREP(SPICC_DRCTL_MASK, SPICC_DRCTL_IGNORE);
+
+	/* Select CS */
+	conf |= FIELD_PREP(SPICC_CS_MASK, spi->chip_select);
+
+	/* Default Clock rate core/4 */
+
+	/* Default 8bit word */
+	conf |= FIELD_PREP(SPICC_BITLENGTH_MASK, 8 - 1);
+
+	writel_relaxed(conf, spicc->base + SPICC_CONREG);
+
+	/* Setup no wait cycles by default */
+	writel_relaxed(0, spicc->base + SPICC_PERIODREG);
+
+	writel_bits_relaxed(BIT(24), BIT(24), spicc->base + SPICC_TESTREG);
+
+	return 0;
+}
+
+static int meson_spicc_unprepare_transfer(struct spi_master *master)
+{
+	struct meson_spicc_device *spicc = spi_master_get_devdata(master);
+
+	/* Disable all IRQs */
+	writel(0, spicc->base + SPICC_INTREG);
+
+	/* Disable controller */
+	writel_bits_relaxed(SPICC_ENABLE, 0, spicc->base + SPICC_CONREG);
+
+	device_reset_optional(&spicc->pdev->dev);
+
+	return 0;
+}
+
+static int meson_spicc_setup(struct spi_device *spi)
+{
+	int ret = 0;
+
+	if (!spi->controller_state)
+		spi->controller_state = spi_master_get_devdata(spi->master);
+	else if (gpio_is_valid(spi->cs_gpio))
+		goto out_gpio;
+	else if (spi->cs_gpio == -ENOENT)
+		return 0;
+
+	if (gpio_is_valid(spi->cs_gpio)) {
+		ret = gpio_request(spi->cs_gpio, dev_name(&spi->dev));
+		if (ret) {
+			dev_err(&spi->dev, "failed to request cs gpio\n");
+			return ret;
+		}
+	}
+
+out_gpio:
+	ret = gpio_direction_output(spi->cs_gpio,
+			!(spi->mode & SPI_CS_HIGH));
+
+	return ret;
+}
+
+static void meson_spicc_cleanup(struct spi_device *spi)
+{
+	if (gpio_is_valid(spi->cs_gpio))
+		gpio_free(spi->cs_gpio);
+
+	spi->controller_state = NULL;
+}
+
+static int meson_spicc_probe(struct platform_device *pdev)
+{
+	struct spi_master *master;
+	struct meson_spicc_device *spicc;
+	struct resource *res;
+	int ret, irq, rate;
+
+	master = spi_alloc_master(&pdev->dev, sizeof(*spicc));
+	if (!master) {
+		dev_err(&pdev->dev, "master allocation failed\n");
+		return -ENOMEM;
+	}
+	spicc = spi_master_get_devdata(master);
+	spicc->master = master;
+
+	spicc->pdev = pdev;
+	platform_set_drvdata(pdev, spicc);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	spicc->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(spicc->base)) {
+		dev_err(&pdev->dev, "io resource mapping failed\n");
+		ret = PTR_ERR(spicc->base);
+		goto out_master;
+	}
+
+	/* Disable all IRQs */
+	writel_relaxed(0, spicc->base + SPICC_INTREG);
+
+	irq = platform_get_irq(pdev, 0);
+	ret = devm_request_irq(&pdev->dev, irq, meson_spicc_irq,
+			       0, NULL, spicc);
+	if (ret) {
+		dev_err(&pdev->dev, "irq request failed\n");
+		goto out_master;
+	}
+
+	spicc->core = devm_clk_get(&pdev->dev, "core");
+	if (IS_ERR(spicc->core)) {
+		dev_err(&pdev->dev, "core clock request failed\n");
+		ret = PTR_ERR(spicc->core);
+		goto out_master;
+	}
+
+	ret = clk_prepare_enable(spicc->core);
+	if (ret) {
+		dev_err(&pdev->dev, "core clock enable failed\n");
+		goto out_master;
+	}
+	rate = clk_get_rate(spicc->core);
+
+	device_reset_optional(&pdev->dev);
+
+	master->num_chipselect = 4;
+	master->dev.of_node = pdev->dev.of_node;
+	master->mode_bits = SPI_CPHA | SPI_CPOL | SPI_CS_HIGH;
+	master->bits_per_word_mask = SPI_BPW_MASK(32) |
+				     SPI_BPW_MASK(24) |
+				     SPI_BPW_MASK(16) |
+				     SPI_BPW_MASK(8);
+	master->flags = (SPI_MASTER_MUST_RX | SPI_MASTER_MUST_TX);
+	master->min_speed_hz = rate >> 9;
+	master->setup = meson_spicc_setup;
+	master->cleanup = meson_spicc_cleanup;
+	master->prepare_message = meson_spicc_prepare_message;
+	master->unprepare_transfer_hardware = meson_spicc_unprepare_transfer;
+	master->transfer_one = meson_spicc_transfer_one;
+
+	/* Setup max rate according to the Meson GX datasheet */
+	if ((rate >> 2) > SPICC_MAX_FREQ)
+		master->max_speed_hz = SPICC_MAX_FREQ;
+	else
+		master->max_speed_hz = rate >> 2;
+
+	ret = devm_spi_register_master(&pdev->dev, master);
+	if (!ret)
+		return 0;
+
+	dev_err(&pdev->dev, "spi master registration failed\n");
+
+out_master:
+	spi_master_put(master);
+
+	return ret;
+}
+
+static int meson_spicc_remove(struct platform_device *pdev)
+{
+	struct meson_spicc_device *spicc = platform_get_drvdata(pdev);
+
+	/* Disable SPI */
+	writel(0, spicc->base + SPICC_CONREG);
+
+	clk_disable_unprepare(spicc->core);
+
+	return 0;
+}
+
+static const struct of_device_id meson_spicc_of_match[] = {
+	{ .compatible = "amlogic,meson-gx-spicc", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, meson_spicc_of_match);
+
+static struct platform_driver meson_spicc_driver = {
+	.probe   = meson_spicc_probe,
+	.remove  = meson_spicc_remove,
+	.driver  = {
+		.name = "meson-spicc",
+		.of_match_table = of_match_ptr(meson_spicc_of_match),
+	},
+};
+
+module_platform_driver(meson_spicc_driver);
+
+MODULE_DESCRIPTION("Meson SPI Communication Controller driver");
+MODULE_AUTHOR("Neil Armstrong <narmstrong@baylibre.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/spi/spi-mt65xx.c b/drivers/spi/spi-mt65xx.c
index 278867a31950..86bf45667a04 100644
--- a/drivers/spi/spi-mt65xx.c
+++ b/drivers/spi/spi-mt65xx.c
@@ -35,11 +35,15 @@
 #define SPI_CMD_REG                       0x0018
 #define SPI_STATUS0_REG                   0x001c
 #define SPI_PAD_SEL_REG                   0x0024
+#define SPI_CFG2_REG                      0x0028
 
 #define SPI_CFG0_SCK_HIGH_OFFSET          0
 #define SPI_CFG0_SCK_LOW_OFFSET           8
 #define SPI_CFG0_CS_HOLD_OFFSET           16
 #define SPI_CFG0_CS_SETUP_OFFSET          24
+#define SPI_ADJUST_CFG0_SCK_LOW_OFFSET    16
+#define SPI_ADJUST_CFG0_CS_HOLD_OFFSET    0
+#define SPI_ADJUST_CFG0_CS_SETUP_OFFSET   16
 
 #define SPI_CFG1_CS_IDLE_OFFSET           0
 #define SPI_CFG1_PACKET_LOOP_OFFSET       8
@@ -55,6 +59,8 @@
 #define SPI_CMD_RST                  BIT(2)
 #define SPI_CMD_PAUSE_EN             BIT(4)
 #define SPI_CMD_DEASSERT             BIT(5)
+#define SPI_CMD_SAMPLE_SEL           BIT(6)
+#define SPI_CMD_CS_POL               BIT(7)
 #define SPI_CMD_CPHA                 BIT(8)
 #define SPI_CMD_CPOL                 BIT(9)
 #define SPI_CMD_RX_DMA               BIT(10)
@@ -80,6 +86,8 @@ struct mtk_spi_compatible {
 	bool need_pad_sel;
 	/* Must explicitly send dummy Tx bytes to do Rx only transfer */
 	bool must_tx;
+	/* some IC design adjust cfg register to enhance time accuracy */
+	bool enhance_timing;
 };
 
 struct mtk_spi {
@@ -96,6 +104,16 @@ struct mtk_spi {
 };
 
 static const struct mtk_spi_compatible mtk_common_compat;
+
+static const struct mtk_spi_compatible mt2712_compat = {
+	.must_tx = true,
+};
+
+static const struct mtk_spi_compatible mt7622_compat = {
+	.must_tx = true,
+	.enhance_timing = true,
+};
+
 static const struct mtk_spi_compatible mt8173_compat = {
 	.need_pad_sel = true,
 	.must_tx = true,
@@ -108,15 +126,23 @@ static const struct mtk_spi_compatible mt8173_compat = {
 static const struct mtk_chip_config mtk_default_chip_info = {
 	.rx_mlsb = 1,
 	.tx_mlsb = 1,
+	.cs_pol = 0,
+	.sample_sel = 0,
 };
 
 static const struct of_device_id mtk_spi_of_match[] = {
 	{ .compatible = "mediatek,mt2701-spi",
 		.data = (void *)&mtk_common_compat,
 	},
+	{ .compatible = "mediatek,mt2712-spi",
+		.data = (void *)&mt2712_compat,
+	},
 	{ .compatible = "mediatek,mt6589-spi",
 		.data = (void *)&mtk_common_compat,
 	},
+	{ .compatible = "mediatek,mt7622-spi",
+		.data = (void *)&mt7622_compat,
+	},
 	{ .compatible = "mediatek,mt8135-spi",
 		.data = (void *)&mtk_common_compat,
 	},
@@ -182,6 +208,17 @@ static int mtk_spi_prepare_message(struct spi_master *master,
 	reg_val |= SPI_CMD_RX_ENDIAN;
 #endif
 
+	if (mdata->dev_comp->enhance_timing) {
+		if (chip_config->cs_pol)
+			reg_val |= SPI_CMD_CS_POL;
+		else
+			reg_val &= ~SPI_CMD_CS_POL;
+		if (chip_config->sample_sel)
+			reg_val |= SPI_CMD_SAMPLE_SEL;
+		else
+			reg_val &= ~SPI_CMD_SAMPLE_SEL;
+	}
+
 	/* set finish and pause interrupt always enable */
 	reg_val |= SPI_CMD_FINISH_IE | SPI_CMD_PAUSE_IE;
 
@@ -233,11 +270,25 @@ static void mtk_spi_prepare_transfer(struct spi_master *master,
 	sck_time = (div + 1) / 2;
 	cs_time = sck_time * 2;
 
-	reg_val |= (((sck_time - 1) & 0xff) << SPI_CFG0_SCK_HIGH_OFFSET);
-	reg_val |= (((sck_time - 1) & 0xff) << SPI_CFG0_SCK_LOW_OFFSET);
-	reg_val |= (((cs_time - 1) & 0xff) << SPI_CFG0_CS_HOLD_OFFSET);
-	reg_val |= (((cs_time - 1) & 0xff) << SPI_CFG0_CS_SETUP_OFFSET);
-	writel(reg_val, mdata->base + SPI_CFG0_REG);
+	if (mdata->dev_comp->enhance_timing) {
+		reg_val |= (((sck_time - 1) & 0xffff)
+			   << SPI_CFG0_SCK_HIGH_OFFSET);
+		reg_val |= (((sck_time - 1) & 0xffff)
+			   << SPI_ADJUST_CFG0_SCK_LOW_OFFSET);
+		writel(reg_val, mdata->base + SPI_CFG2_REG);
+		reg_val |= (((cs_time - 1) & 0xffff)
+			   << SPI_ADJUST_CFG0_CS_HOLD_OFFSET);
+		reg_val |= (((cs_time - 1) & 0xffff)
+			   << SPI_ADJUST_CFG0_CS_SETUP_OFFSET);
+		writel(reg_val, mdata->base + SPI_CFG0_REG);
+	} else {
+		reg_val |= (((sck_time - 1) & 0xff)
+			   << SPI_CFG0_SCK_HIGH_OFFSET);
+		reg_val |= (((sck_time - 1) & 0xff) << SPI_CFG0_SCK_LOW_OFFSET);
+		reg_val |= (((cs_time - 1) & 0xff) << SPI_CFG0_CS_HOLD_OFFSET);
+		reg_val |= (((cs_time - 1) & 0xff) << SPI_CFG0_CS_SETUP_OFFSET);
+		writel(reg_val, mdata->base + SPI_CFG0_REG);
+	}
 
 	reg_val = readl(mdata->base + SPI_CFG1_REG);
 	reg_val &= ~SPI_CFG1_CS_IDLE_MASK;
diff --git a/drivers/spi/spi-omap2-mcspi.c b/drivers/spi/spi-omap2-mcspi.c
index 7275223dbcd4..e048268d8ba2 100644
--- a/drivers/spi/spi-omap2-mcspi.c
+++ b/drivers/spi/spi-omap2-mcspi.c
@@ -1412,9 +1412,6 @@ static int omap2_mcspi_probe(struct platform_device *pdev)
 		sprintf(mcspi->dma_channels[i].dma_tx_ch_name, "tx%d", i);
 	}
 
-	if (status < 0)
-		goto free_master;
-
 	pm_runtime_use_autosuspend(&pdev->dev);
 	pm_runtime_set_autosuspend_delay(&pdev->dev, SPI_AUTOSUSPEND_TIMEOUT);
 	pm_runtime_enable(&pdev->dev);
diff --git a/drivers/spi/spi-orion.c b/drivers/spi/spi-orion.c
index be2e87ee8b31..28fc9f161b9d 100644
--- a/drivers/spi/spi-orion.c
+++ b/drivers/spi/spi-orion.c
@@ -22,6 +22,7 @@
 #include <linux/of_device.h>
 #include <linux/clk.h>
 #include <linux/sizes.h>
+#include <linux/gpio.h>
 #include <asm/unaligned.h>
 
 #define DRIVER_NAME			"orion_spi"
@@ -320,12 +321,18 @@ orion_spi_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
 static void orion_spi_set_cs(struct spi_device *spi, bool enable)
 {
 	struct orion_spi *orion_spi;
+	int cs;
+
+	if (gpio_is_valid(spi->cs_gpio))
+		cs = 0;
+	else
+		cs = spi->chip_select;
 
 	orion_spi = spi_master_get_devdata(spi->master);
 
 	orion_spi_clrbits(orion_spi, ORION_SPI_IF_CTRL_REG, ORION_SPI_CS_MASK);
 	orion_spi_setbits(orion_spi, ORION_SPI_IF_CTRL_REG,
-				ORION_SPI_CS(spi->chip_select));
+				ORION_SPI_CS(cs));
 
 	/* Chip select logic is inverted from spi_set_cs */
 	if (!enable)
@@ -606,6 +613,7 @@ static int orion_spi_probe(struct platform_device *pdev)
 	master->setup = orion_spi_setup;
 	master->bits_per_word_mask = SPI_BPW_MASK(8) | SPI_BPW_MASK(16);
 	master->auto_runtime_pm = true;
+	master->flags = SPI_MASTER_GPIO_SS;
 
 	platform_set_drvdata(pdev, master);
 
diff --git a/drivers/spi/spi-pxa2xx.c b/drivers/spi/spi-pxa2xx.c
index 47b65d7c4072..38d053682892 100644
--- a/drivers/spi/spi-pxa2xx.c
+++ b/drivers/spi/spi-pxa2xx.c
@@ -151,6 +151,18 @@ static const struct lpss_config lpss_platforms[] = {
 		.cs_sel_shift = 8,
 		.cs_sel_mask = 3 << 8,
 	},
+	{	/* LPSS_CNL_SSP */
+		.offset = 0x200,
+		.reg_general = -1,
+		.reg_ssp = 0x20,
+		.reg_cs_ctrl = 0x24,
+		.reg_capabilities = 0xfc,
+		.rx_threshold = 1,
+		.tx_threshold_lo = 32,
+		.tx_threshold_hi = 56,
+		.cs_sel_shift = 8,
+		.cs_sel_mask = 3 << 8,
+	},
 };
 
 static inline const struct lpss_config
@@ -167,6 +179,7 @@ static bool is_lpss_ssp(const struct driver_data *drv_data)
 	case LPSS_BSW_SSP:
 	case LPSS_SPT_SSP:
 	case LPSS_BXT_SSP:
+	case LPSS_CNL_SSP:
 		return true;
 	default:
 		return false;
@@ -1275,6 +1288,7 @@ static int setup(struct spi_device *spi)
 	case LPSS_BSW_SSP:
 	case LPSS_SPT_SSP:
 	case LPSS_BXT_SSP:
+	case LPSS_CNL_SSP:
 		config = lpss_get_config(drv_data);
 		tx_thres = config->tx_threshold_lo;
 		tx_hi_thres = config->tx_threshold_hi;
@@ -1470,6 +1484,14 @@ static const struct pci_device_id pxa2xx_spi_pci_compound_match[] = {
 	{ PCI_VDEVICE(INTEL, 0x5ac2), LPSS_BXT_SSP },
 	{ PCI_VDEVICE(INTEL, 0x5ac4), LPSS_BXT_SSP },
 	{ PCI_VDEVICE(INTEL, 0x5ac6), LPSS_BXT_SSP },
+	/* CNL-LP */
+	{ PCI_VDEVICE(INTEL, 0x9daa), LPSS_CNL_SSP },
+	{ PCI_VDEVICE(INTEL, 0x9dab), LPSS_CNL_SSP },
+	{ PCI_VDEVICE(INTEL, 0x9dfb), LPSS_CNL_SSP },
+	/* CNL-H */
+	{ PCI_VDEVICE(INTEL, 0xa32a), LPSS_CNL_SSP },
+	{ PCI_VDEVICE(INTEL, 0xa32b), LPSS_CNL_SSP },
+	{ PCI_VDEVICE(INTEL, 0xa37b), LPSS_CNL_SSP },
 	{ },
 };
 
diff --git a/drivers/spi/spi-rockchip.c b/drivers/spi/spi-rockchip.c
index acf31f36b898..0b4a52b3e1dc 100644
--- a/drivers/spi/spi-rockchip.c
+++ b/drivers/spi/spi-rockchip.c
@@ -25,6 +25,11 @@
 
 #define DRIVER_NAME "rockchip-spi"
 
+#define ROCKCHIP_SPI_CLR_BITS(reg, bits) \
+		writel_relaxed(readl_relaxed(reg) & ~(bits), reg)
+#define ROCKCHIP_SPI_SET_BITS(reg, bits) \
+		writel_relaxed(readl_relaxed(reg) | (bits), reg)
+
 /* SPI register offsets */
 #define ROCKCHIP_SPI_CTRLR0			0x0000
 #define ROCKCHIP_SPI_CTRLR1			0x0004
@@ -149,6 +154,8 @@
  */
 #define ROCKCHIP_SPI_MAX_TRANLEN		0xffff
 
+#define ROCKCHIP_SPI_MAX_CS_NUM			2
+
 enum rockchip_ssi_type {
 	SSI_MOTO_SPI = 0,
 	SSI_TI_SSP,
@@ -193,6 +200,8 @@ struct rockchip_spi {
 	/* protect state */
 	spinlock_t lock;
 
+	bool cs_asserted[ROCKCHIP_SPI_MAX_CS_NUM];
+
 	u32 use_dma;
 	struct sg_table tx_sg;
 	struct sg_table rx_sg;
@@ -264,37 +273,29 @@ static inline u32 rx_max(struct rockchip_spi *rs)
 
 static void rockchip_spi_set_cs(struct spi_device *spi, bool enable)
 {
-	u32 ser;
 	struct spi_master *master = spi->master;
 	struct rockchip_spi *rs = spi_master_get_devdata(master);
+	bool cs_asserted = !enable;
 
-	pm_runtime_get_sync(rs->dev);
+	/* Return immediately for no-op */
+	if (cs_asserted == rs->cs_asserted[spi->chip_select])
+		return;
 
-	ser = readl_relaxed(rs->regs + ROCKCHIP_SPI_SER) & SER_MASK;
+	if (cs_asserted) {
+		/* Keep things powered as long as CS is asserted */
+		pm_runtime_get_sync(rs->dev);
 
-	/*
-	 * drivers/spi/spi.c:
-	 * static void spi_set_cs(struct spi_device *spi, bool enable)
-	 * {
-	 *		if (spi->mode & SPI_CS_HIGH)
-	 *			enable = !enable;
-	 *
-	 *		if (spi->cs_gpio >= 0)
-	 *			gpio_set_value(spi->cs_gpio, !enable);
-	 *		else if (spi->master->set_cs)
-	 *		spi->master->set_cs(spi, !enable);
-	 * }
-	 *
-	 * Note: enable(rockchip_spi_set_cs) = !enable(spi_set_cs)
-	 */
-	if (!enable)
-		ser |= 1 << spi->chip_select;
-	else
-		ser &= ~(1 << spi->chip_select);
+		ROCKCHIP_SPI_SET_BITS(rs->regs + ROCKCHIP_SPI_SER,
+				      BIT(spi->chip_select));
+	} else {
+		ROCKCHIP_SPI_CLR_BITS(rs->regs + ROCKCHIP_SPI_SER,
+				      BIT(spi->chip_select));
 
-	writel_relaxed(ser, rs->regs + ROCKCHIP_SPI_SER);
+		/* Drop reference from when we first asserted CS */
+		pm_runtime_put(rs->dev);
+	}
 
-	pm_runtime_put_sync(rs->dev);
+	rs->cs_asserted[spi->chip_select] = cs_asserted;
 }
 
 static int rockchip_spi_prepare_message(struct spi_master *master,
@@ -684,33 +685,33 @@ static int rockchip_spi_probe(struct platform_device *pdev)
 	rs->regs = devm_ioremap_resource(&pdev->dev, mem);
 	if (IS_ERR(rs->regs)) {
 		ret =  PTR_ERR(rs->regs);
-		goto err_ioremap_resource;
+		goto err_put_master;
 	}
 
 	rs->apb_pclk = devm_clk_get(&pdev->dev, "apb_pclk");
 	if (IS_ERR(rs->apb_pclk)) {
 		dev_err(&pdev->dev, "Failed to get apb_pclk\n");
 		ret = PTR_ERR(rs->apb_pclk);
-		goto err_ioremap_resource;
+		goto err_put_master;
 	}
 
 	rs->spiclk = devm_clk_get(&pdev->dev, "spiclk");
 	if (IS_ERR(rs->spiclk)) {
 		dev_err(&pdev->dev, "Failed to get spi_pclk\n");
 		ret = PTR_ERR(rs->spiclk);
-		goto err_ioremap_resource;
+		goto err_put_master;
 	}
 
 	ret = clk_prepare_enable(rs->apb_pclk);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to enable apb_pclk\n");
-		goto err_ioremap_resource;
+		goto err_put_master;
 	}
 
 	ret = clk_prepare_enable(rs->spiclk);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to enable spi_clk\n");
-		goto err_spiclk_enable;
+		goto err_disable_apbclk;
 	}
 
 	spi_enable_chip(rs, 0);
@@ -728,7 +729,7 @@ static int rockchip_spi_probe(struct platform_device *pdev)
 	if (!rs->fifo_len) {
 		dev_err(&pdev->dev, "Failed to get fifo length\n");
 		ret = -EINVAL;
-		goto err_get_fifo_len;
+		goto err_disable_spiclk;
 	}
 
 	spin_lock_init(&rs->lock);
@@ -739,7 +740,7 @@ static int rockchip_spi_probe(struct platform_device *pdev)
 	master->auto_runtime_pm = true;
 	master->bus_num = pdev->id;
 	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_LOOP;
-	master->num_chipselect = 2;
+	master->num_chipselect = ROCKCHIP_SPI_MAX_CS_NUM;
 	master->dev.of_node = pdev->dev.of_node;
 	master->bits_per_word_mask = SPI_BPW_MASK(16) | SPI_BPW_MASK(8);
 
@@ -749,13 +750,14 @@ static int rockchip_spi_probe(struct platform_device *pdev)
 	master->transfer_one = rockchip_spi_transfer_one;
 	master->max_transfer_size = rockchip_spi_max_transfer_size;
 	master->handle_err = rockchip_spi_handle_err;
+	master->flags = SPI_MASTER_GPIO_SS;
 
 	rs->dma_tx.ch = dma_request_chan(rs->dev, "tx");
 	if (IS_ERR(rs->dma_tx.ch)) {
 		/* Check tx to see if we need defer probing driver */
 		if (PTR_ERR(rs->dma_tx.ch) == -EPROBE_DEFER) {
 			ret = -EPROBE_DEFER;
-			goto err_get_fifo_len;
+			goto err_disable_pm_runtime;
 		}
 		dev_warn(rs->dev, "Failed to request TX DMA channel\n");
 		rs->dma_tx.ch = NULL;
@@ -786,23 +788,24 @@ static int rockchip_spi_probe(struct platform_device *pdev)
 	ret = devm_spi_register_master(&pdev->dev, master);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to register master\n");
-		goto err_register_master;
+		goto err_free_dma_rx;
 	}
 
 	return 0;
 
-err_register_master:
-	pm_runtime_disable(&pdev->dev);
+err_free_dma_rx:
 	if (rs->dma_rx.ch)
 		dma_release_channel(rs->dma_rx.ch);
 err_free_dma_tx:
 	if (rs->dma_tx.ch)
 		dma_release_channel(rs->dma_tx.ch);
-err_get_fifo_len:
+err_disable_pm_runtime:
+	pm_runtime_disable(&pdev->dev);
+err_disable_spiclk:
 	clk_disable_unprepare(rs->spiclk);
-err_spiclk_enable:
+err_disable_apbclk:
 	clk_disable_unprepare(rs->apb_pclk);
-err_ioremap_resource:
+err_put_master:
 	spi_master_put(master);
 
 	return ret;
diff --git a/drivers/spi/spi-sh-msiof.c b/drivers/spi/spi-sh-msiof.c
index 2ce15ca97782..c304c7167866 100644
--- a/drivers/spi/spi-sh-msiof.c
+++ b/drivers/spi/spi-sh-msiof.c
@@ -2,7 +2,8 @@
  * SuperH MSIOF SPI Master Interface
  *
  * Copyright (c) 2009 Magnus Damm
- * Copyright (C) 2014 Glider bvba
+ * Copyright (C) 2014 Renesas Electronics Corporation
+ * Copyright (C) 2014-2017 Glider bvba
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -33,7 +34,6 @@
 
 #include <asm/unaligned.h>
 
-
 struct sh_msiof_chipdata {
 	u16 tx_fifo_size;
 	u16 rx_fifo_size;
@@ -53,6 +53,7 @@ struct sh_msiof_spi_priv {
 	void *rx_dma_page;
 	dma_addr_t tx_dma_addr;
 	dma_addr_t rx_dma_addr;
+	bool slave_aborted;
 };
 
 #define TMDR1	0x00	/* Transmit Mode Register 1 */
@@ -337,7 +338,10 @@ static void sh_msiof_spi_set_pin_regs(struct sh_msiof_spi_priv *p,
 	tmp |= !cs_high << MDR1_SYNCAC_SHIFT;
 	tmp |= lsb_first << MDR1_BITLSB_SHIFT;
 	tmp |= sh_msiof_spi_get_dtdl_and_syncdl(p);
-	sh_msiof_write(p, TMDR1, tmp | MDR1_TRMD | TMDR1_PCON);
+	if (spi_controller_is_slave(p->master))
+		sh_msiof_write(p, TMDR1, tmp | TMDR1_PCON);
+	else
+		sh_msiof_write(p, TMDR1, tmp | MDR1_TRMD | TMDR1_PCON);
 	if (p->master->flags & SPI_MASTER_MUST_TX) {
 		/* These bits are reserved if RX needs TX */
 		tmp &= ~0x0000ffff;
@@ -564,17 +568,19 @@ static int sh_msiof_prepare_message(struct spi_master *master,
 
 static int sh_msiof_spi_start(struct sh_msiof_spi_priv *p, void *rx_buf)
 {
-	int ret;
+	bool slave = spi_controller_is_slave(p->master);
+	int ret = 0;
 
 	/* setup clock and rx/tx signals */
-	ret = sh_msiof_modify_ctr_wait(p, 0, CTR_TSCKE);
+	if (!slave)
+		ret = sh_msiof_modify_ctr_wait(p, 0, CTR_TSCKE);
 	if (rx_buf && !ret)
 		ret = sh_msiof_modify_ctr_wait(p, 0, CTR_RXE);
 	if (!ret)
 		ret = sh_msiof_modify_ctr_wait(p, 0, CTR_TXE);
 
 	/* start by setting frame bit */
-	if (!ret)
+	if (!ret && !slave)
 		ret = sh_msiof_modify_ctr_wait(p, 0, CTR_TFSE);
 
 	return ret;
@@ -582,20 +588,49 @@ static int sh_msiof_spi_start(struct sh_msiof_spi_priv *p, void *rx_buf)
 
 static int sh_msiof_spi_stop(struct sh_msiof_spi_priv *p, void *rx_buf)
 {
-	int ret;
+	bool slave = spi_controller_is_slave(p->master);
+	int ret = 0;
 
 	/* shut down frame, rx/tx and clock signals */
-	ret = sh_msiof_modify_ctr_wait(p, CTR_TFSE, 0);
+	if (!slave)
+		ret = sh_msiof_modify_ctr_wait(p, CTR_TFSE, 0);
 	if (!ret)
 		ret = sh_msiof_modify_ctr_wait(p, CTR_TXE, 0);
 	if (rx_buf && !ret)
 		ret = sh_msiof_modify_ctr_wait(p, CTR_RXE, 0);
-	if (!ret)
+	if (!ret && !slave)
 		ret = sh_msiof_modify_ctr_wait(p, CTR_TSCKE, 0);
 
 	return ret;
 }
 
+static int sh_msiof_slave_abort(struct spi_master *master)
+{
+	struct sh_msiof_spi_priv *p = spi_master_get_devdata(master);
+
+	p->slave_aborted = true;
+	complete(&p->done);
+	return 0;
+}
+
+static int sh_msiof_wait_for_completion(struct sh_msiof_spi_priv *p)
+{
+	if (spi_controller_is_slave(p->master)) {
+		if (wait_for_completion_interruptible(&p->done) ||
+		    p->slave_aborted) {
+			dev_dbg(&p->pdev->dev, "interrupted\n");
+			return -EINTR;
+		}
+	} else {
+		if (!wait_for_completion_timeout(&p->done, HZ)) {
+			dev_err(&p->pdev->dev, "timeout\n");
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
 static int sh_msiof_spi_txrx_once(struct sh_msiof_spi_priv *p,
 				  void (*tx_fifo)(struct sh_msiof_spi_priv *,
 						  const void *, int, int),
@@ -628,6 +663,7 @@ static int sh_msiof_spi_txrx_once(struct sh_msiof_spi_priv *p,
 		tx_fifo(p, tx_buf, words, fifo_shift);
 
 	reinit_completion(&p->done);
+	p->slave_aborted = false;
 
 	ret = sh_msiof_spi_start(p, rx_buf);
 	if (ret) {
@@ -636,11 +672,9 @@ static int sh_msiof_spi_txrx_once(struct sh_msiof_spi_priv *p,
 	}
 
 	/* wait for tx fifo to be emptied / rx fifo to be filled */
-	if (!wait_for_completion_timeout(&p->done, HZ)) {
-		dev_err(&p->pdev->dev, "PIO timeout\n");
-		ret = -ETIMEDOUT;
+	ret = sh_msiof_wait_for_completion(p);
+	if (ret)
 		goto stop_reset;
-	}
 
 	/* read rx fifo */
 	if (rx_buf)
@@ -732,6 +766,7 @@ static int sh_msiof_dma_once(struct sh_msiof_spi_priv *p, const void *tx,
 	sh_msiof_write(p, IER, ier_bits);
 
 	reinit_completion(&p->done);
+	p->slave_aborted = false;
 
 	/* Now start DMA */
 	if (rx)
@@ -746,11 +781,9 @@ static int sh_msiof_dma_once(struct sh_msiof_spi_priv *p, const void *tx,
 	}
 
 	/* wait for tx fifo to be emptied / rx fifo to be filled */
-	if (!wait_for_completion_timeout(&p->done, HZ)) {
-		dev_err(&p->pdev->dev, "DMA timeout\n");
-		ret = -ETIMEDOUT;
+	ret = sh_msiof_wait_for_completion(p);
+	if (ret)
 		goto stop_reset;
-	}
 
 	/* clear status bits */
 	sh_msiof_reset_str(p);
@@ -843,7 +876,8 @@ static int sh_msiof_transfer_one(struct spi_master *master,
 	int ret;
 
 	/* setup clocks (clock already enabled in chipselect()) */
-	sh_msiof_spi_set_clk_regs(p, clk_get_rate(p->clk), t->speed_hz);
+	if (!spi_controller_is_slave(p->master))
+		sh_msiof_spi_set_clk_regs(p, clk_get_rate(p->clk), t->speed_hz);
 
 	while (master->dma_tx && len > 15) {
 		/*
@@ -998,8 +1032,12 @@ static struct sh_msiof_spi_info *sh_msiof_spi_parse_dt(struct device *dev)
 	if (!info)
 		return NULL;
 
+	info->mode = of_property_read_bool(np, "spi-slave") ? MSIOF_SPI_SLAVE
+							    : MSIOF_SPI_MASTER;
+
 	/* Parse the MSIOF properties */
-	of_property_read_u32(np, "num-cs", &num_cs);
+	if (info->mode == MSIOF_SPI_MASTER)
+		of_property_read_u32(np, "num-cs", &num_cs);
 	of_property_read_u32(np, "renesas,tx-fifo-size",
 					&info->tx_fifo_override);
 	of_property_read_u32(np, "renesas,rx-fifo-size",
@@ -1159,34 +1197,40 @@ static int sh_msiof_spi_probe(struct platform_device *pdev)
 	struct spi_master *master;
 	const struct sh_msiof_chipdata *chipdata;
 	const struct of_device_id *of_id;
+	struct sh_msiof_spi_info *info;
 	struct sh_msiof_spi_priv *p;
 	int i;
 	int ret;
 
-	master = spi_alloc_master(&pdev->dev, sizeof(struct sh_msiof_spi_priv));
-	if (master == NULL)
-		return -ENOMEM;
-
-	p = spi_master_get_devdata(master);
-
-	platform_set_drvdata(pdev, p);
-	p->master = master;
-
 	of_id = of_match_device(sh_msiof_match, &pdev->dev);
 	if (of_id) {
 		chipdata = of_id->data;
-		p->info = sh_msiof_spi_parse_dt(&pdev->dev);
+		info = sh_msiof_spi_parse_dt(&pdev->dev);
 	} else {
 		chipdata = (const void *)pdev->id_entry->driver_data;
-		p->info = dev_get_platdata(&pdev->dev);
+		info = dev_get_platdata(&pdev->dev);
 	}
 
-	if (!p->info) {
+	if (!info) {
 		dev_err(&pdev->dev, "failed to obtain device info\n");
-		ret = -ENXIO;
-		goto err1;
+		return -ENXIO;
 	}
 
+	if (info->mode == MSIOF_SPI_SLAVE)
+		master = spi_alloc_slave(&pdev->dev,
+					 sizeof(struct sh_msiof_spi_priv));
+	else
+		master = spi_alloc_master(&pdev->dev,
+					  sizeof(struct sh_msiof_spi_priv));
+	if (master == NULL)
+		return -ENOMEM;
+
+	p = spi_master_get_devdata(master);
+
+	platform_set_drvdata(pdev, p);
+	p->master = master;
+	p->info = info;
+
 	init_completion(&p->done);
 
 	p->clk = devm_clk_get(&pdev->dev, NULL);
@@ -1237,6 +1281,7 @@ static int sh_msiof_spi_probe(struct platform_device *pdev)
 	master->num_chipselect = p->info->num_chipselect;
 	master->setup = sh_msiof_spi_setup;
 	master->prepare_message = sh_msiof_prepare_message;
+	master->slave_abort = sh_msiof_slave_abort;
 	master->bits_per_word_mask = SPI_BPW_RANGE_MASK(8, 32);
 	master->auto_runtime_pm = true;
 	master->transfer_one = sh_msiof_transfer_one;
diff --git a/drivers/spi/spi-sirf.c b/drivers/spi/spi-sirf.c
index 7072276ad354..bbb1a275f718 100644
--- a/drivers/spi/spi-sirf.c
+++ b/drivers/spi/spi-sirf.c
@@ -1158,7 +1158,7 @@ static int spi_sirfsoc_probe(struct platform_device *pdev)
 	ret = spi_bitbang_start(&sspi->bitbang);
 	if (ret)
 		goto free_clk;
-	dev_info(&pdev->dev, "registerred, bus number = %d\n", master->bus_num);
+	dev_info(&pdev->dev, "registered, bus number = %d\n", master->bus_num);
 
 	return 0;
 free_clk:
diff --git a/drivers/spi/spi-slave-system-control.c b/drivers/spi/spi-slave-system-control.c
new file mode 100644
index 000000000000..c0257e937995
--- /dev/null
+++ b/drivers/spi/spi-slave-system-control.c
@@ -0,0 +1,154 @@
+/*
+ * SPI slave handler controlling system state
+ *
+ * This SPI slave handler allows remote control of system reboot, power off,
+ * halt, and suspend.
+ *
+ * Copyright (C) 2016-2017 Glider bvba
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ *
+ * Usage (assuming /dev/spidev2.0 corresponds to the SPI master on the remote
+ * system):
+ *
+ *   # reboot='\x7c\x50'
+ *   # poweroff='\x71\x3f'
+ *   # halt='\x38\x76'
+ *   # suspend='\x1b\x1b'
+ *   # spidev_test -D /dev/spidev2.0 -p $suspend # or $reboot, $poweroff, $halt
+ */
+
+#include <linux/completion.h>
+#include <linux/module.h>
+#include <linux/reboot.h>
+#include <linux/suspend.h>
+#include <linux/spi/spi.h>
+
+/*
+ * The numbers are chosen to display something human-readable on two 7-segment
+ * displays connected to two 74HC595 shift registers
+ */
+#define CMD_REBOOT	0x7c50	/* rb */
+#define CMD_POWEROFF	0x713f	/* OF */
+#define CMD_HALT	0x3876	/* HL */
+#define CMD_SUSPEND	0x1b1b	/* ZZ */
+
+struct spi_slave_system_control_priv {
+	struct spi_device *spi;
+	struct completion finished;
+	struct spi_transfer xfer;
+	struct spi_message msg;
+	__be16 cmd;
+};
+
+static
+int spi_slave_system_control_submit(struct spi_slave_system_control_priv *priv);
+
+static void spi_slave_system_control_complete(void *arg)
+{
+	struct spi_slave_system_control_priv *priv = arg;
+	u16 cmd;
+	int ret;
+
+	if (priv->msg.status)
+		goto terminate;
+
+	cmd = be16_to_cpu(priv->cmd);
+	switch (cmd) {
+	case CMD_REBOOT:
+		dev_info(&priv->spi->dev, "Rebooting system...\n");
+		kernel_restart(NULL);
+
+	case CMD_POWEROFF:
+		dev_info(&priv->spi->dev, "Powering off system...\n");
+		kernel_power_off();
+		break;
+
+	case CMD_HALT:
+		dev_info(&priv->spi->dev, "Halting system...\n");
+		kernel_halt();
+		break;
+
+	case CMD_SUSPEND:
+		dev_info(&priv->spi->dev, "Suspending system...\n");
+		pm_suspend(PM_SUSPEND_MEM);
+		break;
+
+	default:
+		dev_warn(&priv->spi->dev, "Unknown command 0x%x\n", cmd);
+		break;
+	}
+
+	ret = spi_slave_system_control_submit(priv);
+	if (ret)
+		goto terminate;
+
+	return;
+
+terminate:
+	dev_info(&priv->spi->dev, "Terminating\n");
+	complete(&priv->finished);
+}
+
+static
+int spi_slave_system_control_submit(struct spi_slave_system_control_priv *priv)
+{
+	int ret;
+
+	spi_message_init_with_transfers(&priv->msg, &priv->xfer, 1);
+
+	priv->msg.complete = spi_slave_system_control_complete;
+	priv->msg.context = priv;
+
+	ret = spi_async(priv->spi, &priv->msg);
+	if (ret)
+		dev_err(&priv->spi->dev, "spi_async() failed %d\n", ret);
+
+	return ret;
+}
+
+static int spi_slave_system_control_probe(struct spi_device *spi)
+{
+	struct spi_slave_system_control_priv *priv;
+	int ret;
+
+	priv = devm_kzalloc(&spi->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->spi = spi;
+	init_completion(&priv->finished);
+	priv->xfer.rx_buf = &priv->cmd;
+	priv->xfer.len = sizeof(priv->cmd);
+
+	ret = spi_slave_system_control_submit(priv);
+	if (ret)
+		return ret;
+
+	spi_set_drvdata(spi, priv);
+	return 0;
+}
+
+static int spi_slave_system_control_remove(struct spi_device *spi)
+{
+	struct spi_slave_system_control_priv *priv = spi_get_drvdata(spi);
+
+	spi_slave_abort(spi);
+	wait_for_completion(&priv->finished);
+	return 0;
+}
+
+static struct spi_driver spi_slave_system_control_driver = {
+	.driver = {
+		.name	= "spi-slave-system-control",
+	},
+	.probe		= spi_slave_system_control_probe,
+	.remove		= spi_slave_system_control_remove,
+};
+module_spi_driver(spi_slave_system_control_driver);
+
+MODULE_AUTHOR("Geert Uytterhoeven <geert+renesas@glider.be>");
+MODULE_DESCRIPTION("SPI slave handler controlling system state");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/spi/spi-slave-time.c b/drivers/spi/spi-slave-time.c
new file mode 100644
index 000000000000..f2e07a392d68
--- /dev/null
+++ b/drivers/spi/spi-slave-time.c
@@ -0,0 +1,129 @@
+/*
+ * SPI slave handler reporting uptime at reception of previous SPI message
+ *
+ * This SPI slave handler sends the time of reception of the last SPI message
+ * as two 32-bit unsigned integers in binary format and in network byte order,
+ * representing the number of seconds and fractional seconds (in microseconds)
+ * since boot up.
+ *
+ * Copyright (C) 2016-2017 Glider bvba
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ *
+ * Usage (assuming /dev/spidev2.0 corresponds to the SPI master on the remote
+ * system):
+ *
+ *   # spidev_test -D /dev/spidev2.0 -p dummy-8B
+ *   spi mode: 0x0
+ *   bits per word: 8
+ *   max speed: 500000 Hz (500 KHz)
+ *   RX | 00 00 04 6D 00 09 5B BB ...
+ *		^^^^^    ^^^^^^^^
+ *		seconds  microseconds
+ */
+
+#include <linux/completion.h>
+#include <linux/module.h>
+#include <linux/sched/clock.h>
+#include <linux/spi/spi.h>
+
+
+struct spi_slave_time_priv {
+	struct spi_device *spi;
+	struct completion finished;
+	struct spi_transfer xfer;
+	struct spi_message msg;
+	__be32 buf[2];
+};
+
+static int spi_slave_time_submit(struct spi_slave_time_priv *priv);
+
+static void spi_slave_time_complete(void *arg)
+{
+	struct spi_slave_time_priv *priv = arg;
+	int ret;
+
+	ret = priv->msg.status;
+	if (ret)
+		goto terminate;
+
+	ret = spi_slave_time_submit(priv);
+	if (ret)
+		goto terminate;
+
+	return;
+
+terminate:
+	dev_info(&priv->spi->dev, "Terminating\n");
+	complete(&priv->finished);
+}
+
+static int spi_slave_time_submit(struct spi_slave_time_priv *priv)
+{
+	u32 rem_us;
+	int ret;
+	u64 ts;
+
+	ts = local_clock();
+	rem_us = do_div(ts, 1000000000) / 1000;
+
+	priv->buf[0] = cpu_to_be32(ts);
+	priv->buf[1] = cpu_to_be32(rem_us);
+
+	spi_message_init_with_transfers(&priv->msg, &priv->xfer, 1);
+
+	priv->msg.complete = spi_slave_time_complete;
+	priv->msg.context = priv;
+
+	ret = spi_async(priv->spi, &priv->msg);
+	if (ret)
+		dev_err(&priv->spi->dev, "spi_async() failed %d\n", ret);
+
+	return ret;
+}
+
+static int spi_slave_time_probe(struct spi_device *spi)
+{
+	struct spi_slave_time_priv *priv;
+	int ret;
+
+	priv = devm_kzalloc(&spi->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->spi = spi;
+	init_completion(&priv->finished);
+	priv->xfer.tx_buf = priv->buf;
+	priv->xfer.len = sizeof(priv->buf);
+
+	ret = spi_slave_time_submit(priv);
+	if (ret)
+		return ret;
+
+	spi_set_drvdata(spi, priv);
+	return 0;
+}
+
+static int spi_slave_time_remove(struct spi_device *spi)
+{
+	struct spi_slave_time_priv *priv = spi_get_drvdata(spi);
+
+	spi_slave_abort(spi);
+	wait_for_completion(&priv->finished);
+	return 0;
+}
+
+static struct spi_driver spi_slave_time_driver = {
+	.driver = {
+		.name	= "spi-slave-time",
+	},
+	.probe		= spi_slave_time_probe,
+	.remove		= spi_slave_time_remove,
+};
+module_spi_driver(spi_slave_time_driver);
+
+MODULE_AUTHOR("Geert Uytterhoeven <geert+renesas@glider.be>");
+MODULE_DESCRIPTION("SPI slave reporting uptime at previous SPI message");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/spi/spi-st-ssc4.c b/drivers/spi/spi-st-ssc4.c
index e54b59638458..a4e43fc19ece 100644
--- a/drivers/spi/spi-st-ssc4.c
+++ b/drivers/spi/spi-st-ssc4.c
@@ -229,42 +229,42 @@ static int spi_st_setup(struct spi_device *spi)
 		"setting baudrate:target= %u hz, actual= %u hz, sscbrg= %u\n",
 		hz, spi_st->baud, sscbrg);
 
-	 /* Set SSC_CTL and enable SSC */
-	 var = readl_relaxed(spi_st->base + SSC_CTL);
-	 var |= SSC_CTL_MS;
+	/* Set SSC_CTL and enable SSC */
+	var = readl_relaxed(spi_st->base + SSC_CTL);
+	var |= SSC_CTL_MS;
 
-	 if (spi->mode & SPI_CPOL)
+	if (spi->mode & SPI_CPOL)
 		var |= SSC_CTL_PO;
-	 else
+	else
 		var &= ~SSC_CTL_PO;
 
-	 if (spi->mode & SPI_CPHA)
+	if (spi->mode & SPI_CPHA)
 		var |= SSC_CTL_PH;
-	 else
+	else
 		var &= ~SSC_CTL_PH;
 
-	 if ((spi->mode & SPI_LSB_FIRST) == 0)
+	if ((spi->mode & SPI_LSB_FIRST) == 0)
 		var |= SSC_CTL_HB;
-	 else
+	else
 		var &= ~SSC_CTL_HB;
 
-	 if (spi->mode & SPI_LOOP)
+	if (spi->mode & SPI_LOOP)
 		var |= SSC_CTL_LPB;
-	 else
+	else
 		var &= ~SSC_CTL_LPB;
 
-	 var &= ~SSC_CTL_DATA_WIDTH_MSK;
-	 var |= (spi->bits_per_word - 1);
+	var &= ~SSC_CTL_DATA_WIDTH_MSK;
+	var |= (spi->bits_per_word - 1);
 
-	 var |= SSC_CTL_EN_TX_FIFO | SSC_CTL_EN_RX_FIFO;
-	 var |= SSC_CTL_EN;
+	var |= SSC_CTL_EN_TX_FIFO | SSC_CTL_EN_RX_FIFO;
+	var |= SSC_CTL_EN;
 
-	 writel_relaxed(var, spi_st->base + SSC_CTL);
+	writel_relaxed(var, spi_st->base + SSC_CTL);
 
-	 /* Clear the status register */
-	 readl_relaxed(spi_st->base + SSC_RBUF);
+	/* Clear the status register */
+	readl_relaxed(spi_st->base + SSC_RBUF);
 
-	 return 0;
+	return 0;
 
 out_free_gpio:
 	gpio_free(cs);
diff --git a/drivers/spi/spi-stm32.c b/drivers/spi/spi-stm32.c
new file mode 100644
index 000000000000..75644bcd938b
--- /dev/null
+++ b/drivers/spi/spi-stm32.c
@@ -0,0 +1,1322 @@
+/*
+ * STMicroelectronics STM32 SPI Controller driver (master mode only)
+ *
+ * Copyright (C) 2017, STMicroelectronics - All Rights Reserved
+ * Author(s): Amelie Delaunay <amelie.delaunay@st.com> for STMicroelectronics.
+ *
+ * License terms: GPL V2.0.
+ *
+ * spi_stm32 driver is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * spi_stm32 driver is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * spi_stm32 driver. If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/debugfs.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/dmaengine.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/iopoll.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/pm_runtime.h>
+#include <linux/reset.h>
+#include <linux/spi/spi.h>
+
+#define DRIVER_NAME "spi_stm32"
+
+/* STM32 SPI registers */
+#define STM32_SPI_CR1		0x00
+#define STM32_SPI_CR2		0x04
+#define STM32_SPI_CFG1		0x08
+#define STM32_SPI_CFG2		0x0C
+#define STM32_SPI_IER		0x10
+#define STM32_SPI_SR		0x14
+#define STM32_SPI_IFCR		0x18
+#define STM32_SPI_TXDR		0x20
+#define STM32_SPI_RXDR		0x30
+#define STM32_SPI_I2SCFGR	0x50
+
+/* STM32_SPI_CR1 bit fields */
+#define SPI_CR1_SPE		BIT(0)
+#define SPI_CR1_MASRX		BIT(8)
+#define SPI_CR1_CSTART		BIT(9)
+#define SPI_CR1_CSUSP		BIT(10)
+#define SPI_CR1_HDDIR		BIT(11)
+#define SPI_CR1_SSI		BIT(12)
+
+/* STM32_SPI_CR2 bit fields */
+#define SPI_CR2_TSIZE_SHIFT	0
+#define SPI_CR2_TSIZE		GENMASK(15, 0)
+
+/* STM32_SPI_CFG1 bit fields */
+#define SPI_CFG1_DSIZE_SHIFT	0
+#define SPI_CFG1_DSIZE		GENMASK(4, 0)
+#define SPI_CFG1_FTHLV_SHIFT	5
+#define SPI_CFG1_FTHLV		GENMASK(8, 5)
+#define SPI_CFG1_RXDMAEN	BIT(14)
+#define SPI_CFG1_TXDMAEN	BIT(15)
+#define SPI_CFG1_MBR_SHIFT	28
+#define SPI_CFG1_MBR		GENMASK(30, 28)
+#define SPI_CFG1_MBR_MIN	0
+#define SPI_CFG1_MBR_MAX	(GENMASK(30, 28) >> 28)
+
+/* STM32_SPI_CFG2 bit fields */
+#define SPI_CFG2_MIDI_SHIFT	4
+#define SPI_CFG2_MIDI		GENMASK(7, 4)
+#define SPI_CFG2_COMM_SHIFT	17
+#define SPI_CFG2_COMM		GENMASK(18, 17)
+#define SPI_CFG2_SP_SHIFT	19
+#define SPI_CFG2_SP		GENMASK(21, 19)
+#define SPI_CFG2_MASTER		BIT(22)
+#define SPI_CFG2_LSBFRST	BIT(23)
+#define SPI_CFG2_CPHA		BIT(24)
+#define SPI_CFG2_CPOL		BIT(25)
+#define SPI_CFG2_SSM		BIT(26)
+#define SPI_CFG2_AFCNTR		BIT(31)
+
+/* STM32_SPI_IER bit fields */
+#define SPI_IER_RXPIE		BIT(0)
+#define SPI_IER_TXPIE		BIT(1)
+#define SPI_IER_DXPIE		BIT(2)
+#define SPI_IER_EOTIE		BIT(3)
+#define SPI_IER_TXTFIE		BIT(4)
+#define SPI_IER_OVRIE		BIT(6)
+#define SPI_IER_MODFIE		BIT(9)
+#define SPI_IER_ALL		GENMASK(10, 0)
+
+/* STM32_SPI_SR bit fields */
+#define SPI_SR_RXP		BIT(0)
+#define SPI_SR_TXP		BIT(1)
+#define SPI_SR_EOT		BIT(3)
+#define SPI_SR_OVR		BIT(6)
+#define SPI_SR_MODF		BIT(9)
+#define SPI_SR_SUSP		BIT(11)
+#define SPI_SR_RXPLVL_SHIFT	13
+#define SPI_SR_RXPLVL		GENMASK(14, 13)
+#define SPI_SR_RXWNE		BIT(15)
+
+/* STM32_SPI_IFCR bit fields */
+#define SPI_IFCR_ALL		GENMASK(11, 3)
+
+/* STM32_SPI_I2SCFGR bit fields */
+#define SPI_I2SCFGR_I2SMOD	BIT(0)
+
+/* SPI Master Baud Rate min/max divisor */
+#define SPI_MBR_DIV_MIN		(2 << SPI_CFG1_MBR_MIN)
+#define SPI_MBR_DIV_MAX		(2 << SPI_CFG1_MBR_MAX)
+
+/* SPI Communication mode */
+#define SPI_FULL_DUPLEX		0
+#define SPI_SIMPLEX_TX		1
+#define SPI_SIMPLEX_RX		2
+#define SPI_HALF_DUPLEX		3
+
+#define SPI_1HZ_NS		1000000000
+
+/**
+ * struct stm32_spi - private data of the SPI controller
+ * @dev: driver model representation of the controller
+ * @master: controller master interface
+ * @base: virtual memory area
+ * @clk: hw kernel clock feeding the SPI clock generator
+ * @clk_rate: rate of the hw kernel clock feeding the SPI clock generator
+ * @rst: SPI controller reset line
+ * @lock: prevent I/O concurrent access
+ * @irq: SPI controller interrupt line
+ * @fifo_size: size of the embedded fifo in bytes
+ * @cur_midi: master inter-data idleness in ns
+ * @cur_speed: speed configured in Hz
+ * @cur_bpw: number of bits in a single SPI data frame
+ * @cur_fthlv: fifo threshold level (data frames in a single data packet)
+ * @cur_comm: SPI communication mode
+ * @cur_xferlen: current transfer length in bytes
+ * @cur_usedma: boolean to know if dma is used in current transfer
+ * @tx_buf: data to be written, or NULL
+ * @rx_buf: data to be read, or NULL
+ * @tx_len: number of data to be written in bytes
+ * @rx_len: number of data to be read in bytes
+ * @dma_tx: dma channel for TX transfer
+ * @dma_rx: dma channel for RX transfer
+ * @phys_addr: SPI registers physical base address
+ */
+struct stm32_spi {
+	struct device *dev;
+	struct spi_master *master;
+	void __iomem *base;
+	struct clk *clk;
+	u32 clk_rate;
+	struct reset_control *rst;
+	spinlock_t lock; /* prevent I/O concurrent access */
+	int irq;
+	unsigned int fifo_size;
+
+	unsigned int cur_midi;
+	unsigned int cur_speed;
+	unsigned int cur_bpw;
+	unsigned int cur_fthlv;
+	unsigned int cur_comm;
+	unsigned int cur_xferlen;
+	bool cur_usedma;
+
+	const void *tx_buf;
+	void *rx_buf;
+	int tx_len;
+	int rx_len;
+	struct dma_chan *dma_tx;
+	struct dma_chan *dma_rx;
+	dma_addr_t phys_addr;
+};
+
+static inline void stm32_spi_set_bits(struct stm32_spi *spi,
+				      u32 offset, u32 bits)
+{
+	writel_relaxed(readl_relaxed(spi->base + offset) | bits,
+		       spi->base + offset);
+}
+
+static inline void stm32_spi_clr_bits(struct stm32_spi *spi,
+				      u32 offset, u32 bits)
+{
+	writel_relaxed(readl_relaxed(spi->base + offset) & ~bits,
+		       spi->base + offset);
+}
+
+/**
+ * stm32_spi_get_fifo_size - Return fifo size
+ * @spi: pointer to the spi controller data structure
+ */
+static int stm32_spi_get_fifo_size(struct stm32_spi *spi)
+{
+	unsigned long flags;
+	u32 count = 0;
+
+	spin_lock_irqsave(&spi->lock, flags);
+
+	stm32_spi_set_bits(spi, STM32_SPI_CR1, SPI_CR1_SPE);
+
+	while (readl_relaxed(spi->base + STM32_SPI_SR) & SPI_SR_TXP)
+		writeb_relaxed(++count, spi->base + STM32_SPI_TXDR);
+
+	stm32_spi_clr_bits(spi, STM32_SPI_CR1, SPI_CR1_SPE);
+
+	spin_unlock_irqrestore(&spi->lock, flags);
+
+	dev_dbg(spi->dev, "%d x 8-bit fifo size\n", count);
+
+	return count;
+}
+
+/**
+ * stm32_spi_get_bpw_mask - Return bits per word mask
+ * @spi: pointer to the spi controller data structure
+ */
+static int stm32_spi_get_bpw_mask(struct stm32_spi *spi)
+{
+	unsigned long flags;
+	u32 cfg1, max_bpw;
+
+	spin_lock_irqsave(&spi->lock, flags);
+
+	/*
+	 * The most significant bit at DSIZE bit field is reserved when the
+	 * maximum data size of periperal instances is limited to 16-bit
+	 */
+	stm32_spi_set_bits(spi, STM32_SPI_CFG1, SPI_CFG1_DSIZE);
+
+	cfg1 = readl_relaxed(spi->base + STM32_SPI_CFG1);
+	max_bpw = (cfg1 & SPI_CFG1_DSIZE) >> SPI_CFG1_DSIZE_SHIFT;
+	max_bpw += 1;
+
+	spin_unlock_irqrestore(&spi->lock, flags);
+
+	dev_dbg(spi->dev, "%d-bit maximum data frame\n", max_bpw);
+
+	return SPI_BPW_RANGE_MASK(4, max_bpw);
+}
+
+/**
+ * stm32_spi_prepare_mbr - Determine SPI_CFG1.MBR value
+ * @spi: pointer to the spi controller data structure
+ * @speed_hz: requested speed
+ *
+ * Return SPI_CFG1.MBR value in case of success or -EINVAL
+ */
+static int stm32_spi_prepare_mbr(struct stm32_spi *spi, u32 speed_hz)
+{
+	u32 div, mbrdiv;
+
+	div = DIV_ROUND_UP(spi->clk_rate, speed_hz);
+
+	/*
+	 * SPI framework set xfer->speed_hz to master->max_speed_hz if
+	 * xfer->speed_hz is greater than master->max_speed_hz, and it returns
+	 * an error when xfer->speed_hz is lower than master->min_speed_hz, so
+	 * no need to check it there.
+	 * However, we need to ensure the following calculations.
+	 */
+	if ((div < SPI_MBR_DIV_MIN) &&
+	    (div > SPI_MBR_DIV_MAX))
+		return -EINVAL;
+
+	/* Determine the first power of 2 greater than or equal to div */
+	if (div & (div - 1))
+		mbrdiv = fls(div);
+	else
+		mbrdiv = fls(div) - 1;
+
+	spi->cur_speed = spi->clk_rate / (1 << mbrdiv);
+
+	return mbrdiv - 1;
+}
+
+/**
+ * stm32_spi_prepare_fthlv - Determine FIFO threshold level
+ * @spi: pointer to the spi controller data structure
+ */
+static u32 stm32_spi_prepare_fthlv(struct stm32_spi *spi)
+{
+	u32 fthlv, half_fifo;
+
+	/* data packet should not exceed 1/2 of fifo space */
+	half_fifo = (spi->fifo_size / 2);
+
+	if (spi->cur_bpw <= 8)
+		fthlv = half_fifo;
+	else if (spi->cur_bpw <= 16)
+		fthlv = half_fifo / 2;
+	else
+		fthlv = half_fifo / 4;
+
+	/* align packet size with data registers access */
+	if (spi->cur_bpw > 8)
+		fthlv -= (fthlv % 2); /* multiple of 2 */
+	else
+		fthlv -= (fthlv % 4); /* multiple of 4 */
+
+	return fthlv;
+}
+
+/**
+ * stm32_spi_write_txfifo - Write bytes in Transmit Data Register
+ * @spi: pointer to the spi controller data structure
+ *
+ * Read from tx_buf depends on remaining bytes to avoid to read beyond
+ * tx_buf end.
+ */
+static void stm32_spi_write_txfifo(struct stm32_spi *spi)
+{
+	while ((spi->tx_len > 0) &&
+	       (readl_relaxed(spi->base + STM32_SPI_SR) & SPI_SR_TXP)) {
+		u32 offs = spi->cur_xferlen - spi->tx_len;
+
+		if (spi->tx_len >= sizeof(u32)) {
+			const u32 *tx_buf32 = (const u32 *)(spi->tx_buf + offs);
+
+			writel_relaxed(*tx_buf32, spi->base + STM32_SPI_TXDR);
+			spi->tx_len -= sizeof(u32);
+		} else if (spi->tx_len >= sizeof(u16)) {
+			const u16 *tx_buf16 = (const u16 *)(spi->tx_buf + offs);
+
+			writew_relaxed(*tx_buf16, spi->base + STM32_SPI_TXDR);
+			spi->tx_len -= sizeof(u16);
+		} else {
+			const u8 *tx_buf8 = (const u8 *)(spi->tx_buf + offs);
+
+			writeb_relaxed(*tx_buf8, spi->base + STM32_SPI_TXDR);
+			spi->tx_len -= sizeof(u8);
+		}
+	}
+
+	dev_dbg(spi->dev, "%s: %d bytes left\n", __func__, spi->tx_len);
+}
+
+/**
+ * stm32_spi_read_rxfifo - Read bytes in Receive Data Register
+ * @spi: pointer to the spi controller data structure
+ *
+ * Write in rx_buf depends on remaining bytes to avoid to write beyond
+ * rx_buf end.
+ */
+static void stm32_spi_read_rxfifo(struct stm32_spi *spi, bool flush)
+{
+	u32 sr = readl_relaxed(spi->base + STM32_SPI_SR);
+	u32 rxplvl = (sr & SPI_SR_RXPLVL) >> SPI_SR_RXPLVL_SHIFT;
+
+	while ((spi->rx_len > 0) &&
+	       ((sr & SPI_SR_RXP) ||
+		(flush && ((sr & SPI_SR_RXWNE) || (rxplvl > 0))))) {
+		u32 offs = spi->cur_xferlen - spi->rx_len;
+
+		if ((spi->rx_len >= sizeof(u32)) ||
+		    (flush && (sr & SPI_SR_RXWNE))) {
+			u32 *rx_buf32 = (u32 *)(spi->rx_buf + offs);
+
+			*rx_buf32 = readl_relaxed(spi->base + STM32_SPI_RXDR);
+			spi->rx_len -= sizeof(u32);
+		} else if ((spi->rx_len >= sizeof(u16)) ||
+			   (flush && (rxplvl >= 2 || spi->cur_bpw > 8))) {
+			u16 *rx_buf16 = (u16 *)(spi->rx_buf + offs);
+
+			*rx_buf16 = readw_relaxed(spi->base + STM32_SPI_RXDR);
+			spi->rx_len -= sizeof(u16);
+		} else {
+			u8 *rx_buf8 = (u8 *)(spi->rx_buf + offs);
+
+			*rx_buf8 = readb_relaxed(spi->base + STM32_SPI_RXDR);
+			spi->rx_len -= sizeof(u8);
+		}
+
+		sr = readl_relaxed(spi->base + STM32_SPI_SR);
+		rxplvl = (sr & SPI_SR_RXPLVL) >> SPI_SR_RXPLVL_SHIFT;
+	}
+
+	dev_dbg(spi->dev, "%s%s: %d bytes left\n", __func__,
+		flush ? "(flush)" : "", spi->rx_len);
+}
+
+/**
+ * stm32_spi_enable - Enable SPI controller
+ * @spi: pointer to the spi controller data structure
+ *
+ * SPI data transfer is enabled but spi_ker_ck is idle.
+ * SPI_CFG1 and SPI_CFG2 are now write protected.
+ */
+static void stm32_spi_enable(struct stm32_spi *spi)
+{
+	dev_dbg(spi->dev, "enable controller\n");
+
+	stm32_spi_set_bits(spi, STM32_SPI_CR1, SPI_CR1_SPE);
+}
+
+/**
+ * stm32_spi_disable - Disable SPI controller
+ * @spi: pointer to the spi controller data structure
+ *
+ * RX-Fifo is flushed when SPI controller is disabled. To prevent any data
+ * loss, use stm32_spi_read_rxfifo(flush) to read the remaining bytes in
+ * RX-Fifo.
+ */
+static void stm32_spi_disable(struct stm32_spi *spi)
+{
+	unsigned long flags;
+	u32 cr1, sr;
+
+	dev_dbg(spi->dev, "disable controller\n");
+
+	spin_lock_irqsave(&spi->lock, flags);
+
+	cr1 = readl_relaxed(spi->base + STM32_SPI_CR1);
+
+	if (!(cr1 & SPI_CR1_SPE)) {
+		spin_unlock_irqrestore(&spi->lock, flags);
+		return;
+	}
+
+	/* Wait on EOT or suspend the flow */
+	if (readl_relaxed_poll_timeout_atomic(spi->base + STM32_SPI_SR,
+					      sr, !(sr & SPI_SR_EOT),
+					      10, 100000) < 0) {
+		if (cr1 & SPI_CR1_CSTART) {
+			writel_relaxed(cr1 | SPI_CR1_CSUSP,
+				       spi->base + STM32_SPI_CR1);
+			if (readl_relaxed_poll_timeout_atomic(
+						spi->base + STM32_SPI_SR,
+						sr, !(sr & SPI_SR_SUSP),
+						10, 100000) < 0)
+				dev_warn(spi->dev,
+					 "Suspend request timeout\n");
+		}
+	}
+
+	if (!spi->cur_usedma && spi->rx_buf && (spi->rx_len > 0))
+		stm32_spi_read_rxfifo(spi, true);
+
+	if (spi->cur_usedma && spi->tx_buf)
+		dmaengine_terminate_all(spi->dma_tx);
+	if (spi->cur_usedma && spi->rx_buf)
+		dmaengine_terminate_all(spi->dma_rx);
+
+	stm32_spi_clr_bits(spi, STM32_SPI_CR1, SPI_CR1_SPE);
+
+	stm32_spi_clr_bits(spi, STM32_SPI_CFG1, SPI_CFG1_TXDMAEN |
+						SPI_CFG1_RXDMAEN);
+
+	/* Disable interrupts and clear status flags */
+	writel_relaxed(0, spi->base + STM32_SPI_IER);
+	writel_relaxed(SPI_IFCR_ALL, spi->base + STM32_SPI_IFCR);
+
+	spin_unlock_irqrestore(&spi->lock, flags);
+}
+
+/**
+ * stm32_spi_can_dma - Determine if the transfer is eligible for DMA use
+ *
+ * If the current transfer size is greater than fifo size, use DMA.
+ */
+static bool stm32_spi_can_dma(struct spi_master *master,
+			      struct spi_device *spi_dev,
+			      struct spi_transfer *transfer)
+{
+	struct stm32_spi *spi = spi_master_get_devdata(master);
+
+	dev_dbg(spi->dev, "%s: %s\n", __func__,
+		(transfer->len > spi->fifo_size) ? "true" : "false");
+
+	return (transfer->len > spi->fifo_size);
+}
+
+/**
+ * stm32_spi_irq - Interrupt handler for SPI controller events
+ * @irq: interrupt line
+ * @dev_id: SPI controller master interface
+ */
+static irqreturn_t stm32_spi_irq(int irq, void *dev_id)
+{
+	struct spi_master *master = dev_id;
+	struct stm32_spi *spi = spi_master_get_devdata(master);
+	u32 sr, ier, mask;
+	unsigned long flags;
+	bool end = false;
+
+	spin_lock_irqsave(&spi->lock, flags);
+
+	sr = readl_relaxed(spi->base + STM32_SPI_SR);
+	ier = readl_relaxed(spi->base + STM32_SPI_IER);
+
+	mask = ier;
+	/* EOTIE is triggered on EOT, SUSP and TXC events. */
+	mask |= SPI_SR_SUSP;
+	/*
+	 * When TXTF is set, DXPIE and TXPIE are cleared. So in case of
+	 * Full-Duplex, need to poll RXP event to know if there are remaining
+	 * data, before disabling SPI.
+	 */
+	if (spi->rx_buf && !spi->cur_usedma)
+		mask |= SPI_SR_RXP;
+
+	if (!(sr & mask)) {
+		dev_dbg(spi->dev, "spurious IT (sr=0x%08x, ier=0x%08x)\n",
+			sr, ier);
+		spin_unlock_irqrestore(&spi->lock, flags);
+		return IRQ_NONE;
+	}
+
+	if (sr & SPI_SR_SUSP) {
+		dev_warn(spi->dev, "Communication suspended\n");
+		if (!spi->cur_usedma && (spi->rx_buf && (spi->rx_len > 0)))
+			stm32_spi_read_rxfifo(spi, false);
+		/*
+		 * If communication is suspended while using DMA, it means
+		 * that something went wrong, so stop the current transfer
+		 */
+		if (spi->cur_usedma)
+			end = true;
+	}
+
+	if (sr & SPI_SR_MODF) {
+		dev_warn(spi->dev, "Mode fault: transfer aborted\n");
+		end = true;
+	}
+
+	if (sr & SPI_SR_OVR) {
+		dev_warn(spi->dev, "Overrun: received value discarded\n");
+		if (!spi->cur_usedma && (spi->rx_buf && (spi->rx_len > 0)))
+			stm32_spi_read_rxfifo(spi, false);
+		/*
+		 * If overrun is detected while using DMA, it means that
+		 * something went wrong, so stop the current transfer
+		 */
+		if (spi->cur_usedma)
+			end = true;
+	}
+
+	if (sr & SPI_SR_EOT) {
+		if (!spi->cur_usedma && (spi->rx_buf && (spi->rx_len > 0)))
+			stm32_spi_read_rxfifo(spi, true);
+		end = true;
+	}
+
+	if (sr & SPI_SR_TXP)
+		if (!spi->cur_usedma && (spi->tx_buf && (spi->tx_len > 0)))
+			stm32_spi_write_txfifo(spi);
+
+	if (sr & SPI_SR_RXP)
+		if (!spi->cur_usedma && (spi->rx_buf && (spi->rx_len > 0)))
+			stm32_spi_read_rxfifo(spi, false);
+
+	writel_relaxed(mask, spi->base + STM32_SPI_IFCR);
+
+	spin_unlock_irqrestore(&spi->lock, flags);
+
+	if (end) {
+		spi_finalize_current_transfer(master);
+		stm32_spi_disable(spi);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * stm32_spi_setup - setup device chip select
+ */
+static int stm32_spi_setup(struct spi_device *spi_dev)
+{
+	int ret = 0;
+
+	if (!gpio_is_valid(spi_dev->cs_gpio)) {
+		dev_err(&spi_dev->dev, "%d is not a valid gpio\n",
+			spi_dev->cs_gpio);
+		return -EINVAL;
+	}
+
+	dev_dbg(&spi_dev->dev, "%s: set gpio%d output %s\n", __func__,
+		spi_dev->cs_gpio,
+		(spi_dev->mode & SPI_CS_HIGH) ? "low" : "high");
+
+	ret = gpio_direction_output(spi_dev->cs_gpio,
+				    !(spi_dev->mode & SPI_CS_HIGH));
+
+	return ret;
+}
+
+/**
+ * stm32_spi_prepare_msg - set up the controller to transfer a single message
+ */
+static int stm32_spi_prepare_msg(struct spi_master *master,
+				 struct spi_message *msg)
+{
+	struct stm32_spi *spi = spi_master_get_devdata(master);
+	struct spi_device *spi_dev = msg->spi;
+	struct device_node *np = spi_dev->dev.of_node;
+	unsigned long flags;
+	u32 cfg2_clrb = 0, cfg2_setb = 0;
+
+	/* SPI slave device may need time between data frames */
+	spi->cur_midi = 0;
+	if (np && !of_property_read_u32(np, "st,spi-midi-ns", &spi->cur_midi))
+		dev_dbg(spi->dev, "%dns inter-data idleness\n", spi->cur_midi);
+
+	if (spi_dev->mode & SPI_CPOL)
+		cfg2_setb |= SPI_CFG2_CPOL;
+	else
+		cfg2_clrb |= SPI_CFG2_CPOL;
+
+	if (spi_dev->mode & SPI_CPHA)
+		cfg2_setb |= SPI_CFG2_CPHA;
+	else
+		cfg2_clrb |= SPI_CFG2_CPHA;
+
+	if (spi_dev->mode & SPI_LSB_FIRST)
+		cfg2_setb |= SPI_CFG2_LSBFRST;
+	else
+		cfg2_clrb |= SPI_CFG2_LSBFRST;
+
+	dev_dbg(spi->dev, "cpol=%d cpha=%d lsb_first=%d cs_high=%d\n",
+		spi_dev->mode & SPI_CPOL,
+		spi_dev->mode & SPI_CPHA,
+		spi_dev->mode & SPI_LSB_FIRST,
+		spi_dev->mode & SPI_CS_HIGH);
+
+	spin_lock_irqsave(&spi->lock, flags);
+
+	if (cfg2_clrb || cfg2_setb)
+		writel_relaxed(
+			(readl_relaxed(spi->base + STM32_SPI_CFG2) &
+				~cfg2_clrb) | cfg2_setb,
+			       spi->base + STM32_SPI_CFG2);
+
+	spin_unlock_irqrestore(&spi->lock, flags);
+
+	return 0;
+}
+
+/**
+ * stm32_spi_dma_cb - dma callback
+ *
+ * DMA callback is called when the transfer is complete or when an error
+ * occurs. If the transfer is complete, EOT flag is raised.
+ */
+static void stm32_spi_dma_cb(void *data)
+{
+	struct stm32_spi *spi = data;
+	unsigned long flags;
+	u32 sr;
+
+	spin_lock_irqsave(&spi->lock, flags);
+
+	sr = readl_relaxed(spi->base + STM32_SPI_SR);
+
+	spin_unlock_irqrestore(&spi->lock, flags);
+
+	if (!(sr & SPI_SR_EOT))
+		dev_warn(spi->dev, "DMA error (sr=0x%08x)\n", sr);
+
+	/* Now wait for EOT, or SUSP or OVR in case of error */
+}
+
+/**
+ * stm32_spi_dma_config - configure dma slave channel depending on current
+ *			  transfer bits_per_word.
+ */
+static void stm32_spi_dma_config(struct stm32_spi *spi,
+				 struct dma_slave_config *dma_conf,
+				 enum dma_transfer_direction dir)
+{
+	enum dma_slave_buswidth buswidth;
+	u32 maxburst;
+
+	if (spi->cur_bpw <= 8)
+		buswidth = DMA_SLAVE_BUSWIDTH_1_BYTE;
+	else if (spi->cur_bpw <= 16)
+		buswidth = DMA_SLAVE_BUSWIDTH_2_BYTES;
+	else
+		buswidth = DMA_SLAVE_BUSWIDTH_4_BYTES;
+
+	/* Valid for DMA Half or Full Fifo threshold */
+	if (spi->cur_fthlv == 2)
+		maxburst = 1;
+	else
+		maxburst = spi->cur_fthlv;
+
+	memset(dma_conf, 0, sizeof(struct dma_slave_config));
+	dma_conf->direction = dir;
+	if (dma_conf->direction == DMA_DEV_TO_MEM) { /* RX */
+		dma_conf->src_addr = spi->phys_addr + STM32_SPI_RXDR;
+		dma_conf->src_addr_width = buswidth;
+		dma_conf->src_maxburst = maxburst;
+
+		dev_dbg(spi->dev, "Rx DMA config buswidth=%d, maxburst=%d\n",
+			buswidth, maxburst);
+	} else if (dma_conf->direction == DMA_MEM_TO_DEV) { /* TX */
+		dma_conf->dst_addr = spi->phys_addr + STM32_SPI_TXDR;
+		dma_conf->dst_addr_width = buswidth;
+		dma_conf->dst_maxburst = maxburst;
+
+		dev_dbg(spi->dev, "Tx DMA config buswidth=%d, maxburst=%d\n",
+			buswidth, maxburst);
+	}
+}
+
+/**
+ * stm32_spi_transfer_one_irq - transfer a single spi_transfer using
+ *				interrupts
+ *
+ * It must returns 0 if the transfer is finished or 1 if the transfer is still
+ * in progress.
+ */
+static int stm32_spi_transfer_one_irq(struct stm32_spi *spi)
+{
+	unsigned long flags;
+	u32 ier = 0;
+
+	/* Enable the interrupts relative to the current communication mode */
+	if (spi->tx_buf && spi->rx_buf)	/* Full Duplex */
+		ier |= SPI_IER_DXPIE;
+	else if (spi->tx_buf)		/* Half-Duplex TX dir or Simplex TX */
+		ier |= SPI_IER_TXPIE;
+	else if (spi->rx_buf)		/* Half-Duplex RX dir or Simplex RX */
+		ier |= SPI_IER_RXPIE;
+
+	/* Enable the interrupts relative to the end of transfer */
+	ier |= SPI_IER_EOTIE | SPI_IER_TXTFIE |	SPI_IER_OVRIE |	SPI_IER_MODFIE;
+
+	spin_lock_irqsave(&spi->lock, flags);
+
+	stm32_spi_enable(spi);
+
+	/* Be sure to have data in fifo before starting data transfer */
+	if (spi->tx_buf)
+		stm32_spi_write_txfifo(spi);
+
+	stm32_spi_set_bits(spi, STM32_SPI_CR1, SPI_CR1_CSTART);
+
+	writel_relaxed(ier, spi->base + STM32_SPI_IER);
+
+	spin_unlock_irqrestore(&spi->lock, flags);
+
+	return 1;
+}
+
+/**
+ * stm32_spi_transfer_one_dma - transfer a single spi_transfer using DMA
+ *
+ * It must returns 0 if the transfer is finished or 1 if the transfer is still
+ * in progress.
+ */
+static int stm32_spi_transfer_one_dma(struct stm32_spi *spi,
+				      struct spi_transfer *xfer)
+{
+	struct dma_slave_config tx_dma_conf, rx_dma_conf;
+	struct dma_async_tx_descriptor *tx_dma_desc, *rx_dma_desc;
+	unsigned long flags;
+	u32 ier = 0;
+
+	spin_lock_irqsave(&spi->lock, flags);
+
+	rx_dma_desc = NULL;
+	if (spi->rx_buf) {
+		stm32_spi_dma_config(spi, &rx_dma_conf, DMA_DEV_TO_MEM);
+		dmaengine_slave_config(spi->dma_rx, &rx_dma_conf);
+
+		/* Enable Rx DMA request */
+		stm32_spi_set_bits(spi, STM32_SPI_CFG1, SPI_CFG1_RXDMAEN);
+
+		rx_dma_desc = dmaengine_prep_slave_sg(
+					spi->dma_rx, xfer->rx_sg.sgl,
+					xfer->rx_sg.nents,
+					rx_dma_conf.direction,
+					DMA_PREP_INTERRUPT);
+	}
+
+	tx_dma_desc = NULL;
+	if (spi->tx_buf) {
+		stm32_spi_dma_config(spi, &tx_dma_conf, DMA_MEM_TO_DEV);
+		dmaengine_slave_config(spi->dma_tx, &tx_dma_conf);
+
+		tx_dma_desc = dmaengine_prep_slave_sg(
+					spi->dma_tx, xfer->tx_sg.sgl,
+					xfer->tx_sg.nents,
+					tx_dma_conf.direction,
+					DMA_PREP_INTERRUPT);
+	}
+
+	if ((spi->tx_buf && !tx_dma_desc) ||
+	    (spi->rx_buf && !rx_dma_desc))
+		goto dma_desc_error;
+
+	if (rx_dma_desc) {
+		rx_dma_desc->callback = stm32_spi_dma_cb;
+		rx_dma_desc->callback_param = spi;
+
+		if (dma_submit_error(dmaengine_submit(rx_dma_desc))) {
+			dev_err(spi->dev, "Rx DMA submit failed\n");
+			goto dma_desc_error;
+		}
+		/* Enable Rx DMA channel */
+		dma_async_issue_pending(spi->dma_rx);
+	}
+
+	if (tx_dma_desc) {
+		if (spi->cur_comm == SPI_SIMPLEX_TX) {
+			tx_dma_desc->callback = stm32_spi_dma_cb;
+			tx_dma_desc->callback_param = spi;
+		}
+
+		if (dma_submit_error(dmaengine_submit(tx_dma_desc))) {
+			dev_err(spi->dev, "Tx DMA submit failed\n");
+			goto dma_submit_error;
+		}
+		/* Enable Tx DMA channel */
+		dma_async_issue_pending(spi->dma_tx);
+
+		/* Enable Tx DMA request */
+		stm32_spi_set_bits(spi, STM32_SPI_CFG1, SPI_CFG1_TXDMAEN);
+	}
+
+	/* Enable the interrupts relative to the end of transfer */
+	ier |= SPI_IER_EOTIE | SPI_IER_TXTFIE |	SPI_IER_OVRIE |	SPI_IER_MODFIE;
+	writel_relaxed(ier, spi->base + STM32_SPI_IER);
+
+	stm32_spi_enable(spi);
+
+	stm32_spi_set_bits(spi, STM32_SPI_CR1, SPI_CR1_CSTART);
+
+	spin_unlock_irqrestore(&spi->lock, flags);
+
+	return 1;
+
+dma_submit_error:
+	if (spi->rx_buf)
+		dmaengine_terminate_all(spi->dma_rx);
+
+dma_desc_error:
+	stm32_spi_clr_bits(spi, STM32_SPI_CFG1, SPI_CFG1_RXDMAEN);
+
+	spin_unlock_irqrestore(&spi->lock, flags);
+
+	dev_info(spi->dev, "DMA issue: fall back to irq transfer\n");
+
+	return stm32_spi_transfer_one_irq(spi);
+}
+
+/**
+ * stm32_spi_transfer_one_setup - common setup to transfer a single
+ *				  spi_transfer either using DMA or
+ *				  interrupts.
+ */
+static int stm32_spi_transfer_one_setup(struct stm32_spi *spi,
+					struct spi_device *spi_dev,
+					struct spi_transfer *transfer)
+{
+	unsigned long flags;
+	u32 cfg1_clrb = 0, cfg1_setb = 0, cfg2_clrb = 0, cfg2_setb = 0;
+	u32 mode, nb_words;
+	int ret = 0;
+
+	spin_lock_irqsave(&spi->lock, flags);
+
+	if (spi->cur_bpw != transfer->bits_per_word) {
+		u32 bpw, fthlv;
+
+		spi->cur_bpw = transfer->bits_per_word;
+		bpw = spi->cur_bpw - 1;
+
+		cfg1_clrb |= SPI_CFG1_DSIZE;
+		cfg1_setb |= (bpw << SPI_CFG1_DSIZE_SHIFT) & SPI_CFG1_DSIZE;
+
+		spi->cur_fthlv = stm32_spi_prepare_fthlv(spi);
+		fthlv = spi->cur_fthlv - 1;
+
+		cfg1_clrb |= SPI_CFG1_FTHLV;
+		cfg1_setb |= (fthlv << SPI_CFG1_FTHLV_SHIFT) & SPI_CFG1_FTHLV;
+	}
+
+	if (spi->cur_speed != transfer->speed_hz) {
+		int mbr;
+
+		/* Update spi->cur_speed with real clock speed */
+		mbr = stm32_spi_prepare_mbr(spi, transfer->speed_hz);
+		if (mbr < 0) {
+			ret = mbr;
+			goto out;
+		}
+
+		transfer->speed_hz = spi->cur_speed;
+
+		cfg1_clrb |= SPI_CFG1_MBR;
+		cfg1_setb |= ((u32)mbr << SPI_CFG1_MBR_SHIFT) & SPI_CFG1_MBR;
+	}
+
+	if (cfg1_clrb || cfg1_setb)
+		writel_relaxed((readl_relaxed(spi->base + STM32_SPI_CFG1) &
+				~cfg1_clrb) | cfg1_setb,
+			       spi->base + STM32_SPI_CFG1);
+
+	mode = SPI_FULL_DUPLEX;
+	if (spi_dev->mode & SPI_3WIRE) { /* MISO/MOSI signals shared */
+		/*
+		 * SPI_3WIRE and xfer->tx_buf != NULL and xfer->rx_buf != NULL
+		 * is forbidden und unvalidated by SPI subsystem so depending
+		 * on the valid buffer, we can determine the direction of the
+		 * transfer.
+		 */
+		mode = SPI_HALF_DUPLEX;
+		if (!transfer->tx_buf)
+			stm32_spi_clr_bits(spi, STM32_SPI_CR1, SPI_CR1_HDDIR);
+		else if (!transfer->rx_buf)
+			stm32_spi_set_bits(spi, STM32_SPI_CR1, SPI_CR1_HDDIR);
+	} else {
+		if (!transfer->tx_buf)
+			mode = SPI_SIMPLEX_RX;
+		else if (!transfer->rx_buf)
+			mode = SPI_SIMPLEX_TX;
+	}
+	if (spi->cur_comm != mode) {
+		spi->cur_comm = mode;
+
+		cfg2_clrb |= SPI_CFG2_COMM;
+		cfg2_setb |= (mode << SPI_CFG2_COMM_SHIFT) & SPI_CFG2_COMM;
+	}
+
+	cfg2_clrb |= SPI_CFG2_MIDI;
+	if ((transfer->len > 1) && (spi->cur_midi > 0)) {
+		u32 sck_period_ns = DIV_ROUND_UP(SPI_1HZ_NS, spi->cur_speed);
+		u32 midi = min((u32)DIV_ROUND_UP(spi->cur_midi, sck_period_ns),
+			       (u32)SPI_CFG2_MIDI >> SPI_CFG2_MIDI_SHIFT);
+
+		dev_dbg(spi->dev, "period=%dns, midi=%d(=%dns)\n",
+			sck_period_ns, midi, midi * sck_period_ns);
+
+		cfg2_setb |= (midi << SPI_CFG2_MIDI_SHIFT) & SPI_CFG2_MIDI;
+	}
+
+	if (cfg2_clrb || cfg2_setb)
+		writel_relaxed((readl_relaxed(spi->base + STM32_SPI_CFG2) &
+				~cfg2_clrb) | cfg2_setb,
+			       spi->base + STM32_SPI_CFG2);
+
+	if (spi->cur_bpw <= 8)
+		nb_words = transfer->len;
+	else if (spi->cur_bpw <= 16)
+		nb_words = DIV_ROUND_UP(transfer->len * 8, 16);
+	else
+		nb_words = DIV_ROUND_UP(transfer->len * 8, 32);
+	nb_words <<= SPI_CR2_TSIZE_SHIFT;
+
+	if (nb_words <= SPI_CR2_TSIZE) {
+		writel_relaxed(nb_words, spi->base + STM32_SPI_CR2);
+	} else {
+		ret = -EMSGSIZE;
+		goto out;
+	}
+
+	spi->cur_xferlen = transfer->len;
+
+	dev_dbg(spi->dev, "transfer communication mode set to %d\n",
+		spi->cur_comm);
+	dev_dbg(spi->dev,
+		"data frame of %d-bit, data packet of %d data frames\n",
+		spi->cur_bpw, spi->cur_fthlv);
+	dev_dbg(spi->dev, "speed set to %dHz\n", spi->cur_speed);
+	dev_dbg(spi->dev, "transfer of %d bytes (%d data frames)\n",
+		spi->cur_xferlen, nb_words);
+	dev_dbg(spi->dev, "dma %s\n",
+		(spi->cur_usedma) ? "enabled" : "disabled");
+
+out:
+	spin_unlock_irqrestore(&spi->lock, flags);
+
+	return ret;
+}
+
+/**
+ * stm32_spi_transfer_one - transfer a single spi_transfer
+ *
+ * It must return 0 if the transfer is finished or 1 if the transfer is still
+ * in progress.
+ */
+static int stm32_spi_transfer_one(struct spi_master *master,
+				  struct spi_device *spi_dev,
+				  struct spi_transfer *transfer)
+{
+	struct stm32_spi *spi = spi_master_get_devdata(master);
+	int ret;
+
+	spi->tx_buf = transfer->tx_buf;
+	spi->rx_buf = transfer->rx_buf;
+	spi->tx_len = spi->tx_buf ? transfer->len : 0;
+	spi->rx_len = spi->rx_buf ? transfer->len : 0;
+
+	spi->cur_usedma = (master->can_dma &&
+			   stm32_spi_can_dma(master, spi_dev, transfer));
+
+	ret = stm32_spi_transfer_one_setup(spi, spi_dev, transfer);
+	if (ret) {
+		dev_err(spi->dev, "SPI transfer setup failed\n");
+		return ret;
+	}
+
+	if (spi->cur_usedma)
+		return stm32_spi_transfer_one_dma(spi, transfer);
+	else
+		return stm32_spi_transfer_one_irq(spi);
+}
+
+/**
+ * stm32_spi_unprepare_msg - relax the hardware
+ *
+ * Normally, if TSIZE has been configured, we should relax the hardware at the
+ * reception of the EOT interrupt. But in case of error, EOT will not be
+ * raised. So the subsystem unprepare_message call allows us to properly
+ * complete the transfer from an hardware point of view.
+ */
+static int stm32_spi_unprepare_msg(struct spi_master *master,
+				   struct spi_message *msg)
+{
+	struct stm32_spi *spi = spi_master_get_devdata(master);
+
+	stm32_spi_disable(spi);
+
+	return 0;
+}
+
+/**
+ * stm32_spi_config - Configure SPI controller as SPI master
+ */
+static int stm32_spi_config(struct stm32_spi *spi)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&spi->lock, flags);
+
+	/* Ensure I2SMOD bit is kept cleared */
+	stm32_spi_clr_bits(spi, STM32_SPI_I2SCFGR, SPI_I2SCFGR_I2SMOD);
+
+	/*
+	 * - SS input value high
+	 * - transmitter half duplex direction
+	 * - automatic communication suspend when RX-Fifo is full
+	 */
+	stm32_spi_set_bits(spi, STM32_SPI_CR1, SPI_CR1_SSI |
+					       SPI_CR1_HDDIR |
+					       SPI_CR1_MASRX);
+
+	/*
+	 * - Set the master mode (default Motorola mode)
+	 * - Consider 1 master/n slaves configuration and
+	 *   SS input value is determined by the SSI bit
+	 * - keep control of all associated GPIOs
+	 */
+	stm32_spi_set_bits(spi, STM32_SPI_CFG2, SPI_CFG2_MASTER |
+						SPI_CFG2_SSM |
+						SPI_CFG2_AFCNTR);
+
+	spin_unlock_irqrestore(&spi->lock, flags);
+
+	return 0;
+}
+
+static const struct of_device_id stm32_spi_of_match[] = {
+	{ .compatible = "st,stm32h7-spi", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, stm32_spi_of_match);
+
+static int stm32_spi_probe(struct platform_device *pdev)
+{
+	struct spi_master *master;
+	struct stm32_spi *spi;
+	struct resource *res;
+	int i, ret;
+
+	master = spi_alloc_master(&pdev->dev, sizeof(struct stm32_spi));
+	if (!master) {
+		dev_err(&pdev->dev, "spi master allocation failed\n");
+		return -ENOMEM;
+	}
+	platform_set_drvdata(pdev, master);
+
+	spi = spi_master_get_devdata(master);
+	spi->dev = &pdev->dev;
+	spi->master = master;
+	spin_lock_init(&spi->lock);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	spi->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(spi->base)) {
+		ret = PTR_ERR(spi->base);
+		goto err_master_put;
+	}
+	spi->phys_addr = (dma_addr_t)res->start;
+
+	spi->irq = platform_get_irq(pdev, 0);
+	if (spi->irq <= 0) {
+		dev_err(&pdev->dev, "no irq: %d\n", spi->irq);
+		ret = -ENOENT;
+		goto err_master_put;
+	}
+	ret = devm_request_threaded_irq(&pdev->dev, spi->irq, NULL,
+					stm32_spi_irq, IRQF_ONESHOT,
+					pdev->name, master);
+	if (ret) {
+		dev_err(&pdev->dev, "irq%d request failed: %d\n", spi->irq,
+			ret);
+		goto err_master_put;
+	}
+
+	spi->clk = devm_clk_get(&pdev->dev, 0);
+	if (IS_ERR(spi->clk)) {
+		ret = PTR_ERR(spi->clk);
+		dev_err(&pdev->dev, "clk get failed: %d\n", ret);
+		goto err_master_put;
+	}
+
+	ret = clk_prepare_enable(spi->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "clk enable failed: %d\n", ret);
+		goto err_master_put;
+	}
+	spi->clk_rate = clk_get_rate(spi->clk);
+	if (!spi->clk_rate) {
+		dev_err(&pdev->dev, "clk rate = 0\n");
+		ret = -EINVAL;
+		goto err_master_put;
+	}
+
+	spi->rst = devm_reset_control_get(&pdev->dev, NULL);
+	if (!IS_ERR(spi->rst)) {
+		reset_control_assert(spi->rst);
+		udelay(2);
+		reset_control_deassert(spi->rst);
+	}
+
+	spi->fifo_size = stm32_spi_get_fifo_size(spi);
+
+	ret = stm32_spi_config(spi);
+	if (ret) {
+		dev_err(&pdev->dev, "controller configuration failed: %d\n",
+			ret);
+		goto err_clk_disable;
+	}
+
+	master->dev.of_node = pdev->dev.of_node;
+	master->auto_runtime_pm = true;
+	master->bus_num = pdev->id;
+	master->mode_bits = SPI_MODE_3 | SPI_CS_HIGH | SPI_LSB_FIRST |
+			    SPI_3WIRE | SPI_LOOP;
+	master->bits_per_word_mask = stm32_spi_get_bpw_mask(spi);
+	master->max_speed_hz = spi->clk_rate / SPI_MBR_DIV_MIN;
+	master->min_speed_hz = spi->clk_rate / SPI_MBR_DIV_MAX;
+	master->setup = stm32_spi_setup;
+	master->prepare_message = stm32_spi_prepare_msg;
+	master->transfer_one = stm32_spi_transfer_one;
+	master->unprepare_message = stm32_spi_unprepare_msg;
+
+	spi->dma_tx = dma_request_slave_channel(spi->dev, "tx");
+	if (!spi->dma_tx)
+		dev_warn(&pdev->dev, "failed to request tx dma channel\n");
+	else
+		master->dma_tx = spi->dma_tx;
+
+	spi->dma_rx = dma_request_slave_channel(spi->dev, "rx");
+	if (!spi->dma_rx)
+		dev_warn(&pdev->dev, "failed to request rx dma channel\n");
+	else
+		master->dma_rx = spi->dma_rx;
+
+	if (spi->dma_tx || spi->dma_rx)
+		master->can_dma = stm32_spi_can_dma;
+
+	pm_runtime_set_active(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
+	ret = devm_spi_register_master(&pdev->dev, master);
+	if (ret) {
+		dev_err(&pdev->dev, "spi master registration failed: %d\n",
+			ret);
+		goto err_dma_release;
+	}
+
+	if (!master->cs_gpios) {
+		dev_err(&pdev->dev, "no CS gpios available\n");
+		ret = -EINVAL;
+		goto err_dma_release;
+	}
+
+	for (i = 0; i < master->num_chipselect; i++) {
+		if (!gpio_is_valid(master->cs_gpios[i])) {
+			dev_err(&pdev->dev, "%i is not a valid gpio\n",
+				master->cs_gpios[i]);
+			ret = -EINVAL;
+			goto err_dma_release;
+		}
+
+		ret = devm_gpio_request(&pdev->dev, master->cs_gpios[i],
+					DRIVER_NAME);
+		if (ret) {
+			dev_err(&pdev->dev, "can't get CS gpio %i\n",
+				master->cs_gpios[i]);
+			goto err_dma_release;
+		}
+	}
+
+	dev_info(&pdev->dev, "driver initialized\n");
+
+	return 0;
+
+err_dma_release:
+	if (spi->dma_tx)
+		dma_release_channel(spi->dma_tx);
+	if (spi->dma_rx)
+		dma_release_channel(spi->dma_rx);
+
+	pm_runtime_disable(&pdev->dev);
+err_clk_disable:
+	clk_disable_unprepare(spi->clk);
+err_master_put:
+	spi_master_put(master);
+
+	return ret;
+}
+
+static int stm32_spi_remove(struct platform_device *pdev)
+{
+	struct spi_master *master = platform_get_drvdata(pdev);
+	struct stm32_spi *spi = spi_master_get_devdata(master);
+
+	stm32_spi_disable(spi);
+
+	if (master->dma_tx)
+		dma_release_channel(master->dma_tx);
+	if (master->dma_rx)
+		dma_release_channel(master->dma_rx);
+
+	clk_disable_unprepare(spi->clk);
+
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int stm32_spi_runtime_suspend(struct device *dev)
+{
+	struct spi_master *master = dev_get_drvdata(dev);
+	struct stm32_spi *spi = spi_master_get_devdata(master);
+
+	clk_disable_unprepare(spi->clk);
+
+	return 0;
+}
+
+static int stm32_spi_runtime_resume(struct device *dev)
+{
+	struct spi_master *master = dev_get_drvdata(dev);
+	struct stm32_spi *spi = spi_master_get_devdata(master);
+
+	return clk_prepare_enable(spi->clk);
+}
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+static int stm32_spi_suspend(struct device *dev)
+{
+	struct spi_master *master = dev_get_drvdata(dev);
+	int ret;
+
+	ret = spi_master_suspend(master);
+	if (ret)
+		return ret;
+
+	return pm_runtime_force_suspend(dev);
+}
+
+static int stm32_spi_resume(struct device *dev)
+{
+	struct spi_master *master = dev_get_drvdata(dev);
+	struct stm32_spi *spi = spi_master_get_devdata(master);
+	int ret;
+
+	ret = pm_runtime_force_resume(dev);
+	if (ret)
+		return ret;
+
+	ret = spi_master_resume(master);
+	if (ret)
+		clk_disable_unprepare(spi->clk);
+
+	return ret;
+}
+#endif
+
+static const struct dev_pm_ops stm32_spi_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(stm32_spi_suspend, stm32_spi_resume)
+	SET_RUNTIME_PM_OPS(stm32_spi_runtime_suspend,
+			   stm32_spi_runtime_resume, NULL)
+};
+
+static struct platform_driver stm32_spi_driver = {
+	.probe = stm32_spi_probe,
+	.remove = stm32_spi_remove,
+	.driver = {
+		.name = DRIVER_NAME,
+		.pm = &stm32_spi_pm_ops,
+		.of_match_table = stm32_spi_of_match,
+	},
+};
+
+module_platform_driver(stm32_spi_driver);
+
+MODULE_ALIAS("platform:" DRIVER_NAME);
+MODULE_DESCRIPTION("STMicroelectronics STM32 SPI Controller driver");
+MODULE_AUTHOR("Amelie Delaunay <amelie.delaunay@st.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index 89254a55eb2e..4fcbb0aa71d3 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -48,11 +48,11 @@ static void spidev_release(struct device *dev)
 {
 	struct spi_device	*spi = to_spi_device(dev);
 
-	/* spi masters may cleanup for released devices */
-	if (spi->master->cleanup)
-		spi->master->cleanup(spi);
+	/* spi controllers may cleanup for released devices */
+	if (spi->controller->cleanup)
+		spi->controller->cleanup(spi);
 
-	spi_master_put(spi->master);
+	spi_controller_put(spi->controller);
 	kfree(spi);
 }
 
@@ -71,17 +71,17 @@ modalias_show(struct device *dev, struct device_attribute *a, char *buf)
 static DEVICE_ATTR_RO(modalias);
 
 #define SPI_STATISTICS_ATTRS(field, file)				\
-static ssize_t spi_master_##field##_show(struct device *dev,		\
-					 struct device_attribute *attr,	\
-					 char *buf)			\
+static ssize_t spi_controller_##field##_show(struct device *dev,	\
+					     struct device_attribute *attr, \
+					     char *buf)			\
 {									\
-	struct spi_master *master = container_of(dev,			\
-						 struct spi_master, dev); \
-	return spi_statistics_##field##_show(&master->statistics, buf);	\
+	struct spi_controller *ctlr = container_of(dev,			\
+					 struct spi_controller, dev);	\
+	return spi_statistics_##field##_show(&ctlr->statistics, buf);	\
 }									\
-static struct device_attribute dev_attr_spi_master_##field = {		\
-	.attr = { .name = file, .mode = S_IRUGO },			\
-	.show = spi_master_##field##_show,				\
+static struct device_attribute dev_attr_spi_controller_##field = {	\
+	.attr = { .name = file, .mode = 0444 },				\
+	.show = spi_controller_##field##_show,				\
 };									\
 static ssize_t spi_device_##field##_show(struct device *dev,		\
 					 struct device_attribute *attr,	\
@@ -91,7 +91,7 @@ static ssize_t spi_device_##field##_show(struct device *dev,		\
 	return spi_statistics_##field##_show(&spi->statistics, buf);	\
 }									\
 static struct device_attribute dev_attr_spi_device_##field = {		\
-	.attr = { .name = file, .mode = S_IRUGO },			\
+	.attr = { .name = file, .mode = 0444 },				\
 	.show = spi_device_##field##_show,				\
 }
 
@@ -201,51 +201,51 @@ static const struct attribute_group *spi_dev_groups[] = {
 	NULL,
 };
 
-static struct attribute *spi_master_statistics_attrs[] = {
-	&dev_attr_spi_master_messages.attr,
-	&dev_attr_spi_master_transfers.attr,
-	&dev_attr_spi_master_errors.attr,
-	&dev_attr_spi_master_timedout.attr,
-	&dev_attr_spi_master_spi_sync.attr,
-	&dev_attr_spi_master_spi_sync_immediate.attr,
-	&dev_attr_spi_master_spi_async.attr,
-	&dev_attr_spi_master_bytes.attr,
-	&dev_attr_spi_master_bytes_rx.attr,
-	&dev_attr_spi_master_bytes_tx.attr,
-	&dev_attr_spi_master_transfer_bytes_histo0.attr,
-	&dev_attr_spi_master_transfer_bytes_histo1.attr,
-	&dev_attr_spi_master_transfer_bytes_histo2.attr,
-	&dev_attr_spi_master_transfer_bytes_histo3.attr,
-	&dev_attr_spi_master_transfer_bytes_histo4.attr,
-	&dev_attr_spi_master_transfer_bytes_histo5.attr,
-	&dev_attr_spi_master_transfer_bytes_histo6.attr,
-	&dev_attr_spi_master_transfer_bytes_histo7.attr,
-	&dev_attr_spi_master_transfer_bytes_histo8.attr,
-	&dev_attr_spi_master_transfer_bytes_histo9.attr,
-	&dev_attr_spi_master_transfer_bytes_histo10.attr,
-	&dev_attr_spi_master_transfer_bytes_histo11.attr,
-	&dev_attr_spi_master_transfer_bytes_histo12.attr,
-	&dev_attr_spi_master_transfer_bytes_histo13.attr,
-	&dev_attr_spi_master_transfer_bytes_histo14.attr,
-	&dev_attr_spi_master_transfer_bytes_histo15.attr,
-	&dev_attr_spi_master_transfer_bytes_histo16.attr,
-	&dev_attr_spi_master_transfers_split_maxsize.attr,
+static struct attribute *spi_controller_statistics_attrs[] = {
+	&dev_attr_spi_controller_messages.attr,
+	&dev_attr_spi_controller_transfers.attr,
+	&dev_attr_spi_controller_errors.attr,
+	&dev_attr_spi_controller_timedout.attr,
+	&dev_attr_spi_controller_spi_sync.attr,
+	&dev_attr_spi_controller_spi_sync_immediate.attr,
+	&dev_attr_spi_controller_spi_async.attr,
+	&dev_attr_spi_controller_bytes.attr,
+	&dev_attr_spi_controller_bytes_rx.attr,
+	&dev_attr_spi_controller_bytes_tx.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo0.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo1.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo2.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo3.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo4.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo5.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo6.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo7.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo8.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo9.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo10.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo11.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo12.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo13.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo14.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo15.attr,
+	&dev_attr_spi_controller_transfer_bytes_histo16.attr,
+	&dev_attr_spi_controller_transfers_split_maxsize.attr,
 	NULL,
 };
 
-static const struct attribute_group spi_master_statistics_group = {
+static const struct attribute_group spi_controller_statistics_group = {
 	.name  = "statistics",
-	.attrs  = spi_master_statistics_attrs,
+	.attrs  = spi_controller_statistics_attrs,
 };
 
 static const struct attribute_group *spi_master_groups[] = {
-	&spi_master_statistics_group,
+	&spi_controller_statistics_group,
 	NULL,
 };
 
 void spi_statistics_add_transfer_stats(struct spi_statistics *stats,
 				       struct spi_transfer *xfer,
-				       struct spi_master *master)
+				       struct spi_controller *ctlr)
 {
 	unsigned long flags;
 	int l2len = min(fls(xfer->len), SPI_STATISTICS_HISTO_SIZE) - 1;
@@ -260,10 +260,10 @@ void spi_statistics_add_transfer_stats(struct spi_statistics *stats,
 
 	stats->bytes += xfer->len;
 	if ((xfer->tx_buf) &&
-	    (xfer->tx_buf != master->dummy_tx))
+	    (xfer->tx_buf != ctlr->dummy_tx))
 		stats->bytes_tx += xfer->len;
 	if ((xfer->rx_buf) &&
-	    (xfer->rx_buf != master->dummy_rx))
+	    (xfer->rx_buf != ctlr->dummy_rx))
 		stats->bytes_rx += xfer->len;
 
 	spin_unlock_irqrestore(&stats->lock, flags);
@@ -405,7 +405,7 @@ EXPORT_SYMBOL_GPL(__spi_register_driver);
 /*-------------------------------------------------------------------------*/
 
 /* SPI devices should normally not be created by SPI device drivers; that
- * would make them board-specific.  Similarly with SPI master drivers.
+ * would make them board-specific.  Similarly with SPI controller drivers.
  * Device registration normally goes into like arch/.../mach.../board-YYY.c
  * with other readonly (flashable) information about mainboard devices.
  */
@@ -416,17 +416,17 @@ struct boardinfo {
 };
 
 static LIST_HEAD(board_list);
-static LIST_HEAD(spi_master_list);
+static LIST_HEAD(spi_controller_list);
 
 /*
  * Used to protect add/del opertion for board_info list and
- * spi_master list, and their matching process
+ * spi_controller list, and their matching process
  */
 static DEFINE_MUTEX(board_lock);
 
 /**
  * spi_alloc_device - Allocate a new SPI device
- * @master: Controller to which device is connected
+ * @ctlr: Controller to which device is connected
  * Context: can sleep
  *
  * Allows a driver to allocate and initialize a spi_device without
@@ -435,27 +435,27 @@ static DEFINE_MUTEX(board_lock);
  * spi_add_device() on it.
  *
  * Caller is responsible to call spi_add_device() on the returned
- * spi_device structure to add it to the SPI master.  If the caller
+ * spi_device structure to add it to the SPI controller.  If the caller
  * needs to discard the spi_device without adding it, then it should
  * call spi_dev_put() on it.
  *
  * Return: a pointer to the new device, or NULL.
  */
-struct spi_device *spi_alloc_device(struct spi_master *master)
+struct spi_device *spi_alloc_device(struct spi_controller *ctlr)
 {
 	struct spi_device	*spi;
 
-	if (!spi_master_get(master))
+	if (!spi_controller_get(ctlr))
 		return NULL;
 
 	spi = kzalloc(sizeof(*spi), GFP_KERNEL);
 	if (!spi) {
-		spi_master_put(master);
+		spi_controller_put(ctlr);
 		return NULL;
 	}
 
-	spi->master = master;
-	spi->dev.parent = &master->dev;
+	spi->master = spi->controller = ctlr;
+	spi->dev.parent = &ctlr->dev;
 	spi->dev.bus = &spi_bus_type;
 	spi->dev.release = spidev_release;
 	spi->cs_gpio = -ENOENT;
@@ -476,7 +476,7 @@ static void spi_dev_set_name(struct spi_device *spi)
 		return;
 	}
 
-	dev_set_name(&spi->dev, "%s.%u", dev_name(&spi->master->dev),
+	dev_set_name(&spi->dev, "%s.%u", dev_name(&spi->controller->dev),
 		     spi->chip_select);
 }
 
@@ -485,7 +485,7 @@ static int spi_dev_check(struct device *dev, void *data)
 	struct spi_device *spi = to_spi_device(dev);
 	struct spi_device *new_spi = data;
 
-	if (spi->master == new_spi->master &&
+	if (spi->controller == new_spi->controller &&
 	    spi->chip_select == new_spi->chip_select)
 		return -EBUSY;
 	return 0;
@@ -503,15 +503,14 @@ static int spi_dev_check(struct device *dev, void *data)
 int spi_add_device(struct spi_device *spi)
 {
 	static DEFINE_MUTEX(spi_add_lock);
-	struct spi_master *master = spi->master;
-	struct device *dev = master->dev.parent;
+	struct spi_controller *ctlr = spi->controller;
+	struct device *dev = ctlr->dev.parent;
 	int status;
 
 	/* Chipselects are numbered 0..max; validate. */
-	if (spi->chip_select >= master->num_chipselect) {
-		dev_err(dev, "cs%d >= max %d\n",
-			spi->chip_select,
-			master->num_chipselect);
+	if (spi->chip_select >= ctlr->num_chipselect) {
+		dev_err(dev, "cs%d >= max %d\n", spi->chip_select,
+			ctlr->num_chipselect);
 		return -EINVAL;
 	}
 
@@ -531,8 +530,8 @@ int spi_add_device(struct spi_device *spi)
 		goto done;
 	}
 
-	if (master->cs_gpios)
-		spi->cs_gpio = master->cs_gpios[spi->chip_select];
+	if (ctlr->cs_gpios)
+		spi->cs_gpio = ctlr->cs_gpios[spi->chip_select];
 
 	/* Drivers may modify this initial i/o setup, but will
 	 * normally rely on the device being setup.  Devices
@@ -561,7 +560,7 @@ EXPORT_SYMBOL_GPL(spi_add_device);
 
 /**
  * spi_new_device - instantiate one new SPI device
- * @master: Controller to which device is connected
+ * @ctlr: Controller to which device is connected
  * @chip: Describes the SPI device
  * Context: can sleep
  *
@@ -573,7 +572,7 @@ EXPORT_SYMBOL_GPL(spi_add_device);
  *
  * Return: the new device, or NULL.
  */
-struct spi_device *spi_new_device(struct spi_master *master,
+struct spi_device *spi_new_device(struct spi_controller *ctlr,
 				  struct spi_board_info *chip)
 {
 	struct spi_device	*proxy;
@@ -586,7 +585,7 @@ struct spi_device *spi_new_device(struct spi_master *master,
 	 * suggests syslogged diagnostics are best here (ugh).
 	 */
 
-	proxy = spi_alloc_device(master);
+	proxy = spi_alloc_device(ctlr);
 	if (!proxy)
 		return NULL;
 
@@ -604,7 +603,7 @@ struct spi_device *spi_new_device(struct spi_master *master,
 	if (chip->properties) {
 		status = device_add_properties(&proxy->dev, chip->properties);
 		if (status) {
-			dev_err(&master->dev,
+			dev_err(&ctlr->dev,
 				"failed to add properties to '%s': %d\n",
 				chip->modalias, status);
 			goto err_dev_put;
@@ -631,7 +630,7 @@ EXPORT_SYMBOL_GPL(spi_new_device);
  * @spi: spi_device to unregister
  *
  * Start making the passed SPI device vanish. Normally this would be handled
- * by spi_unregister_master().
+ * by spi_unregister_controller().
  */
 void spi_unregister_device(struct spi_device *spi)
 {
@@ -648,17 +647,17 @@ void spi_unregister_device(struct spi_device *spi)
 }
 EXPORT_SYMBOL_GPL(spi_unregister_device);
 
-static void spi_match_master_to_boardinfo(struct spi_master *master,
-				struct spi_board_info *bi)
+static void spi_match_controller_to_boardinfo(struct spi_controller *ctlr,
+					      struct spi_board_info *bi)
 {
 	struct spi_device *dev;
 
-	if (master->bus_num != bi->bus_num)
+	if (ctlr->bus_num != bi->bus_num)
 		return;
 
-	dev = spi_new_device(master, bi);
+	dev = spi_new_device(ctlr, bi);
 	if (!dev)
-		dev_err(master->dev.parent, "can't create new device for %s\n",
+		dev_err(ctlr->dev.parent, "can't create new device for %s\n",
 			bi->modalias);
 }
 
@@ -697,7 +696,7 @@ int spi_register_board_info(struct spi_board_info const *info, unsigned n)
 		return -ENOMEM;
 
 	for (i = 0; i < n; i++, bi++, info++) {
-		struct spi_master *master;
+		struct spi_controller *ctlr;
 
 		memcpy(&bi->board_info, info, sizeof(*info));
 		if (info->properties) {
@@ -709,8 +708,9 @@ int spi_register_board_info(struct spi_board_info const *info, unsigned n)
 
 		mutex_lock(&board_lock);
 		list_add_tail(&bi->list, &board_list);
-		list_for_each_entry(master, &spi_master_list, list)
-			spi_match_master_to_boardinfo(master, &bi->board_info);
+		list_for_each_entry(ctlr, &spi_controller_list, list)
+			spi_match_controller_to_boardinfo(ctlr,
+							  &bi->board_info);
 		mutex_unlock(&board_lock);
 	}
 
@@ -727,16 +727,16 @@ static void spi_set_cs(struct spi_device *spi, bool enable)
 	if (gpio_is_valid(spi->cs_gpio)) {
 		gpio_set_value(spi->cs_gpio, !enable);
 		/* Some SPI masters need both GPIO CS & slave_select */
-		if ((spi->master->flags & SPI_MASTER_GPIO_SS) &&
-		    spi->master->set_cs)
-			spi->master->set_cs(spi, !enable);
-	} else if (spi->master->set_cs) {
-		spi->master->set_cs(spi, !enable);
+		if ((spi->controller->flags & SPI_MASTER_GPIO_SS) &&
+		    spi->controller->set_cs)
+			spi->controller->set_cs(spi, !enable);
+	} else if (spi->controller->set_cs) {
+		spi->controller->set_cs(spi, !enable);
 	}
 }
 
 #ifdef CONFIG_HAS_DMA
-static int spi_map_buf(struct spi_master *master, struct device *dev,
+static int spi_map_buf(struct spi_controller *ctlr, struct device *dev,
 		       struct sg_table *sgt, void *buf, size_t len,
 		       enum dma_data_direction dir)
 {
@@ -761,7 +761,7 @@ static int spi_map_buf(struct spi_master *master, struct device *dev,
 		desc_len = min_t(int, max_seg_size, PAGE_SIZE);
 		sgs = DIV_ROUND_UP(len + offset_in_page(buf), desc_len);
 	} else if (virt_addr_valid(buf)) {
-		desc_len = min_t(int, max_seg_size, master->max_dma_len);
+		desc_len = min_t(int, max_seg_size, ctlr->max_dma_len);
 		sgs = DIV_ROUND_UP(len, desc_len);
 	} else {
 		return -EINVAL;
@@ -811,7 +811,7 @@ static int spi_map_buf(struct spi_master *master, struct device *dev,
 	return 0;
 }
 
-static void spi_unmap_buf(struct spi_master *master, struct device *dev,
+static void spi_unmap_buf(struct spi_controller *ctlr, struct device *dev,
 			  struct sg_table *sgt, enum dma_data_direction dir)
 {
 	if (sgt->orig_nents) {
@@ -820,31 +820,31 @@ static void spi_unmap_buf(struct spi_master *master, struct device *dev,
 	}
 }
 
-static int __spi_map_msg(struct spi_master *master, struct spi_message *msg)
+static int __spi_map_msg(struct spi_controller *ctlr, struct spi_message *msg)
 {
 	struct device *tx_dev, *rx_dev;
 	struct spi_transfer *xfer;
 	int ret;
 
-	if (!master->can_dma)
+	if (!ctlr->can_dma)
 		return 0;
 
-	if (master->dma_tx)
-		tx_dev = master->dma_tx->device->dev;
+	if (ctlr->dma_tx)
+		tx_dev = ctlr->dma_tx->device->dev;
 	else
-		tx_dev = master->dev.parent;
+		tx_dev = ctlr->dev.parent;
 
-	if (master->dma_rx)
-		rx_dev = master->dma_rx->device->dev;
+	if (ctlr->dma_rx)
+		rx_dev = ctlr->dma_rx->device->dev;
 	else
-		rx_dev = master->dev.parent;
+		rx_dev = ctlr->dev.parent;
 
 	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
-		if (!master->can_dma(master, msg->spi, xfer))
+		if (!ctlr->can_dma(ctlr, msg->spi, xfer))
 			continue;
 
 		if (xfer->tx_buf != NULL) {
-			ret = spi_map_buf(master, tx_dev, &xfer->tx_sg,
+			ret = spi_map_buf(ctlr, tx_dev, &xfer->tx_sg,
 					  (void *)xfer->tx_buf, xfer->len,
 					  DMA_TO_DEVICE);
 			if (ret != 0)
@@ -852,79 +852,78 @@ static int __spi_map_msg(struct spi_master *master, struct spi_message *msg)
 		}
 
 		if (xfer->rx_buf != NULL) {
-			ret = spi_map_buf(master, rx_dev, &xfer->rx_sg,
+			ret = spi_map_buf(ctlr, rx_dev, &xfer->rx_sg,
 					  xfer->rx_buf, xfer->len,
 					  DMA_FROM_DEVICE);
 			if (ret != 0) {
-				spi_unmap_buf(master, tx_dev, &xfer->tx_sg,
+				spi_unmap_buf(ctlr, tx_dev, &xfer->tx_sg,
 					      DMA_TO_DEVICE);
 				return ret;
 			}
 		}
 	}
 
-	master->cur_msg_mapped = true;
+	ctlr->cur_msg_mapped = true;
 
 	return 0;
 }
 
-static int __spi_unmap_msg(struct spi_master *master, struct spi_message *msg)
+static int __spi_unmap_msg(struct spi_controller *ctlr, struct spi_message *msg)
 {
 	struct spi_transfer *xfer;
 	struct device *tx_dev, *rx_dev;
 
-	if (!master->cur_msg_mapped || !master->can_dma)
+	if (!ctlr->cur_msg_mapped || !ctlr->can_dma)
 		return 0;
 
-	if (master->dma_tx)
-		tx_dev = master->dma_tx->device->dev;
+	if (ctlr->dma_tx)
+		tx_dev = ctlr->dma_tx->device->dev;
 	else
-		tx_dev = master->dev.parent;
+		tx_dev = ctlr->dev.parent;
 
-	if (master->dma_rx)
-		rx_dev = master->dma_rx->device->dev;
+	if (ctlr->dma_rx)
+		rx_dev = ctlr->dma_rx->device->dev;
 	else
-		rx_dev = master->dev.parent;
+		rx_dev = ctlr->dev.parent;
 
 	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
-		if (!master->can_dma(master, msg->spi, xfer))
+		if (!ctlr->can_dma(ctlr, msg->spi, xfer))
 			continue;
 
-		spi_unmap_buf(master, rx_dev, &xfer->rx_sg, DMA_FROM_DEVICE);
-		spi_unmap_buf(master, tx_dev, &xfer->tx_sg, DMA_TO_DEVICE);
+		spi_unmap_buf(ctlr, rx_dev, &xfer->rx_sg, DMA_FROM_DEVICE);
+		spi_unmap_buf(ctlr, tx_dev, &xfer->tx_sg, DMA_TO_DEVICE);
 	}
 
 	return 0;
 }
 #else /* !CONFIG_HAS_DMA */
-static inline int spi_map_buf(struct spi_master *master,
-			      struct device *dev, struct sg_table *sgt,
-			      void *buf, size_t len,
+static inline int spi_map_buf(struct spi_controller *ctlr, struct device *dev,
+			      struct sg_table *sgt, void *buf, size_t len,
 			      enum dma_data_direction dir)
 {
 	return -EINVAL;
 }
 
-static inline void spi_unmap_buf(struct spi_master *master,
+static inline void spi_unmap_buf(struct spi_controller *ctlr,
 				 struct device *dev, struct sg_table *sgt,
 				 enum dma_data_direction dir)
 {
 }
 
-static inline int __spi_map_msg(struct spi_master *master,
+static inline int __spi_map_msg(struct spi_controller *ctlr,
 				struct spi_message *msg)
 {
 	return 0;
 }
 
-static inline int __spi_unmap_msg(struct spi_master *master,
+static inline int __spi_unmap_msg(struct spi_controller *ctlr,
 				  struct spi_message *msg)
 {
 	return 0;
 }
 #endif /* !CONFIG_HAS_DMA */
 
-static inline int spi_unmap_msg(struct spi_master *master,
+static inline int spi_unmap_msg(struct spi_controller *ctlr,
 				struct spi_message *msg)
 {
 	struct spi_transfer *xfer;
@@ -934,63 +933,63 @@ static inline int spi_unmap_msg(struct spi_master *master,
 		 * Restore the original value of tx_buf or rx_buf if they are
 		 * NULL.
 		 */
-		if (xfer->tx_buf == master->dummy_tx)
+		if (xfer->tx_buf == ctlr->dummy_tx)
 			xfer->tx_buf = NULL;
-		if (xfer->rx_buf == master->dummy_rx)
+		if (xfer->rx_buf == ctlr->dummy_rx)
 			xfer->rx_buf = NULL;
 	}
 
-	return __spi_unmap_msg(master, msg);
+	return __spi_unmap_msg(ctlr, msg);
 }
 
-static int spi_map_msg(struct spi_master *master, struct spi_message *msg)
+static int spi_map_msg(struct spi_controller *ctlr, struct spi_message *msg)
 {
 	struct spi_transfer *xfer;
 	void *tmp;
 	unsigned int max_tx, max_rx;
 
-	if (master->flags & (SPI_MASTER_MUST_RX | SPI_MASTER_MUST_TX)) {
+	if (ctlr->flags & (SPI_CONTROLLER_MUST_RX | SPI_CONTROLLER_MUST_TX)) {
 		max_tx = 0;
 		max_rx = 0;
 
 		list_for_each_entry(xfer, &msg->transfers, transfer_list) {
-			if ((master->flags & SPI_MASTER_MUST_TX) &&
+			if ((ctlr->flags & SPI_CONTROLLER_MUST_TX) &&
 			    !xfer->tx_buf)
 				max_tx = max(xfer->len, max_tx);
-			if ((master->flags & SPI_MASTER_MUST_RX) &&
+			if ((ctlr->flags & SPI_CONTROLLER_MUST_RX) &&
 			    !xfer->rx_buf)
 				max_rx = max(xfer->len, max_rx);
 		}
 
 		if (max_tx) {
-			tmp = krealloc(master->dummy_tx, max_tx,
+			tmp = krealloc(ctlr->dummy_tx, max_tx,
 				       GFP_KERNEL | GFP_DMA);
 			if (!tmp)
 				return -ENOMEM;
-			master->dummy_tx = tmp;
+			ctlr->dummy_tx = tmp;
 			memset(tmp, 0, max_tx);
 		}
 
 		if (max_rx) {
-			tmp = krealloc(master->dummy_rx, max_rx,
+			tmp = krealloc(ctlr->dummy_rx, max_rx,
 				       GFP_KERNEL | GFP_DMA);
 			if (!tmp)
 				return -ENOMEM;
-			master->dummy_rx = tmp;
+			ctlr->dummy_rx = tmp;
 		}
 
 		if (max_tx || max_rx) {
 			list_for_each_entry(xfer, &msg->transfers,
 					    transfer_list) {
 				if (!xfer->tx_buf)
-					xfer->tx_buf = master->dummy_tx;
+					xfer->tx_buf = ctlr->dummy_tx;
 				if (!xfer->rx_buf)
-					xfer->rx_buf = master->dummy_rx;
+					xfer->rx_buf = ctlr->dummy_rx;
 			}
 		}
 	}
 
-	return __spi_map_msg(master, msg);
+	return __spi_map_msg(ctlr, msg);
 }
 
 /*
@@ -1000,14 +999,14 @@ static int spi_map_msg(struct spi_master *master, struct spi_message *msg)
  * drivers which implement a transfer_one() operation.  It provides
  * standard handling of delays and chip select management.
  */
-static int spi_transfer_one_message(struct spi_master *master,
+static int spi_transfer_one_message(struct spi_controller *ctlr,
 				    struct spi_message *msg)
 {
 	struct spi_transfer *xfer;
 	bool keep_cs = false;
 	int ret = 0;
 	unsigned long long ms = 1;
-	struct spi_statistics *statm = &master->statistics;
+	struct spi_statistics *statm = &ctlr->statistics;
 	struct spi_statistics *stats = &msg->spi->statistics;
 
 	spi_set_cs(msg->spi, true);
@@ -1018,13 +1017,13 @@ static int spi_transfer_one_message(struct spi_master *master,
 	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
 		trace_spi_transfer_start(msg, xfer);
 
-		spi_statistics_add_transfer_stats(statm, xfer, master);
-		spi_statistics_add_transfer_stats(stats, xfer, master);
+		spi_statistics_add_transfer_stats(statm, xfer, ctlr);
+		spi_statistics_add_transfer_stats(stats, xfer, ctlr);
 
 		if (xfer->tx_buf || xfer->rx_buf) {
-			reinit_completion(&master->xfer_completion);
+			reinit_completion(&ctlr->xfer_completion);
 
-			ret = master->transfer_one(master, msg->spi, xfer);
+			ret = ctlr->transfer_one(ctlr, msg->spi, xfer);
 			if (ret < 0) {
 				SPI_STATISTICS_INCREMENT_FIELD(statm,
 							       errors);
@@ -1044,7 +1043,7 @@ static int spi_transfer_one_message(struct spi_master *master,
 				if (ms > UINT_MAX)
 					ms = UINT_MAX;
 
-				ms = wait_for_completion_timeout(&master->xfer_completion,
+				ms = wait_for_completion_timeout(&ctlr->xfer_completion,
 								 msecs_to_jiffies(ms));
 			}
 
@@ -1099,33 +1098,33 @@ out:
 	if (msg->status == -EINPROGRESS)
 		msg->status = ret;
 
-	if (msg->status && master->handle_err)
-		master->handle_err(master, msg);
+	if (msg->status && ctlr->handle_err)
+		ctlr->handle_err(ctlr, msg);
 
-	spi_res_release(master, msg);
+	spi_res_release(ctlr, msg);
 
-	spi_finalize_current_message(master);
+	spi_finalize_current_message(ctlr);
 
 	return ret;
 }
 
 /**
  * spi_finalize_current_transfer - report completion of a transfer
- * @master: the master reporting completion
+ * @ctlr: the controller reporting completion
  *
  * Called by SPI drivers using the core transfer_one_message()
  * implementation to notify it that the current interrupt driven
  * transfer has finished and the next one may be scheduled.
  */
-void spi_finalize_current_transfer(struct spi_master *master)
+void spi_finalize_current_transfer(struct spi_controller *ctlr)
 {
-	complete(&master->xfer_completion);
+	complete(&ctlr->xfer_completion);
 }
 EXPORT_SYMBOL_GPL(spi_finalize_current_transfer);
 
 /**
  * __spi_pump_messages - function which processes spi message queue
- * @master: master to process queue for
+ * @ctlr: controller to process queue for
  * @in_kthread: true if we are in the context of the message pump thread
  *
  * This function checks if there is any spi message in the queue that
@@ -1136,136 +1135,136 @@ EXPORT_SYMBOL_GPL(spi_finalize_current_transfer);
  * inside spi_sync(); the queue extraction handling at the top of the
  * function should deal with this safely.
  */
-static void __spi_pump_messages(struct spi_master *master, bool in_kthread)
+static void __spi_pump_messages(struct spi_controller *ctlr, bool in_kthread)
 {
 	unsigned long flags;
 	bool was_busy = false;
 	int ret;
 
 	/* Lock queue */
-	spin_lock_irqsave(&master->queue_lock, flags);
+	spin_lock_irqsave(&ctlr->queue_lock, flags);
 
 	/* Make sure we are not already running a message */
-	if (master->cur_msg) {
-		spin_unlock_irqrestore(&master->queue_lock, flags);
+	if (ctlr->cur_msg) {
+		spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 		return;
 	}
 
 	/* If another context is idling the device then defer */
-	if (master->idling) {
-		kthread_queue_work(&master->kworker, &master->pump_messages);
-		spin_unlock_irqrestore(&master->queue_lock, flags);
+	if (ctlr->idling) {
+		kthread_queue_work(&ctlr->kworker, &ctlr->pump_messages);
+		spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 		return;
 	}
 
 	/* Check if the queue is idle */
-	if (list_empty(&master->queue) || !master->running) {
-		if (!master->busy) {
-			spin_unlock_irqrestore(&master->queue_lock, flags);
+	if (list_empty(&ctlr->queue) || !ctlr->running) {
+		if (!ctlr->busy) {
+			spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 			return;
 		}
 
 		/* Only do teardown in the thread */
 		if (!in_kthread) {
-			kthread_queue_work(&master->kworker,
-					   &master->pump_messages);
-			spin_unlock_irqrestore(&master->queue_lock, flags);
+			kthread_queue_work(&ctlr->kworker,
+					   &ctlr->pump_messages);
+			spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 			return;
 		}
 
-		master->busy = false;
-		master->idling = true;
-		spin_unlock_irqrestore(&master->queue_lock, flags);
-
-		kfree(master->dummy_rx);
-		master->dummy_rx = NULL;
-		kfree(master->dummy_tx);
-		master->dummy_tx = NULL;
-		if (master->unprepare_transfer_hardware &&
-		    master->unprepare_transfer_hardware(master))
-			dev_err(&master->dev,
+		ctlr->busy = false;
+		ctlr->idling = true;
+		spin_unlock_irqrestore(&ctlr->queue_lock, flags);
+
+		kfree(ctlr->dummy_rx);
+		ctlr->dummy_rx = NULL;
+		kfree(ctlr->dummy_tx);
+		ctlr->dummy_tx = NULL;
+		if (ctlr->unprepare_transfer_hardware &&
+		    ctlr->unprepare_transfer_hardware(ctlr))
+			dev_err(&ctlr->dev,
 				"failed to unprepare transfer hardware\n");
-		if (master->auto_runtime_pm) {
-			pm_runtime_mark_last_busy(master->dev.parent);
-			pm_runtime_put_autosuspend(master->dev.parent);
+		if (ctlr->auto_runtime_pm) {
+			pm_runtime_mark_last_busy(ctlr->dev.parent);
+			pm_runtime_put_autosuspend(ctlr->dev.parent);
 		}
-		trace_spi_master_idle(master);
+		trace_spi_controller_idle(ctlr);
 
-		spin_lock_irqsave(&master->queue_lock, flags);
-		master->idling = false;
-		spin_unlock_irqrestore(&master->queue_lock, flags);
+		spin_lock_irqsave(&ctlr->queue_lock, flags);
+		ctlr->idling = false;
+		spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 		return;
 	}
 
 	/* Extract head of queue */
-	master->cur_msg =
-		list_first_entry(&master->queue, struct spi_message, queue);
+	ctlr->cur_msg =
+		list_first_entry(&ctlr->queue, struct spi_message, queue);
 
-	list_del_init(&master->cur_msg->queue);
-	if (master->busy)
+	list_del_init(&ctlr->cur_msg->queue);
+	if (ctlr->busy)
 		was_busy = true;
 	else
-		master->busy = true;
-	spin_unlock_irqrestore(&master->queue_lock, flags);
+		ctlr->busy = true;
+	spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 
-	mutex_lock(&master->io_mutex);
+	mutex_lock(&ctlr->io_mutex);
 
-	if (!was_busy && master->auto_runtime_pm) {
-		ret = pm_runtime_get_sync(master->dev.parent);
+	if (!was_busy && ctlr->auto_runtime_pm) {
+		ret = pm_runtime_get_sync(ctlr->dev.parent);
 		if (ret < 0) {
-			dev_err(&master->dev, "Failed to power device: %d\n",
+			dev_err(&ctlr->dev, "Failed to power device: %d\n",
 				ret);
-			mutex_unlock(&master->io_mutex);
+			mutex_unlock(&ctlr->io_mutex);
 			return;
 		}
 	}
 
 	if (!was_busy)
-		trace_spi_master_busy(master);
+		trace_spi_controller_busy(ctlr);
 
-	if (!was_busy && master->prepare_transfer_hardware) {
-		ret = master->prepare_transfer_hardware(master);
+	if (!was_busy && ctlr->prepare_transfer_hardware) {
+		ret = ctlr->prepare_transfer_hardware(ctlr);
 		if (ret) {
-			dev_err(&master->dev,
+			dev_err(&ctlr->dev,
 				"failed to prepare transfer hardware\n");
 
-			if (master->auto_runtime_pm)
-				pm_runtime_put(master->dev.parent);
-			mutex_unlock(&master->io_mutex);
+			if (ctlr->auto_runtime_pm)
+				pm_runtime_put(ctlr->dev.parent);
+			mutex_unlock(&ctlr->io_mutex);
 			return;
 		}
 	}
 
-	trace_spi_message_start(master->cur_msg);
+	trace_spi_message_start(ctlr->cur_msg);
 
-	if (master->prepare_message) {
-		ret = master->prepare_message(master, master->cur_msg);
+	if (ctlr->prepare_message) {
+		ret = ctlr->prepare_message(ctlr, ctlr->cur_msg);
 		if (ret) {
-			dev_err(&master->dev,
-				"failed to prepare message: %d\n", ret);
-			master->cur_msg->status = ret;
-			spi_finalize_current_message(master);
+			dev_err(&ctlr->dev, "failed to prepare message: %d\n",
+				ret);
+			ctlr->cur_msg->status = ret;
+			spi_finalize_current_message(ctlr);
 			goto out;
 		}
-		master->cur_msg_prepared = true;
+		ctlr->cur_msg_prepared = true;
 	}
 
-	ret = spi_map_msg(master, master->cur_msg);
+	ret = spi_map_msg(ctlr, ctlr->cur_msg);
 	if (ret) {
-		master->cur_msg->status = ret;
-		spi_finalize_current_message(master);
+		ctlr->cur_msg->status = ret;
+		spi_finalize_current_message(ctlr);
 		goto out;
 	}
 
-	ret = master->transfer_one_message(master, master->cur_msg);
+	ret = ctlr->transfer_one_message(ctlr, ctlr->cur_msg);
 	if (ret) {
-		dev_err(&master->dev,
+		dev_err(&ctlr->dev,
 			"failed to transfer one message from queue\n");
 		goto out;
 	}
 
 out:
-	mutex_unlock(&master->io_mutex);
+	mutex_unlock(&ctlr->io_mutex);
 
 	/* Prod the scheduler in case transfer_one() was busy waiting */
 	if (!ret)
@@ -1274,44 +1273,43 @@ out:
 
 /**
  * spi_pump_messages - kthread work function which processes spi message queue
- * @work: pointer to kthread work struct contained in the master struct
+ * @work: pointer to kthread work struct contained in the controller struct
  */
 static void spi_pump_messages(struct kthread_work *work)
 {
-	struct spi_master *master =
-		container_of(work, struct spi_master, pump_messages);
+	struct spi_controller *ctlr =
+		container_of(work, struct spi_controller, pump_messages);
 
-	__spi_pump_messages(master, true);
+	__spi_pump_messages(ctlr, true);
 }
 
-static int spi_init_queue(struct spi_master *master)
+static int spi_init_queue(struct spi_controller *ctlr)
 {
 	struct sched_param param = { .sched_priority = MAX_RT_PRIO - 1 };
 
-	master->running = false;
-	master->busy = false;
+	ctlr->running = false;
+	ctlr->busy = false;
 
-	kthread_init_worker(&master->kworker);
-	master->kworker_task = kthread_run(kthread_worker_fn,
-					   &master->kworker, "%s",
-					   dev_name(&master->dev));
-	if (IS_ERR(master->kworker_task)) {
-		dev_err(&master->dev, "failed to create message pump task\n");
-		return PTR_ERR(master->kworker_task);
+	kthread_init_worker(&ctlr->kworker);
+	ctlr->kworker_task = kthread_run(kthread_worker_fn, &ctlr->kworker,
+					 "%s", dev_name(&ctlr->dev));
+	if (IS_ERR(ctlr->kworker_task)) {
+		dev_err(&ctlr->dev, "failed to create message pump task\n");
+		return PTR_ERR(ctlr->kworker_task);
 	}
-	kthread_init_work(&master->pump_messages, spi_pump_messages);
+	kthread_init_work(&ctlr->pump_messages, spi_pump_messages);
 
 	/*
-	 * Master config will indicate if this controller should run the
+	 * Controller config will indicate if this controller should run the
 	 * message pump with high (realtime) priority to reduce the transfer
 	 * latency on the bus by minimising the delay between a transfer
 	 * request and the scheduling of the message pump thread. Without this
 	 * setting the message pump thread will remain at default priority.
 	 */
-	if (master->rt) {
-		dev_info(&master->dev,
+	if (ctlr->rt) {
+		dev_info(&ctlr->dev,
 			"will run message pump with realtime priority\n");
-		sched_setscheduler(master->kworker_task, SCHED_FIFO, &param);
+		sched_setscheduler(ctlr->kworker_task, SCHED_FIFO, &param);
 	}
 
 	return 0;
@@ -1320,23 +1318,23 @@ static int spi_init_queue(struct spi_master *master)
 /**
  * spi_get_next_queued_message() - called by driver to check for queued
  * messages
- * @master: the master to check for queued messages
+ * @ctlr: the controller to check for queued messages
  *
  * If there are more messages in the queue, the next message is returned from
  * this call.
  *
  * Return: the next message in the queue, else NULL if the queue is empty.
  */
-struct spi_message *spi_get_next_queued_message(struct spi_master *master)
+struct spi_message *spi_get_next_queued_message(struct spi_controller *ctlr)
 {
 	struct spi_message *next;
 	unsigned long flags;
 
 	/* get a pointer to the next message, if any */
-	spin_lock_irqsave(&master->queue_lock, flags);
-	next = list_first_entry_or_null(&master->queue, struct spi_message,
+	spin_lock_irqsave(&ctlr->queue_lock, flags);
+	next = list_first_entry_or_null(&ctlr->queue, struct spi_message,
 					queue);
-	spin_unlock_irqrestore(&master->queue_lock, flags);
+	spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 
 	return next;
 }
@@ -1344,36 +1342,36 @@ EXPORT_SYMBOL_GPL(spi_get_next_queued_message);
 
 /**
  * spi_finalize_current_message() - the current message is complete
- * @master: the master to return the message to
+ * @ctlr: the controller to return the message to
  *
  * Called by the driver to notify the core that the message in the front of the
  * queue is complete and can be removed from the queue.
  */
-void spi_finalize_current_message(struct spi_master *master)
+void spi_finalize_current_message(struct spi_controller *ctlr)
 {
 	struct spi_message *mesg;
 	unsigned long flags;
 	int ret;
 
-	spin_lock_irqsave(&master->queue_lock, flags);
-	mesg = master->cur_msg;
-	spin_unlock_irqrestore(&master->queue_lock, flags);
+	spin_lock_irqsave(&ctlr->queue_lock, flags);
+	mesg = ctlr->cur_msg;
+	spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 
-	spi_unmap_msg(master, mesg);
+	spi_unmap_msg(ctlr, mesg);
 
-	if (master->cur_msg_prepared && master->unprepare_message) {
-		ret = master->unprepare_message(master, mesg);
+	if (ctlr->cur_msg_prepared && ctlr->unprepare_message) {
+		ret = ctlr->unprepare_message(ctlr, mesg);
 		if (ret) {
-			dev_err(&master->dev,
-				"failed to unprepare message: %d\n", ret);
+			dev_err(&ctlr->dev, "failed to unprepare message: %d\n",
+				ret);
 		}
 	}
 
-	spin_lock_irqsave(&master->queue_lock, flags);
-	master->cur_msg = NULL;
-	master->cur_msg_prepared = false;
-	kthread_queue_work(&master->kworker, &master->pump_messages);
-	spin_unlock_irqrestore(&master->queue_lock, flags);
+	spin_lock_irqsave(&ctlr->queue_lock, flags);
+	ctlr->cur_msg = NULL;
+	ctlr->cur_msg_prepared = false;
+	kthread_queue_work(&ctlr->kworker, &ctlr->pump_messages);
+	spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 
 	trace_spi_message_done(mesg);
 
@@ -1383,66 +1381,65 @@ void spi_finalize_current_message(struct spi_master *master)
 }
 EXPORT_SYMBOL_GPL(spi_finalize_current_message);
 
-static int spi_start_queue(struct spi_master *master)
+static int spi_start_queue(struct spi_controller *ctlr)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&master->queue_lock, flags);
+	spin_lock_irqsave(&ctlr->queue_lock, flags);
 
-	if (master->running || master->busy) {
-		spin_unlock_irqrestore(&master->queue_lock, flags);
+	if (ctlr->running || ctlr->busy) {
+		spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 		return -EBUSY;
 	}
 
-	master->running = true;
-	master->cur_msg = NULL;
-	spin_unlock_irqrestore(&master->queue_lock, flags);
+	ctlr->running = true;
+	ctlr->cur_msg = NULL;
+	spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 
-	kthread_queue_work(&master->kworker, &master->pump_messages);
+	kthread_queue_work(&ctlr->kworker, &ctlr->pump_messages);
 
 	return 0;
 }
 
-static int spi_stop_queue(struct spi_master *master)
+static int spi_stop_queue(struct spi_controller *ctlr)
 {
 	unsigned long flags;
 	unsigned limit = 500;
 	int ret = 0;
 
-	spin_lock_irqsave(&master->queue_lock, flags);
+	spin_lock_irqsave(&ctlr->queue_lock, flags);
 
 	/*
 	 * This is a bit lame, but is optimized for the common execution path.
-	 * A wait_queue on the master->busy could be used, but then the common
+	 * A wait_queue on the ctlr->busy could be used, but then the common
 	 * execution path (pump_messages) would be required to call wake_up or
 	 * friends on every SPI message. Do this instead.
 	 */
-	while ((!list_empty(&master->queue) || master->busy) && limit--) {
-		spin_unlock_irqrestore(&master->queue_lock, flags);
+	while ((!list_empty(&ctlr->queue) || ctlr->busy) && limit--) {
+		spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 		usleep_range(10000, 11000);
-		spin_lock_irqsave(&master->queue_lock, flags);
+		spin_lock_irqsave(&ctlr->queue_lock, flags);
 	}
 
-	if (!list_empty(&master->queue) || master->busy)
+	if (!list_empty(&ctlr->queue) || ctlr->busy)
 		ret = -EBUSY;
 	else
-		master->running = false;
+		ctlr->running = false;
 
-	spin_unlock_irqrestore(&master->queue_lock, flags);
+	spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 
 	if (ret) {
-		dev_warn(&master->dev,
-			 "could not stop message queue\n");
+		dev_warn(&ctlr->dev, "could not stop message queue\n");
 		return ret;
 	}
 	return ret;
 }
 
-static int spi_destroy_queue(struct spi_master *master)
+static int spi_destroy_queue(struct spi_controller *ctlr)
 {
 	int ret;
 
-	ret = spi_stop_queue(master);
+	ret = spi_stop_queue(ctlr);
 
 	/*
 	 * kthread_flush_worker will block until all work is done.
@@ -1451,12 +1448,12 @@ static int spi_destroy_queue(struct spi_master *master)
 	 * return anyway.
 	 */
 	if (ret) {
-		dev_err(&master->dev, "problem destroying queue\n");
+		dev_err(&ctlr->dev, "problem destroying queue\n");
 		return ret;
 	}
 
-	kthread_flush_worker(&master->kworker);
-	kthread_stop(master->kworker_task);
+	kthread_flush_worker(&ctlr->kworker);
+	kthread_stop(ctlr->kworker_task);
 
 	return 0;
 }
@@ -1465,23 +1462,23 @@ static int __spi_queued_transfer(struct spi_device *spi,
 				 struct spi_message *msg,
 				 bool need_pump)
 {
-	struct spi_master *master = spi->master;
+	struct spi_controller *ctlr = spi->controller;
 	unsigned long flags;
 
-	spin_lock_irqsave(&master->queue_lock, flags);
+	spin_lock_irqsave(&ctlr->queue_lock, flags);
 
-	if (!master->running) {
-		spin_unlock_irqrestore(&master->queue_lock, flags);
+	if (!ctlr->running) {
+		spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 		return -ESHUTDOWN;
 	}
 	msg->actual_length = 0;
 	msg->status = -EINPROGRESS;
 
-	list_add_tail(&msg->queue, &master->queue);
-	if (!master->busy && need_pump)
-		kthread_queue_work(&master->kworker, &master->pump_messages);
+	list_add_tail(&msg->queue, &ctlr->queue);
+	if (!ctlr->busy && need_pump)
+		kthread_queue_work(&ctlr->kworker, &ctlr->pump_messages);
 
-	spin_unlock_irqrestore(&master->queue_lock, flags);
+	spin_unlock_irqrestore(&ctlr->queue_lock, flags);
 	return 0;
 }
 
@@ -1497,31 +1494,31 @@ static int spi_queued_transfer(struct spi_device *spi, struct spi_message *msg)
 	return __spi_queued_transfer(spi, msg, true);
 }
 
-static int spi_master_initialize_queue(struct spi_master *master)
+static int spi_controller_initialize_queue(struct spi_controller *ctlr)
 {
 	int ret;
 
-	master->transfer = spi_queued_transfer;
-	if (!master->transfer_one_message)
-		master->transfer_one_message = spi_transfer_one_message;
+	ctlr->transfer = spi_queued_transfer;
+	if (!ctlr->transfer_one_message)
+		ctlr->transfer_one_message = spi_transfer_one_message;
 
 	/* Initialize and start queue */
-	ret = spi_init_queue(master);
+	ret = spi_init_queue(ctlr);
 	if (ret) {
-		dev_err(&master->dev, "problem initializing queue\n");
+		dev_err(&ctlr->dev, "problem initializing queue\n");
 		goto err_init_queue;
 	}
-	master->queued = true;
-	ret = spi_start_queue(master);
+	ctlr->queued = true;
+	ret = spi_start_queue(ctlr);
 	if (ret) {
-		dev_err(&master->dev, "problem starting queue\n");
+		dev_err(&ctlr->dev, "problem starting queue\n");
 		goto err_start_queue;
 	}
 
 	return 0;
 
 err_start_queue:
-	spi_destroy_queue(master);
+	spi_destroy_queue(ctlr);
 err_init_queue:
 	return ret;
 }
@@ -1529,21 +1526,12 @@ err_init_queue:
 /*-------------------------------------------------------------------------*/
 
 #if defined(CONFIG_OF)
-static int of_spi_parse_dt(struct spi_master *master, struct spi_device *spi,
+static int of_spi_parse_dt(struct spi_controller *ctlr, struct spi_device *spi,
 			   struct device_node *nc)
 {
 	u32 value;
 	int rc;
 
-	/* Device address */
-	rc = of_property_read_u32(nc, "reg", &value);
-	if (rc) {
-		dev_err(&master->dev, "%s has no valid 'reg' property (%d)\n",
-			nc->full_name, rc);
-		return rc;
-	}
-	spi->chip_select = value;
-
 	/* Mode (clock phase/polarity/etc.) */
 	if (of_find_property(nc, "spi-cpha", NULL))
 		spi->mode |= SPI_CPHA;
@@ -1568,7 +1556,7 @@ static int of_spi_parse_dt(struct spi_master *master, struct spi_device *spi,
 			spi->mode |= SPI_TX_QUAD;
 			break;
 		default:
-			dev_warn(&master->dev,
+			dev_warn(&ctlr->dev,
 				"spi-tx-bus-width %d not supported\n",
 				value);
 			break;
@@ -1586,17 +1574,36 @@ static int of_spi_parse_dt(struct spi_master *master, struct spi_device *spi,
 			spi->mode |= SPI_RX_QUAD;
 			break;
 		default:
-			dev_warn(&master->dev,
+			dev_warn(&ctlr->dev,
 				"spi-rx-bus-width %d not supported\n",
 				value);
 			break;
 		}
 	}
 
+	if (spi_controller_is_slave(ctlr)) {
+		if (strcmp(nc->name, "slave")) {
+			dev_err(&ctlr->dev, "%s is not called 'slave'\n",
+				nc->full_name);
+			return -EINVAL;
+		}
+		return 0;
+	}
+
+	/* Device address */
+	rc = of_property_read_u32(nc, "reg", &value);
+	if (rc) {
+		dev_err(&ctlr->dev, "%s has no valid 'reg' property (%d)\n",
+			nc->full_name, rc);
+		return rc;
+	}
+	spi->chip_select = value;
+
 	/* Device speed */
 	rc = of_property_read_u32(nc, "spi-max-frequency", &value);
 	if (rc) {
-		dev_err(&master->dev, "%s has no valid 'spi-max-frequency' property (%d)\n",
+		dev_err(&ctlr->dev,
+			"%s has no valid 'spi-max-frequency' property (%d)\n",
 			nc->full_name, rc);
 		return rc;
 	}
@@ -1606,15 +1613,15 @@ static int of_spi_parse_dt(struct spi_master *master, struct spi_device *spi,
 }
 
 static struct spi_device *
-of_register_spi_device(struct spi_master *master, struct device_node *nc)
+of_register_spi_device(struct spi_controller *ctlr, struct device_node *nc)
 {
 	struct spi_device *spi;
 	int rc;
 
 	/* Alloc an spi_device */
-	spi = spi_alloc_device(master);
+	spi = spi_alloc_device(ctlr);
 	if (!spi) {
-		dev_err(&master->dev, "spi_device alloc error for %s\n",
+		dev_err(&ctlr->dev, "spi_device alloc error for %s\n",
 			nc->full_name);
 		rc = -ENOMEM;
 		goto err_out;
@@ -1624,12 +1631,12 @@ of_register_spi_device(struct spi_master *master, struct device_node *nc)
 	rc = of_modalias_node(nc, spi->modalias,
 				sizeof(spi->modalias));
 	if (rc < 0) {
-		dev_err(&master->dev, "cannot find modalias for %s\n",
+		dev_err(&ctlr->dev, "cannot find modalias for %s\n",
 			nc->full_name);
 		goto err_out;
 	}
 
-	rc = of_spi_parse_dt(master, spi, nc);
+	rc = of_spi_parse_dt(ctlr, spi, nc);
 	if (rc)
 		goto err_out;
 
@@ -1640,7 +1647,7 @@ of_register_spi_device(struct spi_master *master, struct device_node *nc)
 	/* Register the new device */
 	rc = spi_add_device(spi);
 	if (rc) {
-		dev_err(&master->dev, "spi_device register error %s\n",
+		dev_err(&ctlr->dev, "spi_device register error %s\n",
 			nc->full_name);
 		goto err_of_node_put;
 	}
@@ -1656,39 +1663,40 @@ err_out:
 
 /**
  * of_register_spi_devices() - Register child devices onto the SPI bus
- * @master:	Pointer to spi_master device
+ * @ctlr:	Pointer to spi_controller device
  *
- * Registers an spi_device for each child node of master node which has a 'reg'
- * property.
+ * Registers an spi_device for each child node of controller node which
+ * represents a valid SPI slave.
  */
-static void of_register_spi_devices(struct spi_master *master)
+static void of_register_spi_devices(struct spi_controller *ctlr)
 {
 	struct spi_device *spi;
 	struct device_node *nc;
 
-	if (!master->dev.of_node)
+	if (!ctlr->dev.of_node)
 		return;
 
-	for_each_available_child_of_node(master->dev.of_node, nc) {
+	for_each_available_child_of_node(ctlr->dev.of_node, nc) {
 		if (of_node_test_and_set_flag(nc, OF_POPULATED))
 			continue;
-		spi = of_register_spi_device(master, nc);
+		spi = of_register_spi_device(ctlr, nc);
 		if (IS_ERR(spi)) {
-			dev_warn(&master->dev, "Failed to create SPI device for %s\n",
-				nc->full_name);
+			dev_warn(&ctlr->dev,
+				 "Failed to create SPI device for %s\n",
+				 nc->full_name);
 			of_node_clear_flag(nc, OF_POPULATED);
 		}
 	}
 }
 #else
-static void of_register_spi_devices(struct spi_master *master) { }
+static void of_register_spi_devices(struct spi_controller *ctlr) { }
 #endif
 
 #ifdef CONFIG_ACPI
 static int acpi_spi_add_resource(struct acpi_resource *ares, void *data)
 {
 	struct spi_device *spi = data;
-	struct spi_master *master = spi->master;
+	struct spi_controller *ctlr = spi->controller;
 
 	if (ares->type == ACPI_RESOURCE_TYPE_SERIAL_BUS) {
 		struct acpi_resource_spi_serialbus *sb;
@@ -1702,8 +1710,8 @@ static int acpi_spi_add_resource(struct acpi_resource *ares, void *data)
 			 * 0 .. max - 1 so we need to ask the driver to
 			 * translate between the two schemes.
 			 */
-			if (master->fw_translate_cs) {
-				int cs = master->fw_translate_cs(master,
+			if (ctlr->fw_translate_cs) {
+				int cs = ctlr->fw_translate_cs(ctlr,
 						sb->device_selection);
 				if (cs < 0)
 					return cs;
@@ -1732,7 +1740,7 @@ static int acpi_spi_add_resource(struct acpi_resource *ares, void *data)
 	return 1;
 }
 
-static acpi_status acpi_register_spi_device(struct spi_master *master,
+static acpi_status acpi_register_spi_device(struct spi_controller *ctlr,
 					    struct acpi_device *adev)
 {
 	struct list_head resource_list;
@@ -1743,9 +1751,9 @@ static acpi_status acpi_register_spi_device(struct spi_master *master,
 	    acpi_device_enumerated(adev))
 		return AE_OK;
 
-	spi = spi_alloc_device(master);
+	spi = spi_alloc_device(ctlr);
 	if (!spi) {
-		dev_err(&master->dev, "failed to allocate SPI device for %s\n",
+		dev_err(&ctlr->dev, "failed to allocate SPI device for %s\n",
 			dev_name(&adev->dev));
 		return AE_NO_MEMORY;
 	}
@@ -1774,7 +1782,7 @@ static acpi_status acpi_register_spi_device(struct spi_master *master,
 	adev->power.flags.ignore_parent = true;
 	if (spi_add_device(spi)) {
 		adev->power.flags.ignore_parent = false;
-		dev_err(&master->dev, "failed to add SPI device %s from ACPI\n",
+		dev_err(&ctlr->dev, "failed to add SPI device %s from ACPI\n",
 			dev_name(&adev->dev));
 		spi_dev_put(spi);
 	}
@@ -1785,104 +1793,211 @@ static acpi_status acpi_register_spi_device(struct spi_master *master,
 static acpi_status acpi_spi_add_device(acpi_handle handle, u32 level,
 				       void *data, void **return_value)
 {
-	struct spi_master *master = data;
+	struct spi_controller *ctlr = data;
 	struct acpi_device *adev;
 
 	if (acpi_bus_get_device(handle, &adev))
 		return AE_OK;
 
-	return acpi_register_spi_device(master, adev);
+	return acpi_register_spi_device(ctlr, adev);
 }
 
-static void acpi_register_spi_devices(struct spi_master *master)
+static void acpi_register_spi_devices(struct spi_controller *ctlr)
 {
 	acpi_status status;
 	acpi_handle handle;
 
-	handle = ACPI_HANDLE(master->dev.parent);
+	handle = ACPI_HANDLE(ctlr->dev.parent);
 	if (!handle)
 		return;
 
 	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, handle, 1,
-				     acpi_spi_add_device, NULL,
-				     master, NULL);
+				     acpi_spi_add_device, NULL, ctlr, NULL);
 	if (ACPI_FAILURE(status))
-		dev_warn(&master->dev, "failed to enumerate SPI slaves\n");
+		dev_warn(&ctlr->dev, "failed to enumerate SPI slaves\n");
 }
 #else
-static inline void acpi_register_spi_devices(struct spi_master *master) {}
+static inline void acpi_register_spi_devices(struct spi_controller *ctlr) {}
 #endif /* CONFIG_ACPI */
 
-static void spi_master_release(struct device *dev)
+static void spi_controller_release(struct device *dev)
 {
-	struct spi_master *master;
+	struct spi_controller *ctlr;
 
-	master = container_of(dev, struct spi_master, dev);
-	kfree(master);
+	ctlr = container_of(dev, struct spi_controller, dev);
+	kfree(ctlr);
 }
 
 static struct class spi_master_class = {
 	.name		= "spi_master",
 	.owner		= THIS_MODULE,
-	.dev_release	= spi_master_release,
+	.dev_release	= spi_controller_release,
 	.dev_groups	= spi_master_groups,
 };
 
+#ifdef CONFIG_SPI_SLAVE
+/**
+ * spi_slave_abort - abort the ongoing transfer request on an SPI slave
+ *		     controller
+ * @spi: device used for the current transfer
+ */
+int spi_slave_abort(struct spi_device *spi)
+{
+	struct spi_controller *ctlr = spi->controller;
+
+	if (spi_controller_is_slave(ctlr) && ctlr->slave_abort)
+		return ctlr->slave_abort(ctlr);
+
+	return -ENOTSUPP;
+}
+EXPORT_SYMBOL_GPL(spi_slave_abort);
+
+static int match_true(struct device *dev, void *data)
+{
+	return 1;
+}
+
+static ssize_t spi_slave_show(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	struct spi_controller *ctlr = container_of(dev, struct spi_controller,
+						   dev);
+	struct device *child;
+
+	child = device_find_child(&ctlr->dev, NULL, match_true);
+	return sprintf(buf, "%s\n",
+		       child ? to_spi_device(child)->modalias : NULL);
+}
+
+static ssize_t spi_slave_store(struct device *dev,
+			       struct device_attribute *attr, const char *buf,
+			       size_t count)
+{
+	struct spi_controller *ctlr = container_of(dev, struct spi_controller,
+						   dev);
+	struct spi_device *spi;
+	struct device *child;
+	char name[32];
+	int rc;
+
+	rc = sscanf(buf, "%31s", name);
+	if (rc != 1 || !name[0])
+		return -EINVAL;
+
+	child = device_find_child(&ctlr->dev, NULL, match_true);
+	if (child) {
+		/* Remove registered slave */
+		device_unregister(child);
+		put_device(child);
+	}
+
+	if (strcmp(name, "(null)")) {
+		/* Register new slave */
+		spi = spi_alloc_device(ctlr);
+		if (!spi)
+			return -ENOMEM;
+
+		strlcpy(spi->modalias, name, sizeof(spi->modalias));
+
+		rc = spi_add_device(spi);
+		if (rc) {
+			spi_dev_put(spi);
+			return rc;
+		}
+	}
+
+	return count;
+}
+
+static DEVICE_ATTR(slave, 0644, spi_slave_show, spi_slave_store);
+
+static struct attribute *spi_slave_attrs[] = {
+	&dev_attr_slave.attr,
+	NULL,
+};
+
+static const struct attribute_group spi_slave_group = {
+	.attrs = spi_slave_attrs,
+};
+
+static const struct attribute_group *spi_slave_groups[] = {
+	&spi_controller_statistics_group,
+	&spi_slave_group,
+	NULL,
+};
+
+static struct class spi_slave_class = {
+	.name		= "spi_slave",
+	.owner		= THIS_MODULE,
+	.dev_release	= spi_controller_release,
+	.dev_groups	= spi_slave_groups,
+};
+#else
+extern struct class spi_slave_class;	/* dummy */
+#endif
 
 /**
- * spi_alloc_master - allocate SPI master controller
+ * __spi_alloc_controller - allocate an SPI master or slave controller
  * @dev: the controller, possibly using the platform_bus
  * @size: how much zeroed driver-private data to allocate; the pointer to this
  *	memory is in the driver_data field of the returned device,
- *	accessible with spi_master_get_devdata().
+ *	accessible with spi_controller_get_devdata().
+ * @slave: flag indicating whether to allocate an SPI master (false) or SPI
+ *	slave (true) controller
  * Context: can sleep
  *
- * This call is used only by SPI master controller drivers, which are the
+ * This call is used only by SPI controller drivers, which are the
  * only ones directly touching chip registers.  It's how they allocate
- * an spi_master structure, prior to calling spi_register_master().
+ * an spi_controller structure, prior to calling spi_register_controller().
  *
  * This must be called from context that can sleep.
  *
- * The caller is responsible for assigning the bus number and initializing
- * the master's methods before calling spi_register_master(); and (after errors
- * adding the device) calling spi_master_put() to prevent a memory leak.
+ * The caller is responsible for assigning the bus number and initializing the
+ * controller's methods before calling spi_register_controller(); and (after
+ * errors adding the device) calling spi_controller_put() to prevent a memory
+ * leak.
  *
- * Return: the SPI master structure on success, else NULL.
+ * Return: the SPI controller structure on success, else NULL.
  */
-struct spi_master *spi_alloc_master(struct device *dev, unsigned size)
+struct spi_controller *__spi_alloc_controller(struct device *dev,
+					      unsigned int size, bool slave)
 {
-	struct spi_master	*master;
+	struct spi_controller	*ctlr;
 
 	if (!dev)
 		return NULL;
 
-	master = kzalloc(size + sizeof(*master), GFP_KERNEL);
-	if (!master)
+	ctlr = kzalloc(size + sizeof(*ctlr), GFP_KERNEL);
+	if (!ctlr)
 		return NULL;
 
-	device_initialize(&master->dev);
-	master->bus_num = -1;
-	master->num_chipselect = 1;
-	master->dev.class = &spi_master_class;
-	master->dev.parent = dev;
-	pm_suspend_ignore_children(&master->dev, true);
-	spi_master_set_devdata(master, &master[1]);
+	device_initialize(&ctlr->dev);
+	ctlr->bus_num = -1;
+	ctlr->num_chipselect = 1;
+	ctlr->slave = slave;
+	if (IS_ENABLED(CONFIG_SPI_SLAVE) && slave)
+		ctlr->dev.class = &spi_slave_class;
+	else
+		ctlr->dev.class = &spi_master_class;
+	ctlr->dev.parent = dev;
+	pm_suspend_ignore_children(&ctlr->dev, true);
+	spi_controller_set_devdata(ctlr, &ctlr[1]);
 
-	return master;
+	return ctlr;
 }
-EXPORT_SYMBOL_GPL(spi_alloc_master);
+EXPORT_SYMBOL_GPL(__spi_alloc_controller);
 
 #ifdef CONFIG_OF
-static int of_spi_register_master(struct spi_master *master)
+static int of_spi_register_master(struct spi_controller *ctlr)
 {
 	int nb, i, *cs;
-	struct device_node *np = master->dev.of_node;
+	struct device_node *np = ctlr->dev.of_node;
 
 	if (!np)
 		return 0;
 
 	nb = of_gpio_named_count(np, "cs-gpios");
-	master->num_chipselect = max_t(int, nb, master->num_chipselect);
+	ctlr->num_chipselect = max_t(int, nb, ctlr->num_chipselect);
 
 	/* Return error only for an incorrectly formed cs-gpios property */
 	if (nb == 0 || nb == -ENOENT)
@@ -1890,15 +2005,14 @@ static int of_spi_register_master(struct spi_master *master)
 	else if (nb < 0)
 		return nb;
 
-	cs = devm_kzalloc(&master->dev,
-			  sizeof(int) * master->num_chipselect,
+	cs = devm_kzalloc(&ctlr->dev, sizeof(int) * ctlr->num_chipselect,
 			  GFP_KERNEL);
-	master->cs_gpios = cs;
+	ctlr->cs_gpios = cs;
 
-	if (!master->cs_gpios)
+	if (!ctlr->cs_gpios)
 		return -ENOMEM;
 
-	for (i = 0; i < master->num_chipselect; i++)
+	for (i = 0; i < ctlr->num_chipselect; i++)
 		cs[i] = -ENOENT;
 
 	for (i = 0; i < nb; i++)
@@ -1907,20 +2021,21 @@ static int of_spi_register_master(struct spi_master *master)
 	return 0;
 }
 #else
-static int of_spi_register_master(struct spi_master *master)
+static int of_spi_register_master(struct spi_controller *ctlr)
 {
 	return 0;
 }
 #endif
 
 /**
- * spi_register_master - register SPI master controller
- * @master: initialized master, originally from spi_alloc_master()
+ * spi_register_controller - register SPI master or slave controller
+ * @ctlr: initialized master, originally from spi_alloc_master() or
+ *	spi_alloc_slave()
  * Context: can sleep
  *
- * SPI master controllers connect to their drivers using some non-SPI bus,
+ * SPI controllers connect to their drivers using some non-SPI bus,
  * such as the platform bus.  The final stage of probe() in that code
- * includes calling spi_register_master() to hook up to this SPI bus glue.
+ * includes calling spi_register_controller() to hook up to this SPI bus glue.
  *
  * SPI controllers use board specific (often SOC specific) bus numbers,
  * and board-specific addressing for SPI devices combines those numbers
@@ -1929,16 +2044,16 @@ static int of_spi_register_master(struct spi_master *master)
  * chip is at which address.
  *
  * This must be called from context that can sleep.  It returns zero on
- * success, else a negative error code (dropping the master's refcount).
+ * success, else a negative error code (dropping the controller's refcount).
  * After a successful return, the caller is responsible for calling
- * spi_unregister_master().
+ * spi_unregister_controller().
  *
  * Return: zero on success, else a negative error code.
  */
-int spi_register_master(struct spi_master *master)
+int spi_register_controller(struct spi_controller *ctlr)
 {
 	static atomic_t		dyn_bus_id = ATOMIC_INIT((1<<15) - 1);
-	struct device		*dev = master->dev.parent;
+	struct device		*dev = ctlr->dev.parent;
 	struct boardinfo	*bi;
 	int			status = -ENODEV;
 	int			dynamic = 0;
@@ -1946,103 +2061,109 @@ int spi_register_master(struct spi_master *master)
 	if (!dev)
 		return -ENODEV;
 
-	status = of_spi_register_master(master);
-	if (status)
-		return status;
+	if (!spi_controller_is_slave(ctlr)) {
+		status = of_spi_register_master(ctlr);
+		if (status)
+			return status;
+	}
 
 	/* even if it's just one always-selected device, there must
 	 * be at least one chipselect
 	 */
-	if (master->num_chipselect == 0)
+	if (ctlr->num_chipselect == 0)
 		return -EINVAL;
 
-	if ((master->bus_num < 0) && master->dev.of_node)
-		master->bus_num = of_alias_get_id(master->dev.of_node, "spi");
+	if ((ctlr->bus_num < 0) && ctlr->dev.of_node)
+		ctlr->bus_num = of_alias_get_id(ctlr->dev.of_node, "spi");
 
 	/* convention:  dynamically assigned bus IDs count down from the max */
-	if (master->bus_num < 0) {
+	if (ctlr->bus_num < 0) {
 		/* FIXME switch to an IDR based scheme, something like
 		 * I2C now uses, so we can't run out of "dynamic" IDs
 		 */
-		master->bus_num = atomic_dec_return(&dyn_bus_id);
+		ctlr->bus_num = atomic_dec_return(&dyn_bus_id);
 		dynamic = 1;
 	}
 
-	INIT_LIST_HEAD(&master->queue);
-	spin_lock_init(&master->queue_lock);
-	spin_lock_init(&master->bus_lock_spinlock);
-	mutex_init(&master->bus_lock_mutex);
-	mutex_init(&master->io_mutex);
-	master->bus_lock_flag = 0;
-	init_completion(&master->xfer_completion);
-	if (!master->max_dma_len)
-		master->max_dma_len = INT_MAX;
+	INIT_LIST_HEAD(&ctlr->queue);
+	spin_lock_init(&ctlr->queue_lock);
+	spin_lock_init(&ctlr->bus_lock_spinlock);
+	mutex_init(&ctlr->bus_lock_mutex);
+	mutex_init(&ctlr->io_mutex);
+	ctlr->bus_lock_flag = 0;
+	init_completion(&ctlr->xfer_completion);
+	if (!ctlr->max_dma_len)
+		ctlr->max_dma_len = INT_MAX;
 
 	/* register the device, then userspace will see it.
 	 * registration fails if the bus ID is in use.
 	 */
-	dev_set_name(&master->dev, "spi%u", master->bus_num);
-	status = device_add(&master->dev);
+	dev_set_name(&ctlr->dev, "spi%u", ctlr->bus_num);
+	status = device_add(&ctlr->dev);
 	if (status < 0)
 		goto done;
-	dev_dbg(dev, "registered master %s%s\n", dev_name(&master->dev),
-			dynamic ? " (dynamic)" : "");
+	dev_dbg(dev, "registered %s %s%s\n",
+			spi_controller_is_slave(ctlr) ? "slave" : "master",
+			dev_name(&ctlr->dev), dynamic ? " (dynamic)" : "");
 
 	/* If we're using a queued driver, start the queue */
-	if (master->transfer)
-		dev_info(dev, "master is unqueued, this is deprecated\n");
+	if (ctlr->transfer)
+		dev_info(dev, "controller is unqueued, this is deprecated\n");
 	else {
-		status = spi_master_initialize_queue(master);
+		status = spi_controller_initialize_queue(ctlr);
 		if (status) {
-			device_del(&master->dev);
+			device_del(&ctlr->dev);
 			goto done;
 		}
 	}
 	/* add statistics */
-	spin_lock_init(&master->statistics.lock);
+	spin_lock_init(&ctlr->statistics.lock);
 
 	mutex_lock(&board_lock);
-	list_add_tail(&master->list, &spi_master_list);
+	list_add_tail(&ctlr->list, &spi_controller_list);
 	list_for_each_entry(bi, &board_list, list)
-		spi_match_master_to_boardinfo(master, &bi->board_info);
+		spi_match_controller_to_boardinfo(ctlr, &bi->board_info);
 	mutex_unlock(&board_lock);
 
 	/* Register devices from the device tree and ACPI */
-	of_register_spi_devices(master);
-	acpi_register_spi_devices(master);
+	of_register_spi_devices(ctlr);
+	acpi_register_spi_devices(ctlr);
 done:
 	return status;
 }
-EXPORT_SYMBOL_GPL(spi_register_master);
+EXPORT_SYMBOL_GPL(spi_register_controller);
 
 static void devm_spi_unregister(struct device *dev, void *res)
 {
-	spi_unregister_master(*(struct spi_master **)res);
+	spi_unregister_controller(*(struct spi_controller **)res);
 }
 
 /**
- * dev_spi_register_master - register managed SPI master controller
- * @dev:    device managing SPI master
- * @master: initialized master, originally from spi_alloc_master()
+ * devm_spi_register_controller - register managed SPI master or slave
+ *	controller
+ * @dev:    device managing SPI controller
+ * @ctlr: initialized controller, originally from spi_alloc_master() or
+ *	spi_alloc_slave()
  * Context: can sleep
  *
- * Register a SPI device as with spi_register_master() which will
+ * Register a SPI device as with spi_register_controller() which will
  * automatically be unregister
  *
  * Return: zero on success, else a negative error code.
  */
-int devm_spi_register_master(struct device *dev, struct spi_master *master)
+int devm_spi_register_controller(struct device *dev,
+				 struct spi_controller *ctlr)
 {
-	struct spi_master **ptr;
+	struct spi_controller **ptr;
 	int ret;
 
 	ptr = devres_alloc(devm_spi_unregister, sizeof(*ptr), GFP_KERNEL);
 	if (!ptr)
 		return -ENOMEM;
 
-	ret = spi_register_master(master);
+	ret = spi_register_controller(ctlr);
 	if (!ret) {
-		*ptr = master;
+		*ptr = ctlr;
 		devres_add(dev, ptr);
 	} else {
 		devres_free(ptr);
@@ -2050,7 +2171,7 @@ int devm_spi_register_master(struct device *dev, struct spi_master *master)
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(devm_spi_register_master);
+EXPORT_SYMBOL_GPL(devm_spi_register_controller);
 
 static int __unregister(struct device *dev, void *null)
 {
@@ -2059,71 +2180,71 @@ static int __unregister(struct device *dev, void *null)
 }
 
 /**
- * spi_unregister_master - unregister SPI master controller
- * @master: the master being unregistered
+ * spi_unregister_controller - unregister SPI master or slave controller
+ * @ctlr: the controller being unregistered
  * Context: can sleep
  *
- * This call is used only by SPI master controller drivers, which are the
+ * This call is used only by SPI controller drivers, which are the
  * only ones directly touching chip registers.
  *
  * This must be called from context that can sleep.
  */
-void spi_unregister_master(struct spi_master *master)
+void spi_unregister_controller(struct spi_controller *ctlr)
 {
 	int dummy;
 
-	if (master->queued) {
-		if (spi_destroy_queue(master))
-			dev_err(&master->dev, "queue remove failed\n");
+	if (ctlr->queued) {
+		if (spi_destroy_queue(ctlr))
+			dev_err(&ctlr->dev, "queue remove failed\n");
 	}
 
 	mutex_lock(&board_lock);
-	list_del(&master->list);
+	list_del(&ctlr->list);
 	mutex_unlock(&board_lock);
 
-	dummy = device_for_each_child(&master->dev, NULL, __unregister);
-	device_unregister(&master->dev);
+	dummy = device_for_each_child(&ctlr->dev, NULL, __unregister);
+	device_unregister(&ctlr->dev);
 }
-EXPORT_SYMBOL_GPL(spi_unregister_master);
+EXPORT_SYMBOL_GPL(spi_unregister_controller);
 
-int spi_master_suspend(struct spi_master *master)
+int spi_controller_suspend(struct spi_controller *ctlr)
 {
 	int ret;
 
-	/* Basically no-ops for non-queued masters */
-	if (!master->queued)
+	/* Basically no-ops for non-queued controllers */
+	if (!ctlr->queued)
 		return 0;
 
-	ret = spi_stop_queue(master);
+	ret = spi_stop_queue(ctlr);
 	if (ret)
-		dev_err(&master->dev, "queue stop failed\n");
+		dev_err(&ctlr->dev, "queue stop failed\n");
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(spi_master_suspend);
+EXPORT_SYMBOL_GPL(spi_controller_suspend);
 
-int spi_master_resume(struct spi_master *master)
+int spi_controller_resume(struct spi_controller *ctlr)
 {
 	int ret;
 
-	if (!master->queued)
+	if (!ctlr->queued)
 		return 0;
 
-	ret = spi_start_queue(master);
+	ret = spi_start_queue(ctlr);
 	if (ret)
-		dev_err(&master->dev, "queue restart failed\n");
+		dev_err(&ctlr->dev, "queue restart failed\n");
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(spi_master_resume);
+EXPORT_SYMBOL_GPL(spi_controller_resume);
 
-static int __spi_master_match(struct device *dev, const void *data)
+static int __spi_controller_match(struct device *dev, const void *data)
 {
-	struct spi_master *m;
+	struct spi_controller *ctlr;
 	const u16 *bus_num = data;
 
-	m = container_of(dev, struct spi_master, dev);
-	return m->bus_num == *bus_num;
+	ctlr = container_of(dev, struct spi_controller, dev);
+	return ctlr->bus_num == *bus_num;
 }
 
 /**
@@ -2133,22 +2254,22 @@ static int __spi_master_match(struct device *dev, const void *data)
  *
  * This call may be used with devices that are registered after
  * arch init time.  It returns a refcounted pointer to the relevant
- * spi_master (which the caller must release), or NULL if there is
+ * spi_controller (which the caller must release), or NULL if there is
  * no such master registered.
  *
  * Return: the SPI master structure on success, else NULL.
  */
-struct spi_master *spi_busnum_to_master(u16 bus_num)
+struct spi_controller *spi_busnum_to_master(u16 bus_num)
 {
 	struct device		*dev;
-	struct spi_master	*master = NULL;
+	struct spi_controller	*ctlr = NULL;
 
 	dev = class_find_device(&spi_master_class, NULL, &bus_num,
-				__spi_master_match);
+				__spi_controller_match);
 	if (dev)
-		master = container_of(dev, struct spi_master, dev);
+		ctlr = container_of(dev, struct spi_controller, dev);
 	/* reference got in class_find_device */
-	return master;
+	return ctlr;
 }
 EXPORT_SYMBOL_GPL(spi_busnum_to_master);
 
@@ -2168,7 +2289,7 @@ EXPORT_SYMBOL_GPL(spi_busnum_to_master);
  * Return: the pointer to the allocated data
  *
  * This may get enhanced in the future to allocate from a memory pool
- * of the @spi_device or @spi_master to avoid repeated allocations.
+ * of the @spi_device or @spi_controller to avoid repeated allocations.
  */
 void *spi_res_alloc(struct spi_device *spi,
 		    spi_res_release_t release,
@@ -2220,11 +2341,10 @@ EXPORT_SYMBOL_GPL(spi_res_add);
 
 /**
  * spi_res_release - release all spi resources for this message
- * @master:  the @spi_master
+ * @ctlr:  the @spi_controller
  * @message: the @spi_message
  */
-void spi_res_release(struct spi_master *master,
-		     struct spi_message *message)
+void spi_res_release(struct spi_controller *ctlr, struct spi_message *message)
 {
 	struct spi_res *res;
 
@@ -2233,7 +2353,7 @@ void spi_res_release(struct spi_master *master,
 				      struct spi_res, entry);
 
 		if (res->release)
-			res->release(master, message, res->data);
+			res->release(ctlr, message, res->data);
 
 		list_del(&res->entry);
 
@@ -2246,7 +2366,7 @@ EXPORT_SYMBOL_GPL(spi_res_release);
 
 /* Core methods for spi_message alterations */
 
-static void __spi_replace_transfers_release(struct spi_master *master,
+static void __spi_replace_transfers_release(struct spi_controller *ctlr,
 					    struct spi_message *msg,
 					    void *res)
 {
@@ -2255,7 +2375,7 @@ static void __spi_replace_transfers_release(struct spi_master *master,
 
 	/* call extra callback if requested */
 	if (rxfer->release)
-		rxfer->release(master, msg, res);
+		rxfer->release(ctlr, msg, res);
 
 	/* insert replaced transfers back into the message */
 	list_splice(&rxfer->replaced_transfers, rxfer->replaced_after);
@@ -2375,7 +2495,7 @@ struct spi_replaced_transfers *spi_replace_transfers(
 }
 EXPORT_SYMBOL_GPL(spi_replace_transfers);
 
-static int __spi_split_transfer_maxsize(struct spi_master *master,
+static int __spi_split_transfer_maxsize(struct spi_controller *ctlr,
 					struct spi_message *msg,
 					struct spi_transfer **xferp,
 					size_t maxsize,
@@ -2437,7 +2557,7 @@ static int __spi_split_transfer_maxsize(struct spi_master *master,
 	*xferp = &xfers[count - 1];
 
 	/* increment statistics counters */
-	SPI_STATISTICS_INCREMENT_FIELD(&master->statistics,
+	SPI_STATISTICS_INCREMENT_FIELD(&ctlr->statistics,
 				       transfers_split_maxsize);
 	SPI_STATISTICS_INCREMENT_FIELD(&msg->spi->statistics,
 				       transfers_split_maxsize);
@@ -2449,14 +2569,14 @@ static int __spi_split_transfer_maxsize(struct spi_master *master,
  * spi_split_tranfers_maxsize - split spi transfers into multiple transfers
  *                              when an individual transfer exceeds a
  *                              certain size
- * @master:    the @spi_master for this transfer
+ * @ctlr:    the @spi_controller for this transfer
  * @msg:   the @spi_message to transform
  * @maxsize:  the maximum when to apply this
  * @gfp: GFP allocation flags
  *
  * Return: status of transformation
  */
-int spi_split_transfers_maxsize(struct spi_master *master,
+int spi_split_transfers_maxsize(struct spi_controller *ctlr,
 				struct spi_message *msg,
 				size_t maxsize,
 				gfp_t gfp)
@@ -2472,8 +2592,8 @@ int spi_split_transfers_maxsize(struct spi_master *master,
 	 */
 	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
 		if (xfer->len > maxsize) {
-			ret = __spi_split_transfer_maxsize(
-				master, msg, &xfer, maxsize, gfp);
+			ret = __spi_split_transfer_maxsize(ctlr, msg, &xfer,
+							   maxsize, gfp);
 			if (ret)
 				return ret;
 		}
@@ -2485,18 +2605,18 @@ EXPORT_SYMBOL_GPL(spi_split_transfers_maxsize);
 
 /*-------------------------------------------------------------------------*/
 
-/* Core methods for SPI master protocol drivers.  Some of the
+/* Core methods for SPI controller protocol drivers.  Some of the
  * other core methods are currently defined as inline functions.
  */
 
-static int __spi_validate_bits_per_word(struct spi_master *master, u8 bits_per_word)
+static int __spi_validate_bits_per_word(struct spi_controller *ctlr,
+					u8 bits_per_word)
 {
-	if (master->bits_per_word_mask) {
+	if (ctlr->bits_per_word_mask) {
 		/* Only 32 bits fit in the mask */
 		if (bits_per_word > 32)
 			return -EINVAL;
-		if (!(master->bits_per_word_mask &
-				SPI_BPW_MASK(bits_per_word)))
+		if (!(ctlr->bits_per_word_mask & SPI_BPW_MASK(bits_per_word)))
 			return -EINVAL;
 	}
 
@@ -2542,9 +2662,9 @@ int spi_setup(struct spi_device *spi)
 		(SPI_TX_DUAL | SPI_TX_QUAD | SPI_RX_DUAL | SPI_RX_QUAD)))
 		return -EINVAL;
 	/* help drivers fail *cleanly* when they need options
-	 * that aren't supported with their current master
+	 * that aren't supported with their current controller
 	 */
-	bad_bits = spi->mode & ~spi->master->mode_bits;
+	bad_bits = spi->mode & ~spi->controller->mode_bits;
 	ugly_bits = bad_bits &
 		    (SPI_TX_DUAL | SPI_TX_QUAD | SPI_RX_DUAL | SPI_RX_QUAD);
 	if (ugly_bits) {
@@ -2563,15 +2683,16 @@ int spi_setup(struct spi_device *spi)
 	if (!spi->bits_per_word)
 		spi->bits_per_word = 8;
 
-	status = __spi_validate_bits_per_word(spi->master, spi->bits_per_word);
+	status = __spi_validate_bits_per_word(spi->controller,
+					      spi->bits_per_word);
 	if (status)
 		return status;
 
 	if (!spi->max_speed_hz)
-		spi->max_speed_hz = spi->master->max_speed_hz;
+		spi->max_speed_hz = spi->controller->max_speed_hz;
 
-	if (spi->master->setup)
-		status = spi->master->setup(spi);
+	if (spi->controller->setup)
+		status = spi->controller->setup(spi);
 
 	spi_set_cs(spi, false);
 
@@ -2590,7 +2711,7 @@ EXPORT_SYMBOL_GPL(spi_setup);
 
 static int __spi_validate(struct spi_device *spi, struct spi_message *message)
 {
-	struct spi_master *master = spi->master;
+	struct spi_controller *ctlr = spi->controller;
 	struct spi_transfer *xfer;
 	int w_size;
 
@@ -2602,16 +2723,16 @@ static int __spi_validate(struct spi_device *spi, struct spi_message *message)
 	 * either MOSI or MISO is missing.  They can also be caused by
 	 * software limitations.
 	 */
-	if ((master->flags & SPI_MASTER_HALF_DUPLEX)
-			|| (spi->mode & SPI_3WIRE)) {
-		unsigned flags = master->flags;
+	if ((ctlr->flags & SPI_CONTROLLER_HALF_DUPLEX) ||
+	    (spi->mode & SPI_3WIRE)) {
+		unsigned flags = ctlr->flags;
 
 		list_for_each_entry(xfer, &message->transfers, transfer_list) {
 			if (xfer->rx_buf && xfer->tx_buf)
 				return -EINVAL;
-			if ((flags & SPI_MASTER_NO_TX) && xfer->tx_buf)
+			if ((flags & SPI_CONTROLLER_NO_TX) && xfer->tx_buf)
 				return -EINVAL;
-			if ((flags & SPI_MASTER_NO_RX) && xfer->rx_buf)
+			if ((flags & SPI_CONTROLLER_NO_RX) && xfer->rx_buf)
 				return -EINVAL;
 		}
 	}
@@ -2631,13 +2752,12 @@ static int __spi_validate(struct spi_device *spi, struct spi_message *message)
 		if (!xfer->speed_hz)
 			xfer->speed_hz = spi->max_speed_hz;
 		if (!xfer->speed_hz)
-			xfer->speed_hz = master->max_speed_hz;
+			xfer->speed_hz = ctlr->max_speed_hz;
 
-		if (master->max_speed_hz &&
-		    xfer->speed_hz > master->max_speed_hz)
-			xfer->speed_hz = master->max_speed_hz;
+		if (ctlr->max_speed_hz && xfer->speed_hz > ctlr->max_speed_hz)
+			xfer->speed_hz = ctlr->max_speed_hz;
 
-		if (__spi_validate_bits_per_word(master, xfer->bits_per_word))
+		if (__spi_validate_bits_per_word(ctlr, xfer->bits_per_word))
 			return -EINVAL;
 
 		/*
@@ -2655,8 +2775,8 @@ static int __spi_validate(struct spi_device *spi, struct spi_message *message)
 		if (xfer->len % w_size)
 			return -EINVAL;
 
-		if (xfer->speed_hz && master->min_speed_hz &&
-		    xfer->speed_hz < master->min_speed_hz)
+		if (xfer->speed_hz && ctlr->min_speed_hz &&
+		    xfer->speed_hz < ctlr->min_speed_hz)
 			return -EINVAL;
 
 		if (xfer->tx_buf && !xfer->tx_nbits)
@@ -2701,16 +2821,16 @@ static int __spi_validate(struct spi_device *spi, struct spi_message *message)
 
 static int __spi_async(struct spi_device *spi, struct spi_message *message)
 {
-	struct spi_master *master = spi->master;
+	struct spi_controller *ctlr = spi->controller;
 
 	message->spi = spi;
 
-	SPI_STATISTICS_INCREMENT_FIELD(&master->statistics, spi_async);
+	SPI_STATISTICS_INCREMENT_FIELD(&ctlr->statistics, spi_async);
 	SPI_STATISTICS_INCREMENT_FIELD(&spi->statistics, spi_async);
 
 	trace_spi_message_submit(message);
 
-	return master->transfer(spi, message);
+	return ctlr->transfer(spi, message);
 }
 
 /**
@@ -2746,7 +2866,7 @@ static int __spi_async(struct spi_device *spi, struct spi_message *message)
  */
 int spi_async(struct spi_device *spi, struct spi_message *message)
 {
-	struct spi_master *master = spi->master;
+	struct spi_controller *ctlr = spi->controller;
 	int ret;
 	unsigned long flags;
 
@@ -2754,14 +2874,14 @@ int spi_async(struct spi_device *spi, struct spi_message *message)
 	if (ret != 0)
 		return ret;
 
-	spin_lock_irqsave(&master->bus_lock_spinlock, flags);
+	spin_lock_irqsave(&ctlr->bus_lock_spinlock, flags);
 
-	if (master->bus_lock_flag)
+	if (ctlr->bus_lock_flag)
 		ret = -EBUSY;
 	else
 		ret = __spi_async(spi, message);
 
-	spin_unlock_irqrestore(&master->bus_lock_spinlock, flags);
+	spin_unlock_irqrestore(&ctlr->bus_lock_spinlock, flags);
 
 	return ret;
 }
@@ -2800,7 +2920,7 @@ EXPORT_SYMBOL_GPL(spi_async);
  */
 int spi_async_locked(struct spi_device *spi, struct spi_message *message)
 {
-	struct spi_master *master = spi->master;
+	struct spi_controller *ctlr = spi->controller;
 	int ret;
 	unsigned long flags;
 
@@ -2808,11 +2928,11 @@ int spi_async_locked(struct spi_device *spi, struct spi_message *message)
 	if (ret != 0)
 		return ret;
 
-	spin_lock_irqsave(&master->bus_lock_spinlock, flags);
+	spin_lock_irqsave(&ctlr->bus_lock_spinlock, flags);
 
 	ret = __spi_async(spi, message);
 
-	spin_unlock_irqrestore(&master->bus_lock_spinlock, flags);
+	spin_unlock_irqrestore(&ctlr->bus_lock_spinlock, flags);
 
 	return ret;
 
@@ -2824,7 +2944,7 @@ int spi_flash_read(struct spi_device *spi,
 		   struct spi_flash_read_message *msg)
 
 {
-	struct spi_master *master = spi->master;
+	struct spi_controller *master = spi->controller;
 	struct device *rx_dev = NULL;
 	int ret;
 
@@ -2878,7 +2998,7 @@ EXPORT_SYMBOL_GPL(spi_flash_read);
 
 /*-------------------------------------------------------------------------*/
 
-/* Utility methods for SPI master protocol drivers, layered on
+/* Utility methods for SPI protocol drivers, layered on
  * top of the core.  Some other utility methods are defined as
  * inline functions.
  */
@@ -2892,7 +3012,7 @@ static int __spi_sync(struct spi_device *spi, struct spi_message *message)
 {
 	DECLARE_COMPLETION_ONSTACK(done);
 	int status;
-	struct spi_master *master = spi->master;
+	struct spi_controller *ctlr = spi->controller;
 	unsigned long flags;
 
 	status = __spi_validate(spi, message);
@@ -2903,7 +3023,7 @@ static int __spi_sync(struct spi_device *spi, struct spi_message *message)
 	message->context = &done;
 	message->spi = spi;
 
-	SPI_STATISTICS_INCREMENT_FIELD(&master->statistics, spi_sync);
+	SPI_STATISTICS_INCREMENT_FIELD(&ctlr->statistics, spi_sync);
 	SPI_STATISTICS_INCREMENT_FIELD(&spi->statistics, spi_sync);
 
 	/* If we're not using the legacy transfer method then we will
@@ -2911,14 +3031,14 @@ static int __spi_sync(struct spi_device *spi, struct spi_message *message)
 	 * This code would be less tricky if we could remove the
 	 * support for driver implemented message queues.
 	 */
-	if (master->transfer == spi_queued_transfer) {
-		spin_lock_irqsave(&master->bus_lock_spinlock, flags);
+	if (ctlr->transfer == spi_queued_transfer) {
+		spin_lock_irqsave(&ctlr->bus_lock_spinlock, flags);
 
 		trace_spi_message_submit(message);
 
 		status = __spi_queued_transfer(spi, message, false);
 
-		spin_unlock_irqrestore(&master->bus_lock_spinlock, flags);
+		spin_unlock_irqrestore(&ctlr->bus_lock_spinlock, flags);
 	} else {
 		status = spi_async_locked(spi, message);
 	}
@@ -2927,12 +3047,12 @@ static int __spi_sync(struct spi_device *spi, struct spi_message *message)
 		/* Push out the messages in the calling context if we
 		 * can.
 		 */
-		if (master->transfer == spi_queued_transfer) {
-			SPI_STATISTICS_INCREMENT_FIELD(&master->statistics,
+		if (ctlr->transfer == spi_queued_transfer) {
+			SPI_STATISTICS_INCREMENT_FIELD(&ctlr->statistics,
 						       spi_sync_immediate);
 			SPI_STATISTICS_INCREMENT_FIELD(&spi->statistics,
 						       spi_sync_immediate);
-			__spi_pump_messages(master, false);
+			__spi_pump_messages(ctlr, false);
 		}
 
 		wait_for_completion(&done);
@@ -2967,9 +3087,9 @@ int spi_sync(struct spi_device *spi, struct spi_message *message)
 {
 	int ret;
 
-	mutex_lock(&spi->master->bus_lock_mutex);
+	mutex_lock(&spi->controller->bus_lock_mutex);
 	ret = __spi_sync(spi, message);
-	mutex_unlock(&spi->master->bus_lock_mutex);
+	mutex_unlock(&spi->controller->bus_lock_mutex);
 
 	return ret;
 }
@@ -2999,7 +3119,7 @@ EXPORT_SYMBOL_GPL(spi_sync_locked);
 
 /**
  * spi_bus_lock - obtain a lock for exclusive SPI bus usage
- * @master: SPI bus master that should be locked for exclusive bus access
+ * @ctlr: SPI bus master that should be locked for exclusive bus access
  * Context: can sleep
  *
  * This call may only be used from a context that may sleep.  The sleep
@@ -3012,15 +3132,15 @@ EXPORT_SYMBOL_GPL(spi_sync_locked);
  *
  * Return: always zero.
  */
-int spi_bus_lock(struct spi_master *master)
+int spi_bus_lock(struct spi_controller *ctlr)
 {
 	unsigned long flags;
 
-	mutex_lock(&master->bus_lock_mutex);
+	mutex_lock(&ctlr->bus_lock_mutex);
 
-	spin_lock_irqsave(&master->bus_lock_spinlock, flags);
-	master->bus_lock_flag = 1;
-	spin_unlock_irqrestore(&master->bus_lock_spinlock, flags);
+	spin_lock_irqsave(&ctlr->bus_lock_spinlock, flags);
+	ctlr->bus_lock_flag = 1;
+	spin_unlock_irqrestore(&ctlr->bus_lock_spinlock, flags);
 
 	/* mutex remains locked until spi_bus_unlock is called */
 
@@ -3030,7 +3150,7 @@ EXPORT_SYMBOL_GPL(spi_bus_lock);
 
 /**
  * spi_bus_unlock - release the lock for exclusive SPI bus usage
- * @master: SPI bus master that was locked for exclusive bus access
+ * @ctlr: SPI bus master that was locked for exclusive bus access
  * Context: can sleep
  *
  * This call may only be used from a context that may sleep.  The sleep
@@ -3041,11 +3161,11 @@ EXPORT_SYMBOL_GPL(spi_bus_lock);
  *
  * Return: always zero.
  */
-int spi_bus_unlock(struct spi_master *master)
+int spi_bus_unlock(struct spi_controller *ctlr)
 {
-	master->bus_lock_flag = 0;
+	ctlr->bus_lock_flag = 0;
 
-	mutex_unlock(&master->bus_lock_mutex);
+	mutex_unlock(&ctlr->bus_lock_mutex);
 
 	return 0;
 }
@@ -3147,45 +3267,48 @@ static struct spi_device *of_find_spi_device_by_node(struct device_node *node)
 	return dev ? to_spi_device(dev) : NULL;
 }
 
-static int __spi_of_master_match(struct device *dev, const void *data)
+static int __spi_of_controller_match(struct device *dev, const void *data)
 {
 	return dev->of_node == data;
 }
 
-/* the spi masters are not using spi_bus, so we find it with another way */
-static struct spi_master *of_find_spi_master_by_node(struct device_node *node)
+/* the spi controllers are not using spi_bus, so we find it with another way */
+static struct spi_controller *of_find_spi_controller_by_node(struct device_node *node)
 {
 	struct device *dev;
 
 	dev = class_find_device(&spi_master_class, NULL, node,
-				__spi_of_master_match);
+				__spi_of_controller_match);
+	if (!dev && IS_ENABLED(CONFIG_SPI_SLAVE))
+		dev = class_find_device(&spi_slave_class, NULL, node,
+					__spi_of_controller_match);
 	if (!dev)
 		return NULL;
 
 	/* reference got in class_find_device */
-	return container_of(dev, struct spi_master, dev);
+	return container_of(dev, struct spi_controller, dev);
 }
 
 static int of_spi_notify(struct notifier_block *nb, unsigned long action,
 			 void *arg)
 {
 	struct of_reconfig_data *rd = arg;
-	struct spi_master *master;
+	struct spi_controller *ctlr;
 	struct spi_device *spi;
 
 	switch (of_reconfig_get_state_change(action, arg)) {
 	case OF_RECONFIG_CHANGE_ADD:
-		master = of_find_spi_master_by_node(rd->dn->parent);
-		if (master == NULL)
+		ctlr = of_find_spi_controller_by_node(rd->dn->parent);
+		if (ctlr == NULL)
 			return NOTIFY_OK;	/* not for us */
 
 		if (of_node_test_and_set_flag(rd->dn, OF_POPULATED)) {
-			put_device(&master->dev);
+			put_device(&ctlr->dev);
 			return NOTIFY_OK;
 		}
 
-		spi = of_register_spi_device(master, rd->dn);
-		put_device(&master->dev);
+		spi = of_register_spi_device(ctlr, rd->dn);
+		put_device(&ctlr->dev);
 
 		if (IS_ERR(spi)) {
 			pr_err("%s: failed to create for '%s'\n",
@@ -3224,7 +3347,7 @@ extern struct notifier_block spi_of_notifier;
 #endif /* IS_ENABLED(CONFIG_OF_DYNAMIC) */
 
 #if IS_ENABLED(CONFIG_ACPI)
-static int spi_acpi_master_match(struct device *dev, const void *data)
+static int spi_acpi_controller_match(struct device *dev, const void *data)
 {
 	return ACPI_COMPANION(dev->parent) == data;
 }
@@ -3234,16 +3357,19 @@ static int spi_acpi_device_match(struct device *dev, void *data)
 	return ACPI_COMPANION(dev) == data;
 }
 
-static struct spi_master *acpi_spi_find_master_by_adev(struct acpi_device *adev)
+static struct spi_controller *acpi_spi_find_controller_by_adev(struct acpi_device *adev)
 {
 	struct device *dev;
 
 	dev = class_find_device(&spi_master_class, NULL, adev,
-				spi_acpi_master_match);
+				spi_acpi_controller_match);
+	if (!dev && IS_ENABLED(CONFIG_SPI_SLAVE))
+		dev = class_find_device(&spi_slave_class, NULL, adev,
+					spi_acpi_controller_match);
 	if (!dev)
 		return NULL;
 
-	return container_of(dev, struct spi_master, dev);
+	return container_of(dev, struct spi_controller, dev);
 }
 
 static struct spi_device *acpi_spi_find_device_by_adev(struct acpi_device *adev)
@@ -3259,17 +3385,17 @@ static int acpi_spi_notify(struct notifier_block *nb, unsigned long value,
 			   void *arg)
 {
 	struct acpi_device *adev = arg;
-	struct spi_master *master;
+	struct spi_controller *ctlr;
 	struct spi_device *spi;
 
 	switch (value) {
 	case ACPI_RECONFIG_DEVICE_ADD:
-		master = acpi_spi_find_master_by_adev(adev->parent);
-		if (!master)
+		ctlr = acpi_spi_find_controller_by_adev(adev->parent);
+		if (!ctlr)
 			break;
 
-		acpi_register_spi_device(master, adev);
-		put_device(&master->dev);
+		acpi_register_spi_device(ctlr, adev);
+		put_device(&ctlr->dev);
 		break;
 	case ACPI_RECONFIG_DEVICE_REMOVE:
 		if (!acpi_device_enumerated(adev))
@@ -3312,6 +3438,12 @@ static int __init spi_init(void)
 	if (status < 0)
 		goto err2;
 
+	if (IS_ENABLED(CONFIG_SPI_SLAVE)) {
+		status = class_register(&spi_slave_class);
+		if (status < 0)
+			goto err3;
+	}
+
 	if (IS_ENABLED(CONFIG_OF_DYNAMIC))
 		WARN_ON(of_reconfig_notifier_register(&spi_of_notifier));
 	if (IS_ENABLED(CONFIG_ACPI))
@@ -3319,6 +3451,8 @@ static int __init spi_init(void)
 
 	return 0;
 
+err3:
+	class_unregister(&spi_master_class);
 err2:
 	bus_unregister(&spi_bus_type);
 err1:
diff --git a/drivers/spi/spidev.c b/drivers/spi/spidev.c
index 9a2a79a871ba..d4d2d8d9f3e7 100644
--- a/drivers/spi/spidev.c
+++ b/drivers/spi/spidev.c
@@ -99,7 +99,6 @@ MODULE_PARM_DESC(bufsiz, "data bytes in biggest supported SPI message");
 static ssize_t
 spidev_sync(struct spidev_data *spidev, struct spi_message *message)
 {
-	DECLARE_COMPLETION_ONSTACK(done);
 	int status;
 	struct spi_device *spi;
 
@@ -325,7 +324,6 @@ static struct spi_ioc_transfer *
 spidev_get_ioc_message(unsigned int cmd, struct spi_ioc_transfer __user *u_ioc,
 		unsigned *n_ioc)
 {
-	struct spi_ioc_transfer	*ioc;
 	u32	tmp;
 
 	/* Check type, command number and direction */
@@ -342,14 +340,7 @@ spidev_get_ioc_message(unsigned int cmd, struct spi_ioc_transfer __user *u_ioc,
 		return NULL;
 
 	/* copy into scratch area */
-	ioc = kmalloc(tmp, GFP_KERNEL);
-	if (!ioc)
-		return ERR_PTR(-ENOMEM);
-	if (__copy_from_user(ioc, u_ioc, tmp)) {
-		kfree(ioc);
-		return ERR_PTR(-EFAULT);
-	}
-	return ioc;
+	return memdup_user(u_ioc, tmp);
 }
 
 static long
diff --git a/drivers/spmi/spmi-pmic-arb.c b/drivers/spmi/spmi-pmic-arb.c
index 5ec3a595dc7d..2afe3597982e 100644
--- a/drivers/spmi/spmi-pmic-arb.c
+++ b/drivers/spmi/spmi-pmic-arb.c
@@ -28,6 +28,7 @@
 /* PMIC Arbiter configuration registers */
 #define PMIC_ARB_VERSION		0x0000
 #define PMIC_ARB_VERSION_V2_MIN		0x20010000
+#define PMIC_ARB_VERSION_V3_MIN		0x30000000
 #define PMIC_ARB_INT_EN			0x0004
 
 /* PMIC Arbiter channel registers offsets */
@@ -58,10 +59,10 @@
 
 /* Channel Status fields */
 enum pmic_arb_chnl_status {
-	PMIC_ARB_STATUS_DONE	= (1 << 0),
-	PMIC_ARB_STATUS_FAILURE	= (1 << 1),
-	PMIC_ARB_STATUS_DENIED	= (1 << 2),
-	PMIC_ARB_STATUS_DROPPED	= (1 << 3),
+	PMIC_ARB_STATUS_DONE	= BIT(0),
+	PMIC_ARB_STATUS_FAILURE	= BIT(1),
+	PMIC_ARB_STATUS_DENIED	= BIT(2),
+	PMIC_ARB_STATUS_DROPPED	= BIT(3),
 };
 
 /* Command register fields */
@@ -96,10 +97,26 @@ enum pmic_arb_cmd_op_code {
 /* interrupt enable bit */
 #define SPMI_PIC_ACC_ENABLE_BIT		BIT(0)
 
+#define HWIRQ(slave_id, periph_id, irq_id, apid) \
+	((((slave_id) & 0xF)   << 28) | \
+	(((periph_id) & 0xFF)  << 20) | \
+	(((irq_id)    & 0x7)   << 16) | \
+	(((apid)      & 0x1FF) << 0))
+
+#define HWIRQ_SID(hwirq)  (((hwirq) >> 28) & 0xF)
+#define HWIRQ_PER(hwirq)  (((hwirq) >> 20) & 0xFF)
+#define HWIRQ_IRQ(hwirq)  (((hwirq) >> 16) & 0x7)
+#define HWIRQ_APID(hwirq) (((hwirq) >> 0)  & 0x1FF)
+
 struct pmic_arb_ver_ops;
 
+struct apid_data {
+	u16		ppid;
+	u8		owner;
+};
+
 /**
- * spmi_pmic_arb_dev - SPMI PMIC Arbiter object
+ * spmi_pmic_arb - SPMI PMIC Arbiter object
  *
  * @rd_base:		on v1 "core", on v2 "observer" register base off DT.
  * @wr_base:		on v1 "core", on v2 "chnls"    register base off DT.
@@ -111,15 +128,15 @@ struct pmic_arb_ver_ops;
  * @ee:			the current Execution Environment
  * @min_apid:		minimum APID (used for bounding IRQ search)
  * @max_apid:		maximum APID
+ * @max_periph:		maximum number of PMIC peripherals supported by HW.
  * @mapping_table:	in-memory copy of PPID -> APID mapping table.
  * @domain:		irq domain object for PMIC IRQ domain
  * @spmic:		SPMI controller object
- * @apid_to_ppid:	in-memory copy of APID -> PPID mapping table.
  * @ver_ops:		version dependent operations.
- * @ppid_to_chan	in-memory copy of PPID -> channel (APID) mapping table.
+ * @ppid_to_apid	in-memory copy of PPID -> channel (APID) mapping table.
  *			v2 only.
  */
-struct spmi_pmic_arb_dev {
+struct spmi_pmic_arb {
 	void __iomem		*rd_base;
 	void __iomem		*wr_base;
 	void __iomem		*intr;
@@ -132,19 +149,23 @@ struct spmi_pmic_arb_dev {
 	u8			ee;
 	u16			min_apid;
 	u16			max_apid;
+	u16			max_periph;
 	u32			*mapping_table;
 	DECLARE_BITMAP(mapping_table_valid, PMIC_ARB_MAX_PERIPHS);
 	struct irq_domain	*domain;
 	struct spmi_controller	*spmic;
-	u16			*apid_to_ppid;
 	const struct pmic_arb_ver_ops *ver_ops;
-	u16			*ppid_to_chan;
-	u16			last_channel;
+	u16			*ppid_to_apid;
+	u16			last_apid;
+	struct apid_data	apid_data[PMIC_ARB_MAX_PERIPHS];
 };
 
 /**
  * pmic_arb_ver: version dependent functionality.
  *
+ * @ver_str:		version string.
+ * @ppid_to_apid:	finds the apid for a given ppid.
+ * @mode:		access rights to specified pmic peripheral.
  * @non_data_cmd:	on v1 issues an spmi non-data command.
  *			on v2 no HW support, returns -EOPNOTSUPP.
  * @offset:		on v1 offset of per-ee channel.
@@ -160,28 +181,33 @@ struct spmi_pmic_arb_dev {
  *			on v2 offset of SPMI_PIC_IRQ_CLEARn.
  */
 struct pmic_arb_ver_ops {
+	const char *ver_str;
+	int (*ppid_to_apid)(struct spmi_pmic_arb *pa, u8 sid, u16 addr,
+			u16 *apid);
+	int (*mode)(struct spmi_pmic_arb *dev, u8 sid, u16 addr,
+			mode_t *mode);
 	/* spmi commands (read_cmd, write_cmd, cmd) functionality */
-	int (*offset)(struct spmi_pmic_arb_dev *dev, u8 sid, u16 addr,
+	int (*offset)(struct spmi_pmic_arb *dev, u8 sid, u16 addr,
 		      u32 *offset);
 	u32 (*fmt_cmd)(u8 opc, u8 sid, u16 addr, u8 bc);
 	int (*non_data_cmd)(struct spmi_controller *ctrl, u8 opc, u8 sid);
 	/* Interrupts controller functionality (offset of PIC registers) */
-	u32 (*owner_acc_status)(u8 m, u8 n);
-	u32 (*acc_enable)(u8 n);
-	u32 (*irq_status)(u8 n);
-	u32 (*irq_clear)(u8 n);
+	u32 (*owner_acc_status)(u8 m, u16 n);
+	u32 (*acc_enable)(u16 n);
+	u32 (*irq_status)(u16 n);
+	u32 (*irq_clear)(u16 n);
 };
 
-static inline void pmic_arb_base_write(struct spmi_pmic_arb_dev *dev,
+static inline void pmic_arb_base_write(struct spmi_pmic_arb *pa,
 				       u32 offset, u32 val)
 {
-	writel_relaxed(val, dev->wr_base + offset);
+	writel_relaxed(val, pa->wr_base + offset);
 }
 
-static inline void pmic_arb_set_rd_cmd(struct spmi_pmic_arb_dev *dev,
+static inline void pmic_arb_set_rd_cmd(struct spmi_pmic_arb *pa,
 				       u32 offset, u32 val)
 {
-	writel_relaxed(val, dev->rd_base + offset);
+	writel_relaxed(val, pa->rd_base + offset);
 }
 
 /**
@@ -190,9 +216,10 @@ static inline void pmic_arb_set_rd_cmd(struct spmi_pmic_arb_dev *dev,
  * @reg:	register's address
  * @buf:	output parameter, length must be bc + 1
  */
-static void pa_read_data(struct spmi_pmic_arb_dev *dev, u8 *buf, u32 reg, u8 bc)
+static void pa_read_data(struct spmi_pmic_arb *pa, u8 *buf, u32 reg, u8 bc)
 {
-	u32 data = __raw_readl(dev->rd_base + reg);
+	u32 data = __raw_readl(pa->rd_base + reg);
+
 	memcpy(buf, &data, (bc & 3) + 1);
 }
 
@@ -203,23 +230,24 @@ static void pa_read_data(struct spmi_pmic_arb_dev *dev, u8 *buf, u32 reg, u8 bc)
  * @buf:	buffer to write. length must be bc + 1.
  */
 static void
-pa_write_data(struct spmi_pmic_arb_dev *dev, const u8 *buf, u32 reg, u8 bc)
+pa_write_data(struct spmi_pmic_arb *pa, const u8 *buf, u32 reg, u8 bc)
 {
 	u32 data = 0;
+
 	memcpy(&data, buf, (bc & 3) + 1);
-	__raw_writel(data, dev->wr_base + reg);
+	pmic_arb_base_write(pa, reg, data);
 }
 
 static int pmic_arb_wait_for_done(struct spmi_controller *ctrl,
 				  void __iomem *base, u8 sid, u16 addr)
 {
-	struct spmi_pmic_arb_dev *dev = spmi_controller_get_drvdata(ctrl);
+	struct spmi_pmic_arb *pa = spmi_controller_get_drvdata(ctrl);
 	u32 status = 0;
 	u32 timeout = PMIC_ARB_TIMEOUT_US;
 	u32 offset;
 	int rc;
 
-	rc = dev->ver_ops->offset(dev, sid, addr, &offset);
+	rc = pa->ver_ops->offset(pa, sid, addr, &offset);
 	if (rc)
 		return rc;
 
@@ -264,22 +292,22 @@ static int pmic_arb_wait_for_done(struct spmi_controller *ctrl,
 static int
 pmic_arb_non_data_cmd_v1(struct spmi_controller *ctrl, u8 opc, u8 sid)
 {
-	struct spmi_pmic_arb_dev *pmic_arb = spmi_controller_get_drvdata(ctrl);
+	struct spmi_pmic_arb *pa = spmi_controller_get_drvdata(ctrl);
 	unsigned long flags;
 	u32 cmd;
 	int rc;
 	u32 offset;
 
-	rc = pmic_arb->ver_ops->offset(pmic_arb, sid, 0, &offset);
+	rc = pa->ver_ops->offset(pa, sid, 0, &offset);
 	if (rc)
 		return rc;
 
 	cmd = ((opc | 0x40) << 27) | ((sid & 0xf) << 20);
 
-	raw_spin_lock_irqsave(&pmic_arb->lock, flags);
-	pmic_arb_base_write(pmic_arb, offset + PMIC_ARB_CMD, cmd);
-	rc = pmic_arb_wait_for_done(ctrl, pmic_arb->wr_base, sid, 0);
-	raw_spin_unlock_irqrestore(&pmic_arb->lock, flags);
+	raw_spin_lock_irqsave(&pa->lock, flags);
+	pmic_arb_base_write(pa, offset + PMIC_ARB_CMD, cmd);
+	rc = pmic_arb_wait_for_done(ctrl, pa->wr_base, sid, 0);
+	raw_spin_unlock_irqrestore(&pa->lock, flags);
 
 	return rc;
 }
@@ -293,7 +321,7 @@ pmic_arb_non_data_cmd_v2(struct spmi_controller *ctrl, u8 opc, u8 sid)
 /* Non-data command */
 static int pmic_arb_cmd(struct spmi_controller *ctrl, u8 opc, u8 sid)
 {
-	struct spmi_pmic_arb_dev *pmic_arb = spmi_controller_get_drvdata(ctrl);
+	struct spmi_pmic_arb *pa = spmi_controller_get_drvdata(ctrl);
 
 	dev_dbg(&ctrl->dev, "cmd op:0x%x sid:%d\n", opc, sid);
 
@@ -301,23 +329,35 @@ static int pmic_arb_cmd(struct spmi_controller *ctrl, u8 opc, u8 sid)
 	if (opc < SPMI_CMD_RESET || opc > SPMI_CMD_WAKEUP)
 		return -EINVAL;
 
-	return pmic_arb->ver_ops->non_data_cmd(ctrl, opc, sid);
+	return pa->ver_ops->non_data_cmd(ctrl, opc, sid);
 }
 
 static int pmic_arb_read_cmd(struct spmi_controller *ctrl, u8 opc, u8 sid,
 			     u16 addr, u8 *buf, size_t len)
 {
-	struct spmi_pmic_arb_dev *pmic_arb = spmi_controller_get_drvdata(ctrl);
+	struct spmi_pmic_arb *pa = spmi_controller_get_drvdata(ctrl);
 	unsigned long flags;
 	u8 bc = len - 1;
 	u32 cmd;
 	int rc;
 	u32 offset;
+	mode_t mode;
+
+	rc = pa->ver_ops->offset(pa, sid, addr, &offset);
+	if (rc)
+		return rc;
 
-	rc = pmic_arb->ver_ops->offset(pmic_arb, sid, addr, &offset);
+	rc = pa->ver_ops->mode(pa, sid, addr, &mode);
 	if (rc)
 		return rc;
 
+	if (!(mode & S_IRUSR)) {
+		dev_err(&pa->spmic->dev,
+			"error: impermissible read from peripheral sid:%d addr:0x%x\n",
+			sid, addr);
+		return -EPERM;
+	}
+
 	if (bc >= PMIC_ARB_MAX_TRANS_BYTES) {
 		dev_err(&ctrl->dev,
 			"pmic-arb supports 1..%d bytes per trans, but:%zu requested",
@@ -335,40 +375,51 @@ static int pmic_arb_read_cmd(struct spmi_controller *ctrl, u8 opc, u8 sid,
 	else
 		return -EINVAL;
 
-	cmd = pmic_arb->ver_ops->fmt_cmd(opc, sid, addr, bc);
+	cmd = pa->ver_ops->fmt_cmd(opc, sid, addr, bc);
 
-	raw_spin_lock_irqsave(&pmic_arb->lock, flags);
-	pmic_arb_set_rd_cmd(pmic_arb, offset + PMIC_ARB_CMD, cmd);
-	rc = pmic_arb_wait_for_done(ctrl, pmic_arb->rd_base, sid, addr);
+	raw_spin_lock_irqsave(&pa->lock, flags);
+	pmic_arb_set_rd_cmd(pa, offset + PMIC_ARB_CMD, cmd);
+	rc = pmic_arb_wait_for_done(ctrl, pa->rd_base, sid, addr);
 	if (rc)
 		goto done;
 
-	pa_read_data(pmic_arb, buf, offset + PMIC_ARB_RDATA0,
+	pa_read_data(pa, buf, offset + PMIC_ARB_RDATA0,
 		     min_t(u8, bc, 3));
 
 	if (bc > 3)
-		pa_read_data(pmic_arb, buf + 4,
-				offset + PMIC_ARB_RDATA1, bc - 4);
+		pa_read_data(pa, buf + 4, offset + PMIC_ARB_RDATA1, bc - 4);
 
 done:
-	raw_spin_unlock_irqrestore(&pmic_arb->lock, flags);
+	raw_spin_unlock_irqrestore(&pa->lock, flags);
 	return rc;
 }
 
 static int pmic_arb_write_cmd(struct spmi_controller *ctrl, u8 opc, u8 sid,
 			      u16 addr, const u8 *buf, size_t len)
 {
-	struct spmi_pmic_arb_dev *pmic_arb = spmi_controller_get_drvdata(ctrl);
+	struct spmi_pmic_arb *pa = spmi_controller_get_drvdata(ctrl);
 	unsigned long flags;
 	u8 bc = len - 1;
 	u32 cmd;
 	int rc;
 	u32 offset;
+	mode_t mode;
+
+	rc = pa->ver_ops->offset(pa, sid, addr, &offset);
+	if (rc)
+		return rc;
 
-	rc = pmic_arb->ver_ops->offset(pmic_arb, sid, addr, &offset);
+	rc = pa->ver_ops->mode(pa, sid, addr, &mode);
 	if (rc)
 		return rc;
 
+	if (!(mode & S_IWUSR)) {
+		dev_err(&pa->spmic->dev,
+			"error: impermissible write to peripheral sid:%d addr:0x%x\n",
+			sid, addr);
+		return -EPERM;
+	}
+
 	if (bc >= PMIC_ARB_MAX_TRANS_BYTES) {
 		dev_err(&ctrl->dev,
 			"pmic-arb supports 1..%d bytes per trans, but:%zu requested",
@@ -388,20 +439,18 @@ static int pmic_arb_write_cmd(struct spmi_controller *ctrl, u8 opc, u8 sid,
 	else
 		return -EINVAL;
 
-	cmd = pmic_arb->ver_ops->fmt_cmd(opc, sid, addr, bc);
+	cmd = pa->ver_ops->fmt_cmd(opc, sid, addr, bc);
 
 	/* Write data to FIFOs */
-	raw_spin_lock_irqsave(&pmic_arb->lock, flags);
-	pa_write_data(pmic_arb, buf, offset + PMIC_ARB_WDATA0,
-		      min_t(u8, bc, 3));
+	raw_spin_lock_irqsave(&pa->lock, flags);
+	pa_write_data(pa, buf, offset + PMIC_ARB_WDATA0, min_t(u8, bc, 3));
 	if (bc > 3)
-		pa_write_data(pmic_arb, buf + 4,
-				offset + PMIC_ARB_WDATA1, bc - 4);
+		pa_write_data(pa, buf + 4, offset + PMIC_ARB_WDATA1, bc - 4);
 
 	/* Start the transaction */
-	pmic_arb_base_write(pmic_arb, offset + PMIC_ARB_CMD, cmd);
-	rc = pmic_arb_wait_for_done(ctrl, pmic_arb->wr_base, sid, addr);
-	raw_spin_unlock_irqrestore(&pmic_arb->lock, flags);
+	pmic_arb_base_write(pa, offset + PMIC_ARB_CMD, cmd);
+	rc = pmic_arb_wait_for_done(ctrl, pa->wr_base, sid, addr);
+	raw_spin_unlock_irqrestore(&pa->lock, flags);
 
 	return rc;
 }
@@ -427,9 +476,9 @@ struct spmi_pmic_arb_qpnpint_type {
 static void qpnpint_spmi_write(struct irq_data *d, u8 reg, void *buf,
 			       size_t len)
 {
-	struct spmi_pmic_arb_dev *pa = irq_data_get_irq_chip_data(d);
-	u8 sid = d->hwirq >> 24;
-	u8 per = d->hwirq >> 16;
+	struct spmi_pmic_arb *pa = irq_data_get_irq_chip_data(d);
+	u8 sid = HWIRQ_SID(d->hwirq);
+	u8 per = HWIRQ_PER(d->hwirq);
 
 	if (pmic_arb_write_cmd(pa->spmic, SPMI_CMD_EXT_WRITEL, sid,
 			       (per << 8) + reg, buf, len))
@@ -440,9 +489,9 @@ static void qpnpint_spmi_write(struct irq_data *d, u8 reg, void *buf,
 
 static void qpnpint_spmi_read(struct irq_data *d, u8 reg, void *buf, size_t len)
 {
-	struct spmi_pmic_arb_dev *pa = irq_data_get_irq_chip_data(d);
-	u8 sid = d->hwirq >> 24;
-	u8 per = d->hwirq >> 16;
+	struct spmi_pmic_arb *pa = irq_data_get_irq_chip_data(d);
+	u8 sid = HWIRQ_SID(d->hwirq);
+	u8 per = HWIRQ_PER(d->hwirq);
 
 	if (pmic_arb_read_cmd(pa->spmic, SPMI_CMD_EXT_READL, sid,
 			      (per << 8) + reg, buf, len))
@@ -451,33 +500,58 @@ static void qpnpint_spmi_read(struct irq_data *d, u8 reg, void *buf, size_t len)
 				    d->irq);
 }
 
-static void periph_interrupt(struct spmi_pmic_arb_dev *pa, u8 apid)
+static void cleanup_irq(struct spmi_pmic_arb *pa, u16 apid, int id)
+{
+	u16 ppid = pa->apid_data[apid].ppid;
+	u8 sid = ppid >> 8;
+	u8 per = ppid & 0xFF;
+	u8 irq_mask = BIT(id);
+
+	writel_relaxed(irq_mask, pa->intr + pa->ver_ops->irq_clear(apid));
+
+	if (pmic_arb_write_cmd(pa->spmic, SPMI_CMD_EXT_WRITEL, sid,
+			(per << 8) + QPNPINT_REG_LATCHED_CLR, &irq_mask, 1))
+		dev_err_ratelimited(&pa->spmic->dev,
+				"failed to ack irq_mask = 0x%x for ppid = %x\n",
+				irq_mask, ppid);
+
+	if (pmic_arb_write_cmd(pa->spmic, SPMI_CMD_EXT_WRITEL, sid,
+			       (per << 8) + QPNPINT_REG_EN_CLR, &irq_mask, 1))
+		dev_err_ratelimited(&pa->spmic->dev,
+				"failed to ack irq_mask = 0x%x for ppid = %x\n",
+				irq_mask, ppid);
+}
+
+static void periph_interrupt(struct spmi_pmic_arb *pa, u16 apid)
 {
 	unsigned int irq;
 	u32 status;
 	int id;
+	u8 sid = (pa->apid_data[apid].ppid >> 8) & 0xF;
+	u8 per = pa->apid_data[apid].ppid & 0xFF;
 
 	status = readl_relaxed(pa->intr + pa->ver_ops->irq_status(apid));
 	while (status) {
 		id = ffs(status) - 1;
-		status &= ~(1 << id);
-		irq = irq_find_mapping(pa->domain,
-				       pa->apid_to_ppid[apid] << 16
-				     | id << 8
-				     | apid);
+		status &= ~BIT(id);
+		irq = irq_find_mapping(pa->domain, HWIRQ(sid, per, id, apid));
+		if (irq == 0) {
+			cleanup_irq(pa, apid, id);
+			continue;
+		}
 		generic_handle_irq(irq);
 	}
 }
 
 static void pmic_arb_chained_irq(struct irq_desc *desc)
 {
-	struct spmi_pmic_arb_dev *pa = irq_desc_get_handler_data(desc);
+	struct spmi_pmic_arb *pa = irq_desc_get_handler_data(desc);
 	struct irq_chip *chip = irq_desc_get_chip(desc);
 	void __iomem *intr = pa->intr;
 	int first = pa->min_apid >> 5;
 	int last = pa->max_apid >> 5;
-	u32 status;
-	int i, id;
+	u32 status, enable;
+	int i, id, apid;
 
 	chained_irq_enter(chip, desc);
 
@@ -486,8 +560,12 @@ static void pmic_arb_chained_irq(struct irq_desc *desc)
 				      pa->ver_ops->owner_acc_status(pa->ee, i));
 		while (status) {
 			id = ffs(status) - 1;
-			status &= ~(1 << id);
-			periph_interrupt(pa, id + i * 32);
+			status &= ~BIT(id);
+			apid = id + i * 32;
+			enable = readl_relaxed(intr +
+					pa->ver_ops->acc_enable(apid));
+			if (enable & SPMI_PIC_ACC_ENABLE_BIT)
+				periph_interrupt(pa, apid);
 		}
 	}
 
@@ -496,100 +574,81 @@ static void pmic_arb_chained_irq(struct irq_desc *desc)
 
 static void qpnpint_irq_ack(struct irq_data *d)
 {
-	struct spmi_pmic_arb_dev *pa = irq_data_get_irq_chip_data(d);
-	u8 irq  = d->hwirq >> 8;
-	u8 apid = d->hwirq;
-	unsigned long flags;
+	struct spmi_pmic_arb *pa = irq_data_get_irq_chip_data(d);
+	u8 irq = HWIRQ_IRQ(d->hwirq);
+	u16 apid = HWIRQ_APID(d->hwirq);
 	u8 data;
 
-	raw_spin_lock_irqsave(&pa->lock, flags);
-	writel_relaxed(1 << irq, pa->intr + pa->ver_ops->irq_clear(apid));
-	raw_spin_unlock_irqrestore(&pa->lock, flags);
+	writel_relaxed(BIT(irq), pa->intr + pa->ver_ops->irq_clear(apid));
 
-	data = 1 << irq;
+	data = BIT(irq);
 	qpnpint_spmi_write(d, QPNPINT_REG_LATCHED_CLR, &data, 1);
 }
 
 static void qpnpint_irq_mask(struct irq_data *d)
 {
-	struct spmi_pmic_arb_dev *pa = irq_data_get_irq_chip_data(d);
-	u8 irq  = d->hwirq >> 8;
-	u8 apid = d->hwirq;
-	unsigned long flags;
-	u32 status;
-	u8 data;
+	u8 irq = HWIRQ_IRQ(d->hwirq);
+	u8 data = BIT(irq);
 
-	raw_spin_lock_irqsave(&pa->lock, flags);
-	status = readl_relaxed(pa->intr + pa->ver_ops->acc_enable(apid));
-	if (status & SPMI_PIC_ACC_ENABLE_BIT) {
-		status = status & ~SPMI_PIC_ACC_ENABLE_BIT;
-		writel_relaxed(status, pa->intr +
-			       pa->ver_ops->acc_enable(apid));
-	}
-	raw_spin_unlock_irqrestore(&pa->lock, flags);
-
-	data = 1 << irq;
 	qpnpint_spmi_write(d, QPNPINT_REG_EN_CLR, &data, 1);
 }
 
 static void qpnpint_irq_unmask(struct irq_data *d)
 {
-	struct spmi_pmic_arb_dev *pa = irq_data_get_irq_chip_data(d);
-	u8 irq  = d->hwirq >> 8;
-	u8 apid = d->hwirq;
-	unsigned long flags;
-	u32 status;
-	u8 data;
-
-	raw_spin_lock_irqsave(&pa->lock, flags);
-	status = readl_relaxed(pa->intr + pa->ver_ops->acc_enable(apid));
-	if (!(status & SPMI_PIC_ACC_ENABLE_BIT)) {
-		writel_relaxed(status | SPMI_PIC_ACC_ENABLE_BIT,
-				pa->intr + pa->ver_ops->acc_enable(apid));
+	struct spmi_pmic_arb *pa = irq_data_get_irq_chip_data(d);
+	u8 irq = HWIRQ_IRQ(d->hwirq);
+	u16 apid = HWIRQ_APID(d->hwirq);
+	u8 buf[2];
+
+	writel_relaxed(SPMI_PIC_ACC_ENABLE_BIT,
+		pa->intr + pa->ver_ops->acc_enable(apid));
+
+	qpnpint_spmi_read(d, QPNPINT_REG_EN_SET, &buf[0], 1);
+	if (!(buf[0] & BIT(irq))) {
+		/*
+		 * Since the interrupt is currently disabled, write to both the
+		 * LATCHED_CLR and EN_SET registers so that a spurious interrupt
+		 * cannot be triggered when the interrupt is enabled
+		 */
+		buf[0] = BIT(irq);
+		buf[1] = BIT(irq);
+		qpnpint_spmi_write(d, QPNPINT_REG_LATCHED_CLR, &buf, 2);
 	}
-	raw_spin_unlock_irqrestore(&pa->lock, flags);
-
-	data = 1 << irq;
-	qpnpint_spmi_write(d, QPNPINT_REG_EN_SET, &data, 1);
-}
-
-static void qpnpint_irq_enable(struct irq_data *d)
-{
-	u8 irq  = d->hwirq >> 8;
-	u8 data;
-
-	qpnpint_irq_unmask(d);
-
-	data = 1 << irq;
-	qpnpint_spmi_write(d, QPNPINT_REG_LATCHED_CLR, &data, 1);
 }
 
 static int qpnpint_irq_set_type(struct irq_data *d, unsigned int flow_type)
 {
 	struct spmi_pmic_arb_qpnpint_type type;
-	u8 irq = d->hwirq >> 8;
+	u8 irq = HWIRQ_IRQ(d->hwirq);
+	u8 bit_mask_irq = BIT(irq);
 
 	qpnpint_spmi_read(d, QPNPINT_REG_SET_TYPE, &type, sizeof(type));
 
 	if (flow_type & (IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING)) {
-		type.type |= 1 << irq;
+		type.type |= bit_mask_irq;
 		if (flow_type & IRQF_TRIGGER_RISING)
-			type.polarity_high |= 1 << irq;
+			type.polarity_high |= bit_mask_irq;
 		if (flow_type & IRQF_TRIGGER_FALLING)
-			type.polarity_low  |= 1 << irq;
+			type.polarity_low  |= bit_mask_irq;
 	} else {
 		if ((flow_type & (IRQF_TRIGGER_HIGH)) &&
 		    (flow_type & (IRQF_TRIGGER_LOW)))
 			return -EINVAL;
 
-		type.type &= ~(1 << irq); /* level trig */
+		type.type &= ~bit_mask_irq; /* level trig */
 		if (flow_type & IRQF_TRIGGER_HIGH)
-			type.polarity_high |= 1 << irq;
+			type.polarity_high |= bit_mask_irq;
 		else
-			type.polarity_low  |= 1 << irq;
+			type.polarity_low  |= bit_mask_irq;
 	}
 
 	qpnpint_spmi_write(d, QPNPINT_REG_SET_TYPE, &type, sizeof(type));
+
+	if (flow_type & IRQ_TYPE_EDGE_BOTH)
+		irq_set_handler_locked(d, handle_edge_irq);
+	else
+		irq_set_handler_locked(d, handle_level_irq);
+
 	return 0;
 }
 
@@ -597,7 +656,7 @@ static int qpnpint_get_irqchip_state(struct irq_data *d,
 				     enum irqchip_irq_state which,
 				     bool *state)
 {
-	u8 irq = d->hwirq >> 8;
+	u8 irq = HWIRQ_IRQ(d->hwirq);
 	u8 status = 0;
 
 	if (which != IRQCHIP_STATE_LINE_LEVEL)
@@ -611,7 +670,6 @@ static int qpnpint_get_irqchip_state(struct irq_data *d,
 
 static struct irq_chip pmic_arb_irqchip = {
 	.name		= "pmic_arb",
-	.irq_enable	= qpnpint_irq_enable,
 	.irq_ack	= qpnpint_irq_ack,
 	.irq_mask	= qpnpint_irq_mask,
 	.irq_unmask	= qpnpint_irq_unmask,
@@ -621,48 +679,6 @@ static struct irq_chip pmic_arb_irqchip = {
 			| IRQCHIP_SKIP_SET_WAKE,
 };
 
-struct spmi_pmic_arb_irq_spec {
-	unsigned slave:4;
-	unsigned per:8;
-	unsigned irq:3;
-};
-
-static int search_mapping_table(struct spmi_pmic_arb_dev *pa,
-				struct spmi_pmic_arb_irq_spec *spec,
-				u8 *apid)
-{
-	u16 ppid = spec->slave << 8 | spec->per;
-	u32 *mapping_table = pa->mapping_table;
-	int index = 0, i;
-	u32 data;
-
-	for (i = 0; i < SPMI_MAPPING_TABLE_TREE_DEPTH; ++i) {
-		if (!test_and_set_bit(index, pa->mapping_table_valid))
-			mapping_table[index] = readl_relaxed(pa->cnfg +
-						SPMI_MAPPING_TABLE_REG(index));
-
-		data = mapping_table[index];
-
-		if (ppid & (1 << SPMI_MAPPING_BIT_INDEX(data))) {
-			if (SPMI_MAPPING_BIT_IS_1_FLAG(data)) {
-				index = SPMI_MAPPING_BIT_IS_1_RESULT(data);
-			} else {
-				*apid = SPMI_MAPPING_BIT_IS_1_RESULT(data);
-				return 0;
-			}
-		} else {
-			if (SPMI_MAPPING_BIT_IS_0_FLAG(data)) {
-				index = SPMI_MAPPING_BIT_IS_0_RESULT(data);
-			} else {
-				*apid = SPMI_MAPPING_BIT_IS_0_RESULT(data);
-				return 0;
-			}
-		}
-	}
-
-	return -ENODEV;
-}
-
 static int qpnpint_irq_domain_dt_translate(struct irq_domain *d,
 					   struct device_node *controller,
 					   const u32 *intspec,
@@ -670,10 +686,9 @@ static int qpnpint_irq_domain_dt_translate(struct irq_domain *d,
 					   unsigned long *out_hwirq,
 					   unsigned int *out_type)
 {
-	struct spmi_pmic_arb_dev *pa = d->host_data;
-	struct spmi_pmic_arb_irq_spec spec;
-	int err;
-	u8 apid;
+	struct spmi_pmic_arb *pa = d->host_data;
+	int rc;
+	u16 apid;
 
 	dev_dbg(&pa->spmic->dev,
 		"intspec[0] 0x%1x intspec[1] 0x%02x intspec[2] 0x%02x\n",
@@ -686,15 +701,14 @@ static int qpnpint_irq_domain_dt_translate(struct irq_domain *d,
 	if (intspec[0] > 0xF || intspec[1] > 0xFF || intspec[2] > 0x7)
 		return -EINVAL;
 
-	spec.slave = intspec[0];
-	spec.per   = intspec[1];
-	spec.irq   = intspec[2];
-
-	err = search_mapping_table(pa, &spec, &apid);
-	if (err)
-		return err;
-
-	pa->apid_to_ppid[apid] = spec.slave << 8 | spec.per;
+	rc = pa->ver_ops->ppid_to_apid(pa, intspec[0],
+			(intspec[1] << 8), &apid);
+	if (rc < 0) {
+		dev_err(&pa->spmic->dev,
+		"failed to xlate sid = 0x%x, periph = 0x%x, irq = %x rc = %d\n",
+		intspec[0], intspec[1], intspec[2], rc);
+		return rc;
+	}
 
 	/* Keep track of {max,min}_apid for bounding search during interrupt */
 	if (apid > pa->max_apid)
@@ -702,10 +716,7 @@ static int qpnpint_irq_domain_dt_translate(struct irq_domain *d,
 	if (apid < pa->min_apid)
 		pa->min_apid = apid;
 
-	*out_hwirq = spec.slave << 24
-		   | spec.per   << 16
-		   | spec.irq   << 8
-		   | apid;
+	*out_hwirq = HWIRQ(intspec[0], intspec[1], intspec[2], apid);
 	*out_type  = intspec[3] & IRQ_TYPE_SENSE_MASK;
 
 	dev_dbg(&pa->spmic->dev, "out_hwirq = %lu\n", *out_hwirq);
@@ -717,7 +728,7 @@ static int qpnpint_irq_domain_map(struct irq_domain *d,
 				  unsigned int virq,
 				  irq_hw_number_t hwirq)
 {
-	struct spmi_pmic_arb_dev *pa = d->host_data;
+	struct spmi_pmic_arb *pa = d->host_data;
 
 	dev_dbg(&pa->spmic->dev, "virq = %u, hwirq = %lu\n", virq, hwirq);
 
@@ -727,26 +738,85 @@ static int qpnpint_irq_domain_map(struct irq_domain *d,
 	return 0;
 }
 
+static int
+pmic_arb_ppid_to_apid_v1(struct spmi_pmic_arb *pa, u8 sid, u16 addr, u16 *apid)
+{
+	u16 ppid = sid << 8 | ((addr >> 8) & 0xFF);
+	u32 *mapping_table = pa->mapping_table;
+	int index = 0, i;
+	u16 apid_valid;
+	u32 data;
+
+	apid_valid = pa->ppid_to_apid[ppid];
+	if (apid_valid & PMIC_ARB_CHAN_VALID) {
+		*apid = (apid_valid & ~PMIC_ARB_CHAN_VALID);
+		return 0;
+	}
+
+	for (i = 0; i < SPMI_MAPPING_TABLE_TREE_DEPTH; ++i) {
+		if (!test_and_set_bit(index, pa->mapping_table_valid))
+			mapping_table[index] = readl_relaxed(pa->cnfg +
+						SPMI_MAPPING_TABLE_REG(index));
+
+		data = mapping_table[index];
+
+		if (ppid & BIT(SPMI_MAPPING_BIT_INDEX(data))) {
+			if (SPMI_MAPPING_BIT_IS_1_FLAG(data)) {
+				index = SPMI_MAPPING_BIT_IS_1_RESULT(data);
+			} else {
+				*apid = SPMI_MAPPING_BIT_IS_1_RESULT(data);
+				pa->ppid_to_apid[ppid]
+					= *apid | PMIC_ARB_CHAN_VALID;
+				pa->apid_data[*apid].ppid = ppid;
+				return 0;
+			}
+		} else {
+			if (SPMI_MAPPING_BIT_IS_0_FLAG(data)) {
+				index = SPMI_MAPPING_BIT_IS_0_RESULT(data);
+			} else {
+				*apid = SPMI_MAPPING_BIT_IS_0_RESULT(data);
+				pa->ppid_to_apid[ppid]
+					= *apid | PMIC_ARB_CHAN_VALID;
+				pa->apid_data[*apid].ppid = ppid;
+				return 0;
+			}
+		}
+	}
+
+	return -ENODEV;
+}
+
+static int
+pmic_arb_mode_v1_v3(struct spmi_pmic_arb *pa, u8 sid, u16 addr, mode_t *mode)
+{
+	*mode = S_IRUSR | S_IWUSR;
+	return 0;
+}
+
 /* v1 offset per ee */
 static int
-pmic_arb_offset_v1(struct spmi_pmic_arb_dev *pa, u8 sid, u16 addr, u32 *offset)
+pmic_arb_offset_v1(struct spmi_pmic_arb *pa, u8 sid, u16 addr, u32 *offset)
 {
 	*offset = 0x800 + 0x80 * pa->channel;
 	return 0;
 }
 
-static u16 pmic_arb_find_chan(struct spmi_pmic_arb_dev *pa, u16 ppid)
+static u16 pmic_arb_find_apid(struct spmi_pmic_arb *pa, u16 ppid)
 {
 	u32 regval, offset;
-	u16 chan;
+	u16 apid;
 	u16 id;
 
 	/*
 	 * PMIC_ARB_REG_CHNL is a table in HW mapping channel to ppid.
-	 * ppid_to_chan is an in-memory invert of that table.
+	 * ppid_to_apid is an in-memory invert of that table.
 	 */
-	for (chan = pa->last_channel; ; chan++) {
-		offset = PMIC_ARB_REG_CHNL(chan);
+	for (apid = pa->last_apid; apid < pa->max_periph; apid++) {
+		regval = readl_relaxed(pa->cnfg +
+				      SPMI_OWNERSHIP_TABLE_REG(apid));
+		pa->apid_data[apid].owner = SPMI_OWNERSHIP_PERIPH2OWNER(regval);
+
+		offset = PMIC_ARB_REG_CHNL(apid);
 		if (offset >= pa->core_size)
 			break;
 
@@ -755,33 +825,65 @@ static u16 pmic_arb_find_chan(struct spmi_pmic_arb_dev *pa, u16 ppid)
 			continue;
 
 		id = (regval >> 8) & PMIC_ARB_PPID_MASK;
-		pa->ppid_to_chan[id] = chan | PMIC_ARB_CHAN_VALID;
+		pa->ppid_to_apid[id] = apid | PMIC_ARB_CHAN_VALID;
+		pa->apid_data[apid].ppid = id;
 		if (id == ppid) {
-			chan |= PMIC_ARB_CHAN_VALID;
+			apid |= PMIC_ARB_CHAN_VALID;
 			break;
 		}
 	}
-	pa->last_channel = chan & ~PMIC_ARB_CHAN_VALID;
+	pa->last_apid = apid & ~PMIC_ARB_CHAN_VALID;
 
-	return chan;
+	return apid;
 }
 
 
-/* v2 offset per ppid (chan) and per ee */
 static int
-pmic_arb_offset_v2(struct spmi_pmic_arb_dev *pa, u8 sid, u16 addr, u32 *offset)
+pmic_arb_ppid_to_apid_v2(struct spmi_pmic_arb *pa, u8 sid, u16 addr, u16 *apid)
 {
 	u16 ppid = (sid << 8) | (addr >> 8);
-	u16 chan;
+	u16 apid_valid;
 
-	chan = pa->ppid_to_chan[ppid];
-	if (!(chan & PMIC_ARB_CHAN_VALID))
-		chan = pmic_arb_find_chan(pa, ppid);
-	if (!(chan & PMIC_ARB_CHAN_VALID))
+	apid_valid = pa->ppid_to_apid[ppid];
+	if (!(apid_valid & PMIC_ARB_CHAN_VALID))
+		apid_valid = pmic_arb_find_apid(pa, ppid);
+	if (!(apid_valid & PMIC_ARB_CHAN_VALID))
 		return -ENODEV;
-	chan &= ~PMIC_ARB_CHAN_VALID;
 
-	*offset = 0x1000 * pa->ee + 0x8000 * chan;
+	*apid = (apid_valid & ~PMIC_ARB_CHAN_VALID);
+	return 0;
+}
+
+static int
+pmic_arb_mode_v2(struct spmi_pmic_arb *pa, u8 sid, u16 addr, mode_t *mode)
+{
+	u16 apid;
+	int rc;
+
+	rc = pmic_arb_ppid_to_apid_v2(pa, sid, addr, &apid);
+	if (rc < 0)
+		return rc;
+
+	*mode = 0;
+	*mode |= S_IRUSR;
+
+	if (pa->ee == pa->apid_data[apid].owner)
+		*mode |= S_IWUSR;
+	return 0;
+}
+
+/* v2 offset per ppid and per ee */
+static int
+pmic_arb_offset_v2(struct spmi_pmic_arb *pa, u8 sid, u16 addr, u32 *offset)
+{
+	u16 apid;
+	int rc;
+
+	rc = pmic_arb_ppid_to_apid_v2(pa, sid, addr, &apid);
+	if (rc < 0)
+		return rc;
+
+	*offset = 0x1000 * pa->ee + 0x8000 * apid;
 	return 0;
 }
 
@@ -795,47 +897,55 @@ static u32 pmic_arb_fmt_cmd_v2(u8 opc, u8 sid, u16 addr, u8 bc)
 	return (opc << 27) | ((addr & 0xff) << 4) | (bc & 0x7);
 }
 
-static u32 pmic_arb_owner_acc_status_v1(u8 m, u8 n)
+static u32 pmic_arb_owner_acc_status_v1(u8 m, u16 n)
 {
 	return 0x20 * m + 0x4 * n;
 }
 
-static u32 pmic_arb_owner_acc_status_v2(u8 m, u8 n)
+static u32 pmic_arb_owner_acc_status_v2(u8 m, u16 n)
 {
 	return 0x100000 + 0x1000 * m + 0x4 * n;
 }
 
-static u32 pmic_arb_acc_enable_v1(u8 n)
+static u32 pmic_arb_owner_acc_status_v3(u8 m, u16 n)
+{
+	return 0x200000 + 0x1000 * m + 0x4 * n;
+}
+
+static u32 pmic_arb_acc_enable_v1(u16 n)
 {
 	return 0x200 + 0x4 * n;
 }
 
-static u32 pmic_arb_acc_enable_v2(u8 n)
+static u32 pmic_arb_acc_enable_v2(u16 n)
 {
 	return 0x1000 * n;
 }
 
-static u32 pmic_arb_irq_status_v1(u8 n)
+static u32 pmic_arb_irq_status_v1(u16 n)
 {
 	return 0x600 + 0x4 * n;
 }
 
-static u32 pmic_arb_irq_status_v2(u8 n)
+static u32 pmic_arb_irq_status_v2(u16 n)
 {
 	return 0x4 + 0x1000 * n;
 }
 
-static u32 pmic_arb_irq_clear_v1(u8 n)
+static u32 pmic_arb_irq_clear_v1(u16 n)
 {
 	return 0xA00 + 0x4 * n;
 }
 
-static u32 pmic_arb_irq_clear_v2(u8 n)
+static u32 pmic_arb_irq_clear_v2(u16 n)
 {
 	return 0x8 + 0x1000 * n;
 }
 
 static const struct pmic_arb_ver_ops pmic_arb_v1 = {
+	.ver_str		= "v1",
+	.ppid_to_apid		= pmic_arb_ppid_to_apid_v1,
+	.mode			= pmic_arb_mode_v1_v3,
 	.non_data_cmd		= pmic_arb_non_data_cmd_v1,
 	.offset			= pmic_arb_offset_v1,
 	.fmt_cmd		= pmic_arb_fmt_cmd_v1,
@@ -846,6 +956,9 @@ static const struct pmic_arb_ver_ops pmic_arb_v1 = {
 };
 
 static const struct pmic_arb_ver_ops pmic_arb_v2 = {
+	.ver_str		= "v2",
+	.ppid_to_apid		= pmic_arb_ppid_to_apid_v2,
+	.mode			= pmic_arb_mode_v2,
 	.non_data_cmd		= pmic_arb_non_data_cmd_v2,
 	.offset			= pmic_arb_offset_v2,
 	.fmt_cmd		= pmic_arb_fmt_cmd_v2,
@@ -855,6 +968,19 @@ static const struct pmic_arb_ver_ops pmic_arb_v2 = {
 	.irq_clear		= pmic_arb_irq_clear_v2,
 };
 
+static const struct pmic_arb_ver_ops pmic_arb_v3 = {
+	.ver_str		= "v3",
+	.ppid_to_apid		= pmic_arb_ppid_to_apid_v2,
+	.mode			= pmic_arb_mode_v1_v3,
+	.non_data_cmd		= pmic_arb_non_data_cmd_v2,
+	.offset			= pmic_arb_offset_v2,
+	.fmt_cmd		= pmic_arb_fmt_cmd_v2,
+	.owner_acc_status	= pmic_arb_owner_acc_status_v3,
+	.acc_enable		= pmic_arb_acc_enable_v2,
+	.irq_status		= pmic_arb_irq_status_v2,
+	.irq_clear		= pmic_arb_irq_clear_v2,
+};
+
 static const struct irq_domain_ops pmic_arb_irq_domain_ops = {
 	.map	= qpnpint_irq_domain_map,
 	.xlate	= qpnpint_irq_domain_dt_translate,
@@ -862,13 +988,12 @@ static const struct irq_domain_ops pmic_arb_irq_domain_ops = {
 
 static int spmi_pmic_arb_probe(struct platform_device *pdev)
 {
-	struct spmi_pmic_arb_dev *pa;
+	struct spmi_pmic_arb *pa;
 	struct spmi_controller *ctrl;
 	struct resource *res;
 	void __iomem *core;
 	u32 channel, ee, hw_ver;
 	int err;
-	bool is_v1;
 
 	ctrl = spmi_controller_alloc(&pdev->dev, sizeof(*pa));
 	if (!ctrl)
@@ -879,6 +1004,12 @@ static int spmi_pmic_arb_probe(struct platform_device *pdev)
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "core");
 	pa->core_size = resource_size(res);
+	if (pa->core_size <= 0x800) {
+		dev_err(&pdev->dev, "core_size is smaller than 0x800. Failing Probe\n");
+		err = -EINVAL;
+		goto err_put_ctrl;
+	}
+
 	core = devm_ioremap_resource(&ctrl->dev, res);
 	if (IS_ERR(core)) {
 		err = PTR_ERR(core);
@@ -886,18 +1017,21 @@ static int spmi_pmic_arb_probe(struct platform_device *pdev)
 	}
 
 	hw_ver = readl_relaxed(core + PMIC_ARB_VERSION);
-	is_v1  = (hw_ver < PMIC_ARB_VERSION_V2_MIN);
-
-	dev_info(&ctrl->dev, "PMIC Arb Version-%d (0x%x)\n", (is_v1 ? 1 : 2),
-		hw_ver);
 
-	if (is_v1) {
+	if (hw_ver < PMIC_ARB_VERSION_V2_MIN) {
 		pa->ver_ops = &pmic_arb_v1;
 		pa->wr_base = core;
 		pa->rd_base = core;
 	} else {
 		pa->core = core;
-		pa->ver_ops = &pmic_arb_v2;
+
+		if (hw_ver < PMIC_ARB_VERSION_V3_MIN)
+			pa->ver_ops = &pmic_arb_v2;
+		else
+			pa->ver_ops = &pmic_arb_v3;
+
+		/* the apid to ppid table starts at PMIC_ARB_REG_CHNL(0) */
+		pa->max_periph = (pa->core_size - PMIC_ARB_REG_CHNL(0)) / 4;
 
 		res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
 						   "obsrvr");
@@ -915,16 +1049,19 @@ static int spmi_pmic_arb_probe(struct platform_device *pdev)
 			goto err_put_ctrl;
 		}
 
-		pa->ppid_to_chan = devm_kcalloc(&ctrl->dev,
+		pa->ppid_to_apid = devm_kcalloc(&ctrl->dev,
 						PMIC_ARB_MAX_PPID,
-						sizeof(*pa->ppid_to_chan),
+						sizeof(*pa->ppid_to_apid),
 						GFP_KERNEL);
-		if (!pa->ppid_to_chan) {
+		if (!pa->ppid_to_apid) {
 			err = -ENOMEM;
 			goto err_put_ctrl;
 		}
 	}
 
+	dev_info(&ctrl->dev, "PMIC arbiter version %s (0x%x)\n",
+		 pa->ver_ops->ver_str, hw_ver);
+
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "intr");
 	pa->intr = devm_ioremap_resource(&ctrl->dev, res);
 	if (IS_ERR(pa->intr)) {
@@ -974,14 +1111,6 @@ static int spmi_pmic_arb_probe(struct platform_device *pdev)
 
 	pa->ee = ee;
 
-	pa->apid_to_ppid = devm_kcalloc(&ctrl->dev, PMIC_ARB_MAX_PERIPHS,
-					    sizeof(*pa->apid_to_ppid),
-					    GFP_KERNEL);
-	if (!pa->apid_to_ppid) {
-		err = -ENOMEM;
-		goto err_put_ctrl;
-	}
-
 	pa->mapping_table = devm_kcalloc(&ctrl->dev, PMIC_ARB_MAX_PERIPHS - 1,
 					sizeof(*pa->mapping_table), GFP_KERNEL);
 	if (!pa->mapping_table) {
@@ -1011,6 +1140,7 @@ static int spmi_pmic_arb_probe(struct platform_device *pdev)
 	}
 
 	irq_set_chained_handler_and_data(pa->irq, pmic_arb_chained_irq, pa);
+	enable_irq_wake(pa->irq);
 
 	err = spmi_controller_add(ctrl);
 	if (err)
@@ -1029,7 +1159,7 @@ err_put_ctrl:
 static int spmi_pmic_arb_remove(struct platform_device *pdev)
 {
 	struct spmi_controller *ctrl = platform_get_drvdata(pdev);
-	struct spmi_pmic_arb_dev *pa = spmi_controller_get_drvdata(ctrl);
+	struct spmi_pmic_arb *pa = spmi_controller_get_drvdata(ctrl);
 	spmi_controller_remove(ctrl);
 	irq_set_chained_handler_and_data(pa->irq, NULL, NULL);
 	irq_domain_remove(pa->domain);
diff --git a/drivers/ssb/main.c b/drivers/ssb/main.c
index d1a750760cf3..65420a9f0e82 100644
--- a/drivers/ssb/main.c
+++ b/drivers/ssb/main.c
@@ -480,7 +480,6 @@ static int ssb_devices_register(struct ssb_bus *bus)
 
 		devwrap = kzalloc(sizeof(*devwrap), GFP_KERNEL);
 		if (!devwrap) {
-			ssb_err("Could not allocate device\n");
 			err = -ENOMEM;
 			goto error;
 		}
diff --git a/drivers/staging/android/ion/ion-ioctl.c b/drivers/staging/android/ion/ion-ioctl.c
index 76427e4773a8..d9f8b1424da1 100644
--- a/drivers/staging/android/ion/ion-ioctl.c
+++ b/drivers/staging/android/ion/ion-ioctl.c
@@ -83,8 +83,8 @@ long ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 		int fd;
 
 		fd = ion_alloc(data.allocation.len,
-						data.allocation.heap_id_mask,
-						data.allocation.flags);
+			       data.allocation.heap_id_mask,
+			       data.allocation.flags);
 		if (fd < 0)
 			return fd;
 
diff --git a/drivers/staging/android/ion/ion.c b/drivers/staging/android/ion/ion.c
index 03d3a4fce0e2..93e2c90fa77d 100644
--- a/drivers/staging/android/ion/ion.c
+++ b/drivers/staging/android/ion/ion.c
@@ -103,7 +103,7 @@ static struct ion_buffer *ion_buffer_create(struct ion_heap *heap,
 			goto err2;
 	}
 
-	if (buffer->sg_table == NULL) {
+	if (!buffer->sg_table) {
 		WARN_ONCE(1, "This heap needs to set the sgtable");
 		ret = -EINVAL;
 		goto err1;
@@ -115,7 +115,6 @@ static struct ion_buffer *ion_buffer_create(struct ion_heap *heap,
 
 	buffer->dev = dev;
 	buffer->size = len;
-	INIT_LIST_HEAD(&buffer->vmas);
 	INIT_LIST_HEAD(&buffer->attachments);
 	mutex_init(&buffer->lock);
 	mutex_lock(&dev->buffer_lock);
@@ -135,7 +134,6 @@ void ion_buffer_destroy(struct ion_buffer *buffer)
 	if (WARN_ON(buffer->kmap_cnt > 0))
 		buffer->heap->ops->unmap_kernel(buffer->heap, buffer);
 	buffer->heap->ops->free(buffer);
-	vfree(buffer->pages);
 	kfree(buffer);
 }
 
@@ -163,7 +161,7 @@ static void *ion_buffer_kmap_get(struct ion_buffer *buffer)
 		return buffer->vaddr;
 	}
 	vaddr = buffer->heap->ops->map_kernel(buffer->heap, buffer);
-	if (WARN_ONCE(vaddr == NULL,
+	if (WARN_ONCE(!vaddr,
 		      "heap->ops->map_kernel should return ERR_PTR on error"))
 		return ERR_PTR(-EINVAL);
 	if (IS_ERR(vaddr))
@@ -221,7 +219,7 @@ struct ion_dma_buf_attachment {
 };
 
 static int ion_dma_buf_attach(struct dma_buf *dmabuf, struct device *dev,
-				struct dma_buf_attachment *attachment)
+			      struct dma_buf_attachment *attachment)
 {
 	struct ion_dma_buf_attachment *a;
 	struct sg_table *table;
@@ -264,26 +262,19 @@ static void ion_dma_buf_detatch(struct dma_buf *dmabuf,
 	kfree(a);
 }
 
-
 static struct sg_table *ion_map_dma_buf(struct dma_buf_attachment *attachment,
 					enum dma_data_direction direction)
 {
 	struct ion_dma_buf_attachment *a = attachment->priv;
 	struct sg_table *table;
-	int ret;
 
 	table = a->table;
 
 	if (!dma_map_sg(attachment->dev, table->sgl, table->nents,
-			direction)){
-		ret = -ENOMEM;
-		goto err;
-	}
-	return table;
+			direction))
+		return ERR_PTR(-ENOMEM);
 
-err:
-	free_duped_table(table);
-	return ERR_PTR(ret);
+	return table;
 }
 
 static void ion_unmap_dma_buf(struct dma_buf_attachment *attachment,
@@ -354,11 +345,10 @@ static int ion_dma_buf_begin_cpu_access(struct dma_buf *dmabuf,
 		mutex_unlock(&buffer->lock);
 	}
 
-
 	mutex_lock(&buffer->lock);
 	list_for_each_entry(a, &buffer->attachments, list) {
 		dma_sync_sg_for_cpu(a->dev, a->table->sgl, a->table->nents,
-					DMA_BIDIRECTIONAL);
+				    DMA_BIDIRECTIONAL);
 	}
 	mutex_unlock(&buffer->lock);
 
@@ -380,7 +370,7 @@ static int ion_dma_buf_end_cpu_access(struct dma_buf *dmabuf,
 	mutex_lock(&buffer->lock);
 	list_for_each_entry(a, &buffer->attachments, list) {
 		dma_sync_sg_for_device(a->dev, a->table->sgl, a->table->nents,
-					DMA_BIDIRECTIONAL);
+				       DMA_BIDIRECTIONAL);
 	}
 	mutex_unlock(&buffer->lock);
 
@@ -435,7 +425,7 @@ int ion_alloc(size_t len, unsigned int heap_id_mask, unsigned int flags)
 	}
 	up_read(&dev->lock);
 
-	if (buffer == NULL)
+	if (!buffer)
 		return -ENODEV;
 
 	if (IS_ERR(buffer))
@@ -596,7 +586,7 @@ void ion_device_add_heap(struct ion_heap *heap)
 }
 EXPORT_SYMBOL(ion_device_add_heap);
 
-int ion_device_create(void)
+static int ion_device_create(void)
 {
 	struct ion_device *idev;
 	int ret;
diff --git a/drivers/staging/android/ion/ion.h b/drivers/staging/android/ion/ion.h
index ace8416bd509..fa9ed81ab972 100644
--- a/drivers/staging/android/ion/ion.h
+++ b/drivers/staging/android/ion/ion.h
@@ -68,14 +68,6 @@ struct ion_platform_heap {
  * @kmap_cnt:		number of times the buffer is mapped to the kernel
  * @vaddr:		the kernel mapping if kmap_cnt is not zero
  * @sg_table:		the sg table for the buffer if dmap_cnt is not zero
- * @pages:		flat array of pages in the buffer -- used by fault
- *			handler and only valid for buffers that are faulted in
- * @vmas:		list of vma's mapping this buffer
- * @handle_count:	count of handles referencing this buffer
- * @task_comm:		taskcomm of last client to reference this buffer in a
- *			handle, used for debugging
- * @pid:		pid of last client to reference this buffer in a
- *			handle, used for debugging
  */
 struct ion_buffer {
 	union {
@@ -92,13 +84,7 @@ struct ion_buffer {
 	int kmap_cnt;
 	void *vaddr;
 	struct sg_table *sg_table;
-	struct page **pages;
-	struct list_head vmas;
 	struct list_head attachments;
-	/* used to track orphaned buffers */
-	int handle_count;
-	char task_comm[TASK_COMM_LEN];
-	pid_t pid;
 };
 void ion_buffer_destroy(struct ion_buffer *buffer);
 
diff --git a/drivers/staging/android/ion/ion_carveout_heap.c b/drivers/staging/android/ion/ion_carveout_heap.c
index 5fdc1f328f61..fee7650d6fbb 100644
--- a/drivers/staging/android/ion/ion_carveout_heap.c
+++ b/drivers/staging/android/ion/ion_carveout_heap.c
@@ -33,7 +33,7 @@ struct ion_carveout_heap {
 };
 
 static phys_addr_t ion_carveout_allocate(struct ion_heap *heap,
-					     unsigned long size)
+					 unsigned long size)
 {
 	struct ion_carveout_heap *carveout_heap =
 		container_of(heap, struct ion_carveout_heap, heap);
diff --git a/drivers/staging/android/ion/ion_system_heap.c b/drivers/staging/android/ion/ion_system_heap.c
index c50f2d9fc58c..5964bf21fd80 100644
--- a/drivers/staging/android/ion/ion_system_heap.c
+++ b/drivers/staging/android/ion/ion_system_heap.c
@@ -153,7 +153,7 @@ static int ion_system_heap_allocate(struct ion_heap *heap,
 		max_order = compound_order(page);
 		i++;
 	}
-	table = kmalloc(sizeof(struct sg_table), GFP_KERNEL);
+	table = kmalloc(sizeof(*table), GFP_KERNEL);
 	if (!table)
 		goto free_pages;
 
@@ -383,7 +383,7 @@ static int ion_system_contig_heap_allocate(struct ion_heap *heap,
 	for (i = len >> PAGE_SHIFT; i < (1 << order); i++)
 		__free_page(page + i);
 
-	table = kmalloc(sizeof(struct sg_table), GFP_KERNEL);
+	table = kmalloc(sizeof(*table), GFP_KERNEL);
 	if (!table) {
 		ret = -ENOMEM;
 		goto free_pages;
@@ -433,7 +433,7 @@ static struct ion_heap *__ion_system_contig_heap_create(void)
 {
 	struct ion_heap *heap;
 
-	heap = kzalloc(sizeof(struct ion_heap), GFP_KERNEL);
+	heap = kzalloc(sizeof(*heap), GFP_KERNEL);
 	if (!heap)
 		return ERR_PTR(-ENOMEM);
 	heap->ops = &kmalloc_ops;
diff --git a/drivers/staging/android/uapi/ion.h b/drivers/staging/android/uapi/ion.h
index b76db1b2e197..9e21451149d0 100644
--- a/drivers/staging/android/uapi/ion.h
+++ b/drivers/staging/android/uapi/ion.h
@@ -57,12 +57,6 @@ enum ion_heap_type {
  */
 #define ION_FLAG_CACHED 1
 
-/*
- * mappings of this buffer will created at mmap time, if this is set
- * caches must be managed manually
- */
-#define ION_FLAG_CACHED_NEEDS_SYNC 2
-
 /**
  * DOC: Ion Userspace API
  *
@@ -131,24 +125,6 @@ struct ion_heap_query {
 				      struct ion_allocation_data)
 
 /**
- * DOC: ION_IOC_FREE - free memory
- *
- * Takes an ion_handle_data struct and frees the handle.
- */
-#define ION_IOC_FREE		_IOWR(ION_IOC_MAGIC, 1, struct ion_handle_data)
-
-/**
- * DOC: ION_IOC_SHARE - creates a file descriptor to use to share an allocation
- *
- * Takes an ion_fd_data struct with the handle field populated with a valid
- * opaque handle.  Returns the struct with the fd field set to a file
- * descriptor open in the current address space.  This file descriptor
- * can then be passed to another process.  The corresponding opaque handle can
- * be retrieved via ION_IOC_IMPORT.
- */
-#define ION_IOC_SHARE		_IOWR(ION_IOC_MAGIC, 4, struct ion_fd_data)
-
-/**
  * DOC: ION_IOC_HEAP_QUERY - information about available heaps
  *
  * Takes an ion_heap_query structure and populates information about
diff --git a/drivers/staging/ccree/Kconfig b/drivers/staging/ccree/Kconfig
index 4be87f503e3b..36a87c686a2a 100644
--- a/drivers/staging/ccree/Kconfig
+++ b/drivers/staging/ccree/Kconfig
@@ -18,7 +18,7 @@ config CRYPTO_DEV_CCREE
 	select CRYPTO_CTR
 	select CRYPTO_XTS
 	help
-	  Say 'Y' to enable a driver for the Arm TrustZone CryptoCell 
+	  Say 'Y' to enable a driver for the Arm TrustZone CryptoCell
 	  C7xx. Currently only the CryptoCell 712 REE is supported.
 	  Choose this if you wish to use hardware acceleration of
 	  cryptographic operations on the system REE.
@@ -32,12 +32,3 @@ config CCREE_FIPS_SUPPORT
 	  Say 'Y' to enable support for FIPS compliant mode by the
 	  CCREE driver.
 	  If unsure say N.
-
-config CCREE_DISABLE_COHERENT_DMA_OPS
-	bool "Disable Coherent DMA operations for the CCREE driver"
-	depends on CRYPTO_DEV_CCREE
-	default n
-	help
-	  Say 'Y' to disable the use of coherent DMA operations by the
-	  CCREE driver for debugging purposes.  
-	  If unsure say N.
diff --git a/drivers/staging/ccree/Makefile b/drivers/staging/ccree/Makefile
index 44f3e3e33989..318c2b39acf6 100644
--- a/drivers/staging/ccree/Makefile
+++ b/drivers/staging/ccree/Makefile
@@ -1,3 +1,3 @@
 obj-$(CONFIG_CRYPTO_DEV_CCREE) := ccree.o
-ccree-y := ssi_driver.o ssi_sysfs.o ssi_buffer_mgr.o ssi_request_mgr.o ssi_cipher.o ssi_hash.o ssi_aead.o ssi_ivgen.o ssi_sram_mgr.o ssi_pm.o ssi_pm_ext.o
+ccree-y := ssi_driver.o ssi_sysfs.o ssi_buffer_mgr.o ssi_request_mgr.o ssi_cipher.o ssi_hash.o ssi_aead.o ssi_ivgen.o ssi_sram_mgr.o ssi_pm.o
 ccree-$(CCREE_FIPS_SUPPORT) += ssi_fips.o ssi_fips_ll.o ssi_fips_ext.o ssi_fips_local.o
diff --git a/drivers/staging/ccree/cc_bitops.h b/drivers/staging/ccree/cc_bitops.h
deleted file mode 100644
index 3a39565ef73b..000000000000
--- a/drivers/staging/ccree/cc_bitops.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/*
- * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
- * This program is free software; you can 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/>.
- */
-
-/*!
- * \file cc_bitops.h
- * Bit fields operations macros.
- */
-#ifndef _CC_BITOPS_H_
-#define _CC_BITOPS_H_
-
-#define BITMASK(mask_size) (((mask_size) < 32) ?	\
-	((1UL << (mask_size)) - 1) : 0xFFFFFFFFUL)
-#define BITMASK_AT(mask_size, mask_offset) (BITMASK(mask_size) << (mask_offset))
-
-#define BITFIELD_GET(word, bit_offset, bit_size) \
-	(((word) >> (bit_offset)) & BITMASK(bit_size))
-#define BITFIELD_SET(word, bit_offset, bit_size, new_val)   do {    \
-	word = ((word) & ~BITMASK_AT(bit_size, bit_offset)) |	    \
-		(((new_val) & BITMASK(bit_size)) << (bit_offset));  \
-} while (0)
-
-/* Is val aligned to "align" ("align" must be power of 2) */
-#ifndef IS_ALIGNED
-#define IS_ALIGNED(val, align)		\
-	(((uintptr_t)(val) & ((align) - 1)) == 0)
-#endif
-
-#define SWAP_ENDIAN(word)		\
-	(((word) >> 24) | (((word) & 0x00FF0000) >> 8) | \
-	(((word) & 0x0000FF00) << 8) | (((word) & 0x000000FF) << 24))
-
-#ifdef BIG__ENDIAN
-#define SWAP_TO_LE(word) SWAP_ENDIAN(word)
-#define SWAP_TO_BE(word) word
-#else
-#define SWAP_TO_LE(word) word
-#define SWAP_TO_BE(word) SWAP_ENDIAN(word)
-#endif
-
-
-
-/* Is val a multiple of "mult" ("mult" must be power of 2) */
-#define IS_MULT(val, mult)              \
-	(((val) & ((mult) - 1)) == 0)
-
-#define IS_NULL_ADDR(adr)		\
-	(!(adr))
-
-#endif /*_CC_BITOPS_H_*/
diff --git a/drivers/staging/ccree/cc_crypto_ctx.h b/drivers/staging/ccree/cc_crypto_ctx.h
index 9e10b2670313..591f6fdadc59 100644
--- a/drivers/staging/ccree/cc_crypto_ctx.h
+++ b/drivers/staging/ccree/cc_crypto_ctx.h
@@ -1,35 +1,23 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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 _CC_CRYPTO_CTX_H_
 #define _CC_CRYPTO_CTX_H_
 
-#ifdef __KERNEL__
 #include <linux/types.h>
-#define INT32_MAX 0x7FFFFFFFL
-#else
-#include <stdint.h>
-#endif
-
-
-#ifndef max
-#define max(a, b) ((a) > (b) ? (a) : (b))
-#define min(a, b) ((a) < (b) ? (a) : (b))
-#endif
 
 /* context size */
 #ifndef CC_CTX_SIZE_LOG2
@@ -39,7 +27,7 @@
 #define CC_CTX_SIZE_LOG2 7
 #endif
 #endif
-#define CC_CTX_SIZE (1<<CC_CTX_SIZE_LOG2)
+#define CC_CTX_SIZE BIT(CC_CTX_SIZE_LOG2)
 #define CC_DRV_CTX_SIZE_WORDS (CC_CTX_SIZE >> 2)
 
 #define CC_DRV_DES_IV_SIZE 8
@@ -65,13 +53,13 @@
 #define CC_AES_KEY_SIZE_MAX			CC_AES_256_BIT_KEY_SIZE
 #define CC_AES_KEY_SIZE_WORDS_MAX		(CC_AES_KEY_SIZE_MAX >> 2)
 
-#define CC_MD5_DIGEST_SIZE 	16
-#define CC_SHA1_DIGEST_SIZE 	20
-#define CC_SHA224_DIGEST_SIZE 	28
-#define CC_SHA256_DIGEST_SIZE 	32
+#define CC_MD5_DIGEST_SIZE	16
+#define CC_SHA1_DIGEST_SIZE	20
+#define CC_SHA224_DIGEST_SIZE	28
+#define CC_SHA256_DIGEST_SIZE	32
 #define CC_SHA256_DIGEST_SIZE_IN_WORDS 8
-#define CC_SHA384_DIGEST_SIZE 	48
-#define CC_SHA512_DIGEST_SIZE 	64
+#define CC_SHA384_DIGEST_SIZE	48
+#define CC_SHA512_DIGEST_SIZE	64
 
 #define CC_SHA1_BLOCK_SIZE 64
 #define CC_SHA1_BLOCK_SIZE_IN_WORDS 16
@@ -94,18 +82,17 @@
 
 #define CC_HMAC_BLOCK_SIZE_MAX CC_HASH_BLOCK_SIZE_MAX
 
-#define CC_MULTI2_SYSTEM_KEY_SIZE 		32
-#define CC_MULTI2_DATA_KEY_SIZE 		8
-#define CC_MULTI2_SYSTEM_N_DATA_KEY_SIZE 	(CC_MULTI2_SYSTEM_KEY_SIZE + CC_MULTI2_DATA_KEY_SIZE)
+#define CC_MULTI2_SYSTEM_KEY_SIZE		32
+#define CC_MULTI2_DATA_KEY_SIZE		8
+#define CC_MULTI2_SYSTEM_N_DATA_KEY_SIZE \
+		(CC_MULTI2_SYSTEM_KEY_SIZE + CC_MULTI2_DATA_KEY_SIZE)
 #define	CC_MULTI2_BLOCK_SIZE					8
 #define	CC_MULTI2_IV_SIZE					8
 #define	CC_MULTI2_MIN_NUM_ROUNDS				8
 #define	CC_MULTI2_MAX_NUM_ROUNDS				128
 
-
 #define CC_DRV_ALG_MAX_BLOCK_SIZE CC_HASH_BLOCK_SIZE_MAX
 
-
 enum drv_engine_type {
 	DRV_ENGINE_NULL = 0,
 	DRV_ENGINE_AES = 1,
@@ -113,7 +100,7 @@ enum drv_engine_type {
 	DRV_ENGINE_HASH = 3,
 	DRV_ENGINE_RC4 = 4,
 	DRV_ENGINE_DOUT = 5,
-	DRV_ENGINE_RESERVE32B = INT32_MAX,
+	DRV_ENGINE_RESERVE32B = S32_MAX,
 };
 
 enum drv_crypto_alg {
@@ -126,7 +113,7 @@ enum drv_crypto_alg {
 	DRV_CRYPTO_ALG_AEAD = 5,
 	DRV_CRYPTO_ALG_BYPASS = 6,
 	DRV_CRYPTO_ALG_NUM = 7,
-	DRV_CRYPTO_ALG_RESERVE32B = INT32_MAX
+	DRV_CRYPTO_ALG_RESERVE32B = S32_MAX
 };
 
 enum drv_crypto_direction {
@@ -134,7 +121,7 @@ enum drv_crypto_direction {
 	DRV_CRYPTO_DIRECTION_ENCRYPT = 0,
 	DRV_CRYPTO_DIRECTION_DECRYPT = 1,
 	DRV_CRYPTO_DIRECTION_DECRYPT_ENCRYPT = 3,
-	DRV_CRYPTO_DIRECTION_RESERVE32B = INT32_MAX
+	DRV_CRYPTO_DIRECTION_RESERVE32B = S32_MAX
 };
 
 enum drv_cipher_mode {
@@ -152,7 +139,7 @@ enum drv_cipher_mode {
 	DRV_CIPHER_GCTR = 12,
 	DRV_CIPHER_ESSIV = 13,
 	DRV_CIPHER_BITLOCKER = 14,
-	DRV_CIPHER_RESERVE32B = INT32_MAX
+	DRV_CIPHER_RESERVE32B = S32_MAX
 };
 
 enum drv_hash_mode {
@@ -163,11 +150,11 @@ enum drv_hash_mode {
 	DRV_HASH_SHA512 = 3,
 	DRV_HASH_SHA384 = 4,
 	DRV_HASH_MD5 = 5,
-	DRV_HASH_CBC_MAC = 6, 
+	DRV_HASH_CBC_MAC = 6,
 	DRV_HASH_XCBC_MAC = 7,
 	DRV_HASH_CMAC = 8,
 	DRV_HASH_MODE_NUM = 9,
-	DRV_HASH_RESERVE32B = INT32_MAX
+	DRV_HASH_RESERVE32B = S32_MAX
 };
 
 enum drv_hash_hw_mode {
@@ -178,7 +165,7 @@ enum drv_hash_hw_mode {
 	DRV_HASH_HW_SHA512 = 4,
 	DRV_HASH_HW_SHA384 = 12,
 	DRV_HASH_HW_GHASH = 6,
-	DRV_HASH_HW_RESERVE32B = INT32_MAX
+	DRV_HASH_HW_RESERVE32B = S32_MAX
 };
 
 enum drv_multi2_mode {
@@ -186,10 +173,9 @@ enum drv_multi2_mode {
 	DRV_MULTI2_ECB = 0,
 	DRV_MULTI2_CBC = 1,
 	DRV_MULTI2_OFB = 2,
-	DRV_MULTI2_RESERVE32B = INT32_MAX
+	DRV_MULTI2_RESERVE32B = S32_MAX
 };
 
-
 /* drv_crypto_key_type[1:0] is mapped to cipher_do[1:0] */
 /* drv_crypto_key_type[2] is mapped to cipher_config2 */
 enum drv_crypto_key_type {
@@ -201,99 +187,14 @@ enum drv_crypto_key_type {
 	DRV_APPLET_KEY = 4,		/* NA */
 	DRV_PLATFORM_KEY = 5,		/* 0x101 */
 	DRV_CUSTOMER_KEY = 6,		/* 0x110 */
-	DRV_END_OF_KEYS = INT32_MAX,
+	DRV_END_OF_KEYS = S32_MAX,
 };
 
 enum drv_crypto_padding_type {
 	DRV_PADDING_NONE = 0,
 	DRV_PADDING_PKCS7 = 1,
-	DRV_PADDING_RESERVE32B = INT32_MAX
+	DRV_PADDING_RESERVE32B = S32_MAX
 };
 
-/*******************************************************************/
-/***************** DESCRIPTOR BASED CONTEXTS ***********************/
-/*******************************************************************/
-
- /* Generic context ("super-class") */
-struct drv_ctx_generic {
-	enum drv_crypto_alg alg;
-} __attribute__((__may_alias__));
-
-
-struct drv_ctx_hash {
-	enum drv_crypto_alg alg; /* DRV_CRYPTO_ALG_HASH */
-	enum drv_hash_mode mode;
-	uint8_t digest[CC_DIGEST_SIZE_MAX];
-	/* reserve to end of allocated context size */
-	uint8_t reserved[CC_CTX_SIZE - 2 * sizeof(uint32_t) -
-			CC_DIGEST_SIZE_MAX];
-};
-
-/* !!!! drv_ctx_hmac should have the same structure as drv_ctx_hash except
-   k0, k0_size fields */
-struct drv_ctx_hmac {
-	enum drv_crypto_alg alg; /* DRV_CRYPTO_ALG_HMAC */
-	enum drv_hash_mode mode;
-	uint8_t digest[CC_DIGEST_SIZE_MAX];
-	uint32_t k0[CC_HMAC_BLOCK_SIZE_MAX/sizeof(uint32_t)];
-	uint32_t k0_size;
-	/* reserve to end of allocated context size */
-	uint8_t reserved[CC_CTX_SIZE - 3 * sizeof(uint32_t) -
-			CC_DIGEST_SIZE_MAX - CC_HMAC_BLOCK_SIZE_MAX];
-};
-
-struct drv_ctx_cipher {
-	enum drv_crypto_alg alg; /* DRV_CRYPTO_ALG_AES */
-	enum drv_cipher_mode mode;
-	enum drv_crypto_direction direction;
-	enum drv_crypto_key_type crypto_key_type;
-	enum drv_crypto_padding_type padding_type;
-	uint32_t key_size; /* numeric value in bytes   */
-	uint32_t data_unit_size; /* required for XTS */
-	/* block_state is the AES engine block state.
-	*  It is used by the host to pass IV or counter at initialization.
-	*  It is used by SeP for intermediate block chaining state and for
-	*  returning MAC algorithms results.           */
-	uint8_t block_state[CC_AES_BLOCK_SIZE];
-	uint8_t key[CC_AES_KEY_SIZE_MAX];
-	uint8_t xex_key[CC_AES_KEY_SIZE_MAX];
-	/* reserve to end of allocated context size */
-	uint32_t reserved[CC_DRV_CTX_SIZE_WORDS - 7 -
-		CC_AES_BLOCK_SIZE/sizeof(uint32_t) - 2 *
-		(CC_AES_KEY_SIZE_MAX/sizeof(uint32_t))];
-};
-
-/* authentication and encryption with associated data class */
-struct drv_ctx_aead {
-	enum drv_crypto_alg alg; /* DRV_CRYPTO_ALG_AES */
-	enum drv_cipher_mode mode;
-	enum drv_crypto_direction direction;
-	uint32_t key_size; /* numeric value in bytes   */
-	uint32_t nonce_size; /* nonce size (octets) */
-	uint32_t header_size; /* finit additional data size (octets) */
-	uint32_t text_size; /* finit text data size (octets) */
-	uint32_t tag_size; /* mac size, element of {4, 6, 8, 10, 12, 14, 16} */
-	/* block_state1/2 is the AES engine block state */
-	uint8_t block_state[CC_AES_BLOCK_SIZE];
-	uint8_t mac_state[CC_AES_BLOCK_SIZE]; /* MAC result */
-	uint8_t nonce[CC_AES_BLOCK_SIZE]; /* nonce buffer */
-	uint8_t key[CC_AES_KEY_SIZE_MAX];
-	/* reserve to end of allocated context size */
-	uint32_t reserved[CC_DRV_CTX_SIZE_WORDS - 8 -
-		3 * (CC_AES_BLOCK_SIZE/sizeof(uint32_t)) -
-		CC_AES_KEY_SIZE_MAX/sizeof(uint32_t)];
-};
-
-/*******************************************************************/
-/***************** MESSAGE BASED CONTEXTS **************************/
-/*******************************************************************/
-
-
-/* Get the address of a @member within a given @ctx address
-   @ctx: The context address
-   @type: Type of context structure
-   @member: Associated context field */
-#define GET_CTX_FIELD_ADDR(ctx, type, member) (ctx + offsetof(type, member))
-
 #endif /* _CC_CRYPTO_CTX_H_ */
 
diff --git a/drivers/staging/ccree/cc_hal.h b/drivers/staging/ccree/cc_hal.h
index 75a0ce3e80d6..eecc866dfc74 100644
--- a/drivers/staging/ccree/cc_hal.h
+++ b/drivers/staging/ccree/cc_hal.h
@@ -1,20 +1,22 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
-/* pseudo cc_hal.h for cc7x_perf_test_driver (to be able to include code from CC drivers) */
+/* pseudo cc_hal.h for cc7x_perf_test_driver (to be able to include code from
+ * CC drivers).
+ */
 
 #ifndef __CC_HAL_H__
 #define __CC_HAL_H__
@@ -24,7 +26,8 @@
 #define READ_REGISTER(_addr) ioread32((_addr))
 #define WRITE_REGISTER(_addr, _data)  iowrite32((_data), (_addr))
 
-#define CC_HAL_WRITE_REGISTER(offset, val) WRITE_REGISTER(cc_base + offset, val)
-#define CC_HAL_READ_REGISTER(offset) READ_REGISTER(cc_base + offset)
+#define CC_HAL_WRITE_REGISTER(offset, val) \
+	WRITE_REGISTER(cc_base + (offset), val)
+#define CC_HAL_READ_REGISTER(offset) READ_REGISTER(cc_base + (offset))
 
 #endif
diff --git a/drivers/staging/ccree/cc_hw_queue_defs.h b/drivers/staging/ccree/cc_hw_queue_defs.h
index fbaf1b6fcd90..e6b8cea3f88d 100644
--- a/drivers/staging/ccree/cc_hw_queue_defs.h
+++ b/drivers/staging/ccree/cc_hw_queue_defs.h
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -17,63 +17,95 @@
 #ifndef __CC_HW_QUEUE_DEFS_H__
 #define __CC_HW_QUEUE_DEFS_H__
 
-#include "cc_pal_log.h"
-#include "cc_regs.h"
-#include "dx_crys_kernel.h"
-
-#ifdef __KERNEL__
 #include <linux/types.h>
-#define UINT32_MAX 0xFFFFFFFFL
-#define INT32_MAX  0x7FFFFFFFL
-#define UINT16_MAX 0xFFFFL
-#else
-#include <stdint.h>
-#endif
-
-/******************************************************************************
-*                        	DEFINITIONS
-******************************************************************************/
 
+#include "dx_crys_kernel.h"
+#include <linux/bitfield.h>
 
-/* Dma AXI Secure bit */
-#define	AXI_SECURE	0
-#define AXI_NOT_SECURE	1
+/******************************************************************************
+ *				DEFINITIONS
+ ******************************************************************************/
 
 #define HW_DESC_SIZE_WORDS		6
 #define HW_QUEUE_SLOTS_MAX              15 /* Max. available slots in HW queue */
 
-#define _HW_DESC_MONITOR_KICK 0x7FFFC00
+#define CC_REG_NAME(word, name) DX_DSCRPTR_QUEUE_WORD ## word ## _ ## name
+
+#define CC_REG_LOW(word, name)  \
+	(DX_DSCRPTR_QUEUE_WORD ## word ## _ ## name ## _BIT_SHIFT)
+
+#define CC_REG_HIGH(word, name) \
+	(CC_REG_LOW(word, name) + \
+	 DX_DSCRPTR_QUEUE_WORD ## word ## _ ## name ## _BIT_SIZE - 1)
+
+#define CC_GENMASK(word, name) \
+	GENMASK(CC_REG_HIGH(word, name), CC_REG_LOW(word, name))
+
+#define WORD0_VALUE		CC_GENMASK(0, VALUE)
+#define WORD1_DIN_CONST_VALUE	CC_GENMASK(1, DIN_CONST_VALUE)
+#define WORD1_DIN_DMA_MODE	CC_GENMASK(1, DIN_DMA_MODE)
+#define WORD1_DIN_SIZE		CC_GENMASK(1, DIN_SIZE)
+#define WORD1_NOT_LAST		CC_GENMASK(1, NOT_LAST)
+#define WORD1_NS_BIT		CC_GENMASK(1, NS_BIT)
+#define WORD2_VALUE		CC_GENMASK(2, VALUE)
+#define WORD3_DOUT_DMA_MODE	CC_GENMASK(3, DOUT_DMA_MODE)
+#define WORD3_DOUT_LAST_IND	CC_GENMASK(3, DOUT_LAST_IND)
+#define WORD3_DOUT_SIZE		CC_GENMASK(3, DOUT_SIZE)
+#define WORD3_HASH_XOR_BIT	CC_GENMASK(3, HASH_XOR_BIT)
+#define WORD3_NS_BIT		CC_GENMASK(3, NS_BIT)
+#define WORD3_QUEUE_LAST_IND	CC_GENMASK(3, QUEUE_LAST_IND)
+#define WORD4_ACK_NEEDED	CC_GENMASK(4, ACK_NEEDED)
+#define WORD4_AES_SEL_N_HASH	CC_GENMASK(4, AES_SEL_N_HASH)
+#define WORD4_BYTES_SWAP	CC_GENMASK(4, BYTES_SWAP)
+#define WORD4_CIPHER_CONF0	CC_GENMASK(4, CIPHER_CONF0)
+#define WORD4_CIPHER_CONF1	CC_GENMASK(4, CIPHER_CONF1)
+#define WORD4_CIPHER_CONF2	CC_GENMASK(4, CIPHER_CONF2)
+#define WORD4_CIPHER_DO		CC_GENMASK(4, CIPHER_DO)
+#define WORD4_CIPHER_MODE	CC_GENMASK(4, CIPHER_MODE)
+#define WORD4_CMAC_SIZE0	CC_GENMASK(4, CMAC_SIZE0)
+#define WORD4_DATA_FLOW_MODE	CC_GENMASK(4, DATA_FLOW_MODE)
+#define WORD4_KEY_SIZE		CC_GENMASK(4, KEY_SIZE)
+#define WORD4_SETUP_OPERATION	CC_GENMASK(4, SETUP_OPERATION)
+#define WORD5_DIN_ADDR_HIGH	CC_GENMASK(5, DIN_ADDR_HIGH)
+#define WORD5_DOUT_ADDR_HIGH	CC_GENMASK(5, DOUT_ADDR_HIGH)
 
 /******************************************************************************
-*				TYPE DEFINITIONS
-******************************************************************************/
+ *				TYPE DEFINITIONS
+ ******************************************************************************/
+
+struct cc_hw_desc {
+	union {
+		u32 word[HW_DESC_SIZE_WORDS];
+		u16 hword[HW_DESC_SIZE_WORDS * 2];
+	};
+};
 
-typedef struct HwDesc {
-	uint32_t word[HW_DESC_SIZE_WORDS];
-} HwDesc_s;
+enum cc_axi_sec {
+	AXI_SECURE = 0,
+	AXI_NOT_SECURE = 1
+};
 
-typedef enum DescDirection {
+enum cc_desc_direction {
 	DESC_DIRECTION_ILLEGAL = -1,
 	DESC_DIRECTION_ENCRYPT_ENCRYPT = 0,
 	DESC_DIRECTION_DECRYPT_DECRYPT = 1,
 	DESC_DIRECTION_DECRYPT_ENCRYPT = 3,
-	DESC_DIRECTION_END = INT32_MAX,
-}DescDirection_t;
+	DESC_DIRECTION_END = S32_MAX,
+};
 
-typedef enum DmaMode {
+enum cc_dma_mode {
 	DMA_MODE_NULL		= -1,
-	NO_DMA 			= 0,
+	NO_DMA			= 0,
 	DMA_SRAM		= 1,
 	DMA_DLLI		= 2,
 	DMA_MLLI		= 3,
-	DmaMode_OPTIONTS,
-	DmaMode_END 		= INT32_MAX,
-}DmaMode_t;
+	DMA_MODE_END		= S32_MAX,
+};
 
-typedef enum FlowMode {
+enum cc_flow_mode {
 	FLOW_MODE_NULL		= -1,
 	/* data flows */
- 	BYPASS			= 0,
+	BYPASS			= 0,
 	DIN_AES_DOUT		= 1,
 	AES_to_HASH		= 2,
 	AES_and_HASH		= 3,
@@ -93,11 +125,11 @@ typedef enum FlowMode {
 	DIN_AES_AESMAC		= 17,
 	HASH_to_DOUT		= 18,
 	/* setup flows */
- 	S_DIN_to_AES 		= 32,
+	S_DIN_to_AES		= 32,
 	S_DIN_to_AES2		= 33,
 	S_DIN_to_DES		= 34,
 	S_DIN_to_RC4		= 35,
- 	S_DIN_to_MULTI2		= 36,
+	S_DIN_to_MULTI2		= 36,
 	S_DIN_to_HASH		= 37,
 	S_AES_to_DOUT		= 38,
 	S_AES2_to_DOUT		= 39,
@@ -105,47 +137,43 @@ typedef enum FlowMode {
 	S_DES_to_DOUT		= 42,
 	S_HASH_to_DOUT		= 43,
 	SET_FLOW_ID		= 44,
-	FlowMode_OPTIONTS,
-	FlowMode_END = INT32_MAX,
-}FlowMode_t;
+	FLOW_MODE_END = S32_MAX,
+};
 
-typedef enum TunnelOp {
+enum cc_tunnel_op {
 	TUNNEL_OP_INVALID = -1,
 	TUNNEL_OFF = 0,
 	TUNNEL_ON = 1,
-	TunnelOp_OPTIONS,
-	TunnelOp_END = INT32_MAX,
-} TunnelOp_t;
+	TUNNEL_OP_END = S32_MAX,
+};
 
-typedef enum SetupOp {
+enum cc_setup_op {
 	SETUP_LOAD_NOP		= 0,
 	SETUP_LOAD_STATE0	= 1,
 	SETUP_LOAD_STATE1	= 2,
 	SETUP_LOAD_STATE2	= 3,
 	SETUP_LOAD_KEY0		= 4,
 	SETUP_LOAD_XEX_KEY	= 5,
-	SETUP_WRITE_STATE0	= 8, 
+	SETUP_WRITE_STATE0	= 8,
 	SETUP_WRITE_STATE1	= 9,
 	SETUP_WRITE_STATE2	= 10,
 	SETUP_WRITE_STATE3	= 11,
-	setupOp_OPTIONTS,
-	setupOp_END = INT32_MAX,	
-}SetupOp_t;
+	SETUP_OP_END = S32_MAX,
+};
 
-enum AesMacSelector {
+enum cc_aes_mac_selector {
 	AES_SK = 1,
 	AES_CMAC_INIT = 2,
 	AES_CMAC_SIZE0 = 3,
-	AesMacEnd = INT32_MAX,
+	AES_MAC_END = S32_MAX,
 };
 
-#define HW_KEY_MASK_CIPHER_DO 	  0x3
+#define HW_KEY_MASK_CIPHER_DO	  0x3
 #define HW_KEY_SHIFT_CIPHER_CFG2  2
 
-
 /* HwCryptoKey[1:0] is mapped to cipher_do[1:0] */
 /* HwCryptoKey[2:3] is mapped to cipher_config2[1:0] */
-typedef enum HwCryptoKey {
+enum cc_hw_crypto_key {
 	USER_KEY = 0,			/* 0x0000 */
 	ROOT_KEY = 1,			/* 0x0001 */
 	PROVISIONING_KEY = 2,		/* 0x0010 */ /* ==KCP */
@@ -157,447 +185,409 @@ typedef enum HwCryptoKey {
 	KFDE1_KEY = 9,			/* 0x1001 */
 	KFDE2_KEY = 10,			/* 0x1010 */
 	KFDE3_KEY = 11,			/* 0x1011 */
-	END_OF_KEYS = INT32_MAX,
-}HwCryptoKey_t;
+	END_OF_KEYS = S32_MAX,
+};
 
-typedef enum HwAesKeySize {
+enum cc_hw_aes_key_size {
 	AES_128_KEY = 0,
 	AES_192_KEY = 1,
 	AES_256_KEY = 2,
-	END_OF_AES_KEYS = INT32_MAX,
-}HwAesKeySize_t;
+	END_OF_AES_KEYS = S32_MAX,
+};
 
-typedef enum HwDesKeySize {
+enum cc_hw_des_key_size {
 	DES_ONE_KEY = 0,
 	DES_TWO_KEYS = 1,
 	DES_THREE_KEYS = 2,
-	END_OF_DES_KEYS = INT32_MAX,
-}HwDesKeySize_t;
+	END_OF_DES_KEYS = S32_MAX,
+};
 
 /*****************************/
 /* Descriptor packing macros */
 /*****************************/
 
-#define GET_HW_Q_DESC_WORD_IDX(descWordIdx) (CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD ## descWordIdx) )
-
-#define HW_DESC_INIT(pDesc)  do { \
-	(pDesc)->word[0] = 0;     \
-	(pDesc)->word[1] = 0;     \
-	(pDesc)->word[2] = 0;     \
-	(pDesc)->word[3] = 0;     \
-	(pDesc)->word[4] = 0;     \
-	(pDesc)->word[5] = 0;     \
-} while (0)
-
-/* HW descriptor debug functions */
-int createDetailedDump(HwDesc_s *pDesc);
-void descriptor_log(HwDesc_s *desc);
-
-#if defined(HW_DESCRIPTOR_LOG) || defined(HW_DESC_DUMP_HOST_BUF)
-#define LOG_HW_DESC(pDesc) descriptor_log(pDesc)
-#else
-#define LOG_HW_DESC(pDesc) 
-#endif
-
-#if (CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_TRACE) || defined(OEMFW_LOG)
-
-#ifdef UART_PRINTF
-#define CREATE_DETAILED_DUMP(pDesc) createDetailedDump(pDesc)
-#else
-#define CREATE_DETAILED_DUMP(pDesc) 
-#endif 
-
-#define HW_DESC_DUMP(pDesc) do {            			\
-	CC_PAL_LOG_TRACE("\n---------------------------------------------------\n");	\
-	CREATE_DETAILED_DUMP(pDesc); 				\
-	CC_PAL_LOG_TRACE("0x%08X, ", (unsigned int)(pDesc)->word[0]);  	\
-	CC_PAL_LOG_TRACE("0x%08X, ", (unsigned int)(pDesc)->word[1]);  	\
-	CC_PAL_LOG_TRACE("0x%08X, ", (unsigned int)(pDesc)->word[2]);  	\
-	CC_PAL_LOG_TRACE("0x%08X, ", (unsigned int)(pDesc)->word[3]);  	\
-	CC_PAL_LOG_TRACE("0x%08X, ", (unsigned int)(pDesc)->word[4]);  	\
-	CC_PAL_LOG_TRACE("0x%08X\n", (unsigned int)(pDesc)->word[5]);  	\
-	CC_PAL_LOG_TRACE("---------------------------------------------------\n\n");    \
-} while (0)
-
-#else
-#define HW_DESC_DUMP(pDesc) do {} while (0)
-#endif
-
-
-/*!
- * This macro indicates the end of current HW descriptors flow and release the HW engines.
- * 
- * \param pDesc pointer HW descriptor struct
- */
-#define HW_DESC_SET_QUEUE_LAST_IND(pDesc) 								\
-	do {												\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, QUEUE_LAST_IND, (pDesc)->word[3], 1);	\
-	} while (0)
-
-/*!
- * This macro signs the end of HW descriptors flow by asking for completion ack, and release the HW engines
- * 
- * \param pDesc pointer HW descriptor struct 
- */
-#define HW_DESC_SET_ACK_LAST(pDesc) 									\
-	do {												\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, QUEUE_LAST_IND, (pDesc)->word[3], 1);	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, ACK_NEEDED, (pDesc)->word[4], 1);	\
-	} while (0)
-
-
-#define MSB64(_addr) (sizeof(_addr) == 4 ? 0 : ((_addr) >> 32)&UINT16_MAX)
-
-/*!
- * This macro sets the DIN field of a HW descriptors
- * 
- * \param pDesc pointer HW descriptor struct 
- * \param dmaMode The DMA mode: NO_DMA, SRAM, DLLI, MLLI, CONSTANT
- * \param dinAdr DIN address
- * \param dinSize Data size in bytes 
- * \param axiNs AXI secure bit
+/*
+ * Init a HW descriptor struct
+ * @pdesc: pointer HW descriptor struct
  */
-#define HW_DESC_SET_DIN_TYPE(pDesc, dmaMode, dinAdr, dinSize, axiNs)								\
-	do {		                                                                                        		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0, VALUE, (pDesc)->word[0], (dinAdr)&UINT32_MAX );			\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD5, DIN_ADDR_HIGH, (pDesc)->word[5], MSB64(dinAdr) );		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_DMA_MODE, (pDesc)->word[1], (dmaMode));			\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_SIZE, (pDesc)->word[1], (dinSize));				\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, NS_BIT, (pDesc)->word[1], (axiNs));				\
-	} while (0)
-
-
-/*!
- * This macro sets the DIN field of a HW descriptors to NO DMA mode. Used for NOP descriptor, register patches and 
- * other special modes 
- * 
- * \param pDesc pointer HW descriptor struct
- * \param dinAdr DIN address
- * \param dinSize Data size in bytes 
+static inline void hw_desc_init(struct cc_hw_desc *pdesc)
+{
+	memset(pdesc, 0, sizeof(struct cc_hw_desc));
+}
+
+/*
+ * Indicates the end of current HW descriptors flow and release the HW engines.
+ *
+ * @pdesc: pointer HW descriptor struct
  */
-#define HW_DESC_SET_DIN_NO_DMA(pDesc, dinAdr, dinSize)									\
-	do {		                                                                                        	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0, VALUE, (pDesc)->word[0], (uint32_t)(dinAdr));		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_SIZE, (pDesc)->word[1], (dinSize));			\
-	} while (0)
-
-/*!
- * This macro sets the DIN field of a HW descriptors to SRAM mode. 
- * Note: No need to check SRAM alignment since host requests do not use SRAM and 
- * adaptor will enforce alignment check. 
- * 
- * \param pDesc pointer HW descriptor struct
- * \param dinAdr DIN address
- * \param dinSize Data size in bytes 
+static inline void set_queue_last_ind(struct cc_hw_desc *pdesc)
+{
+	pdesc->word[3] |= FIELD_PREP(WORD3_QUEUE_LAST_IND, 1);
+}
+
+/*
+ * Set the DIN field of a HW descriptors
+ *
+ * @pdesc: pointer HW descriptor struct
+ * @dma_mode: dmaMode The DMA mode: NO_DMA, SRAM, DLLI, MLLI, CONSTANT
+ * @addr: dinAdr DIN address
+ * @size: Data size in bytes
+ * @axi_sec: AXI secure bit
  */
-#define HW_DESC_SET_DIN_SRAM(pDesc, dinAdr, dinSize)									\
-	do {		                                                                                        	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0, VALUE, (pDesc)->word[0], (uint32_t)(dinAdr));		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_DMA_MODE, (pDesc)->word[1], DMA_SRAM);		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_SIZE, (pDesc)->word[1], (dinSize));			\
-	} while (0)
-
-/*! This macro sets the DIN field of a HW descriptors to CONST mode 
- * 
- * \param pDesc pointer HW descriptor struct
- * \param val DIN const value
- * \param dinSize Data size in bytes 
+static inline void set_din_type(struct cc_hw_desc *pdesc,
+				enum cc_dma_mode dma_mode, dma_addr_t addr,
+				u32 size, enum cc_axi_sec axi_sec)
+{
+	pdesc->word[0] = (u32)addr;
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+	pdesc->word[5] |= FIELD_PREP(WORD5_DIN_ADDR_HIGH, ((u16)(addr >> 32)));
+#endif
+	pdesc->word[1] |= FIELD_PREP(WORD1_DIN_DMA_MODE, dma_mode) |
+				FIELD_PREP(WORD1_DIN_SIZE, size) |
+				FIELD_PREP(WORD1_NS_BIT, axi_sec);
+}
+
+/*
+ * Set the DIN field of a HW descriptors to NO DMA mode.
+ * Used for NOP descriptor, register patches and other special modes.
+ *
+ * @pdesc: pointer HW descriptor struct
+ * @addr: DIN address
+ * @size: Data size in bytes
  */
-#define HW_DESC_SET_DIN_CONST(pDesc, val, dinSize)									\
-	do {		                                                                                        	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0, VALUE, (pDesc)->word[0], (uint32_t)(val));		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_CONST_VALUE, (pDesc)->word[1], 1);			\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_DMA_MODE, (pDesc)->word[1], DMA_SRAM);		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_SIZE, (pDesc)->word[1], (dinSize));			\
-	} while (0)
-
-/*!
- * This macro sets the DIN not last input data indicator
- * 
- * \param pDesc pointer HW descriptor struct
+static inline void set_din_no_dma(struct cc_hw_desc *pdesc, u32 addr, u32 size)
+{
+	pdesc->word[0] = addr;
+	pdesc->word[1] |= FIELD_PREP(WORD1_DIN_SIZE, size);
+}
+
+/*
+ * Set the DIN field of a HW descriptors to SRAM mode.
+ * Note: No need to check SRAM alignment since host requests do not use SRAM and
+ * adaptor will enforce alignment check.
+ *
+ * @pdesc: pointer HW descriptor struct
+ * @addr: DIN address
+ * @size Data size in bytes
  */
-#define HW_DESC_SET_DIN_NOT_LAST_INDICATION(pDesc)									\
-	do {		                                                                                        	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, NOT_LAST, (pDesc)->word[1], 1);				\
-	} while (0)
-
-/*!
- * This macro sets the DOUT field of a HW descriptors 
- * 
- * \param pDesc pointer HW descriptor struct 
- * \param dmaMode The DMA mode: NO_DMA, SRAM, DLLI, MLLI, CONSTANT
- * \param doutAdr DOUT address
- * \param doutSize Data size in bytes 
- * \param axiNs AXI secure bit
+static inline void set_din_sram(struct cc_hw_desc *pdesc, dma_addr_t addr,
+				u32 size)
+{
+	pdesc->word[0] = (u32)addr;
+	pdesc->word[1] |= FIELD_PREP(WORD1_DIN_SIZE, size) |
+				FIELD_PREP(WORD1_DIN_DMA_MODE, DMA_SRAM);
+}
+
+/*
+ * Set the DIN field of a HW descriptors to CONST mode
+ *
+ * @pdesc: pointer HW descriptor struct
+ * @val: DIN const value
+ * @size: Data size in bytes
  */
-#define HW_DESC_SET_DOUT_TYPE(pDesc, dmaMode, doutAdr, doutSize, axiNs)							\
-	do {		                                                                                        	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (doutAdr)&UINT32_MAX );		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD5, DOUT_ADDR_HIGH, (pDesc)->word[5], MSB64(doutAdr) );	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_DMA_MODE, (pDesc)->word[3], (dmaMode));		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_SIZE, (pDesc)->word[3], (doutSize));		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, NS_BIT, (pDesc)->word[3], (axiNs));			\
-	} while (0)
-
-/*!
- * This macro sets the DOUT field of a HW descriptors to DLLI type 
- * The LAST INDICATION is provided by the user 
- * 
- * \param pDesc pointer HW descriptor struct 
- * \param doutAdr DOUT address
- * \param doutSize Data size in bytes 
- * \param lastInd The last indication bit
- * \param axiNs AXI secure bit 
+static inline void set_din_const(struct cc_hw_desc *pdesc, u32 val, u32 size)
+{
+	pdesc->word[0] = val;
+	pdesc->word[1] |= FIELD_PREP(WORD1_DIN_CONST_VALUE, 1) |
+			FIELD_PREP(WORD1_DIN_DMA_MODE, DMA_SRAM) |
+			FIELD_PREP(WORD1_DIN_SIZE, size);
+}
+
+/*
+ * Set the DIN not last input data indicator
+ *
+ * @pdesc: pointer HW descriptor struct
  */
-#define HW_DESC_SET_DOUT_DLLI(pDesc, doutAdr, doutSize, axiNs ,lastInd)								\
-	do {		                                                                                        		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (doutAdr)&UINT32_MAX );		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD5, DOUT_ADDR_HIGH, (pDesc)->word[5], MSB64(doutAdr) );	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_DMA_MODE, (pDesc)->word[3], DMA_DLLI);			\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_SIZE, (pDesc)->word[3], (doutSize));			\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_LAST_IND, (pDesc)->word[3], lastInd);			\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, NS_BIT, (pDesc)->word[3], (axiNs));				\
-	} while (0)
-
-/*!
- * This macro sets the DOUT field of a HW descriptors to DLLI type 
- * The LAST INDICATION is provided by the user 
- * 
- * \param pDesc pointer HW descriptor struct 
- * \param doutAdr DOUT address
- * \param doutSize Data size in bytes 
- * \param lastInd The last indication bit
- * \param axiNs AXI secure bit 
+static inline void set_din_not_last_indication(struct cc_hw_desc *pdesc)
+{
+	pdesc->word[1] |= FIELD_PREP(WORD1_NOT_LAST, 1);
+}
+
+/*
+ * Set the DOUT field of a HW descriptors
+ *
+ * @pdesc: pointer HW descriptor struct
+ * @dma_mode: The DMA mode: NO_DMA, SRAM, DLLI, MLLI, CONSTANT
+ * @addr: DOUT address
+ * @size: Data size in bytes
+ * @axi_sec: AXI secure bit
  */
-#define HW_DESC_SET_DOUT_MLLI(pDesc, doutAdr, doutSize, axiNs ,lastInd)								\
-	do {		                                                                                        		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (doutAdr)&UINT32_MAX );		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD5, DOUT_ADDR_HIGH, (pDesc)->word[5], MSB64(doutAdr) );	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_DMA_MODE, (pDesc)->word[3], DMA_MLLI);			\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_SIZE, (pDesc)->word[3], (doutSize));			\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_LAST_IND, (pDesc)->word[3], lastInd);			\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, NS_BIT, (pDesc)->word[3], (axiNs));				\
-	} while (0)
-
-/*!
- * This macro sets the DOUT field of a HW descriptors to NO DMA mode. Used for NOP descriptor, register patches and 
- * other special modes 
- * 
- * \param pDesc pointer HW descriptor struct
- * \param doutAdr DOUT address
- * \param doutSize Data size in bytes  
- * \param registerWriteEnable Enables a write operation to a register
+static inline void set_dout_type(struct cc_hw_desc *pdesc,
+				 enum cc_dma_mode dma_mode, dma_addr_t addr,
+				 u32 size, enum cc_axi_sec axi_sec)
+{
+	pdesc->word[2] = (u32)addr;
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+	pdesc->word[5] |= FIELD_PREP(WORD5_DOUT_ADDR_HIGH, ((u16)(addr >> 32)));
+#endif
+	pdesc->word[3] |= FIELD_PREP(WORD3_DOUT_DMA_MODE, dma_mode) |
+				FIELD_PREP(WORD3_DOUT_SIZE, size) |
+				FIELD_PREP(WORD3_NS_BIT, axi_sec);
+}
+
+/*
+ * Set the DOUT field of a HW descriptors to DLLI type
+ * The LAST INDICATION is provided by the user
+ *
+ * @pdesc pointer HW descriptor struct
+ * @addr: DOUT address
+ * @size: Data size in bytes
+ * @last_ind: The last indication bit
+ * @axi_sec: AXI secure bit
  */
-#define HW_DESC_SET_DOUT_NO_DMA(pDesc, doutAdr, doutSize, registerWriteEnable)							\
-	do {		                                                                                        		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (uint32_t)(doutAdr));			\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_SIZE, (pDesc)->word[3], (doutSize));			\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_LAST_IND, (pDesc)->word[3], (registerWriteEnable));	\
-	} while (0)
-
-/*!
- * This macro sets the word for the XOR operation. 
- * 
- * \param pDesc pointer HW descriptor struct
- * \param xorVal xor data value
+static inline void set_dout_dlli(struct cc_hw_desc *pdesc, dma_addr_t addr,
+				 u32 size, enum cc_axi_sec axi_sec,
+				 u32 last_ind)
+{
+	set_dout_type(pdesc, DMA_DLLI, addr, size, axi_sec);
+	pdesc->word[3] |= FIELD_PREP(WORD3_DOUT_LAST_IND, last_ind);
+}
+
+/*
+ * Set the DOUT field of a HW descriptors to DLLI type
+ * The LAST INDICATION is provided by the user
+ *
+ * @pdesc: pointer HW descriptor struct
+ * @addr: DOUT address
+ * @size: Data size in bytes
+ * @last_ind: The last indication bit
+ * @axi_sec: AXI secure bit
  */
-#define HW_DESC_SET_XOR_VAL(pDesc, xorVal)										\
-	do {		                                                                                        	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (uint32_t)(xorVal));		\
-	} while (0)
-
-/*!
- * This macro sets the XOR indicator bit in the descriptor
- * 
- * \param pDesc pointer HW descriptor struct
+static inline void set_dout_mlli(struct cc_hw_desc *pdesc, dma_addr_t addr,
+				 u32 size, enum cc_axi_sec axi_sec,
+				 bool last_ind)
+{
+	set_dout_type(pdesc, DMA_MLLI, addr, size, axi_sec);
+	pdesc->word[3] |= FIELD_PREP(WORD3_DOUT_LAST_IND, last_ind);
+}
+
+/*
+ * Set the DOUT field of a HW descriptors to NO DMA mode.
+ * Used for NOP descriptor, register patches and other special modes.
+ *
+ * @pdesc: pointer HW descriptor struct
+ * @addr: DOUT address
+ * @size: Data size in bytes
+ * @write_enable: Enables a write operation to a register
  */
-#define HW_DESC_SET_XOR_ACTIVE(pDesc)											\
-	do {		                                                                                        	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, HASH_XOR_BIT, (pDesc)->word[3], 1);			\
-	} while (0)
-
-/*!
- * This macro selects the AES engine instead of HASH engine when setting up combined mode with AES XCBC MAC
- * 
- * \param pDesc pointer HW descriptor struct
+static inline void set_dout_no_dma(struct cc_hw_desc *pdesc, u32 addr,
+				   u32 size, bool write_enable)
+{
+	pdesc->word[2] = addr;
+	pdesc->word[3] |= FIELD_PREP(WORD3_DOUT_SIZE, size) |
+			FIELD_PREP(WORD3_DOUT_LAST_IND, write_enable);
+}
+
+/*
+ * Set the word for the XOR operation.
+ *
+ * @pdesc: pointer HW descriptor struct
+ * @val: xor data value
  */
-#define HW_DESC_SET_AES_NOT_HASH_MODE(pDesc)										\
-	do {		                                                                                       	 	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, AES_SEL_N_HASH, (pDesc)->word[4], 1);			\
-	} while (0)
-
-/*!
- * This macro sets the DOUT field of a HW descriptors to SRAM mode
- * Note: No need to check SRAM alignment since host requests do not use SRAM and 
- * adaptor will enforce alignment check. 
- * 
- * \param pDesc pointer HW descriptor struct
- * \param doutAdr DOUT address
- * \param doutSize Data size in bytes 
+static inline void set_xor_val(struct cc_hw_desc *pdesc, u32 val)
+{
+	pdesc->word[2] = val;
+}
+
+/*
+ * Sets the XOR indicator bit in the descriptor
+ *
+ * @pdesc: pointer HW descriptor struct
  */
-#define HW_DESC_SET_DOUT_SRAM(pDesc, doutAdr, doutSize)									\
-	do {		                                                                                        	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (uint32_t)(doutAdr));		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_DMA_MODE, (pDesc)->word[3], DMA_SRAM);		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_SIZE, (pDesc)->word[3], (doutSize));		\
-	} while (0)
-
-
-/*!
- * This macro sets the data unit size for XEX mode in data_out_addr[15:0]
- * 
- * \param pDesc pointer HW descriptor struct
- * \param dataUnitSize data unit size for XEX mode
+static inline void set_xor_active(struct cc_hw_desc *pdesc)
+{
+	pdesc->word[3] |= FIELD_PREP(WORD3_HASH_XOR_BIT, 1);
+}
+
+/*
+ * Select the AES engine instead of HASH engine when setting up combined mode
+ * with AES XCBC MAC
+ *
+ * @pdesc: pointer HW descriptor struct
  */
-#define HW_DESC_SET_XEX_DATA_UNIT_SIZE(pDesc, dataUnitSize)								\
-	do {														\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (uint32_t)(dataUnitSize));	\
-	} while (0)
+static inline void set_aes_not_hash_mode(struct cc_hw_desc *pdesc)
+{
+	pdesc->word[4] |= FIELD_PREP(WORD4_AES_SEL_N_HASH, 1);
+}
 
-/*!
- * This macro sets the number of rounds for Multi2 in data_out_addr[15:0]
+/*
+ * Set the DOUT field of a HW descriptors to SRAM mode
+ * Note: No need to check SRAM alignment since host requests do not use SRAM and
+ * adaptor will enforce alignment check.
  *
- * \param pDesc pointer HW descriptor struct
- * \param numRounds number of rounds for Multi2
-*/
-#define HW_DESC_SET_MULTI2_NUM_ROUNDS(pDesc, numRounds)									\
-	do {														\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (uint32_t)(numRounds));	\
-	} while (0)
-
-/*!
- * This macro sets the flow mode.
+ * @pdesc: pointer HW descriptor struct
+ * @addr: DOUT address
+ * @size: Data size in bytes
+ */
+static inline void set_dout_sram(struct cc_hw_desc *pdesc, u32 addr, u32 size)
+{
+	pdesc->word[2] = addr;
+	pdesc->word[3] |= FIELD_PREP(WORD3_DOUT_DMA_MODE, DMA_SRAM) |
+			FIELD_PREP(WORD3_DOUT_SIZE, size);
+}
+
+/*
+ * Sets the data unit size for XEX mode in data_out_addr[15:0]
  *
- * \param pDesc pointer HW descriptor struct
- * \param flowMode Any one of the modes defined in [CC7x-DESC]
-*/
+ * @pdesc: pDesc pointer HW descriptor struct
+ * @size: data unit size for XEX mode
+ */
+static inline void set_xex_data_unit_size(struct cc_hw_desc *pdesc, u32 size)
+{
+	pdesc->word[2] = size;
+}
 
-#define HW_DESC_SET_FLOW_MODE(pDesc, flowMode)										\
-	do {														\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, DATA_FLOW_MODE, (pDesc)->word[4], (flowMode));		\
-	} while (0)
+/*
+ * Set the number of rounds for Multi2 in data_out_addr[15:0]
+ *
+ * @pdesc: pointer HW descriptor struct
+ * @num: number of rounds for Multi2
+ */
+static inline void set_multi2_num_rounds(struct cc_hw_desc *pdesc, u32 num)
+{
+	pdesc->word[2] = num;
+}
 
-/*!
- * This macro sets the cipher mode.
+/*
+ * Set the flow mode.
  *
- * \param pDesc pointer HW descriptor struct
- * \param cipherMode Any one of the modes defined in [CC7x-DESC]
-*/
-#define HW_DESC_SET_CIPHER_MODE(pDesc, cipherMode)									\
-	do {														\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CIPHER_MODE, (pDesc)->word[4], (cipherMode));		\
-	} while (0)
-
-/*!
- * This macro sets the cipher configuration fields.
+ * @pdesc: pointer HW descriptor struct
+ * @mode: Any one of the modes defined in [CC7x-DESC]
+ */
+static inline void set_flow_mode(struct cc_hw_desc *pdesc,
+				 enum cc_flow_mode mode)
+{
+	pdesc->word[4] |= FIELD_PREP(WORD4_DATA_FLOW_MODE, mode);
+}
+
+/*
+ * Set the cipher mode.
  *
- * \param pDesc pointer HW descriptor struct
- * \param cipherConfig Any one of the modes defined in [CC7x-DESC]
-*/
-#define HW_DESC_SET_CIPHER_CONFIG0(pDesc, cipherConfig)									\
-	do {														\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CIPHER_CONF0, (pDesc)->word[4], (cipherConfig));	\
-	} while (0)
-
-/*!
- * This macro sets the cipher configuration fields.
+ * @pdesc: pointer HW descriptor struct
+ * @mode:  Any one of the modes defined in [CC7x-DESC]
+ */
+static inline void set_cipher_mode(struct cc_hw_desc *pdesc,
+				   enum drv_cipher_mode mode)
+{
+	pdesc->word[4] |= FIELD_PREP(WORD4_CIPHER_MODE, mode);
+}
+
+/*
+ * Set the cipher configuration fields.
  *
- * \param pDesc pointer HW descriptor struct
- * \param cipherConfig Any one of the modes defined in [CC7x-DESC]
-*/
-#define HW_DESC_SET_CIPHER_CONFIG1(pDesc, cipherConfig)									\
-	do {														\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CIPHER_CONF1, (pDesc)->word[4], (cipherConfig));	\
-	} while (0)
-
-/*!
- * This macro sets HW key configuration fields.
+ * @pdesc: pointer HW descriptor struct
+ * @mode: Any one of the modes defined in [CC7x-DESC]
+ */
+static inline void set_cipher_config0(struct cc_hw_desc *pdesc,
+				      enum drv_crypto_direction mode)
+{
+	pdesc->word[4] |= FIELD_PREP(WORD4_CIPHER_CONF0, mode);
+}
+
+/*
+ * Set the cipher configuration fields.
  *
- * \param pDesc pointer HW descriptor struct
- * \param hwKey The hw key number as in enun HwCryptoKey
-*/
-#define HW_DESC_SET_HW_CRYPTO_KEY(pDesc, hwKey)										\
-	do {														\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CIPHER_DO, (pDesc)->word[4], (hwKey)&HW_KEY_MASK_CIPHER_DO);		\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CIPHER_CONF2, (pDesc)->word[4], (hwKey>>HW_KEY_SHIFT_CIPHER_CFG2));	\
-	} while (0)
-
-/*!
- * This macro changes the bytes order of all setup-finalize descriptosets.
+ * @pdesc: pointer HW descriptor struct
+ * @config: Any one of the modes defined in [CC7x-DESC]
+ */
+static inline void set_cipher_config1(struct cc_hw_desc *pdesc,
+				      enum cc_hash_conf_pad config)
+{
+	pdesc->word[4] |= FIELD_PREP(WORD4_CIPHER_CONF1, config);
+}
+
+/*
+ * Set HW key configuration fields.
  *
- * \param pDesc pointer HW descriptor struct
- * \param swapConfig Any one of the modes defined in [CC7x-DESC]
-*/
-#define HW_DESC_SET_BYTES_SWAP(pDesc, swapConfig)									\
-	do {														\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, BYTES_SWAP, (pDesc)->word[4], (swapConfig));		\
-	} while (0)
-
-/*!
- * This macro sets the CMAC_SIZE0 mode.
+ * @pdesc: pointer HW descriptor struct
+ * @hw_key: The HW key slot asdefined in enum cc_hw_crypto_key
+ */
+static inline void set_hw_crypto_key(struct cc_hw_desc *pdesc,
+				     enum cc_hw_crypto_key hw_key)
+{
+	pdesc->word[4] |= FIELD_PREP(WORD4_CIPHER_DO,
+				     (hw_key & HW_KEY_MASK_CIPHER_DO)) |
+			FIELD_PREP(WORD4_CIPHER_CONF2,
+				   (hw_key >> HW_KEY_SHIFT_CIPHER_CFG2));
+}
+
+/*
+ * Set byte order of all setup-finalize descriptors.
  *
- * \param pDesc pointer HW descriptor struct
-*/
-#define HW_DESC_SET_CMAC_SIZE0_MODE(pDesc)										\
-	do {														\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CMAC_SIZE0, (pDesc)->word[4], 0x1);			\
-	} while (0)
-
-/*!
- * This macro sets the key size for AES engine.
+ * @pdesc: pointer HW descriptor struct
+ * @config: Any one of the modes defined in [CC7x-DESC]
+ */
+static inline void set_bytes_swap(struct cc_hw_desc *pdesc, bool config)
+{
+	pdesc->word[4] |= FIELD_PREP(WORD4_BYTES_SWAP, config);
+}
+
+/*
+ * Set CMAC_SIZE0 mode.
  *
- * \param pDesc pointer HW descriptor struct
- * \param keySize key size in bytes (NOT size code)
-*/
-#define HW_DESC_SET_KEY_SIZE_AES(pDesc, keySize)									\
-	do {													        \
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, KEY_SIZE, (pDesc)->word[4], ((keySize) >> 3) - 2);	\
-	} while (0)
-
-/*!
- * This macro sets the key size for DES engine.
+ * @pdesc: pointer HW descriptor struct
+ */
+static inline void set_cmac_size0_mode(struct cc_hw_desc *pdesc)
+{
+	pdesc->word[4] |= FIELD_PREP(WORD4_CMAC_SIZE0, 1);
+}
+
+/*
+ * Set key size descriptor field.
  *
- * \param pDesc pointer HW descriptor struct
- * \param keySize key size in bytes (NOT size code)
-*/
-#define HW_DESC_SET_KEY_SIZE_DES(pDesc, keySize)									\
-	do {													        \
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, KEY_SIZE, (pDesc)->word[4], ((keySize) >> 3) - 1);	\
-	} while (0)
-
-/*!
- * This macro sets the descriptor's setup mode
+ * @pdesc: pointer HW descriptor struct
+ * @size: key size in bytes (NOT size code)
+ */
+static inline void set_key_size(struct cc_hw_desc *pdesc, u32 size)
+{
+	pdesc->word[4] |= FIELD_PREP(WORD4_KEY_SIZE, size);
+}
+
+/*
+ * Set AES key size.
  *
- * \param pDesc pointer HW descriptor struct
- * \param setupMode Any one of the setup modes defined in [CC7x-DESC]
-*/
-#define HW_DESC_SET_SETUP_MODE(pDesc, setupMode)									\
-	do {														\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, SETUP_OPERATION, (pDesc)->word[4], (setupMode));	\
-	} while (0)
-
-/*!
- * This macro sets the descriptor's cipher do
+ * @pdesc: pointer HW descriptor struct
+ * @size: key size in bytes (NOT size code)
+ */
+static inline void set_key_size_aes(struct cc_hw_desc *pdesc, u32 size)
+{
+	set_key_size(pdesc, ((size >> 3) - 2));
+}
+
+/*
+ * Set DES key size.
  *
- * \param pDesc pointer HW descriptor struct
- * \param cipherDo Any one of the cipher do defined in [CC7x-DESC]
-*/
-#define HW_DESC_SET_CIPHER_DO(pDesc, cipherDo)											\
-	do {															\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CIPHER_DO, (pDesc)->word[4], (cipherDo)&HW_KEY_MASK_CIPHER_DO);	\
-	} while (0)
-
-/*!
- * This macro sets the DIN field of a HW descriptors to star/stop monitor descriptor. 
- * Used for performance measurements and debug purposes.
- * 
- * \param pDesc pointer HW descriptor struct
+ * @pdesc: pointer HW descriptor struct
+ * @size: key size in bytes (NOT size code)
  */
-#define HW_DESC_SET_DIN_MONITOR_CNTR(pDesc)										\
-	do {		                                                                                        	\
-		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_MEASURE_CNTR, VALUE, (pDesc)->word[1], _HW_DESC_MONITOR_KICK);	\
-	} while (0)
+static inline void set_key_size_des(struct cc_hw_desc *pdesc, u32 size)
+{
+	set_key_size(pdesc, ((size >> 3) - 1));
+}
 
+/*
+ * Set the descriptor setup mode
+ *
+ * @pdesc: pointer HW descriptor struct
+ * @mode: Any one of the setup modes defined in [CC7x-DESC]
+ */
+static inline void set_setup_mode(struct cc_hw_desc *pdesc,
+				  enum cc_setup_op mode)
+{
+	pdesc->word[4] |= FIELD_PREP(WORD4_SETUP_OPERATION, mode);
+}
 
+/*
+ * Set the descriptor cipher DO
+ *
+ * @pdesc: pointer HW descriptor struct
+ * @config: Any one of the cipher do defined in [CC7x-DESC]
+ */
+static inline void set_cipher_do(struct cc_hw_desc *pdesc,
+				 enum cc_hash_cipher_pad config)
+{
+	pdesc->word[4] |= FIELD_PREP(WORD4_CIPHER_DO,
+				(config & HW_KEY_MASK_CIPHER_DO));
+}
 
 #endif /*__CC_HW_QUEUE_DEFS_H__*/
diff --git a/drivers/staging/ccree/cc_lli_defs.h b/drivers/staging/ccree/cc_lli_defs.h
index 697f1ed181e0..851d3907167e 100644
--- a/drivers/staging/ccree/cc_lli_defs.h
+++ b/drivers/staging/ccree/cc_lli_defs.h
@@ -1,46 +1,42 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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 _CC_LLI_DEFS_H_
 #define _CC_LLI_DEFS_H_
-#ifdef __KERNEL__
-#include <linux/types.h>
-#else
-#include <stdint.h>
-#endif
-#include "cc_bitops.h"
 
-/* Max DLLI size */
-#define DLLI_SIZE_BIT_SIZE	0x18	// DX_DSCRPTR_QUEUE_WORD1_DIN_SIZE_BIT_SIZE
-
-#define CC_MAX_MLLI_ENTRY_SIZE 0x10000
+#include <linux/types.h>
 
-#define MSB64(_addr) (sizeof(_addr) == 4 ? 0 : ((_addr) >> 32)&UINT16_MAX)
+/* Max DLLI size
+ *  AKA DX_DSCRPTR_QUEUE_WORD1_DIN_SIZE_BIT_SIZE
+ */
+#define DLLI_SIZE_BIT_SIZE	0x18
 
-#define LLI_SET_ADDR(lli_p, addr) \
-		BITFIELD_SET(((uint32_t *)(lli_p))[LLI_WORD0_OFFSET], LLI_LADDR_BIT_OFFSET, LLI_LADDR_BIT_SIZE, (addr & UINT32_MAX)); \
-		BITFIELD_SET(((uint32_t *)(lli_p))[LLI_WORD1_OFFSET], LLI_HADDR_BIT_OFFSET, LLI_HADDR_BIT_SIZE, MSB64(addr));
+#define CC_MAX_MLLI_ENTRY_SIZE 0xFFFF
 
-#define LLI_SET_SIZE(lli_p, size) \
-		BITFIELD_SET(((uint32_t *)(lli_p))[LLI_WORD1_OFFSET], LLI_SIZE_BIT_OFFSET, LLI_SIZE_BIT_SIZE, size)
+#define LLI_MAX_NUM_OF_DATA_ENTRIES 128
+#define LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES 4
+#define MLLI_TABLE_MIN_ALIGNMENT 4 /* 32 bit alignment */
+#define MAX_NUM_OF_BUFFERS_IN_MLLI 4
+#define MAX_NUM_OF_TOTAL_MLLI_ENTRIES \
+		(2 * LLI_MAX_NUM_OF_DATA_ENTRIES + \
+		 LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES)
 
 /* Size of entry */
 #define LLI_ENTRY_WORD_SIZE 2
-#define LLI_ENTRY_BYTE_SIZE (LLI_ENTRY_WORD_SIZE * sizeof(uint32_t))
+#define LLI_ENTRY_BYTE_SIZE (LLI_ENTRY_WORD_SIZE * sizeof(u32))
 
 /* Word0[31:0] = ADDR[31:0] */
 #define LLI_WORD0_OFFSET 0
@@ -53,5 +49,24 @@
 #define LLI_HADDR_BIT_OFFSET 16
 #define LLI_HADDR_BIT_SIZE 16
 
+#define LLI_SIZE_MASK GENMASK((LLI_SIZE_BIT_SIZE - 1), LLI_SIZE_BIT_OFFSET)
+#define LLI_HADDR_MASK GENMASK( \
+			       (LLI_HADDR_BIT_OFFSET + LLI_HADDR_BIT_SIZE - 1),\
+				LLI_HADDR_BIT_OFFSET)
+
+static inline void cc_lli_set_addr(u32 *lli_p, dma_addr_t addr)
+{
+	lli_p[LLI_WORD0_OFFSET] = (addr & U32_MAX);
+#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+	lli_p[LLI_WORD1_OFFSET] &= ~LLI_HADDR_MASK;
+	lli_p[LLI_WORD1_OFFSET] |= FIELD_PREP(LLI_HADDR_MASK, (addr >> 16));
+#endif /* CONFIG_ARCH_DMA_ADDR_T_64BIT */
+}
+
+static inline void cc_lli_set_size(u32 *lli_p, u16 size)
+{
+	lli_p[LLI_WORD1_OFFSET] &= ~LLI_SIZE_MASK;
+	lli_p[LLI_WORD1_OFFSET] |= FIELD_PREP(LLI_SIZE_MASK, size);
+}
 
 #endif /*_CC_LLI_DEFS_H_*/
diff --git a/drivers/staging/ccree/cc_pal_log.h b/drivers/staging/ccree/cc_pal_log.h
deleted file mode 100644
index e5f5a8737833..000000000000
--- a/drivers/staging/ccree/cc_pal_log.h
+++ /dev/null
@@ -1,188 +0,0 @@
-/*
- * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
- * This program is free software; you can 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 _CC_PAL_LOG_H_
-#define _CC_PAL_LOG_H_
-
-#include "cc_pal_types.h"
-#include "cc_pal_log_plat.h"
-
-/*!
-@file 
-@brief This file contains the PAL layer log definitions, by default the log is disabled. 
-@defgroup cc_pal_log CryptoCell PAL logging APIs and definitions
-@{
-@ingroup cc_pal
-*/
-
-/* PAL log levels (to be used in CC_PAL_logLevel) */
-/*! PAL log level - disabled. */
-#define CC_PAL_LOG_LEVEL_NULL      (-1) /*!< \internal Disable logging */
-/*! PAL log level - error. */
-#define CC_PAL_LOG_LEVEL_ERR       0
-/*! PAL log level - warning. */
-#define CC_PAL_LOG_LEVEL_WARN      1
-/*! PAL log level - info. */
-#define CC_PAL_LOG_LEVEL_INFO      2
-/*! PAL log level - debug. */
-#define CC_PAL_LOG_LEVEL_DEBUG     3
-/*! PAL log level - trace. */
-#define CC_PAL_LOG_LEVEL_TRACE     4
-/*! PAL log level - data. */
-#define CC_PAL_LOG_LEVEL_DATA      5
-
-#ifndef CC_PAL_LOG_CUR_COMPONENT
-/* Setting default component mask in case caller did not define */
-/* (a mask that is always on for every log mask value but full masking) */
-/*! Default log debugged component.*/
-#define CC_PAL_LOG_CUR_COMPONENT 0xFFFFFFFF
-#endif
-#ifndef CC_PAL_LOG_CUR_COMPONENT_NAME
-/*! Default log debugged component.*/
-#define CC_PAL_LOG_CUR_COMPONENT_NAME "CC"
-#endif
-
-/* Select compile time log level (default if not explicitly specified by caller) */
-#ifndef CC_PAL_MAX_LOG_LEVEL /* Can be overriden by external definition of this constant */
-#ifdef DEBUG
-/*! Default debug log level (when debug is set to on).*/
-#define CC_PAL_MAX_LOG_LEVEL  CC_PAL_LOG_LEVEL_ERR /*CC_PAL_LOG_LEVEL_DEBUG*/
-#else /* Disable logging */
-/*! Default debug log level (when debug is set to on).*/
-#define CC_PAL_MAX_LOG_LEVEL CC_PAL_LOG_LEVEL_NULL
-#endif
-#endif /*CC_PAL_MAX_LOG_LEVEL*/
-/*! Evaluate CC_PAL_MAX_LOG_LEVEL in case provided by caller */
-#define __CC_PAL_LOG_LEVEL_EVAL(level) level
-/*! Maximal log level defintion.*/
-#define _CC_PAL_MAX_LOG_LEVEL __CC_PAL_LOG_LEVEL_EVAL(CC_PAL_MAX_LOG_LEVEL)
-
-
-#ifdef ARM_DSM
-/*! Log init function. */
-#define CC_PalLogInit() do {} while (0)
-/*! Log set level function - sets the level of logging in case of debug. */
-#define CC_PalLogLevelSet(setLevel) do {} while (0)
-/*! Log set mask function - sets the component masking in case of debug. */
-#define CC_PalLogMaskSet(setMask) do {} while (0)
-#else
-#if _CC_PAL_MAX_LOG_LEVEL > CC_PAL_LOG_LEVEL_NULL
-/*! Log init function. */
-void CC_PalLogInit(void);
-/*! Log set level function - sets the level of logging in case of debug. */
-void CC_PalLogLevelSet(int setLevel);
-/*! Log set mask function - sets the component masking in case of debug. */
-void CC_PalLogMaskSet(uint32_t setMask);
-/*! Global variable for log level */
-extern int CC_PAL_logLevel;
-/*! Global variable for log mask */
-extern uint32_t CC_PAL_logMask;
-#else /* No log */
-/*! Log init function. */
-static inline void CC_PalLogInit(void) {}
-/*! Log set level function - sets the level of logging in case of debug. */
-static inline void CC_PalLogLevelSet(int setLevel) {CC_UNUSED_PARAM(setLevel);}
-/*! Log set mask function - sets the component masking in case of debug. */
-static inline void CC_PalLogMaskSet(uint32_t setMask) {CC_UNUSED_PARAM(setMask);}
-#endif
-#endif
-
-/*! Filter logging based on logMask and dispatch to platform specific logging mechanism. */
-#define _CC_PAL_LOG(level, format, ...)  \
-	if (CC_PAL_logMask & CC_PAL_LOG_CUR_COMPONENT) \
-		__CC_PAL_LOG_PLAT(CC_PAL_LOG_LEVEL_ ## level, "%s:%s: " format, CC_PAL_LOG_CUR_COMPONENT_NAME, __func__, ##__VA_ARGS__)
-
-#if (_CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_ERR)
-/*! Log messages according to log level.*/
-#define CC_PAL_LOG_ERR(format, ... ) \
-	_CC_PAL_LOG(ERR, format, ##__VA_ARGS__)
-#else
-/*! Log messages according to log level.*/
-#define CC_PAL_LOG_ERR( ... ) do {} while (0)
-#endif
-
-#if (_CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_WARN)
-/*! Log messages according to log level.*/
-#define CC_PAL_LOG_WARN(format, ... ) \
-	if (CC_PAL_logLevel >= CC_PAL_LOG_LEVEL_WARN) \
-		_CC_PAL_LOG(WARN, format, ##__VA_ARGS__)
-#else
-/*! Log messages according to log level.*/
-#define CC_PAL_LOG_WARN( ... ) do {} while (0)
-#endif
-
-#if (_CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_INFO)
-/*! Log messages according to log level.*/
-#define CC_PAL_LOG_INFO(format, ... ) \
-	if (CC_PAL_logLevel >= CC_PAL_LOG_LEVEL_INFO) \
-		_CC_PAL_LOG(INFO, format, ##__VA_ARGS__)
-#else
-/*! Log messages according to log level.*/
-#define CC_PAL_LOG_INFO( ... ) do {} while (0)
-#endif
-
-#if (_CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_DEBUG)
-/*! Log messages according to log level.*/
-#define CC_PAL_LOG_DEBUG(format, ... ) \
-	if (CC_PAL_logLevel >= CC_PAL_LOG_LEVEL_DEBUG) \
-		_CC_PAL_LOG(DEBUG, format, ##__VA_ARGS__)
-
-/*! Log message buffer.*/
-#define CC_PAL_LOG_DUMP_BUF(msg, buf, size)		\
-	do {						\
-	int i;						\
-	uint8_t	*pData = (uint8_t*)buf;			\
-							\
-	PRINTF("%s (%d):\n", msg, size);		\
-	for (i = 0; i < size; i++) {			\
-		PRINTF("0x%02X ", pData[i]);		\
-		if ((i & 0xF) == 0xF) {			\
-			PRINTF("\n");			\
-		}					\
-	}						\
-	PRINTF("\n");					\
-	} while (0)
-#else
-/*! Log debug messages.*/
-#define CC_PAL_LOG_DEBUG( ... ) do {} while (0)
-/*! Log debug buffer.*/
-#define CC_PAL_LOG_DUMP_BUF(msg, buf, size)	do {} while (0)
-#endif
-
-#if (_CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_TRACE)
-/*! Log debug trace.*/
-#define CC_PAL_LOG_TRACE(format, ... ) \
-	if (CC_PAL_logLevel >= CC_PAL_LOG_LEVEL_TRACE) \
-		_CC_PAL_LOG(TRACE, format, ##__VA_ARGS__)
-#else
-/*! Log debug trace.*/
-#define CC_PAL_LOG_TRACE(...) do {} while (0)
-#endif
-
-#if (_CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_TRACE)
-/*! Log debug data.*/
-#define CC_PAL_LOG_DATA(format, ...) \
-	if (CC_PAL_logLevel >= CC_PAL_LOG_LEVEL_TRACE) \
-		_CC_PAL_LOG(DATA, format, ##__VA_ARGS__)
-#else
-/*! Log debug data.*/
-#define CC_PAL_LOG_DATA( ...) do {} while (0)
-#endif
-/** 
-@}
- */
-
-#endif /*_CC_PAL_LOG_H_*/
diff --git a/drivers/staging/ccree/cc_pal_log_plat.h b/drivers/staging/ccree/cc_pal_log_plat.h
deleted file mode 100644
index a05a200cf6eb..000000000000
--- a/drivers/staging/ccree/cc_pal_log_plat.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
- * This program is free software; you can 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/>.
- */
-
-/* Dummy pal_log_plat for test driver in kernel */
-
-#ifndef _SSI_PAL_LOG_PLAT_H_
-#define _SSI_PAL_LOG_PLAT_H_
-
-#if defined(DEBUG)
-
-#define __CC_PAL_LOG_PLAT(level, format, ...) printk(level "cc7x_test::" format , ##__VA_ARGS__)
-
-#else /* Disable all prints */
-
-#define __CC_PAL_LOG_PLAT(...)  do {} while (0)
-
-#endif
-
-#endif /*_SASI_PAL_LOG_PLAT_H_*/
-
diff --git a/drivers/staging/ccree/cc_pal_types.h b/drivers/staging/ccree/cc_pal_types.h
deleted file mode 100644
index 9b59bbb34515..000000000000
--- a/drivers/staging/ccree/cc_pal_types.h
+++ /dev/null
@@ -1,97 +0,0 @@
-/*
- * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
- * This program is free software; you can 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 CC_PAL_TYPES_H
-#define CC_PAL_TYPES_H
-
-/*! 
-@file 
-@brief This file contains platform-dependent definitions and types. 
-@defgroup cc_pal_types CryptoCell PAL platform dependant types
-@{
-@ingroup cc_pal
-
-*/
- 
-#include "cc_pal_types_plat.h"
-
-/*! Boolean definition.*/
-typedef enum {
-	/*! Boolean false definition.*/
-	CC_FALSE = 0,
-	/*! Boolean true definition.*/
-	CC_TRUE = 1
-} CCBool;
-
-/*! Success definition. */
-#define CC_SUCCESS              0UL
-/*! Failure definition. */
-#define CC_FAIL		  	1UL
-
-/*! Defintion of 1KB in bytes. */
-#define CC_1K_SIZE_IN_BYTES	1024
-/*! Defintion of number of bits in a byte. */
-#define CC_BITS_IN_BYTE		8
-/*! Defintion of number of bits in a 32bits word. */
-#define CC_BITS_IN_32BIT_WORD	32
-/*! Defintion of number of bytes in a 32bits word. */
-#define CC_32BIT_WORD_SIZE	(sizeof(uint32_t))
-
-/*! Success (OK) defintion. */
-#define CC_OK   0
-
-/*! Macro that handles unused parameters in the code (to avoid compilation warnings).  */
-#define CC_UNUSED_PARAM(prm)  ((void)prm)
-
-/*! Maximal uint32 value.*/
-#define CC_MAX_UINT32_VAL 	(0xFFFFFFFF)
-
-
-/* Minimum and Maximum macros */
-#ifdef  min
-/*! Definition for minimum. */
-#define CC_MIN(a,b) min( a , b )
-#else
-/*! Definition for minimum. */
-#define CC_MIN( a , b ) ( ( (a) < (b) ) ? (a) : (b) )
-#endif
-
-#ifdef max    
-/*! Definition for maximum. */    
-#define CC_MAX(a,b) max( a , b )
-#else
-/*! Definition for maximum. */    
-#define CC_MAX( a , b ) ( ( (a) > (b) ) ? (a) : (b) )
-#endif
-
-/*! Macro that calculates number of full bytes from bits (i.e. 7 bits are 1 byte). */    
-#define CALC_FULL_BYTES(numBits) 		((numBits)/CC_BITS_IN_BYTE + (((numBits) & (CC_BITS_IN_BYTE-1)) > 0)) 
-/*! Macro that calculates number of full 32bits words from bits (i.e. 31 bits are 1 word). */    
-#define CALC_FULL_32BIT_WORDS(numBits) 		((numBits)/CC_BITS_IN_32BIT_WORD +  (((numBits) & (CC_BITS_IN_32BIT_WORD-1)) > 0))   
-/*! Macro that calculates number of full 32bits words from bytes (i.e. 3 bytes are 1 word). */    
-#define CALC_32BIT_WORDS_FROM_BYTES(sizeBytes)  ((sizeBytes)/CC_32BIT_WORD_SIZE + (((sizeBytes) & (CC_32BIT_WORD_SIZE-1)) > 0)) 
-/*! Macro that round up bits to 32bits words. */     
-#define ROUNDUP_BITS_TO_32BIT_WORD(numBits) 	(CALC_FULL_32BIT_WORDS(numBits) * CC_BITS_IN_32BIT_WORD)
-/*! Macro that round up bits to bytes. */    
-#define ROUNDUP_BITS_TO_BYTES(numBits) 		(CALC_FULL_BYTES(numBits) * CC_BITS_IN_BYTE)
-/*! Macro that round up bytes to 32bits words. */    
-#define ROUNDUP_BYTES_TO_32BIT_WORD(sizeBytes) 	(CALC_32BIT_WORDS_FROM_BYTES(sizeBytes) * CC_32BIT_WORD_SIZE)     
-
-
-/** 
-@}
- */
-#endif
diff --git a/drivers/staging/ccree/cc_pal_types_plat.h b/drivers/staging/ccree/cc_pal_types_plat.h
deleted file mode 100644
index 6e4211262231..000000000000
--- a/drivers/staging/ccree/cc_pal_types_plat.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
- * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
- * This program is free software; you can 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 SSI_PAL_TYPES_PLAT_H
-#define SSI_PAL_TYPES_PLAT_H
-/* Linux kernel types */
-
-#include <linux/types.h>
-
-#ifndef NULL /* Missing in Linux kernel */
-#define NULL (0x0L)
-#endif
-
-
-#endif /*SSI_PAL_TYPES_PLAT_H*/
diff --git a/drivers/staging/ccree/cc_regs.h b/drivers/staging/ccree/cc_regs.h
index 963f8148cd28..4a893a6ba6ef 100644
--- a/drivers/staging/ccree/cc_regs.h
+++ b/drivers/staging/ccree/cc_regs.h
@@ -1,106 +1,42 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
-
 /*!
- * @file 
- * @brief This file contains macro definitions for accessing ARM TrustZone CryptoCell register space.
+ * @file
+ * @brief This file contains macro definitions for accessing ARM TrustZone
+ *        CryptoCell register space.
  */
 
 #ifndef _CC_REGS_H_
 #define _CC_REGS_H_
 
-#include "cc_bitops.h"
+#include <linux/bitfield.h>
+
+#define AXIM_MON_BASE_OFFSET CC_REG_OFFSET(CRY_KERNEL, AXIM_MON_COMP)
+#define AXIM_MON_COMP_VALUE GENMASK(DX_AXIM_MON_COMP_VALUE_BIT_SIZE + \
+		DX_AXIM_MON_COMP_VALUE_BIT_SHIFT, \
+		DX_AXIM_MON_COMP_VALUE_BIT_SHIFT)
+
+#define AXIM_MON_BASE_OFFSET CC_REG_OFFSET(CRY_KERNEL, AXIM_MON_COMP)
+#define AXIM_MON_COMP_VALUE GENMASK(DX_AXIM_MON_COMP_VALUE_BIT_SIZE + \
+		DX_AXIM_MON_COMP_VALUE_BIT_SHIFT, \
+		DX_AXIM_MON_COMP_VALUE_BIT_SHIFT)
 
 /* Register Offset macro */
 #define CC_REG_OFFSET(unit_name, reg_name)               \
 	(DX_BASE_ ## unit_name + DX_ ## reg_name ## _REG_OFFSET)
 
-#define CC_REG_BIT_SHIFT(reg_name, field_name)               \
-	(DX_ ## reg_name ## _ ## field_name ## _BIT_SHIFT)
-
-/* Register Offset macros (from registers base address in host) */
-#include "dx_reg_base_host.h"
-
-/* Read-Modify-Write a field of a register */
-#define MODIFY_REGISTER_FLD(unitName, regName, fldName, fldVal)         \
-do {								            \
-	uint32_t regVal;						    \
-	regVal = READ_REGISTER(CC_REG_ADDR(unitName, regName));       \
-	CC_REG_FLD_SET(unitName, regName, fldName, regVal, fldVal); \
-	WRITE_REGISTER(CC_REG_ADDR(unitName, regName), regVal);       \
-} while (0)
-
-/* Registers address macros for ENV registers (development FPGA only) */
-#ifdef DX_BASE_ENV_REGS
-
-/* This offset should be added to mapping address of DX_BASE_ENV_REGS */
-#define CC_ENV_REG_OFFSET(reg_name) (DX_ENV_ ## reg_name ## _REG_OFFSET)
-
-#endif /*DX_BASE_ENV_REGS*/
-
-/*! Bit fields get */
-#define CC_REG_FLD_GET(unit_name, reg_name, fld_name, reg_val)	      \
-	(DX_ ## reg_name ## _ ## fld_name ## _BIT_SIZE == 0x20 ?	      \
-	reg_val /*!< \internal Optimization for 32b fields */ :			      \
-	BITFIELD_GET(reg_val, DX_ ## reg_name ## _ ## fld_name ## _BIT_SHIFT, \
-		     DX_ ## reg_name ## _ ## fld_name ## _BIT_SIZE))
-
-/*! Bit fields access */
-#define CC_REG_FLD_GET2(unit_name, reg_name, fld_name, reg_val)	      \
-	(CC_ ## reg_name ## _ ## fld_name ## _BIT_SIZE == 0x20 ?	      \
-	reg_val /*!< \internal Optimization for 32b fields */ :			      \
-	BITFIELD_GET(reg_val, CC_ ## reg_name ## _ ## fld_name ## _BIT_SHIFT, \
-		     CC_ ## reg_name ## _ ## fld_name ## _BIT_SIZE))
-
-/* yael TBD !!! -       				      * 
-* all HW includes should start with CC_ and not DX_ !!	      */
-
-
-/*! Bit fields set */
-#define CC_REG_FLD_SET(                                               \
-	unit_name, reg_name, fld_name, reg_shadow_var, new_fld_val)      \
-do {                                                                     \
-	if (DX_ ## reg_name ## _ ## fld_name ## _BIT_SIZE == 0x20)       \
-		reg_shadow_var = new_fld_val; /*!< \internal Optimization for 32b fields */\
-	else                                                             \
-		BITFIELD_SET(reg_shadow_var,                             \
-			DX_ ## reg_name ## _ ## fld_name ## _BIT_SHIFT,  \
-			DX_ ## reg_name ## _ ## fld_name ## _BIT_SIZE,   \
-			new_fld_val);                                    \
-} while (0)
-
-/*! Bit fields set */
-#define CC_REG_FLD_SET2(                                               \
-	unit_name, reg_name, fld_name, reg_shadow_var, new_fld_val)      \
-do {                                                                     \
-	if (CC_ ## reg_name ## _ ## fld_name ## _BIT_SIZE == 0x20)       \
-		reg_shadow_var = new_fld_val; /*!< \internal Optimization for 32b fields */\
-	else                                                             \
-		BITFIELD_SET(reg_shadow_var,                             \
-			CC_ ## reg_name ## _ ## fld_name ## _BIT_SHIFT,  \
-			CC_ ## reg_name ## _ ## fld_name ## _BIT_SIZE,   \
-			new_fld_val);                                    \
-} while (0)
-
-/* Usage example:
-   uint32_t reg_shadow = READ_REGISTER(CC_REG_ADDR(CRY_KERNEL,AES_CONTROL));
-   CC_REG_FLD_SET(CRY_KERNEL,AES_CONTROL,NK_KEY0,reg_shadow, 3);
-   CC_REG_FLD_SET(CRY_KERNEL,AES_CONTROL,NK_KEY1,reg_shadow, 1);
-   WRITE_REGISTER(CC_REG_ADDR(CRY_KERNEL,AES_CONTROL), reg_shadow);
- */
-
 #endif /*_CC_REGS_H_*/
diff --git a/drivers/staging/ccree/dx_crys_kernel.h b/drivers/staging/ccree/dx_crys_kernel.h
index 703469c4a828..219603030344 100644
--- a/drivers/staging/ccree/dx_crys_kernel.h
+++ b/drivers/staging/ccree/dx_crys_kernel.h
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -20,161 +20,161 @@
 // --------------------------------------
 // BLOCK: DSCRPTR
 // --------------------------------------
-#define DX_DSCRPTR_COMPLETION_COUNTER_REG_OFFSET 	0xE00UL 
-#define DX_DSCRPTR_COMPLETION_COUNTER_COMPLETION_COUNTER_BIT_SHIFT 	0x0UL
-#define DX_DSCRPTR_COMPLETION_COUNTER_COMPLETION_COUNTER_BIT_SIZE 	0x6UL
-#define DX_DSCRPTR_COMPLETION_COUNTER_OVERFLOW_COUNTER_BIT_SHIFT 	0x6UL
-#define DX_DSCRPTR_COMPLETION_COUNTER_OVERFLOW_COUNTER_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_SW_RESET_REG_OFFSET 	0xE40UL 
-#define DX_DSCRPTR_SW_RESET_VALUE_BIT_SHIFT 	0x0UL
-#define DX_DSCRPTR_SW_RESET_VALUE_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_SRAM_SIZE_REG_OFFSET 	0xE60UL 
-#define DX_DSCRPTR_QUEUE_SRAM_SIZE_NUM_OF_DSCRPTR_BIT_SHIFT 	0x0UL
-#define DX_DSCRPTR_QUEUE_SRAM_SIZE_NUM_OF_DSCRPTR_BIT_SIZE 	0xAUL
-#define DX_DSCRPTR_QUEUE_SRAM_SIZE_DSCRPTR_SRAM_SIZE_BIT_SHIFT 	0xAUL
-#define DX_DSCRPTR_QUEUE_SRAM_SIZE_DSCRPTR_SRAM_SIZE_BIT_SIZE 	0xCUL
-#define DX_DSCRPTR_QUEUE_SRAM_SIZE_SRAM_SIZE_BIT_SHIFT 	0x16UL
-#define DX_DSCRPTR_QUEUE_SRAM_SIZE_SRAM_SIZE_BIT_SIZE 	0x3UL
-#define DX_DSCRPTR_SINGLE_ADDR_EN_REG_OFFSET 	0xE64UL 
-#define DX_DSCRPTR_SINGLE_ADDR_EN_VALUE_BIT_SHIFT 	0x0UL
-#define DX_DSCRPTR_SINGLE_ADDR_EN_VALUE_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_MEASURE_CNTR_REG_OFFSET 	0xE68UL 
-#define DX_DSCRPTR_MEASURE_CNTR_VALUE_BIT_SHIFT 	0x0UL
-#define DX_DSCRPTR_MEASURE_CNTR_VALUE_BIT_SIZE 	0x20UL
-#define DX_DSCRPTR_QUEUE_WORD0_REG_OFFSET 	0xE80UL 
-#define DX_DSCRPTR_QUEUE_WORD0_VALUE_BIT_SHIFT 	0x0UL
-#define DX_DSCRPTR_QUEUE_WORD0_VALUE_BIT_SIZE 	0x20UL
-#define DX_DSCRPTR_QUEUE_WORD1_REG_OFFSET 	0xE84UL 
-#define DX_DSCRPTR_QUEUE_WORD1_DIN_DMA_MODE_BIT_SHIFT 	0x0UL
-#define DX_DSCRPTR_QUEUE_WORD1_DIN_DMA_MODE_BIT_SIZE 	0x2UL
-#define DX_DSCRPTR_QUEUE_WORD1_DIN_SIZE_BIT_SHIFT 	0x2UL
-#define DX_DSCRPTR_QUEUE_WORD1_DIN_SIZE_BIT_SIZE 	0x18UL
-#define DX_DSCRPTR_QUEUE_WORD1_NS_BIT_BIT_SHIFT 	0x1AUL
-#define DX_DSCRPTR_QUEUE_WORD1_NS_BIT_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD1_DIN_CONST_VALUE_BIT_SHIFT 	0x1BUL
-#define DX_DSCRPTR_QUEUE_WORD1_DIN_CONST_VALUE_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD1_NOT_LAST_BIT_SHIFT 	0x1CUL
-#define DX_DSCRPTR_QUEUE_WORD1_NOT_LAST_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD1_LOCK_QUEUE_BIT_SHIFT 	0x1DUL
-#define DX_DSCRPTR_QUEUE_WORD1_LOCK_QUEUE_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD1_NOT_USED_BIT_SHIFT 	0x1EUL
-#define DX_DSCRPTR_QUEUE_WORD1_NOT_USED_BIT_SIZE 	0x2UL
-#define DX_DSCRPTR_QUEUE_WORD2_REG_OFFSET 	0xE88UL 
-#define DX_DSCRPTR_QUEUE_WORD2_VALUE_BIT_SHIFT 	0x0UL
-#define DX_DSCRPTR_QUEUE_WORD2_VALUE_BIT_SIZE 	0x20UL
-#define DX_DSCRPTR_QUEUE_WORD3_REG_OFFSET 	0xE8CUL 
-#define DX_DSCRPTR_QUEUE_WORD3_DOUT_DMA_MODE_BIT_SHIFT 	0x0UL
-#define DX_DSCRPTR_QUEUE_WORD3_DOUT_DMA_MODE_BIT_SIZE 	0x2UL
-#define DX_DSCRPTR_QUEUE_WORD3_DOUT_SIZE_BIT_SHIFT 	0x2UL
-#define DX_DSCRPTR_QUEUE_WORD3_DOUT_SIZE_BIT_SIZE 	0x18UL
-#define DX_DSCRPTR_QUEUE_WORD3_NS_BIT_BIT_SHIFT 	0x1AUL
-#define DX_DSCRPTR_QUEUE_WORD3_NS_BIT_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD3_DOUT_LAST_IND_BIT_SHIFT 	0x1BUL
-#define DX_DSCRPTR_QUEUE_WORD3_DOUT_LAST_IND_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD3_HASH_XOR_BIT_BIT_SHIFT 	0x1DUL
-#define DX_DSCRPTR_QUEUE_WORD3_HASH_XOR_BIT_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD3_NOT_USED_BIT_SHIFT 	0x1EUL
-#define DX_DSCRPTR_QUEUE_WORD3_NOT_USED_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD3_QUEUE_LAST_IND_BIT_SHIFT 	0x1FUL
-#define DX_DSCRPTR_QUEUE_WORD3_QUEUE_LAST_IND_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD4_REG_OFFSET 	0xE90UL 
-#define DX_DSCRPTR_QUEUE_WORD4_DATA_FLOW_MODE_BIT_SHIFT 	0x0UL
-#define DX_DSCRPTR_QUEUE_WORD4_DATA_FLOW_MODE_BIT_SIZE 	0x6UL
-#define DX_DSCRPTR_QUEUE_WORD4_AES_SEL_N_HASH_BIT_SHIFT 	0x6UL
-#define DX_DSCRPTR_QUEUE_WORD4_AES_SEL_N_HASH_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD4_AES_XOR_CRYPTO_KEY_BIT_SHIFT 	0x7UL
-#define DX_DSCRPTR_QUEUE_WORD4_AES_XOR_CRYPTO_KEY_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD4_ACK_NEEDED_BIT_SHIFT 	0x8UL
-#define DX_DSCRPTR_QUEUE_WORD4_ACK_NEEDED_BIT_SIZE 	0x2UL
-#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_MODE_BIT_SHIFT 	0xAUL
-#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_MODE_BIT_SIZE 	0x4UL
-#define DX_DSCRPTR_QUEUE_WORD4_CMAC_SIZE0_BIT_SHIFT 	0xEUL
-#define DX_DSCRPTR_QUEUE_WORD4_CMAC_SIZE0_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_DO_BIT_SHIFT 	0xFUL
-#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_DO_BIT_SIZE 	0x2UL
-#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF0_BIT_SHIFT 	0x11UL
-#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF0_BIT_SIZE 	0x2UL
-#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF1_BIT_SHIFT 	0x13UL
-#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF1_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF2_BIT_SHIFT 	0x14UL
-#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF2_BIT_SIZE 	0x2UL
-#define DX_DSCRPTR_QUEUE_WORD4_KEY_SIZE_BIT_SHIFT 	0x16UL
-#define DX_DSCRPTR_QUEUE_WORD4_KEY_SIZE_BIT_SIZE 	0x2UL
-#define DX_DSCRPTR_QUEUE_WORD4_SETUP_OPERATION_BIT_SHIFT 	0x18UL
-#define DX_DSCRPTR_QUEUE_WORD4_SETUP_OPERATION_BIT_SIZE 	0x4UL
-#define DX_DSCRPTR_QUEUE_WORD4_DIN_SRAM_ENDIANNESS_BIT_SHIFT 	0x1CUL
-#define DX_DSCRPTR_QUEUE_WORD4_DIN_SRAM_ENDIANNESS_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD4_DOUT_SRAM_ENDIANNESS_BIT_SHIFT 	0x1DUL
-#define DX_DSCRPTR_QUEUE_WORD4_DOUT_SRAM_ENDIANNESS_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD4_WORD_SWAP_BIT_SHIFT 	0x1EUL
-#define DX_DSCRPTR_QUEUE_WORD4_WORD_SWAP_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD4_BYTES_SWAP_BIT_SHIFT 	0x1FUL
-#define DX_DSCRPTR_QUEUE_WORD4_BYTES_SWAP_BIT_SIZE 	0x1UL
-#define DX_DSCRPTR_QUEUE_WORD5_REG_OFFSET 	0xE94UL 
-#define DX_DSCRPTR_QUEUE_WORD5_DIN_ADDR_HIGH_BIT_SHIFT 	0x0UL
-#define DX_DSCRPTR_QUEUE_WORD5_DIN_ADDR_HIGH_BIT_SIZE 	0x10UL
-#define DX_DSCRPTR_QUEUE_WORD5_DOUT_ADDR_HIGH_BIT_SHIFT 	0x10UL
-#define DX_DSCRPTR_QUEUE_WORD5_DOUT_ADDR_HIGH_BIT_SIZE 	0x10UL
-#define DX_DSCRPTR_QUEUE_WATERMARK_REG_OFFSET 	0xE98UL 
-#define DX_DSCRPTR_QUEUE_WATERMARK_VALUE_BIT_SHIFT 	0x0UL
-#define DX_DSCRPTR_QUEUE_WATERMARK_VALUE_BIT_SIZE 	0xAUL
-#define DX_DSCRPTR_QUEUE_CONTENT_REG_OFFSET 	0xE9CUL 
-#define DX_DSCRPTR_QUEUE_CONTENT_VALUE_BIT_SHIFT 	0x0UL
-#define DX_DSCRPTR_QUEUE_CONTENT_VALUE_BIT_SIZE 	0xAUL
+#define DX_DSCRPTR_COMPLETION_COUNTER_REG_OFFSET	0xE00UL
+#define DX_DSCRPTR_COMPLETION_COUNTER_COMPLETION_COUNTER_BIT_SHIFT	0x0UL
+#define DX_DSCRPTR_COMPLETION_COUNTER_COMPLETION_COUNTER_BIT_SIZE	0x6UL
+#define DX_DSCRPTR_COMPLETION_COUNTER_OVERFLOW_COUNTER_BIT_SHIFT	0x6UL
+#define DX_DSCRPTR_COMPLETION_COUNTER_OVERFLOW_COUNTER_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_SW_RESET_REG_OFFSET	0xE40UL
+#define DX_DSCRPTR_SW_RESET_VALUE_BIT_SHIFT	0x0UL
+#define DX_DSCRPTR_SW_RESET_VALUE_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_REG_OFFSET	0xE60UL
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_NUM_OF_DSCRPTR_BIT_SHIFT	0x0UL
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_NUM_OF_DSCRPTR_BIT_SIZE	0xAUL
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_DSCRPTR_SRAM_SIZE_BIT_SHIFT	0xAUL
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_DSCRPTR_SRAM_SIZE_BIT_SIZE	0xCUL
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_SRAM_SIZE_BIT_SHIFT	0x16UL
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_SRAM_SIZE_BIT_SIZE	0x3UL
+#define DX_DSCRPTR_SINGLE_ADDR_EN_REG_OFFSET	0xE64UL
+#define DX_DSCRPTR_SINGLE_ADDR_EN_VALUE_BIT_SHIFT	0x0UL
+#define DX_DSCRPTR_SINGLE_ADDR_EN_VALUE_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_MEASURE_CNTR_REG_OFFSET	0xE68UL
+#define DX_DSCRPTR_MEASURE_CNTR_VALUE_BIT_SHIFT	0x0UL
+#define DX_DSCRPTR_MEASURE_CNTR_VALUE_BIT_SIZE	0x20UL
+#define DX_DSCRPTR_QUEUE_WORD0_REG_OFFSET	0xE80UL
+#define DX_DSCRPTR_QUEUE_WORD0_VALUE_BIT_SHIFT	0x0UL
+#define DX_DSCRPTR_QUEUE_WORD0_VALUE_BIT_SIZE	0x20UL
+#define DX_DSCRPTR_QUEUE_WORD1_REG_OFFSET	0xE84UL
+#define DX_DSCRPTR_QUEUE_WORD1_DIN_DMA_MODE_BIT_SHIFT	0x0UL
+#define DX_DSCRPTR_QUEUE_WORD1_DIN_DMA_MODE_BIT_SIZE	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD1_DIN_SIZE_BIT_SHIFT	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD1_DIN_SIZE_BIT_SIZE	0x18UL
+#define DX_DSCRPTR_QUEUE_WORD1_NS_BIT_BIT_SHIFT	0x1AUL
+#define DX_DSCRPTR_QUEUE_WORD1_NS_BIT_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD1_DIN_CONST_VALUE_BIT_SHIFT	0x1BUL
+#define DX_DSCRPTR_QUEUE_WORD1_DIN_CONST_VALUE_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD1_NOT_LAST_BIT_SHIFT	0x1CUL
+#define DX_DSCRPTR_QUEUE_WORD1_NOT_LAST_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD1_LOCK_QUEUE_BIT_SHIFT	0x1DUL
+#define DX_DSCRPTR_QUEUE_WORD1_LOCK_QUEUE_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD1_NOT_USED_BIT_SHIFT	0x1EUL
+#define DX_DSCRPTR_QUEUE_WORD1_NOT_USED_BIT_SIZE	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD2_REG_OFFSET	0xE88UL
+#define DX_DSCRPTR_QUEUE_WORD2_VALUE_BIT_SHIFT	0x0UL
+#define DX_DSCRPTR_QUEUE_WORD2_VALUE_BIT_SIZE	0x20UL
+#define DX_DSCRPTR_QUEUE_WORD3_REG_OFFSET	0xE8CUL
+#define DX_DSCRPTR_QUEUE_WORD3_DOUT_DMA_MODE_BIT_SHIFT	0x0UL
+#define DX_DSCRPTR_QUEUE_WORD3_DOUT_DMA_MODE_BIT_SIZE	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD3_DOUT_SIZE_BIT_SHIFT	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD3_DOUT_SIZE_BIT_SIZE	0x18UL
+#define DX_DSCRPTR_QUEUE_WORD3_NS_BIT_BIT_SHIFT	0x1AUL
+#define DX_DSCRPTR_QUEUE_WORD3_NS_BIT_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD3_DOUT_LAST_IND_BIT_SHIFT	0x1BUL
+#define DX_DSCRPTR_QUEUE_WORD3_DOUT_LAST_IND_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD3_HASH_XOR_BIT_BIT_SHIFT	0x1DUL
+#define DX_DSCRPTR_QUEUE_WORD3_HASH_XOR_BIT_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD3_NOT_USED_BIT_SHIFT	0x1EUL
+#define DX_DSCRPTR_QUEUE_WORD3_NOT_USED_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD3_QUEUE_LAST_IND_BIT_SHIFT	0x1FUL
+#define DX_DSCRPTR_QUEUE_WORD3_QUEUE_LAST_IND_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_REG_OFFSET	0xE90UL
+#define DX_DSCRPTR_QUEUE_WORD4_DATA_FLOW_MODE_BIT_SHIFT	0x0UL
+#define DX_DSCRPTR_QUEUE_WORD4_DATA_FLOW_MODE_BIT_SIZE	0x6UL
+#define DX_DSCRPTR_QUEUE_WORD4_AES_SEL_N_HASH_BIT_SHIFT	0x6UL
+#define DX_DSCRPTR_QUEUE_WORD4_AES_SEL_N_HASH_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_AES_XOR_CRYPTO_KEY_BIT_SHIFT	0x7UL
+#define DX_DSCRPTR_QUEUE_WORD4_AES_XOR_CRYPTO_KEY_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_ACK_NEEDED_BIT_SHIFT	0x8UL
+#define DX_DSCRPTR_QUEUE_WORD4_ACK_NEEDED_BIT_SIZE	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_MODE_BIT_SHIFT	0xAUL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_MODE_BIT_SIZE	0x4UL
+#define DX_DSCRPTR_QUEUE_WORD4_CMAC_SIZE0_BIT_SHIFT	0xEUL
+#define DX_DSCRPTR_QUEUE_WORD4_CMAC_SIZE0_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_DO_BIT_SHIFT	0xFUL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_DO_BIT_SIZE	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF0_BIT_SHIFT	0x11UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF0_BIT_SIZE	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF1_BIT_SHIFT	0x13UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF1_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF2_BIT_SHIFT	0x14UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF2_BIT_SIZE	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD4_KEY_SIZE_BIT_SHIFT	0x16UL
+#define DX_DSCRPTR_QUEUE_WORD4_KEY_SIZE_BIT_SIZE	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD4_SETUP_OPERATION_BIT_SHIFT	0x18UL
+#define DX_DSCRPTR_QUEUE_WORD4_SETUP_OPERATION_BIT_SIZE	0x4UL
+#define DX_DSCRPTR_QUEUE_WORD4_DIN_SRAM_ENDIANNESS_BIT_SHIFT	0x1CUL
+#define DX_DSCRPTR_QUEUE_WORD4_DIN_SRAM_ENDIANNESS_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_DOUT_SRAM_ENDIANNESS_BIT_SHIFT	0x1DUL
+#define DX_DSCRPTR_QUEUE_WORD4_DOUT_SRAM_ENDIANNESS_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_WORD_SWAP_BIT_SHIFT	0x1EUL
+#define DX_DSCRPTR_QUEUE_WORD4_WORD_SWAP_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_BYTES_SWAP_BIT_SHIFT	0x1FUL
+#define DX_DSCRPTR_QUEUE_WORD4_BYTES_SWAP_BIT_SIZE	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD5_REG_OFFSET	0xE94UL
+#define DX_DSCRPTR_QUEUE_WORD5_DIN_ADDR_HIGH_BIT_SHIFT	0x0UL
+#define DX_DSCRPTR_QUEUE_WORD5_DIN_ADDR_HIGH_BIT_SIZE	0x10UL
+#define DX_DSCRPTR_QUEUE_WORD5_DOUT_ADDR_HIGH_BIT_SHIFT	0x10UL
+#define DX_DSCRPTR_QUEUE_WORD5_DOUT_ADDR_HIGH_BIT_SIZE	0x10UL
+#define DX_DSCRPTR_QUEUE_WATERMARK_REG_OFFSET	0xE98UL
+#define DX_DSCRPTR_QUEUE_WATERMARK_VALUE_BIT_SHIFT	0x0UL
+#define DX_DSCRPTR_QUEUE_WATERMARK_VALUE_BIT_SIZE	0xAUL
+#define DX_DSCRPTR_QUEUE_CONTENT_REG_OFFSET	0xE9CUL
+#define DX_DSCRPTR_QUEUE_CONTENT_VALUE_BIT_SHIFT	0x0UL
+#define DX_DSCRPTR_QUEUE_CONTENT_VALUE_BIT_SIZE	0xAUL
 // --------------------------------------
 // BLOCK: AXI_P
 // --------------------------------------
-#define DX_AXIM_MON_INFLIGHT_REG_OFFSET 	0xB00UL 
-#define DX_AXIM_MON_INFLIGHT_VALUE_BIT_SHIFT 	0x0UL
-#define DX_AXIM_MON_INFLIGHT_VALUE_BIT_SIZE 	0x8UL
-#define DX_AXIM_MON_INFLIGHTLAST_REG_OFFSET 	0xB40UL 
-#define DX_AXIM_MON_INFLIGHTLAST_VALUE_BIT_SHIFT 	0x0UL
-#define DX_AXIM_MON_INFLIGHTLAST_VALUE_BIT_SIZE 	0x8UL
-#define DX_AXIM_MON_COMP_REG_OFFSET 	0xB80UL 
-#define DX_AXIM_MON_COMP_VALUE_BIT_SHIFT 	0x0UL
-#define DX_AXIM_MON_COMP_VALUE_BIT_SIZE 	0x10UL
-#define DX_AXIM_MON_ERR_REG_OFFSET 	0xBC4UL 
-#define DX_AXIM_MON_ERR_BRESP_BIT_SHIFT 	0x0UL
-#define DX_AXIM_MON_ERR_BRESP_BIT_SIZE 	0x2UL
-#define DX_AXIM_MON_ERR_BID_BIT_SHIFT 	0x2UL
-#define DX_AXIM_MON_ERR_BID_BIT_SIZE 	0x4UL
-#define DX_AXIM_MON_ERR_RRESP_BIT_SHIFT 	0x10UL
-#define DX_AXIM_MON_ERR_RRESP_BIT_SIZE 	0x2UL
-#define DX_AXIM_MON_ERR_RID_BIT_SHIFT 	0x12UL
-#define DX_AXIM_MON_ERR_RID_BIT_SIZE 	0x4UL
-#define DX_AXIM_CFG_REG_OFFSET 	0xBE8UL 
-#define DX_AXIM_CFG_BRESPMASK_BIT_SHIFT 	0x4UL
-#define DX_AXIM_CFG_BRESPMASK_BIT_SIZE 	0x1UL
-#define DX_AXIM_CFG_RRESPMASK_BIT_SHIFT 	0x5UL
-#define DX_AXIM_CFG_RRESPMASK_BIT_SIZE 	0x1UL
-#define DX_AXIM_CFG_INFLTMASK_BIT_SHIFT 	0x6UL
-#define DX_AXIM_CFG_INFLTMASK_BIT_SIZE 	0x1UL
-#define DX_AXIM_CFG_COMPMASK_BIT_SHIFT 	0x7UL
-#define DX_AXIM_CFG_COMPMASK_BIT_SIZE 	0x1UL
-#define DX_AXIM_ACE_CONST_REG_OFFSET 	0xBECUL 
-#define DX_AXIM_ACE_CONST_ARDOMAIN_BIT_SHIFT 	0x0UL
-#define DX_AXIM_ACE_CONST_ARDOMAIN_BIT_SIZE 	0x2UL
-#define DX_AXIM_ACE_CONST_AWDOMAIN_BIT_SHIFT 	0x2UL
-#define DX_AXIM_ACE_CONST_AWDOMAIN_BIT_SIZE 	0x2UL
-#define DX_AXIM_ACE_CONST_ARBAR_BIT_SHIFT 	0x4UL
-#define DX_AXIM_ACE_CONST_ARBAR_BIT_SIZE 	0x2UL
-#define DX_AXIM_ACE_CONST_AWBAR_BIT_SHIFT 	0x6UL
-#define DX_AXIM_ACE_CONST_AWBAR_BIT_SIZE 	0x2UL
-#define DX_AXIM_ACE_CONST_ARSNOOP_BIT_SHIFT 	0x8UL
-#define DX_AXIM_ACE_CONST_ARSNOOP_BIT_SIZE 	0x4UL
-#define DX_AXIM_ACE_CONST_AWSNOOP_NOT_ALIGNED_BIT_SHIFT 	0xCUL
-#define DX_AXIM_ACE_CONST_AWSNOOP_NOT_ALIGNED_BIT_SIZE 	0x3UL
-#define DX_AXIM_ACE_CONST_AWSNOOP_ALIGNED_BIT_SHIFT 	0xFUL
-#define DX_AXIM_ACE_CONST_AWSNOOP_ALIGNED_BIT_SIZE 	0x3UL
-#define DX_AXIM_ACE_CONST_AWADDR_NOT_MASKED_BIT_SHIFT 	0x12UL
-#define DX_AXIM_ACE_CONST_AWADDR_NOT_MASKED_BIT_SIZE 	0x7UL
-#define DX_AXIM_ACE_CONST_AWLEN_VAL_BIT_SHIFT 	0x19UL
-#define DX_AXIM_ACE_CONST_AWLEN_VAL_BIT_SIZE 	0x4UL
-#define DX_AXIM_CACHE_PARAMS_REG_OFFSET 	0xBF0UL 
-#define DX_AXIM_CACHE_PARAMS_AWCACHE_LAST_BIT_SHIFT 	0x0UL
-#define DX_AXIM_CACHE_PARAMS_AWCACHE_LAST_BIT_SIZE 	0x4UL
-#define DX_AXIM_CACHE_PARAMS_AWCACHE_BIT_SHIFT 	0x4UL
-#define DX_AXIM_CACHE_PARAMS_AWCACHE_BIT_SIZE 	0x4UL
-#define DX_AXIM_CACHE_PARAMS_ARCACHE_BIT_SHIFT 	0x8UL
-#define DX_AXIM_CACHE_PARAMS_ARCACHE_BIT_SIZE 	0x4UL
+#define DX_AXIM_MON_INFLIGHT_REG_OFFSET	0xB00UL
+#define DX_AXIM_MON_INFLIGHT_VALUE_BIT_SHIFT	0x0UL
+#define DX_AXIM_MON_INFLIGHT_VALUE_BIT_SIZE	0x8UL
+#define DX_AXIM_MON_INFLIGHTLAST_REG_OFFSET	0xB40UL
+#define DX_AXIM_MON_INFLIGHTLAST_VALUE_BIT_SHIFT	0x0UL
+#define DX_AXIM_MON_INFLIGHTLAST_VALUE_BIT_SIZE	0x8UL
+#define DX_AXIM_MON_COMP_REG_OFFSET	0xB80UL
+#define DX_AXIM_MON_COMP_VALUE_BIT_SHIFT	0x0UL
+#define DX_AXIM_MON_COMP_VALUE_BIT_SIZE	0x10UL
+#define DX_AXIM_MON_ERR_REG_OFFSET	0xBC4UL
+#define DX_AXIM_MON_ERR_BRESP_BIT_SHIFT	0x0UL
+#define DX_AXIM_MON_ERR_BRESP_BIT_SIZE	0x2UL
+#define DX_AXIM_MON_ERR_BID_BIT_SHIFT	0x2UL
+#define DX_AXIM_MON_ERR_BID_BIT_SIZE	0x4UL
+#define DX_AXIM_MON_ERR_RRESP_BIT_SHIFT	0x10UL
+#define DX_AXIM_MON_ERR_RRESP_BIT_SIZE	0x2UL
+#define DX_AXIM_MON_ERR_RID_BIT_SHIFT	0x12UL
+#define DX_AXIM_MON_ERR_RID_BIT_SIZE	0x4UL
+#define DX_AXIM_CFG_REG_OFFSET	0xBE8UL
+#define DX_AXIM_CFG_BRESPMASK_BIT_SHIFT	0x4UL
+#define DX_AXIM_CFG_BRESPMASK_BIT_SIZE	0x1UL
+#define DX_AXIM_CFG_RRESPMASK_BIT_SHIFT	0x5UL
+#define DX_AXIM_CFG_RRESPMASK_BIT_SIZE	0x1UL
+#define DX_AXIM_CFG_INFLTMASK_BIT_SHIFT	0x6UL
+#define DX_AXIM_CFG_INFLTMASK_BIT_SIZE	0x1UL
+#define DX_AXIM_CFG_COMPMASK_BIT_SHIFT	0x7UL
+#define DX_AXIM_CFG_COMPMASK_BIT_SIZE	0x1UL
+#define DX_AXIM_ACE_CONST_REG_OFFSET	0xBECUL
+#define DX_AXIM_ACE_CONST_ARDOMAIN_BIT_SHIFT	0x0UL
+#define DX_AXIM_ACE_CONST_ARDOMAIN_BIT_SIZE	0x2UL
+#define DX_AXIM_ACE_CONST_AWDOMAIN_BIT_SHIFT	0x2UL
+#define DX_AXIM_ACE_CONST_AWDOMAIN_BIT_SIZE	0x2UL
+#define DX_AXIM_ACE_CONST_ARBAR_BIT_SHIFT	0x4UL
+#define DX_AXIM_ACE_CONST_ARBAR_BIT_SIZE	0x2UL
+#define DX_AXIM_ACE_CONST_AWBAR_BIT_SHIFT	0x6UL
+#define DX_AXIM_ACE_CONST_AWBAR_BIT_SIZE	0x2UL
+#define DX_AXIM_ACE_CONST_ARSNOOP_BIT_SHIFT	0x8UL
+#define DX_AXIM_ACE_CONST_ARSNOOP_BIT_SIZE	0x4UL
+#define DX_AXIM_ACE_CONST_AWSNOOP_NOT_ALIGNED_BIT_SHIFT	0xCUL
+#define DX_AXIM_ACE_CONST_AWSNOOP_NOT_ALIGNED_BIT_SIZE	0x3UL
+#define DX_AXIM_ACE_CONST_AWSNOOP_ALIGNED_BIT_SHIFT	0xFUL
+#define DX_AXIM_ACE_CONST_AWSNOOP_ALIGNED_BIT_SIZE	0x3UL
+#define DX_AXIM_ACE_CONST_AWADDR_NOT_MASKED_BIT_SHIFT	0x12UL
+#define DX_AXIM_ACE_CONST_AWADDR_NOT_MASKED_BIT_SIZE	0x7UL
+#define DX_AXIM_ACE_CONST_AWLEN_VAL_BIT_SHIFT	0x19UL
+#define DX_AXIM_ACE_CONST_AWLEN_VAL_BIT_SIZE	0x4UL
+#define DX_AXIM_CACHE_PARAMS_REG_OFFSET	0xBF0UL
+#define DX_AXIM_CACHE_PARAMS_AWCACHE_LAST_BIT_SHIFT	0x0UL
+#define DX_AXIM_CACHE_PARAMS_AWCACHE_LAST_BIT_SIZE	0x4UL
+#define DX_AXIM_CACHE_PARAMS_AWCACHE_BIT_SHIFT	0x4UL
+#define DX_AXIM_CACHE_PARAMS_AWCACHE_BIT_SIZE	0x4UL
+#define DX_AXIM_CACHE_PARAMS_ARCACHE_BIT_SHIFT	0x8UL
+#define DX_AXIM_CACHE_PARAMS_ARCACHE_BIT_SIZE	0x4UL
 #endif	// __DX_CRYS_KERNEL_H__
diff --git a/drivers/staging/ccree/dx_env.h b/drivers/staging/ccree/dx_env.h
deleted file mode 100644
index 08040604b6da..000000000000
--- a/drivers/staging/ccree/dx_env.h
+++ /dev/null
@@ -1,224 +0,0 @@
-/*
- * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
- * This program is free software; you can 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 __DX_ENV_H__
-#define __DX_ENV_H__
-
-// --------------------------------------
-// BLOCK: FPGA_ENV_REGS
-// --------------------------------------
-#define DX_ENV_PKA_DEBUG_MODE_REG_OFFSET 	0x024UL 
-#define DX_ENV_PKA_DEBUG_MODE_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_PKA_DEBUG_MODE_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_SCAN_MODE_REG_OFFSET 	0x030UL 
-#define DX_ENV_SCAN_MODE_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_SCAN_MODE_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_CC_ALLOW_SCAN_REG_OFFSET 	0x034UL 
-#define DX_ENV_CC_ALLOW_SCAN_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_CC_ALLOW_SCAN_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_CC_HOST_INT_REG_OFFSET 	0x0A0UL 
-#define DX_ENV_CC_HOST_INT_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_CC_HOST_INT_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_CC_PUB_HOST_INT_REG_OFFSET 	0x0A4UL 
-#define DX_ENV_CC_PUB_HOST_INT_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_CC_PUB_HOST_INT_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_CC_RST_N_REG_OFFSET 	0x0A8UL 
-#define DX_ENV_CC_RST_N_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_CC_RST_N_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_RST_OVERRIDE_REG_OFFSET 	0x0ACUL 
-#define DX_ENV_RST_OVERRIDE_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_RST_OVERRIDE_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_CC_POR_N_ADDR_REG_OFFSET 	0x0E0UL 
-#define DX_ENV_CC_POR_N_ADDR_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_CC_POR_N_ADDR_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_CC_COLD_RST_REG_OFFSET 	0x0FCUL 
-#define DX_ENV_CC_COLD_RST_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_CC_COLD_RST_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_DUMMY_ADDR_REG_OFFSET 	0x108UL 
-#define DX_ENV_DUMMY_ADDR_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_DUMMY_ADDR_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_COUNTER_CLR_REG_OFFSET 	0x118UL 
-#define DX_ENV_COUNTER_CLR_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_COUNTER_CLR_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_COUNTER_RD_REG_OFFSET 	0x11CUL 
-#define DX_ENV_COUNTER_RD_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_COUNTER_RD_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_RNG_DEBUG_ENABLE_REG_OFFSET 	0x430UL 
-#define DX_ENV_RNG_DEBUG_ENABLE_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_RNG_DEBUG_ENABLE_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_CC_LCS_REG_OFFSET 	0x43CUL 
-#define DX_ENV_CC_LCS_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_CC_LCS_VALUE_BIT_SIZE 	0x8UL
-#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_REG_OFFSET 	0x440UL 
-#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_CM_BIT_SHIFT 	0x0UL
-#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_CM_BIT_SIZE 	0x1UL
-#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_DM_BIT_SHIFT 	0x1UL
-#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_DM_BIT_SIZE 	0x1UL
-#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_SECURE_BIT_SHIFT 	0x2UL
-#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_SECURE_BIT_SIZE 	0x1UL
-#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_RMA_BIT_SHIFT 	0x3UL
-#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_RMA_BIT_SIZE 	0x1UL
-#define DX_ENV_DCU_EN_REG_OFFSET 	0x444UL 
-#define DX_ENV_DCU_EN_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_DCU_EN_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_CC_LCS_IS_VALID_REG_OFFSET 	0x448UL 
-#define DX_ENV_CC_LCS_IS_VALID_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_CC_LCS_IS_VALID_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_POWER_DOWN_REG_OFFSET 	0x478UL 
-#define DX_ENV_POWER_DOWN_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_POWER_DOWN_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_DCU_H_EN_REG_OFFSET 	0x484UL 
-#define DX_ENV_DCU_H_EN_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_DCU_H_EN_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_VERSION_REG_OFFSET 	0x488UL 
-#define DX_ENV_VERSION_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_VERSION_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_ROSC_WRITE_REG_OFFSET 	0x48CUL 
-#define DX_ENV_ROSC_WRITE_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_ROSC_WRITE_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_ROSC_ADDR_REG_OFFSET 	0x490UL 
-#define DX_ENV_ROSC_ADDR_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_ROSC_ADDR_VALUE_BIT_SIZE 	0x8UL
-#define DX_ENV_RESET_SESSION_KEY_REG_OFFSET 	0x494UL 
-#define DX_ENV_RESET_SESSION_KEY_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_RESET_SESSION_KEY_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_SESSION_KEY_0_REG_OFFSET 	0x4A0UL 
-#define DX_ENV_SESSION_KEY_0_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_SESSION_KEY_0_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_SESSION_KEY_1_REG_OFFSET 	0x4A4UL 
-#define DX_ENV_SESSION_KEY_1_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_SESSION_KEY_1_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_SESSION_KEY_2_REG_OFFSET 	0x4A8UL 
-#define DX_ENV_SESSION_KEY_2_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_SESSION_KEY_2_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_SESSION_KEY_3_REG_OFFSET 	0x4ACUL 
-#define DX_ENV_SESSION_KEY_3_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_SESSION_KEY_3_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_SESSION_KEY_VALID_REG_OFFSET 	0x4B0UL 
-#define DX_ENV_SESSION_KEY_VALID_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_SESSION_KEY_VALID_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_SPIDEN_REG_OFFSET 	0x4D0UL 
-#define DX_ENV_SPIDEN_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_SPIDEN_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_AXIM_USER_PARAMS_REG_OFFSET 	0x600UL 
-#define DX_ENV_AXIM_USER_PARAMS_ARUSER_BIT_SHIFT 	0x0UL
-#define DX_ENV_AXIM_USER_PARAMS_ARUSER_BIT_SIZE 	0x5UL
-#define DX_ENV_AXIM_USER_PARAMS_AWUSER_BIT_SHIFT 	0x5UL
-#define DX_ENV_AXIM_USER_PARAMS_AWUSER_BIT_SIZE 	0x5UL
-#define DX_ENV_SECURITY_MODE_OVERRIDE_REG_OFFSET 	0x604UL 
-#define DX_ENV_SECURITY_MODE_OVERRIDE_AWPROT_NS_BIT_BIT_SHIFT 	0x0UL
-#define DX_ENV_SECURITY_MODE_OVERRIDE_AWPROT_NS_BIT_BIT_SIZE 	0x1UL
-#define DX_ENV_SECURITY_MODE_OVERRIDE_AWPROT_NS_OVERRIDE_BIT_SHIFT 	0x1UL
-#define DX_ENV_SECURITY_MODE_OVERRIDE_AWPROT_NS_OVERRIDE_BIT_SIZE 	0x1UL
-#define DX_ENV_SECURITY_MODE_OVERRIDE_ARPROT_NS_BIT_BIT_SHIFT 	0x2UL
-#define DX_ENV_SECURITY_MODE_OVERRIDE_ARPROT_NS_BIT_BIT_SIZE 	0x1UL
-#define DX_ENV_SECURITY_MODE_OVERRIDE_ARPROT_NS_OVERRIDE_BIT_SHIFT 	0x3UL
-#define DX_ENV_SECURITY_MODE_OVERRIDE_ARPROT_NS_OVERRIDE_BIT_SIZE 	0x1UL
-#define DX_ENV_AO_CC_KPLT_0_REG_OFFSET 	0x620UL 
-#define DX_ENV_AO_CC_KPLT_0_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_AO_CC_KPLT_0_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_AO_CC_KPLT_1_REG_OFFSET 	0x624UL 
-#define DX_ENV_AO_CC_KPLT_1_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_AO_CC_KPLT_1_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_AO_CC_KPLT_2_REG_OFFSET 	0x628UL 
-#define DX_ENV_AO_CC_KPLT_2_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_AO_CC_KPLT_2_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_AO_CC_KPLT_3_REG_OFFSET 	0x62CUL 
-#define DX_ENV_AO_CC_KPLT_3_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_AO_CC_KPLT_3_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_AO_CC_KCST_0_REG_OFFSET 	0x630UL 
-#define DX_ENV_AO_CC_KCST_0_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_AO_CC_KCST_0_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_AO_CC_KCST_1_REG_OFFSET 	0x634UL 
-#define DX_ENV_AO_CC_KCST_1_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_AO_CC_KCST_1_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_AO_CC_KCST_2_REG_OFFSET 	0x638UL 
-#define DX_ENV_AO_CC_KCST_2_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_AO_CC_KCST_2_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_AO_CC_KCST_3_REG_OFFSET 	0x63CUL 
-#define DX_ENV_AO_CC_KCST_3_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_AO_CC_KCST_3_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_APB_FIPS_ADDR_REG_OFFSET 	0x650UL 
-#define DX_ENV_APB_FIPS_ADDR_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_APB_FIPS_ADDR_VALUE_BIT_SIZE 	0xCUL
-#define DX_ENV_APB_FIPS_VAL_REG_OFFSET 	0x654UL 
-#define DX_ENV_APB_FIPS_VAL_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_APB_FIPS_VAL_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_APB_FIPS_MASK_REG_OFFSET 	0x658UL 
-#define DX_ENV_APB_FIPS_MASK_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_APB_FIPS_MASK_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_APB_FIPS_CNT_REG_OFFSET 	0x65CUL 
-#define DX_ENV_APB_FIPS_CNT_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_APB_FIPS_CNT_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_APB_FIPS_NEW_ADDR_REG_OFFSET 	0x660UL 
-#define DX_ENV_APB_FIPS_NEW_ADDR_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_APB_FIPS_NEW_ADDR_VALUE_BIT_SIZE 	0xCUL
-#define DX_ENV_APB_FIPS_NEW_VAL_REG_OFFSET 	0x664UL 
-#define DX_ENV_APB_FIPS_NEW_VAL_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_APB_FIPS_NEW_VAL_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_APBP_FIPS_ADDR_REG_OFFSET 	0x670UL 
-#define DX_ENV_APBP_FIPS_ADDR_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_APBP_FIPS_ADDR_VALUE_BIT_SIZE 	0xCUL
-#define DX_ENV_APBP_FIPS_VAL_REG_OFFSET 	0x674UL 
-#define DX_ENV_APBP_FIPS_VAL_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_APBP_FIPS_VAL_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_APBP_FIPS_MASK_REG_OFFSET 	0x678UL 
-#define DX_ENV_APBP_FIPS_MASK_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_APBP_FIPS_MASK_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_APBP_FIPS_CNT_REG_OFFSET 	0x67CUL 
-#define DX_ENV_APBP_FIPS_CNT_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_APBP_FIPS_CNT_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_APBP_FIPS_NEW_ADDR_REG_OFFSET 	0x680UL 
-#define DX_ENV_APBP_FIPS_NEW_ADDR_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_APBP_FIPS_NEW_ADDR_VALUE_BIT_SIZE 	0xCUL
-#define DX_ENV_APBP_FIPS_NEW_VAL_REG_OFFSET 	0x684UL 
-#define DX_ENV_APBP_FIPS_NEW_VAL_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_APBP_FIPS_NEW_VAL_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_CC_POWERDOWN_EN_REG_OFFSET 	0x690UL 
-#define DX_ENV_CC_POWERDOWN_EN_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_CC_POWERDOWN_EN_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_CC_POWERDOWN_RST_EN_REG_OFFSET 	0x694UL 
-#define DX_ENV_CC_POWERDOWN_RST_EN_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_CC_POWERDOWN_RST_EN_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_POWERDOWN_RST_CNTR_REG_OFFSET 	0x698UL 
-#define DX_ENV_POWERDOWN_RST_CNTR_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_POWERDOWN_RST_CNTR_VALUE_BIT_SIZE 	0x20UL
-#define DX_ENV_POWERDOWN_EN_DEBUG_REG_OFFSET 	0x69CUL 
-#define DX_ENV_POWERDOWN_EN_DEBUG_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_POWERDOWN_EN_DEBUG_VALUE_BIT_SIZE 	0x1UL
-// --------------------------------------
-// BLOCK: ENV_CC_MEMORIES
-// --------------------------------------
-#define DX_ENV_FUSE_READY_REG_OFFSET 	0x000UL 
-#define DX_ENV_FUSE_READY_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_FUSE_READY_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_PERF_RAM_MASTER_REG_OFFSET 	0x0ECUL 
-#define DX_ENV_PERF_RAM_MASTER_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_PERF_RAM_MASTER_VALUE_BIT_SIZE 	0x1UL
-#define DX_ENV_PERF_RAM_ADDR_HIGH4_REG_OFFSET 	0x0F0UL 
-#define DX_ENV_PERF_RAM_ADDR_HIGH4_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_PERF_RAM_ADDR_HIGH4_VALUE_BIT_SIZE 	0x2UL
-#define DX_ENV_FUSES_RAM_REG_OFFSET 	0x3ECUL 
-#define DX_ENV_FUSES_RAM_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_FUSES_RAM_VALUE_BIT_SIZE 	0x20UL
-// --------------------------------------
-// BLOCK: ENV_PERF_RAM_BASE
-// --------------------------------------
-#define DX_ENV_PERF_RAM_BASE_REG_OFFSET 	0x000UL 
-#define DX_ENV_PERF_RAM_BASE_VALUE_BIT_SHIFT 	0x0UL
-#define DX_ENV_PERF_RAM_BASE_VALUE_BIT_SIZE 	0x20UL
-
-#endif /*__DX_ENV_H__*/
diff --git a/drivers/staging/ccree/dx_host.h b/drivers/staging/ccree/dx_host.h
index 4e42e748dc5f..863c2670d826 100644
--- a/drivers/staging/ccree/dx_host.h
+++ b/drivers/staging/ccree/dx_host.h
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -20,136 +20,136 @@
 // --------------------------------------
 // BLOCK: HOST_P
 // --------------------------------------
-#define DX_HOST_IRR_REG_OFFSET 	0xA00UL 
-#define DX_HOST_IRR_DSCRPTR_COMPLETION_LOW_INT_BIT_SHIFT 	0x2UL
-#define DX_HOST_IRR_DSCRPTR_COMPLETION_LOW_INT_BIT_SIZE 	0x1UL
-#define DX_HOST_IRR_AXI_ERR_INT_BIT_SHIFT 	0x8UL
-#define DX_HOST_IRR_AXI_ERR_INT_BIT_SIZE 	0x1UL
-#define DX_HOST_IRR_GPR0_BIT_SHIFT 	0xBUL
-#define DX_HOST_IRR_GPR0_BIT_SIZE 	0x1UL
-#define DX_HOST_IRR_DSCRPTR_WATERMARK_INT_BIT_SHIFT 	0x13UL
-#define DX_HOST_IRR_DSCRPTR_WATERMARK_INT_BIT_SIZE 	0x1UL
-#define DX_HOST_IRR_AXIM_COMP_INT_BIT_SHIFT 	0x17UL
-#define DX_HOST_IRR_AXIM_COMP_INT_BIT_SIZE 	0x1UL
-#define DX_HOST_IMR_REG_OFFSET 	0xA04UL 
-#define DX_HOST_IMR_NOT_USED_MASK_BIT_SHIFT 	0x1UL
-#define DX_HOST_IMR_NOT_USED_MASK_BIT_SIZE 	0x1UL
-#define DX_HOST_IMR_DSCRPTR_COMPLETION_MASK_BIT_SHIFT 	0x2UL
-#define DX_HOST_IMR_DSCRPTR_COMPLETION_MASK_BIT_SIZE 	0x1UL
-#define DX_HOST_IMR_AXI_ERR_MASK_BIT_SHIFT 	0x8UL
-#define DX_HOST_IMR_AXI_ERR_MASK_BIT_SIZE 	0x1UL
-#define DX_HOST_IMR_GPR0_BIT_SHIFT 	0xBUL
-#define DX_HOST_IMR_GPR0_BIT_SIZE 	0x1UL
-#define DX_HOST_IMR_DSCRPTR_WATERMARK_MASK0_BIT_SHIFT 	0x13UL
-#define DX_HOST_IMR_DSCRPTR_WATERMARK_MASK0_BIT_SIZE 	0x1UL
-#define DX_HOST_IMR_AXIM_COMP_INT_MASK_BIT_SHIFT 	0x17UL
-#define DX_HOST_IMR_AXIM_COMP_INT_MASK_BIT_SIZE 	0x1UL
-#define DX_HOST_ICR_REG_OFFSET 	0xA08UL 
-#define DX_HOST_ICR_DSCRPTR_COMPLETION_BIT_SHIFT 	0x2UL
-#define DX_HOST_ICR_DSCRPTR_COMPLETION_BIT_SIZE 	0x1UL
-#define DX_HOST_ICR_AXI_ERR_CLEAR_BIT_SHIFT 	0x8UL
-#define DX_HOST_ICR_AXI_ERR_CLEAR_BIT_SIZE 	0x1UL
-#define DX_HOST_ICR_GPR_INT_CLEAR_BIT_SHIFT 	0xBUL
-#define DX_HOST_ICR_GPR_INT_CLEAR_BIT_SIZE 	0x1UL
-#define DX_HOST_ICR_DSCRPTR_WATERMARK_QUEUE0_CLEAR_BIT_SHIFT 	0x13UL
-#define DX_HOST_ICR_DSCRPTR_WATERMARK_QUEUE0_CLEAR_BIT_SIZE 	0x1UL
-#define DX_HOST_ICR_AXIM_COMP_INT_CLEAR_BIT_SHIFT 	0x17UL
-#define DX_HOST_ICR_AXIM_COMP_INT_CLEAR_BIT_SIZE 	0x1UL
-#define DX_HOST_SIGNATURE_REG_OFFSET 	0xA24UL 
-#define DX_HOST_SIGNATURE_VALUE_BIT_SHIFT 	0x0UL
-#define DX_HOST_SIGNATURE_VALUE_BIT_SIZE 	0x20UL
-#define DX_HOST_BOOT_REG_OFFSET 	0xA28UL 
-#define DX_HOST_BOOT_SYNTHESIS_CONFIG_BIT_SHIFT 	0x0UL
-#define DX_HOST_BOOT_SYNTHESIS_CONFIG_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_LARGE_RKEK_LOCAL_BIT_SHIFT 	0x1UL
-#define DX_HOST_BOOT_LARGE_RKEK_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_HASH_IN_FUSES_LOCAL_BIT_SHIFT 	0x2UL
-#define DX_HOST_BOOT_HASH_IN_FUSES_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_EXT_MEM_SECURED_LOCAL_BIT_SHIFT 	0x3UL
-#define DX_HOST_BOOT_EXT_MEM_SECURED_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_RKEK_ECC_EXISTS_LOCAL_N_BIT_SHIFT 	0x5UL
-#define DX_HOST_BOOT_RKEK_ECC_EXISTS_LOCAL_N_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_SRAM_SIZE_LOCAL_BIT_SHIFT 	0x6UL
-#define DX_HOST_BOOT_SRAM_SIZE_LOCAL_BIT_SIZE 	0x3UL
-#define DX_HOST_BOOT_DSCRPTR_EXISTS_LOCAL_BIT_SHIFT 	0x9UL
-#define DX_HOST_BOOT_DSCRPTR_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_PAU_EXISTS_LOCAL_BIT_SHIFT 	0xAUL
-#define DX_HOST_BOOT_PAU_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_RNG_EXISTS_LOCAL_BIT_SHIFT 	0xBUL
-#define DX_HOST_BOOT_RNG_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_PKA_EXISTS_LOCAL_BIT_SHIFT 	0xCUL
-#define DX_HOST_BOOT_PKA_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_RC4_EXISTS_LOCAL_BIT_SHIFT 	0xDUL
-#define DX_HOST_BOOT_RC4_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_SHA_512_PRSNT_LOCAL_BIT_SHIFT 	0xEUL
-#define DX_HOST_BOOT_SHA_512_PRSNT_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_SHA_256_PRSNT_LOCAL_BIT_SHIFT 	0xFUL
-#define DX_HOST_BOOT_SHA_256_PRSNT_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_MD5_PRSNT_LOCAL_BIT_SHIFT 	0x10UL
-#define DX_HOST_BOOT_MD5_PRSNT_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_HASH_EXISTS_LOCAL_BIT_SHIFT 	0x11UL
-#define DX_HOST_BOOT_HASH_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_C2_EXISTS_LOCAL_BIT_SHIFT 	0x12UL
-#define DX_HOST_BOOT_C2_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_DES_EXISTS_LOCAL_BIT_SHIFT 	0x13UL
-#define DX_HOST_BOOT_DES_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_AES_XCBC_MAC_EXISTS_LOCAL_BIT_SHIFT 	0x14UL
-#define DX_HOST_BOOT_AES_XCBC_MAC_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_AES_CMAC_EXISTS_LOCAL_BIT_SHIFT 	0x15UL
-#define DX_HOST_BOOT_AES_CMAC_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_AES_CCM_EXISTS_LOCAL_BIT_SHIFT 	0x16UL
-#define DX_HOST_BOOT_AES_CCM_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_AES_XEX_HW_T_CALC_LOCAL_BIT_SHIFT 	0x17UL
-#define DX_HOST_BOOT_AES_XEX_HW_T_CALC_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_AES_XEX_EXISTS_LOCAL_BIT_SHIFT 	0x18UL
-#define DX_HOST_BOOT_AES_XEX_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_CTR_EXISTS_LOCAL_BIT_SHIFT 	0x19UL
-#define DX_HOST_BOOT_CTR_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_AES_DIN_BYTE_RESOLUTION_LOCAL_BIT_SHIFT 	0x1AUL
-#define DX_HOST_BOOT_AES_DIN_BYTE_RESOLUTION_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_TUNNELING_ENB_LOCAL_BIT_SHIFT 	0x1BUL
-#define DX_HOST_BOOT_TUNNELING_ENB_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_SUPPORT_256_192_KEY_LOCAL_BIT_SHIFT 	0x1CUL
-#define DX_HOST_BOOT_SUPPORT_256_192_KEY_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_ONLY_ENCRYPT_LOCAL_BIT_SHIFT 	0x1DUL
-#define DX_HOST_BOOT_ONLY_ENCRYPT_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_BOOT_AES_EXISTS_LOCAL_BIT_SHIFT 	0x1EUL
-#define DX_HOST_BOOT_AES_EXISTS_LOCAL_BIT_SIZE 	0x1UL
-#define DX_HOST_VERSION_REG_OFFSET 	0xA40UL 
-#define DX_HOST_VERSION_VALUE_BIT_SHIFT 	0x0UL
-#define DX_HOST_VERSION_VALUE_BIT_SIZE 	0x20UL
-#define DX_HOST_KFDE0_VALID_REG_OFFSET 	0xA60UL 
-#define DX_HOST_KFDE0_VALID_VALUE_BIT_SHIFT 	0x0UL
-#define DX_HOST_KFDE0_VALID_VALUE_BIT_SIZE 	0x1UL
-#define DX_HOST_KFDE1_VALID_REG_OFFSET 	0xA64UL 
-#define DX_HOST_KFDE1_VALID_VALUE_BIT_SHIFT 	0x0UL
-#define DX_HOST_KFDE1_VALID_VALUE_BIT_SIZE 	0x1UL
-#define DX_HOST_KFDE2_VALID_REG_OFFSET 	0xA68UL 
-#define DX_HOST_KFDE2_VALID_VALUE_BIT_SHIFT 	0x0UL
-#define DX_HOST_KFDE2_VALID_VALUE_BIT_SIZE 	0x1UL
-#define DX_HOST_KFDE3_VALID_REG_OFFSET 	0xA6CUL 
-#define DX_HOST_KFDE3_VALID_VALUE_BIT_SHIFT 	0x0UL
-#define DX_HOST_KFDE3_VALID_VALUE_BIT_SIZE 	0x1UL
-#define DX_HOST_GPR0_REG_OFFSET 	0xA70UL 
-#define DX_HOST_GPR0_VALUE_BIT_SHIFT 	0x0UL
-#define DX_HOST_GPR0_VALUE_BIT_SIZE 	0x20UL
-#define DX_GPR_HOST_REG_OFFSET 	0xA74UL 
-#define DX_GPR_HOST_VALUE_BIT_SHIFT 	0x0UL
-#define DX_GPR_HOST_VALUE_BIT_SIZE 	0x20UL
-#define DX_HOST_POWER_DOWN_EN_REG_OFFSET 	0xA78UL 
-#define DX_HOST_POWER_DOWN_EN_VALUE_BIT_SHIFT 	0x0UL
-#define DX_HOST_POWER_DOWN_EN_VALUE_BIT_SIZE 	0x1UL
+#define DX_HOST_IRR_REG_OFFSET	0xA00UL
+#define DX_HOST_IRR_DSCRPTR_COMPLETION_LOW_INT_BIT_SHIFT	0x2UL
+#define DX_HOST_IRR_DSCRPTR_COMPLETION_LOW_INT_BIT_SIZE	0x1UL
+#define DX_HOST_IRR_AXI_ERR_INT_BIT_SHIFT	0x8UL
+#define DX_HOST_IRR_AXI_ERR_INT_BIT_SIZE	0x1UL
+#define DX_HOST_IRR_GPR0_BIT_SHIFT	0xBUL
+#define DX_HOST_IRR_GPR0_BIT_SIZE	0x1UL
+#define DX_HOST_IRR_DSCRPTR_WATERMARK_INT_BIT_SHIFT	0x13UL
+#define DX_HOST_IRR_DSCRPTR_WATERMARK_INT_BIT_SIZE	0x1UL
+#define DX_HOST_IRR_AXIM_COMP_INT_BIT_SHIFT	0x17UL
+#define DX_HOST_IRR_AXIM_COMP_INT_BIT_SIZE	0x1UL
+#define DX_HOST_IMR_REG_OFFSET	0xA04UL
+#define DX_HOST_IMR_NOT_USED_MASK_BIT_SHIFT	0x1UL
+#define DX_HOST_IMR_NOT_USED_MASK_BIT_SIZE	0x1UL
+#define DX_HOST_IMR_DSCRPTR_COMPLETION_MASK_BIT_SHIFT	0x2UL
+#define DX_HOST_IMR_DSCRPTR_COMPLETION_MASK_BIT_SIZE	0x1UL
+#define DX_HOST_IMR_AXI_ERR_MASK_BIT_SHIFT	0x8UL
+#define DX_HOST_IMR_AXI_ERR_MASK_BIT_SIZE	0x1UL
+#define DX_HOST_IMR_GPR0_BIT_SHIFT	0xBUL
+#define DX_HOST_IMR_GPR0_BIT_SIZE	0x1UL
+#define DX_HOST_IMR_DSCRPTR_WATERMARK_MASK0_BIT_SHIFT	0x13UL
+#define DX_HOST_IMR_DSCRPTR_WATERMARK_MASK0_BIT_SIZE	0x1UL
+#define DX_HOST_IMR_AXIM_COMP_INT_MASK_BIT_SHIFT	0x17UL
+#define DX_HOST_IMR_AXIM_COMP_INT_MASK_BIT_SIZE	0x1UL
+#define DX_HOST_ICR_REG_OFFSET	0xA08UL
+#define DX_HOST_ICR_DSCRPTR_COMPLETION_BIT_SHIFT	0x2UL
+#define DX_HOST_ICR_DSCRPTR_COMPLETION_BIT_SIZE	0x1UL
+#define DX_HOST_ICR_AXI_ERR_CLEAR_BIT_SHIFT	0x8UL
+#define DX_HOST_ICR_AXI_ERR_CLEAR_BIT_SIZE	0x1UL
+#define DX_HOST_ICR_GPR_INT_CLEAR_BIT_SHIFT	0xBUL
+#define DX_HOST_ICR_GPR_INT_CLEAR_BIT_SIZE	0x1UL
+#define DX_HOST_ICR_DSCRPTR_WATERMARK_QUEUE0_CLEAR_BIT_SHIFT	0x13UL
+#define DX_HOST_ICR_DSCRPTR_WATERMARK_QUEUE0_CLEAR_BIT_SIZE	0x1UL
+#define DX_HOST_ICR_AXIM_COMP_INT_CLEAR_BIT_SHIFT	0x17UL
+#define DX_HOST_ICR_AXIM_COMP_INT_CLEAR_BIT_SIZE	0x1UL
+#define DX_HOST_SIGNATURE_REG_OFFSET	0xA24UL
+#define DX_HOST_SIGNATURE_VALUE_BIT_SHIFT	0x0UL
+#define DX_HOST_SIGNATURE_VALUE_BIT_SIZE	0x20UL
+#define DX_HOST_BOOT_REG_OFFSET	0xA28UL
+#define DX_HOST_BOOT_SYNTHESIS_CONFIG_BIT_SHIFT	0x0UL
+#define DX_HOST_BOOT_SYNTHESIS_CONFIG_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_LARGE_RKEK_LOCAL_BIT_SHIFT	0x1UL
+#define DX_HOST_BOOT_LARGE_RKEK_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_HASH_IN_FUSES_LOCAL_BIT_SHIFT	0x2UL
+#define DX_HOST_BOOT_HASH_IN_FUSES_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_EXT_MEM_SECURED_LOCAL_BIT_SHIFT	0x3UL
+#define DX_HOST_BOOT_EXT_MEM_SECURED_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_RKEK_ECC_EXISTS_LOCAL_N_BIT_SHIFT	0x5UL
+#define DX_HOST_BOOT_RKEK_ECC_EXISTS_LOCAL_N_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_SRAM_SIZE_LOCAL_BIT_SHIFT	0x6UL
+#define DX_HOST_BOOT_SRAM_SIZE_LOCAL_BIT_SIZE	0x3UL
+#define DX_HOST_BOOT_DSCRPTR_EXISTS_LOCAL_BIT_SHIFT	0x9UL
+#define DX_HOST_BOOT_DSCRPTR_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_PAU_EXISTS_LOCAL_BIT_SHIFT	0xAUL
+#define DX_HOST_BOOT_PAU_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_RNG_EXISTS_LOCAL_BIT_SHIFT	0xBUL
+#define DX_HOST_BOOT_RNG_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_PKA_EXISTS_LOCAL_BIT_SHIFT	0xCUL
+#define DX_HOST_BOOT_PKA_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_RC4_EXISTS_LOCAL_BIT_SHIFT	0xDUL
+#define DX_HOST_BOOT_RC4_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_SHA_512_PRSNT_LOCAL_BIT_SHIFT	0xEUL
+#define DX_HOST_BOOT_SHA_512_PRSNT_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_SHA_256_PRSNT_LOCAL_BIT_SHIFT	0xFUL
+#define DX_HOST_BOOT_SHA_256_PRSNT_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_MD5_PRSNT_LOCAL_BIT_SHIFT	0x10UL
+#define DX_HOST_BOOT_MD5_PRSNT_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_HASH_EXISTS_LOCAL_BIT_SHIFT	0x11UL
+#define DX_HOST_BOOT_HASH_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_C2_EXISTS_LOCAL_BIT_SHIFT	0x12UL
+#define DX_HOST_BOOT_C2_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_DES_EXISTS_LOCAL_BIT_SHIFT	0x13UL
+#define DX_HOST_BOOT_DES_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_AES_XCBC_MAC_EXISTS_LOCAL_BIT_SHIFT	0x14UL
+#define DX_HOST_BOOT_AES_XCBC_MAC_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_AES_CMAC_EXISTS_LOCAL_BIT_SHIFT	0x15UL
+#define DX_HOST_BOOT_AES_CMAC_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_AES_CCM_EXISTS_LOCAL_BIT_SHIFT	0x16UL
+#define DX_HOST_BOOT_AES_CCM_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_AES_XEX_HW_T_CALC_LOCAL_BIT_SHIFT	0x17UL
+#define DX_HOST_BOOT_AES_XEX_HW_T_CALC_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_AES_XEX_EXISTS_LOCAL_BIT_SHIFT	0x18UL
+#define DX_HOST_BOOT_AES_XEX_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_CTR_EXISTS_LOCAL_BIT_SHIFT	0x19UL
+#define DX_HOST_BOOT_CTR_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_AES_DIN_BYTE_RESOLUTION_LOCAL_BIT_SHIFT	0x1AUL
+#define DX_HOST_BOOT_AES_DIN_BYTE_RESOLUTION_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_TUNNELING_ENB_LOCAL_BIT_SHIFT	0x1BUL
+#define DX_HOST_BOOT_TUNNELING_ENB_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_SUPPORT_256_192_KEY_LOCAL_BIT_SHIFT	0x1CUL
+#define DX_HOST_BOOT_SUPPORT_256_192_KEY_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_ONLY_ENCRYPT_LOCAL_BIT_SHIFT	0x1DUL
+#define DX_HOST_BOOT_ONLY_ENCRYPT_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_BOOT_AES_EXISTS_LOCAL_BIT_SHIFT	0x1EUL
+#define DX_HOST_BOOT_AES_EXISTS_LOCAL_BIT_SIZE	0x1UL
+#define DX_HOST_VERSION_REG_OFFSET	0xA40UL
+#define DX_HOST_VERSION_VALUE_BIT_SHIFT	0x0UL
+#define DX_HOST_VERSION_VALUE_BIT_SIZE	0x20UL
+#define DX_HOST_KFDE0_VALID_REG_OFFSET	0xA60UL
+#define DX_HOST_KFDE0_VALID_VALUE_BIT_SHIFT	0x0UL
+#define DX_HOST_KFDE0_VALID_VALUE_BIT_SIZE	0x1UL
+#define DX_HOST_KFDE1_VALID_REG_OFFSET	0xA64UL
+#define DX_HOST_KFDE1_VALID_VALUE_BIT_SHIFT	0x0UL
+#define DX_HOST_KFDE1_VALID_VALUE_BIT_SIZE	0x1UL
+#define DX_HOST_KFDE2_VALID_REG_OFFSET	0xA68UL
+#define DX_HOST_KFDE2_VALID_VALUE_BIT_SHIFT	0x0UL
+#define DX_HOST_KFDE2_VALID_VALUE_BIT_SIZE	0x1UL
+#define DX_HOST_KFDE3_VALID_REG_OFFSET	0xA6CUL
+#define DX_HOST_KFDE3_VALID_VALUE_BIT_SHIFT	0x0UL
+#define DX_HOST_KFDE3_VALID_VALUE_BIT_SIZE	0x1UL
+#define DX_HOST_GPR0_REG_OFFSET	0xA70UL
+#define DX_HOST_GPR0_VALUE_BIT_SHIFT	0x0UL
+#define DX_HOST_GPR0_VALUE_BIT_SIZE	0x20UL
+#define DX_GPR_HOST_REG_OFFSET	0xA74UL
+#define DX_GPR_HOST_VALUE_BIT_SHIFT	0x0UL
+#define DX_GPR_HOST_VALUE_BIT_SIZE	0x20UL
+#define DX_HOST_POWER_DOWN_EN_REG_OFFSET	0xA78UL
+#define DX_HOST_POWER_DOWN_EN_VALUE_BIT_SHIFT	0x0UL
+#define DX_HOST_POWER_DOWN_EN_VALUE_BIT_SIZE	0x1UL
 // --------------------------------------
 // BLOCK: HOST_SRAM
 // --------------------------------------
-#define DX_SRAM_DATA_REG_OFFSET 	0xF00UL 
-#define DX_SRAM_DATA_VALUE_BIT_SHIFT 	0x0UL
-#define DX_SRAM_DATA_VALUE_BIT_SIZE 	0x20UL
-#define DX_SRAM_ADDR_REG_OFFSET 	0xF04UL 
-#define DX_SRAM_ADDR_VALUE_BIT_SHIFT 	0x0UL
-#define DX_SRAM_ADDR_VALUE_BIT_SIZE 	0xFUL
-#define DX_SRAM_DATA_READY_REG_OFFSET 	0xF08UL 
-#define DX_SRAM_DATA_READY_VALUE_BIT_SHIFT 	0x0UL
-#define DX_SRAM_DATA_READY_VALUE_BIT_SIZE 	0x1UL
+#define DX_SRAM_DATA_REG_OFFSET	0xF00UL
+#define DX_SRAM_DATA_VALUE_BIT_SHIFT	0x0UL
+#define DX_SRAM_DATA_VALUE_BIT_SIZE	0x20UL
+#define DX_SRAM_ADDR_REG_OFFSET	0xF04UL
+#define DX_SRAM_ADDR_VALUE_BIT_SHIFT	0x0UL
+#define DX_SRAM_ADDR_VALUE_BIT_SIZE	0xFUL
+#define DX_SRAM_DATA_READY_REG_OFFSET	0xF08UL
+#define DX_SRAM_DATA_READY_VALUE_BIT_SHIFT	0x0UL
+#define DX_SRAM_DATA_READY_VALUE_BIT_SIZE	0x1UL
 
 #endif //__DX_HOST_H__
diff --git a/drivers/staging/ccree/dx_reg_base_host.h b/drivers/staging/ccree/dx_reg_base_host.h
index 58dafe05fbeb..47bbadbcd1df 100644
--- a/drivers/staging/ccree/dx_reg_base_host.h
+++ b/drivers/staging/ccree/dx_reg_base_host.h
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -17,16 +17,7 @@
 #ifndef __DX_REG_BASE_HOST_H__
 #define __DX_REG_BASE_HOST_H__
 
-/* Identify platform: Xilinx Zynq7000 ZC706 */
-#define DX_PLAT_ZYNQ7000 1
-#define DX_PLAT_ZYNQ7000_ZC706 1
-
 #define DX_BASE_CC 0x80000000
-
-#define DX_BASE_ENV_REGS 0x40008000
-#define DX_BASE_ENV_CC_MEMORIES 0x40008000
-#define DX_BASE_ENV_PERF_RAM 0x40009000
-
 #define DX_BASE_HOST_RGF 0x0UL
 #define DX_BASE_CRY_KERNEL     0x0UL
 #define DX_BASE_ROM     0x40000000
diff --git a/drivers/staging/ccree/dx_reg_common.h b/drivers/staging/ccree/dx_reg_common.h
index 4ffed386521c..d5132ffaf6e6 100644
--- a/drivers/staging/ccree/dx_reg_common.h
+++ b/drivers/staging/ccree/dx_reg_common.h
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -19,7 +19,7 @@
 
 #define DX_DEV_SIGNATURE 0xDCC71200UL
 
-#define CC_HW_VERSION 0xef840015UL 
+#define CC_HW_VERSION 0xef840015UL
 
 #define DX_DEV_SHA_MAX 512
 
diff --git a/drivers/staging/ccree/hash_defs.h b/drivers/staging/ccree/hash_defs.h
index 5ab0861fd1bb..f52656f5a3ea 100644
--- a/drivers/staging/ccree/hash_defs.h
+++ b/drivers/staging/ccree/hash_defs.h
@@ -1,78 +1,36 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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  _HASH_DEFS_H__
-#define  _HASH_DEFS_H__
+#ifndef _HASH_DEFS_H_
+#define _HASH_DEFS_H_
 
 #include "cc_crypto_ctx.h"
 
-/* this files provides definitions required for hash engine drivers */
-#ifndef CC_CONFIG_HASH_SHA_512_SUPPORTED
-#define SEP_HASH_LENGTH_WORDS		2
-#else
-#define SEP_HASH_LENGTH_WORDS		4
-#endif
-
-#ifdef BIG__ENDIAN
-#define OPAD_CURRENT_LENGTH 0x40000000, 0x00000000 , 0x00000000, 0x00000000
-#define HASH_LARVAL_MD5  0x76543210, 0xFEDCBA98, 0x89ABCDEF, 0x01234567
-#define HASH_LARVAL_SHA1 0xF0E1D2C3, 0x76543210, 0xFEDCBA98, 0x89ABCDEF, 0x01234567
-#define HASH_LARVAL_SHA224 0XA44FFABE, 0XA78FF964, 0X11155868, 0X310BC0FF, 0X39590EF7, 0X17DD7030, 0X07D57C36, 0XD89E05C1
-#define HASH_LARVAL_SHA256 0X19CDE05B, 0XABD9831F, 0X8C68059B, 0X7F520E51, 0X3AF54FA5, 0X72F36E3C, 0X85AE67BB, 0X67E6096A
-#define HASH_LARVAL_SHA384 0X1D48B547, 0XA44FFABE, 0X0D2E0CDB, 0XA78FF964, 0X874AB48E, 0X11155868, 0X67263367, 0X310BC0FF, 0XD8EC2F15, 0X39590EF7, 0X5A015991, 0X17DD7030, 0X2A299A62, 0X07D57C36, 0X5D9DBBCB, 0XD89E05C1
-#define HASH_LARVAL_SHA512 0X19CDE05B, 0X79217E13, 0XABD9831F, 0X6BBD41FB, 0X8C68059B, 0X1F6C3E2B, 0X7F520E51, 0XD182E6AD, 0X3AF54FA5, 0XF1361D5F, 0X72F36E3C, 0X2BF894FE, 0X85AE67BB, 0X3BA7CA84, 0X67E6096A, 0X08C9BCF3
-#else
-#define OPAD_CURRENT_LENGTH 0x00000040, 0x00000000, 0x00000000, 0x00000000
-#define HASH_LARVAL_MD5  0x10325476, 0x98BADCFE, 0xEFCDAB89, 0x67452301
-#define HASH_LARVAL_SHA1 0xC3D2E1F0, 0x10325476, 0x98BADCFE, 0xEFCDAB89, 0x67452301
-#define HASH_LARVAL_SHA224 0xbefa4fa4, 0x64f98fa7, 0x68581511, 0xffc00b31, 0xf70e5939, 0x3070dd17, 0x367cd507, 0xc1059ed8
-#define HASH_LARVAL_SHA256 0x5be0cd19, 0x1f83d9ab, 0x9b05688c, 0x510e527f, 0xa54ff53a, 0x3c6ef372, 0xbb67ae85, 0x6a09e667
-#define HASH_LARVAL_SHA384 0X47B5481D, 0XBEFA4FA4, 0XDB0C2E0D, 0X64F98FA7, 0X8EB44A87, 0X68581511, 0X67332667, 0XFFC00B31, 0X152FECD8, 0XF70E5939, 0X9159015A, 0X3070DD17, 0X629A292A, 0X367CD507, 0XCBBB9D5D, 0XC1059ED8
-#define HASH_LARVAL_SHA512 0x5be0cd19, 0x137e2179, 0x1f83d9ab, 0xfb41bd6b, 0x9b05688c, 0x2b3e6c1f, 0x510e527f, 0xade682d1, 0xa54ff53a, 0x5f1d36f1, 0x3c6ef372, 0xfe94f82b, 0xbb67ae85, 0x84caa73b, 0x6a09e667, 0xf3bcc908
-#endif
-
-enum HashConfig1Padding {
+enum cc_hash_conf_pad {
 	HASH_PADDING_DISABLED = 0,
 	HASH_PADDING_ENABLED = 1,
 	HASH_DIGEST_RESULT_LITTLE_ENDIAN = 2,
-	HASH_CONFIG1_PADDING_RESERVE32 = INT32_MAX,
+	HASH_CONFIG1_PADDING_RESERVE32 = S32_MAX,
 };
 
-enum HashCipherDoPadding {
+enum cc_hash_cipher_pad {
 	DO_NOT_PAD = 0,
 	DO_PAD = 1,
-	HASH_CIPHER_DO_PADDING_RESERVE32 = INT32_MAX,
+	HASH_CIPHER_DO_PADDING_RESERVE32 = S32_MAX,
 };
 
-typedef struct SepHashPrivateContext {
-	/* The current length is placed at the end of the context buffer because the hash 
-	   context is used for all HMAC operations as well. HMAC context includes a 64 bytes 
-	   K0 field.  The size of struct drv_ctx_hash reserved field is  88/184 bytes depend if t
-	   he SHA512 is supported ( in this case teh context size is 256 bytes).
-	   The size of struct drv_ctx_hash reseved field is 20 or 52 depend if the SHA512 is supported.
-	   This means that this structure size (without the reserved field can be up to 20 bytes ,
-	   in case sha512 is not suppported it is 20 bytes (SEP_HASH_LENGTH_WORDS define to 2 ) and in the other
-	   case it is 28 (SEP_HASH_LENGTH_WORDS define to 4) */
-	uint32_t reserved[(sizeof(struct drv_ctx_hash)/sizeof(uint32_t)) - SEP_HASH_LENGTH_WORDS - 3];
-	uint32_t CurrentDigestedLength[SEP_HASH_LENGTH_WORDS];
-	uint32_t KeyType;
-	uint32_t dataCompleted;
-	uint32_t hmacFinalization;
-	/* no space left */
-} SepHashPrivateContext_s;
-
-#endif /*_HASH_DEFS_H__*/
+#endif /*_HASH_DEFS_H_*/
 
diff --git a/drivers/staging/ccree/hw_queue_defs_plat.h b/drivers/staging/ccree/hw_queue_defs_plat.h
deleted file mode 100644
index aee02cc7588a..000000000000
--- a/drivers/staging/ccree/hw_queue_defs_plat.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
- * This program is free software; you can 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 __HW_QUEUE_DEFS_PLAT_H__
-#define __HW_QUEUE_DEFS_PLAT_H__
-
-
-/*****************************/
-/* Descriptor packing macros */
-/*****************************/
-
-#define HW_QUEUE_FREE_SLOTS_GET() (CC_HAL_READ_REGISTER(CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_CONTENT)) & HW_QUEUE_SLOTS_MAX)
-
-#define HW_QUEUE_POLL_QUEUE_UNTIL_FREE_SLOTS(seqLen)						\
-	do {											\
-	} while (HW_QUEUE_FREE_SLOTS_GET() < (seqLen))
-
-#define HW_DESC_PUSH_TO_QUEUE(pDesc) do {        				  \
-	LOG_HW_DESC(pDesc);							  \
-	HW_DESC_DUMP(pDesc);							  \
-	CC_HAL_WRITE_REGISTER(GET_HW_Q_DESC_WORD_IDX(0), (pDesc)->word[0]); \
-	CC_HAL_WRITE_REGISTER(GET_HW_Q_DESC_WORD_IDX(1), (pDesc)->word[1]); \
-	CC_HAL_WRITE_REGISTER(GET_HW_Q_DESC_WORD_IDX(2), (pDesc)->word[2]); \
-	CC_HAL_WRITE_REGISTER(GET_HW_Q_DESC_WORD_IDX(3), (pDesc)->word[3]); \
-	CC_HAL_WRITE_REGISTER(GET_HW_Q_DESC_WORD_IDX(4), (pDesc)->word[4]); \
-	wmb();									   \
-	CC_HAL_WRITE_REGISTER(GET_HW_Q_DESC_WORD_IDX(5), (pDesc)->word[5]); \
-} while (0)
-
-#endif /*__HW_QUEUE_DEFS_PLAT_H__*/
diff --git a/drivers/staging/ccree/ssi_aead.c b/drivers/staging/ccree/ssi_aead.c
index 038291773b59..1fc0b05ea0d5 100644
--- a/drivers/staging/ccree/ssi_aead.c
+++ b/drivers/staging/ccree/ssi_aead.c
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -49,9 +49,8 @@
 #define AES_CCM_RFC4309_NONCE_SIZE 3
 #define MAX_NONCE_SIZE CTR_RFC3686_NONCE_SIZE
 
-
 /* Value of each ICV_CMP byte (of 8) in case of success */
-#define ICV_VERIF_OK 0x01	
+#define ICV_VERIF_OK 0x01
 
 struct ssi_aead_handle {
 	ssi_sram_addr_t sram_workspace_addr;
@@ -60,18 +59,18 @@ struct ssi_aead_handle {
 
 struct ssi_aead_ctx {
 	struct ssi_drvdata *drvdata;
-	uint8_t ctr_nonce[MAX_NONCE_SIZE]; /* used for ctr3686 iv and aes ccm */
-	uint8_t *enckey;
+	u8 ctr_nonce[MAX_NONCE_SIZE]; /* used for ctr3686 iv and aes ccm */
+	u8 *enckey;
 	dma_addr_t enckey_dma_addr;
 	union {
 		struct {
-			uint8_t *padded_authkey;
-			uint8_t *ipad_opad; /* IPAD, OPAD*/
+			u8 *padded_authkey;
+			u8 *ipad_opad; /* IPAD, OPAD*/
 			dma_addr_t padded_authkey_dma_addr;
 			dma_addr_t ipad_opad_dma_addr;
 		} hmac;
 		struct {
-			uint8_t *xcbc_keys; /* K1,K2,K3 */
+			u8 *xcbc_keys; /* K1,K2,K3 */
 			dma_addr_t xcbc_keys_dma_addr;
 		} xcbc;
 	} auth_state;
@@ -79,7 +78,7 @@ struct ssi_aead_ctx {
 	unsigned int auth_keylen;
 	unsigned int authsize; /* Actual (reduced?) size of the MAC/ICv */
 	enum drv_cipher_mode cipher_mode;
-	enum FlowMode flow_mode;
+	enum cc_flow_mode flow_mode;
 	enum drv_hash_mode auth_mode;
 };
 
@@ -96,23 +95,20 @@ static void ssi_aead_exit(struct crypto_aead *tfm)
 	SSI_LOG_DEBUG("Clearing context @%p for %s\n",
 		crypto_aead_ctx(tfm), crypto_tfm_alg_name(&(tfm->base)));
 
- 	dev = &ctx->drvdata->plat_dev->dev;
+	dev = &ctx->drvdata->plat_dev->dev;
 	/* Unmap enckey buffer */
-	if (ctx->enckey != NULL) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->enckey_dma_addr);
+	if (ctx->enckey) {
 		dma_free_coherent(dev, AES_MAX_KEY_SIZE, ctx->enckey, ctx->enckey_dma_addr);
 		SSI_LOG_DEBUG("Freed enckey DMA buffer enckey_dma_addr=0x%llX\n",
 			(unsigned long long)ctx->enckey_dma_addr);
 		ctx->enckey_dma_addr = 0;
 		ctx->enckey = NULL;
 	}
-	
+
 	if (ctx->auth_mode == DRV_HASH_XCBC_MAC) { /* XCBC authetication */
-		if (ctx->auth_state.xcbc.xcbc_keys != NULL) {
-			SSI_RESTORE_DMA_ADDR_TO_48BIT(
-				ctx->auth_state.xcbc.xcbc_keys_dma_addr);
+		if (ctx->auth_state.xcbc.xcbc_keys) {
 			dma_free_coherent(dev, CC_AES_128_BIT_KEY_SIZE * 3,
-				ctx->auth_state.xcbc.xcbc_keys, 
+				ctx->auth_state.xcbc.xcbc_keys,
 				ctx->auth_state.xcbc.xcbc_keys_dma_addr);
 		}
 		SSI_LOG_DEBUG("Freed xcbc_keys DMA buffer xcbc_keys_dma_addr=0x%llX\n",
@@ -120,9 +116,7 @@ static void ssi_aead_exit(struct crypto_aead *tfm)
 		ctx->auth_state.xcbc.xcbc_keys_dma_addr = 0;
 		ctx->auth_state.xcbc.xcbc_keys = NULL;
 	} else if (ctx->auth_mode != DRV_HASH_NULL) { /* HMAC auth. */
-		if (ctx->auth_state.hmac.ipad_opad != NULL) {
-			SSI_RESTORE_DMA_ADDR_TO_48BIT(
-				ctx->auth_state.hmac.ipad_opad_dma_addr);
+		if (ctx->auth_state.hmac.ipad_opad) {
 			dma_free_coherent(dev, 2 * MAX_HMAC_DIGEST_SIZE,
 				ctx->auth_state.hmac.ipad_opad,
 				ctx->auth_state.hmac.ipad_opad_dma_addr);
@@ -131,9 +125,7 @@ static void ssi_aead_exit(struct crypto_aead *tfm)
 			ctx->auth_state.hmac.ipad_opad_dma_addr = 0;
 			ctx->auth_state.hmac.ipad_opad = NULL;
 		}
-		if (ctx->auth_state.hmac.padded_authkey != NULL) {
-			SSI_RESTORE_DMA_ADDR_TO_48BIT(
-				ctx->auth_state.hmac.padded_authkey_dma_addr);
+		if (ctx->auth_state.hmac.padded_authkey) {
 			dma_free_coherent(dev, MAX_HMAC_BLOCK_SIZE,
 				ctx->auth_state.hmac.padded_authkey,
 				ctx->auth_state.hmac.padded_authkey_dma_addr);
@@ -162,16 +154,15 @@ static int ssi_aead_init(struct crypto_aead *tfm)
 	ctx->auth_mode = ssi_alg->auth_mode;
 	ctx->drvdata = ssi_alg->drvdata;
 	dev = &ctx->drvdata->plat_dev->dev;
-	crypto_aead_set_reqsize(tfm,sizeof(struct aead_req_ctx));
+	crypto_aead_set_reqsize(tfm, sizeof(struct aead_req_ctx));
 
 	/* Allocate key buffer, cache line aligned */
 	ctx->enckey = dma_alloc_coherent(dev, AES_MAX_KEY_SIZE,
 		&ctx->enckey_dma_addr, GFP_KERNEL);
-	if (ctx->enckey == NULL) {
+	if (!ctx->enckey) {
 		SSI_LOG_ERR("Failed allocating key buffer\n");
 		goto init_failed;
 	}
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->enckey_dma_addr, AES_MAX_KEY_SIZE);
 	SSI_LOG_DEBUG("Allocated enckey buffer in context ctx->enckey=@%p\n", ctx->enckey);
 
 	/* Set default authlen value */
@@ -182,38 +173,29 @@ static int ssi_aead_init(struct crypto_aead *tfm)
 		ctx->auth_state.xcbc.xcbc_keys = dma_alloc_coherent(dev,
 			CC_AES_128_BIT_KEY_SIZE * 3,
 			&ctx->auth_state.xcbc.xcbc_keys_dma_addr, GFP_KERNEL);
-		if (ctx->auth_state.xcbc.xcbc_keys == NULL) {
+		if (!ctx->auth_state.xcbc.xcbc_keys) {
 			SSI_LOG_ERR("Failed allocating buffer for XCBC keys\n");
 			goto init_failed;
 		}
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(
-			ctx->auth_state.xcbc.xcbc_keys_dma_addr,
-			CC_AES_128_BIT_KEY_SIZE * 3);
 	} else if (ctx->auth_mode != DRV_HASH_NULL) { /* HMAC authentication */
 		/* Allocate dma-coherent buffer for IPAD + OPAD */
 		ctx->auth_state.hmac.ipad_opad = dma_alloc_coherent(dev,
 			2 * MAX_HMAC_DIGEST_SIZE,
 			&ctx->auth_state.hmac.ipad_opad_dma_addr, GFP_KERNEL);
-		if (ctx->auth_state.hmac.ipad_opad == NULL) {
+		if (!ctx->auth_state.hmac.ipad_opad) {
 			SSI_LOG_ERR("Failed allocating IPAD/OPAD buffer\n");
 			goto init_failed;
 		}
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(
-			ctx->auth_state.hmac.ipad_opad_dma_addr,
-			2 * MAX_HMAC_DIGEST_SIZE);
 		SSI_LOG_DEBUG("Allocated authkey buffer in context ctx->authkey=@%p\n",
 			ctx->auth_state.hmac.ipad_opad);
-	
+
 		ctx->auth_state.hmac.padded_authkey = dma_alloc_coherent(dev,
 			MAX_HMAC_BLOCK_SIZE,
 			&ctx->auth_state.hmac.padded_authkey_dma_addr, GFP_KERNEL);
-		if (ctx->auth_state.hmac.padded_authkey == NULL) {
+		if (!ctx->auth_state.hmac.padded_authkey) {
 			SSI_LOG_ERR("failed to allocate padded_authkey\n");
 			goto init_failed;
-		}	
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(
-			ctx->auth_state.hmac.padded_authkey_dma_addr,
-			MAX_HMAC_BLOCK_SIZE);
+		}
 	} else {
 		ctx->auth_state.hmac.ipad_opad = NULL;
 		ctx->auth_state.hmac.padded_authkey = NULL;
@@ -225,7 +207,6 @@ init_failed:
 	ssi_aead_exit(tfm);
 	return -ENOMEM;
 }
- 
 
 static void ssi_aead_complete(struct device *dev, void *ssi_req, void __iomem *cc_base)
 {
@@ -234,9 +215,6 @@ static void ssi_aead_complete(struct device *dev, void *ssi_req, void __iomem *c
 	struct crypto_aead *tfm = crypto_aead_reqtfm(ssi_req);
 	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
 	int err = 0;
-	DECL_CYCLE_COUNT_RESOURCES;
-
-	START_CYCLE_COUNT();
 
 	ssi_buffer_mgr_unmap_aead_request(dev, areq);
 
@@ -250,72 +228,75 @@ static void ssi_aead_complete(struct device *dev, void *ssi_req, void __iomem *c
 				"(auth-size=%d, cipher=%d).\n",
 				ctx->authsize, ctx->cipher_mode);
 			/* In case of payload authentication failure, MUST NOT
-			   revealed the decrypted message --> zero its memory. */
+			 * revealed the decrypted message --> zero its memory.
+			 */
 			ssi_buffer_mgr_zero_sgl(areq->dst, areq_ctx->cryptlen);
 			err = -EBADMSG;
 		}
 	} else { /*ENCRYPT*/
-		if (unlikely(areq_ctx->is_icv_fragmented == true))
+		if (unlikely(areq_ctx->is_icv_fragmented))
 			ssi_buffer_mgr_copy_scatterlist_portion(
-				areq_ctx->mac_buf, areq_ctx->dstSgl, areq->cryptlen+areq_ctx->dstOffset,
-				areq->cryptlen+areq_ctx->dstOffset + ctx->authsize, SSI_SG_FROM_BUF);
+				areq_ctx->mac_buf, areq_ctx->dstSgl, areq->cryptlen + areq_ctx->dstOffset,
+				areq->cryptlen + areq_ctx->dstOffset + ctx->authsize, SSI_SG_FROM_BUF);
 
 		/* If an IV was generated, copy it back to the user provided buffer. */
-		if (areq_ctx->backup_giv != NULL) {
-			if (ctx->cipher_mode == DRV_CIPHER_CTR) {
+		if (areq_ctx->backup_giv) {
+			if (ctx->cipher_mode == DRV_CIPHER_CTR)
 				memcpy(areq_ctx->backup_giv, areq_ctx->ctr_iv + CTR_RFC3686_NONCE_SIZE, CTR_RFC3686_IV_SIZE);
-			} else if (ctx->cipher_mode == DRV_CIPHER_CCM) {
+			else if (ctx->cipher_mode == DRV_CIPHER_CCM)
 				memcpy(areq_ctx->backup_giv, areq_ctx->ctr_iv + CCM_BLOCK_IV_OFFSET, CCM_BLOCK_IV_SIZE);
-			}
 		}
 	}
 
-	END_CYCLE_COUNT(STAT_OP_TYPE_GENERIC, STAT_PHASE_4);
 	aead_request_complete(areq, err);
 }
 
-static int xcbc_setkey(HwDesc_s *desc, struct ssi_aead_ctx *ctx)
+static int xcbc_setkey(struct cc_hw_desc *desc, struct ssi_aead_ctx *ctx)
 {
 	/* Load the AES key */
-	HW_DESC_INIT(&desc[0]);
+	hw_desc_init(&desc[0]);
 	/* We are using for the source/user key the same buffer as for the output keys,
-	   because after this key loading it is not needed anymore */
-	HW_DESC_SET_DIN_TYPE(&desc[0], DMA_DLLI, ctx->auth_state.xcbc.xcbc_keys_dma_addr, ctx->auth_keylen, NS_BIT);
-	HW_DESC_SET_CIPHER_MODE(&desc[0], DRV_CIPHER_ECB);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[0], DRV_CRYPTO_DIRECTION_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[0], ctx->auth_keylen);
-	HW_DESC_SET_FLOW_MODE(&desc[0], S_DIN_to_AES);
-	HW_DESC_SET_SETUP_MODE(&desc[0], SETUP_LOAD_KEY0);
-
-	HW_DESC_INIT(&desc[1]);
-	HW_DESC_SET_DIN_CONST(&desc[1], 0x01010101, CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[1], DIN_AES_DOUT);
-	HW_DESC_SET_DOUT_DLLI(&desc[1], ctx->auth_state.xcbc.xcbc_keys_dma_addr, AES_KEYSIZE_128, NS_BIT, 0);
-
-	HW_DESC_INIT(&desc[2]);
-	HW_DESC_SET_DIN_CONST(&desc[2], 0x02020202, CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[2], DIN_AES_DOUT);
-	HW_DESC_SET_DOUT_DLLI(&desc[2], (ctx->auth_state.xcbc.xcbc_keys_dma_addr
+	 * because after this key loading it is not needed anymore
+	 */
+	set_din_type(&desc[0], DMA_DLLI,
+		     ctx->auth_state.xcbc.xcbc_keys_dma_addr, ctx->auth_keylen,
+		     NS_BIT);
+	set_cipher_mode(&desc[0], DRV_CIPHER_ECB);
+	set_cipher_config0(&desc[0], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	set_key_size_aes(&desc[0], ctx->auth_keylen);
+	set_flow_mode(&desc[0], S_DIN_to_AES);
+	set_setup_mode(&desc[0], SETUP_LOAD_KEY0);
+
+	hw_desc_init(&desc[1]);
+	set_din_const(&desc[1], 0x01010101, CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[1], DIN_AES_DOUT);
+	set_dout_dlli(&desc[1], ctx->auth_state.xcbc.xcbc_keys_dma_addr,
+		      AES_KEYSIZE_128, NS_BIT, 0);
+
+	hw_desc_init(&desc[2]);
+	set_din_const(&desc[2], 0x02020202, CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[2], DIN_AES_DOUT);
+	set_dout_dlli(&desc[2], (ctx->auth_state.xcbc.xcbc_keys_dma_addr
 					 + AES_KEYSIZE_128),
 			      AES_KEYSIZE_128, NS_BIT, 0);
 
-	HW_DESC_INIT(&desc[3]);
-	HW_DESC_SET_DIN_CONST(&desc[3], 0x03030303, CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[3], DIN_AES_DOUT);
-	HW_DESC_SET_DOUT_DLLI(&desc[3], (ctx->auth_state.xcbc.xcbc_keys_dma_addr
+	hw_desc_init(&desc[3]);
+	set_din_const(&desc[3], 0x03030303, CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[3], DIN_AES_DOUT);
+	set_dout_dlli(&desc[3], (ctx->auth_state.xcbc.xcbc_keys_dma_addr
 					  + 2 * AES_KEYSIZE_128),
 			      AES_KEYSIZE_128, NS_BIT, 0);
 
 	return 4;
 }
 
-static int hmac_setkey(HwDesc_s *desc, struct ssi_aead_ctx *ctx)
+static int hmac_setkey(struct cc_hw_desc *desc, struct ssi_aead_ctx *ctx)
 {
 	unsigned int hmacPadConst[2] = { HMAC_IPAD_CONST, HMAC_OPAD_CONST };
 	unsigned int digest_ofs = 0;
-	unsigned int hash_mode = (ctx->auth_mode == DRV_HASH_SHA1) ? 
+	unsigned int hash_mode = (ctx->auth_mode == DRV_HASH_SHA1) ?
 			DRV_HASH_HW_SHA1 : DRV_HASH_HW_SHA256;
-	unsigned int digest_size = (ctx->auth_mode == DRV_HASH_SHA1) ? 
+	unsigned int digest_size = (ctx->auth_mode == DRV_HASH_SHA1) ?
 			CC_SHA1_DIGEST_SIZE : CC_SHA256_DIGEST_SIZE;
 
 	int idx = 0;
@@ -324,52 +305,51 @@ static int hmac_setkey(HwDesc_s *desc, struct ssi_aead_ctx *ctx)
 	/* calc derived HMAC key */
 	for (i = 0; i < 2; i++) {
 		/* Load hash initial state */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
-		HW_DESC_SET_DIN_SRAM(&desc[idx],
-			ssi_ahash_get_larval_digest_sram_addr(
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], hash_mode);
+		set_din_sram(&desc[idx],
+			     ssi_ahash_get_larval_digest_sram_addr(
 				ctx->drvdata, ctx->auth_mode),
-			digest_size);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+			     digest_size);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 		idx++;
 
 		/* Load the hash current length*/
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
-		HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], hash_mode);
+		set_din_const(&desc[idx], 0, HASH_LEN_SIZE);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 		idx++;
 
 		/* Prepare ipad key */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_XOR_VAL(&desc[idx], hmacPadConst[i]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+		hw_desc_init(&desc[idx]);
+		set_xor_val(&desc[idx], hmacPadConst[i]);
+		set_cipher_mode(&desc[idx], hash_mode);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
 		idx++;
 
 		/* Perform HASH update */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-				   ctx->auth_state.hmac.padded_authkey_dma_addr,
-				     SHA256_BLOCK_SIZE, NS_BIT);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
-		HW_DESC_SET_XOR_ACTIVE(&desc[idx]);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI,
+			     ctx->auth_state.hmac.padded_authkey_dma_addr,
+			     SHA256_BLOCK_SIZE, NS_BIT);
+		set_cipher_mode(&desc[idx], hash_mode);
+		set_xor_active(&desc[idx]);
+		set_flow_mode(&desc[idx], DIN_HASH);
 		idx++;
 
 		/* Get the digset */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
-		HW_DESC_SET_DOUT_DLLI(&desc[idx], 
-				      (ctx->auth_state.hmac.ipad_opad_dma_addr +
-				       digest_ofs),
-				      digest_size, NS_BIT, 0);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-		HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], hash_mode);
+		set_dout_dlli(&desc[idx],
+			      (ctx->auth_state.hmac.ipad_opad_dma_addr +
+			       digest_ofs), digest_size, NS_BIT, 0);
+		set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+		set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+		set_cipher_config1(&desc[idx], HASH_PADDING_DISABLED);
 		idx++;
 
 		digest_ofs += digest_size;
@@ -420,15 +400,17 @@ static int validate_keys_sizes(struct ssi_aead_ctx *ctx)
 
 	return 0; /* All tests of keys sizes passed */
 }
-/*This function prepers the user key so it can pass to the hmac processing 
-  (copy to intenral buffer or hash in case of key longer than block */
+
+/* This function prepers the user key so it can pass to the hmac processing
+ * (copy to intenral buffer or hash in case of key longer than block
+ */
 static int
 ssi_get_plain_hmac_key(struct crypto_aead *tfm, const u8 *key, unsigned int keylen)
 {
 	dma_addr_t key_dma_addr = 0;
 	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
 	struct device *dev = &ctx->drvdata->plat_dev->dev;
-	uint32_t larval_addr = ssi_ahash_get_larval_digest_sram_addr(
+	u32 larval_addr = ssi_ahash_get_larval_digest_sram_addr(
 					ctx->drvdata, ctx->auth_mode);
 	struct ssi_crypto_req ssi_req = {};
 	unsigned int blocksize;
@@ -436,8 +418,8 @@ ssi_get_plain_hmac_key(struct crypto_aead *tfm, const u8 *key, unsigned int keyl
 	unsigned int hashmode;
 	unsigned int idx = 0;
 	int rc = 0;
-	HwDesc_s desc[MAX_AEAD_SETKEY_SEQ];
-	dma_addr_t padded_authkey_dma_addr = 
+	struct cc_hw_desc desc[MAX_AEAD_SETKEY_SEQ];
+	dma_addr_t padded_authkey_dma_addr =
 		ctx->auth_state.hmac.padded_authkey_dma_addr;
 
 	switch (ctx->auth_mode) { /* auth_key required and >0 */
@@ -460,107 +442,89 @@ ssi_get_plain_hmac_key(struct crypto_aead *tfm, const u8 *key, unsigned int keyl
 				   " DMA failed\n", key, keylen);
 			return -ENOMEM;
 		}
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(key_dma_addr, keylen);
 		if (keylen > blocksize) {
 			/* Load hash initial state */
-			HW_DESC_INIT(&desc[idx]);
-			HW_DESC_SET_CIPHER_MODE(&desc[idx], hashmode);
-			HW_DESC_SET_DIN_SRAM(&desc[idx], larval_addr, digestsize);
-			HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+			hw_desc_init(&desc[idx]);
+			set_cipher_mode(&desc[idx], hashmode);
+			set_din_sram(&desc[idx], larval_addr, digestsize);
+			set_flow_mode(&desc[idx], S_DIN_to_HASH);
+			set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 			idx++;
-	
+
 			/* Load the hash current length*/
-			HW_DESC_INIT(&desc[idx]);
-			HW_DESC_SET_CIPHER_MODE(&desc[idx], hashmode);
-			HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
-			HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
-			HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+			hw_desc_init(&desc[idx]);
+			set_cipher_mode(&desc[idx], hashmode);
+			set_din_const(&desc[idx], 0, HASH_LEN_SIZE);
+			set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+			set_flow_mode(&desc[idx], S_DIN_to_HASH);
+			set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 			idx++;
-	
-			HW_DESC_INIT(&desc[idx]);
-			HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-					     key_dma_addr, 
-					     keylen, NS_BIT);
-			HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+
+			hw_desc_init(&desc[idx]);
+			set_din_type(&desc[idx], DMA_DLLI,
+				     key_dma_addr, keylen, NS_BIT);
+			set_flow_mode(&desc[idx], DIN_HASH);
 			idx++;
-	
+
 			/* Get hashed key */
-			HW_DESC_INIT(&desc[idx]);
-			HW_DESC_SET_CIPHER_MODE(&desc[idx], hashmode); 
-			HW_DESC_SET_DOUT_DLLI(&desc[idx],
-					 padded_authkey_dma_addr,
-					 digestsize,
-					 NS_BIT, 0);
-			HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-			HW_DESC_SET_CIPHER_CONFIG1(&desc[idx],
-							HASH_PADDING_DISABLED);
-			HW_DESC_SET_CIPHER_CONFIG0(&desc[idx],
-						   HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+			hw_desc_init(&desc[idx]);
+			set_cipher_mode(&desc[idx], hashmode);
+			set_dout_dlli(&desc[idx], padded_authkey_dma_addr,
+				      digestsize, NS_BIT, 0);
+			set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+			set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+			set_cipher_config1(&desc[idx], HASH_PADDING_DISABLED);
+			set_cipher_config0(&desc[idx],
+					   HASH_DIGEST_RESULT_LITTLE_ENDIAN);
 			idx++;
-	
-			HW_DESC_INIT(&desc[idx]);
-			HW_DESC_SET_DIN_CONST(&desc[idx], 0, (blocksize - digestsize));
-			HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
-			HW_DESC_SET_DOUT_DLLI(&desc[idx], 
-					      (padded_authkey_dma_addr + digestsize),
-					      (blocksize - digestsize),
-					      NS_BIT, 0);
+
+			hw_desc_init(&desc[idx]);
+			set_din_const(&desc[idx], 0, (blocksize - digestsize));
+			set_flow_mode(&desc[idx], BYPASS);
+			set_dout_dlli(&desc[idx], (padded_authkey_dma_addr +
+				      digestsize), (blocksize - digestsize),
+				      NS_BIT, 0);
 			idx++;
 		} else {
-			HW_DESC_INIT(&desc[idx]);
-			HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-					     key_dma_addr, 
-					     keylen, NS_BIT);
-			HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
-			HW_DESC_SET_DOUT_DLLI(&desc[idx], 
-					      (padded_authkey_dma_addr),
-					      keylen, NS_BIT, 0);
+			hw_desc_init(&desc[idx]);
+			set_din_type(&desc[idx], DMA_DLLI, key_dma_addr,
+				     keylen, NS_BIT);
+			set_flow_mode(&desc[idx], BYPASS);
+			set_dout_dlli(&desc[idx], padded_authkey_dma_addr,
+				      keylen, NS_BIT, 0);
 			idx++;
-	
+
 			if ((blocksize - keylen) != 0) {
-				HW_DESC_INIT(&desc[idx]);
-				HW_DESC_SET_DIN_CONST(&desc[idx], 0,
-						      (blocksize - keylen));
-				HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
-				HW_DESC_SET_DOUT_DLLI(&desc[idx], 
-					(padded_authkey_dma_addr + keylen),
-					(blocksize - keylen),
-					NS_BIT, 0);
+				hw_desc_init(&desc[idx]);
+				set_din_const(&desc[idx], 0,
+					      (blocksize - keylen));
+				set_flow_mode(&desc[idx], BYPASS);
+				set_dout_dlli(&desc[idx],
+					      (padded_authkey_dma_addr +
+					       keylen),
+					      (blocksize - keylen), NS_BIT, 0);
 				idx++;
 			}
 		}
 	} else {
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_CONST(&desc[idx], 0,
-				      (blocksize - keylen));
-		HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
-		HW_DESC_SET_DOUT_DLLI(&desc[idx], 
-			padded_authkey_dma_addr,
-			blocksize,
-			NS_BIT, 0);
+		hw_desc_init(&desc[idx]);
+		set_din_const(&desc[idx], 0, (blocksize - keylen));
+		set_flow_mode(&desc[idx], BYPASS);
+		set_dout_dlli(&desc[idx], padded_authkey_dma_addr,
+			      blocksize, NS_BIT, 0);
 		idx++;
 	}
 
-#ifdef ENABLE_CYCLE_COUNT
-	ssi_req.op_type = STAT_OP_TYPE_SETKEY;
-#endif
-
 	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 0);
 	if (unlikely(rc != 0))
 		SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
 
-	if (likely(key_dma_addr != 0)) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(key_dma_addr);
+	if (likely(key_dma_addr != 0))
 		dma_unmap_single(dev, key_dma_addr, keylen, DMA_TO_DEVICE);
-	}
 
 	return rc;
 }
 
-
 static int
 ssi_aead_setkey(struct crypto_aead *tfm, const u8 *key, unsigned int keylen)
 {
@@ -568,16 +532,14 @@ ssi_aead_setkey(struct crypto_aead *tfm, const u8 *key, unsigned int keylen)
 	struct rtattr *rta = (struct rtattr *)key;
 	struct ssi_crypto_req ssi_req = {};
 	struct crypto_authenc_key_param *param;
-	HwDesc_s desc[MAX_AEAD_SETKEY_SEQ];
+	struct cc_hw_desc desc[MAX_AEAD_SETKEY_SEQ];
 	int seq_len = 0, rc = -EINVAL;
-	DECL_CYCLE_COUNT_RESOURCES;
 
 	SSI_LOG_DEBUG("Setting key in context @%p for %s. key=%p keylen=%u\n",
 		ctx, crypto_tfm_alg_name(crypto_aead_tfm(tfm)), key, keylen);
 
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
 	/* STAT_PHASE_0: Init and sanity checks */
-	START_CYCLE_COUNT();
 
 	if (ctx->auth_mode != DRV_HASH_NULL) { /* authenc() alg. */
 		if (!RTA_OK(rta, keylen))
@@ -600,7 +562,8 @@ ssi_aead_setkey(struct crypto_aead *tfm, const u8 *key, unsigned int keylen)
 			    (AES_MIN_KEY_SIZE + CTR_RFC3686_NONCE_SIZE))
 				goto badkey;
 			/* Copy nonce from last 4 bytes in CTR key to
-			*  first 4 bytes in CTR IV */
+			 *  first 4 bytes in CTR IV
+			 */
 			memcpy(ctx->ctr_nonce, key + ctx->auth_keylen + ctx->enc_keylen -
 				CTR_RFC3686_NONCE_SIZE, CTR_RFC3686_NONCE_SIZE);
 			/* Set CTR key size */
@@ -615,9 +578,7 @@ ssi_aead_setkey(struct crypto_aead *tfm, const u8 *key, unsigned int keylen)
 	if (unlikely(rc != 0))
 		goto badkey;
 
-	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_0);
 	/* STAT_PHASE_1: Copy key to ctx */
-	START_CYCLE_COUNT();
 
 	/* Get key material */
 	memcpy(ctx->enckey, key + ctx->auth_keylen, ctx->enc_keylen);
@@ -631,10 +592,7 @@ ssi_aead_setkey(struct crypto_aead *tfm, const u8 *key, unsigned int keylen)
 			goto badkey;
 	}
 
-	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_1);
-	
 	/* STAT_PHASE_2: Create sequence */
-	START_CYCLE_COUNT();
 
 	switch (ctx->auth_mode) {
 	case DRV_HASH_SHA1:
@@ -652,15 +610,9 @@ ssi_aead_setkey(struct crypto_aead *tfm, const u8 *key, unsigned int keylen)
 		goto badkey;
 	}
 
-	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_2);
-
 	/* STAT_PHASE_3: Submit sequence to HW */
-	START_CYCLE_COUNT();
-	
+
 	if (seq_len > 0) { /* For CCM there is no sequence to setup the key */
-#ifdef ENABLE_CYCLE_COUNT
-		ssi_req.op_type = STAT_OP_TYPE_SETKEY;
-#endif
 		rc = send_request(ctx->drvdata, &ssi_req, desc, seq_len, 0);
 		if (unlikely(rc != 0)) {
 			SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
@@ -669,7 +621,6 @@ ssi_aead_setkey(struct crypto_aead *tfm, const u8 *key, unsigned int keylen)
 	}
 
 	/* Update STAT_PHASE_3 */
-	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_3);
 	return rc;
 
 badkey:
@@ -684,7 +635,7 @@ static int ssi_rfc4309_ccm_setkey(struct crypto_aead *tfm, const u8 *key, unsign
 {
 	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
 	int rc = 0;
-	
+
 	if (keylen < 3)
 		return -EINVAL;
 
@@ -702,11 +653,11 @@ static int ssi_aead_setauthsize(
 	unsigned int authsize)
 {
 	struct ssi_aead_ctx *ctx = crypto_aead_ctx(authenc);
-	
+
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
 	/* Unsupported auth. sizes */
 	if ((authsize == 0) ||
-	    (authsize >crypto_aead_maxauthsize(authenc))) {
+	    (authsize > crypto_aead_maxauthsize(authenc))) {
 		return -ENOTSUPP;
 	}
 
@@ -752,11 +703,11 @@ static int ssi_ccm_setauthsize(struct crypto_aead *authenc,
 }
 #endif /*SSI_CC_HAS_AES_CCM*/
 
-static inline void 
+static inline void
 ssi_aead_create_assoc_desc(
-	struct aead_request *areq, 
+	struct aead_request *areq,
 	unsigned int flow_mode,
-	HwDesc_s desc[], 
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct crypto_aead *tfm = crypto_aead_reqtfm(areq);
@@ -768,24 +719,23 @@ ssi_aead_create_assoc_desc(
 	switch (assoc_dma_type) {
 	case SSI_DMA_BUF_DLLI:
 		SSI_LOG_DEBUG("ASSOC buffer type DLLI\n");
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-			sg_dma_address(areq->src),
-			areq->assoclen, NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
-		if (ctx->auth_mode == DRV_HASH_XCBC_MAC && (areq_ctx->cryptlen > 0) )
-			HW_DESC_SET_DIN_NOT_LAST_INDICATION(&desc[idx]);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI, sg_dma_address(areq->src),
+			     areq->assoclen, NS_BIT); set_flow_mode(&desc[idx],
+			     flow_mode);
+		if ((ctx->auth_mode == DRV_HASH_XCBC_MAC) &&
+		    (areq_ctx->cryptlen > 0))
+			set_din_not_last_indication(&desc[idx]);
 		break;
 	case SSI_DMA_BUF_MLLI:
 		SSI_LOG_DEBUG("ASSOC buffer type MLLI\n");
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_MLLI,
-				     areq_ctx->assoc.sram_addr,
-				     areq_ctx->assoc.mlli_nents,
-				     NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
-		if (ctx->auth_mode == DRV_HASH_XCBC_MAC && (areq_ctx->cryptlen > 0) )
-			HW_DESC_SET_DIN_NOT_LAST_INDICATION(&desc[idx]);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_MLLI, areq_ctx->assoc.sram_addr,
+			     areq_ctx->assoc.mlli_nents, NS_BIT);
+		set_flow_mode(&desc[idx], flow_mode);
+		if ((ctx->auth_mode == DRV_HASH_XCBC_MAC) &&
+		    (areq_ctx->cryptlen > 0))
+			set_din_not_last_indication(&desc[idx]);
 		break;
 	case SSI_DMA_BUF_NULL:
 	default:
@@ -797,9 +747,9 @@ ssi_aead_create_assoc_desc(
 
 static inline void
 ssi_aead_process_authenc_data_desc(
-	struct aead_request *areq, 
+	struct aead_request *areq,
 	unsigned int flow_mode,
-	HwDesc_s desc[], 
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size,
 	int direct)
 {
@@ -814,27 +764,28 @@ ssi_aead_process_authenc_data_desc(
 			(direct == DRV_CRYPTO_DIRECTION_ENCRYPT) ?
 			areq_ctx->dstSgl : areq_ctx->srcSgl;
 
-		unsigned int offset = 
+		unsigned int offset =
 			(direct == DRV_CRYPTO_DIRECTION_ENCRYPT) ?
 			areq_ctx->dstOffset : areq_ctx->srcOffset;
 		SSI_LOG_DEBUG("AUTHENC: SRC/DST buffer type DLLI\n");
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			(sg_dma_address(cipher)+ offset), areq_ctx->cryptlen,
-			NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI,
+			     (sg_dma_address(cipher) + offset),
+			     areq_ctx->cryptlen, NS_BIT);
+		set_flow_mode(&desc[idx], flow_mode);
 		break;
 	}
 	case SSI_DMA_BUF_MLLI:
 	{
 		/* DOUBLE-PASS flow (as default)
 		 * assoc. + iv + data -compact in one table
-		 * if assoclen is ZERO only IV perform */
+		 * if assoclen is ZERO only IV perform
+		 */
 		ssi_sram_addr_t mlli_addr = areq_ctx->assoc.sram_addr;
-		uint32_t mlli_nents = areq_ctx->assoc.mlli_nents;
+		u32 mlli_nents = areq_ctx->assoc.mlli_nents;
 
-		if (likely(areq_ctx->is_single_pass == true)) {
-			if (direct == DRV_CRYPTO_DIRECTION_ENCRYPT){
+		if (likely(areq_ctx->is_single_pass)) {
+			if (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) {
 				mlli_addr = areq_ctx->dst.sram_addr;
 				mlli_nents = areq_ctx->dst.mlli_nents;
 			} else {
@@ -844,10 +795,10 @@ ssi_aead_process_authenc_data_desc(
 		}
 
 		SSI_LOG_DEBUG("AUTHENC: SRC/DST buffer type MLLI\n");
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_MLLI,
-			mlli_addr, mlli_nents, NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_MLLI, mlli_addr, mlli_nents,
+			     NS_BIT);
+		set_flow_mode(&desc[idx], flow_mode);
 		break;
 	}
 	case SSI_DMA_BUF_NULL:
@@ -860,9 +811,9 @@ ssi_aead_process_authenc_data_desc(
 
 static inline void
 ssi_aead_process_cipher_data_desc(
-	struct aead_request *areq, 
+	struct aead_request *areq,
 	unsigned int flow_mode,
-	HwDesc_s desc[], 
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	unsigned int idx = *seq_size;
@@ -875,25 +826,24 @@ ssi_aead_process_cipher_data_desc(
 	switch (data_dma_type) {
 	case SSI_DMA_BUF_DLLI:
 		SSI_LOG_DEBUG("CIPHER: SRC/DST buffer type DLLI\n");
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			(sg_dma_address(areq_ctx->srcSgl)+areq_ctx->srcOffset),
-			areq_ctx->cryptlen, NS_BIT);
-		HW_DESC_SET_DOUT_DLLI(&desc[idx],
-			(sg_dma_address(areq_ctx->dstSgl)+areq_ctx->dstOffset),
-			areq_ctx->cryptlen, NS_BIT, 0);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI,
+			     (sg_dma_address(areq_ctx->srcSgl) +
+			      areq_ctx->srcOffset), areq_ctx->cryptlen, NS_BIT);
+		set_dout_dlli(&desc[idx],
+			      (sg_dma_address(areq_ctx->dstSgl) +
+			       areq_ctx->dstOffset),
+			      areq_ctx->cryptlen, NS_BIT, 0);
+		set_flow_mode(&desc[idx], flow_mode);
 		break;
 	case SSI_DMA_BUF_MLLI:
 		SSI_LOG_DEBUG("CIPHER: SRC/DST buffer type MLLI\n");
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_MLLI,
-			areq_ctx->src.sram_addr,
-			areq_ctx->src.mlli_nents, NS_BIT);
-		HW_DESC_SET_DOUT_MLLI(&desc[idx],
-			areq_ctx->dst.sram_addr,
-			areq_ctx->dst.mlli_nents, NS_BIT, 0);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_MLLI, areq_ctx->src.sram_addr,
+			     areq_ctx->src.mlli_nents, NS_BIT);
+		set_dout_mlli(&desc[idx], areq_ctx->dst.sram_addr,
+			      areq_ctx->dst.mlli_nents, NS_BIT, 0);
+		set_flow_mode(&desc[idx], flow_mode);
 		break;
 	case SSI_DMA_BUF_NULL:
 	default:
@@ -905,7 +855,7 @@ ssi_aead_process_cipher_data_desc(
 
 static inline void ssi_aead_process_digest_result_desc(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
@@ -918,35 +868,36 @@ static inline void ssi_aead_process_digest_result_desc(
 
 	/* Get final ICV result */
 	if (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) {
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-		HW_DESC_SET_DOUT_DLLI(&desc[idx], req_ctx->icv_dma_addr,
-			ctx->authsize, NS_BIT, 1);
-		HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
+		hw_desc_init(&desc[idx]);
+		set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+		set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+		set_dout_dlli(&desc[idx], req_ctx->icv_dma_addr, ctx->authsize,
+			      NS_BIT, 1);
+		set_queue_last_ind(&desc[idx]);
 		if (ctx->auth_mode == DRV_HASH_XCBC_MAC) {
-			HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
-			HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC); 
+			set_aes_not_hash_mode(&desc[idx]);
+			set_cipher_mode(&desc[idx], DRV_CIPHER_XCBC_MAC);
 		} else {
-			HW_DESC_SET_CIPHER_CONFIG0(&desc[idx],
-				HASH_DIGEST_RESULT_LITTLE_ENDIAN);
-			HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+			set_cipher_config0(&desc[idx],
+					   HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+			set_cipher_mode(&desc[idx], hash_mode);
 		}
 	} else { /*Decrypt*/
 		/* Get ICV out from hardware */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-		HW_DESC_SET_DOUT_DLLI(&desc[idx], req_ctx->mac_buf_dma_addr,
-			ctx->authsize, NS_BIT, 1);
-		HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
-		HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+		hw_desc_init(&desc[idx]);
+		set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+		set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+		set_dout_dlli(&desc[idx], req_ctx->mac_buf_dma_addr,
+			      ctx->authsize, NS_BIT, 1);
+		set_queue_last_ind(&desc[idx]);
+		set_cipher_config0(&desc[idx],
+				   HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+		set_cipher_config1(&desc[idx], HASH_PADDING_DISABLED);
 		if (ctx->auth_mode == DRV_HASH_XCBC_MAC) {
-			HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
-			HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+			set_cipher_mode(&desc[idx], DRV_CIPHER_XCBC_MAC);
+			set_aes_not_hash_mode(&desc[idx]);
 		} else {
-			HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+			set_cipher_mode(&desc[idx], hash_mode);
 		}
 	}
 
@@ -955,7 +906,7 @@ static inline void ssi_aead_process_digest_result_desc(
 
 static inline void ssi_aead_setup_cipher_desc(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
@@ -966,35 +917,34 @@ static inline void ssi_aead_setup_cipher_desc(
 	int direct = req_ctx->gen_ctx.op_type;
 
 	/* Setup cipher state */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direct);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], ctx->flow_mode);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-		req_ctx->gen_ctx.iv_dma_addr, hw_iv_size, NS_BIT);
-	if (ctx->cipher_mode == DRV_CIPHER_CTR) {
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
-	} else {
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
-	}
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->cipher_mode);
+	hw_desc_init(&desc[idx]);
+	set_cipher_config0(&desc[idx], direct);
+	set_flow_mode(&desc[idx], ctx->flow_mode);
+	set_din_type(&desc[idx], DMA_DLLI, req_ctx->gen_ctx.iv_dma_addr,
+		     hw_iv_size, NS_BIT);
+	if (ctx->cipher_mode == DRV_CIPHER_CTR)
+		set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
+	else
+		set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
+	set_cipher_mode(&desc[idx], ctx->cipher_mode);
 	idx++;
 
 	/* Setup enc. key */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direct);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], ctx->flow_mode);
+	hw_desc_init(&desc[idx]);
+	set_cipher_config0(&desc[idx], direct);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
+	set_flow_mode(&desc[idx], ctx->flow_mode);
 	if (ctx->flow_mode == S_DIN_to_AES) {
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr, 
-			((ctx->enc_keylen == 24) ?
-			 CC_AES_KEY_SIZE_MAX : ctx->enc_keylen), NS_BIT);
-		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
+		set_din_type(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr,
+			     ((ctx->enc_keylen == 24) ? CC_AES_KEY_SIZE_MAX :
+			      ctx->enc_keylen), NS_BIT);
+		set_key_size_aes(&desc[idx], ctx->enc_keylen);
 	} else {
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr,
-			ctx->enc_keylen, NS_BIT);
-		HW_DESC_SET_KEY_SIZE_DES(&desc[idx], ctx->enc_keylen);
+		set_din_type(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr,
+			     ctx->enc_keylen, NS_BIT);
+		set_key_size_des(&desc[idx], ctx->enc_keylen);
 	}
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->cipher_mode);
+	set_cipher_mode(&desc[idx], ctx->cipher_mode);
 	idx++;
 
 	*seq_size = idx;
@@ -1002,7 +952,7 @@ static inline void ssi_aead_setup_cipher_desc(
 
 static inline void ssi_aead_process_cipher(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size,
 	unsigned int data_flow_mode)
 {
@@ -1017,9 +967,9 @@ static inline void ssi_aead_process_cipher(
 	ssi_aead_process_cipher_data_desc(req, data_flow_mode, desc, &idx);
 	if (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) {
 		/* We must wait for DMA to write all cipher */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-		HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+		hw_desc_init(&desc[idx]);
+		set_din_no_dma(&desc[idx], 0, 0xfffff0);
+		set_dout_no_dma(&desc[idx], 0, 0, 1);
 		idx++;
 	}
 
@@ -1028,35 +978,36 @@ static inline void ssi_aead_process_cipher(
 
 static inline void ssi_aead_hmac_setup_digest_desc(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
 	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
 	unsigned int hash_mode = (ctx->auth_mode == DRV_HASH_SHA1) ?
 				DRV_HASH_HW_SHA1 : DRV_HASH_HW_SHA256;
-	unsigned int digest_size = (ctx->auth_mode == DRV_HASH_SHA1) ? 
+	unsigned int digest_size = (ctx->auth_mode == DRV_HASH_SHA1) ?
 				CC_SHA1_DIGEST_SIZE : CC_SHA256_DIGEST_SIZE;
 	unsigned int idx = *seq_size;
 
 	/* Loading hash ipad xor key state */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-		ctx->auth_state.hmac.ipad_opad_dma_addr,
-		digest_size, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], hash_mode);
+	set_din_type(&desc[idx], DMA_DLLI,
+		     ctx->auth_state.hmac.ipad_opad_dma_addr, digest_size,
+		     NS_BIT);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 	idx++;
 
 	/* Load init. digest len (64 bytes) */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
-	HW_DESC_SET_DIN_SRAM(&desc[idx],
-		ssi_ahash_get_initial_digest_len_sram_addr(ctx->drvdata, hash_mode),
-		HASH_LEN_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], hash_mode);
+	set_din_sram(&desc[idx],
+		     ssi_ahash_get_initial_digest_len_sram_addr(ctx->drvdata,
+								hash_mode),
+								HASH_LEN_SIZE);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 	idx++;
 
 	*seq_size = idx;
@@ -1064,7 +1015,7 @@ static inline void ssi_aead_hmac_setup_digest_desc(
 
 static inline void ssi_aead_xcbc_setup_digest_desc(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
@@ -1072,55 +1023,53 @@ static inline void ssi_aead_xcbc_setup_digest_desc(
 	unsigned int idx = *seq_size;
 
 	/* Loading MAC state */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_CONST(&desc[idx], 0, CC_AES_BLOCK_SIZE);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	hw_desc_init(&desc[idx]);
+	set_din_const(&desc[idx], 0, CC_AES_BLOCK_SIZE);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_key_size_aes(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
 	idx++;
 
 	/* Setup XCBC MAC K1 */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			     ctx->auth_state.xcbc.xcbc_keys_dma_addr,
-			     AES_KEYSIZE_128, NS_BIT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI,
+		     ctx->auth_state.xcbc.xcbc_keys_dma_addr,
+		     AES_KEYSIZE_128, NS_BIT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_key_size_aes(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
 	idx++;
 
 	/* Setup XCBC MAC K2 */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			     (ctx->auth_state.xcbc.xcbc_keys_dma_addr + 
-			      AES_KEYSIZE_128),
-			     AES_KEYSIZE_128, NS_BIT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI,
+		     (ctx->auth_state.xcbc.xcbc_keys_dma_addr +
+		      AES_KEYSIZE_128), AES_KEYSIZE_128, NS_BIT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_key_size_aes(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
 	idx++;
 
 	/* Setup XCBC MAC K3 */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			     (ctx->auth_state.xcbc.xcbc_keys_dma_addr +
-			      2 * AES_KEYSIZE_128),
-			     AES_KEYSIZE_128, NS_BIT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE2);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI,
+		     (ctx->auth_state.xcbc.xcbc_keys_dma_addr +
+		      2 * AES_KEYSIZE_128), AES_KEYSIZE_128, NS_BIT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE2);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_key_size_aes(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
 	idx++;
 
 	*seq_size = idx;
@@ -1128,7 +1077,7 @@ static inline void ssi_aead_xcbc_setup_digest_desc(
 
 static inline void ssi_aead_process_digest_header_desc(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	unsigned int idx = *seq_size;
@@ -1142,7 +1091,7 @@ static inline void ssi_aead_process_digest_header_desc(
 
 static inline void ssi_aead_process_digest_scheme_desc(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
@@ -1150,55 +1099,56 @@ static inline void ssi_aead_process_digest_scheme_desc(
 	struct ssi_aead_handle *aead_handle = ctx->drvdata->aead_handle;
 	unsigned int hash_mode = (ctx->auth_mode == DRV_HASH_SHA1) ?
 				DRV_HASH_HW_SHA1 : DRV_HASH_HW_SHA256;
-	unsigned int digest_size = (ctx->auth_mode == DRV_HASH_SHA1) ? 
+	unsigned int digest_size = (ctx->auth_mode == DRV_HASH_SHA1) ?
 				CC_SHA1_DIGEST_SIZE : CC_SHA256_DIGEST_SIZE;
 	unsigned int idx = *seq_size;
 
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
-	HW_DESC_SET_DOUT_SRAM(&desc[idx], aead_handle->sram_workspace_addr,
-			HASH_LEN_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE1);
-	HW_DESC_SET_CIPHER_DO(&desc[idx], DO_PAD);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], hash_mode);
+	set_dout_sram(&desc[idx], aead_handle->sram_workspace_addr,
+		      HASH_LEN_SIZE);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE1);
+	set_cipher_do(&desc[idx], DO_PAD);
 	idx++;
 
 	/* Get final ICV result */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DOUT_SRAM(&desc[idx], aead_handle->sram_workspace_addr,
-			digest_size);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+	hw_desc_init(&desc[idx]);
+	set_dout_sram(&desc[idx], aead_handle->sram_workspace_addr,
+		      digest_size);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	set_cipher_config0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+	set_cipher_mode(&desc[idx], hash_mode);
 	idx++;
 
 	/* Loading hash opad xor key state */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-		(ctx->auth_state.hmac.ipad_opad_dma_addr + digest_size),
-		digest_size, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], hash_mode);
+	set_din_type(&desc[idx], DMA_DLLI,
+		     (ctx->auth_state.hmac.ipad_opad_dma_addr + digest_size),
+		     digest_size, NS_BIT);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 	idx++;
 
 	/* Load init. digest len (64 bytes) */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
-	HW_DESC_SET_DIN_SRAM(&desc[idx],
-		ssi_ahash_get_initial_digest_len_sram_addr(ctx->drvdata, hash_mode),
-		HASH_LEN_SIZE);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], hash_mode);
+	set_din_sram(&desc[idx],
+		     ssi_ahash_get_initial_digest_len_sram_addr(ctx->drvdata,
+								hash_mode),
+		     HASH_LEN_SIZE);
+	set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 	idx++;
 
 	/* Perform HASH update */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_SRAM(&desc[idx], aead_handle->sram_workspace_addr,
-			digest_size);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	hw_desc_init(&desc[idx]);
+	set_din_sram(&desc[idx], aead_handle->sram_workspace_addr,
+		     digest_size);
+	set_flow_mode(&desc[idx], DIN_HASH);
 	idx++;
 
 	*seq_size = idx;
@@ -1206,7 +1156,7 @@ static inline void ssi_aead_process_digest_scheme_desc(
 
 static inline void ssi_aead_load_mlli_to_sram(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
@@ -1216,29 +1166,29 @@ static inline void ssi_aead_load_mlli_to_sram(
 	if (unlikely(
 		(req_ctx->assoc_buff_type == SSI_DMA_BUF_MLLI) ||
 		(req_ctx->data_buff_type == SSI_DMA_BUF_MLLI) ||
-		(req_ctx->is_single_pass == false))) {
+		!req_ctx->is_single_pass)) {
 		SSI_LOG_DEBUG("Copy-to-sram: mlli_dma=%08x, mlli_size=%u\n",
 			(unsigned int)ctx->drvdata->mlli_sram_addr,
 			req_ctx->mlli_params.mlli_len);
 		/* Copy MLLI table host-to-sram */
-		HW_DESC_INIT(&desc[*seq_size]);
-		HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
-			req_ctx->mlli_params.mlli_dma_addr,
-			req_ctx->mlli_params.mlli_len, NS_BIT);
-		HW_DESC_SET_DOUT_SRAM(&desc[*seq_size],
-			ctx->drvdata->mlli_sram_addr,
-			req_ctx->mlli_params.mlli_len);
-		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], BYPASS);
+		hw_desc_init(&desc[*seq_size]);
+		set_din_type(&desc[*seq_size], DMA_DLLI,
+			     req_ctx->mlli_params.mlli_dma_addr,
+			     req_ctx->mlli_params.mlli_len, NS_BIT);
+		set_dout_sram(&desc[*seq_size],
+			      ctx->drvdata->mlli_sram_addr,
+			      req_ctx->mlli_params.mlli_len);
+		set_flow_mode(&desc[*seq_size], BYPASS);
 		(*seq_size)++;
 	}
 }
 
-static inline enum FlowMode ssi_aead_get_data_flow_mode(
+static inline enum cc_flow_mode ssi_aead_get_data_flow_mode(
 	enum drv_crypto_direction direct,
-	enum FlowMode setup_flow_mode,
+	enum cc_flow_mode setup_flow_mode,
 	bool is_single_pass)
 {
-	enum FlowMode data_flow_mode;
+	enum cc_flow_mode data_flow_mode;
 
 	if (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) {
 		if (setup_flow_mode == S_DIN_to_AES)
@@ -1261,7 +1211,7 @@ static inline enum FlowMode ssi_aead_get_data_flow_mode(
 
 static inline void ssi_aead_hmac_authenc(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
@@ -1271,7 +1221,7 @@ static inline void ssi_aead_hmac_authenc(
 	unsigned int data_flow_mode = ssi_aead_get_data_flow_mode(
 		direct, ctx->flow_mode, req_ctx->is_single_pass);
 
-	if (req_ctx->is_single_pass == true) {
+	if (req_ctx->is_single_pass) {
 		/**
 		 * Single-pass flow
 		 */
@@ -1284,10 +1234,11 @@ static inline void ssi_aead_hmac_authenc(
 		return;
 	}
 
-	/** 
+	/**
 	 * Double-pass flow
-	 * Fallback for unsupported single-pass modes, 
-	 * i.e. using assoc. data of non-word-multiple */
+	 * Fallback for unsupported single-pass modes,
+	 * i.e. using assoc. data of non-word-multiple
+	 */
 	if (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) {
 		/* encrypt first.. */
 		ssi_aead_process_cipher(req, desc, seq_size, data_flow_mode);
@@ -1305,7 +1256,8 @@ static inline void ssi_aead_hmac_authenc(
 		/* decrypt after.. */
 		ssi_aead_process_cipher(req, desc, seq_size, data_flow_mode);
 		/* read the digest result with setting the completion bit
-		   must be after the cipher operation */
+		 * must be after the cipher operation
+		 */
 		ssi_aead_process_digest_result_desc(req, desc, seq_size);
 	}
 }
@@ -1313,7 +1265,7 @@ static inline void ssi_aead_hmac_authenc(
 static inline void
 ssi_aead_xcbc_authenc(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
@@ -1323,7 +1275,7 @@ ssi_aead_xcbc_authenc(
 	unsigned int data_flow_mode = ssi_aead_get_data_flow_mode(
 		direct, ctx->flow_mode, req_ctx->is_single_pass);
 
-	if (req_ctx->is_single_pass == true) {
+	if (req_ctx->is_single_pass) {
 		/**
 		 * Single-pass flow
 		 */
@@ -1335,10 +1287,11 @@ ssi_aead_xcbc_authenc(
 		return;
 	}
 
-	/** 
+	/**
 	 * Double-pass flow
-	 * Fallback for unsupported single-pass modes, 
-	 * i.e. using assoc. data of non-word-multiple */
+	 * Fallback for unsupported single-pass modes,
+	 * i.e. using assoc. data of non-word-multiple
+	 */
 	if (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) {
 		/* encrypt first.. */
 		ssi_aead_process_cipher(req, desc, seq_size, data_flow_mode);
@@ -1353,7 +1306,8 @@ ssi_aead_xcbc_authenc(
 		/* decrypt after..*/
 		ssi_aead_process_cipher(req, desc, seq_size, data_flow_mode);
 		/* read the digest result with setting the completion bit
-		   must be after the cipher operation */
+		 * must be after the cipher operation
+		 */
 		ssi_aead_process_digest_result_desc(req, desc, seq_size);
 	}
 }
@@ -1379,18 +1333,17 @@ static int validate_data_size(struct ssi_aead_ctx *ctx,
 			goto data_size_err;
 		if (ctx->cipher_mode == DRV_CIPHER_CCM)
 			break;
-		if (ctx->cipher_mode == DRV_CIPHER_GCTR)
-		{
-			if (areq_ctx->plaintext_authenticate_only == true)
-				areq_ctx->is_single_pass = false; 
+		if (ctx->cipher_mode == DRV_CIPHER_GCTR) {
+			if (areq_ctx->plaintext_authenticate_only)
+				areq_ctx->is_single_pass = false;
 			break;
 		}
 
-		if (!IS_ALIGNED(assoclen, sizeof(uint32_t)))
+		if (!IS_ALIGNED(assoclen, sizeof(u32)))
 			areq_ctx->is_single_pass = false;
 
 		if ((ctx->cipher_mode == DRV_CIPHER_CTR) &&
-		    !IS_ALIGNED(cipherlen, sizeof(uint32_t)))
+		    !IS_ALIGNED(cipherlen, sizeof(u32)))
 			areq_ctx->is_single_pass = false;
 
 		break;
@@ -1412,13 +1365,14 @@ data_size_err:
 }
 
 #if SSI_CC_HAS_AES_CCM
-static unsigned int format_ccm_a0(uint8_t *pA0Buff, uint32_t headerSize)
+static unsigned int format_ccm_a0(u8 *pA0Buff, u32 headerSize)
 {
 	unsigned int len = 0;
-	if ( headerSize == 0 ) {
+
+	if (headerSize == 0)
 		return 0;
-	} 
-	if ( headerSize < ((1UL << 16) - (1UL << 8) )) {
+
+	if (headerSize < ((1UL << 16) - (1UL << 8))) {
 		len = 2;
 
 		pA0Buff[0] = (headerSize >> 8) & 0xFF;
@@ -1457,7 +1411,7 @@ static int set_msg_len(u8 *block, unsigned int msglen, unsigned int csize)
 
 static inline int ssi_aead_ccm(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
@@ -1467,7 +1421,6 @@ static inline int ssi_aead_ccm(
 	unsigned int cipher_flow_mode;
 	dma_addr_t mac_result;
 
-
 	if (req_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_DECRYPT) {
 		cipher_flow_mode = AES_to_HASH_and_DOUT;
 		mac_result = req_ctx->mac_buf_dma_addr;
@@ -1477,118 +1430,111 @@ static inline int ssi_aead_ccm(
 	}
 
 	/* load key */
-	HW_DESC_INIT(&desc[idx]);	
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CTR);	
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr, 
-			((ctx->enc_keylen == 24) ? 
-			 CC_AES_KEY_SIZE_MAX : ctx->enc_keylen), 
-			 NS_BIT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CTR);
+	set_din_type(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr,
+		     ((ctx->enc_keylen == 24) ?  CC_AES_KEY_SIZE_MAX :
+		      ctx->enc_keylen), NS_BIT);
+	set_key_size_aes(&desc[idx], ctx->enc_keylen);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* load ctr state */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CTR);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			req_ctx->gen_ctx.iv_dma_addr, 
-			     AES_BLOCK_SIZE, NS_BIT);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);	
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CTR);
+	set_key_size_aes(&desc[idx], ctx->enc_keylen);
+	set_din_type(&desc[idx], DMA_DLLI,
+		     req_ctx->gen_ctx.iv_dma_addr, AES_BLOCK_SIZE, NS_BIT);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* load MAC key */
-	HW_DESC_INIT(&desc[idx]);	
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CBC_MAC);	
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr, 
-			((ctx->enc_keylen == 24) ? 
-			 CC_AES_KEY_SIZE_MAX : ctx->enc_keylen), 
-			 NS_BIT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CBC_MAC);
+	set_din_type(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr,
+		     ((ctx->enc_keylen == 24) ?  CC_AES_KEY_SIZE_MAX :
+		      ctx->enc_keylen), NS_BIT);
+	set_key_size_aes(&desc[idx], ctx->enc_keylen);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
 	idx++;
 
 	/* load MAC state */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CBC_MAC);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			req_ctx->mac_buf_dma_addr, 
-			     AES_BLOCK_SIZE, NS_BIT);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);	
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CBC_MAC);
+	set_key_size_aes(&desc[idx], ctx->enc_keylen);
+	set_din_type(&desc[idx], DMA_DLLI, req_ctx->mac_buf_dma_addr,
+		     AES_BLOCK_SIZE, NS_BIT);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
 	idx++;
 
-
 	/* process assoc data */
 	if (req->assoclen > 0) {
 		ssi_aead_create_assoc_desc(req, DIN_HASH, desc, &idx);
 	} else {
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-				      sg_dma_address(&req_ctx->ccm_adata_sg),
-				     AES_BLOCK_SIZE + req_ctx->ccm_hdr_size,
-				     NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI,
+			     sg_dma_address(&req_ctx->ccm_adata_sg),
+			     AES_BLOCK_SIZE + req_ctx->ccm_hdr_size, NS_BIT);
+		set_flow_mode(&desc[idx], DIN_HASH);
 		idx++;
 	}
 
 	/* process the cipher */
-	if (req_ctx->cryptlen != 0) {
+	if (req_ctx->cryptlen != 0)
 		ssi_aead_process_cipher_data_desc(req, cipher_flow_mode, desc, &idx);
-	}
 
 	/* Read temporal MAC */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CBC_MAC);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], req_ctx->mac_buf_dma_addr,
-			      ctx->authsize, NS_BIT, 0);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CBC_MAC);
+	set_dout_dlli(&desc[idx], req_ctx->mac_buf_dma_addr, ctx->authsize,
+		      NS_BIT, 0);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	set_cipher_config0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_aes_not_hash_mode(&desc[idx]);
 	idx++;
 
 	/* load AES-CTR state (for last MAC calculation)*/
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CTR);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			     req_ctx->ccm_iv0_dma_addr ,
-			     AES_BLOCK_SIZE, NS_BIT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CTR);
+	set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	set_din_type(&desc[idx], DMA_DLLI, req_ctx->ccm_iv0_dma_addr,
+		     AES_BLOCK_SIZE, NS_BIT);
+	set_key_size_aes(&desc[idx], ctx->enc_keylen);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	hw_desc_init(&desc[idx]);
+	set_din_no_dma(&desc[idx], 0, 0xfffff0);
+	set_dout_no_dma(&desc[idx], 0, 0, 1);
 	idx++;
 
 	/* encrypt the "T" value and store MAC in mac_state */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			req_ctx->mac_buf_dma_addr , ctx->authsize, NS_BIT);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], mac_result , ctx->authsize, NS_BIT, 1);
-	HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
-	idx++;	
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, req_ctx->mac_buf_dma_addr,
+		     ctx->authsize, NS_BIT);
+	set_dout_dlli(&desc[idx], mac_result, ctx->authsize, NS_BIT, 1);
+	set_queue_last_ind(&desc[idx]);
+	set_flow_mode(&desc[idx], DIN_AES_DOUT);
+	idx++;
 
 	*seq_size = idx;
 	return 0;
 }
 
-static int config_ccm_adata(struct aead_request *req) {
+static int config_ccm_adata(struct aead_request *req)
+{
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
 	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
 	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
@@ -1597,21 +1543,22 @@ static int config_ccm_adata(struct aead_request *req) {
 	/* Note: The code assume that req->iv[0] already contains the value of L' of RFC3610 */
 	unsigned int l = lp + 1;  /* This is L' of RFC 3610. */
 	unsigned int m = ctx->authsize;  /* This is M' of RFC 3610. */
-	uint8_t *b0 = req_ctx->ccm_config + CCM_B0_OFFSET;
-	uint8_t *a0 = req_ctx->ccm_config + CCM_A0_OFFSET;
-	uint8_t *ctr_count_0 = req_ctx->ccm_config + CCM_CTR_COUNT_0_OFFSET;
-	unsigned int cryptlen = (req_ctx->gen_ctx.op_type == 
-				 DRV_CRYPTO_DIRECTION_ENCRYPT) ? 
-				req->cryptlen : 
+	u8 *b0 = req_ctx->ccm_config + CCM_B0_OFFSET;
+	u8 *a0 = req_ctx->ccm_config + CCM_A0_OFFSET;
+	u8 *ctr_count_0 = req_ctx->ccm_config + CCM_CTR_COUNT_0_OFFSET;
+	unsigned int cryptlen = (req_ctx->gen_ctx.op_type ==
+				 DRV_CRYPTO_DIRECTION_ENCRYPT) ?
+				req->cryptlen :
 				(req->cryptlen - ctx->authsize);
 	int rc;
+
 	memset(req_ctx->mac_buf, 0, AES_BLOCK_SIZE);
-	memset(req_ctx->ccm_config, 0, AES_BLOCK_SIZE*3);
+	memset(req_ctx->ccm_config, 0, AES_BLOCK_SIZE * 3);
 
 	/* taken from crypto/ccm.c */
 	/* 2 <= L <= 8, so 1 <= L' <= 7. */
 	if (2 > l || l > 8) {
-		SSI_LOG_ERR("illegal iv value %X\n",req->iv[0]);
+		SSI_LOG_ERR("illegal iv value %X\n", req->iv[0]);
 		return -EINVAL;
 	}
 	memcpy(b0, req->iv, AES_BLOCK_SIZE);
@@ -1622,20 +1569,19 @@ static int config_ccm_adata(struct aead_request *req) {
 	*b0 |= (8 * ((m - 2) / 2));
 	if (req->assoclen > 0)
 		*b0 |= 64;  /* Enable bit 6 if Adata exists. */
-	
+
 	rc = set_msg_len(b0 + 16 - l, cryptlen, l);  /* Write L'. */
-	if (rc != 0) {
+	if (rc != 0)
 		return rc;
-	}
 	 /* END of "taken from crypto/ccm.c" */
-	
+
 	/* l(a) - size of associated data. */
-	req_ctx->ccm_hdr_size = format_ccm_a0 (a0, req->assoclen);
+	req_ctx->ccm_hdr_size = format_ccm_a0(a0, req->assoclen);
 
 	memset(req->iv + 15 - req->iv[0], 0, req->iv[0] + 1);
-	req->iv [15] = 1;
+	req->iv[15] = 1;
 
-	memcpy(ctr_count_0, req->iv, AES_BLOCK_SIZE) ;
+	memcpy(ctr_count_0, req->iv, AES_BLOCK_SIZE);
 	ctr_count_0[15] = 0;
 
 	return 0;
@@ -1654,7 +1600,7 @@ static void ssi_rfc4309_ccm_process(struct aead_request *req)
 	/* In RFC 4309 there is an 11-bytes nonce+IV part, that we build here. */
 	memcpy(areq_ctx->ctr_iv + CCM_BLOCK_NONCE_OFFSET, ctx->ctr_nonce, CCM_BLOCK_NONCE_SIZE);
 	memcpy(areq_ctx->ctr_iv + CCM_BLOCK_IV_OFFSET,    req->iv,        CCM_BLOCK_IV_SIZE);
-	req->iv = areq_ctx->ctr_iv;	
+	req->iv = areq_ctx->ctr_iv;
 	req->assoclen -= CCM_BLOCK_IV_SIZE;
 }
 #endif /*SSI_CC_HAS_AES_CCM*/
@@ -1663,7 +1609,7 @@ static void ssi_rfc4309_ccm_process(struct aead_request *req)
 
 static inline void ssi_aead_gcm_setup_ghash_desc(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
@@ -1672,69 +1618,68 @@ static inline void ssi_aead_gcm_setup_ghash_desc(
 	unsigned int idx = *seq_size;
 
 	/* load key to AES*/
-	HW_DESC_INIT(&desc[idx]);	
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_ECB);	
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr, 
-			ctx->enc_keylen, NS_BIT); 
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_ECB);
+	set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	set_din_type(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr,
+		     ctx->enc_keylen, NS_BIT);
+	set_key_size_aes(&desc[idx], ctx->enc_keylen);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* process one zero block to generate hkey */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_CONST(&desc[idx], 0x0, AES_BLOCK_SIZE);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx],
-				  req_ctx->hkey_dma_addr,
-				  AES_BLOCK_SIZE,
-				  NS_BIT, 0); 
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	hw_desc_init(&desc[idx]);
+	set_din_const(&desc[idx], 0x0, AES_BLOCK_SIZE);
+	set_dout_dlli(&desc[idx], req_ctx->hkey_dma_addr, AES_BLOCK_SIZE,
+		      NS_BIT, 0);
+	set_flow_mode(&desc[idx], DIN_AES_DOUT);
 	idx++;
 
 	/* Memory Barrier */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	hw_desc_init(&desc[idx]);
+	set_din_no_dma(&desc[idx], 0, 0xfffff0);
+	set_dout_no_dma(&desc[idx], 0, 0, 1);
 	idx++;
 
 	/* Load GHASH subkey */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			req_ctx->hkey_dma_addr, 
-				 AES_BLOCK_SIZE, NS_BIT);
-	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);	
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);	
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, req_ctx->hkey_dma_addr,
+		     AES_BLOCK_SIZE, NS_BIT);
+	set_dout_no_dma(&desc[idx], 0, 0, 1);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_HASH_HW_GHASH);
+	set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 	idx++;
 
 	/* Configure Hash Engine to work with GHASH.
-	   Since it was not possible to extend HASH submodes to add GHASH,
-	   The following command is necessary in order to select GHASH (according to HW designers)*/
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);	
-	HW_DESC_SET_CIPHER_DO(&desc[idx], 1); //1=AES_SK RKEK
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED); 
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	 * Since it was not possible to extend HASH submodes to add GHASH,
+	 * The following command is necessary in order to
+	 * select GHASH (according to HW designers)
+	 */
+	hw_desc_init(&desc[idx]);
+	set_din_no_dma(&desc[idx], 0, 0xfffff0);
+	set_dout_no_dma(&desc[idx], 0, 0, 1);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_HASH_HW_GHASH);
+	set_cipher_do(&desc[idx], 1); //1=AES_SK RKEK
+	set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 	idx++;
 
 	/* Load GHASH initial STATE (which is 0). (for any hash there is an initial state) */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_CONST(&desc[idx], 0x0, AES_BLOCK_SIZE);
-	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED); 
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	hw_desc_init(&desc[idx]);
+	set_din_const(&desc[idx], 0x0, AES_BLOCK_SIZE);
+	set_dout_no_dma(&desc[idx], 0, 0, 1);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_HASH_HW_GHASH);
+	set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 	idx++;
 
 	*seq_size = idx;
@@ -1742,7 +1687,7 @@ static inline void ssi_aead_gcm_setup_ghash_desc(
 
 static inline void ssi_aead_gcm_setup_gctr_desc(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
@@ -1751,27 +1696,27 @@ static inline void ssi_aead_gcm_setup_gctr_desc(
 	unsigned int idx = *seq_size;
 
 	/* load key to AES*/
-	HW_DESC_INIT(&desc[idx]);	
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);	
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr, 
-			ctx->enc_keylen, NS_BIT); 
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_GCTR);
+	set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	set_din_type(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr,
+		     ctx->enc_keylen, NS_BIT);
+	set_key_size_aes(&desc[idx], ctx->enc_keylen);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
-	if ((req_ctx->cryptlen != 0) && (req_ctx->plaintext_authenticate_only==false)){
+	if ((req_ctx->cryptlen != 0) && (!req_ctx->plaintext_authenticate_only)) {
 		/* load AES/CTR initial CTR value inc by 2*/
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);
-		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-				req_ctx->gcm_iv_inc2_dma_addr, 
-					 AES_BLOCK_SIZE, NS_BIT);
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);	
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], DRV_CIPHER_GCTR);
+		set_key_size_aes(&desc[idx], ctx->enc_keylen);
+		set_din_type(&desc[idx], DMA_DLLI,
+			     req_ctx->gcm_iv_inc2_dma_addr, AES_BLOCK_SIZE,
+			     NS_BIT);
+		set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+		set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
+		set_flow_mode(&desc[idx], S_DIN_to_AES);
 		idx++;
 	}
 
@@ -1780,13 +1725,13 @@ static inline void ssi_aead_gcm_setup_gctr_desc(
 
 static inline void ssi_aead_process_gcm_result_desc(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
 	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
 	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
-	dma_addr_t mac_result; 
+	dma_addr_t mac_result;
 	unsigned int idx = *seq_size;
 
 	if (req_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_DECRYPT) {
@@ -1796,60 +1741,57 @@ static inline void ssi_aead_process_gcm_result_desc(
 	}
 
 	/* process(ghash) gcm_block_len */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-		req_ctx->gcm_block_len_dma_addr,
-		AES_BLOCK_SIZE, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, req_ctx->gcm_block_len_dma_addr,
+		     AES_BLOCK_SIZE, NS_BIT);
+	set_flow_mode(&desc[idx], DIN_HASH);
 	idx++;
 
 	/* Store GHASH state after GHASH(Associated Data + Cipher +LenBlock) */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);
-	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], req_ctx->mac_buf_dma_addr,
-				  AES_BLOCK_SIZE, NS_BIT, 0);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_HASH_HW_GHASH);
+	set_din_no_dma(&desc[idx], 0, 0xfffff0);
+	set_dout_dlli(&desc[idx], req_ctx->mac_buf_dma_addr, AES_BLOCK_SIZE,
+		      NS_BIT, 0);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_aes_not_hash_mode(&desc[idx]);
 
-	idx++; 
+	idx++;
 
 	/* load AES/CTR initial CTR value inc by 1*/
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-				 req_ctx->gcm_iv_inc1_dma_addr, 
-				 AES_BLOCK_SIZE, NS_BIT);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);	
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_GCTR);
+	set_key_size_aes(&desc[idx], ctx->enc_keylen);
+	set_din_type(&desc[idx], DMA_DLLI, req_ctx->gcm_iv_inc1_dma_addr,
+		     AES_BLOCK_SIZE, NS_BIT);
+	set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* Memory Barrier */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	hw_desc_init(&desc[idx]);
+	set_din_no_dma(&desc[idx], 0, 0xfffff0);
+	set_dout_no_dma(&desc[idx], 0, 0, 1);
 	idx++;
 
 	/* process GCTR on stored GHASH and store MAC in mac_state*/
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-		req_ctx->mac_buf_dma_addr,
-		AES_BLOCK_SIZE, NS_BIT);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], mac_result, ctx->authsize, NS_BIT, 1);
-	HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
-	idx++;	
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_GCTR);
+	set_din_type(&desc[idx], DMA_DLLI, req_ctx->mac_buf_dma_addr,
+		     AES_BLOCK_SIZE, NS_BIT);
+	set_dout_dlli(&desc[idx], mac_result, ctx->authsize, NS_BIT, 1);
+	set_queue_last_ind(&desc[idx]);
+	set_flow_mode(&desc[idx], DIN_AES_DOUT);
+	idx++;
 
 	*seq_size = idx;
 }
 
 static inline int ssi_aead_gcm(
 	struct aead_request *req,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
@@ -1862,9 +1804,8 @@ static inline int ssi_aead_gcm(
 		cipher_flow_mode = AES_to_HASH_and_DOUT;
 	}
 
-
 	//in RFC4543 no data to encrypt. just copy data from src to dest.
-	if (req_ctx->plaintext_authenticate_only==true){     
+	if (req_ctx->plaintext_authenticate_only) {
 		ssi_aead_process_cipher_data_desc(req, BYPASS, desc, seq_size);
 		ssi_aead_gcm_setup_ghash_desc(req, desc, seq_size);
 		/* process(ghash) assoc data */
@@ -1883,7 +1824,7 @@ static inline int ssi_aead_gcm(
 	ssi_aead_gcm_setup_gctr_desc(req, desc, seq_size);
 	/* process(gctr+ghash) */
 	if (req_ctx->cryptlen != 0)
-		ssi_aead_process_cipher_data_desc(req, cipher_flow_mode, desc, seq_size); 
+		ssi_aead_process_cipher_data_desc(req, cipher_flow_mode, desc, seq_size);
 	ssi_aead_process_gcm_result_desc(req, desc, seq_size);
 
 	idx = *seq_size;
@@ -1892,7 +1833,7 @@ static inline int ssi_aead_gcm(
 
 #ifdef CC_DEBUG
 static inline void ssi_aead_dump_gcm(
-	const char* title,
+	const char *title,
 	struct aead_request *req)
 {
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
@@ -1902,52 +1843,50 @@ static inline void ssi_aead_dump_gcm(
 	if (ctx->cipher_mode != DRV_CIPHER_GCTR)
 		return;
 
-	if (title != NULL) {
+	if (title) {
 		SSI_LOG_DEBUG("----------------------------------------------------------------------------------");
 		SSI_LOG_DEBUG("%s\n", title);
 	}
 
-	SSI_LOG_DEBUG("cipher_mode %d, authsize %d, enc_keylen %d, assoclen %d, cryptlen %d \n", \
-				 ctx->cipher_mode, ctx->authsize, ctx->enc_keylen, req->assoclen, req_ctx->cryptlen );
+	SSI_LOG_DEBUG("cipher_mode %d, authsize %d, enc_keylen %d, assoclen %d, cryptlen %d\n", \
+				 ctx->cipher_mode, ctx->authsize, ctx->enc_keylen, req->assoclen, req_ctx->cryptlen);
 
-	if ( ctx->enckey != NULL ) {
-		dump_byte_array("mac key",ctx->enckey, 16);
-	}
+	if (ctx->enckey)
+		dump_byte_array("mac key", ctx->enckey, 16);
 
-	dump_byte_array("req->iv",req->iv, AES_BLOCK_SIZE);
+	dump_byte_array("req->iv", req->iv, AES_BLOCK_SIZE);
 
-	dump_byte_array("gcm_iv_inc1",req_ctx->gcm_iv_inc1, AES_BLOCK_SIZE);
+	dump_byte_array("gcm_iv_inc1", req_ctx->gcm_iv_inc1, AES_BLOCK_SIZE);
 
-	dump_byte_array("gcm_iv_inc2",req_ctx->gcm_iv_inc2, AES_BLOCK_SIZE);
+	dump_byte_array("gcm_iv_inc2", req_ctx->gcm_iv_inc2, AES_BLOCK_SIZE);
 
-	dump_byte_array("hkey",req_ctx->hkey, AES_BLOCK_SIZE);
+	dump_byte_array("hkey", req_ctx->hkey, AES_BLOCK_SIZE);
 
-	dump_byte_array("mac_buf",req_ctx->mac_buf, AES_BLOCK_SIZE);
+	dump_byte_array("mac_buf", req_ctx->mac_buf, AES_BLOCK_SIZE);
 
-	dump_byte_array("gcm_len_block",req_ctx->gcm_len_block.lenA, AES_BLOCK_SIZE);
+	dump_byte_array("gcm_len_block", req_ctx->gcm_len_block.lenA, AES_BLOCK_SIZE);
 
-	if (req->src!=NULL && req->cryptlen) {
-		dump_byte_array("req->src",sg_virt(req->src), req->cryptlen+req->assoclen);
-	}
+	if (req->src && req->cryptlen)
+		dump_byte_array("req->src", sg_virt(req->src), req->cryptlen + req->assoclen);
 
-	if (req->dst!=NULL) {
-		dump_byte_array("req->dst",sg_virt(req->dst), req->cryptlen+ctx->authsize+req->assoclen);
-    }
+	if (req->dst)
+		dump_byte_array("req->dst", sg_virt(req->dst), req->cryptlen + ctx->authsize + req->assoclen);
 }
 #endif
 
-static int config_gcm_context(struct aead_request *req) {
+static int config_gcm_context(struct aead_request *req)
+{
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
 	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
 	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
-	
-	unsigned int cryptlen = (req_ctx->gen_ctx.op_type == 
-				 DRV_CRYPTO_DIRECTION_ENCRYPT) ? 
-				req->cryptlen : 
+
+	unsigned int cryptlen = (req_ctx->gen_ctx.op_type ==
+				 DRV_CRYPTO_DIRECTION_ENCRYPT) ?
+				req->cryptlen :
 				(req->cryptlen - ctx->authsize);
 	__be32 counter = cpu_to_be32(2);
 
-	SSI_LOG_DEBUG("config_gcm_context() cryptlen = %d, req->assoclen = %d ctx->authsize = %d \n", cryptlen, req->assoclen, ctx->authsize);
+	SSI_LOG_DEBUG("config_gcm_context() cryptlen = %d, req->assoclen = %d ctx->authsize = %d\n", cryptlen, req->assoclen, ctx->authsize);
 
 	memset(req_ctx->hkey, 0, AES_BLOCK_SIZE);
 
@@ -1960,21 +1899,20 @@ static int config_gcm_context(struct aead_request *req) {
 	memcpy(req->iv + 12, &counter, 4);
 	memcpy(req_ctx->gcm_iv_inc1, req->iv, 16);
 
-
-	if (req_ctx->plaintext_authenticate_only == false)
-	{
+	if (!req_ctx->plaintext_authenticate_only) {
 		__be64 temp64;
+
 		temp64 = cpu_to_be64(req->assoclen * 8);
-		memcpy ( &req_ctx->gcm_len_block.lenA , &temp64, sizeof(temp64) );
+		memcpy(&req_ctx->gcm_len_block.lenA, &temp64, sizeof(temp64));
 		temp64 = cpu_to_be64(cryptlen * 8);
-		memcpy ( &req_ctx->gcm_len_block.lenC , &temp64, 8 );
-	}
-	else { //rfc4543=>  all data(AAD,IV,Plain) are considered additional data that is nothing is encrypted.
+		memcpy(&req_ctx->gcm_len_block.lenC, &temp64, 8);
+	} else { //rfc4543=>  all data(AAD,IV,Plain) are considered additional data that is nothing is encrypted.
 		__be64 temp64;
-		temp64 = cpu_to_be64((req->assoclen+GCM_BLOCK_RFC4_IV_SIZE+cryptlen) * 8);
-		memcpy ( &req_ctx->gcm_len_block.lenA , &temp64, sizeof(temp64) );
+
+		temp64 = cpu_to_be64((req->assoclen + GCM_BLOCK_RFC4_IV_SIZE + cryptlen) * 8);
+		memcpy(&req_ctx->gcm_len_block.lenA, &temp64, sizeof(temp64));
 		temp64 = 0;
-		memcpy ( &req_ctx->gcm_len_block.lenC , &temp64, 8 );
+		memcpy(&req_ctx->gcm_len_block.lenC, &temp64, 8);
 	}
 
 	return 0;
@@ -1988,34 +1926,30 @@ static void ssi_rfc4_gcm_process(struct aead_request *req)
 
 	memcpy(areq_ctx->ctr_iv + GCM_BLOCK_RFC4_NONCE_OFFSET, ctx->ctr_nonce, GCM_BLOCK_RFC4_NONCE_SIZE);
 	memcpy(areq_ctx->ctr_iv + GCM_BLOCK_RFC4_IV_OFFSET,    req->iv, GCM_BLOCK_RFC4_IV_SIZE);
-	req->iv = areq_ctx->ctr_iv;	
+	req->iv = areq_ctx->ctr_iv;
 	req->assoclen -= GCM_BLOCK_RFC4_IV_SIZE;
 }
 
-
 #endif /*SSI_CC_HAS_AES_GCM*/
 
 static int ssi_aead_process(struct aead_request *req, enum drv_crypto_direction direct)
 {
 	int rc = 0;
 	int seq_len = 0;
-	HwDesc_s desc[MAX_AEAD_PROCESS_SEQ]; 
+	struct cc_hw_desc desc[MAX_AEAD_PROCESS_SEQ];
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
 	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
 	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
 	struct device *dev = &ctx->drvdata->plat_dev->dev;
 	struct ssi_crypto_req ssi_req = {};
 
-	DECL_CYCLE_COUNT_RESOURCES;
-
 	SSI_LOG_DEBUG("%s context=%p req=%p iv=%p src=%p src_ofs=%d dst=%p dst_ofs=%d cryptolen=%d\n",
-		((direct==DRV_CRYPTO_DIRECTION_ENCRYPT)?"Encrypt":"Decrypt"), ctx, req, req->iv,
+		((direct == DRV_CRYPTO_DIRECTION_ENCRYPT) ? "Encrypt" : "Decrypt"), ctx, req, req->iv,
 		sg_virt(req->src), req->src->offset, sg_virt(req->dst), req->dst->offset, req->cryptlen);
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
 
 	/* STAT_PHASE_0: Init and sanity checks */
-	START_CYCLE_COUNT();
-	
+
 	/* Check data length according to mode */
 	if (unlikely(validate_data_size(ctx, direct, req) != 0)) {
 		SSI_LOG_ERR("Unsupported crypt/assoc len %d/%d.\n",
@@ -2028,25 +1962,19 @@ static int ssi_aead_process(struct aead_request *req, enum drv_crypto_direction
 	ssi_req.user_cb = (void *)ssi_aead_complete;
 	ssi_req.user_arg = (void *)req;
 
-#ifdef ENABLE_CYCLE_COUNT
-	ssi_req.op_type = (direct == DRV_CRYPTO_DIRECTION_DECRYPT) ?
-		STAT_OP_TYPE_DECODE : STAT_OP_TYPE_ENCODE;
-#endif
 	/* Setup request context */
 	areq_ctx->gen_ctx.op_type = direct;
 	areq_ctx->req_authsize = ctx->authsize;
 	areq_ctx->cipher_mode = ctx->cipher_mode;
 
-	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_0);
-
 	/* STAT_PHASE_1: Map buffers */
-	START_CYCLE_COUNT();
-	
+
 	if (ctx->cipher_mode == DRV_CIPHER_CTR) {
 		/* Build CTR IV - Copy nonce from last 4 bytes in
-		*  CTR key to first 4 bytes in CTR IV */
+		 * CTR key to first 4 bytes in CTR IV
+		 */
 		memcpy(areq_ctx->ctr_iv, ctx->ctr_nonce, CTR_RFC3686_NONCE_SIZE);
-		if (areq_ctx->backup_giv == NULL) /*User none-generated IV*/
+		if (!areq_ctx->backup_giv) /*User none-generated IV*/
 			memcpy(areq_ctx->ctr_iv + CTR_RFC3686_NONCE_SIZE,
 				req->iv, CTR_RFC3686_IV_SIZE);
 		/* Initialize counter portion of counter block */
@@ -2056,8 +1984,8 @@ static int ssi_aead_process(struct aead_request *req, enum drv_crypto_direction
 		/* Replace with counter iv */
 		req->iv = areq_ctx->ctr_iv;
 		areq_ctx->hw_iv_size = CTR_RFC3686_BLOCK_SIZE;
-	} else if ((ctx->cipher_mode == DRV_CIPHER_CCM) || 
-		   (ctx->cipher_mode == DRV_CIPHER_GCTR) ) {
+	} else if ((ctx->cipher_mode == DRV_CIPHER_CCM) ||
+		   (ctx->cipher_mode == DRV_CIPHER_GCTR)) {
 		areq_ctx->hw_iv_size = AES_BLOCK_SIZE;
 		if (areq_ctx->ctr_iv != req->iv) {
 			memcpy(areq_ctx->ctr_iv, req->iv, crypto_aead_ivsize(tfm));
@@ -2072,23 +2000,23 @@ static int ssi_aead_process(struct aead_request *req, enum drv_crypto_direction
 		rc = config_ccm_adata(req);
 		if (unlikely(rc != 0)) {
 			SSI_LOG_ERR("config_ccm_adata() returned with a failure %d!", rc);
-			goto exit; 
+			goto exit;
 		}
 	} else {
-		areq_ctx->ccm_hdr_size = ccm_header_size_null;		
+		areq_ctx->ccm_hdr_size = ccm_header_size_null;
 	}
 #else
-	areq_ctx->ccm_hdr_size = ccm_header_size_null;		
+	areq_ctx->ccm_hdr_size = ccm_header_size_null;
 #endif /*SSI_CC_HAS_AES_CCM*/
 
-#if SSI_CC_HAS_AES_GCM 
+#if SSI_CC_HAS_AES_GCM
 	if (ctx->cipher_mode == DRV_CIPHER_GCTR) {
 		rc = config_gcm_context(req);
 		if (unlikely(rc != 0)) {
 			SSI_LOG_ERR("config_gcm_context() returned with a failure %d!", rc);
-			goto exit; 
+			goto exit;
 		}
-	} 
+	}
 #endif /*SSI_CC_HAS_AES_GCM*/
 
 	rc = ssi_buffer_mgr_map_aead_request(ctx->drvdata, req);
@@ -2098,17 +2026,17 @@ static int ssi_aead_process(struct aead_request *req, enum drv_crypto_direction
 	}
 
 	/* do we need to generate IV? */
-	if (areq_ctx->backup_giv != NULL) {
-
+	if (areq_ctx->backup_giv) {
 		/* set the DMA mapped IV address*/
 		if (ctx->cipher_mode == DRV_CIPHER_CTR) {
 			ssi_req.ivgen_dma_addr[0] = areq_ctx->gen_ctx.iv_dma_addr + CTR_RFC3686_NONCE_SIZE;
 			ssi_req.ivgen_dma_addr_len = 1;
 		} else if (ctx->cipher_mode == DRV_CIPHER_CCM) {
 			/* In ccm, the IV needs to exist both inside B0 and inside the counter.
-			   It is also copied to iv_dma_addr for other reasons (like returning
-			   it to the user).
-			   So, using 3 (identical) IV outputs. */
+			 * It is also copied to iv_dma_addr for other reasons (like returning
+			 * it to the user).
+			 * So, using 3 (identical) IV outputs.
+			 */
 			ssi_req.ivgen_dma_addr[0] = areq_ctx->gen_ctx.iv_dma_addr + CCM_BLOCK_IV_OFFSET;
 			ssi_req.ivgen_dma_addr[1] = sg_dma_address(&areq_ctx->ccm_adata_sg) + CCM_B0_OFFSET          + CCM_BLOCK_IV_OFFSET;
 			ssi_req.ivgen_dma_addr[2] = sg_dma_address(&areq_ctx->ccm_adata_sg) + CCM_CTR_COUNT_0_OFFSET + CCM_BLOCK_IV_OFFSET;
@@ -2122,10 +2050,7 @@ static int ssi_aead_process(struct aead_request *req, enum drv_crypto_direction
 		ssi_req.ivgen_size = crypto_aead_ivsize(tfm);
 	}
 
-	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_1);
-
 	/* STAT_PHASE_2: Create sequence */
-	START_CYCLE_COUNT();
 
 	/* Load MLLI tables to SRAM if necessary */
 	ssi_aead_load_mlli_to_sram(req, desc, &seq_len);
@@ -2139,31 +2064,26 @@ static int ssi_aead_process(struct aead_request *req, enum drv_crypto_direction
 	case DRV_HASH_XCBC_MAC:
 		ssi_aead_xcbc_authenc(req, desc, &seq_len);
 		break;
-#if ( SSI_CC_HAS_AES_CCM || SSI_CC_HAS_AES_GCM )
+#if (SSI_CC_HAS_AES_CCM || SSI_CC_HAS_AES_GCM)
 	case DRV_HASH_NULL:
 #if SSI_CC_HAS_AES_CCM
-		if (ctx->cipher_mode == DRV_CIPHER_CCM) {
+		if (ctx->cipher_mode == DRV_CIPHER_CCM)
 			ssi_aead_ccm(req, desc, &seq_len);
-		}
 #endif /*SSI_CC_HAS_AES_CCM*/
 #if SSI_CC_HAS_AES_GCM
-		if (ctx->cipher_mode == DRV_CIPHER_GCTR) {
+		if (ctx->cipher_mode == DRV_CIPHER_GCTR)
 			ssi_aead_gcm(req, desc, &seq_len);
-		}
 #endif /*SSI_CC_HAS_AES_GCM*/
 			break;
 #endif
-	default:	
+	default:
 		SSI_LOG_ERR("Unsupported authenc (%d)\n", ctx->auth_mode);
 		ssi_buffer_mgr_unmap_aead_request(dev, req);
 		rc = -ENOTSUPP;
 		goto exit;
 	}
 
-	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_2);
-
 	/* STAT_PHASE_3: Lock HW and push sequence */
-	START_CYCLE_COUNT();
 
 	rc = send_request(ctx->drvdata, &ssi_req, desc, seq_len, 1);
 
@@ -2172,8 +2092,6 @@ static int ssi_aead_process(struct aead_request *req, enum drv_crypto_direction
 		ssi_buffer_mgr_unmap_aead_request(dev, req);
 	}
 
-	
-	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_3);
 exit:
 	return rc;
 }
@@ -2206,7 +2124,7 @@ static int ssi_rfc4309_ccm_encrypt(struct aead_request *req)
 	int rc = -EINVAL;
 
 	if (!valid_assoclen(req)) {
-		SSI_LOG_ERR("invalid Assoclen:%u\n", req->assoclen );
+		SSI_LOG_ERR("invalid Assoclen:%u\n", req->assoclen);
 		goto out;
 	}
 
@@ -2214,9 +2132,9 @@ static int ssi_rfc4309_ccm_encrypt(struct aead_request *req)
 	areq_ctx->backup_iv = req->iv;
 	areq_ctx->backup_giv = NULL;
 	areq_ctx->is_gcm4543 = true;
-	
+
 	ssi_rfc4309_ccm_process(req);
-	
+
 	rc = ssi_aead_process(req, DRV_CRYPTO_DIRECTION_ENCRYPT);
 	if (rc != -EINPROGRESS)
 		req->iv = areq_ctx->backup_iv;
@@ -2242,7 +2160,6 @@ static int ssi_aead_decrypt(struct aead_request *req)
 		req->iv = areq_ctx->backup_iv;
 
 	return rc;
-
 }
 
 #if SSI_CC_HAS_AES_CCM
@@ -2261,10 +2178,10 @@ static int ssi_rfc4309_ccm_decrypt(struct aead_request *req)
 	/* No generated IV required */
 	areq_ctx->backup_iv = req->iv;
 	areq_ctx->backup_giv = NULL;
-	
+
 	areq_ctx->is_gcm4543 = true;
 	ssi_rfc4309_ccm_process(req);
-	
+
 	rc = ssi_aead_process(req, DRV_CRYPTO_DIRECTION_DECRYPT);
 	if (rc != -EINPROGRESS)
 		req->iv = areq_ctx->backup_iv;
@@ -2280,8 +2197,8 @@ static int ssi_rfc4106_gcm_setkey(struct crypto_aead *tfm, const u8 *key, unsign
 {
 	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
 	int rc = 0;
-	
-	SSI_LOG_DEBUG("ssi_rfc4106_gcm_setkey()  keylen %d, key %p \n", keylen, key );
+
+	SSI_LOG_DEBUG("ssi_rfc4106_gcm_setkey()  keylen %d, key %p\n", keylen, key);
 
 	if (keylen < 4)
 		return -EINVAL;
@@ -2298,8 +2215,8 @@ static int ssi_rfc4543_gcm_setkey(struct crypto_aead *tfm, const u8 *key, unsign
 {
 	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
 	int rc = 0;
-	
-	SSI_LOG_DEBUG("ssi_rfc4543_gcm_setkey()  keylen %d, key %p \n", keylen, key );
+
+	SSI_LOG_DEBUG("ssi_rfc4543_gcm_setkey()  keylen %d, key %p\n", keylen, key);
 
 	if (keylen < 4)
 		return -EINVAL;
@@ -2334,24 +2251,24 @@ static int ssi_gcm_setauthsize(struct crypto_aead *authenc,
 static int ssi_rfc4106_gcm_setauthsize(struct crypto_aead *authenc,
 				      unsigned int authsize)
 {
-        SSI_LOG_DEBUG("ssi_rfc4106_gcm_setauthsize()  authsize %d \n", authsize );
-
-        switch (authsize) {
-        case 8:
-        case 12:
-        case 16:
-                break;
-        default:
-                return -EINVAL;
-        }
-
-        return ssi_aead_setauthsize(authenc, authsize);
+	SSI_LOG_DEBUG("ssi_rfc4106_gcm_setauthsize()  authsize %d\n", authsize);
+
+	switch (authsize) {
+	case 8:
+	case 12:
+	case 16:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return ssi_aead_setauthsize(authenc, authsize);
 }
 
 static int ssi_rfc4543_gcm_setauthsize(struct crypto_aead *authenc,
-				      unsigned int authsize)
+				       unsigned int authsize)
 {
-	SSI_LOG_DEBUG("ssi_rfc4543_gcm_setauthsize()  authsize %d \n", authsize );
+	SSI_LOG_DEBUG("ssi_rfc4543_gcm_setauthsize()  authsize %d\n", authsize);
 
 	if (authsize != 16)
 		return -EINVAL;
@@ -2364,7 +2281,7 @@ static int ssi_rfc4106_gcm_encrypt(struct aead_request *req)
 	/* Very similar to ssi_aead_encrypt() above. */
 
 	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
-        int rc = -EINVAL;
+	int rc = -EINVAL;
 
 	if (!valid_assoclen(req)) {
 		SSI_LOG_ERR("invalid Assoclen:%u\n", req->assoclen);
@@ -2374,7 +2291,7 @@ static int ssi_rfc4106_gcm_encrypt(struct aead_request *req)
 	/* No generated IV required */
 	areq_ctx->backup_iv = req->iv;
 	areq_ctx->backup_giv = NULL;
-	
+
 	areq_ctx->plaintext_authenticate_only = false;
 
 	ssi_rfc4_gcm_process(req);
@@ -2393,14 +2310,14 @@ static int ssi_rfc4543_gcm_encrypt(struct aead_request *req)
 
 	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
 	int rc;
-	
+
 	//plaintext is not encryped with rfc4543
 	areq_ctx->plaintext_authenticate_only = true;
 
 	/* No generated IV required */
 	areq_ctx->backup_iv = req->iv;
 	areq_ctx->backup_giv = NULL;
-	
+
 	ssi_rfc4_gcm_process(req);
 	areq_ctx->is_gcm4543 = true;
 
@@ -2416,7 +2333,7 @@ static int ssi_rfc4106_gcm_decrypt(struct aead_request *req)
 	/* Very similar to ssi_aead_decrypt() above. */
 
 	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
-        int rc = -EINVAL;
+	int rc = -EINVAL;
 
 	if (!valid_assoclen(req)) {
 		SSI_LOG_ERR("invalid Assoclen:%u\n", req->assoclen);
@@ -2426,7 +2343,7 @@ static int ssi_rfc4106_gcm_decrypt(struct aead_request *req)
 	/* No generated IV required */
 	areq_ctx->backup_iv = req->iv;
 	areq_ctx->backup_giv = NULL;
-	
+
 	areq_ctx->plaintext_authenticate_only = false;
 
 	ssi_rfc4_gcm_process(req);
@@ -2452,7 +2369,7 @@ static int ssi_rfc4543_gcm_decrypt(struct aead_request *req)
 	/* No generated IV required */
 	areq_ctx->backup_iv = req->iv;
 	areq_ctx->backup_giv = NULL;
-	
+
 	ssi_rfc4_gcm_process(req);
 	areq_ctx->is_gcm4543 = true;
 
@@ -2715,7 +2632,7 @@ static struct ssi_alg_template aead_algs[] = {
 		.cipher_mode = DRV_CIPHER_GCTR,
 		.flow_mode = S_DIN_to_AES,
 		.auth_mode = DRV_HASH_NULL,
-	}, 
+	},
 #endif /*SSI_CC_HAS_AES_GCM*/
 };
 
@@ -2758,7 +2675,7 @@ int ssi_aead_free(struct ssi_drvdata *drvdata)
 	struct ssi_aead_handle *aead_handle =
 		(struct ssi_aead_handle *)drvdata->aead_handle;
 
-	if (aead_handle != NULL) {
+	if (aead_handle) {
 		/* Remove registered algs */
 		list_for_each_entry_safe(t_alg, n, &aead_handle->aead_list, entry) {
 			crypto_unregister_aead(&t_alg->aead_alg);
@@ -2780,7 +2697,7 @@ int ssi_aead_alloc(struct ssi_drvdata *drvdata)
 	int alg;
 
 	aead_handle = kmalloc(sizeof(struct ssi_aead_handle), GFP_KERNEL);
-	if (aead_handle == NULL) {
+	if (!aead_handle) {
 		rc = -ENOMEM;
 		goto fail0;
 	}
@@ -2827,6 +2744,3 @@ fail1:
 fail0:
 	return rc;
 }
-
-
-
diff --git a/drivers/staging/ccree/ssi_aead.h b/drivers/staging/ccree/ssi_aead.h
index fe88c9e04f88..39cc633a3ffa 100644
--- a/drivers/staging/ccree/ssi_aead.h
+++ b/drivers/staging/ccree/ssi_aead.h
@@ -1,21 +1,21 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
 /* \file ssi_aead.h
-   ARM CryptoCell AEAD Crypto API
+ * ARM CryptoCell AEAD Crypto API
  */
 
 #ifndef __SSI_AEAD_H__
@@ -25,22 +25,18 @@
 #include <crypto/algapi.h>
 #include <crypto/ctr.h>
 
-
 /* mac_cmp - HW writes 8 B but all bytes hold the same value */
 #define ICV_CMP_SIZE 8
-#define CCM_CONFIG_BUF_SIZE (AES_BLOCK_SIZE*3)
+#define CCM_CONFIG_BUF_SIZE (AES_BLOCK_SIZE * 3)
 #define MAX_MAC_SIZE MAX(SHA256_DIGEST_SIZE, AES_BLOCK_SIZE)
 
-
 /* defines for AES GCM configuration buffer */
 #define GCM_BLOCK_LEN_SIZE 8
 
-#define GCM_BLOCK_RFC4_IV_OFFSET    	4  
-#define GCM_BLOCK_RFC4_IV_SIZE  	    8  /* IV size for rfc's */
-#define GCM_BLOCK_RFC4_NONCE_OFFSET 	0  
-#define GCM_BLOCK_RFC4_NONCE_SIZE   	4  
-
-
+#define GCM_BLOCK_RFC4_IV_OFFSET	4
+#define GCM_BLOCK_RFC4_IV_SIZE		8  /* IV size for rfc's */
+#define GCM_BLOCK_RFC4_NONCE_OFFSET	0
+#define GCM_BLOCK_RFC4_NONCE_SIZE	4
 
 /* Offsets into AES CCM configuration buffer */
 #define CCM_B0_OFFSET 0
@@ -57,48 +53,49 @@ enum aead_ccm_header_size {
 	ccm_header_size_zero = 0,
 	ccm_header_size_2 = 2,
 	ccm_header_size_6 = 6,
-	ccm_header_size_max = INT32_MAX
+	ccm_header_size_max = S32_MAX
 };
 
 struct aead_req_ctx {
 	/* Allocate cache line although only 4 bytes are needed to
-	*  assure next field falls @ cache line 
-	*  Used for both: digest HW compare and CCM/GCM MAC value */
-	uint8_t mac_buf[MAX_MAC_SIZE] ____cacheline_aligned;
-	uint8_t ctr_iv[AES_BLOCK_SIZE] ____cacheline_aligned;
-
-	//used in gcm 
-	uint8_t gcm_iv_inc1[AES_BLOCK_SIZE] ____cacheline_aligned;
-	uint8_t gcm_iv_inc2[AES_BLOCK_SIZE] ____cacheline_aligned;
-	uint8_t hkey[AES_BLOCK_SIZE] ____cacheline_aligned;
+	 *  assure next field falls @ cache line
+	 *  Used for both: digest HW compare and CCM/GCM MAC value
+	 */
+	u8 mac_buf[MAX_MAC_SIZE] ____cacheline_aligned;
+	u8 ctr_iv[AES_BLOCK_SIZE] ____cacheline_aligned;
+
+	//used in gcm
+	u8 gcm_iv_inc1[AES_BLOCK_SIZE] ____cacheline_aligned;
+	u8 gcm_iv_inc2[AES_BLOCK_SIZE] ____cacheline_aligned;
+	u8 hkey[AES_BLOCK_SIZE] ____cacheline_aligned;
 	struct {
-		uint8_t lenA[GCM_BLOCK_LEN_SIZE] ____cacheline_aligned;
-		uint8_t lenC[GCM_BLOCK_LEN_SIZE] ;
+		u8 lenA[GCM_BLOCK_LEN_SIZE] ____cacheline_aligned;
+		u8 lenC[GCM_BLOCK_LEN_SIZE];
 	} gcm_len_block;
 
-	uint8_t ccm_config[CCM_CONFIG_BUF_SIZE] ____cacheline_aligned;
+	u8 ccm_config[CCM_CONFIG_BUF_SIZE] ____cacheline_aligned;
 	unsigned int hw_iv_size ____cacheline_aligned; /*HW actual size input*/
-	uint8_t backup_mac[MAX_MAC_SIZE]; /*used to prevent cache coherence problem*/
-	uint8_t *backup_iv; /*store iv for generated IV flow*/
-	uint8_t *backup_giv; /*store iv for rfc3686(ctr) flow*/
+	u8 backup_mac[MAX_MAC_SIZE]; /*used to prevent cache coherence problem*/
+	u8 *backup_iv; /*store iv for generated IV flow*/
+	u8 *backup_giv; /*store iv for rfc3686(ctr) flow*/
 	dma_addr_t mac_buf_dma_addr; /* internal ICV DMA buffer */
 	dma_addr_t ccm_iv0_dma_addr; /* buffer for internal ccm configurations */
 	dma_addr_t icv_dma_addr; /* Phys. address of ICV */
 
-	//used in gcm 
+	//used in gcm
 	dma_addr_t gcm_iv_inc1_dma_addr; /* buffer for internal gcm configurations */
 	dma_addr_t gcm_iv_inc2_dma_addr; /* buffer for internal gcm configurations */
 	dma_addr_t hkey_dma_addr; /* Phys. address of hkey */
 	dma_addr_t gcm_block_len_dma_addr; /* Phys. address of gcm block len */
 	bool is_gcm4543;
 
-	uint8_t *icv_virt_addr; /* Virt. address of ICV */
+	u8 *icv_virt_addr; /* Virt. address of ICV */
 	struct async_gen_req_ctx gen_ctx;
 	struct ssi_mlli assoc;
 	struct ssi_mlli src;
 	struct ssi_mlli dst;
-	struct scatterlist* srcSgl;
-	struct scatterlist* dstSgl;
+	struct scatterlist *srcSgl;
+	struct scatterlist *dstSgl;
 	unsigned int srcOffset;
 	unsigned int dstOffset;
 	enum ssi_req_dma_buf_type assoc_buff_type;
diff --git a/drivers/staging/ccree/ssi_buffer_mgr.c b/drivers/staging/ccree/ssi_buffer_mgr.c
index 6471d3d2d375..b35871eeabd1 100644
--- a/drivers/staging/ccree/ssi_buffer_mgr.c
+++ b/drivers/staging/ccree/ssi_buffer_mgr.c
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -33,42 +33,15 @@
 #include "ssi_hash.h"
 #include "ssi_aead.h"
 
-#define LLI_MAX_NUM_OF_DATA_ENTRIES 128
-#define LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES 4
-#define MLLI_TABLE_MIN_ALIGNMENT 4 /*Force the MLLI table to be align to uint32 */
-#define MAX_NUM_OF_BUFFERS_IN_MLLI 4
-#define MAX_NUM_OF_TOTAL_MLLI_ENTRIES (2*LLI_MAX_NUM_OF_DATA_ENTRIES + \
-					LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES )
-
 #ifdef CC_DEBUG
-#define DUMP_SGL(sg) \
-	while (sg) { \
-		SSI_LOG_DEBUG("page=%lu offset=%u length=%u (dma_len=%u) " \
-			     "dma_addr=%08x\n", (sg)->page_link, (sg)->offset, \
-			(sg)->length, sg_dma_len(sg), (sg)->dma_address); \
-		(sg) = sg_next(sg); \
-	}
-#define DUMP_MLLI_TABLE(mlli_p, nents) \
-	do { \
-		SSI_LOG_DEBUG("mlli=%pK nents=%u\n", (mlli_p), (nents)); \
-		while((nents)--) { \
-			SSI_LOG_DEBUG("addr=0x%08X size=0x%08X\n", \
-			     (mlli_p)[LLI_WORD0_OFFSET], \
-			     (mlli_p)[LLI_WORD1_OFFSET]); \
-			(mlli_p) += LLI_ENTRY_WORD_SIZE; \
-		} \
-	} while (0)
 #define GET_DMA_BUFFER_TYPE(buff_type) ( \
 	((buff_type) == SSI_DMA_BUF_NULL) ? "BUF_NULL" : \
 	((buff_type) == SSI_DMA_BUF_DLLI) ? "BUF_DLLI" : \
 	((buff_type) == SSI_DMA_BUF_MLLI) ? "BUF_MLLI" : "BUF_INVALID")
 #else
-#define DX_BUFFER_MGR_DUMP_SGL(sg)
-#define DX_BUFFER_MGR_DUMP_MLLI_TABLE(mlli_p, nents)
 #define GET_DMA_BUFFER_TYPE(buff_type)
 #endif
 
-
 enum dma_buffer_type {
 	DMA_NULL_TYPE = -1,
 	DMA_SGL_TYPE = 1,
@@ -92,103 +65,55 @@ struct buffer_array {
 	int total_data_len[MAX_NUM_OF_BUFFERS_IN_MLLI];
 	enum dma_buffer_type type[MAX_NUM_OF_BUFFERS_IN_MLLI];
 	bool is_last[MAX_NUM_OF_BUFFERS_IN_MLLI];
-	uint32_t * mlli_nents[MAX_NUM_OF_BUFFERS_IN_MLLI];
+	u32 *mlli_nents[MAX_NUM_OF_BUFFERS_IN_MLLI];
 };
 
-#ifdef CC_DMA_48BIT_SIM
-dma_addr_t ssi_buff_mgr_update_dma_addr(dma_addr_t orig_addr, uint32_t data_len)
-{
-	dma_addr_t tmp_dma_addr;
-#ifdef CC_DMA_48BIT_SIM_FULL
-	/* With this code all addresses will be switched to 48 bits. */
-	/* The if condition protects from double expention */
-	if((((orig_addr >> 16) & 0xFFFF) != 0xFFFF) && 
-		(data_len <= CC_MAX_MLLI_ENTRY_SIZE)) {
-#else
-	if((!(((orig_addr >> 16) & 0xFF) % 2)) && 
-		(data_len <= CC_MAX_MLLI_ENTRY_SIZE)) {
-#endif
-		tmp_dma_addr = ((orig_addr<<16) | 0xFFFF0000 | 
-				(orig_addr & UINT16_MAX));
-			SSI_LOG_DEBUG("MAP DMA: orig address=0x%llX "
-				    "dma_address=0x%llX\n",
-				     orig_addr, tmp_dma_addr);
-			return tmp_dma_addr;	
-	}
-	return orig_addr;
-}
-
-dma_addr_t ssi_buff_mgr_restore_dma_addr(dma_addr_t orig_addr)
-{
-	dma_addr_t tmp_dma_addr;
-#ifdef CC_DMA_48BIT_SIM_FULL
-	/* With this code all addresses will be restored from 48 bits. */
-	/* The if condition protects from double restoring */
-	if((orig_addr >> 32) & 0xFFFF ) {
-#else
-	if(((orig_addr >> 32) & 0xFFFF) && 
-		!(((orig_addr >> 32) & 0xFF) % 2) ) {
-#endif
-		/*return high 16 bits*/
-		tmp_dma_addr = ((orig_addr >> 16));
-		/*clean the 0xFFFF in the lower bits (set in the add expansion)*/
-		tmp_dma_addr &= 0xFFFF0000; 
-		/* Set the original 16 bits */
-		tmp_dma_addr |= (orig_addr & UINT16_MAX); 
-		SSI_LOG_DEBUG("Release DMA: orig address=0x%llX "
-			     "dma_address=0x%llX\n",
-			     orig_addr, tmp_dma_addr);
-			return tmp_dma_addr;	
-	}
-	return orig_addr;
-}
-#endif
 /**
  * ssi_buffer_mgr_get_sgl_nents() - Get scatterlist number of entries.
- * 
+ *
  * @sg_list: SG list
  * @nbytes: [IN] Total SGL data bytes.
- * @lbytes: [OUT] Returns the amount of bytes at the last entry 
+ * @lbytes: [OUT] Returns the amount of bytes at the last entry
  */
 static unsigned int ssi_buffer_mgr_get_sgl_nents(
-	struct scatterlist *sg_list, unsigned int nbytes, uint32_t *lbytes, bool *is_chained)
+	struct scatterlist *sg_list, unsigned int nbytes, u32 *lbytes, bool *is_chained)
 {
 	unsigned int nents = 0;
+
 	while (nbytes != 0) {
 		if (sg_is_chain(sg_list)) {
-			SSI_LOG_ERR("Unexpected chanined entry "
-				   "in sg (entry =0x%X) \n", nents);
+			SSI_LOG_ERR("Unexpected chained entry "
+				   "in sg (entry =0x%X)\n", nents);
 			BUG();
 		}
 		if (sg_list->length != 0) {
 			nents++;
 			/* get the number of bytes in the last entry */
 			*lbytes = nbytes;
-			nbytes -= ( sg_list->length > nbytes ) ? nbytes : sg_list->length;
+			nbytes -= (sg_list->length > nbytes) ? nbytes : sg_list->length;
 			sg_list = sg_next(sg_list);
 		} else {
 			sg_list = (struct scatterlist *)sg_page(sg_list);
-			if (is_chained != NULL) {
+			if (is_chained)
 				*is_chained = true;
-			}
 		}
 	}
-	SSI_LOG_DEBUG("nents %d last bytes %d\n",nents, *lbytes);
+	SSI_LOG_DEBUG("nents %d last bytes %d\n", nents, *lbytes);
 	return nents;
 }
 
 /**
  * ssi_buffer_mgr_zero_sgl() - Zero scatter scatter list data.
- * 
+ *
  * @sgl:
  */
-void ssi_buffer_mgr_zero_sgl(struct scatterlist *sgl, uint32_t data_len)
+void ssi_buffer_mgr_zero_sgl(struct scatterlist *sgl, u32 data_len)
 {
 	struct scatterlist *current_sg = sgl;
 	int sg_index = 0;
 
 	while (sg_index <= data_len) {
-		if (current_sg == NULL) {
+		if (!current_sg) {
 			/* reached the end of the sgl --> just return back */
 			return;
 		}
@@ -201,7 +126,7 @@ void ssi_buffer_mgr_zero_sgl(struct scatterlist *sgl, uint32_t data_len)
 /**
  * ssi_buffer_mgr_copy_scatterlist_portion() - Copy scatter list data,
  * from to_skip to end, to dest and vice versa
- * 
+ *
  * @dest:
  * @sg:
  * @to_skip:
@@ -210,10 +135,10 @@ void ssi_buffer_mgr_zero_sgl(struct scatterlist *sgl, uint32_t data_len)
  */
 void ssi_buffer_mgr_copy_scatterlist_portion(
 	u8 *dest, struct scatterlist *sg,
-	uint32_t to_skip,  uint32_t end,
+	u32 to_skip,  u32 end,
 	enum ssi_sg_cpy_direct direct)
 {
-	uint32_t nents, lbytes;
+	u32 nents, lbytes;
 
 	nents = ssi_buffer_mgr_get_sgl_nents(sg, end, &lbytes, NULL);
 	sg_copy_buffer(sg, nents, (void *)dest, (end - to_skip + 1), to_skip,
@@ -221,37 +146,33 @@ void ssi_buffer_mgr_copy_scatterlist_portion(
 }
 
 static inline int ssi_buffer_mgr_render_buff_to_mlli(
-	dma_addr_t buff_dma, uint32_t buff_size, uint32_t *curr_nents,
-	uint32_t **mlli_entry_pp)
+	dma_addr_t buff_dma, u32 buff_size, u32 *curr_nents,
+	u32 **mlli_entry_pp)
 {
-	uint32_t *mlli_entry_p = *mlli_entry_pp;
-	uint32_t new_nents;;
+	u32 *mlli_entry_p = *mlli_entry_pp;
+	u32 new_nents;;
 
 	/* Verify there is no memory overflow*/
-	new_nents = (*curr_nents + buff_size/CC_MAX_MLLI_ENTRY_SIZE + 1);
-	if (new_nents > MAX_NUM_OF_TOTAL_MLLI_ENTRIES ) {
+	new_nents = (*curr_nents + buff_size / CC_MAX_MLLI_ENTRY_SIZE + 1);
+	if (new_nents > MAX_NUM_OF_TOTAL_MLLI_ENTRIES)
 		return -ENOMEM;
-	}
 
 	/*handle buffer longer than 64 kbytes */
-	while (buff_size > CC_MAX_MLLI_ENTRY_SIZE ) {
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(buff_dma, CC_MAX_MLLI_ENTRY_SIZE);
-		LLI_SET_ADDR(mlli_entry_p,buff_dma);
-		LLI_SET_SIZE(mlli_entry_p, CC_MAX_MLLI_ENTRY_SIZE);
-		SSI_LOG_DEBUG("entry[%d]: single_buff=0x%08X size=%08X\n",*curr_nents,
+	while (buff_size > CC_MAX_MLLI_ENTRY_SIZE) {
+		cc_lli_set_addr(mlli_entry_p, buff_dma);
+		cc_lli_set_size(mlli_entry_p, CC_MAX_MLLI_ENTRY_SIZE);
+		SSI_LOG_DEBUG("entry[%d]: single_buff=0x%08X size=%08X\n", *curr_nents,
 			   mlli_entry_p[LLI_WORD0_OFFSET],
 			   mlli_entry_p[LLI_WORD1_OFFSET]);
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(buff_dma);
 		buff_dma += CC_MAX_MLLI_ENTRY_SIZE;
 		buff_size -= CC_MAX_MLLI_ENTRY_SIZE;
 		mlli_entry_p = mlli_entry_p + 2;
 		(*curr_nents)++;
 	}
 	/*Last entry */
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(buff_dma, buff_size);
-	LLI_SET_ADDR(mlli_entry_p,buff_dma);
-	LLI_SET_SIZE(mlli_entry_p, buff_size);
-	SSI_LOG_DEBUG("entry[%d]: single_buff=0x%08X size=%08X\n",*curr_nents,
+	cc_lli_set_addr(mlli_entry_p, buff_dma);
+	cc_lli_set_size(mlli_entry_p, buff_size);
+	SSI_LOG_DEBUG("entry[%d]: single_buff=0x%08X size=%08X\n", *curr_nents,
 		   mlli_entry_p[LLI_WORD0_OFFSET],
 		   mlli_entry_p[LLI_WORD1_OFFSET]);
 	mlli_entry_p = mlli_entry_p + 2;
@@ -260,28 +181,27 @@ static inline int ssi_buffer_mgr_render_buff_to_mlli(
 	return 0;
 }
 
-
 static inline int ssi_buffer_mgr_render_scatterlist_to_mlli(
-	struct scatterlist *sgl, uint32_t sgl_data_len, uint32_t sglOffset, uint32_t *curr_nents,
-	uint32_t **mlli_entry_pp)
+	struct scatterlist *sgl, u32 sgl_data_len, u32 sglOffset, u32 *curr_nents,
+	u32 **mlli_entry_pp)
 {
 	struct scatterlist *curr_sgl = sgl;
-	uint32_t *mlli_entry_p = *mlli_entry_pp;
-	int32_t rc = 0;
+	u32 *mlli_entry_p = *mlli_entry_pp;
+	s32 rc = 0;
 
-	for ( ; (curr_sgl != NULL) && (sgl_data_len != 0);
+	for ( ; (curr_sgl) && (sgl_data_len != 0);
 	      curr_sgl = sg_next(curr_sgl)) {
-		uint32_t entry_data_len =
+		u32 entry_data_len =
 			(sgl_data_len > sg_dma_len(curr_sgl) - sglOffset) ?
-				sg_dma_len(curr_sgl) - sglOffset : sgl_data_len ;
+				sg_dma_len(curr_sgl) - sglOffset : sgl_data_len;
 		sgl_data_len -= entry_data_len;
 		rc = ssi_buffer_mgr_render_buff_to_mlli(
 			sg_dma_address(curr_sgl) + sglOffset, entry_data_len, curr_nents,
 			&mlli_entry_p);
-		if(rc != 0) {
+		if (rc != 0)
 			return rc;
-		}
-		sglOffset=0;
+
+		sglOffset = 0;
 	}
 	*mlli_entry_pp = mlli_entry_p;
 	return 0;
@@ -292,8 +212,8 @@ static int ssi_buffer_mgr_generate_mlli(
 	struct buffer_array *sg_data,
 	struct mlli_params *mlli_params)
 {
-	uint32_t *mlli_p;
-	uint32_t total_nents = 0,prev_total_nents = 0;
+	u32 *mlli_p;
+	u32 total_nents = 0, prev_total_nents = 0;
 	int rc = 0, i;
 
 	SSI_LOG_DEBUG("NUM of SG's = %d\n", sg_data->num_of_buffers);
@@ -302,21 +222,18 @@ static int ssi_buffer_mgr_generate_mlli(
 	mlli_params->mlli_virt_addr = dma_pool_alloc(
 			mlli_params->curr_pool, GFP_KERNEL,
 			&(mlli_params->mlli_dma_addr));
-	if (unlikely(mlli_params->mlli_virt_addr == NULL)) {
+	if (unlikely(!mlli_params->mlli_virt_addr)) {
 		SSI_LOG_ERR("dma_pool_alloc() failed\n");
-		rc =-ENOMEM;
+		rc = -ENOMEM;
 		goto build_mlli_exit;
 	}
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(mlli_params->mlli_dma_addr, 
-						(MAX_NUM_OF_TOTAL_MLLI_ENTRIES*
-						LLI_ENTRY_BYTE_SIZE));
 	/* Point to start of MLLI */
-	mlli_p = (uint32_t *)mlli_params->mlli_virt_addr;
+	mlli_p = (u32 *)mlli_params->mlli_virt_addr;
 	/* go over all SG's and link it to one MLLI table */
 	for (i = 0; i < sg_data->num_of_buffers; i++) {
 		if (sg_data->type[i] == DMA_SGL_TYPE)
 			rc = ssi_buffer_mgr_render_scatterlist_to_mlli(
-				sg_data->entry[i].sgl, 
+				sg_data->entry[i].sgl,
 				sg_data->total_data_len[i], sg_data->offset[i], &total_nents,
 				&mlli_p);
 		else /*DMA_BUFF_TYPE*/
@@ -324,15 +241,15 @@ static int ssi_buffer_mgr_generate_mlli(
 				sg_data->entry[i].buffer_dma,
 				sg_data->total_data_len[i], &total_nents,
 				&mlli_p);
-		if(rc != 0) {
+		if (rc != 0)
 			return rc;
-		}
 
 		/* set last bit in the current table */
-		if (sg_data->mlli_nents[i] != NULL) {
-			/*Calculate the current MLLI table length for the 
-			length field in the descriptor*/
-			*(sg_data->mlli_nents[i]) += 
+		if (sg_data->mlli_nents[i]) {
+			/*Calculate the current MLLI table length for the
+			 *length field in the descriptor
+			 */
+			*(sg_data->mlli_nents[i]) +=
 				(total_nents - prev_total_nents);
 			prev_total_nents = total_nents;
 		}
@@ -354,7 +271,7 @@ build_mlli_exit:
 static inline void ssi_buffer_mgr_add_buffer_entry(
 	struct buffer_array *sgl_data,
 	dma_addr_t buffer_dma, unsigned int buffer_len,
-	bool is_last_entry, uint32_t *mlli_nents)
+	bool is_last_entry, u32 *mlli_nents)
 {
 	unsigned int index = sgl_data->num_of_buffers;
 
@@ -368,7 +285,7 @@ static inline void ssi_buffer_mgr_add_buffer_entry(
 	sgl_data->type[index] = DMA_BUFF_TYPE;
 	sgl_data->is_last[index] = is_last_entry;
 	sgl_data->mlli_nents[index] = mlli_nents;
-	if (sgl_data->mlli_nents[index] != NULL)
+	if (sgl_data->mlli_nents[index])
 		*sgl_data->mlli_nents[index] = 0;
 	sgl_data->num_of_buffers++;
 }
@@ -380,7 +297,7 @@ static inline void ssi_buffer_mgr_add_scatterlist_entry(
 	unsigned int data_len,
 	unsigned int data_offset,
 	bool is_last_table,
-	uint32_t *mlli_nents)
+	u32 *mlli_nents)
 {
 	unsigned int index = sgl_data->num_of_buffers;
 
@@ -393,22 +310,22 @@ static inline void ssi_buffer_mgr_add_scatterlist_entry(
 	sgl_data->type[index] = DMA_SGL_TYPE;
 	sgl_data->is_last[index] = is_last_table;
 	sgl_data->mlli_nents[index] = mlli_nents;
-	if (sgl_data->mlli_nents[index] != NULL)
+	if (sgl_data->mlli_nents[index])
 		*sgl_data->mlli_nents[index] = 0;
 	sgl_data->num_of_buffers++;
 }
 
 static int
-ssi_buffer_mgr_dma_map_sg(struct device *dev, struct scatterlist *sg, uint32_t nents,
+ssi_buffer_mgr_dma_map_sg(struct device *dev, struct scatterlist *sg, u32 nents,
 			 enum dma_data_direction direction)
 {
-	uint32_t i , j;
+	u32 i, j;
 	struct scatterlist *l_sg = sg;
+
 	for (i = 0; i < nents; i++) {
-		if (l_sg == NULL) {
+		if (!l_sg)
 			break;
-		}
-		if (unlikely(dma_map_sg(dev, l_sg, 1, direction) != 1)){
+		if (unlikely(dma_map_sg(dev, l_sg, 1, direction) != 1)) {
 			SSI_LOG_ERR("dma_map_page() sg buffer failed\n");
 			goto err;
 		}
@@ -419,10 +336,9 @@ ssi_buffer_mgr_dma_map_sg(struct device *dev, struct scatterlist *sg, uint32_t n
 err:
 	/* Restore mapped parts */
 	for (j = 0; j < i; j++) {
-		if (sg == NULL) {
+		if (!sg)
 			break;
-		}
-		dma_unmap_sg(dev,sg,1,direction);
+		dma_unmap_sg(dev, sg, 1, direction);
 		sg = sg_next(sg);
 	}
 	return 0;
@@ -431,8 +347,8 @@ err:
 static int ssi_buffer_mgr_map_scatterlist(
 	struct device *dev, struct scatterlist *sg,
 	unsigned int nbytes, int direction,
-	uint32_t *nents, uint32_t max_sg_nents,
-	uint32_t *lbytes, uint32_t *mapped_nents)
+	u32 *nents, u32 max_sg_nents,
+	u32 *lbytes, u32 *mapped_nents)
 {
 	bool is_chained = false;
 
@@ -441,20 +357,19 @@ static int ssi_buffer_mgr_map_scatterlist(
 		if (unlikely(dma_map_sg(dev, sg, 1, direction) != 1)) {
 			SSI_LOG_ERR("dma_map_sg() single buffer failed\n");
 			return -ENOMEM;
-		} 
+		}
 		SSI_LOG_DEBUG("Mapped sg: dma_address=0x%llX "
-			     "page_link=0x%08lX addr=%pK offset=%u "
+			     "page=%p addr=%pK offset=%u "
 			     "length=%u\n",
-			     (unsigned long long)sg_dma_address(sg), 
-			     sg->page_link, 
-			     sg_virt(sg), 
+			     (unsigned long long)sg_dma_address(sg),
+			     sg_page(sg),
+			     sg_virt(sg),
 			     sg->offset, sg->length);
 		*lbytes = nbytes;
 		*nents = 1;
 		*mapped_nents = 1;
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(sg_dma_address(sg), sg_dma_len(sg));
 	} else {  /*sg_is_last*/
-		*nents = ssi_buffer_mgr_get_sgl_nents(sg, nbytes, lbytes, 
+		*nents = ssi_buffer_mgr_get_sgl_nents(sg, nbytes, lbytes,
 						     &is_chained);
 		if (*nents > max_sg_nents) {
 			*nents = 0;
@@ -464,21 +379,23 @@ static int ssi_buffer_mgr_map_scatterlist(
 		}
 		if (!is_chained) {
 			/* In case of mmu the number of mapped nents might
-			be changed from the original sgl nents */
+			 * be changed from the original sgl nents
+			 */
 			*mapped_nents = dma_map_sg(dev, sg, *nents, direction);
-			if (unlikely(*mapped_nents == 0)){
+			if (unlikely(*mapped_nents == 0)) {
 				*nents = 0;
 				SSI_LOG_ERR("dma_map_sg() sg buffer failed\n");
 				return -ENOMEM;
 			}
 		} else {
 			/*In this case the driver maps entry by entry so it
-			must have the same nents before and after map */
+			 * must have the same nents before and after map
+			 */
 			*mapped_nents = ssi_buffer_mgr_dma_map_sg(dev,
 								 sg,
 								 *nents,
 								 direction);
-			if (unlikely(*mapped_nents != *nents)){
+			if (unlikely(*mapped_nents != *nents)) {
 				*nents = *mapped_nents;
 				SSI_LOG_ERR("dma_map_sg() sg buffer failed\n");
 				return -ENOMEM;
@@ -492,48 +409,47 @@ static int ssi_buffer_mgr_map_scatterlist(
 static inline int
 ssi_aead_handle_config_buf(struct device *dev,
 	struct aead_req_ctx *areq_ctx,
-	uint8_t* config_data,
+	u8 *config_data,
 	struct buffer_array *sg_data,
 	unsigned int assoclen)
 {
-	SSI_LOG_DEBUG(" handle additional data config set to   DLLI \n");
+	SSI_LOG_DEBUG(" handle additional data config set to   DLLI\n");
 	/* create sg for the current buffer */
 	sg_init_one(&areq_ctx->ccm_adata_sg, config_data, AES_BLOCK_SIZE + areq_ctx->ccm_hdr_size);
-	if (unlikely(dma_map_sg(dev, &areq_ctx->ccm_adata_sg, 1, 
+	if (unlikely(dma_map_sg(dev, &areq_ctx->ccm_adata_sg, 1,
 				DMA_TO_DEVICE) != 1)) {
 			SSI_LOG_ERR("dma_map_sg() "
 			   "config buffer failed\n");
 			return -ENOMEM;
 	}
 	SSI_LOG_DEBUG("Mapped curr_buff: dma_address=0x%llX "
-		     "page_link=0x%08lX addr=%pK "
+		     "page=%p addr=%pK "
 		     "offset=%u length=%u\n",
-		     (unsigned long long)sg_dma_address(&areq_ctx->ccm_adata_sg), 
-		     areq_ctx->ccm_adata_sg.page_link, 
+		     (unsigned long long)sg_dma_address(&areq_ctx->ccm_adata_sg),
+		     sg_page(&areq_ctx->ccm_adata_sg),
 		     sg_virt(&areq_ctx->ccm_adata_sg),
-		     areq_ctx->ccm_adata_sg.offset, 
+		     areq_ctx->ccm_adata_sg.offset,
 		     areq_ctx->ccm_adata_sg.length);
 	/* prepare for case of MLLI */
 	if (assoclen > 0) {
-		ssi_buffer_mgr_add_scatterlist_entry(sg_data, 1, 
+		ssi_buffer_mgr_add_scatterlist_entry(sg_data, 1,
 						    &areq_ctx->ccm_adata_sg,
-						    (AES_BLOCK_SIZE + 
+						    (AES_BLOCK_SIZE +
 						    areq_ctx->ccm_hdr_size), 0,
 						    false, NULL);
 	}
 	return 0;
 }
 
-
 static inline int ssi_ahash_handle_curr_buf(struct device *dev,
 					   struct ahash_req_ctx *areq_ctx,
-					   uint8_t* curr_buff,
-					   uint32_t curr_buff_cnt,
+					   u8 *curr_buff,
+					   u32 curr_buff_cnt,
 					   struct buffer_array *sg_data)
 {
-	SSI_LOG_DEBUG(" handle curr buff %x set to   DLLI \n", curr_buff_cnt);
+	SSI_LOG_DEBUG(" handle curr buff %x set to   DLLI\n", curr_buff_cnt);
 	/* create sg for the current buffer */
-	sg_init_one(areq_ctx->buff_sg,curr_buff, curr_buff_cnt);
+	sg_init_one(areq_ctx->buff_sg, curr_buff, curr_buff_cnt);
 	if (unlikely(dma_map_sg(dev, areq_ctx->buff_sg, 1,
 				DMA_TO_DEVICE) != 1)) {
 			SSI_LOG_ERR("dma_map_sg() "
@@ -541,12 +457,12 @@ static inline int ssi_ahash_handle_curr_buf(struct device *dev,
 			return -ENOMEM;
 	}
 	SSI_LOG_DEBUG("Mapped curr_buff: dma_address=0x%llX "
-		     "page_link=0x%08lX addr=%pK "
+		     "page=%p addr=%pK "
 		     "offset=%u length=%u\n",
-		     (unsigned long long)sg_dma_address(areq_ctx->buff_sg), 
-		     areq_ctx->buff_sg->page_link, 
+		     (unsigned long long)sg_dma_address(areq_ctx->buff_sg),
+		     sg_page(areq_ctx->buff_sg),
 		     sg_virt(areq_ctx->buff_sg),
-		     areq_ctx->buff_sg->offset, 
+		     areq_ctx->buff_sg->offset,
 		     areq_ctx->buff_sg->length);
 	areq_ctx->data_dma_buf_type = SSI_DMA_BUF_DLLI;
 	areq_ctx->curr_sg = areq_ctx->buff_sg;
@@ -567,32 +483,28 @@ void ssi_buffer_mgr_unmap_blkcipher_request(
 	struct blkcipher_req_ctx *req_ctx = (struct blkcipher_req_ctx *)ctx;
 
 	if (likely(req_ctx->gen_ctx.iv_dma_addr != 0)) {
-		SSI_LOG_DEBUG("Unmapped iv: iv_dma_addr=0x%llX iv_size=%u\n", 
+		SSI_LOG_DEBUG("Unmapped iv: iv_dma_addr=0x%llX iv_size=%u\n",
 			(unsigned long long)req_ctx->gen_ctx.iv_dma_addr,
 			ivsize);
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(req_ctx->gen_ctx.iv_dma_addr);
-		dma_unmap_single(dev, req_ctx->gen_ctx.iv_dma_addr, 
-				 ivsize, 
+		dma_unmap_single(dev, req_ctx->gen_ctx.iv_dma_addr,
+				 ivsize,
 				 req_ctx->is_giv ? DMA_BIDIRECTIONAL :
 				 DMA_TO_DEVICE);
 	}
 	/* Release pool */
 	if (req_ctx->dma_buf_type == SSI_DMA_BUF_MLLI) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(req_ctx->mlli_params.mlli_dma_addr);
 		dma_pool_free(req_ctx->mlli_params.curr_pool,
 			      req_ctx->mlli_params.mlli_virt_addr,
 			      req_ctx->mlli_params.mlli_dma_addr);
 	}
 
-	SSI_RESTORE_DMA_ADDR_TO_48BIT(sg_dma_address(src));
 	dma_unmap_sg(dev, src, req_ctx->in_nents,
 		DMA_BIDIRECTIONAL);
-	SSI_LOG_DEBUG("Unmapped req->src=%pK\n", 
+	SSI_LOG_DEBUG("Unmapped req->src=%pK\n",
 		     sg_virt(src));
 
 	if (src != dst) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(sg_dma_address(dst));
-		dma_unmap_sg(dev, dst, req_ctx->out_nents, 
+		dma_unmap_sg(dev, dst, req_ctx->out_nents,
 			DMA_BIDIRECTIONAL);
 		SSI_LOG_DEBUG("Unmapped req->dst=%pK\n",
 			sg_virt(dst));
@@ -609,40 +521,39 @@ int ssi_buffer_mgr_map_blkcipher_request(
 	struct scatterlist *dst)
 {
 	struct blkcipher_req_ctx *req_ctx = (struct blkcipher_req_ctx *)ctx;
-	struct mlli_params *mlli_params = &req_ctx->mlli_params;	
+	struct mlli_params *mlli_params = &req_ctx->mlli_params;
 	struct buff_mgr_handle *buff_mgr = drvdata->buff_mgr_handle;
 	struct device *dev = &drvdata->plat_dev->dev;
 	struct buffer_array sg_data;
-	uint32_t dummy = 0;
+	u32 dummy = 0;
 	int rc = 0;
-	uint32_t mapped_nents = 0;
+	u32 mapped_nents = 0;
 
 	req_ctx->dma_buf_type = SSI_DMA_BUF_DLLI;
 	mlli_params->curr_pool = NULL;
 	sg_data.num_of_buffers = 0;
 
 	/* Map IV buffer */
-	if (likely(ivsize != 0) ) {
-		dump_byte_array("iv", (uint8_t *)info, ivsize);
-		req_ctx->gen_ctx.iv_dma_addr = 
-			dma_map_single(dev, (void *)info, 
-				       ivsize, 
-				       req_ctx->is_giv ? DMA_BIDIRECTIONAL:
+	if (likely(ivsize != 0)) {
+		dump_byte_array("iv", (u8 *)info, ivsize);
+		req_ctx->gen_ctx.iv_dma_addr =
+			dma_map_single(dev, (void *)info,
+				       ivsize,
+				       req_ctx->is_giv ? DMA_BIDIRECTIONAL :
 				       DMA_TO_DEVICE);
-		if (unlikely(dma_mapping_error(dev, 
+		if (unlikely(dma_mapping_error(dev,
 					req_ctx->gen_ctx.iv_dma_addr))) {
 			SSI_LOG_ERR("Mapping iv %u B at va=%pK "
 				   "for DMA failed\n", ivsize, info);
 			return -ENOMEM;
 		}
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(req_ctx->gen_ctx.iv_dma_addr,
-								ivsize);
 		SSI_LOG_DEBUG("Mapped iv %u B at va=%pK to dma=0x%llX\n",
 			ivsize, info,
 			(unsigned long long)req_ctx->gen_ctx.iv_dma_addr);
-	} else
+	} else {
 		req_ctx->gen_ctx.iv_dma_addr = 0;
-	
+	}
+
 	/* Map the src SGL */
 	rc = ssi_buffer_mgr_map_scatterlist(dev, src,
 		nbytes, DMA_BIDIRECTIONAL, &req_ctx->in_nents,
@@ -665,7 +576,7 @@ int ssi_buffer_mgr_map_blkcipher_request(
 	} else {
 		/* Map the dst sg */
 		if (unlikely(ssi_buffer_mgr_map_scatterlist(
-			dev,dst, nbytes,
+			dev, dst, nbytes,
 			DMA_BIDIRECTIONAL, &req_ctx->out_nents,
 			LLI_MAX_NUM_OF_DATA_ENTRIES, &dummy,
 			&mapped_nents))){
@@ -682,17 +593,16 @@ int ssi_buffer_mgr_map_blkcipher_request(
 				&req_ctx->in_mlli_nents);
 			ssi_buffer_mgr_add_scatterlist_entry(&sg_data,
 				req_ctx->out_nents, dst,
-				nbytes, 0, true, 
+				nbytes, 0, true,
 				&req_ctx->out_mlli_nents);
 		}
 	}
-	
+
 	if (unlikely(req_ctx->dma_buf_type == SSI_DMA_BUF_MLLI)) {
 		mlli_params->curr_pool = buff_mgr->mlli_buffs_pool;
 		rc = ssi_buffer_mgr_generate_mlli(dev, &sg_data, mlli_params);
-		if (unlikely(rc!= 0))
+		if (unlikely(rc != 0))
 			goto ablkcipher_exit;
-
 	}
 
 	SSI_LOG_DEBUG("areq_ctx->dma_buf_type = %s\n",
@@ -711,39 +621,35 @@ void ssi_buffer_mgr_unmap_aead_request(
 	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
 	unsigned int hw_iv_size = areq_ctx->hw_iv_size;
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
-	uint32_t dummy;
+	struct ssi_drvdata *drvdata = dev_get_drvdata(dev);
+	u32 dummy;
 	bool chained;
-	uint32_t size_to_unmap = 0;
+	u32 size_to_unmap = 0;
 
 	if (areq_ctx->mac_buf_dma_addr != 0) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->mac_buf_dma_addr);
-		dma_unmap_single(dev, areq_ctx->mac_buf_dma_addr, 
+		dma_unmap_single(dev, areq_ctx->mac_buf_dma_addr,
 			MAX_MAC_SIZE, DMA_BIDIRECTIONAL);
 	}
 
 #if SSI_CC_HAS_AES_GCM
 	if (areq_ctx->cipher_mode == DRV_CIPHER_GCTR) {
 		if (areq_ctx->hkey_dma_addr != 0) {
-			SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->hkey_dma_addr);
 			dma_unmap_single(dev, areq_ctx->hkey_dma_addr,
 					 AES_BLOCK_SIZE, DMA_BIDIRECTIONAL);
 		}
-	
+
 		if (areq_ctx->gcm_block_len_dma_addr != 0) {
-			SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->gcm_block_len_dma_addr);
 			dma_unmap_single(dev, areq_ctx->gcm_block_len_dma_addr,
 					 AES_BLOCK_SIZE, DMA_TO_DEVICE);
 		}
-	
+
 		if (areq_ctx->gcm_iv_inc1_dma_addr != 0) {
-			SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->gcm_iv_inc1_dma_addr);
-			dma_unmap_single(dev, areq_ctx->gcm_iv_inc1_dma_addr, 
+			dma_unmap_single(dev, areq_ctx->gcm_iv_inc1_dma_addr,
 				AES_BLOCK_SIZE, DMA_TO_DEVICE);
 		}
-	
+
 		if (areq_ctx->gcm_iv_inc2_dma_addr != 0) {
-			SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->gcm_iv_inc2_dma_addr);
-			dma_unmap_single(dev, areq_ctx->gcm_iv_inc2_dma_addr, 
+			dma_unmap_single(dev, areq_ctx->gcm_iv_inc2_dma_addr,
 				AES_BLOCK_SIZE, DMA_TO_DEVICE);
 		}
 	}
@@ -751,93 +657,87 @@ void ssi_buffer_mgr_unmap_aead_request(
 
 	if (areq_ctx->ccm_hdr_size != ccm_header_size_null) {
 		if (areq_ctx->ccm_iv0_dma_addr != 0) {
-			SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->ccm_iv0_dma_addr);
-			dma_unmap_single(dev, areq_ctx->ccm_iv0_dma_addr, 
+			dma_unmap_single(dev, areq_ctx->ccm_iv0_dma_addr,
 				AES_BLOCK_SIZE, DMA_TO_DEVICE);
 		}
 
 		dma_unmap_sg(dev, &areq_ctx->ccm_adata_sg, 1, DMA_TO_DEVICE);
 	}
 	if (areq_ctx->gen_ctx.iv_dma_addr != 0) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->gen_ctx.iv_dma_addr);
 		dma_unmap_single(dev, areq_ctx->gen_ctx.iv_dma_addr,
 				 hw_iv_size, DMA_BIDIRECTIONAL);
 	}
 
-	/*In case a pool was set, a table was 
-	  allocated and should be released */
-	if (areq_ctx->mlli_params.curr_pool != NULL) {
-		SSI_LOG_DEBUG("free MLLI buffer: dma=0x%08llX virt=%pK\n", 
+	/*In case a pool was set, a table was
+	 *allocated and should be released
+	 */
+	if (areq_ctx->mlli_params.curr_pool) {
+		SSI_LOG_DEBUG("free MLLI buffer: dma=0x%08llX virt=%pK\n",
 			(unsigned long long)areq_ctx->mlli_params.mlli_dma_addr,
 			areq_ctx->mlli_params.mlli_virt_addr);
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->mlli_params.mlli_dma_addr);
 		dma_pool_free(areq_ctx->mlli_params.curr_pool,
 			      areq_ctx->mlli_params.mlli_virt_addr,
 			      areq_ctx->mlli_params.mlli_dma_addr);
 	}
 
-	SSI_LOG_DEBUG("Unmapping src sgl: req->src=%pK areq_ctx->src.nents=%u areq_ctx->assoc.nents=%u assoclen:%u cryptlen=%u\n", sg_virt(req->src),areq_ctx->src.nents,areq_ctx->assoc.nents,req->assoclen,req->cryptlen);
-	SSI_RESTORE_DMA_ADDR_TO_48BIT(sg_dma_address(req->src));
-	size_to_unmap = req->assoclen+req->cryptlen;
-	if(areq_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_ENCRYPT){
+	SSI_LOG_DEBUG("Unmapping src sgl: req->src=%pK areq_ctx->src.nents=%u areq_ctx->assoc.nents=%u assoclen:%u cryptlen=%u\n", sg_virt(req->src), areq_ctx->src.nents, areq_ctx->assoc.nents, req->assoclen, req->cryptlen);
+	size_to_unmap = req->assoclen + req->cryptlen;
+	if (areq_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_ENCRYPT)
 		size_to_unmap += areq_ctx->req_authsize;
-	}
 	if (areq_ctx->is_gcm4543)
 		size_to_unmap += crypto_aead_ivsize(tfm);
 
-	dma_unmap_sg(dev, req->src, ssi_buffer_mgr_get_sgl_nents(req->src,size_to_unmap,&dummy,&chained) , DMA_BIDIRECTIONAL);
+	dma_unmap_sg(dev, req->src, ssi_buffer_mgr_get_sgl_nents(req->src, size_to_unmap, &dummy, &chained), DMA_BIDIRECTIONAL);
 	if (unlikely(req->src != req->dst)) {
-		SSI_LOG_DEBUG("Unmapping dst sgl: req->dst=%pK\n", 
+		SSI_LOG_DEBUG("Unmapping dst sgl: req->dst=%pK\n",
 			sg_virt(req->dst));
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(sg_dma_address(req->dst));
-		dma_unmap_sg(dev, req->dst, ssi_buffer_mgr_get_sgl_nents(req->dst,size_to_unmap,&dummy,&chained),
+		dma_unmap_sg(dev, req->dst, ssi_buffer_mgr_get_sgl_nents(req->dst, size_to_unmap, &dummy, &chained),
 			DMA_BIDIRECTIONAL);
 	}
-#if DX_HAS_ACP
-	if ((areq_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_DECRYPT) &&
+	if (drvdata->coherent &&
+	    (areq_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_DECRYPT) &&
 	    likely(req->src == req->dst))
 	{
-		uint32_t size_to_skip = req->assoclen;
-		if (areq_ctx->is_gcm4543) {
+		u32 size_to_skip = req->assoclen;
+
+		if (areq_ctx->is_gcm4543)
 			size_to_skip += crypto_aead_ivsize(tfm);
-		}
+
 		/* copy mac to a temporary location to deal with possible
-		  data memory overriding that caused by cache coherence problem. */
+		 * data memory overriding that caused by cache coherence problem.
+		 */
 		ssi_buffer_mgr_copy_scatterlist_portion(
 			areq_ctx->backup_mac, req->src,
-			size_to_skip+ req->cryptlen - areq_ctx->req_authsize,
-			size_to_skip+ req->cryptlen, SSI_SG_FROM_BUF);
+			size_to_skip + req->cryptlen - areq_ctx->req_authsize,
+			size_to_skip + req->cryptlen, SSI_SG_FROM_BUF);
 	}
-#endif
 }
 
 static inline int ssi_buffer_mgr_get_aead_icv_nents(
 	struct scatterlist *sgl,
 	unsigned int sgl_nents,
 	unsigned int authsize,
-	uint32_t last_entry_data_size,
+	u32 last_entry_data_size,
 	bool *is_icv_fragmented)
 {
 	unsigned int icv_max_size = 0;
 	unsigned int icv_required_size = authsize > last_entry_data_size ? (authsize - last_entry_data_size) : authsize;
 	unsigned int nents;
 	unsigned int i;
-	
+
 	if (sgl_nents < MAX_ICV_NENTS_SUPPORTED) {
 		*is_icv_fragmented = false;
 		return 0;
 	}
-	
-	for( i = 0 ; i < (sgl_nents - MAX_ICV_NENTS_SUPPORTED) ; i++) {
-		if (sgl == NULL) {
+
+	for (i = 0 ; i < (sgl_nents - MAX_ICV_NENTS_SUPPORTED) ; i++) {
+		if (!sgl)
 			break;
-		}
 		sgl = sg_next(sgl);
 	}
 
-	if (sgl != NULL) {
+	if (sgl)
 		icv_max_size = sgl->length;
-	}
 
 	if (last_entry_data_size > authsize) {
 		nents = 0; /* ICV attached to data in last entry (not fragmented!) */
@@ -873,7 +773,7 @@ static inline int ssi_buffer_mgr_aead_chain_iv(
 	struct device *dev = &drvdata->plat_dev->dev;
 	int rc = 0;
 
-	if (unlikely(req->iv == NULL)) {
+	if (unlikely(!req->iv)) {
 		areq_ctx->gen_ctx.iv_dma_addr = 0;
 		goto chain_iv_exit;
 	}
@@ -884,14 +784,13 @@ static inline int ssi_buffer_mgr_aead_chain_iv(
 		SSI_LOG_ERR("Mapping iv %u B at va=%pK for DMA failed\n",
 			hw_iv_size, req->iv);
 		rc = -ENOMEM;
-		goto chain_iv_exit; 
+		goto chain_iv_exit;
 	}
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->gen_ctx.iv_dma_addr, hw_iv_size);
 
 	SSI_LOG_DEBUG("Mapped iv %u B at va=%pK to dma=0x%llX\n",
-		hw_iv_size, req->iv, 
+		hw_iv_size, req->iv,
 		(unsigned long long)areq_ctx->gen_ctx.iv_dma_addr);
-	if (do_chain == true && areq_ctx->plaintext_authenticate_only == true){  // TODO: what about CTR?? ask Ron
+	if (do_chain && areq_ctx->plaintext_authenticate_only) {  // TODO: what about CTR?? ask Ron
 		struct crypto_aead *tfm = crypto_aead_reqtfm(req);
 		unsigned int iv_size_to_authenc = crypto_aead_ivsize(tfm);
 		unsigned int iv_ofs = GCM_BLOCK_RFC4_IV_OFFSET;
@@ -915,17 +814,16 @@ static inline int ssi_buffer_mgr_aead_chain_assoc(
 {
 	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
 	int rc = 0;
-	uint32_t mapped_nents = 0;
+	u32 mapped_nents = 0;
 	struct scatterlist *current_sg = req->src;
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
 	unsigned int sg_index = 0;
-	uint32_t size_of_assoc = req->assoclen;
+	u32 size_of_assoc = req->assoclen;
 
-	if (areq_ctx->is_gcm4543) {
+	if (areq_ctx->is_gcm4543)
 		size_of_assoc += crypto_aead_ivsize(tfm);
-	}
 
-	if (sg_data == NULL) {
+	if (!sg_data) {
 		rc = -EINVAL;
 		goto chain_assoc_exit;
 	}
@@ -944,14 +842,13 @@ static inline int ssi_buffer_mgr_aead_chain_assoc(
 	//it is assumed that if we reach here , the sgl is already mapped
 	sg_index = current_sg->length;
 	if (sg_index > size_of_assoc) { //the first entry in the scatter list contains all the associated data
-		mapped_nents++;        
-	}
-	else{
+		mapped_nents++;
+	} else {
 		while (sg_index <= size_of_assoc) {
 			current_sg = sg_next(current_sg);
 			//if have reached the end of the sgl, then this is unexpected
-			if (current_sg == NULL) {
-				SSI_LOG_ERR("reached end of sg list. unexpected \n");
+			if (!current_sg) {
+				SSI_LOG_ERR("reached end of sg list. unexpected\n");
 				BUG();
 			}
 			sg_index += current_sg->length;
@@ -966,11 +863,11 @@ static inline int ssi_buffer_mgr_aead_chain_assoc(
 	areq_ctx->assoc.nents = mapped_nents;
 
 	/* in CCM case we have additional entry for
-	*  ccm header configurations */
+	 * ccm header configurations
+	 */
 	if (areq_ctx->ccm_hdr_size != ccm_header_size_null) {
 		if (unlikely((mapped_nents + 1) >
 			LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES)) {
-
 			SSI_LOG_ERR("CCM case.Too many fragments. "
 				"Current %d max %d\n",
 				(areq_ctx->assoc.nents + 1),
@@ -986,9 +883,8 @@ static inline int ssi_buffer_mgr_aead_chain_assoc(
 	else
 		areq_ctx->assoc_buff_type = SSI_DMA_BUF_MLLI;
 
-	if (unlikely((do_chain == true) ||
+	if (unlikely((do_chain) ||
 		(areq_ctx->assoc_buff_type == SSI_DMA_BUF_MLLI))) {
-
 		SSI_LOG_DEBUG("Chain assoc: buff_type=%s nents=%u\n",
 			GET_DMA_BUFFER_TYPE(areq_ctx->assoc_buff_type),
 			areq_ctx->assoc.nents);
@@ -1005,7 +901,7 @@ chain_assoc_exit:
 
 static inline void ssi_buffer_mgr_prepare_aead_data_dlli(
 	struct aead_request *req,
-	uint32_t *src_last_bytes, uint32_t *dst_last_bytes)
+	u32 *src_last_bytes, u32 *dst_last_bytes)
 {
 	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
 	enum drv_crypto_direction direct = areq_ctx->gen_ctx.op_type;
@@ -1015,7 +911,7 @@ static inline void ssi_buffer_mgr_prepare_aead_data_dlli(
 	if (likely(req->src == req->dst)) {
 		/*INPLACE*/
 		areq_ctx->icv_dma_addr = sg_dma_address(
-			areq_ctx->srcSgl)+
+			areq_ctx->srcSgl) +
 			(*src_last_bytes - authsize);
 		areq_ctx->icv_virt_addr = sg_virt(
 			areq_ctx->srcSgl) +
@@ -1034,7 +930,7 @@ static inline void ssi_buffer_mgr_prepare_aead_data_dlli(
 			areq_ctx->dstSgl) +
 			(*dst_last_bytes - authsize);
 		areq_ctx->icv_virt_addr = sg_virt(
-			areq_ctx->dstSgl)+
+			areq_ctx->dstSgl) +
 			(*dst_last_bytes - authsize);
 	}
 }
@@ -1043,7 +939,7 @@ static inline int ssi_buffer_mgr_prepare_aead_data_mlli(
 	struct ssi_drvdata *drvdata,
 	struct aead_request *req,
 	struct buffer_array *sg_data,
-	uint32_t *src_last_bytes, uint32_t *dst_last_bytes,
+	u32 *src_last_bytes, u32 *dst_last_bytes,
 	bool is_last_table)
 {
 	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
@@ -1056,7 +952,7 @@ static inline int ssi_buffer_mgr_prepare_aead_data_mlli(
 		/*INPLACE*/
 		ssi_buffer_mgr_add_scatterlist_entry(sg_data,
 			areq_ctx->src.nents, areq_ctx->srcSgl,
-			areq_ctx->cryptlen,areq_ctx->srcOffset, is_last_table,
+			areq_ctx->cryptlen, areq_ctx->srcOffset, is_last_table,
 			&areq_ctx->src.mlli_nents);
 
 		icv_nents = ssi_buffer_mgr_get_aead_icv_nents(areq_ctx->srcSgl,
@@ -1067,24 +963,30 @@ static inline int ssi_buffer_mgr_prepare_aead_data_mlli(
 			goto prepare_data_mlli_exit;
 		}
 
-		if (unlikely(areq_ctx->is_icv_fragmented == true)) {
+		if (unlikely(areq_ctx->is_icv_fragmented)) {
 			/* Backup happens only when ICV is fragmented, ICV
-			   verification is made by CPU compare in order to simplify
-			   MAC verification upon request completion */
+			 * verification is made by CPU compare in order to simplify
+			 * MAC verification upon request completion
+			 */
 			if (direct == DRV_CRYPTO_DIRECTION_DECRYPT) {
-#if !DX_HAS_ACP
-				/* In ACP platform we already copying ICV
-				   for any INPLACE-DECRYPT operation, hence
-				   we must neglect this code. */
-				uint32_t size_to_skip = req->assoclen;
-				if (areq_ctx->is_gcm4543) {
-					size_to_skip += crypto_aead_ivsize(tfm);
+				if (!drvdata->coherent) {
+				/* In coherent platforms (e.g. ACP)
+				 * already copying ICV for any
+				 * INPLACE-DECRYPT operation, hence
+				 * we must neglect this code.
+				 */
+					u32 skip = req->assoclen;
+
+					if (areq_ctx->is_gcm4543)
+						skip += crypto_aead_ivsize(tfm);
+
+					ssi_buffer_mgr_copy_scatterlist_portion(
+						areq_ctx->backup_mac, req->src,
+						(skip + req->cryptlen -
+						 areq_ctx->req_authsize),
+						skip + req->cryptlen,
+						SSI_SG_TO_BUF);
 				}
-				ssi_buffer_mgr_copy_scatterlist_portion(
-					areq_ctx->backup_mac, req->src,
-					size_to_skip+ req->cryptlen - areq_ctx->req_authsize,
-					size_to_skip+ req->cryptlen, SSI_SG_TO_BUF);
-#endif
 				areq_ctx->icv_virt_addr = areq_ctx->backup_mac;
 			} else {
 				areq_ctx->icv_virt_addr = areq_ctx->mac_buf;
@@ -1096,7 +998,7 @@ static inline int ssi_buffer_mgr_prepare_aead_data_mlli(
 				&areq_ctx->srcSgl[areq_ctx->src.nents - 1]) +
 				(*src_last_bytes - authsize);
 			areq_ctx->icv_virt_addr = sg_virt(
-				&areq_ctx->srcSgl[areq_ctx->src.nents - 1]) + 
+				&areq_ctx->srcSgl[areq_ctx->src.nents - 1]) +
 				(*src_last_bytes - authsize);
 		}
 
@@ -1104,11 +1006,11 @@ static inline int ssi_buffer_mgr_prepare_aead_data_mlli(
 		/*NON-INPLACE and DECRYPT*/
 		ssi_buffer_mgr_add_scatterlist_entry(sg_data,
 			areq_ctx->src.nents, areq_ctx->srcSgl,
-			areq_ctx->cryptlen, areq_ctx->srcOffset,is_last_table,
+			areq_ctx->cryptlen, areq_ctx->srcOffset, is_last_table,
 			&areq_ctx->src.mlli_nents);
 		ssi_buffer_mgr_add_scatterlist_entry(sg_data,
 			areq_ctx->dst.nents, areq_ctx->dstSgl,
-			areq_ctx->cryptlen,areq_ctx->dstOffset, is_last_table,
+			areq_ctx->cryptlen, areq_ctx->dstOffset, is_last_table,
 			&areq_ctx->dst.mlli_nents);
 
 		icv_nents = ssi_buffer_mgr_get_aead_icv_nents(areq_ctx->srcSgl,
@@ -1119,18 +1021,20 @@ static inline int ssi_buffer_mgr_prepare_aead_data_mlli(
 			goto prepare_data_mlli_exit;
 		}
 
-		if (unlikely(areq_ctx->is_icv_fragmented == true)) {
+		if (unlikely(areq_ctx->is_icv_fragmented)) {
 			/* Backup happens only when ICV is fragmented, ICV
-			   verification is made by CPU compare in order to simplify
-			   MAC verification upon request completion */
-			  uint32_t size_to_skip = req->assoclen;
-			  if (areq_ctx->is_gcm4543) {
+			 * verification is made by CPU compare in order to simplify
+			 * MAC verification upon request completion
+			 */
+			  u32 size_to_skip = req->assoclen;
+
+			  if (areq_ctx->is_gcm4543)
 				  size_to_skip += crypto_aead_ivsize(tfm);
-			  }
+
 			  ssi_buffer_mgr_copy_scatterlist_portion(
 				  areq_ctx->backup_mac, req->src,
-				  size_to_skip+ req->cryptlen - areq_ctx->req_authsize,
-				  size_to_skip+ req->cryptlen, SSI_SG_TO_BUF);
+				  size_to_skip + req->cryptlen - areq_ctx->req_authsize,
+				  size_to_skip + req->cryptlen, SSI_SG_TO_BUF);
 			areq_ctx->icv_virt_addr = areq_ctx->backup_mac;
 		} else { /* Contig. ICV */
 			/*Should hanlde if the sg is not contig.*/
@@ -1146,11 +1050,11 @@ static inline int ssi_buffer_mgr_prepare_aead_data_mlli(
 		/*NON-INPLACE and ENCRYPT*/
 		ssi_buffer_mgr_add_scatterlist_entry(sg_data,
 			areq_ctx->dst.nents, areq_ctx->dstSgl,
-			areq_ctx->cryptlen,areq_ctx->dstOffset, is_last_table,
+			areq_ctx->cryptlen, areq_ctx->dstOffset, is_last_table,
 			&areq_ctx->dst.mlli_nents);
 		ssi_buffer_mgr_add_scatterlist_entry(sg_data,
 			areq_ctx->src.nents, areq_ctx->srcSgl,
-			areq_ctx->cryptlen, areq_ctx->srcOffset,is_last_table,
+			areq_ctx->cryptlen, areq_ctx->srcOffset, is_last_table,
 			&areq_ctx->src.mlli_nents);
 
 		icv_nents = ssi_buffer_mgr_get_aead_icv_nents(areq_ctx->dstSgl,
@@ -1161,7 +1065,7 @@ static inline int ssi_buffer_mgr_prepare_aead_data_mlli(
 			goto prepare_data_mlli_exit;
 		}
 
-		if (likely(areq_ctx->is_icv_fragmented == false)) {
+		if (likely(!areq_ctx->is_icv_fragmented)) {
 			/* Contig. ICV */
 			areq_ctx->icv_dma_addr = sg_dma_address(
 				&areq_ctx->dstSgl[areq_ctx->dst.nents - 1]) +
@@ -1191,40 +1095,40 @@ static inline int ssi_buffer_mgr_aead_chain_data(
 	unsigned int authsize = areq_ctx->req_authsize;
 	int src_last_bytes = 0, dst_last_bytes = 0;
 	int rc = 0;
-	uint32_t src_mapped_nents = 0, dst_mapped_nents = 0;
-	uint32_t offset = 0;
-	unsigned int size_for_map = req->assoclen +req->cryptlen; /*non-inplace mode*/
+	u32 src_mapped_nents = 0, dst_mapped_nents = 0;
+	u32 offset = 0;
+	unsigned int size_for_map = req->assoclen + req->cryptlen; /*non-inplace mode*/
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
-	uint32_t sg_index = 0;
+	u32 sg_index = 0;
 	bool chained = false;
 	bool is_gcm4543 = areq_ctx->is_gcm4543;
-	uint32_t size_to_skip = req->assoclen;
-	if (is_gcm4543) {
+	u32 size_to_skip = req->assoclen;
+
+	if (is_gcm4543)
 		size_to_skip += crypto_aead_ivsize(tfm);
-	}
+
 	offset = size_to_skip;
 
-	if (sg_data == NULL) {
+	if (!sg_data) {
 		rc = -EINVAL;
 		goto chain_data_exit;
 	}
 	areq_ctx->srcSgl = req->src;
 	areq_ctx->dstSgl = req->dst;
 
-	if (is_gcm4543) {
+	if (is_gcm4543)
 		size_for_map += crypto_aead_ivsize(tfm);
-	}
 
-	size_for_map += (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) ? authsize:0;	
-	src_mapped_nents = ssi_buffer_mgr_get_sgl_nents(req->src,size_for_map,&src_last_bytes, &chained);  
+	size_for_map += (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) ? authsize : 0;
+	src_mapped_nents = ssi_buffer_mgr_get_sgl_nents(req->src, size_for_map, &src_last_bytes, &chained);
 	sg_index = areq_ctx->srcSgl->length;
 	//check where the data starts
 	while (sg_index <= size_to_skip) {
 		offset -= areq_ctx->srcSgl->length;
 		areq_ctx->srcSgl = sg_next(areq_ctx->srcSgl);
 		//if have reached the end of the sgl, then this is unexpected
-		if (areq_ctx->srcSgl == NULL) {
-			SSI_LOG_ERR("reached end of sg list. unexpected \n");
+		if (!areq_ctx->srcSgl) {
+			SSI_LOG_ERR("reached end of sg list. unexpected\n");
 			BUG();
 		}
 		sg_index += areq_ctx->srcSgl->length;
@@ -1239,14 +1143,13 @@ static inline int ssi_buffer_mgr_aead_chain_data(
 
 	areq_ctx->src.nents = src_mapped_nents;
 
-	areq_ctx->srcOffset = offset;  
+	areq_ctx->srcOffset = offset;
 
 	if (req->src != req->dst) {
-		size_for_map = req->assoclen +req->cryptlen;
+		size_for_map = req->assoclen + req->cryptlen;
 		size_for_map += (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) ? authsize : 0;
-		if (is_gcm4543) {
+		if (is_gcm4543)
 			size_for_map += crypto_aead_ivsize(tfm);
-		}
 
 		rc = ssi_buffer_mgr_map_scatterlist(dev, req->dst, size_for_map,
 			 DMA_BIDIRECTIONAL, &(areq_ctx->dst.nents),
@@ -1254,22 +1157,21 @@ static inline int ssi_buffer_mgr_aead_chain_data(
 						   &dst_mapped_nents);
 		if (unlikely(rc != 0)) {
 			rc = -ENOMEM;
-			goto chain_data_exit; 
+			goto chain_data_exit;
 		}
 	}
 
-	dst_mapped_nents = ssi_buffer_mgr_get_sgl_nents(req->dst,size_for_map,&dst_last_bytes, &chained);
+	dst_mapped_nents = ssi_buffer_mgr_get_sgl_nents(req->dst, size_for_map, &dst_last_bytes, &chained);
 	sg_index = areq_ctx->dstSgl->length;
 	offset = size_to_skip;
 
 	//check where the data starts
 	while (sg_index <= size_to_skip) {
-
 		offset -= areq_ctx->dstSgl->length;
 		areq_ctx->dstSgl = sg_next(areq_ctx->dstSgl);
 		//if have reached the end of the sgl, then this is unexpected
-		if (areq_ctx->dstSgl == NULL) {
-			SSI_LOG_ERR("reached end of sg list. unexpected \n");
+		if (!areq_ctx->dstSgl) {
+			SSI_LOG_ERR("reached end of sg list. unexpected\n");
 			BUG();
 		}
 		sg_index += areq_ctx->dstSgl->length;
@@ -1285,7 +1187,7 @@ static inline int ssi_buffer_mgr_aead_chain_data(
 	areq_ctx->dstOffset = offset;
 	if ((src_mapped_nents > 1) ||
 	    (dst_mapped_nents  > 1) ||
-	    (do_chain == true)) {
+	    do_chain) {
 		areq_ctx->data_buff_type = SSI_DMA_BUF_MLLI;
 		rc = ssi_buffer_mgr_prepare_aead_data_mlli(drvdata, req, sg_data,
 			&src_last_bytes, &dst_last_bytes, is_last_table);
@@ -1299,15 +1201,15 @@ chain_data_exit:
 	return rc;
 }
 
-static void ssi_buffer_mgr_update_aead_mlli_nents( struct ssi_drvdata *drvdata,
+static void ssi_buffer_mgr_update_aead_mlli_nents(struct ssi_drvdata *drvdata,
 					   struct aead_request *req)
 {
 	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
-	uint32_t curr_mlli_size = 0;
-	
+	u32 curr_mlli_size = 0;
+
 	if (areq_ctx->assoc_buff_type == SSI_DMA_BUF_MLLI) {
 		areq_ctx->assoc.sram_addr = drvdata->mlli_sram_addr;
-		curr_mlli_size = areq_ctx->assoc.mlli_nents * 
+		curr_mlli_size = areq_ctx->assoc.mlli_nents *
 						LLI_ENTRY_BYTE_SIZE;
 	}
 
@@ -1318,32 +1220,32 @@ static void ssi_buffer_mgr_update_aead_mlli_nents( struct ssi_drvdata *drvdata,
 			areq_ctx->src.sram_addr = drvdata->mlli_sram_addr +
 								curr_mlli_size;
 			areq_ctx->dst.sram_addr = areq_ctx->src.sram_addr;
-			if (areq_ctx->is_single_pass == false)
-				areq_ctx->assoc.mlli_nents += 
+			if (!areq_ctx->is_single_pass)
+				areq_ctx->assoc.mlli_nents +=
 					areq_ctx->src.mlli_nents;
 		} else {
-			if (areq_ctx->gen_ctx.op_type == 
+			if (areq_ctx->gen_ctx.op_type ==
 					DRV_CRYPTO_DIRECTION_DECRYPT) {
-				areq_ctx->src.sram_addr = 
+				areq_ctx->src.sram_addr =
 						drvdata->mlli_sram_addr +
 								curr_mlli_size;
-				areq_ctx->dst.sram_addr = 
-						areq_ctx->src.sram_addr + 
-						areq_ctx->src.mlli_nents * 
+				areq_ctx->dst.sram_addr =
+						areq_ctx->src.sram_addr +
+						areq_ctx->src.mlli_nents *
 						LLI_ENTRY_BYTE_SIZE;
-				if (areq_ctx->is_single_pass == false)
-					areq_ctx->assoc.mlli_nents += 
+				if (!areq_ctx->is_single_pass)
+					areq_ctx->assoc.mlli_nents +=
 						areq_ctx->src.mlli_nents;
 			} else {
-				areq_ctx->dst.sram_addr = 
+				areq_ctx->dst.sram_addr =
 						drvdata->mlli_sram_addr +
 								curr_mlli_size;
-				areq_ctx->src.sram_addr = 
+				areq_ctx->src.sram_addr =
 						areq_ctx->dst.sram_addr +
-						areq_ctx->dst.mlli_nents * 
+						areq_ctx->dst.mlli_nents *
 						LLI_ENTRY_BYTE_SIZE;
-				if (areq_ctx->is_single_pass == false)
-					areq_ctx->assoc.mlli_nents += 
+				if (!areq_ctx->is_single_pass)
+					areq_ctx->assoc.mlli_nents +=
 						areq_ctx->dst.mlli_nents;
 			}
 		}
@@ -1363,33 +1265,34 @@ int ssi_buffer_mgr_map_aead_request(
 	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
 	bool is_gcm4543 = areq_ctx->is_gcm4543;
 
-	uint32_t mapped_nents = 0;
-	uint32_t dummy = 0; /*used for the assoc data fragments */
-	uint32_t size_to_map = 0;
+	u32 mapped_nents = 0;
+	u32 dummy = 0; /*used for the assoc data fragments */
+	u32 size_to_map = 0;
 
 	mlli_params->curr_pool = NULL;
 	sg_data.num_of_buffers = 0;
 
-#if DX_HAS_ACP
-	if ((areq_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_DECRYPT) &&
+	if (drvdata->coherent &&
+	    (areq_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_DECRYPT) &&
 	    likely(req->src == req->dst))
 	{
-		uint32_t size_to_skip = req->assoclen;
-		if (is_gcm4543) {
+		u32 size_to_skip = req->assoclen;
+
+		if (is_gcm4543)
 			size_to_skip += crypto_aead_ivsize(tfm);
-		}
+
 		/* copy mac to a temporary location to deal with possible
-		   data memory overriding that caused by cache coherence problem. */
+		 * data memory overriding that caused by cache coherence problem.
+		 */
 		ssi_buffer_mgr_copy_scatterlist_portion(
 			areq_ctx->backup_mac, req->src,
-			size_to_skip+ req->cryptlen - areq_ctx->req_authsize,
-			size_to_skip+ req->cryptlen, SSI_SG_TO_BUF);
+			size_to_skip + req->cryptlen - areq_ctx->req_authsize,
+			size_to_skip + req->cryptlen, SSI_SG_TO_BUF);
 	}
-#endif
 
 	/* cacluate the size for cipher remove ICV in decrypt*/
-	areq_ctx->cryptlen = (areq_ctx->gen_ctx.op_type == 
-				 DRV_CRYPTO_DIRECTION_ENCRYPT) ? 
+	areq_ctx->cryptlen = (areq_ctx->gen_ctx.op_type ==
+				 DRV_CRYPTO_DIRECTION_ENCRYPT) ?
 				req->cryptlen :
 				(req->cryptlen - authsize);
 
@@ -1401,7 +1304,6 @@ int ssi_buffer_mgr_map_aead_request(
 		rc = -ENOMEM;
 		goto aead_map_failure;
 	}
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->mac_buf_dma_addr, MAX_MAC_SIZE);
 
 	if (areq_ctx->ccm_hdr_size != ccm_header_size_null) {
 		areq_ctx->ccm_iv0_dma_addr = dma_map_single(dev,
@@ -1416,8 +1318,6 @@ int ssi_buffer_mgr_map_aead_request(
 			rc = -ENOMEM;
 			goto aead_map_failure;
 		}
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->ccm_iv0_dma_addr,
-								AES_BLOCK_SIZE);
 		if (ssi_aead_handle_config_buf(dev, areq_ctx,
 			areq_ctx->ccm_config, &sg_data, req->assoclen) != 0) {
 			rc = -ENOMEM;
@@ -1435,7 +1335,6 @@ int ssi_buffer_mgr_map_aead_request(
 			rc = -ENOMEM;
 			goto aead_map_failure;
 		}
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->hkey_dma_addr, AES_BLOCK_SIZE);
 
 		areq_ctx->gcm_block_len_dma_addr = dma_map_single(dev,
 			&areq_ctx->gcm_len_block, AES_BLOCK_SIZE, DMA_TO_DEVICE);
@@ -1445,7 +1344,6 @@ int ssi_buffer_mgr_map_aead_request(
 			rc = -ENOMEM;
 			goto aead_map_failure;
 		}
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->gcm_block_len_dma_addr, AES_BLOCK_SIZE);
 
 		areq_ctx->gcm_iv_inc1_dma_addr = dma_map_single(dev,
 			areq_ctx->gcm_iv_inc1,
@@ -1459,8 +1357,6 @@ int ssi_buffer_mgr_map_aead_request(
 			rc = -ENOMEM;
 			goto aead_map_failure;
 		}
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->gcm_iv_inc1_dma_addr,
-								AES_BLOCK_SIZE);
 
 		areq_ctx->gcm_iv_inc2_dma_addr = dma_map_single(dev,
 			areq_ctx->gcm_iv_inc2,
@@ -1474,32 +1370,30 @@ int ssi_buffer_mgr_map_aead_request(
 			rc = -ENOMEM;
 			goto aead_map_failure;
 		}
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->gcm_iv_inc2_dma_addr,
-								AES_BLOCK_SIZE);
 	}
 #endif /*SSI_CC_HAS_AES_GCM*/
 
 	size_to_map = req->cryptlen + req->assoclen;
-	if (areq_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_ENCRYPT) {
+	if (areq_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_ENCRYPT)
 		size_to_map += authsize;
-	}
+
 	if (is_gcm4543)
 		size_to_map += crypto_aead_ivsize(tfm);
 	rc = ssi_buffer_mgr_map_scatterlist(dev, req->src,
 					    size_to_map, DMA_BIDIRECTIONAL, &(areq_ctx->src.nents),
-					    LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES+LLI_MAX_NUM_OF_DATA_ENTRIES, &dummy, &mapped_nents);
+					    LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES + LLI_MAX_NUM_OF_DATA_ENTRIES, &dummy, &mapped_nents);
 	if (unlikely(rc != 0)) {
 		rc = -ENOMEM;
-		goto aead_map_failure; 
+		goto aead_map_failure;
 	}
 
-	if (likely(areq_ctx->is_single_pass == true)) {
+	if (likely(areq_ctx->is_single_pass)) {
 		/*
-		* Create MLLI table for: 
-		*   (1) Assoc. data
-		*   (2) Src/Dst SGLs
-		*   Note: IV is contg. buffer (not an SGL) 
-		*/
+		 * Create MLLI table for:
+		 *   (1) Assoc. data
+		 *   (2) Src/Dst SGLs
+		 *   Note: IV is contg. buffer (not an SGL)
+		 */
 		rc = ssi_buffer_mgr_aead_chain_assoc(drvdata, req, &sg_data, true, false);
 		if (unlikely(rc != 0))
 			goto aead_map_failure;
@@ -1511,25 +1405,25 @@ int ssi_buffer_mgr_map_aead_request(
 			goto aead_map_failure;
 	} else { /* DOUBLE-PASS flow */
 		/*
-		* Prepare MLLI table(s) in this order:
-		*  
-		* If ENCRYPT/DECRYPT (inplace):
-		*   (1) MLLI table for assoc
-		*   (2) IV entry (chained right after end of assoc)
-		*   (3) MLLI for src/dst (inplace operation)
-		*  
-		* If ENCRYPT (non-inplace) 
-		*   (1) MLLI table for assoc
-		*   (2) IV entry (chained right after end of assoc)
-		*   (3) MLLI for dst
-		*   (4) MLLI for src
-		*  
-		* If DECRYPT (non-inplace) 
-		*   (1) MLLI table for assoc
-		*   (2) IV entry (chained right after end of assoc)
-		*   (3) MLLI for src
-		*   (4) MLLI for dst
-		*/
+		 * Prepare MLLI table(s) in this order:
+		 *
+		 * If ENCRYPT/DECRYPT (inplace):
+		 *   (1) MLLI table for assoc
+		 *   (2) IV entry (chained right after end of assoc)
+		 *   (3) MLLI for src/dst (inplace operation)
+		 *
+		 * If ENCRYPT (non-inplace)
+		 *   (1) MLLI table for assoc
+		 *   (2) IV entry (chained right after end of assoc)
+		 *   (3) MLLI for dst
+		 *   (4) MLLI for src
+		 *
+		 * If DECRYPT (non-inplace)
+		 *   (1) MLLI table for assoc
+		 *   (2) IV entry (chained right after end of assoc)
+		 *   (3) MLLI for src
+		 *   (4) MLLI for dst
+		 */
 		rc = ssi_buffer_mgr_aead_chain_assoc(drvdata, req, &sg_data, false, true);
 		if (unlikely(rc != 0))
 			goto aead_map_failure;
@@ -1545,17 +1439,15 @@ int ssi_buffer_mgr_map_aead_request(
 	if (unlikely(
 		(areq_ctx->assoc_buff_type == SSI_DMA_BUF_MLLI) ||
 		(areq_ctx->data_buff_type == SSI_DMA_BUF_MLLI))) {
-
 		mlli_params->curr_pool = buff_mgr->mlli_buffs_pool;
 		rc = ssi_buffer_mgr_generate_mlli(dev, &sg_data, mlli_params);
-		if (unlikely(rc != 0)) {
+		if (unlikely(rc != 0))
 			goto aead_map_failure;
-		}
 
 		ssi_buffer_mgr_update_aead_mlli_nents(drvdata, req);
-		SSI_LOG_DEBUG("assoc params mn %d\n",areq_ctx->assoc.mlli_nents);
-		SSI_LOG_DEBUG("src params mn %d\n",areq_ctx->src.mlli_nents);
-		SSI_LOG_DEBUG("dst params mn %d\n",areq_ctx->dst.mlli_nents);
+		SSI_LOG_DEBUG("assoc params mn %d\n", areq_ctx->assoc.mlli_nents);
+		SSI_LOG_DEBUG("src params mn %d\n", areq_ctx->src.mlli_nents);
+		SSI_LOG_DEBUG("dst params mn %d\n", areq_ctx->dst.mlli_nents);
 	}
 	return 0;
 
@@ -1569,15 +1461,15 @@ int ssi_buffer_mgr_map_hash_request_final(
 {
 	struct ahash_req_ctx *areq_ctx = (struct ahash_req_ctx *)ctx;
 	struct device *dev = &drvdata->plat_dev->dev;
-	uint8_t* curr_buff = areq_ctx->buff_index ? areq_ctx->buff1 :
+	u8 *curr_buff = areq_ctx->buff_index ? areq_ctx->buff1 :
 			areq_ctx->buff0;
-	uint32_t *curr_buff_cnt = areq_ctx->buff_index ? &areq_ctx->buff1_cnt :
+	u32 *curr_buff_cnt = areq_ctx->buff_index ? &areq_ctx->buff1_cnt :
 			&areq_ctx->buff0_cnt;
-	struct mlli_params *mlli_params = &areq_ctx->mlli_params;	
+	struct mlli_params *mlli_params = &areq_ctx->mlli_params;
 	struct buffer_array sg_data;
 	struct buff_mgr_handle *buff_mgr = drvdata->buff_mgr_handle;
-	uint32_t dummy = 0;
-	uint32_t mapped_nents = 0;
+	u32 dummy = 0;
+	u32 mapped_nents = 0;
 
 	SSI_LOG_DEBUG(" final params : curr_buff=%pK "
 		     "curr_buff_cnt=0x%X nbytes = 0x%X "
@@ -1594,10 +1486,10 @@ int ssi_buffer_mgr_map_hash_request_final(
 		/* nothing to do */
 		return 0;
 	}
-	
+
 	/*TODO: copy data in case that buffer is enough for operation */
 	/* map the previous buffer */
-	if (*curr_buff_cnt != 0 ) {
+	if (*curr_buff_cnt != 0) {
 		if (ssi_ahash_handle_curr_buf(dev, areq_ctx, curr_buff,
 					    *curr_buff_cnt, &sg_data) != 0) {
 			return -ENOMEM;
@@ -1605,7 +1497,7 @@ int ssi_buffer_mgr_map_hash_request_final(
 	}
 
 	if (src && (nbytes > 0) && do_update) {
-		if ( unlikely( ssi_buffer_mgr_map_scatterlist( dev,src,
+		if (unlikely(ssi_buffer_mgr_map_scatterlist(dev, src,
 					  nbytes,
 					  DMA_TO_DEVICE,
 					  &areq_ctx->in_nents,
@@ -1613,9 +1505,9 @@ int ssi_buffer_mgr_map_hash_request_final(
 					  &dummy, &mapped_nents))){
 			goto unmap_curr_buff;
 		}
-		if ( src && (mapped_nents == 1) 
-		     && (areq_ctx->data_dma_buf_type == SSI_DMA_BUF_NULL) ) {
-			memcpy(areq_ctx->buff_sg,src,
+		if (src && (mapped_nents == 1)
+		     && (areq_ctx->data_dma_buf_type == SSI_DMA_BUF_NULL)) {
+			memcpy(areq_ctx->buff_sg, src,
 			       sizeof(struct scatterlist));
 			areq_ctx->buff_sg->length = nbytes;
 			areq_ctx->curr_sg = areq_ctx->buff_sg;
@@ -1623,7 +1515,6 @@ int ssi_buffer_mgr_map_hash_request_final(
 		} else {
 			areq_ctx->data_dma_buf_type = SSI_DMA_BUF_MLLI;
 		}
-
 	}
 
 	/*build mlli */
@@ -1641,7 +1532,7 @@ int ssi_buffer_mgr_map_hash_request_final(
 		}
 	}
 	/* change the buffer index for the unmap function */
-	areq_ctx->buff_index = (areq_ctx->buff_index^1);
+	areq_ctx->buff_index = (areq_ctx->buff_index ^ 1);
 	SSI_LOG_DEBUG("areq_ctx->data_dma_buf_type = %s\n",
 		GET_DMA_BUFFER_TYPE(areq_ctx->data_dma_buf_type));
 	return 0;
@@ -1650,9 +1541,9 @@ fail_unmap_din:
 	dma_unmap_sg(dev, src, areq_ctx->in_nents, DMA_TO_DEVICE);
 
 unmap_curr_buff:
-	if (*curr_buff_cnt != 0 ) {
+	if (*curr_buff_cnt != 0)
 		dma_unmap_sg(dev, areq_ctx->buff_sg, 1, DMA_TO_DEVICE);
-	}
+
 	return -ENOMEM;
 }
 
@@ -1661,26 +1552,26 @@ int ssi_buffer_mgr_map_hash_request_update(
 {
 	struct ahash_req_ctx *areq_ctx = (struct ahash_req_ctx *)ctx;
 	struct device *dev = &drvdata->plat_dev->dev;
-	uint8_t* curr_buff = areq_ctx->buff_index ? areq_ctx->buff1 :
+	u8 *curr_buff = areq_ctx->buff_index ? areq_ctx->buff1 :
 			areq_ctx->buff0;
-	uint32_t *curr_buff_cnt = areq_ctx->buff_index ? &areq_ctx->buff1_cnt :
+	u32 *curr_buff_cnt = areq_ctx->buff_index ? &areq_ctx->buff1_cnt :
 			&areq_ctx->buff0_cnt;
-	uint8_t* next_buff = areq_ctx->buff_index ? areq_ctx->buff0 :
+	u8 *next_buff = areq_ctx->buff_index ? areq_ctx->buff0 :
 			areq_ctx->buff1;
-	uint32_t *next_buff_cnt = areq_ctx->buff_index ? &areq_ctx->buff0_cnt :
+	u32 *next_buff_cnt = areq_ctx->buff_index ? &areq_ctx->buff0_cnt :
 			&areq_ctx->buff1_cnt;
-	struct mlli_params *mlli_params = &areq_ctx->mlli_params;	
+	struct mlli_params *mlli_params = &areq_ctx->mlli_params;
 	unsigned int update_data_len;
-	uint32_t total_in_len = nbytes + *curr_buff_cnt;
+	u32 total_in_len = nbytes + *curr_buff_cnt;
 	struct buffer_array sg_data;
 	struct buff_mgr_handle *buff_mgr = drvdata->buff_mgr_handle;
 	unsigned int swap_index = 0;
-	uint32_t dummy = 0;
-	uint32_t mapped_nents = 0;
-		
+	u32 dummy = 0;
+	u32 mapped_nents = 0;
+
 	SSI_LOG_DEBUG(" update params : curr_buff=%pK "
 		     "curr_buff_cnt=0x%X nbytes=0x%X "
-		     "src=%pK curr_index=%u \n",
+		     "src=%pK curr_index=%u\n",
 		     curr_buff, *curr_buff_cnt, nbytes,
 		     src, areq_ctx->buff_index);
 	/* Init the type of the dma buffer */
@@ -1695,12 +1586,12 @@ int ssi_buffer_mgr_map_hash_request_update(
 			     "*curr_buff_cnt=0x%X copy_to=%pK\n",
 			curr_buff, *curr_buff_cnt,
 			&curr_buff[*curr_buff_cnt]);
-		areq_ctx->in_nents = 
+		areq_ctx->in_nents =
 			ssi_buffer_mgr_get_sgl_nents(src,
 						    nbytes,
 						    &dummy, NULL);
 		sg_copy_to_buffer(src, areq_ctx->in_nents,
-				  &curr_buff[*curr_buff_cnt], nbytes); 
+				  &curr_buff[*curr_buff_cnt], nbytes);
 		*curr_buff_cnt += nbytes;
 		return 1;
 	}
@@ -1717,12 +1608,12 @@ int ssi_buffer_mgr_map_hash_request_update(
 	/* Copy the new residue to next buffer */
 	if (*next_buff_cnt != 0) {
 		SSI_LOG_DEBUG(" handle residue: next buff %pK skip data %u"
-			     " residue %u \n", next_buff,
+			     " residue %u\n", next_buff,
 			     (update_data_len - *curr_buff_cnt),
 			     *next_buff_cnt);
 		ssi_buffer_mgr_copy_scatterlist_portion(next_buff, src,
-			     (update_data_len -*curr_buff_cnt),
-			     nbytes,SSI_SG_TO_BUF);
+			     (update_data_len - *curr_buff_cnt),
+			     nbytes, SSI_SG_TO_BUF);
 		/* change the buffer index for next operation */
 		swap_index = 1;
 	}
@@ -1735,20 +1626,20 @@ int ssi_buffer_mgr_map_hash_request_update(
 		/* change the buffer index for next operation */
 		swap_index = 1;
 	}
-	
-	if ( update_data_len > *curr_buff_cnt ) {
-		if ( unlikely( ssi_buffer_mgr_map_scatterlist( dev,src,
-					  (update_data_len -*curr_buff_cnt),
+
+	if (update_data_len > *curr_buff_cnt) {
+		if (unlikely(ssi_buffer_mgr_map_scatterlist(dev, src,
+					  (update_data_len - *curr_buff_cnt),
 					  DMA_TO_DEVICE,
 					  &areq_ctx->in_nents,
 					  LLI_MAX_NUM_OF_DATA_ENTRIES,
 					  &dummy, &mapped_nents))){
 			goto unmap_curr_buff;
 		}
-		if ( (mapped_nents == 1) 
-		     && (areq_ctx->data_dma_buf_type == SSI_DMA_BUF_NULL) ) {
+		if ((mapped_nents == 1)
+		     && (areq_ctx->data_dma_buf_type == SSI_DMA_BUF_NULL)) {
 			/* only one entry in the SG and no previous data */
-			memcpy(areq_ctx->buff_sg,src,
+			memcpy(areq_ctx->buff_sg, src,
 			       sizeof(struct scatterlist));
 			areq_ctx->buff_sg->length = update_data_len;
 			areq_ctx->data_dma_buf_type = SSI_DMA_BUF_DLLI;
@@ -1770,9 +1661,8 @@ int ssi_buffer_mgr_map_hash_request_update(
 						  mlli_params) != 0)) {
 			goto fail_unmap_din;
 		}
-
 	}
-	areq_ctx->buff_index = (areq_ctx->buff_index^swap_index);
+	areq_ctx->buff_index = (areq_ctx->buff_index ^ swap_index);
 
 	return 0;
 
@@ -1780,9 +1670,9 @@ fail_unmap_din:
 	dma_unmap_sg(dev, src, areq_ctx->in_nents, DMA_TO_DEVICE);
 
 unmap_curr_buff:
-	if (*curr_buff_cnt != 0 ) {
+	if (*curr_buff_cnt != 0)
 		dma_unmap_sg(dev, areq_ctx->buff_sg, 1, DMA_TO_DEVICE);
-	}
+
 	return -ENOMEM;
 }
 
@@ -1790,36 +1680,35 @@ void ssi_buffer_mgr_unmap_hash_request(
 	struct device *dev, void *ctx, struct scatterlist *src, bool do_revert)
 {
 	struct ahash_req_ctx *areq_ctx = (struct ahash_req_ctx *)ctx;
-	uint32_t *prev_len = areq_ctx->buff_index ?  &areq_ctx->buff0_cnt :
+	u32 *prev_len = areq_ctx->buff_index ?  &areq_ctx->buff0_cnt :
 						&areq_ctx->buff1_cnt;
 
-	/*In case a pool was set, a table was 
-	  allocated and should be released */
-	if (areq_ctx->mlli_params.curr_pool != NULL) {
-		SSI_LOG_DEBUG("free MLLI buffer: dma=0x%llX virt=%pK\n", 
+	/*In case a pool was set, a table was
+	 *allocated and should be released
+	 */
+	if (areq_ctx->mlli_params.curr_pool) {
+		SSI_LOG_DEBUG("free MLLI buffer: dma=0x%llX virt=%pK\n",
 			     (unsigned long long)areq_ctx->mlli_params.mlli_dma_addr,
 			     areq_ctx->mlli_params.mlli_virt_addr);
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->mlli_params.mlli_dma_addr);
 		dma_pool_free(areq_ctx->mlli_params.curr_pool,
 			      areq_ctx->mlli_params.mlli_virt_addr,
 			      areq_ctx->mlli_params.mlli_dma_addr);
 	}
-	
+
 	if ((src) && likely(areq_ctx->in_nents != 0)) {
 		SSI_LOG_DEBUG("Unmapped sg src: virt=%pK dma=0x%llX len=0x%X\n",
 			     sg_virt(src),
-			     (unsigned long long)sg_dma_address(src), 
+			     (unsigned long long)sg_dma_address(src),
 			     sg_dma_len(src));
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(sg_dma_address(src));
-		dma_unmap_sg(dev, src, 
+		dma_unmap_sg(dev, src,
 			     areq_ctx->in_nents, DMA_TO_DEVICE);
 	}
 
 	if (*prev_len != 0) {
 		SSI_LOG_DEBUG("Unmapped buffer: areq_ctx->buff_sg=%pK"
-			     "dma=0x%llX len 0x%X\n", 
+			     " dma=0x%llX len 0x%X\n",
 				sg_virt(areq_ctx->buff_sg),
-				(unsigned long long)sg_dma_address(areq_ctx->buff_sg), 
+				(unsigned long long)sg_dma_address(areq_ctx->buff_sg),
 				sg_dma_len(areq_ctx->buff_sg));
 		dma_unmap_sg(dev, areq_ctx->buff_sg, 1, DMA_TO_DEVICE);
 		if (!do_revert) {
@@ -1838,18 +1727,18 @@ int ssi_buffer_mgr_init(struct ssi_drvdata *drvdata)
 
 	buff_mgr_handle = (struct buff_mgr_handle *)
 		kmalloc(sizeof(struct buff_mgr_handle), GFP_KERNEL);
-	if (buff_mgr_handle == NULL)
+	if (!buff_mgr_handle)
 		return -ENOMEM;
 
 	drvdata->buff_mgr_handle = buff_mgr_handle;
 
 	buff_mgr_handle->mlli_buffs_pool = dma_pool_create(
 				"dx_single_mlli_tables", dev,
-				MAX_NUM_OF_TOTAL_MLLI_ENTRIES * 
+				MAX_NUM_OF_TOTAL_MLLI_ENTRIES *
 				LLI_ENTRY_BYTE_SIZE,
 				MLLI_TABLE_MIN_ALIGNMENT, 0);
 
-	if (unlikely(buff_mgr_handle->mlli_buffs_pool == NULL))
+	if (unlikely(!buff_mgr_handle->mlli_buffs_pool))
 		goto error;
 
 	return 0;
@@ -1863,12 +1752,10 @@ int ssi_buffer_mgr_fini(struct ssi_drvdata *drvdata)
 {
 	struct buff_mgr_handle *buff_mgr_handle = drvdata->buff_mgr_handle;
 
-	if (buff_mgr_handle  != NULL) {
+	if (buff_mgr_handle) {
 		dma_pool_destroy(buff_mgr_handle->mlli_buffs_pool);
 		kfree(drvdata->buff_mgr_handle);
 		drvdata->buff_mgr_handle = NULL;
-
 	}
 	return 0;
 }
-
diff --git a/drivers/staging/ccree/ssi_buffer_mgr.h b/drivers/staging/ccree/ssi_buffer_mgr.h
index 5f4b032389f1..41f5223730f8 100644
--- a/drivers/staging/ccree/ssi_buffer_mgr.h
+++ b/drivers/staging/ccree/ssi_buffer_mgr.h
@@ -1,21 +1,21 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
 /* \file buffer_mgr.h
-   Buffer Manager
+ * Buffer Manager
  */
 
 #ifndef __SSI_BUFFER_MGR_H__
@@ -26,7 +26,6 @@
 #include "ssi_config.h"
 #include "ssi_driver.h"
 
-
 enum ssi_req_dma_buf_type {
 	SSI_DMA_BUF_NULL = 0,
 	SSI_DMA_BUF_DLLI,
@@ -46,9 +45,9 @@ struct ssi_mlli {
 
 struct mlli_params {
 	struct dma_pool *curr_pool;
-	uint8_t *mlli_virt_addr;
+	u8 *mlli_virt_addr;
 	dma_addr_t mlli_dma_addr;
-	uint32_t mlli_len;  
+	u32 mlli_len;
 };
 
 int ssi_buffer_mgr_init(struct ssi_drvdata *drvdata);
@@ -65,7 +64,7 @@ int ssi_buffer_mgr_map_blkcipher_request(
 	struct scatterlist *dst);
 
 void ssi_buffer_mgr_unmap_blkcipher_request(
-	struct device *dev, 
+	struct device *dev,
 	void *ctx,
 	unsigned int ivsize,
 	struct scatterlist *src,
@@ -81,25 +80,9 @@ int ssi_buffer_mgr_map_hash_request_update(struct ssi_drvdata *drvdata, void *ct
 
 void ssi_buffer_mgr_unmap_hash_request(struct device *dev, void *ctx, struct scatterlist *src, bool do_revert);
 
-void ssi_buffer_mgr_copy_scatterlist_portion(u8 *dest, struct scatterlist *sg, uint32_t to_skip, uint32_t end, enum ssi_sg_cpy_direct direct);
-
-void ssi_buffer_mgr_zero_sgl(struct scatterlist *sgl, uint32_t data_len);
-
-
-#ifdef CC_DMA_48BIT_SIM
-dma_addr_t ssi_buff_mgr_update_dma_addr(dma_addr_t orig_addr, uint32_t data_len);
-dma_addr_t ssi_buff_mgr_restore_dma_addr(dma_addr_t orig_addr);
-
-#define SSI_UPDATE_DMA_ADDR_TO_48BIT(addr,size) addr = \
-					ssi_buff_mgr_update_dma_addr(addr,size)
-#define SSI_RESTORE_DMA_ADDR_TO_48BIT(addr) addr = \
-					ssi_buff_mgr_restore_dma_addr(addr)
-#else
-
-#define SSI_UPDATE_DMA_ADDR_TO_48BIT(addr,size) addr = addr
-#define SSI_RESTORE_DMA_ADDR_TO_48BIT(addr) addr = addr
+void ssi_buffer_mgr_copy_scatterlist_portion(u8 *dest, struct scatterlist *sg, u32 to_skip, u32 end, enum ssi_sg_cpy_direct direct);
 
-#endif
+void ssi_buffer_mgr_zero_sgl(struct scatterlist *sgl, u32 data_len);
 
 #endif /*__BUFFER_MGR_H__*/
 
diff --git a/drivers/staging/ccree/ssi_cipher.c b/drivers/staging/ccree/ssi_cipher.c
index 664ed7e52cf2..cd2eafc04232 100644
--- a/drivers/staging/ccree/ssi_cipher.c
+++ b/drivers/staging/ccree/ssi_cipher.c
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -36,7 +36,6 @@
 #define MAX_ABLKCIPHER_SEQ_LEN 6
 
 #define template_ablkcipher	template_u.ablkcipher
-#define template_sblkcipher	template_u.blkcipher
 
 #define SSI_MIN_AES_XTS_SIZE 0x10
 #define SSI_MAX_AES_XTS_SIZE 0x2000
@@ -45,12 +44,13 @@ struct ssi_blkcipher_handle {
 };
 
 struct cc_user_key_info {
-	uint8_t *key;
+	u8 *key;
 	dma_addr_t key_dma_addr;
 };
+
 struct cc_hw_key_info {
-	enum HwCryptoKey key1_slot;
-	enum HwCryptoKey key2_slot;
+	enum cc_hw_crypto_key key1_slot;
+	enum cc_hw_crypto_key key2_slot;
 };
 
 struct ssi_ablkcipher_ctx {
@@ -68,11 +68,10 @@ struct ssi_ablkcipher_ctx {
 
 static void ssi_ablkcipher_complete(struct device *dev, void *ssi_req, void __iomem *cc_base);
 
-
-static int validate_keys_sizes(struct ssi_ablkcipher_ctx *ctx_p, uint32_t size) {
-	switch (ctx_p->flow_mode){
+static int validate_keys_sizes(struct ssi_ablkcipher_ctx *ctx_p, u32 size) {
+	switch (ctx_p->flow_mode) {
 	case S_DIN_to_AES:
-		switch (size){
+		switch (size) {
 		case CC_AES_128_BIT_KEY_SIZE:
 		case CC_AES_192_BIT_KEY_SIZE:
 			if (likely((ctx_p->cipher_mode != DRV_CIPHER_XTS) &&
@@ -82,8 +81,8 @@ static int validate_keys_sizes(struct ssi_ablkcipher_ctx *ctx_p, uint32_t size)
 			break;
 		case CC_AES_256_BIT_KEY_SIZE:
 			return 0;
-		case (CC_AES_192_BIT_KEY_SIZE*2):
-		case (CC_AES_256_BIT_KEY_SIZE*2):
+		case (CC_AES_192_BIT_KEY_SIZE * 2):
+		case (CC_AES_256_BIT_KEY_SIZE * 2):
 			if (likely((ctx_p->cipher_mode == DRV_CIPHER_XTS) ||
 				   (ctx_p->cipher_mode == DRV_CIPHER_ESSIV) ||
 				   (ctx_p->cipher_mode == DRV_CIPHER_BITLOCKER)))
@@ -105,19 +104,17 @@ static int validate_keys_sizes(struct ssi_ablkcipher_ctx *ctx_p, uint32_t size)
 #endif
 	default:
 		break;
-
 	}
 	return -EINVAL;
 }
 
-
 static int validate_data_size(struct ssi_ablkcipher_ctx *ctx_p, unsigned int size) {
-	switch (ctx_p->flow_mode){
+	switch (ctx_p->flow_mode) {
 	case S_DIN_to_AES:
-		switch (ctx_p->cipher_mode){
+		switch (ctx_p->cipher_mode) {
 		case DRV_CIPHER_XTS:
 			if ((size >= SSI_MIN_AES_XTS_SIZE) &&
-			    (size <= SSI_MAX_AES_XTS_SIZE) && 
+			    (size <= SSI_MAX_AES_XTS_SIZE) &&
 			    IS_ALIGNED(size, AES_BLOCK_SIZE))
 				return 0;
 			break;
@@ -159,7 +156,6 @@ static int validate_data_size(struct ssi_ablkcipher_ctx *ctx_p, unsigned int siz
 #endif /*SSI_CC_HAS_MULTI2*/
 	default:
 		break;
-
 	}
 	return -EINVAL;
 }
@@ -168,13 +164,11 @@ static unsigned int get_max_keysize(struct crypto_tfm *tfm)
 {
 	struct ssi_crypto_alg *ssi_alg = container_of(tfm->__crt_alg, struct ssi_crypto_alg, crypto_alg);
 
-	if ((ssi_alg->crypto_alg.cra_flags & CRYPTO_ALG_TYPE_MASK) == CRYPTO_ALG_TYPE_ABLKCIPHER) {
+	if ((ssi_alg->crypto_alg.cra_flags & CRYPTO_ALG_TYPE_MASK) == CRYPTO_ALG_TYPE_ABLKCIPHER)
 		return ssi_alg->crypto_alg.cra_ablkcipher.max_keysize;
-	}
 
-	if ((ssi_alg->crypto_alg.cra_flags & CRYPTO_ALG_TYPE_MASK) == CRYPTO_ALG_TYPE_BLKCIPHER) {
+	if ((ssi_alg->crypto_alg.cra_flags & CRYPTO_ALG_TYPE_MASK) == CRYPTO_ALG_TYPE_BLKCIPHER)
 		return ssi_alg->crypto_alg.cra_blkcipher.max_keysize;
-	}
 
 	return 0;
 }
@@ -189,7 +183,7 @@ static int ssi_blkcipher_init(struct crypto_tfm *tfm)
 	int rc = 0;
 	unsigned int max_key_buf_size = get_max_keysize(tfm);
 
-	SSI_LOG_DEBUG("Initializing context @%p for %s\n", ctx_p, 
+	SSI_LOG_DEBUG("Initializing context @%p for %s\n", ctx_p,
 						crypto_tfm_alg_name(tfm));
 
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
@@ -199,7 +193,7 @@ static int ssi_blkcipher_init(struct crypto_tfm *tfm)
 	dev = &ctx_p->drvdata->plat_dev->dev;
 
 	/* Allocate key buffer, cache line aligned */
-	ctx_p->user.key = kmalloc(max_key_buf_size, GFP_KERNEL|GFP_DMA);
+	ctx_p->user.key = kmalloc(max_key_buf_size, GFP_KERNEL | GFP_DMA);
 	if (!ctx_p->user.key) {
 		SSI_LOG_ERR("Allocating key buffer in context failed\n");
 		rc = -ENOMEM;
@@ -215,7 +209,6 @@ static int ssi_blkcipher_init(struct crypto_tfm *tfm)
 			max_key_buf_size, ctx_p->user.key);
 		return -ENOMEM;
 	}
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx_p->user.key_dma_addr, max_key_buf_size);
 	SSI_LOG_DEBUG("Mapped key %u B at va=%pK to dma=0x%llX\n",
 		max_key_buf_size, ctx_p->user.key,
 		(unsigned long long)ctx_p->user.key_dma_addr);
@@ -248,10 +241,9 @@ static void ssi_blkcipher_exit(struct crypto_tfm *tfm)
 	}
 
 	/* Unmap key buffer */
-	SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx_p->user.key_dma_addr);
 	dma_unmap_single(dev, ctx_p->user.key_dma_addr, max_key_buf_size,
 								DMA_TO_DEVICE);
-	SSI_LOG_DEBUG("Unmapped key buffer key_dma_addr=0x%llX\n", 
+	SSI_LOG_DEBUG("Unmapped key buffer key_dma_addr=0x%llX\n",
 		(unsigned long long)ctx_p->user.key_dma_addr);
 
 	/* Free key buffer in context */
@@ -259,50 +251,48 @@ static void ssi_blkcipher_exit(struct crypto_tfm *tfm)
 	SSI_LOG_DEBUG("Free key buffer in context. key=@%p\n", ctx_p->user.key);
 }
 
+struct tdes_keys {
+	u8	key1[DES_KEY_SIZE];
+	u8	key2[DES_KEY_SIZE];
+	u8	key3[DES_KEY_SIZE];
+};
 
-typedef struct tdes_keys{
-        u8      key1[DES_KEY_SIZE];
-        u8      key2[DES_KEY_SIZE];
-        u8      key3[DES_KEY_SIZE];
-}tdes_keys_t;
-
-static const u8 zero_buff[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 
-                               0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
-                               0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 
-                               0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+static const u8 zero_buff[] = {	0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+				0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+				0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+				0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 
 /* The function verifies that tdes keys are not weak.*/
 static int ssi_fips_verify_3des_keys(const u8 *key, unsigned int keylen)
 {
 #ifdef CCREE_FIPS_SUPPORT
-        tdes_keys_t *tdes_key = (tdes_keys_t*)key;
+	struct tdes_keys *tdes_key = (struct tdes_keys *)key;
 
 	/* verify key1 != key2 and key3 != key2*/
-        if (unlikely( (memcmp((u8*)tdes_key->key1, (u8*)tdes_key->key2, sizeof(tdes_key->key1)) == 0) || 
-		      (memcmp((u8*)tdes_key->key3, (u8*)tdes_key->key2, sizeof(tdes_key->key3)) == 0) )) {
-                return -ENOEXEC;
-        }
+	if (unlikely((memcmp((u8 *)tdes_key->key1, (u8 *)tdes_key->key2, sizeof(tdes_key->key1)) == 0) ||
+		      (memcmp((u8 *)tdes_key->key3, (u8 *)tdes_key->key2, sizeof(tdes_key->key3)) == 0))) {
+		return -ENOEXEC;
+	}
 #endif /* CCREE_FIPS_SUPPORT */
 
-        return 0;
+	return 0;
 }
 
 /* The function verifies that xts keys are not weak.*/
 static int ssi_fips_verify_xts_keys(const u8 *key, unsigned int keylen)
 {
 #ifdef CCREE_FIPS_SUPPORT
-        /* Weak key is define as key that its first half (128/256 lsb) equals its second half (128/256 msb) */
-        int singleKeySize = keylen >> 1;
+	/* Weak key is define as key that its first half (128/256 lsb) equals its second half (128/256 msb) */
+	int singleKeySize = keylen >> 1;
 
-	if (unlikely(memcmp(key, &key[singleKeySize], singleKeySize) == 0)) {
+	if (unlikely(memcmp(key, &key[singleKeySize], singleKeySize) == 0))
 		return -ENOEXEC;
-	}
 #endif /* CCREE_FIPS_SUPPORT */
 
-        return 0;
+	return 0;
 }
 
-static enum HwCryptoKey hw_key_to_cc_hw_key(int slot_num)
+static enum cc_hw_crypto_key hw_key_to_cc_hw_key(int slot_num)
 {
 	switch (slot_num) {
 	case 0:
@@ -317,35 +307,32 @@ static enum HwCryptoKey hw_key_to_cc_hw_key(int slot_num)
 	return END_OF_KEYS;
 }
 
-static int ssi_blkcipher_setkey(struct crypto_tfm *tfm, 
-				const u8 *key, 
+static int ssi_blkcipher_setkey(struct crypto_tfm *tfm,
+				const u8 *key,
 				unsigned int keylen)
 {
 	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
 	struct device *dev = &ctx_p->drvdata->plat_dev->dev;
 	u32 tmp[DES_EXPKEY_WORDS];
 	unsigned int max_key_buf_size = get_max_keysize(tfm);
-	DECL_CYCLE_COUNT_RESOURCES;
 
 	SSI_LOG_DEBUG("Setting key in context @%p for %s. keylen=%u\n",
 		ctx_p, crypto_tfm_alg_name(tfm), keylen);
-	dump_byte_array("key", (uint8_t *)key, keylen);
+	dump_byte_array("key", (u8 *)key, keylen);
 
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
 
 	SSI_LOG_DEBUG("ssi_blkcipher_setkey: after FIPS check");
-	
+
 	/* STAT_PHASE_0: Init and sanity checks */
-	START_CYCLE_COUNT();
 
 #if SSI_CC_HAS_MULTI2
 	/*last byte of key buffer is round number and should not be a part of key size*/
-	if (ctx_p->flow_mode == S_DIN_to_MULTI2) {
-		keylen -=1;
-	}
+	if (ctx_p->flow_mode == S_DIN_to_MULTI2)
+		keylen -= 1;
 #endif /*SSI_CC_HAS_MULTI2*/
 
-	if (unlikely(validate_keys_sizes(ctx_p,keylen) != 0)) {
+	if (unlikely(validate_keys_sizes(ctx_p, keylen) != 0)) {
 		SSI_LOG_ERR("Unsupported key size %d.\n", keylen);
 		crypto_tfm_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
 		return -EINVAL;
@@ -353,7 +340,7 @@ static int ssi_blkcipher_setkey(struct crypto_tfm *tfm,
 
 	if (ssi_is_hw_key(tfm)) {
 		/* setting HW key slots */
-		struct arm_hw_key_info *hki = (struct arm_hw_key_info*)key;
+		struct arm_hw_key_info *hki = (struct arm_hw_key_info *)key;
 
 		if (unlikely(ctx_p->flow_mode != S_DIN_to_AES)) {
 			SSI_LOG_ERR("HW key not supported for non-AES flows\n");
@@ -381,7 +368,6 @@ static int ssi_blkcipher_setkey(struct crypto_tfm *tfm,
 		}
 
 		ctx_p->keylen = keylen;
-		END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_0);
 		SSI_LOG_DEBUG("ssi_blkcipher_setkey: ssi_is_hw_key ret 0");
 
 		return 0;
@@ -396,28 +382,24 @@ static int ssi_blkcipher_setkey(struct crypto_tfm *tfm,
 			return -EINVAL;
 		}
 	}
-	if ((ctx_p->cipher_mode == DRV_CIPHER_XTS) && 
+	if ((ctx_p->cipher_mode == DRV_CIPHER_XTS) &&
 	    ssi_fips_verify_xts_keys(key, keylen) != 0) {
 		SSI_LOG_DEBUG("ssi_blkcipher_setkey: weak XTS key");
 		return -EINVAL;
 	}
-	if ((ctx_p->flow_mode == S_DIN_to_DES) && 
-	    (keylen == DES3_EDE_KEY_SIZE) && 
+	if ((ctx_p->flow_mode == S_DIN_to_DES) &&
+	    (keylen == DES3_EDE_KEY_SIZE) &&
 	    ssi_fips_verify_3des_keys(key, keylen) != 0) {
 		SSI_LOG_DEBUG("ssi_blkcipher_setkey: weak 3DES key");
 		return -EINVAL;
 	}
 
-
-	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_0);
-
 	/* STAT_PHASE_1: Copy key to ctx */
-	START_CYCLE_COUNT();
-	SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx_p->user.key_dma_addr);
-	dma_sync_single_for_cpu(dev, ctx_p->user.key_dma_addr, 
+	dma_sync_single_for_cpu(dev, ctx_p->user.key_dma_addr,
 					max_key_buf_size, DMA_TO_DEVICE);
-#if SSI_CC_HAS_MULTI2
+
 	if (ctx_p->flow_mode == S_DIN_to_MULTI2) {
+#if SSI_CC_HAS_MULTI2
 		memcpy(ctx_p->user.key, key, CC_MULTI2_SYSTEM_N_DATA_KEY_SIZE);
 		ctx_p->key_round_number = key[CC_MULTI2_SYSTEM_N_DATA_KEY_SIZE];
 		if (ctx_p->key_round_number < CC_MULTI2_MIN_NUM_ROUNDS ||
@@ -425,10 +407,8 @@ static int ssi_blkcipher_setkey(struct crypto_tfm *tfm,
 			crypto_tfm_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
 			SSI_LOG_DEBUG("ssi_blkcipher_setkey: SSI_CC_HAS_MULTI2 einval");
 			return -EINVAL;
-		}
-	} else 
 #endif /*SSI_CC_HAS_MULTI2*/
-	{
+	} else {
 		memcpy(ctx_p->user.key, key, keylen);
 		if (keylen == 24)
 			memset(ctx_p->user.key + 24, 0, CC_AES_KEY_SIZE_MAX - 24);
@@ -438,6 +418,7 @@ static int ssi_blkcipher_setkey(struct crypto_tfm *tfm,
 			int key_len = keylen >> 1;
 			int err;
 			SHASH_DESC_ON_STACK(desc, ctx_p->shash_tfm);
+
 			desc->tfm = ctx_p->shash_tfm;
 
 			err = crypto_shash_digest(desc, ctx_p->user.key, key_len, ctx_p->user.key + key_len);
@@ -447,12 +428,9 @@ static int ssi_blkcipher_setkey(struct crypto_tfm *tfm,
 			}
 		}
 	}
-	dma_sync_single_for_device(dev, ctx_p->user.key_dma_addr, 
+	dma_sync_single_for_device(dev, ctx_p->user.key_dma_addr,
 					max_key_buf_size, DMA_TO_DEVICE);
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx_p->user.key_dma_addr ,max_key_buf_size);
 	ctx_p->keylen = keylen;
-	
-	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_1);
 
 	 SSI_LOG_DEBUG("ssi_blkcipher_setkey: return safely");
 	return 0;
@@ -464,7 +442,7 @@ ssi_blkcipher_create_setup_desc(
 	struct blkcipher_req_ctx *req_ctx,
 	unsigned int ivsize,
 	unsigned int nbytes,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
@@ -489,96 +467,92 @@ ssi_blkcipher_create_setup_desc(
 	case DRV_CIPHER_CTR:
 	case DRV_CIPHER_OFB:
 		/* Load cipher state */
-		HW_DESC_INIT(&desc[*seq_size]);
-		HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
-				     iv_dma_addr, ivsize,
-				     NS_BIT);
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
-		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], flow_mode);
-		HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], cipher_mode);
-		if ((cipher_mode == DRV_CIPHER_CTR) || 
-		    (cipher_mode == DRV_CIPHER_OFB) ) {
-			HW_DESC_SET_SETUP_MODE(&desc[*seq_size],
-					       SETUP_LOAD_STATE1);
+		hw_desc_init(&desc[*seq_size]);
+		set_din_type(&desc[*seq_size], DMA_DLLI, iv_dma_addr, ivsize,
+			     NS_BIT);
+		set_cipher_config0(&desc[*seq_size], direction);
+		set_flow_mode(&desc[*seq_size], flow_mode);
+		set_cipher_mode(&desc[*seq_size], cipher_mode);
+		if ((cipher_mode == DRV_CIPHER_CTR) ||
+		    (cipher_mode == DRV_CIPHER_OFB)) {
+			set_setup_mode(&desc[*seq_size], SETUP_LOAD_STATE1);
 		} else {
-			HW_DESC_SET_SETUP_MODE(&desc[*seq_size],
-					       SETUP_LOAD_STATE0);
+			set_setup_mode(&desc[*seq_size], SETUP_LOAD_STATE0);
 		}
 		(*seq_size)++;
 		/*FALLTHROUGH*/
 	case DRV_CIPHER_ECB:
 		/* Load key */
-		HW_DESC_INIT(&desc[*seq_size]);
-		HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], cipher_mode);
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
+		hw_desc_init(&desc[*seq_size]);
+		set_cipher_mode(&desc[*seq_size], cipher_mode);
+		set_cipher_config0(&desc[*seq_size], direction);
 		if (flow_mode == S_DIN_to_AES) {
-
 			if (ssi_is_hw_key(tfm)) {
-				HW_DESC_SET_HW_CRYPTO_KEY(&desc[*seq_size], ctx_p->hw.key1_slot);
+				set_hw_crypto_key(&desc[*seq_size],
+						  ctx_p->hw.key1_slot);
 			} else {
-				HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
-						     key_dma_addr, 
-						     ((key_len == 24) ? AES_MAX_KEY_SIZE : key_len),
-						     NS_BIT);
+				set_din_type(&desc[*seq_size], DMA_DLLI,
+					     key_dma_addr, ((key_len == 24) ?
+							    AES_MAX_KEY_SIZE :
+							    key_len), NS_BIT);
 			}
-			HW_DESC_SET_KEY_SIZE_AES(&desc[*seq_size], key_len);
+			set_key_size_aes(&desc[*seq_size], key_len);
 		} else {
 			/*des*/
-			HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
-					     key_dma_addr, key_len,
-					     NS_BIT);
-			HW_DESC_SET_KEY_SIZE_DES(&desc[*seq_size], key_len);
+			set_din_type(&desc[*seq_size], DMA_DLLI, key_dma_addr,
+				     key_len, NS_BIT);
+			set_key_size_des(&desc[*seq_size], key_len);
 		}
-		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], flow_mode);
-		HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_KEY0);
+		set_flow_mode(&desc[*seq_size], flow_mode);
+		set_setup_mode(&desc[*seq_size], SETUP_LOAD_KEY0);
 		(*seq_size)++;
 		break;
 	case DRV_CIPHER_XTS:
 	case DRV_CIPHER_ESSIV:
 	case DRV_CIPHER_BITLOCKER:
 		/* Load AES key */
-		HW_DESC_INIT(&desc[*seq_size]);
-		HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], cipher_mode);
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
+		hw_desc_init(&desc[*seq_size]);
+		set_cipher_mode(&desc[*seq_size], cipher_mode);
+		set_cipher_config0(&desc[*seq_size], direction);
 		if (ssi_is_hw_key(tfm)) {
-			HW_DESC_SET_HW_CRYPTO_KEY(&desc[*seq_size], ctx_p->hw.key1_slot);
+			set_hw_crypto_key(&desc[*seq_size],
+					  ctx_p->hw.key1_slot);
 		} else {
-			HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
-					     key_dma_addr, key_len/2,
-					     NS_BIT);
+			set_din_type(&desc[*seq_size], DMA_DLLI, key_dma_addr,
+				     (key_len / 2), NS_BIT);
 		}
-		HW_DESC_SET_KEY_SIZE_AES(&desc[*seq_size], key_len/2);
-		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], flow_mode);
-		HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_KEY0);
+		set_key_size_aes(&desc[*seq_size], (key_len / 2));
+		set_flow_mode(&desc[*seq_size], flow_mode);
+		set_setup_mode(&desc[*seq_size], SETUP_LOAD_KEY0);
 		(*seq_size)++;
 
 		/* load XEX key */
-		HW_DESC_INIT(&desc[*seq_size]);
-		HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], cipher_mode);
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
+		hw_desc_init(&desc[*seq_size]);
+		set_cipher_mode(&desc[*seq_size], cipher_mode);
+		set_cipher_config0(&desc[*seq_size], direction);
 		if (ssi_is_hw_key(tfm)) {
-			HW_DESC_SET_HW_CRYPTO_KEY(&desc[*seq_size], ctx_p->hw.key2_slot);
+			set_hw_crypto_key(&desc[*seq_size],
+					  ctx_p->hw.key2_slot);
 		} else {
-			HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI, 
-					     (key_dma_addr+key_len/2), key_len/2,
-					     NS_BIT);
+			set_din_type(&desc[*seq_size], DMA_DLLI,
+				     (key_dma_addr + (key_len / 2)),
+				     (key_len / 2), NS_BIT);
 		}
-		HW_DESC_SET_XEX_DATA_UNIT_SIZE(&desc[*seq_size], du_size);
-		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], S_DIN_to_AES2);
-		HW_DESC_SET_KEY_SIZE_AES(&desc[*seq_size], key_len/2);
-		HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_XEX_KEY);
+		set_xex_data_unit_size(&desc[*seq_size], du_size);
+		set_flow_mode(&desc[*seq_size], S_DIN_to_AES2);
+		set_key_size_aes(&desc[*seq_size], (key_len / 2));
+		set_setup_mode(&desc[*seq_size], SETUP_LOAD_XEX_KEY);
 		(*seq_size)++;
-	
+
 		/* Set state */
-		HW_DESC_INIT(&desc[*seq_size]);
-		HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_STATE1);
-		HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], cipher_mode);
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
-		HW_DESC_SET_KEY_SIZE_AES(&desc[*seq_size], key_len/2);
-		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], flow_mode);
-		HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
-				     iv_dma_addr, CC_AES_BLOCK_SIZE,
-				     NS_BIT);
+		hw_desc_init(&desc[*seq_size]);
+		set_setup_mode(&desc[*seq_size], SETUP_LOAD_STATE1);
+		set_cipher_mode(&desc[*seq_size], cipher_mode);
+		set_cipher_config0(&desc[*seq_size], direction);
+		set_key_size_aes(&desc[*seq_size], (key_len / 2));
+		set_flow_mode(&desc[*seq_size], flow_mode);
+		set_din_type(&desc[*seq_size], DMA_DLLI, iv_dma_addr,
+			     CC_AES_BLOCK_SIZE, NS_BIT);
 		(*seq_size)++;
 		break;
 	default:
@@ -592,49 +566,43 @@ static inline void ssi_blkcipher_create_multi2_setup_desc(
 	struct crypto_tfm *tfm,
 	struct blkcipher_req_ctx *req_ctx,
 	unsigned int ivsize,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
-	
+
 	int direction = req_ctx->gen_ctx.op_type;
 	/* Load system key */
-	HW_DESC_INIT(&desc[*seq_size]);
-	HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], ctx_p->cipher_mode);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
-	HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI, ctx_p->user.key_dma_addr,
-						CC_MULTI2_SYSTEM_KEY_SIZE,
-						NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[*seq_size], ctx_p->flow_mode);
-	HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_KEY0);
+	hw_desc_init(&desc[*seq_size]);
+	set_cipher_mode(&desc[*seq_size], ctx_p->cipher_mode);
+	set_cipher_config0(&desc[*seq_size], direction);
+	set_din_type(&desc[*seq_size], DMA_DLLI, ctx_p->user.key_dma_addr,
+		     CC_MULTI2_SYSTEM_KEY_SIZE, NS_BIT);
+	set_flow_mode(&desc[*seq_size], ctx_p->flow_mode);
+	set_setup_mode(&desc[*seq_size], SETUP_LOAD_KEY0);
 	(*seq_size)++;
 
 	/* load data key */
-	HW_DESC_INIT(&desc[*seq_size]);
-	HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI, 
-					(ctx_p->user.key_dma_addr + 
-						CC_MULTI2_SYSTEM_KEY_SIZE),
-				CC_MULTI2_DATA_KEY_SIZE, NS_BIT);
-	HW_DESC_SET_MULTI2_NUM_ROUNDS(&desc[*seq_size],
-						ctx_p->key_round_number);
-	HW_DESC_SET_FLOW_MODE(&desc[*seq_size], ctx_p->flow_mode);
-	HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], ctx_p->cipher_mode);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
-	HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_STATE0 );
+	hw_desc_init(&desc[*seq_size]);
+	set_din_type(&desc[*seq_size], DMA_DLLI,
+		     (ctx_p->user.key_dma_addr + CC_MULTI2_SYSTEM_KEY_SIZE),
+		     CC_MULTI2_DATA_KEY_SIZE, NS_BIT);
+	set_multi2_num_rounds(&desc[*seq_size], ctx_p->key_round_number);
+	set_flow_mode(&desc[*seq_size], ctx_p->flow_mode);
+	set_cipher_mode(&desc[*seq_size], ctx_p->cipher_mode);
+	set_cipher_config0(&desc[*seq_size], direction);
+	set_setup_mode(&desc[*seq_size], SETUP_LOAD_STATE0);
 	(*seq_size)++;
-	
-	
+
 	/* Set state */
-	HW_DESC_INIT(&desc[*seq_size]);
-	HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
-			     req_ctx->gen_ctx.iv_dma_addr,
-			     ivsize, NS_BIT);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
-	HW_DESC_SET_FLOW_MODE(&desc[*seq_size], ctx_p->flow_mode);
-	HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], ctx_p->cipher_mode);
-	HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_STATE1);	
+	hw_desc_init(&desc[*seq_size]);
+	set_din_type(&desc[*seq_size], DMA_DLLI, req_ctx->gen_ctx.iv_dma_addr,
+		     ivsize, NS_BIT);
+	set_cipher_config0(&desc[*seq_size], direction);
+	set_flow_mode(&desc[*seq_size], ctx_p->flow_mode);
+	set_cipher_mode(&desc[*seq_size], ctx_p->cipher_mode);
+	set_setup_mode(&desc[*seq_size], SETUP_LOAD_STATE1);
 	(*seq_size)++;
-	
 }
 #endif /*SSI_CC_HAS_MULTI2*/
 
@@ -645,7 +613,7 @@ ssi_blkcipher_create_data_desc(
 	struct scatterlist *dst, struct scatterlist *src,
 	unsigned int nbytes,
 	void *areq,
-	HwDesc_s desc[],
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size)
 {
 	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
@@ -668,25 +636,22 @@ ssi_blkcipher_create_data_desc(
 		return;
 	}
 	/* Process */
-	if (likely(req_ctx->dma_buf_type == SSI_DMA_BUF_DLLI)){
+	if (likely(req_ctx->dma_buf_type == SSI_DMA_BUF_DLLI)) {
 		SSI_LOG_DEBUG(" data params addr 0x%llX length 0x%X \n",
 			     (unsigned long long)sg_dma_address(src),
 			     nbytes);
 		SSI_LOG_DEBUG(" data params addr 0x%llX length 0x%X \n",
 			     (unsigned long long)sg_dma_address(dst),
 			     nbytes);
-		HW_DESC_INIT(&desc[*seq_size]);
-		HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
-				     sg_dma_address(src),
-				     nbytes, NS_BIT);
-		HW_DESC_SET_DOUT_DLLI(&desc[*seq_size],
-				      sg_dma_address(dst),
-				      nbytes,
-				      NS_BIT, (areq == NULL)? 0:1);
-		if (areq != NULL) {
-			HW_DESC_SET_QUEUE_LAST_IND(&desc[*seq_size]);
-		}
-		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], flow_mode);
+		hw_desc_init(&desc[*seq_size]);
+		set_din_type(&desc[*seq_size], DMA_DLLI, sg_dma_address(src),
+			     nbytes, NS_BIT);
+		set_dout_dlli(&desc[*seq_size], sg_dma_address(dst),
+			      nbytes, NS_BIT, (!areq ? 0 : 1));
+		if (areq)
+			set_queue_last_ind(&desc[*seq_size]);
+
+		set_flow_mode(&desc[*seq_size], flow_mode);
 		(*seq_size)++;
 	} else {
 		/* bypass */
@@ -695,77 +660,72 @@ ssi_blkcipher_create_data_desc(
 			(unsigned long long)req_ctx->mlli_params.mlli_dma_addr,
 			req_ctx->mlli_params.mlli_len,
 			(unsigned int)ctx_p->drvdata->mlli_sram_addr);
-		HW_DESC_INIT(&desc[*seq_size]);
-		HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
-				     req_ctx->mlli_params.mlli_dma_addr,
-				     req_ctx->mlli_params.mlli_len,
-				     NS_BIT);
-		HW_DESC_SET_DOUT_SRAM(&desc[*seq_size],
-				      ctx_p->drvdata->mlli_sram_addr,
-				      req_ctx->mlli_params.mlli_len);
-		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], BYPASS);
+		hw_desc_init(&desc[*seq_size]);
+		set_din_type(&desc[*seq_size], DMA_DLLI,
+			     req_ctx->mlli_params.mlli_dma_addr,
+			     req_ctx->mlli_params.mlli_len, NS_BIT);
+		set_dout_sram(&desc[*seq_size],
+			      ctx_p->drvdata->mlli_sram_addr,
+			      req_ctx->mlli_params.mlli_len);
+		set_flow_mode(&desc[*seq_size], BYPASS);
 		(*seq_size)++;
 
-		HW_DESC_INIT(&desc[*seq_size]);
-		HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_MLLI,
-			ctx_p->drvdata->mlli_sram_addr,
-				     req_ctx->in_mlli_nents, NS_BIT);
+		hw_desc_init(&desc[*seq_size]);
+		set_din_type(&desc[*seq_size], DMA_MLLI,
+			     ctx_p->drvdata->mlli_sram_addr,
+			     req_ctx->in_mlli_nents, NS_BIT);
 		if (req_ctx->out_nents == 0) {
 			SSI_LOG_DEBUG(" din/dout params addr 0x%08X "
 				     "addr 0x%08X\n",
 			(unsigned int)ctx_p->drvdata->mlli_sram_addr,
 			(unsigned int)ctx_p->drvdata->mlli_sram_addr);
-			HW_DESC_SET_DOUT_MLLI(&desc[*seq_size], 
-			ctx_p->drvdata->mlli_sram_addr,
-					      req_ctx->in_mlli_nents,
-					      NS_BIT,(areq == NULL)? 0:1);
+			set_dout_mlli(&desc[*seq_size],
+				      ctx_p->drvdata->mlli_sram_addr,
+				      req_ctx->in_mlli_nents, NS_BIT,
+				      (!areq ? 0 : 1));
 		} else {
 			SSI_LOG_DEBUG(" din/dout params "
 				     "addr 0x%08X addr 0x%08X\n",
 				(unsigned int)ctx_p->drvdata->mlli_sram_addr,
-				(unsigned int)ctx_p->drvdata->mlli_sram_addr + 
-				(uint32_t)LLI_ENTRY_BYTE_SIZE * 
+				(unsigned int)ctx_p->drvdata->mlli_sram_addr +
+				(u32)LLI_ENTRY_BYTE_SIZE *
 							req_ctx->in_nents);
-			HW_DESC_SET_DOUT_MLLI(&desc[*seq_size], 
-				(ctx_p->drvdata->mlli_sram_addr +
-				LLI_ENTRY_BYTE_SIZE * 
-						req_ctx->in_mlli_nents), 
-				req_ctx->out_mlli_nents, NS_BIT,(areq == NULL)? 0:1);
+			set_dout_mlli(&desc[*seq_size],
+				      (ctx_p->drvdata->mlli_sram_addr +
+				       (LLI_ENTRY_BYTE_SIZE *
+					req_ctx->in_mlli_nents)),
+				      req_ctx->out_mlli_nents, NS_BIT,
+				      (!areq ? 0 : 1));
 		}
-		if (areq != NULL) {
-			HW_DESC_SET_QUEUE_LAST_IND(&desc[*seq_size]);
-		}
-		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], flow_mode);
+		if (areq)
+			set_queue_last_ind(&desc[*seq_size]);
+
+		set_flow_mode(&desc[*seq_size], flow_mode);
 		(*seq_size)++;
 	}
 }
 
 static int ssi_blkcipher_complete(struct device *dev,
-                                  struct ssi_ablkcipher_ctx *ctx_p, 
-                                  struct blkcipher_req_ctx *req_ctx,
-                                  struct scatterlist *dst, struct scatterlist *src,
-                                  void *info, //req info
-                                  unsigned int ivsize,
-                                  void *areq,
-                                  void __iomem *cc_base)
+				struct ssi_ablkcipher_ctx *ctx_p,
+				struct blkcipher_req_ctx *req_ctx,
+				struct scatterlist *dst,
+				struct scatterlist *src,
+				unsigned int ivsize,
+				void *areq,
+				void __iomem *cc_base)
 {
 	int completion_error = 0;
-	uint32_t inflight_counter;
-	DECL_CYCLE_COUNT_RESOURCES;
+	u32 inflight_counter;
 
-	START_CYCLE_COUNT();
 	ssi_buffer_mgr_unmap_blkcipher_request(dev, req_ctx, ivsize, src, dst);
-	info = req_ctx->backup_info;
-	END_CYCLE_COUNT(STAT_OP_TYPE_GENERIC, STAT_PHASE_4);
-
 
 	/*Set the inflight couter value to local variable*/
 	inflight_counter =  ctx_p->drvdata->inflight_counter;
 	/*Decrease the inflight counter*/
-	if(ctx_p->flow_mode == BYPASS && ctx_p->drvdata->inflight_counter > 0)
+	if (ctx_p->flow_mode == BYPASS && ctx_p->drvdata->inflight_counter > 0)
 		ctx_p->drvdata->inflight_counter--;
 
-	if(areq){
+	if (areq) {
 		ablkcipher_request_complete(areq, completion_error);
 		return 0;
 	}
@@ -779,24 +739,22 @@ static int ssi_blkcipher_process(
 	unsigned int nbytes,
 	void *info, //req info
 	unsigned int ivsize,
-	void *areq, 
+	void *areq,
 	enum drv_crypto_direction direction)
 {
 	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
 	struct device *dev = &ctx_p->drvdata->plat_dev->dev;
-	HwDesc_s desc[MAX_ABLKCIPHER_SEQ_LEN];
+	struct cc_hw_desc desc[MAX_ABLKCIPHER_SEQ_LEN];
 	struct ssi_crypto_req ssi_req = {};
-	int rc, seq_len = 0,cts_restore_flag = 0;
-	DECL_CYCLE_COUNT_RESOURCES;
+	int rc, seq_len = 0, cts_restore_flag = 0;
 
 	SSI_LOG_DEBUG("%s areq=%p info=%p nbytes=%d\n",
-		((direction==DRV_CRYPTO_DIRECTION_ENCRYPT)?"Encrypt":"Decrypt"),
+		((direction == DRV_CRYPTO_DIRECTION_ENCRYPT) ? "Encrypt" : "Decrypt"),
 		     areq, info, nbytes);
 
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
 	/* STAT_PHASE_0: Init and sanity checks */
-	START_CYCLE_COUNT();
-	
+
 	/* TODO: check data length according to mode */
 	if (unlikely(validate_data_size(ctx_p, nbytes))) {
 		SSI_LOG_ERR("Unsupported data size %d.\n", nbytes);
@@ -807,9 +765,8 @@ static int ssi_blkcipher_process(
 		/* No data to process is valid */
 		return 0;
 	}
-        /*For CTS in case of data size aligned to 16 use CBC mode*/
-	if (((nbytes % AES_BLOCK_SIZE) == 0) && (ctx_p->cipher_mode == DRV_CIPHER_CBC_CTS)){
-
+	/*For CTS in case of data size aligned to 16 use CBC mode*/
+	if (((nbytes % AES_BLOCK_SIZE) == 0) && (ctx_p->cipher_mode == DRV_CIPHER_CBC_CTS)) {
 		ctx_p->cipher_mode = DRV_CIPHER_CBC;
 		cts_restore_flag = 1;
 	}
@@ -826,83 +783,65 @@ static int ssi_blkcipher_process(
 
 	/* Setup request context */
 	req_ctx->gen_ctx.op_type = direction;
-	
-	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_0);
 
 	/* STAT_PHASE_1: Map buffers */
-	START_CYCLE_COUNT();
-	
+
 	rc = ssi_buffer_mgr_map_blkcipher_request(ctx_p->drvdata, req_ctx, ivsize, nbytes, info, src, dst);
 	if (unlikely(rc != 0)) {
 		SSI_LOG_ERR("map_request() failed\n");
 		goto exit_process;
 	}
 
-	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_1);
-
 	/* STAT_PHASE_2: Create sequence */
-	START_CYCLE_COUNT();
 
 	/* Setup processing */
 #if SSI_CC_HAS_MULTI2
-	if (ctx_p->flow_mode == S_DIN_to_MULTI2) {
-		ssi_blkcipher_create_multi2_setup_desc(tfm,
-						       req_ctx,
-						       ivsize,
-						       desc,
-						       &seq_len);
-	} else
+	if (ctx_p->flow_mode == S_DIN_to_MULTI2)
+		ssi_blkcipher_create_multi2_setup_desc(tfm, req_ctx, ivsize,
+						       desc, &seq_len);
+	else
 #endif /*SSI_CC_HAS_MULTI2*/
-	{
-		ssi_blkcipher_create_setup_desc(tfm,
-						req_ctx,
-						ivsize,
-						nbytes,
-						desc,
-						&seq_len);
-	}
+		ssi_blkcipher_create_setup_desc(tfm, req_ctx, ivsize, nbytes,
+						desc, &seq_len);
 	/* Data processing */
 	ssi_blkcipher_create_data_desc(tfm,
-			      req_ctx, 
+			      req_ctx,
 			      dst, src,
 			      nbytes,
 			      areq,
 			      desc, &seq_len);
 
 	/* do we need to generate IV? */
-	if (req_ctx->is_giv == true) {
+	if (req_ctx->is_giv) {
 		ssi_req.ivgen_dma_addr[0] = req_ctx->gen_ctx.iv_dma_addr;
 		ssi_req.ivgen_dma_addr_len = 1;
 		/* set the IV size (8/16 B long)*/
 		ssi_req.ivgen_size = ivsize;
 	}
-	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_2);
 
 	/* STAT_PHASE_3: Lock HW and push sequence */
-	START_CYCLE_COUNT();
-	
-	rc = send_request(ctx_p->drvdata, &ssi_req, desc, seq_len, (areq == NULL)? 0:1);
-	if(areq != NULL) {
+
+	rc = send_request(ctx_p->drvdata, &ssi_req, desc, seq_len, (!areq) ? 0 : 1);
+	if (areq) {
 		if (unlikely(rc != -EINPROGRESS)) {
 			/* Failed to send the request or request completed synchronously */
 			ssi_buffer_mgr_unmap_blkcipher_request(dev, req_ctx, ivsize, src, dst);
 		}
 
-		END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_3);
 	} else {
 		if (rc != 0) {
 			ssi_buffer_mgr_unmap_blkcipher_request(dev, req_ctx, ivsize, src, dst);
-			END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_3);            
 		} else {
-			END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_3);
-			rc = ssi_blkcipher_complete(dev, ctx_p, req_ctx, dst, src, info, ivsize, NULL, ctx_p->drvdata->cc_base);
-		} 
+			rc = ssi_blkcipher_complete(dev, ctx_p, req_ctx, dst,
+						    src, ivsize, NULL,
+						    ctx_p->drvdata->cc_base);
+		}
 	}
 
 exit_process:
 	if (cts_restore_flag != 0)
 		ctx_p->cipher_mode = DRV_CIPHER_CBC_CTS;
-	
+
 	return rc;
 }
 
@@ -916,86 +855,23 @@ static void ssi_ablkcipher_complete(struct device *dev, void *ssi_req, void __io
 
 	CHECK_AND_RETURN_VOID_UPON_FIPS_ERROR();
 
-	ssi_blkcipher_complete(dev, ctx_p, req_ctx, areq->dst, areq->src, areq->info, ivsize, areq, cc_base);
-}
-
-
-
-static int ssi_sblkcipher_init(struct crypto_tfm *tfm)
-{
-	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
-
-	/* Allocate sync ctx buffer */
-	ctx_p->sync_ctx = kmalloc(sizeof(struct blkcipher_req_ctx), GFP_KERNEL|GFP_DMA);
-	if (!ctx_p->sync_ctx) {
-		SSI_LOG_ERR("Allocating sync ctx buffer in context failed\n");
-		return -ENOMEM;
-	}
-	SSI_LOG_DEBUG("Allocated sync ctx buffer in context ctx_p->sync_ctx=@%p\n",
-								ctx_p->sync_ctx);
-
-	return ssi_blkcipher_init(tfm);
-}
-
-
-static void ssi_sblkcipher_exit(struct crypto_tfm *tfm)
-{
-	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
-	
-	kfree(ctx_p->sync_ctx);
-	SSI_LOG_DEBUG("Free sync ctx buffer in context ctx_p->sync_ctx=@%p\n", ctx_p->sync_ctx);
-
-	ssi_blkcipher_exit(tfm);
-}
-
-#ifdef SYNC_ALGS
-static int ssi_sblkcipher_encrypt(struct blkcipher_desc *desc,
-                        struct scatterlist *dst, struct scatterlist *src,
-                        unsigned int nbytes)
-{
-	struct crypto_blkcipher *blk_tfm = desc->tfm;
-	struct crypto_tfm *tfm = crypto_blkcipher_tfm(blk_tfm);
-	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
-	struct blkcipher_req_ctx *req_ctx = ctx_p->sync_ctx;
-	unsigned int ivsize = crypto_blkcipher_ivsize(blk_tfm);
-
-	req_ctx->backup_info = desc->info;
-	req_ctx->is_giv = false;
-
-	return ssi_blkcipher_process(tfm, req_ctx, dst, src, nbytes, desc->info, ivsize, NULL, DRV_CRYPTO_DIRECTION_ENCRYPT);
-}
-
-static int ssi_sblkcipher_decrypt(struct blkcipher_desc *desc,
-                        struct scatterlist *dst, struct scatterlist *src,
-                        unsigned int nbytes)
-{
-	struct crypto_blkcipher *blk_tfm = desc->tfm;
-	struct crypto_tfm *tfm = crypto_blkcipher_tfm(blk_tfm);
-	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
-	struct blkcipher_req_ctx *req_ctx = ctx_p->sync_ctx;
-	unsigned int ivsize = crypto_blkcipher_ivsize(blk_tfm);
-
-	req_ctx->backup_info = desc->info;
-	req_ctx->is_giv = false;
-
-	return ssi_blkcipher_process(tfm, req_ctx, dst, src, nbytes, desc->info, ivsize, NULL, DRV_CRYPTO_DIRECTION_DECRYPT);
+	ssi_blkcipher_complete(dev, ctx_p, req_ctx, areq->dst, areq->src,
+			       ivsize, areq, cc_base);
 }
-#endif
 
 /* Async wrap functions */
 
 static int ssi_ablkcipher_init(struct crypto_tfm *tfm)
 {
 	struct ablkcipher_tfm *ablktfm = &tfm->crt_ablkcipher;
-	
+
 	ablktfm->reqsize = sizeof(struct blkcipher_req_ctx);
 
 	return ssi_blkcipher_init(tfm);
 }
 
-
-static int ssi_ablkcipher_setkey(struct crypto_ablkcipher *tfm, 
-				const u8 *key, 
+static int ssi_ablkcipher_setkey(struct crypto_ablkcipher *tfm,
+				const u8 *key,
 				unsigned int keylen)
 {
 	return ssi_blkcipher_setkey(crypto_ablkcipher_tfm(tfm), key, keylen);
@@ -1026,7 +902,6 @@ static int ssi_ablkcipher_decrypt(struct ablkcipher_request *req)
 	return ssi_blkcipher_process(tfm, req_ctx, req->dst, req->src, req->nbytes, req->info, ivsize, (void *)req, DRV_CRYPTO_DIRECTION_DECRYPT);
 }
 
-
 /* DX Block cipher alg */
 static struct ssi_alg_template blkcipher_algs[] = {
 /* Async template */
@@ -1047,7 +922,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_XTS,
 		.flow_mode = S_DIN_to_AES,
-        .synchronous = false,
 	},
 	{
 		.name = "xts(aes)",
@@ -1064,7 +938,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_XTS,
 		.flow_mode = S_DIN_to_AES,
-	.synchronous = false,
 	},
 	{
 		.name = "xts(aes)",
@@ -1081,7 +954,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_XTS,
 		.flow_mode = S_DIN_to_AES,
-	.synchronous = false,
 	},
 #endif /*SSI_CC_HAS_AES_XTS*/
 #if SSI_CC_HAS_AES_ESSIV
@@ -1100,7 +972,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_ESSIV,
 		.flow_mode = S_DIN_to_AES,
-		.synchronous = false,
 	},
 	{
 		.name = "essiv(aes)",
@@ -1117,7 +988,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_ESSIV,
 		.flow_mode = S_DIN_to_AES,
-		.synchronous = false,
 	},
 	{
 		.name = "essiv(aes)",
@@ -1134,7 +1004,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_ESSIV,
 		.flow_mode = S_DIN_to_AES,
-		.synchronous = false,
 	},
 #endif /*SSI_CC_HAS_AES_ESSIV*/
 #if SSI_CC_HAS_AES_BITLOCKER
@@ -1153,7 +1022,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_BITLOCKER,
 		.flow_mode = S_DIN_to_AES,
-		.synchronous = false,
 	},
 	{
 		.name = "bitlocker(aes)",
@@ -1170,7 +1038,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_BITLOCKER,
 		.flow_mode = S_DIN_to_AES,
-		.synchronous = false,
 	},
 	{
 		.name = "bitlocker(aes)",
@@ -1187,7 +1054,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_BITLOCKER,
 		.flow_mode = S_DIN_to_AES,
-		.synchronous = false,
 	},
 #endif /*SSI_CC_HAS_AES_BITLOCKER*/
 	{
@@ -1205,7 +1071,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_ECB,
 		.flow_mode = S_DIN_to_AES,
-        .synchronous = false,
 	},
 	{
 		.name = "cbc(aes)",
@@ -1219,10 +1084,9 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			.min_keysize = AES_MIN_KEY_SIZE,
 			.max_keysize = AES_MAX_KEY_SIZE,
 			.ivsize = AES_BLOCK_SIZE,
-			},
+		},
 		.cipher_mode = DRV_CIPHER_CBC,
 		.flow_mode = S_DIN_to_AES,
-        .synchronous = false,
 	},
 	{
 		.name = "ofb(aes)",
@@ -1239,7 +1103,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_OFB,
 		.flow_mode = S_DIN_to_AES,
-        .synchronous = false,
 	},
 #if SSI_CC_HAS_AES_CTS
 	{
@@ -1257,7 +1120,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_CBC_CTS,
 		.flow_mode = S_DIN_to_AES,
-        .synchronous = false,
 	},
 #endif
 	{
@@ -1275,7 +1137,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_CTR,
 		.flow_mode = S_DIN_to_AES,
-        .synchronous = false,
 	},
 	{
 		.name = "cbc(des3_ede)",
@@ -1292,7 +1153,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_CBC,
 		.flow_mode = S_DIN_to_DES,
-        .synchronous = false,
 	},
 	{
 		.name = "ecb(des3_ede)",
@@ -1309,7 +1169,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_ECB,
 		.flow_mode = S_DIN_to_DES,
-        .synchronous = false,
 	},
 	{
 		.name = "cbc(des)",
@@ -1326,7 +1185,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_CBC,
 		.flow_mode = S_DIN_to_DES,
-        .synchronous = false,
 	},
 	{
 		.name = "ecb(des)",
@@ -1343,7 +1201,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_CIPHER_ECB,
 		.flow_mode = S_DIN_to_DES,
-        .synchronous = false,
 	},
 #if SSI_CC_HAS_MULTI2
 	{
@@ -1361,7 +1218,6 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_MULTI2_CBC,
 		.flow_mode = S_DIN_to_MULTI2,
-        .synchronous = false,
 	},
 	{
 		.name = "ofb(multi2)",
@@ -1378,12 +1234,11 @@ static struct ssi_alg_template blkcipher_algs[] = {
 			},
 		.cipher_mode = DRV_MULTI2_OFB,
 		.flow_mode = S_DIN_to_MULTI2,
-        .synchronous = false,
 	},
 #endif /*SSI_CC_HAS_MULTI2*/
 };
 
-static 
+static
 struct ssi_crypto_alg *ssi_ablkcipher_create_alg(struct ssi_alg_template *template)
 {
 	struct ssi_crypto_alg *t_alg;
@@ -1405,19 +1260,13 @@ struct ssi_crypto_alg *ssi_ablkcipher_create_alg(struct ssi_alg_template *templa
 	alg->cra_blocksize = template->blocksize;
 	alg->cra_alignmask = 0;
 	alg->cra_ctxsize = sizeof(struct ssi_ablkcipher_ctx);
-	
-	alg->cra_init = template->synchronous? ssi_sblkcipher_init:ssi_ablkcipher_init;
-	alg->cra_exit = template->synchronous? ssi_sblkcipher_exit:ssi_blkcipher_exit;
-	alg->cra_type = template->synchronous? &crypto_blkcipher_type:&crypto_ablkcipher_type;
-	if(template->synchronous) {
-		alg->cra_blkcipher = template->template_sblkcipher;
-		alg->cra_flags = CRYPTO_ALG_KERN_DRIVER_ONLY |
-				template->type;
-	} else {
-		alg->cra_ablkcipher = template->template_ablkcipher;
-		alg->cra_flags = CRYPTO_ALG_ASYNC | CRYPTO_ALG_KERN_DRIVER_ONLY |
+
+	alg->cra_init = ssi_ablkcipher_init;
+	alg->cra_exit = ssi_blkcipher_exit;
+	alg->cra_type = &crypto_ablkcipher_type;
+	alg->cra_ablkcipher = template->template_ablkcipher;
+	alg->cra_flags = CRYPTO_ALG_ASYNC | CRYPTO_ALG_KERN_DRIVER_ONLY |
 				template->type;
-	}
 
 	t_alg->cipher_mode = template->cipher_mode;
 	t_alg->flow_mode = template->flow_mode;
@@ -1428,12 +1277,13 @@ struct ssi_crypto_alg *ssi_ablkcipher_create_alg(struct ssi_alg_template *templa
 int ssi_ablkcipher_free(struct ssi_drvdata *drvdata)
 {
 	struct ssi_crypto_alg *t_alg, *n;
-	struct ssi_blkcipher_handle *blkcipher_handle = 
+	struct ssi_blkcipher_handle *blkcipher_handle =
 						drvdata->blkcipher_handle;
 	struct device *dev;
+
 	dev = &drvdata->plat_dev->dev;
 
-	if (blkcipher_handle != NULL) {
+	if (blkcipher_handle) {
 		/* Remove registered algs */
 		list_for_each_entry_safe(t_alg, n,
 				&blkcipher_handle->blkcipher_alg_list,
@@ -1448,8 +1298,6 @@ int ssi_ablkcipher_free(struct ssi_drvdata *drvdata)
 	return 0;
 }
 
-
-
 int ssi_ablkcipher_alloc(struct ssi_drvdata *drvdata)
 {
 	struct ssi_blkcipher_handle *ablkcipher_handle;
@@ -1459,7 +1307,7 @@ int ssi_ablkcipher_alloc(struct ssi_drvdata *drvdata)
 
 	ablkcipher_handle = kmalloc(sizeof(struct ssi_blkcipher_handle),
 		GFP_KERNEL);
-	if (ablkcipher_handle == NULL)
+	if (!ablkcipher_handle)
 		return -ENOMEM;
 
 	drvdata->blkcipher_handle = ablkcipher_handle;
@@ -1489,9 +1337,9 @@ int ssi_ablkcipher_alloc(struct ssi_drvdata *drvdata)
 			kfree(t_alg);
 			goto fail0;
 		} else {
-			list_add_tail(&t_alg->entry, 
+			list_add_tail(&t_alg->entry,
 				      &ablkcipher_handle->blkcipher_alg_list);
-			SSI_LOG_DEBUG("Registered %s\n", 
+			SSI_LOG_DEBUG("Registered %s\n",
 					t_alg->crypto_alg.cra_driver_name);
 		}
 	}
diff --git a/drivers/staging/ccree/ssi_cipher.h b/drivers/staging/ccree/ssi_cipher.h
index ba4eb7c4893f..296b375d5d89 100644
--- a/drivers/staging/ccree/ssi_cipher.h
+++ b/drivers/staging/ccree/ssi_cipher.h
@@ -1,21 +1,21 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
 /* \file ssi_cipher.h
-   ARM CryptoCell Cipher Crypto API
+ * ARM CryptoCell Cipher Crypto API
  */
 
 #ifndef __SSI_CIPHER_H__
@@ -26,7 +26,6 @@
 #include "ssi_driver.h"
 #include "ssi_buffer_mgr.h"
 
-
 /* Crypto cipher flags */
 #define CC_CRYPTO_CIPHER_KEY_KFDE0    (1 << 0)
 #define CC_CRYPTO_CIPHER_KEY_KFDE1    (1 << 1)
@@ -36,21 +35,18 @@
 
 #define CC_CRYPTO_CIPHER_KEY_KFDE_MASK (CC_CRYPTO_CIPHER_KEY_KFDE0 | CC_CRYPTO_CIPHER_KEY_KFDE1 | CC_CRYPTO_CIPHER_KEY_KFDE2 | CC_CRYPTO_CIPHER_KEY_KFDE3)
 
-
 struct blkcipher_req_ctx {
 	struct async_gen_req_ctx gen_ctx;
 	enum ssi_req_dma_buf_type dma_buf_type;
-	uint32_t in_nents;
-	uint32_t in_mlli_nents;
-	uint32_t out_nents;
-	uint32_t out_mlli_nents;
-	uint8_t *backup_info; /*store iv for generated IV flow*/
+	u32 in_nents;
+	u32 in_mlli_nents;
+	u32 out_nents;
+	u32 out_mlli_nents;
+	u8 *backup_info; /*store iv for generated IV flow*/
 	bool is_giv;
 	struct mlli_params mlli_params;
 };
 
-
-
 int ssi_ablkcipher_alloc(struct ssi_drvdata *drvdata);
 
 int ssi_ablkcipher_free(struct ssi_drvdata *drvdata);
@@ -63,7 +59,6 @@ int ssi_ablkcipher_free(struct ssi_drvdata *drvdata);
 				CRYPTO_ALG_BULK_DU_4096)
 #endif /* CRYPTO_ALG_BULK_MASK */
 
-
 #ifdef CRYPTO_TFM_REQ_HW_KEY
 
 static inline bool ssi_is_hw_key(struct crypto_tfm *tfm)
@@ -71,7 +66,7 @@ static inline bool ssi_is_hw_key(struct crypto_tfm *tfm)
 	return (crypto_tfm_get_flags(tfm) & CRYPTO_TFM_REQ_HW_KEY);
 }
 
-#else 
+#else
 
 struct arm_hw_key_info {
 	int hw_key1;
@@ -85,5 +80,4 @@ static inline bool ssi_is_hw_key(struct crypto_tfm *tfm)
 
 #endif /* CRYPTO_TFM_REQ_HW_KEY */
 
-
 #endif /*__SSI_CIPHER_H__*/
diff --git a/drivers/staging/ccree/ssi_config.h b/drivers/staging/ccree/ssi_config.h
index d96a5436f6d7..ff7597c2d77e 100644
--- a/drivers/staging/ccree/ssi_config.h
+++ b/drivers/staging/ccree/ssi_config.h
@@ -1,21 +1,21 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
 /* \file ssi_config.h
-   Definitions for ARM CryptoCell Linux Crypto Driver
+ * Definitions for ARM CryptoCell Linux Crypto Driver
  */
 
 #ifndef __SSI_CONFIG_H__
@@ -23,39 +23,14 @@
 
 #include <linux/version.h>
 
-#define DISABLE_COHERENT_DMA_OPS
 //#define FLUSH_CACHE_ALL
 //#define COMPLETION_DELAY
 //#define DX_DUMP_DESCS
 // #define DX_DUMP_BYTES
 // #define CC_DEBUG
 #define ENABLE_CC_SYSFS		/* Enable sysfs interface for debugging REE driver */
-//#define ENABLE_CC_CYCLE_COUNT
 //#define DX_IRQ_DELAY 100000
 #define DMA_BIT_MASK_LEN	48	/* was 32 bit, but for juno's sake it was enlarged to 48 bit */
 
-#if defined ENABLE_CC_CYCLE_COUNT && defined ENABLE_CC_SYSFS
-#define CC_CYCLE_COUNT
-#endif
-
-
-#if defined (CONFIG_ARM64)	// TODO currently only this mode was test on Juno (which is ARM64), need to enable coherent also.
-#define DISABLE_COHERENT_DMA_OPS
-#endif
-
-/* Define the CryptoCell DMA cache coherency signals configuration */
-#if defined (DISABLE_COHERENT_DMA_OPS)
-	/* Software Controlled Cache Coherency (SCCC) */ 
-	#define SSI_CACHE_PARAMS (0x000)
-	/* CC attached to NONE-ACP such as HPP/ACE/AMBA4.
-	 * The customer is responsible to enable/disable this feature
-	 * according to his platform type. */
-	#define DX_HAS_ACP 0
-#else
-	#define SSI_CACHE_PARAMS (0xEEE)
-	/* CC attached to ACP */
-	#define DX_HAS_ACP 1
-#endif
-
 #endif /*__DX_CONFIG_H__*/
 
diff --git a/drivers/staging/ccree/ssi_driver.c b/drivers/staging/ccree/ssi_driver.c
index bc19adce6dee..78709b92736d 100644
--- a/drivers/staging/ccree/ssi_driver.c
+++ b/drivers/staging/ccree/ssi_driver.c
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -57,6 +57,8 @@
 #include <linux/sched.h>
 #include <linux/random.h>
 #include <linux/of.h>
+#include <linux/clk.h>
+#include <linux/of_address.h>
 
 #include "ssi_config.h"
 #include "ssi_driver.h"
@@ -71,15 +73,14 @@
 #include "ssi_pm.h"
 #include "ssi_fips_local.h"
 
-
 #ifdef DX_DUMP_BYTES
-void dump_byte_array(const char *name, const uint8_t *the_array, unsigned long size)
+void dump_byte_array(const char *name, const u8 *the_array, unsigned long size)
 {
-	int i , line_offset = 0, ret = 0;
-	const uint8_t *cur_byte;
+	int i, line_offset = 0, ret = 0;
+	const u8 *cur_byte;
 	char line_buf[80];
 
-	if (the_array == NULL) {
+	if (!the_array) {
 		SSI_LOG_ERR("cannot dump_byte_array - NULL pointer\n");
 		return;
 	}
@@ -87,17 +88,17 @@ void dump_byte_array(const char *name, const uint8_t *the_array, unsigned long s
 	ret = snprintf(line_buf, sizeof(line_buf), "%s[%lu]: ",
 		name, size);
 	if (ret < 0) {
-		SSI_LOG_ERR("snprintf returned %d . aborting buffer array dump\n",ret);
+		SSI_LOG_ERR("snprintf returned %d . aborting buffer array dump\n", ret);
 		return;
 	}
 	line_offset = ret;
-	for (i = 0 , cur_byte = the_array;
+	for (i = 0, cur_byte = the_array;
 	     (i < size) && (line_offset < sizeof(line_buf)); i++, cur_byte++) {
 			ret = snprintf(line_buf + line_offset,
 					sizeof(line_buf) - line_offset,
 					"0x%02X ", *cur_byte);
 		if (ret < 0) {
-			SSI_LOG_ERR("snprintf returned %d . aborting buffer array dump\n",ret);
+			SSI_LOG_ERR("snprintf returned %d . aborting buffer array dump\n", ret);
 			return;
 		}
 		line_offset += ret;
@@ -116,12 +117,10 @@ static irqreturn_t cc_isr(int irq, void *dev_id)
 {
 	struct ssi_drvdata *drvdata = (struct ssi_drvdata *)dev_id;
 	void __iomem *cc_base = drvdata->cc_base;
-	uint32_t irr;
-	uint32_t imr;
-	DECL_CYCLE_COUNT_RESOURCES;
+	u32 irr;
+	u32 imr;
 
 	/* STAT_OP_TYPE_GENERIC STAT_PHASE_0: Interrupt */
-	START_CYCLE_COUNT();
 
 	/* read the interrupt status */
 	irr = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IRR));
@@ -154,12 +153,12 @@ static irqreturn_t cc_isr(int irq, void *dev_id)
 #endif
 	/* AXI error interrupt */
 	if (unlikely((irr & SSI_AXI_ERR_IRQ_MASK) != 0)) {
-		uint32_t axi_err;
-		
+		u32 axi_err;
+
 		/* Read the AXI error ID */
 		axi_err = CC_HAL_READ_REGISTER(CC_REG_OFFSET(CRY_KERNEL, AXIM_MON_ERR));
 		SSI_LOG_DEBUG("AXI completion error: axim_mon_err=0x%08X\n", axi_err);
-		
+
 		irr &= ~SSI_AXI_ERR_IRQ_MASK;
 	}
 
@@ -168,15 +167,12 @@ static irqreturn_t cc_isr(int irq, void *dev_id)
 		/* Just warning */
 	}
 
-	END_CYCLE_COUNT(STAT_OP_TYPE_GENERIC, STAT_PHASE_0);
-	START_CYCLE_COUNT_AT(drvdata->isr_exit_cycles);
-
 	return IRQ_HANDLED;
 }
 
 int init_cc_regs(struct ssi_drvdata *drvdata, bool is_probe)
 {
-	unsigned int val;
+	unsigned int val, cache_params;
 	void __iomem *cc_base = drvdata->cc_base;
 
 	/* Unmask all AXI interrupt sources AXI_CFG1 register */
@@ -192,7 +188,7 @@ int init_cc_regs(struct ssi_drvdata *drvdata, bool is_probe)
 	/* Unmask relevant interrupt cause */
 	val = (~(SSI_COMP_IRQ_MASK | SSI_AXI_ERR_IRQ_MASK | SSI_GPR0_IRQ_MASK));
 	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IMR), val);
-		
+
 #ifdef DX_HOST_IRQ_TIMER_INIT_VAL_REG_OFFSET
 #ifdef DX_IRQ_DELAY
 	/* Set CC IRQ delay */
@@ -205,15 +201,20 @@ int init_cc_regs(struct ssi_drvdata *drvdata, bool is_probe)
 	}
 #endif
 
+	cache_params = (drvdata->coherent ? CC_COHERENT_CACHE_PARAMS : 0x0);
+
 	val = CC_HAL_READ_REGISTER(CC_REG_OFFSET(CRY_KERNEL, AXIM_CACHE_PARAMS));
-	if (is_probe == true) {
+
+	if (is_probe)
 		SSI_LOG_INFO("Cache params previous: 0x%08X\n", val);
-	}
-	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(CRY_KERNEL, AXIM_CACHE_PARAMS), SSI_CACHE_PARAMS);
+
+	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(CRY_KERNEL, AXIM_CACHE_PARAMS),
+			      cache_params);
 	val = CC_HAL_READ_REGISTER(CC_REG_OFFSET(CRY_KERNEL, AXIM_CACHE_PARAMS));
-	if (is_probe == true) {
-		SSI_LOG_INFO("Cache params current: 0x%08X  (expected: 0x%08X)\n", val, SSI_CACHE_PARAMS);
-	}
+
+	if (is_probe)
+		SSI_LOG_INFO("Cache params current: 0x%08X (expect: 0x%08X)\n",
+			     val, cache_params);
 
 	return 0;
 }
@@ -224,15 +225,20 @@ static int init_cc_resources(struct platform_device *plat_dev)
 	void __iomem *cc_base = NULL;
 	bool irq_registered = false;
 	struct ssi_drvdata *new_drvdata = kzalloc(sizeof(struct ssi_drvdata), GFP_KERNEL);
-	uint32_t signature_val;
+	struct device *dev = &plat_dev->dev;
+	struct device_node *np = dev->of_node;
+	u32 signature_val;
 	int rc = 0;
 
-	if (unlikely(new_drvdata == NULL)) {
+	if (unlikely(!new_drvdata)) {
 		SSI_LOG_ERR("Failed to allocate drvdata");
 		rc = -ENOMEM;
 		goto init_cc_res_err;
 	}
 
+	new_drvdata->clk = of_clk_get(np, 0);
+	new_drvdata->coherent = of_dma_is_coherent(np);
+
 	/*Initialize inflight counter used in dx_ablkcipher_secure_complete used for count of BYSPASS blocks operations*/
 	new_drvdata->inflight_counter = 0;
 
@@ -240,7 +246,7 @@ static int init_cc_resources(struct platform_device *plat_dev)
 	/* Get device resources */
 	/* First CC registers space */
 	new_drvdata->res_mem = platform_get_resource(plat_dev, IORESOURCE_MEM, 0);
-	if (unlikely(new_drvdata->res_mem == NULL)) {
+	if (unlikely(!new_drvdata->res_mem)) {
 		SSI_LOG_ERR("Failed getting IO memory resource\n");
 		rc = -ENODEV;
 		goto init_cc_res_err;
@@ -251,14 +257,14 @@ static int init_cc_resources(struct platform_device *plat_dev)
 		(unsigned long long)new_drvdata->res_mem->end);
 	/* Map registers space */
 	req_mem_cc_regs = request_mem_region(new_drvdata->res_mem->start, resource_size(new_drvdata->res_mem), "arm_cc7x_regs");
-	if (unlikely(req_mem_cc_regs == NULL)) {
+	if (unlikely(!req_mem_cc_regs)) {
 		SSI_LOG_ERR("Couldn't allocate registers memory region at "
 			     "0x%08X\n", (unsigned int)new_drvdata->res_mem->start);
 		rc = -EBUSY;
 		goto init_cc_res_err;
 	}
 	cc_base = ioremap(new_drvdata->res_mem->start, resource_size(new_drvdata->res_mem));
-	if (unlikely(cc_base == NULL)) {
+	if (unlikely(!cc_base)) {
 		SSI_LOG_ERR("ioremap[CC](0x%08X,0x%08X) failed\n",
 			(unsigned int)new_drvdata->res_mem->start, (unsigned int)resource_size(new_drvdata->res_mem));
 		rc = -ENOMEM;
@@ -266,11 +272,10 @@ static int init_cc_resources(struct platform_device *plat_dev)
 	}
 	SSI_LOG_DEBUG("CC registers mapped from %pa to 0x%p\n", &new_drvdata->res_mem->start, cc_base);
 	new_drvdata->cc_base = cc_base;
-	
 
 	/* Then IRQ */
 	new_drvdata->res_irq = platform_get_resource(plat_dev, IORESOURCE_IRQ, 0);
-	if (unlikely(new_drvdata->res_irq == NULL)) {
+	if (unlikely(!new_drvdata->res_irq)) {
 		SSI_LOG_ERR("Failed getting IRQ resource\n");
 		rc = -ENODEV;
 		goto init_cc_res_err;
@@ -291,9 +296,13 @@ static int init_cc_resources(struct platform_device *plat_dev)
 
 	new_drvdata->plat_dev = plat_dev;
 
-	if(new_drvdata->plat_dev->dev.dma_mask == NULL)
+	rc = cc_clk_on(new_drvdata);
+	if (rc)
+		goto init_cc_res_err;
+
+	if (!new_drvdata->plat_dev->dev.dma_mask)
 	{
-		new_drvdata->plat_dev->dev.dma_mask = & new_drvdata->plat_dev->dev.coherent_dma_mask;
+		new_drvdata->plat_dev->dev.dma_mask = &new_drvdata->plat_dev->dev.coherent_dma_mask;
 	}
 	if (!new_drvdata->plat_dev->dev.coherent_dma_mask)
 	{
@@ -304,7 +313,7 @@ static int init_cc_resources(struct platform_device *plat_dev)
 	signature_val = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_SIGNATURE));
 	if (signature_val != DX_DEV_SIGNATURE) {
 		SSI_LOG_ERR("Invalid CC signature: SIGNATURE=0x%08X != expected=0x%08X\n",
-			signature_val, (uint32_t)DX_DEV_SIGNATURE);
+			signature_val, (u32)DX_DEV_SIGNATURE);
 		rc = -EINVAL;
 		goto init_cc_res_err;
 	}
@@ -396,8 +405,8 @@ static int init_cc_resources(struct platform_device *plat_dev)
 
 init_cc_res_err:
 	SSI_LOG_ERR("Freeing CC HW resources!\n");
-	
-	if (new_drvdata != NULL) {
+
+	if (new_drvdata) {
 		ssi_aead_free(new_drvdata);
 		ssi_hash_free(new_drvdata);
 		ssi_ablkcipher_free(new_drvdata);
@@ -410,8 +419,8 @@ init_cc_res_err:
 #ifdef ENABLE_CC_SYSFS
 		ssi_sysfs_fini();
 #endif
-	
-		if (req_mem_cc_regs != NULL) {
+
+		if (req_mem_cc_regs) {
 			if (irq_registered) {
 				free_irq(new_drvdata->res_irq->start, new_drvdata);
 				new_drvdata->res_irq = NULL;
@@ -432,9 +441,8 @@ init_cc_res_err:
 void fini_cc_regs(struct ssi_drvdata *drvdata)
 {
 	/* Mask all interrupts */
-	WRITE_REGISTER(drvdata->cc_base + 
+	WRITE_REGISTER(drvdata->cc_base +
 		       CC_REG_OFFSET(HOST_RGF, HOST_IMR), 0xFFFFFFFF);
-
 }
 
 static void cleanup_cc_resources(struct platform_device *plat_dev)
@@ -442,9 +450,9 @@ static void cleanup_cc_resources(struct platform_device *plat_dev)
 	struct ssi_drvdata *drvdata =
 		(struct ssi_drvdata *)dev_get_drvdata(&plat_dev->dev);
 
-        ssi_aead_free(drvdata);
-        ssi_hash_free(drvdata);
-        ssi_ablkcipher_free(drvdata);
+	ssi_aead_free(drvdata);
+	ssi_hash_free(drvdata);
+	ssi_ablkcipher_free(drvdata);
 	ssi_ivgen_fini(drvdata);
 	ssi_power_mgr_fini(drvdata);
 	ssi_buffer_mgr_fini(drvdata);
@@ -455,15 +463,12 @@ static void cleanup_cc_resources(struct platform_device *plat_dev)
 	ssi_sysfs_fini();
 #endif
 
-	/* Mask all interrupts */
-	WRITE_REGISTER(drvdata->cc_base + CC_REG_OFFSET(HOST_RGF, HOST_IMR),
-		0xFFFFFFFF);
+	fini_cc_regs(drvdata);
+	cc_clk_off(drvdata);
 	free_irq(drvdata->res_irq->start, drvdata);
 	drvdata->res_irq = NULL;
 
-	fini_cc_regs(drvdata);
-
-	if (drvdata->cc_base != NULL) {
+	if (drvdata->cc_base) {
 		iounmap(drvdata->cc_base);
 		release_mem_region(drvdata->res_mem->start,
 			resource_size(drvdata->res_mem));
@@ -475,11 +480,38 @@ static void cleanup_cc_resources(struct platform_device *plat_dev)
 	dev_set_drvdata(&plat_dev->dev, NULL);
 }
 
+int cc_clk_on(struct ssi_drvdata *drvdata)
+{
+	struct clk *clk = drvdata->clk;
+	int rc;
+
+	if (IS_ERR(clk))
+		/* Not all devices have a clock associated with CCREE  */
+		return 0;
+
+	rc = clk_prepare_enable(clk);
+	if (rc)
+		return rc;
+
+	return 0;
+}
+
+void cc_clk_off(struct ssi_drvdata *drvdata)
+{
+	struct clk *clk = drvdata->clk;
+
+	if (IS_ERR(clk))
+		/* Not all devices have a clock associated with CCREE */
+		return;
+
+	clk_disable_unprepare(clk);
+}
+
 static int cc7x_probe(struct platform_device *plat_dev)
 {
 	int rc;
 #if defined(CONFIG_ARM) && defined(CC_DEBUG)
-	uint32_t ctr, cacheline_size;
+	u32 ctr, cacheline_size;
 
 	asm volatile("mrc p15, 0, %0, c0, c0, 1" : "=r" (ctr));
 	cacheline_size =  4 << ((ctr >> 16) & 0xf);
@@ -489,7 +521,7 @@ static int cc7x_probe(struct platform_device *plat_dev)
 	asm volatile("mrc p15, 0, %0, c0, c0, 0" : "=r" (ctr));
 	SSI_LOG_DEBUG("Main ID register (MIDR): Implementer 0x%02X, Arch 0x%01X,"
 		     " Part 0x%03X, Rev r%dp%d\n",
-		(ctr>>24), (ctr>>16)&0xF, (ctr>>4)&0xFFF, (ctr>>20)&0xF, ctr&0xF);
+		(ctr >> 24), (ctr >> 16) & 0xF, (ctr >> 4) & 0xFFF, (ctr >> 20) & 0xF, ctr & 0xF);
 #endif
 
 	/* Map registers space */
@@ -505,29 +537,26 @@ static int cc7x_probe(struct platform_device *plat_dev)
 static int cc7x_remove(struct platform_device *plat_dev)
 {
 	SSI_LOG_DEBUG("Releasing cc7x resources...\n");
-	
+
 	cleanup_cc_resources(plat_dev);
 
 	SSI_LOG(KERN_INFO, "ARM cc7x_ree device terminated\n");
-#ifdef ENABLE_CYCLE_COUNT
-	display_all_stat_db();
-#endif
-	
+
 	return 0;
 }
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 static struct dev_pm_ops arm_cc7x_driver_pm = {
 	SET_RUNTIME_PM_OPS(ssi_power_mgr_runtime_suspend, ssi_power_mgr_runtime_resume, NULL)
 };
 #endif
 
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 #define	DX_DRIVER_RUNTIME_PM	(&arm_cc7x_driver_pm)
 #else
 #define	DX_DRIVER_RUNTIME_PM	NULL
 #endif
 
-
 #ifdef CONFIG_OF
 static const struct of_device_id arm_cc7x_dev_of_match[] = {
 	{.compatible = "arm,cryptocell-712-ree"},
diff --git a/drivers/staging/ccree/ssi_driver.h b/drivers/staging/ccree/ssi_driver.h
index 891958b99634..c1ed61f1a202 100644
--- a/drivers/staging/ccree/ssi_driver.h
+++ b/drivers/staging/ccree/ssi_driver.h
@@ -1,21 +1,21 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
 /* \file ssi_driver.h
-   ARM CryptoCell Linux Crypto Driver
+ * ARM CryptoCell Linux Crypto Driver
  */
 
 #ifndef __SSI_DRIVER_H__
@@ -36,29 +36,27 @@
 #include <crypto/authenc.h>
 #include <crypto/hash.h>
 #include <linux/version.h>
-
-#ifndef INT32_MAX /* Missing in Linux kernel */
-#define INT32_MAX 0x7FFFFFFFL
-#endif
+#include <linux/clk.h>
 
 /* Registers definitions from shared/hw/ree_include */
 #include "dx_reg_base_host.h"
 #include "dx_host.h"
-#define DX_CC_HOST_VIRT /* must be defined before including dx_cc_regs.h */
-#include "cc_hw_queue_defs.h"
 #include "cc_regs.h"
 #include "dx_reg_common.h"
 #include "cc_hal.h"
-#include "ssi_sram_mgr.h"
 #define CC_SUPPORT_SHA DX_DEV_SHA_MAX
 #include "cc_crypto_ctx.h"
 #include "ssi_sysfs.h"
 #include "hash_defs.h"
 #include "ssi_fips_local.h"
+#include "cc_hw_queue_defs.h"
+#include "ssi_sram_mgr.h"
 
 #define DRV_MODULE_VERSION "3.0"
 
 #define SSI_DEV_NAME_STR "cc715ree"
+#define CC_COHERENT_CACHE_PARAMS 0xEEE
+
 #define SSI_CC_HAS_AES_CCM 1
 #define SSI_CC_HAS_AES_GCM 1
 #define SSI_CC_HAS_AES_XTS 1
@@ -89,12 +87,13 @@
 /* Definitions for HW descriptors DIN/DOUT fields */
 #define NS_BIT 1
 #define AXI_ID 0
-/* AXI_ID is not actually the AXI ID of the transaction but the value of AXI_ID 
-   field in the HW descriptor. The DMA engine +8 that value. */
+/* AXI_ID is not actually the AXI ID of the transaction but the value of AXI_ID
+ * field in the HW descriptor. The DMA engine +8 that value.
+ */
 
 /* Logging macros */
 #define SSI_LOG(level, format, ...) \
-	printk(level "cc715ree::%s: " format , __func__, ##__VA_ARGS__)
+	printk(level "cc715ree::%s: " format, __func__, ##__VA_ARGS__)
 #define SSI_LOG_ERR(format, ...) SSI_LOG(KERN_ERR, format, ##__VA_ARGS__)
 #define SSI_LOG_WARNING(format, ...) SSI_LOG(KERN_WARNING, format, ##__VA_ARGS__)
 #define SSI_LOG_NOTICE(format, ...) SSI_LOG(KERN_NOTICE, format, ##__VA_ARGS__)
@@ -108,21 +107,18 @@
 #define MIN(a, b) (((a) < (b)) ? (a) : (b))
 #define MAX(a, b) (((a) > (b)) ? (a) : (b))
 
-#define SSI_MAX_IVGEN_DMA_ADDRESSES 	3
+#define SSI_MAX_IVGEN_DMA_ADDRESSES	3
 struct ssi_crypto_req {
 	void (*user_cb)(struct device *dev, void *req, void __iomem *cc_base);
 	void *user_arg;
-	dma_addr_t ivgen_dma_addr[SSI_MAX_IVGEN_DMA_ADDRESSES]; /* For the first 'ivgen_dma_addr_len' addresses of this array,
-					 generated IV would be placed in it by send_request().
-					 Same generated IV for all addresses! */
+	dma_addr_t ivgen_dma_addr[SSI_MAX_IVGEN_DMA_ADDRESSES];
+	/* For the first 'ivgen_dma_addr_len' addresses of this array,
+	 * generated IV would be placed in it by send_request().
+	 * Same generated IV for all addresses!
+	 */
 	unsigned int ivgen_dma_addr_len; /* Amount of 'ivgen_dma_addr' elements to be filled. */
 	unsigned int ivgen_size; /* The generated IV size required, 8/16 B allowed. */
 	struct completion seq_compl; /* request completion */
-#ifdef ENABLE_CYCLE_COUNT
-	enum stat_op op_type;
-	cycles_t submit_cycle;
-	bool is_monitored_p;
-#endif
 };
 
 /**
@@ -136,15 +132,13 @@ struct ssi_drvdata {
 	struct resource *res_mem;
 	struct resource *res_irq;
 	void __iomem *cc_base;
-#ifdef DX_BASE_ENV_REGS
-	void __iomem *env_base; /* ARM CryptoCell development FPGAs only */
-#endif
 	unsigned int irq;
-	uint32_t irq_mask;
-	uint32_t fw_ver;
+	u32 irq_mask;
+	u32 fw_ver;
 	/* Calibration time of start/stop
-	*  monitor descriptors */
-	uint32_t monitor_null_cycles;
+	 * monitor descriptors
+	 */
+	u32 monitor_null_cycles;
 	struct platform_device *plat_dev;
 	ssi_sram_addr_t mlli_sram_addr;
 	struct completion icache_setup_completion;
@@ -156,12 +150,9 @@ struct ssi_drvdata {
 	void *fips_handle;
 	void *ivgen_handle;
 	void *sram_mgr_handle;
-
-#ifdef ENABLE_CYCLE_COUNT
-	cycles_t isr_exit_cycles; /* Save for isr-to-tasklet latency */
-#endif
-	uint32_t inflight_counter;
-
+	u32 inflight_counter;
+	struct clk *clk;
+	bool coherent;
 };
 
 struct ssi_crypto_alg {
@@ -189,7 +180,6 @@ struct ssi_alg_template {
 	int cipher_mode;
 	int flow_mode; /* Note: currently, refers to the cipher mode only. */
 	int auth_mode;
-	bool synchronous;
 	struct ssi_drvdata *drvdata;
 };
 
@@ -199,30 +189,16 @@ struct async_gen_req_ctx {
 };
 
 #ifdef DX_DUMP_BYTES
-void dump_byte_array(const char *name, const uint8_t *the_array, unsigned long size);
+void dump_byte_array(const char *name, const u8 *the_array, unsigned long size);
 #else
 #define dump_byte_array(name, array, size) do {	\
 } while (0);
 #endif
 
-#ifdef ENABLE_CYCLE_COUNT
-#define DECL_CYCLE_COUNT_RESOURCES cycles_t _last_cycles_read
-#define START_CYCLE_COUNT() do { _last_cycles_read = get_cycles(); } while (0)
-#define END_CYCLE_COUNT(_stat_op_type, _stat_phase) update_host_stat(_stat_op_type, _stat_phase, get_cycles() - _last_cycles_read)
-#define GET_START_CYCLE_COUNT() _last_cycles_read
-#define START_CYCLE_COUNT_AT(_var) do { _var = get_cycles(); } while(0)
-#define END_CYCLE_COUNT_AT(_var, _stat_op_type, _stat_phase) update_host_stat(_stat_op_type, _stat_phase, get_cycles() - _var)
-#else
-#define DECL_CYCLE_COUNT_RESOURCES 
-#define START_CYCLE_COUNT() do { } while (0)
-#define END_CYCLE_COUNT(_stat_op_type, _stat_phase) do { } while (0)
-#define GET_START_CYCLE_COUNT() 0
-#define START_CYCLE_COUNT_AT(_var) do { } while (0)
-#define END_CYCLE_COUNT_AT(_var, _stat_op_type, _stat_phase) do { } while (0)
-#endif /*ENABLE_CYCLE_COUNT*/
-
 int init_cc_regs(struct ssi_drvdata *drvdata, bool is_probe);
 void fini_cc_regs(struct ssi_drvdata *drvdata);
+int cc_clk_on(struct ssi_drvdata *drvdata);
+void cc_clk_off(struct ssi_drvdata *drvdata);
 
 #endif /*__SSI_DRIVER_H__*/
 
diff --git a/drivers/staging/ccree/ssi_fips.c b/drivers/staging/ccree/ssi_fips.c
index 50f748511979..fdc40f38332a 100644
--- a/drivers/staging/ccree/ssi_fips.c
+++ b/drivers/staging/ccree/ssi_fips.c
@@ -1,42 +1,39 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
-
 /**************************************************************
-This file defines the driver FIPS APIs                                                             *
-***************************************************************/
+ * This file defines the driver FIPS APIs                     *
+ **************************************************************/
 
 #include <linux/module.h>
 #include "ssi_fips.h"
 
-
-extern int ssi_fips_ext_get_state(ssi_fips_state_t *p_state);
-extern int ssi_fips_ext_get_error(ssi_fips_error_t *p_err);
+extern int ssi_fips_ext_get_state(enum cc_fips_state_t *p_state);
+extern int ssi_fips_ext_get_error(enum cc_fips_error *p_err);
 
 /*
-This function returns the REE FIPS state.  
-It should be called by kernel module. 
-*/
-int ssi_fips_get_state(ssi_fips_state_t *p_state)
+ * This function returns the REE FIPS state.
+ * It should be called by kernel module.
+ */
+int ssi_fips_get_state(enum cc_fips_state_t *p_state)
 {
-        int rc = 0;
+	int rc = 0;
 
-	if (p_state == NULL) {
+	if (!p_state)
 		return -EINVAL;
-	}
 
 	rc = ssi_fips_ext_get_state(p_state);
 
@@ -46,16 +43,15 @@ int ssi_fips_get_state(ssi_fips_state_t *p_state)
 EXPORT_SYMBOL(ssi_fips_get_state);
 
 /*
-This function returns the REE FIPS error.  
-It should be called by kernel module. 
-*/
-int ssi_fips_get_error(ssi_fips_error_t *p_err)
+ * This function returns the REE FIPS error.
+ * It should be called by kernel module.
+ */
+int ssi_fips_get_error(enum cc_fips_error *p_err)
 {
-        int rc = 0;
+	int rc = 0;
 
-	if (p_err == NULL) {
+	if (!p_err)
 		return -EINVAL;
-	}
 
 	rc = ssi_fips_ext_get_error(p_err);
 
diff --git a/drivers/staging/ccree/ssi_fips.h b/drivers/staging/ccree/ssi_fips.h
index 19bcdeb34308..4f5c6a9a8363 100644
--- a/drivers/staging/ccree/ssi_fips.h
+++ b/drivers/staging/ccree/ssi_fips.h
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -17,26 +17,19 @@
 #ifndef __SSI_FIPS_H__
 #define __SSI_FIPS_H__
 
+/*!
+ * @file
+ * @brief This file contains FIPS related defintions and APIs.
+ */
 
-#ifndef INT32_MAX /* Missing in Linux kernel */
-#define INT32_MAX 0x7FFFFFFFL
-#endif
-
-
-/*! 
-@file
-@brief This file contains FIPS related defintions and APIs.
-*/
-
-typedef enum ssi_fips_state {
-        CC_FIPS_STATE_NOT_SUPPORTED = 0,
-        CC_FIPS_STATE_SUPPORTED,
-        CC_FIPS_STATE_ERROR,
-        CC_FIPS_STATE_RESERVE32B = INT32_MAX
-} ssi_fips_state_t;
-
+enum cc_fips_state {
+	CC_FIPS_STATE_NOT_SUPPORTED = 0,
+	CC_FIPS_STATE_SUPPORTED,
+	CC_FIPS_STATE_ERROR,
+	CC_FIPS_STATE_RESERVE32B = S32_MAX
+};
 
-typedef enum ssi_fips_error {
+enum cc_fips_error {
 	CC_REE_FIPS_ERROR_OK = 0,
 	CC_REE_FIPS_ERROR_GENERAL,
 	CC_REE_FIPS_ERROR_FROM_TEE,
@@ -58,13 +51,11 @@ typedef enum ssi_fips_error {
 	CC_REE_FIPS_ERROR_HMAC_SHA256_PUT,
 	CC_REE_FIPS_ERROR_HMAC_SHA512_PUT,
 	CC_REE_FIPS_ERROR_ROM_CHECKSUM,
-	CC_REE_FIPS_ERROR_RESERVE32B = INT32_MAX
-} ssi_fips_error_t;
-
-
+	CC_REE_FIPS_ERROR_RESERVE32B = S32_MAX
+};
 
-int ssi_fips_get_state(ssi_fips_state_t *p_state);
-int ssi_fips_get_error(ssi_fips_error_t *p_err);
+int ssi_fips_get_state(enum cc_fips_state *p_state);
+int ssi_fips_get_error(enum cc_fips_error *p_err);
 
 #endif  /*__SSI_FIPS_H__*/
 
diff --git a/drivers/staging/ccree/ssi_fips_data.h b/drivers/staging/ccree/ssi_fips_data.h
index 3fddd8f74e07..c41671dbee40 100644
--- a/drivers/staging/ccree/ssi_fips_data.h
+++ b/drivers/staging/ccree/ssi_fips_data.h
@@ -1,67 +1,66 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
 /*
-The test vectors were taken from:
-
-* AES
-NIST Special Publication 800-38A 2001 Edition
-Recommendation for Block Cipher Modes of Operation
-http://csrc.nist.gov/publications/nistpubs/800-38a/sp800-38a.pdf
-Appendix F: Example Vectors for Modes of Operation of the AES
-
-* AES CTS
-Advanced Encryption Standard (AES) Encryption for Kerberos 5
-February 2005
-https://tools.ietf.org/html/rfc3962#appendix-B
-B.  Sample Test Vectors
-
-* AES XTS
-http://csrc.nist.gov/groups/STM/cavp/#08
-http://csrc.nist.gov/groups/STM/cavp/documents/aes/XTSTestVectors.zip
-
-* AES CMAC
-http://csrc.nist.gov/groups/STM/cavp/index.html#07
-http://csrc.nist.gov/groups/STM/cavp/documents/mac/cmactestvectors.zip
- 
-* AES-CCM
-http://csrc.nist.gov/groups/STM/cavp/#07
-http://csrc.nist.gov/groups/STM/cavp/documents/mac/ccmtestvectors.zip
-
-* AES-GCM
-http://csrc.nist.gov/groups/STM/cavp/documents/mac/gcmtestvectors.zip
-
-* Triple-DES
-NIST Special Publication 800-67 January 2012
-Recommendation for the Triple Data Encryption Algorithm (TDEA) Block Cipher
-http://csrc.nist.gov/publications/nistpubs/800-67-Rev1/SP-800-67-Rev1.pdf
-APPENDIX B: EXAMPLE OF TDEA FORWARD AND INVERSE CIPHER OPERATIONS
-and
-http://csrc.nist.gov/groups/STM/cavp/#01
-http://csrc.nist.gov/groups/STM/cavp/documents/des/tdesmct_intermediate.zip
-
-* HASH
-http://csrc.nist.gov/groups/STM/cavp/#03
-http://csrc.nist.gov/groups/STM/cavp/documents/shs/shabytetestvectors.zip 
- 
-* HMAC 
-http://csrc.nist.gov/groups/STM/cavp/#07
-http://csrc.nist.gov/groups/STM/cavp/documents/mac/hmactestvectors.zip 
- 
-*/
+ * The test vectors were taken from:
+ *
+ * * AES
+ * NIST Special Publication 800-38A 2001 Edition
+ * Recommendation for Block Cipher Modes of Operation
+ * http://csrc.nist.gov/publications/nistpubs/800-38a/sp800-38a.pdf
+ * Appendix F: Example Vectors for Modes of Operation of the AES
+ *
+ * * AES CTS
+ * Advanced Encryption Standard (AES) Encryption for Kerberos 5
+ * February 2005
+ * https://tools.ietf.org/html/rfc3962#appendix-B
+ * B.  Sample Test Vectors
+ *
+ * * AES XTS
+ * http://csrc.nist.gov/groups/STM/cavp/#08
+ * http://csrc.nist.gov/groups/STM/cavp/documents/aes/XTSTestVectors.zip
+ *
+ * * AES CMAC
+ * http://csrc.nist.gov/groups/STM/cavp/index.html#07
+ * http://csrc.nist.gov/groups/STM/cavp/documents/mac/cmactestvectors.zip
+ *
+ * * AES-CCM
+ * http://csrc.nist.gov/groups/STM/cavp/#07
+ * http://csrc.nist.gov/groups/STM/cavp/documents/mac/ccmtestvectors.zip
+ *
+ * * AES-GCM
+ * http://csrc.nist.gov/groups/STM/cavp/documents/mac/gcmtestvectors.zip
+ *
+ * * Triple-DES
+ * NIST Special Publication 800-67 January 2012
+ * Recommendation for the Triple Data Encryption Algorithm (TDEA) Block Cipher
+ * http://csrc.nist.gov/publications/nistpubs/800-67-Rev1/SP-800-67-Rev1.pdf
+ * APPENDIX B: EXAMPLE OF TDEA FORWARD AND INVERSE CIPHER OPERATIONS
+ * and
+ * http://csrc.nist.gov/groups/STM/cavp/#01
+ * http://csrc.nist.gov/groups/STM/cavp/documents/des/tdesmct_intermediate.zip
+ *
+ * * HASH
+ * http://csrc.nist.gov/groups/STM/cavp/#03
+ * http://csrc.nist.gov/groups/STM/cavp/documents/shs/shabytetestvectors.zip
+ *
+ * * HMAC
+ * http://csrc.nist.gov/groups/STM/cavp/#07
+ * http://csrc.nist.gov/groups/STM/cavp/documents/mac/hmactestvectors.zip
+ */
 
 /* NIST AES */
 #define AES_128_BIT_KEY_SIZE    16
@@ -86,19 +85,18 @@ http://csrc.nist.gov/groups/STM/cavp/documents/mac/hmactestvectors.zip
 
 #define NIST_AES_CBC_IV         { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f }
 #define NIST_AES_128_CBC_CIPHER { 0x76, 0x49, 0xab, 0xac, 0x81, 0x19, 0xb2, 0x46, 0xce, 0xe9, 0x8e, 0x9b, 0x12, 0xe9, 0x19, 0x7d }
-#define NIST_AES_192_CBC_CIPHER { 0x4f, 0x02, 0x1d, 0xb2, 0x43, 0xbc, 0x63, 0x3d, 0x71, 0x78, 0x18, 0x3a, 0x9f, 0xa0, 0x71, 0xe8 } 
-#define NIST_AES_256_CBC_CIPHER { 0xf5, 0x8c, 0x4c, 0x04, 0xd6, 0xe5, 0xf1, 0xba, 0x77, 0x9e, 0xab, 0xfb, 0x5f, 0x7b, 0xfb, 0xd6 } 
+#define NIST_AES_192_CBC_CIPHER { 0x4f, 0x02, 0x1d, 0xb2, 0x43, 0xbc, 0x63, 0x3d, 0x71, 0x78, 0x18, 0x3a, 0x9f, 0xa0, 0x71, 0xe8 }
+#define NIST_AES_256_CBC_CIPHER { 0xf5, 0x8c, 0x4c, 0x04, 0xd6, 0xe5, 0xf1, 0xba, 0x77, 0x9e, 0xab, 0xfb, 0x5f, 0x7b, 0xfb, 0xd6 }
 
 #define NIST_AES_OFB_IV         { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f }
 #define NIST_AES_128_OFB_CIPHER { 0x3b, 0x3f, 0xd9, 0x2e, 0xb7, 0x2d, 0xad, 0x20, 0x33, 0x34, 0x49, 0xf8, 0xe8, 0x3c, 0xfb, 0x4a }
-#define NIST_AES_192_OFB_CIPHER { 0xcd, 0xc8, 0x0d, 0x6f, 0xdd, 0xf1, 0x8c, 0xab, 0x34, 0xc2, 0x59, 0x09, 0xc9, 0x9a, 0x41, 0x74 } 
+#define NIST_AES_192_OFB_CIPHER { 0xcd, 0xc8, 0x0d, 0x6f, 0xdd, 0xf1, 0x8c, 0xab, 0x34, 0xc2, 0x59, 0x09, 0xc9, 0x9a, 0x41, 0x74 }
 #define NIST_AES_256_OFB_CIPHER { 0xdc, 0x7e, 0x84, 0xbf, 0xda, 0x79, 0x16, 0x4b, 0x7e, 0xcd, 0x84, 0x86, 0x98, 0x5d, 0x38, 0x60 }
 
 #define NIST_AES_CTR_IV         { 0xf0, 0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa, 0xfb, 0xfc, 0xfd, 0xfe, 0xff }
 #define NIST_AES_128_CTR_CIPHER { 0x87, 0x4d, 0x61, 0x91, 0xb6, 0x20, 0xe3, 0x26, 0x1b, 0xef, 0x68, 0x64, 0x99, 0x0d, 0xb6, 0xce }
-#define NIST_AES_192_CTR_CIPHER { 0x1a, 0xbc, 0x93, 0x24, 0x17, 0x52, 0x1c, 0xa2, 0x4f, 0x2b, 0x04, 0x59, 0xfe, 0x7e, 0x6e, 0x0b } 
-#define NIST_AES_256_CTR_CIPHER { 0x60, 0x1e, 0xc3, 0x13, 0x77, 0x57, 0x89, 0xa5, 0xb7, 0xa7, 0xf5, 0x04, 0xbb, 0xf3, 0xd2, 0x28 } 
-
+#define NIST_AES_192_CTR_CIPHER { 0x1a, 0xbc, 0x93, 0x24, 0x17, 0x52, 0x1c, 0xa2, 0x4f, 0x2b, 0x04, 0x59, 0xfe, 0x7e, 0x6e, 0x0b }
+#define NIST_AES_256_CTR_CIPHER { 0x60, 0x1e, 0xc3, 0x13, 0x77, 0x57, 0x89, 0xa5, 0xb7, 0xa7, 0xf5, 0x04, 0xbb, 0xf3, 0xd2, 0x28 }
 
 #define RFC3962_AES_128_KEY            { 0x63, 0x68, 0x69, 0x63, 0x6b, 0x65, 0x6e, 0x20, 0x74, 0x65, 0x72, 0x69, 0x79, 0x61, 0x6b, 0x69 }
 #define RFC3962_AES_VECTOR_SIZE        17
@@ -106,13 +104,12 @@ http://csrc.nist.gov/groups/STM/cavp/documents/mac/hmactestvectors.zip
 #define RFC3962_AES_CBC_CTS_IV         { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
 #define RFC3962_AES_128_CBC_CTS_CIPHER { 0xc6, 0x35, 0x35, 0x68, 0xf2, 0xbf, 0x8c, 0xb4, 0xd8, 0xa5, 0x80, 0x36, 0x2d, 0xa7, 0xff, 0x7f, 0x97 }
 
-
 #define NIST_AES_256_XTS_KEY            { 0xa1, 0xb9, 0x0c, 0xba, 0x3f, 0x06, 0xac, 0x35, 0x3b, 0x2c, 0x34, 0x38, 0x76, 0x08, 0x17, 0x62, \
 					  0x09, 0x09, 0x23, 0x02, 0x6e, 0x91, 0x77, 0x18, 0x15, 0xf2, 0x9d, 0xab, 0x01, 0x93, 0x2f, 0x2f }
 #define NIST_AES_256_XTS_IV             { 0x4f, 0xae, 0xf7, 0x11, 0x7c, 0xda, 0x59, 0xc6, 0x6e, 0x4b, 0x92, 0x01, 0x3e, 0x76, 0x8a, 0xd5 }
 #define NIST_AES_256_XTS_VECTOR_SIZE    16
-#define NIST_AES_256_XTS_PLAIN          { 0xeb, 0xab, 0xce, 0x95, 0xb1, 0x4d, 0x3c, 0x8d, 0x6f, 0xb3, 0x50, 0x39, 0x07, 0x90, 0x31, 0x1c } 
-#define NIST_AES_256_XTS_CIPHER         { 0x77, 0x8a, 0xe8, 0xb4, 0x3c, 0xb9, 0x8d, 0x5a, 0x82, 0x50, 0x81, 0xd5, 0xbe, 0x47, 0x1c, 0x63 } 
+#define NIST_AES_256_XTS_PLAIN          { 0xeb, 0xab, 0xce, 0x95, 0xb1, 0x4d, 0x3c, 0x8d, 0x6f, 0xb3, 0x50, 0x39, 0x07, 0x90, 0x31, 0x1c }
+#define NIST_AES_256_XTS_CIPHER         { 0x77, 0x8a, 0xe8, 0xb4, 0x3c, 0xb9, 0x8d, 0x5a, 0x82, 0x50, 0x81, 0xd5, 0xbe, 0x47, 0x1c, 0x63 }
 
 #define NIST_AES_512_XTS_KEY            { 0x1e, 0xa6, 0x61, 0xc5, 0x8d, 0x94, 0x3a, 0x0e, 0x48, 0x01, 0xe4, 0x2f, 0x4b, 0x09, 0x47, 0x14, \
 					  0x9e, 0x7f, 0x9f, 0x8e, 0x3e, 0x68, 0xd0, 0xc7, 0x50, 0x52, 0x10, 0xbd, 0x31, 0x1a, 0x0e, 0x7c, \
@@ -121,10 +118,9 @@ http://csrc.nist.gov/groups/STM/cavp/documents/mac/hmactestvectors.zip
 #define NIST_AES_512_XTS_IV             { 0xad, 0xf8, 0xd9, 0x26, 0x27, 0x46, 0x4a, 0xd2, 0xf0, 0x42, 0x8e, 0x84, 0xa9, 0xf8, 0x75, 0x64,  }
 #define NIST_AES_512_XTS_VECTOR_SIZE    32
 #define NIST_AES_512_XTS_PLAIN          { 0x2e, 0xed, 0xea, 0x52, 0xcd, 0x82, 0x15, 0xe1, 0xac, 0xc6, 0x47, 0xe8, 0x10, 0xbb, 0xc3, 0x64, \
-					  0x2e, 0x87, 0x28, 0x7f, 0x8d, 0x2e, 0x57, 0xe3, 0x6c, 0x0a, 0x24, 0xfb, 0xc1, 0x2a, 0x20, 0x2e } 
+					  0x2e, 0x87, 0x28, 0x7f, 0x8d, 0x2e, 0x57, 0xe3, 0x6c, 0x0a, 0x24, 0xfb, 0xc1, 0x2a, 0x20, 0x2e }
 #define NIST_AES_512_XTS_CIPHER         { 0xcb, 0xaa, 0xd0, 0xe2, 0xf6, 0xce, 0xa3, 0xf5, 0x0b, 0x37, 0xf9, 0x34, 0xd4, 0x6a, 0x9b, 0x13, \
-					  0x0b, 0x9d, 0x54, 0xf0, 0x7e, 0x34, 0xf3, 0x6a, 0xf7, 0x93, 0xe8, 0x6f, 0x73, 0xc6, 0xd7, 0xdb } 
-
+					  0x0b, 0x9d, 0x54, 0xf0, 0x7e, 0x34, 0xf3, 0x6a, 0xf7, 0x93, 0xe8, 0x6f, 0x73, 0xc6, 0xd7, 0xdb }
 
 /* NIST AES-CMAC */
 #define NIST_AES_128_CMAC_KEY           { 0x67, 0x08, 0xc9, 0x88, 0x7b, 0x84, 0x70, 0x84, 0xf1, 0x23, 0xd3, 0xdd, 0x9c, 0x3a, 0x81, 0x36 }
@@ -148,27 +144,25 @@ http://csrc.nist.gov/groups/STM/cavp/documents/mac/hmactestvectors.zip
 #define NIST_AES_256_CMAC_VECTOR_SIZE   16
 #define NIST_AES_256_CMAC_OUTPUT_SIZE   10
 
-
 /* NIST TDES */
 #define TDES_NUM_OF_KEYS                3
 #define NIST_TDES_VECTOR_SIZE           8
 #define NIST_TDES_IV_SIZE               8
 
-#define NIST_TDES_ECB_IV             	{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
+#define NIST_TDES_ECB_IV		{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
 
 #define NIST_TDES_ECB3_KEY		{ 0x01, 0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef, \
 					  0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef, 0x01, \
 					  0x45, 0x67, 0x89, 0xab, 0xcd, 0xef, 0x01, 0x23 }
-#define NIST_TDES_ECB3_PLAIN_DATA    	{ 0x54, 0x68, 0x65, 0x20, 0x71, 0x75, 0x66, 0x63 }
-#define NIST_TDES_ECB3_CIPHER        	{ 0xa8, 0x26, 0xfd, 0x8c, 0xe5, 0x3b, 0x85, 0x5f }
+#define NIST_TDES_ECB3_PLAIN_DATA	{ 0x54, 0x68, 0x65, 0x20, 0x71, 0x75, 0x66, 0x63 }
+#define NIST_TDES_ECB3_CIPHER		{ 0xa8, 0x26, 0xfd, 0x8c, 0xe5, 0x3b, 0x85, 0x5f }
 
-#define NIST_TDES_CBC3_IV            	{ 0xf8, 0xee, 0xe1, 0x35, 0x9c, 0x6e, 0x54, 0x40 }
+#define NIST_TDES_CBC3_IV		{ 0xf8, 0xee, 0xe1, 0x35, 0x9c, 0x6e, 0x54, 0x40 }
 #define NIST_TDES_CBC3_KEY		{ 0xe9, 0xda, 0x37, 0xf8, 0xdc, 0x97, 0x6d, 0x5b, \
 					  0xb6, 0x8c, 0x04, 0xe3, 0xec, 0x98, 0x20, 0x15, \
 					  0xf4, 0x0e, 0x08, 0xb5, 0x97, 0x29, 0xf2, 0x8f }
-#define NIST_TDES_CBC3_PLAIN_DATA    	{ 0x3b, 0xb7, 0xa7, 0xdb, 0xa3, 0xd5, 0x92, 0x91 }
-#define NIST_TDES_CBC3_CIPHER        	{ 0x5b, 0x84, 0x24, 0xd2, 0x39, 0x3e, 0x55, 0xa2 }
-
+#define NIST_TDES_CBC3_PLAIN_DATA	{ 0x3b, 0xb7, 0xa7, 0xdb, 0xa3, 0xd5, 0x92, 0x91 }
+#define NIST_TDES_CBC3_CIPHER		{ 0x5b, 0x84, 0x24, 0xd2, 0x39, 0x3e, 0x55, 0xa2 }
 
 /* NIST AES-CCM */
 #define NIST_AESCCM_128_BIT_KEY_SIZE    16
@@ -208,7 +202,6 @@ http://csrc.nist.gov/groups/STM/cavp/documents/mac/hmactestvectors.zip
 #define NIST_AESCCM_256_CIPHER          { 0xcc, 0x17, 0xbf, 0x87, 0x94, 0xc8, 0x43, 0x45, 0x7d, 0x89, 0x93, 0x91, 0x89, 0x8e, 0xd2, 0x2a }
 #define NIST_AESCCM_256_MAC             { 0x6f, 0x9d, 0x28, 0xfc, 0xb6, 0x42, 0x34, 0xe1, 0xcd, 0x79, 0x3c, 0x41, 0x44, 0xf1, 0xda, 0x50 }
 
-
 /* NIST AES-GCM */
 #define NIST_AESGCM_128_BIT_KEY_SIZE    16
 #define NIST_AESGCM_192_BIT_KEY_SIZE    24
@@ -242,7 +235,6 @@ http://csrc.nist.gov/groups/STM/cavp/documents/mac/hmactestvectors.zip
 #define NIST_AESGCM_256_CIPHER          { 0x42, 0x6e, 0x0e, 0xfc, 0x69, 0x3b, 0x7b, 0xe1, 0xf3, 0x01, 0x8d, 0xb7, 0xdd, 0xbb, 0x7e, 0x4d }
 #define NIST_AESGCM_256_MAC             { 0xee, 0x82, 0x57, 0x79, 0x5b, 0xe6, 0xa1, 0x16, 0x4d, 0x7e, 0x1d, 0x2d, 0x6c, 0xac, 0x77, 0xa7 }
 
-
 /* NIST HASH */
 #define NIST_SHA_MSG_SIZE               16
 
@@ -260,7 +252,6 @@ http://csrc.nist.gov/groups/STM/cavp/documents/mac/hmactestvectors.zip
 					  0x8f, 0x2b, 0xa9, 0x1c, 0x3a, 0x9f, 0x0c, 0x16, 0x53, 0xc4, 0xbf, 0x0a, 0xda, 0x35, 0x64, 0x55, \
 					  0xea, 0x36, 0xfd, 0x31, 0xf8, 0xe7, 0x3e, 0x39, 0x51, 0xca, 0xd4, 0xeb, 0xba, 0x8c, 0x6e, 0x04 }
 
-
 /* NIST HMAC */
 #define NIST_HMAC_MSG_SIZE              128
 
diff --git a/drivers/staging/ccree/ssi_fips_ext.c b/drivers/staging/ccree/ssi_fips_ext.c
index 2ac432fe1233..e7bf1843f60c 100644
--- a/drivers/staging/ccree/ssi_fips_ext.c
+++ b/drivers/staging/ccree/ssi_fips_ext.c
@@ -1,49 +1,47 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
 /**************************************************************
-This file defines the driver FIPS functions that should be 
-implemented by the driver user. Current implementation is sample code only.
-***************************************************************/
+ * This file defines the driver FIPS functions that should be
+ * implemented by the driver user. Current implementation is sample code only.
+ ***************************************************************/
 
 #include <linux/module.h>
 #include "ssi_fips_local.h"
 #include "ssi_driver.h"
 
-
 static bool tee_error;
 module_param(tee_error, bool, 0644);
 MODULE_PARM_DESC(tee_error, "Simulate TEE library failure flag: 0 - no error (default), 1 - TEE error occured ");
 
-static ssi_fips_state_t fips_state = CC_FIPS_STATE_NOT_SUPPORTED;
-static ssi_fips_error_t fips_error = CC_REE_FIPS_ERROR_OK;
+static enum cc_fips_state_t fips_state = CC_FIPS_STATE_NOT_SUPPORTED;
+static enum cc_fips_error fips_error = CC_REE_FIPS_ERROR_OK;
 
 /*
-This function returns the FIPS REE state. 
-The function should be implemented by the driver user, depends on where                          .
-the state value is stored. 
-The reference code uses global variable. 
-*/
-int ssi_fips_ext_get_state(ssi_fips_state_t *p_state)
+ * This function returns the FIPS REE state.
+ * The function should be implemented by the driver user, depends on where
+ * the state value is stored.
+ * The reference code uses global variable.
+ */
+int ssi_fips_ext_get_state(enum cc_fips_state_t *p_state)
 {
-        int rc = 0;
+	int rc = 0;
 
-	if (p_state == NULL) {
+	if (!p_state)
 		return -EINVAL;
-	}
 
 	*p_state = fips_state;
 
@@ -51,18 +49,17 @@ int ssi_fips_ext_get_state(ssi_fips_state_t *p_state)
 }
 
 /*
-This function returns the FIPS REE error. 
-The function should be implemented by the driver user, depends on where                          .
-the error value is stored. 
-The reference code uses global variable. 
-*/
-int ssi_fips_ext_get_error(ssi_fips_error_t *p_err)
+ * This function returns the FIPS REE error.
+ * The function should be implemented by the driver user, depends on where
+ * the error value is stored.
+ * The reference code uses global variable.
+ */
+int ssi_fips_ext_get_error(enum cc_fips_error *p_err)
 {
-        int rc = 0;
+	int rc = 0;
 
-	if (p_err == NULL) {
+	if (!p_err)
 		return -EINVAL;
-	}
 
 	*p_err = fips_error;
 
@@ -70,27 +67,26 @@ int ssi_fips_ext_get_error(ssi_fips_error_t *p_err)
 }
 
 /*
-This function sets the FIPS REE state. 
-The function should be implemented by the driver user, depends on where                          .
-the state value is stored. 
-The reference code uses global variable. 
-*/
-int ssi_fips_ext_set_state(ssi_fips_state_t state)
+ * This function sets the FIPS REE state.
+ * The function should be implemented by the driver user, depends on where
+ * the state value is stored.
+ * The reference code uses global variable.
+ */
+int ssi_fips_ext_set_state(enum cc_fips_state_t state)
 {
 	fips_state = state;
 	return 0;
 }
 
 /*
-This function sets the FIPS REE error. 
-The function should be implemented by the driver user, depends on where                          .
-the error value is stored. 
-The reference code uses global variable. 
-*/
-int ssi_fips_ext_set_error(ssi_fips_error_t err)
+ * This function sets the FIPS REE error.
+ * The function should be implemented by the driver user, depends on where
+ * the error value is stored.
+ * The reference code uses global variable.
+ */
+int ssi_fips_ext_set_error(enum cc_fips_error err)
 {
 	fips_error = err;
 	return 0;
 }
 
-
diff --git a/drivers/staging/ccree/ssi_fips_ll.c b/drivers/staging/ccree/ssi_fips_ll.c
index d573574bbb98..3557e20c9e36 100644
--- a/drivers/staging/ccree/ssi_fips_ll.c
+++ b/drivers/staging/ccree/ssi_fips_ll.c
@@ -1,23 +1,23 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
 /**************************************************************
-This file defines the driver FIPS Low Level implmentaion functions,
-that executes the KAT.
-***************************************************************/
+ * This file defines the driver FIPS Low Level implmentaion functions,
+ * that executes the KAT.
+ ***************************************************************/
 #include <linux/kernel.h>
 
 #include "ssi_driver.h"
@@ -27,151 +27,143 @@ that executes the KAT.
 #include "ssi_hash.h"
 #include "ssi_request_mgr.h"
 
-
-static const uint32_t digest_len_init[] = {
+static const u32 digest_len_init[] = {
 	0x00000040, 0x00000000, 0x00000000, 0x00000000 };
-static const uint32_t sha1_init[] = { 
+static const u32 sha1_init[] = {
 	SHA1_H4, SHA1_H3, SHA1_H2, SHA1_H1, SHA1_H0 };
-static const uint32_t sha256_init[] = {
+static const u32 sha256_init[] = {
 	SHA256_H7, SHA256_H6, SHA256_H5, SHA256_H4,
 	SHA256_H3, SHA256_H2, SHA256_H1, SHA256_H0 };
 #if (CC_SUPPORT_SHA > 256)
-static const uint32_t digest_len_sha512_init[] = { 
+static const u32 digest_len_sha512_init[] = {
 	0x00000080, 0x00000000, 0x00000000, 0x00000000 };
-static const uint64_t sha512_init[] = {
+static const u64 sha512_init[] = {
 	SHA512_H7, SHA512_H6, SHA512_H5, SHA512_H4,
 	SHA512_H3, SHA512_H2, SHA512_H1, SHA512_H0 };
 #endif
 
-
 #define NIST_CIPHER_AES_MAX_VECTOR_SIZE      32
 
 struct fips_cipher_ctx {
-	uint8_t iv[CC_AES_IV_SIZE];
-	uint8_t key[AES_512_BIT_KEY_SIZE];
-	uint8_t din[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
-	uint8_t dout[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
+	u8 iv[CC_AES_IV_SIZE];
+	u8 key[AES_512_BIT_KEY_SIZE];
+	u8 din[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
+	u8 dout[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
 };
 
 typedef struct _FipsCipherData {
-	uint8_t                   isAes;
-	uint8_t                   key[AES_512_BIT_KEY_SIZE];
+	u8                   isAes;
+	u8                   key[AES_512_BIT_KEY_SIZE];
 	size_t                    keySize;
-	uint8_t                   iv[CC_AES_IV_SIZE];
+	u8                   iv[CC_AES_IV_SIZE];
 	enum drv_crypto_direction direction;
 	enum drv_cipher_mode      oprMode;
-	uint8_t                   dataIn[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
-	uint8_t                   dataOut[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
+	u8                   dataIn[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
+	u8                   dataOut[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
 	size_t                    dataInSize;
 } FipsCipherData;
 
-
 struct fips_cmac_ctx {
-	uint8_t key[AES_256_BIT_KEY_SIZE];
-	uint8_t din[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
-	uint8_t mac_res[CC_DIGEST_SIZE_MAX];
+	u8 key[AES_256_BIT_KEY_SIZE];
+	u8 din[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
+	u8 mac_res[CC_DIGEST_SIZE_MAX];
 };
 
 typedef struct _FipsCmacData {
 	enum drv_crypto_direction direction;
-	uint8_t                   key[AES_256_BIT_KEY_SIZE];
+	u8                   key[AES_256_BIT_KEY_SIZE];
 	size_t                    key_size;
-	uint8_t                   data_in[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
+	u8                   data_in[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
 	size_t                    data_in_size;
-	uint8_t                   mac_res[CC_DIGEST_SIZE_MAX];
+	u8                   mac_res[CC_DIGEST_SIZE_MAX];
 	size_t                    mac_res_size;
 } FipsCmacData;
 
-
 struct fips_hash_ctx {
-	uint8_t initial_digest[CC_DIGEST_SIZE_MAX];
-	uint8_t din[NIST_SHA_MSG_SIZE];
-	uint8_t mac_res[CC_DIGEST_SIZE_MAX];
+	u8 initial_digest[CC_DIGEST_SIZE_MAX];
+	u8 din[NIST_SHA_MSG_SIZE];
+	u8 mac_res[CC_DIGEST_SIZE_MAX];
 };
 
 typedef struct _FipsHashData {
 	enum drv_hash_mode    hash_mode;
-	uint8_t               data_in[NIST_SHA_MSG_SIZE];
+	u8               data_in[NIST_SHA_MSG_SIZE];
 	size_t                data_in_size;
-	uint8_t               mac_res[CC_DIGEST_SIZE_MAX];
+	u8               mac_res[CC_DIGEST_SIZE_MAX];
 } FipsHashData;
 
-
 /* note that the hmac key length must be equal or less than block size (block size is 64 up to sha256 and 128 for sha384/512) */
 struct fips_hmac_ctx {
-	uint8_t initial_digest[CC_DIGEST_SIZE_MAX];
-	uint8_t key[CC_HMAC_BLOCK_SIZE_MAX];
-	uint8_t k0[CC_HMAC_BLOCK_SIZE_MAX];
-	uint8_t digest_bytes_len[HASH_LEN_SIZE];
-	uint8_t tmp_digest[CC_DIGEST_SIZE_MAX];
-	uint8_t din[NIST_HMAC_MSG_SIZE];
-	uint8_t mac_res[CC_DIGEST_SIZE_MAX];
+	u8 initial_digest[CC_DIGEST_SIZE_MAX];
+	u8 key[CC_HMAC_BLOCK_SIZE_MAX];
+	u8 k0[CC_HMAC_BLOCK_SIZE_MAX];
+	u8 digest_bytes_len[HASH_LEN_SIZE];
+	u8 tmp_digest[CC_DIGEST_SIZE_MAX];
+	u8 din[NIST_HMAC_MSG_SIZE];
+	u8 mac_res[CC_DIGEST_SIZE_MAX];
 };
 
 typedef struct _FipsHmacData {
 	enum drv_hash_mode    hash_mode;
-	uint8_t               key[CC_HMAC_BLOCK_SIZE_MAX];
+	u8               key[CC_HMAC_BLOCK_SIZE_MAX];
 	size_t                key_size;
-	uint8_t               data_in[NIST_HMAC_MSG_SIZE];
+	u8               data_in[NIST_HMAC_MSG_SIZE];
 	size_t                data_in_size;
-	uint8_t               mac_res[CC_DIGEST_SIZE_MAX];
+	u8               mac_res[CC_DIGEST_SIZE_MAX];
 } FipsHmacData;
 
-
 #define FIPS_CCM_B0_A0_ADATA_SIZE   (NIST_AESCCM_IV_SIZE + NIST_AESCCM_IV_SIZE + NIST_AESCCM_ADATA_SIZE)
 
 struct fips_ccm_ctx {
-	uint8_t b0_a0_adata[FIPS_CCM_B0_A0_ADATA_SIZE];
-	uint8_t iv[NIST_AESCCM_IV_SIZE];
-	uint8_t ctr_cnt_0[NIST_AESCCM_IV_SIZE];
-	uint8_t key[CC_AES_KEY_SIZE_MAX];
-	uint8_t din[NIST_AESCCM_TEXT_SIZE];
-	uint8_t dout[NIST_AESCCM_TEXT_SIZE];
-	uint8_t mac_res[NIST_AESCCM_TAG_SIZE];
+	u8 b0_a0_adata[FIPS_CCM_B0_A0_ADATA_SIZE];
+	u8 iv[NIST_AESCCM_IV_SIZE];
+	u8 ctr_cnt_0[NIST_AESCCM_IV_SIZE];
+	u8 key[CC_AES_KEY_SIZE_MAX];
+	u8 din[NIST_AESCCM_TEXT_SIZE];
+	u8 dout[NIST_AESCCM_TEXT_SIZE];
+	u8 mac_res[NIST_AESCCM_TAG_SIZE];
 };
 
 typedef struct _FipsCcmData {
 	enum drv_crypto_direction direction;
-	uint8_t                   key[CC_AES_KEY_SIZE_MAX];
+	u8                   key[CC_AES_KEY_SIZE_MAX];
 	size_t                    keySize;
-	uint8_t                   nonce[NIST_AESCCM_NONCE_SIZE];
-	uint8_t                   adata[NIST_AESCCM_ADATA_SIZE];
+	u8                   nonce[NIST_AESCCM_NONCE_SIZE];
+	u8                   adata[NIST_AESCCM_ADATA_SIZE];
 	size_t                    adataSize;
-	uint8_t                   dataIn[NIST_AESCCM_TEXT_SIZE];
+	u8                   dataIn[NIST_AESCCM_TEXT_SIZE];
 	size_t                    dataInSize;
-	uint8_t                   dataOut[NIST_AESCCM_TEXT_SIZE];
-	uint8_t                   tagSize;
-	uint8_t                   macResOut[NIST_AESCCM_TAG_SIZE];
+	u8                   dataOut[NIST_AESCCM_TEXT_SIZE];
+	u8                   tagSize;
+	u8                   macResOut[NIST_AESCCM_TAG_SIZE];
 } FipsCcmData;
 
-
 struct fips_gcm_ctx {
-	uint8_t adata[NIST_AESGCM_ADATA_SIZE];
-	uint8_t key[CC_AES_KEY_SIZE_MAX];
-	uint8_t hkey[CC_AES_KEY_SIZE_MAX];
-	uint8_t din[NIST_AESGCM_TEXT_SIZE];
-	uint8_t dout[NIST_AESGCM_TEXT_SIZE];
-	uint8_t mac_res[NIST_AESGCM_TAG_SIZE];
-	uint8_t len_block[AES_BLOCK_SIZE];
-	uint8_t iv_inc1[AES_BLOCK_SIZE];
-	uint8_t iv_inc2[AES_BLOCK_SIZE];
+	u8 adata[NIST_AESGCM_ADATA_SIZE];
+	u8 key[CC_AES_KEY_SIZE_MAX];
+	u8 hkey[CC_AES_KEY_SIZE_MAX];
+	u8 din[NIST_AESGCM_TEXT_SIZE];
+	u8 dout[NIST_AESGCM_TEXT_SIZE];
+	u8 mac_res[NIST_AESGCM_TAG_SIZE];
+	u8 len_block[AES_BLOCK_SIZE];
+	u8 iv_inc1[AES_BLOCK_SIZE];
+	u8 iv_inc2[AES_BLOCK_SIZE];
 };
 
 typedef struct _FipsGcmData {
 	enum drv_crypto_direction direction;
-	uint8_t                   key[CC_AES_KEY_SIZE_MAX];
+	u8                   key[CC_AES_KEY_SIZE_MAX];
 	size_t                    keySize;
-	uint8_t                   iv[NIST_AESGCM_IV_SIZE];
-	uint8_t                   adata[NIST_AESGCM_ADATA_SIZE];
+	u8                   iv[NIST_AESGCM_IV_SIZE];
+	u8                   adata[NIST_AESGCM_ADATA_SIZE];
 	size_t                    adataSize;
-	uint8_t                   dataIn[NIST_AESGCM_TEXT_SIZE];
+	u8                   dataIn[NIST_AESGCM_TEXT_SIZE];
 	size_t                    dataInSize;
-	uint8_t                   dataOut[NIST_AESGCM_TEXT_SIZE];
-	uint8_t                   tagSize;
-	uint8_t                   macResOut[NIST_AESGCM_TAG_SIZE];
+	u8                   dataOut[NIST_AESGCM_TEXT_SIZE];
+	u8                   tagSize;
+	u8                   macResOut[NIST_AESGCM_TAG_SIZE];
 } FipsGcmData;
 
-
 typedef union _fips_ctx {
 	struct fips_cipher_ctx cipher;
 	struct fips_cmac_ctx cmac;
@@ -181,7 +173,6 @@ typedef union _fips_ctx {
 	struct fips_gcm_ctx gcm;
 } fips_ctx;
 
-
 /* test data tables */
 static const FipsCipherData FipsCipherDataTable[] = {
 	/* AES */
@@ -214,8 +205,8 @@ static const FipsCipherData FipsCipherDataTable[] = {
 	{ 1, NIST_AES_256_XTS_KEY, CC_AES_256_BIT_KEY_SIZE,   NIST_AES_256_XTS_IV,  DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_XTS,     NIST_AES_256_XTS_PLAIN, NIST_AES_256_XTS_CIPHER, NIST_AES_256_XTS_VECTOR_SIZE },
 	{ 1, NIST_AES_256_XTS_KEY, CC_AES_256_BIT_KEY_SIZE,   NIST_AES_256_XTS_IV,  DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_XTS,     NIST_AES_256_XTS_CIPHER, NIST_AES_256_XTS_PLAIN, NIST_AES_256_XTS_VECTOR_SIZE },
 #if (CC_SUPPORT_SHA > 256)
-	{ 1, NIST_AES_512_XTS_KEY, 2*CC_AES_256_BIT_KEY_SIZE, NIST_AES_512_XTS_IV,  DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_XTS,     NIST_AES_512_XTS_PLAIN, NIST_AES_512_XTS_CIPHER, NIST_AES_512_XTS_VECTOR_SIZE },
-	{ 1, NIST_AES_512_XTS_KEY, 2*CC_AES_256_BIT_KEY_SIZE, NIST_AES_512_XTS_IV,  DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_XTS,     NIST_AES_512_XTS_CIPHER, NIST_AES_512_XTS_PLAIN, NIST_AES_512_XTS_VECTOR_SIZE },
+	{ 1, NIST_AES_512_XTS_KEY, 2 * CC_AES_256_BIT_KEY_SIZE, NIST_AES_512_XTS_IV,  DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_XTS,     NIST_AES_512_XTS_PLAIN, NIST_AES_512_XTS_CIPHER, NIST_AES_512_XTS_VECTOR_SIZE },
+	{ 1, NIST_AES_512_XTS_KEY, 2 * CC_AES_256_BIT_KEY_SIZE, NIST_AES_512_XTS_IV,  DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_XTS,     NIST_AES_512_XTS_CIPHER, NIST_AES_512_XTS_PLAIN, NIST_AES_512_XTS_VECTOR_SIZE },
 #endif
 	/* DES */
 	{ 0, NIST_TDES_ECB3_KEY, CC_DRV_DES_TRIPLE_KEY_SIZE, NIST_TDES_ECB_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_ECB, NIST_TDES_ECB3_PLAIN_DATA, NIST_TDES_ECB3_CIPHER, NIST_TDES_VECTOR_SIZE },
@@ -223,6 +214,7 @@ static const FipsCipherData FipsCipherDataTable[] = {
 	{ 0, NIST_TDES_CBC3_KEY, CC_DRV_DES_TRIPLE_KEY_SIZE, NIST_TDES_CBC3_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_CBC, NIST_TDES_CBC3_PLAIN_DATA, NIST_TDES_CBC3_CIPHER, NIST_TDES_VECTOR_SIZE },
 	{ 0, NIST_TDES_CBC3_KEY, CC_DRV_DES_TRIPLE_KEY_SIZE, NIST_TDES_CBC3_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_CBC, NIST_TDES_CBC3_CIPHER, NIST_TDES_CBC3_PLAIN_DATA, NIST_TDES_VECTOR_SIZE },
 };
+
 #define FIPS_CIPHER_NUM_OF_TESTS        (sizeof(FipsCipherDataTable) / sizeof(FipsCipherData))
 
 static const FipsCmacData FipsCmacDataTable[] = {
@@ -230,56 +222,60 @@ static const FipsCmacData FipsCmacDataTable[] = {
 	{ DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AES_192_CMAC_KEY, AES_192_BIT_KEY_SIZE, NIST_AES_192_CMAC_PLAIN_DATA, NIST_AES_192_CMAC_VECTOR_SIZE, NIST_AES_192_CMAC_MAC, NIST_AES_192_CMAC_OUTPUT_SIZE },
 	{ DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AES_256_CMAC_KEY, AES_256_BIT_KEY_SIZE, NIST_AES_256_CMAC_PLAIN_DATA, NIST_AES_256_CMAC_VECTOR_SIZE, NIST_AES_256_CMAC_MAC, NIST_AES_256_CMAC_OUTPUT_SIZE },
 };
+
 #define FIPS_CMAC_NUM_OF_TESTS        (sizeof(FipsCmacDataTable) / sizeof(FipsCmacData))
 
 static const FipsHashData FipsHashDataTable[] = {
-        { DRV_HASH_SHA1,   NIST_SHA_1_MSG,   NIST_SHA_MSG_SIZE, NIST_SHA_1_MD },
-        { DRV_HASH_SHA256, NIST_SHA_256_MSG, NIST_SHA_MSG_SIZE, NIST_SHA_256_MD },
+	{ DRV_HASH_SHA1,   NIST_SHA_1_MSG,   NIST_SHA_MSG_SIZE, NIST_SHA_1_MD },
+	{ DRV_HASH_SHA256, NIST_SHA_256_MSG, NIST_SHA_MSG_SIZE, NIST_SHA_256_MD },
 #if (CC_SUPPORT_SHA > 256)
 //        { DRV_HASH_SHA512, NIST_SHA_512_MSG, NIST_SHA_MSG_SIZE, NIST_SHA_512_MD },
 #endif
 };
+
 #define FIPS_HASH_NUM_OF_TESTS        (sizeof(FipsHashDataTable) / sizeof(FipsHashData))
 
 static const FipsHmacData FipsHmacDataTable[] = {
-        { DRV_HASH_SHA1,   NIST_HMAC_SHA1_KEY,   NIST_HMAC_SHA1_KEY_SIZE,   NIST_HMAC_SHA1_MSG,   NIST_HMAC_MSG_SIZE, NIST_HMAC_SHA1_MD },
-        { DRV_HASH_SHA256, NIST_HMAC_SHA256_KEY, NIST_HMAC_SHA256_KEY_SIZE, NIST_HMAC_SHA256_MSG, NIST_HMAC_MSG_SIZE, NIST_HMAC_SHA256_MD },
+	{ DRV_HASH_SHA1,   NIST_HMAC_SHA1_KEY,   NIST_HMAC_SHA1_KEY_SIZE,   NIST_HMAC_SHA1_MSG,   NIST_HMAC_MSG_SIZE, NIST_HMAC_SHA1_MD },
+	{ DRV_HASH_SHA256, NIST_HMAC_SHA256_KEY, NIST_HMAC_SHA256_KEY_SIZE, NIST_HMAC_SHA256_MSG, NIST_HMAC_MSG_SIZE, NIST_HMAC_SHA256_MD },
 #if (CC_SUPPORT_SHA > 256)
 //        { DRV_HASH_SHA512, NIST_HMAC_SHA512_KEY, NIST_HMAC_SHA512_KEY_SIZE, NIST_HMAC_SHA512_MSG, NIST_HMAC_MSG_SIZE, NIST_HMAC_SHA512_MD },
 #endif
 };
+
 #define FIPS_HMAC_NUM_OF_TESTS        (sizeof(FipsHmacDataTable) / sizeof(FipsHmacData))
 
 static const FipsCcmData FipsCcmDataTable[] = {
-        { DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESCCM_128_KEY, NIST_AESCCM_128_BIT_KEY_SIZE, NIST_AESCCM_128_NONCE, NIST_AESCCM_128_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_128_PLAIN_TEXT, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_128_CIPHER, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_128_MAC },
-        { DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESCCM_128_KEY, NIST_AESCCM_128_BIT_KEY_SIZE, NIST_AESCCM_128_NONCE, NIST_AESCCM_128_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_128_CIPHER, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_128_PLAIN_TEXT, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_128_MAC },
-        { DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESCCM_192_KEY, NIST_AESCCM_192_BIT_KEY_SIZE, NIST_AESCCM_192_NONCE, NIST_AESCCM_192_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_192_PLAIN_TEXT, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_192_CIPHER, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_192_MAC },
-        { DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESCCM_192_KEY, NIST_AESCCM_192_BIT_KEY_SIZE, NIST_AESCCM_192_NONCE, NIST_AESCCM_192_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_192_CIPHER, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_192_PLAIN_TEXT, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_192_MAC },
-        { DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESCCM_256_KEY, NIST_AESCCM_256_BIT_KEY_SIZE, NIST_AESCCM_256_NONCE, NIST_AESCCM_256_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_256_PLAIN_TEXT, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_256_CIPHER, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_256_MAC },
-        { DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESCCM_256_KEY, NIST_AESCCM_256_BIT_KEY_SIZE, NIST_AESCCM_256_NONCE, NIST_AESCCM_256_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_256_CIPHER, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_256_PLAIN_TEXT, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_256_MAC },
+	{ DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESCCM_128_KEY, NIST_AESCCM_128_BIT_KEY_SIZE, NIST_AESCCM_128_NONCE, NIST_AESCCM_128_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_128_PLAIN_TEXT, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_128_CIPHER, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_128_MAC },
+	{ DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESCCM_128_KEY, NIST_AESCCM_128_BIT_KEY_SIZE, NIST_AESCCM_128_NONCE, NIST_AESCCM_128_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_128_CIPHER, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_128_PLAIN_TEXT, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_128_MAC },
+	{ DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESCCM_192_KEY, NIST_AESCCM_192_BIT_KEY_SIZE, NIST_AESCCM_192_NONCE, NIST_AESCCM_192_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_192_PLAIN_TEXT, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_192_CIPHER, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_192_MAC },
+	{ DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESCCM_192_KEY, NIST_AESCCM_192_BIT_KEY_SIZE, NIST_AESCCM_192_NONCE, NIST_AESCCM_192_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_192_CIPHER, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_192_PLAIN_TEXT, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_192_MAC },
+	{ DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESCCM_256_KEY, NIST_AESCCM_256_BIT_KEY_SIZE, NIST_AESCCM_256_NONCE, NIST_AESCCM_256_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_256_PLAIN_TEXT, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_256_CIPHER, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_256_MAC },
+	{ DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESCCM_256_KEY, NIST_AESCCM_256_BIT_KEY_SIZE, NIST_AESCCM_256_NONCE, NIST_AESCCM_256_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_256_CIPHER, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_256_PLAIN_TEXT, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_256_MAC },
 };
+
 #define FIPS_CCM_NUM_OF_TESTS        (sizeof(FipsCcmDataTable) / sizeof(FipsCcmData))
 
 static const FipsGcmData FipsGcmDataTable[] = {
-        { DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESGCM_128_KEY, NIST_AESGCM_128_BIT_KEY_SIZE, NIST_AESGCM_128_IV, NIST_AESGCM_128_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_128_PLAIN_TEXT, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_128_CIPHER, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_128_MAC },
-        { DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESGCM_128_KEY, NIST_AESGCM_128_BIT_KEY_SIZE, NIST_AESGCM_128_IV, NIST_AESGCM_128_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_128_CIPHER, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_128_PLAIN_TEXT, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_128_MAC },
-        { DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESGCM_192_KEY, NIST_AESGCM_192_BIT_KEY_SIZE, NIST_AESGCM_192_IV, NIST_AESGCM_192_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_192_PLAIN_TEXT, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_192_CIPHER, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_192_MAC },
-        { DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESGCM_192_KEY, NIST_AESGCM_192_BIT_KEY_SIZE, NIST_AESGCM_192_IV, NIST_AESGCM_192_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_192_CIPHER, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_192_PLAIN_TEXT, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_192_MAC },
-        { DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESGCM_256_KEY, NIST_AESGCM_256_BIT_KEY_SIZE, NIST_AESGCM_256_IV, NIST_AESGCM_256_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_256_PLAIN_TEXT, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_256_CIPHER, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_256_MAC },
-        { DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESGCM_256_KEY, NIST_AESGCM_256_BIT_KEY_SIZE, NIST_AESGCM_256_IV, NIST_AESGCM_256_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_256_CIPHER, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_256_PLAIN_TEXT, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_256_MAC },
+	{ DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESGCM_128_KEY, NIST_AESGCM_128_BIT_KEY_SIZE, NIST_AESGCM_128_IV, NIST_AESGCM_128_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_128_PLAIN_TEXT, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_128_CIPHER, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_128_MAC },
+	{ DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESGCM_128_KEY, NIST_AESGCM_128_BIT_KEY_SIZE, NIST_AESGCM_128_IV, NIST_AESGCM_128_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_128_CIPHER, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_128_PLAIN_TEXT, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_128_MAC },
+	{ DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESGCM_192_KEY, NIST_AESGCM_192_BIT_KEY_SIZE, NIST_AESGCM_192_IV, NIST_AESGCM_192_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_192_PLAIN_TEXT, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_192_CIPHER, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_192_MAC },
+	{ DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESGCM_192_KEY, NIST_AESGCM_192_BIT_KEY_SIZE, NIST_AESGCM_192_IV, NIST_AESGCM_192_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_192_CIPHER, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_192_PLAIN_TEXT, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_192_MAC },
+	{ DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESGCM_256_KEY, NIST_AESGCM_256_BIT_KEY_SIZE, NIST_AESGCM_256_IV, NIST_AESGCM_256_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_256_PLAIN_TEXT, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_256_CIPHER, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_256_MAC },
+	{ DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESGCM_256_KEY, NIST_AESGCM_256_BIT_KEY_SIZE, NIST_AESGCM_256_IV, NIST_AESGCM_256_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_256_CIPHER, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_256_PLAIN_TEXT, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_256_MAC },
 };
-#define FIPS_GCM_NUM_OF_TESTS        (sizeof(FipsGcmDataTable) / sizeof(FipsGcmData))
 
+#define FIPS_GCM_NUM_OF_TESTS        (sizeof(FipsGcmDataTable) / sizeof(FipsGcmData))
 
-static inline ssi_fips_error_t 
+static inline enum cc_fips_error
 FIPS_CipherToFipsError(enum drv_cipher_mode mode, bool is_aes)
 {
 	switch (mode)
 	{
 	case DRV_CIPHER_ECB:
-		return is_aes ? CC_REE_FIPS_ERROR_AES_ECB_PUT : CC_REE_FIPS_ERROR_DES_ECB_PUT ;
+		return is_aes ? CC_REE_FIPS_ERROR_AES_ECB_PUT : CC_REE_FIPS_ERROR_DES_ECB_PUT;
 	case DRV_CIPHER_CBC:
-		return is_aes ? CC_REE_FIPS_ERROR_AES_CBC_PUT : CC_REE_FIPS_ERROR_DES_CBC_PUT ;
+		return is_aes ? CC_REE_FIPS_ERROR_AES_CBC_PUT : CC_REE_FIPS_ERROR_DES_CBC_PUT;
 	case DRV_CIPHER_OFB:
 		return CC_REE_FIPS_ERROR_AES_OFB_PUT;
 	case DRV_CIPHER_CTR:
@@ -295,8 +291,7 @@ FIPS_CipherToFipsError(enum drv_cipher_mode mode, bool is_aes)
 	return CC_REE_FIPS_ERROR_GENERAL;
 }
 
-
-static inline int 
+static inline int
 ssi_cipher_fips_run_test(struct ssi_drvdata *drvdata,
 			 bool is_aes,
 			 int cipher_mode,
@@ -314,7 +309,7 @@ ssi_cipher_fips_run_test(struct ssi_drvdata *drvdata,
 
 	int rc;
 	struct ssi_crypto_req ssi_req = {0};
-	HwDesc_s desc[FIPS_CIPHER_MAX_SEQ_LEN];
+	struct cc_hw_desc desc[FIPS_CIPHER_MAX_SEQ_LEN];
 	int idx = 0;
 	int s_flow_mode = is_aes ? S_DIN_to_AES : S_DIN_to_DES;
 
@@ -325,74 +320,73 @@ ssi_cipher_fips_run_test(struct ssi_drvdata *drvdata,
 	case DRV_CIPHER_CTR:
 	case DRV_CIPHER_OFB:
 		/* Load cipher state */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-				     iv_dma_addr, iv_len, NS_BIT);
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direction);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], s_flow_mode);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], cipher_mode);
-		if ((cipher_mode == DRV_CIPHER_CTR) || 
-		    (cipher_mode == DRV_CIPHER_OFB) ) {
-			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI,
+			     iv_dma_addr, iv_len, NS_BIT);
+		set_cipher_config0(&desc[idx], direction);
+		set_flow_mode(&desc[idx], s_flow_mode);
+		set_cipher_mode(&desc[idx], cipher_mode);
+		if ((cipher_mode == DRV_CIPHER_CTR) ||
+		    (cipher_mode == DRV_CIPHER_OFB)) {
+			set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
 		} else {
-			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+			set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 		}
 		idx++;
 		/*FALLTHROUGH*/
 	case DRV_CIPHER_ECB:
 		/* Load key */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], cipher_mode);
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direction);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], cipher_mode);
+		set_cipher_config0(&desc[idx], direction);
 		if (is_aes) {
-			HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-					     key_dma_addr, 
-					     ((key_len == 24) ? AES_MAX_KEY_SIZE : key_len),
-					     NS_BIT);
-			HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len);
+			set_din_type(&desc[idx], DMA_DLLI, key_dma_addr,
+				     ((key_len == 24) ? AES_MAX_KEY_SIZE :
+				      key_len), NS_BIT);
+			set_key_size_aes(&desc[idx], key_len);
 		} else {/*des*/
-			HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-					     key_dma_addr, key_len,
-					     NS_BIT);
-			HW_DESC_SET_KEY_SIZE_DES(&desc[idx], key_len);
+			set_din_type(&desc[idx], DMA_DLLI, key_dma_addr,
+				     key_len, NS_BIT);
+			set_key_size_des(&desc[idx], key_len);
 		}
-		HW_DESC_SET_FLOW_MODE(&desc[idx], s_flow_mode);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		set_flow_mode(&desc[idx], s_flow_mode);
+		set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 		idx++;
 		break;
 	case DRV_CIPHER_XTS:
 		/* Load AES key */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], cipher_mode);
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direction);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-				     key_dma_addr, key_len/2, NS_BIT);
-		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len/2);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], s_flow_mode);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], cipher_mode);
+		set_cipher_config0(&desc[idx], direction);
+		set_din_type(&desc[idx], DMA_DLLI, key_dma_addr, (key_len / 2),
+			     NS_BIT);
+		set_key_size_aes(&desc[idx], (key_len / 2));
+		set_flow_mode(&desc[idx], s_flow_mode);
+		set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 		idx++;
 
 		/* load XEX key */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], cipher_mode);
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direction);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-				     (key_dma_addr+key_len/2), key_len/2, NS_BIT);
-		HW_DESC_SET_XEX_DATA_UNIT_SIZE(&desc[idx], data_size);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], s_flow_mode);
-		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len/2);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_XEX_KEY);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], cipher_mode);
+		set_cipher_config0(&desc[idx], direction);
+		set_din_type(&desc[idx], DMA_DLLI,
+			     (key_dma_addr + (key_len / 2)),
+			     (key_len / 2), NS_BIT);
+		set_xex_data_unit_size(&desc[idx], data_size);
+		set_flow_mode(&desc[idx], s_flow_mode);
+		set_key_size_aes(&desc[idx], (key_len / 2));
+		set_setup_mode(&desc[idx], SETUP_LOAD_XEX_KEY);
 		idx++;
 
 		/* Set state */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], cipher_mode);
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direction);
-		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len/2);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], s_flow_mode);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-				     iv_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT);
+		hw_desc_init(&desc[idx]);
+		set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
+		set_cipher_mode(&desc[idx], cipher_mode);
+		set_cipher_config0(&desc[idx], direction);
+		set_key_size_aes(&desc[idx], (key_len / 2));
+		set_flow_mode(&desc[idx], s_flow_mode);
+		set_din_type(&desc[idx], DMA_DLLI, iv_dma_addr,
+			     CC_AES_BLOCK_SIZE, NS_BIT);
 		idx++;
 		break;
 	default:
@@ -401,10 +395,10 @@ ssi_cipher_fips_run_test(struct ssi_drvdata *drvdata,
 	}
 
 	/* create data descriptor */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, din_dma_addr, data_size, NS_BIT);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], dout_dma_addr, data_size, NS_BIT, 0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], is_aes ? DIN_AES_DOUT : DIN_DES_DOUT);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, din_dma_addr, data_size, NS_BIT);
+	set_dout_dlli(&desc[idx], dout_dma_addr, data_size, NS_BIT, 0);
+	set_flow_mode(&desc[idx], is_aes ? DIN_AES_DOUT : DIN_DES_DOUT);
 	idx++;
 
 	/* perform the operation - Lock HW and push sequence */
@@ -415,11 +409,10 @@ ssi_cipher_fips_run_test(struct ssi_drvdata *drvdata,
 	return rc;
 }
 
-
-ssi_fips_error_t
+enum cc_fips_error
 ssi_cipher_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer)
 {
-	ssi_fips_error_t error = CC_REE_FIPS_ERROR_OK;
+	enum cc_fips_error error = CC_REE_FIPS_ERROR_OK;
 	size_t i;
 	struct fips_cipher_ctx *virt_ctx = (struct fips_cipher_ctx *)cpu_addr_buffer;
 
@@ -431,9 +424,9 @@ ssi_cipher_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffe
 
 	for (i = 0; i < FIPS_CIPHER_NUM_OF_TESTS; ++i)
 	{
-		FipsCipherData *cipherData = (FipsCipherData*)&FipsCipherDataTable[i];
+		FipsCipherData *cipherData = (FipsCipherData *)&FipsCipherDataTable[i];
 		int rc = 0;
-		size_t iv_size = cipherData->isAes ? NIST_AES_IV_SIZE : NIST_TDES_IV_SIZE ;
+		size_t iv_size = cipherData->isAes ? NIST_AES_IV_SIZE : NIST_TDES_IV_SIZE;
 
 		memset(cpu_addr_buffer, 0, sizeof(struct fips_cipher_ctx));
 
@@ -480,8 +473,7 @@ ssi_cipher_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffe
 	return error;
 }
 
-
-static inline int 
+static inline int
 ssi_cmac_fips_run_test(struct ssi_drvdata *drvdata,
 		       dma_addr_t key_dma_addr,
 		       size_t key_len,
@@ -495,46 +487,45 @@ ssi_cmac_fips_run_test(struct ssi_drvdata *drvdata,
 
 	int rc;
 	struct ssi_crypto_req ssi_req = {0};
-	HwDesc_s desc[FIPS_CMAC_MAX_SEQ_LEN];
+	struct cc_hw_desc desc[FIPS_CMAC_MAX_SEQ_LEN];
 	int idx = 0;
 
 	/* Setup CMAC Key */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, key_dma_addr,
-			     ((key_len == 24) ? AES_MAX_KEY_SIZE : key_len), NS_BIT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CMAC);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, key_dma_addr,
+		     ((key_len == 24) ? AES_MAX_KEY_SIZE : key_len), NS_BIT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CMAC);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_key_size_aes(&desc[idx], key_len);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* Load MAC state */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, digest_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CMAC);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, digest_dma_addr, CC_AES_BLOCK_SIZE,
+		     NS_BIT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CMAC);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_key_size_aes(&desc[idx], key_len);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
-
 	//ssi_hash_create_data_desc(state, ctx, DIN_AES_DOUT, desc, false, &idx);
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-			     din_dma_addr, 
-			     din_len, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, din_dma_addr, din_len, NS_BIT);
+	set_flow_mode(&desc[idx], DIN_AES_DOUT);
 	idx++;
-	
+
 	/* Get final MAC result */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], digest_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT, 0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_AES_to_DOUT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CMAC); 
+	hw_desc_init(&desc[idx]);
+	set_dout_dlli(&desc[idx], digest_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT,
+		      0);
+	set_flow_mode(&desc[idx], S_AES_to_DOUT);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CMAC);
 	idx++;
 
 	/* perform the operation - Lock HW and push sequence */
@@ -545,10 +536,10 @@ ssi_cmac_fips_run_test(struct ssi_drvdata *drvdata,
 	return rc;
 }
 
-ssi_fips_error_t
+enum cc_fips_error
 ssi_cmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer)
 {
-	ssi_fips_error_t error = CC_REE_FIPS_ERROR_OK;
+	enum cc_fips_error error = CC_REE_FIPS_ERROR_OK;
 	size_t i;
 	struct fips_cmac_ctx *virt_ctx = (struct fips_cmac_ctx *)cpu_addr_buffer;
 
@@ -559,7 +550,7 @@ ssi_cmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 
 	for (i = 0; i < FIPS_CMAC_NUM_OF_TESTS; ++i)
 	{
-		FipsCmacData *cmac_data = (FipsCmacData*)&FipsCmacDataTable[i];
+		FipsCmacData *cmac_data = (FipsCmacData *)&FipsCmacDataTable[i];
 		int rc = 0;
 
 		memset(cpu_addr_buffer, 0, sizeof(struct fips_cmac_ctx));
@@ -604,8 +595,7 @@ ssi_cmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 	return error;
 }
 
-
-static inline ssi_fips_error_t 
+static inline enum cc_fips_error
 FIPS_HashToFipsError(enum drv_hash_mode hash_mode)
 {
 	switch (hash_mode) {
@@ -624,7 +614,7 @@ FIPS_HashToFipsError(enum drv_hash_mode hash_mode)
 	return CC_REE_FIPS_ERROR_GENERAL;
 }
 
-static inline int 
+static inline int
 ssi_hash_fips_run_test(struct ssi_drvdata *drvdata,
 		       dma_addr_t initial_digest_dma_addr,
 		       dma_addr_t din_dma_addr,
@@ -640,45 +630,47 @@ ssi_hash_fips_run_test(struct ssi_drvdata *drvdata,
 
 	int rc;
 	struct ssi_crypto_req ssi_req = {0};
-	HwDesc_s desc[FIPS_HASH_MAX_SEQ_LEN];
+	struct cc_hw_desc desc[FIPS_HASH_MAX_SEQ_LEN];
 	int idx = 0;
 
 	/* Load initial digest */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, initial_digest_dma_addr, inter_digestsize, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], hw_mode);
+	set_din_type(&desc[idx], DMA_DLLI, initial_digest_dma_addr,
+		     inter_digestsize, NS_BIT);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 	idx++;
 
 	/* Load the hash current length */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
-	HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], hw_mode);
+	set_din_const(&desc[idx], 0, HASH_LEN_SIZE);
+	set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 	idx++;
 
 	/* data descriptor */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, din_dma_addr, data_in_size, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, din_dma_addr, data_in_size, NS_BIT);
+	set_flow_mode(&desc[idx], DIN_HASH);
 	idx++;
 
 	/* Get final MAC result */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], mac_res_dma_addr, digest_size, NS_BIT, 0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], hw_mode);
+	set_dout_dlli(&desc[idx], mac_res_dma_addr, digest_size, NS_BIT, 0);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	set_cipher_config1(&desc[idx], HASH_PADDING_DISABLED);
 	if (unlikely((hash_mode == DRV_HASH_MD5) ||
 		     (hash_mode == DRV_HASH_SHA384) ||
 		     (hash_mode == DRV_HASH_SHA512))) {
-		HW_DESC_SET_BYTES_SWAP(&desc[idx], 1);
+		set_bytes_swap(&desc[idx], 1);
 	} else {
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+		set_cipher_config0(&desc[idx],
+				   HASH_DIGEST_RESULT_LITTLE_ENDIAN);
 	}
 	idx++;
 
@@ -689,10 +681,10 @@ ssi_hash_fips_run_test(struct ssi_drvdata *drvdata,
 	return rc;
 }
 
-ssi_fips_error_t
+enum cc_fips_error
 ssi_hash_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer)
 {
-	ssi_fips_error_t error = CC_REE_FIPS_ERROR_OK;
+	enum cc_fips_error error = CC_REE_FIPS_ERROR_OK;
 	size_t i;
 	struct fips_hash_ctx *virt_ctx = (struct fips_hash_ctx *)cpu_addr_buffer;
 
@@ -703,7 +695,7 @@ ssi_hash_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 
 	for (i = 0; i < FIPS_HASH_NUM_OF_TESTS; ++i)
 	{
-		FipsHashData *hash_data = (FipsHashData*)&FipsHashDataTable[i];
+		FipsHashData *hash_data = (FipsHashData *)&FipsHashDataTable[i];
 		int rc = 0;
 		enum drv_hash_hw_mode hw_mode = 0;
 		int digest_size = 0;
@@ -717,20 +709,20 @@ ssi_hash_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 			digest_size = CC_SHA1_DIGEST_SIZE;
 			inter_digestsize = CC_SHA1_DIGEST_SIZE;
 			/* copy the initial digest into the allocated cache coherent buffer */
-			memcpy(virt_ctx->initial_digest, (void*)sha1_init, CC_SHA1_DIGEST_SIZE);
+			memcpy(virt_ctx->initial_digest, (void *)sha1_init, CC_SHA1_DIGEST_SIZE);
 			break;
 		case DRV_HASH_SHA256:
 			hw_mode = DRV_HASH_HW_SHA256;
 			digest_size = CC_SHA256_DIGEST_SIZE;
 			inter_digestsize = CC_SHA256_DIGEST_SIZE;
-			memcpy(virt_ctx->initial_digest, (void*)sha256_init, CC_SHA256_DIGEST_SIZE);
+			memcpy(virt_ctx->initial_digest, (void *)sha256_init, CC_SHA256_DIGEST_SIZE);
 			break;
 #if (CC_SUPPORT_SHA > 256)
 		case DRV_HASH_SHA512:
 			hw_mode = DRV_HASH_HW_SHA512;
 			digest_size = CC_SHA512_DIGEST_SIZE;
 			inter_digestsize = CC_SHA512_DIGEST_SIZE;
-			memcpy(virt_ctx->initial_digest, (void*)sha512_init, CC_SHA512_DIGEST_SIZE);
+			memcpy(virt_ctx->initial_digest, (void *)sha512_init, CC_SHA512_DIGEST_SIZE);
 			break;
 #endif
 		default:
@@ -757,7 +749,7 @@ ssi_hash_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 			FIPS_LOG("ssi_hash_fips_run_test %d returned error - rc = %d \n", i, rc);
 			error = FIPS_HashToFipsError(hash_data->hash_mode);
 			break;
-                }
+		}
 
 		/* compare actual mac result to expected */
 		if (memcmp(virt_ctx->mac_res, hash_data->mac_res, digest_size) != 0)
@@ -772,14 +764,13 @@ ssi_hash_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 
 			error = FIPS_HashToFipsError(hash_data->hash_mode);
 			break;
-                }
+		}
 	}
 
 	return error;
 }
 
-
-static inline ssi_fips_error_t 
+static inline enum cc_fips_error
 FIPS_HmacToFipsError(enum drv_hash_mode hash_mode)
 {
 	switch (hash_mode) {
@@ -798,7 +789,7 @@ FIPS_HmacToFipsError(enum drv_hash_mode hash_mode)
 	return CC_REE_FIPS_ERROR_GENERAL;
 }
 
-static inline int 
+static inline int
 ssi_hmac_fips_run_test(struct ssi_drvdata *drvdata,
 		       dma_addr_t initial_digest_dma_addr,
 		       dma_addr_t key_dma_addr,
@@ -816,34 +807,34 @@ ssi_hmac_fips_run_test(struct ssi_drvdata *drvdata,
 		       dma_addr_t digest_bytes_len_dma_addr)
 {
 	/* The implemented flow is not the same as the one implemented in ssi_hash.c (setkey + digest flows).
-	   In this flow, there is no need to store and reload some of the intermidiate results. */
+	 * In this flow, there is no need to store and reload some of the intermidiate results.
+	 */
 
 	/* max number of descriptors used for the flow */
 	#define FIPS_HMAC_MAX_SEQ_LEN 12
 
 	int rc;
 	struct ssi_crypto_req ssi_req = {0};
-	HwDesc_s desc[FIPS_HMAC_MAX_SEQ_LEN];
+	struct cc_hw_desc desc[FIPS_HMAC_MAX_SEQ_LEN];
 	int idx = 0;
 	int i;
 	/* calc the hash opad first and ipad only afterwards (unlike the flow in ssi_hash.c) */
 	unsigned int hmacPadConst[2] = { HMAC_OPAD_CONST, HMAC_IPAD_CONST };
 
 	// assume (key_size <= block_size)
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, key_dma_addr, key_size, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], k0_dma_addr, key_size, NS_BIT, 0);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, key_dma_addr, key_size, NS_BIT);
+	set_flow_mode(&desc[idx], BYPASS);
+	set_dout_dlli(&desc[idx], k0_dma_addr, key_size, NS_BIT, 0);
 	idx++;
 
 	// if needed, append Key with zeros to create K0
 	if ((block_size - key_size) != 0) {
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_CONST(&desc[idx], 0, (block_size - key_size));
-		HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
-		HW_DESC_SET_DOUT_DLLI(&desc[idx], 
-				      (k0_dma_addr + key_size), (block_size - key_size),
-				      NS_BIT, 0);
+		hw_desc_init(&desc[idx]);
+		set_din_const(&desc[idx], 0, (block_size - key_size));
+		set_flow_mode(&desc[idx], BYPASS);
+		set_dout_dlli(&desc[idx], (k0_dma_addr + key_size),
+			      (block_size - key_size), NS_BIT, 0);
 		idx++;
 	}
 
@@ -858,50 +849,47 @@ ssi_hmac_fips_run_test(struct ssi_drvdata *drvdata,
 	/* calc derived HMAC key */
 	for (i = 0; i < 2; i++) {
 		/* Load hash initial state */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, initial_digest_dma_addr, inter_digestsize, NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], hw_mode);
+		set_din_type(&desc[idx], DMA_DLLI, initial_digest_dma_addr,
+			     inter_digestsize, NS_BIT);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 		idx++;
 
-
 		/* Load the hash current length*/
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
-		HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], hw_mode);
+		set_din_const(&desc[idx], 0, HASH_LEN_SIZE);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 		idx++;
 
 		/* Prepare opad/ipad key */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_XOR_VAL(&desc[idx], hmacPadConst[i]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+		hw_desc_init(&desc[idx]);
+		set_xor_val(&desc[idx], hmacPadConst[i]);
+		set_cipher_mode(&desc[idx], hw_mode);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
 		idx++;
 
 		/* Perform HASH update */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-				     k0_dma_addr,
-				     block_size, NS_BIT);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx],hw_mode);
-		HW_DESC_SET_XOR_ACTIVE(&desc[idx]);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI, k0_dma_addr, block_size,
+			     NS_BIT);
+		set_cipher_mode(&desc[idx], hw_mode);
+		set_xor_active(&desc[idx]);
+		set_flow_mode(&desc[idx], DIN_HASH);
 		idx++;
 
 		if (i == 0) {
 			/* First iteration - calc H(K0^opad) into tmp_digest_dma_addr */
-			HW_DESC_INIT(&desc[idx]);
-			HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
-			HW_DESC_SET_DOUT_DLLI(&desc[idx],
-					      tmp_digest_dma_addr,
-					      inter_digestsize,
-					      NS_BIT, 0);
-			HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+			hw_desc_init(&desc[idx]);
+			set_cipher_mode(&desc[idx], hw_mode);
+			set_dout_dlli(&desc[idx], tmp_digest_dma_addr,
+				      inter_digestsize, NS_BIT, 0);
+			set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+			set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
 			idx++;
 
 			// is this needed?? or continue with current descriptors??
@@ -916,85 +904,86 @@ ssi_hmac_fips_run_test(struct ssi_drvdata *drvdata,
 	}
 
 	/* data descriptor */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-			     din_dma_addr, data_in_size,
-			     NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, din_dma_addr, data_in_size, NS_BIT);
+	set_flow_mode(&desc[idx], DIN_HASH);
 	idx++;
 
 	/* HW last hash block padding (aka. "DO_PAD") */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], k0_dma_addr, HASH_LEN_SIZE, NS_BIT, 0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE1);
-	HW_DESC_SET_CIPHER_DO(&desc[idx], DO_PAD);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], hw_mode);
+	set_dout_dlli(&desc[idx], k0_dma_addr, HASH_LEN_SIZE, NS_BIT, 0);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE1);
+	set_cipher_do(&desc[idx], DO_PAD);
 	idx++;
 
 	/* store the hash digest result in the context */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], k0_dma_addr, digest_size, NS_BIT, 0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], hw_mode);
+	set_dout_dlli(&desc[idx], k0_dma_addr, digest_size, NS_BIT, 0);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
 	if (unlikely((hash_mode == DRV_HASH_MD5) ||
 		     (hash_mode == DRV_HASH_SHA384) ||
 		     (hash_mode == DRV_HASH_SHA512))) {
-		HW_DESC_SET_BYTES_SWAP(&desc[idx], 1);
+		set_bytes_swap(&desc[idx], 1);
 	} else {
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+		set_cipher_config0(&desc[idx],
+				   HASH_DIGEST_RESULT_LITTLE_ENDIAN);
 	}
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
 	idx++;
 
 	/* at this point:
-	   tmp_digest = H(o_key_pad)
-	   k0 = H(i_key_pad || m)
-	   */
+	 * tmp_digest = H(o_key_pad)
+	 * k0 = H(i_key_pad || m)
+	 */
 
 	/* Loading hash opad xor key state */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, tmp_digest_dma_addr, inter_digestsize, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], hw_mode);
+	set_din_type(&desc[idx], DMA_DLLI, tmp_digest_dma_addr,
+		     inter_digestsize, NS_BIT);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 	idx++;
 
 	/* Load the hash current length */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], hw_mode);
+	set_din_type(&desc[idx], DMA_DLLI, digest_bytes_len_dma_addr,
+		     HASH_LEN_SIZE, NS_BIT);
+	set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 	idx++;
 
 	/* Memory Barrier: wait for IPAD/OPAD axi write to complete */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	hw_desc_init(&desc[idx]);
+	set_din_no_dma(&desc[idx], 0, 0xfffff0);
+	set_dout_no_dma(&desc[idx], 0, 0, 1);
 	idx++;
 
 	/* Perform HASH update */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, k0_dma_addr, digest_size, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, k0_dma_addr, digest_size, NS_BIT);
+	set_flow_mode(&desc[idx], DIN_HASH);
 	idx++;
 
-
 	/* Get final MAC result */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], mac_res_dma_addr, digest_size, NS_BIT, 0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], hw_mode);
+	set_dout_dlli(&desc[idx], mac_res_dma_addr, digest_size, NS_BIT, 0);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	set_cipher_config1(&desc[idx], HASH_PADDING_DISABLED);
 	if (unlikely((hash_mode == DRV_HASH_MD5) ||
 		     (hash_mode == DRV_HASH_SHA384) ||
 		     (hash_mode == DRV_HASH_SHA512))) {
-		HW_DESC_SET_BYTES_SWAP(&desc[idx], 1);
+		set_bytes_swap(&desc[idx], 1);
 	} else {
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+		set_cipher_config0(&desc[idx],
+				   HASH_DIGEST_RESULT_LITTLE_ENDIAN);
 	}
 	idx++;
 
@@ -1005,10 +994,10 @@ ssi_hmac_fips_run_test(struct ssi_drvdata *drvdata,
 	return rc;
 }
 
-ssi_fips_error_t
+enum cc_fips_error
 ssi_hmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer)
 {
-	ssi_fips_error_t error = CC_REE_FIPS_ERROR_OK;
+	enum cc_fips_error error = CC_REE_FIPS_ERROR_OK;
 	size_t i;
 	struct fips_hmac_ctx *virt_ctx = (struct fips_hmac_ctx *)cpu_addr_buffer;
 
@@ -1023,7 +1012,7 @@ ssi_hmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 
 	for (i = 0; i < FIPS_HMAC_NUM_OF_TESTS; ++i)
 	{
-		FipsHmacData *hmac_data = (FipsHmacData*)&FipsHmacDataTable[i];
+		FipsHmacData *hmac_data = (FipsHmacData *)&FipsHmacDataTable[i];
 		int rc = 0;
 		enum drv_hash_hw_mode hw_mode = 0;
 		int digest_size = 0;
@@ -1038,7 +1027,7 @@ ssi_hmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 			digest_size = CC_SHA1_DIGEST_SIZE;
 			block_size = CC_SHA1_BLOCK_SIZE;
 			inter_digestsize = CC_SHA1_DIGEST_SIZE;
-			memcpy(virt_ctx->initial_digest, (void*)sha1_init, CC_SHA1_DIGEST_SIZE);
+			memcpy(virt_ctx->initial_digest, (void *)sha1_init, CC_SHA1_DIGEST_SIZE);
 			memcpy(virt_ctx->digest_bytes_len, digest_len_init, HASH_LEN_SIZE);
 			break;
 		case DRV_HASH_SHA256:
@@ -1046,7 +1035,7 @@ ssi_hmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 			digest_size = CC_SHA256_DIGEST_SIZE;
 			block_size = CC_SHA256_BLOCK_SIZE;
 			inter_digestsize = CC_SHA256_DIGEST_SIZE;
-			memcpy(virt_ctx->initial_digest, (void*)sha256_init, CC_SHA256_DIGEST_SIZE);
+			memcpy(virt_ctx->initial_digest, (void *)sha256_init, CC_SHA256_DIGEST_SIZE);
 			memcpy(virt_ctx->digest_bytes_len, digest_len_init, HASH_LEN_SIZE);
 			break;
 #if (CC_SUPPORT_SHA > 256)
@@ -1055,7 +1044,7 @@ ssi_hmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 			digest_size = CC_SHA512_DIGEST_SIZE;
 			block_size = CC_SHA512_BLOCK_SIZE;
 			inter_digestsize = CC_SHA512_DIGEST_SIZE;
-			memcpy(virt_ctx->initial_digest, (void*)sha512_init, CC_SHA512_DIGEST_SIZE);
+			memcpy(virt_ctx->initial_digest, (void *)sha512_init, CC_SHA512_DIGEST_SIZE);
 			memcpy(virt_ctx->digest_bytes_len, digest_len_sha512_init, HASH_LEN_SIZE);
 			break;
 #endif
@@ -1111,8 +1100,7 @@ ssi_hmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 	return error;
 }
 
-
-static inline int 
+static inline int
 ssi_ccm_fips_run_test(struct ssi_drvdata *drvdata,
 		      enum drv_crypto_direction direction,
 		      dma_addr_t key_dma_addr,
@@ -1131,7 +1119,7 @@ ssi_ccm_fips_run_test(struct ssi_drvdata *drvdata,
 
 	int rc;
 	struct ssi_crypto_req ssi_req = {0};
-	HwDesc_s desc[FIPS_CCM_MAX_SEQ_LEN];
+	struct cc_hw_desc desc[FIPS_CCM_MAX_SEQ_LEN];
 	unsigned int idx = 0;
 	unsigned int cipher_flow_mode;
 
@@ -1142,100 +1130,103 @@ ssi_ccm_fips_run_test(struct ssi_drvdata *drvdata,
 	}
 
 	/* load key */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CTR);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, key_dma_addr,
-			     ((key_size == NIST_AESCCM_192_BIT_KEY_SIZE) ? CC_AES_KEY_SIZE_MAX : key_size),
-			     NS_BIT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CTR);
+	set_din_type(&desc[idx], DMA_DLLI, key_dma_addr,
+		     ((key_size == NIST_AESCCM_192_BIT_KEY_SIZE) ?
+		     CC_AES_KEY_SIZE_MAX : key_size), NS_BIT)
+	set_key_size_aes(&desc[idx], key_size);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* load ctr state */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CTR);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			     iv_dma_addr, AES_BLOCK_SIZE,
-			     NS_BIT);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);	
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CTR);
+	set_key_size_aes(&desc[idx], key_size);
+	set_din_type(&desc[idx], DMA_DLLI, iv_dma_addr, AES_BLOCK_SIZE,
+		     NS_BIT);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* load MAC key */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CBC_MAC);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, key_dma_addr,
-			     ((key_size == NIST_AESCCM_192_BIT_KEY_SIZE) ? CC_AES_KEY_SIZE_MAX : key_size),
-			     NS_BIT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CBC_MAC);
+	set_din_type(&desc[idx], DMA_DLLI, key_dma_addr,
+		     ((key_size == NIST_AESCCM_192_BIT_KEY_SIZE) ?
+		     CC_AES_KEY_SIZE_MAX : key_size), NS_BIT);
+	set_key_size_aes(&desc[idx], key_size);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
 	idx++;
 
 	/* load MAC state */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CBC_MAC);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, mac_res_dma_addr, NIST_AESCCM_TAG_SIZE, NS_BIT);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);	
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CBC_MAC);
+	set_key_size_aes(&desc[idx], key_size);
+	set_din_type(&desc[idx], DMA_DLLI, mac_res_dma_addr,
+		     NIST_AESCCM_TAG_SIZE, NS_BIT);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
 	idx++;
 
 	/* prcess assoc data */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, b0_a0_adata_dma_addr, b0_a0_adata_size, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, b0_a0_adata_dma_addr,
+		     b0_a0_adata_size, NS_BIT);
+	set_flow_mode(&desc[idx], DIN_HASH);
 	idx++;
 
 	/* process the cipher */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, din_dma_addr, din_size, NS_BIT);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], dout_dma_addr, din_size, NS_BIT, 0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], cipher_flow_mode);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, din_dma_addr, din_size, NS_BIT);
+	set_dout_dlli(&desc[idx], dout_dma_addr, din_size, NS_BIT, 0);
+	set_flow_mode(&desc[idx], cipher_flow_mode);
 	idx++;
 
 	/* Read temporal MAC */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CBC_MAC);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], mac_res_dma_addr, NIST_AESCCM_TAG_SIZE, NS_BIT, 0);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CBC_MAC);
+	set_dout_dlli(&desc[idx], mac_res_dma_addr, NIST_AESCCM_TAG_SIZE,
+		      NS_BIT, 0);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	set_cipher_config0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_aes_not_hash_mode(&desc[idx]);
 	idx++;
 
 	/* load AES-CTR state (for last MAC calculation)*/
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CTR);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			     ctr_cnt_0_dma_addr,
-			     AES_BLOCK_SIZE, NS_BIT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CTR);
+	set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	set_din_type(&desc[idx], DMA_DLLI, ctr_cnt_0_dma_addr, AES_BLOCK_SIZE,
+		     NS_BIT);
+	set_key_size_aes(&desc[idx], key_size);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* Memory Barrier */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	hw_desc_init(&desc[idx]);
+	set_din_no_dma(&desc[idx], 0, 0xfffff0);
+	set_dout_no_dma(&desc[idx], 0, 0, 1);
 	idx++;
 
 	/* encrypt the "T" value and store MAC inplace */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, mac_res_dma_addr, NIST_AESCCM_TAG_SIZE, NS_BIT);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], mac_res_dma_addr, NIST_AESCCM_TAG_SIZE, NS_BIT, 0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
-	idx++;	
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, mac_res_dma_addr,
+		     NIST_AESCCM_TAG_SIZE, NS_BIT);
+	set_dout_dlli(&desc[idx], mac_res_dma_addr, NIST_AESCCM_TAG_SIZE,
+		      NS_BIT, 0);
+	set_flow_mode(&desc[idx], DIN_AES_DOUT);
+	idx++;
 
 	/* perform the operation - Lock HW and push sequence */
 	BUG_ON(idx > FIPS_CCM_MAX_SEQ_LEN);
@@ -1244,10 +1235,10 @@ ssi_ccm_fips_run_test(struct ssi_drvdata *drvdata,
 	return rc;
 }
 
-ssi_fips_error_t
+enum cc_fips_error
 ssi_ccm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer)
 {
-	ssi_fips_error_t error = CC_REE_FIPS_ERROR_OK;
+	enum cc_fips_error error = CC_REE_FIPS_ERROR_OK;
 	size_t i;
 	struct fips_ccm_ctx *virt_ctx = (struct fips_ccm_ctx *)cpu_addr_buffer;
 
@@ -1262,7 +1253,7 @@ ssi_ccm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 
 	for (i = 0; i < FIPS_CCM_NUM_OF_TESTS; ++i)
 	{
-		FipsCcmData *ccmData = (FipsCcmData*)&FipsCcmDataTable[i];
+		FipsCcmData *ccmData = (FipsCcmData *)&FipsCcmDataTable[i];
 		int rc = 0;
 
 		memset(cpu_addr_buffer, 0, sizeof(struct fips_ccm_ctx));
@@ -1273,6 +1264,7 @@ ssi_ccm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 		{
 			/* build B0 -- B0, nonce, l(m) */
 			__be16 data = cpu_to_be16(NIST_AESCCM_TEXT_SIZE);
+
 			virt_ctx->b0_a0_adata[0] = NIST_AESCCM_B0_VAL;
 			memcpy(virt_ctx->b0_a0_adata + 1, ccmData->nonce, NIST_AESCCM_NONCE_SIZE);
 			memcpy(virt_ctx->b0_a0_adata + 14, (u8 *)&data, sizeof(__be16));
@@ -1313,9 +1305,9 @@ ssi_ccm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 		if (memcmp(virt_ctx->dout, ccmData->dataOut, ccmData->dataInSize) != 0)
 		{
 			FIPS_LOG("dout comparison error %d - size=%d \n", i, ccmData->dataInSize);
-                        error = CC_REE_FIPS_ERROR_AESCCM_PUT;
+			error = CC_REE_FIPS_ERROR_AESCCM_PUT;
 			break;
-                }
+		}
 
 		/* compare actual mac result to expected */
 		if (memcmp(virt_ctx->mac_res, ccmData->macResOut, ccmData->tagSize) != 0)
@@ -1336,7 +1328,6 @@ ssi_ccm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 	return error;
 }
 
-
 static inline int
 ssi_gcm_fips_run_test(struct ssi_drvdata *drvdata,
 		      enum drv_crypto_direction direction,
@@ -1358,7 +1349,7 @@ ssi_gcm_fips_run_test(struct ssi_drvdata *drvdata,
 
 	int rc;
 	struct ssi_crypto_req ssi_req = {0};
-	HwDesc_s desc[FIPS_GCM_MAX_SEQ_LEN];
+	struct cc_hw_desc desc[FIPS_GCM_MAX_SEQ_LEN];
 	unsigned int idx = 0;
 	unsigned int cipher_flow_mode;
 
@@ -1372,186 +1363,162 @@ ssi_gcm_fips_run_test(struct ssi_drvdata *drvdata,
 //	ssi_aead_gcm_setup_ghash_desc(req, desc, seq_size);
 /////////////////////////////////   1   ////////////////////////////////////
 
-	/* load key to AES*/
-	HW_DESC_INIT(&desc[idx]);	
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_ECB);	
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
-	HW_DESC_SET_DIN_TYPE(&desc[idx],
-			     DMA_DLLI, key_dma_addr, key_size,
-			     NS_BIT); 
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	/* load key to AES */
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_ECB);
+	set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	set_din_type(&desc[idx], DMA_DLLI, key_dma_addr, key_size, NS_BIT);
+	set_key_size_aes(&desc[idx], key_size);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* process one zero block to generate hkey */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_CONST(&desc[idx], 0x0, AES_BLOCK_SIZE);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx],
-			      hkey_dma_addr, AES_BLOCK_SIZE,
-			      NS_BIT, 0); 
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	hw_desc_init(&desc[idx]);
+	set_din_const(&desc[idx], 0x0, AES_BLOCK_SIZE);
+	set_dout_dlli(&desc[idx], hkey_dma_addr, AES_BLOCK_SIZE, NS_BIT, 0);
+	set_flow_mode(&desc[idx], DIN_AES_DOUT);
 	idx++;
 
 	/* Memory Barrier */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	hw_desc_init(&desc[idx]);
+	set_din_no_dma(&desc[idx], 0, 0xfffff0);
+	set_dout_no_dma(&desc[idx], 0, 0, 1);
 	idx++;
 
 	/* Load GHASH subkey */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			     hkey_dma_addr, AES_BLOCK_SIZE,
-			     NS_BIT);
-	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);	
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);	
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, hkey_dma_addr, AES_BLOCK_SIZE,
+		     NS_BIT);
+	set_dout_no_dma(&desc[idx], 0, 0, 1);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_HASH_HW_GHASH);
+	set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 	idx++;
 
 	/* Configure Hash Engine to work with GHASH.
-	   Since it was not possible to extend HASH submodes to add GHASH,
-	   The following command is necessary in order to select GHASH (according to HW designers)*/
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);	
-	HW_DESC_SET_CIPHER_DO(&desc[idx], 1); //1=AES_SK RKEK
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED); 
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	 * Since it was not possible to extend HASH submodes to add GHASH,
+	 * The following command is necessary in order to
+	 * select GHASH (according to HW designers)
+	 */
+	hw_desc_init(&desc[idx]);
+	set_din_no_dma(&desc[idx], 0, 0xfffff0);
+	set_dout_no_dma(&desc[idx], 0, 0, 1);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_HASH_HW_GHASH);
+	set_cipher_do(&desc[idx], 1); //1=AES_SK RKEK
+	set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 	idx++;
 
 	/* Load GHASH initial STATE (which is 0). (for any hash there is an initial state) */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_CONST(&desc[idx], 0x0, AES_BLOCK_SIZE);
-	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED); 
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	hw_desc_init(&desc[idx]);
+	set_din_const(&desc[idx], 0x0, AES_BLOCK_SIZE);
+	set_dout_no_dma(&desc[idx], 0, 0, 1);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_aes_not_hash_mode(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_HASH_HW_GHASH);
+	set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 	idx++;
 
-
-
 /////////////////////////////////   2   ////////////////////////////////////
 	/* prcess(ghash) assoc data */
 //	if (req->assoclen > 0)
 //		ssi_aead_create_assoc_desc(req, DIN_HASH, desc, seq_size);
 /////////////////////////////////   2   ////////////////////////////////////
 
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-			     adata_dma_addr, adata_size,
-			     NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, adata_dma_addr, adata_size, NS_BIT);
+	set_flow_mode(&desc[idx], DIN_HASH);
 	idx++;
 
-
 /////////////////////////////////   3   ////////////////////////////////////
 //	ssi_aead_gcm_setup_gctr_desc(req, desc, seq_size);
 /////////////////////////////////   3   ////////////////////////////////////
 
 	/* load key to AES*/
-	HW_DESC_INIT(&desc[idx]);	
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);	
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			     key_dma_addr, key_size,
-			     NS_BIT); 
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_GCTR);
+	set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	set_din_type(&desc[idx], DMA_DLLI, key_dma_addr, key_size, NS_BIT);
+	set_key_size_aes(&desc[idx], key_size);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* load AES/CTR initial CTR value inc by 2*/
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			     iv_inc2_dma_addr, AES_BLOCK_SIZE,
-			     NS_BIT);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);	
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_GCTR);
+	set_key_size_aes(&desc[idx], key_size);
+	set_din_type(&desc[idx], DMA_DLLI, iv_inc2_dma_addr, AES_BLOCK_SIZE,
+		     NS_BIT);
+	set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
-
 /////////////////////////////////   4   ////////////////////////////////////
 	/* process(gctr+ghash) */
 //	if (req_ctx->cryptlen != 0)
-//		ssi_aead_process_cipher_data_desc(req, cipher_flow_mode, desc, seq_size); 
+//		ssi_aead_process_cipher_data_desc(req, cipher_flow_mode, desc, seq_size);
 /////////////////////////////////   4   ////////////////////////////////////
 
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			     din_dma_addr, din_size,
-			     NS_BIT);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx],
-			      dout_dma_addr, din_size,
-			      NS_BIT, 0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], cipher_flow_mode);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, din_dma_addr, din_size, NS_BIT);
+	set_dout_dlli(&desc[idx], dout_dma_addr, din_size, NS_BIT, 0);
+	set_flow_mode(&desc[idx], cipher_flow_mode);
 	idx++;
 
-
 /////////////////////////////////   5   ////////////////////////////////////
 //	ssi_aead_process_gcm_result_desc(req, desc, seq_size);
 /////////////////////////////////   5   ////////////////////////////////////
 
 	/* prcess(ghash) gcm_block_len */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-			     block_len_dma_addr, AES_BLOCK_SIZE,
-			     NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, block_len_dma_addr, AES_BLOCK_SIZE,
+		     NS_BIT);
+	set_flow_mode(&desc[idx], DIN_HASH);
 	idx++;
 
 	/* Store GHASH state after GHASH(Associated Data + Cipher +LenBlock) */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);
-	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx],
-			      mac_res_dma_addr, AES_BLOCK_SIZE,
-			      NS_BIT, 0);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
-	idx++; 
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_HASH_HW_GHASH);
+	set_din_no_dma(&desc[idx], 0, 0xfffff0);
+	set_dout_dlli(&desc[idx], mac_res_dma_addr, AES_BLOCK_SIZE, NS_BIT, 0);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_aes_not_hash_mode(&desc[idx]);
+	idx++;
 
 	/* load AES/CTR initial CTR value inc by 1*/
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			     iv_inc1_dma_addr, AES_BLOCK_SIZE,
-			     NS_BIT);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);	
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_GCTR);
+	set_key_size_aes(&desc[idx], key_size);
+	set_din_type(&desc[idx], DMA_DLLI, iv_inc1_dma_addr, AES_BLOCK_SIZE,
+		     NS_BIT);
+	set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* Memory Barrier */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	hw_desc_init(&desc[idx]);
+	set_din_no_dma(&desc[idx], 0, 0xfffff0);
+	set_dout_no_dma(&desc[idx], 0, 0, 1);
 	idx++;
 
 	/* process GCTR on stored GHASH and store MAC inplace */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-			     mac_res_dma_addr, AES_BLOCK_SIZE,
-			     NS_BIT);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx],
-			      mac_res_dma_addr, AES_BLOCK_SIZE,
-			      NS_BIT, 0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_GCTR);
+	set_din_type(&desc[idx], DMA_DLLI, mac_res_dma_addr, AES_BLOCK_SIZE,
+		     NS_BIT);
+	set_dout_dlli(&desc[idx], mac_res_dma_addr, AES_BLOCK_SIZE, NS_BIT, 0);
+	set_flow_mode(&desc[idx], DIN_AES_DOUT);
 	idx++;
 
 	/* perform the operation - Lock HW and push sequence */
@@ -1561,10 +1528,10 @@ ssi_gcm_fips_run_test(struct ssi_drvdata *drvdata,
 	return rc;
 }
 
-ssi_fips_error_t
+enum cc_fips_error
 ssi_gcm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer)
 {
-	ssi_fips_error_t error = CC_REE_FIPS_ERROR_OK;
+	enum cc_fips_error error = CC_REE_FIPS_ERROR_OK;
 	size_t i;
 	struct fips_gcm_ctx *virt_ctx = (struct fips_gcm_ctx *)cpu_addr_buffer;
 
@@ -1581,7 +1548,7 @@ ssi_gcm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 
 	for (i = 0; i < FIPS_GCM_NUM_OF_TESTS; ++i)
 	{
-		FipsGcmData *gcmData = (FipsGcmData*)&FipsGcmDataTable[i];
+		FipsGcmData *gcmData = (FipsGcmData *)&FipsGcmDataTable[i];
 		int rc = 0;
 
 		memset(cpu_addr_buffer, 0, sizeof(struct fips_gcm_ctx));
@@ -1594,6 +1561,7 @@ ssi_gcm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 		/* len_block */
 		{
 			__be64 len_bits;
+
 			len_bits = cpu_to_be64(gcmData->adataSize * 8);
 			memcpy(virt_ctx->len_block, &len_bits, sizeof(len_bits));
 			len_bits = cpu_to_be64(gcmData->dataInSize * 8);
@@ -1602,6 +1570,7 @@ ssi_gcm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 		/* iv_inc1, iv_inc2 */
 		{
 			__be32 counter = cpu_to_be32(1);
+
 			memcpy(virt_ctx->iv_inc1, gcmData->iv, NIST_AESGCM_IV_SIZE);
 			memcpy(virt_ctx->iv_inc1 + NIST_AESGCM_IV_SIZE, &counter, sizeof(counter));
 			counter = cpu_to_be32(2);
@@ -1666,7 +1635,6 @@ ssi_gcm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer,
 	return error;
 }
 
-
 size_t ssi_fips_max_mem_alloc_size(void)
 {
 	FIPS_DBG("sizeof(struct fips_cipher_ctx) %d \n", sizeof(struct fips_cipher_ctx));
diff --git a/drivers/staging/ccree/ssi_fips_local.c b/drivers/staging/ccree/ssi_fips_local.c
index 51b535a2a09a..aefb71dc9e9a 100644
--- a/drivers/staging/ccree/ssi_fips_local.c
+++ b/drivers/staging/ccree/ssi_fips_local.c
@@ -1,22 +1,22 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
 /**************************************************************
-This file defines the driver FIPS internal function, used by the driver itself.
-***************************************************************/
+ * This file defines the driver FIPS internal function, used by the driver itself.
+ ***************************************************************/
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
@@ -26,7 +26,6 @@ This file defines the driver FIPS internal function, used by the driver itself.
 #include "ssi_driver.h"
 #include "cc_hal.h"
 
-
 #define FIPS_POWER_UP_TEST_CIPHER	1
 #define FIPS_POWER_UP_TEST_CMAC		1
 #define FIPS_POWER_UP_TEST_HASH		1
@@ -49,62 +48,57 @@ struct ssi_fips_handle {
 #endif
 };
 
-
-extern int ssi_fips_get_state(ssi_fips_state_t *p_state);
-extern int ssi_fips_get_error(ssi_fips_error_t *p_err);
-extern int ssi_fips_ext_set_state(ssi_fips_state_t state);
-extern int ssi_fips_ext_set_error(ssi_fips_error_t err);
+extern int ssi_fips_get_state(enum cc_fips_state_t *p_state);
+extern int ssi_fips_get_error(enum cc_fips_error *p_err);
+extern int ssi_fips_ext_set_state(enum cc_fips_state_t state);
+extern int ssi_fips_ext_set_error(enum cc_fips_error err);
 
 /* FIPS power-up tests */
-extern ssi_fips_error_t ssi_cipher_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
-extern ssi_fips_error_t ssi_cmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
-extern ssi_fips_error_t ssi_hash_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
-extern ssi_fips_error_t ssi_hmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
-extern ssi_fips_error_t ssi_ccm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
-extern ssi_fips_error_t ssi_gcm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
+extern enum cc_fips_error ssi_cipher_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
+extern enum cc_fips_error ssi_cmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
+extern enum cc_fips_error ssi_hash_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
+extern enum cc_fips_error ssi_hmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
+extern enum cc_fips_error ssi_ccm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
+extern enum cc_fips_error ssi_gcm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
 extern size_t ssi_fips_max_mem_alloc_size(void);
 
-
 /* The function called once at driver entry point to check whether TEE FIPS error occured.*/
 static enum ssi_fips_error ssi_fips_get_tee_error(struct ssi_drvdata *drvdata)
 {
-	uint32_t regVal;
+	u32 regVal;
 	void __iomem *cc_base = drvdata->cc_base;
 
 	regVal = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, GPR_HOST));
-	if (regVal == (CC_FIPS_SYNC_TEE_STATUS | CC_FIPS_SYNC_MODULE_OK)) {
+	if (regVal == (CC_FIPS_SYNC_TEE_STATUS | CC_FIPS_SYNC_MODULE_OK))
 		return CC_REE_FIPS_ERROR_OK;
-	} 
+
 	return CC_REE_FIPS_ERROR_FROM_TEE;
 }
 
-
-/* 
- This function should push the FIPS REE library status towards the TEE library.
- By writing the error state to HOST_GPR0 register. The function is called from  						.
- driver entry point so no need to protect by mutex.
-*/
-static void ssi_fips_update_tee_upon_ree_status(struct ssi_drvdata *drvdata, ssi_fips_error_t err)
+/*
+ * This function should push the FIPS REE library status towards the TEE library.
+ * By writing the error state to HOST_GPR0 register. The function is called from
+ * driver entry point so no need to protect by mutex.
+ */
+static void ssi_fips_update_tee_upon_ree_status(struct ssi_drvdata *drvdata, enum cc_fips_error err)
 {
 	void __iomem *cc_base = drvdata->cc_base;
-	if (err == CC_REE_FIPS_ERROR_OK) {
-		CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_GPR0), (CC_FIPS_SYNC_REE_STATUS|CC_FIPS_SYNC_MODULE_OK));
-	} else {
-		CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_GPR0), (CC_FIPS_SYNC_REE_STATUS|CC_FIPS_SYNC_MODULE_ERROR));
-	}
-}
-
 
+	if (err == CC_REE_FIPS_ERROR_OK)
+		CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_GPR0), (CC_FIPS_SYNC_REE_STATUS | CC_FIPS_SYNC_MODULE_OK));
+	else
+		CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_GPR0), (CC_FIPS_SYNC_REE_STATUS | CC_FIPS_SYNC_MODULE_ERROR));
+}
 
 void ssi_fips_fini(struct ssi_drvdata *drvdata)
 {
 	struct ssi_fips_handle *fips_h = drvdata->fips_handle;
 
-	if (fips_h == NULL)
+	if (!fips_h)
 		return; /* Not allocated */
 
 #ifdef COMP_IN_WQ
-	if (fips_h->workq != NULL) {
+	if (fips_h->workq) {
 		flush_workqueue(fips_h->workq);
 		destroy_workqueue(fips_h->workq);
 	}
@@ -119,7 +113,7 @@ void ssi_fips_fini(struct ssi_drvdata *drvdata)
 
 void fips_handler(struct ssi_drvdata *drvdata)
 {
-	struct ssi_fips_handle *fips_handle_ptr = 
+	struct ssi_fips_handle *fips_handle_ptr =
 						drvdata->fips_handle;
 #ifdef COMP_IN_WQ
 	queue_delayed_work(fips_handle_ptr->workq, &fips_handle_ptr->fipswork, 0);
@@ -128,8 +122,6 @@ void fips_handler(struct ssi_drvdata *drvdata)
 #endif
 }
 
-
-
 #ifdef COMP_IN_WQ
 static void fips_wq_handler(struct work_struct *work)
 {
@@ -145,29 +137,27 @@ static void fips_dsr(unsigned long devarg)
 {
 	struct ssi_drvdata *drvdata = (struct ssi_drvdata *)devarg;
 	void __iomem *cc_base = drvdata->cc_base;
-	uint32_t irq;
-	uint32_t teeFipsError = 0;
+	u32 irq;
+	u32 teeFipsError = 0;
 
 	irq = (drvdata->irq & (SSI_GPR0_IRQ_MASK));
 
 	if (irq & SSI_GPR0_IRQ_MASK) {
 		teeFipsError = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, GPR_HOST));
-		if (teeFipsError != (CC_FIPS_SYNC_TEE_STATUS | CC_FIPS_SYNC_MODULE_OK)) {
+		if (teeFipsError != (CC_FIPS_SYNC_TEE_STATUS | CC_FIPS_SYNC_MODULE_OK))
 			ssi_fips_set_error(drvdata, CC_REE_FIPS_ERROR_FROM_TEE);
-		} 
 	}
 
 	/* after verifing that there is nothing to do, Unmask AXI completion interrupt */
-	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IMR), 
+	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IMR),
 		CC_HAL_READ_REGISTER(
 		CC_REG_OFFSET(HOST_RGF, HOST_IMR)) & ~irq);
 }
 
-
-ssi_fips_error_t cc_fips_run_power_up_tests(struct ssi_drvdata *drvdata)
+enum cc_fips_error cc_fips_run_power_up_tests(struct ssi_drvdata *drvdata)
 {
-	ssi_fips_error_t fips_error = CC_REE_FIPS_ERROR_OK;
-	void * cpu_addr_buffer = NULL;
+	enum cc_fips_error fips_error = CC_REE_FIPS_ERROR_OK;
+	void *cpu_addr_buffer = NULL;
 	dma_addr_t dma_handle;
 	size_t alloc_buff_size = ssi_fips_max_mem_alloc_size();
 	struct device *dev = &drvdata->plat_dev->dev;
@@ -177,9 +167,9 @@ ssi_fips_error_t cc_fips_run_power_up_tests(struct ssi_drvdata *drvdata)
 	// the dma_handle is the returned phy address - use it in the HW descriptor
 	FIPS_DBG("dma_alloc_coherent \n");
 	cpu_addr_buffer = dma_alloc_coherent(dev, alloc_buff_size, &dma_handle, GFP_KERNEL);
-	if (cpu_addr_buffer == NULL) {
+	if (!cpu_addr_buffer)
 		return CC_REE_FIPS_ERROR_GENERAL;
-	}
+
 	FIPS_DBG("allocated coherent buffer - addr 0x%08X , size = %d \n", (size_t)cpu_addr_buffer, alloc_buff_size);
 
 #if FIPS_POWER_UP_TEST_CIPHER
@@ -229,13 +219,12 @@ ssi_fips_error_t cc_fips_run_power_up_tests(struct ssi_drvdata *drvdata)
 	return fips_error;
 }
 
-
-
-/* The function checks if FIPS supported and FIPS error exists.* 
-*  It should be used in every driver API.*/
+/* The function checks if FIPS supported and FIPS error exists.*
+ * It should be used in every driver API.
+ */
 int ssi_fips_check_fips_error(void)
 {
-	ssi_fips_state_t fips_state; 
+	enum cc_fips_state_t fips_state;
 
 	if (ssi_fips_get_state(&fips_state) != 0) {
 		FIPS_LOG("ssi_fips_get_state FAILED, returning.. \n");
@@ -248,62 +237,61 @@ int ssi_fips_check_fips_error(void)
 	return 0;
 }
 
-
-/* The function sets the REE FIPS state.* 
-*  It should be used while driver is being loaded .*/
-int ssi_fips_set_state(ssi_fips_state_t state)
+/* The function sets the REE FIPS state.*
+ * It should be used while driver is being loaded.
+ */
+int ssi_fips_set_state(enum cc_fips_state_t state)
 {
 	return ssi_fips_ext_set_state(state);
 }
 
-/* The function sets the REE FIPS error, and pushes the error to TEE library. * 
-*  It should be used when any of the KAT tests fails .*/
-int ssi_fips_set_error(struct ssi_drvdata *p_drvdata, ssi_fips_error_t err)
+/* The function sets the REE FIPS error, and pushes the error to TEE library. *
+ * It should be used when any of the KAT tests fails.
+ */
+int ssi_fips_set_error(struct ssi_drvdata *p_drvdata, enum cc_fips_error err)
 {
 	int rc = 0;
-        ssi_fips_error_t current_err;
+	enum cc_fips_error current_err;
 
-        FIPS_LOG("ssi_fips_set_error - fips_error = %d \n", err);
+	FIPS_LOG("ssi_fips_set_error - fips_error = %d \n", err);
 
 	// setting no error is not allowed
-	if (err == CC_REE_FIPS_ERROR_OK) {
-                return -ENOEXEC;
-	} 
-        // If error exists, do not set new error
-        if (ssi_fips_get_error(&current_err) != 0) {
-                return -ENOEXEC;
-        }
-        if (current_err != CC_REE_FIPS_ERROR_OK) {
-                return -ENOEXEC;
-        }
-        // set REE internal error and state
+	if (err == CC_REE_FIPS_ERROR_OK)
+		return -ENOEXEC;
+
+	// If error exists, do not set new error
+	if (ssi_fips_get_error(&current_err) != 0)
+		return -ENOEXEC;
+
+	if (current_err != CC_REE_FIPS_ERROR_OK)
+		return -ENOEXEC;
+
+	// set REE internal error and state
 	rc = ssi_fips_ext_set_error(err);
-	if (rc != 0) {
-                return -ENOEXEC;
-	}
+	if (rc != 0)
+		return -ENOEXEC;
+
 	rc = ssi_fips_ext_set_state(CC_FIPS_STATE_ERROR);
-	if (rc != 0) {
-                return -ENOEXEC;
-	}
+	if (rc != 0)
+		return -ENOEXEC;
 
-        // push error towards TEE libraray, if it's not TEE error
-	if (err != CC_REE_FIPS_ERROR_FROM_TEE) {
+	// push error towards TEE libraray, if it's not TEE error
+	if (err != CC_REE_FIPS_ERROR_FROM_TEE)
 		ssi_fips_update_tee_upon_ree_status(p_drvdata, err);
-	}
+
 	return rc;
 }
 
-
 /* The function called once at driver entry point .*/
 int ssi_fips_init(struct ssi_drvdata *p_drvdata)
 {
-	ssi_fips_error_t rc = CC_REE_FIPS_ERROR_OK;
+	enum cc_fips_error rc = CC_REE_FIPS_ERROR_OK;
 	struct ssi_fips_handle *fips_h;
 
 	FIPS_DBG("CC FIPS code ..  (fips=%d) \n", ssi_fips_support);
 
-	fips_h = kzalloc(sizeof(struct ssi_fips_handle),GFP_KERNEL);
-	if (fips_h == NULL) {
+	fips_h = kzalloc(sizeof(struct ssi_fips_handle), GFP_KERNEL);
+	if (!fips_h) {
 		ssi_fips_set_error(p_drvdata, CC_REE_FIPS_ERROR_GENERAL);
 		return -ENOMEM;
 	}
@@ -313,7 +301,7 @@ int ssi_fips_init(struct ssi_drvdata *p_drvdata)
 #ifdef COMP_IN_WQ
 	SSI_LOG_DEBUG("Initializing fips workqueue\n");
 	fips_h->workq = create_singlethread_workqueue("arm_cc7x_fips_wq");
-	if (unlikely(fips_h->workq == NULL)) {
+	if (unlikely(!fips_h->workq)) {
 		SSI_LOG_ERR("Failed creating fips work queue\n");
 		ssi_fips_set_error(p_drvdata, CC_REE_FIPS_ERROR_GENERAL);
 		rc = -ENOMEM;
@@ -326,7 +314,7 @@ int ssi_fips_init(struct ssi_drvdata *p_drvdata)
 #endif
 
 	/* init fips driver data */
-	rc = ssi_fips_set_state((ssi_fips_support == 0)? CC_FIPS_STATE_NOT_SUPPORTED : CC_FIPS_STATE_SUPPORTED);
+	rc = ssi_fips_set_state((ssi_fips_support == 0) ? CC_FIPS_STATE_NOT_SUPPORTED : CC_FIPS_STATE_SUPPORTED);
 	if (unlikely(rc != 0)) {
 		ssi_fips_set_error(p_drvdata, CC_REE_FIPS_ERROR_GENERAL);
 		rc = -EAGAIN;
diff --git a/drivers/staging/ccree/ssi_fips_local.h b/drivers/staging/ccree/ssi_fips_local.h
index 65997c15a20e..8c7994fe9fae 100644
--- a/drivers/staging/ccree/ssi_fips_local.h
+++ b/drivers/staging/ccree/ssi_fips_local.h
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -17,32 +17,23 @@
 #ifndef __SSI_FIPS_LOCAL_H__
 #define __SSI_FIPS_LOCAL_H__
 
-
 #ifdef CONFIG_CCX7REE_FIPS_SUPPORT
 
 #include "ssi_fips.h"
 struct ssi_drvdata;
 
-// IG - how to make 1 file for TEE and REE
-typedef enum CC_FipsSyncStatus{
-	CC_FIPS_SYNC_MODULE_OK 		= 0x0,
-	CC_FIPS_SYNC_MODULE_ERROR 	= 0x1,
-	CC_FIPS_SYNC_REE_STATUS 	= 0x4,
-	CC_FIPS_SYNC_TEE_STATUS 	= 0x8,
-	CC_FIPS_SYNC_STATUS_RESERVE32B 	= INT32_MAX
-}CCFipsSyncStatus_t;
-
-
 #define CHECK_AND_RETURN_UPON_FIPS_ERROR() {\
-        if (ssi_fips_check_fips_error() != 0) {\
-                return -ENOEXEC;\
-        }\
+	if (ssi_fips_check_fips_error() != 0) {\
+		return -ENOEXEC;\
+	} \
 }
+
 #define CHECK_AND_RETURN_VOID_UPON_FIPS_ERROR() {\
-        if (ssi_fips_check_fips_error() != 0) {\
-                return;\
-        }\
+	if (ssi_fips_check_fips_error() != 0) {\
+		return;\
+	} \
 }
+
 #define SSI_FIPS_INIT(p_drvData)  (ssi_fips_init(p_drvData))
 #define SSI_FIPS_FINI(p_drvData)  (ssi_fips_fini(p_drvData))
 
@@ -53,7 +44,7 @@ typedef enum CC_FipsSyncStatus{
 int ssi_fips_init(struct ssi_drvdata *p_drvdata);
 void ssi_fips_fini(struct ssi_drvdata *drvdata);
 int ssi_fips_check_fips_error(void);
-int ssi_fips_set_error(struct ssi_drvdata *p_drvdata, ssi_fips_error_t err);
+int ssi_fips_set_error(struct ssi_drvdata *p_drvdata, enum cc_fips_error err);
 void fips_handler(struct ssi_drvdata *drvdata);
 
 #else  /* CONFIG_CC7XXREE_FIPS_SUPPORT */
@@ -72,6 +63,5 @@ void fips_handler(struct ssi_drvdata *drvdata);
 
 #endif  /* CONFIG_CC7XXREE_FIPS_SUPPORT */
 
-
 #endif  /*__SSI_FIPS_LOCAL_H__*/
 
diff --git a/drivers/staging/ccree/ssi_hash.c b/drivers/staging/ccree/ssi_hash.c
index f99d4219b01e..ae8f36af3837 100644
--- a/drivers/staging/ccree/ssi_hash.c
+++ b/drivers/staging/ccree/ssi_hash.c
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -42,64 +42,61 @@ struct ssi_hash_handle {
 	struct completion init_comp;
 };
 
-static const uint32_t digest_len_init[] = {
+static const u32 digest_len_init[] = {
 	0x00000040, 0x00000000, 0x00000000, 0x00000000 };
-static const uint32_t md5_init[] = { 
+static const u32 md5_init[] = {
 	SHA1_H3, SHA1_H2, SHA1_H1, SHA1_H0 };
-static const uint32_t sha1_init[] = { 
+static const u32 sha1_init[] = {
 	SHA1_H4, SHA1_H3, SHA1_H2, SHA1_H1, SHA1_H0 };
-static const uint32_t sha224_init[] = { 
+static const u32 sha224_init[] = {
 	SHA224_H7, SHA224_H6, SHA224_H5, SHA224_H4,
 	SHA224_H3, SHA224_H2, SHA224_H1, SHA224_H0 };
-static const uint32_t sha256_init[] = {
+static const u32 sha256_init[] = {
 	SHA256_H7, SHA256_H6, SHA256_H5, SHA256_H4,
 	SHA256_H3, SHA256_H2, SHA256_H1, SHA256_H0 };
 #if (DX_DEV_SHA_MAX > 256)
-static const uint32_t digest_len_sha512_init[] = { 
+static const u32 digest_len_sha512_init[] = {
 	0x00000080, 0x00000000, 0x00000000, 0x00000000 };
-static const uint64_t sha384_init[] = {
+static const u64 sha384_init[] = {
 	SHA384_H7, SHA384_H6, SHA384_H5, SHA384_H4,
 	SHA384_H3, SHA384_H2, SHA384_H1, SHA384_H0 };
-static const uint64_t sha512_init[] = {
+static const u64 sha512_init[] = {
 	SHA512_H7, SHA512_H6, SHA512_H5, SHA512_H4,
 	SHA512_H3, SHA512_H2, SHA512_H1, SHA512_H0 };
 #endif
 
 static void ssi_hash_create_xcbc_setup(
-	struct ahash_request *areq, 
-	HwDesc_s desc[],
+	struct ahash_request *areq,
+	struct cc_hw_desc desc[],
 	unsigned int *seq_size);
 
-static void ssi_hash_create_cmac_setup(struct ahash_request *areq, 
-				  HwDesc_s desc[],
+static void ssi_hash_create_cmac_setup(struct ahash_request *areq,
+				  struct cc_hw_desc desc[],
 				  unsigned int *seq_size);
 
 struct ssi_hash_alg {
 	struct list_head entry;
-	bool synchronize;
 	int hash_mode;
 	int hw_mode;
 	int inter_digestsize;
 	struct ssi_drvdata *drvdata;
-	union {
-		struct ahash_alg ahash_alg;
-		struct shash_alg shash_alg;
-	};
+	struct ahash_alg ahash_alg;
 };
 
-
 struct hash_key_req_ctx {
-	uint32_t keylen;
+	u32 keylen;
 	dma_addr_t key_dma_addr;
 };
 
 /* hash per-session context */
 struct ssi_hash_ctx {
 	struct ssi_drvdata *drvdata;
-	/* holds the origin digest; the digest after "setkey" if HMAC,* 
-	   the initial digest if HASH. */
-	uint8_t digest_buff[SSI_MAX_HASH_DIGEST_SIZE]  ____cacheline_aligned;
-	uint8_t opad_tmp_keys_buff[SSI_MAX_HASH_OPAD_TMP_KEYS_SIZE]  ____cacheline_aligned;
+	/* holds the origin digest; the digest after "setkey" if HMAC,*
+	 * the initial digest if HASH.
+	 */
+	u8 digest_buff[SSI_MAX_HASH_DIGEST_SIZE]  ____cacheline_aligned;
+	u8 opad_tmp_keys_buff[SSI_MAX_HASH_OPAD_TMP_KEYS_SIZE]  ____cacheline_aligned;
+
 	dma_addr_t opad_tmp_keys_dma_addr  ____cacheline_aligned;
 	dma_addr_t digest_buff_dma_addr;
 	/* use for hmac with key large then mode block size */
@@ -111,31 +108,29 @@ struct ssi_hash_ctx {
 	bool is_hmac;
 };
 
-static const struct crypto_type crypto_shash_type;
-
 static void ssi_hash_create_data_desc(
 	struct ahash_req_ctx *areq_ctx,
-	struct ssi_hash_ctx *ctx, 
-	unsigned int flow_mode,HwDesc_s desc[],
+	struct ssi_hash_ctx *ctx,
+	unsigned int flow_mode, struct cc_hw_desc desc[],
 	bool is_not_last_data,
 	unsigned int *seq_size);
 
-static inline void ssi_set_hash_endianity(uint32_t mode, HwDesc_s *desc)
+static inline void ssi_set_hash_endianity(u32 mode, struct cc_hw_desc *desc)
 {
 	if (unlikely((mode == DRV_HASH_MD5) ||
 		(mode == DRV_HASH_SHA384) ||
 		(mode == DRV_HASH_SHA512))) {
-		HW_DESC_SET_BYTES_SWAP(desc, 1);
+		set_bytes_swap(desc, 1);
 	} else {
-		HW_DESC_SET_CIPHER_CONFIG0(desc, HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+		set_cipher_config0(desc, HASH_DIGEST_RESULT_LITTLE_ENDIAN);
 	}
 }
 
-static int ssi_hash_map_result(struct device *dev, 
-			       struct ahash_req_ctx *state, 
+static int ssi_hash_map_result(struct device *dev,
+			       struct ahash_req_ctx *state,
 			       unsigned int digestsize)
 {
-	state->digest_result_dma_addr = 
+	state->digest_result_dma_addr =
 		dma_map_single(dev, (void *)state->digest_result_buff,
 			       digestsize,
 			       DMA_BIDIRECTIONAL);
@@ -144,8 +139,6 @@ static int ssi_hash_map_result(struct device *dev,
 			digestsize);
 		return -ENOMEM;
 	}
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(state->digest_result_dma_addr,
-						digestsize);
 	SSI_LOG_DEBUG("Mapped digest result buffer %u B "
 		     "at va=%pK to dma=0x%llX\n",
 		digestsize, state->digest_result_buff,
@@ -154,33 +147,33 @@ static int ssi_hash_map_result(struct device *dev,
 	return 0;
 }
 
-static int ssi_hash_map_request(struct device *dev, 
-				struct ahash_req_ctx *state, 
+static int ssi_hash_map_request(struct device *dev,
+				struct ahash_req_ctx *state,
 				struct ssi_hash_ctx *ctx)
 {
 	bool is_hmac = ctx->is_hmac;
 	ssi_sram_addr_t larval_digest_addr = ssi_ahash_get_larval_digest_sram_addr(
 					ctx->drvdata, ctx->hash_mode);
 	struct ssi_crypto_req ssi_req = {};
-	HwDesc_s desc;
+	struct cc_hw_desc desc;
 	int rc = -ENOMEM;
 
-	state->buff0 = kzalloc(SSI_MAX_HASH_BLCK_SIZE ,GFP_KERNEL|GFP_DMA);
+	state->buff0 = kzalloc(SSI_MAX_HASH_BLCK_SIZE, GFP_KERNEL | GFP_DMA);
 	if (!state->buff0) {
 		SSI_LOG_ERR("Allocating buff0 in context failed\n");
 		goto fail0;
 	}
-	state->buff1 = kzalloc(SSI_MAX_HASH_BLCK_SIZE ,GFP_KERNEL|GFP_DMA);
+	state->buff1 = kzalloc(SSI_MAX_HASH_BLCK_SIZE, GFP_KERNEL | GFP_DMA);
 	if (!state->buff1) {
 		SSI_LOG_ERR("Allocating buff1 in context failed\n");
 		goto fail_buff0;
 	}
-	state->digest_result_buff = kzalloc(SSI_MAX_HASH_DIGEST_SIZE ,GFP_KERNEL|GFP_DMA);
+	state->digest_result_buff = kzalloc(SSI_MAX_HASH_DIGEST_SIZE, GFP_KERNEL | GFP_DMA);
 	if (!state->digest_result_buff) {
 		SSI_LOG_ERR("Allocating digest_result_buff in context failed\n");
 		goto fail_buff1;
 	}
-	state->digest_buff = kzalloc(ctx->inter_digestsize, GFP_KERNEL|GFP_DMA);
+	state->digest_buff = kzalloc(ctx->inter_digestsize, GFP_KERNEL | GFP_DMA);
 	if (!state->digest_buff) {
 		SSI_LOG_ERR("Allocating digest-buffer in context failed\n");
 		goto fail_digest_result_buff;
@@ -188,7 +181,7 @@ static int ssi_hash_map_request(struct device *dev,
 
 	SSI_LOG_DEBUG("Allocated digest-buffer in context ctx->digest_buff=@%p\n", state->digest_buff);
 	if (ctx->hw_mode != DRV_CIPHER_XCBC_MAC) {
-		state->digest_bytes_len = kzalloc(HASH_LEN_SIZE, GFP_KERNEL|GFP_DMA);
+		state->digest_bytes_len = kzalloc(HASH_LEN_SIZE, GFP_KERNEL | GFP_DMA);
 		if (!state->digest_bytes_len) {
 			SSI_LOG_ERR("Allocating digest-bytes-len in context failed\n");
 			goto fail1;
@@ -198,7 +191,7 @@ static int ssi_hash_map_request(struct device *dev,
 		state->digest_bytes_len = NULL;
 	}
 
-	state->opad_digest_buff = kzalloc(ctx->inter_digestsize, GFP_KERNEL|GFP_DMA);
+	state->opad_digest_buff = kzalloc(ctx->inter_digestsize, GFP_KERNEL | GFP_DMA);
 	if (!state->opad_digest_buff) {
 		SSI_LOG_ERR("Allocating opad-digest-buffer in context failed\n");
 		goto fail2;
@@ -211,50 +204,40 @@ static int ssi_hash_map_request(struct device *dev,
 		ctx->inter_digestsize, state->digest_buff);
 		goto fail3;
 	}
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(state->digest_buff_dma_addr, 
-							ctx->inter_digestsize);
 	SSI_LOG_DEBUG("Mapped digest %d B at va=%pK to dma=0x%llX\n",
 		ctx->inter_digestsize, state->digest_buff,
 		(unsigned long long)state->digest_buff_dma_addr);
 
 	if (is_hmac) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->digest_buff_dma_addr);
 		dma_sync_single_for_cpu(dev, ctx->digest_buff_dma_addr, ctx->inter_digestsize, DMA_BIDIRECTIONAL);
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->digest_buff_dma_addr, 
-							ctx->inter_digestsize);
 		if ((ctx->hw_mode == DRV_CIPHER_XCBC_MAC) || (ctx->hw_mode == DRV_CIPHER_CMAC)) {
 			memset(state->digest_buff, 0, ctx->inter_digestsize);
 		} else { /*sha*/
 			memcpy(state->digest_buff, ctx->digest_buff, ctx->inter_digestsize);
 #if (DX_DEV_SHA_MAX > 256)
-			if (unlikely((ctx->hash_mode == DRV_HASH_SHA512) || (ctx->hash_mode == DRV_HASH_SHA384))) {
+			if (unlikely((ctx->hash_mode == DRV_HASH_SHA512) || (ctx->hash_mode == DRV_HASH_SHA384)))
 				memcpy(state->digest_bytes_len, digest_len_sha512_init, HASH_LEN_SIZE);
-			} else {
+			else
 				memcpy(state->digest_bytes_len, digest_len_init, HASH_LEN_SIZE);
-			}
 #else
 			memcpy(state->digest_bytes_len, digest_len_init, HASH_LEN_SIZE);
 #endif
 		}
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->digest_buff_dma_addr);
 		dma_sync_single_for_device(dev, state->digest_buff_dma_addr, ctx->inter_digestsize, DMA_BIDIRECTIONAL);
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(state->digest_buff_dma_addr, 
-							ctx->inter_digestsize);
 
 		if (ctx->hash_mode != DRV_HASH_NULL) {
-			SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->opad_tmp_keys_dma_addr);
 			dma_sync_single_for_cpu(dev, ctx->opad_tmp_keys_dma_addr, ctx->inter_digestsize, DMA_BIDIRECTIONAL);
 			memcpy(state->opad_digest_buff, ctx->opad_tmp_keys_buff, ctx->inter_digestsize);
-			SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->opad_tmp_keys_dma_addr, 
-							ctx->inter_digestsize);
-		} 
+		}
 	} else { /*hash*/
 		/* Copy the initial digests if hash flow. The SRAM contains the
-		initial digests in the expected order for all SHA* */
-		HW_DESC_INIT(&desc);
-		HW_DESC_SET_DIN_SRAM(&desc, larval_digest_addr, ctx->inter_digestsize);
-		HW_DESC_SET_DOUT_DLLI(&desc, state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT, 0);
-		HW_DESC_SET_FLOW_MODE(&desc, BYPASS);
+		 * initial digests in the expected order for all SHA*
+		 */
+		hw_desc_init(&desc);
+		set_din_sram(&desc, larval_digest_addr, ctx->inter_digestsize);
+		set_dout_dlli(&desc, state->digest_buff_dma_addr,
+			      ctx->inter_digestsize, NS_BIT, 0);
+		set_flow_mode(&desc, BYPASS);
 
 		rc = send_request(ctx->drvdata, &ssi_req, &desc, 1, 0);
 		if (unlikely(rc != 0)) {
@@ -270,8 +253,6 @@ static int ssi_hash_map_request(struct device *dev,
 			HASH_LEN_SIZE, state->digest_bytes_len);
 			goto fail4;
 		}
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(state->digest_bytes_len_dma_addr,
-								HASH_LEN_SIZE);
 		SSI_LOG_DEBUG("Mapped digest len %u B at va=%pK to dma=0x%llX\n",
 			HASH_LEN_SIZE, state->digest_bytes_len,
 			(unsigned long long)state->digest_bytes_len_dma_addr);
@@ -286,8 +267,6 @@ static int ssi_hash_map_request(struct device *dev,
 			ctx->inter_digestsize, state->opad_digest_buff);
 			goto fail5;
 		}
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(state->opad_digest_dma_addr,
-							ctx->inter_digestsize);
 		SSI_LOG_DEBUG("Mapped opad digest %d B at va=%pK to dma=0x%llX\n",
 			ctx->inter_digestsize, state->opad_digest_buff,
 			(unsigned long long)state->opad_digest_dma_addr);
@@ -303,13 +282,11 @@ static int ssi_hash_map_request(struct device *dev,
 
 fail5:
 	if (state->digest_bytes_len_dma_addr != 0) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->digest_bytes_len_dma_addr);
 		dma_unmap_single(dev, state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, DMA_BIDIRECTIONAL);
 		state->digest_bytes_len_dma_addr = 0;
 	}
 fail4:
 	if (state->digest_buff_dma_addr != 0) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->digest_buff_dma_addr);
 		dma_unmap_single(dev, state->digest_buff_dma_addr, ctx->inter_digestsize, DMA_BIDIRECTIONAL);
 		state->digest_buff_dma_addr = 0;
 	}
@@ -320,17 +297,17 @@ fail2:
 fail1:
 	 kfree(state->digest_buff);
 fail_digest_result_buff:
-	 if (state->digest_result_buff != NULL) {
+	 if (state->digest_result_buff) {
 		 kfree(state->digest_result_buff);
 	     state->digest_result_buff = NULL;
 	 }
 fail_buff1:
-	 if (state->buff1 != NULL) {
+	 if (state->buff1) {
 		 kfree(state->buff1);
 	     state->buff1 = NULL;
 	 }
 fail_buff0:
-	 if (state->buff0 != NULL) {
+	 if (state->buff0) {
 		 kfree(state->buff0);
 	     state->buff0 = NULL;
 	 }
@@ -338,12 +315,11 @@ fail0:
 	return rc;
 }
 
-static void ssi_hash_unmap_request(struct device *dev, 
-				   struct ahash_req_ctx *state, 
+static void ssi_hash_unmap_request(struct device *dev,
+				   struct ahash_req_ctx *state,
 				   struct ssi_hash_ctx *ctx)
 {
 	if (state->digest_buff_dma_addr != 0) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->digest_buff_dma_addr);
 		dma_unmap_single(dev, state->digest_buff_dma_addr,
 				 ctx->inter_digestsize, DMA_BIDIRECTIONAL);
 		SSI_LOG_DEBUG("Unmapped digest-buffer: digest_buff_dma_addr=0x%llX\n",
@@ -351,7 +327,6 @@ static void ssi_hash_unmap_request(struct device *dev,
 		state->digest_buff_dma_addr = 0;
 	}
 	if (state->digest_bytes_len_dma_addr != 0) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->digest_bytes_len_dma_addr);
 		dma_unmap_single(dev, state->digest_bytes_len_dma_addr,
 				 HASH_LEN_SIZE, DMA_BIDIRECTIONAL);
 		SSI_LOG_DEBUG("Unmapped digest-bytes-len buffer: digest_bytes_len_dma_addr=0x%llX\n",
@@ -359,7 +334,6 @@ static void ssi_hash_unmap_request(struct device *dev,
 		state->digest_bytes_len_dma_addr = 0;
 	}
 	if (state->opad_digest_dma_addr != 0) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->opad_digest_dma_addr);
 		dma_unmap_single(dev, state->opad_digest_dma_addr,
 				 ctx->inter_digestsize, DMA_BIDIRECTIONAL);
 		SSI_LOG_DEBUG("Unmapped opad-digest: opad_digest_dma_addr=0x%llX\n",
@@ -375,19 +349,18 @@ static void ssi_hash_unmap_request(struct device *dev,
 	kfree(state->buff0);
 }
 
-static void ssi_hash_unmap_result(struct device *dev, 
-				  struct ahash_req_ctx *state, 
+static void ssi_hash_unmap_result(struct device *dev,
+				  struct ahash_req_ctx *state,
 				  unsigned int digestsize, u8 *result)
 {
 	if (state->digest_result_dma_addr != 0) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->digest_result_dma_addr);
 		dma_unmap_single(dev,
 				 state->digest_result_dma_addr,
 				 digestsize,
-				  DMA_BIDIRECTIONAL);	
+				  DMA_BIDIRECTIONAL);
 		SSI_LOG_DEBUG("unmpa digest result buffer "
 			     "va (%pK) pa (%llx) len %u\n",
-			     state->digest_result_buff, 
+			     state->digest_result_buff,
 			     (unsigned long long)state->digest_result_dma_addr,
 			     digestsize);
 		memcpy(result,
@@ -414,8 +387,8 @@ static void ssi_hash_digest_complete(struct device *dev, void *ssi_req, void __i
 	struct ahash_req_ctx *state = ahash_request_ctx(req);
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
-	uint32_t digestsize = crypto_ahash_digestsize(tfm);
-	
+	u32 digestsize = crypto_ahash_digestsize(tfm);
+
 	SSI_LOG_DEBUG("req=%pK\n", req);
 
 	ssi_buffer_mgr_unmap_hash_request(dev, state, req->src, false);
@@ -430,8 +403,8 @@ static void ssi_hash_complete(struct device *dev, void *ssi_req, void __iomem *c
 	struct ahash_req_ctx *state = ahash_request_ctx(req);
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
-	uint32_t digestsize = crypto_ahash_digestsize(tfm);
-	
+	u32 digestsize = crypto_ahash_digestsize(tfm);
+
 	SSI_LOG_DEBUG("req=%pK\n", req);
 
 	ssi_buffer_mgr_unmap_hash_request(dev, state, req->src, false);
@@ -440,24 +413,23 @@ static void ssi_hash_complete(struct device *dev, void *ssi_req, void __iomem *c
 	req->base.complete(&req->base, 0);
 }
 
-static int ssi_hash_digest(struct ahash_req_ctx *state, 
-			   struct ssi_hash_ctx *ctx, 
-			   unsigned int digestsize, 
-			   struct scatterlist *src, 
-			   unsigned int nbytes, u8 *result, 
+static int ssi_hash_digest(struct ahash_req_ctx *state,
+			   struct ssi_hash_ctx *ctx,
+			   unsigned int digestsize,
+			   struct scatterlist *src,
+			   unsigned int nbytes, u8 *result,
 			   void *async_req)
 {
 	struct device *dev = &ctx->drvdata->plat_dev->dev;
 	bool is_hmac = ctx->is_hmac;
 	struct ssi_crypto_req ssi_req = {};
-	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	struct cc_hw_desc desc[SSI_MAX_AHASH_SEQ_LEN];
 	ssi_sram_addr_t larval_digest_addr = ssi_ahash_get_larval_digest_sram_addr(
 					ctx->drvdata, ctx->hash_mode);
 	int idx = 0;
 	int rc = 0;
 
-
-	SSI_LOG_DEBUG("===== %s-digest (%d) ====\n", is_hmac?"hmac":"hash", nbytes);
+	SSI_LOG_DEBUG("===== %s-digest (%d) ====\n", is_hmac ? "hmac" : "hash", nbytes);
 
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
 
@@ -480,102 +452,109 @@ static int ssi_hash_digest(struct ahash_req_ctx *state,
 		/* Setup DX request structure */
 		ssi_req.user_cb = (void *)ssi_hash_digest_complete;
 		ssi_req.user_arg = (void *)async_req;
-#ifdef ENABLE_CYCLE_COUNT
-		ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
-#endif
 	}
 
 	/* If HMAC then load hash IPAD xor key, if HASH then load initial digest */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
 	if (is_hmac) {
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT);
+		set_din_type(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr,
+			     ctx->inter_digestsize, NS_BIT);
 	} else {
-		HW_DESC_SET_DIN_SRAM(&desc[idx], larval_digest_addr, ctx->inter_digestsize);
+		set_din_sram(&desc[idx], larval_digest_addr,
+			     ctx->inter_digestsize);
 	}
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 	idx++;
 
 	/* Load the hash current length */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
 
 	if (is_hmac) {
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT);
+		set_din_type(&desc[idx], DMA_DLLI,
+			     state->digest_bytes_len_dma_addr, HASH_LEN_SIZE,
+			     NS_BIT);
 	} else {
-		HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
-		if (likely(nbytes != 0)) {
-			HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
-		} else {
-			HW_DESC_SET_CIPHER_DO(&desc[idx], DO_PAD);
-		}
-	}
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		set_din_const(&desc[idx], 0, HASH_LEN_SIZE);
+		if (likely(nbytes != 0))
+			set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+		else
+			set_cipher_do(&desc[idx], DO_PAD);
+	}
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 	idx++;
 
 	ssi_hash_create_data_desc(state, ctx, DIN_HASH, desc, false, &idx);
 
 	if (is_hmac) {
 		/* HW last hash block padding (aka. "DO_PAD") */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, HASH_LEN_SIZE, NS_BIT, 0);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE1);
-		HW_DESC_SET_CIPHER_DO(&desc[idx], DO_PAD);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_dout_dlli(&desc[idx], state->digest_buff_dma_addr,
+			      HASH_LEN_SIZE, NS_BIT, 0);
+		set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+		set_setup_mode(&desc[idx], SETUP_WRITE_STATE1);
+		set_cipher_do(&desc[idx], DO_PAD);
 		idx++;
 
 		/* store the hash digest result in the context */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, digestsize, NS_BIT, 0);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_dout_dlli(&desc[idx], state->digest_buff_dma_addr,
+			      digestsize, NS_BIT, 0);
+		set_flow_mode(&desc[idx], S_HASH_to_DOUT);
 		ssi_set_hash_endianity(ctx->hash_mode, &desc[idx]);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+		set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
 		idx++;
 
 		/* Loading hash opad xor key state */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->opad_digest_dma_addr, ctx->inter_digestsize, NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_din_type(&desc[idx], DMA_DLLI, state->opad_digest_dma_addr,
+			     ctx->inter_digestsize, NS_BIT);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 		idx++;
 
 		/* Load the hash current length */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_DIN_SRAM(&desc[idx], ssi_ahash_get_initial_digest_len_sram_addr(ctx->drvdata, ctx->hash_mode), HASH_LEN_SIZE);
-		HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_din_sram(&desc[idx],
+			     ssi_ahash_get_initial_digest_len_sram_addr(
+ctx->drvdata, ctx->hash_mode), HASH_LEN_SIZE);
+		set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 		idx++;
 
 		/* Memory Barrier: wait for IPAD/OPAD axi write to complete */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-		HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+		hw_desc_init(&desc[idx]);
+		set_din_no_dma(&desc[idx], 0, 0xfffff0);
+		set_dout_no_dma(&desc[idx], 0, 0, 1);
 		idx++;
 
 		/* Perform HASH update */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, digestsize, NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr,
+			     digestsize, NS_BIT);
+		set_flow_mode(&desc[idx], DIN_HASH);
 		idx++;
 	}
 
 	/* Get final MAC result */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode); 
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_result_dma_addr, digestsize, NS_BIT, async_req? 1:0);   /*TODO*/
-	if (async_req) {
-		HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
-	}
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
+	/* TODO */
+	set_dout_dlli(&desc[idx], state->digest_result_dma_addr, digestsize,
+		      NS_BIT, (async_req ? 1 : 0));
+	if (async_req)
+		set_queue_last_ind(&desc[idx]);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	set_cipher_config1(&desc[idx], HASH_PADDING_DISABLED);
 	ssi_set_hash_endianity(ctx->hash_mode, &desc[idx]);
 	idx++;
 
@@ -593,7 +572,7 @@ static int ssi_hash_digest(struct ahash_req_ctx *state,
 			SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
 			ssi_buffer_mgr_unmap_hash_request(dev, state, src, true);
 		} else {
-			ssi_buffer_mgr_unmap_hash_request(dev, state, src, false);			
+			ssi_buffer_mgr_unmap_hash_request(dev, state, src, false);
 		}
 		ssi_hash_unmap_result(dev, state, digestsize, result);
 		ssi_hash_unmap_request(dev, state, ctx);
@@ -601,21 +580,21 @@ static int ssi_hash_digest(struct ahash_req_ctx *state,
 	return rc;
 }
 
-static int ssi_hash_update(struct ahash_req_ctx *state, 
-			   struct ssi_hash_ctx *ctx, 
-			   unsigned int block_size, 
-			   struct scatterlist *src, 
-			   unsigned int nbytes, 
+static int ssi_hash_update(struct ahash_req_ctx *state,
+			   struct ssi_hash_ctx *ctx,
+			   unsigned int block_size,
+			   struct scatterlist *src,
+			   unsigned int nbytes,
 			   void *async_req)
 {
 	struct device *dev = &ctx->drvdata->plat_dev->dev;
 	struct ssi_crypto_req ssi_req = {};
-	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
-	uint32_t idx = 0;
+	struct cc_hw_desc desc[SSI_MAX_AHASH_SEQ_LEN];
+	u32 idx = 0;
 	int rc;
 
 	SSI_LOG_DEBUG("===== %s-update (%d) ====\n", ctx->is_hmac ?
-					"hmac":"hash", nbytes);
+					"hmac" : "hash", nbytes);
 
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
 	if (nbytes == 0) {
@@ -638,45 +617,45 @@ static int ssi_hash_update(struct ahash_req_ctx *state,
 		/* Setup DX request structure */
 		ssi_req.user_cb = (void *)ssi_hash_update_complete;
 		ssi_req.user_arg = async_req;
-#ifdef ENABLE_CYCLE_COUNT
-		ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
-#endif
 	}
 
 	/* Restore hash digest */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
+	set_din_type(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr,
+		     ctx->inter_digestsize, NS_BIT);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 	idx++;
 	/* Restore hash current length */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
+	set_din_type(&desc[idx], DMA_DLLI, state->digest_bytes_len_dma_addr,
+		     HASH_LEN_SIZE, NS_BIT);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 	idx++;
 
 	ssi_hash_create_data_desc(state, ctx, DIN_HASH, desc, false, &idx);
 
 	/* store the hash digest result in context */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT, 0);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
+	set_dout_dlli(&desc[idx], state->digest_buff_dma_addr,
+		      ctx->inter_digestsize, NS_BIT, 0);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
 	idx++;
 
 	/* store current hash length in context */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT, async_req? 1:0);
-	if (async_req) {
-		HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
-	}
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE1);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
+	set_dout_dlli(&desc[idx], state->digest_bytes_len_dma_addr,
+		      HASH_LEN_SIZE, NS_BIT, (async_req ? 1 : 0));
+	if (async_req)
+		set_queue_last_ind(&desc[idx]);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE1);
 	idx++;
 
 	if (async_req) {
@@ -697,26 +676,26 @@ static int ssi_hash_update(struct ahash_req_ctx *state,
 	return rc;
 }
 
-static int ssi_hash_finup(struct ahash_req_ctx *state, 
-			  struct ssi_hash_ctx *ctx, 
-			  unsigned int digestsize, 
-			  struct scatterlist *src, 
-			  unsigned int nbytes, 
-			  u8 *result, 
+static int ssi_hash_finup(struct ahash_req_ctx *state,
+			  struct ssi_hash_ctx *ctx,
+			  unsigned int digestsize,
+			  struct scatterlist *src,
+			  unsigned int nbytes,
+			  u8 *result,
 			  void *async_req)
 {
 	struct device *dev = &ctx->drvdata->plat_dev->dev;
 	bool is_hmac = ctx->is_hmac;
 	struct ssi_crypto_req ssi_req = {};
-	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	struct cc_hw_desc desc[SSI_MAX_AHASH_SEQ_LEN];
 	int idx = 0;
 	int rc;
 
-	SSI_LOG_DEBUG("===== %s-finup (%d) ====\n", is_hmac?"hmac":"hash", nbytes);
+	SSI_LOG_DEBUG("===== %s-finup (%d) ====\n", is_hmac ? "hmac" : "hash", nbytes);
 
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
 
-	if (unlikely(ssi_buffer_mgr_map_hash_request_final(ctx->drvdata, state, src , nbytes, 1) != 0)) {
+	if (unlikely(ssi_buffer_mgr_map_hash_request_final(ctx->drvdata, state, src, nbytes, 1) != 0)) {
 		SSI_LOG_ERR("map_ahash_request_final() failed\n");
 		return -ENOMEM;
 	}
@@ -729,81 +708,86 @@ static int ssi_hash_finup(struct ahash_req_ctx *state,
 		/* Setup DX request structure */
 		ssi_req.user_cb = (void *)ssi_hash_complete;
 		ssi_req.user_arg = async_req;
-#ifdef ENABLE_CYCLE_COUNT
-		ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
-#endif
 	}
 
 	/* Restore hash digest */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
+	set_din_type(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr,
+		     ctx->inter_digestsize, NS_BIT);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 	idx++;
 
 	/* Restore hash current length */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
+	set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+	set_din_type(&desc[idx], DMA_DLLI, state->digest_bytes_len_dma_addr,
+		     HASH_LEN_SIZE, NS_BIT);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 	idx++;
 
 	ssi_hash_create_data_desc(state, ctx, DIN_HASH, desc, false, &idx);
 
 	if (is_hmac) {
 		/* Store the hash digest result in the context */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, digestsize, NS_BIT, 0);
-		ssi_set_hash_endianity(ctx->hash_mode,&desc[idx]);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_dout_dlli(&desc[idx], state->digest_buff_dma_addr,
+			      digestsize, NS_BIT, 0);
+		ssi_set_hash_endianity(ctx->hash_mode, &desc[idx]);
+		set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+		set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
 		idx++;
 
 		/* Loading hash OPAD xor key state */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->opad_digest_dma_addr, ctx->inter_digestsize, NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_din_type(&desc[idx], DMA_DLLI, state->opad_digest_dma_addr,
+			     ctx->inter_digestsize, NS_BIT);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 		idx++;
 
 		/* Load the hash current length */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_DIN_SRAM(&desc[idx], ssi_ahash_get_initial_digest_len_sram_addr(ctx->drvdata, ctx->hash_mode), HASH_LEN_SIZE);
-		HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_din_sram(&desc[idx],
+			     ssi_ahash_get_initial_digest_len_sram_addr(
+ctx->drvdata, ctx->hash_mode), HASH_LEN_SIZE);
+		set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 		idx++;
 
 		/* Memory Barrier: wait for IPAD/OPAD axi write to complete */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-		HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+		hw_desc_init(&desc[idx]);
+		set_din_no_dma(&desc[idx], 0, 0xfffff0);
+		set_dout_no_dma(&desc[idx], 0, 0, 1);
 		idx++;
 
 		/* Perform HASH update on last digest */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, digestsize, NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr,
+			     digestsize, NS_BIT);
+		set_flow_mode(&desc[idx], DIN_HASH);
 		idx++;
 	}
 
 	/* Get final MAC result */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_result_dma_addr, digestsize, NS_BIT, async_req? 1:0); /*TODO*/
-	if (async_req) {
-		HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
-	}
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	ssi_set_hash_endianity(ctx->hash_mode,&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode); 
+	hw_desc_init(&desc[idx]);
+	/* TODO */
+	set_dout_dlli(&desc[idx], state->digest_result_dma_addr, digestsize,
+		      NS_BIT, (async_req ? 1 : 0));
+	if (async_req)
+		set_queue_last_ind(&desc[idx]);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_cipher_config1(&desc[idx], HASH_PADDING_DISABLED);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	ssi_set_hash_endianity(ctx->hash_mode, &desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
 	idx++;
 
 	if (async_req) {
@@ -828,22 +812,22 @@ static int ssi_hash_finup(struct ahash_req_ctx *state,
 	return rc;
 }
 
-static int ssi_hash_final(struct ahash_req_ctx *state, 
-			  struct ssi_hash_ctx *ctx, 
-			  unsigned int digestsize, 
-			  struct scatterlist *src, 
-			  unsigned int nbytes, 
-			  u8 *result, 
+static int ssi_hash_final(struct ahash_req_ctx *state,
+			  struct ssi_hash_ctx *ctx,
+			  unsigned int digestsize,
+			  struct scatterlist *src,
+			  unsigned int nbytes,
+			  u8 *result,
 			  void *async_req)
 {
 	struct device *dev = &ctx->drvdata->plat_dev->dev;
 	bool is_hmac = ctx->is_hmac;
 	struct ssi_crypto_req ssi_req = {};
-	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	struct cc_hw_desc desc[SSI_MAX_AHASH_SEQ_LEN];
 	int idx = 0;
 	int rc;
 
-	SSI_LOG_DEBUG("===== %s-final (%d) ====\n", is_hmac?"hmac":"hash", nbytes);
+	SSI_LOG_DEBUG("===== %s-final (%d) ====\n", is_hmac ? "hmac" : "hash", nbytes);
 
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
 
@@ -861,90 +845,95 @@ static int ssi_hash_final(struct ahash_req_ctx *state,
 		/* Setup DX request structure */
 		ssi_req.user_cb = (void *)ssi_hash_complete;
 		ssi_req.user_arg = async_req;
-#ifdef ENABLE_CYCLE_COUNT
-		ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
-#endif
 	}
 
 	/* Restore hash digest */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
+	set_din_type(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr,
+		     ctx->inter_digestsize, NS_BIT);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 	idx++;
 
 	/* Restore hash current length */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
+	set_cipher_config1(&desc[idx], HASH_PADDING_DISABLED);
+	set_din_type(&desc[idx], DMA_DLLI, state->digest_bytes_len_dma_addr,
+		     HASH_LEN_SIZE, NS_BIT);
+	set_flow_mode(&desc[idx], S_DIN_to_HASH);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 	idx++;
 
 	ssi_hash_create_data_desc(state, ctx, DIN_HASH, desc, false, &idx);
 
 	/* "DO-PAD" must be enabled only when writing current length to HW */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_DO(&desc[idx], DO_PAD);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT, 0);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE1);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	hw_desc_init(&desc[idx]);
+	set_cipher_do(&desc[idx], DO_PAD);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
+	set_dout_dlli(&desc[idx], state->digest_bytes_len_dma_addr,
+		      HASH_LEN_SIZE, NS_BIT, 0);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE1);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
 	idx++;
 
 	if (is_hmac) {
 		/* Store the hash digest result in the context */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, digestsize, NS_BIT, 0);
-		ssi_set_hash_endianity(ctx->hash_mode,&desc[idx]);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_dout_dlli(&desc[idx], state->digest_buff_dma_addr,
+			      digestsize, NS_BIT, 0);
+		ssi_set_hash_endianity(ctx->hash_mode, &desc[idx]);
+		set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+		set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
 		idx++;
 
 		/* Loading hash OPAD xor key state */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->opad_digest_dma_addr, ctx->inter_digestsize, NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_din_type(&desc[idx], DMA_DLLI, state->opad_digest_dma_addr,
+			     ctx->inter_digestsize, NS_BIT);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 		idx++;
 
 		/* Load the hash current length */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_DIN_SRAM(&desc[idx], ssi_ahash_get_initial_digest_len_sram_addr(ctx->drvdata, ctx->hash_mode), HASH_LEN_SIZE);
-		HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_din_sram(&desc[idx],
+			     ssi_ahash_get_initial_digest_len_sram_addr(
+ctx->drvdata, ctx->hash_mode), HASH_LEN_SIZE);
+		set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 		idx++;
 
 		/* Memory Barrier: wait for IPAD/OPAD axi write to complete */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-		HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+		hw_desc_init(&desc[idx]);
+		set_din_no_dma(&desc[idx], 0, 0xfffff0);
+		set_dout_no_dma(&desc[idx], 0, 0, 1);
 		idx++;
 
 		/* Perform HASH update on last digest */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, digestsize, NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr,
+			     digestsize, NS_BIT);
+		set_flow_mode(&desc[idx], DIN_HASH);
 		idx++;
 	}
 
 	/* Get final MAC result */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_result_dma_addr, digestsize, NS_BIT, async_req? 1:0);
-	if (async_req) {
-		HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
-	}
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	ssi_set_hash_endianity(ctx->hash_mode,&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	hw_desc_init(&desc[idx]);
+	set_dout_dlli(&desc[idx], state->digest_result_dma_addr, digestsize,
+		      NS_BIT, (async_req ? 1 : 0));
+	if (async_req)
+		set_queue_last_ind(&desc[idx]);
+	set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+	set_cipher_config1(&desc[idx], HASH_PADDING_DISABLED);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	ssi_set_hash_endianity(ctx->hash_mode, &desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
 	idx++;
 
 	if (async_req) {
@@ -972,33 +961,18 @@ static int ssi_hash_final(struct ahash_req_ctx *state,
 static int ssi_hash_init(struct ahash_req_ctx *state, struct ssi_hash_ctx *ctx)
 {
 	struct device *dev = &ctx->drvdata->plat_dev->dev;
-	state->xcbc_count = 0;	
 
-	CHECK_AND_RETURN_UPON_FIPS_ERROR();
-	ssi_hash_map_request(dev, state, ctx);
-
-	return 0;
-}
+	state->xcbc_count = 0;
 
-#ifdef EXPORT_FIXED
-static int ssi_hash_export(struct ssi_hash_ctx *ctx, void *out)
-{
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
-	memcpy(out, ctx, sizeof(struct ssi_hash_ctx));
-	return 0;
-}
+	ssi_hash_map_request(dev, state, ctx);
 
-static int ssi_hash_import(struct ssi_hash_ctx *ctx, const void *in)
-{
-	CHECK_AND_RETURN_UPON_FIPS_ERROR();
-	memcpy(ctx, in, sizeof(struct ssi_hash_ctx));
 	return 0;
 }
-#endif
 
 static int ssi_hash_setkey(void *hash,
-			   const u8 *key, 
-			   unsigned int keylen, 
+			   const u8 *key,
+			   unsigned int keylen,
 			   bool synchronize)
 {
 	unsigned int hmacPadConst[2] = { HMAC_IPAD_CONST, HMAC_OPAD_CONST };
@@ -1007,27 +981,22 @@ static int ssi_hash_setkey(void *hash,
 	int blocksize = 0;
 	int digestsize = 0;
 	int i, idx = 0, rc = 0;
-	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	struct cc_hw_desc desc[SSI_MAX_AHASH_SEQ_LEN];
 	ssi_sram_addr_t larval_addr;
 
 	 SSI_LOG_DEBUG("ssi_hash_setkey: start keylen: %d", keylen);
-	
+
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
-	if (synchronize) {
-		ctx = crypto_shash_ctx(((struct crypto_shash *)hash));
-		blocksize = crypto_tfm_alg_blocksize(&((struct crypto_shash *)hash)->base);
-		digestsize = crypto_shash_digestsize(((struct crypto_shash *)hash));
-	} else {
-		ctx = crypto_ahash_ctx(((struct crypto_ahash *)hash));
-		blocksize = crypto_tfm_alg_blocksize(&((struct crypto_ahash *)hash)->base);
-		digestsize = crypto_ahash_digestsize(((struct crypto_ahash *)hash));
-	}
-	
+	ctx = crypto_ahash_ctx(((struct crypto_ahash *)hash));
+	blocksize = crypto_tfm_alg_blocksize(&((struct crypto_ahash *)hash)->base);
+	digestsize = crypto_ahash_digestsize(((struct crypto_ahash *)hash));
+
 	larval_addr = ssi_ahash_get_larval_digest_sram_addr(
 					ctx->drvdata, ctx->hash_mode);
 
 	/* The keylen value distinguishes HASH in case keylen is ZERO bytes,
-	   any NON-ZERO value utilizes HMAC flow */
+	 * any NON-ZERO value utilizes HMAC flow
+	 */
 	ctx->key_params.keylen = keylen;
 	ctx->key_params.key_dma_addr = 0;
 	ctx->is_hmac = true;
@@ -1043,7 +1012,6 @@ static int ssi_hash_setkey(void *hash,
 				   " DMA failed\n", key, keylen);
 			return -ENOMEM;
 		}
-		SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->key_params.key_dma_addr, keylen);
 		SSI_LOG_DEBUG("mapping key-buffer: key_dma_addr=0x%llX "
 			     "keylen=%u\n",
 			     (unsigned long long)ctx->key_params.key_dma_addr,
@@ -1051,79 +1019,76 @@ static int ssi_hash_setkey(void *hash,
 
 		if (keylen > blocksize) {
 			/* Load hash initial state */
-			HW_DESC_INIT(&desc[idx]);
-			HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-			HW_DESC_SET_DIN_SRAM(&desc[idx], larval_addr,
-					ctx->inter_digestsize);
-			HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+			hw_desc_init(&desc[idx]);
+			set_cipher_mode(&desc[idx], ctx->hw_mode);
+			set_din_sram(&desc[idx], larval_addr,
+				     ctx->inter_digestsize);
+			set_flow_mode(&desc[idx], S_DIN_to_HASH);
+			set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 			idx++;
-	
+
 			/* Load the hash current length*/
-			HW_DESC_INIT(&desc[idx]);
-			HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-			HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
-			HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
-			HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+			hw_desc_init(&desc[idx]);
+			set_cipher_mode(&desc[idx], ctx->hw_mode);
+			set_din_const(&desc[idx], 0, HASH_LEN_SIZE);
+			set_cipher_config1(&desc[idx], HASH_PADDING_ENABLED);
+			set_flow_mode(&desc[idx], S_DIN_to_HASH);
+			set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 			idx++;
-	
-			HW_DESC_INIT(&desc[idx]);
-			HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-					     ctx->key_params.key_dma_addr, 
-					     keylen, NS_BIT);
-			HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+
+			hw_desc_init(&desc[idx]);
+			set_din_type(&desc[idx], DMA_DLLI,
+				     ctx->key_params.key_dma_addr, keylen,
+				     NS_BIT);
+			set_flow_mode(&desc[idx], DIN_HASH);
 			idx++;
-	
+
 			/* Get hashed key */
-			HW_DESC_INIT(&desc[idx]);
-			HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode); 
-			HW_DESC_SET_DOUT_DLLI(&desc[idx], ctx->opad_tmp_keys_dma_addr,
-					      digestsize, NS_BIT, 0);
-			HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-			HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
-			ssi_set_hash_endianity(ctx->hash_mode,&desc[idx]);
+			hw_desc_init(&desc[idx]);
+			set_cipher_mode(&desc[idx], ctx->hw_mode);
+			set_dout_dlli(&desc[idx], ctx->opad_tmp_keys_dma_addr,
+				      digestsize, NS_BIT, 0);
+			set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+			set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+			set_cipher_config1(&desc[idx], HASH_PADDING_DISABLED);
+			ssi_set_hash_endianity(ctx->hash_mode, &desc[idx]);
 			idx++;
-	
-			HW_DESC_INIT(&desc[idx]);
-			HW_DESC_SET_DIN_CONST(&desc[idx], 0, (blocksize - digestsize));
-			HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
-			HW_DESC_SET_DOUT_DLLI(&desc[idx], 
-					      (ctx->opad_tmp_keys_dma_addr + digestsize),
-					      (blocksize - digestsize),
-					      NS_BIT, 0);
+
+			hw_desc_init(&desc[idx]);
+			set_din_const(&desc[idx], 0, (blocksize - digestsize));
+			set_flow_mode(&desc[idx], BYPASS);
+			set_dout_dlli(&desc[idx], (ctx->opad_tmp_keys_dma_addr +
+						   digestsize),
+				      (blocksize - digestsize), NS_BIT, 0);
 			idx++;
 		} else {
-			HW_DESC_INIT(&desc[idx]);
-			HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-					     ctx->key_params.key_dma_addr, 
-					     keylen, NS_BIT);
-			HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
-			HW_DESC_SET_DOUT_DLLI(&desc[idx],
-					(ctx->opad_tmp_keys_dma_addr),
-					keylen, NS_BIT, 0);
+			hw_desc_init(&desc[idx]);
+			set_din_type(&desc[idx], DMA_DLLI,
+				     ctx->key_params.key_dma_addr, keylen,
+				     NS_BIT);
+			set_flow_mode(&desc[idx], BYPASS);
+			set_dout_dlli(&desc[idx], ctx->opad_tmp_keys_dma_addr,
+				      keylen, NS_BIT, 0);
 			idx++;
 
 			if ((blocksize - keylen) != 0) {
-				HW_DESC_INIT(&desc[idx]);
-				HW_DESC_SET_DIN_CONST(&desc[idx], 0, (blocksize - keylen));
-				HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
-				HW_DESC_SET_DOUT_DLLI(&desc[idx], 
-						      (ctx->opad_tmp_keys_dma_addr + keylen),
-						      (blocksize - keylen),
-						      NS_BIT, 0);
+				hw_desc_init(&desc[idx]);
+				set_din_const(&desc[idx], 0,
+					      (blocksize - keylen));
+				set_flow_mode(&desc[idx], BYPASS);
+				set_dout_dlli(&desc[idx],
+					      (ctx->opad_tmp_keys_dma_addr +
+					       keylen), (blocksize - keylen),
+					      NS_BIT, 0);
 				idx++;
 			}
 		}
 	} else {
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_CONST(&desc[idx], 0, blocksize);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
-		HW_DESC_SET_DOUT_DLLI(&desc[idx], 
-				      (ctx->opad_tmp_keys_dma_addr),
-				      blocksize,
-				      NS_BIT, 0);
+		hw_desc_init(&desc[idx]);
+		set_din_const(&desc[idx], 0, blocksize);
+		set_flow_mode(&desc[idx], BYPASS);
+		set_dout_dlli(&desc[idx], (ctx->opad_tmp_keys_dma_addr),
+			      blocksize, NS_BIT, 0);
 		idx++;
 	}
 
@@ -1136,71 +1101,59 @@ static int ssi_hash_setkey(void *hash,
 	/* calc derived HMAC key */
 	for (idx = 0, i = 0; i < 2; i++) {
 		/* Load hash initial state */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_DIN_SRAM(&desc[idx], larval_addr,
-				ctx->inter_digestsize);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_din_sram(&desc[idx], larval_addr, ctx->inter_digestsize);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
 		idx++;
 
 		/* Load the hash current length*/
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_din_const(&desc[idx], 0, HASH_LEN_SIZE);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 		idx++;
 
 		/* Prepare ipad key */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_XOR_VAL(&desc[idx], hmacPadConst[i]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+		hw_desc_init(&desc[idx]);
+		set_xor_val(&desc[idx], hmacPadConst[i]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_flow_mode(&desc[idx], S_DIN_to_HASH);
+		set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
 		idx++;
 
 		/* Perform HASH update */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
-				     ctx->opad_tmp_keys_dma_addr,
-				     blocksize, NS_BIT);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx],ctx->hw_mode);
-		HW_DESC_SET_XOR_ACTIVE(&desc[idx]);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI, ctx->opad_tmp_keys_dma_addr,
+			     blocksize, NS_BIT);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_xor_active(&desc[idx]);
+		set_flow_mode(&desc[idx], DIN_HASH);
 		idx++;
 
 		/* Get the IPAD/OPAD xor key (Note, IPAD is the initial digest of the first HASH "update" state) */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
 		if (i > 0) /* Not first iteration */
-			HW_DESC_SET_DOUT_DLLI(&desc[idx],
-					      ctx->opad_tmp_keys_dma_addr,
-					      ctx->inter_digestsize,
-					      NS_BIT, 0);
+			set_dout_dlli(&desc[idx], ctx->opad_tmp_keys_dma_addr,
+				      ctx->inter_digestsize, NS_BIT, 0);
 		else /* First iteration */
-			HW_DESC_SET_DOUT_DLLI(&desc[idx],
-					      ctx->digest_buff_dma_addr,
-					      ctx->inter_digestsize,
-					      NS_BIT, 0);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+			set_dout_dlli(&desc[idx], ctx->digest_buff_dma_addr,
+				      ctx->inter_digestsize, NS_BIT, 0);
+		set_flow_mode(&desc[idx], S_HASH_to_DOUT);
+		set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
 		idx++;
 	}
 
 	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 0);
 
 out:
-	if (rc != 0) {
-		if (synchronize) {
-			crypto_shash_set_flags((struct crypto_shash *)hash, CRYPTO_TFM_RES_BAD_KEY_LEN);
-		} else {
-			crypto_ahash_set_flags((struct crypto_ahash *)hash, CRYPTO_TFM_RES_BAD_KEY_LEN);
-		}
-	}
+	if (rc)
+		crypto_ahash_set_flags((struct crypto_ahash *)hash, CRYPTO_TFM_RES_BAD_KEY_LEN);
 
 	if (ctx->key_params.key_dma_addr) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->key_params.key_dma_addr);
 		dma_unmap_single(&ctx->drvdata->plat_dev->dev,
 				ctx->key_params.key_dma_addr,
 				ctx->key_params.keylen, DMA_TO_DEVICE);
@@ -1211,14 +1164,13 @@ out:
 	return rc;
 }
 
-
 static int ssi_xcbc_setkey(struct crypto_ahash *ahash,
 			const u8 *key, unsigned int keylen)
 {
 	struct ssi_crypto_req ssi_req = {};
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(ahash);
 	int idx = 0, rc = 0;
-	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	struct cc_hw_desc desc[SSI_MAX_AHASH_SEQ_LEN];
 
 	SSI_LOG_DEBUG("===== setkey (%d) ====\n", keylen);
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
@@ -1244,43 +1196,43 @@ static int ssi_xcbc_setkey(struct crypto_ahash *ahash,
 			   " DMA failed\n", key, keylen);
 		return -ENOMEM;
 	}
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->key_params.key_dma_addr, keylen);
 	SSI_LOG_DEBUG("mapping key-buffer: key_dma_addr=0x%llX "
 		     "keylen=%u\n",
 		     (unsigned long long)ctx->key_params.key_dma_addr,
 		     ctx->key_params.keylen);
-	
+
 	ctx->is_hmac = true;
 	/* 1. Load the AES key */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->key_params.key_dma_addr, keylen, NS_BIT);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_ECB);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], keylen);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, ctx->key_params.key_dma_addr,
+		     keylen, NS_BIT);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_ECB);
+	set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	set_key_size_aes(&desc[idx], keylen);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 	idx++;
 
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_CONST(&desc[idx], 0x01010101, CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], (ctx->opad_tmp_keys_dma_addr + 
-					   XCBC_MAC_K1_OFFSET), 
+	hw_desc_init(&desc[idx]);
+	set_din_const(&desc[idx], 0x01010101, CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[idx], DIN_AES_DOUT);
+	set_dout_dlli(&desc[idx], (ctx->opad_tmp_keys_dma_addr +
+					   XCBC_MAC_K1_OFFSET),
 			      CC_AES_128_BIT_KEY_SIZE, NS_BIT, 0);
 	idx++;
 
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_CONST(&desc[idx], 0x02020202, CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], (ctx->opad_tmp_keys_dma_addr + 
-					   XCBC_MAC_K2_OFFSET), 
+	hw_desc_init(&desc[idx]);
+	set_din_const(&desc[idx], 0x02020202, CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[idx], DIN_AES_DOUT);
+	set_dout_dlli(&desc[idx], (ctx->opad_tmp_keys_dma_addr +
+					   XCBC_MAC_K2_OFFSET),
 			      CC_AES_128_BIT_KEY_SIZE, NS_BIT, 0);
 	idx++;
 
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_CONST(&desc[idx], 0x03030303, CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], (ctx->opad_tmp_keys_dma_addr + 
+	hw_desc_init(&desc[idx]);
+	set_din_const(&desc[idx], 0x03030303, CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[idx], DIN_AES_DOUT);
+	set_dout_dlli(&desc[idx], (ctx->opad_tmp_keys_dma_addr +
 					   XCBC_MAC_K3_OFFSET),
 			       CC_AES_128_BIT_KEY_SIZE, NS_BIT, 0);
 	idx++;
@@ -1290,7 +1242,6 @@ static int ssi_xcbc_setkey(struct crypto_ahash *ahash,
 	if (rc != 0)
 		crypto_ahash_set_flags(ahash, CRYPTO_TFM_RES_BAD_KEY_LEN);
 
-	SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->key_params.key_dma_addr);
 	dma_unmap_single(&ctx->drvdata->plat_dev->dev,
 			ctx->key_params.key_dma_addr,
 			ctx->key_params.keylen, DMA_TO_DEVICE);
@@ -1300,12 +1251,13 @@ static int ssi_xcbc_setkey(struct crypto_ahash *ahash,
 
 	return rc;
 }
+
 #if SSI_CC_HAS_CMAC
 static int ssi_cmac_setkey(struct crypto_ahash *ahash,
 			const u8 *key, unsigned int keylen)
 {
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(ahash);
-	DECL_CYCLE_COUNT_RESOURCES;
+
 	SSI_LOG_DEBUG("===== setkey (%d) ====\n", keylen);
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
 
@@ -1323,25 +1275,20 @@ static int ssi_cmac_setkey(struct crypto_ahash *ahash,
 	ctx->key_params.keylen = keylen;
 
 	/* STAT_PHASE_1: Copy key to ctx */
-	START_CYCLE_COUNT();
-	
-	SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->opad_tmp_keys_dma_addr);
+
 	dma_sync_single_for_cpu(&ctx->drvdata->plat_dev->dev,
-				ctx->opad_tmp_keys_dma_addr, 
+				ctx->opad_tmp_keys_dma_addr,
 				keylen, DMA_TO_DEVICE);
 
 	memcpy(ctx->opad_tmp_keys_buff, key, keylen);
 	if (keylen == 24)
 		memset(ctx->opad_tmp_keys_buff + 24, 0, CC_AES_KEY_SIZE_MAX - 24);
-	
+
 	dma_sync_single_for_device(&ctx->drvdata->plat_dev->dev,
-				   ctx->opad_tmp_keys_dma_addr, 
+				   ctx->opad_tmp_keys_dma_addr,
 				   keylen, DMA_TO_DEVICE);
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->opad_tmp_keys_dma_addr, keylen);
-		
+
 	ctx->key_params.keylen = keylen;
-	
-	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_1);
 
 	return 0;
 }
@@ -1352,7 +1299,6 @@ static void ssi_hash_free_ctx(struct ssi_hash_ctx *ctx)
 	struct device *dev = &ctx->drvdata->plat_dev->dev;
 
 	if (ctx->digest_buff_dma_addr != 0) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->digest_buff_dma_addr);
 		dma_unmap_single(dev, ctx->digest_buff_dma_addr,
 				 sizeof(ctx->digest_buff), DMA_BIDIRECTIONAL);
 		SSI_LOG_DEBUG("Unmapped digest-buffer: "
@@ -1361,7 +1307,6 @@ static void ssi_hash_free_ctx(struct ssi_hash_ctx *ctx)
 		ctx->digest_buff_dma_addr = 0;
 	}
 	if (ctx->opad_tmp_keys_dma_addr != 0) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->opad_tmp_keys_dma_addr);
 		dma_unmap_single(dev, ctx->opad_tmp_keys_dma_addr,
 				 sizeof(ctx->opad_tmp_keys_buff),
 				 DMA_BIDIRECTIONAL);
@@ -1372,10 +1317,8 @@ static void ssi_hash_free_ctx(struct ssi_hash_ctx *ctx)
 	}
 
 	ctx->key_params.keylen = 0;
-
 }
 
-
 static int ssi_hash_alloc_ctx(struct ssi_hash_ctx *ctx)
 {
 	struct device *dev = &ctx->drvdata->plat_dev->dev;
@@ -1388,8 +1331,6 @@ static int ssi_hash_alloc_ctx(struct ssi_hash_ctx *ctx)
 			sizeof(ctx->digest_buff), ctx->digest_buff);
 		goto fail;
 	}
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->digest_buff_dma_addr,
-						sizeof(ctx->digest_buff));
 	SSI_LOG_DEBUG("Mapped digest %zu B at va=%pK to dma=0x%llX\n",
 		sizeof(ctx->digest_buff), ctx->digest_buff,
 		(unsigned long long)ctx->digest_buff_dma_addr);
@@ -1401,8 +1342,6 @@ static int ssi_hash_alloc_ctx(struct ssi_hash_ctx *ctx)
 			ctx->opad_tmp_keys_buff);
 		goto fail;
 	}
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->opad_tmp_keys_dma_addr,
-					sizeof(ctx->opad_tmp_keys_buff));
 	SSI_LOG_DEBUG("Mapped opad_tmp_keys %zu B at va=%pK to dma=0x%llX\n",
 		sizeof(ctx->opad_tmp_keys_buff), ctx->opad_tmp_keys_buff,
 		(unsigned long long)ctx->opad_tmp_keys_dma_addr);
@@ -1415,34 +1354,16 @@ fail:
 	return -ENOMEM;
 }
 
-static int ssi_shash_cra_init(struct crypto_tfm *tfm)
-{		
-	struct ssi_hash_ctx *ctx = crypto_tfm_ctx(tfm);
-	struct shash_alg * shash_alg = 
-		container_of(tfm->__crt_alg, struct shash_alg, base);
-	struct ssi_hash_alg *ssi_alg =
-			container_of(shash_alg, struct ssi_hash_alg, shash_alg);
-        	
-	CHECK_AND_RETURN_UPON_FIPS_ERROR();
-	ctx->hash_mode = ssi_alg->hash_mode;
-	ctx->hw_mode = ssi_alg->hw_mode;
-	ctx->inter_digestsize = ssi_alg->inter_digestsize;
-	ctx->drvdata = ssi_alg->drvdata;
-
-	return ssi_hash_alloc_ctx(ctx);
-}
-
 static int ssi_ahash_cra_init(struct crypto_tfm *tfm)
 {
 	struct ssi_hash_ctx *ctx = crypto_tfm_ctx(tfm);
-	struct hash_alg_common * hash_alg_common = 
+	struct hash_alg_common *hash_alg_common =
 		container_of(tfm->__crt_alg, struct hash_alg_common, base);
-	struct ahash_alg *ahash_alg = 
+	struct ahash_alg *ahash_alg =
 		container_of(hash_alg_common, struct ahash_alg, halg);
 	struct ssi_hash_alg *ssi_alg =
 			container_of(ahash_alg, struct ssi_hash_alg, ahash_alg);
 
-
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
 	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
 				sizeof(struct ahash_req_ctx));
@@ -1471,9 +1392,9 @@ static int ssi_mac_update(struct ahash_request *req)
 	struct device *dev = &ctx->drvdata->plat_dev->dev;
 	unsigned int block_size = crypto_tfm_alg_blocksize(&tfm->base);
 	struct ssi_crypto_req ssi_req = {};
-	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	struct cc_hw_desc desc[SSI_MAX_AHASH_SEQ_LEN];
 	int rc;
-	uint32_t idx = 0;
+	u32 idx = 0;
 
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
 	if (req->nbytes == 0) {
@@ -1494,29 +1415,26 @@ static int ssi_mac_update(struct ahash_request *req)
 		return -ENOMEM;
 	}
 
-	if (ctx->hw_mode == DRV_CIPHER_XCBC_MAC) {
+	if (ctx->hw_mode == DRV_CIPHER_XCBC_MAC)
 		ssi_hash_create_xcbc_setup(req, desc, &idx);
-	} else {
+	else
 		ssi_hash_create_cmac_setup(req, desc, &idx);
-	}
-	
+
 	ssi_hash_create_data_desc(state, ctx, DIN_AES_DOUT, desc, true, &idx);
 
 	/* store the hash digest result in context */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT, 1);
-	HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_AES_to_DOUT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	hw_desc_init(&desc[idx]);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
+	set_dout_dlli(&desc[idx], state->digest_buff_dma_addr,
+		      ctx->inter_digestsize, NS_BIT, 1);
+	set_queue_last_ind(&desc[idx]);
+	set_flow_mode(&desc[idx], S_AES_to_DOUT);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
 	idx++;
 
 	/* Setup DX request structure */
 	ssi_req.user_cb = (void *)ssi_hash_update_complete;
 	ssi_req.user_arg = (void *)req;
-#ifdef ENABLE_CYCLE_COUNT
-	ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
-#endif
 
 	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 1);
 	if (unlikely(rc != -EINPROGRESS)) {
@@ -1533,15 +1451,14 @@ static int ssi_mac_final(struct ahash_request *req)
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
 	struct device *dev = &ctx->drvdata->plat_dev->dev;
 	struct ssi_crypto_req ssi_req = {};
-	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	struct cc_hw_desc desc[SSI_MAX_AHASH_SEQ_LEN];
 	int idx = 0;
 	int rc = 0;
-	uint32_t keySize, keyLen;
-	uint32_t digestsize = crypto_ahash_digestsize(tfm);
+	u32 keySize, keyLen;
+	u32 digestsize = crypto_ahash_digestsize(tfm);
 
-	uint32_t rem_cnt = state->buff_index ? state->buff1_cnt :
+	u32 rem_cnt = state->buff_index ? state->buff1_cnt :
 			state->buff0_cnt;
-	
 
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
 	if (ctx->hw_mode == DRV_CIPHER_XCBC_MAC) {
@@ -1567,68 +1484,66 @@ static int ssi_mac_final(struct ahash_request *req)
 	/* Setup DX request structure */
 	ssi_req.user_cb = (void *)ssi_hash_complete;
 	ssi_req.user_arg = (void *)req;
-#ifdef ENABLE_CYCLE_COUNT
-	ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
-#endif
 
 	if (state->xcbc_count && (rem_cnt == 0)) {
 		/* Load key for ECB decryption */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_ECB);
-		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_DECRYPT);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-				     (ctx->opad_tmp_keys_dma_addr + 
-				      XCBC_MAC_K1_OFFSET),
-				    keySize, NS_BIT);
-		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], keyLen);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
-		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], DRV_CIPHER_ECB);
+		set_cipher_config0(&desc[idx], DRV_CRYPTO_DIRECTION_DECRYPT);
+		set_din_type(&desc[idx], DMA_DLLI,
+			     (ctx->opad_tmp_keys_dma_addr +
+			      XCBC_MAC_K1_OFFSET), keySize, NS_BIT);
+		set_key_size_aes(&desc[idx], keyLen);
+		set_flow_mode(&desc[idx], S_DIN_to_AES);
+		set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
 		idx++;
 
-
 		/* Initiate decryption of block state to previous block_state-XOR-M[n] */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT);
-		HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT,0);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr,
+			     CC_AES_BLOCK_SIZE, NS_BIT);
+		set_dout_dlli(&desc[idx], state->digest_buff_dma_addr,
+			      CC_AES_BLOCK_SIZE, NS_BIT, 0);
+		set_flow_mode(&desc[idx], DIN_AES_DOUT);
 		idx++;
 
 		/* Memory Barrier: wait for axi write to complete */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
-		HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+		hw_desc_init(&desc[idx]);
+		set_din_no_dma(&desc[idx], 0, 0xfffff0);
+		set_dout_no_dma(&desc[idx], 0, 0, 1);
 		idx++;
 	}
-	
-	if (ctx->hw_mode == DRV_CIPHER_XCBC_MAC) {
+
+	if (ctx->hw_mode == DRV_CIPHER_XCBC_MAC)
 		ssi_hash_create_xcbc_setup(req, desc, &idx);
-	} else {
+	else
 		ssi_hash_create_cmac_setup(req, desc, &idx);
-	}
 
 	if (state->xcbc_count == 0) {
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], keyLen);
-		HW_DESC_SET_CMAC_SIZE0_MODE(&desc[idx]);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_key_size_aes(&desc[idx], keyLen);
+		set_cmac_size0_mode(&desc[idx]);
+		set_flow_mode(&desc[idx], S_DIN_to_AES);
 		idx++;
 	} else if (rem_cnt > 0) {
 		ssi_hash_create_data_desc(state, ctx, DIN_AES_DOUT, desc, false, &idx);
 	} else {
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_CONST(&desc[idx], 0x00, CC_AES_BLOCK_SIZE);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+		hw_desc_init(&desc[idx]);
+		set_din_const(&desc[idx], 0x00, CC_AES_BLOCK_SIZE);
+		set_flow_mode(&desc[idx], DIN_AES_DOUT);
 		idx++;
 	}
-	
+
 	/* Get final MAC result */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_result_dma_addr, digestsize, NS_BIT, 1); /*TODO*/
-	HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_AES_to_DOUT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode); 
+	hw_desc_init(&desc[idx]);
+	/* TODO */
+	set_dout_dlli(&desc[idx], state->digest_result_dma_addr,
+		      digestsize, NS_BIT, 1);
+	set_queue_last_ind(&desc[idx]);
+	set_flow_mode(&desc[idx], S_AES_to_DOUT);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
 	idx++;
 
 	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 1);
@@ -1647,11 +1562,11 @@ static int ssi_mac_finup(struct ahash_request *req)
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
 	struct device *dev = &ctx->drvdata->plat_dev->dev;
 	struct ssi_crypto_req ssi_req = {};
-	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	struct cc_hw_desc desc[SSI_MAX_AHASH_SEQ_LEN];
 	int idx = 0;
 	int rc = 0;
-	uint32_t key_len = 0;
-	uint32_t digestsize = crypto_ahash_digestsize(tfm);
+	u32 key_len = 0;
+	u32 digestsize = crypto_ahash_digestsize(tfm);
 
 	SSI_LOG_DEBUG("===== finup xcbc(%d) ====\n", req->nbytes);
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
@@ -1659,7 +1574,7 @@ static int ssi_mac_finup(struct ahash_request *req)
 		SSI_LOG_DEBUG("No data to update. Call to fdx_mac_final \n");
 		return ssi_mac_final(req);
 	}
-	
+
 	if (unlikely(ssi_buffer_mgr_map_hash_request_final(ctx->drvdata, state, req->src, req->nbytes, 1) != 0)) {
 		SSI_LOG_ERR("map_ahash_request_final() failed\n");
 		return -ENOMEM;
@@ -1672,9 +1587,6 @@ static int ssi_mac_finup(struct ahash_request *req)
 	/* Setup DX request structure */
 	ssi_req.user_cb = (void *)ssi_hash_complete;
 	ssi_req.user_arg = (void *)req;
-#ifdef ENABLE_CYCLE_COUNT
-	ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
-#endif
 
 	if (ctx->hw_mode == DRV_CIPHER_XCBC_MAC) {
 		key_len = CC_AES_128_BIT_KEY_SIZE;
@@ -1685,23 +1597,25 @@ static int ssi_mac_finup(struct ahash_request *req)
 	}
 
 	if (req->nbytes == 0) {
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len);
-		HW_DESC_SET_CMAC_SIZE0_MODE(&desc[idx]);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_key_size_aes(&desc[idx], key_len);
+		set_cmac_size0_mode(&desc[idx]);
+		set_flow_mode(&desc[idx], S_DIN_to_AES);
 		idx++;
 	} else {
 		ssi_hash_create_data_desc(state, ctx, DIN_AES_DOUT, desc, false, &idx);
 	}
-	
+
 	/* Get final MAC result */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_result_dma_addr, digestsize, NS_BIT, 1); /*TODO*/
-	HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_AES_to_DOUT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode); 
+	hw_desc_init(&desc[idx]);
+	/* TODO */
+	set_dout_dlli(&desc[idx], state->digest_result_dma_addr,
+		      digestsize, NS_BIT, 1);
+	set_queue_last_ind(&desc[idx]);
+	set_flow_mode(&desc[idx], S_AES_to_DOUT);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
 	idx++;
 
 	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 1);
@@ -1719,16 +1633,16 @@ static int ssi_mac_digest(struct ahash_request *req)
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
 	struct device *dev = &ctx->drvdata->plat_dev->dev;
-	uint32_t digestsize = crypto_ahash_digestsize(tfm);
+	u32 digestsize = crypto_ahash_digestsize(tfm);
 	struct ssi_crypto_req ssi_req = {};
-	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
-	uint32_t keyLen;
+	struct cc_hw_desc desc[SSI_MAX_AHASH_SEQ_LEN];
+	u32 keyLen;
 	int idx = 0;
 	int rc;
 
 	SSI_LOG_DEBUG("===== -digest mac (%d) ====\n",  req->nbytes);
 	CHECK_AND_RETURN_UPON_FIPS_ERROR();
-	
+
 	if (unlikely(ssi_hash_map_request(dev, state, ctx) != 0)) {
 		SSI_LOG_ERR("map_ahash_source() failed\n");
 		return -ENOMEM;
@@ -1742,15 +1656,11 @@ static int ssi_mac_digest(struct ahash_request *req)
 		SSI_LOG_ERR("map_ahash_request_final() failed\n");
 		return -ENOMEM;
 	}
-	
+
 	/* Setup DX request structure */
 	ssi_req.user_cb = (void *)ssi_hash_digest_complete;
 	ssi_req.user_arg = (void *)req;
-#ifdef ENABLE_CYCLE_COUNT
-	ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
-#endif
 
-	
 	if (ctx->hw_mode == DRV_CIPHER_XCBC_MAC) {
 		keyLen = CC_AES_128_BIT_KEY_SIZE;
 		ssi_hash_create_xcbc_setup(req, desc, &idx);
@@ -1760,24 +1670,25 @@ static int ssi_mac_digest(struct ahash_request *req)
 	}
 
 	if (req->nbytes == 0) {
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
-		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], keyLen);
-		HW_DESC_SET_CMAC_SIZE0_MODE(&desc[idx]);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+		hw_desc_init(&desc[idx]);
+		set_cipher_mode(&desc[idx], ctx->hw_mode);
+		set_key_size_aes(&desc[idx], keyLen);
+		set_cmac_size0_mode(&desc[idx]);
+		set_flow_mode(&desc[idx], S_DIN_to_AES);
 		idx++;
 	} else {
 		ssi_hash_create_data_desc(state, ctx, DIN_AES_DOUT, desc, false, &idx);
 	}
-	
+
 	/* Get final MAC result */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_result_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT,1);
-	HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_AES_to_DOUT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx],DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode); 
+	hw_desc_init(&desc[idx]);
+	set_dout_dlli(&desc[idx], state->digest_result_dma_addr,
+		      CC_AES_BLOCK_SIZE, NS_BIT, 1);
+	set_queue_last_ind(&desc[idx]);
+	set_flow_mode(&desc[idx], S_AES_to_DOUT);
+	set_setup_mode(&desc[idx], SETUP_WRITE_STATE0);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_cipher_mode(&desc[idx], ctx->hw_mode);
 	idx++;
 
 	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 1);
@@ -1790,108 +1701,14 @@ static int ssi_mac_digest(struct ahash_request *req)
 	return rc;
 }
 
-//shash wrap functions
-#ifdef SYNC_ALGS
-static int ssi_shash_digest(struct shash_desc *desc, 
-			    const u8 *data, unsigned int len, u8 *out)
-{
-	struct ahash_req_ctx *state = shash_desc_ctx(desc);
-	struct crypto_shash *tfm = desc->tfm;
-	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
-	uint32_t digestsize = crypto_shash_digestsize(tfm);
-	struct scatterlist src;
-
-	if (len == 0) {
-		return ssi_hash_digest(state, ctx, digestsize, NULL, 0, out, NULL);
-	}
-	
-	/* sg_init_one may crash when len is 0 (depends on kernel configuration) */
-	sg_init_one(&src, (const void *)data, len);
-		
-	return ssi_hash_digest(state, ctx, digestsize, &src, len, out, NULL);
-}
-
-static int ssi_shash_update(struct shash_desc *desc, 
-						const u8 *data, unsigned int len)
-{
-	struct ahash_req_ctx *state = shash_desc_ctx(desc);
-	struct crypto_shash *tfm = desc->tfm;
-	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
-	uint32_t blocksize = crypto_tfm_alg_blocksize(&tfm->base);
-	struct scatterlist src;
-
-	sg_init_one(&src, (const void *)data, len);
-	
-	return ssi_hash_update(state, ctx, blocksize, &src, len, NULL);
-}
-
-static int ssi_shash_finup(struct shash_desc *desc, 
-			   const u8 *data, unsigned int len, u8 *out)
-{
-	struct ahash_req_ctx *state = shash_desc_ctx(desc);
-	struct crypto_shash *tfm = desc->tfm;
-	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
-	uint32_t digestsize = crypto_shash_digestsize(tfm);
-	struct scatterlist src;
-	
-	sg_init_one(&src, (const void *)data, len);
-	
-	return ssi_hash_finup(state, ctx, digestsize, &src, len, out, NULL);
-}
-
-static int ssi_shash_final(struct shash_desc *desc, u8 *out)
-{
-	struct ahash_req_ctx *state = shash_desc_ctx(desc);
-	struct crypto_shash *tfm = desc->tfm;
-	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
-	uint32_t digestsize = crypto_shash_digestsize(tfm);
-		
-	return ssi_hash_final(state, ctx, digestsize, NULL, 0, out, NULL);
-}
-
-static int ssi_shash_init(struct shash_desc *desc)
-{
-	struct ahash_req_ctx *state = shash_desc_ctx(desc);
-	struct crypto_shash *tfm = desc->tfm;
-	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
-
-	return ssi_hash_init(state, ctx);
-}
-
-#ifdef EXPORT_FIXED
-static int ssi_shash_export(struct shash_desc *desc, void *out)
-{
-	struct crypto_shash *tfm = desc->tfm;
-	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
-
-	return ssi_hash_export(ctx, out);
-}
-
-static int ssi_shash_import(struct shash_desc *desc, const void *in)
-{
-	struct crypto_shash *tfm = desc->tfm;
-	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
-	
-	return ssi_hash_import(ctx, in);
-}
-#endif
-
-static int ssi_shash_setkey(struct crypto_shash *tfm, 
-			    const u8 *key, unsigned int keylen)
-{
-	return ssi_hash_setkey((void *) tfm, key, keylen, true);
-}
-
-#endif /* SYNC_ALGS */
-
 //ahash wrap functions
 static int ssi_ahash_digest(struct ahash_request *req)
 {
 	struct ahash_req_ctx *state = ahash_request_ctx(req);
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
-	uint32_t digestsize = crypto_ahash_digestsize(tfm);
-	
+	u32 digestsize = crypto_ahash_digestsize(tfm);
+
 	return ssi_hash_digest(state, ctx, digestsize, req->src, req->nbytes, req->result, (void *)req);
 }
 
@@ -1901,7 +1718,7 @@ static int ssi_ahash_update(struct ahash_request *req)
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
 	unsigned int block_size = crypto_tfm_alg_blocksize(&tfm->base);
-	
+
 	return ssi_hash_update(state, ctx, block_size, req->src, req->nbytes, (void *)req);
 }
 
@@ -1910,8 +1727,8 @@ static int ssi_ahash_finup(struct ahash_request *req)
 	struct ahash_req_ctx *state = ahash_request_ctx(req);
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
-	uint32_t digestsize = crypto_ahash_digestsize(tfm);
-	
+	u32 digestsize = crypto_ahash_digestsize(tfm);
+
 	return ssi_hash_finup(state, ctx, digestsize, req->src, req->nbytes, req->result, (void *)req);
 }
 
@@ -1920,8 +1737,8 @@ static int ssi_ahash_final(struct ahash_request *req)
 	struct ahash_req_ctx *state = ahash_request_ctx(req);
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
-	uint32_t digestsize = crypto_ahash_digestsize(tfm);
-	
+	u32 digestsize = crypto_ahash_digestsize(tfm);
+
 	return ssi_hash_final(state, ctx, digestsize, req->src, req->nbytes, req->result, (void *)req);
 }
 
@@ -1929,81 +1746,161 @@ static int ssi_ahash_init(struct ahash_request *req)
 {
 	struct ahash_req_ctx *state = ahash_request_ctx(req);
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
-	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);	
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
 	SSI_LOG_DEBUG("===== init (%d) ====\n", req->nbytes);
 
 	return ssi_hash_init(state, ctx);
 }
 
-#ifdef EXPORT_FIXED
 static int ssi_ahash_export(struct ahash_request *req, void *out)
 {
 	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(ahash);
-	
-	return ssi_hash_export(ctx, out);
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+	u8 *curr_buff = state->buff_index ? state->buff1 : state->buff0;
+	u32 curr_buff_cnt = state->buff_index ? state->buff1_cnt :
+				state->buff0_cnt;
+	const u32 tmp = CC_EXPORT_MAGIC;
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+
+	memcpy(out, &tmp, sizeof(u32));
+	out += sizeof(u32);
+
+	dma_sync_single_for_cpu(dev, state->digest_buff_dma_addr,
+				ctx->inter_digestsize, DMA_BIDIRECTIONAL);
+	memcpy(out, state->digest_buff, ctx->inter_digestsize);
+	out += ctx->inter_digestsize;
+
+	if (state->digest_bytes_len_dma_addr) {
+		dma_sync_single_for_cpu(dev, state->digest_bytes_len_dma_addr,
+					HASH_LEN_SIZE, DMA_BIDIRECTIONAL);
+		memcpy(out, state->digest_bytes_len, HASH_LEN_SIZE);
+	} else {
+		/* Poison the unused exported digest len field. */
+		memset(out, 0x5F, HASH_LEN_SIZE);
+	}
+	out += HASH_LEN_SIZE;
+
+	memcpy(out, &curr_buff_cnt, sizeof(u32));
+	out += sizeof(u32);
+
+	memcpy(out, curr_buff, curr_buff_cnt);
+
+	/* No sync for device ineeded since we did not change the data,
+	 * we only copy it
+	 */
+
+	return 0;
 }
 
 static int ssi_ahash_import(struct ahash_request *req, const void *in)
 {
 	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(ahash);
-	
-	return ssi_hash_import(ctx, in);
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+	u32 tmp;
+	int rc;
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+
+	memcpy(&tmp, in, sizeof(u32));
+	if (tmp != CC_EXPORT_MAGIC) {
+		rc = -EINVAL;
+		goto out;
+	}
+	in += sizeof(u32);
+
+	rc = ssi_hash_init(state, ctx);
+	if (rc)
+		goto out;
+
+	dma_sync_single_for_cpu(dev, state->digest_buff_dma_addr,
+				ctx->inter_digestsize, DMA_BIDIRECTIONAL);
+	memcpy(state->digest_buff, in, ctx->inter_digestsize);
+	in += ctx->inter_digestsize;
+
+	if (state->digest_bytes_len_dma_addr) {
+		dma_sync_single_for_cpu(dev, state->digest_bytes_len_dma_addr,
+					HASH_LEN_SIZE, DMA_BIDIRECTIONAL);
+		memcpy(state->digest_bytes_len, in, HASH_LEN_SIZE);
+	}
+	in += HASH_LEN_SIZE;
+
+	dma_sync_single_for_device(dev, state->digest_buff_dma_addr,
+				   ctx->inter_digestsize, DMA_BIDIRECTIONAL);
+
+	if (state->digest_bytes_len_dma_addr)
+		dma_sync_single_for_device(dev,
+					   state->digest_bytes_len_dma_addr,
+					   HASH_LEN_SIZE, DMA_BIDIRECTIONAL);
+
+	state->buff_index = 0;
+
+	/* Sanity check the data as much as possible */
+	memcpy(&tmp, in, sizeof(u32));
+	if (tmp > SSI_MAX_HASH_BLCK_SIZE) {
+		rc = -EINVAL;
+		goto out;
+	}
+	in += sizeof(u32);
+
+	state->buff0_cnt = tmp;
+	memcpy(state->buff0, in, state->buff0_cnt);
+
+out:
+	return rc;
 }
-#endif
 
 static int ssi_ahash_setkey(struct crypto_ahash *ahash,
 			const u8 *key, unsigned int keylen)
-{	
-	return ssi_hash_setkey((void *) ahash, key, keylen, false);
+{
+	return ssi_hash_setkey((void *)ahash, key, keylen, false);
 }
 
 struct ssi_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];
+	char mac_name[CRYPTO_MAX_ALG_NAME];
+	char mac_driver_name[CRYPTO_MAX_ALG_NAME];
 	unsigned int blocksize;
 	bool synchronize;
-	union {
-		struct ahash_alg template_ahash;
-		struct shash_alg template_shash;
-	};	
+	struct ahash_alg template_ahash;
 	int hash_mode;
 	int hw_mode;
 	int inter_digestsize;
 	struct ssi_drvdata *drvdata;
 };
 
+#define CC_STATE_SIZE(_x) \
+	((_x) + HASH_LEN_SIZE + SSI_MAX_HASH_BLCK_SIZE + (2 * sizeof(u32)))
+
 /* hash descriptors */
 static struct ssi_hash_template driver_hash[] = {
 	//Asynchronize hash template
 	{
 		.name = "sha1",
 		.driver_name = "sha1-dx",
-		.hmac_name = "hmac(sha1)",
-		.hmac_driver_name = "hmac-sha1-dx",
+		.mac_name = "hmac(sha1)",
+		.mac_driver_name = "hmac-sha1-dx",
 		.blocksize = SHA1_BLOCK_SIZE,
 		.synchronize = false,
-		{
-			.template_ahash = {
-				.init = ssi_ahash_init,
-				.update = ssi_ahash_update,
-				.final = ssi_ahash_final,
-				.finup = ssi_ahash_finup,
-				.digest = ssi_ahash_digest,
-#ifdef EXPORT_FIXED
-				.export = ssi_ahash_export,
-				.import = ssi_ahash_import,
-#endif
-				.setkey = ssi_ahash_setkey,
-				.halg = {
-					.digestsize = SHA1_DIGEST_SIZE,
-					.statesize = sizeof(struct sha1_state),
-					},
-				},
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_ahash_update,
+			.final = ssi_ahash_final,
+			.finup = ssi_ahash_finup,
+			.digest = ssi_ahash_digest,
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+			.setkey = ssi_ahash_setkey,
+			.halg = {
+				.digestsize = SHA1_DIGEST_SIZE,
+				.statesize = CC_STATE_SIZE(SHA1_DIGEST_SIZE),
+			},
 		},
 		.hash_mode = DRV_HASH_SHA1,
 		.hw_mode = DRV_HASH_HW_SHA1,
@@ -2012,27 +1909,22 @@ static struct ssi_hash_template driver_hash[] = {
 	{
 		.name = "sha256",
 		.driver_name = "sha256-dx",
-		.hmac_name = "hmac(sha256)",
-		.hmac_driver_name = "hmac-sha256-dx",
+		.mac_name = "hmac(sha256)",
+		.mac_driver_name = "hmac-sha256-dx",
 		.blocksize = SHA256_BLOCK_SIZE,
-		.synchronize = false,
-		{
-			.template_ahash = {
-				.init = ssi_ahash_init,
-				.update = ssi_ahash_update,
-				.final = ssi_ahash_final,
-				.finup = ssi_ahash_finup,
-				.digest = ssi_ahash_digest,
-#ifdef EXPORT_FIXED
-				.export = ssi_ahash_export,
-				.import = ssi_ahash_import,
-#endif
-				.setkey = ssi_ahash_setkey,
-				.halg = {
-					.digestsize = SHA256_DIGEST_SIZE,
-					.statesize = sizeof(struct sha256_state),
-					},
-				},
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_ahash_update,
+			.final = ssi_ahash_final,
+			.finup = ssi_ahash_finup,
+			.digest = ssi_ahash_digest,
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+			.setkey = ssi_ahash_setkey,
+			.halg = {
+				.digestsize = SHA256_DIGEST_SIZE,
+				.statesize = CC_STATE_SIZE(SHA256_DIGEST_SIZE)
+			},
 		},
 		.hash_mode = DRV_HASH_SHA256,
 		.hw_mode = DRV_HASH_HW_SHA256,
@@ -2041,27 +1933,22 @@ static struct ssi_hash_template driver_hash[] = {
 	{
 		.name = "sha224",
 		.driver_name = "sha224-dx",
-		.hmac_name = "hmac(sha224)",
-		.hmac_driver_name = "hmac-sha224-dx",
+		.mac_name = "hmac(sha224)",
+		.mac_driver_name = "hmac-sha224-dx",
 		.blocksize = SHA224_BLOCK_SIZE,
-		.synchronize = false,
-		{
-			.template_ahash = {
-				.init = ssi_ahash_init,
-				.update = ssi_ahash_update,
-				.final = ssi_ahash_final,
-				.finup = ssi_ahash_finup,
-				.digest = ssi_ahash_digest,
-#ifdef EXPORT_FIXED
-				.export = ssi_ahash_export,
-				.import = ssi_ahash_import,
-#endif
-				.setkey = ssi_ahash_setkey,
-				.halg = {
-					.digestsize = SHA224_DIGEST_SIZE,
-					.statesize = sizeof(struct sha256_state),
-					},
-				},
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_ahash_update,
+			.final = ssi_ahash_final,
+			.finup = ssi_ahash_finup,
+			.digest = ssi_ahash_digest,
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+			.setkey = ssi_ahash_setkey,
+			.halg = {
+				.digestsize = SHA224_DIGEST_SIZE,
+				.statesize = CC_STATE_SIZE(SHA224_DIGEST_SIZE),
+			},
 		},
 		.hash_mode = DRV_HASH_SHA224,
 		.hw_mode = DRV_HASH_HW_SHA256,
@@ -2071,27 +1958,22 @@ static struct ssi_hash_template driver_hash[] = {
 	{
 		.name = "sha384",
 		.driver_name = "sha384-dx",
-		.hmac_name = "hmac(sha384)",
-		.hmac_driver_name = "hmac-sha384-dx",
+		.mac_name = "hmac(sha384)",
+		.mac_driver_name = "hmac-sha384-dx",
 		.blocksize = SHA384_BLOCK_SIZE,
-		.synchronize = false,
-		{
-			.template_ahash = {
-				.init = ssi_ahash_init,
-				.update = ssi_ahash_update,
-				.final = ssi_ahash_final,
-				.finup = ssi_ahash_finup,
-				.digest = ssi_ahash_digest,
-#ifdef EXPORT_FIXED
-				.export = ssi_ahash_export,
-				.import = ssi_ahash_import,
-#endif
-				.setkey = ssi_ahash_setkey,
-				.halg = {
-					.digestsize = SHA384_DIGEST_SIZE,
-					.statesize = sizeof(struct sha512_state),
-					},
-				},
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_ahash_update,
+			.final = ssi_ahash_final,
+			.finup = ssi_ahash_finup,
+			.digest = ssi_ahash_digest,
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+			.setkey = ssi_ahash_setkey,
+			.halg = {
+				.digestsize = SHA384_DIGEST_SIZE,
+				.statesize = CC_STATE_SIZE(SHA384_DIGEST_SIZE),
+			},
 		},
 		.hash_mode = DRV_HASH_SHA384,
 		.hw_mode = DRV_HASH_HW_SHA512,
@@ -2100,27 +1982,22 @@ static struct ssi_hash_template driver_hash[] = {
 	{
 		.name = "sha512",
 		.driver_name = "sha512-dx",
-		.hmac_name = "hmac(sha512)",
-		.hmac_driver_name = "hmac-sha512-dx",
+		.mac_name = "hmac(sha512)",
+		.mac_driver_name = "hmac-sha512-dx",
 		.blocksize = SHA512_BLOCK_SIZE,
-		.synchronize = false,
-		{
-			.template_ahash = {
-				.init = ssi_ahash_init,
-				.update = ssi_ahash_update,
-				.final = ssi_ahash_final,
-				.finup = ssi_ahash_finup,
-				.digest = ssi_ahash_digest,
-#ifdef EXPORT_FIXED
-				.export = ssi_ahash_export,
-				.import = ssi_ahash_import,
-#endif
-				.setkey = ssi_ahash_setkey,
-				.halg = {
-					.digestsize = SHA512_DIGEST_SIZE,
-					.statesize = sizeof(struct sha512_state),
-					},
-				},
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_ahash_update,
+			.final = ssi_ahash_final,
+			.finup = ssi_ahash_finup,
+			.digest = ssi_ahash_digest,
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+			.setkey = ssi_ahash_setkey,
+			.halg = {
+				.digestsize = SHA512_DIGEST_SIZE,
+				.statesize = CC_STATE_SIZE(SHA512_DIGEST_SIZE),
+			},
 		},
 		.hash_mode = DRV_HASH_SHA512,
 		.hw_mode = DRV_HASH_HW_SHA512,
@@ -2130,54 +2007,44 @@ static struct ssi_hash_template driver_hash[] = {
 	{
 		.name = "md5",
 		.driver_name = "md5-dx",
-		.hmac_name = "hmac(md5)",
-		.hmac_driver_name = "hmac-md5-dx",
+		.mac_name = "hmac(md5)",
+		.mac_driver_name = "hmac-md5-dx",
 		.blocksize = MD5_HMAC_BLOCK_SIZE,
-		.synchronize = false,
-		{
-			.template_ahash = {
-				.init = ssi_ahash_init,
-				.update = ssi_ahash_update,
-				.final = ssi_ahash_final,
-				.finup = ssi_ahash_finup,
-				.digest = ssi_ahash_digest,
-#ifdef EXPORT_FIXED
-				.export = ssi_ahash_export,
-				.import = ssi_ahash_import,
-#endif
-				.setkey = ssi_ahash_setkey,
-				.halg = {
-					.digestsize = MD5_DIGEST_SIZE,
-					.statesize = sizeof(struct md5_state),
-					},
-				},
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_ahash_update,
+			.final = ssi_ahash_final,
+			.finup = ssi_ahash_finup,
+			.digest = ssi_ahash_digest,
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+			.setkey = ssi_ahash_setkey,
+			.halg = {
+				.digestsize = MD5_DIGEST_SIZE,
+				.statesize = CC_STATE_SIZE(MD5_DIGEST_SIZE),
+			},
 		},
 		.hash_mode = DRV_HASH_MD5,
 		.hw_mode = DRV_HASH_HW_MD5,
 		.inter_digestsize = MD5_DIGEST_SIZE,
 	},
 	{
-		.name = "xcbc(aes)",
-		.driver_name = "xcbc-aes-dx",
+		.mac_name = "xcbc(aes)",
+		.mac_driver_name = "xcbc-aes-dx",
 		.blocksize = AES_BLOCK_SIZE,
-		.synchronize = false,
-		{
-			.template_ahash = {
-				.init = ssi_ahash_init,
-				.update = ssi_mac_update,
-				.final = ssi_mac_final,
-				.finup = ssi_mac_finup,
-				.digest = ssi_mac_digest,
-				.setkey = ssi_xcbc_setkey,
-#ifdef EXPORT_FIXED
-				.export = ssi_ahash_export,
-				.import = ssi_ahash_import,
-#endif
-				.halg = {
-					.digestsize = AES_BLOCK_SIZE,
-					.statesize = sizeof(struct aeshash_state),
-					},
-				},
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_mac_update,
+			.final = ssi_mac_final,
+			.finup = ssi_mac_finup,
+			.digest = ssi_mac_digest,
+			.setkey = ssi_xcbc_setkey,
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+			.halg = {
+				.digestsize = AES_BLOCK_SIZE,
+				.statesize = CC_STATE_SIZE(AES_BLOCK_SIZE),
+			},
 		},
 		.hash_mode = DRV_HASH_NULL,
 		.hw_mode = DRV_CIPHER_XCBC_MAC,
@@ -2185,34 +2052,29 @@ static struct ssi_hash_template driver_hash[] = {
 	},
 #if SSI_CC_HAS_CMAC
 	{
-		.name = "cmac(aes)",
-		.driver_name = "cmac-aes-dx",
+		.mac_name = "cmac(aes)",
+		.mac_driver_name = "cmac-aes-dx",
 		.blocksize = AES_BLOCK_SIZE,
-		.synchronize = false,
-		{
-			.template_ahash = {
-				.init = ssi_ahash_init,
-				.update = ssi_mac_update,
-				.final = ssi_mac_final,
-				.finup = ssi_mac_finup,
-				.digest = ssi_mac_digest,
-				.setkey = ssi_cmac_setkey,
-#ifdef EXPORT_FIXED
-				.export = ssi_ahash_export,
-				.import = ssi_ahash_import,
-#endif
-				.halg = {
-					.digestsize = AES_BLOCK_SIZE,
-					.statesize = sizeof(struct aeshash_state),
-					},
-				},
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_mac_update,
+			.final = ssi_mac_final,
+			.finup = ssi_mac_finup,
+			.digest = ssi_mac_digest,
+			.setkey = ssi_cmac_setkey,
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+			.halg = {
+				.digestsize = AES_BLOCK_SIZE,
+				.statesize = CC_STATE_SIZE(AES_BLOCK_SIZE),
+			},
 		},
 		.hash_mode = DRV_HASH_NULL,
 		.hw_mode = DRV_CIPHER_CMAC,
 		.inter_digestsize = AES_BLOCK_SIZE,
 	},
 #endif
-	
+
 };
 
 static struct ssi_hash_alg *
@@ -2220,6 +2082,7 @@ ssi_hash_create_alg(struct ssi_hash_template *template, bool keyed)
 {
 	struct ssi_hash_alg *t_crypto_alg;
 	struct crypto_alg *alg;
+	struct ahash_alg *halg;
 
 	t_crypto_alg = kzalloc(sizeof(struct ssi_hash_alg), GFP_KERNEL);
 	if (!t_crypto_alg) {
@@ -2227,27 +2090,17 @@ ssi_hash_create_alg(struct ssi_hash_template *template, bool keyed)
 		return ERR_PTR(-ENOMEM);
 	}
 
-	t_crypto_alg->synchronize = template->synchronize;
-	if (template->synchronize) {
-		struct shash_alg *halg;
-		t_crypto_alg->shash_alg = template->template_shash;
-		halg = &t_crypto_alg->shash_alg;
-		alg = &halg->base;
-		if (!keyed) halg->setkey = NULL;
-	} else {
-		struct ahash_alg *halg;
-		t_crypto_alg->ahash_alg = template->template_ahash;
-		halg = &t_crypto_alg->ahash_alg;
-		alg = &halg->halg.base;
-		if (!keyed) halg->setkey = NULL;
-	}
+	t_crypto_alg->ahash_alg = template->template_ahash;
+	halg = &t_crypto_alg->ahash_alg;
+	alg = &halg->halg.base;
 
 	if (keyed) {
 		snprintf(alg->cra_name, CRYPTO_MAX_ALG_NAME, "%s",
-			 template->hmac_name);
+			 template->mac_name);
 		snprintf(alg->cra_driver_name, CRYPTO_MAX_ALG_NAME, "%s",
-			 template->hmac_driver_name);
+			 template->mac_driver_name);
 	} else {
+		halg->setkey = NULL;
 		snprintf(alg->cra_name, CRYPTO_MAX_ALG_NAME, "%s",
 			 template->name);
 		snprintf(alg->cra_driver_name, CRYPTO_MAX_ALG_NAME, "%s",
@@ -2259,18 +2112,11 @@ ssi_hash_create_alg(struct ssi_hash_template *template, bool keyed)
 	alg->cra_blocksize = template->blocksize;
 	alg->cra_alignmask = 0;
 	alg->cra_exit = ssi_hash_cra_exit;
-	
-	if (template->synchronize) {
-		alg->cra_init = ssi_shash_cra_init;		
-		alg->cra_flags = CRYPTO_ALG_TYPE_SHASH |
-			CRYPTO_ALG_KERN_DRIVER_ONLY;
-		alg->cra_type = &crypto_shash_type;
-	} else {
-		alg->cra_init = ssi_ahash_cra_init;
-		alg->cra_flags = CRYPTO_ALG_ASYNC | CRYPTO_ALG_TYPE_AHASH |
+
+	alg->cra_init = ssi_ahash_cra_init;
+	alg->cra_flags = CRYPTO_ALG_ASYNC | CRYPTO_ALG_TYPE_AHASH |
 			CRYPTO_ALG_KERN_DRIVER_ONLY;
-		alg->cra_type = &crypto_ahash_type;
-	}
+	alg->cra_type = &crypto_ahash_type;
 
 	t_crypto_alg->hash_mode = template->hash_mode;
 	t_crypto_alg->hw_mode = template->hw_mode;
@@ -2284,7 +2130,7 @@ int ssi_hash_init_sram_digest_consts(struct ssi_drvdata *drvdata)
 	struct ssi_hash_handle *hash_handle = drvdata->hash_handle;
 	ssi_sram_addr_t sram_buff_ofs = hash_handle->digest_len_sram_addr;
 	unsigned int larval_seq_len = 0;
-	HwDesc_s larval_seq[CC_DIGEST_SIZE_MAX/sizeof(uint32_t)];
+	struct cc_hw_desc larval_seq[CC_DIGEST_SIZE_MAX / sizeof(u32)];
 	int rc = 0;
 #if (DX_DEV_SHA_MAX > 256)
 	int i;
@@ -2351,15 +2197,15 @@ int ssi_hash_init_sram_digest_consts(struct ssi_drvdata *drvdata)
 #if (DX_DEV_SHA_MAX > 256)
 	/* We are forced to swap each double-word larval before copying to sram */
 	for (i = 0; i < ARRAY_SIZE(sha384_init); i++) {
-		const uint32_t const0 = ((uint32_t *)((uint64_t *)&sha384_init[i]))[1];
-		const uint32_t const1 = ((uint32_t *)((uint64_t *)&sha384_init[i]))[0];
+		const u32 const0 = ((u32 *)((u64 *)&sha384_init[i]))[1];
+		const u32 const1 = ((u32 *)((u64 *)&sha384_init[i]))[0];
 
 		ssi_sram_mgr_const2sram_desc(&const0, sram_buff_ofs, 1,
 			larval_seq, &larval_seq_len);
-		sram_buff_ofs += sizeof(uint32_t);
+		sram_buff_ofs += sizeof(u32);
 		ssi_sram_mgr_const2sram_desc(&const1, sram_buff_ofs, 1,
 			larval_seq, &larval_seq_len);
-		sram_buff_ofs += sizeof(uint32_t);
+		sram_buff_ofs += sizeof(u32);
 	}
 	rc = send_request_init(drvdata, larval_seq, larval_seq_len);
 	if (unlikely(rc != 0)) {
@@ -2369,15 +2215,15 @@ int ssi_hash_init_sram_digest_consts(struct ssi_drvdata *drvdata)
 	larval_seq_len = 0;
 
 	for (i = 0; i < ARRAY_SIZE(sha512_init); i++) {
-		const uint32_t const0 = ((uint32_t *)((uint64_t *)&sha512_init[i]))[1];
-		const uint32_t const1 = ((uint32_t *)((uint64_t *)&sha512_init[i]))[0];
+		const u32 const0 = ((u32 *)((u64 *)&sha512_init[i]))[1];
+		const u32 const1 = ((u32 *)((u64 *)&sha512_init[i]))[0];
 
 		ssi_sram_mgr_const2sram_desc(&const0, sram_buff_ofs, 1,
 			larval_seq, &larval_seq_len);
-		sram_buff_ofs += sizeof(uint32_t);
+		sram_buff_ofs += sizeof(u32);
 		ssi_sram_mgr_const2sram_desc(&const1, sram_buff_ofs, 1,
 			larval_seq, &larval_seq_len);
-		sram_buff_ofs += sizeof(uint32_t);
+		sram_buff_ofs += sizeof(u32);
 	}
 	rc = send_request_init(drvdata, larval_seq, larval_seq_len);
 	if (unlikely(rc != 0)) {
@@ -2394,12 +2240,12 @@ int ssi_hash_alloc(struct ssi_drvdata *drvdata)
 {
 	struct ssi_hash_handle *hash_handle;
 	ssi_sram_addr_t sram_buff;
-	uint32_t sram_size_to_alloc;
+	u32 sram_size_to_alloc;
 	int rc = 0;
 	int alg;
 
 	hash_handle = kzalloc(sizeof(struct ssi_hash_handle), GFP_KERNEL);
-	if (hash_handle == NULL) {
+	if (!hash_handle) {
 		SSI_LOG_ERR("kzalloc failed to allocate %zu B\n",
 			sizeof(struct ssi_hash_handle));
 		rc = -ENOMEM;
@@ -2418,7 +2264,7 @@ int ssi_hash_alloc(struct ssi_drvdata *drvdata)
 			sizeof(sha1_init) +
 			sizeof(sha224_init) +
 			sizeof(sha256_init);
-				
+
 	sram_buff = ssi_sram_mgr_alloc(drvdata, sram_size_to_alloc);
 	if (sram_buff == NULL_SRAM_ADDR) {
 		SSI_LOG_ERR("SRAM pool exhausted\n");
@@ -2441,41 +2287,33 @@ int ssi_hash_alloc(struct ssi_drvdata *drvdata)
 	/* ahash registration */
 	for (alg = 0; alg < ARRAY_SIZE(driver_hash); alg++) {
 		struct ssi_hash_alg *t_alg;
-		
+		int hw_mode = driver_hash[alg].hw_mode;
+
 		/* register hmac version */
+		t_alg = ssi_hash_create_alg(&driver_hash[alg], true);
+		if (IS_ERR(t_alg)) {
+			rc = PTR_ERR(t_alg);
+			SSI_LOG_ERR("%s alg allocation failed\n",
+				    driver_hash[alg].driver_name);
+			goto fail;
+		}
+		t_alg->drvdata = drvdata;
 
-		if ((((struct ssi_hash_template)driver_hash[alg]).hw_mode != DRV_CIPHER_XCBC_MAC) &&
-			(((struct ssi_hash_template)driver_hash[alg]).hw_mode != DRV_CIPHER_CMAC)) {
-			t_alg = ssi_hash_create_alg(&driver_hash[alg], true);
-			if (IS_ERR(t_alg)) {
-				rc = PTR_ERR(t_alg);
-				SSI_LOG_ERR("%s alg allocation failed\n",
-					 driver_hash[alg].driver_name);
-				goto fail;
-			}
-			t_alg->drvdata = drvdata;
-	
-			if (t_alg->synchronize) {
-				rc = crypto_register_shash(&t_alg->shash_alg);
-				if (unlikely(rc != 0)) {
-					SSI_LOG_ERR("%s alg registration failed\n",
-						t_alg->shash_alg.base.cra_driver_name);
-					kfree(t_alg);
-					goto fail;
-				} else
-					list_add_tail(&t_alg->entry, &hash_handle->hash_list);
-			} else {
-				rc = crypto_register_ahash(&t_alg->ahash_alg);
-				if (unlikely(rc != 0)) {
-					SSI_LOG_ERR("%s alg registration failed\n",
-						t_alg->ahash_alg.halg.base.cra_driver_name);
-					kfree(t_alg);
-					goto fail;
-				} else
-					list_add_tail(&t_alg->entry, &hash_handle->hash_list);
-			}
+		rc = crypto_register_ahash(&t_alg->ahash_alg);
+		if (unlikely(rc)) {
+			SSI_LOG_ERR("%s alg registration failed\n",
+				    driver_hash[alg].driver_name);
+			kfree(t_alg);
+			goto fail;
+		} else {
+			list_add_tail(&t_alg->entry,
+				      &hash_handle->hash_list);
 		}
 
+		if ((hw_mode == DRV_CIPHER_XCBC_MAC) ||
+		    (hw_mode == DRV_CIPHER_CMAC))
+			continue;
+
 		/* register hash version */
 		t_alg = ssi_hash_create_alg(&driver_hash[alg], false);
 		if (IS_ERR(t_alg)) {
@@ -2485,26 +2323,15 @@ int ssi_hash_alloc(struct ssi_drvdata *drvdata)
 			goto fail;
 		}
 		t_alg->drvdata = drvdata;
-		
-		if (t_alg->synchronize) {
-			rc = crypto_register_shash(&t_alg->shash_alg);
-			if (unlikely(rc != 0)) {
-				SSI_LOG_ERR("%s alg registration failed\n",
-					t_alg->shash_alg.base.cra_driver_name);
-				kfree(t_alg);
-				goto fail;
-			} else
-				list_add_tail(&t_alg->entry, &hash_handle->hash_list);	
-				
+
+		rc = crypto_register_ahash(&t_alg->ahash_alg);
+		if (unlikely(rc)) {
+			SSI_LOG_ERR("%s alg registration failed\n",
+				    driver_hash[alg].driver_name);
+			kfree(t_alg);
+			goto fail;
 		} else {
-			rc = crypto_register_ahash(&t_alg->ahash_alg);
-			if (unlikely(rc != 0)) {
-				SSI_LOG_ERR("%s alg registration failed\n",
-					t_alg->ahash_alg.halg.base.cra_driver_name);
-				kfree(t_alg);
-				goto fail;
-			} else
-				list_add_tail(&t_alg->entry, &hash_handle->hash_list);
+			list_add_tail(&t_alg->entry, &hash_handle->hash_list);
 		}
 	}
 
@@ -2512,7 +2339,7 @@ int ssi_hash_alloc(struct ssi_drvdata *drvdata)
 
 fail:
 
-	if (drvdata->hash_handle != NULL) {
+	if (drvdata->hash_handle) {
 		kfree(drvdata->hash_handle);
 		drvdata->hash_handle = NULL;
 	}
@@ -2524,26 +2351,21 @@ int ssi_hash_free(struct ssi_drvdata *drvdata)
 	struct ssi_hash_alg *t_hash_alg, *hash_n;
 	struct ssi_hash_handle *hash_handle = drvdata->hash_handle;
 
-	if (hash_handle != NULL) {
-
+	if (hash_handle) {
 		list_for_each_entry_safe(t_hash_alg, hash_n, &hash_handle->hash_list, entry) {
-			if (t_hash_alg->synchronize) {
-				crypto_unregister_shash(&t_hash_alg->shash_alg);
-			} else {
-				crypto_unregister_ahash(&t_hash_alg->ahash_alg);
-			}
+			crypto_unregister_ahash(&t_hash_alg->ahash_alg);
 			list_del(&t_hash_alg->entry);
 			kfree(t_hash_alg);
 		}
-		
+
 		kfree(hash_handle);
 		drvdata->hash_handle = NULL;
 	}
 	return 0;
 }
 
-static void ssi_hash_create_xcbc_setup(struct ahash_request *areq, 
-				  HwDesc_s desc[],
+static void ssi_hash_create_xcbc_setup(struct ahash_request *areq,
+				  struct cc_hw_desc desc[],
 				  unsigned int *seq_size) {
 	unsigned int idx = *seq_size;
 	struct ahash_req_ctx *state = ahash_request_ctx(areq);
@@ -2551,55 +2373,56 @@ static void ssi_hash_create_xcbc_setup(struct ahash_request *areq,
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
 	/* Setup XCBC MAC K1 */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, (ctx->opad_tmp_keys_dma_addr 
-						    + XCBC_MAC_K1_OFFSET),
-			     CC_AES_128_BIT_KEY_SIZE, NS_BIT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, (ctx->opad_tmp_keys_dma_addr +
+					    XCBC_MAC_K1_OFFSET),
+		     CC_AES_128_BIT_KEY_SIZE, NS_BIT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_key_size_aes(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* Setup XCBC MAC K2 */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, (ctx->opad_tmp_keys_dma_addr 
-						    + XCBC_MAC_K2_OFFSET),
-			      CC_AES_128_BIT_KEY_SIZE, NS_BIT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, (ctx->opad_tmp_keys_dma_addr +
+					    XCBC_MAC_K2_OFFSET),
+		     CC_AES_128_BIT_KEY_SIZE, NS_BIT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE1);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_key_size_aes(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* Setup XCBC MAC K3 */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, (ctx->opad_tmp_keys_dma_addr 
-						    + XCBC_MAC_K3_OFFSET),
-			     CC_AES_128_BIT_KEY_SIZE, NS_BIT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE2);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, (ctx->opad_tmp_keys_dma_addr +
+					    XCBC_MAC_K3_OFFSET),
+		     CC_AES_128_BIT_KEY_SIZE, NS_BIT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE2);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_key_size_aes(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* Loading MAC state */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr,
+		     CC_AES_BLOCK_SIZE, NS_BIT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_key_size_aes(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 	*seq_size = idx;
 }
 
-static void ssi_hash_create_cmac_setup(struct ahash_request *areq, 
-				  HwDesc_s desc[],
+static void ssi_hash_create_cmac_setup(struct ahash_request *areq,
+				  struct cc_hw_desc desc[],
 				  unsigned int *seq_size)
 {
 	unsigned int idx = *seq_size;
@@ -2608,24 +2431,26 @@ static void ssi_hash_create_cmac_setup(struct ahash_request *areq,
 	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
 	/* Setup CMAC Key */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->opad_tmp_keys_dma_addr,
-		((ctx->key_params.keylen == 24) ? AES_MAX_KEY_SIZE : ctx->key_params.keylen), NS_BIT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CMAC);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->key_params.keylen);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, ctx->opad_tmp_keys_dma_addr,
+		     ((ctx->key_params.keylen == 24) ? AES_MAX_KEY_SIZE :
+		      ctx->key_params.keylen), NS_BIT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_KEY0);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CMAC);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_key_size_aes(&desc[idx], ctx->key_params.keylen);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 
 	/* Load MAC state */
-	HW_DESC_INIT(&desc[idx]);
-	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT);
-	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
-	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CMAC);
-	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->key_params.keylen);
-	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	hw_desc_init(&desc[idx]);
+	set_din_type(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr,
+		     CC_AES_BLOCK_SIZE, NS_BIT);
+	set_setup_mode(&desc[idx], SETUP_LOAD_STATE0);
+	set_cipher_mode(&desc[idx], DRV_CIPHER_CMAC);
+	set_cipher_config0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_key_size_aes(&desc[idx], ctx->key_params.keylen);
+	set_flow_mode(&desc[idx], S_DIN_to_AES);
 	idx++;
 	*seq_size = idx;
 }
@@ -2633,18 +2458,18 @@ static void ssi_hash_create_cmac_setup(struct ahash_request *areq,
 static void ssi_hash_create_data_desc(struct ahash_req_ctx *areq_ctx,
 				      struct ssi_hash_ctx *ctx,
 				      unsigned int flow_mode,
-				      HwDesc_s desc[],
-				      bool is_not_last_data, 
+				      struct cc_hw_desc desc[],
+				      bool is_not_last_data,
 				      unsigned int *seq_size)
 {
 	unsigned int idx = *seq_size;
 
 	if (likely(areq_ctx->data_dma_buf_type == SSI_DMA_BUF_DLLI)) {
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-				     sg_dma_address(areq_ctx->curr_sg), 
-				     areq_ctx->curr_sg->length, NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI,
+			     sg_dma_address(areq_ctx->curr_sg),
+			     areq_ctx->curr_sg->length, NS_BIT);
+		set_flow_mode(&desc[idx], flow_mode);
 		idx++;
 	} else {
 		if (areq_ctx->data_dma_buf_type == SSI_DMA_BUF_NULL) {
@@ -2653,42 +2478,38 @@ static void ssi_hash_create_data_desc(struct ahash_req_ctx *areq_ctx,
 			return;
 		}
 		/* bypass */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
-				     areq_ctx->mlli_params.mlli_dma_addr, 
-				     areq_ctx->mlli_params.mlli_len, 
-				     NS_BIT);
-		HW_DESC_SET_DOUT_SRAM(&desc[idx], 
-				      ctx->drvdata->mlli_sram_addr, 
-				      areq_ctx->mlli_params.mlli_len);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_DLLI,
+			     areq_ctx->mlli_params.mlli_dma_addr,
+			     areq_ctx->mlli_params.mlli_len, NS_BIT);
+		set_dout_sram(&desc[idx], ctx->drvdata->mlli_sram_addr,
+			      areq_ctx->mlli_params.mlli_len);
+		set_flow_mode(&desc[idx], BYPASS);
 		idx++;
 		/* process */
-		HW_DESC_INIT(&desc[idx]);
-		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_MLLI, 
-				     ctx->drvdata->mlli_sram_addr, 
-				     areq_ctx->mlli_nents,
-				     NS_BIT);
-		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		hw_desc_init(&desc[idx]);
+		set_din_type(&desc[idx], DMA_MLLI,
+			     ctx->drvdata->mlli_sram_addr,
+			     areq_ctx->mlli_nents, NS_BIT);
+		set_flow_mode(&desc[idx], flow_mode);
 		idx++;
 	}
-	if (is_not_last_data) {
-		HW_DESC_SET_DIN_NOT_LAST_INDICATION(&desc[idx-1]);
-	}
+	if (is_not_last_data)
+		set_din_not_last_indication(&desc[(idx - 1)]);
 	/* return updated desc sequence size */
 	*seq_size = idx;
 }
 
 /*!
- * Gets the address of the initial digest in SRAM 
+ * Gets the address of the initial digest in SRAM
  * according to the given hash mode
- * 
+ *
  * \param drvdata
  * \param mode The Hash mode. Supported modes: MD5/SHA1/SHA224/SHA256
- * 
- * \return uint32_t The address of the inital digest in SRAM
+ *
+ * \return u32 The address of the inital digest in SRAM
  */
-ssi_sram_addr_t ssi_ahash_get_larval_digest_sram_addr(void *drvdata, uint32_t mode)
+ssi_sram_addr_t ssi_ahash_get_larval_digest_sram_addr(void *drvdata, u32 mode)
 {
 	struct ssi_drvdata *_drvdata = (struct ssi_drvdata *)drvdata;
 	struct ssi_hash_handle *hash_handle = _drvdata->hash_handle;
@@ -2734,7 +2555,7 @@ ssi_sram_addr_t ssi_ahash_get_larval_digest_sram_addr(void *drvdata, uint32_t mo
 }
 
 ssi_sram_addr_t
-ssi_ahash_get_initial_digest_len_sram_addr(void *drvdata, uint32_t mode)
+ssi_ahash_get_initial_digest_len_sram_addr(void *drvdata, u32 mode)
 {
 	struct ssi_drvdata *_drvdata = (struct ssi_drvdata *)drvdata;
 	struct ssi_hash_handle *hash_handle = _drvdata->hash_handle;
diff --git a/drivers/staging/ccree/ssi_hash.h b/drivers/staging/ccree/ssi_hash.h
index a2b076d3af72..2400e389d65a 100644
--- a/drivers/staging/ccree/ssi_hash.h
+++ b/drivers/staging/ccree/ssi_hash.h
@@ -1,21 +1,21 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
 /* \file ssi_hash.h
-   ARM CryptoCell Hash Crypto API
+ * ARM CryptoCell Hash Crypto API
  */
 
 #ifndef __SSI_HASH_H__
@@ -39,6 +39,8 @@
 #define XCBC_MAC_K2_OFFSET 16
 #define XCBC_MAC_K3_OFFSET 32
 
+#define CC_EXPORT_MAGIC 0xC2EE1070U
+
 // this struct was taken from drivers/crypto/nx/nx-aes-xcbc.c and it is used for xcbc/cmac statesize
 struct aeshash_state {
 	u8 state[AES_BLOCK_SIZE];
@@ -48,27 +50,27 @@ struct aeshash_state {
 
 /* ahash state */
 struct ahash_req_ctx {
-	uint8_t* buff0;
-	uint8_t* buff1;
-	uint8_t* digest_result_buff;
+	u8 *buff0;
+	u8 *buff1;
+	u8 *digest_result_buff;
 	struct async_gen_req_ctx gen_ctx;
 	enum ssi_req_dma_buf_type data_dma_buf_type;
-	uint8_t *digest_buff;
-	uint8_t *opad_digest_buff;
-	uint8_t *digest_bytes_len;
+	u8 *digest_buff;
+	u8 *opad_digest_buff;
+	u8 *digest_bytes_len;
 	dma_addr_t opad_digest_dma_addr;
 	dma_addr_t digest_buff_dma_addr;
 	dma_addr_t digest_bytes_len_dma_addr;
 	dma_addr_t digest_result_dma_addr;
-	uint32_t buff0_cnt;
-	uint32_t buff1_cnt;
-	uint32_t buff_index;
-	uint32_t xcbc_count; /* count xcbc update operatations */
+	u32 buff0_cnt;
+	u32 buff1_cnt;
+	u32 buff_index;
+	u32 xcbc_count; /* count xcbc update operatations */
 	struct scatterlist buff_sg[2];
 	struct scatterlist *curr_sg;
-	uint32_t in_nents;
-	uint32_t mlli_nents;
-	struct mlli_params mlli_params;	
+	u32 in_nents;
+	u32 mlli_nents;
+	struct mlli_params mlli_params;
 };
 
 int ssi_hash_alloc(struct ssi_drvdata *drvdata);
@@ -77,25 +79,25 @@ int ssi_hash_free(struct ssi_drvdata *drvdata);
 
 /*!
  * Gets the initial digest length
- * 
- * \param drvdata 
+ *
+ * \param drvdata
  * \param mode The Hash mode. Supported modes: MD5/SHA1/SHA224/SHA256/SHA384/SHA512
- * 
- * \return uint32_t returns the address of the initial digest length in SRAM
+ *
+ * \return u32 returns the address of the initial digest length in SRAM
  */
 ssi_sram_addr_t
-ssi_ahash_get_initial_digest_len_sram_addr(void *drvdata, uint32_t mode);
+ssi_ahash_get_initial_digest_len_sram_addr(void *drvdata, u32 mode);
 
 /*!
- * Gets the address of the initial digest in SRAM 
+ * Gets the address of the initial digest in SRAM
  * according to the given hash mode
- * 
- * \param drvdata 
+ *
+ * \param drvdata
  * \param mode The Hash mode. Supported modes: MD5/SHA1/SHA224/SHA256/SHA384/SHA512
- * 
- * \return uint32_t The address of the inital digest in SRAM
+ *
+ * \return u32 The address of the inital digest in SRAM
  */
-ssi_sram_addr_t ssi_ahash_get_larval_digest_sram_addr(void *drvdata, uint32_t mode);
+ssi_sram_addr_t ssi_ahash_get_larval_digest_sram_addr(void *drvdata, u32 mode);
 
 #endif /*__SSI_HASH_H__*/
 
diff --git a/drivers/staging/ccree/ssi_ivgen.c b/drivers/staging/ccree/ssi_ivgen.c
index f16f4692f404..5ff3368c04d9 100644
--- a/drivers/staging/ccree/ssi_ivgen.c
+++ b/drivers/staging/ccree/ssi_ivgen.c
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -26,13 +26,14 @@
 /* The max. size of pool *MUST* be <= SRAM total size */
 #define SSI_IVPOOL_SIZE 1024
 /* The first 32B fraction of pool are dedicated to the
-   next encryption "key" & "IV" for pool regeneration */
+ * next encryption "key" & "IV" for pool regeneration
+ */
 #define SSI_IVPOOL_META_SIZE (CC_AES_IV_SIZE + AES_KEYSIZE_128)
 #define SSI_IVPOOL_GEN_SEQ_LEN	4
 
 /**
- * struct ssi_ivgen_ctx -IV pool generation context 
- * @pool:          the start address of the iv-pool resides in internal RAM 
+ * struct ssi_ivgen_ctx -IV pool generation context
+ * @pool:          the start address of the iv-pool resides in internal RAM
  * @ctr_key_dma:   address of pool's encryption key material in internal RAM
  * @ctr_iv_dma:    address of pool's counter iv in internal RAM
  * @next_iv_ofs:   the offset to the next available IV in pool
@@ -43,62 +44,62 @@ struct ssi_ivgen_ctx {
 	ssi_sram_addr_t pool;
 	ssi_sram_addr_t ctr_key;
 	ssi_sram_addr_t ctr_iv;
-	uint32_t next_iv_ofs;
-	uint8_t *pool_meta;
+	u32 next_iv_ofs;
+	u8 *pool_meta;
 	dma_addr_t pool_meta_dma;
 };
 
 /*!
- * Generates SSI_IVPOOL_SIZE of random bytes by 
+ * Generates SSI_IVPOOL_SIZE of random bytes by
  * encrypting 0's using AES128-CTR.
- * 
+ *
  * \param ivgen iv-pool context
  * \param iv_seq IN/OUT array to the descriptors sequence
- * \param iv_seq_len IN/OUT pointer to the sequence length 
+ * \param iv_seq_len IN/OUT pointer to the sequence length
  */
 static int ssi_ivgen_generate_pool(
 	struct ssi_ivgen_ctx *ivgen_ctx,
-	HwDesc_s iv_seq[],
+	struct cc_hw_desc iv_seq[],
 	unsigned int *iv_seq_len)
 {
 	unsigned int idx = *iv_seq_len;
 
-	if ( (*iv_seq_len + SSI_IVPOOL_GEN_SEQ_LEN) > SSI_IVPOOL_SEQ_LEN) {
+	if ((*iv_seq_len + SSI_IVPOOL_GEN_SEQ_LEN) > SSI_IVPOOL_SEQ_LEN) {
 		/* The sequence will be longer than allowed */
 		return -EINVAL;
 	}
 	/* Setup key */
-	HW_DESC_INIT(&iv_seq[idx]);
-	HW_DESC_SET_DIN_SRAM(&iv_seq[idx], ivgen_ctx->ctr_key, AES_KEYSIZE_128);
-	HW_DESC_SET_SETUP_MODE(&iv_seq[idx], SETUP_LOAD_KEY0);
-	HW_DESC_SET_CIPHER_CONFIG0(&iv_seq[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_FLOW_MODE(&iv_seq[idx], S_DIN_to_AES);
-	HW_DESC_SET_KEY_SIZE_AES(&iv_seq[idx], CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_CIPHER_MODE(&iv_seq[idx], DRV_CIPHER_CTR);
+	hw_desc_init(&iv_seq[idx]);
+	set_din_sram(&iv_seq[idx], ivgen_ctx->ctr_key, AES_KEYSIZE_128);
+	set_setup_mode(&iv_seq[idx], SETUP_LOAD_KEY0);
+	set_cipher_config0(&iv_seq[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_flow_mode(&iv_seq[idx], S_DIN_to_AES);
+	set_key_size_aes(&iv_seq[idx], CC_AES_128_BIT_KEY_SIZE);
+	set_cipher_mode(&iv_seq[idx], DRV_CIPHER_CTR);
 	idx++;
 
 	/* Setup cipher state */
-	HW_DESC_INIT(&iv_seq[idx]);
-	HW_DESC_SET_DIN_SRAM(&iv_seq[idx], ivgen_ctx->ctr_iv, CC_AES_IV_SIZE);
-	HW_DESC_SET_CIPHER_CONFIG0(&iv_seq[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
-	HW_DESC_SET_FLOW_MODE(&iv_seq[idx], S_DIN_to_AES);
-	HW_DESC_SET_SETUP_MODE(&iv_seq[idx], SETUP_LOAD_STATE1);
-	HW_DESC_SET_KEY_SIZE_AES(&iv_seq[idx], CC_AES_128_BIT_KEY_SIZE);
-	HW_DESC_SET_CIPHER_MODE(&iv_seq[idx], DRV_CIPHER_CTR);
+	hw_desc_init(&iv_seq[idx]);
+	set_din_sram(&iv_seq[idx], ivgen_ctx->ctr_iv, CC_AES_IV_SIZE);
+	set_cipher_config0(&iv_seq[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	set_flow_mode(&iv_seq[idx], S_DIN_to_AES);
+	set_setup_mode(&iv_seq[idx], SETUP_LOAD_STATE1);
+	set_key_size_aes(&iv_seq[idx], CC_AES_128_BIT_KEY_SIZE);
+	set_cipher_mode(&iv_seq[idx], DRV_CIPHER_CTR);
 	idx++;
 
 	/* Perform dummy encrypt to skip first block */
-	HW_DESC_INIT(&iv_seq[idx]);
-	HW_DESC_SET_DIN_CONST(&iv_seq[idx], 0, CC_AES_IV_SIZE);
-	HW_DESC_SET_DOUT_SRAM(&iv_seq[idx], ivgen_ctx->pool, CC_AES_IV_SIZE);
-	HW_DESC_SET_FLOW_MODE(&iv_seq[idx], DIN_AES_DOUT);
+	hw_desc_init(&iv_seq[idx]);
+	set_din_const(&iv_seq[idx], 0, CC_AES_IV_SIZE);
+	set_dout_sram(&iv_seq[idx], ivgen_ctx->pool, CC_AES_IV_SIZE);
+	set_flow_mode(&iv_seq[idx], DIN_AES_DOUT);
 	idx++;
 
 	/* Generate IV pool */
-	HW_DESC_INIT(&iv_seq[idx]);
-	HW_DESC_SET_DIN_CONST(&iv_seq[idx], 0, SSI_IVPOOL_SIZE);
-	HW_DESC_SET_DOUT_SRAM(&iv_seq[idx], ivgen_ctx->pool, SSI_IVPOOL_SIZE);
-	HW_DESC_SET_FLOW_MODE(&iv_seq[idx], DIN_AES_DOUT);
+	hw_desc_init(&iv_seq[idx]);
+	set_din_const(&iv_seq[idx], 0, SSI_IVPOOL_SIZE);
+	set_dout_sram(&iv_seq[idx], ivgen_ctx->pool, SSI_IVPOOL_SIZE);
+	set_flow_mode(&iv_seq[idx], DIN_AES_DOUT);
 	idx++;
 
 	*iv_seq_len = idx; /* Update sequence length */
@@ -110,17 +111,17 @@ static int ssi_ivgen_generate_pool(
 }
 
 /*!
- * Generates the initial pool in SRAM. 
- * This function should be invoked when resuming DX driver. 
- * 
- * \param drvdata 
- *  
+ * Generates the initial pool in SRAM.
+ * This function should be invoked when resuming DX driver.
+ *
+ * \param drvdata
+ *
  * \return int Zero for success, negative value otherwise.
  */
 int ssi_ivgen_init_sram_pool(struct ssi_drvdata *drvdata)
 {
 	struct ssi_ivgen_ctx *ivgen_ctx = drvdata->ivgen_handle;
-	HwDesc_s iv_seq[SSI_IVPOOL_SEQ_LEN];
+	struct cc_hw_desc iv_seq[SSI_IVPOOL_SEQ_LEN];
 	unsigned int iv_seq_len = 0;
 	int rc;
 
@@ -132,40 +133,38 @@ int ssi_ivgen_init_sram_pool(struct ssi_drvdata *drvdata)
 	ivgen_ctx->ctr_iv = ivgen_ctx->pool + AES_KEYSIZE_128;
 
 	/* Copy initial enc. key and IV to SRAM at a single descriptor */
-	HW_DESC_INIT(&iv_seq[iv_seq_len]);
-	HW_DESC_SET_DIN_TYPE(&iv_seq[iv_seq_len], DMA_DLLI,
-		ivgen_ctx->pool_meta_dma, SSI_IVPOOL_META_SIZE,
-		NS_BIT);
-	HW_DESC_SET_DOUT_SRAM(&iv_seq[iv_seq_len], ivgen_ctx->pool,
-		SSI_IVPOOL_META_SIZE);
-	HW_DESC_SET_FLOW_MODE(&iv_seq[iv_seq_len], BYPASS);
+	hw_desc_init(&iv_seq[iv_seq_len]);
+	set_din_type(&iv_seq[iv_seq_len], DMA_DLLI, ivgen_ctx->pool_meta_dma,
+		     SSI_IVPOOL_META_SIZE, NS_BIT);
+	set_dout_sram(&iv_seq[iv_seq_len], ivgen_ctx->pool,
+		      SSI_IVPOOL_META_SIZE);
+	set_flow_mode(&iv_seq[iv_seq_len], BYPASS);
 	iv_seq_len++;
 
 	/* Generate initial pool */
 	rc = ssi_ivgen_generate_pool(ivgen_ctx, iv_seq, &iv_seq_len);
-	if (unlikely(rc != 0)) {
+	if (unlikely(rc != 0))
 		return rc;
-	}
+
 	/* Fire-and-forget */
 	return send_request_init(drvdata, iv_seq, iv_seq_len);
 }
 
 /*!
  * Free iv-pool and ivgen context.
- *  
- * \param drvdata 
+ *
+ * \param drvdata
  */
 void ssi_ivgen_fini(struct ssi_drvdata *drvdata)
 {
 	struct ssi_ivgen_ctx *ivgen_ctx = drvdata->ivgen_handle;
 	struct device *device = &(drvdata->plat_dev->dev);
 
-	if (ivgen_ctx == NULL)
+	if (!ivgen_ctx)
 		return;
 
-	if (ivgen_ctx->pool_meta != NULL) {
+	if (ivgen_ctx->pool_meta) {
 		memset(ivgen_ctx->pool_meta, 0, SSI_IVPOOL_META_SIZE);
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(ivgen_ctx->pool_meta_dma);
 		dma_free_coherent(device, SSI_IVPOOL_META_SIZE,
 			ivgen_ctx->pool_meta, ivgen_ctx->pool_meta_dma);
 	}
@@ -177,11 +176,11 @@ void ssi_ivgen_fini(struct ssi_drvdata *drvdata)
 }
 
 /*!
- * Allocates iv-pool and maps resources. 
- * This function generates the first IV pool.  
- * 
+ * Allocates iv-pool and maps resources.
+ * This function generates the first IV pool.
+ *
  * \param drvdata Driver's private context
- * 
+ *
  * \return int Zero for success, negative value otherwise.
  */
 int ssi_ivgen_init(struct ssi_drvdata *drvdata)
@@ -209,8 +208,6 @@ int ssi_ivgen_init(struct ssi_drvdata *drvdata)
 		rc = -ENOMEM;
 		goto out;
 	}
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(ivgen_ctx->pool_meta_dma,
-							SSI_IVPOOL_META_SIZE);
 	/* Allocate IV pool in SRAM */
 	ivgen_ctx->pool = ssi_sram_mgr_alloc(drvdata, SSI_IVPOOL_SIZE);
 	if (ivgen_ctx->pool == NULL_SRAM_ADDR) {
@@ -228,22 +225,22 @@ out:
 
 /*!
  * Acquires 16 Bytes IV from the iv-pool
- * 
+ *
  * \param drvdata Driver private context
  * \param iv_out_dma Array of physical IV out addresses
  * \param iv_out_dma_len Length of iv_out_dma array (additional elements of iv_out_dma array are ignore)
- * \param iv_out_size May be 8 or 16 bytes long 
+ * \param iv_out_size May be 8 or 16 bytes long
  * \param iv_seq IN/OUT array to the descriptors sequence
- * \param iv_seq_len IN/OUT pointer to the sequence length 
- *  
- * \return int Zero for success, negative value otherwise. 
+ * \param iv_seq_len IN/OUT pointer to the sequence length
+ *
+ * \return int Zero for success, negative value otherwise.
  */
 int ssi_ivgen_getiv(
 	struct ssi_drvdata *drvdata,
 	dma_addr_t iv_out_dma[],
 	unsigned int iv_out_dma_len,
 	unsigned int iv_out_size,
-	HwDesc_s iv_seq[],
+	struct cc_hw_desc iv_seq[],
 	unsigned int *iv_seq_len)
 {
 	struct ssi_ivgen_ctx *ivgen_ctx = drvdata->ivgen_handle;
@@ -254,34 +251,35 @@ int ssi_ivgen_getiv(
 	    (iv_out_size != CTR_RFC3686_IV_SIZE)) {
 		return -EINVAL;
 	}
-	if ( (iv_out_dma_len + 1) > SSI_IVPOOL_SEQ_LEN) {
+	if ((iv_out_dma_len + 1) > SSI_IVPOOL_SEQ_LEN) {
 		/* The sequence will be longer than allowed */
 		return -EINVAL;
 	}
 
 	//check that number of generated IV is limited to max dma address iv buffer size
-	if ( iv_out_dma_len > SSI_MAX_IVGEN_DMA_ADDRESSES) {
+	if (iv_out_dma_len > SSI_MAX_IVGEN_DMA_ADDRESSES) {
 		/* The sequence will be longer than allowed */
 		return -EINVAL;
 	}
 
 	for (t = 0; t < iv_out_dma_len; t++) {
 		/* Acquire IV from pool */
-		HW_DESC_INIT(&iv_seq[idx]);
-		HW_DESC_SET_DIN_SRAM(&iv_seq[idx],
-			ivgen_ctx->pool + ivgen_ctx->next_iv_ofs,
-			iv_out_size);
-		HW_DESC_SET_DOUT_DLLI(&iv_seq[idx], iv_out_dma[t],
-			iv_out_size, NS_BIT, 0);
-		HW_DESC_SET_FLOW_MODE(&iv_seq[idx], BYPASS);
+		hw_desc_init(&iv_seq[idx]);
+		set_din_sram(&iv_seq[idx], (ivgen_ctx->pool +
+					    ivgen_ctx->next_iv_ofs),
+			     iv_out_size);
+		set_dout_dlli(&iv_seq[idx], iv_out_dma[t], iv_out_size,
+			      NS_BIT, 0);
+		set_flow_mode(&iv_seq[idx], BYPASS);
 		idx++;
 	}
 
 	/* Bypass operation is proceeded by crypto sequence, hence must
-	*  assure bypass-write-transaction by a memory barrier */
-	HW_DESC_INIT(&iv_seq[idx]);
-	HW_DESC_SET_DIN_NO_DMA(&iv_seq[idx], 0, 0xfffff0);
-	HW_DESC_SET_DOUT_NO_DMA(&iv_seq[idx], 0, 0, 1);
+	 *  assure bypass-write-transaction by a memory barrier
+	 */
+	hw_desc_init(&iv_seq[idx]);
+	set_din_no_dma(&iv_seq[idx], 0, 0xfffff0);
+	set_dout_no_dma(&iv_seq[idx], 0, 0, 1);
 	idx++;
 
 	*iv_seq_len = idx; /* update seq length */
@@ -298,4 +296,3 @@ int ssi_ivgen_getiv(
 	return 0;
 }
 
-
diff --git a/drivers/staging/ccree/ssi_ivgen.h b/drivers/staging/ccree/ssi_ivgen.h
index bc69cd8ca418..961aea411cb3 100644
--- a/drivers/staging/ccree/ssi_ivgen.h
+++ b/drivers/staging/ccree/ssi_ivgen.h
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -19,54 +19,53 @@
 
 #include "cc_hw_queue_defs.h"
 
-
 #define SSI_IVPOOL_SEQ_LEN 8
 
 /*!
- * Allocates iv-pool and maps resources. 
- * This function generates the first IV pool.  
- * 
+ * Allocates iv-pool and maps resources.
+ * This function generates the first IV pool.
+ *
  * \param drvdata Driver's private context
- * 
+ *
  * \return int Zero for success, negative value otherwise.
  */
 int ssi_ivgen_init(struct ssi_drvdata *drvdata);
 
 /*!
  * Free iv-pool and ivgen context.
- *  
- * \param drvdata 
+ *
+ * \param drvdata
  */
 void ssi_ivgen_fini(struct ssi_drvdata *drvdata);
 
 /*!
- * Generates the initial pool in SRAM. 
- * This function should be invoked when resuming DX driver. 
- * 
- * \param drvdata 
- *  
+ * Generates the initial pool in SRAM.
+ * This function should be invoked when resuming DX driver.
+ *
+ * \param drvdata
+ *
  * \return int Zero for success, negative value otherwise.
  */
 int ssi_ivgen_init_sram_pool(struct ssi_drvdata *drvdata);
 
 /*!
  * Acquires 16 Bytes IV from the iv-pool
- * 
+ *
  * \param drvdata Driver private context
  * \param iv_out_dma Array of physical IV out addresses
  * \param iv_out_dma_len Length of iv_out_dma array (additional elements of iv_out_dma array are ignore)
- * \param iv_out_size May be 8 or 16 bytes long 
+ * \param iv_out_size May be 8 or 16 bytes long
  * \param iv_seq IN/OUT array to the descriptors sequence
- * \param iv_seq_len IN/OUT pointer to the sequence length 
- *  
- * \return int Zero for success, negative value otherwise. 
+ * \param iv_seq_len IN/OUT pointer to the sequence length
+ *
+ * \return int Zero for success, negative value otherwise.
  */
 int ssi_ivgen_getiv(
 	struct ssi_drvdata *drvdata,
 	dma_addr_t iv_out_dma[],
 	unsigned int iv_out_dma_len,
 	unsigned int iv_out_size,
-	HwDesc_s iv_seq[],
+	struct cc_hw_desc iv_seq[],
 	unsigned int *iv_seq_len);
 
 #endif /*__SSI_IVGEN_H__*/
diff --git a/drivers/staging/ccree/ssi_pm.c b/drivers/staging/ccree/ssi_pm.c
index dd399f28a68c..52a8ed579177 100644
--- a/drivers/staging/ccree/ssi_pm.c
+++ b/drivers/staging/ccree/ssi_pm.c
@@ -1,20 +1,19 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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 "ssi_config.h"
 #include <linux/kernel.h>
 #include <linux/platform_device.h>
@@ -29,15 +28,12 @@
 #include "ssi_ivgen.h"
 #include "ssi_hash.h"
 #include "ssi_pm.h"
-#include "ssi_pm_ext.h"
-
 
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 
 #define POWER_DOWN_ENABLE 0x01
 #define POWER_DOWN_DISABLE 0x00
 
-
 int ssi_power_mgr_runtime_suspend(struct device *dev)
 {
 	struct ssi_drvdata *drvdata =
@@ -52,9 +48,7 @@ int ssi_power_mgr_runtime_suspend(struct device *dev)
 		return rc;
 	}
 	fini_cc_regs(drvdata);
-
-	/* Specific HW suspend code */
-	ssi_pm_ext_hw_suspend(dev);
+	cc_clk_off(drvdata);
 	return 0;
 }
 
@@ -66,24 +60,28 @@ int ssi_power_mgr_runtime_resume(struct device *dev)
 
 	SSI_LOG_DEBUG("ssi_power_mgr_runtime_resume , unset HOST_POWER_DOWN_EN\n");
 	WRITE_REGISTER(drvdata->cc_base + CC_REG_OFFSET(HOST_RGF, HOST_POWER_DOWN_EN), POWER_DOWN_DISABLE);
-	/* Specific HW resume code */
-	ssi_pm_ext_hw_resume(dev);
+
+	rc = cc_clk_on(drvdata);
+	if (rc) {
+		SSI_LOG_ERR("failed getting clock back on. We're toast.\n");
+		return rc;
+	}
 
 	rc = init_cc_regs(drvdata, false);
-	if (rc !=0) {
-		SSI_LOG_ERR("init_cc_regs (%x)\n",rc);
+	if (rc != 0) {
+		SSI_LOG_ERR("init_cc_regs (%x)\n", rc);
 		return rc;
 	}
 
 	rc = ssi_request_mgr_runtime_resume_queue(drvdata);
-	if (rc !=0) {
-		SSI_LOG_ERR("ssi_request_mgr_runtime_resume_queue (%x)\n",rc);
+	if (rc != 0) {
+		SSI_LOG_ERR("ssi_request_mgr_runtime_resume_queue (%x)\n", rc);
 		return rc;
 	}
 
 	/* must be after the queue resuming as it uses the HW queue*/
 	ssi_hash_init_sram_digest_consts(drvdata);
-	
+
 	ssi_ivgen_init_sram_pool(drvdata);
 	return 0;
 }
@@ -109,26 +107,22 @@ int ssi_power_mgr_runtime_put_suspend(struct device *dev)
 				(struct ssi_drvdata *)dev_get_drvdata(dev))) {
 		pm_runtime_mark_last_busy(dev);
 		rc = pm_runtime_put_autosuspend(dev);
-	}
-	else {
+	} else {
 		/* Something wrong happens*/
 		BUG();
 	}
 	return rc;
-
 }
 
 #endif
 
-
-
 int ssi_power_mgr_init(struct ssi_drvdata *drvdata)
 {
 	int rc = 0;
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 	struct platform_device *plat_dev = drvdata->plat_dev;
 	/* must be before the enabling to avoid resdundent suspending */
-	pm_runtime_set_autosuspend_delay(&plat_dev->dev,SSI_SUSPEND_TIMEOUT);
+	pm_runtime_set_autosuspend_delay(&plat_dev->dev, SSI_SUSPEND_TIMEOUT);
 	pm_runtime_use_autosuspend(&plat_dev->dev);
 	/* activate the PM module */
 	rc = pm_runtime_set_active(&plat_dev->dev);
@@ -142,7 +136,7 @@ int ssi_power_mgr_init(struct ssi_drvdata *drvdata)
 
 void ssi_power_mgr_fini(struct ssi_drvdata *drvdata)
 {
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 	struct platform_device *plat_dev = drvdata->plat_dev;
 
 	pm_runtime_disable(&plat_dev->dev);
diff --git a/drivers/staging/ccree/ssi_pm.h b/drivers/staging/ccree/ssi_pm.h
index 516fc3f445a8..63673f60d2d8 100644
--- a/drivers/staging/ccree/ssi_pm.h
+++ b/drivers/staging/ccree/ssi_pm.h
@@ -1,38 +1,35 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
 /* \file ssi_pm.h
-    */
+ */
 
 #ifndef __SSI_POWER_MGR_H__
 #define __SSI_POWER_MGR_H__
 
-
 #include "ssi_config.h"
 #include "ssi_driver.h"
 
-
 #define SSI_SUSPEND_TIMEOUT 3000
 
-
 int ssi_power_mgr_init(struct ssi_drvdata *drvdata);
 
 void ssi_power_mgr_fini(struct ssi_drvdata *drvdata);
 
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 int ssi_power_mgr_runtime_suspend(struct device *dev);
 
 int ssi_power_mgr_runtime_resume(struct device *dev);
diff --git a/drivers/staging/ccree/ssi_pm_ext.c b/drivers/staging/ccree/ssi_pm_ext.c
deleted file mode 100644
index f86bbab22073..000000000000
--- a/drivers/staging/ccree/ssi_pm_ext.c
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
- * This program is free software; you can 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 "ssi_config.h"
-#include <linux/kernel.h>
-#include <linux/platform_device.h>
-#include <linux/interrupt.h>
-#include <crypto/ctr.h>
-#include <linux/pm_runtime.h>
-#include "ssi_driver.h"
-#include "ssi_sram_mgr.h"
-#include "ssi_pm_ext.h"
-
-/*
-This function should suspend the HW (if possiable), It should be implemented by 
-the driver user. 
-The reference code clears the internal SRAM to imitate lose of state. 
-*/
-void ssi_pm_ext_hw_suspend(struct device *dev)
-{
-	struct ssi_drvdata *drvdata =
-		(struct ssi_drvdata *)dev_get_drvdata(dev);
-	unsigned int val;
-	void __iomem *cc_base = drvdata->cc_base;
-	unsigned int  sram_addr = 0;
-
-	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, SRAM_ADDR), sram_addr);
-
-	for (;sram_addr < SSI_CC_SRAM_SIZE ; sram_addr+=4) {
-		CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, SRAM_DATA), 0x0);
-
-		do {
-			val = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, SRAM_DATA_READY));
-		} while (!(val &0x1));
-	}
-}
-
-/*
-This function should resume the HW (if possiable).It should be implemented by 
-the driver user. 
-*/
-void ssi_pm_ext_hw_resume(struct device *dev)
-{
-	return;
-}
-
diff --git a/drivers/staging/ccree/ssi_pm_ext.h b/drivers/staging/ccree/ssi_pm_ext.h
deleted file mode 100644
index b4e2795de29e..000000000000
--- a/drivers/staging/ccree/ssi_pm_ext.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
- * This program is free software; you can 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/>.
- */
-
-/* \file ssi_pm_ext.h
-    */
-
-#ifndef __PM_EXT_H__
-#define __PM_EXT_H__
-
-
-#include "ssi_config.h"
-#include "ssi_driver.h"
-
-void ssi_pm_ext_hw_suspend(struct device *dev);
-
-void ssi_pm_ext_hw_resume(struct device *dev);
-
-
-#endif /*__POWER_MGR_H__*/
-
diff --git a/drivers/staging/ccree/ssi_request_mgr.c b/drivers/staging/ccree/ssi_request_mgr.c
index 8611adf3bb2e..46d9396f9ff9 100644
--- a/drivers/staging/ccree/ssi_request_mgr.c
+++ b/drivers/staging/ccree/ssi_request_mgr.c
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -35,91 +35,22 @@
 
 #define SSI_MAX_POLL_ITER	10
 
-#define AXIM_MON_BASE_OFFSET CC_REG_OFFSET(CRY_KERNEL, AXIM_MON_COMP)
-
-#ifdef CC_CYCLE_COUNT
-
-#define MONITOR_CNTR_BIT 0
-
-/**
- * Monitor descriptor. 
- * Used to measure CC performance. 
- */
-#define INIT_CC_MONITOR_DESC(desc_p) \
-do { \
-	HW_DESC_INIT(desc_p); \
-	HW_DESC_SET_DIN_MONITOR_CNTR(desc_p); \
-} while (0)
-
-/** 
- * Try adding monitor descriptor BEFORE enqueuing sequence.
- */
-#define CC_CYCLE_DESC_HEAD(cc_base_addr, desc_p, lock_p, is_monitored_p) \
-do { \
-	if (!test_and_set_bit(MONITOR_CNTR_BIT, (lock_p))) { \
-		enqueue_seq((cc_base_addr), (desc_p), 1); \
-		*(is_monitored_p) = true; \
-	} else { \
-		*(is_monitored_p) = false; \
-	} \
-} while (0)
-
-/**
- * If CC_CYCLE_DESC_HEAD was successfully added: 
- * 1. Add memory barrier descriptor to ensure last AXI transaction.  
- * 2. Add monitor descriptor to sequence tail AFTER enqueuing sequence.
- */
-#define CC_CYCLE_DESC_TAIL(cc_base_addr, desc_p, is_monitored) \
-do { \
-	if ((is_monitored) == true) { \
-		HwDesc_s barrier_desc; \
-		HW_DESC_INIT(&barrier_desc); \
-		HW_DESC_SET_DIN_NO_DMA(&barrier_desc, 0, 0xfffff0); \
-		HW_DESC_SET_DOUT_NO_DMA(&barrier_desc, 0, 0, 1); \
-		enqueue_seq((cc_base_addr), &barrier_desc, 1); \
-		enqueue_seq((cc_base_addr), (desc_p), 1); \
-	} \
-} while (0)
-
-/**
- * Try reading CC monitor counter value upon sequence complete. 
- * Can only succeed if the lock_p is taken by the owner of the given request.
- */
-#define END_CC_MONITOR_COUNT(cc_base_addr, stat_op_type, stat_phase, monitor_null_cycles, lock_p, is_monitored) \
-do { \
-	uint32_t elapsed_cycles; \
-	if ((is_monitored) == true) { \
-		elapsed_cycles = READ_REGISTER((cc_base_addr) + CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_MEASURE_CNTR)); \
-		clear_bit(MONITOR_CNTR_BIT, (lock_p)); \
-		if (elapsed_cycles > 0) \
-			update_cc_stat(stat_op_type, stat_phase, (elapsed_cycles - monitor_null_cycles)); \
-	} \
-} while (0)
-
-#else /*CC_CYCLE_COUNT*/
-
-#define INIT_CC_MONITOR_DESC(desc_p) do { } while (0)
-#define CC_CYCLE_DESC_HEAD(cc_base_addr, desc_p, lock_p, is_monitored_p) do { } while (0)
-#define CC_CYCLE_DESC_TAIL(cc_base_addr, desc_p, is_monitored) do { } while (0)
-#define END_CC_MONITOR_COUNT(cc_base_addr, stat_op_type, stat_phase, monitor_null_cycles, lock_p, is_monitored) do { } while (0)
-#endif /*CC_CYCLE_COUNT*/
-
-
 struct ssi_request_mgr_handle {
 	/* Request manager resources */
 	unsigned int hw_queue_size; /* HW capability */
 	unsigned int min_free_hw_slots;
 	unsigned int max_used_sw_slots;
 	struct ssi_crypto_req req_queue[MAX_REQUEST_QUEUE_SIZE];
-	uint32_t req_queue_head;
-	uint32_t req_queue_tail;
-	uint32_t axi_completed;
-	uint32_t q_free_slots;
+	u32 req_queue_head;
+	u32 req_queue_tail;
+	u32 axi_completed;
+	u32 q_free_slots;
 	spinlock_t hw_lock;
-	HwDesc_s compl_desc;
-	uint8_t *dummy_comp_buff;
+	struct cc_hw_desc compl_desc;
+	u8 *dummy_comp_buff;
 	dma_addr_t dummy_comp_buff_dma;
-	HwDesc_s monitor_desc;
+	struct cc_hw_desc monitor_desc;
+
 	volatile unsigned long monitor_lock;
 #ifdef COMP_IN_WQ
 	struct workqueue_struct *workq;
@@ -127,7 +58,7 @@ struct ssi_request_mgr_handle {
 #else
 	struct tasklet_struct comptask;
 #endif
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 	bool is_runtime_suspended;
 #endif
 };
@@ -141,18 +72,17 @@ void request_mgr_fini(struct ssi_drvdata *drvdata)
 {
 	struct ssi_request_mgr_handle *req_mgr_h = drvdata->request_mgr_handle;
 
-	if (req_mgr_h == NULL)
+	if (!req_mgr_h)
 		return; /* Not allocated */
 
 	if (req_mgr_h->dummy_comp_buff_dma != 0) {
-		SSI_RESTORE_DMA_ADDR_TO_48BIT(req_mgr_h->dummy_comp_buff_dma);
 		dma_free_coherent(&drvdata->plat_dev->dev,
-				  sizeof(uint32_t), req_mgr_h->dummy_comp_buff,
+				  sizeof(u32), req_mgr_h->dummy_comp_buff,
 				  req_mgr_h->dummy_comp_buff_dma);
 	}
 
 	SSI_LOG_DEBUG("max_used_hw_slots=%d\n", (req_mgr_h->hw_queue_size -
-						req_mgr_h->min_free_hw_slots) );
+						req_mgr_h->min_free_hw_slots));
 	SSI_LOG_DEBUG("max_used_sw_slots=%d\n", req_mgr_h->max_used_sw_slots);
 
 #ifdef COMP_IN_WQ
@@ -169,15 +99,11 @@ void request_mgr_fini(struct ssi_drvdata *drvdata)
 
 int request_mgr_init(struct ssi_drvdata *drvdata)
 {
-#ifdef CC_CYCLE_COUNT
-	HwDesc_s monitor_desc[2];
-	struct ssi_crypto_req monitor_req = {0};
-#endif
 	struct ssi_request_mgr_handle *req_mgr_h;
 	int rc = 0;
 
-	req_mgr_h = kzalloc(sizeof(struct ssi_request_mgr_handle),GFP_KERNEL);
-	if (req_mgr_h == NULL) {
+	req_mgr_h = kzalloc(sizeof(struct ssi_request_mgr_handle), GFP_KERNEL);
+	if (!req_mgr_h) {
 		rc = -ENOMEM;
 		goto req_mgr_init_err;
 	}
@@ -188,7 +114,7 @@ int request_mgr_init(struct ssi_drvdata *drvdata)
 #ifdef COMP_IN_WQ
 	SSI_LOG_DEBUG("Initializing completion workqueue\n");
 	req_mgr_h->workq = create_singlethread_workqueue("arm_cc7x_wq");
-	if (unlikely(req_mgr_h->workq == NULL)) {
+	if (unlikely(!req_mgr_h->workq)) {
 		SSI_LOG_ERR("Failed creating work queue\n");
 		rc = -ENOMEM;
 		goto req_mgr_init_err;
@@ -210,45 +136,23 @@ int request_mgr_init(struct ssi_drvdata *drvdata)
 	req_mgr_h->min_free_hw_slots = req_mgr_h->hw_queue_size;
 	req_mgr_h->max_used_sw_slots = 0;
 
-
 	/* Allocate DMA word for "dummy" completion descriptor use */
 	req_mgr_h->dummy_comp_buff = dma_alloc_coherent(&drvdata->plat_dev->dev,
-		sizeof(uint32_t), &req_mgr_h->dummy_comp_buff_dma, GFP_KERNEL);
+		sizeof(u32), &req_mgr_h->dummy_comp_buff_dma, GFP_KERNEL);
 	if (!req_mgr_h->dummy_comp_buff) {
 		SSI_LOG_ERR("Not enough memory to allocate DMA (%zu) dropped "
-			   "buffer\n", sizeof(uint32_t));
+			   "buffer\n", sizeof(u32));
 		rc = -ENOMEM;
 		goto req_mgr_init_err;
 	}
-	SSI_UPDATE_DMA_ADDR_TO_48BIT(req_mgr_h->dummy_comp_buff_dma,
-							     sizeof(uint32_t));
 
 	/* Init. "dummy" completion descriptor */
-	HW_DESC_INIT(&req_mgr_h->compl_desc);
-	HW_DESC_SET_DIN_CONST(&req_mgr_h->compl_desc, 0, sizeof(uint32_t));
-	HW_DESC_SET_DOUT_DLLI(&req_mgr_h->compl_desc,
-		req_mgr_h->dummy_comp_buff_dma,
-		sizeof(uint32_t), NS_BIT, 1);
-	HW_DESC_SET_FLOW_MODE(&req_mgr_h->compl_desc, BYPASS);
-	HW_DESC_SET_QUEUE_LAST_IND(&req_mgr_h->compl_desc);
-
-#ifdef CC_CYCLE_COUNT
-	/* For CC-HW cycle performance trace */
-	INIT_CC_MONITOR_DESC(&req_mgr_h->monitor_desc);
-	set_bit(MONITOR_CNTR_BIT, &req_mgr_h->monitor_lock);
-	monitor_desc[0] = req_mgr_h->monitor_desc;
-	monitor_desc[1] = req_mgr_h->monitor_desc;
-
-	rc = send_request(drvdata, &monitor_req, monitor_desc, 2, 0);
-	if (unlikely(rc != 0))
-		goto req_mgr_init_err;
-
-	drvdata->monitor_null_cycles = READ_REGISTER(drvdata->cc_base +
-		CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_MEASURE_CNTR));
-	SSI_LOG_ERR("Calibration time=0x%08x\n", drvdata->monitor_null_cycles);
-
-	clear_bit(MONITOR_CNTR_BIT, &req_mgr_h->monitor_lock);
-#endif
+	hw_desc_init(&req_mgr_h->compl_desc);
+	set_din_const(&req_mgr_h->compl_desc, 0, sizeof(u32));
+	set_dout_dlli(&req_mgr_h->compl_desc, req_mgr_h->dummy_comp_buff_dma,
+		      sizeof(u32), NS_BIT, 1);
+	set_flow_mode(&req_mgr_h->compl_desc, BYPASS);
+	set_queue_last_ind(&req_mgr_h->compl_desc);
 
 	return 0;
 
@@ -259,18 +163,18 @@ req_mgr_init_err:
 
 static inline void enqueue_seq(
 	void __iomem *cc_base,
-	HwDesc_s seq[], unsigned int seq_len)
+	struct cc_hw_desc seq[], unsigned int seq_len)
 {
 	int i;
 
 	for (i = 0; i < seq_len; i++) {
-		writel_relaxed(seq[i].word[0], (volatile void __iomem *)(cc_base+CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
-		writel_relaxed(seq[i].word[1], (volatile void __iomem *)(cc_base+CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
-		writel_relaxed(seq[i].word[2], (volatile void __iomem *)(cc_base+CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
-		writel_relaxed(seq[i].word[3], (volatile void __iomem *)(cc_base+CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
-		writel_relaxed(seq[i].word[4], (volatile void __iomem *)(cc_base+CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
+		writel_relaxed(seq[i].word[0], (volatile void __iomem *)(cc_base + CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
+		writel_relaxed(seq[i].word[1], (volatile void __iomem *)(cc_base + CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
+		writel_relaxed(seq[i].word[2], (volatile void __iomem *)(cc_base + CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
+		writel_relaxed(seq[i].word[3], (volatile void __iomem *)(cc_base + CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
+		writel_relaxed(seq[i].word[4], (volatile void __iomem *)(cc_base + CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
 		wmb();
-		writel_relaxed(seq[i].word[5], (volatile void __iomem *)(cc_base+CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
+		writel_relaxed(seq[i].word[5], (volatile void __iomem *)(cc_base + CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
 #ifdef DX_DUMP_DESCS
 		SSI_LOG_DEBUG("desc[%02d]: 0x%08X 0x%08X 0x%08X 0x%08X 0x%08X 0x%08X\n", i,
 			seq[i].word[0], seq[i].word[1], seq[i].word[2], seq[i].word[3], seq[i].word[4], seq[i].word[5]);
@@ -279,62 +183,63 @@ static inline void enqueue_seq(
 }
 
 /*!
- * Completion will take place if and only if user requested completion 
- * by setting "is_dout = 0" in send_request().  
- * 
- * \param dev 
+ * Completion will take place if and only if user requested completion
+ * by setting "is_dout = 0" in send_request().
+ *
+ * \param dev
  * \param dx_compl_h The completion event to signal
  */
 static void request_mgr_complete(struct device *dev, void *dx_compl_h, void __iomem *cc_base)
 {
 	struct completion *this_compl = dx_compl_h;
+
 	complete(this_compl);
 }
 
-
 static inline int request_mgr_queues_status_check(
 		struct ssi_request_mgr_handle *req_mgr_h,
 		void __iomem *cc_base,
 		unsigned int total_seq_len)
 {
 	unsigned long poll_queue;
-	
-	/* SW queue is checked only once as it will not 
-	   be chaned during the poll becasue the spinlock_bh 
-	   is held by the thread */
+
+	/* SW queue is checked only once as it will not
+	 * be chaned during the poll becasue the spinlock_bh
+	 * is held by the thread
+	 */
 	if (unlikely(((req_mgr_h->req_queue_head + 1) &
-		      (MAX_REQUEST_QUEUE_SIZE - 1)) == 
+		      (MAX_REQUEST_QUEUE_SIZE - 1)) ==
 		     req_mgr_h->req_queue_tail)) {
-		SSI_LOG_ERR("SW FIFO is full. req_queue_head=%d sw_fifo_len=%d\n", 
+		SSI_LOG_ERR("SW FIFO is full. req_queue_head=%d sw_fifo_len=%d\n",
 			   req_mgr_h->req_queue_head, MAX_REQUEST_QUEUE_SIZE);
 		return -EBUSY;
 	}
 
-	if ((likely(req_mgr_h->q_free_slots >= total_seq_len)) ) {
+	if ((likely(req_mgr_h->q_free_slots >= total_seq_len)))
 		return 0;
-	}
+
 	/* Wait for space in HW queue. Poll constant num of iterations. */
-	for (poll_queue =0; poll_queue < SSI_MAX_POLL_ITER ; poll_queue ++) {
-		req_mgr_h->q_free_slots = 
+	for (poll_queue = 0; poll_queue < SSI_MAX_POLL_ITER ; poll_queue++) {
+		req_mgr_h->q_free_slots =
 			CC_HAL_READ_REGISTER(
 				CC_REG_OFFSET(CRY_KERNEL,
 						 DSCRPTR_QUEUE_CONTENT));
-		if (unlikely(req_mgr_h->q_free_slots < 
+		if (unlikely(req_mgr_h->q_free_slots <
 						req_mgr_h->min_free_hw_slots)) {
 			req_mgr_h->min_free_hw_slots = req_mgr_h->q_free_slots;
 		}
 
-		if (likely (req_mgr_h->q_free_slots >= total_seq_len)) {
+		if (likely(req_mgr_h->q_free_slots >= total_seq_len)) {
 			/* If there is enough place return */
 			return 0;
 		}
 
-		SSI_LOG_DEBUG("HW FIFO is full. q_free_slots=%d total_seq_len=%d\n", 
+		SSI_LOG_DEBUG("HW FIFO is full. q_free_slots=%d total_seq_len=%d\n",
 			req_mgr_h->q_free_slots, total_seq_len);
 	}
 	/* No room in the HW queue try again later */
 	SSI_LOG_DEBUG("HW FIFO full, timeout. req_queue_head=%d "
-		   "sw_fifo_len=%d q_free_slots=%d total_seq_len=%d\n", 
+		   "sw_fifo_len=%d q_free_slots=%d total_seq_len=%d\n",
 		     req_mgr_h->req_queue_head,
 		   MAX_REQUEST_QUEUE_SIZE,
 		   req_mgr_h->q_free_slots,
@@ -344,38 +249,37 @@ static inline int request_mgr_queues_status_check(
 
 /*!
  * Enqueue caller request to crypto hardware.
- * 
- * \param drvdata 
+ *
+ * \param drvdata
  * \param ssi_req The request to enqueue
  * \param desc The crypto sequence
  * \param len The crypto sequence length
- * \param is_dout If "true": completion is handled by the caller 
- *      	  If "false": this function adds a dummy descriptor completion
- *      	  and waits upon completion signal.
- * 
+ * \param is_dout If "true": completion is handled by the caller
+ *	  If "false": this function adds a dummy descriptor completion
+ *	  and waits upon completion signal.
+ *
  * \return int Returns -EINPROGRESS if "is_dout=true"; "0" if "is_dout=false"
  */
 int send_request(
 	struct ssi_drvdata *drvdata, struct ssi_crypto_req *ssi_req,
-	HwDesc_s *desc, unsigned int len, bool is_dout)
+	struct cc_hw_desc *desc, unsigned int len, bool is_dout)
 {
 	void __iomem *cc_base = drvdata->cc_base;
 	struct ssi_request_mgr_handle *req_mgr_h = drvdata->request_mgr_handle;
 	unsigned int used_sw_slots;
 	unsigned int iv_seq_len = 0;
 	unsigned int total_seq_len = len; /*initial sequence length*/
-	HwDesc_s iv_seq[SSI_IVPOOL_SEQ_LEN];
+	struct cc_hw_desc iv_seq[SSI_IVPOOL_SEQ_LEN];
 	int rc;
 	unsigned int max_required_seq_len = (total_seq_len +
 					((ssi_req->ivgen_dma_addr_len == 0) ? 0 :
-					SSI_IVPOOL_SEQ_LEN ) +
-					((is_dout == 0 )? 1 : 0));
-	DECL_CYCLE_COUNT_RESOURCES;
+					SSI_IVPOOL_SEQ_LEN) +
+					((is_dout == 0) ? 1 : 0));
 
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 	rc = ssi_power_mgr_runtime_get(&drvdata->plat_dev->dev);
 	if (rc != 0) {
-		SSI_LOG_ERR("ssi_power_mgr_runtime_get returned %x\n",rc);
+		SSI_LOG_ERR("ssi_power_mgr_runtime_get returned %x\n", rc);
 		return rc;
 	}
 #endif
@@ -384,21 +288,23 @@ int send_request(
 		spin_lock_bh(&req_mgr_h->hw_lock);
 
 		/* Check if there is enough place in the SW/HW queues
-		in case iv gen add the max size and in case of no dout add 1 
-		for the internal completion descriptor */
+		 * in case iv gen add the max size and in case of no dout add 1
+		 * for the internal completion descriptor
+		 */
 		rc = request_mgr_queues_status_check(req_mgr_h,
 					       cc_base,
 					       max_required_seq_len);
-		if (likely(rc == 0 ))
+		if (likely(rc == 0))
 			/* There is enough place in the queue */
 			break;
 		/* something wrong release the spinlock*/
 		spin_unlock_bh(&req_mgr_h->hw_lock);
 
 		if (rc != -EAGAIN) {
-			/* Any error other than HW queue full 
-			   (SW queue is full) */
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+			/* Any error other than HW queue full
+			 * (SW queue is full)
+			 */
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 			ssi_power_mgr_runtime_put_suspend(&drvdata->plat_dev->dev);
 #endif
 			return rc;
@@ -409,7 +315,8 @@ int send_request(
 	} while (1);
 
 	/* Additional completion descriptor is needed incase caller did not
-	   enabled any DLLI/MLLI DOUT bit in the given sequence */
+	 * enabled any DLLI/MLLI DOUT bit in the given sequence
+	 */
 	if (!is_dout) {
 		init_completion(&ssi_req->seq_compl);
 		ssi_req->user_cb = request_mgr_complete;
@@ -432,7 +339,7 @@ int send_request(
 		if (unlikely(rc != 0)) {
 			SSI_LOG_ERR("Failed to generate IV (rc=%d)\n", rc);
 			spin_unlock_bh(&req_mgr_h->hw_lock);
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 			ssi_power_mgr_runtime_put_suspend(&drvdata->plat_dev->dev);
 #endif
 			return rc;
@@ -440,18 +347,13 @@ int send_request(
 
 		total_seq_len += iv_seq_len;
 	}
-	
-	used_sw_slots = ((req_mgr_h->req_queue_head - req_mgr_h->req_queue_tail) & (MAX_REQUEST_QUEUE_SIZE-1));
-	if (unlikely(used_sw_slots > req_mgr_h->max_used_sw_slots)) {
+
+	used_sw_slots = ((req_mgr_h->req_queue_head - req_mgr_h->req_queue_tail) & (MAX_REQUEST_QUEUE_SIZE - 1));
+	if (unlikely(used_sw_slots > req_mgr_h->max_used_sw_slots))
 		req_mgr_h->max_used_sw_slots = used_sw_slots;
-	}
-	
-	CC_CYCLE_DESC_HEAD(cc_base, &req_mgr_h->monitor_desc,
-			&req_mgr_h->monitor_lock, &ssi_req->is_monitored_p);
 
 	/* Enqueue request - must be locked with HW lock*/
 	req_mgr_h->req_queue[req_mgr_h->req_queue_head] = *ssi_req;
-	START_CYCLE_COUNT_AT(req_mgr_h->req_queue[req_mgr_h->req_queue_head].submit_cycle);
 	req_mgr_h->req_queue_head = (req_mgr_h->req_queue_head + 1) & (MAX_REQUEST_QUEUE_SIZE - 1);
 	/* TODO: Use circ_buf.h ? */
 
@@ -462,13 +364,9 @@ int send_request(
 #endif
 
 	/* STAT_PHASE_4: Push sequence */
-	START_CYCLE_COUNT();
 	enqueue_seq(cc_base, iv_seq, iv_seq_len);
 	enqueue_seq(cc_base, desc, len);
 	enqueue_seq(cc_base, &req_mgr_h->compl_desc, (is_dout ? 0 : 1));
-	END_CYCLE_COUNT(ssi_req->op_type, STAT_PHASE_4);
-
-	CC_CYCLE_DESC_TAIL(cc_base, &req_mgr_h->monitor_desc, ssi_req->is_monitored_p);
 
 	if (unlikely(req_mgr_h->q_free_slots < total_seq_len)) {
 		/*This means that there was a problem with the resume*/
@@ -481,28 +379,29 @@ int send_request(
 
 	if (!is_dout) {
 		/* Wait upon sequence completion.
-		*  Return "0" -Operation done successfully. */
-		return wait_for_completion_interruptible(&ssi_req->seq_compl);
+		 *  Return "0" -Operation done successfully.
+		 */
+		wait_for_completion(&ssi_req->seq_compl);
+		return 0;
 	} else {
 		/* Operation still in process */
 		return -EINPROGRESS;
 	}
 }
 
-
 /*!
  * Enqueue caller request to crypto hardware during init process.
  * assume this function is not called in middle of a flow,
  * since we set QUEUE_LAST_IND flag in the last descriptor.
- * 
- * \param drvdata 
+ *
+ * \param drvdata
  * \param desc The crypto sequence
  * \param len The crypto sequence length
- * 
+ *
  * \return int Returns "0" upon success
  */
 int send_request_init(
-	struct ssi_drvdata *drvdata, HwDesc_s *desc, unsigned int len)
+	struct ssi_drvdata *drvdata, struct cc_hw_desc *desc, unsigned int len)
 {
 	void __iomem *cc_base = drvdata->cc_base;
 	struct ssi_request_mgr_handle *req_mgr_h = drvdata->request_mgr_handle;
@@ -511,10 +410,10 @@ int send_request_init(
 
 	/* Wait for space in HW and SW FIFO. Poll for as much as FIFO_TIMEOUT. */
 	rc = request_mgr_queues_status_check(req_mgr_h, cc_base, total_seq_len);
-	if (unlikely(rc != 0 )) {
+	if (unlikely(rc != 0))
 		return rc;
-	}
-	HW_DESC_SET_QUEUE_LAST_IND(&desc[len-1]);
+
+	set_queue_last_ind(&desc[(len - 1)]);
 
 	enqueue_seq(cc_base, desc, len);
 
@@ -526,10 +425,9 @@ int send_request_init(
 	return 0;
 }
 
-
 void complete_request(struct ssi_drvdata *drvdata)
 {
-	struct ssi_request_mgr_handle *request_mgr_handle = 
+	struct ssi_request_mgr_handle *request_mgr_handle =
 						drvdata->request_mgr_handle;
 #ifdef COMP_IN_WQ
 	queue_delayed_work(request_mgr_handle->workq, &request_mgr_handle->compwork, 0);
@@ -552,14 +450,13 @@ static void proc_completions(struct ssi_drvdata *drvdata)
 {
 	struct ssi_crypto_req *ssi_req;
 	struct platform_device *plat_dev = drvdata->plat_dev;
-	struct ssi_request_mgr_handle * request_mgr_handle = 
+	struct ssi_request_mgr_handle *request_mgr_handle =
 						drvdata->request_mgr_handle;
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 	int rc = 0;
 #endif
-	DECL_CYCLE_COUNT_RESOURCES;
 
-	while(request_mgr_handle->axi_completed) {
+	while (request_mgr_handle->axi_completed) {
 		request_mgr_handle->axi_completed--;
 
 		/* Dequeue request */
@@ -569,9 +466,6 @@ static void proc_completions(struct ssi_drvdata *drvdata)
 		}
 
 		ssi_req = &request_mgr_handle->req_queue[request_mgr_handle->req_queue_tail];
-		END_CYCLE_COUNT_AT(ssi_req->submit_cycle, ssi_req->op_type, STAT_PHASE_5); /* Seq. Comp. */
-		END_CC_MONITOR_COUNT(drvdata->cc_base, ssi_req->op_type, STAT_PHASE_6,
-			drvdata->monitor_null_cycles, &request_mgr_handle->monitor_lock, ssi_req->is_monitored_p);
 
 #ifdef FLUSH_CACHE_ALL
 		flush_cache_all();
@@ -580,116 +474,109 @@ static void proc_completions(struct ssi_drvdata *drvdata)
 #ifdef COMPLETION_DELAY
 		/* Delay */
 		{
-			uint32_t axi_err;
+			u32 axi_err;
 			int i;
+
 			SSI_LOG_INFO("Delay\n");
-			for (i=0;i<1000000;i++) {
+			for (i = 0; i < 1000000; i++)
 				axi_err = READ_REGISTER(drvdata->cc_base + CC_REG_OFFSET(CRY_KERNEL, AXIM_MON_ERR));
-			}
 		}
 #endif /* COMPLETION_DELAY */
 
-		if (likely(ssi_req->user_cb != NULL)) {
-			START_CYCLE_COUNT();
+		if (likely(ssi_req->user_cb))
 			ssi_req->user_cb(&plat_dev->dev, ssi_req->user_arg, drvdata->cc_base);
-			END_CYCLE_COUNT(STAT_OP_TYPE_GENERIC, STAT_PHASE_3);
-		}
 		request_mgr_handle->req_queue_tail = (request_mgr_handle->req_queue_tail + 1) & (MAX_REQUEST_QUEUE_SIZE - 1);
 		SSI_LOG_DEBUG("Dequeue request tail=%u\n", request_mgr_handle->req_queue_tail);
 		SSI_LOG_DEBUG("Request completed. axi_completed=%d\n", request_mgr_handle->axi_completed);
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 		rc = ssi_power_mgr_runtime_put_suspend(&plat_dev->dev);
-		if (rc != 0) {
-			SSI_LOG_ERR("Failed to set runtime suspension %d\n",rc);
-		}
+		if (rc != 0)
+			SSI_LOG_ERR("Failed to set runtime suspension %d\n", rc);
 #endif
 	}
 }
 
+static inline u32 cc_axi_comp_count(void __iomem *cc_base)
+{
+	/* The CC_HAL_READ_REGISTER macro implictly requires and uses
+	 * a base MMIO register address variable named cc_base.
+	 */
+	return FIELD_GET(AXIM_MON_COMP_VALUE,
+			 CC_HAL_READ_REGISTER(AXIM_MON_BASE_OFFSET));
+}
+
 /* Deferred service handler, run as interrupt-fired tasklet */
 static void comp_handler(unsigned long devarg)
 {
 	struct ssi_drvdata *drvdata = (struct ssi_drvdata *)devarg;
 	void __iomem *cc_base = drvdata->cc_base;
-	struct ssi_request_mgr_handle * request_mgr_handle = 
+	struct ssi_request_mgr_handle *request_mgr_handle =
 						drvdata->request_mgr_handle;
 
-	uint32_t irq;
-
-	DECL_CYCLE_COUNT_RESOURCES;
-
-	START_CYCLE_COUNT();
+	u32 irq;
 
 	irq = (drvdata->irq & SSI_COMP_IRQ_MASK);
 
 	if (irq & SSI_COMP_IRQ_MASK) {
 		/* To avoid the interrupt from firing as we unmask it, we clear it now */
 		CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_ICR), SSI_COMP_IRQ_MASK);
-	
+
 		/* Avoid race with above clear: Test completion counter once more */
-		request_mgr_handle->axi_completed += CC_REG_FLD_GET(CRY_KERNEL, AXIM_MON_COMP, VALUE, 
-			CC_HAL_READ_REGISTER(AXIM_MON_BASE_OFFSET));
-	
-		/* ISR-to-Tasklet latency */
-		if (request_mgr_handle->axi_completed) {
-			/* Only if actually reflects ISR-to-completion-handling latency, i.e.,
-			   not duplicate as a result of interrupt after AXIM_MON_ERR clear, before end of loop */
-			END_CYCLE_COUNT_AT(drvdata->isr_exit_cycles, STAT_OP_TYPE_GENERIC, STAT_PHASE_1);
-		}
-	
+		request_mgr_handle->axi_completed +=
+				cc_axi_comp_count(cc_base);
+
 		while (request_mgr_handle->axi_completed) {
 			do {
 				proc_completions(drvdata);
-				/* At this point (after proc_completions()), request_mgr_handle->axi_completed is always 0.
-				   The following assignment was changed to = (previously was +=) to conform KW restrictions. */
-				request_mgr_handle->axi_completed = CC_REG_FLD_GET(CRY_KERNEL, AXIM_MON_COMP, VALUE, 
-					CC_HAL_READ_REGISTER(AXIM_MON_BASE_OFFSET));
+				/* At this point (after proc_completions()),
+				 * request_mgr_handle->axi_completed is 0.
+				 */
+				request_mgr_handle->axi_completed =
+						cc_axi_comp_count(cc_base);
 			} while (request_mgr_handle->axi_completed > 0);
-	
+
 			/* To avoid the interrupt from firing as we unmask it, we clear it now */
 			CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_ICR), SSI_COMP_IRQ_MASK);
-			
+
 			/* Avoid race with above clear: Test completion counter once more */
-			request_mgr_handle->axi_completed += CC_REG_FLD_GET(CRY_KERNEL, AXIM_MON_COMP, VALUE, 
-				CC_HAL_READ_REGISTER(AXIM_MON_BASE_OFFSET));
+			request_mgr_handle->axi_completed +=
+					cc_axi_comp_count(cc_base);
 		}
-	
 	}
 	/* after verifing that there is nothing to do, Unmask AXI completion interrupt */
-	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IMR), 
+	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IMR),
 		CC_HAL_READ_REGISTER(
 		CC_REG_OFFSET(HOST_RGF, HOST_IMR)) & ~irq);
-	END_CYCLE_COUNT(STAT_OP_TYPE_GENERIC, STAT_PHASE_2);
 }
 
 /*
-resume the queue configuration - no need to take the lock as this happens inside
-the spin lock protection
-*/
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+ * resume the queue configuration - no need to take the lock as this happens inside
+ * the spin lock protection
+ */
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 int ssi_request_mgr_runtime_resume_queue(struct ssi_drvdata *drvdata)
 {
-	struct ssi_request_mgr_handle * request_mgr_handle = drvdata->request_mgr_handle;
+	struct ssi_request_mgr_handle *request_mgr_handle = drvdata->request_mgr_handle;
 
 	spin_lock_bh(&request_mgr_handle->hw_lock);
 	request_mgr_handle->is_runtime_suspended = false;
 	spin_unlock_bh(&request_mgr_handle->hw_lock);
 
-	return 0 ;
+	return 0;
 }
 
 /*
-suspend the queue configuration. Since it is used for the runtime suspend
-only verify that the queue can be suspended.
-*/
+ * suspend the queue configuration. Since it is used for the runtime suspend
+ * only verify that the queue can be suspended.
+ */
 int ssi_request_mgr_runtime_suspend_queue(struct ssi_drvdata *drvdata)
 {
-	struct ssi_request_mgr_handle * request_mgr_handle = 
+	struct ssi_request_mgr_handle *request_mgr_handle =
 						drvdata->request_mgr_handle;
-	
+
 	/* lock the send_request */
 	spin_lock_bh(&request_mgr_handle->hw_lock);
-	if (request_mgr_handle->req_queue_head != 
+	if (request_mgr_handle->req_queue_head !=
 	    request_mgr_handle->req_queue_tail) {
 		spin_unlock_bh(&request_mgr_handle->hw_lock);
 		return -EBUSY;
@@ -702,10 +589,10 @@ int ssi_request_mgr_runtime_suspend_queue(struct ssi_drvdata *drvdata)
 
 bool ssi_request_mgr_is_queue_runtime_suspend(struct ssi_drvdata *drvdata)
 {
-	struct ssi_request_mgr_handle * request_mgr_handle = 
+	struct ssi_request_mgr_handle *request_mgr_handle =
 						drvdata->request_mgr_handle;
 
-	return 	request_mgr_handle->is_runtime_suspended;
+	return	request_mgr_handle->is_runtime_suspended;
 }
 
 #endif
diff --git a/drivers/staging/ccree/ssi_request_mgr.h b/drivers/staging/ccree/ssi_request_mgr.h
index c09339b566d0..bdbbf89e5367 100644
--- a/drivers/staging/ccree/ssi_request_mgr.h
+++ b/drivers/staging/ccree/ssi_request_mgr.h
@@ -1,21 +1,21 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
 /* \file request_mgr.h
-   Request Manager
+ * Request Manager
  */
 
 #ifndef __REQUEST_MGR_H__
@@ -27,29 +27,29 @@ int request_mgr_init(struct ssi_drvdata *drvdata);
 
 /*!
  * Enqueue caller request to crypto hardware.
- * 
- * \param drvdata 
+ *
+ * \param drvdata
  * \param ssi_req The request to enqueue
  * \param desc The crypto sequence
  * \param len The crypto sequence length
- * \param is_dout If "true": completion is handled by the caller 
- *      	  If "false": this function adds a dummy descriptor completion
- *      	  and waits upon completion signal.
- * 
+ * \param is_dout If "true": completion is handled by the caller
+ *	  If "false": this function adds a dummy descriptor completion
+ *	  and waits upon completion signal.
+ *
  * \return int Returns -EINPROGRESS if "is_dout=ture"; "0" if "is_dout=false"
  */
 int send_request(
 	struct ssi_drvdata *drvdata, struct ssi_crypto_req *ssi_req,
-	HwDesc_s *desc, unsigned int len, bool is_dout);
+	struct cc_hw_desc *desc, unsigned int len, bool is_dout);
 
 int send_request_init(
-	struct ssi_drvdata *drvdata, HwDesc_s *desc, unsigned int len);
+	struct ssi_drvdata *drvdata, struct cc_hw_desc *desc, unsigned int len);
 
 void complete_request(struct ssi_drvdata *drvdata);
 
 void request_mgr_fini(struct ssi_drvdata *drvdata);
 
-#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+#if defined(CONFIG_PM_RUNTIME) || defined(CONFIG_PM_SLEEP)
 int ssi_request_mgr_runtime_resume_queue(struct ssi_drvdata *drvdata);
 
 int ssi_request_mgr_runtime_suspend_queue(struct ssi_drvdata *drvdata);
diff --git a/drivers/staging/ccree/ssi_sram_mgr.c b/drivers/staging/ccree/ssi_sram_mgr.c
index 50066e17d1d3..e05c0c13c2eb 100644
--- a/drivers/staging/ccree/ssi_sram_mgr.c
+++ b/drivers/staging/ccree/ssi_sram_mgr.c
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -17,7 +17,6 @@
 #include "ssi_driver.h"
 #include "ssi_sram_mgr.h"
 
-
 /**
  * struct ssi_sram_mgr_ctx -Internal RAM context manager
  * @sram_free_offset:   the offset to the non-allocated area
@@ -26,10 +25,9 @@ struct ssi_sram_mgr_ctx {
 	ssi_sram_addr_t sram_free_offset;
 };
 
-
 /**
  * ssi_sram_mgr_fini() - Cleanup SRAM pool.
- * 
+ *
  * @drvdata: Associated device driver context
  */
 void ssi_sram_mgr_fini(struct ssi_drvdata *drvdata)
@@ -37,17 +35,17 @@ void ssi_sram_mgr_fini(struct ssi_drvdata *drvdata)
 	struct ssi_sram_mgr_ctx *smgr_ctx = drvdata->sram_mgr_handle;
 
 	/* Free "this" context */
-	if (smgr_ctx != NULL) {
+	if (smgr_ctx) {
 		memset(smgr_ctx, 0, sizeof(struct ssi_sram_mgr_ctx));
 		kfree(smgr_ctx);
 	}
 }
 
 /**
- * ssi_sram_mgr_init() - Initializes SRAM pool. 
+ * ssi_sram_mgr_init() - Initializes SRAM pool.
  *      The pool starts right at the beginning of SRAM.
  *      Returns zero for success, negative value otherwise.
- * 
+ *
  * @drvdata: Associated device driver context
  */
 int ssi_sram_mgr_init(struct ssi_drvdata *drvdata)
@@ -77,15 +75,15 @@ out:
 }
 
 /*!
- * Allocated buffer from SRAM pool. 
- * Note: Caller is responsible to free the LAST allocated buffer. 
- * This function does not taking care of any fragmentation may occur 
- * by the order of calls to alloc/free. 
- * 
- * \param drvdata 
+ * Allocated buffer from SRAM pool.
+ * Note: Caller is responsible to free the LAST allocated buffer.
+ * This function does not taking care of any fragmentation may occur
+ * by the order of calls to alloc/free.
+ *
+ * \param drvdata
  * \param size The requested bytes to allocate
  */
-ssi_sram_addr_t ssi_sram_mgr_alloc(struct ssi_drvdata *drvdata, uint32_t size)
+ssi_sram_addr_t ssi_sram_mgr_alloc(struct ssi_drvdata *drvdata, u32 size)
 {
 	struct ssi_sram_mgr_ctx *smgr_ctx = drvdata->sram_mgr_handle;
 	ssi_sram_addr_t p;
@@ -100,7 +98,7 @@ ssi_sram_addr_t ssi_sram_mgr_alloc(struct ssi_drvdata *drvdata, uint32_t size)
 			size, smgr_ctx->sram_free_offset);
 		return NULL_SRAM_ADDR;
 	}
-	
+
 	p = smgr_ctx->sram_free_offset;
 	smgr_ctx->sram_free_offset += size;
 	SSI_LOG_DEBUG("Allocated %u B @ %u\n", size, (unsigned int)p);
@@ -109,9 +107,9 @@ ssi_sram_addr_t ssi_sram_mgr_alloc(struct ssi_drvdata *drvdata, uint32_t size)
 
 /**
  * ssi_sram_mgr_const2sram_desc() - Create const descriptors sequence to
- *	set values in given array into SRAM. 
+ *	set values in given array into SRAM.
  * Note: each const value can't exceed word size.
- * 
+ *
  * @src:	  A pointer to array of words to set as consts.
  * @dst:	  The target SRAM buffer to set into
  * @nelements:	  The number of words in "src" array
@@ -119,18 +117,18 @@ ssi_sram_addr_t ssi_sram_mgr_alloc(struct ssi_drvdata *drvdata, uint32_t size)
  * @seq_len:	  A pointer to the given IN/OUT sequence length
  */
 void ssi_sram_mgr_const2sram_desc(
-	const uint32_t *src, ssi_sram_addr_t dst,
+	const u32 *src, ssi_sram_addr_t dst,
 	unsigned int nelement,
-	HwDesc_s *seq, unsigned int *seq_len)
+	struct cc_hw_desc *seq, unsigned int *seq_len)
 {
-	uint32_t i;
+	u32 i;
 	unsigned int idx = *seq_len;
 
 	for (i = 0; i < nelement; i++, idx++) {
-		HW_DESC_INIT(&seq[idx]);
-		HW_DESC_SET_DIN_CONST(&seq[idx], src[i], sizeof(uint32_t));
-		HW_DESC_SET_DOUT_SRAM(&seq[idx], dst + (i * sizeof(uint32_t)), sizeof(uint32_t));
-		HW_DESC_SET_FLOW_MODE(&seq[idx], BYPASS);
+		hw_desc_init(&seq[idx]);
+		set_din_const(&seq[idx], src[i], sizeof(u32));
+		set_dout_sram(&seq[idx], dst + (i * sizeof(u32)), sizeof(u32));
+		set_flow_mode(&seq[idx], BYPASS);
 	}
 
 	*seq_len = idx;
diff --git a/drivers/staging/ccree/ssi_sram_mgr.h b/drivers/staging/ccree/ssi_sram_mgr.h
index d71fbaf9ac44..9ba1d59a0bae 100644
--- a/drivers/staging/ccree/ssi_sram_mgr.h
+++ b/drivers/staging/ccree/ssi_sram_mgr.h
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -17,7 +17,6 @@
 #ifndef __SSI_SRAM_MGR_H__
 #define __SSI_SRAM_MGR_H__
 
-
 #ifndef SSI_CC_SRAM_SIZE
 #define SSI_CC_SRAM_SIZE 4096
 #endif
@@ -28,44 +27,44 @@ struct ssi_drvdata;
  * Address (offset) within CC internal SRAM
  */
 
-typedef uint64_t ssi_sram_addr_t;
+typedef u64 ssi_sram_addr_t;
 
 #define NULL_SRAM_ADDR ((ssi_sram_addr_t)-1)
 
 /*!
- * Initializes SRAM pool. 
- * The first X bytes of SRAM are reserved for ROM usage, hence, pool 
- * starts right after X bytes. 
- *  
- * \param drvdata 
- *  
+ * Initializes SRAM pool.
+ * The first X bytes of SRAM are reserved for ROM usage, hence, pool
+ * starts right after X bytes.
+ *
+ * \param drvdata
+ *
  * \return int Zero for success, negative value otherwise.
  */
 int ssi_sram_mgr_init(struct ssi_drvdata *drvdata);
 
 /*!
  * Uninits SRAM pool.
- * 
- * \param drvdata 
+ *
+ * \param drvdata
  */
 void ssi_sram_mgr_fini(struct ssi_drvdata *drvdata);
 
 /*!
- * Allocated buffer from SRAM pool. 
- * Note: Caller is responsible to free the LAST allocated buffer. 
- * This function does not taking care of any fragmentation may occur 
- * by the order of calls to alloc/free. 
- * 
- * \param drvdata 
+ * Allocated buffer from SRAM pool.
+ * Note: Caller is responsible to free the LAST allocated buffer.
+ * This function does not taking care of any fragmentation may occur
+ * by the order of calls to alloc/free.
+ *
+ * \param drvdata
  * \param size The requested bytes to allocate
  */
-ssi_sram_addr_t ssi_sram_mgr_alloc(struct ssi_drvdata *drvdata, uint32_t size);
+ssi_sram_addr_t ssi_sram_mgr_alloc(struct ssi_drvdata *drvdata, u32 size);
 
 /**
  * ssi_sram_mgr_const2sram_desc() - Create const descriptors sequence to
- *	set values in given array into SRAM. 
+ *	set values in given array into SRAM.
  * Note: each const value can't exceed word size.
- * 
+ *
  * @src:	  A pointer to array of words to set as consts.
  * @dst:	  The target SRAM buffer to set into
  * @nelements:	  The number of words in "src" array
@@ -73,8 +72,8 @@ ssi_sram_addr_t ssi_sram_mgr_alloc(struct ssi_drvdata *drvdata, uint32_t size);
  * @seq_len:	  A pointer to the given IN/OUT sequence length
  */
 void ssi_sram_mgr_const2sram_desc(
-	const uint32_t *src, ssi_sram_addr_t dst,
+	const u32 *src, ssi_sram_addr_t dst,
 	unsigned int nelement,
-	HwDesc_s *seq, unsigned int *seq_len);
+	struct cc_hw_desc *seq, unsigned int *seq_len);
 
 #endif /*__SSI_SRAM_MGR_H__*/
diff --git a/drivers/staging/ccree/ssi_sysfs.c b/drivers/staging/ccree/ssi_sysfs.c
index 7c514c1072a9..dbcd1634aad1 100644
--- a/drivers/staging/ccree/ssi_sysfs.c
+++ b/drivers/staging/ccree/ssi_sysfs.c
@@ -1,15 +1,15 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
@@ -40,7 +40,7 @@ struct stat_name {
 	const char *stat_phase_name[MAX_STAT_PHASES];
 };
 
-static struct stat_name stat_name_db[MAX_STAT_OP_TYPES] = 
+static struct stat_name stat_name_db[MAX_STAT_OP_TYPES] =
 {
 	{
 		/* STAT_OP_TYPE_NULL */
@@ -50,8 +50,8 @@ static struct stat_name stat_name_db[MAX_STAT_OP_TYPES] =
 	{
 		.op_type_name = "Encode",
 		.stat_phase_name[STAT_PHASE_0] = "Init and sanity checks",
-		.stat_phase_name[STAT_PHASE_1] = "Map buffers", 
-		.stat_phase_name[STAT_PHASE_2] = "Create sequence", 
+		.stat_phase_name[STAT_PHASE_1] = "Map buffers",
+		.stat_phase_name[STAT_PHASE_2] = "Create sequence",
 		.stat_phase_name[STAT_PHASE_3] = "Send Request",
 		.stat_phase_name[STAT_PHASE_4] = "HW-Q push",
 		.stat_phase_name[STAT_PHASE_5] = "Sequence completion",
@@ -59,14 +59,14 @@ static struct stat_name stat_name_db[MAX_STAT_OP_TYPES] =
 	},
 	{	.op_type_name = "Decode",
 		.stat_phase_name[STAT_PHASE_0] = "Init and sanity checks",
-		.stat_phase_name[STAT_PHASE_1] = "Map buffers", 
-		.stat_phase_name[STAT_PHASE_2] = "Create sequence", 
+		.stat_phase_name[STAT_PHASE_1] = "Map buffers",
+		.stat_phase_name[STAT_PHASE_2] = "Create sequence",
 		.stat_phase_name[STAT_PHASE_3] = "Send Request",
 		.stat_phase_name[STAT_PHASE_4] = "HW-Q push",
 		.stat_phase_name[STAT_PHASE_5] = "Sequence completion",
 		.stat_phase_name[STAT_PHASE_6] = "HW cycles",
 	},
-	{ 	.op_type_name = "Setkey",
+	{	.op_type_name = "Setkey",
 		.stat_phase_name[STAT_PHASE_0] = "Init and sanity checks",
 		.stat_phase_name[STAT_PHASE_1] = "Copy key to ctx",
 		.stat_phase_name[STAT_PHASE_2] = "Create sequence",
@@ -88,14 +88,14 @@ static struct stat_name stat_name_db[MAX_STAT_OP_TYPES] =
 };
 
 /*
- * Structure used to create a directory 
+ * Structure used to create a directory
  * and its attributes in sysfs.
  */
 struct sys_dir {
 	struct kobject *sys_dir_kobj;
 	struct attribute_group sys_dir_attr_group;
 	struct attribute **sys_dir_attr_list;
-	uint32_t num_of_attrs;
+	u32 num_of_attrs;
 	struct ssi_drvdata *drvdata; /* Associated driver context */
 };
 
@@ -108,14 +108,13 @@ static DEFINE_SPINLOCK(stat_lock);
 static struct stat_item stat_host_db[MAX_STAT_OP_TYPES][MAX_STAT_PHASES];
 static struct stat_item stat_cc_db[MAX_STAT_OP_TYPES][MAX_STAT_PHASES];
 
-
 static void init_db(struct stat_item item[MAX_STAT_OP_TYPES][MAX_STAT_PHASES])
 {
 	unsigned int i, j;
 
 	/* Clear db */
-	for (i=0; i<MAX_STAT_OP_TYPES; i++) {
-		for (j=0; j<MAX_STAT_PHASES; j++) {
+	for (i = 0; i < MAX_STAT_OP_TYPES; i++) {
+		for (j = 0; j < MAX_STAT_PHASES; j++) {
 			item[i][j].min = 0xFFFFFFFF;
 			item[i][j].max = 0;
 			item[i][j].sum = 0;
@@ -130,29 +129,28 @@ static void update_db(struct stat_item *item, unsigned int result)
 	item->sum += result;
 	if (result < item->min)
 		item->min = result;
-	if (result > item->max )
+	if (result > item->max)
 		item->max = result;
 }
 
 static void display_db(struct stat_item item[MAX_STAT_OP_TYPES][MAX_STAT_PHASES])
 {
 	unsigned int i, j;
-	uint64_t avg;
+	u64 avg;
 
-	for (i=STAT_OP_TYPE_ENCODE; i<MAX_STAT_OP_TYPES; i++) {
-		for (j=0; j<MAX_STAT_PHASES; j++) {	
+	for (i = STAT_OP_TYPE_ENCODE; i < MAX_STAT_OP_TYPES; i++) {
+		for (j = 0; j < MAX_STAT_PHASES; j++) {
 			if (item[i][j].count > 0) {
-				avg = (uint64_t)item[i][j].sum;
+				avg = (u64)item[i][j].sum;
 				do_div(avg, item[i][j].count);
-				SSI_LOG_ERR("%s, %s: min=%d avg=%d max=%d sum=%lld count=%d\n", 
-					stat_name_db[i].op_type_name, stat_name_db[i].stat_phase_name[j], 
+				SSI_LOG_ERR("%s, %s: min=%d avg=%d max=%d sum=%lld count=%d\n",
+					stat_name_db[i].op_type_name, stat_name_db[i].stat_phase_name[j],
 					item[i][j].min, (int)avg, item[i][j].max, (long long)item[i][j].sum, item[i][j].count);
 			}
 		}
 	}
 }
 
-
 /**************************************
  * Attributes show functions section  *
  **************************************/
@@ -174,38 +172,38 @@ static ssize_t ssi_sys_stats_cc_db_clear(struct kobject *kobj,
 static ssize_t ssi_sys_stat_host_db_show(struct kobject *kobj,
 		struct kobj_attribute *attr, char *buf)
 {
-	int i, j ;
+	int i, j;
 	char line[512];
-	uint32_t min_cyc, max_cyc;
-	uint64_t avg;
-	ssize_t buf_len, tmp_len=0;
+	u32 min_cyc, max_cyc;
+	u64 avg;
+	ssize_t buf_len, tmp_len = 0;
 
-	buf_len = scnprintf(buf,PAGE_SIZE,
+	buf_len = scnprintf(buf, PAGE_SIZE,
 		"phase\t\t\t\t\t\t\tmin[cy]\tavg[cy]\tmax[cy]\t#samples\n");
-	if ( buf_len <0 )/* scnprintf shouldn't return negative value according to its implementation*/
+	if (buf_len < 0)/* scnprintf shouldn't return negative value according to its implementation*/
 		return buf_len;
-	for (i=STAT_OP_TYPE_ENCODE; i<MAX_STAT_OP_TYPES; i++) {
-		for (j=0; j<MAX_STAT_PHASES-1; j++) {
+	for (i = STAT_OP_TYPE_ENCODE; i < MAX_STAT_OP_TYPES; i++) {
+		for (j = 0; j < MAX_STAT_PHASES - 1; j++) {
 			if (stat_host_db[i][j].count > 0) {
-				avg = (uint64_t)stat_host_db[i][j].sum;
+				avg = (u64)stat_host_db[i][j].sum;
 				do_div(avg, stat_host_db[i][j].count);
 				min_cyc = stat_host_db[i][j].min;
 				max_cyc = stat_host_db[i][j].max;
 			} else {
 				avg = min_cyc = max_cyc = 0;
 			}
-			tmp_len = scnprintf(line,512,
+			tmp_len = scnprintf(line, 512,
 				"%s::%s\t\t\t\t\t%6u\t%6u\t%6u\t%7u\n",
 				stat_name_db[i].op_type_name,
 				stat_name_db[i].stat_phase_name[j],
 				min_cyc, (unsigned int)avg, max_cyc,
 				stat_host_db[i][j].count);
-			if ( tmp_len <0 )/* scnprintf shouldn't return negative value according to its implementation*/
+			if (tmp_len < 0)/* scnprintf shouldn't return negative value according to its implementation*/
 				return buf_len;
-			if ( buf_len + tmp_len >= PAGE_SIZE)
+			if (buf_len + tmp_len >= PAGE_SIZE)
 				return buf_len;
 			buf_len += tmp_len;
-			strncat(buf, line,512);
+			strncat(buf, line, 512);
 		}
 	}
 	return buf_len;
@@ -216,24 +214,24 @@ static ssize_t ssi_sys_stat_cc_db_show(struct kobject *kobj,
 {
 	int i;
 	char line[256];
-	uint32_t min_cyc, max_cyc;
-	uint64_t avg;
-	ssize_t buf_len,tmp_len=0;
+	u32 min_cyc, max_cyc;
+	u64 avg;
+	ssize_t buf_len, tmp_len = 0;
 
-	buf_len = scnprintf(buf,PAGE_SIZE,
+	buf_len = scnprintf(buf, PAGE_SIZE,
 		"phase\tmin[cy]\tavg[cy]\tmax[cy]\t#samples\n");
-	if ( buf_len <0 )/* scnprintf shouldn't return negative value according to its implementation*/
+	if (buf_len < 0)/* scnprintf shouldn't return negative value according to its implementation*/
 		return buf_len;
-	for (i=STAT_OP_TYPE_ENCODE; i<MAX_STAT_OP_TYPES; i++) {
+	for (i = STAT_OP_TYPE_ENCODE; i < MAX_STAT_OP_TYPES; i++) {
 		if (stat_cc_db[i][STAT_PHASE_6].count > 0) {
-			avg = (uint64_t)stat_cc_db[i][STAT_PHASE_6].sum;
+			avg = (u64)stat_cc_db[i][STAT_PHASE_6].sum;
 			do_div(avg, stat_cc_db[i][STAT_PHASE_6].count);
 			min_cyc = stat_cc_db[i][STAT_PHASE_6].min;
 			max_cyc = stat_cc_db[i][STAT_PHASE_6].max;
 		} else {
 			avg = min_cyc = max_cyc = 0;
 		}
-		tmp_len = scnprintf(line,256,
+		tmp_len = scnprintf(line, 256,
 			"%s\t%6u\t%6u\t%6u\t%7u\n",
 			stat_name_db[i].op_type_name,
 			min_cyc,
@@ -241,13 +239,13 @@ static ssize_t ssi_sys_stat_cc_db_show(struct kobject *kobj,
 			max_cyc,
 			stat_cc_db[i][STAT_PHASE_6].count);
 
-		if ( tmp_len < 0 )/* scnprintf shouldn't return negative value according to its implementation*/
+		if (tmp_len < 0)/* scnprintf shouldn't return negative value according to its implementation*/
 			return buf_len;
 
-		if ( buf_len + tmp_len >= PAGE_SIZE)
+		if (buf_len + tmp_len >= PAGE_SIZE)
 			return buf_len;
 		buf_len += tmp_len;
-		strncat(buf, line,256);
+		strncat(buf, line, 256);
 	}
 	return buf_len;
 }
@@ -271,21 +269,19 @@ void update_cc_stat(
 
 void display_all_stat_db(void)
 {
-	SSI_LOG_ERR("\n=======    CYCLE COUNT STATS    =======\n"); 
+	SSI_LOG_ERR("\n=======    CYCLE COUNT STATS    =======\n");
 	display_db(stat_host_db);
-	SSI_LOG_ERR("\n======= CC HW CYCLE COUNT STATS =======\n"); 
+	SSI_LOG_ERR("\n======= CC HW CYCLE COUNT STATS =======\n");
 	display_db(stat_cc_db);
 }
 #endif /*CC_CYCLE_COUNT*/
 
-
-
 static ssize_t ssi_sys_regdump_show(struct kobject *kobj,
 		struct kobj_attribute *attr, char *buf)
 {
 	struct ssi_drvdata *drvdata = sys_get_drvdata();
-	uint32_t register_value;
-	void __iomem* cc_base = drvdata->cc_base;
+	u32 register_value;
+	void __iomem *cc_base = drvdata->cc_base;
 	int offset = 0;
 
 	register_value = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_SIGNATURE));
@@ -304,7 +300,7 @@ static ssize_t ssi_sys_regdump_show(struct kobject *kobj,
 static ssize_t ssi_sys_help_show(struct kobject *kobj,
 		struct kobj_attribute *attr, char *buf)
 {
-	char* help_str[]={
+	char *help_str[] = {
 				"cat reg_dump              ", "Print several of CC register values",
 		#if defined CC_CYCLE_COUNT
 				"cat stats_host            ", "Print host statistics",
@@ -313,12 +309,12 @@ static ssize_t ssi_sys_help_show(struct kobject *kobj,
 				"echo <number> > stats_cc  ", "Clear CC statistics database",
 		#endif
 				};
-	int i=0, offset = 0;
+	int i = 0, offset = 0;
 
 	offset += scnprintf(buf + offset, PAGE_SIZE - offset, "Usage:\n");
-	for ( i = 0; i < ARRAY_SIZE(help_str); i+=2) {
-	   offset += scnprintf(buf + offset, PAGE_SIZE - offset, "%s\t\t%s\n", help_str[i], help_str[i+1]);
-	}
+	for (i = 0; i < ARRAY_SIZE(help_str); i += 2)
+	   offset += scnprintf(buf + offset, PAGE_SIZE - offset, "%s\t\t%s\n", help_str[i], help_str[i + 1]);
+
 	return offset;
 }
 
@@ -333,7 +329,7 @@ struct sys_dir {
 	struct kobject *sys_dir_kobj;
 	struct attribute_group sys_dir_attr_group;
 	struct attribute **sys_dir_attr_list;
-	uint32_t num_of_attrs;
+	u32 num_of_attrs;
 	struct ssi_drvdata *drvdata; /* Associated driver context */
 };
 
@@ -355,13 +351,14 @@ static struct ssi_drvdata *sys_get_drvdata(void)
 {
 	/* TODO: supporting multiple SeP devices would require avoiding
 	 * global "top_dir" and finding associated "top_dir" by traversing
-	 * up the tree to the kobject which matches one of the top_dir's */
+	 * up the tree to the kobject which matches one of the top_dir's
+	 */
 	return sys_top_dir.drvdata;
 }
 
 static int sys_init_dir(struct sys_dir *sys_dir, struct ssi_drvdata *drvdata,
 		 struct kobject *parent_dir_kobj, const char *dir_name,
-		 struct kobj_attribute *attrs, uint32_t num_of_attrs)
+		 struct kobj_attribute *attrs, u32 num_of_attrs)
 {
 	int i;
 
@@ -407,7 +404,7 @@ static void sys_free_dir(struct sys_dir *sys_dir)
 
 	kfree(sys_dir->sys_dir_attr_list);
 
-	if (sys_dir->sys_dir_kobj != NULL)
+	if (sys_dir->sys_dir_kobj)
 		kobject_put(sys_dir->sys_dir_kobj);
 }
 
diff --git a/drivers/staging/ccree/ssi_sysfs.h b/drivers/staging/ccree/ssi_sysfs.h
index baeac1d99c07..44ae3d4c40b3 100644
--- a/drivers/staging/ccree/ssi_sysfs.h
+++ b/drivers/staging/ccree/ssi_sysfs.h
@@ -1,21 +1,21 @@
 /*
  * Copyright (C) 2012-2017 ARM Limited or its affiliates.
- * 
+ *
  * This program is free software; you can 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/>.
  */
 
 /* \file ssi_sysfs.h
-   ARM CryptoCell sysfs APIs
+ * ARM CryptoCell sysfs APIs
  */
 
 #ifndef __SSI_SYSFS_H__
@@ -36,6 +36,7 @@ enum stat_phase {
 	STAT_PHASE_6,
 	MAX_STAT_PHASES,
 };
+
 enum stat_op {
 	STAT_OP_TYPE_NULL = 0,
 	STAT_OP_TYPE_ENCODE,
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index f191c2a75732..ca11be21f64b 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -2881,29 +2881,25 @@ static int __init comedi_init(void)
 	retval = register_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
 					COMEDI_NUM_MINORS, "comedi");
 	if (retval)
-		return -EIO;
+		return retval;
+
 	cdev_init(&comedi_cdev, &comedi_fops);
 	comedi_cdev.owner = THIS_MODULE;
 
 	retval = kobject_set_name(&comedi_cdev.kobj, "comedi");
-	if (retval) {
-		unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
-					 COMEDI_NUM_MINORS);
-		return retval;
-	}
+	if (retval)
+		goto out_unregister_chrdev_region;
+
+	retval = cdev_add(&comedi_cdev, MKDEV(COMEDI_MAJOR, 0),
+			  COMEDI_NUM_MINORS);
+	if (retval)
+		goto out_unregister_chrdev_region;
 
-	if (cdev_add(&comedi_cdev, MKDEV(COMEDI_MAJOR, 0), COMEDI_NUM_MINORS)) {
-		unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
-					 COMEDI_NUM_MINORS);
-		return -EIO;
-	}
 	comedi_class = class_create(THIS_MODULE, "comedi");
 	if (IS_ERR(comedi_class)) {
+		retval = PTR_ERR(comedi_class);
 		pr_err("failed to create class\n");
-		cdev_del(&comedi_cdev);
-		unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
-					 COMEDI_NUM_MINORS);
-		return PTR_ERR(comedi_class);
+		goto out_cdev_del;
 	}
 
 	comedi_class->dev_groups = comedi_dev_groups;
@@ -2914,11 +2910,8 @@ static int __init comedi_init(void)
 
 		dev = comedi_alloc_board_minor(NULL);
 		if (IS_ERR(dev)) {
-			comedi_cleanup_board_minors();
-			cdev_del(&comedi_cdev);
-			unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
-						 COMEDI_NUM_MINORS);
-			return PTR_ERR(dev);
+			retval = PTR_ERR(dev);
+			goto out_cleanup_board_minors;
 		}
 		/* comedi_alloc_board_minor() locked the mutex */
 		mutex_unlock(&dev->mutex);
@@ -2928,6 +2921,15 @@ static int __init comedi_init(void)
 	comedi_proc_init();
 
 	return 0;
+
+out_cleanup_board_minors:
+	comedi_cleanup_board_minors();
+	class_destroy(comedi_class);
+out_cdev_del:
+	cdev_del(&comedi_cdev);
+out_unregister_chrdev_region:
+	unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0), COMEDI_NUM_MINORS);
+	return retval;
 }
 module_init(comedi_init);
 
diff --git a/drivers/staging/comedi/drivers/ni_labpc_isadma.h b/drivers/staging/comedi/drivers/ni_labpc_isadma.h
index b8a1b0ee6290..e93f79050e60 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_isadma.h
+++ b/drivers/staging/comedi/drivers/ni_labpc_isadma.h
@@ -1,6 +1,6 @@
 /*
  * ni_labpc ISA DMA support.
-*/
+ */
 
 #ifndef _NI_LABPC_ISADMA_H
 #define _NI_LABPC_ISADMA_H
diff --git a/drivers/staging/comedi/drivers/ni_labpc_regs.h b/drivers/staging/comedi/drivers/ni_labpc_regs.h
index 8c52179e36fc..6003e9d5fe37 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_regs.h
+++ b/drivers/staging/comedi/drivers/ni_labpc_regs.h
@@ -1,6 +1,6 @@
 /*
  * ni_labpc register definitions.
-*/
+ */
 
 #ifndef _NI_LABPC_REGS_H
 #define _NI_LABPC_REGS_H
diff --git a/drivers/staging/comedi/drivers/s626.c b/drivers/staging/comedi/drivers/s626.c
index 4b9c22665748..c906c9a5d944 100644
--- a/drivers/staging/comedi/drivers/s626.c
+++ b/drivers/staging/comedi/drivers/s626.c
@@ -580,11 +580,14 @@ static int s626_set_dac(struct comedi_device *dev,
 	 * running after the packet has been sent to the target DAC.
 	 */
 	val = 0x0F000000;	/* Continue clock after target DAC data
-				 * (write to non-existent trimdac). */
+				 * (write to non-existent trimdac).
+				 */
 	val |= 0x00004000;	/* Address the two main dual-DAC devices
-				 * (TSL's chip select enables target device). */
+				 * (TSL's chip select enables target device).
+				 */
 	val |= ((u32)(chan & 1) << 15);	/* Address the DAC channel
-						 * within the device. */
+					 * within the device.
+					 */
 	val |= (u32)dacdata;	/* Include DAC setpoint data. */
 	return s626_send_dac(dev, val);
 }
diff --git a/drivers/staging/dgnc/dgnc_driver.c b/drivers/staging/dgnc/dgnc_driver.c
index 253f38b25a54..c1b6079384e9 100644
--- a/drivers/staging/dgnc/dgnc_driver.c
+++ b/drivers/staging/dgnc/dgnc_driver.c
@@ -96,7 +96,6 @@ static int dgnc_do_remap(struct dgnc_board *brd)
 	return 0;
 }
 
-
 /* A board has been found, initialize  it. */
 static struct dgnc_board *dgnc_found_board(struct pci_dev *pdev, int id)
 {
@@ -287,7 +286,6 @@ static void dgnc_free_irq(struct dgnc_board *brd)
 		free_irq(brd->irq, brd);
 }
 
-
  /*
   * As each timer expires, it determines (a) whether the "transmit"
   * waiter needs to be woken up, and (b) whether the poller needs to
diff --git a/drivers/staging/dgnc/dgnc_driver.h b/drivers/staging/dgnc/dgnc_driver.h
index 980410fc4801..764d6fe0d030 100644
--- a/drivers/staging/dgnc/dgnc_driver.h
+++ b/drivers/staging/dgnc/dgnc_driver.h
@@ -52,19 +52,6 @@
 
 #define dgnc_jiffies_from_ms(a) (((a) * HZ) / 1000)
 
-/*
- * Define a local default termios struct. All ports will be created
- * with this termios initially.  This is the same structure that is defined
- * as the default in tty_io.c with the same settings overridden as in serial.c
- *
- * In short, this should match the internal serial ports' defaults.
- */
-#define	DEFAULT_IFLAGS	(ICRNL | IXON)
-#define	DEFAULT_OFLAGS	(OPOST | ONLCR)
-#define	DEFAULT_CFLAGS	(B9600 | CS8 | CREAD | HUPCL | CLOCAL)
-#define	DEFAULT_LFLAGS	(ISIG | ICANON | ECHO | ECHOE | ECHOK | \
-			ECHOCTL | ECHOKE | IEXTEN)
-
 #ifndef _POSIX_VDISABLE
 #define   _POSIX_VDISABLE '\0'
 #endif
diff --git a/drivers/staging/dgnc/dgnc_tty.c b/drivers/staging/dgnc/dgnc_tty.c
index 9e98781ca6fe..d3736daf8cf2 100644
--- a/drivers/staging/dgnc/dgnc_tty.c
+++ b/drivers/staging/dgnc/dgnc_tty.c
@@ -51,22 +51,6 @@ static const struct digi_t dgnc_digi_init = {
 	.digi_term =	"ansi"		/* default terminal type */
 };
 
-/*
- * Define a local default termios struct. All ports will be created
- * with this termios initially.
- *
- * This defines a raw port at 9600 baud, 8 data bits, no parity,
- * 1 stop bit.
- */
-static const struct ktermios default_termios = {
-	.c_iflag =	(DEFAULT_IFLAGS),
-	.c_oflag =	(DEFAULT_OFLAGS),
-	.c_cflag =	(DEFAULT_CFLAGS),
-	.c_lflag =	(DEFAULT_LFLAGS),
-	.c_cc =		INIT_C_CC,
-	.c_line =	0,
-};
-
 static int dgnc_tty_open(struct tty_struct *tty, struct file *file);
 static void dgnc_tty_close(struct tty_struct *tty, struct file *file);
 static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file,
@@ -129,6 +113,49 @@ static const struct tty_operations dgnc_tty_ops = {
 
 /* TTY Initialization/Cleanup Functions */
 
+static struct tty_driver *dgnc_tty_create(char *serial_name, uint maxports,
+					  int major, int minor)
+{
+	int rc;
+	struct tty_driver *drv;
+
+	drv = tty_alloc_driver(maxports,
+			       TTY_DRIVER_REAL_RAW |
+			       TTY_DRIVER_DYNAMIC_DEV |
+			       TTY_DRIVER_HARDWARE_BREAK);
+	if (IS_ERR(drv))
+		return drv;
+
+	drv->name = serial_name;
+	drv->name_base = 0;
+	drv->major = major;
+	drv->minor_start = minor;
+	drv->type = TTY_DRIVER_TYPE_SERIAL;
+	drv->subtype = SERIAL_TYPE_NORMAL;
+	drv->init_termios = tty_std_termios;
+	drv->init_termios.c_cflag = (B9600 | CS8 | CREAD | HUPCL | CLOCAL);
+	drv->init_termios.c_ispeed = 9600;
+	drv->init_termios.c_ospeed = 9600;
+	drv->driver_name = DRVSTR;
+	/*
+	 * Entry points for driver.  Called by the kernel from
+	 * tty_io.c and n_tty.c.
+	 */
+	tty_set_operations(drv, &dgnc_tty_ops);
+	rc = tty_register_driver(drv);
+	if (rc < 0) {
+		put_tty_driver(drv);
+		return ERR_PTR(rc);
+	}
+	return drv;
+}
+
+static void dgnc_tty_free(struct tty_driver *drv)
+{
+	tty_unregister_driver(drv);
+	put_tty_driver(drv);
+}
+
 /**
  * dgnc_tty_register() - Init the tty subsystem for this board.
  */
@@ -136,95 +163,36 @@ int dgnc_tty_register(struct dgnc_board *brd)
 {
 	int rc;
 
-	brd->serial_driver = tty_alloc_driver(brd->maxports,
-					      TTY_DRIVER_REAL_RAW |
-					      TTY_DRIVER_DYNAMIC_DEV |
-					      TTY_DRIVER_HARDWARE_BREAK);
-	if (IS_ERR(brd->serial_driver))
-		return PTR_ERR(brd->serial_driver);
-
 	snprintf(brd->serial_name, MAXTTYNAMELEN, "tty_dgnc_%d_",
 		 brd->boardnum);
 
-	brd->serial_driver->name = brd->serial_name;
-	brd->serial_driver->name_base = 0;
-	brd->serial_driver->major = 0;
-	brd->serial_driver->minor_start = 0;
-	brd->serial_driver->type = TTY_DRIVER_TYPE_SERIAL;
-	brd->serial_driver->subtype = SERIAL_TYPE_NORMAL;
-	brd->serial_driver->init_termios = default_termios;
-	brd->serial_driver->driver_name = DRVSTR;
-
-	/*
-	 * Entry points for driver.  Called by the kernel from
-	 * tty_io.c and n_tty.c.
-	 */
-	tty_set_operations(brd->serial_driver, &dgnc_tty_ops);
-
-	rc = tty_register_driver(brd->serial_driver);
-	if (rc < 0) {
-		dev_dbg(&brd->pdev->dev,
-			"Can't register tty device (%d)\n", rc);
-		goto free_serial_driver;
+	brd->serial_driver = dgnc_tty_create(brd->serial_name,
+					     brd->maxports, 0, 0);
+	if (IS_ERR(brd->serial_driver)) {
+		rc = PTR_ERR(brd->serial_driver);
+		dev_dbg(&brd->pdev->dev, "Can't register tty device (%d)\n",
+			rc);
+		return rc;
 	}
 
-	/*
-	 * If we're doing transparent print, we have to do all of the above
-	 * again, separately so we don't get the LD confused about what major
-	 * we are when we get into the dgnc_tty_open() routine.
-	 */
-	brd->print_driver = tty_alloc_driver(brd->maxports,
-					     TTY_DRIVER_REAL_RAW |
-					     TTY_DRIVER_DYNAMIC_DEV |
-					     TTY_DRIVER_HARDWARE_BREAK);
+	snprintf(brd->print_name, MAXTTYNAMELEN, "pr_dgnc_%d_", brd->boardnum);
+	brd->print_driver = dgnc_tty_create(brd->print_name, brd->maxports,
+					    0x80,
+					    brd->serial_driver->major);
 	if (IS_ERR(brd->print_driver)) {
 		rc = PTR_ERR(brd->print_driver);
-		goto unregister_serial_driver;
-	}
-
-	snprintf(brd->print_name, MAXTTYNAMELEN, "pr_dgnc_%d_", brd->boardnum);
-
-	brd->print_driver->name = brd->print_name;
-	brd->print_driver->name_base = 0;
-	brd->print_driver->major = brd->serial_driver->major;
-	brd->print_driver->minor_start = 0x80;
-	brd->print_driver->type = TTY_DRIVER_TYPE_SERIAL;
-	brd->print_driver->subtype = SERIAL_TYPE_NORMAL;
-	brd->print_driver->init_termios = default_termios;
-	brd->print_driver->driver_name = DRVSTR;
-
-	/*
-	 * Entry points for driver.  Called by the kernel from
-	 * tty_io.c and n_tty.c.
-	 */
-	tty_set_operations(brd->print_driver, &dgnc_tty_ops);
-
-	rc = tty_register_driver(brd->print_driver);
-	if (rc < 0) {
 		dev_dbg(&brd->pdev->dev,
-			"Can't register Transparent Print device(%d)\n",
-			rc);
-		goto free_print_driver;
+			"Can't register Transparent Print device(%d)\n", rc);
+		dgnc_tty_free(brd->serial_driver);
+		return rc;
 	}
-
 	return 0;
-
-free_print_driver:
-	put_tty_driver(brd->print_driver);
-unregister_serial_driver:
-	tty_unregister_driver(brd->serial_driver);
-free_serial_driver:
-	put_tty_driver(brd->serial_driver);
-
-	return rc;
 }
 
 void dgnc_tty_unregister(struct dgnc_board *brd)
 {
-	tty_unregister_driver(brd->print_driver);
-	tty_unregister_driver(brd->serial_driver);
-	put_tty_driver(brd->print_driver);
-	put_tty_driver(brd->serial_driver);
+	dgnc_tty_free(brd->print_driver);
+	dgnc_tty_free(brd->serial_driver);
 }
 
 /**
diff --git a/drivers/staging/emxx_udc/emxx_udc.c b/drivers/staging/emxx_udc/emxx_udc.c
index 77b242e09932..bb010cb98a1c 100644
--- a/drivers/staging/emxx_udc/emxx_udc.c
+++ b/drivers/staging/emxx_udc/emxx_udc.c
@@ -200,13 +200,13 @@ static u32 _nbu2ss_get_begin_ram_address(struct nbu2ss_udc *udc)
 	for (num = 0; num < NUM_ENDPOINTS - 1; num++) {
 		p_ep_regs = &udc->p_regs->EP_REGS[num];
 		data = _nbu2ss_readl(&p_ep_regs->EP_PCKT_ADRS);
-		buf_type = _nbu2ss_readl(&p_ep_regs->EP_CONTROL) & EPn_BUF_TYPE;
+		buf_type = _nbu2ss_readl(&p_ep_regs->EP_CONTROL) & EPN_BUF_TYPE;
 		if (buf_type == 0) {
 			/* Single Buffer */
-			use_ram_size += (data & EPn_MPKT) / sizeof(u32);
+			use_ram_size += (data & EPN_MPKT) / sizeof(u32);
 		} else {
 			/* Double Buffer */
-			use_ram_size += ((data & EPn_MPKT) / sizeof(u32)) * 2;
+			use_ram_size += ((data & EPN_MPKT) / sizeof(u32)) * 2;
 		}
 
 		if ((data >> 16) > last_ram_adr)
@@ -245,15 +245,15 @@ static int _nbu2ss_ep_init(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
 	/*   Bulk, Interrupt, ISO */
 	switch (ep->ep_type) {
 	case USB_ENDPOINT_XFER_BULK:
-		data = EPn_BULK;
+		data = EPN_BULK;
 		break;
 
 	case USB_ENDPOINT_XFER_INT:
-		data = EPn_BUF_SINGLE | EPn_INTERRUPT;
+		data = EPN_BUF_SINGLE | EPN_INTERRUPT;
 		break;
 
 	case USB_ENDPOINT_XFER_ISOC:
-		data = EPn_ISO;
+		data = EPN_ISO;
 		break;
 
 	default:
@@ -267,24 +267,24 @@ static int _nbu2ss_ep_init(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
 	if (ep->direct == USB_DIR_OUT) {
 		/*---------------------------------------------------------*/
 		/* OUT */
-		data = EPn_EN | EPn_BCLR | EPn_DIR0;
+		data = EPN_EN | EPN_BCLR | EPN_DIR0;
 		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
 
-		data = EPn_ONAK | EPn_OSTL_EN | EPn_OSTL;
+		data = EPN_ONAK | EPN_OSTL_EN | EPN_OSTL;
 		_nbu2ss_bitclr(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
 
-		data = EPn_OUT_EN | EPn_OUT_END_EN;
+		data = EPN_OUT_EN | EPN_OUT_END_EN;
 		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_INT_ENA, data);
 	} else {
 		/*---------------------------------------------------------*/
 		/* IN */
-		data = EPn_EN | EPn_BCLR | EPn_AUTO;
+		data = EPN_EN | EPN_BCLR | EPN_AUTO;
 		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
 
-		data = EPn_ISTL;
+		data = EPN_ISTL;
 		_nbu2ss_bitclr(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
 
-		data = EPn_IN_EN | EPn_IN_END_EN;
+		data = EPN_IN_EN | EPN_IN_END_EN;
 		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_INT_ENA, data);
 	}
 
@@ -315,24 +315,24 @@ static int _nbu2ss_epn_exit(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
 	if (ep->direct == USB_DIR_OUT) {
 		/*---------------------------------------------------------*/
 		/* OUT */
-		data = EPn_ONAK | EPn_BCLR;
+		data = EPN_ONAK | EPN_BCLR;
 		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
 
-		data = EPn_EN | EPn_DIR0;
+		data = EPN_EN | EPN_DIR0;
 		_nbu2ss_bitclr(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
 
-		data = EPn_OUT_EN | EPn_OUT_END_EN;
+		data = EPN_OUT_EN | EPN_OUT_END_EN;
 		_nbu2ss_bitclr(&udc->p_regs->EP_REGS[num].EP_INT_ENA, data);
 	} else {
 		/*---------------------------------------------------------*/
 		/* IN */
-		data = EPn_BCLR;
+		data = EPN_BCLR;
 		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
 
-		data = EPn_EN | EPn_AUTO;
+		data = EPN_EN | EPN_AUTO;
 		_nbu2ss_bitclr(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
 
-		data = EPn_IN_EN | EPn_IN_END_EN;
+		data = EPN_IN_EN | EPN_IN_END_EN;
 		_nbu2ss_bitclr(&udc->p_regs->EP_REGS[num].EP_INT_ENA, data);
 	}
 
@@ -360,21 +360,21 @@ static void _nbu2ss_ep_dma_init(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
 
 		/*---------------------------------------------------------*/
 		/* Transfer Direct */
-		data = DCR1_EPn_DIR0;
+		data = DCR1_EPN_DIR0;
 		_nbu2ss_bitset(&udc->p_regs->EP_DCR[num].EP_DCR1, data);
 
 		/*---------------------------------------------------------*/
 		/* DMA Mode etc. */
-		data = EPn_STOP_MODE | EPn_STOP_SET  | EPn_DMAMODE0;
+		data = EPN_STOP_MODE | EPN_STOP_SET  | EPN_DMAMODE0;
 		_nbu2ss_writel(&udc->p_regs->EP_REGS[num].EP_DMA_CTRL, data);
 	} else {
 		/*---------------------------------------------------------*/
 		/* IN */
-		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, EPn_AUTO);
+		_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, EPN_AUTO);
 
 		/*---------------------------------------------------------*/
 		/* DMA Mode etc. */
-		data = EPn_BURST_SET | EPn_DMAMODE0;
+		data = EPN_BURST_SET | EPN_DMAMODE0;
 		_nbu2ss_writel(&udc->p_regs->EP_REGS[num].EP_DMA_CTRL, data);
 	}
 }
@@ -402,12 +402,12 @@ static void _nbu2ss_ep_dma_exit(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
 		/*---------------------------------------------------------*/
 		/* OUT */
 		_nbu2ss_writel(&preg->EP_DCR[num].EP_DCR2, 0);
-		_nbu2ss_bitclr(&preg->EP_DCR[num].EP_DCR1, DCR1_EPn_DIR0);
+		_nbu2ss_bitclr(&preg->EP_DCR[num].EP_DCR1, DCR1_EPN_DIR0);
 		_nbu2ss_writel(&preg->EP_REGS[num].EP_DMA_CTRL, 0);
 	} else {
 		/*---------------------------------------------------------*/
 		/* IN */
-		_nbu2ss_bitclr(&preg->EP_REGS[num].EP_CONTROL, EPn_AUTO);
+		_nbu2ss_bitclr(&preg->EP_REGS[num].EP_CONTROL, EPN_AUTO);
 		_nbu2ss_writel(&preg->EP_REGS[num].EP_DMA_CTRL, 0);
 	}
 }
@@ -418,9 +418,9 @@ static void _nbu2ss_ep_dma_abort(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
 {
 	struct fc_regs	*preg = udc->p_regs;
 
-	_nbu2ss_bitclr(&preg->EP_DCR[ep->epnum - 1].EP_DCR1, DCR1_EPn_REQEN);
-	mdelay(DMA_DISABLE_TIME);	/* DCR1_EPn_REQEN Clear */
-	_nbu2ss_bitclr(&preg->EP_REGS[ep->epnum - 1].EP_DMA_CTRL, EPn_DMA_EN);
+	_nbu2ss_bitclr(&preg->EP_DCR[ep->epnum - 1].EP_DCR1, DCR1_EPN_REQEN);
+	mdelay(DMA_DISABLE_TIME);	/* DCR1_EPN_REQEN Clear */
+	_nbu2ss_bitclr(&preg->EP_REGS[ep->epnum - 1].EP_DMA_CTRL, EPN_DMA_EN);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -453,16 +453,16 @@ static void _nbu2ss_ep_in_end(
 	} else {
 		num = epnum - 1;
 
-		_nbu2ss_bitclr(&preg->EP_REGS[num].EP_CONTROL, EPn_AUTO);
+		_nbu2ss_bitclr(&preg->EP_REGS[num].EP_CONTROL, EPN_AUTO);
 
 		/* Writing of 1-4 bytes */
 		if (length)
 			_nbu2ss_writel(&preg->EP_REGS[num].EP_WRITE, data32);
 
-		data = (((length) << 5) & EPn_DW) | EPn_DEND;
+		data = (((length) << 5) & EPN_DW) | EPN_DEND;
 		_nbu2ss_bitset(&preg->EP_REGS[num].EP_CONTROL, data);
 
-		_nbu2ss_bitset(&preg->EP_REGS[num].EP_CONTROL, EPn_AUTO);
+		_nbu2ss_bitset(&preg->EP_REGS[num].EP_CONTROL, EPN_AUTO);
 	}
 }
 
@@ -526,12 +526,13 @@ static void _nbu2ss_dma_unmap_single(
 			if (direct == USB_DIR_OUT)
 				memcpy(req->req.buf, ep->virt_buf,
 				       req->req.actual & 0xfffffffc);
-		} else
+		} else {
 			dma_unmap_single(udc->gadget.dev.parent,
 					 req->req.dma, req->req.length,
 				(direct == USB_DIR_IN)
 				? DMA_TO_DEVICE
 				: DMA_FROM_DEVICE);
+		}
 		req->req.dma = DMA_ADDR_INVALID;
 		req->mapped = 0;
 	} else {
@@ -573,65 +574,67 @@ static int ep0_out_pio(struct nbu2ss_udc *udc, u8 *buf, u32 length)
 
 /*-------------------------------------------------------------------------*/
 /* Endpoint 0 OUT Transfer (PIO, OverBytes) */
-static int EP0_out_OverBytes(struct nbu2ss_udc *udc, u8 *pBuf, u32 length)
+static int ep0_out_overbytes(struct nbu2ss_udc *udc, u8 *p_buf, u32 length)
 {
 	u32		i;
-	u32		iReadSize = 0;
-	union usb_reg_access  Temp32;
-	union usb_reg_access  *pBuf32 = (union usb_reg_access *)pBuf;
+	u32		i_read_size = 0;
+	union usb_reg_access  temp_32;
+	union usb_reg_access  *p_buf_32 = (union usb_reg_access *)p_buf;
 
 	if ((length > 0) && (length < sizeof(u32))) {
-		Temp32.dw = _nbu2ss_readl(&udc->p_regs->EP0_READ);
+		temp_32.dw = _nbu2ss_readl(&udc->p_regs->EP0_READ);
 		for (i = 0 ; i < length ; i++)
-			pBuf32->byte.DATA[i] = Temp32.byte.DATA[i];
-		iReadSize += length;
+			p_buf_32->byte.DATA[i] = temp_32.byte.DATA[i];
+		i_read_size += length;
 	}
 
-	return iReadSize;
+	return i_read_size;
 }
 
 /*-------------------------------------------------------------------------*/
 /* Endpoint 0 IN Transfer (PIO) */
-static int EP0_in_PIO(struct nbu2ss_udc *udc, u8 *pBuf, u32 length)
+static int EP0_in_PIO(struct nbu2ss_udc *udc, u8 *p_buf, u32 length)
 {
 	u32		i;
-	u32		iMaxLength   = EP0_PACKETSIZE;
-	u32		iWordLength  = 0;
-	u32		iWriteLength = 0;
-	union usb_reg_access  *pBuf32 = (union usb_reg_access *)pBuf;
+	u32		i_max_length   = EP0_PACKETSIZE;
+	u32		i_word_length  = 0;
+	u32		i_write_length = 0;
+	union usb_reg_access  *p_buf_32 = (union usb_reg_access *)p_buf;
 
 	/*------------------------------------------------------------*/
 	/* Transfer Length */
-	if (iMaxLength < length)
-		iWordLength = iMaxLength / sizeof(u32);
+	if (i_max_length < length)
+		i_word_length = i_max_length / sizeof(u32);
 	else
-		iWordLength = length / sizeof(u32);
+		i_word_length = length / sizeof(u32);
 
 	/*------------------------------------------------------------*/
 	/* PIO */
-	for (i = 0; i < iWordLength; i++) {
-		_nbu2ss_writel(&udc->p_regs->EP0_WRITE, pBuf32->dw);
-		pBuf32++;
-		iWriteLength += sizeof(u32);
+	for (i = 0; i < i_word_length; i++) {
+		_nbu2ss_writel(&udc->p_regs->EP0_WRITE, p_buf_32->dw);
+		p_buf_32++;
+		i_write_length += sizeof(u32);
 	}
 
-	return iWriteLength;
+	return i_write_length;
 }
 
 /*-------------------------------------------------------------------------*/
 /* Endpoint 0 IN Transfer (PIO, OverBytes) */
-static int EP0_in_OverBytes(struct nbu2ss_udc *udc, u8 *pBuf, u32 iRemainSize)
+static int ep0_in_overbytes(struct nbu2ss_udc *udc,
+			    u8 *p_buf,
+			    u32 i_remain_size)
 {
 	u32		i;
-	union usb_reg_access  Temp32;
-	union usb_reg_access  *pBuf32 = (union usb_reg_access *)pBuf;
+	union usb_reg_access  temp_32;
+	union usb_reg_access  *p_buf_32 = (union usb_reg_access *)p_buf;
 
-	if ((iRemainSize > 0) && (iRemainSize < sizeof(u32))) {
-		for (i = 0 ; i < iRemainSize ; i++)
-			Temp32.byte.DATA[i] = pBuf32->byte.DATA[i];
-		_nbu2ss_ep_in_end(udc, 0, Temp32.dw, iRemainSize);
+	if ((i_remain_size > 0) && (i_remain_size < sizeof(u32))) {
+		for (i = 0 ; i < i_remain_size ; i++)
+			temp_32.byte.DATA[i] = p_buf_32->byte.DATA[i];
+		_nbu2ss_ep_in_end(udc, 0, temp_32.dw, i_remain_size);
 
-		return iRemainSize;
+		return i_remain_size;
 	}
 
 	return 0;
@@ -679,9 +682,9 @@ static int _nbu2ss_ep0_in_transfer(
 	struct nbu2ss_req *req
 )
 {
-	u8		*pBuffer;			/* IN Data Buffer */
+	u8		*p_buffer;			/* IN Data Buffer */
 	u32		data;
-	u32		iRemainSize = 0;
+	u32		i_remain_size = 0;
 	int		result = 0;
 
 	/*-------------------------------------------------------------*/
@@ -705,25 +708,25 @@ static int _nbu2ss_ep0_in_transfer(
 	data &= ~(u32)EP0_INAK;
 	_nbu2ss_writel(&udc->p_regs->EP0_CONTROL, data);
 
-	iRemainSize = req->req.length - req->req.actual;
-	pBuffer = (u8 *)req->req.buf;
-	pBuffer += req->req.actual;
+	i_remain_size = req->req.length - req->req.actual;
+	p_buffer = (u8 *)req->req.buf;
+	p_buffer += req->req.actual;
 
 	/*-------------------------------------------------------------*/
 	/* Data transfer */
-	result = EP0_in_PIO(udc, pBuffer, iRemainSize);
+	result = EP0_in_PIO(udc, p_buffer, i_remain_size);
 
 	req->div_len = result;
-	iRemainSize -= result;
+	i_remain_size -= result;
 
-	if (iRemainSize == 0) {
+	if (i_remain_size == 0) {
 		EP0_send_NULL(udc, FALSE);
 		return result;
 	}
 
-	if ((iRemainSize < sizeof(u32)) && (result != EP0_PACKETSIZE)) {
-		pBuffer += result;
-		result += EP0_in_OverBytes(udc, pBuffer, iRemainSize);
+	if ((i_remain_size < sizeof(u32)) && (result != EP0_PACKETSIZE)) {
+		p_buffer += result;
+		result += ep0_in_overbytes(udc, p_buffer, i_remain_size);
 		req->div_len = result;
 	}
 
@@ -736,40 +739,40 @@ static int _nbu2ss_ep0_out_transfer(
 	struct nbu2ss_req *req
 )
 {
-	u8		*pBuffer;
-	u32		iRemainSize;
-	u32		iRecvLength;
+	u8		*p_buffer;
+	u32		i_remain_size;
+	u32		i_recv_length;
 	int		result = 0;
-	int		fRcvZero;
+	int		f_rcv_zero;
 
 	/*-------------------------------------------------------------*/
 	/* Receive data confirmation */
-	iRecvLength = _nbu2ss_readl(&udc->p_regs->EP0_LENGTH) & EP0_LDATA;
-	if (iRecvLength != 0) {
-		fRcvZero = 0;
+	i_recv_length = _nbu2ss_readl(&udc->p_regs->EP0_LENGTH) & EP0_LDATA;
+	if (i_recv_length != 0) {
+		f_rcv_zero = 0;
 
-		iRemainSize = req->req.length - req->req.actual;
-		pBuffer = (u8 *)req->req.buf;
-		pBuffer += req->req.actual;
+		i_remain_size = req->req.length - req->req.actual;
+		p_buffer = (u8 *)req->req.buf;
+		p_buffer += req->req.actual;
 
-		result = ep0_out_pio(udc, pBuffer
-					, min(iRemainSize, iRecvLength));
+		result = ep0_out_pio(udc, p_buffer
+					, min(i_remain_size, i_recv_length));
 		if (result < 0)
 			return result;
 
 		req->req.actual += result;
-		iRecvLength -= result;
+		i_recv_length -= result;
 
-		if ((iRecvLength > 0) && (iRecvLength < sizeof(u32))) {
-			pBuffer += result;
-			iRemainSize -= result;
+		if ((i_recv_length > 0) && (i_recv_length < sizeof(u32))) {
+			p_buffer += result;
+			i_remain_size -= result;
 
-			result = EP0_out_OverBytes(udc, pBuffer
-					, min(iRemainSize, iRecvLength));
+			result = ep0_out_overbytes(udc, p_buffer
+					, min(i_remain_size, i_recv_length));
 			req->req.actual += result;
 		}
 	} else {
-		fRcvZero = 1;
+		f_rcv_zero = 1;
 	}
 
 	/*-------------------------------------------------------------*/
@@ -794,9 +797,9 @@ static int _nbu2ss_ep0_out_transfer(
 		return -EOVERFLOW;
 	}
 
-	if (fRcvZero != 0) {
-		iRemainSize = _nbu2ss_readl(&udc->p_regs->EP0_CONTROL);
-		if (iRemainSize & EP0_ONAK) {
+	if (f_rcv_zero != 0) {
+		i_remain_size = _nbu2ss_readl(&udc->p_regs->EP0_CONTROL);
+		if (i_remain_size & EP0_ONAK) {
 			/*---------------------------------------------------*/
 			/* NACK release */
 			_nbu2ss_bitclr(&udc->p_regs->EP0_CONTROL, EP0_ONAK);
@@ -815,7 +818,7 @@ static int _nbu2ss_out_dma(
 	u32		length
 )
 {
-	dma_addr_t	pBuffer;
+	dma_addr_t	p_buffer;
 	u32		mpkt;
 	u32		lmpkt;
 	u32		dmacnt;
@@ -828,14 +831,14 @@ static int _nbu2ss_out_dma(
 		return 1;		/* DMA is forwarded */
 
 	req->dma_flag = TRUE;
-	pBuffer = req->req.dma;
-	pBuffer += req->req.actual;
+	p_buffer = req->req.dma;
+	p_buffer += req->req.actual;
 
 	/* DMA Address */
-	_nbu2ss_writel(&preg->EP_DCR[num].EP_TADR, (u32)pBuffer);
+	_nbu2ss_writel(&preg->EP_DCR[num].EP_TADR, (u32)p_buffer);
 
 	/* Number of transfer packets */
-	mpkt = _nbu2ss_readl(&preg->EP_REGS[num].EP_PCKT_ADRS) & EPn_MPKT;
+	mpkt = _nbu2ss_readl(&preg->EP_REGS[num].EP_PCKT_ADRS) & EPN_MPKT;
 	dmacnt = length / mpkt;
 	lmpkt = (length % mpkt) & ~(u32)0x03;
 
@@ -851,18 +854,18 @@ static int _nbu2ss_out_dma(
 	data = mpkt | (lmpkt << 16);
 	_nbu2ss_writel(&preg->EP_DCR[num].EP_DCR2, data);
 
-	data = ((dmacnt & 0xff) << 16) | DCR1_EPn_DIR0 | DCR1_EPn_REQEN;
+	data = ((dmacnt & 0xff) << 16) | DCR1_EPN_DIR0 | DCR1_EPN_REQEN;
 	_nbu2ss_writel(&preg->EP_DCR[num].EP_DCR1, data);
 
 	if (burst == 0) {
 		_nbu2ss_writel(&preg->EP_REGS[num].EP_LEN_DCNT, 0);
-		_nbu2ss_bitclr(&preg->EP_REGS[num].EP_DMA_CTRL, EPn_BURST_SET);
+		_nbu2ss_bitclr(&preg->EP_REGS[num].EP_DMA_CTRL, EPN_BURST_SET);
 	} else {
 		_nbu2ss_writel(&preg->EP_REGS[num].EP_LEN_DCNT
 				, (dmacnt << 16));
-		_nbu2ss_bitset(&preg->EP_REGS[num].EP_DMA_CTRL, EPn_BURST_SET);
+		_nbu2ss_bitset(&preg->EP_REGS[num].EP_DMA_CTRL, EPN_BURST_SET);
 	}
-	_nbu2ss_bitset(&preg->EP_REGS[num].EP_DMA_CTRL, EPn_DMA_EN);
+	_nbu2ss_bitset(&preg->EP_REGS[num].EP_DMA_CTRL, EPN_DMA_EN);
 
 	result = length & ~(u32)0x03;
 	req->div_len = result;
@@ -878,12 +881,12 @@ static int _nbu2ss_epn_out_pio(
 	u32		length
 )
 {
-	u8		*pBuffer;
+	u8		*p_buffer;
 	u32		i;
 	u32		data;
-	u32		iWordLength;
-	union usb_reg_access	Temp32;
-	union usb_reg_access	*pBuf32;
+	u32		i_word_length;
+	union usb_reg_access	temp_32;
+	union usb_reg_access	*p_buf_32;
 	int		result = 0;
 	struct fc_regs	*preg = udc->p_regs;
 
@@ -893,28 +896,29 @@ static int _nbu2ss_epn_out_pio(
 	if (length == 0)
 		return 0;
 
-	pBuffer = (u8 *)req->req.buf;
-	pBuf32 = (union usb_reg_access *)(pBuffer + req->req.actual);
+	p_buffer = (u8 *)req->req.buf;
+	p_buf_32 = (union usb_reg_access *)(p_buffer + req->req.actual);
 
-	iWordLength = length / sizeof(u32);
-	if (iWordLength > 0) {
+	i_word_length = length / sizeof(u32);
+	if (i_word_length > 0) {
 		/*---------------------------------------------------------*/
 		/* Copy of every four bytes */
-		for (i = 0; i < iWordLength; i++) {
-			pBuf32->dw =
+		for (i = 0; i < i_word_length; i++) {
+			p_buf_32->dw =
 			_nbu2ss_readl(&preg->EP_REGS[ep->epnum - 1].EP_READ);
-			pBuf32++;
+			p_buf_32++;
 		}
-		result = iWordLength * sizeof(u32);
+		result = i_word_length * sizeof(u32);
 	}
 
 	data = length - result;
 	if (data > 0) {
 		/*---------------------------------------------------------*/
 		/* Copy of fraction byte */
-		Temp32.dw = _nbu2ss_readl(&preg->EP_REGS[ep->epnum - 1].EP_READ);
+		temp_32.dw =
+			_nbu2ss_readl(&preg->EP_REGS[ep->epnum - 1].EP_READ);
 		for (i = 0 ; i < data ; i++)
-			pBuf32->byte.DATA[i] = Temp32.byte.DATA[i];
+			p_buf_32->byte.DATA[i] = temp_32.byte.DATA[i];
 		result += data;
 	}
 
@@ -937,7 +941,7 @@ static int _nbu2ss_epn_out_data(
 )
 {
 	u32		num;
-	u32		iBufSize;
+	u32		i_buf_size;
 	int		nret = 1;
 
 	if (ep->epnum == 0)
@@ -945,14 +949,14 @@ static int _nbu2ss_epn_out_data(
 
 	num = ep->epnum - 1;
 
-	iBufSize = min((req->req.length - req->req.actual), data_size);
+	i_buf_size = min((req->req.length - req->req.actual), data_size);
 
 	if ((ep->ep_type != USB_ENDPOINT_XFER_INT) && (req->req.dma != 0) &&
-	    (iBufSize  >= sizeof(u32))) {
-		nret = _nbu2ss_out_dma(udc, req, num, iBufSize);
+	    (i_buf_size  >= sizeof(u32))) {
+		nret = _nbu2ss_out_dma(udc, req, num, i_buf_size);
 	} else {
-		iBufSize = min_t(u32, iBufSize, ep->ep.maxpacket);
-		nret = _nbu2ss_epn_out_pio(udc, ep, req, iBufSize);
+		i_buf_size = min_t(u32, i_buf_size, ep->ep.maxpacket);
+		nret = _nbu2ss_epn_out_pio(udc, ep, req, i_buf_size);
 	}
 
 	return nret;
@@ -966,7 +970,7 @@ static int _nbu2ss_epn_out_transfer(
 )
 {
 	u32		num;
-	u32		iRecvLength;
+	u32		i_recv_length;
 	int		result = 1;
 	struct fc_regs	*preg = udc->p_regs;
 
@@ -977,13 +981,13 @@ static int _nbu2ss_epn_out_transfer(
 
 	/*-------------------------------------------------------------*/
 	/* Receive Length */
-	iRecvLength
-		= _nbu2ss_readl(&preg->EP_REGS[num].EP_LEN_DCNT) & EPn_LDATA;
+	i_recv_length
+		= _nbu2ss_readl(&preg->EP_REGS[num].EP_LEN_DCNT) & EPN_LDATA;
 
-	if (iRecvLength != 0) {
-		result = _nbu2ss_epn_out_data(udc, ep, req, iRecvLength);
-		if (iRecvLength < ep->ep.maxpacket) {
-			if (iRecvLength == result) {
+	if (i_recv_length != 0) {
+		result = _nbu2ss_epn_out_data(udc, ep, req, i_recv_length);
+		if (i_recv_length < ep->ep.maxpacket) {
+			if (i_recv_length == result) {
 				req->req.actual += result;
 				result = 0;
 			}
@@ -1023,11 +1027,11 @@ static int _nbu2ss_in_dma(
 	u32		length
 )
 {
-	dma_addr_t	pBuffer;
+	dma_addr_t	p_buffer;
 	u32		mpkt;		/* MaxPacketSize */
 	u32		lmpkt;		/* Last Packet Data Size */
 	u32		dmacnt;		/* IN Data Size */
-	u32		iWriteLength;
+	u32		i_write_length;
 	u32		data;
 	int		result = -EINVAL;
 	struct fc_regs	*preg = udc->p_regs;
@@ -1042,18 +1046,18 @@ static int _nbu2ss_in_dma(
 	req->dma_flag = TRUE;
 
 	/* MAX Packet Size */
-	mpkt = _nbu2ss_readl(&preg->EP_REGS[num].EP_PCKT_ADRS) & EPn_MPKT;
+	mpkt = _nbu2ss_readl(&preg->EP_REGS[num].EP_PCKT_ADRS) & EPN_MPKT;
 
 	if ((DMA_MAX_COUNT * mpkt) < length)
-		iWriteLength = DMA_MAX_COUNT * mpkt;
+		i_write_length = DMA_MAX_COUNT * mpkt;
 	else
-		iWriteLength = length;
+		i_write_length = length;
 
 	/*------------------------------------------------------------*/
 	/* Number of transmission packets */
-	if (mpkt < iWriteLength) {
-		dmacnt = iWriteLength / mpkt;
-		lmpkt  = (iWriteLength % mpkt) & ~(u32)0x3;
+	if (mpkt < i_write_length) {
+		dmacnt = i_write_length / mpkt;
+		lmpkt  = (i_write_length % mpkt) & ~(u32)0x3;
 		if (lmpkt != 0)
 			dmacnt++;
 		else
@@ -1061,7 +1065,7 @@ static int _nbu2ss_in_dma(
 
 	} else {
 		dmacnt = 1;
-		lmpkt  = iWriteLength & ~(u32)0x3;
+		lmpkt  = i_write_length & ~(u32)0x3;
 	}
 
 	/* Packet setting */
@@ -1069,12 +1073,12 @@ static int _nbu2ss_in_dma(
 	_nbu2ss_writel(&preg->EP_DCR[num].EP_DCR2, data);
 
 	/* Address setting */
-	pBuffer = req->req.dma;
-	pBuffer += req->req.actual;
-	_nbu2ss_writel(&preg->EP_DCR[num].EP_TADR, (u32)pBuffer);
+	p_buffer = req->req.dma;
+	p_buffer += req->req.actual;
+	_nbu2ss_writel(&preg->EP_DCR[num].EP_TADR, (u32)p_buffer);
 
 	/* Packet and DMA setting */
-	data = ((dmacnt & 0xff) << 16) | DCR1_EPn_REQEN;
+	data = ((dmacnt & 0xff) << 16) | DCR1_EPN_REQEN;
 	_nbu2ss_writel(&preg->EP_DCR[num].EP_DCR1, data);
 
 	/* Packet setting of EPC */
@@ -1082,9 +1086,9 @@ static int _nbu2ss_in_dma(
 	_nbu2ss_writel(&preg->EP_REGS[num].EP_LEN_DCNT, data);
 
 	/*DMA setting of EPC */
-	_nbu2ss_bitset(&preg->EP_REGS[num].EP_DMA_CTRL, EPn_DMA_EN);
+	_nbu2ss_bitset(&preg->EP_REGS[num].EP_DMA_CTRL, EPN_DMA_EN);
 
-	result = iWriteLength & ~(u32)0x3;
+	result = i_write_length & ~(u32)0x3;
 	req->div_len = result;
 
 	return result;
@@ -1098,12 +1102,12 @@ static int _nbu2ss_epn_in_pio(
 	u32		length
 )
 {
-	u8		*pBuffer;
+	u8		*p_buffer;
 	u32		i;
 	u32		data;
-	u32		iWordLength;
-	union usb_reg_access	Temp32;
-	union usb_reg_access	*pBuf32 = NULL;
+	u32		i_word_length;
+	union usb_reg_access	temp_32;
+	union usb_reg_access	*p_buf_32 = NULL;
 	int		result = 0;
 	struct fc_regs	*preg = udc->p_regs;
 
@@ -1111,30 +1115,30 @@ static int _nbu2ss_epn_in_pio(
 		return 1;		/* DMA is forwarded */
 
 	if (length > 0) {
-		pBuffer = (u8 *)req->req.buf;
-		pBuf32 = (union usb_reg_access *)(pBuffer + req->req.actual);
+		p_buffer = (u8 *)req->req.buf;
+		p_buf_32 = (union usb_reg_access *)(p_buffer + req->req.actual);
 
-		iWordLength = length / sizeof(u32);
-		if (iWordLength > 0) {
-			for (i = 0; i < iWordLength; i++) {
+		i_word_length = length / sizeof(u32);
+		if (i_word_length > 0) {
+			for (i = 0; i < i_word_length; i++) {
 				_nbu2ss_writel(
 					&preg->EP_REGS[ep->epnum - 1].EP_WRITE
-					, pBuf32->dw
+					, p_buf_32->dw
 				);
 
-				pBuf32++;
+				p_buf_32++;
 			}
-			result = iWordLength * sizeof(u32);
+			result = i_word_length * sizeof(u32);
 		}
 	}
 
 	if (result != ep->ep.maxpacket) {
 		data = length - result;
-		Temp32.dw = 0;
+		temp_32.dw = 0;
 		for (i = 0 ; i < data ; i++)
-			Temp32.byte.DATA[i] = pBuf32->byte.DATA[i];
+			temp_32.byte.DATA[i] = p_buf_32->byte.DATA[i];
 
-		_nbu2ss_ep_in_end(udc, ep->epnum, Temp32.dw, data);
+		_nbu2ss_ep_in_end(udc, ep->epnum, temp_32.dw, data);
 		result += data;
 	}
 
@@ -1178,7 +1182,7 @@ static int _nbu2ss_epn_in_transfer(
 )
 {
 	u32		num;
-	u32		iBufSize;
+	u32		i_buf_size;
 	int		result = 0;
 	u32		status;
 
@@ -1192,19 +1196,19 @@ static int _nbu2ss_epn_in_transfer(
 	/*-------------------------------------------------------------*/
 	/* State confirmation of FIFO */
 	if (req->req.actual == 0) {
-		if ((status & EPn_IN_EMPTY) == 0)
+		if ((status & EPN_IN_EMPTY) == 0)
 			return 1;	/* Not Empty */
 
 	} else {
-		if ((status & EPn_IN_FULL) != 0)
+		if ((status & EPN_IN_FULL) != 0)
 			return 1;	/* Not Empty */
 	}
 
 	/*-------------------------------------------------------------*/
 	/* Start transfer */
-	iBufSize = req->req.length - req->req.actual;
-	if (iBufSize > 0)
-		result = _nbu2ss_epn_in_data(udc, ep, req, iBufSize);
+	i_buf_size = req->req.length - req->req.actual;
+	if (i_buf_size > 0)
+		result = _nbu2ss_epn_in_data(udc, ep, req, i_buf_size);
 	else if (req->req.length == 0)
 		_nbu2ss_zero_len_pkt(udc, ep->epnum);
 
@@ -1252,7 +1256,7 @@ static int _nbu2ss_start_transfer(
 		}
 
 	} else {
-		/* EPn */
+		/* EPN */
 		if (ep->direct == USB_DIR_OUT) {
 			/* OUT */
 			if (!bflag)
@@ -1281,7 +1285,7 @@ static void _nbu2ss_restert_transfer(struct nbu2ss_ep *ep)
 		length = _nbu2ss_readl(
 			&ep->udc->p_regs->EP_REGS[ep->epnum - 1].EP_LEN_DCNT);
 
-		length &= EPn_LDATA;
+		length &= EPN_LDATA;
 		if (length < ep->ep.maxpacket)
 			bflag = TRUE;
 	}
@@ -1304,9 +1308,9 @@ static void _nbu2ss_endpoint_toggle_reset(
 	num = (ep_adrs & 0x7F) - 1;
 
 	if (ep_adrs & USB_DIR_IN)
-		data = EPn_IPIDCLR;
+		data = EPN_IPIDCLR;
 	else
-		data = EPn_BCLR | EPn_OPIDCLR;
+		data = EPN_BCLR | EPN_OPIDCLR;
 
 	_nbu2ss_bitset(&udc->p_regs->EP_REGS[num].EP_CONTROL, data);
 }
@@ -1341,9 +1345,9 @@ static void _nbu2ss_set_endpoint_stall(
 			ep->halted = TRUE;
 
 			if (ep_adrs & USB_DIR_IN)
-				data = EPn_BCLR | EPn_ISTL;
+				data = EPN_BCLR | EPN_ISTL;
 			else
-				data = EPn_OSTL_EN | EPn_OSTL;
+				data = EPN_OSTL_EN | EPN_OSTL;
 
 			_nbu2ss_bitset(&preg->EP_REGS[num].EP_CONTROL, data);
 		} else {
@@ -1351,13 +1355,13 @@ static void _nbu2ss_set_endpoint_stall(
 			ep->stalled = FALSE;
 			if (ep_adrs & USB_DIR_IN) {
 				_nbu2ss_bitclr(&preg->EP_REGS[num].EP_CONTROL
-						, EPn_ISTL);
+						, EPN_ISTL);
 			} else {
 				data =
 				_nbu2ss_readl(&preg->EP_REGS[num].EP_CONTROL);
 
-				data &= ~EPn_OSTL;
-				data |= EPn_OSTL_EN;
+				data &= ~EPN_OSTL;
+				data |= EPN_OSTL_EN;
 
 				_nbu2ss_writel(&preg->EP_REGS[num].EP_CONTROL
 						, data);
@@ -1453,13 +1457,13 @@ static int _nbu2ss_get_ep_stall(struct nbu2ss_udc *udc, u8 ep_adrs)
 
 	} else {
 		data = _nbu2ss_readl(&preg->EP_REGS[epnum - 1].EP_CONTROL);
-		if ((data & EPn_EN) == 0)
+		if ((data & EPN_EN) == 0)
 			return -1;
 
 		if (ep_adrs & USB_ENDPOINT_DIR_MASK)
-			bit_data = EPn_ISTL;
+			bit_data = EPN_ISTL;
 		else
-			bit_data = EPn_OSTL;
+			bit_data = EPN_OSTL;
 	}
 
 	if ((data & bit_data) == 0)
@@ -1548,7 +1552,7 @@ static void _nbu2ss_epn_set_stall(
 			regdata = _nbu2ss_readl(
 				&preg->EP_REGS[ep->epnum - 1].EP_STATUS);
 
-			if ((regdata & EPn_IN_DATA) == 0)
+			if ((regdata & EPN_IN_DATA) == 0)
 				break;
 
 			mdelay(1);
@@ -1651,7 +1655,7 @@ static int std_req_set_address(struct nbu2ss_udc *udc)
 /*-------------------------------------------------------------------------*/
 static int std_req_set_configuration(struct nbu2ss_udc *udc)
 {
-	u32 ConfigValue = (u32)(udc->ctrl.wValue & 0x00ff);
+	u32 config_value = (u32)(udc->ctrl.wValue & 0x00ff);
 
 	if ((udc->ctrl.wIndex != 0x0000)	||
 	    (udc->ctrl.wLength != 0x0000)	||
@@ -1659,9 +1663,9 @@ static int std_req_set_configuration(struct nbu2ss_udc *udc)
 		return -EINVAL;
 	}
 
-	udc->curr_config = ConfigValue;
+	udc->curr_config = config_value;
 
-	if (ConfigValue > 0) {
+	if (config_value > 0) {
 		_nbu2ss_bitset(&udc->p_regs->USB_CONTROL, CONF);
 		udc->devstate = USB_STATE_CONFIGURED;
 
@@ -1968,7 +1972,7 @@ static inline void _nbu2ss_epn_in_int(
 			status =
 			_nbu2ss_readl(&preg->EP_REGS[ep->epnum - 1].EP_STATUS);
 
-			if ((status & EPn_IN_FULL) == 0) {
+			if ((status & EPN_IN_FULL) == 0) {
 				/*-----------------------------------------*/
 				/* 0 Length Packet */
 				req->zero = false;
@@ -2059,18 +2063,18 @@ static inline void _nbu2ss_epn_out_dma_int(
 	}
 
 	ep_dmacnt = _nbu2ss_readl(&preg->EP_REGS[num].EP_LEN_DCNT)
-		 & EPn_DMACNT;
+		 & EPN_DMACNT;
 	ep_dmacnt >>= 16;
 
 	for (i = 0; i < EPC_PLL_LOCK_COUNT; i++) {
 		dmacnt = _nbu2ss_readl(&preg->EP_DCR[num].EP_DCR1)
-			 & DCR1_EPn_DMACNT;
+			 & DCR1_EPN_DMACNT;
 		dmacnt >>= 16;
 		if (ep_dmacnt == dmacnt)
 			break;
 	}
 
-	_nbu2ss_bitclr(&preg->EP_DCR[num].EP_DCR1, DCR1_EPn_REQEN);
+	_nbu2ss_bitclr(&preg->EP_DCR[num].EP_DCR1, DCR1_EPN_REQEN);
 
 	if (dmacnt != 0) {
 		mpkt = ep->ep.maxpacket;
@@ -2117,20 +2121,20 @@ static inline void _nbu2ss_epn_int(struct nbu2ss_udc *udc, u32 epnum)
 		return;
 	}
 
-	if (status & EPn_OUT_END_INT) {
-		status &= ~EPn_OUT_INT;
+	if (status & EPN_OUT_END_INT) {
+		status &= ~EPN_OUT_INT;
 		_nbu2ss_epn_out_dma_int(udc, ep, req);
 	}
 
-	if (status & EPn_OUT_INT)
+	if (status & EPN_OUT_INT)
 		_nbu2ss_epn_out_int(udc, ep, req);
 
-	if (status & EPn_IN_END_INT) {
-		status &= ~EPn_IN_INT;
+	if (status & EPN_IN_END_INT) {
+		status &= ~EPN_IN_INT;
 		_nbu2ss_epn_in_dma_int(udc, ep, req);
 	}
 
-	if (status & EPn_IN_INT)
+	if (status & EPN_IN_INT)
 		_nbu2ss_epn_in_int(udc, ep, req);
 }
 
@@ -2231,9 +2235,9 @@ static void _nbu2ss_fifo_flush(struct nbu2ss_udc *udc, struct nbu2ss_ep *ep)
 		_nbu2ss_bitset(&p->EP0_CONTROL, EP0_BCLR);
 
 	} else {
-		/* EPn */
+		/* EPN */
 		_nbu2ss_ep_dma_abort(udc, ep);
-		_nbu2ss_bitset(&p->EP_REGS[ep->epnum - 1].EP_CONTROL, EPn_BCLR);
+		_nbu2ss_bitset(&p->EP_REGS[ep->epnum - 1].EP_CONTROL, EPN_BCLR);
 	}
 }
 
@@ -2478,7 +2482,7 @@ static irqreturn_t _nbu2ss_udc_irq(int irq, void *_udc)
 			suspend_flag = 1;
 		}
 
-		if (status & EPn_INT) {
+		if (status & EPN_INT) {
 			/* EP INT */
 			int_bit = status >> 8;
 
@@ -2651,7 +2655,9 @@ static int nbu2ss_ep_queue(
 	}
 
 	req = container_of(_req, struct nbu2ss_req, req);
-	if (unlikely(!_req->complete || !_req->buf || !list_empty(&req->queue))) {
+	if (unlikely(!_req->complete ||
+		     !_req->buf ||
+		     !list_empty(&req->queue))) {
 		if (!_req->complete)
 			pr_err("udc: %s --- !_req->complete\n", __func__);
 
@@ -2868,7 +2874,7 @@ static int nbu2ss_ep_fifo_status(struct usb_ep *_ep)
 
 	} else {
 		data = _nbu2ss_readl(&preg->EP_REGS[ep->epnum - 1].EP_LEN_DCNT)
-			& EPn_LDATA;
+			& EPN_LDATA;
 	}
 
 	spin_unlock_irqrestore(&udc->lock, flags);
diff --git a/drivers/staging/emxx_udc/emxx_udc.h b/drivers/staging/emxx_udc/emxx_udc.h
index 78c08e15a1f9..928d531a5115 100644
--- a/drivers/staging/emxx_udc/emxx_udc.h
+++ b/drivers/staging/emxx_udc/emxx_udc.h
@@ -144,7 +144,7 @@
 /*------- (0x001C) Setup Data 1 Register */
 
 /*------- (0x0020) USB Interrupt Status Register */
-#define EPn_INT				0x00FFFF00
+#define EPN_INT				0x00FFFF00
 #define EP15_INT			BIT23
 #define EP14_INT			BIT22
 #define EP13_INT			BIT21
@@ -264,102 +264,102 @@
 /*------- (0x0038) EP0 Read Register */
 /*------- (0x003C) EP0 Write Register */
 
-/*------- (0x0040:) EPn Control Register */
-#define EPn_EN				BIT31
-#define EPn_BUF_TYPE			BIT30
-#define EPn_BUF_SINGLE			BIT30
-
-#define EPn_DIR0			BIT26
-#define EPn_MODE			(BIT25 + BIT24)
-#define EPn_BULK			0
-#define EPn_INTERRUPT			BIT24
-#define EPn_ISO				BIT25
-
-#define EPn_OVERSEL			BIT17
-#define EPn_AUTO			BIT16
-
-#define EPn_IPIDCLR			BIT11
-#define EPn_OPIDCLR			BIT10
-#define EPn_BCLR			BIT09
-#define EPn_CBCLR			BIT08
-#define EPn_DEND			BIT07
-#define EPn_DW				(BIT06 + BIT05)
-#define EPn_DW4				0
-#define EPn_DW3				(BIT06 + BIT05)
-#define EPn_DW2				BIT06
-#define EPn_DW1				BIT05
-
-#define EPn_OSTL_EN			BIT04
-#define EPn_ISTL			BIT03
-#define EPn_OSTL			BIT02
-
-#define EPn_ONAK			BIT00
-
-/*------- (0x0044:) EPn Status Register	*/
-#define EPn_ISO_PIDERR			BIT29		/* R */
-#define EPn_OPID			BIT28		/* R */
-#define EPn_OUT_NOTKN			BIT27		/* R */
-#define EPn_ISO_OR			BIT26		/* R */
-
-#define EPn_ISO_CRC			BIT24		/* R */
-#define EPn_OUT_END_INT			BIT23		/* RW */
-#define EPn_OUT_OR_INT			BIT22		/* RW */
-#define EPn_OUT_NAK_ERR_INT		BIT21		/* RW */
-#define EPn_OUT_STALL_INT		BIT20		/* RW */
-#define EPn_OUT_INT			BIT19		/* RW */
-#define EPn_OUT_NULL_INT		BIT18		/* RW */
-#define EPn_OUT_FULL			BIT17		/* R */
-#define EPn_OUT_EMPTY			BIT16		/* R */
-
-#define EPn_IPID			BIT10		/* R */
-#define EPn_IN_NOTKN			BIT09		/* R */
-#define EPn_ISO_UR			BIT08		/* R */
-#define EPn_IN_END_INT			BIT07		/* RW */
-
-#define EPn_IN_NAK_ERR_INT		BIT05		/* RW */
-#define EPn_IN_STALL_INT		BIT04		/* RW */
-#define EPn_IN_INT			BIT03		/* RW */
-#define EPn_IN_DATA			BIT02		/* R */
-#define EPn_IN_FULL			BIT01		/* R */
-#define EPn_IN_EMPTY			BIT00		/* R */
-
-#define EPn_INT_EN	\
-	(EPn_OUT_END_INT | EPn_OUT_INT | EPn_IN_END_INT | EPn_IN_INT)
-
-/*------- (0x0048:) EPn Interrupt Enable Register */
-#define EPn_OUT_END_EN			BIT23		/* RW */
-#define EPn_OUT_OR_EN			BIT22		/* RW */
-#define EPn_OUT_NAK_ERR_EN		BIT21		/* RW */
-#define EPn_OUT_STALL_EN		BIT20		/* RW */
-#define EPn_OUT_EN			BIT19		/* RW */
-#define EPn_OUT_NULL_EN			BIT18		/* RW */
-
-#define EPn_IN_END_EN			BIT07		/* RW */
-
-#define EPn_IN_NAK_ERR_EN		BIT05		/* RW */
-#define EPn_IN_STALL_EN			BIT04		/* RW */
-#define EPn_IN_EN			BIT03		/* RW */
-
-/*------- (0x004C:) EPn Interrupt Enable Register */
-#define EPn_STOP_MODE			BIT11
-#define EPn_DEND_SET			BIT10
-#define EPn_BURST_SET			BIT09
-#define EPn_STOP_SET			BIT08
-
-#define EPn_DMA_EN			BIT04
-
-#define EPn_DMAMODE0			BIT00
-
-/*------- (0x0050:) EPn MaxPacket & BaseAddress Register */
-#define EPn_BASEAD			0x1FFF0000
-#define EPn_MPKT			0x000007FF
-
-/*------- (0x0054:) EPn Length & DMA Count Register */
-#define EPn_DMACNT			0x01FF0000
-#define EPn_LDATA			0x000007FF
-
-/*------- (0x0058:) EPn Read Register */
-/*------- (0x005C:) EPn Write Register */
+/*------- (0x0040:) EPN Control Register */
+#define EPN_EN				BIT31
+#define EPN_BUF_TYPE			BIT30
+#define EPN_BUF_SINGLE			BIT30
+
+#define EPN_DIR0			BIT26
+#define EPN_MODE			(BIT25 + BIT24)
+#define EPN_BULK			0
+#define EPN_INTERRUPT			BIT24
+#define EPN_ISO				BIT25
+
+#define EPN_OVERSEL			BIT17
+#define EPN_AUTO			BIT16
+
+#define EPN_IPIDCLR			BIT11
+#define EPN_OPIDCLR			BIT10
+#define EPN_BCLR			BIT09
+#define EPN_CBCLR			BIT08
+#define EPN_DEND			BIT07
+#define EPN_DW				(BIT06 + BIT05)
+#define EPN_DW4				0
+#define EPN_DW3				(BIT06 + BIT05)
+#define EPN_DW2				BIT06
+#define EPN_DW1				BIT05
+
+#define EPN_OSTL_EN			BIT04
+#define EPN_ISTL			BIT03
+#define EPN_OSTL			BIT02
+
+#define EPN_ONAK			BIT00
+
+/*------- (0x0044:) EPN Status Register	*/
+#define EPN_ISO_PIDERR			BIT29		/* R */
+#define EPN_OPID			BIT28		/* R */
+#define EPN_OUT_NOTKN			BIT27		/* R */
+#define EPN_ISO_OR			BIT26		/* R */
+
+#define EPN_ISO_CRC			BIT24		/* R */
+#define EPN_OUT_END_INT			BIT23		/* RW */
+#define EPN_OUT_OR_INT			BIT22		/* RW */
+#define EPN_OUT_NAK_ERR_INT		BIT21		/* RW */
+#define EPN_OUT_STALL_INT		BIT20		/* RW */
+#define EPN_OUT_INT			BIT19		/* RW */
+#define EPN_OUT_NULL_INT		BIT18		/* RW */
+#define EPN_OUT_FULL			BIT17		/* R */
+#define EPN_OUT_EMPTY			BIT16		/* R */
+
+#define EPN_IPID			BIT10		/* R */
+#define EPN_IN_NOTKN			BIT09		/* R */
+#define EPN_ISO_UR			BIT08		/* R */
+#define EPN_IN_END_INT			BIT07		/* RW */
+
+#define EPN_IN_NAK_ERR_INT		BIT05		/* RW */
+#define EPN_IN_STALL_INT		BIT04		/* RW */
+#define EPN_IN_INT			BIT03		/* RW */
+#define EPN_IN_DATA			BIT02		/* R */
+#define EPN_IN_FULL			BIT01		/* R */
+#define EPN_IN_EMPTY			BIT00		/* R */
+
+#define EPN_INT_EN	\
+	(EPN_OUT_END_INT | EPN_OUT_INT | EPN_IN_END_INT | EPN_IN_INT)
+
+/*------- (0x0048:) EPN Interrupt Enable Register */
+#define EPN_OUT_END_EN			BIT23		/* RW */
+#define EPN_OUT_OR_EN			BIT22		/* RW */
+#define EPN_OUT_NAK_ERR_EN		BIT21		/* RW */
+#define EPN_OUT_STALL_EN		BIT20		/* RW */
+#define EPN_OUT_EN			BIT19		/* RW */
+#define EPN_OUT_NULL_EN			BIT18		/* RW */
+
+#define EPN_IN_END_EN			BIT07		/* RW */
+
+#define EPN_IN_NAK_ERR_EN		BIT05		/* RW */
+#define EPN_IN_STALL_EN			BIT04		/* RW */
+#define EPN_IN_EN			BIT03		/* RW */
+
+/*------- (0x004C:) EPN Interrupt Enable Register */
+#define EPN_STOP_MODE			BIT11
+#define EPN_DEND_SET			BIT10
+#define EPN_BURST_SET			BIT09
+#define EPN_STOP_SET			BIT08
+
+#define EPN_DMA_EN			BIT04
+
+#define EPN_DMAMODE0			BIT00
+
+/*------- (0x0050:) EPN MaxPacket & BaseAddress Register */
+#define EPN_BASEAD			0x1FFF0000
+#define EPN_MPKT			0x000007FF
+
+/*------- (0x0054:) EPN Length & DMA Count Register */
+#define EPN_DMACNT			0x01FF0000
+#define EPN_LDATA			0x000007FF
+
+/*------- (0x0058:) EPN Read Register */
+/*------- (0x005C:) EPN Write Register */
 
 /*------- (0x1000) AHBSCTR Register */
 #define WAIT_MODE			BIT00
@@ -428,19 +428,19 @@
 #define EP_AVAILABLE			0xFFFF0000	/* R */
 #define DMA_AVAILABLE			0x0000FFFF	/* R */
 
-/*------- (0x1110:) EPnDCR1 Register */
-#define DCR1_EPn_DMACNT			0x00FF0000	/* RW */
+/*------- (0x1110:) EPNDCR1 Register */
+#define DCR1_EPN_DMACNT			0x00FF0000	/* RW */
 
-#define DCR1_EPn_DIR0			BIT01		/* RW */
-#define DCR1_EPn_REQEN			BIT00		/* RW */
+#define DCR1_EPN_DIR0			BIT01		/* RW */
+#define DCR1_EPN_REQEN			BIT00		/* RW */
 
-/*------- (0x1114:) EPnDCR2 Register */
-#define DCR2_EPn_LMPKT			0x07FF0000	/* RW */
+/*------- (0x1114:) EPNDCR2 Register */
+#define DCR2_EPN_LMPKT			0x07FF0000	/* RW */
 
-#define DCR2_EPn_MPKT			0x000007FF	/* RW */
+#define DCR2_EPN_MPKT			0x000007FF	/* RW */
 
-/*------- (0x1118:) EPnTADR Register */
-#define EPn_TADR			0xFFFFFFFF	/* RW */
+/*------- (0x1118:) EPNTADR Register */
+#define EPN_TADR			0xFFFFFFFF	/* RW */
 
 /*===========================================================================*/
 /* Struct */
@@ -471,7 +471,7 @@ struct fc_regs {
 	u32 USB_ADDRESS;		/* (0x0008) USB Address */
 	u32 UTMI_CHARACTER_1;		/* (0x000C) UTMI Setting */
 	u32 TEST_CONTROL;		/* (0x0010) TEST Control */
-	u32 Reserved_14;		/* (0x0014) Reserved */
+	u32 reserved_14;		/* (0x0014) Reserved */
 	u32 SETUP_DATA0;		/* (0x0018) Setup Data0 */
 	u32 SETUP_DATA1;		/* (0x001C) Setup Data1 */
 	u32 USB_INT_STA;		/* (0x0020) USB Interrupt Status */
@@ -485,7 +485,7 @@ struct fc_regs {
 
 	struct ep_regs EP_REGS[REG_EP_NUM];	/* Endpoint Register */
 
-	u8 Reserved220[0x1000 - 0x220];	/* (0x0220:0x0FFF) Reserved */
+	u8 reserved_220[0x1000 - 0x220];	/* (0x0220:0x0FFF) Reserved */
 
 	u32 AHBSCTR;			/* (0x1000) AHBSCTR */
 	u32 AHBMCTR;			/* (0x1004) AHBMCTR */
@@ -494,25 +494,25 @@ struct fc_regs {
 	u32 EPCTR;			/* (0x1010) EPCTR */
 	u32 USBF_EPTEST;		/* (0x1014) USBF_EPTEST */
 
-	u8 Reserved1018[0x20 - 0x18];	/* (0x1018:0x101F) Reserved */
+	u8 reserved_1018[0x20 - 0x18];	/* (0x1018:0x101F) Reserved */
 
 	u32 USBSSVER;			/* (0x1020) USBSSVER */
 	u32 USBSSCONF;			/* (0x1024) USBSSCONF */
 
-	u8 Reserved1028[0x110 - 0x28];	/* (0x1028:0x110F) Reserved */
+	u8 reserved_1028[0x110 - 0x28];	/* (0x1028:0x110F) Reserved */
 
 	struct ep_dcr EP_DCR[REG_EP_NUM];	/* */
 
-	u8 Reserved1200[0x1000 - 0x200];	/* Reserved */
+	u8 reserved_1200[0x1000 - 0x200];	/* Reserved */
 } __aligned(32);
 
 #define EP0_PACKETSIZE			64
 #define EP_PACKETSIZE			1024
 
-/* EPn RAM SIZE */
+/* EPN RAM SIZE */
 #define D_RAM_SIZE_CTRL			64
 
-/* EPn Bulk Endpoint Max Packet Size */
+/* EPN Bulk Endpoint Max Packet Size */
 #define D_FS_RAM_SIZE_BULK		64
 #define D_HS_RAM_SIZE_BULK		512
 
diff --git a/drivers/staging/fbtft/fb_agm1264k-fl.c b/drivers/staging/fbtft/fb_agm1264k-fl.c
index 489151a6bf80..456a8dd65caf 100644
--- a/drivers/staging/fbtft/fb_agm1264k-fl.c
+++ b/drivers/staging/fbtft/fb_agm1264k-fl.c
@@ -282,10 +282,10 @@ static void iterate_diffusion_matrix(u32 xres, u32 yres, int x,
 				continue;
 			write_pos = &convert_buf[(y + j) * xres + x + i];
 			coeff = diffusing_matrix[i][j];
-			if (-1 == coeff)
+			if (-1 == coeff) {
 				/* pixel itself */
 				*write_pos = pixel;
-			else {
+			} else {
 				signed short p = *write_pos + error * coeff;
 
 				if (p > WHITE)
diff --git a/drivers/staging/fbtft/fb_hx8340bn.c b/drivers/staging/fbtft/fb_hx8340bn.c
index 1ca1fcd353d6..fbd5ef525243 100644
--- a/drivers/staging/fbtft/fb_hx8340bn.c
+++ b/drivers/staging/fbtft/fb_hx8340bn.c
@@ -157,7 +157,7 @@ static int set_var(struct fbtft_par *par)
  *   OP0 OP1 CP0 CP1 CP2 CP3 CP4 MP0 MP1 MP2 MP3 MP4 MP5 CGM0 CGM1
  *   ON0 ON1 CN0 CN1 CN2 CN3 CN4 MN0 MN1 MN2 MN3 MN4 MN5 XXXX  GC
  */
-#define CURVE(num, idx)  curves[num * par->gamma.num_values + idx]
+#define CURVE(num, idx)  curves[(num) * par->gamma.num_values + (idx)]
 static int set_gamma(struct fbtft_par *par, u32 *curves)
 {
 	unsigned long mask[] = {
diff --git a/drivers/staging/fbtft/fbtft-io.c b/drivers/staging/fbtft/fbtft-io.c
index d86840548b74..ffb9a3b4d454 100644
--- a/drivers/staging/fbtft/fbtft-io.c
+++ b/drivers/staging/fbtft/fbtft-io.c
@@ -71,7 +71,7 @@ int fbtft_write_spi_emulate_9(struct fbtft_par *par, void *buf, size_t len)
 			src++;
 		}
 		tmp |= ((*src & 0x0100) ? 1 : 0);
-		*(u64 *)dst = cpu_to_be64(tmp);
+		*(__be64 *)dst = cpu_to_be64(tmp);
 		dst += 8;
 		*dst++ = (u8)(*src++ & 0x00FF);
 		added++;
diff --git a/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.c b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.c
index 6f9eed66c64d..b9a0a315e6fb 100644
--- a/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.c
+++ b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.c
@@ -37,9 +37,9 @@
 #include <linux/interrupt.h>
 #include <linux/msi.h>
 #include <linux/kthread.h>
+#include <linux/iommu.h>
 
 #include "../../fsl-mc/include/mc.h"
-#include "../../fsl-mc/include/mc-sys.h"
 #include "dpaa2-eth.h"
 
 /* CREATE_TRACE_POINTS only needs to be defined once. Other dpa files
@@ -54,6 +54,16 @@ MODULE_DESCRIPTION("Freescale DPAA2 Ethernet Driver");
 
 const char dpaa2_eth_drv_version[] = "0.1";
 
+static void *dpaa2_iova_to_virt(struct iommu_domain *domain,
+				dma_addr_t iova_addr)
+{
+	phys_addr_t phys_addr;
+
+	phys_addr = domain ? iommu_iova_to_phys(domain, iova_addr) : iova_addr;
+
+	return phys_to_virt(phys_addr);
+}
+
 static void validate_rx_csum(struct dpaa2_eth_priv *priv,
 			     u32 fd_status,
 			     struct sk_buff *skb)
@@ -98,12 +108,11 @@ static void free_rx_fd(struct dpaa2_eth_priv *priv,
 	sgt = vaddr + dpaa2_fd_get_offset(fd);
 	for (i = 0; i < DPAA2_ETH_MAX_SG_ENTRIES; i++) {
 		addr = dpaa2_sg_get_addr(&sgt[i]);
+		sg_vaddr = dpaa2_iova_to_virt(priv->iommu_domain, addr);
 		dma_unmap_single(dev, addr, DPAA2_ETH_RX_BUF_SIZE,
 				 DMA_FROM_DEVICE);
 
-		sg_vaddr = phys_to_virt(addr);
 		skb_free_frag(sg_vaddr);
-
 		if (dpaa2_sg_is_final(&sgt[i]))
 			break;
 	}
@@ -159,10 +168,10 @@ static struct sk_buff *build_frag_skb(struct dpaa2_eth_priv *priv,
 
 		/* Get the address and length from the S/G entry */
 		sg_addr = dpaa2_sg_get_addr(sge);
+		sg_vaddr = dpaa2_iova_to_virt(priv->iommu_domain, sg_addr);
 		dma_unmap_single(dev, sg_addr, DPAA2_ETH_RX_BUF_SIZE,
 				 DMA_FROM_DEVICE);
 
-		sg_vaddr = phys_to_virt(sg_addr);
 		sg_length = dpaa2_sg_get_len(sge);
 
 		if (i == 0) {
@@ -217,16 +226,19 @@ static void dpaa2_eth_rx(struct dpaa2_eth_priv *priv,
 	struct dpaa2_eth_drv_stats *percpu_extras;
 	struct device *dev = priv->net_dev->dev.parent;
 	struct dpaa2_fas *fas;
+	void *buf_data;
 	u32 status = 0;
 
 	/* Tracing point */
 	trace_dpaa2_rx_fd(priv->net_dev, fd);
 
+	vaddr = dpaa2_iova_to_virt(priv->iommu_domain, addr);
 	dma_unmap_single(dev, addr, DPAA2_ETH_RX_BUF_SIZE, DMA_FROM_DEVICE);
-	vaddr = phys_to_virt(addr);
 
-	prefetch(vaddr + priv->buf_layout.private_data_size);
-	prefetch(vaddr + dpaa2_fd_get_offset(fd));
+	fas = dpaa2_get_fas(vaddr);
+	prefetch(fas);
+	buf_data = vaddr + dpaa2_fd_get_offset(fd);
+	prefetch(buf_data);
 
 	percpu_stats = this_cpu_ptr(priv->percpu_stats);
 	percpu_extras = this_cpu_ptr(priv->percpu_extras);
@@ -234,9 +246,7 @@ static void dpaa2_eth_rx(struct dpaa2_eth_priv *priv,
 	if (fd_format == dpaa2_fd_single) {
 		skb = build_linear_skb(priv, ch, fd, vaddr);
 	} else if (fd_format == dpaa2_fd_sg) {
-		struct dpaa2_sg_entry *sgt =
-				vaddr + dpaa2_fd_get_offset(fd);
-		skb = build_frag_skb(priv, ch, sgt);
+		skb = build_frag_skb(priv, ch, buf_data);
 		skb_free_frag(vaddr);
 		percpu_extras->rx_sg_frames++;
 		percpu_extras->rx_sg_bytes += dpaa2_fd_get_len(fd);
@@ -252,8 +262,6 @@ static void dpaa2_eth_rx(struct dpaa2_eth_priv *priv,
 
 	/* Check if we need to validate the L4 csum */
 	if (likely(dpaa2_fd_get_frc(fd) & DPAA2_FD_FRC_FASV)) {
-		fas = (struct dpaa2_fas *)
-				(vaddr + priv->buf_layout.private_data_size);
 		status = le32_to_cpu(fas->status);
 		validate_rx_csum(priv, status, skb);
 	}
@@ -263,10 +271,7 @@ static void dpaa2_eth_rx(struct dpaa2_eth_priv *priv,
 	percpu_stats->rx_packets++;
 	percpu_stats->rx_bytes += dpaa2_fd_get_len(fd);
 
-	if (priv->net_dev->features & NETIF_F_GRO)
-		napi_gro_receive(napi, skb);
-	else
-		netif_receive_skb(skb);
+	napi_gro_receive(napi, skb);
 
 	return;
 
@@ -320,7 +325,6 @@ static int build_sg_fd(struct dpaa2_eth_priv *priv,
 {
 	struct device *dev = priv->net_dev->dev.parent;
 	void *sgt_buf = NULL;
-	void *hwa;
 	dma_addr_t addr;
 	int nr_frags = skb_shinfo(skb)->nr_frags;
 	struct dpaa2_sg_entry *sgt;
@@ -330,6 +334,7 @@ static int build_sg_fd(struct dpaa2_eth_priv *priv,
 	int num_sg;
 	int num_dma_bufs;
 	struct dpaa2_eth_swa *swa;
+	struct dpaa2_fas *fas;
 
 	/* Create and map scatterlist.
 	 * We don't advertise NETIF_F_FRAGLIST, so skb_to_sgvec() will not have
@@ -345,7 +350,7 @@ static int build_sg_fd(struct dpaa2_eth_priv *priv,
 
 	sg_init_table(scl, nr_frags + 1);
 	num_sg = skb_to_sgvec(skb, scl, 0, skb->len);
-	num_dma_bufs = dma_map_sg(dev, scl, num_sg, DMA_TO_DEVICE);
+	num_dma_bufs = dma_map_sg(dev, scl, num_sg, DMA_BIDIRECTIONAL);
 	if (unlikely(!num_dma_bufs)) {
 		err = -ENOMEM;
 		goto dma_map_sg_failed;
@@ -366,8 +371,8 @@ static int build_sg_fd(struct dpaa2_eth_priv *priv,
 	 * on TX confirmation. We are clearing FAS (Frame Annotation Status)
 	 * field from the hardware annotation area
 	 */
-	hwa = sgt_buf + priv->buf_layout.private_data_size;
-	memset(hwa + DPAA2_FAS_OFFSET, 0, DPAA2_FAS_SIZE);
+	fas = dpaa2_get_fas(sgt_buf);
+	memset(fas, 0, DPAA2_FAS_SIZE);
 
 	sgt = (struct dpaa2_sg_entry *)(sgt_buf + priv->tx_data_offset);
 
@@ -396,7 +401,7 @@ static int build_sg_fd(struct dpaa2_eth_priv *priv,
 	swa->num_dma_bufs = num_dma_bufs;
 
 	/* Separately map the SGT buffer */
-	addr = dma_map_single(dev, sgt_buf, sgt_buf_size, DMA_TO_DEVICE);
+	addr = dma_map_single(dev, sgt_buf, sgt_buf_size, DMA_BIDIRECTIONAL);
 	if (unlikely(dma_mapping_error(dev, addr))) {
 		err = -ENOMEM;
 		goto dma_map_single_failed;
@@ -413,7 +418,7 @@ static int build_sg_fd(struct dpaa2_eth_priv *priv,
 dma_map_single_failed:
 	kfree(sgt_buf);
 sgt_buf_alloc_failed:
-	dma_unmap_sg(dev, scl, num_sg, DMA_TO_DEVICE);
+	dma_unmap_sg(dev, scl, num_sg, DMA_BIDIRECTIONAL);
 dma_map_sg_failed:
 	kfree(scl);
 	return err;
@@ -426,7 +431,7 @@ static int build_single_fd(struct dpaa2_eth_priv *priv,
 {
 	struct device *dev = priv->net_dev->dev.parent;
 	u8 *buffer_start;
-	void *hwa;
+	struct dpaa2_fas *fas;
 	struct sk_buff **skbh;
 	dma_addr_t addr;
 
@@ -439,8 +444,8 @@ static int build_single_fd(struct dpaa2_eth_priv *priv,
 	 * on TX confirmation. We are clearing FAS (Frame Annotation Status)
 	 * field from the hardware annotation area
 	 */
-	hwa = buffer_start + priv->buf_layout.private_data_size;
-	memset(hwa + DPAA2_FAS_OFFSET, 0, DPAA2_FAS_SIZE);
+	fas = dpaa2_get_fas(buffer_start);
+	memset(fas, 0, DPAA2_FAS_SIZE);
 
 	/* Store a backpointer to the skb at the beginning of the buffer
 	 * (in the private data area) such that we can release it
@@ -451,7 +456,7 @@ static int build_single_fd(struct dpaa2_eth_priv *priv,
 
 	addr = dma_map_single(dev, buffer_start,
 			      skb_tail_pointer(skb) - buffer_start,
-			      DMA_TO_DEVICE);
+			      DMA_BIDIRECTIONAL);
 	if (unlikely(dma_mapping_error(dev, addr)))
 		return -ENOMEM;
 
@@ -490,7 +495,8 @@ static void free_tx_fd(const struct dpaa2_eth_priv *priv,
 	struct dpaa2_fas *fas;
 
 	fd_addr = dpaa2_fd_get_addr(fd);
-	skbh = phys_to_virt(fd_addr);
+	skbh = dpaa2_iova_to_virt(priv->iommu_domain, fd_addr);
+	fas = dpaa2_get_fas(skbh);
 
 	if (fd_format == dpaa2_fd_single) {
 		skb = *skbh;
@@ -500,7 +506,7 @@ static void free_tx_fd(const struct dpaa2_eth_priv *priv,
 		 */
 		dma_unmap_single(dev, fd_addr,
 				 skb_tail_pointer(skb) - buffer_start,
-				 DMA_TO_DEVICE);
+				 DMA_BIDIRECTIONAL);
 	} else if (fd_format == dpaa2_fd_sg) {
 		swa = (struct dpaa2_eth_swa *)skbh;
 		skb = swa->skb;
@@ -509,13 +515,13 @@ static void free_tx_fd(const struct dpaa2_eth_priv *priv,
 		num_dma_bufs = swa->num_dma_bufs;
 
 		/* Unmap the scatterlist */
-		dma_unmap_sg(dev, scl, num_sg, DMA_TO_DEVICE);
+		dma_unmap_sg(dev, scl, num_sg, DMA_BIDIRECTIONAL);
 		kfree(scl);
 
 		/* Unmap the SGT buffer */
 		unmap_size = priv->tx_data_offset +
 		       sizeof(struct dpaa2_sg_entry) * (1 + num_dma_bufs);
-		dma_unmap_single(dev, fd_addr, unmap_size, DMA_TO_DEVICE);
+		dma_unmap_single(dev, fd_addr, unmap_size, DMA_BIDIRECTIONAL);
 	} else {
 		/* Unsupported format, mark it as errored and give up */
 		if (status)
@@ -527,11 +533,8 @@ static void free_tx_fd(const struct dpaa2_eth_priv *priv,
 	 * buffer but before we free it. The caller function is responsible
 	 * for checking the status value.
 	 */
-	if (status && (dpaa2_fd_get_frc(fd) & DPAA2_FD_FRC_FASV)) {
-		fas = (struct dpaa2_fas *)
-			((void *)skbh + priv->buf_layout.private_data_size);
+	if (status)
 		*status = le32_to_cpu(fas->status);
-	}
 
 	/* Free SGT buffer kmalloc'ed on tx */
 	if (fd_format != dpaa2_fd_single)
@@ -541,7 +544,7 @@ static void free_tx_fd(const struct dpaa2_eth_priv *priv,
 	dev_kfree_skb(skb);
 }
 
-static int dpaa2_eth_tx(struct sk_buff *skb, struct net_device *net_dev)
+static netdev_tx_t dpaa2_eth_tx(struct sk_buff *skb, struct net_device *net_dev)
 {
 	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
 	struct dpaa2_fd fd;
@@ -634,6 +637,8 @@ static void dpaa2_eth_tx_conf(struct dpaa2_eth_priv *priv,
 	struct rtnl_link_stats64 *percpu_stats;
 	struct dpaa2_eth_drv_stats *percpu_extras;
 	u32 status = 0;
+	u32 fd_errors;
+	bool has_fas_errors = false;
 
 	/* Tracing point */
 	trace_dpaa2_tx_conf_fd(priv->net_dev, fd);
@@ -642,13 +647,31 @@ static void dpaa2_eth_tx_conf(struct dpaa2_eth_priv *priv,
 	percpu_extras->tx_conf_frames++;
 	percpu_extras->tx_conf_bytes += dpaa2_fd_get_len(fd);
 
-	free_tx_fd(priv, fd, &status);
-
-	if (unlikely(status & DPAA2_ETH_TXCONF_ERR_MASK)) {
-		percpu_stats = this_cpu_ptr(priv->percpu_stats);
-		/* Tx-conf logically pertains to the egress path. */
-		percpu_stats->tx_errors++;
+	/* Check frame errors in the FD field */
+	fd_errors = dpaa2_fd_get_ctrl(fd) & DPAA2_FD_TX_ERR_MASK;
+	if (unlikely(fd_errors)) {
+		/* We only check error bits in the FAS field if corresponding
+		 * FAERR bit is set in FD and the FAS field is marked as valid
+		 */
+		has_fas_errors = (fd_errors & DPAA2_FD_CTRL_FAERR) &&
+				 !!(dpaa2_fd_get_frc(fd) & DPAA2_FD_FRC_FASV);
+		if (net_ratelimit())
+			netdev_dbg(priv->net_dev, "TX frame FD error: %x08\n",
+				   fd_errors);
 	}
+
+	free_tx_fd(priv, fd, has_fas_errors ? &status : NULL);
+
+	if (likely(!fd_errors))
+		return;
+
+	percpu_stats = this_cpu_ptr(priv->percpu_stats);
+	/* Tx-conf logically pertains to the egress path. */
+	percpu_stats->tx_errors++;
+
+	if (has_fas_errors && net_ratelimit())
+		netdev_dbg(priv->net_dev, "TX frame FAS error: %x08\n",
+			   status & DPAA2_FAS_TX_ERR_MASK);
 }
 
 static int set_rx_csum(struct dpaa2_eth_priv *priv, bool enable)
@@ -794,7 +817,7 @@ static void drain_bufs(struct dpaa2_eth_priv *priv, int count)
 	int ret, i;
 
 	do {
-		ret = dpaa2_io_service_acquire(NULL, priv->dpbp_attrs.bpid,
+		ret = dpaa2_io_service_acquire(NULL, priv->bpid,
 					       buf_array, count);
 		if (ret < 0) {
 			netdev_err(priv->net_dev, "dpaa2_io_service_acquire() failed\n");
@@ -802,10 +825,11 @@ static void drain_bufs(struct dpaa2_eth_priv *priv, int count)
 		}
 		for (i = 0; i < ret; i++) {
 			/* Same logic as on regular Rx path */
+			vaddr = dpaa2_iova_to_virt(priv->iommu_domain,
+						   buf_array[i]);
 			dma_unmap_single(dev, buf_array[i],
 					 DPAA2_ETH_RX_BUF_SIZE,
 					 DMA_FROM_DEVICE);
-			vaddr = phys_to_virt(buf_array[i]);
 			skb_free_frag(vaddr);
 		}
 	} while (ret);
@@ -890,7 +914,7 @@ static int dpaa2_eth_poll(struct napi_struct *napi, int budget)
 			break;
 
 		/* Refill pool if appropriate */
-		refill_pool(priv, ch, priv->dpbp_attrs.bpid);
+		refill_pool(priv, ch, priv->bpid);
 
 		store_cleaned = consume_frames(ch);
 		cleaned += store_cleaned;
@@ -964,7 +988,7 @@ static int link_state_update(struct dpaa2_eth_priv *priv)
 		netif_carrier_off(priv->net_dev);
 	}
 
-	netdev_info(priv->net_dev, "Link Event: state %s",
+	netdev_info(priv->net_dev, "Link Event: state %s\n",
 		    state.up ? "up" : "down");
 
 	return 0;
@@ -975,14 +999,14 @@ static int dpaa2_eth_open(struct net_device *net_dev)
 	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
 	int err;
 
-	err = seed_pool(priv, priv->dpbp_attrs.bpid);
+	err = seed_pool(priv, priv->bpid);
 	if (err) {
 		/* Not much to do; the buffer pool, though not filled up,
 		 * may still contain some buffers which would enable us
 		 * to limp on.
 		 */
 		netdev_err(net_dev, "Buffer seeding failed for DPBP %d (bpid=%d)\n",
-			   priv->dpbp_dev->obj_desc.id, priv->dpbp_attrs.bpid);
+			   priv->dpbp_dev->obj_desc.id, priv->bpid);
 	}
 
 	/* We'll only start the txqs when the link is actually ready; make sure
@@ -1151,8 +1175,8 @@ static int dpaa2_eth_set_addr(struct net_device *net_dev, void *addr)
 /** Fill in counters maintained by the GPP driver. These may be different from
  * the hardware counters obtained by ethtool.
  */
-void dpaa2_eth_get_stats(struct net_device *net_dev,
-			 struct rtnl_link_stats64 *stats)
+static void dpaa2_eth_get_stats(struct net_device *net_dev,
+				struct rtnl_link_stats64 *stats)
 {
 	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
 	struct rtnl_link_stats64 *percpu_stats;
@@ -1502,6 +1526,7 @@ static int setup_dpio(struct dpaa2_eth_priv *priv)
 		if (!channel) {
 			dev_info(dev,
 				 "No affine channel for cpu %d and above\n", i);
+			err = -ENODEV;
 			goto err_alloc_ch;
 		}
 
@@ -1516,10 +1541,13 @@ static int setup_dpio(struct dpaa2_eth_priv *priv)
 		/* Register the new context */
 		err = dpaa2_io_service_register(NULL, nctx);
 		if (err) {
-			dev_info(dev, "No affine DPIO for cpu %d\n", i);
+			dev_dbg(dev, "No affine DPIO for cpu %d\n", i);
 			/* If no affine DPIO for this core, there's probably
-			 * none available for next cores either.
+			 * none available for next cores either. Signal we want
+			 * to retry later, in case the DPIO devices weren't
+			 * probed yet.
 			 */
+			err = -EPROBE_DEFER;
 			goto err_service_reg;
 		}
 
@@ -1557,7 +1585,7 @@ err_service_reg:
 err_alloc_ch:
 	if (cpumask_empty(&priv->dpio_cpumask)) {
 		dev_err(dev, "No cpu with an affine DPIO/DPCON\n");
-		return -ENODEV;
+		return err;
 	}
 
 	dev_info(dev, "Cores %*pbl available for processing ingress traffic\n",
@@ -1662,6 +1690,7 @@ static int setup_dpbp(struct dpaa2_eth_priv *priv)
 	int err;
 	struct fsl_mc_device *dpbp_dev;
 	struct device *dev = priv->net_dev->dev.parent;
+	struct dpbp_attr dpbp_attrs;
 
 	err = fsl_mc_object_allocate(to_fsl_mc_device(dev), FSL_MC_POOL_DPBP,
 				     &dpbp_dev);
@@ -1679,6 +1708,12 @@ static int setup_dpbp(struct dpaa2_eth_priv *priv)
 		goto err_open;
 	}
 
+	err = dpbp_reset(priv->mc_io, 0, dpbp_dev->mc_handle);
+	if (err) {
+		dev_err(dev, "dpbp_reset() failed\n");
+		goto err_reset;
+	}
+
 	err = dpbp_enable(priv->mc_io, 0, dpbp_dev->mc_handle);
 	if (err) {
 		dev_err(dev, "dpbp_enable() failed\n");
@@ -1686,17 +1721,19 @@ static int setup_dpbp(struct dpaa2_eth_priv *priv)
 	}
 
 	err = dpbp_get_attributes(priv->mc_io, 0, dpbp_dev->mc_handle,
-				  &priv->dpbp_attrs);
+				  &dpbp_attrs);
 	if (err) {
 		dev_err(dev, "dpbp_get_attributes() failed\n");
 		goto err_get_attr;
 	}
+	priv->bpid = dpbp_attrs.bpid;
 
 	return 0;
 
 err_get_attr:
 	dpbp_disable(priv->mc_io, 0, dpbp_dev->mc_handle);
 err_enable:
+err_reset:
 	dpbp_close(priv->mc_io, 0, dpbp_dev->mc_handle);
 err_open:
 	fsl_mc_object_free(dpbp_dev);
@@ -1718,15 +1755,14 @@ static int setup_dpni(struct fsl_mc_device *ls_dev)
 	struct device *dev = &ls_dev->dev;
 	struct dpaa2_eth_priv *priv;
 	struct net_device *net_dev;
+	struct dpni_buffer_layout buf_layout = {0};
 	int err;
 
 	net_dev = dev_get_drvdata(dev);
 	priv = netdev_priv(net_dev);
 
-	priv->dpni_id = ls_dev->obj_desc.id;
-
 	/* get a handle for the DPNI object */
-	err = dpni_open(priv->mc_io, 0, priv->dpni_id, &priv->mc_token);
+	err = dpni_open(priv->mc_io, 0, ls_dev->obj_desc.id, &priv->mc_token);
 	if (err) {
 		dev_err(dev, "dpni_open() failed\n");
 		goto err_open;
@@ -1750,35 +1786,35 @@ static int setup_dpni(struct fsl_mc_device *ls_dev)
 
 	/* Configure buffer layouts */
 	/* rx buffer */
-	priv->buf_layout.pass_parser_result = true;
-	priv->buf_layout.pass_frame_status = true;
-	priv->buf_layout.private_data_size = DPAA2_ETH_SWA_SIZE;
-	priv->buf_layout.data_align = DPAA2_ETH_RX_BUF_ALIGN;
-	priv->buf_layout.options = DPNI_BUF_LAYOUT_OPT_PARSER_RESULT |
-				   DPNI_BUF_LAYOUT_OPT_FRAME_STATUS |
-				   DPNI_BUF_LAYOUT_OPT_PRIVATE_DATA_SIZE |
-				   DPNI_BUF_LAYOUT_OPT_DATA_ALIGN;
+	buf_layout.pass_parser_result = true;
+	buf_layout.pass_frame_status = true;
+	buf_layout.private_data_size = DPAA2_ETH_SWA_SIZE;
+	buf_layout.data_align = DPAA2_ETH_RX_BUF_ALIGN;
+	buf_layout.options = DPNI_BUF_LAYOUT_OPT_PARSER_RESULT |
+			     DPNI_BUF_LAYOUT_OPT_FRAME_STATUS |
+			     DPNI_BUF_LAYOUT_OPT_PRIVATE_DATA_SIZE |
+			     DPNI_BUF_LAYOUT_OPT_DATA_ALIGN;
 	err = dpni_set_buffer_layout(priv->mc_io, 0, priv->mc_token,
-				     DPNI_QUEUE_RX, &priv->buf_layout);
+				     DPNI_QUEUE_RX, &buf_layout);
 	if (err) {
 		dev_err(dev, "dpni_set_buffer_layout(RX) failed\n");
 		goto err_buf_layout;
 	}
 
 	/* tx buffer */
-	priv->buf_layout.options = DPNI_BUF_LAYOUT_OPT_FRAME_STATUS |
-				   DPNI_BUF_LAYOUT_OPT_PRIVATE_DATA_SIZE;
+	buf_layout.options = DPNI_BUF_LAYOUT_OPT_FRAME_STATUS |
+			     DPNI_BUF_LAYOUT_OPT_PRIVATE_DATA_SIZE;
 	err = dpni_set_buffer_layout(priv->mc_io, 0, priv->mc_token,
-				     DPNI_QUEUE_TX, &priv->buf_layout);
+				     DPNI_QUEUE_TX, &buf_layout);
 	if (err) {
 		dev_err(dev, "dpni_set_buffer_layout(TX) failed\n");
 		goto err_buf_layout;
 	}
 
 	/* tx-confirm buffer */
-	priv->buf_layout.options = DPNI_BUF_LAYOUT_OPT_FRAME_STATUS;
+	buf_layout.options = DPNI_BUF_LAYOUT_OPT_FRAME_STATUS;
 	err = dpni_set_buffer_layout(priv->mc_io, 0, priv->mc_token,
-				     DPNI_QUEUE_TX_CONFIRM, &priv->buf_layout);
+				     DPNI_QUEUE_TX_CONFIRM, &buf_layout);
 	if (err) {
 		dev_err(dev, "dpni_set_buffer_layout(TX_CONF) failed\n");
 		goto err_buf_layout;
@@ -1795,7 +1831,7 @@ static int setup_dpni(struct fsl_mc_device *ls_dev)
 	}
 
 	if ((priv->tx_data_offset % 64) != 0)
-		dev_warn(dev, "Tx data offset (%d) not a multiple of 64B",
+		dev_warn(dev, "Tx data offset (%d) not a multiple of 64B\n",
 			 priv->tx_data_offset);
 
 	/* Accommodate software annotation space (SWA) */
@@ -1947,7 +1983,7 @@ static const struct dpaa2_eth_hash_fields hash_fields[] = {
 /* Set RX hash options
  * flags is a combination of RXH_ bits
  */
-int dpaa2_eth_set_hash(struct net_device *net_dev, u64 flags)
+static int dpaa2_eth_set_hash(struct net_device *net_dev, u64 flags)
 {
 	struct device *dev = net_dev->dev.parent;
 	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
@@ -1958,8 +1994,8 @@ int dpaa2_eth_set_hash(struct net_device *net_dev, u64 flags)
 	int err = 0;
 
 	if (!dpaa2_eth_hash_enabled(priv)) {
-		dev_err(dev, "Hashing support is not enabled\n");
-		return -EOPNOTSUPP;
+		dev_dbg(dev, "Hashing support is not enabled\n");
+		return 0;
 	}
 
 	memset(&cls_cfg, 0, sizeof(cls_cfg));
@@ -1985,23 +2021,23 @@ int dpaa2_eth_set_hash(struct net_device *net_dev, u64 flags)
 		priv->rx_hash_fields |= hash_fields[i].rxnfc_field;
 	}
 
-	dma_mem = kzalloc(DPAA2_CLASSIFIER_DMA_SIZE, GFP_DMA | GFP_KERNEL);
+	dma_mem = kzalloc(DPAA2_CLASSIFIER_DMA_SIZE, GFP_KERNEL);
 	if (!dma_mem)
 		return -ENOMEM;
 
 	err = dpni_prepare_key_cfg(&cls_cfg, dma_mem);
 	if (err) {
-		dev_err(dev, "dpni_prepare_key_cfg error %d", err);
+		dev_err(dev, "dpni_prepare_key_cfg error %d\n", err);
 		goto err_prep_key;
 	}
 
 	memset(&dist_cfg, 0, sizeof(dist_cfg));
 
 	/* Prepare for setting the rx dist */
-	dist_cfg.key_cfg_iova = dma_map_single(net_dev->dev.parent, dma_mem,
+	dist_cfg.key_cfg_iova = dma_map_single(dev, dma_mem,
 					       DPAA2_CLASSIFIER_DMA_SIZE,
 					       DMA_TO_DEVICE);
-	if (dma_mapping_error(net_dev->dev.parent, dist_cfg.key_cfg_iova)) {
+	if (dma_mapping_error(dev, dist_cfg.key_cfg_iova)) {
 		dev_err(dev, "DMA mapping failed\n");
 		err = -ENOMEM;
 		goto err_dma_map;
@@ -2011,7 +2047,7 @@ int dpaa2_eth_set_hash(struct net_device *net_dev, u64 flags)
 	dist_cfg.dist_mode = DPNI_DIST_MODE_HASH;
 
 	err = dpni_set_rx_tc_dist(priv->mc_io, 0, priv->mc_token, 0, &dist_cfg);
-	dma_unmap_single(net_dev->dev.parent, dist_cfg.key_cfg_iova,
+	dma_unmap_single(dev, dist_cfg.key_cfg_iova,
 			 DPAA2_CLASSIFIER_DMA_SIZE, DMA_TO_DEVICE);
 	if (err)
 		dev_err(dev, "dpni_set_rx_tc_dist() error %d\n", err);
@@ -2052,7 +2088,7 @@ static int bind_dpni(struct dpaa2_eth_priv *priv)
 		netdev_err(net_dev, "Failed to configure hashing\n");
 
 	/* Configure handling of error frames */
-	err_cfg.errors = DPAA2_ETH_RX_ERR_MASK;
+	err_cfg.errors = DPAA2_FAS_RX_ERR_MASK;
 	err_cfg.set_frame_annotation = 1;
 	err_cfg.error_action = DPNI_ERROR_ACTION_DISCARD;
 	err = dpni_set_errors_behavior(priv->mc_io, 0, priv->mc_token,
@@ -2125,15 +2161,12 @@ static void free_rings(struct dpaa2_eth_priv *priv)
 		dpaa2_io_store_destroy(priv->channel[i]->store);
 }
 
-static int netdev_init(struct net_device *net_dev)
+static int set_mac_addr(struct dpaa2_eth_priv *priv)
 {
-	int err;
+	struct net_device *net_dev = priv->net_dev;
 	struct device *dev = net_dev->dev.parent;
-	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
 	u8 mac_addr[ETH_ALEN], dpni_mac_addr[ETH_ALEN];
-	u8 bcast_addr[ETH_ALEN];
-
-	net_dev->netdev_ops = &dpaa2_eth_ops;
+	int err;
 
 	/* Get firmware address, if any */
 	err = dpni_get_port_mac_addr(priv->mc_io, 0, priv->mc_token, mac_addr);
@@ -2146,7 +2179,7 @@ static int netdev_init(struct net_device *net_dev)
 	err = dpni_get_primary_mac_addr(priv->mc_io, 0, priv->mc_token,
 					dpni_mac_addr);
 	if (err) {
-		dev_err(dev, "dpni_get_primary_mac_addr() failed (%d)\n", err);
+		dev_err(dev, "dpni_get_primary_mac_addr() failed\n");
 		return err;
 	}
 
@@ -2164,18 +2197,19 @@ static int netdev_init(struct net_device *net_dev)
 		}
 		memcpy(net_dev->dev_addr, mac_addr, net_dev->addr_len);
 	} else if (is_zero_ether_addr(dpni_mac_addr)) {
-		/* Fills in net_dev->dev_addr, as required by
-		 * register_netdevice()
+		/* No MAC address configured, fill in net_dev->dev_addr
+		 * with a random one
 		 */
 		eth_hw_addr_random(net_dev);
-		/* Make the user aware, without cluttering the boot log */
-		dev_dbg_once(dev, " device(s) have all-zero hwaddr, replaced with random\n");
+		dev_dbg_once(dev, "device(s) have all-zero hwaddr, replaced with random\n");
+
 		err = dpni_set_primary_mac_addr(priv->mc_io, 0, priv->mc_token,
 						net_dev->dev_addr);
 		if (err) {
-			dev_err(dev, "dpni_set_primary_mac_addr(): %d\n", err);
+			dev_err(dev, "dpni_set_primary_mac_addr() failed\n");
 			return err;
 		}
+
 		/* Override NET_ADDR_RANDOM set by eth_hw_addr_random(); for all
 		 * practical purposes, this will be our "permanent" mac address,
 		 * at least until the next reboot. This move will also permit
@@ -2189,14 +2223,29 @@ static int netdev_init(struct net_device *net_dev)
 		memcpy(net_dev->dev_addr, dpni_mac_addr, net_dev->addr_len);
 	}
 
-	/* Explicitly add the broadcast address to the MAC filtering table;
-	 * the MC won't do that for us.
-	 */
+	return 0;
+}
+
+static int netdev_init(struct net_device *net_dev)
+{
+	struct device *dev = net_dev->dev.parent;
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	u8 bcast_addr[ETH_ALEN];
+	u8 num_queues;
+	int err;
+
+	net_dev->netdev_ops = &dpaa2_eth_ops;
+
+	err = set_mac_addr(priv);
+	if (err)
+		return err;
+
+	/* Explicitly add the broadcast address to the MAC filtering table */
 	eth_broadcast_addr(bcast_addr);
 	err = dpni_add_mac_addr(priv->mc_io, 0, priv->mc_token, bcast_addr);
 	if (err) {
-		dev_warn(dev, "dpni_add_mac_addr() failed (%d)\n", err);
-		/* Won't return an error; at least, we'd have egress traffic */
+		dev_err(dev, "dpni_add_mac_addr() failed\n");
+		return err;
 	}
 
 	/* Reserve enough space to align buffer as per hardware requirement;
@@ -2208,6 +2257,19 @@ static int netdev_init(struct net_device *net_dev)
 	net_dev->min_mtu = 68;
 	net_dev->max_mtu = DPAA2_ETH_MAX_MTU;
 
+	/* Set actual number of queues in the net device */
+	num_queues = dpaa2_eth_queue_count(priv);
+	err = netif_set_real_num_tx_queues(net_dev, num_queues);
+	if (err) {
+		dev_err(dev, "netif_set_real_num_tx_queues() failed\n");
+		return err;
+	}
+	err = netif_set_real_num_rx_queues(net_dev, num_queues);
+	if (err) {
+		dev_err(dev, "netif_set_real_num_rx_queues() failed\n");
+		return err;
+	}
+
 	/* Our .ndo_init will be called herein */
 	err = register_netdev(net_dev);
 	if (err < 0) {
@@ -2241,7 +2303,7 @@ static irqreturn_t dpni_irq0_handler(int irq_num, void *arg)
 
 static irqreturn_t dpni_irq0_handler_thread(int irq_num, void *arg)
 {
-	u32 status, clear = 0;
+	u32 status = 0, clear = 0;
 	struct device *dev = (struct device *)arg;
 	struct fsl_mc_device *dpni_dev = to_fsl_mc_device(dev);
 	struct net_device *net_dev = dev_get_drvdata(dev);
@@ -2250,7 +2312,7 @@ static irqreturn_t dpni_irq0_handler_thread(int irq_num, void *arg)
 	err = dpni_get_irq_status(dpni_dev->mc_io, 0, dpni_dev->mc_handle,
 				  DPNI_IRQ_INDEX, &status);
 	if (unlikely(err)) {
-		netdev_err(net_dev, "Can't get irq status (err %d)", err);
+		netdev_err(net_dev, "Can't get irq status (err %d)\n", err);
 		clear = 0xffffffff;
 		goto out;
 	}
@@ -2284,21 +2346,21 @@ static int setup_irqs(struct fsl_mc_device *ls_dev)
 					IRQF_NO_SUSPEND | IRQF_ONESHOT,
 					dev_name(&ls_dev->dev), &ls_dev->dev);
 	if (err < 0) {
-		dev_err(&ls_dev->dev, "devm_request_threaded_irq(): %d", err);
+		dev_err(&ls_dev->dev, "devm_request_threaded_irq(): %d\n", err);
 		goto free_mc_irq;
 	}
 
 	err = dpni_set_irq_mask(ls_dev->mc_io, 0, ls_dev->mc_handle,
 				DPNI_IRQ_INDEX, DPNI_IRQ_EVENT_LINK_CHANGED);
 	if (err < 0) {
-		dev_err(&ls_dev->dev, "dpni_set_irq_mask(): %d", err);
+		dev_err(&ls_dev->dev, "dpni_set_irq_mask(): %d\n", err);
 		goto free_irq;
 	}
 
 	err = dpni_set_irq_enable(ls_dev->mc_io, 0, ls_dev->mc_handle,
 				  DPNI_IRQ_INDEX, 1);
 	if (err < 0) {
-		dev_err(&ls_dev->dev, "dpni_set_irq_enable(): %d", err);
+		dev_err(&ls_dev->dev, "dpni_set_irq_enable(): %d\n", err);
 		goto free_irq;
 	}
 
@@ -2358,6 +2420,8 @@ static int dpaa2_eth_probe(struct fsl_mc_device *dpni_dev)
 	priv = netdev_priv(net_dev);
 	priv->net_dev = net_dev;
 
+	priv->iommu_domain = iommu_get_domain_for_dev(dev);
+
 	/* Obtain a MC portal */
 	err = fsl_mc_portal_allocate(dpni_dev, FSL_MC_IO_ATOMIC_CONTEXT_PORTAL,
 				     &priv->mc_io);
diff --git a/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.h b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.h
index c67cced55b72..e6d28a249fc1 100644
--- a/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.h
+++ b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.h
@@ -120,6 +120,19 @@ struct dpaa2_eth_swa {
 #define DPAA2_FD_FRC_FASWOV		0x0800
 #define DPAA2_FD_FRC_FAICFDV		0x0400
 
+/* Error bits in FD CTRL */
+#define DPAA2_FD_CTRL_UFD		0x00000004
+#define DPAA2_FD_CTRL_SBE		0x00000008
+#define DPAA2_FD_CTRL_FSE		0x00000010
+#define DPAA2_FD_CTRL_FAERR		0x00000020
+
+#define DPAA2_FD_RX_ERR_MASK		(DPAA2_FD_CTRL_SBE	| \
+					 DPAA2_FD_CTRL_FAERR)
+#define DPAA2_FD_TX_ERR_MASK		(DPAA2_FD_CTRL_UFD	| \
+					 DPAA2_FD_CTRL_SBE	| \
+					 DPAA2_FD_CTRL_FSE	| \
+					 DPAA2_FD_CTRL_FAERR)
+
 /* Annotation bits in FD CTRL */
 #define DPAA2_FD_CTRL_ASAL		0x00020000	/* ASAL = 128 */
 #define DPAA2_FD_CTRL_PTA		0x00800000
@@ -139,6 +152,12 @@ struct dpaa2_fas {
 #define DPAA2_FAS_OFFSET		0
 #define DPAA2_FAS_SIZE			(sizeof(struct dpaa2_fas))
 
+/* Accessors for the hardware annotation fields that we use */
+#define dpaa2_get_hwa(buf_addr) \
+	((void *)(buf_addr) + DPAA2_ETH_SWA_SIZE)
+#define dpaa2_get_fas(buf_addr) \
+	(struct dpaa2_fas *)(dpaa2_get_hwa(buf_addr) + DPAA2_FAS_OFFSET)
+
 /* Error and status bits in the frame annotation status word */
 /* Debug frame, otherwise supposed to be discarded */
 #define DPAA2_FAS_DISC			0x80000000
@@ -171,7 +190,7 @@ struct dpaa2_fas {
 /* L4 csum error */
 #define DPAA2_FAS_L4CE			0x00000001
 /* Possible errors on the ingress path */
-#define DPAA2_ETH_RX_ERR_MASK		(DPAA2_FAS_KSE		| \
+#define DPAA2_FAS_RX_ERR_MASK		(DPAA2_FAS_KSE		| \
 					 DPAA2_FAS_EOFHE	| \
 					 DPAA2_FAS_MNLE		| \
 					 DPAA2_FAS_TIDE		| \
@@ -185,7 +204,7 @@ struct dpaa2_fas {
 					 DPAA2_FAS_L3CE		| \
 					 DPAA2_FAS_L4CE)
 /* Tx errors */
-#define DPAA2_ETH_TXCONF_ERR_MASK	(DPAA2_FAS_KSE		| \
+#define DPAA2_FAS_TX_ERR_MASK		(DPAA2_FAS_KSE		| \
 					 DPAA2_FAS_EOFHE	| \
 					 DPAA2_FAS_MNLE		| \
 					 DPAA2_FAS_TIDE)
@@ -291,16 +310,12 @@ struct dpaa2_eth_priv {
 	u8 num_channels;
 	struct dpaa2_eth_channel *channel[DPAA2_ETH_MAX_DPCONS];
 
-	int dpni_id;
 	struct dpni_attr dpni_attrs;
-	/* Insofar as the MC is concerned, we're using one layout on all 3 types
-	 * of buffers (Rx, Tx, Tx-Conf).
-	 */
-	struct dpni_buffer_layout buf_layout;
 	u16 tx_data_offset;
 
 	struct fsl_mc_device *dpbp_dev;
-	struct dpbp_attr dpbp_attrs;
+	u16 bpid;
+	struct iommu_domain *iommu_domain;
 
 	u16 tx_qdid;
 	struct fsl_mc_io *mc_io;
@@ -338,8 +353,6 @@ struct dpaa2_eth_priv {
 extern const struct ethtool_ops dpaa2_ethtool_ops;
 extern const char dpaa2_eth_drv_version[];
 
-int dpaa2_eth_set_hash(struct net_device *net_dev, u64 flags);
-
 static int dpaa2_eth_queue_count(struct dpaa2_eth_priv *priv)
 {
 	return priv->dpni_attrs.num_queues;
diff --git a/drivers/staging/fsl-dpaa2/ethernet/dpaa2-ethtool.c b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-ethtool.c
index dd0cffa908ef..5312edc26f01 100644
--- a/drivers/staging/fsl-dpaa2/ethernet/dpaa2-ethtool.c
+++ b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-ethtool.c
@@ -34,41 +34,41 @@
 #include "dpaa2-eth.h"
 
 /* To be kept in sync with DPNI statistics */
-char dpaa2_ethtool_stats[][ETH_GSTRING_LEN] = {
-	"rx frames",
-	"rx bytes",
-	"rx mcast frames",
-	"rx mcast bytes",
-	"rx bcast frames",
-	"rx bcast bytes",
-	"tx frames",
-	"tx bytes",
-	"tx mcast frames",
-	"tx mcast bytes",
-	"tx bcast frames",
-	"tx bcast bytes",
-	"rx filtered frames",
-	"rx discarded frames",
-	"rx nobuffer discards",
-	"tx discarded frames",
-	"tx confirmed frames",
+static char dpaa2_ethtool_stats[][ETH_GSTRING_LEN] = {
+	"[hw] rx frames",
+	"[hw] rx bytes",
+	"[hw] rx mcast frames",
+	"[hw] rx mcast bytes",
+	"[hw] rx bcast frames",
+	"[hw] rx bcast bytes",
+	"[hw] tx frames",
+	"[hw] tx bytes",
+	"[hw] tx mcast frames",
+	"[hw] tx mcast bytes",
+	"[hw] tx bcast frames",
+	"[hw] tx bcast bytes",
+	"[hw] rx filtered frames",
+	"[hw] rx discarded frames",
+	"[hw] rx nobuffer discards",
+	"[hw] tx discarded frames",
+	"[hw] tx confirmed frames",
 };
 
 #define DPAA2_ETH_NUM_STATS	ARRAY_SIZE(dpaa2_ethtool_stats)
 
-char dpaa2_ethtool_extras[][ETH_GSTRING_LEN] = {
+static char dpaa2_ethtool_extras[][ETH_GSTRING_LEN] = {
 	/* per-cpu stats */
-	"tx conf frames",
-	"tx conf bytes",
-	"tx sg frames",
-	"tx sg bytes",
-	"rx sg frames",
-	"rx sg bytes",
-	"enqueue portal busy",
+	"[drv] tx conf frames",
+	"[drv] tx conf bytes",
+	"[drv] tx sg frames",
+	"[drv] tx sg bytes",
+	"[drv] rx sg frames",
+	"[drv] rx sg bytes",
+	"[drv] enqueue portal busy",
 	/* Channel stats */
-	"dequeue portal busy",
-	"channel pull errors",
-	"cdan",
+	"[drv] dequeue portal busy",
+	"[drv] channel pull errors",
+	"[drv] cdan",
 };
 
 #define DPAA2_ETH_NUM_EXTRA_STATS	ARRAY_SIZE(dpaa2_ethtool_extras)
@@ -94,7 +94,7 @@ dpaa2_eth_get_link_ksettings(struct net_device *net_dev,
 
 	err = dpni_get_link_state(priv->mc_io, 0, priv->mc_token, &state);
 	if (err) {
-		netdev_err(net_dev, "ERROR %d getting link state", err);
+		netdev_err(net_dev, "ERROR %d getting link state\n", err);
 		goto out;
 	}
 
@@ -147,7 +147,7 @@ dpaa2_eth_set_link_ksettings(struct net_device *net_dev,
 		/* ethtool will be loud enough if we return an error; no point
 		 * in putting our own error message on the console by default
 		 */
-		netdev_dbg(net_dev, "ERROR %d setting link cfg", err);
+		netdev_dbg(net_dev, "ERROR %d setting link cfg\n", err);
 
 	return err;
 }
@@ -206,7 +206,7 @@ static void dpaa2_eth_get_ethtool_stats(struct net_device *net_dev,
 		err = dpni_get_statistics(priv->mc_io, 0, priv->mc_token,
 					  j, &dpni_stats);
 		if (err != 0)
-			netdev_warn(net_dev, "dpni_get_stats(%d) failed", j);
+			netdev_warn(net_dev, "dpni_get_stats(%d) failed\n", j);
 		switch (j) {
 		case 0:
 			num_cnt = sizeof(dpni_stats.page_0) / sizeof(u64);
diff --git a/drivers/staging/fsl-dpaa2/ethernet/dpni.c b/drivers/staging/fsl-dpaa2/ethernet/dpni.c
index cea46edec6a2..5b9d4424e4fb 100644
--- a/drivers/staging/fsl-dpaa2/ethernet/dpni.c
+++ b/drivers/staging/fsl-dpaa2/ethernet/dpni.c
@@ -30,8 +30,9 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-#include "../../fsl-mc/include/mc-sys.h"
-#include "../../fsl-mc/include/mc-cmd.h"
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include "../../fsl-mc/include/mc.h"
 #include "dpni.h"
 #include "dpni-cmd.h"
 
diff --git a/drivers/staging/fsl-mc/README.txt b/drivers/staging/fsl-mc/README.txt
index 179536a9b7a1..524eda1de04f 100644
--- a/drivers/staging/fsl-mc/README.txt
+++ b/drivers/staging/fsl-mc/README.txt
@@ -131,9 +131,7 @@ in creating a network interfaces.
 
     DPRCs can be defined statically and populated with objects
     via a config file passed to the MC when firmware starts
-    it.  There is also a Linux user space tool called "restool"
-    that can be used to create/destroy containers and objects
-    dynamically.
+    it.
 
 -DPAA2 Objects for an Ethernet Network Interface
 
@@ -322,6 +320,8 @@ A brief description of each driver is provided below.
        -creates an MSI IRQ domain
        -doing a 'device add' to expose the 'root' DPRC, in turn triggering
         a bind of the root DPRC to the DPRC driver
+    The binding for the MC-bus device-tree node can be consulted here:
+        Documentation/devicetree/bindings/misc/fsl,qoriq-mc.txt
 
     DPRC driver
     -----------
diff --git a/drivers/staging/fsl-mc/bus/Makefile b/drivers/staging/fsl-mc/bus/Makefile
index 659eccf52a4f..6df407edfade 100644
--- a/drivers/staging/fsl-mc/bus/Makefile
+++ b/drivers/staging/fsl-mc/bus/Makefile
@@ -11,7 +11,6 @@ mc-bus-driver-objs := fsl-mc-bus.o \
 		      mc-sys.o \
 		      mc-io.o \
 		      dprc.o \
-		      dpmng.o \
 		      dprc-driver.o \
 		      fsl-mc-allocator.o \
 		      fsl-mc-msi.o \
diff --git a/drivers/staging/fsl-mc/bus/dpbp-cmd.h b/drivers/staging/fsl-mc/bus/dpbp-cmd.h
index 8aa65452c872..5904836fd741 100644
--- a/drivers/staging/fsl-mc/bus/dpbp-cmd.h
+++ b/drivers/staging/fsl-mc/bus/dpbp-cmd.h
@@ -40,7 +40,7 @@
 #define DPBP_CMD_BASE_VERSION			1
 #define DPBP_CMD_ID_OFFSET			4
 
-#define DPBP_CMD(id)	((id << DPBP_CMD_ID_OFFSET) | DPBP_CMD_BASE_VERSION)
+#define DPBP_CMD(id)	(((id) << DPBP_CMD_ID_OFFSET) | DPBP_CMD_BASE_VERSION)
 
 /* Command IDs */
 #define DPBP_CMDID_CLOSE		DPBP_CMD(0x800)
diff --git a/drivers/staging/fsl-mc/bus/dpbp.c b/drivers/staging/fsl-mc/bus/dpbp.c
index d9e450a6bad6..363730a80cbb 100644
--- a/drivers/staging/fsl-mc/bus/dpbp.c
+++ b/drivers/staging/fsl-mc/bus/dpbp.c
@@ -29,8 +29,8 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-#include "../include/mc-sys.h"
-#include "../include/mc-cmd.h"
+#include <linux/kernel.h>
+#include "../include/mc.h"
 #include "../include/dpbp.h"
 
 #include "dpbp-cmd.h"
diff --git a/drivers/staging/fsl-mc/bus/dpcon.c b/drivers/staging/fsl-mc/bus/dpcon.c
index eb713578b817..ca1da85c6dda 100644
--- a/drivers/staging/fsl-mc/bus/dpcon.c
+++ b/drivers/staging/fsl-mc/bus/dpcon.c
@@ -29,8 +29,8 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-#include "../include/mc-sys.h"
-#include "../include/mc-cmd.h"
+#include <linux/kernel.h>
+#include "../include/mc.h"
 #include "../include/dpcon.h"
 
 #include "dpcon-cmd.h"
diff --git a/drivers/staging/fsl-mc/bus/dpio/dpio-service.c b/drivers/staging/fsl-mc/bus/dpio/dpio-service.c
index e5d66749614c..f8096828f5b7 100644
--- a/drivers/staging/fsl-mc/bus/dpio/dpio-service.c
+++ b/drivers/staging/fsl-mc/bus/dpio/dpio-service.c
@@ -260,9 +260,9 @@ int dpaa2_io_service_register(struct dpaa2_io *d,
 
 	/* Enable the generation of CDAN notifications */
 	if (ctx->is_cdan)
-		qbman_swp_CDAN_set_context_enable(d->swp,
-						  (u16)ctx->id,
-						  ctx->qman64);
+		return qbman_swp_CDAN_set_context_enable(d->swp,
+							 (u16)ctx->id,
+							 ctx->qman64);
 	return 0;
 }
 EXPORT_SYMBOL(dpaa2_io_service_register);
@@ -514,7 +514,7 @@ EXPORT_SYMBOL(dpaa2_io_service_acquire);
  * The size of the storage is "max_frames*sizeof(struct dpaa2_dq)".
  * The 'dpaa2_io_store' returned is a DPIO service managed object.
  *
- * Return pointer to dpaa2_io_store struct for successfuly created storage
+ * Return pointer to dpaa2_io_store struct for successfully created storage
  * memory, or NULL on error.
  */
 struct dpaa2_io_store *dpaa2_io_store_create(unsigned int max_frames,
diff --git a/drivers/staging/fsl-mc/bus/dpio/dpio.c b/drivers/staging/fsl-mc/bus/dpio/dpio.c
index d81e0232f6c1..00eb22186f42 100644
--- a/drivers/staging/fsl-mc/bus/dpio/dpio.c
+++ b/drivers/staging/fsl-mc/bus/dpio/dpio.c
@@ -30,8 +30,8 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-#include "../../include/mc-sys.h"
-#include "../../include/mc-cmd.h"
+#include <linux/kernel.h>
+#include "../../include/mc.h"
 
 #include "dpio.h"
 #include "dpio-cmd.h"
diff --git a/drivers/staging/fsl-mc/bus/dpmcp-cmd.h b/drivers/staging/fsl-mc/bus/dpmcp-cmd.h
index 384a13d0b07f..861b2a708af8 100644
--- a/drivers/staging/fsl-mc/bus/dpmcp-cmd.h
+++ b/drivers/staging/fsl-mc/bus/dpmcp-cmd.h
@@ -40,7 +40,7 @@
 #define DPMCP_CMD_BASE_VERSION		1
 #define DPMCP_CMD_ID_OFFSET		4
 
-#define DPMCP_CMD(id)	((id << DPMCP_CMD_ID_OFFSET) | DPMCP_CMD_BASE_VERSION)
+#define DPMCP_CMD(id)	(((id) << DPMCP_CMD_ID_OFFSET) | DPMCP_CMD_BASE_VERSION)
 
 /* Command IDs */
 #define DPMCP_CMDID_CLOSE		DPMCP_CMD(0x800)
diff --git a/drivers/staging/fsl-mc/bus/dpmcp.c b/drivers/staging/fsl-mc/bus/dpmcp.c
index ad4c8b43f065..eea42f61af86 100644
--- a/drivers/staging/fsl-mc/bus/dpmcp.c
+++ b/drivers/staging/fsl-mc/bus/dpmcp.c
@@ -29,8 +29,8 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-#include "../include/mc-sys.h"
-#include "../include/mc-cmd.h"
+#include <linux/kernel.h>
+#include "../include/mc.h"
 
 #include "dpmcp.h"
 #include "dpmcp-cmd.h"
diff --git a/drivers/staging/fsl-mc/bus/dpmng-cmd.h b/drivers/staging/fsl-mc/bus/dpmng-cmd.h
index cdddfb80eecc..d1f04ac18b78 100644
--- a/drivers/staging/fsl-mc/bus/dpmng-cmd.h
+++ b/drivers/staging/fsl-mc/bus/dpmng-cmd.h
@@ -44,7 +44,7 @@
 #define DPMNG_CMD_BASE_VERSION		1
 #define DPMNG_CMD_ID_OFFSET		4
 
-#define DPMNG_CMD(id)	((id << DPMNG_CMD_ID_OFFSET) | DPMNG_CMD_BASE_VERSION)
+#define DPMNG_CMD(id)	(((id) << DPMNG_CMD_ID_OFFSET) | DPMNG_CMD_BASE_VERSION)
 
 /* Command IDs */
 #define DPMNG_CMDID_GET_VERSION		DPMNG_CMD(0x831)
diff --git a/drivers/staging/fsl-mc/bus/dpmng.c b/drivers/staging/fsl-mc/bus/dpmng.c
deleted file mode 100644
index ad5d5bbec529..000000000000
--- a/drivers/staging/fsl-mc/bus/dpmng.c
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
- * Copyright 2013-2016 Freescale Semiconductor Inc.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of 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") as published by the Free Software
- * Foundation, either version 2 of that License or (at your option) any
- * later version.
- *
- * THIS SOFTWARE IS PROVIDED BY 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 HOLDERS 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 "../include/mc-sys.h"
-#include "../include/mc-cmd.h"
-#include "../include/dpmng.h"
-
-#include "dpmng-cmd.h"
-
-/**
- * mc_get_version() - Retrieves the Management Complex firmware
- *			version information
- * @mc_io:		Pointer to opaque I/O object
- * @cmd_flags:		Command flags; one or more of 'MC_CMD_FLAG_'
- * @mc_ver_info:	Returned version information structure
- *
- * Return:	'0' on Success; Error code otherwise.
- */
-int mc_get_version(struct fsl_mc_io *mc_io,
-		   u32 cmd_flags,
-		   struct mc_version *mc_ver_info)
-{
-	struct mc_command cmd = { 0 };
-	struct dpmng_rsp_get_version *rsp_params;
-	int err;
-
-	/* prepare command */
-	cmd.header = mc_encode_cmd_header(DPMNG_CMDID_GET_VERSION,
-					  cmd_flags,
-					  0);
-
-	/* send command to mc*/
-	err = mc_send_command(mc_io, &cmd);
-	if (err)
-		return err;
-
-	/* retrieve response parameters */
-	rsp_params = (struct dpmng_rsp_get_version *)cmd.params;
-	mc_ver_info->revision = le32_to_cpu(rsp_params->revision);
-	mc_ver_info->major = le32_to_cpu(rsp_params->version_major);
-	mc_ver_info->minor = le32_to_cpu(rsp_params->version_minor);
-
-	return 0;
-}
-EXPORT_SYMBOL(mc_get_version);
-
diff --git a/drivers/staging/fsl-mc/bus/dprc-cmd.h b/drivers/staging/fsl-mc/bus/dprc-cmd.h
index e9fdca41f324..d9b2dcde468e 100644
--- a/drivers/staging/fsl-mc/bus/dprc-cmd.h
+++ b/drivers/staging/fsl-mc/bus/dprc-cmd.h
@@ -48,7 +48,7 @@
 #define DPRC_CMD_BASE_VERSION			1
 #define DPRC_CMD_ID_OFFSET			4
 
-#define DPRC_CMD(id)	((id << DPRC_CMD_ID_OFFSET) | DPRC_CMD_BASE_VERSION)
+#define DPRC_CMD(id)	(((id) << DPRC_CMD_ID_OFFSET) | DPRC_CMD_BASE_VERSION)
 
 /* Command IDs */
 #define DPRC_CMDID_CLOSE                        DPRC_CMD(0x800)
diff --git a/drivers/staging/fsl-mc/bus/dprc-driver.c b/drivers/staging/fsl-mc/bus/dprc-driver.c
index e4b0341d42d7..4cdd190a338b 100644
--- a/drivers/staging/fsl-mc/bus/dprc-driver.c
+++ b/drivers/staging/fsl-mc/bus/dprc-driver.c
@@ -13,27 +13,30 @@
 #include <linux/slab.h>
 #include <linux/interrupt.h>
 #include <linux/msi.h>
-#include "../include/mc-bus.h"
-#include "../include/mc-sys.h"
+#include "../include/mc.h"
 
 #include "dprc-cmd.h"
 #include "fsl-mc-private.h"
 
 #define FSL_MC_DPRC_DRIVER_NAME    "fsl_mc_dprc"
 
-#define FSL_MC_DEVICE_MATCH(_mc_dev, _obj_desc) \
-	(strcmp((_mc_dev)->obj_desc.type, (_obj_desc)->type) == 0 && \
-	 (_mc_dev)->obj_desc.id == (_obj_desc)->id)
-
-struct dprc_child_objs {
+struct fsl_mc_child_objs {
 	int child_count;
-	struct dprc_obj_desc *child_array;
+	struct fsl_mc_obj_desc *child_array;
 };
 
+static bool fsl_mc_device_match(struct fsl_mc_device *mc_dev,
+				struct fsl_mc_obj_desc *obj_desc)
+{
+	return mc_dev->obj_desc.id == obj_desc->id &&
+	       !strcmp(mc_dev->obj_desc.type, obj_desc->type);
+
+}
+
 static int __fsl_mc_device_remove_if_not_in_mc(struct device *dev, void *data)
 {
 	int i;
-	struct dprc_child_objs *objs;
+	struct fsl_mc_child_objs *objs;
 	struct fsl_mc_device *mc_dev;
 
 	WARN_ON(!dev);
@@ -42,10 +45,10 @@ static int __fsl_mc_device_remove_if_not_in_mc(struct device *dev, void *data)
 	objs = data;
 
 	for (i = 0; i < objs->child_count; i++) {
-		struct dprc_obj_desc *obj_desc = &objs->child_array[i];
+		struct fsl_mc_obj_desc *obj_desc = &objs->child_array[i];
 
 		if (strlen(obj_desc->type) != 0 &&
-		    FSL_MC_DEVICE_MATCH(mc_dev, obj_desc))
+		    fsl_mc_device_match(mc_dev, obj_desc))
 			break;
 	}
 
@@ -76,7 +79,7 @@ static int __fsl_mc_device_remove(struct device *dev, void *data)
  * been dynamically removed in the physical DPRC.
  */
 static void dprc_remove_devices(struct fsl_mc_device *mc_bus_dev,
-				struct dprc_obj_desc *obj_desc_array,
+				struct fsl_mc_obj_desc *obj_desc_array,
 				int num_child_objects_in_mc)
 {
 	if (num_child_objects_in_mc != 0) {
@@ -84,7 +87,7 @@ static void dprc_remove_devices(struct fsl_mc_device *mc_bus_dev,
 		 * Remove child objects that are in the DPRC in Linux,
 		 * but not in the MC:
 		 */
-		struct dprc_child_objs objs;
+		struct fsl_mc_child_objs objs;
 
 		objs.child_count = num_child_objects_in_mc;
 		objs.child_array = obj_desc_array;
@@ -102,13 +105,13 @@ static void dprc_remove_devices(struct fsl_mc_device *mc_bus_dev,
 
 static int __fsl_mc_device_match(struct device *dev, void *data)
 {
-	struct dprc_obj_desc *obj_desc = data;
+	struct fsl_mc_obj_desc *obj_desc = data;
 	struct fsl_mc_device *mc_dev = to_fsl_mc_device(dev);
 
-	return FSL_MC_DEVICE_MATCH(mc_dev, obj_desc);
+	return fsl_mc_device_match(mc_dev, obj_desc);
 }
 
-static struct fsl_mc_device *fsl_mc_device_lookup(struct dprc_obj_desc
+static struct fsl_mc_device *fsl_mc_device_lookup(struct fsl_mc_obj_desc
 								*obj_desc,
 						  struct fsl_mc_device
 								*mc_bus_dev)
@@ -133,16 +136,16 @@ static struct fsl_mc_device *fsl_mc_device_lookup(struct dprc_obj_desc
  * device is unbound from the corresponding device driver.
  */
 static void check_plugged_state_change(struct fsl_mc_device *mc_dev,
-				       struct dprc_obj_desc *obj_desc)
+				       struct fsl_mc_obj_desc *obj_desc)
 {
 	int error;
 	u32 plugged_flag_at_mc =
-			obj_desc->state & DPRC_OBJ_STATE_PLUGGED;
+			obj_desc->state & FSL_MC_OBJ_STATE_PLUGGED;
 
 	if (plugged_flag_at_mc !=
-	    (mc_dev->obj_desc.state & DPRC_OBJ_STATE_PLUGGED)) {
+	    (mc_dev->obj_desc.state & FSL_MC_OBJ_STATE_PLUGGED)) {
 		if (plugged_flag_at_mc) {
-			mc_dev->obj_desc.state |= DPRC_OBJ_STATE_PLUGGED;
+			mc_dev->obj_desc.state |= FSL_MC_OBJ_STATE_PLUGGED;
 			error = device_attach(&mc_dev->dev);
 			if (error < 0) {
 				dev_err(&mc_dev->dev,
@@ -150,7 +153,7 @@ static void check_plugged_state_change(struct fsl_mc_device *mc_dev,
 					error);
 			}
 		} else {
-			mc_dev->obj_desc.state &= ~DPRC_OBJ_STATE_PLUGGED;
+			mc_dev->obj_desc.state &= ~FSL_MC_OBJ_STATE_PLUGGED;
 			device_release_driver(&mc_dev->dev);
 		}
 	}
@@ -169,7 +172,7 @@ static void check_plugged_state_change(struct fsl_mc_device *mc_dev,
  * in the physical DPRC.
  */
 static void dprc_add_new_devices(struct fsl_mc_device *mc_bus_dev,
-				 struct dprc_obj_desc *obj_desc_array,
+				 struct fsl_mc_obj_desc *obj_desc_array,
 				 int num_child_objects_in_mc)
 {
 	int error;
@@ -177,7 +180,7 @@ static void dprc_add_new_devices(struct fsl_mc_device *mc_bus_dev,
 
 	for (i = 0; i < num_child_objects_in_mc; i++) {
 		struct fsl_mc_device *child_dev;
-		struct dprc_obj_desc *obj_desc = &obj_desc_array[i];
+		struct fsl_mc_obj_desc *obj_desc = &obj_desc_array[i];
 
 		if (strlen(obj_desc->type) == 0)
 			continue;
@@ -217,14 +220,14 @@ static void dprc_add_new_devices(struct fsl_mc_device *mc_bus_dev,
  * populated before they can get allocation requests from probe callbacks
  * of the device drivers for the non-allocatable devices.
  */
-int dprc_scan_objects(struct fsl_mc_device *mc_bus_dev,
-		      unsigned int *total_irq_count)
+static int dprc_scan_objects(struct fsl_mc_device *mc_bus_dev,
+			     unsigned int *total_irq_count)
 {
 	int num_child_objects;
 	int dprc_get_obj_failures;
 	int error;
 	unsigned int irq_count = mc_bus_dev->obj_desc.irq_count;
-	struct dprc_obj_desc *child_obj_desc_array = NULL;
+	struct fsl_mc_obj_desc *child_obj_desc_array = NULL;
 
 	error = dprc_get_obj_count(mc_bus_dev->mc_io,
 				   0,
@@ -251,7 +254,7 @@ int dprc_scan_objects(struct fsl_mc_device *mc_bus_dev,
 		 */
 		dprc_get_obj_failures = 0;
 		for (i = 0; i < num_child_objects; i++) {
-			struct dprc_obj_desc *obj_desc =
+			struct fsl_mc_obj_desc *obj_desc =
 			    &child_obj_desc_array[i];
 
 			error = dprc_get_obj(mc_bus_dev->mc_io,
@@ -279,7 +282,7 @@ int dprc_scan_objects(struct fsl_mc_device *mc_bus_dev,
 			if ((strcmp(obj_desc->type, "dpseci") == 0) &&
 			    (obj_desc->ver_major < 4))
 				obj_desc->flags |=
-					DPRC_OBJ_FLAG_NO_MEM_SHAREABILITY;
+					FSL_MC_OBJ_FLAG_NO_MEM_SHAREABILITY;
 
 			irq_count += obj_desc->irq_count;
 			dev_dbg(&mc_bus_dev->dev,
@@ -306,7 +309,6 @@ int dprc_scan_objects(struct fsl_mc_device *mc_bus_dev,
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(dprc_scan_objects);
 
 /**
  * dprc_scan_container - Scans a physical DPRC and synchronizes Linux bus state
@@ -317,7 +319,7 @@ EXPORT_SYMBOL_GPL(dprc_scan_objects);
  * bus driver with the actual state of the MC by adding and removing
  * devices as appropriate.
  */
-int dprc_scan_container(struct fsl_mc_device *mc_bus_dev)
+static int dprc_scan_container(struct fsl_mc_device *mc_bus_dev)
 {
 	int error;
 	unsigned int irq_count;
@@ -353,7 +355,6 @@ error:
 	fsl_mc_cleanup_all_resource_pools(mc_bus_dev);
 	return error;
 }
-EXPORT_SYMBOL_GPL(dprc_scan_container);
 
 /**
  * dprc_irq0_handler - Regular ISR for DPRC interrupt 0
@@ -681,8 +682,8 @@ static int dprc_probe(struct fsl_mc_device *mc_dev)
 	}
 
 	if (major_ver < DPRC_MIN_VER_MAJOR ||
-	   (major_ver == DPRC_MIN_VER_MAJOR &&
-	    minor_ver < DPRC_MIN_VER_MINOR)) {
+	    (major_ver == DPRC_MIN_VER_MAJOR &&
+	     minor_ver < DPRC_MIN_VER_MINOR)) {
 		dev_err(&mc_dev->dev,
 			"ERROR: DPRC version %d.%d not supported\n",
 			major_ver, minor_ver);
diff --git a/drivers/staging/fsl-mc/bus/dprc.c b/drivers/staging/fsl-mc/bus/dprc.c
index fcf7b4767dc0..6f6c65a42166 100644
--- a/drivers/staging/fsl-mc/bus/dprc.c
+++ b/drivers/staging/fsl-mc/bus/dprc.c
@@ -29,9 +29,9 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-#include "../include/mc-sys.h"
-#include "../include/mc-cmd.h"
-#include "../include/dprc.h"
+#include <linux/kernel.h>
+#include "../include/mc.h"
+#include "dprc.h"
 
 #include "dprc-cmd.h"
 
@@ -496,7 +496,7 @@ int dprc_get_obj(struct fsl_mc_io *mc_io,
 		 u32 cmd_flags,
 		 u16 token,
 		 int obj_index,
-		 struct dprc_obj_desc *obj_desc)
+		 struct fsl_mc_obj_desc *obj_desc)
 {
 	struct mc_command cmd = { 0 };
 	struct dprc_cmd_get_obj *cmd_params;
diff --git a/drivers/staging/fsl-mc/include/dprc.h b/drivers/staging/fsl-mc/bus/dprc.h
index dc985cc1246f..21295e4feb04 100644
--- a/drivers/staging/fsl-mc/include/dprc.h
+++ b/drivers/staging/fsl-mc/bus/dprc.h
@@ -33,14 +33,13 @@
 #ifndef _FSL_DPRC_H
 #define _FSL_DPRC_H
 
-#include "mc-cmd.h"
-
 /*
  * Data Path Resource Container API
  * Contains DPRC API for managing and querying DPAA resources
  */
 
 struct fsl_mc_io;
+struct fsl_mc_obj_desc;
 
 int dprc_open(struct fsl_mc_io *mc_io,
 	      u32 cmd_flags,
@@ -51,7 +50,6 @@ int dprc_close(struct fsl_mc_io *mc_io,
 	       u32 cmd_flags,
 	       u16 token);
 
-
 /* IRQ */
 
 /* IRQ index */
@@ -170,59 +168,18 @@ int dprc_get_obj_count(struct fsl_mc_io *mc_io,
 		       u16 token,
 		       int *obj_count);
 
-/* Objects Attributes Flags */
-
-/* Opened state - Indicates that an object is open by at least one owner */
-#define DPRC_OBJ_STATE_OPEN		0x00000001
-/* Plugged state - Indicates that the object is plugged */
-#define DPRC_OBJ_STATE_PLUGGED		0x00000002
-
-/**
- * Shareability flag - Object flag indicating no memory shareability.
- * the object generates memory accesses that are non coherent with other
- * masters;
- * user is responsible for proper memory handling through IOMMU configuration.
- */
-#define DPRC_OBJ_FLAG_NO_MEM_SHAREABILITY	0x0001
-
-/**
- * struct dprc_obj_desc - Object descriptor, returned from dprc_get_obj()
- * @type: Type of object: NULL terminated string
- * @id: ID of logical object resource
- * @vendor: Object vendor identifier
- * @ver_major: Major version number
- * @ver_minor:  Minor version number
- * @irq_count: Number of interrupts supported by the object
- * @region_count: Number of mappable regions supported by the object
- * @state: Object state: combination of DPRC_OBJ_STATE_ states
- * @label: Object label
- * @flags: Object's flags
- */
-struct dprc_obj_desc {
-	char type[16];
-	int id;
-	u16 vendor;
-	u16 ver_major;
-	u16 ver_minor;
-	u8 irq_count;
-	u8 region_count;
-	u32 state;
-	char label[16];
-	u16 flags;
-};
-
 int dprc_get_obj(struct fsl_mc_io *mc_io,
 		 u32 cmd_flags,
 		 u16 token,
 		 int obj_index,
-		 struct dprc_obj_desc *obj_desc);
+		 struct fsl_mc_obj_desc *obj_desc);
 
 int dprc_get_obj_desc(struct fsl_mc_io *mc_io,
 		      u32 cmd_flags,
 		      u16 token,
 		      char *obj_type,
 		      int obj_id,
-		      struct dprc_obj_desc *obj_desc);
+		      struct fsl_mc_obj_desc *obj_desc);
 
 int dprc_set_obj_irq(struct fsl_mc_io *mc_io,
 		     u32 cmd_flags,
diff --git a/drivers/staging/fsl-mc/bus/fsl-mc-allocator.c b/drivers/staging/fsl-mc/bus/fsl-mc-allocator.c
index ce07096c3b1f..b37a6f48225f 100644
--- a/drivers/staging/fsl-mc/bus/fsl-mc-allocator.c
+++ b/drivers/staging/fsl-mc/bus/fsl-mc-allocator.c
@@ -10,17 +10,16 @@
 
 #include <linux/module.h>
 #include <linux/msi.h>
-#include "../include/mc-bus.h"
-#include "../include/mc-sys.h"
+#include "../include/mc.h"
 
-#include "dpbp-cmd.h"
-#include "dpcon-cmd.h"
 #include "fsl-mc-private.h"
 
-#define FSL_MC_IS_ALLOCATABLE(_obj_type) \
-	(strcmp(_obj_type, "dpbp") == 0 || \
-	 strcmp(_obj_type, "dpmcp") == 0 || \
-	 strcmp(_obj_type, "dpcon") == 0)
+static bool __must_check fsl_mc_is_allocatable(const char *obj_type)
+{
+	return strcmp(obj_type, "dpbp") ||
+	       strcmp(obj_type, "dpmcp") ||
+	       strcmp(obj_type, "dpcon");
+}
 
 /**
  * fsl_mc_resource_pool_add_device - add allocatable object to a resource
@@ -44,7 +43,7 @@ static int __must_check fsl_mc_resource_pool_add_device(struct fsl_mc_bus
 
 	if (WARN_ON(pool_type < 0 || pool_type >= FSL_MC_NUM_POOL_TYPES))
 		goto out;
-	if (WARN_ON(!FSL_MC_IS_ALLOCATABLE(mc_dev->obj_desc.type)))
+	if (WARN_ON(!fsl_mc_is_allocatable(mc_dev->obj_desc.type)))
 		goto out;
 	if (WARN_ON(mc_dev->resource))
 		goto out;
@@ -106,7 +105,7 @@ static int __must_check fsl_mc_resource_pool_remove_device(struct fsl_mc_device
 	struct fsl_mc_resource *resource;
 	int error = -EINVAL;
 
-	if (WARN_ON(!FSL_MC_IS_ALLOCATABLE(mc_dev->obj_desc.type)))
+	if (WARN_ON(!fsl_mc_is_allocatable(mc_dev->obj_desc.type)))
 		goto out;
 
 	resource = mc_dev->resource;
@@ -586,7 +585,7 @@ static int fsl_mc_allocator_probe(struct fsl_mc_device *mc_dev)
 	struct fsl_mc_bus *mc_bus;
 	int error;
 
-	if (WARN_ON(!FSL_MC_IS_ALLOCATABLE(mc_dev->obj_desc.type)))
+	if (WARN_ON(!fsl_mc_is_allocatable(mc_dev->obj_desc.type)))
 		return -EINVAL;
 
 	mc_bus_dev = to_fsl_mc_device(mc_dev->dev.parent);
@@ -615,7 +614,7 @@ static int fsl_mc_allocator_remove(struct fsl_mc_device *mc_dev)
 {
 	int error;
 
-	if (WARN_ON(!FSL_MC_IS_ALLOCATABLE(mc_dev->obj_desc.type)))
+	if (WARN_ON(!fsl_mc_is_allocatable(mc_dev->obj_desc.type)))
 		return -EINVAL;
 
 	if (mc_dev->resource) {
diff --git a/drivers/staging/fsl-mc/bus/fsl-mc-bus.c b/drivers/staging/fsl-mc/bus/fsl-mc-bus.c
index 3be5f25ff113..19606e8d25dd 100644
--- a/drivers/staging/fsl-mc/bus/fsl-mc-bus.c
+++ b/drivers/staging/fsl-mc/bus/fsl-mc-bus.c
@@ -20,12 +20,10 @@
 #include <linux/bitops.h>
 #include <linux/msi.h>
 #include <linux/dma-mapping.h>
-#include "../include/mc-bus.h"
-#include "../include/dpmng.h"
-#include "../include/mc-sys.h"
 
 #include "fsl-mc-private.h"
 #include "dprc-cmd.h"
+#include "dpmng-cmd.h"
 
 /**
  * Default DMA mask for devices on a fsl-mc bus
@@ -61,6 +59,20 @@ struct fsl_mc_addr_translation_range {
 };
 
 /**
+ * struct mc_version
+ * @major: Major version number: incremented on API compatibility changes
+ * @minor: Minor version number: incremented on API additions (that are
+ *		backward compatible); reset when major version is incremented
+ * @revision: Internal revision number: incremented on implementation changes
+ *		and/or bug fixes that have no impact on API
+ */
+struct mc_version {
+	u32 major;
+	u32 minor;
+	u32 revision;
+};
+
+/**
  * fsl_mc_bus_match - device to driver matching callback
  * @dev: the fsl-mc device to match against
  * @drv: the device driver to search for matching fsl-mc object type
@@ -82,7 +94,7 @@ static int fsl_mc_bus_match(struct device *dev, struct device_driver *drv)
 	 * If the object is not 'plugged' don't match.
 	 * Only exception is the root DPRC, which is a special case.
 	 */
-	if ((mc_dev->obj_desc.state & DPRC_OBJ_STATE_PLUGGED) == 0 &&
+	if ((mc_dev->obj_desc.state & FSL_MC_OBJ_STATE_PLUGGED) == 0 &&
 	    !fsl_mc_is_root_dprc(&mc_dev->dev))
 		goto out;
 
@@ -239,10 +251,46 @@ void fsl_mc_driver_unregister(struct fsl_mc_driver *mc_driver)
 EXPORT_SYMBOL_GPL(fsl_mc_driver_unregister);
 
 /**
+ * mc_get_version() - Retrieves the Management Complex firmware
+ *			version information
+ * @mc_io:		Pointer to opaque I/O object
+ * @cmd_flags:		Command flags; one or more of 'MC_CMD_FLAG_'
+ * @mc_ver_info:	Returned version information structure
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+static int mc_get_version(struct fsl_mc_io *mc_io,
+			  u32 cmd_flags,
+			  struct mc_version *mc_ver_info)
+{
+	struct mc_command cmd = { 0 };
+	struct dpmng_rsp_get_version *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPMNG_CMDID_GET_VERSION,
+					  cmd_flags,
+					  0);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpmng_rsp_get_version *)cmd.params;
+	mc_ver_info->revision = le32_to_cpu(rsp_params->revision);
+	mc_ver_info->major = le32_to_cpu(rsp_params->version_major);
+	mc_ver_info->minor = le32_to_cpu(rsp_params->version_minor);
+
+	return 0;
+}
+
+/**
  * fsl_mc_get_root_dprc - function to traverse to the root dprc
  */
-void fsl_mc_get_root_dprc(struct device *dev,
-			  struct device **root_dprc_dev)
+static void fsl_mc_get_root_dprc(struct device *dev,
+				 struct device **root_dprc_dev)
 {
 	if (WARN_ON(!dev)) {
 		*root_dprc_dev = NULL;
@@ -254,7 +302,6 @@ void fsl_mc_get_root_dprc(struct device *dev,
 			*root_dprc_dev = (*root_dprc_dev)->parent;
 	}
 }
-EXPORT_SYMBOL_GPL(fsl_mc_get_root_dprc);
 
 static int get_dprc_attr(struct fsl_mc_io *mc_io,
 			 int container_id, struct dprc_attributes *attr)
@@ -339,7 +386,7 @@ static int fsl_mc_device_get_mmio_regions(struct fsl_mc_device *mc_dev,
 	int i;
 	int error;
 	struct resource *regions;
-	struct dprc_obj_desc *obj_desc = &mc_dev->obj_desc;
+	struct fsl_mc_obj_desc *obj_desc = &mc_dev->obj_desc;
 	struct device *parent_dev = mc_dev->dev.parent;
 	enum dprc_region_type mc_region_type;
 
@@ -420,15 +467,11 @@ bool fsl_mc_is_root_dprc(struct device *dev)
 static void fsl_mc_device_release(struct device *dev)
 {
 	struct fsl_mc_device *mc_dev = to_fsl_mc_device(dev);
-	struct fsl_mc_bus *mc_bus = NULL;
 
 	kfree(mc_dev->regions);
 
 	if (strcmp(mc_dev->obj_desc.type, "dprc") == 0)
-		mc_bus = to_fsl_mc_bus(mc_dev);
-
-	if (mc_bus)
-		kfree(mc_bus);
+		kfree(to_fsl_mc_bus(mc_dev));
 	else
 		kfree(mc_dev);
 }
@@ -436,7 +479,7 @@ static void fsl_mc_device_release(struct device *dev)
 /**
  * Add a newly discovered fsl-mc device to be visible in Linux
  */
-int fsl_mc_device_add(struct dprc_obj_desc *obj_desc,
+int fsl_mc_device_add(struct fsl_mc_obj_desc *obj_desc,
 		      struct fsl_mc_io *mc_io,
 		      struct device *parent_dev,
 		      struct fsl_mc_device **new_mc_dev)
@@ -538,7 +581,7 @@ int fsl_mc_device_add(struct dprc_obj_desc *obj_desc,
 	}
 
 	/* Objects are coherent, unless 'no shareability' flag set. */
-	if (!(obj_desc->flags & DPRC_OBJ_FLAG_NO_MEM_SHAREABILITY))
+	if (!(obj_desc->flags & FSL_MC_OBJ_FLAG_NO_MEM_SHAREABILITY))
 		arch_setup_dma_ops(&mc_dev->dev, 0, 0, NULL, true);
 
 	/*
@@ -559,10 +602,8 @@ int fsl_mc_device_add(struct dprc_obj_desc *obj_desc,
 
 error_cleanup_dev:
 	kfree(mc_dev->regions);
-	if (mc_bus)
-		kfree(mc_bus);
-	else
-		kfree(mc_dev);
+	kfree(mc_bus);
+	kfree(mc_dev);
 
 	return error;
 }
@@ -644,10 +685,10 @@ static int get_mc_addr_translation_ranges(struct device *dev,
 	const __be32 *cell;
 
 	ret = parse_mc_ranges(dev,
-				&paddr_cells,
-				&mc_addr_cells,
-				&mc_size_cells,
-				&ranges_start);
+			      &paddr_cells,
+			      &mc_addr_cells,
+			      &mc_size_cells,
+			      &ranges_start);
 	if (ret < 0)
 		return ret;
 
@@ -693,7 +734,7 @@ static int get_mc_addr_translation_ranges(struct device *dev,
  */
 static int fsl_mc_bus_probe(struct platform_device *pdev)
 {
-	struct dprc_obj_desc obj_desc;
+	struct fsl_mc_obj_desc obj_desc;
 	int error;
 	struct fsl_mc *mc;
 	struct fsl_mc_device *mc_bus_dev = NULL;
@@ -752,7 +793,7 @@ static int fsl_mc_bus_probe(struct platform_device *pdev)
 		goto error_cleanup_mc_io;
 	}
 
-	memset(&obj_desc, 0, sizeof(struct dprc_obj_desc));
+	memset(&obj_desc, 0, sizeof(struct fsl_mc_obj_desc));
 	error = dprc_get_api_version(mc_io, 0,
 				     &obj_desc.ver_major,
 				     &obj_desc.ver_minor);
diff --git a/drivers/staging/fsl-mc/bus/fsl-mc-msi.c b/drivers/staging/fsl-mc/bus/fsl-mc-msi.c
index b8b2c86e63d4..c04a2f2b3409 100644
--- a/drivers/staging/fsl-mc/bus/fsl-mc-msi.c
+++ b/drivers/staging/fsl-mc/bus/fsl-mc-msi.c
@@ -16,7 +16,6 @@
 #include <linux/irq.h>
 #include <linux/irqdomain.h>
 #include <linux/msi.h>
-#include "../include/mc-bus.h"
 #include "fsl-mc-private.h"
 
 /*
@@ -170,7 +169,7 @@ struct irq_domain *fsl_mc_msi_create_irq_domain(struct fwnode_handle *fwnode,
 
 	domain = msi_create_irq_domain(fwnode, info, parent);
 	if (domain)
-		domain->bus_token = DOMAIN_BUS_FSL_MC_MSI;
+		irq_domain_update_bus_token(domain, DOMAIN_BUS_FSL_MC_MSI);
 
 	return domain;
 }
diff --git a/drivers/staging/fsl-mc/bus/fsl-mc-private.h b/drivers/staging/fsl-mc/bus/fsl-mc-private.h
index 5c49c9d2df6a..62d398947605 100644
--- a/drivers/staging/fsl-mc/bus/fsl-mc-private.h
+++ b/drivers/staging/fsl-mc/bus/fsl-mc-private.h
@@ -11,9 +11,56 @@
 #define _FSL_MC_PRIVATE_H_
 
 #include "../include/mc.h"
-#include "../include/mc-bus.h"
+#include "dprc.h"
+#include <linux/mutex.h>
 
-int __must_check fsl_mc_device_add(struct dprc_obj_desc *obj_desc,
+/**
+ * Maximum number of total IRQs that can be pre-allocated for an MC bus'
+ * IRQ pool
+ */
+#define FSL_MC_IRQ_POOL_MAX_TOTAL_IRQS	256
+
+/**
+ * struct fsl_mc_resource_pool - Pool of MC resources of a given
+ * type
+ * @type: type of resources in the pool
+ * @max_count: maximum number of resources in the pool
+ * @free_count: number of free resources in the pool
+ * @mutex: mutex to serialize access to the pool's free list
+ * @free_list: anchor node of list of free resources in the pool
+ * @mc_bus: pointer to the MC bus that owns this resource pool
+ */
+struct fsl_mc_resource_pool {
+	enum fsl_mc_pool_type type;
+	int max_count;
+	int free_count;
+	struct mutex mutex;	/* serializes access to free_list */
+	struct list_head free_list;
+	struct fsl_mc_bus *mc_bus;
+};
+
+/**
+ * struct fsl_mc_bus - logical bus that corresponds to a physical DPRC
+ * @mc_dev: fsl-mc device for the bus device itself.
+ * @resource_pools: array of resource pools (one pool per resource type)
+ * for this MC bus. These resources represent allocatable entities
+ * from the physical DPRC.
+ * @irq_resources: Pointer to array of IRQ objects for the IRQ pool
+ * @scan_mutex: Serializes bus scanning
+ * @dprc_attr: DPRC attributes
+ */
+struct fsl_mc_bus {
+	struct fsl_mc_device mc_dev;
+	struct fsl_mc_resource_pool resource_pools[FSL_MC_NUM_POOL_TYPES];
+	struct fsl_mc_device_irq *irq_resources;
+	struct mutex scan_mutex;    /* serializes bus scanning */
+	struct dprc_attributes dprc_attr;
+};
+
+#define to_fsl_mc_bus(_mc_dev) \
+	container_of(_mc_dev, struct fsl_mc_bus, mc_dev)
+
+int __must_check fsl_mc_device_add(struct fsl_mc_obj_desc *obj_desc,
 				   struct fsl_mc_io *mc_io,
 				   struct device *parent_dev,
 				   struct fsl_mc_device **new_mc_dev);
@@ -28,6 +75,10 @@ int __init fsl_mc_allocator_driver_init(void);
 
 void fsl_mc_allocator_driver_exit(void);
 
+void fsl_mc_init_all_resource_pools(struct fsl_mc_device *mc_bus_dev);
+
+void fsl_mc_cleanup_all_resource_pools(struct fsl_mc_device *mc_bus_dev);
+
 int __must_check fsl_mc_resource_allocate(struct fsl_mc_bus *mc_bus,
 					  enum fsl_mc_pool_type pool_type,
 					  struct fsl_mc_resource
@@ -44,6 +95,14 @@ int __init its_fsl_mc_msi_init(void);
 
 void its_fsl_mc_msi_cleanup(void);
 
+int fsl_mc_find_msi_domain(struct device *mc_platform_dev,
+			   struct irq_domain **mc_msi_domain);
+
+int fsl_mc_populate_irq_pool(struct fsl_mc_bus *mc_bus,
+			     unsigned int irq_count);
+
+void fsl_mc_cleanup_irq_pool(struct fsl_mc_bus *mc_bus);
+
 int __must_check fsl_create_mc_io(struct device *dev,
 				  phys_addr_t mc_portal_phys_addr,
 				  u32 mc_portal_size,
@@ -52,4 +111,6 @@ int __must_check fsl_create_mc_io(struct device *dev,
 
 void fsl_destroy_mc_io(struct fsl_mc_io *mc_io);
 
+bool fsl_mc_is_root_dprc(struct device *dev);
+
 #endif /* _FSL_MC_PRIVATE_H_ */
diff --git a/drivers/staging/fsl-mc/bus/irq-gic-v3-its-fsl-mc-msi.c b/drivers/staging/fsl-mc/bus/irq-gic-v3-its-fsl-mc-msi.c
index 49127acb85b2..865d38517508 100644
--- a/drivers/staging/fsl-mc/bus/irq-gic-v3-its-fsl-mc-msi.c
+++ b/drivers/staging/fsl-mc/bus/irq-gic-v3-its-fsl-mc-msi.c
@@ -16,7 +16,6 @@
 #include <linux/msi.h>
 #include <linux/of.h>
 #include <linux/of_irq.h>
-#include "../include/mc-bus.h"
 #include "fsl-mc-private.h"
 
 static struct irq_chip its_msi_irq_chip = {
diff --git a/drivers/staging/fsl-mc/bus/mc-io.c b/drivers/staging/fsl-mc/bus/mc-io.c
index d66b87f0903b..35221a17858b 100644
--- a/drivers/staging/fsl-mc/bus/mc-io.c
+++ b/drivers/staging/fsl-mc/bus/mc-io.c
@@ -31,8 +31,7 @@
  */
 
 #include <linux/io.h>
-#include "../include/mc-bus.h"
-#include "../include/mc-sys.h"
+#include "../include/mc.h"
 
 #include "fsl-mc-private.h"
 #include "dpmcp.h"
diff --git a/drivers/staging/fsl-mc/bus/mc-sys.c b/drivers/staging/fsl-mc/bus/mc-sys.c
index 4d82802b384d..a1704c3a6a78 100644
--- a/drivers/staging/fsl-mc/bus/mc-sys.c
+++ b/drivers/staging/fsl-mc/bus/mc-sys.c
@@ -37,8 +37,6 @@
 #include <linux/ioport.h>
 #include <linux/device.h>
 #include <linux/io.h>
-#include "../include/mc-sys.h"
-#include "../include/mc-cmd.h"
 #include "../include/mc.h"
 
 #include "dpmcp.h"
diff --git a/drivers/staging/fsl-mc/include/dpmng.h b/drivers/staging/fsl-mc/include/dpmng.h
deleted file mode 100644
index 170c07dd376a..000000000000
--- a/drivers/staging/fsl-mc/include/dpmng.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/*
- * Copyright 2013-2016 Freescale Semiconductor Inc.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of 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") as published by the Free Software
- * Foundation, either version 2 of that License or (at your option) any
- * later version.
- *
- * THIS SOFTWARE IS PROVIDED BY 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 HOLDERS 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 __FSL_DPMNG_H
-#define __FSL_DPMNG_H
-
-/*
- * Management Complex General API
- * Contains general API for the Management Complex firmware
- */
-
-struct fsl_mc_io;
-
-/**
- * Management Complex firmware version information
- */
-#define MC_VER_MAJOR 8
-#define MC_VER_MINOR 0
-
-/**
- * struct mc_version
- * @major: Major version number: incremented on API compatibility changes
- * @minor: Minor version number: incremented on API additions (that are
- *		backward compatible); reset when major version is incremented
- * @revision: Internal revision number: incremented on implementation changes
- *		and/or bug fixes that have no impact on API
- */
-struct mc_version {
-	u32 major;
-	u32 minor;
-	u32 revision;
-};
-
-int mc_get_version(struct fsl_mc_io *mc_io,
-		   u32 cmd_flags,
-		   struct mc_version *mc_ver_info);
-
-#endif /* __FSL_DPMNG_H */
diff --git a/drivers/staging/fsl-mc/include/mc-bus.h b/drivers/staging/fsl-mc/include/mc-bus.h
deleted file mode 100644
index 42700de94d59..000000000000
--- a/drivers/staging/fsl-mc/include/mc-bus.h
+++ /dev/null
@@ -1,111 +0,0 @@
-/*
- * Freescale Management Complex (MC) bus declarations
- *
- * Copyright (C) 2014-2016 Freescale Semiconductor, Inc.
- * Author: German Rivera <German.Rivera@freescale.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.
- */
-#ifndef _FSL_MC_MCBUS_H_
-#define _FSL_MC_MCBUS_H_
-
-#include "../include/mc.h"
-#include <linux/mutex.h>
-
-struct irq_domain;
-struct msi_domain_info;
-
-/**
- * Maximum number of total IRQs that can be pre-allocated for an MC bus'
- * IRQ pool
- */
-#define FSL_MC_IRQ_POOL_MAX_TOTAL_IRQS	256
-
-#ifdef CONFIG_FSL_MC_BUS
-#define dev_is_fsl_mc(_dev) ((_dev)->bus == &fsl_mc_bus_type)
-#else
-/* If fsl-mc bus is not present device cannot belong to fsl-mc bus */
-#define dev_is_fsl_mc(_dev) (0)
-#endif
-
-/**
- * struct fsl_mc_resource_pool - Pool of MC resources of a given
- * type
- * @type: type of resources in the pool
- * @max_count: maximum number of resources in the pool
- * @free_count: number of free resources in the pool
- * @mutex: mutex to serialize access to the pool's free list
- * @free_list: anchor node of list of free resources in the pool
- * @mc_bus: pointer to the MC bus that owns this resource pool
- */
-struct fsl_mc_resource_pool {
-	enum fsl_mc_pool_type type;
-	int max_count;
-	int free_count;
-	struct mutex mutex;	/* serializes access to free_list */
-	struct list_head free_list;
-	struct fsl_mc_bus *mc_bus;
-};
-
-/**
- * struct fsl_mc_bus - logical bus that corresponds to a physical DPRC
- * @mc_dev: fsl-mc device for the bus device itself.
- * @resource_pools: array of resource pools (one pool per resource type)
- * for this MC bus. These resources represent allocatable entities
- * from the physical DPRC.
- * @irq_resources: Pointer to array of IRQ objects for the IRQ pool
- * @scan_mutex: Serializes bus scanning
- * @dprc_attr: DPRC attributes
- */
-struct fsl_mc_bus {
-	struct fsl_mc_device mc_dev;
-	struct fsl_mc_resource_pool resource_pools[FSL_MC_NUM_POOL_TYPES];
-	struct fsl_mc_device_irq *irq_resources;
-	struct mutex scan_mutex;    /* serializes bus scanning */
-	struct dprc_attributes dprc_attr;
-};
-
-#define to_fsl_mc_bus(_mc_dev) \
-	container_of(_mc_dev, struct fsl_mc_bus, mc_dev)
-
-int dprc_scan_container(struct fsl_mc_device *mc_bus_dev);
-
-int dprc_scan_objects(struct fsl_mc_device *mc_bus_dev,
-		      unsigned int *total_irq_count);
-
-int __init dprc_driver_init(void);
-
-void dprc_driver_exit(void);
-
-int __init fsl_mc_allocator_driver_init(void);
-
-void fsl_mc_allocator_driver_exit(void);
-
-struct irq_domain *fsl_mc_msi_create_irq_domain(struct fwnode_handle *fwnode,
-						struct msi_domain_info *info,
-						struct irq_domain *parent);
-
-int fsl_mc_find_msi_domain(struct device *mc_platform_dev,
-			   struct irq_domain **mc_msi_domain);
-
-int fsl_mc_populate_irq_pool(struct fsl_mc_bus *mc_bus,
-			     unsigned int irq_count);
-
-void fsl_mc_cleanup_irq_pool(struct fsl_mc_bus *mc_bus);
-
-void fsl_mc_init_all_resource_pools(struct fsl_mc_device *mc_bus_dev);
-
-void fsl_mc_cleanup_all_resource_pools(struct fsl_mc_device *mc_bus_dev);
-
-bool fsl_mc_bus_exists(void);
-
-void fsl_mc_get_root_dprc(struct device *dev,
-			  struct device **root_dprc_dev);
-
-bool fsl_mc_is_root_dprc(struct device *dev);
-
-extern struct bus_type fsl_mc_bus_type;
-
-#endif /* _FSL_MC_MCBUS_H_ */
diff --git a/drivers/staging/fsl-mc/include/mc-cmd.h b/drivers/staging/fsl-mc/include/mc-cmd.h
deleted file mode 100644
index 2e08aa31b084..000000000000
--- a/drivers/staging/fsl-mc/include/mc-cmd.h
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
- * Copyright 2013-2016 Freescale Semiconductor Inc.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of 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") as published by the Free Software
- * Foundation, either version 2 of that License or (at your option) any
- * later version.
- *
- * THIS SOFTWARE IS PROVIDED BY 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 HOLDERS 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 __FSL_MC_CMD_H
-#define __FSL_MC_CMD_H
-
-#define MC_CMD_NUM_OF_PARAMS	7
-
-struct mc_cmd_header {
-	u8 src_id;
-	u8 flags_hw;
-	u8 status;
-	u8 flags_sw;
-	__le16 token;
-	__le16 cmd_id;
-};
-
-struct mc_command {
-	u64 header;
-	u64 params[MC_CMD_NUM_OF_PARAMS];
-};
-
-struct mc_rsp_create {
-	__le32 object_id;
-};
-
-struct mc_rsp_api_ver {
-	__le16 major_ver;
-	__le16 minor_ver;
-};
-
-enum mc_cmd_status {
-	MC_CMD_STATUS_OK = 0x0, /* Completed successfully */
-	MC_CMD_STATUS_READY = 0x1, /* Ready to be processed */
-	MC_CMD_STATUS_AUTH_ERR = 0x3, /* Authentication error */
-	MC_CMD_STATUS_NO_PRIVILEGE = 0x4, /* No privilege */
-	MC_CMD_STATUS_DMA_ERR = 0x5, /* DMA or I/O error */
-	MC_CMD_STATUS_CONFIG_ERR = 0x6, /* Configuration error */
-	MC_CMD_STATUS_TIMEOUT = 0x7, /* Operation timed out */
-	MC_CMD_STATUS_NO_RESOURCE = 0x8, /* No resources */
-	MC_CMD_STATUS_NO_MEMORY = 0x9, /* No memory available */
-	MC_CMD_STATUS_BUSY = 0xA, /* Device is busy */
-	MC_CMD_STATUS_UNSUPPORTED_OP = 0xB, /* Unsupported operation */
-	MC_CMD_STATUS_INVALID_STATE = 0xC /* Invalid state */
-};
-
-/*
- * MC command flags
- */
-
-/* High priority flag */
-#define MC_CMD_FLAG_PRI		0x80
-/* Command completion flag */
-#define MC_CMD_FLAG_INTR_DIS	0x01
-
-static inline u64 mc_encode_cmd_header(u16 cmd_id,
-				       u32 cmd_flags,
-				       u16 token)
-{
-	u64 header = 0;
-	struct mc_cmd_header *hdr = (struct mc_cmd_header *)&header;
-
-	hdr->cmd_id = cpu_to_le16(cmd_id);
-	hdr->token  = cpu_to_le16(token);
-	hdr->status = MC_CMD_STATUS_READY;
-	if (cmd_flags & MC_CMD_FLAG_PRI)
-		hdr->flags_hw = MC_CMD_FLAG_PRI;
-	if (cmd_flags & MC_CMD_FLAG_INTR_DIS)
-		hdr->flags_sw = MC_CMD_FLAG_INTR_DIS;
-
-	return header;
-}
-
-static inline u16 mc_cmd_hdr_read_token(struct mc_command *cmd)
-{
-	struct mc_cmd_header *hdr = (struct mc_cmd_header *)&cmd->header;
-	u16 token = le16_to_cpu(hdr->token);
-
-	return token;
-}
-
-static inline u32 mc_cmd_read_object_id(struct mc_command *cmd)
-{
-	struct mc_rsp_create *rsp_params;
-
-	rsp_params = (struct mc_rsp_create *)cmd->params;
-	return le32_to_cpu(rsp_params->object_id);
-}
-
-static inline void mc_cmd_read_api_version(struct mc_command *cmd,
-					   u16 *major_ver,
-					   u16 *minor_ver)
-{
-	struct mc_rsp_api_ver *rsp_params;
-
-	rsp_params = (struct mc_rsp_api_ver *)cmd->params;
-	*major_ver = le16_to_cpu(rsp_params->major_ver);
-	*minor_ver = le16_to_cpu(rsp_params->minor_ver);
-}
-
-#endif /* __FSL_MC_CMD_H */
diff --git a/drivers/staging/fsl-mc/include/mc-sys.h b/drivers/staging/fsl-mc/include/mc-sys.h
deleted file mode 100644
index dca7f9084e05..000000000000
--- a/drivers/staging/fsl-mc/include/mc-sys.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- * Copyright 2013-2016 Freescale Semiconductor Inc.
- *
- * Interface of the I/O services to send MC commands to the MC hardware
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *     * Redistributions of 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 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") as published by the Free Software
- * Foundation, either version 2 of that License or (at your option) any
- * later version.
- *
- * THIS SOFTWARE IS PROVIDED BY 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 HOLDERS 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 _FSL_MC_SYS_H
-#define _FSL_MC_SYS_H
-
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/mutex.h>
-#include <linux/spinlock.h>
-
-/**
- * Bit masks for a MC I/O object (struct fsl_mc_io) flags
- */
-#define FSL_MC_IO_ATOMIC_CONTEXT_PORTAL	0x0001
-
-struct fsl_mc_resource;
-struct mc_command;
-
-/**
- * struct fsl_mc_io - MC I/O object to be passed-in to mc_send_command()
- * @dev: device associated with this Mc I/O object
- * @flags: flags for mc_send_command()
- * @portal_size: MC command portal size in bytes
- * @portal_phys_addr: MC command portal physical address
- * @portal_virt_addr: MC command portal virtual address
- * @dpmcp_dev: pointer to the DPMCP device associated with the MC portal.
- *
- * Fields are only meaningful if the FSL_MC_IO_ATOMIC_CONTEXT_PORTAL flag is not
- * set:
- * @mutex: Mutex to serialize mc_send_command() calls that use the same MC
- * portal, if the fsl_mc_io object was created with the
- * FSL_MC_IO_ATOMIC_CONTEXT_PORTAL flag off. mc_send_command() calls for this
- * fsl_mc_io object must be made only from non-atomic context.
- *
- * Fields are only meaningful if the FSL_MC_IO_ATOMIC_CONTEXT_PORTAL flag is
- * set:
- * @spinlock: Spinlock to serialize mc_send_command() calls that use the same MC
- * portal, if the fsl_mc_io object was created with the
- * FSL_MC_IO_ATOMIC_CONTEXT_PORTAL flag on. mc_send_command() calls for this
- * fsl_mc_io object can be made from atomic or non-atomic context.
- */
-struct fsl_mc_io {
-	struct device *dev;
-	u16 flags;
-	u16 portal_size;
-	phys_addr_t portal_phys_addr;
-	void __iomem *portal_virt_addr;
-	struct fsl_mc_device *dpmcp_dev;
-	union {
-		/*
-		 * This field is only meaningful if the
-		 * FSL_MC_IO_ATOMIC_CONTEXT_PORTAL flag is not set
-		 */
-		struct mutex mutex; /* serializes mc_send_command() */
-
-		/*
-		 * This field is only meaningful if the
-		 * FSL_MC_IO_ATOMIC_CONTEXT_PORTAL flag is set
-		 */
-		spinlock_t spinlock;	/* serializes mc_send_command() */
-	};
-};
-
-int mc_send_command(struct fsl_mc_io *mc_io, struct mc_command *cmd);
-
-#endif /* _FSL_MC_SYS_H */
diff --git a/drivers/staging/fsl-mc/include/mc.h b/drivers/staging/fsl-mc/include/mc.h
index 1c46c0c2a895..aafe63a21f49 100644
--- a/drivers/staging/fsl-mc/include/mc.h
+++ b/drivers/staging/fsl-mc/include/mc.h
@@ -14,10 +14,12 @@
 #include <linux/device.h>
 #include <linux/mod_devicetable.h>
 #include <linux/interrupt.h>
-#include "../include/dprc.h"
 
 #define FSL_MC_VENDOR_FREESCALE	0x1957
 
+struct irq_domain;
+struct msi_domain_info;
+
 struct fsl_mc_device;
 struct fsl_mc_io;
 
@@ -104,6 +106,45 @@ struct fsl_mc_device_irq {
 #define to_fsl_mc_irq(_mc_resource) \
 	container_of(_mc_resource, struct fsl_mc_device_irq, resource)
 
+/* Opened state - Indicates that an object is open by at least one owner */
+#define FSL_MC_OBJ_STATE_OPEN		0x00000001
+/* Plugged state - Indicates that the object is plugged */
+#define FSL_MC_OBJ_STATE_PLUGGED	0x00000002
+
+/**
+ * Shareability flag - Object flag indicating no memory shareability.
+ * the object generates memory accesses that are non coherent with other
+ * masters;
+ * user is responsible for proper memory handling through IOMMU configuration.
+ */
+#define FSL_MC_OBJ_FLAG_NO_MEM_SHAREABILITY	0x0001
+
+/**
+ * struct fsl_mc_obj_desc - Object descriptor
+ * @type: Type of object: NULL terminated string
+ * @id: ID of logical object resource
+ * @vendor: Object vendor identifier
+ * @ver_major: Major version number
+ * @ver_minor:  Minor version number
+ * @irq_count: Number of interrupts supported by the object
+ * @region_count: Number of mappable regions supported by the object
+ * @state: Object state: combination of FSL_MC_OBJ_STATE_ states
+ * @label: Object label: NULL terminated string
+ * @flags: Object's flags
+ */
+struct fsl_mc_obj_desc {
+	char type[16];
+	int id;
+	u16 vendor;
+	u16 ver_major;
+	u16 ver_minor;
+	u8 irq_count;
+	u8 region_count;
+	u32 state;
+	char label[16];
+	u16 flags;
+};
+
 /**
  * Bit masks for a MC object device (struct fsl_mc_device) flags
  */
@@ -150,7 +191,7 @@ struct fsl_mc_device {
 	u16 icid;
 	u16 mc_handle;
 	struct fsl_mc_io *mc_io;
-	struct dprc_obj_desc obj_desc;
+	struct fsl_mc_obj_desc obj_desc;
 	struct resource *regions;
 	struct fsl_mc_device_irq **irqs;
 	struct fsl_mc_resource *resource;
@@ -159,6 +200,159 @@ struct fsl_mc_device {
 #define to_fsl_mc_device(_dev) \
 	container_of(_dev, struct fsl_mc_device, dev)
 
+#define MC_CMD_NUM_OF_PARAMS	7
+
+struct mc_cmd_header {
+	u8 src_id;
+	u8 flags_hw;
+	u8 status;
+	u8 flags_sw;
+	__le16 token;
+	__le16 cmd_id;
+};
+
+struct mc_command {
+	u64 header;
+	u64 params[MC_CMD_NUM_OF_PARAMS];
+};
+
+enum mc_cmd_status {
+	MC_CMD_STATUS_OK = 0x0, /* Completed successfully */
+	MC_CMD_STATUS_READY = 0x1, /* Ready to be processed */
+	MC_CMD_STATUS_AUTH_ERR = 0x3, /* Authentication error */
+	MC_CMD_STATUS_NO_PRIVILEGE = 0x4, /* No privilege */
+	MC_CMD_STATUS_DMA_ERR = 0x5, /* DMA or I/O error */
+	MC_CMD_STATUS_CONFIG_ERR = 0x6, /* Configuration error */
+	MC_CMD_STATUS_TIMEOUT = 0x7, /* Operation timed out */
+	MC_CMD_STATUS_NO_RESOURCE = 0x8, /* No resources */
+	MC_CMD_STATUS_NO_MEMORY = 0x9, /* No memory available */
+	MC_CMD_STATUS_BUSY = 0xA, /* Device is busy */
+	MC_CMD_STATUS_UNSUPPORTED_OP = 0xB, /* Unsupported operation */
+	MC_CMD_STATUS_INVALID_STATE = 0xC /* Invalid state */
+};
+
+/*
+ * MC command flags
+ */
+
+/* High priority flag */
+#define MC_CMD_FLAG_PRI		0x80
+/* Command completion flag */
+#define MC_CMD_FLAG_INTR_DIS	0x01
+
+static inline u64 mc_encode_cmd_header(u16 cmd_id,
+				       u32 cmd_flags,
+				       u16 token)
+{
+	u64 header = 0;
+	struct mc_cmd_header *hdr = (struct mc_cmd_header *)&header;
+
+	hdr->cmd_id = cpu_to_le16(cmd_id);
+	hdr->token  = cpu_to_le16(token);
+	hdr->status = MC_CMD_STATUS_READY;
+	if (cmd_flags & MC_CMD_FLAG_PRI)
+		hdr->flags_hw = MC_CMD_FLAG_PRI;
+	if (cmd_flags & MC_CMD_FLAG_INTR_DIS)
+		hdr->flags_sw = MC_CMD_FLAG_INTR_DIS;
+
+	return header;
+}
+
+static inline u16 mc_cmd_hdr_read_token(struct mc_command *cmd)
+{
+	struct mc_cmd_header *hdr = (struct mc_cmd_header *)&cmd->header;
+	u16 token = le16_to_cpu(hdr->token);
+
+	return token;
+}
+
+struct mc_rsp_create {
+	__le32 object_id;
+};
+
+struct mc_rsp_api_ver {
+	__le16 major_ver;
+	__le16 minor_ver;
+};
+
+static inline u32 mc_cmd_read_object_id(struct mc_command *cmd)
+{
+	struct mc_rsp_create *rsp_params;
+
+	rsp_params = (struct mc_rsp_create *)cmd->params;
+	return le32_to_cpu(rsp_params->object_id);
+}
+
+static inline void mc_cmd_read_api_version(struct mc_command *cmd,
+					   u16 *major_ver,
+					   u16 *minor_ver)
+{
+	struct mc_rsp_api_ver *rsp_params;
+
+	rsp_params = (struct mc_rsp_api_ver *)cmd->params;
+	*major_ver = le16_to_cpu(rsp_params->major_ver);
+	*minor_ver = le16_to_cpu(rsp_params->minor_ver);
+}
+
+/**
+ * Bit masks for a MC I/O object (struct fsl_mc_io) flags
+ */
+#define FSL_MC_IO_ATOMIC_CONTEXT_PORTAL	0x0001
+
+/**
+ * struct fsl_mc_io - MC I/O object to be passed-in to mc_send_command()
+ * @dev: device associated with this Mc I/O object
+ * @flags: flags for mc_send_command()
+ * @portal_size: MC command portal size in bytes
+ * @portal_phys_addr: MC command portal physical address
+ * @portal_virt_addr: MC command portal virtual address
+ * @dpmcp_dev: pointer to the DPMCP device associated with the MC portal.
+ *
+ * Fields are only meaningful if the FSL_MC_IO_ATOMIC_CONTEXT_PORTAL flag is not
+ * set:
+ * @mutex: Mutex to serialize mc_send_command() calls that use the same MC
+ * portal, if the fsl_mc_io object was created with the
+ * FSL_MC_IO_ATOMIC_CONTEXT_PORTAL flag off. mc_send_command() calls for this
+ * fsl_mc_io object must be made only from non-atomic context.
+ *
+ * Fields are only meaningful if the FSL_MC_IO_ATOMIC_CONTEXT_PORTAL flag is
+ * set:
+ * @spinlock: Spinlock to serialize mc_send_command() calls that use the same MC
+ * portal, if the fsl_mc_io object was created with the
+ * FSL_MC_IO_ATOMIC_CONTEXT_PORTAL flag on. mc_send_command() calls for this
+ * fsl_mc_io object can be made from atomic or non-atomic context.
+ */
+struct fsl_mc_io {
+	struct device *dev;
+	u16 flags;
+	u16 portal_size;
+	phys_addr_t portal_phys_addr;
+	void __iomem *portal_virt_addr;
+	struct fsl_mc_device *dpmcp_dev;
+	union {
+		/*
+		 * This field is only meaningful if the
+		 * FSL_MC_IO_ATOMIC_CONTEXT_PORTAL flag is not set
+		 */
+		struct mutex mutex; /* serializes mc_send_command() */
+
+		/*
+		 * This field is only meaningful if the
+		 * FSL_MC_IO_ATOMIC_CONTEXT_PORTAL flag is set
+		 */
+		spinlock_t spinlock;	/* serializes mc_send_command() */
+	};
+};
+
+int mc_send_command(struct fsl_mc_io *mc_io, struct mc_command *cmd);
+
+#ifdef CONFIG_FSL_MC_BUS
+#define dev_is_fsl_mc(_dev) ((_dev)->bus == &fsl_mc_bus_type)
+#else
+/* If fsl-mc bus is not present device cannot belong to fsl-mc bus */
+#define dev_is_fsl_mc(_dev) (0)
+#endif
+
 /*
  * module_fsl_mc_driver() - Helper macro for drivers that don't do
  * anything special in module init/exit.  This eliminates a lot of
@@ -194,8 +388,14 @@ int __must_check fsl_mc_object_allocate(struct fsl_mc_device *mc_dev,
 
 void fsl_mc_object_free(struct fsl_mc_device *mc_adev);
 
+struct irq_domain *fsl_mc_msi_create_irq_domain(struct fwnode_handle *fwnode,
+						struct msi_domain_info *info,
+						struct irq_domain *parent);
+
 int __must_check fsl_mc_allocate_irqs(struct fsl_mc_device *mc_dev);
 
 void fsl_mc_free_irqs(struct fsl_mc_device *mc_dev);
 
+extern struct bus_type fsl_mc_bus_type;
+
 #endif /* _FSL_MC_H_ */
diff --git a/drivers/staging/gdm724x/gdm_lte.c b/drivers/staging/gdm724x/gdm_lte.c
index cf809987f79f..9ab6ce231f11 100644
--- a/drivers/staging/gdm724x/gdm_lte.c
+++ b/drivers/staging/gdm724x/gdm_lte.c
@@ -161,12 +161,9 @@ static int gdm_lte_emulate_arp(struct sk_buff *skb_in, u32 nic_type)
 		return -ENOMEM;
 	skb_reserve(skb_out, NET_IP_ALIGN);
 
-	memcpy(skb_put(skb_out, mac_header_len), mac_header_data,
-	       mac_header_len);
-	memcpy(skb_put(skb_out, sizeof(struct arphdr)), arp_out,
-	       sizeof(struct arphdr));
-	memcpy(skb_put(skb_out, sizeof(struct arpdata)), arp_data_out,
-	       sizeof(struct arpdata));
+	skb_put_data(skb_out, mac_header_data, mac_header_len);
+	skb_put_data(skb_out, arp_out, sizeof(struct arphdr));
+	skb_put_data(skb_out, arp_data_out, sizeof(struct arpdata));
 
 	skb_out->protocol = ((struct ethhdr *)mac_header_data)->h_proto;
 	skb_out->dev = skb_in->dev;
@@ -322,14 +319,10 @@ static int gdm_lte_emulate_ndp(struct sk_buff *skb_in, u32 nic_type)
 		return -ENOMEM;
 	skb_reserve(skb_out, NET_IP_ALIGN);
 
-	memcpy(skb_put(skb_out, mac_header_len), mac_header_data,
-	       mac_header_len);
-	memcpy(skb_put(skb_out, sizeof(struct ipv6hdr)), &ipv6_out,
-	       sizeof(struct ipv6hdr));
-	memcpy(skb_put(skb_out, sizeof(struct icmp6hdr)), &icmp6_out,
-	       sizeof(struct icmp6hdr));
-	memcpy(skb_put(skb_out, sizeof(struct neighbour_advertisement)), &na,
-	       sizeof(struct neighbour_advertisement));
+	skb_put_data(skb_out, mac_header_data, mac_header_len);
+	skb_put_data(skb_out, &ipv6_out, sizeof(struct ipv6hdr));
+	skb_put_data(skb_out, &icmp6_out, sizeof(struct icmp6hdr));
+	skb_put_data(skb_out, &na, sizeof(struct neighbour_advertisement));
 
 	skb_out->protocol = ((struct ethhdr *)mac_header_data)->h_proto;
 	skb_out->dev = skb_in->dev;
@@ -669,8 +662,8 @@ static void gdm_lte_netif_rx(struct net_device *dev, char *buf,
 		return;
 	skb_reserve(skb, NET_IP_ALIGN);
 
-	memcpy(skb_put(skb, mac_header_len), mac_header_data, mac_header_len);
-	memcpy(skb_put(skb, len), buf, len);
+	skb_put_data(skb, mac_header_data, mac_header_len);
+	skb_put_data(skb, buf, len);
 
 	skb->protocol = ((struct ethhdr *)mac_header_data)->h_proto;
 	skb->dev = dev;
diff --git a/drivers/staging/gdm724x/gdm_usb.c b/drivers/staging/gdm724x/gdm_usb.c
index 15a7e81ec2d2..87cd1f827455 100644
--- a/drivers/staging/gdm724x/gdm_usb.c
+++ b/drivers/staging/gdm724x/gdm_usb.c
@@ -738,11 +738,11 @@ static int gdm_usb_sdu_send(void *priv_dev, void *data, int len,
 	sdu->cmd_evt = gdm_cpu_to_dev16(&udev->gdm_ed, LTE_TX_SDU);
 	if (nic_type == NIC_TYPE_ARP) {
 		send_len = len + SDU_PARAM_LEN;
-	    memcpy(sdu->data, data, len);
+		memcpy(sdu->data, data, len);
 	} else {
-	    send_len = len - ETH_HLEN;
-	    send_len += SDU_PARAM_LEN;
-	    memcpy(sdu->data, data + ETH_HLEN, len - ETH_HLEN);
+		send_len = len - ETH_HLEN;
+		send_len += SDU_PARAM_LEN;
+		memcpy(sdu->data, data + ETH_HLEN, len - ETH_HLEN);
 	}
 
 	sdu->len = gdm_cpu_to_dev16(&udev->gdm_ed, send_len);
diff --git a/drivers/staging/greybus/Kconfig b/drivers/staging/greybus/Kconfig
index 50de2d72dde0..ab096bcef98c 100644
--- a/drivers/staging/greybus/Kconfig
+++ b/drivers/staging/greybus/Kconfig
@@ -216,4 +216,14 @@ config GREYBUS_USB
 	  will be called gb-usb.ko
 
 endif	# GREYBUS_BRIDGED_PHY
+
+config GREYBUS_ARCHE
+	tristate "Greybus Arche Platform driver"
+	depends on USB_HSIC_USB3613 || COMPILE_TEST
+	---help---
+	  Select this option if you have an Arche device.
+
+	  To compile this code as a module, chose M here: the module
+	  will be called gb-arche.ko
+
 endif	# GREYBUS
diff --git a/drivers/staging/greybus/Makefile b/drivers/staging/greybus/Makefile
index b26b9a35bdd5..23e1cb7bff8e 100644
--- a/drivers/staging/greybus/Makefile
+++ b/drivers/staging/greybus/Makefile
@@ -91,4 +91,4 @@ obj-$(CONFIG_GREYBUS_USB)		+= gb-usb.o
 # Greybus Platform driver
 gb-arche-y	:= arche-platform.o arche-apb-ctrl.o
 
-obj-$(CONFIG_USB_HSIC_USB3613)	+= gb-arche.o
+obj-$(CONFIG_GREYBUS_ARCHE)	+= gb-arche.o
diff --git a/drivers/staging/greybus/arche-apb-ctrl.c b/drivers/staging/greybus/arche-apb-ctrl.c
index 02243b4fd898..0412f3d06efb 100644
--- a/drivers/staging/greybus/arche-apb-ctrl.c
+++ b/drivers/staging/greybus/arche-apb-ctrl.c
@@ -22,6 +22,8 @@
 #include "arche_platform.h"
 
 
+static void apb_bootret_deassert(struct device *dev);
+
 struct arche_apb_ctrl_drvdata {
 	/* Control GPIO signals to and from AP <=> AP Bridges */
 	int resetn_gpio;
@@ -222,14 +224,7 @@ static void poweroff_seq(struct platform_device *pdev)
 	/* TODO: May have to send an event to SVC about this exit */
 }
 
-void apb_bootret_assert(struct device *dev)
-{
-	struct arche_apb_ctrl_drvdata *apb = dev_get_drvdata(dev);
-
-	gpio_set_value(apb->boot_ret_gpio, 1);
-}
-
-void apb_bootret_deassert(struct device *dev)
+static void apb_bootret_deassert(struct device *dev)
 {
 	struct arche_apb_ctrl_drvdata *apb = dev_get_drvdata(dev);
 
diff --git a/drivers/staging/greybus/arche-platform.c b/drivers/staging/greybus/arche-platform.c
index aac1145f1983..eced2d26467b 100644
--- a/drivers/staging/greybus/arche-platform.c
+++ b/drivers/staging/greybus/arche-platform.c
@@ -24,7 +24,14 @@
 #include "arche_platform.h"
 #include "greybus.h"
 
+#if IS_ENABLED(CONFIG_USB_HSIC_USB3613)
 #include <linux/usb/usb3613.h>
+#else
+static inline int usb3613_hub_mode_ctrl(bool unused)
+{
+	return 0;
+}
+#endif
 
 #define WD_COLDBOOT_PULSE_WIDTH_MS	30
 
@@ -35,7 +42,6 @@ enum svc_wakedetect_state {
 	WD_STATE_STANDBYBOOT_TRIG,	/* As of now not used ?? */
 	WD_STATE_COLDBOOT_START,	/* Cold boot process started */
 	WD_STATE_STANDBYBOOT_START,	/* Not used */
-	WD_STATE_TIMESYNC,
 };
 
 struct arche_platform_drvdata {
@@ -59,26 +65,12 @@ struct arche_platform_drvdata {
 	int wake_detect_irq;
 	spinlock_t wake_lock;			/* Protect wake_detect_state */
 	struct mutex platform_state_mutex;	/* Protect state */
-	wait_queue_head_t wq;			/* WQ for arche_pdata->state */
 	unsigned long wake_detect_start;
 	struct notifier_block pm_notifier;
 
 	struct device *dev;
-	struct gb_timesync_svc *timesync_svc_pdata;
 };
 
-static int arche_apb_bootret_assert(struct device *dev, void *data)
-{
-	apb_bootret_assert(dev);
-	return 0;
-}
-
-static int arche_apb_bootret_deassert(struct device *dev, void *data)
-{
-	apb_bootret_deassert(dev);
-	return 0;
-}
-
 /* Requires calling context to hold arche_pdata->platform_state_mutex */
 static void arche_platform_set_state(struct arche_platform_drvdata *arche_pdata,
 				     enum arche_platform_state state)
@@ -86,112 +78,6 @@ static void arche_platform_set_state(struct arche_platform_drvdata *arche_pdata,
 	arche_pdata->state = state;
 }
 
-/*
- * arche_platform_change_state: Change the operational state
- *
- * This exported function allows external drivers to change the state
- * of the arche-platform driver.
- * Note that this function only supports transitions between two states
- * with limited functionality.
- *
- *  - ARCHE_PLATFORM_STATE_TIME_SYNC:
- *    Once set, allows timesync operations between SVC <=> AP and makes
- *    sure that arche-platform driver ignores any subsequent events/pulses
- *    from SVC over wake/detect.
- *
- *  - ARCHE_PLATFORM_STATE_ACTIVE:
- *    Puts back driver to active state, where any pulse from SVC on wake/detect
- *    line would trigger either cold/standby boot.
- *    Note: Transition request from this function does not trigger cold/standby
- *          boot. It just puts back driver book keeping variable back to ACTIVE
- *          state and restores the interrupt.
- *
- * Returns -ENODEV if device not found, -EAGAIN if the driver cannot currently
- * satisfy the requested state-transition or -EINVAL for all other
- * state-transition requests.
- */
-int arche_platform_change_state(enum arche_platform_state state,
-				struct gb_timesync_svc *timesync_svc_pdata)
-{
-	struct arche_platform_drvdata *arche_pdata;
-	struct platform_device *pdev;
-	struct device_node *np;
-	int ret = -EAGAIN;
-	unsigned long flags;
-
-	np = of_find_compatible_node(NULL, NULL, "google,arche-platform");
-	if (!np) {
-		pr_err("google,arche-platform device node not found\n");
-		return -ENODEV;
-	}
-
-	pdev = of_find_device_by_node(np);
-	if (!pdev) {
-		pr_err("arche-platform device not found\n");
-		of_node_put(np);
-		return -ENODEV;
-	}
-
-	arche_pdata = platform_get_drvdata(pdev);
-
-	mutex_lock(&arche_pdata->platform_state_mutex);
-	spin_lock_irqsave(&arche_pdata->wake_lock, flags);
-
-	if (arche_pdata->state == state) {
-		ret = 0;
-		goto exit;
-	}
-
-	switch (state) {
-	case ARCHE_PLATFORM_STATE_TIME_SYNC:
-		if (arche_pdata->state != ARCHE_PLATFORM_STATE_ACTIVE) {
-			ret = -EINVAL;
-			goto exit;
-		}
-		if (arche_pdata->wake_detect_state != WD_STATE_IDLE) {
-			dev_err(arche_pdata->dev,
-				"driver busy with wake/detect line ops\n");
-			goto  exit;
-		}
-		device_for_each_child(arche_pdata->dev, NULL,
-				      arche_apb_bootret_assert);
-		arche_pdata->wake_detect_state = WD_STATE_TIMESYNC;
-		break;
-	case ARCHE_PLATFORM_STATE_ACTIVE:
-		if (arche_pdata->state != ARCHE_PLATFORM_STATE_TIME_SYNC) {
-			ret = -EINVAL;
-			goto exit;
-		}
-		device_for_each_child(arche_pdata->dev, NULL,
-				      arche_apb_bootret_deassert);
-		arche_pdata->wake_detect_state = WD_STATE_IDLE;
-		break;
-	case ARCHE_PLATFORM_STATE_OFF:
-	case ARCHE_PLATFORM_STATE_STANDBY:
-	case ARCHE_PLATFORM_STATE_FW_FLASHING:
-		dev_err(arche_pdata->dev, "busy, request to retry later\n");
-		goto exit;
-	default:
-		ret = -EINVAL;
-		dev_err(arche_pdata->dev,
-			"invalid state transition request\n");
-		goto exit;
-	}
-	arche_pdata->timesync_svc_pdata = timesync_svc_pdata;
-	arche_platform_set_state(arche_pdata, state);
-	if (state == ARCHE_PLATFORM_STATE_ACTIVE)
-		wake_up(&arche_pdata->wq);
-
-	ret = 0;
-exit:
-	spin_unlock_irqrestore(&arche_pdata->wake_lock, flags);
-	mutex_unlock(&arche_pdata->platform_state_mutex);
-	put_device(&pdev->dev);
-	of_node_put(np);
-	return ret;
-}
-EXPORT_SYMBOL_GPL(arche_platform_change_state);
-
 /* Requires arche_pdata->wake_lock is held by calling context */
 static void arche_platform_set_wake_detect_state(
 				struct arche_platform_drvdata *arche_pdata,
@@ -275,11 +161,6 @@ static irqreturn_t arche_platform_wd_irq(int irq, void *devid)
 
 	spin_lock_irqsave(&arche_pdata->wake_lock, flags);
 
-	if (arche_pdata->wake_detect_state == WD_STATE_TIMESYNC) {
-		gb_timesync_irq(arche_pdata->timesync_svc_pdata);
-		goto exit;
-	}
-
 	if (gpio_get_value(arche_pdata->wake_detect_gpio)) {
 		/* wake/detect rising */
 
@@ -323,7 +204,6 @@ static irqreturn_t arche_platform_wd_irq(int irq, void *devid)
 		}
 	}
 
-exit:
 	spin_unlock_irqrestore(&arche_pdata->wake_lock, flags);
 
 	return IRQ_HANDLED;
@@ -436,17 +316,7 @@ static ssize_t state_store(struct device *dev,
 	struct arche_platform_drvdata *arche_pdata = platform_get_drvdata(pdev);
 	int ret = 0;
 
-retry:
 	mutex_lock(&arche_pdata->platform_state_mutex);
-	if (arche_pdata->state == ARCHE_PLATFORM_STATE_TIME_SYNC) {
-		mutex_unlock(&arche_pdata->platform_state_mutex);
-		ret = wait_event_interruptible(
-			arche_pdata->wq,
-			arche_pdata->state != ARCHE_PLATFORM_STATE_TIME_SYNC);
-		if (ret)
-			return ret;
-		goto retry;
-	}
 
 	if (sysfs_streq(buf, "off")) {
 		if (arche_pdata->state == ARCHE_PLATFORM_STATE_OFF)
@@ -517,8 +387,6 @@ static ssize_t state_show(struct device *dev,
 		return sprintf(buf, "standby\n");
 	case ARCHE_PLATFORM_STATE_FW_FLASHING:
 		return sprintf(buf, "fw_flashing\n");
-	case ARCHE_PLATFORM_STATE_TIME_SYNC:
-		return sprintf(buf, "time_sync\n");
 	default:
 		return sprintf(buf, "unknown state\n");
 	}
@@ -665,7 +533,6 @@ static int arche_platform_probe(struct platform_device *pdev)
 
 	spin_lock_init(&arche_pdata->wake_lock);
 	mutex_init(&arche_pdata->platform_state_mutex);
-	init_waitqueue_head(&arche_pdata->wq);
 	arche_pdata->wake_detect_irq =
 		gpio_to_irq(arche_pdata->wake_detect_gpio);
 
@@ -701,9 +568,6 @@ static int arche_platform_probe(struct platform_device *pdev)
 		goto err_device_remove;
 	}
 
-	/* Register callback pointer */
-	arche_platform_change_state_cb = arche_platform_change_state;
-
 	/* Explicitly power off if requested */
 	if (!of_property_read_bool(pdev->dev.of_node, "arche,init-off")) {
 		mutex_lock(&arche_pdata->platform_state_mutex);
@@ -751,7 +615,7 @@ static int arche_platform_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static int arche_platform_suspend(struct device *dev)
+static __maybe_unused int arche_platform_suspend(struct device *dev)
 {
 	/*
 	 * If timing profile premits, we may shutdown bridge
@@ -765,7 +629,7 @@ static int arche_platform_suspend(struct device *dev)
 	return 0;
 }
 
-static int arche_platform_resume(struct device *dev)
+static __maybe_unused int arche_platform_resume(struct device *dev)
 {
 	/*
 	 * Atleast for ES2 we have to meet the delay requirement between
diff --git a/drivers/staging/greybus/arche_platform.h b/drivers/staging/greybus/arche_platform.h
index c0591df9b9d6..bcffc69d0960 100644
--- a/drivers/staging/greybus/arche_platform.h
+++ b/drivers/staging/greybus/arche_platform.h
@@ -15,14 +15,8 @@ enum arche_platform_state {
 	ARCHE_PLATFORM_STATE_ACTIVE,
 	ARCHE_PLATFORM_STATE_STANDBY,
 	ARCHE_PLATFORM_STATE_FW_FLASHING,
-	ARCHE_PLATFORM_STATE_TIME_SYNC,
 };
 
-int arche_platform_change_state(enum arche_platform_state state,
-				struct gb_timesync_svc *pdata);
-
-extern int (*arche_platform_change_state_cb)(enum arche_platform_state state,
-					     struct gb_timesync_svc *pdata);
 int __init arche_apb_init(void);
 void __exit arche_apb_exit(void);
 
@@ -31,7 +25,5 @@ int apb_ctrl_coldboot(struct device *dev);
 int apb_ctrl_fw_flashing(struct device *dev);
 int apb_ctrl_standby_boot(struct device *dev);
 void apb_ctrl_poweroff(struct device *dev);
-void apb_bootret_assert(struct device *dev);
-void apb_bootret_deassert(struct device *dev);
 
 #endif	/* __ARCHE_PLATFORM_H */
diff --git a/drivers/staging/greybus/light.c b/drivers/staging/greybus/light.c
index 16813628eda1..861a249e6ef1 100644
--- a/drivers/staging/greybus/light.c
+++ b/drivers/staging/greybus/light.c
@@ -1030,7 +1030,7 @@ static int gb_lights_light_config(struct gb_lights *glights, u8 id)
 	light->channels_count = conf.channel_count;
 	light->name = kstrndup(conf.name, NAMES_MAX, GFP_KERNEL);
 
-	light->channels = kzalloc(light->channels_count *
+	light->channels = kcalloc(light->channels_count,
 				  sizeof(struct gb_channel), GFP_KERNEL);
 	if (!light->channels)
 		return -ENOMEM;
@@ -1167,7 +1167,7 @@ static int gb_lights_create_all(struct gb_lights *glights)
 	if (ret < 0)
 		goto out;
 
-	glights->lights = kzalloc(glights->lights_count *
+	glights->lights = kcalloc(glights->lights_count,
 				  sizeof(struct gb_light), GFP_KERNEL);
 	if (!glights->lights) {
 		ret = -ENOMEM;
diff --git a/drivers/staging/greybus/power_supply.c b/drivers/staging/greybus/power_supply.c
index e85c988b7034..20cac20518d7 100644
--- a/drivers/staging/greybus/power_supply.c
+++ b/drivers/staging/greybus/power_supply.c
@@ -944,7 +944,7 @@ static int gb_power_supplies_setup(struct gb_power_supplies *supplies)
 	if (ret < 0)
 		goto out;
 
-	supplies->supply = kzalloc(supplies->supplies_count *
+	supplies->supply = kcalloc(supplies->supplies_count,
 				     sizeof(struct gb_power_supply),
 				     GFP_KERNEL);
 
diff --git a/drivers/staging/iio/cdc/ad7152.c b/drivers/staging/iio/cdc/ad7152.c
index dc6ecd824365..ff10d1f0a7e4 100644
--- a/drivers/staging/iio/cdc/ad7152.c
+++ b/drivers/staging/iio/cdc/ad7152.c
@@ -231,16 +231,12 @@ static int ad7152_write_raw_samp_freq(struct device *dev, int val)
 	if (i >= ARRAY_SIZE(ad7152_filter_rate_table))
 		i = ARRAY_SIZE(ad7152_filter_rate_table) - 1;
 
-	mutex_lock(&chip->state_lock);
 	ret = i2c_smbus_write_byte_data(chip->client,
 					AD7152_REG_CFG2, AD7152_CFG2_OSR(i));
-	if (ret < 0) {
-		mutex_unlock(&chip->state_lock);
+	if (ret < 0)
 		return ret;
-	}
 
 	chip->filter_rate_setup = i;
-	mutex_unlock(&chip->state_lock);
 
 	return ret;
 }
diff --git a/drivers/staging/iio/frequency/ad9834.c b/drivers/staging/iio/frequency/ad9834.c
index af108e96b3ec..995acdd7c942 100644
--- a/drivers/staging/iio/frequency/ad9834.c
+++ b/drivers/staging/iio/frequency/ad9834.c
@@ -292,7 +292,7 @@ ssize_t ad9834_show_out0_wavetype_available(struct device *dev,
 	return sprintf(buf, "%s\n", str);
 }
 
-static IIO_DEVICE_ATTR(out_altvoltage0_out0_wavetype_available, S_IRUGO,
+static IIO_DEVICE_ATTR(out_altvoltage0_out0_wavetype_available, 0444,
 		       ad9834_show_out0_wavetype_available, NULL, 0);
 
 static
@@ -312,27 +312,27 @@ ssize_t ad9834_show_out1_wavetype_available(struct device *dev,
 	return sprintf(buf, "%s\n", str);
 }
 
-static IIO_DEVICE_ATTR(out_altvoltage0_out1_wavetype_available, S_IRUGO,
+static IIO_DEVICE_ATTR(out_altvoltage0_out1_wavetype_available, 0444,
 		       ad9834_show_out1_wavetype_available, NULL, 0);
 
 /**
  * see dds.h for further information
  */
 
-static IIO_DEV_ATTR_FREQ(0, 0, S_IWUSR, NULL, ad9834_write, AD9834_REG_FREQ0);
-static IIO_DEV_ATTR_FREQ(0, 1, S_IWUSR, NULL, ad9834_write, AD9834_REG_FREQ1);
-static IIO_DEV_ATTR_FREQSYMBOL(0, S_IWUSR, NULL, ad9834_write, AD9834_FSEL);
+static IIO_DEV_ATTR_FREQ(0, 0, 0200, NULL, ad9834_write, AD9834_REG_FREQ0);
+static IIO_DEV_ATTR_FREQ(0, 1, 0200, NULL, ad9834_write, AD9834_REG_FREQ1);
+static IIO_DEV_ATTR_FREQSYMBOL(0, 0200, NULL, ad9834_write, AD9834_FSEL);
 static IIO_CONST_ATTR_FREQ_SCALE(0, "1"); /* 1Hz */
 
-static IIO_DEV_ATTR_PHASE(0, 0, S_IWUSR, NULL, ad9834_write, AD9834_REG_PHASE0);
-static IIO_DEV_ATTR_PHASE(0, 1, S_IWUSR, NULL, ad9834_write, AD9834_REG_PHASE1);
-static IIO_DEV_ATTR_PHASESYMBOL(0, S_IWUSR, NULL, ad9834_write, AD9834_PSEL);
+static IIO_DEV_ATTR_PHASE(0, 0, 0200, NULL, ad9834_write, AD9834_REG_PHASE0);
+static IIO_DEV_ATTR_PHASE(0, 1, 0200, NULL, ad9834_write, AD9834_REG_PHASE1);
+static IIO_DEV_ATTR_PHASESYMBOL(0, 0200, NULL, ad9834_write, AD9834_PSEL);
 static IIO_CONST_ATTR_PHASE_SCALE(0, "0.0015339808"); /* 2PI/2^12 rad*/
 
-static IIO_DEV_ATTR_PINCONTROL_EN(0, S_IWUSR, NULL,
+static IIO_DEV_ATTR_PINCONTROL_EN(0, 0200, NULL,
 	ad9834_write, AD9834_PIN_SW);
-static IIO_DEV_ATTR_OUT_ENABLE(0, S_IWUSR, NULL, ad9834_write, AD9834_RESET);
-static IIO_DEV_ATTR_OUTY_ENABLE(0, 1, S_IWUSR, NULL,
+static IIO_DEV_ATTR_OUT_ENABLE(0, 0200, NULL, ad9834_write, AD9834_RESET);
+static IIO_DEV_ATTR_OUTY_ENABLE(0, 1, 0200, NULL,
 	ad9834_write, AD9834_OPBITEN);
 static IIO_DEV_ATTR_OUT_WAVETYPE(0, 0, ad9834_store_wavetype, 0);
 static IIO_DEV_ATTR_OUT_WAVETYPE(0, 1, ad9834_store_wavetype, 1);
diff --git a/drivers/staging/iio/frequency/dds.h b/drivers/staging/iio/frequency/dds.h
index fe53e7324c94..d6ccd99c14d7 100644
--- a/drivers/staging/iio/frequency/dds.h
+++ b/drivers/staging/iio/frequency/dds.h
@@ -101,7 +101,7 @@
 
 #define IIO_DEV_ATTR_OUT_WAVETYPE(_channel, _output, _store, _addr)	\
 	IIO_DEVICE_ATTR(out_altvoltage##_channel##_out##_output##_wavetype,\
-			S_IWUSR, NULL, _store, _addr)
+			0200, NULL, _store, _addr)
 
 /**
  * /sys/bus/iio/devices/.../out_altvoltageX_outY_wavetype_available
diff --git a/drivers/staging/iio/light/Kconfig b/drivers/staging/iio/light/Kconfig
index 4fbf6298c0f3..aacb0ae58c0e 100644
--- a/drivers/staging/iio/light/Kconfig
+++ b/drivers/staging/iio/light/Kconfig
@@ -3,16 +3,6 @@
 #
 menu "Light sensors"
 
-config SENSORS_ISL29028
-	tristate "Intersil ISL29028 Concurrent Light and Proximity Sensor"
-	depends on I2C
-	select REGMAP_I2C
-	help
-	 Provides driver for the Intersil's ISL29028 device.
-	 This driver supports the sysfs interface to get the ALS, IR intensity,
-	 Proximity value via iio. The ISL29028 provides the concurrent sensing
-	 of ambient light and proximity.
-
 config TSL2x7x
 	tristate "TAOS TSL/TMD2x71 and TSL/TMD2x72 Family of light and proximity sensors"
 	depends on I2C
diff --git a/drivers/staging/iio/light/Makefile b/drivers/staging/iio/light/Makefile
index f8693e9fdc94..ab8dc3a3d10b 100644
--- a/drivers/staging/iio/light/Makefile
+++ b/drivers/staging/iio/light/Makefile
@@ -2,5 +2,4 @@
 # Makefile for industrial I/O Light sensors
 #
 
-obj-$(CONFIG_SENSORS_ISL29028)	+= isl29028.o
-obj-$(CONFIG_TSL2x7x)	+= tsl2x7x_core.o
+obj-$(CONFIG_TSL2x7x)	+= tsl2x7x.o
diff --git a/drivers/staging/iio/light/tsl2x7x_core.c b/drivers/staging/iio/light/tsl2x7x.c
index af3910bc1b4f..146719928fb3 100644
--- a/drivers/staging/iio/light/tsl2x7x_core.c
+++ b/drivers/staging/iio/light/tsl2x7x.c
@@ -13,10 +13,6 @@
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
- *
- * You 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>
@@ -919,7 +915,7 @@ static void tsl2x7x_prox_cal(struct iio_dev *indio_dev)
 		tsl2x7x_chip_on(indio_dev);
 }
 
-static ssize_t tsl2x7x_power_state_show(struct device *dev,
+static ssize_t power_state_show(struct device *dev,
 					struct device_attribute *attr,
 					char *buf)
 {
@@ -928,7 +924,7 @@ static ssize_t tsl2x7x_power_state_show(struct device *dev,
 	return snprintf(buf, PAGE_SIZE, "%d\n", chip->tsl2x7x_chip_status);
 }
 
-static ssize_t tsl2x7x_power_state_store(struct device *dev,
+static ssize_t power_state_store(struct device *dev,
 					 struct device_attribute *attr,
 					 const char *buf, size_t len)
 {
@@ -946,7 +942,7 @@ static ssize_t tsl2x7x_power_state_store(struct device *dev,
 	return len;
 }
 
-static ssize_t tsl2x7x_gain_available_show(struct device *dev,
+static ssize_t in_illuminance0_calibscale_available_show(struct device *dev,
 					   struct device_attribute *attr,
 					   char *buf)
 {
@@ -964,14 +960,14 @@ static ssize_t tsl2x7x_gain_available_show(struct device *dev,
 	return snprintf(buf, PAGE_SIZE, "%s\n", "1 8 16 120");
 }
 
-static ssize_t tsl2x7x_prox_gain_available_show(struct device *dev,
+static ssize_t in_proximity0_calibscale_available_show(struct device *dev,
 						struct device_attribute *attr,
 						char *buf)
 {
 		return snprintf(buf, PAGE_SIZE, "%s\n", "1 2 4 8");
 }
 
-static ssize_t tsl2x7x_als_time_show(struct device *dev,
+static ssize_t in_illuminance0_integration_time_show(struct device *dev,
 				     struct device_attribute *attr,
 				     char *buf)
 {
@@ -986,7 +982,7 @@ static ssize_t tsl2x7x_als_time_show(struct device *dev,
 	return snprintf(buf, PAGE_SIZE, "%d.%03d\n", y, z);
 }
 
-static ssize_t tsl2x7x_als_time_store(struct device *dev,
+static ssize_t in_illuminance0_integration_time_store(struct device *dev,
 				      struct device_attribute *attr,
 				      const char *buf, size_t len)
 {
@@ -1014,7 +1010,7 @@ static ssize_t tsl2x7x_als_time_store(struct device *dev,
 static IIO_CONST_ATTR(in_illuminance0_integration_time_available,
 		".00272 - .696");
 
-static ssize_t tsl2x7x_als_cal_target_show(struct device *dev,
+static ssize_t in_illuminance0_target_input_show(struct device *dev,
 					   struct device_attribute *attr,
 					   char *buf)
 {
@@ -1024,7 +1020,7 @@ static ssize_t tsl2x7x_als_cal_target_show(struct device *dev,
 			chip->tsl2x7x_settings.als_cal_target);
 }
 
-static ssize_t tsl2x7x_als_cal_target_store(struct device *dev,
+static ssize_t in_illuminance0_target_input_store(struct device *dev,
 					    struct device_attribute *attr,
 					    const char *buf, size_t len)
 {
@@ -1044,7 +1040,7 @@ static ssize_t tsl2x7x_als_cal_target_store(struct device *dev,
 }
 
 /* persistence settings */
-static ssize_t tsl2x7x_als_persistence_show(struct device *dev,
+static ssize_t in_intensity0_thresh_period_show(struct device *dev,
 					    struct device_attribute *attr,
 					    char *buf)
 {
@@ -1061,7 +1057,7 @@ static ssize_t tsl2x7x_als_persistence_show(struct device *dev,
 	return snprintf(buf, PAGE_SIZE, "%d.%03d\n", y, z);
 }
 
-static ssize_t tsl2x7x_als_persistence_store(struct device *dev,
+static ssize_t in_intensity0_thresh_period_store(struct device *dev,
 					     struct device_attribute *attr,
 					     const char *buf, size_t len)
 {
@@ -1092,7 +1088,7 @@ static ssize_t tsl2x7x_als_persistence_store(struct device *dev,
 	return IIO_VAL_INT_PLUS_MICRO;
 }
 
-static ssize_t tsl2x7x_prox_persistence_show(struct device *dev,
+static ssize_t in_proximity0_thresh_period_show(struct device *dev,
 					     struct device_attribute *attr,
 					     char *buf)
 {
@@ -1109,7 +1105,7 @@ static ssize_t tsl2x7x_prox_persistence_show(struct device *dev,
 	return snprintf(buf, PAGE_SIZE, "%d.%03d\n", y, z);
 }
 
-static ssize_t tsl2x7x_prox_persistence_store(struct device *dev,
+static ssize_t in_proximity0_thresh_period_store(struct device *dev,
 					      struct device_attribute *attr,
 					      const char *buf, size_t len)
 {
@@ -1140,7 +1136,7 @@ static ssize_t tsl2x7x_prox_persistence_store(struct device *dev,
 	return IIO_VAL_INT_PLUS_MICRO;
 }
 
-static ssize_t tsl2x7x_do_calibrate(struct device *dev,
+static ssize_t in_illuminance0_calibrate_store(struct device *dev,
 				    struct device_attribute *attr,
 				    const char *buf, size_t len)
 {
@@ -1158,7 +1154,7 @@ static ssize_t tsl2x7x_do_calibrate(struct device *dev,
 	return len;
 }
 
-static ssize_t tsl2x7x_luxtable_show(struct device *dev,
+static ssize_t in_illuminance0_lux_table_show(struct device *dev,
 				     struct device_attribute *attr,
 				     char *buf)
 {
@@ -1186,7 +1182,7 @@ static ssize_t tsl2x7x_luxtable_show(struct device *dev,
 	return offset;
 }
 
-static ssize_t tsl2x7x_luxtable_store(struct device *dev,
+static ssize_t in_illuminance0_lux_table_store(struct device *dev,
 				      struct device_attribute *attr,
 				      const char *buf, size_t len)
 {
@@ -1226,7 +1222,7 @@ static ssize_t tsl2x7x_luxtable_store(struct device *dev,
 	return len;
 }
 
-static ssize_t tsl2x7x_do_prox_calibrate(struct device *dev,
+static ssize_t in_proximity0_calibrate_store(struct device *dev,
 					 struct device_attribute *attr,
 					 const char *buf, size_t len)
 {
@@ -1498,35 +1494,25 @@ static int tsl2x7x_write_raw(struct iio_dev *indio_dev,
 	return 0;
 }
 
-static DEVICE_ATTR(power_state, S_IRUGO | S_IWUSR,
-		tsl2x7x_power_state_show, tsl2x7x_power_state_store);
+static DEVICE_ATTR_RW(power_state);
 
-static DEVICE_ATTR(in_proximity0_calibscale_available, S_IRUGO,
-		tsl2x7x_prox_gain_available_show, NULL);
+static DEVICE_ATTR_RO(in_proximity0_calibscale_available);
 
-static DEVICE_ATTR(in_illuminance0_calibscale_available, S_IRUGO,
-		tsl2x7x_gain_available_show, NULL);
+static DEVICE_ATTR_RO(in_illuminance0_calibscale_available);
 
-static DEVICE_ATTR(in_illuminance0_integration_time, S_IRUGO | S_IWUSR,
-		tsl2x7x_als_time_show, tsl2x7x_als_time_store);
+static DEVICE_ATTR_RW(in_illuminance0_integration_time);
 
-static DEVICE_ATTR(in_illuminance0_target_input, S_IRUGO | S_IWUSR,
-		tsl2x7x_als_cal_target_show, tsl2x7x_als_cal_target_store);
+static DEVICE_ATTR_RW(in_illuminance0_target_input);
 
-static DEVICE_ATTR(in_illuminance0_calibrate, S_IWUSR, NULL,
-		tsl2x7x_do_calibrate);
+static DEVICE_ATTR_WO(in_illuminance0_calibrate);
 
-static DEVICE_ATTR(in_proximity0_calibrate, S_IWUSR, NULL,
-		tsl2x7x_do_prox_calibrate);
+static DEVICE_ATTR_WO(in_proximity0_calibrate);
 
-static DEVICE_ATTR(in_illuminance0_lux_table, S_IRUGO | S_IWUSR,
-		tsl2x7x_luxtable_show, tsl2x7x_luxtable_store);
+static DEVICE_ATTR_RW(in_illuminance0_lux_table);
 
-static DEVICE_ATTR(in_intensity0_thresh_period, S_IRUGO | S_IWUSR,
-		tsl2x7x_als_persistence_show, tsl2x7x_als_persistence_store);
+static DEVICE_ATTR_RW(in_intensity0_thresh_period);
 
-static DEVICE_ATTR(in_proximity0_thresh_period, S_IRUGO | S_IWUSR,
-		tsl2x7x_prox_persistence_show, tsl2x7x_prox_persistence_store);
+static DEVICE_ATTR_RW(in_proximity0_thresh_period);
 
 /* Use the default register values to identify the Taos device */
 static int tsl2x7x_device_id(unsigned char *id, int target)
diff --git a/drivers/staging/iio/meter/ade7753.c b/drivers/staging/iio/meter/ade7753.c
index b71fbd313778..ce26abdeab92 100644
--- a/drivers/staging/iio/meter/ade7753.c
+++ b/drivers/staging/iio/meter/ade7753.c
@@ -107,9 +107,8 @@ static int ade7753_spi_write_reg_8(struct device *dev,
 	return ret;
 }
 
-static int ade7753_spi_write_reg_16(struct device *dev,
-		u8 reg_address,
-		u16 value)
+static int ade7753_spi_write_reg_16(struct device *dev, u8 reg_address,
+				    u16 value)
 {
 	int ret;
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
@@ -126,8 +125,8 @@ static int ade7753_spi_write_reg_16(struct device *dev,
 }
 
 static int ade7753_spi_read_reg_8(struct device *dev,
-		u8 reg_address,
-		u8 *val)
+				  u8 reg_address,
+				  u8 *val)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7753_state *st = iio_priv(indio_dev);
@@ -136,7 +135,7 @@ static int ade7753_spi_read_reg_8(struct device *dev,
 	ret = spi_w8r8(st->us, ADE7753_READ_REG(reg_address));
 	if (ret < 0) {
 		dev_err(&st->us->dev, "problem when reading 8 bit register 0x%02X",
-				reg_address);
+			reg_address);
 		return ret;
 	}
 	*val = ret;
@@ -145,8 +144,8 @@ static int ade7753_spi_read_reg_8(struct device *dev,
 }
 
 static int ade7753_spi_read_reg_16(struct device *dev,
-		u8 reg_address,
-		u16 *val)
+				   u8 reg_address,
+				   u16 *val)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7753_state *st = iio_priv(indio_dev);
@@ -165,8 +164,8 @@ static int ade7753_spi_read_reg_16(struct device *dev,
 }
 
 static int ade7753_spi_read_reg_24(struct device *dev,
-		u8 reg_address,
-		u32 *val)
+				   u8 reg_address,
+				   u32 *val)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7753_state *st = iio_priv(indio_dev);
@@ -189,7 +188,7 @@ static int ade7753_spi_read_reg_24(struct device *dev,
 	ret = spi_sync_transfer(st->us, xfers, ARRAY_SIZE(xfers));
 	if (ret) {
 		dev_err(&st->us->dev, "problem when reading 24 bit register 0x%02X",
-				reg_address);
+			reg_address);
 		goto error_ret;
 	}
 	*val = (st->rx[0] << 16) | (st->rx[1] << 8) | st->rx[2];
@@ -200,8 +199,8 @@ error_ret:
 }
 
 static ssize_t ade7753_read_8bit(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
+				 struct device_attribute *attr,
+				 char *buf)
 {
 	int ret;
 	u8 val;
@@ -215,8 +214,8 @@ static ssize_t ade7753_read_8bit(struct device *dev,
 }
 
 static ssize_t ade7753_read_16bit(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
+				  struct device_attribute *attr,
+				  char *buf)
 {
 	int ret;
 	u16 val;
@@ -230,8 +229,8 @@ static ssize_t ade7753_read_16bit(struct device *dev,
 }
 
 static ssize_t ade7753_read_24bit(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
+				  struct device_attribute *attr,
+				  char *buf)
 {
 	int ret;
 	u32 val;
@@ -245,9 +244,9 @@ static ssize_t ade7753_read_24bit(struct device *dev,
 }
 
 static ssize_t ade7753_write_8bit(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
+				  struct device_attribute *attr,
+				  const char *buf,
+				  size_t len)
 {
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret;
@@ -263,9 +262,9 @@ error_ret:
 }
 
 static ssize_t ade7753_write_16bit(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
+				   struct device_attribute *attr,
+				   const char *buf,
+				   size_t len)
 {
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret;
@@ -298,92 +297,92 @@ static IIO_DEV_ATTR_AENERGY(ade7753_read_24bit, ADE7753_AENERGY);
 static IIO_DEV_ATTR_LAENERGY(ade7753_read_24bit, ADE7753_LAENERGY);
 static IIO_DEV_ATTR_VAENERGY(ade7753_read_24bit, ADE7753_VAENERGY);
 static IIO_DEV_ATTR_LVAENERGY(ade7753_read_24bit, ADE7753_LVAENERGY);
-static IIO_DEV_ATTR_CFDEN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CFDEN(0644,
 		ade7753_read_16bit,
 		ade7753_write_16bit,
 		ADE7753_CFDEN);
-static IIO_DEV_ATTR_CFNUM(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CFNUM(0644,
 		ade7753_read_8bit,
 		ade7753_write_8bit,
 		ADE7753_CFNUM);
 static IIO_DEV_ATTR_CHKSUM(ade7753_read_8bit, ADE7753_CHKSUM);
-static IIO_DEV_ATTR_PHCAL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_PHCAL(0644,
 		ade7753_read_16bit,
 		ade7753_write_16bit,
 		ADE7753_PHCAL);
-static IIO_DEV_ATTR_APOS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_APOS(0644,
 		ade7753_read_16bit,
 		ade7753_write_16bit,
 		ADE7753_APOS);
-static IIO_DEV_ATTR_SAGCYC(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_SAGCYC(0644,
 		ade7753_read_8bit,
 		ade7753_write_8bit,
 		ADE7753_SAGCYC);
-static IIO_DEV_ATTR_SAGLVL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_SAGLVL(0644,
 		ade7753_read_8bit,
 		ade7753_write_8bit,
 		ADE7753_SAGLVL);
-static IIO_DEV_ATTR_LINECYC(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_LINECYC(0644,
 		ade7753_read_8bit,
 		ade7753_write_8bit,
 		ADE7753_LINECYC);
-static IIO_DEV_ATTR_WDIV(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_WDIV(0644,
 		ade7753_read_8bit,
 		ade7753_write_8bit,
 		ADE7753_WDIV);
-static IIO_DEV_ATTR_IRMS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_IRMS(0644,
 		ade7753_read_24bit,
 		NULL,
 		ADE7753_IRMS);
-static IIO_DEV_ATTR_VRMS(S_IRUGO,
+static IIO_DEV_ATTR_VRMS(0444,
 		ade7753_read_24bit,
 		NULL,
 		ADE7753_VRMS);
-static IIO_DEV_ATTR_IRMSOS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_IRMSOS(0644,
 		ade7753_read_16bit,
 		ade7753_write_16bit,
 		ADE7753_IRMSOS);
-static IIO_DEV_ATTR_VRMSOS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_VRMSOS(0644,
 		ade7753_read_16bit,
 		ade7753_write_16bit,
 		ADE7753_VRMSOS);
-static IIO_DEV_ATTR_WGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_WGAIN(0644,
 		ade7753_read_16bit,
 		ade7753_write_16bit,
 		ADE7753_WGAIN);
-static IIO_DEV_ATTR_VAGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_VAGAIN(0644,
 		ade7753_read_16bit,
 		ade7753_write_16bit,
 		ADE7753_VAGAIN);
-static IIO_DEV_ATTR_PGA_GAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_PGA_GAIN(0644,
 		ade7753_read_16bit,
 		ade7753_write_16bit,
 		ADE7753_GAIN);
-static IIO_DEV_ATTR_IPKLVL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_IPKLVL(0644,
 		ade7753_read_8bit,
 		ade7753_write_8bit,
 		ADE7753_IPKLVL);
-static IIO_DEV_ATTR_VPKLVL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_VPKLVL(0644,
 		ade7753_read_8bit,
 		ade7753_write_8bit,
 		ADE7753_VPKLVL);
-static IIO_DEV_ATTR_IPEAK(S_IRUGO,
+static IIO_DEV_ATTR_IPEAK(0444,
 		ade7753_read_24bit,
 		NULL,
 		ADE7753_IPEAK);
-static IIO_DEV_ATTR_VPEAK(S_IRUGO,
+static IIO_DEV_ATTR_VPEAK(0444,
 		ade7753_read_24bit,
 		NULL,
 		ADE7753_VPEAK);
-static IIO_DEV_ATTR_VPERIOD(S_IRUGO,
+static IIO_DEV_ATTR_VPERIOD(0444,
 		ade7753_read_16bit,
 		NULL,
 		ADE7753_PERIOD);
-static IIO_DEV_ATTR_CH_OFF(1, S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CH_OFF(1, 0644,
 		ade7753_read_8bit,
 		ade7753_write_8bit,
 		ADE7753_CH1OS);
-static IIO_DEV_ATTR_CH_OFF(2, S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CH_OFF(2, 0644,
 		ade7753_read_8bit,
 		ade7753_write_8bit,
 		ADE7753_CH2OS);
@@ -450,8 +449,8 @@ err_ret:
 }
 
 static ssize_t ade7753_read_frequency(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
+				      struct device_attribute *attr,
+				      char *buf)
 {
 	int ret;
 	u16 t;
@@ -468,9 +467,9 @@ static ssize_t ade7753_read_frequency(struct device *dev,
 }
 
 static ssize_t ade7753_write_frequency(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
+				       struct device_attribute *attr,
+				       const char *buf,
+				       size_t len)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7753_state *st = iio_priv(indio_dev);
@@ -514,7 +513,7 @@ static IIO_DEV_ATTR_TEMP_RAW(ade7753_read_8bit);
 static IIO_CONST_ATTR(in_temp_offset, "-25 C");
 static IIO_CONST_ATTR(in_temp_scale, "0.67 C");
 
-static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_SAMP_FREQ(0644,
 		ade7753_read_frequency,
 		ade7753_write_frequency);
 
diff --git a/drivers/staging/iio/meter/ade7754.c b/drivers/staging/iio/meter/ade7754.c
index 32dc50341746..be0df3fe4230 100644
--- a/drivers/staging/iio/meter/ade7754.c
+++ b/drivers/staging/iio/meter/ade7754.c
@@ -316,111 +316,111 @@ static IIO_DEV_ATTR_AENERGY(ade7754_read_24bit, ADE7754_AENERGY);
 static IIO_DEV_ATTR_LAENERGY(ade7754_read_24bit, ADE7754_LAENERGY);
 static IIO_DEV_ATTR_VAENERGY(ade7754_read_24bit, ADE7754_VAENERGY);
 static IIO_DEV_ATTR_LVAENERGY(ade7754_read_24bit, ADE7754_LVAENERGY);
-static IIO_DEV_ATTR_VPEAK(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_VPEAK(0644,
 		ade7754_read_8bit,
 		ade7754_write_8bit,
 		ADE7754_VPEAK);
-static IIO_DEV_ATTR_IPEAK(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_IPEAK(0644,
 		ade7754_read_8bit,
 		ade7754_write_8bit,
 		ADE7754_VPEAK);
-static IIO_DEV_ATTR_APHCAL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_APHCAL(0644,
 		ade7754_read_8bit,
 		ade7754_write_8bit,
 		ADE7754_APHCAL);
-static IIO_DEV_ATTR_BPHCAL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_BPHCAL(0644,
 		ade7754_read_8bit,
 		ade7754_write_8bit,
 		ADE7754_BPHCAL);
-static IIO_DEV_ATTR_CPHCAL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CPHCAL(0644,
 		ade7754_read_8bit,
 		ade7754_write_8bit,
 		ADE7754_CPHCAL);
-static IIO_DEV_ATTR_AAPOS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_AAPOS(0644,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_AAPOS);
-static IIO_DEV_ATTR_BAPOS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_BAPOS(0644,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_BAPOS);
-static IIO_DEV_ATTR_CAPOS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CAPOS(0644,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_CAPOS);
-static IIO_DEV_ATTR_WDIV(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_WDIV(0644,
 		ade7754_read_8bit,
 		ade7754_write_8bit,
 		ADE7754_WDIV);
-static IIO_DEV_ATTR_VADIV(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_VADIV(0644,
 		ade7754_read_8bit,
 		ade7754_write_8bit,
 		ADE7754_VADIV);
-static IIO_DEV_ATTR_CFNUM(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CFNUM(0644,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_CFNUM);
-static IIO_DEV_ATTR_CFDEN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CFDEN(0644,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_CFDEN);
-static IIO_DEV_ATTR_ACTIVE_POWER_A_GAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_ACTIVE_POWER_A_GAIN(0644,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_AAPGAIN);
-static IIO_DEV_ATTR_ACTIVE_POWER_B_GAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_ACTIVE_POWER_B_GAIN(0644,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_BAPGAIN);
-static IIO_DEV_ATTR_ACTIVE_POWER_C_GAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_ACTIVE_POWER_C_GAIN(0644,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_CAPGAIN);
-static IIO_DEV_ATTR_AIRMS(S_IRUGO,
+static IIO_DEV_ATTR_AIRMS(0444,
 		ade7754_read_24bit,
 		NULL,
 		ADE7754_AIRMS);
-static IIO_DEV_ATTR_BIRMS(S_IRUGO,
+static IIO_DEV_ATTR_BIRMS(0444,
 		ade7754_read_24bit,
 		NULL,
 		ADE7754_BIRMS);
-static IIO_DEV_ATTR_CIRMS(S_IRUGO,
+static IIO_DEV_ATTR_CIRMS(0444,
 		ade7754_read_24bit,
 		NULL,
 		ADE7754_CIRMS);
-static IIO_DEV_ATTR_AVRMS(S_IRUGO,
+static IIO_DEV_ATTR_AVRMS(0444,
 		ade7754_read_24bit,
 		NULL,
 		ADE7754_AVRMS);
-static IIO_DEV_ATTR_BVRMS(S_IRUGO,
+static IIO_DEV_ATTR_BVRMS(0444,
 		ade7754_read_24bit,
 		NULL,
 		ADE7754_BVRMS);
-static IIO_DEV_ATTR_CVRMS(S_IRUGO,
+static IIO_DEV_ATTR_CVRMS(0444,
 		ade7754_read_24bit,
 		NULL,
 		ADE7754_CVRMS);
-static IIO_DEV_ATTR_AIRMSOS(S_IRUGO,
+static IIO_DEV_ATTR_AIRMSOS(0444,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_AIRMSOS);
-static IIO_DEV_ATTR_BIRMSOS(S_IRUGO,
+static IIO_DEV_ATTR_BIRMSOS(0444,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_BIRMSOS);
-static IIO_DEV_ATTR_CIRMSOS(S_IRUGO,
+static IIO_DEV_ATTR_CIRMSOS(0444,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_CIRMSOS);
-static IIO_DEV_ATTR_AVRMSOS(S_IRUGO,
+static IIO_DEV_ATTR_AVRMSOS(0444,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_AVRMSOS);
-static IIO_DEV_ATTR_BVRMSOS(S_IRUGO,
+static IIO_DEV_ATTR_BVRMSOS(0444,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_BVRMSOS);
-static IIO_DEV_ATTR_CVRMSOS(S_IRUGO,
+static IIO_DEV_ATTR_CVRMSOS(0444,
 		ade7754_read_16bit,
 		ade7754_write_16bit,
 		ADE7754_CVRMSOS);
@@ -549,7 +549,7 @@ static IIO_DEV_ATTR_TEMP_RAW(ade7754_read_8bit);
 static IIO_CONST_ATTR(in_temp_offset, "129 C");
 static IIO_CONST_ATTR(in_temp_scale, "4 C");
 
-static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_SAMP_FREQ(0644,
 		ade7754_read_frequency,
 		ade7754_write_frequency);
 
diff --git a/drivers/staging/iio/meter/ade7758_core.c b/drivers/staging/iio/meter/ade7758_core.c
index 99c89e606c8d..40498af4dc46 100644
--- a/drivers/staging/iio/meter/ade7758_core.c
+++ b/drivers/staging/iio/meter/ade7758_core.c
@@ -301,103 +301,103 @@ static int ade7758_reset(struct device *dev)
 	return ret;
 }
 
-static IIO_DEV_ATTR_VPEAK(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_VPEAK(0644,
 		ade7758_read_8bit,
 		ade7758_write_8bit,
 		ADE7758_VPEAK);
-static IIO_DEV_ATTR_IPEAK(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_IPEAK(0644,
 		ade7758_read_8bit,
 		ade7758_write_8bit,
 		ADE7758_VPEAK);
-static IIO_DEV_ATTR_APHCAL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_APHCAL(0644,
 		ade7758_read_8bit,
 		ade7758_write_8bit,
 		ADE7758_APHCAL);
-static IIO_DEV_ATTR_BPHCAL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_BPHCAL(0644,
 		ade7758_read_8bit,
 		ade7758_write_8bit,
 		ADE7758_BPHCAL);
-static IIO_DEV_ATTR_CPHCAL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CPHCAL(0644,
 		ade7758_read_8bit,
 		ade7758_write_8bit,
 		ADE7758_CPHCAL);
-static IIO_DEV_ATTR_WDIV(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_WDIV(0644,
 		ade7758_read_8bit,
 		ade7758_write_8bit,
 		ADE7758_WDIV);
-static IIO_DEV_ATTR_VADIV(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_VADIV(0644,
 		ade7758_read_8bit,
 		ade7758_write_8bit,
 		ADE7758_VADIV);
-static IIO_DEV_ATTR_AIRMS(S_IRUGO,
+static IIO_DEV_ATTR_AIRMS(0444,
 		ade7758_read_24bit,
 		NULL,
 		ADE7758_AIRMS);
-static IIO_DEV_ATTR_BIRMS(S_IRUGO,
+static IIO_DEV_ATTR_BIRMS(0444,
 		ade7758_read_24bit,
 		NULL,
 		ADE7758_BIRMS);
-static IIO_DEV_ATTR_CIRMS(S_IRUGO,
+static IIO_DEV_ATTR_CIRMS(0444,
 		ade7758_read_24bit,
 		NULL,
 		ADE7758_CIRMS);
-static IIO_DEV_ATTR_AVRMS(S_IRUGO,
+static IIO_DEV_ATTR_AVRMS(0444,
 		ade7758_read_24bit,
 		NULL,
 		ADE7758_AVRMS);
-static IIO_DEV_ATTR_BVRMS(S_IRUGO,
+static IIO_DEV_ATTR_BVRMS(0444,
 		ade7758_read_24bit,
 		NULL,
 		ADE7758_BVRMS);
-static IIO_DEV_ATTR_CVRMS(S_IRUGO,
+static IIO_DEV_ATTR_CVRMS(0444,
 		ade7758_read_24bit,
 		NULL,
 		ADE7758_CVRMS);
-static IIO_DEV_ATTR_AIRMSOS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_AIRMSOS(0644,
 		ade7758_read_16bit,
 		ade7758_write_16bit,
 		ADE7758_AIRMSOS);
-static IIO_DEV_ATTR_BIRMSOS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_BIRMSOS(0644,
 		ade7758_read_16bit,
 		ade7758_write_16bit,
 		ADE7758_BIRMSOS);
-static IIO_DEV_ATTR_CIRMSOS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CIRMSOS(0644,
 		ade7758_read_16bit,
 		ade7758_write_16bit,
 		ADE7758_CIRMSOS);
-static IIO_DEV_ATTR_AVRMSOS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_AVRMSOS(0644,
 		ade7758_read_16bit,
 		ade7758_write_16bit,
 		ADE7758_AVRMSOS);
-static IIO_DEV_ATTR_BVRMSOS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_BVRMSOS(0644,
 		ade7758_read_16bit,
 		ade7758_write_16bit,
 		ADE7758_BVRMSOS);
-static IIO_DEV_ATTR_CVRMSOS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CVRMSOS(0644,
 		ade7758_read_16bit,
 		ade7758_write_16bit,
 		ADE7758_CVRMSOS);
-static IIO_DEV_ATTR_AIGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_AIGAIN(0644,
 		ade7758_read_16bit,
 		ade7758_write_16bit,
 		ADE7758_AIGAIN);
-static IIO_DEV_ATTR_BIGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_BIGAIN(0644,
 		ade7758_read_16bit,
 		ade7758_write_16bit,
 		ADE7758_BIGAIN);
-static IIO_DEV_ATTR_CIGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CIGAIN(0644,
 		ade7758_read_16bit,
 		ade7758_write_16bit,
 		ADE7758_CIGAIN);
-static IIO_DEV_ATTR_AVRMSGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_AVRMSGAIN(0644,
 		ade7758_read_16bit,
 		ade7758_write_16bit,
 		ADE7758_AVRMSGAIN);
-static IIO_DEV_ATTR_BVRMSGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_BVRMSGAIN(0644,
 		ade7758_read_16bit,
 		ade7758_write_16bit,
 		ADE7758_BVRMSGAIN);
-static IIO_DEV_ATTR_CVRMSGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CVRMSGAIN(0644,
 		ade7758_read_16bit,
 		ade7758_write_16bit,
 		ADE7758_CVRMSGAIN);
diff --git a/drivers/staging/iio/meter/ade7854.c b/drivers/staging/iio/meter/ade7854.c
index c6cffc11b0ba..70612da64a8b 100644
--- a/drivers/staging/iio/meter/ade7854.c
+++ b/drivers/staging/iio/meter/ade7854.c
@@ -186,127 +186,127 @@ static int ade7854_reset(struct device *dev)
 	return st->write_reg_16(dev, ADE7854_CONFIG, val);
 }
 
-static IIO_DEV_ATTR_AIGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_AIGAIN(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_AIGAIN);
-static IIO_DEV_ATTR_BIGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_BIGAIN(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_BIGAIN);
-static IIO_DEV_ATTR_CIGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CIGAIN(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_CIGAIN);
-static IIO_DEV_ATTR_NIGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_NIGAIN(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_NIGAIN);
-static IIO_DEV_ATTR_AVGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_AVGAIN(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_AVGAIN);
-static IIO_DEV_ATTR_BVGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_BVGAIN(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_BVGAIN);
-static IIO_DEV_ATTR_CVGAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CVGAIN(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_CVGAIN);
-static IIO_DEV_ATTR_APPARENT_POWER_A_GAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_APPARENT_POWER_A_GAIN(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_AVAGAIN);
-static IIO_DEV_ATTR_APPARENT_POWER_B_GAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_APPARENT_POWER_B_GAIN(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_BVAGAIN);
-static IIO_DEV_ATTR_APPARENT_POWER_C_GAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_APPARENT_POWER_C_GAIN(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_CVAGAIN);
-static IIO_DEV_ATTR_ACTIVE_POWER_A_OFFSET(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_ACTIVE_POWER_A_OFFSET(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_AWATTOS);
-static IIO_DEV_ATTR_ACTIVE_POWER_B_OFFSET(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_ACTIVE_POWER_B_OFFSET(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_BWATTOS);
-static IIO_DEV_ATTR_ACTIVE_POWER_C_OFFSET(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_ACTIVE_POWER_C_OFFSET(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_CWATTOS);
-static IIO_DEV_ATTR_REACTIVE_POWER_A_GAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_REACTIVE_POWER_A_GAIN(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_AVARGAIN);
-static IIO_DEV_ATTR_REACTIVE_POWER_B_GAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_REACTIVE_POWER_B_GAIN(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_BVARGAIN);
-static IIO_DEV_ATTR_REACTIVE_POWER_C_GAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_REACTIVE_POWER_C_GAIN(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_CVARGAIN);
-static IIO_DEV_ATTR_REACTIVE_POWER_A_OFFSET(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_REACTIVE_POWER_A_OFFSET(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_AVAROS);
-static IIO_DEV_ATTR_REACTIVE_POWER_B_OFFSET(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_REACTIVE_POWER_B_OFFSET(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_BVAROS);
-static IIO_DEV_ATTR_REACTIVE_POWER_C_OFFSET(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_REACTIVE_POWER_C_OFFSET(0644,
 		ade7854_read_24bit,
 		ade7854_write_24bit,
 		ADE7854_CVAROS);
-static IIO_DEV_ATTR_VPEAK(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_VPEAK(0644,
 		ade7854_read_32bit,
 		ade7854_write_32bit,
 		ADE7854_VPEAK);
-static IIO_DEV_ATTR_IPEAK(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_IPEAK(0644,
 		ade7854_read_32bit,
 		ade7854_write_32bit,
 		ADE7854_VPEAK);
-static IIO_DEV_ATTR_APHCAL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_APHCAL(0644,
 		ade7854_read_16bit,
 		ade7854_write_16bit,
 		ADE7854_APHCAL);
-static IIO_DEV_ATTR_BPHCAL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_BPHCAL(0644,
 		ade7854_read_16bit,
 		ade7854_write_16bit,
 		ADE7854_BPHCAL);
-static IIO_DEV_ATTR_CPHCAL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CPHCAL(0644,
 		ade7854_read_16bit,
 		ade7854_write_16bit,
 		ADE7854_CPHCAL);
-static IIO_DEV_ATTR_CF1DEN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CF1DEN(0644,
 		ade7854_read_16bit,
 		ade7854_write_16bit,
 		ADE7854_CF1DEN);
-static IIO_DEV_ATTR_CF2DEN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CF2DEN(0644,
 		ade7854_read_16bit,
 		ade7854_write_16bit,
 		ADE7854_CF2DEN);
-static IIO_DEV_ATTR_CF3DEN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CF3DEN(0644,
 		ade7854_read_16bit,
 		ade7854_write_16bit,
 		ADE7854_CF3DEN);
-static IIO_DEV_ATTR_LINECYC(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_LINECYC(0644,
 		ade7854_read_16bit,
 		ade7854_write_16bit,
 		ADE7854_LINECYC);
-static IIO_DEV_ATTR_SAGCYC(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_SAGCYC(0644,
 		ade7854_read_8bit,
 		ade7854_write_8bit,
 		ADE7854_SAGCYC);
-static IIO_DEV_ATTR_CFCYC(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CFCYC(0644,
 		ade7854_read_8bit,
 		ade7854_write_8bit,
 		ADE7854_CFCYC);
-static IIO_DEV_ATTR_PEAKCYC(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_PEAKCYC(0644,
 		ade7854_read_8bit,
 		ade7854_write_8bit,
 		ADE7854_PEAKCYC);
@@ -318,55 +318,55 @@ static IIO_DEV_ATTR_ANGLE1(ade7854_read_24bit,
 		ADE7854_ANGLE1);
 static IIO_DEV_ATTR_ANGLE2(ade7854_read_24bit,
 		ADE7854_ANGLE2);
-static IIO_DEV_ATTR_AIRMS(S_IRUGO,
+static IIO_DEV_ATTR_AIRMS(0444,
 		ade7854_read_24bit,
 		NULL,
 		ADE7854_AIRMS);
-static IIO_DEV_ATTR_BIRMS(S_IRUGO,
+static IIO_DEV_ATTR_BIRMS(0444,
 		ade7854_read_24bit,
 		NULL,
 		ADE7854_BIRMS);
-static IIO_DEV_ATTR_CIRMS(S_IRUGO,
+static IIO_DEV_ATTR_CIRMS(0444,
 		ade7854_read_24bit,
 		NULL,
 		ADE7854_CIRMS);
-static IIO_DEV_ATTR_NIRMS(S_IRUGO,
+static IIO_DEV_ATTR_NIRMS(0444,
 		ade7854_read_24bit,
 		NULL,
 		ADE7854_NIRMS);
-static IIO_DEV_ATTR_AVRMS(S_IRUGO,
+static IIO_DEV_ATTR_AVRMS(0444,
 		ade7854_read_24bit,
 		NULL,
 		ADE7854_AVRMS);
-static IIO_DEV_ATTR_BVRMS(S_IRUGO,
+static IIO_DEV_ATTR_BVRMS(0444,
 		ade7854_read_24bit,
 		NULL,
 		ADE7854_BVRMS);
-static IIO_DEV_ATTR_CVRMS(S_IRUGO,
+static IIO_DEV_ATTR_CVRMS(0444,
 		ade7854_read_24bit,
 		NULL,
 		ADE7854_CVRMS);
-static IIO_DEV_ATTR_AIRMSOS(S_IRUGO,
+static IIO_DEV_ATTR_AIRMSOS(0444,
 		ade7854_read_16bit,
 		ade7854_write_16bit,
 		ADE7854_AIRMSOS);
-static IIO_DEV_ATTR_BIRMSOS(S_IRUGO,
+static IIO_DEV_ATTR_BIRMSOS(0444,
 		ade7854_read_16bit,
 		ade7854_write_16bit,
 		ADE7854_BIRMSOS);
-static IIO_DEV_ATTR_CIRMSOS(S_IRUGO,
+static IIO_DEV_ATTR_CIRMSOS(0444,
 		ade7854_read_16bit,
 		ade7854_write_16bit,
 		ADE7854_CIRMSOS);
-static IIO_DEV_ATTR_AVRMSOS(S_IRUGO,
+static IIO_DEV_ATTR_AVRMSOS(0444,
 		ade7854_read_16bit,
 		ade7854_write_16bit,
 		ADE7854_AVRMSOS);
-static IIO_DEV_ATTR_BVRMSOS(S_IRUGO,
+static IIO_DEV_ATTR_BVRMSOS(0444,
 		ade7854_read_16bit,
 		ade7854_write_16bit,
 		ADE7854_BVRMSOS);
-static IIO_DEV_ATTR_CVRMSOS(S_IRUGO,
+static IIO_DEV_ATTR_CVRMSOS(0444,
 		ade7854_read_16bit,
 		ade7854_write_16bit,
 		ADE7854_CVRMSOS);
diff --git a/drivers/staging/ks7010/eap_packet.h b/drivers/staging/ks7010/eap_packet.h
index b2d25ef1cd6b..ae03f7477324 100644
--- a/drivers/staging/ks7010/eap_packet.h
+++ b/drivers/staging/ks7010/eap_packet.h
@@ -18,7 +18,7 @@ struct ether_hdr {
 	unsigned char h_source_snap;
 	unsigned char h_command;
 	unsigned char h_vendor_id[3];
-	unsigned short h_proto;	/* packet type ID field */
+	__be16 h_proto;	/* packet type ID field */
 #define ETHER_PROTOCOL_TYPE_EAP		0x888e
 #define ETHER_PROTOCOL_TYPE_IP		0x0800
 #define ETHER_PROTOCOL_TYPE_ARP		0x0806
@@ -91,7 +91,7 @@ struct ieee802_1x_eapol_key {
 
 struct wpa_eapol_key {
 	unsigned char type;
-	unsigned short key_info;
+	__be16 key_info;
 	unsigned short key_length;
 	unsigned char replay_counter[WPA_REPLAY_COUNTER_LEN];
 	unsigned char key_nonce[WPA_NONCE_LEN];
diff --git a/drivers/staging/ks7010/ks7010_sdio.c b/drivers/staging/ks7010/ks7010_sdio.c
index c325f4846209..9b28ee1cfb1e 100644
--- a/drivers/staging/ks7010/ks7010_sdio.c
+++ b/drivers/staging/ks7010/ks7010_sdio.c
@@ -269,7 +269,8 @@ static int write_to_device(struct ks_wlan_private *priv, unsigned char *buffer,
 	hdr = (struct hostif_hdr *)buffer;
 
 	DPRINTK(4, "size=%d\n", hdr->size);
-	if (hdr->event < HIF_DATA_REQ || HIF_REQ_MAX < hdr->event) {
+	if (le16_to_cpu(hdr->event) < HIF_DATA_REQ ||
+	    le16_to_cpu(hdr->event) > HIF_REQ_MAX) {
 		DPRINTK(1, "unknown event=%04X\n", hdr->event);
 		return 0;
 	}
@@ -327,13 +328,14 @@ int ks_wlan_hw_tx(struct ks_wlan_private *priv, void *p, unsigned long size,
 
 	hdr = (struct hostif_hdr *)p;
 
-	if (hdr->event < HIF_DATA_REQ || HIF_REQ_MAX < hdr->event) {
+	if (le16_to_cpu(hdr->event) < HIF_DATA_REQ ||
+	    le16_to_cpu(hdr->event) > HIF_REQ_MAX) {
 		DPRINTK(1, "unknown event=%04X\n", hdr->event);
 		return 0;
 	}
 
 	/* add event to hostt buffer */
-	priv->hostt.buff[priv->hostt.qtail] = hdr->event;
+	priv->hostt.buff[priv->hostt.qtail] = le16_to_cpu(hdr->event);
 	priv->hostt.qtail = (priv->hostt.qtail + 1) % SME_EVENT_BUFF_SIZE;
 
 	DPRINTK(4, "event=%04X\n", hdr->event);
@@ -403,7 +405,7 @@ static void ks_wlan_hw_rx(struct ks_wlan_private *priv, uint16_t size)
 
 	hdr = (struct hostif_hdr *)&rx_buffer->data[0];
 	rx_buffer->size = le16_to_cpu(hdr->size) + sizeof(hdr->size);
-	event = hdr->event;
+	event = le16_to_cpu(hdr->event);
 	inc_rxqtail(priv);
 
 	ret = ks7010_sdio_writeb(priv, READ_STATUS, REG_STATUS_IDLE);
diff --git a/drivers/staging/ks7010/ks_hostif.c b/drivers/staging/ks7010/ks_hostif.c
index 49e95426ac30..24a63161f92c 100644
--- a/drivers/staging/ks7010/ks_hostif.c
+++ b/drivers/staging/ks7010/ks_hostif.c
@@ -147,7 +147,7 @@ int get_current_ap(struct ks_wlan_private *priv, struct link_ap_info_t *ap_info)
 	/* noise */
 	ap->noise = ap_info->noise;
 	/* capability */
-	ap->capability = ap_info->capability;
+	ap->capability = le16_to_cpu(ap_info->capability);
 	/* rsn */
 	if ((ap_info->rsn_mode & RSN_MODE_WPA2) &&
 	    (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2)) {
@@ -233,12 +233,12 @@ int get_ap_information(struct ks_wlan_private *priv, struct ap_info_t *ap_info,
 	/* noise */
 	ap->noise = ap_info->noise;
 	/* capability */
-	ap->capability = ap_info->capability;
+	ap->capability = le16_to_cpu(ap_info->capability);
 	/* channel */
 	ap->channel = ap_info->ch_info;
 
 	bp = ap_info->body;
-	bsize = ap_info->body_size;
+	bsize = le16_to_cpu(ap_info->body_size);
 	offset = 0;
 
 	while (bsize > offset) {
@@ -466,12 +466,12 @@ void hostif_data_indication(struct ks_wlan_private *priv)
 		DPRINTK(4, "SNAP, rx_ind_size = %d\n", rx_ind_size);
 
 		size = ETH_ALEN * 2;
-		memcpy(skb_put(skb, size), priv->rxp, size);
+		skb_put_data(skb, priv->rxp, size);
 
 		/* (SNAP+UI..) skip */
 
 		size = rx_ind_size - (ETH_ALEN * 2);
-		memcpy(skb_put(skb, size), &eth_hdr->h_proto, size);
+		skb_put_data(skb, &eth_hdr->h_proto, size);
 
 		aa1x_hdr = (struct ieee802_1x_hdr *)(priv->rxp + ETHER_HDR_SIZE);
 		break;
@@ -484,14 +484,13 @@ void hostif_data_indication(struct ks_wlan_private *priv)
 		}
 		DPRINTK(3, "NETBEUI/NetBIOS rx_ind_size=%d\n", rx_ind_size);
 
-		memcpy(skb_put(skb, 12), priv->rxp, 12);	/* 8802/FDDI MAC copy */
+		skb_put_data(skb, priv->rxp, 12);	/* 8802/FDDI MAC copy */
 
 		temp[0] = (((rx_ind_size - 12) >> 8) & 0xff);	/* NETBEUI size add */
 		temp[1] = ((rx_ind_size - 12) & 0xff);
-		memcpy(skb_put(skb, 2), temp, 2);
+		skb_put_data(skb, temp, 2);
 
-		memcpy(skb_put(skb, rx_ind_size - 14), priv->rxp + 12,
-		       rx_ind_size - 14);	/* copy after Type */
+		skb_put_data(skb, priv->rxp + 12, rx_ind_size - 14);	/* copy after Type */
 
 		aa1x_hdr = (struct ieee802_1x_hdr *)(priv->rxp + 14);
 		break;
@@ -567,9 +566,9 @@ void hostif_mib_get_confirm(struct ks_wlan_private *priv)
 		break;
 	case LOCAL_GAIN:
 		memcpy(&priv->gain, priv->rxp, sizeof(priv->gain));
-		DPRINTK(3, "TxMode=%d, RxMode=%d, TxGain=%d, RxGain=%d\n",
-			priv->gain.TxMode, priv->gain.RxMode, priv->gain.TxGain,
-			priv->gain.RxGain);
+		DPRINTK(3, "tx_mode=%d, rx_mode=%d, tx_gain=%d, rx_gain=%d\n",
+			priv->gain.tx_mode, priv->gain.rx_mode,
+			priv->gain.tx_gain, priv->gain.rx_gain);
 		break;
 	case LOCAL_EEPROM_SUM:
 		memcpy(&priv->eeprom_sum, priv->rxp, sizeof(priv->eeprom_sum));
@@ -948,18 +947,18 @@ void hostif_associate_indication(struct ks_wlan_private *priv)
 	wrqu.data.length += sizeof(associnfo_leader0) - 1;
 	pbuf += sizeof(associnfo_leader0) - 1;
 
-	for (i = 0; i < assoc_req->reqIEs_size; i++)
+	for (i = 0; i < le16_to_cpu(assoc_req->req_ies_size); i++)
 		pbuf += sprintf(pbuf, "%02x", *(pb + i));
-	wrqu.data.length += (assoc_req->reqIEs_size) * 2;
+	wrqu.data.length += (le16_to_cpu(assoc_req->req_ies_size)) * 2;
 
 	memcpy(pbuf, associnfo_leader1, sizeof(associnfo_leader1) - 1);
 	wrqu.data.length += sizeof(associnfo_leader1) - 1;
 	pbuf += sizeof(associnfo_leader1) - 1;
 
-	pb += assoc_req->reqIEs_size;
-	for (i = 0; i < assoc_resp->respIEs_size; i++)
+	pb += le16_to_cpu(assoc_req->req_ies_size);
+	for (i = 0; i < le16_to_cpu(assoc_resp->resp_ies_size); i++)
 		pbuf += sprintf(pbuf, "%02x", *(pb + i));
-	wrqu.data.length += (assoc_resp->respIEs_size) * 2;
+	wrqu.data.length += (le16_to_cpu(assoc_resp->resp_ies_size)) * 2;
 
 	pbuf += sprintf(pbuf, ")");
 	wrqu.data.length += 1;
@@ -994,22 +993,22 @@ void hostif_phy_information_confirm(struct ks_wlan_private *priv)
 {
 	struct iw_statistics *wstats = &priv->wstats;
 	unsigned char rssi, signal, noise;
-	unsigned char LinkSpeed;
-	unsigned int TransmittedFrameCount, ReceivedFragmentCount;
-	unsigned int FailedCount, FCSErrorCount;
+	unsigned char link_speed;
+	unsigned int transmitted_frame_count, received_fragment_count;
+	unsigned int failed_count, fcs_error_count;
 
 	DPRINTK(3, "\n");
 	rssi = get_BYTE(priv);
 	signal = get_BYTE(priv);
 	noise = get_BYTE(priv);
-	LinkSpeed = get_BYTE(priv);
-	TransmittedFrameCount = get_DWORD(priv);
-	ReceivedFragmentCount = get_DWORD(priv);
-	FailedCount = get_DWORD(priv);
-	FCSErrorCount = get_DWORD(priv);
+	link_speed = get_BYTE(priv);
+	transmitted_frame_count = get_DWORD(priv);
+	received_fragment_count = get_DWORD(priv);
+	failed_count = get_DWORD(priv);
+	fcs_error_count = get_DWORD(priv);
 
 	DPRINTK(4, "phyinfo confirm rssi=%d signal=%d\n", rssi, signal);
-	priv->current_rate = (LinkSpeed & RATE_MASK);
+	priv->current_rate = (link_speed & RATE_MASK);
 	wstats->qual.qual = signal;
 	wstats->qual.level = 256 - rssi;
 	wstats->qual.noise = 0;	/* invalid noise value */
@@ -1017,14 +1016,13 @@ void hostif_phy_information_confirm(struct ks_wlan_private *priv)
 
 	DPRINTK(3, "\n    rssi=%u\n"
 		   "    signal=%u\n"
-		   "    LinkSpeed=%ux500Kbps\n"
-		   "    TransmittedFrameCount=%u\n"
-		   "    ReceivedFragmentCount=%u\n"
-		   "    FailedCount=%u\n"
-		   "    FCSErrorCount=%u\n",
-		rssi, signal, LinkSpeed, TransmittedFrameCount,
-		ReceivedFragmentCount, FailedCount, FCSErrorCount);
-
+		   "    link_speed=%ux500Kbps\n"
+		   "    transmitted_frame_count=%u\n"
+		   "    received_fragment_count=%u\n"
+		   "    failed_count=%u\n"
+		   "    fcs_error_count=%u\n",
+		rssi, signal, link_speed, transmitted_frame_count,
+		received_fragment_count, failed_count, fcs_error_count);
 	/* wake_up_interruptible_all(&priv->confirm_wait); */
 	complete(&priv->confirm_wait);
 }
@@ -1660,13 +1658,13 @@ void hostif_phy_information_request(struct ks_wlan_private *priv)
 
 static
 void hostif_power_mgmt_request(struct ks_wlan_private *priv,
-			       unsigned long mode, unsigned long wake_up,
-			       unsigned long receiveDTIMs)
+				unsigned long mode, unsigned long wake_up,
+				unsigned long receive_dtims)
 {
 	struct hostif_power_mgmt_request_t *pp;
 
-	DPRINTK(3, "mode=%lu wake_up=%lu receiveDTIMs=%lu\n", mode, wake_up,
-		receiveDTIMs);
+	DPRINTK(3, "mode=%lu wake_up=%lu receive_dtims=%lu\n", mode, wake_up,
+		receive_dtims);
 
 	pp = hostif_generic_request(sizeof(*pp), HIF_POWER_MGMT_REQ);
 	if (!pp)
@@ -1674,7 +1672,7 @@ void hostif_power_mgmt_request(struct ks_wlan_private *priv,
 
 	pp->mode = cpu_to_le32((uint32_t)mode);
 	pp->wake_up = cpu_to_le32((uint32_t)wake_up);
-	pp->receiveDTIMs = cpu_to_le32((uint32_t)receiveDTIMs);
+	pp->receive_dtims = cpu_to_le32((uint32_t)receive_dtims);
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
@@ -1822,7 +1820,7 @@ void hostif_receive(struct ks_wlan_private *priv, unsigned char *p,
 static
 void hostif_sme_set_wep(struct ks_wlan_private *priv, int type)
 {
-	u32 val;
+	__le32 val;
 
 	switch (type) {
 	case SME_WEP_INDEX_REQUEST:
@@ -1871,13 +1869,13 @@ void hostif_sme_set_wep(struct ks_wlan_private *priv, int type)
 }
 
 struct wpa_suite_t {
-	unsigned short size;
+	__le16 size;
 	unsigned char suite[4][CIPHER_ID_LEN];
 } __packed;
 
 struct rsn_mode_t {
-	u32 rsn_mode;
-	u16 rsn_capability;
+	__le32 rsn_mode;
+	__le16 rsn_capability;
 } __packed;
 
 static
@@ -1885,7 +1883,7 @@ void hostif_sme_set_rsn(struct ks_wlan_private *priv, int type)
 {
 	struct wpa_suite_t wpa_suite;
 	struct rsn_mode_t rsn_mode;
-	u32 val;
+	__le32 val;
 
 	memset(&wpa_suite, 0, sizeof(wpa_suite));
 
@@ -1937,7 +1935,8 @@ void hostif_sme_set_rsn(struct ks_wlan_private *priv, int type)
 
 		hostif_mib_set_request(priv, DOT11_RSN_CONFIG_UNICAST_CIPHER,
 				       sizeof(wpa_suite.size) +
-				       CIPHER_ID_LEN * wpa_suite.size,
+				       CIPHER_ID_LEN *
+				       le16_to_cpu(wpa_suite.size),
 				       MIB_VALUE_TYPE_OSTRING, &wpa_suite);
 		break;
 	case SME_RSN_MCAST_REQUEST:
@@ -2029,7 +2028,8 @@ void hostif_sme_set_rsn(struct ks_wlan_private *priv, int type)
 
 		hostif_mib_set_request(priv, DOT11_RSN_CONFIG_AUTH_SUITE,
 				       sizeof(wpa_suite.size) +
-				       KEY_MGMT_ID_LEN * wpa_suite.size,
+				       KEY_MGMT_ID_LEN *
+				       le16_to_cpu(wpa_suite.size),
 				       MIB_VALUE_TYPE_OSTRING, &wpa_suite);
 		break;
 	case SME_RSN_ENABLED_REQUEST:
@@ -2166,7 +2166,7 @@ void hostif_sme_multicast_set(struct ks_wlan_private *priv)
 	int mc_count;
 	struct netdev_hw_addr *ha;
 	char set_address[NIC_MAX_MCAST_LIST * ETH_ALEN];
-	unsigned long filter_type;
+	__le32 filter_type;
 	int i = 0;
 
 	DPRINTK(3, "\n");
@@ -2218,44 +2218,44 @@ spin_unlock:
 static
 void hostif_sme_power_mgmt_set(struct ks_wlan_private *priv)
 {
-	unsigned long mode, wake_up, receiveDTIMs;
+	unsigned long mode, wake_up, receive_dtims;
 
 	DPRINTK(3, "\n");
 	switch (priv->reg.power_mgmt) {
 	case POWER_MGMT_ACTIVE:
 		mode = POWER_ACTIVE;
 		wake_up = 0;
-		receiveDTIMs = 0;
+		receive_dtims = 0;
 		break;
 	case POWER_MGMT_SAVE1:
 		if (priv->reg.operation_mode == MODE_INFRASTRUCTURE) {
 			mode = POWER_SAVE;
 			wake_up = 0;
-			receiveDTIMs = 0;
+			receive_dtims = 0;
 		} else {
 			mode = POWER_ACTIVE;
 			wake_up = 0;
-			receiveDTIMs = 0;
+			receive_dtims = 0;
 		}
 		break;
 	case POWER_MGMT_SAVE2:
 		if (priv->reg.operation_mode == MODE_INFRASTRUCTURE) {
 			mode = POWER_SAVE;
 			wake_up = 0;
-			receiveDTIMs = 1;
+			receive_dtims = 1;
 		} else {
 			mode = POWER_ACTIVE;
 			wake_up = 0;
-			receiveDTIMs = 0;
+			receive_dtims = 0;
 		}
 		break;
 	default:
 		mode = POWER_ACTIVE;
 		wake_up = 0;
-		receiveDTIMs = 0;
+		receive_dtims = 0;
 		break;
 	}
-	hostif_power_mgmt_request(priv, mode, wake_up, receiveDTIMs);
+	hostif_power_mgmt_request(priv, mode, wake_up, receive_dtims);
 }
 
 static
@@ -2277,7 +2277,7 @@ void hostif_sme_sleep_set(struct ks_wlan_private *priv)
 static
 void hostif_sme_set_key(struct ks_wlan_private *priv, int type)
 {
-	u32 val;
+	__le32 val;
 
 	switch (type) {
 	case SME_SET_FLAG:
@@ -2336,7 +2336,7 @@ static
 void hostif_sme_set_pmksa(struct ks_wlan_private *priv)
 {
 	struct pmk_cache_t {
-		u16 size;
+		__le16 size;
 		struct {
 			u8 bssid[ETH_ALEN];
 			u8 pmkid[IW_PMKID_LEN];
@@ -2367,7 +2367,7 @@ void hostif_sme_set_pmksa(struct ks_wlan_private *priv)
 static
 void hostif_sme_execute(struct ks_wlan_private *priv, int event)
 {
-	u32 val;
+	__le32 val;
 
 	DPRINTK(3, "event=%d\n", event);
 	switch (event) {
diff --git a/drivers/staging/ks7010/ks_hostif.h b/drivers/staging/ks7010/ks_hostif.h
index d758076d419d..5bae8d468e23 100644
--- a/drivers/staging/ks7010/ks_hostif.h
+++ b/drivers/staging/ks7010/ks_hostif.h
@@ -62,27 +62,27 @@
  */
 
 struct hostif_hdr {
-	u16 size;
-	u16 event;
+	__le16 size;
+	__le16 event;
 } __packed;
 
 struct hostif_data_request_t {
 	struct hostif_hdr header;
-	u16 auth_type;
+	__le16 auth_type;
 #define TYPE_DATA 0x0000
 #define TYPE_AUTH 0x0001
-	u16 reserved;
+	__le16 reserved;
 	u8 data[0];
 } __packed;
 
 struct hostif_data_indication_t {
 	struct hostif_hdr header;
-	u16 auth_type;
+	__le16 auth_type;
 /* #define TYPE_DATA 0x0000 */
 #define TYPE_PMK1 0x0001
 #define TYPE_GMK1 0x0002
 #define TYPE_GMK2 0x0003
-	u16 reserved;
+	__le16 reserved;
 	u8 data[0];
 } __packed;
 
@@ -143,12 +143,12 @@ struct channel_list_t {
 
 struct hostif_mib_get_request_t {
 	struct hostif_hdr header;
-	u32 mib_attribute;
+	__le32 mib_attribute;
 } __packed;
 
 struct hostif_mib_value_t {
-	u16 size;
-	u16 type;
+	__le16 size;
+	__le16 type;
 #define MIB_VALUE_TYPE_NULL     0
 #define MIB_VALUE_TYPE_INT      1
 #define MIB_VALUE_TYPE_BOOL     2
@@ -159,36 +159,36 @@ struct hostif_mib_value_t {
 
 struct hostif_mib_get_confirm_t {
 	struct hostif_hdr header;
-	u32 mib_status;
+	__le32 mib_status;
 #define MIB_SUCCESS    0
 #define MIB_INVALID    1
 #define MIB_READ_ONLY  2
 #define MIB_WRITE_ONLY 3
-	u32 mib_attribute;
+	__le32 mib_attribute;
 	struct hostif_mib_value_t mib_value;
 } __packed;
 
 struct hostif_mib_set_request_t {
 	struct hostif_hdr header;
-	u32 mib_attribute;
+	__le32 mib_attribute;
 	struct hostif_mib_value_t mib_value;
 } __packed;
 
 struct hostif_mib_set_confirm_t {
 	struct hostif_hdr header;
-	u32 mib_status;
-	u32 mib_attribute;
+	__le32 mib_status;
+	__le32 mib_attribute;
 } __packed;
 
 struct hostif_power_mgmt_request_t {
 	struct hostif_hdr header;
-	u32 mode;
+	__le32 mode;
 #define POWER_ACTIVE  1
 #define POWER_SAVE    2
-	u32 wake_up;
+	__le32 wake_up;
 #define SLEEP_FALSE 0
 #define SLEEP_TRUE  1	/* not used */
-	u32 receiveDTIMs;
+	__le32 receive_dtims;
 #define DTIM_FALSE 0
 #define DTIM_TRUE  1
 } __packed;
@@ -207,12 +207,12 @@ enum power_mgmt_mode_type {
 
 struct hostif_power_mgmt_confirm_t {
 	struct hostif_hdr header;
-	u16 result_code;
+	__le16 result_code;
 } __packed;
 
 struct hostif_start_request_t {
 	struct hostif_hdr header;
-	u16 mode;
+	__le16 mode;
 #define MODE_PSEUDO_ADHOC   0
 #define MODE_INFRASTRUCTURE 1
 #define MODE_AP             2	/* not used */
@@ -221,7 +221,7 @@ struct hostif_start_request_t {
 
 struct hostif_start_confirm_t {
 	struct hostif_hdr header;
-	u16 result_code;
+	__le16 result_code;
 } __packed;
 
 #define SSID_MAX_SIZE 32
@@ -238,26 +238,26 @@ struct rate_set8_t {
 	u8 rate_pad;
 } __packed;
 
-struct FhParms_t {
-	u16 dwellTime;
-	u8 hopSet;
-	u8 hopPattern;
-	u8 hopIndex;
+struct fh_parms_t {
+	__le16 dwell_time;
+	u8 hop_set;
+	u8 hop_pattern;
+	u8 hop_index;
 } __packed;
 
-struct DsParms_t {
+struct ds_parms_t {
 	u8 channel;
 } __packed;
 
-struct CfParms_t {
+struct cf_parms_t {
 	u8 count;
 	u8 period;
-	u16 maxDuration;
-	u16 durRemaining;
+	__le16 max_duration;
+	__le16 dur_remaining;
 } __packed;
 
-struct IbssParms_t {
-	u16 atimWindow;
+struct ibss_parms_t {
+	__le16 atim_window;
 } __packed;
 
 struct rsn_t {
@@ -266,7 +266,7 @@ struct rsn_t {
 	u8 body[RSN_BODY_SIZE];
 } __packed;
 
-struct ErpParams_t {
+struct erp_params_t {
 	u8 erp_info;
 } __packed;
 
@@ -282,8 +282,8 @@ struct ap_info_t {
 	u8 sq;	/* +07 */
 	u8 noise;	/* +08 */
 	u8 pad0;	/* +09 */
-	u16 beacon_period;	/* +10 */
-	u16 capability;	/* +12 */
+	__le16 beacon_period;	/* +10 */
+	__le16 capability;	/* +12 */
 #define BSS_CAP_ESS             BIT(0)
 #define BSS_CAP_IBSS            BIT(1)
 #define BSS_CAP_CF_POLABLE      BIT(2)
@@ -298,7 +298,7 @@ struct ap_info_t {
 	u8 ch_info;	/* +15 */
 #define FRAME_TYPE_BEACON	0x80
 #define FRAME_TYPE_PROBE_RESP	0x50
-	u16 body_size;	/* +16 */
+	__le16 body_size;	/* +16 */
 	u8 body[1024];	/* +18 */
 	/* +1032 */
 } __packed;
@@ -309,14 +309,14 @@ struct link_ap_info_t {
 	u8 sq;	/* +07 */
 	u8 noise;	/* +08 */
 	u8 pad0;	/* +09 */
-	u16 beacon_period;	/* +10 */
-	u16 capability;	/* +12 */
+	__le16 beacon_period;	/* +10 */
+	__le16 capability;	/* +12 */
 	struct rate_set8_t rate_set;	/* +14 */
-	struct FhParms_t fh_parameter;	/* +24 */
-	struct DsParms_t ds_parameter;	/* +29 */
-	struct CfParms_t cf_parameter;	/* +30 */
-	struct IbssParms_t ibss_parameter;	/* +36 */
-	struct ErpParams_t erp_parameter;	/* +38 */
+	struct fh_parms_t fh_parameter;	/* +24 */
+	struct ds_parms_t ds_parameter;	/* +29 */
+	struct cf_parms_t cf_parameter;	/* +30 */
+	struct ibss_parms_t ibss_parameter;	/* +36 */
+	struct erp_params_t erp_parameter;	/* +38 */
 	u8 pad1;	/* +39 */
 	struct rate_set8_t ext_rate_set;	/* +40 */
 	u8 DTIM_period;	/* +50 */
@@ -332,7 +332,7 @@ struct link_ap_info_t {
 
 struct hostif_connect_indication_t {
 	struct hostif_hdr header;
-	u16 connect_code;
+	__le16 connect_code;
 #define RESULT_CONNECT    0
 #define RESULT_DISCONNECT 1
 	struct link_ap_info_t link_ap_info;
@@ -344,7 +344,7 @@ struct hostif_stop_request_t {
 
 struct hostif_stop_confirm_t {
 	struct hostif_hdr header;
-	u16 result_code;
+	__le16 result_code;
 } __packed;
 
 /**
@@ -356,23 +356,23 @@ struct hostif_stop_confirm_t {
  */
 struct hostif_ps_adhoc_set_request_t {
 	struct hostif_hdr header;
-	u16 phy_type;
+	__le16 phy_type;
 #define D_11B_ONLY_MODE		0
 #define D_11G_ONLY_MODE		1
 #define D_11BG_COMPATIBLE_MODE	2
 #define D_11A_ONLY_MODE		3
-	u16 cts_mode;
+	__le16 cts_mode;
 #define CTS_MODE_FALSE	0
 #define CTS_MODE_TRUE	1
-	u16 channel;
+	__le16 channel;
 	struct rate_set16_t rate_set;
-	u16 capability;
-	u16 scan_type;
+	__le16 capability;
+	__le16 scan_type;
 } __packed;
 
 struct hostif_ps_adhoc_set_confirm_t {
 	struct hostif_hdr header;
-	u16 result_code;
+	__le16 result_code;
 } __packed;
 
 /**
@@ -384,17 +384,17 @@ struct hostif_ps_adhoc_set_confirm_t {
  */
 struct hostif_infrastructure_set_request_t {
 	struct hostif_hdr header;
-	u16 phy_type;
-	u16 cts_mode;
+	__le16 phy_type;
+	__le16 cts_mode;
 	struct rate_set16_t rate_set;
 	struct ssid_t ssid;
-	u16 capability;
-	u16 beacon_lost_count;
-	u16 auth_type;
+	__le16 capability;
+	__le16 beacon_lost_count;
+	__le16 auth_type;
 #define AUTH_TYPE_OPEN_SYSTEM 0
 #define AUTH_TYPE_SHARED_KEY  1
 	struct channel_list_t channel_list;
-	u16 scan_type;
+	__le16 scan_type;
 } __packed;
 
 /**
@@ -406,23 +406,23 @@ struct hostif_infrastructure_set_request_t {
  */
 struct hostif_infrastructure_set2_request_t {
 	struct hostif_hdr header;
-	u16 phy_type;
-	u16 cts_mode;
+	__le16 phy_type;
+	__le16 cts_mode;
 	struct rate_set16_t rate_set;
 	struct ssid_t ssid;
-	u16 capability;
-	u16 beacon_lost_count;
-	u16 auth_type;
+	__le16 capability;
+	__le16 beacon_lost_count;
+	__le16 auth_type;
 #define AUTH_TYPE_OPEN_SYSTEM 0
 #define AUTH_TYPE_SHARED_KEY  1
 	struct channel_list_t channel_list;
-	u16 scan_type;
+	__le16 scan_type;
 	u8 bssid[ETH_ALEN];
 } __packed;
 
 struct hostif_infrastructure_set_confirm_t {
 	struct hostif_hdr header;
-	u16 result_code;
+	__le16 result_code;
 } __packed;
 
 /**
@@ -434,13 +434,13 @@ struct hostif_infrastructure_set_confirm_t {
  */
 struct hostif_adhoc_set_request_t {
 	struct hostif_hdr header;
-	u16 phy_type;
-	u16 cts_mode;
-	u16 channel;
+	__le16 phy_type;
+	__le16 cts_mode;
+	__le16 channel;
 	struct rate_set16_t rate_set;
 	struct ssid_t ssid;
-	u16 capability;
-	u16 scan_type;
+	__le16 capability;
+	__le16 scan_type;
 } __packed;
 
 /**
@@ -452,20 +452,20 @@ struct hostif_adhoc_set_request_t {
  */
 struct hostif_adhoc_set2_request_t {
 	struct hostif_hdr header;
-	u16 phy_type;
-	u16 cts_mode;
-	u16 reserved;
+	__le16 phy_type;
+	__le16 cts_mode;
+	__le16 reserved;
 	struct rate_set16_t rate_set;
 	struct ssid_t ssid;
-	u16 capability;
-	u16 scan_type;
+	__le16 capability;
+	__le16 scan_type;
 	struct channel_list_t channel_list;
 	u8 bssid[ETH_ALEN];
 } __packed;
 
 struct hostif_adhoc_set_confirm_t {
 	struct hostif_hdr header;
-	u16 result_code;
+	__le16 result_code;
 } __packed;
 
 struct last_associate_t {
@@ -478,10 +478,10 @@ struct association_request_t {
 #define FRAME_TYPE_ASSOC_REQ	0x00
 #define FRAME_TYPE_REASSOC_REQ	0x20
 	u8 pad;
-	u16 capability;
-	u16 listen_interval;
+	__le16 capability;
+	__le16 listen_interval;
 	u8 ap_address[6];
-	u16 reqIEs_size;
+	__le16 req_ies_size;
 } __packed;
 
 struct association_response_t {
@@ -489,17 +489,17 @@ struct association_response_t {
 #define FRAME_TYPE_ASSOC_RESP	0x10
 #define FRAME_TYPE_REASSOC_RESP	0x30
 	u8 pad;
-	u16 capability;
-	u16 status;
-	u16 association_id;
-	u16 respIEs_size;
+	__le16 capability;
+	__le16 status;
+	__le16 association_id;
+	__le16 resp_ies_size;
 } __packed;
 
 struct hostif_associate_indication_t {
 	struct hostif_hdr header;
 	struct association_request_t assoc_req;
 	struct association_response_t assoc_resp;
-	/* followed by (reqIEs_size + respIEs_size) octets of data */
+	/* followed by (req_ies_size + resp_ies_size) octets of data */
 	/* reqIEs data *//* respIEs data */
 } __packed;
 
@@ -509,24 +509,24 @@ struct hostif_bss_scan_request_t {
 #define ACTIVE_SCAN  0
 #define PASSIVE_SCAN 1
 	u8 pad[3];
-	u32 ch_time_min;
-	u32 ch_time_max;
+	__le32 ch_time_min;
+	__le32 ch_time_max;
 	struct channel_list_t channel_list;
 	struct ssid_t ssid;
 } __packed;
 
 struct hostif_bss_scan_confirm_t {
 	struct hostif_hdr header;
-	u16 result_code;
-	u16 reserved;
+	__le16 result_code;
+	__le16 reserved;
 } __packed;
 
 struct hostif_phy_information_request_t {
 	struct hostif_hdr header;
-	u16 type;
+	__le16 type;
 #define NORMAL_TYPE	0
 #define TIME_TYPE	1
-	u16 time;	/* unit 100ms */
+	__le16 time;	/* unit 100ms */
 } __packed;
 
 struct hostif_phy_information_confirm_t {
@@ -535,10 +535,10 @@ struct hostif_phy_information_confirm_t {
 	u8 sq;
 	u8 noise;
 	u8 link_speed;
-	u32 tx_frame;
-	u32 rx_frame;
-	u32 tx_error;
-	u32 rx_error;
+	__le32 tx_frame;
+	__le32 rx_frame;
+	__le32 tx_error;
+	__le32 rx_error;
 } __packed;
 
 enum sleep_mode_type {
@@ -552,18 +552,18 @@ struct hostif_sleep_request_t {
 
 struct hostif_sleep_confirm_t {
 	struct hostif_hdr header;
-	u16 result_code;
+	__le16 result_code;
 } __packed;
 
 struct hostif_mic_failure_request_t {
 	struct hostif_hdr header;
-	u16 failure_count;
-	u16 timer;
+	__le16 failure_count;
+	__le16 timer;
 } __packed;
 
 struct hostif_mic_failure_confirm_t {
 	struct hostif_hdr header;
-	u16 result_code;
+	__le16 result_code;
 } __packed;
 
 #define BASIC_RATE	0x80
diff --git a/drivers/staging/ks7010/ks_wlan.h b/drivers/staging/ks7010/ks_wlan.h
index cd4f56ddbea8..3767079be00d 100644
--- a/drivers/staging/ks7010/ks_wlan.h
+++ b/drivers/staging/ks7010/ks_wlan.h
@@ -264,10 +264,10 @@ struct local_aplist_t {
 };
 
 struct local_gain_t {
-	u8 TxMode;
-	u8 RxMode;
-	u8 TxGain;
-	u8 RxGain;
+	u8 tx_mode;
+	u8 rx_mode;
+	u8 tx_gain;
+	u8 rx_gain;
 };
 
 struct local_eeprom_sum_t {
diff --git a/drivers/staging/ks7010/ks_wlan_net.c b/drivers/staging/ks7010/ks_wlan_net.c
index 5a43f193dcc8..8aa12e813bd7 100644
--- a/drivers/staging/ks7010/ks_wlan_net.c
+++ b/drivers/staging/ks7010/ks_wlan_net.c
@@ -2273,7 +2273,7 @@ static int ks_wlan_set_sleep_mode(struct net_device *dev,
 		netdev_info(dev, "SET_SLEEP_MODE %d\n", priv->sleep_mode);
 		hostif_sme_enqueue(priv, SME_SLEEP_REQUEST);
 	} else {
-		netdev_err(dev, "SET_SLEEP_MODE %d errror\n", *uwrq);
+		netdev_err(dev, "SET_SLEEP_MODE %d error\n", *uwrq);
 		return -EINVAL;
 	}
 
@@ -2378,14 +2378,14 @@ static int ks_wlan_set_tx_gain(struct net_device *dev,
 		return -EPERM;
 	/* for SLEEP MODE */
 	if (*uwrq >= 0 && *uwrq <= 0xFF)	/* 0-255 */
-		priv->gain.TxGain = (uint8_t)*uwrq;
+		priv->gain.tx_gain = (uint8_t)*uwrq;
 	else
 		return -EINVAL;
 
-	if (priv->gain.TxGain < 0xFF)
-		priv->gain.TxMode = 1;
+	if (priv->gain.tx_gain < 0xFF)
+		priv->gain.tx_mode = 1;
 	else
-		priv->gain.TxMode = 0;
+		priv->gain.tx_mode = 0;
 
 	hostif_sme_enqueue(priv, SME_SET_GAIN);
 	return 0;
@@ -2400,7 +2400,7 @@ static int ks_wlan_get_tx_gain(struct net_device *dev,
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
 	/* for SLEEP MODE */
-	*uwrq = priv->gain.TxGain;
+	*uwrq = priv->gain.tx_gain;
 	hostif_sme_enqueue(priv, SME_GET_GAIN);
 	return 0;
 }
@@ -2415,14 +2415,14 @@ static int ks_wlan_set_rx_gain(struct net_device *dev,
 		return -EPERM;
 	/* for SLEEP MODE */
 	if (*uwrq >= 0 && *uwrq <= 0xFF)	/* 0-255 */
-		priv->gain.RxGain = (uint8_t)*uwrq;
+		priv->gain.rx_gain = (uint8_t)*uwrq;
 	else
 		return -EINVAL;
 
-	if (priv->gain.RxGain < 0xFF)
-		priv->gain.RxMode = 1;
+	if (priv->gain.rx_gain < 0xFF)
+		priv->gain.rx_mode = 1;
 	else
-		priv->gain.RxMode = 0;
+		priv->gain.rx_mode = 0;
 
 	hostif_sme_enqueue(priv, SME_SET_GAIN);
 	return 0;
@@ -2437,7 +2437,7 @@ static int ks_wlan_get_rx_gain(struct net_device *dev,
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
 	/* for SLEEP MODE */
-	*uwrq = priv->gain.RxGain;
+	*uwrq = priv->gain.rx_gain;
 	hostif_sme_enqueue(priv, SME_GET_GAIN);
 	return 0;
 }
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
index 79321e4aaf30..0520f02f670d 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
@@ -2306,7 +2306,7 @@ static int kiblnd_dev_need_failover(struct kib_dev *dev)
 
 	memset(&srcaddr, 0, sizeof(srcaddr));
 	srcaddr.sin_family = AF_INET;
-	srcaddr.sin_addr.s_addr = (__force u32)htonl(dev->ibd_ifip);
+	srcaddr.sin_addr.s_addr = htonl(dev->ibd_ifip);
 
 	memset(&dstaddr, 0, sizeof(dstaddr));
 	dstaddr.sin_family = AF_INET;
@@ -2378,7 +2378,7 @@ int kiblnd_dev_failover(struct kib_dev *dev)
 
 	memset(&addr, 0, sizeof(addr));
 	addr.sin_family      = AF_INET;
-	addr.sin_addr.s_addr = (__force u32)htonl(dev->ibd_ifip);
+	addr.sin_addr.s_addr = htonl(dev->ibd_ifip);
 	addr.sin_port	= htons(*kiblnd_tunables.kib_service);
 
 	/* Bind to failover device or port */
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
index 0db662d6abdd..85b242ec5f9b 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
@@ -3267,7 +3267,7 @@ int
 kiblnd_connd(void *arg)
 {
 	spinlock_t *lock = &kiblnd_data.kib_connd_lock;
-	wait_queue_t wait;
+	wait_queue_entry_t wait;
 	unsigned long flags;
 	struct kib_conn *conn;
 	int timeout;
@@ -3521,7 +3521,7 @@ kiblnd_scheduler(void *arg)
 	long id = (long)arg;
 	struct kib_sched_info *sched;
 	struct kib_conn *conn;
-	wait_queue_t wait;
+	wait_queue_entry_t wait;
 	unsigned long flags;
 	struct ib_wc wc;
 	int did_something;
@@ -3656,7 +3656,7 @@ kiblnd_failover_thread(void *arg)
 {
 	rwlock_t *glock = &kiblnd_data.kib_global_lock;
 	struct kib_dev *dev;
-	wait_queue_t wait;
+	wait_queue_entry_t wait;
 	unsigned long flags;
 	int rc;
 
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
index 3ed3b08c122c..6b38d5a8fe92 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
@@ -2166,7 +2166,7 @@ ksocknal_connd(void *arg)
 {
 	spinlock_t *connd_lock = &ksocknal_data.ksnd_connd_lock;
 	struct ksock_connreq *cr;
-	wait_queue_t wait;
+	wait_queue_entry_t wait;
 	int nloops = 0;
 	int cons_retry = 0;
 
@@ -2554,7 +2554,7 @@ ksocknal_check_peer_timeouts(int idx)
 int
 ksocknal_reaper(void *arg)
 {
-	wait_queue_t wait;
+	wait_queue_entry_t wait;
 	struct ksock_conn *conn;
 	struct ksock_sched *sched;
 	struct list_head enomem_conns;
diff --git a/drivers/staging/lustre/lnet/libcfs/debug.c b/drivers/staging/lustre/lnet/libcfs/debug.c
index c56e9922cd5b..49deb448b044 100644
--- a/drivers/staging/lustre/lnet/libcfs/debug.c
+++ b/drivers/staging/lustre/lnet/libcfs/debug.c
@@ -361,7 +361,7 @@ static int libcfs_debug_dumplog_thread(void *arg)
 
 void libcfs_debug_dumplog(void)
 {
-	wait_queue_t wait;
+	wait_queue_entry_t wait;
 	struct task_struct *dumper;
 
 	/* we're being careful to ensure that the kernel thread is
diff --git a/drivers/staging/lustre/lnet/libcfs/linux/linux-tracefile.c b/drivers/staging/lustre/lnet/libcfs/linux/linux-tracefile.c
index 75eb84e7f0f8..a5a94788f11f 100644
--- a/drivers/staging/lustre/lnet/libcfs/linux/linux-tracefile.c
+++ b/drivers/staging/lustre/lnet/libcfs/linux/linux-tracefile.c
@@ -57,8 +57,9 @@ int cfs_tracefile_init_arch(void)
 	memset(cfs_trace_data, 0, sizeof(cfs_trace_data));
 	for (i = 0; i < CFS_TCD_TYPE_MAX; i++) {
 		cfs_trace_data[i] =
-			kmalloc(sizeof(union cfs_trace_data_union) *
-				num_possible_cpus(), GFP_KERNEL);
+			kmalloc_array(num_possible_cpus(),
+				      sizeof(union cfs_trace_data_union),
+				      GFP_KERNEL);
 		if (!cfs_trace_data[i])
 			goto out;
 	}
diff --git a/drivers/staging/lustre/lnet/libcfs/tracefile.c b/drivers/staging/lustre/lnet/libcfs/tracefile.c
index 9599b7441feb..d1aa79bb2017 100644
--- a/drivers/staging/lustre/lnet/libcfs/tracefile.c
+++ b/drivers/staging/lustre/lnet/libcfs/tracefile.c
@@ -191,10 +191,9 @@ cfs_trace_get_tage_try(struct cfs_trace_cpu_data *tcd, unsigned long len)
 		} else {
 			tage = cfs_tage_alloc(GFP_ATOMIC);
 			if (unlikely(!tage)) {
-				if ((!memory_pressure_get() ||
-				     in_interrupt()) && printk_ratelimit())
-					pr_warn("cannot allocate a tage (%ld)\n",
-						tcd->tcd_cur_pages);
+				if (!memory_pressure_get() || in_interrupt())
+					pr_warn_ratelimited("cannot allocate a tage (%ld)\n",
+							    tcd->tcd_cur_pages);
 				return NULL;
 			}
 		}
@@ -229,9 +228,8 @@ static void cfs_tcd_shrink(struct cfs_trace_cpu_data *tcd)
 	 * from here: this will lead to infinite recursion.
 	 */
 
-	if (printk_ratelimit())
-		pr_warn("debug daemon buffer overflowed; discarding 10%% of pages (%d of %ld)\n",
-			pgcount + 1, tcd->tcd_cur_pages);
+	pr_warn_ratelimited("debug daemon buffer overflowed; discarding 10%% of pages (%d of %ld)\n",
+			    pgcount + 1, tcd->tcd_cur_pages);
 
 	INIT_LIST_HEAD(&pc.pc_pages);
 
@@ -990,7 +988,7 @@ static int tracefiled(void *arg)
 	complete(&tctl->tctl_start);
 
 	while (1) {
-		wait_queue_t __wait;
+		wait_queue_entry_t __wait;
 
 		pc.pc_want_daemon_pages = 0;
 		collect_pages(&pc);
diff --git a/drivers/staging/lustre/lnet/lnet/lib-eq.c b/drivers/staging/lustre/lnet/lnet/lib-eq.c
index ce4b83584e17..9ebba4ef5f90 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-eq.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-eq.c
@@ -312,7 +312,7 @@ __must_hold(&the_lnet.ln_eq_wait_lock)
 {
 	int tms = *timeout_ms;
 	int wait;
-	wait_queue_t wl;
+	wait_queue_entry_t wl;
 	unsigned long now;
 
 	if (!tms)
diff --git a/drivers/staging/lustre/lnet/lnet/lib-move.c b/drivers/staging/lustre/lnet/lnet/lib-move.c
index a99c5c0aa672..20ebe247071f 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-move.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-move.c
@@ -1274,9 +1274,9 @@ lnet_parse_put(struct lnet_ni *ni, struct lnet_msg *msg)
 	int rc;
 
 	/* Convert put fields to host byte order */
-	hdr->msg.put.match_bits	= le64_to_cpu(hdr->msg.put.match_bits);
-	hdr->msg.put.ptl_index	= le32_to_cpu(hdr->msg.put.ptl_index);
-	hdr->msg.put.offset	= le32_to_cpu(hdr->msg.put.offset);
+	le64_to_cpus(&hdr->msg.put.match_bits);
+	le32_to_cpus(&hdr->msg.put.ptl_index);
+	le32_to_cpus(&hdr->msg.put.offset);
 
 	info.mi_id.nid	= hdr->src_nid;
 	info.mi_id.pid	= hdr->src_pid;
@@ -1332,10 +1332,10 @@ lnet_parse_get(struct lnet_ni *ni, struct lnet_msg *msg, int rdma_get)
 	int rc;
 
 	/* Convert get fields to host byte order */
-	hdr->msg.get.match_bits  = le64_to_cpu(hdr->msg.get.match_bits);
-	hdr->msg.get.ptl_index   = le32_to_cpu(hdr->msg.get.ptl_index);
-	hdr->msg.get.sink_length = le32_to_cpu(hdr->msg.get.sink_length);
-	hdr->msg.get.src_offset  = le32_to_cpu(hdr->msg.get.src_offset);
+	le64_to_cpus(&hdr->msg.get.match_bits);
+	le32_to_cpus(&hdr->msg.get.ptl_index);
+	le32_to_cpus(&hdr->msg.get.sink_length);
+	le32_to_cpus(&hdr->msg.get.src_offset);
 
 	info.mi_id.nid  = hdr->src_nid;
 	info.mi_id.pid  = hdr->src_pid;
@@ -1464,8 +1464,8 @@ lnet_parse_ack(struct lnet_ni *ni, struct lnet_msg *msg)
 	src.pid = hdr->src_pid;
 
 	/* Convert ack fields to host byte order */
-	hdr->msg.ack.match_bits = le64_to_cpu(hdr->msg.ack.match_bits);
-	hdr->msg.ack.mlength = le32_to_cpu(hdr->msg.ack.mlength);
+	le64_to_cpus(&hdr->msg.ack.match_bits);
+	le32_to_cpus(&hdr->msg.ack.mlength);
 
 	cpt = lnet_cpt_of_cookie(hdr->msg.ack.dst_wmd.wh_object_cookie);
 	lnet_res_lock(cpt);
@@ -1798,7 +1798,7 @@ lnet_parse(struct lnet_ni *ni, struct lnet_hdr *hdr, lnet_nid_t from_nid,
 		/* convert common msg->hdr fields to host byteorder */
 		msg->msg_hdr.type	= type;
 		msg->msg_hdr.src_nid	= src_nid;
-		msg->msg_hdr.src_pid	= le32_to_cpu(msg->msg_hdr.src_pid);
+		le32_to_cpus(&msg->msg_hdr.src_pid);
 		msg->msg_hdr.dest_nid	= dest_nid;
 		msg->msg_hdr.dest_pid	= dest_pid;
 		msg->msg_hdr.payload_length = payload_length;
diff --git a/drivers/staging/lustre/lnet/lnet/lib-socket.c b/drivers/staging/lustre/lnet/lnet/lib-socket.c
index 9fca8d225ee0..800f4f6c6593 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-socket.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-socket.c
@@ -89,7 +89,7 @@ lnet_ipif_query(char *name, int *up, __u32 *ip, __u32 *mask)
 	struct ifreq ifr;
 	int nob;
 	int rc;
-	__u32 val;
+	__be32 val;
 
 	nob = strnlen(name, IFNAMSIZ);
 	if (nob == IFNAMSIZ) {
@@ -516,7 +516,7 @@ lnet_sock_listen(struct socket **sockp, __u32 local_ip, int local_port,
 int
 lnet_sock_accept(struct socket **newsockp, struct socket *sock)
 {
-	wait_queue_t wait;
+	wait_queue_entry_t wait;
 	struct socket *newsock;
 	int rc;
 
diff --git a/drivers/staging/lustre/lustre/fid/fid_request.c b/drivers/staging/lustre/lustre/fid/fid_request.c
index 999f250ceed0..19895faf7626 100644
--- a/drivers/staging/lustre/lustre/fid/fid_request.c
+++ b/drivers/staging/lustre/lustre/fid/fid_request.c
@@ -192,7 +192,7 @@ static int seq_client_alloc_seq(const struct lu_env *env,
 }
 
 static int seq_fid_alloc_prep(struct lu_client_seq *seq,
-			      wait_queue_t *link)
+			      wait_queue_entry_t *link)
 {
 	if (seq->lcs_update) {
 		add_wait_queue(&seq->lcs_waitq, link);
@@ -223,7 +223,7 @@ static void seq_fid_alloc_fini(struct lu_client_seq *seq)
 int seq_client_alloc_fid(const struct lu_env *env,
 			 struct lu_client_seq *seq, struct lu_fid *fid)
 {
-	wait_queue_t link;
+	wait_queue_entry_t link;
 	int rc;
 
 	LASSERT(seq);
@@ -259,7 +259,7 @@ int seq_client_alloc_fid(const struct lu_env *env,
 			return rc;
 		}
 
-		CDEBUG(D_INFO, "%s: Switch to sequence [0x%16.16Lx]\n",
+		CDEBUG(D_INFO, "%s: Switch to sequence [0x%16.16llx]\n",
 		       seq->lcs_name, seqnr);
 
 		seq->lcs_fid.f_oid = LUSTRE_FID_INIT_OID;
@@ -279,7 +279,7 @@ int seq_client_alloc_fid(const struct lu_env *env,
 	*fid = seq->lcs_fid;
 	mutex_unlock(&seq->lcs_mutex);
 
-	CDEBUG(D_INFO, "%s: Allocated FID "DFID"\n", seq->lcs_name,  PFID(fid));
+	CDEBUG(D_INFO, "%s: Allocated FID " DFID "\n", seq->lcs_name,  PFID(fid));
 	return rc;
 }
 EXPORT_SYMBOL(seq_client_alloc_fid);
@@ -290,7 +290,7 @@ EXPORT_SYMBOL(seq_client_alloc_fid);
  */
 void seq_client_flush(struct lu_client_seq *seq)
 {
-	wait_queue_t link;
+	wait_queue_entry_t link;
 
 	LASSERT(seq);
 	init_waitqueue_entry(&link, current);
diff --git a/drivers/staging/lustre/lustre/fld/fld_cache.c b/drivers/staging/lustre/lustre/fld/fld_cache.c
index 11f697496180..b852fed0b10f 100644
--- a/drivers/staging/lustre/lustre/fld/fld_cache.c
+++ b/drivers/staging/lustre/lustre/fld/fld_cache.c
@@ -151,7 +151,7 @@ restart_fixup:
 			continue;
 
 		LASSERTF(c_range->lsr_start <= n_range->lsr_start,
-			 "cur lsr_start "DRANGE" next lsr_start "DRANGE"\n",
+			 "cur lsr_start " DRANGE " next lsr_start " DRANGE "\n",
 			 PRANGE(c_range), PRANGE(n_range));
 
 		/* check merge possibility with next range */
@@ -349,7 +349,7 @@ static void fld_cache_overlap_handle(struct fld_cache *cache,
 		f_curr->fce_range.lsr_end = new_start;
 		fld_cache_entry_add(cache, f_new, &f_curr->fce_list);
 	} else
-		CERROR("NEW range ="DRANGE" curr = "DRANGE"\n",
+		CERROR("NEW range =" DRANGE " curr = " DRANGE "\n",
 		       PRANGE(range), PRANGE(&f_curr->fce_range));
 }
 
@@ -415,7 +415,7 @@ static int fld_cache_insert_nolock(struct fld_cache *cache,
 	if (!prev)
 		prev = head;
 
-	CDEBUG(D_INFO, "insert range "DRANGE"\n", PRANGE(&f_new->fce_range));
+	CDEBUG(D_INFO, "insert range " DRANGE "\n", PRANGE(&f_new->fce_range));
 	/* Add new entry to cache and lru list. */
 	fld_cache_entry_add(cache, f_new, prev);
 out:
diff --git a/drivers/staging/lustre/lustre/fld/lproc_fld.c b/drivers/staging/lustre/lustre/fld/lproc_fld.c
index 61ac420798af..b83d7ebb2d18 100644
--- a/drivers/staging/lustre/lustre/fld/lproc_fld.c
+++ b/drivers/staging/lustre/lustre/fld/lproc_fld.c
@@ -136,7 +136,7 @@ fld_debugfs_cache_flush_release(struct inode *inode, struct file *file)
 	return 0;
 }
 
-static struct file_operations fld_debugfs_cache_flush_fops = {
+static const struct file_operations fld_debugfs_cache_flush_fops = {
 	.owner		= THIS_MODULE,
 	.open           = simple_open,
 	.write		= fld_debugfs_cache_flush_write,
diff --git a/drivers/staging/lustre/lustre/include/cl_object.h b/drivers/staging/lustre/lustre/include/cl_object.h
index 2bc3ee51b069..90a0c501e1ea 100644
--- a/drivers/staging/lustre/lustre/include/cl_object.h
+++ b/drivers/staging/lustre/lustre/include/cl_object.h
@@ -1287,7 +1287,7 @@ do {								    \
  * @{
  */
 struct cl_page_list {
-	unsigned	     pl_nr;
+	unsigned int		 pl_nr;
 	struct list_head	   pl_pages;
 	struct task_struct	*pl_owner;
 };
@@ -1842,7 +1842,7 @@ struct cl_io {
 	/**
 	 * Number of pages owned by this IO. For invariant checking.
 	 */
-	unsigned	     ci_owned_nr;
+	unsigned int	     ci_owned_nr;
 };
 
 /** @} cl_io */
diff --git a/drivers/staging/lustre/lustre/include/lprocfs_status.h b/drivers/staging/lustre/lustre/include/lprocfs_status.h
index 242abb881766..915283c04094 100644
--- a/drivers/staging/lustre/lustre/include/lprocfs_status.h
+++ b/drivers/staging/lustre/lustre/include/lprocfs_status.h
@@ -49,7 +49,7 @@
 
 struct lprocfs_vars {
 	const char		*name;
-	struct file_operations	*fops;
+	const struct file_operations	*fops;
 	void			*data;
 	/**
 	 * sysfs file mode.
@@ -449,7 +449,7 @@ int lprocfs_exp_cleanup(struct obd_export *exp);
 struct dentry *ldebugfs_add_simple(struct dentry *root,
 				   char *name,
 				   void *data,
-				   struct file_operations *fops);
+				   const struct file_operations *fops);
 
 int ldebugfs_register_stats(struct dentry *parent,
 			    const char *name,
@@ -523,8 +523,8 @@ unsigned long lprocfs_oh_sum(struct obd_histogram *oh);
 void lprocfs_stats_collect(struct lprocfs_stats *stats, int idx,
 			   struct lprocfs_counter *cnt);
 
-int lprocfs_single_release(struct inode *, struct file *);
-int lprocfs_seq_release(struct inode *, struct file *);
+int lprocfs_single_release(struct inode *inode, struct file *file);
+int lprocfs_seq_release(struct inode *inode, struct file *file);
 
 /* write the name##_seq_show function, call LPROC_SEQ_FOPS_RO for read-only
  * proc entries; otherwise, you will define name##_seq_write function also for
@@ -536,7 +536,7 @@ static int name##_single_open(struct inode *inode, struct file *file)	\
 {									\
 	return single_open(file, name##_seq_show, inode->i_private);	\
 }									\
-static struct file_operations name##_fops = {				\
+static const struct file_operations name##_fops = {			\
 	.owner   = THIS_MODULE,					    \
 	.open    = name##_single_open,				     \
 	.read    = seq_read,					       \
@@ -581,7 +581,7 @@ static struct file_operations name##_fops = {				\
 	{								\
 		return single_open(file, NULL, inode->i_private);	\
 	}								\
-	static struct file_operations name##_##type##_fops = {	\
+	static const struct file_operations name##_##type##_fops = {	\
 		.open	= name##_##type##_open,				\
 		.write	= name##_##type##_write,			\
 		.release = lprocfs_single_release,			\
diff --git a/drivers/staging/lustre/lustre/include/lu_object.h b/drivers/staging/lustre/lustre/include/lu_object.h
index 73ecc232967b..2e70602dc2e2 100644
--- a/drivers/staging/lustre/lustre/include/lu_object.h
+++ b/drivers/staging/lustre/lustre/include/lu_object.h
@@ -968,11 +968,11 @@ struct lu_context {
 	 * Version counter used to skip calls to lu_context_refill() when no
 	 * keys were registered.
 	 */
-	unsigned	       lc_version;
+	unsigned int		lc_version;
 	/**
 	 * Debugging cookie.
 	 */
-	unsigned	       lc_cookie;
+	unsigned int		lc_cookie;
 };
 
 /**
diff --git a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
index df48b8d6fe52..77995fa47691 100644
--- a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
+++ b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
@@ -546,7 +546,7 @@ static inline void ostid_set_id(struct ost_id *oi, __u64 oid)
 static inline int fid_set_id(struct lu_fid *fid, __u64 oid)
 {
 	if (unlikely(fid_seq_is_igif(fid->f_seq))) {
-		CERROR("bad IGIF, "DFID"\n", PFID(fid));
+		CERROR("bad IGIF, " DFID "\n", PFID(fid));
 		return -EBADF;
 	}
 
@@ -585,7 +585,7 @@ static inline int ostid_to_fid(struct lu_fid *fid, struct ost_id *ostid,
 	__u64 seq = ostid_seq(ostid);
 
 	if (ost_idx > 0xffff) {
-		CERROR("bad ost_idx, "DOSTID" ost_idx:%u\n", POSTID(ostid),
+		CERROR("bad ost_idx, " DOSTID " ost_idx:%u\n", POSTID(ostid),
 		       ost_idx);
 		return -EBADF;
 	}
@@ -630,7 +630,7 @@ static inline int ostid_to_fid(struct lu_fid *fid, struct ost_id *ostid,
 static inline int fid_to_ostid(const struct lu_fid *fid, struct ost_id *ostid)
 {
 	if (unlikely(fid_seq_is_igif(fid->f_seq))) {
-		CERROR("bad IGIF, "DFID"\n", PFID(fid));
+		CERROR("bad IGIF, " DFID "\n", PFID(fid));
 		return -EBADF;
 	}
 
diff --git a/drivers/staging/lustre/lustre/include/lustre/lustre_user.h b/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
index 7d8628ce0d3b..edff8dc34430 100644
--- a/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
+++ b/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
@@ -532,7 +532,7 @@ static inline void obd_uuid2fsname(char *buf, char *uuid, int buflen)
 #define FID_NOBRACE_LEN 40
 #define FID_LEN (FID_NOBRACE_LEN + 2)
 #define DFID_NOBRACE "%#llx:0x%x:0x%x"
-#define DFID "["DFID_NOBRACE"]"
+#define DFID "[" DFID_NOBRACE "]"
 #define PFID(fid) (unsigned long long)(fid)->f_seq, (fid)->f_oid, (fid)->f_ver
 
 /* scanf input parse format for fids in DFID_NOBRACE format
diff --git a/drivers/staging/lustre/lustre/include/lustre_fid.h b/drivers/staging/lustre/lustre/include/lustre_fid.h
index b5a1aadbcb93..6dc24a76ddb6 100644
--- a/drivers/staging/lustre/lustre/include/lustre_fid.h
+++ b/drivers/staging/lustre/lustre/include/lustre_fid.h
@@ -606,7 +606,7 @@ static inline __u32 fid_flatten32(const struct lu_fid *fid)
 static inline int lu_fid_diff(const struct lu_fid *fid1,
 			      const struct lu_fid *fid2)
 {
-	LASSERTF(fid_seq(fid1) == fid_seq(fid2), "fid1:"DFID", fid2:"DFID"\n",
+	LASSERTF(fid_seq(fid1) == fid_seq(fid2), "fid1:" DFID ", fid2:" DFID "\n",
 		 PFID(fid1), PFID(fid2));
 
 	if (fid_is_idif(fid1) && fid_is_idif(fid2))
diff --git a/drivers/staging/lustre/lustre/include/lustre_lib.h b/drivers/staging/lustre/lustre/include/lustre_lib.h
index b04d613846ee..f24970da8323 100644
--- a/drivers/staging/lustre/lustre/include/lustre_lib.h
+++ b/drivers/staging/lustre/lustre/include/lustre_lib.h
@@ -201,7 +201,7 @@ struct l_wait_info {
 			   sigmask(SIGALRM))
 
 /**
- * wait_queue_t of Linux (version < 2.6.34) is a FIFO list for exclusively
+ * wait_queue_entry_t of Linux (version < 2.6.34) is a FIFO list for exclusively
  * waiting threads, which is not always desirable because all threads will
  * be waken up again and again, even user only needs a few of them to be
  * active most time. This is not good for performance because cache can
@@ -228,7 +228,7 @@ struct l_wait_info {
  */
 #define __l_wait_event(wq, condition, info, ret, l_add_wait)		   \
 do {									   \
-	wait_queue_t __wait;						 \
+	wait_queue_entry_t __wait;						 \
 	long __timeout = info->lwi_timeout;			  \
 	sigset_t   __blocked;					      \
 	int   __allow_intr = info->lwi_allow_intr;			     \
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
index 5d24b4825796..ec3b23cd09ec 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
@@ -79,11 +79,11 @@ static inline int ldlm_ns_empty(struct ldlm_namespace *ns)
 	return atomic_read(&ns->ns_bref) == 0;
 }
 
-void ldlm_namespace_move_to_active_locked(struct ldlm_namespace *,
-					  enum ldlm_side);
-void ldlm_namespace_move_to_inactive_locked(struct ldlm_namespace *,
-					    enum ldlm_side);
-struct ldlm_namespace *ldlm_namespace_first_locked(enum ldlm_side);
+void ldlm_namespace_move_to_active_locked(struct ldlm_namespace *ns,
+					  enum ldlm_side client);
+void ldlm_namespace_move_to_inactive_locked(struct ldlm_namespace *ns,
+					    enum ldlm_side client);
+struct ldlm_namespace *ldlm_namespace_first_locked(enum ldlm_side client);
 
 /* ldlm_request.c */
 /* Cancel lru flag, it indicates we cancel aged locks. */
@@ -130,16 +130,19 @@ void ldlm_grant_lock(struct ldlm_lock *lock, struct list_head *work_list);
 int ldlm_fill_lvb(struct ldlm_lock *lock, struct req_capsule *pill,
 		  enum req_location loc, void *data, int size);
 struct ldlm_lock *
-ldlm_lock_create(struct ldlm_namespace *ns, const struct ldlm_res_id *,
+ldlm_lock_create(struct ldlm_namespace *ns, const struct ldlm_res_id *id,
 		 enum ldlm_type type, enum ldlm_mode mode,
 		 const struct ldlm_callback_suite *cbs,
 		 void *data, __u32 lvb_len, enum lvb_type lvb_type);
-enum ldlm_error ldlm_lock_enqueue(struct ldlm_namespace *, struct ldlm_lock **,
-				  void *cookie, __u64 *flags);
-void ldlm_lock_addref_internal(struct ldlm_lock *, enum ldlm_mode mode);
-void ldlm_lock_addref_internal_nolock(struct ldlm_lock *, enum ldlm_mode mode);
-void ldlm_lock_decref_internal(struct ldlm_lock *, enum ldlm_mode mode);
-void ldlm_lock_decref_internal_nolock(struct ldlm_lock *, enum ldlm_mode mode);
+enum ldlm_error ldlm_lock_enqueue(struct ldlm_namespace *ns,
+				  struct ldlm_lock **lock, void *cookie,
+				  __u64 *flags);
+void ldlm_lock_addref_internal(struct ldlm_lock *lock, enum ldlm_mode mode);
+void ldlm_lock_addref_internal_nolock(struct ldlm_lock *lock,
+				      enum ldlm_mode mode);
+void ldlm_lock_decref_internal(struct ldlm_lock *lock, enum ldlm_mode mode);
+void ldlm_lock_decref_internal_nolock(struct ldlm_lock *lock,
+				      enum ldlm_mode mode);
 int ldlm_run_ast_work(struct ldlm_namespace *ns, struct list_head *rpc_list,
 		      enum ldlm_desc_ast_t ast_type);
 int ldlm_lock_remove_from_lru_check(struct ldlm_lock *lock, time_t last_use);
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
index 3663c5cdb051..4dc7baee1f28 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
@@ -363,17 +363,16 @@ int client_obd_setup(struct obd_device *obddev, struct lustre_cfg *lcfg)
 	 */
 	cli->cl_chunkbits = PAGE_SHIFT;
 
-	if (!strcmp(name, LUSTRE_MDC_NAME)) {
+	if (!strcmp(name, LUSTRE_MDC_NAME))
 		cli->cl_max_rpcs_in_flight = OBD_MAX_RIF_DEFAULT;
-	} else if (totalram_pages >> (20 - PAGE_SHIFT) <= 128 /* MB */) {
+	else if (totalram_pages >> (20 - PAGE_SHIFT) <= 128 /* MB */)
 		cli->cl_max_rpcs_in_flight = 2;
-	} else if (totalram_pages >> (20 - PAGE_SHIFT) <= 256 /* MB */) {
+	else if (totalram_pages >> (20 - PAGE_SHIFT) <= 256 /* MB */)
 		cli->cl_max_rpcs_in_flight = 3;
-	} else if (totalram_pages >> (20 - PAGE_SHIFT) <= 512 /* MB */) {
+	else if (totalram_pages >> (20 - PAGE_SHIFT) <= 512 /* MB */)
 		cli->cl_max_rpcs_in_flight = 4;
-	} else {
+	else
 		cli->cl_max_rpcs_in_flight = OBD_MAX_RIF_DEFAULT;
-	}
 
 	spin_lock_init(&cli->cl_mod_rpcs_lock);
 	spin_lock_init(&cli->cl_mod_rpcs_hist.oh_lock);
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
index 84eeaa552113..4028e11249a2 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
@@ -445,8 +445,8 @@ int ldlm_cli_enqueue_fini(struct obd_export *exp, struct ptlrpc_request *req,
 
 		if (!ldlm_res_eq(&reply->lock_desc.l_resource.lr_name,
 				 &lock->l_resource->lr_name)) {
-			CDEBUG(D_INFO, "remote intent success, locking "DLDLMRES
-				       " instead of "DLDLMRES"\n",
+			CDEBUG(D_INFO, "remote intent success, locking " DLDLMRES
+				       " instead of " DLDLMRES "\n",
 			       PLDLMRES(&reply->lock_desc.l_resource),
 			       PLDLMRES(lock->l_resource));
 
@@ -1677,7 +1677,7 @@ int ldlm_cli_cancel_unused_resource(struct ldlm_namespace *ns,
 					   0, flags | LCF_BL_AST, opaque);
 	rc = ldlm_cli_cancel_list(&cancels, count, NULL, flags);
 	if (rc != ELDLM_OK)
-		CERROR("canceling unused lock "DLDLMRES": rc = %d\n",
+		CERROR("canceling unused lock " DLDLMRES ": rc = %d\n",
 		       PLDLMRES(res), rc);
 
 	LDLM_RESOURCE_DELREF(res);
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
index 633f65b078eb..c9ef247d9be4 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
@@ -78,7 +78,25 @@ lprocfs_wr_dump_ns(struct file *file, const char __user *buffer,
 
 LPROC_SEQ_FOPS_WR_ONLY(ldlm, dump_ns);
 
-LPROC_SEQ_FOPS_RW_TYPE(ldlm_rw, uint);
+static int ldlm_rw_uint_seq_show(struct seq_file *m, void *v)
+{
+	seq_printf(m, "%u\n", *(unsigned int *)m->private);
+	return 0;
+}
+
+static ssize_t
+ldlm_rw_uint_seq_write(struct file *file, const char __user *buffer,
+		       size_t count, loff_t *off)
+{
+	struct seq_file *seq = file->private_data;
+
+	if (count == 0)
+		return 0;
+	return kstrtouint_from_user(buffer, count, 0,
+				    (unsigned int *)seq->private);
+}
+
+LPROC_SEQ_FOPS(ldlm_rw_uint);
 
 static struct lprocfs_vars ldlm_debugfs_list[] = {
 	{ "dump_namespaces", &ldlm_dump_ns_fops, NULL, 0222 },
@@ -831,7 +849,7 @@ static int ldlm_resource_complain(struct cfs_hash *hs, struct cfs_hash_bd *bd,
 	struct ldlm_resource  *res = cfs_hash_object(hs, hnode);
 
 	lock_res(res);
-	CERROR("%s: namespace resource "DLDLMRES
+	CERROR("%s: namespace resource " DLDLMRES
 	       " (%p) refcount nonzero (%d) after lock cleanup; forcing cleanup.\n",
 	       ldlm_ns_name(ldlm_res_to_ns(res)), PLDLMRES(res), res,
 	       atomic_read(&res->lr_refcount) - 1);
@@ -1373,7 +1391,7 @@ void ldlm_resource_dump(int level, struct ldlm_resource *res)
 	if (!((libcfs_debug | D_ERROR) & level))
 		return;
 
-	CDEBUG(level, "--- Resource: "DLDLMRES" (%p) refcount = %d\n",
+	CDEBUG(level, "--- Resource: " DLDLMRES " (%p) refcount = %d\n",
 	       PLDLMRES(res), res, atomic_read(&res->lr_refcount));
 
 	if (!list_empty(&res->lr_granted)) {
diff --git a/drivers/staging/lustre/lustre/llite/dcache.c b/drivers/staging/lustre/lustre/llite/dcache.c
index 38f84662cf02..d20425fb8cbe 100644
--- a/drivers/staging/lustre/lustre/llite/dcache.c
+++ b/drivers/staging/lustre/lustre/llite/dcache.c
@@ -180,7 +180,7 @@ void ll_invalidate_aliases(struct inode *inode)
 {
 	struct dentry *dentry;
 
-	CDEBUG(D_INODE, "marking dentries for ino "DFID"(%p) invalid\n",
+	CDEBUG(D_INODE, "marking dentries for ino " DFID "(%p) invalid\n",
 	       PFID(ll_inode2fid(inode)), inode);
 
 	spin_lock(&inode->i_lock);
@@ -216,7 +216,7 @@ void ll_lookup_finish_locks(struct lookup_intent *it, struct inode *inode)
 	if (it->it_lock_mode && inode) {
 		struct ll_sb_info *sbi = ll_i2sbi(inode);
 
-		CDEBUG(D_DLMTRACE, "setting l_data to inode "DFID"(%p)\n",
+		CDEBUG(D_DLMTRACE, "setting l_data to inode " DFID "(%p)\n",
 		       PFID(ll_inode2fid(inode)), inode);
 		ll_set_lock_data(sbi->ll_md_exp, inode, it, NULL);
 	}
diff --git a/drivers/staging/lustre/lustre/llite/dir.c b/drivers/staging/lustre/lustre/llite/dir.c
index 13b35922a4ca..03a72c07f57c 100644
--- a/drivers/staging/lustre/lustre/llite/dir.c
+++ b/drivers/staging/lustre/lustre/llite/dir.c
@@ -303,7 +303,7 @@ static int ll_readdir(struct file *filp, struct dir_context *ctx)
 	struct md_op_data *op_data;
 	int			rc;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p) pos/size %lu/%llu 32bit_api %d\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p) pos/size %lu/%llu 32bit_api %d\n",
 	       PFID(ll_inode2fid(inode)), inode, (unsigned long)pos,
 	       i_size_read(inode), api32);
 
@@ -419,7 +419,7 @@ static int ll_dir_setdirstripe(struct inode *parent, struct lmv_user_md *lump,
 	if (unlikely(lump->lum_magic != LMV_USER_MAGIC))
 		return -EINVAL;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p) name %s stripe_offset %d, stripe_count: %u\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p) name %s stripe_offset %d, stripe_count: %u\n",
 	       PFID(ll_inode2fid(parent)), parent, dirname,
 	       (int)lump->lum_stripe_offset, lump->lum_stripe_count);
 
@@ -606,7 +606,7 @@ int ll_dir_getstripe(struct inode *inode, void **plmm, int *plmm_size,
 	rc = md_getattr(sbi->ll_md_exp, op_data, &req);
 	ll_finish_md_op_data(op_data);
 	if (rc < 0) {
-		CDEBUG(D_INFO, "md_getattr failed on inode "DFID": rc %d\n",
+		CDEBUG(D_INFO, "md_getattr failed on inode " DFID ": rc %d\n",
 		       PFID(ll_inode2fid(inode)), rc);
 		goto out;
 	}
@@ -733,7 +733,7 @@ static int ll_ioc_copy_start(struct super_block *sb, struct hsm_copy *copy)
 		iput(inode);
 		if (rc != 0) {
 			CDEBUG(D_HSM, "Could not read file data version of "
-				      DFID" (rc = %d). Archive request (%#llx) could not be done.\n",
+				      DFID " (rc = %d). Archive request (%#llx) could not be done.\n",
 				      PFID(&copy->hc_hai.hai_fid), rc,
 				      copy->hc_hai.hai_cookie);
 			hpk.hpk_flags |= HP_FLAG_RETRY;
@@ -833,7 +833,7 @@ static int ll_ioc_copy_end(struct super_block *sb, struct hsm_copy *copy)
 		if ((copy->hc_hai.hai_action == HSMA_ARCHIVE) &&
 		    (copy->hc_data_version != data_version)) {
 			CDEBUG(D_HSM, "File data version mismatched. File content was changed during archiving. "
-			       DFID", start:%#llx current:%#llx\n",
+			       DFID ", start:%#llx current:%#llx\n",
 			       PFID(&copy->hc_hai.hai_fid),
 			       copy->hc_data_version, data_version);
 			/* File was changed, send error to cdt. Do not ask for
@@ -1037,7 +1037,7 @@ static long ll_dir_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	struct obd_ioctl_data *data;
 	int rc = 0;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p), cmd=%#x\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p), cmd=%#x\n",
 	       PFID(ll_inode2fid(inode)), inode, cmd);
 
 	/* asm-ppc{,64} declares TCGETS, et. al. as type 't' not 'T' */
@@ -1141,7 +1141,7 @@ out_free:
 		}
 
 #if OBD_OCD_VERSION(2, 9, 50, 0) > LUSTRE_VERSION_CODE
-		mode = data->ioc_type != 0 ? data->ioc_type : S_IRWXUGO;
+		mode = data->ioc_type != 0 ? data->ioc_type : 0777;
 #else
 		mode = data->ioc_type;
 #endif
diff --git a/drivers/staging/lustre/lustre/llite/file.c b/drivers/staging/lustre/lustre/llite/file.c
index 67c4b9cc6e75..ab1c85c1ed38 100644
--- a/drivers/staging/lustre/lustre/llite/file.c
+++ b/drivers/staging/lustre/lustre/llite/file.c
@@ -316,7 +316,7 @@ int ll_file_release(struct inode *inode, struct file *file)
 	struct ll_inode_info *lli = ll_i2info(inode);
 	int rc;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p)\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p)\n",
 	       PFID(ll_inode2fid(inode)), inode);
 
 	if (!is_root_inode(inode))
@@ -494,7 +494,7 @@ int ll_file_open(struct inode *inode, struct file *file)
 	struct ll_file_data *fd;
 	int rc = 0;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p), flags %o\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p), flags %o\n",
 	       PFID(ll_inode2fid(inode)), inode, file->f_flags);
 
 	it = file->private_data; /* XXX: compat macro */
@@ -834,7 +834,7 @@ out_close:
 	}
 	rc2 = ll_close_inode_openhandle(inode, och, 0, NULL);
 	if (rc2 < 0)
-		CERROR("%s: error closing file "DFID": %d\n",
+		CERROR("%s: error closing file " DFID ": %d\n",
 		       ll_get_fsname(inode->i_sb, NULL, 0),
 		       PFID(&ll_i2info(inode)->lli_fid), rc2);
 	och = NULL; /* och has been freed in ll_close_inode_openhandle() */
@@ -1665,7 +1665,7 @@ int ll_hsm_release(struct inode *inode)
 	int rc;
 	u16 refcheck;
 
-	CDEBUG(D_INODE, "%s: Releasing file "DFID".\n",
+	CDEBUG(D_INODE, "%s: Releasing file " DFID ".\n",
 	       ll_get_fsname(inode->i_sb, NULL, 0),
 	       PFID(&ll_i2info(inode)->lli_fid));
 
@@ -1928,7 +1928,7 @@ ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	struct ll_file_data	*fd = LUSTRE_FPRIVATE(file);
 	int			 flags, rc;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p),cmd=%x\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p),cmd=%x\n",
 	       PFID(ll_inode2fid(inode)), inode, cmd);
 	ll_stats_ops_tally(ll_i2sbi(inode), LPROC_LL_IOCTL, 1);
 
@@ -2263,7 +2263,7 @@ static loff_t ll_file_seek(struct file *file, loff_t offset, int origin)
 
 	retval = offset + ((origin == SEEK_END) ? i_size_read(inode) :
 			   (origin == SEEK_CUR) ? file->f_pos : 0);
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p), to=%llu=%#llx(%d)\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p), to=%llu=%#llx(%d)\n",
 	       PFID(ll_inode2fid(inode)), inode, retval, retval, origin);
 	ll_stats_ops_tally(ll_i2sbi(inode), LPROC_LL_LLSEEK, 1);
 
@@ -2360,7 +2360,7 @@ int ll_fsync(struct file *file, loff_t start, loff_t end, int datasync)
 	struct ptlrpc_request *req;
 	int rc, err;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p)\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p)\n",
 	       PFID(ll_inode2fid(inode)), inode);
 	ll_stats_ops_tally(ll_i2sbi(inode), LPROC_LL_FSYNC, 1);
 
@@ -2422,7 +2422,7 @@ ll_file_flock(struct file *file, int cmd, struct file_lock *file_lock)
 	int rc;
 	int rc2 = 0;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID" file_lock=%p\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID " file_lock=%p\n",
 	       PFID(ll_inode2fid(inode)), file_lock);
 
 	ll_stats_ops_tally(ll_i2sbi(inode), LPROC_LL_FLOCK, 1);
@@ -2507,7 +2507,7 @@ ll_file_flock(struct file *file, int cmd, struct file_lock *file_lock)
 	if (IS_ERR(op_data))
 		return PTR_ERR(op_data);
 
-	CDEBUG(D_DLMTRACE, "inode="DFID", pid=%u, flags=%#llx, mode=%u, start=%llu, end=%llu\n",
+	CDEBUG(D_DLMTRACE, "inode=" DFID ", pid=%u, flags=%#llx, mode=%u, start=%llu, end=%llu\n",
 	       PFID(ll_inode2fid(inode)), flock.l_flock.pid, flags,
 	       einfo.ei_mode, flock.l_flock.start, flock.l_flock.end);
 
@@ -2582,7 +2582,7 @@ int ll_migrate(struct inode *parent, struct file *file, int mdtidx,
 	struct qstr qstr;
 	int rc;
 
-	CDEBUG(D_VFSTRACE, "migrate %s under "DFID" to MDT%d\n",
+	CDEBUG(D_VFSTRACE, "migrate %s under " DFID " to MDT%d\n",
 	       name, PFID(ll_inode2fid(parent)), mdtidx);
 
 	op_data = ll_prep_md_op_data(NULL, parent, NULL, name, namelen,
@@ -2617,7 +2617,7 @@ int ll_migrate(struct inode *parent, struct file *file, int mdtidx,
 	inode_lock(child_inode);
 	op_data->op_fid3 = *ll_inode2fid(child_inode);
 	if (!fid_is_sane(&op_data->op_fid3)) {
-		CERROR("%s: migrate %s, but fid "DFID" is insane\n",
+		CERROR("%s: migrate %s, but fid " DFID " is insane\n",
 		       ll_get_fsname(parent->i_sb, NULL, 0), name,
 		       PFID(&op_data->op_fid3));
 		rc = -EINVAL;
@@ -2629,7 +2629,7 @@ int ll_migrate(struct inode *parent, struct file *file, int mdtidx,
 		goto out_unlock;
 
 	if (rc == mdtidx) {
-		CDEBUG(D_INFO, "%s:"DFID" is already on MDT%d.\n", name,
+		CDEBUG(D_INFO, "%s: " DFID " is already on MDT%d.\n", name,
 		       PFID(&op_data->op_fid3), mdtidx);
 		rc = 0;
 		goto out_unlock;
@@ -2733,7 +2733,7 @@ int ll_have_md_lock(struct inode *inode, __u64 *bits,
 		return 0;
 
 	fid = &ll_i2info(inode)->lli_fid;
-	CDEBUG(D_INFO, "trying to match res "DFID" mode %s\n", PFID(fid),
+	CDEBUG(D_INFO, "trying to match res " DFID " mode %s\n", PFID(fid),
 	       ldlm_lockname[mode]);
 
 	flags = LDLM_FL_BLOCK_GRANTED | LDLM_FL_CBPENDING | LDLM_FL_TEST_LOCK;
@@ -2767,7 +2767,7 @@ enum ldlm_mode ll_take_md_lock(struct inode *inode, __u64 bits,
 	struct lu_fid *fid;
 
 	fid = &ll_i2info(inode)->lli_fid;
-	CDEBUG(D_INFO, "trying to match res "DFID"\n", PFID(fid));
+	CDEBUG(D_INFO, "trying to match res " DFID "\n", PFID(fid));
 
 	return md_lock_match(ll_i2mdexp(inode), flags | LDLM_FL_BLOCK_GRANTED,
 			     fid, LDLM_IBITS, &policy, mode, lockh);
@@ -2792,7 +2792,7 @@ static int ll_inode_revalidate_fini(struct inode *inode, int rc)
 			return 0;
 	} else if (rc != 0) {
 		CDEBUG_LIMIT((rc == -EACCES || rc == -EIDRM) ? D_INFO : D_ERROR,
-			     "%s: revalidate FID "DFID" error: rc = %d\n",
+			     "%s: revalidate FID " DFID " error: rc = %d\n",
 			     ll_get_fsname(inode->i_sb, NULL, 0),
 			     PFID(ll_inode2fid(inode)), rc);
 	}
@@ -2807,7 +2807,7 @@ static int __ll_inode_revalidate(struct dentry *dentry, __u64 ibits)
 	struct obd_export *exp;
 	int rc = 0;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p),name=%pd\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p),name=%pd\n",
 	       PFID(ll_inode2fid(inode)), inode, dentry);
 
 	exp = ll_i2mdexp(inode);
@@ -3067,7 +3067,7 @@ int ll_inode_permission(struct inode *inode, int mask)
 			return rc;
 	}
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p), inode mode %x mask %o\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p), inode mode %x mask %o\n",
 	       PFID(ll_inode2fid(inode)), inode, inode->i_mode, mask);
 
 	/* squash fsuid/fsgid if needed */
@@ -3114,7 +3114,7 @@ int ll_inode_permission(struct inode *inode, int mask)
 }
 
 /* -o localflock - only provides locally consistent flock locks */
-struct file_operations ll_file_operations = {
+const struct file_operations ll_file_operations = {
 	.read_iter = ll_file_read_iter,
 	.write_iter = ll_file_write_iter,
 	.unlocked_ioctl = ll_file_ioctl,
@@ -3127,7 +3127,7 @@ struct file_operations ll_file_operations = {
 	.flush	  = ll_flush
 };
 
-struct file_operations ll_file_operations_flock = {
+const struct file_operations ll_file_operations_flock = {
 	.read_iter    = ll_file_read_iter,
 	.write_iter   = ll_file_write_iter,
 	.unlocked_ioctl = ll_file_ioctl,
@@ -3143,7 +3143,7 @@ struct file_operations ll_file_operations_flock = {
 };
 
 /* These are for -o noflock - to return ENOSYS on flock calls */
-struct file_operations ll_file_operations_noflock = {
+const struct file_operations ll_file_operations_noflock = {
 	.read_iter    = ll_file_read_iter,
 	.write_iter   = ll_file_write_iter,
 	.unlocked_ioctl = ll_file_ioctl,
@@ -3322,7 +3322,7 @@ static int ll_layout_fetch(struct inode *inode, struct ldlm_lock *lock)
 	int lmmsize;
 	int rc;
 
-	CDEBUG(D_INODE, DFID" LVB_READY=%d l_lvb_data=%p l_lvb_len=%d\n",
+	CDEBUG(D_INODE, DFID " LVB_READY=%d l_lvb_data=%p l_lvb_len=%d\n",
 	       PFID(ll_inode2fid(inode)), ldlm_is_lvb_ready(lock),
 	       lock->l_lvb_data, lock->l_lvb_len);
 
@@ -3447,7 +3447,7 @@ out:
 
 	/* wait for IO to complete if it's still being used. */
 	if (wait_layout) {
-		CDEBUG(D_INODE, "%s: "DFID"(%p) wait for layout reconf\n",
+		CDEBUG(D_INODE, "%s: " DFID "(%p) wait for layout reconf\n",
 		       ll_get_fsname(inode->i_sb, NULL, 0),
 		       PFID(&lli->lli_fid), inode);
 
@@ -3458,7 +3458,7 @@ out:
 		if (rc == 0)
 			rc = -EAGAIN;
 
-		CDEBUG(D_INODE, "%s: file="DFID" waiting layout return: %d.\n",
+		CDEBUG(D_INODE, "%s: file=" DFID " waiting layout return: %d.\n",
 		       ll_get_fsname(inode->i_sb, NULL, 0),
 		       PFID(&lli->lli_fid), rc);
 	}
@@ -3504,7 +3504,7 @@ again:
 	it.it_op = IT_LAYOUT;
 	lockh.cookie = 0ULL;
 
-	LDLM_DEBUG_NOLOCK("%s: requeue layout lock for file "DFID"(%p)",
+	LDLM_DEBUG_NOLOCK("%s: requeue layout lock for file " DFID "(%p)",
 			  ll_get_fsname(inode->i_sb, NULL, 0),
 			  PFID(&lli->lli_fid), inode);
 
diff --git a/drivers/staging/lustre/lustre/llite/lcommon_cl.c b/drivers/staging/lustre/lustre/llite/lcommon_cl.c
index 8af611033e12..96515b839436 100644
--- a/drivers/staging/lustre/lustre/llite/lcommon_cl.c
+++ b/drivers/staging/lustre/lustre/llite/lcommon_cl.c
@@ -207,7 +207,7 @@ int cl_file_inode_init(struct inode *inode, struct lustre_md *md)
 static void cl_object_put_last(struct lu_env *env, struct cl_object *obj)
 {
 	struct lu_object_header *header = obj->co_lu.lo_header;
-	wait_queue_t	   waiter;
+	wait_queue_entry_t	   waiter;
 
 	if (unlikely(atomic_read(&header->loh_ref) != 1)) {
 		struct lu_site *site = obj->co_lu.lo_dev->ld_site;
diff --git a/drivers/staging/lustre/lustre/llite/llite_internal.h b/drivers/staging/lustre/lustre/llite/llite_internal.h
index d2a0fabd8a69..cd3311abf999 100644
--- a/drivers/staging/lustre/lustre/llite/llite_internal.h
+++ b/drivers/staging/lustre/lustre/llite/llite_internal.h
@@ -470,7 +470,7 @@ struct ll_sb_info {
 
 	struct ll_ra_info	 ll_ra_info;
 	unsigned int	      ll_namelen;
-	struct file_operations   *ll_fop;
+	const struct file_operations	*ll_fop;
 
 	unsigned int		  ll_md_brw_pages; /* readdir pages per RPC */
 
@@ -718,14 +718,14 @@ extern const struct inode_operations ll_special_inode_operations;
 struct inode *ll_iget(struct super_block *sb, ino_t hash,
 		      struct lustre_md *lic);
 int ll_test_inode_by_fid(struct inode *inode, void *opaque);
-int ll_md_blocking_ast(struct ldlm_lock *, struct ldlm_lock_desc *,
+int ll_md_blocking_ast(struct ldlm_lock *lock, struct ldlm_lock_desc *desc,
 		       void *data, int flag);
 struct dentry *ll_splice_alias(struct inode *inode, struct dentry *de);
 void ll_update_times(struct ptlrpc_request *request, struct inode *inode);
 
 /* llite/rw.c */
 int ll_writepage(struct page *page, struct writeback_control *wbc);
-int ll_writepages(struct address_space *, struct writeback_control *wbc);
+int ll_writepages(struct address_space *mapping, struct writeback_control *wbc);
 int ll_readpage(struct file *file, struct page *page);
 void ll_readahead_init(struct inode *inode, struct ll_readahead_state *ras);
 int vvp_io_write_commit(const struct lu_env *env, struct cl_io *io);
@@ -736,9 +736,9 @@ void ll_cl_remove(struct file *file, const struct lu_env *env);
 extern const struct address_space_operations ll_aops;
 
 /* llite/file.c */
-extern struct file_operations ll_file_operations;
-extern struct file_operations ll_file_operations_flock;
-extern struct file_operations ll_file_operations_noflock;
+extern const struct file_operations ll_file_operations;
+extern const struct file_operations ll_file_operations_flock;
+extern const struct file_operations ll_file_operations_noflock;
 extern const struct inode_operations ll_file_inode_operations;
 int ll_have_md_lock(struct inode *inode, __u64 *bits,
 		    enum ldlm_mode l_req_mode);
@@ -747,7 +747,7 @@ enum ldlm_mode ll_take_md_lock(struct inode *inode, __u64 bits,
 			       enum ldlm_mode mode);
 int ll_file_open(struct inode *inode, struct file *file);
 int ll_file_release(struct inode *inode, struct file *file);
-int ll_release_openhandle(struct inode *, struct lookup_intent *);
+int ll_release_openhandle(struct inode *inode, struct lookup_intent *it);
 int ll_md_real_close(struct inode *inode, fmode_t fmode);
 int ll_getattr(const struct path *path, struct kstat *stat,
 	       u32 request_mask, unsigned int flags);
@@ -778,9 +778,9 @@ int ll_hsm_state_set(struct inode *inode, struct hsm_state_set *hss);
 /* llite/dcache.c */
 
 extern const struct dentry_operations ll_d_ops;
-void ll_intent_drop_lock(struct lookup_intent *);
-void ll_intent_release(struct lookup_intent *);
-void ll_invalidate_aliases(struct inode *);
+void ll_intent_drop_lock(struct lookup_intent *it);
+void ll_intent_release(struct lookup_intent *it);
+void ll_invalidate_aliases(struct inode *inode);
 void ll_lookup_finish_locks(struct lookup_intent *it, struct inode *inode);
 int ll_revalidate_it_finish(struct ptlrpc_request *request,
 			    struct lookup_intent *it, struct inode *inode);
@@ -811,7 +811,7 @@ int ll_remount_fs(struct super_block *sb, int *flags, char *data);
 int ll_show_options(struct seq_file *seq, struct dentry *dentry);
 void ll_dirty_page_discard_warn(struct page *page, int ioret);
 int ll_prep_inode(struct inode **inode, struct ptlrpc_request *req,
-		  struct super_block *, struct lookup_intent *);
+		  struct super_block *sb, struct lookup_intent *it);
 int ll_obd_statfs(struct inode *inode, void __user *arg);
 int ll_get_max_mdsize(struct ll_sb_info *sbi, int *max_mdsize);
 int ll_get_default_mdsize(struct ll_sb_info *sbi, int *default_mdsize);
@@ -1253,7 +1253,7 @@ static inline void ll_set_lock_data(struct obd_export *exp, struct inode *inode,
 		 */
 		if (it->it_remote_lock_mode) {
 			handle.cookie = it->it_remote_lock_handle;
-			CDEBUG(D_DLMTRACE, "setting l_data to inode "DFID"%p for remote lock %#llx\n",
+			CDEBUG(D_DLMTRACE, "setting l_data to inode " DFID "%p for remote lock %#llx\n",
 			       PFID(ll_inode2fid(inode)), inode,
 			       handle.cookie);
 			md_set_lock_data(exp, &handle, inode, NULL);
@@ -1261,7 +1261,7 @@ static inline void ll_set_lock_data(struct obd_export *exp, struct inode *inode,
 
 		handle.cookie = it->it_lock_handle;
 
-		CDEBUG(D_DLMTRACE, "setting l_data to inode "DFID"%p for lock %#llx\n",
+		CDEBUG(D_DLMTRACE, "setting l_data to inode " DFID "%p for lock %#llx\n",
 		       PFID(ll_inode2fid(inode)), inode, handle.cookie);
 
 		md_set_lock_data(exp, &handle, inode, &it->it_lock_bits);
diff --git a/drivers/staging/lustre/lustre/llite/llite_lib.c b/drivers/staging/lustre/lustre/llite/llite_lib.c
index ca5040c69217..974a05d6c969 100644
--- a/drivers/staging/lustre/lustre/llite/llite_lib.c
+++ b/drivers/staging/lustre/lustre/llite/llite_lib.c
@@ -427,13 +427,13 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 		goto out_lock_cn_cb;
 	}
 	if (!fid_is_sane(&sbi->ll_root_fid)) {
-		CERROR("%s: Invalid root fid "DFID" during mount\n",
+		CERROR("%s: Invalid root fid " DFID " during mount\n",
 		       sbi->ll_md_exp->exp_obd->obd_name,
 		       PFID(&sbi->ll_root_fid));
 		err = -EINVAL;
 		goto out_lock_cn_cb;
 	}
-	CDEBUG(D_SUPER, "rootfid "DFID"\n", PFID(&sbi->ll_root_fid));
+	CDEBUG(D_SUPER, "rootfid " DFID "\n", PFID(&sbi->ll_root_fid));
 
 	sb->s_op = &lustre_super_operations;
 	sb->s_xattr = ll_xattr_handlers;
@@ -1079,7 +1079,7 @@ static struct inode *ll_iget_anon_dir(struct super_block *sb,
 	ino = cl_fid_build_ino(fid, sbi->ll_flags & LL_SBI_32BIT_API);
 	inode = iget_locked(sb, ino);
 	if (!inode) {
-		CERROR("%s: failed get simple inode "DFID": rc = -ENOENT\n",
+		CERROR("%s: failed get simple inode " DFID ": rc = -ENOENT\n",
 		       ll_get_fsname(sb, NULL, 0), PFID(fid));
 		return ERR_PTR(-ENOENT);
 	}
@@ -1090,7 +1090,7 @@ static struct inode *ll_iget_anon_dir(struct super_block *sb,
 
 		inode->i_mode = (inode->i_mode & ~S_IFMT) |
 				(body->mbo_mode & S_IFMT);
-		LASSERTF(S_ISDIR(inode->i_mode), "Not slave inode "DFID"\n",
+		LASSERTF(S_ISDIR(inode->i_mode), "Not slave inode " DFID "\n",
 			 PFID(fid));
 
 		LTIME_S(inode->i_mtime) = 0;
@@ -1106,7 +1106,7 @@ static struct inode *ll_iget_anon_dir(struct super_block *sb,
 		LASSERT(lsm);
 		/* master object FID */
 		lli->lli_pfid = body->mbo_fid1;
-		CDEBUG(D_INODE, "lli %p slave "DFID" master "DFID"\n",
+		CDEBUG(D_INODE, "lli %p slave " DFID " master " DFID "\n",
 		       lli, PFID(fid), PFID(&lli->lli_pfid));
 		unlock_new_inode(inode);
 	}
@@ -1174,7 +1174,7 @@ static int ll_update_lsm_md(struct inode *inode, struct lustre_md *md)
 	int rc;
 
 	LASSERT(S_ISDIR(inode->i_mode));
-	CDEBUG(D_INODE, "update lsm %p of "DFID"\n", lli->lli_lsm_md,
+	CDEBUG(D_INODE, "update lsm %p of " DFID "\n", lli->lli_lsm_md,
 	       PFID(ll_inode2fid(inode)));
 
 	/* no striped information from request. */
@@ -1187,7 +1187,7 @@ static int ll_update_lsm_md(struct inode *inode, struct lustre_md *md)
 			 * migration is done, the temporay MIGRATE layout has
 			 * been removed
 			 */
-			CDEBUG(D_INODE, DFID" finish migration.\n",
+			CDEBUG(D_INODE, DFID " finish migration.\n",
 			       PFID(ll_inode2fid(inode)));
 			lmv_free_memmd(lli->lli_lsm_md);
 			lli->lli_lsm_md = NULL;
@@ -1241,7 +1241,7 @@ static int ll_update_lsm_md(struct inode *inode, struct lustre_md *md)
 
 		kfree(attr);
 
-		CDEBUG(D_INODE, "Set lsm %p magic %x to "DFID"\n", lsm,
+		CDEBUG(D_INODE, "Set lsm %p magic %x to " DFID "\n", lsm,
 		       lsm->lsm_md_magic, PFID(ll_inode2fid(inode)));
 		return 0;
 	}
@@ -1251,7 +1251,7 @@ static int ll_update_lsm_md(struct inode *inode, struct lustre_md *md)
 		struct lmv_stripe_md *old_lsm = lli->lli_lsm_md;
 		int idx;
 
-		CERROR("%s: inode "DFID"(%p)'s lmv layout mismatch (%p)/(%p) magic:0x%x/0x%x stripe count: %d/%d master_mdt: %d/%d hash_type:0x%x/0x%x layout: 0x%x/0x%x pool:%s/%s\n",
+		CERROR("%s: inode " DFID "(%p)'s lmv layout mismatch (%p)/(%p) magic:0x%x/0x%x stripe count: %d/%d master_mdt: %d/%d hash_type:0x%x/0x%x layout: 0x%x/0x%x pool:%s/%s\n",
 		       ll_get_fsname(inode->i_sb, NULL, 0), PFID(&lli->lli_fid),
 		       inode, lsm, old_lsm,
 		       lsm->lsm_md_magic, old_lsm->lsm_md_magic,
@@ -1266,13 +1266,13 @@ static int ll_update_lsm_md(struct inode *inode, struct lustre_md *md)
 		       old_lsm->lsm_md_pool_name);
 
 		for (idx = 0; idx < old_lsm->lsm_md_stripe_count; idx++) {
-			CERROR("%s: sub FIDs in old lsm idx %d, old: "DFID"\n",
+			CERROR("%s: sub FIDs in old lsm idx %d, old: " DFID "\n",
 			       ll_get_fsname(inode->i_sb, NULL, 0), idx,
 			       PFID(&old_lsm->lsm_md_oinfo[idx].lmo_fid));
 		}
 
 		for (idx = 0; idx < lsm->lsm_md_stripe_count; idx++) {
-			CERROR("%s: sub FIDs in new lsm idx %d, new: "DFID"\n",
+			CERROR("%s: sub FIDs in new lsm idx %d, new: " DFID "\n",
 			       ll_get_fsname(inode->i_sb, NULL, 0), idx,
 			       PFID(&lsm->lsm_md_oinfo[idx].lmo_fid));
 		}
@@ -1288,7 +1288,7 @@ void ll_clear_inode(struct inode *inode)
 	struct ll_inode_info *lli = ll_i2info(inode);
 	struct ll_sb_info *sbi = ll_i2sbi(inode);
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p)\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p)\n",
 	       PFID(ll_inode2fid(inode)), inode);
 
 	if (S_ISDIR(inode->i_mode)) {
@@ -1421,7 +1421,7 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 	struct md_op_data *op_data = NULL;
 	int rc = 0;
 
-	CDEBUG(D_VFSTRACE, "%s: setattr inode "DFID"(%p) from %llu to %llu, valid %x, hsm_import %d\n",
+	CDEBUG(D_VFSTRACE, "%s: setattr inode " DFID "(%p) from %llu to %llu, valid %x, hsm_import %d\n",
 	       ll_get_fsname(inode->i_sb, NULL, 0), PFID(&lli->lli_fid), inode,
 	       i_size_read(inode), attr->ia_size, attr->ia_valid, hsm_import);
 
@@ -1436,7 +1436,7 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 		 * needs another check in addition to the VFS check above.
 		 */
 		if (attr->ia_size > ll_file_maxbytes(inode)) {
-			CDEBUG(D_INODE, "file "DFID" too large %llu > %llu\n",
+			CDEBUG(D_INODE, "file " DFID " too large %llu > %llu\n",
 			       PFID(&lli->lli_fid), attr->ia_size,
 			       ll_file_maxbytes(inode));
 			return -EFBIG;
@@ -1785,7 +1785,7 @@ int ll_update_inode(struct inode *inode, struct lustre_md *md)
 		/* FID shouldn't be changed! */
 		if (fid_is_sane(&lli->lli_fid)) {
 			LASSERTF(lu_fid_eq(&lli->lli_fid, &body->mbo_fid1),
-				 "Trying to change FID "DFID" to the "DFID", inode "DFID"(%p)\n",
+				 "Trying to change FID " DFID " to the " DFID ", inode " DFID "(%p)\n",
 				 PFID(&lli->lli_fid), PFID(&body->mbo_fid1),
 				 PFID(ll_inode2fid(inode)), inode);
 		} else {
@@ -1820,7 +1820,7 @@ int ll_read_inode2(struct inode *inode, void *opaque)
 	struct ll_inode_info *lli = ll_i2info(inode);
 	int rc;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p)\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p)\n",
 	       PFID(&lli->lli_fid), inode);
 
 	/* Core attributes from the MDS first.  This is a new inode, and
@@ -1902,7 +1902,7 @@ int ll_iocontrol(struct inode *inode, struct file *file,
 		rc = md_getattr(sbi->ll_md_exp, op_data, &req);
 		ll_finish_md_op_data(op_data);
 		if (rc) {
-			CERROR("%s: failure inode "DFID": rc = %d\n",
+			CERROR("%s: failure inode " DFID ": rc = %d\n",
 			       sbi->ll_md_exp->exp_obd->obd_name,
 			       PFID(ll_inode2fid(inode)), rc);
 			return -abs(rc);
diff --git a/drivers/staging/lustre/lustre/llite/llite_mmap.c b/drivers/staging/lustre/lustre/llite/llite_mmap.c
index cbbfdaf127a7..ccc7ae15a943 100644
--- a/drivers/staging/lustre/lustre/llite/llite_mmap.c
+++ b/drivers/staging/lustre/lustre/llite/llite_mmap.c
@@ -378,7 +378,7 @@ static int ll_page_mkwrite(struct vm_fault *vmf)
 		if (!printed && ++count > 16) {
 			const struct dentry *de = vma->vm_file->f_path.dentry;
 
-			CWARN("app(%s): the page %lu of file "DFID" is under heavy contention\n",
+			CWARN("app(%s): the page %lu of file " DFID " is under heavy contention\n",
 			      current->comm, vmf->pgoff,
 			      PFID(ll_inode2fid(de->d_inode)));
 			printed = true;
diff --git a/drivers/staging/lustre/lustre/llite/llite_nfs.c b/drivers/staging/lustre/lustre/llite/llite_nfs.c
index 49a930f0fc5d..e50c637fab54 100644
--- a/drivers/staging/lustre/lustre/llite/llite_nfs.c
+++ b/drivers/staging/lustre/lustre/llite/llite_nfs.c
@@ -84,7 +84,7 @@ struct inode *search_inode_for_lustre(struct super_block *sb,
 	struct  md_op_data    *op_data;
 	int		   rc;
 
-	CDEBUG(D_INFO, "searching inode for:(%lu,"DFID")\n", hash, PFID(fid));
+	CDEBUG(D_INFO, "searching inode for:(%lu," DFID ")\n", hash, PFID(fid));
 
 	inode = ilookup5(sb, hash, ll_test_inode_by_fid, (void *)fid);
 	if (inode)
@@ -109,7 +109,7 @@ struct inode *search_inode_for_lustre(struct super_block *sb,
 	rc = md_getattr(sbi->ll_md_exp, op_data, &req);
 	kfree(op_data);
 	if (rc) {
-		CDEBUG(D_INFO, "can't get object attrs, fid "DFID", rc %d\n",
+		CDEBUG(D_INFO, "can't get object attrs, fid " DFID ", rc %d\n",
 		       PFID(fid), rc);
 		return ERR_PTR(rc);
 	}
@@ -195,7 +195,7 @@ static int ll_encode_fh(struct inode *inode, __u32 *fh, int *plen,
 	int fileid_len = sizeof(struct lustre_nfs_fid) / 4;
 	struct lustre_nfs_fid *nfs_fid = (void *)fh;
 
-	CDEBUG(D_INFO, "%s: encoding for ("DFID") maxlen=%d minlen=%d\n",
+	CDEBUG(D_INFO, "%s: encoding for (" DFID ") maxlen=%d minlen=%d\n",
 	       ll_get_fsname(inode->i_sb, NULL, 0),
 	       PFID(ll_inode2fid(inode)), *plen, fileid_len);
 
@@ -312,7 +312,7 @@ int ll_dir_get_parent_fid(struct inode *dir, struct lu_fid *parent_fid)
 
 	sbi = ll_s2sbi(dir->i_sb);
 
-	CDEBUG(D_INFO, "%s: getting parent for ("DFID")\n",
+	CDEBUG(D_INFO, "%s: getting parent for (" DFID ")\n",
 	       ll_get_fsname(dir->i_sb, NULL, 0),
 	       PFID(ll_inode2fid(dir)));
 
@@ -329,7 +329,7 @@ int ll_dir_get_parent_fid(struct inode *dir, struct lu_fid *parent_fid)
 	rc = md_getattr_name(sbi->ll_md_exp, op_data, &req);
 	ll_finish_md_op_data(op_data);
 	if (rc) {
-		CERROR("%s: failure inode "DFID" get parent: rc = %d\n",
+		CERROR("%s: failure inode " DFID " get parent: rc = %d\n",
 		       ll_get_fsname(dir->i_sb, NULL, 0),
 		       PFID(ll_inode2fid(dir)), rc);
 		return rc;
diff --git a/drivers/staging/lustre/lustre/llite/lproc_llite.c b/drivers/staging/lustre/lustre/llite/lproc_llite.c
index c742cba60199..aeae6670e262 100644
--- a/drivers/staging/lustre/lustre/llite/lproc_llite.c
+++ b/drivers/staging/lustre/lustre/llite/lproc_llite.c
@@ -39,9 +39,9 @@
 #include "vvp_internal.h"
 
 /* debugfs llite mount point registration */
-static struct file_operations ll_rw_extents_stats_fops;
-static struct file_operations ll_rw_extents_stats_pp_fops;
-static struct file_operations ll_rw_offset_stats_fops;
+static const struct file_operations ll_rw_extents_stats_fops;
+static const struct file_operations ll_rw_extents_stats_pp_fops;
+static const struct file_operations ll_rw_offset_stats_fops;
 
 static ssize_t blocksize_show(struct kobject *kobj, struct attribute *attr,
 			      char *buf)
diff --git a/drivers/staging/lustre/lustre/llite/namei.c b/drivers/staging/lustre/lustre/llite/namei.c
index d583696e8378..a208a8b02c2c 100644
--- a/drivers/staging/lustre/lustre/llite/namei.c
+++ b/drivers/staging/lustre/lustre/llite/namei.c
@@ -86,7 +86,7 @@ static int ll_set_inode(struct inode *inode, void *opaque)
 
 	inode->i_mode = (inode->i_mode & ~S_IFMT) | (body->mbo_mode & S_IFMT);
 	if (unlikely(inode->i_mode == 0)) {
-		CERROR("Invalid inode "DFID" type\n", PFID(&lli->lli_fid));
+		CERROR("Invalid inode " DFID " type\n", PFID(&lli->lli_fid));
 		return -EINVAL;
 	}
 
@@ -134,7 +134,7 @@ struct inode *ll_iget(struct super_block *sb, ino_t hash,
 		}
 	} else if (!(inode->i_state & (I_FREEING | I_CLEAR))) {
 		rc = ll_update_inode(inode, md);
-		CDEBUG(D_VFSTRACE, "got inode: "DFID"(%p): rc = %d\n",
+		CDEBUG(D_VFSTRACE, "got inode: " DFID "(%p): rc = %d\n",
 		       PFID(&md->body->mbo_fid1), inode, rc);
 		if (rc) {
 			if (S_ISDIR(inode->i_mode))
@@ -205,7 +205,7 @@ int ll_md_blocking_ast(struct ldlm_lock *lock, struct ldlm_lock_desc *desc,
 
 		if (!fid_res_name_eq(ll_inode2fid(inode),
 				     &lock->l_resource->lr_name)) {
-			LDLM_ERROR(lock, "data mismatch with object "DFID"(%p)",
+			LDLM_ERROR(lock, "data mismatch with object " DFID "(%p)",
 				   PFID(ll_inode2fid(inode)), inode);
 			LBUG();
 		}
@@ -257,7 +257,7 @@ int ll_md_blocking_ast(struct ldlm_lock *lock, struct ldlm_lock_desc *desc,
 			rc = ll_layout_conf(inode, &conf);
 			if (rc < 0)
 				CDEBUG(D_INODE, "cannot invalidate layout of "
-				       DFID": rc = %d\n",
+				       DFID ": rc = %d\n",
 				       PFID(ll_inode2fid(inode)), rc);
 		}
 
@@ -274,7 +274,7 @@ int ll_md_blocking_ast(struct ldlm_lock *lock, struct ldlm_lock_desc *desc,
 		if ((bits & MDS_INODELOCK_UPDATE) && S_ISDIR(inode->i_mode)) {
 			struct ll_inode_info *lli = ll_i2info(inode);
 
-			CDEBUG(D_INODE, "invalidating inode "DFID" lli = %p, pfid  = "DFID"\n",
+			CDEBUG(D_INODE, "invalidating inode " DFID " lli = %p, pfid  = " DFID "\n",
 			       PFID(ll_inode2fid(inode)), lli,
 			       PFID(&lli->lli_pfid));
 
@@ -290,7 +290,7 @@ int ll_md_blocking_ast(struct ldlm_lock *lock, struct ldlm_lock_desc *desc,
 				 * we have to invalidate the negative children
 				 * on master inode
 				 */
-				CDEBUG(D_INODE, "Invalidate s"DFID" m"DFID"\n",
+				CDEBUG(D_INODE, "Invalidate s" DFID " m" DFID "\n",
 				       PFID(ll_inode2fid(inode)),
 				       PFID(&lli->lli_pfid));
 
@@ -542,7 +542,7 @@ static struct dentry *ll_lookup_it(struct inode *parent, struct dentry *dentry,
 	if (dentry->d_name.len > ll_i2sbi(parent)->ll_namelen)
 		return ERR_PTR(-ENAMETOOLONG);
 
-	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir="DFID"(%p),intent=%s\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir=" DFID "(%p),intent=%s\n",
 	       dentry, PFID(ll_inode2fid(parent)), parent, LL_IT2STR(it));
 
 	if (d_mountpoint(dentry))
@@ -650,7 +650,7 @@ static struct dentry *ll_lookup_nd(struct inode *parent, struct dentry *dentry,
 	struct lookup_intent *itp, it = { .it_op = IT_GETATTR };
 	struct dentry *de;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir="DFID"(%p),flags=%u\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir=" DFID "(%p),flags=%u\n",
 	       dentry, PFID(ll_inode2fid(parent)), parent, flags);
 
 	/* Optimize away (CREATE && !OPEN). Let .create handle the race.
@@ -678,14 +678,14 @@ static struct dentry *ll_lookup_nd(struct inode *parent, struct dentry *dentry,
  * together.
  */
 static int ll_atomic_open(struct inode *dir, struct dentry *dentry,
-			  struct file *file, unsigned open_flags,
+			  struct file *file, unsigned int open_flags,
 			  umode_t mode, int *opened)
 {
 	struct lookup_intent *it;
 	struct dentry *de;
 	int rc = 0;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir="DFID"(%p),file %p,open_flags %x,mode %x opened %d\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir=" DFID "(%p),file %p,open_flags %x,mode %x opened %d\n",
 	       dentry, PFID(ll_inode2fid(dir)), dir, file, open_flags, mode,
 	       *opened);
 
@@ -792,7 +792,7 @@ static struct inode *ll_create_node(struct inode *dir, struct lookup_intent *it)
 	 * lock on the inode.  Since we finally have an inode pointer,
 	 * stuff it in the lock.
 	 */
-	CDEBUG(D_DLMTRACE, "setting l_ast_data to inode "DFID"(%p)\n",
+	CDEBUG(D_DLMTRACE, "setting l_ast_data to inode " DFID "(%p)\n",
 	       PFID(ll_inode2fid(dir)), inode);
 	ll_set_lock_data(sbi->ll_md_exp, inode, it, NULL);
  out:
@@ -820,7 +820,7 @@ static int ll_create_it(struct inode *dir, struct dentry *dentry,
 	struct inode *inode;
 	int rc = 0;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir="DFID"(%p), intent=%s\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir=" DFID "(%p), intent=%s\n",
 	       dentry, PFID(ll_inode2fid(dir)), dir, LL_IT2STR(it));
 
 	rc = it_open_error(DISP_OPEN_CREATE, it);
@@ -844,7 +844,7 @@ void ll_update_times(struct ptlrpc_request *request, struct inode *inode)
 	LASSERT(body);
 	if (body->mbo_valid & OBD_MD_FLMTIME &&
 	    body->mbo_mtime > LTIME_S(inode->i_mtime)) {
-		CDEBUG(D_INODE, "setting fid "DFID" mtime from %lu to %llu\n",
+		CDEBUG(D_INODE, "setting fid " DFID " mtime from %lu to %llu\n",
 		       PFID(ll_inode2fid(inode)), LTIME_S(inode->i_mtime),
 		       body->mbo_mtime);
 		LTIME_S(inode->i_mtime) = body->mbo_mtime;
@@ -942,7 +942,7 @@ static int ll_mknod(struct inode *dir, struct dentry *dchild,
 {
 	int err;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir="DFID"(%p) mode %o dev %x\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir=" DFID "(%p) mode %o dev %x\n",
 	       dchild, PFID(ll_inode2fid(dir)), dir, mode,
 	       old_encode_dev(rdev));
 
@@ -982,7 +982,7 @@ static int ll_create_nd(struct inode *dir, struct dentry *dentry,
 {
 	int rc;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir="DFID"(%p), flags=%u, excl=%d\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir=" DFID "(%p), flags=%u, excl=%d\n",
 	       dentry, PFID(ll_inode2fid(dir)), dir, mode, want_excl);
 
 	rc = ll_mknod(dir, dentry, mode, 0);
@@ -1032,7 +1032,7 @@ static int ll_mkdir(struct inode *dir, struct dentry *dentry, umode_t mode)
 {
 	int err;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir"DFID"(%p)\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir" DFID "(%p)\n",
 	       dentry, PFID(ll_inode2fid(dir)), dir);
 
 	if (!IS_POSIXACL(dir) || !exp_connect_umask(ll_i2mdexp(dir)))
@@ -1052,7 +1052,7 @@ static int ll_rmdir(struct inode *dir, struct dentry *dchild)
 	struct md_op_data *op_data;
 	int rc;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir="DFID"(%p)\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir=" DFID "(%p)\n",
 	       dchild, PFID(ll_inode2fid(dir)), dir);
 
 	op_data = ll_prep_md_op_data(NULL, dir, NULL,
@@ -1082,7 +1082,7 @@ static int ll_symlink(struct inode *dir, struct dentry *dentry,
 {
 	int err;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir="DFID"(%p),target=%.*s\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:name=%pd, dir=" DFID "(%p),target=%.*s\n",
 	       dentry, PFID(ll_inode2fid(dir)), dir, 3000, oldname);
 
 	err = ll_new_node(dir, dentry, oldname, S_IFLNK | 0777,
@@ -1103,7 +1103,7 @@ static int ll_link(struct dentry *old_dentry, struct inode *dir,
 	struct md_op_data *op_data;
 	int err;
 
-	CDEBUG(D_VFSTRACE, "VFS Op: inode="DFID"(%p), dir="DFID"(%p), target=%pd\n",
+	CDEBUG(D_VFSTRACE, "VFS Op: inode=" DFID "(%p), dir=" DFID "(%p), target=%pd\n",
 	       PFID(ll_inode2fid(src)), src, PFID(ll_inode2fid(dir)), dir,
 	       new_dentry);
 
@@ -1138,7 +1138,7 @@ static int ll_rename(struct inode *src, struct dentry *src_dchild,
 		return -EINVAL;
 
 	CDEBUG(D_VFSTRACE,
-	       "VFS Op:oldname=%pd, src_dir="DFID"(%p), newname=%pd, tgt_dir="DFID"(%p)\n",
+	       "VFS Op:oldname=%pd, src_dir=" DFID "(%p), newname=%pd, tgt_dir=" DFID "(%p)\n",
 	       src_dchild, PFID(ll_inode2fid(src)), src,
 	       tgt_dchild, PFID(ll_inode2fid(tgt)), tgt);
 
diff --git a/drivers/staging/lustre/lustre/llite/rw26.c b/drivers/staging/lustre/lustre/llite/rw26.c
index 420f296f9658..3619cd8bb5f3 100644
--- a/drivers/staging/lustre/lustre/llite/rw26.c
+++ b/drivers/staging/lustre/lustre/llite/rw26.c
@@ -327,7 +327,7 @@ static ssize_t ll_direct_IO_26(struct kiocb *iocb, struct iov_iter *iter)
 	if ((file_offset & ~PAGE_MASK) || (count & ~PAGE_MASK))
 		return -EINVAL;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p), size=%zd (max %lu), offset=%lld=%llx, pages %zd (max %lu)\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p), size=%zd (max %lu), offset=%lld=%llx, pages %zd (max %lu)\n",
 	       PFID(ll_inode2fid(inode)), inode, count, MAX_DIO_SIZE,
 	       file_offset, file_offset, count >> PAGE_SHIFT,
 	       MAX_DIO_SIZE >> PAGE_SHIFT);
@@ -547,7 +547,7 @@ out:
 }
 
 static int ll_write_end(struct file *file, struct address_space *mapping,
-			loff_t pos, unsigned len, unsigned copied,
+			loff_t pos, unsigned int len, unsigned int copied,
 			struct page *vmpage, void *fsdata)
 {
 	struct ll_cl_context *lcc = fsdata;
diff --git a/drivers/staging/lustre/lustre/llite/statahead.c b/drivers/staging/lustre/lustre/llite/statahead.c
index fb7c315b33cb..9bbca018a5fe 100644
--- a/drivers/staging/lustre/lustre/llite/statahead.c
+++ b/drivers/staging/lustre/lustre/llite/statahead.c
@@ -528,7 +528,7 @@ static void ll_agl_trigger(struct inode *inode, struct ll_statahead_info *sai)
 	}
 
 	CDEBUG(D_READA, "Handling (init) async glimpse: inode = "
-	       DFID", idx = %llu\n", PFID(&lli->lli_fid), index);
+	       DFID ", idx = %llu\n", PFID(&lli->lli_fid), index);
 
 	cl_agl(inode);
 	lli->lli_agl_index = 0;
@@ -536,7 +536,7 @@ static void ll_agl_trigger(struct inode *inode, struct ll_statahead_info *sai)
 	up_write(&lli->lli_glimpse_sem);
 
 	CDEBUG(D_READA, "Handled (init) async glimpse: inode= "
-	       DFID", idx = %llu, rc = %d\n",
+	       DFID ", idx = %llu, rc = %d\n",
 	       PFID(&lli->lli_fid), index, rc);
 
 	iput(inode);
@@ -1008,7 +1008,7 @@ static int ll_statahead_thread(void *arg)
 		sai->sai_in_readpage = 0;
 		if (IS_ERR(page)) {
 			rc = PTR_ERR(page);
-			CDEBUG(D_READA, "error reading dir "DFID" at %llu/%llu: opendir_pid = %u: rc = %d\n",
+			CDEBUG(D_READA, "error reading dir " DFID " at %llu/%llu: opendir_pid = %u: rc = %d\n",
 			       PFID(ll_inode2fid(dir)), pos, sai->sai_index,
 			       lli->lli_opendir_pid, rc);
 			break;
@@ -1105,7 +1105,7 @@ static int ll_statahead_thread(void *arg)
 		if (sa_low_hit(sai)) {
 			rc = -EFAULT;
 			atomic_inc(&sbi->ll_sa_wrong);
-			CDEBUG(D_READA, "Statahead for dir "DFID" hit ratio too low: hit/miss %llu/%llu, sent/replied %llu/%llu, stopping statahead thread: pid %d\n",
+			CDEBUG(D_READA, "Statahead for dir " DFID " hit ratio too low: hit/miss %llu/%llu, sent/replied %llu/%llu, stopping statahead thread: pid %d\n",
 			       PFID(&lli->lli_fid), sai->sai_hit,
 			       sai->sai_miss, sai->sai_sent,
 			       sai->sai_replied, current_pid());
@@ -1211,7 +1211,7 @@ void ll_deauthorize_statahead(struct inode *dir, void *key)
 	LASSERT(lli->lli_opendir_key == key);
 	LASSERT(lli->lli_opendir_pid);
 
-	CDEBUG(D_READA, "deauthorize statahead for "DFID"\n",
+	CDEBUG(D_READA, "deauthorize statahead for " DFID "\n",
 	       PFID(&lli->lli_fid));
 
 	spin_lock(&lli->lli_sa_lock);
@@ -1274,7 +1274,7 @@ static int is_first_dirent(struct inode *dir, struct dentry *dentry)
 			struct ll_inode_info *lli = ll_i2info(dir);
 
 			rc = PTR_ERR(page);
-			CERROR("%s: error reading dir "DFID" at %llu: opendir_pid = %u : rc = %d\n",
+			CERROR("%s: error reading dir " DFID " at %llu: opendir_pid = %u : rc = %d\n",
 			       ll_get_fsname(dir->i_sb, NULL, 0),
 			       PFID(ll_inode2fid(dir)), pos,
 			       lli->lli_opendir_pid, rc);
@@ -1471,7 +1471,7 @@ static int revalidate_statahead_dentry(struct inode *dir,
 			} else if ((*dentryp)->d_inode != inode) {
 				/* revalidate, but inode is recreated */
 				CDEBUG(D_READA,
-				       "%s: stale dentry %pd inode "DFID", statahead inode "DFID"\n",
+				       "%s: stale dentry %pd inode " DFID ", statahead inode " DFID "\n",
 				       ll_get_fsname((*dentryp)->d_inode->i_sb,
 						     NULL, 0),
 				       *dentryp,
diff --git a/drivers/staging/lustre/lustre/llite/symlink.c b/drivers/staging/lustre/lustre/llite/symlink.c
index 60aac42e5344..3cd33483afaf 100644
--- a/drivers/staging/lustre/lustre/llite/symlink.c
+++ b/drivers/staging/lustre/lustre/llite/symlink.c
@@ -72,7 +72,7 @@ static int ll_readlink_internal(struct inode *inode,
 	ll_finish_md_op_data(op_data);
 	if (rc) {
 		if (rc != -ENOENT)
-			CERROR("%s: inode "DFID": rc = %d\n",
+			CERROR("%s: inode " DFID ": rc = %d\n",
 			       ll_get_fsname(inode->i_sb, NULL, 0),
 			       PFID(ll_inode2fid(inode)), rc);
 		goto failed;
@@ -87,7 +87,7 @@ static int ll_readlink_internal(struct inode *inode,
 
 	LASSERT(symlen != 0);
 	if (body->mbo_eadatasize != symlen) {
-		CERROR("%s: inode "DFID": symlink length %d not expected %d\n",
+		CERROR("%s: inode " DFID ": symlink length %d not expected %d\n",
 		       ll_get_fsname(inode->i_sb, NULL, 0),
 		       PFID(ll_inode2fid(inode)), body->mbo_eadatasize - 1,
 		       symlen - 1);
diff --git a/drivers/staging/lustre/lustre/llite/vvp_dev.c b/drivers/staging/lustre/lustre/llite/vvp_dev.c
index 6cb2db28eb60..8e45672b4617 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_dev.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_dev.c
@@ -381,11 +381,11 @@ int cl_sb_fini(struct super_block *sb)
 #define PGC_DEPTH_SHIFT (32)
 
 struct vvp_pgcache_id {
-	unsigned		 vpi_bucket;
-	unsigned		 vpi_depth;
+	unsigned int		 vpi_bucket;
+	unsigned int		 vpi_depth;
 	uint32_t		 vpi_index;
 
-	unsigned		 vpi_curdep;
+	unsigned int		 vpi_curdep;
 	struct lu_object_header *vpi_obj;
 };
 
diff --git a/drivers/staging/lustre/lustre/llite/vvp_io.c b/drivers/staging/lustre/lustre/llite/vvp_io.c
index aa31bc0a58a6..c5ba265ef6ad 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_io.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_io.c
@@ -325,7 +325,7 @@ static void vvp_io_fini(const struct lu_env *env, const struct cl_io_slice *ios)
 		io->ci_need_restart = vio->vui_layout_gen != gen;
 		if (io->ci_need_restart) {
 			CDEBUG(D_VFSTRACE,
-			       DFID" layout changed from %d to %d.\n",
+			       DFID " layout changed from %d to %d.\n",
 			       PFID(lu_object_fid(&obj->co_lu)),
 			       vio->vui_layout_gen, gen);
 			/* today successful restore is the only possible case */
diff --git a/drivers/staging/lustre/lustre/llite/vvp_object.c b/drivers/staging/lustre/lustre/llite/vvp_object.c
index 8e18cf86cefc..9bfd72e514d1 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_object.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_object.c
@@ -70,7 +70,7 @@ static int vvp_object_print(const struct lu_env *env, void *cookie,
 	     atomic_read(&obj->vob_mmap_cnt), inode);
 	if (inode) {
 		lli = ll_i2info(inode);
-		(*p)(env, cookie, "%lu/%u %o %u %d %p "DFID,
+		(*p)(env, cookie, "%lu/%u %o %u %d %p " DFID,
 		     inode->i_ino, inode->i_generation, inode->i_mode,
 		     inode->i_nlink, atomic_read(&inode->i_count),
 		     lli->lli_clob, PFID(&lli->lli_fid));
diff --git a/drivers/staging/lustre/lustre/llite/xattr.c b/drivers/staging/lustre/lustre/llite/xattr.c
index 6187bffec8c4..bd30abdecfb9 100644
--- a/drivers/staging/lustre/lustre/llite/xattr.c
+++ b/drivers/staging/lustre/lustre/llite/xattr.c
@@ -195,7 +195,7 @@ static int ll_xattr_set(const struct xattr_handler *handler,
 	LASSERT(inode);
 	LASSERT(name);
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p), xattr %s\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p), xattr %s\n",
 	       PFID(ll_inode2fid(inode)), inode, name);
 
 	if (!strcmp(name, "lov")) {
@@ -370,7 +370,7 @@ static int ll_xattr_get_common(const struct xattr_handler *handler,
 #endif
 	int rc;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p)\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p)\n",
 	       PFID(ll_inode2fid(inode)), inode);
 
 	ll_stats_ops_tally(ll_i2sbi(inode), LPROC_LL_GETXATTR, 1);
@@ -523,7 +523,7 @@ ssize_t ll_listxattr(struct dentry *dentry, char *buffer, size_t size)
 
 	LASSERT(inode);
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode="DFID"(%p)\n",
+	CDEBUG(D_VFSTRACE, "VFS Op:inode=" DFID "(%p)\n",
 	       PFID(ll_inode2fid(inode)), inode);
 
 	ll_stats_ops_tally(ll_i2sbi(inode), LPROC_LL_LISTXATTR, 1);
diff --git a/drivers/staging/lustre/lustre/llite/xattr_cache.c b/drivers/staging/lustre/lustre/llite/xattr_cache.c
index 38f75f6aa887..82cf4211cc0f 100644
--- a/drivers/staging/lustre/lustre/llite/xattr_cache.c
+++ b/drivers/staging/lustre/lustre/llite/xattr_cache.c
@@ -311,7 +311,7 @@ static int ll_xattr_find_get_lock(struct inode *inode,
 
 	if (rc < 0) {
 		CDEBUG(D_CACHE,
-		       "md_intent_lock failed with %d for fid "DFID"\n",
+		       "md_intent_lock failed with %d for fid " DFID "\n",
 		       rc, PFID(ll_inode2fid(inode)));
 		mutex_unlock(&lli->lli_xattrs_enq_lock);
 		return rc;
@@ -365,7 +365,7 @@ static int ll_xattr_cache_refill(struct inode *inode, struct lookup_intent *oit)
 	}
 
 	if (oit->it_status < 0) {
-		CDEBUG(D_CACHE, "getxattr intent returned %d for fid "DFID"\n",
+		CDEBUG(D_CACHE, "getxattr intent returned %d for fid " DFID "\n",
 		       oit->it_status, PFID(ll_inode2fid(inode)));
 		rc = oit->it_status;
 		/* xattr data is so large that we don't want to cache it */
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_fld.c b/drivers/staging/lustre/lustre/lmv/lmv_fld.c
index a5265f9b5797..6f8070fb3226 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_fld.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_fld.c
@@ -70,7 +70,7 @@ int lmv_fld_lookup(struct lmv_obd *lmv, const struct lu_fid *fid, u32 *mds)
 		return rc;
 	}
 
-	CDEBUG(D_INODE, "FLD lookup got mds #%x for fid="DFID"\n",
+	CDEBUG(D_INODE, "FLD lookup got mds #%x for fid=" DFID "\n",
 	       *mds, PFID(fid));
 
 	if (*mds >= lmv->desc.ld_tgt_count) {
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_intent.c b/drivers/staging/lustre/lustre/lmv/lmv_intent.c
index aa42066678e0..f49db6c23217 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_intent.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_intent.c
@@ -213,7 +213,7 @@ int lmv_revalidate_slaves(struct obd_export *exp,
 		lockh = (struct lustre_handle *)&it.it_lock_handle;
 		if (rc > 0 && !req) {
 			/* slave inode is still valid */
-			CDEBUG(D_INODE, "slave "DFID" is still valid.\n",
+			CDEBUG(D_INODE, "slave " DFID " is still valid.\n",
 			       PFID(&fid));
 			rc = 0;
 		} else {
@@ -435,7 +435,7 @@ static int lmv_intent_lookup(struct obd_export *exp,
 			if (IS_ERR(tgt))
 				return PTR_ERR(tgt);
 
-			CDEBUG(D_INODE, "Try other stripes " DFID"\n",
+			CDEBUG(D_INODE, "Try other stripes " DFID "\n",
 			       PFID(&oinfo->lmo_fid));
 
 			op_data->op_fid1 = oinfo->lmo_fid;
@@ -479,7 +479,7 @@ int lmv_intent_lock(struct obd_export *exp, struct md_op_data *op_data,
 
 	LASSERT(fid_is_sane(&op_data->op_fid1));
 
-	CDEBUG(D_INODE, "INTENT LOCK '%s' for "DFID" '%*s' on "DFID"\n",
+	CDEBUG(D_INODE, "INTENT LOCK '%s' for " DFID " '%*s' on " DFID "\n",
 	       LL_IT2STR(it), PFID(&op_data->op_fid2),
 	       (int)op_data->op_namelen, op_data->op_name,
 	       PFID(&op_data->op_fid1));
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_obd.c b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
index 732595125d8a..64fcaef0bacd 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_obd.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
@@ -75,7 +75,7 @@ static void lmv_activate_target(struct lmv_obd *lmv,
 static int lmv_set_mdc_active(struct lmv_obd *lmv, const struct obd_uuid *uuid,
 			      int activate)
 {
-	struct lmv_tgt_desc    *uninitialized_var(tgt);
+	struct lmv_tgt_desc *tgt = NULL;
 	struct obd_device      *obd;
 	u32		     i;
 	int		     rc = 0;
@@ -673,7 +673,7 @@ repeat_fid2path:
 		*ptr = '/';
 	}
 
-	CDEBUG(D_INFO, "%s: get path %s "DFID" rec: %llu ln: %u\n",
+	CDEBUG(D_INFO, "%s: get path %s " DFID " rec: %llu ln: %u\n",
 	       tgt->ltd_exp->exp_obd->obd_name,
 	       gf->gf_path, PFID(&gf->gf_fid), gf->gf_recno,
 	       gf->gf_linkno);
@@ -693,7 +693,7 @@ repeat_fid2path:
 	}
 
 	if (!fid_is_sane(&gf->gf_fid)) {
-		CERROR("%s: invalid FID "DFID": rc = %d\n",
+		CERROR("%s: invalid FID " DFID ": rc = %d\n",
 		       tgt->ltd_exp->exp_obd->obd_name,
 		       PFID(&gf->gf_fid), -EINVAL);
 		rc = -EINVAL;
@@ -1508,7 +1508,7 @@ static int lmv_null_inode(struct obd_export *exp, const struct lu_fid *fid)
 	if (rc)
 		return rc;
 
-	CDEBUG(D_INODE, "CBDATA for "DFID"\n", PFID(fid));
+	CDEBUG(D_INODE, "CBDATA for " DFID "\n", PFID(fid));
 
 	/*
 	 * With DNE every object can have two locks in different namespaces:
@@ -1540,7 +1540,7 @@ static int lmv_close(struct obd_export *exp, struct md_op_data *op_data,
 	if (IS_ERR(tgt))
 		return PTR_ERR(tgt);
 
-	CDEBUG(D_INODE, "CLOSE "DFID"\n", PFID(&op_data->op_fid1));
+	CDEBUG(D_INODE, "CLOSE " DFID "\n", PFID(&op_data->op_fid1));
 	return md_close(tgt->ltd_exp, op_data, mod, request);
 }
 
@@ -1672,7 +1672,7 @@ static int lmv_create(struct obd_export *exp, struct md_op_data *op_data,
 	if (IS_ERR(tgt))
 		return PTR_ERR(tgt);
 
-	CDEBUG(D_INODE, "CREATE name '%.*s' on "DFID" -> mds #%x\n",
+	CDEBUG(D_INODE, "CREATE name '%.*s' on " DFID " -> mds #%x\n",
 	       (int)op_data->op_namelen, op_data->op_name,
 	       PFID(&op_data->op_fid1), op_data->op_mds);
 
@@ -1694,7 +1694,7 @@ static int lmv_create(struct obd_export *exp, struct md_op_data *op_data,
 		CDEBUG(D_CONFIG, "Server doesn't support striped dirs\n");
 	}
 
-	CDEBUG(D_INODE, "CREATE obj "DFID" -> mds #%x\n",
+	CDEBUG(D_INODE, "CREATE obj " DFID " -> mds #%x\n",
 	       PFID(&op_data->op_fid1), op_data->op_mds);
 
 	op_data->op_flags |= MF_MDC_CANCEL_FID1;
@@ -1704,7 +1704,7 @@ static int lmv_create(struct obd_export *exp, struct md_op_data *op_data,
 	if (rc == 0) {
 		if (!*request)
 			return rc;
-		CDEBUG(D_INODE, "Created - "DFID"\n", PFID(&op_data->op_fid2));
+		CDEBUG(D_INODE, "Created - " DFID "\n", PFID(&op_data->op_fid2));
 	}
 	return rc;
 }
@@ -1724,7 +1724,7 @@ lmv_enqueue(struct obd_export *exp, struct ldlm_enqueue_info *einfo,
 	if (rc)
 		return rc;
 
-	CDEBUG(D_INODE, "ENQUEUE '%s' on "DFID"\n",
+	CDEBUG(D_INODE, "ENQUEUE '%s' on " DFID "\n",
 	       LL_IT2STR(it), PFID(&op_data->op_fid1));
 
 	tgt = lmv_locate_mds(lmv, op_data, &op_data->op_fid1);
@@ -1769,7 +1769,7 @@ lmv_getattr_name(struct obd_export *exp, struct md_op_data *op_data,
 	if (body->mbo_valid & OBD_MD_MDS) {
 		struct lu_fid rid = body->mbo_fid1;
 
-		CDEBUG(D_INODE, "Request attrs for "DFID"\n",
+		CDEBUG(D_INODE, "Request attrs for " DFID "\n",
 		       PFID(&rid));
 
 		tgt = lmv_find_target(lmv, &rid);
@@ -1818,13 +1818,13 @@ static int lmv_early_cancel(struct obd_export *exp, struct lmv_tgt_desc *tgt,
 	}
 
 	if (tgt->ltd_idx != op_tgt) {
-		CDEBUG(D_INODE, "EARLY_CANCEL on "DFID"\n", PFID(fid));
+		CDEBUG(D_INODE, "EARLY_CANCEL on " DFID "\n", PFID(fid));
 		policy.l_inodebits.bits = bits;
 		rc = md_cancel_unused(tgt->ltd_exp, fid, &policy,
 				      mode, LCF_ASYNC, NULL);
 	} else {
 		CDEBUG(D_INODE,
-		       "EARLY_CANCEL skip operation target %d on "DFID"\n",
+		       "EARLY_CANCEL skip operation target %d on " DFID "\n",
 		       op_tgt, PFID(fid));
 		op_data->op_flags |= flag;
 		rc = 0;
@@ -1851,7 +1851,7 @@ static int lmv_link(struct obd_export *exp, struct md_op_data *op_data,
 
 	LASSERT(op_data->op_namelen != 0);
 
-	CDEBUG(D_INODE, "LINK "DFID":%*s to "DFID"\n",
+	CDEBUG(D_INODE, "LINK " DFID ":%*s to " DFID "\n",
 	       PFID(&op_data->op_fid2), (int)op_data->op_namelen,
 	       op_data->op_name, PFID(&op_data->op_fid1));
 
@@ -1901,7 +1901,7 @@ static int lmv_rename(struct obd_export *exp, struct md_op_data *op_data,
 
 	LASSERT(oldlen != 0);
 
-	CDEBUG(D_INODE, "RENAME %.*s in "DFID":%d to %.*s in "DFID":%d\n",
+	CDEBUG(D_INODE, "RENAME %.*s in " DFID ":%d to %.*s in " DFID ":%d\n",
 	       (int)oldlen, old, PFID(&op_data->op_fid1),
 	       op_data->op_mea1 ? op_data->op_mea1->lsm_md_stripe_count : 0,
 	       (int)newlen, new, PFID(&op_data->op_fid2),
@@ -1916,7 +1916,7 @@ static int lmv_rename(struct obd_export *exp, struct md_op_data *op_data,
 	op_data->op_cap = cfs_curproc_cap_pack();
 
 	if (op_data->op_cli_flags & CLI_MIGRATE) {
-		LASSERTF(fid_is_sane(&op_data->op_fid3), "invalid FID "DFID"\n",
+		LASSERTF(fid_is_sane(&op_data->op_fid3), "invalid FID " DFID "\n",
 			 PFID(&op_data->op_fid3));
 
 		if (op_data->op_mea1) {
@@ -2069,7 +2069,7 @@ static int lmv_setattr(struct obd_export *exp, struct md_op_data *op_data,
 	if (rc)
 		return rc;
 
-	CDEBUG(D_INODE, "SETATTR for "DFID", valid 0x%x\n",
+	CDEBUG(D_INODE, "SETATTR for " DFID ", valid 0x%x\n",
 	       PFID(&op_data->op_fid1), op_data->op_attr.ia_valid);
 
 	op_data->op_flags |= MF_MDC_CANCEL_FID1;
@@ -2274,6 +2274,7 @@ static int lmv_read_striped_page(struct obd_export *exp,
 	struct lu_fid master_fid = op_data->op_fid1;
 	struct obd_device *obd = exp->exp_obd;
 	__u64 hash_offset = offset;
+	__u32 ldp_flags;
 	struct page *min_ent_page = NULL;
 	struct page *ent_page = NULL;
 	struct lu_dirent *min_ent = NULL;
@@ -2301,7 +2302,7 @@ static int lmv_read_striped_page(struct obd_export *exp,
 	dp = kmap(ent_page);
 	memset(dp, 0, sizeof(*dp));
 	dp->ldp_hash_start = cpu_to_le64(offset);
-	dp->ldp_flags |= LDF_COLLIDE;
+	ldp_flags = LDF_COLLIDE;
 
 	area = dp + 1;
 	left_bytes = PAGE_SIZE - sizeof(*dp);
@@ -2379,8 +2380,8 @@ out:
 		ent_page = NULL;
 	} else {
 		if (ent == area)
-			dp->ldp_flags |= LDF_EMPTY;
-		dp->ldp_flags = cpu_to_le32(dp->ldp_flags);
+			ldp_flags |= LDF_EMPTY;
+		dp->ldp_flags |= cpu_to_le32(ldp_flags);
 		dp->ldp_hash_end = cpu_to_le64(hash_offset);
 	}
 
@@ -2413,9 +2414,8 @@ static int lmv_read_page(struct obd_export *exp, struct md_op_data *op_data,
 	if (rc)
 		return rc;
 
-	if (unlikely(lsm)) {
+	if (unlikely(lsm))
 		return lmv_read_striped_page(exp, op_data, cb_op, offset, ppage);
-	}
 
 	tgt = lmv_find_target(lmv, &op_data->op_fid1);
 	if (IS_ERR(tgt))
@@ -2577,7 +2577,7 @@ try_next_stripe:
 	if (likely(!(body->mbo_valid & OBD_MD_MDS)))
 		return rc;
 
-	CDEBUG(D_INODE, "%s: try unlink to another MDT for "DFID"\n",
+	CDEBUG(D_INODE, "%s: try unlink to another MDT for " DFID "\n",
 	       exp->exp_obd->obd_name, PFID(&body->mbo_fid1));
 
 	/* This is a remote object, try remote MDT, Note: it may
@@ -2781,7 +2781,7 @@ static int lmv_unpack_md_v1(struct obd_export *exp, struct lmv_stripe_md *lsm,
 				    &lsm->lsm_md_oinfo[i].lmo_mds);
 		if (rc)
 			return rc;
-		CDEBUG(D_INFO, "unpack fid #%d "DFID"\n", i,
+		CDEBUG(D_INFO, "unpack fid #%d " DFID "\n", i,
 		       PFID(&lsm->lsm_md_oinfo[i].lmo_fid));
 	}
 
@@ -2925,7 +2925,7 @@ static enum ldlm_mode lmv_lock_match(struct obd_export *exp, __u64 flags,
 	int tgt;
 	u32 i;
 
-	CDEBUG(D_INODE, "Lock match for "DFID"\n", PFID(fid));
+	CDEBUG(D_INODE, "Lock match for " DFID "\n", PFID(fid));
 
 	/*
 	 * With DNE every object can have two locks in different namespaces:
@@ -2937,7 +2937,7 @@ static enum ldlm_mode lmv_lock_match(struct obd_export *exp, __u64 flags,
 	     i < lmv->desc.ld_tgt_count;
 	     i++, tgt = (tgt + 1) % lmv->desc.ld_tgt_count) {
 		if (tgt < 0) {
-			CDEBUG(D_HA, "%s: "DFID" is inaccessible: rc = %d\n",
+			CDEBUG(D_HA, "%s: " DFID " is inaccessible: rc = %d\n",
 			       obd->obd_name, PFID(fid), tgt);
 			tgt = 0;
 		}
@@ -3107,9 +3107,8 @@ static int lmv_quotactl(struct obd_device *unused, struct obd_export *exp,
 		return -EIO;
 	}
 
-	if (oqctl->qc_cmd != Q_GETOQUOTA) {
+	if (oqctl->qc_cmd != Q_GETOQUOTA)
 		return obd_quotactl(tgt->ltd_exp, oqctl);
-	}
 
 	for (i = 0; i < lmv->desc.ld_tgt_count; i++) {
 		int err;
@@ -3149,7 +3148,7 @@ static int lmv_merge_attr(struct obd_export *exp,
 	for (i = 0; i < lsm->lsm_md_stripe_count; i++) {
 		struct inode *inode = lsm->lsm_md_oinfo[i].lmo_root;
 
-		CDEBUG(D_INFO, ""DFID" size %llu, blocks %llu nlink %u, atime %lu ctime %lu, mtime %lu.\n",
+		CDEBUG(D_INFO, "" DFID " size %llu, blocks %llu nlink %u, atime %lu ctime %lu, mtime %lu.\n",
 		       PFID(&lsm->lsm_md_oinfo[i].lmo_fid),
 		       i_size_read(inode), (unsigned long long)inode->i_blocks,
 		       inode->i_nlink, LTIME_S(inode->i_atime),
diff --git a/drivers/staging/lustre/lustre/lov/lov_cl_internal.h b/drivers/staging/lustre/lustre/lov/lov_cl_internal.h
index 391c632365ae..e889d3a7de9c 100644
--- a/drivers/staging/lustre/lustre/lov/lov_cl_internal.h
+++ b/drivers/staging/lustre/lustre/lov/lov_cl_internal.h
@@ -370,7 +370,7 @@ struct lov_thread_info {
 	struct ost_lvb	  lti_lvb;
 	struct cl_2queue	lti_cl2q;
 	struct cl_page_list     lti_plist;
-	wait_queue_t	  lti_waiter;
+	wait_queue_entry_t	  lti_waiter;
 	struct cl_attr          lti_attr;
 };
 
diff --git a/drivers/staging/lustre/lustre/lov/lov_io.c b/drivers/staging/lustre/lustre/lov/lov_io.c
index df77b2586612..babf39adef85 100644
--- a/drivers/staging/lustre/lustre/lov/lov_io.c
+++ b/drivers/staging/lustre/lustre/lov/lov_io.c
@@ -1021,7 +1021,7 @@ int lov_io_init_empty(const struct lu_env *env, struct cl_object *obj,
 		break;
 	case CIT_FAULT:
 		result = -EFAULT;
-		CERROR("Page fault on a file without stripes: "DFID"\n",
+		CERROR("Page fault on a file without stripes: " DFID "\n",
 		       PFID(lu_object_fid(&obj->co_lu)));
 		break;
 	}
diff --git a/drivers/staging/lustre/lustre/lov/lov_merge.c b/drivers/staging/lustre/lustre/lov/lov_merge.c
index 391dfd207177..034b4fcb38f5 100644
--- a/drivers/staging/lustre/lustre/lov/lov_merge.c
+++ b/drivers/staging/lustre/lustre/lov/lov_merge.c
@@ -57,7 +57,7 @@ int lov_merge_lvb_kms(struct lov_stripe_md *lsm,
 	assert_spin_locked(&lsm->lsm_lock);
 	LASSERT(lsm->lsm_lock_owner == current_pid());
 
-	CDEBUG(D_INODE, "MDT ID "DOSTID" initial value: s=%llu m=%llu a=%llu c=%llu b=%llu\n",
+	CDEBUG(D_INODE, "MDT ID " DOSTID " initial value: s=%llu m=%llu a=%llu c=%llu b=%llu\n",
 	       POSTID(&lsm->lsm_oi), lvb->lvb_size, lvb->lvb_mtime,
 	       lvb->lvb_atime, lvb->lvb_ctime, lvb->lvb_blocks);
 	for (i = 0; i < lsm->lsm_stripe_count; i++) {
@@ -89,7 +89,7 @@ int lov_merge_lvb_kms(struct lov_stripe_md *lsm,
 		if (loi->loi_lvb.lvb_ctime > current_ctime)
 			current_ctime = loi->loi_lvb.lvb_ctime;
 
-		CDEBUG(D_INODE, "MDT ID "DOSTID" on OST[%u]: s=%llu m=%llu a=%llu c=%llu b=%llu\n",
+		CDEBUG(D_INODE, "MDT ID " DOSTID " on OST[%u]: s=%llu m=%llu a=%llu c=%llu b=%llu\n",
 		       POSTID(&lsm->lsm_oi), loi->loi_ost_idx,
 		       loi->loi_lvb.lvb_size, loi->loi_lvb.lvb_mtime,
 		       loi->loi_lvb.lvb_atime, loi->loi_lvb.lvb_ctime,
diff --git a/drivers/staging/lustre/lustre/lov/lov_object.c b/drivers/staging/lustre/lustre/lov/lov_object.c
index ab3ecfeeadc8..14f38268d414 100644
--- a/drivers/staging/lustre/lustre/lov/lov_object.c
+++ b/drivers/staging/lustre/lustre/lov/lov_object.c
@@ -153,8 +153,7 @@ static int lov_init_sub(const struct lu_env *env, struct lov_object *lov,
 	subhdr = cl_object_header(stripe);
 
 	oinfo = lov->lo_lsm->lsm_oinfo[idx];
-	CDEBUG(D_INODE, DFID"@%p[%d] -> "DFID"@%p: ostid: "DOSTID
-	       " idx: %d gen: %d\n",
+	CDEBUG(D_INODE, DFID "@%p[%d] -> " DFID "@%p: ostid: " DOSTID " idx: %d gen: %d\n",
 	       PFID(&subhdr->coh_lu.loh_fid), subhdr, idx,
 	       PFID(&hdr->coh_lu.loh_fid), hdr, POSTID(&oinfo->loi_oi),
 	       oinfo->loi_ost_idx, oinfo->loi_ost_gen);
@@ -371,7 +370,7 @@ static void lov_subobject_kill(const struct lu_env *env, struct lov_object *lov,
 	struct lov_layout_raid0 *r0;
 	struct lu_site	  *site;
 	struct lu_site_bkt_data *bkt;
-	wait_queue_t	  *waiter;
+	wait_queue_entry_t	  *waiter;
 
 	r0  = &lov->u.raid0;
 	LASSERT(r0->lo_sub[idx] == los);
@@ -757,7 +756,7 @@ static int lov_layout_change(const struct lu_env *unused,
 
 	LASSERT(0 <= llt && llt < ARRAY_SIZE(lov_dispatch));
 
-	CDEBUG(D_INODE, DFID" from %s to %s\n",
+	CDEBUG(D_INODE, DFID " from %s to %s\n",
 	       PFID(lu_object_fid(lov2lu(lov))),
 	       llt2str(lov->lo_type), llt2str(llt));
 
@@ -904,7 +903,7 @@ static int lov_conf_set(const struct lu_env *env, struct cl_object *obj,
 out:
 	lov_conf_unlock(lov);
 	lov_lsm_put(lsm);
-	CDEBUG(D_INODE, DFID" lo_layout_invalid=%d\n",
+	CDEBUG(D_INODE, DFID " lo_layout_invalid=%d\n",
 	       PFID(lu_object_fid(lov2lu(lov))), lov->lo_layout_invalid);
 	return result;
 }
diff --git a/drivers/staging/lustre/lustre/lov/lov_pack.c b/drivers/staging/lustre/lustre/lov/lov_pack.c
index e6727cefde05..638b7646ca2c 100644
--- a/drivers/staging/lustre/lustre/lov/lov_pack.c
+++ b/drivers/staging/lustre/lustre/lov/lov_pack.c
@@ -56,7 +56,7 @@ void lov_dump_lmm_common(int level, void *lmmp)
 	struct ost_id	oi;
 
 	lmm_oi_le_to_cpu(&oi, &lmm->lmm_oi);
-	CDEBUG(level, "objid "DOSTID", magic 0x%08x, pattern %#x\n",
+	CDEBUG(level, "objid " DOSTID ", magic 0x%08x, pattern %#x\n",
 	       POSTID(&oi), le32_to_cpu(lmm->lmm_magic),
 	       le32_to_cpu(lmm->lmm_pattern));
 	CDEBUG(level, "stripe_size %u, stripe_count %u, layout_gen %u\n",
@@ -80,7 +80,7 @@ static void lov_dump_lmm_objects(int level, struct lov_ost_data *lod,
 		struct ost_id	oi;
 
 		ostid_le_to_cpu(&lod->l_ost_oi, &oi);
-		CDEBUG(level, "stripe %u idx %u subobj "DOSTID"\n", i,
+		CDEBUG(level, "stripe %u idx %u subobj " DOSTID "\n", i,
 		       le32_to_cpu(lod->l_ost_idx), POSTID(&oi));
 	}
 }
@@ -95,7 +95,7 @@ void lov_dump_lmm_v1(int level, struct lov_mds_md_v1 *lmm)
 void lov_dump_lmm_v3(int level, struct lov_mds_md_v3 *lmm)
 {
 	lov_dump_lmm_common(level, lmm);
-	CDEBUG(level, "pool_name "LOV_POOLNAMEF"\n", lmm->lmm_pool_name);
+	CDEBUG(level, "pool_name " LOV_POOLNAMEF "\n", lmm->lmm_pool_name);
 	lov_dump_lmm_objects(level, lmm->lmm_objects,
 			     le16_to_cpu(lmm->lmm_stripe_count));
 }
diff --git a/drivers/staging/lustre/lustre/lov/lov_pool.c b/drivers/staging/lustre/lustre/lov/lov_pool.c
index 7daa8671fdc3..39daa17e0736 100644
--- a/drivers/staging/lustre/lustre/lov/lov_pool.c
+++ b/drivers/staging/lustre/lustre/lov/lov_pool.c
@@ -286,7 +286,7 @@ static int pool_proc_open(struct inode *inode, struct file *file)
 	return rc;
 }
 
-static struct file_operations pool_proc_operations = {
+static const struct file_operations pool_proc_operations = {
 	.open	   = pool_proc_open,
 	.read	   = seq_read,
 	.llseek	 = seq_lseek,
@@ -429,7 +429,7 @@ int lov_pool_new(struct obd_device *obd, char *poolname)
 						poolname, new_pool,
 						&pool_proc_operations);
 	if (IS_ERR_OR_NULL(new_pool->pool_debugfs_entry)) {
-		CWARN("Cannot add debugfs pool entry "LOV_POOLNAMEF"\n",
+		CWARN("Cannot add debugfs pool entry " LOV_POOLNAMEF "\n",
 		      poolname);
 		new_pool->pool_debugfs_entry = NULL;
 		lov_pool_putref(new_pool);
@@ -450,7 +450,7 @@ int lov_pool_new(struct obd_device *obd, char *poolname)
 		goto out_err;
 	}
 
-	CDEBUG(D_CONFIG, LOV_POOLNAMEF" is pool #%d\n",
+	CDEBUG(D_CONFIG, LOV_POOLNAMEF " is pool #%d\n",
 	       poolname, lov->lov_pool_count);
 
 	return 0;
@@ -531,7 +531,7 @@ int lov_pool_add(struct obd_device *obd, char *poolname, char *ostname)
 	if (rc)
 		goto out;
 
-	CDEBUG(D_CONFIG, "Added %s to "LOV_POOLNAMEF" as member %d\n",
+	CDEBUG(D_CONFIG, "Added %s to " LOV_POOLNAMEF " as member %d\n",
 	       ostname, poolname,  pool_tgt_count(pool));
 
 out:
@@ -575,7 +575,7 @@ int lov_pool_remove(struct obd_device *obd, char *poolname, char *ostname)
 
 	lov_ost_pool_remove(&pool->pool_obds, lov_idx);
 
-	CDEBUG(D_CONFIG, "%s removed from "LOV_POOLNAMEF"\n", ostname,
+	CDEBUG(D_CONFIG, "%s removed from " LOV_POOLNAMEF "\n", ostname,
 	       poolname);
 
 out:
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_locks.c b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
index 392b0e38a91e..3eb66cea65db 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_locks.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
@@ -830,7 +830,7 @@ resend:
 		ptlrpc_req_finished(req);
 		resends++;
 
-		CDEBUG(D_HA, "%s: resend:%d op:%d "DFID"/"DFID"\n",
+		CDEBUG(D_HA, "%s: resend:%d op:%d " DFID "/" DFID "\n",
 		       obddev->obd_name, resends, it->it_op,
 		       PFID(&op_data->op_fid1), PFID(&op_data->op_fid2));
 
@@ -911,13 +911,13 @@ static int mdc_finish_intent_lock(struct obd_export *exp,
 		OBD_FAIL_TIMEOUT(OBD_FAIL_MDC_ENQUEUE_PAUSE, obd_timeout);
 	}
 
-	if (it->it_op & IT_CREAT) {
+	if (it->it_op & IT_CREAT)
 		/* XXX this belongs in ll_create_it */
-	} else if (it->it_op == IT_OPEN) {
+		;
+	else if (it->it_op == IT_OPEN)
 		LASSERT(!it_disposition(it, DISP_OPEN_CREATE));
-	} else {
+	else
 		LASSERT(it->it_op & (IT_GETATTR | IT_LOOKUP | IT_LAYOUT));
-	}
 
 	/* If we already have a matching lock, then cancel the new
 	 * one.  We have to set the data here instead of in
@@ -933,7 +933,7 @@ static int mdc_finish_intent_lock(struct obd_export *exp,
 
 		LASSERTF(fid_res_name_eq(&mdt_body->mbo_fid1,
 					 &lock->l_resource->lr_name),
-			 "Lock res_id: "DLDLMRES", fid: "DFID"\n",
+			 "Lock res_id: " DLDLMRES ", fid: " DFID "\n",
 			 PLDLMRES(lock->l_resource), PFID(&mdt_body->mbo_fid1));
 		LDLM_LOCK_PUT(lock);
 
@@ -1063,7 +1063,7 @@ int mdc_intent_lock(struct obd_export *exp, struct md_op_data *op_data,
 
 	LASSERT(it);
 
-	CDEBUG(D_DLMTRACE, "(name: %.*s,"DFID") in obj "DFID
+	CDEBUG(D_DLMTRACE, "(name: %.*s," DFID ") in obj " DFID
 		", intent: %s flags %#Lo\n", (int)op_data->op_namelen,
 		op_data->op_name, PFID(&op_data->op_fid2),
 		PFID(&op_data->op_fid1), ldlm_it2str(it->it_op),
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_reint.c b/drivers/staging/lustre/lustre/mdc/mdc_reint.c
index 07b168490f09..2287bd46d527 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_reint.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_reint.c
@@ -227,7 +227,7 @@ rebuild:
 		ptlrpc_req_finished(req);
 		resends++;
 
-		CDEBUG(D_HA, "%s: resend:%d create on "DFID"/"DFID"\n",
+		CDEBUG(D_HA, "%s: resend:%d create on " DFID "/" DFID "\n",
 		       exp->exp_obd->obd_name, resends,
 		       PFID(&op_data->op_fid1), PFID(&op_data->op_fid2));
 
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_request.c b/drivers/staging/lustre/lustre/mdc/mdc_request.c
index 6bc2fb858680..1a3fa1bb7f25 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_request.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_request.c
@@ -105,7 +105,7 @@ static int mdc_getstatus(struct obd_export *exp, struct lu_fid *rootfid)
 
 	*rootfid = body->mbo_fid1;
 	CDEBUG(D_NET,
-	       "root fid="DFID", last_committed=%llu\n",
+	       "root fid=" DFID ", last_committed=%llu\n",
 	       PFID(rootfid),
 	       lustre_msg_get_last_committed(req->rq_repmsg));
 out:
@@ -713,7 +713,7 @@ static int mdc_close(struct obd_export *exp, struct md_op_data *op_data,
 		/* allocate a FID for volatile file */
 		rc = mdc_fid_alloc(NULL, exp, &op_data->op_fid2, op_data);
 		if (rc < 0) {
-			CERROR("%s: "DFID" failed to allocate FID: %d\n",
+			CERROR("%s: " DFID " failed to allocate FID: %d\n",
 			       obd->obd_name, PFID(&op_data->op_fid1), rc);
 			/* save the errcode and proceed to close */
 			saved_rc = rc;
@@ -753,7 +753,7 @@ static int mdc_close(struct obd_export *exp, struct md_op_data *op_data,
 		/*
 		 * TODO: repeat close after errors
 		 */
-		CWARN("%s: close of FID "DFID" failed, file reference will be dropped when this client unmounts or is evicted\n",
+		CWARN("%s: close of FID " DFID " failed, file reference will be dropped when this client unmounts or is evicted\n",
 		      obd->obd_name, PFID(&op_data->op_fid1));
 		rc = -ENOMEM;
 		goto out;
@@ -1254,7 +1254,7 @@ static int mdc_read_page(struct obd_export *exp, struct md_op_data *op_data,
 		ptlrpc_req_finished(enq_req);
 
 	if (rc < 0) {
-		CERROR("%s: "DFID" lock enqueue fails: rc = %d\n",
+		CERROR("%s: " DFID " lock enqueue fails: rc = %d\n",
 		       exp->exp_obd->obd_name, PFID(&op_data->op_fid1), rc);
 		return rc;
 	}
@@ -1298,7 +1298,7 @@ static int mdc_read_page(struct obd_export *exp, struct md_op_data *op_data,
 					    rp_param.rp_hash64),
 			       mdc_read_page_remote, &rp_param);
 	if (IS_ERR(page)) {
-		CERROR("%s: read cache page: "DFID" at %llu: rc %ld\n",
+		CERROR("%s: read cache page: " DFID " at %llu: rc %ld\n",
 		       exp->exp_obd->obd_name, PFID(&op_data->op_fid1),
 		       rp_param.rp_off, PTR_ERR(page));
 		rc = PTR_ERR(page);
@@ -1308,7 +1308,7 @@ static int mdc_read_page(struct obd_export *exp, struct md_op_data *op_data,
 	wait_on_page_locked(page);
 	(void)kmap(page);
 	if (!PageUptodate(page)) {
-		CERROR("%s: page not updated: "DFID" at %llu: rc %d\n",
+		CERROR("%s: page not updated: " DFID " at %llu: rc %d\n",
 		       exp->exp_obd->obd_name, PFID(&op_data->op_fid1),
 		       rp_param.rp_off, -5);
 		goto fail;
@@ -1316,7 +1316,7 @@ static int mdc_read_page(struct obd_export *exp, struct md_op_data *op_data,
 	if (!PageChecked(page))
 		SetPageChecked(page);
 	if (PageError(page)) {
-		CERROR("%s: page error: "DFID" at %llu: rc %d\n",
+		CERROR("%s: page error: " DFID " at %llu: rc %d\n",
 		       exp->exp_obd->obd_name, PFID(&op_data->op_fid1),
 		       rp_param.rp_off, -5);
 		goto fail;
@@ -1436,7 +1436,7 @@ static int mdc_ioc_fid2path(struct obd_export *exp, struct getinfo_fid2path *gf)
 	memcpy(key, KEY_FID2PATH, sizeof(KEY_FID2PATH));
 	memcpy(key + cfs_size_round(sizeof(KEY_FID2PATH)), gf, sizeof(*gf));
 
-	CDEBUG(D_IOCTL, "path get "DFID" from %llu #%d\n",
+	CDEBUG(D_IOCTL, "path get " DFID " from %llu #%d\n",
 	       PFID(&gf->gf_fid), gf->gf_recno, gf->gf_linkno);
 
 	if (!fid_is_sane(&gf->gf_fid)) {
diff --git a/drivers/staging/lustre/lustre/mgc/mgc_request.c b/drivers/staging/lustre/lustre/mgc/mgc_request.c
index 6a76605b3c3d..eee0b667a33c 100644
--- a/drivers/staging/lustre/lustre/mgc/mgc_request.c
+++ b/drivers/staging/lustre/lustre/mgc/mgc_request.c
@@ -800,7 +800,7 @@ static int mgc_blocking_ast(struct ldlm_lock *lock, struct ldlm_lock_desc *desc,
 		/* We've given up the lock, prepare ourselves to update. */
 		LDLM_DEBUG(lock, "MGC cancel CB");
 
-		CDEBUG(D_MGC, "Lock res "DLDLMRES" (%.8s)\n",
+		CDEBUG(D_MGC, "Lock res " DLDLMRES " (%.8s)\n",
 		       PLDLMRES(lock->l_resource),
 		       (char *)&lock->l_resource->lr_name.name[0]);
 
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_lock.c b/drivers/staging/lustre/lustre/obdclass/cl_lock.c
index 9d7b5939b0fd..a343e3ab2257 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_lock.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_lock.c
@@ -246,7 +246,7 @@ void cl_lock_descr_print(const struct lu_env *env, void *cookie,
 	const struct lu_fid  *fid;
 
 	fid = lu_object_fid(&descr->cld_obj->co_lu);
-	(*printer)(env, cookie, DDESCR"@"DFID, PDESCR(descr), PFID(fid));
+	(*printer)(env, cookie, DDESCR "@" DFID, PDESCR(descr), PFID(fid));
 }
 EXPORT_SYMBOL(cl_lock_descr_print);
 
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_page.c b/drivers/staging/lustre/lustre/obdclass/cl_page.c
index 71fcc4cc9e72..6b8c41b6f379 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_page.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_page.c
@@ -193,7 +193,7 @@ struct cl_page *cl_page_find(const struct lu_env *env,
 
 	hdr = cl_object_header(o);
 
-	CDEBUG(D_PAGE, "%lu@"DFID" %p %lx %d\n",
+	CDEBUG(D_PAGE, "%lu@" DFID " %p %lx %d\n",
 	       idx, PFID(&hdr->coh_lu.loh_fid), vmpage, vmpage->private, type);
 	/* fast path. */
 	if (type == CPT_CACHEABLE) {
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_cat.c b/drivers/staging/lustre/lustre/obdclass/llog_cat.c
index ce8e2f6f002a..8f1533c127a8 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_cat.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_cat.c
@@ -78,7 +78,7 @@ static int llog_cat_id2handle(const struct lu_env *env,
 		if (ostid_id(&cgl->lgl_oi) == ostid_id(&logid->lgl_oi) &&
 		    ostid_seq(&cgl->lgl_oi) == ostid_seq(&logid->lgl_oi)) {
 			if (cgl->lgl_ogen != logid->lgl_ogen) {
-				CERROR("%s: log "DOSTID" generation %x != %x\n",
+				CERROR("%s: log " DOSTID " generation %x != %x\n",
 				       loghandle->lgh_ctxt->loc_obd->obd_name,
 				       POSTID(&logid->lgl_oi), cgl->lgl_ogen,
 				       logid->lgl_ogen);
@@ -95,7 +95,7 @@ static int llog_cat_id2handle(const struct lu_env *env,
 	rc = llog_open(env, cathandle->lgh_ctxt, &loghandle, logid, NULL,
 		       LLOG_OPEN_EXISTS);
 	if (rc < 0) {
-		CERROR("%s: error opening log id "DOSTID":%x: rc = %d\n",
+		CERROR("%s: error opening log id " DOSTID ":%x: rc = %d\n",
 		       cathandle->lgh_ctxt->loc_obd->obd_name,
 		       POSTID(&logid->lgl_oi), logid->lgl_ogen, rc);
 		return rc;
@@ -152,13 +152,13 @@ static int llog_cat_process_cb(const struct lu_env *env,
 		CERROR("invalid record in catalog\n");
 		return -EINVAL;
 	}
-	CDEBUG(D_HA, "processing log "DOSTID":%x at index %u of catalog "
-	       DOSTID"\n", POSTID(&lir->lid_id.lgl_oi), lir->lid_id.lgl_ogen,
+	CDEBUG(D_HA, "processing log " DOSTID ":%x at index %u of catalog "
+	       DOSTID "\n", POSTID(&lir->lid_id.lgl_oi), lir->lid_id.lgl_ogen,
 	       rec->lrh_index, POSTID(&cat_llh->lgh_id.lgl_oi));
 
 	rc = llog_cat_id2handle(env, cat_llh, &llh, &lir->lid_id);
 	if (rc) {
-		CERROR("%s: cannot find handle for llog "DOSTID": %d\n",
+		CERROR("%s: cannot find handle for llog " DOSTID ": %d\n",
 		       cat_llh->lgh_ctxt->loc_obd->obd_name,
 		       POSTID(&lir->lid_id.lgl_oi), rc);
 		return rc;
@@ -204,7 +204,7 @@ static int llog_cat_process_or_fork(const struct lu_env *env,
 	if (llh->llh_cat_idx > cat_llh->lgh_last_idx) {
 		struct llog_process_cat_data cd;
 
-		CWARN("catlog "DOSTID" crosses index zero\n",
+		CWARN("catlog " DOSTID " crosses index zero\n",
 		      POSTID(&cat_llh->lgh_id.lgl_oi));
 
 		cd.lpcd_first_idx = llh->llh_cat_idx;
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_swab.c b/drivers/staging/lustre/lustre/obdclass/llog_swab.c
index 723c212c6747..016046d26010 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_swab.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_swab.c
@@ -44,7 +44,7 @@
 static void print_llogd_body(struct llogd_body *d)
 {
 	CDEBUG(D_OTHER, "llogd body: %p\n", d);
-	CDEBUG(D_OTHER, "\tlgd_logid.lgl_oi: "DOSTID"\n",
+	CDEBUG(D_OTHER, "\tlgd_logid.lgl_oi: " DOSTID "\n",
 	       POSTID(&d->lgd_logid.lgl_oi));
 	CDEBUG(D_OTHER, "\tlgd_logid.lgl_ogen: %#x\n", d->lgd_logid.lgl_ogen);
 	CDEBUG(D_OTHER, "\tlgd_ctxt_idx: %#x\n", d->lgd_ctxt_idx);
diff --git a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
index 1ec6e3767d81..bc19f19d38d9 100644
--- a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
+++ b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
@@ -301,7 +301,7 @@ EXPORT_SYMBOL(lprocfs_seq_release);
 
 struct dentry *ldebugfs_add_simple(struct dentry *root,
 				   char *name, void *data,
-				   struct file_operations *fops)
+				   const struct file_operations *fops)
 {
 	struct dentry *entry;
 	umode_t mode = 0;
@@ -389,40 +389,6 @@ out:
 EXPORT_SYMBOL_GPL(ldebugfs_register);
 
 /* Generic callbacks */
-int lprocfs_rd_uint(struct seq_file *m, void *data)
-{
-	seq_printf(m, "%u\n", *(unsigned int *)data);
-	return 0;
-}
-EXPORT_SYMBOL(lprocfs_rd_uint);
-
-int lprocfs_wr_uint(struct file *file, const char __user *buffer,
-		    unsigned long count, void *data)
-{
-	unsigned *p = data;
-	char dummy[MAX_STRING_SIZE + 1], *end;
-	unsigned long tmp;
-
-	if (count >= sizeof(dummy))
-		return -EINVAL;
-
-	if (count == 0)
-		return 0;
-
-	if (copy_from_user(dummy, buffer, count))
-		return -EFAULT;
-
-	dummy[count] = '\0';
-
-	tmp = simple_strtoul(dummy, &end, 0);
-	if (dummy == end)
-		return -EINVAL;
-
-	*p = (unsigned int)tmp;
-	return count;
-}
-EXPORT_SYMBOL(lprocfs_wr_uint);
-
 static ssize_t uuid_show(struct kobject *kobj, struct attribute *attr,
 			 char *buf)
 {
@@ -736,7 +702,7 @@ static int obd_import_flags2str(struct obd_import *imp, struct seq_file *m)
 	bool first = true;
 
 	if (imp->imp_obd->obd_no_recov) {
-		seq_printf(m, "no_recov");
+		seq_puts(m, "no_recov");
 		first = false;
 	}
 
@@ -802,15 +768,15 @@ int lprocfs_rd_import(struct seq_file *m, void *data)
 		   imp->imp_connect_data.ocd_instance);
 	obd_connect_seq_flags2str(m, imp->imp_connect_data.ocd_connect_flags,
 				  ", ");
-	seq_printf(m, " ]\n");
+	seq_puts(m, " ]\n");
 	obd_connect_data_seqprint(m, ocd);
-	seq_printf(m, "    import_flags: [ ");
+	seq_puts(m, "    import_flags: [ ");
 	obd_import_flags2str(imp, m);
 
-	seq_printf(m,
-		   " ]\n"
-		   "    connection:\n"
-		   "       failover_nids: [ ");
+	seq_puts(m,
+		 " ]\n"
+		 "    connection:\n"
+		 "       failover_nids: [ ");
 	spin_lock(&imp->imp_lock);
 	j = 0;
 	list_for_each_entry(conn, &imp->imp_conn_list, oic_item) {
@@ -943,7 +909,7 @@ int lprocfs_rd_state(struct seq_file *m, void *data)
 
 	seq_printf(m, "current_state: %s\n",
 		   ptlrpc_import_state_name(imp->imp_state));
-	seq_printf(m, "state_history:\n");
+	seq_puts(m, "state_history:\n");
 	k = imp->imp_state_hist_idx;
 	for (j = 0; j < IMP_STATE_HIST_LEN; j++) {
 		struct import_state_hist *ish =
@@ -965,7 +931,7 @@ int lprocfs_at_hist_helper(struct seq_file *m, struct adaptive_timeout *at)
 
 	for (i = 0; i < AT_BINS; i++)
 		seq_printf(m, "%3u ", at->at_hist[i]);
-	seq_printf(m, "\n");
+	seq_puts(m, "\n");
 	return 0;
 }
 EXPORT_SYMBOL(lprocfs_at_hist_helper);
@@ -1033,7 +999,7 @@ int lprocfs_rd_connect_flags(struct seq_file *m, void *data)
 	flags = obd->u.cli.cl_import->imp_connect_data.ocd_connect_flags;
 	seq_printf(m, "flags=%#llx\n", flags);
 	obd_connect_seq_flags2str(m, flags, "\n");
-	seq_printf(m, "\n");
+	seq_puts(m, "\n");
 	up_read(&obd->u.cli.cl_sem);
 	return 0;
 }
diff --git a/drivers/staging/lustre/lustre/obdclass/lu_object.c b/drivers/staging/lustre/lustre/obdclass/lu_object.c
index abcf951208d2..bb9d514525ce 100644
--- a/drivers/staging/lustre/lustre/obdclass/lu_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/lu_object.c
@@ -512,7 +512,7 @@ void lu_object_header_print(const struct lu_env *env, void *cookie,
 			    lu_printer_t printer,
 			    const struct lu_object_header *hdr)
 {
-	(*printer)(env, cookie, "header@%p[%#lx, %d, "DFID"%s%s%s]",
+	(*printer)(env, cookie, "header@%p[%#lx, %d, " DFID "%s%s%s]",
 		   hdr, hdr->loh_flags, atomic_read(&hdr->loh_ref),
 		   PFID(&hdr->loh_fid),
 		   hlist_unhashed(&hdr->loh_hash) ? "" : " hash",
@@ -556,7 +556,7 @@ EXPORT_SYMBOL(lu_object_print);
 static struct lu_object *htable_lookup(struct lu_site *s,
 				       struct cfs_hash_bd *bd,
 				       const struct lu_fid *f,
-				       wait_queue_t *waiter,
+				       wait_queue_entry_t *waiter,
 				       __u64 *version)
 {
 	struct lu_site_bkt_data *bkt;
@@ -670,7 +670,7 @@ static struct lu_object *lu_object_find_try(const struct lu_env *env,
 					    struct lu_device *dev,
 					    const struct lu_fid *f,
 					    const struct lu_object_conf *conf,
-					    wait_queue_t *waiter)
+					    wait_queue_entry_t *waiter)
 {
 	struct lu_object      *o;
 	struct lu_object      *shadow;
@@ -750,7 +750,7 @@ struct lu_object *lu_object_find_at(const struct lu_env *env,
 {
 	struct lu_site_bkt_data *bkt;
 	struct lu_object	*obj;
-	wait_queue_t	   wait;
+	wait_queue_entry_t	   wait;
 
 	while (1) {
 		obj = lu_object_find_try(env, dev, f, conf, &wait);
@@ -918,9 +918,8 @@ static unsigned long lu_htable_order(struct lu_device *top)
 	cache_size = cache_size / 100 * lu_cache_percent *
 		(PAGE_SIZE / 1024);
 
-	for (bits = 1; (1 << bits) < cache_size; ++bits) {
+	for (bits = 1; (1 << bits) < cache_size; ++bits)
 		;
-	}
 	return clamp_t(typeof(bits), bits, LU_SITE_BITS_MIN, bits_max);
 }
 
diff --git a/drivers/staging/lustre/lustre/obdecho/echo_client.c b/drivers/staging/lustre/lustre/obdecho/echo_client.c
index 77b4c5504689..1c4a8fe87dd8 100644
--- a/drivers/staging/lustre/lustre/obdecho/echo_client.c
+++ b/drivers/staging/lustre/lustre/obdecho/echo_client.c
@@ -277,7 +277,7 @@ static int echo_page_print(const struct lu_env *env,
 {
 	struct echo_page *ep = cl2echo_page(slice);
 
-	(*printer)(env, cookie, LUSTRE_ECHO_CLIENT_NAME"-page@%p %d vm@%p\n",
+	(*printer)(env, cookie, LUSTRE_ECHO_CLIENT_NAME "-page@%p %d vm@%p\n",
 		   ep, mutex_is_locked(&ep->ep_lock),
 		   slice->cpl_page->cp_vmpage);
 	return 0;
@@ -1121,7 +1121,7 @@ static int echo_create_object(const struct lu_env *env, struct echo_device *ed,
 	}
 	cl_echo_object_put(eco);
 
-	CDEBUG(D_INFO, "oa oid "DOSTID"\n", POSTID(&oa->o_oi));
+	CDEBUG(D_INFO, "oa oid " DOSTID "\n", POSTID(&oa->o_oi));
 
  failed:
 	if (created && rc)
@@ -1646,9 +1646,8 @@ static int echo_client_connect(const struct lu_env *env,
 	struct lustre_handle conn = { 0 };
 
 	rc = class_connect(&conn, src, cluuid);
-	if (rc == 0) {
+	if (rc == 0)
 		*exp = class_conn2export(&conn);
-	}
 
 	return rc;
 }
diff --git a/drivers/staging/lustre/lustre/osc/osc_cache.c b/drivers/staging/lustre/lustre/osc/osc_cache.c
index c5ccf568313a..d8a95f8fe1ff 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cache.c
+++ b/drivers/staging/lustre/lustre/osc/osc_cache.c
@@ -472,7 +472,7 @@ static void osc_extent_insert(struct osc_object *obj, struct osc_extent *ext)
 		else if (ext->oe_start > tmp->oe_end)
 			n = &(*n)->rb_right;
 		else
-			EASSERTF(0, tmp, EXTSTR"\n", EXTPARA(ext));
+			EASSERTF(0, tmp, EXTSTR "\n", EXTPARA(ext));
 	}
 	rb_link_node(&ext->oe_node, parent, n);
 	rb_insert_color(&ext->oe_node, &obj->oo_root);
@@ -690,7 +690,7 @@ static struct osc_extent *osc_extent_find(const struct lu_env *env,
 	/* grants has been allocated by caller */
 	LASSERTF(*grants >= chunksize + cli->cl_extent_tax,
 		 "%u/%u/%u.\n", *grants, chunksize, cli->cl_extent_tax);
-	LASSERTF((max_end - cur->oe_start) < max_pages, EXTSTR"\n",
+	LASSERTF((max_end - cur->oe_start) < max_pages, EXTSTR "\n",
 		 EXTPARA(cur));
 
 restart:
@@ -709,7 +709,7 @@ restart:
 		/* if covering by different locks, no chance to match */
 		if (olck->ols_dlmlock != ext->oe_dlmlock) {
 			EASSERTF(!overlapped(ext, cur), ext,
-				 EXTSTR"\n", EXTPARA(cur));
+				 EXTSTR "\n", EXTPARA(cur));
 
 			ext = next_extent(ext);
 			continue;
@@ -732,7 +732,7 @@ restart:
 			 */
 			EASSERTF((ext->oe_start <= cur->oe_start &&
 				  ext->oe_end >= cur->oe_end),
-				 ext, EXTSTR"\n", EXTPARA(cur));
+				 ext, EXTSTR "\n", EXTPARA(cur));
 
 			if (ext->oe_state > OES_CACHE || ext->oe_fsync_wait) {
 				/* for simplicity, we wait for this extent to
@@ -1406,9 +1406,8 @@ static void osc_release_write_grant(struct client_obd *cli,
 				    struct brw_page *pga)
 {
 	assert_spin_locked(&cli->cl_loi_list_lock);
-	if (!(pga->flag & OBD_BRW_FROM_GRANT)) {
+	if (!(pga->flag & OBD_BRW_FROM_GRANT))
 		return;
-	}
 
 	pga->flag &= ~OBD_BRW_FROM_GRANT;
 	atomic_long_dec(&obd_dirty_pages);
@@ -1890,10 +1889,10 @@ struct extent_rpc_data {
 	unsigned int		erd_max_chunks;
 };
 
-static inline unsigned osc_extent_chunks(const struct osc_extent *ext)
+static inline unsigned int osc_extent_chunks(const struct osc_extent *ext)
 {
 	struct client_obd *cli = osc_cli(ext->oe_obj);
-	unsigned ppc_bits = cli->cl_chunkbits - PAGE_SHIFT;
+	unsigned int ppc_bits = cli->cl_chunkbits - PAGE_SHIFT;
 
 	return (ext->oe_end >> ppc_bits) - (ext->oe_start >> ppc_bits) + 1;
 }
@@ -1951,7 +1950,7 @@ static int try_to_add_extent_for_io(struct client_obd *cli,
 	return 1;
 }
 
-static inline unsigned osc_max_write_chunks(const struct client_obd *cli)
+static inline unsigned int osc_max_write_chunks(const struct client_obd *cli)
 {
 	/*
 	 * LU-8135:
diff --git a/drivers/staging/lustre/lustre/osc/osc_internal.h b/drivers/staging/lustre/lustre/osc/osc_internal.h
index 845e795d8795..13a40f6423ff 100644
--- a/drivers/staging/lustre/lustre/osc/osc_internal.h
+++ b/drivers/staging/lustre/lustre/osc/osc_internal.h
@@ -62,7 +62,7 @@ struct osc_async_page {
 	struct list_head	      oap_rpc_item;
 
 	u64		 oap_obj_off;
-	unsigned		oap_page_off;
+	unsigned int		oap_page_off;
 	enum async_flags	oap_async_flags;
 
 	struct brw_page	 oap_brw_page;
diff --git a/drivers/staging/lustre/lustre/osc/osc_request.c b/drivers/staging/lustre/lustre/osc/osc_request.c
index d8aa3fb468c7..922d0cbe83dc 100644
--- a/drivers/staging/lustre/lustre/osc/osc_request.c
+++ b/drivers/staging/lustre/lustre/osc/osc_request.c
@@ -1227,8 +1227,7 @@ static int check_write_checksum(struct obdo *oa,
 		msg = "changed in transit AND doesn't match the original - likely false positive due to mmap IO (bug 11742)"
 			;
 
-	LCONSOLE_ERROR_MSG(0x132, "BAD WRITE CHECKSUM: %s: from %s inode "DFID
-			   " object "DOSTID" extent [%llu-%llu]\n",
+	LCONSOLE_ERROR_MSG(0x132, "BAD WRITE CHECKSUM: %s: from %s inode " DFID " object " DOSTID " extent [%llu-%llu]\n",
 			   msg, libcfs_nid2str(peer->nid),
 			   oa->o_valid & OBD_MD_FLFID ? oa->o_parent_seq : (__u64)0,
 			   oa->o_valid & OBD_MD_FLFID ? oa->o_parent_oid : 0,
diff --git a/drivers/staging/lustre/lustre/ptlrpc/client.c b/drivers/staging/lustre/lustre/ptlrpc/client.c
index 6466974a43e7..1c7779215eed 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/client.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/client.c
@@ -367,8 +367,8 @@ void ptlrpc_at_adj_net_latency(struct ptlrpc_request *req,
 		 */
 		CDEBUG((lustre_msg_get_flags(req->rq_reqmsg) & MSG_RESENT) ?
 		       D_ADAPTTO : D_WARNING,
-		       "Reported service time %u > total measured time "
-		       CFS_DURATION_T"\n", service_time,
+		       "Reported service time %u > total measured time " CFS_DURATION_T "\n",
+		       service_time,
 		       (long)(now - req->rq_sent));
 		return;
 	}
diff --git a/drivers/staging/lustre/lustre/ptlrpc/import.c b/drivers/staging/lustre/lustre/ptlrpc/import.c
index 93e172fe9ce4..52cb1f0c9c94 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/import.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/import.c
@@ -1182,17 +1182,15 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 	}
 
 	/* Sanity checks for a reconnected import. */
-	if (!(imp->imp_replayable) != !(msg_flags & MSG_CONNECT_REPLAYABLE)) {
+	if (!(imp->imp_replayable) != !(msg_flags & MSG_CONNECT_REPLAYABLE))
 		CERROR("imp_replayable flag does not match server after reconnect. We should LBUG right here.\n");
-	}
 
 	if (lustre_msg_get_last_committed(request->rq_repmsg) > 0 &&
 	    lustre_msg_get_last_committed(request->rq_repmsg) <
-	    aa->pcaa_peer_committed) {
+	    aa->pcaa_peer_committed)
 		CERROR("%s went back in time (transno %lld was previously committed, server now claims %lld)!  See https://bugzilla.lustre.org/show_bug.cgi?id=9646\n",
 		       obd2cli_tgt(imp->imp_obd), aa->pcaa_peer_committed,
 		       lustre_msg_get_last_committed(request->rq_repmsg));
-	}
 
 finish:
 	ptlrpc_prepare_replay(imp);
@@ -1437,20 +1435,17 @@ int ptlrpc_import_recovery_state_machine(struct obd_import *imp)
 		rc = 0;
 	}
 
-	if (imp->imp_state == LUSTRE_IMP_REPLAY_LOCKS) {
+	if (imp->imp_state == LUSTRE_IMP_REPLAY_LOCKS)
 		if (atomic_read(&imp->imp_replay_inflight) == 0) {
 			IMPORT_SET_STATE(imp, LUSTRE_IMP_REPLAY_WAIT);
 			rc = signal_completed_replay(imp);
 			if (rc)
 				goto out;
 		}
-	}
 
-	if (imp->imp_state == LUSTRE_IMP_REPLAY_WAIT) {
-		if (atomic_read(&imp->imp_replay_inflight) == 0) {
+	if (imp->imp_state == LUSTRE_IMP_REPLAY_WAIT)
+		if (atomic_read(&imp->imp_replay_inflight) == 0)
 			IMPORT_SET_STATE(imp, LUSTRE_IMP_RECOVER);
-		}
-	}
 
 	if (imp->imp_state == LUSTRE_IMP_RECOVER) {
 		CDEBUG(D_HA, "reconnected to %s@%s\n",
diff --git a/drivers/staging/lustre/lustre/ptlrpc/layout.c b/drivers/staging/lustre/lustre/ptlrpc/layout.c
index 8177e1a31ca9..5810bbab6585 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/layout.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/layout.c
@@ -1761,7 +1761,7 @@ static u32 __req_capsule_offset(const struct req_capsule *pill,
 		 field->rmf_name, offset, loc);
 	offset--;
 
-	LASSERT(0 <= offset && offset < REQ_MAX_FIELD_NR);
+	LASSERT(offset < REQ_MAX_FIELD_NR);
 	return offset;
 }
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
index 9456a1825918..55e8696e7d86 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
@@ -2089,7 +2089,7 @@ static void dump_obdo(struct obdo *oa)
 
 	CDEBUG(D_RPCTRACE, "obdo: o_valid = %08x\n", valid);
 	if (valid & OBD_MD_FLID)
-		CDEBUG(D_RPCTRACE, "obdo: id = "DOSTID"\n", POSTID(&oa->o_oi));
+		CDEBUG(D_RPCTRACE, "obdo: id = " DOSTID "\n", POSTID(&oa->o_oi));
 	if (valid & OBD_MD_FLFID)
 		CDEBUG(D_RPCTRACE, "obdo: o_parent_seq = %#llx\n",
 		       oa->o_parent_seq);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
index d2707a323c47..c38e166f1502 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
@@ -68,7 +68,7 @@ void ptlrpc_init_xid(void);
 void ptlrpc_set_add_new_req(struct ptlrpcd_ctl *pc,
 			    struct ptlrpc_request *req);
 int ptlrpc_expired_set(void *data);
-int ptlrpc_set_next_timeout(struct ptlrpc_request_set *);
+int ptlrpc_set_next_timeout(struct ptlrpc_request_set *set);
 void ptlrpc_resend_req(struct ptlrpc_request *request);
 void ptlrpc_set_bulk_mbits(struct ptlrpc_request *req);
 void ptlrpc_assign_next_xid_nolock(struct ptlrpc_request *req);
@@ -79,7 +79,7 @@ void ptlrpc_add_unreplied(struct ptlrpc_request *req);
 int ptlrpc_init_portals(void);
 void ptlrpc_exit_portals(void);
 
-void ptlrpc_request_handle_notconn(struct ptlrpc_request *);
+void ptlrpc_request_handle_notconn(struct ptlrpc_request *req);
 void lustre_assert_wire_constants(void);
 int ptlrpc_import_in_recovery(struct obd_import *imp);
 int ptlrpc_set_import_discon(struct obd_import *imp, __u32 conn_cnt);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/service.c b/drivers/staging/lustre/lustre/ptlrpc/service.c
index b8091c118302..759aa6c16e28 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/service.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/service.c
@@ -1565,7 +1565,7 @@ ptlrpc_server_handle_req_in(struct ptlrpc_service_part *svcpt,
 
 	/* req_in handling should/must be fast */
 	if (ktime_get_real_seconds() - req->rq_arrival_time.tv_sec > 5)
-		DEBUG_REQ(D_WARNING, req, "Slow req_in handling "CFS_DURATION_T"s",
+		DEBUG_REQ(D_WARNING, req, "Slow req_in handling " CFS_DURATION_T "s",
 			  (long)(ktime_get_real_seconds() -
 				 req->rq_arrival_time.tv_sec));
 
diff --git a/drivers/staging/media/Kconfig b/drivers/staging/media/Kconfig
index dbda4d9a08e7..f8c25ee082ef 100644
--- a/drivers/staging/media/Kconfig
+++ b/drivers/staging/media/Kconfig
@@ -27,6 +27,8 @@ source "drivers/staging/media/cxd2099/Kconfig"
 
 source "drivers/staging/media/davinci_vpfe/Kconfig"
 
+source "drivers/staging/media/imx/Kconfig"
+
 source "drivers/staging/media/omap4iss/Kconfig"
 
 # Keep LIRC at the end, as it has sub-menus
diff --git a/drivers/staging/media/Makefile b/drivers/staging/media/Makefile
index c04600c81264..ac090c5fce30 100644
--- a/drivers/staging/media/Makefile
+++ b/drivers/staging/media/Makefile
@@ -1,5 +1,6 @@
 obj-$(CONFIG_I2C_BCM2048)	+= bcm2048/
 obj-$(CONFIG_DVB_CXD2099)	+= cxd2099/
+obj-$(CONFIG_VIDEO_IMX_MEDIA)	+= imx/
 obj-$(CONFIG_LIRC_STAGING)	+= lirc/
 obj-$(CONFIG_VIDEO_DM365_VPFE)	+= davinci_vpfe/
 obj-$(CONFIG_VIDEO_OMAP4)	+= omap4iss/
diff --git a/drivers/staging/media/atomisp/i2c/Makefile b/drivers/staging/media/atomisp/i2c/Makefile
index 466517c7c8e6..be13fab92175 100644
--- a/drivers/staging/media/atomisp/i2c/Makefile
+++ b/drivers/staging/media/atomisp/i2c/Makefile
@@ -19,3 +19,9 @@ obj-$(CONFIG_VIDEO_AP1302)     += ap1302.o
 
 obj-$(CONFIG_VIDEO_LM3554) += lm3554.o
 
+# HACK! While this driver is in bad shape, don't enable several warnings
+#       that would be otherwise enabled with W=1
+ccflags-y += $(call cc-disable-warning, unused-but-set-variable)
+ccflags-y += $(call cc-disable-warning, unused-const-variable)
+ccflags-y += $(call cc-disable-warning, missing-prototypes)
+ccflags-y += $(call cc-disable-warning, missing-declarations)
diff --git a/drivers/staging/media/atomisp/i2c/gc0310.c b/drivers/staging/media/atomisp/i2c/gc0310.c
index 1ec616a15086..350fd7fd5b86 100644
--- a/drivers/staging/media/atomisp/i2c/gc0310.c
+++ b/drivers/staging/media/atomisp/i2c/gc0310.c
@@ -1455,6 +1455,7 @@ out_free:
 
 static struct acpi_device_id gc0310_acpi_match[] = {
 	{"XXGC0310"},
+	{"INT0310"},
 	{},
 };
 
diff --git a/drivers/staging/media/atomisp/i2c/imx/Makefile b/drivers/staging/media/atomisp/i2c/imx/Makefile
index 6b13a3a66e49..b6578f09546e 100644
--- a/drivers/staging/media/atomisp/i2c/imx/Makefile
+++ b/drivers/staging/media/atomisp/i2c/imx/Makefile
@@ -4,3 +4,10 @@ imx1x5-objs := imx.o drv201.o ad5816g.o dw9714.o dw9719.o dw9718.o vcm.o otp.o o
 
 ov8858_driver-objs := ../ov8858.o dw9718.o vcm.o
 obj-$(CONFIG_VIDEO_OV8858)     += ov8858_driver.o
+
+# HACK! While this driver is in bad shape, don't enable several warnings
+#       that would be otherwise enabled with W=1
+ccflags-y += $(call cc-disable-warning, unused-but-set-variable)
+ccflags-y += $(call cc-disable-warning, unused-const-variable)
+ccflags-y += $(call cc-disable-warning, missing-prototypes)
+ccflags-y += $(call cc-disable-warning, missing-declarations)
diff --git a/drivers/staging/media/atomisp/i2c/lm3554.c b/drivers/staging/media/atomisp/i2c/lm3554.c
index dd9c9c3ffff7..2b170c07aaba 100644
--- a/drivers/staging/media/atomisp/i2c/lm3554.c
+++ b/drivers/staging/media/atomisp/i2c/lm3554.c
@@ -497,7 +497,7 @@ static const struct v4l2_ctrl_ops ctrl_ops = {
 	.g_volatile_ctrl = lm3554_g_volatile_ctrl
 };
 
-struct v4l2_ctrl_config lm3554_controls[] = {
+static const struct v4l2_ctrl_config lm3554_controls[] = {
 	{
 	 .ops = &ctrl_ops,
 	 .id = V4L2_CID_FLASH_TIMEOUT,
@@ -825,7 +825,7 @@ static int lm3554_gpio_uninit(struct i2c_client *client)
 	return 0;
 }
 
-void *lm3554_platform_data_func(struct i2c_client *client)
+static void *lm3554_platform_data_func(struct i2c_client *client)
 {
 	static struct lm3554_platform_data platform_data;
 
diff --git a/drivers/staging/media/atomisp/i2c/mt9m114.c b/drivers/staging/media/atomisp/i2c/mt9m114.c
index ced175c268d1..3fa915313e53 100644
--- a/drivers/staging/media/atomisp/i2c/mt9m114.c
+++ b/drivers/staging/media/atomisp/i2c/mt9m114.c
@@ -1499,7 +1499,7 @@ static struct v4l2_ctrl_config mt9m114_controls[] = {
 	 .type = V4L2_CTRL_TYPE_MENU,
 	 .min = 0,
 	 .max = 3,
-	 .step = 1,
+	 .step = 0,
 	 .def = 1,
 	 .flags = 0,
 	 },
diff --git a/drivers/staging/media/atomisp/i2c/ov2680.c b/drivers/staging/media/atomisp/i2c/ov2680.c
index 566091035c64..3cabfe54c669 100644
--- a/drivers/staging/media/atomisp/i2c/ov2680.c
+++ b/drivers/staging/media/atomisp/i2c/ov2680.c
@@ -885,11 +885,12 @@ static int gpio_ctrl(struct v4l2_subdev *sd, bool flag)
 	if (flag) {
 		ret = dev->platform_data->gpio0_ctrl(sd, 1);
 		usleep_range(10000, 15000);
-		ret |= dev->platform_data->gpio1_ctrl(sd, 1);
+		/* Ignore return from second gpio, it may not be there */
+		dev->platform_data->gpio1_ctrl(sd, 1);
 		usleep_range(10000, 15000);
 	} else {
-		ret = dev->platform_data->gpio1_ctrl(sd, 0);
-		ret |= dev->platform_data->gpio0_ctrl(sd, 0);
+		dev->platform_data->gpio1_ctrl(sd, 0);
+		ret = dev->platform_data->gpio0_ctrl(sd, 0);
 	}
 	return ret;
 }
@@ -1190,9 +1191,8 @@ static int ov2680_detect(struct i2c_client *client)
 					OV2680_SC_CMMN_SUB_ID, &high);
 	revision = (u8) high & 0x0f;
 
-	dev_err(&client->dev, "sensor_revision id  = 0x%x\n", id);
-	dev_err(&client->dev, "detect ov2680 success\n");
-	dev_err(&client->dev, "################5##########\n");
+	dev_info(&client->dev, "sensor_revision id = 0x%x\n", id);
+
 	return 0;
 }
 
@@ -1447,8 +1447,6 @@ static int ov2680_probe(struct i2c_client *client,
 	void *pdata;
 	unsigned int i;
 
-	printk("++++ov2680_probe++++\n");
-	dev_info(&client->dev, "++++ov2680_probe++++\n");
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 	if (!dev) {
 		dev_err(&client->dev, "out of memory\n");
@@ -1521,6 +1519,7 @@ out_free:
 
 static struct acpi_device_id ov2680_acpi_match[] = {
 	{"XXOV2680"},
+	{"OVTI2680"},
 	{},
 };
 MODULE_DEVICE_TABLE(acpi, ov2680_acpi_match);
diff --git a/drivers/staging/media/atomisp/i2c/ov5693/Makefile b/drivers/staging/media/atomisp/i2c/ov5693/Makefile
index c9c0e1245858..4e3833aaec05 100644
--- a/drivers/staging/media/atomisp/i2c/ov5693/Makefile
+++ b/drivers/staging/media/atomisp/i2c/ov5693/Makefile
@@ -1 +1,8 @@
 obj-$(CONFIG_VIDEO_OV5693) += ov5693.o
+
+# HACK! While this driver is in bad shape, don't enable several warnings
+#       that would be otherwise enabled with W=1
+ccflags-y += $(call cc-disable-warning, unused-but-set-variable)
+ccflags-y += $(call cc-disable-warning, unused-const-variable)
+ccflags-y += $(call cc-disable-warning, missing-prototypes)
+ccflags-y += $(call cc-disable-warning, missing-declarations)
diff --git a/drivers/staging/media/atomisp/i2c/ov5693/ov5693.c b/drivers/staging/media/atomisp/i2c/ov5693/ov5693.c
index 5e9dafe7cc32..d6447398f5ef 100644
--- a/drivers/staging/media/atomisp/i2c/ov5693/ov5693.c
+++ b/drivers/staging/media/atomisp/i2c/ov5693/ov5693.c
@@ -706,7 +706,7 @@ static int ov5693_read_otp_reg_array(struct i2c_client *client, u16 size,
 {
 	u16 index;
 	int ret;
-	u16 *pVal = 0;
+	u16 *pVal = NULL;
 
 	for (index = 0; index <= size; index++) {
 		pVal = (u16 *) (buf + index);
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/Makefile b/drivers/staging/media/atomisp/pci/atomisp2/Makefile
index f126a89a08e9..726eaa293c55 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/Makefile
+++ b/drivers/staging/media/atomisp/pci/atomisp2/Makefile
@@ -108,7 +108,6 @@ atomisp-objs += \
 	css2400/sh_css_metadata.o \
 	css2400/base/refcount/src/refcount.o \
 	css2400/base/circbuf/src/circbuf.o \
-	css2400/sh_css_irq.o \
 	css2400/camera/pipe/src/pipe_binarydesc.o \
 	css2400/camera/pipe/src/pipe_util.o \
 	css2400/camera/pipe/src/pipe_stagedesc.o \
@@ -353,3 +352,9 @@ DEFINES += -DSYSTEM_hive_isp_css_2400_system -DISP2400
 
 ccflags-y += $(INCLUDES) $(DEFINES) -fno-common
 
+# HACK! While this driver is in bad shape, don't enable several warnings
+#       that would be otherwise enabled with W=1
+ccflags-y += -Wno-unused-const-variable -Wno-missing-prototypes \
+	     -Wno-unused-but-set-variable -Wno-missing-declarations \
+	     -Wno-suggest-attribute=format -Wno-missing-prototypes \
+	     -Wno-implicit-fallthrough
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_css20.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_css20.c
index b830b241e2e6..ad2c610d2ce3 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_css20.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_css20.c
@@ -2506,7 +2506,6 @@ static void __configure_capture_pp_input(struct atomisp_sub_device *asd,
 	struct ia_css_pipe_extra_config *pipe_extra_configs =
 		&stream_env->pipe_extra_configs[pipe_id];
 	unsigned int hor_ds_factor = 0, ver_ds_factor = 0;
-#define CEIL_DIV(a, b)       ((b) ? ((a) + (b) - 1) / (b) : 0)
 
 	if (width == 0 && height == 0)
 		return;
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_fops.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_fops.c
index 7ce8803cf6f9..c151c848cf8f 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_fops.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_fops.c
@@ -130,9 +130,9 @@ static int atomisp_q_one_metadata_buffer(struct atomisp_sub_device *asd,
 	return 0;
 }
 
-int atomisp_q_one_s3a_buffer(struct atomisp_sub_device *asd,
-				enum atomisp_input_stream_id stream_id,
-				enum atomisp_css_pipe_id css_pipe_id)
+static int atomisp_q_one_s3a_buffer(struct atomisp_sub_device *asd,
+				    enum atomisp_input_stream_id stream_id,
+				    enum atomisp_css_pipe_id css_pipe_id)
 {
 	struct atomisp_s3a_buf *s3a_buf;
 	struct list_head *s3a_list;
@@ -172,9 +172,9 @@ int atomisp_q_one_s3a_buffer(struct atomisp_sub_device *asd,
 	return 0;
 }
 
-int atomisp_q_one_dis_buffer(struct atomisp_sub_device *asd,
-				enum atomisp_input_stream_id stream_id,
-				enum atomisp_css_pipe_id css_pipe_id)
+static int atomisp_q_one_dis_buffer(struct atomisp_sub_device *asd,
+				    enum atomisp_input_stream_id stream_id,
+				    enum atomisp_css_pipe_id css_pipe_id)
 {
 	struct atomisp_dis_buf *dis_buf;
 	unsigned long irqflags;
@@ -744,7 +744,7 @@ static void atomisp_subdev_init_struct(struct atomisp_sub_device *asd)
 /*
  * file operation functions
  */
-unsigned int atomisp_subdev_users(struct atomisp_sub_device *asd)
+static unsigned int atomisp_subdev_users(struct atomisp_sub_device *asd)
 {
 	return asd->video_out_preview.users +
 	       asd->video_out_vf.users +
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_ioctl.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_ioctl.c
index 6064bb823a47..aa0526ebaff1 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_ioctl.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_ioctl.c
@@ -683,7 +683,7 @@ static int atomisp_s_input(struct file *file, void *fh, unsigned int input)
 	int ret;
 
 	rt_mutex_lock(&isp->mutex);
-	if (input >= ATOM_ISP_MAX_INPUTS || input > isp->input_cnt) {
+	if (input >= ATOM_ISP_MAX_INPUTS || input >= isp->input_cnt) {
 		dev_dbg(isp->dev, "input_cnt: %d\n", isp->input_cnt);
 		ret = -EINVAL;
 		goto error;
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_tpg.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_tpg.c
index 996d1bdebad4..48b96048cab4 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_tpg.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_tpg.c
@@ -56,6 +56,7 @@ static int tpg_set_fmt(struct v4l2_subdev *sd,
 		       struct v4l2_subdev_format *format)
 {
 	struct v4l2_mbus_framefmt *fmt = &format->format;
+
 	if (format->pad)
 		return -EINVAL;
 	/* only raw8 grbg is supported by TPG */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_v4l2.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_v4l2.c
index e3fdbdba0b34..a543def739fc 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_v4l2.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_v4l2.c
@@ -51,12 +51,12 @@
 /* G-Min addition: pull this in from intel_mid_pm.h */
 #define CSTATE_EXIT_LATENCY_C1  1
 
-static uint skip_fwload = 0;
+static uint skip_fwload;
 module_param(skip_fwload, uint, 0644);
 MODULE_PARM_DESC(skip_fwload, "Skip atomisp firmware load");
 
 /* set reserved memory pool size in page */
-unsigned int repool_pgnr;
+static unsigned int repool_pgnr;
 module_param(repool_pgnr, uint, 0644);
 MODULE_PARM_DESC(repool_pgnr,
 		"Set the reserved memory pool size in page (default:0)");
@@ -384,7 +384,7 @@ done:
  * WA for DDR DVFS enable/disable
  * By default, ISP will force DDR DVFS 1600MHz before disable DVFS
  */
-void punit_ddr_dvfs_enable(bool enable)
+static void punit_ddr_dvfs_enable(bool enable)
 {
 	int reg = intel_mid_msgbus_read32(PUNIT_PORT, MRFLD_ISPSSDVFS);
 	int door_bell = 1 << 8;
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/Makefile b/drivers/staging/media/atomisp/pci/atomisp2/css2400/Makefile
index 04defaafa02c..ee5631b0e635 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/Makefile
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/Makefile
@@ -1,4 +1,2 @@
 ccflags-y += -DISP2400B0
 ISP2400B0 := y
-
-include $(srctree)/$(src)/../Makefile.common
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/math_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/math_support.h
index 48d84bc0ad9e..f74b405b0f39 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/math_support.h
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/math_support.h
@@ -62,15 +62,15 @@
 #define MAX(a, b)            (((a) > (b)) ? (a) : (b))
 #define MIN(a, b)            (((a) < (b)) ? (a) : (b))
 #ifdef ISP2401
-#define ROUND_DIV(a, b)      ((b) ? ((a) + ((b) >> 1)) / (b) : 0)
+#define ROUND_DIV(a, b)      (((b) != 0) ? ((a) + ((b) >> 1)) / (b) : 0)
 #endif
-#define CEIL_DIV(a, b)       ((b) ? ((a) + (b) - 1) / (b) : 0)
+#define CEIL_DIV(a, b)       (((b) != 0) ? ((a) + (b) - 1) / (b) : 0)
 #define CEIL_MUL(a, b)       (CEIL_DIV(a, b) * (b))
 #define CEIL_MUL2(a, b)      (((a) + (b) - 1) & ~((b) - 1))
 #define CEIL_SHIFT(a, b)     (((a) + (1 << (b)) - 1)>>(b))
 #define CEIL_SHIFT_MUL(a, b) (CEIL_SHIFT(a, b) << (b))
 #ifdef ISP2401
-#define ROUND_HALF_DOWN_DIV(a, b)	((b) ? ((a) + (b / 2) - 1) / (b) : 0)
+#define ROUND_HALF_DOWN_DIV(a, b)	(((b) != 0) ? ((a) + (b / 2) - 1) / (b) : 0)
 #define ROUND_HALF_DOWN_MUL(a, b)	(ROUND_HALF_DOWN_DIV(a, b) * (b))
 #endif
 
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/string_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/string_support.h
index 568631698a3d..c53241a7a281 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/string_support.h
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/string_support.h
@@ -72,9 +72,8 @@ static size_t strnlen_s(
 		return 0;
 	}
 
-	for (ix=0;
-		((src_str[ix] != '\0') && (ix< max_len));
-		++ix) /*Nothing else to do*/;
+	for (ix = 0; ix < max_len && src_str[ix] != '\0'; ix++)
+		;
 
 	/* On Error, it will return src_size == max_len*/
 	return ix;
@@ -118,7 +117,7 @@ STORAGE_CLASS_INLINE int strncpy_s(
 
 	/* dest_str is big enough for the len */
 	strncpy(dest_str, src_str, len);
-	dest_str[len+1] = '\0';
+	dest_str[len] = '\0';
 	return 0;
 }
 
@@ -158,7 +157,7 @@ STORAGE_CLASS_INLINE int strcpy_s(
 
 	/* dest_str is big enough for the len */
 	strncpy(dest_str, src_str, len);
-	dest_str[len+1] = '\0';
+	dest_str[len] = '\0';
 	return 0;
 }
 
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_mmu_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_mmu_private.h
index 7c8500903b5c..1021e4f380a5 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_mmu_private.h
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_mmu_private.h
@@ -1,4 +1,3 @@
-#ifdef ISP2401
 /*
  * Support for Intel Camera Imaging ISP subsystem.
  * Copyright (c) 2015, Intel Corporation.
@@ -28,4 +27,3 @@ void
 sh_css_mmu_set_page_table_base_index(hrt_data base_index);
 
 #endif /* __IA_CSS_MMU_PRIVATE_H */
-#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.c
index 0daab1176865..9478c12abe89 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.c
@@ -265,9 +265,9 @@ ia_css_translate_dvs_statistics(
 	assert(isp_stats->hor_proj != NULL);
 	assert(isp_stats->ver_proj != NULL);
 
-	IA_CSS_ENTER("hproj=%p, vproj=%p, haddr=%x, vaddr=%x",
-			host_stats->hor_proj, host_stats->ver_proj,
-			isp_stats->hor_proj, isp_stats->ver_proj);
+	IA_CSS_ENTER("hproj=%p, vproj=%p, haddr=%p, vaddr=%p",
+		     host_stats->hor_proj, host_stats->ver_proj,
+		     isp_stats->hor_proj, isp_stats->ver_proj);
 
 	hor_num_isp = host_stats->grid.aligned_height;
 	ver_num_isp = host_stats->grid.aligned_width;
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.c
index 5a0c103e9eb7..9bccb6473154 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.c
@@ -213,7 +213,7 @@ ia_css_translate_dvs2_statistics(
 		     "hor_coefs.even_real=%p, hor_coefs.even_imag=%p, "
 		     "ver_coefs.odd_real=%p, ver_coefs.odd_imag=%p, "
 		     "ver_coefs.even_real=%p, ver_coefs.even_imag=%p, "
-		     "haddr=%x, vaddr=%x",
+		     "haddr=%p, vaddr=%p",
 		host_stats->hor_prod.odd_real, host_stats->hor_prod.odd_imag,
 		host_stats->hor_prod.even_real, host_stats->hor_prod.even_imag,
 		host_stats->ver_prod.odd_real, host_stats->ver_prod.odd_imag,
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.c
index 804c19ab4485..222a7bd7f176 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.c
@@ -55,7 +55,7 @@ ia_css_tnr_dump(
 			"tnr_coef", tnr->coef);
 	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
 			"tnr_threshold_Y", tnr->threshold_Y);
-	ia_css_debug_dtrace(level, "\t%-32s = %d\n"
+	ia_css_debug_dtrace(level, "\t%-32s = %d\n",
 			"tnr_threshold_C", tnr->threshold_C);
 }
 
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_const.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_const.h
index 005eaaa9eb6c..2f215dc2ac32 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_const.h
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_const.h
@@ -398,17 +398,6 @@ more details.
  * so the calc for the output buffer vmem size is:
  * ((width[vectors]/num_of_stripes) + 2[vectors])
  */
-#if defined(HAS_RES_MGR)
-#define MAX_VECTORS_PER_OUTPUT_LINE \
-	(CEIL_DIV(CEIL_DIV(ISP_MAX_OUTPUT_WIDTH, ISP_NUM_STRIPES) + ISP_LEFT_PADDING, ISP_VEC_NELEMS) + \
-	ITERATOR_VECTOR_INCREMENT)
-
-#define MAX_VECTORS_PER_INPUT_LINE	CEIL_DIV(ISP_MAX_INPUT_WIDTH, ISP_VEC_NELEMS)
-#define MAX_VECTORS_PER_INPUT_STRIPE	(CEIL_ROUND_DIV_STRIPE(CEIL_DIV(ISP_MAX_INPUT_WIDTH, ISP_VEC_NELEMS) , \
-							      ISP_NUM_STRIPES, \
-							      ISP_LEFT_PADDING_VECS) + \
-							      ITERATOR_VECTOR_INCREMENT)
-#else /* !defined(HAS_RES_MGR)*/
 #define MAX_VECTORS_PER_OUTPUT_LINE \
 	CEIL_DIV(CEIL_DIV(ISP_MAX_OUTPUT_WIDTH, ISP_NUM_STRIPES) + ISP_LEFT_PADDING, ISP_VEC_NELEMS)
 
@@ -417,7 +406,6 @@ more details.
 #define MAX_VECTORS_PER_INPUT_STRIPE	CEIL_ROUND_DIV_STRIPE(MAX_VECTORS_PER_INPUT_LINE, \
 							      ISP_NUM_STRIPES, \
 							      ISP_LEFT_PADDING_VECS)
-#endif /* HAS_RES_MGR */
 
 
 /* Add 2 for left croppping */
@@ -470,15 +458,11 @@ more details.
 
 #define RAW_BUF_LINES ((ENABLE_RAW_BINNING || ENABLE_FIXED_BAYER_DS) ? 4 : 2)
 
-#if defined(HAS_RES_MGR)
-#define RAW_BUF_STRIDE (MAX_VECTORS_PER_INPUT_STRIPE)
-#else /* !defined(HAS_RES_MGR) */
 #define RAW_BUF_STRIDE \
 	(BINARY_ID == SH_CSS_BINARY_ID_POST_ISP ? MAX_VECTORS_PER_INPUT_CHUNK : \
 	 ISP_NUM_STRIPES > 1 ? MAX_VECTORS_PER_INPUT_STRIPE+_ISP_EXTRA_PADDING_VECS : \
 	 !ENABLE_CONTINUOUS ? MAX_VECTORS_PER_INPUT_LINE : \
 	 MAX_VECTORS_PER_INPUT_CHUNK)
-#endif /* HAS_RES_MGR */
 
 /* [isp vmem] table size[vectors] per line per color (GR,R,B,GB),
    multiples of NWAY */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_exprs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_exprs.h
index 8b59a8caec52..e625ba62cc15 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_exprs.h
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_exprs.h
@@ -214,24 +214,6 @@ more details.
 /******* STRIPING-RELATED MACROS *******/
 #define NO_STRIPING (ISP_NUM_STRIPES == 1)
 
-#if defined(HAS_RES_MGR)
-
-#define ISP_OUTPUT_CHUNK_VECS ISP_INTERNAL_WIDTH_VECS
-
-#if defined(__ISP)
-#define VECTORS_PER_LINE ISP_INTERNAL_WIDTH_VECS
-#else
-#define VECTORS_PER_LINE \
-	(NO_STRIPING 	? ISP_INTERNAL_WIDTH_VECS \
-				: ISP_IO_STRIPE_WIDTH_VECS(ISP_INTERNAL_WIDTH_VECS, ISP_LEFT_PADDING_VECS, ISP_NUM_STRIPES, ISP_MIN_STRIPE_WIDTH) )
-#endif
-
-#define VECTORS_PER_INPUT_LINE \
-	(NO_STRIPING 	? ISP_INPUT_WIDTH_VECS \
-				: ISP_IO_STRIPE_WIDTH_VECS(ISP_INPUT_WIDTH_VECS, ISP_LEFT_PADDING_VECS, ISP_NUM_STRIPES, ISP_MIN_STRIPE_WIDTH) )
-
-#else
-
 #define ISP_OUTPUT_CHUNK_VECS \
 	(NO_STRIPING 	? CEIL_DIV_CHUNKS(ISP_OUTPUT_VECS_EXTRA_CROP, OUTPUT_NUM_CHUNKS) \
 				: ISP_IO_STRIPE_WIDTH_VECS(ISP_OUTPUT_VECS_EXTRA_CROP, ISP_LEFT_PADDING_VECS, ISP_NUM_STRIPES, ISP_MIN_STRIPE_WIDTH) )
@@ -244,7 +226,6 @@ more details.
 	(NO_STRIPING 	? ISP_INPUT_WIDTH_VECS \
 				: ISP_IO_STRIPE_WIDTH_VECS(ISP_INPUT_WIDTH_VECS, ISP_LEFT_PADDING_VECS, ISP_NUM_STRIPES, ISP_MIN_STRIPE_WIDTH)+_ISP_EXTRA_PADDING_VECS)
 
-#endif
 
 #define ISP_MAX_VF_OUTPUT_STRIPE_VECS \
 	(NO_STRIPING 	? ISP_MAX_VF_OUTPUT_VECS \
@@ -282,11 +263,7 @@ more details.
 #define OUTPUT_VECTORS_PER_CHUNK	CEIL_DIV_CHUNKS(VECTORS_PER_LINE,OUTPUT_NUM_CHUNKS)
 
 /* should be even?? */
-#if !defined(HAS_RES_MGR)
 #define OUTPUT_C_VECTORS_PER_CHUNK  	CEIL_DIV(OUTPUT_VECTORS_PER_CHUNK, 2)
-#else
-#define OUTPUT_C_VECTORS_PER_CHUNK  	CEIL_DIV(MAX_VECTORS_PER_CHUNK, 2)
-#endif
 
 #ifndef ISP2401
 /**** SCTBL defs *******/
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/binary/src/binary.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/binary/src/binary.c
index a8b93a756e41..9f8a125f0d74 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/binary/src/binary.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/binary/src/binary.c
@@ -36,10 +36,6 @@
 #endif
 
 #include "camera/pipe/interface/ia_css_pipe_binarydesc.h"
-#if defined(HAS_RES_MGR)
-#include <components/resolutions_mgr/src/host/resolutions_mgr.host.h>
-#include <components/acc_cluster/acc_dvs_stat/host/dvs_stat.host.h>
-#endif
 
 #include "memory_access.h"
 
@@ -110,10 +106,6 @@ ia_css_binary_internal_res(const struct ia_css_frame_info *in_info,
 	internal_res->height = __ISP_INTERNAL_HEIGHT(isp_tmp_internal_height,
 		info->pipeline.top_cropping,
 		binary_dvs_env.height);
-#if defined(HAS_RES_MGR)
-	internal_res->height = (bds_out_info == NULL) ? internal_res->height : bds_out_info->res.height;
-	internal_res->width = (bds_out_info == NULL) ? internal_res->width: bds_out_info->res.width;
-#endif
 }
 
 #ifndef ISP2401
@@ -787,25 +779,6 @@ ia_css_binary_dvs_stat_grid_info(
 	struct ia_css_grid_info *info,
 	struct ia_css_pipe *pipe)
 {
-#if defined(HAS_RES_MGR)
-	struct ia_css_dvs_stat_grid_info *dvs_stat_info;
-	unsigned int i;
-
-	assert(binary != NULL);
-	assert(info != NULL);
-	dvs_stat_info = &info->dvs_grid.dvs_stat_grid_info;
-
-	if (binary->info->sp.enable.dvs_stats) {
-		for (i = 0; i < IA_CSS_SKC_DVS_STAT_NUM_OF_LEVELS; i++) {
-			dvs_stat_info->grd_cfg[i].grd_start.enable = 1;
-		}
-		ia_css_dvs_stat_grid_calculate(pipe, dvs_stat_info);
-	}
-	else {
-		memset(dvs_stat_info, 0, sizeof(struct ia_css_dvs_stat_grid_info));
-	}
-
-#endif
 	(void)pipe;
 	sh_css_binary_common_grid_info(binary, info);
 	return;
@@ -1088,9 +1061,6 @@ binary_in_frame_padded_width(int in_frame_width,
 	/* in other cases, the left padding pixels are always 128 */
 	nr_of_left_paddings = 2*ISP_VEC_NELEMS;
 #endif
-#if defined(HAS_RES_MGR)
-	(void)dvs_env_width;
-#endif
 	if (need_scaling) {
 		/* In SDV use-case, we need to match left-padding of
 		 * primary and the video binary. */
@@ -1101,9 +1071,7 @@ binary_in_frame_padded_width(int in_frame_width,
 					2*ISP_VEC_NELEMS);
 		} else {
 			/* Different than before, we do left&right padding. */
-#if !defined(HAS_RES_MGR) /* dvs env is included already */
 			in_frame_width += dvs_env_width;
-#endif
 			rval =
 				CEIL_MUL(in_frame_width +
 					(left_cropping ? nr_of_left_paddings : 0),
@@ -1214,10 +1182,8 @@ ia_css_binary_fill_info(const struct ia_css_binary_xinfo *xinfo,
 		binary->in_frame_info.res.width = in_info->res.width + info->pipeline.left_cropping;
 		binary->in_frame_info.res.height = in_info->res.height + info->pipeline.top_cropping;
 
-#if !defined(HAS_RES_MGR) /* dvs env is included already */
 		binary->in_frame_info.res.width += dvs_env_width;
 		binary->in_frame_info.res.height += dvs_env_height;
-#endif
 
 		binary->in_frame_info.padded_width =
 			binary_in_frame_padded_width(in_info->res.width,
@@ -1658,7 +1624,7 @@ ia_css_binary_find(struct ia_css_binary_descr *descr,
 			candidate->internal.max_height);
 			continue;
 		}
-		if (!candidate->enable.ds && need_ds & !(xcandidate->num_output_pins > 1)) {
+		if (!candidate->enable.ds && need_ds && !(xcandidate->num_output_pins > 1)) {
 			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
 				"ia_css_binary_find() [%d] continue: !%d && %d\n",
 				__LINE__, candidate->enable.ds, (int)need_ds);
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/bufq/src/bufq.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/bufq/src/bufq.c
index ed33d4c4c84a..5d40afd482f5 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/bufq/src/bufq.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/bufq/src/bufq.c
@@ -239,7 +239,7 @@ static ia_css_queue_t *bufq_get_qhandle(
 	enum sh_css_queue_id id,
 	int thread)
 {
-	ia_css_queue_t *q = 0;
+	ia_css_queue_t *q = NULL;
 
 	switch (type) {
 	case sh_css_host2sp_buffer_queue:
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/interface/ia_css_debug.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/interface/ia_css_debug.h
index be7df3a30c21..91c105cc6204 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/interface/ia_css_debug.h
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/interface/ia_css_debug.h
@@ -137,6 +137,7 @@ ia_css_debug_vdtrace(unsigned int level, const char *fmt, va_list args)
 		sh_css_vprint(fmt, args);
 }
 
+__printf(2, 3)
 extern void ia_css_debug_dtrace(unsigned int level, const char *fmt, ...);
 
 /*! @brief Dump sp thread's stack contents
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/src/ia_css_debug.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/src/ia_css_debug.c
index 030810bd0878..0fa7cb2423d8 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/src/ia_css_debug.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/src/ia_css_debug.c
@@ -176,7 +176,6 @@ void ia_css_debug_dtrace(unsigned int level, const char *fmt, ...)
 	va_end(ap);
 }
 
-#if !defined(HRT_UNSCHED)
 static void debug_dump_long_array_formatted(
 	const sp_ID_t sp_id,
 	hrt_address stack_sp_addr,
@@ -249,12 +248,6 @@ void ia_css_debug_dump_sp_stack_info(void)
 {
 	debug_dump_sp_stack_info(SP0_ID);
 }
-#else
-/* Empty def for crun */
-void ia_css_debug_dump_sp_stack_info(void)
-{
-}
-#endif /* #if !HRT_UNSCHED */
 
 
 void ia_css_debug_set_dtrace_level(const unsigned int trace_level)
@@ -3148,8 +3141,8 @@ ia_css_debug_dump_pipe_config(
 		ia_css_debug_dump_frame_info(&config->vf_output_info[i],
 				"vf_output_info");
 	}
-	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "acc_extension: 0x%x\n",
-			config->acc_extension);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "acc_extension: %p\n",
+			    config->acc_extension);
 	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "num_acc_stages: %d\n",
 			config->num_acc_stages);
 	ia_css_debug_dump_capture_config(&config->default_capture_config);
@@ -3179,7 +3172,7 @@ ia_css_debug_dump_stream_config_source(
 		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "timeout: %d\n",
 				config->source.port.timeout);
 		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "compression: %d\n",
-				config->source.port.compression);
+				config->source.port.compression.type);
 		break;
 	case IA_CSS_INPUT_MODE_TPG:
 		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "source.tpg\n");
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/spctrl/src/spctrl.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/spctrl/src/spctrl.c
index b36d7b00ebe8..d9178e80dab2 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/spctrl/src/spctrl.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/spctrl/src/spctrl.c
@@ -57,17 +57,11 @@ enum ia_css_err ia_css_spctrl_load_fw(sp_ID_t sp_id,
 	hrt_vaddress code_addr = mmgr_NULL;
 	struct ia_css_sp_init_dmem_cfg *init_dmem_cfg;
 
-	if ((sp_id >= N_SP_ID) || (spctrl_cfg == 0))
+	if ((sp_id >= N_SP_ID) || (spctrl_cfg == NULL))
 		return IA_CSS_ERR_INVALID_ARGUMENTS;
 
 	spctrl_cofig_info[sp_id].code_addr = mmgr_NULL;
 
-#if defined(HRT_UNSCHED)
-	(void)init_dmem_cfg;
-	code_addr = mmgr_malloc(1);
-	if (code_addr == mmgr_NULL)
-		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
-#else
 	init_dmem_cfg = &spctrl_cofig_info[sp_id].dmem_config;
 	init_dmem_cfg->dmem_data_addr = spctrl_cfg->dmem_data_addr;
 	init_dmem_cfg->dmem_bss_addr  = spctrl_cfg->dmem_bss_addr;
@@ -104,7 +98,7 @@ enum ia_css_err ia_css_spctrl_load_fw(sp_ID_t sp_id,
 		code_addr = mmgr_NULL;
 		return IA_CSS_ERR_INTERNAL_ERROR;
 	}
-#endif
+
 	spctrl_cofig_info[sp_id].sp_entry = spctrl_cfg->sp_entry;
 	spctrl_cofig_info[sp_id].code_addr = code_addr;
 	spctrl_cofig_info[sp_id].program_name = spctrl_cfg->program_name;
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css.c
index 73c76583610a..471f2be974e2 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css.c
@@ -64,7 +64,7 @@
 #include "input_system.h"
 #endif
 #include "mmu_device.h"		/* mmu_set_page_table_base_index(), ... */
-//#include "ia_css_mmu_private.h" /* sh_css_mmu_set_page_table_base_index() */
+#include "ia_css_mmu_private.h" /* sh_css_mmu_set_page_table_base_index() */
 #include "gdc_device.h"		/* HRT_GDC_N */
 #include "dma.h"		/* dma_set_max_burst_size() */
 #include "irq.h"			/* virq */
@@ -98,18 +98,8 @@ static int thread_alive;
 
 #include "isp/modes/interface/input_buf.isp.h"
 
-#if defined(HAS_BL)
-#include "support/bootloader/interface/ia_css_blctrl.h"
-#endif
-#if defined(HAS_RES_MGR)
-#include "components/acc_cluster/gen/host/acc_cluster.host.h"
-#endif
-
 /* Name of the sp program: should not be built-in */
 #define SP_PROG_NAME "sp"
-#if defined(HAS_BL)
-#define BL_PROG_NAME "bootloader"
-#endif
 /* Size of Refcount List */
 #define REFCOUNT_SIZE 1000
 
@@ -252,11 +242,6 @@ ia_css_reset_defaults(struct sh_css* css);
 static void
 sh_css_init_host_sp_control_vars(void);
 
-#ifndef ISP2401
-static void
-sh_css_mmu_set_page_table_base_index(hrt_data base_index);
-
-#endif
 static enum ia_css_err set_num_primary_stages(unsigned int *num, enum ia_css_pipe_version version);
 
 static bool
@@ -385,13 +370,8 @@ sh_css_hmm_buffer_record_uninit(void);
 static void
 sh_css_hmm_buffer_record_reset(struct sh_css_hmm_buffer_record *buffer_record);
 
-#ifndef ISP2401
-static bool
-sh_css_hmm_buffer_record_acquire(struct ia_css_rmgr_vbuf_handle *h_vbuf,
-#else
 static struct sh_css_hmm_buffer_record
 *sh_css_hmm_buffer_record_acquire(struct ia_css_rmgr_vbuf_handle *h_vbuf,
-#endif
 			enum ia_css_buffer_type type,
 			hrt_address kernel_ptr);
 
@@ -1475,30 +1455,17 @@ static void start_pipe(
 				copy_ovrd,
 				input_mode,
 				&me->stream->config.metadata_config,
-#ifndef ISP2401
 				&me->stream->info.metadata_info
-#else
-				&me->stream->info.metadata_info,
-#endif
 #if !defined(HAS_NO_INPUT_SYSTEM)
-#ifndef ISP2401
-				, (input_mode==IA_CSS_INPUT_MODE_MEMORY)?
-#else
-				(input_mode == IA_CSS_INPUT_MODE_MEMORY) ?
-#endif
+				,(input_mode==IA_CSS_INPUT_MODE_MEMORY) ?
 					(mipi_port_ID_t)0 :
-#ifndef ISP2401
 					me->stream->config.source.port.port
-#else
-					me->stream->config.source.port.port,
 #endif
+#ifdef ISP2401
+				,&me->config.internal_frame_origin_bqs_on_sctbl,
+				me->stream->isp_params_configs
 #endif
-#ifndef ISP2401
-				);
-#else
-				&me->config.internal_frame_origin_bqs_on_sctbl,
-				me->stream->isp_params_configs);
-#endif
+			);
 
 	if (me->config.mode != IA_CSS_PIPE_MODE_COPY) {
 		struct ia_css_pipeline_stage *stage;
@@ -1571,34 +1538,7 @@ enable_interrupts(enum ia_css_irq_type irq_type)
 }
 
 #endif
-#if defined(HAS_BL)
-static bool sh_css_setup_blctrl_config(const struct ia_css_fw_info *fw,
-							const char *program,
-							ia_css_blctrl_cfg  *blctrl_cfg)
-{
-	if((fw == NULL)||(blctrl_cfg == NULL))
-		return false;
-	blctrl_cfg->bl_entry = 0;
-	blctrl_cfg->program_name = (char *)(program);
-
-#if !defined(HRT_UNSCHED)
-	blctrl_cfg->ddr_data_offset =  fw->blob.data_source;
-	blctrl_cfg->dmem_data_addr = fw->blob.data_target;
-	blctrl_cfg->dmem_bss_addr = fw->blob.bss_target;
-	blctrl_cfg->data_size = fw->blob.data_size ;
-	blctrl_cfg->bss_size = fw->blob.bss_size;
 
-	blctrl_cfg->blctrl_state_dmem_addr = fw->info.bl.sw_state;
-	blctrl_cfg->blctrl_dma_cmd_list = fw->info.bl.dma_cmd_list;
-	blctrl_cfg->blctrl_nr_of_dma_cmds = fw->info.bl.num_dma_cmds;
-
-	blctrl_cfg->code_size = fw->blob.size;
-	blctrl_cfg->code      = fw->blob.code;
-	blctrl_cfg->bl_entry  = fw->info.bl.bl_entry; /* entry function ptr on Bootloader */
-#endif
-	return true;
-}
-#endif
 static bool sh_css_setup_spctrl_config(const struct ia_css_fw_info *fw,
 							const char * program,
 							ia_css_spctrl_cfg  *spctrl_cfg)
@@ -1608,7 +1548,6 @@ static bool sh_css_setup_spctrl_config(const struct ia_css_fw_info *fw,
 	spctrl_cfg->sp_entry = 0;
 	spctrl_cfg->program_name = (char *)(program);
 
-#if !defined(HRT_UNSCHED)
 	spctrl_cfg->ddr_data_offset =  fw->blob.data_source;
 	spctrl_cfg->dmem_data_addr = fw->blob.data_target;
 	spctrl_cfg->dmem_bss_addr = fw->blob.bss_target;
@@ -1621,7 +1560,7 @@ static bool sh_css_setup_spctrl_config(const struct ia_css_fw_info *fw,
 	spctrl_cfg->code_size = fw->blob.size;
 	spctrl_cfg->code      = fw->blob.code;
 	spctrl_cfg->sp_entry  = fw->info.sp.sp_entry; /* entry function ptr on SP */
-#endif
+
 	return true;
 }
 void
@@ -1708,9 +1647,6 @@ ia_css_init(const struct ia_css_env *env,
 {
 	enum ia_css_err err;
 	ia_css_spctrl_cfg spctrl_cfg;
-#if defined(HAS_BL)
-	ia_css_blctrl_cfg blctrl_cfg;
-#endif
 
 	void (*flush_func)(struct ia_css_acc_fw *fw);
 	hrt_data select, enable;
@@ -1863,26 +1799,6 @@ ia_css_init(const struct ia_css_env *env,
 		return err;
 	}
 
-#if defined(HAS_BL)
-	if (!sh_css_setup_blctrl_config(&sh_css_bl_fw, BL_PROG_NAME, &blctrl_cfg))
-		return IA_CSS_ERR_INTERNAL_ERROR;
-	err = ia_css_blctrl_load_fw(&blctrl_cfg);
-	if (err != IA_CSS_SUCCESS) {
-		IA_CSS_LEAVE_ERR(err);
-		return err;
-	}
-
-#ifdef ISP2401
-	err = ia_css_blctrl_add_target_fw_info(&sh_css_sp_fw, IA_CSS_SP0,
-					 get_sp_code_addr(SP0_ID));
-
-#endif
-	if (err != IA_CSS_SUCCESS) {
-		IA_CSS_LEAVE_ERR(err);
-		return err;
-	}
-#endif /* HAS_BL */
-
 #if WITH_PC_MONITORING
 	if (!thread_alive) {
 		thread_alive++;
@@ -2003,7 +1919,7 @@ ia_css_enable_isys_event_queue(bool enable)
 
 void *sh_css_malloc(size_t size)
 {
-	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "sh_css_malloc() enter: size=%d\n",size);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "sh_css_malloc() enter: size=%zu\n",size);
 	/* FIXME: This first test can probably go away */
 	if (size == 0)
 		return NULL;
@@ -2016,7 +1932,7 @@ void *sh_css_calloc(size_t N, size_t size)
 {
 	void *p;
 
-	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "sh_css_calloc() enter: N=%d, size=%d\n",N,size);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "sh_css_calloc() enter: N=%zu, size=%zu\n",N,size);
 
 	/* FIXME: this test can probably go away */
 	if (size > 0) {
@@ -2059,7 +1975,8 @@ map_sp_threads(struct ia_css_stream *stream, bool map)
 	enum ia_css_pipe_id pipe_id;
 
 	assert(stream != NULL);
-	IA_CSS_ENTER_PRIVATE("stream = %p, map = %p", stream, map);
+	IA_CSS_ENTER_PRIVATE("stream = %p, map = %s",
+			     stream, map ? "true" : "false");
 
 	if (stream == NULL) {
 		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
@@ -2611,15 +2528,8 @@ ia_css_pipe_destroy(struct ia_css_pipe *pipe)
 		break;
 	}
 
-#ifndef ISP2401
-	if (pipe->scaler_pp_lut != mmgr_NULL) {
-		hmm_free(pipe->scaler_pp_lut);
-		pipe->scaler_pp_lut = mmgr_NULL;
-	}
-#else
 	sh_css_params_free_gdc_lut(pipe->scaler_pp_lut);
 	pipe->scaler_pp_lut = mmgr_NULL;
-#endif
 
 	my_css.active_pipes[ia_css_pipe_get_pipe_num(pipe)] = NULL;
 	sh_css_pipe_free_shading_table(pipe);
@@ -2666,9 +2576,6 @@ ia_css_uninit(void)
 	}
 	ia_css_spctrl_unload_fw(SP0_ID);
 	sh_css_sp_set_sp_running(false);
-#if defined(HAS_BL)
-	ia_css_blctrl_unload_fw();
-#endif
 #if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
 	/* check and free any remaining mipi frames */
 	free_mipi_frames(NULL);
@@ -2683,23 +2590,6 @@ ia_css_uninit(void)
 	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_uninit() leave: return_void\n");
 }
 
-#ifndef ISP2401
-/* Deprecated, this is an HRT backend function (memory_access.h) */
-static void
-sh_css_mmu_set_page_table_base_index(hrt_data base_index)
-{
-	int i;
-	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "sh_css_mmu_set_page_table_base_index() enter: base_index=0x%08x\n",base_index);
-	my_css.page_table_base_index = base_index;
-	for (i = 0; i < (int)N_MMU_ID; i++) {
-		mmu_ID_t mmu_id = (mmu_ID_t)i;
-		mmu_set_page_table_base_index(mmu_id, base_index);
-		mmu_invalidate_cache(mmu_id);
-	}
-	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "sh_css_mmu_set_page_table_base_index() leave: return_void\n");
-}
-
-#endif
 #if defined(HAS_IRQ_MAP_VERSION_2)
 enum ia_css_err ia_css_irq_translate(
 	unsigned int *irq_infos)
@@ -2766,7 +2656,7 @@ enum ia_css_err ia_css_irq_translate(
 		*irq_infos = infos;
 
 	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_irq_translate() "
-		"leave: irq_infos=%p\n", infos);
+		"leave: irq_infos=%u\n", infos);
 
 	return IA_CSS_SUCCESS;
 }
@@ -3004,11 +2894,8 @@ load_preview_binaries(struct ia_css_pipe *pipe)
 #endif
 	/* preview only have 1 output pin now */
 	struct ia_css_frame_info *pipe_out_info = &pipe->output_info[0];
-#ifdef ISP2401
 	struct ia_css_preview_settings *mycs  = &pipe->pipe_settings.preview;
 
-#endif
-
 	IA_CSS_ENTER_PRIVATE("");
 	assert(pipe != NULL);
 	assert(pipe->stream != NULL);
@@ -3020,11 +2907,7 @@ load_preview_binaries(struct ia_css_pipe *pipe)
 	sensor = pipe->stream->config.mode == IA_CSS_INPUT_MODE_SENSOR;
 #endif
 
-#ifndef ISP2401
-	if (pipe->pipe_settings.preview.preview_binary.info)
-#else
 	if (mycs->preview_binary.info)
-#endif
 		return IA_CSS_SUCCESS;
 
 	err = ia_css_util_check_input(&pipe->stream->config, false, false);
@@ -3077,12 +2960,7 @@ load_preview_binaries(struct ia_css_pipe *pipe)
 			&prev_vf_info);
 	if (err != IA_CSS_SUCCESS)
 		return err;
-	err = ia_css_binary_find(&preview_descr,
-#ifndef ISP2401
-				 &pipe->pipe_settings.preview.preview_binary);
-#else
-				 &mycs->preview_binary);
-#endif
+	err = ia_css_binary_find(&preview_descr, &mycs->preview_binary);
 	if (err != IA_CSS_SUCCESS)
 		return err;
 
@@ -3098,24 +2976,15 @@ load_preview_binaries(struct ia_css_pipe *pipe)
 
 #endif
 	/* The vf_pp binary is needed when (further) YUV downscaling is required */
-#ifndef ISP2401
-	need_vf_pp |= pipe->pipe_settings.preview.preview_binary.out_frame_info[0].res.width != pipe_out_info->res.width;
-	need_vf_pp |= pipe->pipe_settings.preview.preview_binary.out_frame_info[0].res.height != pipe_out_info->res.height;
-#else
 	need_vf_pp |= mycs->preview_binary.out_frame_info[0].res.width != pipe_out_info->res.width;
 	need_vf_pp |= mycs->preview_binary.out_frame_info[0].res.height != pipe_out_info->res.height;
-#endif
 
 	/* When vf_pp is needed, then the output format of the selected
 	 * preview binary must be yuv_line. If this is not the case,
 	 * then the preview binary selection is done again.
 	 */
 	if (need_vf_pp &&
-#ifndef ISP2401
-		(pipe->pipe_settings.preview.preview_binary.out_frame_info[0].format != IA_CSS_FRAME_FORMAT_YUV_LINE)) {
-#else
 		(mycs->preview_binary.out_frame_info[0].format != IA_CSS_FRAME_FORMAT_YUV_LINE)) {
-#endif
 
 		/* Preview step 2 */
 		if (pipe->vf_yuv_ds_input_info.res.width)
@@ -3136,11 +3005,7 @@ load_preview_binaries(struct ia_css_pipe *pipe)
 		if (err != IA_CSS_SUCCESS)
 			return err;
 		err = ia_css_binary_find(&preview_descr,
-#ifndef ISP2401
-				&pipe->pipe_settings.preview.preview_binary);
-#else
 				&mycs->preview_binary);
-#endif
 		if (err != IA_CSS_SUCCESS)
 			return err;
 	}
@@ -3150,18 +3015,10 @@ load_preview_binaries(struct ia_css_pipe *pipe)
 
 		/* Viewfinder post-processing */
 		ia_css_pipe_get_vfpp_binarydesc(pipe, &vf_pp_descr,
-#ifndef ISP2401
-			&pipe->pipe_settings.preview.preview_binary.out_frame_info[0],
-#else
 			&mycs->preview_binary.out_frame_info[0],
-#endif
 			pipe_out_info);
 		err = ia_css_binary_find(&vf_pp_descr,
-#ifndef ISP2401
-				 &pipe->pipe_settings.preview.vf_pp_binary);
-#else
 				 &mycs->vf_pp_binary);
-#endif
 		if (err != IA_CSS_SUCCESS)
 			return err;
 	}
@@ -3187,13 +3044,8 @@ load_preview_binaries(struct ia_css_pipe *pipe)
 	/* Copy */
 	if (need_isp_copy_binary) {
 		err = load_copy_binary(pipe,
-#ifndef ISP2401
-				       &pipe->pipe_settings.preview.copy_binary,
-				       &pipe->pipe_settings.preview.preview_binary);
-#else
 				       &mycs->copy_binary,
 				       &mycs->preview_binary);
-#endif
 		if (err != IA_CSS_SUCCESS)
 			return err;
 	}
@@ -4499,22 +4351,10 @@ ia_css_pipe_enqueue_buffer(struct ia_css_pipe *pipe,
 	}
 
 	if (return_err == IA_CSS_SUCCESS) {
-#ifndef ISP2401
-		bool found_record = false;
-		found_record = sh_css_hmm_buffer_record_acquire(
-#else
-		struct sh_css_hmm_buffer_record *hmm_buffer_record = NULL;
-
-		hmm_buffer_record = sh_css_hmm_buffer_record_acquire(
-#endif
-					h_vbuf, buf_type,
-					HOST_ADDRESS(ddr_buffer.kernel_ptr));
-#ifndef ISP2401
-		if (found_record == true) {
-#else
-		if (hmm_buffer_record) {
-#endif
-			IA_CSS_LOG("send vbuf=0x%x", h_vbuf);
+		if (sh_css_hmm_buffer_record_acquire(
+				h_vbuf, buf_type,
+				HOST_ADDRESS(ddr_buffer.kernel_ptr))) {
+			IA_CSS_LOG("send vbuf=%p", h_vbuf);
 		} else {
 			return_err = IA_CSS_ERR_INTERNAL_ERROR;
 			IA_CSS_ERROR("hmm_buffer_record[]: no available slots\n");
@@ -4624,7 +4464,7 @@ ia_css_pipe_dequeue_buffer(struct ia_css_pipe *pipe,
 			ia_css_rmgr_rel_vbuf(hmm_buffer_pool, &hmm_buffer_record->h_vbuf);
 			sh_css_hmm_buffer_record_reset(hmm_buffer_record);
 		} else {
-			IA_CSS_ERROR("hmm_buffer_record not found (0x%p) buf_type(%d)",
+			IA_CSS_ERROR("hmm_buffer_record not found (0x%u) buf_type(%d)",
 				 ddr_buffer_addr, buf_type);
 			IA_CSS_LEAVE_ERR(IA_CSS_ERR_INTERNAL_ERROR);
 			return IA_CSS_ERR_INTERNAL_ERROR;
@@ -4640,8 +4480,8 @@ ia_css_pipe_dequeue_buffer(struct ia_css_pipe *pipe,
 		if ((ddr_buffer.kernel_ptr == 0) ||
 		    (kernel_ptr != HOST_ADDRESS(ddr_buffer.kernel_ptr))) {
 			IA_CSS_ERROR("kernel_ptr invalid");
-			IA_CSS_ERROR("expected: (0x%p)", kernel_ptr);
-			IA_CSS_ERROR("actual: (0x%p)", HOST_ADDRESS(ddr_buffer.kernel_ptr));
+			IA_CSS_ERROR("expected: (0x%llx)", (u64)kernel_ptr);
+			IA_CSS_ERROR("actual: (0x%llx)", (u64)HOST_ADDRESS(ddr_buffer.kernel_ptr));
 			IA_CSS_ERROR("buf_type: %d\n", buf_type);
 			IA_CSS_LEAVE_ERR(IA_CSS_ERR_INTERNAL_ERROR);
 			return IA_CSS_ERR_INTERNAL_ERROR;
@@ -6316,9 +6156,6 @@ static enum ia_css_err load_primary_binaries(
 #else
 				 *pipe_vf_out_info;
 #endif
-#if defined(HAS_RES_MGR)
-	struct ia_css_frame_info bds_out_info;
-#endif
 	enum ia_css_err err = IA_CSS_SUCCESS;
 	struct ia_css_capture_settings *mycs;
 	unsigned int i;
@@ -6440,10 +6277,6 @@ static enum ia_css_err load_primary_binaries(
 				&cas_scaler_descr.out_info[i],
 				&cas_scaler_descr.internal_out_info[i],
 				&cas_scaler_descr.vf_info[i]);
-#if defined(HAS_RES_MGR)
-			bds_out_info.res = pipe->config.bayer_ds_out_res;
-			yuv_scaler_descr.bds_out_info = &bds_out_info;
-#endif
 			err = ia_css_binary_find(&yuv_scaler_descr,
 						&mycs->yuv_scaler_binary[i]);
 			if (err != IA_CSS_SUCCESS) {
@@ -6494,10 +6327,6 @@ static enum ia_css_err load_primary_binaries(
 			&capture_pp_descr, &prim_out_info,
 #endif
 			&capt_pp_out_info, &vf_info);
-#if defined(HAS_RES_MGR)
-			bds_out_info.res = pipe->config.bayer_ds_out_res;
-			capture_pp_descr.bds_out_info = &bds_out_info;
-#endif
 		err = ia_css_binary_find(&capture_pp_descr,
 					&mycs->capture_pp_binary);
 		if (err != IA_CSS_SUCCESS) {
@@ -6533,10 +6362,6 @@ static enum ia_css_err load_primary_binaries(
 			if (pipe->enable_viewfinder[IA_CSS_PIPE_OUTPUT_STAGE_0] && (i == mycs->num_primary_stage - 1))
 				local_vf_info = &vf_info;
 			ia_css_pipe_get_primary_binarydesc(pipe, &prim_descr[i], &prim_in_info, &prim_out_info, local_vf_info, i);
-#if defined(HAS_RES_MGR)
-			bds_out_info.res = pipe->config.bayer_ds_out_res;
-			prim_descr[i].bds_out_info = &bds_out_info;
-#endif
 			err = ia_css_binary_find(&prim_descr[i], &mycs->primary_binary[i]);
 			if (err != IA_CSS_SUCCESS) {
 				IA_CSS_LEAVE_ERR_PRIVATE(err);
@@ -6570,10 +6395,6 @@ static enum ia_css_err load_primary_binaries(
 
 		ia_css_pipe_get_vfpp_binarydesc(pipe,
 				&vf_pp_descr, vf_pp_in_info, pipe_vf_out_info);
-#if defined(HAS_RES_MGR)
-		bds_out_info.res = pipe->config.bayer_ds_out_res;
-		vf_pp_descr.bds_out_info = &bds_out_info;
-#endif
 		err = ia_css_binary_find(&vf_pp_descr, &mycs->vf_pp_binary);
 		if (err != IA_CSS_SUCCESS) {
 			IA_CSS_LEAVE_ERR_PRIVATE(err);
@@ -6621,7 +6442,7 @@ allocate_delay_frames(struct ia_css_pipe *pipe)
 	IA_CSS_ENTER_PRIVATE("");
 
 	if (pipe == NULL) {
-		IA_CSS_ERROR("Invalid args - pipe %x", pipe);
+		IA_CSS_ERROR("Invalid args - pipe %p", pipe);
 		return IA_CSS_ERR_INVALID_ARGUMENTS;
 	}
 
@@ -8628,9 +8449,7 @@ remove_firmware(struct ia_css_fw_info **l, struct ia_css_fw_info *firmware)
 	return; /* removing single and multiple firmware is handled in acc_unload_extension() */
 }
 
-#if !defined(HRT_UNSCHED)
-static enum ia_css_err
-upload_isp_code(struct ia_css_fw_info *firmware)
+static enum ia_css_err upload_isp_code(struct ia_css_fw_info *firmware)
 {
 	hrt_vaddress binary;
 
@@ -8658,12 +8477,10 @@ upload_isp_code(struct ia_css_fw_info *firmware)
 		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
 	return IA_CSS_SUCCESS;
 }
-#endif
 
 static enum ia_css_err
 acc_load_extension(struct ia_css_fw_info *firmware)
 {
-#if !defined(HRT_UNSCHED)
 	enum ia_css_err err;
 	struct ia_css_fw_info *hd = firmware;
 	while (hd){
@@ -8672,7 +8489,6 @@ acc_load_extension(struct ia_css_fw_info *firmware)
 			return err;
 		hd = hd->next;
 	}
-#endif
 
 	if (firmware == NULL)
 		return IA_CSS_ERR_INVALID_ARGUMENTS;
@@ -9879,9 +9695,6 @@ ia_css_stream_create(const struct ia_css_stream_config *stream_config,
 		/* take over effective info */
 
 		effective_res = curr_pipe->config.input_effective_res;
-#endif
-
-#ifndef ISP2401
 		err = ia_css_util_check_res(
 					effective_res.width,
 					effective_res.height);
@@ -9902,13 +9715,6 @@ ia_css_stream_create(const struct ia_css_stream_config *stream_config,
 		if (err != IA_CSS_SUCCESS)
 			goto ERR;
 
-#if defined(HAS_RES_MGR)
-		/* update acc configuration - striping info is ready */
-		err = ia_css_update_cfg_stripe_info(curr_pipe);
-		if (err != IA_CSS_SUCCESS)
-			goto ERR;
-#endif
-
 		/* handle each pipe */
 		pipe_info = &curr_pipe->info;
 		for (j = 0; j < IA_CSS_PIPE_MAX_OUTPUT_STAGE; j++) {
@@ -10587,39 +10393,6 @@ ia_css_pipe_get_isp_pipe_version(const struct ia_css_pipe *pipe)
 	return (unsigned int)pipe->config.isp_pipe_version;
 }
 
-#if defined(HAS_BL)
-#define BL_START_TIMEOUT_US 30000000
-static enum ia_css_err
-ia_css_start_bl(void)
-{
-	enum ia_css_err err = IA_CSS_SUCCESS;
-	unsigned long timeout;
-
-	IA_CSS_ENTER("");
-	sh_css_start_bl();
-	/* waiting for the Bootloader to complete execution */
-	timeout = BL_START_TIMEOUT_US;
-	while((ia_css_blctrl_get_state() == BOOTLOADER_BUSY) && timeout) {
-		timeout--;
-		hrt_sleep();
-	}
-	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
-			    "Bootloader state %d\n", ia_css_blctrl_get_state());
-	if (timeout == 0) {
-		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
-				    "Bootloader Execution Timeout\n");
-		err = IA_CSS_ERR_INTERNAL_ERROR;
-	}
-	if (ia_css_blctrl_get_state() != BOOTLOADER_OK) {
-		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
-				    "Bootloader Execution Failed\n");
-		err = IA_CSS_ERR_INTERNAL_ERROR;
-	}
-	IA_CSS_LEAVE_ERR(err);
-	return err;
-}
-#endif
-
 #define SP_START_TIMEOUT_US 30000000
 
 enum ia_css_err
@@ -10629,15 +10402,6 @@ ia_css_start_sp(void)
 	enum ia_css_err err = IA_CSS_SUCCESS;
 
 	IA_CSS_ENTER("");
-#if defined(HAS_BL)
-	/* Starting bootloader before Sp0 and Sp1
-	 * and not exposing CSS API */
-	err = ia_css_start_bl();
-	if (err != IA_CSS_SUCCESS) {
-		IA_CSS_LEAVE("Bootloader fails");
-		return err;
-	}
-#endif
 	sh_css_sp_start_isp();
 
 	/* waiting for the SP is completely started */
@@ -11291,23 +11055,14 @@ sh_css_hmm_buffer_record_reset(struct sh_css_hmm_buffer_record *buffer_record)
 	buffer_record->kernel_ptr = 0;
 }
 
-#ifndef ISP2401
-static bool
-sh_css_hmm_buffer_record_acquire(struct ia_css_rmgr_vbuf_handle *h_vbuf,
-#else
 static struct sh_css_hmm_buffer_record
 *sh_css_hmm_buffer_record_acquire(struct ia_css_rmgr_vbuf_handle *h_vbuf,
-#endif
 			enum ia_css_buffer_type type,
 			hrt_address kernel_ptr)
 {
 	int i;
 	struct sh_css_hmm_buffer_record *buffer_record = NULL;
-#ifndef ISP2401
-	bool found_record = false;
-#else
 	struct sh_css_hmm_buffer_record *out_buffer_record = NULL;
-#endif
 
 	assert(h_vbuf != NULL);
 	assert((type > IA_CSS_BUFFER_TYPE_INVALID) && (type < IA_CSS_NUM_DYNAMIC_BUFFER_TYPE));
@@ -11320,21 +11075,13 @@ static struct sh_css_hmm_buffer_record
 			buffer_record->type = type;
 			buffer_record->h_vbuf = h_vbuf;
 			buffer_record->kernel_ptr = kernel_ptr;
-#ifndef ISP2401
-			found_record = true;
-#else
 			out_buffer_record = buffer_record;
-#endif
 			break;
 		}
 		buffer_record++;
 	}
 
-#ifndef ISP2401
-	return found_record;
-#else
 	return out_buffer_record;
-#endif
 }
 
 static struct sh_css_hmm_buffer_record
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_firmware.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_firmware.c
index 34cc56f0b471..eecd8cf71951 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_firmware.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_firmware.c
@@ -63,9 +63,6 @@ static const char *release_version = STR(irci_ecr-master_20150911_0724);
 static char FW_rel_ver_name[MAX_FW_REL_VER_NAME] = "---";
 
 struct ia_css_fw_info	  sh_css_sp_fw;
-#if defined(HAS_BL)
-struct ia_css_fw_info     sh_css_bl_fw;
-#endif /* HAS_BL */
 struct ia_css_blob_descr *sh_css_blob_info; /* Only ISP blob info (no SP) */
 unsigned		  sh_css_num_binaries; /* This includes 1 SP binary */
 
@@ -95,12 +92,7 @@ setup_binary(struct ia_css_fw_info *fw, const char *fw_data, struct ia_css_fw_in
 
 	*sh_css_fw = *fw;
 
-#if defined(HRT_UNSCHED)
-	sh_css_fw->blob.code = vmalloc(1);
-#else
 	sh_css_fw->blob.code = vmalloc(fw->blob.size);
-#endif
-
 	if (sh_css_fw->blob.code == NULL)
 		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
 
@@ -137,12 +129,7 @@ sh_css_load_blob_info(const char *fw, const struct ia_css_fw_info *bi, struct ia
 	bd->blob = blob;
 	bd->header = *bi;
 
-	if ((bi->type == ia_css_isp_firmware) || (bi->type == ia_css_sp_firmware)
-#if defined(HAS_BL)
-	|| (bi->type == ia_css_bootloader_firmware)
-#endif /* HAS_BL */
-	)
-	{
+	if (bi->type == ia_css_isp_firmware || bi->type == ia_css_sp_firmware) {
 		char *namebuffer;
 		int namelength = (int)strlen(name);
 
@@ -242,9 +229,9 @@ sh_css_load_firmware(const char *fw_data,
 
 	sh_css_num_binaries = file_header->binary_nr;
 	/* Only allocate memory for ISP blob info */
-	if (sh_css_num_binaries > (NUM_OF_SPS + NUM_OF_BLS)) {
+	if (sh_css_num_binaries > NUM_OF_SPS) {
 		sh_css_blob_info = kmalloc(
-					(sh_css_num_binaries - (NUM_OF_SPS + NUM_OF_BLS)) *
+					(sh_css_num_binaries - NUM_OF_SPS) *
 					sizeof(*sh_css_blob_info), GFP_KERNEL);
 		if (sh_css_blob_info == NULL)
 			return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
@@ -279,25 +266,16 @@ sh_css_load_firmware(const char *fw_data,
 			err = setup_binary(bi, fw_data, &sh_css_sp_fw, i);
 			if (err != IA_CSS_SUCCESS)
 				return err;
-#if defined(HAS_BL)
-		} else if (bi->type == ia_css_bootloader_firmware) {
-			if (i != BOOTLOADER_FIRMWARE)
-				return IA_CSS_ERR_INTERNAL_ERROR;
-			err = setup_binary(bi, fw_data, &sh_css_bl_fw, i);
-			if (err != IA_CSS_SUCCESS)
-				return err;
-			IA_CSS_LOG("Bootloader binary recognized\n");
-#endif
 		} else {
-			/* All subsequent binaries (including bootloaders) (i>NUM_OF_SPS+NUM_OF_BLS) are ISP firmware */
-			if (i < (NUM_OF_SPS + NUM_OF_BLS))
+			/* All subsequent binaries (including bootloaders) (i>NUM_OF_SPS) are ISP firmware */
+			if (i < NUM_OF_SPS)
 				return IA_CSS_ERR_INTERNAL_ERROR;
 
 			if (bi->type != ia_css_isp_firmware)
 				return IA_CSS_ERR_INTERNAL_ERROR;
 			if (sh_css_blob_info == NULL) /* cannot happen but KW does not see this */
 				return IA_CSS_ERR_INTERNAL_ERROR;
-			sh_css_blob_info[i-(NUM_OF_SPS + NUM_OF_BLS)] = bd;
+			sh_css_blob_info[i - NUM_OF_SPS] = bd;
 		}
 	}
 
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_internal.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_internal.h
index e2b6f06ed099..5b2b78f96dc5 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_internal.h
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_internal.h
@@ -154,18 +154,11 @@
 /* Number of SP's */
 #define NUM_OF_SPS 1
 
-#if defined(HAS_BL)
-#define NUM_OF_BLS 1
-#else
 #define NUM_OF_BLS 0
-#endif
 
 /* Enum for order of Binaries */
 enum sh_css_order_binaries {
 	SP_FIRMWARE = 0,
-#if defined(HAS_BL)
-	BOOTLOADER_FIRMWARE,
-#endif
 	ISP_FIRMWARE
 };
 
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_irq.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_irq.c
deleted file mode 100644
index 37e954aea36f..000000000000
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_irq.c
+++ /dev/null
@@ -1,16 +0,0 @@
-/*
- * Support for Intel Camera Imaging ISP subsystem.
- * Copyright (c) 2015, Intel Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- */
-
-/* This file will contain the code to implement the functions declared in ia_css_irq.h
-   and associated helper functions */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mipi.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mipi.c
index 7e3893c6c08a..36aaa3019a15 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mipi.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mipi.c
@@ -681,7 +681,7 @@ send_mipi_frames(struct ia_css_pipe *pipe)
 	unsigned int port = 0;
 #endif
 
-	IA_CSS_ENTER_PRIVATE("pipe=%d", pipe);
+	IA_CSS_ENTER_PRIVATE("pipe=%p", pipe);
 
 	assert(pipe != NULL);
 	assert(pipe->stream != NULL);
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mmu.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mmu.c
index 6de8472f1b07..237e38b2f0c1 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mmu.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mmu.c
@@ -13,16 +13,12 @@
  */
 
 #include "ia_css_mmu.h"
-#ifdef ISP2401
 #include "ia_css_mmu_private.h"
-#endif
 #include <ia_css_debug.h>
 #include "sh_css_sp.h"
 #include "sh_css_firmware.h"
 #include "sp.h"
-#ifdef ISP2401
 #include "mmu_device.h"
-#endif
 
 void
 ia_css_mmu_invalidate_cache(void)
@@ -44,7 +40,6 @@ ia_css_mmu_invalidate_cache(void)
 	}
 	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_mmu_invalidate_cache() leave\n");
 }
-#ifdef ISP2401
 
 /* Deprecated, this is an HRT backend function (memory_access.h) */
 void
@@ -59,4 +54,3 @@ sh_css_mmu_set_page_table_base_index(hrt_data base_index)
 	}
 	IA_CSS_LEAVE_PRIVATE("");
 }
-#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_params.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_params.c
index 561f4a7236f7..48224370b8bf 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_params.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_params.c
@@ -3356,15 +3356,8 @@ enum ia_css_err ia_css_pipe_set_bci_scaler_lut(struct ia_css_pipe *pipe,
 	}
 
 	/* Free any existing tables. */
-#ifndef ISP2401
-	if (pipe->scaler_pp_lut != mmgr_NULL) {
-		hmm_free(pipe->scaler_pp_lut);
-		pipe->scaler_pp_lut = mmgr_NULL;
-	}
-#else
 	sh_css_params_free_gdc_lut(pipe->scaler_pp_lut);
 	pipe->scaler_pp_lut = mmgr_NULL;
-#endif
 
 #ifndef ISP2401
 	if (store) {
@@ -3375,7 +3368,7 @@ enum ia_css_err ia_css_pipe_set_bci_scaler_lut(struct ia_css_pipe *pipe,
 #endif
 		if (pipe->scaler_pp_lut == mmgr_NULL) {
 #ifndef ISP2401
-			IA_CSS_LEAVE("lut(%p) err=%d", pipe->scaler_pp_lut, err);
+			IA_CSS_LEAVE("lut(%u) err=%d", pipe->scaler_pp_lut, err);
 			return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
 #else
 			ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
@@ -3397,7 +3390,7 @@ enum ia_css_err ia_css_pipe_set_bci_scaler_lut(struct ia_css_pipe *pipe,
 #endif
 	}
 
-	IA_CSS_LEAVE("lut(%p) err=%d", pipe->scaler_pp_lut, err);
+	IA_CSS_LEAVE("lut(%u) err=%d", pipe->scaler_pp_lut, err);
 	return err;
 }
 
@@ -3437,7 +3430,7 @@ enum ia_css_err sh_css_params_map_and_store_default_gdc_lut(void)
 	mmgr_store(default_gdc_lut, (int *)interleaved_lut_temp,
 		sizeof(zoom_table));
 
-	IA_CSS_LEAVE_PRIVATE("lut(%p) err=%d", default_gdc_lut, err);
+	IA_CSS_LEAVE_PRIVATE("lut(%u) err=%d", default_gdc_lut, err);
 	return err;
 }
 
@@ -3445,15 +3438,8 @@ void sh_css_params_free_default_gdc_lut(void)
 {
 	IA_CSS_ENTER_PRIVATE("void");
 
-#ifndef ISP2401
-	if (default_gdc_lut != mmgr_NULL) {
-		hmm_free(default_gdc_lut);
-		default_gdc_lut = mmgr_NULL;
-	}
-#else
 	sh_css_params_free_gdc_lut(default_gdc_lut);
 	default_gdc_lut = mmgr_NULL;
-#endif
 
 	IA_CSS_LEAVE_PRIVATE("void");
 
@@ -3859,7 +3845,7 @@ sh_css_param_update_isp_params(struct ia_css_pipe *curr_pipe,
 		/* When API change is implemented making good distinction between
 		* stream config and pipe config this skipping code can be moved out of the #ifdef */
 		if (pipe_in && (pipe != pipe_in)) {
-			IA_CSS_LOG("skipping pipe %x", pipe);
+			IA_CSS_LOG("skipping pipe %p", pipe);
 			continue;
 		}
 
@@ -4590,7 +4576,7 @@ free_ia_css_isp_parameter_set_info(
 	unsigned int i;
 	hrt_vaddress *addrs = (hrt_vaddress *)&isp_params_info.mem_map;
 
-	IA_CSS_ENTER_PRIVATE("ptr = %p", ptr);
+	IA_CSS_ENTER_PRIVATE("ptr = %u", ptr);
 
 	/* sanity check - ptr must be valid */
 	if (!ia_css_refcount_is_valid(ptr)) {
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm.c b/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm.c
index 57295397da3e..05eeff58a229 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm.c
@@ -43,6 +43,7 @@ struct hmm_bo_device bo_device;
 struct hmm_pool	dynamic_pool;
 struct hmm_pool	reserved_pool;
 static ia_css_ptr dummy_ptr;
+static bool hmm_initialized;
 struct _hmm_mem_stat hmm_mem_stat;
 
 /* p: private
@@ -186,6 +187,8 @@ int hmm_init(void)
 	if (ret)
 		dev_err(atomisp_dev, "hmm_bo_device_init failed.\n");
 
+	hmm_initialized = true;
+
 	/*
 	 * As hmm use NULL to indicate invalid ISP virtual address,
 	 * and ISP_VM_START is defined to 0 too, so we allocate
@@ -193,7 +196,7 @@ int hmm_init(void)
 	 * at the beginning, to avoid hmm_alloc return 0 in the
 	 * further allocation.
 	 */
-	dummy_ptr = hmm_alloc(1, HMM_BO_PRIVATE, 0, 0, HMM_UNCACHED);
+	dummy_ptr = hmm_alloc(1, HMM_BO_PRIVATE, 0, NULL, HMM_UNCACHED);
 
 	if (!ret) {
 		ret = sysfs_create_group(&atomisp_dev->kobj,
@@ -217,6 +220,7 @@ void hmm_cleanup(void)
 	dummy_ptr = 0;
 
 	hmm_bo_device_exit(&bo_device);
+	hmm_initialized = false;
 }
 
 ia_css_ptr hmm_alloc(size_t bytes, enum hmm_bo_type type,
@@ -229,7 +233,7 @@ ia_css_ptr hmm_alloc(size_t bytes, enum hmm_bo_type type,
 	/* Check if we are initialized. In the ideal world we wouldn't need
 	   this but we can tackle it once the driver is a lot cleaner */
 
-	if (!dummy_ptr)
+	if (!hmm_initialized)
 		hmm_init();
 	/*Get page number from size*/
 	pgnr = size_to_pgnr_ceil(bytes);
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/hrt/hive_isp_css_mm_hrt.c b/drivers/staging/media/atomisp/pci/atomisp2/hrt/hive_isp_css_mm_hrt.c
index 7dff22f59e29..2e78976bb2ac 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/hrt/hive_isp_css_mm_hrt.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/hrt/hive_isp_css_mm_hrt.c
@@ -55,7 +55,7 @@ static ia_css_ptr __hrt_isp_css_mm_alloc(size_t bytes, void *userptr,
 	if (type == HRT_USR_PTR) {
 		if (userptr == NULL)
 			return hmm_alloc(bytes, HMM_BO_PRIVATE, 0,
-						 0, cached);
+						 NULL, cached);
 		else {
 			if (num_pages < ((__page_align(bytes)) >> PAGE_SHIFT))
 				dev_err(atomisp_dev,
@@ -94,7 +94,7 @@ ia_css_ptr hrt_isp_css_mm_alloc_user_ptr(size_t bytes, void *userptr,
 ia_css_ptr hrt_isp_css_mm_alloc_cached(size_t bytes)
 {
 	if (my_userptr == NULL)
-		return hmm_alloc(bytes, HMM_BO_PRIVATE, 0, 0,
+		return hmm_alloc(bytes, HMM_BO_PRIVATE, 0, NULL,
 						HMM_CACHED);
 	else {
 		if (my_num_pages < ((__page_align(bytes)) >> PAGE_SHIFT))
diff --git a/drivers/staging/media/atomisp/platform/intel-mid/atomisp_gmin_platform.c b/drivers/staging/media/atomisp/platform/intel-mid/atomisp_gmin_platform.c
index 5b4506a71126..edaae93af8f9 100644
--- a/drivers/staging/media/atomisp/platform/intel-mid/atomisp_gmin_platform.c
+++ b/drivers/staging/media/atomisp/platform/intel-mid/atomisp_gmin_platform.c
@@ -51,7 +51,7 @@ struct gmin_subdev {
 
 static struct gmin_subdev gmin_subdevs[MAX_SUBDEVS];
 
-static enum { PMIC_UNSET = 0, PMIC_REGULATOR, PMIC_AXP, PMIC_TI ,
+static enum { PMIC_UNSET = 0, PMIC_REGULATOR, PMIC_AXP, PMIC_TI,
 	PMIC_CRYSTALCOVE } pmic_id;
 
 /* The atomisp uses type==0 for the end-of-list marker, so leave space. */
@@ -119,7 +119,7 @@ static int af_power_ctrl(struct v4l2_subdev *subdev, int flag)
 	/*
 	 * The power here is used for dw9817,
 	 * regulator is from rear sensor
-	*/
+	 */
 	if (gs->v2p8_vcm_reg) {
 		if (flag)
 			return regulator_enable(gs->v2p8_vcm_reg);
@@ -152,13 +152,13 @@ const struct camera_af_platform_data *camera_get_af_platform_data(void)
 EXPORT_SYMBOL_GPL(camera_get_af_platform_data);
 
 int atomisp_register_i2c_module(struct v4l2_subdev *subdev,
-                                struct camera_sensor_platform_data *plat_data,
-                                enum intel_v4l2_subdev_type type)
+				struct camera_sensor_platform_data *plat_data,
+				enum intel_v4l2_subdev_type type)
 {
 	int i;
 	struct i2c_board_info *bi;
 	struct gmin_subdev *gs;
-        struct i2c_client *client = v4l2_get_subdevdata(subdev);
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
 	struct acpi_device *adev;
 
 	dev_info(&client->dev, "register atomisp i2c module type %d\n", type);
@@ -167,12 +167,13 @@ int atomisp_register_i2c_module(struct v4l2_subdev *subdev,
 	 * uses ACPI runtime power management for camera devices, but
 	 * we don't.  Disable it, or else the rails will be needlessly
 	 * tickled during suspend/resume.  This has caused power and
-	 * performance issues on multiple devices. */
+	 * performance issues on multiple devices.
+	 */
 	adev = ACPI_COMPANION(&client->dev);
 	if (adev)
 		adev->power.flags.power_resources = 0;
 
-	for (i=0; i < MAX_SUBDEVS; i++)
+	for (i = 0; i < MAX_SUBDEVS; i++)
 		if (!pdata.subdevs[i].type)
 			break;
 
@@ -182,7 +183,8 @@ int atomisp_register_i2c_module(struct v4l2_subdev *subdev,
 	/* Note subtlety of initialization order: at the point where
 	 * this registration API gets called, the platform data
 	 * callbacks have probably already been invoked, so the
-	 * gmin_subdev struct is already initialized for us. */
+	 * gmin_subdev struct is already initialized for us.
+	 */
 	gs = find_gmin_subdev(subdev);
 
 	pdata.subdevs[i].type = type;
@@ -206,8 +208,10 @@ struct v4l2_subdev *atomisp_gmin_find_subdev(struct i2c_adapter *adapter,
 					     struct i2c_board_info *board_info)
 {
 	int i;
-	for (i=0; i < MAX_SUBDEVS && pdata.subdevs[i].type; i++) {
+
+	for (i = 0; i < MAX_SUBDEVS && pdata.subdevs[i].type; i++) {
 		struct intel_v4l2_subdev_table *sd = &pdata.subdevs[i];
+
 		if (sd->v4l2_subdev.i2c_adapter_id == adapter->nr &&
 		    sd->v4l2_subdev.board_info.addr == board_info->addr)
 			return sd->subdev;
@@ -261,7 +265,8 @@ static const struct gmin_cfg_var ffrd8_vars[] = {
 };
 
 /* Cribbed from MCG defaults in the mt9m114 driver, not actually verified
- * vs. T100 hardware */
+ * vs. T100 hardware
+ */
 static const struct gmin_cfg_var t100_vars[] = {
 	{ "INT33F0:00_CsiPort",  "0" },
 	{ "INT33F0:00_CsiLanes", "1" },
@@ -270,45 +275,45 @@ static const struct gmin_cfg_var t100_vars[] = {
 };
 
 static const struct gmin_cfg_var mrd7_vars[] = {
-        {"INT33F8:00_CamType", "1"},
-        {"INT33F8:00_CsiPort", "1"},
-        {"INT33F8:00_CsiLanes","2"},
-        {"INT33F8:00_CsiFmt","13"},
-        {"INT33F8:00_CsiBayer", "0"},
-        {"INT33F8:00_CamClk", "0"},
-        {"INT33F9:00_CamType", "1"},
-        {"INT33F9:00_CsiPort", "0"},
-        {"INT33F9:00_CsiLanes","1"},
-        {"INT33F9:00_CsiFmt","13"},
-        {"INT33F9:00_CsiBayer", "0"},
-        {"INT33F9:00_CamClk", "1"},
-        {},
+	{"INT33F8:00_CamType", "1"},
+	{"INT33F8:00_CsiPort", "1"},
+	{"INT33F8:00_CsiLanes", "2"},
+	{"INT33F8:00_CsiFmt", "13"},
+	{"INT33F8:00_CsiBayer", "0"},
+	{"INT33F8:00_CamClk", "0"},
+	{"INT33F9:00_CamType", "1"},
+	{"INT33F9:00_CsiPort", "0"},
+	{"INT33F9:00_CsiLanes", "1"},
+	{"INT33F9:00_CsiFmt", "13"},
+	{"INT33F9:00_CsiBayer", "0"},
+	{"INT33F9:00_CamClk", "1"},
+	{},
 };
 
 static const struct gmin_cfg_var ecs7_vars[] = {
-        {"INT33BE:00_CsiPort", "1"},
-        {"INT33BE:00_CsiLanes","2"},
-        {"INT33BE:00_CsiFmt","13"},
-        {"INT33BE:00_CsiBayer", "2"},
-        {"INT33BE:00_CamClk", "0"},
-        {"INT33F0:00_CsiPort", "0"},
-        {"INT33F0:00_CsiLanes","1"},
-        {"INT33F0:00_CsiFmt","13"},
-        {"INT33F0:00_CsiBayer", "0"},
-        {"INT33F0:00_CamClk", "1"},
-        {"gmin_V2P8GPIO","402"},
-        {},
+	{"INT33BE:00_CsiPort", "1"},
+	{"INT33BE:00_CsiLanes", "2"},
+	{"INT33BE:00_CsiFmt", "13"},
+	{"INT33BE:00_CsiBayer", "2"},
+	{"INT33BE:00_CamClk", "0"},
+	{"INT33F0:00_CsiPort", "0"},
+	{"INT33F0:00_CsiLanes", "1"},
+	{"INT33F0:00_CsiFmt", "13"},
+	{"INT33F0:00_CsiBayer", "0"},
+	{"INT33F0:00_CamClk", "1"},
+	{"gmin_V2P8GPIO", "402"},
+	{},
 };
 
 
 static const struct gmin_cfg_var i8880_vars[] = {
-        {"XXOV2680:00_CsiPort", "1"},
-        {"XXOV2680:00_CsiLanes","1"},
-        {"XXOV2680:00_CamClk","0"},
-        {"XXGC0310:00_CsiPort", "0"},
-        {"XXGC0310:00_CsiLanes", "1"},
-        {"XXGC0310:00_CamClk", "1"},
-        {},
+	{"XXOV2680:00_CsiPort", "1"},
+	{"XXOV2680:00_CsiLanes", "1"},
+	{"XXOV2680:00_CamClk", "0"},
+	{"XXGC0310:00_CsiPort", "0"},
+	{"XXGC0310:00_CsiLanes", "1"},
+	{"XXGC0310:00_CamClk", "1"},
+	{},
 };
 
 static const struct {
@@ -317,9 +322,9 @@ static const struct {
 } hard_vars[] = {
 	{ "BYT-T FFD8", ffrd8_vars },
 	{ "T100TA", t100_vars },
-        { "MRD7", mrd7_vars },
-        { "ST70408", ecs7_vars },
-        { "VTA0803", i8880_vars },
+	{ "MRD7", mrd7_vars },
+	{ "ST70408", ecs7_vars },
+	{ "VTA0803", i8880_vars },
 };
 
 
@@ -343,19 +348,17 @@ static struct gmin_subdev *gmin_subdev_add(struct v4l2_subdev *subdev)
 {
 	int i, ret;
 	struct device *dev;
-        struct i2c_client *client = v4l2_get_subdevdata(subdev);
-
-	if (!pmic_id) {
+	struct i2c_client *client = v4l2_get_subdevdata(subdev);
 
-			pmic_id = PMIC_REGULATOR;
-	}
+	if (!pmic_id)
+		pmic_id = PMIC_REGULATOR;
 
 	if (!client)
 		return NULL;
 
 	dev = &client->dev;
 
-	for (i=0; i < MAX_SUBDEVS && gmin_subdevs[i].subdev; i++)
+	for (i = 0; i < MAX_SUBDEVS && gmin_subdevs[i].subdev; i++)
 		;
 	if (i >= MAX_SUBDEVS)
 		return NULL;
@@ -401,7 +404,8 @@ static struct gmin_subdev *gmin_subdev_add(struct v4l2_subdev *subdev)
 		 * API is broken with the current drivers, returning
 		 * "1" for a regulator that will then emit a
 		 * "unbalanced disable" WARNing if we try to disable
-		 * it. */
+		 * it.
+		 */
 	}
 
 	return &gmin_subdevs[i];
@@ -410,7 +414,8 @@ static struct gmin_subdev *gmin_subdev_add(struct v4l2_subdev *subdev)
 static struct gmin_subdev *find_gmin_subdev(struct v4l2_subdev *subdev)
 {
 	int i;
-	for (i=0; i < MAX_SUBDEVS; i++)
+
+	for (i = 0; i < MAX_SUBDEVS; i++)
 		if (gmin_subdevs[i].subdev == subdev)
 			return &gmin_subdevs[i];
 	return gmin_subdev_add(subdev);
@@ -419,6 +424,7 @@ static struct gmin_subdev *find_gmin_subdev(struct v4l2_subdev *subdev)
 static int gmin_gpio0_ctrl(struct v4l2_subdev *subdev, int on)
 {
 	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+
 	if (gs && gs->gpio0) {
 		gpiod_set_value(gs->gpio0, on);
 		return 0;
@@ -429,6 +435,7 @@ static int gmin_gpio0_ctrl(struct v4l2_subdev *subdev, int on)
 static int gmin_gpio1_ctrl(struct v4l2_subdev *subdev, int on)
 {
 	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+
 	if (gs && gs->gpio1) {
 		gpiod_set_value(gs->gpio1, on);
 		return 0;
@@ -436,7 +443,7 @@ static int gmin_gpio1_ctrl(struct v4l2_subdev *subdev, int on)
 	return -EINVAL;
 }
 
-int gmin_v1p2_ctrl(struct v4l2_subdev *subdev, int on)
+static int gmin_v1p2_ctrl(struct v4l2_subdev *subdev, int on)
 {
 	struct gmin_subdev *gs = find_gmin_subdev(subdev);
 
@@ -455,7 +462,8 @@ int gmin_v1p2_ctrl(struct v4l2_subdev *subdev, int on)
 
 	return -EINVAL;
 }
-int gmin_v1p8_ctrl(struct v4l2_subdev *subdev, int on)
+
+static int gmin_v1p8_ctrl(struct v4l2_subdev *subdev, int on)
 {
 	struct gmin_subdev *gs = find_gmin_subdev(subdev);
 	int ret;
@@ -481,7 +489,7 @@ int gmin_v1p8_ctrl(struct v4l2_subdev *subdev, int on)
 		gpio_set_value(v1p8_gpio, on);
 
 	if (gs->v1p8_reg) {
-           regulator_set_voltage(gs->v1p8_reg, 1800000, 1800000);
+		regulator_set_voltage(gs->v1p8_reg, 1800000, 1800000);
 		if (on)
 			return regulator_enable(gs->v1p8_reg);
 		else
@@ -491,7 +499,7 @@ int gmin_v1p8_ctrl(struct v4l2_subdev *subdev, int on)
 	return -EINVAL;
 }
 
-int gmin_v2p8_ctrl(struct v4l2_subdev *subdev, int on)
+static int gmin_v2p8_ctrl(struct v4l2_subdev *subdev, int on)
 {
 	struct gmin_subdev *gs = find_gmin_subdev(subdev);
 	int ret;
@@ -517,7 +525,7 @@ int gmin_v2p8_ctrl(struct v4l2_subdev *subdev, int on)
 		gpio_set_value(v2p8_gpio, on);
 
 	if (gs->v2p8_reg) {
-           regulator_set_voltage(gs->v2p8_reg, 2900000, 2900000);
+		regulator_set_voltage(gs->v2p8_reg, 2900000, 2900000);
 		if (on)
 			return regulator_enable(gs->v2p8_reg);
 		else
@@ -527,10 +535,11 @@ int gmin_v2p8_ctrl(struct v4l2_subdev *subdev, int on)
 	return -EINVAL;
 }
 
-int gmin_flisclk_ctrl(struct v4l2_subdev *subdev, int on)
+static int gmin_flisclk_ctrl(struct v4l2_subdev *subdev, int on)
 {
 	int ret = 0;
 	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+
 	if (on)
 		ret = vlv2_plat_set_clock_freq(gs->clock_num, gs->clock_src);
 	if (ret)
@@ -595,6 +604,7 @@ struct camera_sensor_platform_data *gmin_camera_platform_data(
 		enum atomisp_bayer_order csi_bayer)
 {
 	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+
 	gs->csi_fmt = csi_format;
 	gs->csi_bayer = csi_bayer;
 
@@ -617,30 +627,31 @@ EXPORT_SYMBOL_GPL(atomisp_gmin_register_vcm_control);
 
 /* Retrieves a device-specific configuration variable.  The dev
  * argument should be a device with an ACPI companion, as all
- * configuration is based on firmware ID. */
-int gmin_get_config_var(struct device *dev, const char *var, char *out, size_t *out_len)
+ * configuration is based on firmware ID.
+ */
+int gmin_get_config_var(struct device *dev, const char *var, char *out,
+			size_t *out_len)
 {
 	char var8[CFG_VAR_NAME_MAX];
 	efi_char16_t var16[CFG_VAR_NAME_MAX];
 	struct efivar_entry *ev;
-	u32 efiattr_dummy;
 	int i, j, ret;
-	unsigned long efilen;
 
-        if (dev && ACPI_COMPANION(dev))
-                dev = &ACPI_COMPANION(dev)->dev;
+	if (dev && ACPI_COMPANION(dev))
+		dev = &ACPI_COMPANION(dev)->dev;
 
-        if (dev)
-                ret = snprintf(var8, sizeof(var8), "%s_%s", dev_name(dev), var);
-        else
-                ret = snprintf(var8, sizeof(var8), "gmin_%s", var);
+	if (dev)
+		ret = snprintf(var8, sizeof(var8), "%s_%s", dev_name(dev), var);
+	else
+		ret = snprintf(var8, sizeof(var8), "gmin_%s", var);
 
 	if (ret < 0 || ret >= sizeof(var8) - 1)
 		return -EINVAL;
 
 	/* First check a hard-coded list of board-specific variables.
 	 * Some device firmwares lack the ability to set EFI variables at
-	 * runtime. */
+	 * runtime.
+	 */
 	for (i = 0; i < ARRAY_SIZE(hard_vars); i++) {
 		if (dmi_match(DMI_BOARD_NAME, hard_vars[i].dmi_board_name)) {
 			for (j = 0; hard_vars[i].vars[j].name; j++) {
@@ -665,7 +676,8 @@ int gmin_get_config_var(struct device *dev, const char *var, char *out, size_t *
 	}
 
 	/* Our variable names are ASCII by construction, but EFI names
-	 * are wide chars.  Convert and zero-pad. */
+	 * are wide chars.  Convert and zero-pad.
+	 */
 	memset(var16, 0, sizeof(var16));
 	for (i = 0; i < sizeof(var8) && var8[i]; i++)
 		var16[i] = var8[i];
@@ -678,21 +690,25 @@ int gmin_get_config_var(struct device *dev, const char *var, char *out, size_t *
 	 * implementation simply uses VariableName and VendorGuid from
 	 * the struct and ignores the rest, but it seems like there
 	 * ought to be an "official" efivar_entry registered
-	 * somewhere? */
+	 * somewhere?
+	 */
 	ev = kzalloc(sizeof(*ev), GFP_KERNEL);
 	if (!ev)
 		return -ENOMEM;
 	memcpy(&ev->var.VariableName, var16, sizeof(var16));
 	ev->var.VendorGuid = GMIN_CFG_VAR_EFI_GUID;
-
-	efilen = *out_len;
-	ret = efivar_entry_get(ev, &efiattr_dummy, &efilen, out);
+	ev->var.DataSize = *out_len;
+
+	ret = efivar_entry_get(ev, &ev->var.Attributes,
+			       &ev->var.DataSize, ev->var.Data);
+	if (ret == 0) {
+		memcpy(out, ev->var.Data, ev->var.DataSize);
+		*out_len = ev->var.DataSize;
+	} else if (dev) {
+		dev_warn(dev, "Failed to find gmin variable %s\n", var8);
+	}
 
 	kfree(ev);
-	*out_len = efilen;
-
-	if (ret)
- 		dev_warn(dev, "Failed to find gmin variable %s\n", var8);
 
 	return ret;
 }
@@ -718,38 +734,39 @@ EXPORT_SYMBOL_GPL(gmin_get_var_int);
 int camera_sensor_csi(struct v4l2_subdev *sd, u32 port,
 		      u32 lanes, u32 format, u32 bayer_order, int flag)
 {
-        struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct camera_mipi_info *csi = NULL;
-
-        if (flag) {
-                csi = kzalloc(sizeof(*csi), GFP_KERNEL);
-                if (!csi) {
-                        dev_err(&client->dev, "out of memory\n");
-                        return -ENOMEM;
-                }
-                csi->port = port;
-                csi->num_lanes = lanes;
-                csi->input_format = format;
-                csi->raw_bayer_order = bayer_order;
-                v4l2_set_subdev_hostdata(sd, (void *)csi);
-                csi->metadata_format = ATOMISP_INPUT_FORMAT_EMBEDDED;
-                csi->metadata_effective_width = NULL;
-                dev_info(&client->dev,
-                         "camera pdata: port: %d lanes: %d order: %8.8x\n",
-                         port, lanes, bayer_order);
-        } else {
-                csi = v4l2_get_subdev_hostdata(sd);
-                kfree(csi);
-        }
-
-        return 0;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct camera_mipi_info *csi = NULL;
+
+	if (flag) {
+		csi = kzalloc(sizeof(*csi), GFP_KERNEL);
+		if (!csi) {
+			dev_err(&client->dev, "out of memory\n");
+			return -ENOMEM;
+		}
+		csi->port = port;
+		csi->num_lanes = lanes;
+		csi->input_format = format;
+		csi->raw_bayer_order = bayer_order;
+		v4l2_set_subdev_hostdata(sd, (void *)csi);
+		csi->metadata_format = ATOMISP_INPUT_FORMAT_EMBEDDED;
+		csi->metadata_effective_width = NULL;
+		dev_info(&client->dev,
+			 "camera pdata: port: %d lanes: %d order: %8.8x\n",
+			 port, lanes, bayer_order);
+	} else {
+		csi = v4l2_get_subdev_hostdata(sd);
+		kfree(csi);
+	}
+
+	return 0;
 }
 EXPORT_SYMBOL_GPL(camera_sensor_csi);
 
 /* PCI quirk: The BYT ISP advertises PCI runtime PM but it doesn't
  * work.  Disable so the kernel framework doesn't hang the device
  * trying.  The driver itself does direct calls to the PUNIT to manage
- * ISP power. */
+ * ISP power.
+ */
 static void isp_pm_cap_fixup(struct pci_dev *dev)
 {
 	dev_info(&dev->dev, "Disabling PCI power management on camera ISP\n");
diff --git a/drivers/staging/media/atomisp/platform/intel-mid/intel_mid_pcihelpers.c b/drivers/staging/media/atomisp/platform/intel-mid/intel_mid_pcihelpers.c
index a6c0f5f8c3f8..cd452cc20fea 100644
--- a/drivers/staging/media/atomisp/platform/intel-mid/intel_mid_pcihelpers.c
+++ b/drivers/staging/media/atomisp/platform/intel-mid/intel_mid_pcihelpers.c
@@ -5,7 +5,8 @@
 
 /* G-Min addition: "platform_is()" lives in intel_mid_pm.h in the MCG
  * tree, but it's just platform ID info and we don't want to pull in
- * the whole SFI-based PM architecture. */
+ * the whole SFI-based PM architecture.
+ */
 #define INTEL_ATOM_MRST 0x26
 #define INTEL_ATOM_MFLD 0x27
 #define INTEL_ATOM_CLV 0x35
@@ -22,7 +23,7 @@
 #endif
 static inline int platform_is(u8 model)
 {
-        return (boot_cpu_data.x86_model == model);
+	return (boot_cpu_data.x86_model == model);
 }
 
 #include "../../include/asm/intel_mid_pcihelpers.h"
@@ -32,7 +33,6 @@ static DEFINE_SPINLOCK(msgbus_lock);
 
 static struct pci_dev *pci_root;
 static struct pm_qos_request pm_qos;
-int qos;
 
 #define DW_I2C_NEED_QOS	(platform_is(INTEL_ATOM_BYT))
 
@@ -136,8 +136,8 @@ u32 intel_mid_msgbus_read32(u8 port, u32 addr)
 
 	return data;
 }
-
 EXPORT_SYMBOL(intel_mid_msgbus_read32);
+
 void intel_mid_msgbus_write32(u8 port, u32 addr, u32 data)
 {
 	unsigned long irq_flags;
@@ -171,8 +171,8 @@ EXPORT_SYMBOL(intel_mid_soc_stepping);
 
 static bool is_south_complex_device(struct pci_dev *dev)
 {
-	unsigned base_class = dev->class >> 16;
-	unsigned sub_class  = (dev->class & SUB_CLASS_MASK) >> 8;
+	unsigned int base_class = dev->class >> 16;
+	unsigned int sub_class  = (dev->class & SUB_CLASS_MASK) >> 8;
 
 	/* other than camera, pci bridges and display,
 	 * everything else are south complex devices.
diff --git a/drivers/staging/media/cxd2099/cxd2099.c b/drivers/staging/media/cxd2099/cxd2099.c
index 18186d0fa1a6..370ecb959543 100644
--- a/drivers/staging/media/cxd2099/cxd2099.c
+++ b/drivers/staging/media/cxd2099/cxd2099.c
@@ -473,7 +473,7 @@ static int slot_shutdown(struct dvb_ca_en50221 *ca, int slot)
 {
 	struct cxd *ci = ca->data;
 
-	dev_info(&ci->i2c->dev, "slot_shutdown\n");
+	dev_info(&ci->i2c->dev, "%s\n", __func__);
 	mutex_lock(&ci->lock);
 	write_regm(ci, 0x09, 0x08, 0x08);
 	write_regm(ci, 0x20, 0x80, 0x80); /* Reset CAM Mode */
@@ -564,7 +564,7 @@ static int read_data(struct dvb_ca_en50221 *ca, int slot, u8 *ebuf, int ecount)
 	campoll(ci);
 	mutex_unlock(&ci->lock);
 
-	dev_info(&ci->i2c->dev, "read_data\n");
+	dev_info(&ci->i2c->dev, "%s\n", __func__);
 	if (!ci->dr)
 		return 0;
 
@@ -584,7 +584,7 @@ static int write_data(struct dvb_ca_en50221 *ca, int slot, u8 *ebuf, int ecount)
 	struct cxd *ci = ca->data;
 
 	mutex_lock(&ci->lock);
-	dev_info(&ci->i2c->dev, "write_data %d\n", ecount);
+	dev_info(&ci->i2c->dev, "%s %d\n", __func__, ecount);
 	write_reg(ci, 0x0d, ecount >> 8);
 	write_reg(ci, 0x0e, ecount & 0xff);
 	write_block(ci, 0x11, ebuf, ecount);
diff --git a/drivers/staging/media/imx/Kconfig b/drivers/staging/media/imx/Kconfig
new file mode 100644
index 000000000000..7eff50bcea39
--- /dev/null
+++ b/drivers/staging/media/imx/Kconfig
@@ -0,0 +1,21 @@
+config VIDEO_IMX_MEDIA
+	tristate "i.MX5/6 V4L2 media core driver"
+	depends on MEDIA_CONTROLLER && VIDEO_V4L2 && ARCH_MXC && IMX_IPUV3_CORE
+	select V4L2_FWNODE
+	---help---
+	  Say yes here to enable support for video4linux media controller
+	  driver for the i.MX5/6 SOC.
+
+if VIDEO_IMX_MEDIA
+menu "i.MX5/6 Media Sub devices"
+
+config VIDEO_IMX_CSI
+	tristate "i.MX5/6 Camera Sensor Interface driver"
+	depends on VIDEO_IMX_MEDIA && VIDEO_DEV && I2C
+	select VIDEOBUF2_DMA_CONTIG
+	default y
+	---help---
+	  A video4linux camera sensor interface driver for i.MX5/6.
+
+endmenu
+endif
diff --git a/drivers/staging/media/imx/Makefile b/drivers/staging/media/imx/Makefile
new file mode 100644
index 000000000000..3569625b6305
--- /dev/null
+++ b/drivers/staging/media/imx/Makefile
@@ -0,0 +1,12 @@
+imx-media-objs := imx-media-dev.o imx-media-internal-sd.o imx-media-of.o
+imx-media-common-objs := imx-media-utils.o imx-media-fim.o
+imx-media-ic-objs := imx-ic-common.o imx-ic-prp.o imx-ic-prpencvf.o
+
+obj-$(CONFIG_VIDEO_IMX_MEDIA) += imx-media.o
+obj-$(CONFIG_VIDEO_IMX_MEDIA) += imx-media-common.o
+obj-$(CONFIG_VIDEO_IMX_MEDIA) += imx-media-capture.o
+obj-$(CONFIG_VIDEO_IMX_MEDIA) += imx-media-vdic.o
+obj-$(CONFIG_VIDEO_IMX_MEDIA) += imx-media-ic.o
+
+obj-$(CONFIG_VIDEO_IMX_CSI) += imx-media-csi.o
+obj-$(CONFIG_VIDEO_IMX_CSI) += imx6-mipi-csi2.o
diff --git a/drivers/staging/media/imx/TODO b/drivers/staging/media/imx/TODO
new file mode 100644
index 000000000000..0bee3132b26f
--- /dev/null
+++ b/drivers/staging/media/imx/TODO
@@ -0,0 +1,23 @@
+
+- Clean up and move the ov5642 subdev driver to drivers/media/i2c, or
+  merge support for OV5642 into drivers/media/i2c/ov5640.c, and create
+  the binding docs for it.
+
+- The Frame Interval Monitor could be exported to v4l2-core for
+  general use.
+
+- At driver load time, the device-tree node that is the original source
+  (the "sensor"), is parsed to record its media bus configuration, and
+  this info is required in imx-media-csi.c to setup the CSI.
+  Laurent Pinchart argues that instead the CSI subdev should call its
+  neighbor's g_mbus_config op (which should be propagated if necessary)
+  to get this info. However Hans Verkuil is planning to remove the
+  g_mbus_config op. For now this driver uses the parsed DT mbus config
+  method until this issue is resolved.
+
+- This media driver supports inheriting V4L2 controls to the
+  video capture devices, from the subdevices in the capture device's
+  pipeline. The controls for each capture device are updated in the
+  link_notify callback when the pipeline is modified. It should be
+  decided whether this feature is useful enough to make it generally
+  available by exporting to v4l2-core.
diff --git a/drivers/staging/media/imx/imx-ic-common.c b/drivers/staging/media/imx/imx-ic-common.c
new file mode 100644
index 000000000000..cfdd4900a3be
--- /dev/null
+++ b/drivers/staging/media/imx/imx-ic-common.c
@@ -0,0 +1,113 @@
+/*
+ * V4L2 Image Converter Subdev for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2014-2016 Mentor Graphics Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include "imx-media.h"
+#include "imx-ic.h"
+
+#define IC_TASK_PRP IC_NUM_TASKS
+#define IC_NUM_OPS  (IC_NUM_TASKS + 1)
+
+static struct imx_ic_ops *ic_ops[IC_NUM_OPS] = {
+	[IC_TASK_PRP]            = &imx_ic_prp_ops,
+	[IC_TASK_ENCODER]        = &imx_ic_prpencvf_ops,
+	[IC_TASK_VIEWFINDER]     = &imx_ic_prpencvf_ops,
+};
+
+static int imx_ic_probe(struct platform_device *pdev)
+{
+	struct imx_media_internal_sd_platformdata *pdata;
+	struct imx_ic_priv *priv;
+	int ret;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, &priv->sd);
+	priv->dev = &pdev->dev;
+
+	/* get our ipu_id, grp_id and IC task id */
+	pdata = priv->dev->platform_data;
+	priv->ipu_id = pdata->ipu_id;
+	switch (pdata->grp_id) {
+	case IMX_MEDIA_GRP_ID_IC_PRP:
+		priv->task_id = IC_TASK_PRP;
+		break;
+	case IMX_MEDIA_GRP_ID_IC_PRPENC:
+		priv->task_id = IC_TASK_ENCODER;
+		break;
+	case IMX_MEDIA_GRP_ID_IC_PRPVF:
+		priv->task_id = IC_TASK_VIEWFINDER;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	v4l2_subdev_init(&priv->sd, ic_ops[priv->task_id]->subdev_ops);
+	v4l2_set_subdevdata(&priv->sd, priv);
+	priv->sd.internal_ops = ic_ops[priv->task_id]->internal_ops;
+	priv->sd.entity.ops = ic_ops[priv->task_id]->entity_ops;
+	priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
+	priv->sd.dev = &pdev->dev;
+	priv->sd.owner = THIS_MODULE;
+	priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	priv->sd.grp_id = pdata->grp_id;
+	strncpy(priv->sd.name, pdata->sd_name, sizeof(priv->sd.name));
+
+	ret = ic_ops[priv->task_id]->init(priv);
+	if (ret)
+		return ret;
+
+	ret = v4l2_async_register_subdev(&priv->sd);
+	if (ret)
+		ic_ops[priv->task_id]->remove(priv);
+
+	return ret;
+}
+
+static int imx_ic_remove(struct platform_device *pdev)
+{
+	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
+	struct imx_ic_priv *priv = container_of(sd, struct imx_ic_priv, sd);
+
+	v4l2_info(sd, "Removing\n");
+
+	ic_ops[priv->task_id]->remove(priv);
+
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+
+	return 0;
+}
+
+static const struct platform_device_id imx_ic_ids[] = {
+	{ .name = "imx-ipuv3-ic" },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, imx_ic_ids);
+
+static struct platform_driver imx_ic_driver = {
+	.probe = imx_ic_probe,
+	.remove = imx_ic_remove,
+	.id_table = imx_ic_ids,
+	.driver = {
+		.name = "imx-ipuv3-ic",
+	},
+};
+module_platform_driver(imx_ic_driver);
+
+MODULE_DESCRIPTION("i.MX IC subdev driver");
+MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:imx-ipuv3-ic");
diff --git a/drivers/staging/media/imx/imx-ic-prp.c b/drivers/staging/media/imx/imx-ic-prp.c
new file mode 100644
index 000000000000..c2bb5ef2acb4
--- /dev/null
+++ b/drivers/staging/media/imx/imx-ic-prp.c
@@ -0,0 +1,518 @@
+/*
+ * V4L2 Capture IC Preprocess Subdev for Freescale i.MX5/6 SOC
+ *
+ * This subdevice handles capture of video frames from the CSI or VDIC,
+ * which are routed directly to the Image Converter preprocess tasks,
+ * for resizing, colorspace conversion, and rotation.
+ *
+ * Copyright (c) 2012-2017 Mentor Graphics Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/timer.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-subdev.h>
+#include <media/imx.h>
+#include "imx-media.h"
+#include "imx-ic.h"
+
+/*
+ * Min/Max supported width and heights.
+ */
+#define MIN_W       176
+#define MIN_H       144
+#define MAX_W      4096
+#define MAX_H      4096
+#define W_ALIGN    4 /* multiple of 16 pixels */
+#define H_ALIGN    1 /* multiple of 2 lines */
+#define S_ALIGN    1 /* multiple of 2 */
+
+struct prp_priv {
+	struct imx_media_dev *md;
+	struct imx_ic_priv *ic_priv;
+	struct media_pad pad[PRP_NUM_PADS];
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	/* IPU units we require */
+	struct ipu_soc *ipu;
+
+	struct v4l2_subdev *src_sd;
+	struct v4l2_subdev *sink_sd_prpenc;
+	struct v4l2_subdev *sink_sd_prpvf;
+
+	/* the CSI id at link validate */
+	int csi_id;
+
+	struct v4l2_mbus_framefmt format_mbus;
+	struct v4l2_fract frame_interval;
+
+	int stream_count;
+};
+
+static inline struct prp_priv *sd_to_priv(struct v4l2_subdev *sd)
+{
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+
+	return ic_priv->prp_priv;
+}
+
+static int prp_start(struct prp_priv *priv)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	bool src_is_vdic;
+
+	priv->ipu = priv->md->ipu[ic_priv->ipu_id];
+
+	/* set IC to receive from CSI or VDI depending on source */
+	src_is_vdic = !!(priv->src_sd->grp_id & IMX_MEDIA_GRP_ID_VDIC);
+
+	ipu_set_ic_src_mux(priv->ipu, priv->csi_id, src_is_vdic);
+
+	return 0;
+}
+
+static void prp_stop(struct prp_priv *priv)
+{
+}
+
+static struct v4l2_mbus_framefmt *
+__prp_get_fmt(struct prp_priv *priv, struct v4l2_subdev_pad_config *cfg,
+	      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&ic_priv->sd, cfg, pad);
+	else
+		return &priv->format_mbus;
+}
+
+/*
+ * V4L2 subdev operations.
+ */
+
+static int prp_enum_mbus_code(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	struct v4l2_mbus_framefmt *infmt;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	switch (code->pad) {
+	case PRP_SINK_PAD:
+		ret = imx_media_enum_ipu_format(&code->code, code->index,
+						CS_SEL_ANY);
+		break;
+	case PRP_SRC_PAD_PRPENC:
+	case PRP_SRC_PAD_PRPVF:
+		if (code->index != 0) {
+			ret = -EINVAL;
+			goto out;
+		}
+		infmt = __prp_get_fmt(priv, cfg, PRP_SINK_PAD, code->which);
+		code->code = infmt->code;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_get_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= PRP_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	fmt = __prp_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
+	if (!fmt) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	sdformat->format = *fmt;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_set_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	struct v4l2_mbus_framefmt *fmt, *infmt;
+	const struct imx_media_pixfmt *cc;
+	int ret = 0;
+	u32 code;
+
+	if (sdformat->pad >= PRP_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->stream_count > 0) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	infmt = __prp_get_fmt(priv, cfg, PRP_SINK_PAD, sdformat->which);
+
+	switch (sdformat->pad) {
+	case PRP_SINK_PAD:
+		v4l_bound_align_image(&sdformat->format.width, MIN_W, MAX_W,
+				      W_ALIGN, &sdformat->format.height,
+				      MIN_H, MAX_H, H_ALIGN, S_ALIGN);
+
+		cc = imx_media_find_ipu_format(sdformat->format.code,
+					       CS_SEL_ANY);
+		if (!cc) {
+			imx_media_enum_ipu_format(&code, 0, CS_SEL_ANY);
+			cc = imx_media_find_ipu_format(code, CS_SEL_ANY);
+			sdformat->format.code = cc->codes[0];
+		}
+
+		imx_media_fill_default_mbus_fields(&sdformat->format, infmt,
+						   true);
+		break;
+	case PRP_SRC_PAD_PRPENC:
+	case PRP_SRC_PAD_PRPVF:
+		/* Output pads mirror input pad */
+		sdformat->format = *infmt;
+		break;
+	}
+
+	fmt = __prp_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
+	*fmt = sdformat->format;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_link_setup(struct media_entity *entity,
+			  const struct media_pad *local,
+			  const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+	struct prp_priv *priv = ic_priv->prp_priv;
+	struct v4l2_subdev *remote_sd;
+	int ret = 0;
+
+	dev_dbg(ic_priv->dev, "link setup %s -> %s", remote->entity->name,
+		local->entity->name);
+
+	remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+
+	mutex_lock(&priv->lock);
+
+	if (local->flags & MEDIA_PAD_FL_SINK) {
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (priv->src_sd) {
+				ret = -EBUSY;
+				goto out;
+			}
+			if (priv->sink_sd_prpenc && (remote_sd->grp_id &
+						     IMX_MEDIA_GRP_ID_VDIC)) {
+				ret = -EINVAL;
+				goto out;
+			}
+			priv->src_sd = remote_sd;
+		} else {
+			priv->src_sd = NULL;
+		}
+
+		goto out;
+	}
+
+	/* this is a source pad */
+	if (flags & MEDIA_LNK_FL_ENABLED) {
+		switch (local->index) {
+		case PRP_SRC_PAD_PRPENC:
+			if (priv->sink_sd_prpenc) {
+				ret = -EBUSY;
+				goto out;
+			}
+			if (priv->src_sd && (priv->src_sd->grp_id &
+					     IMX_MEDIA_GRP_ID_VDIC)) {
+				ret = -EINVAL;
+				goto out;
+			}
+			priv->sink_sd_prpenc = remote_sd;
+			break;
+		case PRP_SRC_PAD_PRPVF:
+			if (priv->sink_sd_prpvf) {
+				ret = -EBUSY;
+				goto out;
+			}
+			priv->sink_sd_prpvf = remote_sd;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+	} else {
+		switch (local->index) {
+		case PRP_SRC_PAD_PRPENC:
+			priv->sink_sd_prpenc = NULL;
+			break;
+		case PRP_SRC_PAD_PRPVF:
+			priv->sink_sd_prpvf = NULL;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+	}
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_link_validate(struct v4l2_subdev *sd,
+			     struct media_link *link,
+			     struct v4l2_subdev_format *source_fmt,
+			     struct v4l2_subdev_format *sink_fmt)
+{
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+	struct prp_priv *priv = ic_priv->prp_priv;
+	struct imx_media_subdev *csi;
+	int ret;
+
+	ret = v4l2_subdev_link_validate_default(sd, link,
+						source_fmt, sink_fmt);
+	if (ret)
+		return ret;
+
+	csi = imx_media_find_upstream_subdev(priv->md, &ic_priv->sd.entity,
+					     IMX_MEDIA_GRP_ID_CSI);
+	if (IS_ERR(csi))
+		csi = NULL;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->src_sd->grp_id & IMX_MEDIA_GRP_ID_VDIC) {
+		/*
+		 * the ->PRPENC link cannot be enabled if the source
+		 * is the VDIC
+		 */
+		if (priv->sink_sd_prpenc)
+			ret = -EINVAL;
+		goto out;
+	} else {
+		/* the source is a CSI */
+		if (!csi) {
+			ret = -EINVAL;
+			goto out;
+		}
+	}
+
+	if (csi) {
+		switch (csi->sd->grp_id) {
+		case IMX_MEDIA_GRP_ID_CSI0:
+			priv->csi_id = 0;
+			break;
+		case IMX_MEDIA_GRP_ID_CSI1:
+			priv->csi_id = 1;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+	} else {
+		priv->csi_id = 0;
+	}
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+	struct prp_priv *priv = ic_priv->prp_priv;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	if (!priv->src_sd || (!priv->sink_sd_prpenc && !priv->sink_sd_prpvf)) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	/*
+	 * enable/disable streaming only if stream_count is
+	 * going from 0 to 1 / 1 to 0.
+	 */
+	if (priv->stream_count != !enable)
+		goto update_count;
+
+	dev_dbg(ic_priv->dev, "stream %s\n", enable ? "ON" : "OFF");
+
+	if (enable)
+		ret = prp_start(priv);
+	else
+		prp_stop(priv);
+	if (ret)
+		goto out;
+
+	/* start/stop upstream */
+	ret = v4l2_subdev_call(priv->src_sd, video, s_stream, enable);
+	ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
+	if (ret) {
+		if (enable)
+			prp_stop(priv);
+		goto out;
+	}
+
+update_count:
+	priv->stream_count += enable ? 1 : -1;
+	if (priv->stream_count < 0)
+		priv->stream_count = 0;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_g_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+
+	if (fi->pad >= PRP_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+	fi->interval = priv->frame_interval;
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+static int prp_s_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+
+	if (fi->pad >= PRP_NUM_PADS)
+		return -EINVAL;
+
+	/* No limits on frame interval */
+	mutex_lock(&priv->lock);
+	priv->frame_interval = fi->interval;
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+/*
+ * retrieve our pads parsed from the OF graph by the media device
+ */
+static int prp_registered(struct v4l2_subdev *sd)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	int i, ret;
+	u32 code;
+
+	/* get media device */
+	priv->md = dev_get_drvdata(sd->v4l2_dev->dev);
+
+	for (i = 0; i < PRP_NUM_PADS; i++) {
+		priv->pad[i].flags = (i == PRP_SINK_PAD) ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+	}
+
+	/* init default frame interval */
+	priv->frame_interval.numerator = 1;
+	priv->frame_interval.denominator = 30;
+
+	/* set a default mbus format  */
+	imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
+	ret = imx_media_init_mbus_fmt(&priv->format_mbus, 640, 480, code,
+				      V4L2_FIELD_NONE, NULL);
+	if (ret)
+		return ret;
+
+	return media_entity_pads_init(&sd->entity, PRP_NUM_PADS, priv->pad);
+}
+
+static const struct v4l2_subdev_pad_ops prp_pad_ops = {
+	.enum_mbus_code = prp_enum_mbus_code,
+	.get_fmt = prp_get_fmt,
+	.set_fmt = prp_set_fmt,
+	.link_validate = prp_link_validate,
+};
+
+static const struct v4l2_subdev_video_ops prp_video_ops = {
+	.g_frame_interval = prp_g_frame_interval,
+	.s_frame_interval = prp_s_frame_interval,
+	.s_stream = prp_s_stream,
+};
+
+static const struct media_entity_operations prp_entity_ops = {
+	.link_setup = prp_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_ops prp_subdev_ops = {
+	.video = &prp_video_ops,
+	.pad = &prp_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops prp_internal_ops = {
+	.registered = prp_registered,
+};
+
+static int prp_init(struct imx_ic_priv *ic_priv)
+{
+	struct prp_priv *priv;
+
+	priv = devm_kzalloc(ic_priv->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	mutex_init(&priv->lock);
+	ic_priv->prp_priv = priv;
+	priv->ic_priv = ic_priv;
+
+	return 0;
+}
+
+static void prp_remove(struct imx_ic_priv *ic_priv)
+{
+	struct prp_priv *priv = ic_priv->prp_priv;
+
+	mutex_destroy(&priv->lock);
+}
+
+struct imx_ic_ops imx_ic_prp_ops = {
+	.subdev_ops = &prp_subdev_ops,
+	.internal_ops = &prp_internal_ops,
+	.entity_ops = &prp_entity_ops,
+	.init = prp_init,
+	.remove = prp_remove,
+};
diff --git a/drivers/staging/media/imx/imx-ic-prpencvf.c b/drivers/staging/media/imx/imx-ic-prpencvf.c
new file mode 100644
index 000000000000..ed363fe3b3d0
--- /dev/null
+++ b/drivers/staging/media/imx/imx-ic-prpencvf.c
@@ -0,0 +1,1309 @@
+/*
+ * V4L2 Capture IC Preprocess Subdev for Freescale i.MX5/6 SOC
+ *
+ * This subdevice handles capture of video frames from the CSI or VDIC,
+ * which are routed directly to the Image Converter preprocess tasks,
+ * for resizing, colorspace conversion, and rotation.
+ *
+ * Copyright (c) 2012-2017 Mentor Graphics Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/timer.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+#include <media/imx.h>
+#include "imx-media.h"
+#include "imx-ic.h"
+
+/*
+ * Min/Max supported width and heights.
+ *
+ * We allow planar output, so we have to align width at the source pad
+ * by 16 pixels to meet IDMAC alignment requirements for possible planar
+ * output.
+ *
+ * TODO: move this into pad format negotiation, if capture device
+ * has not requested a planar format, we should allow 8 pixel
+ * alignment at the source pad.
+ */
+#define MIN_W_SINK  176
+#define MIN_H_SINK  144
+#define MAX_W_SINK 4096
+#define MAX_H_SINK 4096
+#define W_ALIGN_SINK  3 /* multiple of 8 pixels */
+#define H_ALIGN_SINK  1 /* multiple of 2 lines */
+
+#define MAX_W_SRC  1024
+#define MAX_H_SRC  1024
+#define W_ALIGN_SRC   4 /* multiple of 16 pixels */
+#define H_ALIGN_SRC   1 /* multiple of 2 lines */
+
+#define S_ALIGN       1 /* multiple of 2 */
+
+struct prp_priv {
+	struct imx_media_dev *md;
+	struct imx_ic_priv *ic_priv;
+	struct media_pad pad[PRPENCVF_NUM_PADS];
+	/* the video device at output pad */
+	struct imx_media_video_dev *vdev;
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	/* IPU units we require */
+	struct ipu_soc *ipu;
+	struct ipu_ic *ic;
+	struct ipuv3_channel *out_ch;
+	struct ipuv3_channel *rot_in_ch;
+	struct ipuv3_channel *rot_out_ch;
+
+	/* active vb2 buffers to send to video dev sink */
+	struct imx_media_buffer *active_vb2_buf[2];
+	struct imx_media_dma_buf underrun_buf;
+
+	int ipu_buf_num;  /* ipu double buffer index: 0-1 */
+
+	/* the sink for the captured frames */
+	struct media_entity *sink;
+	/* the source subdev */
+	struct v4l2_subdev *src_sd;
+
+	struct v4l2_mbus_framefmt format_mbus[PRPENCVF_NUM_PADS];
+	const struct imx_media_pixfmt *cc[PRPENCVF_NUM_PADS];
+	struct v4l2_fract frame_interval;
+
+	struct imx_media_dma_buf rot_buf[2];
+
+	/* controls */
+	struct v4l2_ctrl_handler ctrl_hdlr;
+	int  rotation; /* degrees */
+	bool hflip;
+	bool vflip;
+
+	/* derived from rotation, hflip, vflip controls */
+	enum ipu_rotate_mode rot_mode;
+
+	spinlock_t irqlock; /* protect eof_irq handler */
+
+	struct timer_list eof_timeout_timer;
+	int eof_irq;
+	int nfb4eof_irq;
+
+	int stream_count;
+	bool last_eof;  /* waiting for last EOF at stream off */
+	bool nfb4eof;    /* NFB4EOF encountered during streaming */
+	struct completion last_eof_comp;
+};
+
+static const struct prp_channels {
+	u32 out_ch;
+	u32 rot_in_ch;
+	u32 rot_out_ch;
+} prp_channel[] = {
+	[IC_TASK_ENCODER] = {
+		.out_ch = IPUV3_CHANNEL_IC_PRP_ENC_MEM,
+		.rot_in_ch = IPUV3_CHANNEL_MEM_ROT_ENC,
+		.rot_out_ch = IPUV3_CHANNEL_ROT_ENC_MEM,
+	},
+	[IC_TASK_VIEWFINDER] = {
+		.out_ch = IPUV3_CHANNEL_IC_PRP_VF_MEM,
+		.rot_in_ch = IPUV3_CHANNEL_MEM_ROT_VF,
+		.rot_out_ch = IPUV3_CHANNEL_ROT_VF_MEM,
+	},
+};
+
+static inline struct prp_priv *sd_to_priv(struct v4l2_subdev *sd)
+{
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+
+	return ic_priv->task_priv;
+}
+
+static void prp_put_ipu_resources(struct prp_priv *priv)
+{
+	if (!IS_ERR_OR_NULL(priv->ic))
+		ipu_ic_put(priv->ic);
+	priv->ic = NULL;
+
+	if (!IS_ERR_OR_NULL(priv->out_ch))
+		ipu_idmac_put(priv->out_ch);
+	priv->out_ch = NULL;
+
+	if (!IS_ERR_OR_NULL(priv->rot_in_ch))
+		ipu_idmac_put(priv->rot_in_ch);
+	priv->rot_in_ch = NULL;
+
+	if (!IS_ERR_OR_NULL(priv->rot_out_ch))
+		ipu_idmac_put(priv->rot_out_ch);
+	priv->rot_out_ch = NULL;
+}
+
+static int prp_get_ipu_resources(struct prp_priv *priv)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	int ret, task = ic_priv->task_id;
+
+	priv->ipu = priv->md->ipu[ic_priv->ipu_id];
+
+	priv->ic = ipu_ic_get(priv->ipu, task);
+	if (IS_ERR(priv->ic)) {
+		v4l2_err(&ic_priv->sd, "failed to get IC\n");
+		ret = PTR_ERR(priv->ic);
+		goto out;
+	}
+
+	priv->out_ch = ipu_idmac_get(priv->ipu,
+				     prp_channel[task].out_ch);
+	if (IS_ERR(priv->out_ch)) {
+		v4l2_err(&ic_priv->sd, "could not get IDMAC channel %u\n",
+			 prp_channel[task].out_ch);
+		ret = PTR_ERR(priv->out_ch);
+		goto out;
+	}
+
+	priv->rot_in_ch = ipu_idmac_get(priv->ipu,
+					prp_channel[task].rot_in_ch);
+	if (IS_ERR(priv->rot_in_ch)) {
+		v4l2_err(&ic_priv->sd, "could not get IDMAC channel %u\n",
+			 prp_channel[task].rot_in_ch);
+		ret = PTR_ERR(priv->rot_in_ch);
+		goto out;
+	}
+
+	priv->rot_out_ch = ipu_idmac_get(priv->ipu,
+					 prp_channel[task].rot_out_ch);
+	if (IS_ERR(priv->rot_out_ch)) {
+		v4l2_err(&ic_priv->sd, "could not get IDMAC channel %u\n",
+			 prp_channel[task].rot_out_ch);
+		ret = PTR_ERR(priv->rot_out_ch);
+		goto out;
+	}
+
+	return 0;
+out:
+	prp_put_ipu_resources(priv);
+	return ret;
+}
+
+static void prp_vb2_buf_done(struct prp_priv *priv, struct ipuv3_channel *ch)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_media_buffer *done, *next;
+	struct vb2_buffer *vb;
+	dma_addr_t phys;
+
+	done = priv->active_vb2_buf[priv->ipu_buf_num];
+	if (done) {
+		vb = &done->vbuf.vb2_buf;
+		vb->timestamp = ktime_get_ns();
+		vb2_buffer_done(vb, priv->nfb4eof ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+	}
+
+	priv->nfb4eof = false;
+
+	/* get next queued buffer */
+	next = imx_media_capture_device_next_buf(vdev);
+	if (next) {
+		phys = vb2_dma_contig_plane_dma_addr(&next->vbuf.vb2_buf, 0);
+		priv->active_vb2_buf[priv->ipu_buf_num] = next;
+	} else {
+		phys = priv->underrun_buf.phys;
+		priv->active_vb2_buf[priv->ipu_buf_num] = NULL;
+	}
+
+	if (ipu_idmac_buffer_is_ready(ch, priv->ipu_buf_num))
+		ipu_idmac_clear_buffer(ch, priv->ipu_buf_num);
+
+	ipu_cpmem_set_buffer(ch, priv->ipu_buf_num, phys);
+}
+
+static irqreturn_t prp_eof_interrupt(int irq, void *dev_id)
+{
+	struct prp_priv *priv = dev_id;
+	struct ipuv3_channel *channel;
+
+	spin_lock(&priv->irqlock);
+
+	if (priv->last_eof) {
+		complete(&priv->last_eof_comp);
+		priv->last_eof = false;
+		goto unlock;
+	}
+
+	channel = (ipu_rot_mode_is_irt(priv->rot_mode)) ?
+		priv->rot_out_ch : priv->out_ch;
+
+	prp_vb2_buf_done(priv, channel);
+
+	/* select new IPU buf */
+	ipu_idmac_select_buffer(channel, priv->ipu_buf_num);
+	/* toggle IPU double-buffer index */
+	priv->ipu_buf_num ^= 1;
+
+	/* bump the EOF timeout timer */
+	mod_timer(&priv->eof_timeout_timer,
+		  jiffies + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+
+unlock:
+	spin_unlock(&priv->irqlock);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t prp_nfb4eof_interrupt(int irq, void *dev_id)
+{
+	struct prp_priv *priv = dev_id;
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+
+	spin_lock(&priv->irqlock);
+
+	/*
+	 * this is not an unrecoverable error, just mark
+	 * the next captured frame with vb2 error flag.
+	 */
+	priv->nfb4eof = true;
+
+	v4l2_err(&ic_priv->sd, "NFB4EOF\n");
+
+	spin_unlock(&priv->irqlock);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * EOF timeout timer function.
+ */
+/*
+ * EOF timeout timer function. This is an unrecoverable condition
+ * without a stream restart.
+ */
+static void prp_eof_timeout(unsigned long data)
+{
+	struct prp_priv *priv = (struct prp_priv *)data;
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+
+	v4l2_err(&ic_priv->sd, "EOF timeout\n");
+
+	/* signal a fatal error to capture device */
+	imx_media_capture_device_error(vdev);
+}
+
+static void prp_setup_vb2_buf(struct prp_priv *priv, dma_addr_t *phys)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_media_buffer *buf;
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		buf = imx_media_capture_device_next_buf(vdev);
+		if (buf) {
+			priv->active_vb2_buf[i] = buf;
+			phys[i] = vb2_dma_contig_plane_dma_addr(
+				&buf->vbuf.vb2_buf, 0);
+		} else {
+			priv->active_vb2_buf[i] = NULL;
+			phys[i] = priv->underrun_buf.phys;
+		}
+	}
+}
+
+static void prp_unsetup_vb2_buf(struct prp_priv *priv,
+				enum vb2_buffer_state return_status)
+{
+	struct imx_media_buffer *buf;
+	int i;
+
+	/* return any remaining active frames with return_status */
+	for (i = 0; i < 2; i++) {
+		buf = priv->active_vb2_buf[i];
+		if (buf) {
+			struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
+
+			vb->timestamp = ktime_get_ns();
+			vb2_buffer_done(vb, return_status);
+		}
+	}
+}
+
+static int prp_setup_channel(struct prp_priv *priv,
+			     struct ipuv3_channel *channel,
+			     enum ipu_rotate_mode rot_mode,
+			     dma_addr_t addr0, dma_addr_t addr1,
+			     bool rot_swap_width_height)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	const struct imx_media_pixfmt *outcc;
+	struct v4l2_mbus_framefmt *infmt;
+	unsigned int burst_size;
+	struct ipu_image image;
+	int ret;
+
+	infmt = &priv->format_mbus[PRPENCVF_SINK_PAD];
+	outcc = vdev->cc;
+
+	ipu_cpmem_zero(channel);
+
+	memset(&image, 0, sizeof(image));
+	image.pix = vdev->fmt.fmt.pix;
+	image.rect.width = image.pix.width;
+	image.rect.height = image.pix.height;
+
+	if (rot_swap_width_height) {
+		swap(image.pix.width, image.pix.height);
+		swap(image.rect.width, image.rect.height);
+		/* recalc stride using swapped width */
+		image.pix.bytesperline = outcc->planar ?
+			image.pix.width :
+			(image.pix.width * outcc->bpp) >> 3;
+	}
+
+	image.phys0 = addr0;
+	image.phys1 = addr1;
+
+	ret = ipu_cpmem_set_image(channel, &image);
+	if (ret)
+		return ret;
+
+	if (channel == priv->rot_in_ch ||
+	    channel == priv->rot_out_ch) {
+		burst_size = 8;
+		ipu_cpmem_set_block_mode(channel);
+	} else {
+		burst_size = (image.pix.width & 0xf) ? 8 : 16;
+	}
+
+	ipu_cpmem_set_burstsize(channel, burst_size);
+
+	if (rot_mode)
+		ipu_cpmem_set_rotation(channel, rot_mode);
+
+	if (image.pix.field == V4L2_FIELD_NONE &&
+	    V4L2_FIELD_HAS_BOTH(infmt->field) &&
+	    channel == priv->out_ch)
+		ipu_cpmem_interlaced_scan(channel, image.pix.bytesperline);
+
+	ret = ipu_ic_task_idma_init(priv->ic, channel,
+				    image.pix.width, image.pix.height,
+				    burst_size, rot_mode);
+	if (ret)
+		return ret;
+
+	ipu_cpmem_set_axi_id(channel, 1);
+
+	ipu_idmac_set_double_buffer(channel, true);
+
+	return 0;
+}
+
+static int prp_setup_rotation(struct prp_priv *priv)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	const struct imx_media_pixfmt *outcc, *incc;
+	struct v4l2_mbus_framefmt *infmt;
+	struct v4l2_pix_format *outfmt;
+	dma_addr_t phys[2];
+	int ret;
+
+	infmt = &priv->format_mbus[PRPENCVF_SINK_PAD];
+	outfmt = &vdev->fmt.fmt.pix;
+	incc = priv->cc[PRPENCVF_SINK_PAD];
+	outcc = vdev->cc;
+
+	ret = imx_media_alloc_dma_buf(priv->md, &priv->rot_buf[0],
+				      outfmt->sizeimage);
+	if (ret) {
+		v4l2_err(&ic_priv->sd, "failed to alloc rot_buf[0], %d\n", ret);
+		return ret;
+	}
+	ret = imx_media_alloc_dma_buf(priv->md, &priv->rot_buf[1],
+				      outfmt->sizeimage);
+	if (ret) {
+		v4l2_err(&ic_priv->sd, "failed to alloc rot_buf[1], %d\n", ret);
+		goto free_rot0;
+	}
+
+	ret = ipu_ic_task_init(priv->ic,
+			       infmt->width, infmt->height,
+			       outfmt->height, outfmt->width,
+			       incc->cs, outcc->cs);
+	if (ret) {
+		v4l2_err(&ic_priv->sd, "ipu_ic_task_init failed, %d\n", ret);
+		goto free_rot1;
+	}
+
+	/* init the IC-PRP-->MEM IDMAC channel */
+	ret = prp_setup_channel(priv, priv->out_ch, IPU_ROTATE_NONE,
+				priv->rot_buf[0].phys, priv->rot_buf[1].phys,
+				true);
+	if (ret) {
+		v4l2_err(&ic_priv->sd,
+			 "prp_setup_channel(out_ch) failed, %d\n", ret);
+		goto free_rot1;
+	}
+
+	/* init the MEM-->IC-PRP ROT IDMAC channel */
+	ret = prp_setup_channel(priv, priv->rot_in_ch, priv->rot_mode,
+				priv->rot_buf[0].phys, priv->rot_buf[1].phys,
+				true);
+	if (ret) {
+		v4l2_err(&ic_priv->sd,
+			 "prp_setup_channel(rot_in_ch) failed, %d\n", ret);
+		goto free_rot1;
+	}
+
+	prp_setup_vb2_buf(priv, phys);
+
+	/* init the destination IC-PRP ROT-->MEM IDMAC channel */
+	ret = prp_setup_channel(priv, priv->rot_out_ch, IPU_ROTATE_NONE,
+				phys[0], phys[1],
+				false);
+	if (ret) {
+		v4l2_err(&ic_priv->sd,
+			 "prp_setup_channel(rot_out_ch) failed, %d\n", ret);
+		goto unsetup_vb2;
+	}
+
+	/* now link IC-PRP-->MEM to MEM-->IC-PRP ROT */
+	ipu_idmac_link(priv->out_ch, priv->rot_in_ch);
+
+	/* enable the IC */
+	ipu_ic_enable(priv->ic);
+
+	/* set buffers ready */
+	ipu_idmac_select_buffer(priv->out_ch, 0);
+	ipu_idmac_select_buffer(priv->out_ch, 1);
+	ipu_idmac_select_buffer(priv->rot_out_ch, 0);
+	ipu_idmac_select_buffer(priv->rot_out_ch, 1);
+
+	/* enable the channels */
+	ipu_idmac_enable_channel(priv->out_ch);
+	ipu_idmac_enable_channel(priv->rot_in_ch);
+	ipu_idmac_enable_channel(priv->rot_out_ch);
+
+	/* and finally enable the IC PRP task */
+	ipu_ic_task_enable(priv->ic);
+
+	return 0;
+
+unsetup_vb2:
+	prp_unsetup_vb2_buf(priv, VB2_BUF_STATE_QUEUED);
+free_rot1:
+	imx_media_free_dma_buf(priv->md, &priv->rot_buf[1]);
+free_rot0:
+	imx_media_free_dma_buf(priv->md, &priv->rot_buf[0]);
+	return ret;
+}
+
+static void prp_unsetup_rotation(struct prp_priv *priv)
+{
+	ipu_ic_task_disable(priv->ic);
+
+	ipu_idmac_disable_channel(priv->out_ch);
+	ipu_idmac_disable_channel(priv->rot_in_ch);
+	ipu_idmac_disable_channel(priv->rot_out_ch);
+
+	ipu_idmac_unlink(priv->out_ch, priv->rot_in_ch);
+
+	ipu_ic_disable(priv->ic);
+
+	imx_media_free_dma_buf(priv->md, &priv->rot_buf[0]);
+	imx_media_free_dma_buf(priv->md, &priv->rot_buf[1]);
+}
+
+static int prp_setup_norotation(struct prp_priv *priv)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	const struct imx_media_pixfmt *outcc, *incc;
+	struct v4l2_mbus_framefmt *infmt;
+	struct v4l2_pix_format *outfmt;
+	dma_addr_t phys[2];
+	int ret;
+
+	infmt = &priv->format_mbus[PRPENCVF_SINK_PAD];
+	outfmt = &vdev->fmt.fmt.pix;
+	incc = priv->cc[PRPENCVF_SINK_PAD];
+	outcc = vdev->cc;
+
+	ret = ipu_ic_task_init(priv->ic,
+			       infmt->width, infmt->height,
+			       outfmt->width, outfmt->height,
+			       incc->cs, outcc->cs);
+	if (ret) {
+		v4l2_err(&ic_priv->sd, "ipu_ic_task_init failed, %d\n", ret);
+		return ret;
+	}
+
+	prp_setup_vb2_buf(priv, phys);
+
+	/* init the IC PRP-->MEM IDMAC channel */
+	ret = prp_setup_channel(priv, priv->out_ch, priv->rot_mode,
+				phys[0], phys[1], false);
+	if (ret) {
+		v4l2_err(&ic_priv->sd,
+			 "prp_setup_channel(out_ch) failed, %d\n", ret);
+		goto unsetup_vb2;
+	}
+
+	ipu_cpmem_dump(priv->out_ch);
+	ipu_ic_dump(priv->ic);
+	ipu_dump(priv->ipu);
+
+	ipu_ic_enable(priv->ic);
+
+	/* set buffers ready */
+	ipu_idmac_select_buffer(priv->out_ch, 0);
+	ipu_idmac_select_buffer(priv->out_ch, 1);
+
+	/* enable the channels */
+	ipu_idmac_enable_channel(priv->out_ch);
+
+	/* enable the IC task */
+	ipu_ic_task_enable(priv->ic);
+
+	return 0;
+
+unsetup_vb2:
+	prp_unsetup_vb2_buf(priv, VB2_BUF_STATE_QUEUED);
+	return ret;
+}
+
+static void prp_unsetup_norotation(struct prp_priv *priv)
+{
+	ipu_ic_task_disable(priv->ic);
+	ipu_idmac_disable_channel(priv->out_ch);
+	ipu_ic_disable(priv->ic);
+}
+
+static void prp_unsetup(struct prp_priv *priv,
+			enum vb2_buffer_state state)
+{
+	if (ipu_rot_mode_is_irt(priv->rot_mode))
+		prp_unsetup_rotation(priv);
+	else
+		prp_unsetup_norotation(priv);
+
+	prp_unsetup_vb2_buf(priv, state);
+}
+
+static int prp_start(struct prp_priv *priv)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct v4l2_pix_format *outfmt;
+	int ret;
+
+	ret = prp_get_ipu_resources(priv);
+	if (ret)
+		return ret;
+
+	outfmt = &vdev->fmt.fmt.pix;
+
+	ret = imx_media_alloc_dma_buf(priv->md, &priv->underrun_buf,
+				      outfmt->sizeimage);
+	if (ret)
+		goto out_put_ipu;
+
+	priv->ipu_buf_num = 0;
+
+	/* init EOF completion waitq */
+	init_completion(&priv->last_eof_comp);
+	priv->last_eof = false;
+	priv->nfb4eof = false;
+
+	if (ipu_rot_mode_is_irt(priv->rot_mode))
+		ret = prp_setup_rotation(priv);
+	else
+		ret = prp_setup_norotation(priv);
+	if (ret)
+		goto out_free_underrun;
+
+	priv->nfb4eof_irq = ipu_idmac_channel_irq(priv->ipu,
+						  priv->out_ch,
+						  IPU_IRQ_NFB4EOF);
+	ret = devm_request_irq(ic_priv->dev, priv->nfb4eof_irq,
+			       prp_nfb4eof_interrupt, 0,
+			       "imx-ic-prp-nfb4eof", priv);
+	if (ret) {
+		v4l2_err(&ic_priv->sd,
+			 "Error registering NFB4EOF irq: %d\n", ret);
+		goto out_unsetup;
+	}
+
+	if (ipu_rot_mode_is_irt(priv->rot_mode))
+		priv->eof_irq = ipu_idmac_channel_irq(
+			priv->ipu, priv->rot_out_ch, IPU_IRQ_EOF);
+	else
+		priv->eof_irq = ipu_idmac_channel_irq(
+			priv->ipu, priv->out_ch, IPU_IRQ_EOF);
+
+	ret = devm_request_irq(ic_priv->dev, priv->eof_irq,
+			       prp_eof_interrupt, 0,
+			       "imx-ic-prp-eof", priv);
+	if (ret) {
+		v4l2_err(&ic_priv->sd,
+			 "Error registering eof irq: %d\n", ret);
+		goto out_free_nfb4eof_irq;
+	}
+
+	/* start the EOF timeout timer */
+	mod_timer(&priv->eof_timeout_timer,
+		  jiffies + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+
+	return 0;
+
+out_free_nfb4eof_irq:
+	devm_free_irq(ic_priv->dev, priv->nfb4eof_irq, priv);
+out_unsetup:
+	prp_unsetup(priv, VB2_BUF_STATE_QUEUED);
+out_free_underrun:
+	imx_media_free_dma_buf(priv->md, &priv->underrun_buf);
+out_put_ipu:
+	prp_put_ipu_resources(priv);
+	return ret;
+}
+
+static void prp_stop(struct prp_priv *priv)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	unsigned long flags;
+	int ret;
+
+	/* mark next EOF interrupt as the last before stream off */
+	spin_lock_irqsave(&priv->irqlock, flags);
+	priv->last_eof = true;
+	spin_unlock_irqrestore(&priv->irqlock, flags);
+
+	/*
+	 * and then wait for interrupt handler to mark completion.
+	 */
+	ret = wait_for_completion_timeout(
+		&priv->last_eof_comp,
+		msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+	if (ret == 0)
+		v4l2_warn(&ic_priv->sd, "wait last EOF timeout\n");
+
+	devm_free_irq(ic_priv->dev, priv->eof_irq, priv);
+	devm_free_irq(ic_priv->dev, priv->nfb4eof_irq, priv);
+
+	prp_unsetup(priv, VB2_BUF_STATE_ERROR);
+
+	imx_media_free_dma_buf(priv->md, &priv->underrun_buf);
+
+	/* cancel the EOF timeout timer */
+	del_timer_sync(&priv->eof_timeout_timer);
+
+	prp_put_ipu_resources(priv);
+}
+
+static struct v4l2_mbus_framefmt *
+__prp_get_fmt(struct prp_priv *priv, struct v4l2_subdev_pad_config *cfg,
+	      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&ic_priv->sd, cfg, pad);
+	else
+		return &priv->format_mbus[pad];
+}
+
+/*
+ * Applies IC resizer and IDMAC alignment restrictions to output
+ * rectangle given the input rectangle, and depending on given
+ * rotation mode.
+ *
+ * The IC resizer cannot downsize more than 4:1. Note also that
+ * for 90 or 270 rotation, _both_ output width and height must
+ * be aligned by W_ALIGN_SRC, because the intermediate rotation
+ * buffer swaps output width/height, and the final output buffer
+ * does not.
+ *
+ * Returns true if the output rectangle was modified.
+ */
+static bool prp_bound_align_output(struct v4l2_mbus_framefmt *outfmt,
+				   struct v4l2_mbus_framefmt *infmt,
+				   enum ipu_rotate_mode rot_mode)
+{
+	u32 orig_width = outfmt->width;
+	u32 orig_height = outfmt->height;
+
+	if (ipu_rot_mode_is_irt(rot_mode))
+		v4l_bound_align_image(&outfmt->width,
+				      infmt->height / 4, MAX_H_SRC,
+				      W_ALIGN_SRC,
+				      &outfmt->height,
+				      infmt->width / 4, MAX_W_SRC,
+				      W_ALIGN_SRC, S_ALIGN);
+	else
+		v4l_bound_align_image(&outfmt->width,
+				      infmt->width / 4, MAX_W_SRC,
+				      W_ALIGN_SRC,
+				      &outfmt->height,
+				      infmt->height / 4, MAX_H_SRC,
+				      H_ALIGN_SRC, S_ALIGN);
+
+	return outfmt->width != orig_width || outfmt->height != orig_height;
+}
+
+/*
+ * V4L2 subdev operations.
+ */
+
+static int prp_enum_mbus_code(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad >= PRPENCVF_NUM_PADS)
+		return -EINVAL;
+
+	return imx_media_enum_ipu_format(&code->code, code->index, CS_SEL_ANY);
+}
+
+static int prp_get_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= PRPENCVF_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	fmt = __prp_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
+	if (!fmt) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	sdformat->format = *fmt;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static void prp_try_fmt(struct prp_priv *priv,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *sdformat,
+			const struct imx_media_pixfmt **cc)
+{
+	struct v4l2_mbus_framefmt *infmt;
+
+	*cc = imx_media_find_ipu_format(sdformat->format.code, CS_SEL_ANY);
+	if (!*cc) {
+		u32 code;
+
+		imx_media_enum_ipu_format(&code, 0, CS_SEL_ANY);
+		*cc = imx_media_find_ipu_format(code, CS_SEL_ANY);
+		sdformat->format.code = (*cc)->codes[0];
+	}
+
+	infmt = __prp_get_fmt(priv, cfg, PRPENCVF_SINK_PAD, sdformat->which);
+
+	if (sdformat->pad == PRPENCVF_SRC_PAD) {
+		if (sdformat->format.field != V4L2_FIELD_NONE)
+			sdformat->format.field = infmt->field;
+
+		prp_bound_align_output(&sdformat->format, infmt,
+				       priv->rot_mode);
+
+		/* propagate colorimetry from sink */
+		sdformat->format.colorspace = infmt->colorspace;
+		sdformat->format.xfer_func = infmt->xfer_func;
+		sdformat->format.quantization = infmt->quantization;
+		sdformat->format.ycbcr_enc = infmt->ycbcr_enc;
+	} else {
+		v4l_bound_align_image(&sdformat->format.width,
+				      MIN_W_SINK, MAX_W_SINK, W_ALIGN_SINK,
+				      &sdformat->format.height,
+				      MIN_H_SINK, MAX_H_SINK, H_ALIGN_SINK,
+				      S_ALIGN);
+
+		imx_media_fill_default_mbus_fields(&sdformat->format, infmt,
+						   true);
+	}
+}
+
+static int prp_set_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	struct imx_media_video_dev *vdev = priv->vdev;
+	const struct imx_media_pixfmt *cc;
+	struct v4l2_pix_format vdev_fmt;
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= PRPENCVF_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->stream_count > 0) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	prp_try_fmt(priv, cfg, sdformat, &cc);
+
+	fmt = __prp_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
+	*fmt = sdformat->format;
+
+	/* propagate a default format to source pad */
+	if (sdformat->pad == PRPENCVF_SINK_PAD) {
+		const struct imx_media_pixfmt *outcc;
+		struct v4l2_mbus_framefmt *outfmt;
+		struct v4l2_subdev_format format;
+
+		format.pad = PRPENCVF_SRC_PAD;
+		format.which = sdformat->which;
+		format.format = sdformat->format;
+		prp_try_fmt(priv, cfg, &format, &outcc);
+
+		outfmt = __prp_get_fmt(priv, cfg, PRPENCVF_SRC_PAD,
+				       sdformat->which);
+		*outfmt = format.format;
+		if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+			priv->cc[PRPENCVF_SRC_PAD] = outcc;
+	}
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY)
+		goto out;
+
+	priv->cc[sdformat->pad] = cc;
+
+	/* propagate output pad format to capture device */
+	imx_media_mbus_fmt_to_pix_fmt(&vdev_fmt,
+				      &priv->format_mbus[PRPENCVF_SRC_PAD],
+				      priv->cc[PRPENCVF_SRC_PAD]);
+	mutex_unlock(&priv->lock);
+	imx_media_capture_device_set_format(vdev, &vdev_fmt);
+
+	return 0;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_enum_frame_size(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	struct v4l2_subdev_format format = {0};
+	const struct imx_media_pixfmt *cc;
+	int ret = 0;
+
+	if (fse->pad >= PRPENCVF_NUM_PADS || fse->index != 0)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	format.pad = fse->pad;
+	format.which = fse->which;
+	format.format.code = fse->code;
+	format.format.width = 1;
+	format.format.height = 1;
+	prp_try_fmt(priv, cfg, &format, &cc);
+	fse->min_width = format.format.width;
+	fse->min_height = format.format.height;
+
+	if (format.format.code != fse->code) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	format.format.code = fse->code;
+	format.format.width = -1;
+	format.format.height = -1;
+	prp_try_fmt(priv, cfg, &format, &cc);
+	fse->max_width = format.format.width;
+	fse->max_height = format.format.height;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_link_setup(struct media_entity *entity,
+			  const struct media_pad *local,
+			  const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+	struct prp_priv *priv = ic_priv->task_priv;
+	struct v4l2_subdev *remote_sd;
+	int ret = 0;
+
+	dev_dbg(ic_priv->dev, "link setup %s -> %s", remote->entity->name,
+		local->entity->name);
+
+	mutex_lock(&priv->lock);
+
+	if (local->flags & MEDIA_PAD_FL_SINK) {
+		if (!is_media_entity_v4l2_subdev(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (priv->src_sd) {
+				ret = -EBUSY;
+				goto out;
+			}
+			priv->src_sd = remote_sd;
+		} else {
+			priv->src_sd = NULL;
+		}
+
+		goto out;
+	}
+
+	/* this is the source pad */
+
+	/* the remote must be the device node */
+	if (!is_media_entity_v4l2_video_device(remote->entity)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (flags & MEDIA_LNK_FL_ENABLED) {
+		if (priv->sink) {
+			ret = -EBUSY;
+			goto out;
+		}
+	} else {
+		priv->sink = NULL;
+		goto out;
+	}
+
+	priv->sink = remote->entity;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct prp_priv *priv = container_of(ctrl->handler,
+					       struct prp_priv, ctrl_hdlr);
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	enum ipu_rotate_mode rot_mode;
+	int rotation, ret = 0;
+	bool hflip, vflip;
+
+	mutex_lock(&priv->lock);
+
+	rotation = priv->rotation;
+	hflip = priv->hflip;
+	vflip = priv->vflip;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		hflip = (ctrl->val == 1);
+		break;
+	case V4L2_CID_VFLIP:
+		vflip = (ctrl->val == 1);
+		break;
+	case V4L2_CID_ROTATE:
+		rotation = ctrl->val;
+		break;
+	default:
+		v4l2_err(&ic_priv->sd, "Invalid control\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = ipu_degrees_to_rot_mode(&rot_mode, rotation, hflip, vflip);
+	if (ret)
+		goto out;
+
+	if (rot_mode != priv->rot_mode) {
+		struct v4l2_mbus_framefmt outfmt, infmt;
+
+		/* can't change rotation mid-streaming */
+		if (priv->stream_count > 0) {
+			ret = -EBUSY;
+			goto out;
+		}
+
+		outfmt = priv->format_mbus[PRPENCVF_SRC_PAD];
+		infmt = priv->format_mbus[PRPENCVF_SINK_PAD];
+
+		if (prp_bound_align_output(&outfmt, &infmt, rot_mode)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		priv->rot_mode = rot_mode;
+		priv->rotation = rotation;
+		priv->hflip = hflip;
+		priv->vflip = vflip;
+	}
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops prp_ctrl_ops = {
+	.s_ctrl = prp_s_ctrl,
+};
+
+static int prp_init_controls(struct prp_priv *priv)
+{
+	struct imx_ic_priv *ic_priv = priv->ic_priv;
+	struct v4l2_ctrl_handler *hdlr = &priv->ctrl_hdlr;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdlr, 3);
+
+	v4l2_ctrl_new_std(hdlr, &prp_ctrl_ops, V4L2_CID_HFLIP,
+			  0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdlr, &prp_ctrl_ops, V4L2_CID_VFLIP,
+			  0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdlr, &prp_ctrl_ops, V4L2_CID_ROTATE,
+			  0, 270, 90, 0);
+
+	ic_priv->sd.ctrl_handler = hdlr;
+
+	if (hdlr->error) {
+		ret = hdlr->error;
+		goto out_free;
+	}
+
+	v4l2_ctrl_handler_setup(hdlr);
+	return 0;
+
+out_free:
+	v4l2_ctrl_handler_free(hdlr);
+	return ret;
+}
+
+static int prp_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx_ic_priv *ic_priv = v4l2_get_subdevdata(sd);
+	struct prp_priv *priv = ic_priv->task_priv;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	if (!priv->src_sd || !priv->sink) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	/*
+	 * enable/disable streaming only if stream_count is
+	 * going from 0 to 1 / 1 to 0.
+	 */
+	if (priv->stream_count != !enable)
+		goto update_count;
+
+	dev_dbg(ic_priv->dev, "stream %s\n", enable ? "ON" : "OFF");
+
+	if (enable)
+		ret = prp_start(priv);
+	else
+		prp_stop(priv);
+	if (ret)
+		goto out;
+
+	/* start/stop upstream */
+	ret = v4l2_subdev_call(priv->src_sd, video, s_stream, enable);
+	ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
+	if (ret) {
+		if (enable)
+			prp_stop(priv);
+		goto out;
+	}
+
+update_count:
+	priv->stream_count += enable ? 1 : -1;
+	if (priv->stream_count < 0)
+		priv->stream_count = 0;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int prp_g_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+
+	if (fi->pad >= PRPENCVF_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+	fi->interval = priv->frame_interval;
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+static int prp_s_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+
+	if (fi->pad >= PRPENCVF_NUM_PADS)
+		return -EINVAL;
+
+	/* No limits on frame interval */
+	mutex_lock(&priv->lock);
+	priv->frame_interval = fi->interval;
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+/*
+ * retrieve our pads parsed from the OF graph by the media device
+ */
+static int prp_registered(struct v4l2_subdev *sd)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+	int i, ret;
+	u32 code;
+
+	/* get media device */
+	priv->md = dev_get_drvdata(sd->v4l2_dev->dev);
+
+	for (i = 0; i < PRPENCVF_NUM_PADS; i++) {
+		priv->pad[i].flags = (i == PRPENCVF_SINK_PAD) ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+
+		/* set a default mbus format  */
+		imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
+		ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
+					      640, 480, code, V4L2_FIELD_NONE,
+					      &priv->cc[i]);
+		if (ret)
+			return ret;
+	}
+
+	/* init default frame interval */
+	priv->frame_interval.numerator = 1;
+	priv->frame_interval.denominator = 30;
+
+	ret = media_entity_pads_init(&sd->entity, PRPENCVF_NUM_PADS,
+				     priv->pad);
+	if (ret)
+		return ret;
+
+	ret = imx_media_capture_device_register(priv->vdev);
+	if (ret)
+		return ret;
+
+	ret = imx_media_add_video_device(priv->md, priv->vdev);
+	if (ret)
+		goto unreg;
+
+	ret = prp_init_controls(priv);
+	if (ret)
+		goto unreg;
+
+	return 0;
+unreg:
+	imx_media_capture_device_unregister(priv->vdev);
+	return ret;
+}
+
+static void prp_unregistered(struct v4l2_subdev *sd)
+{
+	struct prp_priv *priv = sd_to_priv(sd);
+
+	imx_media_capture_device_unregister(priv->vdev);
+	v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+}
+
+static const struct v4l2_subdev_pad_ops prp_pad_ops = {
+	.enum_mbus_code = prp_enum_mbus_code,
+	.enum_frame_size = prp_enum_frame_size,
+	.get_fmt = prp_get_fmt,
+	.set_fmt = prp_set_fmt,
+};
+
+static const struct v4l2_subdev_video_ops prp_video_ops = {
+	.g_frame_interval = prp_g_frame_interval,
+	.s_frame_interval = prp_s_frame_interval,
+	.s_stream = prp_s_stream,
+};
+
+static const struct media_entity_operations prp_entity_ops = {
+	.link_setup = prp_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_ops prp_subdev_ops = {
+	.video = &prp_video_ops,
+	.pad = &prp_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops prp_internal_ops = {
+	.registered = prp_registered,
+	.unregistered = prp_unregistered,
+};
+
+static int prp_init(struct imx_ic_priv *ic_priv)
+{
+	struct prp_priv *priv;
+
+	priv = devm_kzalloc(ic_priv->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	ic_priv->task_priv = priv;
+	priv->ic_priv = ic_priv;
+
+	spin_lock_init(&priv->irqlock);
+	init_timer(&priv->eof_timeout_timer);
+	priv->eof_timeout_timer.data = (unsigned long)priv;
+	priv->eof_timeout_timer.function = prp_eof_timeout;
+
+	priv->vdev = imx_media_capture_device_init(&ic_priv->sd,
+						   PRPENCVF_SRC_PAD);
+	if (IS_ERR(priv->vdev))
+		return PTR_ERR(priv->vdev);
+
+	mutex_init(&priv->lock);
+
+	return 0;
+}
+
+static void prp_remove(struct imx_ic_priv *ic_priv)
+{
+	struct prp_priv *priv = ic_priv->task_priv;
+
+	mutex_destroy(&priv->lock);
+	imx_media_capture_device_remove(priv->vdev);
+}
+
+struct imx_ic_ops imx_ic_prpencvf_ops = {
+	.subdev_ops = &prp_subdev_ops,
+	.internal_ops = &prp_internal_ops,
+	.entity_ops = &prp_entity_ops,
+	.init = prp_init,
+	.remove = prp_remove,
+};
diff --git a/drivers/staging/media/imx/imx-ic.h b/drivers/staging/media/imx/imx-ic.h
new file mode 100644
index 000000000000..6b2267bda8ab
--- /dev/null
+++ b/drivers/staging/media/imx/imx-ic.h
@@ -0,0 +1,38 @@
+/*
+ * V4L2 Image Converter Subdev for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2016 Mentor Graphics 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.
+ */
+#ifndef _IMX_IC_H
+#define _IMX_IC_H
+
+#include <media/v4l2-subdev.h>
+
+struct imx_ic_priv {
+	struct device *dev;
+	struct v4l2_subdev sd;
+	int    ipu_id;
+	int    task_id;
+	void   *prp_priv;
+	void   *task_priv;
+};
+
+struct imx_ic_ops {
+	const struct v4l2_subdev_ops *subdev_ops;
+	const struct v4l2_subdev_internal_ops *internal_ops;
+	const struct media_entity_operations *entity_ops;
+
+	int (*init)(struct imx_ic_priv *ic_priv);
+	void (*remove)(struct imx_ic_priv *ic_priv);
+};
+
+extern struct imx_ic_ops imx_ic_prp_ops;
+extern struct imx_ic_ops imx_ic_prpencvf_ops;
+extern struct imx_ic_ops imx_ic_pp_ops;
+
+#endif
diff --git a/drivers/staging/media/imx/imx-media-capture.c b/drivers/staging/media/imx/imx-media-capture.c
new file mode 100644
index 000000000000..ddab4c249da2
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-capture.c
@@ -0,0 +1,775 @@
+/*
+ * Video Capture Subdev for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2012-2016 Mentor Graphics Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/timer.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-contig.h>
+#include <video/imx-ipu-v3.h>
+#include <media/imx.h>
+#include "imx-media.h"
+
+struct capture_priv {
+	struct imx_media_video_dev vdev;
+
+	struct v4l2_subdev    *src_sd;
+	int                   src_sd_pad;
+	struct device         *dev;
+
+	struct imx_media_dev  *md;
+
+	struct media_pad      vdev_pad;
+
+	struct mutex          mutex;       /* capture device mutex */
+
+	/* the videobuf2 queue */
+	struct vb2_queue       q;
+	/* list of ready imx_media_buffer's from q */
+	struct list_head       ready_q;
+	/* protect ready_q */
+	spinlock_t             q_lock;
+
+	/* controls inherited from subdevs */
+	struct v4l2_ctrl_handler ctrl_hdlr;
+
+	/* misc status */
+	bool                  stop;          /* streaming is stopping */
+};
+
+#define to_capture_priv(v) container_of(v, struct capture_priv, vdev)
+
+/* In bytes, per queue */
+#define VID_MEM_LIMIT	SZ_64M
+
+static struct vb2_ops capture_qops;
+
+/*
+ * Video ioctls follow
+ */
+
+static int vidioc_querycap(struct file *file, void *fh,
+			   struct v4l2_capability *cap)
+{
+	struct capture_priv *priv = video_drvdata(file);
+
+	strncpy(cap->driver, "imx-media-capture", sizeof(cap->driver) - 1);
+	strncpy(cap->card, "imx-media-capture", sizeof(cap->card) - 1);
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+		 "platform:%s", priv->src_sd->name);
+
+	return 0;
+}
+
+static int capture_enum_framesizes(struct file *file, void *fh,
+				   struct v4l2_frmsizeenum *fsize)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	const struct imx_media_pixfmt *cc;
+	struct v4l2_subdev_frame_size_enum fse = {
+		.index = fsize->index,
+		.pad = priv->src_sd_pad,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	cc = imx_media_find_format(fsize->pixel_format, CS_SEL_ANY, true);
+	if (!cc)
+		return -EINVAL;
+
+	fse.code = cc->codes[0];
+
+	ret = v4l2_subdev_call(priv->src_sd, pad, enum_frame_size, NULL, &fse);
+	if (ret)
+		return ret;
+
+	if (fse.min_width == fse.max_width &&
+	    fse.min_height == fse.max_height) {
+		fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+		fsize->discrete.width = fse.min_width;
+		fsize->discrete.height = fse.min_height;
+	} else {
+		fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+		fsize->stepwise.min_width = fse.min_width;
+		fsize->stepwise.max_width = fse.max_width;
+		fsize->stepwise.min_height = fse.min_height;
+		fsize->stepwise.max_height = fse.max_height;
+		fsize->stepwise.step_width = 1;
+		fsize->stepwise.step_height = 1;
+	}
+
+	return 0;
+}
+
+static int capture_enum_frameintervals(struct file *file, void *fh,
+				       struct v4l2_frmivalenum *fival)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	const struct imx_media_pixfmt *cc;
+	struct v4l2_subdev_frame_interval_enum fie = {
+		.index = fival->index,
+		.pad = priv->src_sd_pad,
+		.width = fival->width,
+		.height = fival->height,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	cc = imx_media_find_format(fival->pixel_format, CS_SEL_ANY, true);
+	if (!cc)
+		return -EINVAL;
+
+	fie.code = cc->codes[0];
+
+	ret = v4l2_subdev_call(priv->src_sd, pad, enum_frame_interval, NULL, &fie);
+	if (ret)
+		return ret;
+
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete = fie.interval;
+
+	return 0;
+}
+
+static int capture_enum_fmt_vid_cap(struct file *file, void *fh,
+				    struct v4l2_fmtdesc *f)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	const struct imx_media_pixfmt *cc_src;
+	struct v4l2_subdev_format fmt_src;
+	u32 fourcc;
+	int ret;
+
+	fmt_src.pad = priv->src_sd_pad;
+	fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(priv->src_sd, pad, get_fmt, NULL, &fmt_src);
+	if (ret) {
+		v4l2_err(priv->src_sd, "failed to get src_sd format\n");
+		return ret;
+	}
+
+	cc_src = imx_media_find_ipu_format(fmt_src.format.code, CS_SEL_ANY);
+	if (!cc_src)
+		cc_src = imx_media_find_mbus_format(fmt_src.format.code,
+						    CS_SEL_ANY, true);
+	if (!cc_src)
+		return -EINVAL;
+
+	if (cc_src->bayer) {
+		if (f->index != 0)
+			return -EINVAL;
+		fourcc = cc_src->fourcc;
+	} else {
+		u32 cs_sel = (cc_src->cs == IPUV3_COLORSPACE_YUV) ?
+			CS_SEL_YUV : CS_SEL_RGB;
+
+		ret = imx_media_enum_format(&fourcc, f->index, cs_sel);
+		if (ret)
+			return ret;
+	}
+
+	f->pixelformat = fourcc;
+
+	return 0;
+}
+
+static int capture_g_fmt_vid_cap(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct capture_priv *priv = video_drvdata(file);
+
+	*f = priv->vdev.fmt;
+
+	return 0;
+}
+
+static int capture_try_fmt_vid_cap(struct file *file, void *fh,
+				   struct v4l2_format *f)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	struct v4l2_subdev_format fmt_src;
+	const struct imx_media_pixfmt *cc, *cc_src;
+	int ret;
+
+	fmt_src.pad = priv->src_sd_pad;
+	fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(priv->src_sd, pad, get_fmt, NULL, &fmt_src);
+	if (ret)
+		return ret;
+
+	cc_src = imx_media_find_ipu_format(fmt_src.format.code, CS_SEL_ANY);
+	if (!cc_src)
+		cc_src = imx_media_find_mbus_format(fmt_src.format.code,
+						    CS_SEL_ANY, true);
+	if (!cc_src)
+		return -EINVAL;
+
+	if (cc_src->bayer) {
+		cc = cc_src;
+	} else {
+		u32 fourcc, cs_sel;
+
+		cs_sel = (cc_src->cs == IPUV3_COLORSPACE_YUV) ?
+			CS_SEL_YUV : CS_SEL_RGB;
+		fourcc = f->fmt.pix.pixelformat;
+
+		cc = imx_media_find_format(fourcc, cs_sel, false);
+		if (!cc) {
+			imx_media_enum_format(&fourcc, 0, cs_sel);
+			cc = imx_media_find_format(fourcc, cs_sel, false);
+		}
+	}
+
+	imx_media_mbus_fmt_to_pix_fmt(&f->fmt.pix, &fmt_src.format, cc);
+
+	return 0;
+}
+
+static int capture_s_fmt_vid_cap(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	int ret;
+
+	if (vb2_is_busy(&priv->q)) {
+		v4l2_err(priv->src_sd, "%s queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	ret = capture_try_fmt_vid_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	priv->vdev.fmt.fmt.pix = f->fmt.pix;
+	priv->vdev.cc = imx_media_find_format(f->fmt.pix.pixelformat,
+					      CS_SEL_ANY, true);
+
+	return 0;
+}
+
+static int capture_querystd(struct file *file, void *fh, v4l2_std_id *std)
+{
+	struct capture_priv *priv = video_drvdata(file);
+
+	return v4l2_subdev_call(priv->src_sd, video, querystd, std);
+}
+
+static int capture_g_std(struct file *file, void *fh, v4l2_std_id *std)
+{
+	struct capture_priv *priv = video_drvdata(file);
+
+	return v4l2_subdev_call(priv->src_sd, video, g_std, std);
+}
+
+static int capture_s_std(struct file *file, void *fh, v4l2_std_id std)
+{
+	struct capture_priv *priv = video_drvdata(file);
+
+	if (vb2_is_busy(&priv->q))
+		return -EBUSY;
+
+	return v4l2_subdev_call(priv->src_sd, video, s_std, std);
+}
+
+static int capture_g_parm(struct file *file, void *fh,
+			  struct v4l2_streamparm *a)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	struct v4l2_subdev_frame_interval fi;
+	int ret;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	memset(&fi, 0, sizeof(fi));
+	fi.pad = priv->src_sd_pad;
+	ret = v4l2_subdev_call(priv->src_sd, video, g_frame_interval, &fi);
+	if (ret < 0)
+		return ret;
+
+	a->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	a->parm.capture.timeperframe = fi.interval;
+
+	return 0;
+}
+
+static int capture_s_parm(struct file *file, void *fh,
+			  struct v4l2_streamparm *a)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	struct v4l2_subdev_frame_interval fi;
+	int ret;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	memset(&fi, 0, sizeof(fi));
+	fi.pad = priv->src_sd_pad;
+	fi.interval = a->parm.capture.timeperframe;
+	ret = v4l2_subdev_call(priv->src_sd, video, s_frame_interval, &fi);
+	if (ret < 0)
+		return ret;
+
+	a->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	a->parm.capture.timeperframe = fi.interval;
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops capture_ioctl_ops = {
+	.vidioc_querycap	= vidioc_querycap,
+
+	.vidioc_enum_framesizes = capture_enum_framesizes,
+	.vidioc_enum_frameintervals = capture_enum_frameintervals,
+
+	.vidioc_enum_fmt_vid_cap        = capture_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap           = capture_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap         = capture_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap           = capture_s_fmt_vid_cap,
+
+	.vidioc_querystd        = capture_querystd,
+	.vidioc_g_std           = capture_g_std,
+	.vidioc_s_std           = capture_s_std,
+
+	.vidioc_g_parm          = capture_g_parm,
+	.vidioc_s_parm          = capture_s_parm,
+
+	.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_expbuf		= vb2_ioctl_expbuf,
+	.vidioc_streamon	= vb2_ioctl_streamon,
+	.vidioc_streamoff	= vb2_ioctl_streamoff,
+};
+
+/*
+ * Queue operations
+ */
+
+static int capture_queue_setup(struct vb2_queue *vq,
+			       unsigned int *nbuffers,
+			       unsigned int *nplanes,
+			       unsigned int sizes[],
+			       struct device *alloc_devs[])
+{
+	struct capture_priv *priv = vb2_get_drv_priv(vq);
+	struct v4l2_pix_format *pix = &priv->vdev.fmt.fmt.pix;
+	unsigned int count = *nbuffers;
+
+	if (vq->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (*nplanes) {
+		if (*nplanes != 1 || sizes[0] < pix->sizeimage)
+			return -EINVAL;
+		count += vq->num_buffers;
+	}
+
+	count = min_t(__u32, VID_MEM_LIMIT / pix->sizeimage, count);
+
+	if (*nplanes)
+		*nbuffers = (count < vq->num_buffers) ? 0 :
+			count - vq->num_buffers;
+	else
+		*nbuffers = count;
+
+	*nplanes = 1;
+	sizes[0] = pix->sizeimage;
+
+	return 0;
+}
+
+static int capture_buf_init(struct vb2_buffer *vb)
+{
+	struct imx_media_buffer *buf = to_imx_media_vb(vb);
+
+	INIT_LIST_HEAD(&buf->list);
+
+	return 0;
+}
+
+static int capture_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct capture_priv *priv = vb2_get_drv_priv(vq);
+	struct v4l2_pix_format *pix = &priv->vdev.fmt.fmt.pix;
+
+	if (vb2_plane_size(vb, 0) < pix->sizeimage) {
+		v4l2_err(priv->src_sd,
+			 "data will not fit into plane (%lu < %lu)\n",
+			 vb2_plane_size(vb, 0), (long)pix->sizeimage);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, pix->sizeimage);
+
+	return 0;
+}
+
+static void capture_buf_queue(struct vb2_buffer *vb)
+{
+	struct capture_priv *priv = vb2_get_drv_priv(vb->vb2_queue);
+	struct imx_media_buffer *buf = to_imx_media_vb(vb);
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->q_lock, flags);
+
+	list_add_tail(&buf->list, &priv->ready_q);
+
+	spin_unlock_irqrestore(&priv->q_lock, flags);
+}
+
+static int capture_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct capture_priv *priv = vb2_get_drv_priv(vq);
+	struct imx_media_buffer *buf, *tmp;
+	unsigned long flags;
+	int ret;
+
+	if (vb2_is_streaming(vq))
+		return 0;
+
+	ret = imx_media_pipeline_set_stream(priv->md, &priv->src_sd->entity,
+					    true);
+	if (ret) {
+		v4l2_err(priv->src_sd, "pipeline start failed with %d\n", ret);
+		goto return_bufs;
+	}
+
+	priv->stop = false;
+
+	return 0;
+
+return_bufs:
+	spin_lock_irqsave(&priv->q_lock, flags);
+	list_for_each_entry_safe(buf, tmp, &priv->ready_q, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vbuf.vb2_buf, VB2_BUF_STATE_QUEUED);
+	}
+	spin_unlock_irqrestore(&priv->q_lock, flags);
+	return ret;
+}
+
+static void capture_stop_streaming(struct vb2_queue *vq)
+{
+	struct capture_priv *priv = vb2_get_drv_priv(vq);
+	struct imx_media_buffer *frame;
+	unsigned long flags;
+	int ret;
+
+	if (!vb2_is_streaming(vq))
+		return;
+
+	spin_lock_irqsave(&priv->q_lock, flags);
+	priv->stop = true;
+	spin_unlock_irqrestore(&priv->q_lock, flags);
+
+	ret = imx_media_pipeline_set_stream(priv->md, &priv->src_sd->entity,
+					    false);
+	if (ret)
+		v4l2_warn(priv->src_sd, "pipeline stop failed with %d\n", ret);
+
+	/* release all active buffers */
+	spin_lock_irqsave(&priv->q_lock, flags);
+	while (!list_empty(&priv->ready_q)) {
+		frame = list_entry(priv->ready_q.next,
+				   struct imx_media_buffer, list);
+		list_del(&frame->list);
+		vb2_buffer_done(&frame->vbuf.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irqrestore(&priv->q_lock, flags);
+}
+
+static struct vb2_ops capture_qops = {
+	.queue_setup	 = capture_queue_setup,
+	.buf_init        = capture_buf_init,
+	.buf_prepare	 = capture_buf_prepare,
+	.buf_queue	 = capture_buf_queue,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
+	.start_streaming = capture_start_streaming,
+	.stop_streaming  = capture_stop_streaming,
+};
+
+/*
+ * File operations
+ */
+static int capture_open(struct file *file)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	struct video_device *vfd = priv->vdev.vfd;
+	int ret;
+
+	if (mutex_lock_interruptible(&priv->mutex))
+		return -ERESTARTSYS;
+
+	ret = v4l2_fh_open(file);
+	if (ret)
+		v4l2_err(priv->src_sd, "v4l2_fh_open failed\n");
+
+	ret = v4l2_pipeline_pm_use(&vfd->entity, 1);
+	if (ret)
+		v4l2_fh_release(file);
+
+	mutex_unlock(&priv->mutex);
+	return ret;
+}
+
+static int capture_release(struct file *file)
+{
+	struct capture_priv *priv = video_drvdata(file);
+	struct video_device *vfd = priv->vdev.vfd;
+	struct vb2_queue *vq = &priv->q;
+	int ret = 0;
+
+	mutex_lock(&priv->mutex);
+
+	if (file->private_data == vq->owner) {
+		vb2_queue_release(vq);
+		vq->owner = NULL;
+	}
+
+	v4l2_pipeline_pm_use(&vfd->entity, 0);
+
+	v4l2_fh_release(file);
+	mutex_unlock(&priv->mutex);
+	return ret;
+}
+
+static const struct v4l2_file_operations capture_fops = {
+	.owner		= THIS_MODULE,
+	.open		= capture_open,
+	.release	= capture_release,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= vb2_fop_mmap,
+};
+
+static struct video_device capture_videodev = {
+	.fops		= &capture_fops,
+	.ioctl_ops	= &capture_ioctl_ops,
+	.minor		= -1,
+	.release	= video_device_release,
+	.vfl_dir	= VFL_DIR_RX,
+	.tvnorms	= V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM,
+	.device_caps	= V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING,
+};
+
+void imx_media_capture_device_set_format(struct imx_media_video_dev *vdev,
+					 struct v4l2_pix_format *pix)
+{
+	struct capture_priv *priv = to_capture_priv(vdev);
+
+	mutex_lock(&priv->mutex);
+	priv->vdev.fmt.fmt.pix = *pix;
+	priv->vdev.cc = imx_media_find_format(pix->pixelformat, CS_SEL_ANY,
+					      true);
+	mutex_unlock(&priv->mutex);
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_set_format);
+
+struct imx_media_buffer *
+imx_media_capture_device_next_buf(struct imx_media_video_dev *vdev)
+{
+	struct capture_priv *priv = to_capture_priv(vdev);
+	struct imx_media_buffer *buf = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->q_lock, flags);
+
+	/* get next queued buffer */
+	if (!list_empty(&priv->ready_q)) {
+		buf = list_entry(priv->ready_q.next, struct imx_media_buffer,
+				 list);
+		list_del(&buf->list);
+	}
+
+	spin_unlock_irqrestore(&priv->q_lock, flags);
+
+	return buf;
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_next_buf);
+
+void imx_media_capture_device_error(struct imx_media_video_dev *vdev)
+{
+	struct capture_priv *priv = to_capture_priv(vdev);
+	struct vb2_queue *vq = &priv->q;
+	unsigned long flags;
+
+	if (!vb2_is_streaming(vq))
+		return;
+
+	spin_lock_irqsave(&priv->q_lock, flags);
+	vb2_queue_error(vq);
+	spin_unlock_irqrestore(&priv->q_lock, flags);
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_error);
+
+int imx_media_capture_device_register(struct imx_media_video_dev *vdev)
+{
+	struct capture_priv *priv = to_capture_priv(vdev);
+	struct v4l2_subdev *sd = priv->src_sd;
+	struct video_device *vfd = vdev->vfd;
+	struct vb2_queue *vq = &priv->q;
+	struct v4l2_subdev_format fmt_src;
+	int ret;
+
+	/* get media device */
+	priv->md = dev_get_drvdata(sd->v4l2_dev->dev);
+
+	vfd->v4l2_dev = sd->v4l2_dev;
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		v4l2_err(sd, "Failed to register video device\n");
+		return ret;
+	}
+
+	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	vq->drv_priv = priv;
+	vq->buf_struct_size = sizeof(struct imx_media_buffer);
+	vq->ops = &capture_qops;
+	vq->mem_ops = &vb2_dma_contig_memops;
+	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vq->lock = &priv->mutex;
+	vq->min_buffers_needed = 2;
+	vq->dev = priv->dev;
+
+	ret = vb2_queue_init(vq);
+	if (ret) {
+		v4l2_err(sd, "vb2_queue_init failed\n");
+		goto unreg;
+	}
+
+	INIT_LIST_HEAD(&priv->ready_q);
+
+	priv->vdev_pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(&vfd->entity, 1, &priv->vdev_pad);
+	if (ret) {
+		v4l2_err(sd, "failed to init dev pad\n");
+		goto unreg;
+	}
+
+	/* create the link from the src_sd devnode pad to device node */
+	ret = media_create_pad_link(&sd->entity, priv->src_sd_pad,
+				    &vfd->entity, 0, 0);
+	if (ret) {
+		v4l2_err(sd, "failed to create link to device node\n");
+		goto unreg;
+	}
+
+	/* setup default format */
+	fmt_src.pad = priv->src_sd_pad;
+	fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt_src);
+	if (ret) {
+		v4l2_err(sd, "failed to get src_sd format\n");
+		goto unreg;
+	}
+
+	vdev->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	imx_media_mbus_fmt_to_pix_fmt(&vdev->fmt.fmt.pix,
+				      &fmt_src.format, NULL);
+	vdev->cc = imx_media_find_format(vdev->fmt.fmt.pix.pixelformat,
+					 CS_SEL_ANY, false);
+
+	v4l2_info(sd, "Registered %s as /dev/%s\n", vfd->name,
+		  video_device_node_name(vfd));
+
+	vfd->ctrl_handler = &priv->ctrl_hdlr;
+
+	return 0;
+unreg:
+	video_unregister_device(vfd);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_register);
+
+void imx_media_capture_device_unregister(struct imx_media_video_dev *vdev)
+{
+	struct capture_priv *priv = to_capture_priv(vdev);
+	struct video_device *vfd = priv->vdev.vfd;
+
+	mutex_lock(&priv->mutex);
+
+	if (video_is_registered(vfd)) {
+		video_unregister_device(vfd);
+		media_entity_cleanup(&vfd->entity);
+	}
+
+	mutex_unlock(&priv->mutex);
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_unregister);
+
+struct imx_media_video_dev *
+imx_media_capture_device_init(struct v4l2_subdev *src_sd, int pad)
+{
+	struct capture_priv *priv;
+	struct video_device *vfd;
+
+	priv = devm_kzalloc(src_sd->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return ERR_PTR(-ENOMEM);
+
+	priv->src_sd = src_sd;
+	priv->src_sd_pad = pad;
+	priv->dev = src_sd->dev;
+
+	mutex_init(&priv->mutex);
+	spin_lock_init(&priv->q_lock);
+
+	snprintf(capture_videodev.name, sizeof(capture_videodev.name),
+		 "%s capture", src_sd->name);
+
+	vfd = video_device_alloc();
+	if (!vfd)
+		return ERR_PTR(-ENOMEM);
+
+	*vfd = capture_videodev;
+	vfd->lock = &priv->mutex;
+	vfd->queue = &priv->q;
+	priv->vdev.vfd = vfd;
+
+	video_set_drvdata(vfd, priv);
+
+	v4l2_ctrl_handler_init(&priv->ctrl_hdlr, 0);
+
+	return &priv->vdev;
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_init);
+
+void imx_media_capture_device_remove(struct imx_media_video_dev *vdev)
+{
+	struct capture_priv *priv = to_capture_priv(vdev);
+
+	v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+}
+EXPORT_SYMBOL_GPL(imx_media_capture_device_remove);
+
+MODULE_DESCRIPTION("i.MX5/6 v4l2 video capture interface driver");
+MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/imx/imx-media-csi.c b/drivers/staging/media/imx/imx-media-csi.c
new file mode 100644
index 000000000000..a2d26693912e
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-csi.c
@@ -0,0 +1,1817 @@
+/*
+ * V4L2 Capture CSI Subdev for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2014-2017 Mentor Graphics Inc.
+ * Copyright (C) 2017 Pengutronix, Philipp Zabel <kernel@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/delay.h>
+#include <linux/gcd.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-contig.h>
+#include <video/imx-ipu-v3.h>
+#include <media/imx.h>
+#include "imx-media.h"
+
+/*
+ * Min/Max supported width and heights.
+ *
+ * We allow planar output, so we have to align width by 16 pixels
+ * to meet IDMAC alignment requirements.
+ *
+ * TODO: move this into pad format negotiation, if capture device
+ * has not requested planar formats, we should allow 8 pixel
+ * alignment.
+ */
+#define MIN_W       176
+#define MIN_H       144
+#define MAX_W      4096
+#define MAX_H      4096
+#define W_ALIGN    4 /* multiple of 16 pixels */
+#define H_ALIGN    1 /* multiple of 2 lines */
+#define S_ALIGN    1 /* multiple of 2 */
+
+/*
+ * struct csi_skip_desc - CSI frame skipping descriptor
+ * @keep - number of frames kept per max_ratio frames
+ * @max_ratio - width of skip_smfc, written to MAX_RATIO bitfield
+ * @skip_smfc - skip pattern written to the SKIP_SMFC bitfield
+ */
+struct csi_skip_desc {
+	u8 keep;
+	u8 max_ratio;
+	u8 skip_smfc;
+};
+
+struct csi_priv {
+	struct device *dev;
+	struct ipu_soc *ipu;
+	struct imx_media_dev *md;
+	struct v4l2_subdev sd;
+	struct media_pad pad[CSI_NUM_PADS];
+	/* the video device at IDMAC output pad */
+	struct imx_media_video_dev *vdev;
+	struct imx_media_fim *fim;
+	int csi_id;
+	int smfc_id;
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	int active_output_pad;
+
+	struct ipuv3_channel *idmac_ch;
+	struct ipu_smfc *smfc;
+	struct ipu_csi *csi;
+
+	struct v4l2_mbus_framefmt format_mbus[CSI_NUM_PADS];
+	const struct imx_media_pixfmt *cc[CSI_NUM_PADS];
+	struct v4l2_fract frame_interval[CSI_NUM_PADS];
+	struct v4l2_rect crop;
+	struct v4l2_rect compose;
+	const struct csi_skip_desc *skip;
+
+	/* active vb2 buffers to send to video dev sink */
+	struct imx_media_buffer *active_vb2_buf[2];
+	struct imx_media_dma_buf underrun_buf;
+
+	int ipu_buf_num;  /* ipu double buffer index: 0-1 */
+
+	/* the sink for the captured frames */
+	struct media_entity *sink;
+	enum ipu_csi_dest dest;
+	/* the source subdev */
+	struct v4l2_subdev *src_sd;
+
+	/* the mipi virtual channel number at link validate */
+	int vc_num;
+
+	/* the attached sensor at stream on */
+	struct imx_media_subdev *sensor;
+
+	spinlock_t irqlock; /* protect eof_irq handler */
+	struct timer_list eof_timeout_timer;
+	int eof_irq;
+	int nfb4eof_irq;
+
+	struct v4l2_ctrl_handler ctrl_hdlr;
+
+	int stream_count; /* streaming counter */
+	bool last_eof;   /* waiting for last EOF at stream off */
+	bool nfb4eof;    /* NFB4EOF encountered during streaming */
+	struct completion last_eof_comp;
+};
+
+static inline struct csi_priv *sd_to_dev(struct v4l2_subdev *sdev)
+{
+	return container_of(sdev, struct csi_priv, sd);
+}
+
+static void csi_idmac_put_ipu_resources(struct csi_priv *priv)
+{
+	if (!IS_ERR_OR_NULL(priv->idmac_ch))
+		ipu_idmac_put(priv->idmac_ch);
+	priv->idmac_ch = NULL;
+
+	if (!IS_ERR_OR_NULL(priv->smfc))
+		ipu_smfc_put(priv->smfc);
+	priv->smfc = NULL;
+}
+
+static int csi_idmac_get_ipu_resources(struct csi_priv *priv)
+{
+	int ch_num, ret;
+
+	ch_num = IPUV3_CHANNEL_CSI0 + priv->smfc_id;
+
+	priv->smfc = ipu_smfc_get(priv->ipu, ch_num);
+	if (IS_ERR(priv->smfc)) {
+		v4l2_err(&priv->sd, "failed to get SMFC\n");
+		ret = PTR_ERR(priv->smfc);
+		goto out;
+	}
+
+	priv->idmac_ch = ipu_idmac_get(priv->ipu, ch_num);
+	if (IS_ERR(priv->idmac_ch)) {
+		v4l2_err(&priv->sd, "could not get IDMAC channel %u\n",
+			 ch_num);
+		ret = PTR_ERR(priv->idmac_ch);
+		goto out;
+	}
+
+	return 0;
+out:
+	csi_idmac_put_ipu_resources(priv);
+	return ret;
+}
+
+static void csi_vb2_buf_done(struct csi_priv *priv)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_media_buffer *done, *next;
+	struct vb2_buffer *vb;
+	dma_addr_t phys;
+
+	done = priv->active_vb2_buf[priv->ipu_buf_num];
+	if (done) {
+		vb = &done->vbuf.vb2_buf;
+		vb->timestamp = ktime_get_ns();
+		vb2_buffer_done(vb, priv->nfb4eof ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+	}
+
+	priv->nfb4eof = false;
+
+	/* get next queued buffer */
+	next = imx_media_capture_device_next_buf(vdev);
+	if (next) {
+		phys = vb2_dma_contig_plane_dma_addr(&next->vbuf.vb2_buf, 0);
+		priv->active_vb2_buf[priv->ipu_buf_num] = next;
+	} else {
+		phys = priv->underrun_buf.phys;
+		priv->active_vb2_buf[priv->ipu_buf_num] = NULL;
+	}
+
+	if (ipu_idmac_buffer_is_ready(priv->idmac_ch, priv->ipu_buf_num))
+		ipu_idmac_clear_buffer(priv->idmac_ch, priv->ipu_buf_num);
+
+	ipu_cpmem_set_buffer(priv->idmac_ch, priv->ipu_buf_num, phys);
+}
+
+static irqreturn_t csi_idmac_eof_interrupt(int irq, void *dev_id)
+{
+	struct csi_priv *priv = dev_id;
+
+	spin_lock(&priv->irqlock);
+
+	if (priv->last_eof) {
+		complete(&priv->last_eof_comp);
+		priv->last_eof = false;
+		goto unlock;
+	}
+
+	if (priv->fim) {
+		struct timespec cur_ts;
+
+		ktime_get_ts(&cur_ts);
+		/* call frame interval monitor */
+		imx_media_fim_eof_monitor(priv->fim, &cur_ts);
+	}
+
+	csi_vb2_buf_done(priv);
+
+	/* select new IPU buf */
+	ipu_idmac_select_buffer(priv->idmac_ch, priv->ipu_buf_num);
+	/* toggle IPU double-buffer index */
+	priv->ipu_buf_num ^= 1;
+
+	/* bump the EOF timeout timer */
+	mod_timer(&priv->eof_timeout_timer,
+		  jiffies + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+
+unlock:
+	spin_unlock(&priv->irqlock);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t csi_idmac_nfb4eof_interrupt(int irq, void *dev_id)
+{
+	struct csi_priv *priv = dev_id;
+
+	spin_lock(&priv->irqlock);
+
+	/*
+	 * this is not an unrecoverable error, just mark
+	 * the next captured frame with vb2 error flag.
+	 */
+	priv->nfb4eof = true;
+
+	v4l2_err(&priv->sd, "NFB4EOF\n");
+
+	spin_unlock(&priv->irqlock);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * EOF timeout timer function. This is an unrecoverable condition
+ * without a stream restart.
+ */
+static void csi_idmac_eof_timeout(unsigned long data)
+{
+	struct csi_priv *priv = (struct csi_priv *)data;
+	struct imx_media_video_dev *vdev = priv->vdev;
+
+	v4l2_err(&priv->sd, "EOF timeout\n");
+
+	/* signal a fatal error to capture device */
+	imx_media_capture_device_error(vdev);
+}
+
+static void csi_idmac_setup_vb2_buf(struct csi_priv *priv, dma_addr_t *phys)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct imx_media_buffer *buf;
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		buf = imx_media_capture_device_next_buf(vdev);
+		if (buf) {
+			priv->active_vb2_buf[i] = buf;
+			phys[i] = vb2_dma_contig_plane_dma_addr(
+				&buf->vbuf.vb2_buf, 0);
+		} else {
+			priv->active_vb2_buf[i] = NULL;
+			phys[i] = priv->underrun_buf.phys;
+		}
+	}
+}
+
+static void csi_idmac_unsetup_vb2_buf(struct csi_priv *priv,
+				      enum vb2_buffer_state return_status)
+{
+	struct imx_media_buffer *buf;
+	int i;
+
+	/* return any remaining active frames with return_status */
+	for (i = 0; i < 2; i++) {
+		buf = priv->active_vb2_buf[i];
+		if (buf) {
+			struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
+
+			vb->timestamp = ktime_get_ns();
+			vb2_buffer_done(vb, return_status);
+		}
+	}
+}
+
+/* init the SMFC IDMAC channel */
+static int csi_idmac_setup_channel(struct csi_priv *priv)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct v4l2_fwnode_endpoint *sensor_ep;
+	struct v4l2_mbus_framefmt *infmt;
+	struct ipu_image image;
+	u32 passthrough_bits;
+	dma_addr_t phys[2];
+	bool passthrough;
+	u32 burst_size;
+	int ret;
+
+	infmt = &priv->format_mbus[CSI_SINK_PAD];
+	sensor_ep = &priv->sensor->sensor_ep;
+
+	ipu_cpmem_zero(priv->idmac_ch);
+
+	memset(&image, 0, sizeof(image));
+	image.pix = vdev->fmt.fmt.pix;
+	image.rect.width = image.pix.width;
+	image.rect.height = image.pix.height;
+
+	csi_idmac_setup_vb2_buf(priv, phys);
+
+	image.phys0 = phys[0];
+	image.phys1 = phys[1];
+
+	/*
+	 * Check for conditions that require the IPU to handle the
+	 * data internally as generic data, aka passthrough mode:
+	 * - raw bayer formats
+	 * - the sensor bus is 16-bit parallel
+	 */
+	switch (image.pix.pixelformat) {
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SGBRG8:
+	case V4L2_PIX_FMT_SGRBG8:
+	case V4L2_PIX_FMT_SRGGB8:
+		burst_size = 8;
+		passthrough = true;
+		passthrough_bits = 8;
+		break;
+	case V4L2_PIX_FMT_SBGGR16:
+	case V4L2_PIX_FMT_SGBRG16:
+	case V4L2_PIX_FMT_SGRBG16:
+	case V4L2_PIX_FMT_SRGGB16:
+		burst_size = 4;
+		passthrough = true;
+		passthrough_bits = 16;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_NV12:
+		burst_size = (image.pix.width & 0x3f) ?
+			     ((image.pix.width & 0x1f) ?
+			      ((image.pix.width & 0xf) ? 8 : 16) : 32) : 64;
+		passthrough = (sensor_ep->bus_type != V4L2_MBUS_CSI2 &&
+			       sensor_ep->bus.parallel.bus_width >= 16);
+		passthrough_bits = 16;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_UYVY:
+		burst_size = (image.pix.width & 0x1f) ?
+			     ((image.pix.width & 0xf) ? 8 : 16) : 32;
+		passthrough = (sensor_ep->bus_type != V4L2_MBUS_CSI2 &&
+			       sensor_ep->bus.parallel.bus_width >= 16);
+		passthrough_bits = 16;
+		break;
+	default:
+		burst_size = (image.pix.width & 0xf) ? 8 : 16;
+		passthrough = (sensor_ep->bus_type != V4L2_MBUS_CSI2 &&
+			       sensor_ep->bus.parallel.bus_width >= 16);
+		passthrough_bits = 16;
+		break;
+	}
+
+	if (passthrough) {
+		ipu_cpmem_set_resolution(priv->idmac_ch, image.rect.width,
+					 image.rect.height);
+		ipu_cpmem_set_stride(priv->idmac_ch, image.pix.bytesperline);
+		ipu_cpmem_set_buffer(priv->idmac_ch, 0, image.phys0);
+		ipu_cpmem_set_buffer(priv->idmac_ch, 1, image.phys1);
+		ipu_cpmem_set_format_passthrough(priv->idmac_ch,
+						 passthrough_bits);
+	} else {
+		ret = ipu_cpmem_set_image(priv->idmac_ch, &image);
+		if (ret)
+			goto unsetup_vb2;
+	}
+
+	ipu_cpmem_set_burstsize(priv->idmac_ch, burst_size);
+
+	/*
+	 * Set the channel for the direct CSI-->memory via SMFC
+	 * use-case to very high priority, by enabling the watermark
+	 * signal in the SMFC, enabling WM in the channel, and setting
+	 * the channel priority to high.
+	 *
+	 * Refer to the i.mx6 rev. D TRM Table 36-8: Calculated priority
+	 * value.
+	 *
+	 * The WM's are set very low by intention here to ensure that
+	 * the SMFC FIFOs do not overflow.
+	 */
+	ipu_smfc_set_watermark(priv->smfc, 0x02, 0x01);
+	ipu_cpmem_set_high_priority(priv->idmac_ch);
+	ipu_idmac_enable_watermark(priv->idmac_ch, true);
+	ipu_cpmem_set_axi_id(priv->idmac_ch, 0);
+
+	burst_size = passthrough ?
+		(burst_size >> 3) - 1 : (burst_size >> 2) - 1;
+
+	ipu_smfc_set_burstsize(priv->smfc, burst_size);
+
+	if (image.pix.field == V4L2_FIELD_NONE &&
+	    V4L2_FIELD_HAS_BOTH(infmt->field))
+		ipu_cpmem_interlaced_scan(priv->idmac_ch,
+					  image.pix.bytesperline);
+
+	ipu_idmac_set_double_buffer(priv->idmac_ch, true);
+
+	return 0;
+
+unsetup_vb2:
+	csi_idmac_unsetup_vb2_buf(priv, VB2_BUF_STATE_QUEUED);
+	return ret;
+}
+
+static void csi_idmac_unsetup(struct csi_priv *priv,
+			      enum vb2_buffer_state state)
+{
+	ipu_idmac_disable_channel(priv->idmac_ch);
+	ipu_smfc_disable(priv->smfc);
+
+	csi_idmac_unsetup_vb2_buf(priv, state);
+}
+
+static int csi_idmac_setup(struct csi_priv *priv)
+{
+	int ret;
+
+	ret = csi_idmac_setup_channel(priv);
+	if (ret)
+		return ret;
+
+	ipu_cpmem_dump(priv->idmac_ch);
+	ipu_dump(priv->ipu);
+
+	ipu_smfc_enable(priv->smfc);
+
+	/* set buffers ready */
+	ipu_idmac_select_buffer(priv->idmac_ch, 0);
+	ipu_idmac_select_buffer(priv->idmac_ch, 1);
+
+	/* enable the channels */
+	ipu_idmac_enable_channel(priv->idmac_ch);
+
+	return 0;
+}
+
+static int csi_idmac_start(struct csi_priv *priv)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	struct v4l2_pix_format *outfmt;
+	int ret;
+
+	ret = csi_idmac_get_ipu_resources(priv);
+	if (ret)
+		return ret;
+
+	ipu_smfc_map_channel(priv->smfc, priv->csi_id, priv->vc_num);
+
+	outfmt = &vdev->fmt.fmt.pix;
+
+	ret = imx_media_alloc_dma_buf(priv->md, &priv->underrun_buf,
+				      outfmt->sizeimage);
+	if (ret)
+		goto out_put_ipu;
+
+	priv->ipu_buf_num = 0;
+
+	/* init EOF completion waitq */
+	init_completion(&priv->last_eof_comp);
+	priv->last_eof = false;
+	priv->nfb4eof = false;
+
+	ret = csi_idmac_setup(priv);
+	if (ret) {
+		v4l2_err(&priv->sd, "csi_idmac_setup failed: %d\n", ret);
+		goto out_free_dma_buf;
+	}
+
+	priv->nfb4eof_irq = ipu_idmac_channel_irq(priv->ipu,
+						 priv->idmac_ch,
+						 IPU_IRQ_NFB4EOF);
+	ret = devm_request_irq(priv->dev, priv->nfb4eof_irq,
+			       csi_idmac_nfb4eof_interrupt, 0,
+			       "imx-smfc-nfb4eof", priv);
+	if (ret) {
+		v4l2_err(&priv->sd,
+			 "Error registering NFB4EOF irq: %d\n", ret);
+		goto out_unsetup;
+	}
+
+	priv->eof_irq = ipu_idmac_channel_irq(priv->ipu, priv->idmac_ch,
+					      IPU_IRQ_EOF);
+
+	ret = devm_request_irq(priv->dev, priv->eof_irq,
+			       csi_idmac_eof_interrupt, 0,
+			       "imx-smfc-eof", priv);
+	if (ret) {
+		v4l2_err(&priv->sd,
+			 "Error registering eof irq: %d\n", ret);
+		goto out_free_nfb4eof_irq;
+	}
+
+	/* start the EOF timeout timer */
+	mod_timer(&priv->eof_timeout_timer,
+		  jiffies + msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+
+	return 0;
+
+out_free_nfb4eof_irq:
+	devm_free_irq(priv->dev, priv->nfb4eof_irq, priv);
+out_unsetup:
+	csi_idmac_unsetup(priv, VB2_BUF_STATE_QUEUED);
+out_free_dma_buf:
+	imx_media_free_dma_buf(priv->md, &priv->underrun_buf);
+out_put_ipu:
+	csi_idmac_put_ipu_resources(priv);
+	return ret;
+}
+
+static void csi_idmac_stop(struct csi_priv *priv)
+{
+	unsigned long flags;
+	int ret;
+
+	/* mark next EOF interrupt as the last before stream off */
+	spin_lock_irqsave(&priv->irqlock, flags);
+	priv->last_eof = true;
+	spin_unlock_irqrestore(&priv->irqlock, flags);
+
+	/*
+	 * and then wait for interrupt handler to mark completion.
+	 */
+	ret = wait_for_completion_timeout(
+		&priv->last_eof_comp, msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+	if (ret == 0)
+		v4l2_warn(&priv->sd, "wait last EOF timeout\n");
+
+	devm_free_irq(priv->dev, priv->eof_irq, priv);
+	devm_free_irq(priv->dev, priv->nfb4eof_irq, priv);
+
+	csi_idmac_unsetup(priv, VB2_BUF_STATE_ERROR);
+
+	imx_media_free_dma_buf(priv->md, &priv->underrun_buf);
+
+	/* cancel the EOF timeout timer */
+	del_timer_sync(&priv->eof_timeout_timer);
+
+	csi_idmac_put_ipu_resources(priv);
+}
+
+/* Update the CSI whole sensor and active windows */
+static int csi_setup(struct csi_priv *priv)
+{
+	struct v4l2_mbus_framefmt *infmt, *outfmt;
+	struct v4l2_mbus_config sensor_mbus_cfg;
+	struct v4l2_fwnode_endpoint *sensor_ep;
+	struct v4l2_mbus_framefmt if_fmt;
+
+	infmt = &priv->format_mbus[CSI_SINK_PAD];
+	outfmt = &priv->format_mbus[priv->active_output_pad];
+	sensor_ep = &priv->sensor->sensor_ep;
+
+	/* compose mbus_config from sensor endpoint */
+	sensor_mbus_cfg.type = sensor_ep->bus_type;
+	sensor_mbus_cfg.flags = (sensor_ep->bus_type == V4L2_MBUS_CSI2) ?
+		sensor_ep->bus.mipi_csi2.flags :
+		sensor_ep->bus.parallel.flags;
+
+	/*
+	 * we need to pass input sensor frame to CSI interface, but
+	 * with translated field type from output format
+	 */
+	if_fmt = *infmt;
+	if_fmt.field = outfmt->field;
+
+	ipu_csi_set_window(priv->csi, &priv->crop);
+
+	ipu_csi_set_downsize(priv->csi,
+			     priv->crop.width == 2 * priv->compose.width,
+			     priv->crop.height == 2 * priv->compose.height);
+
+	ipu_csi_init_interface(priv->csi, &sensor_mbus_cfg, &if_fmt);
+
+	ipu_csi_set_dest(priv->csi, priv->dest);
+
+	if (priv->dest == IPU_CSI_DEST_IDMAC)
+		ipu_csi_set_skip_smfc(priv->csi, priv->skip->skip_smfc,
+				      priv->skip->max_ratio - 1, 0);
+
+	ipu_csi_dump(priv->csi);
+
+	return 0;
+}
+
+static int csi_start(struct csi_priv *priv)
+{
+	struct v4l2_fract *output_fi, *input_fi;
+	u32 bad_frames = 0;
+	int ret;
+
+	if (!priv->sensor) {
+		v4l2_err(&priv->sd, "no sensor attached\n");
+		return -EINVAL;
+	}
+
+	output_fi = &priv->frame_interval[priv->active_output_pad];
+	input_fi = &priv->frame_interval[CSI_SINK_PAD];
+
+	ret = v4l2_subdev_call(priv->sensor->sd, sensor,
+			       g_skip_frames, &bad_frames);
+	if (!ret && bad_frames) {
+		u32 delay_usec;
+
+		/*
+		 * This sensor has bad frames when it is turned on,
+		 * add a delay to avoid them before enabling the CSI
+		 * hardware. Especially for sensors with a bt.656 interface,
+		 * any shifts in the SAV/EAV sync codes will cause the CSI
+		 * to lose vert/horiz sync.
+		 */
+		delay_usec = DIV_ROUND_UP_ULL(
+			(u64)USEC_PER_SEC * input_fi->numerator * bad_frames,
+			input_fi->denominator);
+		usleep_range(delay_usec, delay_usec + 1000);
+	}
+
+	if (priv->dest == IPU_CSI_DEST_IDMAC) {
+		ret = csi_idmac_start(priv);
+		if (ret)
+			return ret;
+	}
+
+	ret = csi_setup(priv);
+	if (ret)
+		goto idmac_stop;
+
+	/* start the frame interval monitor */
+	if (priv->fim && priv->dest == IPU_CSI_DEST_IDMAC) {
+		ret = imx_media_fim_set_stream(priv->fim, output_fi, true);
+		if (ret)
+			goto idmac_stop;
+	}
+
+	ret = ipu_csi_enable(priv->csi);
+	if (ret) {
+		v4l2_err(&priv->sd, "CSI enable error: %d\n", ret);
+		goto fim_off;
+	}
+
+	return 0;
+
+fim_off:
+	if (priv->fim && priv->dest == IPU_CSI_DEST_IDMAC)
+		imx_media_fim_set_stream(priv->fim, NULL, false);
+idmac_stop:
+	if (priv->dest == IPU_CSI_DEST_IDMAC)
+		csi_idmac_stop(priv);
+	return ret;
+}
+
+static void csi_stop(struct csi_priv *priv)
+{
+	if (priv->dest == IPU_CSI_DEST_IDMAC) {
+		csi_idmac_stop(priv);
+
+		/* stop the frame interval monitor */
+		if (priv->fim)
+			imx_media_fim_set_stream(priv->fim, NULL, false);
+	}
+
+	ipu_csi_disable(priv->csi);
+}
+
+static const struct csi_skip_desc csi_skip[12] = {
+	{ 1, 1, 0x00 }, /* Keep all frames */
+	{ 5, 6, 0x10 }, /* Skip every sixth frame */
+	{ 4, 5, 0x08 }, /* Skip every fifth frame */
+	{ 3, 4, 0x04 }, /* Skip every fourth frame */
+	{ 2, 3, 0x02 }, /* Skip every third frame */
+	{ 3, 5, 0x0a }, /* Skip frames 1 and 3 of every 5 */
+	{ 1, 2, 0x01 }, /* Skip every second frame */
+	{ 2, 5, 0x0b }, /* Keep frames 1 and 4 of every 5 */
+	{ 1, 3, 0x03 }, /* Keep one in three frames */
+	{ 1, 4, 0x07 }, /* Keep one in four frames */
+	{ 1, 5, 0x0f }, /* Keep one in five frames */
+	{ 1, 6, 0x1f }, /* Keep one in six frames */
+};
+
+static void csi_apply_skip_interval(const struct csi_skip_desc *skip,
+				    struct v4l2_fract *interval)
+{
+	unsigned int div;
+
+	interval->numerator *= skip->max_ratio;
+	interval->denominator *= skip->keep;
+
+	/* Reduce fraction to lowest terms */
+	div = gcd(interval->numerator, interval->denominator);
+	if (div > 1) {
+		interval->numerator /= div;
+		interval->denominator /= div;
+	}
+}
+
+/*
+ * Find the skip pattern to produce the output frame interval closest to the
+ * requested one, for the given input frame interval. Updates the output frame
+ * interval to the exact value.
+ */
+static const struct csi_skip_desc *csi_find_best_skip(struct v4l2_fract *in,
+						      struct v4l2_fract *out)
+{
+	const struct csi_skip_desc *skip = &csi_skip[0], *best_skip = skip;
+	u32 min_err = UINT_MAX;
+	u64 want_us;
+	int i;
+
+	/* Default to 1:1 ratio */
+	if (out->numerator == 0 || out->denominator == 0 ||
+	    in->numerator == 0 || in->denominator == 0) {
+		*out = *in;
+		return best_skip;
+	}
+
+	want_us = div_u64((u64)USEC_PER_SEC * out->numerator, out->denominator);
+
+	/* Find the reduction closest to the requested time per frame */
+	for (i = 0; i < ARRAY_SIZE(csi_skip); i++, skip++) {
+		u64 tmp, err;
+
+		tmp = div_u64((u64)USEC_PER_SEC * in->numerator *
+			      skip->max_ratio, in->denominator * skip->keep);
+
+		err = abs((s64)tmp - want_us);
+		if (err < min_err) {
+			min_err = err;
+			best_skip = skip;
+		}
+	}
+
+	*out = *in;
+	csi_apply_skip_interval(best_skip, out);
+
+	return best_skip;
+}
+
+/*
+ * V4L2 subdev operations.
+ */
+
+static int csi_g_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+
+	if (fi->pad >= CSI_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	fi->interval = priv->frame_interval[fi->pad];
+
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+static int csi_s_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_fract *input_fi;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	input_fi = &priv->frame_interval[CSI_SINK_PAD];
+
+	switch (fi->pad) {
+	case CSI_SINK_PAD:
+		/* No limits on input frame interval */
+		/* Reset output intervals and frame skipping ratio to 1:1 */
+		priv->frame_interval[CSI_SRC_PAD_IDMAC] = fi->interval;
+		priv->frame_interval[CSI_SRC_PAD_DIRECT] = fi->interval;
+		priv->skip = &csi_skip[0];
+		break;
+	case CSI_SRC_PAD_IDMAC:
+		/*
+		 * frame interval at IDMAC output pad depends on input
+		 * interval, modified by frame skipping.
+		 */
+		priv->skip = csi_find_best_skip(input_fi, &fi->interval);
+		break;
+	case CSI_SRC_PAD_DIRECT:
+		/*
+		 * frame interval at DIRECT output pad is same as input
+		 * interval.
+		 */
+		fi->interval = *input_fi;
+		break;
+	default:
+		ret = -EINVAL;
+		goto out;
+	}
+
+	priv->frame_interval[fi->pad] = fi->interval;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	if (!priv->src_sd || !priv->sink) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	/*
+	 * enable/disable streaming only if stream_count is
+	 * going from 0 to 1 / 1 to 0.
+	 */
+	if (priv->stream_count != !enable)
+		goto update_count;
+
+	if (enable) {
+		/* upstream must be started first, before starting CSI */
+		ret = v4l2_subdev_call(priv->src_sd, video, s_stream, 1);
+		ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
+		if (ret)
+			goto out;
+
+		dev_dbg(priv->dev, "stream ON\n");
+		ret = csi_start(priv);
+		if (ret) {
+			v4l2_subdev_call(priv->src_sd, video, s_stream, 0);
+			goto out;
+		}
+	} else {
+		dev_dbg(priv->dev, "stream OFF\n");
+		/* CSI must be stopped first, then stop upstream */
+		csi_stop(priv);
+		v4l2_subdev_call(priv->src_sd, video, s_stream, 0);
+	}
+
+update_count:
+	priv->stream_count += enable ? 1 : -1;
+	if (priv->stream_count < 0)
+		priv->stream_count = 0;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_link_setup(struct media_entity *entity,
+			  const struct media_pad *local,
+			  const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev *remote_sd;
+	int ret = 0;
+
+	dev_dbg(priv->dev, "link setup %s -> %s\n", remote->entity->name,
+		local->entity->name);
+
+	mutex_lock(&priv->lock);
+
+	if (local->flags & MEDIA_PAD_FL_SINK) {
+		if (!is_media_entity_v4l2_subdev(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (priv->src_sd) {
+				ret = -EBUSY;
+				goto out;
+			}
+			priv->src_sd = remote_sd;
+		} else {
+			priv->src_sd = NULL;
+		}
+
+		goto out;
+	}
+
+	/* this is a source pad */
+
+	if (flags & MEDIA_LNK_FL_ENABLED) {
+		if (priv->sink) {
+			ret = -EBUSY;
+			goto out;
+		}
+	} else {
+		v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+		v4l2_ctrl_handler_init(&priv->ctrl_hdlr, 0);
+		priv->sink = NULL;
+		goto out;
+	}
+
+	/* record which output pad is now active */
+	priv->active_output_pad = local->index;
+
+	/* set CSI destination */
+	if (local->index == CSI_SRC_PAD_IDMAC) {
+		if (!is_media_entity_v4l2_video_device(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		if (priv->fim) {
+			ret = imx_media_fim_add_controls(priv->fim);
+			if (ret)
+				goto out;
+		}
+
+		priv->dest = IPU_CSI_DEST_IDMAC;
+	} else {
+		if (!is_media_entity_v4l2_subdev(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+		switch (remote_sd->grp_id) {
+		case IMX_MEDIA_GRP_ID_VDIC:
+			priv->dest = IPU_CSI_DEST_VDIC;
+			break;
+		case IMX_MEDIA_GRP_ID_IC_PRP:
+			priv->dest = IPU_CSI_DEST_IC;
+			break;
+		default:
+			ret = -EINVAL;
+			goto out;
+		}
+	}
+
+	priv->sink = remote->entity;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_link_validate(struct v4l2_subdev *sd,
+			     struct media_link *link,
+			     struct v4l2_subdev_format *source_fmt,
+			     struct v4l2_subdev_format *sink_fmt)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_fwnode_endpoint *sensor_ep;
+	const struct imx_media_pixfmt *incc;
+	struct imx_media_subdev *sensor;
+	bool is_csi2;
+	int ret;
+
+	ret = v4l2_subdev_link_validate_default(sd, link,
+						source_fmt, sink_fmt);
+	if (ret)
+		return ret;
+
+	sensor = __imx_media_find_sensor(priv->md, &priv->sd.entity);
+	if (IS_ERR(sensor)) {
+		v4l2_err(&priv->sd, "no sensor attached\n");
+		return PTR_ERR(priv->sensor);
+	}
+
+	mutex_lock(&priv->lock);
+
+	priv->sensor = sensor;
+	sensor_ep = &priv->sensor->sensor_ep;
+	is_csi2 = (sensor_ep->bus_type == V4L2_MBUS_CSI2);
+	incc = priv->cc[CSI_SINK_PAD];
+
+	if (priv->dest != IPU_CSI_DEST_IDMAC &&
+	    (incc->bayer || (!is_csi2 &&
+			     sensor_ep->bus.parallel.bus_width >= 16))) {
+		v4l2_err(&priv->sd,
+			 "bayer/16-bit parallel buses must go to IDMAC pad\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (is_csi2) {
+		int vc_num = 0;
+		/*
+		 * NOTE! It seems the virtual channels from the mipi csi-2
+		 * receiver are used only for routing by the video mux's,
+		 * or for hard-wired routing to the CSI's. Once the stream
+		 * enters the CSI's however, they are treated internally
+		 * in the IPU as virtual channel 0.
+		 */
+#if 0
+		mutex_unlock(&priv->lock);
+		vc_num = imx_media_find_mipi_csi2_channel(priv->md,
+							  &priv->sd.entity);
+		if (vc_num < 0)
+			return vc_num;
+		mutex_lock(&priv->lock);
+#endif
+		ipu_csi_set_mipi_datatype(priv->csi, vc_num,
+					  &priv->format_mbus[CSI_SINK_PAD]);
+	}
+
+	/* select either parallel or MIPI-CSI2 as input to CSI */
+	ipu_set_csi_src_mux(priv->ipu, priv->csi_id, is_csi2);
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+__csi_get_fmt(struct csi_priv *priv, struct v4l2_subdev_pad_config *cfg,
+	      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&priv->sd, cfg, pad);
+	else
+		return &priv->format_mbus[pad];
+}
+
+static struct v4l2_rect *
+__csi_get_crop(struct csi_priv *priv, struct v4l2_subdev_pad_config *cfg,
+	       enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_crop(&priv->sd, cfg, CSI_SINK_PAD);
+	else
+		return &priv->crop;
+}
+
+static struct v4l2_rect *
+__csi_get_compose(struct csi_priv *priv, struct v4l2_subdev_pad_config *cfg,
+		  enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_compose(&priv->sd, cfg,
+						   CSI_SINK_PAD);
+	else
+		return &priv->compose;
+}
+
+static void csi_try_crop(struct csi_priv *priv,
+			 struct v4l2_rect *crop,
+			 struct v4l2_subdev_pad_config *cfg,
+			 struct v4l2_mbus_framefmt *infmt,
+			 struct imx_media_subdev *sensor)
+{
+	struct v4l2_fwnode_endpoint *sensor_ep;
+
+	sensor_ep = &sensor->sensor_ep;
+
+	crop->width = min_t(__u32, infmt->width, crop->width);
+	if (crop->left + crop->width > infmt->width)
+		crop->left = infmt->width - crop->width;
+	/* adjust crop left/width to h/w alignment restrictions */
+	crop->left &= ~0x3;
+	crop->width &= ~0x7;
+
+	/*
+	 * FIXME: not sure why yet, but on interlaced bt.656,
+	 * changing the vertical cropping causes loss of vertical
+	 * sync, so fix it to NTSC/PAL active lines. NTSC contains
+	 * 2 extra lines of active video that need to be cropped.
+	 */
+	if (sensor_ep->bus_type == V4L2_MBUS_BT656 &&
+	    (V4L2_FIELD_HAS_BOTH(infmt->field) ||
+	     infmt->field == V4L2_FIELD_ALTERNATE)) {
+		crop->height = infmt->height;
+		crop->top = (infmt->height == 480) ? 2 : 0;
+	} else {
+		crop->height = min_t(__u32, infmt->height, crop->height);
+		if (crop->top + crop->height > infmt->height)
+			crop->top = infmt->height - crop->height;
+	}
+}
+
+static int csi_enum_mbus_code(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	const struct imx_media_pixfmt *incc;
+	struct v4l2_mbus_framefmt *infmt;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	infmt = __csi_get_fmt(priv, cfg, CSI_SINK_PAD, code->which);
+	incc = imx_media_find_mbus_format(infmt->code, CS_SEL_ANY, true);
+
+	switch (code->pad) {
+	case CSI_SINK_PAD:
+		ret = imx_media_enum_mbus_format(&code->code, code->index,
+						 CS_SEL_ANY, true);
+		break;
+	case CSI_SRC_PAD_DIRECT:
+	case CSI_SRC_PAD_IDMAC:
+		if (incc->bayer) {
+			if (code->index != 0) {
+				ret = -EINVAL;
+				goto out;
+			}
+			code->code = infmt->code;
+		} else {
+			u32 cs_sel = (incc->cs == IPUV3_COLORSPACE_YUV) ?
+				CS_SEL_YUV : CS_SEL_RGB;
+			ret = imx_media_enum_ipu_format(&code->code,
+							code->index,
+							cs_sel);
+		}
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_enum_frame_size(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_rect *crop;
+	int ret = 0;
+
+	if (fse->pad >= CSI_NUM_PADS ||
+	    fse->index > (fse->pad == CSI_SINK_PAD ? 0 : 3))
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	if (fse->pad == CSI_SINK_PAD) {
+		fse->min_width = MIN_W;
+		fse->max_width = MAX_W;
+		fse->min_height = MIN_H;
+		fse->max_height = MAX_H;
+	} else {
+		crop = __csi_get_crop(priv, cfg, fse->which);
+
+		fse->min_width = fse->max_width = fse->index & 1 ?
+			crop->width / 2 : crop->width;
+		fse->min_height = fse->max_height = fse->index & 2 ?
+			crop->height / 2 : crop->height;
+	}
+
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_enum_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_pad_config *cfg,
+				   struct v4l2_subdev_frame_interval_enum *fie)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_fract *input_fi;
+	struct v4l2_rect *crop;
+	int ret = 0;
+
+	if (fie->pad >= CSI_NUM_PADS ||
+	    fie->index >= (fie->pad != CSI_SRC_PAD_IDMAC ?
+			   1 : ARRAY_SIZE(csi_skip)))
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	input_fi = &priv->frame_interval[CSI_SINK_PAD];
+	crop = __csi_get_crop(priv, cfg, fie->which);
+
+	if ((fie->width != crop->width && fie->width != crop->width / 2) ||
+	    (fie->height != crop->height && fie->height != crop->height / 2)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	fie->interval = *input_fi;
+
+	if (fie->pad == CSI_SRC_PAD_IDMAC)
+		csi_apply_skip_interval(&csi_skip[fie->index],
+					&fie->interval);
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_get_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= CSI_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	fmt = __csi_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
+	if (!fmt) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	sdformat->format = *fmt;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static void csi_try_fmt(struct csi_priv *priv,
+			struct imx_media_subdev *sensor,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *sdformat,
+			struct v4l2_rect *crop,
+			struct v4l2_rect *compose,
+			const struct imx_media_pixfmt **cc)
+{
+	const struct imx_media_pixfmt *incc;
+	struct v4l2_mbus_framefmt *infmt;
+	u32 code;
+
+	infmt = __csi_get_fmt(priv, cfg, CSI_SINK_PAD, sdformat->which);
+
+	switch (sdformat->pad) {
+	case CSI_SRC_PAD_DIRECT:
+	case CSI_SRC_PAD_IDMAC:
+		incc = imx_media_find_mbus_format(infmt->code,
+						  CS_SEL_ANY, true);
+
+		sdformat->format.width = compose->width;
+		sdformat->format.height = compose->height;
+
+		if (incc->bayer) {
+			sdformat->format.code = infmt->code;
+			*cc = incc;
+		} else {
+			u32 cs_sel = (incc->cs == IPUV3_COLORSPACE_YUV) ?
+				CS_SEL_YUV : CS_SEL_RGB;
+
+			*cc = imx_media_find_ipu_format(sdformat->format.code,
+							cs_sel);
+			if (!*cc) {
+				imx_media_enum_ipu_format(&code, 0, cs_sel);
+				*cc = imx_media_find_ipu_format(code, cs_sel);
+				sdformat->format.code = (*cc)->codes[0];
+			}
+		}
+
+		if (sdformat->pad == CSI_SRC_PAD_DIRECT ||
+		    sdformat->format.field != V4L2_FIELD_NONE)
+			sdformat->format.field = infmt->field;
+
+		/*
+		 * translate V4L2_FIELD_ALTERNATE to SEQ_TB or SEQ_BT
+		 * depending on input height (assume NTSC top-bottom
+		 * order if 480 lines, otherwise PAL bottom-top order).
+		 */
+		if (sdformat->format.field == V4L2_FIELD_ALTERNATE) {
+			sdformat->format.field =  (infmt->height == 480) ?
+				V4L2_FIELD_SEQ_TB : V4L2_FIELD_SEQ_BT;
+		}
+
+		/* propagate colorimetry from sink */
+		sdformat->format.colorspace = infmt->colorspace;
+		sdformat->format.xfer_func = infmt->xfer_func;
+		sdformat->format.quantization = infmt->quantization;
+		sdformat->format.ycbcr_enc = infmt->ycbcr_enc;
+		break;
+	case CSI_SINK_PAD:
+		v4l_bound_align_image(&sdformat->format.width, MIN_W, MAX_W,
+				      W_ALIGN, &sdformat->format.height,
+				      MIN_H, MAX_H, H_ALIGN, S_ALIGN);
+
+		/* Reset crop and compose rectangles */
+		crop->left = 0;
+		crop->top = 0;
+		crop->width = sdformat->format.width;
+		crop->height = sdformat->format.height;
+		csi_try_crop(priv, crop, cfg, &sdformat->format, sensor);
+		compose->left = 0;
+		compose->top = 0;
+		compose->width = crop->width;
+		compose->height = crop->height;
+
+		*cc = imx_media_find_mbus_format(sdformat->format.code,
+						 CS_SEL_ANY, true);
+		if (!*cc) {
+			imx_media_enum_mbus_format(&code, 0,
+						   CS_SEL_ANY, false);
+			*cc = imx_media_find_mbus_format(code,
+							CS_SEL_ANY, false);
+			sdformat->format.code = (*cc)->codes[0];
+		}
+
+		imx_media_fill_default_mbus_fields(
+			&sdformat->format, infmt,
+			priv->active_output_pad == CSI_SRC_PAD_DIRECT);
+		break;
+	}
+}
+
+static int csi_set_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_format *sdformat)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct imx_media_video_dev *vdev = priv->vdev;
+	const struct imx_media_pixfmt *cc;
+	struct imx_media_subdev *sensor;
+	struct v4l2_pix_format vdev_fmt;
+	struct v4l2_mbus_framefmt *fmt;
+	struct v4l2_rect *crop, *compose;
+	int ret = 0;
+
+	if (sdformat->pad >= CSI_NUM_PADS)
+		return -EINVAL;
+
+	sensor = imx_media_find_sensor(priv->md, &priv->sd.entity);
+	if (IS_ERR(sensor)) {
+		v4l2_err(&priv->sd, "no sensor attached\n");
+		return PTR_ERR(sensor);
+	}
+
+	mutex_lock(&priv->lock);
+
+	if (priv->stream_count > 0) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	crop = __csi_get_crop(priv, cfg, sdformat->which);
+	compose = __csi_get_compose(priv, cfg, sdformat->which);
+
+	csi_try_fmt(priv, sensor, cfg, sdformat, crop, compose, &cc);
+
+	fmt = __csi_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
+	*fmt = sdformat->format;
+
+	if (sdformat->pad == CSI_SINK_PAD) {
+		int pad;
+
+		/* propagate format to source pads */
+		for (pad = CSI_SINK_PAD + 1; pad < CSI_NUM_PADS; pad++) {
+			const struct imx_media_pixfmt *outcc;
+			struct v4l2_mbus_framefmt *outfmt;
+			struct v4l2_subdev_format format;
+
+			format.pad = pad;
+			format.which = sdformat->which;
+			format.format = sdformat->format;
+			csi_try_fmt(priv, sensor, cfg, &format, NULL, compose,
+				    &outcc);
+
+			outfmt = __csi_get_fmt(priv, cfg, pad, sdformat->which);
+			*outfmt = format.format;
+
+			if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+				priv->cc[pad] = outcc;
+		}
+	}
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY)
+		goto out;
+
+	priv->cc[sdformat->pad] = cc;
+
+	/* propagate IDMAC output pad format to capture device */
+	imx_media_mbus_fmt_to_pix_fmt(&vdev_fmt,
+				      &priv->format_mbus[CSI_SRC_PAD_IDMAC],
+				      priv->cc[CSI_SRC_PAD_IDMAC]);
+	mutex_unlock(&priv->lock);
+	imx_media_capture_device_set_format(vdev, &vdev_fmt);
+
+	return 0;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_get_selection(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_pad_config *cfg,
+			     struct v4l2_subdev_selection *sel)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *infmt;
+	struct v4l2_rect *crop, *compose;
+	int ret = 0;
+
+	if (sel->pad != CSI_SINK_PAD)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	infmt = __csi_get_fmt(priv, cfg, CSI_SINK_PAD, sel->which);
+	crop = __csi_get_crop(priv, cfg, sel->which);
+	compose = __csi_get_compose(priv, cfg, sel->which);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = infmt->width;
+		sel->r.height = infmt->height;
+		break;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *crop;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = crop->width;
+		sel->r.height = crop->height;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		sel->r = *compose;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_set_scale(u32 *compose, u32 crop, u32 flags)
+{
+	if ((flags & (V4L2_SEL_FLAG_LE | V4L2_SEL_FLAG_GE)) ==
+		     (V4L2_SEL_FLAG_LE | V4L2_SEL_FLAG_GE) &&
+	    *compose != crop && *compose != crop / 2)
+		return -ERANGE;
+
+	if (*compose <= crop / 2 ||
+	    (*compose < crop * 3 / 4 && !(flags & V4L2_SEL_FLAG_GE)) ||
+	    (*compose < crop && (flags & V4L2_SEL_FLAG_LE)))
+		*compose = crop / 2;
+	else
+		*compose = crop;
+
+	return 0;
+}
+
+static int csi_set_selection(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_pad_config *cfg,
+			     struct v4l2_subdev_selection *sel)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *infmt;
+	struct v4l2_rect *crop, *compose;
+	struct imx_media_subdev *sensor;
+	int pad, ret = 0;
+
+	if (sel->pad != CSI_SINK_PAD)
+		return -EINVAL;
+
+	sensor = imx_media_find_sensor(priv->md, &priv->sd.entity);
+	if (IS_ERR(sensor)) {
+		v4l2_err(&priv->sd, "no sensor attached\n");
+		return PTR_ERR(sensor);
+	}
+
+	mutex_lock(&priv->lock);
+
+	if (priv->stream_count > 0) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	infmt = __csi_get_fmt(priv, cfg, CSI_SINK_PAD, sel->which);
+	crop = __csi_get_crop(priv, cfg, sel->which);
+	compose = __csi_get_compose(priv, cfg, sel->which);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		/*
+		 * Modifying the crop rectangle always changes the format on
+		 * the source pads. If the KEEP_CONFIG flag is set, just return
+		 * the current crop rectangle.
+		 */
+		if (sel->flags & V4L2_SEL_FLAG_KEEP_CONFIG) {
+			sel->r = priv->crop;
+			if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
+				*crop = sel->r;
+			goto out;
+		}
+
+		csi_try_crop(priv, &sel->r, cfg, infmt, sensor);
+
+		*crop = sel->r;
+
+		/* Reset scaling to 1:1 */
+		compose->width = crop->width;
+		compose->height = crop->height;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		/*
+		 * Modifying the compose rectangle always changes the format on
+		 * the source pads. If the KEEP_CONFIG flag is set, just return
+		 * the current compose rectangle.
+		 */
+		if (sel->flags & V4L2_SEL_FLAG_KEEP_CONFIG) {
+			sel->r = priv->compose;
+			if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
+				*compose = sel->r;
+			goto out;
+		}
+
+		sel->r.left = 0;
+		sel->r.top = 0;
+		ret = csi_set_scale(&sel->r.width, crop->width, sel->flags);
+		if (ret)
+			goto out;
+		ret = csi_set_scale(&sel->r.height, crop->height, sel->flags);
+		if (ret)
+			goto out;
+
+		*compose = sel->r;
+		break;
+	default:
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* Reset source pads to sink compose rectangle */
+	for (pad = CSI_SINK_PAD + 1; pad < CSI_NUM_PADS; pad++) {
+		struct v4l2_mbus_framefmt *outfmt;
+
+		outfmt = __csi_get_fmt(priv, cfg, pad, sel->which);
+		outfmt->width = compose->width;
+		outfmt->height = compose->height;
+	}
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int csi_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+			       struct v4l2_event_subscription *sub)
+{
+	if (sub->type != V4L2_EVENT_IMX_FRAME_INTERVAL_ERROR)
+		return -EINVAL;
+	if (sub->id != 0)
+		return -EINVAL;
+
+	return v4l2_event_subscribe(fh, sub, 0, NULL);
+}
+
+static int csi_unsubscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+				 struct v4l2_event_subscription *sub)
+{
+	return v4l2_event_unsubscribe(fh, sub);
+}
+
+/*
+ * retrieve our pads parsed from the OF graph by the media device
+ */
+static int csi_registered(struct v4l2_subdev *sd)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+	int i, ret;
+	u32 code;
+
+	/* get media device */
+	priv->md = dev_get_drvdata(sd->v4l2_dev->dev);
+
+	/* get handle to IPU CSI */
+	priv->csi = ipu_csi_get(priv->ipu, priv->csi_id);
+	if (IS_ERR(priv->csi)) {
+		v4l2_err(&priv->sd, "failed to get CSI%d\n", priv->csi_id);
+		return PTR_ERR(priv->csi);
+	}
+
+	for (i = 0; i < CSI_NUM_PADS; i++) {
+		priv->pad[i].flags = (i == CSI_SINK_PAD) ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+
+		code = 0;
+		if (i != CSI_SINK_PAD)
+			imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
+
+		/* set a default mbus format  */
+		ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
+					      640, 480, code, V4L2_FIELD_NONE,
+					      &priv->cc[i]);
+		if (ret)
+			goto put_csi;
+
+		/* init default frame interval */
+		priv->frame_interval[i].numerator = 1;
+		priv->frame_interval[i].denominator = 30;
+	}
+
+	/* disable frame skipping */
+	priv->skip = &csi_skip[0];
+
+	/* init default crop and compose rectangle sizes */
+	priv->crop.width = 640;
+	priv->crop.height = 480;
+	priv->compose.width = 640;
+	priv->compose.height = 480;
+
+	priv->fim = imx_media_fim_init(&priv->sd);
+	if (IS_ERR(priv->fim)) {
+		ret = PTR_ERR(priv->fim);
+		goto put_csi;
+	}
+
+	ret = media_entity_pads_init(&sd->entity, CSI_NUM_PADS, priv->pad);
+	if (ret)
+		goto free_fim;
+
+	ret = imx_media_capture_device_register(priv->vdev);
+	if (ret)
+		goto free_fim;
+
+	ret = imx_media_add_video_device(priv->md, priv->vdev);
+	if (ret)
+		goto unreg;
+
+	return 0;
+unreg:
+	imx_media_capture_device_unregister(priv->vdev);
+free_fim:
+	if (priv->fim)
+		imx_media_fim_free(priv->fim);
+put_csi:
+	ipu_csi_put(priv->csi);
+	return ret;
+}
+
+static void csi_unregistered(struct v4l2_subdev *sd)
+{
+	struct csi_priv *priv = v4l2_get_subdevdata(sd);
+
+	imx_media_capture_device_unregister(priv->vdev);
+
+	if (priv->fim)
+		imx_media_fim_free(priv->fim);
+
+	if (!IS_ERR_OR_NULL(priv->csi))
+		ipu_csi_put(priv->csi);
+}
+
+static const struct media_entity_operations csi_entity_ops = {
+	.link_setup = csi_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_core_ops csi_core_ops = {
+	.subscribe_event = csi_subscribe_event,
+	.unsubscribe_event = csi_unsubscribe_event,
+};
+
+static const struct v4l2_subdev_video_ops csi_video_ops = {
+	.g_frame_interval = csi_g_frame_interval,
+	.s_frame_interval = csi_s_frame_interval,
+	.s_stream = csi_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops csi_pad_ops = {
+	.enum_mbus_code = csi_enum_mbus_code,
+	.enum_frame_size = csi_enum_frame_size,
+	.enum_frame_interval = csi_enum_frame_interval,
+	.get_fmt = csi_get_fmt,
+	.set_fmt = csi_set_fmt,
+	.get_selection = csi_get_selection,
+	.set_selection = csi_set_selection,
+	.link_validate = csi_link_validate,
+};
+
+static const struct v4l2_subdev_ops csi_subdev_ops = {
+	.core = &csi_core_ops,
+	.video = &csi_video_ops,
+	.pad = &csi_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops csi_internal_ops = {
+	.registered = csi_registered,
+	.unregistered = csi_unregistered,
+};
+
+static int imx_csi_probe(struct platform_device *pdev)
+{
+	struct ipu_client_platformdata *pdata;
+	struct pinctrl *pinctrl;
+	struct csi_priv *priv;
+	int ret;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, &priv->sd);
+	priv->dev = &pdev->dev;
+
+	ret = dma_set_coherent_mask(priv->dev, DMA_BIT_MASK(32));
+	if (ret)
+		return ret;
+
+	/* get parent IPU */
+	priv->ipu = dev_get_drvdata(priv->dev->parent);
+
+	/* get our CSI id */
+	pdata = priv->dev->platform_data;
+	priv->csi_id = pdata->csi;
+	priv->smfc_id = (priv->csi_id == 0) ? 0 : 2;
+
+	init_timer(&priv->eof_timeout_timer);
+	priv->eof_timeout_timer.data = (unsigned long)priv;
+	priv->eof_timeout_timer.function = csi_idmac_eof_timeout;
+	spin_lock_init(&priv->irqlock);
+
+	v4l2_subdev_init(&priv->sd, &csi_subdev_ops);
+	v4l2_set_subdevdata(&priv->sd, priv);
+	priv->sd.internal_ops = &csi_internal_ops;
+	priv->sd.entity.ops = &csi_entity_ops;
+	priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+	priv->sd.dev = &pdev->dev;
+	priv->sd.fwnode = of_fwnode_handle(pdata->of_node);
+	priv->sd.owner = THIS_MODULE;
+	priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	priv->sd.grp_id = priv->csi_id ?
+		IMX_MEDIA_GRP_ID_CSI1 : IMX_MEDIA_GRP_ID_CSI0;
+	imx_media_grp_id_to_sd_name(priv->sd.name, sizeof(priv->sd.name),
+				    priv->sd.grp_id, ipu_get_num(priv->ipu));
+
+	priv->vdev = imx_media_capture_device_init(&priv->sd,
+						   CSI_SRC_PAD_IDMAC);
+	if (IS_ERR(priv->vdev))
+		return PTR_ERR(priv->vdev);
+
+	mutex_init(&priv->lock);
+
+	v4l2_ctrl_handler_init(&priv->ctrl_hdlr, 0);
+	priv->sd.ctrl_handler = &priv->ctrl_hdlr;
+
+	/*
+	 * The IPUv3 driver did not assign an of_node to this
+	 * device. As a result, pinctrl does not automatically
+	 * configure our pin groups, so we need to do that manually
+	 * here, after setting this device's of_node.
+	 */
+	priv->dev->of_node = pdata->of_node;
+	pinctrl = devm_pinctrl_get_select_default(priv->dev);
+
+	ret = v4l2_async_register_subdev(&priv->sd);
+	if (ret)
+		goto free;
+
+	return 0;
+free:
+	v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+	mutex_destroy(&priv->lock);
+	imx_media_capture_device_remove(priv->vdev);
+	return ret;
+}
+
+static int imx_csi_remove(struct platform_device *pdev)
+{
+	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
+	struct csi_priv *priv = sd_to_dev(sd);
+
+	v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+	mutex_destroy(&priv->lock);
+	imx_media_capture_device_remove(priv->vdev);
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+
+	return 0;
+}
+
+static const struct platform_device_id imx_csi_ids[] = {
+	{ .name = "imx-ipuv3-csi" },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, imx_csi_ids);
+
+static struct platform_driver imx_csi_driver = {
+	.probe = imx_csi_probe,
+	.remove = imx_csi_remove,
+	.id_table = imx_csi_ids,
+	.driver = {
+		.name = "imx-ipuv3-csi",
+	},
+};
+module_platform_driver(imx_csi_driver);
+
+MODULE_DESCRIPTION("i.MX CSI subdev driver");
+MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:imx-ipuv3-csi");
diff --git a/drivers/staging/media/imx/imx-media-dev.c b/drivers/staging/media/imx/imx-media-dev.c
new file mode 100644
index 000000000000..48cbc7716758
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-dev.c
@@ -0,0 +1,667 @@
+/*
+ * V4L2 Media Controller Driver for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2016 Mentor Graphics Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/timer.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include <video/imx-ipu-v3.h>
+#include <media/imx.h>
+#include "imx-media.h"
+
+static inline struct imx_media_dev *notifier2dev(struct v4l2_async_notifier *n)
+{
+	return container_of(n, struct imx_media_dev, subdev_notifier);
+}
+
+/*
+ * Find a subdev by device node or device name. This is called during
+ * driver load to form the async subdev list and bind them.
+ */
+struct imx_media_subdev *
+imx_media_find_async_subdev(struct imx_media_dev *imxmd,
+			    struct device_node *np,
+			    const char *devname)
+{
+	struct fwnode_handle *fwnode = np ? of_fwnode_handle(np) : NULL;
+	struct imx_media_subdev *imxsd;
+	int i;
+
+	for (i = 0; i < imxmd->subdev_notifier.num_subdevs; i++) {
+		imxsd = &imxmd->subdev[i];
+		switch (imxsd->asd.match_type) {
+		case V4L2_ASYNC_MATCH_FWNODE:
+			if (fwnode && imxsd->asd.match.fwnode.fwnode == fwnode)
+				return imxsd;
+			break;
+		case V4L2_ASYNC_MATCH_DEVNAME:
+			if (devname &&
+			    !strcmp(imxsd->asd.match.device_name.name, devname))
+				return imxsd;
+			break;
+		default:
+			break;
+		}
+	}
+
+	return NULL;
+}
+
+
+/*
+ * Adds a subdev to the async subdev list. If np is non-NULL, adds
+ * the async as a V4L2_ASYNC_MATCH_FWNODE match type, otherwise as
+ * a V4L2_ASYNC_MATCH_DEVNAME match type using the dev_name of the
+ * given platform_device. This is called during driver load when
+ * forming the async subdev list.
+ */
+struct imx_media_subdev *
+imx_media_add_async_subdev(struct imx_media_dev *imxmd,
+			   struct device_node *np,
+			   struct platform_device *pdev)
+{
+	struct imx_media_subdev *imxsd;
+	struct v4l2_async_subdev *asd;
+	const char *devname = NULL;
+	int sd_idx;
+
+	mutex_lock(&imxmd->mutex);
+
+	if (pdev)
+		devname = dev_name(&pdev->dev);
+
+	/* return NULL if this subdev already added */
+	if (imx_media_find_async_subdev(imxmd, np, devname)) {
+		dev_dbg(imxmd->md.dev, "%s: already added %s\n",
+			__func__, np ? np->name : devname);
+		imxsd = NULL;
+		goto out;
+	}
+
+	sd_idx = imxmd->subdev_notifier.num_subdevs;
+	if (sd_idx >= IMX_MEDIA_MAX_SUBDEVS) {
+		dev_err(imxmd->md.dev, "%s: too many subdevs! can't add %s\n",
+			__func__, np ? np->name : devname);
+		imxsd = ERR_PTR(-ENOSPC);
+		goto out;
+	}
+
+	imxsd = &imxmd->subdev[sd_idx];
+
+	asd = &imxsd->asd;
+	if (np) {
+		asd->match_type = V4L2_ASYNC_MATCH_FWNODE;
+		asd->match.fwnode.fwnode = of_fwnode_handle(np);
+	} else {
+		asd->match_type = V4L2_ASYNC_MATCH_DEVNAME;
+		strncpy(imxsd->devname, devname, sizeof(imxsd->devname));
+		asd->match.device_name.name = imxsd->devname;
+		imxsd->pdev = pdev;
+	}
+
+	imxmd->async_ptrs[sd_idx] = asd;
+	imxmd->subdev_notifier.num_subdevs++;
+
+	dev_dbg(imxmd->md.dev, "%s: added %s, match type %s\n",
+		__func__, np ? np->name : devname, np ? "FWNODE" : "DEVNAME");
+
+out:
+	mutex_unlock(&imxmd->mutex);
+	return imxsd;
+}
+
+/*
+ * Adds an imx-media link to a subdev pad's link list. This is called
+ * during driver load when forming the links between subdevs.
+ *
+ * @pad: the local pad
+ * @remote_node: the device node of the remote subdev
+ * @remote_devname: the device name of the remote subdev
+ * @local_pad: local pad index
+ * @remote_pad: remote pad index
+ */
+int imx_media_add_pad_link(struct imx_media_dev *imxmd,
+			   struct imx_media_pad *pad,
+			   struct device_node *remote_node,
+			   const char *remote_devname,
+			   int local_pad, int remote_pad)
+{
+	struct imx_media_link *link;
+	int link_idx, ret = 0;
+
+	mutex_lock(&imxmd->mutex);
+
+	link_idx = pad->num_links;
+	if (link_idx >= IMX_MEDIA_MAX_LINKS) {
+		dev_err(imxmd->md.dev, "%s: too many links!\n", __func__);
+		ret = -ENOSPC;
+		goto out;
+	}
+
+	link = &pad->link[link_idx];
+
+	link->remote_sd_node = remote_node;
+	if (remote_devname)
+		strncpy(link->remote_devname, remote_devname,
+			sizeof(link->remote_devname));
+
+	link->local_pad = local_pad;
+	link->remote_pad = remote_pad;
+
+	pad->num_links++;
+out:
+	mutex_unlock(&imxmd->mutex);
+	return ret;
+}
+
+/*
+ * get IPU from this CSI and add it to the list of IPUs
+ * the media driver will control.
+ */
+static int imx_media_get_ipu(struct imx_media_dev *imxmd,
+			     struct v4l2_subdev *csi_sd)
+{
+	struct ipu_soc *ipu;
+	int ipu_id;
+
+	ipu = dev_get_drvdata(csi_sd->dev->parent);
+	if (!ipu) {
+		v4l2_err(&imxmd->v4l2_dev,
+			 "CSI %s has no parent IPU!\n", csi_sd->name);
+		return -ENODEV;
+	}
+
+	ipu_id = ipu_get_num(ipu);
+	if (ipu_id > 1) {
+		v4l2_err(&imxmd->v4l2_dev, "invalid IPU id %d!\n", ipu_id);
+		return -ENODEV;
+	}
+
+	if (!imxmd->ipu[ipu_id])
+		imxmd->ipu[ipu_id] = ipu;
+
+	return 0;
+}
+
+/* async subdev bound notifier */
+static int imx_media_subdev_bound(struct v4l2_async_notifier *notifier,
+				  struct v4l2_subdev *sd,
+				  struct v4l2_async_subdev *asd)
+{
+	struct imx_media_dev *imxmd = notifier2dev(notifier);
+	struct device_node *np = to_of_node(sd->fwnode);
+	struct imx_media_subdev *imxsd;
+	int ret = 0;
+
+	mutex_lock(&imxmd->mutex);
+
+	imxsd = imx_media_find_async_subdev(imxmd, np, dev_name(sd->dev));
+	if (!imxsd) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (sd->grp_id & IMX_MEDIA_GRP_ID_CSI) {
+		ret = imx_media_get_ipu(imxmd, sd);
+		if (ret)
+			goto out_unlock;
+	} else if (sd->entity.function == MEDIA_ENT_F_VID_MUX) {
+		/* this is a video mux */
+		sd->grp_id = IMX_MEDIA_GRP_ID_VIDMUX;
+	} else if (imxsd->num_sink_pads == 0) {
+		/*
+		 * this is an original source of video frames, it
+		 * could be a camera sensor, an analog decoder, or
+		 * a bridge device (HDMI -> MIPI CSI-2 for example).
+		 * This group ID is used to locate the entity that
+		 * is the original source of video in a pipeline.
+		 */
+		sd->grp_id = IMX_MEDIA_GRP_ID_SENSOR;
+	}
+
+	/* attach the subdev */
+	imxsd->sd = sd;
+out:
+	if (ret)
+		v4l2_warn(&imxmd->v4l2_dev,
+			  "Received unknown subdev %s\n", sd->name);
+	else
+		v4l2_info(&imxmd->v4l2_dev,
+			  "Registered subdev %s\n", sd->name);
+
+out_unlock:
+	mutex_unlock(&imxmd->mutex);
+	return ret;
+}
+
+/*
+ * Create a single source->sink media link given a subdev and a single
+ * link from one of its source pads. Called after all subdevs have
+ * registered.
+ */
+static int imx_media_create_link(struct imx_media_dev *imxmd,
+				 struct imx_media_subdev *src,
+				 struct imx_media_link *link)
+{
+	struct imx_media_subdev *sink;
+	u16 source_pad, sink_pad;
+	int ret;
+
+	sink = imx_media_find_async_subdev(imxmd, link->remote_sd_node,
+					   link->remote_devname);
+	if (!sink) {
+		v4l2_warn(&imxmd->v4l2_dev, "%s: no sink for %s:%d\n",
+			  __func__, src->sd->name, link->local_pad);
+		return 0;
+	}
+
+	source_pad = link->local_pad;
+	sink_pad = link->remote_pad;
+
+	v4l2_info(&imxmd->v4l2_dev, "%s: %s:%d -> %s:%d\n", __func__,
+		  src->sd->name, source_pad, sink->sd->name, sink_pad);
+
+	ret = media_create_pad_link(&src->sd->entity, source_pad,
+				    &sink->sd->entity, sink_pad, 0);
+	if (ret)
+		v4l2_err(&imxmd->v4l2_dev,
+			 "create_pad_link failed: %d\n", ret);
+
+	return ret;
+}
+
+/*
+ * create the media links from all imx-media pads and their links.
+ * Called after all subdevs have registered.
+ */
+static int imx_media_create_links(struct imx_media_dev *imxmd)
+{
+	struct imx_media_subdev *imxsd;
+	struct imx_media_link *link;
+	struct imx_media_pad *pad;
+	int num_pads, i, j, k;
+	int ret = 0;
+
+	for (i = 0; i < imxmd->num_subdevs; i++) {
+		imxsd = &imxmd->subdev[i];
+		num_pads = imxsd->num_sink_pads + imxsd->num_src_pads;
+
+		for (j = 0; j < num_pads; j++) {
+			pad = &imxsd->pad[j];
+
+			/* only create the source->sink links */
+			if (!(pad->pad.flags & MEDIA_PAD_FL_SOURCE))
+				continue;
+
+			for (k = 0; k < pad->num_links; k++) {
+				link = &pad->link[k];
+
+				ret = imx_media_create_link(imxmd, imxsd, link);
+				if (ret)
+					goto out;
+			}
+		}
+	}
+
+out:
+	return ret;
+}
+
+/*
+ * adds given video device to given imx-media source pad vdev list.
+ * Continues upstream from the pad entity's sink pads.
+ */
+static int imx_media_add_vdev_to_pad(struct imx_media_dev *imxmd,
+				     struct imx_media_video_dev *vdev,
+				     struct media_pad *srcpad)
+{
+	struct media_entity *entity = srcpad->entity;
+	struct imx_media_subdev *imxsd;
+	struct imx_media_pad *imxpad;
+	struct media_link *link;
+	struct v4l2_subdev *sd;
+	int i, vdev_idx, ret;
+
+	/* skip this entity if not a v4l2_subdev */
+	if (!is_media_entity_v4l2_subdev(entity))
+		return 0;
+
+	sd = media_entity_to_v4l2_subdev(entity);
+	imxsd = imx_media_find_subdev_by_sd(imxmd, sd);
+	if (IS_ERR(imxsd))
+		return PTR_ERR(imxsd);
+
+	imxpad = &imxsd->pad[srcpad->index];
+	vdev_idx = imxpad->num_vdevs;
+
+	/* just return if we've been here before */
+	for (i = 0; i < vdev_idx; i++)
+		if (vdev == imxpad->vdev[i])
+			return 0;
+
+	if (vdev_idx >= IMX_MEDIA_MAX_VDEVS) {
+		dev_err(imxmd->md.dev, "can't add %s to pad %s:%u\n",
+			vdev->vfd->entity.name, entity->name, srcpad->index);
+		return -ENOSPC;
+	}
+
+	dev_dbg(imxmd->md.dev, "adding %s to pad %s:%u\n",
+		vdev->vfd->entity.name, entity->name, srcpad->index);
+	imxpad->vdev[vdev_idx] = vdev;
+	imxpad->num_vdevs++;
+
+	/* move upstream from this entity's sink pads */
+	for (i = 0; i < entity->num_pads; i++) {
+		struct media_pad *pad = &entity->pads[i];
+
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			continue;
+
+		list_for_each_entry(link, &entity->links, list) {
+			if (link->sink != pad)
+				continue;
+			ret = imx_media_add_vdev_to_pad(imxmd, vdev,
+							link->source);
+			if (ret)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
+/* form the vdev lists in all imx-media source pads */
+static int imx_media_create_pad_vdev_lists(struct imx_media_dev *imxmd)
+{
+	struct imx_media_video_dev *vdev;
+	struct media_link *link;
+	int i, ret;
+
+	for (i = 0; i < imxmd->num_vdevs; i++) {
+		vdev = imxmd->vdev[i];
+		link = list_first_entry(&vdev->vfd->entity.links,
+					struct media_link, list);
+		ret = imx_media_add_vdev_to_pad(imxmd, vdev, link->source);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+/* async subdev complete notifier */
+static int imx_media_probe_complete(struct v4l2_async_notifier *notifier)
+{
+	struct imx_media_dev *imxmd = notifier2dev(notifier);
+	int i, ret;
+
+	mutex_lock(&imxmd->mutex);
+
+	/* make sure all subdevs were bound */
+	for (i = 0; i < imxmd->num_subdevs; i++) {
+		if (!imxmd->subdev[i].sd) {
+			v4l2_err(&imxmd->v4l2_dev, "unbound subdev!\n");
+			ret = -ENODEV;
+			goto unlock;
+		}
+	}
+
+	ret = imx_media_create_links(imxmd);
+	if (ret)
+		goto unlock;
+
+	ret = imx_media_create_pad_vdev_lists(imxmd);
+	if (ret)
+		goto unlock;
+
+	ret = v4l2_device_register_subdev_nodes(&imxmd->v4l2_dev);
+unlock:
+	mutex_unlock(&imxmd->mutex);
+	if (ret)
+		return ret;
+
+	return media_device_register(&imxmd->md);
+}
+
+/*
+ * adds controls to a video device from an entity subdevice.
+ * Continues upstream from the entity's sink pads.
+ */
+static int imx_media_inherit_controls(struct imx_media_dev *imxmd,
+				      struct video_device *vfd,
+				      struct media_entity *entity)
+{
+	int i, ret = 0;
+
+	if (is_media_entity_v4l2_subdev(entity)) {
+		struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+
+		dev_dbg(imxmd->md.dev,
+			"adding controls to %s from %s\n",
+			vfd->entity.name, sd->entity.name);
+
+		ret = v4l2_ctrl_add_handler(vfd->ctrl_handler,
+					    sd->ctrl_handler,
+					    NULL);
+		if (ret)
+			return ret;
+	}
+
+	/* move upstream */
+	for (i = 0; i < entity->num_pads; i++) {
+		struct media_pad *pad, *spad = &entity->pads[i];
+
+		if (!(spad->flags & MEDIA_PAD_FL_SINK))
+			continue;
+
+		pad = media_entity_remote_pad(spad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			continue;
+
+		ret = imx_media_inherit_controls(imxmd, vfd, pad->entity);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+static int imx_media_link_notify(struct media_link *link, u32 flags,
+				 unsigned int notification)
+{
+	struct media_entity *source = link->source->entity;
+	struct imx_media_subdev *imxsd;
+	struct imx_media_pad *imxpad;
+	struct imx_media_dev *imxmd;
+	struct video_device *vfd;
+	struct v4l2_subdev *sd;
+	int i, pad_idx, ret;
+
+	ret = v4l2_pipeline_link_notify(link, flags, notification);
+	if (ret)
+		return ret;
+
+	/* don't bother if source is not a subdev */
+	if (!is_media_entity_v4l2_subdev(source))
+		return 0;
+
+	sd = media_entity_to_v4l2_subdev(source);
+	pad_idx = link->source->index;
+
+	imxmd = dev_get_drvdata(sd->v4l2_dev->dev);
+
+	imxsd = imx_media_find_subdev_by_sd(imxmd, sd);
+	if (IS_ERR(imxsd))
+		return PTR_ERR(imxsd);
+	imxpad = &imxsd->pad[pad_idx];
+
+	/*
+	 * Before disabling a link, reset controls for all video
+	 * devices reachable from this link.
+	 *
+	 * After enabling a link, refresh controls for all video
+	 * devices reachable from this link.
+	 */
+	if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH &&
+	    !(flags & MEDIA_LNK_FL_ENABLED)) {
+		for (i = 0; i < imxpad->num_vdevs; i++) {
+			vfd = imxpad->vdev[i]->vfd;
+			dev_dbg(imxmd->md.dev,
+				"reset controls for %s\n",
+				vfd->entity.name);
+			v4l2_ctrl_handler_free(vfd->ctrl_handler);
+			v4l2_ctrl_handler_init(vfd->ctrl_handler, 0);
+		}
+	} else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
+		   (link->flags & MEDIA_LNK_FL_ENABLED)) {
+		for (i = 0; i < imxpad->num_vdevs; i++) {
+			vfd = imxpad->vdev[i]->vfd;
+			dev_dbg(imxmd->md.dev,
+				"refresh controls for %s\n",
+				vfd->entity.name);
+			ret = imx_media_inherit_controls(imxmd, vfd,
+							 &vfd->entity);
+			if (ret)
+				break;
+		}
+	}
+
+	return ret;
+}
+
+static const struct media_device_ops imx_media_md_ops = {
+	.link_notify = imx_media_link_notify,
+};
+
+static int imx_media_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *node = dev->of_node;
+	struct imx_media_subdev *csi[4] = {0};
+	struct imx_media_dev *imxmd;
+	int ret;
+
+	imxmd = devm_kzalloc(dev, sizeof(*imxmd), GFP_KERNEL);
+	if (!imxmd)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, imxmd);
+
+	strlcpy(imxmd->md.model, "imx-media", sizeof(imxmd->md.model));
+	imxmd->md.ops = &imx_media_md_ops;
+	imxmd->md.dev = dev;
+
+	mutex_init(&imxmd->mutex);
+
+	imxmd->v4l2_dev.mdev = &imxmd->md;
+	strlcpy(imxmd->v4l2_dev.name, "imx-media",
+		sizeof(imxmd->v4l2_dev.name));
+
+	media_device_init(&imxmd->md);
+
+	ret = v4l2_device_register(dev, &imxmd->v4l2_dev);
+	if (ret < 0) {
+		v4l2_err(&imxmd->v4l2_dev,
+			 "Failed to register v4l2_device: %d\n", ret);
+		goto cleanup;
+	}
+
+	dev_set_drvdata(imxmd->v4l2_dev.dev, imxmd);
+
+	ret = imx_media_of_parse(imxmd, &csi, node);
+	if (ret) {
+		v4l2_err(&imxmd->v4l2_dev,
+			 "imx_media_of_parse failed with %d\n", ret);
+		goto unreg_dev;
+	}
+
+	ret = imx_media_add_internal_subdevs(imxmd, csi);
+	if (ret) {
+		v4l2_err(&imxmd->v4l2_dev,
+			 "add_internal_subdevs failed with %d\n", ret);
+		goto unreg_dev;
+	}
+
+	/* no subdevs? just bail */
+	imxmd->num_subdevs = imxmd->subdev_notifier.num_subdevs;
+	if (imxmd->num_subdevs == 0) {
+		ret = -ENODEV;
+		goto unreg_dev;
+	}
+
+	/* prepare the async subdev notifier and register it */
+	imxmd->subdev_notifier.subdevs = imxmd->async_ptrs;
+	imxmd->subdev_notifier.bound = imx_media_subdev_bound;
+	imxmd->subdev_notifier.complete = imx_media_probe_complete;
+	ret = v4l2_async_notifier_register(&imxmd->v4l2_dev,
+					   &imxmd->subdev_notifier);
+	if (ret) {
+		v4l2_err(&imxmd->v4l2_dev,
+			 "v4l2_async_notifier_register failed with %d\n", ret);
+		goto del_int;
+	}
+
+	return 0;
+
+del_int:
+	imx_media_remove_internal_subdevs(imxmd);
+unreg_dev:
+	v4l2_device_unregister(&imxmd->v4l2_dev);
+cleanup:
+	media_device_cleanup(&imxmd->md);
+	return ret;
+}
+
+static int imx_media_remove(struct platform_device *pdev)
+{
+	struct imx_media_dev *imxmd =
+		(struct imx_media_dev *)platform_get_drvdata(pdev);
+
+	v4l2_info(&imxmd->v4l2_dev, "Removing imx-media\n");
+
+	v4l2_async_notifier_unregister(&imxmd->subdev_notifier);
+	imx_media_remove_internal_subdevs(imxmd);
+	v4l2_device_unregister(&imxmd->v4l2_dev);
+	media_device_unregister(&imxmd->md);
+	media_device_cleanup(&imxmd->md);
+
+	return 0;
+}
+
+static const struct of_device_id imx_media_dt_ids[] = {
+	{ .compatible = "fsl,imx-capture-subsystem" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, imx_media_dt_ids);
+
+static struct platform_driver imx_media_pdrv = {
+	.probe		= imx_media_probe,
+	.remove		= imx_media_remove,
+	.driver		= {
+		.name	= "imx-media",
+		.of_match_table	= imx_media_dt_ids,
+	},
+};
+
+module_platform_driver(imx_media_pdrv);
+
+MODULE_DESCRIPTION("i.MX5/6 v4l2 media controller driver");
+MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/imx/imx-media-fim.c b/drivers/staging/media/imx/imx-media-fim.c
new file mode 100644
index 000000000000..47275ef803f3
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-fim.c
@@ -0,0 +1,494 @@
+/*
+ * Frame Interval Monitor.
+ *
+ * Copyright (c) 2016 Mentor Graphics Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/delay.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+#include <media/imx.h>
+#include "imx-media.h"
+
+enum {
+	FIM_CL_ENABLE = 0,
+	FIM_CL_NUM,
+	FIM_CL_TOLERANCE_MIN,
+	FIM_CL_TOLERANCE_MAX,
+	FIM_CL_NUM_SKIP,
+	FIM_NUM_CONTROLS,
+};
+
+enum {
+	FIM_CL_ICAP_EDGE = 0,
+	FIM_CL_ICAP_CHANNEL,
+	FIM_NUM_ICAP_CONTROLS,
+};
+
+#define FIM_CL_ENABLE_DEF          0 /* FIM disabled by default */
+#define FIM_CL_NUM_DEF             8 /* average 8 frames */
+#define FIM_CL_NUM_SKIP_DEF        2 /* skip 2 frames after restart */
+#define FIM_CL_TOLERANCE_MIN_DEF  50 /* usec */
+#define FIM_CL_TOLERANCE_MAX_DEF   0 /* no max tolerance (unbounded) */
+
+struct imx_media_fim {
+	struct imx_media_dev *md;
+
+	/* the owning subdev of this fim instance */
+	struct v4l2_subdev *sd;
+
+	/* FIM's control handler */
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	/* control clusters */
+	struct v4l2_ctrl  *ctrl[FIM_NUM_CONTROLS];
+	struct v4l2_ctrl  *icap_ctrl[FIM_NUM_ICAP_CONTROLS];
+
+	spinlock_t        lock; /* protect control values */
+
+	/* current control values */
+	bool              enabled;
+	int               num_avg;
+	int               num_skip;
+	unsigned long     tolerance_min; /* usec */
+	unsigned long     tolerance_max; /* usec */
+	/* input capture method of measuring FI */
+	int               icap_channel;
+	int               icap_flags;
+
+	int               counter;
+	struct timespec   last_ts;
+	unsigned long     sum;       /* usec */
+	unsigned long     nominal;   /* usec */
+
+	struct completion icap_first_event;
+	bool              stream_on;
+};
+
+#define icap_enabled(fim) ((fim)->icap_flags != IRQ_TYPE_NONE)
+
+static void update_fim_nominal(struct imx_media_fim *fim,
+			       const struct v4l2_fract *fi)
+{
+	if (fi->denominator == 0) {
+		dev_dbg(fim->sd->dev, "no frame interval, FIM disabled\n");
+		fim->enabled = false;
+		return;
+	}
+
+	fim->nominal = DIV_ROUND_CLOSEST_ULL(1000000ULL * (u64)fi->numerator,
+					     fi->denominator);
+
+	dev_dbg(fim->sd->dev, "FI=%lu usec\n", fim->nominal);
+}
+
+static void reset_fim(struct imx_media_fim *fim, bool curval)
+{
+	struct v4l2_ctrl *icap_chan = fim->icap_ctrl[FIM_CL_ICAP_CHANNEL];
+	struct v4l2_ctrl *icap_edge = fim->icap_ctrl[FIM_CL_ICAP_EDGE];
+	struct v4l2_ctrl *en = fim->ctrl[FIM_CL_ENABLE];
+	struct v4l2_ctrl *num = fim->ctrl[FIM_CL_NUM];
+	struct v4l2_ctrl *skip = fim->ctrl[FIM_CL_NUM_SKIP];
+	struct v4l2_ctrl *tol_min = fim->ctrl[FIM_CL_TOLERANCE_MIN];
+	struct v4l2_ctrl *tol_max = fim->ctrl[FIM_CL_TOLERANCE_MAX];
+
+	if (curval) {
+		fim->enabled = en->cur.val;
+		fim->icap_flags = icap_edge->cur.val;
+		fim->icap_channel = icap_chan->cur.val;
+		fim->num_avg = num->cur.val;
+		fim->num_skip = skip->cur.val;
+		fim->tolerance_min = tol_min->cur.val;
+		fim->tolerance_max = tol_max->cur.val;
+	} else {
+		fim->enabled = en->val;
+		fim->icap_flags = icap_edge->val;
+		fim->icap_channel = icap_chan->val;
+		fim->num_avg = num->val;
+		fim->num_skip = skip->val;
+		fim->tolerance_min = tol_min->val;
+		fim->tolerance_max = tol_max->val;
+	}
+
+	/* disable tolerance range if max <= min */
+	if (fim->tolerance_max <= fim->tolerance_min)
+		fim->tolerance_max = 0;
+
+	/* num_skip must be >= 1 if input capture not used */
+	if (!icap_enabled(fim))
+		fim->num_skip = max_t(int, fim->num_skip, 1);
+
+	fim->counter = -fim->num_skip;
+	fim->sum = 0;
+}
+
+static void send_fim_event(struct imx_media_fim *fim, unsigned long error)
+{
+	static const struct v4l2_event ev = {
+		.type = V4L2_EVENT_IMX_FRAME_INTERVAL_ERROR,
+	};
+
+	v4l2_subdev_notify_event(fim->sd, &ev);
+}
+
+/*
+ * Monitor an averaged frame interval. If the average deviates too much
+ * from the nominal frame rate, send the frame interval error event. The
+ * frame intervals are averaged in order to quiet noise from
+ * (presumably random) interrupt latency.
+ */
+static void frame_interval_monitor(struct imx_media_fim *fim,
+				   struct timespec *ts)
+{
+	unsigned long interval, error, error_avg;
+	bool send_event = false;
+	struct timespec diff;
+
+	if (!fim->enabled || ++fim->counter <= 0)
+		goto out_update_ts;
+
+	diff = timespec_sub(*ts, fim->last_ts);
+	interval = diff.tv_sec * 1000 * 1000 + diff.tv_nsec / 1000;
+	error = abs(interval - fim->nominal);
+
+	if (fim->tolerance_max && error >= fim->tolerance_max) {
+		dev_dbg(fim->sd->dev,
+			"FIM: %lu ignored, out of tolerance bounds\n",
+			error);
+		fim->counter--;
+		goto out_update_ts;
+	}
+
+	fim->sum += error;
+
+	if (fim->counter == fim->num_avg) {
+		error_avg = DIV_ROUND_CLOSEST(fim->sum, fim->num_avg);
+
+		if (error_avg > fim->tolerance_min)
+			send_event = true;
+
+		dev_dbg(fim->sd->dev, "FIM: error: %lu usec%s\n",
+			error_avg, send_event ? " (!!!)" : "");
+
+		fim->counter = 0;
+		fim->sum = 0;
+	}
+
+out_update_ts:
+	fim->last_ts = *ts;
+	if (send_event)
+		send_fim_event(fim, error_avg);
+}
+
+#ifdef CONFIG_IMX_GPT_ICAP
+/*
+ * Input Capture method of measuring frame intervals. Not subject
+ * to interrupt latency.
+ */
+static void fim_input_capture_handler(int channel, void *dev_id,
+				      struct timespec *ts)
+{
+	struct imx_media_fim *fim = dev_id;
+	unsigned long flags;
+
+	spin_lock_irqsave(&fim->lock, flags);
+
+	frame_interval_monitor(fim, ts);
+
+	if (!completion_done(&fim->icap_first_event))
+		complete(&fim->icap_first_event);
+
+	spin_unlock_irqrestore(&fim->lock, flags);
+}
+
+static int fim_request_input_capture(struct imx_media_fim *fim)
+{
+	init_completion(&fim->icap_first_event);
+
+	return mxc_request_input_capture(fim->icap_channel,
+					 fim_input_capture_handler,
+					 fim->icap_flags, fim);
+}
+
+static void fim_free_input_capture(struct imx_media_fim *fim)
+{
+	mxc_free_input_capture(fim->icap_channel, fim);
+}
+
+#else /* CONFIG_IMX_GPT_ICAP */
+
+static int fim_request_input_capture(struct imx_media_fim *fim)
+{
+	return 0;
+}
+
+static void fim_free_input_capture(struct imx_media_fim *fim)
+{
+}
+
+#endif /* CONFIG_IMX_GPT_ICAP */
+
+/*
+ * In case we are monitoring the first frame interval after streamon
+ * (when fim->num_skip = 0), we need a valid fim->last_ts before we
+ * can begin. This only applies to the input capture method. It is not
+ * possible to accurately measure the first FI after streamon using the
+ * EOF method, so fim->num_skip minimum is set to 1 in that case, so this
+ * function is a noop when the EOF method is used.
+ */
+static void fim_acquire_first_ts(struct imx_media_fim *fim)
+{
+	unsigned long ret;
+
+	if (!fim->enabled || fim->num_skip > 0)
+		return;
+
+	ret = wait_for_completion_timeout(
+		&fim->icap_first_event,
+		msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
+	if (ret == 0)
+		v4l2_warn(fim->sd, "wait first icap event timeout\n");
+}
+
+/* FIM Controls */
+static int fim_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx_media_fim *fim = container_of(ctrl->handler,
+						 struct imx_media_fim,
+						 ctrl_handler);
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&fim->lock, flags);
+
+	switch (ctrl->id) {
+	case V4L2_CID_IMX_FIM_ENABLE:
+		break;
+	case V4L2_CID_IMX_FIM_ICAP_EDGE:
+		if (fim->stream_on)
+			ret = -EBUSY;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	if (!ret)
+		reset_fim(fim, false);
+
+	spin_unlock_irqrestore(&fim->lock, flags);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops fim_ctrl_ops = {
+	.s_ctrl = fim_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config fim_ctrl[] = {
+	[FIM_CL_ENABLE] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_ENABLE,
+		.name = "FIM Enable",
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.def = FIM_CL_ENABLE_DEF,
+		.min = 0,
+		.max = 1,
+		.step = 1,
+	},
+	[FIM_CL_NUM] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_NUM,
+		.name = "FIM Num Average",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.def = FIM_CL_NUM_DEF,
+		.min =  1, /* no averaging */
+		.max = 64, /* average 64 frames */
+		.step = 1,
+	},
+	[FIM_CL_TOLERANCE_MIN] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_TOLERANCE_MIN,
+		.name = "FIM Tolerance Min",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.def = FIM_CL_TOLERANCE_MIN_DEF,
+		.min =    2,
+		.max =  200,
+		.step =   1,
+	},
+	[FIM_CL_TOLERANCE_MAX] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_TOLERANCE_MAX,
+		.name = "FIM Tolerance Max",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.def = FIM_CL_TOLERANCE_MAX_DEF,
+		.min =    0,
+		.max =  500,
+		.step =   1,
+	},
+	[FIM_CL_NUM_SKIP] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_NUM_SKIP,
+		.name = "FIM Num Skip",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.def = FIM_CL_NUM_SKIP_DEF,
+		.min =   0, /* skip no frames */
+		.max = 256, /* skip 256 frames */
+		.step =  1,
+	},
+};
+
+static const struct v4l2_ctrl_config fim_icap_ctrl[] = {
+	[FIM_CL_ICAP_EDGE] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_ICAP_EDGE,
+		.name = "FIM Input Capture Edge",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.def =  IRQ_TYPE_NONE, /* input capture disabled by default */
+		.min =  IRQ_TYPE_NONE,
+		.max =  IRQ_TYPE_EDGE_BOTH,
+		.step = 1,
+	},
+	[FIM_CL_ICAP_CHANNEL] = {
+		.ops = &fim_ctrl_ops,
+		.id = V4L2_CID_IMX_FIM_ICAP_CHANNEL,
+		.name = "FIM Input Capture Channel",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.def =  0,
+		.min =  0,
+		.max =  1,
+		.step = 1,
+	},
+};
+
+static int init_fim_controls(struct imx_media_fim *fim)
+{
+	struct v4l2_ctrl_handler *hdlr = &fim->ctrl_handler;
+	int i, ret;
+
+	v4l2_ctrl_handler_init(hdlr, FIM_NUM_CONTROLS + FIM_NUM_ICAP_CONTROLS);
+
+	for (i = 0; i < FIM_NUM_CONTROLS; i++)
+		fim->ctrl[i] = v4l2_ctrl_new_custom(hdlr,
+						    &fim_ctrl[i],
+						    NULL);
+	for (i = 0; i < FIM_NUM_ICAP_CONTROLS; i++)
+		fim->icap_ctrl[i] = v4l2_ctrl_new_custom(hdlr,
+							 &fim_icap_ctrl[i],
+							 NULL);
+	if (hdlr->error) {
+		ret = hdlr->error;
+		goto err_free;
+	}
+
+	v4l2_ctrl_cluster(FIM_NUM_CONTROLS, fim->ctrl);
+	v4l2_ctrl_cluster(FIM_NUM_ICAP_CONTROLS, fim->icap_ctrl);
+
+	return 0;
+err_free:
+	v4l2_ctrl_handler_free(hdlr);
+	return ret;
+}
+
+/*
+ * Monitor frame intervals via EOF interrupt. This method is
+ * subject to uncertainty errors introduced by interrupt latency.
+ *
+ * This is a noop if the Input Capture method is being used, since
+ * the frame_interval_monitor() is called by the input capture event
+ * callback handler in that case.
+ */
+void imx_media_fim_eof_monitor(struct imx_media_fim *fim, struct timespec *ts)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&fim->lock, flags);
+
+	if (!icap_enabled(fim))
+		frame_interval_monitor(fim, ts);
+
+	spin_unlock_irqrestore(&fim->lock, flags);
+}
+EXPORT_SYMBOL_GPL(imx_media_fim_eof_monitor);
+
+/* Called by the subdev in its s_stream callback */
+int imx_media_fim_set_stream(struct imx_media_fim *fim,
+			     const struct v4l2_fract *fi,
+			     bool on)
+{
+	unsigned long flags;
+	int ret = 0;
+
+	v4l2_ctrl_lock(fim->ctrl[FIM_CL_ENABLE]);
+
+	if (fim->stream_on == on)
+		goto out;
+
+	if (on) {
+		spin_lock_irqsave(&fim->lock, flags);
+		reset_fim(fim, true);
+		update_fim_nominal(fim, fi);
+		spin_unlock_irqrestore(&fim->lock, flags);
+
+		if (icap_enabled(fim)) {
+			ret = fim_request_input_capture(fim);
+			if (ret)
+				goto out;
+			fim_acquire_first_ts(fim);
+		}
+	} else {
+		if (icap_enabled(fim))
+			fim_free_input_capture(fim);
+	}
+
+	fim->stream_on = on;
+out:
+	v4l2_ctrl_unlock(fim->ctrl[FIM_CL_ENABLE]);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(imx_media_fim_set_stream);
+
+int imx_media_fim_add_controls(struct imx_media_fim *fim)
+{
+	/* add the FIM controls to the calling subdev ctrl handler */
+	return v4l2_ctrl_add_handler(fim->sd->ctrl_handler,
+				     &fim->ctrl_handler, NULL);
+}
+EXPORT_SYMBOL_GPL(imx_media_fim_add_controls);
+
+/* Called by the subdev in its subdev registered callback */
+struct imx_media_fim *imx_media_fim_init(struct v4l2_subdev *sd)
+{
+	struct imx_media_fim *fim;
+	int ret;
+
+	fim = devm_kzalloc(sd->dev, sizeof(*fim), GFP_KERNEL);
+	if (!fim)
+		return ERR_PTR(-ENOMEM);
+
+	/* get media device */
+	fim->md = dev_get_drvdata(sd->v4l2_dev->dev);
+	fim->sd = sd;
+
+	spin_lock_init(&fim->lock);
+
+	ret = init_fim_controls(fim);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return fim;
+}
+EXPORT_SYMBOL_GPL(imx_media_fim_init);
+
+void imx_media_fim_free(struct imx_media_fim *fim)
+{
+	v4l2_ctrl_handler_free(&fim->ctrl_handler);
+}
+EXPORT_SYMBOL_GPL(imx_media_fim_free);
diff --git a/drivers/staging/media/imx/imx-media-internal-sd.c b/drivers/staging/media/imx/imx-media-internal-sd.c
new file mode 100644
index 000000000000..cdfbf40dfcbe
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-internal-sd.c
@@ -0,0 +1,349 @@
+/*
+ * Media driver for Freescale i.MX5/6 SOC
+ *
+ * Adds the internal subdevices and the media links between them.
+ *
+ * Copyright (c) 2016 Mentor Graphics Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/platform_device.h>
+#include "imx-media.h"
+
+enum isd_enum {
+	isd_csi0 = 0,
+	isd_csi1,
+	isd_vdic,
+	isd_ic_prp,
+	isd_ic_prpenc,
+	isd_ic_prpvf,
+	num_isd,
+};
+
+static const struct internal_subdev_id {
+	enum isd_enum index;
+	const char *name;
+	u32 grp_id;
+} isd_id[num_isd] = {
+	[isd_csi0] = {
+		.index = isd_csi0,
+		.grp_id = IMX_MEDIA_GRP_ID_CSI0,
+		.name = "imx-ipuv3-csi",
+	},
+	[isd_csi1] = {
+		.index = isd_csi1,
+		.grp_id = IMX_MEDIA_GRP_ID_CSI1,
+		.name = "imx-ipuv3-csi",
+	},
+	[isd_vdic] = {
+		.index = isd_vdic,
+		.grp_id = IMX_MEDIA_GRP_ID_VDIC,
+		.name = "imx-ipuv3-vdic",
+	},
+	[isd_ic_prp] = {
+		.index = isd_ic_prp,
+		.grp_id = IMX_MEDIA_GRP_ID_IC_PRP,
+		.name = "imx-ipuv3-ic",
+	},
+	[isd_ic_prpenc] = {
+		.index = isd_ic_prpenc,
+		.grp_id = IMX_MEDIA_GRP_ID_IC_PRPENC,
+		.name = "imx-ipuv3-ic",
+	},
+	[isd_ic_prpvf] = {
+		.index = isd_ic_prpvf,
+		.grp_id = IMX_MEDIA_GRP_ID_IC_PRPVF,
+		.name = "imx-ipuv3-ic",
+	},
+};
+
+struct internal_link {
+	const struct internal_subdev_id *remote_id;
+	int remote_pad;
+};
+
+struct internal_pad {
+	bool devnode; /* does this pad link to a device node */
+	struct internal_link link[IMX_MEDIA_MAX_LINKS];
+};
+
+static const struct internal_subdev {
+	const struct internal_subdev_id *id;
+	struct internal_pad pad[IMX_MEDIA_MAX_PADS];
+	int num_sink_pads;
+	int num_src_pads;
+} internal_subdev[num_isd] = {
+	[isd_csi0] = {
+		.id = &isd_id[isd_csi0],
+		.num_sink_pads = CSI_NUM_SINK_PADS,
+		.num_src_pads = CSI_NUM_SRC_PADS,
+		.pad[CSI_SRC_PAD_DIRECT] = {
+			.link = {
+				{
+					.remote_id = &isd_id[isd_ic_prp],
+					.remote_pad = PRP_SINK_PAD,
+				}, {
+					.remote_id =  &isd_id[isd_vdic],
+					.remote_pad = VDIC_SINK_PAD_DIRECT,
+				},
+			},
+		},
+		.pad[CSI_SRC_PAD_IDMAC] = {
+			.devnode = true,
+		},
+	},
+
+	[isd_csi1] = {
+		.id = &isd_id[isd_csi1],
+		.num_sink_pads = CSI_NUM_SINK_PADS,
+		.num_src_pads = CSI_NUM_SRC_PADS,
+		.pad[CSI_SRC_PAD_DIRECT] = {
+			.link = {
+				{
+					.remote_id = &isd_id[isd_ic_prp],
+					.remote_pad = PRP_SINK_PAD,
+				}, {
+					.remote_id =  &isd_id[isd_vdic],
+					.remote_pad = VDIC_SINK_PAD_DIRECT,
+				},
+			},
+		},
+		.pad[CSI_SRC_PAD_IDMAC] = {
+			.devnode = true,
+		},
+	},
+
+	[isd_vdic] = {
+		.id = &isd_id[isd_vdic],
+		.num_sink_pads = VDIC_NUM_SINK_PADS,
+		.num_src_pads = VDIC_NUM_SRC_PADS,
+		.pad[VDIC_SINK_PAD_IDMAC] = {
+			.devnode = true,
+		},
+		.pad[VDIC_SRC_PAD_DIRECT] = {
+			.link = {
+				{
+					.remote_id =  &isd_id[isd_ic_prp],
+					.remote_pad = PRP_SINK_PAD,
+				},
+			},
+		},
+	},
+
+	[isd_ic_prp] = {
+		.id = &isd_id[isd_ic_prp],
+		.num_sink_pads = PRP_NUM_SINK_PADS,
+		.num_src_pads = PRP_NUM_SRC_PADS,
+		.pad[PRP_SRC_PAD_PRPENC] = {
+			.link = {
+				{
+					.remote_id = &isd_id[isd_ic_prpenc],
+					.remote_pad = 0,
+				},
+			},
+		},
+		.pad[PRP_SRC_PAD_PRPVF] = {
+			.link = {
+				{
+					.remote_id = &isd_id[isd_ic_prpvf],
+					.remote_pad = 0,
+				},
+			},
+		},
+	},
+
+	[isd_ic_prpenc] = {
+		.id = &isd_id[isd_ic_prpenc],
+		.num_sink_pads = PRPENCVF_NUM_SINK_PADS,
+		.num_src_pads = PRPENCVF_NUM_SRC_PADS,
+		.pad[PRPENCVF_SRC_PAD] = {
+			.devnode = true,
+		},
+	},
+
+	[isd_ic_prpvf] = {
+		.id = &isd_id[isd_ic_prpvf],
+		.num_sink_pads = PRPENCVF_NUM_SINK_PADS,
+		.num_src_pads = PRPENCVF_NUM_SRC_PADS,
+		.pad[PRPENCVF_SRC_PAD] = {
+			.devnode = true,
+		},
+	},
+};
+
+/* form a device name given a group id and ipu id */
+static inline void isd_id_to_devname(char *devname, int sz,
+				     const struct internal_subdev_id *id,
+				     int ipu_id)
+{
+	int pdev_id = ipu_id * num_isd + id->index;
+
+	snprintf(devname, sz, "%s.%d", id->name, pdev_id);
+}
+
+/* adds the links from given internal subdev */
+static int add_internal_links(struct imx_media_dev *imxmd,
+			      const struct internal_subdev *isd,
+			      struct imx_media_subdev *imxsd,
+			      int ipu_id)
+{
+	int i, num_pads, ret;
+
+	num_pads = isd->num_sink_pads + isd->num_src_pads;
+
+	for (i = 0; i < num_pads; i++) {
+		const struct internal_pad *intpad = &isd->pad[i];
+		struct imx_media_pad *pad = &imxsd->pad[i];
+		int j;
+
+		/* init the pad flags for this internal subdev */
+		pad->pad.flags = (i < isd->num_sink_pads) ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+		/* export devnode pad flag to the subdevs */
+		pad->devnode = intpad->devnode;
+
+		for (j = 0; ; j++) {
+			const struct internal_link *link;
+			char remote_devname[32];
+
+			link = &intpad->link[j];
+
+			if (!link->remote_id)
+				break;
+
+			isd_id_to_devname(remote_devname,
+					  sizeof(remote_devname),
+					  link->remote_id, ipu_id);
+
+			ret = imx_media_add_pad_link(imxmd, pad,
+						     NULL, remote_devname,
+						     i, link->remote_pad);
+			if (ret)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
+/* register an internal subdev as a platform device */
+static struct imx_media_subdev *
+add_internal_subdev(struct imx_media_dev *imxmd,
+		    const struct internal_subdev *isd,
+		    int ipu_id)
+{
+	struct imx_media_internal_sd_platformdata pdata;
+	struct platform_device_info pdevinfo = {0};
+	struct imx_media_subdev *imxsd;
+	struct platform_device *pdev;
+
+	pdata.grp_id = isd->id->grp_id;
+
+	/* the id of IPU this subdev will control */
+	pdata.ipu_id = ipu_id;
+
+	/* create subdev name */
+	imx_media_grp_id_to_sd_name(pdata.sd_name, sizeof(pdata.sd_name),
+				    pdata.grp_id, ipu_id);
+
+	pdevinfo.name = isd->id->name;
+	pdevinfo.id = ipu_id * num_isd + isd->id->index;
+	pdevinfo.parent = imxmd->md.dev;
+	pdevinfo.data = &pdata;
+	pdevinfo.size_data = sizeof(pdata);
+	pdevinfo.dma_mask = DMA_BIT_MASK(32);
+
+	pdev = platform_device_register_full(&pdevinfo);
+	if (IS_ERR(pdev))
+		return ERR_CAST(pdev);
+
+	imxsd = imx_media_add_async_subdev(imxmd, NULL, pdev);
+	if (IS_ERR(imxsd))
+		return imxsd;
+
+	imxsd->num_sink_pads = isd->num_sink_pads;
+	imxsd->num_src_pads = isd->num_src_pads;
+
+	return imxsd;
+}
+
+/* adds the internal subdevs in one ipu */
+static int add_ipu_internal_subdevs(struct imx_media_dev *imxmd,
+				    struct imx_media_subdev *csi0,
+				    struct imx_media_subdev *csi1,
+				    int ipu_id)
+{
+	enum isd_enum i;
+	int ret;
+
+	for (i = 0; i < num_isd; i++) {
+		const struct internal_subdev *isd = &internal_subdev[i];
+		struct imx_media_subdev *imxsd;
+
+		/*
+		 * the CSIs are represented in the device-tree, so those
+		 * devices are added already, and are added to the async
+		 * subdev list by of_parse_subdev(), so we are given those
+		 * subdevs as csi0 and csi1.
+		 */
+		switch (isd->id->grp_id) {
+		case IMX_MEDIA_GRP_ID_CSI0:
+			imxsd = csi0;
+			break;
+		case IMX_MEDIA_GRP_ID_CSI1:
+			imxsd = csi1;
+			break;
+		default:
+			imxsd = add_internal_subdev(imxmd, isd, ipu_id);
+			break;
+		}
+
+		if (IS_ERR(imxsd))
+			return PTR_ERR(imxsd);
+
+		/* add the links from this subdev */
+		if (imxsd) {
+			ret = add_internal_links(imxmd, isd, imxsd, ipu_id);
+			if (ret)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
+int imx_media_add_internal_subdevs(struct imx_media_dev *imxmd,
+				   struct imx_media_subdev *csi[4])
+{
+	int ret;
+
+	ret = add_ipu_internal_subdevs(imxmd, csi[0], csi[1], 0);
+	if (ret)
+		goto remove;
+
+	ret = add_ipu_internal_subdevs(imxmd, csi[2], csi[3], 1);
+	if (ret)
+		goto remove;
+
+	return 0;
+
+remove:
+	imx_media_remove_internal_subdevs(imxmd);
+	return ret;
+}
+
+void imx_media_remove_internal_subdevs(struct imx_media_dev *imxmd)
+{
+	struct imx_media_subdev *imxsd;
+	int i;
+
+	for (i = 0; i < imxmd->subdev_notifier.num_subdevs; i++) {
+		imxsd = &imxmd->subdev[i];
+		if (!imxsd->pdev)
+			continue;
+		platform_device_unregister(imxsd->pdev);
+	}
+}
diff --git a/drivers/staging/media/imx/imx-media-of.c b/drivers/staging/media/imx/imx-media-of.c
new file mode 100644
index 000000000000..b026fe66467c
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-of.c
@@ -0,0 +1,270 @@
+/*
+ * Media driver for Freescale i.MX5/6 SOC
+ *
+ * Open Firmware parsing.
+ *
+ * Copyright (c) 2016 Mentor Graphics Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/of_platform.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-contig.h>
+#include <linux/of_graph.h>
+#include <video/imx-ipu-v3.h>
+#include "imx-media.h"
+
+static int of_add_pad_link(struct imx_media_dev *imxmd,
+			   struct imx_media_pad *pad,
+			   struct device_node *local_sd_node,
+			   struct device_node *remote_sd_node,
+			   int local_pad, int remote_pad)
+{
+	dev_dbg(imxmd->md.dev, "%s: adding %s:%d -> %s:%d\n", __func__,
+		local_sd_node->name, local_pad,
+		remote_sd_node->name, remote_pad);
+
+	return imx_media_add_pad_link(imxmd, pad, remote_sd_node, NULL,
+				      local_pad, remote_pad);
+}
+
+static void of_parse_sensor(struct imx_media_dev *imxmd,
+			    struct imx_media_subdev *sensor,
+			    struct device_node *sensor_np)
+{
+	struct device_node *endpoint;
+
+	endpoint = of_graph_get_next_endpoint(sensor_np, NULL);
+	if (endpoint) {
+		v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint),
+					   &sensor->sensor_ep);
+		of_node_put(endpoint);
+	}
+}
+
+static int of_get_port_count(const struct device_node *np)
+{
+	struct device_node *ports, *child;
+	int num = 0;
+
+	/* check if this node has a ports subnode */
+	ports = of_get_child_by_name(np, "ports");
+	if (ports)
+		np = ports;
+
+	for_each_child_of_node(np, child)
+		if (of_node_cmp(child->name, "port") == 0)
+			num++;
+
+	of_node_put(ports);
+	return num;
+}
+
+/*
+ * find the remote device node and remote port id (remote pad #)
+ * given local endpoint node
+ */
+static void of_get_remote_pad(struct device_node *epnode,
+			      struct device_node **remote_node,
+			      int *remote_pad)
+{
+	struct device_node *rp, *rpp;
+	struct device_node *remote;
+
+	rp = of_graph_get_remote_port(epnode);
+	rpp = of_graph_get_remote_port_parent(epnode);
+
+	if (of_device_is_compatible(rpp, "fsl,imx6q-ipu")) {
+		/* the remote is one of the CSI ports */
+		remote = rp;
+		*remote_pad = 0;
+		of_node_put(rpp);
+	} else {
+		remote = rpp;
+		if (of_property_read_u32(rp, "reg", remote_pad))
+			*remote_pad = 0;
+		of_node_put(rp);
+	}
+
+	if (!of_device_is_available(remote)) {
+		of_node_put(remote);
+		*remote_node = NULL;
+	} else {
+		*remote_node = remote;
+	}
+}
+
+static struct imx_media_subdev *
+of_parse_subdev(struct imx_media_dev *imxmd, struct device_node *sd_np,
+		bool is_csi_port)
+{
+	struct imx_media_subdev *imxsd;
+	int i, num_pads, ret;
+
+	if (!of_device_is_available(sd_np)) {
+		dev_dbg(imxmd->md.dev, "%s: %s not enabled\n", __func__,
+			sd_np->name);
+		return NULL;
+	}
+
+	/* register this subdev with async notifier */
+	imxsd = imx_media_add_async_subdev(imxmd, sd_np, NULL);
+	if (IS_ERR_OR_NULL(imxsd))
+		return imxsd;
+
+	if (is_csi_port) {
+		/*
+		 * the ipu-csi has one sink port and two source ports.
+		 * The source ports are not represented in the device tree,
+		 * but are described by the internal pads and links later.
+		 */
+		num_pads = CSI_NUM_PADS;
+		imxsd->num_sink_pads = CSI_NUM_SINK_PADS;
+	} else if (of_device_is_compatible(sd_np, "fsl,imx6-mipi-csi2")) {
+		num_pads = of_get_port_count(sd_np);
+		/* the mipi csi2 receiver has only one sink port */
+		imxsd->num_sink_pads = 1;
+	} else if (of_device_is_compatible(sd_np, "video-mux")) {
+		num_pads = of_get_port_count(sd_np);
+		/* for the video mux, all but the last port are sinks */
+		imxsd->num_sink_pads = num_pads - 1;
+	} else {
+		num_pads = of_get_port_count(sd_np);
+		if (num_pads != 1) {
+			dev_warn(imxmd->md.dev,
+				 "%s: unknown device %s with %d ports\n",
+				 __func__, sd_np->name, num_pads);
+			return NULL;
+		}
+
+		/*
+		 * we got to this node from this single source port,
+		 * there are no sink pads.
+		 */
+		imxsd->num_sink_pads = 0;
+	}
+
+	if (imxsd->num_sink_pads >= num_pads)
+		return ERR_PTR(-EINVAL);
+
+	imxsd->num_src_pads = num_pads - imxsd->num_sink_pads;
+
+	dev_dbg(imxmd->md.dev, "%s: %s has %d pads (%d sink, %d src)\n",
+		__func__, sd_np->name, num_pads,
+		imxsd->num_sink_pads, imxsd->num_src_pads);
+
+	/*
+	 * With no sink, this subdev node is the original source
+	 * of video, parse it's media bus for use by the pipeline.
+	 */
+	if (imxsd->num_sink_pads == 0)
+		of_parse_sensor(imxmd, imxsd, sd_np);
+
+	for (i = 0; i < num_pads; i++) {
+		struct device_node *epnode = NULL, *port, *remote_np;
+		struct imx_media_subdev *remote_imxsd;
+		struct imx_media_pad *pad;
+		int remote_pad;
+
+		/* init this pad */
+		pad = &imxsd->pad[i];
+		pad->pad.flags = (i < imxsd->num_sink_pads) ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+
+		if (is_csi_port)
+			port = (i < imxsd->num_sink_pads) ? sd_np : NULL;
+		else
+			port = of_graph_get_port_by_id(sd_np, i);
+		if (!port)
+			continue;
+
+		for_each_child_of_node(port, epnode) {
+			of_get_remote_pad(epnode, &remote_np, &remote_pad);
+			if (!remote_np)
+				continue;
+
+			ret = of_add_pad_link(imxmd, pad, sd_np, remote_np,
+					      i, remote_pad);
+			if (ret) {
+				imxsd = ERR_PTR(ret);
+				break;
+			}
+
+			if (i < imxsd->num_sink_pads) {
+				/* follow sink endpoints upstream */
+				remote_imxsd = of_parse_subdev(imxmd,
+							       remote_np,
+							       false);
+				if (IS_ERR(remote_imxsd)) {
+					imxsd = remote_imxsd;
+					break;
+				}
+			}
+
+			of_node_put(remote_np);
+		}
+
+		if (port != sd_np)
+			of_node_put(port);
+		if (IS_ERR(imxsd)) {
+			of_node_put(remote_np);
+			of_node_put(epnode);
+			break;
+		}
+	}
+
+	return imxsd;
+}
+
+int imx_media_of_parse(struct imx_media_dev *imxmd,
+		       struct imx_media_subdev *(*csi)[4],
+		       struct device_node *np)
+{
+	struct imx_media_subdev *lcsi;
+	struct device_node *csi_np;
+	u32 ipu_id, csi_id;
+	int i, ret;
+
+	for (i = 0; ; i++) {
+		csi_np = of_parse_phandle(np, "ports", i);
+		if (!csi_np)
+			break;
+
+		lcsi = of_parse_subdev(imxmd, csi_np, true);
+		if (IS_ERR(lcsi)) {
+			ret = PTR_ERR(lcsi);
+			goto err_put;
+		}
+
+		ret = of_property_read_u32(csi_np, "reg", &csi_id);
+		if (ret) {
+			dev_err(imxmd->md.dev,
+				"%s: csi port missing reg property!\n",
+				__func__);
+			goto err_put;
+		}
+
+		ipu_id = of_alias_get_id(csi_np->parent, "ipu");
+		of_node_put(csi_np);
+
+		if (ipu_id > 1 || csi_id > 1) {
+			dev_err(imxmd->md.dev,
+				"%s: invalid ipu/csi id (%u/%u)\n",
+				__func__, ipu_id, csi_id);
+			return -EINVAL;
+		}
+
+		(*csi)[ipu_id * 2 + csi_id] = lcsi;
+	}
+
+	return 0;
+err_put:
+	of_node_put(csi_np);
+	return ret;
+}
diff --git a/drivers/staging/media/imx/imx-media-utils.c b/drivers/staging/media/imx/imx-media-utils.c
new file mode 100644
index 000000000000..59523872a886
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-utils.c
@@ -0,0 +1,896 @@
+/*
+ * V4L2 Media Controller Driver for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2016 Mentor Graphics Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/module.h>
+#include "imx-media.h"
+
+/*
+ * List of supported pixel formats for the subdevs.
+ *
+ * In all of these tables, the non-mbus formats (with no
+ * mbus codes) must all fall at the end of the table.
+ */
+
+static const struct imx_media_pixfmt yuv_formats[] = {
+	{
+		.fourcc	= V4L2_PIX_FMT_UYVY,
+		.codes  = {
+			MEDIA_BUS_FMT_UYVY8_2X8,
+			MEDIA_BUS_FMT_UYVY8_1X16
+		},
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 16,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_YUYV,
+		.codes  = {
+			MEDIA_BUS_FMT_YUYV8_2X8,
+			MEDIA_BUS_FMT_YUYV8_1X16
+		},
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 16,
+	},
+	/***
+	 * non-mbus YUV formats start here. NOTE! when adding non-mbus
+	 * formats, NUM_NON_MBUS_YUV_FORMATS must be updated below.
+	 ***/
+	{
+		.fourcc	= V4L2_PIX_FMT_YUV420,
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 12,
+		.planar = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_YVU420,
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 12,
+		.planar = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_YUV422P,
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 16,
+		.planar = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_NV12,
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 12,
+		.planar = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_NV16,
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 16,
+		.planar = true,
+	},
+};
+
+#define NUM_NON_MBUS_YUV_FORMATS 5
+#define NUM_YUV_FORMATS ARRAY_SIZE(yuv_formats)
+#define NUM_MBUS_YUV_FORMATS (NUM_YUV_FORMATS - NUM_NON_MBUS_YUV_FORMATS)
+
+static const struct imx_media_pixfmt rgb_formats[] = {
+	{
+		.fourcc	= V4L2_PIX_FMT_RGB565,
+		.codes  = {MEDIA_BUS_FMT_RGB565_2X8_LE},
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 16,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_RGB24,
+		.codes  = {
+			MEDIA_BUS_FMT_RGB888_1X24,
+			MEDIA_BUS_FMT_RGB888_2X12_LE
+		},
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 24,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_RGB32,
+		.codes  = {MEDIA_BUS_FMT_ARGB8888_1X32},
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 32,
+		.ipufmt = true,
+	},
+	/*** raw bayer formats start here ***/
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR8,
+		.codes  = {MEDIA_BUS_FMT_SBGGR8_1X8},
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 8,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGBRG8,
+		.codes  = {MEDIA_BUS_FMT_SGBRG8_1X8},
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 8,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGRBG8,
+		.codes  = {MEDIA_BUS_FMT_SGRBG8_1X8},
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 8,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SRGGB8,
+		.codes  = {MEDIA_BUS_FMT_SRGGB8_1X8},
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 8,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SBGGR16,
+		.codes  = {
+			MEDIA_BUS_FMT_SBGGR10_1X10,
+			MEDIA_BUS_FMT_SBGGR12_1X12,
+			MEDIA_BUS_FMT_SBGGR14_1X14,
+			MEDIA_BUS_FMT_SBGGR16_1X16
+		},
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 16,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGBRG16,
+		.codes  = {
+			MEDIA_BUS_FMT_SGBRG10_1X10,
+			MEDIA_BUS_FMT_SGBRG12_1X12,
+			MEDIA_BUS_FMT_SGBRG14_1X14,
+			MEDIA_BUS_FMT_SGBRG16_1X16,
+		},
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 16,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SGRBG16,
+		.codes  = {
+			MEDIA_BUS_FMT_SGRBG10_1X10,
+			MEDIA_BUS_FMT_SGRBG12_1X12,
+			MEDIA_BUS_FMT_SGRBG14_1X14,
+			MEDIA_BUS_FMT_SGRBG16_1X16,
+		},
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 16,
+		.bayer  = true,
+	}, {
+		.fourcc = V4L2_PIX_FMT_SRGGB16,
+		.codes  = {
+			MEDIA_BUS_FMT_SRGGB10_1X10,
+			MEDIA_BUS_FMT_SRGGB12_1X12,
+			MEDIA_BUS_FMT_SRGGB14_1X14,
+			MEDIA_BUS_FMT_SRGGB16_1X16,
+		},
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 16,
+		.bayer  = true,
+	},
+	/***
+	 * non-mbus RGB formats start here. NOTE! when adding non-mbus
+	 * formats, NUM_NON_MBUS_RGB_FORMATS must be updated below.
+	 ***/
+	{
+		.fourcc	= V4L2_PIX_FMT_BGR24,
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 24,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_BGR32,
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 32,
+	},
+};
+
+#define NUM_NON_MBUS_RGB_FORMATS 2
+#define NUM_RGB_FORMATS ARRAY_SIZE(rgb_formats)
+#define NUM_MBUS_RGB_FORMATS (NUM_RGB_FORMATS - NUM_NON_MBUS_RGB_FORMATS)
+
+static const struct imx_media_pixfmt ipu_yuv_formats[] = {
+	{
+		.fourcc = V4L2_PIX_FMT_YUV32,
+		.codes  = {MEDIA_BUS_FMT_AYUV8_1X32},
+		.cs     = IPUV3_COLORSPACE_YUV,
+		.bpp    = 32,
+		.ipufmt = true,
+	},
+};
+
+#define NUM_IPU_YUV_FORMATS ARRAY_SIZE(ipu_yuv_formats)
+
+static const struct imx_media_pixfmt ipu_rgb_formats[] = {
+	{
+		.fourcc	= V4L2_PIX_FMT_RGB32,
+		.codes  = {MEDIA_BUS_FMT_ARGB8888_1X32},
+		.cs     = IPUV3_COLORSPACE_RGB,
+		.bpp    = 32,
+		.ipufmt = true,
+	},
+};
+
+#define NUM_IPU_RGB_FORMATS ARRAY_SIZE(ipu_rgb_formats)
+
+static void init_mbus_colorimetry(struct v4l2_mbus_framefmt *mbus,
+				  const struct imx_media_pixfmt *fmt)
+{
+	mbus->colorspace = (fmt->cs == IPUV3_COLORSPACE_RGB) ?
+		V4L2_COLORSPACE_SRGB : V4L2_COLORSPACE_SMPTE170M;
+	mbus->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(mbus->colorspace);
+	mbus->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(mbus->colorspace);
+	mbus->quantization =
+		V4L2_MAP_QUANTIZATION_DEFAULT(fmt->cs == IPUV3_COLORSPACE_RGB,
+					      mbus->colorspace,
+					      mbus->ycbcr_enc);
+}
+
+static const struct imx_media_pixfmt *find_format(u32 fourcc,
+						  u32 code,
+						  enum codespace_sel cs_sel,
+						  bool allow_non_mbus,
+						  bool allow_bayer)
+{
+	const struct imx_media_pixfmt *array, *fmt, *ret = NULL;
+	u32 array_size;
+	int i, j;
+
+	switch (cs_sel) {
+	case CS_SEL_YUV:
+		array_size = NUM_YUV_FORMATS;
+		array = yuv_formats;
+		break;
+	case CS_SEL_RGB:
+		array_size = NUM_RGB_FORMATS;
+		array = rgb_formats;
+		break;
+	case CS_SEL_ANY:
+		array_size = NUM_YUV_FORMATS + NUM_RGB_FORMATS;
+		array = yuv_formats;
+		break;
+	default:
+		return NULL;
+	}
+
+	for (i = 0; i < array_size; i++) {
+		if (cs_sel == CS_SEL_ANY && i >= NUM_YUV_FORMATS)
+			fmt = &rgb_formats[i - NUM_YUV_FORMATS];
+		else
+			fmt = &array[i];
+
+		if ((!allow_non_mbus && fmt->codes[0] == 0) ||
+		    (!allow_bayer && fmt->bayer))
+			continue;
+
+		if (fourcc && fmt->fourcc == fourcc) {
+			ret = fmt;
+			goto out;
+		}
+
+		for (j = 0; code && fmt->codes[j]; j++) {
+			if (code == fmt->codes[j]) {
+				ret = fmt;
+				goto out;
+			}
+		}
+	}
+
+out:
+	return ret;
+}
+
+static int enum_format(u32 *fourcc, u32 *code, u32 index,
+		       enum codespace_sel cs_sel,
+		       bool allow_non_mbus,
+		       bool allow_bayer)
+{
+	const struct imx_media_pixfmt *fmt;
+	u32 mbus_yuv_sz = NUM_MBUS_YUV_FORMATS;
+	u32 mbus_rgb_sz = NUM_MBUS_RGB_FORMATS;
+	u32 yuv_sz = NUM_YUV_FORMATS;
+	u32 rgb_sz = NUM_RGB_FORMATS;
+
+	switch (cs_sel) {
+	case CS_SEL_YUV:
+		if (index >= yuv_sz ||
+		    (!allow_non_mbus && index >= mbus_yuv_sz))
+			return -EINVAL;
+		fmt = &yuv_formats[index];
+		break;
+	case CS_SEL_RGB:
+		if (index >= rgb_sz ||
+		    (!allow_non_mbus && index >= mbus_rgb_sz))
+			return -EINVAL;
+		fmt = &rgb_formats[index];
+		if (!allow_bayer && fmt->bayer)
+			return -EINVAL;
+		break;
+	case CS_SEL_ANY:
+		if (!allow_non_mbus) {
+			if (index >= mbus_yuv_sz) {
+				index -= mbus_yuv_sz;
+				if (index >= mbus_rgb_sz)
+					return -EINVAL;
+				fmt = &rgb_formats[index];
+				if (!allow_bayer && fmt->bayer)
+					return -EINVAL;
+			} else {
+				fmt = &yuv_formats[index];
+			}
+		} else {
+			if (index >= yuv_sz + rgb_sz)
+				return -EINVAL;
+			if (index >= yuv_sz) {
+				fmt = &rgb_formats[index - yuv_sz];
+				if (!allow_bayer && fmt->bayer)
+					return -EINVAL;
+			} else {
+				fmt = &yuv_formats[index];
+			}
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (fourcc)
+		*fourcc = fmt->fourcc;
+	if (code)
+		*code = fmt->codes[0];
+
+	return 0;
+}
+
+const struct imx_media_pixfmt *
+imx_media_find_format(u32 fourcc, enum codespace_sel cs_sel, bool allow_bayer)
+{
+	return find_format(fourcc, 0, cs_sel, true, allow_bayer);
+}
+EXPORT_SYMBOL_GPL(imx_media_find_format);
+
+int imx_media_enum_format(u32 *fourcc, u32 index, enum codespace_sel cs_sel)
+{
+	return enum_format(fourcc, NULL, index, cs_sel, true, false);
+}
+EXPORT_SYMBOL_GPL(imx_media_enum_format);
+
+const struct imx_media_pixfmt *
+imx_media_find_mbus_format(u32 code, enum codespace_sel cs_sel,
+			   bool allow_bayer)
+{
+	return find_format(0, code, cs_sel, false, allow_bayer);
+}
+EXPORT_SYMBOL_GPL(imx_media_find_mbus_format);
+
+int imx_media_enum_mbus_format(u32 *code, u32 index, enum codespace_sel cs_sel,
+			       bool allow_bayer)
+{
+	return enum_format(NULL, code, index, cs_sel, false, allow_bayer);
+}
+EXPORT_SYMBOL_GPL(imx_media_enum_mbus_format);
+
+const struct imx_media_pixfmt *
+imx_media_find_ipu_format(u32 code, enum codespace_sel cs_sel)
+{
+	const struct imx_media_pixfmt *array, *fmt, *ret = NULL;
+	u32 array_size;
+	int i, j;
+
+	switch (cs_sel) {
+	case CS_SEL_YUV:
+		array_size = NUM_IPU_YUV_FORMATS;
+		array = ipu_yuv_formats;
+		break;
+	case CS_SEL_RGB:
+		array_size = NUM_IPU_RGB_FORMATS;
+		array = ipu_rgb_formats;
+		break;
+	case CS_SEL_ANY:
+		array_size = NUM_IPU_YUV_FORMATS + NUM_IPU_RGB_FORMATS;
+		array = ipu_yuv_formats;
+		break;
+	default:
+		return NULL;
+	}
+
+	for (i = 0; i < array_size; i++) {
+		if (cs_sel == CS_SEL_ANY && i >= NUM_IPU_YUV_FORMATS)
+			fmt = &ipu_rgb_formats[i - NUM_IPU_YUV_FORMATS];
+		else
+			fmt = &array[i];
+
+		for (j = 0; code && fmt->codes[j]; j++) {
+			if (code == fmt->codes[j]) {
+				ret = fmt;
+				goto out;
+			}
+		}
+	}
+
+out:
+	return ret;
+}
+EXPORT_SYMBOL_GPL(imx_media_find_ipu_format);
+
+int imx_media_enum_ipu_format(u32 *code, u32 index, enum codespace_sel cs_sel)
+{
+	switch (cs_sel) {
+	case CS_SEL_YUV:
+		if (index >= NUM_IPU_YUV_FORMATS)
+			return -EINVAL;
+		*code = ipu_yuv_formats[index].codes[0];
+		break;
+	case CS_SEL_RGB:
+		if (index >= NUM_IPU_RGB_FORMATS)
+			return -EINVAL;
+		*code = ipu_rgb_formats[index].codes[0];
+		break;
+	case CS_SEL_ANY:
+		if (index >= NUM_IPU_YUV_FORMATS + NUM_IPU_RGB_FORMATS)
+			return -EINVAL;
+		if (index >= NUM_IPU_YUV_FORMATS) {
+			index -= NUM_IPU_YUV_FORMATS;
+			*code = ipu_rgb_formats[index].codes[0];
+		} else {
+			*code = ipu_yuv_formats[index].codes[0];
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_enum_ipu_format);
+
+int imx_media_init_mbus_fmt(struct v4l2_mbus_framefmt *mbus,
+			    u32 width, u32 height, u32 code, u32 field,
+			    const struct imx_media_pixfmt **cc)
+{
+	const struct imx_media_pixfmt *lcc;
+
+	mbus->width = width;
+	mbus->height = height;
+	mbus->field = field;
+	if (code == 0)
+		imx_media_enum_mbus_format(&code, 0, CS_SEL_YUV, false);
+	lcc = imx_media_find_mbus_format(code, CS_SEL_ANY, false);
+	if (!lcc) {
+		lcc = imx_media_find_ipu_format(code, CS_SEL_ANY);
+		if (!lcc)
+			return -EINVAL;
+	}
+
+	mbus->code = code;
+	init_mbus_colorimetry(mbus, lcc);
+	if (cc)
+		*cc = lcc;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_init_mbus_fmt);
+
+/*
+ * Check whether the field and colorimetry parameters in tryfmt are
+ * uninitialized, and if so fill them with the values from fmt,
+ * or if tryfmt->colorspace has been initialized, all the default
+ * colorimetry params can be derived from tryfmt->colorspace.
+ *
+ * tryfmt->code must be set on entry.
+ *
+ * If this format is destined to be routed through the Image Converter,
+ * quantization and Y`CbCr encoding must be fixed. The IC expects and
+ * produces fixed quantization and Y`CbCr encoding at its input and output
+ * (full range for RGB, limited range for YUV, and V4L2_YCBCR_ENC_601).
+ */
+void imx_media_fill_default_mbus_fields(struct v4l2_mbus_framefmt *tryfmt,
+					struct v4l2_mbus_framefmt *fmt,
+					bool ic_route)
+{
+	const struct imx_media_pixfmt *cc;
+	bool is_rgb = false;
+
+	cc = imx_media_find_mbus_format(tryfmt->code, CS_SEL_ANY, true);
+	if (!cc)
+		cc = imx_media_find_ipu_format(tryfmt->code, CS_SEL_ANY);
+	if (cc && cc->cs != IPUV3_COLORSPACE_YUV)
+		is_rgb = true;
+
+	/* fill field if necessary */
+	if (tryfmt->field == V4L2_FIELD_ANY)
+		tryfmt->field = fmt->field;
+
+	/* fill colorimetry if necessary */
+	if (tryfmt->colorspace == V4L2_COLORSPACE_DEFAULT) {
+		tryfmt->colorspace = fmt->colorspace;
+		tryfmt->xfer_func = fmt->xfer_func;
+		tryfmt->ycbcr_enc = fmt->ycbcr_enc;
+		tryfmt->quantization = fmt->quantization;
+	} else {
+		if (tryfmt->xfer_func == V4L2_XFER_FUNC_DEFAULT) {
+			tryfmt->xfer_func =
+				V4L2_MAP_XFER_FUNC_DEFAULT(tryfmt->colorspace);
+		}
+		if (tryfmt->ycbcr_enc == V4L2_YCBCR_ENC_DEFAULT) {
+			tryfmt->ycbcr_enc =
+				V4L2_MAP_YCBCR_ENC_DEFAULT(tryfmt->colorspace);
+		}
+		if (tryfmt->quantization == V4L2_QUANTIZATION_DEFAULT) {
+			tryfmt->quantization =
+				V4L2_MAP_QUANTIZATION_DEFAULT(
+					is_rgb, tryfmt->colorspace,
+					tryfmt->ycbcr_enc);
+		}
+	}
+
+	if (ic_route) {
+		tryfmt->quantization = is_rgb ?
+			V4L2_QUANTIZATION_FULL_RANGE :
+			V4L2_QUANTIZATION_LIM_RANGE;
+		tryfmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+	}
+}
+EXPORT_SYMBOL_GPL(imx_media_fill_default_mbus_fields);
+
+int imx_media_mbus_fmt_to_pix_fmt(struct v4l2_pix_format *pix,
+				  struct v4l2_mbus_framefmt *mbus,
+				  const struct imx_media_pixfmt *cc)
+{
+	u32 stride;
+
+	if (!cc) {
+		cc = imx_media_find_ipu_format(mbus->code, CS_SEL_ANY);
+		if (!cc)
+			cc = imx_media_find_mbus_format(mbus->code, CS_SEL_ANY,
+							true);
+		if (!cc)
+			return -EINVAL;
+	}
+
+	/*
+	 * TODO: the IPU currently does not support the AYUV32 format,
+	 * so until it does convert to a supported YUV format.
+	 */
+	if (cc->ipufmt && cc->cs == IPUV3_COLORSPACE_YUV) {
+		u32 code;
+
+		imx_media_enum_mbus_format(&code, 0, CS_SEL_YUV, false);
+		cc = imx_media_find_mbus_format(code, CS_SEL_YUV, false);
+	}
+
+	stride = cc->planar ? mbus->width : (mbus->width * cc->bpp) >> 3;
+
+	pix->width = mbus->width;
+	pix->height = mbus->height;
+	pix->pixelformat = cc->fourcc;
+	pix->colorspace = mbus->colorspace;
+	pix->xfer_func = mbus->xfer_func;
+	pix->ycbcr_enc = mbus->ycbcr_enc;
+	pix->quantization = mbus->quantization;
+	pix->field = mbus->field;
+	pix->bytesperline = stride;
+	pix->sizeimage = (pix->width * pix->height * cc->bpp) >> 3;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_mbus_fmt_to_pix_fmt);
+
+int imx_media_mbus_fmt_to_ipu_image(struct ipu_image *image,
+				    struct v4l2_mbus_framefmt *mbus)
+{
+	int ret;
+
+	memset(image, 0, sizeof(*image));
+
+	ret = imx_media_mbus_fmt_to_pix_fmt(&image->pix, mbus, NULL);
+	if (ret)
+		return ret;
+
+	image->rect.width = mbus->width;
+	image->rect.height = mbus->height;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_mbus_fmt_to_ipu_image);
+
+int imx_media_ipu_image_to_mbus_fmt(struct v4l2_mbus_framefmt *mbus,
+				    struct ipu_image *image)
+{
+	const struct imx_media_pixfmt *fmt;
+
+	fmt = imx_media_find_format(image->pix.pixelformat, CS_SEL_ANY, true);
+	if (!fmt)
+		return -EINVAL;
+
+	memset(mbus, 0, sizeof(*mbus));
+	mbus->width = image->pix.width;
+	mbus->height = image->pix.height;
+	mbus->code = fmt->codes[0];
+	mbus->field = image->pix.field;
+	mbus->colorspace = image->pix.colorspace;
+	mbus->xfer_func = image->pix.xfer_func;
+	mbus->ycbcr_enc = image->pix.ycbcr_enc;
+	mbus->quantization = image->pix.quantization;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_ipu_image_to_mbus_fmt);
+
+void imx_media_free_dma_buf(struct imx_media_dev *imxmd,
+			    struct imx_media_dma_buf *buf)
+{
+	if (buf->virt)
+		dma_free_coherent(imxmd->md.dev, buf->len,
+				  buf->virt, buf->phys);
+
+	buf->virt = NULL;
+	buf->phys = 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_free_dma_buf);
+
+int imx_media_alloc_dma_buf(struct imx_media_dev *imxmd,
+			    struct imx_media_dma_buf *buf,
+			    int size)
+{
+	imx_media_free_dma_buf(imxmd, buf);
+
+	buf->len = PAGE_ALIGN(size);
+	buf->virt = dma_alloc_coherent(imxmd->md.dev, buf->len, &buf->phys,
+				       GFP_DMA | GFP_KERNEL);
+	if (!buf->virt) {
+		dev_err(imxmd->md.dev, "failed to alloc dma buffer\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(imx_media_alloc_dma_buf);
+
+/* form a subdev name given a group id and ipu id */
+void imx_media_grp_id_to_sd_name(char *sd_name, int sz, u32 grp_id, int ipu_id)
+{
+	int id;
+
+	switch (grp_id) {
+	case IMX_MEDIA_GRP_ID_CSI0...IMX_MEDIA_GRP_ID_CSI1:
+		id = (grp_id >> IMX_MEDIA_GRP_ID_CSI_BIT) - 1;
+		snprintf(sd_name, sz, "ipu%d_csi%d", ipu_id + 1, id);
+		break;
+	case IMX_MEDIA_GRP_ID_VDIC:
+		snprintf(sd_name, sz, "ipu%d_vdic", ipu_id + 1);
+		break;
+	case IMX_MEDIA_GRP_ID_IC_PRP:
+		snprintf(sd_name, sz, "ipu%d_ic_prp", ipu_id + 1);
+		break;
+	case IMX_MEDIA_GRP_ID_IC_PRPENC:
+		snprintf(sd_name, sz, "ipu%d_ic_prpenc", ipu_id + 1);
+		break;
+	case IMX_MEDIA_GRP_ID_IC_PRPVF:
+		snprintf(sd_name, sz, "ipu%d_ic_prpvf", ipu_id + 1);
+		break;
+	default:
+		break;
+	}
+}
+EXPORT_SYMBOL_GPL(imx_media_grp_id_to_sd_name);
+
+struct imx_media_subdev *
+imx_media_find_subdev_by_sd(struct imx_media_dev *imxmd,
+			    struct v4l2_subdev *sd)
+{
+	struct imx_media_subdev *imxsd;
+	int i;
+
+	for (i = 0; i < imxmd->num_subdevs; i++) {
+		imxsd = &imxmd->subdev[i];
+		if (sd == imxsd->sd)
+			return imxsd;
+	}
+
+	return ERR_PTR(-ENODEV);
+}
+EXPORT_SYMBOL_GPL(imx_media_find_subdev_by_sd);
+
+struct imx_media_subdev *
+imx_media_find_subdev_by_id(struct imx_media_dev *imxmd, u32 grp_id)
+{
+	struct imx_media_subdev *imxsd;
+	int i;
+
+	for (i = 0; i < imxmd->num_subdevs; i++) {
+		imxsd = &imxmd->subdev[i];
+		if (imxsd->sd && imxsd->sd->grp_id == grp_id)
+			return imxsd;
+	}
+
+	return ERR_PTR(-ENODEV);
+}
+EXPORT_SYMBOL_GPL(imx_media_find_subdev_by_id);
+
+/*
+ * Adds a video device to the master video device list. This is called by
+ * an async subdev that owns a video device when it is registered.
+ */
+int imx_media_add_video_device(struct imx_media_dev *imxmd,
+			       struct imx_media_video_dev *vdev)
+{
+	int vdev_idx, ret = 0;
+
+	mutex_lock(&imxmd->mutex);
+
+	vdev_idx = imxmd->num_vdevs;
+	if (vdev_idx >= IMX_MEDIA_MAX_VDEVS) {
+		dev_err(imxmd->md.dev,
+			"%s: too many video devices! can't add %s\n",
+			__func__, vdev->vfd->name);
+		ret = -ENOSPC;
+		goto out;
+	}
+
+	imxmd->vdev[vdev_idx] = vdev;
+	imxmd->num_vdevs++;
+out:
+	mutex_unlock(&imxmd->mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(imx_media_add_video_device);
+
+/*
+ * Search upstream or downstream for a subdevice in the current pipeline
+ * with given grp_id, starting from start_entity. Returns the subdev's
+ * source/sink pad that it was reached from. Must be called with
+ * mdev->graph_mutex held.
+ */
+static struct media_pad *
+find_pipeline_pad(struct imx_media_dev *imxmd,
+		  struct media_entity *start_entity,
+		  u32 grp_id, bool upstream)
+{
+	struct media_entity *me = start_entity;
+	struct media_pad *pad = NULL;
+	struct v4l2_subdev *sd;
+	int i;
+
+	for (i = 0; i < me->num_pads; i++) {
+		struct media_pad *spad = &me->pads[i];
+
+		if ((upstream && !(spad->flags & MEDIA_PAD_FL_SINK)) ||
+		    (!upstream && !(spad->flags & MEDIA_PAD_FL_SOURCE)))
+			continue;
+
+		pad = media_entity_remote_pad(spad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			continue;
+
+		sd = media_entity_to_v4l2_subdev(pad->entity);
+		if (sd->grp_id & grp_id)
+			return pad;
+
+		return find_pipeline_pad(imxmd, pad->entity, grp_id, upstream);
+	}
+
+	return NULL;
+}
+
+/*
+ * Search upstream for a subdev in the current pipeline with
+ * given grp_id. Must be called with mdev->graph_mutex held.
+ */
+static struct v4l2_subdev *
+find_upstream_subdev(struct imx_media_dev *imxmd,
+		     struct media_entity *start_entity,
+		     u32 grp_id)
+{
+	struct v4l2_subdev *sd;
+	struct media_pad *pad;
+
+	if (is_media_entity_v4l2_subdev(start_entity)) {
+		sd = media_entity_to_v4l2_subdev(start_entity);
+		if (sd->grp_id & grp_id)
+			return sd;
+	}
+
+	pad = find_pipeline_pad(imxmd, start_entity, grp_id, true);
+
+	return pad ? media_entity_to_v4l2_subdev(pad->entity) : NULL;
+}
+
+
+/*
+ * Find the upstream mipi-csi2 virtual channel reached from the given
+ * start entity in the current pipeline.
+ * Must be called with mdev->graph_mutex held.
+ */
+int imx_media_find_mipi_csi2_channel(struct imx_media_dev *imxmd,
+				     struct media_entity *start_entity)
+{
+	struct media_pad *pad;
+	int ret = -EPIPE;
+
+	pad = find_pipeline_pad(imxmd, start_entity, IMX_MEDIA_GRP_ID_CSI2,
+				true);
+	if (pad) {
+		ret = pad->index - 1;
+		dev_dbg(imxmd->md.dev, "found vc%d from %s\n",
+			ret, start_entity->name);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(imx_media_find_mipi_csi2_channel);
+
+/*
+ * Find a subdev reached upstream from the given start entity in
+ * the current pipeline.
+ * Must be called with mdev->graph_mutex held.
+ */
+struct imx_media_subdev *
+imx_media_find_upstream_subdev(struct imx_media_dev *imxmd,
+			       struct media_entity *start_entity,
+			       u32 grp_id)
+{
+	struct v4l2_subdev *sd;
+
+	sd = find_upstream_subdev(imxmd, start_entity, grp_id);
+	if (!sd)
+		return ERR_PTR(-ENODEV);
+
+	return imx_media_find_subdev_by_sd(imxmd, sd);
+}
+EXPORT_SYMBOL_GPL(imx_media_find_upstream_subdev);
+
+struct imx_media_subdev *
+__imx_media_find_sensor(struct imx_media_dev *imxmd,
+			struct media_entity *start_entity)
+{
+	return imx_media_find_upstream_subdev(imxmd, start_entity,
+					      IMX_MEDIA_GRP_ID_SENSOR);
+}
+EXPORT_SYMBOL_GPL(__imx_media_find_sensor);
+
+struct imx_media_subdev *
+imx_media_find_sensor(struct imx_media_dev *imxmd,
+		      struct media_entity *start_entity)
+{
+	struct imx_media_subdev *sensor;
+
+	mutex_lock(&imxmd->md.graph_mutex);
+	sensor = __imx_media_find_sensor(imxmd, start_entity);
+	mutex_unlock(&imxmd->md.graph_mutex);
+
+	return sensor;
+}
+EXPORT_SYMBOL_GPL(imx_media_find_sensor);
+
+/*
+ * Turn current pipeline streaming on/off starting from entity.
+ */
+int imx_media_pipeline_set_stream(struct imx_media_dev *imxmd,
+				  struct media_entity *entity,
+				  bool on)
+{
+	struct v4l2_subdev *sd;
+	int ret = 0;
+
+	if (!is_media_entity_v4l2_subdev(entity))
+		return -EINVAL;
+	sd = media_entity_to_v4l2_subdev(entity);
+
+	mutex_lock(&imxmd->md.graph_mutex);
+
+	if (on) {
+		ret = __media_pipeline_start(entity, &imxmd->pipe);
+		if (ret)
+			goto out;
+		ret = v4l2_subdev_call(sd, video, s_stream, 1);
+		if (ret)
+			__media_pipeline_stop(entity);
+	} else {
+		v4l2_subdev_call(sd, video, s_stream, 0);
+		if (entity->pipe)
+			__media_pipeline_stop(entity);
+	}
+
+out:
+	mutex_unlock(&imxmd->md.graph_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(imx_media_pipeline_set_stream);
+
+MODULE_DESCRIPTION("i.MX5/6 v4l2 media controller driver");
+MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/imx/imx-media-vdic.c b/drivers/staging/media/imx/imx-media-vdic.c
new file mode 100644
index 000000000000..7eabdc4aa79f
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media-vdic.c
@@ -0,0 +1,1009 @@
+/*
+ * V4L2 Deinterlacer Subdev for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2017 Mentor Graphics Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/timer.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+#include <media/imx.h>
+#include "imx-media.h"
+
+/*
+ * This subdev implements two different video pipelines:
+ *
+ * CSI -> VDIC
+ *
+ * In this pipeline, the CSI sends a single interlaced field F(n-1)
+ * directly to the VDIC (and optionally the following field F(n)
+ * can be sent to memory via IDMAC channel 13). This pipeline only works
+ * in VDIC's high motion mode, which only requires a single field for
+ * processing. The other motion modes (low and medium) require three
+ * fields, so this pipeline does not work in those modes. Also, it is
+ * not clear how this pipeline can deal with the various field orders
+ * (sequential BT/TB, interlaced BT/TB).
+ *
+ * MEM -> CH8,9,10 -> VDIC
+ *
+ * In this pipeline, previous field F(n-1), current field F(n), and next
+ * field F(n+1) are transferred to the VDIC via IDMAC channels 8,9,10.
+ * These memory buffers can come from a video output or mem2mem device.
+ * All motion modes are supported by this pipeline.
+ *
+ * The "direct" CSI->VDIC pipeline requires no DMA, but it can only be
+ * used in high motion mode.
+ */
+
+struct vdic_priv;
+
+struct vdic_pipeline_ops {
+	int (*setup)(struct vdic_priv *priv);
+	void (*start)(struct vdic_priv *priv);
+	void (*stop)(struct vdic_priv *priv);
+	void (*disable)(struct vdic_priv *priv);
+};
+
+/*
+ * Min/Max supported width and heights.
+ */
+#define MIN_W       176
+#define MIN_H       144
+#define MAX_W_VDIC  968
+#define MAX_H_VDIC 2048
+#define W_ALIGN    4 /* multiple of 16 pixels */
+#define H_ALIGN    1 /* multiple of 2 lines */
+#define S_ALIGN    1 /* multiple of 2 */
+
+struct vdic_priv {
+	struct device        *dev;
+	struct ipu_soc       *ipu;
+	struct imx_media_dev *md;
+	struct v4l2_subdev   sd;
+	struct media_pad pad[VDIC_NUM_PADS];
+	int ipu_id;
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	/* IPU units we require */
+	struct ipu_vdi *vdi;
+
+	int active_input_pad;
+
+	struct ipuv3_channel *vdi_in_ch_p; /* F(n-1) transfer channel */
+	struct ipuv3_channel *vdi_in_ch;   /* F(n) transfer channel */
+	struct ipuv3_channel *vdi_in_ch_n; /* F(n+1) transfer channel */
+
+	/* pipeline operations */
+	struct vdic_pipeline_ops *ops;
+
+	/* current and previous input buffers indirect path */
+	struct imx_media_buffer *curr_in_buf;
+	struct imx_media_buffer *prev_in_buf;
+
+	/*
+	 * translated field type, input line stride, and field size
+	 * for indirect path
+	 */
+	u32 fieldtype;
+	u32 in_stride;
+	u32 field_size;
+
+	/* the source (a video device or subdev) */
+	struct media_entity *src;
+	/* the sink that will receive the progressive out buffers */
+	struct v4l2_subdev *sink_sd;
+
+	struct v4l2_mbus_framefmt format_mbus[VDIC_NUM_PADS];
+	const struct imx_media_pixfmt *cc[VDIC_NUM_PADS];
+	struct v4l2_fract frame_interval[VDIC_NUM_PADS];
+
+	/* the video device at IDMAC input pad */
+	struct imx_media_video_dev *vdev;
+
+	bool csi_direct;  /* using direct CSI->VDIC->IC pipeline */
+
+	/* motion select control */
+	struct v4l2_ctrl_handler ctrl_hdlr;
+	enum ipu_motion_sel motion;
+
+	int stream_count;
+};
+
+static void vdic_put_ipu_resources(struct vdic_priv *priv)
+{
+	if (!IS_ERR_OR_NULL(priv->vdi_in_ch_p))
+		ipu_idmac_put(priv->vdi_in_ch_p);
+	priv->vdi_in_ch_p = NULL;
+
+	if (!IS_ERR_OR_NULL(priv->vdi_in_ch))
+		ipu_idmac_put(priv->vdi_in_ch);
+	priv->vdi_in_ch = NULL;
+
+	if (!IS_ERR_OR_NULL(priv->vdi_in_ch_n))
+		ipu_idmac_put(priv->vdi_in_ch_n);
+	priv->vdi_in_ch_n = NULL;
+
+	if (!IS_ERR_OR_NULL(priv->vdi))
+		ipu_vdi_put(priv->vdi);
+	priv->vdi = NULL;
+}
+
+static int vdic_get_ipu_resources(struct vdic_priv *priv)
+{
+	int ret, err_chan;
+
+	priv->ipu = priv->md->ipu[priv->ipu_id];
+
+	priv->vdi = ipu_vdi_get(priv->ipu);
+	if (IS_ERR(priv->vdi)) {
+		v4l2_err(&priv->sd, "failed to get VDIC\n");
+		ret = PTR_ERR(priv->vdi);
+		goto out;
+	}
+
+	if (!priv->csi_direct) {
+		priv->vdi_in_ch_p = ipu_idmac_get(priv->ipu,
+						  IPUV3_CHANNEL_MEM_VDI_PREV);
+		if (IS_ERR(priv->vdi_in_ch_p)) {
+			err_chan = IPUV3_CHANNEL_MEM_VDI_PREV;
+			ret = PTR_ERR(priv->vdi_in_ch_p);
+			goto out_err_chan;
+		}
+
+		priv->vdi_in_ch = ipu_idmac_get(priv->ipu,
+						IPUV3_CHANNEL_MEM_VDI_CUR);
+		if (IS_ERR(priv->vdi_in_ch)) {
+			err_chan = IPUV3_CHANNEL_MEM_VDI_CUR;
+			ret = PTR_ERR(priv->vdi_in_ch);
+			goto out_err_chan;
+		}
+
+		priv->vdi_in_ch_n = ipu_idmac_get(priv->ipu,
+						  IPUV3_CHANNEL_MEM_VDI_NEXT);
+		if (IS_ERR(priv->vdi_in_ch_n)) {
+			err_chan = IPUV3_CHANNEL_MEM_VDI_NEXT;
+			ret = PTR_ERR(priv->vdi_in_ch_n);
+			goto out_err_chan;
+		}
+	}
+
+	return 0;
+
+out_err_chan:
+	v4l2_err(&priv->sd, "could not get IDMAC channel %u\n", err_chan);
+out:
+	vdic_put_ipu_resources(priv);
+	return ret;
+}
+
+/*
+ * This function is currently unused, but will be called when the
+ * output/mem2mem device at the IDMAC input pad sends us a new
+ * buffer. It kicks off the IDMAC read channels to bring in the
+ * buffer fields from memory and begin the conversions.
+ */
+static void __maybe_unused prepare_vdi_in_buffers(struct vdic_priv *priv,
+						  struct imx_media_buffer *curr)
+{
+	dma_addr_t prev_phys, curr_phys, next_phys;
+	struct imx_media_buffer *prev;
+	struct vb2_buffer *curr_vb, *prev_vb;
+	u32 fs = priv->field_size;
+	u32 is = priv->in_stride;
+
+	/* current input buffer is now previous */
+	priv->prev_in_buf = priv->curr_in_buf;
+	priv->curr_in_buf = curr;
+	prev = priv->prev_in_buf ? priv->prev_in_buf : curr;
+
+	prev_vb = &prev->vbuf.vb2_buf;
+	curr_vb = &curr->vbuf.vb2_buf;
+
+	switch (priv->fieldtype) {
+	case V4L2_FIELD_SEQ_TB:
+		prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0);
+		curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + fs;
+		next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
+		break;
+	case V4L2_FIELD_SEQ_BT:
+		prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + fs;
+		curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
+		next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + fs;
+		break;
+	case V4L2_FIELD_INTERLACED_BT:
+		prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + is;
+		curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
+		next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + is;
+		break;
+	default:
+		/* assume V4L2_FIELD_INTERLACED_TB */
+		prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0);
+		curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + is;
+		next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
+		break;
+	}
+
+	ipu_cpmem_set_buffer(priv->vdi_in_ch_p, 0, prev_phys);
+	ipu_cpmem_set_buffer(priv->vdi_in_ch,   0, curr_phys);
+	ipu_cpmem_set_buffer(priv->vdi_in_ch_n, 0, next_phys);
+
+	ipu_idmac_select_buffer(priv->vdi_in_ch_p, 0);
+	ipu_idmac_select_buffer(priv->vdi_in_ch, 0);
+	ipu_idmac_select_buffer(priv->vdi_in_ch_n, 0);
+}
+
+static int setup_vdi_channel(struct vdic_priv *priv,
+			     struct ipuv3_channel *channel,
+			     dma_addr_t phys0, dma_addr_t phys1)
+{
+	struct imx_media_video_dev *vdev = priv->vdev;
+	unsigned int burst_size;
+	struct ipu_image image;
+	int ret;
+
+	ipu_cpmem_zero(channel);
+
+	memset(&image, 0, sizeof(image));
+	image.pix = vdev->fmt.fmt.pix;
+	/* one field to VDIC channels */
+	image.pix.height /= 2;
+	image.rect.width = image.pix.width;
+	image.rect.height = image.pix.height;
+	image.phys0 = phys0;
+	image.phys1 = phys1;
+
+	ret = ipu_cpmem_set_image(channel, &image);
+	if (ret)
+		return ret;
+
+	burst_size = (image.pix.width & 0xf) ? 8 : 16;
+	ipu_cpmem_set_burstsize(channel, burst_size);
+
+	ipu_cpmem_set_axi_id(channel, 1);
+
+	ipu_idmac_set_double_buffer(channel, false);
+
+	return 0;
+}
+
+static int vdic_setup_direct(struct vdic_priv *priv)
+{
+	/* set VDIC to receive from CSI for direct path */
+	ipu_fsu_link(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
+		     IPUV3_CHANNEL_CSI_VDI_PREV);
+
+	return 0;
+}
+
+static void vdic_start_direct(struct vdic_priv *priv)
+{
+}
+
+static void vdic_stop_direct(struct vdic_priv *priv)
+{
+}
+
+static void vdic_disable_direct(struct vdic_priv *priv)
+{
+	ipu_fsu_unlink(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
+		       IPUV3_CHANNEL_CSI_VDI_PREV);
+}
+
+static int vdic_setup_indirect(struct vdic_priv *priv)
+{
+	struct v4l2_mbus_framefmt *infmt;
+	const struct imx_media_pixfmt *incc;
+	int in_size, ret;
+
+	infmt = &priv->format_mbus[VDIC_SINK_PAD_IDMAC];
+	incc = priv->cc[VDIC_SINK_PAD_IDMAC];
+
+	in_size = (infmt->width * incc->bpp * infmt->height) >> 3;
+
+	/* 1/2 full image size */
+	priv->field_size = in_size / 2;
+	priv->in_stride = incc->planar ?
+		infmt->width : (infmt->width * incc->bpp) >> 3;
+
+	priv->prev_in_buf = NULL;
+	priv->curr_in_buf = NULL;
+
+	priv->fieldtype = infmt->field;
+
+	/* init the vdi-in channels */
+	ret = setup_vdi_channel(priv, priv->vdi_in_ch_p, 0, 0);
+	if (ret)
+		return ret;
+	ret = setup_vdi_channel(priv, priv->vdi_in_ch, 0, 0);
+	if (ret)
+		return ret;
+	return setup_vdi_channel(priv, priv->vdi_in_ch_n, 0, 0);
+}
+
+static void vdic_start_indirect(struct vdic_priv *priv)
+{
+	/* enable the channels */
+	ipu_idmac_enable_channel(priv->vdi_in_ch_p);
+	ipu_idmac_enable_channel(priv->vdi_in_ch);
+	ipu_idmac_enable_channel(priv->vdi_in_ch_n);
+}
+
+static void vdic_stop_indirect(struct vdic_priv *priv)
+{
+	/* disable channels */
+	ipu_idmac_disable_channel(priv->vdi_in_ch_p);
+	ipu_idmac_disable_channel(priv->vdi_in_ch);
+	ipu_idmac_disable_channel(priv->vdi_in_ch_n);
+}
+
+static void vdic_disable_indirect(struct vdic_priv *priv)
+{
+}
+
+static struct vdic_pipeline_ops direct_ops = {
+	.setup = vdic_setup_direct,
+	.start = vdic_start_direct,
+	.stop = vdic_stop_direct,
+	.disable = vdic_disable_direct,
+};
+
+static struct vdic_pipeline_ops indirect_ops = {
+	.setup = vdic_setup_indirect,
+	.start = vdic_start_indirect,
+	.stop = vdic_stop_indirect,
+	.disable = vdic_disable_indirect,
+};
+
+static int vdic_start(struct vdic_priv *priv)
+{
+	struct v4l2_mbus_framefmt *infmt;
+	int ret;
+
+	infmt = &priv->format_mbus[priv->active_input_pad];
+
+	priv->ops = priv->csi_direct ? &direct_ops : &indirect_ops;
+
+	ret = vdic_get_ipu_resources(priv);
+	if (ret)
+		return ret;
+
+	/*
+	 * init the VDIC.
+	 *
+	 * note we don't give infmt->code to ipu_vdi_setup(). The VDIC
+	 * only supports 4:2:2 or 4:2:0, and this subdev will only
+	 * negotiate 4:2:2 at its sink pads.
+	 */
+	ipu_vdi_setup(priv->vdi, MEDIA_BUS_FMT_UYVY8_2X8,
+		      infmt->width, infmt->height);
+	ipu_vdi_set_field_order(priv->vdi, V4L2_STD_UNKNOWN, infmt->field);
+	ipu_vdi_set_motion(priv->vdi, priv->motion);
+
+	ret = priv->ops->setup(priv);
+	if (ret)
+		goto out_put_ipu;
+
+	ipu_vdi_enable(priv->vdi);
+
+	priv->ops->start(priv);
+
+	return 0;
+
+out_put_ipu:
+	vdic_put_ipu_resources(priv);
+	return ret;
+}
+
+static void vdic_stop(struct vdic_priv *priv)
+{
+	priv->ops->stop(priv);
+	ipu_vdi_disable(priv->vdi);
+	priv->ops->disable(priv);
+
+	vdic_put_ipu_resources(priv);
+}
+
+/*
+ * V4L2 subdev operations.
+ */
+
+static int vdic_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vdic_priv *priv = container_of(ctrl->handler,
+					      struct vdic_priv, ctrl_hdlr);
+	enum ipu_motion_sel motion;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	switch (ctrl->id) {
+	case V4L2_CID_DEINTERLACING_MODE:
+		motion = ctrl->val;
+		if (motion != priv->motion) {
+			/* can't change motion control mid-streaming */
+			if (priv->stream_count > 0) {
+				ret = -EBUSY;
+				goto out;
+			}
+			priv->motion = motion;
+		}
+		break;
+	default:
+		v4l2_err(&priv->sd, "Invalid control\n");
+		ret = -EINVAL;
+	}
+
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops vdic_ctrl_ops = {
+	.s_ctrl = vdic_s_ctrl,
+};
+
+static const char * const vdic_ctrl_motion_menu[] = {
+	"No Motion Compensation",
+	"Low Motion",
+	"Medium Motion",
+	"High Motion",
+};
+
+static int vdic_init_controls(struct vdic_priv *priv)
+{
+	struct v4l2_ctrl_handler *hdlr = &priv->ctrl_hdlr;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdlr, 1);
+
+	v4l2_ctrl_new_std_menu_items(hdlr, &vdic_ctrl_ops,
+				     V4L2_CID_DEINTERLACING_MODE,
+				     HIGH_MOTION, 0, HIGH_MOTION,
+				     vdic_ctrl_motion_menu);
+
+	priv->sd.ctrl_handler = hdlr;
+
+	if (hdlr->error) {
+		ret = hdlr->error;
+		goto out_free;
+	}
+
+	v4l2_ctrl_handler_setup(hdlr);
+	return 0;
+
+out_free:
+	v4l2_ctrl_handler_free(hdlr);
+	return ret;
+}
+
+static int vdic_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev *src_sd = NULL;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	if (!priv->src || !priv->sink_sd) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	if (priv->csi_direct)
+		src_sd = media_entity_to_v4l2_subdev(priv->src);
+
+	/*
+	 * enable/disable streaming only if stream_count is
+	 * going from 0 to 1 / 1 to 0.
+	 */
+	if (priv->stream_count != !enable)
+		goto update_count;
+
+	dev_dbg(priv->dev, "stream %s\n", enable ? "ON" : "OFF");
+
+	if (enable)
+		ret = vdic_start(priv);
+	else
+		vdic_stop(priv);
+	if (ret)
+		goto out;
+
+	if (src_sd) {
+		/* start/stop upstream */
+		ret = v4l2_subdev_call(src_sd, video, s_stream, enable);
+		ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
+		if (ret) {
+			if (enable)
+				vdic_stop(priv);
+			goto out;
+		}
+	}
+
+update_count:
+	priv->stream_count += enable ? 1 : -1;
+	if (priv->stream_count < 0)
+		priv->stream_count = 0;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+__vdic_get_fmt(struct vdic_priv *priv, struct v4l2_subdev_pad_config *cfg,
+	       unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&priv->sd, cfg, pad);
+	else
+		return &priv->format_mbus[pad];
+}
+
+static int vdic_enum_mbus_code(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad >= VDIC_NUM_PADS)
+		return -EINVAL;
+
+	return imx_media_enum_ipu_format(&code->code, code->index, CS_SEL_YUV);
+}
+
+static int vdic_get_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *sdformat)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= VDIC_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	fmt = __vdic_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
+	if (!fmt) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	sdformat->format = *fmt;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static void vdic_try_fmt(struct vdic_priv *priv,
+			 struct v4l2_subdev_pad_config *cfg,
+			 struct v4l2_subdev_format *sdformat,
+			 const struct imx_media_pixfmt **cc)
+{
+	struct v4l2_mbus_framefmt *infmt;
+
+	*cc = imx_media_find_ipu_format(sdformat->format.code, CS_SEL_YUV);
+	if (!*cc) {
+		u32 code;
+
+		imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
+		*cc = imx_media_find_ipu_format(code, CS_SEL_YUV);
+		sdformat->format.code = (*cc)->codes[0];
+	}
+
+	infmt = __vdic_get_fmt(priv, cfg, priv->active_input_pad,
+			       sdformat->which);
+
+	switch (sdformat->pad) {
+	case VDIC_SRC_PAD_DIRECT:
+		sdformat->format = *infmt;
+		/* output is always progressive! */
+		sdformat->format.field = V4L2_FIELD_NONE;
+		break;
+	case VDIC_SINK_PAD_DIRECT:
+	case VDIC_SINK_PAD_IDMAC:
+		v4l_bound_align_image(&sdformat->format.width,
+				      MIN_W, MAX_W_VDIC, W_ALIGN,
+				      &sdformat->format.height,
+				      MIN_H, MAX_H_VDIC, H_ALIGN, S_ALIGN);
+
+		imx_media_fill_default_mbus_fields(&sdformat->format, infmt,
+						   true);
+
+		/* input must be interlaced! Choose SEQ_TB if not */
+		if (!V4L2_FIELD_HAS_BOTH(sdformat->format.field))
+			sdformat->format.field = V4L2_FIELD_SEQ_TB;
+		break;
+	}
+}
+
+static int vdic_set_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *sdformat)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	const struct imx_media_pixfmt *cc;
+	struct v4l2_mbus_framefmt *fmt;
+	int ret = 0;
+
+	if (sdformat->pad >= VDIC_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->stream_count > 0) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	vdic_try_fmt(priv, cfg, sdformat, &cc);
+
+	fmt = __vdic_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
+	*fmt = sdformat->format;
+
+	/* propagate format to source pad */
+	if (sdformat->pad == VDIC_SINK_PAD_DIRECT ||
+	    sdformat->pad == VDIC_SINK_PAD_IDMAC) {
+		const struct imx_media_pixfmt *outcc;
+		struct v4l2_mbus_framefmt *outfmt;
+		struct v4l2_subdev_format format;
+
+		format.pad = VDIC_SRC_PAD_DIRECT;
+		format.which = sdformat->which;
+		format.format = sdformat->format;
+		vdic_try_fmt(priv, cfg, &format, &outcc);
+
+		outfmt = __vdic_get_fmt(priv, cfg, VDIC_SRC_PAD_DIRECT,
+					sdformat->which);
+		*outfmt = format.format;
+		if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+			priv->cc[VDIC_SRC_PAD_DIRECT] = outcc;
+	}
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		priv->cc[sdformat->pad] = cc;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int vdic_link_setup(struct media_entity *entity,
+			    const struct media_pad *local,
+			    const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev *remote_sd;
+	int ret = 0;
+
+	dev_dbg(priv->dev, "link setup %s -> %s", remote->entity->name,
+		local->entity->name);
+
+	mutex_lock(&priv->lock);
+
+	if (local->flags & MEDIA_PAD_FL_SOURCE) {
+		if (!is_media_entity_v4l2_subdev(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (priv->sink_sd) {
+				ret = -EBUSY;
+				goto out;
+			}
+			priv->sink_sd = remote_sd;
+		} else {
+			priv->sink_sd = NULL;
+		}
+
+		goto out;
+	}
+
+	/* this is a sink pad */
+
+	if (flags & MEDIA_LNK_FL_ENABLED) {
+		if (priv->src) {
+			ret = -EBUSY;
+			goto out;
+		}
+	} else {
+		priv->src = NULL;
+		goto out;
+	}
+
+	if (local->index == VDIC_SINK_PAD_IDMAC) {
+		struct imx_media_video_dev *vdev = priv->vdev;
+
+		if (!is_media_entity_v4l2_video_device(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+		if (!vdev) {
+			ret = -ENODEV;
+			goto out;
+		}
+
+		priv->csi_direct = false;
+	} else {
+		if (!is_media_entity_v4l2_subdev(remote->entity)) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+
+		/* direct pad must connect to a CSI */
+		if (!(remote_sd->grp_id & IMX_MEDIA_GRP_ID_CSI) ||
+		    remote->index != CSI_SRC_PAD_DIRECT) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		priv->csi_direct = true;
+	}
+
+	priv->src = remote->entity;
+	/* record which input pad is now active */
+	priv->active_input_pad = local->index;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int vdic_link_validate(struct v4l2_subdev *sd,
+			      struct media_link *link,
+			      struct v4l2_subdev_format *source_fmt,
+			      struct v4l2_subdev_format *sink_fmt)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	int ret;
+
+	ret = v4l2_subdev_link_validate_default(sd, link,
+						source_fmt, sink_fmt);
+	if (ret)
+		return ret;
+
+	mutex_lock(&priv->lock);
+
+	if (priv->csi_direct && priv->motion != HIGH_MOTION) {
+		v4l2_err(&priv->sd,
+			 "direct CSI pipeline requires high motion\n");
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+static int vdic_g_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+
+	if (fi->pad >= VDIC_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&priv->lock);
+
+	fi->interval = priv->frame_interval[fi->pad];
+
+	mutex_unlock(&priv->lock);
+
+	return 0;
+}
+
+static int vdic_s_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *fi)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	struct v4l2_fract *input_fi, *output_fi;
+	int ret = 0;
+
+	mutex_lock(&priv->lock);
+
+	input_fi = &priv->frame_interval[priv->active_input_pad];
+	output_fi = &priv->frame_interval[VDIC_SRC_PAD_DIRECT];
+
+	switch (fi->pad) {
+	case VDIC_SINK_PAD_DIRECT:
+	case VDIC_SINK_PAD_IDMAC:
+		/* No limits on input frame interval */
+		/* Reset output interval */
+		*output_fi = fi->interval;
+		if (priv->csi_direct)
+			output_fi->denominator *= 2;
+		break;
+	case VDIC_SRC_PAD_DIRECT:
+		/*
+		 * frame rate at output pad is double input
+		 * rate when using direct CSI->VDIC pipeline.
+		 *
+		 * TODO: implement VDIC frame skipping
+		 */
+		fi->interval = *input_fi;
+		if (priv->csi_direct)
+			fi->interval.denominator *= 2;
+		break;
+	default:
+		ret = -EINVAL;
+		goto out;
+	}
+
+	priv->frame_interval[fi->pad] = fi->interval;
+out:
+	mutex_unlock(&priv->lock);
+	return ret;
+}
+
+/*
+ * retrieve our pads parsed from the OF graph by the media device
+ */
+static int vdic_registered(struct v4l2_subdev *sd)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+	int i, ret;
+	u32 code;
+
+	/* get media device */
+	priv->md = dev_get_drvdata(sd->v4l2_dev->dev);
+
+	for (i = 0; i < VDIC_NUM_PADS; i++) {
+		priv->pad[i].flags = (i == VDIC_SRC_PAD_DIRECT) ?
+			MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
+
+		code = 0;
+		if (i != VDIC_SINK_PAD_IDMAC)
+			imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
+
+		/* set a default mbus format  */
+		ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
+					      640, 480, code, V4L2_FIELD_NONE,
+					      &priv->cc[i]);
+		if (ret)
+			return ret;
+
+		/* init default frame interval */
+		priv->frame_interval[i].numerator = 1;
+		priv->frame_interval[i].denominator = 30;
+		if (i == VDIC_SRC_PAD_DIRECT)
+			priv->frame_interval[i].denominator *= 2;
+	}
+
+	priv->active_input_pad = VDIC_SINK_PAD_DIRECT;
+
+	ret = vdic_init_controls(priv);
+	if (ret)
+		return ret;
+
+	ret = media_entity_pads_init(&sd->entity, VDIC_NUM_PADS, priv->pad);
+	if (ret)
+		v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+
+	return ret;
+}
+
+static void vdic_unregistered(struct v4l2_subdev *sd)
+{
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+
+	v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
+}
+
+static const struct v4l2_subdev_pad_ops vdic_pad_ops = {
+	.enum_mbus_code = vdic_enum_mbus_code,
+	.get_fmt = vdic_get_fmt,
+	.set_fmt = vdic_set_fmt,
+	.link_validate = vdic_link_validate,
+};
+
+static const struct v4l2_subdev_video_ops vdic_video_ops = {
+	.g_frame_interval = vdic_g_frame_interval,
+	.s_frame_interval = vdic_s_frame_interval,
+	.s_stream = vdic_s_stream,
+};
+
+static const struct media_entity_operations vdic_entity_ops = {
+	.link_setup = vdic_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_ops vdic_subdev_ops = {
+	.video = &vdic_video_ops,
+	.pad = &vdic_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops vdic_internal_ops = {
+	.registered = vdic_registered,
+	.unregistered = vdic_unregistered,
+};
+
+static int imx_vdic_probe(struct platform_device *pdev)
+{
+	struct imx_media_internal_sd_platformdata *pdata;
+	struct vdic_priv *priv;
+	int ret;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, &priv->sd);
+	priv->dev = &pdev->dev;
+
+	pdata = priv->dev->platform_data;
+	priv->ipu_id = pdata->ipu_id;
+
+	v4l2_subdev_init(&priv->sd, &vdic_subdev_ops);
+	v4l2_set_subdevdata(&priv->sd, priv);
+	priv->sd.internal_ops = &vdic_internal_ops;
+	priv->sd.entity.ops = &vdic_entity_ops;
+	priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+	priv->sd.dev = &pdev->dev;
+	priv->sd.owner = THIS_MODULE;
+	priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	/* get our group id */
+	priv->sd.grp_id = pdata->grp_id;
+	strncpy(priv->sd.name, pdata->sd_name, sizeof(priv->sd.name));
+
+	mutex_init(&priv->lock);
+
+	ret = v4l2_async_register_subdev(&priv->sd);
+	if (ret)
+		goto free;
+
+	return 0;
+free:
+	mutex_destroy(&priv->lock);
+	return ret;
+}
+
+static int imx_vdic_remove(struct platform_device *pdev)
+{
+	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
+	struct vdic_priv *priv = v4l2_get_subdevdata(sd);
+
+	v4l2_info(sd, "Removing\n");
+
+	v4l2_async_unregister_subdev(sd);
+	mutex_destroy(&priv->lock);
+	media_entity_cleanup(&sd->entity);
+
+	return 0;
+}
+
+static const struct platform_device_id imx_vdic_ids[] = {
+	{ .name = "imx-ipuv3-vdic" },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, imx_vdic_ids);
+
+static struct platform_driver imx_vdic_driver = {
+	.probe = imx_vdic_probe,
+	.remove = imx_vdic_remove,
+	.id_table = imx_vdic_ids,
+	.driver = {
+		.name = "imx-ipuv3-vdic",
+	},
+};
+module_platform_driver(imx_vdic_driver);
+
+MODULE_DESCRIPTION("i.MX VDIC subdev driver");
+MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:imx-ipuv3-vdic");
diff --git a/drivers/staging/media/imx/imx-media.h b/drivers/staging/media/imx/imx-media.h
new file mode 100644
index 000000000000..d409170632bd
--- /dev/null
+++ b/drivers/staging/media/imx/imx-media.h
@@ -0,0 +1,325 @@
+/*
+ * V4L2 Media Controller Driver for Freescale i.MX5/6 SOC
+ *
+ * Copyright (c) 2016 Mentor Graphics 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.
+ */
+#ifndef _IMX_MEDIA_H
+#define _IMX_MEDIA_H
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-contig.h>
+#include <video/imx-ipu-v3.h>
+
+/*
+ * This is somewhat arbitrary, but we need at least:
+ * - 4 video devices per IPU
+ * - 3 IC subdevs per IPU
+ * - 1 VDIC subdev per IPU
+ * - 2 CSI subdevs per IPU
+ * - 1 mipi-csi2 receiver subdev
+ * - 2 video-mux subdevs
+ * - 2 camera sensor subdevs per IPU (1 parallel, 1 mipi-csi2)
+ *
+ */
+/* max video devices */
+#define IMX_MEDIA_MAX_VDEVS          8
+/* max subdevices */
+#define IMX_MEDIA_MAX_SUBDEVS       32
+/* max pads per subdev */
+#define IMX_MEDIA_MAX_PADS          16
+/* max links per pad */
+#define IMX_MEDIA_MAX_LINKS          8
+
+/*
+ * Pad definitions for the subdevs with multiple source or
+ * sink pads
+ */
+
+/* ipu_csi */
+enum {
+	CSI_SINK_PAD = 0,
+	CSI_SRC_PAD_DIRECT,
+	CSI_SRC_PAD_IDMAC,
+	CSI_NUM_PADS,
+};
+
+#define CSI_NUM_SINK_PADS 1
+#define CSI_NUM_SRC_PADS  2
+
+/* ipu_vdic */
+enum {
+	VDIC_SINK_PAD_DIRECT = 0,
+	VDIC_SINK_PAD_IDMAC,
+	VDIC_SRC_PAD_DIRECT,
+	VDIC_NUM_PADS,
+};
+
+#define VDIC_NUM_SINK_PADS 2
+#define VDIC_NUM_SRC_PADS  1
+
+/* ipu_ic_prp */
+enum {
+	PRP_SINK_PAD = 0,
+	PRP_SRC_PAD_PRPENC,
+	PRP_SRC_PAD_PRPVF,
+	PRP_NUM_PADS,
+};
+
+#define PRP_NUM_SINK_PADS 1
+#define PRP_NUM_SRC_PADS  2
+
+/* ipu_ic_prpencvf */
+enum {
+	PRPENCVF_SINK_PAD = 0,
+	PRPENCVF_SRC_PAD,
+	PRPENCVF_NUM_PADS,
+};
+
+#define PRPENCVF_NUM_SINK_PADS 1
+#define PRPENCVF_NUM_SRC_PADS  1
+
+/* How long to wait for EOF interrupts in the buffer-capture subdevs */
+#define IMX_MEDIA_EOF_TIMEOUT       1000
+
+struct imx_media_pixfmt {
+	u32     fourcc;
+	u32     codes[4];
+	int     bpp;     /* total bpp */
+	enum ipu_color_space cs;
+	bool    planar;  /* is a planar format */
+	bool    bayer;   /* is a raw bayer format */
+	bool    ipufmt;  /* is one of the IPU internal formats */
+};
+
+struct imx_media_buffer {
+	struct vb2_v4l2_buffer vbuf; /* v4l buffer must be first */
+	struct list_head  list;
+};
+
+struct imx_media_video_dev {
+	struct video_device *vfd;
+
+	/* the user format */
+	struct v4l2_format fmt;
+	const struct imx_media_pixfmt *cc;
+};
+
+static inline struct imx_media_buffer *to_imx_media_vb(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	return container_of(vbuf, struct imx_media_buffer, vbuf);
+}
+
+struct imx_media_link {
+	struct device_node *remote_sd_node;
+	char               remote_devname[32];
+	int                local_pad;
+	int                remote_pad;
+};
+
+struct imx_media_pad {
+	struct media_pad  pad;
+	struct imx_media_link link[IMX_MEDIA_MAX_LINKS];
+	bool devnode; /* does this pad link to a device node */
+	int num_links;
+
+	/*
+	 * list of video devices that can be reached from this pad,
+	 * list is only valid for source pads.
+	 */
+	struct imx_media_video_dev *vdev[IMX_MEDIA_MAX_VDEVS];
+	int num_vdevs;
+};
+
+struct imx_media_internal_sd_platformdata {
+	char sd_name[V4L2_SUBDEV_NAME_SIZE];
+	u32 grp_id;
+	int ipu_id;
+};
+
+struct imx_media_subdev {
+	struct v4l2_async_subdev asd;
+	struct v4l2_subdev       *sd; /* set when bound */
+
+	struct imx_media_pad     pad[IMX_MEDIA_MAX_PADS];
+	int num_sink_pads;
+	int num_src_pads;
+
+	/* the platform device if this is an internal subdev */
+	struct platform_device *pdev;
+	/* the devname is needed for async devname match */
+	char devname[32];
+
+	/* if this is a sensor */
+	struct v4l2_fwnode_endpoint sensor_ep;
+};
+
+struct imx_media_dev {
+	struct media_device md;
+	struct v4l2_device  v4l2_dev;
+
+	/* the pipeline object */
+	struct media_pipeline pipe;
+
+	struct mutex mutex; /* protect elements below */
+
+	/* master subdevice list */
+	struct imx_media_subdev subdev[IMX_MEDIA_MAX_SUBDEVS];
+	int num_subdevs;
+
+	/* master video device list */
+	struct imx_media_video_dev *vdev[IMX_MEDIA_MAX_VDEVS];
+	int num_vdevs;
+
+	/* IPUs this media driver control, valid after subdevs bound */
+	struct ipu_soc *ipu[2];
+
+	/* for async subdev registration */
+	struct v4l2_async_subdev *async_ptrs[IMX_MEDIA_MAX_SUBDEVS];
+	struct v4l2_async_notifier subdev_notifier;
+};
+
+enum codespace_sel {
+	CS_SEL_YUV = 0,
+	CS_SEL_RGB,
+	CS_SEL_ANY,
+};
+
+const struct imx_media_pixfmt *
+imx_media_find_format(u32 fourcc, enum codespace_sel cs_sel, bool allow_bayer);
+int imx_media_enum_format(u32 *fourcc, u32 index, enum codespace_sel cs_sel);
+const struct imx_media_pixfmt *
+imx_media_find_mbus_format(u32 code, enum codespace_sel cs_sel,
+			   bool allow_bayer);
+int imx_media_enum_mbus_format(u32 *code, u32 index, enum codespace_sel cs_sel,
+			       bool allow_bayer);
+const struct imx_media_pixfmt *
+imx_media_find_ipu_format(u32 code, enum codespace_sel cs_sel);
+int imx_media_enum_ipu_format(u32 *code, u32 index, enum codespace_sel cs_sel);
+
+int imx_media_init_mbus_fmt(struct v4l2_mbus_framefmt *mbus,
+			    u32 width, u32 height, u32 code, u32 field,
+			    const struct imx_media_pixfmt **cc);
+void imx_media_fill_default_mbus_fields(struct v4l2_mbus_framefmt *tryfmt,
+					struct v4l2_mbus_framefmt *fmt,
+					bool ic_route);
+int imx_media_mbus_fmt_to_pix_fmt(struct v4l2_pix_format *pix,
+				  struct v4l2_mbus_framefmt *mbus,
+				  const struct imx_media_pixfmt *cc);
+int imx_media_mbus_fmt_to_ipu_image(struct ipu_image *image,
+				    struct v4l2_mbus_framefmt *mbus);
+int imx_media_ipu_image_to_mbus_fmt(struct v4l2_mbus_framefmt *mbus,
+				    struct ipu_image *image);
+
+struct imx_media_subdev *
+imx_media_find_async_subdev(struct imx_media_dev *imxmd,
+			    struct device_node *np,
+			    const char *devname);
+struct imx_media_subdev *
+imx_media_add_async_subdev(struct imx_media_dev *imxmd,
+			   struct device_node *np,
+			   struct platform_device *pdev);
+int imx_media_add_pad_link(struct imx_media_dev *imxmd,
+			   struct imx_media_pad *pad,
+			   struct device_node *remote_node,
+			   const char *remote_devname,
+			   int local_pad, int remote_pad);
+
+void imx_media_grp_id_to_sd_name(char *sd_name, int sz,
+				 u32 grp_id, int ipu_id);
+
+int imx_media_add_internal_subdevs(struct imx_media_dev *imxmd,
+				   struct imx_media_subdev *csi[4]);
+void imx_media_remove_internal_subdevs(struct imx_media_dev *imxmd);
+
+struct imx_media_subdev *
+imx_media_find_subdev_by_sd(struct imx_media_dev *imxmd,
+			    struct v4l2_subdev *sd);
+struct imx_media_subdev *
+imx_media_find_subdev_by_id(struct imx_media_dev *imxmd,
+			    u32 grp_id);
+int imx_media_add_video_device(struct imx_media_dev *imxmd,
+			       struct imx_media_video_dev *vdev);
+int imx_media_find_mipi_csi2_channel(struct imx_media_dev *imxmd,
+				     struct media_entity *start_entity);
+struct imx_media_subdev *
+imx_media_find_upstream_subdev(struct imx_media_dev *imxmd,
+			       struct media_entity *start_entity,
+			       u32 grp_id);
+struct imx_media_subdev *
+__imx_media_find_sensor(struct imx_media_dev *imxmd,
+			struct media_entity *start_entity);
+struct imx_media_subdev *
+imx_media_find_sensor(struct imx_media_dev *imxmd,
+		      struct media_entity *start_entity);
+
+struct imx_media_dma_buf {
+	void          *virt;
+	dma_addr_t     phys;
+	unsigned long  len;
+};
+
+void imx_media_free_dma_buf(struct imx_media_dev *imxmd,
+			    struct imx_media_dma_buf *buf);
+int imx_media_alloc_dma_buf(struct imx_media_dev *imxmd,
+			    struct imx_media_dma_buf *buf,
+			    int size);
+
+int imx_media_pipeline_set_stream(struct imx_media_dev *imxmd,
+				  struct media_entity *entity,
+				  bool on);
+
+/* imx-media-fim.c */
+struct imx_media_fim;
+void imx_media_fim_eof_monitor(struct imx_media_fim *fim, struct timespec *ts);
+int imx_media_fim_set_stream(struct imx_media_fim *fim,
+			     const struct v4l2_fract *frame_interval,
+			     bool on);
+int imx_media_fim_add_controls(struct imx_media_fim *fim);
+struct imx_media_fim *imx_media_fim_init(struct v4l2_subdev *sd);
+void imx_media_fim_free(struct imx_media_fim *fim);
+
+/* imx-media-of.c */
+struct imx_media_subdev *
+imx_media_of_find_subdev(struct imx_media_dev *imxmd,
+			 struct device_node *np,
+			 const char *name);
+int imx_media_of_parse(struct imx_media_dev *dev,
+		       struct imx_media_subdev *(*csi)[4],
+		       struct device_node *np);
+
+/* imx-media-capture.c */
+struct imx_media_video_dev *
+imx_media_capture_device_init(struct v4l2_subdev *src_sd, int pad);
+void imx_media_capture_device_remove(struct imx_media_video_dev *vdev);
+int imx_media_capture_device_register(struct imx_media_video_dev *vdev);
+void imx_media_capture_device_unregister(struct imx_media_video_dev *vdev);
+struct imx_media_buffer *
+imx_media_capture_device_next_buf(struct imx_media_video_dev *vdev);
+void imx_media_capture_device_set_format(struct imx_media_video_dev *vdev,
+					 struct v4l2_pix_format *pix);
+void imx_media_capture_device_error(struct imx_media_video_dev *vdev);
+
+/* subdev group ids */
+#define IMX_MEDIA_GRP_ID_SENSOR    (1 << 8)
+#define IMX_MEDIA_GRP_ID_VIDMUX    (1 << 9)
+#define IMX_MEDIA_GRP_ID_CSI2      (1 << 10)
+#define IMX_MEDIA_GRP_ID_CSI_BIT   11
+#define IMX_MEDIA_GRP_ID_CSI       (0x3 << IMX_MEDIA_GRP_ID_CSI_BIT)
+#define IMX_MEDIA_GRP_ID_CSI0      (1 << IMX_MEDIA_GRP_ID_CSI_BIT)
+#define IMX_MEDIA_GRP_ID_CSI1      (2 << IMX_MEDIA_GRP_ID_CSI_BIT)
+#define IMX_MEDIA_GRP_ID_VDIC      (1 << 13)
+#define IMX_MEDIA_GRP_ID_IC_PRP    (1 << 14)
+#define IMX_MEDIA_GRP_ID_IC_PRPENC (1 << 15)
+#define IMX_MEDIA_GRP_ID_IC_PRPVF  (1 << 16)
+
+#endif
diff --git a/drivers/staging/media/imx/imx6-mipi-csi2.c b/drivers/staging/media/imx/imx6-mipi-csi2.c
new file mode 100644
index 000000000000..5061f3f524fd
--- /dev/null
+++ b/drivers/staging/media/imx/imx6-mipi-csi2.c
@@ -0,0 +1,698 @@
+/*
+ * MIPI CSI-2 Receiver Subdev for Freescale i.MX6 SOC.
+ *
+ * Copyright (c) 2012-2017 Mentor Graphics Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/iopoll.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+#include "imx-media.h"
+
+/*
+ * there must be 5 pads: 1 input pad from sensor, and
+ * the 4 virtual channel output pads
+ */
+#define CSI2_SINK_PAD       0
+#define CSI2_NUM_SINK_PADS  1
+#define CSI2_NUM_SRC_PADS   4
+#define CSI2_NUM_PADS       5
+
+/*
+ * The default maximum bit-rate per lane in Mbps, if the
+ * source subdev does not provide V4L2_CID_LINK_FREQ.
+ */
+#define CSI2_DEFAULT_MAX_MBPS 849
+
+struct csi2_dev {
+	struct device          *dev;
+	struct v4l2_subdev      sd;
+	struct media_pad       pad[CSI2_NUM_PADS];
+	struct clk             *dphy_clk;
+	struct clk             *pllref_clk;
+	struct clk             *pix_clk; /* what is this? */
+	void __iomem           *base;
+	struct v4l2_fwnode_bus_mipi_csi2 bus;
+
+	/* lock to protect all members below */
+	struct mutex lock;
+
+	struct v4l2_mbus_framefmt format_mbus;
+
+	int                     stream_count;
+	struct v4l2_subdev      *src_sd;
+	bool                    sink_linked[CSI2_NUM_SRC_PADS];
+};
+
+#define DEVICE_NAME "imx6-mipi-csi2"
+
+/* Register offsets */
+#define CSI2_VERSION            0x000
+#define CSI2_N_LANES            0x004
+#define CSI2_PHY_SHUTDOWNZ      0x008
+#define CSI2_DPHY_RSTZ          0x00c
+#define CSI2_RESETN             0x010
+#define CSI2_PHY_STATE          0x014
+#define PHY_STOPSTATEDATA_BIT   4
+#define PHY_STOPSTATEDATA(n)    BIT(PHY_STOPSTATEDATA_BIT + (n))
+#define PHY_RXCLKACTIVEHS       BIT(8)
+#define PHY_RXULPSCLKNOT        BIT(9)
+#define PHY_STOPSTATECLK        BIT(10)
+#define CSI2_DATA_IDS_1         0x018
+#define CSI2_DATA_IDS_2         0x01c
+#define CSI2_ERR1               0x020
+#define CSI2_ERR2               0x024
+#define CSI2_MSK1               0x028
+#define CSI2_MSK2               0x02c
+#define CSI2_PHY_TST_CTRL0      0x030
+#define PHY_TESTCLR		BIT(0)
+#define PHY_TESTCLK		BIT(1)
+#define CSI2_PHY_TST_CTRL1      0x034
+#define PHY_TESTEN		BIT(16)
+/*
+ * i.MX CSI2IPU Gasket registers follow. The CSI2IPU gasket is
+ * not part of the MIPI CSI-2 core, but its registers fall in the
+ * same register map range.
+ */
+#define CSI2IPU_GASKET		0xf00
+#define CSI2IPU_YUV422_YUYV	BIT(2)
+
+static inline struct csi2_dev *sd_to_dev(struct v4l2_subdev *sdev)
+{
+	return container_of(sdev, struct csi2_dev, sd);
+}
+
+/*
+ * The required sequence of MIPI CSI-2 startup as specified in the i.MX6
+ * reference manual is as follows:
+ *
+ * 1. Deassert presetn signal (global reset).
+ *        It's not clear what this "global reset" signal is (maybe APB
+ *        global reset), but in any case this step would be probably
+ *        be carried out during driver load in csi2_probe().
+ *
+ * 2. Configure MIPI Camera Sensor to put all Tx lanes in LP-11 state.
+ *        This must be carried out by the MIPI sensor's s_power(ON) subdev
+ *        op.
+ *
+ * 3. D-PHY initialization.
+ * 4. CSI2 Controller programming (Set N_LANES, deassert PHY_SHUTDOWNZ,
+ *    deassert PHY_RSTZ, deassert CSI2_RESETN).
+ * 5. Read the PHY status register (PHY_STATE) to confirm that all data and
+ *    clock lanes of the D-PHY are in LP-11 state.
+ * 6. Configure the MIPI Camera Sensor to start transmitting a clock on the
+ *    D-PHY clock lane.
+ * 7. CSI2 Controller programming - Read the PHY status register (PHY_STATE)
+ *    to confirm that the D-PHY is receiving a clock on the D-PHY clock lane.
+ *
+ * All steps 3 through 7 are carried out by csi2_s_stream(ON) here. Step
+ * 6 is accomplished by calling the source subdev's s_stream(ON) between
+ * steps 5 and 7.
+ */
+
+static void csi2_enable(struct csi2_dev *csi2, bool enable)
+{
+	if (enable) {
+		writel(0x1, csi2->base + CSI2_PHY_SHUTDOWNZ);
+		writel(0x1, csi2->base + CSI2_DPHY_RSTZ);
+		writel(0x1, csi2->base + CSI2_RESETN);
+	} else {
+		writel(0x0, csi2->base + CSI2_PHY_SHUTDOWNZ);
+		writel(0x0, csi2->base + CSI2_DPHY_RSTZ);
+		writel(0x0, csi2->base + CSI2_RESETN);
+	}
+}
+
+static void csi2_set_lanes(struct csi2_dev *csi2)
+{
+	int lanes = csi2->bus.num_data_lanes;
+
+	writel(lanes - 1, csi2->base + CSI2_N_LANES);
+}
+
+static void dw_mipi_csi2_phy_write(struct csi2_dev *csi2,
+				   u32 test_code, u32 test_data)
+{
+	/* Clear PHY test interface */
+	writel(PHY_TESTCLR, csi2->base + CSI2_PHY_TST_CTRL0);
+	writel(0x0, csi2->base + CSI2_PHY_TST_CTRL1);
+	writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
+
+	/* Raise test interface strobe signal */
+	writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0);
+
+	/* Configure address write on falling edge and lower strobe signal */
+	writel(PHY_TESTEN | test_code, csi2->base + CSI2_PHY_TST_CTRL1);
+	writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
+
+	/* Configure data write on rising edge and raise strobe signal */
+	writel(test_data, csi2->base + CSI2_PHY_TST_CTRL1);
+	writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0);
+
+	/* Clear strobe signal */
+	writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
+}
+
+/*
+ * This table is based on the table documented at
+ * https://community.nxp.com/docs/DOC-94312. It assumes
+ * a 27MHz D-PHY pll reference clock.
+ */
+static const struct {
+	u32 max_mbps;
+	u32 hsfreqrange_sel;
+} hsfreq_map[] = {
+	{ 90, 0x00}, {100, 0x20}, {110, 0x40}, {125, 0x02},
+	{140, 0x22}, {150, 0x42}, {160, 0x04}, {180, 0x24},
+	{200, 0x44}, {210, 0x06}, {240, 0x26}, {250, 0x46},
+	{270, 0x08}, {300, 0x28}, {330, 0x48}, {360, 0x2a},
+	{400, 0x4a}, {450, 0x0c}, {500, 0x2c}, {550, 0x0e},
+	{600, 0x2e}, {650, 0x10}, {700, 0x30}, {750, 0x12},
+	{800, 0x32}, {850, 0x14}, {900, 0x34}, {950, 0x54},
+	{1000, 0x74},
+};
+
+static int max_mbps_to_hsfreqrange_sel(u32 max_mbps)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(hsfreq_map); i++)
+		if (hsfreq_map[i].max_mbps > max_mbps)
+			return hsfreq_map[i].hsfreqrange_sel;
+
+	return -EINVAL;
+}
+
+static int csi2_dphy_init(struct csi2_dev *csi2)
+{
+	struct v4l2_ctrl *ctrl;
+	u32 mbps_per_lane;
+	int sel;
+
+	ctrl = v4l2_ctrl_find(csi2->src_sd->ctrl_handler,
+			      V4L2_CID_LINK_FREQ);
+	if (!ctrl)
+		mbps_per_lane = CSI2_DEFAULT_MAX_MBPS;
+	else
+		mbps_per_lane = DIV_ROUND_UP_ULL(2 * ctrl->qmenu_int[ctrl->val],
+						 USEC_PER_SEC);
+
+	sel = max_mbps_to_hsfreqrange_sel(mbps_per_lane);
+	if (sel < 0)
+		return sel;
+
+	dw_mipi_csi2_phy_write(csi2, 0x44, sel);
+
+	return 0;
+}
+
+/*
+ * Waits for ultra-low-power state on D-PHY clock lane. This is currently
+ * unused and may not be needed at all, but keep around just in case.
+ */
+static int __maybe_unused csi2_dphy_wait_ulp(struct csi2_dev *csi2)
+{
+	u32 reg;
+	int ret;
+
+	/* wait for ULP on clock lane */
+	ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg,
+				 !(reg & PHY_RXULPSCLKNOT), 0, 500000);
+	if (ret) {
+		v4l2_err(&csi2->sd, "ULP timeout, phy_state = 0x%08x\n", reg);
+		return ret;
+	}
+
+	/* wait until no errors on bus */
+	ret = readl_poll_timeout(csi2->base + CSI2_ERR1, reg,
+				 reg == 0x0, 0, 500000);
+	if (ret) {
+		v4l2_err(&csi2->sd, "stable bus timeout, err1 = 0x%08x\n", reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* Waits for low-power LP-11 state on data and clock lanes. */
+static int csi2_dphy_wait_stopstate(struct csi2_dev *csi2)
+{
+	u32 mask, reg;
+	int ret;
+
+	mask = PHY_STOPSTATECLK |
+		((csi2->bus.num_data_lanes - 1) << PHY_STOPSTATEDATA_BIT);
+
+	ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg,
+				 (reg & mask) == mask, 0, 500000);
+	if (ret) {
+		v4l2_err(&csi2->sd, "LP-11 timeout, phy_state = 0x%08x\n", reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* Wait for active clock on the clock lane. */
+static int csi2_dphy_wait_clock_lane(struct csi2_dev *csi2)
+{
+	u32 reg;
+	int ret;
+
+	ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg,
+				 (reg & PHY_RXCLKACTIVEHS), 0, 500000);
+	if (ret) {
+		v4l2_err(&csi2->sd, "clock lane timeout, phy_state = 0x%08x\n",
+			 reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* Setup the i.MX CSI2IPU Gasket */
+static void csi2ipu_gasket_init(struct csi2_dev *csi2)
+{
+	u32 reg = 0;
+
+	switch (csi2->format_mbus.code) {
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+		reg = CSI2IPU_YUV422_YUYV;
+		break;
+	default:
+		break;
+	}
+
+	writel(reg, csi2->base + CSI2IPU_GASKET);
+}
+
+static int csi2_start(struct csi2_dev *csi2)
+{
+	int ret;
+
+	ret = clk_prepare_enable(csi2->pix_clk);
+	if (ret)
+		return ret;
+
+	/* setup the gasket */
+	csi2ipu_gasket_init(csi2);
+
+	/* Step 3 */
+	ret = csi2_dphy_init(csi2);
+	if (ret)
+		goto err_disable_clk;
+
+	/* Step 4 */
+	csi2_set_lanes(csi2);
+	csi2_enable(csi2, true);
+
+	/* Step 5 */
+	ret = csi2_dphy_wait_stopstate(csi2);
+	if (ret)
+		goto err_assert_reset;
+
+	/* Step 6 */
+	ret = v4l2_subdev_call(csi2->src_sd, video, s_stream, 1);
+	ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
+	if (ret)
+		goto err_assert_reset;
+
+	/* Step 7 */
+	ret = csi2_dphy_wait_clock_lane(csi2);
+	if (ret)
+		goto err_stop_upstream;
+
+	return 0;
+
+err_stop_upstream:
+	v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
+err_assert_reset:
+	csi2_enable(csi2, false);
+err_disable_clk:
+	clk_disable_unprepare(csi2->pix_clk);
+	return ret;
+}
+
+static void csi2_stop(struct csi2_dev *csi2)
+{
+	/* stop upstream */
+	v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
+
+	csi2_enable(csi2, false);
+	clk_disable_unprepare(csi2->pix_clk);
+}
+
+/*
+ * V4L2 subdev operations.
+ */
+
+static int csi2_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+	int i, ret = 0;
+
+	mutex_lock(&csi2->lock);
+
+	if (!csi2->src_sd) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	for (i = 0; i < CSI2_NUM_SRC_PADS; i++) {
+		if (csi2->sink_linked[i])
+			break;
+	}
+	if (i >= CSI2_NUM_SRC_PADS) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	/*
+	 * enable/disable streaming only if stream_count is
+	 * going from 0 to 1 / 1 to 0.
+	 */
+	if (csi2->stream_count != !enable)
+		goto update_count;
+
+	dev_dbg(csi2->dev, "stream %s\n", enable ? "ON" : "OFF");
+	if (enable)
+		ret = csi2_start(csi2);
+	else
+		csi2_stop(csi2);
+	if (ret)
+		goto out;
+
+update_count:
+	csi2->stream_count += enable ? 1 : -1;
+	if (csi2->stream_count < 0)
+		csi2->stream_count = 0;
+out:
+	mutex_unlock(&csi2->lock);
+	return ret;
+}
+
+static int csi2_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+	struct v4l2_subdev *remote_sd;
+	int ret = 0;
+
+	dev_dbg(csi2->dev, "link setup %s -> %s", remote->entity->name,
+		local->entity->name);
+
+	remote_sd = media_entity_to_v4l2_subdev(remote->entity);
+
+	mutex_lock(&csi2->lock);
+
+	if (local->flags & MEDIA_PAD_FL_SOURCE) {
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (csi2->sink_linked[local->index - 1]) {
+				ret = -EBUSY;
+				goto out;
+			}
+			csi2->sink_linked[local->index - 1] = true;
+		} else {
+			csi2->sink_linked[local->index - 1] = false;
+		}
+	} else {
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (csi2->src_sd) {
+				ret = -EBUSY;
+				goto out;
+			}
+			csi2->src_sd = remote_sd;
+		} else {
+			csi2->src_sd = NULL;
+		}
+	}
+
+out:
+	mutex_unlock(&csi2->lock);
+	return ret;
+}
+
+static int csi2_get_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *sdformat)
+{
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	mutex_lock(&csi2->lock);
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt = v4l2_subdev_get_try_format(&csi2->sd, cfg,
+						 sdformat->pad);
+	else
+		fmt = &csi2->format_mbus;
+
+	sdformat->format = *fmt;
+
+	mutex_unlock(&csi2->lock);
+
+	return 0;
+}
+
+static int csi2_set_fmt(struct v4l2_subdev *sd,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_format *sdformat)
+{
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+	int ret = 0;
+
+	if (sdformat->pad >= CSI2_NUM_PADS)
+		return -EINVAL;
+
+	mutex_lock(&csi2->lock);
+
+	if (csi2->stream_count > 0) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	/* Output pads mirror active input pad, no limits on input pads */
+	if (sdformat->pad != CSI2_SINK_PAD)
+		sdformat->format = csi2->format_mbus;
+
+	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY)
+		cfg->try_fmt = sdformat->format;
+	else
+		csi2->format_mbus = sdformat->format;
+out:
+	mutex_unlock(&csi2->lock);
+	return ret;
+}
+
+/*
+ * retrieve our pads parsed from the OF graph by the media device
+ */
+static int csi2_registered(struct v4l2_subdev *sd)
+{
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+	int i, ret;
+
+	for (i = 0; i < CSI2_NUM_PADS; i++) {
+		csi2->pad[i].flags = (i == CSI2_SINK_PAD) ?
+		MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+	}
+
+	/* set a default mbus format  */
+	ret = imx_media_init_mbus_fmt(&csi2->format_mbus,
+				      640, 480, 0, V4L2_FIELD_NONE, NULL);
+	if (ret)
+		return ret;
+
+	return media_entity_pads_init(&sd->entity, CSI2_NUM_PADS, csi2->pad);
+}
+
+static const struct media_entity_operations csi2_entity_ops = {
+	.link_setup = csi2_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_video_ops csi2_video_ops = {
+	.s_stream = csi2_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
+	.get_fmt = csi2_get_fmt,
+	.set_fmt = csi2_set_fmt,
+};
+
+static const struct v4l2_subdev_ops csi2_subdev_ops = {
+	.video = &csi2_video_ops,
+	.pad = &csi2_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops csi2_internal_ops = {
+	.registered = csi2_registered,
+};
+
+static int csi2_parse_endpoints(struct csi2_dev *csi2)
+{
+	struct device_node *node = csi2->dev->of_node;
+	struct device_node *epnode;
+	struct v4l2_fwnode_endpoint ep;
+
+	epnode = of_graph_get_endpoint_by_regs(node, 0, -1);
+	if (!epnode) {
+		v4l2_err(&csi2->sd, "failed to get sink endpoint node\n");
+		return -EINVAL;
+	}
+
+	v4l2_fwnode_endpoint_parse(of_fwnode_handle(epnode), &ep);
+	of_node_put(epnode);
+
+	if (ep.bus_type != V4L2_MBUS_CSI2) {
+		v4l2_err(&csi2->sd, "invalid bus type, must be MIPI CSI2\n");
+		return -EINVAL;
+	}
+
+	csi2->bus = ep.bus.mipi_csi2;
+
+	dev_dbg(csi2->dev, "data lanes: %d\n", csi2->bus.num_data_lanes);
+	dev_dbg(csi2->dev, "flags: 0x%08x\n", csi2->bus.flags);
+	return 0;
+}
+
+static int csi2_probe(struct platform_device *pdev)
+{
+	struct csi2_dev *csi2;
+	struct resource *res;
+	int ret;
+
+	csi2 = devm_kzalloc(&pdev->dev, sizeof(*csi2), GFP_KERNEL);
+	if (!csi2)
+		return -ENOMEM;
+
+	csi2->dev = &pdev->dev;
+
+	v4l2_subdev_init(&csi2->sd, &csi2_subdev_ops);
+	v4l2_set_subdevdata(&csi2->sd, &pdev->dev);
+	csi2->sd.internal_ops = &csi2_internal_ops;
+	csi2->sd.entity.ops = &csi2_entity_ops;
+	csi2->sd.dev = &pdev->dev;
+	csi2->sd.owner = THIS_MODULE;
+	csi2->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	strcpy(csi2->sd.name, DEVICE_NAME);
+	csi2->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	csi2->sd.grp_id = IMX_MEDIA_GRP_ID_CSI2;
+
+	ret = csi2_parse_endpoints(csi2);
+	if (ret)
+		return ret;
+
+	csi2->pllref_clk = devm_clk_get(&pdev->dev, "ref");
+	if (IS_ERR(csi2->pllref_clk)) {
+		v4l2_err(&csi2->sd, "failed to get pll reference clock\n");
+		ret = PTR_ERR(csi2->pllref_clk);
+		return ret;
+	}
+
+	csi2->dphy_clk = devm_clk_get(&pdev->dev, "dphy");
+	if (IS_ERR(csi2->dphy_clk)) {
+		v4l2_err(&csi2->sd, "failed to get dphy clock\n");
+		ret = PTR_ERR(csi2->dphy_clk);
+		return ret;
+	}
+
+	csi2->pix_clk = devm_clk_get(&pdev->dev, "pix");
+	if (IS_ERR(csi2->pix_clk)) {
+		v4l2_err(&csi2->sd, "failed to get pixel clock\n");
+		ret = PTR_ERR(csi2->pix_clk);
+		return ret;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		v4l2_err(&csi2->sd, "failed to get platform resources\n");
+		return -ENODEV;
+	}
+
+	csi2->base = devm_ioremap(&pdev->dev, res->start, PAGE_SIZE);
+	if (!csi2->base) {
+		v4l2_err(&csi2->sd, "failed to map CSI-2 registers\n");
+		return -ENOMEM;
+	}
+
+	mutex_init(&csi2->lock);
+
+	ret = clk_prepare_enable(csi2->pllref_clk);
+	if (ret) {
+		v4l2_err(&csi2->sd, "failed to enable pllref_clk\n");
+		goto rmmutex;
+	}
+
+	ret = clk_prepare_enable(csi2->dphy_clk);
+	if (ret) {
+		v4l2_err(&csi2->sd, "failed to enable dphy_clk\n");
+		goto pllref_off;
+	}
+
+	platform_set_drvdata(pdev, &csi2->sd);
+
+	ret = v4l2_async_register_subdev(&csi2->sd);
+	if (ret)
+		goto dphy_off;
+
+	return 0;
+
+dphy_off:
+	clk_disable_unprepare(csi2->dphy_clk);
+pllref_off:
+	clk_disable_unprepare(csi2->pllref_clk);
+rmmutex:
+	mutex_destroy(&csi2->lock);
+	return ret;
+}
+
+static int csi2_remove(struct platform_device *pdev)
+{
+	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	clk_disable_unprepare(csi2->dphy_clk);
+	clk_disable_unprepare(csi2->pllref_clk);
+	mutex_destroy(&csi2->lock);
+	media_entity_cleanup(&sd->entity);
+
+	return 0;
+}
+
+static const struct of_device_id csi2_dt_ids[] = {
+	{ .compatible = "fsl,imx6-mipi-csi2", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, csi2_dt_ids);
+
+static struct platform_driver csi2_driver = {
+	.driver = {
+		.name = DEVICE_NAME,
+		.of_match_table = csi2_dt_ids,
+	},
+	.probe = csi2_probe,
+	.remove = csi2_remove,
+};
+
+module_platform_driver(csi2_driver);
+
+MODULE_DESCRIPTION("i.MX5/6 MIPI CSI-2 Receiver driver");
+MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/lirc/TODO b/drivers/staging/media/lirc/TODO
index cbea5d84fed3..a97800a8e127 100644
--- a/drivers/staging/media/lirc/TODO
+++ b/drivers/staging/media/lirc/TODO
@@ -1,13 +1,36 @@
-- All drivers should either be ported to ir-core, or dropped entirely
-  (see drivers/media/IR/mceusb.c vs. lirc_mceusb.c in lirc cvs for an
-  example of a previously completed port).
-
-- lirc_bt829 uses registers on a Mach64 VT, which has a separate kernel
-  framebuffer driver (atyfb) and userland X driver (mach64).  It can't
-  simply be converted to a normal PCI driver, but ideally it should be
-  coordinated with the other drivers.
-
-Please send patches to:
-Jarod Wilson <jarod@wilsonet.com>
-Greg Kroah-Hartman <greg@kroah.com>
+1. Both ir-kbd-i2c and lirc_zilog provide support for RX events for
+the chips supported by lirc_zilog.  Before moving lirc_zilog out of staging:
+
+a. ir-kbd-i2c needs a module parameter added to allow the user to tell
+   ir-kbd-i2c to ignore Z8 IR units.
+
+b. lirc_zilog should provide Rx key presses to the rc core like ir-kbd-i2c
+   does.
+
+
+2. lirc_zilog module ref-counting need examination.  It has not been
+verified that cdev and lirc_dev will take the proper module references on
+lirc_zilog to prevent removal of lirc_zilog when the /dev/lircN device node
+is open.
+
+(The good news is ref-counting of lirc_zilog internal structures appears to be
+complete.  Testing has shown the cx18 module can be unloaded out from under
+irw + lircd + lirc_dev, with the /dev/lirc0 device node open, with no adverse
+effects.  The cx18 module could then be reloaded and irw properly began
+receiving button presses again and ir_send worked without error.)
+
+
+3. Bridge drivers, if able, should provide a chip reset() callback
+to lirc_zilog via struct IR_i2c_init_data.  cx18 and ivtv already have routines
+to perform Z8 chip resets via GPIO manipulations.  This would allow lirc_zilog
+to bring the chip back to normal when it hangs, in the same places the
+original lirc_pvr150 driver code does.  This is not strictly needed, so it
+is not required to move lirc_zilog out of staging.
+
+Note: Both lirc_zilog and ir-kbd-i2c support the Zilog Z8 for IR, as programmed
+and installed on Hauppauge products.  When working on either module, developers
+must consider at least the following bridge drivers which mention an IR Rx unit
+at address 0x71 (indicative of a Z8):
+
+	ivtv cx18 hdpvr pvrusb2 bt8xx cx88 saa7134
 
diff --git a/drivers/staging/media/lirc/TODO.lirc_zilog b/drivers/staging/media/lirc/TODO.lirc_zilog
deleted file mode 100644
index a97800a8e127..000000000000
--- a/drivers/staging/media/lirc/TODO.lirc_zilog
+++ /dev/null
@@ -1,36 +0,0 @@
-1. Both ir-kbd-i2c and lirc_zilog provide support for RX events for
-the chips supported by lirc_zilog.  Before moving lirc_zilog out of staging:
-
-a. ir-kbd-i2c needs a module parameter added to allow the user to tell
-   ir-kbd-i2c to ignore Z8 IR units.
-
-b. lirc_zilog should provide Rx key presses to the rc core like ir-kbd-i2c
-   does.
-
-
-2. lirc_zilog module ref-counting need examination.  It has not been
-verified that cdev and lirc_dev will take the proper module references on
-lirc_zilog to prevent removal of lirc_zilog when the /dev/lircN device node
-is open.
-
-(The good news is ref-counting of lirc_zilog internal structures appears to be
-complete.  Testing has shown the cx18 module can be unloaded out from under
-irw + lircd + lirc_dev, with the /dev/lirc0 device node open, with no adverse
-effects.  The cx18 module could then be reloaded and irw properly began
-receiving button presses again and ir_send worked without error.)
-
-
-3. Bridge drivers, if able, should provide a chip reset() callback
-to lirc_zilog via struct IR_i2c_init_data.  cx18 and ivtv already have routines
-to perform Z8 chip resets via GPIO manipulations.  This would allow lirc_zilog
-to bring the chip back to normal when it hangs, in the same places the
-original lirc_pvr150 driver code does.  This is not strictly needed, so it
-is not required to move lirc_zilog out of staging.
-
-Note: Both lirc_zilog and ir-kbd-i2c support the Zilog Z8 for IR, as programmed
-and installed on Hauppauge products.  When working on either module, developers
-must consider at least the following bridge drivers which mention an IR Rx unit
-at address 0x71 (indicative of a Z8):
-
-	ivtv cx18 hdpvr pvrusb2 bt8xx cx88 saa7134
-
diff --git a/drivers/staging/media/lirc/lirc_zilog.c b/drivers/staging/media/lirc/lirc_zilog.c
index 8ce1db04414a..015e41bd036e 100644
--- a/drivers/staging/media/lirc/lirc_zilog.c
+++ b/drivers/staging/media/lirc/lirc_zilog.c
@@ -156,7 +156,6 @@ static struct mutex tx_data_lock;
 /* module parameters */
 static bool debug;	/* debug output */
 static bool tx_only;	/* only handle the IR Tx function */
-static int minor = -1;	/* minor number */
 
 
 /* struct IR reference counting */
@@ -184,10 +183,11 @@ static void release_ir_device(struct kref *ref)
 	 * ir->open_count ==  0 - happens on final close()
 	 * ir_lock, tx_ref_lock, rx_ref_lock, all released
 	 */
-	if (ir->l.minor >= 0 && ir->l.minor < MAX_IRCTL_DEVICES) {
+	if (ir->l.minor >= 0) {
 		lirc_unregister_driver(ir->l.minor);
-		ir->l.minor = MAX_IRCTL_DEVICES;
+		ir->l.minor = -1;
 	}
+
 	if (kfifo_initialized(&ir->rbuf.fifo))
 		lirc_buffer_free(&ir->rbuf);
 	list_del(&ir->list);
@@ -215,7 +215,7 @@ static struct IR_rx *get_ir_rx(struct IR *ir)
 
 	spin_lock(&ir->rx_ref_lock);
 	rx = ir->rx;
-	if (rx != NULL)
+	if (rx)
 		kref_get(&rx->ref);
 	spin_unlock(&ir->rx_ref_lock);
 	return rx;
@@ -277,7 +277,7 @@ static struct IR_tx *get_ir_tx(struct IR *ir)
 
 	spin_lock(&ir->tx_ref_lock);
 	tx = ir->tx;
-	if (tx != NULL)
+	if (tx)
 		kref_get(&tx->ref);
 	spin_unlock(&ir->tx_ref_lock);
 	return tx;
@@ -327,12 +327,12 @@ static int add_to_buf(struct IR *ir)
 	}
 
 	rx = get_ir_rx(ir);
-	if (rx == NULL)
+	if (!rx)
 		return -ENXIO;
 
 	/* Ensure our rx->c i2c_client remains valid for the duration */
 	mutex_lock(&rx->client_lock);
-	if (rx->c == NULL) {
+	if (!rx->c) {
 		mutex_unlock(&rx->client_lock);
 		put_ir_rx(rx, false);
 		return -ENXIO;
@@ -388,7 +388,7 @@ static int add_to_buf(struct IR *ir)
 				break;
 			}
 			schedule_timeout((100 * HZ + 999) / 1000);
-			if (tx != NULL)
+			if (tx)
 				tx->need_boot = 1;
 
 			++failures;
@@ -444,7 +444,7 @@ static int add_to_buf(struct IR *ir)
 	} while (!lirc_buffer_full(rbuf));
 
 	mutex_unlock(&rx->client_lock);
-	if (tx != NULL)
+	if (tx)
 		put_ir_tx(tx, false);
 	put_ir_rx(rx, false);
 	return ret;
@@ -472,7 +472,7 @@ static int lirc_thread(void *arg)
 
 		/* if device not opened, we can sleep half a second */
 		if (atomic_read(&ir->open_count) == 0) {
-			schedule_timeout(HZ/2);
+			schedule_timeout(HZ / 2);
 			continue;
 		}
 
@@ -497,18 +497,9 @@ static int lirc_thread(void *arg)
 	return 0;
 }
 
-static int set_use_inc(void *data)
-{
-	return 0;
-}
-
-static void set_use_dec(void *data)
-{
-}
-
 /* safe read of a uint32 (always network byte order) */
 static int read_uint32(unsigned char **data,
-				     unsigned char *endp, unsigned int *val)
+		       unsigned char *endp, unsigned int *val)
 {
 	if (*data + 4 > endp)
 		return 0;
@@ -520,7 +511,7 @@ static int read_uint32(unsigned char **data,
 
 /* safe read of a uint8 */
 static int read_uint8(unsigned char **data,
-				    unsigned char *endp, unsigned char *val)
+		      unsigned char *endp, unsigned char *val)
 {
 	if (*data + 1 > endp)
 		return 0;
@@ -530,7 +521,7 @@ static int read_uint8(unsigned char **data,
 
 /* safe skipping of N bytes */
 static int skip(unsigned char **data,
-			      unsigned char *endp, unsigned int distance)
+		unsigned char *endp, unsigned int distance)
 {
 	if (*data + distance > endp)
 		return 0;
@@ -540,7 +531,7 @@ static int skip(unsigned char **data,
 
 /* decompress key data into the given buffer */
 static int get_key_data(unsigned char *buf,
-			     unsigned int codeset, unsigned int key)
+			unsigned int codeset, unsigned int key)
 {
 	unsigned char *data, *endp, *diffs, *key_block;
 	unsigned char keys, ndiffs, id;
@@ -554,9 +545,9 @@ static int get_key_data(unsigned char *buf,
 		if (!read_uint32(&data, tx_data->endp, &i))
 			goto corrupt;
 
-		if (i == codeset)
+		if (i == codeset) {
 			break;
-		else if (codeset > i) {
+		} else if (codeset > i) {
 			base = pos + 1;
 			--lim;
 		}
@@ -772,7 +763,7 @@ static int fw_load(struct IR_tx *tx)
 
 	/* Parse the file */
 	tx_data = vmalloc(sizeof(*tx_data));
-	if (tx_data == NULL) {
+	if (!tx_data) {
 		release_firmware(fw_entry);
 		ret = -ENOMEM;
 		goto out;
@@ -781,7 +772,7 @@ static int fw_load(struct IR_tx *tx)
 
 	/* Copy the data so hotplug doesn't get confused and timeout */
 	tx_data->datap = vmalloc(fw_entry->size);
-	if (tx_data->datap == NULL) {
+	if (!tx_data->datap) {
 		release_firmware(fw_entry);
 		vfree(tx_data);
 		ret = -ENOMEM;
@@ -810,7 +801,7 @@ static int fw_load(struct IR_tx *tx)
 		goto corrupt;
 
 	if (!read_uint32(&data, tx_data->endp,
-			      &tx_data->num_code_sets))
+			 &tx_data->num_code_sets))
 		goto corrupt;
 
 	dev_dbg(tx->ir->l.dev, "%u IR blaster codesets loaded\n",
@@ -818,7 +809,7 @@ static int fw_load(struct IR_tx *tx)
 
 	tx_data->code_sets = vmalloc(
 		tx_data->num_code_sets * sizeof(char *));
-	if (tx_data->code_sets == NULL) {
+	if (!tx_data->code_sets) {
 		fw_unload_locked();
 		ret = -ENOMEM;
 		goto out;
@@ -866,12 +857,12 @@ static int fw_load(struct IR_tx *tx)
 		 * global fixed
 		 */
 		if (!skip(&data, tx_data->endp,
-			       1 + TX_BLOCK_SIZE - num_global_fixed))
+			  1 + TX_BLOCK_SIZE - num_global_fixed))
 			goto corrupt;
 
 		/* Then we have keys-1 blocks of key id+diffs */
 		if (!skip(&data, tx_data->endp,
-			       (ndiffs + 1) * (keys - 1)))
+			  (ndiffs + 1) * (keys - 1)))
 			goto corrupt;
 	}
 	ret = 0;
@@ -905,7 +896,7 @@ static ssize_t read(struct file *filep, char __user *outbuf, size_t n,
 	}
 
 	rx = get_ir_rx(ir);
-	if (rx == NULL)
+	if (!rx)
 		return -ENXIO;
 
 	/*
@@ -990,8 +981,9 @@ static int send_code(struct IR_tx *tx, unsigned int code, unsigned int key)
 			"failed to get data for code %u, key %u -- check lircd.conf entries\n",
 			code, key);
 		return ret;
-	} else if (ret != 0)
+	} else if (ret != 0) {
 		return ret;
+	}
 
 	/* Send the data block */
 	ret = send_data_block(tx, data_block);
@@ -1065,7 +1057,7 @@ static int send_code(struct IR_tx *tx, unsigned int code, unsigned int key)
 			break;
 		dev_dbg(tx->ir->l.dev,
 			"NAK expected: i2c_master_send failed with %d (try %d)\n",
-			ret, i+1);
+			ret, i + 1);
 	}
 	if (ret != 1) {
 		dev_err(tx->ir->l.dev,
@@ -1111,12 +1103,12 @@ static ssize_t write(struct file *filep, const char __user *buf, size_t n,
 
 	/* Get a struct IR_tx reference */
 	tx = get_ir_tx(ir);
-	if (tx == NULL)
+	if (!tx)
 		return -ENXIO;
 
 	/* Ensure our tx->c i2c_client remains valid for the duration */
 	mutex_lock(&tx->client_lock);
-	if (tx->c == NULL) {
+	if (!tx->c) {
 		mutex_unlock(&tx->client_lock);
 		put_ir_tx(tx, false);
 		return -ENXIO;
@@ -1188,8 +1180,9 @@ static ssize_t write(struct file *filep, const char __user *buf, size_t n,
 			schedule_timeout((100 * HZ + 999) / 1000);
 			tx->need_boot = 1;
 			++failures;
-		} else
+		} else {
 			i += sizeof(int);
+		}
 	}
 
 	/* Release i2c bus */
@@ -1212,15 +1205,15 @@ static unsigned int poll(struct file *filep, poll_table *wait)
 	struct lirc_buffer *rbuf = ir->l.rbuf;
 	unsigned int ret;
 
-	dev_dbg(ir->l.dev, "poll called\n");
+	dev_dbg(ir->l.dev, "%s called\n", __func__);
 
 	rx = get_ir_rx(ir);
-	if (rx == NULL) {
+	if (!rx) {
 		/*
 		 * Revisit this, if our poll function ever reports writeable
 		 * status for Tx
 		 */
-		dev_dbg(ir->l.dev, "poll result = POLLERR\n");
+		dev_dbg(ir->l.dev, "%s result = POLLERR\n", __func__);
 		return POLLERR;
 	}
 
@@ -1231,9 +1224,9 @@ static unsigned int poll(struct file *filep, poll_table *wait)
 	poll_wait(filep, &rbuf->wait_poll, wait);
 
 	/* Indicate what ops could happen immediately without blocking */
-	ret = lirc_buffer_empty(rbuf) ? 0 : (POLLIN|POLLRDNORM);
+	ret = lirc_buffer_empty(rbuf) ? 0 : (POLLIN | POLLRDNORM);
 
-	dev_dbg(ir->l.dev, "poll result = %s\n",
+	dev_dbg(ir->l.dev, "%s result = %s\n", __func__,
 		ret ? "POLLIN|POLLRDNORM" : "none");
 	return ret;
 }
@@ -1255,15 +1248,15 @@ static long ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 		result = put_user(features, uptr);
 		break;
 	case LIRC_GET_REC_MODE:
-		if (!(features&LIRC_CAN_REC_MASK))
+		if (!(features & LIRC_CAN_REC_MASK))
 			return -ENOSYS;
 
 		result = put_user(LIRC_REC2MODE
-				  (features&LIRC_CAN_REC_MASK),
+				  (features & LIRC_CAN_REC_MASK),
 				  uptr);
 		break;
 	case LIRC_SET_REC_MODE:
-		if (!(features&LIRC_CAN_REC_MASK))
+		if (!(features & LIRC_CAN_REC_MASK))
 			return -ENOSYS;
 
 		result = get_user(mode, uptr);
@@ -1271,13 +1264,13 @@ static long ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 			result = -EINVAL;
 		break;
 	case LIRC_GET_SEND_MODE:
-		if (!(features&LIRC_CAN_SEND_MASK))
+		if (!(features & LIRC_CAN_SEND_MASK))
 			return -ENOSYS;
 
 		result = put_user(LIRC_MODE_PULSE, uptr);
 		break;
 	case LIRC_SET_SEND_MODE:
-		if (!(features&LIRC_CAN_SEND_MASK))
+		if (!(features & LIRC_CAN_SEND_MASK))
 			return -ENOSYS;
 
 		result = get_user(mode, uptr);
@@ -1322,7 +1315,7 @@ static int open(struct inode *node, struct file *filep)
 	/* find our IR struct */
 	ir = get_ir_device_by_minor(minor);
 
-	if (ir == NULL)
+	if (!ir)
 		return -ENODEV;
 
 	atomic_inc(&ir->open_count);
@@ -1340,8 +1333,9 @@ static int close(struct inode *node, struct file *filep)
 	/* find our IR struct */
 	struct IR *ir = filep->private_data;
 
-	if (ir == NULL) {
-		pr_err("ir: close: no private_data attached to the file!\n");
+	if (!ir) {
+		pr_err("ir: %s: no private_data attached to the file!\n",
+		       __func__);
 		return -ENODEV;
 	}
 
@@ -1394,10 +1388,7 @@ static struct lirc_driver lirc_template = {
 	.minor		= -1,
 	.code_length	= 13,
 	.buffer_size	= BUFLEN / 2,
-	.sample_rate	= 0, /* tell lirc_dev to not start its own kthread */
 	.chunk_size	= 2,
-	.set_use_inc	= set_use_inc,
-	.set_use_dec	= set_use_dec,
 	.fops		= &lirc_fops,
 	.owner		= THIS_MODULE,
 };
@@ -1407,7 +1398,7 @@ static int ir_remove(struct i2c_client *client)
 	if (strncmp("ir_tx_z8", client->name, 8) == 0) {
 		struct IR_tx *tx = i2c_get_clientdata(client);
 
-		if (tx != NULL) {
+		if (tx) {
 			mutex_lock(&tx->client_lock);
 			tx->c = NULL;
 			mutex_unlock(&tx->client_lock);
@@ -1416,7 +1407,7 @@ static int ir_remove(struct i2c_client *client)
 	} else if (strncmp("ir_rx_z8", client->name, 8) == 0) {
 		struct IR_rx *rx = i2c_get_clientdata(client);
 
-		if (rx != NULL) {
+		if (rx) {
 			mutex_lock(&rx->client_lock);
 			rx->c = NULL;
 			mutex_unlock(&rx->client_lock);
@@ -1426,7 +1417,6 @@ static int ir_remove(struct i2c_client *client)
 	return 0;
 }
 
-
 /* ir_devices_lock must be held */
 static struct IR *get_ir_device_by_adapter(struct i2c_adapter *adapter)
 {
@@ -1467,14 +1457,14 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		return -ENXIO;
 
 	pr_info("probing IR %s on %s (i2c-%d)\n",
-		   tx_probe ? "Tx" : "Rx", adap->name, adap->nr);
+		tx_probe ? "Tx" : "Rx", adap->name, adap->nr);
 
 	mutex_lock(&ir_devices_lock);
 
 	/* Use a single struct IR instance for both the Rx and Tx functions */
 	ir = get_ir_device_by_adapter(adap);
-	if (ir == NULL) {
-		ir = kzalloc(sizeof(struct IR), GFP_KERNEL);
+	if (!ir) {
+		ir = kzalloc(sizeof(*ir), GFP_KERNEL);
 		if (!ir) {
 			ret = -ENOMEM;
 			goto out_no_ir;
@@ -1514,7 +1504,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		rx = get_ir_rx(ir);
 
 		/* Set up a struct IR_tx instance */
-		tx = kzalloc(sizeof(struct IR_tx), GFP_KERNEL);
+		tx = kzalloc(sizeof(*tx), GFP_KERNEL);
 		if (!tx) {
 			ret = -ENOMEM;
 			goto out_put_xx;
@@ -1547,7 +1537,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		fw_load(tx);
 
 		/* Proceed only if the Rx client is also ready or not needed */
-		if (rx == NULL && !tx_only) {
+		if (!rx && !tx_only) {
 			dev_info(tx->ir->l.dev,
 				 "probe of IR Tx on %s (i2c-%d) done. Waiting on IR Rx.\n",
 				 adap->name, adap->nr);
@@ -1558,7 +1548,7 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		tx = get_ir_tx(ir);
 
 		/* Set up a struct IR_rx instance */
-		rx = kzalloc(sizeof(struct IR_rx), GFP_KERNEL);
+		rx = kzalloc(sizeof(*rx), GFP_KERNEL);
 		if (!rx) {
 			ret = -ENOMEM;
 			goto out_put_xx;
@@ -1601,20 +1591,19 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		}
 
 		/* Proceed only if the Tx client is also ready */
-		if (tx == NULL) {
+		if (!tx) {
 			pr_info("probe of IR Rx on %s (i2c-%d) done. Waiting on IR Tx.\n",
-				   adap->name, adap->nr);
+				adap->name, adap->nr);
 			goto out_ok;
 		}
 	}
 
 	/* register with lirc */
-	ir->l.minor = minor; /* module option: user requested minor number */
 	ir->l.minor = lirc_register_driver(&ir->l);
-	if (ir->l.minor < 0 || ir->l.minor >= MAX_IRCTL_DEVICES) {
+	if (ir->l.minor < 0) {
 		dev_err(tx->ir->l.dev,
-			"%s: \"minor\" must be between 0 and %d (%d)!\n",
-			__func__, MAX_IRCTL_DEVICES-1, ir->l.minor);
+			"%s: lirc_register_driver() failed: %i\n",
+			__func__, ir->l.minor);
 		ret = -EBADRQC;
 		goto out_put_xx;
 	}
@@ -1623,9 +1612,9 @@ static int ir_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		 adap->name, adap->nr, ir->l.minor);
 
 out_ok:
-	if (rx != NULL)
+	if (rx)
 		put_ir_rx(rx, true);
-	if (tx != NULL)
+	if (tx)
 		put_ir_tx(tx, true);
 	put_ir_device(ir, true);
 	dev_info(ir->l.dev,
@@ -1635,10 +1624,10 @@ out_ok:
 	return 0;
 
 out_put_xx:
-	if (rx != NULL)
+	if (rx)
 		put_ir_rx(rx, true);
 out_put_tx:
-	if (tx != NULL)
+	if (tx)
 		put_ir_tx(tx, true);
 out_put_ir:
 	put_ir_device(ir, true);
@@ -1686,9 +1675,6 @@ MODULE_LICENSE("GPL");
 /* for compat with old name, which isn't all that accurate anymore */
 MODULE_ALIAS("lirc_pvr150");
 
-module_param(minor, int, 0444);
-MODULE_PARM_DESC(minor, "Preferred minor device number");
-
 module_param(debug, bool, 0644);
 MODULE_PARM_DESC(debug, "Enable debugging messages");
 
diff --git a/drivers/staging/most/aim-network/networking.c b/drivers/staging/most/aim-network/networking.c
index ce1764cba5f0..936f013c350e 100644
--- a/drivers/staging/most/aim-network/networking.c
+++ b/drivers/staging/most/aim-network/networking.c
@@ -22,7 +22,6 @@
 #include <linux/wait.h>
 #include <linux/kobject.h>
 #include "mostcore.h"
-#include "networking.h"
 
 #define MEP_HDR_LEN 8
 #define MDP_HDR_LEN 16
@@ -65,17 +64,16 @@ struct net_dev_channel {
 
 struct net_dev_context {
 	struct most_interface *iface;
-	bool channels_opened;
 	bool is_mamac;
 	struct net_device *dev;
 	struct net_dev_channel rx;
 	struct net_dev_channel tx;
-	struct completion mac_compl;
 	struct list_head list;
 };
 
 static struct list_head net_devices = LIST_HEAD_INIT(net_devices);
-static struct spinlock list_lock;
+static struct mutex probe_disc_mt; /* ch->linked = true, most_nd_open */
+static struct spinlock list_lock; /* list_head, ch->linked = false, dev_hold */
 static struct most_aim aim;
 
 static int skb_to_mamac(const struct sk_buff *skb, struct mbo *mbo)
@@ -157,14 +155,12 @@ static int skb_to_mep(const struct sk_buff *skb, struct mbo *mbo)
 
 static int most_nd_set_mac_address(struct net_device *dev, void *p)
 {
-	struct net_dev_context *nd = dev->ml_priv;
+	struct net_dev_context *nd = netdev_priv(dev);
 	int err = eth_mac_addr(dev, p);
 
 	if (err)
 		return err;
 
-	BUG_ON(nd->dev != dev);
-
 	nd->is_mamac =
 		(dev->dev_addr[0] == 0 && dev->dev_addr[1] == 0 &&
 		 dev->dev_addr[2] == 0 && dev->dev_addr[3] == 0);
@@ -178,71 +174,52 @@ static int most_nd_set_mac_address(struct net_device *dev, void *p)
 	return 0;
 }
 
+static void on_netinfo(struct most_interface *iface,
+		       unsigned char link_stat, unsigned char *mac_addr);
+
 static int most_nd_open(struct net_device *dev)
 {
-	struct net_dev_context *nd = dev->ml_priv;
-	long ret;
-
-	netdev_info(dev, "open net device\n");
-
-	BUG_ON(nd->dev != dev);
+	struct net_dev_context *nd = netdev_priv(dev);
+	int ret = 0;
 
-	if (nd->channels_opened)
-		return -EFAULT;
-
-	BUG_ON(!nd->tx.linked || !nd->rx.linked);
+	mutex_lock(&probe_disc_mt);
 
 	if (most_start_channel(nd->iface, nd->rx.ch_id, &aim)) {
 		netdev_err(dev, "most_start_channel() failed\n");
-		return -EBUSY;
+		ret = -EBUSY;
+		goto unlock;
 	}
 
 	if (most_start_channel(nd->iface, nd->tx.ch_id, &aim)) {
 		netdev_err(dev, "most_start_channel() failed\n");
 		most_stop_channel(nd->iface, nd->rx.ch_id, &aim);
-		return -EBUSY;
+		ret = -EBUSY;
+		goto unlock;
 	}
 
-	if (!is_valid_ether_addr(dev->dev_addr)) {
-		nd->iface->request_netinfo(nd->iface, nd->tx.ch_id);
-		ret = wait_for_completion_interruptible_timeout(
-			      &nd->mac_compl, msecs_to_jiffies(5000));
-		if (!ret) {
-			netdev_err(dev, "mac timeout\n");
-			ret = -EBUSY;
-			goto err;
-		}
-
-		if (ret < 0) {
-			netdev_warn(dev, "mac waiting interrupted\n");
-			goto err;
-		}
-	}
-
-	nd->channels_opened = true;
+	netif_carrier_off(dev);
+	if (is_valid_ether_addr(dev->dev_addr))
+		netif_dormant_off(dev);
+	else
+		netif_dormant_on(dev);
 	netif_wake_queue(dev);
-	return 0;
+	if (nd->iface->request_netinfo)
+		nd->iface->request_netinfo(nd->iface, nd->tx.ch_id, on_netinfo);
 
-err:
-	most_stop_channel(nd->iface, nd->tx.ch_id, &aim);
-	most_stop_channel(nd->iface, nd->rx.ch_id, &aim);
+unlock:
+	mutex_unlock(&probe_disc_mt);
 	return ret;
 }
 
 static int most_nd_stop(struct net_device *dev)
 {
-	struct net_dev_context *nd = dev->ml_priv;
-
-	netdev_info(dev, "stop net device\n");
+	struct net_dev_context *nd = netdev_priv(dev);
 
-	BUG_ON(nd->dev != dev);
 	netif_stop_queue(dev);
-
-	if (nd->channels_opened) {
-		most_stop_channel(nd->iface, nd->rx.ch_id, &aim);
-		most_stop_channel(nd->iface, nd->tx.ch_id, &aim);
-		nd->channels_opened = false;
-	}
+	if (nd->iface->request_netinfo)
+		nd->iface->request_netinfo(nd->iface, nd->tx.ch_id, NULL);
+	most_stop_channel(nd->iface, nd->rx.ch_id, &aim);
+	most_stop_channel(nd->iface, nd->tx.ch_id, &aim);
 
 	return 0;
 }
@@ -250,12 +227,10 @@ static int most_nd_stop(struct net_device *dev)
 static netdev_tx_t most_nd_start_xmit(struct sk_buff *skb,
 				      struct net_device *dev)
 {
-	struct net_dev_context *nd = dev->ml_priv;
+	struct net_dev_context *nd = netdev_priv(dev);
 	struct mbo *mbo;
 	int ret;
 
-	BUG_ON(nd->dev != dev);
-
 	mbo = most_get_mbo(nd->iface, nd->tx.ch_id, &aim);
 
 	if (!mbo) {
@@ -296,33 +271,29 @@ static void most_nd_setup(struct net_device *dev)
 	dev->netdev_ops = &most_nd_ops;
 }
 
-static void most_net_rm_netdev_safe(struct net_dev_context *nd)
+static struct net_dev_context *get_net_dev(struct most_interface *iface)
 {
-	if (!nd->dev)
-		return;
-
-	pr_info("remove net device %p\n", nd->dev);
+	struct net_dev_context *nd;
 
-	unregister_netdev(nd->dev);
-	free_netdev(nd->dev);
-	nd->dev = NULL;
+	list_for_each_entry(nd, &net_devices, list)
+		if (nd->iface == iface)
+			return nd;
+	return NULL;
 }
 
-static struct net_dev_context *get_net_dev_context(
-	struct most_interface *iface)
+static struct net_dev_context *get_net_dev_hold(struct most_interface *iface)
 {
-	struct net_dev_context *nd, *tmp;
+	struct net_dev_context *nd;
 	unsigned long flags;
 
 	spin_lock_irqsave(&list_lock, flags);
-	list_for_each_entry_safe(nd, tmp, &net_devices, list) {
-		if (nd->iface == iface) {
-			spin_unlock_irqrestore(&list_lock, flags);
-			return nd;
-		}
-	}
+	nd = get_net_dev(iface);
+	if (nd && nd->rx.linked && nd->tx.linked)
+		dev_hold(nd->dev);
+	else
+		nd = NULL;
 	spin_unlock_irqrestore(&list_lock, flags);
-	return NULL;
+	return nd;
 }
 
 static int aim_probe_channel(struct most_interface *iface, int channel_idx,
@@ -331,7 +302,9 @@ static int aim_probe_channel(struct most_interface *iface, int channel_idx,
 {
 	struct net_dev_context *nd;
 	struct net_dev_channel *ch;
+	struct net_device *dev;
 	unsigned long flags;
+	int ret = 0;
 
 	if (!iface)
 		return -EINVAL;
@@ -339,54 +312,45 @@ static int aim_probe_channel(struct most_interface *iface, int channel_idx,
 	if (ccfg->data_type != MOST_CH_ASYNC)
 		return -EINVAL;
 
-	nd = get_net_dev_context(iface);
-
+	mutex_lock(&probe_disc_mt);
+	nd = get_net_dev(iface);
 	if (!nd) {
-		nd = kzalloc(sizeof(*nd), GFP_KERNEL);
-		if (!nd)
-			return -ENOMEM;
+		dev = alloc_netdev(sizeof(struct net_dev_context), "meth%d",
+				   NET_NAME_UNKNOWN, most_nd_setup);
+		if (!dev) {
+			ret = -ENOMEM;
+			goto unlock;
+		}
 
-		init_completion(&nd->mac_compl);
+		nd = netdev_priv(dev);
 		nd->iface = iface;
+		nd->dev = dev;
 
 		spin_lock_irqsave(&list_lock, flags);
 		list_add(&nd->list, &net_devices);
 		spin_unlock_irqrestore(&list_lock, flags);
-	}
 
-	ch = ccfg->direction == MOST_CH_TX ? &nd->tx : &nd->rx;
-	if (ch->linked) {
-		pr_err("only one channel per instance & direction allowed\n");
-		return -EINVAL;
-	}
-
-	if (nd->tx.linked || nd->rx.linked) {
-		struct net_device *dev =
-			alloc_netdev(0, "meth%d", NET_NAME_UNKNOWN,
-				     most_nd_setup);
-
-		if (!dev) {
-			pr_err("no memory for net_device\n");
-			return -ENOMEM;
+		ch = ccfg->direction == MOST_CH_TX ? &nd->tx : &nd->rx;
+	} else {
+		ch = ccfg->direction == MOST_CH_TX ? &nd->tx : &nd->rx;
+		if (ch->linked) {
+			pr_err("direction is allocated\n");
+			ret = -EINVAL;
+			goto unlock;
 		}
 
-		nd->dev = dev;
-		ch->ch_id = channel_idx;
-		ch->linked = true;
-
-		dev->ml_priv = nd;
-		if (register_netdev(dev)) {
-			pr_err("registering net device failed\n");
-			ch->linked = false;
-			free_netdev(dev);
-			return -EINVAL;
+		if (register_netdev(nd->dev)) {
+			pr_err("register_netdev() failed\n");
+			ret = -EINVAL;
+			goto unlock;
 		}
 	}
-
 	ch->ch_id = channel_idx;
 	ch->linked = true;
 
-	return 0;
+unlock:
+	mutex_unlock(&probe_disc_mt);
+	return ret;
 }
 
 static int aim_disconnect_channel(struct most_interface *iface,
@@ -395,34 +359,45 @@ static int aim_disconnect_channel(struct most_interface *iface,
 	struct net_dev_context *nd;
 	struct net_dev_channel *ch;
 	unsigned long flags;
+	int ret = 0;
 
-	nd = get_net_dev_context(iface);
-	if (!nd)
-		return -EINVAL;
+	mutex_lock(&probe_disc_mt);
+	nd = get_net_dev(iface);
+	if (!nd) {
+		ret = -EINVAL;
+		goto unlock;
+	}
 
-	if (nd->rx.linked && channel_idx == nd->rx.ch_id)
+	if (nd->rx.linked && channel_idx == nd->rx.ch_id) {
 		ch = &nd->rx;
-	else if (nd->tx.linked && channel_idx == nd->tx.ch_id)
+	} else if (nd->tx.linked && channel_idx == nd->tx.ch_id) {
 		ch = &nd->tx;
-	else
-		return -EINVAL;
-
-	ch->linked = false;
+	} else {
+		ret = -EINVAL;
+		goto unlock;
+	}
 
-	/*
-	 * do not call most_stop_channel() here, because channels are
-	 * going to be closed in ndo_stop() after unregister_netdev()
-	 */
-	most_net_rm_netdev_safe(nd);
+	if (nd->rx.linked && nd->tx.linked) {
+		spin_lock_irqsave(&list_lock, flags);
+		ch->linked = false;
+		spin_unlock_irqrestore(&list_lock, flags);
 
-	if (!nd->rx.linked && !nd->tx.linked) {
+		/*
+		 * do not call most_stop_channel() here, because channels are
+		 * going to be closed in ndo_stop() after unregister_netdev()
+		 */
+		unregister_netdev(nd->dev);
+	} else {
 		spin_lock_irqsave(&list_lock, flags);
 		list_del(&nd->list);
 		spin_unlock_irqrestore(&list_lock, flags);
-		kfree(nd);
+
+		free_netdev(nd->dev);
 	}
 
-	return 0;
+unlock:
+	mutex_unlock(&probe_disc_mt);
+	return ret;
 }
 
 static int aim_resume_tx_channel(struct most_interface *iface,
@@ -430,14 +405,17 @@ static int aim_resume_tx_channel(struct most_interface *iface,
 {
 	struct net_dev_context *nd;
 
-	nd = get_net_dev_context(iface);
-	if (!nd || !nd->channels_opened || nd->tx.ch_id != channel_idx)
+	nd = get_net_dev_hold(iface);
+	if (!nd)
 		return 0;
 
-	if (!nd->dev)
-		return 0;
+	if (nd->tx.ch_id != channel_idx)
+		goto put_nd;
 
 	netif_wake_queue(nd->dev);
+
+put_nd:
+	dev_put(nd->dev);
 	return 0;
 }
 
@@ -450,25 +428,31 @@ static int aim_rx_data(struct mbo *mbo)
 	struct sk_buff *skb;
 	struct net_device *dev;
 	unsigned int skb_len;
+	int ret = 0;
 
-	nd = get_net_dev_context(mbo->ifp);
-	if (!nd || !nd->channels_opened || nd->rx.ch_id != mbo->hdm_channel_id)
+	nd = get_net_dev_hold(mbo->ifp);
+	if (!nd)
 		return -EIO;
 
-	dev = nd->dev;
-	if (!dev) {
-		pr_err_once("drop packet: missing net_device\n");
-		return -EIO;
+	if (nd->rx.ch_id != mbo->hdm_channel_id) {
+		ret = -EIO;
+		goto put_nd;
 	}
 
+	dev = nd->dev;
+
 	if (nd->is_mamac) {
-		if (!PMS_IS_MAMAC(buf, len))
-			return -EIO;
+		if (!PMS_IS_MAMAC(buf, len)) {
+			ret = -EIO;
+			goto put_nd;
+		}
 
 		skb = dev_alloc_skb(len - MDP_HDR_LEN + 2 * ETH_ALEN + 2);
 	} else {
-		if (!PMS_IS_MEP(buf, len))
-			return -EIO;
+		if (!PMS_IS_MEP(buf, len)) {
+			ret = -EIO;
+			goto put_nd;
+		}
 
 		skb = dev_alloc_skb(len - MEP_HDR_LEN);
 	}
@@ -486,11 +470,11 @@ static int aim_rx_data(struct mbo *mbo)
 		ether_addr_copy(skb_put(skb, ETH_ALEN), dev->dev_addr);
 
 		/* src */
-		memcpy(skb_put(skb, 4), &zero, 4);
-		memcpy(skb_put(skb, 2), buf + 5, 2);
+		skb_put_data(skb, &zero, 4);
+		skb_put_data(skb, buf + 5, 2);
 
 		/* eth type */
-		memcpy(skb_put(skb, 2), buf + 10, 2);
+		skb_put_data(skb, buf + 10, 2);
 
 		buf += MDP_HDR_LEN;
 		len -= MDP_HDR_LEN;
@@ -499,7 +483,7 @@ static int aim_rx_data(struct mbo *mbo)
 		len -= MEP_HDR_LEN;
 	}
 
-	memcpy(skb_put(skb, len), buf, len);
+	skb_put_data(skb, buf, len);
 	skb->protocol = eth_type_trans(skb, dev);
 	skb_len = skb->len;
 	if (netif_rx(skb) == NET_RX_SUCCESS) {
@@ -511,7 +495,10 @@ static int aim_rx_data(struct mbo *mbo)
 
 out:
 	most_put_mbo(mbo);
-	return 0;
+
+put_nd:
+	dev_put(nd->dev);
+	return ret;
 }
 
 static struct most_aim aim = {
@@ -524,68 +511,54 @@ static struct most_aim aim = {
 
 static int __init most_net_init(void)
 {
-	pr_info("most_net_init()\n");
 	spin_lock_init(&list_lock);
+	mutex_init(&probe_disc_mt);
 	return most_register_aim(&aim);
 }
 
 static void __exit most_net_exit(void)
 {
-	struct net_dev_context *nd, *tmp;
-	unsigned long flags;
-
-	spin_lock_irqsave(&list_lock, flags);
-	list_for_each_entry_safe(nd, tmp, &net_devices, list) {
-		list_del(&nd->list);
-		spin_unlock_irqrestore(&list_lock, flags);
-		/*
-		 * do not call most_stop_channel() here, because channels are
-		 * going to be closed in ndo_stop() after unregister_netdev()
-		 */
-		most_net_rm_netdev_safe(nd);
-		kfree(nd);
-		spin_lock_irqsave(&list_lock, flags);
-	}
-	spin_unlock_irqrestore(&list_lock, flags);
-
 	most_deregister_aim(&aim);
-	pr_info("most_net_exit()\n");
 }
 
 /**
- * most_deliver_netinfo - callback for HDM to be informed about HW's MAC
+ * on_netinfo - callback for HDM to be informed about HW's MAC
  * @param iface - most interface instance
  * @param link_stat - link status
  * @param mac_addr - MAC address
  */
-void most_deliver_netinfo(struct most_interface *iface,
-			  unsigned char link_stat, unsigned char *mac_addr)
+static void on_netinfo(struct most_interface *iface,
+		       unsigned char link_stat, unsigned char *mac_addr)
 {
 	struct net_dev_context *nd;
 	struct net_device *dev;
 	const u8 *m = mac_addr;
 
-	nd = get_net_dev_context(iface);
+	nd = get_net_dev_hold(iface);
 	if (!nd)
 		return;
 
 	dev = nd->dev;
-	if (!dev)
-		return;
+
+	if (link_stat)
+		netif_carrier_on(dev);
+	else
+		netif_carrier_off(dev);
 
 	if (m && is_valid_ether_addr(m)) {
 		if (!is_valid_ether_addr(dev->dev_addr)) {
 			netdev_info(dev, "set mac %02x-%02x-%02x-%02x-%02x-%02x\n",
 				    m[0], m[1], m[2], m[3], m[4], m[5]);
 			ether_addr_copy(dev->dev_addr, m);
-			complete(&nd->mac_compl);
+			netif_dormant_off(dev);
 		} else if (!ether_addr_equal(dev->dev_addr, m)) {
 			netdev_warn(dev, "reject mac %02x-%02x-%02x-%02x-%02x-%02x\n",
 				    m[0], m[1], m[2], m[3], m[4], m[5]);
 		}
 	}
+
+	dev_put(nd->dev);
 }
-EXPORT_SYMBOL(most_deliver_netinfo);
 
 module_init(most_net_init);
 module_exit(most_net_exit);
diff --git a/drivers/staging/most/aim-network/networking.h b/drivers/staging/most/aim-network/networking.h
deleted file mode 100644
index 6f346d410525..000000000000
--- a/drivers/staging/most/aim-network/networking.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- * Networking AIM - Networking Application Interface Module for MostCore
- *
- * Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT 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 licensed under GPLv2.
- */
-#ifndef _NETWORKING_H_
-#define _NETWORKING_H_
-
-#include "mostcore.h"
-
-void most_deliver_netinfo(struct most_interface *iface,
-			  unsigned char link_stat, unsigned char *mac_addr);
-
-#endif
diff --git a/drivers/staging/most/hdm-dim2/Kconfig b/drivers/staging/most/hdm-dim2/Kconfig
index 28a0e1791600..663bfebff674 100644
--- a/drivers/staging/most/hdm-dim2/Kconfig
+++ b/drivers/staging/most/hdm-dim2/Kconfig
@@ -4,7 +4,6 @@
 
 config HDM_DIM2
 	tristate "DIM2 HDM"
-	depends on AIM_NETWORK
 	depends on HAS_IOMEM
 
 	---help---
diff --git a/drivers/staging/most/hdm-dim2/dim2_hal.c b/drivers/staging/most/hdm-dim2/dim2_hal.c
index d604ec09df28..91484643d289 100644
--- a/drivers/staging/most/hdm-dim2/dim2_hal.c
+++ b/drivers/staging/most/hdm-dim2/dim2_hal.c
@@ -217,12 +217,15 @@ static inline void dim2_clear_ctr(u32 ctr_addr)
 }
 
 static void dim2_configure_cat(u8 cat_base, u8 ch_addr, u8 ch_type,
-			       bool read_not_write, bool sync_mfe)
+			       bool read_not_write)
 {
+	bool isoc_fce = ch_type == CAT_CT_VAL_ISOC;
+	bool sync_mfe = ch_type == CAT_CT_VAL_SYNC;
 	u16 const cat =
 		(read_not_write << CAT_RNW_BIT) |
 		(ch_type << CAT_CT_SHIFT) |
 		(ch_addr << CAT_CL_SHIFT) |
+		(isoc_fce << CAT_FCE_BIT) |
 		(sync_mfe << CAT_MFE_BIT) |
 		(false << CAT_MT_BIT) |
 		(true << CAT_CE_BIT);
@@ -350,13 +353,13 @@ static void dim2_clear_ctram(void)
 
 static void dim2_configure_channel(
 	u8 ch_addr, u8 type, u8 is_tx, u16 dbr_address, u16 hw_buffer_size,
-	u16 packet_length, bool sync_mfe)
+	u16 packet_length)
 {
 	dim2_configure_cdt(ch_addr, dbr_address, hw_buffer_size, packet_length);
-	dim2_configure_cat(MLB_CAT, ch_addr, type, is_tx ? 1 : 0, sync_mfe);
+	dim2_configure_cat(MLB_CAT, ch_addr, type, is_tx ? 1 : 0);
 
 	dim2_configure_adt(ch_addr);
-	dim2_configure_cat(AHB_CAT, ch_addr, type, is_tx ? 0 : 1, sync_mfe);
+	dim2_configure_cat(AHB_CAT, ch_addr, type, is_tx ? 0 : 1);
 
 	/* unmask interrupt for used channel, enable mlb_sys_int[0] interrupt */
 	dimcb_io_write(&g.dim2->ACMR0,
@@ -771,7 +774,7 @@ static u8 init_ctrl_async(struct dim_channel *ch, u8 type, u8 is_tx,
 	channel_init(ch, ch_address / 2);
 
 	dim2_configure_channel(ch->addr, type, is_tx,
-			       ch->dbr_addr, ch->dbr_size, 0, false);
+			       ch->dbr_addr, ch->dbr_size, 0);
 
 	return DIM_NO_ERROR;
 }
@@ -857,7 +860,7 @@ u8 dim_init_isoc(struct dim_channel *ch, u8 is_tx, u16 ch_address,
 	isoc_init(ch, ch_address / 2, packet_length);
 
 	dim2_configure_channel(ch->addr, CAT_CT_VAL_ISOC, is_tx, ch->dbr_addr,
-			       ch->dbr_size, packet_length, false);
+			       ch->dbr_size, packet_length);
 
 	return DIM_NO_ERROR;
 }
@@ -885,7 +888,7 @@ u8 dim_init_sync(struct dim_channel *ch, u8 is_tx, u16 ch_address,
 
 	dim2_clear_dbr(ch->dbr_addr, ch->dbr_size);
 	dim2_configure_channel(ch->addr, CAT_CT_VAL_SYNC, is_tx,
-			       ch->dbr_addr, ch->dbr_size, 0, true);
+			       ch->dbr_addr, ch->dbr_size, 0);
 
 	return DIM_NO_ERROR;
 }
diff --git a/drivers/staging/most/hdm-dim2/dim2_hdm.c b/drivers/staging/most/hdm-dim2/dim2_hdm.c
index 902824e728ea..4607d03c577b 100644
--- a/drivers/staging/most/hdm-dim2/dim2_hdm.c
+++ b/drivers/staging/most/hdm-dim2/dim2_hdm.c
@@ -26,7 +26,6 @@
 #include <linux/kthread.h>
 
 #include <mostcore.h>
-#include <networking.h>
 #include "dim2_hal.h"
 #include "dim2_hdm.h"
 #include "dim2_errors.h"
@@ -107,6 +106,8 @@ struct dim2_hdm {
 	unsigned char link_state;
 	int atx_idx;
 	struct medialb_bus bus;
+	void (*on_netinfo)(struct most_interface *,
+			   unsigned char, unsigned char *);
 };
 
 #define iface_to_hdm(iface) container_of(iface, struct dim2_hdm, most_iface)
@@ -287,8 +288,11 @@ static int deliver_netinfo_thread(void *data)
 
 		if (dev->deliver_netinfo) {
 			dev->deliver_netinfo--;
-			most_deliver_netinfo(&dev->most_iface, dev->link_state,
-					     dev->mac_addrs);
+			if (dev->on_netinfo) {
+				dev->on_netinfo(&dev->most_iface,
+						dev->link_state,
+						dev->mac_addrs);
+			}
 		}
 	}
 
@@ -654,12 +658,18 @@ static int enqueue(struct most_interface *most_iface, int ch_idx,
  * Send a command to INIC which triggers retrieving of network info by means of
  * "Message exchange over MDP/MEP". Return 0 on success, negative on failure.
  */
-static void request_netinfo(struct most_interface *most_iface, int ch_idx)
+static void request_netinfo(struct most_interface *most_iface, int ch_idx,
+			    void (*on_netinfo)(struct most_interface *,
+					       unsigned char, unsigned char *))
 {
 	struct dim2_hdm *dev = iface_to_hdm(most_iface);
 	struct mbo *mbo;
 	u8 *data;
 
+	dev->on_netinfo = on_netinfo;
+	if (!on_netinfo)
+		return;
+
 	if (dev->atx_idx < 0) {
 		pr_err("Async Tx Not initialized\n");
 		return;
diff --git a/drivers/staging/most/hdm-dim2/dim2_reg.h b/drivers/staging/most/hdm-dim2/dim2_reg.h
index 01fe499411ff..f7d9fbcd29f2 100644
--- a/drivers/staging/most/hdm-dim2/dim2_reg.h
+++ b/drivers/staging/most/hdm-dim2/dim2_reg.h
@@ -141,6 +141,7 @@ enum {
 	ADT1_CTRL_ASYNC_BD_MASK = DIM2_MASK(11),
 	ADT1_ISOC_SYNC_BD_MASK = DIM2_MASK(13),
 
+	CAT_FCE_BIT = 14,
 	CAT_MFE_BIT = 14,
 
 	CAT_MT_BIT = 13,
diff --git a/drivers/staging/most/hdm-i2c/hdm_i2c.c b/drivers/staging/most/hdm-i2c/hdm_i2c.c
index 1d5b22927bcd..2b4de404e46a 100644
--- a/drivers/staging/most/hdm-i2c/hdm_i2c.c
+++ b/drivers/staging/most/hdm-i2c/hdm_i2c.c
@@ -185,12 +185,6 @@ static int poison_channel(struct most_interface *most_iface,
 	return 0;
 }
 
-static void request_netinfo(struct most_interface *most_iface,
-			    int ch_idx)
-{
-	pr_info("request_netinfo()\n");
-}
-
 static void do_rx_work(struct hdm_i2c *dev)
 {
 	struct mbo *mbo;
@@ -343,7 +337,6 @@ static int i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	dev->most_iface.configure = configure_channel;
 	dev->most_iface.enqueue = enqueue;
 	dev->most_iface.poison_channel = poison_channel;
-	dev->most_iface.request_netinfo = request_netinfo;
 
 	INIT_LIST_HEAD(&dev->rx.list);
 	mutex_init(&dev->rx.list_mutex);
diff --git a/drivers/staging/most/hdm-usb/Kconfig b/drivers/staging/most/hdm-usb/Kconfig
index ec1546312ee6..487f1f34776c 100644
--- a/drivers/staging/most/hdm-usb/Kconfig
+++ b/drivers/staging/most/hdm-usb/Kconfig
@@ -5,7 +5,7 @@
 config HDM_USB
 	tristate "USB HDM"
 	depends on USB && NET
-	select AIM_NETWORK
+
 	---help---
 	  Say Y here if you want to connect via USB to network tranceiver.
 	  This device driver depends on the networking AIM.
diff --git a/drivers/staging/most/hdm-usb/hdm_usb.c b/drivers/staging/most/hdm-usb/hdm_usb.c
index a95b5910d9fc..d0f68cb3c173 100644
--- a/drivers/staging/most/hdm-usb/hdm_usb.c
+++ b/drivers/staging/most/hdm-usb/hdm_usb.c
@@ -30,7 +30,6 @@
 #include <linux/etherdevice.h>
 #include <linux/uaccess.h>
 #include "mostcore.h"
-#include "networking.h"
 
 #define USB_MTU			512
 #define NO_ISOCHRONOUS_URB	0
@@ -126,6 +125,8 @@ struct most_dev {
 	struct mutex io_mutex;
 	struct timer_list link_stat_timer;
 	struct work_struct poll_work_obj;
+	void (*on_netinfo)(struct most_interface *, unsigned char,
+			   unsigned char *);
 };
 
 #define to_mdev(d) container_of(d, struct most_dev, iface)
@@ -719,12 +720,19 @@ exit:
  * polls for the NI state of the INIC every 2 seconds.
  *
  */
-static void hdm_request_netinfo(struct most_interface *iface, int channel)
+static void hdm_request_netinfo(struct most_interface *iface, int channel,
+				void (*on_netinfo)(struct most_interface *,
+						   unsigned char,
+						   unsigned char *))
 {
 	struct most_dev *mdev;
 
 	BUG_ON(!iface);
 	mdev = to_mdev(iface);
+	mdev->on_netinfo = on_netinfo;
+	if (!on_netinfo)
+		return;
+
 	mdev->link_stat_timer.expires = jiffies + HZ;
 	mod_timer(&mdev->link_stat_timer, mdev->link_stat_timer.expires);
 }
@@ -786,7 +794,8 @@ static void wq_netinfo(struct work_struct *wq_obj)
 	hw_addr[4] = lo >> 8;
 	hw_addr[5] = lo;
 
-	most_deliver_netinfo(&mdev->iface, link, hw_addr);
+	if (mdev->on_netinfo)
+		mdev->on_netinfo(&mdev->iface, link, hw_addr);
 }
 
 /**
diff --git a/drivers/staging/most/mostcore/mostcore.h b/drivers/staging/most/mostcore/mostcore.h
index 5f8339bd046f..915e5159d1eb 100644
--- a/drivers/staging/most/mostcore/mostcore.h
+++ b/drivers/staging/most/mostcore/mostcore.h
@@ -233,6 +233,8 @@ struct mbo {
  *   The callback returns a negative value on error, otherwise 0.
  * @request_netinfo: triggers retrieving of network info from the HDM by
  *   means of "Message exchange over MDP/MEP"
+ *   The call of the function request_netinfo with the parameter on_netinfo as
+ *   NULL prohibits use of the previously obtained function pointer.
  * @priv Private field used by mostcore to store context information.
  */
 struct most_interface {
@@ -246,7 +248,10 @@ struct most_interface {
 	int (*enqueue)(struct most_interface *iface, int channel_idx,
 		       struct mbo *mbo);
 	int (*poison_channel)(struct most_interface *iface, int channel_idx);
-	void (*request_netinfo)(struct most_interface *iface, int channel_idx);
+	void (*request_netinfo)(struct most_interface *iface, int channel_idx,
+				void (*on_netinfo)(struct most_interface *iface,
+						   unsigned char link_stat,
+						   unsigned char *mac_addr));
 	void *priv;
 };
 
diff --git a/drivers/staging/netlogic/xlr_net.c b/drivers/staging/netlogic/xlr_net.c
index 781ef623233e..e05ae4645d91 100644
--- a/drivers/staging/netlogic/xlr_net.c
+++ b/drivers/staging/netlogic/xlr_net.c
@@ -179,7 +179,10 @@ static int xlr_get_link_ksettings(struct net_device *ndev,
 
 	if (!phydev)
 		return -ENODEV;
-	return phy_ethtool_ksettings_get(phydev, ecmd);
+
+	phy_ethtool_ksettings_get(phydev, ecmd);
+
+	return 0;
 }
 
 static int xlr_set_link_ksettings(struct net_device *ndev,
diff --git a/drivers/staging/octeon-usb/octeon-hcd.c b/drivers/staging/octeon-usb/octeon-hcd.c
index 9a7858a300fd..068aece25d37 100644
--- a/drivers/staging/octeon-usb/octeon-hcd.c
+++ b/drivers/staging/octeon-usb/octeon-hcd.c
@@ -3659,14 +3659,14 @@ static int octeon_usb_probe(struct platform_device *pdev)
 	status = cvmx_usb_initialize(dev, usb);
 	if (status) {
 		dev_dbg(dev, "USB initialization failed with %d\n", status);
-		kfree(hcd);
+		usb_put_hcd(hcd);
 		return -1;
 	}
 
 	status = usb_add_hcd(hcd, irq, 0);
 	if (status) {
 		dev_dbg(dev, "USB add HCD failed with %d\n", status);
-		kfree(hcd);
+		usb_put_hcd(hcd);
 		return -1;
 	}
 	device_wakeup_enable(hcd->self.controller);
@@ -3691,7 +3691,7 @@ static int octeon_usb_remove(struct platform_device *pdev)
 	if (status)
 		dev_dbg(dev, "USB shutdown failed with %d\n", status);
 
-	kfree(hcd);
+	usb_put_hcd(hcd);
 
 	return 0;
 }
diff --git a/drivers/staging/octeon/ethernet-rx.c b/drivers/staging/octeon/ethernet-rx.c
index 65a285631994..72baedefa0f1 100644
--- a/drivers/staging/octeon/ethernet-rx.c
+++ b/drivers/staging/octeon/ethernet-rx.c
@@ -287,8 +287,7 @@ static int cvm_oct_poll(struct oct_rx_group *rx_group, int budget)
 					else
 						ptr += 6;
 				}
-				memcpy(skb_put(skb, work->word1.len), ptr,
-				       work->word1.len);
+				skb_put_data(skb, ptr, work->word1.len);
 				/* No packet buffers to free */
 			} else {
 				int segments = work->word2.s.bufs;
@@ -323,10 +322,9 @@ static int cvm_oct_poll(struct oct_rx_group *rx_group, int budget)
 					if (segment_size > len)
 						segment_size = len;
 					/* Copy the data into the packet */
-					memcpy(skb_put(skb, segment_size),
-					       cvmx_phys_to_ptr(
-					       segment_ptr.s.addr),
-					       segment_size);
+					skb_put_data(skb,
+						     cvmx_phys_to_ptr(segment_ptr.s.addr),
+						     segment_size);
 					len -= segment_size;
 					segment_ptr = next_ptr;
 				}
diff --git a/drivers/staging/octeon/ethernet-tx.c b/drivers/staging/octeon/ethernet-tx.c
index ff4119e8de42..31f35025d19e 100644
--- a/drivers/staging/octeon/ethernet-tx.c
+++ b/drivers/staging/octeon/ethernet-tx.c
@@ -251,8 +251,7 @@ int cvm_oct_xmit(struct sk_buff *skb, struct net_device *dev)
 
 				if ((skb_tail_pointer(skb) + add_bytes) <=
 				    skb_end_pointer(skb))
-					memset(__skb_put(skb, add_bytes), 0,
-					       add_bytes);
+					__skb_put_zero(skb, add_bytes);
 			}
 		}
 	}
diff --git a/drivers/staging/octeon/ethernet-util.h b/drivers/staging/octeon/ethernet-util.h
index 617da8037a4d..cb5540dc0e9d 100644
--- a/drivers/staging/octeon/ethernet-util.h
+++ b/drivers/staging/octeon/ethernet-util.h
@@ -39,7 +39,7 @@ static inline int INTERFACE(int ipd_port)
 	interface = cvmx_helper_get_interface_num(ipd_port);
 	if (interface >= 0)
 		return interface;
-	panic("Illegal ipd_port %d passed to INTERFACE\n", ipd_port);
+	panic("Illegal ipd_port %d passed to %s\n", ipd_port, __func__);
 }
 
 /**
diff --git a/drivers/staging/rtl8188eu/core/rtw_ap.c b/drivers/staging/rtl8188eu/core/rtw_ap.c
index 519b4d3584a2..647a922d79d1 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ap.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ap.c
@@ -735,9 +735,11 @@ static void start_bss_network(struct adapter *padapter, u8 *pbuf)
 	cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 
 
-	/* check if there is wps ie, */
-	/* if there is wpsie in beacon, the hostapd will update beacon twice when stating hostapd, */
-	/* and at first time the security ie (RSN/WPA IE) will not include in beacon. */
+	/* check if there is wps ie,
+	 * if there is wpsie in beacon, the hostapd will update
+	 * beacon twice when stating hostapd, and at first time the
+	 * security ie (RSN/WPA IE) will not include in beacon.
+	 */
 	if (!rtw_get_wps_ie(pnetwork->IEs + _FIXED_IE_LENGTH_, pnetwork->IELength - _FIXED_IE_LENGTH_, NULL, NULL))
 		pmlmeext->bstart_bss = true;
 
@@ -751,8 +753,11 @@ static void start_bss_network(struct adapter *padapter, u8 *pbuf)
 		update_hw_ht_param(padapter);
 	}
 
-	if (pmlmepriv->cur_network.join_res != true) { /* setting only at  first time */
-		/* WEP Key will be set before this function, do not clear CAM. */
+	/* setting only at  first time */
+	if (!(pmlmepriv->cur_network.join_res)) {
+		/* WEP Key will be set before this function, do not
+		 * clear CAM.
+		 */
 		if ((psecuritypriv->dot11PrivacyAlgrthm != _WEP40_) &&
 		    (psecuritypriv->dot11PrivacyAlgrthm != _WEP104_))
 			flush_all_cam_entry(padapter);	/* clear CAM */
@@ -809,7 +814,9 @@ static void start_bss_network(struct adapter *padapter, u8 *pbuf)
 			}
 		}
 	}
-	/* TODO: need to judge the phy parameters on concurrent mode for single phy */
+	/* TODO: need to judge the phy parameters on concurrent
+	 * mode for single phy
+	 */
 	set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
 
 	DBG_88E("CH =%d, BW =%d, offset =%d\n", cur_channel, cur_bwmode, cur_ch_offset);
@@ -991,7 +998,7 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 			}
 			break;
 		}
-		if ((p == NULL) || (ie_len == 0))
+		if ((!p) || (ie_len == 0))
 			break;
 	}
 
@@ -1005,9 +1012,12 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 			if ((p) && !memcmp(p + 2, WMM_PARA_IE, 6)) {
 				pmlmepriv->qospriv.qos_option = 1;
 
-				*(p + 8) |= BIT(7);/* QoS Info, support U-APSD */
+				/* QoS Info, support U-APSD */
+				*(p + 8) |= BIT(7);
 
-				/* disable all ACM bits since the WMM admission control is not supported */
+				/* disable all ACM bits since the WMM
+				 * admission control is not supported
+				 */
 				*(p + 10) &= ~BIT(4); /* BE */
 				*(p + 14) &= ~BIT(4); /* BK */
 				*(p + 18) &= ~BIT(4); /* VI */
@@ -1015,7 +1025,7 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 				break;
 			}
 
-			if ((p == NULL) || (ie_len == 0))
+			if ((!p) || (ie_len == 0))
 				break;
 		}
 	}
@@ -1097,7 +1107,7 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	psta = rtw_get_stainfo(&padapter->stapriv, pbss_network->MacAddress);
 	if (!psta) {
 		psta = rtw_alloc_stainfo(&padapter->stapriv, pbss_network->MacAddress);
-		if (psta == NULL)
+		if (!psta)
 			return _FAIL;
 	}
 
@@ -1268,12 +1278,12 @@ static void update_bcn_wps_ie(struct adapter *padapter)
 	DBG_88E("%s\n", __func__);
 
 	pwps_ie_src = pmlmepriv->wps_beacon_ie;
-	if (pwps_ie_src == NULL)
+	if (!pwps_ie_src)
 		return;
 
 	pwps_ie = rtw_get_wps_ie(ie+_FIXED_IE_LENGTH_, ielen-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
 
-	if (pwps_ie == NULL || wps_ielen == 0)
+	if (!pwps_ie || wps_ielen == 0)
 		return;
 
 	wps_offset = (uint)(pwps_ie-ie);
@@ -1834,7 +1844,9 @@ void stop_ap_mode(struct adapter *padapter)
 	pmlmepriv->update_bcn = false;
 	pmlmeext->bstart_bss = false;
 
-	/* reset and init security priv , this can refine with rtw_reset_securitypriv */
+	/* reset and init security priv , this can refine with
+	 * rtw_reset_securitypriv
+	 */
 	memset((unsigned char *)&padapter->securitypriv, 0, sizeof(struct security_priv));
 	padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
 	padapter->securitypriv.ndisencryptstatus = Ndis802_11WEPDisabled;
diff --git a/drivers/staging/rtl8188eu/core/rtw_cmd.c b/drivers/staging/rtl8188eu/core/rtw_cmd.c
index 9754322b506e..002d09159896 100644
--- a/drivers/staging/rtl8188eu/core/rtw_cmd.c
+++ b/drivers/staging/rtl8188eu/core/rtw_cmd.c
@@ -21,35 +21,33 @@
 #include <rtw_mlme_ext.h>
 
 /*
-Caller and the rtw_cmd_thread can protect cmd_q by spin_lock.
-No irqsave is necessary.
-*/
+ * Caller and the rtw_cmd_thread can protect cmd_q by spin_lock.
+ * No irqsave is necessary.
+ */
 
 int rtw_init_cmd_priv(struct cmd_priv *pcmdpriv)
 {
 	init_completion(&pcmdpriv->cmd_queue_comp);
 	init_completion(&pcmdpriv->terminate_cmdthread_comp);
 
-	_rtw_init_queue(&(pcmdpriv->cmd_queue));
+	_rtw_init_queue(&pcmdpriv->cmd_queue);
 	return _SUCCESS;
 }
 
 /*
-Calling Context:
-
-rtw_enqueue_cmd can only be called between kernel thread,
-since only spin_lock is used.
-
-ISR/Call-Back functions can't call this sub-function.
-
-*/
+ * Calling Context:
+ *
+ * rtw_enqueue_cmd can only be called between kernel thread,
+ * since only spin_lock is used.
+ *
+ * ISR/Call-Back functions can't call this sub-function.
+ */
 
 static int _rtw_enqueue_cmd(struct __queue *queue, struct cmd_obj *obj)
 {
 	unsigned long irqL;
 
-
-	if (obj == NULL)
+	if (!obj)
 		goto exit;
 
 	spin_lock_irqsave(&queue->lock, irqL);
@@ -60,7 +58,6 @@ static int _rtw_enqueue_cmd(struct __queue *queue, struct cmd_obj *obj)
 
 exit:
 
-
 	return _SUCCESS;
 }
 
@@ -107,8 +104,7 @@ u32 rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 	int res = _FAIL;
 	struct adapter *padapter = pcmdpriv->padapter;
 
-
-	if (cmd_obj == NULL)
+	if (!cmd_obj)
 		goto exit;
 
 	cmd_obj->padapter = padapter;
@@ -126,19 +122,17 @@ u32 rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 
 exit:
 
-
 	return res;
 }
 
 void rtw_free_cmd_obj(struct cmd_obj *pcmd)
 {
-
 	if ((pcmd->cmdcode != _JoinBss_CMD_) && (pcmd->cmdcode != _CreateBss_CMD_)) {
 		/* free parmbuf in cmd_obj */
 		kfree(pcmd->parmbuf);
 	}
 
-	if (pcmd->rsp != NULL) {
+	if (!pcmd->rsp) {
 		if (pcmd->rspsz != 0) {
 			/* free rsp in cmd_obj */
 			kfree(pcmd->rsp);
@@ -147,7 +141,6 @@ void rtw_free_cmd_obj(struct cmd_obj *pcmd)
 
 	/* free cmd_obj */
 	kfree(pcmd);
-
 }
 
 int rtw_cmd_thread(void *context)
@@ -157,14 +150,15 @@ int rtw_cmd_thread(void *context)
 	u8 (*cmd_hdl)(struct adapter *padapter, u8 *pbuf);
 	void (*pcmd_callback)(struct adapter *dev, struct cmd_obj *pcmd);
 	struct adapter *padapter = context;
-	struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
 	allow_signal(SIGTERM);
 
 	pcmdpriv->cmdthd_running = true;
 	complete(&pcmdpriv->terminate_cmdthread_comp);
 
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("start r871x rtw_cmd_thread !!!!\n"));
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
+		 ("start r871x %s !!!!\n", __func__));
 
 	while (1) {
 		if (wait_for_completion_interruptible(&pcmdpriv->cmd_queue_comp))
@@ -208,7 +202,7 @@ _next:
 		/* call callback function for post-processed */
 		if (pcmd->cmdcode < ARRAY_SIZE(rtw_cmd_callback)) {
 			pcmd_callback = rtw_cmd_callback[pcmd->cmdcode].callback;
-			if (pcmd_callback == NULL) {
+			if (!pcmd_callback) {
 				RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("mlme_cmd_hdl(): pcmd_callback = 0x%p, cmdcode = 0x%x\n", pcmd_callback, pcmd->cmdcode));
 				rtw_free_cmd_obj(pcmd);
 			} else {
@@ -236,15 +230,15 @@ _next:
 
 	complete(&pcmdpriv->terminate_cmdthread_comp);
 
-
 	complete_and_exit(NULL, 0);
 }
 
 /*
-rtw_sitesurvey_cmd(~)
-	### NOTE:#### (!!!!)
-	MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock
-*/
+ * rtw_sitesurvey_cmd(~)
+ * ### NOTE:#### (!!!!)
+ * MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE
+ * LOCKED pmlmepriv->lock
+ */
 u8 rtw_sitesurvey_cmd(struct adapter  *padapter, struct ndis_802_11_ssid *ssid, int ssid_num,
 	struct rtw_ieee80211_channel *ch, int ch_num)
 {
@@ -315,13 +309,11 @@ u8 rtw_sitesurvey_cmd(struct adapter  *padapter, struct ndis_802_11_ssid *ssid,
 		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
 	}
 
-
 	return res;
 }
 
 void rtw_readtssi_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
 {
-
 	kfree(pcmd->parmbuf);
 	kfree(pcmd);
 }
@@ -334,7 +326,6 @@ u8 rtw_createbss_cmd(struct adapter  *padapter)
 	struct wlan_bssid_ex *pdev_network = &padapter->registrypriv.dev_network;
 	u8	res = _SUCCESS;
 
-
 	LedControl8188eu(padapter, LED_CTL_START_TO_LINK);
 
 	if (pmlmepriv->assoc_ssid.SsidLength == 0)
@@ -358,7 +349,6 @@ u8 rtw_createbss_cmd(struct adapter  *padapter)
 	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
 exit:
 
-
 	return res;
 }
 
@@ -376,8 +366,7 @@ u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network *pnetwork)
 	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
 	enum ndis_802_11_network_infra ndis_network_mode = pnetwork->network.InfrastructureMode;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
 
 	LedControl8188eu(padapter, LED_CTL_START_TO_LINK);
 
@@ -394,9 +383,8 @@ u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network *pnetwork)
 	/* for IEs is fix buf size */
 	t_len = sizeof(struct wlan_bssid_ex);
 
-
 	/* for hidden ap to set fw_state here */
-	if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE)) {
+	if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_ADHOC_STATE)) {
 		switch (ndis_network_mode) {
 		case Ndis802_11IBSS:
 			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
@@ -412,12 +400,13 @@ u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network *pnetwork)
 	}
 
 	psecnetwork = (struct wlan_bssid_ex *)&psecuritypriv->sec_bss;
-	if (psecnetwork == NULL) {
+	if (!psecnetwork) {
 		kfree(pcmd);
 
 		res = _FAIL;
 
-		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd :psecnetwork == NULL!!!\n"));
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
+			 ("%s :psecnetwork == NULL!!!\n", __func__));
 
 		goto exit;
 	}
@@ -444,7 +433,6 @@ u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network *pnetwork)
 
 	psecnetwork->IELength = rtw_restruct_sec_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength);
 
-
 	pqospriv->qos_option = 0;
 
 	if (pregistrypriv->wmm_enable) {
@@ -498,7 +486,6 @@ u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network *pnetwork)
 
 exit:
 
-
 	return res;
 }
 
@@ -509,8 +496,7 @@ u8 rtw_disassoc_cmd(struct adapter *padapter, u32 deauth_timeout_ms, bool enqueu
 	struct cmd_priv *cmdpriv = &padapter->cmdpriv;
 	u8 res = _SUCCESS;
 
-
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_disassoc_cmd\n"));
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+%s\n", __func__));
 
 	/* prepare cmd parameter */
 	param = kzalloc(sizeof(*param), GFP_KERNEL);
@@ -539,7 +525,6 @@ u8 rtw_disassoc_cmd(struct adapter *padapter, u32 deauth_timeout_ms, bool enqueu
 
 exit:
 
-
 	return res;
 }
 
@@ -617,7 +602,6 @@ u8 rtw_clearstakey_cmd(struct adapter *padapter, u8 *psta, u8 entry, u8 enqueue)
 	struct sta_info *sta = (struct sta_info *)psta;
 	u8	res = _SUCCESS;
 
-
 	if (!enqueue) {
 		clear_cam_entry(padapter, entry);
 	} else {
@@ -656,7 +640,6 @@ u8 rtw_clearstakey_cmd(struct adapter *padapter, u8 *psta, u8 entry, u8 enqueue)
 	}
 exit:
 
-
 	return res;
 }
 
@@ -667,7 +650,6 @@ u8 rtw_addbareq_cmd(struct adapter *padapter, u8 tid, u8 *addr)
 	struct addBaReq_parm *paddbareq_parm;
 	u8	res = _SUCCESS;
 
-
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c) {
 		res = _FAIL;
@@ -693,7 +675,6 @@ u8 rtw_addbareq_cmd(struct adapter *padapter, u8 tid, u8 *addr)
 
 exit:
 
-
 	return res;
 }
 
@@ -704,7 +685,6 @@ u8 rtw_dynamic_chk_wk_cmd(struct adapter *padapter)
 	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
 	u8	res = _SUCCESS;
 
-
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c) {
 		res = _FAIL;
@@ -724,7 +704,6 @@ u8 rtw_dynamic_chk_wk_cmd(struct adapter *padapter)
 
 	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, _Set_Drv_Extra_CMD_);
 
-
 	/* rtw_enqueue_cmd(pcmdpriv, ph2c); */
 	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 exit:
@@ -739,8 +718,7 @@ u8 rtw_set_chplan_cmd(struct adapter *padapter, u8 chplan, u8 enqueue)
 
 	u8	res = _SUCCESS;
 
-
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_chplan_cmd\n"));
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+%s\n", __func__));
 
 	/* check input parameter */
 	if (!rtw_is_channel_plan_valid(chplan)) {
@@ -781,7 +759,6 @@ u8 rtw_set_chplan_cmd(struct adapter *padapter, u8 chplan, u8 enqueue)
 
 exit:
 
-
 	return res;
 }
 
@@ -790,7 +767,7 @@ static void traffic_status_watchdog(struct adapter *padapter)
 	u8	bEnterPS;
 	u8	bBusyTraffic = false, bTxBusyTraffic = false, bRxBusyTraffic = false;
 	u8	bHigherBusyTraffic = false, bHigherBusyRxTraffic = false, bHigherBusyTxTraffic = false;
-	struct mlme_priv		*pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
 
 	/*  */
 	/*  Determine if our traffic is busy now */
@@ -849,7 +826,7 @@ static void dynamic_chk_wk_hdl(struct adapter *padapter, u8 *pbuf, int sz)
 	struct mlme_priv *pmlmepriv;
 
 	padapter = (struct adapter *)pbuf;
-	pmlmepriv = &(padapter->mlmepriv);
+	pmlmepriv = &padapter->mlmepriv;
 
 #ifdef CONFIG_88EU_AP_MODE
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
@@ -865,10 +842,9 @@ static void dynamic_chk_wk_hdl(struct adapter *padapter, u8 *pbuf, int sz)
 static void lps_ctrl_wk_hdl(struct adapter *padapter, u8 lps_ctrl_type)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	u8	mstatus;
 
-
 	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
 	    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
 		return;
@@ -905,7 +881,6 @@ static void lps_ctrl_wk_hdl(struct adapter *padapter, u8 lps_ctrl_type)
 	default:
 		break;
 	}
-
 }
 
 u8 rtw_lps_ctrl_wk_cmd(struct adapter *padapter, u8 lps_ctrl_type, u8 enqueue)
@@ -943,7 +918,6 @@ u8 rtw_lps_ctrl_wk_cmd(struct adapter *padapter, u8 lps_ctrl_type, u8 enqueue)
 
 exit:
 
-
 	return res;
 }
 
@@ -980,7 +954,6 @@ u8 rtw_rpt_timer_cfg_cmd(struct adapter *padapter, u16 min_time)
 	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 exit:
 
-
 	return res;
 }
 
@@ -1026,7 +999,6 @@ u8 rtw_antenna_select_cmd(struct adapter *padapter, u8 antenna, u8 enqueue)
 	}
 exit:
 
-
 	return res;
 }
 
@@ -1169,7 +1141,6 @@ void rtw_survey_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
 {
 	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-
 	if (pcmd->res == H2C_DROPPED) {
 		/* TODO: cancel timer and do timeout handler directly... */
 		/* need to make timeout handlerOS independent */
@@ -1183,13 +1154,12 @@ void rtw_survey_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
 
 	/*  free cmd */
 	rtw_free_cmd_obj(pcmd);
-
 }
+
 void rtw_disassoc_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
 {
 	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-
 	if (pcmd->res != H2C_SUCCESS) {
 		spin_lock_bh(&pmlmepriv->lock);
 		set_fwstate(pmlmepriv, _FW_LINKED);
@@ -1207,7 +1177,6 @@ void rtw_joinbss_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
 {
 	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-
 	if (pcmd->res == H2C_DROPPED) {
 		/* TODO: cancel timer and do timeout handler directly... */
 		/* need to make timeout handlerOS independent */
@@ -1220,7 +1189,6 @@ void rtw_joinbss_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
 	}
 
 	rtw_free_cmd_obj(pcmd);
-
 }
 
 void rtw_createbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
@@ -1229,11 +1197,11 @@ void rtw_createbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
 	struct wlan_network *pwlan = NULL;
 	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)pcmd->parmbuf;
-	struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
-
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
 
 	if (pcmd->res != H2C_SUCCESS) {
-		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: rtw_createbss_cmd_callback  Fail ************\n\n."));
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
+			 ("\n **** Error: %s  Fail ****\n\n.", __func__));
 		mod_timer(&pmlmepriv->assoc_timer,
 			  jiffies + msecs_to_jiffies(1));
 	}
@@ -1246,7 +1214,7 @@ void rtw_createbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
 		psta = rtw_get_stainfo(&padapter->stapriv, pnetwork->MacAddress);
 		if (!psta) {
 			psta = rtw_alloc_stainfo(&padapter->stapriv, pnetwork->MacAddress);
-			if (psta == NULL) {
+			if (!psta) {
 				RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nCan't alloc sta_info when createbss_cmd_callback\n"));
 				goto createbss_cmd_fail;
 			}
@@ -1255,28 +1223,31 @@ void rtw_createbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
 		rtw_indicate_connect(padapter);
 	} else {
 		pwlan = _rtw_alloc_network(pmlmepriv);
-		spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
-		if (pwlan == NULL) {
+		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+		if (!pwlan) {
 			pwlan = rtw_get_oldest_wlan_network(&pmlmepriv->scanned_queue);
-			if (pwlan == NULL) {
+			if (!pwlan) {
 				RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n Error:  can't get pwlan in rtw_joinbss_event_callback\n"));
 				spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 				goto createbss_cmd_fail;
 			}
 			pwlan->last_scanned = jiffies;
 		} else {
-			list_add_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue);
+			list_add_tail(&pwlan->list,
+				      &pmlmepriv->scanned_queue.queue);
 		}
 
 		pnetwork->Length = get_wlan_bssid_ex_sz(pnetwork);
-		memcpy(&(pwlan->network), pnetwork, pnetwork->Length);
+		memcpy(&pwlan->network, pnetwork, pnetwork->Length);
 
 		memcpy(&tgt_network->network, pnetwork, (get_wlan_bssid_ex_sz(pnetwork)));
 
 		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 
 		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
-		/*  we will set _FW_LINKED when there is one more sat to join us (rtw_stassoc_event_callback) */
+		/*  we will set _FW_LINKED when there is one more sat to
+		 *  join us (rtw_stassoc_event_callback)
+		 */
 	}
 
 createbss_cmd_fail:
@@ -1284,7 +1255,6 @@ createbss_cmd_fail:
 	spin_unlock_bh(&pmlmepriv->lock);
 
 	rtw_free_cmd_obj(pcmd);
-
 }
 
 void rtw_setstaKey_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
@@ -1293,8 +1263,7 @@ void rtw_setstaKey_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pc
 	struct set_stakey_rsp *psetstakey_rsp = (struct set_stakey_rsp *)(pcmd->rsp);
 	struct sta_info *psta = rtw_get_stainfo(pstapriv, psetstakey_rsp->addr);
 
-
-	if (psta == NULL) {
+	if (!psta) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: %s => can't get sta_info\n\n", __func__));
 		goto exit;
 	}
@@ -1310,8 +1279,7 @@ void rtw_setassocsta_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *
 	struct set_assocsta_rsp *passocsta_rsp = (struct set_assocsta_rsp *)(pcmd->rsp);
 	struct sta_info *psta = rtw_get_stainfo(pstapriv, passocsta_parm->addr);
 
-
-	if (psta == NULL) {
+	if (!psta) {
 		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: %s => can't get sta_info\n\n", __func__));
 		goto exit;
 	}
@@ -1326,5 +1294,4 @@ void rtw_setassocsta_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *
 
 exit:
 	rtw_free_cmd_obj(pcmd);
-
 }
diff --git a/drivers/staging/rtl8188eu/core/rtw_ieee80211.c b/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
index d1dafe0f20c6..bb867a987c2b 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
@@ -955,50 +955,6 @@ void rtw_macaddr_cfg(u8 *mac_addr)
 	DBG_88E("rtw_macaddr_cfg MAC Address  = %pM\n", (mac_addr));
 }
 
-/* Baron adds to avoid FreeBSD warning */
-int ieee80211_is_empty_essid(const char *essid, int essid_len)
-{
-	/* Single white space is for Linksys APs */
-	if (essid_len == 1 && essid[0] == ' ')
-		return 1;
-
-	/* Otherwise, if the entire essid is 0, we assume it is hidden */
-	while (essid_len) {
-		essid_len--;
-		if (essid[essid_len] != '\0')
-			return 0;
-	}
-
-	return 1;
-}
-
-int ieee80211_get_hdrlen(u16 fc)
-{
-	int hdrlen = 24;
-
-	switch (WLAN_FC_GET_TYPE(fc)) {
-	case RTW_IEEE80211_FTYPE_DATA:
-		if (fc & RTW_IEEE80211_STYPE_QOS_DATA)
-			hdrlen += 2;
-		if ((fc & RTW_IEEE80211_FCTL_FROMDS) && (fc & RTW_IEEE80211_FCTL_TODS))
-			hdrlen += 6; /* Addr4 */
-		break;
-	case RTW_IEEE80211_FTYPE_CTL:
-		switch (WLAN_FC_GET_STYPE(fc)) {
-		case RTW_IEEE80211_STYPE_CTS:
-		case RTW_IEEE80211_STYPE_ACK:
-			hdrlen = 10;
-			break;
-		default:
-			hdrlen = 16;
-			break;
-		}
-		break;
-	}
-
-	return hdrlen;
-}
-
 static int rtw_get_cipher_info(struct wlan_network *pnetwork)
 {
 	uint wpa_ielen;
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme.c b/drivers/staging/rtl8188eu/core/rtw_mlme.c
index 301085a459c9..fde306087844 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme.c
@@ -370,7 +370,7 @@ void update_network(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
 		sq_final = padapter->recvpriv.signal_qual;
 		/* the rssi value here is undecorated, and will be used for antenna diversity */
 		if (sq_smp != 101) /* from the right channel */
-			rssi_final = (src->Rssi+dst->Rssi*4)/5;
+			rssi_final = (src->Rssi + dst->Rssi * 4) / 5;
 		else
 			rssi_final = rssi_ori;
 	} else {
@@ -1081,10 +1081,10 @@ void rtw_joinbss_event_prehandle(struct adapter *adapter, u8 *pbuf)
 					RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("adhoc mode, fw_state:%x", get_fwstate(pmlmepriv)));
 				}
 
-			/* s5. Cancle assoc_timer */
+			/* s5. Cancel assoc_timer */
 			del_timer_sync(&pmlmepriv->assoc_timer);
 
-			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("Cancle assoc_timer\n"));
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("Cancel assoc_timer\n"));
 
 		} else {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_joinbss_event_callback err: fw_state:%x", get_fwstate(pmlmepriv)));
@@ -1926,7 +1926,7 @@ unsigned int rtw_restructure_ht_ie(struct adapter *padapter, u8 *in_ie, u8 *out_
 
 		if (pqospriv->qos_option == 0) {
 			out_len = *pout_len;
-			rtw_set_ie(out_ie+out_len, _VENDOR_SPECIFIC_IE_,
+			rtw_set_ie(out_ie + out_len, _VENDOR_SPECIFIC_IE_,
 				   _WMM_IE_Length_, WMM_IE, pout_len);
 
 			pqospriv->qos_option = 1;
@@ -2058,8 +2058,8 @@ void rtw_issue_addbareq_cmd(struct adapter *padapter, struct xmit_frame *pxmitfr
 	phtpriv = &psta->htpriv;
 
 	if ((phtpriv->ht_option) && (phtpriv->ampdu_enable)) {
-		issued = (phtpriv->agg_enable_bitmap>>priority)&0x1;
-		issued |= (phtpriv->candidate_tid_bitmap>>priority)&0x1;
+		issued = (phtpriv->agg_enable_bitmap >> priority) & 0x1;
+		issued |= (phtpriv->candidate_tid_bitmap >> priority) & 0x1;
 
 		if (issued == 0) {
 			DBG_88E("rtw_issue_addbareq_cmd, p=%d\n", priority);
diff --git a/drivers/staging/rtl8188eu/core/rtw_recv.c b/drivers/staging/rtl8188eu/core/rtw_recv.c
index c6c4404e717b..3fd5f4102b36 100644
--- a/drivers/staging/rtl8188eu/core/rtw_recv.c
+++ b/drivers/staging/rtl8188eu/core/rtw_recv.c
@@ -259,12 +259,10 @@ static int recvframe_chkmic(struct adapter *adapter,
 			}
 
 			/* icv_len included the mic code */
-			datalen = precvframe->pkt->len-prxattrib->hdrlen -
-				  prxattrib->iv_len-prxattrib->icv_len-8;
+			datalen = precvframe->pkt->len-prxattrib->hdrlen - 8;
 			pframe = precvframe->pkt->data;
-			payload = pframe+prxattrib->hdrlen+prxattrib->iv_len;
+			payload = pframe+prxattrib->hdrlen;
 
-			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n prxattrib->iv_len=%d prxattrib->icv_len=%d\n", prxattrib->iv_len, prxattrib->icv_len));
 			rtw_seccalctkipmic(mickey, pframe, payload, datalen, &miccode[0],
 					   (unsigned char)prxattrib->priority); /* care the length of the data */
 
@@ -409,9 +407,15 @@ static struct recv_frame *decryptor(struct adapter *padapter,
 		default:
 			break;
 		}
+		if (res != _FAIL) {
+			memmove(precv_frame->pkt->data + precv_frame->attrib.iv_len, precv_frame->pkt->data, precv_frame->attrib.hdrlen);
+			skb_pull(precv_frame->pkt, precv_frame->attrib.iv_len);
+			skb_trim(precv_frame->pkt, precv_frame->pkt->len - precv_frame->attrib.icv_len);
+		}
 	} else if (prxattrib->bdecrypted == 1 && prxattrib->encrypt > 0 &&
-		   (psecuritypriv->busetkipkey == 1 || prxattrib->encrypt != _TKIP_))
-			psecuritypriv->hw_decrypted = true;
+		   (psecuritypriv->busetkipkey == 1 || prxattrib->encrypt != _TKIP_)) {
+		psecuritypriv->hw_decrypted = true;
+	}
 
 	if (res == _FAIL) {
 		rtw_free_recvframe(return_packet, &padapter->recvpriv.free_recv_queue);
@@ -452,7 +456,7 @@ static struct recv_frame *portctrl(struct adapter *adapter,
 
 	if (auth_alg == 2) {
 		/* get ether_type */
-		ptr = ptr + pfhdr->attrib.hdrlen + LLC_HEADER_SIZE + pfhdr->attrib.iv_len;
+		ptr = ptr + pfhdr->attrib.hdrlen + LLC_HEADER_SIZE;
 		memcpy(&be_tmp, ptr, 2);
 		ether_type = ntohs(be_tmp);
 
@@ -1134,6 +1138,8 @@ static int validate_recv_data_frame(struct adapter *adapter,
 	}
 
 	if (pattrib->privacy) {
+		struct sk_buff *skb = precv_frame->pkt;
+
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("validate_recv_data_frame:pattrib->privacy=%x\n", pattrib->privacy));
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n ^^^^^^^^^^^IS_MCAST(pattrib->ra(0x%02x))=%d^^^^^^^^^^^^^^^6\n", pattrib->ra[0], IS_MCAST(pattrib->ra)));
 
@@ -1142,6 +1148,13 @@ static int validate_recv_data_frame(struct adapter *adapter,
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n pattrib->encrypt=%d\n", pattrib->encrypt));
 
 		SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len, pattrib->encrypt);
+
+		if (pattrib->bdecrypted == 1 && pattrib->encrypt > 0) {
+			memmove(skb->data + pattrib->iv_len,
+				skb->data, pattrib->hdrlen);
+			skb_pull(skb, pattrib->iv_len);
+			skb_trim(skb, skb->len - pattrib->icv_len);
+		}
 	} else {
 		pattrib->encrypt = 0;
 		pattrib->iv_len = 0;
@@ -1261,6 +1274,7 @@ static int validate_recv_frame(struct adapter *adapter,
 	 * Hence forward the frame to the monitor anyway to preserve the order
 	 * in which frames were received.
 	 */
+
 	rtl88eu_mon_recv_hook(adapter->pmondev, precv_frame);
 
 exit:
@@ -1282,11 +1296,8 @@ static int wlanhdr_to_ethhdr(struct recv_frame *precvframe)
 	u8 *ptr = precvframe->pkt->data;
 	struct rx_pkt_attrib *pattrib = &precvframe->attrib;
 
-	if (pattrib->encrypt)
-		skb_trim(precvframe->pkt, precvframe->pkt->len - pattrib->icv_len);
-
-	psnap = (struct ieee80211_snap_hdr *)(ptr+pattrib->hdrlen + pattrib->iv_len);
-	psnap_type = ptr+pattrib->hdrlen + pattrib->iv_len+SNAP_SIZE;
+	psnap = (struct ieee80211_snap_hdr *)(ptr+pattrib->hdrlen);
+	psnap_type = ptr+pattrib->hdrlen + SNAP_SIZE;
 	/* convert hdr + possible LLC headers into Ethernet header */
 	if ((!memcmp(psnap, rtw_rfc1042_header, SNAP_SIZE) &&
 	     (!memcmp(psnap_type, SNAP_ETH_TYPE_IPX, 2) == false) &&
@@ -1299,12 +1310,9 @@ static int wlanhdr_to_ethhdr(struct recv_frame *precvframe)
 		bsnaphdr = false;
 	}
 
-	rmv_len = pattrib->hdrlen + pattrib->iv_len + (bsnaphdr ? SNAP_SIZE : 0);
+	rmv_len = pattrib->hdrlen + (bsnaphdr ? SNAP_SIZE : 0);
 	len = precvframe->pkt->len - rmv_len;
 
-	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-		 ("\n===pattrib->hdrlen: %x,  pattrib->iv_len:%x===\n\n", pattrib->hdrlen,  pattrib->iv_len));
-
 	memcpy(&be_tmp, ptr+rmv_len, 2);
 	eth_type = ntohs(be_tmp); /* pattrib->ether_type */
 	pattrib->eth_type = eth_type;
@@ -1329,7 +1337,6 @@ static struct recv_frame *recvframe_defrag(struct adapter *adapter,
 					   struct __queue *defrag_q)
 {
 	struct list_head *plist, *phead;
-	u8 wlanhdr_offset;
 	u8	curfragnum;
 	struct recv_frame *pfhdr, *pnfhdr;
 	struct recv_frame *prframe, *pnextrframe;
@@ -1378,12 +1385,7 @@ static struct recv_frame *recvframe_defrag(struct adapter *adapter,
 		/* copy the 2nd~n fragment frame's payload to the first fragment */
 		/* get the 2nd~last fragment frame's payload */
 
-		wlanhdr_offset = pnfhdr->attrib.hdrlen + pnfhdr->attrib.iv_len;
-
-		skb_pull(pnextrframe->pkt, wlanhdr_offset);
-
-		/* append  to first fragment frame's tail (if privacy frame, pull the ICV) */
-		skb_trim(prframe->pkt, prframe->pkt->len - pfhdr->attrib.icv_len);
+		skb_pull(pnextrframe->pkt, pnfhdr->attrib.hdrlen);
 
 		/* memcpy */
 		memcpy(skb_tail_pointer(pfhdr->pkt), pnfhdr->pkt->data,
@@ -1391,7 +1393,7 @@ static struct recv_frame *recvframe_defrag(struct adapter *adapter,
 
 		skb_put(prframe->pkt, pnfhdr->pkt->len);
 
-		pfhdr->attrib.icv_len = pnfhdr->attrib.icv_len;
+		pfhdr->attrib.icv_len = 0;
 		plist = plist->next;
 	}
 
@@ -1510,7 +1512,6 @@ static int amsdu_to_msdu(struct adapter *padapter, struct recv_frame *prframe)
 	u8	nr_subframes, i;
 	unsigned char *pdata;
 	struct rx_pkt_attrib *pattrib;
-	unsigned char *data_ptr;
 	struct sk_buff *sub_skb, *subframes[MAX_SUBFRAME_COUNT];
 	struct recv_priv *precvpriv = &padapter->recvpriv;
 	struct __queue *pfree_recv_queue = &(precvpriv->free_recv_queue);
@@ -1518,11 +1519,6 @@ static int amsdu_to_msdu(struct adapter *padapter, struct recv_frame *prframe)
 	nr_subframes = 0;
 	pattrib = &prframe->attrib;
 
-	skb_pull(prframe->pkt, prframe->attrib.hdrlen);
-
-	if (prframe->attrib.iv_len > 0)
-		skb_pull(prframe->pkt, prframe->attrib.iv_len);
-
 	a_len = prframe->pkt->len;
 
 	pdata = prframe->pkt->data;
@@ -1544,8 +1540,7 @@ static int amsdu_to_msdu(struct adapter *padapter, struct recv_frame *prframe)
 		sub_skb = dev_alloc_skb(nSubframe_Length + 12);
 		if (sub_skb) {
 			skb_reserve(sub_skb, 12);
-			data_ptr = (u8 *)skb_put(sub_skb, nSubframe_Length);
-			memcpy(data_ptr, pdata, nSubframe_Length);
+			skb_put_data(sub_skb, pdata, nSubframe_Length);
 		} else {
 			sub_skb = skb_clone(prframe->pkt, GFP_ATOMIC);
 			if (sub_skb) {
@@ -1892,24 +1887,6 @@ static int process_recv_indicatepkts(struct adapter *padapter,
 	return retval;
 }
 
-static int recv_func_prehandle(struct adapter *padapter,
-			       struct recv_frame *rframe)
-{
-	int ret = _SUCCESS;
-	struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
-
-	/* check the frame crtl field and decache */
-	ret = validate_recv_frame(padapter, rframe);
-	if (ret != _SUCCESS) {
-		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("recv_func: validate_recv_frame fail! drop pkt\n"));
-		rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */
-		goto exit;
-	}
-
-exit:
-	return ret;
-}
-
 static int recv_func_posthandle(struct adapter *padapter,
 				struct recv_frame *prframe)
 {
@@ -1962,6 +1939,7 @@ static int recv_func(struct adapter *padapter, struct recv_frame *rframe)
 	struct rx_pkt_attrib *prxattrib = &rframe->attrib;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+	struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
 
 	/* check if need to handle uc_swdec_pending_queue*/
 	if (check_fwstate(mlmepriv, WIFI_STATION_STATE) && psecuritypriv->busetkipkey) {
@@ -1973,9 +1951,12 @@ static int recv_func(struct adapter *padapter, struct recv_frame *rframe)
 		}
 	}
 
-	ret = recv_func_prehandle(padapter, rframe);
-
-	if (ret == _SUCCESS) {
+	/* check the frame crtl field and decache */
+	ret = validate_recv_frame(padapter, rframe);
+	if (ret != _SUCCESS) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("recv_func: validate_recv_frame fail! drop pkt\n"));
+		rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */
+	} else {
 		/* check if need to enqueue into uc_swdec_pending_queue*/
 		if (check_fwstate(mlmepriv, WIFI_STATION_STATE) &&
 		    !IS_MCAST(prxattrib->ra) && prxattrib->encrypt > 0 &&
diff --git a/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c b/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c
index 2ecfb117bf3f..22cf362b8528 100644
--- a/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c
+++ b/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c
@@ -61,7 +61,6 @@ static void _rtw_init_stainfo(struct sta_info *psta)
 	psta->keep_alive_trycnt = 0;
 
 #endif	/*  CONFIG_88EU_AP_MODE */
-
 }
 
 u32	_rtw_init_sta_priv(struct	sta_priv *pstapriv)
@@ -69,7 +68,6 @@ u32	_rtw_init_sta_priv(struct	sta_priv *pstapriv)
 	struct sta_info *psta;
 	s32 i;
 
-
 	pstapriv->pallocated_stainfo_buf = vzalloc(sizeof(struct sta_info) * NUM_STA + 4);
 
 	if (!pstapriv->pallocated_stainfo_buf)
@@ -116,7 +114,6 @@ u32	_rtw_init_sta_priv(struct	sta_priv *pstapriv)
 	pstapriv->max_num_sta = NUM_STA;
 #endif
 
-
 	return _SUCCESS;
 }
 
@@ -263,7 +260,6 @@ u32	rtw_free_stainfo(struct adapter *padapter, struct sta_info *psta)
 	struct	xmit_priv	*pxmitpriv = &padapter->xmitpriv;
 	struct	sta_priv *pstapriv = &padapter->stapriv;
 
-
 	if (!psta)
 		goto exit;
 
@@ -381,7 +377,6 @@ u32	rtw_free_stainfo(struct adapter *padapter, struct sta_info *psta)
 
 exit:
 
-
 	return _SUCCESS;
 }
 
@@ -394,7 +389,6 @@ void rtw_free_all_stainfo(struct adapter *padapter)
 	struct	sta_priv *pstapriv = &padapter->stapriv;
 	struct sta_info *pbcmc_stainfo = rtw_get_bcmc_stainfo(padapter);
 
-
 	if (pstapriv->asoc_sta_count == 1)
 		return;
 
@@ -425,7 +419,6 @@ struct sta_info *rtw_get_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
 	u8 *addr;
 	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
-
 	if (!hwaddr)
 		return NULL;
 
@@ -463,7 +456,6 @@ u32 rtw_init_bcmc_stainfo(struct adapter *padapter)
 	unsigned char bcast_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 	struct	sta_priv *pstapriv = &padapter->stapriv;
 
-
 	psta = rtw_alloc_stainfo(pstapriv, bcast_addr);
 
 	if (!psta) {
diff --git a/drivers/staging/rtl8188eu/hal/phy.c b/drivers/staging/rtl8188eu/hal/phy.c
index 054f5996f60d..3039bbe44a25 100644
--- a/drivers/staging/rtl8188eu/hal/phy.c
+++ b/drivers/staging/rtl8188eu/hal/phy.c
@@ -1091,7 +1091,7 @@ static void phy_iq_calibrate(struct adapter *adapt, s32 result[][8],
 		}
 	}
 
-	if (0x00 == path_a_ok) {
+	if (path_a_ok == 0x00) {
 		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
 			     ("Path A IQK failed!!\n"));
 	}
@@ -1122,7 +1122,7 @@ static void phy_iq_calibrate(struct adapter *adapt, s32 result[][8],
 			}
 		}
 
-		if (0x00 == path_b_ok) {
+		if (path_b_ok == 0x00) {
 			ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
 				     ("Path B IQK failed!!\n"));
 		}
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_rxdesc.c b/drivers/staging/rtl8188eu/hal/rtl8188e_rxdesc.c
index d9fa290c5f78..9f51f54f866a 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_rxdesc.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_rxdesc.c
@@ -58,7 +58,7 @@ static void process_link_qual(struct adapter *padapter,
 }
 
 void rtl8188e_process_phy_info(struct adapter *padapter,
-		               struct recv_frame *precvframe)
+			       struct recv_frame *precvframe)
 {
 	/*  Check RSSI */
 	process_rssi(padapter, precvframe);
diff --git a/drivers/staging/rtl8188eu/include/ieee80211.h b/drivers/staging/rtl8188eu/include/ieee80211.h
index 22ab0c43e376..284db7d00f50 100644
--- a/drivers/staging/rtl8188eu/include/ieee80211.h
+++ b/drivers/staging/rtl8188eu/include/ieee80211.h
@@ -306,68 +306,6 @@ enum eap_type {
 #define MIN_FRAG_THRESHOLD     256U
 #define	MAX_FRAG_THRESHOLD     2346U
 
-/* Frame control field constants */
-#define RTW_IEEE80211_FCTL_VERS		0x0003
-#define RTW_IEEE80211_FCTL_FTYPE	0x000c
-#define RTW_IEEE80211_FCTL_STYPE	0x00f0
-#define RTW_IEEE80211_FCTL_TODS		0x0100
-#define RTW_IEEE80211_FCTL_FROMDS	0x0200
-#define RTW_IEEE80211_FCTL_MOREFRAGS	0x0400
-#define RTW_IEEE80211_FCTL_RETRY	0x0800
-#define RTW_IEEE80211_FCTL_PM		0x1000
-#define RTW_IEEE80211_FCTL_MOREDATA	0x2000
-#define RTW_IEEE80211_FCTL_PROTECTED	0x4000
-#define RTW_IEEE80211_FCTL_ORDER	0x8000
-#define RTW_IEEE80211_FCTL_CTL_EXT	0x0f00
-
-#define RTW_IEEE80211_FTYPE_MGMT	0x0000
-#define RTW_IEEE80211_FTYPE_CTL		0x0004
-#define RTW_IEEE80211_FTYPE_DATA	0x0008
-#define RTW_IEEE80211_FTYPE_EXT		0x000c
-
-/* management */
-#define RTW_IEEE80211_STYPE_ASSOC_REQ	0x0000
-#define RTW_IEEE80211_STYPE_ASSOC_RESP	0x0010
-#define RTW_IEEE80211_STYPE_REASSOC_REQ	0x0020
-#define RTW_IEEE80211_STYPE_REASSOC_RESP	0x0030
-#define RTW_IEEE80211_STYPE_PROBE_REQ	0x0040
-#define RTW_IEEE80211_STYPE_PROBE_RESP	0x0050
-#define RTW_IEEE80211_STYPE_BEACON	0x0080
-#define RTW_IEEE80211_STYPE_ATIM	0x0090
-#define RTW_IEEE80211_STYPE_DISASSOC	0x00A0
-#define RTW_IEEE80211_STYPE_AUTH	0x00B0
-#define RTW_IEEE80211_STYPE_DEAUTH	0x00C0
-#define RTW_IEEE80211_STYPE_ACTION	0x00D0
-
-/* control */
-#define RTW_IEEE80211_STYPE_CTL_EXT	0x0060
-#define RTW_IEEE80211_STYPE_BACK_REQ	0x0080
-#define RTW_IEEE80211_STYPE_BACK	0x0090
-#define RTW_IEEE80211_STYPE_PSPOLL	0x00A0
-#define RTW_IEEE80211_STYPE_RTS		0x00B0
-#define RTW_IEEE80211_STYPE_CTS		0x00C0
-#define RTW_IEEE80211_STYPE_ACK		0x00D0
-#define RTW_IEEE80211_STYPE_CFEND	0x00E0
-#define RTW_IEEE80211_STYPE_CFENDACK	0x00F0
-
-/* data */
-#define RTW_IEEE80211_STYPE_DATA	0x0000
-#define RTW_IEEE80211_STYPE_DATA_CFACK	0x0010
-#define RTW_IEEE80211_STYPE_DATA_CFPOLL	0x0020
-#define RTW_IEEE80211_STYPE_DATA_CFACKPOLL	0x0030
-#define RTW_IEEE80211_STYPE_NULLFUNC	0x0040
-#define RTW_IEEE80211_STYPE_CFACK	0x0050
-#define RTW_IEEE80211_STYPE_CFPOLL	0x0060
-#define RTW_IEEE80211_STYPE_CFACKPOLL	0x0070
-#define RTW_IEEE80211_STYPE_QOS_DATA	0x0080
-#define RTW_IEEE80211_STYPE_QOS_DATA_CFACK	0x0090
-#define RTW_IEEE80211_STYPE_QOS_DATA_CFPOLL	0x00A0
-#define RTW_IEEE80211_STYPE_QOS_DATA_CFACKPOLL	0x00B0
-#define RTW_IEEE80211_STYPE_QOS_NULLFUNC	0x00C0
-#define RTW_IEEE80211_STYPE_QOS_CFACK		0x00D0
-#define RTW_IEEE80211_STYPE_QOS_CFPOLL		0x00E0
-#define RTW_IEEE80211_STYPE_QOS_CFACKPOLL	0x00F0
-
 /* sequence control field */
 #define RTW_IEEE80211_SCTL_FRAG	0x000F
 #define RTW_IEEE80211_SCTL_SEQ	0xFFF0
@@ -408,9 +346,6 @@ struct ieee80211_snap_hdr {
 
 #define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
 
-#define WLAN_FC_GET_TYPE(fc) ((fc) & RTW_IEEE80211_FCTL_FTYPE)
-#define WLAN_FC_GET_STYPE(fc) ((fc) & RTW_IEEE80211_FCTL_STYPE)
-
 #define WLAN_QC_GET_TID(qc) ((qc) & 0x0f)
 
 #define WLAN_GET_SEQ_FRAG(seq) ((seq) & RTW_IEEE80211_SCTL_FRAG)
@@ -423,14 +358,6 @@ struct ieee80211_snap_hdr {
 #define IEEE80211_DATA_HDR3_LEN 24
 #define IEEE80211_DATA_HDR4_LEN 30
 
-
-#define IEEE80211_STATMASK_SIGNAL (1<<0)
-#define IEEE80211_STATMASK_RSSI (1<<1)
-#define IEEE80211_STATMASK_NOISE (1<<2)
-#define IEEE80211_STATMASK_RATE (1<<3)
-#define IEEE80211_STATMASK_WEMASK 0x7
-
-
 #define IEEE80211_CCK_MODULATION    (1<<0)
 #define IEEE80211_OFDM_MODULATION   (1<<1)
 
@@ -488,9 +415,6 @@ struct ieee80211_snap_hdr {
 	IEEE80211_OFDM_RATE_36MB_MASK |					\
 	IEEE80211_OFDM_RATE_48MB_MASK |					\
 	IEEE80211_OFDM_RATE_54MB_MASK)
-#define IEEE80211_DEFAULT_RATES_MASK					\
-	(IEEE80211_OFDM_DEFAULT_RATES_MASK |				\
-	 IEEE80211_CCK_DEFAULT_RATES_MASK)
 
 #define IEEE80211_NUM_OFDM_RATES	8
 #define IEEE80211_NUM_CCK_RATES		4
@@ -521,25 +445,6 @@ struct ieee80211_snap_hdr {
 #define WEP_KEYS 4
 #define WEP_KEY_LEN 13
 
-#define BEACON_PROBE_SSID_ID_POSITION 12
-
-/* Management Frame Information Element Types */
-#define MFIE_TYPE_SSID		0
-#define MFIE_TYPE_RATES		1
-#define MFIE_TYPE_FH_SET	2
-#define MFIE_TYPE_DS_SET	3
-#define MFIE_TYPE_CF_SET	4
-#define MFIE_TYPE_TIM		5
-#define MFIE_TYPE_IBSS_SET	6
-#define MFIE_TYPE_CHALLENGE	16
-#define MFIE_TYPE_ERP		42
-#define MFIE_TYPE_RSN		48
-#define MFIE_TYPE_RATES_EX	50
-#define MFIE_TYPE_GENERIC	221
-
-#define IEEE80211_DEFAULT_TX_ESSID "Penguin"
-#define IEEE80211_DEFAULT_BASIC_RATE 10
-
 /* SWEEP TABLE ENTRIES NUMBER*/
 #define MAX_SWEEP_TAB_ENTRIES		  42
 #define MAX_SWEEP_TAB_ENTRIES_PER_PACKET  7
@@ -566,14 +471,6 @@ struct ieee80211_snap_hdr {
 #define NETWORK_HAS_OFDM    (1<<1)
 #define NETWORK_HAS_CCK     (1<<2)
 
-#define IEEE80211_DTIM_MBCAST 4
-#define IEEE80211_DTIM_UCAST 2
-#define IEEE80211_DTIM_VALID 1
-#define IEEE80211_DTIM_INVALID 0
-
-#define IEEE80211_PS_DISABLED 0
-#define IEEE80211_PS_UNICAST IEEE80211_DTIM_UCAST
-#define IEEE80211_PS_MBCAST IEEE80211_DTIM_MBCAST
 #define IW_ESSID_MAX_SIZE 32
 /*
 join_res:
@@ -644,10 +541,6 @@ static inline int is_broadcast_mac_addr(const u8 *addr)
 #define IEEE_G	    (1<<2)
 #define IEEE_MODE_MASK    (IEEE_A|IEEE_B|IEEE_G)
 
-/* Baron move to ieee80211.c */
-int ieee80211_is_empty_essid(const char *essid, int essid_len);
-int ieee80211_get_hdrlen(u16 fc);
-
 /* Action category code */
 enum rtw_ieee80211_category {
 	RTW_WLAN_CATEGORY_SPECTRUM_MGMT = 0,
diff --git a/drivers/staging/rtl8188eu/os_dep/mon.c b/drivers/staging/rtl8188eu/os_dep/mon.c
index cfe37eb026d6..37fd52d7364f 100644
--- a/drivers/staging/rtl8188eu/os_dep/mon.c
+++ b/drivers/staging/rtl8188eu/os_dep/mon.c
@@ -53,7 +53,7 @@ static void mon_recv_decrypted(struct net_device *dev, const u8 *data,
 	skb = netdev_alloc_skb(dev, data_len);
 	if (!skb)
 		return;
-	memcpy(skb_put(skb, data_len), data, data_len);
+	skb_put_data(skb, data, data_len);
 
 	/*
 	 * Frame data is not encrypted. Strip off protection so
@@ -66,6 +66,34 @@ static void mon_recv_decrypted(struct net_device *dev, const u8 *data,
 	netif_rx(skb);
 }
 
+static void mon_recv_decrypted_recv(struct net_device *dev, const u8 *data,
+				    int data_len)
+{
+	struct sk_buff *skb;
+	struct ieee80211_hdr *hdr;
+	int hdr_len;
+
+	skb = netdev_alloc_skb(dev, data_len);
+	if (!skb)
+		return;
+	memcpy(skb_put(skb, data_len), data, data_len);
+
+	/*
+	 * Frame data is not encrypted. Strip off protection so
+	 * userspace doesn't think that it is.
+	 */
+
+	hdr = (struct ieee80211_hdr *)skb->data;
+	hdr_len = ieee80211_hdrlen(hdr->frame_control);
+
+	if (ieee80211_has_protected(hdr->frame_control))
+		hdr->frame_control &= ~cpu_to_le16(IEEE80211_FCTL_PROTECTED);
+
+	skb->ip_summed = CHECKSUM_UNNECESSARY;
+	skb->protocol = eth_type_trans(skb, dev);
+	netif_rx(skb);
+}
+
 static void mon_recv_encrypted(struct net_device *dev, const u8 *data,
 			       int data_len)
 {
@@ -82,7 +110,6 @@ static void mon_recv_encrypted(struct net_device *dev, const u8 *data,
 void rtl88eu_mon_recv_hook(struct net_device *dev, struct recv_frame *frame)
 {
 	struct rx_pkt_attrib *attr;
-	int iv_len, icv_len;
 	int data_len;
 	u8 *data;
 
@@ -95,11 +122,8 @@ void rtl88eu_mon_recv_hook(struct net_device *dev, struct recv_frame *frame)
 	data = frame->pkt->data;
 	data_len = frame->pkt->len;
 
-	/* Broadcast and multicast frames don't have attr->{iv,icv}_len set */
-	SET_ICE_IV_LEN(iv_len, icv_len, attr->encrypt);
-
 	if (attr->bdecrypted)
-		mon_recv_decrypted(dev, data, data_len, iv_len, icv_len);
+		mon_recv_decrypted_recv(dev, data, data_len);
 	else
 		mon_recv_encrypted(dev, data, data_len);
 }
@@ -152,7 +176,7 @@ static const struct net_device_ops mon_netdev_ops = {
 static void mon_setup(struct net_device *dev)
 {
 	dev->netdev_ops = &mon_netdev_ops;
-	dev->destructor = free_netdev;
+	dev->needs_free_netdev = true;
 	ether_setup(dev);
 	dev->priv_flags |= IFF_NO_QUEUE;
 	dev->type = ARPHRD_IEEE80211;
diff --git a/drivers/staging/rtl8192e/dot11d.h b/drivers/staging/rtl8192e/dot11d.h
index 623075cf0c05..7fa3c4d963c4 100644
--- a/drivers/staging/rtl8192e/dot11d.h
+++ b/drivers/staging/rtl8192e/dot11d.h
@@ -30,8 +30,8 @@ enum dot11d_state {
 };
 
 /**
- * struct rt_dot11d_info * @CountryIeLen: value greater than 0 if @CountryIeBuf contains
- *		  valid country information element.
+ * struct rt_dot11d_info * @CountryIeLen: value greater than 0 if
+ *		  @CountryIeBuf contains valid country information element.
  * @channel_map: holds channel values
  *		0 - invalid,
  *		1 - valid (active scan),
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
index a4d1bac4a844..aca52654825b 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
@@ -2275,131 +2275,6 @@ static int _rtl92e_set_mac_adr(struct net_device *dev, void *mac)
 	return 0;
 }
 
-/* based on ipw2200 driver */
-static int _rtl92e_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
-{
-	struct r8192_priv *priv = rtllib_priv(dev);
-	struct iwreq *wrq = (struct iwreq *)rq;
-	int ret = -1;
-	struct rtllib_device *ieee = priv->rtllib;
-	u32 key[4];
-	const u8 broadcast_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-	struct iw_point *p = &wrq->u.data;
-	struct ieee_param *ipw = NULL;
-
-	mutex_lock(&priv->wx_mutex);
-
-	switch (cmd) {
-	case RTL_IOCTL_WPA_SUPPLICANT:
-		if (p->length < sizeof(struct ieee_param) || !p->pointer) {
-			ret = -EINVAL;
-			goto out;
-		}
-
-		ipw = memdup_user(p->pointer, p->length);
-		if (IS_ERR(ipw)) {
-			ret = PTR_ERR(ipw);
-			goto out;
-		}
-
-		if (ipw->cmd == IEEE_CMD_SET_ENCRYPTION) {
-			if (ipw->u.crypt.set_tx) {
-				if (strcmp(ipw->u.crypt.alg, "CCMP") == 0)
-					ieee->pairwise_key_type = KEY_TYPE_CCMP;
-				else if (strcmp(ipw->u.crypt.alg, "TKIP") == 0)
-					ieee->pairwise_key_type = KEY_TYPE_TKIP;
-				else if (strcmp(ipw->u.crypt.alg, "WEP") == 0) {
-					if (ipw->u.crypt.key_len == 13)
-						ieee->pairwise_key_type =
-							 KEY_TYPE_WEP104;
-					else if (ipw->u.crypt.key_len == 5)
-						ieee->pairwise_key_type =
-							 KEY_TYPE_WEP40;
-				} else {
-					ieee->pairwise_key_type = KEY_TYPE_NA;
-				}
-
-				if (ieee->pairwise_key_type) {
-					if (is_zero_ether_addr(ieee->ap_mac_addr))
-						ieee->iw_mode = IW_MODE_ADHOC;
-					memcpy((u8 *)key, ipw->u.crypt.key, 16);
-					rtl92e_enable_hw_security_config(dev);
-					rtl92e_set_swcam(dev, 4,
-							 ipw->u.crypt.idx,
-							 ieee->pairwise_key_type,
-							 (u8 *)ieee->ap_mac_addr,
-							 0, key, 0);
-					rtl92e_set_key(dev, 4, ipw->u.crypt.idx,
-						       ieee->pairwise_key_type,
-						       (u8 *)ieee->ap_mac_addr,
-						       0, key);
-					if (ieee->iw_mode == IW_MODE_ADHOC) {
-						rtl92e_set_swcam(dev,
-								 ipw->u.crypt.idx,
-								 ipw->u.crypt.idx,
-								 ieee->pairwise_key_type,
-								 (u8 *)ieee->ap_mac_addr,
-								 0, key, 0);
-						rtl92e_set_key(dev,
-							       ipw->u.crypt.idx,
-							       ipw->u.crypt.idx,
-							       ieee->pairwise_key_type,
-							       (u8 *)ieee->ap_mac_addr,
-							       0, key);
-					}
-				}
-				if ((ieee->pairwise_key_type ==
-				     KEY_TYPE_CCMP) &&
-				     ieee->pHTInfo->bCurrentHTSupport) {
-					rtl92e_writeb(dev, 0x173, 1);
-				}
-
-			} else {
-				memcpy((u8 *)key, ipw->u.crypt.key, 16);
-				if (strcmp(ipw->u.crypt.alg, "CCMP") == 0)
-					ieee->group_key_type = KEY_TYPE_CCMP;
-				else if (strcmp(ipw->u.crypt.alg, "TKIP") == 0)
-					ieee->group_key_type = KEY_TYPE_TKIP;
-				else if (strcmp(ipw->u.crypt.alg, "WEP") == 0) {
-					if (ipw->u.crypt.key_len == 13)
-						ieee->group_key_type =
-							 KEY_TYPE_WEP104;
-					else if (ipw->u.crypt.key_len == 5)
-						ieee->group_key_type =
-							 KEY_TYPE_WEP40;
-				} else
-					ieee->group_key_type = KEY_TYPE_NA;
-
-				if (ieee->group_key_type) {
-					rtl92e_set_swcam(dev, ipw->u.crypt.idx,
-							 ipw->u.crypt.idx,
-							 ieee->group_key_type,
-							 broadcast_addr, 0, key,
-							 0);
-					rtl92e_set_key(dev, ipw->u.crypt.idx,
-						       ipw->u.crypt.idx,
-						       ieee->group_key_type,
-						       broadcast_addr, 0, key);
-				}
-			}
-		}
-
-		ret = rtllib_wpa_supplicant_ioctl(priv->rtllib, &wrq->u.data,
-						  0);
-		kfree(ipw);
-		break;
-	default:
-		ret = -EOPNOTSUPP;
-		break;
-	}
-
-out:
-	mutex_unlock(&priv->wx_mutex);
-
-	return ret;
-}
-
-
 static irqreturn_t _rtl92e_irq(int irq, void *netdev)
 {
 	struct net_device *dev = netdev;
@@ -2542,7 +2417,6 @@ static const struct net_device_ops rtl8192_netdev_ops = {
 	.ndo_open = _rtl92e_open,
 	.ndo_stop = _rtl92e_close,
 	.ndo_tx_timeout = _rtl92e_tx_timeout,
-	.ndo_do_ioctl = _rtl92e_ioctl,
 	.ndo_set_rx_mode = _rtl92e_set_multicast,
 	.ndo_set_mac_address = _rtl92e_set_mac_adr,
 	.ndo_validate_addr = eth_validate_addr,
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
index 0335823e2766..9d3089cb6a5a 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
@@ -102,8 +102,6 @@
 
 #define	PHY_RSSI_SLID_WIN_MAX			100
 
-#define RTL_IOCTL_WPA_SUPPLICANT		(SIOCIWFIRSTPRIV + 30)
-
 #define TxBBGainTableLength			37
 #define CCKTxBBGainTableLength			23
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
index 1a43c684f9f3..b8205ebafd72 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
@@ -1693,22 +1693,6 @@ static void _rtl92e_dm_check_edca_turbo(struct net_device *dev)
 	if (priv->rtllib->pHTInfo->IOTAction & HT_IOT_ACT_DISABLE_EDCA_TURBO)
 		goto dm_CheckEdcaTurbo_EXIT;
 
-	{
-		u8 *peername[11] = {
-			"unknown", "realtek_90", "realtek_92se", "broadcom",
-			"ralink", "atheros", "cisco", "marvell", "92u_softap",
-			"self_softap"
-		};
-		static int wb_tmp;
-
-		if (wb_tmp == 0) {
-			netdev_info(dev,
-				    "%s():iot peer is %s, bssid: %pM\n",
-				    __func__, peername[pHTInfo->IOTPeer],
-				    priv->rtllib->current_network.bssid);
-			wb_tmp = 1;
-		}
-	}
 	if (!priv->rtllib->bis_any_nonbepkts) {
 		curTxOkCnt = priv->stats.txbytesunicast - lastTxOkCnt;
 		curRxOkCnt = priv->stats.rxbytesunicast - lastRxOkCnt;
diff --git a/drivers/staging/rtl8192e/rtl819x_BAProc.c b/drivers/staging/rtl8192e/rtl819x_BAProc.c
index 1d3963136295..1720e1b6ae04 100644
--- a/drivers/staging/rtl8192e/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_BAProc.c
@@ -95,8 +95,7 @@ static struct sk_buff *rtllib_ADDBA(struct rtllib_device *ieee, u8 *Dst,
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	BAReq = (struct rtllib_hdr_3addr *)skb_put(skb,
-		 sizeof(struct rtllib_hdr_3addr));
+	BAReq = skb_put(skb, sizeof(struct rtllib_hdr_3addr));
 
 	ether_addr_copy(BAReq->addr1, Dst);
 	ether_addr_copy(BAReq->addr2, ieee->dev->dev_addr);
@@ -104,7 +103,7 @@ static struct sk_buff *rtllib_ADDBA(struct rtllib_device *ieee, u8 *Dst,
 	ether_addr_copy(BAReq->addr3, ieee->current_network.bssid);
 	BAReq->frame_ctl = cpu_to_le16(RTLLIB_STYPE_MANAGE_ACT);
 
-	tag = (u8 *)skb_put(skb, 9);
+	tag = skb_put(skb, 9);
 	*tag++ = ACT_CAT_BA;
 	*tag++ = type;
 	*tag++ = pBA->DialogToken;
@@ -159,15 +158,14 @@ static struct sk_buff *rtllib_DELBA(struct rtllib_device *ieee, u8 *dst,
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	Delba = (struct rtllib_hdr_3addr *) skb_put(skb,
-		 sizeof(struct rtllib_hdr_3addr));
+	Delba = skb_put(skb, sizeof(struct rtllib_hdr_3addr));
 
 	ether_addr_copy(Delba->addr1, dst);
 	ether_addr_copy(Delba->addr2, ieee->dev->dev_addr);
 	ether_addr_copy(Delba->addr3, ieee->current_network.bssid);
 	Delba->frame_ctl = cpu_to_le16(RTLLIB_STYPE_MANAGE_ACT);
 
-	tag = (u8 *)skb_put(skb, 6);
+	tag = skb_put(skb, 6);
 
 	*tag++ = ACT_CAT_BA;
 	*tag++ = ACT_DELBA;
diff --git a/drivers/staging/rtl8192e/rtl819x_HTProc.c b/drivers/staging/rtl8192e/rtl819x_HTProc.c
index 4ae1d382ac5c..f0e11726a72a 100644
--- a/drivers/staging/rtl8192e/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_HTProc.c
@@ -908,8 +908,8 @@ void HTSetConnectBwMode(struct rtllib_device *ieee,
 		pHTInfo->CurSTAExtChnlOffset = HT_EXTCHNL_OFFSET_NO_EXT;
 	}
 
-	pr_info("%s():pHTInfo->bCurBW40MHz:%x\n", __func__,
-	       pHTInfo->bCurBW40MHz);
+	netdev_dbg(ieee->dev, "%s():pHTInfo->bCurBW40MHz:%x\n", __func__,
+		   pHTInfo->bCurBW40MHz);
 
 	pHTInfo->bSwBwInProgress = true;
 
diff --git a/drivers/staging/rtl8192e/rtllib.h b/drivers/staging/rtl8192e/rtllib.h
index 827651b62791..0042a0f6cf79 100644
--- a/drivers/staging/rtl8192e/rtllib.h
+++ b/drivers/staging/rtl8192e/rtllib.h
@@ -335,60 +335,8 @@ enum rt_op_mode {
 
 #define MGMT_QUEUE_NUM 5
 
-#define IEEE_CMD_SET_WPA_PARAM			1
-#define	IEEE_CMD_SET_WPA_IE			2
-#define IEEE_CMD_SET_ENCRYPTION			3
-#define IEEE_CMD_MLME				4
-
-#define IEEE_PARAM_WPA_ENABLED			1
-#define IEEE_PARAM_TKIP_COUNTERMEASURES		2
-#define IEEE_PARAM_DROP_UNENCRYPTED		3
-#define IEEE_PARAM_PRIVACY_INVOKED		4
-#define IEEE_PARAM_AUTH_ALGS			5
-#define IEEE_PARAM_IEEE_802_1X			6
-#define IEEE_PARAM_WPAX_SELECT			7
-
-#define IEEE_MLME_STA_DEAUTH			1
-#define IEEE_MLME_STA_DISASSOC			2
-
-
-#define IEEE_CRYPT_ERR_UNKNOWN_ALG		2
-#define IEEE_CRYPT_ERR_CRYPT_INIT_FAILED	4
-#define IEEE_CRYPT_ERR_KEY_SET_FAILED		5
-#define IEEE_CRYPT_ERR_CARD_CONF_FAILED		7
-#define	IEEE_CRYPT_ALG_NAME_LEN			16
-
 #define MAX_IE_LEN  0xff
 
-struct ieee_param {
-	u32 cmd;
-	u8 sta_addr[ETH_ALEN];
-	union {
-		struct {
-			u8 name;
-			u32 value;
-		} wpa_param;
-		struct {
-			u32 len;
-			u8 reserved[32];
-			u8 data[0];
-		} wpa_ie;
-		struct {
-			int command;
-			int reason_code;
-		} mlme;
-		struct {
-			u8 alg[IEEE_CRYPT_ALG_NAME_LEN];
-			u8 set_tx;
-			u32 err;
-			u8 idx;
-			u8 seq[8]; /* sequence counter (set: RX, get: TX) */
-			u16 key_len;
-			u8 key[0];
-		} crypt;
-	} u;
-};
-
 #define msleep_interruptible_rsl  msleep_interruptible
 
 /* Maximum size for the MA-UNITDATA primitive, 802.11 standard section
@@ -2066,8 +2014,6 @@ void rtllib_stop_all_queues(struct rtllib_device *ieee);
 struct sk_buff *rtllib_get_beacon(struct rtllib_device *ieee);
 void rtllib_start_send_beacons(struct rtllib_device *ieee);
 void rtllib_stop_send_beacons(struct rtllib_device *ieee);
-int rtllib_wpa_supplicant_ioctl(struct rtllib_device *ieee,
-				struct iw_point *p, u8 is_mesh);
 
 void notify_wx_assoc_event(struct rtllib_device *ieee);
 void rtllib_ps_tx_ack(struct rtllib_device *ieee, short success);
diff --git a/drivers/staging/rtl8192e/rtllib_rx.c b/drivers/staging/rtl8192e/rtllib_rx.c
index 43a77745e6fb..7bc9cb131bcc 100644
--- a/drivers/staging/rtl8192e/rtllib_rx.c
+++ b/drivers/staging/rtl8192e/rtllib_rx.c
@@ -782,7 +782,6 @@ static u8 parse_subframe(struct rtllib_device *ieee, struct sk_buff *skb,
 	u8		nPadding_Length = 0;
 	u16		SeqNum = 0;
 	struct sk_buff *sub_skb;
-	u8	     *data_ptr;
 	/* just for debug purpose */
 	SeqNum = WLAN_GET_SEQ_SEQ(le16_to_cpu(hdr->seq_ctl));
 	if ((RTLLIB_QOS_HAS_SEQ(fc)) &&
@@ -817,8 +816,7 @@ static u8 parse_subframe(struct rtllib_device *ieee, struct sk_buff *skb,
 		if (!sub_skb)
 			return 0;
 		skb_reserve(sub_skb, 12);
-		data_ptr = (u8 *)skb_put(sub_skb, skb->len);
-		memcpy(data_ptr, skb->data, skb->len);
+		skb_put_data(sub_skb, skb->data, skb->len);
 		sub_skb->dev = ieee->dev;
 
 		rxb->subframes[0] = sub_skb;
@@ -870,8 +868,7 @@ static u8 parse_subframe(struct rtllib_device *ieee, struct sk_buff *skb,
 		if (!sub_skb)
 			return 0;
 		skb_reserve(sub_skb, 12);
-		data_ptr = (u8 *)skb_put(sub_skb, nSubframe_Length);
-		memcpy(data_ptr, skb->data, nSubframe_Length);
+		skb_put_data(sub_skb, skb->data, nSubframe_Length);
 
 		sub_skb->dev = ieee->dev;
 		rxb->subframes[rxb->nr_subframes++] = sub_skb;
@@ -1141,13 +1138,12 @@ static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 			/* copy first fragment (including full headers) into
 			 * beginning of the fragment cache skb
 			 */
-			memcpy(skb_put(frag_skb, flen), skb->data, flen);
+			skb_put_data(frag_skb, skb->data, flen);
 		} else {
 			/* append frame payload to the end of the fragment
 			 * cache skb
 			 */
-			memcpy(skb_put(frag_skb, flen), skb->data + hdrlen,
-			       flen);
+			skb_put_data(frag_skb, skb->data + hdrlen, flen);
 		}
 		dev_kfree_skb_any(skb);
 		skb = NULL;
@@ -1214,9 +1210,6 @@ static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 		return -1;
 	}
 
-	if (rtllib_is_eapol_frame(ieee, skb, hdrlen))
-		netdev_warn(ieee->dev, "RX: IEEE802.1X EAPOL frame!\n");
-
 	return 0;
 }
 
diff --git a/drivers/staging/rtl8192e/rtllib_softmac.c b/drivers/staging/rtl8192e/rtllib_softmac.c
index eeda17d6409b..e4be85af31e7 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac.c
@@ -351,8 +351,7 @@ static inline struct sk_buff *rtllib_probe_req(struct rtllib_device *ieee)
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	req = (struct rtllib_probe_request *) skb_put(skb,
-	      sizeof(struct rtllib_probe_request));
+	req = skb_put(skb, sizeof(struct rtllib_probe_request));
 	req->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_PROBE_REQ);
 	req->header.duration_id = 0;
 
@@ -360,7 +359,7 @@ static inline struct sk_buff *rtllib_probe_req(struct rtllib_device *ieee)
 	ether_addr_copy(req->header.addr2, ieee->dev->dev_addr);
 	eth_broadcast_addr(req->header.addr3);
 
-	tag = (u8 *) skb_put(skb, len + 2 + rate_len);
+	tag = skb_put(skb, len + 2 + rate_len);
 
 	*tag++ = MFIE_TYPE_SSID;
 	*tag++ = len;
@@ -789,8 +788,7 @@ rtllib_authentication_req(struct rtllib_network *beacon,
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	auth = (struct rtllib_authentication *)
-		skb_put(skb, sizeof(struct rtllib_authentication));
+	auth = skb_put(skb, sizeof(struct rtllib_authentication));
 
 	auth->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_AUTH);
 	if (challengelen)
@@ -889,8 +887,7 @@ static struct sk_buff *rtllib_probe_resp(struct rtllib_device *ieee,
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	beacon_buf = (struct rtllib_probe_response *) skb_put(skb,
-		     (beacon_size - ieee->tx_headroom));
+	beacon_buf = skb_put(skb, (beacon_size - ieee->tx_headroom));
 	ether_addr_copy(beacon_buf->header.addr1, dest);
 	ether_addr_copy(beacon_buf->header.addr2, ieee->dev->dev_addr);
 	ether_addr_copy(beacon_buf->header.addr3, ieee->current_network.bssid);
@@ -984,8 +981,7 @@ static struct sk_buff *rtllib_assoc_resp(struct rtllib_device *ieee, u8 *dest)
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	assoc = (struct rtllib_assoc_response_frame *)
-		skb_put(skb, sizeof(struct rtllib_assoc_response_frame));
+	assoc = skb_put(skb, sizeof(struct rtllib_assoc_response_frame));
 
 	assoc->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_ASSOC_RESP);
 	ether_addr_copy(assoc->header.addr1, dest);
@@ -1016,7 +1012,7 @@ static struct sk_buff *rtllib_assoc_resp(struct rtllib_device *ieee, u8 *dest)
 	else
 		ieee->assoc_id++;
 
-	tag = (u8 *) skb_put(skb, rate_len);
+	tag = skb_put(skb, rate_len);
 	rtllib_MFIE_Brate(ieee, &tag);
 	rtllib_MFIE_Grate(ieee, &tag);
 
@@ -1038,8 +1034,7 @@ static struct sk_buff *rtllib_auth_resp(struct rtllib_device *ieee, int status,
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	auth = (struct rtllib_authentication *)
-		skb_put(skb, sizeof(struct rtllib_authentication));
+	auth = skb_put(skb, sizeof(struct rtllib_authentication));
 
 	auth->status = cpu_to_le16(status);
 	auth->transaction = cpu_to_le16(2);
@@ -1065,8 +1060,7 @@ static struct sk_buff *rtllib_null_func(struct rtllib_device *ieee, short pwr)
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	hdr = (struct rtllib_hdr_3addr *)skb_put(skb,
-	      sizeof(struct rtllib_hdr_3addr));
+	hdr = skb_put(skb, sizeof(struct rtllib_hdr_3addr));
 
 	ether_addr_copy(hdr->addr1, ieee->current_network.bssid);
 	ether_addr_copy(hdr->addr2, ieee->dev->dev_addr);
@@ -1092,8 +1086,7 @@ static struct sk_buff *rtllib_pspoll_func(struct rtllib_device *ieee)
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	hdr = (struct rtllib_pspoll_hdr *)skb_put(skb,
-	      sizeof(struct rtllib_pspoll_hdr));
+	hdr = skb_put(skb, sizeof(struct rtllib_pspoll_hdr));
 
 	ether_addr_copy(hdr->bssid, ieee->current_network.bssid);
 	ether_addr_copy(hdr->ta, ieee->dev->dev_addr);
@@ -1243,8 +1236,7 @@ rtllib_association_req(struct rtllib_network *beacon,
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	hdr = (struct rtllib_assoc_request_frame *)
-		skb_put(skb, sizeof(struct rtllib_assoc_request_frame) + 2);
+	hdr = skb_put(skb, sizeof(struct rtllib_assoc_request_frame) + 2);
 
 
 	hdr->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_ASSOC_REQ);
@@ -1272,8 +1264,7 @@ rtllib_association_req(struct rtllib_network *beacon,
 	hdr->info_element[0].id = MFIE_TYPE_SSID;
 
 	hdr->info_element[0].len = beacon->ssid_len;
-	tag = skb_put(skb, beacon->ssid_len);
-	memcpy(tag, beacon->ssid, beacon->ssid_len);
+	skb_put_data(skb, beacon->ssid, beacon->ssid_len);
 
 	tag = skb_put(skb, rate_len);
 
@@ -1349,8 +1340,7 @@ rtllib_association_req(struct rtllib_network *beacon,
 	}
 
 	if (wpa_ie_len) {
-		tag = skb_put(skb, ieee->wpa_ie_len);
-		memcpy(tag, ieee->wpa_ie, ieee->wpa_ie_len);
+		skb_put_data(skb, ieee->wpa_ie, ieee->wpa_ie_len);
 
 		if (PMKCacheIdx >= 0) {
 			tag = skb_put(skb, 18);
@@ -1366,13 +1356,13 @@ rtllib_association_req(struct rtllib_network *beacon,
 	}
 
 	if (wps_ie_len && ieee->wps_ie) {
-		tag = skb_put(skb, wps_ie_len);
-		memcpy(tag, ieee->wps_ie, wps_ie_len);
+		skb_put_data(skb, ieee->wps_ie, wps_ie_len);
 	}
 
-	tag = skb_put(skb, turbo_info_len);
-	if (turbo_info_len)
+	if (turbo_info_len) {
+		tag = skb_put(skb, turbo_info_len);
 		rtllib_TURBO_Info(ieee, &tag);
+	}
 
 	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT) {
 		if (ieee->pHTInfo->ePeerHTSpecVer == HT_SPEC_VER_EWC) {
@@ -1525,7 +1515,8 @@ static void rtllib_associate_complete_wq(void *data)
 				     associate_complete_wq);
 	struct rt_pwr_save_ctrl *pPSC = &(ieee->PowerSaveControl);
 
-	netdev_info(ieee->dev, "Associated successfully\n");
+	netdev_info(ieee->dev, "Associated successfully with %pM\n",
+		    ieee->current_network.bssid);
 	if (!ieee->is_silent_reset) {
 		netdev_info(ieee->dev, "normal associate\n");
 		notify_wx_assoc_event(ieee);
@@ -2309,7 +2300,8 @@ static void rtllib_rx_auth_resp(struct rtllib_device *ieee, struct sk_buff *skb)
 	if (errcode) {
 		ieee->softmac_stats.rx_auth_rs_err++;
 		netdev_info(ieee->dev,
-			    "Authentication respose status code 0x%x", errcode);
+			    "Authentication response status code 0x%x",
+			    errcode);
 		rtllib_associate_abort(ieee);
 		return;
 	}
@@ -3076,333 +3068,6 @@ void rtllib_softmac_free(struct rtllib_device *ieee)
 	tasklet_kill(&ieee->ps_task);
 }
 
-/********************************************************
- * Start of WPA code.				        *
- * this is stolen from the ipw2200 driver	        *
- ********************************************************/
-
-
-static int rtllib_wpa_enable(struct rtllib_device *ieee, int value)
-{
-	/* This is called when wpa_supplicant loads and closes the driver
-	 * interface.
-	 */
-	netdev_info(ieee->dev, "%s WPA\n", value ? "enabling" : "disabling");
-	ieee->wpa_enabled = value;
-	eth_zero_addr(ieee->ap_mac_addr);
-	return 0;
-}
-
-
-static void rtllib_wpa_assoc_frame(struct rtllib_device *ieee, char *wpa_ie,
-				   int wpa_ie_len)
-{
-	/* make sure WPA is enabled */
-	rtllib_wpa_enable(ieee, 1);
-
-	rtllib_disassociate(ieee);
-}
-
-
-static int rtllib_wpa_mlme(struct rtllib_device *ieee, int command, int reason)
-{
-
-	int ret = 0;
-
-	switch (command) {
-	case IEEE_MLME_STA_DEAUTH:
-		break;
-
-	case IEEE_MLME_STA_DISASSOC:
-		rtllib_disassociate(ieee);
-		break;
-
-	default:
-		netdev_info(ieee->dev, "Unknown MLME request: %d\n", command);
-		ret = -EOPNOTSUPP;
-	}
-
-	return ret;
-}
-
-
-static int rtllib_wpa_set_wpa_ie(struct rtllib_device *ieee,
-			      struct ieee_param *param, int plen)
-{
-	u8 *buf;
-
-	if (param->u.wpa_ie.len > MAX_WPA_IE_LEN ||
-	    (param->u.wpa_ie.len && param->u.wpa_ie.data == NULL))
-		return -EINVAL;
-
-	if (param->u.wpa_ie.len) {
-		buf = kmemdup(param->u.wpa_ie.data, param->u.wpa_ie.len,
-			      GFP_KERNEL);
-		if (buf == NULL)
-			return -ENOMEM;
-
-		kfree(ieee->wpa_ie);
-		ieee->wpa_ie = buf;
-		ieee->wpa_ie_len = param->u.wpa_ie.len;
-	} else {
-		kfree(ieee->wpa_ie);
-		ieee->wpa_ie = NULL;
-		ieee->wpa_ie_len = 0;
-	}
-
-	rtllib_wpa_assoc_frame(ieee, ieee->wpa_ie, ieee->wpa_ie_len);
-	return 0;
-}
-
-#define AUTH_ALG_OPEN_SYSTEM			0x1
-#define AUTH_ALG_SHARED_KEY			0x2
-#define AUTH_ALG_LEAP				0x4
-static int rtllib_wpa_set_auth_algs(struct rtllib_device *ieee, int value)
-{
-
-	struct rtllib_security sec = {
-		.flags = SEC_AUTH_MODE,
-	};
-
-	if (value & AUTH_ALG_SHARED_KEY) {
-		sec.auth_mode = WLAN_AUTH_SHARED_KEY;
-		ieee->open_wep = 0;
-		ieee->auth_mode = 1;
-	} else if (value & AUTH_ALG_OPEN_SYSTEM) {
-		sec.auth_mode = WLAN_AUTH_OPEN;
-		ieee->open_wep = 1;
-		ieee->auth_mode = 0;
-	} else if (value & AUTH_ALG_LEAP) {
-		sec.auth_mode = WLAN_AUTH_LEAP  >> 6;
-		ieee->open_wep = 1;
-		ieee->auth_mode = 2;
-	}
-
-
-	if (ieee->set_security)
-		ieee->set_security(ieee->dev, &sec);
-
-	return 0;
-}
-
-static int rtllib_wpa_set_param(struct rtllib_device *ieee, u8 name, u32 value)
-{
-	int ret = 0;
-	unsigned long flags;
-
-	switch (name) {
-	case IEEE_PARAM_WPA_ENABLED:
-		ret = rtllib_wpa_enable(ieee, value);
-		break;
-
-	case IEEE_PARAM_TKIP_COUNTERMEASURES:
-		ieee->tkip_countermeasures = value;
-		break;
-
-	case IEEE_PARAM_DROP_UNENCRYPTED:
-	{
-		/* HACK:
-		 *
-		 * wpa_supplicant calls set_wpa_enabled when the driver
-		 * is loaded and unloaded, regardless of if WPA is being
-		 * used.  No other calls are made which can be used to
-		 * determine if encryption will be used or not prior to
-		 * association being expected.  If encryption is not being
-		 * used, drop_unencrypted is set to false, else true -- we
-		 * can use this to determine if the CAP_PRIVACY_ON bit should
-		 * be set.
-		 */
-		struct rtllib_security sec = {
-			.flags = SEC_ENABLED,
-			.enabled = value,
-		};
-		ieee->drop_unencrypted = value;
-		/* We only change SEC_LEVEL for open mode. Others
-		 * are set by ipw_wpa_set_encryption.
-		 */
-		if (!value) {
-			sec.flags |= SEC_LEVEL;
-			sec.level = SEC_LEVEL_0;
-		} else {
-			sec.flags |= SEC_LEVEL;
-			sec.level = SEC_LEVEL_1;
-		}
-		if (ieee->set_security)
-			ieee->set_security(ieee->dev, &sec);
-		break;
-	}
-
-	case IEEE_PARAM_PRIVACY_INVOKED:
-		ieee->privacy_invoked = value;
-		break;
-
-	case IEEE_PARAM_AUTH_ALGS:
-		ret = rtllib_wpa_set_auth_algs(ieee, value);
-		break;
-
-	case IEEE_PARAM_IEEE_802_1X:
-		ieee->ieee802_1x = value;
-		break;
-	case IEEE_PARAM_WPAX_SELECT:
-		spin_lock_irqsave(&ieee->wpax_suitlist_lock, flags);
-		spin_unlock_irqrestore(&ieee->wpax_suitlist_lock, flags);
-		break;
-
-	default:
-		netdev_info(ieee->dev, "Unknown WPA param: %d\n", name);
-		ret = -EOPNOTSUPP;
-	}
-
-	return ret;
-}
-
-/* implementation borrowed from hostap driver */
-static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
-				  struct ieee_param *param, int param_len,
-				  u8 is_mesh)
-{
-	int ret = 0;
-	struct lib80211_crypto_ops *ops;
-	struct lib80211_crypt_data **crypt;
-
-	struct rtllib_security sec = {
-		.flags = 0,
-	};
-
-	param->u.crypt.err = 0;
-	param->u.crypt.alg[IEEE_CRYPT_ALG_NAME_LEN - 1] = '\0';
-
-	if (param_len !=
-	    (int) ((char *) param->u.crypt.key - (char *) param) +
-	    param->u.crypt.key_len) {
-		netdev_info(ieee->dev, "Len mismatch %d, %d\n", param_len,
-			    param->u.crypt.key_len);
-		return -EINVAL;
-	}
-	if (is_broadcast_ether_addr(param->sta_addr)) {
-		if (param->u.crypt.idx >= NUM_WEP_KEYS)
-			return -EINVAL;
-		crypt = &ieee->crypt_info.crypt[param->u.crypt.idx];
-	} else {
-		return -EINVAL;
-	}
-
-	if (strcmp(param->u.crypt.alg, "none") == 0) {
-		if (crypt) {
-			sec.enabled = 0;
-			sec.level = SEC_LEVEL_0;
-			sec.flags |= SEC_ENABLED | SEC_LEVEL;
-			lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
-		}
-		goto done;
-	}
-	sec.enabled = 1;
-	sec.flags |= SEC_ENABLED;
-
-	/* IPW HW cannot build TKIP MIC, host decryption still needed. */
-	if (!(ieee->host_encrypt || ieee->host_decrypt) &&
-	    strcmp(param->u.crypt.alg, "R-TKIP"))
-		goto skip_host_crypt;
-
-	ops = lib80211_get_crypto_ops(param->u.crypt.alg);
-	if (ops == NULL && strcmp(param->u.crypt.alg, "R-WEP") == 0) {
-		request_module("rtllib_crypt_wep");
-		ops = lib80211_get_crypto_ops(param->u.crypt.alg);
-	} else if (ops == NULL && strcmp(param->u.crypt.alg, "R-TKIP") == 0) {
-		request_module("rtllib_crypt_tkip");
-		ops = lib80211_get_crypto_ops(param->u.crypt.alg);
-	} else if (ops == NULL && strcmp(param->u.crypt.alg, "R-CCMP") == 0) {
-		request_module("rtllib_crypt_ccmp");
-		ops = lib80211_get_crypto_ops(param->u.crypt.alg);
-	}
-	if (ops == NULL) {
-		netdev_info(ieee->dev, "unknown crypto alg '%s'\n",
-			    param->u.crypt.alg);
-		param->u.crypt.err = IEEE_CRYPT_ERR_UNKNOWN_ALG;
-		ret = -EINVAL;
-		goto done;
-	}
-	if (*crypt == NULL || (*crypt)->ops != ops) {
-		struct lib80211_crypt_data *new_crypt;
-
-		lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
-
-		new_crypt = kzalloc(sizeof(*new_crypt), GFP_KERNEL);
-		if (new_crypt == NULL) {
-			ret = -ENOMEM;
-			goto done;
-		}
-		new_crypt->ops = ops;
-		if (new_crypt->ops && try_module_get(new_crypt->ops->owner))
-			new_crypt->priv =
-				new_crypt->ops->init(param->u.crypt.idx);
-
-		if (new_crypt->priv == NULL) {
-			kfree(new_crypt);
-			param->u.crypt.err = IEEE_CRYPT_ERR_CRYPT_INIT_FAILED;
-			ret = -EINVAL;
-			goto done;
-		}
-
-		*crypt = new_crypt;
-	}
-
-	if (param->u.crypt.key_len > 0 && (*crypt)->ops->set_key &&
-	    (*crypt)->ops->set_key(param->u.crypt.key,
-	    param->u.crypt.key_len, param->u.crypt.seq,
-	    (*crypt)->priv) < 0) {
-		netdev_info(ieee->dev, "key setting failed\n");
-		param->u.crypt.err = IEEE_CRYPT_ERR_KEY_SET_FAILED;
-		ret = -EINVAL;
-		goto done;
-	}
-
- skip_host_crypt:
-	if (param->u.crypt.set_tx) {
-		ieee->crypt_info.tx_keyidx = param->u.crypt.idx;
-		sec.active_key = param->u.crypt.idx;
-		sec.flags |= SEC_ACTIVE_KEY;
-	} else
-		sec.flags &= ~SEC_ACTIVE_KEY;
-
-	memcpy(sec.keys[param->u.crypt.idx],
-	       param->u.crypt.key,
-	       param->u.crypt.key_len);
-	sec.key_sizes[param->u.crypt.idx] = param->u.crypt.key_len;
-	sec.flags |= (1 << param->u.crypt.idx);
-
-	if (strcmp(param->u.crypt.alg, "R-WEP") == 0) {
-		sec.flags |= SEC_LEVEL;
-		sec.level = SEC_LEVEL_1;
-	} else if (strcmp(param->u.crypt.alg, "R-TKIP") == 0) {
-		sec.flags |= SEC_LEVEL;
-		sec.level = SEC_LEVEL_2;
-	} else if (strcmp(param->u.crypt.alg, "R-CCMP") == 0) {
-		sec.flags |= SEC_LEVEL;
-		sec.level = SEC_LEVEL_3;
-	}
- done:
-	if (ieee->set_security)
-		ieee->set_security(ieee->dev, &sec);
-
-	/* Do not reset port if card is in Managed mode since resetting will
-	 * generate new IEEE 802.11 authentication which may end up in looping
-	 * with IEEE 802.1X.  If your hardware requires a reset after WEP
-	 * configuration (for example... Prism2), implement the reset_port in
-	 * the callbacks structures used to initialize the 802.11 stack.
-	 */
-	if (ieee->reset_on_keychange &&
-	    ieee->iw_mode != IW_MODE_INFRA &&
-	    ieee->reset_port &&
-	    ieee->reset_port(ieee->dev)) {
-		netdev_info(ieee->dev, "reset_port failed\n");
-		param->u.crypt.err = IEEE_CRYPT_ERR_CARD_CONF_FAILED;
-		return -EINVAL;
-	}
-
-	return ret;
-}
-
 static inline struct sk_buff *
 rtllib_disauth_skb(struct rtllib_network *beacon,
 		   struct rtllib_device *ieee, u16 asRsn)
@@ -3417,8 +3082,7 @@ rtllib_disauth_skb(struct rtllib_network *beacon,
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	disauth = (struct rtllib_disauth *) skb_put(skb,
-		  sizeof(struct rtllib_disauth));
+	disauth = skb_put(skb, sizeof(struct rtllib_disauth));
 	disauth->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_DEAUTH);
 	disauth->header.duration_id = 0;
 
@@ -3445,8 +3109,7 @@ rtllib_disassociate_skb(struct rtllib_network *beacon,
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	disass = (struct rtllib_disassoc *) skb_put(skb,
-					 sizeof(struct rtllib_disassoc));
+	disass = skb_put(skb, sizeof(struct rtllib_disassoc));
 	disass->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_DISASSOC);
 	disass->header.duration_id = 0;
 
@@ -3501,62 +3164,6 @@ u8 rtllib_ap_sec_type(struct rtllib_device *ieee)
 	}
 }
 
-int rtllib_wpa_supplicant_ioctl(struct rtllib_device *ieee, struct iw_point *p,
-				u8 is_mesh)
-{
-	struct ieee_param *param;
-	int ret = 0;
-
-	mutex_lock(&ieee->wx_mutex);
-
-	if (p->length < sizeof(struct ieee_param) || !p->pointer) {
-		ret = -EINVAL;
-		goto out;
-	}
-
-	param = memdup_user(p->pointer, p->length);
-	if (IS_ERR(param)) {
-		ret = PTR_ERR(param);
-		goto out;
-	}
-
-	switch (param->cmd) {
-	case IEEE_CMD_SET_WPA_PARAM:
-		ret = rtllib_wpa_set_param(ieee, param->u.wpa_param.name,
-					param->u.wpa_param.value);
-		break;
-
-	case IEEE_CMD_SET_WPA_IE:
-		ret = rtllib_wpa_set_wpa_ie(ieee, param, p->length);
-		break;
-
-	case IEEE_CMD_SET_ENCRYPTION:
-		ret = rtllib_wpa_set_encryption(ieee, param, p->length, 0);
-		break;
-
-	case IEEE_CMD_MLME:
-		ret = rtllib_wpa_mlme(ieee, param->u.mlme.command,
-				   param->u.mlme.reason_code);
-		break;
-
-	default:
-		netdev_info(ieee->dev, "Unknown WPA supplicant request: %d\n",
-			    param->cmd);
-		ret = -EOPNOTSUPP;
-		break;
-	}
-
-	if (ret == 0 && copy_to_user(p->pointer, param, p->length))
-		ret = -EFAULT;
-
-	kfree(param);
-out:
-	mutex_unlock(&ieee->wx_mutex);
-
-	return ret;
-}
-EXPORT_SYMBOL(rtllib_wpa_supplicant_ioctl);
-
 static void rtllib_MgntDisconnectIBSS(struct rtllib_device *rtllib)
 {
 	u8	OpMode;
diff --git a/drivers/staging/rtl8192e/rtllib_tx.c b/drivers/staging/rtl8192e/rtllib_tx.c
index 78a3ad5b231f..fc88d47dea43 100644
--- a/drivers/staging/rtl8192e/rtllib_tx.c
+++ b/drivers/staging/rtl8192e/rtllib_tx.c
@@ -624,8 +624,7 @@ static int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 
 			txb->encrypted = 0;
 			txb->payload_size = cpu_to_le16(skb->len);
-			memcpy(skb_put(txb->fragments[0], skb->len), skb->data,
-			       skb->len);
+			skb_put_data(txb->fragments[0], skb->data, skb->len);
 
 			goto success;
 		}
@@ -818,9 +817,7 @@ static int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 			} else {
 				tcb_desc->bHwSec = 0;
 			}
-			frag_hdr = (struct rtllib_hdr_3addrqos *)
-				   skb_put(skb_frag, hdr_len);
-			memcpy(frag_hdr, &header, hdr_len);
+			frag_hdr = skb_put_data(skb_frag, &header, hdr_len);
 
 			/* If this is not the last fragment, then add the
 			 * MOREFRAGS bit to the frame control
@@ -852,7 +849,7 @@ static int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 				bytes -= SNAP_SIZE + sizeof(u16);
 			}
 
-			memcpy(skb_put(skb_frag, bytes), skb->data, bytes);
+			skb_put_data(skb_frag, skb->data, bytes);
 
 			/* Advance the SKB... */
 			skb_pull(skb, bytes);
@@ -895,8 +892,7 @@ static int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 
 		txb->encrypted = 0;
 		txb->payload_size = cpu_to_le16(skb->len);
-		memcpy(skb_put(txb->fragments[0], skb->len), skb->data,
-		       skb->len);
+		skb_put_data(txb->fragments[0], skb->data, skb->len);
 	}
 
  success:
diff --git a/drivers/staging/rtl8192e/rtllib_wx.c b/drivers/staging/rtl8192e/rtllib_wx.c
index c5c0d9676dc8..f7eba01b5d15 100644
--- a/drivers/staging/rtl8192e/rtllib_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_wx.c
@@ -595,7 +595,7 @@ int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
 		ret = -EINVAL;
 		goto done;
 	}
-	netdev_info(dev, "alg name:%s\n", alg);
+	netdev_dbg(dev, "alg name:%s\n", alg);
 
 	ops = lib80211_get_crypto_ops(alg);
 	if (ops == NULL) {
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211.h b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
index 899c77ed2a43..b062cad052b9 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211.h
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
@@ -2187,7 +2187,7 @@ int ieee80211_encrypt_fragment(struct ieee80211_device *ieee,
 			       struct sk_buff *frag, int hdr_len);
 
 int ieee80211_xmit(struct sk_buff *skb, struct net_device *dev);
-void ieee80211_txb_free(struct ieee80211_txb *);
+void ieee80211_txb_free(struct ieee80211_txb *txb);
 
 
 /* ieee80211_rx.c */
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.h b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.h
index 005bf89aae65..a0aa0f5be63a 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.h
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt.h
@@ -82,8 +82,8 @@ struct ieee80211_crypt_data {
 int ieee80211_register_crypto_ops(struct ieee80211_crypto_ops *ops);
 int ieee80211_unregister_crypto_ops(struct ieee80211_crypto_ops *ops);
 struct ieee80211_crypto_ops *ieee80211_get_crypto_ops(const char *name);
-void ieee80211_crypt_deinit_entries(struct ieee80211_device *, int);
-void ieee80211_crypt_deinit_handler(unsigned long);
+void ieee80211_crypt_deinit_entries(struct ieee80211_device *ieee, int force);
+void ieee80211_crypt_deinit_handler(unsigned long data);
 void ieee80211_crypt_delayed_deinit(struct ieee80211_device *ieee,
 				    struct ieee80211_crypt_data **crypt);
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
index 5039172409e3..60ecfec71112 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
@@ -171,13 +171,6 @@ static inline u16 Mk16(u8 hi, u8 lo)
 	return lo | (((u16) hi) << 8);
 }
 
-
-static inline u16 Mk16_le(u16 *v)
-{
-	return le16_to_cpu(*v);
-}
-
-
 static const u16 Sbox[256] = {
 	0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
 	0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
@@ -264,15 +257,15 @@ static void tkip_mixing_phase2(u8 *WEPSeed, const u8 *TK, const u16 *TTAK,
 	PPK[5] = TTAK[4] + IV16;
 
 	/* Step 2 - 96-bit bijective mixing using S-box */
-	PPK[0] += _S_(PPK[5] ^ Mk16_le((u16 *) &TK[0]));
-	PPK[1] += _S_(PPK[0] ^ Mk16_le((u16 *) &TK[2]));
-	PPK[2] += _S_(PPK[1] ^ Mk16_le((u16 *) &TK[4]));
-	PPK[3] += _S_(PPK[2] ^ Mk16_le((u16 *) &TK[6]));
-	PPK[4] += _S_(PPK[3] ^ Mk16_le((u16 *) &TK[8]));
-	PPK[5] += _S_(PPK[4] ^ Mk16_le((u16 *) &TK[10]));
-
-	PPK[0] += RotR1(PPK[5] ^ Mk16_le((u16 *) &TK[12]));
-	PPK[1] += RotR1(PPK[0] ^ Mk16_le((u16 *) &TK[14]));
+	PPK[0] += _S_(PPK[5] ^ le16_to_cpu(*(__le16 *)(&TK[0])));
+	PPK[1] += _S_(PPK[0] ^ le16_to_cpu(*(__le16 *)(&TK[2])));
+	PPK[2] += _S_(PPK[1] ^ le16_to_cpu(*(__le16 *)(&TK[4])));
+	PPK[3] += _S_(PPK[2] ^ le16_to_cpu(*(__le16 *)(&TK[6])));
+	PPK[4] += _S_(PPK[3] ^ le16_to_cpu(*(__le16 *)(&TK[8])));
+	PPK[5] += _S_(PPK[4] ^ le16_to_cpu(*(__le16 *)(&TK[10])));
+
+	PPK[0] += RotR1(PPK[5] ^ le16_to_cpu(*(__le16 *)(&TK[12])));
+	PPK[1] += RotR1(PPK[0] ^ le16_to_cpu(*(__le16 *)(&TK[14])));
 	PPK[2] += RotR1(PPK[1]);
 	PPK[3] += RotR1(PPK[2]);
 	PPK[4] += RotR1(PPK[3]);
@@ -285,7 +278,7 @@ static void tkip_mixing_phase2(u8 *WEPSeed, const u8 *TK, const u16 *TTAK,
 	WEPSeed[0] = Hi8(IV16);
 	WEPSeed[1] = (Hi8(IV16) | 0x20) & 0x7F;
 	WEPSeed[2] = Lo8(IV16);
-	WEPSeed[3] = Lo8((PPK[5] ^ Mk16_le((u16 *) &TK[0])) >> 1);
+	WEPSeed[3] = Lo8((PPK[5] ^ le16_to_cpu(*(__le16 *)(&TK[0]))) >> 1);
 
 #ifdef __BIG_ENDIAN
 	{
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
index a791175b86f5..8f236b332a47 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
@@ -157,8 +157,7 @@ struct net_device *alloc_ieee80211(int sizeof_priv)
 	ieee80211_softmac_init(ieee);
 
 	ieee->pHTInfo = kzalloc(sizeof(RT_HIGH_THROUGHPUT), GFP_KERNEL);
-	if (ieee->pHTInfo == NULL)
-	{
+	if (ieee->pHTInfo == NULL) {
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't alloc memory for HTInfo\n");
 		goto failed;
 	}
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
index 7a31510f0524..a4aedb489e92 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
@@ -780,7 +780,6 @@ static u8 parse_subframe(struct sk_buff *skb,
 	u16		SeqNum=0;
 
 	struct sk_buff *sub_skb;
-	u8             *data_ptr;
 	/* just for debug purpose */
 	SeqNum = WLAN_GET_SEQ_SEQ(le16_to_cpu(hdr->seq_ctl));
 
@@ -848,8 +847,7 @@ static u8 parse_subframe(struct sk_buff *skb,
 			if (!sub_skb)
 				return 0;
 			skb_reserve(sub_skb, 12);
-			data_ptr = (u8 *)skb_put(sub_skb, nSubframe_Length);
-			memcpy(data_ptr, skb->data, nSubframe_Length);
+			skb_put_data(sub_skb, skb->data, nSubframe_Length);
 #endif
 			rxb->subframes[rxb->nr_subframes++] = sub_skb;
 			if (rxb->nr_subframes >= MAX_SUBFRAME_COUNT) {
@@ -1180,12 +1178,11 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 		if (frag == 0) {
 			/* copy first fragment (including full headers) into
 			 * beginning of the fragment cache skb */
-			memcpy(skb_put(frag_skb, flen), skb->data, flen);
+			skb_put_data(frag_skb, skb->data, flen);
 		} else {
 			/* append frame payload to the end of the fragment
 			 * cache skb */
-			memcpy(skb_put(frag_skb, flen), skb->data + hdrlen,
-			       flen);
+			skb_put_data(frag_skb, skb->data + hdrlen, flen);
 		}
 		dev_kfree_skb_any(skb);
 		skb = NULL;
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
index 14aea26804f4..fe6f38b7ec35 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
@@ -341,7 +341,7 @@ static inline struct sk_buff *ieee80211_probe_req(struct ieee80211_device *ieee)
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	req = (struct ieee80211_probe_request *) skb_put(skb,sizeof(struct ieee80211_probe_request));
+	req = skb_put(skb, sizeof(struct ieee80211_probe_request));
 	req->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
 	req->header.duration_id = 0; /* FIXME: is this OK? */
 
@@ -349,7 +349,7 @@ static inline struct sk_buff *ieee80211_probe_req(struct ieee80211_device *ieee)
 	memcpy(req->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
 	eth_broadcast_addr(req->header.addr3);
 
-	tag = (u8 *) skb_put(skb,len+2+rate_len);
+	tag = skb_put(skb, len + 2 + rate_len);
 
 	*tag++ = MFIE_TYPE_SSID;
 	*tag++ = len;
@@ -659,8 +659,7 @@ ieee80211_authentication_req(struct ieee80211_network *beacon,
 	if (!skb) return NULL;
 
 	skb_reserve(skb, ieee->tx_headroom);
-	auth = (struct ieee80211_authentication *)
-		skb_put(skb, sizeof(struct ieee80211_authentication));
+	auth = skb_put(skb, sizeof(struct ieee80211_authentication));
 
 	if (challengelen)
 		auth->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_AUTH
@@ -768,7 +767,7 @@ static struct sk_buff *ieee80211_probe_resp(struct ieee80211_device *ieee, u8 *d
 	if (!skb)
 		return NULL;
 	skb_reserve(skb, ieee->tx_headroom);
-	beacon_buf = (struct ieee80211_probe_response *) skb_put(skb, (beacon_size - ieee->tx_headroom));
+	beacon_buf = skb_put(skb, (beacon_size - ieee->tx_headroom));
 	memcpy (beacon_buf->header.addr1, dest,ETH_ALEN);
 	memcpy (beacon_buf->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
 	memcpy (beacon_buf->header.addr3, ieee->current_network.bssid, ETH_ALEN);
@@ -864,8 +863,7 @@ static struct sk_buff *ieee80211_assoc_resp(struct ieee80211_device *ieee,
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	assoc = (struct ieee80211_assoc_response_frame *)
-		skb_put(skb, sizeof(struct ieee80211_assoc_response_frame));
+	assoc = skb_put(skb, sizeof(struct ieee80211_assoc_response_frame));
 
 	assoc->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_ASSOC_RESP);
 	memcpy(assoc->header.addr1, dest,ETH_ALEN);
@@ -892,7 +890,7 @@ static struct sk_buff *ieee80211_assoc_resp(struct ieee80211_device *ieee,
 	if (ieee->assoc_id == 0x2007) ieee->assoc_id=0;
 	else ieee->assoc_id++;
 
-	tag = (u8 *) skb_put(skb, rate_len);
+	tag = skb_put(skb, rate_len);
 
 	ieee80211_MFIE_Brate(ieee, &tag);
 	ieee80211_MFIE_Grate(ieee, &tag);
@@ -940,7 +938,7 @@ static struct sk_buff *ieee80211_null_func(struct ieee80211_device *ieee,
 	if (!skb)
 		return NULL;
 
-	hdr = (struct rtl_80211_hdr_3addr *)skb_put(skb,sizeof(struct rtl_80211_hdr_3addr));
+	hdr = skb_put(skb, sizeof(struct rtl_80211_hdr_3addr));
 
 	memcpy(hdr->addr1, ieee->current_network.bssid, ETH_ALEN);
 	memcpy(hdr->addr2, ieee->dev->dev_addr, ETH_ALEN);
@@ -1086,8 +1084,7 @@ ieee80211_association_req(struct ieee80211_network *beacon,
 
 	skb_reserve(skb, ieee->tx_headroom);
 
-	hdr = (struct ieee80211_assoc_request_frame *)
-		skb_put(skb, sizeof(struct ieee80211_assoc_request_frame)+2);
+	hdr = skb_put(skb, sizeof(struct ieee80211_assoc_request_frame) + 2);
 
 
 	hdr->header.frame_ctl = IEEE80211_STYPE_ASSOC_REQ;
@@ -1115,8 +1112,7 @@ ieee80211_association_req(struct ieee80211_network *beacon,
 	hdr->info_element[0].id = MFIE_TYPE_SSID;
 
 	hdr->info_element[0].len = beacon->ssid_len;
-	tag = skb_put(skb, beacon->ssid_len);
-	memcpy(tag, beacon->ssid, beacon->ssid_len);
+	skb_put_data(skb, beacon->ssid, beacon->ssid_len);
 
 	tag = skb_put(skb, rate_len);
 
@@ -1188,18 +1184,17 @@ ieee80211_association_req(struct ieee80211_network *beacon,
 
 
 	//choose what wpa_supplicant gives to associate.
-	tag = skb_put(skb, wpa_ie_len);
 	if (wpa_ie_len) {
-		memcpy(tag, ieee->wpa_ie, ieee->wpa_ie_len);
+		skb_put_data(skb, ieee->wpa_ie, wpa_ie_len);
 	}
 
-	tag = skb_put(skb, wmm_info_len);
 	if (wmm_info_len) {
-	  ieee80211_WMM_Info(ieee, &tag);
+		tag = skb_put(skb, wmm_info_len);
+		ieee80211_WMM_Info(ieee, &tag);
 	}
 #ifdef THOMAS_TURBO
-	tag = skb_put(skb, turbo_info_len);
 	if (turbo_info_len) {
+		tag = skb_put(skb, turbo_info_len);
 		ieee80211_TURBO_Info(ieee, &tag);
 	}
 #endif
@@ -3111,7 +3106,7 @@ static inline struct sk_buff *ieee80211_disassociate_skb(
 	if (!skb)
 		return NULL;
 
-	disass = (struct ieee80211_disassoc *) skb_put(skb, sizeof(struct ieee80211_disassoc));
+	disass = skb_put(skb, sizeof(struct ieee80211_disassoc));
 	disass->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_DISASSOC);
 	disass->header.duration_id = 0;
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
index bdb96a45a9eb..f58971a4a2e3 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
@@ -794,8 +794,7 @@ int ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
 			{
 				tcb_desc->bHwSec = 0;
 			}
-			frag_hdr = (struct rtl_80211_hdr_3addrqos *)skb_put(skb_frag, hdr_len);
-			memcpy(frag_hdr, &header, hdr_len);
+			frag_hdr = skb_put_data(skb_frag, &header, hdr_len);
 
 			/* If this is not the last fragment, then add the MOREFRAGS
 			 * bit to the frame control
@@ -826,7 +825,7 @@ int ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
 				bytes -= SNAP_SIZE + sizeof(u16);
 			}
 
-			memcpy(skb_put(skb_frag, bytes), skb->data, bytes);
+			skb_put_data(skb_frag, skb->data, bytes);
 
 			/* Advance the SKB... */
 			skb_pull(skb, bytes);
@@ -869,7 +868,7 @@ int ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
 
 		txb->encrypted = 0;
 		txb->payload_size = __cpu_to_le16(skb->len);
-		memcpy(skb_put(txb->fragments[0],skb->len), skb->data, skb->len);
+		skb_put_data(txb->fragments[0], skb->data, skb->len);
 	}
 
  success:
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
index e82b5073c3f1..8aa38dcf0dfd 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
@@ -125,7 +125,7 @@ static struct sk_buff *ieee80211_ADDBA(struct ieee80211_device *ieee, u8 *Dst, P
 	memset(skb->data, 0, sizeof( struct rtl_80211_hdr_3addr));	//I wonder whether it's necessary. Apparently kernel will not do it when alloc a skb.
 	skb_reserve(skb, ieee->tx_headroom);
 
-	BAReq = ( struct rtl_80211_hdr_3addr *) skb_put(skb,sizeof( struct rtl_80211_hdr_3addr));
+	BAReq = skb_put(skb, sizeof(struct rtl_80211_hdr_3addr));
 
 	memcpy(BAReq->addr1, Dst, ETH_ALEN);
 	memcpy(BAReq->addr2, ieee->dev->dev_addr, ETH_ALEN);
@@ -135,7 +135,7 @@ static struct sk_buff *ieee80211_ADDBA(struct ieee80211_device *ieee, u8 *Dst, P
 	BAReq->frame_ctl = cpu_to_le16(IEEE80211_STYPE_MANAGE_ACT); //action frame
 
 	//tag += sizeof( struct rtl_80211_hdr_3addr); //move to action field
-	tag = (u8 *)skb_put(skb, 9);
+	tag = skb_put(skb, 9);
 	*tag ++= ACT_CAT_BA;
 	*tag ++= type;
 	// Dialog Token
@@ -209,14 +209,14 @@ static struct sk_buff *ieee80211_DELBA(
 //	memset(skb->data, 0, len+sizeof( struct rtl_80211_hdr_3addr));
 	skb_reserve(skb, ieee->tx_headroom);
 
-	Delba = ( struct rtl_80211_hdr_3addr *) skb_put(skb,sizeof( struct rtl_80211_hdr_3addr));
+	Delba = skb_put(skb, sizeof(struct rtl_80211_hdr_3addr));
 
 	memcpy(Delba->addr1, dst, ETH_ALEN);
 	memcpy(Delba->addr2, ieee->dev->dev_addr, ETH_ALEN);
 	memcpy(Delba->addr3, ieee->current_network.bssid, ETH_ALEN);
 	Delba->frame_ctl = cpu_to_le16(IEEE80211_STYPE_MANAGE_ACT); //action frame
 
-	tag = (u8 *)skb_put(skb, 6);
+	tag = skb_put(skb, 6);
 
 	*tag ++= ACT_CAT_BA;
 	*tag ++= ACT_DELBA;
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
index b4c13fff2c65..f98bb03aa293 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
@@ -36,18 +36,15 @@ static void RxPktPendingTimeout(unsigned long data)
 
 	spin_lock_irqsave(&(ieee->reorder_spinlock), flags);
 	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"==================>%s()\n",__func__);
-	if(pRxTs->RxTimeoutIndicateSeq != 0xffff)
-	{
+	if(pRxTs->RxTimeoutIndicateSeq != 0xffff) {
 		// Indicate the pending packets sequentially according to SeqNum until meet the gap.
-		while(!list_empty(&pRxTs->RxPendingPktList))
-		{
+		while(!list_empty(&pRxTs->RxPendingPktList)) {
 			pReorderEntry = (PRX_REORDER_ENTRY)list_entry(pRxTs->RxPendingPktList.prev,RX_REORDER_ENTRY,List);
 			if(index == 0)
 				pRxTs->RxIndicateSeq = pReorderEntry->SeqNum;
 
 			if( SN_LESS(pReorderEntry->SeqNum, pRxTs->RxIndicateSeq) ||
-				SN_EQUAL(pReorderEntry->SeqNum, pRxTs->RxIndicateSeq)	)
-			{
+				SN_EQUAL(pReorderEntry->SeqNum, pRxTs->RxIndicateSeq)	) {
 				list_del_init(&pReorderEntry->List);
 
 				if(SN_EQUAL(pReorderEntry->SeqNum, pRxTs->RxIndicateSeq))
@@ -58,22 +55,19 @@ static void RxPktPendingTimeout(unsigned long data)
 				index++;
 
 				list_add_tail(&pReorderEntry->List, &ieee->RxReorder_Unused_List);
-			}
-			else
-			{
+			} else {
 				bPktInBuf = true;
 				break;
 			}
 		}
 	}
 
-	if(index>0)
-	{
+	if(index>0) {
 		// Set RxTimeoutIndicateSeq to 0xffff to indicate no pending packets in buffer now.
 		pRxTs->RxTimeoutIndicateSeq = 0xffff;
 
 		// Indicate packets
-		if(index > REORDER_WIN_SIZE){
+		if(index > REORDER_WIN_SIZE) {
 			IEEE80211_DEBUG(IEEE80211_DL_ERR, "RxReorderIndicatePacket(): Rx Reorder buffer full!! \n");
 			spin_unlock_irqrestore(&(ieee->reorder_spinlock), flags);
 			return;
@@ -81,8 +75,7 @@ static void RxPktPendingTimeout(unsigned long data)
 		ieee80211_indicate_packets(ieee, ieee->stats_IndicateArray, index);
 	}
 
-	if(bPktInBuf && (pRxTs->RxTimeoutIndicateSeq==0xffff))
-	{
+	if(bPktInBuf && (pRxTs->RxTimeoutIndicateSeq==0xffff)) {
 		pRxTs->RxTimeoutIndicateSeq = pRxTs->RxIndicateSeq;
 		mod_timer(&pRxTs->RxPktPendingTimer,
 			  jiffies + msecs_to_jiffies(ieee->pHTInfo->RxReorderPendingTime));
@@ -147,8 +140,7 @@ void TSInitialize(struct ieee80211_device *ieee)
 	INIT_LIST_HEAD(&ieee->Tx_TS_Pending_List);
 	INIT_LIST_HEAD(&ieee->Tx_TS_Unused_List);
 
-	for(count = 0; count < TOTAL_TS_NUM; count++)
-	{
+	for(count = 0; count < TOTAL_TS_NUM; count++) {
 		//
 		pTxTS->num = count;
 		// The timers for the operation of Traffic Stream and Block Ack.
@@ -172,8 +164,7 @@ void TSInitialize(struct ieee80211_device *ieee)
 	INIT_LIST_HEAD(&ieee->Rx_TS_Admit_List);
 	INIT_LIST_HEAD(&ieee->Rx_TS_Pending_List);
 	INIT_LIST_HEAD(&ieee->Rx_TS_Unused_List);
-	for(count = 0; count < TOTAL_TS_NUM; count++)
-	{
+	for(count = 0; count < TOTAL_TS_NUM; count++) {
 		pRxTS->num = count;
 		INIT_LIST_HEAD(&pRxTS->RxPendingPktList);
 		setup_timer(&pRxTS->TsCommonInfo.SetupTimer, TsSetupTimeOut,
@@ -191,15 +182,13 @@ void TSInitialize(struct ieee80211_device *ieee)
 	// Initialize unused Rx Reorder List.
 	INIT_LIST_HEAD(&ieee->RxReorder_Unused_List);
 //#ifdef TO_DO_LIST
-	for(count = 0; count < REORDER_ENTRY_NUM; count++)
-	{
+	for(count = 0; count < REORDER_ENTRY_NUM; count++) {
 		list_add_tail( &pRxReorderEntry->List,&ieee->RxReorder_Unused_List);
 		if(count == (REORDER_ENTRY_NUM-1))
 			break;
 		pRxReorderEntry = &ieee->RxReorderEntry[count+1];
 	}
 //#endif
-
 }
 
 static void AdmitTS(struct ieee80211_device *ieee,
@@ -223,36 +212,25 @@ static PTS_COMMON_INFO SearchAdmitTRStream(struct ieee80211_device *ieee,
 	bool				search_dir[4] = {0};
 	struct list_head		*psearch_list; //FIXME
 	PTS_COMMON_INFO	pRet = NULL;
-	if(ieee->iw_mode == IW_MODE_MASTER) //ap mode
-	{
-		if(TxRxSelect == TX_DIR)
-		{
+	if(ieee->iw_mode == IW_MODE_MASTER) { //ap mode
+		if(TxRxSelect == TX_DIR) {
 			search_dir[DIR_DOWN] = true;
 			search_dir[DIR_BI_DIR]= true;
-		}
-		else
-		{
+		} else {
 			search_dir[DIR_UP]	= true;
 			search_dir[DIR_BI_DIR]= true;
 		}
-	}
-	else if(ieee->iw_mode == IW_MODE_ADHOC)
-	{
+	} else if(ieee->iw_mode == IW_MODE_ADHOC) {
 		if(TxRxSelect == TX_DIR)
 			search_dir[DIR_UP]	= true;
 		else
 			search_dir[DIR_DOWN] = true;
-	}
-	else
-	{
-		if(TxRxSelect == TX_DIR)
-		{
+	} else {
+		if(TxRxSelect == TX_DIR) {
 			search_dir[DIR_UP]	= true;
 			search_dir[DIR_BI_DIR]= true;
 			search_dir[DIR_DIRECT]= true;
-		}
-		else
-		{
+		} else {
 			search_dir[DIR_DOWN] = true;
 			search_dir[DIR_BI_DIR]= true;
 			search_dir[DIR_DIRECT]= true;
@@ -265,28 +243,24 @@ static PTS_COMMON_INFO SearchAdmitTRStream(struct ieee80211_device *ieee,
 		psearch_list = &ieee->Rx_TS_Admit_List;
 
 	//for(dir = DIR_UP; dir <= DIR_BI_DIR; dir++)
-	for(dir = 0; dir <= DIR_BI_DIR; dir++)
-	{
+	for(dir = 0; dir <= DIR_BI_DIR; dir++) {
 		if (!search_dir[dir])
 			continue;
 		list_for_each_entry(pRet, psearch_list, List){
 	//		IEEE80211_DEBUG(IEEE80211_DL_TS, "ADD:%pM, TID:%d, dir:%d\n", pRet->Addr, pRet->TSpec.f.TSInfo.field.ucTSID, pRet->TSpec.f.TSInfo.field.ucDirection);
 			if (memcmp(pRet->Addr, Addr, 6) == 0)
 				if (pRet->TSpec.f.TSInfo.field.ucTSID == TID)
-					if(pRet->TSpec.f.TSInfo.field.ucDirection == dir)
-					{
+					if(pRet->TSpec.f.TSInfo.field.ucDirection == dir) {
 	//					printk("Bingo! got it\n");
 						break;
 					}
-
 		}
 		if(&pRet->List  != psearch_list)
 			break;
 	}
 
-	if(&pRet->List  != psearch_list){
+	if(&pRet->List  != psearch_list)
 		return pRet ;
-	}
 	else
 		return NULL;
 }
@@ -327,25 +301,21 @@ bool GetTs(
 	// We do not build any TS for Broadcast or Multicast stream.
 	// So reject these kinds of search here.
 	//
-	if (is_multicast_ether_addr(Addr))
-	{
+	if (is_multicast_ether_addr(Addr)) {
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "get TS for Broadcast or Multicast\n");
 		return false;
 	}
 
-	if (ieee->current_network.qos_data.supported == 0)
+	if (ieee->current_network.qos_data.supported == 0) {
 		UP = 0;
-	else
-	{
+	} else {
 		// In WMM case: we use 4 TID only
-		if (!IsACValid(TID))
-		{
+		if (!IsACValid(TID)) {
 			IEEE80211_DEBUG(IEEE80211_DL_ERR, " in %s(), TID(%d) is not valid\n", __func__, TID);
 			return false;
 		}
 
-		switch (TID)
-		{
+		switch (TID) {
 		case 0:
 		case 3:
 			UP = 0;
@@ -373,18 +343,13 @@ bool GetTs(
 			Addr,
 			UP,
 			TxRxSelect);
-	if(*ppTS != NULL)
-	{
+	if(*ppTS != NULL) {
 		return true;
-	}
-	else
-	{
+	} else {
 		if (!bAddNewTs) {
 			IEEE80211_DEBUG(IEEE80211_DL_TS, "add new TS failed(tid:%d)\n", UP);
 			return false;
-		}
-		else
-		{
+		} else {
 			//
 			// Create a new Traffic stream for current Tx/Rx
 			// This is for EDCA and WMM to add a new TS.
@@ -406,16 +371,13 @@ bool GetTs(
 								((TxRxSelect==TX_DIR)?DIR_DOWN:DIR_UP):
 								((TxRxSelect==TX_DIR)?DIR_UP:DIR_DOWN);
 			IEEE80211_DEBUG(IEEE80211_DL_TS, "to add Ts\n");
-			if(!list_empty(pUnusedList))
-			{
+			if(!list_empty(pUnusedList)) {
 				(*ppTS) = list_entry(pUnusedList->next, TS_COMMON_INFO, List);
 				list_del_init(&(*ppTS)->List);
-				if(TxRxSelect==TX_DIR)
-				{
+				if(TxRxSelect==TX_DIR) {
 					PTX_TS_RECORD tmp = container_of(*ppTS, TX_TS_RECORD, TsCommonInfo);
 					ResetTxTsEntry(tmp);
-				}
-				else{
+				} else {
 					PRX_TS_RECORD tmp = container_of(*ppTS, RX_TS_RECORD, TsCommonInfo);
 					ResetRxTsEntry(tmp);
 				}
@@ -438,9 +400,7 @@ bool GetTs(
 				// if there is DirectLink, we need to do additional operation here!!
 
 				return true;
-			}
-			else
-			{
+			} else {
 				IEEE80211_DEBUG(IEEE80211_DL_ERR, "in function %s() There is not enough TS record to be used!!", __func__);
 				return false;
 			}
@@ -457,16 +417,14 @@ static void RemoveTsEntry(struct ieee80211_device *ieee, PTS_COMMON_INFO pTs,
 	del_timer_sync(&pTs->InactTimer);
 	TsInitDelBA(ieee, pTs, TxRxSelect);
 
-	if(TxRxSelect == RX_DIR)
-	{
+	if(TxRxSelect == RX_DIR) {
 //#ifdef TO_DO_LIST
 		PRX_REORDER_ENTRY	pRxReorderEntry;
 		PRX_TS_RECORD		pRxTS = (PRX_TS_RECORD)pTs;
 		if(timer_pending(&pRxTS->RxPktPendingTimer))
 			del_timer_sync(&pRxTS->RxPktPendingTimer);
 
-		while(!list_empty(&pRxTS->RxPendingPktList))
-		{
+		while(!list_empty(&pRxTS->RxPendingPktList)) {
 			spin_lock_irqsave(&(ieee->reorder_spinlock), flags);
 			//pRxReorderEntry = list_entry(&pRxTS->RxPendingPktList.prev,RX_REORDER_ENTRY,List);
 			pRxReorderEntry = (PRX_REORDER_ENTRY)list_entry(pRxTS->RxPendingPktList.prev,RX_REORDER_ENTRY,List);
@@ -474,14 +432,13 @@ static void RemoveTsEntry(struct ieee80211_device *ieee, PTS_COMMON_INFO pTs,
 			{
 				int i = 0;
 				struct ieee80211_rxb *prxb = pRxReorderEntry->prxb;
-				if (unlikely(!prxb))
-				{
+				if (unlikely(!prxb)) {
 					spin_unlock_irqrestore(&(ieee->reorder_spinlock), flags);
 					return;
 				}
-				for(i =0; i < prxb->nr_subframes; i++) {
+				for(i =0; i < prxb->nr_subframes; i++)
 					dev_kfree_skb(prxb->subframes[i]);
-				}
+
 				kfree(prxb);
 				prxb = NULL;
 			}
@@ -490,9 +447,7 @@ static void RemoveTsEntry(struct ieee80211_device *ieee, PTS_COMMON_INFO pTs,
 		}
 
 //#endif
-	}
-	else
-	{
+	} else {
 		PTX_TS_RECORD pTxTS = (PTX_TS_RECORD)pTs;
 		del_timer_sync(&pTxTS->TsAddBaTimer);
 	}
@@ -503,20 +458,16 @@ void RemovePeerTS(struct ieee80211_device *ieee, u8 *Addr)
 	PTS_COMMON_INFO	pTS, pTmpTS;
 
 	printk("===========>RemovePeerTS,%pM\n", Addr);
-	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Pending_List, List)
-	{
-		if (memcmp(pTS->Addr, Addr, 6) == 0)
-		{
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Pending_List, List) {
+		if (memcmp(pTS->Addr, Addr, 6) == 0) {
 			RemoveTsEntry(ieee, pTS, TX_DIR);
 			list_del_init(&pTS->List);
 			list_add_tail(&pTS->List, &ieee->Tx_TS_Unused_List);
 		}
 	}
 
-	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Admit_List, List)
-	{
-		if (memcmp(pTS->Addr, Addr, 6) == 0)
-		{
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Admit_List, List) {
+		if (memcmp(pTS->Addr, Addr, 6) == 0) {
 			printk("====>remove Tx_TS_admin_list\n");
 			RemoveTsEntry(ieee, pTS, TX_DIR);
 			list_del_init(&pTS->List);
@@ -524,20 +475,16 @@ void RemovePeerTS(struct ieee80211_device *ieee, u8 *Addr)
 		}
 	}
 
-	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Pending_List, List)
-	{
-		if (memcmp(pTS->Addr, Addr, 6) == 0)
-		{
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Pending_List, List) {
+		if (memcmp(pTS->Addr, Addr, 6) == 0) {
 			RemoveTsEntry(ieee, pTS, RX_DIR);
 			list_del_init(&pTS->List);
 			list_add_tail(&pTS->List, &ieee->Rx_TS_Unused_List);
 		}
 	}
 
-	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Admit_List, List)
-	{
-		if (memcmp(pTS->Addr, Addr, 6) == 0)
-		{
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Admit_List, List) {
+		if (memcmp(pTS->Addr, Addr, 6) == 0) {
 			RemoveTsEntry(ieee, pTS, RX_DIR);
 			list_del_init(&pTS->List);
 			list_add_tail(&pTS->List, &ieee->Rx_TS_Unused_List);
@@ -549,29 +496,25 @@ void RemoveAllTS(struct ieee80211_device *ieee)
 {
 	PTS_COMMON_INFO pTS, pTmpTS;
 
-	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Pending_List, List)
-	{
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Pending_List, List) {
 		RemoveTsEntry(ieee, pTS, TX_DIR);
 		list_del_init(&pTS->List);
 		list_add_tail(&pTS->List, &ieee->Tx_TS_Unused_List);
 	}
 
-	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Admit_List, List)
-	{
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Admit_List, List) {
 		RemoveTsEntry(ieee, pTS, TX_DIR);
 		list_del_init(&pTS->List);
 		list_add_tail(&pTS->List, &ieee->Tx_TS_Unused_List);
 	}
 
-	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Pending_List, List)
-	{
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Pending_List, List) {
 		RemoveTsEntry(ieee, pTS, RX_DIR);
 		list_del_init(&pTS->List);
 		list_add_tail(&pTS->List, &ieee->Rx_TS_Unused_List);
 	}
 
-	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Admit_List, List)
-	{
+	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Rx_TS_Admit_List, List) {
 		RemoveTsEntry(ieee, pTS, RX_DIR);
 		list_del_init(&pTS->List);
 		list_add_tail(&pTS->List, &ieee->Rx_TS_Unused_List);
@@ -580,21 +523,17 @@ void RemoveAllTS(struct ieee80211_device *ieee)
 
 void TsStartAddBaProcess(struct ieee80211_device *ieee, PTX_TS_RECORD	pTxTS)
 {
-	if(!pTxTS->bAddBaReqInProgress)
-	{
+	if(!pTxTS->bAddBaReqInProgress) {
 		pTxTS->bAddBaReqInProgress = true;
-		if(pTxTS->bAddBaReqDelayed)
-		{
+		if(pTxTS->bAddBaReqDelayed)	{
 			IEEE80211_DEBUG(IEEE80211_DL_BA, "TsStartAddBaProcess(): Delayed Start ADDBA after 60 sec!!\n");
 			mod_timer(&pTxTS->TsAddBaTimer,
 				  jiffies + msecs_to_jiffies(TS_ADDBA_DELAY));
-		}
-		else
-		{
+		} else {
 			IEEE80211_DEBUG(IEEE80211_DL_BA,"TsStartAddBaProcess(): Immediately Start ADDBA now!!\n");
 			mod_timer(&pTxTS->TsAddBaTimer, jiffies+10); //set 10 ticks
 		}
-	}
-	else
+	} else {
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "%s()==>BA timer is already added\n", __func__);
+	}
 }
diff --git a/drivers/staging/rtl8192u/r8192U.h b/drivers/staging/rtl8192u/r8192U.h
index e702afb5a70e..51c150a39fc2 100644
--- a/drivers/staging/rtl8192u/r8192U.h
+++ b/drivers/staging/rtl8192u/r8192U.h
@@ -34,7 +34,7 @@
 #include <linux/proc_fs.h>
 #include <linux/if_arp.h>
 #include <linux/random.h>
-#include <asm/io.h>
+#include <linux/io.h>
 #include "ieee80211/ieee80211.h"
 
 #define RTL8192U
@@ -1147,9 +1147,9 @@ int write_nic_word(struct net_device *dev, int x, u16 y);
 int write_nic_dword(struct net_device *dev, int x, u32 y);
 void force_pci_posting(struct net_device *dev);
 
-void rtl8192_rtx_disable(struct net_device *);
-void rtl8192_rx_enable(struct net_device *);
-void rtl8192_tx_enable(struct net_device *);
+void rtl8192_rtx_disable(struct net_device *dev);
+void rtl8192_rx_enable(struct net_device *dev);
+void rtl8192_tx_enable(struct net_device *dev);
 
 void rtl8192_disassociate(struct net_device *dev);
 void rtl8185_set_rf_pins_enable(struct net_device *dev, u32 a);
diff --git a/drivers/staging/rtl8192u/r8192U_core.c b/drivers/staging/rtl8192u/r8192U_core.c
index 9f370e8d84d3..779ecdbc4e17 100644
--- a/drivers/staging/rtl8192u/r8192U_core.c
+++ b/drivers/staging/rtl8192u/r8192U_core.c
@@ -1797,8 +1797,8 @@ static void rtl8192_link_change(struct net_device *dev)
 		 * way, but there is no chance to set this as wep will not set
 		 * group key in wext.
 		 */
-		if (KEY_TYPE_WEP40 == ieee->pairwise_key_type ||
-		    KEY_TYPE_WEP104 == ieee->pairwise_key_type)
+		if (ieee->pairwise_key_type == KEY_TYPE_WEP40 ||
+		    ieee->pairwise_key_type == KEY_TYPE_WEP104)
 			EnableHWSecurityConfig8192(dev);
 	}
 	/*update timing params*/
@@ -2071,7 +2071,7 @@ static bool GetNmodeSupportBySecCfg8192(struct net_device *dev)
 	 */
 	encrypt = (network->capability & WLAN_CAPABILITY_PRIVACY) ||
 		  (ieee->host_encrypt && crypt && crypt->ops &&
-		   (0 == strcmp(crypt->ops->name, "WEP")));
+		   (strcmp(crypt->ops->name, "WEP") == 0));
 
 	/* simply judge  */
 	if (encrypt && (wpa_ie_len == 0)) {
@@ -4498,7 +4498,7 @@ static void TranslateRxSignalStuff819xUsb(struct sk_buff *skb,
 	praddr = hdr->addr1;
 
 	/* Check if the received packet is acceptable. */
-	bpacket_match_bssid = (IEEE80211_FTYPE_CTL != type) &&
+	bpacket_match_bssid = (type != IEEE80211_FTYPE_CTL) &&
 			       (eqMacAddr(priv->ieee80211->current_network.bssid,  (fc & IEEE80211_FCTL_TODS) ? hdr->addr1 : (fc & IEEE80211_FCTL_FROMDS) ? hdr->addr2 : hdr->addr3))
 			       && (!pstats->bHwError) && (!pstats->bCRC) && (!pstats->bICV);
 	bpacket_toself =  bpacket_match_bssid &
@@ -5098,7 +5098,7 @@ void EnableHWSecurityConfig8192(struct net_device *dev)
 	struct ieee80211_device *ieee = priv->ieee80211;
 
 	SECR_value = SCR_TxEncEnable | SCR_RxDecEnable;
-	if (((KEY_TYPE_WEP40 == ieee->pairwise_key_type) || (KEY_TYPE_WEP104 == ieee->pairwise_key_type)) && (priv->ieee80211->auth_mode != 2)) {
+	if (((ieee->pairwise_key_type == KEY_TYPE_WEP40) || (ieee->pairwise_key_type == KEY_TYPE_WEP104)) && (priv->ieee80211->auth_mode != 2)) {
 		SECR_value |= SCR_RxUseDK;
 		SECR_value |= SCR_TxUseDK;
 	} else if ((ieee->iw_mode == IW_MODE_ADHOC) && (ieee->pairwise_key_type & (KEY_TYPE_CCMP | KEY_TYPE_TKIP))) {
diff --git a/drivers/staging/rtl8192u/r8192U_dm.c b/drivers/staging/rtl8192u/r8192U_dm.c
index 975f707827e1..e6f8d1da65d9 100644
--- a/drivers/staging/rtl8192u/r8192U_dm.c
+++ b/drivers/staging/rtl8192u/r8192U_dm.c
@@ -2300,43 +2300,52 @@ static void dm_check_edca_turbo(
 		 * Restore original EDCA according to the declaration of AP.
 		 */
 		if (priv->bcurrent_turbo_EDCA) {
+			u8	u1bAIFS;
+			u32	u4bAcParam, op_limit, cw_max, cw_min;
+
+			struct ieee80211_qos_parameters *qos_parameters = &priv->ieee80211->current_network.qos_data.parameters;
+			u8 mode = priv->ieee80211->mode;
+
+			/*  For Each time updating EDCA parameter, reset EDCA turbo mode status. */
+			dm_init_edca_turbo(dev);
+
+			u1bAIFS = qos_parameters->aifs[0] * ((mode & (IEEE_G | IEEE_N_24G)) ? 9 : 20) + aSifsTime;
+
+			op_limit = (u32)le16_to_cpu(qos_parameters->tx_op_limit[0]);
+			cw_max   = (u32)le16_to_cpu(qos_parameters->cw_max[0]);
+			cw_min   = (u32)le16_to_cpu(qos_parameters->cw_min[0]);
+
+			op_limit <<= AC_PARAM_TXOP_LIMIT_OFFSET;
+			cw_max   <<= AC_PARAM_ECW_MAX_OFFSET;
+			cw_min   <<= AC_PARAM_ECW_MIN_OFFSET;
+			u1bAIFS  <<= AC_PARAM_AIFS_OFFSET;
+
+			u4bAcParam = op_limit | cw_max | cw_min | u1bAIFS;
+			cpu_to_le32s(&u4bAcParam);
+
+			write_nic_dword(dev, EDCAPARA_BE, u4bAcParam);
+
+
+			/*
+			 * Check ACM bit.
+			 * If it is set, immediately set ACM control bit to downgrading AC for passing WMM testplan. Annie, 2005-12-13.
+			 */
 			{
-				u8		u1bAIFS;
-				u32		u4bAcParam;
-				struct ieee80211_qos_parameters *qos_parameters = &priv->ieee80211->current_network.qos_data.parameters;
-				u8 mode = priv->ieee80211->mode;
-
-				/*  For Each time updating EDCA parameter, reset EDCA turbo mode status. */
-				dm_init_edca_turbo(dev);
-				u1bAIFS = qos_parameters->aifs[0] * ((mode&(IEEE_G|IEEE_N_24G)) ? 9 : 20) + aSifsTime;
-				u4bAcParam = (((le16_to_cpu(qos_parameters->tx_op_limit[0])) << AC_PARAM_TXOP_LIMIT_OFFSET)|
-					((le16_to_cpu(qos_parameters->cw_max[0])) << AC_PARAM_ECW_MAX_OFFSET)|
-					((le16_to_cpu(qos_parameters->cw_min[0])) << AC_PARAM_ECW_MIN_OFFSET)|
-					((u32)u1bAIFS << AC_PARAM_AIFS_OFFSET));
-				/*write_nic_dword(dev, WDCAPARA_ADD[i], u4bAcParam);*/
-				write_nic_dword(dev, EDCAPARA_BE,  u4bAcParam);
-
-				/*
-				 * Check ACM bit.
-				 * If it is set, immediately set ACM control bit to downgrading AC for passing WMM testplan. Annie, 2005-12-13.
-				 */
-				{
-					/*  TODO:  Modified this part and try to set acm control in only 1 IO processing!! */
-
-					PACI_AIFSN	pAciAifsn = (PACI_AIFSN)&(qos_parameters->aifs[0]);
-					u8		AcmCtrl;
-
-					read_nic_byte(dev, AcmHwCtrl, &AcmCtrl);
-
-					if (pAciAifsn->f.ACM) { /*  ACM bit is 1. */
-						AcmCtrl |= AcmHw_BeqEn;
-					} else {	/* ACM bit is 0. */
-						AcmCtrl &= (~AcmHw_BeqEn);
-					}
+				/*  TODO:  Modified this part and try to set acm control in only 1 IO processing!! */
 
-					RT_TRACE(COMP_QOS, "SetHwReg8190pci(): [HW_VAR_ACM_CTRL] Write 0x%X\n", AcmCtrl);
-					write_nic_byte(dev, AcmHwCtrl, AcmCtrl);
+				PACI_AIFSN	pAciAifsn = (PACI_AIFSN)&(qos_parameters->aifs[0]);
+				u8		AcmCtrl;
+
+				read_nic_byte(dev, AcmHwCtrl, &AcmCtrl);
+
+				if (pAciAifsn->f.ACM) { /*  ACM bit is 1. */
+					AcmCtrl |= AcmHw_BeqEn;
+				} else {	/* ACM bit is 0. */
+					AcmCtrl &= (~AcmHw_BeqEn);
 				}
+
+				RT_TRACE(COMP_QOS, "SetHwReg8190pci(): [HW_VAR_ACM_CTRL] Write 0x%X\n", AcmCtrl);
+				write_nic_byte(dev, AcmHwCtrl, AcmCtrl);
 			}
 			priv->bcurrent_turbo_EDCA = false;
 		}
diff --git a/drivers/staging/rtl8192u/r819xU_cmdpkt.c b/drivers/staging/rtl8192u/r819xU_cmdpkt.c
index bb6d8bd6c7ac..87ab3ba760fc 100644
--- a/drivers/staging/rtl8192u/r819xU_cmdpkt.c
+++ b/drivers/staging/rtl8192u/r819xU_cmdpkt.c
@@ -31,7 +31,6 @@ rt_status SendTxCommandPacket(struct net_device *dev, void *pData, u32 DataLen)
 	struct r8192_priv   *priv = ieee80211_priv(dev);
 	struct sk_buff	    *skb;
 	struct cb_desc	    *tcb_desc;
-	unsigned char	    *ptr_buf;
 
 	/* Get TCB and local buffer from common pool.
 	 * (It is shared by CmdQ, MgntQ, and USB coalesce DataQ)
@@ -45,8 +44,7 @@ rt_status SendTxCommandPacket(struct net_device *dev, void *pData, u32 DataLen)
 	tcb_desc->bCmdOrInit = DESC_PACKET_TYPE_NORMAL;
 	tcb_desc->bLastIniPkt = 0;
 	skb_reserve(skb, USB_HWDESC_HEADER_LEN);
-	ptr_buf = skb_put(skb, DataLen);
-	memcpy(ptr_buf, pData, DataLen);
+	skb_put_data(skb, pData, DataLen);
 	tcb_desc->txbuf_size = (u16)DataLen;
 
 	if (!priv->ieee80211->check_nic_enough_desc(dev, tcb_desc->queue_index) ||
diff --git a/drivers/staging/rtl8712/ieee80211.c b/drivers/staging/rtl8712/ieee80211.c
index f35121eedac6..33e82a9dd462 100644
--- a/drivers/staging/rtl8712/ieee80211.c
+++ b/drivers/staging/rtl8712/ieee80211.c
@@ -166,7 +166,7 @@ static uint r8712_get_rateset_len(u8 *rateset)
 
 int r8712_generate_ie(struct registry_priv *pregistrypriv)
 {
-	int sz = 0, rateLen;
+	int sz = 0, rate_len;
 	struct wlan_bssid_ex *pdev_network = &pregistrypriv->dev_network;
 	u8 *ie = pdev_network->IEs;
 
@@ -191,15 +191,16 @@ int r8712_generate_ie(struct registry_priv *pregistrypriv)
 			  pdev_network->Ssid.Ssid, &sz);
 	/*supported rates*/
 	set_supported_rate(pdev_network->rates, pregistrypriv->wireless_mode);
-	rateLen = r8712_get_rateset_len(pdev_network->rates);
-	if (rateLen > 8) {
+	rate_len = r8712_get_rateset_len(pdev_network->rates);
+	if (rate_len > 8) {
 		ie = r8712_set_ie(ie, _SUPPORTEDRATES_IE_, 8,
 				  pdev_network->rates, &sz);
-		ie = r8712_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8),
+		ie = r8712_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8),
 				  (pdev_network->rates + 8), &sz);
-	} else
+	} else {
 		ie = r8712_set_ie(ie, _SUPPORTEDRATES_IE_,
-				  rateLen, pdev_network->rates, &sz);
+				  rate_len, pdev_network->rates, &sz);
+	}
 	/*DS parameter set*/
 	ie = r8712_set_ie(ie, _DSSET_IE_, 1,
 			  (u8 *)&pdev_network->Configuration.DSConfig, &sz);
diff --git a/drivers/staging/rtl8712/os_intfs.c b/drivers/staging/rtl8712/os_intfs.c
index 8836b31b4ef8..e698f6ede449 100644
--- a/drivers/staging/rtl8712/os_intfs.c
+++ b/drivers/staging/rtl8712/os_intfs.c
@@ -93,7 +93,7 @@ static char *initmac;
  */
 static int wifi_test;
 
-module_param_string(ifname, ifname, sizeof(ifname), S_IRUGO | S_IWUSR);
+module_param_string(ifname, ifname, sizeof(ifname), 0644);
 module_param(wifi_test, int, 0644);
 module_param(initmac, charp, 0644);
 module_param(video_mode, int, 0644);
diff --git a/drivers/staging/rtl8712/rtl8712_recv.c b/drivers/staging/rtl8712/rtl8712_recv.c
index 266ffefd55ed..ea3eb94b28b3 100644
--- a/drivers/staging/rtl8712/rtl8712_recv.c
+++ b/drivers/staging/rtl8712/rtl8712_recv.c
@@ -340,7 +340,7 @@ static int amsdu_to_msdu(struct _adapter *padapter, union recv_frame *prframe)
 	int	a_len, padding_len;
 	u16	eth_type, nSubframe_Length;
 	u8	nr_subframes, i;
-	unsigned char *data_ptr, *pdata;
+	unsigned char *pdata;
 	struct rx_pkt_attrib *pattrib;
 	_pkt *sub_skb, *subframes[MAX_SUBFRAME_COUNT];
 	struct recv_priv *precvpriv = &padapter->recvpriv;
@@ -372,8 +372,7 @@ static int amsdu_to_msdu(struct _adapter *padapter, union recv_frame *prframe)
 		if (!sub_skb)
 			break;
 		skb_reserve(sub_skb, 12);
-		data_ptr = (u8 *)skb_put(sub_skb, nSubframe_Length);
-		memcpy(data_ptr, pdata, nSubframe_Length);
+		skb_put_data(sub_skb, pdata, nSubframe_Length);
 		subframes[nr_subframes++] = sub_skb;
 		if (nr_subframes >= MAX_SUBFRAME_COUNT) {
 			netdev_warn(padapter->pnetdev, "r8712u: ParseSubframe(): Too many Subframes! Packets dropped!\n");
diff --git a/drivers/staging/rtl8712/wifi.h b/drivers/staging/rtl8712/wifi.h
index 556367bfbe8a..0ed2f44ab4e9 100644
--- a/drivers/staging/rtl8712/wifi.h
+++ b/drivers/staging/rtl8712/wifi.h
@@ -170,8 +170,10 @@ enum WIFI_REG_DOMAIN {
 	*(__le16 *)(pbuf) &= (~cpu_to_le16(_FROM_DS_)); \
 })
 
-#define get_tofr_ds(pframe)	((GetToDs(pframe) << 1) | GetFrDs(pframe))
-
+static inline unsigned char get_tofr_ds(unsigned char *pframe)
+{
+	return ((GetToDs(pframe) << 1) | GetFrDs(pframe));
+}
 
 #define SetMFrag(pbuf) ({ \
 	*(__le16 *)(pbuf) |= cpu_to_le16(_MORE_FRAG_); \
diff --git a/drivers/staging/rtl8723bs/core/rtw_mlme.c b/drivers/staging/rtl8723bs/core/rtw_mlme.c
index 9e355734f0c0..d5ab12305e59 100644
--- a/drivers/staging/rtl8723bs/core/rtw_mlme.c
+++ b/drivers/staging/rtl8723bs/core/rtw_mlme.c
@@ -249,7 +249,7 @@ void _rtw_free_network_nolock(struct	mlme_priv *pmlmepriv, struct wlan_network *
 /*
 	return the wlan_network with the matching addr
 
-	Shall be calle under atomic context... to avoid possible racing condition...
+	Shall be called under atomic context... to avoid possible racing condition...
 */
 struct wlan_network *_rtw_find_network(struct __queue *scanned_queue, u8 *addr)
 {
@@ -412,7 +412,7 @@ void rtw_free_network_queue(struct adapter *dev, u8 isfreeall)
 /*
 	return the wlan_network with the matching addr
 
-	Shall be calle under atomic context... to avoid possible racing condition...
+	Shall be called under atomic context... to avoid possible racing condition...
 */
 struct	wlan_network *rtw_find_network(struct __queue *scanned_queue, u8 *addr)
 {
@@ -564,7 +564,7 @@ void update_network(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
 			sq_final = ((u32)(src->PhyInfo.SignalQuality)+(u32)(dst->PhyInfo.SignalQuality)*4)/5;
 			rssi_final = (src->Rssi+dst->Rssi*4)/5;
 		} else {
-			/* bss info not receving from the right channel, use the original RX signal infos */
+			/* bss info not receiving from the right channel, use the original RX signal infos */
 			ss_final = dst->PhyInfo.SignalStrength;
 			sq_final = dst->PhyInfo.SignalQuality;
 			rssi_final = dst->Rssi;
@@ -680,7 +680,7 @@ void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *t
 			pnetwork->aid = 0;
 			pnetwork->join_res = 0;
 
-			/* bss info not receving from the right channel */
+			/* bss info not receiving from the right channel */
 			if (pnetwork->network.PhyInfo.SignalQuality == 101)
 				pnetwork->network.PhyInfo.SignalQuality = 0;
 		} else {
@@ -699,7 +699,7 @@ void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *t
 
 			pnetwork->last_scanned = jiffies;
 
-			/* bss info not receving from the right channel */
+			/* bss info not receiving from the right channel */
 			if (pnetwork->network.PhyInfo.SignalQuality == 101)
 				pnetwork->network.PhyInfo.SignalQuality = 0;
 
@@ -715,7 +715,7 @@ void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *t
 
 		pnetwork->last_scanned = jiffies;
 
-		/* target.Reserved[0]== 1, means that scaned network is a bcn frame. */
+		/* target.Reserved[0]== 1, means that scanned network is a bcn frame. */
 		if ((pnetwork->network.IELength > target->IELength) && (target->Reserved[0] == 1))
 			update_ie = false;
 
@@ -1264,7 +1264,7 @@ static struct sta_info *rtw_joinbss_update_stainfo(struct adapter *padapter, str
 
 		/* 	Commented by Albert 2012/07/21 */
 		/* 	When doing the WPS, the wps_ie_len won't equal to 0 */
-		/* 	And the Wi-Fi driver shouldn't allow the data packet to be tramsmitted. */
+		/* 	And the Wi-Fi driver shouldn't allow the data packet to be transmitted. */
 		if (padapter->securitypriv.wps_ie_len != 0) {
 			psta->ieee8021x_blocked = true;
 			padapter->securitypriv.wps_ie_len = 0;
@@ -1272,7 +1272,7 @@ static struct sta_info *rtw_joinbss_update_stainfo(struct adapter *padapter, str
 
 
 		/* for A-MPDU Rx reordering buffer control for bmc_sta & sta_info */
-		/* if A-MPDU Rx is enabled, reseting  rx_ordering_ctrl wstart_b(indicate_seq) to default value = 0xffff */
+		/* if A-MPDU Rx is enabled, resetting  rx_ordering_ctrl wstart_b(indicate_seq) to default value = 0xffff */
 		/* todo: check if AP can send A-MPDU packets */
 		for (i = 0; i < 16 ; i++) {
 			/* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
@@ -1374,7 +1374,7 @@ static void rtw_joinbss_update_network(struct adapter *padapter, struct wlan_net
 	rtw_update_ht_cap(padapter, cur_network->network.IEs, cur_network->network.IELength, (u8) cur_network->network.Configuration.DSConfig);
 }
 
-/* Notes: the fucntion could be > passive_level (the same context as Rx tasklet) */
+/* Notes: the function could be > passive_level (the same context as Rx tasklet) */
 /* pnetwork : returns from rtw_joinbss_event_callback */
 /* ptarget_wlan: found from scanned_queue */
 /* if join_res > 0, for (fw_state ==WIFI_STATION_STATE), we check if  "ptarget_sta" & "ptarget_wlan" exist. */
@@ -1482,10 +1482,10 @@ void rtw_joinbss_event_prehandle(struct adapter *adapter, u8 *pbuf)
 			}
 
 
-			/* s5. Cancle assoc_timer */
+			/* s5. Cancel assoc_timer */
 			_cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
 
-			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("Cancle assoc_timer\n"));
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("Cancel assoc_timer\n"));
 
 		} else{
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_joinbss_event_callback err: fw_state:%x", get_fwstate(pmlmepriv)));
@@ -1817,7 +1817,7 @@ void rtw_wmm_event_callback(struct adapter *padapter, u8 *pbuf)
 }
 
 /*
-* _rtw_join_timeout_handler - Timeout/faliure handler for CMD JoinBss
+* _rtw_join_timeout_handler - Timeout/failure handler for CMD JoinBss
 * @adapter: pointer to struct adapter structure
 */
 void _rtw_join_timeout_handler (struct adapter *adapter)
@@ -1870,7 +1870,7 @@ void _rtw_join_timeout_handler (struct adapter *adapter)
 }
 
 /*
-* rtw_scan_timeout_handler - Timeout/Faliure handler for CMD SiteSurvey
+* rtw_scan_timeout_handler - Timeout/Failure handler for CMD SiteSurvey
 * @adapter: pointer to struct adapter structure
 */
 void rtw_scan_timeout_handler (struct adapter *adapter)
@@ -2622,7 +2622,7 @@ void rtw_get_encrypt_decrypt_from_registrypriv(struct adapter *adapter)
 {
 }
 
-/* the fucntion is at passive_level */
+/* the function is at passive_level */
 void rtw_joinbss_reset(struct adapter *padapter)
 {
 	u8 threshold;
@@ -2727,7 +2727,7 @@ void rtw_build_wmm_ie_ht(struct adapter *padapter, u8 *out_ie, uint *pout_len)
 	}
 }
 
-/* the fucntion is >= passive_level */
+/* the function is >= passive_level */
 unsigned int rtw_restructure_ht_ie(struct adapter *padapter, u8 *in_ie, u8 *out_ie, uint in_len, uint *pout_len, u8 channel)
 {
 	u32 ielen, out_len;
@@ -2879,7 +2879,7 @@ unsigned int rtw_restructure_ht_ie(struct adapter *padapter, u8 *in_ie, u8 *out_
 
 }
 
-/* the fucntion is > passive_level (in critical_section) */
+/* the function is > passive_level (in critical_section) */
 void rtw_update_ht_cap(struct adapter *padapter, u8 *pie, uint ie_len, u8 channel)
 {
 	u8 *p, max_ampdu_sz;
diff --git a/drivers/staging/rtl8723bs/hal/HalBtc8723b1Ant.c b/drivers/staging/rtl8723bs/hal/HalBtc8723b1Ant.c
index 86040adb436c..37f42bfc55ed 100644
--- a/drivers/staging/rtl8723bs/hal/HalBtc8723b1Ant.c
+++ b/drivers/staging/rtl8723bs/hal/HalBtc8723b1Ant.c
@@ -404,7 +404,7 @@ static void halbtc8723b1ant_MonitorWiFiCtr(PBTC_COEXIST pBtCoexist)
 {
 	s32	wifiRssi = 0;
 	bool bWifiBusy = false, bWifiUnderBMode = false;
-	static u8 nCCKLockCounter = 0;
+	static u8 nCCKLockCounter;
 
 	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
 	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
@@ -488,7 +488,7 @@ static void halbtc8723b1ant_MonitorWiFiCtr(PBTC_COEXIST pBtCoexist)
 
 static bool halbtc8723b1ant_IsWifiStatusChanged(PBTC_COEXIST pBtCoexist)
 {
-	static bool	bPreWifiBusy = false, bPreUnder4way = false, bPreBtHsOn = false;
+	static bool	bPreWifiBusy, bPreUnder4way, bPreBtHsOn;
 	bool bWifiBusy = false, bUnder4way = false, bBtHsOn = false;
 	bool bWifiConnected = false;
 
@@ -2754,7 +2754,7 @@ void EXhalbtc8723b1ant_DisplayCoexInfo(PBTC_COEXIST pBtCoexist)
 	u32 wifiBw, wifiTrafficDir, faOfdm, faCck, wifiLinkStatus;
 	u8 wifiDot11Chnl, wifiHsChnl;
 	u32 fwVer = 0, btPatchVer = 0;
-	static u8 PopReportIn10s = 0;
+	static u8 PopReportIn10s;
 
 	CL_SPRINTF(
 		cliBuf,
@@ -3751,7 +3751,7 @@ void EXhalbtc8723b1ant_PnpNotify(PBTC_COEXIST pBtCoexist, u8 pnpState)
 
 void EXhalbtc8723b1ant_Periodical(PBTC_COEXIST pBtCoexist)
 {
-	static u8 disVerInfoCnt = 0;
+	static u8 disVerInfoCnt;
 	u32 fwVer = 0, btPatchVer = 0;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ==========================Periodical ===========================\n"));
diff --git a/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.c b/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.c
index f5bc511d02f2..33610d39333f 100644
--- a/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.c
+++ b/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.c
@@ -282,7 +282,7 @@ static void halbtc8723b2ant_QueryBtInfo(PBTC_COEXIST pBtCoexist)
 
 static bool halbtc8723b2ant_IsWifiStatusChanged(PBTC_COEXIST pBtCoexist)
 {
-	static bool	bPreWifiBusy = false, bPreUnder4way = false, bPreBtHsOn = false;
+	static bool	bPreWifiBusy, bPreUnder4way, bPreBtHsOn;
 	bool bWifiBusy = false, bUnder4way = false, bBtHsOn = false;
 	bool bWifiConnected = false;
 
@@ -3706,7 +3706,7 @@ void EXhalbtc8723b2ant_PnpNotify(PBTC_COEXIST pBtCoexist, u8 pnpState)
 
 void EXhalbtc8723b2ant_Periodical(PBTC_COEXIST pBtCoexist)
 {
-	static u8 disVerInfoCnt = 0;
+	static u8 disVerInfoCnt;
 	u32 fwVer = 0, btPatchVer = 0;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ==========================Periodical ===========================\n"));
diff --git a/drivers/staging/rtl8723bs/hal/hal_btcoex.c b/drivers/staging/rtl8723bs/hal/hal_btcoex.c
index cc7e0903ee9d..9e08a4de4895 100644
--- a/drivers/staging/rtl8723bs/hal/hal_btcoex.c
+++ b/drivers/staging/rtl8723bs/hal/hal_btcoex.c
@@ -385,7 +385,7 @@ static s32 halbtcoutsrc_GetWifiRssi(struct adapter *padapter)
 static u8 halbtcoutsrc_GetWifiScanAPNum(struct adapter *padapter)
 {
 	struct mlme_ext_priv *pmlmeext;
-	static u8 scan_AP_num = 0;
+	static u8 scan_AP_num;
 
 	pmlmeext = &padapter->mlmeextpriv;
 
diff --git a/drivers/staging/rtl8723bs/hal/hal_com.c b/drivers/staging/rtl8723bs/hal/hal_com.c
index 1880d4140bee..e3a98322f475 100644
--- a/drivers/staging/rtl8723bs/hal/hal_com.c
+++ b/drivers/staging/rtl8723bs/hal/hal_com.c
@@ -26,7 +26,7 @@ u8 rtw_hal_data_init(struct adapter *padapter)
 		padapter->hal_data_sz = sizeof(struct hal_com_data);
 		padapter->HalData = vzalloc(padapter->hal_data_sz);
 		if (padapter->HalData == NULL) {
-			DBG_8192C("cant not alloc memory for HAL DATA\n");
+			DBG_8192C("cannot alloc memory for HAL DATA\n");
 			return _FAIL;
 		}
 	}
@@ -1415,7 +1415,8 @@ bool GetHexValueFromString(char *szStr, u32 *pu4bVal, u32 *pu4bMove)
 
 	/*  Check input parameter. */
 	if (szStr == NULL || pu4bVal == NULL || pu4bMove == NULL) {
-		DBG_871X("GetHexValueFromString(): Invalid inpur argumetns! szStr: %p, pu4bVal: %p, pu4bMove: %p\n", szStr, pu4bVal, pu4bMove);
+		DBG_871X("GetHexValueFromString(): Invalid input arguments! szStr: %p, pu4bVal: %p, pu4bMove: %p\n",
+			 szStr, pu4bVal, pu4bMove);
 		return false;
 	}
 
diff --git a/drivers/staging/rtl8723bs/hal/odm.h b/drivers/staging/rtl8723bs/hal/odm.h
index 0b3541a91548..87a76bafecb3 100644
--- a/drivers/staging/rtl8723bs/hal/odm.h
+++ b/drivers/staging/rtl8723bs/hal/odm.h
@@ -209,7 +209,10 @@ typedef struct _ODM_RATE_ADAPTIVE {
 
 #define AVG_THERMAL_NUM		8
 #define IQK_Matrix_REG_NUM	8
-#define IQK_Matrix_Settings_NUM	14+24+21 /*  Channels_2_4G_NUM + Channels_5G_20M_NUM + Channels_5G */
+#define IQK_Matrix_Settings_NUM	(14 + 24 + 21) /*   Channels_2_4G_NUM
+						* + Channels_5G_20M_NUM
+						* + Channels_5G
+						*/
 
 #define		DM_Type_ByFW			0
 #define		DM_Type_ByDriver		1
diff --git a/drivers/staging/rtl8723bs/hal/odm_DIG.c b/drivers/staging/rtl8723bs/hal/odm_DIG.c
index ba8e8eb534ef..0bde9444471d 100644
--- a/drivers/staging/rtl8723bs/hal/odm_DIG.c
+++ b/drivers/staging/rtl8723bs/hal/odm_DIG.c
@@ -278,11 +278,11 @@ void odm_Adaptivity(void *pDM_VOID, u8 IGI)
 
 	if (!pDM_Odm->ForceEDCCA) {
 		if (pDM_Odm->RSSI_Min > pDM_Odm->AdapEn_RSSI)
-			EDCCA_State = 1;
+			EDCCA_State = true;
 		else if (pDM_Odm->RSSI_Min < (pDM_Odm->AdapEn_RSSI - 5))
-			EDCCA_State = 0;
+			EDCCA_State = false;
 	} else
-		EDCCA_State = 1;
+		EDCCA_State = true;
 
 	if (
 		pDM_Odm->bLinked &&
@@ -305,7 +305,7 @@ void odm_Adaptivity(void *pDM_VOID, u8 IGI)
 		)
 	);
 
-	if (EDCCA_State == 1) {
+	if (EDCCA_State) {
 		Diff = IGI_target-(s8)IGI;
 		TH_L2H_dmc = pDM_Odm->TH_L2H_ini + Diff;
 		if (TH_L2H_dmc > 10)
@@ -372,7 +372,7 @@ void odm_PauseDIG(
 {
 	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
 	pDIG_T pDM_DigTable = &pDM_Odm->DM_DigTable;
-	static bool bPaused = false;
+	static bool bPaused;
 
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG() =========>\n"));
 
diff --git a/drivers/staging/rtl8723bs/hal/odm_debug.h b/drivers/staging/rtl8723bs/hal/odm_debug.h
index 2ec4baf57464..ff131361248c 100644
--- a/drivers/staging/rtl8723bs/hal/odm_debug.h
+++ b/drivers/staging/rtl8723bs/hal/odm_debug.h
@@ -105,51 +105,60 @@
 
 #if DBG
 #define ODM_RT_TRACE(pDM_Odm, comp, level, fmt)\
-	if (\
-		(comp & pDM_Odm->DebugComponents) &&\
-		(level <= pDM_Odm->DebugLevel || level == ODM_DBG_SERIOUS)\
-	) {\
-		RT_PRINTK fmt;\
-	}
+	do {\
+		if (\
+			(comp & pDM_Odm->DebugComponents) &&\
+			(level <= pDM_Odm->DebugLevel ||\
+			 level == ODM_DBG_SERIOUS)\
+		) {\
+			RT_PRINTK fmt;\
+		} \
+	} while (0)
 
 #define ODM_RT_TRACE_F(pDM_Odm, comp, level, fmt)\
-	if (\
-		(comp & pDM_Odm->DebugComponents) &&\
-		(level <= pDM_Odm->DebugLevel)\
-	) {\
-		RT_PRINTK fmt;\
-	}
+	do {\
+		if (\
+			(comp & pDM_Odm->DebugComponents) &&\
+			(level <= pDM_Odm->DebugLevel)\
+		) {\
+			RT_PRINTK fmt;\
+		} \
+	} while (0)
 
 #define ODM_RT_ASSERT(pDM_Odm, expr, fmt)\
-	if (!expr) {\
-		DbgPrint("Assertion failed! %s at ......\n", #expr);\
-		DbgPrint(\
-			"      ......%s,%s, line =%d\n",\
-			__FILE__,\
-			__func__,\
-			__LINE__\
-		);\
-		RT_PRINTK fmt;\
-		ASSERT(false);\
-	}
+	do {\
+		if (!expr) {\
+			DbgPrint("Assertion failed! %s at ......\n", #expr);\
+			DbgPrint(\
+				"      ......%s,%s, line =%d\n",\
+				__FILE__,\
+				__func__,\
+				__LINE__\
+			);\
+			RT_PRINTK fmt;\
+			ASSERT(false);\
+		} \
+	} while (0)
 #define ODM_dbg_enter() { DbgPrint("==> %s\n", __func__); }
 #define ODM_dbg_exit() { DbgPrint("<== %s\n", __func__); }
 #define ODM_dbg_trace(str) { DbgPrint("%s:%s\n", __func__, str); }
 
 #define ODM_PRINT_ADDR(pDM_Odm, comp, level, title_str, ptr)\
-	if (\
-		(comp & pDM_Odm->DebugComponents) &&\
-		(level <= pDM_Odm->DebugLevel)\
-	) {\
-		int __i;\
-		u8 *__ptr = (u8 *)ptr;\
-		DbgPrint("[ODM] ");\
-		DbgPrint(title_str);\
-		DbgPrint(" ");\
-		for (__i = 0; __i < 6; __i++)\
-			DbgPrint("%02X%s", __ptr[__i], (__i == 5) ? "" : "-");\
-		DbgPrint("\n");\
-	}
+	do {\
+		if (\
+			(comp & pDM_Odm->DebugComponents) &&\
+			(level <= pDM_Odm->DebugLevel)\
+		) {\
+			int __i;\
+			u8 *__ptr = (u8 *)ptr;\
+			DbgPrint("[ODM] ");\
+			DbgPrint(title_str);\
+			DbgPrint(" ");\
+			for (__i = 0; __i < 6; __i++)\
+				DbgPrint("%02X%s", __ptr[__i], (__i == 5) ? "" : "-");\
+			DbgPrint("\n");\
+		} \
+	} while (0)
 #else
 #define ODM_RT_TRACE(pDM_Odm, comp, level, fmt)		no_printk fmt
 #define ODM_RT_TRACE_F(pDM_Odm, comp, level, fmt)	no_printk fmt
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723b_hal_init.c b/drivers/staging/rtl8723bs/hal/rtl8723b_hal_init.c
index 163537faefd9..84a89ef74169 100644
--- a/drivers/staging/rtl8723bs/hal/rtl8723b_hal_init.c
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_hal_init.c
@@ -1741,7 +1741,8 @@ static u8 hal_EfusePgPacketWrite2ByteHeader(
 
 	efuse_addr = *pAddr;
 	if (efuse_addr >= efuse_max_available_len) {
-		DBG_8192C("%s: addr(%d) over avaliable(%d)!!\n", __func__, efuse_addr, efuse_max_available_len);
+		DBG_8192C("%s: addr(%d) over available (%d)!!\n", __func__,
+			  efuse_addr, efuse_max_available_len);
 		return false;
 	}
 
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723b_phycfg.c b/drivers/staging/rtl8723bs/hal/rtl8723b_phycfg.c
index 28d1a229c3a6..21ec890fd60c 100644
--- a/drivers/staging/rtl8723bs/hal/rtl8723b_phycfg.c
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_phycfg.c
@@ -385,8 +385,7 @@ s32 PHY_MACConfig8723B(struct adapter *Adapter)
 	/*  Config MAC */
 	/*  */
 	rtStatus = phy_ConfigMACWithParaFile(Adapter, pszMACRegFile);
-	if (rtStatus == _FAIL)
-	{
+	if (rtStatus == _FAIL) {
 		ODM_ConfigMACWithHeaderFile(&pHalData->odmpriv);
 		rtStatus = _SUCCESS;
 	}
@@ -459,8 +458,7 @@ static int phy_BB8723b_Config_ParaFile(struct adapter *Adapter)
 		Adapter->registrypriv.RegEnableTxPowerLimit == 1 ||
 		(Adapter->registrypriv.RegEnableTxPowerLimit == 2 && pHalData->EEPROMRegulatory == 1)
 	) {
-		if (PHY_ConfigRFWithPowerLimitTableParaFile(Adapter, pszRFTxPwrLmtFile) == _FAIL)
-		{
+		if (PHY_ConfigRFWithPowerLimitTableParaFile(Adapter, pszRFTxPwrLmtFile) == _FAIL) {
 			if (HAL_STATUS_SUCCESS != ODM_ConfigRFWithHeaderFile(&pHalData->odmpriv, CONFIG_RF_TXPWR_LMT, (ODM_RF_RADIO_PATH_E)0))
 				rtStatus = _FAIL;
 		}
@@ -474,8 +472,8 @@ static int phy_BB8723b_Config_ParaFile(struct adapter *Adapter)
 	/*  */
 	/*  1. Read PHY_REG.TXT BB INIT!! */
 	/*  */
-	if (phy_ConfigBBWithParaFile(Adapter, pszBBRegFile, CONFIG_BB_PHY_REG) == _FAIL)
-	{
+	if (phy_ConfigBBWithParaFile(Adapter, pszBBRegFile, CONFIG_BB_PHY_REG) ==
+		_FAIL) {
 		if (HAL_STATUS_SUCCESS != ODM_ConfigBBWithHeaderFile(&pHalData->odmpriv, CONFIG_BB_PHY_REG))
 			rtStatus = _FAIL;
 	}
@@ -491,8 +489,8 @@ static int phy_BB8723b_Config_ParaFile(struct adapter *Adapter)
 		Adapter->registrypriv.RegEnableTxPowerByRate == 1 ||
 		(Adapter->registrypriv.RegEnableTxPowerByRate == 2 && pHalData->EEPROMRegulatory != 2)
 	) {
-		if (phy_ConfigBBWithPgParaFile(Adapter, pszBBRegPgFile) == _FAIL)
-		{
+		if (phy_ConfigBBWithPgParaFile(Adapter, pszBBRegPgFile) ==
+			_FAIL) {
 			if (HAL_STATUS_SUCCESS != ODM_ConfigBBWithHeaderFile(&pHalData->odmpriv, CONFIG_BB_PHY_REG_PG))
 				rtStatus = _FAIL;
 		}
@@ -514,8 +512,8 @@ static int phy_BB8723b_Config_ParaFile(struct adapter *Adapter)
 	/*  */
 	/*  2. Read BB AGC table Initialization */
 	/*  */
-	if (phy_ConfigBBWithParaFile(Adapter, pszAGCTableFile, CONFIG_BB_AGC_TAB) == _FAIL)
-	{
+	if (phy_ConfigBBWithParaFile(Adapter, pszAGCTableFile,
+				     CONFIG_BB_AGC_TAB) == _FAIL) {
 		if (HAL_STATUS_SUCCESS != ODM_ConfigBBWithHeaderFile(&pHalData->odmpriv, CONFIG_BB_AGC_TAB))
 			rtStatus = _FAIL;
 	}
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723b_rf6052.c b/drivers/staging/rtl8723bs/hal/rtl8723b_rf6052.c
index 3a85d0cddfda..a71b552eca9a 100644
--- a/drivers/staging/rtl8723bs/hal/rtl8723b_rf6052.c
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_rf6052.c
@@ -144,15 +144,15 @@ static int phy_RF6052_Config_ParaFile(struct adapter *Adapter)
 		/*----Initialize RF fom connfiguration file----*/
 		switch (eRFPath) {
 		case RF_PATH_A:
-			if (PHY_ConfigRFWithParaFile(Adapter, pszRadioAFile, eRFPath) == _FAIL)
-			{
+			if (PHY_ConfigRFWithParaFile(Adapter, pszRadioAFile,
+						     eRFPath) == _FAIL) {
 				if (HAL_STATUS_FAILURE == ODM_ConfigRFWithHeaderFile(&pHalData->odmpriv, CONFIG_RF_RADIO, (ODM_RF_RADIO_PATH_E)eRFPath))
 					rtStatus = _FAIL;
 			}
 			break;
 		case RF_PATH_B:
-			if (PHY_ConfigRFWithParaFile(Adapter, pszRadioBFile, eRFPath) == _FAIL)
-			{
+			if (PHY_ConfigRFWithParaFile(Adapter, pszRadioBFile,
+						     eRFPath) == _FAIL) {
 				if (HAL_STATUS_FAILURE == ODM_ConfigRFWithHeaderFile(&pHalData->odmpriv, CONFIG_RF_RADIO, (ODM_RF_RADIO_PATH_E)eRFPath))
 					rtStatus = _FAIL;
 			}
@@ -186,8 +186,8 @@ static int phy_RF6052_Config_ParaFile(struct adapter *Adapter)
 	/* 3 Configuration of Tx Power Tracking */
 	/* 3 ----------------------------------------------------------------- */
 
-	if (PHY_ConfigRFWithTxPwrTrackParaFile(Adapter, pszTxPwrTrackFile) == _FAIL)
-	{
+	if (PHY_ConfigRFWithTxPwrTrackParaFile(Adapter, pszTxPwrTrackFile) ==
+		_FAIL) {
 		ODM_ConfigRFWithTxPwrTrackHeaderFile(&pHalData->odmpriv);
 	}
 
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723bs_xmit.c b/drivers/staging/rtl8723bs/hal/rtl8723bs_xmit.c
index ca6ad9659b09..9bee2e40be32 100644
--- a/drivers/staging/rtl8723bs/hal/rtl8723bs_xmit.c
+++ b/drivers/staging/rtl8723bs/hal/rtl8723bs_xmit.c
@@ -23,8 +23,7 @@ static u8 rtw_sdio_wait_enough_TxOQT_space(struct adapter *padapter, u8 agg_num)
 	u32 n = 0;
 	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
 
-	while (pHalData->SdioTxOQTFreeSpace < agg_num)
-	{
+	while (pHalData->SdioTxOQTFreeSpace < agg_num) {
 		if (
 			(padapter->bSurpriseRemoved == true) ||
 			(padapter->bDriverStopped == true)
@@ -59,7 +58,7 @@ static s32 rtl8723_dequeue_writeport(struct adapter *padapter)
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
 	struct xmit_buf *pxmitbuf;
-	struct adapter * pri_padapter = padapter;
+	struct adapter *pri_padapter = padapter;
 	s32 ret = 0;
 	u8 PageIdx = 0;
 	u32 deviceId;
@@ -231,7 +230,7 @@ static s32 xmit_xmitframes(struct adapter *padapter, struct xmit_priv *pxmitpriv
 	pxmitbuf = NULL;
 
 	if (padapter->registrypriv.wifi_spec == 1) {
-		for (idx = 0; idx<4; idx++)
+		for (idx = 0; idx < 4; idx++)
 			inx[idx] = pxmitpriv->wmm_para_seq[idx];
 	} else {
 		inx[0] = 0;
@@ -299,9 +298,10 @@ static s32 xmit_xmitframes(struct adapter *padapter, struct xmit_priv *pxmitpriv
 				) {
 					if (pxmitbuf) {
 						/* pxmitbuf->priv_data will be NULL, and will crash here */
-						if (pxmitbuf->len > 0 && pxmitbuf->priv_data) {
+						if (pxmitbuf->len > 0 &&
+						    pxmitbuf->priv_data) {
 							struct xmit_frame *pframe;
-							pframe = (struct xmit_frame*)pxmitbuf->priv_data;
+							pframe = (struct xmit_frame *)pxmitbuf->priv_data;
 							pframe->agg_num = k;
 							pxmitbuf->agg_num = k;
 							rtl8723b_update_txdesc(pframe, pframe->buf_addr);
@@ -392,7 +392,7 @@ static s32 xmit_xmitframes(struct adapter *padapter, struct xmit_priv *pxmitpriv
 
 			if (pxmitbuf->len > 0) {
 				struct xmit_frame *pframe;
-				pframe = (struct xmit_frame*)pxmitbuf->priv_data;
+				pframe = (struct xmit_frame *)pxmitbuf->priv_data;
 				pframe->agg_num = k;
 				pxmitbuf->agg_num = k;
 				rtl8723b_update_txdesc(pframe, pframe->buf_addr);
@@ -400,8 +400,7 @@ static s32 xmit_xmitframes(struct adapter *padapter, struct xmit_priv *pxmitpriv
 				pxmitbuf->priv_data = NULL;
 				enqueue_pending_xmitbuf(pxmitpriv, pxmitbuf);
 				yield();
-			}
-			else
+			} else
 				rtw_free_xmitbuf(pxmitpriv, pxmitbuf);
 			pxmitbuf = NULL;
 		}
@@ -611,7 +610,8 @@ s32	rtl8723bs_hal_xmitframe_enqueue(
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	s32 err;
 
-	if ((err = rtw_xmitframe_enqueue(padapter, pxmitframe)) != _SUCCESS) {
+	err = rtw_xmitframe_enqueue(padapter, pxmitframe);
+	if (err != _SUCCESS) {
 		rtw_free_xmitframe(pxmitpriv, pxmitframe);
 
 		pxmitpriv->tx_drop++;
diff --git a/drivers/staging/rtl8723bs/include/ieee80211.h b/drivers/staging/rtl8723bs/include/ieee80211.h
index 6dc6dc73d72f..73ce63770c3c 100644
--- a/drivers/staging/rtl8723bs/include/ieee80211.h
+++ b/drivers/staging/rtl8723bs/include/ieee80211.h
@@ -1003,10 +1003,10 @@ enum ieee80211_state {
 
 #define DEFAULT_MAX_SCAN_AGE (15 * HZ)
 #define DEFAULT_FTS 2346
-#define MAC_FMT "%02x:%02x:%02x:%02x:%02x:%02x"
-#define MAC_ARG(x) ((u8 *)(x))[0], ((u8 *)(x))[1], ((u8 *)(x))[2], ((u8 *)(x))[3], ((u8 *)(x))[4], ((u8 *)(x))[5]
-#define IP_FMT "%d.%d.%d.%d"
-#define IP_ARG(x) ((u8 *)(x))[0], ((u8 *)(x))[1], ((u8 *)(x))[2], ((u8 *)(x))[3]
+#define MAC_FMT "%pM"
+#define MAC_ARG(x) (x)
+#define IP_FMT "%pI4"
+#define IP_ARG(x) (x)
 
 extern __inline int is_multicast_mac_addr(const u8 *addr)
 {
diff --git a/drivers/staging/rtl8723bs/include/osdep_service.h b/drivers/staging/rtl8723bs/include/osdep_service.h
index fdeabc1daeca..ac9ffe0e3b84 100644
--- a/drivers/staging/rtl8723bs/include/osdep_service.h
+++ b/drivers/staging/rtl8723bs/include/osdep_service.h
@@ -169,10 +169,10 @@ __inline static u32 _RND8(u32 sz)
 }
 
 #ifndef MAC_FMT
-#define MAC_FMT "%02x:%02x:%02x:%02x:%02x:%02x"
+#define MAC_FMT "%pM"
 #endif
 #ifndef MAC_ARG
-#define MAC_ARG(x) ((u8 *)(x))[0], ((u8 *)(x))[1], ((u8 *)(x))[2], ((u8 *)(x))[3], ((u8 *)(x))[4], ((u8 *)(x))[5]
+#define MAC_ARG(x) (x)
 #endif
 
 
diff --git a/drivers/staging/rtl8723bs/include/osdep_service_linux.h b/drivers/staging/rtl8723bs/include/osdep_service_linux.h
index 486e8184b0b2..0c9b4f622fee 100644
--- a/drivers/staging/rtl8723bs/include/osdep_service_linux.h
+++ b/drivers/staging/rtl8723bs/include/osdep_service_linux.h
@@ -26,10 +26,10 @@
 	/* include <linux/smp_lock.h> */
 	#include <linux/netdevice.h>
 	#include <linux/skbuff.h>
-	#include <asm/uaccess.h>
+	#include <linux/uaccess.h>
 	#include <asm/byteorder.h>
-	#include <asm/atomic.h>
-	#include <asm/io.h>
+	#include <linux/atomic.h>
+	#include <linux/io.h>
 	#include <linux/semaphore.h>
 	#include <linux/sem.h>
 	#include <linux/sched.h>
diff --git a/drivers/staging/rtl8723bs/include/rtl8192c_rf.h b/drivers/staging/rtl8723bs/include/rtl8192c_rf.h
index 0dbee562d19b..97900a31b326 100644
--- a/drivers/staging/rtl8723bs/include/rtl8192c_rf.h
+++ b/drivers/staging/rtl8723bs/include/rtl8192c_rf.h
@@ -19,19 +19,16 @@
 /*  */
 /*  RF RL6052 Series API */
 /*  */
-void 	rtl8192c_RF_ChangeTxPath(struct adapter *Adapter,
-				u16 	DataRate);
-void 	rtl8192c_PHY_RF6052SetBandwidth(
-				struct adapter *			Adapter,
-				enum CHANNEL_WIDTH		Bandwidth);
-void rtl8192c_PHY_RF6052SetCckTxPower(
-				struct adapter *Adapter,
-				u8*	pPowerlevel);
-void rtl8192c_PHY_RF6052SetOFDMTxPower(
-				struct adapter *Adapter,
-				u8*	pPowerLevel,
-				u8 Channel);
-int	PHY_RF6052_Config8192C(struct adapter *	Adapter	);
+void rtl8192c_RF_ChangeTxPath(struct adapter *Adapter,
+			      u16 DataRate);
+void rtl8192c_PHY_RF6052SetBandwidth(struct adapter *Adapter,
+				     enum CHANNEL_WIDTH Bandwidth);
+void rtl8192c_PHY_RF6052SetCckTxPower(struct adapter *Adapter,
+				      u8 *pPowerlevel);
+void rtl8192c_PHY_RF6052SetOFDMTxPower(struct adapter *Adapter,
+				       u8 *pPowerLevel,
+				       u8 Channel);
+int PHY_RF6052_Config8192C(struct adapter *Adapter);
 
 /*--------------------------Exported Function prototype---------------------*/
 
diff --git a/drivers/staging/rtl8723bs/include/rtl8723b_spec.h b/drivers/staging/rtl8723bs/include/rtl8723b_spec.h
index 8d78f4ef5438..1906ff2038f5 100644
--- a/drivers/staging/rtl8723bs/include/rtl8723b_spec.h
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_spec.h
@@ -17,12 +17,11 @@
 
 #include <autoconf.h>
 
-
 #define HAL_NAV_UPPER_UNIT_8723B		128		/*  micro-second */
 
 /*  */
 /*  */
-/* 	0x0000h ~ 0x00FFh	System Configuration */
+/*	0x0000h ~ 0x00FFh	System Configuration */
 /*  */
 /*  */
 #define REG_RSV_CTRL_8723B				0x001C	/*  3 Byte */
@@ -42,7 +41,7 @@
 
 /*  */
 /*  */
-/* 	0x0100h ~ 0x01FFh	MACTOP General Configuration */
+/*	0x0100h ~ 0x01FFh	MACTOP General Configuration */
 /*  */
 /*  */
 #define REG_C2HEVT_CMD_ID_8723B	0x01A0
@@ -58,13 +57,13 @@
 
 /*  */
 /*  */
-/* 	0x0200h ~ 0x027Fh	TXDMA Configuration */
+/*	0x0200h ~ 0x027Fh	TXDMA Configuration */
 /*  */
 /*  */
 
 /*  */
 /*  */
-/* 	0x0280h ~ 0x02FFh	RXDMA Configuration */
+/*	0x0280h ~ 0x02FFh	RXDMA Configuration */
 /*  */
 /*  */
 #define REG_RXDMA_CONTROL_8723B		0x0286 /*  Control the RX DMA. */
@@ -72,7 +71,7 @@
 
 /*  */
 /*  */
-/* 	0x0300h ~ 0x03FFh	PCIe */
+/*	0x0300h ~ 0x03FFh	PCIe */
 /*  */
 /*  */
 #define	REG_PCIE_CTRL_REG_8723B		0x0300
@@ -99,7 +98,7 @@
 
 /*  */
 /*  */
-/* 	0x0400h ~ 0x047Fh	Protocol Configuration */
+/*	0x0400h ~ 0x047Fh	Protocol Configuration */
 /*  */
 /*  */
 #define REG_TXPKTBUF_BCNQ_BDNY_8723B	0x0424
@@ -113,18 +112,17 @@
 
 /*  */
 /*  */
-/* 	0x0500h ~ 0x05FFh	EDCA Configuration */
+/*	0x0500h ~ 0x05FFh	EDCA Configuration */
 /*  */
 /*  */
 #define REG_SECONDARY_CCA_CTRL_8723B	0x0577
 
 /*  */
 /*  */
-/* 	0x0600h ~ 0x07FFh	WMAC Configuration */
+/*	0x0600h ~ 0x07FFh	WMAC Configuration */
 /*  */
 /*  */
 
-
 /*  */
 /*  SDIO Bus Specification */
 /*  */
@@ -142,9 +140,8 @@
 /*  */
 #define SDIO_REG_HCPWM1_8723B	0x025 /*  HCI Current Power Mode 1 */
 
-
 /*  */
-/* 	8723 Regsiter Bit and Content definition */
+/*	8723 Register Bit and Content definition */
 /*  */
 
 /* 2 HSISR */
@@ -157,20 +154,19 @@
 
 /*  */
 /*  */
-/* 	0x0100h ~ 0x01FFh	MACTOP General Configuration */
+/*	0x0100h ~ 0x01FFh	MACTOP General Configuration */
 /*  */
 /*  */
 
-
 /*  */
 /*  */
-/* 	0x0200h ~ 0x027Fh	TXDMA Configuration */
+/*	0x0200h ~ 0x027Fh	TXDMA Configuration */
 /*  */
 /*  */
 
 /*  */
 /*  */
-/* 	0x0280h ~ 0x02FFh	RXDMA Configuration */
+/*	0x0280h ~ 0x02FFh	RXDMA Configuration */
 /*  */
 /*  */
 #define BIT_USB_RXDMA_AGG_EN	BIT(31)
@@ -184,7 +180,7 @@
 
 /*  */
 /*  */
-/* 	0x0400h ~ 0x047Fh	Protocol Configuration */
+/*	0x0400h ~ 0x047Fh	Protocol Configuration */
 /*  */
 /*  */
 
@@ -195,19 +191,18 @@
 
 /*  */
 /*  */
-/* 	0x0500h ~ 0x05FFh	EDCA Configuration */
+/*	0x0500h ~ 0x05FFh	EDCA Configuration */
 /*  */
 /*  */
 
 /*  */
 /*  */
-/* 	0x0600h ~ 0x07FFh	WMAC Configuration */
+/*	0x0600h ~ 0x07FFh	WMAC Configuration */
 /*  */
 /*  */
 #define EEPROM_RF_GAIN_OFFSET			0xC1
 #define EEPROM_RF_GAIN_VAL			0x1F6
 
-
 /*  */
 /*        8195 IMR/ISR bits						(offset 0xB0,  8bits) */
 /*  */
@@ -246,13 +241,13 @@
 #define	IMR_BCNDMAINT3_8723B				BIT23		/*  Beacon DMA Interrupt 3 */
 #define	IMR_BCNDMAINT2_8723B				BIT22		/*  Beacon DMA Interrupt 2 */
 #define	IMR_BCNDMAINT1_8723B				BIT21		/*  Beacon DMA Interrupt 1 */
-#define	IMR_BCNDOK7_8723B					BIT20		/*  Beacon Queue DMA OK Interrup 7 */
-#define	IMR_BCNDOK6_8723B					BIT19		/*  Beacon Queue DMA OK Interrup 6 */
-#define	IMR_BCNDOK5_8723B					BIT18		/*  Beacon Queue DMA OK Interrup 5 */
-#define	IMR_BCNDOK4_8723B					BIT17		/*  Beacon Queue DMA OK Interrup 4 */
-#define	IMR_BCNDOK3_8723B					BIT16		/*  Beacon Queue DMA OK Interrup 3 */
-#define	IMR_BCNDOK2_8723B					BIT15		/*  Beacon Queue DMA OK Interrup 2 */
-#define	IMR_BCNDOK1_8723B					BIT14		/*  Beacon Queue DMA OK Interrup 1 */
+#define	IMR_BCNDOK7_8723B					BIT20		/*  Beacon Queue DMA OK Interrupt 7 */
+#define	IMR_BCNDOK6_8723B					BIT19		/*  Beacon Queue DMA OK Interrupt 6 */
+#define	IMR_BCNDOK5_8723B					BIT18		/*  Beacon Queue DMA OK Interrupt 5 */
+#define	IMR_BCNDOK4_8723B					BIT17		/*  Beacon Queue DMA OK Interrupt 4 */
+#define	IMR_BCNDOK3_8723B					BIT16		/*  Beacon Queue DMA OK Interrupt 3 */
+#define	IMR_BCNDOK2_8723B					BIT15		/*  Beacon Queue DMA OK Interrupt 2 */
+#define	IMR_BCNDOK1_8723B					BIT14		/*  Beacon Queue DMA OK Interrupt 1 */
 #define	IMR_ATIMEND_E_8723B				BIT13		/*  ATIM Window End Extension for Win7 */
 #define	IMR_TXERR_8723B					BIT11		/*  Tx Error Flag Interrupt Status, write 1 clear. */
 #define	IMR_RXERR_8723B					BIT10		/*  Rx Error Flag INT Status, Write 1 clear */
diff --git a/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c b/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
index 36c3189fc4b7..bd4352fe2de3 100644
--- a/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
+++ b/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
@@ -2667,7 +2667,8 @@ static int rtw_cfg80211_add_monitor_if (struct adapter *padapter, char *name, st
 	mon_ndev->type = ARPHRD_IEEE80211_RADIOTAP;
 	strncpy(mon_ndev->name, name, IFNAMSIZ);
 	mon_ndev->name[IFNAMSIZ - 1] = 0;
-	mon_ndev->destructor = rtw_ndev_destructor;
+	mon_ndev->needs_free_netdev = true;
+	mon_ndev->priv_destructor = rtw_ndev_destructor;
 
 	mon_ndev->netdev_ops = &rtw_cfg80211_monitor_if_ops;
 
diff --git a/drivers/staging/rtl8723bs/os_dep/ioctl_linux.c b/drivers/staging/rtl8723bs/os_dep/ioctl_linux.c
index 916741371bee..79d8383d4b9b 100644
--- a/drivers/staging/rtl8723bs/os_dep/ioctl_linux.c
+++ b/drivers/staging/rtl8723bs/os_dep/ioctl_linux.c
@@ -766,7 +766,7 @@ static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
 
 exit:
 
-	kfree((u8 *)pwep);
+	kfree(pwep);
 	return ret;
 }
 
@@ -2500,7 +2500,7 @@ static int rtw_wx_set_enc_ext(struct net_device *dev,
 	ret =  wpa_set_encryption(dev, param, param_len);
 
 exit:
-	kfree((u8 *)param);
+	kfree(param);
 
 	return ret;
 }
@@ -3767,7 +3767,7 @@ static int wpa_supplicant_ioctl(struct net_device *dev, struct iw_point *p)
 
 	if (copy_from_user(param, p->pointer, p->length))
 	{
-		kfree((u8 *)param);
+		kfree(param);
 		ret = -EFAULT;
 		goto out;
 	}
@@ -3801,7 +3801,7 @@ static int wpa_supplicant_ioctl(struct net_device *dev, struct iw_point *p)
 	if (ret == 0 && copy_to_user(p->pointer, param, p->length))
 		ret = -EFAULT;
 
-	kfree((u8 *)param);
+	kfree(param);
 
 out:
 
@@ -4130,7 +4130,7 @@ static int rtw_set_encryption(struct net_device *dev, struct ieee_param *param,
 	}
 
 exit:
-	kfree((u8 *)pwep);
+	kfree(pwep);
 
 	return ret;
 
@@ -4713,7 +4713,7 @@ static int rtw_hostapd_ioctl(struct net_device *dev, struct iw_point *p)
 
 	if (copy_from_user(param, p->pointer, p->length))
 	{
-		kfree((u8 *)param);
+		kfree(param);
 		ret = -EFAULT;
 		goto out;
 	}
@@ -4817,7 +4817,7 @@ static int rtw_hostapd_ioctl(struct net_device *dev, struct iw_point *p)
 		ret = -EFAULT;
 
 
-	kfree((u8 *)param);
+	kfree(param);
 
 out:
 
diff --git a/drivers/staging/rtl8723bs/os_dep/os_intfs.c b/drivers/staging/rtl8723bs/os_dep/os_intfs.c
index f83cfc76505c..021589913681 100644
--- a/drivers/staging/rtl8723bs/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8723bs/os_dep/os_intfs.c
@@ -1207,8 +1207,6 @@ void rtw_ndev_destructor(struct net_device *ndev)
 
 	if (ndev->ieee80211_ptr)
 		kfree((u8 *)ndev->ieee80211_ptr);
-
-	free_netdev(ndev);
 }
 
 void rtw_dev_unload(struct adapter *padapter)
diff --git a/drivers/staging/rtl8723bs/os_dep/osdep_service.c b/drivers/staging/rtl8723bs/os_dep/osdep_service.c
index 02db59e8b593..aa16d1ab955b 100644
--- a/drivers/staging/rtl8723bs/os_dep/osdep_service.c
+++ b/drivers/staging/rtl8723bs/os_dep/osdep_service.c
@@ -160,7 +160,7 @@ static int isFileReadable(char *path)
 		oldfs = get_fs(); set_fs(get_ds());
 
 		if (1!=readFile(fp, &buf, 1))
-			ret = PTR_ERR(fp);
+			ret = -EINVAL;
 
 		set_fs(oldfs);
 		filp_close(fp, NULL);
diff --git a/drivers/staging/rtl8723bs/os_dep/recv_linux.c b/drivers/staging/rtl8723bs/os_dep/recv_linux.c
index e731ab4e2bd7..f42e00081e0e 100644
--- a/drivers/staging/rtl8723bs/os_dep/recv_linux.c
+++ b/drivers/staging/rtl8723bs/os_dep/recv_linux.c
@@ -72,7 +72,6 @@ int rtw_os_recvbuf_resource_free(struct adapter *padapter, struct recv_buf *prec
 _pkt *rtw_os_alloc_msdu_pkt(union recv_frame *prframe, u16 nSubframe_Length, u8 *pdata)
 {
 	u16 eth_type;
-	u8 *data_ptr;
 	_pkt *sub_skb;
 	struct rx_pkt_attrib *pattrib;
 
@@ -82,8 +81,7 @@ _pkt *rtw_os_alloc_msdu_pkt(union recv_frame *prframe, u16 nSubframe_Length, u8
 	if (sub_skb)
 	{
 		skb_reserve(sub_skb, 12);
-		data_ptr = (u8 *)skb_put(sub_skb, nSubframe_Length);
-		memcpy(data_ptr, (pdata + ETH_HLEN), nSubframe_Length);
+		skb_put_data(sub_skb, (pdata + ETH_HLEN), nSubframe_Length);
 	}
 	else
 	{
diff --git a/drivers/staging/rtl8723bs/os_dep/sdio_ops_linux.c b/drivers/staging/rtl8723bs/os_dep/sdio_ops_linux.c
index 33f0f83b002d..3aa3e6548fd5 100644
--- a/drivers/staging/rtl8723bs/os_dep/sdio_ops_linux.c
+++ b/drivers/staging/rtl8723bs/os_dep/sdio_ops_linux.c
@@ -272,7 +272,7 @@ u32 sd_read32(struct intf_hdl *pintfhdl, u32 addr, s32 *err)
 		DBG_871X(KERN_ERR "%s: (%d) addr = 0x%05x, val = 0x%x\n", __func__, *err, addr, v);
 
 		*err = 0;
-		for (i = 0; i<SD_IO_TRY_CNT; i++)
+		for (i = 0; i < SD_IO_TRY_CNT; i++)
 		{
 			if (claim_needed) sdio_claim_host(func);
 			v = sdio_readl(func, addr, err);
@@ -294,7 +294,7 @@ u32 sd_read32(struct intf_hdl *pintfhdl, u32 addr, s32 *err)
 			}
 		}
 
-		if (i ==SD_IO_TRY_CNT)
+		if (i == SD_IO_TRY_CNT)
 			DBG_871X(KERN_ERR "%s: FAIL!(%d) addr = 0x%05x, val = 0x%x, try_cnt =%d\n", __func__, *err, addr, v, i);
 		else
 			DBG_871X(KERN_ERR "%s: (%d) addr = 0x%05x, val = 0x%x, try_cnt =%d\n", __func__, *err, addr, v, i);
@@ -317,7 +317,7 @@ void sd_write8(struct intf_hdl *pintfhdl, u32 addr, u8 v, s32 *err)
 
 	if (padapter->bSurpriseRemoved) {
 		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
-		return ;
+		return;
 	}
 
 	func = psdio->func;
@@ -346,7 +346,7 @@ void sd_write32(struct intf_hdl *pintfhdl, u32 addr, u32 v, s32 *err)
 
 	if (padapter->bSurpriseRemoved) {
 		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
-		return ;
+		return;
 	}
 
 	func = psdio->func;
@@ -365,7 +365,7 @@ void sd_write32(struct intf_hdl *pintfhdl, u32 addr, u32 v, s32 *err)
 		DBG_871X(KERN_ERR "%s: (%d) addr = 0x%05x val = 0x%08x\n", __func__, *err, addr, v);
 
 		*err = 0;
-		for (i = 0; i<SD_IO_TRY_CNT; i++)
+		for (i = 0; i < SD_IO_TRY_CNT; i++)
 		{
 			if (claim_needed) sdio_claim_host(func);
 			sdio_writel(func, v, addr, err);
@@ -386,7 +386,7 @@ void sd_write32(struct intf_hdl *pintfhdl, u32 addr, u32 v, s32 *err)
 			}
 		}
 
-		if (i ==SD_IO_TRY_CNT)
+		if (i == SD_IO_TRY_CNT)
 			DBG_871X(KERN_ERR "%s: FAIL!(%d) addr = 0x%05x val = 0x%08x, try_cnt =%d\n", __func__, *err, addr, v, i);
 		else
 			DBG_871X(KERN_ERR "%s: (%d) addr = 0x%05x val = 0x%08x, try_cnt =%d\n", __func__, *err, addr, v, i);
@@ -428,7 +428,7 @@ s32 _sd_read(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata)
 
 	func = psdio->func;
 
-	if (unlikely((cnt == 1) || (cnt ==2)))
+	if (unlikely((cnt == 1) || (cnt == 2)))
 	{
 		int i;
 		u8 *pbuf = (u8 *)pdata;
@@ -465,7 +465,7 @@ s32 _sd_read(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata)
  *0		Success
  *others	Fail
  */
-s32 sd_read(struct intf_hdl * pintfhdl, u32 addr, u32 cnt, void *pdata)
+s32 sd_read(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata)
 {
 	struct adapter *padapter;
 	struct dvobj_priv *psdiodev;
@@ -517,7 +517,7 @@ s32 _sd_write(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata)
 
 	struct sdio_func *func;
 	u32 size;
-	s32 err =-EPERM;
+	s32 err =  -EPERM;
 
 	padapter = pintfhdl->padapter;
 	psdiodev = pintfhdl->pintf_dev;
@@ -529,9 +529,9 @@ s32 _sd_write(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata)
 	}
 
 	func = psdio->func;
-/* 	size = sdio_align_size(func, cnt); */
+/*	size = sdio_align_size(func, cnt); */
 
-	if (unlikely((cnt == 1) || (cnt ==2)))
+	if (unlikely((cnt == 1) || (cnt == 2)))
 	{
 		int i;
 		u8 *pbuf = (u8 *)pdata;
@@ -576,7 +576,7 @@ s32 sd_write(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, void *pdata)
 	PSDIO_DATA psdio;
 	struct sdio_func *func;
 	bool claim_needed;
-	s32 err =-EPERM;
+	s32 err =  -EPERM;
 
 	padapter = pintfhdl->padapter;
 	psdiodev = pintfhdl->pintf_dev;
diff --git a/drivers/staging/rtl8723bs/os_dep/wifi_regd.c b/drivers/staging/rtl8723bs/os_dep/wifi_regd.c
index 9c61125f5910..305e88a6b2ca 100644
--- a/drivers/staging/rtl8723bs/os_dep/wifi_regd.c
+++ b/drivers/staging/rtl8723bs/os_dep/wifi_regd.c
@@ -14,42 +14,43 @@
  */
 
 /*
- *Only these channels all allow active
- *scan on all world regulatory domains
+ * Only these channels all allow active
+ * scan on all world regulatory domains
  */
 
 /* 2G chan 01 - chan 11 */
 #define RTW_2GHZ_CH01_11	\
-	REG_RULE(2412-10, 2462+10, 40, 0, 20, 0)
+	REG_RULE(2412 - 10, 2462 + 10, 40, 0, 20, 0)
 
 /*
- *We enable active scan on these a case
- *by case basis by regulatory domain
+ * We enable active scan on these a case
+ * by case basis by regulatory domain
  */
 
 /* 2G chan 12 - chan 13, PASSIV SCAN */
 #define RTW_2GHZ_CH12_13	\
-	REG_RULE(2467-10, 2472+10, 40, 0, 20,	\
+	REG_RULE(2467 - 10, 2472 + 10, 40, 0, 20,	\
 	NL80211_RRF_PASSIVE_SCAN)
 
 /* 2G chan 14, PASSIVS SCAN, NO OFDM (B only) */
 #define RTW_2GHZ_CH14	\
-	REG_RULE(2484-10, 2484+10, 40, 0, 20,	\
+	REG_RULE(2484 - 10, 2484 + 10, 40, 0, 20,	\
 	NL80211_RRF_PASSIVE_SCAN | NL80211_RRF_NO_OFDM)
 
 static const struct ieee80211_regdomain rtw_regdom_rd = {
 	.n_reg_rules = 3,
 	.alpha2 = "99",
 	.reg_rules = {
-		      RTW_2GHZ_CH01_11,
-		      RTW_2GHZ_CH12_13,
-		      }
+		RTW_2GHZ_CH01_11,
+		RTW_2GHZ_CH12_13,
+	}
 };
 
 static int rtw_ieee80211_channel_to_frequency(int chan, int band)
 {
 	/* see 802.11 17.3.8.3.2 and Annex J
-	 * there are overlapping channel numbers in 5GHz and 2GHz bands */
+	 * there are overlapping channel numbers in 5GHz and 2GHz bands
+	 */
 
 	/* NL80211_BAND_2GHZ */
 	if (chan == 14)
@@ -73,7 +74,7 @@ static void _rtw_reg_apply_flags(struct wiphy *wiphy)
 	u16 channel;
 	u32 freq;
 
-	/*  all channels disable */
+	/* all channels disable */
 	for (i = 0; i < NUM_NL80211_BANDS; i++) {
 		sband = wiphy->bands[i];
 
@@ -87,7 +88,7 @@ static void _rtw_reg_apply_flags(struct wiphy *wiphy)
 		}
 	}
 
-	/*  channels apply by channel plans. */
+	/* channels apply by channel plans. */
 	for (i = 0; i < max_chan_nums; i++) {
 		channel = channel_set[i].ChannelNum;
 		freq =
@@ -96,12 +97,10 @@ static void _rtw_reg_apply_flags(struct wiphy *wiphy)
 
 		ch = ieee80211_get_channel(wiphy, freq);
 		if (ch) {
-			if (channel_set[i].ScanType == SCAN_PASSIVE) {
+			if (channel_set[i].ScanType == SCAN_PASSIVE)
 				ch->flags = IEEE80211_CHAN_NO_IR;
-			}
-			else {
+			else
 				ch->flags = 0;
-			}
 		}
 	}
 }
@@ -123,10 +122,11 @@ static const struct ieee80211_regdomain *_rtw_regdomain_select(struct
 }
 
 static void _rtw_regd_init_wiphy(struct rtw_regulatory *reg,
-				struct wiphy *wiphy,
-				void (*reg_notifier) (struct wiphy * wiphy,
-						     struct regulatory_request *
-						     request))
+				 struct wiphy *wiphy,
+				 void (*reg_notifier)(struct wiphy *wiphy,
+						      struct
+						      regulatory_request *
+						      request))
 {
 	const struct ieee80211_regdomain *regd;
 
@@ -144,11 +144,11 @@ static void _rtw_regd_init_wiphy(struct rtw_regulatory *reg,
 }
 
 int rtw_regd_init(struct adapter *padapter,
-		  void (*reg_notifier) (struct wiphy * wiphy,
+		  void (*reg_notifier)(struct wiphy *wiphy,
 				       struct regulatory_request *request))
 {
-	/* struct registry_priv  *registrypriv = &padapter->registrypriv; */
 	struct wiphy *wiphy = padapter->rtw_wdev->wiphy;
+
 	_rtw_regd_init_wiphy(NULL, wiphy, reg_notifier);
 
 	return 0;
diff --git a/drivers/staging/rts5208/rtsx_scsi.c b/drivers/staging/rts5208/rtsx_scsi.c
index a95c5de1aa00..36b5a11f21d2 100644
--- a/drivers/staging/rts5208/rtsx_scsi.c
+++ b/drivers/staging/rts5208/rtsx_scsi.c
@@ -536,7 +536,7 @@ static int inquiry(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if (sendbytes > 8) {
 		memcpy(buf, inquiry_buf, 8);
-		memcpy(buf + 8, inquiry_string,	sendbytes - 8);
+		strncpy(buf + 8, inquiry_string, sendbytes - 8);
 		if (pro_formatter_flag) {
 			/* Additional Length */
 			buf[4] = 0x33;
diff --git a/drivers/staging/rts5208/sd.c b/drivers/staging/rts5208/sd.c
index bdd35b611f27..c2eb072cbe1d 100644
--- a/drivers/staging/rts5208/sd.c
+++ b/drivers/staging/rts5208/sd.c
@@ -1057,7 +1057,7 @@ fail:
 
 	rtsx_write_register(chip, SD_DCMPS_CTL, DCMPS_CHANGE, 0);
 	rtsx_write_register(chip, SD_VP_CTL, PHASE_CHANGE, 0);
-	wait_timeout(10);
+	mdelay(10);
 	sd_reset_dcm(chip, tune_dir);
 	return STATUS_FAIL;
 }
@@ -5231,7 +5231,7 @@ int sd_power_off_card3v3(struct rtsx_chip *chip)
 			return STATUS_FAIL;
 		}
 
-		wait_timeout(50);
+		mdelay(50);
 	}
 
 	if (chip->asic_code) {
diff --git a/drivers/staging/rts5208/xd.c b/drivers/staging/rts5208/xd.c
index 85aba05acbc1..74d36f9a4c1d 100644
--- a/drivers/staging/rts5208/xd.c
+++ b/drivers/staging/rts5208/xd.c
@@ -1268,7 +1268,7 @@ static int xd_copy_page(struct rtsx_chip *chip, u32 old_blk, u32 new_blk,
 			reg = 0;
 			rtsx_read_register(chip, XD_CTL, &reg);
 			if (reg & (XD_ECC1_ERROR | XD_ECC2_ERROR)) {
-				wait_timeout(100);
+				mdelay(100);
 
 				if (detect_card_cd(chip,
 						   XD_CARD) != STATUS_SUCCESS) {
diff --git a/drivers/staging/sm750fb/ddk750_chip.c b/drivers/staging/sm750fb/ddk750_chip.c
index 5e4bfb601cea..944dd25924be 100644
--- a/drivers/staging/sm750fb/ddk750_chip.c
+++ b/drivers/staging/sm750fb/ddk750_chip.c
@@ -175,7 +175,7 @@ static void set_master_clock(unsigned int frequency)
 		}
 
 		sm750_set_current_gate(reg);
-		}
+	}
 }
 
 unsigned int ddk750_get_vm_size(void)
@@ -224,7 +224,7 @@ int ddk750_init_hw(struct initchip_param *pInitParam)
 	sm750_set_current_gate(reg);
 
 	if (sm750_get_chip_type() != SM750LE) {
-		/*	set panel pll and graphic mode via mmio_88 */
+		/* set panel pll and graphic mode via mmio_88 */
 		reg = peek32(VGA_CONFIGURATION);
 		reg |= (VGA_CONFIGURATION_PLL | VGA_CONFIGURATION_MODE);
 		poke32(VGA_CONFIGURATION, reg);
@@ -309,7 +309,8 @@ int ddk750_init_hw(struct initchip_param *pInitParam)
  * M = {1,...,255}
  * N = {2,...,15}
  */
-unsigned int sm750_calc_pll_value(unsigned int request_orig, struct pll_value *pll)
+unsigned int sm750_calc_pll_value(unsigned int request_orig,
+				  struct pll_value *pll)
 {
 	/*
 	 * as sm750 register definition,
diff --git a/drivers/staging/sm750fb/ddk750_dvi.c b/drivers/staging/sm750fb/ddk750_dvi.c
index 171ae063f06f..87a199d6cdaf 100644
--- a/drivers/staging/sm750fb/ddk750_dvi.c
+++ b/drivers/staging/sm750fb/ddk750_dvi.c
@@ -29,26 +29,31 @@ static dvi_ctrl_device_t g_dcftSupportedDviController[] = {
 #endif
 };
 
-int dviInit(
-	unsigned char edgeSelect,
-	unsigned char busSelect,
-	unsigned char dualEdgeClkSelect,
-	unsigned char hsyncEnable,
-	unsigned char vsyncEnable,
-	unsigned char deskewEnable,
-	unsigned char deskewSetting,
-	unsigned char continuousSyncEnable,
-	unsigned char pllFilterEnable,
-	unsigned char pllFilterValue
-			)
+int dviInit(unsigned char edgeSelect,
+	    unsigned char busSelect,
+	    unsigned char dualEdgeClkSelect,
+	    unsigned char hsyncEnable,
+	    unsigned char vsyncEnable,
+	    unsigned char deskewEnable,
+	    unsigned char deskewSetting,
+	    unsigned char continuousSyncEnable,
+	    unsigned char pllFilterEnable,
+	    unsigned char pllFilterValue)
 {
 	dvi_ctrl_device_t *pCurrentDviCtrl;
 
 	pCurrentDviCtrl = g_dcftSupportedDviController;
 	if (pCurrentDviCtrl->pfnInit) {
-		return pCurrentDviCtrl->pfnInit(edgeSelect, busSelect, dualEdgeClkSelect, hsyncEnable,
-						vsyncEnable, deskewEnable, deskewSetting, continuousSyncEnable,
-						pllFilterEnable, pllFilterValue);
+		return pCurrentDviCtrl->pfnInit(edgeSelect,
+						busSelect,
+						dualEdgeClkSelect,
+						hsyncEnable,
+						vsyncEnable,
+						deskewEnable,
+						deskewSetting,
+						continuousSyncEnable,
+						pllFilterEnable,
+						pllFilterValue);
 	}
 	return -1; /* error */
 }
diff --git a/drivers/staging/sm750fb/ddk750_dvi.h b/drivers/staging/sm750fb/ddk750_dvi.h
index 677939cb5130..4a8394561f76 100644
--- a/drivers/staging/sm750fb/ddk750_dvi.h
+++ b/drivers/staging/sm750fb/ddk750_dvi.h
@@ -3,17 +3,16 @@
 
 /* dvi chip stuffs structros */
 
-typedef long (*PFN_DVICTRL_INIT)(
-	unsigned char edgeSelect,
-	unsigned char busSelect,
-	unsigned char dualEdgeClkSelect,
-	unsigned char hsyncEnable,
-	unsigned char vsyncEnable,
-	unsigned char deskewEnable,
-	unsigned char deskewSetting,
-	unsigned char continuousSyncEnable,
-	unsigned char pllFilterEnable,
-	unsigned char pllFilterValue);
+typedef long (*PFN_DVICTRL_INIT)(unsigned char edgeSelect,
+				 unsigned char busSelect,
+				 unsigned char dualEdgeClkSelect,
+				 unsigned char hsyncEnable,
+				 unsigned char vsyncEnable,
+				 unsigned char deskewEnable,
+				 unsigned char deskewSetting,
+				 unsigned char continuousSyncEnable,
+				 unsigned char pllFilterEnable,
+				 unsigned char pllFilterValue);
 
 typedef void (*PFN_DVICTRL_RESETCHIP)(void);
 typedef char* (*PFN_DVICTRL_GETCHIPSTRING)(void);
@@ -42,18 +41,16 @@ typedef struct _dvi_ctrl_device_t {
 #define DVI_CTRL_SII164
 
 /* dvi functions prototype */
-int dviInit(
-	unsigned char edgeSelect,
-	unsigned char busSelect,
-	unsigned char dualEdgeClkSelect,
-	unsigned char hsyncEnable,
-	unsigned char vsyncEnable,
-	unsigned char deskewEnable,
-	unsigned char deskewSetting,
-	unsigned char continuousSyncEnable,
-	unsigned char pllFilterEnable,
-	unsigned char pllFilterValue
-);
+int dviInit(unsigned char edgeSelect,
+	    unsigned char busSelect,
+	    unsigned char dualEdgeClkSelect,
+	    unsigned char hsyncEnable,
+	    unsigned char vsyncEnable,
+	    unsigned char deskewEnable,
+	    unsigned char deskewSetting,
+	    unsigned char continuousSyncEnable,
+	    unsigned char pllFilterEnable,
+	    unsigned char pllFilterValue);
 
 #endif
 
diff --git a/drivers/staging/sm750fb/ddk750_hwi2c.c b/drivers/staging/sm750fb/ddk750_hwi2c.c
index fe814e4881f9..ec556a978a98 100644
--- a/drivers/staging/sm750fb/ddk750_hwi2c.c
+++ b/drivers/staging/sm750fb/ddk750_hwi2c.c
@@ -8,9 +8,7 @@
 #define MAX_HWI2C_FIFO                  16
 #define HWI2C_WAIT_TIMEOUT              0xF0000
 
-int sm750_hw_i2c_init(
-unsigned char bus_speed_mode
-)
+int sm750_hw_i2c_init(unsigned char bus_speed_mode)
 {
 	unsigned int value;
 
@@ -81,11 +79,9 @@ static long hw_i2c_wait_tx_done(void)
  *  Return Value:
  *      Total number of bytes those are actually written.
  */
-static unsigned int hw_i2c_write_data(
-	unsigned char addr,
-	unsigned int length,
-	unsigned char *buf
-)
+static unsigned int hw_i2c_write_data(unsigned char addr,
+				      unsigned int length,
+				      unsigned char *buf)
 {
 	unsigned char count, i;
 	unsigned int total_bytes = 0;
@@ -148,11 +144,9 @@ static unsigned int hw_i2c_write_data(
  *  Return Value:
  *      Total number of actual bytes read from the slave device
  */
-static unsigned int hw_i2c_read_data(
-	unsigned char addr,
-	unsigned int length,
-	unsigned char *buf
-)
+static unsigned int hw_i2c_read_data(unsigned char addr,
+				     unsigned int length,
+				     unsigned char *buf)
 {
 	unsigned char count, i;
 	unsigned int total_bytes = 0;
@@ -212,10 +206,7 @@ static unsigned int hw_i2c_read_data(
  *  Return Value:
  *      Register value
  */
-unsigned char sm750_hw_i2c_read_reg(
-	unsigned char addr,
-	unsigned char reg
-)
+unsigned char sm750_hw_i2c_read_reg(unsigned char addr, unsigned char reg)
 {
 	unsigned char value = 0xFF;
 
@@ -238,11 +229,9 @@ unsigned char sm750_hw_i2c_read_reg(
  *          0   - Success
  *         -1   - Fail
  */
-int sm750_hw_i2c_write_reg(
-	unsigned char addr,
-	unsigned char reg,
-	unsigned char data
-)
+int sm750_hw_i2c_write_reg(unsigned char addr,
+			   unsigned char reg,
+			   unsigned char data)
 {
 	unsigned char value[2];
 
diff --git a/drivers/staging/sm750fb/ddk750_sii164.c b/drivers/staging/sm750fb/ddk750_sii164.c
index 259006ace219..0431833de781 100644
--- a/drivers/staging/sm750fb/ddk750_sii164.c
+++ b/drivers/staging/sm750fb/ddk750_sii164.c
@@ -112,18 +112,16 @@ unsigned short sii164GetDeviceID(void)
  *      0   - Success
  *     -1   - Fail.
  */
-long sii164InitChip(
-	unsigned char edgeSelect,
-	unsigned char busSelect,
-	unsigned char dualEdgeClkSelect,
-	unsigned char hsyncEnable,
-	unsigned char vsyncEnable,
-	unsigned char deskewEnable,
-	unsigned char deskewSetting,
-	unsigned char continuousSyncEnable,
-	unsigned char pllFilterEnable,
-	unsigned char pllFilterValue
-)
+long sii164InitChip(unsigned char edgeSelect,
+		    unsigned char busSelect,
+		    unsigned char dualEdgeClkSelect,
+		    unsigned char hsyncEnable,
+		    unsigned char vsyncEnable,
+		    unsigned char deskewEnable,
+		    unsigned char deskewSetting,
+		    unsigned char continuousSyncEnable,
+		    unsigned char pllFilterEnable,
+		    unsigned char pllFilterValue)
 {
 	unsigned char config;
 
@@ -259,7 +257,6 @@ void sii164ResetChip(void)
 	sii164SetPower(1);
 }
 
-
 /*
  * sii164GetChipString
  *      This function returns a char string name of the current DVI Controller chip.
@@ -270,7 +267,6 @@ char *sii164GetChipString(void)
 	return gDviCtrlChipName;
 }
 
-
 /*
  *  sii164SetPower
  *      This function sets the power configuration of the DVI Controller Chip.
@@ -278,9 +274,7 @@ char *sii164GetChipString(void)
  *  Input:
  *      powerUp - Flag to set the power down or up
  */
-void sii164SetPower(
-	unsigned char powerUp
-)
+void sii164SetPower(unsigned char powerUp)
 {
 	unsigned char config;
 
@@ -298,18 +292,16 @@ void sii164SetPower(
 	}
 }
 
-
 /*
  *  sii164SelectHotPlugDetectionMode
  *      This function selects the mode of the hot plug detection.
  */
-static void sii164SelectHotPlugDetectionMode(
-	sii164_hot_plug_mode_t hotPlugMode
-)
+static void sii164SelectHotPlugDetectionMode(sii164_hot_plug_mode_t hotPlugMode)
 {
 	unsigned char detectReg;
 
-	detectReg = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT) & ~SII164_DETECT_MONITOR_SENSE_OUTPUT_FLAG;
+	detectReg = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT) &
+		    ~SII164_DETECT_MONITOR_SENSE_OUTPUT_FLAG;
 	switch (hotPlugMode) {
 	case SII164_HOTPLUG_DISABLE:
 		detectReg |= SII164_DETECT_MONITOR_SENSE_OUTPUT_HIGH;
@@ -336,9 +328,7 @@ static void sii164SelectHotPlugDetectionMode(
  *
  *  enableHotPlug   - Enable (=1) / disable (=0) Hot Plug detection
  */
-void sii164EnableHotPlugDetection(
-	unsigned char enableHotPlug
-)
+void sii164EnableHotPlugDetection(unsigned char enableHotPlug)
 {
 	unsigned char detectReg;
 
@@ -365,7 +355,8 @@ unsigned char sii164IsConnected(void)
 {
 	unsigned char hotPlugValue;
 
-	hotPlugValue = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT) & SII164_DETECT_HOT_PLUG_STATUS_MASK;
+	hotPlugValue = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT) &
+		       SII164_DETECT_HOT_PLUG_STATUS_MASK;
 	if (hotPlugValue == SII164_DETECT_HOT_PLUG_STATUS_ON)
 		return 1;
 	else
@@ -384,7 +375,8 @@ unsigned char sii164CheckInterrupt(void)
 {
 	unsigned char detectReg;
 
-	detectReg = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT) & SII164_DETECT_MONITOR_STATE_MASK;
+	detectReg = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT) &
+		    SII164_DETECT_MONITOR_STATE_MASK;
 	if (detectReg == SII164_DETECT_MONITOR_STATE_CHANGE)
 		return 1;
 	else
@@ -401,7 +393,8 @@ void sii164ClearInterrupt(void)
 
 	/* Clear the MDI interrupt */
 	detectReg = i2cReadReg(SII164_I2C_ADDRESS, SII164_DETECT);
-	i2cWriteReg(SII164_I2C_ADDRESS, SII164_DETECT, detectReg | SII164_DETECT_MONITOR_STATE_CLEAR);
+	i2cWriteReg(SII164_I2C_ADDRESS, SII164_DETECT,
+		    detectReg | SII164_DETECT_MONITOR_STATE_CLEAR);
 }
 
 #endif
diff --git a/drivers/staging/sm750fb/ddk750_sii164.h b/drivers/staging/sm750fb/ddk750_sii164.h
index 664ad089f753..6968cf532f16 100644
--- a/drivers/staging/sm750fb/ddk750_sii164.h
+++ b/drivers/staging/sm750fb/ddk750_sii164.h
@@ -13,18 +13,16 @@ typedef enum _sii164_hot_plug_mode_t {
 
 
 /* Silicon Image SiI164 chip prototype */
-long sii164InitChip(
-	unsigned char edgeSelect,
-	unsigned char busSelect,
-	unsigned char dualEdgeClkSelect,
-	unsigned char hsyncEnable,
-	unsigned char vsyncEnable,
-	unsigned char deskewEnable,
-	unsigned char deskewSetting,
-	unsigned char continuousSyncEnable,
-	unsigned char pllFilterEnable,
-	unsigned char pllFilterValue
-);
+long sii164InitChip(unsigned char edgeSelect,
+		    unsigned char busSelect,
+		    unsigned char dualEdgeClkSelect,
+		    unsigned char hsyncEnable,
+		    unsigned char vsyncEnable,
+		    unsigned char deskewEnable,
+		    unsigned char deskewSetting,
+		    unsigned char continuousSyncEnable,
+		    unsigned char pllFilterEnable,
+		    unsigned char pllFilterValue);
 
 unsigned short sii164GetVendorID(void);
 unsigned short sii164GetDeviceID(void);
diff --git a/drivers/staging/sm750fb/ddk750_swi2c.c b/drivers/staging/sm750fb/ddk750_swi2c.c
index a4ac07cd50cb..19c5ffc72b16 100644
--- a/drivers/staging/sm750fb/ddk750_swi2c.c
+++ b/drivers/staging/sm750fb/ddk750_swi2c.c
@@ -349,8 +349,7 @@ static unsigned char sw_i2c_read_byte(unsigned char ack)
  *      -1   - Fail to initialize the i2c
  *       0   - Success
  */
-static long sm750le_i2c_init(unsigned char clk_gpio,
-			     unsigned char data_gpio)
+static long sm750le_i2c_init(unsigned char clk_gpio, unsigned char data_gpio)
 {
 	int i;
 
@@ -388,10 +387,7 @@ static long sm750le_i2c_init(unsigned char clk_gpio,
  *      -1   - Fail to initialize the i2c
  *       0   - Success
  */
-long sm750_sw_i2c_init(
-	unsigned char clk_gpio,
-	unsigned char data_gpio
-)
+long sm750_sw_i2c_init(unsigned char clk_gpio, unsigned char data_gpio)
 {
 	int i;
 
@@ -448,10 +444,7 @@ long sm750_sw_i2c_init(
  *  Return Value:
  *      Register value
  */
-unsigned char sm750_sw_i2c_read_reg(
-	unsigned char addr,
-	unsigned char reg
-)
+unsigned char sm750_sw_i2c_read_reg(unsigned char addr, unsigned char reg)
 {
 	unsigned char data;
 
@@ -488,11 +481,9 @@ unsigned char sm750_sw_i2c_read_reg(
  *          0   - Success
  *         -1   - Fail
  */
-long sm750_sw_i2c_write_reg(
-	unsigned char addr,
-	unsigned char reg,
-	unsigned char data
-)
+long sm750_sw_i2c_write_reg(unsigned char addr,
+			    unsigned char reg,
+			    unsigned char data)
 {
 	long ret = 0;
 
diff --git a/drivers/staging/sm750fb/ddk750_swi2c.h b/drivers/staging/sm750fb/ddk750_swi2c.h
index 5a9466efc7bd..3b8a96d6d25a 100644
--- a/drivers/staging/sm750fb/ddk750_swi2c.h
+++ b/drivers/staging/sm750fb/ddk750_swi2c.h
@@ -28,10 +28,7 @@
  *      -1   - Fail to initialize the i2c
  *       0   - Success
  */
-long sm750_sw_i2c_init(
-	unsigned char clk_gpio,
-	unsigned char data_gpio
-);
+long sm750_sw_i2c_init(unsigned char clk_gpio, unsigned char data_gpio);
 
 /*
  *  This function reads the slave device's register
@@ -44,10 +41,7 @@ long sm750_sw_i2c_init(
  *  Return Value:
  *      Register value
  */
-unsigned char sm750_sw_i2c_read_reg(
-	unsigned char addr,
-	unsigned char reg
-);
+unsigned char sm750_sw_i2c_read_reg(unsigned char addr, unsigned char reg);
 
 /*
  *  This function writes a value to the slave device's register
@@ -62,10 +56,8 @@ unsigned char sm750_sw_i2c_read_reg(
  *          0   - Success
  *         -1   - Fail
  */
-long sm750_sw_i2c_write_reg(
-	unsigned char addr,
-	unsigned char reg,
-	unsigned char data
-);
+long sm750_sw_i2c_write_reg(unsigned char addr,
+			    unsigned char reg,
+			    unsigned char data);
 
 #endif  /* _SWI2C_H_ */
diff --git a/drivers/staging/sm750fb/sm750.c b/drivers/staging/sm750fb/sm750.c
index 386d4adcd91d..3aa4128703d5 100644
--- a/drivers/staging/sm750fb/sm750.c
+++ b/drivers/staging/sm750fb/sm750.c
@@ -33,7 +33,7 @@ static int g_hwcursor = 1;
 static int g_noaccel;
 static int g_nomtrr;
 static const char *g_fbmode[] = {NULL, NULL};
-static const char *g_def_fbmode = "800x600-16@60";
+static const char *g_def_fbmode = "1024x768-32@60";
 static char *g_settings;
 static int g_dualview;
 static char *g_option;
@@ -112,42 +112,42 @@ static int lynxfb_ops_cursor(struct fb_info *info, struct fb_cursor *fbcursor)
 	cursor = &crtc->cursor;
 
 	if (fbcursor->image.width > cursor->maxW ||
-	   fbcursor->image.height > cursor->maxH ||
-	   fbcursor->image.depth > 1) {
+	    fbcursor->image.height > cursor->maxH ||
+	    fbcursor->image.depth > 1) {
 		return -ENXIO;
 	}
 
 	sm750_hw_cursor_disable(cursor);
 	if (fbcursor->set & FB_CUR_SETSIZE)
 		sm750_hw_cursor_setSize(cursor,
-				  fbcursor->image.width,
-				  fbcursor->image.height);
+					fbcursor->image.width,
+					fbcursor->image.height);
 
 	if (fbcursor->set & FB_CUR_SETPOS)
 		sm750_hw_cursor_setPos(cursor,
-				 fbcursor->image.dx - info->var.xoffset,
-				 fbcursor->image.dy - info->var.yoffset);
+				       fbcursor->image.dx - info->var.xoffset,
+				       fbcursor->image.dy - info->var.yoffset);
 
 	if (fbcursor->set & FB_CUR_SETCMAP) {
 		/* get the 16bit color of kernel means */
 		u16 fg, bg;
 
 		fg = ((info->cmap.red[fbcursor->image.fg_color] & 0xf800)) |
-		      ((info->cmap.green[fbcursor->image.fg_color] & 0xfc00) >> 5) |
-		      ((info->cmap.blue[fbcursor->image.fg_color] & 0xf800) >> 11);
+		     ((info->cmap.green[fbcursor->image.fg_color] & 0xfc00) >> 5) |
+		     ((info->cmap.blue[fbcursor->image.fg_color] & 0xf800) >> 11);
 
 		bg = ((info->cmap.red[fbcursor->image.bg_color] & 0xf800)) |
-		      ((info->cmap.green[fbcursor->image.bg_color] & 0xfc00) >> 5) |
-		      ((info->cmap.blue[fbcursor->image.bg_color] & 0xf800) >> 11);
+		     ((info->cmap.green[fbcursor->image.bg_color] & 0xfc00) >> 5) |
+		     ((info->cmap.blue[fbcursor->image.bg_color] & 0xf800) >> 11);
 
 		sm750_hw_cursor_setColor(cursor, fg, bg);
 	}
 
 	if (fbcursor->set & (FB_CUR_SETSHAPE | FB_CUR_SETIMAGE)) {
 		sm750_hw_cursor_setData(cursor,
-				  fbcursor->rop,
-				  fbcursor->image.data,
-				  fbcursor->mask);
+					fbcursor->rop,
+					fbcursor->image.data,
+					fbcursor->mask);
 	}
 
 	if (fbcursor->enable)
@@ -183,19 +183,19 @@ static void lynxfb_ops_fillrect(struct fb_info *info,
 	rop = (region->rop != ROP_COPY) ? HW_ROP2_XOR : HW_ROP2_COPY;
 
 	/*
-	 * If not use spin_lock,system will die if user load driver
+	 * If not use spin_lock, system will die if user load driver
 	 * and immediately unload driver frequently (dual)
+	 * since they fb_count could change during the lifetime of
+	 * this lock, we are holding it for all cases.
 	 */
-	if (sm750_dev->fb_count > 1)
-		spin_lock(&sm750_dev->slock);
+	spin_lock(&sm750_dev->slock);
 
 	sm750_dev->accel.de_fillrect(&sm750_dev->accel,
 				     base, pitch, Bpp,
 				     region->dx, region->dy,
 				     region->width, region->height,
 				     color, rop);
-	if (sm750_dev->fb_count > 1)
-		spin_unlock(&sm750_dev->slock);
+	spin_unlock(&sm750_dev->slock);
 }
 
 static void lynxfb_ops_copyarea(struct fb_info *info,
@@ -219,17 +219,17 @@ static void lynxfb_ops_copyarea(struct fb_info *info,
 	/*
 	 * If not use spin_lock, system will die if user load driver
 	 * and immediately unload driver frequently (dual)
+	 * since they fb_count could change during the lifetime of
+	 * this lock, we are holding it for all cases.
 	 */
-	if (sm750_dev->fb_count > 1)
-		spin_lock(&sm750_dev->slock);
+	spin_lock(&sm750_dev->slock);
 
 	sm750_dev->accel.de_copyarea(&sm750_dev->accel,
 				     base, pitch, region->sx, region->sy,
 				     base, pitch, Bpp, region->dx, region->dy,
 				     region->width, region->height,
 				     HW_ROP2_COPY);
-	if (sm750_dev->fb_count > 1)
-		spin_unlock(&sm750_dev->slock);
+	spin_unlock(&sm750_dev->slock);
 }
 
 static void lynxfb_ops_imageblit(struct fb_info *info,
@@ -268,9 +268,10 @@ static void lynxfb_ops_imageblit(struct fb_info *info,
 	/*
 	 * If not use spin_lock, system will die if user load driver
 	 * and immediately unload driver frequently (dual)
+	 * since they fb_count could change during the lifetime of
+	 * this lock, we are holding it for all cases.
 	 */
-	if (sm750_dev->fb_count > 1)
-		spin_lock(&sm750_dev->slock);
+	spin_lock(&sm750_dev->slock);
 
 	sm750_dev->accel.de_imageblit(&sm750_dev->accel,
 				      image->data, image->width >> 3, 0,
@@ -278,8 +279,7 @@ static void lynxfb_ops_imageblit(struct fb_info *info,
 				      image->dx, image->dy,
 				      image->width, image->height,
 				      fgcol, bgcol, HW_ROP2_COPY);
-	if (sm750_dev->fb_count > 1)
-		spin_unlock(&sm750_dev->slock);
+	spin_unlock(&sm750_dev->slock);
 }
 
 static int lynxfb_ops_pan_display(struct fb_var_screeninfo *var,
diff --git a/drivers/staging/sm750fb/sm750.h b/drivers/staging/sm750fb/sm750.h
index 5b186dafedec..4386122799b2 100644
--- a/drivers/staging/sm750fb/sm750.h
+++ b/drivers/staging/sm750fb/sm750.h
@@ -189,14 +189,22 @@ void hw_sm750_initAccel(struct sm750_dev *sm750_dev);
 int hw_sm750_deWait(void);
 int hw_sm750le_deWait(void);
 
-int hw_sm750_output_setMode(struct lynxfb_output*, struct fb_var_screeninfo*,
-			    struct fb_fix_screeninfo*);
-int hw_sm750_crtc_checkMode(struct lynxfb_crtc*, struct fb_var_screeninfo*);
-int hw_sm750_crtc_setMode(struct lynxfb_crtc*, struct fb_var_screeninfo*,
-			  struct fb_fix_screeninfo*);
-int hw_sm750_setColReg(struct lynxfb_crtc*, ushort, ushort, ushort, ushort);
-int hw_sm750_setBLANK(struct lynxfb_output*, int);
-int hw_sm750le_setBLANK(struct lynxfb_output*, int);
+int hw_sm750_output_setMode(struct lynxfb_output *output,
+			    struct fb_var_screeninfo *var,
+			    struct fb_fix_screeninfo *fix);
+
+int hw_sm750_crtc_checkMode(struct lynxfb_crtc *crtc,
+			    struct fb_var_screeninfo *var);
+
+int hw_sm750_crtc_setMode(struct lynxfb_crtc *crtc,
+			  struct fb_var_screeninfo *var,
+			  struct fb_fix_screeninfo *fix);
+
+int hw_sm750_setColReg(struct lynxfb_crtc *crtc, ushort index,
+		       ushort red, ushort green, ushort blue);
+
+int hw_sm750_setBLANK(struct lynxfb_output *output, int blank);
+int hw_sm750le_setBLANK(struct lynxfb_output *output, int blank);
 int hw_sm750_pan_display(struct lynxfb_crtc *crtc,
 			 const struct fb_var_screeninfo *var,
 			 const struct fb_info *info);
diff --git a/drivers/staging/sm750fb/sm750_accel.c b/drivers/staging/sm750fb/sm750_accel.c
index 6be86e4963be..4b720cfa05de 100644
--- a/drivers/staging/sm750fb/sm750_accel.c
+++ b/drivers/staging/sm750fb/sm750_accel.c
@@ -42,10 +42,11 @@ void sm750_hw_de_init(struct lynx_accel *accel)
 	/* dpr1c */
 	reg =  0x3;
 
-	clr = DE_STRETCH_FORMAT_PATTERN_XY | DE_STRETCH_FORMAT_PATTERN_Y_MASK |
-		DE_STRETCH_FORMAT_PATTERN_X_MASK |
-		DE_STRETCH_FORMAT_ADDRESSING_MASK |
-		DE_STRETCH_FORMAT_SOURCE_HEIGHT_MASK;
+	clr = DE_STRETCH_FORMAT_PATTERN_XY |
+	      DE_STRETCH_FORMAT_PATTERN_Y_MASK |
+	      DE_STRETCH_FORMAT_PATTERN_X_MASK |
+	      DE_STRETCH_FORMAT_ADDRESSING_MASK |
+	      DE_STRETCH_FORMAT_SOURCE_HEIGHT_MASK;
 
 	/* DE_STRETCH bpp format need be initialized in setMode routine */
 	write_dpr(accel, DE_STRETCH_FORMAT,
@@ -84,9 +85,9 @@ void sm750_hw_set2dformat(struct lynx_accel *accel, int fmt)
 }
 
 int sm750_hw_fillrect(struct lynx_accel *accel,
-				u32 base, u32 pitch, u32 Bpp,
-				u32 x, u32 y, u32 width, u32 height,
-				u32 color, u32 rop)
+		      u32 base, u32 pitch, u32 Bpp,
+		      u32 x, u32 y, u32 width, u32 height,
+		      u32 color, u32 rop)
 {
 	u32 deCtrl;
 
diff --git a/drivers/staging/sm750fb/sm750_cursor.c b/drivers/staging/sm750fb/sm750_cursor.c
index b64dc8a4a8fb..aa47a16ac75c 100644
--- a/drivers/staging/sm750fb/sm750_cursor.c
+++ b/drivers/staging/sm750fb/sm750_cursor.c
@@ -60,15 +60,13 @@ void sm750_hw_cursor_disable(struct lynx_cursor *cursor)
 	poke32(HWC_ADDRESS, 0);
 }
 
-void sm750_hw_cursor_setSize(struct lynx_cursor *cursor,
-						int w, int h)
+void sm750_hw_cursor_setSize(struct lynx_cursor *cursor, int w, int h)
 {
 	cursor->w = w;
 	cursor->h = h;
 }
 
-void sm750_hw_cursor_setPos(struct lynx_cursor *cursor,
-						int x, int y)
+void sm750_hw_cursor_setPos(struct lynx_cursor *cursor, int x, int y)
 {
 	u32 reg;
 
@@ -77,8 +75,7 @@ void sm750_hw_cursor_setPos(struct lynx_cursor *cursor,
 	poke32(HWC_LOCATION, reg);
 }
 
-void sm750_hw_cursor_setColor(struct lynx_cursor *cursor,
-						u32 fg, u32 bg)
+void sm750_hw_cursor_setColor(struct lynx_cursor *cursor, u32 fg, u32 bg)
 {
 	u32 reg = (fg << HWC_COLOR_12_2_RGB565_SHIFT) &
 		HWC_COLOR_12_2_RGB565_MASK;
@@ -87,8 +84,8 @@ void sm750_hw_cursor_setColor(struct lynx_cursor *cursor,
 	poke32(HWC_COLOR_3, 0xffe0);
 }
 
-void sm750_hw_cursor_setData(struct lynx_cursor *cursor,
-			u16 rop, const u8 *pcol, const u8 *pmsk)
+void sm750_hw_cursor_setData(struct lynx_cursor *cursor, u16 rop,
+			     const u8 *pcol, const u8 *pmsk)
 {
 	int i, j, count, pitch, offset;
 	u8 color, mask, opr;
@@ -138,8 +135,8 @@ void sm750_hw_cursor_setData(struct lynx_cursor *cursor,
 }
 
 
-void sm750_hw_cursor_setData2(struct lynx_cursor *cursor,
-			u16 rop, const u8 *pcol, const u8 *pmsk)
+void sm750_hw_cursor_setData2(struct lynx_cursor *cursor, u16 rop,
+			      const u8 *pcol, const u8 *pmsk)
 {
 	int i, j, count, pitch, offset;
 	u8 color, mask;
diff --git a/drivers/staging/speakup/Makefile b/drivers/staging/speakup/Makefile
index c5e43a59822f..c864ea69c40d 100644
--- a/drivers/staging/speakup/Makefile
+++ b/drivers/staging/speakup/Makefile
@@ -25,6 +25,7 @@ speakup-y := \
 	kobjects.o \
 	selection.o \
 	serialio.o \
+	spk_ttyio.o \
 	synth.o \
 	thread.o \
 	varhandlers.o
diff --git a/drivers/staging/speakup/main.c b/drivers/staging/speakup/main.c
index d2ad596850f3..82e5de248947 100644
--- a/drivers/staging/speakup/main.c
+++ b/drivers/staging/speakup/main.c
@@ -1945,6 +1945,7 @@ static int handle_goto(struct vc_data *vc, u_char type, u_char ch, u_short key)
 		goto oops;
 	if (ch == 8) {
 		u16 wch;
+
 		if (num == 0)
 			return -1;
 		wch = goto_buf[--num];
@@ -2287,6 +2288,7 @@ static int vt_notifier_call(struct notifier_block *nb,
 			speakup_bs(vc);
 		} else {
 			u16 d = param->c;
+
 			speakup_con_write(vc, &d, 1);
 		}
 		break;
diff --git a/drivers/staging/speakup/serialio.c b/drivers/staging/speakup/serialio.c
index ba060d0ceca2..9cfc8142a318 100644
--- a/drivers/staging/speakup/serialio.c
+++ b/drivers/staging/speakup/serialio.c
@@ -28,11 +28,17 @@ static int timeouts;
 static int spk_serial_out(struct spk_synth *in_synth, const char ch);
 static void spk_serial_send_xchar(char ch);
 static void spk_serial_tiocmset(unsigned int set, unsigned int clear);
+static unsigned char spk_serial_in(void);
+static unsigned char spk_serial_in_nowait(void);
+static void spk_serial_flush_buffer(void);
 
 struct spk_io_ops spk_serial_io_ops = {
 	.synth_out = spk_serial_out,
 	.send_xchar = spk_serial_send_xchar,
 	.tiocmset = spk_serial_tiocmset,
+	.synth_in = spk_serial_in,
+	.synth_in_nowait = spk_serial_in_nowait,
+	.flush_buffer = spk_serial_flush_buffer,
 };
 EXPORT_SYMBOL_GPL(spk_serial_io_ops);
 
@@ -132,8 +138,8 @@ static void start_serial_interrupt(int irq)
 	outb(UART_MCR_DTR | UART_MCR_RTS | UART_MCR_OUT2,
 	     speakup_info.port_tts + UART_MCR);
 	/* Turn on Interrupts */
-	outb(UART_IER_MSI|UART_IER_RLSI|UART_IER_RDI,
-			speakup_info.port_tts + UART_IER);
+	outb(UART_IER_MSI | UART_IER_RLSI | UART_IER_RDI,
+	     speakup_info.port_tts + UART_IER);
 	inb(speakup_info.port_tts + UART_LSR);
 	inb(speakup_info.port_tts + UART_RX);
 	inb(speakup_info.port_tts + UART_IIR);
@@ -156,6 +162,7 @@ static void spk_serial_send_xchar(char ch)
 static void spk_serial_tiocmset(unsigned int set, unsigned int clear)
 {
 	int old = inb(speakup_info.port_tts + UART_MCR);
+
 	outb((old & ~clear) | set, speakup_info.port_tts + UART_MCR);
 }
 
@@ -221,7 +228,8 @@ int spk_wait_for_xmitr(struct spk_synth *in_synth)
 	}
 	while (spk_serial_tx_busy()) {
 		if (--tmout == 0) {
-			pr_warn("%s: timed out (tx busy)\n", in_synth->long_name);
+			pr_warn("%s: timed out (tx busy)\n",
+				in_synth->long_name);
 			timeouts++;
 			return 0;
 		}
@@ -240,7 +248,7 @@ int spk_wait_for_xmitr(struct spk_synth *in_synth)
 	return 1;
 }
 
-unsigned char spk_serial_in(void)
+static unsigned char spk_serial_in(void)
 {
 	int tmout = SPK_SERIAL_TIMEOUT;
 
@@ -253,9 +261,8 @@ unsigned char spk_serial_in(void)
 	}
 	return inb_p(speakup_info.port_tts + UART_RX);
 }
-EXPORT_SYMBOL_GPL(spk_serial_in);
 
-unsigned char spk_serial_in_nowait(void)
+static unsigned char spk_serial_in_nowait(void)
 {
 	unsigned char lsr;
 
@@ -264,7 +271,11 @@ unsigned char spk_serial_in_nowait(void)
 		return 0;
 	return inb_p(speakup_info.port_tts + UART_RX);
 }
-EXPORT_SYMBOL_GPL(spk_serial_in_nowait);
+
+static void spk_serial_flush_buffer(void)
+{
+	/* TODO: flush the UART 16550 buffer */
+}
 
 static int spk_serial_out(struct spk_synth *in_synth, const char ch)
 {
@@ -275,7 +286,8 @@ static int spk_serial_out(struct spk_synth *in_synth, const char ch)
 	return 0;
 }
 
-const char *spk_serial_synth_immediate(struct spk_synth *synth, const char *buff)
+const char *spk_serial_synth_immediate(struct spk_synth *synth,
+				       const char *buff)
 {
 	u_char ch;
 
diff --git a/drivers/staging/speakup/serialio.h b/drivers/staging/speakup/serialio.h
index 3ad7ff0bc3c3..89de6fff9cb2 100644
--- a/drivers/staging/speakup/serialio.h
+++ b/drivers/staging/speakup/serialio.h
@@ -8,6 +8,8 @@
 #endif
 #include <linux/serial_core.h>
 
+#include "spk_priv.h"
+
 /*
  * this is cut&paste from 8250.h. Get rid of the structure, the definitions
  * and this whole broken driver.
@@ -21,7 +23,7 @@ struct old_serial_port {
 };
 
 /* countdown values for serial timeouts in us */
-#define SPK_SERIAL_TIMEOUT 100000
+#define SPK_SERIAL_TIMEOUT SPK_SYNTH_TIMEOUT
 /* countdown values transmitter/dsr timeouts in us */
 #define SPK_XMITR_TIMEOUT 100000
 /* countdown values cts timeouts in us */
diff --git a/drivers/staging/speakup/speakup_acntsa.c b/drivers/staging/speakup/speakup_acntsa.c
index de67ffda7d45..0e10404e2e8c 100644
--- a/drivers/staging/speakup/speakup_acntsa.c
+++ b/drivers/staging/speakup/speakup_acntsa.c
@@ -96,13 +96,14 @@ static struct spk_synth synth_acntsa = {
 	.trigger = 50,
 	.jiffies = 30,
 	.full = 40000,
+	.dev_name = SYNTH_DEFAULT_DEV,
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
-	.io_ops = &spk_serial_io_ops,
+	.io_ops = &spk_ttyio_ops,
 	.probe = synth_probe,
-	.release = spk_serial_release,
-	.synth_immediate = spk_serial_synth_immediate,
+	.release = spk_ttyio_release,
+	.synth_immediate = spk_ttyio_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = spk_synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -125,7 +126,7 @@ static int synth_probe(struct spk_synth *synth)
 {
 	int failed;
 
-	failed = spk_serial_synth_probe(synth);
+	failed = spk_ttyio_synth_probe(synth);
 	if (failed == 0) {
 		synth->synth_immediate(synth, "\033=R\r");
 		mdelay(100);
@@ -135,9 +136,11 @@ static int synth_probe(struct spk_synth *synth)
 }
 
 module_param_named(ser, synth_acntsa.ser, int, 0444);
+module_param_named(dev, synth_acntsa.dev_name, charp, S_IRUGO);
 module_param_named(start, synth_acntsa.startup, short, 0444);
 
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
+MODULE_PARM_DESC(dev, "Set the device e.g. ttyUSB0, for the synthesizer.");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
 module_spk_synth(synth_acntsa);
diff --git a/drivers/staging/speakup/speakup_apollo.c b/drivers/staging/speakup/speakup_apollo.c
index cead8b1b1bfc..2edb56c8a559 100644
--- a/drivers/staging/speakup/speakup_apollo.c
+++ b/drivers/staging/speakup/speakup_apollo.c
@@ -22,9 +22,9 @@
 #include <linux/sched.h>
 #include <linux/timer.h>
 #include <linux/kthread.h>
+#include <linux/serial_reg.h>	/* for UART_MCR* constants */
 
 #include "spk_priv.h"
-#include "serialio.h"
 #include "speakup.h"
 
 #define DRV_VERSION "2.21"
@@ -105,13 +105,14 @@ static struct spk_synth synth_apollo = {
 	.trigger = 50,
 	.jiffies = 50,
 	.full = 40000,
+	.dev_name = SYNTH_DEFAULT_DEV,
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
-	.io_ops = &spk_serial_io_ops,
-	.probe = spk_serial_synth_probe,
-	.release = spk_serial_release,
-	.synth_immediate = spk_serial_synth_immediate,
+	.io_ops = &spk_ttyio_ops,
+	.probe = spk_ttyio_synth_probe,
+	.release = spk_ttyio_release,
+	.synth_immediate = spk_ttyio_synth_immediate,
 	.catch_up = do_catch_up,
 	.flush = spk_synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -199,9 +200,11 @@ static void do_catch_up(struct spk_synth *synth)
 }
 
 module_param_named(ser, synth_apollo.ser, int, 0444);
+module_param_named(dev, synth_apollo.dev_name, charp, S_IRUGO);
 module_param_named(start, synth_apollo.startup, short, 0444);
 
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
+MODULE_PARM_DESC(dev, "Set the device e.g. ttyUSB0, for the synthesizer.");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
 module_spk_synth(synth_apollo);
diff --git a/drivers/staging/speakup/speakup_audptr.c b/drivers/staging/speakup/speakup_audptr.c
index 6880352a7b74..8ae826eba71c 100644
--- a/drivers/staging/speakup/speakup_audptr.c
+++ b/drivers/staging/speakup/speakup_audptr.c
@@ -20,7 +20,6 @@
  */
 #include "spk_priv.h"
 #include "speakup.h"
-#include "serialio.h"
 
 #define DRV_VERSION "2.11"
 #define SYNTH_CLEAR 0x18 /* flush synth buffer */
@@ -101,13 +100,14 @@ static struct spk_synth synth_audptr = {
 	.trigger = 50,
 	.jiffies = 30,
 	.full = 18000,
+	.dev_name = SYNTH_DEFAULT_DEV,
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
-	.io_ops = &spk_serial_io_ops,
+	.io_ops = &spk_ttyio_ops,
 	.probe = synth_probe,
-	.release = spk_serial_release,
-	.synth_immediate = spk_serial_synth_immediate,
+	.release = spk_ttyio_release,
+	.synth_immediate = spk_ttyio_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -128,6 +128,7 @@ static struct spk_synth synth_audptr = {
 
 static void synth_flush(struct spk_synth *synth)
 {
+	synth->io_ops->flush_buffer();
 	synth->io_ops->send_xchar(SYNTH_CLEAR);
 	synth->io_ops->synth_out(synth, PROCSPEECH);
 }
@@ -138,11 +139,11 @@ static void synth_version(struct spk_synth *synth)
 	char synth_id[40] = "";
 
 	synth->synth_immediate(synth, "\x05[Q]");
-	synth_id[test] = spk_serial_in();
+	synth_id[test] = synth->io_ops->synth_in();
 	if (synth_id[test] == 'A') {
 		do {
 			/* read version string from synth */
-			synth_id[++test] = spk_serial_in();
+			synth_id[++test] = synth->io_ops->synth_in();
 		} while (synth_id[test] != '\n' && test < 32);
 		synth_id[++test] = 0x00;
 	}
@@ -154,7 +155,7 @@ static int synth_probe(struct spk_synth *synth)
 {
 	int failed;
 
-	failed = spk_serial_synth_probe(synth);
+	failed = spk_ttyio_synth_probe(synth);
 	if (failed == 0)
 		synth_version(synth);
 	synth->alive = !failed;
@@ -162,9 +163,11 @@ static int synth_probe(struct spk_synth *synth)
 }
 
 module_param_named(ser, synth_audptr.ser, int, 0444);
+module_param_named(dev, synth_audptr.dev_name, charp, S_IRUGO);
 module_param_named(start, synth_audptr.startup, short, 0444);
 
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
+MODULE_PARM_DESC(dev, "Set the device e.g. ttyUSB0, for the synthesizer.");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
 module_spk_synth(synth_audptr);
diff --git a/drivers/staging/speakup/speakup_bns.c b/drivers/staging/speakup/speakup_bns.c
index a972a5147c6b..60bcf0df8123 100644
--- a/drivers/staging/speakup/speakup_bns.c
+++ b/drivers/staging/speakup/speakup_bns.c
@@ -93,13 +93,14 @@ static struct spk_synth synth_bns = {
 	.trigger = 50,
 	.jiffies = 50,
 	.full = 40000,
+	.dev_name = SYNTH_DEFAULT_DEV,
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
-	.io_ops = &spk_serial_io_ops,
-	.probe = spk_serial_synth_probe,
-	.release = spk_serial_release,
-	.synth_immediate = spk_serial_synth_immediate,
+	.io_ops = &spk_ttyio_ops,
+	.probe = spk_ttyio_synth_probe,
+	.release = spk_ttyio_release,
+	.synth_immediate = spk_ttyio_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = spk_synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -119,9 +120,11 @@ static struct spk_synth synth_bns = {
 };
 
 module_param_named(ser, synth_bns.ser, int, 0444);
+module_param_named(dev, synth_bns.dev_name, charp, S_IRUGO);
 module_param_named(start, synth_bns.startup, short, 0444);
 
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
+MODULE_PARM_DESC(dev, "Set the device e.g. ttyUSB0, for the synthesizer.");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
 module_spk_synth(synth_bns);
diff --git a/drivers/staging/speakup/speakup_decext.c b/drivers/staging/speakup/speakup_decext.c
index c564bf8e1531..95f4b2116d0c 100644
--- a/drivers/staging/speakup/speakup_decext.c
+++ b/drivers/staging/speakup/speakup_decext.c
@@ -24,26 +24,22 @@
 #include <linux/kthread.h>
 
 #include "spk_priv.h"
-#include "serialio.h"
 #include "speakup.h"
 
 #define DRV_VERSION "2.14"
 #define SYNTH_CLEAR 0x03
 #define PROCSPEECH 0x0b
-static unsigned char last_char;
 
-static inline u_char get_last_char(void)
-{
-	u_char avail = inb_p(speakup_info.port_tts + UART_LSR) & UART_LSR_DR;
+static volatile unsigned char last_char;
 
-	if (avail)
-		last_char = inb_p(speakup_info.port_tts + UART_RX);
-	return last_char;
+static void read_buff_add(u_char ch)
+{
+	last_char = ch;
 }
 
 static inline bool synth_full(void)
 {
-	return get_last_char() == 0x13;
+	return last_char == 0x13;
 }
 
 static void do_catch_up(struct spk_synth *synth);
@@ -124,18 +120,19 @@ static struct spk_synth synth_decext = {
 	.jiffies = 50,
 	.full = 40000,
 	.flags = SF_DEC,
+	.dev_name = SYNTH_DEFAULT_DEV,
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
-	.io_ops = &spk_serial_io_ops,
-	.probe = spk_serial_synth_probe,
-	.release = spk_serial_release,
-	.synth_immediate = spk_serial_synth_immediate,
+	.io_ops = &spk_ttyio_ops,
+	.probe = spk_ttyio_synth_probe,
+	.release = spk_ttyio_release,
+	.synth_immediate = spk_ttyio_synth_immediate,
 	.catch_up = do_catch_up,
 	.flush = synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
 	.synth_adjust = NULL,
-	.read_buff_add = NULL,
+	.read_buff_add = read_buff_add,
 	.get_index = NULL,
 	.indexing = {
 		.command = NULL,
@@ -225,13 +222,16 @@ static void do_catch_up(struct spk_synth *synth)
 static void synth_flush(struct spk_synth *synth)
 {
 	in_escape = 0;
+	synth->io_ops->flush_buffer();
 	synth->synth_immediate(synth, "\033P;10z\033\\");
 }
 
 module_param_named(ser, synth_decext.ser, int, 0444);
+module_param_named(dev, synth_decext.dev_name, charp, S_IRUGO);
 module_param_named(start, synth_decext.startup, short, 0444);
 
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
+MODULE_PARM_DESC(dev, "Set the device e.g. ttyUSB0, for the synthesizer.");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
 module_spk_synth(synth_decext);
diff --git a/drivers/staging/speakup/speakup_decpc.c b/drivers/staging/speakup/speakup_decpc.c
index 5d22c3b7edd4..7a8df7dc1e38 100644
--- a/drivers/staging/speakup/speakup_decpc.c
+++ b/drivers/staging/speakup/speakup_decpc.c
@@ -84,7 +84,7 @@
 #define	CTRL_last_index		0x0b00	/* get last index spoken */
 #define	CTRL_io_priority	0x0c00	/* change i/o priority */
 #define	CTRL_free_mem		0x0d00	/* get free paragraphs on module */
-#define	CTRL_get_lang		0x0e00	/* return bit mask of loaded languages */
+#define	CTRL_get_lang		0x0e00	/* return bitmask of loaded languages */
 #define	CMD_test		0x2000	/* self-test request */
 #define	TEST_mask		0x0F00	/* isolate test field */
 #define	TEST_null		0x0000	/* no test requested */
diff --git a/drivers/staging/speakup/speakup_dectlk.c b/drivers/staging/speakup/speakup_dectlk.c
index 0cdbd5e9b36b..f06995480022 100644
--- a/drivers/staging/speakup/speakup_dectlk.c
+++ b/drivers/staging/speakup/speakup_dectlk.c
@@ -27,7 +27,6 @@
 #include <linux/kthread.h>
 #include "speakup.h"
 #include "spk_priv.h"
-#include "serialio.h"
 
 #define DRV_VERSION "2.20"
 #define SYNTH_CLEAR 0x03
@@ -42,7 +41,7 @@ static inline int synth_full(void)
 static void do_catch_up(struct spk_synth *synth);
 static void synth_flush(struct spk_synth *synth);
 static void read_buff_add(u_char c);
-static unsigned char get_index(void);
+static unsigned char get_index(struct spk_synth *synth);
 
 static int in_escape;
 static int is_flushing;
@@ -125,15 +124,16 @@ static struct spk_synth synth_dectlk = {
 	.trigger = 50,
 	.jiffies = 50,
 	.full = 40000,
+	.dev_name = SYNTH_DEFAULT_DEV,
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
 	.default_pitch = ap_defaults,
 	.default_vol = g5_defaults,
-	.io_ops = &spk_serial_io_ops,
-	.probe = spk_serial_synth_probe,
-	.release = spk_serial_release,
-	.synth_immediate = spk_serial_synth_immediate,
+	.io_ops = &spk_ttyio_ops,
+	.probe = spk_ttyio_synth_probe,
+	.release = spk_ttyio_release,
+	.synth_immediate = spk_ttyio_synth_immediate,
 	.catch_up = do_catch_up,
 	.flush = synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -163,7 +163,7 @@ static int is_indnum(u_char *ch)
 
 static u_char lastind;
 
-static unsigned char get_index(void)
+static unsigned char get_index(struct spk_synth *synth)
 {
 	u_char rv;
 
@@ -294,13 +294,16 @@ static void synth_flush(struct spk_synth *synth)
 		synth->io_ops->synth_out(synth, ']');
 	in_escape = 0;
 	is_flushing = 1;
+	synth->io_ops->flush_buffer();
 	synth->io_ops->synth_out(synth, SYNTH_CLEAR);
 }
 
 module_param_named(ser, synth_dectlk.ser, int, 0444);
+module_param_named(dev, synth_dectlk.dev_name, charp, S_IRUGO);
 module_param_named(start, synth_dectlk.startup, short, 0444);
 
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
+MODULE_PARM_DESC(dev, "Set the device e.g. ttyUSB0, for the synthesizer.");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
 module_spk_synth(synth_dectlk);
diff --git a/drivers/staging/speakup/speakup_dtlk.c b/drivers/staging/speakup/speakup_dtlk.c
index 33180937222d..8999e3eb5c26 100644
--- a/drivers/staging/speakup/speakup_dtlk.c
+++ b/drivers/staging/speakup/speakup_dtlk.c
@@ -138,7 +138,7 @@ static struct spk_synth synth_dtlk = {
 	.is_alive = spk_synth_is_alive_nop,
 	.synth_adjust = NULL,
 	.read_buff_add = NULL,
-	.get_index = spk_serial_in_nowait,
+	.get_index = spk_synth_get_index,
 	.indexing = {
 		.command = "\x01%di",
 		.lowindex = 1,
diff --git a/drivers/staging/speakup/speakup_dtlk.h b/drivers/staging/speakup/speakup_dtlk.h
index 46d885fcfb20..51ac0f2fcded 100644
--- a/drivers/staging/speakup/speakup_dtlk.h
+++ b/drivers/staging/speakup/speakup_dtlk.h
@@ -24,11 +24,11 @@
 				 * usec later.
 				 */
 #define TTS_ALMOST_FULL	0x08	/* mask for AF bit: When set to 1,
-					 * indicates that less than 300 bytes
-					 * are available in the TTS input
-					 * buffer. AF is always 0 in the PCM,
-					 * TGN and CVSD modes.
-					 */
+				 * indicates that less than 300 bytes
+				 * are available in the TTS input
+				 * buffer. AF is always 0 in the PCM,
+				 * TGN and CVSD modes.
+				 */
 #define TTS_ALMOST_EMPTY 0x04	/* mask for AE bit: When set to 1,
 				 * indicates that less than 300 bytes
 				 * are remaining in DoubleTalk's input
diff --git a/drivers/staging/speakup/speakup_dummy.c b/drivers/staging/speakup/speakup_dummy.c
index 8db7aa358f31..851953d5eefb 100644
--- a/drivers/staging/speakup/speakup_dummy.c
+++ b/drivers/staging/speakup/speakup_dummy.c
@@ -95,13 +95,14 @@ static struct spk_synth synth_dummy = {
 	.trigger = 50,
 	.jiffies = 50,
 	.full = 40000,
+	.dev_name = SYNTH_DEFAULT_DEV,
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
-	.io_ops = &spk_serial_io_ops,
-	.probe = spk_serial_synth_probe,
-	.release = spk_serial_release,
-	.synth_immediate = spk_serial_synth_immediate,
+	.io_ops = &spk_ttyio_ops,
+	.probe = spk_ttyio_synth_probe,
+	.release = spk_ttyio_release,
+	.synth_immediate = spk_ttyio_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = spk_synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -121,9 +122,11 @@ static struct spk_synth synth_dummy = {
 };
 
 module_param_named(ser, synth_dummy.ser, int, 0444);
+module_param_named(dev, synth_dummy.dev_name, charp, S_IRUGO);
 module_param_named(start, synth_dummy.startup, short, 0444);
 
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
+MODULE_PARM_DESC(dev, "Set the device e.g. ttyUSB0, for the synthesizer.");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
 module_spk_synth(synth_dummy);
diff --git a/drivers/staging/speakup/speakup_ltlk.c b/drivers/staging/speakup/speakup_ltlk.c
index 11275f49bea4..423795f88f53 100644
--- a/drivers/staging/speakup/speakup_ltlk.c
+++ b/drivers/staging/speakup/speakup_ltlk.c
@@ -20,7 +20,6 @@
  */
 #include "speakup.h"
 #include "spk_priv.h"
-#include "serialio.h"
 #include "speakup_dtlk.h" /* local header file for LiteTalk values */
 
 #define DRV_VERSION "2.11"
@@ -108,19 +107,20 @@ static struct spk_synth synth_ltlk = {
 	.trigger = 50,
 	.jiffies = 50,
 	.full = 40000,
+	.dev_name = SYNTH_DEFAULT_DEV,
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
-	.io_ops = &spk_serial_io_ops,
+	.io_ops = &spk_ttyio_ops,
 	.probe = synth_probe,
-	.release = spk_serial_release,
-	.synth_immediate = spk_serial_synth_immediate,
+	.release = spk_ttyio_release,
+	.synth_immediate = spk_ttyio_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = spk_synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
 	.synth_adjust = NULL,
 	.read_buff_add = NULL,
-	.get_index = spk_serial_in_nowait,
+	.get_index = spk_synth_get_index,
 	.indexing = {
 		.command = "\x01%di",
 		.lowindex = 1,
@@ -141,7 +141,7 @@ static void synth_interrogate(struct spk_synth *synth)
 
 	synth->synth_immediate(synth, "\x18\x01?");
 	for (i = 0; i < 50; i++) {
-		buf[i] = spk_serial_in();
+		buf[i] = synth->io_ops->synth_in();
 		if (i > 2 && buf[i] == 0x7f)
 			break;
 	}
@@ -159,7 +159,7 @@ static int synth_probe(struct spk_synth *synth)
 {
 	int failed = 0;
 
-	failed = spk_serial_synth_probe(synth);
+	failed = spk_ttyio_synth_probe(synth);
 	if (failed == 0)
 		synth_interrogate(synth);
 	synth->alive = !failed;
@@ -167,9 +167,11 @@ static int synth_probe(struct spk_synth *synth)
 }
 
 module_param_named(ser, synth_ltlk.ser, int, 0444);
+module_param_named(dev, synth_ltlk.dev_name, charp, S_IRUGO);
 module_param_named(start, synth_ltlk.startup, short, 0444);
 
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
+MODULE_PARM_DESC(dev, "Set the device e.g. ttyUSB0, for the synthesizer.");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
 module_spk_synth(synth_ltlk);
diff --git a/drivers/staging/speakup/speakup_soft.c b/drivers/staging/speakup/speakup_soft.c
index e454f5685f70..d99daf69e501 100644
--- a/drivers/staging/speakup/speakup_soft.c
+++ b/drivers/staging/speakup/speakup_soft.c
@@ -36,7 +36,7 @@
 static int softsynth_probe(struct spk_synth *synth);
 static void softsynth_release(void);
 static int softsynth_is_alive(struct spk_synth *synth);
-static unsigned char get_index(void);
+static unsigned char get_index(struct spk_synth *synth);
 
 static struct miscdevice synth_device, synthu_device;
 static int init_pos;
@@ -340,7 +340,7 @@ static unsigned int softsynth_poll(struct file *fp, struct poll_table_struct *wa
 	return ret;
 }
 
-static unsigned char get_index(void)
+static unsigned char get_index(struct spk_synth *synth)
 {
 	int rv;
 
diff --git a/drivers/staging/speakup/speakup_spkout.c b/drivers/staging/speakup/speakup_spkout.c
index d95c375a0736..9ca21edc42ce 100644
--- a/drivers/staging/speakup/speakup_spkout.c
+++ b/drivers/staging/speakup/speakup_spkout.c
@@ -20,7 +20,6 @@
  */
 #include "spk_priv.h"
 #include "speakup.h"
-#include "serialio.h"
 
 #define DRV_VERSION "2.11"
 #define SYNTH_CLEAR 0x18
@@ -99,19 +98,20 @@ static struct spk_synth synth_spkout = {
 	.trigger = 50,
 	.jiffies = 50,
 	.full = 40000,
+	.dev_name = SYNTH_DEFAULT_DEV,
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
-	.io_ops = &spk_serial_io_ops,
-	.probe = spk_serial_synth_probe,
-	.release = spk_serial_release,
-	.synth_immediate = spk_serial_synth_immediate,
+	.io_ops = &spk_ttyio_ops,
+	.probe = spk_ttyio_synth_probe,
+	.release = spk_ttyio_release,
+	.synth_immediate = spk_ttyio_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
 	.synth_adjust = NULL,
 	.read_buff_add = NULL,
-	.get_index = spk_serial_in_nowait,
+	.get_index = spk_synth_get_index,
 	.indexing = {
 		.command = "\x05[%c",
 		.lowindex = 1,
@@ -126,13 +126,16 @@ static struct spk_synth synth_spkout = {
 
 static void synth_flush(struct spk_synth *synth)
 {
+	synth->io_ops->flush_buffer();
 	synth->io_ops->send_xchar(SYNTH_CLEAR);
 }
 
 module_param_named(ser, synth_spkout.ser, int, 0444);
+module_param_named(dev, synth_spkout.dev_name, charp, S_IRUGO);
 module_param_named(start, synth_spkout.startup, short, 0444);
 
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
+MODULE_PARM_DESC(dev, "Set the device e.g. ttyUSB0, for the synthesizer.");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
 module_spk_synth(synth_spkout);
diff --git a/drivers/staging/speakup/speakup_txprt.c b/drivers/staging/speakup/speakup_txprt.c
index 3f531fb99fd3..831ee404e7a1 100644
--- a/drivers/staging/speakup/speakup_txprt.c
+++ b/drivers/staging/speakup/speakup_txprt.c
@@ -92,13 +92,14 @@ static struct spk_synth synth_txprt = {
 	.trigger = 50,
 	.jiffies = 50,
 	.full = 40000,
+	.dev_name = SYNTH_DEFAULT_DEV,
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
-	.io_ops = &spk_serial_io_ops,
-	.probe = spk_serial_synth_probe,
-	.release = spk_serial_release,
-	.synth_immediate = spk_serial_synth_immediate,
+	.io_ops = &spk_ttyio_ops,
+	.probe = spk_ttyio_synth_probe,
+	.release = spk_ttyio_release,
+	.synth_immediate = spk_ttyio_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = spk_synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -118,9 +119,11 @@ static struct spk_synth synth_txprt = {
 };
 
 module_param_named(ser, synth_txprt.ser, int, 0444);
+module_param_named(dev, synth_txprt.dev_name, charp, S_IRUGO);
 module_param_named(start, synth_txprt.startup, short, 0444);
 
 MODULE_PARM_DESC(ser, "Set the serial port for the synthesizer (0-based).");
+MODULE_PARM_DESC(dev, "Set the device e.g. ttyUSB0, for the synthesizer.");
 MODULE_PARM_DESC(start, "Start the synthesizer once it is loaded.");
 
 module_spk_synth(synth_txprt);
diff --git a/drivers/staging/speakup/spk_priv.h b/drivers/staging/speakup/spk_priv.h
index 995f586bddcd..87b6a0a4c54d 100644
--- a/drivers/staging/speakup/spk_priv.h
+++ b/drivers/staging/speakup/spk_priv.h
@@ -39,13 +39,15 @@
 #endif
 
 #define KT_SPKUP 15
+#define SPK_SYNTH_TIMEOUT 100000 /* in micro-seconds */
+#define SYNTH_DEFAULT_DEV "ttyS0"
+#define SYNTH_DEFAULT_SER 0
 
 const struct old_serial_port *spk_serial_init(int index);
 void spk_stop_serial_interrupt(void);
 int spk_wait_for_xmitr(struct spk_synth *in_synth);
-unsigned char spk_serial_in(void);
-unsigned char spk_serial_in_nowait(void);
 void spk_serial_release(void);
+void spk_ttyio_release(void);
 
 void synth_buffer_skip_nonlatin1(void);
 u16 synth_buffer_getc(void);
@@ -58,9 +60,12 @@ ssize_t spk_var_store(struct kobject *kobj, struct kobj_attribute *attr,
 		      const char *buf, size_t count);
 
 int spk_serial_synth_probe(struct spk_synth *synth);
+int spk_ttyio_synth_probe(struct spk_synth *synth);
 const char *spk_serial_synth_immediate(struct spk_synth *synth, const char *buff);
+const char *spk_ttyio_synth_immediate(struct spk_synth *synth, const char *buff);
 void spk_do_catch_up(struct spk_synth *synth);
 void spk_synth_flush(struct spk_synth *synth);
+unsigned char spk_synth_get_index(struct spk_synth *synth);
 int spk_synth_is_alive_nop(struct spk_synth *synth);
 int spk_synth_is_alive_restart(struct spk_synth *synth);
 __printf(1, 2)
@@ -79,5 +84,6 @@ extern struct speakup_info_t speakup_info;
 extern struct var_t synth_time_vars[];
 
 extern struct spk_io_ops spk_serial_io_ops;
+extern struct spk_io_ops spk_ttyio_ops;
 
 #endif
diff --git a/drivers/staging/speakup/spk_ttyio.c b/drivers/staging/speakup/spk_ttyio.c
new file mode 100644
index 000000000000..ed8e96b06ead
--- /dev/null
+++ b/drivers/staging/speakup/spk_ttyio.c
@@ -0,0 +1,320 @@
+#include <linux/types.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <linux/slab.h>
+
+#include "speakup.h"
+#include "spk_types.h"
+#include "spk_priv.h"
+
+#define DEV_PREFIX_LP "lp"
+
+static const char * const lp_supported[] = { "acntsa", "bns", "dummy",
+	"txprt" };
+
+struct spk_ldisc_data {
+	char buf;
+	struct semaphore sem;
+	bool buf_free;
+};
+
+static struct spk_synth *spk_ttyio_synth;
+static struct tty_struct *speakup_tty;
+
+static int ser_to_dev(int ser, dev_t *dev_no)
+{
+	if (ser < 0 || ser > (255 - 64)) {
+		pr_err("speakup: Invalid ser param. Must be between 0 and 191 inclusive.\n");
+		return -EINVAL;
+	}
+
+	*dev_no = MKDEV(4, (64 + ser));
+	return 0;
+}
+
+static int get_dev_to_use(struct spk_synth *synth, dev_t *dev_no)
+{
+	/* use ser only when dev is not specified */
+	if (strcmp(synth->dev_name, SYNTH_DEFAULT_DEV) ||
+	    synth->ser == SYNTH_DEFAULT_SER) {
+		/* for /dev/lp* check if synth is supported */
+		if (strncmp(synth->dev_name, DEV_PREFIX_LP,
+		    strlen(DEV_PREFIX_LP)) == 0)
+			if (match_string(lp_supported, ARRAY_SIZE(lp_supported),
+			    synth->name) < 0)  {
+				int i;
+
+				pr_err("speakup: lp* is only supported on:");
+				for (i = 0; i < ARRAY_SIZE(lp_supported); i++)
+					pr_cont(" %s", lp_supported[i]);
+				pr_cont("\n");
+
+				return -ENOTSUPP;
+			}
+
+		return tty_dev_name_to_number(synth->dev_name, dev_no);
+	}
+
+	return ser_to_dev(synth->ser, dev_no);
+}
+
+static int spk_ttyio_ldisc_open(struct tty_struct *tty)
+{
+	struct spk_ldisc_data *ldisc_data;
+
+	if (tty->ops->write == NULL)
+		return -EOPNOTSUPP;
+	speakup_tty = tty;
+
+	ldisc_data = kmalloc(sizeof(struct spk_ldisc_data), GFP_KERNEL);
+	if (!ldisc_data) {
+		pr_err("speakup: Failed to allocate ldisc_data.\n");
+		return -ENOMEM;
+	}
+
+	sema_init(&ldisc_data->sem, 0);
+	ldisc_data->buf_free = true;
+	speakup_tty->disc_data = ldisc_data;
+
+	return 0;
+}
+
+static void spk_ttyio_ldisc_close(struct tty_struct *tty)
+{
+	kfree(speakup_tty->disc_data);
+	speakup_tty = NULL;
+}
+
+static int spk_ttyio_receive_buf2(struct tty_struct *tty,
+		const unsigned char *cp, char *fp, int count)
+{
+	struct spk_ldisc_data *ldisc_data = tty->disc_data;
+
+	if (spk_ttyio_synth->read_buff_add) {
+		int i;
+
+		for (i = 0; i < count; i++)
+			spk_ttyio_synth->read_buff_add(cp[i]);
+
+		return count;
+	}
+
+	if (!ldisc_data->buf_free)
+		/* ttyio_in will tty_schedule_flip */
+		return 0;
+
+	/* Make sure the consumer has read buf before we have seen
+	 * buf_free == true and overwrite buf */
+	mb();
+
+	ldisc_data->buf = cp[0];
+	ldisc_data->buf_free = false;
+	up(&ldisc_data->sem);
+
+	return 1;
+}
+
+static struct tty_ldisc_ops spk_ttyio_ldisc_ops = {
+	.owner          = THIS_MODULE,
+	.magic          = TTY_LDISC_MAGIC,
+	.name           = "speakup_ldisc",
+	.open           = spk_ttyio_ldisc_open,
+	.close          = spk_ttyio_ldisc_close,
+	.receive_buf2	= spk_ttyio_receive_buf2,
+};
+
+static int spk_ttyio_out(struct spk_synth *in_synth, const char ch);
+static void spk_ttyio_send_xchar(char ch);
+static void spk_ttyio_tiocmset(unsigned int set, unsigned int clear);
+static unsigned char spk_ttyio_in(void);
+static unsigned char spk_ttyio_in_nowait(void);
+static void spk_ttyio_flush_buffer(void);
+
+struct spk_io_ops spk_ttyio_ops = {
+	.synth_out = spk_ttyio_out,
+	.send_xchar = spk_ttyio_send_xchar,
+	.tiocmset = spk_ttyio_tiocmset,
+	.synth_in = spk_ttyio_in,
+	.synth_in_nowait = spk_ttyio_in_nowait,
+	.flush_buffer = spk_ttyio_flush_buffer,
+};
+EXPORT_SYMBOL_GPL(spk_ttyio_ops);
+
+static inline void get_termios(struct tty_struct *tty, struct ktermios *out_termios)
+{
+	down_read(&tty->termios_rwsem);
+	*out_termios = tty->termios;
+	up_read(&tty->termios_rwsem);
+}
+
+static int spk_ttyio_initialise_ldisc(struct spk_synth *synth)
+{
+	int ret = 0;
+	struct tty_struct *tty;
+	struct ktermios tmp_termios;
+	dev_t dev;
+
+	ret = tty_register_ldisc(N_SPEAKUP, &spk_ttyio_ldisc_ops);
+	if (ret) {
+		pr_err("Error registering line discipline.\n");
+		return ret;
+	}
+
+	ret = get_dev_to_use(synth, &dev);
+	if (ret)
+		return ret;
+
+	tty = tty_open_by_driver(dev, NULL, NULL);
+	if (IS_ERR(tty))
+		return PTR_ERR(tty);
+
+	if (tty->ops->open)
+		ret = tty->ops->open(tty, NULL);
+	else
+		ret = -ENODEV;
+
+	if (ret) {
+		tty_unlock(tty);
+		return ret;
+	}
+
+	clear_bit(TTY_HUPPED, &tty->flags);
+	/* ensure hardware flow control is enabled */
+	get_termios(tty, &tmp_termios);
+	if (!(tmp_termios.c_cflag & CRTSCTS)) {
+		tmp_termios.c_cflag |= CRTSCTS;
+		tty_set_termios(tty, &tmp_termios);
+		/*
+		 * check c_cflag to see if it's updated as tty_set_termios may not return
+		 * error even when no tty bits are changed by the request.
+		 */
+		get_termios(tty, &tmp_termios);
+		if (!(tmp_termios.c_cflag & CRTSCTS))
+			pr_warn("speakup: Failed to set hardware flow control\n");
+	}
+
+	tty_unlock(tty);
+
+	ret = tty_set_ldisc(tty, N_SPEAKUP);
+
+	return ret;
+}
+
+static int spk_ttyio_out(struct spk_synth *in_synth, const char ch)
+{
+	if (in_synth->alive && speakup_tty && speakup_tty->ops->write) {
+		int ret = speakup_tty->ops->write(speakup_tty, &ch, 1);
+
+		if (ret == 0)
+			/* No room */
+			return 0;
+		if (ret < 0) {
+			pr_warn("%s: I/O error, deactivating speakup\n", in_synth->long_name);
+			/* No synth any more, so nobody will restart TTYs, and we thus
+			 * need to do it ourselves.  Now that there is no synth we can
+			 * let application flood anyway
+			 */
+			in_synth->alive = 0;
+			speakup_start_ttys();
+			return 0;
+		}
+		return 1;
+	}
+	return 0;
+}
+
+static void spk_ttyio_send_xchar(char ch)
+{
+	speakup_tty->ops->send_xchar(speakup_tty, ch);
+}
+
+static void spk_ttyio_tiocmset(unsigned int set, unsigned int clear)
+{
+	speakup_tty->ops->tiocmset(speakup_tty, set, clear);
+}
+
+static unsigned char ttyio_in(int timeout)
+{
+	struct spk_ldisc_data *ldisc_data = speakup_tty->disc_data;
+	char rv;
+
+	if (down_timeout(&ldisc_data->sem, usecs_to_jiffies(timeout)) == -ETIME) {
+		if (timeout)
+			pr_warn("spk_ttyio: timeout (%d)  while waiting for input\n",
+				timeout);
+		return 0xff;
+	}
+
+	rv = ldisc_data->buf;
+	/* Make sure we have read buf before we set buf_free to let
+	 * the producer overwrite it */
+	mb();
+	ldisc_data->buf_free = true;
+	/* Let TTY push more characters */
+	tty_schedule_flip(speakup_tty->port);
+
+	return rv;
+}
+
+static unsigned char spk_ttyio_in(void)
+{
+	return ttyio_in(SPK_SYNTH_TIMEOUT);
+}
+
+static unsigned char spk_ttyio_in_nowait(void)
+{
+	u8 rv = ttyio_in(0);
+
+	return (rv == 0xff) ? 0 : rv;
+}
+
+static void spk_ttyio_flush_buffer(void)
+{
+	if (speakup_tty->ops->flush_buffer)
+		speakup_tty->ops->flush_buffer(speakup_tty);
+}
+
+int spk_ttyio_synth_probe(struct spk_synth *synth)
+{
+	int rv = spk_ttyio_initialise_ldisc(synth);
+
+	if (rv)
+		return rv;
+
+	synth->alive = 1;
+	spk_ttyio_synth = synth;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(spk_ttyio_synth_probe);
+
+void spk_ttyio_release(void)
+{
+	if (!speakup_tty)
+		return;
+
+	tty_lock(speakup_tty);
+
+	if (speakup_tty->ops->close)
+		speakup_tty->ops->close(speakup_tty, NULL);
+
+	tty_ldisc_flush(speakup_tty);
+	tty_unlock(speakup_tty);
+	tty_ldisc_release(speakup_tty);
+}
+EXPORT_SYMBOL_GPL(spk_ttyio_release);
+
+const char *spk_ttyio_synth_immediate(struct spk_synth *synth, const char *buff)
+{
+	u_char ch;
+
+	while ((ch = *buff)) {
+		if (ch == '\n')
+			ch = synth->procspeech;
+		if (tty_write_room(speakup_tty) < 1 || !synth->io_ops->synth_out(synth, ch))
+			return buff;
+		buff++;
+	}
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(spk_ttyio_synth_immediate);
diff --git a/drivers/staging/speakup/spk_types.h b/drivers/staging/speakup/spk_types.h
index c156975392c8..22f657d45e46 100644
--- a/drivers/staging/speakup/spk_types.h
+++ b/drivers/staging/speakup/spk_types.h
@@ -152,6 +152,9 @@ struct spk_io_ops {
 	int (*synth_out)(struct spk_synth *synth, const char ch);
 	void (*send_xchar)(char ch);
 	void (*tiocmset)(unsigned int set, unsigned int clear);
+	unsigned char (*synth_in)(void);
+	unsigned char (*synth_in_nowait)(void);
+	void (*flush_buffer)(void);
 };
 
 struct spk_synth {
@@ -166,6 +169,7 @@ struct spk_synth {
 	int jiffies;
 	int full;
 	int ser;
+	char *dev_name;
 	short flags;
 	short startup;
 	const int checkval; /* for validating a proper synth module */
@@ -182,7 +186,7 @@ struct spk_synth {
 	int (*is_alive)(struct spk_synth *synth);
 	int (*synth_adjust)(struct st_var_header *var);
 	void (*read_buff_add)(u_char);
-	unsigned char (*get_index)(void);
+	unsigned char (*get_index)(struct spk_synth *synth);
 	struct synth_indexing indexing;
 	int alive;
 	struct attribute_group attributes;
diff --git a/drivers/staging/speakup/synth.c b/drivers/staging/speakup/synth.c
index 352e9eebc3de..a1ca68c76579 100644
--- a/drivers/staging/speakup/synth.c
+++ b/drivers/staging/speakup/synth.c
@@ -120,10 +120,17 @@ EXPORT_SYMBOL_GPL(spk_do_catch_up);
 
 void spk_synth_flush(struct spk_synth *synth)
 {
+	synth->io_ops->flush_buffer();
 	synth->io_ops->synth_out(synth, synth->clear);
 }
 EXPORT_SYMBOL_GPL(spk_synth_flush);
 
+unsigned char spk_synth_get_index(struct spk_synth *synth)
+{
+	return synth->io_ops->synth_in_nowait();
+}
+EXPORT_SYMBOL_GPL(spk_synth_get_index);
+
 int spk_synth_is_alive_nop(struct spk_synth *synth)
 {
 	synth->alive = 1;
@@ -249,7 +256,7 @@ void spk_reset_index_count(int sc)
 	if (first)
 		first = 0;
 	else
-		synth->get_index();
+		synth->get_index(synth);
 	index_count = 0;
 	sentence_count = sc;
 }
@@ -282,7 +289,7 @@ void synth_insert_next_index(int sent_num)
 
 void spk_get_index_count(int *linecount, int *sentcount)
 {
-	int ind = synth->get_index();
+	int ind = synth->get_index(synth);
 
 	if (ind) {
 		sentence_count = ind % 10;
@@ -438,10 +445,15 @@ int synth_add(struct spk_synth *in_synth)
 		mutex_unlock(&spk_mutex);
 		return -1;
 	}
-	synths[i++] = in_synth;
-	synths[i] = NULL;
+
 	if (in_synth->startup)
 		status = do_synth_init(in_synth);
+
+	if (!status) {
+		synths[i++] = in_synth;
+		synths[i] = NULL;
+	}
+
 	mutex_unlock(&spk_mutex);
 	return status;
 }
diff --git a/drivers/staging/typec/fusb302/fusb302.c b/drivers/staging/typec/fusb302/fusb302.c
index 4a356e509fe4..03a3809d18f0 100644
--- a/drivers/staging/typec/fusb302/fusb302.c
+++ b/drivers/staging/typec/fusb302/fusb302.c
@@ -1039,8 +1039,8 @@ static int fusb302_pd_send_message(struct fusb302_chip *chip,
 	}
 	/* packsym tells the FUSB302 chip that the next X bytes are payload */
 	buf[pos++] = FUSB302_TKN_PACKSYM | (len & 0x1F);
-	buf[pos++] = msg->header & 0xFF;
-	buf[pos++] = (msg->header >> 8) & 0xFF;
+	memcpy(&buf[pos], &msg->header, sizeof(msg->header));
+	pos += sizeof(msg->header);
 
 	len -= 2;
 	memcpy(&buf[pos], msg->payload, len);
diff --git a/drivers/staging/unisys/Documentation/overview.txt b/drivers/staging/unisys/Documentation/overview.txt
index 1146c1cf5c2a..e0466bfada2f 100644
--- a/drivers/staging/unisys/Documentation/overview.txt
+++ b/drivers/staging/unisys/Documentation/overview.txt
@@ -221,7 +221,7 @@ The following files exist under /sys/devices/visorbus<x>/vbus<x>:dev<y>:
 
 The visorhba driver registers with visorbus as the function driver to
 handle virtual scsi disk devices, specified using the
-SPAR_VHBA_CHANNEL_PROTOCOL_UUID type in the visorbus_register_visor_driver()
+VISOR_VHBA_CHANNEL_UUID type in the visorbus_register_visor_driver()
 call. visorhba uses scsi_add_host() to expose a Linux block device
 (e.g., /sys/block/) in the guest environment for each s-Par virtual device.
 
@@ -240,7 +240,7 @@ When compiled as a module, visorhba can be autoloaded by visorbus in
 standard udev/systemd environments, as it includes the modules.alias
 definition:
 
-    "visorbus:"+SPAR_VHBA_CHANNEL_PROTOCOL_UUID_STR
+    "visorbus:"+VISOR_VHBA_CHANNEL_UUID_STR
 
 i.e.:
 
@@ -252,7 +252,7 @@ i.e.:
 
 The visornic driver registers with visorbus as the function driver to
 handle virtual network devices, specified using the
-SPAR_VNIC_CHANNEL_PROTOCOL_UUID type in the visorbus_register_visor_driver()
+VISOR_VNIC_CHANNEL_UUID type in the visorbus_register_visor_driver()
 call. visornic uses register_netdev() to expose a Linux device of class net
 (e.g., /sys/class/net/) in the guest environment for each s-Par virtual
 device.
@@ -270,7 +270,7 @@ When compiled as a module, visornic can be autoloaded by visorbus in
 standard udev/systemd environments, as it includes the modules.alias
 definition:
 
-    "visorbus:"+SPAR_VNIC_CHANNEL_PROTOCOL_UUID_STR
+    "visorbus:"+VISOR_VNIC_CHANNEL_UUID_STR
 
 i.e.:
 
@@ -282,7 +282,7 @@ i.e.:
 
 The visorinput driver registers with visorbus as the function driver to
 handle human input devices, specified using the
-SPAR_KEYBOARD_CHANNEL_PROTOCOL_UUID and SPAR_MOUSE_CHANNEL_PROTOCOL_UUID
+VISOR_KEYBOARD_CHANNEL_UUID and VISOR_MOUSE_CHANNEL_UUID
 types in the visorbus_register_visor_driver() call. visorinput uses
 input_register_device() to expose devices of class input
 (e.g., /sys/class/input/) for virtual keyboard and virtual mouse devices.
@@ -307,8 +307,8 @@ When compiled as a module, visorinput can be autoloaded by visorbus in
 standard udev/systemd environments, as it includes the modules.alias
 definition:
 
-    "visorbus:"+SPAR_MOUSE_CHANNEL_PROTOCOL_UUID_STR
-    "visorbus:"+SPAR_KEYBOARD_CHANNEL_PROTOCOL_UUID_STR
+    "visorbus:"+VISOR_MOUSE_CHANNEL_UUID_STR
+    "visorbus:"+VISOR_KEYBOARD_CHANNEL_UUID_STR
 
 i.e.:
 
diff --git a/drivers/staging/unisys/include/channel.h b/drivers/staging/unisys/include/channel.h
index 057421eeb1ae..692efcb38245 100644
--- a/drivers/staging/unisys/include/channel.h
+++ b/drivers/staging/unisys/include/channel.h
@@ -32,7 +32,7 @@
 #define COVER(v, d) ((d) * DIV_ROUND_UP(v, d))
 #endif
 
-#define ULTRA_CHANNEL_PROTOCOL_SIGNATURE SIGNATURE_32('E', 'C', 'N', 'L')
+#define VISOR_CHANNEL_SIGNATURE SIGNATURE_32('E', 'C', 'N', 'L')
 
 enum channel_serverstate {
 	CHANNELSRV_UNINITIALIZED = 0,	/* channel is in an undefined state */
@@ -59,10 +59,10 @@ enum channel_clientstate {
 				/* access channel anytime */
 };
 
-#define SPAR_CHANNEL_SERVER_READY(ch) \
+#define VISOR_CHANNEL_SERVER_READY(ch) \
 	(readl(&(ch)->srv_state) == CHANNELSRV_READY)
 
-#define ULTRA_VALID_CHANNELCLI_TRANSITION(o, n) \
+#define VISOR_VALID_CHANNELCLI_TRANSITION(o, n) \
 	(((((o) == CHANNELCLI_DETACHED) && ((n) == CHANNELCLI_DISABLED)) || \
 	  (((o) == CHANNELCLI_ATTACHING) && ((n) == CHANNELCLI_DISABLED)) || \
 	  (((o) == CHANNELCLI_ATTACHED) && ((n) == CHANNELCLI_DISABLED)) || \
@@ -80,33 +80,33 @@ enum channel_clientstate {
 	  (((o) == CHANNELCLI_BUSY) && ((n) == CHANNELCLI_OWNED)) || (0)) \
 	 ? (1) : (0))
 
-/* Values for ULTRA_CHANNEL_PROTOCOL.CliErrorBoot: */
+/* Values for VISORA_CHANNEL_PROTOCOL.CliErrorBoot: */
 /* throttling invalid boot channel statetransition error due to client
  * disabled
  */
-#define ULTRA_CLIERRORBOOT_THROTTLEMSG_DISABLED 0x01
+#define VISOR_CLIERRORBOOT_THROTTLEMSG_DISABLED 0x01
 
 /* throttling invalid boot channel statetransition error due to client
  * not attached
  */
-#define ULTRA_CLIERRORBOOT_THROTTLEMSG_NOTATTACHED 0x02
+#define VISOR_CLIERRORBOOT_THROTTLEMSG_NOTATTACHED 0x02
 
 /* throttling invalid boot channel statetransition error due to busy channel */
-#define ULTRA_CLIERRORBOOT_THROTTLEMSG_BUSY 0x04
+#define VISOR_CLIERRORBOOT_THROTTLEMSG_BUSY 0x04
 
-/* Values for ULTRA_CHANNEL_PROTOCOL.Features: This define exists so
+/* Values for VISOR_CHANNEL_PROTOCOL.Features: This define exists so
  * that windows guest can look at the FeatureFlags in the io channel,
  * and configure the windows driver to use interrupts or not based on
  * this setting.  This flag is set in uislib after the
- * ULTRA_VHBA_init_channel is called.  All feature bits for all
+ * VISOR_VHBA_init_channel is called.  All feature bits for all
  * channels should be defined here.  The io channel feature bits are
  * defined right here
  */
-#define ULTRA_IO_DRIVER_ENABLES_INTS (0x1ULL << 1)
-#define ULTRA_IO_CHANNEL_IS_POLLING (0x1ULL << 3)
-#define ULTRA_IO_IOVM_IS_OK_WITH_DRIVER_DISABLING_INTS (0x1ULL << 4)
-#define ULTRA_IO_DRIVER_DISABLES_INTS (0x1ULL << 5)
-#define ULTRA_IO_DRIVER_SUPPORTS_ENHANCED_RCVBUF_CHECKING (0x1ULL << 6)
+#define VISOR_DRIVER_ENABLES_INTS (0x1ULL << 1)
+#define VISOR_CHANNEL_IS_POLLING (0x1ULL << 3)
+#define VISOR_IOVM_OK_DRIVER_DISABLING_INTS (0x1ULL << 4)
+#define VISOR_DRIVER_DISABLES_INTS (0x1ULL << 5)
+#define VISOR_DRIVER_ENHANCED_RCVBUF_CHECKING (0x1ULL << 6)
 
 /* Common Channel Header */
 struct channel_header {
@@ -156,7 +156,7 @@ struct channel_header {
 	u8 recover_channel;
 } __packed;
 
-#define ULTRA_CHANNEL_ENABLE_INTS (0x1ULL << 0)
+#define VISOR_CHANNEL_ENABLE_INTS (0x1ULL << 0)
 
 /* Subheader for the Signal Type variation of the Common Channel */
 struct signal_queue_header {
@@ -204,12 +204,12 @@ struct signal_queue_header {
  * is used to pass the EFI_DIAG_CAPTURE_PROTOCOL needed to log messages.
  */
 static inline int
-spar_check_channel(struct channel_header *ch,
-		   uuid_le expected_uuid,
-		   char *chname,
-		   u64 expected_min_bytes,
-		   u32 expected_version,
-		   u64 expected_signature)
+visor_check_channel(struct channel_header *ch,
+		    uuid_le expected_uuid,
+		    char *chname,
+		    u64 expected_min_bytes,
+		    u32 expected_version,
+		    u64 expected_signature)
 {
 	if (uuid_le_cmp(expected_uuid, NULL_UUID_LE) != 0) {
 		/* caller wants us to verify type GUID */
@@ -254,27 +254,25 @@ spar_check_channel(struct channel_header *ch,
  */
 
 /* {414815ed-c58c-11da-95a9-00e08161165f} */
-#define SPAR_VHBA_CHANNEL_PROTOCOL_UUID \
+#define VISOR_VHBA_CHANNEL_UUID \
 	UUID_LE(0x414815ed, 0xc58c, 0x11da, \
 		0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
-static const uuid_le spar_vhba_channel_protocol_uuid =
-	SPAR_VHBA_CHANNEL_PROTOCOL_UUID;
-#define SPAR_VHBA_CHANNEL_PROTOCOL_UUID_STR \
+static const uuid_le visor_vhba_channel_uuid = VISOR_VHBA_CHANNEL_UUID;
+#define VISOR_VHBA_CHANNEL_UUID_STR \
 	"414815ed-c58c-11da-95a9-00e08161165f"
 
 /* {8cd5994d-c58e-11da-95a9-00e08161165f} */
-#define SPAR_VNIC_CHANNEL_PROTOCOL_UUID \
+#define VISOR_VNIC_CHANNEL_UUID \
 	UUID_LE(0x8cd5994d, 0xc58e, 0x11da, \
 		0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
-static const uuid_le spar_vnic_channel_protocol_uuid =
-	SPAR_VNIC_CHANNEL_PROTOCOL_UUID;
-#define SPAR_VNIC_CHANNEL_PROTOCOL_UUID_STR \
+static const uuid_le visor_vnic_channel_uuid = VISOR_VNIC_CHANNEL_UUID;
+#define VISOR_VNIC_CHANNEL_UUID_STR \
 	"8cd5994d-c58e-11da-95a9-00e08161165f"
 
 /* {72120008-4AAB-11DC-8530-444553544200} */
-#define SPAR_SIOVM_UUID \
+#define VISOR_SIOVM_UUID \
 	UUID_LE(0x72120008, 0x4AAB, 0x11DC, \
 		0x85, 0x30, 0x44, 0x45, 0x53, 0x54, 0x42, 0x00)
-static const uuid_le spar_siovm_uuid = SPAR_SIOVM_UUID;
+static const uuid_le visor_siovm_uuid = VISOR_SIOVM_UUID;
 
 #endif
diff --git a/drivers/staging/unisys/include/iochannel.h b/drivers/staging/unisys/include/iochannel.h
index 9bde848a321c..c7cb3fbde7b2 100644
--- a/drivers/staging/unisys/include/iochannel.h
+++ b/drivers/staging/unisys/include/iochannel.h
@@ -34,10 +34,9 @@
 #include <linux/dma-direction.h>
 #include "channel.h"
 
-#define ULTRA_VHBA_CHANNEL_PROTOCOL_SIGNATURE ULTRA_CHANNEL_PROTOCOL_SIGNATURE
-#define ULTRA_VNIC_CHANNEL_PROTOCOL_SIGNATURE ULTRA_CHANNEL_PROTOCOL_SIGNATURE
-#define ULTRA_VSWITCH_CHANNEL_PROTOCOL_SIGNATURE \
-	ULTRA_CHANNEL_PROTOCOL_SIGNATURE
+#define VISOR_VHBA_CHANNEL_SIGNATURE VISOR_CHANNEL_SIGNATURE
+#define VISOR_VNIC_CHANNEL_SIGNATURE VISOR_CHANNEL_SIGNATURE
+#define VISOR_VSWITCH_CHANNEL_SIGNATURE VISOR_CHANNEL_SIGNATURE
 
 /*
  * Must increment these whenever you insert or delete fields within this channel
@@ -46,21 +45,21 @@
  * usually add fields to the END of the channel struct without needing to
  * increment this.
  */
-#define ULTRA_VHBA_CHANNEL_PROTOCOL_VERSIONID 2
-#define ULTRA_VNIC_CHANNEL_PROTOCOL_VERSIONID 2
-#define ULTRA_VSWITCH_CHANNEL_PROTOCOL_VERSIONID 1
-
-#define SPAR_VHBA_CHANNEL_OK_CLIENT(ch) \
-	(spar_check_channel(ch, spar_vhba_channel_protocol_uuid, \
-			    "vhba", MIN_IO_CHANNEL_SIZE,	\
-			    ULTRA_VHBA_CHANNEL_PROTOCOL_VERSIONID, \
-			    ULTRA_VHBA_CHANNEL_PROTOCOL_SIGNATURE))
-
-#define SPAR_VNIC_CHANNEL_OK_CLIENT(ch) \
-	(spar_check_channel(ch, spar_vnic_channel_protocol_uuid, \
-			    "vnic", MIN_IO_CHANNEL_SIZE,	\
-			    ULTRA_VNIC_CHANNEL_PROTOCOL_VERSIONID, \
-			    ULTRA_VNIC_CHANNEL_PROTOCOL_SIGNATURE))
+#define VISOR_VHBA_CHANNEL_VERSIONID 2
+#define VISOR_VNIC_CHANNEL_VERSIONID 2
+#define VISOR_VSWITCH_CHANNEL_VERSIONID 1
+
+#define VISOR_VHBA_CHANNEL_OK_CLIENT(ch) \
+	(visor_check_channel(ch, visor_vhba_channel_uuid, \
+			     "vhba", MIN_IO_CHANNEL_SIZE, \
+			     VISOR_VHBA_CHANNEL_VERSIONID, \
+			     VISOR_VHBA_CHANNEL_SIGNATURE))
+
+#define VISOR_VNIC_CHANNEL_OK_CLIENT(ch) \
+	(visor_check_channel(ch, visor_vnic_channel_uuid, \
+			     "vnic", MIN_IO_CHANNEL_SIZE, \
+			     VISOR_VNIC_CHANNEL_VERSIONID, \
+			     VISOR_VNIC_CHANNEL_SIGNATURE))
 
 /*
  * Everything necessary to handle SCSI & NIC traffic between Guest Partition and
@@ -530,7 +529,7 @@ struct iochannel_vnic {
  * this header there is a large region of memory which contains the command and
  * response queues as specified in cmd_q and rsp_q SIGNAL_QUEUE_HEADERS.
  */
-struct spar_io_channel_protocol {
+struct visor_io_channel {
 	struct channel_header channel_header;
 	struct signal_queue_header cmd_q;
 	struct signal_queue_header rsp_q;
diff --git a/drivers/staging/unisys/visorbus/controlvmchannel.h b/drivers/staging/unisys/visorbus/controlvmchannel.h
index 274f72422166..ed045eff0e33 100644
--- a/drivers/staging/unisys/visorbus/controlvmchannel.h
+++ b/drivers/staging/unisys/visorbus/controlvmchannel.h
@@ -19,12 +19,11 @@
 #include "channel.h"
 
 /* {2B3C2D10-7EF5-4ad8-B966-3448B7386B3D} */
-#define SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID \
+#define VISOR_CONTROLVM_CHANNEL_UUID \
 	UUID_LE(0x2b3c2d10, 0x7ef5, 0x4ad8, \
 		0xb9, 0x66, 0x34, 0x48, 0xb7, 0x38, 0x6b, 0x3d)
 
-#define ULTRA_CONTROLVM_CHANNEL_PROTOCOL_SIGNATURE \
-	ULTRA_CHANNEL_PROTOCOL_SIGNATURE
+#define VISOR_CONTROLVM_CHANNEL_SIGNATURE VISOR_CHANNEL_SIGNATURE
 #define CONTROLVM_MESSAGE_MAX 64
 
 /* Must increment this whenever you insert or delete fields within
@@ -33,15 +32,15 @@
  * software.  Note that you can usually add fields to the END of the
  * channel struct withOUT needing to increment this.
  */
-#define ULTRA_CONTROLVM_CHANNEL_PROTOCOL_VERSIONID 1
+#define VISOR_CONTROLVM_CHANNEL_VERSIONID 1
 
-#define SPAR_CONTROLVM_CHANNEL_OK_CLIENT(ch) \
-	(spar_check_channel(ch, \
-			    SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID, \
-			    "controlvm", \
-			    sizeof(struct spar_controlvm_channel_protocol), \
-			    ULTRA_CONTROLVM_CHANNEL_PROTOCOL_VERSIONID, \
-			    ULTRA_CONTROLVM_CHANNEL_PROTOCOL_SIGNATURE))
+#define VISOR_CONTROLVM_CHANNEL_OK_CLIENT(ch) \
+	(visor_check_channel(ch, \
+			     VISOR_CONTROLVM_CHANNEL_UUID, \
+			     "controlvm", \
+			     sizeof(struct visor_controlvm_channel), \
+			     VISOR_CONTROLVM_CHANNEL_VERSIONID, \
+			     VISOR_CONTROLVM_CHANNEL_SIGNATURE))
 
 /* Defines for various channel queues */
 #define CONTROLVM_QUEUE_REQUEST	 0
@@ -52,7 +51,7 @@
 /* Max num of messages stored during IOVM creation to be reused after crash */
 #define CONTROLVM_CRASHMSG_MAX 2
 
-struct spar_segment_state  {
+struct visor_segment_state  {
 	/* Bit 0: May enter other states */
 	u16 enabled:1;
 	/* Bit 1: Assigned to active partition */
@@ -76,15 +75,15 @@ struct spar_segment_state  {
  */
 } __packed;
 
-static const struct spar_segment_state segment_state_running = {
+static const struct visor_segment_state segment_state_running = {
 	1, 1, 1, 0, 1, 1, 1, 1
 };
 
-static const struct spar_segment_state segment_state_paused = {
+static const struct visor_segment_state segment_state_paused = {
 	1, 1, 1, 0, 1, 1, 1, 0
 };
 
-static const struct spar_segment_state segment_state_standby = {
+static const struct visor_segment_state segment_state_standby = {
 	1, 1, 0, 0, 1, 1, 1, 0
 };
 
@@ -149,7 +148,7 @@ struct irq_info {
 	u8 reserved[3];	/* Natural alignment purposes */
 } __packed;
 
-struct efi_spar_indication  {
+struct efi_visor_indication  {
 	u64 boot_to_fw_ui:1;		/* Bit 0: Stop in uefi ui */
 	u64 clear_nvram:1;		/* Bit 1: Clear NVRAM */
 	u64 clear_cmos:1;		/* Bit 2: Clear CMOS */
@@ -158,9 +157,9 @@ struct efi_spar_indication  {
 	u64 reserved:60;		/* Natural alignment */
 } __packed;
 
-enum ultra_chipset_feature {
-	ULTRA_CHIPSET_FEATURE_REPLY = 0x00000001,
-	ULTRA_CHIPSET_FEATURE_PARA_HOTPLUG = 0x00000002,
+enum visor_chipset_feature {
+	VISOR_CHIPSET_FEATURE_REPLY = 0x00000001,
+	VISOR_CHIPSET_FEATURE_PARA_HOTPLUG = 0x00000002,
 };
 
 /* This is the common structure that is at the beginning of every
@@ -298,13 +297,13 @@ struct controlvm_message_packet  {
 			/* for CONTROLVM_DEVICE_RECONFIGURE */
 		struct  {
 			u32 bus_no;
-			struct spar_segment_state state;
+			struct visor_segment_state state;
 			u8 reserved[2];	/* Natural alignment purposes */
 		} __packed bus_change_state; /* for CONTROLVM_BUS_CHANGESTATE */
 		struct  {
 			u32 bus_no;
 			u32 dev_no;
-			struct spar_segment_state state;
+			struct visor_segment_state state;
 			struct  {
 				/* =1 if message is for a physical device */
 				u32 phys_device:1;
@@ -317,7 +316,7 @@ struct controlvm_message_packet  {
 		struct  {
 			u32 bus_no;
 			u32 dev_no;
-			struct spar_segment_state state;
+			struct visor_segment_state state;
 			u8 reserved[6];	/* Natural alignment purposes */
 		} __packed device_change_state_event;
 			/* for CONTROLVM_DEVICE_CHANGESTATE_EVENT */
@@ -326,7 +325,7 @@ struct controlvm_message_packet  {
 			u32 bus_count;
 			/* indicates the max number of switches */
 			u32 switch_count;
-			enum ultra_chipset_feature features;
+			enum visor_chipset_feature features;
 			u32 platform_number;	/* Platform Number */
 		} __packed init_chipset;	/* for CONTROLVM_CHIPSET_INIT */
 		struct  {
@@ -349,7 +348,7 @@ struct controlvm_message {
 	struct controlvm_message_packet cmd;
 } __packed;
 
-struct spar_controlvm_channel_protocol {
+struct visor_controlvm_channel {
 	struct channel_header header;
 	u64 gp_controlvm;	/* guest phys addr of this channel */
 	u64 gp_partition_tables;/* guest phys addr of partition tables */
@@ -371,7 +370,7 @@ struct spar_controlvm_channel_protocol {
 	u32 message_count;		/* CONTROLVM_MESSAGE_MAX */
 	u64 gp_smbios_table;		/* guest phys addr of SMBIOS tables */
 	u64 gp_physical_smbios_table;	/* guest phys addr of SMBIOS table  */
-	/* ULTRA_MAX_GUESTS_PER_SERVICE */
+	/* VISOR_MAX_GUESTS_PER_SERVICE */
 	char gp_reserved[2688];
 
 	/* guest physical address of EFI firmware image base  */
@@ -402,11 +401,10 @@ struct spar_controlvm_channel_protocol {
 	u32 installation_text_id;	/* Id of string to display */
 	/* Number of remaining installation  steps (for progress bars) */
 	u16 installation_remaining_steps;
-	/* ULTRA_TOOL_ACTIONS Installation Action field */
+	/* VISOR_TOOL_ACTIONS Installation Action field */
 	u8 tool_action;
 	u8 reserved;		/* alignment */
-	struct efi_spar_indication efi_spar_ind;
-	struct efi_spar_indication efi_spar_ind_supported;
+	struct efi_visor_indication efi_visor_ind;
 	u32 sp_reserved;
 	/* Force signals to begin on 128-byte cache line */
 	u8 reserved2[28];
@@ -444,7 +442,7 @@ struct spar_controlvm_channel_protocol {
  * of total_length should equal PayloadBytes. The format of the strings at
  * PayloadVmOffset will take different forms depending on the message.
  */
-struct spar_controlvm_parameters_header {
+struct visor_controlvm_parameters_header {
 	u32 total_length;
 	u32 header_length;
 	u32 connection_offset;
diff --git a/drivers/staging/unisys/visorbus/vbuschannel.h b/drivers/staging/unisys/visorbus/vbuschannel.h
index f0ef5ecf3d7d..01d7d517dba7 100644
--- a/drivers/staging/unisys/visorbus/vbuschannel.h
+++ b/drivers/staging/unisys/visorbus/vbuschannel.h
@@ -27,13 +27,12 @@
 #include "channel.h"
 
 /* {193b331b-c58f-11da-95a9-00e08161165f} */
-#define SPAR_VBUS_CHANNEL_PROTOCOL_UUID \
+#define VISOR_VBUS_CHANNEL_UUID \
 	UUID_LE(0x193b331b, 0xc58f, 0x11da, \
 		0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
-static const uuid_le spar_vbus_channel_protocol_uuid =
-	SPAR_VBUS_CHANNEL_PROTOCOL_UUID;
+static const uuid_le visor_vbus_channel_uuid = VISOR_VBUS_CHANNEL_UUID;
 
-#define SPAR_VBUS_CHANNEL_PROTOCOL_SIGNATURE ULTRA_CHANNEL_PROTOCOL_SIGNATURE
+#define VISOR_VBUS_CHANNEL_SIGNATURE VISOR_CHANNEL_SIGNATURE
 
 /* Must increment this whenever you insert or delete fields within this channel
  * struct.  Also increment whenever you change the meaning of fields within this
@@ -41,7 +40,7 @@ static const uuid_le spar_vbus_channel_protocol_uuid =
  * usually add fields to the END of the channel struct withOUT needing to
  * increment this.
  */
-#define SPAR_VBUS_CHANNEL_PROTOCOL_VERSIONID 1
+#define VISOR_VBUS_CHANNEL_VERSIONID 1
 
 /*
  * An array of this struct is present in the channel area for each vbus.
@@ -49,16 +48,16 @@ static const uuid_le spar_vbus_channel_protocol_uuid =
  * It is filled in by the client side to provide info about the device
  * and driver from the client's perspective.
  */
-struct ultra_vbus_deviceinfo {
+struct visor_vbus_deviceinfo {
 	u8 devtype[16];		/* short string identifying the device type */
 	u8 drvname[16];		/* driver .sys file name */
 	u8 infostrs[96];	/* kernel version */
 	u8 reserved[128];	/* pad size to 256 bytes */
 } __packed;
 
-struct spar_vbus_headerinfo {
+struct visor_vbus_headerinfo {
 	u32 struct_bytes;	/* size of this struct in bytes */
-	u32 device_info_struct_bytes;	/* sizeof(ULTRA_VBUS_DEVICEINFO) */
+	u32 device_info_struct_bytes;	/* sizeof(VISOR_VBUS_DEVICEINFO) */
 	u32 dev_info_count;	/* num of items in DevInfo member */
 	/* (this is the allocated size) */
 	u32 chp_info_offset;	/* byte offset from beginning of this struct */
@@ -70,15 +69,15 @@ struct spar_vbus_headerinfo {
 	u8 reserved[104];
 } __packed;
 
-struct spar_vbus_channel_protocol {
+struct visor_vbus_channel {
 	struct channel_header channel_header;	/* initialized by server */
-	struct spar_vbus_headerinfo hdr_info;	/* initialized by server */
+	struct visor_vbus_headerinfo hdr_info;	/* initialized by server */
 	/* the remainder of this channel is filled in by the client */
-	struct ultra_vbus_deviceinfo chp_info;
+	struct visor_vbus_deviceinfo chp_info;
 	/* describes client chipset device and driver */
-	struct ultra_vbus_deviceinfo bus_info;
+	struct visor_vbus_deviceinfo bus_info;
 	/* describes client bus device and driver */
-	struct ultra_vbus_deviceinfo dev_info[0];
+	struct visor_vbus_deviceinfo dev_info[0];
 	/* describes client device and driver for each device on the bus */
 } __packed;
 
diff --git a/drivers/staging/unisys/visorbus/visorbus_main.c b/drivers/staging/unisys/visorbus/visorbus_main.c
index a692561c81c8..1c785dd19ddd 100644
--- a/drivers/staging/unisys/visorbus/visorbus_main.c
+++ b/drivers/staging/unisys/visorbus/visorbus_main.c
@@ -64,9 +64,9 @@ static const struct attribute_group *visorbus_dev_groups[] = {
 };
 
 /* filled in with info about parent chipset driver when we register with it */
-static struct ultra_vbus_deviceinfo chipset_driverinfo;
+static struct visor_vbus_deviceinfo chipset_driverinfo;
 /* filled in with info about this driver, wrt it servicing client busses */
-static struct ultra_vbus_deviceinfo clientbus_driverinfo;
+static struct visor_vbus_deviceinfo clientbus_driverinfo;
 
 /* list of visor_device structs, linked via .list_all */
 static LIST_HEAD(list_all_bus_instances);
@@ -356,9 +356,9 @@ static const struct attribute_group *visorbus_groups[] = {
  *  /sys/kernel/debug/visorbus/visorbus<n>.
  */
 /*
- * vbuschannel_print_devinfo() - format a struct ultra_vbus_deviceinfo
+ * vbuschannel_print_devinfo() - format a struct visor_vbus_deviceinfo
  *                               and write it to a seq_file
- * @devinfo: the struct ultra_vbus_deviceinfo to format
+ * @devinfo: the struct visor_vbus_deviceinfo to format
  * @seq: seq_file to write to
  * @devix: the device index to be included in the output data, or -1 if no
  *         device index is to be included
@@ -366,7 +366,7 @@ static const struct attribute_group *visorbus_groups[] = {
  * Reads @devInfo, and writes it in human-readable notation to @seq.
  */
 static void
-vbuschannel_print_devinfo(struct ultra_vbus_deviceinfo *devinfo,
+vbuschannel_print_devinfo(struct visor_vbus_deviceinfo *devinfo,
 			  struct seq_file *seq, int devix)
 {
 	if (!isprint(devinfo->devtype[0]))
@@ -397,7 +397,7 @@ static int client_bus_info_debugfs_show(struct seq_file *seq, void *v)
 
 	int i;
 	unsigned long off;
-	struct ultra_vbus_deviceinfo dev_info;
+	struct visor_vbus_deviceinfo dev_info;
 
 	if (!channel)
 		return 0;
@@ -407,16 +407,14 @@ static int client_bus_info_debugfs_show(struct seq_file *seq, void *v)
 		   ((vdev->name) ? (char *)(vdev->name) : ""),
 		   vdev->chipset_bus_no);
 	if (visorchannel_read(channel,
-			      offsetof(struct spar_vbus_channel_protocol,
-				       chp_info),
+			      offsetof(struct visor_vbus_channel, chp_info),
 			      &dev_info, sizeof(dev_info)) >= 0)
 		vbuschannel_print_devinfo(&dev_info, seq, -1);
 	if (visorchannel_read(channel,
-			      offsetof(struct spar_vbus_channel_protocol,
-				       bus_info),
+			      offsetof(struct visor_vbus_channel, bus_info),
 			      &dev_info, sizeof(dev_info)) >= 0)
 		vbuschannel_print_devinfo(&dev_info, seq, -1);
-	off = offsetof(struct spar_vbus_channel_protocol, dev_info);
+	off = offsetof(struct visor_vbus_channel, dev_info);
 	i = 0;
 	while (off + sizeof(dev_info) <= visorchannel_get_nbytes(channel)) {
 		if (visorchannel_read(channel, off, &dev_info,
@@ -684,16 +682,16 @@ remove_visor_device(struct visor_device *dev)
 
 static int
 get_vbus_header_info(struct visorchannel *chan,
-		     struct spar_vbus_headerinfo *hdr_info)
+		     struct visor_vbus_headerinfo *hdr_info)
 {
 	int err;
 
-	if (!spar_check_channel(visorchannel_get_header(chan),
-				spar_vbus_channel_protocol_uuid,
-				"vbus",
-				sizeof(struct spar_vbus_channel_protocol),
-				SPAR_VBUS_CHANNEL_PROTOCOL_VERSIONID,
-				SPAR_VBUS_CHANNEL_PROTOCOL_SIGNATURE))
+	if (!visor_check_channel(visorchannel_get_header(chan),
+				 visor_vbus_channel_uuid,
+				 "vbus",
+				 sizeof(struct visor_vbus_channel),
+				 VISOR_VBUS_CHANNEL_VERSIONID,
+				 VISOR_VBUS_CHANNEL_SIGNATURE))
 		return -EINVAL;
 
 	err = visorchannel_read(chan, sizeof(struct channel_header), hdr_info,
@@ -701,11 +699,11 @@ get_vbus_header_info(struct visorchannel *chan,
 	if (err < 0)
 		return err;
 
-	if (hdr_info->struct_bytes < sizeof(struct spar_vbus_headerinfo))
+	if (hdr_info->struct_bytes < sizeof(struct visor_vbus_headerinfo))
 		return -EINVAL;
 
 	if (hdr_info->device_info_struct_bytes <
-	    sizeof(struct ultra_vbus_deviceinfo))
+	    sizeof(struct visor_vbus_deviceinfo))
 		return -EINVAL;
 
 	return 0;
@@ -713,7 +711,7 @@ get_vbus_header_info(struct visorchannel *chan,
 
 /*
  * write_vbus_chp_info() - write the contents of <info> to the struct
- *                         spar_vbus_channel_protocol.chp_info
+ *                         visor_vbus_channel.chp_info
  * @chan:     indentifies the s-Par channel that will be updated
  * @hdr_info: used to find appropriate channel offset to write data
  * @info:     contains the information to write
@@ -726,8 +724,8 @@ get_vbus_header_info(struct visorchannel *chan,
  */
 static void
 write_vbus_chp_info(struct visorchannel *chan,
-		    struct spar_vbus_headerinfo *hdr_info,
-		    struct ultra_vbus_deviceinfo *info)
+		    struct visor_vbus_headerinfo *hdr_info,
+		    struct visor_vbus_deviceinfo *info)
 {
 	int off = sizeof(struct channel_header) + hdr_info->chp_info_offset;
 
@@ -739,7 +737,7 @@ write_vbus_chp_info(struct visorchannel *chan,
 
 /*
  * write_vbus_bus_info() - write the contents of <info> to the struct
- *                         spar_vbus_channel_protocol.bus_info
+ *                         visor_vbus_channel.bus_info
  * @chan:     indentifies the s-Par channel that will be updated
  * @hdr_info: used to find appropriate channel offset to write data
  * @info:     contains the information to write
@@ -752,8 +750,8 @@ write_vbus_chp_info(struct visorchannel *chan,
  */
 static void
 write_vbus_bus_info(struct visorchannel *chan,
-		    struct spar_vbus_headerinfo *hdr_info,
-		    struct ultra_vbus_deviceinfo *info)
+		    struct visor_vbus_headerinfo *hdr_info,
+		    struct visor_vbus_deviceinfo *info)
 {
 	int off = sizeof(struct channel_header) + hdr_info->bus_info_offset;
 
@@ -765,7 +763,7 @@ write_vbus_bus_info(struct visorchannel *chan,
 
 /*
  * write_vbus_dev_info() - write the contents of <info> to the struct
- *                         spar_vbus_channel_protocol.dev_info[<devix>]
+ *                         visor_vbus_channel.dev_info[<devix>]
  * @chan:     indentifies the s-Par channel that will be updated
  * @hdr_info: used to find appropriate channel offset to write data
  * @info:     contains the information to write
@@ -779,8 +777,8 @@ write_vbus_bus_info(struct visorchannel *chan,
  */
 static void
 write_vbus_dev_info(struct visorchannel *chan,
-		    struct spar_vbus_headerinfo *hdr_info,
-		    struct ultra_vbus_deviceinfo *info, unsigned int devix)
+		    struct visor_vbus_headerinfo *hdr_info,
+		    struct visor_vbus_deviceinfo *info, unsigned int devix)
 {
 	int off =
 	    (sizeof(struct channel_header) + hdr_info->dev_info_offset) +
@@ -793,10 +791,10 @@ write_vbus_dev_info(struct visorchannel *chan,
 }
 
 static void bus_device_info_init(
-		struct ultra_vbus_deviceinfo *bus_device_info_ptr,
+		struct visor_vbus_deviceinfo *bus_device_info_ptr,
 		const char *dev_type, const char *drv_name)
 {
-	memset(bus_device_info_ptr, 0, sizeof(struct ultra_vbus_deviceinfo));
+	memset(bus_device_info_ptr, 0, sizeof(struct visor_vbus_deviceinfo));
 	snprintf(bus_device_info_ptr->devtype,
 		 sizeof(bus_device_info_ptr->devtype),
 		 "%s", (dev_type) ? dev_type : "unknownType");
@@ -823,9 +821,9 @@ fix_vbus_dev_info(struct visor_device *visordev)
 	struct visor_driver *visordrv;
 	u32 bus_no = visordev->chipset_bus_no;
 	u32 dev_no = visordev->chipset_dev_no;
-	struct ultra_vbus_deviceinfo dev_info;
+	struct visor_vbus_deviceinfo dev_info;
 	const char *chan_type_name = NULL;
-	struct spar_vbus_headerinfo *hdr_info;
+	struct visor_vbus_headerinfo *hdr_info;
 
 	if (!visordev->device.driver)
 		return;
@@ -833,7 +831,7 @@ fix_vbus_dev_info(struct visor_device *visordev)
 	bdev = visorbus_get_device_by_id(bus_no, BUS_ROOT_DEVICE, NULL);
 	if (!bdev)
 		return;
-	hdr_info = (struct spar_vbus_headerinfo *)bdev->vbus_hdr_info;
+	hdr_info = (struct visor_vbus_headerinfo *)bdev->vbus_hdr_info;
 	if (!hdr_info)
 		return;
 	visordrv = to_visor_driver(visordev->device.driver);
@@ -981,18 +979,18 @@ int visorbus_register_visor_driver(struct visor_driver *drv)
 EXPORT_SYMBOL_GPL(visorbus_register_visor_driver);
 
 /*
- * create_bus_instance() - create a device instance for the visor bus itself
+ * visorbus_create_instance() - create a device instance for the visorbus itself
  * @dev: struct visor_device indicating the bus instance
  *
  * Return: 0 for success, otherwise negative errno value indicating reason for
  *         failure
  */
 static int
-create_bus_instance(struct visor_device *dev)
+visorbus_create_instance(struct visor_device *dev)
 {
 	int id = dev->chipset_bus_no;
 	int err;
-	struct spar_vbus_headerinfo *hdr_info;
+	struct visor_vbus_headerinfo *hdr_info;
 
 	hdr_info = kzalloc(sizeof(*hdr_info), GFP_KERNEL);
 	if (!hdr_info)
@@ -1032,16 +1030,16 @@ create_bus_instance(struct visor_device *dev)
 err_debugfs_dir:
 	debugfs_remove_recursive(dev->debugfs_dir);
 	kfree(hdr_info);
-	dev_err(&dev->device, "create_bus_instance failed: %d\n", err);
+	dev_err(&dev->device, "visorbus_create_instance failed: %d\n", err);
 	return err;
 }
 
 /*
- * remove_bus_instance() - remove a device instance for the visor bus itself
+ * visorbus_remove_instance() - remove a device instance for the visorbus itself
  * @dev: struct visor_device indentifying the bus to remove
  */
 static void
-remove_bus_instance(struct visor_device *dev)
+visorbus_remove_instance(struct visor_device *dev)
 {
 	/*
 	 * Note that this will result in the release method for
@@ -1061,7 +1059,7 @@ remove_bus_instance(struct visor_device *dev)
 }
 
 /*
- * remove_all_visor_devices() - remove all child visor bus device instances
+ * remove_all_visor_devices() - remove all child visorbus device instances
  */
 static void
 remove_all_visor_devices(void)
@@ -1077,29 +1075,29 @@ remove_all_visor_devices(void)
 }
 
 int
-chipset_bus_create(struct visor_device *dev)
+visorchipset_bus_create(struct visor_device *dev)
 {
 	int err;
 
-	err = create_bus_instance(dev);
+	err = visorbus_create_instance(dev);
 
 	if (err < 0)
 		return err;
 
-	bus_create_response(dev, err);
+	visorbus_create_response(dev, err);
 
 	return 0;
 }
 
 void
-chipset_bus_destroy(struct visor_device *dev)
+visorchipset_bus_destroy(struct visor_device *dev)
 {
-	remove_bus_instance(dev);
-	bus_destroy_response(dev, 0);
+	visorbus_remove_instance(dev);
+	visorbus_destroy_response(dev, 0);
 }
 
 int
-chipset_device_create(struct visor_device *dev_info)
+visorchipset_device_create(struct visor_device *dev_info)
 {
 	int err;
 
@@ -1107,17 +1105,17 @@ chipset_device_create(struct visor_device *dev_info)
 	if (err < 0)
 		return err;
 
-	device_create_response(dev_info, err);
+	visorbus_device_create_response(dev_info, err);
 
 	return 0;
 }
 
 void
-chipset_device_destroy(struct visor_device *dev_info)
+visorchipset_device_destroy(struct visor_device *dev_info)
 {
 	remove_visor_device(dev_info);
 
-	device_destroy_response(dev_info, 0);
+	visorbus_device_destroy_response(dev_info, 0);
 }
 
 /*
@@ -1137,7 +1135,7 @@ pause_state_change_complete(struct visor_device *dev, int status)
 
 	dev->pausing = false;
 
-	device_pause_response(dev, status);
+	visorbus_device_pause_response(dev, status);
 }
 
 /*
@@ -1162,12 +1160,12 @@ resume_state_change_complete(struct visor_device *dev, int status)
 	 * which will presumably want to send some sort of response to
 	 * the initiator.
 	 */
-	device_resume_response(dev, status);
+	visorbus_device_resume_response(dev, status);
 }
 
 /*
- * initiate_chipset_device_pause_resume() - start a pause or resume operation
- *                                          for a visor device
+ * visorchipset_initiate_device_pause_resume() - start a pause or resume
+ *                                               operation for a visor device
  * @dev: struct visor_device identifying the device being paused or resumed
  * @is_pause: true to indicate pause operation, false to indicate resume
  *
@@ -1177,7 +1175,8 @@ resume_state_change_complete(struct visor_device *dev, int status)
  * resume_state_change_complete().
  */
 static int
-initiate_chipset_device_pause_resume(struct visor_device *dev, bool is_pause)
+visorchipset_initiate_device_pause_resume(struct visor_device *dev,
+					  bool is_pause)
 {
 	int err;
 	struct visor_driver *drv = NULL;
@@ -1211,7 +1210,7 @@ initiate_chipset_device_pause_resume(struct visor_device *dev, bool is_pause)
 }
 
 /**
- * chipset_device_pause() - start a pause operation for a visor device
+ * visorchipset_device_pause() - start a pause operation for a visor device
  * @dev_info: struct visor_device identifying the device being paused
  *
  * Tell the subordinate function driver for a specific device to pause
@@ -1219,11 +1218,11 @@ initiate_chipset_device_pause_resume(struct visor_device *dev, bool is_pause)
  * via a callback function; see pause_state_change_complete().
  */
 int
-chipset_device_pause(struct visor_device *dev_info)
+visorchipset_device_pause(struct visor_device *dev_info)
 {
 	int err;
 
-	err = initiate_chipset_device_pause_resume(dev_info, true);
+	err = visorchipset_initiate_device_pause_resume(dev_info, true);
 
 	if (err < 0) {
 		dev_info->pausing = false;
@@ -1234,7 +1233,7 @@ chipset_device_pause(struct visor_device *dev_info)
 }
 
 /**
- * chipset_device_resume() - start a resume operation for a visor device
+ * visorchipset_device_resume() - start a resume operation for a visor device
  * @dev_info: struct visor_device identifying the device being resumed
  *
  * Tell the subordinate function driver for a specific device to resume
@@ -1242,11 +1241,11 @@ chipset_device_pause(struct visor_device *dev_info)
  * via a callback function; see resume_state_change_complete().
  */
 int
-chipset_device_resume(struct visor_device *dev_info)
+visorchipset_device_resume(struct visor_device *dev_info)
 {
 	int err;
 
-	err = initiate_chipset_device_pause_resume(dev_info, false);
+	err = visorchipset_initiate_device_pause_resume(dev_info, false);
 
 	if (err < 0) {
 		dev_info->resuming = false;
@@ -1289,7 +1288,7 @@ visorbus_exit(void)
 		struct visor_device *dev = list_entry(listentry,
 						      struct visor_device,
 						      list_all);
-		remove_bus_instance(dev);
+		visorbus_remove_instance(dev);
 	}
 
 	bus_unregister(&visorbus_type);
diff --git a/drivers/staging/unisys/visorbus/visorbus_private.h b/drivers/staging/unisys/visorbus/visorbus_private.h
index 9f030b1f589f..98a5af19189d 100644
--- a/drivers/staging/unisys/visorbus/visorbus_private.h
+++ b/drivers/staging/unisys/visorbus/visorbus_private.h
@@ -27,19 +27,19 @@
  * command line
  */
 
-int chipset_bus_create(struct visor_device *bus_info);
-void chipset_bus_destroy(struct visor_device *bus_info);
-int chipset_device_create(struct visor_device *dev_info);
-void chipset_device_destroy(struct visor_device *dev_info);
-int chipset_device_pause(struct visor_device *dev_info);
-int chipset_device_resume(struct visor_device *dev_info);
+int visorchipset_bus_create(struct visor_device *bus_info);
+void visorchipset_bus_destroy(struct visor_device *bus_info);
+int visorchipset_device_create(struct visor_device *dev_info);
+void visorchipset_device_destroy(struct visor_device *dev_info);
+int visorchipset_device_pause(struct visor_device *dev_info);
+int visorchipset_device_resume(struct visor_device *dev_info);
 
-void bus_create_response(struct visor_device *p, int response);
-void bus_destroy_response(struct visor_device *p, int response);
-void device_create_response(struct visor_device *p, int response);
-void device_destroy_response(struct visor_device *p, int response);
-void device_resume_response(struct visor_device *p, int response);
-void device_pause_response(struct visor_device *p, int response);
+void visorbus_create_response(struct visor_device *p, int response);
+void visorbus_destroy_response(struct visor_device *p, int response);
+void visorbus_device_create_response(struct visor_device *p, int response);
+void visorbus_device_destroy_response(struct visor_device *p, int response);
+void visorbus_device_resume_response(struct visor_device *p, int response);
+void visorbus_device_pause_response(struct visor_device *p, int response);
 
 int visorbus_init(void);
 void visorbus_exit(void);
diff --git a/drivers/staging/unisys/visorbus/visorchannel.c b/drivers/staging/unisys/visorbus/visorchannel.c
index 9e1cea22ce68..6885c2cb7135 100644
--- a/drivers/staging/unisys/visorbus/visorchannel.c
+++ b/drivers/staging/unisys/visorbus/visorchannel.c
@@ -28,11 +28,11 @@
 
 #define MYDRVNAME "visorchannel"
 
-#define SPAR_CONSOLEVIDEO_CHANNEL_PROTOCOL_GUID \
+#define VISOR_CONSOLEVIDEO_CHANNEL_GUID \
 	UUID_LE(0x3cd6e705, 0xd6a2, 0x4aa5, \
 		0xad, 0x5c, 0x7b, 0x8, 0x88, 0x9d, 0xff, 0xe2)
 
-static const uuid_le spar_video_guid = SPAR_CONSOLEVIDEO_CHANNEL_PROTOCOL_GUID;
+static const uuid_le visor_video_guid = VISOR_CONSOLEVIDEO_CHANNEL_GUID;
 
 struct visorchannel {
 	u64 physaddr;
@@ -415,7 +415,7 @@ visorchannel_create_guts(u64 physaddr, unsigned long channel_bytes,
 	 * release later on.
 	 */
 	channel->requested = request_mem_region(physaddr, size, MYDRVNAME);
-	if (!channel->requested && uuid_le_cmp(guid, spar_video_guid))
+	if (!channel->requested && uuid_le_cmp(guid, visor_video_guid))
 		/* we only care about errors if this is not the video channel */
 		goto err_destroy_channel;
 
@@ -445,7 +445,7 @@ visorchannel_create_guts(u64 physaddr, unsigned long channel_bytes,
 	channel->mapped = NULL;
 	channel->requested = request_mem_region(channel->physaddr,
 						channel_bytes, MYDRVNAME);
-	if (!channel->requested && uuid_le_cmp(guid, spar_video_guid))
+	if (!channel->requested && uuid_le_cmp(guid, visor_video_guid))
 		/* we only care about errors if this is not the video channel */
 		goto err_destroy_channel;
 
diff --git a/drivers/staging/unisys/visorbus/visorchipset.c b/drivers/staging/unisys/visorbus/visorchipset.c
index 4cfd0fae9bd5..22150564b4fb 100644
--- a/drivers/staging/unisys/visorbus/visorchipset.c
+++ b/drivers/staging/unisys/visorbus/visorchipset.c
@@ -34,12 +34,12 @@
 
 #define MAX_CONTROLVM_PAYLOAD_BYTES (1024 * 128)
 
-#define UNISYS_SPAR_LEAF_ID 0x40000000
+#define UNISYS_VISOR_LEAF_ID 0x40000000
 
 /* The s-Par leaf ID returns "UnisysSpar64" encoded across ebx, ecx, edx */
-#define UNISYS_SPAR_ID_EBX 0x73696e55
-#define UNISYS_SPAR_ID_ECX 0x70537379
-#define UNISYS_SPAR_ID_EDX 0x34367261
+#define UNISYS_VISOR_ID_EBX 0x73696e55
+#define UNISYS_VISOR_ID_ECX 0x70537379
+#define UNISYS_VISOR_ID_EDX 0x34367261
 
 /*
  * When the controlvm channel is idle for at least MIN_IDLE_SECONDS,
@@ -101,7 +101,7 @@ static ssize_t toolaction_show(struct device *dev,
 	int err;
 
 	err = visorchannel_read(chipset_dev->controlvm_channel,
-				offsetof(struct spar_controlvm_channel_protocol,
+				offsetof(struct visor_controlvm_channel,
 					 tool_action),
 				&tool_action, sizeof(u8));
 	if (err)
@@ -120,11 +120,10 @@ static ssize_t toolaction_store(struct device *dev,
 	if (kstrtou8(buf, 10, &tool_action))
 		return -EINVAL;
 
-	err = visorchannel_write
-		(chipset_dev->controlvm_channel,
-		 offsetof(struct spar_controlvm_channel_protocol,
-			  tool_action),
-		 &tool_action, sizeof(u8));
+	err = visorchannel_write(chipset_dev->controlvm_channel,
+				 offsetof(struct visor_controlvm_channel,
+					  tool_action),
+				 &tool_action, sizeof(u8));
 
 	if (err)
 		return err;
@@ -136,18 +135,18 @@ static ssize_t boottotool_show(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
 {
-	struct efi_spar_indication efi_spar_indication;
+	struct efi_visor_indication efi_visor_indication;
 	int err;
 
 	err = visorchannel_read(chipset_dev->controlvm_channel,
-				offsetof(struct spar_controlvm_channel_protocol,
-					 efi_spar_ind),
-				&efi_spar_indication,
-				sizeof(struct efi_spar_indication));
+				offsetof(struct visor_controlvm_channel,
+					 efi_visor_ind),
+				&efi_visor_indication,
+				sizeof(struct efi_visor_indication));
 
 	if (err)
 		return err;
-	return sprintf(buf, "%u\n", efi_spar_indication.boot_to_tool);
+	return sprintf(buf, "%u\n", efi_visor_indication.boot_to_tool);
 }
 
 static ssize_t boottotool_store(struct device *dev,
@@ -155,17 +154,17 @@ static ssize_t boottotool_store(struct device *dev,
 				const char *buf, size_t count)
 {
 	int val, err;
-	struct efi_spar_indication efi_spar_indication;
+	struct efi_visor_indication efi_visor_indication;
 
 	if (kstrtoint(buf, 10, &val))
 		return -EINVAL;
 
-	efi_spar_indication.boot_to_tool = val;
-	err = visorchannel_write
-		(chipset_dev->controlvm_channel,
-		 offsetof(struct spar_controlvm_channel_protocol,
-			  efi_spar_ind), &(efi_spar_indication),
-		 sizeof(struct efi_spar_indication));
+	efi_visor_indication.boot_to_tool = val;
+	err = visorchannel_write(chipset_dev->controlvm_channel,
+				 offsetof(struct visor_controlvm_channel,
+					  efi_visor_ind),
+				 &(efi_visor_indication),
+				 sizeof(struct efi_visor_indication));
 
 	if (err)
 		return err;
@@ -180,7 +179,7 @@ static ssize_t error_show(struct device *dev, struct device_attribute *attr,
 	int err;
 
 	err = visorchannel_read(chipset_dev->controlvm_channel,
-				offsetof(struct spar_controlvm_channel_protocol,
+				offsetof(struct visor_controlvm_channel,
 					 installation_error),
 				&error, sizeof(u32));
 	if (err)
@@ -197,11 +196,10 @@ static ssize_t error_store(struct device *dev, struct device_attribute *attr,
 	if (kstrtou32(buf, 10, &error))
 		return -EINVAL;
 
-	err = visorchannel_write
-		(chipset_dev->controlvm_channel,
-		 offsetof(struct spar_controlvm_channel_protocol,
-			  installation_error),
-		 &error, sizeof(u32));
+	err = visorchannel_write(chipset_dev->controlvm_channel,
+				 offsetof(struct visor_controlvm_channel,
+					  installation_error),
+				 &error, sizeof(u32));
 	if (err)
 		return err;
 	return count;
@@ -214,11 +212,10 @@ static ssize_t textid_show(struct device *dev, struct device_attribute *attr,
 	u32 text_id = 0;
 	int err;
 
-	err = visorchannel_read
-			(chipset_dev->controlvm_channel,
-			 offsetof(struct spar_controlvm_channel_protocol,
-				  installation_text_id),
-			 &text_id, sizeof(u32));
+	err = visorchannel_read(chipset_dev->controlvm_channel,
+				offsetof(struct visor_controlvm_channel,
+					 installation_text_id),
+				&text_id, sizeof(u32));
 	if (err)
 		return err;
 
@@ -234,11 +231,10 @@ static ssize_t textid_store(struct device *dev, struct device_attribute *attr,
 	if (kstrtou32(buf, 10, &text_id))
 		return -EINVAL;
 
-	err = visorchannel_write
-		(chipset_dev->controlvm_channel,
-		 offsetof(struct spar_controlvm_channel_protocol,
-			  installation_text_id),
-		 &text_id, sizeof(u32));
+	err = visorchannel_write(chipset_dev->controlvm_channel,
+				 offsetof(struct visor_controlvm_channel,
+					  installation_text_id),
+				 &text_id, sizeof(u32));
 	if (err)
 		return err;
 	return count;
@@ -252,7 +248,7 @@ static ssize_t remaining_steps_show(struct device *dev,
 	int err;
 
 	err = visorchannel_read(chipset_dev->controlvm_channel,
-				offsetof(struct spar_controlvm_channel_protocol,
+				offsetof(struct visor_controlvm_channel,
 					 installation_remaining_steps),
 				&remaining_steps, sizeof(u16));
 	if (err)
@@ -271,11 +267,10 @@ static ssize_t remaining_steps_store(struct device *dev,
 	if (kstrtou16(buf, 10, &remaining_steps))
 		return -EINVAL;
 
-	err = visorchannel_write
-		(chipset_dev->controlvm_channel,
-		 offsetof(struct spar_controlvm_channel_protocol,
-			  installation_remaining_steps),
-		 &remaining_steps, sizeof(u16));
+	err = visorchannel_write(chipset_dev->controlvm_channel,
+				 offsetof(struct visor_controlvm_channel,
+					  installation_remaining_steps),
+				 &remaining_steps, sizeof(u16));
 	if (err)
 		return err;
 	return count;
@@ -285,9 +280,9 @@ static DEVICE_ATTR_RW(remaining_steps);
 static uuid_le
 parser_id_get(struct parser_context *ctx)
 {
-	struct spar_controlvm_parameters_header *phdr = NULL;
+	struct visor_controlvm_parameters_header *phdr = NULL;
 
-	phdr = (struct spar_controlvm_parameters_header *)(ctx->data);
+	phdr = (struct visor_controlvm_parameters_header *)(ctx->data);
 	return phdr->id;
 }
 
@@ -331,9 +326,9 @@ parser_string_get(struct parser_context *ctx)
 static void *
 parser_name_get(struct parser_context *ctx)
 {
-	struct spar_controlvm_parameters_header *phdr = NULL;
+	struct visor_controlvm_parameters_header *phdr = NULL;
 
-	phdr = (struct spar_controlvm_parameters_header *)(ctx->data);
+	phdr = (struct visor_controlvm_parameters_header *)(ctx->data);
 
 	if (phdr->name_offset + phdr->name_length > ctx->param_bytes)
 		return NULL;
@@ -400,7 +395,7 @@ controlvm_init_response(struct controlvm_message *msg,
 static int
 controlvm_respond_chipset_init(struct controlvm_message_header *msg_hdr,
 			       int response,
-			       enum ultra_chipset_feature features)
+			       enum visor_chipset_feature features)
 {
 	struct controlvm_message outmsg;
 
@@ -414,7 +409,7 @@ static int
 chipset_init(struct controlvm_message *inmsg)
 {
 	static int chipset_inited;
-	enum ultra_chipset_feature features = 0;
+	enum visor_chipset_feature features = 0;
 	int rc = CONTROLVM_RESP_SUCCESS;
 	int res = 0;
 
@@ -430,13 +425,13 @@ chipset_init(struct controlvm_message *inmsg)
 	 * also supports it).
 	 */
 	features = inmsg->cmd.init_chipset.features &
-		   ULTRA_CHIPSET_FEATURE_PARA_HOTPLUG;
+		   VISOR_CHIPSET_FEATURE_PARA_HOTPLUG;
 
 	/*
 	 * Set the "reply" bit so Command knows this is a
 	 * features-aware driver.
 	 */
-	features |= ULTRA_CHIPSET_FEATURE_REPLY;
+	features |= VISOR_CHIPSET_FEATURE_REPLY;
 
 out_respond:
 	if (inmsg->hdr.flags.response_expected)
@@ -447,7 +442,7 @@ out_respond:
 
 static int
 controlvm_respond(struct controlvm_message_header *msg_hdr, int response,
-		  struct spar_segment_state *state)
+		  struct visor_segment_state *state)
 {
 	struct controlvm_message outmsg;
 
@@ -470,14 +465,14 @@ enum crash_obj_type {
 };
 
 static int
-save_crash_message(struct controlvm_message *msg, enum crash_obj_type typ)
+save_crash_message(struct controlvm_message *msg, enum crash_obj_type cr_type)
 {
 	u32 local_crash_msg_offset;
 	u16 local_crash_msg_count;
 	int err;
 
 	err = visorchannel_read(chipset_dev->controlvm_channel,
-				offsetof(struct spar_controlvm_channel_protocol,
+				offsetof(struct visor_controlvm_channel,
 					 saved_crash_message_count),
 				&local_crash_msg_count, sizeof(u16));
 	if (err) {
@@ -493,7 +488,7 @@ save_crash_message(struct controlvm_message *msg, enum crash_obj_type typ)
 	}
 
 	err = visorchannel_read(chipset_dev->controlvm_channel,
-				offsetof(struct spar_controlvm_channel_protocol,
+				offsetof(struct visor_controlvm_channel,
 					 saved_crash_message_offset),
 				&local_crash_msg_offset, sizeof(u32));
 	if (err) {
@@ -502,7 +497,7 @@ save_crash_message(struct controlvm_message *msg, enum crash_obj_type typ)
 		return err;
 	}
 
-	switch (typ) {
+	switch (cr_type) {
 	case CRASH_DEV:
 		local_crash_msg_offset += sizeof(struct controlvm_message);
 		err = visorchannel_write(chipset_dev->controlvm_channel,
@@ -551,7 +546,7 @@ controlvm_responder(enum controlvm_id cmd_id,
 static int
 device_changestate_responder(enum controlvm_id cmd_id,
 			     struct visor_device *p, int response,
-			     struct spar_segment_state response_state)
+			     struct visor_segment_state response_state)
 {
 	struct controlvm_message outmsg;
 	u32 bus_no = p->chipset_bus_no;
@@ -573,7 +568,7 @@ device_changestate_responder(enum controlvm_id cmd_id,
 }
 
 static int
-bus_create(struct controlvm_message *inmsg)
+visorbus_create(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
 	struct controlvm_message_header *pmsg_hdr = NULL;
@@ -585,7 +580,7 @@ bus_create(struct controlvm_message *inmsg)
 	bus_info = visorbus_get_device_by_id(bus_no, BUS_ROOT_DEVICE, NULL);
 	if (bus_info && (bus_info->state.created == 1)) {
 		dev_err(&chipset_dev->acpi_device->dev,
-			"failed bus_create: already exists\n");
+			"failed visorbus_create: already exists\n");
 		err = -EEXIST;
 		goto err_respond;
 	}
@@ -600,7 +595,7 @@ bus_create(struct controlvm_message *inmsg)
 	bus_info->chipset_bus_no = bus_no;
 	bus_info->chipset_dev_no = BUS_ROOT_DEVICE;
 
-	if (uuid_le_cmp(cmd->create_bus.bus_inst_uuid, spar_siovm_uuid) == 0) {
+	if (uuid_le_cmp(cmd->create_bus.bus_inst_uuid, visor_siovm_uuid) == 0) {
 		err = save_crash_message(inmsg, CRASH_BUS);
 		if (err)
 			goto err_free_bus_info;
@@ -630,9 +625,9 @@ bus_create(struct controlvm_message *inmsg)
 	}
 	bus_info->visorchannel = visorchannel;
 
-	/* Response will be handled by chipset_bus_create */
-	err = chipset_bus_create(bus_info);
-	/* If error chipset_bus_create didn't respond, need to respond here */
+	/* Response will be handled by visorchipset_bus_create */
+	err = visorchipset_bus_create(bus_info);
+	/* If visorchipset_bus_create didn't respond, need to respond here */
 	if (err)
 		goto err_destroy_channel;
 
@@ -654,7 +649,7 @@ err_respond:
 }
 
 static int
-bus_destroy(struct controlvm_message *inmsg)
+visorbus_destroy(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
 	struct controlvm_message_header *pmsg_hdr = NULL;
@@ -688,8 +683,8 @@ bus_destroy(struct controlvm_message *inmsg)
 		bus_info->pending_msg_hdr = pmsg_hdr;
 	}
 
-	/* Response will be handled by chipset_bus_destroy */
-	chipset_bus_destroy(bus_info);
+	/* Response will be handled by visorchipset_bus_destroy */
+	visorchipset_bus_destroy(bus_info);
 	return 0;
 
 err_respond:
@@ -699,8 +694,8 @@ err_respond:
 }
 
 static int
-bus_configure(struct controlvm_message *inmsg,
-	      struct parser_context *parser_ctx)
+visorbus_configure(struct controlvm_message *inmsg,
+		   struct parser_context *parser_ctx)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
 	u32 bus_no;
@@ -737,14 +732,14 @@ bus_configure(struct controlvm_message *inmsg,
 
 err_respond:
 	dev_err(&chipset_dev->acpi_device->dev,
-		"bus_configured exited with err: %d\n", err);
+		"visorbus_configure exited with err: %d\n", err);
 	if (inmsg->hdr.flags.response_expected == 1)
 		controlvm_responder(inmsg->hdr.id, &inmsg->hdr, err);
 	return err;
 }
 
 static int
-my_device_create(struct controlvm_message *inmsg)
+visorbus_device_create(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
 	struct controlvm_message_header *pmsg_hdr = NULL;
@@ -807,7 +802,7 @@ my_device_create(struct controlvm_message *inmsg)
 	dev_info->visorchannel = visorchannel;
 	dev_info->channel_type_guid = cmd->create_device.data_type_uuid;
 	if (uuid_le_cmp(cmd->create_device.data_type_uuid,
-			spar_vhba_channel_protocol_uuid) == 0) {
+			visor_vhba_channel_uuid) == 0) {
 		err = save_crash_message(inmsg, CRASH_DEV);
 		if (err)
 			goto err_destroy_visorchannel;
@@ -824,8 +819,8 @@ my_device_create(struct controlvm_message *inmsg)
 		       sizeof(struct controlvm_message_header));
 		dev_info->pending_msg_hdr = pmsg_hdr;
 	}
-	/* Chipset_device_create will send response */
-	err = chipset_device_create(dev_info);
+	/* visorchipset_device_create will send response */
+	err = visorchipset_device_create(dev_info);
 	if (err)
 		goto err_destroy_visorchannel;
 
@@ -844,13 +839,13 @@ err_respond:
 }
 
 static int
-my_device_changestate(struct controlvm_message *inmsg)
+visorbus_device_changestate(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
 	struct controlvm_message_header *pmsg_hdr = NULL;
 	u32 bus_no = cmd->device_change_state.bus_no;
 	u32 dev_no = cmd->device_change_state.dev_no;
-	struct spar_segment_state state = cmd->device_change_state.state;
+	struct visor_segment_state state = cmd->device_change_state.state;
 	struct visor_device *dev_info;
 	int err = 0;
 
@@ -882,16 +877,16 @@ my_device_changestate(struct controlvm_message *inmsg)
 
 	if (state.alive == segment_state_running.alive &&
 	    state.operating == segment_state_running.operating)
-		/* Response will be sent from chipset_device_resume */
-		err = chipset_device_resume(dev_info);
+		/* Response will be sent from visorchipset_device_resume */
+		err = visorchipset_device_resume(dev_info);
 	/* ServerNotReady / ServerLost / SegmentStateStandby */
 	else if (state.alive == segment_state_standby.alive &&
 		 state.operating == segment_state_standby.operating)
 		/*
 		 * technically this is standby case where server is lost.
-		 * Response will be sent from chipset_device_pause.
+		 * Response will be sent from visorchipset_device_pause.
 		 */
-		err = chipset_device_pause(dev_info);
+		err = visorchipset_device_pause(dev_info);
 	if (err)
 		goto err_respond;
 
@@ -905,7 +900,7 @@ err_respond:
 }
 
 static int
-my_device_destroy(struct controlvm_message *inmsg)
+visorbus_device_destroy(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
 	struct controlvm_message_header *pmsg_hdr = NULL;
@@ -941,7 +936,7 @@ my_device_destroy(struct controlvm_message *inmsg)
 		dev_info->pending_msg_hdr = pmsg_hdr;
 	}
 
-	chipset_device_destroy(dev_info);
+	visorchipset_device_destroy(dev_info);
 	return 0;
 
 err_respond:
@@ -1179,15 +1174,15 @@ parahotplug_request_kickoff(struct parahotplug_request *req)
 		env_cmd, env_id, env_state, env_bus, env_dev, env_func, NULL
 	};
 
-	sprintf(env_cmd, "SPAR_PARAHOTPLUG=1");
-	sprintf(env_id, "SPAR_PARAHOTPLUG_ID=%d", req->id);
-	sprintf(env_state, "SPAR_PARAHOTPLUG_STATE=%d",
+	sprintf(env_cmd, "VISOR_PARAHOTPLUG=1");
+	sprintf(env_id, "VISOR_PARAHOTPLUG_ID=%d", req->id);
+	sprintf(env_state, "VISOR_PARAHOTPLUG_STATE=%d",
 		cmd->device_change_state.state.active);
-	sprintf(env_bus, "SPAR_PARAHOTPLUG_BUS=%d",
+	sprintf(env_bus, "VISOR_PARAHOTPLUG_BUS=%d",
 		cmd->device_change_state.bus_no);
-	sprintf(env_dev, "SPAR_PARAHOTPLUG_DEVICE=%d",
+	sprintf(env_dev, "VISOR_PARAHOTPLUG_DEVICE=%d",
 		cmd->device_change_state.dev_no >> 3);
-	sprintf(env_func, "SPAR_PARAHOTPLUG_FUNCTION=%d",
+	sprintf(env_func, "VISOR_PARAHOTPLUG_FUNCTION=%d",
 		cmd->device_change_state.dev_no & 0x7);
 
 	return kobject_uevent_env(&chipset_dev->acpi_device->dev.kobj,
@@ -1387,7 +1382,7 @@ setup_crash_devices_work_queue(struct work_struct *work)
 
 	/* get saved message count */
 	if (visorchannel_read(chipset_dev->controlvm_channel,
-			      offsetof(struct spar_controlvm_channel_protocol,
+			      offsetof(struct visor_controlvm_channel,
 				       saved_crash_message_count),
 			      &local_crash_msg_count, sizeof(u16)) < 0) {
 		dev_err(&chipset_dev->acpi_device->dev,
@@ -1403,7 +1398,7 @@ setup_crash_devices_work_queue(struct work_struct *work)
 
 	/* get saved crash message offset */
 	if (visorchannel_read(chipset_dev->controlvm_channel,
-			      offsetof(struct spar_controlvm_channel_protocol,
+			      offsetof(struct visor_controlvm_channel,
 				       saved_crash_message_offset),
 			      &local_crash_msg_offset, sizeof(u32)) < 0) {
 		dev_err(&chipset_dev->acpi_device->dev,
@@ -1438,7 +1433,7 @@ setup_crash_devices_work_queue(struct work_struct *work)
 			"no valid create_bus message\n");
 		return;
 	}
-	bus_create(&local_crash_bus_msg);
+	visorbus_create(&local_crash_bus_msg);
 
 	/* reuse create device message for storage device */
 	if (!local_crash_dev_msg.cmd.create_device.channel_addr) {
@@ -1446,11 +1441,11 @@ setup_crash_devices_work_queue(struct work_struct *work)
 			"no valid create_device message\n");
 		return;
 	}
-	my_device_create(&local_crash_dev_msg);
+	visorbus_device_create(&local_crash_dev_msg);
 }
 
 void
-bus_create_response(struct visor_device *bus_info, int response)
+visorbus_create_response(struct visor_device *bus_info, int response)
 {
 	if (response >= 0)
 		bus_info->state.created = 1;
@@ -1463,7 +1458,7 @@ bus_create_response(struct visor_device *bus_info, int response)
 }
 
 void
-bus_destroy_response(struct visor_device *bus_info, int response)
+visorbus_destroy_response(struct visor_device *bus_info, int response)
 {
 	controlvm_responder(CONTROLVM_BUS_DESTROY, bus_info->pending_msg_hdr,
 			    response);
@@ -1473,7 +1468,7 @@ bus_destroy_response(struct visor_device *bus_info, int response)
 }
 
 void
-device_create_response(struct visor_device *dev_info, int response)
+visorbus_device_create_response(struct visor_device *dev_info, int response)
 {
 	if (response >= 0)
 		dev_info->state.created = 1;
@@ -1486,7 +1481,7 @@ device_create_response(struct visor_device *dev_info, int response)
 }
 
 void
-device_destroy_response(struct visor_device *dev_info, int response)
+visorbus_device_destroy_response(struct visor_device *dev_info, int response)
 {
 	controlvm_responder(CONTROLVM_DEVICE_DESTROY, dev_info->pending_msg_hdr,
 			    response);
@@ -1496,8 +1491,7 @@ device_destroy_response(struct visor_device *dev_info, int response)
 }
 
 void
-device_pause_response(struct visor_device *dev_info,
-		      int response)
+visorbus_device_pause_response(struct visor_device *dev_info, int response)
 {
 	device_changestate_responder(CONTROLVM_DEVICE_CHANGESTATE,
 				     dev_info, response,
@@ -1508,7 +1502,7 @@ device_pause_response(struct visor_device *dev_info,
 }
 
 void
-device_resume_response(struct visor_device *dev_info, int response)
+visorbus_device_resume_response(struct visor_device *dev_info, int response)
 {
 	device_changestate_responder(CONTROLVM_DEVICE_CHANGESTATE,
 				     dev_info, response,
@@ -1599,9 +1593,6 @@ handle_command(struct controlvm_message inmsg, u64 channel_addr)
 
 	/* create parsing context if necessary */
 	local_addr = (inmsg.hdr.flags.test_message == 1);
-	if (channel_addr == 0)
-		return -EINVAL;
-
 	parm_addr = channel_addr + inmsg.hdr.payload_vm_offset;
 	parm_bytes = inmsg.hdr.payload_bytes;
 
@@ -1634,16 +1625,16 @@ handle_command(struct controlvm_message inmsg, u64 channel_addr)
 		err = chipset_init(&inmsg);
 		break;
 	case CONTROLVM_BUS_CREATE:
-		err = bus_create(&inmsg);
+		err = visorbus_create(&inmsg);
 		break;
 	case CONTROLVM_BUS_DESTROY:
-		err = bus_destroy(&inmsg);
+		err = visorbus_destroy(&inmsg);
 		break;
 	case CONTROLVM_BUS_CONFIGURE:
-		err = bus_configure(&inmsg, parser_ctx);
+		err = visorbus_configure(&inmsg, parser_ctx);
 		break;
 	case CONTROLVM_DEVICE_CREATE:
-		err = my_device_create(&inmsg);
+		err = visorbus_device_create(&inmsg);
 		break;
 	case CONTROLVM_DEVICE_CHANGESTATE:
 		if (cmd->device_change_state.flags.phys_device) {
@@ -1653,12 +1644,12 @@ handle_command(struct controlvm_message inmsg, u64 channel_addr)
 			 * save the hdr and cmd structures for later use
 			 * when sending back the response to Command
 			 */
-			err = my_device_changestate(&inmsg);
+			err = visorbus_device_changestate(&inmsg);
 			break;
 		}
 		break;
 	case CONTROLVM_DEVICE_DESTROY:
-		err = my_device_destroy(&inmsg);
+		err = visorbus_device_destroy(&inmsg);
 		break;
 	case CONTROLVM_DEVICE_CONFIGURE:
 		/* no op just send a respond that we passed */
@@ -1793,6 +1784,11 @@ controlvm_periodic_work(struct work_struct *work)
 	/* parahotplug_worker */
 	parahotplug_process_list();
 
+/*
+ * The controlvm messages are sent in a bulk. If we start receiving messages, we
+ * want the polling to be fast. If we do not receive any message for
+ * MIN_IDLE_SECONDS, we can slow down the polling.
+ */
 schedule_out:
 	if (time_after(jiffies, chipset_dev->most_recent_message_jiffies +
 				(HZ * MIN_IDLE_SECONDS))) {
@@ -1821,7 +1817,7 @@ visorchipset_init(struct acpi_device *acpi_device)
 {
 	int err = -ENODEV;
 	u64 addr;
-	uuid_le uuid = SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID;
+	uuid_le uuid = VISOR_CONTROLVM_CHANNEL_UUID;
 	struct visorchannel *controlvm_channel;
 
 	chipset_dev = kzalloc(sizeof(*chipset_dev), GFP_KERNEL);
@@ -1849,7 +1845,7 @@ visorchipset_init(struct acpi_device *acpi_device)
 	if (err < 0)
 		goto error_destroy_channel;
 
-	if (!SPAR_CONTROLVM_CHANNEL_OK_CLIENT(
+	if (!VISOR_CONTROLVM_CHANNEL_OK_CLIENT(
 				visorchannel_get_header(controlvm_channel)))
 		goto error_delete_groups;
 
@@ -1928,10 +1924,10 @@ static __init int visorutil_spar_detect(void)
 
 	if (boot_cpu_has(X86_FEATURE_HYPERVISOR)) {
 		/* check the ID */
-		cpuid(UNISYS_SPAR_LEAF_ID, &eax, &ebx, &ecx, &edx);
-		return  (ebx == UNISYS_SPAR_ID_EBX) &&
-			(ecx == UNISYS_SPAR_ID_ECX) &&
-			(edx == UNISYS_SPAR_ID_EDX);
+		cpuid(UNISYS_VISOR_LEAF_ID, &eax, &ebx, &ecx, &edx);
+		return  (ebx == UNISYS_VISOR_ID_EBX) &&
+			(ecx == UNISYS_VISOR_ID_ECX) &&
+			(edx == UNISYS_VISOR_ID_EDX);
 	} else {
 		return 0;
 	}
diff --git a/drivers/staging/unisys/visorhba/visorhba_main.c b/drivers/staging/unisys/visorhba/visorhba_main.c
index 6997b16b4dcd..a6e7a6bbc428 100644
--- a/drivers/staging/unisys/visorhba/visorhba_main.c
+++ b/drivers/staging/unisys/visorhba/visorhba_main.c
@@ -37,14 +37,14 @@ static struct dentry *visorhba_debugfs_dir;
 /* GUIDS for HBA channel type supported by this driver */
 static struct visor_channeltype_descriptor visorhba_channel_types[] = {
 	/* Note that the only channel type we expect to be reported by the
-	 * bus driver is the SPAR_VHBA channel.
+	 * bus driver is the VISOR_VHBA channel.
 	 */
-	{ SPAR_VHBA_CHANNEL_PROTOCOL_UUID, "sparvhba" },
+	{ VISOR_VHBA_CHANNEL_UUID, "sparvhba" },
 	{ NULL_UUID_LE, NULL }
 };
 
 MODULE_DEVICE_TABLE(visorbus, visorhba_channel_types);
-MODULE_ALIAS("visorbus:" SPAR_VHBA_CHANNEL_PROTOCOL_UUID_STR);
+MODULE_ALIAS("visorbus:" VISOR_VHBA_CHANNEL_UUID_STR);
 
 struct visordisk_info {
 	u32 valid;
@@ -657,7 +657,7 @@ static int info_debugfs_show(struct seq_file *seq, void *v)
 		seq_printf(seq, "phys_flags_addr = 0x%016llx\n",
 			   phys_flags_addr);
 		seq_printf(seq, "FeatureFlags = %llu\n",
-			   (__le64)readq(devdata->flags_addr));
+			   (u64)readq(devdata->flags_addr));
 	}
 	seq_printf(seq, "acquire_failed_cnt = %llu\n",
 		   devdata->acquire_failed_cnt);
@@ -1060,8 +1060,7 @@ static int visorhba_probe(struct visor_device *dev)
 	if (!scsihost)
 		return -ENODEV;
 
-	channel_offset = offsetof(struct spar_io_channel_protocol,
-				  vhba.max);
+	channel_offset = offsetof(struct visor_io_channel, vhba.max);
 	err = visorbus_read_channel(dev, channel_offset, &max,
 				    sizeof(struct vhba_config_max));
 	if (err < 0)
@@ -1091,7 +1090,7 @@ static int visorhba_probe(struct visor_device *dev)
 		goto err_scsi_remove_host;
 	}
 	devdata->debugfs_info =
-		debugfs_create_file("info", S_IRUSR | S_IRGRP,
+		debugfs_create_file("info", 0440,
 				    devdata->debugfs_dir, devdata,
 				    &info_debugfs_fops);
 	if (!devdata->debugfs_info) {
@@ -1105,12 +1104,12 @@ static int visorhba_probe(struct visor_device *dev)
 	devdata->serverchangingstate = false;
 	devdata->scsihost = scsihost;
 
-	channel_offset = offsetof(struct spar_io_channel_protocol,
+	channel_offset = offsetof(struct visor_io_channel,
 				  channel_header.features);
 	err = visorbus_read_channel(dev, channel_offset, &features, 8);
 	if (err)
 		goto err_debugfs_info;
-	features |= ULTRA_IO_CHANNEL_IS_POLLING;
+	features |= VISOR_CHANNEL_IS_POLLING;
 	err = visorbus_write_channel(dev, channel_offset, &features, 8);
 	if (err)
 		goto err_debugfs_info;
@@ -1166,7 +1165,7 @@ static void visorhba_remove(struct visor_device *dev)
 	debugfs_remove_recursive(devdata->debugfs_dir);
 }
 
-/* This is used to tell the visor bus driver which types of visor devices
+/* This is used to tell the visorbus driver which types of visor devices
  * we support, and what functions to call when a visor device that we support
  * is attached or removed.
  */
diff --git a/drivers/staging/unisys/visorinput/ultrainputreport.h b/drivers/staging/unisys/visorinput/ultrainputreport.h
index 53dde7c53809..a4baea53c518 100644
--- a/drivers/staging/unisys/visorinput/ultrainputreport.h
+++ b/drivers/staging/unisys/visorinput/ultrainputreport.h
@@ -17,26 +17,23 @@
 
 #include <linux/types.h>
 
-/* Identifies mouse and keyboard activity which is specified by the firmware to
- *  the host using the cmsimpleinput protocol.  @ingroup coretypes
+/* These defines identify mouse and keyboard activity which is specified by the
+ * firmware to the host using the cmsimpleinput protocol.  @ingroup coretypes
  */
-enum ultra_inputaction {
-	inputaction_none = 0,
-	inputaction_xy_motion = 1,	/* only motion; arg1=x, arg2=y */
-	inputaction_mouse_button_down = 2, /* arg1: 1=left,2=center,3=right */
-	inputaction_mouse_button_up = 3, /* arg1: 1=left,2=center,3=right */
-	inputaction_mouse_button_click = 4, /* arg1: 1=left,2=center,3=right */
-	inputaction_mouse_button_dclick = 5, /* arg1: 1=left,2=center,
-					      * 3=right
-					      */
-	inputaction_wheel_rotate_away = 6, /* arg1: wheel rotation away from
-					    * user
-					    */
-	inputaction_wheel_rotate_toward = 7, /* arg1: wheel rotation toward
-					      * user
-					      */
-	inputaction_set_max_xy = 8,	/* set screen maxXY; arg1=x, arg2=y */
-	inputaction_key_down = 64,	/* arg1: scancode, as follows:
+#define INPUTACTION_XY_MOTION 1		/* only motion; arg1=x, arg2=y */
+#define INPUTACTION_MOUSE_BUTTON_DOWN 2 /* arg1: 1=left,2=center,3=right */
+#define INPUTACTION_MOUSE_BUTTON_UP 3	/* arg1: 1=left,2=center,3=right */
+#define INPUTACTION_MOUSE_BUTTON_CLICK 4 /* arg1: 1=left,2=center,3=right */
+#define INPUTACTION_MOUSE_BUTTON_DCLICK 5 /* arg1: 1=left,2=center,
+					   * 3=right
+					   */
+#define INPUTACTION_WHEEL_ROTATE_AWAY 6  /* arg1: wheel rotation away from
+					  * user
+					  */
+#define INPUTACTION_WHEEL_ROTATE_TOWARD 7 /* arg1: wheel rotation toward
+					   * user
+					   */
+#define INPUTACTION_KEY_DOWN 64		/* arg1: scancode, as follows:
 					 * If arg1 <= 0xff, it's a 1-byte
 					 * scancode and arg1 is that scancode.
 					 * If arg1 > 0xff, it's a 2-byte
@@ -45,10 +42,10 @@ enum ultra_inputaction {
 					 * high 8 bits.  E.g., the right ALT key
 					 * would appear as x'38e0'.
 					 */
-	inputaction_key_up = 65,	/* arg1: scancode (in same format as
+#define INPUTACTION_KEY_UP 65		/* arg1: scancode (in same format as
 					 * inputaction_keyDown)
 					 */
-	inputaction_set_locking_key_state = 66,
+#define INPUTACTION_SET_LOCKING_KEY_STATE 66
 					/* arg1: scancode (in same format
 					 *	 as inputaction_keyDown);
 					 *	 MUST refer to one of the
@@ -58,22 +55,20 @@ enum ultra_inputaction {
 					 *	 in the LOCKED position
 					 *	 (e.g., light is ON)
 					 */
-	inputaction_key_down_up = 67,	/* arg1: scancode (in same format
+#define INPUTACTION_KEY_DOWN_UP 67	/* arg1: scancode (in same format
 					 *	 as inputaction_keyDown)
 					 */
-	inputaction_last
-};
 
-struct ultra_inputactivity {
+struct visor_inputactivity {
 	u16 action;
 	u16 arg1;
 	u16 arg2;
 	u16 arg3;
 } __packed;
 
-struct ultra_inputreport {
+struct visor_inputreport {
 	u64 seq_no;
-	struct ultra_inputactivity activity;
+	struct visor_inputactivity activity;
 } __packed;
 
 #endif
diff --git a/drivers/staging/unisys/visorinput/visorinput.c b/drivers/staging/unisys/visorinput/visorinput.c
index cdd35437f0a0..45bc340d4e9d 100644
--- a/drivers/staging/unisys/visorinput/visorinput.c
+++ b/drivers/staging/unisys/visorinput/visorinput.c
@@ -33,17 +33,16 @@
 #include "ultrainputreport.h"
 
 /* Keyboard channel {c73416d0-b0b8-44af-b304-9d2ae99f1b3d} */
-#define SPAR_KEYBOARD_CHANNEL_PROTOCOL_UUID \
+#define VISOR_KEYBOARD_CHANNEL_UUID \
 	UUID_LE(0xc73416d0, 0xb0b8, 0x44af, \
 		0xb3, 0x4, 0x9d, 0x2a, 0xe9, 0x9f, 0x1b, 0x3d)
-#define SPAR_KEYBOARD_CHANNEL_PROTOCOL_UUID_STR "c73416d0-b0b8-44af-b304-9d2ae99f1b3d"
+#define VISOR_KEYBOARD_CHANNEL_UUID_STR "c73416d0-b0b8-44af-b304-9d2ae99f1b3d"
 
 /* Mouse channel {addf07d4-94a9-46e2-81c3-61abcdbdbd87} */
-#define SPAR_MOUSE_CHANNEL_PROTOCOL_UUID \
+#define VISOR_MOUSE_CHANNEL_UUID \
 	UUID_LE(0xaddf07d4, 0x94a9, 0x46e2, \
 		0x81, 0xc3, 0x61, 0xab, 0xcd, 0xbd, 0xbd, 0x87)
-#define SPAR_MOUSE_CHANNEL_PROTOCOL_UUID_STR \
-	"addf07d4-94a9-46e2-81c3-61abcdbdbd87"
+#define VISOR_MOUSE_CHANNEL_UUID_STR "addf07d4-94a9-46e2-81c3-61abcdbdbd87"
 
 #define PIXELS_ACROSS_DEFAULT 800
 #define PIXELS_DOWN_DEFAULT   600
@@ -70,10 +69,8 @@ struct visorinput_devdata {
 	unsigned char keycode_table[0];
 };
 
-static const uuid_le spar_keyboard_channel_protocol_uuid =
-	SPAR_KEYBOARD_CHANNEL_PROTOCOL_UUID;
-static const uuid_le spar_mouse_channel_protocol_uuid =
-	SPAR_MOUSE_CHANNEL_PROTOCOL_UUID;
+static const uuid_le visor_keyboard_channel_uuid = VISOR_KEYBOARD_CHANNEL_UUID;
+static const uuid_le visor_mouse_channel_uuid = VISOR_MOUSE_CHANNEL_UUID;
 
 /*
  * Borrowed from drivers/input/keyboard/atakbd.c
@@ -456,9 +453,9 @@ visorinput_probe(struct visor_device *dev)
 	enum visorinput_device_type devtype;
 
 	guid = visorchannel_get_uuid(dev->visorchannel);
-	if (uuid_le_cmp(guid, spar_mouse_channel_protocol_uuid) == 0)
+	if (uuid_le_cmp(guid, visor_mouse_channel_uuid) == 0)
 		devtype = visorinput_mouse;
-	else if (uuid_le_cmp(guid, spar_keyboard_channel_protocol_uuid) == 0)
+	else if (uuid_le_cmp(guid, visor_keyboard_channel_uuid) == 0)
 		devtype = visorinput_keyboard;
 	else
 		return -ENODEV;
@@ -568,7 +565,7 @@ calc_button(int x)
 static void
 visorinput_channel_interrupt(struct visor_device *dev)
 {
-	struct ultra_inputreport r;
+	struct visor_inputreport r;
 	int scancode, keycode;
 	struct input_dev *visorinput_dev;
 	int xmotion, ymotion, button;
@@ -585,46 +582,46 @@ visorinput_channel_interrupt(struct visor_device *dev)
 		scancode = r.activity.arg1;
 		keycode = scancode_to_keycode(scancode);
 		switch (r.activity.action) {
-		case inputaction_key_down:
+		case INPUTACTION_KEY_DOWN:
 			input_report_key(visorinput_dev, keycode, 1);
 			input_sync(visorinput_dev);
 			break;
-		case inputaction_key_up:
+		case INPUTACTION_KEY_UP:
 			input_report_key(visorinput_dev, keycode, 0);
 			input_sync(visorinput_dev);
 			break;
-		case inputaction_key_down_up:
+		case INPUTACTION_KEY_DOWN_UP:
 			input_report_key(visorinput_dev, keycode, 1);
 			input_sync(visorinput_dev);
 			input_report_key(visorinput_dev, keycode, 0);
 			input_sync(visorinput_dev);
 			break;
-		case inputaction_set_locking_key_state:
+		case INPUTACTION_SET_LOCKING_KEY_STATE:
 			handle_locking_key(visorinput_dev, keycode,
 					   r.activity.arg2);
 			break;
-		case inputaction_xy_motion:
+		case INPUTACTION_XY_MOTION:
 			xmotion = r.activity.arg1;
 			ymotion = r.activity.arg2;
 			input_report_abs(visorinput_dev, ABS_X, xmotion);
 			input_report_abs(visorinput_dev, ABS_Y, ymotion);
 			input_sync(visorinput_dev);
 			break;
-		case inputaction_mouse_button_down:
+		case INPUTACTION_MOUSE_BUTTON_DOWN:
 			button = calc_button(r.activity.arg1);
 			if (button < 0)
 				break;
 			input_report_key(visorinput_dev, button, 1);
 			input_sync(visorinput_dev);
 			break;
-		case inputaction_mouse_button_up:
+		case INPUTACTION_MOUSE_BUTTON_UP:
 			button = calc_button(r.activity.arg1);
 			if (button < 0)
 				break;
 			input_report_key(visorinput_dev, button, 0);
 			input_sync(visorinput_dev);
 			break;
-		case inputaction_mouse_button_click:
+		case INPUTACTION_MOUSE_BUTTON_CLICK:
 			button = calc_button(r.activity.arg1);
 			if (button < 0)
 				break;
@@ -634,7 +631,7 @@ visorinput_channel_interrupt(struct visor_device *dev)
 			input_report_key(visorinput_dev, button, 0);
 			input_sync(visorinput_dev);
 			break;
-		case inputaction_mouse_button_dclick:
+		case INPUTACTION_MOUSE_BUTTON_DCLICK:
 			button = calc_button(r.activity.arg1);
 			if (button < 0)
 				break;
@@ -645,11 +642,11 @@ visorinput_channel_interrupt(struct visor_device *dev)
 				input_sync(visorinput_dev);
 			}
 			break;
-		case inputaction_wheel_rotate_away:
+		case INPUTACTION_WHEEL_ROTATE_AWAY:
 			input_report_rel(visorinput_dev, REL_WHEEL, 1);
 			input_sync(visorinput_dev);
 			break;
-		case inputaction_wheel_rotate_toward:
+		case INPUTACTION_WHEEL_ROTATE_TOWARD:
 			input_report_rel(visorinput_dev, REL_WHEEL, -1);
 			input_sync(visorinput_dev);
 			break;
@@ -730,8 +727,8 @@ out:
 
 /* GUIDS for all channel types supported by this driver. */
 static struct visor_channeltype_descriptor visorinput_channel_types[] = {
-	{ SPAR_KEYBOARD_CHANNEL_PROTOCOL_UUID, "keyboard"},
-	{ SPAR_MOUSE_CHANNEL_PROTOCOL_UUID, "mouse"},
+	{ VISOR_KEYBOARD_CHANNEL_UUID, "keyboard"},
+	{ VISOR_MOUSE_CHANNEL_UUID, "mouse"},
 	{ NULL_UUID_LE, NULL }
 };
 
@@ -767,5 +764,5 @@ MODULE_AUTHOR("Unisys");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("s-Par human input driver for virtual keyboard/mouse");
 
-MODULE_ALIAS("visorbus:" SPAR_MOUSE_CHANNEL_PROTOCOL_UUID_STR);
-MODULE_ALIAS("visorbus:" SPAR_KEYBOARD_CHANNEL_PROTOCOL_UUID_STR);
+MODULE_ALIAS("visorbus:" VISOR_MOUSE_CHANNEL_UUID_STR);
+MODULE_ALIAS("visorbus:" VISOR_KEYBOARD_CHANNEL_UUID_STR);
diff --git a/drivers/staging/unisys/visornic/visornic_main.c b/drivers/staging/unisys/visornic/visornic_main.c
index adebf224f73a..2891622eef18 100644
--- a/drivers/staging/unisys/visornic/visornic_main.c
+++ b/drivers/staging/unisys/visornic/visornic_main.c
@@ -39,9 +39,9 @@
 /* GUIDS for director channel type supported by this driver.  */
 static struct visor_channeltype_descriptor visornic_channel_types[] = {
 	/* Note that the only channel type we expect to be reported by the
-	 * bus driver is the SPAR_VNIC channel.
+	 * bus driver is the VISOR_VNIC channel.
 	 */
-	{ SPAR_VNIC_CHANNEL_PROTOCOL_UUID, "ultravnic" },
+	{ VISOR_VNIC_CHANNEL_UUID, "ultravnic" },
 	{ NULL_UUID_LE, NULL }
 };
 MODULE_DEVICE_TABLE(visorbus, visornic_channel_types);
@@ -52,7 +52,7 @@ MODULE_DEVICE_TABLE(visorbus, visornic_channel_types);
  * must be added to scripts/mode/file2alias.c, etc., to get this working
  * properly.
  */
-MODULE_ALIAS("visorbus:" SPAR_VNIC_CHANNEL_PROTOCOL_UUID_STR);
+MODULE_ALIAS("visorbus:" VISOR_VNIC_CHANNEL_UUID_STR);
 
 struct chanstat {
 	unsigned long got_rcv;
@@ -1807,8 +1807,7 @@ static int visornic_probe(struct visor_device *dev)
 
 	/* Get MAC address from channel and read it into the device. */
 	netdev->addr_len = ETH_ALEN;
-	channel_offset = offsetof(struct spar_io_channel_protocol,
-				  vnic.macaddr);
+	channel_offset = offsetof(struct visor_io_channel, vnic.macaddr);
 	err = visorbus_read_channel(dev, channel_offset, netdev->dev_addr,
 				    ETH_ALEN);
 	if (err < 0) {
@@ -1836,8 +1835,7 @@ static int visornic_probe(struct visor_device *dev)
 	atomic_set(&devdata->usage, 1);
 
 	/* Setup rcv bufs */
-	channel_offset = offsetof(struct spar_io_channel_protocol,
-				  vnic.num_rcv_bufs);
+	channel_offset = offsetof(struct visor_io_channel, vnic.num_rcv_bufs);
 	err = visorbus_read_channel(dev, channel_offset,
 				    &devdata->num_rcv_bufs, 4);
 	if (err) {
@@ -1884,8 +1882,7 @@ static int visornic_probe(struct visor_device *dev)
 	devdata->server_change_state = false;
 
 	/*set the default mtu */
-	channel_offset = offsetof(struct spar_io_channel_protocol,
-				  vnic.mtu);
+	channel_offset = offsetof(struct visor_io_channel, vnic.mtu);
 	err = visorbus_read_channel(dev, channel_offset, &netdev->mtu, 4);
 	if (err) {
 		dev_err(&dev->device,
@@ -1906,7 +1903,7 @@ static int visornic_probe(struct visor_device *dev)
 	 */
 	mod_timer(&devdata->irq_poll_timer, msecs_to_jiffies(2));
 
-	channel_offset = offsetof(struct spar_io_channel_protocol,
+	channel_offset = offsetof(struct visor_io_channel,
 				  channel_header.features);
 	err = visorbus_read_channel(dev, channel_offset, &features, 8);
 	if (err) {
@@ -1916,8 +1913,8 @@ static int visornic_probe(struct visor_device *dev)
 		goto cleanup_napi_add;
 	}
 
-	features |= ULTRA_IO_CHANNEL_IS_POLLING;
-	features |= ULTRA_IO_DRIVER_SUPPORTS_ENHANCED_RCVBUF_CHECKING;
+	features |= VISOR_CHANNEL_IS_POLLING;
+	features |= VISOR_DRIVER_ENHANCED_RCVBUF_CHECKING;
 	err = visorbus_write_channel(dev, channel_offset, &features, 8);
 	if (err) {
 		dev_err(&dev->device,
@@ -2115,7 +2112,7 @@ static int visornic_resume(struct visor_device *dev,
 	return 0;
 }
 
-/* This is used to tell the visor bus driver which types of visor devices
+/* This is used to tell the visorbus driver which types of visor devices
  * we support, and what functions to call when a visor device that we support
  * is attached or removed.
  */
diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
index e8cf0b97bf02..3637ddf909a4 100644
--- a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
@@ -353,9 +353,8 @@ static int snd_bcm2835_pcm_ack(struct snd_pcm_substream *substream)
 	struct snd_pcm_indirect *pcm_indirect = &alsa_stream->pcm_indirect;
 
 	pcm_indirect->hw_queue_size = runtime->hw.buffer_bytes_max;
-	snd_pcm_indirect_playback_transfer(substream, pcm_indirect,
-					   snd_bcm2835_pcm_transfer);
-	return 0;
+	return snd_pcm_indirect_playback_transfer(substream, pcm_indirect,
+						  snd_bcm2835_pcm_transfer);
 }
 
 /* trigger callback */
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_2835_arm.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_2835_arm.c
index d04db3f55519..0159ca4407d8 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_2835_arm.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_2835_arm.c
@@ -153,7 +153,6 @@ int vchiq_platform_init(struct platform_device *pdev, VCHIQ_STATE_T *state)
 		MAX_FRAGMENTS;
 
 	g_fragments_base = (char *)slot_mem + slot_mem_size;
-	slot_mem_size += frag_mem_size;
 
 	g_free_fragments = g_fragments_base;
 	for (i = 0; i < (MAX_FRAGMENTS - 1); i++) {
@@ -365,7 +364,7 @@ vchiq_doorbell_irq(int irq, void *dev_id)
 }
 
 static void
-cleaup_pagelistinfo(struct vchiq_pagelist_info *pagelistinfo)
+cleanup_pagelistinfo(struct vchiq_pagelist_info *pagelistinfo)
 {
 	if (pagelistinfo->scatterlist_mapped) {
 		dma_unmap_sg(g_dev, pagelistinfo->scatterlist,
@@ -460,6 +459,11 @@ create_pagelist(char __user *buf, size_t count, unsigned short type,
 								 PAGE_SIZE));
 			size_t bytes = PAGE_SIZE - off;
 
+			if (!pg) {
+				cleanup_pagelistinfo(pagelistinfo);
+				return NULL;
+			}
+
 			if (bytes > length)
 				bytes = length;
 			pages[actual_pages] = pg;
@@ -470,7 +474,7 @@ create_pagelist(char __user *buf, size_t count, unsigned short type,
 	} else {
 		down_read(&task->mm->mmap_sem);
 		actual_pages = get_user_pages(
-				          (unsigned long)buf & ~(PAGE_SIZE - 1),
+					  (unsigned long)buf & PAGE_MASK,
 					  num_pages,
 					  (type == PAGELIST_READ) ? FOLL_WRITE : 0,
 					  pages,
@@ -489,7 +493,7 @@ create_pagelist(char __user *buf, size_t count, unsigned short type,
 				actual_pages--;
 				put_page(pages[actual_pages]);
 			}
-			cleaup_pagelistinfo(pagelistinfo);
+			cleanup_pagelistinfo(pagelistinfo);
 			return NULL;
 		}
 		 /* release user pages */
@@ -518,7 +522,7 @@ create_pagelist(char __user *buf, size_t count, unsigned short type,
 				 pagelistinfo->dma_dir);
 
 	if (dma_buffers == 0) {
-		cleaup_pagelistinfo(pagelistinfo);
+		cleanup_pagelistinfo(pagelistinfo);
 		return NULL;
 	}
 
@@ -555,7 +559,7 @@ create_pagelist(char __user *buf, size_t count, unsigned short type,
 		char *fragments;
 
 		if (down_interruptible(&g_free_fragments_sema) != 0) {
-			cleaup_pagelistinfo(pagelistinfo);
+			cleanup_pagelistinfo(pagelistinfo);
 			return NULL;
 		}
 
@@ -577,7 +581,6 @@ static void
 free_pagelist(struct vchiq_pagelist_info *pagelistinfo,
 	      int actual)
 {
-	unsigned int i;
 	PAGELIST_T *pagelist   = pagelistinfo->pagelist;
 	struct page **pages    = pagelistinfo->pages;
 	unsigned int num_pages = pagelistinfo->num_pages;
@@ -633,9 +636,11 @@ free_pagelist(struct vchiq_pagelist_info *pagelistinfo,
 	/* Need to mark all the pages dirty. */
 	if (pagelist->type != PAGELIST_WRITE &&
 	    pagelistinfo->pages_need_release) {
+		unsigned int i;
+
 		for (i = 0; i < num_pages; i++)
 			set_page_dirty(pages[i]);
 	}
 
-	cleaup_pagelistinfo(pagelistinfo);
+	cleanup_pagelistinfo(pagelistinfo);
 }
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
index e823f1d5d177..030bec855d86 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
@@ -2070,7 +2070,7 @@ dump_phys_mem(void *virt_addr, u32 num_bytes)
 	struct page  **pages;
 	u8            *kmapped_virt_ptr;
 
-	/* Align virtAddr and endVirtAddr to 16 byte boundaries. */
+	/* Align virt_addr and end_virt_addr to 16 byte boundaries. */
 
 	virt_addr = (void *)((unsigned long)virt_addr & ~0x0fuL);
 	end_virt_addr = (void *)(((unsigned long)end_virt_addr + 15uL) &
@@ -3276,12 +3276,12 @@ vchiq_dump_service_use_state(VCHIQ_STATE_T *state)
 		if (only_nonzero && !service_ptr->service_use_count)
 			continue;
 
-		if (service_ptr->srvstate != VCHIQ_SRVSTATE_FREE) {
-			service_data[j].fourcc = service_ptr->base.fourcc;
-			service_data[j].clientid = service_ptr->client_id;
-			service_data[j++].use_count = service_ptr->
-							service_use_count;
-		}
+		if (service_ptr->srvstate == VCHIQ_SRVSTATE_FREE)
+			continue;
+
+		service_data[j].fourcc = service_ptr->base.fourcc;
+		service_data[j].clientid = service_ptr->client_id;
+		service_data[j++].use_count = service_ptr->service_use_count;
 	}
 
 	read_unlock_bh(&arm_state->susp_res_lock);
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
index 4f9e738abddf..486be990d7fc 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
@@ -175,7 +175,7 @@ find_service_by_handle(VCHIQ_SERVICE_HANDLE_T handle)
 	service = handle_to_service(handle);
 	if (service && (service->srvstate != VCHIQ_SRVSTATE_FREE) &&
 		(service->handle == handle)) {
-		BUG_ON(service->ref_count == 0);
+		WARN_ON(service->ref_count == 0);
 		service->ref_count++;
 	} else
 		service = NULL;
@@ -197,7 +197,7 @@ find_service_by_port(VCHIQ_STATE_T *state, int localport)
 		spin_lock(&service_spinlock);
 		service = state->services[localport];
 		if (service && (service->srvstate != VCHIQ_SRVSTATE_FREE)) {
-			BUG_ON(service->ref_count == 0);
+			WARN_ON(service->ref_count == 0);
 			service->ref_count++;
 		} else
 			service = NULL;
@@ -221,7 +221,7 @@ find_service_for_instance(VCHIQ_INSTANCE_T instance,
 	if (service && (service->srvstate != VCHIQ_SRVSTATE_FREE) &&
 		(service->handle == handle) &&
 		(service->instance == instance)) {
-		BUG_ON(service->ref_count == 0);
+		WARN_ON(service->ref_count == 0);
 		service->ref_count++;
 	} else
 		service = NULL;
@@ -246,7 +246,7 @@ find_closed_service_for_instance(VCHIQ_INSTANCE_T instance,
 		 (service->srvstate == VCHIQ_SRVSTATE_CLOSED)) &&
 		(service->handle == handle) &&
 		(service->instance == instance)) {
-		BUG_ON(service->ref_count == 0);
+		WARN_ON(service->ref_count == 0);
 		service->ref_count++;
 	} else
 		service = NULL;
@@ -273,7 +273,7 @@ next_service_by_instance(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance,
 		if (srv && (srv->srvstate != VCHIQ_SRVSTATE_FREE) &&
 			(srv->instance == instance)) {
 			service = srv;
-			BUG_ON(service->ref_count == 0);
+			WARN_ON(service->ref_count == 0);
 			service->ref_count++;
 			break;
 		}
@@ -289,9 +289,11 @@ void
 lock_service(VCHIQ_SERVICE_T *service)
 {
 	spin_lock(&service_spinlock);
-	BUG_ON(!service || (service->ref_count == 0));
-	if (service)
+	WARN_ON(!service);
+	if (service) {
+		WARN_ON(service->ref_count == 0);
 		service->ref_count++;
+	}
 	spin_unlock(&service_spinlock);
 }
 
@@ -299,17 +301,24 @@ void
 unlock_service(VCHIQ_SERVICE_T *service)
 {
 	spin_lock(&service_spinlock);
-	BUG_ON(!service || (service->ref_count == 0));
-	if (service && service->ref_count) {
-		service->ref_count--;
-		if (!service->ref_count) {
-			VCHIQ_STATE_T *state = service->state;
-
-			BUG_ON(service->srvstate != VCHIQ_SRVSTATE_FREE);
-			state->services[service->localport] = NULL;
-		} else
-			service = NULL;
+	if (!service) {
+		WARN(1, "%s: service is NULL\n", __func__);
+		goto unlock;
+	}
+	if (!service->ref_count) {
+		WARN(1, "%s: ref_count is zero\n", __func__);
+		goto unlock;
 	}
+	service->ref_count--;
+	if (!service->ref_count) {
+		VCHIQ_STATE_T *state = service->state;
+
+		WARN_ON(service->srvstate != VCHIQ_SRVSTATE_FREE);
+		state->services[service->localport] = NULL;
+	} else {
+		service = NULL;
+	}
+unlock:
 	spin_unlock(&service_spinlock);
 
 	if (service && service->userdata_term)
@@ -591,8 +600,10 @@ reserve_space(VCHIQ_STATE_T *state, size_t space, int is_blocking)
 				return NULL; /* No space available */
 		}
 
-		BUG_ON(tx_pos ==
-			(state->slot_queue_available * VCHIQ_SLOT_SIZE));
+		if (tx_pos == (state->slot_queue_available * VCHIQ_SLOT_SIZE)) {
+			pr_warn("%s: invalid tx_pos: %d\n", __func__, tx_pos);
+			return NULL;
+		}
 
 		slot_index = local->slot_queue[
 			SLOT_QUEUE_INDEX_FROM_POS(tx_pos) &
@@ -822,9 +833,14 @@ queue_message(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service,
 	if (type == VCHIQ_MSG_DATA) {
 		int tx_end_index;
 
-		BUG_ON(!service);
-		BUG_ON((flags & (QMFLAGS_NO_MUTEX_LOCK |
-				 QMFLAGS_NO_MUTEX_UNLOCK)) != 0);
+		if (!service) {
+			WARN(1, "%s: service is NULL\n", __func__);
+			mutex_unlock(&state->slot_mutex);
+			return VCHIQ_ERROR;
+		}
+
+		WARN_ON((flags & (QMFLAGS_NO_MUTEX_LOCK |
+				  QMFLAGS_NO_MUTEX_UNLOCK)) != 0);
 
 		if (service->closing) {
 			/* The service has been closed */
@@ -923,9 +939,8 @@ queue_message(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service,
 			header, size, VCHIQ_MSG_SRCPORT(msgid),
 			VCHIQ_MSG_DSTPORT(msgid));
 
-		BUG_ON(!service);
-		BUG_ON((flags & (QMFLAGS_NO_MUTEX_LOCK |
-				 QMFLAGS_NO_MUTEX_UNLOCK)) != 0);
+		WARN_ON((flags & (QMFLAGS_NO_MUTEX_LOCK |
+				  QMFLAGS_NO_MUTEX_UNLOCK)) != 0);
 
 		callback_result =
 			copy_message_data(copy_callback, context,
@@ -1376,7 +1391,6 @@ resolve_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *queue)
 {
 	VCHIQ_STATE_T *state = service->state;
 	int resolved = 0;
-	int rc;
 
 	while ((queue->process != queue->local_insert) &&
 		(queue->process != queue->remote_insert)) {
@@ -1392,8 +1406,7 @@ resolve_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *queue)
 		WARN_ON(!((int)(queue->local_insert - queue->process) > 0));
 		WARN_ON(!((int)(queue->remote_insert - queue->process) > 0));
 
-		rc = mutex_lock_killable(&state->bulk_transfer_mutex);
-		if (rc != 0)
+		if (mutex_lock_killable(&state->bulk_transfer_mutex))
 			break;
 
 		vchiq_transfer_bulk(bulk);
@@ -1952,9 +1965,14 @@ parse_rx_slots(VCHIQ_STATE_T *state)
 					mutex_unlock(&service->bulk_mutex);
 					break;
 				}
-
-				BUG_ON(queue->process == queue->local_insert);
-				BUG_ON(queue->process != queue->remote_insert);
+				if (queue->process != queue->remote_insert) {
+					pr_err("%s: p %x != ri %x\n",
+					       __func__,
+					       queue->process,
+					       queue->remote_insert);
+					mutex_unlock(&service->bulk_mutex);
+					goto bail_not_ready;
+				}
 
 				bulk = &queue->bulks[
 					BULK_INDEX(queue->remote_insert)];
@@ -2139,7 +2157,6 @@ slot_handler_func(void *v)
 					vchiq_log_error(vchiq_core_log_level,
 						"Failed to send RESUME "
 						"message");
-					BUG();
 				}
 				break;
 
@@ -2351,13 +2368,17 @@ vchiq_init_state(VCHIQ_STATE_T *state, VCHIQ_SLOT_ZERO_T *slot_zero,
 	VCHIQ_SHARED_STATE_T *remote;
 	VCHIQ_STATUS_T status;
 	char threadname[16];
-	static int id;
 	int i;
 
 	vchiq_log_warning(vchiq_core_log_level,
 		"%s: slot_zero = %pK, is_master = %d",
 		__func__, slot_zero, is_master);
 
+	if (vchiq_states[0]) {
+		pr_err("%s: VCHIQ state already initialized\n", __func__);
+		return VCHIQ_ERROR;
+	}
+
 	/* Check the input configuration */
 
 	if (slot_zero->magic != VCHIQ_MAGIC) {
@@ -2439,7 +2460,6 @@ vchiq_init_state(VCHIQ_STATE_T *state, VCHIQ_SLOT_ZERO_T *slot_zero,
 
 	memset(state, 0, sizeof(VCHIQ_STATE_T));
 
-	state->id = id++;
 	state->is_master = is_master;
 
 	/*
@@ -2558,8 +2578,7 @@ vchiq_init_state(VCHIQ_STATE_T *state, VCHIQ_SLOT_ZERO_T *slot_zero,
 	set_user_nice(state->sync_thread, -20);
 	wake_up_process(state->sync_thread);
 
-	BUG_ON(state->id >= VCHIQ_MAX_STATES);
-	vchiq_states[state->id] = state;
+	vchiq_states[0] = state;
 
 	/* Indicate readiness to the other side */
 	local->initialised = 1;
@@ -3185,7 +3204,7 @@ vchiq_close_service(VCHIQ_SERVICE_HANDLE_T handle)
 	if (current == service->state->slot_handler_thread) {
 		status = vchiq_close_service_internal(service,
 			0/*!close_recvd*/);
-		BUG_ON(status == VCHIQ_RETRY);
+		WARN_ON(status == VCHIQ_RETRY);
 	} else {
 	/* Mark the service for termination by the slot handler */
 		request_poll(service->state, service, VCHIQ_POLL_TERMINATE);
@@ -3247,7 +3266,7 @@ vchiq_remove_service(VCHIQ_SERVICE_HANDLE_T handle)
 
 		status = vchiq_close_service_internal(service,
 			0/*!close_recvd*/);
-		BUG_ON(status == VCHIQ_RETRY);
+		WARN_ON(status == VCHIQ_RETRY);
 	} else {
 		/* Mark the service for removal by the slot handler */
 		request_poll(service->state, service, VCHIQ_POLL_REMOVE);
diff --git a/drivers/staging/vme/devices/vme_pio2.h b/drivers/staging/vme/devices/vme_pio2.h
index 5577df3199e7..ac4a4bad4091 100644
--- a/drivers/staging/vme/devices/vme_pio2.h
+++ b/drivers/staging/vme/devices/vme_pio2.h
@@ -68,38 +68,38 @@ static const int PIO2_CHANNEL_BANK[32] = { 0, 0, 0, 0, 0, 0, 0, 0,
 					2, 2, 2, 2, 2, 2, 2, 2,
 					3, 3, 3, 3, 3, 3, 3, 3 };
 
-#define PIO2_CHANNEL0_BIT		(1 << 0)
-#define PIO2_CHANNEL1_BIT		(1 << 1)
-#define PIO2_CHANNEL2_BIT		(1 << 2)
-#define PIO2_CHANNEL3_BIT		(1 << 3)
-#define PIO2_CHANNEL4_BIT		(1 << 4)
-#define PIO2_CHANNEL5_BIT		(1 << 5)
-#define PIO2_CHANNEL6_BIT		(1 << 6)
-#define PIO2_CHANNEL7_BIT		(1 << 7)
-#define PIO2_CHANNEL8_BIT		(1 << 0)
-#define PIO2_CHANNEL9_BIT		(1 << 1)
-#define PIO2_CHANNEL10_BIT		(1 << 2)
-#define PIO2_CHANNEL11_BIT		(1 << 3)
-#define PIO2_CHANNEL12_BIT		(1 << 4)
-#define PIO2_CHANNEL13_BIT		(1 << 5)
-#define PIO2_CHANNEL14_BIT		(1 << 6)
-#define PIO2_CHANNEL15_BIT		(1 << 7)
-#define PIO2_CHANNEL16_BIT		(1 << 0)
-#define PIO2_CHANNEL17_BIT		(1 << 1)
-#define PIO2_CHANNEL18_BIT		(1 << 2)
-#define PIO2_CHANNEL19_BIT		(1 << 3)
-#define PIO2_CHANNEL20_BIT		(1 << 4)
-#define PIO2_CHANNEL21_BIT		(1 << 5)
-#define PIO2_CHANNEL22_BIT		(1 << 6)
-#define PIO2_CHANNEL23_BIT		(1 << 7)
-#define PIO2_CHANNEL24_BIT		(1 << 0)
-#define PIO2_CHANNEL25_BIT		(1 << 1)
-#define PIO2_CHANNEL26_BIT		(1 << 2)
-#define PIO2_CHANNEL27_BIT		(1 << 3)
-#define PIO2_CHANNEL28_BIT		(1 << 4)
-#define PIO2_CHANNEL29_BIT		(1 << 5)
-#define PIO2_CHANNEL30_BIT		(1 << 6)
-#define PIO2_CHANNEL31_BIT		(1 << 7)
+#define PIO2_CHANNEL0_BIT		BIT(0)
+#define PIO2_CHANNEL1_BIT		BIT(1)
+#define PIO2_CHANNEL2_BIT		BIT(2)
+#define PIO2_CHANNEL3_BIT		BIT(3)
+#define PIO2_CHANNEL4_BIT		BIT(4)
+#define PIO2_CHANNEL5_BIT		BIT(5)
+#define PIO2_CHANNEL6_BIT		BIT(6)
+#define PIO2_CHANNEL7_BIT		BIT(7)
+#define PIO2_CHANNEL8_BIT		BIT(0)
+#define PIO2_CHANNEL9_BIT		BIT(1)
+#define PIO2_CHANNEL10_BIT		BIT(2)
+#define PIO2_CHANNEL11_BIT		BIT(3)
+#define PIO2_CHANNEL12_BIT		BIT(4)
+#define PIO2_CHANNEL13_BIT		BIT(5)
+#define PIO2_CHANNEL14_BIT		BIT(6)
+#define PIO2_CHANNEL15_BIT		BIT(7)
+#define PIO2_CHANNEL16_BIT		BIT(0)
+#define PIO2_CHANNEL17_BIT		BIT(1)
+#define PIO2_CHANNEL18_BIT		BIT(2)
+#define PIO2_CHANNEL19_BIT		BIT(3)
+#define PIO2_CHANNEL20_BIT		BIT(4)
+#define PIO2_CHANNEL21_BIT		BIT(5)
+#define PIO2_CHANNEL22_BIT		BIT(6)
+#define PIO2_CHANNEL23_BIT		BIT(7)
+#define PIO2_CHANNEL24_BIT		BIT(0)
+#define PIO2_CHANNEL25_BIT		BIT(1)
+#define PIO2_CHANNEL26_BIT		BIT(2)
+#define PIO2_CHANNEL27_BIT		BIT(3)
+#define PIO2_CHANNEL28_BIT		BIT(4)
+#define PIO2_CHANNEL29_BIT		BIT(5)
+#define PIO2_CHANNEL30_BIT		BIT(6)
+#define PIO2_CHANNEL31_BIT		BIT(7)
 
 static const int PIO2_CHANNEL_BIT[32] = { PIO2_CHANNEL0_BIT, PIO2_CHANNEL1_BIT,
 					PIO2_CHANNEL2_BIT, PIO2_CHANNEL3_BIT,
@@ -120,12 +120,12 @@ static const int PIO2_CHANNEL_BIT[32] = { PIO2_CHANNEL0_BIT, PIO2_CHANNEL1_BIT,
 					};
 
 /* PIO2_REGS_INT_STAT_CNTR (0xc) */
-#define PIO2_COUNTER0			(1 << 0)
-#define PIO2_COUNTER1			(1 << 1)
-#define PIO2_COUNTER2			(1 << 2)
-#define PIO2_COUNTER3			(1 << 3)
-#define PIO2_COUNTER4			(1 << 4)
-#define PIO2_COUNTER5			(1 << 5)
+#define PIO2_COUNTER0			BIT(0)
+#define PIO2_COUNTER1			BIT(1)
+#define PIO2_COUNTER2			BIT(2)
+#define PIO2_COUNTER3			BIT(3)
+#define PIO2_COUNTER4			BIT(4)
+#define PIO2_COUNTER5			BIT(5)
 
 static const int PIO2_COUNTER[6] = { PIO2_COUNTER0, PIO2_COUNTER1,
 					PIO2_COUNTER2, PIO2_COUNTER3,
@@ -133,8 +133,8 @@ static const int PIO2_COUNTER[6] = { PIO2_COUNTER0, PIO2_COUNTER1,
 
 /* PIO2_REGS_CTRL (0x18) */
 #define PIO2_VME_INT_MASK		0x7
-#define PIO2_LED			(1 << 6)
-#define PIO2_LOOP			(1 << 7)
+#define PIO2_LED			BIT(6)
+#define PIO2_LOOP			BIT(7)
 
 /* PIO2_REGS_VME_VECTOR (0x19) */
 #define PIO2_VME_VECTOR_SPUR		0x0
diff --git a/drivers/staging/vt6655/card.c b/drivers/staging/vt6655/card.c
index 5463cf869d1b..f5db2b3d9045 100644
--- a/drivers/staging/vt6655/card.c
+++ b/drivers/staging/vt6655/card.c
@@ -913,7 +913,7 @@ u64 CARDqGetTSFOffset(unsigned char byRxRate, u64 qwTSF1, u64 qwTSF2)
 {
 	unsigned short wRxBcnTSFOffst;
 
-	wRxBcnTSFOffst = cwRXBCNTSFOff[byRxRate%MAX_RATE];
+	wRxBcnTSFOffst = cwRXBCNTSFOff[byRxRate % MAX_RATE];
 
 	qwTSF2 += (u64)wRxBcnTSFOffst;
 
diff --git a/drivers/staging/vt6655/card.h b/drivers/staging/vt6655/card.h
index 44420b5a445f..1a04dbb57d42 100644
--- a/drivers/staging/vt6655/card.h
+++ b/drivers/staging/vt6655/card.h
@@ -63,26 +63,26 @@ typedef enum _CARD_STATUS_TYPE {
 
 struct vnt_private;
 
-void CARDvSetRSPINF(struct vnt_private *, u8);
-void CARDvUpdateBasicTopRate(struct vnt_private *);
-bool CARDbIsOFDMinBasicRate(struct vnt_private *);
-void CARDvSetLoopbackMode(struct vnt_private *, unsigned short wLoopbackMode);
-bool CARDbSoftwareReset(struct vnt_private *);
-void CARDvSetFirstNextTBTT(struct vnt_private *,
+void CARDvSetRSPINF(struct vnt_private *priv, u8 bb_type);
+void CARDvUpdateBasicTopRate(struct vnt_private *priv);
+bool CARDbIsOFDMinBasicRate(struct vnt_private *priv);
+void CARDvSetLoopbackMode(struct vnt_private *priv, unsigned short wLoopbackMode);
+bool CARDbSoftwareReset(struct vnt_private *priv);
+void CARDvSetFirstNextTBTT(struct vnt_private *priv,
 			   unsigned short wBeaconInterval);
-void CARDvUpdateNextTBTT(struct vnt_private *, u64 qwTSF,
+void CARDvUpdateNextTBTT(struct vnt_private *priv, u64 qwTSF,
 			 unsigned short wBeaconInterval);
-bool CARDbGetCurrentTSF(struct vnt_private *, u64 *pqwCurrTSF);
+bool CARDbGetCurrentTSF(struct vnt_private *priv, u64 *pqwCurrTSF);
 u64 CARDqGetNextTBTT(u64 qwTSF, unsigned short wBeaconInterval);
 u64 CARDqGetTSFOffset(unsigned char byRxRate, u64 qwTSF1, u64 qwTSF2);
-unsigned char CARDbyGetPktType(struct vnt_private *);
-void CARDvSafeResetTx(struct vnt_private *);
-void CARDvSafeResetRx(struct vnt_private *);
-bool CARDbRadioPowerOff(struct vnt_private *);
-bool CARDbRadioPowerOn(struct vnt_private *);
-bool CARDbSetPhyParameter(struct vnt_private *, u8);
-bool CARDbUpdateTSF(struct vnt_private *, unsigned char byRxRate,
+unsigned char CARDbyGetPktType(struct vnt_private *priv);
+void CARDvSafeResetTx(struct vnt_private *priv);
+void CARDvSafeResetRx(struct vnt_private *priv);
+bool CARDbRadioPowerOff(struct vnt_private *priv);
+bool CARDbRadioPowerOn(struct vnt_private *priv);
+bool CARDbSetPhyParameter(struct vnt_private *priv, u8 bb_type);
+bool CARDbUpdateTSF(struct vnt_private *priv, unsigned char byRxRate,
 		    u64 qwBSSTimestamp);
-bool CARDbSetBeaconPeriod(struct vnt_private *, unsigned short wBeaconInterval);
+bool CARDbSetBeaconPeriod(struct vnt_private *priv, unsigned short wBeaconInterval);
 
 #endif /* __CARD_H__ */
diff --git a/drivers/staging/vt6655/channel.h b/drivers/staging/vt6655/channel.h
index 2621dfabff06..8fe70760e548 100644
--- a/drivers/staging/vt6655/channel.h
+++ b/drivers/staging/vt6655/channel.h
@@ -21,8 +21,8 @@
 
 #include "card.h"
 
-void vnt_init_bands(struct vnt_private *);
+void vnt_init_bands(struct vnt_private *priv);
 
-bool set_channel(struct vnt_private *, struct ieee80211_channel *);
+bool set_channel(struct vnt_private *priv, struct ieee80211_channel *ch);
 
 #endif /* _CHANNEL_H_ */
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
index da0f71191009..9fcf2e223f71 100644
--- a/drivers/staging/vt6655/device_main.c
+++ b/drivers/staging/vt6655/device_main.c
@@ -157,7 +157,7 @@ static void vt6655_remove(struct pci_dev *pcid)
 {
 	struct vnt_private *priv = pci_get_drvdata(pcid);
 
-	if (priv == NULL)
+	if (!priv)
 		return;
 	device_free_info(priv);
 }
@@ -453,7 +453,7 @@ static bool device_init_rings(struct vnt_private *priv)
 				       priv->opts.tx_descs[0] * sizeof(struct vnt_tx_desc) +
 				       priv->opts.tx_descs[1] * sizeof(struct vnt_tx_desc),
 				       &priv->pool_dma, GFP_ATOMIC);
-	if (vir_pool == NULL) {
+	if (!vir_pool) {
 		dev_err(&priv->pcid->dev, "allocate desc dma memory failed\n");
 		return false;
 	}
@@ -1018,7 +1018,6 @@ static void vnt_interrupt_process(struct vnt_private *priv)
 			}
 
 			/* TODO: adhoc PS mode */
-
 		}
 
 		if (isr & ISR_BNTX) {
@@ -1311,8 +1310,8 @@ static int vnt_config(struct ieee80211_hw *hw, u32 changed)
 }
 
 static void vnt_bss_info_changed(struct ieee80211_hw *hw,
-		struct ieee80211_vif *vif, struct ieee80211_bss_conf *conf,
-		u32 changed)
+				 struct ieee80211_vif *vif,
+				 struct ieee80211_bss_conf *conf, u32 changed)
 {
 	struct vnt_private *priv = hw->priv;
 
@@ -1402,7 +1401,7 @@ static void vnt_bss_info_changed(struct ieee80211_hw *hw,
 }
 
 static u64 vnt_prepare_multicast(struct ieee80211_hw *hw,
-	struct netdev_hw_addr_list *mc_list)
+				 struct netdev_hw_addr_list *mc_list)
 {
 	struct vnt_private *priv = hw->priv;
 	struct netdev_hw_addr *ha;
@@ -1421,7 +1420,8 @@ static u64 vnt_prepare_multicast(struct ieee80211_hw *hw,
 }
 
 static void vnt_configure(struct ieee80211_hw *hw,
-	unsigned int changed_flags, unsigned int *total_flags, u64 multicast)
+			  unsigned int changed_flags,
+			  unsigned int *total_flags, u64 multicast)
 {
 	struct vnt_private *priv = hw->priv;
 	u8 rx_mode = 0;
@@ -1482,8 +1482,8 @@ static void vnt_configure(struct ieee80211_hw *hw,
 }
 
 static int vnt_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 ieee80211_vif *vif, struct ieee80211_sta *sta,
+		       struct ieee80211_key_conf *key)
 {
 	struct vnt_private *priv = hw->priv;
 
@@ -1702,7 +1702,6 @@ static int vt6655_suspend(struct pci_dev *pcid, pm_message_t state)
 
 static int vt6655_resume(struct pci_dev *pcid)
 {
-
 	pci_set_power_state(pcid, PCI_D0);
 	pci_enable_wake(pcid, PCI_D0, 0);
 	pci_restore_state(pcid);
diff --git a/drivers/staging/vt6655/key.c b/drivers/staging/vt6655/key.c
index dad9e292d4da..d7ede73a1a01 100644
--- a/drivers/staging/vt6655/key.c
+++ b/drivers/staging/vt6655/key.c
@@ -27,8 +27,8 @@
 #include "mac.h"
 
 static int vnt_set_keymode(struct ieee80211_hw *hw, u8 *mac_addr,
-	struct ieee80211_key_conf *key, u32 key_type, u32 mode,
-	bool onfly_latch)
+			   struct ieee80211_key_conf *key, u32 key_type,
+			   u32 mode, bool onfly_latch)
 {
 	struct vnt_private *priv = hw->priv;
 	u8 broadcast[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
diff --git a/drivers/staging/vt6655/mac.h b/drivers/staging/vt6655/mac.h
index 33b758cb79d4..db401e32ae23 100644
--- a/drivers/staging/vt6655/mac.h
+++ b/drivers/staging/vt6655/mac.h
@@ -885,57 +885,57 @@ do {								\
 #define MACvSetRFLE_LatchBase(iobase)                                 \
 	MACvWordRegBitsOn(iobase, MAC_REG_SOFTPWRCTL, SOFTPWRCTL_RFLEOPT)
 
-bool MACbIsRegBitsOn(struct vnt_private *, unsigned char byRegOfs,
+bool MACbIsRegBitsOn(struct vnt_private *priv, unsigned char byRegOfs,
 		     unsigned char byTestBits);
-bool MACbIsRegBitsOff(struct vnt_private *, unsigned char byRegOfs,
+bool MACbIsRegBitsOff(struct vnt_private *priv, unsigned char byRegOfs,
 		      unsigned char byTestBits);
 
-bool MACbIsIntDisable(struct vnt_private *);
+bool MACbIsIntDisable(struct vnt_private *priv);
 
-void MACvSetShortRetryLimit(struct vnt_private *, unsigned char byRetryLimit);
+void MACvSetShortRetryLimit(struct vnt_private *priv, unsigned char byRetryLimit);
 
-void MACvSetLongRetryLimit(struct vnt_private *, unsigned char byRetryLimit);
-void MACvGetLongRetryLimit(struct vnt_private *,
+void MACvSetLongRetryLimit(struct vnt_private *priv, unsigned char byRetryLimit);
+void MACvGetLongRetryLimit(struct vnt_private *priv,
 			   unsigned char *pbyRetryLimit);
 
-void MACvSetLoopbackMode(struct vnt_private *, unsigned char byLoopbackMode);
+void MACvSetLoopbackMode(struct vnt_private *priv, unsigned char byLoopbackMode);
 
-void MACvSaveContext(struct vnt_private *, unsigned char *pbyCxtBuf);
-void MACvRestoreContext(struct vnt_private *, unsigned char *pbyCxtBuf);
+void MACvSaveContext(struct vnt_private *priv, unsigned char *pbyCxtBuf);
+void MACvRestoreContext(struct vnt_private *priv, unsigned char *pbyCxtBuf);
 
-bool MACbSoftwareReset(struct vnt_private *);
-bool MACbSafeSoftwareReset(struct vnt_private *);
-bool MACbSafeRxOff(struct vnt_private *);
-bool MACbSafeTxOff(struct vnt_private *);
-bool MACbSafeStop(struct vnt_private *);
-bool MACbShutdown(struct vnt_private *);
-void MACvInitialize(struct vnt_private *);
-void MACvSetCurrRx0DescAddr(struct vnt_private *,
+bool MACbSoftwareReset(struct vnt_private *priv);
+bool MACbSafeSoftwareReset(struct vnt_private *priv);
+bool MACbSafeRxOff(struct vnt_private *priv);
+bool MACbSafeTxOff(struct vnt_private *priv);
+bool MACbSafeStop(struct vnt_private *priv);
+bool MACbShutdown(struct vnt_private *priv);
+void MACvInitialize(struct vnt_private *priv);
+void MACvSetCurrRx0DescAddr(struct vnt_private *priv,
 			    u32 curr_desc_addr);
-void MACvSetCurrRx1DescAddr(struct vnt_private *,
+void MACvSetCurrRx1DescAddr(struct vnt_private *priv,
 			    u32 curr_desc_addr);
-void MACvSetCurrTXDescAddr(int iTxType, struct vnt_private *,
+void MACvSetCurrTXDescAddr(int iTxType, struct vnt_private *priv,
 			   u32 curr_desc_addr);
-void MACvSetCurrTx0DescAddrEx(struct vnt_private *,
+void MACvSetCurrTx0DescAddrEx(struct vnt_private *priv,
 			      u32 curr_desc_addr);
-void MACvSetCurrAC0DescAddrEx(struct vnt_private *,
+void MACvSetCurrAC0DescAddrEx(struct vnt_private *priv,
 			      u32 curr_desc_addr);
-void MACvSetCurrSyncDescAddrEx(struct vnt_private *,
+void MACvSetCurrSyncDescAddrEx(struct vnt_private *priv,
 			       u32 curr_desc_addr);
-void MACvSetCurrATIMDescAddrEx(struct vnt_private *,
+void MACvSetCurrATIMDescAddrEx(struct vnt_private *priv,
 			       u32 curr_desc_addr);
-void MACvTimer0MicroSDelay(struct vnt_private *, unsigned int uDelay);
-void MACvOneShotTimer1MicroSec(struct vnt_private *, unsigned int uDelayTime);
+void MACvTimer0MicroSDelay(struct vnt_private *priv, unsigned int uDelay);
+void MACvOneShotTimer1MicroSec(struct vnt_private *priv, unsigned int uDelayTime);
 
-void MACvSetMISCFifo(struct vnt_private *, unsigned short wOffset,
+void MACvSetMISCFifo(struct vnt_private *priv, unsigned short wOffset,
 		     u32 dwData);
 
-bool MACbPSWakeup(struct vnt_private *);
+bool MACbPSWakeup(struct vnt_private *priv);
 
-void MACvSetKeyEntry(struct vnt_private *, unsigned short wKeyCtl,
+void MACvSetKeyEntry(struct vnt_private *priv, unsigned short wKeyCtl,
 		     unsigned int uEntryIdx, unsigned int uKeyIdx,
 		     unsigned char *pbyAddr, u32 *pdwKey,
 		     unsigned char byLocalID);
-void MACvDisableKeyEntry(struct vnt_private *, unsigned int uEntryIdx);
+void MACvDisableKeyEntry(struct vnt_private *priv, unsigned int uEntryIdx);
 
 #endif /* __MAC_H__ */
diff --git a/drivers/staging/vt6655/power.h b/drivers/staging/vt6655/power.h
index dfcb0ca8b448..f360c5966523 100644
--- a/drivers/staging/vt6655/power.h
+++ b/drivers/staging/vt6655/power.h
@@ -31,20 +31,10 @@
 #define PS_FAST_INTERVAL         1       /* Fast power saving listen interval */
 #define PS_MAX_INTERVAL          4       /* MAX power saving listen interval */
 
-void
-PSvDisablePowerSaving(
-	struct vnt_private *
-);
+void PSvDisablePowerSaving(struct vnt_private *priv);
 
-void
-PSvEnablePowerSaving(
-	struct vnt_private *,
-	unsigned short wListenInterval
-);
+void PSvEnablePowerSaving(struct vnt_private *priv, unsigned short wListenInterval);
 
-bool
-PSbIsNextTBTTWakeUp(
-	struct vnt_private *
-);
+bool PSbIsNextTBTTWakeUp(struct vnt_private *priv);
 
 #endif /* __POWER_H__ */
diff --git a/drivers/staging/vt6655/rf.h b/drivers/staging/vt6655/rf.h
index 37600093cab2..ba222301d49d 100644
--- a/drivers/staging/vt6655/rf.h
+++ b/drivers/staging/vt6655/rf.h
@@ -68,28 +68,28 @@
 
 /*---------------------  Export Functions  --------------------------*/
 
-bool IFRFbWriteEmbedded(struct vnt_private *, unsigned long dwData);
-bool RFbSelectChannel(struct vnt_private *, unsigned char byRFType, u16);
+bool IFRFbWriteEmbedded(struct vnt_private *priv, unsigned long dwData);
+bool RFbSelectChannel(struct vnt_private *priv, unsigned char byRFType, u16 byChannel);
 bool RFbInit(
-	struct vnt_private *
+	struct vnt_private *priv
 );
-bool RFvWriteWakeProgSyn(struct vnt_private *, unsigned char byRFType, u16);
-bool RFbSetPower(struct vnt_private *, unsigned int rate, u16);
+bool RFvWriteWakeProgSyn(struct vnt_private *priv, unsigned char byRFType, u16 uChannel);
+bool RFbSetPower(struct vnt_private *priv, unsigned int rate, u16 uCH);
 bool RFbRawSetPower(
-	struct vnt_private *,
+	struct vnt_private *priv,
 	unsigned char byPwr,
 	unsigned int rate
 );
 
 void
 RFvRSSITodBm(
-	struct vnt_private *,
+	struct vnt_private *priv,
 	unsigned char byCurrRSSI,
 	long    *pldBm
 );
 
 /* {{ RobertYu: 20050104 */
-bool RFbAL7230SelectChannelPostProcess(struct vnt_private *, u16, u16);
+bool RFbAL7230SelectChannelPostProcess(struct vnt_private *priv, u16 byOldChannel, u16 byNewChannel);
 /* }} RobertYu */
 
 #endif /* __RF_H__ */
diff --git a/drivers/staging/vt6656/card.c b/drivers/staging/vt6656/card.c
index 0e5a99375099..c61422ea8846 100644
--- a/drivers/staging/vt6656/card.c
+++ b/drivers/staging/vt6656/card.c
@@ -359,35 +359,18 @@ void vnt_update_ifs(struct vnt_private *priv)
 		priv->sifs = C_SIFS_A;
 		priv->difs = C_SIFS_A + 2 * C_SLOT_SHORT;
 		max_min = 4;
-	} else if (priv->packet_type == PK_TYPE_11B) {
-		priv->slot = C_SLOT_LONG;
-		priv->sifs = C_SIFS_BG;
-		priv->difs = C_SIFS_BG + 2 * C_SLOT_LONG;
-		max_min = 5;
-	} else {/* PK_TYPE_11GA & PK_TYPE_11GB */
-		bool ofdm_rate = false;
-		unsigned int ii = 0;
-
+	} else {
 		priv->sifs = C_SIFS_BG;
 
-		if (priv->short_slot_time)
+		if (priv->short_slot_time) {
 			priv->slot = C_SLOT_SHORT;
-		else
+			max_min = 4;
+		} else {
 			priv->slot = C_SLOT_LONG;
-
-		priv->difs = C_SIFS_BG + 2 * priv->slot;
-
-		for (ii = RATE_54M; ii >= RATE_6M; ii--) {
-			if (priv->basic_rates & ((u32)(0x1 << ii))) {
-				ofdm_rate = true;
-				break;
-			}
+			max_min = 5;
 		}
 
-		if (ofdm_rate)
-			max_min = 4;
-		else
-			max_min = 5;
+		priv->difs = C_SIFS_BG + 2 * priv->slot;
 	}
 
 	priv->eifs = C_EIFS;
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index 028f54b453d0..095b85567306 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -513,6 +513,9 @@ static int vnt_start(struct ieee80211_hw *hw)
 		goto free_all;
 	}
 
+	if (vnt_key_init_table(priv))
+		goto free_all;
+
 	priv->int_interval = 1;  /* bInterval is set to 1 */
 
 	vnt_int_start_interrupt(priv);
@@ -634,7 +637,6 @@ static int vnt_config(struct ieee80211_hw *hw, u32 changed)
 {
 	struct vnt_private *priv = hw->priv;
 	struct ieee80211_conf *conf = &hw->conf;
-	u8 bb_type;
 
 	if (changed & IEEE80211_CONF_CHANGE_PS) {
 		if (conf->flags & IEEE80211_CONF_PS)
@@ -648,15 +650,9 @@ static int vnt_config(struct ieee80211_hw *hw, u32 changed)
 		vnt_set_channel(priv, conf->chandef.chan->hw_value);
 
 		if (conf->chandef.chan->band == NL80211_BAND_5GHZ)
-			bb_type = BB_TYPE_11A;
+			priv->bb_type = BB_TYPE_11A;
 		else
-			bb_type = BB_TYPE_11G;
-
-		if (priv->bb_type != bb_type) {
-			priv->bb_type = bb_type;
-
-			vnt_set_bss_mode(priv);
-		}
+			priv->bb_type = BB_TYPE_11G;
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_POWER) {
@@ -687,6 +683,7 @@ static void vnt_bss_info_changed(struct ieee80211_hw *hw,
 		priv->basic_rates = conf->basic_rates;
 
 		vnt_update_top_rates(priv);
+		vnt_set_bss_mode(priv);
 
 		dev_dbg(&priv->usb->dev, "basic rates %x\n", conf->basic_rates);
 	}
@@ -715,6 +712,7 @@ static void vnt_bss_info_changed(struct ieee80211_hw *hw,
 			priv->short_slot_time = false;
 
 		vnt_set_short_slot_time(priv);
+		vnt_update_ifs(priv);
 		vnt_set_vga_gain_offset(priv, priv->bb_vga[0]);
 		vnt_update_pre_ed_threshold(priv, false);
 	}
@@ -846,7 +844,6 @@ static void vnt_sw_scan_start(struct ieee80211_hw *hw,
 {
 	struct vnt_private *priv = hw->priv;
 
-	vnt_set_bss_mode(priv);
 	/* Set max sensitivity*/
 	vnt_update_pre_ed_threshold(priv, true);
 }
diff --git a/drivers/staging/vt6656/rxtx.c b/drivers/staging/vt6656/rxtx.c
index 63413492e61d..a44abcce6fb4 100644
--- a/drivers/staging/vt6656/rxtx.c
+++ b/drivers/staging/vt6656/rxtx.c
@@ -114,7 +114,7 @@ static __le16 vnt_time_stamp_off(struct vnt_private *priv, u16 rate)
 }
 
 static u32 vnt_get_rsvtime(struct vnt_private *priv, u8 pkt_type,
-			    u32 frame_length, u16 rate, int need_ack)
+			   u32 frame_length, u16 rate, int need_ack)
 {
 	u32 data_time, ack_time;
 
@@ -135,30 +135,37 @@ static u32 vnt_get_rsvtime(struct vnt_private *priv, u8 pkt_type,
 }
 
 static __le16 vnt_rxtx_rsvtime_le16(struct vnt_private *priv, u8 pkt_type,
-				     u32 frame_length, u16 rate, int need_ack)
+				    u32 frame_length, u16 rate, int need_ack)
 {
 	return cpu_to_le16((u16)vnt_get_rsvtime(priv, pkt_type,
 		frame_length, rate, need_ack));
 }
 
-static __le16 vnt_get_rtscts_rsvtime_le(struct vnt_private *priv,
-					 u8 rsv_type, u8 pkt_type, u32 frame_length, u16 current_rate)
+static __le16 vnt_get_rtscts_rsvtime_le(struct vnt_private *priv, u8 rsv_type,
+					u8 pkt_type, u32 frame_length,
+					u16 current_rate)
 {
 	u32 rrv_time, rts_time, cts_time, ack_time, data_time;
 
-	rrv_time = rts_time = cts_time = ack_time = data_time = 0;
+	rrv_time = 0;
+	rts_time = 0;
+	cts_time = 0;
+	ack_time = 0;
 
 	data_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
 				       frame_length, current_rate);
 
 	if (rsv_type == 0) {
-		rts_time = vnt_get_frame_time(priv->preamble_type,
-			pkt_type, 20, priv->top_cck_basic_rate);
-		cts_time = ack_time = vnt_get_frame_time(priv->preamble_type,
-			pkt_type, 14, priv->top_cck_basic_rate);
+		rts_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
+					      20, priv->top_cck_basic_rate);
+		ack_time = vnt_get_frame_time(priv->preamble_type,
+					      pkt_type, 14,
+					      priv->top_cck_basic_rate);
+		cts_time = ack_time;
+
 	} else if (rsv_type == 1) {
-		rts_time = vnt_get_frame_time(priv->preamble_type,
-			pkt_type, 20, priv->top_cck_basic_rate);
+		rts_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
+					      20, priv->top_cck_basic_rate);
 		cts_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
 					      14, priv->top_cck_basic_rate);
 		ack_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
@@ -166,8 +173,11 @@ static __le16 vnt_get_rtscts_rsvtime_le(struct vnt_private *priv,
 	} else if (rsv_type == 2) {
 		rts_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
 					      20, priv->top_ofdm_basic_rate);
-		cts_time = ack_time = vnt_get_frame_time(priv->preamble_type,
-			pkt_type, 14, priv->top_ofdm_basic_rate);
+		ack_time = vnt_get_frame_time(priv->preamble_type,
+					      pkt_type, 14,
+					      priv->top_ofdm_basic_rate);
+		cts_time = ack_time;
+
 	} else if (rsv_type == 3) {
 		cts_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
 					      14, priv->top_cck_basic_rate);
@@ -184,18 +194,20 @@ static __le16 vnt_get_rtscts_rsvtime_le(struct vnt_private *priv,
 	return cpu_to_le16((u16)rrv_time);
 }
 
-static __le16 vnt_get_duration_le(struct vnt_private *priv,
-				   u8 pkt_type, int need_ack)
+static __le16 vnt_get_duration_le(struct vnt_private *priv, u8 pkt_type,
+				  int need_ack)
 {
 	u32 ack_time = 0;
 
 	if (need_ack) {
 		if (pkt_type == PK_TYPE_11B)
 			ack_time = vnt_get_frame_time(priv->preamble_type,
-				pkt_type, 14, priv->top_cck_basic_rate);
+						      pkt_type, 14,
+						      priv->top_cck_basic_rate);
 		else
 			ack_time = vnt_get_frame_time(priv->preamble_type,
-				pkt_type, 14, priv->top_ofdm_basic_rate);
+						      pkt_type, 14,
+						      priv->top_ofdm_basic_rate);
 
 		return cpu_to_le16((u16)(priv->sifs + ack_time));
 	}
@@ -216,8 +228,8 @@ static __le16 vnt_get_rtscts_duration_le(struct vnt_usb_send_context *context,
 	case RTSDUR_BA:
 	case RTSDUR_BA_F0:
 	case RTSDUR_BA_F1:
-		cts_time = vnt_get_frame_time(priv->preamble_type,
-				pkt_type, 14, priv->top_cck_basic_rate);
+		cts_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
+					      14, priv->top_cck_basic_rate);
 		dur_time = cts_time + 2 * priv->sifs +
 			vnt_get_rsvtime(priv, pkt_type,
 					frame_length, rate, need_ack);
@@ -226,8 +238,8 @@ static __le16 vnt_get_rtscts_duration_le(struct vnt_usb_send_context *context,
 	case RTSDUR_AA:
 	case RTSDUR_AA_F0:
 	case RTSDUR_AA_F1:
-		cts_time = vnt_get_frame_time(priv->preamble_type,
-					      pkt_type, 14, priv->top_ofdm_basic_rate);
+		cts_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
+					      14, priv->top_ofdm_basic_rate);
 		dur_time = cts_time + 2 * priv->sifs +
 			vnt_get_rsvtime(priv, pkt_type,
 					frame_length, rate, need_ack);
@@ -248,7 +260,7 @@ static __le16 vnt_get_rtscts_duration_le(struct vnt_usb_send_context *context,
 }
 
 static u16 vnt_mac_hdr_pos(struct vnt_usb_send_context *tx_context,
-	struct ieee80211_hdr *hdr)
+			   struct ieee80211_hdr *hdr)
 {
 	u8 *head = tx_context->data + offsetof(struct vnt_tx_buffer, fifo_head);
 	u8 *hdr_pos = (u8 *)hdr;
@@ -263,7 +275,6 @@ static u16 vnt_mac_hdr_pos(struct vnt_usb_send_context *tx_context,
 static u16 vnt_rxtx_datahead_g(struct vnt_usb_send_context *tx_context,
 			       struct vnt_tx_datahead_g *buf)
 {
-
 	struct vnt_private *priv = tx_context->priv;
 	struct ieee80211_hdr *hdr =
 				(struct ieee80211_hdr *)tx_context->skb->data;
@@ -274,7 +285,7 @@ static u16 vnt_rxtx_datahead_g(struct vnt_usb_send_context *tx_context,
 	/* Get SignalField,ServiceField,Length */
 	vnt_get_phy_field(priv, frame_len, rate, tx_context->pkt_type, &buf->a);
 	vnt_get_phy_field(priv, frame_len, priv->top_cck_basic_rate,
-							PK_TYPE_11B, &buf->b);
+			  PK_TYPE_11B, &buf->b);
 
 	/* Get Duration and TimeStamp */
 	if (ieee80211_is_pspoll(hdr->frame_control)) {
@@ -310,7 +321,7 @@ static u16 vnt_rxtx_datahead_g_fb(struct vnt_usb_send_context *tx_context,
 	vnt_get_phy_field(priv, frame_len, rate, tx_context->pkt_type, &buf->a);
 
 	vnt_get_phy_field(priv, frame_len, priv->top_cck_basic_rate,
-						PK_TYPE_11B, &buf->b);
+			  PK_TYPE_11B, &buf->b);
 
 	/* Get Duration and TimeStamp */
 	buf->duration_a = vnt_get_duration_le(priv, tx_context->pkt_type,
@@ -387,7 +398,7 @@ static u16 vnt_rxtx_datahead_ab(struct vnt_usb_send_context *tx_context,
 }
 
 static int vnt_fill_ieee80211_rts(struct vnt_usb_send_context *tx_context,
-	struct ieee80211_rts *rts, __le16 duration)
+				  struct ieee80211_rts *rts, __le16 duration)
 {
 	struct ieee80211_hdr *hdr =
 				(struct ieee80211_hdr *)tx_context->skb->data;
@@ -499,8 +510,8 @@ static u16 vnt_rxtx_rts_a_fb_head(struct vnt_usb_send_context *tx_context,
 	u16 current_rate = tx_context->tx_rate;
 	u16 rts_frame_len = 20;
 
-	vnt_get_phy_field(priv, rts_frame_len,
-		priv->top_ofdm_basic_rate, tx_context->pkt_type, &buf->a);
+	vnt_get_phy_field(priv, rts_frame_len, priv->top_ofdm_basic_rate,
+			  tx_context->pkt_type, &buf->a);
 
 	buf->duration = vnt_get_rtscts_duration_le(tx_context, RTSDUR_AA,
 						   tx_context->pkt_type,
@@ -683,11 +694,10 @@ static u16 vnt_rxtx_ab(struct vnt_usb_send_context *tx_context,
 }
 
 static u16 vnt_generate_tx_parameter(struct vnt_usb_send_context *tx_context,
-				      struct vnt_tx_buffer *tx_buffer,
-				      struct vnt_mic_hdr **mic_hdr, u32 need_mic,
-				      bool need_rts)
+				     struct vnt_tx_buffer *tx_buffer,
+				     struct vnt_mic_hdr **mic_hdr, u32 need_mic,
+				     bool need_rts)
 {
-
 	if (tx_context->pkt_type == PK_TYPE_11GB ||
 	    tx_context->pkt_type == PK_TYPE_11GA) {
 		if (need_rts) {
@@ -712,8 +722,9 @@ static u16 vnt_generate_tx_parameter(struct vnt_usb_send_context *tx_context,
 }
 
 static void vnt_fill_txkey(struct vnt_usb_send_context *tx_context,
-	u8 *key_buffer, struct ieee80211_key_conf *tx_key, struct sk_buff *skb,
-	u16 payload_len, struct vnt_mic_hdr *mic_hdr)
+			   u8 *key_buffer, struct ieee80211_key_conf *tx_key,
+			   struct sk_buff *skb, u16 payload_len,
+			   struct vnt_mic_hdr *mic_hdr)
 {
 	struct ieee80211_hdr *hdr = tx_context->hdr;
 	u64 pn64;
@@ -774,14 +785,12 @@ static void vnt_fill_txkey(struct vnt_usb_send_context *tx_context,
 		if (ieee80211_has_a4(hdr->frame_control))
 			ether_addr_copy(mic_hdr->addr4, hdr->addr4);
 
-
 		memcpy(key_buffer, tx_key->key, WLAN_KEY_LEN_CCMP);
 
 		break;
 	default:
 		break;
 	}
-
 }
 
 int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
@@ -807,7 +816,7 @@ int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
 
 	current_rate = rate->hw_value;
 	if (priv->current_rate != current_rate &&
-			!(priv->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL)) {
+	    !(priv->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL)) {
 		priv->current_rate = current_rate;
 		vnt_schedule_command(priv, WLAN_CMD_SETPOWER);
 	}
@@ -964,7 +973,7 @@ int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
 		tx_key = info->control.hw_key;
 		if (tx_key->keylen > 0)
 			vnt_fill_txkey(tx_context, tx_buffer_head->tx_key,
-				tx_key, skb, tx_body_size, mic_hdr);
+				       tx_key, skb, tx_body_size, mic_hdr);
 	}
 
 	priv->seq_counter = (le16_to_cpu(hdr->seq_ctrl) &
@@ -991,8 +1000,7 @@ int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
 	return 0;
 }
 
-static int vnt_beacon_xmit(struct vnt_private *priv,
-	struct sk_buff *skb)
+static int vnt_beacon_xmit(struct vnt_private *priv, struct sk_buff *skb)
 {
 	struct vnt_beacon_buffer *beacon_buffer;
 	struct vnt_tx_short_buf_head *short_head;
@@ -1101,7 +1109,7 @@ int vnt_beacon_make(struct vnt_private *priv, struct ieee80211_vif *vif)
 }
 
 int vnt_beacon_enable(struct vnt_private *priv, struct ieee80211_vif *vif,
-		       struct ieee80211_bss_conf *conf)
+		      struct ieee80211_bss_conf *conf)
 {
 	vnt_mac_reg_bits_off(priv, MAC_REG_TCR, TCR_AUTOBCNTX);
 
diff --git a/drivers/staging/wilc1000/host_interface.c b/drivers/staging/wilc1000/host_interface.c
index c3a8af081880..2568dfc15181 100644
--- a/drivers/staging/wilc1000/host_interface.c
+++ b/drivers/staging/wilc1000/host_interface.c
@@ -329,25 +329,53 @@ static void handle_set_channel(struct wilc_vif *vif,
 		netdev_err(vif->ndev, "Failed to set channel\n");
 }
 
-static void handle_set_wfi_drv_handler(struct wilc_vif *vif,
-				       struct drv_handler *hif_drv_handler)
+static int handle_set_wfi_drv_handler(struct wilc_vif *vif,
+				      struct drv_handler *hif_drv_handler)
 {
 	int ret = 0;
 	struct wid wid;
+	u8 *currbyte, *buffer;
+	struct host_if_drv *hif_drv = NULL;
+
+	if (!vif->hif_drv)
+		return -EINVAL;
+
+	if (!hif_drv_handler)
+		return -EINVAL;
+
+	hif_drv	= vif->hif_drv;
+
+	buffer = kzalloc(DRV_HANDLER_SIZE, GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
+
+	currbyte = buffer;
+	*currbyte = hif_drv->driver_handler_id & DRV_HANDLER_MASK;
+	currbyte++;
+	*currbyte = (u32)0 & DRV_HANDLER_MASK;
+	currbyte++;
+	*currbyte = (u32)0 & DRV_HANDLER_MASK;
+	currbyte++;
+	*currbyte = (u32)0 & DRV_HANDLER_MASK;
+	currbyte++;
+	*currbyte = (hif_drv_handler->name | (hif_drv_handler->mode << 1));
 
 	wid.id = (u16)WID_SET_DRV_HANDLER;
 	wid.type = WID_STR;
-	wid.val = (s8 *)hif_drv_handler;
-	wid.size = sizeof(*hif_drv_handler);
+	wid.val = (s8 *)buffer;
+	wid.size = DRV_HANDLER_SIZE;
 
 	ret = wilc_send_config_pkt(vif, SET_CFG, &wid, 1,
-				   hif_drv_handler->handler);
-
-	if (!hif_drv_handler->handler)
-		complete(&hif_driver_comp);
-
-	if (ret)
+				   hif_drv->driver_handler_id);
+	if (ret) {
 		netdev_err(vif->ndev, "Failed to set driver handler\n");
+		complete(&hif_driver_comp);
+		kfree(buffer);
+		return ret;
+	}
+	complete(&hif_driver_comp);
+	kfree(buffer);
+	return 0;
 }
 
 static void handle_set_operation_mode(struct wilc_vif *vif,
@@ -1188,7 +1216,7 @@ static s32 Handle_ConnectTimeout(struct wilc_vif *vif)
 	result = wilc_send_config_pkt(vif, SET_CFG, &wid, 1,
 				      wilc_get_vif_idx(vif));
 	if (result)
-		netdev_err(vif->ndev, "Failed to send dissconect\n");
+		netdev_err(vif->ndev, "Failed to send disconnect\n");
 
 	hif_drv->usr_conn_req.ssid_len = 0;
 	kfree(hif_drv->usr_conn_req.ssid);
@@ -2052,23 +2080,9 @@ static u32 WILC_HostIf_PackStaParam(u8 *pu8Buffer,
 	pu8CurrByte += pstrStationParam->rates_len;
 
 	*pu8CurrByte++ = pstrStationParam->ht_supported;
-	*pu8CurrByte++ = pstrStationParam->ht_capa_info & 0xFF;
-	*pu8CurrByte++ = (pstrStationParam->ht_capa_info >> 8) & 0xFF;
-
-	*pu8CurrByte++ = pstrStationParam->ht_ampdu_params;
-	memcpy(pu8CurrByte, pstrStationParam->ht_supp_mcs_set,
-	       WILC_SUPP_MCS_SET_SIZE);
-	pu8CurrByte += WILC_SUPP_MCS_SET_SIZE;
-
-	*pu8CurrByte++ = pstrStationParam->ht_ext_params & 0xFF;
-	*pu8CurrByte++ = (pstrStationParam->ht_ext_params >> 8) & 0xFF;
-
-	*pu8CurrByte++ = pstrStationParam->ht_tx_bf_cap & 0xFF;
-	*pu8CurrByte++ = (pstrStationParam->ht_tx_bf_cap >> 8) & 0xFF;
-	*pu8CurrByte++ = (pstrStationParam->ht_tx_bf_cap >> 16) & 0xFF;
-	*pu8CurrByte++ = (pstrStationParam->ht_tx_bf_cap >> 24) & 0xFF;
-
-	*pu8CurrByte++ = pstrStationParam->ht_ante_sel;
+	memcpy(pu8CurrByte, &pstrStationParam->ht_capa,
+	       sizeof(struct ieee80211_ht_cap));
+	pu8CurrByte += sizeof(struct ieee80211_ht_cap);
 
 	*pu8CurrByte++ = pstrStationParam->flags_mask & 0xFF;
 	*pu8CurrByte++ = (pstrStationParam->flags_mask >> 8) & 0xFF;
@@ -2403,9 +2417,9 @@ static void Handle_SetMulticastFilter(struct wilc_vif *vif,
 
 	pu8CurrByte = wid.val;
 	*pu8CurrByte++ = (strHostIfSetMulti->enabled & 0xFF);
-	*pu8CurrByte++ = 0;
-	*pu8CurrByte++ = 0;
-	*pu8CurrByte++ = 0;
+	*pu8CurrByte++ = ((strHostIfSetMulti->enabled >> 8) & 0xFF);
+	*pu8CurrByte++ = ((strHostIfSetMulti->enabled >> 16) & 0xFF);
+	*pu8CurrByte++ = ((strHostIfSetMulti->enabled >> 24) & 0xFF);
 
 	*pu8CurrByte++ = (strHostIfSetMulti->cnt & 0xFF);
 	*pu8CurrByte++ = ((strHostIfSetMulti->cnt >> 8) & 0xFF);
@@ -2463,6 +2477,7 @@ static void host_if_work(struct work_struct *work)
 {
 	struct host_if_msg *msg;
 	struct wilc *wilc;
+	int ret = 0;
 
 	msg = container_of(work, struct host_if_msg, work);
 	wilc = msg->vif->wilc;
@@ -2568,7 +2583,7 @@ static void host_if_work(struct work_struct *work)
 		break;
 
 	case HOST_IF_MSG_SET_WFIDRV_HANDLER:
-		handle_set_wfi_drv_handler(msg->vif, &msg->body.drv);
+		ret = handle_set_wfi_drv_handler(msg->vif, &msg->body.drv);
 		break;
 
 	case HOST_IF_MSG_SET_OPERATION_MODE:
@@ -2622,6 +2637,8 @@ static void host_if_work(struct work_struct *work)
 		break;
 	}
 free_msg:
+	if (ret)
+		netdev_err(msg->vif->ndev, "Host cmd %d failed\n", msg->id);
 	kfree(msg);
 	complete(&hif_thread_comp);
 }
@@ -3099,7 +3116,8 @@ int wilc_set_mac_chnl_num(struct wilc_vif *vif, u8 channel)
 	return 0;
 }
 
-int wilc_set_wfi_drv_handler(struct wilc_vif *vif, int index, u8 mac_idx)
+int wilc_set_wfi_drv_handler(struct wilc_vif *vif, int index, u8 mode,
+			     u8 ifc_id)
 {
 	int result = 0;
 	struct host_if_msg msg;
@@ -3107,7 +3125,8 @@ int wilc_set_wfi_drv_handler(struct wilc_vif *vif, int index, u8 mac_idx)
 	memset(&msg, 0, sizeof(struct host_if_msg));
 	msg.id = HOST_IF_MSG_SET_WFIDRV_HANDLER;
 	msg.body.drv.handler = index;
-	msg.body.drv.mac_idx = mac_idx;
+	msg.body.drv.mode = mode;
+	msg.body.drv.name = ifc_id;
 	msg.vif = vif;
 
 	result = wilc_enqueue_cmd(&msg);
@@ -3330,6 +3349,7 @@ int wilc_init(struct net_device *dev, struct host_if_drv **hif_drv_handler)
 	for (i = 0; i < wilc->vif_num; i++)
 		if (dev == wilc->vif[i]->ndev) {
 			wilc->vif[i]->hif_drv = hif_drv;
+			hif_drv->driver_handler_id = i + 1;
 			break;
 		}
 
@@ -3403,7 +3423,7 @@ int wilc_deinit(struct wilc_vif *vif)
 	del_timer_sync(&periodic_rssi);
 	del_timer_sync(&hif_drv->remain_on_ch_timer);
 
-	wilc_set_wfi_drv_handler(vif, 0, 0);
+	wilc_set_wfi_drv_handler(vif, 0, 0, 0);
 	wait_for_completion(&hif_driver_comp);
 
 	if (hif_drv->usr_scan_req.scan_result) {
diff --git a/drivers/staging/wilc1000/host_interface.h b/drivers/staging/wilc1000/host_interface.h
index f36d3b5a0370..1ce5ead318c7 100644
--- a/drivers/staging/wilc1000/host_interface.h
+++ b/drivers/staging/wilc1000/host_interface.h
@@ -1,6 +1,6 @@
 #ifndef HOST_INT_H
 #define HOST_INT_H
-
+#include <linux/ieee80211.h>
 #include "coreconfigurator.h"
 
 #define IP_ALEN  4
@@ -47,10 +47,11 @@
 #define ETH_ALEN				6
 #define PMKID_LEN				16
 #define WILC_MAX_NUM_PMKIDS			16
-#define WILC_SUPP_MCS_SET_SIZE			16
 #define WILC_ADD_STA_LENGTH			40
 #define SCAN_EVENT_DONE_ABORTED
 #define NUM_CONCURRENT_IFC			2
+#define DRV_HANDLER_SIZE			5
+#define DRV_HANDLER_MASK			0x000000FF
 
 struct rf_info {
 	u8 link_speed;
@@ -217,7 +218,8 @@ struct user_conn_req {
 
 struct drv_handler {
 	u32 handler;
-	u8 mac_idx;
+	u8 mode;
+	u8 name;
 };
 
 struct op_mode {
@@ -281,6 +283,7 @@ struct host_if_drv {
 	struct timer_list remain_on_ch_timer;
 
 	bool IFC_UP;
+	int driver_handler_id;
 };
 
 struct add_sta_param {
@@ -289,12 +292,7 @@ struct add_sta_param {
 	u8 rates_len;
 	const u8 *rates;
 	bool ht_supported;
-	u16 ht_capa_info;
-	u8 ht_ampdu_params;
-	u8 ht_supp_mcs_set[16];
-	u16 ht_ext_params;
-	u32 ht_tx_bf_cap;
-	u8 ht_ante_sel;
+	struct ieee80211_ht_cap ht_capa;
 	u16 flags_mask;
 	u16 flags_set;
 };
@@ -354,7 +352,8 @@ int wilc_remain_on_channel(struct wilc_vif *vif, u32 session_id,
 			   void *user_arg);
 int wilc_listen_state_expired(struct wilc_vif *vif, u32 session_id);
 int wilc_frame_register(struct wilc_vif *vif, u16 frame_type, bool reg);
-int wilc_set_wfi_drv_handler(struct wilc_vif *vif, int index, u8 mac_idx);
+int wilc_set_wfi_drv_handler(struct wilc_vif *vif, int index, u8 mode,
+			     u8 ifc_id);
 int wilc_set_operation_mode(struct wilc_vif *vif, u32 mode);
 int wilc_get_statistics(struct wilc_vif *vif, struct rf_info *stats);
 void wilc_resolve_disconnect_aberration(struct wilc_vif *vif);
diff --git a/drivers/staging/wilc1000/linux_mon.c b/drivers/staging/wilc1000/linux_mon.c
index c9782d452b07..01efa80b4f88 100644
--- a/drivers/staging/wilc1000/linux_mon.c
+++ b/drivers/staging/wilc1000/linux_mon.c
@@ -72,9 +72,9 @@ void WILC_WFI_monitor_rx(u8 *buff, u32 size)
 		if (!skb)
 			return;
 
-		memcpy(skb_put(skb, size), buff, size);
+		skb_put_data(skb, buff, size);
 
-		cb_hdr = (struct wilc_wfi_radiotap_cb_hdr *)skb_push(skb, sizeof(*cb_hdr));
+		cb_hdr = skb_push(skb, sizeof(*cb_hdr));
 		memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));
 
 		cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
@@ -100,8 +100,8 @@ void WILC_WFI_monitor_rx(u8 *buff, u32 size)
 		if (!skb)
 			return;
 
-		memcpy(skb_put(skb, size), buff, size);
-		hdr = (struct wilc_wfi_radiotap_hdr *)skb_push(skb, sizeof(*hdr));
+		skb_put_data(skb, buff, size);
+		hdr = skb_push(skb, sizeof(*hdr));
 		memset(hdr, 0, sizeof(struct wilc_wfi_radiotap_hdr));
 		hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
 		hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_hdr));
@@ -200,9 +200,9 @@ static netdev_tx_t WILC_WFI_mon_xmit(struct sk_buff *skb,
 		if (!skb2)
 			return -ENOMEM;
 
-		memcpy(skb_put(skb2, skb->len), skb->data, skb->len);
+		skb_put_data(skb2, skb->data, skb->len);
 
-		cb_hdr = (struct wilc_wfi_radiotap_cb_hdr *)skb_push(skb2, sizeof(*cb_hdr));
+		cb_hdr = skb_push(skb2, sizeof(*cb_hdr));
 		memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));
 
 		cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
diff --git a/drivers/staging/wilc1000/linux_wlan.c b/drivers/staging/wilc1000/linux_wlan.c
index d6d803416be2..dbb3e24615be 100644
--- a/drivers/staging/wilc1000/linux_wlan.c
+++ b/drivers/staging/wilc1000/linux_wlan.c
@@ -858,34 +858,15 @@ static int wilc_mac_open(struct net_device *ndev)
 
 	for (i = 0; i < wl->vif_num; i++) {
 		if (ndev == wl->vif[i]->ndev) {
-			if (vif->iftype == AP_MODE) {
-				wilc_set_wfi_drv_handler(vif,
-							 wilc_get_vif_idx(vif),
-							 0);
-			} else if (!wilc_wlan_get_num_conn_ifcs(wl)) {
-				wilc_set_wfi_drv_handler(vif,
-							 wilc_get_vif_idx(vif),
-							 wl->open_ifcs);
-			} else {
-				if (memcmp(wl->vif[i ^ 1]->bssid,
-					   wl->vif[i ^ 1]->src_addr, 6))
-					wilc_set_wfi_drv_handler(vif,
-							 wilc_get_vif_idx(vif),
-							 0);
-				else
-					wilc_set_wfi_drv_handler(vif,
-							 wilc_get_vif_idx(vif),
-							 1);
-			}
+			wilc_set_wfi_drv_handler(vif, wilc_get_vif_idx(vif),
+						 vif->iftype, vif->ifc_id);
 			wilc_set_operation_mode(vif, vif->iftype);
-
-			wilc_get_mac_address(vif, mac_add);
-			netdev_dbg(ndev, "Mac address: %pM\n", mac_add);
-			memcpy(wl->vif[i]->src_addr, mac_add, ETH_ALEN);
-
 			break;
 		}
 	}
+			wilc_get_mac_address(vif, mac_add);
+			netdev_dbg(ndev, "Mac address: %pM\n", mac_add);
+			memcpy(wl->vif[i]->src_addr, mac_add, ETH_ALEN);
 
 	memcpy(ndev->dev_addr, wl->vif[i]->src_addr, ETH_ALEN);
 
@@ -1160,7 +1141,7 @@ void wilc_frmw_to_linux(struct wilc *wilc, u8 *buff, u32 size, u32 pkt_offset)
 
 		skb->dev = wilc_netdev;
 
-		memcpy(skb_put(skb, frame_len), buff_to_send, frame_len);
+		skb_put_data(skb, buff_to_send, frame_len);
 
 		skb->protocol = eth_type_trans(skb, wilc_netdev);
 		vif->netstats.rx_packets++;
@@ -1246,11 +1227,13 @@ int wilc_netdev_init(struct wilc **wilc, struct device *dev, int io_type,
 		vif = netdev_priv(ndev);
 		memset(vif, 0, sizeof(struct wilc_vif));
 
-		if (i == 0)
+		if (i == 0) {
 			strcpy(ndev->name, "wlan%d");
-		else
+			vif->ifc_id = 1;
+		} else {
 			strcpy(ndev->name, "p2p%d");
-
+			vif->ifc_id = 0;
+		}
 		vif->wilc = *wilc;
 		vif->ndev = ndev;
 		wl->vif[i] = vif;
diff --git a/drivers/staging/wilc1000/wilc_debugfs.c b/drivers/staging/wilc1000/wilc_debugfs.c
index 7d32de930576..ce54864569c7 100644
--- a/drivers/staging/wilc1000/wilc_debugfs.c
+++ b/drivers/staging/wilc1000/wilc_debugfs.c
@@ -20,7 +20,7 @@
 static struct dentry *wilc_dir;
 
 /*
- * --------------------------------------------------------------------------------
+ * ----------------------------------------------------------------------------
  */
 #define DEBUG           BIT(0)
 #define INFO            BIT(1)
@@ -32,10 +32,11 @@ static atomic_t WILC_DEBUG_LEVEL = ATOMIC_INIT(ERR);
 EXPORT_SYMBOL_GPL(WILC_DEBUG_LEVEL);
 
 /*
- * --------------------------------------------------------------------------------
+ * ----------------------------------------------------------------------------
  */
 
-static ssize_t wilc_debug_level_read(struct file *file, char __user *userbuf, size_t count, loff_t *ppos)
+static ssize_t wilc_debug_level_read(struct file *file, char __user *userbuf,
+				     size_t count, loff_t *ppos)
 {
 	char buf[128];
 	int res = 0;
@@ -44,13 +45,15 @@ static ssize_t wilc_debug_level_read(struct file *file, char __user *userbuf, si
 	if (*ppos > 0)
 		return 0;
 
-	res = scnprintf(buf, sizeof(buf), "Debug Level: %x\n", atomic_read(&WILC_DEBUG_LEVEL));
+	res = scnprintf(buf, sizeof(buf), "Debug Level: %x\n",
+			atomic_read(&WILC_DEBUG_LEVEL));
 
 	return simple_read_from_buffer(userbuf, count, ppos, buf, res);
 }
 
-static ssize_t wilc_debug_level_write(struct file *filp, const char __user *buf,
-				      size_t count, loff_t *ppos)
+static ssize_t wilc_debug_level_write(struct file *filp,
+				      const char __user *buf, size_t count,
+				      loff_t *ppos)
 {
 	int flag = 0;
 	int ret;
@@ -60,7 +63,8 @@ static ssize_t wilc_debug_level_write(struct file *filp, const char __user *buf,
 		return ret;
 
 	if (flag > DBG_LEVEL_ALL) {
-		pr_info("%s, value (0x%08x) is out of range, stay previous flag (0x%08x)\n", __func__, flag, atomic_read(&WILC_DEBUG_LEVEL));
+		pr_info("%s, value (0x%08x) is out of range, stay previous flag (0x%08x)\n",
+			__func__, flag, atomic_read(&WILC_DEBUG_LEVEL));
 		return -EINVAL;
 	}
 
@@ -75,7 +79,7 @@ static ssize_t wilc_debug_level_write(struct file *filp, const char __user *buf,
 }
 
 /*
- * --------------------------------------------------------------------------------
+ * ----------------------------------------------------------------------------
  */
 
 #define FOPS(_open, _read, _write, _poll) { \
@@ -94,7 +98,12 @@ struct wilc_debugfs_info_t {
 };
 
 static struct wilc_debugfs_info_t debugfs_info[] = {
-	{ "wilc_debug_level",	0666,	(DEBUG | ERR), FOPS(NULL, wilc_debug_level_read, wilc_debug_level_write, NULL), },
+	{
+		"wilc_debug_level",
+		0666,
+		(DEBUG | ERR),
+		FOPS(NULL, wilc_debug_level_read, wilc_debug_level_write, NULL),
+	},
 };
 
 static int __init wilc_debugfs_init(void)
@@ -122,4 +131,3 @@ static void __exit wilc_debugfs_remove(void)
 module_exit(wilc_debugfs_remove);
 
 #endif
-
diff --git a/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c b/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c
index 44a12bdd6a46..68fd5b3b8b2d 100644
--- a/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c
+++ b/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c
@@ -84,7 +84,6 @@ static const struct wiphy_wowlan_support wowlan_support = {
 #define TCP_ACK_FILTER_LINK_SPEED_THRESH	54
 #define DEFAULT_LINK_SPEED			72
 
-
 #define IS_MANAGMEMENT				0x100
 #define IS_MANAGMEMENT_CALLBACK			0x080
 #define IS_MGMT_STATUS_SUCCES			0x040
@@ -163,12 +162,12 @@ static struct ieee80211_supported_band WILC_WFI_band_2ghz = {
 	.n_bitrates = ARRAY_SIZE(ieee80211_bitrates),
 };
 
-
 struct add_key_params {
 	u8 key_idx;
 	bool pairwise;
 	u8 *mac_addr;
 };
+
 static struct add_key_params g_add_gtk_key_params;
 static struct wilc_wfi_key g_key_gtk_params;
 static struct add_key_params g_add_ptk_key_params;
@@ -281,7 +280,6 @@ static void remove_network_from_shadow(unsigned long arg)
 	unsigned long now = jiffies;
 	int i, j;
 
-
 	for (i = 0; i < last_scanned_cnt; i++) {
 		if (time_after(now, last_scanned_shadow[i].time_scan +
 			       (unsigned long)(SCAN_RESULT_EXPIRE))) {
@@ -526,7 +524,6 @@ static void CfgConnectResult(enum conn_event enuConnDisconnEvent,
 
 			memcpy(priv->au8AssociatedBss, pstrConnectInfo->bssid, ETH_ALEN);
 
-
 			for (i = 0; i < last_scanned_cnt; i++) {
 				if (memcmp(last_scanned_shadow[i].bssid,
 					   pstrConnectInfo->bssid,
@@ -626,7 +623,6 @@ static int scan(struct wiphy *wiphy, struct cfg80211_scan_request *request)
 				return -ENOMEM;
 			strHiddenNetwork.n_ssids = request->n_ssids;
 
-
 			for (i = 0; i < request->n_ssids; i++) {
 				if (request->ssids[i].ssid_len != 0) {
 					strHiddenNetwork.net_info[i].ssid = kmalloc(request->ssids[i].ssid_len, GFP_KERNEL);
@@ -927,8 +923,6 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 				priv->wilc_ptk[key_index]->seq = NULL;
 			}
 
-
-
 			if (!pairwise) {
 				if (params->cipher == WLAN_CIPHER_SUITE_TKIP)
 					u8gmode = ENCRYPT_ENABLED | WPA | TKIP;
@@ -968,7 +962,6 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 				else
 					u8pmode = priv->wilc_groupkey | AES;
 
-
 				if (params->key_len > 16 && params->cipher == WLAN_CIPHER_SUITE_TKIP) {
 					pu8TxMic = params->key + 24;
 					pu8RxMic = params->key + 16;
@@ -1153,7 +1146,6 @@ static int get_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 
 	priv = wiphy_priv(wiphy);
 
-
 	if (!pairwise) {
 		key_params.key = priv->wilc_gtk[key_index]->key;
 		key_params.cipher = priv->wilc_gtk[key_index]->cipher;
@@ -1298,7 +1290,6 @@ static int set_pmksa(struct wiphy *wiphy, struct net_device *netdev,
 
 	vif = netdev_priv(priv->dev);
 
-
 	for (i = 0; i < priv->pmkid_list.numpmkid; i++)	{
 		if (!memcmp(pmksa->bssid, priv->pmkid_list.pmkidlist[i].bssid,
 			    ETH_ALEN)) {
@@ -1511,7 +1502,6 @@ void WILC_WFI_p2p_rx(struct net_device *dev, u8 *buff, u32 size)
 						}
 					}
 
-
 					if ((buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP) && (wilc_ie))	{
 						cfg80211_rx_mgmt(priv->wdev, s32Freq, 0, buff, size - 7, 0);
 						return;
@@ -1533,7 +1523,6 @@ static void WILC_WFI_mgmt_tx_complete(void *priv, int status)
 {
 	struct p2p_mgmt_data *pv_data = priv;
 
-
 	kfree(pv_data->buff);
 	kfree(pv_data);
 }
@@ -1653,7 +1642,6 @@ static int mgmt_tx(struct wiphy *wiphy,
 		memcpy(mgmt_tx->buff, buf, len);
 		mgmt_tx->size = len;
 
-
 		if (ieee80211_is_probe_resp(mgmt->frame_control)) {
 			wilc_set_mac_chnl_num(vif, chan->hw_value);
 			curr_channel = chan->hw_value;
@@ -1832,7 +1820,6 @@ static int set_power_mgmt(struct wiphy *wiphy, struct net_device *dev,
 	if (wilc_enable_ps)
 		wilc_set_power_mgmt(vif, enabled, timeout);
 
-
 	return 0;
 }
 
@@ -1887,7 +1874,7 @@ static int change_virtual_intf(struct wiphy *wiphy, struct net_device *dev,
 
 		if (wl->initialized) {
 			wilc_set_wfi_drv_handler(vif, wilc_get_vif_idx(vif),
-						 0);
+						 0, vif->ifc_id);
 			wilc_set_operation_mode(vif, AP_MODE);
 			wilc_set_power_mgmt(vif, 0, 0);
 		}
@@ -2003,14 +1990,7 @@ static int add_station(struct wiphy *wiphy, struct net_device *dev,
 			strStaParams.ht_supported = false;
 		} else {
 			strStaParams.ht_supported = true;
-			strStaParams.ht_capa_info = params->ht_capa->cap_info;
-			strStaParams.ht_ampdu_params = params->ht_capa->ampdu_params_info;
-			memcpy(strStaParams.ht_supp_mcs_set,
-			       &params->ht_capa->mcs,
-			       WILC_SUPP_MCS_SET_SIZE);
-			strStaParams.ht_ext_params = params->ht_capa->extended_ht_cap_info;
-			strStaParams.ht_tx_bf_cap = params->ht_capa->tx_BF_cap_info;
-			strStaParams.ht_ante_sel = params->ht_capa->antenna_selection_info;
+			strStaParams.ht_capa = *params->ht_capa;
 		}
 
 		strStaParams.flags_mask = params->sta_flags_mask;
@@ -2075,14 +2055,7 @@ static int change_station(struct wiphy *wiphy, struct net_device *dev,
 			strStaParams.ht_supported = false;
 		} else {
 			strStaParams.ht_supported = true;
-			strStaParams.ht_capa_info = params->ht_capa->cap_info;
-			strStaParams.ht_ampdu_params = params->ht_capa->ampdu_params_info;
-			memcpy(strStaParams.ht_supp_mcs_set,
-			       &params->ht_capa->mcs,
-			       WILC_SUPP_MCS_SET_SIZE);
-			strStaParams.ht_ext_params = params->ht_capa->extended_ht_cap_info;
-			strStaParams.ht_tx_bf_cap = params->ht_capa->tx_BF_cap_info;
-			strStaParams.ht_ante_sel = params->ht_capa->antenna_selection_info;
+			strStaParams.ht_capa = *params->ht_capa;
 		}
 
 		strStaParams.flags_mask = params->sta_flags_mask;
@@ -2108,7 +2081,6 @@ static struct wireless_dev *add_virtual_intf(struct wiphy *wiphy,
 	priv = wiphy_priv(wiphy);
 	vif = netdev_priv(priv->wdev->netdev);
 
-
 	if (type == NL80211_IFTYPE_MONITOR) {
 		new_ifc = WILC_WFI_init_mon_interface(name, vif->ndev);
 		if (new_ifc) {
diff --git a/drivers/staging/wilc1000/wilc_wfi_netdevice.h b/drivers/staging/wilc1000/wilc_wfi_netdevice.h
index d431673bc46c..c89bf4301096 100644
--- a/drivers/staging/wilc1000/wilc_wfi_netdevice.h
+++ b/drivers/staging/wilc1000/wilc_wfi_netdevice.h
@@ -158,6 +158,7 @@ struct wilc_vif {
 	struct host_if_drv *hif_drv;
 	struct net_device *ndev;
 	u8 mode;
+	u8 ifc_id;
 };
 
 struct wilc {
diff --git a/drivers/staging/wilc1000/wilc_wlan_if.h b/drivers/staging/wilc1000/wilc_wlan_if.h
index 439ac6f8d533..f4d60057a06e 100644
--- a/drivers/staging/wilc1000/wilc_wlan_if.h
+++ b/drivers/staging/wilc1000/wilc_wlan_if.h
@@ -845,7 +845,7 @@ typedef enum {
 	WID_MODEL_NAME			= 0x3027, /*Added for CAPI tool */
 	WID_MODEL_NUM			= 0x3028, /*Added for CAPI tool */
 	WID_DEVICE_NAME			= 0x3029, /*Added for CAPI tool */
-	WID_SET_DRV_HANDLER		= 0x3030,
+	WID_SET_DRV_HANDLER		= 0x3079,
 
 	/* NMAC String WID list */
 	WID_11N_P_ACTION_REQ		= 0x3080,
diff --git a/drivers/staging/wlan-ng/hfa384x.h b/drivers/staging/wlan-ng/hfa384x.h
index 310e2c454590..018db2299d0c 100644
--- a/drivers/staging/wlan-ng/hfa384x.h
+++ b/drivers/staging/wlan-ng/hfa384x.h
@@ -353,12 +353,12 @@
 /*-------------------------------------------------------------*/
 /* Commonly used basic types */
 struct hfa384x_bytestr {
-	u16 len;
+	__le16 len;
 	u8 data[0];
 } __packed;
 
 struct hfa384x_bytestr32 {
-	u16 len;
+	__le16 len;
 	u8 data[32];
 } __packed;
 
@@ -399,8 +399,8 @@ struct hfa384x_caplevel {
 
 /*-- Configuration Record: HostScanRequest (data portion only) --*/
 struct hfa384x_host_scan_request_data {
-	u16 channel_list;
-	u16 tx_rate;
+	__le16 channel_list;
+	__le16 tx_rate;
 	struct hfa384x_bytestr32 ssid;
 } __packed;
 
@@ -419,7 +419,7 @@ struct hfa384x_authenticate_station_data {
 
 /*-- Configuration Record: WPAData       (data portion only) --*/
 struct hfa384x_wpa_data {
-	u16 datalen;
+	__le16 datalen;
 	u8 data[0];		/* max 80 */
 } __packed;
 
@@ -682,16 +682,16 @@ struct hfa384x_ch_info_result {
 
 /*--  Inquiry Frame, Diagnose: Host Scan Results & Subfields--*/
 struct hfa384x_hscan_result_sub {
-	u16 chid;
-	u16 anl;
-	u16 sl;
+	__le16 chid;
+	__le16 anl;
+	__le16 sl;
 	u8 bssid[WLAN_BSSID_LEN];
-	u16 bcnint;
-	u16 capinfo;
+	__le16 bcnint;
+	__le16 capinfo;
 	struct hfa384x_bytestr32 ssid;
 	u8 supprates[10];	/* 802.11 info element */
 	u16 proberesp_rate;
-	u16 atim;
+	__le16 atim;
 } __packed;
 
 struct hfa384x_hscan_result {
diff --git a/drivers/staging/wlan-ng/hfa384x_usb.c b/drivers/staging/wlan-ng/hfa384x_usb.c
index a812e55ba1b0..ee5fa86e941d 100644
--- a/drivers/staging/wlan-ng/hfa384x_usb.c
+++ b/drivers/staging/wlan-ng/hfa384x_usb.c
@@ -1840,15 +1840,15 @@ int hfa384x_drvr_flashdl_enable(struct hfa384x *hw)
 	if (result)
 		return result;
 
-	hw->bufinfo.page = le16_to_cpu(hw->bufinfo.page);
-	hw->bufinfo.offset = le16_to_cpu(hw->bufinfo.offset);
-	hw->bufinfo.len = le16_to_cpu(hw->bufinfo.len);
+	le16_to_cpus(&hw->bufinfo.page);
+	le16_to_cpus(&hw->bufinfo.offset);
+	le16_to_cpus(&hw->bufinfo.len);
 	result = hfa384x_drvr_getconfig16(hw, HFA384x_RID_MAXLOADTIME,
 					  &hw->dltimeout);
 	if (result)
 		return result;
 
-	hw->dltimeout = le16_to_cpu(hw->dltimeout);
+	le16_to_cpus(&hw->dltimeout);
 
 	pr_debug("flashdl_enable\n");
 
@@ -2644,8 +2644,7 @@ int hfa384x_drvr_txframe(struct hfa384x *hw, struct sk_buff *skb,
 	    HFA384x_TX_MACPORT_SET(0) | HFA384x_TX_STRUCTYPE_SET(1) |
 	    HFA384x_TX_TXEX_SET(0) | HFA384x_TX_TXOK_SET(0);
 #endif
-	hw->txbuff.txfrm.desc.tx_control =
-	    cpu_to_le16(hw->txbuff.txfrm.desc.tx_control);
+	cpu_to_le16s(&hw->txbuff.txfrm.desc.tx_control);
 
 	/* copy the header over to the txdesc */
 	memcpy(&hw->txbuff.txfrm.desc.frame_control, p80211_hdr,
@@ -3380,8 +3379,8 @@ static void hfa384x_usbin_rx(struct wlandevice *wlandev, struct sk_buff *skb)
 	u16 fc;
 
 	/* Byte order convert once up front. */
-	usbin->rxfrm.desc.status = le16_to_cpu(usbin->rxfrm.desc.status);
-	usbin->rxfrm.desc.time = le32_to_cpu(usbin->rxfrm.desc.time);
+	le16_to_cpus(&usbin->rxfrm.desc.status);
+	le32_to_cpus(&usbin->rxfrm.desc.time);
 
 	/* Now handle frame based on port# */
 	switch (HFA384x_RXSTATUS_MACPORT_GET(usbin->rxfrm.desc.status)) {
@@ -3530,13 +3529,11 @@ static void hfa384x_int_rxmonitor(struct wlandevice *wlandev,
 	/* Copy the 802.11 header to the skb
 	 * (ctl frames may be less than a full header)
 	 */
-	datap = skb_put(skb, hdrlen);
-	memcpy(datap, &rxdesc->frame_control, hdrlen);
+	skb_put_data(skb, &rxdesc->frame_control, hdrlen);
 
 	/* If any, copy the data from the card to the skb */
 	if (datalen > 0) {
-		datap = skb_put(skb, datalen);
-		memcpy(datap, rxfrm->data, datalen);
+		datap = skb_put_data(skb, rxfrm->data, datalen);
 
 		/* check for unencrypted stuff if WEP bit set. */
 		if (*(datap - hdrlen + 1) & 0x40)	/* wep set */
@@ -3576,8 +3573,7 @@ static void hfa384x_int_rxmonitor(struct wlandevice *wlandev,
 static void hfa384x_usbin_info(struct wlandevice *wlandev,
 			       union hfa384x_usbin *usbin)
 {
-	usbin->infofrm.info.framelen =
-	    le16_to_cpu(usbin->infofrm.info.framelen);
+	le16_to_cpus(&usbin->infofrm.info.framelen);
 	prism2sta_ev_info(wlandev, &usbin->infofrm.info);
 }
 
diff --git a/drivers/staging/wlan-ng/p80211conv.c b/drivers/staging/wlan-ng/p80211conv.c
index a062e80361ef..fc8ad33ade9f 100644
--- a/drivers/staging/wlan-ng/p80211conv.c
+++ b/drivers/staging/wlan-ng/p80211conv.c
@@ -148,9 +148,7 @@ int skb_ether_to_p80211(struct wlandevice *wlandev, u32 ethconv,
 			skb_pull(skb, ETH_HLEN);
 
 			/* tack on SNAP */
-			e_snap =
-			    (struct wlan_snap *)skb_push(skb,
-				sizeof(struct wlan_snap));
+			e_snap = skb_push(skb, sizeof(struct wlan_snap));
 			e_snap->type = htons(proto);
 			if (ethconv == WLAN_ETHCONV_8021h &&
 			    p80211_stt_findproto(proto)) {
@@ -162,9 +160,7 @@ int skb_ether_to_p80211(struct wlandevice *wlandev, u32 ethconv,
 			}
 
 			/* tack on llc */
-			e_llc =
-			    (struct wlan_llc *)skb_push(skb,
-				sizeof(struct wlan_llc));
+			e_llc = skb_push(skb, sizeof(struct wlan_llc));
 			e_llc->dsap = 0xAA;	/* SNAP, see IEEE 802 */
 			e_llc->ssap = 0xAA;
 			e_llc->ctl = 0x03;
@@ -407,7 +403,7 @@ int skb_p80211_to_ether(struct wlandevice *wlandev, u32 ethconv,
 		skb_pull(skb, payload_offset);
 
 		/* create 802.3 header at beginning of skb. */
-		e_hdr = (struct wlan_ethhdr *)skb_push(skb, ETH_HLEN);
+		e_hdr = skb_push(skb, ETH_HLEN);
 		ether_addr_copy(e_hdr->daddr, daddr);
 		ether_addr_copy(e_hdr->saddr, saddr);
 		e_hdr->type = htons(payload_length);
@@ -448,7 +444,7 @@ int skb_p80211_to_ether(struct wlandevice *wlandev, u32 ethconv,
 		skb_pull(skb, sizeof(struct wlan_snap));
 
 		/* create 802.3 header at beginning of skb. */
-		e_hdr = (struct wlan_ethhdr *)skb_push(skb, ETH_HLEN);
+		e_hdr = skb_push(skb, ETH_HLEN);
 		e_hdr->type = e_snap->type;
 		ether_addr_copy(e_hdr->daddr, daddr);
 		ether_addr_copy(e_hdr->saddr, saddr);
@@ -475,7 +471,7 @@ int skb_p80211_to_ether(struct wlandevice *wlandev, u32 ethconv,
 		skb_pull(skb, payload_offset);
 
 		/* create 802.3 header at beginning of skb. */
-		e_hdr = (struct wlan_ethhdr *)skb_push(skb, ETH_HLEN);
+		e_hdr = skb_push(skb, ETH_HLEN);
 		ether_addr_copy(e_hdr->daddr, daddr);
 		ether_addr_copy(e_hdr->saddr, saddr);
 		e_hdr->type = htons(payload_length);
diff --git a/drivers/staging/wlan-ng/prism2fw.c b/drivers/staging/wlan-ng/prism2fw.c
index afd877fb4557..1a0c786c7616 100644
--- a/drivers/staging/wlan-ng/prism2fw.c
+++ b/drivers/staging/wlan-ng/prism2fw.c
@@ -617,28 +617,28 @@ static int mkpdrlist(struct pda *pda)
 		    HFA384x_PDR_NICID) {
 			memcpy(&nicid, &pda->rec[pda->nrec]->data.nicid,
 			       sizeof(nicid));
-			nicid.id = le16_to_cpu(nicid.id);
-			nicid.variant = le16_to_cpu(nicid.variant);
-			nicid.major = le16_to_cpu(nicid.major);
-			nicid.minor = le16_to_cpu(nicid.minor);
+			le16_to_cpus(&nicid.id);
+			le16_to_cpus(&nicid.variant);
+			le16_to_cpus(&nicid.major);
+			le16_to_cpus(&nicid.minor);
 		}
 		if (le16_to_cpu(pda->rec[pda->nrec]->code) ==
 		    HFA384x_PDR_MFISUPRANGE) {
 			memcpy(&rfid, &pda->rec[pda->nrec]->data.mfisuprange,
 			       sizeof(rfid));
-			rfid.id = le16_to_cpu(rfid.id);
-			rfid.variant = le16_to_cpu(rfid.variant);
-			rfid.bottom = le16_to_cpu(rfid.bottom);
-			rfid.top = le16_to_cpu(rfid.top);
+			le16_to_cpus(&rfid.id);
+			le16_to_cpus(&rfid.variant);
+			le16_to_cpus(&rfid.bottom);
+			le16_to_cpus(&rfid.top);
 		}
 		if (le16_to_cpu(pda->rec[pda->nrec]->code) ==
 		    HFA384x_PDR_CFISUPRANGE) {
 			memcpy(&macid, &pda->rec[pda->nrec]->data.cfisuprange,
 			       sizeof(macid));
-			macid.id = le16_to_cpu(macid.id);
-			macid.variant = le16_to_cpu(macid.variant);
-			macid.bottom = le16_to_cpu(macid.bottom);
-			macid.top = le16_to_cpu(macid.top);
+			le16_to_cpus(&macid.id);
+			le16_to_cpus(&macid.variant);
+			le16_to_cpus(&macid.bottom);
+			le16_to_cpus(&macid.top);
 		}
 
 		(pda->nrec)++;
diff --git a/drivers/staging/wlan-ng/prism2mgmt.c b/drivers/staging/wlan-ng/prism2mgmt.c
index e23a0d0e7b09..c4aa9e7e7003 100644
--- a/drivers/staging/wlan-ng/prism2mgmt.c
+++ b/drivers/staging/wlan-ng/prism2mgmt.c
@@ -170,7 +170,7 @@ int prism2mgmt_scan(struct wlandevice *wlandev, void *msgp)
 				     hw->ident_sta_fw.variant) >
 	    HFA384x_FIRMWARE_VERSION(1, 5, 0)) {
 		if (msg->scantype.data != P80211ENUM_scantype_active)
-			word = cpu_to_le16(msg->maxchanneltime.data);
+			word = msg->maxchanneltime.data;
 		else
 			word = 0;
 
@@ -213,7 +213,7 @@ int prism2mgmt_scan(struct wlandevice *wlandev, void *msgp)
 		goto exit;
 	}
 	if (word == HFA384x_PORTSTATUS_DISABLED) {
-		u16 wordbuf[17];
+		__le16 wordbuf[17];
 
 		result = hfa384x_drvr_setconfig16(hw,
 					HFA384x_RID_CNFROAMINGMODE,
diff --git a/drivers/staging/wlan-ng/prism2mib.c b/drivers/staging/wlan-ng/prism2mib.c
index 28df1f3d6f4a..e41207d97309 100644
--- a/drivers/staging/wlan-ng/prism2mib.c
+++ b/drivers/staging/wlan-ng/prism2mib.c
@@ -774,7 +774,7 @@ void prism2mgmt_pstr2bytestr(struct hfa384x_bytestr *bytestr,
 void prism2mgmt_bytestr2pstr(struct hfa384x_bytestr *bytestr,
 			     struct p80211pstrd *pstr)
 {
-	pstr->len = (u8)(le16_to_cpu((u16)(bytestr->len)));
+	pstr->len = (u8)(le16_to_cpu(bytestr->len));
 	memcpy(pstr->data, bytestr->data, pstr->len);
 }
 
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
index 9c2b4ef2de58..e16da34389cd 100644
--- a/drivers/staging/wlan-ng/prism2sta.c
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -603,10 +603,10 @@ static int prism2sta_getcardinfo(struct wlandevice *wlandev)
 	}
 
 	/* get all the nic id fields in host byte order */
-	hw->ident_nic.id = le16_to_cpu(hw->ident_nic.id);
-	hw->ident_nic.variant = le16_to_cpu(hw->ident_nic.variant);
-	hw->ident_nic.major = le16_to_cpu(hw->ident_nic.major);
-	hw->ident_nic.minor = le16_to_cpu(hw->ident_nic.minor);
+	le16_to_cpus(&hw->ident_nic.id);
+	le16_to_cpus(&hw->ident_nic.variant);
+	le16_to_cpus(&hw->ident_nic.major);
+	le16_to_cpus(&hw->ident_nic.minor);
 
 	netdev_info(wlandev->netdev, "ident: nic h/w: id=0x%02x %d.%d.%d\n",
 		    hw->ident_nic.id, hw->ident_nic.major,
@@ -622,10 +622,10 @@ static int prism2sta_getcardinfo(struct wlandevice *wlandev)
 	}
 
 	/* get all the private fw id fields in host byte order */
-	hw->ident_pri_fw.id = le16_to_cpu(hw->ident_pri_fw.id);
-	hw->ident_pri_fw.variant = le16_to_cpu(hw->ident_pri_fw.variant);
-	hw->ident_pri_fw.major = le16_to_cpu(hw->ident_pri_fw.major);
-	hw->ident_pri_fw.minor = le16_to_cpu(hw->ident_pri_fw.minor);
+	le16_to_cpus(&hw->ident_pri_fw.id);
+	le16_to_cpus(&hw->ident_pri_fw.variant);
+	le16_to_cpus(&hw->ident_pri_fw.major);
+	le16_to_cpus(&hw->ident_pri_fw.minor);
 
 	netdev_info(wlandev->netdev, "ident: pri f/w: id=0x%02x %d.%d.%d\n",
 		    hw->ident_pri_fw.id, hw->ident_pri_fw.major,
@@ -648,10 +648,10 @@ static int prism2sta_getcardinfo(struct wlandevice *wlandev)
 	}
 
 	/* get all the station fw id fields in host byte order */
-	hw->ident_sta_fw.id = le16_to_cpu(hw->ident_sta_fw.id);
-	hw->ident_sta_fw.variant = le16_to_cpu(hw->ident_sta_fw.variant);
-	hw->ident_sta_fw.major = le16_to_cpu(hw->ident_sta_fw.major);
-	hw->ident_sta_fw.minor = le16_to_cpu(hw->ident_sta_fw.minor);
+	le16_to_cpus(&hw->ident_sta_fw.id);
+	le16_to_cpus(&hw->ident_sta_fw.variant);
+	le16_to_cpus(&hw->ident_sta_fw.major);
+	le16_to_cpus(&hw->ident_sta_fw.minor);
 
 	/* strip out the 'special' variant bits */
 	hw->mm_mods = hw->ident_sta_fw.variant & GENMASK(15, 14);
@@ -683,11 +683,11 @@ static int prism2sta_getcardinfo(struct wlandevice *wlandev)
 	/* get all the Compatibility range, modem interface supplier
 	 * fields in byte order
 	 */
-	hw->cap_sup_mfi.role = le16_to_cpu(hw->cap_sup_mfi.role);
-	hw->cap_sup_mfi.id = le16_to_cpu(hw->cap_sup_mfi.id);
-	hw->cap_sup_mfi.variant = le16_to_cpu(hw->cap_sup_mfi.variant);
-	hw->cap_sup_mfi.bottom = le16_to_cpu(hw->cap_sup_mfi.bottom);
-	hw->cap_sup_mfi.top = le16_to_cpu(hw->cap_sup_mfi.top);
+	le16_to_cpus(&hw->cap_sup_mfi.role);
+	le16_to_cpus(&hw->cap_sup_mfi.id);
+	le16_to_cpus(&hw->cap_sup_mfi.variant);
+	le16_to_cpus(&hw->cap_sup_mfi.bottom);
+	le16_to_cpus(&hw->cap_sup_mfi.top);
 
 	netdev_info(wlandev->netdev,
 		    "MFI:SUP:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
@@ -707,11 +707,11 @@ static int prism2sta_getcardinfo(struct wlandevice *wlandev)
 	/* get all the Compatibility range, controller interface supplier
 	 * fields in byte order
 	 */
-	hw->cap_sup_cfi.role = le16_to_cpu(hw->cap_sup_cfi.role);
-	hw->cap_sup_cfi.id = le16_to_cpu(hw->cap_sup_cfi.id);
-	hw->cap_sup_cfi.variant = le16_to_cpu(hw->cap_sup_cfi.variant);
-	hw->cap_sup_cfi.bottom = le16_to_cpu(hw->cap_sup_cfi.bottom);
-	hw->cap_sup_cfi.top = le16_to_cpu(hw->cap_sup_cfi.top);
+	le16_to_cpus(&hw->cap_sup_cfi.role);
+	le16_to_cpus(&hw->cap_sup_cfi.id);
+	le16_to_cpus(&hw->cap_sup_cfi.variant);
+	le16_to_cpus(&hw->cap_sup_cfi.bottom);
+	le16_to_cpus(&hw->cap_sup_cfi.top);
 
 	netdev_info(wlandev->netdev,
 		    "CFI:SUP:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
@@ -731,11 +731,11 @@ static int prism2sta_getcardinfo(struct wlandevice *wlandev)
 	/* get all the Compatibility range, primary firmware supplier
 	 * fields in byte order
 	 */
-	hw->cap_sup_pri.role = le16_to_cpu(hw->cap_sup_pri.role);
-	hw->cap_sup_pri.id = le16_to_cpu(hw->cap_sup_pri.id);
-	hw->cap_sup_pri.variant = le16_to_cpu(hw->cap_sup_pri.variant);
-	hw->cap_sup_pri.bottom = le16_to_cpu(hw->cap_sup_pri.bottom);
-	hw->cap_sup_pri.top = le16_to_cpu(hw->cap_sup_pri.top);
+	le16_to_cpus(&hw->cap_sup_pri.role);
+	le16_to_cpus(&hw->cap_sup_pri.id);
+	le16_to_cpus(&hw->cap_sup_pri.variant);
+	le16_to_cpus(&hw->cap_sup_pri.bottom);
+	le16_to_cpus(&hw->cap_sup_pri.top);
 
 	netdev_info(wlandev->netdev,
 		    "PRI:SUP:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
@@ -755,11 +755,11 @@ static int prism2sta_getcardinfo(struct wlandevice *wlandev)
 	/* get all the Compatibility range, station firmware supplier
 	 * fields in byte order
 	 */
-	hw->cap_sup_sta.role = le16_to_cpu(hw->cap_sup_sta.role);
-	hw->cap_sup_sta.id = le16_to_cpu(hw->cap_sup_sta.id);
-	hw->cap_sup_sta.variant = le16_to_cpu(hw->cap_sup_sta.variant);
-	hw->cap_sup_sta.bottom = le16_to_cpu(hw->cap_sup_sta.bottom);
-	hw->cap_sup_sta.top = le16_to_cpu(hw->cap_sup_sta.top);
+	le16_to_cpus(&hw->cap_sup_sta.role);
+	le16_to_cpus(&hw->cap_sup_sta.id);
+	le16_to_cpus(&hw->cap_sup_sta.variant);
+	le16_to_cpus(&hw->cap_sup_sta.bottom);
+	le16_to_cpus(&hw->cap_sup_sta.top);
 
 	if (hw->cap_sup_sta.id == 0x04) {
 		netdev_info(wlandev->netdev,
@@ -787,11 +787,11 @@ static int prism2sta_getcardinfo(struct wlandevice *wlandev)
 	/* get all the Compatibility range, primary f/w actor, CFI supplier
 	 * fields in byte order
 	 */
-	hw->cap_act_pri_cfi.role = le16_to_cpu(hw->cap_act_pri_cfi.role);
-	hw->cap_act_pri_cfi.id = le16_to_cpu(hw->cap_act_pri_cfi.id);
-	hw->cap_act_pri_cfi.variant = le16_to_cpu(hw->cap_act_pri_cfi.variant);
-	hw->cap_act_pri_cfi.bottom = le16_to_cpu(hw->cap_act_pri_cfi.bottom);
-	hw->cap_act_pri_cfi.top = le16_to_cpu(hw->cap_act_pri_cfi.top);
+	le16_to_cpus(&hw->cap_act_pri_cfi.role);
+	le16_to_cpus(&hw->cap_act_pri_cfi.id);
+	le16_to_cpus(&hw->cap_act_pri_cfi.variant);
+	le16_to_cpus(&hw->cap_act_pri_cfi.bottom);
+	le16_to_cpus(&hw->cap_act_pri_cfi.top);
 
 	netdev_info(wlandev->netdev,
 		    "PRI-CFI:ACT:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
@@ -811,11 +811,11 @@ static int prism2sta_getcardinfo(struct wlandevice *wlandev)
 	/* get all the Compatibility range, station f/w actor, CFI supplier
 	 * fields in byte order
 	 */
-	hw->cap_act_sta_cfi.role = le16_to_cpu(hw->cap_act_sta_cfi.role);
-	hw->cap_act_sta_cfi.id = le16_to_cpu(hw->cap_act_sta_cfi.id);
-	hw->cap_act_sta_cfi.variant = le16_to_cpu(hw->cap_act_sta_cfi.variant);
-	hw->cap_act_sta_cfi.bottom = le16_to_cpu(hw->cap_act_sta_cfi.bottom);
-	hw->cap_act_sta_cfi.top = le16_to_cpu(hw->cap_act_sta_cfi.top);
+	le16_to_cpus(&hw->cap_act_sta_cfi.role);
+	le16_to_cpus(&hw->cap_act_sta_cfi.id);
+	le16_to_cpus(&hw->cap_act_sta_cfi.variant);
+	le16_to_cpus(&hw->cap_act_sta_cfi.bottom);
+	le16_to_cpus(&hw->cap_act_sta_cfi.top);
 
 	netdev_info(wlandev->netdev,
 		    "STA-CFI:ACT:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
@@ -835,11 +835,11 @@ static int prism2sta_getcardinfo(struct wlandevice *wlandev)
 	/* get all the Compatibility range, station f/w actor, MFI supplier
 	 * fields in byte order
 	 */
-	hw->cap_act_sta_mfi.role = le16_to_cpu(hw->cap_act_sta_mfi.role);
-	hw->cap_act_sta_mfi.id = le16_to_cpu(hw->cap_act_sta_mfi.id);
-	hw->cap_act_sta_mfi.variant = le16_to_cpu(hw->cap_act_sta_mfi.variant);
-	hw->cap_act_sta_mfi.bottom = le16_to_cpu(hw->cap_act_sta_mfi.bottom);
-	hw->cap_act_sta_mfi.top = le16_to_cpu(hw->cap_act_sta_mfi.top);
+	le16_to_cpus(&hw->cap_act_sta_mfi.role);
+	le16_to_cpus(&hw->cap_act_sta_mfi.id);
+	le16_to_cpus(&hw->cap_act_sta_mfi.variant);
+	le16_to_cpus(&hw->cap_act_sta_mfi.bottom);
+	le16_to_cpus(&hw->cap_act_sta_mfi.top);
 
 	netdev_info(wlandev->netdev,
 		    "STA-MFI:ACT:role=0x%02x:id=0x%02x:var=0x%02x:b/t=%d/%d\n",
@@ -1478,8 +1478,8 @@ static void prism2sta_inf_assocstatus(struct wlandevice *wlandev,
 	int i;
 
 	memcpy(&rec, &inf->info.assocstatus, sizeof(rec));
-	rec.assocstatus = le16_to_cpu(rec.assocstatus);
-	rec.reason = le16_to_cpu(rec.reason);
+	le16_to_cpus(&rec.assocstatus);
+	le16_to_cpus(&rec.reason);
 
 	/*
 	 * Find the address in the list of authenticated stations.
@@ -1748,7 +1748,7 @@ static void prism2sta_inf_psusercnt(struct wlandevice *wlandev,
 void prism2sta_ev_info(struct wlandevice *wlandev,
 		       struct hfa384x_inf_frame *inf)
 {
-	inf->infotype = le16_to_cpu(inf->infotype);
+	le16_to_cpus(&inf->infotype);
 	/* Dispatch */
 	switch (inf->infotype) {
 	case HFA384x_IT_HANDOVERADDR:
diff --git a/drivers/staging/xgifb/vb_table.h b/drivers/staging/xgifb/vb_table.h
index f9f98e06e6d5..31dd52c513df 100644
--- a/drivers/staging/xgifb/vb_table.h
+++ b/drivers/staging/xgifb/vb_table.h
@@ -2448,15 +2448,15 @@ static const struct XGI301C_Tap4TimingStruct PALTap4Timing[] = {
 		}
 	},
 	{0xFFFF, {
-		 0x04, 0x1A, 0x04, 0x7E, 0x02, 0x1B, 0x05, 0x7E, /* ; C0-C7 */
-		 0x01, 0x1A, 0x07, 0x7E, 0x00, 0x1A, 0x09, 0x7D, /* ; C8-CF */
-		 0x7F, 0x19, 0x0B, 0x7D, 0x7E, 0x18, 0x0D, 0x7D, /* ; D0-D7 */
-		 0x7D, 0x17, 0x10, 0x7C, 0x7D, 0x15, 0x12, 0x7C, /* ; D8-DF */
-		 0x7C, 0x14, 0x14, 0x7C, 0x7C, 0x12, 0x15, 0x7D, /* ; E0-E7 */
-		 0x7C, 0x10, 0x17, 0x7D, 0x7C, 0x0D, 0x18, 0x7F, /* ; EA-EF */
-		 0x7D, 0x0B, 0x19, 0x7F, 0x7D, 0x09, 0x1A, 0x00, /* ; F0-F7 */
-		 0x7D, 0x07, 0x1A, 0x02, 0x7E, 0x05, 0x1B, 0x02  /* ; F8-FF */
-		 }
+		0x04, 0x1A, 0x04, 0x7E, 0x02, 0x1B, 0x05, 0x7E, /* ; C0-C7 */
+		0x01, 0x1A, 0x07, 0x7E, 0x00, 0x1A, 0x09, 0x7D, /* ; C8-CF */
+		0x7F, 0x19, 0x0B, 0x7D, 0x7E, 0x18, 0x0D, 0x7D, /* ; D0-D7 */
+		0x7D, 0x17, 0x10, 0x7C, 0x7D, 0x15, 0x12, 0x7C, /* ; D8-DF */
+		0x7C, 0x14, 0x14, 0x7C, 0x7C, 0x12, 0x15, 0x7D, /* ; E0-E7 */
+		0x7C, 0x10, 0x17, 0x7D, 0x7C, 0x0D, 0x18, 0x7F, /* ; EA-EF */
+		0x7D, 0x0B, 0x19, 0x7F, 0x7D, 0x09, 0x1A, 0x00, /* ; F0-F7 */
+		0x7D, 0x07, 0x1A, 0x02, 0x7E, 0x05, 0x1B, 0x02  /* ; F8-FF */
+		}
 	}
 };
 
diff --git a/drivers/target/iscsi/cxgbit/cxgbit_cm.c b/drivers/target/iscsi/cxgbit/cxgbit_cm.c
index 37a05185dcbe..e583dd8a418b 100644
--- a/drivers/target/iscsi/cxgbit/cxgbit_cm.c
+++ b/drivers/target/iscsi/cxgbit/cxgbit_cm.c
@@ -752,7 +752,8 @@ void _cxgbit_free_csk(struct kref *kref)
 				   &sin6->sin6_addr.s6_addr, 1);
 	}
 
-	cxgb4_remove_tid(csk->com.cdev->lldi.tids, 0, csk->tid);
+	cxgb4_remove_tid(csk->com.cdev->lldi.tids, 0, csk->tid,
+			 csk->com.local_addr.ss_family);
 	dst_release(csk->dst);
 	cxgb4_l2t_release(csk->l2t);
 
@@ -1084,8 +1085,7 @@ cxgbit_pass_accept_rpl(struct cxgbit_sock *csk, struct cpl_pass_accept_req *req)
 		return;
 	}
 
-	rpl5 = (struct cpl_t5_pass_accept_rpl *)__skb_put(skb, len);
-	memset(rpl5, 0, len);
+	rpl5 = __skb_put_zero(skb, len);
 
 	INIT_TP_WR(rpl5, csk->tid);
 	OPCODE_TID(rpl5) = cpu_to_be32(MK_OPCODE_TID(CPL_PASS_ACCEPT_RPL,
@@ -1313,8 +1313,7 @@ cxgbit_pass_accept_req(struct cxgbit_device *cdev, struct sk_buff *skb)
 	spin_lock(&cdev->cskq.lock);
 	list_add_tail(&csk->list, &cdev->cskq.list);
 	spin_unlock(&cdev->cskq.lock);
-
-	cxgb4_insert_tid(t, csk, tid);
+	cxgb4_insert_tid(t, csk, tid, csk->com.local_addr.ss_family);
 	cxgbit_pass_accept_rpl(csk, req);
 	goto rel_skb;
 
@@ -1367,8 +1366,7 @@ u32 cxgbit_send_tx_flowc_wr(struct cxgbit_sock *csk)
 	flowclen16 = cxgbit_tx_flowc_wr_credits(csk, &nparams, &flowclen);
 
 	skb = __skb_dequeue(&csk->skbq);
-	flowc = (struct fw_flowc_wr *)__skb_put(skb, flowclen);
-	memset(flowc, 0, flowclen);
+	flowc = __skb_put_zero(skb, flowclen);
 
 	flowc->op_to_nparams = cpu_to_be32(FW_WR_OP_V(FW_FLOWC_WR) |
 					   FW_FLOWC_WR_NPARAMS_V(nparams));
@@ -1439,8 +1437,7 @@ int cxgbit_setup_conn_digest(struct cxgbit_sock *csk)
 		return -ENOMEM;
 
 	/*  set up ulp submode */
-	req = (struct cpl_set_tcb_field *)__skb_put(skb, len);
-	memset(req, 0, len);
+	req = __skb_put_zero(skb, len);
 
 	INIT_TP_WR(req, csk->tid);
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_SET_TCB_FIELD, csk->tid));
@@ -1476,8 +1473,7 @@ int cxgbit_setup_conn_pgidx(struct cxgbit_sock *csk, u32 pg_idx)
 	if (!skb)
 		return -ENOMEM;
 
-	req = (struct cpl_set_tcb_field *)__skb_put(skb, len);
-	memset(req, 0, len);
+	req = __skb_put_zero(skb, len);
 
 	INIT_TP_WR(req, csk->tid);
 	OPCODE_TID(req) = htonl(MK_OPCODE_TID(CPL_SET_TCB_FIELD, csk->tid));
diff --git a/drivers/target/iscsi/cxgbit/cxgbit_ddp.c b/drivers/target/iscsi/cxgbit/cxgbit_ddp.c
index 5d78bdb7fc64..5fdb57cac968 100644
--- a/drivers/target/iscsi/cxgbit/cxgbit_ddp.c
+++ b/drivers/target/iscsi/cxgbit/cxgbit_ddp.c
@@ -79,7 +79,7 @@ cxgbit_ppod_init_idata(struct cxgbit_device *cdev, struct cxgbi_ppm *ppm,
 	if (!skb)
 		return NULL;
 
-	req = (struct ulp_mem_io *)__skb_put(skb, wr_len);
+	req = __skb_put(skb, wr_len);
 	INIT_ULPTX_WR(req, wr_len, 0, tid);
 	req->wr.wr_hi = htonl(FW_WR_OP_V(FW_ULPTX_WR) |
 		FW_WR_ATOMIC_V(0));
diff --git a/drivers/target/iscsi/cxgbit/cxgbit_target.c b/drivers/target/iscsi/cxgbit/cxgbit_target.c
index bdcc8b4c522a..dda13f1af38e 100644
--- a/drivers/target/iscsi/cxgbit/cxgbit_target.c
+++ b/drivers/target/iscsi/cxgbit/cxgbit_target.c
@@ -136,7 +136,7 @@ cxgbit_cpl_tx_data_iso(struct sk_buff *skb, struct cxgbit_iso_info *iso_info)
 	unsigned int fslice = !!(iso_info->flags & CXGBIT_ISO_FSLICE);
 	unsigned int lslice = !!(iso_info->flags & CXGBIT_ISO_LSLICE);
 
-	cpl = (struct cpl_tx_data_iso *)__skb_push(skb, sizeof(*cpl));
+	cpl = __skb_push(skb, sizeof(*cpl));
 
 	cpl->op_to_scsi = htonl(CPL_TX_DATA_ISO_OP_V(CPL_TX_DATA_ISO) |
 			CPL_TX_DATA_ISO_FIRST_V(fslice) |
@@ -183,8 +183,7 @@ cxgbit_tx_data_wr(struct cxgbit_sock *csk, struct sk_buff *skb, u32 dlen,
 	if (cxgbit_is_ofld_imm(skb))
 		immlen += dlen;
 
-	req = (struct fw_ofld_tx_data_wr *)__skb_push(skb,
-							hdr_size);
+	req = __skb_push(skb, hdr_size);
 	req->op_to_immdlen = cpu_to_be32(FW_WR_OP_V(opcode) |
 					FW_WR_COMPL_V(compl) |
 					FW_WR_IMMDLEN_V(immlen));
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 0d8f81591bed..3fdca2cdd8da 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -1279,6 +1279,18 @@ iscsit_get_immediate_data(struct iscsi_cmd *cmd, struct iscsi_scsi_req *hdr,
 	 */
 	if (dump_payload)
 		goto after_immediate_data;
+	/*
+	 * Check for underflow case where both EDTL and immediate data payload
+	 * exceeds what is presented by CDB's TRANSFER LENGTH, and what has
+	 * already been set in target_cmd_size_check() as se_cmd->data_length.
+	 *
+	 * For this special case, fail the command and dump the immediate data
+	 * payload.
+	 */
+	if (cmd->first_burst_len > cmd->se_cmd.data_length) {
+		cmd->sense_reason = TCM_INVALID_CDB_FIELD;
+		goto after_immediate_data;
+	}
 
 	immed_ret = iscsit_handle_immediate_data(cmd, hdr,
 					cmd->first_burst_len);
@@ -4423,8 +4435,11 @@ static void iscsit_logout_post_handler_closesession(
 	 * always sleep waiting for RX/TX thread shutdown to complete
 	 * within iscsit_close_connection().
 	 */
-	if (!conn->conn_transport->rdma_shutdown)
+	if (!conn->conn_transport->rdma_shutdown) {
 		sleep = cmpxchg(&conn->tx_thread_active, true, false);
+		if (!sleep)
+			return;
+	}
 
 	atomic_set(&conn->conn_logout_remove, 0);
 	complete(&conn->conn_logout_comp);
@@ -4440,8 +4455,11 @@ static void iscsit_logout_post_handler_samecid(
 {
 	int sleep = 1;
 
-	if (!conn->conn_transport->rdma_shutdown)
+	if (!conn->conn_transport->rdma_shutdown) {
 		sleep = cmpxchg(&conn->tx_thread_active, true, false);
+		if (!sleep)
+			return;
+	}
 
 	atomic_set(&conn->conn_logout_remove, 0);
 	complete(&conn->conn_logout_comp);
diff --git a/drivers/target/target_core_file.c b/drivers/target/target_core_file.c
index 73b8f93a5fef..e921948415c7 100644
--- a/drivers/target/target_core_file.c
+++ b/drivers/target/target_core_file.c
@@ -273,9 +273,9 @@ static int fd_do_rw(struct se_cmd *cmd, struct file *fd,
 
 	iov_iter_bvec(&iter, ITER_BVEC, bvec, sgl_nents, len);
 	if (is_write)
-		ret = vfs_iter_write(fd, &iter, &pos);
+		ret = vfs_iter_write(fd, &iter, &pos, 0);
 	else
-		ret = vfs_iter_read(fd, &iter, &pos);
+		ret = vfs_iter_read(fd, &iter, &pos, 0);
 
 	if (is_write) {
 		if (ret < 0 || ret != data_length) {
@@ -409,7 +409,7 @@ fd_execute_write_same(struct se_cmd *cmd)
 	}
 
 	iov_iter_bvec(&iter, ITER_BVEC, bvec, nolb, len);
-	ret = vfs_iter_write(fd_dev->fd_file, &iter, &pos);
+	ret = vfs_iter_write(fd_dev->fd_file, &iter, &pos, 0);
 
 	kfree(bvec);
 	if (ret < 0 || ret != len) {
diff --git a/drivers/target/target_core_iblock.c b/drivers/target/target_core_iblock.c
index bb069ebe4aa6..c05d38016556 100644
--- a/drivers/target/target_core_iblock.c
+++ b/drivers/target/target_core_iblock.c
@@ -93,7 +93,7 @@ static int iblock_configure_device(struct se_device *dev)
 		return -EINVAL;
 	}
 
-	ib_dev->ibd_bio_set = bioset_create(IBLOCK_BIO_POOL_SIZE, 0);
+	ib_dev->ibd_bio_set = bioset_create(IBLOCK_BIO_POOL_SIZE, 0, BIOSET_NEED_BVECS);
 	if (!ib_dev->ibd_bio_set) {
 		pr_err("IBLOCK: Unable to create bioset\n");
 		goto out;
@@ -296,8 +296,8 @@ static void iblock_bio_done(struct bio *bio)
 	struct se_cmd *cmd = bio->bi_private;
 	struct iblock_req *ibr = cmd->priv;
 
-	if (bio->bi_error) {
-		pr_err("bio error: %p,  err: %d\n", bio, bio->bi_error);
+	if (bio->bi_status) {
+		pr_err("bio error: %p,  err: %d\n", bio, bio->bi_status);
 		/*
 		 * Bump the ib_bio_err_cnt and release bio.
 		 */
@@ -354,11 +354,11 @@ static void iblock_end_io_flush(struct bio *bio)
 {
 	struct se_cmd *cmd = bio->bi_private;
 
-	if (bio->bi_error)
-		pr_err("IBLOCK: cache flush failed: %d\n", bio->bi_error);
+	if (bio->bi_status)
+		pr_err("IBLOCK: cache flush failed: %d\n", bio->bi_status);
 
 	if (cmd) {
-		if (bio->bi_error)
+		if (bio->bi_status)
 			target_complete_cmd(cmd, SAM_STAT_CHECK_CONDITION);
 		else
 			target_complete_cmd(cmd, SAM_STAT_GOOD);
diff --git a/drivers/target/target_core_internal.h b/drivers/target/target_core_internal.h
index 9ab7090f7c83..0912de7c0cf8 100644
--- a/drivers/target/target_core_internal.h
+++ b/drivers/target/target_core_internal.h
@@ -136,7 +136,7 @@ int	init_se_kmem_caches(void);
 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);
+int	transport_cmd_finish_abort(struct se_cmd *, int);
 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 *,
diff --git a/drivers/target/target_core_pscsi.c b/drivers/target/target_core_pscsi.c
index 3e4abb13f8ea..ceec0211e84e 100644
--- a/drivers/target/target_core_pscsi.c
+++ b/drivers/target/target_core_pscsi.c
@@ -55,7 +55,7 @@ static inline struct pscsi_dev_virt *PSCSI_DEV(struct se_device *dev)
 }
 
 static sense_reason_t pscsi_execute_cmd(struct se_cmd *cmd);
-static void pscsi_req_done(struct request *, int);
+static void pscsi_req_done(struct request *, blk_status_t);
 
 /*	pscsi_attach_hba():
  *
@@ -992,8 +992,6 @@ pscsi_execute_cmd(struct se_cmd *cmd)
 		goto fail;
 	}
 
-	scsi_req_init(req);
-
 	if (sgl) {
 		ret = pscsi_map_sg(cmd, sgl, sgl_nents, req);
 		if (ret)
@@ -1045,7 +1043,7 @@ static sector_t pscsi_get_blocks(struct se_device *dev)
 	return 0;
 }
 
-static void pscsi_req_done(struct request *req, int uptodate)
+static void pscsi_req_done(struct request *req, blk_status_t status)
 {
 	struct se_cmd *cmd = req->end_io_data;
 	struct pscsi_plugin_task *pt = cmd->priv;
diff --git a/drivers/target/target_core_tmr.c b/drivers/target/target_core_tmr.c
index dce1e1b47316..13f47bf4d16b 100644
--- a/drivers/target/target_core_tmr.c
+++ b/drivers/target/target_core_tmr.c
@@ -75,7 +75,7 @@ void core_tmr_release_req(struct se_tmr_req *tmr)
 	kfree(tmr);
 }
 
-static void core_tmr_handle_tas_abort(struct se_cmd *cmd, int tas)
+static int core_tmr_handle_tas_abort(struct se_cmd *cmd, int tas)
 {
 	unsigned long flags;
 	bool remove = true, send_tas;
@@ -91,7 +91,7 @@ static void core_tmr_handle_tas_abort(struct se_cmd *cmd, int tas)
 		transport_send_task_abort(cmd);
 	}
 
-	transport_cmd_finish_abort(cmd, remove);
+	return transport_cmd_finish_abort(cmd, remove);
 }
 
 static int target_check_cdb_and_preempt(struct list_head *list,
@@ -184,8 +184,8 @@ void core_tmr_abort_task(
 		cancel_work_sync(&se_cmd->work);
 		transport_wait_for_tasks(se_cmd);
 
-		transport_cmd_finish_abort(se_cmd, true);
-		target_put_sess_cmd(se_cmd);
+		if (!transport_cmd_finish_abort(se_cmd, true))
+			target_put_sess_cmd(se_cmd);
 
 		printk("ABORT_TASK: Sending TMR_FUNCTION_COMPLETE for"
 				" ref_tag: %llu\n", ref_tag);
@@ -281,8 +281,8 @@ static void core_tmr_drain_tmr_list(
 		cancel_work_sync(&cmd->work);
 		transport_wait_for_tasks(cmd);
 
-		transport_cmd_finish_abort(cmd, 1);
-		target_put_sess_cmd(cmd);
+		if (!transport_cmd_finish_abort(cmd, 1))
+			target_put_sess_cmd(cmd);
 	}
 }
 
@@ -380,8 +380,8 @@ static void core_tmr_drain_state_list(
 		cancel_work_sync(&cmd->work);
 		transport_wait_for_tasks(cmd);
 
-		core_tmr_handle_tas_abort(cmd, tas);
-		target_put_sess_cmd(cmd);
+		if (!core_tmr_handle_tas_abort(cmd, tas))
+			target_put_sess_cmd(cmd);
 	}
 }
 
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 6025935036c9..f1b3a46bdcaf 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -651,9 +651,10 @@ static void transport_lun_remove_cmd(struct se_cmd *cmd)
 		percpu_ref_put(&lun->lun_ref);
 }
 
-void transport_cmd_finish_abort(struct se_cmd *cmd, int remove)
+int transport_cmd_finish_abort(struct se_cmd *cmd, int remove)
 {
 	bool ack_kref = (cmd->se_cmd_flags & SCF_ACK_KREF);
+	int ret = 0;
 
 	if (cmd->se_cmd_flags & SCF_SE_LUN_CMD)
 		transport_lun_remove_cmd(cmd);
@@ -665,9 +666,11 @@ void transport_cmd_finish_abort(struct se_cmd *cmd, int remove)
 		cmd->se_tfo->aborted_task(cmd);
 
 	if (transport_cmd_check_stop_to_fabric(cmd))
-		return;
+		return 1;
 	if (remove && ack_kref)
-		transport_put_cmd(cmd);
+		ret = transport_put_cmd(cmd);
+
+	return ret;
 }
 
 static void target_complete_failure_work(struct work_struct *work)
diff --git a/drivers/thermal/int340x_thermal/int3400_thermal.c b/drivers/thermal/int340x_thermal/int3400_thermal.c
index 9413c4abf0b9..a9ec94ed7a42 100644
--- a/drivers/thermal/int340x_thermal/int3400_thermal.c
+++ b/drivers/thermal/int340x_thermal/int3400_thermal.c
@@ -23,7 +23,7 @@ enum int3400_thermal_uuid {
 	INT3400_THERMAL_MAXIMUM_UUID,
 };
 
-static u8 *int3400_thermal_uuids[INT3400_THERMAL_MAXIMUM_UUID] = {
+static char *int3400_thermal_uuids[INT3400_THERMAL_MAXIMUM_UUID] = {
 	"42A441D6-AE6A-462b-A84B-4A8CE79027D3",
 	"3A95C389-E4B8-4629-A526-C52C88626BAE",
 	"97C68AE7-15FA-499c-B8C9-5DA81D606E0A",
@@ -141,10 +141,10 @@ static int int3400_thermal_get_uuids(struct int3400_thermal_priv *priv)
 		}
 
 		for (j = 0; j < INT3400_THERMAL_MAXIMUM_UUID; j++) {
-			u8 uuid[16];
+			guid_t guid;
 
-			acpi_str_to_uuid(int3400_thermal_uuids[j], uuid);
-			if (!strncmp(uuid, objb->buffer.pointer, 16)) {
+			guid_parse(int3400_thermal_uuids[j], &guid);
+			if (guid_equal((guid_t *)objb->buffer.pointer, &guid)) {
 				priv->uuid_bitmap |= (1 << j);
 				break;
 			}
diff --git a/drivers/thermal/max77620_thermal.c b/drivers/thermal/max77620_thermal.c
index e9a1fe342760..159bbcee8821 100644
--- a/drivers/thermal/max77620_thermal.c
+++ b/drivers/thermal/max77620_thermal.c
@@ -104,8 +104,6 @@ static int max77620_thermal_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	pdev->dev.of_node = pdev->dev.parent->of_node;
-
 	mtherm->dev = &pdev->dev;
 	mtherm->rmap = dev_get_regmap(pdev->dev.parent, NULL);
 	if (!mtherm->rmap) {
@@ -113,6 +111,12 @@ static int max77620_thermal_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
+	/*
+	 * The reference taken to the parent's node which will be balanced on
+	 * reprobe or on platform-device release.
+	 */
+	device_set_of_node_from_dev(&pdev->dev, pdev->dev.parent);
+
 	mtherm->tz_device = devm_thermal_zone_of_sensor_register(&pdev->dev, 0,
 				mtherm, &max77620_thermal_ops);
 	if (IS_ERR(mtherm->tz_device)) {
diff --git a/drivers/thunderbolt/Kconfig b/drivers/thunderbolt/Kconfig
index d35db16aa43f..f4869c38c7e4 100644
--- a/drivers/thunderbolt/Kconfig
+++ b/drivers/thunderbolt/Kconfig
@@ -1,15 +1,16 @@
 menuconfig THUNDERBOLT
-	tristate "Thunderbolt support for Apple devices"
+	tristate "Thunderbolt support"
 	depends on PCI
 	depends on X86 || COMPILE_TEST
 	select APPLE_PROPERTIES if EFI_STUB && X86
 	select CRC32
+	select CRYPTO
+	select CRYPTO_HASH
+	select NVMEM
 	help
-	  Cactus Ridge Thunderbolt Controller driver
-	  This driver is required if you want to hotplug Thunderbolt devices on
-	  Apple hardware.
-
-	  Device chaining is currently not supported.
+	  Thunderbolt Controller driver. This driver is required if you
+	  want to hotplug Thunderbolt devices on Apple hardware or on PCs
+	  with Intel Falcon Ridge or newer.
 
 	  To compile this driver a module, choose M here. The module will be
 	  called thunderbolt.
diff --git a/drivers/thunderbolt/Makefile b/drivers/thunderbolt/Makefile
index 5d1053cdfa54..4900febc6c8a 100644
--- a/drivers/thunderbolt/Makefile
+++ b/drivers/thunderbolt/Makefile
@@ -1,3 +1,3 @@
 obj-${CONFIG_THUNDERBOLT} := thunderbolt.o
 thunderbolt-objs := nhi.o ctl.o tb.o switch.o cap.o path.o tunnel_pci.o eeprom.o
-
+thunderbolt-objs += domain.o dma_port.o icm.o
diff --git a/drivers/thunderbolt/cap.c b/drivers/thunderbolt/cap.c
index a7b47e7cddbd..38bc27a5ce4f 100644
--- a/drivers/thunderbolt/cap.c
+++ b/drivers/thunderbolt/cap.c
@@ -9,6 +9,8 @@
 
 #include "tb.h"
 
+#define CAP_OFFSET_MAX		0xff
+#define VSE_CAP_OFFSET_MAX	0xffff
 
 struct tb_cap_any {
 	union {
@@ -18,99 +20,110 @@ struct tb_cap_any {
 	};
 } __packed;
 
-static bool tb_cap_is_basic(struct tb_cap_any *cap)
-{
-	/* basic.cap is u8. This checks only the lower 8 bit of cap. */
-	return cap->basic.cap != 5;
-}
-
-static bool tb_cap_is_long(struct tb_cap_any *cap)
+/**
+ * tb_port_find_cap() - Find port capability
+ * @port: Port to find the capability for
+ * @cap: Capability to look
+ *
+ * Returns offset to start of capability or %-ENOENT if no such
+ * capability was found. Negative errno is returned if there was an
+ * error.
+ */
+int tb_port_find_cap(struct tb_port *port, enum tb_port_cap cap)
 {
-	return !tb_cap_is_basic(cap)
-	       && cap->extended_short.next == 0
-	       && cap->extended_short.length == 0;
-}
+	u32 offset;
 
-static enum tb_cap tb_cap(struct tb_cap_any *cap)
-{
-	if (tb_cap_is_basic(cap))
-		return cap->basic.cap;
+	/*
+	 * DP out adapters claim to implement TMU capability but in
+	 * reality they do not so we hard code the adapter specific
+	 * capability offset here.
+	 */
+	if (port->config.type == TB_TYPE_DP_HDMI_OUT)
+		offset = 0x39;
 	else
-		/* extended_short/long have cap at the same offset. */
-		return cap->extended_short.cap;
+		offset = 0x1;
+
+	do {
+		struct tb_cap_any header;
+		int ret;
+
+		ret = tb_port_read(port, &header, TB_CFG_PORT, offset, 1);
+		if (ret)
+			return ret;
+
+		if (header.basic.cap == cap)
+			return offset;
+
+		offset = header.basic.next;
+	} while (offset);
+
+	return -ENOENT;
 }
 
-static u32 tb_cap_next(struct tb_cap_any *cap, u32 offset)
+static int tb_switch_find_cap(struct tb_switch *sw, enum tb_switch_cap cap)
 {
-	int next;
-	if (offset == 1) {
-		/*
-		 * The first pointer is part of the switch header and always
-		 * a simple pointer.
-		 */
-		next = cap->basic.next;
-	} else {
-		/*
-		 * Somehow Intel decided to use 3 different types of capability
-		 * headers. It is not like anyone could have predicted that
-		 * single byte offsets are not enough...
-		 */
-		if (tb_cap_is_basic(cap))
-			next = cap->basic.next;
-		else if (!tb_cap_is_long(cap))
-			next = cap->extended_short.next;
-		else
-			next = cap->extended_long.next;
+	int offset = sw->config.first_cap_offset;
+
+	while (offset > 0 && offset < CAP_OFFSET_MAX) {
+		struct tb_cap_any header;
+		int ret;
+
+		ret = tb_sw_read(sw, &header, TB_CFG_SWITCH, offset, 1);
+		if (ret)
+			return ret;
+
+		if (header.basic.cap == cap)
+			return offset;
+
+		offset = header.basic.next;
 	}
-	/*
-	 * "Hey, we could terminate some capability lists with a null offset
-	 *  and others with a pointer to the last element." - "Great idea!"
-	 */
-	if (next == offset)
-		return 0;
-	return next;
+
+	return -ENOENT;
 }
 
 /**
- * tb_find_cap() - find a capability
+ * tb_switch_find_vse_cap() - Find switch vendor specific capability
+ * @sw: Switch to find the capability for
+ * @vsec: Vendor specific capability to look
  *
- * Return: Returns a positive offset if the capability was found and 0 if not.
- * Returns an error code on failure.
+ * Functions enumerates vendor specific capabilities (VSEC) of a switch
+ * and returns offset when capability matching @vsec is found. If no
+ * such capability is found returns %-ENOENT. In case of error returns
+ * negative errno.
  */
-int tb_find_cap(struct tb_port *port, enum tb_cfg_space space, enum tb_cap cap)
+int tb_switch_find_vse_cap(struct tb_switch *sw, enum tb_switch_vse_cap vsec)
 {
-	u32 offset = 1;
 	struct tb_cap_any header;
-	int res;
-	int retries = 10;
-	while (retries--) {
-		res = tb_port_read(port, &header, space, offset, 1);
-		if (res) {
-			/* Intel needs some help with linked lists. */
-			if (space == TB_CFG_PORT && offset == 0xa
-			    && port->config.type == TB_TYPE_DP_HDMI_OUT) {
-				offset = 0x39;
-				continue;
-			}
-			return res;
-		}
-		if (offset != 1) {
-			if (tb_cap(&header) == cap)
+	int offset;
+
+	offset = tb_switch_find_cap(sw, TB_SWITCH_CAP_VSE);
+	if (offset < 0)
+		return offset;
+
+	while (offset > 0 && offset < VSE_CAP_OFFSET_MAX) {
+		int ret;
+
+		ret = tb_sw_read(sw, &header, TB_CFG_SWITCH, offset, 2);
+		if (ret)
+			return ret;
+
+		/*
+		 * Extended vendor specific capabilities come in two
+		 * flavors: short and long. The latter is used when
+		 * offset is over 0xff.
+		 */
+		if (offset >= CAP_OFFSET_MAX) {
+			if (header.extended_long.vsec_id == vsec)
 				return offset;
-			if (tb_cap_is_long(&header)) {
-				/* tb_cap_extended_long is 2 dwords */
-				res = tb_port_read(port, &header, space,
-						   offset, 2);
-				if (res)
-					return res;
-			}
+			offset = header.extended_long.next;
+		} else {
+			if (header.extended_short.vsec_id == vsec)
+				return offset;
+			if (!header.extended_short.length)
+				return -ENOENT;
+			offset = header.extended_short.next;
 		}
-		offset = tb_cap_next(&header, offset);
-		if (!offset)
-			return 0;
 	}
-	tb_port_WARN(port,
-		     "run out of retries while looking for cap %#x in config space %d, last offset: %#x\n",
-		     cap, space, offset);
-	return -EIO;
+
+	return -ENOENT;
 }
diff --git a/drivers/thunderbolt/ctl.c b/drivers/thunderbolt/ctl.c
index 1146ff4210a9..69c0232a22f8 100644
--- a/drivers/thunderbolt/ctl.c
+++ b/drivers/thunderbolt/ctl.c
@@ -5,22 +5,17 @@
  */
 
 #include <linux/crc32.h>
+#include <linux/delay.h>
 #include <linux/slab.h>
 #include <linux/pci.h>
 #include <linux/dmapool.h>
 #include <linux/workqueue.h>
-#include <linux/kfifo.h>
 
 #include "ctl.h"
 
 
-struct ctl_pkg {
-	struct tb_ctl *ctl;
-	void *buffer;
-	struct ring_frame frame;
-};
-
-#define TB_CTL_RX_PKG_COUNT 10
+#define TB_CTL_RX_PKG_COUNT	10
+#define TB_CTL_RETRIES		4
 
 /**
  * struct tb_cfg - thunderbolt control channel
@@ -32,10 +27,11 @@ struct tb_ctl {
 
 	struct dma_pool *frame_pool;
 	struct ctl_pkg *rx_packets[TB_CTL_RX_PKG_COUNT];
-	DECLARE_KFIFO(response_fifo, struct ctl_pkg*, 16);
-	struct completion response_ready;
+	struct mutex request_queue_lock;
+	struct list_head request_queue;
+	bool running;
 
-	hotplug_cb callback;
+	event_cb callback;
 	void *callback_data;
 };
 
@@ -52,102 +48,124 @@ struct tb_ctl {
 #define tb_ctl_info(ctl, format, arg...) \
 	dev_info(&(ctl)->nhi->pdev->dev, format, ## arg)
 
+#define tb_ctl_dbg(ctl, format, arg...) \
+	dev_dbg(&(ctl)->nhi->pdev->dev, format, ## arg)
 
-/* configuration packets definitions */
+static DECLARE_WAIT_QUEUE_HEAD(tb_cfg_request_cancel_queue);
+/* Serializes access to request kref_get/put */
+static DEFINE_MUTEX(tb_cfg_request_lock);
 
-enum tb_cfg_pkg_type {
-	TB_CFG_PKG_READ = 1,
-	TB_CFG_PKG_WRITE = 2,
-	TB_CFG_PKG_ERROR = 3,
-	TB_CFG_PKG_NOTIFY_ACK = 4,
-	TB_CFG_PKG_EVENT = 5,
-	TB_CFG_PKG_XDOMAIN_REQ = 6,
-	TB_CFG_PKG_XDOMAIN_RESP = 7,
-	TB_CFG_PKG_OVERRIDE = 8,
-	TB_CFG_PKG_RESET = 9,
-	TB_CFG_PKG_PREPARE_TO_SLEEP = 0xd,
-};
+/**
+ * tb_cfg_request_alloc() - Allocates a new config request
+ *
+ * This is refcounted object so when you are done with this, call
+ * tb_cfg_request_put() to it.
+ */
+struct tb_cfg_request *tb_cfg_request_alloc(void)
+{
+	struct tb_cfg_request *req;
 
-/* common header */
-struct tb_cfg_header {
-	u32 route_hi:22;
-	u32 unknown:10; /* highest order bit is set on replies */
-	u32 route_lo;
-} __packed;
-
-/* additional header for read/write packets */
-struct tb_cfg_address {
-	u32 offset:13; /* in dwords */
-	u32 length:6; /* in dwords */
-	u32 port:6;
-	enum tb_cfg_space space:2;
-	u32 seq:2; /* sequence number  */
-	u32 zero:3;
-} __packed;
-
-/* TB_CFG_PKG_READ, response for TB_CFG_PKG_WRITE */
-struct cfg_read_pkg {
-	struct tb_cfg_header header;
-	struct tb_cfg_address addr;
-} __packed;
-
-/* TB_CFG_PKG_WRITE, response for TB_CFG_PKG_READ */
-struct cfg_write_pkg {
-	struct tb_cfg_header header;
-	struct tb_cfg_address addr;
-	u32 data[64]; /* maximum size, tb_cfg_address.length has 6 bits */
-} __packed;
-
-/* TB_CFG_PKG_ERROR */
-struct cfg_error_pkg {
-	struct tb_cfg_header header;
-	enum tb_cfg_error error:4;
-	u32 zero1:4;
-	u32 port:6;
-	u32 zero2:2; /* Both should be zero, still they are different fields. */
-	u32 zero3:16;
-} __packed;
-
-/* TB_CFG_PKG_EVENT */
-struct cfg_event_pkg {
-	struct tb_cfg_header header;
-	u32 port:6;
-	u32 zero:25;
-	bool unplug:1;
-} __packed;
-
-/* TB_CFG_PKG_RESET */
-struct cfg_reset_pkg {
-	struct tb_cfg_header header;
-} __packed;
-
-/* TB_CFG_PKG_PREPARE_TO_SLEEP */
-struct cfg_pts_pkg {
-	struct tb_cfg_header header;
-	u32 data;
-} __packed;
+	req = kzalloc(sizeof(*req), GFP_KERNEL);
+	if (!req)
+		return NULL;
 
+	kref_init(&req->kref);
 
-/* utility functions */
+	return req;
+}
 
-static u64 get_route(struct tb_cfg_header header)
+/**
+ * tb_cfg_request_get() - Increase refcount of a request
+ * @req: Request whose refcount is increased
+ */
+void tb_cfg_request_get(struct tb_cfg_request *req)
 {
-	return (u64) header.route_hi << 32 | header.route_lo;
+	mutex_lock(&tb_cfg_request_lock);
+	kref_get(&req->kref);
+	mutex_unlock(&tb_cfg_request_lock);
 }
 
-static struct tb_cfg_header make_header(u64 route)
+static void tb_cfg_request_destroy(struct kref *kref)
 {
-	struct tb_cfg_header header = {
-		.route_hi = route >> 32,
-		.route_lo = route,
-	};
-	/* check for overflow, route_hi is not 32 bits! */
-	WARN_ON(get_route(header) != route);
-	return header;
+	struct tb_cfg_request *req = container_of(kref, typeof(*req), kref);
+
+	kfree(req);
+}
+
+/**
+ * tb_cfg_request_put() - Decrease refcount and possibly release the request
+ * @req: Request whose refcount is decreased
+ *
+ * Call this function when you are done with the request. When refcount
+ * goes to %0 the object is released.
+ */
+void tb_cfg_request_put(struct tb_cfg_request *req)
+{
+	mutex_lock(&tb_cfg_request_lock);
+	kref_put(&req->kref, tb_cfg_request_destroy);
+	mutex_unlock(&tb_cfg_request_lock);
 }
 
-static int check_header(struct ctl_pkg *pkg, u32 len, enum tb_cfg_pkg_type type,
-			u64 route)
+static int tb_cfg_request_enqueue(struct tb_ctl *ctl,
+				  struct tb_cfg_request *req)
+{
+	WARN_ON(test_bit(TB_CFG_REQUEST_ACTIVE, &req->flags));
+	WARN_ON(req->ctl);
+
+	mutex_lock(&ctl->request_queue_lock);
+	if (!ctl->running) {
+		mutex_unlock(&ctl->request_queue_lock);
+		return -ENOTCONN;
+	}
+	req->ctl = ctl;
+	list_add_tail(&req->list, &ctl->request_queue);
+	set_bit(TB_CFG_REQUEST_ACTIVE, &req->flags);
+	mutex_unlock(&ctl->request_queue_lock);
+	return 0;
+}
+
+static void tb_cfg_request_dequeue(struct tb_cfg_request *req)
+{
+	struct tb_ctl *ctl = req->ctl;
+
+	mutex_lock(&ctl->request_queue_lock);
+	list_del(&req->list);
+	clear_bit(TB_CFG_REQUEST_ACTIVE, &req->flags);
+	if (test_bit(TB_CFG_REQUEST_CANCELED, &req->flags))
+		wake_up(&tb_cfg_request_cancel_queue);
+	mutex_unlock(&ctl->request_queue_lock);
+}
+
+static bool tb_cfg_request_is_active(struct tb_cfg_request *req)
+{
+	return test_bit(TB_CFG_REQUEST_ACTIVE, &req->flags);
+}
+
+static struct tb_cfg_request *
+tb_cfg_request_find(struct tb_ctl *ctl, struct ctl_pkg *pkg)
+{
+	struct tb_cfg_request *req;
+	bool found = false;
+
+	mutex_lock(&pkg->ctl->request_queue_lock);
+	list_for_each_entry(req, &pkg->ctl->request_queue, list) {
+		tb_cfg_request_get(req);
+		if (req->match(req, pkg)) {
+			found = true;
+			break;
+		}
+		tb_cfg_request_put(req);
+	}
+	mutex_unlock(&pkg->ctl->request_queue_lock);
+
+	return found ? req : NULL;
+}
+
+/* utility functions */
+
+
+static int check_header(const struct ctl_pkg *pkg, u32 len,
+			enum tb_cfg_pkg_type type, u64 route)
 {
 	struct tb_cfg_header *header = pkg->buffer;
 
@@ -167,9 +185,9 @@ static int check_header(struct ctl_pkg *pkg, u32 len, enum tb_cfg_pkg_type type,
 	if (WARN(header->unknown != 1 << 9,
 			"header->unknown is %#x\n", header->unknown))
 		return -EIO;
-	if (WARN(route != get_route(*header),
+	if (WARN(route != tb_cfg_get_route(header),
 			"wrong route (expected %llx, got %llx)",
-			route, get_route(*header)))
+			route, tb_cfg_get_route(header)))
 		return -EIO;
 	return 0;
 }
@@ -189,8 +207,6 @@ static int check_config_address(struct tb_cfg_address addr,
 	if (WARN(length != addr.length, "wrong space (expected %x, got %x\n)",
 			length, addr.length))
 		return -EIO;
-	if (WARN(addr.seq, "addr.seq is %#x\n", addr.seq))
-		return -EIO;
 	/*
 	 * We cannot check addr->port as it is set to the upstream port of the
 	 * sender.
@@ -198,14 +214,14 @@ static int check_config_address(struct tb_cfg_address addr,
 	return 0;
 }
 
-static struct tb_cfg_result decode_error(struct ctl_pkg *response)
+static struct tb_cfg_result decode_error(const struct ctl_pkg *response)
 {
 	struct cfg_error_pkg *pkg = response->buffer;
 	struct tb_cfg_result res = { 0 };
-	res.response_route = get_route(pkg->header);
+	res.response_route = tb_cfg_get_route(&pkg->header);
 	res.response_port = 0;
 	res.err = check_header(response, sizeof(*pkg), TB_CFG_PKG_ERROR,
-			       get_route(pkg->header));
+			       tb_cfg_get_route(&pkg->header));
 	if (res.err)
 		return res;
 
@@ -219,7 +235,7 @@ static struct tb_cfg_result decode_error(struct ctl_pkg *response)
 
 }
 
-static struct tb_cfg_result parse_header(struct ctl_pkg *pkg, u32 len,
+static struct tb_cfg_result parse_header(const struct ctl_pkg *pkg, u32 len,
 					 enum tb_cfg_pkg_type type, u64 route)
 {
 	struct tb_cfg_header *header = pkg->buffer;
@@ -229,7 +245,7 @@ static struct tb_cfg_result parse_header(struct ctl_pkg *pkg, u32 len,
 		return decode_error(pkg);
 
 	res.response_port = 0; /* will be updated later for cfg_read/write */
-	res.response_route = get_route(*header);
+	res.response_route = tb_cfg_get_route(header);
 	res.err = check_header(pkg, len, type, route);
 	return res;
 }
@@ -273,7 +289,7 @@ static void tb_cfg_print_error(struct tb_ctl *ctl,
 	}
 }
 
-static void cpu_to_be32_array(__be32 *dst, u32 *src, size_t len)
+static void cpu_to_be32_array(__be32 *dst, const u32 *src, size_t len)
 {
 	int i;
 	for (i = 0; i < len; i++)
@@ -287,7 +303,7 @@ static void be32_to_cpu_array(u32 *dst, __be32 *src, size_t len)
 		dst[i] = be32_to_cpu(src[i]);
 }
 
-static __be32 tb_crc(void *data, size_t len)
+static __be32 tb_crc(const void *data, size_t len)
 {
 	return cpu_to_be32(~__crc32c_le(~0, data, len));
 }
@@ -333,7 +349,7 @@ static void tb_ctl_tx_callback(struct tb_ring *ring, struct ring_frame *frame,
  *
  * Return: Returns 0 on success or an error code on failure.
  */
-static int tb_ctl_tx(struct tb_ctl *ctl, void *data, size_t len,
+static int tb_ctl_tx(struct tb_ctl *ctl, const void *data, size_t len,
 		     enum tb_cfg_pkg_type type)
 {
 	int res;
@@ -364,24 +380,12 @@ static int tb_ctl_tx(struct tb_ctl *ctl, void *data, size_t len,
 }
 
 /**
- * tb_ctl_handle_plug_event() - acknowledge a plug event, invoke ctl->callback
+ * tb_ctl_handle_event() - acknowledge a plug event, invoke ctl->callback
  */
-static void tb_ctl_handle_plug_event(struct tb_ctl *ctl,
-				     struct ctl_pkg *response)
+static void tb_ctl_handle_event(struct tb_ctl *ctl, enum tb_cfg_pkg_type type,
+				struct ctl_pkg *pkg, size_t size)
 {
-	struct cfg_event_pkg *pkg = response->buffer;
-	u64 route = get_route(pkg->header);
-
-	if (check_header(response, sizeof(*pkg), TB_CFG_PKG_EVENT, route)) {
-		tb_ctl_warn(ctl, "malformed TB_CFG_PKG_EVENT\n");
-		return;
-	}
-
-	if (tb_cfg_error(ctl, route, pkg->port, TB_CFG_ERROR_ACK_PLUG_EVENT))
-		tb_ctl_warn(ctl, "could not ack plug event on %llx:%x\n",
-			    route, pkg->port);
-	WARN(pkg->zero, "pkg->zero is %#x\n", pkg->zero);
-	ctl->callback(ctl->callback_data, route, pkg->port, pkg->unplug);
+	ctl->callback(ctl->callback_data, type, pkg->buffer, size);
 }
 
 static void tb_ctl_rx_submit(struct ctl_pkg *pkg)
@@ -394,10 +398,30 @@ static void tb_ctl_rx_submit(struct ctl_pkg *pkg)
 					     */
 }
 
+static int tb_async_error(const struct ctl_pkg *pkg)
+{
+	const struct cfg_error_pkg *error = (const struct cfg_error_pkg *)pkg;
+
+	if (pkg->frame.eof != TB_CFG_PKG_ERROR)
+		return false;
+
+	switch (error->error) {
+	case TB_CFG_ERROR_LINK_ERROR:
+	case TB_CFG_ERROR_HEC_ERROR_DETECTED:
+	case TB_CFG_ERROR_FLOW_CONTROL_ERROR:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
 static void tb_ctl_rx_callback(struct tb_ring *ring, struct ring_frame *frame,
 			       bool canceled)
 {
 	struct ctl_pkg *pkg = container_of(frame, typeof(*pkg), frame);
+	struct tb_cfg_request *req;
+	__be32 crc32;
 
 	if (canceled)
 		return; /*
@@ -412,55 +436,168 @@ static void tb_ctl_rx_callback(struct tb_ring *ring, struct ring_frame *frame,
 	}
 
 	frame->size -= 4; /* remove checksum */
-	if (*(__be32 *) (pkg->buffer + frame->size)
-			!= tb_crc(pkg->buffer, frame->size)) {
-		tb_ctl_err(pkg->ctl,
-			   "RX: checksum mismatch, dropping packet\n");
-		goto rx;
-	}
+	crc32 = tb_crc(pkg->buffer, frame->size);
 	be32_to_cpu_array(pkg->buffer, pkg->buffer, frame->size / 4);
 
-	if (frame->eof == TB_CFG_PKG_EVENT) {
-		tb_ctl_handle_plug_event(pkg->ctl, pkg);
+	switch (frame->eof) {
+	case TB_CFG_PKG_READ:
+	case TB_CFG_PKG_WRITE:
+	case TB_CFG_PKG_ERROR:
+	case TB_CFG_PKG_OVERRIDE:
+	case TB_CFG_PKG_RESET:
+		if (*(__be32 *)(pkg->buffer + frame->size) != crc32) {
+			tb_ctl_err(pkg->ctl,
+				   "RX: checksum mismatch, dropping packet\n");
+			goto rx;
+		}
+		if (tb_async_error(pkg)) {
+			tb_ctl_handle_event(pkg->ctl, frame->eof,
+					    pkg, frame->size);
+			goto rx;
+		}
+		break;
+
+	case TB_CFG_PKG_EVENT:
+		if (*(__be32 *)(pkg->buffer + frame->size) != crc32) {
+			tb_ctl_err(pkg->ctl,
+				   "RX: checksum mismatch, dropping packet\n");
+			goto rx;
+		}
+		/* Fall through */
+	case TB_CFG_PKG_ICM_EVENT:
+		tb_ctl_handle_event(pkg->ctl, frame->eof, pkg, frame->size);
 		goto rx;
+
+	default:
+		break;
 	}
-	if (!kfifo_put(&pkg->ctl->response_fifo, pkg)) {
-		tb_ctl_err(pkg->ctl, "RX: fifo is full\n");
-		goto rx;
+
+	/*
+	 * The received packet will be processed only if there is an
+	 * active request and that the packet is what is expected. This
+	 * prevents packets such as replies coming after timeout has
+	 * triggered from messing with the active requests.
+	 */
+	req = tb_cfg_request_find(pkg->ctl, pkg);
+	if (req) {
+		if (req->copy(req, pkg))
+			schedule_work(&req->work);
+		tb_cfg_request_put(req);
 	}
-	complete(&pkg->ctl->response_ready);
-	return;
+
 rx:
 	tb_ctl_rx_submit(pkg);
 }
 
+static void tb_cfg_request_work(struct work_struct *work)
+{
+	struct tb_cfg_request *req = container_of(work, typeof(*req), work);
+
+	if (!test_bit(TB_CFG_REQUEST_CANCELED, &req->flags))
+		req->callback(req->callback_data);
+
+	tb_cfg_request_dequeue(req);
+	tb_cfg_request_put(req);
+}
+
 /**
- * tb_ctl_rx() - receive a packet from the control channel
+ * tb_cfg_request() - Start control request not waiting for it to complete
+ * @ctl: Control channel to use
+ * @req: Request to start
+ * @callback: Callback called when the request is completed
+ * @callback_data: Data to be passed to @callback
+ *
+ * This queues @req on the given control channel without waiting for it
+ * to complete. When the request completes @callback is called.
  */
-static struct tb_cfg_result tb_ctl_rx(struct tb_ctl *ctl, void *buffer,
-				      size_t length, int timeout_msec,
-				      u64 route, enum tb_cfg_pkg_type type)
+int tb_cfg_request(struct tb_ctl *ctl, struct tb_cfg_request *req,
+		   void (*callback)(void *), void *callback_data)
 {
-	struct tb_cfg_result res;
-	struct ctl_pkg *pkg;
+	int ret;
 
-	if (!wait_for_completion_timeout(&ctl->response_ready,
-					 msecs_to_jiffies(timeout_msec))) {
-		tb_ctl_WARN(ctl, "RX: timeout\n");
-		return (struct tb_cfg_result) { .err = -ETIMEDOUT };
-	}
-	if (!kfifo_get(&ctl->response_fifo, &pkg)) {
-		tb_ctl_WARN(ctl, "empty kfifo\n");
-		return (struct tb_cfg_result) { .err = -EIO };
-	}
+	req->flags = 0;
+	req->callback = callback;
+	req->callback_data = callback_data;
+	INIT_WORK(&req->work, tb_cfg_request_work);
+	INIT_LIST_HEAD(&req->list);
 
-	res = parse_header(pkg, length, type, route);
-	if (!res.err)
-		memcpy(buffer, pkg->buffer, length);
-	tb_ctl_rx_submit(pkg);
-	return res;
+	tb_cfg_request_get(req);
+	ret = tb_cfg_request_enqueue(ctl, req);
+	if (ret)
+		goto err_put;
+
+	ret = tb_ctl_tx(ctl, req->request, req->request_size,
+			req->request_type);
+	if (ret)
+		goto err_dequeue;
+
+	if (!req->response)
+		schedule_work(&req->work);
+
+	return 0;
+
+err_dequeue:
+	tb_cfg_request_dequeue(req);
+err_put:
+	tb_cfg_request_put(req);
+
+	return ret;
+}
+
+/**
+ * tb_cfg_request_cancel() - Cancel a control request
+ * @req: Request to cancel
+ * @err: Error to assign to the request
+ *
+ * This function can be used to cancel ongoing request. It will wait
+ * until the request is not active anymore.
+ */
+void tb_cfg_request_cancel(struct tb_cfg_request *req, int err)
+{
+	set_bit(TB_CFG_REQUEST_CANCELED, &req->flags);
+	schedule_work(&req->work);
+	wait_event(tb_cfg_request_cancel_queue, !tb_cfg_request_is_active(req));
+	req->result.err = err;
+}
+
+static void tb_cfg_request_complete(void *data)
+{
+	complete(data);
 }
 
+/**
+ * tb_cfg_request_sync() - Start control request and wait until it completes
+ * @ctl: Control channel to use
+ * @req: Request to start
+ * @timeout_msec: Timeout how long to wait @req to complete
+ *
+ * Starts a control request and waits until it completes. If timeout
+ * triggers the request is canceled before function returns. Note the
+ * caller needs to make sure only one message for given switch is active
+ * at a time.
+ */
+struct tb_cfg_result tb_cfg_request_sync(struct tb_ctl *ctl,
+					 struct tb_cfg_request *req,
+					 int timeout_msec)
+{
+	unsigned long timeout = msecs_to_jiffies(timeout_msec);
+	struct tb_cfg_result res = { 0 };
+	DECLARE_COMPLETION_ONSTACK(done);
+	int ret;
+
+	ret = tb_cfg_request(ctl, req, tb_cfg_request_complete, &done);
+	if (ret) {
+		res.err = ret;
+		return res;
+	}
+
+	if (!wait_for_completion_timeout(&done, timeout))
+		tb_cfg_request_cancel(req, -ETIMEDOUT);
+
+	flush_work(&req->work);
+
+	return req->result;
+}
 
 /* public interface, alloc/start/stop/free */
 
@@ -471,7 +608,7 @@ static struct tb_cfg_result tb_ctl_rx(struct tb_ctl *ctl, void *buffer,
  *
  * Return: Returns a pointer on success or NULL on failure.
  */
-struct tb_ctl *tb_ctl_alloc(struct tb_nhi *nhi, hotplug_cb cb, void *cb_data)
+struct tb_ctl *tb_ctl_alloc(struct tb_nhi *nhi, event_cb cb, void *cb_data)
 {
 	int i;
 	struct tb_ctl *ctl = kzalloc(sizeof(*ctl), GFP_KERNEL);
@@ -481,18 +618,18 @@ struct tb_ctl *tb_ctl_alloc(struct tb_nhi *nhi, hotplug_cb cb, void *cb_data)
 	ctl->callback = cb;
 	ctl->callback_data = cb_data;
 
-	init_completion(&ctl->response_ready);
-	INIT_KFIFO(ctl->response_fifo);
+	mutex_init(&ctl->request_queue_lock);
+	INIT_LIST_HEAD(&ctl->request_queue);
 	ctl->frame_pool = dma_pool_create("thunderbolt_ctl", &nhi->pdev->dev,
 					 TB_FRAME_SIZE, 4, 0);
 	if (!ctl->frame_pool)
 		goto err;
 
-	ctl->tx = ring_alloc_tx(nhi, 0, 10);
+	ctl->tx = ring_alloc_tx(nhi, 0, 10, RING_FLAG_NO_SUSPEND);
 	if (!ctl->tx)
 		goto err;
 
-	ctl->rx = ring_alloc_rx(nhi, 0, 10);
+	ctl->rx = ring_alloc_rx(nhi, 0, 10, RING_FLAG_NO_SUSPEND);
 	if (!ctl->rx)
 		goto err;
 
@@ -520,6 +657,10 @@ err:
 void tb_ctl_free(struct tb_ctl *ctl)
 {
 	int i;
+
+	if (!ctl)
+		return;
+
 	if (ctl->rx)
 		ring_free(ctl->rx);
 	if (ctl->tx)
@@ -546,6 +687,8 @@ void tb_ctl_start(struct tb_ctl *ctl)
 	ring_start(ctl->rx);
 	for (i = 0; i < TB_CTL_RX_PKG_COUNT; i++)
 		tb_ctl_rx_submit(ctl->rx_packets[i]);
+
+	ctl->running = true;
 }
 
 /**
@@ -558,12 +701,16 @@ void tb_ctl_start(struct tb_ctl *ctl)
  */
 void tb_ctl_stop(struct tb_ctl *ctl)
 {
+	mutex_lock(&ctl->request_queue_lock);
+	ctl->running = false;
+	mutex_unlock(&ctl->request_queue_lock);
+
 	ring_stop(ctl->rx);
 	ring_stop(ctl->tx);
 
-	if (!kfifo_is_empty(&ctl->response_fifo))
-		tb_ctl_WARN(ctl, "dangling response in response_fifo\n");
-	kfifo_reset(&ctl->response_fifo);
+	if (!list_empty(&ctl->request_queue))
+		tb_ctl_WARN(ctl, "dangling request in request_queue\n");
+	INIT_LIST_HEAD(&ctl->request_queue);
 	tb_ctl_info(ctl, "control channel stopped\n");
 }
 
@@ -578,7 +725,7 @@ int tb_cfg_error(struct tb_ctl *ctl, u64 route, u32 port,
 		 enum tb_cfg_error error)
 {
 	struct cfg_error_pkg pkg = {
-		.header = make_header(route),
+		.header = tb_cfg_make_header(route),
 		.port = port,
 		.error = error,
 	};
@@ -586,6 +733,49 @@ int tb_cfg_error(struct tb_ctl *ctl, u64 route, u32 port,
 	return tb_ctl_tx(ctl, &pkg, sizeof(pkg), TB_CFG_PKG_ERROR);
 }
 
+static bool tb_cfg_match(const struct tb_cfg_request *req,
+			 const struct ctl_pkg *pkg)
+{
+	u64 route = tb_cfg_get_route(pkg->buffer) & ~BIT_ULL(63);
+
+	if (pkg->frame.eof == TB_CFG_PKG_ERROR)
+		return true;
+
+	if (pkg->frame.eof != req->response_type)
+		return false;
+	if (route != tb_cfg_get_route(req->request))
+		return false;
+	if (pkg->frame.size != req->response_size)
+		return false;
+
+	if (pkg->frame.eof == TB_CFG_PKG_READ ||
+	    pkg->frame.eof == TB_CFG_PKG_WRITE) {
+		const struct cfg_read_pkg *req_hdr = req->request;
+		const struct cfg_read_pkg *res_hdr = pkg->buffer;
+
+		if (req_hdr->addr.seq != res_hdr->addr.seq)
+			return false;
+	}
+
+	return true;
+}
+
+static bool tb_cfg_copy(struct tb_cfg_request *req, const struct ctl_pkg *pkg)
+{
+	struct tb_cfg_result res;
+
+	/* Now make sure it is in expected format */
+	res = parse_header(pkg, req->response_size, req->response_type,
+			   tb_cfg_get_route(req->request));
+	if (!res.err)
+		memcpy(req->response, pkg->buffer, req->response_size);
+
+	req->result = res;
+
+	/* Always complete when first response is received */
+	return true;
+}
+
 /**
  * tb_cfg_reset() - send a reset packet and wait for a response
  *
@@ -596,16 +786,31 @@ int tb_cfg_error(struct tb_ctl *ctl, u64 route, u32 port,
 struct tb_cfg_result tb_cfg_reset(struct tb_ctl *ctl, u64 route,
 				  int timeout_msec)
 {
-	int err;
-	struct cfg_reset_pkg request = { .header = make_header(route) };
+	struct cfg_reset_pkg request = { .header = tb_cfg_make_header(route) };
+	struct tb_cfg_result res = { 0 };
 	struct tb_cfg_header reply;
+	struct tb_cfg_request *req;
+
+	req = tb_cfg_request_alloc();
+	if (!req) {
+		res.err = -ENOMEM;
+		return res;
+	}
+
+	req->match = tb_cfg_match;
+	req->copy = tb_cfg_copy;
+	req->request = &request;
+	req->request_size = sizeof(request);
+	req->request_type = TB_CFG_PKG_RESET;
+	req->response = &reply;
+	req->response_size = sizeof(reply);
+	req->response_type = sizeof(TB_CFG_PKG_RESET);
+
+	res = tb_cfg_request_sync(ctl, req, timeout_msec);
 
-	err = tb_ctl_tx(ctl, &request, sizeof(request), TB_CFG_PKG_RESET);
-	if (err)
-		return (struct tb_cfg_result) { .err = err };
+	tb_cfg_request_put(req);
 
-	return tb_ctl_rx(ctl, &reply, sizeof(reply), timeout_msec, route,
-			 TB_CFG_PKG_RESET);
+	return res;
 }
 
 /**
@@ -619,7 +824,7 @@ struct tb_cfg_result tb_cfg_read_raw(struct tb_ctl *ctl, void *buffer,
 {
 	struct tb_cfg_result res = { 0 };
 	struct cfg_read_pkg request = {
-		.header = make_header(route),
+		.header = tb_cfg_make_header(route),
 		.addr = {
 			.port = port,
 			.space = space,
@@ -628,13 +833,39 @@ struct tb_cfg_result tb_cfg_read_raw(struct tb_ctl *ctl, void *buffer,
 		},
 	};
 	struct cfg_write_pkg reply;
+	int retries = 0;
 
-	res.err = tb_ctl_tx(ctl, &request, sizeof(request), TB_CFG_PKG_READ);
-	if (res.err)
-		return res;
+	while (retries < TB_CTL_RETRIES) {
+		struct tb_cfg_request *req;
+
+		req = tb_cfg_request_alloc();
+		if (!req) {
+			res.err = -ENOMEM;
+			return res;
+		}
+
+		request.addr.seq = retries++;
+
+		req->match = tb_cfg_match;
+		req->copy = tb_cfg_copy;
+		req->request = &request;
+		req->request_size = sizeof(request);
+		req->request_type = TB_CFG_PKG_READ;
+		req->response = &reply;
+		req->response_size = 12 + 4 * length;
+		req->response_type = TB_CFG_PKG_READ;
+
+		res = tb_cfg_request_sync(ctl, req, timeout_msec);
+
+		tb_cfg_request_put(req);
+
+		if (res.err != -ETIMEDOUT)
+			break;
+
+		/* Wait a bit (arbitrary time) until we send a retry */
+		usleep_range(10, 100);
+	}
 
-	res = tb_ctl_rx(ctl, &reply, 12 + 4 * length, timeout_msec, route,
-			TB_CFG_PKG_READ);
 	if (res.err)
 		return res;
 
@@ -650,13 +881,13 @@ struct tb_cfg_result tb_cfg_read_raw(struct tb_ctl *ctl, void *buffer,
  *
  * Offset and length are in dwords.
  */
-struct tb_cfg_result tb_cfg_write_raw(struct tb_ctl *ctl, void *buffer,
+struct tb_cfg_result tb_cfg_write_raw(struct tb_ctl *ctl, const void *buffer,
 		u64 route, u32 port, enum tb_cfg_space space,
 		u32 offset, u32 length, int timeout_msec)
 {
 	struct tb_cfg_result res = { 0 };
 	struct cfg_write_pkg request = {
-		.header = make_header(route),
+		.header = tb_cfg_make_header(route),
 		.addr = {
 			.port = port,
 			.space = space,
@@ -665,15 +896,41 @@ struct tb_cfg_result tb_cfg_write_raw(struct tb_ctl *ctl, void *buffer,
 		},
 	};
 	struct cfg_read_pkg reply;
+	int retries = 0;
 
 	memcpy(&request.data, buffer, length * 4);
 
-	res.err = tb_ctl_tx(ctl, &request, 12 + 4 * length, TB_CFG_PKG_WRITE);
-	if (res.err)
-		return res;
+	while (retries < TB_CTL_RETRIES) {
+		struct tb_cfg_request *req;
+
+		req = tb_cfg_request_alloc();
+		if (!req) {
+			res.err = -ENOMEM;
+			return res;
+		}
+
+		request.addr.seq = retries++;
+
+		req->match = tb_cfg_match;
+		req->copy = tb_cfg_copy;
+		req->request = &request;
+		req->request_size = 12 + 4 * length;
+		req->request_type = TB_CFG_PKG_WRITE;
+		req->response = &reply;
+		req->response_size = sizeof(reply);
+		req->response_type = TB_CFG_PKG_WRITE;
+
+		res = tb_cfg_request_sync(ctl, req, timeout_msec);
+
+		tb_cfg_request_put(req);
+
+		if (res.err != -ETIMEDOUT)
+			break;
+
+		/* Wait a bit (arbitrary time) until we send a retry */
+		usleep_range(10, 100);
+	}
 
-	res = tb_ctl_rx(ctl, &reply, sizeof(reply), timeout_msec, route,
-			TB_CFG_PKG_WRITE);
 	if (res.err)
 		return res;
 
@@ -687,24 +944,52 @@ int tb_cfg_read(struct tb_ctl *ctl, void *buffer, u64 route, u32 port,
 {
 	struct tb_cfg_result res = tb_cfg_read_raw(ctl, buffer, route, port,
 			space, offset, length, TB_CFG_DEFAULT_TIMEOUT);
-	if (res.err == 1) {
+	switch (res.err) {
+	case 0:
+		/* Success */
+		break;
+
+	case 1:
+		/* Thunderbolt error, tb_error holds the actual number */
 		tb_cfg_print_error(ctl, &res);
 		return -EIO;
+
+	case -ETIMEDOUT:
+		tb_ctl_warn(ctl, "timeout reading config space %u from %#x\n",
+			    space, offset);
+		break;
+
+	default:
+		WARN(1, "tb_cfg_read: %d\n", res.err);
+		break;
 	}
-	WARN(res.err, "tb_cfg_read: %d\n", res.err);
 	return res.err;
 }
 
-int tb_cfg_write(struct tb_ctl *ctl, void *buffer, u64 route, u32 port,
+int tb_cfg_write(struct tb_ctl *ctl, const void *buffer, u64 route, u32 port,
 		 enum tb_cfg_space space, u32 offset, u32 length)
 {
 	struct tb_cfg_result res = tb_cfg_write_raw(ctl, buffer, route, port,
 			space, offset, length, TB_CFG_DEFAULT_TIMEOUT);
-	if (res.err == 1) {
+	switch (res.err) {
+	case 0:
+		/* Success */
+		break;
+
+	case 1:
+		/* Thunderbolt error, tb_error holds the actual number */
 		tb_cfg_print_error(ctl, &res);
 		return -EIO;
+
+	case -ETIMEDOUT:
+		tb_ctl_warn(ctl, "timeout writing config space %u to %#x\n",
+			    space, offset);
+		break;
+
+	default:
+		WARN(1, "tb_cfg_write: %d\n", res.err);
+		break;
 	}
-	WARN(res.err, "tb_cfg_write: %d\n", res.err);
 	return res.err;
 }
 
diff --git a/drivers/thunderbolt/ctl.h b/drivers/thunderbolt/ctl.h
index ba87d6e731dd..36fd28b1c1c5 100644
--- a/drivers/thunderbolt/ctl.h
+++ b/drivers/thunderbolt/ctl.h
@@ -7,14 +7,18 @@
 #ifndef _TB_CFG
 #define _TB_CFG
 
+#include <linux/kref.h>
+
 #include "nhi.h"
+#include "tb_msgs.h"
 
 /* control channel */
 struct tb_ctl;
 
-typedef void (*hotplug_cb)(void *data, u64 route, u8 port, bool unplug);
+typedef void (*event_cb)(void *data, enum tb_cfg_pkg_type type,
+			 const void *buf, size_t size);
 
-struct tb_ctl *tb_ctl_alloc(struct tb_nhi *nhi, hotplug_cb cb, void *cb_data);
+struct tb_ctl *tb_ctl_alloc(struct tb_nhi *nhi, event_cb cb, void *cb_data);
 void tb_ctl_start(struct tb_ctl *ctl);
 void tb_ctl_stop(struct tb_ctl *ctl);
 void tb_ctl_free(struct tb_ctl *ctl);
@@ -23,21 +27,6 @@ void tb_ctl_free(struct tb_ctl *ctl);
 
 #define TB_CFG_DEFAULT_TIMEOUT 5000 /* msec */
 
-enum tb_cfg_space {
-	TB_CFG_HOPS = 0,
-	TB_CFG_PORT = 1,
-	TB_CFG_SWITCH = 2,
-	TB_CFG_COUNTERS = 3,
-};
-
-enum tb_cfg_error {
-	TB_CFG_ERROR_PORT_NOT_CONNECTED = 0,
-	TB_CFG_ERROR_INVALID_CONFIG_SPACE = 2,
-	TB_CFG_ERROR_NO_SUCH_PORT = 4,
-	TB_CFG_ERROR_ACK_PLUG_EVENT = 7, /* send as reply to TB_CFG_PKG_EVENT */
-	TB_CFG_ERROR_LOOP = 8,
-};
-
 struct tb_cfg_result {
 	u64 response_route;
 	u32 response_port; /*
@@ -52,6 +41,84 @@ struct tb_cfg_result {
 	enum tb_cfg_error tb_error; /* valid if err == 1 */
 };
 
+struct ctl_pkg {
+	struct tb_ctl *ctl;
+	void *buffer;
+	struct ring_frame frame;
+};
+
+/**
+ * struct tb_cfg_request - Control channel request
+ * @kref: Reference count
+ * @ctl: Pointer to the control channel structure. Only set when the
+ *	 request is queued.
+ * @request_size: Size of the request packet (in bytes)
+ * @request_type: Type of the request packet
+ * @response: Response is stored here
+ * @response_size: Maximum size of one response packet
+ * @response_type: Expected type of the response packet
+ * @npackets: Number of packets expected to be returned with this request
+ * @match: Function used to match the incoming packet
+ * @copy: Function used to copy the incoming packet to @response
+ * @callback: Callback called when the request is finished successfully
+ * @callback_data: Data to be passed to @callback
+ * @flags: Flags for the request
+ * @work: Work item used to complete the request
+ * @result: Result after the request has been completed
+ * @list: Requests are queued using this field
+ *
+ * An arbitrary request over Thunderbolt control channel. For standard
+ * control channel message, one should use tb_cfg_read/write() and
+ * friends if possible.
+ */
+struct tb_cfg_request {
+	struct kref kref;
+	struct tb_ctl *ctl;
+	const void *request;
+	size_t request_size;
+	enum tb_cfg_pkg_type request_type;
+	void *response;
+	size_t response_size;
+	enum tb_cfg_pkg_type response_type;
+	size_t npackets;
+	bool (*match)(const struct tb_cfg_request *req,
+		      const struct ctl_pkg *pkg);
+	bool (*copy)(struct tb_cfg_request *req, const struct ctl_pkg *pkg);
+	void (*callback)(void *callback_data);
+	void *callback_data;
+	unsigned long flags;
+	struct work_struct work;
+	struct tb_cfg_result result;
+	struct list_head list;
+};
+
+#define TB_CFG_REQUEST_ACTIVE		0
+#define TB_CFG_REQUEST_CANCELED		1
+
+struct tb_cfg_request *tb_cfg_request_alloc(void);
+void tb_cfg_request_get(struct tb_cfg_request *req);
+void tb_cfg_request_put(struct tb_cfg_request *req);
+int tb_cfg_request(struct tb_ctl *ctl, struct tb_cfg_request *req,
+		   void (*callback)(void *), void *callback_data);
+void tb_cfg_request_cancel(struct tb_cfg_request *req, int err);
+struct tb_cfg_result tb_cfg_request_sync(struct tb_ctl *ctl,
+			struct tb_cfg_request *req, int timeout_msec);
+
+static inline u64 tb_cfg_get_route(const struct tb_cfg_header *header)
+{
+	return (u64) header->route_hi << 32 | header->route_lo;
+}
+
+static inline struct tb_cfg_header tb_cfg_make_header(u64 route)
+{
+	struct tb_cfg_header header = {
+		.route_hi = route >> 32,
+		.route_lo = route,
+	};
+	/* check for overflow, route_hi is not 32 bits! */
+	WARN_ON(tb_cfg_get_route(&header) != route);
+	return header;
+}
 
 int tb_cfg_error(struct tb_ctl *ctl, u64 route, u32 port,
 		 enum tb_cfg_error error);
@@ -61,13 +128,13 @@ struct tb_cfg_result tb_cfg_read_raw(struct tb_ctl *ctl, void *buffer,
 				     u64 route, u32 port,
 				     enum tb_cfg_space space, u32 offset,
 				     u32 length, int timeout_msec);
-struct tb_cfg_result tb_cfg_write_raw(struct tb_ctl *ctl, void *buffer,
+struct tb_cfg_result tb_cfg_write_raw(struct tb_ctl *ctl, const void *buffer,
 				      u64 route, u32 port,
 				      enum tb_cfg_space space, u32 offset,
 				      u32 length, int timeout_msec);
 int tb_cfg_read(struct tb_ctl *ctl, void *buffer, u64 route, u32 port,
 		enum tb_cfg_space space, u32 offset, u32 length);
-int tb_cfg_write(struct tb_ctl *ctl, void *buffer, u64 route, u32 port,
+int tb_cfg_write(struct tb_ctl *ctl, const void *buffer, u64 route, u32 port,
 		 enum tb_cfg_space space, u32 offset, u32 length);
 int tb_cfg_get_upstream_port(struct tb_ctl *ctl, u64 route);
 
diff --git a/drivers/thunderbolt/dma_port.c b/drivers/thunderbolt/dma_port.c
new file mode 100644
index 000000000000..af6dde347bee
--- /dev/null
+++ b/drivers/thunderbolt/dma_port.c
@@ -0,0 +1,524 @@
+/*
+ * Thunderbolt DMA configuration based mailbox support
+ *
+ * Copyright (C) 2017, Intel Corporation
+ * Authors: Michael Jamet <michael.jamet@intel.com>
+ *          Mika Westerberg <mika.westerberg@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+#include "dma_port.h"
+#include "tb_regs.h"
+
+#define DMA_PORT_CAP			0x3e
+
+#define MAIL_DATA			1
+#define MAIL_DATA_DWORDS		16
+
+#define MAIL_IN				17
+#define MAIL_IN_CMD_SHIFT		28
+#define MAIL_IN_CMD_MASK		GENMASK(31, 28)
+#define MAIL_IN_CMD_FLASH_WRITE		0x0
+#define MAIL_IN_CMD_FLASH_UPDATE_AUTH	0x1
+#define MAIL_IN_CMD_FLASH_READ		0x2
+#define MAIL_IN_CMD_POWER_CYCLE		0x4
+#define MAIL_IN_DWORDS_SHIFT		24
+#define MAIL_IN_DWORDS_MASK		GENMASK(27, 24)
+#define MAIL_IN_ADDRESS_SHIFT		2
+#define MAIL_IN_ADDRESS_MASK		GENMASK(23, 2)
+#define MAIL_IN_CSS			BIT(1)
+#define MAIL_IN_OP_REQUEST		BIT(0)
+
+#define MAIL_OUT			18
+#define MAIL_OUT_STATUS_RESPONSE	BIT(29)
+#define MAIL_OUT_STATUS_CMD_SHIFT	4
+#define MAIL_OUT_STATUS_CMD_MASK	GENMASK(7, 4)
+#define MAIL_OUT_STATUS_MASK		GENMASK(3, 0)
+#define MAIL_OUT_STATUS_COMPLETED	0
+#define MAIL_OUT_STATUS_ERR_AUTH	1
+#define MAIL_OUT_STATUS_ERR_ACCESS	2
+
+#define DMA_PORT_TIMEOUT		5000 /* ms */
+#define DMA_PORT_RETRIES		3
+
+/**
+ * struct tb_dma_port - DMA control port
+ * @sw: Switch the DMA port belongs to
+ * @port: Switch port number where DMA capability is found
+ * @base: Start offset of the mailbox registers
+ * @buf: Temporary buffer to store a single block
+ */
+struct tb_dma_port {
+	struct tb_switch *sw;
+	u8 port;
+	u32 base;
+	u8 *buf;
+};
+
+/*
+ * When the switch is in safe mode it supports very little functionality
+ * so we don't validate that much here.
+ */
+static bool dma_port_match(const struct tb_cfg_request *req,
+			   const struct ctl_pkg *pkg)
+{
+	u64 route = tb_cfg_get_route(pkg->buffer) & ~BIT_ULL(63);
+
+	if (pkg->frame.eof == TB_CFG_PKG_ERROR)
+		return true;
+	if (pkg->frame.eof != req->response_type)
+		return false;
+	if (route != tb_cfg_get_route(req->request))
+		return false;
+	if (pkg->frame.size != req->response_size)
+		return false;
+
+	return true;
+}
+
+static bool dma_port_copy(struct tb_cfg_request *req, const struct ctl_pkg *pkg)
+{
+	memcpy(req->response, pkg->buffer, req->response_size);
+	return true;
+}
+
+static int dma_port_read(struct tb_ctl *ctl, void *buffer, u64 route,
+			 u32 port, u32 offset, u32 length, int timeout_msec)
+{
+	struct cfg_read_pkg request = {
+		.header = tb_cfg_make_header(route),
+		.addr = {
+			.seq = 1,
+			.port = port,
+			.space = TB_CFG_PORT,
+			.offset = offset,
+			.length = length,
+		},
+	};
+	struct tb_cfg_request *req;
+	struct cfg_write_pkg reply;
+	struct tb_cfg_result res;
+
+	req = tb_cfg_request_alloc();
+	if (!req)
+		return -ENOMEM;
+
+	req->match = dma_port_match;
+	req->copy = dma_port_copy;
+	req->request = &request;
+	req->request_size = sizeof(request);
+	req->request_type = TB_CFG_PKG_READ;
+	req->response = &reply;
+	req->response_size = 12 + 4 * length;
+	req->response_type = TB_CFG_PKG_READ;
+
+	res = tb_cfg_request_sync(ctl, req, timeout_msec);
+
+	tb_cfg_request_put(req);
+
+	if (res.err)
+		return res.err;
+
+	memcpy(buffer, &reply.data, 4 * length);
+	return 0;
+}
+
+static int dma_port_write(struct tb_ctl *ctl, const void *buffer, u64 route,
+			  u32 port, u32 offset, u32 length, int timeout_msec)
+{
+	struct cfg_write_pkg request = {
+		.header = tb_cfg_make_header(route),
+		.addr = {
+			.seq = 1,
+			.port = port,
+			.space = TB_CFG_PORT,
+			.offset = offset,
+			.length = length,
+		},
+	};
+	struct tb_cfg_request *req;
+	struct cfg_read_pkg reply;
+	struct tb_cfg_result res;
+
+	memcpy(&request.data, buffer, length * 4);
+
+	req = tb_cfg_request_alloc();
+	if (!req)
+		return -ENOMEM;
+
+	req->match = dma_port_match;
+	req->copy = dma_port_copy;
+	req->request = &request;
+	req->request_size = 12 + 4 * length;
+	req->request_type = TB_CFG_PKG_WRITE;
+	req->response = &reply;
+	req->response_size = sizeof(reply);
+	req->response_type = TB_CFG_PKG_WRITE;
+
+	res = tb_cfg_request_sync(ctl, req, timeout_msec);
+
+	tb_cfg_request_put(req);
+
+	return res.err;
+}
+
+static int dma_find_port(struct tb_switch *sw)
+{
+	int port, ret;
+	u32 type;
+
+	/*
+	 * The DMA (NHI) port is either 3 or 5 depending on the
+	 * controller. Try both starting from 5 which is more common.
+	 */
+	port = 5;
+	ret = dma_port_read(sw->tb->ctl, &type, tb_route(sw), port, 2, 1,
+			    DMA_PORT_TIMEOUT);
+	if (!ret && (type & 0xffffff) == TB_TYPE_NHI)
+		return port;
+
+	port = 3;
+	ret = dma_port_read(sw->tb->ctl, &type, tb_route(sw), port, 2, 1,
+			    DMA_PORT_TIMEOUT);
+	if (!ret && (type & 0xffffff) == TB_TYPE_NHI)
+		return port;
+
+	return -ENODEV;
+}
+
+/**
+ * dma_port_alloc() - Finds DMA control port from a switch pointed by route
+ * @sw: Switch from where find the DMA port
+ *
+ * Function checks if the switch NHI port supports DMA configuration
+ * based mailbox capability and if it does, allocates and initializes
+ * DMA port structure. Returns %NULL if the capabity was not found.
+ *
+ * The DMA control port is functional also when the switch is in safe
+ * mode.
+ */
+struct tb_dma_port *dma_port_alloc(struct tb_switch *sw)
+{
+	struct tb_dma_port *dma;
+	int port;
+
+	port = dma_find_port(sw);
+	if (port < 0)
+		return NULL;
+
+	dma = kzalloc(sizeof(*dma), GFP_KERNEL);
+	if (!dma)
+		return NULL;
+
+	dma->buf = kmalloc_array(MAIL_DATA_DWORDS, sizeof(u32), GFP_KERNEL);
+	if (!dma->buf) {
+		kfree(dma);
+		return NULL;
+	}
+
+	dma->sw = sw;
+	dma->port = port;
+	dma->base = DMA_PORT_CAP;
+
+	return dma;
+}
+
+/**
+ * dma_port_free() - Release DMA control port structure
+ * @dma: DMA control port
+ */
+void dma_port_free(struct tb_dma_port *dma)
+{
+	if (dma) {
+		kfree(dma->buf);
+		kfree(dma);
+	}
+}
+
+static int dma_port_wait_for_completion(struct tb_dma_port *dma,
+					unsigned int timeout)
+{
+	unsigned long end = jiffies + msecs_to_jiffies(timeout);
+	struct tb_switch *sw = dma->sw;
+
+	do {
+		int ret;
+		u32 in;
+
+		ret = dma_port_read(sw->tb->ctl, &in, tb_route(sw), dma->port,
+				    dma->base + MAIL_IN, 1, 50);
+		if (ret) {
+			if (ret != -ETIMEDOUT)
+				return ret;
+		} else if (!(in & MAIL_IN_OP_REQUEST)) {
+			return 0;
+		}
+
+		usleep_range(50, 100);
+	} while (time_before(jiffies, end));
+
+	return -ETIMEDOUT;
+}
+
+static int status_to_errno(u32 status)
+{
+	switch (status & MAIL_OUT_STATUS_MASK) {
+	case MAIL_OUT_STATUS_COMPLETED:
+		return 0;
+	case MAIL_OUT_STATUS_ERR_AUTH:
+		return -EINVAL;
+	case MAIL_OUT_STATUS_ERR_ACCESS:
+		return -EACCES;
+	}
+
+	return -EIO;
+}
+
+static int dma_port_request(struct tb_dma_port *dma, u32 in,
+			    unsigned int timeout)
+{
+	struct tb_switch *sw = dma->sw;
+	u32 out;
+	int ret;
+
+	ret = dma_port_write(sw->tb->ctl, &in, tb_route(sw), dma->port,
+			     dma->base + MAIL_IN, 1, DMA_PORT_TIMEOUT);
+	if (ret)
+		return ret;
+
+	ret = dma_port_wait_for_completion(dma, timeout);
+	if (ret)
+		return ret;
+
+	ret = dma_port_read(sw->tb->ctl, &out, tb_route(sw), dma->port,
+			    dma->base + MAIL_OUT, 1, DMA_PORT_TIMEOUT);
+	if (ret)
+		return ret;
+
+	return status_to_errno(out);
+}
+
+static int dma_port_flash_read_block(struct tb_dma_port *dma, u32 address,
+				     void *buf, u32 size)
+{
+	struct tb_switch *sw = dma->sw;
+	u32 in, dwaddress, dwords;
+	int ret;
+
+	dwaddress = address / 4;
+	dwords = size / 4;
+
+	in = MAIL_IN_CMD_FLASH_READ << MAIL_IN_CMD_SHIFT;
+	if (dwords < MAIL_DATA_DWORDS)
+		in |= (dwords << MAIL_IN_DWORDS_SHIFT) & MAIL_IN_DWORDS_MASK;
+	in |= (dwaddress << MAIL_IN_ADDRESS_SHIFT) & MAIL_IN_ADDRESS_MASK;
+	in |= MAIL_IN_OP_REQUEST;
+
+	ret = dma_port_request(dma, in, DMA_PORT_TIMEOUT);
+	if (ret)
+		return ret;
+
+	return dma_port_read(sw->tb->ctl, buf, tb_route(sw), dma->port,
+			     dma->base + MAIL_DATA, dwords, DMA_PORT_TIMEOUT);
+}
+
+static int dma_port_flash_write_block(struct tb_dma_port *dma, u32 address,
+				      const void *buf, u32 size)
+{
+	struct tb_switch *sw = dma->sw;
+	u32 in, dwaddress, dwords;
+	int ret;
+
+	dwords = size / 4;
+
+	/* Write the block to MAIL_DATA registers */
+	ret = dma_port_write(sw->tb->ctl, buf, tb_route(sw), dma->port,
+			    dma->base + MAIL_DATA, dwords, DMA_PORT_TIMEOUT);
+
+	in = MAIL_IN_CMD_FLASH_WRITE << MAIL_IN_CMD_SHIFT;
+
+	/* CSS header write is always done to the same magic address */
+	if (address >= DMA_PORT_CSS_ADDRESS) {
+		dwaddress = DMA_PORT_CSS_ADDRESS;
+		in |= MAIL_IN_CSS;
+	} else {
+		dwaddress = address / 4;
+	}
+
+	in |= ((dwords - 1) << MAIL_IN_DWORDS_SHIFT) & MAIL_IN_DWORDS_MASK;
+	in |= (dwaddress << MAIL_IN_ADDRESS_SHIFT) & MAIL_IN_ADDRESS_MASK;
+	in |= MAIL_IN_OP_REQUEST;
+
+	return dma_port_request(dma, in, DMA_PORT_TIMEOUT);
+}
+
+/**
+ * dma_port_flash_read() - Read from active flash region
+ * @dma: DMA control port
+ * @address: Address relative to the start of active region
+ * @buf: Buffer where the data is read
+ * @size: Size of the buffer
+ */
+int dma_port_flash_read(struct tb_dma_port *dma, unsigned int address,
+			void *buf, size_t size)
+{
+	unsigned int retries = DMA_PORT_RETRIES;
+	unsigned int offset;
+
+	offset = address & 3;
+	address = address & ~3;
+
+	do {
+		u32 nbytes = min_t(u32, size, MAIL_DATA_DWORDS * 4);
+		int ret;
+
+		ret = dma_port_flash_read_block(dma, address, dma->buf,
+						ALIGN(nbytes, 4));
+		if (ret) {
+			if (ret == -ETIMEDOUT) {
+				if (retries--)
+					continue;
+				ret = -EIO;
+			}
+			return ret;
+		}
+
+		memcpy(buf, dma->buf + offset, nbytes);
+
+		size -= nbytes;
+		address += nbytes;
+		buf += nbytes;
+	} while (size > 0);
+
+	return 0;
+}
+
+/**
+ * dma_port_flash_write() - Write to non-active flash region
+ * @dma: DMA control port
+ * @address: Address relative to the start of non-active region
+ * @buf: Data to write
+ * @size: Size of the buffer
+ *
+ * Writes block of data to the non-active flash region of the switch. If
+ * the address is given as %DMA_PORT_CSS_ADDRESS the block is written
+ * using CSS command.
+ */
+int dma_port_flash_write(struct tb_dma_port *dma, unsigned int address,
+			 const void *buf, size_t size)
+{
+	unsigned int retries = DMA_PORT_RETRIES;
+	unsigned int offset;
+
+	if (address >= DMA_PORT_CSS_ADDRESS) {
+		offset = 0;
+		if (size > DMA_PORT_CSS_MAX_SIZE)
+			return -E2BIG;
+	} else {
+		offset = address & 3;
+		address = address & ~3;
+	}
+
+	do {
+		u32 nbytes = min_t(u32, size, MAIL_DATA_DWORDS * 4);
+		int ret;
+
+		memcpy(dma->buf + offset, buf, nbytes);
+
+		ret = dma_port_flash_write_block(dma, address, buf, nbytes);
+		if (ret) {
+			if (ret == -ETIMEDOUT) {
+				if (retries--)
+					continue;
+				ret = -EIO;
+			}
+			return ret;
+		}
+
+		size -= nbytes;
+		address += nbytes;
+		buf += nbytes;
+	} while (size > 0);
+
+	return 0;
+}
+
+/**
+ * dma_port_flash_update_auth() - Starts flash authenticate cycle
+ * @dma: DMA control port
+ *
+ * Starts the flash update authentication cycle. If the image in the
+ * non-active area was valid, the switch starts upgrade process where
+ * active and non-active area get swapped in the end. Caller should call
+ * dma_port_flash_update_auth_status() to get status of this command.
+ * This is because if the switch in question is root switch the
+ * thunderbolt host controller gets reset as well.
+ */
+int dma_port_flash_update_auth(struct tb_dma_port *dma)
+{
+	u32 in;
+
+	in = MAIL_IN_CMD_FLASH_UPDATE_AUTH << MAIL_IN_CMD_SHIFT;
+	in |= MAIL_IN_OP_REQUEST;
+
+	return dma_port_request(dma, in, 150);
+}
+
+/**
+ * dma_port_flash_update_auth_status() - Reads status of update auth command
+ * @dma: DMA control port
+ * @status: Status code of the operation
+ *
+ * The function checks if there is status available from the last update
+ * auth command. Returns %0 if there is no status and no further
+ * action is required. If there is status, %1 is returned instead and
+ * @status holds the failure code.
+ *
+ * Negative return means there was an error reading status from the
+ * switch.
+ */
+int dma_port_flash_update_auth_status(struct tb_dma_port *dma, u32 *status)
+{
+	struct tb_switch *sw = dma->sw;
+	u32 out, cmd;
+	int ret;
+
+	ret = dma_port_read(sw->tb->ctl, &out, tb_route(sw), dma->port,
+			    dma->base + MAIL_OUT, 1, DMA_PORT_TIMEOUT);
+	if (ret)
+		return ret;
+
+	/* Check if the status relates to flash update auth */
+	cmd = (out & MAIL_OUT_STATUS_CMD_MASK) >> MAIL_OUT_STATUS_CMD_SHIFT;
+	if (cmd == MAIL_IN_CMD_FLASH_UPDATE_AUTH) {
+		if (status)
+			*status = out & MAIL_OUT_STATUS_MASK;
+
+		/* Reset is needed in any case */
+		return 1;
+	}
+
+	return 0;
+}
+
+/**
+ * dma_port_power_cycle() - Power cycles the switch
+ * @dma: DMA control port
+ *
+ * Triggers power cycle to the switch.
+ */
+int dma_port_power_cycle(struct tb_dma_port *dma)
+{
+	u32 in;
+
+	in = MAIL_IN_CMD_POWER_CYCLE << MAIL_IN_CMD_SHIFT;
+	in |= MAIL_IN_OP_REQUEST;
+
+	return dma_port_request(dma, in, 150);
+}
diff --git a/drivers/thunderbolt/dma_port.h b/drivers/thunderbolt/dma_port.h
new file mode 100644
index 000000000000..c4a69e0fbff7
--- /dev/null
+++ b/drivers/thunderbolt/dma_port.h
@@ -0,0 +1,34 @@
+/*
+ * Thunderbolt DMA configuration based mailbox support
+ *
+ * Copyright (C) 2017, Intel Corporation
+ * Authors: Michael Jamet <michael.jamet@intel.com>
+ *          Mika Westerberg <mika.westerberg@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef DMA_PORT_H_
+#define DMA_PORT_H_
+
+#include "tb.h"
+
+struct tb_switch;
+struct tb_dma_port;
+
+#define DMA_PORT_CSS_ADDRESS		0x3fffff
+#define DMA_PORT_CSS_MAX_SIZE		SZ_128
+
+struct tb_dma_port *dma_port_alloc(struct tb_switch *sw);
+void dma_port_free(struct tb_dma_port *dma);
+int dma_port_flash_read(struct tb_dma_port *dma, unsigned int address,
+			void *buf, size_t size);
+int dma_port_flash_update_auth(struct tb_dma_port *dma);
+int dma_port_flash_update_auth_status(struct tb_dma_port *dma, u32 *status);
+int dma_port_flash_write(struct tb_dma_port *dma, unsigned int address,
+			 const void *buf, size_t size);
+int dma_port_power_cycle(struct tb_dma_port *dma);
+
+#endif
diff --git a/drivers/thunderbolt/domain.c b/drivers/thunderbolt/domain.c
new file mode 100644
index 000000000000..9f2dcd48974d
--- /dev/null
+++ b/drivers/thunderbolt/domain.c
@@ -0,0 +1,456 @@
+/*
+ * Thunderbolt bus support
+ *
+ * Copyright (C) 2017, Intel Corporation
+ * Author:  Mika Westerberg <mika.westerberg@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/device.h>
+#include <linux/idr.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <crypto/hash.h>
+
+#include "tb.h"
+
+static DEFINE_IDA(tb_domain_ida);
+
+static const char * const tb_security_names[] = {
+	[TB_SECURITY_NONE] = "none",
+	[TB_SECURITY_USER] = "user",
+	[TB_SECURITY_SECURE] = "secure",
+	[TB_SECURITY_DPONLY] = "dponly",
+};
+
+static ssize_t security_show(struct device *dev, struct device_attribute *attr,
+			     char *buf)
+{
+	struct tb *tb = container_of(dev, struct tb, dev);
+
+	return sprintf(buf, "%s\n", tb_security_names[tb->security_level]);
+}
+static DEVICE_ATTR_RO(security);
+
+static struct attribute *domain_attrs[] = {
+	&dev_attr_security.attr,
+	NULL,
+};
+
+static struct attribute_group domain_attr_group = {
+	.attrs = domain_attrs,
+};
+
+static const struct attribute_group *domain_attr_groups[] = {
+	&domain_attr_group,
+	NULL,
+};
+
+struct bus_type tb_bus_type = {
+	.name = "thunderbolt",
+};
+
+static void tb_domain_release(struct device *dev)
+{
+	struct tb *tb = container_of(dev, struct tb, dev);
+
+	tb_ctl_free(tb->ctl);
+	destroy_workqueue(tb->wq);
+	ida_simple_remove(&tb_domain_ida, tb->index);
+	mutex_destroy(&tb->lock);
+	kfree(tb);
+}
+
+struct device_type tb_domain_type = {
+	.name = "thunderbolt_domain",
+	.release = tb_domain_release,
+};
+
+/**
+ * tb_domain_alloc() - Allocate a domain
+ * @nhi: Pointer to the host controller
+ * @privsize: Size of the connection manager private data
+ *
+ * Allocates and initializes a new Thunderbolt domain. Connection
+ * managers are expected to call this and then fill in @cm_ops
+ * accordingly.
+ *
+ * Call tb_domain_put() to release the domain before it has been added
+ * to the system.
+ *
+ * Return: allocated domain structure on %NULL in case of error
+ */
+struct tb *tb_domain_alloc(struct tb_nhi *nhi, size_t privsize)
+{
+	struct tb *tb;
+
+	/*
+	 * Make sure the structure sizes map with that the hardware
+	 * expects because bit-fields are being used.
+	 */
+	BUILD_BUG_ON(sizeof(struct tb_regs_switch_header) != 5 * 4);
+	BUILD_BUG_ON(sizeof(struct tb_regs_port_header) != 8 * 4);
+	BUILD_BUG_ON(sizeof(struct tb_regs_hop) != 2 * 4);
+
+	tb = kzalloc(sizeof(*tb) + privsize, GFP_KERNEL);
+	if (!tb)
+		return NULL;
+
+	tb->nhi = nhi;
+	mutex_init(&tb->lock);
+
+	tb->index = ida_simple_get(&tb_domain_ida, 0, 0, GFP_KERNEL);
+	if (tb->index < 0)
+		goto err_free;
+
+	tb->wq = alloc_ordered_workqueue("thunderbolt%d", 0, tb->index);
+	if (!tb->wq)
+		goto err_remove_ida;
+
+	tb->dev.parent = &nhi->pdev->dev;
+	tb->dev.bus = &tb_bus_type;
+	tb->dev.type = &tb_domain_type;
+	tb->dev.groups = domain_attr_groups;
+	dev_set_name(&tb->dev, "domain%d", tb->index);
+	device_initialize(&tb->dev);
+
+	return tb;
+
+err_remove_ida:
+	ida_simple_remove(&tb_domain_ida, tb->index);
+err_free:
+	kfree(tb);
+
+	return NULL;
+}
+
+static void tb_domain_event_cb(void *data, enum tb_cfg_pkg_type type,
+			       const void *buf, size_t size)
+{
+	struct tb *tb = data;
+
+	if (!tb->cm_ops->handle_event) {
+		tb_warn(tb, "domain does not have event handler\n");
+		return;
+	}
+
+	tb->cm_ops->handle_event(tb, type, buf, size);
+}
+
+/**
+ * tb_domain_add() - Add domain to the system
+ * @tb: Domain to add
+ *
+ * Starts the domain and adds it to the system. Hotplugging devices will
+ * work after this has been returned successfully. In order to remove
+ * and release the domain after this function has been called, call
+ * tb_domain_remove().
+ *
+ * Return: %0 in case of success and negative errno in case of error
+ */
+int tb_domain_add(struct tb *tb)
+{
+	int ret;
+
+	if (WARN_ON(!tb->cm_ops))
+		return -EINVAL;
+
+	mutex_lock(&tb->lock);
+
+	tb->ctl = tb_ctl_alloc(tb->nhi, tb_domain_event_cb, tb);
+	if (!tb->ctl) {
+		ret = -ENOMEM;
+		goto err_unlock;
+	}
+
+	/*
+	 * tb_schedule_hotplug_handler may be called as soon as the config
+	 * channel is started. Thats why we have to hold the lock here.
+	 */
+	tb_ctl_start(tb->ctl);
+
+	if (tb->cm_ops->driver_ready) {
+		ret = tb->cm_ops->driver_ready(tb);
+		if (ret)
+			goto err_ctl_stop;
+	}
+
+	ret = device_add(&tb->dev);
+	if (ret)
+		goto err_ctl_stop;
+
+	/* Start the domain */
+	if (tb->cm_ops->start) {
+		ret = tb->cm_ops->start(tb);
+		if (ret)
+			goto err_domain_del;
+	}
+
+	/* This starts event processing */
+	mutex_unlock(&tb->lock);
+
+	return 0;
+
+err_domain_del:
+	device_del(&tb->dev);
+err_ctl_stop:
+	tb_ctl_stop(tb->ctl);
+err_unlock:
+	mutex_unlock(&tb->lock);
+
+	return ret;
+}
+
+/**
+ * tb_domain_remove() - Removes and releases a domain
+ * @tb: Domain to remove
+ *
+ * Stops the domain, removes it from the system and releases all
+ * resources once the last reference has been released.
+ */
+void tb_domain_remove(struct tb *tb)
+{
+	mutex_lock(&tb->lock);
+	if (tb->cm_ops->stop)
+		tb->cm_ops->stop(tb);
+	/* Stop the domain control traffic */
+	tb_ctl_stop(tb->ctl);
+	mutex_unlock(&tb->lock);
+
+	flush_workqueue(tb->wq);
+	device_unregister(&tb->dev);
+}
+
+/**
+ * tb_domain_suspend_noirq() - Suspend a domain
+ * @tb: Domain to suspend
+ *
+ * Suspends all devices in the domain and stops the control channel.
+ */
+int tb_domain_suspend_noirq(struct tb *tb)
+{
+	int ret = 0;
+
+	/*
+	 * The control channel interrupt is left enabled during suspend
+	 * and taking the lock here prevents any events happening before
+	 * we actually have stopped the domain and the control channel.
+	 */
+	mutex_lock(&tb->lock);
+	if (tb->cm_ops->suspend_noirq)
+		ret = tb->cm_ops->suspend_noirq(tb);
+	if (!ret)
+		tb_ctl_stop(tb->ctl);
+	mutex_unlock(&tb->lock);
+
+	return ret;
+}
+
+/**
+ * tb_domain_resume_noirq() - Resume a domain
+ * @tb: Domain to resume
+ *
+ * Re-starts the control channel, and resumes all devices connected to
+ * the domain.
+ */
+int tb_domain_resume_noirq(struct tb *tb)
+{
+	int ret = 0;
+
+	mutex_lock(&tb->lock);
+	tb_ctl_start(tb->ctl);
+	if (tb->cm_ops->resume_noirq)
+		ret = tb->cm_ops->resume_noirq(tb);
+	mutex_unlock(&tb->lock);
+
+	return ret;
+}
+
+int tb_domain_suspend(struct tb *tb)
+{
+	int ret;
+
+	mutex_lock(&tb->lock);
+	if (tb->cm_ops->suspend) {
+		ret = tb->cm_ops->suspend(tb);
+		if (ret) {
+			mutex_unlock(&tb->lock);
+			return ret;
+		}
+	}
+	mutex_unlock(&tb->lock);
+	return 0;
+}
+
+void tb_domain_complete(struct tb *tb)
+{
+	mutex_lock(&tb->lock);
+	if (tb->cm_ops->complete)
+		tb->cm_ops->complete(tb);
+	mutex_unlock(&tb->lock);
+}
+
+/**
+ * tb_domain_approve_switch() - Approve switch
+ * @tb: Domain the switch belongs to
+ * @sw: Switch to approve
+ *
+ * This will approve switch by connection manager specific means. In
+ * case of success the connection manager will create tunnels for all
+ * supported protocols.
+ */
+int tb_domain_approve_switch(struct tb *tb, struct tb_switch *sw)
+{
+	struct tb_switch *parent_sw;
+
+	if (!tb->cm_ops->approve_switch)
+		return -EPERM;
+
+	/* The parent switch must be authorized before this one */
+	parent_sw = tb_to_switch(sw->dev.parent);
+	if (!parent_sw || !parent_sw->authorized)
+		return -EINVAL;
+
+	return tb->cm_ops->approve_switch(tb, sw);
+}
+
+/**
+ * tb_domain_approve_switch_key() - Approve switch and add key
+ * @tb: Domain the switch belongs to
+ * @sw: Switch to approve
+ *
+ * For switches that support secure connect, this function first adds
+ * key to the switch NVM using connection manager specific means. If
+ * adding the key is successful, the switch is approved and connected.
+ *
+ * Return: %0 on success and negative errno in case of failure.
+ */
+int tb_domain_approve_switch_key(struct tb *tb, struct tb_switch *sw)
+{
+	struct tb_switch *parent_sw;
+	int ret;
+
+	if (!tb->cm_ops->approve_switch || !tb->cm_ops->add_switch_key)
+		return -EPERM;
+
+	/* The parent switch must be authorized before this one */
+	parent_sw = tb_to_switch(sw->dev.parent);
+	if (!parent_sw || !parent_sw->authorized)
+		return -EINVAL;
+
+	ret = tb->cm_ops->add_switch_key(tb, sw);
+	if (ret)
+		return ret;
+
+	return tb->cm_ops->approve_switch(tb, sw);
+}
+
+/**
+ * tb_domain_challenge_switch_key() - Challenge and approve switch
+ * @tb: Domain the switch belongs to
+ * @sw: Switch to approve
+ *
+ * For switches that support secure connect, this function generates
+ * random challenge and sends it to the switch. The switch responds to
+ * this and if the response matches our random challenge, the switch is
+ * approved and connected.
+ *
+ * Return: %0 on success and negative errno in case of failure.
+ */
+int tb_domain_challenge_switch_key(struct tb *tb, struct tb_switch *sw)
+{
+	u8 challenge[TB_SWITCH_KEY_SIZE];
+	u8 response[TB_SWITCH_KEY_SIZE];
+	u8 hmac[TB_SWITCH_KEY_SIZE];
+	struct tb_switch *parent_sw;
+	struct crypto_shash *tfm;
+	struct shash_desc *shash;
+	int ret;
+
+	if (!tb->cm_ops->approve_switch || !tb->cm_ops->challenge_switch_key)
+		return -EPERM;
+
+	/* The parent switch must be authorized before this one */
+	parent_sw = tb_to_switch(sw->dev.parent);
+	if (!parent_sw || !parent_sw->authorized)
+		return -EINVAL;
+
+	get_random_bytes(challenge, sizeof(challenge));
+	ret = tb->cm_ops->challenge_switch_key(tb, sw, challenge, response);
+	if (ret)
+		return ret;
+
+	tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+	if (IS_ERR(tfm))
+		return PTR_ERR(tfm);
+
+	ret = crypto_shash_setkey(tfm, sw->key, TB_SWITCH_KEY_SIZE);
+	if (ret)
+		goto err_free_tfm;
+
+	shash = kzalloc(sizeof(*shash) + crypto_shash_descsize(tfm),
+			GFP_KERNEL);
+	if (!shash) {
+		ret = -ENOMEM;
+		goto err_free_tfm;
+	}
+
+	shash->tfm = tfm;
+	shash->flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+	memset(hmac, 0, sizeof(hmac));
+	ret = crypto_shash_digest(shash, challenge, sizeof(hmac), hmac);
+	if (ret)
+		goto err_free_shash;
+
+	/* The returned HMAC must match the one we calculated */
+	if (memcmp(response, hmac, sizeof(hmac))) {
+		ret = -EKEYREJECTED;
+		goto err_free_shash;
+	}
+
+	crypto_free_shash(tfm);
+	kfree(shash);
+
+	return tb->cm_ops->approve_switch(tb, sw);
+
+err_free_shash:
+	kfree(shash);
+err_free_tfm:
+	crypto_free_shash(tfm);
+
+	return ret;
+}
+
+/**
+ * tb_domain_disconnect_pcie_paths() - Disconnect all PCIe paths
+ * @tb: Domain whose PCIe paths to disconnect
+ *
+ * This needs to be called in preparation for NVM upgrade of the host
+ * controller. Makes sure all PCIe paths are disconnected.
+ *
+ * Return %0 on success and negative errno in case of error.
+ */
+int tb_domain_disconnect_pcie_paths(struct tb *tb)
+{
+	if (!tb->cm_ops->disconnect_pcie_paths)
+		return -EPERM;
+
+	return tb->cm_ops->disconnect_pcie_paths(tb);
+}
+
+int tb_domain_init(void)
+{
+	return bus_register(&tb_bus_type);
+}
+
+void tb_domain_exit(void)
+{
+	bus_unregister(&tb_bus_type);
+	ida_destroy(&tb_domain_ida);
+	tb_switch_exit();
+}
diff --git a/drivers/thunderbolt/eeprom.c b/drivers/thunderbolt/eeprom.c
index 6392990c984d..308b6e17c88a 100644
--- a/drivers/thunderbolt/eeprom.c
+++ b/drivers/thunderbolt/eeprom.c
@@ -204,6 +204,11 @@ struct tb_drom_entry_header {
 	enum tb_drom_entry_type type:1;
 } __packed;
 
+struct tb_drom_entry_generic {
+	struct tb_drom_entry_header header;
+	u8 data[0];
+} __packed;
+
 struct tb_drom_entry_port {
 	/* BYTES 0-1 */
 	struct tb_drom_entry_header header;
@@ -276,6 +281,9 @@ int tb_drom_read_uid_only(struct tb_switch *sw, u64 *uid)
 	if (res)
 		return res;
 
+	if (drom_offset == 0)
+		return -ENODEV;
+
 	/* read uid */
 	res = tb_eeprom_read_n(sw, drom_offset, data, 9);
 	if (res)
@@ -283,7 +291,7 @@ int tb_drom_read_uid_only(struct tb_switch *sw, u64 *uid)
 
 	crc = tb_crc8(data + 1, 8);
 	if (crc != data[0]) {
-		tb_sw_warn(sw, "uid crc8 missmatch (expected: %#x, got: %#x)\n",
+		tb_sw_warn(sw, "uid crc8 mismatch (expected: %#x, got: %#x)\n",
 				data[0], crc);
 		return -EIO;
 	}
@@ -292,25 +300,39 @@ int tb_drom_read_uid_only(struct tb_switch *sw, u64 *uid)
 	return 0;
 }
 
-static void tb_drom_parse_port_entry(struct tb_port *port,
-		struct tb_drom_entry_port *entry)
+static int tb_drom_parse_entry_generic(struct tb_switch *sw,
+		struct tb_drom_entry_header *header)
 {
-	port->link_nr = entry->link_nr;
-	if (entry->has_dual_link_port)
-		port->dual_link_port =
-				&port->sw->ports[entry->dual_link_port_nr];
+	const struct tb_drom_entry_generic *entry =
+		(const struct tb_drom_entry_generic *)header;
+
+	switch (header->index) {
+	case 1:
+		/* Length includes 2 bytes header so remove it before copy */
+		sw->vendor_name = kstrndup(entry->data,
+			header->len - sizeof(*header), GFP_KERNEL);
+		if (!sw->vendor_name)
+			return -ENOMEM;
+		break;
+
+	case 2:
+		sw->device_name = kstrndup(entry->data,
+			header->len - sizeof(*header), GFP_KERNEL);
+		if (!sw->device_name)
+			return -ENOMEM;
+		break;
+	}
+
+	return 0;
 }
 
-static int tb_drom_parse_entry(struct tb_switch *sw,
-		struct tb_drom_entry_header *header)
+static int tb_drom_parse_entry_port(struct tb_switch *sw,
+				    struct tb_drom_entry_header *header)
 {
 	struct tb_port *port;
 	int res;
 	enum tb_port_type type;
 
-	if (header->type != TB_DROM_ENTRY_PORT)
-		return 0;
-
 	port = &sw->ports[header->index];
 	port->disabled = header->port_disabled;
 	if (port->disabled)
@@ -329,7 +351,10 @@ static int tb_drom_parse_entry(struct tb_switch *sw,
 				header->len, sizeof(struct tb_drom_entry_port));
 			return -EIO;
 		}
-		tb_drom_parse_port_entry(port, entry);
+		port->link_nr = entry->link_nr;
+		if (entry->has_dual_link_port)
+			port->dual_link_port =
+				&port->sw->ports[entry->dual_link_port_nr];
 	}
 	return 0;
 }
@@ -344,6 +369,7 @@ static int tb_drom_parse_entries(struct tb_switch *sw)
 	struct tb_drom_header *header = (void *) sw->drom;
 	u16 pos = sizeof(*header);
 	u16 drom_size = header->data_len + TB_DROM_DATA_START;
+	int res;
 
 	while (pos < drom_size) {
 		struct tb_drom_entry_header *entry = (void *) (sw->drom + pos);
@@ -353,7 +379,16 @@ static int tb_drom_parse_entries(struct tb_switch *sw)
 			return -EIO;
 		}
 
-		tb_drom_parse_entry(sw, entry);
+		switch (entry->type) {
+		case TB_DROM_ENTRY_GENERIC:
+			res = tb_drom_parse_entry_generic(sw, entry);
+			break;
+		case TB_DROM_ENTRY_PORT:
+			res = tb_drom_parse_entry_port(sw, entry);
+			break;
+		}
+		if (res)
+			return res;
 
 		pos += entry->len;
 	}
@@ -394,6 +429,50 @@ err:
 	return -EINVAL;
 }
 
+static int tb_drom_copy_nvm(struct tb_switch *sw, u16 *size)
+{
+	u32 drom_offset;
+	int ret;
+
+	if (!sw->dma_port)
+		return -ENODEV;
+
+	ret = tb_sw_read(sw, &drom_offset, TB_CFG_SWITCH,
+			 sw->cap_plug_events + 12, 1);
+	if (ret)
+		return ret;
+
+	if (!drom_offset)
+		return -ENODEV;
+
+	ret = dma_port_flash_read(sw->dma_port, drom_offset + 14, size,
+				  sizeof(*size));
+	if (ret)
+		return ret;
+
+	/* Size includes CRC8 + UID + CRC32 */
+	*size += 1 + 8 + 4;
+	sw->drom = kzalloc(*size, GFP_KERNEL);
+	if (!sw->drom)
+		return -ENOMEM;
+
+	ret = dma_port_flash_read(sw->dma_port, drom_offset, sw->drom, *size);
+	if (ret)
+		goto err_free;
+
+	/*
+	 * Read UID from the minimal DROM because the one in NVM is just
+	 * a placeholder.
+	 */
+	tb_drom_read_uid_only(sw, &sw->uid);
+	return 0;
+
+err_free:
+	kfree(sw->drom);
+	sw->drom = NULL;
+	return ret;
+}
+
 /**
  * tb_drom_read - copy drom to sw->drom and parse it
  */
@@ -415,6 +494,10 @@ int tb_drom_read(struct tb_switch *sw)
 		if (tb_drom_copy_efi(sw, &size) == 0)
 			goto parse;
 
+		/* Non-Apple hardware has the DROM as part of NVM */
+		if (tb_drom_copy_nvm(sw, &size) == 0)
+			goto parse;
+
 		/*
 		 * The root switch contains only a dummy drom (header only,
 		 * no entries). Hardcode the configuration here.
@@ -475,17 +558,19 @@ parse:
 			header->uid_crc8, crc);
 		goto err;
 	}
-	sw->uid = header->uid;
+	if (!sw->uid)
+		sw->uid = header->uid;
+	sw->vendor = header->vendor_id;
+	sw->device = header->model_id;
 
 	crc = tb_crc32(sw->drom + TB_DROM_DATA_START, header->data_len);
 	if (crc != header->data_crc32) {
 		tb_sw_warn(sw,
-			"drom data crc32 mismatch (expected: %#x, got: %#x), aborting\n",
+			"drom data crc32 mismatch (expected: %#x, got: %#x), continuing\n",
 			header->data_crc32, crc);
-		goto err;
 	}
 
-	if (header->device_rom_revision > 1)
+	if (header->device_rom_revision > 2)
 		tb_sw_warn(sw, "drom device_rom_revision %#x unknown\n",
 			header->device_rom_revision);
 
diff --git a/drivers/thunderbolt/icm.c b/drivers/thunderbolt/icm.c
new file mode 100644
index 000000000000..8ee340290219
--- /dev/null
+++ b/drivers/thunderbolt/icm.c
@@ -0,0 +1,1089 @@
+/*
+ * Internal Thunderbolt Connection Manager. This is a firmware running on
+ * the Thunderbolt host controller performing most of the low-level
+ * handling.
+ *
+ * Copyright (C) 2017, Intel Corporation
+ * Authors: Michael Jamet <michael.jamet@intel.com>
+ *          Mika Westerberg <mika.westerberg@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/dmi.h>
+#include <linux/mutex.h>
+#include <linux/pci.h>
+#include <linux/sizes.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+
+#include "ctl.h"
+#include "nhi_regs.h"
+#include "tb.h"
+
+#define PCIE2CIO_CMD			0x30
+#define PCIE2CIO_CMD_TIMEOUT		BIT(31)
+#define PCIE2CIO_CMD_START		BIT(30)
+#define PCIE2CIO_CMD_WRITE		BIT(21)
+#define PCIE2CIO_CMD_CS_MASK		GENMASK(20, 19)
+#define PCIE2CIO_CMD_CS_SHIFT		19
+#define PCIE2CIO_CMD_PORT_MASK		GENMASK(18, 13)
+#define PCIE2CIO_CMD_PORT_SHIFT		13
+
+#define PCIE2CIO_WRDATA			0x34
+#define PCIE2CIO_RDDATA			0x38
+
+#define PHY_PORT_CS1			0x37
+#define PHY_PORT_CS1_LINK_DISABLE	BIT(14)
+#define PHY_PORT_CS1_LINK_STATE_MASK	GENMASK(29, 26)
+#define PHY_PORT_CS1_LINK_STATE_SHIFT	26
+
+#define ICM_TIMEOUT			5000 /* ms */
+#define ICM_MAX_LINK			4
+#define ICM_MAX_DEPTH			6
+
+/**
+ * struct icm - Internal connection manager private data
+ * @request_lock: Makes sure only one message is send to ICM at time
+ * @rescan_work: Work used to rescan the surviving switches after resume
+ * @upstream_port: Pointer to the PCIe upstream port this host
+ *		   controller is connected. This is only set for systems
+ *		   where ICM needs to be started manually
+ * @vnd_cap: Vendor defined capability where PCIe2CIO mailbox resides
+ *	     (only set when @upstream_port is not %NULL)
+ * @safe_mode: ICM is in safe mode
+ * @is_supported: Checks if we can support ICM on this controller
+ * @get_mode: Read and return the ICM firmware mode (optional)
+ * @get_route: Find a route string for given switch
+ * @device_connected: Handle device connected ICM message
+ * @device_disconnected: Handle device disconnected ICM message
+ */
+struct icm {
+	struct mutex request_lock;
+	struct delayed_work rescan_work;
+	struct pci_dev *upstream_port;
+	int vnd_cap;
+	bool safe_mode;
+	bool (*is_supported)(struct tb *tb);
+	int (*get_mode)(struct tb *tb);
+	int (*get_route)(struct tb *tb, u8 link, u8 depth, u64 *route);
+	void (*device_connected)(struct tb *tb,
+				 const struct icm_pkg_header *hdr);
+	void (*device_disconnected)(struct tb *tb,
+				    const struct icm_pkg_header *hdr);
+};
+
+struct icm_notification {
+	struct work_struct work;
+	struct icm_pkg_header *pkg;
+	struct tb *tb;
+};
+
+static inline struct tb *icm_to_tb(struct icm *icm)
+{
+	return ((void *)icm - sizeof(struct tb));
+}
+
+static inline u8 phy_port_from_route(u64 route, u8 depth)
+{
+	return tb_switch_phy_port_from_link(route >> ((depth - 1) * 8));
+}
+
+static inline u8 dual_link_from_link(u8 link)
+{
+	return link ? ((link - 1) ^ 0x01) + 1 : 0;
+}
+
+static inline u64 get_route(u32 route_hi, u32 route_lo)
+{
+	return (u64)route_hi << 32 | route_lo;
+}
+
+static inline bool is_apple(void)
+{
+	return dmi_match(DMI_BOARD_VENDOR, "Apple Inc.");
+}
+
+static bool icm_match(const struct tb_cfg_request *req,
+		      const struct ctl_pkg *pkg)
+{
+	const struct icm_pkg_header *res_hdr = pkg->buffer;
+	const struct icm_pkg_header *req_hdr = req->request;
+
+	if (pkg->frame.eof != req->response_type)
+		return false;
+	if (res_hdr->code != req_hdr->code)
+		return false;
+
+	return true;
+}
+
+static bool icm_copy(struct tb_cfg_request *req, const struct ctl_pkg *pkg)
+{
+	const struct icm_pkg_header *hdr = pkg->buffer;
+
+	if (hdr->packet_id < req->npackets) {
+		size_t offset = hdr->packet_id * req->response_size;
+
+		memcpy(req->response + offset, pkg->buffer, req->response_size);
+	}
+
+	return hdr->packet_id == hdr->total_packets - 1;
+}
+
+static int icm_request(struct tb *tb, const void *request, size_t request_size,
+		       void *response, size_t response_size, size_t npackets,
+		       unsigned int timeout_msec)
+{
+	struct icm *icm = tb_priv(tb);
+	int retries = 3;
+
+	do {
+		struct tb_cfg_request *req;
+		struct tb_cfg_result res;
+
+		req = tb_cfg_request_alloc();
+		if (!req)
+			return -ENOMEM;
+
+		req->match = icm_match;
+		req->copy = icm_copy;
+		req->request = request;
+		req->request_size = request_size;
+		req->request_type = TB_CFG_PKG_ICM_CMD;
+		req->response = response;
+		req->npackets = npackets;
+		req->response_size = response_size;
+		req->response_type = TB_CFG_PKG_ICM_RESP;
+
+		mutex_lock(&icm->request_lock);
+		res = tb_cfg_request_sync(tb->ctl, req, timeout_msec);
+		mutex_unlock(&icm->request_lock);
+
+		tb_cfg_request_put(req);
+
+		if (res.err != -ETIMEDOUT)
+			return res.err == 1 ? -EIO : res.err;
+
+		usleep_range(20, 50);
+	} while (retries--);
+
+	return -ETIMEDOUT;
+}
+
+static bool icm_fr_is_supported(struct tb *tb)
+{
+	return !is_apple();
+}
+
+static inline int icm_fr_get_switch_index(u32 port)
+{
+	int index;
+
+	if ((port & ICM_PORT_TYPE_MASK) != TB_TYPE_PORT)
+		return 0;
+
+	index = port >> ICM_PORT_INDEX_SHIFT;
+	return index != 0xff ? index : 0;
+}
+
+static int icm_fr_get_route(struct tb *tb, u8 link, u8 depth, u64 *route)
+{
+	struct icm_fr_pkg_get_topology_response *switches, *sw;
+	struct icm_fr_pkg_get_topology request = {
+		.hdr = { .code = ICM_GET_TOPOLOGY },
+	};
+	size_t npackets = ICM_GET_TOPOLOGY_PACKETS;
+	int ret, index;
+	u8 i;
+
+	switches = kcalloc(npackets, sizeof(*switches), GFP_KERNEL);
+	if (!switches)
+		return -ENOMEM;
+
+	ret = icm_request(tb, &request, sizeof(request), switches,
+			  sizeof(*switches), npackets, ICM_TIMEOUT);
+	if (ret)
+		goto err_free;
+
+	sw = &switches[0];
+	index = icm_fr_get_switch_index(sw->ports[link]);
+	if (!index) {
+		ret = -ENODEV;
+		goto err_free;
+	}
+
+	sw = &switches[index];
+	for (i = 1; i < depth; i++) {
+		unsigned int j;
+
+		if (!(sw->first_data & ICM_SWITCH_USED)) {
+			ret = -ENODEV;
+			goto err_free;
+		}
+
+		for (j = 0; j < ARRAY_SIZE(sw->ports); j++) {
+			index = icm_fr_get_switch_index(sw->ports[j]);
+			if (index > sw->switch_index) {
+				sw = &switches[index];
+				break;
+			}
+		}
+	}
+
+	*route = get_route(sw->route_hi, sw->route_lo);
+
+err_free:
+	kfree(switches);
+	return ret;
+}
+
+static int icm_fr_approve_switch(struct tb *tb, struct tb_switch *sw)
+{
+	struct icm_fr_pkg_approve_device request;
+	struct icm_fr_pkg_approve_device reply;
+	int ret;
+
+	memset(&request, 0, sizeof(request));
+	memcpy(&request.ep_uuid, sw->uuid, sizeof(request.ep_uuid));
+	request.hdr.code = ICM_APPROVE_DEVICE;
+	request.connection_id = sw->connection_id;
+	request.connection_key = sw->connection_key;
+
+	memset(&reply, 0, sizeof(reply));
+	/* Use larger timeout as establishing tunnels can take some time */
+	ret = icm_request(tb, &request, sizeof(request), &reply, sizeof(reply),
+			  1, 10000);
+	if (ret)
+		return ret;
+
+	if (reply.hdr.flags & ICM_FLAGS_ERROR) {
+		tb_warn(tb, "PCIe tunnel creation failed\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int icm_fr_add_switch_key(struct tb *tb, struct tb_switch *sw)
+{
+	struct icm_fr_pkg_add_device_key request;
+	struct icm_fr_pkg_add_device_key_response reply;
+	int ret;
+
+	memset(&request, 0, sizeof(request));
+	memcpy(&request.ep_uuid, sw->uuid, sizeof(request.ep_uuid));
+	request.hdr.code = ICM_ADD_DEVICE_KEY;
+	request.connection_id = sw->connection_id;
+	request.connection_key = sw->connection_key;
+	memcpy(request.key, sw->key, TB_SWITCH_KEY_SIZE);
+
+	memset(&reply, 0, sizeof(reply));
+	ret = icm_request(tb, &request, sizeof(request), &reply, sizeof(reply),
+			  1, ICM_TIMEOUT);
+	if (ret)
+		return ret;
+
+	if (reply.hdr.flags & ICM_FLAGS_ERROR) {
+		tb_warn(tb, "Adding key to switch failed\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int icm_fr_challenge_switch_key(struct tb *tb, struct tb_switch *sw,
+				       const u8 *challenge, u8 *response)
+{
+	struct icm_fr_pkg_challenge_device request;
+	struct icm_fr_pkg_challenge_device_response reply;
+	int ret;
+
+	memset(&request, 0, sizeof(request));
+	memcpy(&request.ep_uuid, sw->uuid, sizeof(request.ep_uuid));
+	request.hdr.code = ICM_CHALLENGE_DEVICE;
+	request.connection_id = sw->connection_id;
+	request.connection_key = sw->connection_key;
+	memcpy(request.challenge, challenge, TB_SWITCH_KEY_SIZE);
+
+	memset(&reply, 0, sizeof(reply));
+	ret = icm_request(tb, &request, sizeof(request), &reply, sizeof(reply),
+			  1, ICM_TIMEOUT);
+	if (ret)
+		return ret;
+
+	if (reply.hdr.flags & ICM_FLAGS_ERROR)
+		return -EKEYREJECTED;
+	if (reply.hdr.flags & ICM_FLAGS_NO_KEY)
+		return -ENOKEY;
+
+	memcpy(response, reply.response, TB_SWITCH_KEY_SIZE);
+
+	return 0;
+}
+
+static void remove_switch(struct tb_switch *sw)
+{
+	struct tb_switch *parent_sw;
+
+	parent_sw = tb_to_switch(sw->dev.parent);
+	tb_port_at(tb_route(sw), parent_sw)->remote = NULL;
+	tb_switch_remove(sw);
+}
+
+static void
+icm_fr_device_connected(struct tb *tb, const struct icm_pkg_header *hdr)
+{
+	const struct icm_fr_event_device_connected *pkg =
+		(const struct icm_fr_event_device_connected *)hdr;
+	struct tb_switch *sw, *parent_sw;
+	struct icm *icm = tb_priv(tb);
+	bool authorized = false;
+	u8 link, depth;
+	u64 route;
+	int ret;
+
+	link = pkg->link_info & ICM_LINK_INFO_LINK_MASK;
+	depth = (pkg->link_info & ICM_LINK_INFO_DEPTH_MASK) >>
+		ICM_LINK_INFO_DEPTH_SHIFT;
+	authorized = pkg->link_info & ICM_LINK_INFO_APPROVED;
+
+	ret = icm->get_route(tb, link, depth, &route);
+	if (ret) {
+		tb_err(tb, "failed to find route string for switch at %u.%u\n",
+		       link, depth);
+		return;
+	}
+
+	sw = tb_switch_find_by_uuid(tb, &pkg->ep_uuid);
+	if (sw) {
+		u8 phy_port, sw_phy_port;
+
+		parent_sw = tb_to_switch(sw->dev.parent);
+		sw_phy_port = phy_port_from_route(tb_route(sw), sw->depth);
+		phy_port = phy_port_from_route(route, depth);
+
+		/*
+		 * On resume ICM will send us connected events for the
+		 * devices that still are present. However, that
+		 * information might have changed for example by the
+		 * fact that a switch on a dual-link connection might
+		 * have been enumerated using the other link now. Make
+		 * sure our book keeping matches that.
+		 */
+		if (sw->depth == depth && sw_phy_port == phy_port &&
+		    !!sw->authorized == authorized) {
+			tb_port_at(tb_route(sw), parent_sw)->remote = NULL;
+			tb_port_at(route, parent_sw)->remote =
+				   tb_upstream_port(sw);
+			sw->config.route_hi = upper_32_bits(route);
+			sw->config.route_lo = lower_32_bits(route);
+			sw->connection_id = pkg->connection_id;
+			sw->connection_key = pkg->connection_key;
+			sw->link = link;
+			sw->depth = depth;
+			sw->is_unplugged = false;
+			tb_switch_put(sw);
+			return;
+		}
+
+		/*
+		 * User connected the same switch to another physical
+		 * port or to another part of the topology. Remove the
+		 * existing switch now before adding the new one.
+		 */
+		remove_switch(sw);
+		tb_switch_put(sw);
+	}
+
+	/*
+	 * If the switch was not found by UUID, look for a switch on
+	 * same physical port (taking possible link aggregation into
+	 * account) and depth. If we found one it is definitely a stale
+	 * one so remove it first.
+	 */
+	sw = tb_switch_find_by_link_depth(tb, link, depth);
+	if (!sw) {
+		u8 dual_link;
+
+		dual_link = dual_link_from_link(link);
+		if (dual_link)
+			sw = tb_switch_find_by_link_depth(tb, dual_link, depth);
+	}
+	if (sw) {
+		remove_switch(sw);
+		tb_switch_put(sw);
+	}
+
+	parent_sw = tb_switch_find_by_link_depth(tb, link, depth - 1);
+	if (!parent_sw) {
+		tb_err(tb, "failed to find parent switch for %u.%u\n",
+		       link, depth);
+		return;
+	}
+
+	sw = tb_switch_alloc(tb, &parent_sw->dev, route);
+	if (!sw) {
+		tb_switch_put(parent_sw);
+		return;
+	}
+
+	sw->uuid = kmemdup(&pkg->ep_uuid, sizeof(pkg->ep_uuid), GFP_KERNEL);
+	sw->connection_id = pkg->connection_id;
+	sw->connection_key = pkg->connection_key;
+	sw->link = link;
+	sw->depth = depth;
+	sw->authorized = authorized;
+	sw->security_level = (pkg->hdr.flags & ICM_FLAGS_SLEVEL_MASK) >>
+				ICM_FLAGS_SLEVEL_SHIFT;
+
+	/* Link the two switches now */
+	tb_port_at(route, parent_sw)->remote = tb_upstream_port(sw);
+	tb_upstream_port(sw)->remote = tb_port_at(route, parent_sw);
+
+	ret = tb_switch_add(sw);
+	if (ret) {
+		tb_port_at(tb_route(sw), parent_sw)->remote = NULL;
+		tb_switch_put(sw);
+	}
+	tb_switch_put(parent_sw);
+}
+
+static void
+icm_fr_device_disconnected(struct tb *tb, const struct icm_pkg_header *hdr)
+{
+	const struct icm_fr_event_device_disconnected *pkg =
+		(const struct icm_fr_event_device_disconnected *)hdr;
+	struct tb_switch *sw;
+	u8 link, depth;
+
+	link = pkg->link_info & ICM_LINK_INFO_LINK_MASK;
+	depth = (pkg->link_info & ICM_LINK_INFO_DEPTH_MASK) >>
+		ICM_LINK_INFO_DEPTH_SHIFT;
+
+	if (link > ICM_MAX_LINK || depth > ICM_MAX_DEPTH) {
+		tb_warn(tb, "invalid topology %u.%u, ignoring\n", link, depth);
+		return;
+	}
+
+	sw = tb_switch_find_by_link_depth(tb, link, depth);
+	if (!sw) {
+		tb_warn(tb, "no switch exists at %u.%u, ignoring\n", link,
+			depth);
+		return;
+	}
+
+	remove_switch(sw);
+	tb_switch_put(sw);
+}
+
+static struct pci_dev *get_upstream_port(struct pci_dev *pdev)
+{
+	struct pci_dev *parent;
+
+	parent = pci_upstream_bridge(pdev);
+	while (parent) {
+		if (!pci_is_pcie(parent))
+			return NULL;
+		if (pci_pcie_type(parent) == PCI_EXP_TYPE_UPSTREAM)
+			break;
+		parent = pci_upstream_bridge(parent);
+	}
+
+	if (!parent)
+		return NULL;
+
+	switch (parent->device) {
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_2C_BRIDGE:
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_4C_BRIDGE:
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_LP_BRIDGE:
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_4C_BRIDGE:
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_2C_BRIDGE:
+		return parent;
+	}
+
+	return NULL;
+}
+
+static bool icm_ar_is_supported(struct tb *tb)
+{
+	struct pci_dev *upstream_port;
+	struct icm *icm = tb_priv(tb);
+
+	/*
+	 * Starting from Alpine Ridge we can use ICM on Apple machines
+	 * as well. We just need to reset and re-enable it first.
+	 */
+	if (!is_apple())
+		return true;
+
+	/*
+	 * Find the upstream PCIe port in case we need to do reset
+	 * through its vendor specific registers.
+	 */
+	upstream_port = get_upstream_port(tb->nhi->pdev);
+	if (upstream_port) {
+		int cap;
+
+		cap = pci_find_ext_capability(upstream_port,
+					      PCI_EXT_CAP_ID_VNDR);
+		if (cap > 0) {
+			icm->upstream_port = upstream_port;
+			icm->vnd_cap = cap;
+
+			return true;
+		}
+	}
+
+	return false;
+}
+
+static int icm_ar_get_mode(struct tb *tb)
+{
+	struct tb_nhi *nhi = tb->nhi;
+	int retries = 5;
+	u32 val;
+
+	do {
+		val = ioread32(nhi->iobase + REG_FW_STS);
+		if (val & REG_FW_STS_NVM_AUTH_DONE)
+			break;
+		msleep(30);
+	} while (--retries);
+
+	if (!retries) {
+		dev_err(&nhi->pdev->dev, "ICM firmware not authenticated\n");
+		return -ENODEV;
+	}
+
+	return nhi_mailbox_mode(nhi);
+}
+
+static int icm_ar_get_route(struct tb *tb, u8 link, u8 depth, u64 *route)
+{
+	struct icm_ar_pkg_get_route_response reply;
+	struct icm_ar_pkg_get_route request = {
+		.hdr = { .code = ICM_GET_ROUTE },
+		.link_info = depth << ICM_LINK_INFO_DEPTH_SHIFT | link,
+	};
+	int ret;
+
+	memset(&reply, 0, sizeof(reply));
+	ret = icm_request(tb, &request, sizeof(request), &reply, sizeof(reply),
+			  1, ICM_TIMEOUT);
+	if (ret)
+		return ret;
+
+	if (reply.hdr.flags & ICM_FLAGS_ERROR)
+		return -EIO;
+
+	*route = get_route(reply.route_hi, reply.route_lo);
+	return 0;
+}
+
+static void icm_handle_notification(struct work_struct *work)
+{
+	struct icm_notification *n = container_of(work, typeof(*n), work);
+	struct tb *tb = n->tb;
+	struct icm *icm = tb_priv(tb);
+
+	mutex_lock(&tb->lock);
+
+	switch (n->pkg->code) {
+	case ICM_EVENT_DEVICE_CONNECTED:
+		icm->device_connected(tb, n->pkg);
+		break;
+	case ICM_EVENT_DEVICE_DISCONNECTED:
+		icm->device_disconnected(tb, n->pkg);
+		break;
+	}
+
+	mutex_unlock(&tb->lock);
+
+	kfree(n->pkg);
+	kfree(n);
+}
+
+static void icm_handle_event(struct tb *tb, enum tb_cfg_pkg_type type,
+			     const void *buf, size_t size)
+{
+	struct icm_notification *n;
+
+	n = kmalloc(sizeof(*n), GFP_KERNEL);
+	if (!n)
+		return;
+
+	INIT_WORK(&n->work, icm_handle_notification);
+	n->pkg = kmemdup(buf, size, GFP_KERNEL);
+	n->tb = tb;
+
+	queue_work(tb->wq, &n->work);
+}
+
+static int
+__icm_driver_ready(struct tb *tb, enum tb_security_level *security_level)
+{
+	struct icm_pkg_driver_ready_response reply;
+	struct icm_pkg_driver_ready request = {
+		.hdr.code = ICM_DRIVER_READY,
+	};
+	unsigned int retries = 10;
+	int ret;
+
+	memset(&reply, 0, sizeof(reply));
+	ret = icm_request(tb, &request, sizeof(request), &reply, sizeof(reply),
+			  1, ICM_TIMEOUT);
+	if (ret)
+		return ret;
+
+	if (security_level)
+		*security_level = reply.security_level & 0xf;
+
+	/*
+	 * Hold on here until the switch config space is accessible so
+	 * that we can read root switch config successfully.
+	 */
+	do {
+		struct tb_cfg_result res;
+		u32 tmp;
+
+		res = tb_cfg_read_raw(tb->ctl, &tmp, 0, 0, TB_CFG_SWITCH,
+				      0, 1, 100);
+		if (!res.err)
+			return 0;
+
+		msleep(50);
+	} while (--retries);
+
+	return -ETIMEDOUT;
+}
+
+static int pci2cio_wait_completion(struct icm *icm, unsigned long timeout_msec)
+{
+	unsigned long end = jiffies + msecs_to_jiffies(timeout_msec);
+	u32 cmd;
+
+	do {
+		pci_read_config_dword(icm->upstream_port,
+				      icm->vnd_cap + PCIE2CIO_CMD, &cmd);
+		if (!(cmd & PCIE2CIO_CMD_START)) {
+			if (cmd & PCIE2CIO_CMD_TIMEOUT)
+				break;
+			return 0;
+		}
+
+		msleep(50);
+	} while (time_before(jiffies, end));
+
+	return -ETIMEDOUT;
+}
+
+static int pcie2cio_read(struct icm *icm, enum tb_cfg_space cs,
+			 unsigned int port, unsigned int index, u32 *data)
+{
+	struct pci_dev *pdev = icm->upstream_port;
+	int ret, vnd_cap = icm->vnd_cap;
+	u32 cmd;
+
+	cmd = index;
+	cmd |= (port << PCIE2CIO_CMD_PORT_SHIFT) & PCIE2CIO_CMD_PORT_MASK;
+	cmd |= (cs << PCIE2CIO_CMD_CS_SHIFT) & PCIE2CIO_CMD_CS_MASK;
+	cmd |= PCIE2CIO_CMD_START;
+	pci_write_config_dword(pdev, vnd_cap + PCIE2CIO_CMD, cmd);
+
+	ret = pci2cio_wait_completion(icm, 5000);
+	if (ret)
+		return ret;
+
+	pci_read_config_dword(pdev, vnd_cap + PCIE2CIO_RDDATA, data);
+	return 0;
+}
+
+static int pcie2cio_write(struct icm *icm, enum tb_cfg_space cs,
+			  unsigned int port, unsigned int index, u32 data)
+{
+	struct pci_dev *pdev = icm->upstream_port;
+	int vnd_cap = icm->vnd_cap;
+	u32 cmd;
+
+	pci_write_config_dword(pdev, vnd_cap + PCIE2CIO_WRDATA, data);
+
+	cmd = index;
+	cmd |= (port << PCIE2CIO_CMD_PORT_SHIFT) & PCIE2CIO_CMD_PORT_MASK;
+	cmd |= (cs << PCIE2CIO_CMD_CS_SHIFT) & PCIE2CIO_CMD_CS_MASK;
+	cmd |= PCIE2CIO_CMD_WRITE | PCIE2CIO_CMD_START;
+	pci_write_config_dword(pdev, vnd_cap + PCIE2CIO_CMD, cmd);
+
+	return pci2cio_wait_completion(icm, 5000);
+}
+
+static int icm_firmware_reset(struct tb *tb, struct tb_nhi *nhi)
+{
+	struct icm *icm = tb_priv(tb);
+	u32 val;
+
+	/* Put ARC to wait for CIO reset event to happen */
+	val = ioread32(nhi->iobase + REG_FW_STS);
+	val |= REG_FW_STS_CIO_RESET_REQ;
+	iowrite32(val, nhi->iobase + REG_FW_STS);
+
+	/* Re-start ARC */
+	val = ioread32(nhi->iobase + REG_FW_STS);
+	val |= REG_FW_STS_ICM_EN_INVERT;
+	val |= REG_FW_STS_ICM_EN_CPU;
+	iowrite32(val, nhi->iobase + REG_FW_STS);
+
+	/* Trigger CIO reset now */
+	return pcie2cio_write(icm, TB_CFG_SWITCH, 0, 0x50, BIT(9));
+}
+
+static int icm_firmware_start(struct tb *tb, struct tb_nhi *nhi)
+{
+	unsigned int retries = 10;
+	int ret;
+	u32 val;
+
+	/* Check if the ICM firmware is already running */
+	val = ioread32(nhi->iobase + REG_FW_STS);
+	if (val & REG_FW_STS_ICM_EN)
+		return 0;
+
+	dev_info(&nhi->pdev->dev, "starting ICM firmware\n");
+
+	ret = icm_firmware_reset(tb, nhi);
+	if (ret)
+		return ret;
+
+	/* Wait until the ICM firmware tells us it is up and running */
+	do {
+		/* Check that the ICM firmware is running */
+		val = ioread32(nhi->iobase + REG_FW_STS);
+		if (val & REG_FW_STS_NVM_AUTH_DONE)
+			return 0;
+
+		msleep(300);
+	} while (--retries);
+
+	return -ETIMEDOUT;
+}
+
+static int icm_reset_phy_port(struct tb *tb, int phy_port)
+{
+	struct icm *icm = tb_priv(tb);
+	u32 state0, state1;
+	int port0, port1;
+	u32 val0, val1;
+	int ret;
+
+	if (!icm->upstream_port)
+		return 0;
+
+	if (phy_port) {
+		port0 = 3;
+		port1 = 4;
+	} else {
+		port0 = 1;
+		port1 = 2;
+	}
+
+	/*
+	 * Read link status of both null ports belonging to a single
+	 * physical port.
+	 */
+	ret = pcie2cio_read(icm, TB_CFG_PORT, port0, PHY_PORT_CS1, &val0);
+	if (ret)
+		return ret;
+	ret = pcie2cio_read(icm, TB_CFG_PORT, port1, PHY_PORT_CS1, &val1);
+	if (ret)
+		return ret;
+
+	state0 = val0 & PHY_PORT_CS1_LINK_STATE_MASK;
+	state0 >>= PHY_PORT_CS1_LINK_STATE_SHIFT;
+	state1 = val1 & PHY_PORT_CS1_LINK_STATE_MASK;
+	state1 >>= PHY_PORT_CS1_LINK_STATE_SHIFT;
+
+	/* If they are both up we need to reset them now */
+	if (state0 != TB_PORT_UP || state1 != TB_PORT_UP)
+		return 0;
+
+	val0 |= PHY_PORT_CS1_LINK_DISABLE;
+	ret = pcie2cio_write(icm, TB_CFG_PORT, port0, PHY_PORT_CS1, val0);
+	if (ret)
+		return ret;
+
+	val1 |= PHY_PORT_CS1_LINK_DISABLE;
+	ret = pcie2cio_write(icm, TB_CFG_PORT, port1, PHY_PORT_CS1, val1);
+	if (ret)
+		return ret;
+
+	/* Wait a bit and then re-enable both ports */
+	usleep_range(10, 100);
+
+	ret = pcie2cio_read(icm, TB_CFG_PORT, port0, PHY_PORT_CS1, &val0);
+	if (ret)
+		return ret;
+	ret = pcie2cio_read(icm, TB_CFG_PORT, port1, PHY_PORT_CS1, &val1);
+	if (ret)
+		return ret;
+
+	val0 &= ~PHY_PORT_CS1_LINK_DISABLE;
+	ret = pcie2cio_write(icm, TB_CFG_PORT, port0, PHY_PORT_CS1, val0);
+	if (ret)
+		return ret;
+
+	val1 &= ~PHY_PORT_CS1_LINK_DISABLE;
+	return pcie2cio_write(icm, TB_CFG_PORT, port1, PHY_PORT_CS1, val1);
+}
+
+static int icm_firmware_init(struct tb *tb)
+{
+	struct icm *icm = tb_priv(tb);
+	struct tb_nhi *nhi = tb->nhi;
+	int ret;
+
+	ret = icm_firmware_start(tb, nhi);
+	if (ret) {
+		dev_err(&nhi->pdev->dev, "could not start ICM firmware\n");
+		return ret;
+	}
+
+	if (icm->get_mode) {
+		ret = icm->get_mode(tb);
+
+		switch (ret) {
+		case NHI_FW_SAFE_MODE:
+			icm->safe_mode = true;
+			break;
+
+		case NHI_FW_CM_MODE:
+			/* Ask ICM to accept all Thunderbolt devices */
+			nhi_mailbox_cmd(nhi, NHI_MAILBOX_ALLOW_ALL_DEVS, 0);
+			break;
+
+		default:
+			tb_err(tb, "ICM firmware is in wrong mode: %u\n", ret);
+			return -ENODEV;
+		}
+	}
+
+	/*
+	 * Reset both physical ports if there is anything connected to
+	 * them already.
+	 */
+	ret = icm_reset_phy_port(tb, 0);
+	if (ret)
+		dev_warn(&nhi->pdev->dev, "failed to reset links on port0\n");
+	ret = icm_reset_phy_port(tb, 1);
+	if (ret)
+		dev_warn(&nhi->pdev->dev, "failed to reset links on port1\n");
+
+	return 0;
+}
+
+static int icm_driver_ready(struct tb *tb)
+{
+	struct icm *icm = tb_priv(tb);
+	int ret;
+
+	ret = icm_firmware_init(tb);
+	if (ret)
+		return ret;
+
+	if (icm->safe_mode) {
+		tb_info(tb, "Thunderbolt host controller is in safe mode.\n");
+		tb_info(tb, "You need to update NVM firmware of the controller before it can be used.\n");
+		tb_info(tb, "For latest updates check https://thunderbolttechnology.net/updates.\n");
+		return 0;
+	}
+
+	return __icm_driver_ready(tb, &tb->security_level);
+}
+
+static int icm_suspend(struct tb *tb)
+{
+	return nhi_mailbox_cmd(tb->nhi, NHI_MAILBOX_SAVE_DEVS, 0);
+}
+
+/*
+ * Mark all switches (except root switch) below this one unplugged. ICM
+ * firmware will send us an updated list of switches after we have send
+ * it driver ready command. If a switch is not in that list it will be
+ * removed when we perform rescan.
+ */
+static void icm_unplug_children(struct tb_switch *sw)
+{
+	unsigned int i;
+
+	if (tb_route(sw))
+		sw->is_unplugged = true;
+
+	for (i = 1; i <= sw->config.max_port_number; i++) {
+		struct tb_port *port = &sw->ports[i];
+
+		if (tb_is_upstream_port(port))
+			continue;
+		if (!port->remote)
+			continue;
+
+		icm_unplug_children(port->remote->sw);
+	}
+}
+
+static void icm_free_unplugged_children(struct tb_switch *sw)
+{
+	unsigned int i;
+
+	for (i = 1; i <= sw->config.max_port_number; i++) {
+		struct tb_port *port = &sw->ports[i];
+
+		if (tb_is_upstream_port(port))
+			continue;
+		if (!port->remote)
+			continue;
+
+		if (port->remote->sw->is_unplugged) {
+			tb_switch_remove(port->remote->sw);
+			port->remote = NULL;
+		} else {
+			icm_free_unplugged_children(port->remote->sw);
+		}
+	}
+}
+
+static void icm_rescan_work(struct work_struct *work)
+{
+	struct icm *icm = container_of(work, struct icm, rescan_work.work);
+	struct tb *tb = icm_to_tb(icm);
+
+	mutex_lock(&tb->lock);
+	if (tb->root_switch)
+		icm_free_unplugged_children(tb->root_switch);
+	mutex_unlock(&tb->lock);
+}
+
+static void icm_complete(struct tb *tb)
+{
+	struct icm *icm = tb_priv(tb);
+
+	if (tb->nhi->going_away)
+		return;
+
+	icm_unplug_children(tb->root_switch);
+
+	/*
+	 * Now all existing children should be resumed, start events
+	 * from ICM to get updated status.
+	 */
+	__icm_driver_ready(tb, NULL);
+
+	/*
+	 * We do not get notifications of devices that have been
+	 * unplugged during suspend so schedule rescan to clean them up
+	 * if any.
+	 */
+	queue_delayed_work(tb->wq, &icm->rescan_work, msecs_to_jiffies(500));
+}
+
+static int icm_start(struct tb *tb)
+{
+	struct icm *icm = tb_priv(tb);
+	int ret;
+
+	if (icm->safe_mode)
+		tb->root_switch = tb_switch_alloc_safe_mode(tb, &tb->dev, 0);
+	else
+		tb->root_switch = tb_switch_alloc(tb, &tb->dev, 0);
+	if (!tb->root_switch)
+		return -ENODEV;
+
+	/*
+	 * NVM upgrade has not been tested on Apple systems and they
+	 * don't provide images publicly either. To be on the safe side
+	 * prevent root switch NVM upgrade on Macs for now.
+	 */
+	tb->root_switch->no_nvm_upgrade = is_apple();
+
+	ret = tb_switch_add(tb->root_switch);
+	if (ret)
+		tb_switch_put(tb->root_switch);
+
+	return ret;
+}
+
+static void icm_stop(struct tb *tb)
+{
+	struct icm *icm = tb_priv(tb);
+
+	cancel_delayed_work(&icm->rescan_work);
+	tb_switch_remove(tb->root_switch);
+	tb->root_switch = NULL;
+	nhi_mailbox_cmd(tb->nhi, NHI_MAILBOX_DRV_UNLOADS, 0);
+}
+
+static int icm_disconnect_pcie_paths(struct tb *tb)
+{
+	return nhi_mailbox_cmd(tb->nhi, NHI_MAILBOX_DISCONNECT_PCIE_PATHS, 0);
+}
+
+/* Falcon Ridge and Alpine Ridge */
+static const struct tb_cm_ops icm_fr_ops = {
+	.driver_ready = icm_driver_ready,
+	.start = icm_start,
+	.stop = icm_stop,
+	.suspend = icm_suspend,
+	.complete = icm_complete,
+	.handle_event = icm_handle_event,
+	.approve_switch = icm_fr_approve_switch,
+	.add_switch_key = icm_fr_add_switch_key,
+	.challenge_switch_key = icm_fr_challenge_switch_key,
+	.disconnect_pcie_paths = icm_disconnect_pcie_paths,
+};
+
+struct tb *icm_probe(struct tb_nhi *nhi)
+{
+	struct icm *icm;
+	struct tb *tb;
+
+	tb = tb_domain_alloc(nhi, sizeof(struct icm));
+	if (!tb)
+		return NULL;
+
+	icm = tb_priv(tb);
+	INIT_DELAYED_WORK(&icm->rescan_work, icm_rescan_work);
+	mutex_init(&icm->request_lock);
+
+	switch (nhi->pdev->device) {
+	case PCI_DEVICE_ID_INTEL_FALCON_RIDGE_2C_NHI:
+	case PCI_DEVICE_ID_INTEL_FALCON_RIDGE_4C_NHI:
+		icm->is_supported = icm_fr_is_supported;
+		icm->get_route = icm_fr_get_route;
+		icm->device_connected = icm_fr_device_connected;
+		icm->device_disconnected = icm_fr_device_disconnected;
+		tb->cm_ops = &icm_fr_ops;
+		break;
+
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_2C_NHI:
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_4C_NHI:
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_LP_NHI:
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_4C_NHI:
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_2C_NHI:
+		icm->is_supported = icm_ar_is_supported;
+		icm->get_mode = icm_ar_get_mode;
+		icm->get_route = icm_ar_get_route;
+		icm->device_connected = icm_fr_device_connected;
+		icm->device_disconnected = icm_fr_device_disconnected;
+		tb->cm_ops = &icm_fr_ops;
+		break;
+	}
+
+	if (!icm->is_supported || !icm->is_supported(tb)) {
+		dev_dbg(&nhi->pdev->dev, "ICM not supported on this controller\n");
+		tb_domain_put(tb);
+		return NULL;
+	}
+
+	return tb;
+}
diff --git a/drivers/thunderbolt/nhi.c b/drivers/thunderbolt/nhi.c
index a8c20413dbda..05af126a2435 100644
--- a/drivers/thunderbolt/nhi.c
+++ b/drivers/thunderbolt/nhi.c
@@ -13,7 +13,7 @@
 #include <linux/pci.h>
 #include <linux/interrupt.h>
 #include <linux/module.h>
-#include <linux/dmi.h>
+#include <linux/delay.h>
 
 #include "nhi.h"
 #include "nhi_regs.h"
@@ -21,6 +21,14 @@
 
 #define RING_TYPE(ring) ((ring)->is_tx ? "TX ring" : "RX ring")
 
+/*
+ * Minimal number of vectors when we use MSI-X. Two for control channel
+ * Rx/Tx and the rest four are for cross domain DMA paths.
+ */
+#define MSIX_MIN_VECS		6
+#define MSIX_MAX_VECS		16
+
+#define NHI_MAILBOX_TIMEOUT	500 /* ms */
 
 static int ring_interrupt_index(struct tb_ring *ring)
 {
@@ -42,6 +50,37 @@ static void ring_interrupt_active(struct tb_ring *ring, bool active)
 	int bit = ring_interrupt_index(ring) & 31;
 	int mask = 1 << bit;
 	u32 old, new;
+
+	if (ring->irq > 0) {
+		u32 step, shift, ivr, misc;
+		void __iomem *ivr_base;
+		int index;
+
+		if (ring->is_tx)
+			index = ring->hop;
+		else
+			index = ring->hop + ring->nhi->hop_count;
+
+		/*
+		 * Ask the hardware to clear interrupt status bits automatically
+		 * since we already know which interrupt was triggered.
+		 */
+		misc = ioread32(ring->nhi->iobase + REG_DMA_MISC);
+		if (!(misc & REG_DMA_MISC_INT_AUTO_CLEAR)) {
+			misc |= REG_DMA_MISC_INT_AUTO_CLEAR;
+			iowrite32(misc, ring->nhi->iobase + REG_DMA_MISC);
+		}
+
+		ivr_base = ring->nhi->iobase + REG_INT_VEC_ALLOC_BASE;
+		step = index / REG_INT_VEC_ALLOC_REGS * REG_INT_VEC_ALLOC_BITS;
+		shift = index % REG_INT_VEC_ALLOC_REGS * REG_INT_VEC_ALLOC_BITS;
+		ivr = ioread32(ivr_base + step);
+		ivr &= ~(REG_INT_VEC_ALLOC_MASK << shift);
+		if (active)
+			ivr |= ring->vector << shift;
+		iowrite32(ivr, ivr_base + step);
+	}
+
 	old = ioread32(ring->nhi->iobase + reg);
 	if (active)
 		new = old | mask;
@@ -239,8 +278,50 @@ int __ring_enqueue(struct tb_ring *ring, struct ring_frame *frame)
 	return ret;
 }
 
+static irqreturn_t ring_msix(int irq, void *data)
+{
+	struct tb_ring *ring = data;
+
+	schedule_work(&ring->work);
+	return IRQ_HANDLED;
+}
+
+static int ring_request_msix(struct tb_ring *ring, bool no_suspend)
+{
+	struct tb_nhi *nhi = ring->nhi;
+	unsigned long irqflags;
+	int ret;
+
+	if (!nhi->pdev->msix_enabled)
+		return 0;
+
+	ret = ida_simple_get(&nhi->msix_ida, 0, MSIX_MAX_VECS, GFP_KERNEL);
+	if (ret < 0)
+		return ret;
+
+	ring->vector = ret;
+
+	ring->irq = pci_irq_vector(ring->nhi->pdev, ring->vector);
+	if (ring->irq < 0)
+		return ring->irq;
+
+	irqflags = no_suspend ? IRQF_NO_SUSPEND : 0;
+	return request_irq(ring->irq, ring_msix, irqflags, "thunderbolt", ring);
+}
+
+static void ring_release_msix(struct tb_ring *ring)
+{
+	if (ring->irq <= 0)
+		return;
+
+	free_irq(ring->irq, ring);
+	ida_simple_remove(&ring->nhi->msix_ida, ring->vector);
+	ring->vector = 0;
+	ring->irq = 0;
+}
+
 static struct tb_ring *ring_alloc(struct tb_nhi *nhi, u32 hop, int size,
-				  bool transmit)
+				  bool transmit, unsigned int flags)
 {
 	struct tb_ring *ring = NULL;
 	dev_info(&nhi->pdev->dev, "allocating %s ring %d of size %d\n",
@@ -271,9 +352,14 @@ static struct tb_ring *ring_alloc(struct tb_nhi *nhi, u32 hop, int size,
 	ring->hop = hop;
 	ring->is_tx = transmit;
 	ring->size = size;
+	ring->flags = flags;
 	ring->head = 0;
 	ring->tail = 0;
 	ring->running = false;
+
+	if (ring_request_msix(ring, flags & RING_FLAG_NO_SUSPEND))
+		goto err;
+
 	ring->descriptors = dma_alloc_coherent(&ring->nhi->pdev->dev,
 			size * sizeof(*ring->descriptors),
 			&ring->descriptors_dma, GFP_KERNEL | __GFP_ZERO);
@@ -295,14 +381,16 @@ err:
 	return NULL;
 }
 
-struct tb_ring *ring_alloc_tx(struct tb_nhi *nhi, int hop, int size)
+struct tb_ring *ring_alloc_tx(struct tb_nhi *nhi, int hop, int size,
+			      unsigned int flags)
 {
-	return ring_alloc(nhi, hop, size, true);
+	return ring_alloc(nhi, hop, size, true, flags);
 }
 
-struct tb_ring *ring_alloc_rx(struct tb_nhi *nhi, int hop, int size)
+struct tb_ring *ring_alloc_rx(struct tb_nhi *nhi, int hop, int size,
+			      unsigned int flags)
 {
-	return ring_alloc(nhi, hop, size, false);
+	return ring_alloc(nhi, hop, size, false, flags);
 }
 
 /**
@@ -314,6 +402,8 @@ void ring_start(struct tb_ring *ring)
 {
 	mutex_lock(&ring->nhi->lock);
 	mutex_lock(&ring->lock);
+	if (ring->nhi->going_away)
+		goto err;
 	if (ring->running) {
 		dev_WARN(&ring->nhi->pdev->dev, "ring already started\n");
 		goto err;
@@ -360,6 +450,8 @@ void ring_stop(struct tb_ring *ring)
 	mutex_lock(&ring->lock);
 	dev_info(&ring->nhi->pdev->dev, "stopping %s %d\n",
 		 RING_TYPE(ring), ring->hop);
+	if (ring->nhi->going_away)
+		goto err;
 	if (!ring->running) {
 		dev_WARN(&ring->nhi->pdev->dev, "%s %d already stopped\n",
 			 RING_TYPE(ring), ring->hop);
@@ -413,6 +505,8 @@ void ring_free(struct tb_ring *ring)
 			 RING_TYPE(ring), ring->hop);
 	}
 
+	ring_release_msix(ring);
+
 	dma_free_coherent(&ring->nhi->pdev->dev,
 			  ring->size * sizeof(*ring->descriptors),
 			  ring->descriptors, ring->descriptors_dma);
@@ -428,15 +522,70 @@ void ring_free(struct tb_ring *ring)
 
 	mutex_unlock(&ring->nhi->lock);
 	/**
-	 * ring->work can no longer be scheduled (it is scheduled only by
-	 * nhi_interrupt_work and ring_stop). Wait for it to finish before
-	 * freeing the ring.
+	 * ring->work can no longer be scheduled (it is scheduled only
+	 * by nhi_interrupt_work, ring_stop and ring_msix). Wait for it
+	 * to finish before freeing the ring.
 	 */
 	flush_work(&ring->work);
 	mutex_destroy(&ring->lock);
 	kfree(ring);
 }
 
+/**
+ * nhi_mailbox_cmd() - Send a command through NHI mailbox
+ * @nhi: Pointer to the NHI structure
+ * @cmd: Command to send
+ * @data: Data to be send with the command
+ *
+ * Sends mailbox command to the firmware running on NHI. Returns %0 in
+ * case of success and negative errno in case of failure.
+ */
+int nhi_mailbox_cmd(struct tb_nhi *nhi, enum nhi_mailbox_cmd cmd, u32 data)
+{
+	ktime_t timeout;
+	u32 val;
+
+	iowrite32(data, nhi->iobase + REG_INMAIL_DATA);
+
+	val = ioread32(nhi->iobase + REG_INMAIL_CMD);
+	val &= ~(REG_INMAIL_CMD_MASK | REG_INMAIL_ERROR);
+	val |= REG_INMAIL_OP_REQUEST | cmd;
+	iowrite32(val, nhi->iobase + REG_INMAIL_CMD);
+
+	timeout = ktime_add_ms(ktime_get(), NHI_MAILBOX_TIMEOUT);
+	do {
+		val = ioread32(nhi->iobase + REG_INMAIL_CMD);
+		if (!(val & REG_INMAIL_OP_REQUEST))
+			break;
+		usleep_range(10, 20);
+	} while (ktime_before(ktime_get(), timeout));
+
+	if (val & REG_INMAIL_OP_REQUEST)
+		return -ETIMEDOUT;
+	if (val & REG_INMAIL_ERROR)
+		return -EIO;
+
+	return 0;
+}
+
+/**
+ * nhi_mailbox_mode() - Return current firmware operation mode
+ * @nhi: Pointer to the NHI structure
+ *
+ * The function reads current firmware operation mode using NHI mailbox
+ * registers and returns it to the caller.
+ */
+enum nhi_fw_mode nhi_mailbox_mode(struct tb_nhi *nhi)
+{
+	u32 val;
+
+	val = ioread32(nhi->iobase + REG_OUTMAIL_CMD);
+	val &= REG_OUTMAIL_CMD_OPMODE_MASK;
+	val >>= REG_OUTMAIL_CMD_OPMODE_SHIFT;
+
+	return (enum nhi_fw_mode)val;
+}
+
 static void nhi_interrupt_work(struct work_struct *work)
 {
 	struct tb_nhi *nhi = container_of(work, typeof(*nhi), interrupt_work);
@@ -498,16 +647,40 @@ static int nhi_suspend_noirq(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct tb *tb = pci_get_drvdata(pdev);
-	thunderbolt_suspend(tb);
-	return 0;
+
+	return tb_domain_suspend_noirq(tb);
 }
 
 static int nhi_resume_noirq(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct tb *tb = pci_get_drvdata(pdev);
-	thunderbolt_resume(tb);
-	return 0;
+
+	/*
+	 * Check that the device is still there. It may be that the user
+	 * unplugged last device which causes the host controller to go
+	 * away on PCs.
+	 */
+	if (!pci_device_is_present(pdev))
+		tb->nhi->going_away = true;
+
+	return tb_domain_resume_noirq(tb);
+}
+
+static int nhi_suspend(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct tb *tb = pci_get_drvdata(pdev);
+
+	return tb_domain_suspend(tb);
+}
+
+static void nhi_complete(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct tb *tb = pci_get_drvdata(pdev);
+
+	tb_domain_complete(tb);
 }
 
 static void nhi_shutdown(struct tb_nhi *nhi)
@@ -528,9 +701,52 @@ static void nhi_shutdown(struct tb_nhi *nhi)
 	 * We have to release the irq before calling flush_work. Otherwise an
 	 * already executing IRQ handler could call schedule_work again.
 	 */
-	devm_free_irq(&nhi->pdev->dev, nhi->pdev->irq, nhi);
-	flush_work(&nhi->interrupt_work);
+	if (!nhi->pdev->msix_enabled) {
+		devm_free_irq(&nhi->pdev->dev, nhi->pdev->irq, nhi);
+		flush_work(&nhi->interrupt_work);
+	}
 	mutex_destroy(&nhi->lock);
+	ida_destroy(&nhi->msix_ida);
+}
+
+static int nhi_init_msi(struct tb_nhi *nhi)
+{
+	struct pci_dev *pdev = nhi->pdev;
+	int res, irq, nvec;
+
+	/* In case someone left them on. */
+	nhi_disable_interrupts(nhi);
+
+	ida_init(&nhi->msix_ida);
+
+	/*
+	 * The NHI has 16 MSI-X vectors or a single MSI. We first try to
+	 * get all MSI-X vectors and if we succeed, each ring will have
+	 * one MSI-X. If for some reason that does not work out, we
+	 * fallback to a single MSI.
+	 */
+	nvec = pci_alloc_irq_vectors(pdev, MSIX_MIN_VECS, MSIX_MAX_VECS,
+				     PCI_IRQ_MSIX);
+	if (nvec < 0) {
+		nvec = pci_alloc_irq_vectors(pdev, 1, 1, PCI_IRQ_MSI);
+		if (nvec < 0)
+			return nvec;
+
+		INIT_WORK(&nhi->interrupt_work, nhi_interrupt_work);
+
+		irq = pci_irq_vector(nhi->pdev, 0);
+		if (irq < 0)
+			return irq;
+
+		res = devm_request_irq(&pdev->dev, irq, nhi_msi,
+				       IRQF_NO_SUSPEND, "thunderbolt", nhi);
+		if (res) {
+			dev_err(&pdev->dev, "request_irq failed, aborting\n");
+			return res;
+		}
+	}
+
+	return 0;
 }
 
 static int nhi_probe(struct pci_dev *pdev, const struct pci_device_id *id)
@@ -545,12 +761,6 @@ static int nhi_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		return res;
 	}
 
-	res = pci_enable_msi(pdev);
-	if (res) {
-		dev_err(&pdev->dev, "cannot enable MSI, aborting\n");
-		return res;
-	}
-
 	res = pcim_iomap_regions(pdev, 1 << 0, "thunderbolt");
 	if (res) {
 		dev_err(&pdev->dev, "cannot obtain PCI resources, aborting\n");
@@ -568,7 +778,6 @@ static int nhi_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (nhi->hop_count != 12 && nhi->hop_count != 32)
 		dev_warn(&pdev->dev, "unexpected hop count: %d\n",
 			 nhi->hop_count);
-	INIT_WORK(&nhi->interrupt_work, nhi_interrupt_work);
 
 	nhi->tx_rings = devm_kcalloc(&pdev->dev, nhi->hop_count,
 				     sizeof(*nhi->tx_rings), GFP_KERNEL);
@@ -577,12 +786,9 @@ static int nhi_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (!nhi->tx_rings || !nhi->rx_rings)
 		return -ENOMEM;
 
-	nhi_disable_interrupts(nhi); /* In case someone left them on. */
-	res = devm_request_irq(&pdev->dev, pdev->irq, nhi_msi,
-			       IRQF_NO_SUSPEND, /* must work during _noirq */
-			       "thunderbolt", nhi);
+	res = nhi_init_msi(nhi);
 	if (res) {
-		dev_err(&pdev->dev, "request_irq failed, aborting\n");
+		dev_err(&pdev->dev, "cannot enable MSI, aborting\n");
 		return res;
 	}
 
@@ -593,13 +799,24 @@ static int nhi_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	/* magic value - clock related? */
 	iowrite32(3906250 / 10000, nhi->iobase + 0x38c00);
 
-	dev_info(&nhi->pdev->dev, "NHI initialized, starting thunderbolt\n");
-	tb = thunderbolt_alloc_and_start(nhi);
+	tb = icm_probe(nhi);
+	if (!tb)
+		tb = tb_probe(nhi);
 	if (!tb) {
+		dev_err(&nhi->pdev->dev,
+			"failed to determine connection manager, aborting\n");
+		return -ENODEV;
+	}
+
+	dev_info(&nhi->pdev->dev, "NHI initialized, starting thunderbolt\n");
+
+	res = tb_domain_add(tb);
+	if (res) {
 		/*
 		 * At this point the RX/TX rings might already have been
 		 * activated. Do a proper shutdown.
 		 */
+		tb_domain_put(tb);
 		nhi_shutdown(nhi);
 		return -EIO;
 	}
@@ -612,7 +829,8 @@ static void nhi_remove(struct pci_dev *pdev)
 {
 	struct tb *tb = pci_get_drvdata(pdev);
 	struct tb_nhi *nhi = tb->nhi;
-	thunderbolt_shutdown_and_free(tb);
+
+	tb_domain_remove(tb);
 	nhi_shutdown(nhi);
 }
 
@@ -629,6 +847,10 @@ static const struct dev_pm_ops nhi_pm_ops = {
 					    * pci-tunnels stay alive.
 					    */
 	.restore_noirq = nhi_resume_noirq,
+	.suspend = nhi_suspend,
+	.freeze = nhi_suspend,
+	.poweroff = nhi_suspend,
+	.complete = nhi_complete,
 };
 
 static struct pci_device_id nhi_ids[] = {
@@ -660,6 +882,17 @@ static struct pci_device_id nhi_ids[] = {
 		.device = PCI_DEVICE_ID_INTEL_FALCON_RIDGE_4C_NHI,
 		.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID,
 	},
+
+	/* Thunderbolt 3 */
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_2C_NHI) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_4C_NHI) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_USBONLY_NHI) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_LP_NHI) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_LP_USBONLY_NHI) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_2C_NHI) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_4C_NHI) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_USBONLY_NHI) },
+
 	{ 0,}
 };
 
@@ -676,14 +909,21 @@ static struct pci_driver nhi_driver = {
 
 static int __init nhi_init(void)
 {
-	if (!dmi_match(DMI_BOARD_VENDOR, "Apple Inc."))
-		return -ENOSYS;
-	return pci_register_driver(&nhi_driver);
+	int ret;
+
+	ret = tb_domain_init();
+	if (ret)
+		return ret;
+	ret = pci_register_driver(&nhi_driver);
+	if (ret)
+		tb_domain_exit();
+	return ret;
 }
 
 static void __exit nhi_unload(void)
 {
 	pci_unregister_driver(&nhi_driver);
+	tb_domain_exit();
 }
 
 module_init(nhi_init);
diff --git a/drivers/thunderbolt/nhi.h b/drivers/thunderbolt/nhi.h
index 317242939b31..5b5bb2c436be 100644
--- a/drivers/thunderbolt/nhi.h
+++ b/drivers/thunderbolt/nhi.h
@@ -7,45 +7,78 @@
 #ifndef DSL3510_H_
 #define DSL3510_H_
 
+#include <linux/idr.h>
 #include <linux/mutex.h>
 #include <linux/workqueue.h>
 
 /**
  * struct tb_nhi - thunderbolt native host interface
+ * @lock: Must be held during ring creation/destruction. Is acquired by
+ *	  interrupt_work when dispatching interrupts to individual rings.
+ * @pdev: Pointer to the PCI device
+ * @iobase: MMIO space of the NHI
+ * @tx_rings: All Tx rings available on this host controller
+ * @rx_rings: All Rx rings available on this host controller
+ * @msix_ida: Used to allocate MSI-X vectors for rings
+ * @going_away: The host controller device is about to disappear so when
+ *		this flag is set, avoid touching the hardware anymore.
+ * @interrupt_work: Work scheduled to handle ring interrupt when no
+ *		    MSI-X is used.
+ * @hop_count: Number of rings (end point hops) supported by NHI.
  */
 struct tb_nhi {
-	struct mutex lock; /*
-			    * Must be held during ring creation/destruction.
-			    * Is acquired by interrupt_work when dispatching
-			    * interrupts to individual rings.
-			    **/
+	struct mutex lock;
 	struct pci_dev *pdev;
 	void __iomem *iobase;
 	struct tb_ring **tx_rings;
 	struct tb_ring **rx_rings;
+	struct ida msix_ida;
+	bool going_away;
 	struct work_struct interrupt_work;
-	u32 hop_count; /* Number of rings (end point hops) supported by NHI. */
+	u32 hop_count;
 };
 
 /**
  * struct tb_ring - thunderbolt TX or RX ring associated with a NHI
+ * @lock: Lock serializing actions to this ring. Must be acquired after
+ *	  nhi->lock.
+ * @nhi: Pointer to the native host controller interface
+ * @size: Size of the ring
+ * @hop: Hop (DMA channel) associated with this ring
+ * @head: Head of the ring (write next descriptor here)
+ * @tail: Tail of the ring (complete next descriptor here)
+ * @descriptors: Allocated descriptors for this ring
+ * @queue: Queue holding frames to be transferred over this ring
+ * @in_flight: Queue holding frames that are currently in flight
+ * @work: Interrupt work structure
+ * @is_tx: Is the ring Tx or Rx
+ * @running: Is the ring running
+ * @irq: MSI-X irq number if the ring uses MSI-X. %0 otherwise.
+ * @vector: MSI-X vector number the ring uses (only set if @irq is > 0)
+ * @flags: Ring specific flags
  */
 struct tb_ring {
-	struct mutex lock; /* must be acquired after nhi->lock */
+	struct mutex lock;
 	struct tb_nhi *nhi;
 	int size;
 	int hop;
-	int head; /* write next descriptor here */
-	int tail; /* complete next descriptor here */
+	int head;
+	int tail;
 	struct ring_desc *descriptors;
 	dma_addr_t descriptors_dma;
 	struct list_head queue;
 	struct list_head in_flight;
 	struct work_struct work;
-	bool is_tx:1; /* rx otherwise */
+	bool is_tx:1;
 	bool running:1;
+	int irq;
+	u8 vector;
+	unsigned int flags;
 };
 
+/* Leave ring interrupt enabled on suspend */
+#define RING_FLAG_NO_SUSPEND	BIT(0)
+
 struct ring_frame;
 typedef void (*ring_cb)(struct tb_ring*, struct ring_frame*, bool canceled);
 
@@ -64,8 +97,10 @@ struct ring_frame {
 
 #define TB_FRAME_SIZE 0x100    /* minimum size for ring_rx */
 
-struct tb_ring *ring_alloc_tx(struct tb_nhi *nhi, int hop, int size);
-struct tb_ring *ring_alloc_rx(struct tb_nhi *nhi, int hop, int size);
+struct tb_ring *ring_alloc_tx(struct tb_nhi *nhi, int hop, int size,
+			      unsigned int flags);
+struct tb_ring *ring_alloc_rx(struct tb_nhi *nhi, int hop, int size,
+			      unsigned int flags);
 void ring_start(struct tb_ring *ring);
 void ring_stop(struct tb_ring *ring);
 void ring_free(struct tb_ring *ring);
@@ -111,4 +146,38 @@ static inline int ring_tx(struct tb_ring *ring, struct ring_frame *frame)
 	return __ring_enqueue(ring, frame);
 }
 
+enum nhi_fw_mode {
+	NHI_FW_SAFE_MODE,
+	NHI_FW_AUTH_MODE,
+	NHI_FW_EP_MODE,
+	NHI_FW_CM_MODE,
+};
+
+enum nhi_mailbox_cmd {
+	NHI_MAILBOX_SAVE_DEVS = 0x05,
+	NHI_MAILBOX_DISCONNECT_PCIE_PATHS = 0x06,
+	NHI_MAILBOX_DRV_UNLOADS = 0x07,
+	NHI_MAILBOX_ALLOW_ALL_DEVS = 0x23,
+};
+
+int nhi_mailbox_cmd(struct tb_nhi *nhi, enum nhi_mailbox_cmd cmd, u32 data);
+enum nhi_fw_mode nhi_mailbox_mode(struct tb_nhi *nhi);
+
+/*
+ * PCI IDs used in this driver from Win Ridge forward. There is no
+ * need for the PCI quirk anymore as we will use ICM also on Apple
+ * hardware.
+ */
+#define PCI_DEVICE_ID_INTEL_WIN_RIDGE_2C_NHI            0x157d
+#define PCI_DEVICE_ID_INTEL_WIN_RIDGE_2C_BRIDGE         0x157e
+#define PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_LP_NHI		0x15bf
+#define PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_LP_BRIDGE	0x15c0
+#define PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_4C_NHI	0x15d2
+#define PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_4C_BRIDGE	0x15d3
+#define PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_2C_NHI	0x15d9
+#define PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_2C_BRIDGE	0x15da
+#define PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_LP_USBONLY_NHI	0x15dc
+#define PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_USBONLY_NHI	0x15dd
+#define PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_USBONLY_NHI	0x15de
+
 #endif
diff --git a/drivers/thunderbolt/nhi_regs.h b/drivers/thunderbolt/nhi_regs.h
index 75cf0691e6c5..09ed574e92ff 100644
--- a/drivers/thunderbolt/nhi_regs.h
+++ b/drivers/thunderbolt/nhi_regs.h
@@ -95,7 +95,34 @@ struct ring_desc {
 #define REG_RING_INTERRUPT_BASE	0x38200
 #define RING_INTERRUPT_REG_COUNT(nhi) ((31 + 2 * nhi->hop_count) / 32)
 
+/* Interrupt Vector Allocation */
+#define REG_INT_VEC_ALLOC_BASE	0x38c40
+#define REG_INT_VEC_ALLOC_BITS	4
+#define REG_INT_VEC_ALLOC_MASK	GENMASK(3, 0)
+#define REG_INT_VEC_ALLOC_REGS	(32 / REG_INT_VEC_ALLOC_BITS)
+
 /* The last 11 bits contain the number of hops supported by the NHI port. */
 #define REG_HOP_COUNT		0x39640
 
+#define REG_DMA_MISC			0x39864
+#define REG_DMA_MISC_INT_AUTO_CLEAR     BIT(2)
+
+#define REG_INMAIL_DATA			0x39900
+
+#define REG_INMAIL_CMD			0x39904
+#define REG_INMAIL_CMD_MASK		GENMASK(7, 0)
+#define REG_INMAIL_ERROR		BIT(30)
+#define REG_INMAIL_OP_REQUEST		BIT(31)
+
+#define REG_OUTMAIL_CMD			0x3990c
+#define REG_OUTMAIL_CMD_OPMODE_SHIFT	8
+#define REG_OUTMAIL_CMD_OPMODE_MASK	GENMASK(11, 8)
+
+#define REG_FW_STS			0x39944
+#define REG_FW_STS_NVM_AUTH_DONE	BIT(31)
+#define REG_FW_STS_CIO_RESET_REQ	BIT(30)
+#define REG_FW_STS_ICM_EN_CPU		BIT(2)
+#define REG_FW_STS_ICM_EN_INVERT	BIT(1)
+#define REG_FW_STS_ICM_EN		BIT(0)
+
 #endif
diff --git a/drivers/thunderbolt/switch.c b/drivers/thunderbolt/switch.c
index c6f30b1695a9..ab3e8f410444 100644
--- a/drivers/thunderbolt/switch.c
+++ b/drivers/thunderbolt/switch.c
@@ -5,10 +5,395 @@
  */
 
 #include <linux/delay.h>
+#include <linux/idr.h>
+#include <linux/nvmem-provider.h>
+#include <linux/sizes.h>
 #include <linux/slab.h>
+#include <linux/vmalloc.h>
 
 #include "tb.h"
 
+/* Switch authorization from userspace is serialized by this lock */
+static DEFINE_MUTEX(switch_lock);
+
+/* Switch NVM support */
+
+#define NVM_DEVID		0x05
+#define NVM_VERSION		0x08
+#define NVM_CSS			0x10
+#define NVM_FLASH_SIZE		0x45
+
+#define NVM_MIN_SIZE		SZ_32K
+#define NVM_MAX_SIZE		SZ_512K
+
+static DEFINE_IDA(nvm_ida);
+
+struct nvm_auth_status {
+	struct list_head list;
+	uuid_be uuid;
+	u32 status;
+};
+
+/*
+ * Hold NVM authentication failure status per switch This information
+ * needs to stay around even when the switch gets power cycled so we
+ * keep it separately.
+ */
+static LIST_HEAD(nvm_auth_status_cache);
+static DEFINE_MUTEX(nvm_auth_status_lock);
+
+static struct nvm_auth_status *__nvm_get_auth_status(const struct tb_switch *sw)
+{
+	struct nvm_auth_status *st;
+
+	list_for_each_entry(st, &nvm_auth_status_cache, list) {
+		if (!uuid_be_cmp(st->uuid, *sw->uuid))
+			return st;
+	}
+
+	return NULL;
+}
+
+static void nvm_get_auth_status(const struct tb_switch *sw, u32 *status)
+{
+	struct nvm_auth_status *st;
+
+	mutex_lock(&nvm_auth_status_lock);
+	st = __nvm_get_auth_status(sw);
+	mutex_unlock(&nvm_auth_status_lock);
+
+	*status = st ? st->status : 0;
+}
+
+static void nvm_set_auth_status(const struct tb_switch *sw, u32 status)
+{
+	struct nvm_auth_status *st;
+
+	if (WARN_ON(!sw->uuid))
+		return;
+
+	mutex_lock(&nvm_auth_status_lock);
+	st = __nvm_get_auth_status(sw);
+
+	if (!st) {
+		st = kzalloc(sizeof(*st), GFP_KERNEL);
+		if (!st)
+			goto unlock;
+
+		memcpy(&st->uuid, sw->uuid, sizeof(st->uuid));
+		INIT_LIST_HEAD(&st->list);
+		list_add_tail(&st->list, &nvm_auth_status_cache);
+	}
+
+	st->status = status;
+unlock:
+	mutex_unlock(&nvm_auth_status_lock);
+}
+
+static void nvm_clear_auth_status(const struct tb_switch *sw)
+{
+	struct nvm_auth_status *st;
+
+	mutex_lock(&nvm_auth_status_lock);
+	st = __nvm_get_auth_status(sw);
+	if (st) {
+		list_del(&st->list);
+		kfree(st);
+	}
+	mutex_unlock(&nvm_auth_status_lock);
+}
+
+static int nvm_validate_and_write(struct tb_switch *sw)
+{
+	unsigned int image_size, hdr_size;
+	const u8 *buf = sw->nvm->buf;
+	u16 ds_size;
+	int ret;
+
+	if (!buf)
+		return -EINVAL;
+
+	image_size = sw->nvm->buf_data_size;
+	if (image_size < NVM_MIN_SIZE || image_size > NVM_MAX_SIZE)
+		return -EINVAL;
+
+	/*
+	 * FARB pointer must point inside the image and must at least
+	 * contain parts of the digital section we will be reading here.
+	 */
+	hdr_size = (*(u32 *)buf) & 0xffffff;
+	if (hdr_size + NVM_DEVID + 2 >= image_size)
+		return -EINVAL;
+
+	/* Digital section start should be aligned to 4k page */
+	if (!IS_ALIGNED(hdr_size, SZ_4K))
+		return -EINVAL;
+
+	/*
+	 * Read digital section size and check that it also fits inside
+	 * the image.
+	 */
+	ds_size = *(u16 *)(buf + hdr_size);
+	if (ds_size >= image_size)
+		return -EINVAL;
+
+	if (!sw->safe_mode) {
+		u16 device_id;
+
+		/*
+		 * Make sure the device ID in the image matches the one
+		 * we read from the switch config space.
+		 */
+		device_id = *(u16 *)(buf + hdr_size + NVM_DEVID);
+		if (device_id != sw->config.device_id)
+			return -EINVAL;
+
+		if (sw->generation < 3) {
+			/* Write CSS headers first */
+			ret = dma_port_flash_write(sw->dma_port,
+				DMA_PORT_CSS_ADDRESS, buf + NVM_CSS,
+				DMA_PORT_CSS_MAX_SIZE);
+			if (ret)
+				return ret;
+		}
+
+		/* Skip headers in the image */
+		buf += hdr_size;
+		image_size -= hdr_size;
+	}
+
+	return dma_port_flash_write(sw->dma_port, 0, buf, image_size);
+}
+
+static int nvm_authenticate_host(struct tb_switch *sw)
+{
+	int ret;
+
+	/*
+	 * Root switch NVM upgrade requires that we disconnect the
+	 * existing PCIe paths first (in case it is not in safe mode
+	 * already).
+	 */
+	if (!sw->safe_mode) {
+		ret = tb_domain_disconnect_pcie_paths(sw->tb);
+		if (ret)
+			return ret;
+		/*
+		 * The host controller goes away pretty soon after this if
+		 * everything goes well so getting timeout is expected.
+		 */
+		ret = dma_port_flash_update_auth(sw->dma_port);
+		return ret == -ETIMEDOUT ? 0 : ret;
+	}
+
+	/*
+	 * From safe mode we can get out by just power cycling the
+	 * switch.
+	 */
+	dma_port_power_cycle(sw->dma_port);
+	return 0;
+}
+
+static int nvm_authenticate_device(struct tb_switch *sw)
+{
+	int ret, retries = 10;
+
+	ret = dma_port_flash_update_auth(sw->dma_port);
+	if (ret && ret != -ETIMEDOUT)
+		return ret;
+
+	/*
+	 * Poll here for the authentication status. It takes some time
+	 * for the device to respond (we get timeout for a while). Once
+	 * we get response the device needs to be power cycled in order
+	 * to the new NVM to be taken into use.
+	 */
+	do {
+		u32 status;
+
+		ret = dma_port_flash_update_auth_status(sw->dma_port, &status);
+		if (ret < 0 && ret != -ETIMEDOUT)
+			return ret;
+		if (ret > 0) {
+			if (status) {
+				tb_sw_warn(sw, "failed to authenticate NVM\n");
+				nvm_set_auth_status(sw, status);
+			}
+
+			tb_sw_info(sw, "power cycling the switch now\n");
+			dma_port_power_cycle(sw->dma_port);
+			return 0;
+		}
+
+		msleep(500);
+	} while (--retries);
+
+	return -ETIMEDOUT;
+}
+
+static int tb_switch_nvm_read(void *priv, unsigned int offset, void *val,
+			      size_t bytes)
+{
+	struct tb_switch *sw = priv;
+
+	return dma_port_flash_read(sw->dma_port, offset, val, bytes);
+}
+
+static int tb_switch_nvm_write(void *priv, unsigned int offset, void *val,
+			       size_t bytes)
+{
+	struct tb_switch *sw = priv;
+	int ret = 0;
+
+	if (mutex_lock_interruptible(&switch_lock))
+		return -ERESTARTSYS;
+
+	/*
+	 * Since writing the NVM image might require some special steps,
+	 * for example when CSS headers are written, we cache the image
+	 * locally here and handle the special cases when the user asks
+	 * us to authenticate the image.
+	 */
+	if (!sw->nvm->buf) {
+		sw->nvm->buf = vmalloc(NVM_MAX_SIZE);
+		if (!sw->nvm->buf) {
+			ret = -ENOMEM;
+			goto unlock;
+		}
+	}
+
+	sw->nvm->buf_data_size = offset + bytes;
+	memcpy(sw->nvm->buf + offset, val, bytes);
+
+unlock:
+	mutex_unlock(&switch_lock);
+
+	return ret;
+}
+
+static struct nvmem_device *register_nvmem(struct tb_switch *sw, int id,
+					   size_t size, bool active)
+{
+	struct nvmem_config config;
+
+	memset(&config, 0, sizeof(config));
+
+	if (active) {
+		config.name = "nvm_active";
+		config.reg_read = tb_switch_nvm_read;
+	} else {
+		config.name = "nvm_non_active";
+		config.reg_write = tb_switch_nvm_write;
+	}
+
+	config.id = id;
+	config.stride = 4;
+	config.word_size = 4;
+	config.size = size;
+	config.dev = &sw->dev;
+	config.owner = THIS_MODULE;
+	config.root_only = true;
+	config.priv = sw;
+
+	return nvmem_register(&config);
+}
+
+static int tb_switch_nvm_add(struct tb_switch *sw)
+{
+	struct nvmem_device *nvm_dev;
+	struct tb_switch_nvm *nvm;
+	u32 val;
+	int ret;
+
+	if (!sw->dma_port)
+		return 0;
+
+	nvm = kzalloc(sizeof(*nvm), GFP_KERNEL);
+	if (!nvm)
+		return -ENOMEM;
+
+	nvm->id = ida_simple_get(&nvm_ida, 0, 0, GFP_KERNEL);
+
+	/*
+	 * If the switch is in safe-mode the only accessible portion of
+	 * the NVM is the non-active one where userspace is expected to
+	 * write new functional NVM.
+	 */
+	if (!sw->safe_mode) {
+		u32 nvm_size, hdr_size;
+
+		ret = dma_port_flash_read(sw->dma_port, NVM_FLASH_SIZE, &val,
+					  sizeof(val));
+		if (ret)
+			goto err_ida;
+
+		hdr_size = sw->generation < 3 ? SZ_8K : SZ_16K;
+		nvm_size = (SZ_1M << (val & 7)) / 8;
+		nvm_size = (nvm_size - hdr_size) / 2;
+
+		ret = dma_port_flash_read(sw->dma_port, NVM_VERSION, &val,
+					  sizeof(val));
+		if (ret)
+			goto err_ida;
+
+		nvm->major = val >> 16;
+		nvm->minor = val >> 8;
+
+		nvm_dev = register_nvmem(sw, nvm->id, nvm_size, true);
+		if (IS_ERR(nvm_dev)) {
+			ret = PTR_ERR(nvm_dev);
+			goto err_ida;
+		}
+		nvm->active = nvm_dev;
+	}
+
+	nvm_dev = register_nvmem(sw, nvm->id, NVM_MAX_SIZE, false);
+	if (IS_ERR(nvm_dev)) {
+		ret = PTR_ERR(nvm_dev);
+		goto err_nvm_active;
+	}
+	nvm->non_active = nvm_dev;
+
+	mutex_lock(&switch_lock);
+	sw->nvm = nvm;
+	mutex_unlock(&switch_lock);
+
+	return 0;
+
+err_nvm_active:
+	if (nvm->active)
+		nvmem_unregister(nvm->active);
+err_ida:
+	ida_simple_remove(&nvm_ida, nvm->id);
+	kfree(nvm);
+
+	return ret;
+}
+
+static void tb_switch_nvm_remove(struct tb_switch *sw)
+{
+	struct tb_switch_nvm *nvm;
+
+	mutex_lock(&switch_lock);
+	nvm = sw->nvm;
+	sw->nvm = NULL;
+	mutex_unlock(&switch_lock);
+
+	if (!nvm)
+		return;
+
+	/* Remove authentication status in case the switch is unplugged */
+	if (!nvm->authenticating)
+		nvm_clear_auth_status(sw);
+
+	nvmem_unregister(nvm->non_active);
+	if (nvm->active)
+		nvmem_unregister(nvm->active);
+	ida_simple_remove(&nvm_ida, nvm->id);
+	vfree(nvm->buf);
+	kfree(nvm);
+}
+
 /* port utility functions */
 
 static const char *tb_port_type(struct tb_regs_port_header *port)
@@ -192,7 +577,7 @@ static int tb_init_port(struct tb_port *port)
 
 	/* Port 0 is the switch itself and has no PHY. */
 	if (port->config.type == TB_TYPE_PORT && port->port != 0) {
-		cap = tb_find_cap(port, TB_CFG_PORT, TB_CAP_PHY);
+		cap = tb_port_find_cap(port, TB_PORT_CAP_PHY);
 
 		if (cap > 0)
 			port->cap_phy = cap;
@@ -281,6 +666,9 @@ static int tb_plug_events_active(struct tb_switch *sw, bool active)
 	u32 data;
 	int res;
 
+	if (!sw->config.enabled)
+		return 0;
+
 	sw->config.plug_events_delay = 0xff;
 	res = tb_sw_write(sw, ((u32 *) &sw->config) + 4, TB_CFG_SWITCH, 4, 1);
 	if (res)
@@ -307,36 +695,361 @@ static int tb_plug_events_active(struct tb_switch *sw, bool active)
 			   sw->cap_plug_events + 1, 1);
 }
 
+static ssize_t authorized_show(struct device *dev,
+			       struct device_attribute *attr,
+			       char *buf)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
 
-/**
- * tb_switch_free() - free a tb_switch and all downstream switches
- */
-void tb_switch_free(struct tb_switch *sw)
+	return sprintf(buf, "%u\n", sw->authorized);
+}
+
+static int tb_switch_set_authorized(struct tb_switch *sw, unsigned int val)
 {
-	int i;
-	/* port 0 is the switch itself and never has a remote */
-	for (i = 1; i <= sw->config.max_port_number; i++) {
-		if (tb_is_upstream_port(&sw->ports[i]))
-			continue;
-		if (sw->ports[i].remote)
-			tb_switch_free(sw->ports[i].remote->sw);
-		sw->ports[i].remote = NULL;
+	int ret = -EINVAL;
+
+	if (mutex_lock_interruptible(&switch_lock))
+		return -ERESTARTSYS;
+
+	if (sw->authorized)
+		goto unlock;
+
+	switch (val) {
+	/* Approve switch */
+	case 1:
+		if (sw->key)
+			ret = tb_domain_approve_switch_key(sw->tb, sw);
+		else
+			ret = tb_domain_approve_switch(sw->tb, sw);
+		break;
+
+	/* Challenge switch */
+	case 2:
+		if (sw->key)
+			ret = tb_domain_challenge_switch_key(sw->tb, sw);
+		break;
+
+	default:
+		break;
 	}
 
-	if (!sw->is_unplugged)
-		tb_plug_events_active(sw, false);
+	if (!ret) {
+		sw->authorized = val;
+		/* Notify status change to the userspace */
+		kobject_uevent(&sw->dev.kobj, KOBJ_CHANGE);
+	}
+
+unlock:
+	mutex_unlock(&switch_lock);
+	return ret;
+}
+
+static ssize_t authorized_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t count)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
+	unsigned int val;
+	ssize_t ret;
+
+	ret = kstrtouint(buf, 0, &val);
+	if (ret)
+		return ret;
+	if (val > 2)
+		return -EINVAL;
+
+	ret = tb_switch_set_authorized(sw, val);
+
+	return ret ? ret : count;
+}
+static DEVICE_ATTR_RW(authorized);
+
+static ssize_t device_show(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
+
+	return sprintf(buf, "%#x\n", sw->device);
+}
+static DEVICE_ATTR_RO(device);
+
+static ssize_t
+device_name_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
+
+	return sprintf(buf, "%s\n", sw->device_name ? sw->device_name : "");
+}
+static DEVICE_ATTR_RO(device_name);
+
+static ssize_t key_show(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
+	ssize_t ret;
+
+	if (mutex_lock_interruptible(&switch_lock))
+		return -ERESTARTSYS;
+
+	if (sw->key)
+		ret = sprintf(buf, "%*phN\n", TB_SWITCH_KEY_SIZE, sw->key);
+	else
+		ret = sprintf(buf, "\n");
+
+	mutex_unlock(&switch_lock);
+	return ret;
+}
+
+static ssize_t key_store(struct device *dev, struct device_attribute *attr,
+			 const char *buf, size_t count)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
+	u8 key[TB_SWITCH_KEY_SIZE];
+	ssize_t ret = count;
+
+	if (count < 64)
+		return -EINVAL;
 
+	if (hex2bin(key, buf, sizeof(key)))
+		return -EINVAL;
+
+	if (mutex_lock_interruptible(&switch_lock))
+		return -ERESTARTSYS;
+
+	if (sw->authorized) {
+		ret = -EBUSY;
+	} else {
+		kfree(sw->key);
+		sw->key = kmemdup(key, sizeof(key), GFP_KERNEL);
+		if (!sw->key)
+			ret = -ENOMEM;
+	}
+
+	mutex_unlock(&switch_lock);
+	return ret;
+}
+static DEVICE_ATTR_RW(key);
+
+static ssize_t nvm_authenticate_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
+	u32 status;
+
+	nvm_get_auth_status(sw, &status);
+	return sprintf(buf, "%#x\n", status);
+}
+
+static ssize_t nvm_authenticate_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
+	bool val;
+	int ret;
+
+	if (mutex_lock_interruptible(&switch_lock))
+		return -ERESTARTSYS;
+
+	/* If NVMem devices are not yet added */
+	if (!sw->nvm) {
+		ret = -EAGAIN;
+		goto exit_unlock;
+	}
+
+	ret = kstrtobool(buf, &val);
+	if (ret)
+		goto exit_unlock;
+
+	/* Always clear the authentication status */
+	nvm_clear_auth_status(sw);
+
+	if (val) {
+		ret = nvm_validate_and_write(sw);
+		if (ret)
+			goto exit_unlock;
+
+		sw->nvm->authenticating = true;
+
+		if (!tb_route(sw))
+			ret = nvm_authenticate_host(sw);
+		else
+			ret = nvm_authenticate_device(sw);
+	}
+
+exit_unlock:
+	mutex_unlock(&switch_lock);
+
+	if (ret)
+		return ret;
+	return count;
+}
+static DEVICE_ATTR_RW(nvm_authenticate);
+
+static ssize_t nvm_version_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
+	int ret;
+
+	if (mutex_lock_interruptible(&switch_lock))
+		return -ERESTARTSYS;
+
+	if (sw->safe_mode)
+		ret = -ENODATA;
+	else if (!sw->nvm)
+		ret = -EAGAIN;
+	else
+		ret = sprintf(buf, "%x.%x\n", sw->nvm->major, sw->nvm->minor);
+
+	mutex_unlock(&switch_lock);
+
+	return ret;
+}
+static DEVICE_ATTR_RO(nvm_version);
+
+static ssize_t vendor_show(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
+
+	return sprintf(buf, "%#x\n", sw->vendor);
+}
+static DEVICE_ATTR_RO(vendor);
+
+static ssize_t
+vendor_name_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
+
+	return sprintf(buf, "%s\n", sw->vendor_name ? sw->vendor_name : "");
+}
+static DEVICE_ATTR_RO(vendor_name);
+
+static ssize_t unique_id_show(struct device *dev, struct device_attribute *attr,
+			      char *buf)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
+
+	return sprintf(buf, "%pUb\n", sw->uuid);
+}
+static DEVICE_ATTR_RO(unique_id);
+
+static struct attribute *switch_attrs[] = {
+	&dev_attr_authorized.attr,
+	&dev_attr_device.attr,
+	&dev_attr_device_name.attr,
+	&dev_attr_key.attr,
+	&dev_attr_nvm_authenticate.attr,
+	&dev_attr_nvm_version.attr,
+	&dev_attr_vendor.attr,
+	&dev_attr_vendor_name.attr,
+	&dev_attr_unique_id.attr,
+	NULL,
+};
+
+static umode_t switch_attr_is_visible(struct kobject *kobj,
+				      struct attribute *attr, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct tb_switch *sw = tb_to_switch(dev);
+
+	if (attr == &dev_attr_key.attr) {
+		if (tb_route(sw) &&
+		    sw->tb->security_level == TB_SECURITY_SECURE &&
+		    sw->security_level == TB_SECURITY_SECURE)
+			return attr->mode;
+		return 0;
+	} else if (attr == &dev_attr_nvm_authenticate.attr ||
+		   attr == &dev_attr_nvm_version.attr) {
+		if (sw->dma_port)
+			return attr->mode;
+		return 0;
+	}
+
+	return sw->safe_mode ? 0 : attr->mode;
+}
+
+static struct attribute_group switch_group = {
+	.is_visible = switch_attr_is_visible,
+	.attrs = switch_attrs,
+};
+
+static const struct attribute_group *switch_groups[] = {
+	&switch_group,
+	NULL,
+};
+
+static void tb_switch_release(struct device *dev)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
+
+	dma_port_free(sw->dma_port);
+
+	kfree(sw->uuid);
+	kfree(sw->device_name);
+	kfree(sw->vendor_name);
 	kfree(sw->ports);
 	kfree(sw->drom);
+	kfree(sw->key);
 	kfree(sw);
 }
 
+struct device_type tb_switch_type = {
+	.name = "thunderbolt_device",
+	.release = tb_switch_release,
+};
+
+static int tb_switch_get_generation(struct tb_switch *sw)
+{
+	switch (sw->config.device_id) {
+	case PCI_DEVICE_ID_INTEL_LIGHT_RIDGE:
+	case PCI_DEVICE_ID_INTEL_EAGLE_RIDGE:
+	case PCI_DEVICE_ID_INTEL_LIGHT_PEAK:
+	case PCI_DEVICE_ID_INTEL_CACTUS_RIDGE_2C:
+	case PCI_DEVICE_ID_INTEL_CACTUS_RIDGE_4C:
+	case PCI_DEVICE_ID_INTEL_PORT_RIDGE:
+	case PCI_DEVICE_ID_INTEL_REDWOOD_RIDGE_2C_BRIDGE:
+	case PCI_DEVICE_ID_INTEL_REDWOOD_RIDGE_4C_BRIDGE:
+		return 1;
+
+	case PCI_DEVICE_ID_INTEL_WIN_RIDGE_2C_BRIDGE:
+	case PCI_DEVICE_ID_INTEL_FALCON_RIDGE_2C_BRIDGE:
+	case PCI_DEVICE_ID_INTEL_FALCON_RIDGE_4C_BRIDGE:
+		return 2;
+
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_LP_BRIDGE:
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_2C_BRIDGE:
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_4C_BRIDGE:
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_2C_BRIDGE:
+	case PCI_DEVICE_ID_INTEL_ALPINE_RIDGE_C_4C_BRIDGE:
+		return 3;
+
+	default:
+		/*
+		 * For unknown switches assume generation to be 1 to be
+		 * on the safe side.
+		 */
+		tb_sw_warn(sw, "unsupported switch device id %#x\n",
+			   sw->config.device_id);
+		return 1;
+	}
+}
+
 /**
- * tb_switch_alloc() - allocate and initialize a switch
+ * tb_switch_alloc() - allocate a switch
+ * @tb: Pointer to the owning domain
+ * @parent: Parent device for this switch
+ * @route: Route string for this switch
  *
- * Return: Returns a NULL on failure.
+ * Allocates and initializes a switch. Will not upload configuration to
+ * the switch. For that you need to call tb_switch_configure()
+ * separately. The returned switch should be released by calling
+ * tb_switch_put().
+ *
+ * Return: Pointer to the allocated switch or %NULL in case of failure
  */
-struct tb_switch *tb_switch_alloc(struct tb *tb, u64 route)
+struct tb_switch *tb_switch_alloc(struct tb *tb, struct device *parent,
+				  u64 route)
 {
 	int i;
 	int cap;
@@ -351,11 +1064,9 @@ struct tb_switch *tb_switch_alloc(struct tb *tb, u64 route)
 
 	sw->tb = tb;
 	if (tb_cfg_read(tb->ctl, &sw->config, route, 0, TB_CFG_SWITCH, 0, 5))
-		goto err;
-	tb_info(tb,
-		"initializing Switch at %#llx (depth: %d, up port: %d)\n",
-		route, tb_route_length(route), upstream_port);
-	tb_info(tb, "old switch config:\n");
+		goto err_free_sw_ports;
+
+	tb_info(tb, "current switch config:\n");
 	tb_dump_switch(tb, &sw->config);
 
 	/* configure switch */
@@ -363,30 +1074,13 @@ struct tb_switch *tb_switch_alloc(struct tb *tb, u64 route)
 	sw->config.depth = tb_route_length(route);
 	sw->config.route_lo = route;
 	sw->config.route_hi = route >> 32;
-	sw->config.enabled = 1;
-	/* from here on we may use the tb_sw_* functions & macros */
-
-	if (sw->config.vendor_id != 0x8086)
-		tb_sw_warn(sw, "unknown switch vendor id %#x\n",
-			   sw->config.vendor_id);
-
-	if (sw->config.device_id != PCI_DEVICE_ID_INTEL_LIGHT_RIDGE &&
-	    sw->config.device_id != PCI_DEVICE_ID_INTEL_CACTUS_RIDGE_4C &&
-	    sw->config.device_id != PCI_DEVICE_ID_INTEL_PORT_RIDGE &&
-	    sw->config.device_id != PCI_DEVICE_ID_INTEL_FALCON_RIDGE_2C_BRIDGE &&
-	    sw->config.device_id != PCI_DEVICE_ID_INTEL_FALCON_RIDGE_4C_BRIDGE)
-		tb_sw_warn(sw, "unsupported switch device id %#x\n",
-			   sw->config.device_id);
-
-	/* upload configuration */
-	if (tb_sw_write(sw, 1 + (u32 *) &sw->config, TB_CFG_SWITCH, 1, 3))
-		goto err;
+	sw->config.enabled = 0;
 
 	/* initialize ports */
 	sw->ports = kcalloc(sw->config.max_port_number + 1, sizeof(*sw->ports),
 				GFP_KERNEL);
 	if (!sw->ports)
-		goto err;
+		goto err_free_sw_ports;
 
 	for (i = 0; i <= sw->config.max_port_number; i++) {
 		/* minimum setup for tb_find_cap and tb_drom_read to work */
@@ -394,41 +1088,286 @@ struct tb_switch *tb_switch_alloc(struct tb *tb, u64 route)
 		sw->ports[i].port = i;
 	}
 
-	cap = tb_find_cap(&sw->ports[0], TB_CFG_SWITCH, TB_CAP_PLUG_EVENTS);
+	sw->generation = tb_switch_get_generation(sw);
+
+	cap = tb_switch_find_vse_cap(sw, TB_VSE_CAP_PLUG_EVENTS);
 	if (cap < 0) {
-		tb_sw_warn(sw, "cannot find TB_CAP_PLUG_EVENTS aborting\n");
-		goto err;
+		tb_sw_warn(sw, "cannot find TB_VSE_CAP_PLUG_EVENTS aborting\n");
+		goto err_free_sw_ports;
 	}
 	sw->cap_plug_events = cap;
 
-	/* read drom */
-	if (tb_drom_read(sw))
-		tb_sw_warn(sw, "tb_eeprom_read_rom failed, continuing\n");
-	tb_sw_info(sw, "uid: %#llx\n", sw->uid);
-
-	for (i = 0; i <= sw->config.max_port_number; i++) {
-		if (sw->ports[i].disabled) {
-			tb_port_info(&sw->ports[i], "disabled by eeprom\n");
-			continue;
-		}
-		if (tb_init_port(&sw->ports[i]))
-			goto err;
-	}
-
-	/* TODO: I2C, IECS, link controller */
+	/* Root switch is always authorized */
+	if (!route)
+		sw->authorized = true;
 
-	if (tb_plug_events_active(sw, true))
-		goto err;
+	device_initialize(&sw->dev);
+	sw->dev.parent = parent;
+	sw->dev.bus = &tb_bus_type;
+	sw->dev.type = &tb_switch_type;
+	sw->dev.groups = switch_groups;
+	dev_set_name(&sw->dev, "%u-%llx", tb->index, tb_route(sw));
 
 	return sw;
-err:
+
+err_free_sw_ports:
 	kfree(sw->ports);
-	kfree(sw->drom);
 	kfree(sw);
+
 	return NULL;
 }
 
 /**
+ * tb_switch_alloc_safe_mode() - allocate a switch that is in safe mode
+ * @tb: Pointer to the owning domain
+ * @parent: Parent device for this switch
+ * @route: Route string for this switch
+ *
+ * This creates a switch in safe mode. This means the switch pretty much
+ * lacks all capabilities except DMA configuration port before it is
+ * flashed with a valid NVM firmware.
+ *
+ * The returned switch must be released by calling tb_switch_put().
+ *
+ * Return: Pointer to the allocated switch or %NULL in case of failure
+ */
+struct tb_switch *
+tb_switch_alloc_safe_mode(struct tb *tb, struct device *parent, u64 route)
+{
+	struct tb_switch *sw;
+
+	sw = kzalloc(sizeof(*sw), GFP_KERNEL);
+	if (!sw)
+		return NULL;
+
+	sw->tb = tb;
+	sw->config.depth = tb_route_length(route);
+	sw->config.route_hi = upper_32_bits(route);
+	sw->config.route_lo = lower_32_bits(route);
+	sw->safe_mode = true;
+
+	device_initialize(&sw->dev);
+	sw->dev.parent = parent;
+	sw->dev.bus = &tb_bus_type;
+	sw->dev.type = &tb_switch_type;
+	sw->dev.groups = switch_groups;
+	dev_set_name(&sw->dev, "%u-%llx", tb->index, tb_route(sw));
+
+	return sw;
+}
+
+/**
+ * tb_switch_configure() - Uploads configuration to the switch
+ * @sw: Switch to configure
+ *
+ * Call this function before the switch is added to the system. It will
+ * upload configuration to the switch and makes it available for the
+ * connection manager to use.
+ *
+ * Return: %0 in case of success and negative errno in case of failure
+ */
+int tb_switch_configure(struct tb_switch *sw)
+{
+	struct tb *tb = sw->tb;
+	u64 route;
+	int ret;
+
+	route = tb_route(sw);
+	tb_info(tb,
+		"initializing Switch at %#llx (depth: %d, up port: %d)\n",
+		route, tb_route_length(route), sw->config.upstream_port_number);
+
+	if (sw->config.vendor_id != PCI_VENDOR_ID_INTEL)
+		tb_sw_warn(sw, "unknown switch vendor id %#x\n",
+			   sw->config.vendor_id);
+
+	sw->config.enabled = 1;
+
+	/* upload configuration */
+	ret = tb_sw_write(sw, 1 + (u32 *)&sw->config, TB_CFG_SWITCH, 1, 3);
+	if (ret)
+		return ret;
+
+	return tb_plug_events_active(sw, true);
+}
+
+static void tb_switch_set_uuid(struct tb_switch *sw)
+{
+	u32 uuid[4];
+	int cap;
+
+	if (sw->uuid)
+		return;
+
+	/*
+	 * The newer controllers include fused UUID as part of link
+	 * controller specific registers
+	 */
+	cap = tb_switch_find_vse_cap(sw, TB_VSE_CAP_LINK_CONTROLLER);
+	if (cap > 0) {
+		tb_sw_read(sw, uuid, TB_CFG_SWITCH, cap + 3, 4);
+	} else {
+		/*
+		 * ICM generates UUID based on UID and fills the upper
+		 * two words with ones. This is not strictly following
+		 * UUID format but we want to be compatible with it so
+		 * we do the same here.
+		 */
+		uuid[0] = sw->uid & 0xffffffff;
+		uuid[1] = (sw->uid >> 32) & 0xffffffff;
+		uuid[2] = 0xffffffff;
+		uuid[3] = 0xffffffff;
+	}
+
+	sw->uuid = kmemdup(uuid, sizeof(uuid), GFP_KERNEL);
+}
+
+static int tb_switch_add_dma_port(struct tb_switch *sw)
+{
+	u32 status;
+	int ret;
+
+	switch (sw->generation) {
+	case 3:
+		break;
+
+	case 2:
+		/* Only root switch can be upgraded */
+		if (tb_route(sw))
+			return 0;
+		break;
+
+	default:
+		/*
+		 * DMA port is the only thing available when the switch
+		 * is in safe mode.
+		 */
+		if (!sw->safe_mode)
+			return 0;
+		break;
+	}
+
+	if (sw->no_nvm_upgrade)
+		return 0;
+
+	sw->dma_port = dma_port_alloc(sw);
+	if (!sw->dma_port)
+		return 0;
+
+	/*
+	 * Check status of the previous flash authentication. If there
+	 * is one we need to power cycle the switch in any case to make
+	 * it functional again.
+	 */
+	ret = dma_port_flash_update_auth_status(sw->dma_port, &status);
+	if (ret <= 0)
+		return ret;
+
+	if (status) {
+		tb_sw_info(sw, "switch flash authentication failed\n");
+		tb_switch_set_uuid(sw);
+		nvm_set_auth_status(sw, status);
+	}
+
+	tb_sw_info(sw, "power cycling the switch now\n");
+	dma_port_power_cycle(sw->dma_port);
+
+	/*
+	 * We return error here which causes the switch adding failure.
+	 * It should appear back after power cycle is complete.
+	 */
+	return -ESHUTDOWN;
+}
+
+/**
+ * tb_switch_add() - Add a switch to the domain
+ * @sw: Switch to add
+ *
+ * This is the last step in adding switch to the domain. It will read
+ * identification information from DROM and initializes ports so that
+ * they can be used to connect other switches. The switch will be
+ * exposed to the userspace when this function successfully returns. To
+ * remove and release the switch, call tb_switch_remove().
+ *
+ * Return: %0 in case of success and negative errno in case of failure
+ */
+int tb_switch_add(struct tb_switch *sw)
+{
+	int i, ret;
+
+	/*
+	 * Initialize DMA control port now before we read DROM. Recent
+	 * host controllers have more complete DROM on NVM that includes
+	 * vendor and model identification strings which we then expose
+	 * to the userspace. NVM can be accessed through DMA
+	 * configuration based mailbox.
+	 */
+	ret = tb_switch_add_dma_port(sw);
+	if (ret)
+		return ret;
+
+	if (!sw->safe_mode) {
+		/* read drom */
+		ret = tb_drom_read(sw);
+		if (ret) {
+			tb_sw_warn(sw, "tb_eeprom_read_rom failed\n");
+			return ret;
+		}
+		tb_sw_info(sw, "uid: %#llx\n", sw->uid);
+
+		tb_switch_set_uuid(sw);
+
+		for (i = 0; i <= sw->config.max_port_number; i++) {
+			if (sw->ports[i].disabled) {
+				tb_port_info(&sw->ports[i], "disabled by eeprom\n");
+				continue;
+			}
+			ret = tb_init_port(&sw->ports[i]);
+			if (ret)
+				return ret;
+		}
+	}
+
+	ret = device_add(&sw->dev);
+	if (ret)
+		return ret;
+
+	ret = tb_switch_nvm_add(sw);
+	if (ret)
+		device_del(&sw->dev);
+
+	return ret;
+}
+
+/**
+ * tb_switch_remove() - Remove and release a switch
+ * @sw: Switch to remove
+ *
+ * This will remove the switch from the domain and release it after last
+ * reference count drops to zero. If there are switches connected below
+ * this switch, they will be removed as well.
+ */
+void tb_switch_remove(struct tb_switch *sw)
+{
+	int i;
+
+	/* port 0 is the switch itself and never has a remote */
+	for (i = 1; i <= sw->config.max_port_number; i++) {
+		if (tb_is_upstream_port(&sw->ports[i]))
+			continue;
+		if (sw->ports[i].remote)
+			tb_switch_remove(sw->ports[i].remote->sw);
+		sw->ports[i].remote = NULL;
+	}
+
+	if (!sw->is_unplugged)
+		tb_plug_events_active(sw, false);
+
+	tb_switch_nvm_remove(sw);
+	device_unregister(&sw->dev);
+}
+
+/**
  * tb_sw_set_unplugged() - set is_unplugged on switch and downstream switches
  */
 void tb_sw_set_unplugged(struct tb_switch *sw)
@@ -452,19 +1391,26 @@ void tb_sw_set_unplugged(struct tb_switch *sw)
 int tb_switch_resume(struct tb_switch *sw)
 {
 	int i, err;
-	u64 uid;
 	tb_sw_info(sw, "resuming switch\n");
 
-	err = tb_drom_read_uid_only(sw, &uid);
-	if (err) {
-		tb_sw_warn(sw, "uid read failed\n");
-		return err;
-	}
-	if (sw != sw->tb->root_switch && sw->uid != uid) {
-		tb_sw_info(sw,
-			"changed while suspended (uid %#llx -> %#llx)\n",
-			sw->uid, uid);
-		return -ENODEV;
+	/*
+	 * Check for UID of the connected switches except for root
+	 * switch which we assume cannot be removed.
+	 */
+	if (tb_route(sw)) {
+		u64 uid;
+
+		err = tb_drom_read_uid_only(sw, &uid);
+		if (err) {
+			tb_sw_warn(sw, "uid read failed\n");
+			return err;
+		}
+		if (sw->uid != uid) {
+			tb_sw_info(sw,
+				"changed while suspended (uid %#llx -> %#llx)\n",
+				sw->uid, uid);
+			return -ENODEV;
+		}
 	}
 
 	/* upload configuration */
@@ -509,3 +1455,85 @@ void tb_switch_suspend(struct tb_switch *sw)
 	 * effect?
 	 */
 }
+
+struct tb_sw_lookup {
+	struct tb *tb;
+	u8 link;
+	u8 depth;
+	const uuid_be *uuid;
+};
+
+static int tb_switch_match(struct device *dev, void *data)
+{
+	struct tb_switch *sw = tb_to_switch(dev);
+	struct tb_sw_lookup *lookup = data;
+
+	if (!sw)
+		return 0;
+	if (sw->tb != lookup->tb)
+		return 0;
+
+	if (lookup->uuid)
+		return !memcmp(sw->uuid, lookup->uuid, sizeof(*lookup->uuid));
+
+	/* Root switch is matched only by depth */
+	if (!lookup->depth)
+		return !sw->depth;
+
+	return sw->link == lookup->link && sw->depth == lookup->depth;
+}
+
+/**
+ * tb_switch_find_by_link_depth() - Find switch by link and depth
+ * @tb: Domain the switch belongs
+ * @link: Link number the switch is connected
+ * @depth: Depth of the switch in link
+ *
+ * Returned switch has reference count increased so the caller needs to
+ * call tb_switch_put() when done with the switch.
+ */
+struct tb_switch *tb_switch_find_by_link_depth(struct tb *tb, u8 link, u8 depth)
+{
+	struct tb_sw_lookup lookup;
+	struct device *dev;
+
+	memset(&lookup, 0, sizeof(lookup));
+	lookup.tb = tb;
+	lookup.link = link;
+	lookup.depth = depth;
+
+	dev = bus_find_device(&tb_bus_type, NULL, &lookup, tb_switch_match);
+	if (dev)
+		return tb_to_switch(dev);
+
+	return NULL;
+}
+
+/**
+ * tb_switch_find_by_link_depth() - Find switch by UUID
+ * @tb: Domain the switch belongs
+ * @uuid: UUID to look for
+ *
+ * Returned switch has reference count increased so the caller needs to
+ * call tb_switch_put() when done with the switch.
+ */
+struct tb_switch *tb_switch_find_by_uuid(struct tb *tb, const uuid_be *uuid)
+{
+	struct tb_sw_lookup lookup;
+	struct device *dev;
+
+	memset(&lookup, 0, sizeof(lookup));
+	lookup.tb = tb;
+	lookup.uuid = uuid;
+
+	dev = bus_find_device(&tb_bus_type, NULL, &lookup, tb_switch_match);
+	if (dev)
+		return tb_to_switch(dev);
+
+	return NULL;
+}
+
+void tb_switch_exit(void)
+{
+	ida_destroy(&nvm_ida);
+}
diff --git a/drivers/thunderbolt/tb.c b/drivers/thunderbolt/tb.c
index 24b6d30c3c86..1b02ca0b6129 100644
--- a/drivers/thunderbolt/tb.c
+++ b/drivers/thunderbolt/tb.c
@@ -7,11 +7,24 @@
 #include <linux/slab.h>
 #include <linux/errno.h>
 #include <linux/delay.h>
+#include <linux/dmi.h>
 
 #include "tb.h"
 #include "tb_regs.h"
 #include "tunnel_pci.h"
 
+/**
+ * struct tb_cm - Simple Thunderbolt connection manager
+ * @tunnel_list: List of active tunnels
+ * @hotplug_active: tb_handle_hotplug will stop progressing plug
+ *		    events and exit if this is not set (it needs to
+ *		    acquire the lock one more time). Used to drain wq
+ *		    after cfg has been paused.
+ */
+struct tb_cm {
+	struct list_head tunnel_list;
+	bool hotplug_active;
+};
 
 /* enumeration & hot plug handling */
 
@@ -49,9 +62,23 @@ static void tb_scan_port(struct tb_port *port)
 		tb_port_WARN(port, "port already has a remote!\n");
 		return;
 	}
-	sw = tb_switch_alloc(port->sw->tb, tb_downstream_route(port));
+	sw = tb_switch_alloc(port->sw->tb, &port->sw->dev,
+			     tb_downstream_route(port));
 	if (!sw)
 		return;
+
+	if (tb_switch_configure(sw)) {
+		tb_switch_put(sw);
+		return;
+	}
+
+	sw->authorized = true;
+
+	if (tb_switch_add(sw)) {
+		tb_switch_put(sw);
+		return;
+	}
+
 	port->remote = tb_upstream_port(sw);
 	tb_upstream_port(sw)->remote = port;
 	tb_scan_switch(sw);
@@ -62,12 +89,14 @@ static void tb_scan_port(struct tb_port *port)
  */
 static void tb_free_invalid_tunnels(struct tb *tb)
 {
+	struct tb_cm *tcm = tb_priv(tb);
 	struct tb_pci_tunnel *tunnel;
 	struct tb_pci_tunnel *n;
-	list_for_each_entry_safe(tunnel, n, &tb->tunnel_list, list)
-	{
+
+	list_for_each_entry_safe(tunnel, n, &tcm->tunnel_list, list) {
 		if (tb_pci_is_invalid(tunnel)) {
 			tb_pci_deactivate(tunnel);
+			list_del(&tunnel->list);
 			tb_pci_free(tunnel);
 		}
 	}
@@ -86,7 +115,7 @@ static void tb_free_unplugged_children(struct tb_switch *sw)
 		if (!port->remote)
 			continue;
 		if (port->remote->sw->is_unplugged) {
-			tb_switch_free(port->remote->sw);
+			tb_switch_remove(port->remote->sw);
 			port->remote = NULL;
 		} else {
 			tb_free_unplugged_children(port->remote->sw);
@@ -121,8 +150,8 @@ static struct tb_port *tb_find_unused_down_port(struct tb_switch *sw)
 			continue;
 		if (sw->ports[i].config.type != TB_TYPE_PCIE_DOWN)
 			continue;
-		cap = tb_find_cap(&sw->ports[i], TB_CFG_PORT, TB_CAP_PCIE);
-		if (cap <= 0)
+		cap = tb_port_find_cap(&sw->ports[i], TB_PORT_CAP_ADAP);
+		if (cap < 0)
 			continue;
 		res = tb_port_read(&sw->ports[i], &data, TB_CFG_PORT, cap, 1);
 		if (res < 0)
@@ -149,6 +178,8 @@ static void tb_activate_pcie_devices(struct tb *tb)
 	struct tb_port *up_port;
 	struct tb_port *down_port;
 	struct tb_pci_tunnel *tunnel;
+	struct tb_cm *tcm = tb_priv(tb);
+
 	/* scan for pcie devices at depth 1*/
 	for (i = 1; i <= tb->root_switch->config.max_port_number; i++) {
 		if (tb_is_upstream_port(&tb->root_switch->ports[i]))
@@ -165,8 +196,8 @@ static void tb_activate_pcie_devices(struct tb *tb)
 		}
 
 		/* check whether port is already activated */
-		cap = tb_find_cap(up_port, TB_CFG_PORT, TB_CAP_PCIE);
-		if (cap <= 0)
+		cap = tb_port_find_cap(up_port, TB_PORT_CAP_ADAP);
+		if (cap < 0)
 			continue;
 		if (tb_port_read(up_port, &data, TB_CFG_PORT, cap, 1))
 			continue;
@@ -195,6 +226,7 @@ static void tb_activate_pcie_devices(struct tb *tb)
 			tb_pci_free(tunnel);
 		}
 
+		list_add(&tunnel->list, &tcm->tunnel_list);
 	}
 }
 
@@ -217,10 +249,11 @@ static void tb_handle_hotplug(struct work_struct *work)
 {
 	struct tb_hotplug_event *ev = container_of(work, typeof(*ev), work);
 	struct tb *tb = ev->tb;
+	struct tb_cm *tcm = tb_priv(tb);
 	struct tb_switch *sw;
 	struct tb_port *port;
 	mutex_lock(&tb->lock);
-	if (!tb->hotplug_active)
+	if (!tcm->hotplug_active)
 		goto out; /* during init, suspend or shutdown */
 
 	sw = get_switch_at_route(tb->root_switch, ev->route);
@@ -248,7 +281,7 @@ static void tb_handle_hotplug(struct work_struct *work)
 			tb_port_info(port, "unplugged\n");
 			tb_sw_set_unplugged(port->remote->sw);
 			tb_free_invalid_tunnels(tb);
-			tb_switch_free(port->remote->sw);
+			tb_switch_remove(port->remote->sw);
 			port->remote = NULL;
 		} else {
 			tb_port_info(port,
@@ -281,137 +314,108 @@ out:
  *
  * Delegates to tb_handle_hotplug.
  */
-static void tb_schedule_hotplug_handler(void *data, u64 route, u8 port,
-					bool unplug)
+static void tb_handle_event(struct tb *tb, enum tb_cfg_pkg_type type,
+			    const void *buf, size_t size)
 {
-	struct tb *tb = data;
-	struct tb_hotplug_event *ev = kmalloc(sizeof(*ev), GFP_KERNEL);
+	const struct cfg_event_pkg *pkg = buf;
+	struct tb_hotplug_event *ev;
+	u64 route;
+
+	if (type != TB_CFG_PKG_EVENT) {
+		tb_warn(tb, "unexpected event %#x, ignoring\n", type);
+		return;
+	}
+
+	route = tb_cfg_get_route(&pkg->header);
+
+	if (tb_cfg_error(tb->ctl, route, pkg->port,
+			 TB_CFG_ERROR_ACK_PLUG_EVENT)) {
+		tb_warn(tb, "could not ack plug event on %llx:%x\n", route,
+			pkg->port);
+	}
+
+	ev = kmalloc(sizeof(*ev), GFP_KERNEL);
 	if (!ev)
 		return;
 	INIT_WORK(&ev->work, tb_handle_hotplug);
 	ev->tb = tb;
 	ev->route = route;
-	ev->port = port;
-	ev->unplug = unplug;
+	ev->port = pkg->port;
+	ev->unplug = pkg->unplug;
 	queue_work(tb->wq, &ev->work);
 }
 
-/**
- * thunderbolt_shutdown_and_free() - shutdown everything
- *
- * Free all switches and the config channel.
- *
- * Used in the error path of thunderbolt_alloc_and_start.
- */
-void thunderbolt_shutdown_and_free(struct tb *tb)
+static void tb_stop(struct tb *tb)
 {
+	struct tb_cm *tcm = tb_priv(tb);
 	struct tb_pci_tunnel *tunnel;
 	struct tb_pci_tunnel *n;
 
-	mutex_lock(&tb->lock);
-
 	/* tunnels are only present after everything has been initialized */
-	list_for_each_entry_safe(tunnel, n, &tb->tunnel_list, list) {
+	list_for_each_entry_safe(tunnel, n, &tcm->tunnel_list, list) {
 		tb_pci_deactivate(tunnel);
 		tb_pci_free(tunnel);
 	}
-
-	if (tb->root_switch)
-		tb_switch_free(tb->root_switch);
-	tb->root_switch = NULL;
-
-	if (tb->ctl) {
-		tb_ctl_stop(tb->ctl);
-		tb_ctl_free(tb->ctl);
-	}
-	tb->ctl = NULL;
-	tb->hotplug_active = false; /* signal tb_handle_hotplug to quit */
-
-	/* allow tb_handle_hotplug to acquire the lock */
-	mutex_unlock(&tb->lock);
-	if (tb->wq) {
-		flush_workqueue(tb->wq);
-		destroy_workqueue(tb->wq);
-		tb->wq = NULL;
-	}
-	mutex_destroy(&tb->lock);
-	kfree(tb);
+	tb_switch_remove(tb->root_switch);
+	tcm->hotplug_active = false; /* signal tb_handle_hotplug to quit */
 }
 
-/**
- * thunderbolt_alloc_and_start() - setup the thunderbolt bus
- *
- * Allocates a tb_cfg control channel, initializes the root switch, enables
- * plug events and activates pci devices.
- *
- * Return: Returns NULL on error.
- */
-struct tb *thunderbolt_alloc_and_start(struct tb_nhi *nhi)
+static int tb_start(struct tb *tb)
 {
-	struct tb *tb;
-
-	BUILD_BUG_ON(sizeof(struct tb_regs_switch_header) != 5 * 4);
-	BUILD_BUG_ON(sizeof(struct tb_regs_port_header) != 8 * 4);
-	BUILD_BUG_ON(sizeof(struct tb_regs_hop) != 2 * 4);
+	struct tb_cm *tcm = tb_priv(tb);
+	int ret;
 
-	tb = kzalloc(sizeof(*tb), GFP_KERNEL);
-	if (!tb)
-		return NULL;
-
-	tb->nhi = nhi;
-	mutex_init(&tb->lock);
-	mutex_lock(&tb->lock);
-	INIT_LIST_HEAD(&tb->tunnel_list);
-
-	tb->wq = alloc_ordered_workqueue("thunderbolt", 0);
-	if (!tb->wq)
-		goto err_locked;
+	tb->root_switch = tb_switch_alloc(tb, &tb->dev, 0);
+	if (!tb->root_switch)
+		return -ENOMEM;
 
-	tb->ctl = tb_ctl_alloc(tb->nhi, tb_schedule_hotplug_handler, tb);
-	if (!tb->ctl)
-		goto err_locked;
 	/*
-	 * tb_schedule_hotplug_handler may be called as soon as the config
-	 * channel is started. Thats why we have to hold the lock here.
+	 * ICM firmware upgrade needs running firmware and in native
+	 * mode that is not available so disable firmware upgrade of the
+	 * root switch.
 	 */
-	tb_ctl_start(tb->ctl);
+	tb->root_switch->no_nvm_upgrade = true;
 
-	tb->root_switch = tb_switch_alloc(tb, 0);
-	if (!tb->root_switch)
-		goto err_locked;
+	ret = tb_switch_configure(tb->root_switch);
+	if (ret) {
+		tb_switch_put(tb->root_switch);
+		return ret;
+	}
+
+	/* Announce the switch to the world */
+	ret = tb_switch_add(tb->root_switch);
+	if (ret) {
+		tb_switch_put(tb->root_switch);
+		return ret;
+	}
 
 	/* Full scan to discover devices added before the driver was loaded. */
 	tb_scan_switch(tb->root_switch);
 	tb_activate_pcie_devices(tb);
 
 	/* Allow tb_handle_hotplug to progress events */
-	tb->hotplug_active = true;
-	mutex_unlock(&tb->lock);
-	return tb;
-
-err_locked:
-	mutex_unlock(&tb->lock);
-	thunderbolt_shutdown_and_free(tb);
-	return NULL;
+	tcm->hotplug_active = true;
+	return 0;
 }
 
-void thunderbolt_suspend(struct tb *tb)
+static int tb_suspend_noirq(struct tb *tb)
 {
+	struct tb_cm *tcm = tb_priv(tb);
+
 	tb_info(tb, "suspending...\n");
-	mutex_lock(&tb->lock);
 	tb_switch_suspend(tb->root_switch);
-	tb_ctl_stop(tb->ctl);
-	tb->hotplug_active = false; /* signal tb_handle_hotplug to quit */
-	mutex_unlock(&tb->lock);
+	tcm->hotplug_active = false; /* signal tb_handle_hotplug to quit */
 	tb_info(tb, "suspend finished\n");
+
+	return 0;
 }
 
-void thunderbolt_resume(struct tb *tb)
+static int tb_resume_noirq(struct tb *tb)
 {
+	struct tb_cm *tcm = tb_priv(tb);
 	struct tb_pci_tunnel *tunnel, *n;
+
 	tb_info(tb, "resuming...\n");
-	mutex_lock(&tb->lock);
-	tb_ctl_start(tb->ctl);
 
 	/* remove any pci devices the firmware might have setup */
 	tb_switch_reset(tb, 0);
@@ -419,9 +423,9 @@ void thunderbolt_resume(struct tb *tb)
 	tb_switch_resume(tb->root_switch);
 	tb_free_invalid_tunnels(tb);
 	tb_free_unplugged_children(tb->root_switch);
-	list_for_each_entry_safe(tunnel, n, &tb->tunnel_list, list)
+	list_for_each_entry_safe(tunnel, n, &tcm->tunnel_list, list)
 		tb_pci_restart(tunnel);
-	if (!list_empty(&tb->tunnel_list)) {
+	if (!list_empty(&tcm->tunnel_list)) {
 		/*
 		 * the pcie links need some time to get going.
 		 * 100ms works for me...
@@ -430,7 +434,37 @@ void thunderbolt_resume(struct tb *tb)
 		msleep(100);
 	}
 	 /* Allow tb_handle_hotplug to progress events */
-	tb->hotplug_active = true;
-	mutex_unlock(&tb->lock);
+	tcm->hotplug_active = true;
 	tb_info(tb, "resume finished\n");
+
+	return 0;
+}
+
+static const struct tb_cm_ops tb_cm_ops = {
+	.start = tb_start,
+	.stop = tb_stop,
+	.suspend_noirq = tb_suspend_noirq,
+	.resume_noirq = tb_resume_noirq,
+	.handle_event = tb_handle_event,
+};
+
+struct tb *tb_probe(struct tb_nhi *nhi)
+{
+	struct tb_cm *tcm;
+	struct tb *tb;
+
+	if (!dmi_match(DMI_BOARD_VENDOR, "Apple Inc."))
+		return NULL;
+
+	tb = tb_domain_alloc(nhi, sizeof(*tcm));
+	if (!tb)
+		return NULL;
+
+	tb->security_level = TB_SECURITY_NONE;
+	tb->cm_ops = &tb_cm_ops;
+
+	tcm = tb_priv(tb);
+	INIT_LIST_HEAD(&tcm->tunnel_list);
+
+	return tb;
 }
diff --git a/drivers/thunderbolt/tb.h b/drivers/thunderbolt/tb.h
index 61d57ba64035..3d9f64676e58 100644
--- a/drivers/thunderbolt/tb.h
+++ b/drivers/thunderbolt/tb.h
@@ -7,22 +7,120 @@
 #ifndef TB_H_
 #define TB_H_
 
+#include <linux/nvmem-provider.h>
 #include <linux/pci.h>
+#include <linux/uuid.h>
 
 #include "tb_regs.h"
 #include "ctl.h"
+#include "dma_port.h"
+
+/**
+ * struct tb_switch_nvm - Structure holding switch NVM information
+ * @major: Major version number of the active NVM portion
+ * @minor: Minor version number of the active NVM portion
+ * @id: Identifier used with both NVM portions
+ * @active: Active portion NVMem device
+ * @non_active: Non-active portion NVMem device
+ * @buf: Buffer where the NVM image is stored before it is written to
+ *	 the actual NVM flash device
+ * @buf_data_size: Number of bytes actually consumed by the new NVM
+ *		   image
+ * @authenticating: The switch is authenticating the new NVM
+ */
+struct tb_switch_nvm {
+	u8 major;
+	u8 minor;
+	int id;
+	struct nvmem_device *active;
+	struct nvmem_device *non_active;
+	void *buf;
+	size_t buf_data_size;
+	bool authenticating;
+};
+
+/**
+ * enum tb_security_level - Thunderbolt security level
+ * @TB_SECURITY_NONE: No security, legacy mode
+ * @TB_SECURITY_USER: User approval required at minimum
+ * @TB_SECURITY_SECURE: One time saved key required at minimum
+ * @TB_SECURITY_DPONLY: Only tunnel Display port (and USB)
+ */
+enum tb_security_level {
+	TB_SECURITY_NONE,
+	TB_SECURITY_USER,
+	TB_SECURITY_SECURE,
+	TB_SECURITY_DPONLY,
+};
+
+#define TB_SWITCH_KEY_SIZE		32
+/* Each physical port contains 2 links on modern controllers */
+#define TB_SWITCH_LINKS_PER_PHY_PORT	2
 
 /**
  * struct tb_switch - a thunderbolt switch
+ * @dev: Device for the switch
+ * @config: Switch configuration
+ * @ports: Ports in this switch
+ * @dma_port: If the switch has port supporting DMA configuration based
+ *	      mailbox this will hold the pointer to that (%NULL
+ *	      otherwise). If set it also means the switch has
+ *	      upgradeable NVM.
+ * @tb: Pointer to the domain the switch belongs to
+ * @uid: Unique ID of the switch
+ * @uuid: UUID of the switch (or %NULL if not supported)
+ * @vendor: Vendor ID of the switch
+ * @device: Device ID of the switch
+ * @vendor_name: Name of the vendor (or %NULL if not known)
+ * @device_name: Name of the device (or %NULL if not known)
+ * @generation: Switch Thunderbolt generation
+ * @cap_plug_events: Offset to the plug events capability (%0 if not found)
+ * @is_unplugged: The switch is going away
+ * @drom: DROM of the switch (%NULL if not found)
+ * @nvm: Pointer to the NVM if the switch has one (%NULL otherwise)
+ * @no_nvm_upgrade: Prevent NVM upgrade of this switch
+ * @safe_mode: The switch is in safe-mode
+ * @authorized: Whether the switch is authorized by user or policy
+ * @work: Work used to automatically authorize a switch
+ * @security_level: Switch supported security level
+ * @key: Contains the key used to challenge the device or %NULL if not
+ *	 supported. Size of the key is %TB_SWITCH_KEY_SIZE.
+ * @connection_id: Connection ID used with ICM messaging
+ * @connection_key: Connection key used with ICM messaging
+ * @link: Root switch link this switch is connected (ICM only)
+ * @depth: Depth in the chain this switch is connected (ICM only)
+ *
+ * When the switch is being added or removed to the domain (other
+ * switches) you need to have domain lock held. For switch authorization
+ * internal switch_lock is enough.
  */
 struct tb_switch {
+	struct device dev;
 	struct tb_regs_switch_header config;
 	struct tb_port *ports;
+	struct tb_dma_port *dma_port;
 	struct tb *tb;
 	u64 uid;
-	int cap_plug_events; /* offset, zero if not found */
-	bool is_unplugged; /* unplugged, will go away */
+	uuid_be *uuid;
+	u16 vendor;
+	u16 device;
+	const char *vendor_name;
+	const char *device_name;
+	unsigned int generation;
+	int cap_plug_events;
+	bool is_unplugged;
 	u8 *drom;
+	struct tb_switch_nvm *nvm;
+	bool no_nvm_upgrade;
+	bool safe_mode;
+	unsigned int authorized;
+	struct work_struct work;
+	enum tb_security_level security_level;
+	u8 *key;
+	u8 connection_id;
+	u8 connection_key;
+	u8 link;
+	u8 depth;
 };
 
 /**
@@ -92,29 +190,71 @@ struct tb_path {
 	int path_length; /* number of hops */
 };
 
+/**
+ * struct tb_cm_ops - Connection manager specific operations vector
+ * @driver_ready: Called right after control channel is started. Used by
+ *		  ICM to send driver ready message to the firmware.
+ * @start: Starts the domain
+ * @stop: Stops the domain
+ * @suspend_noirq: Connection manager specific suspend_noirq
+ * @resume_noirq: Connection manager specific resume_noirq
+ * @suspend: Connection manager specific suspend
+ * @complete: Connection manager specific complete
+ * @handle_event: Handle thunderbolt event
+ * @approve_switch: Approve switch
+ * @add_switch_key: Add key to switch
+ * @challenge_switch_key: Challenge switch using key
+ * @disconnect_pcie_paths: Disconnects PCIe paths before NVM update
+ */
+struct tb_cm_ops {
+	int (*driver_ready)(struct tb *tb);
+	int (*start)(struct tb *tb);
+	void (*stop)(struct tb *tb);
+	int (*suspend_noirq)(struct tb *tb);
+	int (*resume_noirq)(struct tb *tb);
+	int (*suspend)(struct tb *tb);
+	void (*complete)(struct tb *tb);
+	void (*handle_event)(struct tb *tb, enum tb_cfg_pkg_type,
+			     const void *buf, size_t size);
+	int (*approve_switch)(struct tb *tb, struct tb_switch *sw);
+	int (*add_switch_key)(struct tb *tb, struct tb_switch *sw);
+	int (*challenge_switch_key)(struct tb *tb, struct tb_switch *sw,
+				    const u8 *challenge, u8 *response);
+	int (*disconnect_pcie_paths)(struct tb *tb);
+};
 
 /**
  * struct tb - main thunderbolt bus structure
+ * @dev: Domain device
+ * @lock: Big lock. Must be held when accessing any struct
+ *	  tb_switch / struct tb_port.
+ * @nhi: Pointer to the NHI structure
+ * @ctl: Control channel for this domain
+ * @wq: Ordered workqueue for all domain specific work
+ * @root_switch: Root switch of this domain
+ * @cm_ops: Connection manager specific operations vector
+ * @index: Linux assigned domain number
+ * @security_level: Current security level
+ * @privdata: Private connection manager specific data
  */
 struct tb {
-	struct mutex lock;	/*
-				 * Big lock. Must be held when accessing cfg or
-				 * any struct tb_switch / struct tb_port.
-				 */
+	struct device dev;
+	struct mutex lock;
 	struct tb_nhi *nhi;
 	struct tb_ctl *ctl;
-	struct workqueue_struct *wq; /* ordered workqueue for plug events */
+	struct workqueue_struct *wq;
 	struct tb_switch *root_switch;
-	struct list_head tunnel_list; /* list of active PCIe tunnels */
-	bool hotplug_active; /*
-			      * tb_handle_hotplug will stop progressing plug
-			      * events and exit if this is not set (it needs to
-			      * acquire the lock one more time). Used to drain
-			      * wq after cfg has been paused.
-			      */
-
+	const struct tb_cm_ops *cm_ops;
+	int index;
+	enum tb_security_level security_level;
+	unsigned long privdata[0];
 };
 
+static inline void *tb_priv(struct tb *tb)
+{
+	return (void *)tb->privdata;
+}
+
 /* helper functions & macros */
 
 /**
@@ -137,6 +277,16 @@ static inline u64 tb_route(struct tb_switch *sw)
 	return ((u64) sw->config.route_hi) << 32 | sw->config.route_lo;
 }
 
+static inline struct tb_port *tb_port_at(u64 route, struct tb_switch *sw)
+{
+	u8 port;
+
+	port = route >> (sw->config.depth * 8);
+	if (WARN_ON(port > sw->config.max_port_number))
+		return NULL;
+	return &sw->ports[port];
+}
+
 static inline int tb_sw_read(struct tb_switch *sw, void *buffer,
 			     enum tb_cfg_space space, u32 offset, u32 length)
 {
@@ -173,7 +323,7 @@ static inline int tb_port_read(struct tb_port *port, void *buffer,
 			   length);
 }
 
-static inline int tb_port_write(struct tb_port *port, void *buffer,
+static inline int tb_port_write(struct tb_port *port, const void *buffer,
 				enum tb_cfg_space space, u32 offset, u32 length)
 {
 	return tb_cfg_write(port->sw->tb->ctl,
@@ -215,25 +365,78 @@ static inline int tb_port_write(struct tb_port *port, void *buffer,
 #define tb_port_info(port, fmt, arg...) \
 	__TB_PORT_PRINT(tb_info, port, fmt, ##arg)
 
+struct tb *icm_probe(struct tb_nhi *nhi);
+struct tb *tb_probe(struct tb_nhi *nhi);
+
+extern struct bus_type tb_bus_type;
+extern struct device_type tb_domain_type;
+extern struct device_type tb_switch_type;
+
+int tb_domain_init(void);
+void tb_domain_exit(void);
+void tb_switch_exit(void);
+
+struct tb *tb_domain_alloc(struct tb_nhi *nhi, size_t privsize);
+int tb_domain_add(struct tb *tb);
+void tb_domain_remove(struct tb *tb);
+int tb_domain_suspend_noirq(struct tb *tb);
+int tb_domain_resume_noirq(struct tb *tb);
+int tb_domain_suspend(struct tb *tb);
+void tb_domain_complete(struct tb *tb);
+int tb_domain_approve_switch(struct tb *tb, struct tb_switch *sw);
+int tb_domain_approve_switch_key(struct tb *tb, struct tb_switch *sw);
+int tb_domain_challenge_switch_key(struct tb *tb, struct tb_switch *sw);
+int tb_domain_disconnect_pcie_paths(struct tb *tb);
+
+static inline void tb_domain_put(struct tb *tb)
+{
+	put_device(&tb->dev);
+}
 
-struct tb *thunderbolt_alloc_and_start(struct tb_nhi *nhi);
-void thunderbolt_shutdown_and_free(struct tb *tb);
-void thunderbolt_suspend(struct tb *tb);
-void thunderbolt_resume(struct tb *tb);
-
-struct tb_switch *tb_switch_alloc(struct tb *tb, u64 route);
-void tb_switch_free(struct tb_switch *sw);
+struct tb_switch *tb_switch_alloc(struct tb *tb, struct device *parent,
+				  u64 route);
+struct tb_switch *tb_switch_alloc_safe_mode(struct tb *tb,
+			struct device *parent, u64 route);
+int tb_switch_configure(struct tb_switch *sw);
+int tb_switch_add(struct tb_switch *sw);
+void tb_switch_remove(struct tb_switch *sw);
 void tb_switch_suspend(struct tb_switch *sw);
 int tb_switch_resume(struct tb_switch *sw);
 int tb_switch_reset(struct tb *tb, u64 route);
 void tb_sw_set_unplugged(struct tb_switch *sw);
 struct tb_switch *get_switch_at_route(struct tb_switch *sw, u64 route);
+struct tb_switch *tb_switch_find_by_link_depth(struct tb *tb, u8 link,
+					       u8 depth);
+struct tb_switch *tb_switch_find_by_uuid(struct tb *tb, const uuid_be *uuid);
+
+static inline unsigned int tb_switch_phy_port_from_link(unsigned int link)
+{
+	return (link - 1) / TB_SWITCH_LINKS_PER_PHY_PORT;
+}
+
+static inline void tb_switch_put(struct tb_switch *sw)
+{
+	put_device(&sw->dev);
+}
+
+static inline bool tb_is_switch(const struct device *dev)
+{
+	return dev->type == &tb_switch_type;
+}
+
+static inline struct tb_switch *tb_to_switch(struct device *dev)
+{
+	if (tb_is_switch(dev))
+		return container_of(dev, struct tb_switch, dev);
+	return NULL;
+}
 
 int tb_wait_for_port(struct tb_port *port, bool wait_if_unplugged);
 int tb_port_add_nfc_credits(struct tb_port *port, int credits);
 int tb_port_clear_counter(struct tb_port *port, int counter);
 
-int tb_find_cap(struct tb_port *port, enum tb_cfg_space space, enum tb_cap cap);
+int tb_switch_find_vse_cap(struct tb_switch *sw, enum tb_switch_vse_cap vsec);
+int tb_port_find_cap(struct tb_port *port, enum tb_port_cap cap);
 
 struct tb_path *tb_path_alloc(struct tb *tb, int num_hops);
 void tb_path_free(struct tb_path *path);
diff --git a/drivers/thunderbolt/tb_msgs.h b/drivers/thunderbolt/tb_msgs.h
new file mode 100644
index 000000000000..85b6d33c0919
--- /dev/null
+++ b/drivers/thunderbolt/tb_msgs.h
@@ -0,0 +1,260 @@
+/*
+ * Thunderbolt control channel messages
+ *
+ * Copyright (C) 2014 Andreas Noever <andreas.noever@gmail.com>
+ * Copyright (C) 2017, Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _TB_MSGS
+#define _TB_MSGS
+
+#include <linux/types.h>
+#include <linux/uuid.h>
+
+enum tb_cfg_pkg_type {
+	TB_CFG_PKG_READ = 1,
+	TB_CFG_PKG_WRITE = 2,
+	TB_CFG_PKG_ERROR = 3,
+	TB_CFG_PKG_NOTIFY_ACK = 4,
+	TB_CFG_PKG_EVENT = 5,
+	TB_CFG_PKG_XDOMAIN_REQ = 6,
+	TB_CFG_PKG_XDOMAIN_RESP = 7,
+	TB_CFG_PKG_OVERRIDE = 8,
+	TB_CFG_PKG_RESET = 9,
+	TB_CFG_PKG_ICM_EVENT = 10,
+	TB_CFG_PKG_ICM_CMD = 11,
+	TB_CFG_PKG_ICM_RESP = 12,
+	TB_CFG_PKG_PREPARE_TO_SLEEP = 0xd,
+
+};
+
+enum tb_cfg_space {
+	TB_CFG_HOPS = 0,
+	TB_CFG_PORT = 1,
+	TB_CFG_SWITCH = 2,
+	TB_CFG_COUNTERS = 3,
+};
+
+enum tb_cfg_error {
+	TB_CFG_ERROR_PORT_NOT_CONNECTED = 0,
+	TB_CFG_ERROR_LINK_ERROR = 1,
+	TB_CFG_ERROR_INVALID_CONFIG_SPACE = 2,
+	TB_CFG_ERROR_NO_SUCH_PORT = 4,
+	TB_CFG_ERROR_ACK_PLUG_EVENT = 7, /* send as reply to TB_CFG_PKG_EVENT */
+	TB_CFG_ERROR_LOOP = 8,
+	TB_CFG_ERROR_HEC_ERROR_DETECTED = 12,
+	TB_CFG_ERROR_FLOW_CONTROL_ERROR = 13,
+};
+
+/* common header */
+struct tb_cfg_header {
+	u32 route_hi:22;
+	u32 unknown:10; /* highest order bit is set on replies */
+	u32 route_lo;
+} __packed;
+
+/* additional header for read/write packets */
+struct tb_cfg_address {
+	u32 offset:13; /* in dwords */
+	u32 length:6; /* in dwords */
+	u32 port:6;
+	enum tb_cfg_space space:2;
+	u32 seq:2; /* sequence number  */
+	u32 zero:3;
+} __packed;
+
+/* TB_CFG_PKG_READ, response for TB_CFG_PKG_WRITE */
+struct cfg_read_pkg {
+	struct tb_cfg_header header;
+	struct tb_cfg_address addr;
+} __packed;
+
+/* TB_CFG_PKG_WRITE, response for TB_CFG_PKG_READ */
+struct cfg_write_pkg {
+	struct tb_cfg_header header;
+	struct tb_cfg_address addr;
+	u32 data[64]; /* maximum size, tb_cfg_address.length has 6 bits */
+} __packed;
+
+/* TB_CFG_PKG_ERROR */
+struct cfg_error_pkg {
+	struct tb_cfg_header header;
+	enum tb_cfg_error error:4;
+	u32 zero1:4;
+	u32 port:6;
+	u32 zero2:2; /* Both should be zero, still they are different fields. */
+	u32 zero3:16;
+} __packed;
+
+/* TB_CFG_PKG_EVENT */
+struct cfg_event_pkg {
+	struct tb_cfg_header header;
+	u32 port:6;
+	u32 zero:25;
+	bool unplug:1;
+} __packed;
+
+/* TB_CFG_PKG_RESET */
+struct cfg_reset_pkg {
+	struct tb_cfg_header header;
+} __packed;
+
+/* TB_CFG_PKG_PREPARE_TO_SLEEP */
+struct cfg_pts_pkg {
+	struct tb_cfg_header header;
+	u32 data;
+} __packed;
+
+/* ICM messages */
+
+enum icm_pkg_code {
+	ICM_GET_TOPOLOGY = 0x1,
+	ICM_DRIVER_READY = 0x3,
+	ICM_APPROVE_DEVICE = 0x4,
+	ICM_CHALLENGE_DEVICE = 0x5,
+	ICM_ADD_DEVICE_KEY = 0x6,
+	ICM_GET_ROUTE = 0xa,
+};
+
+enum icm_event_code {
+	ICM_EVENT_DEVICE_CONNECTED = 3,
+	ICM_EVENT_DEVICE_DISCONNECTED = 4,
+};
+
+struct icm_pkg_header {
+	u8 code;
+	u8 flags;
+	u8 packet_id;
+	u8 total_packets;
+} __packed;
+
+#define ICM_FLAGS_ERROR			BIT(0)
+#define ICM_FLAGS_NO_KEY		BIT(1)
+#define ICM_FLAGS_SLEVEL_SHIFT		3
+#define ICM_FLAGS_SLEVEL_MASK		GENMASK(4, 3)
+
+struct icm_pkg_driver_ready {
+	struct icm_pkg_header hdr;
+} __packed;
+
+struct icm_pkg_driver_ready_response {
+	struct icm_pkg_header hdr;
+	u8 romver;
+	u8 ramver;
+	u16 security_level;
+} __packed;
+
+/* Falcon Ridge & Alpine Ridge common messages */
+
+struct icm_fr_pkg_get_topology {
+	struct icm_pkg_header hdr;
+} __packed;
+
+#define ICM_GET_TOPOLOGY_PACKETS	14
+
+struct icm_fr_pkg_get_topology_response {
+	struct icm_pkg_header hdr;
+	u32 route_lo;
+	u32 route_hi;
+	u8 first_data;
+	u8 second_data;
+	u8 drom_i2c_address_index;
+	u8 switch_index;
+	u32 reserved[2];
+	u32 ports[16];
+	u32 port_hop_info[16];
+} __packed;
+
+#define ICM_SWITCH_USED			BIT(0)
+#define ICM_SWITCH_UPSTREAM_PORT_MASK	GENMASK(7, 1)
+#define ICM_SWITCH_UPSTREAM_PORT_SHIFT	1
+
+#define ICM_PORT_TYPE_MASK		GENMASK(23, 0)
+#define ICM_PORT_INDEX_SHIFT		24
+#define ICM_PORT_INDEX_MASK		GENMASK(31, 24)
+
+struct icm_fr_event_device_connected {
+	struct icm_pkg_header hdr;
+	uuid_be ep_uuid;
+	u8 connection_key;
+	u8 connection_id;
+	u16 link_info;
+	u32 ep_name[55];
+} __packed;
+
+#define ICM_LINK_INFO_LINK_MASK		0x7
+#define ICM_LINK_INFO_DEPTH_SHIFT	4
+#define ICM_LINK_INFO_DEPTH_MASK	GENMASK(7, 4)
+#define ICM_LINK_INFO_APPROVED		BIT(8)
+
+struct icm_fr_pkg_approve_device {
+	struct icm_pkg_header hdr;
+	uuid_be ep_uuid;
+	u8 connection_key;
+	u8 connection_id;
+	u16 reserved;
+} __packed;
+
+struct icm_fr_event_device_disconnected {
+	struct icm_pkg_header hdr;
+	u16 reserved;
+	u16 link_info;
+} __packed;
+
+struct icm_fr_pkg_add_device_key {
+	struct icm_pkg_header hdr;
+	uuid_be ep_uuid;
+	u8 connection_key;
+	u8 connection_id;
+	u16 reserved;
+	u32 key[8];
+} __packed;
+
+struct icm_fr_pkg_add_device_key_response {
+	struct icm_pkg_header hdr;
+	uuid_be ep_uuid;
+	u8 connection_key;
+	u8 connection_id;
+	u16 reserved;
+} __packed;
+
+struct icm_fr_pkg_challenge_device {
+	struct icm_pkg_header hdr;
+	uuid_be ep_uuid;
+	u8 connection_key;
+	u8 connection_id;
+	u16 reserved;
+	u32 challenge[8];
+} __packed;
+
+struct icm_fr_pkg_challenge_device_response {
+	struct icm_pkg_header hdr;
+	uuid_be ep_uuid;
+	u8 connection_key;
+	u8 connection_id;
+	u16 reserved;
+	u32 challenge[8];
+	u32 response[8];
+} __packed;
+
+/* Alpine Ridge only messages */
+
+struct icm_ar_pkg_get_route {
+	struct icm_pkg_header hdr;
+	u16 reserved;
+	u16 link_info;
+} __packed;
+
+struct icm_ar_pkg_get_route_response {
+	struct icm_pkg_header hdr;
+	u16 reserved;
+	u16 link_info;
+	u32 route_hi;
+	u32 route_lo;
+} __packed;
+
+#endif
diff --git a/drivers/thunderbolt/tb_regs.h b/drivers/thunderbolt/tb_regs.h
index 1e2a4a8046be..582bd1f156dc 100644
--- a/drivers/thunderbolt/tb_regs.h
+++ b/drivers/thunderbolt/tb_regs.h
@@ -23,15 +23,22 @@
  */
 #define TB_MAX_CONFIG_RW_LENGTH 60
 
-enum tb_cap {
-	TB_CAP_PHY		= 0x0001,
-	TB_CAP_TIME1		= 0x0003,
-	TB_CAP_PCIE		= 0x0004,
-	TB_CAP_I2C		= 0x0005,
-	TB_CAP_PLUG_EVENTS	= 0x0105, /* also EEPROM */
-	TB_CAP_TIME2		= 0x0305,
-	TB_CAP_IECS		= 0x0405,
-	TB_CAP_LINK_CONTROLLER	= 0x0605, /* also IECS */
+enum tb_switch_cap {
+	TB_SWITCH_CAP_VSE		= 0x05,
+};
+
+enum tb_switch_vse_cap {
+	TB_VSE_CAP_PLUG_EVENTS		= 0x01, /* also EEPROM */
+	TB_VSE_CAP_TIME2		= 0x03,
+	TB_VSE_CAP_IECS			= 0x04,
+	TB_VSE_CAP_LINK_CONTROLLER	= 0x06, /* also IECS */
+};
+
+enum tb_port_cap {
+	TB_PORT_CAP_PHY			= 0x01,
+	TB_PORT_CAP_TIME1		= 0x03,
+	TB_PORT_CAP_ADAP		= 0x04,
+	TB_PORT_CAP_VSE			= 0x05,
 };
 
 enum tb_port_state {
@@ -49,15 +56,34 @@ struct tb_cap_basic {
 	u8 cap; /* if cap == 0x05 then we have a extended capability */
 } __packed;
 
+/**
+ * struct tb_cap_extended_short - Switch extended short capability
+ * @next: Pointer to the next capability. If @next and @length are zero
+ *	  then we have a long cap.
+ * @cap: Base capability ID (see &enum tb_switch_cap)
+ * @vsec_id: Vendor specific capability ID (see &enum switch_vse_cap)
+ * @length: Length of this capability
+ */
 struct tb_cap_extended_short {
-	u8 next; /* if next and length are zero then we have a long cap */
-	enum tb_cap cap:16;
+	u8 next;
+	u8 cap;
+	u8 vsec_id;
 	u8 length;
 } __packed;
 
+/**
+ * struct tb_cap_extended_long - Switch extended long capability
+ * @zero1: This field should be zero
+ * @cap: Base capability ID (see &enum tb_switch_cap)
+ * @vsec_id: Vendor specific capability ID (see &enum switch_vse_cap)
+ * @zero2: This field should be zero
+ * @next: Pointer to the next capability
+ * @length: Length of this capability
+ */
 struct tb_cap_extended_long {
 	u8 zero1;
-	enum tb_cap cap:16;
+	u8 cap;
+	u8 vsec_id;
 	u8 zero2;
 	u16 next;
 	u16 length;
diff --git a/drivers/thunderbolt/tunnel_pci.c b/drivers/thunderbolt/tunnel_pci.c
index baf1cd370446..ca4475907d7a 100644
--- a/drivers/thunderbolt/tunnel_pci.c
+++ b/drivers/thunderbolt/tunnel_pci.c
@@ -147,10 +147,10 @@ bool tb_pci_is_invalid(struct tb_pci_tunnel *tunnel)
 static int tb_pci_port_active(struct tb_port *port, bool active)
 {
 	u32 word = active ? 0x80000000 : 0x0;
-	int cap = tb_find_cap(port, TB_CFG_PORT, TB_CAP_PCIE);
-	if (cap <= 0) {
-		tb_port_warn(port, "TB_CAP_PCIE not found: %d\n", cap);
-		return cap ? cap : -ENXIO;
+	int cap = tb_port_find_cap(port, TB_PORT_CAP_ADAP);
+	if (cap < 0) {
+		tb_port_warn(port, "TB_PORT_CAP_ADAP not found: %d\n", cap);
+		return cap;
 	}
 	return tb_port_write(port, &word, TB_CFG_PORT, cap, 1);
 }
@@ -194,19 +194,13 @@ err:
  */
 int tb_pci_activate(struct tb_pci_tunnel *tunnel)
 {
-	int res;
 	if (tunnel->path_to_up->activated || tunnel->path_to_down->activated) {
 		tb_tunnel_WARN(tunnel,
 			       "trying to activate an already activated tunnel\n");
 		return -EINVAL;
 	}
 
-	res = tb_pci_restart(tunnel);
-	if (res)
-		return res;
-
-	list_add(&tunnel->list, &tunnel->tb->tunnel_list);
-	return 0;
+	return tb_pci_restart(tunnel);
 }
 
 
@@ -227,6 +221,5 @@ void tb_pci_deactivate(struct tb_pci_tunnel *tunnel)
 		tb_path_deactivate(tunnel->path_to_down);
 	if (tunnel->path_to_up->activated)
 		tb_path_deactivate(tunnel->path_to_up);
-	list_del_init(&tunnel->list);
 }
 
diff --git a/drivers/tty/Makefile b/drivers/tty/Makefile
index f02becdb3e33..8689279afdf1 100644
--- a/drivers/tty/Makefile
+++ b/drivers/tty/Makefile
@@ -1,6 +1,7 @@
 obj-$(CONFIG_TTY)		+= tty_io.o n_tty.o tty_ioctl.o tty_ldisc.o \
 				   tty_buffer.o tty_port.o tty_mutex.o \
-				   tty_ldsem.o tty_baudrate.o tty_jobctrl.o
+				   tty_ldsem.o tty_baudrate.o tty_jobctrl.o \
+				   n_null.o
 obj-$(CONFIG_LEGACY_PTYS)	+= pty.o
 obj-$(CONFIG_UNIX98_PTYS)	+= pty.o
 obj-$(CONFIG_AUDIT)		+= tty_audit.o
diff --git a/drivers/tty/amiserial.c b/drivers/tty/amiserial.c
index dea16bb8c46a..9820e20993db 100644
--- a/drivers/tty/amiserial.c
+++ b/drivers/tty/amiserial.c
@@ -570,18 +570,6 @@ static int startup(struct tty_struct *tty, struct serial_state *info)
 	info->xmit.head = info->xmit.tail = 0;
 
 	/*
-	 * Set up the tty->alt_speed kludge
-	 */
-	if ((port->flags & ASYNC_SPD_MASK) == ASYNC_SPD_HI)
-		tty->alt_speed = 57600;
-	if ((port->flags & ASYNC_SPD_MASK) == ASYNC_SPD_VHI)
-		tty->alt_speed = 115200;
-	if ((port->flags & ASYNC_SPD_MASK) == ASYNC_SPD_SHI)
-		tty->alt_speed = 230400;
-	if ((port->flags & ASYNC_SPD_MASK) == ASYNC_SPD_WARP)
-		tty->alt_speed = 460800;
-
-	/*
 	 * and set the speed of the serial port
 	 */
 	change_speed(tty, info, NULL);
@@ -1084,14 +1072,9 @@ static int set_serial_info(struct tty_struct *tty, struct serial_state *state,
 check_and_exit:
 	if (tty_port_initialized(port)) {
 		if (change_spd) {
-			if ((port->flags & ASYNC_SPD_MASK) == ASYNC_SPD_HI)
-				tty->alt_speed = 57600;
-			if ((port->flags & ASYNC_SPD_MASK) == ASYNC_SPD_VHI)
-				tty->alt_speed = 115200;
-			if ((port->flags & ASYNC_SPD_MASK) == ASYNC_SPD_SHI)
-				tty->alt_speed = 230400;
-			if ((port->flags & ASYNC_SPD_MASK) == ASYNC_SPD_WARP)
-				tty->alt_speed = 460800;
+			/* warn about deprecation unless clearing */
+			if (new_serial.flags & ASYNC_SPD_MASK)
+				dev_warn_ratelimited(tty->dev, "use of SPD flags is deprecated\n");
 			change_speed(tty, state, NULL);
 		}
 	} else
diff --git a/drivers/tty/cyclades.c b/drivers/tty/cyclades.c
index 104f09c58163..d272bc4e7fb5 100644
--- a/drivers/tty/cyclades.c
+++ b/drivers/tty/cyclades.c
@@ -1975,18 +1975,6 @@ static void cy_set_line_char(struct cyclades_port *info, struct tty_struct *tty)
 	cflag = tty->termios.c_cflag;
 	iflag = tty->termios.c_iflag;
 
-	/*
-	 * Set up the tty->alt_speed kludge
-	 */
-	if ((info->port.flags & ASYNC_SPD_MASK) == ASYNC_SPD_HI)
-		tty->alt_speed = 57600;
-	if ((info->port.flags & ASYNC_SPD_MASK) == ASYNC_SPD_VHI)
-		tty->alt_speed = 115200;
-	if ((info->port.flags & ASYNC_SPD_MASK) == ASYNC_SPD_SHI)
-		tty->alt_speed = 230400;
-	if ((info->port.flags & ASYNC_SPD_MASK) == ASYNC_SPD_WARP)
-		tty->alt_speed = 460800;
-
 	card = info->card;
 	channel = info->line - card->first_line;
 
@@ -2295,12 +2283,16 @@ cy_set_serial_info(struct cyclades_port *info, struct tty_struct *tty,
 		struct serial_struct __user *new_info)
 {
 	struct serial_struct new_serial;
+	int old_flags;
 	int ret;
 
 	if (copy_from_user(&new_serial, new_info, sizeof(new_serial)))
 		return -EFAULT;
 
 	mutex_lock(&info->port.mutex);
+
+	old_flags = info->port.flags;
+
 	if (!capable(CAP_SYS_ADMIN)) {
 		if (new_serial.close_delay != info->port.close_delay ||
 				new_serial.baud_base != info->baud ||
@@ -2332,6 +2324,11 @@ cy_set_serial_info(struct cyclades_port *info, struct tty_struct *tty,
 
 check_and_exit:
 	if (tty_port_initialized(&info->port)) {
+		if ((new_serial.flags ^ old_flags) & ASYNC_SPD_MASK) {
+			/* warn about deprecation unless clearing */
+			if (new_serial.flags & ASYNC_SPD_MASK)
+				dev_warn_ratelimited(tty->dev, "use of SPD flags is deprecated\n");
+		}
 		cy_set_line_char(info, tty);
 		ret = 0;
 	} else {
diff --git a/drivers/tty/hvc/Kconfig b/drivers/tty/hvc/Kconfig
index 574da15fe618..b8d5ea0ae26b 100644
--- a/drivers/tty/hvc/Kconfig
+++ b/drivers/tty/hvc/Kconfig
@@ -44,7 +44,7 @@ config HVC_RTAS
 
 config HVC_IUCV
 	bool "z/VM IUCV Hypervisor console support (VM only)"
-	depends on S390
+	depends on S390 && NET
 	select HVC_DRIVER
 	select IUCV
 	default y
diff --git a/drivers/tty/hvc/hvcs.c b/drivers/tty/hvc/hvcs.c
index 99bb875178d7..79cc5beea2da 100644
--- a/drivers/tty/hvc/hvcs.c
+++ b/drivers/tty/hvc/hvcs.c
@@ -484,13 +484,13 @@ static struct attribute_group hvcs_attr_group = {
 	.attrs = hvcs_attrs,
 };
 
-static ssize_t hvcs_rescan_show(struct device_driver *ddp, char *buf)
+static ssize_t rescan_show(struct device_driver *ddp, char *buf)
 {
 	/* A 1 means it is updating, a 0 means it is done updating */
 	return snprintf(buf, PAGE_SIZE, "%d\n", hvcs_rescan_status);
 }
 
-static ssize_t hvcs_rescan_store(struct device_driver *ddp, const char * buf,
+static ssize_t rescan_store(struct device_driver *ddp, const char * buf,
 		size_t count)
 {
 	if ((simple_strtol(buf, NULL, 0) != 1)
@@ -505,8 +505,7 @@ static ssize_t hvcs_rescan_store(struct device_driver *ddp, const char * buf,
 	return count;
 }
 
-static DRIVER_ATTR(rescan,
-	S_IRUGO | S_IWUSR, hvcs_rescan_show, hvcs_rescan_store);
+static DRIVER_ATTR_RW(rescan);
 
 static void hvcs_kick(void)
 {
@@ -1242,8 +1241,7 @@ static void hvcs_close(struct tty_struct *tty, struct file *filp)
 		free_irq(irq, hvcsd);
 		return;
 	} else if (hvcsd->port.count < 0) {
-		printk(KERN_ERR "HVCS: vty-server@%X open_count: %d"
-				" is missmanaged.\n",
+		printk(KERN_ERR "HVCS: vty-server@%X open_count: %d is mismanaged.\n",
 		hvcsd->vdev->unit_address, hvcsd->port.count);
 	}
 
diff --git a/drivers/tty/ipwireless/network.c b/drivers/tty/ipwireless/network.c
index c0dfb642383b..c2f9a3263b37 100644
--- a/drivers/tty/ipwireless/network.c
+++ b/drivers/tty/ipwireless/network.c
@@ -355,7 +355,7 @@ static struct sk_buff *ipw_packet_received_skb(unsigned char *data,
 	if (skb == NULL)
 		return NULL;
 	skb_reserve(skb, 2);
-	memcpy(skb_put(skb, length), data, length);
+	skb_put_data(skb, data, length);
 
 	return skb;
 }
diff --git a/drivers/tty/n_gsm.c b/drivers/tty/n_gsm.c
index 2667a205a5ab..2afe5fce68e3 100644
--- a/drivers/tty/n_gsm.c
+++ b/drivers/tty/n_gsm.c
@@ -2015,6 +2015,33 @@ static void gsm_error(struct gsm_mux *gsm,
 	gsm->io_error++;
 }
 
+static int gsm_disconnect(struct gsm_mux *gsm)
+{
+	struct gsm_dlci *dlci = gsm->dlci[0];
+	struct gsm_control *gc;
+
+	if (!dlci)
+		return 0;
+
+	/* In theory disconnecting DLCI 0 is sufficient but for some
+	   modems this is apparently not the case. */
+	gc = gsm_control_send(gsm, CMD_CLD, NULL, 0);
+	if (gc)
+		gsm_control_wait(gsm, gc);
+
+	del_timer_sync(&gsm->t2_timer);
+	/* Now we are sure T2 has stopped */
+
+	gsm_dlci_begin_close(dlci);
+	wait_event_interruptible(gsm->event,
+				dlci->state == DLCI_CLOSED);
+
+	if (signal_pending(current))
+		return -EINTR;
+
+	return 0;
+}
+
 /**
  *	gsm_cleanup_mux		-	generic GSM protocol cleanup
  *	@gsm: our mux
@@ -2029,7 +2056,6 @@ static void gsm_cleanup_mux(struct gsm_mux *gsm)
 	int i;
 	struct gsm_dlci *dlci = gsm->dlci[0];
 	struct gsm_msg *txq, *ntxq;
-	struct gsm_control *gc;
 
 	gsm->dead = 1;
 
@@ -2045,21 +2071,11 @@ static void gsm_cleanup_mux(struct gsm_mux *gsm)
 	if (i == MAX_MUX)
 		return;
 
-	/* In theory disconnecting DLCI 0 is sufficient but for some
-	   modems this is apparently not the case. */
-	if (dlci) {
-		gc = gsm_control_send(gsm, CMD_CLD, NULL, 0);
-		if (gc)
-			gsm_control_wait(gsm, gc);
-	}
 	del_timer_sync(&gsm->t2_timer);
 	/* Now we are sure T2 has stopped */
-	if (dlci) {
+	if (dlci)
 		dlci->dead = 1;
-		gsm_dlci_begin_close(dlci);
-		wait_event_interruptible(gsm->event,
-					dlci->state == DLCI_CLOSED);
-	}
+
 	/* Free up any link layer users */
 	mutex_lock(&gsm->mutex);
 	for (i = 0; i < NUM_DLCI; i++)
@@ -2519,12 +2535,12 @@ static int gsmld_config(struct tty_struct *tty, struct gsm_mux *gsm,
 	 */
 
 	if (need_close || need_restart) {
-		gsm_dlci_begin_close(gsm->dlci[0]);
-		/* This will timeout if the link is down due to N2 expiring */
-		wait_event_interruptible(gsm->event,
-				gsm->dlci[0]->state == DLCI_CLOSED);
-		if (signal_pending(current))
-			return -EINTR;
+		int ret;
+
+		ret = gsm_disconnect(gsm);
+
+		if (ret)
+			return ret;
 	}
 	if (need_restart)
 		gsm_cleanup_mux(gsm);
@@ -2688,7 +2704,7 @@ static void gsm_mux_rx_netchar(struct gsm_dlci *dlci,
 		return;
 	}
 	skb_reserve(skb, NET_IP_ALIGN);
-	memcpy(skb_put(skb, size), in_buf, size);
+	skb_put_data(skb, in_buf, size);
 
 	skb->dev = net;
 	skb->protocol = htons(ETH_P_IP);
diff --git a/drivers/tty/n_null.c b/drivers/tty/n_null.c
new file mode 100644
index 000000000000..d63261c36e42
--- /dev/null
+++ b/drivers/tty/n_null.c
@@ -0,0 +1,80 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/tty.h>
+#include <linux/module.h>
+
+/*
+ *  n_null.c - Null line discipline used in the failure path
+ *
+ *  Copyright (C) Intel 2017
+ *
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ *
+ *  This program is free software; you can 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.
+ *
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ */
+
+static int n_null_open(struct tty_struct *tty)
+{
+	return 0;
+}
+
+static void n_null_close(struct tty_struct *tty)
+{
+}
+
+static ssize_t n_null_read(struct tty_struct *tty, struct file *file,
+			   unsigned char __user * buf, size_t nr)
+{
+	return -EOPNOTSUPP;
+}
+
+static ssize_t n_null_write(struct tty_struct *tty, struct file *file,
+			    const unsigned char *buf, size_t nr)
+{
+	return -EOPNOTSUPP;
+}
+
+static void n_null_receivebuf(struct tty_struct *tty,
+				 const unsigned char *cp, char *fp,
+				 int cnt)
+{
+}
+
+static struct tty_ldisc_ops null_ldisc = {
+	.owner		=	THIS_MODULE,
+	.magic		=	TTY_LDISC_MAGIC,
+	.name		=	"n_null",
+	.open		=	n_null_open,
+	.close		=	n_null_close,
+	.read		=	n_null_read,
+	.write		=	n_null_write,
+	.receive_buf	=	n_null_receivebuf
+};
+
+static int __init n_null_init(void)
+{
+	BUG_ON(tty_register_ldisc(N_NULL, &null_ldisc));
+	return 0;
+}
+
+static void __exit n_null_exit(void)
+{
+	tty_unregister_ldisc(N_NULL);
+}
+
+module_init(n_null_init);
+module_exit(n_null_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Alan Cox");
+MODULE_ALIAS_LDISC(N_NULL);
+MODULE_DESCRIPTION("Null ldisc driver");
diff --git a/drivers/tty/pty.c b/drivers/tty/pty.c
index 65799575c666..d1399aac05a1 100644
--- a/drivers/tty/pty.c
+++ b/drivers/tty/pty.c
@@ -24,6 +24,9 @@
 #include <linux/slab.h>
 #include <linux/mutex.h>
 #include <linux/poll.h>
+#include <linux/mount.h>
+#include <linux/file.h>
+#include <linux/ioctl.h>
 
 #undef TTY_DEBUG_HANGUP
 #ifdef TTY_DEBUG_HANGUP
@@ -66,8 +69,13 @@ static void pty_close(struct tty_struct *tty, struct file *filp)
 #ifdef CONFIG_UNIX98_PTYS
 		if (tty->driver == ptm_driver) {
 			mutex_lock(&devpts_mutex);
-			if (tty->link->driver_data)
-				devpts_pty_kill(tty->link->driver_data);
+			if (tty->link->driver_data) {
+				struct path *path = tty->link->driver_data;
+
+				devpts_pty_kill(path->dentry);
+				path_put(path);
+				kfree(path);
+			}
 			mutex_unlock(&devpts_mutex);
 		}
 #endif
@@ -440,6 +448,48 @@ err:
 	return retval;
 }
 
+/**
+ *	pty_open_peer - open the peer of a pty
+ *	@tty: the peer of the pty being opened
+ *
+ *	Open the cached dentry in tty->link, providing a safe way for userspace
+ *	to get the slave end of a pty (where they have the master fd and cannot
+ *	access or trust the mount namespace /dev/pts was mounted inside).
+ */
+static struct file *pty_open_peer(struct tty_struct *tty, int flags)
+{
+	if (tty->driver->subtype != PTY_TYPE_MASTER)
+		return ERR_PTR(-EIO);
+	return dentry_open(tty->link->driver_data, flags, current_cred());
+}
+
+static int pty_get_peer(struct tty_struct *tty, int flags)
+{
+	int fd = -1;
+	struct file *filp = NULL;
+	int retval = -EINVAL;
+
+	fd = get_unused_fd_flags(0);
+	if (fd < 0) {
+		retval = fd;
+		goto err;
+	}
+
+	filp = pty_open_peer(tty, flags);
+	if (IS_ERR(filp)) {
+		retval = PTR_ERR(filp);
+		goto err_put;
+	}
+
+	fd_install(fd, filp);
+	return fd;
+
+err_put:
+	put_unused_fd(fd);
+err:
+	return retval;
+}
+
 static void pty_cleanup(struct tty_struct *tty)
 {
 	tty_port_put(tty->port);
@@ -481,6 +531,16 @@ static int pty_bsd_ioctl(struct tty_struct *tty,
 	return -ENOIOCTLCMD;
 }
 
+static long pty_bsd_compat_ioctl(struct tty_struct *tty,
+				 unsigned int cmd, unsigned long arg)
+{
+	/*
+	 * PTY ioctls don't require any special translation between 32-bit and
+	 * 64-bit userspace, they are already compatible.
+	 */
+	return pty_bsd_ioctl(tty, cmd, arg);
+}
+
 static int legacy_count = CONFIG_LEGACY_PTY_COUNT;
 /*
  * not really modular, but the easiest way to keep compat with existing
@@ -502,6 +562,7 @@ static const struct tty_operations master_pty_ops_bsd = {
 	.chars_in_buffer = pty_chars_in_buffer,
 	.unthrottle = pty_unthrottle,
 	.ioctl = pty_bsd_ioctl,
+	.compat_ioctl = pty_bsd_compat_ioctl,
 	.cleanup = pty_cleanup,
 	.resize = pty_resize,
 	.remove = pty_remove
@@ -602,6 +663,8 @@ static int pty_unix98_ioctl(struct tty_struct *tty,
 		return pty_get_pktmode(tty, (int __user *)arg);
 	case TIOCGPTN: /* Get PT Number */
 		return put_user(tty->index, (unsigned int __user *)arg);
+	case TIOCGPTPEER: /* Open the other end */
+		return pty_get_peer(tty, (int) arg);
 	case TIOCSIG:    /* Send signal to other side of pty */
 		return pty_signal(tty, (int) arg);
 	}
@@ -609,6 +672,16 @@ static int pty_unix98_ioctl(struct tty_struct *tty,
 	return -ENOIOCTLCMD;
 }
 
+static long pty_unix98_compat_ioctl(struct tty_struct *tty,
+				 unsigned int cmd, unsigned long arg)
+{
+	/*
+	 * PTY ioctls don't require any special translation between 32-bit and
+	 * 64-bit userspace, they are already compatible.
+	 */
+	return pty_unix98_ioctl(tty, cmd, arg);
+}
+
 /**
  *	ptm_unix98_lookup	-	find a pty master
  *	@driver: ptm driver
@@ -681,6 +754,7 @@ static const struct tty_operations ptm_unix98_ops = {
 	.chars_in_buffer = pty_chars_in_buffer,
 	.unthrottle = pty_unthrottle,
 	.ioctl = pty_unix98_ioctl,
+	.compat_ioctl = pty_unix98_compat_ioctl,
 	.resize = pty_resize,
 	.cleanup = pty_cleanup
 };
@@ -718,6 +792,7 @@ static int ptmx_open(struct inode *inode, struct file *filp)
 {
 	struct pts_fs_info *fsi;
 	struct tty_struct *tty;
+	struct path *pts_path;
 	struct dentry *dentry;
 	int retval;
 	int index;
@@ -771,16 +846,26 @@ static int ptmx_open(struct inode *inode, struct file *filp)
 		retval = PTR_ERR(dentry);
 		goto err_release;
 	}
-	tty->link->driver_data = dentry;
+	/* We need to cache a fake path for TIOCGPTPEER. */
+	pts_path = kmalloc(sizeof(struct path), GFP_KERNEL);
+	if (!pts_path)
+		goto err_release;
+	pts_path->mnt = filp->f_path.mnt;
+	pts_path->dentry = dentry;
+	path_get(pts_path);
+	tty->link->driver_data = pts_path;
 
 	retval = ptm_driver->ops->open(tty, filp);
 	if (retval)
-		goto err_release;
+		goto err_path_put;
 
 	tty_debug_hangup(tty, "opening (count=%d)\n", tty->count);
 
 	tty_unlock(tty);
 	return 0;
+err_path_put:
+	path_put(pts_path);
+	kfree(pts_path);
 err_release:
 	tty_unlock(tty);
 	// This will also put-ref the fsi
diff --git a/drivers/tty/rocket.c b/drivers/tty/rocket.c
index b51a877da986..20d79a6007d5 100644
--- a/drivers/tty/rocket.c
+++ b/drivers/tty/rocket.c
@@ -947,18 +947,6 @@ static int rp_open(struct tty_struct *tty, struct file *filp)
 
 		tty_port_set_initialized(&info->port, 1);
 
-		/*
-		 * Set up the tty->alt_speed kludge
-		 */
-		if ((info->flags & ROCKET_SPD_MASK) == ROCKET_SPD_HI)
-			tty->alt_speed = 57600;
-		if ((info->flags & ROCKET_SPD_MASK) == ROCKET_SPD_VHI)
-			tty->alt_speed = 115200;
-		if ((info->flags & ROCKET_SPD_MASK) == ROCKET_SPD_SHI)
-			tty->alt_speed = 230400;
-		if ((info->flags & ROCKET_SPD_MASK) == ROCKET_SPD_WARP)
-			tty->alt_speed = 460800;
-
 		configure_r_port(tty, info, NULL);
 		if (C_BAUD(tty)) {
 			sSetDTR(cp);
@@ -1219,23 +1207,20 @@ static int set_config(struct tty_struct *tty, struct r_port *info,
 			return -EPERM;
 		}
 		info->flags = ((info->flags & ~ROCKET_USR_MASK) | (new_serial.flags & ROCKET_USR_MASK));
-		configure_r_port(tty, info, NULL);
 		mutex_unlock(&info->port.mutex);
 		return 0;
 	}
 
+	if ((new_serial.flags ^ info->flags) & ROCKET_SPD_MASK) {
+		/* warn about deprecation, unless clearing */
+		if (new_serial.flags & ROCKET_SPD_MASK)
+			dev_warn_ratelimited(tty->dev, "use of SPD flags is deprecated\n");
+	}
+
 	info->flags = ((info->flags & ~ROCKET_FLAGS) | (new_serial.flags & ROCKET_FLAGS));
 	info->port.close_delay = new_serial.close_delay;
 	info->port.closing_wait = new_serial.closing_wait;
 
-	if ((info->flags & ROCKET_SPD_MASK) == ROCKET_SPD_HI)
-		tty->alt_speed = 57600;
-	if ((info->flags & ROCKET_SPD_MASK) == ROCKET_SPD_VHI)
-		tty->alt_speed = 115200;
-	if ((info->flags & ROCKET_SPD_MASK) == ROCKET_SPD_SHI)
-		tty->alt_speed = 230400;
-	if ((info->flags & ROCKET_SPD_MASK) == ROCKET_SPD_WARP)
-		tty->alt_speed = 460800;
 	mutex_unlock(&info->port.mutex);
 
 	configure_r_port(tty, info, NULL);
diff --git a/drivers/tty/serdev/core.c b/drivers/tty/serdev/core.c
index f71b47334149..ae1aaa0075d1 100644
--- a/drivers/tty/serdev/core.c
+++ b/drivers/tty/serdev/core.c
@@ -262,11 +262,13 @@ static ssize_t modalias_show(struct device *dev,
 {
 	return of_device_modalias(dev, buf, PAGE_SIZE);
 }
+DEVICE_ATTR_RO(modalias);
 
-static struct device_attribute serdev_device_attrs[] = {
-	__ATTR_RO(modalias),
-	__ATTR_NULL
+static struct attribute *serdev_device_attrs[] = {
+	&dev_attr_modalias.attr,
+	NULL,
 };
+ATTRIBUTE_GROUPS(serdev_device);
 
 static struct bus_type serdev_bus_type = {
 	.name		= "serial",
@@ -274,7 +276,7 @@ static struct bus_type serdev_bus_type = {
 	.probe		= serdev_drv_probe,
 	.remove		= serdev_drv_remove,
 	.uevent		= serdev_uevent,
-	.dev_attrs	= serdev_device_attrs,
+	.dev_groups	= serdev_device_groups,
 };
 
 /**
diff --git a/drivers/tty/serdev/serdev-ttyport.c b/drivers/tty/serdev/serdev-ttyport.c
index d0a021c93986..302018d67efa 100644
--- a/drivers/tty/serdev/serdev-ttyport.c
+++ b/drivers/tty/serdev/serdev-ttyport.c
@@ -148,7 +148,7 @@ static unsigned int ttyport_set_baudrate(struct serdev_controller *ctrl, unsigne
 
 	/* tty_set_termios() return not checked as it is always 0 */
 	tty_set_termios(tty, &ktermios);
-	return speed;
+	return ktermios.c_ospeed;
 }
 
 static void ttyport_set_flow_control(struct serdev_controller *ctrl, bool enable)
diff --git a/drivers/tty/serial/8250/8250.h b/drivers/tty/serial/8250/8250.h
index ce8d4ffcc425..b2bdc35f7495 100644
--- a/drivers/tty/serial/8250/8250.h
+++ b/drivers/tty/serial/8250/8250.h
@@ -81,6 +81,9 @@ struct serial8250_config {
 #define UART_CAP_HFIFO	(1 << 14)	/* UART has a "hidden" FIFO */
 #define UART_CAP_RPM	(1 << 15)	/* Runtime PM is active while idle */
 #define UART_CAP_IRDA	(1 << 16)	/* UART supports IrDA line discipline */
+#define UART_CAP_MINI	(1 << 17)	/* Mini UART on BCM283X family lacks:
+					 * STOP PARITY EPAR SPAR WLEN5 WLEN6
+					 */
 
 #define UART_BUG_QUOT	(1 << 0)	/* UART has buggy quot LSB */
 #define UART_BUG_TXEN	(1 << 1)	/* UART has buggy TX IIR status */
diff --git a/drivers/tty/serial/8250/8250_aspeed_vuart.c b/drivers/tty/serial/8250/8250_aspeed_vuart.c
new file mode 100644
index 000000000000..822be4906763
--- /dev/null
+++ b/drivers/tty/serial/8250/8250_aspeed_vuart.c
@@ -0,0 +1,323 @@
+/*
+ *  Serial Port driver for Aspeed VUART device
+ *
+ *    Copyright (C) 2016 Jeremy Kerr <jk@ozlabs.org>, IBM Corp.
+ *    Copyright (C) 2006 Arnd Bergmann <arnd@arndb.de>, IBM Corp.
+ *
+ *  This program is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU General Public License
+ *  as published by the Free Software Foundation; either version
+ *  2 of the License, or (at your option) any later version.
+ */
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_platform.h>
+#include <linux/clk.h>
+
+#include "8250.h"
+
+#define ASPEED_VUART_GCRA		0x20
+#define ASPEED_VUART_GCRA_VUART_EN		BIT(0)
+#define ASPEED_VUART_GCRA_DISABLE_HOST_TX_DISCARD BIT(5)
+#define ASPEED_VUART_GCRB		0x24
+#define ASPEED_VUART_GCRB_HOST_SIRQ_MASK	GENMASK(7, 4)
+#define ASPEED_VUART_GCRB_HOST_SIRQ_SHIFT	4
+#define ASPEED_VUART_ADDRL		0x28
+#define ASPEED_VUART_ADDRH		0x2c
+
+struct aspeed_vuart {
+	struct device		*dev;
+	void __iomem		*regs;
+	struct clk		*clk;
+	int			line;
+};
+
+/*
+ * The VUART is basically two UART 'front ends' connected by their FIFO
+ * (no actual serial line in between). One is on the BMC side (management
+ * controller) and one is on the host CPU side.
+ *
+ * It allows the BMC to provide to the host a "UART" that pipes into
+ * the BMC itself and can then be turned by the BMC into a network console
+ * of some sort for example.
+ *
+ * This driver is for the BMC side. The sysfs files allow the BMC
+ * userspace which owns the system configuration policy, to specify
+ * at what IO port and interrupt number the host side will appear
+ * to the host on the Host <-> BMC LPC bus. It could be different on a
+ * different system (though most of them use 3f8/4).
+ */
+
+static ssize_t lpc_address_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct aspeed_vuart *vuart = dev_get_drvdata(dev);
+	u16 addr;
+
+	addr = (readb(vuart->regs + ASPEED_VUART_ADDRH) << 8) |
+		(readb(vuart->regs + ASPEED_VUART_ADDRL));
+
+	return snprintf(buf, PAGE_SIZE - 1, "0x%x\n", addr);
+}
+
+static ssize_t lpc_address_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct aspeed_vuart *vuart = dev_get_drvdata(dev);
+	unsigned long val;
+	int err;
+
+	err = kstrtoul(buf, 0, &val);
+	if (err)
+		return err;
+
+	writeb(val >> 8, vuart->regs + ASPEED_VUART_ADDRH);
+	writeb(val >> 0, vuart->regs + ASPEED_VUART_ADDRL);
+
+	return count;
+}
+
+static DEVICE_ATTR_RW(lpc_address);
+
+static ssize_t sirq_show(struct device *dev,
+			 struct device_attribute *attr, char *buf)
+{
+	struct aspeed_vuart *vuart = dev_get_drvdata(dev);
+	u8 reg;
+
+	reg = readb(vuart->regs + ASPEED_VUART_GCRB);
+	reg &= ASPEED_VUART_GCRB_HOST_SIRQ_MASK;
+	reg >>= ASPEED_VUART_GCRB_HOST_SIRQ_SHIFT;
+
+	return snprintf(buf, PAGE_SIZE - 1, "%u\n", reg);
+}
+
+static ssize_t sirq_store(struct device *dev, struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	struct aspeed_vuart *vuart = dev_get_drvdata(dev);
+	unsigned long val;
+	int err;
+	u8 reg;
+
+	err = kstrtoul(buf, 0, &val);
+	if (err)
+		return err;
+
+	val <<= ASPEED_VUART_GCRB_HOST_SIRQ_SHIFT;
+	val &= ASPEED_VUART_GCRB_HOST_SIRQ_MASK;
+
+	reg = readb(vuart->regs + ASPEED_VUART_GCRB);
+	reg &= ~ASPEED_VUART_GCRB_HOST_SIRQ_MASK;
+	reg |= val;
+	writeb(reg, vuart->regs + ASPEED_VUART_GCRB);
+
+	return count;
+}
+
+static DEVICE_ATTR_RW(sirq);
+
+static struct attribute *aspeed_vuart_attrs[] = {
+	&dev_attr_sirq.attr,
+	&dev_attr_lpc_address.attr,
+	NULL,
+};
+
+static const struct attribute_group aspeed_vuart_attr_group = {
+	.attrs = aspeed_vuart_attrs,
+};
+
+static void aspeed_vuart_set_enabled(struct aspeed_vuart *vuart, bool enabled)
+{
+	u8 reg = readb(vuart->regs + ASPEED_VUART_GCRA);
+
+	if (enabled)
+		reg |= ASPEED_VUART_GCRA_VUART_EN;
+	else
+		reg &= ~ASPEED_VUART_GCRA_VUART_EN;
+
+	writeb(reg, vuart->regs + ASPEED_VUART_GCRA);
+}
+
+static void aspeed_vuart_set_host_tx_discard(struct aspeed_vuart *vuart,
+					     bool discard)
+{
+	u8 reg;
+
+	reg = readb(vuart->regs + ASPEED_VUART_GCRA);
+
+	/* If the DISABLE_HOST_TX_DISCARD bit is set, discard is disabled */
+	if (!discard)
+		reg |= ASPEED_VUART_GCRA_DISABLE_HOST_TX_DISCARD;
+	else
+		reg &= ~ASPEED_VUART_GCRA_DISABLE_HOST_TX_DISCARD;
+
+	writeb(reg, vuart->regs + ASPEED_VUART_GCRA);
+}
+
+static int aspeed_vuart_startup(struct uart_port *uart_port)
+{
+	struct uart_8250_port *uart_8250_port = up_to_u8250p(uart_port);
+	struct aspeed_vuart *vuart = uart_8250_port->port.private_data;
+	int rc;
+
+	rc = serial8250_do_startup(uart_port);
+	if (rc)
+		return rc;
+
+	aspeed_vuart_set_host_tx_discard(vuart, false);
+
+	return 0;
+}
+
+static void aspeed_vuart_shutdown(struct uart_port *uart_port)
+{
+	struct uart_8250_port *uart_8250_port = up_to_u8250p(uart_port);
+	struct aspeed_vuart *vuart = uart_8250_port->port.private_data;
+
+	aspeed_vuart_set_host_tx_discard(vuart, true);
+
+	serial8250_do_shutdown(uart_port);
+}
+
+static int aspeed_vuart_probe(struct platform_device *pdev)
+{
+	struct uart_8250_port port;
+	struct aspeed_vuart *vuart;
+	struct device_node *np;
+	struct resource *res;
+	u32 clk, prop;
+	int rc;
+
+	np = pdev->dev.of_node;
+
+	vuart = devm_kzalloc(&pdev->dev, sizeof(*vuart), GFP_KERNEL);
+	if (!vuart)
+		return -ENOMEM;
+
+	vuart->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	vuart->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(vuart->regs))
+		return PTR_ERR(vuart->regs);
+
+	memset(&port, 0, sizeof(port));
+	port.port.private_data = vuart;
+	port.port.membase = vuart->regs;
+	port.port.mapbase = res->start;
+	port.port.mapsize = resource_size(res);
+	port.port.startup = aspeed_vuart_startup;
+	port.port.shutdown = aspeed_vuart_shutdown;
+	port.port.dev = &pdev->dev;
+
+	rc = sysfs_create_group(&vuart->dev->kobj, &aspeed_vuart_attr_group);
+	if (rc < 0)
+		return rc;
+
+	if (of_property_read_u32(np, "clock-frequency", &clk)) {
+		vuart->clk = devm_clk_get(&pdev->dev, NULL);
+		if (IS_ERR(vuart->clk)) {
+			dev_warn(&pdev->dev,
+				"clk or clock-frequency not defined\n");
+			return PTR_ERR(vuart->clk);
+		}
+
+		rc = clk_prepare_enable(vuart->clk);
+		if (rc < 0)
+			return rc;
+
+		clk = clk_get_rate(vuart->clk);
+	}
+
+	/* If current-speed was set, then try not to change it. */
+	if (of_property_read_u32(np, "current-speed", &prop) == 0)
+		port.port.custom_divisor = clk / (16 * prop);
+
+	/* Check for shifted address mapping */
+	if (of_property_read_u32(np, "reg-offset", &prop) == 0)
+		port.port.mapbase += prop;
+
+	/* Check for registers offset within the devices address range */
+	if (of_property_read_u32(np, "reg-shift", &prop) == 0)
+		port.port.regshift = prop;
+
+	/* Check for fifo size */
+	if (of_property_read_u32(np, "fifo-size", &prop) == 0)
+		port.port.fifosize = prop;
+
+	/* Check for a fixed line number */
+	rc = of_alias_get_id(np, "serial");
+	if (rc >= 0)
+		port.port.line = rc;
+
+	port.port.irq = irq_of_parse_and_map(np, 0);
+	port.port.irqflags = IRQF_SHARED;
+	port.port.iotype = UPIO_MEM;
+	port.port.type = PORT_16550A;
+	port.port.uartclk = clk;
+	port.port.flags = UPF_SHARE_IRQ | UPF_BOOT_AUTOCONF
+		| UPF_FIXED_PORT | UPF_FIXED_TYPE | UPF_NO_THRE_TEST;
+
+	if (of_property_read_bool(np, "no-loopback-test"))
+		port.port.flags |= UPF_SKIP_TEST;
+
+	if (port.port.fifosize)
+		port.capabilities = UART_CAP_FIFO;
+
+	if (of_property_read_bool(np, "auto-flow-control"))
+		port.capabilities |= UART_CAP_AFE;
+
+	rc = serial8250_register_8250_port(&port);
+	if (rc < 0)
+		goto err_clk_disable;
+
+	vuart->line = rc;
+
+	aspeed_vuart_set_enabled(vuart, true);
+	aspeed_vuart_set_host_tx_discard(vuart, true);
+	platform_set_drvdata(pdev, vuart);
+
+	return 0;
+
+err_clk_disable:
+	clk_disable_unprepare(vuart->clk);
+	irq_dispose_mapping(port.port.irq);
+	return rc;
+}
+
+static int aspeed_vuart_remove(struct platform_device *pdev)
+{
+	struct aspeed_vuart *vuart = platform_get_drvdata(pdev);
+
+	aspeed_vuart_set_enabled(vuart, false);
+	serial8250_unregister_port(vuart->line);
+	sysfs_remove_group(&vuart->dev->kobj, &aspeed_vuart_attr_group);
+	clk_disable_unprepare(vuart->clk);
+
+	return 0;
+}
+
+static const struct of_device_id aspeed_vuart_table[] = {
+	{ .compatible = "aspeed,ast2400-vuart" },
+	{ .compatible = "aspeed,ast2500-vuart" },
+	{ },
+};
+
+static struct platform_driver aspeed_vuart_driver = {
+	.driver = {
+		.name = "aspeed-vuart",
+		.of_match_table = aspeed_vuart_table,
+	},
+	.probe = aspeed_vuart_probe,
+	.remove = aspeed_vuart_remove,
+};
+
+module_platform_driver(aspeed_vuart_driver);
+
+MODULE_AUTHOR("Jeremy Kerr <jk@ozlabs.org>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Driver for Aspeed VUART device");
diff --git a/drivers/tty/serial/8250/8250_bcm2835aux.c b/drivers/tty/serial/8250/8250_bcm2835aux.c
index e10f1244409b..a23c7da42ea8 100644
--- a/drivers/tty/serial/8250/8250_bcm2835aux.c
+++ b/drivers/tty/serial/8250/8250_bcm2835aux.c
@@ -39,7 +39,7 @@ static int bcm2835aux_serial_probe(struct platform_device *pdev)
 
 	/* initialize data */
 	spin_lock_init(&data->uart.port.lock);
-	data->uart.capabilities = UART_CAP_FIFO;
+	data->uart.capabilities = UART_CAP_FIFO | UART_CAP_MINI;
 	data->uart.port.dev = &pdev->dev;
 	data->uart.port.regshift = 2;
 	data->uart.port.type = PORT_16550;
diff --git a/drivers/tty/serial/8250/8250_core.c b/drivers/tty/serial/8250/8250_core.c
index 1aab3010fbfa..b5def356af63 100644
--- a/drivers/tty/serial/8250/8250_core.c
+++ b/drivers/tty/serial/8250/8250_core.c
@@ -1043,24 +1043,13 @@ int serial8250_register_8250_port(struct uart_8250_port *up)
 		if (up->dl_write)
 			uart->dl_write = up->dl_write;
 
-		if (uart->port.type != PORT_8250_CIR) {
-			if (serial8250_isa_config != NULL)
-				serial8250_isa_config(0, &uart->port,
-						&uart->capabilities);
-
-			ret = uart_add_one_port(&serial8250_reg,
-						&uart->port);
-			if (ret == 0)
-				ret = uart->port.line;
-		} else {
-			dev_info(uart->port.dev,
-				"skipping CIR port at 0x%lx / 0x%llx, IRQ %d\n",
-				uart->port.iobase,
-				(unsigned long long)uart->port.mapbase,
-				uart->port.irq);
+		if (serial8250_isa_config != NULL)
+			serial8250_isa_config(0, &uart->port,
+					&uart->capabilities);
 
-			ret = 0;
-		}
+		ret = uart_add_one_port(&serial8250_reg, &uart->port);
+		if (ret == 0)
+			ret = uart->port.line;
 	}
 	mutex_unlock(&serial_mutex);
 
diff --git a/drivers/tty/serial/8250/8250_exar.c b/drivers/tty/serial/8250/8250_exar.c
index 1270ff163f63..a309bcffffcd 100644
--- a/drivers/tty/serial/8250/8250_exar.c
+++ b/drivers/tty/serial/8250/8250_exar.c
@@ -109,7 +109,6 @@ pci_fastcom335_setup(struct exar8250 *priv, struct pci_dev *pcidev,
 	u8 __iomem *p;
 	int err;
 
-	port->port.flags |= UPF_EXAR_EFR;
 	port->port.uartclk = baud * 16;
 
 	err = default_setup(priv, pcidev, idx, offset, port);
@@ -171,19 +170,26 @@ pci_xr17c154_setup(struct exar8250 *priv, struct pci_dev *pcidev,
 	return default_setup(priv, pcidev, idx, offset, port);
 }
 
-static void setup_gpio(u8 __iomem *p)
+static void setup_gpio(struct pci_dev *pcidev, u8 __iomem *p)
 {
+	/*
+	 * The Commtech adapters required the MPIOs to be driven low. The Exar
+	 * devices will export them as GPIOs, so we pre-configure them safely
+	 * as inputs.
+	 */
+	u8 dir = pcidev->vendor == PCI_VENDOR_ID_EXAR ? 0xff : 0x00;
+
 	writeb(0x00, p + UART_EXAR_MPIOINT_7_0);
 	writeb(0x00, p + UART_EXAR_MPIOLVL_7_0);
 	writeb(0x00, p + UART_EXAR_MPIO3T_7_0);
 	writeb(0x00, p + UART_EXAR_MPIOINV_7_0);
-	writeb(0x00, p + UART_EXAR_MPIOSEL_7_0);
+	writeb(dir,  p + UART_EXAR_MPIOSEL_7_0);
 	writeb(0x00, p + UART_EXAR_MPIOOD_7_0);
 	writeb(0x00, p + UART_EXAR_MPIOINT_15_8);
 	writeb(0x00, p + UART_EXAR_MPIOLVL_15_8);
 	writeb(0x00, p + UART_EXAR_MPIO3T_15_8);
 	writeb(0x00, p + UART_EXAR_MPIOINV_15_8);
-	writeb(0x00, p + UART_EXAR_MPIOSEL_15_8);
+	writeb(dir,  p + UART_EXAR_MPIOSEL_15_8);
 	writeb(0x00, p + UART_EXAR_MPIOOD_15_8);
 }
 
@@ -236,7 +242,7 @@ pci_xr17v35x_setup(struct exar8250 *priv, struct pci_dev *pcidev,
 
 	if (idx == 0) {
 		/* Setup Multipurpose Input/Output pins. */
-		setup_gpio(p);
+		setup_gpio(pcidev, p);
 
 		port->port.private_data = xr17v35x_register_gpio(pcidev);
 	}
diff --git a/drivers/tty/serial/8250/8250_of.c b/drivers/tty/serial/8250/8250_of.c
index 1cbadafc6889..0cf95fddccfc 100644
--- a/drivers/tty/serial/8250/8250_of.c
+++ b/drivers/tty/serial/8250/8250_of.c
@@ -19,11 +19,13 @@
 #include <linux/of_irq.h>
 #include <linux/of_platform.h>
 #include <linux/clk.h>
+#include <linux/reset.h>
 
 #include "8250.h"
 
 struct of_serial_info {
 	struct clk *clk;
+	struct reset_control *rst;
 	int type;
 	int line;
 };
@@ -132,6 +134,13 @@ static int of_platform_serial_setup(struct platform_device *ofdev,
 		}
 	}
 
+	info->rst = devm_reset_control_get_optional_shared(&ofdev->dev, NULL);
+	if (IS_ERR(info->rst))
+		goto out;
+	ret = reset_control_deassert(info->rst);
+	if (ret)
+		goto out;
+
 	port->type = type;
 	port->uartclk = clk;
 	port->flags = UPF_SHARE_IRQ | UPF_BOOT_AUTOCONF | UPF_IOREMAP
@@ -229,6 +238,7 @@ static int of_platform_serial_remove(struct platform_device *ofdev)
 
 	serial8250_unregister_port(info->line);
 
+	reset_control_assert(info->rst);
 	if (info->clk)
 		clk_disable_unprepare(info->clk);
 	kfree(info);
diff --git a/drivers/tty/serial/8250/8250_omap.c b/drivers/tty/serial/8250/8250_omap.c
index e7e64913a748..833771bca0a5 100644
--- a/drivers/tty/serial/8250/8250_omap.c
+++ b/drivers/tty/serial/8250/8250_omap.c
@@ -613,6 +613,10 @@ static int omap_8250_startup(struct uart_port *port)
 	up->lsr_saved_flags = 0;
 	up->msr_saved_flags = 0;
 
+	/* Disable DMA for console UART */
+	if (uart_console(port))
+		up->dma = NULL;
+
 	if (up->dma) {
 		ret = serial8250_request_dma(up);
 		if (ret) {
@@ -782,8 +786,27 @@ unlock:
 
 static void __dma_rx_complete(void *param)
 {
-	__dma_rx_do_complete(param);
-	omap_8250_rx_dma(param);
+	struct uart_8250_port *p = param;
+	struct uart_8250_dma *dma = p->dma;
+	struct dma_tx_state     state;
+	unsigned long flags;
+
+	spin_lock_irqsave(&p->port.lock, flags);
+
+	/*
+	 * If the tx status is not DMA_COMPLETE, then this is a delayed
+	 * completion callback. A previous RX timeout flush would have
+	 * already pushed the data, so exit.
+	 */
+	if (dmaengine_tx_status(dma->rxchan, dma->rx_cookie, &state) !=
+			DMA_COMPLETE) {
+		spin_unlock_irqrestore(&p->port.lock, flags);
+		return;
+	}
+	__dma_rx_do_complete(p);
+	omap_8250_rx_dma(p);
+
+	spin_unlock_irqrestore(&p->port.lock, flags);
 }
 
 static void omap_8250_rx_dma_flush(struct uart_8250_port *p)
diff --git a/drivers/tty/serial/8250/8250_port.c b/drivers/tty/serial/8250/8250_port.c
index 68fd045a7025..a5fe0e66c607 100644
--- a/drivers/tty/serial/8250/8250_port.c
+++ b/drivers/tty/serial/8250/8250_port.c
@@ -1764,6 +1764,10 @@ void serial8250_tx_chars(struct uart_8250_port *up)
 		if ((up->capabilities & UART_CAP_HFIFO) &&
 		    (serial_in(up, UART_LSR) & BOTH_EMPTY) != BOTH_EMPTY)
 			break;
+		/* The BCM2835 MINI UART THRE bit is really a not-full bit. */
+		if ((up->capabilities & UART_CAP_MINI) &&
+		    !(serial_in(up, UART_LSR) & UART_LSR_THRE))
+			break;
 	} while (--count > 0);
 
 	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
@@ -2228,7 +2232,7 @@ int serial8250_do_startup(struct uart_port *port)
 		}
 	}
 
-	if (port->irq) {
+	if (port->irq && !(up->port.flags & UPF_NO_THRE_TEST)) {
 		unsigned char iir1;
 		/*
 		 * Test for UARTs that do not reassert THRE when the
@@ -2585,6 +2589,12 @@ serial8250_do_set_termios(struct uart_port *port, struct ktermios *termios,
 	unsigned long flags;
 	unsigned int baud, quot, frac = 0;
 
+	if (up->capabilities & UART_CAP_MINI) {
+		termios->c_cflag &= ~(CSTOPB | PARENB | PARODD | CMSPAR);
+		if ((termios->c_cflag & CSIZE) == CS5 ||
+		    (termios->c_cflag & CSIZE) == CS6)
+			termios->c_cflag = (termios->c_cflag & ~CSIZE) | CS7;
+	}
 	cval = serial8250_compute_lcr(up, termios->c_cflag);
 
 	baud = serial8250_get_baud_rate(port, termios, old);
diff --git a/drivers/tty/serial/8250/Kconfig b/drivers/tty/serial/8250/Kconfig
index 0e3f529d50e9..a1161ec0256f 100644
--- a/drivers/tty/serial/8250/Kconfig
+++ b/drivers/tty/serial/8250/Kconfig
@@ -224,6 +224,16 @@ config SERIAL_8250_ACCENT
 	  To compile this driver as a module, choose M here: the module
 	  will be called 8250_accent.
 
+config SERIAL_8250_ASPEED_VUART
+	tristate "Aspeed Virtual UART"
+	depends on SERIAL_8250
+	depends on OF
+	help
+	  If you want to use the virtual UART (VUART) device on Aspeed
+	  BMC platforms, enable this option. This enables the 16550A-
+	  compatible device on the local LPC bus, giving a UART device
+	  with no physical RS232 connections.
+
 config SERIAL_8250_BOCA
 	tristate "Support Boca cards"
 	depends on SERIAL_8250 != n && ISA && SERIAL_8250_MANY_PORTS
diff --git a/drivers/tty/serial/8250/Makefile b/drivers/tty/serial/8250/Makefile
index 2f30f9ecdb1b..a44a99a3e623 100644
--- a/drivers/tty/serial/8250/Makefile
+++ b/drivers/tty/serial/8250/Makefile
@@ -14,6 +14,7 @@ obj-$(CONFIG_SERIAL_8250_EXAR)		+= 8250_exar.o
 obj-$(CONFIG_SERIAL_8250_HP300)		+= 8250_hp300.o
 obj-$(CONFIG_SERIAL_8250_CS)		+= serial_cs.o
 obj-$(CONFIG_SERIAL_8250_ACORN)		+= 8250_acorn.o
+obj-$(CONFIG_SERIAL_8250_ASPEED_VUART)	+= 8250_aspeed_vuart.o
 obj-$(CONFIG_SERIAL_8250_BCM2835AUX)	+= 8250_bcm2835aux.o
 obj-$(CONFIG_SERIAL_8250_CONSOLE)	+= 8250_early.o
 obj-$(CONFIG_SERIAL_8250_FOURPORT)	+= 8250_fourport.o
diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index 5c8850f7a2a0..1f096e2bb398 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -114,32 +114,32 @@ config SERIAL_SB1250_DUART_CONSOLE
 	  If unsure, say Y.
 
 config SERIAL_ATMEL
-	bool "AT91 / AT32 on-chip serial port support"
+	bool "AT91 on-chip serial port support"
 	depends on HAS_DMA
-	depends on ARCH_AT91 || AVR32 || COMPILE_TEST
+	depends on ARCH_AT91 || COMPILE_TEST
 	select SERIAL_CORE
 	select SERIAL_MCTRL_GPIO if GPIOLIB
 	help
 	  This enables the driver for the on-chip UARTs of the Atmel
-	  AT91 and AT32 processors.
+	  AT91 processors.
 
 config SERIAL_ATMEL_CONSOLE
-	bool "Support for console on AT91 / AT32 serial port"
+	bool "Support for console on AT91 serial port"
 	depends on SERIAL_ATMEL=y
 	select SERIAL_CORE_CONSOLE
 	help
 	  Say Y here if you wish to use an on-chip UART on a Atmel
-	  AT91 or AT32 processor as the system console (the system
+	  AT91 processor as the system console (the system
 	  console is the device which receives all kernel messages and
 	  warnings and which allows logins in single user mode).
 
 config SERIAL_ATMEL_PDC
-	bool "Support DMA transfers on AT91 / AT32 serial port"
+	bool "Support DMA transfers on AT91 serial port"
 	depends on SERIAL_ATMEL
 	default y
 	help
 	  Say Y here if you wish to use the PDC to do DMA transfers to
-	  and from the Atmel AT91 / AT32 serial port. In order to
+	  and from the Atmel AT91 serial port. In order to
 	  actually use DMA transfers, make sure that the use_dma_tx
 	  and use_dma_rx members in the atmel_uart_data struct is set
 	  appropriately for each port.
@@ -152,7 +152,7 @@ config SERIAL_ATMEL_TTYAT
 	bool "Install as device ttyATn instead of ttySn"
 	depends on SERIAL_ATMEL=y
 	help
-	  Say Y here if you wish to have the internal AT91 / AT32 UARTs
+	  Say Y here if you wish to have the internal AT91 UARTs
 	  appear as /dev/ttyATn (major 204, minor starting at 154)
 	  instead of the normal /dev/ttySn (major 4, minor starting at
 	  64). This is necessary if you also want other UARTs, such as
@@ -1688,6 +1688,25 @@ config SERIAL_MVEBU_CONSOLE
 	  and warnings and which allows logins in single user mode)
 	  Otherwise, say 'N'.
 
+config SERIAL_OWL
+	bool "Actions Semi Owl serial port support"
+	depends on ARCH_ACTIONS || COMPILE_TEST
+	select SERIAL_CORE
+	help
+	  This driver is for Actions Semiconductor S500/S900 SoC's UART.
+	  Say 'Y' here if you wish to use the on-board serial port.
+	  Otherwise, say 'N'.
+
+config SERIAL_OWL_CONSOLE
+	bool "Console on Actions Semi Owl serial port"
+	depends on SERIAL_OWL=y
+	select SERIAL_CORE_CONSOLE
+	select SERIAL_EARLYCON
+	default y
+	help
+	  Say 'Y' here if you wish to use Actions Semiconductor S500/S900 UART
+	  as the system console. Only earlycon is implemented currently.
+
 endmenu
 
 config SERIAL_MCTRL_GPIO
diff --git a/drivers/tty/serial/Makefile b/drivers/tty/serial/Makefile
index 53c03e005132..fe88a75d9a59 100644
--- a/drivers/tty/serial/Makefile
+++ b/drivers/tty/serial/Makefile
@@ -92,6 +92,7 @@ obj-$(CONFIG_SERIAL_STM32)	+= stm32-usart.o
 obj-$(CONFIG_SERIAL_MVEBU_UART)	+= mvebu-uart.o
 obj-$(CONFIG_SERIAL_PIC32)	+= pic32_uart.o
 obj-$(CONFIG_SERIAL_MPS2_UART)	+= mps2-uart.o
+obj-$(CONFIG_SERIAL_OWL)	+= owl-uart.o
 
 # GPIOLIB helpers for modem control lines
 obj-$(CONFIG_SERIAL_MCTRL_GPIO)	+= serial_mctrl_gpio.o
diff --git a/drivers/tty/serial/amba-pl010.c b/drivers/tty/serial/amba-pl010.c
index f2f251075109..24180adb1cbb 100644
--- a/drivers/tty/serial/amba-pl010.c
+++ b/drivers/tty/serial/amba-pl010.c
@@ -697,6 +697,7 @@ static struct console amba_console = {
 #define AMBA_CONSOLE	NULL
 #endif
 
+static DEFINE_MUTEX(amba_reg_lock);
 static struct uart_driver amba_reg = {
 	.owner			= THIS_MODULE,
 	.driver_name		= "ttyAM",
@@ -749,6 +750,19 @@ static int pl010_probe(struct amba_device *dev, const struct amba_id *id)
 	amba_ports[i] = uap;
 
 	amba_set_drvdata(dev, uap);
+
+	mutex_lock(&amba_reg_lock);
+	if (!amba_reg.state) {
+		ret = uart_register_driver(&amba_reg);
+		if (ret < 0) {
+			mutex_unlock(&amba_reg_lock);
+			dev_err(uap->port.dev,
+				"Failed to register AMBA-PL010 driver\n");
+			return ret;
+		}
+	}
+	mutex_unlock(&amba_reg_lock);
+
 	ret = uart_add_one_port(&amba_reg, &uap->port);
 	if (ret)
 		amba_ports[i] = NULL;
@@ -760,12 +774,18 @@ static int pl010_remove(struct amba_device *dev)
 {
 	struct uart_amba_port *uap = amba_get_drvdata(dev);
 	int i;
+	bool busy = false;
 
 	uart_remove_one_port(&amba_reg, &uap->port);
 
 	for (i = 0; i < ARRAY_SIZE(amba_ports); i++)
 		if (amba_ports[i] == uap)
 			amba_ports[i] = NULL;
+		else if (amba_ports[i])
+			busy = true;
+
+	if (!busy)
+		uart_unregister_driver(&amba_reg);
 
 	return 0;
 }
@@ -816,23 +836,14 @@ static struct amba_driver pl010_driver = {
 
 static int __init pl010_init(void)
 {
-	int ret;
-
 	printk(KERN_INFO "Serial: AMBA driver\n");
 
-	ret = uart_register_driver(&amba_reg);
-	if (ret == 0) {
-		ret = amba_driver_register(&pl010_driver);
-		if (ret)
-			uart_unregister_driver(&amba_reg);
-	}
-	return ret;
+	return  amba_driver_register(&pl010_driver);
 }
 
 static void __exit pl010_exit(void)
 {
 	amba_driver_unregister(&pl010_driver);
-	uart_unregister_driver(&amba_reg);
 }
 
 module_init(pl010_init);
diff --git a/drivers/tty/serial/atmel_serial.c b/drivers/tty/serial/atmel_serial.c
index c355ac9abafc..7551cab438ff 100644
--- a/drivers/tty/serial/atmel_serial.c
+++ b/drivers/tty/serial/atmel_serial.c
@@ -1,5 +1,5 @@
 /*
- *  Driver for Atmel AT91 / AT32 Serial ports
+ *  Driver for Atmel AT91 Serial ports
  *  Copyright (C) 2003 Rick Bronson
  *
  *  Based on drivers/char/serial_sa1100.c, by Deep Blue Solutions Ltd.
@@ -46,6 +46,7 @@
 #include <linux/err.h>
 #include <linux/irq.h>
 #include <linux/suspend.h>
+#include <linux/mm.h>
 
 #include <asm/io.h>
 #include <asm/ioctls.h>
@@ -118,7 +119,6 @@ struct atmel_uart_char {
 
 /*
  * at91: 6 USARTs and one DBGU port (SAM9260)
- * avr32: 4
  * samx7: 3 USARTs and 5 UARTs
  */
 #define ATMEL_MAX_UART		8
@@ -228,21 +228,6 @@ static inline void atmel_uart_writel(struct uart_port *port, u32 reg, u32 value)
 	__raw_writel(value, port->membase + reg);
 }
 
-#ifdef CONFIG_AVR32
-
-/* AVR32 cannot handle 8 or 16bit I/O accesses but only 32bit I/O accesses */
-static inline u8 atmel_uart_read_char(struct uart_port *port)
-{
-	return __raw_readl(port->membase + ATMEL_US_RHR);
-}
-
-static inline void atmel_uart_write_char(struct uart_port *port, u8 value)
-{
-	__raw_writel(value, port->membase + ATMEL_US_THR);
-}
-
-#else
-
 static inline u8 atmel_uart_read_char(struct uart_port *port)
 {
 	return __raw_readb(port->membase + ATMEL_US_RHR);
@@ -253,8 +238,6 @@ static inline void atmel_uart_write_char(struct uart_port *port, u8 value)
 	__raw_writeb(value, port->membase + ATMEL_US_THR);
 }
 
-#endif
-
 #ifdef CONFIG_SERIAL_ATMEL_PDC
 static bool atmel_use_pdc_rx(struct uart_port *port)
 {
@@ -959,7 +942,7 @@ static int atmel_prepare_tx_dma(struct uart_port *port)
 	sg_set_page(&atmel_port->sg_tx,
 			virt_to_page(port->state->xmit.buf),
 			UART_XMIT_SIZE,
-			(unsigned long)port->state->xmit.buf & ~PAGE_MASK);
+			offset_in_page(port->state->xmit.buf));
 	nent = dma_map_sg(port->dev,
 				&atmel_port->sg_tx,
 				1,
@@ -1141,7 +1124,7 @@ static int atmel_prepare_rx_dma(struct uart_port *port)
 	sg_set_page(&atmel_port->sg_rx,
 		    virt_to_page(ring->buf),
 		    sizeof(struct atmel_uart_char) * ATMEL_SERIAL_RINGSIZE,
-		    (unsigned long)ring->buf & ~PAGE_MASK);
+		    offset_in_page(ring->buf));
 	nent = dma_map_sg(port->dev,
 			  &atmel_port->sg_rx,
 			  1,
@@ -1655,72 +1638,56 @@ static void atmel_init_property(struct atmel_uart_port *atmel_port,
 				struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
-	struct atmel_uart_data *pdata = dev_get_platdata(&pdev->dev);
-
-	if (np) {
-		/* DMA/PDC usage specification */
-		if (of_property_read_bool(np, "atmel,use-dma-rx")) {
-			if (of_property_read_bool(np, "dmas")) {
-				atmel_port->use_dma_rx  = true;
-				atmel_port->use_pdc_rx  = false;
-			} else {
-				atmel_port->use_dma_rx  = false;
-				atmel_port->use_pdc_rx  = true;
-			}
+
+	/* DMA/PDC usage specification */
+	if (of_property_read_bool(np, "atmel,use-dma-rx")) {
+		if (of_property_read_bool(np, "dmas")) {
+			atmel_port->use_dma_rx  = true;
+			atmel_port->use_pdc_rx  = false;
 		} else {
 			atmel_port->use_dma_rx  = false;
-			atmel_port->use_pdc_rx  = false;
+			atmel_port->use_pdc_rx  = true;
 		}
+	} else {
+		atmel_port->use_dma_rx  = false;
+		atmel_port->use_pdc_rx  = false;
+	}
 
-		if (of_property_read_bool(np, "atmel,use-dma-tx")) {
-			if (of_property_read_bool(np, "dmas")) {
-				atmel_port->use_dma_tx  = true;
-				atmel_port->use_pdc_tx  = false;
-			} else {
-				atmel_port->use_dma_tx  = false;
-				atmel_port->use_pdc_tx  = true;
-			}
+	if (of_property_read_bool(np, "atmel,use-dma-tx")) {
+		if (of_property_read_bool(np, "dmas")) {
+			atmel_port->use_dma_tx  = true;
+			atmel_port->use_pdc_tx  = false;
 		} else {
 			atmel_port->use_dma_tx  = false;
-			atmel_port->use_pdc_tx  = false;
+			atmel_port->use_pdc_tx  = true;
 		}
-
 	} else {
-		atmel_port->use_pdc_rx  = pdata->use_dma_rx;
-		atmel_port->use_pdc_tx  = pdata->use_dma_tx;
-		atmel_port->use_dma_rx  = false;
 		atmel_port->use_dma_tx  = false;
+		atmel_port->use_pdc_tx  = false;
 	}
-
 }
 
 static void atmel_init_rs485(struct uart_port *port,
 				struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
-	struct atmel_uart_data *pdata = dev_get_platdata(&pdev->dev);
-
-	if (np) {
-		struct serial_rs485 *rs485conf = &port->rs485;
-		u32 rs485_delay[2];
-		/* rs485 properties */
-		if (of_property_read_u32_array(np, "rs485-rts-delay",
-					rs485_delay, 2) == 0) {
-			rs485conf->delay_rts_before_send = rs485_delay[0];
-			rs485conf->delay_rts_after_send = rs485_delay[1];
-			rs485conf->flags = 0;
-		}
 
-		if (of_get_property(np, "rs485-rx-during-tx", NULL))
-			rs485conf->flags |= SER_RS485_RX_DURING_TX;
+	struct serial_rs485 *rs485conf = &port->rs485;
+	u32 rs485_delay[2];
 
-		if (of_get_property(np, "linux,rs485-enabled-at-boot-time",
-								NULL))
-			rs485conf->flags |= SER_RS485_ENABLED;
-	} else {
-		port->rs485       = pdata->rs485;
+	/* rs485 properties */
+	if (of_property_read_u32_array(np, "rs485-rts-delay",
+				       rs485_delay, 2) == 0) {
+		rs485conf->delay_rts_before_send = rs485_delay[0];
+		rs485conf->delay_rts_after_send = rs485_delay[1];
+		rs485conf->flags = 0;
 	}
 
+	if (of_get_property(np, "rs485-rx-during-tx", NULL))
+		rs485conf->flags |= SER_RS485_RX_DURING_TX;
+
+	if (of_get_property(np, "linux,rs485-enabled-at-boot-time", NULL))
+		rs485conf->flags |= SER_RS485_ENABLED;
 }
 
 static void atmel_set_ops(struct uart_port *port)
@@ -2402,7 +2369,6 @@ static int atmel_init_port(struct atmel_uart_port *atmel_port,
 {
 	int ret;
 	struct uart_port *port = &atmel_port->uart;
-	struct atmel_uart_data *pdata = dev_get_platdata(&pdev->dev);
 
 	atmel_init_property(atmel_port, pdev);
 	atmel_set_ops(port);
@@ -2410,24 +2376,17 @@ static int atmel_init_port(struct atmel_uart_port *atmel_port,
 	atmel_init_rs485(port, pdev);
 
 	port->iotype		= UPIO_MEM;
-	port->flags		= UPF_BOOT_AUTOCONF;
+	port->flags		= UPF_BOOT_AUTOCONF | UPF_IOREMAP;
 	port->ops		= &atmel_pops;
 	port->fifosize		= 1;
 	port->dev		= &pdev->dev;
 	port->mapbase	= pdev->resource[0].start;
 	port->irq	= pdev->resource[1].start;
 	port->rs485_config	= atmel_config_rs485;
+	port->membase	= NULL;
 
 	memset(&atmel_port->rx_ring, 0, sizeof(atmel_port->rx_ring));
 
-	if (pdata && pdata->regs) {
-		/* Already mapped by setup code */
-		port->membase = pdata->regs;
-	} else {
-		port->flags	|= UPF_IOREMAP;
-		port->membase	= NULL;
-	}
-
 	/* for console, the clock could already be configured */
 	if (!atmel_port->clk) {
 		atmel_port->clk = clk_get(&pdev->dev, "usart");
@@ -2460,8 +2419,6 @@ static int atmel_init_port(struct atmel_uart_port *atmel_port,
 	return 0;
 }
 
-struct platform_device *atmel_default_console_device;	/* the serial console device */
-
 #ifdef CONFIG_SERIAL_ATMEL_CONSOLE
 static void atmel_console_putchar(struct uart_port *port, int ch)
 {
@@ -2594,47 +2551,6 @@ static struct console atmel_console = {
 
 #define ATMEL_CONSOLE_DEVICE	(&atmel_console)
 
-/*
- * Early console initialization (before VM subsystem initialized).
- */
-static int __init atmel_console_init(void)
-{
-	int ret;
-	if (atmel_default_console_device) {
-		struct atmel_uart_data *pdata =
-			dev_get_platdata(&atmel_default_console_device->dev);
-		int id = pdata->num;
-		struct atmel_uart_port *atmel_port = &atmel_ports[id];
-
-		atmel_port->backup_imr = 0;
-		atmel_port->uart.line = id;
-
-		add_preferred_console(ATMEL_DEVICENAME, id, NULL);
-		ret = atmel_init_port(atmel_port, atmel_default_console_device);
-		if (ret)
-			return ret;
-		register_console(&atmel_console);
-	}
-
-	return 0;
-}
-
-console_initcall(atmel_console_init);
-
-/*
- * Late console initialization.
- */
-static int __init atmel_late_console_init(void)
-{
-	if (atmel_default_console_device
-	    && !(atmel_console.flags & CON_ENABLED))
-		register_console(&atmel_console);
-
-	return 0;
-}
-
-core_initcall(atmel_late_console_init);
-
 static inline bool atmel_is_console_port(struct uart_port *port)
 {
 	return port->cons && port->cons->index == port->line;
@@ -2804,19 +2720,13 @@ static int atmel_serial_probe(struct platform_device *pdev)
 {
 	struct atmel_uart_port *atmel_port;
 	struct device_node *np = pdev->dev.of_node;
-	struct atmel_uart_data *pdata = dev_get_platdata(&pdev->dev);
 	void *data;
 	int ret = -ENODEV;
 	bool rs485_enabled;
 
 	BUILD_BUG_ON(ATMEL_SERIAL_RINGSIZE & (ATMEL_SERIAL_RINGSIZE - 1));
 
-	if (np)
-		ret = of_alias_get_id(np, "serial");
-	else
-		if (pdata)
-			ret = pdata->num;
-
+	ret = of_alias_get_id(np, "serial");
 	if (ret < 0)
 		/* port id not found in platform data nor device-tree aliases:
 		 * auto-enumerate it */
diff --git a/drivers/tty/serial/fsl_lpuart.c b/drivers/tty/serial/fsl_lpuart.c
index 15df1ba78095..343de8c384b0 100644
--- a/drivers/tty/serial/fsl_lpuart.c
+++ b/drivers/tty/serial/fsl_lpuart.c
@@ -140,6 +140,8 @@
 #define UARTBAUD_SBNS		0x00002000
 #define UARTBAUD_SBR		0x00000000
 #define UARTBAUD_SBR_MASK	0x1fff
+#define UARTBAUD_OSR_MASK       0x1f
+#define UARTBAUD_OSR_SHIFT      24
 
 #define UARTSTAT_LBKDIF		0x80000000
 #define UARTSTAT_RXEDGIF	0x40000000
@@ -231,12 +233,14 @@
 #define DEV_NAME	"ttyLP"
 #define UART_NR		6
 
+/* IMX lpuart has four extra unused regs located at the beginning */
+#define IMX_REG_OFF	0x10
+
 struct lpuart_port {
 	struct uart_port	port;
 	struct clk		*clk;
 	unsigned int		txfifo_size;
 	unsigned int		rxfifo_size;
-	bool			lpuart32;
 
 	bool			lpuart_dma_tx_use;
 	bool			lpuart_dma_rx_use;
@@ -258,13 +262,28 @@ struct lpuart_port {
 	wait_queue_head_t	dma_wait;
 };
 
+struct lpuart_soc_data {
+	char	iotype;
+	u8	reg_off;
+};
+
+static const struct lpuart_soc_data vf_data = {
+	.iotype = UPIO_MEM,
+};
+
+static const struct lpuart_soc_data ls_data = {
+	.iotype = UPIO_MEM32BE,
+};
+
+static struct lpuart_soc_data imx_data = {
+	.iotype = UPIO_MEM32,
+	.reg_off = IMX_REG_OFF,
+};
+
 static const struct of_device_id lpuart_dt_ids[] = {
-	{
-		.compatible = "fsl,vf610-lpuart",
-	},
-	{
-		.compatible = "fsl,ls1021a-lpuart",
-	},
+	{ .compatible = "fsl,vf610-lpuart",	.data = &vf_data, },
+	{ .compatible = "fsl,ls1021a-lpuart",	.data = &ls_data, },
+	{ .compatible = "fsl,imx7ulp-lpuart",	.data = &imx_data, },
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, lpuart_dt_ids);
@@ -272,14 +291,29 @@ MODULE_DEVICE_TABLE(of, lpuart_dt_ids);
 /* Forward declare this for the dma callbacks*/
 static void lpuart_dma_tx_complete(void *arg);
 
-static u32 lpuart32_read(void __iomem *addr)
+static inline u32 lpuart32_read(struct uart_port *port, u32 off)
 {
-	return ioread32be(addr);
+	switch (port->iotype) {
+	case UPIO_MEM32:
+		return readl(port->membase + off);
+	case UPIO_MEM32BE:
+		return ioread32be(port->membase + off);
+	default:
+		return 0;
+	}
 }
 
-static void lpuart32_write(u32 val, void __iomem *addr)
+static inline void lpuart32_write(struct uart_port *port, u32 val,
+				  u32 off)
 {
-	iowrite32be(val, addr);
+	switch (port->iotype) {
+	case UPIO_MEM32:
+		writel(val, port->membase + off);
+		break;
+	case UPIO_MEM32BE:
+		iowrite32be(val, port->membase + off);
+		break;
+	}
 }
 
 static void lpuart_stop_tx(struct uart_port *port)
@@ -295,9 +329,9 @@ static void lpuart32_stop_tx(struct uart_port *port)
 {
 	unsigned long temp;
 
-	temp = lpuart32_read(port->membase + UARTCTRL);
+	temp = lpuart32_read(port, UARTCTRL);
 	temp &= ~(UARTCTRL_TIE | UARTCTRL_TCIE);
-	lpuart32_write(temp, port->membase + UARTCTRL);
+	lpuart32_write(port, temp, UARTCTRL);
 }
 
 static void lpuart_stop_rx(struct uart_port *port)
@@ -312,8 +346,8 @@ static void lpuart32_stop_rx(struct uart_port *port)
 {
 	unsigned long temp;
 
-	temp = lpuart32_read(port->membase + UARTCTRL);
-	lpuart32_write(temp & ~UARTCTRL_RE, port->membase + UARTCTRL);
+	temp = lpuart32_read(port, UARTCTRL);
+	lpuart32_write(port, temp & ~UARTCTRL_RE, UARTCTRL);
 }
 
 static void lpuart_dma_tx(struct lpuart_port *sport)
@@ -512,14 +546,14 @@ static inline void lpuart32_transmit_buffer(struct lpuart_port *sport)
 	struct circ_buf *xmit = &sport->port.state->xmit;
 	unsigned long txcnt;
 
-	txcnt = lpuart32_read(sport->port.membase + UARTWATER);
+	txcnt = lpuart32_read(&sport->port, UARTWATER);
 	txcnt = txcnt >> UARTWATER_TXCNT_OFF;
 	txcnt &= UARTWATER_COUNT_MASK;
 	while (!uart_circ_empty(xmit) && (txcnt < sport->txfifo_size)) {
-		lpuart32_write(xmit->buf[xmit->tail], sport->port.membase + UARTDATA);
+		lpuart32_write(&sport->port, xmit->buf[xmit->tail], UARTDATA);
 		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
 		sport->port.icount.tx++;
-		txcnt = lpuart32_read(sport->port.membase + UARTWATER);
+		txcnt = lpuart32_read(&sport->port, UARTWATER);
 		txcnt = txcnt >> UARTWATER_TXCNT_OFF;
 		txcnt &= UARTWATER_COUNT_MASK;
 	}
@@ -555,10 +589,10 @@ static void lpuart32_start_tx(struct uart_port *port)
 	struct lpuart_port *sport = container_of(port, struct lpuart_port, port);
 	unsigned long temp;
 
-	temp = lpuart32_read(port->membase + UARTCTRL);
-	lpuart32_write(temp | UARTCTRL_TIE, port->membase + UARTCTRL);
+	temp = lpuart32_read(port, UARTCTRL);
+	lpuart32_write(port, temp | UARTCTRL_TIE, UARTCTRL);
 
-	if (lpuart32_read(port->membase + UARTSTAT) & UARTSTAT_TDRE)
+	if (lpuart32_read(port, UARTSTAT) & UARTSTAT_TDRE)
 		lpuart32_transmit_buffer(sport);
 }
 
@@ -581,7 +615,7 @@ static unsigned int lpuart_tx_empty(struct uart_port *port)
 
 static unsigned int lpuart32_tx_empty(struct uart_port *port)
 {
-	return (lpuart32_read(port->membase + UARTSTAT) & UARTSTAT_TC) ?
+	return (lpuart32_read(port, UARTSTAT) & UARTSTAT_TC) ?
 		TIOCSER_TEMT : 0;
 }
 
@@ -593,22 +627,22 @@ static irqreturn_t lpuart_txint(int irq, void *dev_id)
 
 	spin_lock_irqsave(&sport->port.lock, flags);
 	if (sport->port.x_char) {
-		if (sport->lpuart32)
-			lpuart32_write(sport->port.x_char, sport->port.membase + UARTDATA);
+		if (sport->port.iotype & (UPIO_MEM32 | UPIO_MEM32BE))
+			lpuart32_write(&sport->port, sport->port.x_char, UARTDATA);
 		else
 			writeb(sport->port.x_char, sport->port.membase + UARTDR);
 		goto out;
 	}
 
 	if (uart_circ_empty(xmit) || uart_tx_stopped(&sport->port)) {
-		if (sport->lpuart32)
+		if (sport->port.iotype & (UPIO_MEM32 | UPIO_MEM32BE))
 			lpuart32_stop_tx(&sport->port);
 		else
 			lpuart_stop_tx(&sport->port);
 		goto out;
 	}
 
-	if (sport->lpuart32)
+	if (sport->port.iotype & (UPIO_MEM32 | UPIO_MEM32BE))
 		lpuart32_transmit_buffer(sport);
 	else
 		lpuart_transmit_buffer(sport);
@@ -694,15 +728,15 @@ static irqreturn_t lpuart32_rxint(int irq, void *dev_id)
 
 	spin_lock_irqsave(&sport->port.lock, flags);
 
-	while (!(lpuart32_read(sport->port.membase + UARTFIFO) & UARTFIFO_RXEMPT)) {
+	while (!(lpuart32_read(&sport->port, UARTFIFO) & UARTFIFO_RXEMPT)) {
 		flg = TTY_NORMAL;
 		sport->port.icount.rx++;
 		/*
 		 * to clear the FE, OR, NF, FE, PE flags,
 		 * read STAT then read DATA reg
 		 */
-		sr = lpuart32_read(sport->port.membase + UARTSTAT);
-		rx = lpuart32_read(sport->port.membase + UARTDATA);
+		sr = lpuart32_read(&sport->port, UARTSTAT);
+		rx = lpuart32_read(&sport->port, UARTDATA);
 		rx &= 0x3ff;
 
 		if (uart_handle_sysrq_char(&sport->port, (unsigned char)rx))
@@ -769,18 +803,18 @@ static irqreturn_t lpuart32_int(int irq, void *dev_id)
 	struct lpuart_port *sport = dev_id;
 	unsigned long sts, rxcount;
 
-	sts = lpuart32_read(sport->port.membase + UARTSTAT);
-	rxcount = lpuart32_read(sport->port.membase + UARTWATER);
+	sts = lpuart32_read(&sport->port, UARTSTAT);
+	rxcount = lpuart32_read(&sport->port, UARTWATER);
 	rxcount = rxcount >> UARTWATER_RXCNT_OFF;
 
 	if (sts & UARTSTAT_RDRF || rxcount > 0)
 		lpuart32_rxint(irq, dev_id);
 
 	if ((sts & UARTSTAT_TDRE) &&
-		!(lpuart32_read(sport->port.membase + UARTBAUD) & UARTBAUD_TDMAE))
+		!(lpuart32_read(&sport->port, UARTBAUD) & UARTBAUD_TDMAE))
 		lpuart_txint(irq, dev_id);
 
-	lpuart32_write(sts, sport->port.membase + UARTSTAT);
+	lpuart32_write(&sport->port, sts, UARTSTAT);
 	return IRQ_HANDLED;
 }
 
@@ -1041,7 +1075,7 @@ static unsigned int lpuart32_get_mctrl(struct uart_port *port)
 	unsigned int temp = 0;
 	unsigned long reg;
 
-	reg = lpuart32_read(port->membase + UARTMODIR);
+	reg = lpuart32_read(port, UARTMODIR);
 	if (reg & UARTMODIR_TXCTSE)
 		temp |= TIOCM_CTS;
 
@@ -1076,7 +1110,7 @@ static void lpuart32_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
 	unsigned long temp;
 
-	temp = lpuart32_read(port->membase + UARTMODIR) &
+	temp = lpuart32_read(port, UARTMODIR) &
 			~(UARTMODIR_RXRTSE | UARTMODIR_TXCTSE);
 
 	if (mctrl & TIOCM_RTS)
@@ -1085,7 +1119,7 @@ static void lpuart32_set_mctrl(struct uart_port *port, unsigned int mctrl)
 	if (mctrl & TIOCM_CTS)
 		temp |= UARTMODIR_TXCTSE;
 
-	lpuart32_write(temp, port->membase + UARTMODIR);
+	lpuart32_write(port, temp, UARTMODIR);
 }
 
 static void lpuart_break_ctl(struct uart_port *port, int break_state)
@@ -1104,12 +1138,12 @@ static void lpuart32_break_ctl(struct uart_port *port, int break_state)
 {
 	unsigned long temp;
 
-	temp = lpuart32_read(port->membase + UARTCTRL) & ~UARTCTRL_SBK;
+	temp = lpuart32_read(port, UARTCTRL) & ~UARTCTRL_SBK;
 
 	if (break_state != 0)
 		temp |= UARTCTRL_SBK;
 
-	lpuart32_write(temp, port->membase + UARTCTRL);
+	lpuart32_write(port, temp, UARTCTRL);
 }
 
 static void lpuart_setup_watermark(struct lpuart_port *sport)
@@ -1149,24 +1183,24 @@ static void lpuart32_setup_watermark(struct lpuart_port *sport)
 	unsigned long val, ctrl;
 	unsigned long ctrl_saved;
 
-	ctrl = lpuart32_read(sport->port.membase + UARTCTRL);
+	ctrl = lpuart32_read(&sport->port, UARTCTRL);
 	ctrl_saved = ctrl;
 	ctrl &= ~(UARTCTRL_TIE | UARTCTRL_TCIE | UARTCTRL_TE |
 			UARTCTRL_RIE | UARTCTRL_RE);
-	lpuart32_write(ctrl, sport->port.membase + UARTCTRL);
+	lpuart32_write(&sport->port, ctrl, UARTCTRL);
 
 	/* enable FIFO mode */
-	val = lpuart32_read(sport->port.membase + UARTFIFO);
+	val = lpuart32_read(&sport->port, UARTFIFO);
 	val |= UARTFIFO_TXFE | UARTFIFO_RXFE;
 	val |= UARTFIFO_TXFLUSH | UARTFIFO_RXFLUSH;
-	lpuart32_write(val, sport->port.membase + UARTFIFO);
+	lpuart32_write(&sport->port, val, UARTFIFO);
 
 	/* set the watermark */
 	val = (0x1 << UARTWATER_RXWATER_OFF) | (0x0 << UARTWATER_TXWATER_OFF);
-	lpuart32_write(val, sport->port.membase + UARTWATER);
+	lpuart32_write(&sport->port, val, UARTWATER);
 
 	/* Restore cr2 */
-	lpuart32_write(ctrl_saved, sport->port.membase + UARTCTRL);
+	lpuart32_write(&sport->port, ctrl_saved, UARTCTRL);
 }
 
 static void rx_dma_timer_init(struct lpuart_port *sport)
@@ -1242,7 +1276,7 @@ static int lpuart32_startup(struct uart_port *port)
 	unsigned long temp;
 
 	/* determine FIFO size */
-	temp = lpuart32_read(sport->port.membase + UARTFIFO);
+	temp = lpuart32_read(&sport->port, UARTFIFO);
 
 	sport->txfifo_size = 0x1 << (((temp >> UARTFIFO_TXSIZE_OFF) &
 		UARTFIFO_FIFOSIZE_MASK) - 1);
@@ -1259,10 +1293,10 @@ static int lpuart32_startup(struct uart_port *port)
 
 	lpuart32_setup_watermark(sport);
 
-	temp = lpuart32_read(sport->port.membase + UARTCTRL);
+	temp = lpuart32_read(&sport->port, UARTCTRL);
 	temp |= (UARTCTRL_RIE | UARTCTRL_TIE | UARTCTRL_RE | UARTCTRL_TE);
 	temp |= UARTCTRL_ILIE;
-	lpuart32_write(temp, sport->port.membase + UARTCTRL);
+	lpuart32_write(&sport->port, temp, UARTCTRL);
 
 	spin_unlock_irqrestore(&sport->port.lock, flags);
 	return 0;
@@ -1311,10 +1345,10 @@ static void lpuart32_shutdown(struct uart_port *port)
 	spin_lock_irqsave(&port->lock, flags);
 
 	/* disable Rx/Tx and interrupts */
-	temp = lpuart32_read(port->membase + UARTCTRL);
+	temp = lpuart32_read(port, UARTCTRL);
 	temp &= ~(UARTCTRL_TE | UARTCTRL_RE |
 			UARTCTRL_TIE | UARTCTRL_TCIE | UARTCTRL_RIE);
-	lpuart32_write(temp, port->membase + UARTCTRL);
+	lpuart32_write(port, temp, UARTCTRL);
 
 	spin_unlock_irqrestore(&port->lock, flags);
 
@@ -1479,6 +1513,75 @@ lpuart_set_termios(struct uart_port *port, struct ktermios *termios,
 }
 
 static void
+lpuart32_serial_setbrg(struct lpuart_port *sport, unsigned int baudrate)
+{
+	u32 sbr, osr, baud_diff, tmp_osr, tmp_sbr, tmp_diff, tmp;
+	u32 clk = sport->port.uartclk;
+
+	/*
+	 * The idea is to use the best OSR (over-sampling rate) possible.
+	 * Note, OSR is typically hard-set to 16 in other LPUART instantiations.
+	 * Loop to find the best OSR value possible, one that generates minimum
+	 * baud_diff iterate through the rest of the supported values of OSR.
+	 *
+	 * Calculation Formula:
+	 *  Baud Rate = baud clock / ((OSR+1) × SBR)
+	 */
+	baud_diff = baudrate;
+	osr = 0;
+	sbr = 0;
+
+	for (tmp_osr = 4; tmp_osr <= 32; tmp_osr++) {
+		/* calculate the temporary sbr value  */
+		tmp_sbr = (clk / (baudrate * tmp_osr));
+		if (tmp_sbr == 0)
+			tmp_sbr = 1;
+
+		/*
+		 * calculate the baud rate difference based on the temporary
+		 * osr and sbr values
+		 */
+		tmp_diff = clk / (tmp_osr * tmp_sbr) - baudrate;
+
+		/* select best values between sbr and sbr+1 */
+		tmp = clk / (tmp_osr * (tmp_sbr + 1));
+		if (tmp_diff > (baudrate - tmp)) {
+			tmp_diff = baudrate - tmp;
+			tmp_sbr++;
+		}
+
+		if (tmp_diff <= baud_diff) {
+			baud_diff = tmp_diff;
+			osr = tmp_osr;
+			sbr = tmp_sbr;
+
+			if (!baud_diff)
+				break;
+		}
+	}
+
+	/* handle buadrate outside acceptable rate */
+	if (baud_diff > ((baudrate / 100) * 3))
+		dev_warn(sport->port.dev,
+			 "unacceptable baud rate difference of more than 3%%\n");
+
+	tmp = lpuart32_read(&sport->port, UARTBAUD);
+
+	if ((osr > 3) && (osr < 8))
+		tmp |= UARTBAUD_BOTHEDGE;
+
+	tmp &= ~(UARTBAUD_OSR_MASK << UARTBAUD_OSR_SHIFT);
+	tmp |= (((osr-1) & UARTBAUD_OSR_MASK) << UARTBAUD_OSR_SHIFT);
+
+	tmp &= ~UARTBAUD_SBR_MASK;
+	tmp |= sbr & UARTBAUD_SBR_MASK;
+
+	tmp &= ~(UARTBAUD_TDMAE | UARTBAUD_RDMAE);
+
+	lpuart32_write(&sport->port, tmp, UARTBAUD);
+}
+
+static void
 lpuart32_set_termios(struct uart_port *port, struct ktermios *termios,
 		   struct ktermios *old)
 {
@@ -1487,11 +1590,10 @@ lpuart32_set_termios(struct uart_port *port, struct ktermios *termios,
 	unsigned long ctrl, old_ctrl, bd, modem;
 	unsigned int  baud;
 	unsigned int old_csize = old ? old->c_cflag & CSIZE : CS8;
-	unsigned int sbr;
 
-	ctrl = old_ctrl = lpuart32_read(sport->port.membase + UARTCTRL);
-	bd = lpuart32_read(sport->port.membase + UARTBAUD);
-	modem = lpuart32_read(sport->port.membase + UARTMODIR);
+	ctrl = old_ctrl = lpuart32_read(&sport->port, UARTCTRL);
+	bd = lpuart32_read(&sport->port, UARTBAUD);
+	modem = lpuart32_read(&sport->port, UARTMODIR);
 	/*
 	 * only support CS8 and CS7, and for CS7 must enable PE.
 	 * supported mode:
@@ -1577,21 +1679,16 @@ lpuart32_set_termios(struct uart_port *port, struct ktermios *termios,
 	uart_update_timeout(port, termios->c_cflag, baud);
 
 	/* wait transmit engin complete */
-	while (!(lpuart32_read(sport->port.membase + UARTSTAT) & UARTSTAT_TC))
+	while (!(lpuart32_read(&sport->port, UARTSTAT) & UARTSTAT_TC))
 		barrier();
 
 	/* disable transmit and receive */
-	lpuart32_write(old_ctrl & ~(UARTCTRL_TE | UARTCTRL_RE),
-			sport->port.membase + UARTCTRL);
+	lpuart32_write(&sport->port, old_ctrl & ~(UARTCTRL_TE | UARTCTRL_RE),
+		       UARTCTRL);
 
-	sbr = sport->port.uartclk / (16 * baud);
-	bd &= ~UARTBAUD_SBR_MASK;
-	bd |= sbr & UARTBAUD_SBR_MASK;
-	bd |= UARTBAUD_BOTHEDGE;
-	bd &= ~(UARTBAUD_TDMAE | UARTBAUD_RDMAE);
-	lpuart32_write(bd, sport->port.membase + UARTBAUD);
-	lpuart32_write(modem, sport->port.membase + UARTMODIR);
-	lpuart32_write(ctrl, sport->port.membase + UARTCTRL);
+	lpuart32_serial_setbrg(sport, baud);
+	lpuart32_write(&sport->port, modem, UARTMODIR);
+	lpuart32_write(&sport->port, ctrl, UARTCTRL);
 	/* restore control register */
 
 	spin_unlock_irqrestore(&sport->port.lock, flags);
@@ -1694,10 +1791,10 @@ static void lpuart_console_putchar(struct uart_port *port, int ch)
 
 static void lpuart32_console_putchar(struct uart_port *port, int ch)
 {
-	while (!(lpuart32_read(port->membase + UARTSTAT) & UARTSTAT_TDRE))
+	while (!(lpuart32_read(port, UARTSTAT) & UARTSTAT_TDRE))
 		barrier();
 
-	lpuart32_write(ch, port->membase + UARTDATA);
+	lpuart32_write(port, ch, UARTDATA);
 }
 
 static void
@@ -1745,18 +1842,18 @@ lpuart32_console_write(struct console *co, const char *s, unsigned int count)
 		spin_lock_irqsave(&sport->port.lock, flags);
 
 	/* first save CR2 and then disable interrupts */
-	cr = old_cr = lpuart32_read(sport->port.membase + UARTCTRL);
+	cr = old_cr = lpuart32_read(&sport->port, UARTCTRL);
 	cr |= (UARTCTRL_TE |  UARTCTRL_RE);
 	cr &= ~(UARTCTRL_TIE | UARTCTRL_TCIE | UARTCTRL_RIE);
-	lpuart32_write(cr, sport->port.membase + UARTCTRL);
+	lpuart32_write(&sport->port, cr, UARTCTRL);
 
 	uart_console_write(&sport->port, s, count, lpuart32_console_putchar);
 
 	/* wait for transmitter finish complete and restore CR2 */
-	while (!(lpuart32_read(sport->port.membase + UARTSTAT) & UARTSTAT_TC))
+	while (!(lpuart32_read(&sport->port, UARTSTAT) & UARTSTAT_TC))
 		barrier();
 
-	lpuart32_write(old_cr, sport->port.membase + UARTCTRL);
+	lpuart32_write(&sport->port, old_cr, UARTCTRL);
 
 	if (locked)
 		spin_unlock_irqrestore(&sport->port.lock, flags);
@@ -1822,14 +1919,14 @@ lpuart32_console_get_options(struct lpuart_port *sport, int *baud,
 	unsigned long cr, bd;
 	unsigned int sbr, uartclk, baud_raw;
 
-	cr = lpuart32_read(sport->port.membase + UARTCTRL);
+	cr = lpuart32_read(&sport->port, UARTCTRL);
 	cr &= UARTCTRL_TE | UARTCTRL_RE;
 	if (!cr)
 		return;
 
 	/* ok, the port was enabled */
 
-	cr = lpuart32_read(sport->port.membase + UARTCTRL);
+	cr = lpuart32_read(&sport->port, UARTCTRL);
 
 	*parity = 'n';
 	if (cr & UARTCTRL_PE) {
@@ -1844,7 +1941,7 @@ lpuart32_console_get_options(struct lpuart_port *sport, int *baud,
 	else
 		*bits = 8;
 
-	bd = lpuart32_read(sport->port.membase + UARTBAUD);
+	bd = lpuart32_read(&sport->port, UARTBAUD);
 	bd &= UARTBAUD_SBR_MASK;
 	sbr = bd;
 	uartclk = clk_get_rate(sport->clk);
@@ -1881,12 +1978,12 @@ static int __init lpuart_console_setup(struct console *co, char *options)
 	if (options)
 		uart_parse_options(options, &baud, &parity, &bits, &flow);
 	else
-		if (sport->lpuart32)
+		if (sport->port.iotype & (UPIO_MEM32 | UPIO_MEM32BE))
 			lpuart32_console_get_options(sport, &baud, &parity, &bits);
 		else
 			lpuart_console_get_options(sport, &baud, &parity, &bits);
 
-	if (sport->lpuart32)
+	if (sport->port.iotype & (UPIO_MEM32 | UPIO_MEM32BE))
 		lpuart32_setup_watermark(sport);
 	else
 		lpuart_setup_watermark(sport);
@@ -1945,12 +2042,26 @@ static int __init lpuart32_early_console_setup(struct earlycon_device *device,
 	if (!device->port.membase)
 		return -ENODEV;
 
+	device->port.iotype = UPIO_MEM32BE;
 	device->con->write = lpuart32_early_write;
 	return 0;
 }
 
+static int __init lpuart32_imx_early_console_setup(struct earlycon_device *device,
+						   const char *opt)
+{
+	if (!device->port.membase)
+		return -ENODEV;
+
+	device->port.iotype = UPIO_MEM32;
+	device->port.membase += IMX_REG_OFF;
+	device->con->write = lpuart32_early_write;
+
+	return 0;
+}
 OF_EARLYCON_DECLARE(lpuart, "fsl,vf610-lpuart", lpuart_early_console_setup);
 OF_EARLYCON_DECLARE(lpuart32, "fsl,ls1021a-lpuart", lpuart32_early_console_setup);
+OF_EARLYCON_DECLARE(lpuart32, "fsl,imx7ulp-lpuart", lpuart32_imx_early_console_setup);
 EARLYCON_DECLARE(lpuart, lpuart_early_console_setup);
 EARLYCON_DECLARE(lpuart32, lpuart32_early_console_setup);
 
@@ -1971,6 +2082,9 @@ static struct uart_driver lpuart_reg = {
 
 static int lpuart_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *of_id = of_match_device(lpuart_dt_ids,
+							   &pdev->dev);
+	const struct lpuart_soc_data *sdata = of_id->data;
 	struct device_node *np = pdev->dev.of_node;
 	struct lpuart_port *sport;
 	struct resource *res;
@@ -1988,25 +2102,23 @@ static int lpuart_probe(struct platform_device *pdev)
 		return ret;
 	}
 	sport->port.line = ret;
-	sport->lpuart32 = of_device_is_compatible(np, "fsl,ls1021a-lpuart");
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	sport->port.membase = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(sport->port.membase))
 		return PTR_ERR(sport->port.membase);
 
+	sport->port.membase += sdata->reg_off;
 	sport->port.mapbase = res->start;
 	sport->port.dev = &pdev->dev;
 	sport->port.type = PORT_LPUART;
-	sport->port.iotype = UPIO_MEM;
 	ret = platform_get_irq(pdev, 0);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "cannot obtain irq\n");
 		return ret;
 	}
 	sport->port.irq = ret;
-
-	if (sport->lpuart32)
+	sport->port.iotype = sdata->iotype;
+	if (sport->port.iotype & (UPIO_MEM32 | UPIO_MEM32BE))
 		sport->port.ops = &lpuart32_pops;
 	else
 		sport->port.ops = &lpuart_pops;
@@ -2033,7 +2145,7 @@ static int lpuart_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, &sport->port);
 
-	if (sport->lpuart32)
+	if (sport->port.iotype & (UPIO_MEM32 | UPIO_MEM32BE))
 		lpuart_reg.cons = LPUART32_CONSOLE;
 	else
 		lpuart_reg.cons = LPUART_CONSOLE;
@@ -2086,11 +2198,11 @@ static int lpuart_suspend(struct device *dev)
 	struct lpuart_port *sport = dev_get_drvdata(dev);
 	unsigned long temp;
 
-	if (sport->lpuart32) {
+	if (sport->port.iotype & (UPIO_MEM32 | UPIO_MEM32BE)) {
 		/* disable Rx/Tx and interrupts */
-		temp = lpuart32_read(sport->port.membase + UARTCTRL);
+		temp = lpuart32_read(&sport->port, UARTCTRL);
 		temp &= ~(UARTCTRL_TE | UARTCTRL_TIE | UARTCTRL_TCIE);
-		lpuart32_write(temp, sport->port.membase + UARTCTRL);
+		lpuart32_write(&sport->port, temp, UARTCTRL);
 	} else {
 		/* disable Rx/Tx and interrupts */
 		temp = readb(sport->port.membase + UARTCR2);
@@ -2137,12 +2249,12 @@ static int lpuart_resume(struct device *dev)
 	if (sport->port.suspended && !sport->port.irq_wake)
 		clk_prepare_enable(sport->clk);
 
-	if (sport->lpuart32) {
+	if (sport->port.iotype & (UPIO_MEM32 | UPIO_MEM32BE)) {
 		lpuart32_setup_watermark(sport);
-		temp = lpuart32_read(sport->port.membase + UARTCTRL);
+		temp = lpuart32_read(&sport->port, UARTCTRL);
 		temp |= (UARTCTRL_RIE | UARTCTRL_TIE | UARTCTRL_RE |
 			 UARTCTRL_TE | UARTCTRL_ILIE);
-		lpuart32_write(temp, sport->port.membase + UARTCTRL);
+		lpuart32_write(&sport->port, temp, UARTCTRL);
 	} else {
 		lpuart_setup_watermark(sport);
 		temp = readb(sport->port.membase + UARTCR2);
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index bbefddd92bfe..9e3162bf3bd1 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -186,6 +186,11 @@
 
 #define UART_NR 8
 
+/* RX DMA buffer periods */
+#define RX_DMA_PERIODS 4
+#define RX_BUF_SIZE	(PAGE_SIZE)
+
+
 /* i.MX21 type uart runs on all i.mx except i.MX1 and i.MX6q */
 enum imx_uart_type {
 	IMX1_UART,
@@ -207,9 +212,6 @@ struct imx_port {
 	unsigned int		have_rtscts:1;
 	unsigned int		have_rtsgpio:1;
 	unsigned int		dte_mode:1;
-	unsigned int		irda_inv_rx:1;
-	unsigned int		irda_inv_tx:1;
-	unsigned short		trcv_delay; /* transceiver delay */
 	struct clk		*clk_ipg;
 	struct clk		*clk_per;
 	const struct imx_uart_data *devdata;
@@ -224,6 +226,7 @@ struct imx_port {
 	struct dma_chan		*dma_chan_rx, *dma_chan_tx;
 	struct scatterlist	rx_sgl, tx_sgl[2];
 	void			*rx_buf;
+	unsigned int		rx_buf_size;
 	struct circ_buf		rx_ring;
 	unsigned int		rx_periods;
 	dma_cookie_t		rx_cookie;
@@ -964,8 +967,6 @@ static void imx_timeout(unsigned long data)
 	}
 }
 
-#define RX_BUF_SIZE	(PAGE_SIZE)
-
 /*
  * There are two kinds of RX DMA interrupts(such as in the MX6Q):
  *   [1] the RX DMA buffer is full.
@@ -1048,9 +1049,6 @@ static void dma_rx_callback(void *data)
 	}
 }
 
-/* RX DMA buffer periods */
-#define RX_DMA_PERIODS 4
-
 static int start_rx_dma(struct imx_port *sport)
 {
 	struct scatterlist *sgl = &sport->rx_sgl;
@@ -1061,9 +1059,8 @@ static int start_rx_dma(struct imx_port *sport)
 
 	sport->rx_ring.head = 0;
 	sport->rx_ring.tail = 0;
-	sport->rx_periods = RX_DMA_PERIODS;
 
-	sg_init_one(sgl, sport->rx_buf, RX_BUF_SIZE);
+	sg_init_one(sgl, sport->rx_buf, sport->rx_buf_size);
 	ret = dma_map_sg(dev, sgl, 1, DMA_FROM_DEVICE);
 	if (ret == 0) {
 		dev_err(dev, "DMA mapping error for RX.\n");
@@ -1174,7 +1171,7 @@ static int imx_uart_dma_init(struct imx_port *sport)
 		goto err;
 	}
 
-	sport->rx_buf = kzalloc(PAGE_SIZE, GFP_KERNEL);
+	sport->rx_buf = kzalloc(sport->rx_buf_size, GFP_KERNEL);
 	if (!sport->rx_buf) {
 		ret = -ENOMEM;
 		goto err;
@@ -1302,7 +1299,9 @@ static int imx_startup(struct uart_port *port)
 		imx_enable_dma(sport);
 
 	temp = readl(sport->port.membase + UCR1);
-	temp |= UCR1_RRDYEN | UCR1_RTSDEN | UCR1_UARTEN;
+	temp |= UCR1_RRDYEN | UCR1_UARTEN;
+	if (sport->have_rtscts)
+			temp |= UCR1_RTSDEN;
 
 	writel(temp, sport->port.membase + UCR1);
 
@@ -1340,29 +1339,13 @@ static int imx_startup(struct uart_port *port)
 	imx_enable_ms(&sport->port);
 
 	/*
-	 * If the serial port is opened for reading start RX DMA immediately
-	 * instead of waiting for RX FIFO interrupts. In our iMX53 the average
-	 * delay for the first reception dropped from approximately 35000
-	 * microseconds to 1000 microseconds.
+	 * Start RX DMA immediately instead of waiting for RX FIFO interrupts.
+	 * In our iMX53 the average delay for the first reception dropped from
+	 * approximately 35000 microseconds to 1000 microseconds.
 	 */
 	if (sport->dma_is_enabled) {
-		struct tty_struct *tty = sport->port.state->port.tty;
-		struct tty_file_private *file_priv;
-		int readcnt = 0;
-
-		spin_lock(&tty->files_lock);
-
-		if (!list_empty(&tty->tty_files))
-			list_for_each_entry(file_priv, &tty->tty_files, list)
-				if (!(file_priv->file->f_flags & O_WRONLY))
-					readcnt++;
-
-		spin_unlock(&tty->files_lock);
-
-		if (readcnt > 0) {
-			imx_disable_rx_int(sport);
-			start_rx_dma(sport);
-		}
+		imx_disable_rx_int(sport);
+		start_rx_dma(sport);
 	}
 
 	spin_unlock_irqrestore(&sport->port.lock, flags);
@@ -2053,6 +2036,7 @@ static int serial_imx_probe_dt(struct imx_port *sport,
 {
 	struct device_node *np = pdev->dev.of_node;
 	int ret;
+	u32 dma_buf_size[2];
 
 	sport->devdata = of_device_get_match_data(&pdev->dev);
 	if (!sport->devdata)
@@ -2076,6 +2060,14 @@ static int serial_imx_probe_dt(struct imx_port *sport,
 	if (of_get_property(np, "rts-gpios", NULL))
 		sport->have_rtsgpio = 1;
 
+	if (!of_property_read_u32_array(np, "fsl,dma-size", dma_buf_size, 2)) {
+		sport->rx_buf_size = dma_buf_size[0] * dma_buf_size[1];
+		sport->rx_periods = dma_buf_size[1];
+	} else {
+		sport->rx_buf_size = RX_BUF_SIZE;
+		sport->rx_periods = RX_DMA_PERIODS;
+	}
+
 	return 0;
 }
 #else
diff --git a/drivers/tty/serial/meson_uart.c b/drivers/tty/serial/meson_uart.c
index 60f16795d16b..42e4a4c7597f 100644
--- a/drivers/tty/serial/meson_uart.c
+++ b/drivers/tty/serial/meson_uart.c
@@ -286,7 +286,7 @@ static int meson_uart_startup(struct uart_port *port)
 	writel(val, port->membase + AML_UART_MISC);
 
 	ret = request_irq(port->irq, meson_uart_interrupt, 0,
-			  meson_uart_type(port), port);
+			  port->name, port);
 
 	return ret;
 }
@@ -298,8 +298,6 @@ static void meson_uart_change_speed(struct uart_port *port, unsigned long baud)
 	while (!meson_uart_tx_empty(port))
 		cpu_relax();
 
-	val = readl(port->membase + AML_UART_REG5);
-	val &= ~AML_UART_BAUD_MASK;
 	if (port->uartclk == 24000000) {
 		val = ((port->uartclk / 3) / baud) - 1;
 		val |= AML_UART_BAUD_XTAL;
@@ -355,7 +353,7 @@ static void meson_uart_set_termios(struct uart_port *port,
 	if (cflags & CSTOPB)
 		val |= AML_UART_STOP_BIN_2SB;
 	else
-		val &= ~AML_UART_STOP_BIN_1SB;
+		val |= AML_UART_STOP_BIN_1SB;
 
 	if (cflags & CRTSCTS)
 		val &= ~AML_UART_TWO_WIRE_EN;
@@ -395,51 +393,25 @@ static int meson_uart_verify_port(struct uart_port *port,
 	return ret;
 }
 
-static int meson_uart_res_size(struct uart_port *port)
-{
-	struct platform_device *pdev = to_platform_device(port->dev);
-	struct resource *res;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(port->dev, "cannot obtain I/O memory region");
-		return -ENODEV;
-	}
-
-	return resource_size(res);
-}
-
 static void meson_uart_release_port(struct uart_port *port)
 {
-	int size = meson_uart_res_size(port);
-
-	if (port->flags & UPF_IOREMAP) {
-		devm_release_mem_region(port->dev, port->mapbase, size);
-		devm_iounmap(port->dev, port->membase);
-		port->membase = NULL;
-	}
+	devm_iounmap(port->dev, port->membase);
+	port->membase = NULL;
+	devm_release_mem_region(port->dev, port->mapbase, port->mapsize);
 }
 
 static int meson_uart_request_port(struct uart_port *port)
 {
-	int size = meson_uart_res_size(port);
-
-	if (size < 0)
-		return size;
-
-	if (!devm_request_mem_region(port->dev, port->mapbase, size,
+	if (!devm_request_mem_region(port->dev, port->mapbase, port->mapsize,
 				     dev_name(port->dev))) {
 		dev_err(port->dev, "Memory region busy\n");
 		return -EBUSY;
 	}
 
-	if (port->flags & UPF_IOREMAP) {
-		port->membase = devm_ioremap_nocache(port->dev,
-						     port->mapbase,
-						     size);
-		if (port->membase == NULL)
-			return -ENOMEM;
-	}
+	port->membase = devm_ioremap_nocache(port->dev, port->mapbase,
+					     port->mapsize);
+	if (!port->membase)
+		return -ENOMEM;
 
 	return 0;
 }
@@ -470,6 +442,14 @@ static struct uart_ops meson_uart_ops = {
 };
 
 #ifdef CONFIG_SERIAL_MESON_CONSOLE
+static void meson_uart_enable_tx_engine(struct uart_port *port)
+{
+	u32 val;
+
+	val = readl(port->membase + AML_UART_CONTROL);
+	val |= AML_UART_TX_EN;
+	writel(val, port->membase + AML_UART_CONTROL);
+}
 
 static void meson_console_putchar(struct uart_port *port, int ch)
 {
@@ -499,7 +479,6 @@ static void meson_serial_port_write(struct uart_port *port, const char *s,
 	}
 
 	val = readl(port->membase + AML_UART_CONTROL);
-	val |= AML_UART_TX_EN;
 	tmp = val & ~(AML_UART_TX_INT_EN | AML_UART_RX_INT_EN);
 	writel(tmp, port->membase + AML_UART_CONTROL);
 
@@ -538,6 +517,8 @@ static int meson_serial_console_setup(struct console *co, char *options)
 	if (!port || !port->membase)
 		return -ENODEV;
 
+	meson_uart_enable_tx_engine(port);
+
 	if (options)
 		uart_parse_options(options, &baud, &parity, &bits, &flow);
 
@@ -576,11 +557,16 @@ meson_serial_early_console_setup(struct earlycon_device *device, const char *opt
 	if (!device->port.membase)
 		return -ENODEV;
 
+	meson_uart_enable_tx_engine(&device->port);
 	device->con->write = meson_serial_early_console_write;
 	return 0;
 }
+/* Legacy bindings, should be removed when no more used */
 OF_EARLYCON_DECLARE(meson, "amlogic,meson-uart",
 		    meson_serial_early_console_setup);
+/* Stable bindings */
+OF_EARLYCON_DECLARE(meson, "amlogic,meson-ao-uart",
+		    meson_serial_early_console_setup);
 
 #define MESON_SERIAL_CONSOLE	(&meson_serial_console)
 #else
@@ -595,11 +581,76 @@ static struct uart_driver meson_uart_driver = {
 	.cons		= MESON_SERIAL_CONSOLE,
 };
 
+static inline struct clk *meson_uart_probe_clock(struct device *dev,
+						 const char *id)
+{
+	struct clk *clk = NULL;
+	int ret;
+
+	clk = devm_clk_get(dev, id);
+	if (IS_ERR(clk))
+		return clk;
+
+	ret = clk_prepare_enable(clk);
+	if (ret) {
+		dev_err(dev, "couldn't enable clk\n");
+		return ERR_PTR(ret);
+	}
+
+	devm_add_action_or_reset(dev,
+			(void(*)(void *))clk_disable_unprepare,
+			clk);
+
+	return clk;
+}
+
+/*
+ * This function gets clocks in the legacy non-stable DT bindings.
+ * This code will be remove once all the platforms switch to the
+ * new DT bindings.
+ */
+static int meson_uart_probe_clocks_legacy(struct platform_device *pdev,
+					  struct uart_port *port)
+{
+	struct clk *clk = NULL;
+
+	clk = meson_uart_probe_clock(&pdev->dev, NULL);
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	port->uartclk = clk_get_rate(clk);
+
+	return 0;
+}
+
+static int meson_uart_probe_clocks(struct platform_device *pdev,
+				   struct uart_port *port)
+{
+	struct clk *clk_xtal = NULL;
+	struct clk *clk_pclk = NULL;
+	struct clk *clk_baud = NULL;
+
+	clk_pclk = meson_uart_probe_clock(&pdev->dev, "pclk");
+	if (IS_ERR(clk_pclk))
+		return PTR_ERR(clk_pclk);
+
+	clk_xtal = meson_uart_probe_clock(&pdev->dev, "xtal");
+	if (IS_ERR(clk_xtal))
+		return PTR_ERR(clk_xtal);
+
+	clk_baud = meson_uart_probe_clock(&pdev->dev, "baud");
+	if (IS_ERR(clk_baud))
+		return PTR_ERR(clk_baud);
+
+	port->uartclk = clk_get_rate(clk_baud);
+
+	return 0;
+}
+
 static int meson_uart_probe(struct platform_device *pdev)
 {
 	struct resource *res_mem, *res_irq;
 	struct uart_port *port;
-	struct clk *clk;
 	int ret = 0;
 
 	if (pdev->dev.of_node)
@@ -625,15 +676,20 @@ static int meson_uart_probe(struct platform_device *pdev)
 	if (!port)
 		return -ENOMEM;
 
-	clk = clk_get(&pdev->dev, NULL);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	/* Use legacy way until all platforms switch to new bindings */
+	if (of_device_is_compatible(pdev->dev.of_node, "amlogic,meson-uart"))
+		ret = meson_uart_probe_clocks_legacy(pdev, port);
+	else
+		ret = meson_uart_probe_clocks(pdev, port);
+
+	if (ret)
+		return ret;
 
-	port->uartclk = clk_get_rate(clk);
 	port->iotype = UPIO_MEM;
 	port->mapbase = res_mem->start;
+	port->mapsize = resource_size(res_mem);
 	port->irq = res_irq->start;
-	port->flags = UPF_BOOT_AUTOCONF | UPF_IOREMAP | UPF_LOW_LATENCY;
+	port->flags = UPF_BOOT_AUTOCONF | UPF_LOW_LATENCY;
 	port->dev = &pdev->dev;
 	port->line = pdev->id;
 	port->type = PORT_MESON;
@@ -668,9 +724,14 @@ static int meson_uart_remove(struct platform_device *pdev)
 	return 0;
 }
 
-
 static const struct of_device_id meson_uart_dt_match[] = {
+	/* Legacy bindings, should be removed when no more used */
 	{ .compatible = "amlogic,meson-uart" },
+	/* Stable bindings */
+	{ .compatible = "amlogic,meson6-uart" },
+	{ .compatible = "amlogic,meson8-uart" },
+	{ .compatible = "amlogic,meson8b-uart" },
+	{ .compatible = "amlogic,meson-gx-uart" },
 	{ /* sentinel */ },
 };
 MODULE_DEVICE_TABLE(of, meson_uart_dt_match);
diff --git a/drivers/tty/serial/mpsc.c b/drivers/tty/serial/mpsc.c
index 1a60a2063e75..67ffecc50e42 100644
--- a/drivers/tty/serial/mpsc.c
+++ b/drivers/tty/serial/mpsc.c
@@ -754,9 +754,10 @@ static int mpsc_alloc_ring_mem(struct mpsc_port_info *pi)
 		if (!dma_set_mask(pi->port.dev, 0xffffffff)) {
 			printk(KERN_ERR "MPSC: Inadequate DMA support\n");
 			rc = -ENXIO;
-		} else if ((pi->dma_region = dma_alloc_noncoherent(pi->port.dev,
+		} else if ((pi->dma_region = dma_alloc_attrs(pi->port.dev,
 						MPSC_DMA_ALLOC_SIZE,
-						&pi->dma_region_p, GFP_KERNEL))
+						&pi->dma_region_p, GFP_KERNEL,
+						DMA_ATTR_NON_CONSISTENT))
 				== NULL) {
 			printk(KERN_ERR "MPSC: Can't alloc Desc region\n");
 			rc = -ENOMEM;
@@ -771,8 +772,9 @@ static void mpsc_free_ring_mem(struct mpsc_port_info *pi)
 	pr_debug("mpsc_free_ring_mem[%d]: Freeing ring mem\n", pi->port.line);
 
 	if (pi->dma_region) {
-		dma_free_noncoherent(pi->port.dev, MPSC_DMA_ALLOC_SIZE,
-				pi->dma_region, pi->dma_region_p);
+		dma_free_attrs(pi->port.dev, MPSC_DMA_ALLOC_SIZE,
+				pi->dma_region, pi->dma_region_p,
+				DMA_ATTR_NON_CONSISTENT);
 		pi->dma_region = NULL;
 		pi->dma_region_p = (dma_addr_t)NULL;
 	}
diff --git a/drivers/tty/serial/owl-uart.c b/drivers/tty/serial/owl-uart.c
new file mode 100644
index 000000000000..1b8008797a1b
--- /dev/null
+++ b/drivers/tty/serial/owl-uart.c
@@ -0,0 +1,135 @@
+/*
+ * Actions Semi Owl family serial console
+ *
+ * Copyright 2013 Actions Semi Inc.
+ * Author: Actions Semi, Inc.
+ *
+ * Copyright (c) 2016-2017 Andreas Färber
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the 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/console.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/serial.h>
+#include <linux/serial_core.h>
+
+#define OWL_UART_CTL	0x000
+#define OWL_UART_TXDAT	0x008
+#define OWL_UART_STAT	0x00c
+
+#define OWL_UART_CTL_TRFS_TX		BIT(14)
+#define OWL_UART_CTL_EN			BIT(15)
+#define OWL_UART_CTL_RXIE		BIT(18)
+#define OWL_UART_CTL_TXIE		BIT(19)
+
+#define OWL_UART_STAT_RIP		BIT(0)
+#define OWL_UART_STAT_TIP		BIT(1)
+#define OWL_UART_STAT_TFFU		BIT(6)
+#define OWL_UART_STAT_TRFL_MASK		(0x1f << 11)
+#define OWL_UART_STAT_UTBB		BIT(17)
+
+static inline void owl_uart_write(struct uart_port *port, u32 val, unsigned int off)
+{
+	writel(val, port->membase + off);
+}
+
+static inline u32 owl_uart_read(struct uart_port *port, unsigned int off)
+{
+	return readl(port->membase + off);
+}
+
+#ifdef CONFIG_SERIAL_OWL_CONSOLE
+
+static void owl_console_putchar(struct uart_port *port, int ch)
+{
+	if (!port->membase)
+		return;
+
+	while (owl_uart_read(port, OWL_UART_STAT) & OWL_UART_STAT_TFFU)
+		cpu_relax();
+
+	owl_uart_write(port, ch, OWL_UART_TXDAT);
+}
+
+static void owl_uart_port_write(struct uart_port *port, const char *s,
+				u_int count)
+{
+	u32 old_ctl, val;
+	unsigned long flags;
+	int locked;
+
+	local_irq_save(flags);
+
+	if (port->sysrq)
+		locked = 0;
+	else if (oops_in_progress)
+		locked = spin_trylock(&port->lock);
+	else {
+		spin_lock(&port->lock);
+		locked = 1;
+	}
+
+	old_ctl = owl_uart_read(port, OWL_UART_CTL);
+	val = old_ctl | OWL_UART_CTL_TRFS_TX;
+	/* disable IRQ */
+	val &= ~(OWL_UART_CTL_RXIE | OWL_UART_CTL_TXIE);
+	owl_uart_write(port, val, OWL_UART_CTL);
+
+	uart_console_write(port, s, count, owl_console_putchar);
+
+	/* wait until all contents have been sent out */
+	while (owl_uart_read(port, OWL_UART_STAT) & OWL_UART_STAT_TRFL_MASK)
+		cpu_relax();
+
+	/* clear IRQ pending */
+	val = owl_uart_read(port, OWL_UART_STAT);
+	val |= OWL_UART_STAT_TIP | OWL_UART_STAT_RIP;
+	owl_uart_write(port, val, OWL_UART_STAT);
+
+	owl_uart_write(port, old_ctl, OWL_UART_CTL);
+
+	if (locked)
+		spin_unlock(&port->lock);
+
+	local_irq_restore(flags);
+}
+
+static void owl_uart_early_console_write(struct console *co,
+					 const char *s,
+					 u_int count)
+{
+	struct earlycon_device *dev = co->data;
+
+	owl_uart_port_write(&dev->port, s, count);
+}
+
+static int __init
+owl_uart_early_console_setup(struct earlycon_device *device, const char *opt)
+{
+	if (!device->port.membase)
+		return -ENODEV;
+
+	device->con->write = owl_uart_early_console_write;
+
+	return 0;
+}
+OF_EARLYCON_DECLARE(owl, "actions,owl-uart",
+		    owl_uart_early_console_setup);
+
+#endif /* CONFIG_SERIAL_OWL_CONSOLE */
diff --git a/drivers/tty/serial/pch_uart.c b/drivers/tty/serial/pch_uart.c
index 42caccb5e87e..d3796dc26fa9 100644
--- a/drivers/tty/serial/pch_uart.c
+++ b/drivers/tty/serial/pch_uart.c
@@ -878,8 +878,7 @@ static int dma_handle_rx(struct eg20t_port *priv)
 	sg_dma_len(sg) = priv->trigger_level;
 
 	sg_set_page(&priv->sg_rx, virt_to_page(priv->rx_buf_virt),
-		     sg_dma_len(sg), (unsigned long)priv->rx_buf_virt &
-		     ~PAGE_MASK);
+		     sg_dma_len(sg), offset_in_page(priv->rx_buf_virt));
 
 	sg_dma_address(sg) = priv->rx_buf_dma;
 
diff --git a/drivers/tty/serial/sccnxp.c b/drivers/tty/serial/sccnxp.c
index fcf803ffad19..cdd2f942317c 100644
--- a/drivers/tty/serial/sccnxp.c
+++ b/drivers/tty/serial/sccnxp.c
@@ -884,14 +884,19 @@ static int sccnxp_probe(struct platform_device *pdev)
 
 	clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(clk)) {
-		if (PTR_ERR(clk) == -EPROBE_DEFER) {
-			ret = -EPROBE_DEFER;
+		ret = PTR_ERR(clk);
+		if (ret == -EPROBE_DEFER)
 			goto err_out;
-		}
+		uartclk = 0;
+	} else {
+		clk_prepare_enable(clk);
+		uartclk = clk_get_rate(clk);
+	}
+
+	if (!uartclk) {
 		dev_notice(&pdev->dev, "Using default clock frequency\n");
 		uartclk = s->chip->freq_std;
-	} else
-		uartclk = clk_get_rate(clk);
+	}
 
 	/* Check input frequency */
 	if ((uartclk < s->chip->freq_min) || (uartclk > s->chip->freq_max)) {
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index 13bfd5dcffce..f534a40aebde 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -954,11 +954,10 @@ static int uart_set_info(struct tty_struct *tty, struct tty_port *port,
 		    old_custom_divisor != uport->custom_divisor) {
 			/*
 			 * If they're setting up a custom divisor or speed,
-			 * instead of clearing it, then bitch about it. No
-			 * need to rate-limit; it's CAP_SYS_ADMIN only.
+			 * instead of clearing it, then bitch about it.
 			 */
 			if (uport->flags & UPF_SPD_MASK) {
-				dev_notice(uport->dev,
+				dev_notice_ratelimited(uport->dev,
 				       "%s sets custom speed on %s. This is deprecated.\n",
 				      current->comm,
 				      tty_name(port->tty));
diff --git a/drivers/tty/serial/sh-sci.c b/drivers/tty/serial/sh-sci.c
index 71707e8e6e3f..da5ddfc14778 100644
--- a/drivers/tty/serial/sh-sci.c
+++ b/drivers/tty/serial/sh-sci.c
@@ -1450,8 +1450,7 @@ static struct dma_chan *sci_request_dma_chan(struct uart_port *port,
 	chan = dma_request_slave_channel(port->dev,
 					 dir == DMA_MEM_TO_DEV ? "tx" : "rx");
 	if (!chan) {
-		dev_warn(port->dev,
-			 "dma_request_slave_channel_compat failed\n");
+		dev_warn(port->dev, "dma_request_slave_channel failed\n");
 		return NULL;
 	}
 
@@ -1558,7 +1557,16 @@ static void sci_free_dma(struct uart_port *port)
 	if (s->chan_rx)
 		sci_rx_dma_release(s, false);
 }
-#else
+
+static void sci_flush_buffer(struct uart_port *port)
+{
+	/*
+	 * In uart_flush_buffer(), the xmit circular buffer has just been
+	 * cleared, so we have to reset tx_dma_len accordingly.
+	 */
+	to_sci_port(port)->tx_dma_len = 0;
+}
+#else /* !CONFIG_SERIAL_SH_SCI_DMA */
 static inline void sci_request_dma(struct uart_port *port)
 {
 }
@@ -1566,7 +1574,9 @@ static inline void sci_request_dma(struct uart_port *port)
 static inline void sci_free_dma(struct uart_port *port)
 {
 }
-#endif
+
+#define sci_flush_buffer	NULL
+#endif /* !CONFIG_SERIAL_SH_SCI_DMA */
 
 static irqreturn_t sci_rx_interrupt(int irq, void *ptr)
 {
@@ -2581,6 +2591,7 @@ static const struct uart_ops sci_uart_ops = {
 	.break_ctl	= sci_break_ctl,
 	.startup	= sci_startup,
 	.shutdown	= sci_shutdown,
+	.flush_buffer	= sci_flush_buffer,
 	.set_termios	= sci_set_termios,
 	.pm		= sci_pm,
 	.type		= sci_type,
@@ -2950,6 +2961,7 @@ static inline int sci_probe_earlyprintk(struct platform_device *pdev)
 
 static const char banner[] __initconst = "SuperH (H)SCI(F) driver initialized";
 
+static DEFINE_MUTEX(sci_uart_registration_lock);
 static struct uart_driver sci_uart_driver = {
 	.owner		= THIS_MODULE,
 	.driver_name	= "sci",
@@ -3078,6 +3090,16 @@ static int sci_probe_single(struct platform_device *dev,
 		return -EINVAL;
 	}
 
+	mutex_lock(&sci_uart_registration_lock);
+	if (!sci_uart_driver.state) {
+		ret = uart_register_driver(&sci_uart_driver);
+		if (ret) {
+			mutex_unlock(&sci_uart_registration_lock);
+			return ret;
+		}
+	}
+	mutex_unlock(&sci_uart_registration_lock);
+
 	ret = sci_init_single(dev, sciport, index, p, false);
 	if (ret)
 		return ret;
@@ -3201,24 +3223,17 @@ static struct platform_driver sci_driver = {
 
 static int __init sci_init(void)
 {
-	int ret;
-
 	pr_info("%s\n", banner);
 
-	ret = uart_register_driver(&sci_uart_driver);
-	if (likely(ret == 0)) {
-		ret = platform_driver_register(&sci_driver);
-		if (unlikely(ret))
-			uart_unregister_driver(&sci_uart_driver);
-	}
-
-	return ret;
+	return platform_driver_register(&sci_driver);
 }
 
 static void __exit sci_exit(void)
 {
 	platform_driver_unregister(&sci_driver);
-	uart_unregister_driver(&sci_uart_driver);
+
+	if (sci_uart_driver.state)
+		uart_unregister_driver(&sci_uart_driver);
 }
 
 #ifdef CONFIG_SERIAL_SH_SCI_CONSOLE
diff --git a/drivers/tty/serial/sirfsoc_uart.c b/drivers/tty/serial/sirfsoc_uart.c
index e03282d92b59..684cb8dd8050 100644
--- a/drivers/tty/serial/sirfsoc_uart.c
+++ b/drivers/tty/serial/sirfsoc_uart.c
@@ -1253,7 +1253,7 @@ next_hrt:
 	return HRTIMER_RESTART;
 }
 
-static struct of_device_id sirfsoc_uart_ids[] = {
+static const struct of_device_id sirfsoc_uart_ids[] = {
 	{ .compatible = "sirf,prima2-uart", .data = &sirfsoc_uart,},
 	{ .compatible = "sirf,atlas7-uart", .data = &sirfsoc_uart},
 	{ .compatible = "sirf,prima2-usp-uart", .data = &sirfsoc_usp},
diff --git a/drivers/tty/serial/xilinx_uartps.c b/drivers/tty/serial/xilinx_uartps.c
index c0539950f8d7..fde55dcdea5a 100644
--- a/drivers/tty/serial/xilinx_uartps.c
+++ b/drivers/tty/serial/xilinx_uartps.c
@@ -186,6 +186,7 @@ MODULE_PARM_DESC(rx_timeout, "Rx timeout, 1-255");
  * @pclk:		APB clock
  * @baud:		Current baud rate
  * @clk_rate_change_nb:	Notifier block for clock changes
+ * @quirks:		Flags for RXBS support.
  */
 struct cdns_uart {
 	struct uart_port	*port;
@@ -1587,20 +1588,21 @@ static int cdns_uart_probe(struct platform_device *pdev)
 	if (rc) {
 		dev_err(&pdev->dev,
 			"uart_add_one_port() failed; err=%i\n", rc);
-		goto err_out_notif_unreg;
+		goto err_out_pm_disable;
 	}
 
 	return 0;
 
+err_out_pm_disable:
+	pm_runtime_disable(&pdev->dev);
+	pm_runtime_set_suspended(&pdev->dev);
+	pm_runtime_dont_use_autosuspend(&pdev->dev);
 err_out_notif_unreg:
 #ifdef CONFIG_COMMON_CLK
 	clk_notifier_unregister(cdns_uart_data->uartclk,
 			&cdns_uart_data->clk_rate_change_nb);
 #endif
 err_out_clk_disable:
-	pm_runtime_disable(&pdev->dev);
-	pm_runtime_set_suspended(&pdev->dev);
-	pm_runtime_dont_use_autosuspend(&pdev->dev);
 	clk_disable_unprepare(cdns_uart_data->uartclk);
 err_out_clk_dis_pclk:
 	clk_disable_unprepare(cdns_uart_data->pclk);
diff --git a/drivers/tty/synclink.c b/drivers/tty/synclink.c
index a2c308f7d637..3fafc5a1b2e0 100644
--- a/drivers/tty/synclink.c
+++ b/drivers/tty/synclink.c
@@ -7960,7 +7960,7 @@ static void hdlcdev_rx(struct mgsl_struct *info, char *buf, int size)
 		return;
 	}
 
-	memcpy(skb_put(skb, size), buf, size);
+	skb_put_data(skb, buf, size);
 
 	skb->protocol = hdlc_type_trans(skb, dev);
 
diff --git a/drivers/tty/synclink_gt.c b/drivers/tty/synclink_gt.c
index 31885f20fc15..529c6e3cd537 100644
--- a/drivers/tty/synclink_gt.c
+++ b/drivers/tty/synclink_gt.c
@@ -184,7 +184,7 @@ static void hdlcdev_exit(struct slgt_info *info);
 struct cond_wait {
 	struct cond_wait *next;
 	wait_queue_head_t q;
-	wait_queue_t wait;
+	wait_queue_entry_t wait;
 	unsigned int data;
 };
 static void init_cond_wait(struct cond_wait *w, unsigned int data);
@@ -1755,7 +1755,7 @@ static void hdlcdev_rx(struct slgt_info *info, char *buf, int size)
 		return;
 	}
 
-	memcpy(skb_put(skb, size), buf, size);
+	skb_put_data(skb, buf, size);
 
 	skb->protocol = hdlc_type_trans(skb, dev);
 
diff --git a/drivers/tty/synclinkmp.c b/drivers/tty/synclinkmp.c
index 51e8846cd68f..9b4fb0251c1a 100644
--- a/drivers/tty/synclinkmp.c
+++ b/drivers/tty/synclinkmp.c
@@ -1874,7 +1874,7 @@ static void hdlcdev_rx(SLMP_INFO *info, char *buf, int size)
 		return;
 	}
 
-	memcpy(skb_put(skb, size), buf, size);
+	skb_put_data(skb, buf, size);
 
 	skb->protocol = hdlc_type_trans(skb, dev);
 
diff --git a/drivers/tty/tty_io.c b/drivers/tty/tty_io.c
index 0c150b5a9dd6..974b13d24401 100644
--- a/drivers/tty/tty_io.c
+++ b/drivers/tty/tty_io.c
@@ -325,6 +325,56 @@ static struct tty_driver *get_tty_driver(dev_t device, int *index)
 	return NULL;
 }
 
+/**
+ *	tty_dev_name_to_number	-	return dev_t for device name
+ *	@name: user space name of device under /dev
+ *	@number: pointer to dev_t that this function will populate
+ *
+ *	This function converts device names like ttyS0 or ttyUSB1 into dev_t
+ *	like (4, 64) or (188, 1). If no corresponding driver is registered then
+ *	the function returns -ENODEV.
+ *
+ *	Locking: this acquires tty_mutex to protect the tty_drivers list from
+ *		being modified while we are traversing it, and makes sure to
+ *		release it before exiting.
+ */
+int tty_dev_name_to_number(const char *name, dev_t *number)
+{
+	struct tty_driver *p;
+	int ret;
+	int index, prefix_length = 0;
+	const char *str;
+
+	for (str = name; *str && !isdigit(*str); str++)
+		;
+
+	if (!*str)
+		return -EINVAL;
+
+	ret = kstrtoint(str, 10, &index);
+	if (ret)
+		return ret;
+
+	prefix_length = str - name;
+	mutex_lock(&tty_mutex);
+
+	list_for_each_entry(p, &tty_drivers, tty_drivers)
+		if (prefix_length == strlen(p->name) && strncmp(name,
+					p->name, prefix_length) == 0) {
+			if (index < p->num) {
+				*number = MKDEV(p->major, p->minor_start + index);
+				goto out;
+			}
+		}
+
+	/* if here then driver wasn't found */
+	ret = -ENODEV;
+out:
+	mutex_unlock(&tty_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tty_dev_name_to_number);
+
 #ifdef CONFIG_CONSOLE_POLL
 
 /**
@@ -1083,7 +1133,10 @@ static struct tty_struct *tty_driver_lookup_tty(struct tty_driver *driver,
 	struct tty_struct *tty;
 
 	if (driver->ops->lookup)
-		tty = driver->ops->lookup(driver, file, idx);
+		if (!file)
+			tty = ERR_PTR(-EIO);
+		else
+			tty = driver->ops->lookup(driver, file, idx);
 	else
 		tty = driver->ttys[idx];
 
@@ -1715,7 +1768,7 @@ static struct tty_driver *tty_lookup_driver(dev_t device, struct file *filp,
 		struct tty_driver *console_driver = console_device(index);
 		if (console_driver) {
 			driver = tty_driver_kref_get(console_driver);
-			if (driver) {
+			if (driver && filp) {
 				/* Don't let /dev/console block */
 				filp->f_flags |= O_NONBLOCK;
 				break;
@@ -1748,7 +1801,7 @@ static struct tty_driver *tty_lookup_driver(dev_t device, struct file *filp,
  *	  - concurrent tty driver removal w/ lookup
  *	  - concurrent tty removal from driver table
  */
-static struct tty_struct *tty_open_by_driver(dev_t device, struct inode *inode,
+struct tty_struct *tty_open_by_driver(dev_t device, struct inode *inode,
 					     struct file *filp)
 {
 	struct tty_struct *tty;
@@ -1793,6 +1846,7 @@ out:
 	tty_driver_kref_put(driver);
 	return tty;
 }
+EXPORT_SYMBOL_GPL(tty_open_by_driver);
 
 /**
  *	tty_open		-	open a tty device
diff --git a/drivers/tty/tty_ldisc.c b/drivers/tty/tty_ldisc.c
index e4603b09863a..2fe216b276e2 100644
--- a/drivers/tty/tty_ldisc.c
+++ b/drivers/tty/tty_ldisc.c
@@ -492,6 +492,29 @@ static void tty_ldisc_close(struct tty_struct *tty, struct tty_ldisc *ld)
 }
 
 /**
+ *	tty_ldisc_failto	-	helper for ldisc failback
+ *	@tty: tty to open the ldisc on
+ *	@ld: ldisc we are trying to fail back to
+ *
+ *	Helper to try and recover a tty when switching back to the old
+ *	ldisc fails and we need something attached.
+ */
+
+static int tty_ldisc_failto(struct tty_struct *tty, int ld)
+{
+	struct tty_ldisc *disc = tty_ldisc_get(tty, ld);
+	int r;
+
+	if (IS_ERR(disc))
+		return PTR_ERR(disc);
+	tty->ldisc = disc;
+	tty_set_termios_ldisc(tty, ld);
+	if ((r = tty_ldisc_open(tty, disc)) < 0)
+		tty_ldisc_put(disc);
+	return r;
+}
+
+/**
  *	tty_ldisc_restore	-	helper for tty ldisc change
  *	@tty: tty to recover
  *	@old: previous ldisc
@@ -502,9 +525,6 @@ static void tty_ldisc_close(struct tty_struct *tty, struct tty_ldisc *ld)
 
 static void tty_ldisc_restore(struct tty_struct *tty, struct tty_ldisc *old)
 {
-	struct tty_ldisc *new_ldisc;
-	int r;
-
 	/* There is an outstanding reference here so this is safe */
 	old = tty_ldisc_get(tty, old->ops->num);
 	WARN_ON(IS_ERR(old));
@@ -512,17 +532,13 @@ static void tty_ldisc_restore(struct tty_struct *tty, struct tty_ldisc *old)
 	tty_set_termios_ldisc(tty, old->ops->num);
 	if (tty_ldisc_open(tty, old) < 0) {
 		tty_ldisc_put(old);
-		/* This driver is always present */
-		new_ldisc = tty_ldisc_get(tty, N_TTY);
-		if (IS_ERR(new_ldisc))
-			panic("n_tty: get");
-		tty->ldisc = new_ldisc;
-		tty_set_termios_ldisc(tty, N_TTY);
-		r = tty_ldisc_open(tty, new_ldisc);
-		if (r < 0)
-			panic("Couldn't open N_TTY ldisc for "
-			      "%s --- error %d.",
-			      tty_name(tty), r);
+		/* The traditional behaviour is to fall back to N_TTY, we
+		   want to avoid falling back to N_NULL unless we have no
+		   choice to avoid the risk of breaking anything */
+		if (tty_ldisc_failto(tty, N_TTY) < 0 &&
+		    tty_ldisc_failto(tty, N_NULL) < 0)
+			panic("Couldn't open N_NULL ldisc for %s.",
+			      tty_name(tty));
 	}
 }
 
@@ -605,6 +621,7 @@ err:
 	tty_unlock(tty);
 	return retval;
 }
+EXPORT_SYMBOL_GPL(tty_set_ldisc);
 
 /**
  *	tty_ldisc_kill	-	teardown ldisc
@@ -797,6 +814,7 @@ void tty_ldisc_release(struct tty_struct *tty)
 
 	tty_ldisc_debug(tty, "released\n");
 }
+EXPORT_SYMBOL_GPL(tty_ldisc_release);
 
 /**
  *	tty_ldisc_init		-	ldisc setup for new tty
diff --git a/drivers/tty/vt/consolemap.c b/drivers/tty/vt/consolemap.c
index 1f6e17fc3fb0..a5f88cf0f61d 100644
--- a/drivers/tty/vt/consolemap.c
+++ b/drivers/tty/vt/consolemap.c
@@ -322,15 +322,13 @@ int con_set_trans_old(unsigned char __user * arg)
 {
 	int i;
 	unsigned short inbuf[E_TABSZ];
+	unsigned char ubuf[E_TABSZ];
 
-	if (!access_ok(VERIFY_READ, arg, E_TABSZ))
+	if (copy_from_user(ubuf, arg, E_TABSZ))
 		return -EFAULT;
 
-	for (i = 0; i < E_TABSZ ; i++) {
-		unsigned char uc;
-		__get_user(uc, arg+i);
-		inbuf[i] = UNI_DIRECT_BASE | uc;
-	}
+	for (i = 0; i < E_TABSZ ; i++)
+		inbuf[i] = UNI_DIRECT_BASE | ubuf[i];
 
 	console_lock();
 	memcpy(translations[USER_MAP], inbuf, sizeof(inbuf));
@@ -345,9 +343,6 @@ int con_get_trans_old(unsigned char __user * arg)
 	unsigned short *p = translations[USER_MAP];
 	unsigned char outbuf[E_TABSZ];
 
-	if (!access_ok(VERIFY_WRITE, arg, E_TABSZ))
-		return -EFAULT;
-
 	console_lock();
 	for (i = 0; i < E_TABSZ ; i++)
 	{
@@ -356,22 +351,16 @@ int con_get_trans_old(unsigned char __user * arg)
 	}
 	console_unlock();
 
-	for (i = 0; i < E_TABSZ ; i++)
-		__put_user(outbuf[i], arg+i);
-	return 0;
+	return copy_to_user(arg, outbuf, sizeof(outbuf)) ? -EFAULT : 0;
 }
 
 int con_set_trans_new(ushort __user * arg)
 {
-	int i;
 	unsigned short inbuf[E_TABSZ];
 
-	if (!access_ok(VERIFY_READ, arg, E_TABSZ*sizeof(unsigned short)))
+	if (copy_from_user(inbuf, arg, sizeof(inbuf)))
 		return -EFAULT;
 
-	for (i = 0; i < E_TABSZ ; i++)
-		__get_user(inbuf[i], arg+i);
-
 	console_lock();
 	memcpy(translations[USER_MAP], inbuf, sizeof(inbuf));
 	update_user_maps();
@@ -381,19 +370,13 @@ int con_set_trans_new(ushort __user * arg)
 
 int con_get_trans_new(ushort __user * arg)
 {
-	int i;
 	unsigned short outbuf[E_TABSZ];
 
-	if (!access_ok(VERIFY_WRITE, arg, E_TABSZ*sizeof(unsigned short)))
-		return -EFAULT;
-
 	console_lock();
 	memcpy(outbuf, translations[USER_MAP], sizeof(outbuf));
 	console_unlock();
 
-	for (i = 0; i < E_TABSZ ; i++)
-		__put_user(outbuf[i], arg+i);
-	return 0;
+	return copy_to_user(arg, outbuf, sizeof(outbuf)) ? -EFAULT : 0;
 }
 
 /*
@@ -557,14 +540,9 @@ int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 	if (!ct)
 		return 0;
 
-	unilist = kmalloc_array(ct, sizeof(struct unipair), GFP_KERNEL);
-	if (!unilist)
-		return -ENOMEM;
-
-	for (i = ct, plist = unilist; i; i--, plist++, list++) {
-		__get_user(plist->unicode, &list->unicode);
-		__get_user(plist->fontpos, &list->fontpos);
-	}
+	unilist = memdup_user(list, ct * sizeof(struct unipair));
+	if (IS_ERR(unilist))
+		return PTR_ERR(unilist);
 
 	console_lock();
 
@@ -757,11 +735,11 @@ EXPORT_SYMBOL(con_copy_unimap);
  */
 int con_get_unimap(struct vc_data *vc, ushort ct, ushort __user *uct, struct unipair __user *list)
 {
-	int i, j, k;
+	int i, j, k, ret = 0;
 	ushort ect;
 	u16 **p1, *p2;
 	struct uni_pagedir *p;
-	struct unipair *unilist, *plist;
+	struct unipair *unilist;
 
 	unilist = kmalloc_array(ct, sizeof(struct unipair), GFP_KERNEL);
 	if (!unilist)
@@ -792,13 +770,11 @@ int con_get_unimap(struct vc_data *vc, ushort ct, ushort __user *uct, struct uni
 		}
 	}
 	console_unlock();
-	for (i = min(ect, ct), plist = unilist; i; i--, list++, plist++) {
-		__put_user(plist->unicode, &list->unicode);
-		__put_user(plist->fontpos, &list->fontpos);
-	}
-	__put_user(ect, uct);
+	if (copy_to_user(list, unilist, min(ect, ct) * sizeof(struct unipair)))
+		ret = -EFAULT;
+	put_user(ect, uct);
 	kfree(unilist);
-	return ((ect <= ct) ? 0 : -ENOMEM);
+	return ret ? ret : (ect <= ct) ? 0 : -ENOMEM;
 }
 
 /*
diff --git a/drivers/tty/vt/keyboard.c b/drivers/tty/vt/keyboard.c
index 8af8d9542663..f4166263bb3a 100644
--- a/drivers/tty/vt/keyboard.c
+++ b/drivers/tty/vt/keyboard.c
@@ -1203,8 +1203,7 @@ DECLARE_TASKLET_DISABLED(keyboard_tasklet, kbd_bh, 0);
 #if defined(CONFIG_X86) || defined(CONFIG_IA64) || defined(CONFIG_ALPHA) ||\
     defined(CONFIG_MIPS) || defined(CONFIG_PPC) || defined(CONFIG_SPARC) ||\
     defined(CONFIG_PARISC) || defined(CONFIG_SUPERH) ||\
-    (defined(CONFIG_ARM) && defined(CONFIG_KEYBOARD_ATKBD) && !defined(CONFIG_ARCH_RPC)) ||\
-    defined(CONFIG_AVR32)
+    (defined(CONFIG_ARM) && defined(CONFIG_KEYBOARD_ATKBD) && !defined(CONFIG_ARCH_RPC))
 
 #define HW_RAW(dev) (test_bit(EV_MSC, dev->evbit) && test_bit(MSC_RAW, dev->mscbit) &&\
 			((dev)->id.bustype == BUS_I8042) && ((dev)->id.vendor == 0x0001) && ((dev)->id.product == 0x0001))
diff --git a/drivers/tty/vt/vt.c b/drivers/tty/vt/vt.c
index 9c9945284bcf..2ebaba16f785 100644
--- a/drivers/tty/vt/vt.c
+++ b/drivers/tty/vt/vt.c
@@ -425,7 +425,7 @@ static u8 build_attr(struct vc_data *vc, u8 _color, u8 _intensity, u8 _blink,
 	else if (_underline)
 		a = (a & 0xf0) | vc->vc_ulcolor;
 	else if (_intensity == 0)
-		a = (a & 0xf0) | vc->vc_ulcolor;
+		a = (a & 0xf0) | vc->vc_halfcolor;
 	if (_reverse)
 		a = ((a) & 0x88) | ((((a) >> 4) | ((a) << 4)) & 0x77);
 	if (_blink)
@@ -2709,13 +2709,13 @@ int tioclinux(struct tty_struct *tty, unsigned long arg)
 	 * related to the kernel should not use this.
 	 */
 			data = vt_get_shift_state();
-			ret = __put_user(data, p);
+			ret = put_user(data, p);
 			break;
 		case TIOCL_GETMOUSEREPORTING:
 			console_lock();	/* May be overkill */
 			data = mouse_reporting();
 			console_unlock();
-			ret = __put_user(data, p);
+			ret = put_user(data, p);
 			break;
 		case TIOCL_SETVESABLANK:
 			console_lock();
@@ -2724,7 +2724,7 @@ int tioclinux(struct tty_struct *tty, unsigned long arg)
 			break;
 		case TIOCL_GETKMSGREDIRECT:
 			data = vt_get_kmsg_redirect();
-			ret = __put_user(data, p);
+			ret = put_user(data, p);
 			break;
 		case TIOCL_SETKMSGREDIRECT:
 			if (!capable(CAP_SYS_ADMIN)) {
diff --git a/drivers/tty/vt/vt_ioctl.c b/drivers/tty/vt/vt_ioctl.c
index 0cbfe1ff6f6c..96d389cb506c 100644
--- a/drivers/tty/vt/vt_ioctl.c
+++ b/drivers/tty/vt/vt_ioctl.c
@@ -266,10 +266,6 @@ do_unimap_ioctl(int cmd, struct unimapdesc __user *user_ud, int perm, struct vc_
 
 	if (copy_from_user(&tmp, user_ud, sizeof tmp))
 		return -EFAULT;
-	if (tmp.entries)
-		if (!access_ok(VERIFY_WRITE, tmp.entries,
-				tmp.entry_ct*sizeof(struct unipair)))
-			return -EFAULT;
 	switch (cmd) {
 	case PIO_UNIMAP:
 		if (!perm)
@@ -1170,10 +1166,6 @@ compat_unimap_ioctl(unsigned int cmd, struct compat_unimapdesc __user *user_ud,
 	if (copy_from_user(&tmp, user_ud, sizeof tmp))
 		return -EFAULT;
 	tmp_entries = compat_ptr(tmp.entries);
-	if (tmp_entries)
-		if (!access_ok(VERIFY_WRITE, tmp_entries,
-				tmp.entry_ct*sizeof(struct unipair)))
-			return -EFAULT;
 	switch (cmd) {
 	case PIO_UNIMAP:
 		if (!perm)
diff --git a/drivers/uio/uio_pci_generic.c b/drivers/uio/uio_pci_generic.c
index d0b508b68f3c..a56fdf972dbe 100644
--- a/drivers/uio/uio_pci_generic.c
+++ b/drivers/uio/uio_pci_generic.c
@@ -66,14 +66,7 @@ static int probe(struct pci_dev *pdev,
 		return err;
 	}
 
-	if (!pdev->irq) {
-		dev_warn(&pdev->dev, "No IRQ assigned to device: "
-			 "no support for interrupts?\n");
-		pci_disable_device(pdev);
-		return -ENODEV;
-	}
-
-	if (!pci_intx_mask_supported(pdev)) {
+	if (pdev->irq && !pci_intx_mask_supported(pdev)) {
 		err = -ENODEV;
 		goto err_verify;
 	}
@@ -86,10 +79,15 @@ static int probe(struct pci_dev *pdev,
 
 	gdev->info.name = "uio_pci_generic";
 	gdev->info.version = DRIVER_VERSION;
-	gdev->info.irq = pdev->irq;
-	gdev->info.irq_flags = IRQF_SHARED;
-	gdev->info.handler = irqhandler;
 	gdev->pdev = pdev;
+	if (pdev->irq) {
+		gdev->info.irq = pdev->irq;
+		gdev->info.irq_flags = IRQF_SHARED;
+		gdev->info.handler = irqhandler;
+	} else {
+		dev_warn(&pdev->dev, "No IRQ assigned to device: "
+			 "no support for interrupts?\n");
+	}
 
 	err = uio_register_device(&pdev->dev, &gdev->info);
 	if (err)
diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index fe4fe2440729..b17ed3a9a304 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -818,7 +818,7 @@ static inline void ci_role_destroy(struct ci_hdrc *ci)
 {
 	ci_hdrc_gadget_destroy(ci);
 	ci_hdrc_host_destroy(ci);
-	if (ci->is_otg)
+	if (ci->is_otg && ci->roles[CI_ROLE_GADGET])
 		ci_hdrc_otg_destroy(ci);
 }
 
@@ -980,27 +980,35 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 	/* initialize role(s) before the interrupt is requested */
 	if (dr_mode == USB_DR_MODE_OTG || dr_mode == USB_DR_MODE_HOST) {
 		ret = ci_hdrc_host_init(ci);
-		if (ret)
-			dev_info(dev, "doesn't support host\n");
+		if (ret) {
+			if (ret == -ENXIO)
+				dev_info(dev, "doesn't support host\n");
+			else
+				goto deinit_phy;
+		}
 	}
 
 	if (dr_mode == USB_DR_MODE_OTG || dr_mode == USB_DR_MODE_PERIPHERAL) {
 		ret = ci_hdrc_gadget_init(ci);
-		if (ret)
-			dev_info(dev, "doesn't support gadget\n");
+		if (ret) {
+			if (ret == -ENXIO)
+				dev_info(dev, "doesn't support gadget\n");
+			else
+				goto deinit_host;
+		}
 	}
 
 	if (!ci->roles[CI_ROLE_HOST] && !ci->roles[CI_ROLE_GADGET]) {
 		dev_err(dev, "no supported roles\n");
 		ret = -ENODEV;
-		goto deinit_phy;
+		goto deinit_gadget;
 	}
 
 	if (ci->is_otg && ci->roles[CI_ROLE_GADGET]) {
 		ret = ci_hdrc_otg_init(ci);
 		if (ret) {
 			dev_err(dev, "init otg fails, ret = %d\n", ret);
-			goto stop;
+			goto deinit_gadget;
 		}
 	}
 
@@ -1070,7 +1078,12 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 remove_debug:
 	dbg_remove_files(ci);
 stop:
-	ci_role_destroy(ci);
+	if (ci->is_otg && ci->roles[CI_ROLE_GADGET])
+		ci_hdrc_otg_destroy(ci);
+deinit_gadget:
+	ci_hdrc_gadget_destroy(ci);
+deinit_host:
+	ci_hdrc_host_destroy(ci);
 deinit_phy:
 	ci_usb_phy_exit(ci);
 ulpi_exit:
diff --git a/drivers/usb/chipidea/otg_fsm.c b/drivers/usb/chipidea/otg_fsm.c
index 93e24ce61a3a..949183ede16f 100644
--- a/drivers/usb/chipidea/otg_fsm.c
+++ b/drivers/usb/chipidea/otg_fsm.c
@@ -234,7 +234,7 @@ static void ci_otg_add_timer(struct ci_hdrc *ci, enum otg_fsm_timer t)
 				ktime_set(timer_sec, timer_nsec));
 	ci->enabled_otg_timer_bits |= (1 << t);
 	if ((ci->next_otg_timer == NUM_OTG_FSM_TIMERS) ||
-			(ci->hr_timeouts[ci->next_otg_timer] >
+			ktime_after(ci->hr_timeouts[ci->next_otg_timer],
 						ci->hr_timeouts[t])) {
 			ci->next_otg_timer = t;
 			hrtimer_start_range_ns(&ci->otg_fsm_hrtimer,
@@ -269,7 +269,7 @@ static void ci_otg_del_timer(struct ci_hdrc *ci, enum otg_fsm_timer t)
 			for_each_set_bit(cur_timer, &enabled_timer_bits,
 							NUM_OTG_FSM_TIMERS) {
 				if ((next_timer == NUM_OTG_FSM_TIMERS) ||
-					(ci->hr_timeouts[next_timer] <
+					ktime_before(ci->hr_timeouts[next_timer],
 					 ci->hr_timeouts[cur_timer]))
 					next_timer = cur_timer;
 			}
@@ -397,13 +397,13 @@ static enum hrtimer_restart ci_otg_hrtimer_func(struct hrtimer *t)
 
 	now = ktime_get();
 	for_each_set_bit(cur_timer, &enabled_timer_bits, NUM_OTG_FSM_TIMERS) {
-		if (now >= ci->hr_timeouts[cur_timer]) {
+		if (ktime_compare(now, ci->hr_timeouts[cur_timer]) >= 0) {
 			ci->enabled_otg_timer_bits &= ~(1 << cur_timer);
 			if (otg_timer_handlers[cur_timer])
 				ret = otg_timer_handlers[cur_timer](ci);
 		} else {
 			if ((next_timer == NUM_OTG_FSM_TIMERS) ||
-				(ci->hr_timeouts[cur_timer] <
+				ktime_before(ci->hr_timeouts[cur_timer],
 					ci->hr_timeouts[next_timer]))
 				next_timer = cur_timer;
 		}
diff --git a/drivers/usb/class/cdc-wdm.c b/drivers/usb/class/cdc-wdm.c
index 08669fee6d7f..8f972247b1c1 100644
--- a/drivers/usb/class/cdc-wdm.c
+++ b/drivers/usb/class/cdc-wdm.c
@@ -361,17 +361,9 @@ static ssize_t wdm_write
 	if (we < 0)
 		return usb_translate_errors(we);
 
-	buf = kmalloc(count, GFP_KERNEL);
-	if (!buf) {
-		rv = -ENOMEM;
-		goto outnl;
-	}
-
-	r = copy_from_user(buf, buffer, count);
-	if (r > 0) {
-		rv = -EFAULT;
-		goto out_free_mem;
-	}
+	buf = memdup_user(buffer, count);
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
 	/* concurrent writes and disconnect */
 	r = mutex_lock_interruptible(&desc->wlock);
@@ -441,8 +433,7 @@ static ssize_t wdm_write
 
 	usb_autopm_put_interface(desc->intf);
 	mutex_unlock(&desc->wlock);
-outnl:
-	return rv < 0 ? rv : count;
+	return count;
 
 out_free_mem_pm:
 	usb_autopm_put_interface(desc->intf);
diff --git a/drivers/usb/core/devio.c b/drivers/usb/core/devio.c
index 8e6ef671be9b..0e7d0e81a7cb 100644
--- a/drivers/usb/core/devio.c
+++ b/drivers/usb/core/devio.c
@@ -2537,6 +2537,9 @@ static long usbdev_do_ioctl(struct file *file, unsigned int cmd,
 	case USBDEVFS_DROP_PRIVILEGES:
 		ret = proc_drop_privileges(ps, p);
 		break;
+	case USBDEVFS_GET_SPEED:
+		ret = ps->dev->speed;
+		break;
 	}
 
  done:
diff --git a/drivers/usb/core/hcd-pci.c b/drivers/usb/core/hcd-pci.c
index 7859d738df41..ea829ad798c0 100644
--- a/drivers/usb/core/hcd-pci.c
+++ b/drivers/usb/core/hcd-pci.c
@@ -584,12 +584,7 @@ static int hcd_pci_suspend_noirq(struct device *dev)
 
 static int hcd_pci_resume_noirq(struct device *dev)
 {
-	struct pci_dev		*pci_dev = to_pci_dev(dev);
-
-	powermac_set_asic(pci_dev, 1);
-
-	/* Go back to D0 and disable remote wakeup */
-	pci_back_from_sleep(pci_dev);
+	powermac_set_asic(to_pci_dev(dev), 1);
 	return 0;
 }
 
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 5dea98358c05..ab1bb3b538ac 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -26,6 +26,7 @@
 #include <linux/module.h>
 #include <linux/version.h>
 #include <linux/kernel.h>
+#include <linux/sched/task_stack.h>
 #include <linux/slab.h>
 #include <linux/completion.h>
 #include <linux/utsname.h>
@@ -1076,7 +1077,6 @@ static void usb_deregister_bus (struct usb_bus *bus)
 static int register_root_hub(struct usb_hcd *hcd)
 {
 	struct device *parent_dev = hcd->self.controller;
-	struct device *sysdev = hcd->self.sysdev;
 	struct usb_device *usb_dev = hcd->self.root_hub;
 	const int devnum = 1;
 	int retval;
@@ -1123,7 +1123,6 @@ static int register_root_hub(struct usb_hcd *hcd)
 		/* Did the HC die before the root hub was registered? */
 		if (HCD_DEAD(hcd))
 			usb_hc_died (hcd);	/* This time clean up */
-		usb_dev->dev.of_node = sysdev->of_node;
 	}
 	mutex_unlock(&usb_bus_idr_lock);
 
@@ -1523,6 +1522,14 @@ int usb_hcd_map_urb_for_dma(struct usb_hcd *hcd, struct urb *urb,
 		if (hcd->self.uses_pio_for_control)
 			return ret;
 		if (IS_ENABLED(CONFIG_HAS_DMA) && hcd->self.uses_dma) {
+			if (is_vmalloc_addr(urb->setup_packet)) {
+				WARN_ONCE(1, "setup packet is not dma capable\n");
+				return -EAGAIN;
+			} else if (object_is_on_stack(urb->setup_packet)) {
+				WARN_ONCE(1, "setup packet is on stack\n");
+				return -EAGAIN;
+			}
+
 			urb->setup_dma = dma_map_single(
 					hcd->self.sysdev,
 					urb->setup_packet,
@@ -1587,6 +1594,9 @@ int usb_hcd_map_urb_for_dma(struct usb_hcd *hcd, struct urb *urb,
 			} else if (is_vmalloc_addr(urb->transfer_buffer)) {
 				WARN_ONCE(1, "transfer buffer not dma capable\n");
 				ret = -EAGAIN;
+			} else if (object_is_on_stack(urb->transfer_buffer)) {
+				WARN_ONCE(1, "transfer buffer is on stack\n");
+				ret = -EAGAIN;
 			} else {
 				urb->transfer_dma = dma_map_single(
 						hcd->self.sysdev,
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index b8bb20d7acdb..6e6797d145dd 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -1661,10 +1661,28 @@ static void hub_disconnect(struct usb_interface *intf)
 	kref_put(&hub->kref, hub_release);
 }
 
+static bool hub_descriptor_is_sane(struct usb_host_interface *desc)
+{
+	/* Some hubs have a subclass of 1, which AFAICT according to the */
+	/*  specs is not defined, but it works */
+	if (desc->desc.bInterfaceSubClass != 0 &&
+	    desc->desc.bInterfaceSubClass != 1)
+		return false;
+
+	/* Multiple endpoints? What kind of mutant ninja-hub is this? */
+	if (desc->desc.bNumEndpoints != 1)
+		return false;
+
+	/* If the first endpoint is not interrupt IN, we'd better punt! */
+	if (!usb_endpoint_is_int_in(&desc->endpoint[0].desc))
+		return false;
+
+        return true;
+}
+
 static int hub_probe(struct usb_interface *intf, const struct usb_device_id *id)
 {
 	struct usb_host_interface *desc;
-	struct usb_endpoint_descriptor *endpoint;
 	struct usb_device *hdev;
 	struct usb_hub *hub;
 
@@ -1739,25 +1757,11 @@ static int hub_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	}
 #endif
 
-	/* Some hubs have a subclass of 1, which AFAICT according to the */
-	/*  specs is not defined, but it works */
-	if ((desc->desc.bInterfaceSubClass != 0) &&
-	    (desc->desc.bInterfaceSubClass != 1)) {
-descriptor_error:
+	if (!hub_descriptor_is_sane(desc)) {
 		dev_err(&intf->dev, "bad descriptor, ignoring hub\n");
 		return -EIO;
 	}
 
-	/* Multiple endpoints? What kind of mutant ninja-hub is this? */
-	if (desc->desc.bNumEndpoints != 1)
-		goto descriptor_error;
-
-	endpoint = &desc->endpoint[0].desc;
-
-	/* If it's not an interrupt in endpoint, we'd better punt! */
-	if (!usb_endpoint_is_int_in(endpoint))
-		goto descriptor_error;
-
 	/* We found a hub */
 	dev_info(&intf->dev, "USB hub found\n");
 
@@ -1784,7 +1788,7 @@ descriptor_error:
 	if (id->driver_info & HUB_QUIRK_CHECK_PORT_AUTOSUSPEND)
 		hub->quirk_check_port_auto_suspend = 1;
 
-	if (hub_configure(hub, endpoint) >= 0)
+	if (hub_configure(hub, &desc->endpoint[0].desc) >= 0)
 		return 0;
 
 	hub_disconnect(intf);
@@ -3155,12 +3159,6 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 		if (PMSG_IS_AUTO(msg))
 			goto err_ltm;
 	}
-	if (usb_unlocked_disable_lpm(udev)) {
-		dev_err(&udev->dev, "Failed to disable LPM before suspend\n.");
-		status = -ENOMEM;
-		if (PMSG_IS_AUTO(msg))
-			goto err_lpm3;
-	}
 
 	/* see 7.1.7.6 */
 	if (hub_is_superspeed(hub->hdev))
@@ -3187,9 +3185,7 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 	if (status) {
 		dev_dbg(&port_dev->dev, "can't suspend, status %d\n", status);
 
-		/* Try to enable USB3 LPM and LTM again */
-		usb_unlocked_enable_lpm(udev);
- err_lpm3:
+		/* Try to enable USB3 LTM again */
 		usb_enable_ltm(udev);
  err_ltm:
 		/* Try to enable USB2 hardware LPM again */
@@ -3473,9 +3469,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 LTM and LPM */
+		/* Try to enable USB3 LTM */
 		usb_enable_ltm(udev);
-		usb_unlocked_enable_lpm(udev);
 	}
 
 	usb_unlock_port(port_dev);
diff --git a/drivers/usb/core/ledtrig-usbport.c b/drivers/usb/core/ledtrig-usbport.c
index 1713248ab15a..16c19a31dad1 100644
--- a/drivers/usb/core/ledtrig-usbport.c
+++ b/drivers/usb/core/ledtrig-usbport.c
@@ -11,8 +11,10 @@
 #include <linux/device.h>
 #include <linux/leds.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/slab.h>
 #include <linux/usb.h>
+#include <linux/usb/of.h>
 
 struct usbport_trig_data {
 	struct led_classdev *led_cdev;
@@ -123,6 +125,57 @@ static const struct attribute_group ports_group = {
  * Adding & removing ports
  ***************************************/
 
+/**
+ * usbport_trig_port_observed - Check if port should be observed
+ */
+static bool usbport_trig_port_observed(struct usbport_trig_data *usbport_data,
+				       struct usb_device *usb_dev, int port1)
+{
+	struct device *dev = usbport_data->led_cdev->dev;
+	struct device_node *led_np = dev->of_node;
+	struct of_phandle_args args;
+	struct device_node *port_np;
+	int count, i;
+
+	if (!led_np)
+		return false;
+
+	/* Get node of port being added */
+	port_np = usb_of_get_child_node(usb_dev->dev.of_node, port1);
+	if (!port_np)
+		return false;
+
+	/* Amount of trigger sources for this LED */
+	count = of_count_phandle_with_args(led_np, "trigger-sources",
+					   "#trigger-source-cells");
+	if (count < 0) {
+		dev_warn(dev, "Failed to get trigger sources for %s\n",
+			 led_np->full_name);
+		return false;
+	}
+
+	/* Check list of sources for this specific port */
+	for (i = 0; i < count; i++) {
+		int err;
+
+		err = of_parse_phandle_with_args(led_np, "trigger-sources",
+						 "#trigger-source-cells", i,
+						 &args);
+		if (err) {
+			dev_err(dev, "Failed to get trigger source phandle at index %d: %d\n",
+				i, err);
+			continue;
+		}
+
+		of_node_put(args.np);
+
+		if (args.np == port_np)
+			return true;
+	}
+
+	return false;
+}
+
 static int usbport_trig_add_port(struct usbport_trig_data *usbport_data,
 				 struct usb_device *usb_dev,
 				 const char *hub_name, int portnum)
@@ -141,6 +194,8 @@ static int usbport_trig_add_port(struct usbport_trig_data *usbport_data,
 	port->data = usbport_data;
 	port->hub = usb_dev;
 	port->portnum = portnum;
+	port->observed = usbport_trig_port_observed(usbport_data, usb_dev,
+						    portnum);
 
 	len = strlen(hub_name) + 8;
 	port->port_name = kzalloc(len, GFP_KERNEL);
@@ -255,6 +310,7 @@ static void usbport_trig_activate(struct led_classdev *led_cdev)
 	if (err)
 		goto err_free;
 	usb_for_each_dev(usbport_data, usbport_trig_add_usb_dev_ports);
+	usbport_trig_update_count(usbport_data);
 
 	/* Notifications */
 	usbport_data->nb.notifier_call = usbport_trig_notify,
diff --git a/drivers/usb/core/of.c b/drivers/usb/core/of.c
index d563cbcf76cf..3863bb1ce8c5 100644
--- a/drivers/usb/core/of.c
+++ b/drivers/usb/core/of.c
@@ -28,7 +28,8 @@
  *
  * Find the node from device tree according to its port number.
  *
- * Return: On success, a pointer to the device node, %NULL on failure.
+ * Return: A pointer to the node with incremented refcount if found, or
+ * %NULL otherwise.
  */
 struct device_node *usb_of_get_child_node(struct device_node *parent,
 					int portnum)
diff --git a/drivers/usb/core/quirks.c b/drivers/usb/core/quirks.c
index 96b21b0dac1e..3116edfcdc18 100644
--- a/drivers/usb/core/quirks.c
+++ b/drivers/usb/core/quirks.c
@@ -223,6 +223,10 @@ static const struct usb_device_id usb_quirk_list[] = {
 	/* Blackmagic Design UltraStudio SDI */
 	{ USB_DEVICE(0x1edb, 0xbd4f), .driver_info = USB_QUIRK_NO_LPM },
 
+	/* Hauppauge HVR-950q */
+	{ USB_DEVICE(0x2040, 0x7200), .driver_info =
+			USB_QUIRK_CONFIG_INTF_STRINGS },
+
 	/* INTEL VALUE SSD */
 	{ USB_DEVICE(0x8086, 0xf1a5), .driver_info = USB_QUIRK_RESET_RESUME },
 
diff --git a/drivers/usb/core/usb-acpi.c b/drivers/usb/core/usb-acpi.c
index 2776cfe64c09..ef9cf4a21afe 100644
--- a/drivers/usb/core/usb-acpi.c
+++ b/drivers/usb/core/usb-acpi.c
@@ -127,6 +127,22 @@ out:
  */
 #define USB_ACPI_LOCATION_VALID (1 << 31)
 
+static struct acpi_device *usb_acpi_find_port(struct acpi_device *parent,
+					      int raw)
+{
+	struct acpi_device *adev;
+
+	if (!parent)
+		return NULL;
+
+	list_for_each_entry(adev, &parent->children, node) {
+		if (acpi_device_adr(adev) == raw)
+			return adev;
+	}
+
+	return acpi_find_child_device(parent, raw, false);
+}
+
 static struct acpi_device *usb_acpi_find_companion(struct device *dev)
 {
 	struct usb_device *udev;
@@ -174,8 +190,10 @@ static struct acpi_device *usb_acpi_find_companion(struct device *dev)
 			int raw;
 
 			raw = usb_hcd_find_raw_port_number(hcd, port1);
-			adev = acpi_find_child_device(ACPI_COMPANION(&udev->dev),
-					raw, false);
+
+			adev = usb_acpi_find_port(ACPI_COMPANION(&udev->dev),
+						  raw);
+
 			if (!adev)
 				return NULL;
 		} else {
@@ -186,7 +204,9 @@ static struct acpi_device *usb_acpi_find_companion(struct device *dev)
 				return NULL;
 
 			acpi_bus_get_device(parent_handle, &adev);
-			adev = acpi_find_child_device(adev, port1, false);
+
+			adev = usb_acpi_find_port(adev, port1);
+
 			if (!adev)
 				return NULL;
 		}
diff --git a/drivers/usb/core/usb.c b/drivers/usb/core/usb.c
index 28b053cacc90..17681d5638ac 100644
--- a/drivers/usb/core/usb.c
+++ b/drivers/usb/core/usb.c
@@ -416,6 +416,7 @@ static void usb_release_dev(struct device *dev)
 
 	usb_destroy_configuration(udev);
 	usb_release_bos_descriptor(udev);
+	of_node_put(dev->of_node);
 	usb_put_hcd(hcd);
 	kfree(udev->product);
 	kfree(udev->manufacturer);
@@ -614,6 +615,7 @@ struct usb_device *usb_alloc_dev(struct usb_device *parent,
 		dev->route = 0;
 
 		dev->dev.parent = bus->controller;
+		device_set_of_node_from_dev(&dev->dev, bus->sysdev);
 		dev_set_name(&dev->dev, "usb%d", bus->busnum);
 		root_hub = 1;
 	} else {
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index 455d89a1cd6d..326b302fc440 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -151,11 +151,24 @@ static void __dwc3_set_mode(struct work_struct *work)
 	switch (dwc->desired_dr_role) {
 	case DWC3_GCTL_PRTCAP_HOST:
 		ret = dwc3_host_init(dwc);
-		if (ret)
+		if (ret) {
 			dev_err(dwc->dev, "failed to initialize host\n");
+		} else {
+			if (dwc->usb2_phy)
+				otg_set_vbus(dwc->usb2_phy->otg, true);
+			if (dwc->usb2_generic_phy)
+				phy_set_mode(dwc->usb2_generic_phy, PHY_MODE_USB_HOST);
+
+		}
 		break;
 	case DWC3_GCTL_PRTCAP_DEVICE:
 		dwc3_event_buffers_setup(dwc);
+
+		if (dwc->usb2_phy)
+			otg_set_vbus(dwc->usb2_phy->otg, false);
+		if (dwc->usb2_generic_phy)
+			phy_set_mode(dwc->usb2_generic_phy, PHY_MODE_USB_DEVICE);
+
 		ret = dwc3_gadget_init(dwc);
 		if (ret)
 			dev_err(dwc->dev, "failed to initialize peripheral\n");
@@ -721,6 +734,8 @@ static void dwc3_core_setup_global_control(struct dwc3 *dwc)
 	dwc3_writel(dwc->regs, DWC3_GCTL, reg);
 }
 
+static int dwc3_core_get_phy(struct dwc3 *dwc);
+
 /**
  * dwc3_core_init - Low-level initialization of DWC3 Core
  * @dwc: Pointer to our controller context structure
@@ -759,6 +774,10 @@ static int dwc3_core_init(struct dwc3 *dwc)
 	if (ret)
 		goto err0;
 
+	ret = dwc3_core_get_phy(dwc);
+	if (ret)
+		goto err0;
+
 	dwc3_core_setup_global_control(dwc);
 	dwc3_core_num_eps(dwc);
 
@@ -796,13 +815,19 @@ static int dwc3_core_init(struct dwc3 *dwc)
 		dwc3_writel(dwc->regs, DWC3_GUCTL2, reg);
 	}
 
-	/*
-	 * Enable hardware control of sending remote wakeup in HS when
-	 * the device is in the L1 state.
-	 */
-	if (dwc->revision >= DWC3_REVISION_290A) {
+	if (dwc->revision >= DWC3_REVISION_250A) {
 		reg = dwc3_readl(dwc->regs, DWC3_GUCTL1);
-		reg |= DWC3_GUCTL1_DEV_L1_EXIT_BY_HW;
+
+		/*
+		 * Enable hardware control of sending remote wakeup
+		 * in HS when the device is in the L1 state.
+		 */
+		if (dwc->revision >= DWC3_REVISION_290A)
+			reg |= DWC3_GUCTL1_DEV_L1_EXIT_BY_HW;
+
+		if (dwc->dis_tx_ipgap_linecheck_quirk)
+			reg |= DWC3_GUCTL1_TX_IPGAP_LINECHECK_DIS;
+
 		dwc3_writel(dwc->regs, DWC3_GUCTL1, reg);
 	}
 
@@ -903,6 +928,12 @@ static int dwc3_core_init_mode(struct dwc3 *dwc)
 	switch (dwc->dr_mode) {
 	case USB_DR_MODE_PERIPHERAL:
 		dwc3_set_prtcap(dwc, DWC3_GCTL_PRTCAP_DEVICE);
+
+		if (dwc->usb2_phy)
+			otg_set_vbus(dwc->usb2_phy->otg, false);
+		if (dwc->usb2_generic_phy)
+			phy_set_mode(dwc->usb2_generic_phy, PHY_MODE_USB_DEVICE);
+
 		ret = dwc3_gadget_init(dwc);
 		if (ret) {
 			if (ret != -EPROBE_DEFER)
@@ -912,6 +943,12 @@ static int dwc3_core_init_mode(struct dwc3 *dwc)
 		break;
 	case USB_DR_MODE_HOST:
 		dwc3_set_prtcap(dwc, DWC3_GCTL_PRTCAP_HOST);
+
+		if (dwc->usb2_phy)
+			otg_set_vbus(dwc->usb2_phy->otg, true);
+		if (dwc->usb2_generic_phy)
+			phy_set_mode(dwc->usb2_generic_phy, PHY_MODE_USB_HOST);
+
 		ret = dwc3_host_init(dwc);
 		if (ret) {
 			if (ret != -EPROBE_DEFER)
@@ -1023,6 +1060,8 @@ static void dwc3_get_properties(struct dwc3 *dwc)
 				"snps,dis-u2-freeclk-exists-quirk");
 	dwc->dis_del_phy_power_chg_quirk = device_property_read_bool(dev,
 				"snps,dis-del-phy-power-chg-quirk");
+	dwc->dis_tx_ipgap_linecheck_quirk = device_property_read_bool(dev,
+				"snps,dis-tx-ipgap-linecheck-quirk");
 
 	dwc->tx_de_emphasis_quirk = device_property_read_bool(dev,
 				"snps,tx_de_emphasis_quirk");
@@ -1148,10 +1187,6 @@ static int dwc3_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, dwc);
 	dwc3_cache_hwparams(dwc);
 
-	ret = dwc3_core_get_phy(dwc);
-	if (ret)
-		goto err0;
-
 	spin_lock_init(&dwc->lock);
 
 	pm_runtime_set_active(dev);
diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index 981c77f5628e..ea910acb4bb0 100644
--- a/drivers/usb/dwc3/core.h
+++ b/drivers/usb/dwc3/core.h
@@ -1,4 +1,4 @@
-/**
+/*
  * core.h - DesignWare USB3 DRD Core Header
  *
  * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com
@@ -204,6 +204,7 @@
 #define DWC3_GCTL_DSBLCLKGTNG		BIT(0)
 
 /* Global User Control 1 Register */
+#define DWC3_GUCTL1_TX_IPGAP_LINECHECK_DIS	BIT(28)
 #define DWC3_GUCTL1_DEV_L1_EXIT_BY_HW	BIT(24)
 
 /* Global USB2 PHY Configuration Register */
@@ -522,7 +523,6 @@ struct dwc3_event_buffer {
  * @trb_pool_dma: dma address of @trb_pool
  * @trb_enqueue: enqueue 'pointer' into TRB array
  * @trb_dequeue: dequeue 'pointer' into TRB array
- * @desc: usb_endpoint_descriptor pointer
  * @dwc: pointer to DWC controller
  * @saved_state: ep state saved during hibernation
  * @flags: endpoint flags (wedged, stalled, ...)
@@ -664,7 +664,7 @@ enum dwc3_link_state {
  * @bpl: DW0-3
  * @bph: DW4-7
  * @size: DW8-B
- * @trl: DWC-F
+ * @ctrl: DWC-F
  */
 struct dwc3_trb {
 	u32		bpl;
@@ -674,16 +674,16 @@ struct dwc3_trb {
 } __packed;
 
 /**
- * dwc3_hwparams - copy of HWPARAMS registers
- * @hwparams0 - GHWPARAMS0
- * @hwparams1 - GHWPARAMS1
- * @hwparams2 - GHWPARAMS2
- * @hwparams3 - GHWPARAMS3
- * @hwparams4 - GHWPARAMS4
- * @hwparams5 - GHWPARAMS5
- * @hwparams6 - GHWPARAMS6
- * @hwparams7 - GHWPARAMS7
- * @hwparams8 - GHWPARAMS8
+ * struct dwc3_hwparams - copy of HWPARAMS registers
+ * @hwparams0: GHWPARAMS0
+ * @hwparams1: GHWPARAMS1
+ * @hwparams2: GHWPARAMS2
+ * @hwparams3: GHWPARAMS3
+ * @hwparams4: GHWPARAMS4
+ * @hwparams5: GHWPARAMS5
+ * @hwparams6: GHWPARAMS6
+ * @hwparams7: GHWPARAMS7
+ * @hwparams8: GHWPARAMS8
  */
 struct dwc3_hwparams {
 	u32	hwparams0;
@@ -730,7 +730,8 @@ struct dwc3_hwparams {
  * @unaligned: true for OUT endpoints with length not divisible by maxp
  * @direction: IN or OUT direction flag
  * @mapped: true when request has been dma-mapped
- * @queued: true when request has been queued to HW
+ * @started: request is started
+ * @zero: wants a ZLP
  */
 struct dwc3_request {
 	struct usb_request	request;
@@ -761,17 +762,23 @@ struct dwc3_scratchpad_array {
 
 /**
  * struct dwc3 - representation of our controller
- * @drd_work - workqueue used for role swapping
+ * @drd_work: workqueue used for role swapping
  * @ep0_trb: trb which is used for the ctrl_req
+ * @bounce: address of bounce buffer
+ * @scratchbuf: address of scratch buffer
  * @setup_buf: used while precessing STD USB requests
- * @ep0_trb: dma address of ep0_trb
+ * @ep0_trb_addr: dma address of @ep0_trb
+ * @bounce_addr: dma address of @bounce
  * @ep0_usb_req: dummy req used while handling STD USB requests
  * @scratch_addr: dma address of scratchbuf
  * @ep0_in_setup: one control transfer is completed and enter setup phase
  * @lock: for synchronizing
  * @dev: pointer to our struct device
+ * @sysdev: pointer to the DMA-capable device
  * @xhci: pointer to our xHCI child
- * @event_buffer_list: a list of event buffers
+ * @xhci_resources: struct resources for our @xhci child
+ * @ev_buf: struct dwc3_event_buffer pointer
+ * @eps: endpoint array
  * @gadget: device side representation of the peripheral controller
  * @gadget_driver: pointer to the gadget driver
  * @regs: base address for our registers
@@ -795,8 +802,6 @@ struct dwc3_scratchpad_array {
  * @usb2_generic_phy: pointer to USB2 PHY
  * @usb3_generic_phy: pointer to USB3 PHY
  * @ulpi: pointer to ulpi interface
- * @dcfg: saved contents of DCFG register
- * @gctl: saved contents of GCTL register
  * @isoch_delay: wValue from Set Isochronous Delay request;
  * @u2sel: parameter from Set SEL request.
  * @u2pel: parameter from Set SEL request.
@@ -830,7 +835,6 @@ struct dwc3_scratchpad_array {
  * @pending_events: true when we have pending IRQs to be handled
  * @pullups_connected: true when Run/Stop bit is set
  * @setup_packet_pending: true when there's a Setup Packet in FIFO. Workaround
- * @start_config_issued: true when StartConfig command has been issued
  * @three_stage_setup: set if we perform a three phase setup
  * @usb3_lpm_capable: set if hadrware supports Link Power Management
  * @disable_scramble_quirk: set if we enable the disable scramble quirk
@@ -845,11 +849,14 @@ struct dwc3_scratchpad_array {
  * @dis_u2_susphy_quirk: set if we disable usb2 suspend phy
  * @dis_enblslpm_quirk: set if we clear enblslpm in GUSB2PHYCFG,
  *                      disabling the suspend signal to the PHY.
+ * @dis_rxdet_inp3_quirk: set if we disable Rx.Detect in P3
  * @dis_u2_freeclk_exists_quirk : set if we clear u2_freeclk_exists
  *			in GUSB2PHYCFG, specify that USB2 PHY doesn't
  *			provide a free-running PHY clock.
  * @dis_del_phy_power_chg_quirk: set if we disable delay phy power
  *			change quirk.
+ * @dis_tx_ipgap_linecheck_quirk: set if we disable u2mac linestate
+ *			check during HS transmit.
  * @tx_de_emphasis_quirk: set if we enable Tx de-emphasis quirk
  * @tx_de_emphasis: Tx de-emphasis value
  * 	0	- -6dB de-emphasis
@@ -1004,6 +1011,7 @@ struct dwc3 {
 	unsigned		dis_rxdet_inp3_quirk:1;
 	unsigned		dis_u2_freeclk_exists_quirk:1;
 	unsigned		dis_del_phy_power_chg_quirk:1;
+	unsigned		dis_tx_ipgap_linecheck_quirk:1;
 
 	unsigned		tx_de_emphasis_quirk:1;
 	unsigned		tx_de_emphasis:2;
diff --git a/drivers/usb/dwc3/debug.h b/drivers/usb/dwc3/debug.h
index cb2d8d3f7f3d..5e9c070ec874 100644
--- a/drivers/usb/dwc3/debug.h
+++ b/drivers/usb/dwc3/debug.h
@@ -173,9 +173,8 @@ static inline const char *dwc3_ep0_state_string(enum dwc3_ep0_state state)
  * @event: the event code
  */
 static inline const char *
-dwc3_gadget_event_string(const struct dwc3_event_devt *event)
+dwc3_gadget_event_string(char *str, const struct dwc3_event_devt *event)
 {
-	static char str[256];
 	enum dwc3_link_state state = event->event_info & DWC3_LINK_STATE_MASK;
 
 	switch (event->type) {
@@ -223,15 +222,249 @@ dwc3_gadget_event_string(const struct dwc3_event_devt *event)
 	return str;
 }
 
+static inline void dwc3_decode_get_status(__u8 t, __u16 i, __u16 l, char *str)
+{
+	switch (t & USB_RECIP_MASK) {
+	case USB_RECIP_INTERFACE:
+		sprintf(str, "Get Interface Status(Intf = %d, Length = %d)",
+			i, l);
+		break;
+	case USB_RECIP_ENDPOINT:
+		sprintf(str, "Get Endpoint Status(ep%d%s)",
+			i & ~USB_DIR_IN,
+			i & USB_DIR_IN ? "in" : "out");
+		break;
+	}
+}
+
+static inline void dwc3_decode_set_clear_feature(__u8 t, __u8 b, __u16 v,
+						 __u16 i, char *str)
+{
+	switch (t & USB_RECIP_MASK) {
+	case USB_RECIP_DEVICE:
+		sprintf(str, "%s Device Feature(%s%s)",
+			b == USB_REQ_CLEAR_FEATURE ? "Clear" : "Set",
+			({char *s;
+				switch (v) {
+				case USB_DEVICE_SELF_POWERED:
+					s = "Self Powered";
+					break;
+				case USB_DEVICE_REMOTE_WAKEUP:
+					s = "Remote Wakeup";
+					break;
+				case USB_DEVICE_TEST_MODE:
+					s = "Test Mode";
+					break;
+				default:
+					s = "UNKNOWN";
+				} s; }),
+			v == USB_DEVICE_TEST_MODE ?
+			({ char *s;
+				switch (i) {
+				case TEST_J:
+					s = ": TEST_J";
+					break;
+				case TEST_K:
+					s = ": TEST_K";
+					break;
+				case TEST_SE0_NAK:
+					s = ": TEST_SE0_NAK";
+					break;
+				case TEST_PACKET:
+					s = ": TEST_PACKET";
+					break;
+				case TEST_FORCE_EN:
+					s = ": TEST_FORCE_EN";
+					break;
+				default:
+					s = ": UNKNOWN";
+				} s; }) : "");
+		break;
+	case USB_RECIP_INTERFACE:
+		sprintf(str, "%s Interface Feature(%s)",
+			b == USB_REQ_CLEAR_FEATURE ? "Clear" : "Set",
+			v == USB_INTRF_FUNC_SUSPEND ?
+			"Function Suspend" : "UNKNOWN");
+		break;
+	case USB_RECIP_ENDPOINT:
+		sprintf(str, "%s Endpoint Feature(%s ep%d%s)",
+			b == USB_REQ_CLEAR_FEATURE ? "Clear" : "Set",
+			v == USB_ENDPOINT_HALT ? "Halt" : "UNKNOWN",
+			i & ~USB_DIR_IN,
+			i & USB_DIR_IN ? "in" : "out");
+		break;
+	}
+}
+
+static inline void dwc3_decode_set_address(__u16 v, char *str)
+{
+	sprintf(str, "Set Address(Addr = %02x)", v);
+}
+
+static inline void dwc3_decode_get_set_descriptor(__u8 t, __u8 b, __u16 v,
+						  __u16 i, __u16 l, char *str)
+{
+	sprintf(str, "%s %s Descriptor(Index = %d, Length = %d)",
+		b == USB_REQ_GET_DESCRIPTOR ? "Get" : "Set",
+		({ char *s;
+			switch (v >> 8) {
+			case USB_DT_DEVICE:
+				s = "Device";
+				break;
+			case USB_DT_CONFIG:
+				s = "Configuration";
+				break;
+			case USB_DT_STRING:
+				s = "String";
+				break;
+			case USB_DT_INTERFACE:
+				s = "Interface";
+				break;
+			case USB_DT_ENDPOINT:
+				s = "Endpoint";
+				break;
+			case USB_DT_DEVICE_QUALIFIER:
+				s = "Device Qualifier";
+				break;
+			case USB_DT_OTHER_SPEED_CONFIG:
+				s = "Other Speed Config";
+				break;
+			case USB_DT_INTERFACE_POWER:
+				s = "Interface Power";
+				break;
+			case USB_DT_OTG:
+				s = "OTG";
+				break;
+			case USB_DT_DEBUG:
+				s = "Debug";
+				break;
+			case USB_DT_INTERFACE_ASSOCIATION:
+				s = "Interface Association";
+				break;
+			case USB_DT_BOS:
+				s = "BOS";
+				break;
+			case USB_DT_DEVICE_CAPABILITY:
+				s = "Device Capability";
+				break;
+			case USB_DT_PIPE_USAGE:
+				s = "Pipe Usage";
+				break;
+			case USB_DT_SS_ENDPOINT_COMP:
+				s = "SS Endpoint Companion";
+				break;
+			case USB_DT_SSP_ISOC_ENDPOINT_COMP:
+				s = "SSP Isochronous Endpoint Companion";
+				break;
+			default:
+				s = "UNKNOWN";
+				break;
+			} s; }), v & 0xff, l);
+}
+
+
+static inline void dwc3_decode_get_configuration(__u16 l, char *str)
+{
+	sprintf(str, "Get Configuration(Length = %d)", l);
+}
+
+static inline void dwc3_decode_set_configuration(__u8 v, char *str)
+{
+	sprintf(str, "Set Configuration(Config = %d)", v);
+}
+
+static inline void dwc3_decode_get_intf(__u16 i, __u16 l, char *str)
+{
+	sprintf(str, "Get Interface(Intf = %d, Length = %d)", i, l);
+}
+
+static inline void dwc3_decode_set_intf(__u8 v, __u16 i, char *str)
+{
+	sprintf(str, "Set Interface(Intf = %d, Alt.Setting = %d)", i, v);
+}
+
+static inline void dwc3_decode_synch_frame(__u16 i, __u16 l, char *str)
+{
+	sprintf(str, "Synch Frame(Endpoint = %d, Length = %d)", i, l);
+}
+
+static inline void dwc3_decode_set_sel(__u16 l, char *str)
+{
+	sprintf(str, "Set SEL(Length = %d)", l);
+}
+
+static inline void dwc3_decode_set_isoch_delay(__u8 v, char *str)
+{
+	sprintf(str, "Set Isochronous Delay(Delay = %d ns)", v);
+}
+
+/**
+ * dwc3_decode_ctrl - returns a string represetion of ctrl request
+ */
+static inline const char *dwc3_decode_ctrl(char *str, __u8 bRequestType,
+		__u8 bRequest, __u16 wValue, __u16 wIndex, __u16 wLength)
+{
+	switch (bRequest) {
+	case USB_REQ_GET_STATUS:
+		dwc3_decode_get_status(bRequestType, wIndex, wLength, str);
+		break;
+	case USB_REQ_CLEAR_FEATURE:
+	case USB_REQ_SET_FEATURE:
+		dwc3_decode_set_clear_feature(bRequestType, bRequest, wValue,
+					      wIndex, str);
+		break;
+	case USB_REQ_SET_ADDRESS:
+		dwc3_decode_set_address(wValue, str);
+		break;
+	case USB_REQ_GET_DESCRIPTOR:
+	case USB_REQ_SET_DESCRIPTOR:
+		dwc3_decode_get_set_descriptor(bRequestType, bRequest, wValue,
+					       wIndex, wLength, str);
+		break;
+	case USB_REQ_GET_CONFIGURATION:
+		dwc3_decode_get_configuration(wLength, str);
+		break;
+	case USB_REQ_SET_CONFIGURATION:
+		dwc3_decode_set_configuration(wValue, str);
+		break;
+	case USB_REQ_GET_INTERFACE:
+		dwc3_decode_get_intf(wIndex, wLength, str);
+		break;
+	case USB_REQ_SET_INTERFACE:
+		dwc3_decode_set_intf(wValue, wIndex, str);
+		break;
+	case USB_REQ_SYNCH_FRAME:
+		dwc3_decode_synch_frame(wIndex, wLength, str);
+		break;
+	case USB_REQ_SET_SEL:
+		dwc3_decode_set_sel(wLength, str);
+		break;
+	case USB_REQ_SET_ISOCH_DELAY:
+		dwc3_decode_set_isoch_delay(wValue, str);
+		break;
+	default:
+		sprintf(str, "%02x %02x %02x %02x %02x %02x %02x %02x",
+			bRequestType, bRequest,
+			cpu_to_le16(wValue) & 0xff,
+			cpu_to_le16(wValue) >> 8,
+			cpu_to_le16(wIndex) & 0xff,
+			cpu_to_le16(wIndex) >> 8,
+			cpu_to_le16(wLength) & 0xff,
+			cpu_to_le16(wLength) >> 8);
+	}
+
+	return str;
+}
+
 /**
  * dwc3_ep_event_string - returns event name
  * @event: then event code
  */
 static inline const char *
-dwc3_ep_event_string(const struct dwc3_event_depevt *event, u32 ep0state)
+dwc3_ep_event_string(char *str, const struct dwc3_event_depevt *event,
+		     u32 ep0state)
 {
 	u8 epnum = event->endpoint_number;
-	static char str[256];
 	size_t len;
 	int status;
 	int ret;
@@ -332,14 +565,14 @@ static inline const char *dwc3_gadget_event_type_string(u8 event)
 	}
 }
 
-static inline const char *dwc3_decode_event(u32 event, u32 ep0state)
+static inline const char *dwc3_decode_event(char *str, u32 event, u32 ep0state)
 {
 	const union dwc3_event evt = (union dwc3_event) event;
 
 	if (evt.type.is_devspec)
-		return dwc3_gadget_event_string(&evt.devt);
+		return dwc3_gadget_event_string(str, &evt.devt);
 	else
-		return dwc3_ep_event_string(&evt.depevt, ep0state);
+		return dwc3_ep_event_string(str, &evt.depevt, ep0state);
 }
 
 static inline const char *dwc3_ep_cmd_status_string(int status)
diff --git a/drivers/usb/dwc3/debugfs.c b/drivers/usb/dwc3/debugfs.c
index 7be963dd8e3b..4e09be80e59f 100644
--- a/drivers/usb/dwc3/debugfs.c
+++ b/drivers/usb/dwc3/debugfs.c
@@ -653,16 +653,13 @@ static int dwc3_ep_trb_ring_show(struct seq_file *s, void *unused)
 		goto out;
 	}
 
-	seq_printf(s, "enqueue pointer %d\n", dep->trb_enqueue);
-	seq_printf(s, "dequeue pointer %d\n", dep->trb_dequeue);
-	seq_printf(s, "\n--------------------------------------------------\n\n");
 	seq_printf(s, "buffer_addr,size,type,ioc,isp_imi,csp,chn,lst,hwo\n");
 
 	for (i = 0; i < DWC3_TRB_NUM; i++) {
 		struct dwc3_trb *trb = &dep->trb_pool[i];
 		unsigned int type = DWC3_TRBCTL_TYPE(trb->ctrl);
 
-		seq_printf(s, "%08x%08x,%d,%s,%d,%d,%d,%d,%d,%d\n",
+		seq_printf(s, "%08x%08x,%d,%s,%d,%d,%d,%d,%d,%d       %c%c\n",
 				trb->bph, trb->bpl, trb->size,
 				dwc3_trb_type_string(type),
 				!!(trb->ctrl & DWC3_TRB_CTRL_IOC),
@@ -670,7 +667,9 @@ static int dwc3_ep_trb_ring_show(struct seq_file *s, void *unused)
 				!!(trb->ctrl & DWC3_TRB_CTRL_CSP),
 				!!(trb->ctrl & DWC3_TRB_CTRL_CHN),
 				!!(trb->ctrl & DWC3_TRB_CTRL_LST),
-				!!(trb->ctrl & DWC3_TRB_CTRL_HWO));
+				!!(trb->ctrl & DWC3_TRB_CTRL_HWO),
+				dep->trb_enqueue == i ? 'E' : ' ',
+				dep->trb_dequeue == i ? 'D' : ' ');
 	}
 
 out:
diff --git a/drivers/usb/dwc3/dwc3-exynos.c b/drivers/usb/dwc3/dwc3-exynos.c
index 98f74ff66120..e089df72f766 100644
--- a/drivers/usb/dwc3/dwc3-exynos.c
+++ b/drivers/usb/dwc3/dwc3-exynos.c
@@ -125,12 +125,16 @@ static int dwc3_exynos_probe(struct platform_device *pdev)
 		dev_err(dev, "couldn't get clock\n");
 		return -EINVAL;
 	}
-	clk_prepare_enable(exynos->clk);
+	ret = clk_prepare_enable(exynos->clk);
+	if (ret)
+		return ret;
 
 	exynos->susp_clk = devm_clk_get(dev, "usbdrd30_susp_clk");
 	if (IS_ERR(exynos->susp_clk))
 		exynos->susp_clk = NULL;
-	clk_prepare_enable(exynos->susp_clk);
+	ret = clk_prepare_enable(exynos->susp_clk);
+	if (ret)
+		goto susp_clk_err;
 
 	if (of_device_is_compatible(node, "samsung,exynos7-dwusb3")) {
 		exynos->axius_clk = devm_clk_get(dev, "usbdrd30_axius_clk");
@@ -139,7 +143,9 @@ static int dwc3_exynos_probe(struct platform_device *pdev)
 			ret = -ENODEV;
 			goto axius_clk_err;
 		}
-		clk_prepare_enable(exynos->axius_clk);
+		ret = clk_prepare_enable(exynos->axius_clk);
+		if (ret)
+			goto axius_clk_err;
 	} else {
 		exynos->axius_clk = NULL;
 	}
@@ -197,6 +203,7 @@ vdd33_err:
 	clk_disable_unprepare(exynos->axius_clk);
 axius_clk_err:
 	clk_disable_unprepare(exynos->susp_clk);
+susp_clk_err:
 	clk_disable_unprepare(exynos->clk);
 	return ret;
 }
diff --git a/drivers/usb/dwc3/dwc3-pci.c b/drivers/usb/dwc3/dwc3-pci.c
index 84a2cebfc712..7e995df7a797 100644
--- a/drivers/usb/dwc3/dwc3-pci.c
+++ b/drivers/usb/dwc3/dwc3-pci.c
@@ -42,7 +42,7 @@
 #define PCI_DEVICE_ID_INTEL_CNPLP		0x9dee
 #define PCI_DEVICE_ID_INTEL_CNPH		0xa36e
 
-#define PCI_INTEL_BXT_DSM_UUID		"732b85d5-b7a7-4a1b-9ba0-4bbd00ffd511"
+#define PCI_INTEL_BXT_DSM_GUID		"732b85d5-b7a7-4a1b-9ba0-4bbd00ffd511"
 #define PCI_INTEL_BXT_FUNC_PMU_PWR	4
 #define PCI_INTEL_BXT_STATE_D0		0
 #define PCI_INTEL_BXT_STATE_D3		3
@@ -51,14 +51,14 @@
  * struct dwc3_pci - Driver private structure
  * @dwc3: child dwc3 platform_device
  * @pci: our link to PCI bus
- * @uuid: _DSM UUID
+ * @guid: _DSM GUID
  * @has_dsm_for_pm: true for devices which need to run _DSM on runtime PM
  */
 struct dwc3_pci {
 	struct platform_device *dwc3;
 	struct pci_dev *pci;
 
-	u8 uuid[16];
+	guid_t guid;
 
 	unsigned int has_dsm_for_pm:1;
 };
@@ -120,7 +120,7 @@ static int dwc3_pci_quirks(struct dwc3_pci *dwc)
 
 		if (pdev->device == PCI_DEVICE_ID_INTEL_BXT ||
 				pdev->device == PCI_DEVICE_ID_INTEL_BXT_M) {
-			acpi_str_to_uuid(PCI_INTEL_BXT_DSM_UUID, dwc->uuid);
+			guid_parse(PCI_INTEL_BXT_DSM_GUID, &dwc->guid);
 			dwc->has_dsm_for_pm = true;
 		}
 
@@ -230,7 +230,6 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 	}
 
 	device_init_wakeup(dev, true);
-	device_set_run_wake(dev, true);
 	pci_set_drvdata(pci, dwc);
 	pm_runtime_put(dev);
 
@@ -292,7 +291,7 @@ static int dwc3_pci_dsm(struct dwc3_pci *dwc, int param)
 	tmp.type = ACPI_TYPE_INTEGER;
 	tmp.integer.value = param;
 
-	obj = acpi_evaluate_dsm(ACPI_HANDLE(&dwc->pci->dev), dwc->uuid,
+	obj = acpi_evaluate_dsm(ACPI_HANDLE(&dwc->pci->dev), &dwc->guid,
 			1, PCI_INTEL_BXT_FUNC_PMU_PWR, &argv4);
 	if (!obj) {
 		dev_err(&dwc->pci->dev, "failed to evaluate _DSM\n");
@@ -310,7 +309,7 @@ static int dwc3_pci_runtime_suspend(struct device *dev)
 {
 	struct dwc3_pci		*dwc = dev_get_drvdata(dev);
 
-	if (device_run_wake(dev))
+	if (device_can_wakeup(dev))
 		return dwc3_pci_dsm(dwc, PCI_INTEL_BXT_STATE_D3);
 
 	return -EBUSY;
diff --git a/drivers/usb/dwc3/dwc3-st.c b/drivers/usb/dwc3/dwc3-st.c
index dfbf464eb88c..505676fd3ba4 100644
--- a/drivers/usb/dwc3/dwc3-st.c
+++ b/drivers/usb/dwc3/dwc3-st.c
@@ -230,7 +230,7 @@ static int st_dwc3_probe(struct platform_device *pdev)
 
 	dwc3_data->syscfg_reg_off = res->start;
 
-	dev_vdbg(&pdev->dev, "glue-logic addr 0x%p, syscfg-reg offset 0x%x\n",
+	dev_vdbg(&pdev->dev, "glue-logic addr 0x%pK, syscfg-reg offset 0x%x\n",
 		 dwc3_data->glue_base, dwc3_data->syscfg_reg_off);
 
 	dwc3_data->rstc_pwrdn =
diff --git a/drivers/usb/dwc3/ep0.c b/drivers/usb/dwc3/ep0.c
index a78c78e7a8c3..827e376bfa97 100644
--- a/drivers/usb/dwc3/ep0.c
+++ b/drivers/usb/dwc3/ep0.c
@@ -1,4 +1,4 @@
-/**
+/*
  * ep0.c - DesignWare USB3 DRD Controller Endpoint 0 Handling
  *
  * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com
@@ -319,10 +319,16 @@ static int dwc3_ep0_handle_status(struct dwc3 *dwc,
 {
 	struct dwc3_ep		*dep;
 	u32			recip;
+	u32			value;
 	u32			reg;
 	u16			usb_status = 0;
 	__le16			*response_pkt;
 
+	/* We don't support PTM_STATUS */
+	value = le16_to_cpu(ctrl->wValue);
+	if (value != 0)
+		return -EINVAL;
+
 	recip = ctrl->bRequestType & USB_RECIP_MASK;
 	switch (recip) {
 	case USB_RECIP_DEVICE:
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index aea9a5b948b4..9e41605a276b 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -1,4 +1,4 @@
-/**
+/*
  * gadget.c - DesignWare USB3 DRD Controller Gadget Framework Link
  *
  * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com
@@ -36,13 +36,12 @@
 #include "io.h"
 
 /**
- * dwc3_gadget_set_test_mode - Enables USB2 Test Modes
+ * dwc3_gadget_set_test_mode - enables usb2 test modes
  * @dwc: pointer to our context structure
  * @mode: the mode to set (J, K SE0 NAK, Force Enable)
  *
- * Caller should take care of locking. This function will
- * return 0 on success or -EINVAL if wrong Test Selector
- * is passed
+ * Caller should take care of locking. This function will return 0 on
+ * success or -EINVAL if wrong Test Selector is passed.
  */
 int dwc3_gadget_set_test_mode(struct dwc3 *dwc, int mode)
 {
@@ -69,7 +68,7 @@ int dwc3_gadget_set_test_mode(struct dwc3 *dwc, int mode)
 }
 
 /**
- * dwc3_gadget_get_link_state - Gets current state of USB Link
+ * dwc3_gadget_get_link_state - gets current state of usb link
  * @dwc: pointer to our context structure
  *
  * Caller should take care of locking. This function will
@@ -85,7 +84,7 @@ int dwc3_gadget_get_link_state(struct dwc3 *dwc)
 }
 
 /**
- * dwc3_gadget_set_link_state - Sets USB Link to a particular State
+ * dwc3_gadget_set_link_state - sets usb link to a particular state
  * @dwc: pointer to our context structure
  * @state: the state to put link into
  *
@@ -143,8 +142,8 @@ int dwc3_gadget_set_link_state(struct dwc3 *dwc, enum dwc3_link_state state)
 }
 
 /**
- * dwc3_ep_inc_trb() - Increment a TRB index.
- * @index - Pointer to the TRB index to increment.
+ * dwc3_ep_inc_trb - increment a trb index.
+ * @index: Pointer to the TRB index to increment.
  *
  * The index should never point to the link TRB. After incrementing,
  * if it is point to the link TRB, wrap around to the beginning. The
@@ -157,16 +156,34 @@ static void dwc3_ep_inc_trb(u8 *index)
 		*index = 0;
 }
 
+/**
+ * dwc3_ep_inc_enq - increment endpoint's enqueue pointer
+ * @dep: The endpoint whose enqueue pointer we're incrementing
+ */
 static void dwc3_ep_inc_enq(struct dwc3_ep *dep)
 {
 	dwc3_ep_inc_trb(&dep->trb_enqueue);
 }
 
+/**
+ * dwc3_ep_inc_deq - increment endpoint's dequeue pointer
+ * @dep: The endpoint whose enqueue pointer we're incrementing
+ */
 static void dwc3_ep_inc_deq(struct dwc3_ep *dep)
 {
 	dwc3_ep_inc_trb(&dep->trb_dequeue);
 }
 
+/**
+ * dwc3_gadget_giveback - call struct usb_request's ->complete callback
+ * @dep: The endpoint to whom the request belongs to
+ * @req: The request we're giving back
+ * @status: completion code for the request
+ *
+ * Must be called with controller's lock held and interrupts disabled. This
+ * function will unmap @req and call its ->complete() callback to notify upper
+ * layers that it has completed.
+ */
 void dwc3_gadget_giveback(struct dwc3_ep *dep, struct dwc3_request *req,
 		int status)
 {
@@ -193,6 +210,15 @@ void dwc3_gadget_giveback(struct dwc3_ep *dep, struct dwc3_request *req,
 		pm_runtime_put(dwc->dev);
 }
 
+/**
+ * dwc3_send_gadget_generic_command - issue a generic command for the controller
+ * @dwc: pointer to the controller context
+ * @cmd: the command to be issued
+ * @param: command parameter
+ *
+ * Caller should take care of locking. Issue @cmd with a given @param to @dwc
+ * and wait for its completion.
+ */
 int dwc3_send_gadget_generic_command(struct dwc3 *dwc, unsigned cmd, u32 param)
 {
 	u32		timeout = 500;
@@ -225,6 +251,15 @@ int dwc3_send_gadget_generic_command(struct dwc3 *dwc, unsigned cmd, u32 param)
 
 static int __dwc3_gadget_wakeup(struct dwc3 *dwc);
 
+/**
+ * dwc3_send_gadget_ep_cmd - issue an endpoint command
+ * @dep: the endpoint to which the command is going to be issued
+ * @cmd: the command to be issued
+ * @params: parameters to the command
+ *
+ * Caller should handle locking. This function will issue @cmd with given
+ * @params to @dep and wait for its completion.
+ */
 int dwc3_send_gadget_ep_cmd(struct dwc3_ep *dep, unsigned cmd,
 		struct dwc3_gadget_ep_cmd_params *params)
 {
@@ -422,36 +457,38 @@ static void dwc3_free_trb_pool(struct dwc3_ep *dep)
 static int dwc3_gadget_set_xfer_resource(struct dwc3 *dwc, struct dwc3_ep *dep);
 
 /**
- * dwc3_gadget_start_config - Configure EP resources
+ * dwc3_gadget_start_config - configure ep resources
  * @dwc: pointer to our controller context structure
  * @dep: endpoint that is being enabled
  *
- * The assignment of transfer resources cannot perfectly follow the
- * data book due to the fact that the controller driver does not have
- * all knowledge of the configuration in advance. It is given this
- * information piecemeal by the composite gadget framework after every
- * SET_CONFIGURATION and SET_INTERFACE. Trying to follow the databook
- * programming model in this scenario can cause errors. For two
- * reasons:
+ * Issue a %DWC3_DEPCMD_DEPSTARTCFG command to @dep. After the command's
+ * completion, it will set Transfer Resource for all available endpoints.
  *
- * 1) The databook says to do DEPSTARTCFG for every SET_CONFIGURATION
- * and SET_INTERFACE (8.1.5). This is incorrect in the scenario of
- * multiple interfaces.
+ * The assignment of transfer resources cannot perfectly follow the data book
+ * due to the fact that the controller driver does not have all knowledge of the
+ * configuration in advance. It is given this information piecemeal by the
+ * composite gadget framework after every SET_CONFIGURATION and
+ * SET_INTERFACE. Trying to follow the databook programming model in this
+ * scenario can cause errors. For two reasons:
  *
- * 2) The databook does not mention doing more DEPXFERCFG for new
+ * 1) The databook says to do %DWC3_DEPCMD_DEPSTARTCFG for every
+ * %USB_REQ_SET_CONFIGURATION and %USB_REQ_SET_INTERFACE (8.1.5). This is
+ * incorrect in the scenario of multiple interfaces.
+ *
+ * 2) The databook does not mention doing more %DWC3_DEPCMD_DEPXFERCFG for new
  * endpoint on alt setting (8.1.6).
  *
  * The following simplified method is used instead:
  *
- * All hardware endpoints can be assigned a transfer resource and this
- * setting will stay persistent until either a core reset or
- * hibernation. So whenever we do a DEPSTARTCFG(0) we can go ahead and
- * do DEPXFERCFG for every hardware endpoint as well. We are
+ * All hardware endpoints can be assigned a transfer resource and this setting
+ * will stay persistent until either a core reset or hibernation. So whenever we
+ * do a %DWC3_DEPCMD_DEPSTARTCFG(0) we can go ahead and do
+ * %DWC3_DEPCMD_DEPXFERCFG for every hardware endpoint as well. We are
  * guaranteed that there are as many transfer resources as endpoints.
  *
- * This function is called for each endpoint when it is being enabled
- * but is triggered only when called for EP0-out, which always happens
- * first, and which should only happen in one of the above conditions.
+ * This function is called for each endpoint when it is being enabled but is
+ * triggered only when called for EP0-out, which always happens first, and which
+ * should only happen in one of the above conditions.
  */
 static int dwc3_gadget_start_config(struct dwc3 *dwc, struct dwc3_ep *dep)
 {
@@ -569,11 +606,13 @@ static int dwc3_gadget_set_xfer_resource(struct dwc3 *dwc, struct dwc3_ep *dep)
 }
 
 /**
- * __dwc3_gadget_ep_enable - Initializes a HW endpoint
+ * __dwc3_gadget_ep_enable - initializes a hw endpoint
  * @dep: endpoint to be initialized
- * @desc: USB Endpoint Descriptor
+ * @modify: if true, modify existing endpoint configuration
+ * @restore: if true, restore endpoint configuration from scratch buffer
  *
- * Caller should take care of locking
+ * Caller should take care of locking. Execute all necessary commands to
+ * initialize a HW endpoint so it can be used by a gadget driver.
  */
 static int __dwc3_gadget_ep_enable(struct dwc3_ep *dep,
 		bool modify, bool restore)
@@ -685,11 +724,13 @@ static void dwc3_remove_requests(struct dwc3 *dwc, struct dwc3_ep *dep)
 }
 
 /**
- * __dwc3_gadget_ep_disable - Disables a HW endpoint
+ * __dwc3_gadget_ep_disable - disables a hw endpoint
  * @dep: the endpoint to disable
  *
- * This function also removes requests which are currently processed ny the
- * hardware and those which are not yet scheduled.
+ * This function undoes what __dwc3_gadget_ep_enable did and also removes
+ * requests which are currently being processed by the hardware and those which
+ * are not yet scheduled.
+ *
  * Caller should take care of locking.
  */
 static int __dwc3_gadget_ep_disable(struct dwc3_ep *dep)
@@ -932,7 +973,7 @@ static void dwc3_prepare_one_trb(struct dwc3_ep *dep,
 }
 
 /**
- * dwc3_ep_prev_trb() - Returns the previous TRB in the ring
+ * dwc3_ep_prev_trb - returns the previous TRB in the ring
  * @dep: The endpoint with the TRB ring
  * @index: The index of the current TRB in the ring
  *
@@ -953,7 +994,6 @@ static struct dwc3_trb *dwc3_ep_prev_trb(struct dwc3_ep *dep, u8 index)
 static u32 dwc3_calc_trbs_left(struct dwc3_ep *dep)
 {
 	struct dwc3_trb		*tmp;
-	struct dwc3		*dwc = dep->dwc;
 	u8			trbs_left;
 
 	/*
@@ -965,8 +1005,7 @@ static u32 dwc3_calc_trbs_left(struct dwc3_ep *dep)
 	 */
 	if (dep->trb_enqueue == dep->trb_dequeue) {
 		tmp = dwc3_ep_prev_trb(dep, dep->trb_enqueue);
-		if (dev_WARN_ONCE(dwc->dev, tmp->ctrl & DWC3_TRB_CTRL_HWO,
-				  "%s No TRBS left\n", dep->name))
+		if (tmp->ctrl & DWC3_TRB_CTRL_HWO)
 			return 0;
 
 		return DWC3_TRB_NUM - 1;
@@ -1101,6 +1140,17 @@ static void dwc3_prepare_trbs(struct dwc3_ep *dep)
 	}
 
 	list_for_each_entry_safe(req, n, &dep->pending_list, list) {
+		struct dwc3	*dwc = dep->dwc;
+		int		ret;
+
+		ret = usb_gadget_map_request_by_dev(dwc->sysdev, &req->request,
+						    dep->direction);
+		if (ret)
+			return;
+
+		req->sg			= req->request.sg;
+		req->num_pending_sgs	= req->request.num_mapped_sgs;
+
 		if (req->num_pending_sgs > 0)
 			dwc3_prepare_one_trb_sg(dep, req);
 		else
@@ -1207,7 +1257,7 @@ static void dwc3_gadget_start_isoc(struct dwc3 *dwc,
 static int __dwc3_gadget_ep_queue(struct dwc3_ep *dep, struct dwc3_request *req)
 {
 	struct dwc3		*dwc = dep->dwc;
-	int			ret;
+	int			ret = 0;
 
 	if (!dep->endpoint.desc) {
 		dev_err(dwc->dev, "%s: can't queue to disabled endpoint\n",
@@ -1215,12 +1265,9 @@ static int __dwc3_gadget_ep_queue(struct dwc3_ep *dep, struct dwc3_request *req)
 		return -ESHUTDOWN;
 	}
 
-	if (WARN(req->dep != dep, "request %p belongs to '%s'\n",
-				&req->request, req->dep->name)) {
-		dev_err(dwc->dev, "%s: request %p belongs to '%s'\n",
-				dep->name, &req->request, req->dep->name);
+	if (WARN(req->dep != dep, "request %pK belongs to '%s'\n",
+				&req->request, req->dep->name))
 		return -EINVAL;
-	}
 
 	pm_runtime_get(dwc->dev);
 
@@ -1231,14 +1278,6 @@ static int __dwc3_gadget_ep_queue(struct dwc3_ep *dep, struct dwc3_request *req)
 
 	trace_dwc3_ep_queue(req);
 
-	ret = usb_gadget_map_request_by_dev(dwc->sysdev, &req->request,
-					    dep->direction);
-	if (ret)
-		return ret;
-
-	req->sg			= req->request.sg;
-	req->num_pending_sgs	= req->request.num_mapped_sgs;
-
 	list_add_tail(&req->list, &dep->pending_list);
 
 	/*
@@ -1396,7 +1435,7 @@ static int dwc3_gadget_ep_dequeue(struct usb_ep *ep,
 			}
 			goto out1;
 		}
-		dev_err(dwc->dev, "request %p was not queued to %s\n",
+		dev_err(dwc->dev, "request %pK was not queued to %s\n",
 				request, ep->name);
 		ret = -EINVAL;
 		goto out0;
@@ -1741,8 +1780,8 @@ static irqreturn_t dwc3_interrupt(int irq, void *_dwc);
 static irqreturn_t dwc3_thread_interrupt(int irq, void *_dwc);
 
 /**
- * dwc3_gadget_setup_nump - Calculate and initialize NUMP field of DCFG
- * dwc: pointer to our context structure
+ * dwc3_gadget_setup_nump - calculate and initialize NUMP field of %DWC3_DCFG
+ * @dwc: pointer to our context structure
  *
  * The following looks like complex but it's actually very simple. In order to
  * calculate the number of packets we can burst at once on OUT transfers, we're
@@ -1798,49 +1837,6 @@ static int __dwc3_gadget_start(struct dwc3 *dwc)
 		dwc3_writel(dwc->regs, DWC3_DEV_IMOD(0), 0);
 	}
 
-	reg = dwc3_readl(dwc->regs, DWC3_DCFG);
-	reg &= ~(DWC3_DCFG_SPEED_MASK);
-
-	/**
-	 * WORKAROUND: DWC3 revision < 2.20a have an issue
-	 * which would cause metastability state on Run/Stop
-	 * bit if we try to force the IP to USB2-only mode.
-	 *
-	 * Because of that, we cannot configure the IP to any
-	 * speed other than the SuperSpeed
-	 *
-	 * Refers to:
-	 *
-	 * STAR#9000525659: Clock Domain Crossing on DCTL in
-	 * USB 2.0 Mode
-	 */
-	if (dwc->revision < DWC3_REVISION_220A) {
-		reg |= DWC3_DCFG_SUPERSPEED;
-	} else {
-		switch (dwc->maximum_speed) {
-		case USB_SPEED_LOW:
-			reg |= DWC3_DCFG_LOWSPEED;
-			break;
-		case USB_SPEED_FULL:
-			reg |= DWC3_DCFG_FULLSPEED;
-			break;
-		case USB_SPEED_HIGH:
-			reg |= DWC3_DCFG_HIGHSPEED;
-			break;
-		case USB_SPEED_SUPER_PLUS:
-			reg |= DWC3_DCFG_SUPERSPEED_PLUS;
-			break;
-		default:
-			dev_err(dwc->dev, "invalid dwc->maximum_speed (%d)\n",
-				dwc->maximum_speed);
-			/* fall through */
-		case USB_SPEED_SUPER:
-			reg |= DWC3_DCFG_SUPERSPEED;
-			break;
-		}
-	}
-	dwc3_writel(dwc->regs, DWC3_DCFG, reg);
-
 	/*
 	 * We are telling dwc3 that we want to use DCFG.NUMP as ACK TP's NUMP
 	 * field instead of letting dwc3 itself calculate that automatically.
@@ -1972,6 +1968,63 @@ out:
 	return 0;
 }
 
+static void dwc3_gadget_set_speed(struct usb_gadget *g,
+				  enum usb_device_speed speed)
+{
+	struct dwc3		*dwc = gadget_to_dwc(g);
+	unsigned long		flags;
+	u32			reg;
+
+	spin_lock_irqsave(&dwc->lock, flags);
+	reg = dwc3_readl(dwc->regs, DWC3_DCFG);
+	reg &= ~(DWC3_DCFG_SPEED_MASK);
+
+	/*
+	 * WORKAROUND: DWC3 revision < 2.20a have an issue
+	 * which would cause metastability state on Run/Stop
+	 * bit if we try to force the IP to USB2-only mode.
+	 *
+	 * Because of that, we cannot configure the IP to any
+	 * speed other than the SuperSpeed
+	 *
+	 * Refers to:
+	 *
+	 * STAR#9000525659: Clock Domain Crossing on DCTL in
+	 * USB 2.0 Mode
+	 */
+	if (dwc->revision < DWC3_REVISION_220A) {
+		reg |= DWC3_DCFG_SUPERSPEED;
+	} else {
+		switch (speed) {
+		case USB_SPEED_LOW:
+			reg |= DWC3_DCFG_LOWSPEED;
+			break;
+		case USB_SPEED_FULL:
+			reg |= DWC3_DCFG_FULLSPEED;
+			break;
+		case USB_SPEED_HIGH:
+			reg |= DWC3_DCFG_HIGHSPEED;
+			break;
+		case USB_SPEED_SUPER:
+			reg |= DWC3_DCFG_SUPERSPEED;
+			break;
+		case USB_SPEED_SUPER_PLUS:
+			reg |= DWC3_DCFG_SUPERSPEED_PLUS;
+			break;
+		default:
+			dev_err(dwc->dev, "invalid speed (%d)\n", speed);
+
+			if (dwc->revision & DWC3_REVISION_IS_DWC31)
+				reg |= DWC3_DCFG_SUPERSPEED_PLUS;
+			else
+				reg |= DWC3_DCFG_SUPERSPEED;
+		}
+	}
+	dwc3_writel(dwc->regs, DWC3_DCFG, reg);
+
+	spin_unlock_irqrestore(&dwc->lock, flags);
+}
+
 static const struct usb_gadget_ops dwc3_gadget_ops = {
 	.get_frame		= dwc3_gadget_get_frame,
 	.wakeup			= dwc3_gadget_wakeup,
@@ -1979,19 +2032,21 @@ static const struct usb_gadget_ops dwc3_gadget_ops = {
 	.pullup			= dwc3_gadget_pullup,
 	.udc_start		= dwc3_gadget_start,
 	.udc_stop		= dwc3_gadget_stop,
+	.udc_set_speed		= dwc3_gadget_set_speed,
 };
 
 /* -------------------------------------------------------------------------- */
 
-static int dwc3_gadget_init_endpoints(struct dwc3 *dwc, u8 num)
+static int dwc3_gadget_init_endpoints(struct dwc3 *dwc, u8 total)
 {
 	struct dwc3_ep			*dep;
 	u8				epnum;
 
 	INIT_LIST_HEAD(&dwc->gadget.ep_list);
 
-	for (epnum = 0; epnum < num; epnum++) {
+	for (epnum = 0; epnum < total; epnum++) {
 		bool			direction = epnum & 1;
+		u8			num = epnum >> 1;
 
 		dep = kzalloc(sizeof(*dep), GFP_KERNEL);
 		if (!dep)
@@ -2003,7 +2058,7 @@ static int dwc3_gadget_init_endpoints(struct dwc3 *dwc, u8 num)
 		dep->regs = dwc->regs + DWC3_DEP_BASE(epnum);
 		dwc->eps[epnum] = dep;
 
-		snprintf(dep->name, sizeof(dep->name), "ep%d%s", epnum >> 1,
+		snprintf(dep->name, sizeof(dep->name), "ep%u%s", num,
 				direction ? "in" : "out");
 
 		dep->endpoint.name = dep->name;
@@ -2015,39 +2070,39 @@ static int dwc3_gadget_init_endpoints(struct dwc3 *dwc, u8 num)
 
 		spin_lock_init(&dep->lock);
 
-		if (epnum == 0 || epnum == 1) {
+		if (num == 0) {
 			usb_ep_set_maxpacket_limit(&dep->endpoint, 512);
 			dep->endpoint.maxburst = 1;
 			dep->endpoint.ops = &dwc3_gadget_ep0_ops;
-			if (!epnum)
+			if (!direction)
 				dwc->gadget.ep0 = &dep->endpoint;
 		} else if (direction) {
 			int mdwidth;
+			int kbytes;
 			int size;
 			int ret;
-			int num;
 
 			mdwidth = DWC3_MDWIDTH(dwc->hwparams.hwparams0);
 			/* MDWIDTH is represented in bits, we need it in bytes */
 			mdwidth /= 8;
 
-			size = dwc3_readl(dwc->regs, DWC3_GTXFIFOSIZ(epnum >> 1));
+			size = dwc3_readl(dwc->regs, DWC3_GTXFIFOSIZ(num));
 			size = DWC3_GTXFIFOSIZ_TXFDEF(size);
 
 			/* FIFO Depth is in MDWDITH bytes. Multiply */
 			size *= mdwidth;
 
-			num = size / 1024;
-			if (num == 0)
-				num = 1;
+			kbytes = size / 1024;
+			if (kbytes == 0)
+				kbytes = 1;
 
 			/*
-			 * FIFO sizes account an extra MDWIDTH * (num + 1) bytes for
+			 * FIFO sizes account an extra MDWIDTH * (kbytes + 1) bytes for
 			 * internal overhead. We don't really know how these are used,
 			 * but documentation say it exists.
 			 */
-			size -= mdwidth * (num + 1);
-			size /= num;
+			size -= mdwidth * (kbytes + 1);
+			size /= kbytes;
 
 			usb_ep_set_maxpacket_limit(&dep->endpoint, size);
 
@@ -2073,7 +2128,7 @@ static int dwc3_gadget_init_endpoints(struct dwc3 *dwc, u8 num)
 				return ret;
 		}
 
-		if (epnum == 0 || epnum == 1) {
+		if (num == 0) {
 			dep->endpoint.caps.type_control = true;
 		} else {
 			dep->endpoint.caps.type_iso = true;
@@ -2871,7 +2926,7 @@ static void dwc3_gadget_hibernation_interrupt(struct dwc3 *dwc,
 {
 	unsigned int is_ss = evtinfo & BIT(4);
 
-	/**
+	/*
 	 * WORKAROUND: DWC3 revison 2.20a with hibernation support
 	 * have a known issue which can cause USB CV TD.9.23 to fail
 	 * randomly.
@@ -2943,20 +2998,12 @@ static void dwc3_process_event_entry(struct dwc3 *dwc,
 {
 	trace_dwc3_event(event->raw, dwc);
 
-	/* Endpoint IRQ, handle it and return early */
-	if (event->type.is_devspec == 0) {
-		/* depevt */
-		return dwc3_endpoint_interrupt(dwc, &event->depevt);
-	}
-
-	switch (event->type.type) {
-	case DWC3_EVENT_TYPE_DEV:
+	if (!event->type.is_devspec)
+		dwc3_endpoint_interrupt(dwc, &event->depevt);
+	else if (event->type.type == DWC3_EVENT_TYPE_DEV)
 		dwc3_gadget_interrupt(dwc, &event->devt);
-		break;
-	/* REVISIT what to do with Carkit and I2C events ? */
-	default:
+	else
 		dev_err(dwc->dev, "UNKNOWN IRQ type %d\n", event->raw);
-	}
 }
 
 static irqreturn_t dwc3_process_event_buf(struct dwc3_event_buffer *evt)
@@ -3110,7 +3157,7 @@ out:
 }
 
 /**
- * dwc3_gadget_init - Initializes gadget related registers
+ * dwc3_gadget_init - initializes gadget related registers
  * @dwc: pointer to our controller context structure
  *
  * Returns 0 on success otherwise negative errno.
diff --git a/drivers/usb/dwc3/gadget.h b/drivers/usb/dwc3/gadget.h
index e4602d0e515b..4a3227543255 100644
--- a/drivers/usb/dwc3/gadget.h
+++ b/drivers/usb/dwc3/gadget.h
@@ -1,4 +1,4 @@
-/**
+/*
  * gadget.h - DesignWare USB3 DRD Gadget Header
  *
  * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com
@@ -60,11 +60,25 @@ struct dwc3;
 
 #define to_dwc3_request(r)	(container_of(r, struct dwc3_request, request))
 
+/**
+ * next_request - gets the next request on the given list
+ * @list: the request list to operate on
+ *
+ * Caller should take care of locking. This function return %NULL or the first
+ * request available on @list.
+ */
 static inline struct dwc3_request *next_request(struct list_head *list)
 {
 	return list_first_entry_or_null(list, struct dwc3_request, list);
 }
 
+/**
+ * dwc3_gadget_move_started_request - move @req to the started_list
+ * @req: the request to be moved
+ *
+ * Caller should take care of locking. This function will move @req from its
+ * current list to the endpoint's started_list.
+ */
 static inline void dwc3_gadget_move_started_request(struct dwc3_request *req)
 {
 	struct dwc3_ep		*dep = req->dep;
@@ -87,10 +101,10 @@ int __dwc3_gadget_ep_set_halt(struct dwc3_ep *dep, int value, int protocol);
 
 /**
  * dwc3_gadget_ep_get_transfer_index - Gets transfer index from HW
- * @dwc: DesignWare USB3 Pointer
- * @number: DWC endpoint number
+ * @dep: dwc3 endpoint
  *
- * Caller should take care of locking
+ * Caller should take care of locking. Returns the transfer resource
+ * index for a given endpoint.
  */
 static inline u32 dwc3_gadget_ep_get_transfer_index(struct dwc3_ep *dep)
 {
diff --git a/drivers/usb/dwc3/trace.h b/drivers/usb/dwc3/trace.h
index f1bd444d22a3..6504b116da04 100644
--- a/drivers/usb/dwc3/trace.h
+++ b/drivers/usb/dwc3/trace.h
@@ -60,13 +60,15 @@ DECLARE_EVENT_CLASS(dwc3_log_event,
 	TP_STRUCT__entry(
 		__field(u32, event)
 		__field(u32, ep0state)
+		__dynamic_array(char, str, DWC3_MSG_MAX)
 	),
 	TP_fast_assign(
 		__entry->event = event;
 		__entry->ep0state = dwc->ep0state;
 	),
 	TP_printk("event (%08x): %s", __entry->event,
-			dwc3_decode_event(__entry->event, __entry->ep0state))
+			dwc3_decode_event(__get_str(str), __entry->event,
+					  __entry->ep0state))
 );
 
 DEFINE_EVENT(dwc3_log_event, dwc3_event,
@@ -83,6 +85,7 @@ DECLARE_EVENT_CLASS(dwc3_log_ctrl,
 		__field(__u16, wValue)
 		__field(__u16, wIndex)
 		__field(__u16, wLength)
+		__dynamic_array(char, str, DWC3_MSG_MAX)
 	),
 	TP_fast_assign(
 		__entry->bRequestType = ctrl->bRequestType;
@@ -91,10 +94,9 @@ DECLARE_EVENT_CLASS(dwc3_log_ctrl,
 		__entry->wIndex = le16_to_cpu(ctrl->wIndex);
 		__entry->wLength = le16_to_cpu(ctrl->wLength);
 	),
-	TP_printk("bRequestType %02x bRequest %02x wValue %04x wIndex %04x wLength %d",
-		__entry->bRequestType, __entry->bRequest,
-		__entry->wValue, __entry->wIndex,
-		__entry->wLength
+	TP_printk("%s", dwc3_decode_ctrl(__get_str(str), __entry->bRequestType,
+					__entry->bRequest, __entry->wValue,
+					__entry->wIndex, __entry->wLength)
 	)
 );
 
@@ -107,7 +109,7 @@ DECLARE_EVENT_CLASS(dwc3_log_request,
 	TP_PROTO(struct dwc3_request *req),
 	TP_ARGS(req),
 	TP_STRUCT__entry(
-		__dynamic_array(char, name, DWC3_MSG_MAX)
+		__string(name, req->dep->name)
 		__field(struct dwc3_request *, req)
 		__field(unsigned, actual)
 		__field(unsigned, length)
@@ -117,7 +119,7 @@ DECLARE_EVENT_CLASS(dwc3_log_request,
 		__field(int, no_interrupt)
 	),
 	TP_fast_assign(
-		snprintf(__get_str(name), DWC3_MSG_MAX, "%s", req->dep->name);
+		__assign_str(name, req->dep->name);
 		__entry->req = req;
 		__entry->actual = req->request.actual;
 		__entry->length = req->request.length;
@@ -190,7 +192,7 @@ DECLARE_EVENT_CLASS(dwc3_log_gadget_ep_cmd,
 		struct dwc3_gadget_ep_cmd_params *params, int cmd_status),
 	TP_ARGS(dep, cmd, params, cmd_status),
 	TP_STRUCT__entry(
-		__dynamic_array(char, name, DWC3_MSG_MAX)
+		__string(name, dep->name)
 		__field(unsigned int, cmd)
 		__field(u32, param0)
 		__field(u32, param1)
@@ -198,7 +200,7 @@ DECLARE_EVENT_CLASS(dwc3_log_gadget_ep_cmd,
 		__field(int, cmd_status)
 	),
 	TP_fast_assign(
-		snprintf(__get_str(name), DWC3_MSG_MAX, "%s", dep->name);
+		__assign_str(name, dep->name);
 		__entry->cmd = cmd;
 		__entry->param0 = params->param0;
 		__entry->param1 = params->param1;
@@ -223,7 +225,7 @@ DECLARE_EVENT_CLASS(dwc3_log_trb,
 	TP_PROTO(struct dwc3_ep *dep, struct dwc3_trb *trb),
 	TP_ARGS(dep, trb),
 	TP_STRUCT__entry(
-		__dynamic_array(char, name, DWC3_MSG_MAX)
+		__string(name, dep->name)
 		__field(struct dwc3_trb *, trb)
 		__field(u32, allocated)
 		__field(u32, queued)
@@ -234,7 +236,7 @@ DECLARE_EVENT_CLASS(dwc3_log_trb,
 		__field(u32, type)
 	),
 	TP_fast_assign(
-		snprintf(__get_str(name), DWC3_MSG_MAX, "%s", dep->name);
+		__assign_str(name, dep->name);
 		__entry->trb = trb;
 		__entry->allocated = dep->allocated_requests;
 		__entry->queued = dep->queued_requests;
@@ -291,7 +293,7 @@ DECLARE_EVENT_CLASS(dwc3_log_ep,
 	TP_PROTO(struct dwc3_ep *dep),
 	TP_ARGS(dep),
 	TP_STRUCT__entry(
-		__dynamic_array(char, name, DWC3_MSG_MAX)
+		__string(name, dep->name)
 		__field(unsigned, maxpacket)
 		__field(unsigned, maxpacket_limit)
 		__field(unsigned, max_streams)
@@ -302,7 +304,7 @@ DECLARE_EVENT_CLASS(dwc3_log_ep,
 		__field(u8, trb_dequeue)
 	),
 	TP_fast_assign(
-		snprintf(__get_str(name), DWC3_MSG_MAX, "%s", dep->name);
+		__assign_str(name, dep->name);
 		__entry->maxpacket = dep->endpoint.maxpacket;
 		__entry->maxpacket_limit = dep->endpoint.maxpacket_limit;
 		__entry->max_streams = dep->endpoint.max_streams;
diff --git a/drivers/usb/dwc3/ulpi.c b/drivers/usb/dwc3/ulpi.c
index bd86f84f3790..e87ce8e9edee 100644
--- a/drivers/usb/dwc3/ulpi.c
+++ b/drivers/usb/dwc3/ulpi.c
@@ -41,6 +41,12 @@ static int dwc3_ulpi_read(struct device *dev, u8 addr)
 	u32 reg;
 	int ret;
 
+	reg = dwc3_readl(dwc->regs, DWC3_GUSB2PHYCFG(0));
+	if (reg & DWC3_GUSB2PHYCFG_SUSPHY) {
+		reg &= ~DWC3_GUSB2PHYCFG_SUSPHY;
+		dwc3_writel(dwc->regs, DWC3_GUSB2PHYCFG(0), reg);
+	}
+
 	reg = DWC3_GUSB2PHYACC_NEWREGREQ | DWC3_ULPI_ADDR(addr);
 	dwc3_writel(dwc->regs, DWC3_GUSB2PHYACC(0), reg);
 
@@ -58,6 +64,12 @@ static int dwc3_ulpi_write(struct device *dev, u8 addr, u8 val)
 	struct dwc3 *dwc = dev_get_drvdata(dev);
 	u32 reg;
 
+	reg = dwc3_readl(dwc->regs, DWC3_GUSB2PHYCFG(0));
+	if (reg & DWC3_GUSB2PHYCFG_SUSPHY) {
+		reg &= ~DWC3_GUSB2PHYCFG_SUSPHY;
+		dwc3_writel(dwc->regs, DWC3_GUSB2PHYCFG(0), reg);
+	}
+
 	reg = DWC3_GUSB2PHYACC_NEWREGREQ | DWC3_ULPI_ADDR(addr);
 	reg |= DWC3_GUSB2PHYACC_WRITE | val;
 	dwc3_writel(dwc->regs, DWC3_GUSB2PHYACC(0), reg);
diff --git a/drivers/usb/early/xhci-dbc.c b/drivers/usb/early/xhci-dbc.c
index 1268818e2263..12fe70beae69 100644
--- a/drivers/usb/early/xhci-dbc.c
+++ b/drivers/usb/early/xhci-dbc.c
@@ -32,7 +32,6 @@
 static struct xdbc_state xdbc;
 static bool early_console_keep;
 
-#define XDBC_TRACE
 #ifdef XDBC_TRACE
 #define	xdbc_trace	trace_printk
 #else
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index c164d6b788c3..35cc641d9f31 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -41,7 +41,7 @@ menuconfig USB_GADGET
 	   don't have this kind of hardware (except maybe inside Linux PDAs).
 
 	   For more information, see <http://www.linux-usb.org/gadget> and
-	   the kernel DocBook documentation for this API.
+	   the kernel documentation for this API.
 
 if USB_GADGET
 
@@ -158,6 +158,9 @@ config USB_U_SERIAL
 config USB_U_ETHER
 	tristate
 
+config USB_U_AUDIO
+	tristate
+
 config USB_F_SERIAL
 	tristate
 
@@ -191,6 +194,9 @@ config USB_F_FS
 config USB_F_UAC1
 	tristate
 
+config USB_F_UAC1_LEGACY
+	tristate
+
 config USB_F_UAC2
 	tristate
 
@@ -368,12 +374,30 @@ config USB_CONFIGFS_F_UAC1
 	depends on SND
 	select USB_LIBCOMPOSITE
 	select SND_PCM
+	select USB_U_AUDIO
 	select USB_F_UAC1
 	help
 	  This Audio function implements 1 AudioControl interface,
 	  1 AudioStreaming Interface each for USB-OUT and USB-IN.
-	  This driver requires a real Audio codec to be present
-	  on the device.
+	  This driver doesn't expect any real Audio codec to be present
+	  on the device - the audio streams are simply sinked to and
+	  sourced from a virtual ALSA sound card created. The user-space
+	  application may choose to do whatever it wants with the data
+	  received from the USB Host and choose to provide whatever it
+	  wants as audio data to the USB Host.
+
+config USB_CONFIGFS_F_UAC1_LEGACY
+	bool "Audio Class 1.0 (legacy implementation)"
+	depends on USB_CONFIGFS
+	depends on SND
+	select USB_LIBCOMPOSITE
+	select SND_PCM
+	select USB_F_UAC1_LEGACY
+	help
+	  This Audio function implements 1 AudioControl interface,
+	  1 AudioStreaming Interface each for USB-OUT and USB-IN.
+	  This is a legacy driver and requires a real Audio codec
+	  to be present on the device.
 
 config USB_CONFIGFS_F_UAC2
 	bool "Audio Class 2.0"
@@ -381,6 +405,7 @@ config USB_CONFIGFS_F_UAC2
 	depends on SND
 	select USB_LIBCOMPOSITE
 	select SND_PCM
+	select USB_U_AUDIO
 	select USB_F_UAC2
 	help
 	  This Audio function is compatible with USB Audio Class
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index 49d685ad0da9..dd74c99d6ce1 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -315,6 +315,9 @@ void usb_remove_function(struct usb_configuration *c, struct usb_function *f)
 	list_del(&f->list);
 	if (f->unbind)
 		f->unbind(c, f);
+
+	if (f->bind_deactivated)
+		usb_function_activate(f);
 }
 EXPORT_SYMBOL_GPL(usb_remove_function);
 
@@ -607,7 +610,6 @@ static int count_configs(struct usb_composite_dev *cdev, unsigned type)
 static int bos_desc(struct usb_composite_dev *cdev)
 {
 	struct usb_ext_cap_descriptor	*usb_ext;
-	struct usb_ss_cap_descriptor	*ss_cap;
 	struct usb_dcd_config_params	dcd_config_params;
 	struct usb_bos_descriptor	*bos = cdev->req->buf;
 
@@ -633,29 +635,35 @@ static int bos_desc(struct usb_composite_dev *cdev)
 	 * The Superspeed USB Capability descriptor shall be implemented by all
 	 * SuperSpeed devices.
 	 */
-	ss_cap = cdev->req->buf + le16_to_cpu(bos->wTotalLength);
-	bos->bNumDeviceCaps++;
-	le16_add_cpu(&bos->wTotalLength, USB_DT_USB_SS_CAP_SIZE);
-	ss_cap->bLength = USB_DT_USB_SS_CAP_SIZE;
-	ss_cap->bDescriptorType = USB_DT_DEVICE_CAPABILITY;
-	ss_cap->bDevCapabilityType = USB_SS_CAP_TYPE;
-	ss_cap->bmAttributes = 0; /* LTM is not supported yet */
-	ss_cap->wSpeedSupported = cpu_to_le16(USB_LOW_SPEED_OPERATION |
-				USB_FULL_SPEED_OPERATION |
-				USB_HIGH_SPEED_OPERATION |
-				USB_5GBPS_OPERATION);
-	ss_cap->bFunctionalitySupport = USB_LOW_SPEED_OPERATION;
-
-	/* Get Controller configuration */
-	if (cdev->gadget->ops->get_config_params)
-		cdev->gadget->ops->get_config_params(&dcd_config_params);
-	else {
-		dcd_config_params.bU1devExitLat = USB_DEFAULT_U1_DEV_EXIT_LAT;
-		dcd_config_params.bU2DevExitLat =
-			cpu_to_le16(USB_DEFAULT_U2_DEV_EXIT_LAT);
+	if (gadget_is_superspeed(cdev->gadget)) {
+		struct usb_ss_cap_descriptor *ss_cap;
+
+		ss_cap = cdev->req->buf + le16_to_cpu(bos->wTotalLength);
+		bos->bNumDeviceCaps++;
+		le16_add_cpu(&bos->wTotalLength, USB_DT_USB_SS_CAP_SIZE);
+		ss_cap->bLength = USB_DT_USB_SS_CAP_SIZE;
+		ss_cap->bDescriptorType = USB_DT_DEVICE_CAPABILITY;
+		ss_cap->bDevCapabilityType = USB_SS_CAP_TYPE;
+		ss_cap->bmAttributes = 0; /* LTM is not supported yet */
+		ss_cap->wSpeedSupported = cpu_to_le16(USB_LOW_SPEED_OPERATION |
+						      USB_FULL_SPEED_OPERATION |
+						      USB_HIGH_SPEED_OPERATION |
+						      USB_5GBPS_OPERATION);
+		ss_cap->bFunctionalitySupport = USB_LOW_SPEED_OPERATION;
+
+		/* Get Controller configuration */
+		if (cdev->gadget->ops->get_config_params) {
+			cdev->gadget->ops->get_config_params(
+				&dcd_config_params);
+		} else {
+			dcd_config_params.bU1devExitLat =
+				USB_DEFAULT_U1_DEV_EXIT_LAT;
+			dcd_config_params.bU2DevExitLat =
+				cpu_to_le16(USB_DEFAULT_U2_DEV_EXIT_LAT);
+		}
+		ss_cap->bU1devExitLat = dcd_config_params.bU1devExitLat;
+		ss_cap->bU2DevExitLat = dcd_config_params.bU2DevExitLat;
 	}
-	ss_cap->bU1devExitLat = dcd_config_params.bU1devExitLat;
-	ss_cap->bU2DevExitLat = dcd_config_params.bU2DevExitLat;
 
 	/* The SuperSpeedPlus USB Device Capability descriptor */
 	if (gadget_is_superspeed_plus(cdev->gadget)) {
@@ -956,12 +964,8 @@ static void remove_config(struct usb_composite_dev *cdev,
 
 		f = list_first_entry(&config->functions,
 				struct usb_function, list);
-		list_del(&f->list);
-		if (f->unbind) {
-			DBG(cdev, "unbind function '%s'/%p\n", f->name, f);
-			f->unbind(config, f);
-			/* may free memory for "f" */
-		}
+
+		usb_remove_function(config, f);
 	}
 	list_del(&config->list);
 	if (config->unbind) {
@@ -1603,7 +1607,10 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 					cdev->desc.bcdUSB = cpu_to_le16(0x0210);
 				}
 			} else {
-				cdev->desc.bcdUSB = cpu_to_le16(0x0200);
+				if (gadget->lpm_capable)
+					cdev->desc.bcdUSB = cpu_to_le16(0x0201);
+				else
+					cdev->desc.bcdUSB = cpu_to_le16(0x0200);
 			}
 
 			value = min(w_length, (u16) sizeof cdev->desc);
@@ -1634,7 +1641,8 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 				value = min(w_length, (u16) value);
 			break;
 		case USB_DT_BOS:
-			if (gadget_is_superspeed(gadget)) {
+			if (gadget_is_superspeed(gadget) ||
+			    gadget->lpm_capable) {
 				value = bos_desc(cdev);
 				value = min(w_length, (u16) value);
 			}
diff --git a/drivers/usb/gadget/configfs.c b/drivers/usb/gadget/configfs.c
index cbff3b02840d..a22a892de7b7 100644
--- a/drivers/usb/gadget/configfs.c
+++ b/drivers/usb/gadget/configfs.c
@@ -738,7 +738,7 @@ static inline struct gadget_info *os_desc_item_to_gadget_info(
 
 static ssize_t os_desc_use_show(struct config_item *item, char *page)
 {
-	return sprintf(page, "%d",
+	return sprintf(page, "%d\n",
 			os_desc_item_to_gadget_info(item)->use_os_desc);
 }
 
@@ -762,7 +762,7 @@ static ssize_t os_desc_use_store(struct config_item *item, const char *page,
 
 static ssize_t os_desc_b_vendor_code_show(struct config_item *item, char *page)
 {
-	return sprintf(page, "%d",
+	return sprintf(page, "0x%02x\n",
 			os_desc_item_to_gadget_info(item)->b_vendor_code);
 }
 
@@ -787,9 +787,13 @@ static ssize_t os_desc_b_vendor_code_store(struct config_item *item,
 static ssize_t os_desc_qw_sign_show(struct config_item *item, char *page)
 {
 	struct gadget_info *gi = os_desc_item_to_gadget_info(item);
+	int res;
+
+	res = utf16s_to_utf8s((wchar_t *) gi->qw_sign, OS_STRING_QW_SIGN_LEN,
+			      UTF16_LITTLE_ENDIAN, page, PAGE_SIZE - 1);
+	page[res++] = '\n';
 
-	memcpy(page, gi->qw_sign, OS_STRING_QW_SIGN_LEN);
-	return OS_STRING_QW_SIGN_LEN;
+	return res;
 }
 
 static ssize_t os_desc_qw_sign_store(struct config_item *item, const char *page,
@@ -900,7 +904,7 @@ static inline struct usb_os_desc_ext_prop
 
 static ssize_t ext_prop_type_show(struct config_item *item, char *page)
 {
-	return sprintf(page, "%d", to_usb_os_desc_ext_prop(item)->type);
+	return sprintf(page, "%d\n", to_usb_os_desc_ext_prop(item)->type);
 }
 
 static ssize_t ext_prop_type_store(struct config_item *item,
diff --git a/drivers/usb/gadget/function/Makefile b/drivers/usb/gadget/function/Makefile
index cb8c225e8549..86e825269947 100644
--- a/drivers/usb/gadget/function/Makefile
+++ b/drivers/usb/gadget/function/Makefile
@@ -32,8 +32,11 @@ usb_f_mass_storage-y		:= f_mass_storage.o storage_common.o
 obj-$(CONFIG_USB_F_MASS_STORAGE)+= usb_f_mass_storage.o
 usb_f_fs-y			:= f_fs.o
 obj-$(CONFIG_USB_F_FS)		+= usb_f_fs.o
-usb_f_uac1-y			:= f_uac1.o u_uac1.o
+obj-$(CONFIG_USB_U_AUDIO)	+= u_audio.o
+usb_f_uac1-y			:= f_uac1.o
 obj-$(CONFIG_USB_F_UAC1)	+= usb_f_uac1.o
+usb_f_uac1_legacy-y		:= f_uac1_legacy.o u_uac1_legacy.o
+obj-$(CONFIG_USB_F_UAC1_LEGACY)	+= usb_f_uac1_legacy.o
 usb_f_uac2-y			:= f_uac2.o
 obj-$(CONFIG_USB_F_UAC2)	+= usb_f_uac2.o
 usb_f_uvc-y			:= f_uvc.o uvc_queue.o uvc_v4l2.o uvc_video.o uvc_configfs.o
diff --git a/drivers/usb/gadget/function/f_fs.c b/drivers/usb/gadget/function/f_fs.c
index 47dda3450abd..d21874b35cf6 100644
--- a/drivers/usb/gadget/function/f_fs.c
+++ b/drivers/usb/gadget/function/f_fs.c
@@ -127,7 +127,6 @@ struct ffs_ep {
 struct ffs_epfile {
 	/* Protects ep->ep and ep->req. */
 	struct mutex			mutex;
-	wait_queue_head_t		wait;
 
 	struct ffs_data			*ffs;
 	struct ffs_ep			*ep;	/* P: ffs->eps_lock */
@@ -889,7 +888,8 @@ static ssize_t ffs_epfile_io(struct file *file, struct ffs_io_data *io_data)
 		if (file->f_flags & O_NONBLOCK)
 			return -EAGAIN;
 
-		ret = wait_event_interruptible(epfile->wait, (ep = epfile->ep));
+		ret = wait_event_interruptible(
+				epfile->ffs->wait, (ep = epfile->ep));
 		if (ret)
 			return -EINTR;
 	}
@@ -1189,6 +1189,7 @@ static long ffs_epfile_ioctl(struct file *file, unsigned code,
 			     unsigned long value)
 {
 	struct ffs_epfile *epfile = file->private_data;
+	struct ffs_ep *ep;
 	int ret;
 
 	ENTER();
@@ -1196,50 +1197,65 @@ static long ffs_epfile_ioctl(struct file *file, unsigned code,
 	if (WARN_ON(epfile->ffs->state != FFS_ACTIVE))
 		return -ENODEV;
 
+	/* Wait for endpoint to be enabled */
+	ep = epfile->ep;
+	if (!ep) {
+		if (file->f_flags & O_NONBLOCK)
+			return -EAGAIN;
+
+		ret = wait_event_interruptible(
+				epfile->ffs->wait, (ep = epfile->ep));
+		if (ret)
+			return -EINTR;
+	}
+
 	spin_lock_irq(&epfile->ffs->eps_lock);
-	if (likely(epfile->ep)) {
-		switch (code) {
-		case FUNCTIONFS_FIFO_STATUS:
-			ret = usb_ep_fifo_status(epfile->ep->ep);
-			break;
-		case FUNCTIONFS_FIFO_FLUSH:
-			usb_ep_fifo_flush(epfile->ep->ep);
-			ret = 0;
-			break;
-		case FUNCTIONFS_CLEAR_HALT:
-			ret = usb_ep_clear_halt(epfile->ep->ep);
-			break;
-		case FUNCTIONFS_ENDPOINT_REVMAP:
-			ret = epfile->ep->num;
-			break;
-		case FUNCTIONFS_ENDPOINT_DESC:
-		{
-			int desc_idx;
-			struct usb_endpoint_descriptor *desc;
 
-			switch (epfile->ffs->gadget->speed) {
-			case USB_SPEED_SUPER:
-				desc_idx = 2;
-				break;
-			case USB_SPEED_HIGH:
-				desc_idx = 1;
-				break;
-			default:
-				desc_idx = 0;
-			}
-			desc = epfile->ep->descs[desc_idx];
+	/* In the meantime, endpoint got disabled or changed. */
+	if (epfile->ep != ep) {
+		spin_unlock_irq(&epfile->ffs->eps_lock);
+		return -ESHUTDOWN;
+	}
 
-			spin_unlock_irq(&epfile->ffs->eps_lock);
-			ret = copy_to_user((void *)value, desc, desc->bLength);
-			if (ret)
-				ret = -EFAULT;
-			return ret;
-		}
+	switch (code) {
+	case FUNCTIONFS_FIFO_STATUS:
+		ret = usb_ep_fifo_status(epfile->ep->ep);
+		break;
+	case FUNCTIONFS_FIFO_FLUSH:
+		usb_ep_fifo_flush(epfile->ep->ep);
+		ret = 0;
+		break;
+	case FUNCTIONFS_CLEAR_HALT:
+		ret = usb_ep_clear_halt(epfile->ep->ep);
+		break;
+	case FUNCTIONFS_ENDPOINT_REVMAP:
+		ret = epfile->ep->num;
+		break;
+	case FUNCTIONFS_ENDPOINT_DESC:
+	{
+		int desc_idx;
+		struct usb_endpoint_descriptor *desc;
+
+		switch (epfile->ffs->gadget->speed) {
+		case USB_SPEED_SUPER:
+			desc_idx = 2;
+			break;
+		case USB_SPEED_HIGH:
+			desc_idx = 1;
+			break;
 		default:
-			ret = -ENOTTY;
+			desc_idx = 0;
 		}
-	} else {
-		ret = -ENODEV;
+		desc = epfile->ep->descs[desc_idx];
+
+		spin_unlock_irq(&epfile->ffs->eps_lock);
+		ret = copy_to_user((void *)value, desc, desc->bLength);
+		if (ret)
+			ret = -EFAULT;
+		return ret;
+	}
+	default:
+		ret = -ENOTTY;
 	}
 	spin_unlock_irq(&epfile->ffs->eps_lock);
 
@@ -1593,7 +1609,8 @@ static void ffs_data_put(struct ffs_data *ffs)
 		pr_info("%s(): freeing\n", __func__);
 		ffs_data_clear(ffs);
 		BUG_ON(waitqueue_active(&ffs->ev.waitq) ||
-		       waitqueue_active(&ffs->ep0req_completion.wait));
+		       waitqueue_active(&ffs->ep0req_completion.wait) ||
+		       waitqueue_active(&ffs->wait));
 		kfree(ffs->dev_name);
 		kfree(ffs);
 	}
@@ -1640,6 +1657,7 @@ static struct ffs_data *ffs_data_new(void)
 	mutex_init(&ffs->mutex);
 	spin_lock_init(&ffs->eps_lock);
 	init_waitqueue_head(&ffs->ev.waitq);
+	init_waitqueue_head(&ffs->wait);
 	init_completion(&ffs->ep0req_completion);
 
 	/* XXX REVISIT need to update it in some places, or do we? */
@@ -1761,7 +1779,6 @@ static int ffs_epfiles_create(struct ffs_data *ffs)
 	for (i = 1; i <= count; ++i, ++epfile) {
 		epfile->ffs = ffs;
 		mutex_init(&epfile->mutex);
-		init_waitqueue_head(&epfile->wait);
 		if (ffs->user_flags & FUNCTIONFS_VIRTUAL_ADDR)
 			sprintf(epfile->name, "ep%02x", ffs->eps_addrmap[i]);
 		else
@@ -1786,8 +1803,7 @@ static void ffs_epfiles_destroy(struct ffs_epfile *epfiles, unsigned count)
 	ENTER();
 
 	for (; count; --count, ++epfile) {
-		BUG_ON(mutex_is_locked(&epfile->mutex) ||
-		       waitqueue_active(&epfile->wait));
+		BUG_ON(mutex_is_locked(&epfile->mutex));
 		if (epfile->dentry) {
 			d_delete(epfile->dentry);
 			dput(epfile->dentry);
@@ -1874,11 +1890,11 @@ static int ffs_func_eps_enable(struct ffs_function *func)
 			break;
 		}
 
-		wake_up(&epfile->wait);
-
 		++ep;
 		++epfile;
 	}
+
+	wake_up_interruptible(&ffs->wait);
 	spin_unlock_irqrestore(&func->ffs->eps_lock, flags);
 
 	return ret;
diff --git a/drivers/usb/gadget/function/f_mass_storage.c b/drivers/usb/gadget/function/f_mass_storage.c
index 74d57d6994da..e80b9c123a9d 100644
--- a/drivers/usb/gadget/function/f_mass_storage.c
+++ b/drivers/usb/gadget/function/f_mass_storage.c
@@ -260,12 +260,13 @@ struct fsg_common {
 	struct usb_gadget	*gadget;
 	struct usb_composite_dev *cdev;
 	struct fsg_dev		*fsg, *new_fsg;
+	wait_queue_head_t	io_wait;
 	wait_queue_head_t	fsg_wait;
 
 	/* filesem protects: backing files in use */
 	struct rw_semaphore	filesem;
 
-	/* lock protects: state, all the req_busy's */
+	/* lock protects: state and thread_task */
 	spinlock_t		lock;
 
 	struct usb_ep		*ep0;		/* Copy of gadget->ep0 */
@@ -303,7 +304,6 @@ struct fsg_common {
 	unsigned int		running:1;
 	unsigned int		sysfs:1;
 
-	int			thread_wakeup_needed;
 	struct completion	thread_notifier;
 	struct task_struct	*thread_task;
 
@@ -355,7 +355,7 @@ typedef void (*fsg_routine_t)(struct fsg_dev *);
 
 static int exception_in_progress(struct fsg_common *common)
 {
-	return common->state > FSG_STATE_IDLE;
+	return common->state > FSG_STATE_NORMAL;
 }
 
 /* Make bulk-out requests be divisible by the maxpacket size */
@@ -393,20 +393,6 @@ static int fsg_set_halt(struct fsg_dev *fsg, struct usb_ep *ep)
 
 /* These routines may be called in process context or in_irq */
 
-/* Caller must hold fsg->lock */
-static void wakeup_thread(struct fsg_common *common)
-{
-	/*
-	 * Ensure the reading of thread_wakeup_needed
-	 * and the writing of bh->state are completed
-	 */
-	smp_mb();
-	/* Tell the main thread that something has happened */
-	common->thread_wakeup_needed = 1;
-	if (common->thread_task)
-		wake_up_process(common->thread_task);
-}
-
 static void raise_exception(struct fsg_common *common, enum fsg_state new_state)
 {
 	unsigned long		flags;
@@ -460,13 +446,9 @@ static void bulk_in_complete(struct usb_ep *ep, struct usb_request *req)
 	if (req->status == -ECONNRESET)		/* Request was cancelled */
 		usb_ep_fifo_flush(ep);
 
-	/* Hold the lock while we update the request and buffer states */
-	smp_wmb();
-	spin_lock(&common->lock);
-	bh->inreq_busy = 0;
-	bh->state = BUF_STATE_EMPTY;
-	wakeup_thread(common);
-	spin_unlock(&common->lock);
+	/* Synchronize with the smp_load_acquire() in sleep_thread() */
+	smp_store_release(&bh->state, BUF_STATE_EMPTY);
+	wake_up(&common->io_wait);
 }
 
 static void bulk_out_complete(struct usb_ep *ep, struct usb_request *req)
@@ -481,13 +463,9 @@ static void bulk_out_complete(struct usb_ep *ep, struct usb_request *req)
 	if (req->status == -ECONNRESET)		/* Request was cancelled */
 		usb_ep_fifo_flush(ep);
 
-	/* Hold the lock while we update the request and buffer states */
-	smp_wmb();
-	spin_lock(&common->lock);
-	bh->outreq_busy = 0;
-	bh->state = BUF_STATE_FULL;
-	wakeup_thread(common);
-	spin_unlock(&common->lock);
+	/* Synchronize with the smp_load_acquire() in sleep_thread() */
+	smp_store_release(&bh->state, BUF_STATE_FULL);
+	wake_up(&common->io_wait);
 }
 
 static int _fsg_common_get_max_lun(struct fsg_common *common)
@@ -532,7 +510,7 @@ static int fsg_setup(struct usb_function *f,
 		 * and reinitialize our state.
 		 */
 		DBG(fsg, "bulk reset request\n");
-		raise_exception(fsg->common, FSG_STATE_RESET);
+		raise_exception(fsg->common, FSG_STATE_PROTOCOL_RESET);
 		return USB_GADGET_DELAYED_STATUS;
 
 	case US_BULK_GET_MAX_LUN:
@@ -563,43 +541,39 @@ static int fsg_setup(struct usb_function *f,
 /* All the following routines run in process context */
 
 /* Use this for bulk or interrupt transfers, not ep0 */
-static void start_transfer(struct fsg_dev *fsg, struct usb_ep *ep,
-			   struct usb_request *req, int *pbusy,
-			   enum fsg_buffer_state *state)
+static int start_transfer(struct fsg_dev *fsg, struct usb_ep *ep,
+			   struct usb_request *req)
 {
 	int	rc;
 
 	if (ep == fsg->bulk_in)
 		dump_msg(fsg, "bulk-in", req->buf, req->length);
 
-	spin_lock_irq(&fsg->common->lock);
-	*pbusy = 1;
-	*state = BUF_STATE_BUSY;
-	spin_unlock_irq(&fsg->common->lock);
-
 	rc = usb_ep_queue(ep, req, GFP_KERNEL);
-	if (rc == 0)
-		return;  /* All good, we're done */
-
-	*pbusy = 0;
-	*state = BUF_STATE_EMPTY;
+	if (rc) {
 
-	/* We can't do much more than wait for a reset */
+		/* We can't do much more than wait for a reset */
+		req->status = rc;
 
-	/*
-	 * Note: currently the net2280 driver fails zero-length
-	 * submissions if DMA is enabled.
-	 */
-	if (rc != -ESHUTDOWN && !(rc == -EOPNOTSUPP && req->length == 0))
-		WARNING(fsg, "error in submission: %s --> %d\n", ep->name, rc);
+		/*
+		 * Note: currently the net2280 driver fails zero-length
+		 * submissions if DMA is enabled.
+		 */
+		if (rc != -ESHUTDOWN &&
+				!(rc == -EOPNOTSUPP && req->length == 0))
+			WARNING(fsg, "error in submission: %s --> %d\n",
+					ep->name, rc);
+	}
+	return rc;
 }
 
 static bool start_in_transfer(struct fsg_common *common, struct fsg_buffhd *bh)
 {
 	if (!fsg_is_set(common))
 		return false;
-	start_transfer(common->fsg, common->fsg->bulk_in,
-		       bh->inreq, &bh->inreq_busy, &bh->state);
+	bh->state = BUF_STATE_SENDING;
+	if (start_transfer(common->fsg, common->fsg->bulk_in, bh->inreq))
+		bh->state = BUF_STATE_EMPTY;
 	return true;
 }
 
@@ -607,37 +581,31 @@ static bool start_out_transfer(struct fsg_common *common, struct fsg_buffhd *bh)
 {
 	if (!fsg_is_set(common))
 		return false;
-	start_transfer(common->fsg, common->fsg->bulk_out,
-		       bh->outreq, &bh->outreq_busy, &bh->state);
+	bh->state = BUF_STATE_RECEIVING;
+	if (start_transfer(common->fsg, common->fsg->bulk_out, bh->outreq))
+		bh->state = BUF_STATE_FULL;
 	return true;
 }
 
-static int sleep_thread(struct fsg_common *common, bool can_freeze)
+static int sleep_thread(struct fsg_common *common, bool can_freeze,
+		struct fsg_buffhd *bh)
 {
-	int	rc = 0;
-
-	/* Wait until a signal arrives or we are woken up */
-	for (;;) {
-		if (can_freeze)
-			try_to_freeze();
-		set_current_state(TASK_INTERRUPTIBLE);
-		if (signal_pending(current)) {
-			rc = -EINTR;
-			break;
-		}
-		if (common->thread_wakeup_needed)
-			break;
-		schedule();
-	}
-	__set_current_state(TASK_RUNNING);
-	common->thread_wakeup_needed = 0;
+	int	rc;
 
-	/*
-	 * Ensure the writing of thread_wakeup_needed
-	 * and the reading of bh->state are completed
-	 */
-	smp_mb();
-	return rc;
+	/* Wait until a signal arrives or bh is no longer busy */
+	if (can_freeze)
+		/*
+		 * synchronize with the smp_store_release(&bh->state) in
+		 * bulk_in_complete() or bulk_out_complete()
+		 */
+		rc = wait_event_freezable(common->io_wait,
+				bh && smp_load_acquire(&bh->state) >=
+					BUF_STATE_EMPTY);
+	else
+		rc = wait_event_interruptible(common->io_wait,
+				bh && smp_load_acquire(&bh->state) >=
+					BUF_STATE_EMPTY);
+	return rc ? -EINTR : 0;
 }
 
 
@@ -697,11 +665,9 @@ static int do_read(struct fsg_common *common)
 
 		/* Wait for the next buffer to become available */
 		bh = common->next_buffhd_to_fill;
-		while (bh->state != BUF_STATE_EMPTY) {
-			rc = sleep_thread(common, false);
-			if (rc)
-				return rc;
-		}
+		rc = sleep_thread(common, false, bh);
+		if (rc)
+			return rc;
 
 		/*
 		 * If we were asked to read past the end of file,
@@ -878,84 +844,80 @@ static int do_write(struct fsg_common *common)
 		bh = common->next_buffhd_to_drain;
 		if (bh->state == BUF_STATE_EMPTY && !get_some_more)
 			break;			/* We stopped early */
-		if (bh->state == BUF_STATE_FULL) {
-			smp_rmb();
-			common->next_buffhd_to_drain = bh->next;
-			bh->state = BUF_STATE_EMPTY;
-
-			/* Did something go wrong with the transfer? */
-			if (bh->outreq->status != 0) {
-				curlun->sense_data = SS_COMMUNICATION_FAILURE;
-				curlun->sense_data_info =
+
+		/* Wait for the data to be received */
+		rc = sleep_thread(common, false, bh);
+		if (rc)
+			return rc;
+
+		common->next_buffhd_to_drain = bh->next;
+		bh->state = BUF_STATE_EMPTY;
+
+		/* Did something go wrong with the transfer? */
+		if (bh->outreq->status != 0) {
+			curlun->sense_data = SS_COMMUNICATION_FAILURE;
+			curlun->sense_data_info =
 					file_offset >> curlun->blkbits;
-				curlun->info_valid = 1;
-				break;
-			}
+			curlun->info_valid = 1;
+			break;
+		}
 
-			amount = bh->outreq->actual;
-			if (curlun->file_length - file_offset < amount) {
-				LERROR(curlun,
-				       "write %u @ %llu beyond end %llu\n",
+		amount = bh->outreq->actual;
+		if (curlun->file_length - file_offset < amount) {
+			LERROR(curlun, "write %u @ %llu beyond end %llu\n",
 				       amount, (unsigned long long)file_offset,
 				       (unsigned long long)curlun->file_length);
-				amount = curlun->file_length - file_offset;
-			}
+			amount = curlun->file_length - file_offset;
+		}
 
-			/* Don't accept excess data.  The spec doesn't say
-			 * what to do in this case.  We'll ignore the error.
-			 */
-			amount = min(amount, bh->bulk_out_intended_length);
-
-			/* Don't write a partial block */
-			amount = round_down(amount, curlun->blksize);
-			if (amount == 0)
-				goto empty_write;
-
-			/* Perform the write */
-			file_offset_tmp = file_offset;
-			nwritten = vfs_write(curlun->filp,
-					     (char __user *)bh->buf,
-					     amount, &file_offset_tmp);
-			VLDBG(curlun, "file write %u @ %llu -> %d\n", amount,
-			      (unsigned long long)file_offset, (int)nwritten);
-			if (signal_pending(current))
-				return -EINTR;		/* Interrupted! */
-
-			if (nwritten < 0) {
-				LDBG(curlun, "error in file write: %d\n",
-				     (int)nwritten);
-				nwritten = 0;
-			} else if (nwritten < amount) {
-				LDBG(curlun, "partial file write: %d/%u\n",
-				     (int)nwritten, amount);
-				nwritten = round_down(nwritten, curlun->blksize);
-			}
-			file_offset += nwritten;
-			amount_left_to_write -= nwritten;
-			common->residue -= nwritten;
+		/*
+		 * Don't accept excess data.  The spec doesn't say
+		 * what to do in this case.  We'll ignore the error.
+		 */
+		amount = min(amount, bh->bulk_out_intended_length);
 
-			/* If an error occurred, report it and its position */
-			if (nwritten < amount) {
-				curlun->sense_data = SS_WRITE_ERROR;
-				curlun->sense_data_info =
+		/* Don't write a partial block */
+		amount = round_down(amount, curlun->blksize);
+		if (amount == 0)
+			goto empty_write;
+
+		/* Perform the write */
+		file_offset_tmp = file_offset;
+		nwritten = vfs_write(curlun->filp, (char __user *)bh->buf,
+				amount, &file_offset_tmp);
+		VLDBG(curlun, "file write %u @ %llu -> %d\n", amount,
+				(unsigned long long)file_offset, (int)nwritten);
+		if (signal_pending(current))
+			return -EINTR;		/* Interrupted! */
+
+		if (nwritten < 0) {
+			LDBG(curlun, "error in file write: %d\n",
+					(int) nwritten);
+			nwritten = 0;
+		} else if (nwritten < amount) {
+			LDBG(curlun, "partial file write: %d/%u\n",
+					(int) nwritten, amount);
+			nwritten = round_down(nwritten, curlun->blksize);
+		}
+		file_offset += nwritten;
+		amount_left_to_write -= nwritten;
+		common->residue -= nwritten;
+
+		/* If an error occurred, report it and its position */
+		if (nwritten < amount) {
+			curlun->sense_data = SS_WRITE_ERROR;
+			curlun->sense_data_info =
 					file_offset >> curlun->blkbits;
-				curlun->info_valid = 1;
-				break;
-			}
+			curlun->info_valid = 1;
+			break;
+		}
 
  empty_write:
-			/* Did the host decide to stop early? */
-			if (bh->outreq->actual < bh->bulk_out_intended_length) {
-				common->short_packet_received = 1;
-				break;
-			}
-			continue;
+		/* Did the host decide to stop early? */
+		if (bh->outreq->actual < bh->bulk_out_intended_length) {
+			common->short_packet_received = 1;
+			break;
 		}
-
-		/* Wait for something to happen */
-		rc = sleep_thread(common, false);
-		if (rc)
-			return rc;
 	}
 
 	return -EIO;		/* No default reply */
@@ -1480,7 +1442,7 @@ static int wedge_bulk_in_endpoint(struct fsg_dev *fsg)
 
 static int throw_away_data(struct fsg_common *common)
 {
-	struct fsg_buffhd	*bh;
+	struct fsg_buffhd	*bh, *bh2;
 	u32			amount;
 	int			rc;
 
@@ -1488,26 +1450,10 @@ static int throw_away_data(struct fsg_common *common)
 	     bh->state != BUF_STATE_EMPTY || common->usb_amount_left > 0;
 	     bh = common->next_buffhd_to_drain) {
 
-		/* Throw away the data in a filled buffer */
-		if (bh->state == BUF_STATE_FULL) {
-			smp_rmb();
-			bh->state = BUF_STATE_EMPTY;
-			common->next_buffhd_to_drain = bh->next;
-
-			/* A short packet or an error ends everything */
-			if (bh->outreq->actual < bh->bulk_out_intended_length ||
-			    bh->outreq->status != 0) {
-				raise_exception(common,
-						FSG_STATE_ABORT_BULK_OUT);
-				return -EINTR;
-			}
-			continue;
-		}
-
 		/* Try to submit another request if we need one */
-		bh = common->next_buffhd_to_fill;
-		if (bh->state == BUF_STATE_EMPTY
-		 && common->usb_amount_left > 0) {
+		bh2 = common->next_buffhd_to_fill;
+		if (bh2->state == BUF_STATE_EMPTY &&
+				common->usb_amount_left > 0) {
 			amount = min(common->usb_amount_left, FSG_BUFLEN);
 
 			/*
@@ -1515,19 +1461,30 @@ static int throw_away_data(struct fsg_common *common)
 			 * equal to the buffer size, which is divisible by
 			 * the bulk-out maxpacket size.
 			 */
-			set_bulk_out_req_length(common, bh, amount);
-			if (!start_out_transfer(common, bh))
+			set_bulk_out_req_length(common, bh2, amount);
+			if (!start_out_transfer(common, bh2))
 				/* Dunno what to do if common->fsg is NULL */
 				return -EIO;
-			common->next_buffhd_to_fill = bh->next;
+			common->next_buffhd_to_fill = bh2->next;
 			common->usb_amount_left -= amount;
 			continue;
 		}
 
-		/* Otherwise wait for something to happen */
-		rc = sleep_thread(common, true);
+		/* Wait for the data to be received */
+		rc = sleep_thread(common, false, bh);
 		if (rc)
 			return rc;
+
+		/* Throw away the data in a filled buffer */
+		bh->state = BUF_STATE_EMPTY;
+		common->next_buffhd_to_drain = bh->next;
+
+		/* A short packet or an error ends everything */
+		if (bh->outreq->actual < bh->bulk_out_intended_length ||
+				bh->outreq->status != 0) {
+			raise_exception(common, FSG_STATE_ABORT_BULK_OUT);
+			return -EINTR;
+		}
 	}
 	return 0;
 }
@@ -1634,7 +1591,7 @@ static int finish_reply(struct fsg_common *common)
 	return rc;
 }
 
-static int send_status(struct fsg_common *common)
+static void send_status(struct fsg_common *common)
 {
 	struct fsg_lun		*curlun = common->curlun;
 	struct fsg_buffhd	*bh;
@@ -1645,11 +1602,9 @@ static int send_status(struct fsg_common *common)
 
 	/* Wait for the next buffer to become available */
 	bh = common->next_buffhd_to_fill;
-	while (bh->state != BUF_STATE_EMPTY) {
-		rc = sleep_thread(common, true);
-		if (rc)
-			return rc;
-	}
+	rc = sleep_thread(common, false, bh);
+	if (rc)
+		return;
 
 	if (curlun) {
 		sd = curlun->sense_data;
@@ -1683,10 +1638,10 @@ static int send_status(struct fsg_common *common)
 	bh->inreq->zero = 0;
 	if (!start_in_transfer(common, bh))
 		/* Don't know what to do if common->fsg is NULL */
-		return -EIO;
+		return;
 
 	common->next_buffhd_to_fill = bh->next;
-	return 0;
+	return;
 }
 
 
@@ -1848,11 +1803,10 @@ static int do_scsi_command(struct fsg_common *common)
 	/* Wait for the next buffer to become available for data or status */
 	bh = common->next_buffhd_to_fill;
 	common->next_buffhd_to_drain = bh;
-	while (bh->state != BUF_STATE_EMPTY) {
-		rc = sleep_thread(common, true);
-		if (rc)
-			return rc;
-	}
+	rc = sleep_thread(common, false, bh);
+	if (rc)
+		return rc;
+
 	common->phase_error = 0;
 	common->short_packet_received = 0;
 
@@ -2195,11 +2149,9 @@ static int get_next_command(struct fsg_common *common)
 
 	/* Wait for the next buffer to become available */
 	bh = common->next_buffhd_to_fill;
-	while (bh->state != BUF_STATE_EMPTY) {
-		rc = sleep_thread(common, true);
-		if (rc)
-			return rc;
-	}
+	rc = sleep_thread(common, true, bh);
+	if (rc)
+		return rc;
 
 	/* Queue a request to read a Bulk-only CBW */
 	set_bulk_out_req_length(common, bh, US_BULK_CB_WRAP_LEN);
@@ -2214,12 +2166,10 @@ static int get_next_command(struct fsg_common *common)
 	 */
 
 	/* Wait for the CBW to arrive */
-	while (bh->state != BUF_STATE_FULL) {
-		rc = sleep_thread(common, true);
-		if (rc)
-			return rc;
-	}
-	smp_rmb();
+	rc = sleep_thread(common, true, bh);
+	if (rc)
+		return rc;
+
 	rc = fsg_is_set(common) ? received_cbw(common->fsg, bh) : -EIO;
 	bh->state = BUF_STATE_EMPTY;
 
@@ -2371,9 +2321,11 @@ static void handle_exception(struct fsg_common *common)
 		if (!sig)
 			break;
 		if (sig != SIGUSR1) {
+			spin_lock_irq(&common->lock);
 			if (common->state < FSG_STATE_EXIT)
 				DBG(common, "Main thread exiting on signal\n");
-			raise_exception(common, FSG_STATE_EXIT);
+			common->state = FSG_STATE_EXIT;
+			spin_unlock_irq(&common->lock);
 		}
 	}
 
@@ -2381,23 +2333,14 @@ static void handle_exception(struct fsg_common *common)
 	if (likely(common->fsg)) {
 		for (i = 0; i < common->fsg_num_buffers; ++i) {
 			bh = &common->buffhds[i];
-			if (bh->inreq_busy)
+			if (bh->state == BUF_STATE_SENDING)
 				usb_ep_dequeue(common->fsg->bulk_in, bh->inreq);
-			if (bh->outreq_busy)
+			if (bh->state == BUF_STATE_RECEIVING)
 				usb_ep_dequeue(common->fsg->bulk_out,
 					       bh->outreq);
-		}
 
-		/* Wait until everything is idle */
-		for (;;) {
-			int num_active = 0;
-			for (i = 0; i < common->fsg_num_buffers; ++i) {
-				bh = &common->buffhds[i];
-				num_active += bh->inreq_busy + bh->outreq_busy;
-			}
-			if (num_active == 0)
-				break;
-			if (sleep_thread(common, true))
+			/* Wait for a transfer to become idle */
+			if (sleep_thread(common, false, bh))
 				return;
 		}
 
@@ -2422,10 +2365,9 @@ static void handle_exception(struct fsg_common *common)
 	common->next_buffhd_to_drain = &common->buffhds[0];
 	exception_req_tag = common->exception_req_tag;
 	old_state = common->state;
+	common->state = FSG_STATE_NORMAL;
 
-	if (old_state == FSG_STATE_ABORT_BULK_OUT)
-		common->state = FSG_STATE_STATUS_PHASE;
-	else {
+	if (old_state != FSG_STATE_ABORT_BULK_OUT) {
 		for (i = 0; i < ARRAY_SIZE(common->luns); ++i) {
 			curlun = common->luns[i];
 			if (!curlun)
@@ -2436,21 +2378,19 @@ static void handle_exception(struct fsg_common *common)
 			curlun->sense_data_info = 0;
 			curlun->info_valid = 0;
 		}
-		common->state = FSG_STATE_IDLE;
 	}
 	spin_unlock_irq(&common->lock);
 
 	/* Carry out any extra actions required for the exception */
 	switch (old_state) {
+	case FSG_STATE_NORMAL:
+		break;
+
 	case FSG_STATE_ABORT_BULK_OUT:
 		send_status(common);
-		spin_lock_irq(&common->lock);
-		if (common->state == FSG_STATE_STATUS_PHASE)
-			common->state = FSG_STATE_IDLE;
-		spin_unlock_irq(&common->lock);
 		break;
 
-	case FSG_STATE_RESET:
+	case FSG_STATE_PROTOCOL_RESET:
 		/*
 		 * In case we were forced against our will to halt a
 		 * bulk endpoint, clear the halt now.  (The SuperH UDC
@@ -2483,19 +2423,13 @@ static void handle_exception(struct fsg_common *common)
 		break;
 
 	case FSG_STATE_EXIT:
-	case FSG_STATE_TERMINATED:
 		do_set_interface(common, NULL);		/* Free resources */
 		spin_lock_irq(&common->lock);
 		common->state = FSG_STATE_TERMINATED;	/* Stop the thread */
 		spin_unlock_irq(&common->lock);
 		break;
 
-	case FSG_STATE_INTERFACE_CHANGE:
-	case FSG_STATE_DISCONNECT:
-	case FSG_STATE_COMMAND_PHASE:
-	case FSG_STATE_DATA_PHASE:
-	case FSG_STATE_STATUS_PHASE:
-	case FSG_STATE_IDLE:
+	case FSG_STATE_TERMINATED:
 		break;
 	}
 }
@@ -2534,33 +2468,17 @@ static int fsg_main_thread(void *common_)
 		}
 
 		if (!common->running) {
-			sleep_thread(common, true);
+			sleep_thread(common, true, NULL);
 			continue;
 		}
 
-		if (get_next_command(common))
+		if (get_next_command(common) || exception_in_progress(common))
 			continue;
-
-		spin_lock_irq(&common->lock);
-		if (!exception_in_progress(common))
-			common->state = FSG_STATE_DATA_PHASE;
-		spin_unlock_irq(&common->lock);
-
-		if (do_scsi_command(common) || finish_reply(common))
+		if (do_scsi_command(common) || exception_in_progress(common))
 			continue;
-
-		spin_lock_irq(&common->lock);
-		if (!exception_in_progress(common))
-			common->state = FSG_STATE_STATUS_PHASE;
-		spin_unlock_irq(&common->lock);
-
-		if (send_status(common))
+		if (finish_reply(common) || exception_in_progress(common))
 			continue;
-
-		spin_lock_irq(&common->lock);
-		if (!exception_in_progress(common))
-			common->state = FSG_STATE_IDLE;
-		spin_unlock_irq(&common->lock);
+		send_status(common);
 	}
 
 	spin_lock_irq(&common->lock);
@@ -2680,6 +2598,7 @@ static struct fsg_common *fsg_common_setup(struct fsg_common *common)
 	spin_lock_init(&common->lock);
 	kref_init(&common->ref);
 	init_completion(&common->thread_notifier);
+	init_waitqueue_head(&common->io_wait);
 	init_waitqueue_head(&common->fsg_wait);
 	common->state = FSG_STATE_TERMINATED;
 	memset(common->luns, 0, sizeof(common->luns));
@@ -2981,7 +2900,6 @@ static void fsg_common_release(struct kref *ref)
 	if (common->state != FSG_STATE_TERMINATED) {
 		raise_exception(common, FSG_STATE_EXIT);
 		wait_for_completion(&common->thread_notifier);
-		common->thread_task = NULL;
 	}
 
 	for (i = 0; i < ARRAY_SIZE(common->luns); ++i) {
@@ -3030,11 +2948,11 @@ static int fsg_bind(struct usb_configuration *c, struct usb_function *f)
 	}
 
 	if (!common->thread_task) {
-		common->state = FSG_STATE_IDLE;
+		common->state = FSG_STATE_NORMAL;
 		common->thread_task =
 			kthread_create(fsg_main_thread, common, "file-storage");
 		if (IS_ERR(common->thread_task)) {
-			int ret = PTR_ERR(common->thread_task);
+			ret = PTR_ERR(common->thread_task);
 			common->thread_task = NULL;
 			common->state = FSG_STATE_TERMINATED;
 			return ret;
diff --git a/drivers/usb/gadget/function/f_ncm.c b/drivers/usb/gadget/function/f_ncm.c
index 864819ff9a7d..24e34cfcb4bd 100644
--- a/drivers/usb/gadget/function/f_ncm.c
+++ b/drivers/usb/gadget/function/f_ncm.c
@@ -1004,18 +1004,15 @@ static struct sk_buff *package_for_tx(struct f_ncm *ncm)
 	}
 
 	/* Insert NDP alignment. */
-	ntb_iter = (void *) skb_put(skb2, ndp_pad);
-	memset(ntb_iter, 0, ndp_pad);
+	skb_put_zero(skb2, ndp_pad);
 
 	/* Copy NTB across. */
-	ntb_iter = (void *) skb_put(skb2, ncm->skb_tx_ndp->len);
-	memcpy(ntb_iter, ncm->skb_tx_ndp->data, ncm->skb_tx_ndp->len);
+	skb_put_data(skb2, ncm->skb_tx_ndp->data, ncm->skb_tx_ndp->len);
 	dev_consume_skb_any(ncm->skb_tx_ndp);
 	ncm->skb_tx_ndp = NULL;
 
 	/* Insert zero'd datagram. */
-	ntb_iter = (void *) skb_put(skb2, dgram_idx_len);
-	memset(ntb_iter, 0, dgram_idx_len);
+	skb_put_zero(skb2, dgram_idx_len);
 
 	return skb2;
 }
@@ -1049,7 +1046,7 @@ static struct sk_buff *ncm_wrap_ntb(struct gether *port,
 			crc = ~crc32_le(~0,
 					skb->data,
 					skb->len);
-			crc_pos = (void *) skb_put(skb, sizeof(uint32_t));
+			crc_pos = skb_put(skb, sizeof(uint32_t));
 			put_unaligned_le32(crc, crc_pos);
 		}
 
@@ -1080,8 +1077,7 @@ static struct sk_buff *ncm_wrap_ntb(struct gether *port,
 				goto err;
 
 			ncm->skb_tx_data->dev = ncm->netdev;
-			ntb_data = (void *) skb_put(ncm->skb_tx_data, ncb_len);
-			memset(ntb_data, 0, ncb_len);
+			ntb_data = skb_put_zero(ncm->skb_tx_data, ncb_len);
 			/* dwSignature */
 			put_unaligned_le32(opts->nth_sign, ntb_data);
 			ntb_data += 2;
@@ -1100,8 +1096,7 @@ static struct sk_buff *ncm_wrap_ntb(struct gether *port,
 				goto err;
 
 			ncm->skb_tx_ndp->dev = ncm->netdev;
-			ntb_ndp = (void *) skb_put(ncm->skb_tx_ndp,
-						    opts->ndp_size);
+			ntb_ndp = skb_put(ncm->skb_tx_ndp, opts->ndp_size);
 			memset(ntb_ndp, 0, ncb_len);
 			/* dwSignature */
 			put_unaligned_le32(ncm->ndp_sign, ntb_ndp);
@@ -1118,8 +1113,7 @@ static struct sk_buff *ncm_wrap_ntb(struct gether *port,
 			      HRTIMER_MODE_REL);
 
 		/* Add the datagram position entries */
-		ntb_ndp = (void *) skb_put(ncm->skb_tx_ndp, dgram_idx_len);
-		memset(ntb_ndp, 0, dgram_idx_len);
+		ntb_ndp = skb_put_zero(ncm->skb_tx_ndp, dgram_idx_len);
 
 		ncb_len = ncm->skb_tx_data->len;
 		dgram_pad = ALIGN(ncb_len, div) + rem - ncb_len;
@@ -1132,10 +1126,8 @@ static struct sk_buff *ncm_wrap_ntb(struct gether *port,
 		ncm->ndp_dgram_count++;
 
 		/* Add the new data to the skb */
-		ntb_data = (void *) skb_put(ncm->skb_tx_data, dgram_pad);
-		memset(ntb_data, 0, dgram_pad);
-		ntb_data = (void *) skb_put(ncm->skb_tx_data, skb->len);
-		memcpy(ntb_data, skb->data, skb->len);
+		skb_put_zero(ncm->skb_tx_data, dgram_pad);
+		skb_put_data(ncm->skb_tx_data, skb->data, skb->len);
 		dev_consume_skb_any(skb);
 		skb = NULL;
 
@@ -1318,8 +1310,8 @@ static int ncm_unwrap_ntb(struct gether *port,
 							 dg_len - crc_len);
 			if (skb2 == NULL)
 				goto err;
-			memcpy(skb_put(skb2, dg_len - crc_len),
-			       skb->data + index, dg_len - crc_len);
+			skb_put_data(skb2, skb->data + index,
+				     dg_len - crc_len);
 
 			skb_queue_tail(list, skb2);
 
diff --git a/drivers/usb/gadget/function/f_phonet.c b/drivers/usb/gadget/function/f_phonet.c
index b4058f0000e4..9c4c58e4a1a2 100644
--- a/drivers/usb/gadget/function/f_phonet.c
+++ b/drivers/usb/gadget/function/f_phonet.c
@@ -281,7 +281,7 @@ static void pn_net_setup(struct net_device *dev)
 	dev->tx_queue_len	= 1;
 
 	dev->netdev_ops		= &pn_netdev_ops;
-	dev->destructor		= free_netdev;
+	dev->needs_free_netdev	= true;
 	dev->header_ops		= &phonet_header_ops;
 }
 
@@ -336,7 +336,7 @@ static void pn_rx_complete(struct usb_ep *ep, struct usb_request *req)
 			skb->protocol = htons(ETH_P_PHONET);
 			skb_reset_mac_header(skb);
 			/* Can't use pskb_pull() on page in IRQ */
-			memcpy(skb_put(skb, 1), page_address(page), 1);
+			skb_put_data(skb, page_address(page), 1);
 		}
 
 		skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page,
diff --git a/drivers/usb/gadget/function/f_uac1.c b/drivers/usb/gadget/function/f_uac1.c
index f2ac0cbc29a4..8656f84e17d9 100644
--- a/drivers/usb/gadget/function/f_uac1.c
+++ b/drivers/usb/gadget/function/f_uac1.c
@@ -1,24 +1,38 @@
 /*
- * f_audio.c -- USB Audio class function driver
-  *
- * Copyright (C) 2008 Bryan Wu <cooloney@kernel.org>
- * Copyright (C) 2008 Analog Devices, Inc
+ * f_uac1.c -- USB Audio Class 1.0 Function (using u_audio API)
  *
- * Enter bugs at http://blackfin.uclinux.org/
+ * Copyright (C) 2016 Ruslan Bilovol <ruslan.bilovol@gmail.com>
  *
- * Licensed under the GPL-2 or later.
+ * This driver doesn't expect any real Audio codec to be present
+ * on the device - the audio streams are simply sinked to and
+ * sourced from a virtual ALSA sound card created.
+ *
+ * This file is based on f_uac1.c which is
+ *   Copyright (C) 2008 Bryan Wu <cooloney@kernel.org>
+ *   Copyright (C) 2008 Analog Devices, Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  */
 
-#include <linux/slab.h>
-#include <linux/kernel.h>
+#include <linux/usb/audio.h>
 #include <linux/module.h>
-#include <linux/device.h>
-#include <linux/atomic.h>
 
+#include "u_audio.h"
 #include "u_uac1.h"
 
-static int generic_set_cmd(struct usb_audio_control *con, u8 cmd, int value);
-static int generic_get_cmd(struct usb_audio_control *con, u8 cmd);
+struct f_uac1 {
+	struct g_audio g_audio;
+	u8 ac_intf, as_in_intf, as_out_intf;
+	u8 ac_alt, as_in_alt, as_out_alt;	/* needed for get_alt() */
+};
+
+static inline struct f_uac1 *func_to_uac1(struct usb_function *f)
+{
+	return container_of(f, struct f_uac1, g_audio.func);
+}
 
 /*
  * DESCRIPTORS ... most are static, but strings and full
@@ -26,12 +40,17 @@ static int generic_get_cmd(struct usb_audio_control *con, u8 cmd);
  */
 
 /*
- * We have two interfaces- AudioControl and AudioStreaming
- * TODO: only supcard playback currently
+ * We have three interfaces - one AudioControl and two AudioStreaming
+ *
+ * The driver implements a simple UAC_1 topology.
+ * USB-OUT -> IT_1 -> OT_2 -> ALSA_Capture
+ * ALSA_Playback -> IT_3 -> OT_4 -> USB-IN
  */
-#define F_AUDIO_AC_INTERFACE	0
-#define F_AUDIO_AS_INTERFACE	1
-#define F_AUDIO_NUM_INTERFACES	1
+#define F_AUDIO_AC_INTERFACE		0
+#define F_AUDIO_AS_OUT_INTERFACE	1
+#define F_AUDIO_AS_IN_INTERFACE		2
+/* Number of streaming interfaces */
+#define F_AUDIO_NUM_INTERFACES		2
 
 /* B.3.1  Standard AC Interface Descriptor */
 static struct usb_interface_descriptor ac_interface_desc = {
@@ -46,89 +65,91 @@ static struct usb_interface_descriptor ac_interface_desc = {
  * The number of AudioStreaming and MIDIStreaming interfaces
  * in the Audio Interface Collection
  */
-DECLARE_UAC_AC_HEADER_DESCRIPTOR(1);
+DECLARE_UAC_AC_HEADER_DESCRIPTOR(2);
 
 #define UAC_DT_AC_HEADER_LENGTH	UAC_DT_AC_HEADER_SIZE(F_AUDIO_NUM_INTERFACES)
-/* 1 input terminal, 1 output terminal and 1 feature unit */
-#define UAC_DT_TOTAL_LENGTH (UAC_DT_AC_HEADER_LENGTH + UAC_DT_INPUT_TERMINAL_SIZE \
-	+ UAC_DT_OUTPUT_TERMINAL_SIZE + UAC_DT_FEATURE_UNIT_SIZE(0))
+/* 2 input terminals and 2 output terminals */
+#define UAC_DT_TOTAL_LENGTH (UAC_DT_AC_HEADER_LENGTH \
+	+ 2*UAC_DT_INPUT_TERMINAL_SIZE + 2*UAC_DT_OUTPUT_TERMINAL_SIZE)
 /* B.3.2  Class-Specific AC Interface Descriptor */
-static struct uac1_ac_header_descriptor_1 ac_header_desc = {
+static struct uac1_ac_header_descriptor_2 ac_header_desc = {
 	.bLength =		UAC_DT_AC_HEADER_LENGTH,
 	.bDescriptorType =	USB_DT_CS_INTERFACE,
 	.bDescriptorSubtype =	UAC_HEADER,
-	.bcdADC =		__constant_cpu_to_le16(0x0100),
-	.wTotalLength =		__constant_cpu_to_le16(UAC_DT_TOTAL_LENGTH),
+	.bcdADC =		cpu_to_le16(0x0100),
+	.wTotalLength =		cpu_to_le16(UAC_DT_TOTAL_LENGTH),
 	.bInCollection =	F_AUDIO_NUM_INTERFACES,
 	.baInterfaceNr = {
-	/* Interface number of the first AudioStream interface */
+	/* Interface number of the AudioStream interfaces */
 		[0] =		1,
+		[1] =		2,
 	}
 };
 
-#define INPUT_TERMINAL_ID	1
-static struct uac_input_terminal_descriptor input_terminal_desc = {
+#define USB_OUT_IT_ID	1
+static struct uac_input_terminal_descriptor usb_out_it_desc = {
 	.bLength =		UAC_DT_INPUT_TERMINAL_SIZE,
 	.bDescriptorType =	USB_DT_CS_INTERFACE,
 	.bDescriptorSubtype =	UAC_INPUT_TERMINAL,
-	.bTerminalID =		INPUT_TERMINAL_ID,
+	.bTerminalID =		USB_OUT_IT_ID,
 	.wTerminalType =	UAC_TERMINAL_STREAMING,
 	.bAssocTerminal =	0,
 	.wChannelConfig =	0x3,
 };
 
-DECLARE_UAC_FEATURE_UNIT_DESCRIPTOR(0);
-
-#define FEATURE_UNIT_ID		2
-static struct uac_feature_unit_descriptor_0 feature_unit_desc = {
-	.bLength		= UAC_DT_FEATURE_UNIT_SIZE(0),
+#define IO_OUT_OT_ID	2
+static struct uac1_output_terminal_descriptor io_out_ot_desc = {
+	.bLength		= UAC_DT_OUTPUT_TERMINAL_SIZE,
 	.bDescriptorType	= USB_DT_CS_INTERFACE,
-	.bDescriptorSubtype	= UAC_FEATURE_UNIT,
-	.bUnitID		= FEATURE_UNIT_ID,
-	.bSourceID		= INPUT_TERMINAL_ID,
-	.bControlSize		= 2,
-	.bmaControls[0]		= (UAC_FU_MUTE | UAC_FU_VOLUME),
+	.bDescriptorSubtype	= UAC_OUTPUT_TERMINAL,
+	.bTerminalID		= IO_OUT_OT_ID,
+	.wTerminalType		= UAC_OUTPUT_TERMINAL_SPEAKER,
+	.bAssocTerminal		= 0,
+	.bSourceID		= USB_OUT_IT_ID,
 };
 
-static struct usb_audio_control mute_control = {
-	.list = LIST_HEAD_INIT(mute_control.list),
-	.name = "Mute Control",
-	.type = UAC_FU_MUTE,
-	/* Todo: add real Mute control code */
-	.set = generic_set_cmd,
-	.get = generic_get_cmd,
+#define IO_IN_IT_ID	3
+static struct uac_input_terminal_descriptor io_in_it_desc = {
+	.bLength		= UAC_DT_INPUT_TERMINAL_SIZE,
+	.bDescriptorType	= USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype	= UAC_INPUT_TERMINAL,
+	.bTerminalID		= IO_IN_IT_ID,
+	.wTerminalType		= UAC_INPUT_TERMINAL_MICROPHONE,
+	.bAssocTerminal		= 0,
+	.wChannelConfig		= 0x3,
 };
 
-static struct usb_audio_control volume_control = {
-	.list = LIST_HEAD_INIT(volume_control.list),
-	.name = "Volume Control",
-	.type = UAC_FU_VOLUME,
-	/* Todo: add real Volume control code */
-	.set = generic_set_cmd,
-	.get = generic_get_cmd,
+#define USB_IN_OT_ID	4
+static struct uac1_output_terminal_descriptor usb_in_ot_desc = {
+	.bLength =		UAC_DT_OUTPUT_TERMINAL_SIZE,
+	.bDescriptorType =	USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype =	UAC_OUTPUT_TERMINAL,
+	.bTerminalID =		USB_IN_OT_ID,
+	.wTerminalType =	UAC_TERMINAL_STREAMING,
+	.bAssocTerminal =	0,
+	.bSourceID =		IO_IN_IT_ID,
 };
 
-static struct usb_audio_control_selector feature_unit = {
-	.list = LIST_HEAD_INIT(feature_unit.list),
-	.id = FEATURE_UNIT_ID,
-	.name = "Mute & Volume Control",
-	.type = UAC_FEATURE_UNIT,
-	.desc = (struct usb_descriptor_header *)&feature_unit_desc,
+/* B.4.1  Standard AS Interface Descriptor */
+static struct usb_interface_descriptor as_out_interface_alt_0_desc = {
+	.bLength =		USB_DT_INTERFACE_SIZE,
+	.bDescriptorType =	USB_DT_INTERFACE,
+	.bAlternateSetting =	0,
+	.bNumEndpoints =	0,
+	.bInterfaceClass =	USB_CLASS_AUDIO,
+	.bInterfaceSubClass =	USB_SUBCLASS_AUDIOSTREAMING,
 };
 
-#define OUTPUT_TERMINAL_ID	3
-static struct uac1_output_terminal_descriptor output_terminal_desc = {
-	.bLength		= UAC_DT_OUTPUT_TERMINAL_SIZE,
-	.bDescriptorType	= USB_DT_CS_INTERFACE,
-	.bDescriptorSubtype	= UAC_OUTPUT_TERMINAL,
-	.bTerminalID		= OUTPUT_TERMINAL_ID,
-	.wTerminalType		= UAC_OUTPUT_TERMINAL_SPEAKER,
-	.bAssocTerminal		= FEATURE_UNIT_ID,
-	.bSourceID		= FEATURE_UNIT_ID,
+static struct usb_interface_descriptor as_out_interface_alt_1_desc = {
+	.bLength =		USB_DT_INTERFACE_SIZE,
+	.bDescriptorType =	USB_DT_INTERFACE,
+	.bAlternateSetting =	1,
+	.bNumEndpoints =	1,
+	.bInterfaceClass =	USB_CLASS_AUDIO,
+	.bInterfaceSubClass =	USB_SUBCLASS_AUDIOSTREAMING,
 };
 
-/* B.4.1  Standard AS Interface Descriptor */
-static struct usb_interface_descriptor as_interface_alt_0_desc = {
+static struct usb_interface_descriptor as_in_interface_alt_0_desc = {
 	.bLength =		USB_DT_INTERFACE_SIZE,
 	.bDescriptorType =	USB_DT_INTERFACE,
 	.bAlternateSetting =	0,
@@ -137,7 +158,7 @@ static struct usb_interface_descriptor as_interface_alt_0_desc = {
 	.bInterfaceSubClass =	USB_SUBCLASS_AUDIOSTREAMING,
 };
 
-static struct usb_interface_descriptor as_interface_alt_1_desc = {
+static struct usb_interface_descriptor as_in_interface_alt_1_desc = {
 	.bLength =		USB_DT_INTERFACE_SIZE,
 	.bDescriptorType =	USB_DT_INTERFACE,
 	.bAlternateSetting =	1,
@@ -147,18 +168,27 @@ static struct usb_interface_descriptor as_interface_alt_1_desc = {
 };
 
 /* B.4.2  Class-Specific AS Interface Descriptor */
-static struct uac1_as_header_descriptor as_header_desc = {
+static struct uac1_as_header_descriptor as_out_header_desc = {
 	.bLength =		UAC_DT_AS_HEADER_SIZE,
 	.bDescriptorType =	USB_DT_CS_INTERFACE,
 	.bDescriptorSubtype =	UAC_AS_GENERAL,
-	.bTerminalLink =	INPUT_TERMINAL_ID,
+	.bTerminalLink =	USB_OUT_IT_ID,
+	.bDelay =		1,
+	.wFormatTag =		UAC_FORMAT_TYPE_I_PCM,
+};
+
+static struct uac1_as_header_descriptor as_in_header_desc = {
+	.bLength =		UAC_DT_AS_HEADER_SIZE,
+	.bDescriptorType =	USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype =	UAC_AS_GENERAL,
+	.bTerminalLink =	USB_IN_OT_ID,
 	.bDelay =		1,
 	.wFormatTag =		UAC_FORMAT_TYPE_I_PCM,
 };
 
 DECLARE_UAC_FORMAT_TYPE_I_DISCRETE_DESC(1);
 
-static struct uac_format_type_i_discrete_descriptor_1 as_type_i_desc = {
+static struct uac_format_type_i_discrete_descriptor_1 as_out_type_i_desc = {
 	.bLength =		UAC_FORMAT_TYPE_I_DISCRETE_DESC_SIZE(1),
 	.bDescriptorType =	USB_DT_CS_INTERFACE,
 	.bDescriptorSubtype =	UAC_FORMAT_TYPE,
@@ -184,48 +214,97 @@ static struct uac_iso_endpoint_descriptor as_iso_out_desc = {
 	.bLength =		UAC_ISO_ENDPOINT_DESC_SIZE,
 	.bDescriptorType =	USB_DT_CS_ENDPOINT,
 	.bDescriptorSubtype =	UAC_EP_GENERAL,
-	.bmAttributes = 	1,
+	.bmAttributes =		1,
 	.bLockDelayUnits =	1,
-	.wLockDelay =		__constant_cpu_to_le16(1),
+	.wLockDelay =		cpu_to_le16(1),
+};
+
+static struct uac_format_type_i_discrete_descriptor_1 as_in_type_i_desc = {
+	.bLength =		UAC_FORMAT_TYPE_I_DISCRETE_DESC_SIZE(1),
+	.bDescriptorType =	USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype =	UAC_FORMAT_TYPE,
+	.bFormatType =		UAC_FORMAT_TYPE_I,
+	.bSubframeSize =	2,
+	.bBitResolution =	16,
+	.bSamFreqType =		1,
+};
+
+/* Standard ISO OUT Endpoint Descriptor */
+static struct usb_endpoint_descriptor as_in_ep_desc  = {
+	.bLength =		USB_DT_ENDPOINT_AUDIO_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_SYNC_ASYNC
+				| USB_ENDPOINT_XFER_ISOC,
+	.wMaxPacketSize	=	cpu_to_le16(UAC1_OUT_EP_MAX_PACKET_SIZE),
+	.bInterval =		4,
+};
+
+/* Class-specific AS ISO OUT Endpoint Descriptor */
+static struct uac_iso_endpoint_descriptor as_iso_in_desc = {
+	.bLength =		UAC_ISO_ENDPOINT_DESC_SIZE,
+	.bDescriptorType =	USB_DT_CS_ENDPOINT,
+	.bDescriptorSubtype =	UAC_EP_GENERAL,
+	.bmAttributes =		1,
+	.bLockDelayUnits =	0,
+	.wLockDelay =		0,
 };
 
 static struct usb_descriptor_header *f_audio_desc[] = {
 	(struct usb_descriptor_header *)&ac_interface_desc,
 	(struct usb_descriptor_header *)&ac_header_desc,
 
-	(struct usb_descriptor_header *)&input_terminal_desc,
-	(struct usb_descriptor_header *)&output_terminal_desc,
-	(struct usb_descriptor_header *)&feature_unit_desc,
+	(struct usb_descriptor_header *)&usb_out_it_desc,
+	(struct usb_descriptor_header *)&io_out_ot_desc,
+	(struct usb_descriptor_header *)&io_in_it_desc,
+	(struct usb_descriptor_header *)&usb_in_ot_desc,
 
-	(struct usb_descriptor_header *)&as_interface_alt_0_desc,
-	(struct usb_descriptor_header *)&as_interface_alt_1_desc,
-	(struct usb_descriptor_header *)&as_header_desc,
+	(struct usb_descriptor_header *)&as_out_interface_alt_0_desc,
+	(struct usb_descriptor_header *)&as_out_interface_alt_1_desc,
+	(struct usb_descriptor_header *)&as_out_header_desc,
 
-	(struct usb_descriptor_header *)&as_type_i_desc,
+	(struct usb_descriptor_header *)&as_out_type_i_desc,
 
 	(struct usb_descriptor_header *)&as_out_ep_desc,
 	(struct usb_descriptor_header *)&as_iso_out_desc,
+
+	(struct usb_descriptor_header *)&as_in_interface_alt_0_desc,
+	(struct usb_descriptor_header *)&as_in_interface_alt_1_desc,
+	(struct usb_descriptor_header *)&as_in_header_desc,
+
+	(struct usb_descriptor_header *)&as_in_type_i_desc,
+
+	(struct usb_descriptor_header *)&as_in_ep_desc,
+	(struct usb_descriptor_header *)&as_iso_in_desc,
 	NULL,
 };
 
 enum {
 	STR_AC_IF,
-	STR_INPUT_TERMINAL,
-	STR_INPUT_TERMINAL_CH_NAMES,
-	STR_FEAT_DESC_0,
-	STR_OUTPUT_TERMINAL,
-	STR_AS_IF_ALT0,
-	STR_AS_IF_ALT1,
+	STR_USB_OUT_IT,
+	STR_USB_OUT_IT_CH_NAMES,
+	STR_IO_OUT_OT,
+	STR_IO_IN_IT,
+	STR_IO_IN_IT_CH_NAMES,
+	STR_USB_IN_OT,
+	STR_AS_OUT_IF_ALT0,
+	STR_AS_OUT_IF_ALT1,
+	STR_AS_IN_IF_ALT0,
+	STR_AS_IN_IF_ALT1,
 };
 
 static struct usb_string strings_uac1[] = {
 	[STR_AC_IF].s = "AC Interface",
-	[STR_INPUT_TERMINAL].s = "Input terminal",
-	[STR_INPUT_TERMINAL_CH_NAMES].s = "Channels",
-	[STR_FEAT_DESC_0].s = "Volume control & mute",
-	[STR_OUTPUT_TERMINAL].s = "Output terminal",
-	[STR_AS_IF_ALT0].s = "AS Interface",
-	[STR_AS_IF_ALT1].s = "AS Interface",
+	[STR_USB_OUT_IT].s = "Playback Input terminal",
+	[STR_USB_OUT_IT_CH_NAMES].s = "Playback Channels",
+	[STR_IO_OUT_OT].s = "Playback Output terminal",
+	[STR_IO_IN_IT].s = "Capture Input terminal",
+	[STR_IO_IN_IT_CH_NAMES].s = "Capture Channels",
+	[STR_USB_IN_OT].s = "Capture Output terminal",
+	[STR_AS_OUT_IF_ALT0].s = "Playback Inactive",
+	[STR_AS_OUT_IF_ALT1].s = "Playback Active",
+	[STR_AS_IN_IF_ALT0].s = "Capture Inactive",
+	[STR_AS_IN_IF_ALT1].s = "Capture Active",
 	{ },
 };
 
@@ -243,216 +322,6 @@ static struct usb_gadget_strings *uac1_strings[] = {
  * This function is an ALSA sound card following USB Audio Class Spec 1.0.
  */
 
-/*-------------------------------------------------------------------------*/
-struct f_audio_buf {
-	u8 *buf;
-	int actual;
-	struct list_head list;
-};
-
-static struct f_audio_buf *f_audio_buffer_alloc(int buf_size)
-{
-	struct f_audio_buf *copy_buf;
-
-	copy_buf = kzalloc(sizeof *copy_buf, GFP_ATOMIC);
-	if (!copy_buf)
-		return ERR_PTR(-ENOMEM);
-
-	copy_buf->buf = kzalloc(buf_size, GFP_ATOMIC);
-	if (!copy_buf->buf) {
-		kfree(copy_buf);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	return copy_buf;
-}
-
-static void f_audio_buffer_free(struct f_audio_buf *audio_buf)
-{
-	kfree(audio_buf->buf);
-	kfree(audio_buf);
-}
-/*-------------------------------------------------------------------------*/
-
-struct f_audio {
-	struct gaudio			card;
-
-	/* endpoints handle full and/or high speeds */
-	struct usb_ep			*out_ep;
-
-	spinlock_t			lock;
-	struct f_audio_buf *copy_buf;
-	struct work_struct playback_work;
-	struct list_head play_queue;
-
-	/* Control Set command */
-	struct list_head cs;
-	u8 set_cmd;
-	struct usb_audio_control *set_con;
-};
-
-static inline struct f_audio *func_to_audio(struct usb_function *f)
-{
-	return container_of(f, struct f_audio, card.func);
-}
-
-/*-------------------------------------------------------------------------*/
-
-static void f_audio_playback_work(struct work_struct *data)
-{
-	struct f_audio *audio = container_of(data, struct f_audio,
-					playback_work);
-	struct f_audio_buf *play_buf;
-
-	spin_lock_irq(&audio->lock);
-	if (list_empty(&audio->play_queue)) {
-		spin_unlock_irq(&audio->lock);
-		return;
-	}
-	play_buf = list_first_entry(&audio->play_queue,
-			struct f_audio_buf, list);
-	list_del(&play_buf->list);
-	spin_unlock_irq(&audio->lock);
-
-	u_audio_playback(&audio->card, play_buf->buf, play_buf->actual);
-	f_audio_buffer_free(play_buf);
-}
-
-static int f_audio_out_ep_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct f_audio *audio = req->context;
-	struct usb_composite_dev *cdev = audio->card.func.config->cdev;
-	struct f_audio_buf *copy_buf = audio->copy_buf;
-	struct f_uac1_opts *opts;
-	int audio_buf_size;
-	int err;
-
-	opts = container_of(audio->card.func.fi, struct f_uac1_opts,
-			    func_inst);
-	audio_buf_size = opts->audio_buf_size;
-
-	if (!copy_buf)
-		return -EINVAL;
-
-	/* Copy buffer is full, add it to the play_queue */
-	if (audio_buf_size - copy_buf->actual < req->actual) {
-		list_add_tail(&copy_buf->list, &audio->play_queue);
-		schedule_work(&audio->playback_work);
-		copy_buf = f_audio_buffer_alloc(audio_buf_size);
-		if (IS_ERR(copy_buf))
-			return -ENOMEM;
-	}
-
-	memcpy(copy_buf->buf + copy_buf->actual, req->buf, req->actual);
-	copy_buf->actual += req->actual;
-	audio->copy_buf = copy_buf;
-
-	err = usb_ep_queue(ep, req, GFP_ATOMIC);
-	if (err)
-		ERROR(cdev, "%s queue req: %d\n", ep->name, err);
-
-	return 0;
-
-}
-
-static void f_audio_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct f_audio *audio = req->context;
-	int status = req->status;
-	u32 data = 0;
-	struct usb_ep *out_ep = audio->out_ep;
-
-	switch (status) {
-
-	case 0:				/* normal completion? */
-		if (ep == out_ep)
-			f_audio_out_ep_complete(ep, req);
-		else if (audio->set_con) {
-			memcpy(&data, req->buf, req->length);
-			audio->set_con->set(audio->set_con, audio->set_cmd,
-					le16_to_cpu(data));
-			audio->set_con = NULL;
-		}
-		break;
-	default:
-		break;
-	}
-}
-
-static int audio_set_intf_req(struct usb_function *f,
-		const struct usb_ctrlrequest *ctrl)
-{
-	struct f_audio		*audio = func_to_audio(f);
-	struct usb_composite_dev *cdev = f->config->cdev;
-	struct usb_request	*req = cdev->req;
-	u8			id = ((le16_to_cpu(ctrl->wIndex) >> 8) & 0xFF);
-	u16			len = le16_to_cpu(ctrl->wLength);
-	u16			w_value = le16_to_cpu(ctrl->wValue);
-	u8			con_sel = (w_value >> 8) & 0xFF;
-	u8			cmd = (ctrl->bRequest & 0x0F);
-	struct usb_audio_control_selector *cs;
-	struct usb_audio_control *con;
-
-	DBG(cdev, "bRequest 0x%x, w_value 0x%04x, len %d, entity %d\n",
-			ctrl->bRequest, w_value, len, id);
-
-	list_for_each_entry(cs, &audio->cs, list) {
-		if (cs->id == id) {
-			list_for_each_entry(con, &cs->control, list) {
-				if (con->type == con_sel) {
-					audio->set_con = con;
-					break;
-				}
-			}
-			break;
-		}
-	}
-
-	audio->set_cmd = cmd;
-	req->context = audio;
-	req->complete = f_audio_complete;
-
-	return len;
-}
-
-static int audio_get_intf_req(struct usb_function *f,
-		const struct usb_ctrlrequest *ctrl)
-{
-	struct f_audio		*audio = func_to_audio(f);
-	struct usb_composite_dev *cdev = f->config->cdev;
-	struct usb_request	*req = cdev->req;
-	int			value = -EOPNOTSUPP;
-	u8			id = ((le16_to_cpu(ctrl->wIndex) >> 8) & 0xFF);
-	u16			len = le16_to_cpu(ctrl->wLength);
-	u16			w_value = le16_to_cpu(ctrl->wValue);
-	u8			con_sel = (w_value >> 8) & 0xFF;
-	u8			cmd = (ctrl->bRequest & 0x0F);
-	struct usb_audio_control_selector *cs;
-	struct usb_audio_control *con;
-
-	DBG(cdev, "bRequest 0x%x, w_value 0x%04x, len %d, entity %d\n",
-			ctrl->bRequest, w_value, len, id);
-
-	list_for_each_entry(cs, &audio->cs, list) {
-		if (cs->id == id) {
-			list_for_each_entry(con, &cs->control, list) {
-				if (con->type == con_sel && con->get) {
-					value = con->get(con, cmd);
-					break;
-				}
-			}
-			break;
-		}
-	}
-
-	req->context = audio;
-	req->complete = f_audio_complete;
-	len = min_t(size_t, sizeof(value), len);
-	memcpy(req->buf, &value, len);
-
-	return len;
-}
-
 static int audio_set_endpoint_req(struct usb_function *f,
 		const struct usb_ctrlrequest *ctrl)
 {
@@ -531,14 +400,6 @@ f_audio_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
 	 * activation uses set_alt().
 	 */
 	switch (ctrl->bRequestType) {
-	case USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE:
-		value = audio_set_intf_req(f, ctrl);
-		break;
-
-	case USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE:
-		value = audio_get_intf_req(f, ctrl);
-		break;
-
 	case USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_ENDPOINT:
 		value = audio_set_endpoint_req(f, ctrl);
 		break;
@@ -571,143 +432,158 @@ f_audio_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
 
 static int f_audio_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 {
-	struct f_audio		*audio = func_to_audio(f);
 	struct usb_composite_dev *cdev = f->config->cdev;
-	struct usb_ep *out_ep = audio->out_ep;
-	struct usb_request *req;
-	struct f_uac1_opts *opts;
-	int req_buf_size, req_count, audio_buf_size;
-	int i = 0, err = 0;
-
-	DBG(cdev, "intf %d, alt %d\n", intf, alt);
+	struct usb_gadget *gadget = cdev->gadget;
+	struct device *dev = &gadget->dev;
+	struct f_uac1 *uac1 = func_to_uac1(f);
+	int ret = 0;
+
+	/* No i/f has more than 2 alt settings */
+	if (alt > 1) {
+		dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
+		return -EINVAL;
+	}
 
-	opts = container_of(f->fi, struct f_uac1_opts, func_inst);
-	req_buf_size = opts->req_buf_size;
-	req_count = opts->req_count;
-	audio_buf_size = opts->audio_buf_size;
-
-	if (intf == 1) {
-		if (alt == 1) {
-			err = config_ep_by_speed(cdev->gadget, f, out_ep);
-			if (err)
-				return err;
-
-			usb_ep_enable(out_ep);
-			audio->copy_buf = f_audio_buffer_alloc(audio_buf_size);
-			if (IS_ERR(audio->copy_buf))
-				return -ENOMEM;
-
-			/*
-			 * allocate a bunch of read buffers
-			 * and queue them all at once.
-			 */
-			for (i = 0; i < req_count && err == 0; i++) {
-				req = usb_ep_alloc_request(out_ep, GFP_ATOMIC);
-				if (req) {
-					req->buf = kzalloc(req_buf_size,
-							GFP_ATOMIC);
-					if (req->buf) {
-						req->length = req_buf_size;
-						req->context = audio;
-						req->complete =
-							f_audio_complete;
-						err = usb_ep_queue(out_ep,
-							req, GFP_ATOMIC);
-						if (err)
-							ERROR(cdev,
-							"%s queue req: %d\n",
-							out_ep->name, err);
-					} else
-						err = -ENOMEM;
-				} else
-					err = -ENOMEM;
-			}
-
-		} else {
-			struct f_audio_buf *copy_buf = audio->copy_buf;
-			if (copy_buf) {
-				list_add_tail(&copy_buf->list,
-						&audio->play_queue);
-				schedule_work(&audio->playback_work);
-			}
+	if (intf == uac1->ac_intf) {
+		/* Control I/f has only 1 AltSetting - 0 */
+		if (alt) {
+			dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
+			return -EINVAL;
 		}
+		return 0;
 	}
 
-	return err;
+	if (intf == uac1->as_out_intf) {
+		uac1->as_out_alt = alt;
+
+		if (alt)
+			ret = u_audio_start_capture(&uac1->g_audio);
+		else
+			u_audio_stop_capture(&uac1->g_audio);
+	} else if (intf == uac1->as_in_intf) {
+		uac1->as_in_alt = alt;
+
+			if (alt)
+				ret = u_audio_start_playback(&uac1->g_audio);
+			else
+				u_audio_stop_playback(&uac1->g_audio);
+	} else {
+		dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+
+	return ret;
 }
 
-static void f_audio_disable(struct usb_function *f)
+static int f_audio_get_alt(struct usb_function *f, unsigned intf)
 {
-	return;
+	struct usb_composite_dev *cdev = f->config->cdev;
+	struct usb_gadget *gadget = cdev->gadget;
+	struct device *dev = &gadget->dev;
+	struct f_uac1 *uac1 = func_to_uac1(f);
+
+	if (intf == uac1->ac_intf)
+		return uac1->ac_alt;
+	else if (intf == uac1->as_out_intf)
+		return uac1->as_out_alt;
+	else if (intf == uac1->as_in_intf)
+		return uac1->as_in_alt;
+	else
+		dev_err(dev, "%s:%d Invalid Interface %d!\n",
+			__func__, __LINE__, intf);
+
+	return -EINVAL;
 }
 
-/*-------------------------------------------------------------------------*/
 
-static void f_audio_build_desc(struct f_audio *audio)
+static void f_audio_disable(struct usb_function *f)
 {
-	struct gaudio *card = &audio->card;
-	u8 *sam_freq;
-	int rate;
+	struct f_uac1 *uac1 = func_to_uac1(f);
 
-	/* Set channel numbers */
-	input_terminal_desc.bNrChannels = u_audio_get_playback_channels(card);
-	as_type_i_desc.bNrChannels = u_audio_get_playback_channels(card);
+	uac1->as_out_alt = 0;
+	uac1->as_in_alt = 0;
 
-	/* Set sample rates */
-	rate = u_audio_get_playback_rate(card);
-	sam_freq = as_type_i_desc.tSamFreq[0];
-	memcpy(sam_freq, &rate, 3);
-
-	/* Todo: Set Sample bits and other parameters */
-
-	return;
+	u_audio_stop_capture(&uac1->g_audio);
 }
 
+/*-------------------------------------------------------------------------*/
+
 /* audio function driver setup/binding */
-static int
-f_audio_bind(struct usb_configuration *c, struct usb_function *f)
+static int f_audio_bind(struct usb_configuration *c, struct usb_function *f)
 {
-	struct usb_composite_dev *cdev = c->cdev;
-	struct f_audio		*audio = func_to_audio(f);
-	struct usb_string	*us;
-	int			status;
-	struct usb_ep		*ep = NULL;
-	struct f_uac1_opts	*audio_opts;
+	struct usb_composite_dev	*cdev = c->cdev;
+	struct usb_gadget		*gadget = cdev->gadget;
+	struct f_uac1			*uac1 = func_to_uac1(f);
+	struct g_audio			*audio = func_to_g_audio(f);
+	struct f_uac1_opts		*audio_opts;
+	struct usb_ep			*ep = NULL;
+	struct usb_string		*us;
+	u8				*sam_freq;
+	int				rate;
+	int				status;
 
 	audio_opts = container_of(f->fi, struct f_uac1_opts, func_inst);
-	audio->card.gadget = c->cdev->gadget;
-	/* set up ASLA audio devices */
-	if (!audio_opts->bound) {
-		status = gaudio_setup(&audio->card);
-		if (status < 0)
-			return status;
-		audio_opts->bound = true;
-	}
+
 	us = usb_gstrings_attach(cdev, uac1_strings, ARRAY_SIZE(strings_uac1));
 	if (IS_ERR(us))
 		return PTR_ERR(us);
 	ac_interface_desc.iInterface = us[STR_AC_IF].id;
-	input_terminal_desc.iTerminal = us[STR_INPUT_TERMINAL].id;
-	input_terminal_desc.iChannelNames = us[STR_INPUT_TERMINAL_CH_NAMES].id;
-	feature_unit_desc.iFeature = us[STR_FEAT_DESC_0].id;
-	output_terminal_desc.iTerminal = us[STR_OUTPUT_TERMINAL].id;
-	as_interface_alt_0_desc.iInterface = us[STR_AS_IF_ALT0].id;
-	as_interface_alt_1_desc.iInterface = us[STR_AS_IF_ALT1].id;
+	usb_out_it_desc.iTerminal = us[STR_USB_OUT_IT].id;
+	usb_out_it_desc.iChannelNames = us[STR_USB_OUT_IT_CH_NAMES].id;
+	io_out_ot_desc.iTerminal = us[STR_IO_OUT_OT].id;
+	as_out_interface_alt_0_desc.iInterface = us[STR_AS_OUT_IF_ALT0].id;
+	as_out_interface_alt_1_desc.iInterface = us[STR_AS_OUT_IF_ALT1].id;
+	io_in_it_desc.iTerminal = us[STR_IO_IN_IT].id;
+	io_in_it_desc.iChannelNames = us[STR_IO_IN_IT_CH_NAMES].id;
+	usb_in_ot_desc.iTerminal = us[STR_USB_IN_OT].id;
+	as_in_interface_alt_0_desc.iInterface = us[STR_AS_IN_IF_ALT0].id;
+	as_in_interface_alt_1_desc.iInterface = us[STR_AS_IN_IF_ALT1].id;
 
+	/* Set channel numbers */
+	usb_out_it_desc.bNrChannels = num_channels(audio_opts->c_chmask);
+	usb_out_it_desc.wChannelConfig = cpu_to_le16(audio_opts->c_chmask);
+	as_out_type_i_desc.bNrChannels = num_channels(audio_opts->c_chmask);
+	as_out_type_i_desc.bSubframeSize = audio_opts->c_ssize;
+	as_out_type_i_desc.bBitResolution = audio_opts->c_ssize * 8;
+	io_in_it_desc.bNrChannels = num_channels(audio_opts->p_chmask);
+	io_in_it_desc.wChannelConfig = cpu_to_le16(audio_opts->p_chmask);
+	as_in_type_i_desc.bNrChannels = num_channels(audio_opts->p_chmask);
+	as_in_type_i_desc.bSubframeSize = audio_opts->p_ssize;
+	as_in_type_i_desc.bBitResolution = audio_opts->p_ssize * 8;
 
-	f_audio_build_desc(audio);
+	/* Set sample rates */
+	rate = audio_opts->c_srate;
+	sam_freq = as_out_type_i_desc.tSamFreq[0];
+	memcpy(sam_freq, &rate, 3);
+	rate = audio_opts->p_srate;
+	sam_freq = as_in_type_i_desc.tSamFreq[0];
+	memcpy(sam_freq, &rate, 3);
 
 	/* allocate instance-specific interface IDs, and patch descriptors */
 	status = usb_interface_id(c, f);
 	if (status < 0)
 		goto fail;
 	ac_interface_desc.bInterfaceNumber = status;
+	uac1->ac_intf = status;
+	uac1->ac_alt = 0;
 
 	status = usb_interface_id(c, f);
 	if (status < 0)
 		goto fail;
-	as_interface_alt_0_desc.bInterfaceNumber = status;
-	as_interface_alt_1_desc.bInterfaceNumber = status;
+	as_out_interface_alt_0_desc.bInterfaceNumber = status;
+	as_out_interface_alt_1_desc.bInterfaceNumber = status;
+	uac1->as_out_intf = status;
+	uac1->as_out_alt = 0;
+
+	status = usb_interface_id(c, f);
+	if (status < 0)
+		goto fail;
+	as_in_interface_alt_0_desc.bInterfaceNumber = status;
+	as_in_interface_alt_1_desc.bInterfaceNumber = status;
+	uac1->as_in_intf = status;
+	uac1->as_in_alt = 0;
+
+	audio->gadget = gadget;
 
 	status = -ENODEV;
 
@@ -718,52 +594,42 @@ f_audio_bind(struct usb_configuration *c, struct usb_function *f)
 	audio->out_ep = ep;
 	audio->out_ep->desc = &as_out_ep_desc;
 
-	status = -ENOMEM;
+	ep = usb_ep_autoconfig(cdev->gadget, &as_in_ep_desc);
+	if (!ep)
+		goto fail;
+	audio->in_ep = ep;
+	audio->in_ep->desc = &as_in_ep_desc;
 
 	/* copy descriptors, and track endpoint copies */
 	status = usb_assign_descriptors(f, f_audio_desc, f_audio_desc, NULL,
 					NULL);
 	if (status)
 		goto fail;
+
+	audio->out_ep_maxpsize = as_out_ep_desc.wMaxPacketSize;
+	audio->in_ep_maxpsize = as_in_ep_desc.wMaxPacketSize;
+	audio->params.c_chmask = audio_opts->c_chmask;
+	audio->params.c_srate = audio_opts->c_srate;
+	audio->params.c_ssize = audio_opts->c_ssize;
+	audio->params.p_chmask = audio_opts->p_chmask;
+	audio->params.p_srate = audio_opts->p_srate;
+	audio->params.p_ssize = audio_opts->p_ssize;
+	audio->params.req_number = audio_opts->req_number;
+
+	status = g_audio_setup(audio, "UAC1_PCM", "UAC1_Gadget");
+	if (status)
+		goto err_card_register;
+
 	return 0;
 
+err_card_register:
+	usb_free_all_descriptors(f);
 fail:
-	gaudio_cleanup(&audio->card);
 	return status;
 }
 
 /*-------------------------------------------------------------------------*/
 
-static int generic_set_cmd(struct usb_audio_control *con, u8 cmd, int value)
-{
-	con->data[cmd] = value;
-
-	return 0;
-}
-
-static int generic_get_cmd(struct usb_audio_control *con, u8 cmd)
-{
-	return con->data[cmd];
-}
-
-/* Todo: add more control selecotor dynamically */
-static int control_selector_init(struct f_audio *audio)
-{
-	INIT_LIST_HEAD(&audio->cs);
-	list_add(&feature_unit.list, &audio->cs);
-
-	INIT_LIST_HEAD(&feature_unit.control);
-	list_add(&mute_control.list, &feature_unit.control);
-	list_add(&volume_control.list, &feature_unit.control);
-
-	volume_control.data[UAC__CUR] = 0xffc0;
-	volume_control.data[UAC__MIN] = 0xe3a0;
-	volume_control.data[UAC__MAX] = 0xfff0;
-	volume_control.data[UAC__RES] = 0x0030;
-
-	return 0;
-}
-
 static inline struct f_uac1_opts *to_f_uac1_opts(struct config_item *item)
 {
 	return container_of(to_config_group(item), struct f_uac1_opts,
@@ -781,9 +647,10 @@ static struct configfs_item_operations f_uac1_item_ops = {
 	.release	= f_uac1_attr_release,
 };
 
-#define UAC1_INT_ATTRIBUTE(name)					\
-static ssize_t f_uac1_opts_##name##_show(struct config_item *item,	\
-					 char *page)			\
+#define UAC1_ATTRIBUTE(name)						\
+static ssize_t f_uac1_opts_##name##_show(				\
+					  struct config_item *item,	\
+					  char *page)			\
 {									\
 	struct f_uac1_opts *opts = to_f_uac1_opts(item);		\
 	int result;							\
@@ -795,7 +662,8 @@ static ssize_t f_uac1_opts_##name##_show(struct config_item *item,	\
 	return result;							\
 }									\
 									\
-static ssize_t f_uac1_opts_##name##_store(struct config_item *item,		\
+static ssize_t f_uac1_opts_##name##_store(				\
+					  struct config_item *item,	\
 					  const char *page, size_t len)	\
 {									\
 	struct f_uac1_opts *opts = to_f_uac1_opts(item);		\
@@ -822,64 +690,22 @@ end:									\
 									\
 CONFIGFS_ATTR(f_uac1_opts_, name)
 
-UAC1_INT_ATTRIBUTE(req_buf_size);
-UAC1_INT_ATTRIBUTE(req_count);
-UAC1_INT_ATTRIBUTE(audio_buf_size);
-
-#define UAC1_STR_ATTRIBUTE(name)					\
-static ssize_t f_uac1_opts_##name##_show(struct config_item *item,	\
-					 char *page)			\
-{									\
-	struct f_uac1_opts *opts = to_f_uac1_opts(item);		\
-	int result;							\
-									\
-	mutex_lock(&opts->lock);					\
-	result = sprintf(page, "%s\n", opts->name);			\
-	mutex_unlock(&opts->lock);					\
-									\
-	return result;							\
-}									\
-									\
-static ssize_t f_uac1_opts_##name##_store(struct config_item *item,	\
-					  const char *page, size_t len)	\
-{									\
-	struct f_uac1_opts *opts = to_f_uac1_opts(item);		\
-	int ret = -EBUSY;						\
-	char *tmp;							\
-									\
-	mutex_lock(&opts->lock);					\
-	if (opts->refcnt)						\
-		goto end;						\
-									\
-	tmp = kstrndup(page, len, GFP_KERNEL);				\
-	if (tmp) {							\
-		ret = -ENOMEM;						\
-		goto end;						\
-	}								\
-	if (opts->name##_alloc)						\
-		kfree(opts->name);					\
-	opts->name##_alloc = true;					\
-	opts->name = tmp;						\
-	ret = len;							\
-									\
-end:									\
-	mutex_unlock(&opts->lock);					\
-	return ret;							\
-}									\
-									\
-CONFIGFS_ATTR(f_uac1_opts_, name)
-
-UAC1_STR_ATTRIBUTE(fn_play);
-UAC1_STR_ATTRIBUTE(fn_cap);
-UAC1_STR_ATTRIBUTE(fn_cntl);
+UAC1_ATTRIBUTE(c_chmask);
+UAC1_ATTRIBUTE(c_srate);
+UAC1_ATTRIBUTE(c_ssize);
+UAC1_ATTRIBUTE(p_chmask);
+UAC1_ATTRIBUTE(p_srate);
+UAC1_ATTRIBUTE(p_ssize);
+UAC1_ATTRIBUTE(req_number);
 
 static struct configfs_attribute *f_uac1_attrs[] = {
-	&f_uac1_opts_attr_req_buf_size,
-	&f_uac1_opts_attr_req_count,
-	&f_uac1_opts_attr_audio_buf_size,
-	&f_uac1_opts_attr_fn_play,
-	&f_uac1_opts_attr_fn_cap,
-	&f_uac1_opts_attr_fn_cntl,
+	&f_uac1_opts_attr_c_chmask,
+	&f_uac1_opts_attr_c_srate,
+	&f_uac1_opts_attr_c_ssize,
+	&f_uac1_opts_attr_p_chmask,
+	&f_uac1_opts_attr_p_srate,
+	&f_uac1_opts_attr_p_ssize,
+	&f_uac1_opts_attr_req_number,
 	NULL,
 };
 
@@ -894,12 +720,6 @@ static void f_audio_free_inst(struct usb_function_instance *f)
 	struct f_uac1_opts *opts;
 
 	opts = container_of(f, struct f_uac1_opts, func_inst);
-	if (opts->fn_play_alloc)
-		kfree(opts->fn_play);
-	if (opts->fn_cap_alloc)
-		kfree(opts->fn_cap);
-	if (opts->fn_cntl_alloc)
-		kfree(opts->fn_cntl);
 	kfree(opts);
 }
 
@@ -917,21 +737,22 @@ static struct usb_function_instance *f_audio_alloc_inst(void)
 	config_group_init_type_name(&opts->func_inst.group, "",
 				    &f_uac1_func_type);
 
-	opts->req_buf_size = UAC1_OUT_EP_MAX_PACKET_SIZE;
-	opts->req_count = UAC1_REQ_COUNT;
-	opts->audio_buf_size = UAC1_AUDIO_BUF_SIZE;
-	opts->fn_play = FILE_PCM_PLAYBACK;
-	opts->fn_cap = FILE_PCM_CAPTURE;
-	opts->fn_cntl = FILE_CONTROL;
+	opts->c_chmask = UAC1_DEF_CCHMASK;
+	opts->c_srate = UAC1_DEF_CSRATE;
+	opts->c_ssize = UAC1_DEF_CSSIZE;
+	opts->p_chmask = UAC1_DEF_PCHMASK;
+	opts->p_srate = UAC1_DEF_PSRATE;
+	opts->p_ssize = UAC1_DEF_PSSIZE;
+	opts->req_number = UAC1_DEF_REQ_NUM;
 	return &opts->func_inst;
 }
 
 static void f_audio_free(struct usb_function *f)
 {
-	struct f_audio *audio = func_to_audio(f);
+	struct g_audio *audio;
 	struct f_uac1_opts *opts;
 
-	gaudio_cleanup(&audio->card);
+	audio = func_to_g_audio(f);
 	opts = container_of(f->fi, struct f_uac1_opts, func_inst);
 	kfree(audio);
 	mutex_lock(&opts->lock);
@@ -941,42 +762,41 @@ static void f_audio_free(struct usb_function *f)
 
 static void f_audio_unbind(struct usb_configuration *c, struct usb_function *f)
 {
+	struct g_audio *audio = func_to_g_audio(f);
+
+	g_audio_cleanup(audio);
 	usb_free_all_descriptors(f);
+
+	audio->gadget = NULL;
 }
 
 static struct usb_function *f_audio_alloc(struct usb_function_instance *fi)
 {
-	struct f_audio *audio;
+	struct f_uac1 *uac1;
 	struct f_uac1_opts *opts;
 
 	/* allocate and initialize one new instance */
-	audio = kzalloc(sizeof(*audio), GFP_KERNEL);
-	if (!audio)
+	uac1 = kzalloc(sizeof(*uac1), GFP_KERNEL);
+	if (!uac1)
 		return ERR_PTR(-ENOMEM);
 
-	audio->card.func.name = "g_audio";
-
 	opts = container_of(fi, struct f_uac1_opts, func_inst);
 	mutex_lock(&opts->lock);
 	++opts->refcnt;
 	mutex_unlock(&opts->lock);
-	INIT_LIST_HEAD(&audio->play_queue);
-	spin_lock_init(&audio->lock);
-
-	audio->card.func.bind = f_audio_bind;
-	audio->card.func.unbind = f_audio_unbind;
-	audio->card.func.set_alt = f_audio_set_alt;
-	audio->card.func.setup = f_audio_setup;
-	audio->card.func.disable = f_audio_disable;
-	audio->card.func.free_func = f_audio_free;
-
-	control_selector_init(audio);
 
-	INIT_WORK(&audio->playback_work, f_audio_playback_work);
+	uac1->g_audio.func.name = "uac1_func";
+	uac1->g_audio.func.bind = f_audio_bind;
+	uac1->g_audio.func.unbind = f_audio_unbind;
+	uac1->g_audio.func.set_alt = f_audio_set_alt;
+	uac1->g_audio.func.get_alt = f_audio_get_alt;
+	uac1->g_audio.func.setup = f_audio_setup;
+	uac1->g_audio.func.disable = f_audio_disable;
+	uac1->g_audio.func.free_func = f_audio_free;
 
-	return &audio->card.func;
+	return &uac1->g_audio.func;
 }
 
 DECLARE_USB_FUNCTION_INIT(uac1, f_audio_alloc_inst, f_audio_alloc);
 MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Bryan Wu");
+MODULE_AUTHOR("Ruslan Bilovol");
diff --git a/drivers/usb/gadget/function/f_uac1_legacy.c b/drivers/usb/gadget/function/f_uac1_legacy.c
new file mode 100644
index 000000000000..5d229e72912e
--- /dev/null
+++ b/drivers/usb/gadget/function/f_uac1_legacy.c
@@ -0,0 +1,1021 @@
+/*
+ * f_audio.c -- USB Audio class function driver
+  *
+ * Copyright (C) 2008 Bryan Wu <cooloney@kernel.org>
+ * Copyright (C) 2008 Analog Devices, Inc
+ *
+ * Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/atomic.h>
+
+#include "u_uac1_legacy.h"
+
+static int generic_set_cmd(struct usb_audio_control *con, u8 cmd, int value);
+static int generic_get_cmd(struct usb_audio_control *con, u8 cmd);
+
+/*
+ * DESCRIPTORS ... most are static, but strings and full
+ * configuration descriptors are built on demand.
+ */
+
+/*
+ * We have two interfaces- AudioControl and AudioStreaming
+ * TODO: only supcard playback currently
+ */
+#define F_AUDIO_AC_INTERFACE	0
+#define F_AUDIO_AS_INTERFACE	1
+#define F_AUDIO_NUM_INTERFACES	1
+
+/* B.3.1  Standard AC Interface Descriptor */
+static struct usb_interface_descriptor ac_interface_desc = {
+	.bLength =		USB_DT_INTERFACE_SIZE,
+	.bDescriptorType =	USB_DT_INTERFACE,
+	.bNumEndpoints =	0,
+	.bInterfaceClass =	USB_CLASS_AUDIO,
+	.bInterfaceSubClass =	USB_SUBCLASS_AUDIOCONTROL,
+};
+
+/*
+ * The number of AudioStreaming and MIDIStreaming interfaces
+ * in the Audio Interface Collection
+ */
+DECLARE_UAC_AC_HEADER_DESCRIPTOR(1);
+
+#define UAC_DT_AC_HEADER_LENGTH	UAC_DT_AC_HEADER_SIZE(F_AUDIO_NUM_INTERFACES)
+/* 1 input terminal, 1 output terminal and 1 feature unit */
+#define UAC_DT_TOTAL_LENGTH (UAC_DT_AC_HEADER_LENGTH + UAC_DT_INPUT_TERMINAL_SIZE \
+	+ UAC_DT_OUTPUT_TERMINAL_SIZE + UAC_DT_FEATURE_UNIT_SIZE(0))
+/* B.3.2  Class-Specific AC Interface Descriptor */
+static struct uac1_ac_header_descriptor_1 ac_header_desc = {
+	.bLength =		UAC_DT_AC_HEADER_LENGTH,
+	.bDescriptorType =	USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype =	UAC_HEADER,
+	.bcdADC =		__constant_cpu_to_le16(0x0100),
+	.wTotalLength =		__constant_cpu_to_le16(UAC_DT_TOTAL_LENGTH),
+	.bInCollection =	F_AUDIO_NUM_INTERFACES,
+	.baInterfaceNr = {
+	/* Interface number of the first AudioStream interface */
+		[0] =		1,
+	}
+};
+
+#define INPUT_TERMINAL_ID	1
+static struct uac_input_terminal_descriptor input_terminal_desc = {
+	.bLength =		UAC_DT_INPUT_TERMINAL_SIZE,
+	.bDescriptorType =	USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype =	UAC_INPUT_TERMINAL,
+	.bTerminalID =		INPUT_TERMINAL_ID,
+	.wTerminalType =	UAC_TERMINAL_STREAMING,
+	.bAssocTerminal =	0,
+	.wChannelConfig =	0x3,
+};
+
+DECLARE_UAC_FEATURE_UNIT_DESCRIPTOR(0);
+
+#define FEATURE_UNIT_ID		2
+static struct uac_feature_unit_descriptor_0 feature_unit_desc = {
+	.bLength		= UAC_DT_FEATURE_UNIT_SIZE(0),
+	.bDescriptorType	= USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype	= UAC_FEATURE_UNIT,
+	.bUnitID		= FEATURE_UNIT_ID,
+	.bSourceID		= INPUT_TERMINAL_ID,
+	.bControlSize		= 2,
+	.bmaControls[0]		= (UAC_FU_MUTE | UAC_FU_VOLUME),
+};
+
+static struct usb_audio_control mute_control = {
+	.list = LIST_HEAD_INIT(mute_control.list),
+	.name = "Mute Control",
+	.type = UAC_FU_MUTE,
+	/* Todo: add real Mute control code */
+	.set = generic_set_cmd,
+	.get = generic_get_cmd,
+};
+
+static struct usb_audio_control volume_control = {
+	.list = LIST_HEAD_INIT(volume_control.list),
+	.name = "Volume Control",
+	.type = UAC_FU_VOLUME,
+	/* Todo: add real Volume control code */
+	.set = generic_set_cmd,
+	.get = generic_get_cmd,
+};
+
+static struct usb_audio_control_selector feature_unit = {
+	.list = LIST_HEAD_INIT(feature_unit.list),
+	.id = FEATURE_UNIT_ID,
+	.name = "Mute & Volume Control",
+	.type = UAC_FEATURE_UNIT,
+	.desc = (struct usb_descriptor_header *)&feature_unit_desc,
+};
+
+#define OUTPUT_TERMINAL_ID	3
+static struct uac1_output_terminal_descriptor output_terminal_desc = {
+	.bLength		= UAC_DT_OUTPUT_TERMINAL_SIZE,
+	.bDescriptorType	= USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype	= UAC_OUTPUT_TERMINAL,
+	.bTerminalID		= OUTPUT_TERMINAL_ID,
+	.wTerminalType		= UAC_OUTPUT_TERMINAL_SPEAKER,
+	.bAssocTerminal		= FEATURE_UNIT_ID,
+	.bSourceID		= FEATURE_UNIT_ID,
+};
+
+/* B.4.1  Standard AS Interface Descriptor */
+static struct usb_interface_descriptor as_interface_alt_0_desc = {
+	.bLength =		USB_DT_INTERFACE_SIZE,
+	.bDescriptorType =	USB_DT_INTERFACE,
+	.bAlternateSetting =	0,
+	.bNumEndpoints =	0,
+	.bInterfaceClass =	USB_CLASS_AUDIO,
+	.bInterfaceSubClass =	USB_SUBCLASS_AUDIOSTREAMING,
+};
+
+static struct usb_interface_descriptor as_interface_alt_1_desc = {
+	.bLength =		USB_DT_INTERFACE_SIZE,
+	.bDescriptorType =	USB_DT_INTERFACE,
+	.bAlternateSetting =	1,
+	.bNumEndpoints =	1,
+	.bInterfaceClass =	USB_CLASS_AUDIO,
+	.bInterfaceSubClass =	USB_SUBCLASS_AUDIOSTREAMING,
+};
+
+/* B.4.2  Class-Specific AS Interface Descriptor */
+static struct uac1_as_header_descriptor as_header_desc = {
+	.bLength =		UAC_DT_AS_HEADER_SIZE,
+	.bDescriptorType =	USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype =	UAC_AS_GENERAL,
+	.bTerminalLink =	INPUT_TERMINAL_ID,
+	.bDelay =		1,
+	.wFormatTag =		UAC_FORMAT_TYPE_I_PCM,
+};
+
+DECLARE_UAC_FORMAT_TYPE_I_DISCRETE_DESC(1);
+
+static struct uac_format_type_i_discrete_descriptor_1 as_type_i_desc = {
+	.bLength =		UAC_FORMAT_TYPE_I_DISCRETE_DESC_SIZE(1),
+	.bDescriptorType =	USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype =	UAC_FORMAT_TYPE,
+	.bFormatType =		UAC_FORMAT_TYPE_I,
+	.bSubframeSize =	2,
+	.bBitResolution =	16,
+	.bSamFreqType =		1,
+};
+
+/* Standard ISO OUT Endpoint Descriptor */
+static struct usb_endpoint_descriptor as_out_ep_desc  = {
+	.bLength =		USB_DT_ENDPOINT_AUDIO_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_SYNC_ADAPTIVE
+				| USB_ENDPOINT_XFER_ISOC,
+	.wMaxPacketSize	=	cpu_to_le16(UAC1_OUT_EP_MAX_PACKET_SIZE),
+	.bInterval =		4,
+};
+
+/* Class-specific AS ISO OUT Endpoint Descriptor */
+static struct uac_iso_endpoint_descriptor as_iso_out_desc = {
+	.bLength =		UAC_ISO_ENDPOINT_DESC_SIZE,
+	.bDescriptorType =	USB_DT_CS_ENDPOINT,
+	.bDescriptorSubtype =	UAC_EP_GENERAL,
+	.bmAttributes = 	1,
+	.bLockDelayUnits =	1,
+	.wLockDelay =		__constant_cpu_to_le16(1),
+};
+
+static struct usb_descriptor_header *f_audio_desc[] = {
+	(struct usb_descriptor_header *)&ac_interface_desc,
+	(struct usb_descriptor_header *)&ac_header_desc,
+
+	(struct usb_descriptor_header *)&input_terminal_desc,
+	(struct usb_descriptor_header *)&output_terminal_desc,
+	(struct usb_descriptor_header *)&feature_unit_desc,
+
+	(struct usb_descriptor_header *)&as_interface_alt_0_desc,
+	(struct usb_descriptor_header *)&as_interface_alt_1_desc,
+	(struct usb_descriptor_header *)&as_header_desc,
+
+	(struct usb_descriptor_header *)&as_type_i_desc,
+
+	(struct usb_descriptor_header *)&as_out_ep_desc,
+	(struct usb_descriptor_header *)&as_iso_out_desc,
+	NULL,
+};
+
+enum {
+	STR_AC_IF,
+	STR_INPUT_TERMINAL,
+	STR_INPUT_TERMINAL_CH_NAMES,
+	STR_FEAT_DESC_0,
+	STR_OUTPUT_TERMINAL,
+	STR_AS_IF_ALT0,
+	STR_AS_IF_ALT1,
+};
+
+static struct usb_string strings_uac1[] = {
+	[STR_AC_IF].s = "AC Interface",
+	[STR_INPUT_TERMINAL].s = "Input terminal",
+	[STR_INPUT_TERMINAL_CH_NAMES].s = "Channels",
+	[STR_FEAT_DESC_0].s = "Volume control & mute",
+	[STR_OUTPUT_TERMINAL].s = "Output terminal",
+	[STR_AS_IF_ALT0].s = "AS Interface",
+	[STR_AS_IF_ALT1].s = "AS Interface",
+	{ },
+};
+
+static struct usb_gadget_strings str_uac1 = {
+	.language = 0x0409,	/* en-us */
+	.strings = strings_uac1,
+};
+
+static struct usb_gadget_strings *uac1_strings[] = {
+	&str_uac1,
+	NULL,
+};
+
+/*
+ * This function is an ALSA sound card following USB Audio Class Spec 1.0.
+ */
+
+/*-------------------------------------------------------------------------*/
+struct f_audio_buf {
+	u8 *buf;
+	int actual;
+	struct list_head list;
+};
+
+static struct f_audio_buf *f_audio_buffer_alloc(int buf_size)
+{
+	struct f_audio_buf *copy_buf;
+
+	copy_buf = kzalloc(sizeof *copy_buf, GFP_ATOMIC);
+	if (!copy_buf)
+		return ERR_PTR(-ENOMEM);
+
+	copy_buf->buf = kzalloc(buf_size, GFP_ATOMIC);
+	if (!copy_buf->buf) {
+		kfree(copy_buf);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	return copy_buf;
+}
+
+static void f_audio_buffer_free(struct f_audio_buf *audio_buf)
+{
+	kfree(audio_buf->buf);
+	kfree(audio_buf);
+}
+/*-------------------------------------------------------------------------*/
+
+struct f_audio {
+	struct gaudio			card;
+
+	u8 ac_intf, ac_alt;
+	u8 as_intf, as_alt;
+
+	/* endpoints handle full and/or high speeds */
+	struct usb_ep			*out_ep;
+
+	spinlock_t			lock;
+	struct f_audio_buf *copy_buf;
+	struct work_struct playback_work;
+	struct list_head play_queue;
+
+	/* Control Set command */
+	struct list_head cs;
+	u8 set_cmd;
+	struct usb_audio_control *set_con;
+};
+
+static inline struct f_audio *func_to_audio(struct usb_function *f)
+{
+	return container_of(f, struct f_audio, card.func);
+}
+
+/*-------------------------------------------------------------------------*/
+
+static void f_audio_playback_work(struct work_struct *data)
+{
+	struct f_audio *audio = container_of(data, struct f_audio,
+					playback_work);
+	struct f_audio_buf *play_buf;
+
+	spin_lock_irq(&audio->lock);
+	if (list_empty(&audio->play_queue)) {
+		spin_unlock_irq(&audio->lock);
+		return;
+	}
+	play_buf = list_first_entry(&audio->play_queue,
+			struct f_audio_buf, list);
+	list_del(&play_buf->list);
+	spin_unlock_irq(&audio->lock);
+
+	u_audio_playback(&audio->card, play_buf->buf, play_buf->actual);
+	f_audio_buffer_free(play_buf);
+}
+
+static int f_audio_out_ep_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct f_audio *audio = req->context;
+	struct usb_composite_dev *cdev = audio->card.func.config->cdev;
+	struct f_audio_buf *copy_buf = audio->copy_buf;
+	struct f_uac1_legacy_opts *opts;
+	int audio_buf_size;
+	int err;
+
+	opts = container_of(audio->card.func.fi, struct f_uac1_legacy_opts,
+			    func_inst);
+	audio_buf_size = opts->audio_buf_size;
+
+	if (!copy_buf)
+		return -EINVAL;
+
+	/* Copy buffer is full, add it to the play_queue */
+	if (audio_buf_size - copy_buf->actual < req->actual) {
+		list_add_tail(&copy_buf->list, &audio->play_queue);
+		schedule_work(&audio->playback_work);
+		copy_buf = f_audio_buffer_alloc(audio_buf_size);
+		if (IS_ERR(copy_buf))
+			return -ENOMEM;
+	}
+
+	memcpy(copy_buf->buf + copy_buf->actual, req->buf, req->actual);
+	copy_buf->actual += req->actual;
+	audio->copy_buf = copy_buf;
+
+	err = usb_ep_queue(ep, req, GFP_ATOMIC);
+	if (err)
+		ERROR(cdev, "%s queue req: %d\n", ep->name, err);
+
+	return 0;
+
+}
+
+static void f_audio_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct f_audio *audio = req->context;
+	int status = req->status;
+	u32 data = 0;
+	struct usb_ep *out_ep = audio->out_ep;
+
+	switch (status) {
+
+	case 0:				/* normal completion? */
+		if (ep == out_ep)
+			f_audio_out_ep_complete(ep, req);
+		else if (audio->set_con) {
+			memcpy(&data, req->buf, req->length);
+			audio->set_con->set(audio->set_con, audio->set_cmd,
+					le16_to_cpu(data));
+			audio->set_con = NULL;
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+static int audio_set_intf_req(struct usb_function *f,
+		const struct usb_ctrlrequest *ctrl)
+{
+	struct f_audio		*audio = func_to_audio(f);
+	struct usb_composite_dev *cdev = f->config->cdev;
+	struct usb_request	*req = cdev->req;
+	u8			id = ((le16_to_cpu(ctrl->wIndex) >> 8) & 0xFF);
+	u16			len = le16_to_cpu(ctrl->wLength);
+	u16			w_value = le16_to_cpu(ctrl->wValue);
+	u8			con_sel = (w_value >> 8) & 0xFF;
+	u8			cmd = (ctrl->bRequest & 0x0F);
+	struct usb_audio_control_selector *cs;
+	struct usb_audio_control *con;
+
+	DBG(cdev, "bRequest 0x%x, w_value 0x%04x, len %d, entity %d\n",
+			ctrl->bRequest, w_value, len, id);
+
+	list_for_each_entry(cs, &audio->cs, list) {
+		if (cs->id == id) {
+			list_for_each_entry(con, &cs->control, list) {
+				if (con->type == con_sel) {
+					audio->set_con = con;
+					break;
+				}
+			}
+			break;
+		}
+	}
+
+	audio->set_cmd = cmd;
+	req->context = audio;
+	req->complete = f_audio_complete;
+
+	return len;
+}
+
+static int audio_get_intf_req(struct usb_function *f,
+		const struct usb_ctrlrequest *ctrl)
+{
+	struct f_audio		*audio = func_to_audio(f);
+	struct usb_composite_dev *cdev = f->config->cdev;
+	struct usb_request	*req = cdev->req;
+	int			value = -EOPNOTSUPP;
+	u8			id = ((le16_to_cpu(ctrl->wIndex) >> 8) & 0xFF);
+	u16			len = le16_to_cpu(ctrl->wLength);
+	u16			w_value = le16_to_cpu(ctrl->wValue);
+	u8			con_sel = (w_value >> 8) & 0xFF;
+	u8			cmd = (ctrl->bRequest & 0x0F);
+	struct usb_audio_control_selector *cs;
+	struct usb_audio_control *con;
+
+	DBG(cdev, "bRequest 0x%x, w_value 0x%04x, len %d, entity %d\n",
+			ctrl->bRequest, w_value, len, id);
+
+	list_for_each_entry(cs, &audio->cs, list) {
+		if (cs->id == id) {
+			list_for_each_entry(con, &cs->control, list) {
+				if (con->type == con_sel && con->get) {
+					value = con->get(con, cmd);
+					break;
+				}
+			}
+			break;
+		}
+	}
+
+	req->context = audio;
+	req->complete = f_audio_complete;
+	len = min_t(size_t, sizeof(value), len);
+	memcpy(req->buf, &value, len);
+
+	return len;
+}
+
+static int audio_set_endpoint_req(struct usb_function *f,
+		const struct usb_ctrlrequest *ctrl)
+{
+	struct usb_composite_dev *cdev = f->config->cdev;
+	int			value = -EOPNOTSUPP;
+	u16			ep = le16_to_cpu(ctrl->wIndex);
+	u16			len = le16_to_cpu(ctrl->wLength);
+	u16			w_value = le16_to_cpu(ctrl->wValue);
+
+	DBG(cdev, "bRequest 0x%x, w_value 0x%04x, len %d, endpoint %d\n",
+			ctrl->bRequest, w_value, len, ep);
+
+	switch (ctrl->bRequest) {
+	case UAC_SET_CUR:
+		value = len;
+		break;
+
+	case UAC_SET_MIN:
+		break;
+
+	case UAC_SET_MAX:
+		break;
+
+	case UAC_SET_RES:
+		break;
+
+	case UAC_SET_MEM:
+		break;
+
+	default:
+		break;
+	}
+
+	return value;
+}
+
+static int audio_get_endpoint_req(struct usb_function *f,
+		const struct usb_ctrlrequest *ctrl)
+{
+	struct usb_composite_dev *cdev = f->config->cdev;
+	int value = -EOPNOTSUPP;
+	u8 ep = ((le16_to_cpu(ctrl->wIndex) >> 8) & 0xFF);
+	u16 len = le16_to_cpu(ctrl->wLength);
+	u16 w_value = le16_to_cpu(ctrl->wValue);
+
+	DBG(cdev, "bRequest 0x%x, w_value 0x%04x, len %d, endpoint %d\n",
+			ctrl->bRequest, w_value, len, ep);
+
+	switch (ctrl->bRequest) {
+	case UAC_GET_CUR:
+	case UAC_GET_MIN:
+	case UAC_GET_MAX:
+	case UAC_GET_RES:
+		value = len;
+		break;
+	case UAC_GET_MEM:
+		break;
+	default:
+		break;
+	}
+
+	return value;
+}
+
+static int
+f_audio_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
+{
+	struct usb_composite_dev *cdev = f->config->cdev;
+	struct usb_request	*req = cdev->req;
+	int			value = -EOPNOTSUPP;
+	u16			w_index = le16_to_cpu(ctrl->wIndex);
+	u16			w_value = le16_to_cpu(ctrl->wValue);
+	u16			w_length = le16_to_cpu(ctrl->wLength);
+
+	/* composite driver infrastructure handles everything; interface
+	 * activation uses set_alt().
+	 */
+	switch (ctrl->bRequestType) {
+	case USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE:
+		value = audio_set_intf_req(f, ctrl);
+		break;
+
+	case USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE:
+		value = audio_get_intf_req(f, ctrl);
+		break;
+
+	case USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_ENDPOINT:
+		value = audio_set_endpoint_req(f, ctrl);
+		break;
+
+	case USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_ENDPOINT:
+		value = audio_get_endpoint_req(f, ctrl);
+		break;
+
+	default:
+		ERROR(cdev, "invalid control req%02x.%02x v%04x i%04x l%d\n",
+			ctrl->bRequestType, ctrl->bRequest,
+			w_value, w_index, w_length);
+	}
+
+	/* respond with data transfer or status phase? */
+	if (value >= 0) {
+		DBG(cdev, "audio req%02x.%02x v%04x i%04x l%d\n",
+			ctrl->bRequestType, ctrl->bRequest,
+			w_value, w_index, w_length);
+		req->zero = 0;
+		req->length = value;
+		value = usb_ep_queue(cdev->gadget->ep0, req, GFP_ATOMIC);
+		if (value < 0)
+			ERROR(cdev, "audio response on err %d\n", value);
+	}
+
+	/* device either stalls (value < 0) or reports success */
+	return value;
+}
+
+static int f_audio_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
+{
+	struct f_audio		*audio = func_to_audio(f);
+	struct usb_composite_dev *cdev = f->config->cdev;
+	struct usb_ep *out_ep = audio->out_ep;
+	struct usb_request *req;
+	struct f_uac1_legacy_opts *opts;
+	int req_buf_size, req_count, audio_buf_size;
+	int i = 0, err = 0;
+
+	DBG(cdev, "intf %d, alt %d\n", intf, alt);
+
+	opts = container_of(f->fi, struct f_uac1_legacy_opts, func_inst);
+	req_buf_size = opts->req_buf_size;
+	req_count = opts->req_count;
+	audio_buf_size = opts->audio_buf_size;
+
+	/* No i/f has more than 2 alt settings */
+	if (alt > 1) {
+		ERROR(cdev, "%s:%d Error!\n", __func__, __LINE__);
+		return -EINVAL;
+	}
+
+	if (intf == audio->ac_intf) {
+		/* Control I/f has only 1 AltSetting - 0 */
+		if (alt) {
+			ERROR(cdev, "%s:%d Error!\n", __func__, __LINE__);
+			return -EINVAL;
+		}
+		return 0;
+	} else if (intf == audio->as_intf) {
+		if (alt == 1) {
+			err = config_ep_by_speed(cdev->gadget, f, out_ep);
+			if (err)
+				return err;
+
+			usb_ep_enable(out_ep);
+			audio->copy_buf = f_audio_buffer_alloc(audio_buf_size);
+			if (IS_ERR(audio->copy_buf))
+				return -ENOMEM;
+
+			/*
+			 * allocate a bunch of read buffers
+			 * and queue them all at once.
+			 */
+			for (i = 0; i < req_count && err == 0; i++) {
+				req = usb_ep_alloc_request(out_ep, GFP_ATOMIC);
+				if (req) {
+					req->buf = kzalloc(req_buf_size,
+							GFP_ATOMIC);
+					if (req->buf) {
+						req->length = req_buf_size;
+						req->context = audio;
+						req->complete =
+							f_audio_complete;
+						err = usb_ep_queue(out_ep,
+							req, GFP_ATOMIC);
+						if (err)
+							ERROR(cdev,
+							"%s queue req: %d\n",
+							out_ep->name, err);
+					} else
+						err = -ENOMEM;
+				} else
+					err = -ENOMEM;
+			}
+
+		} else {
+			struct f_audio_buf *copy_buf = audio->copy_buf;
+			if (copy_buf) {
+				list_add_tail(&copy_buf->list,
+						&audio->play_queue);
+				schedule_work(&audio->playback_work);
+			}
+		}
+		audio->as_alt = alt;
+	}
+
+	return err;
+}
+
+static int f_audio_get_alt(struct usb_function *f, unsigned intf)
+{
+	struct f_audio		*audio = func_to_audio(f);
+	struct usb_composite_dev *cdev = f->config->cdev;
+
+	if (intf == audio->ac_intf)
+		return audio->ac_alt;
+	else if (intf == audio->as_intf)
+		return audio->as_alt;
+	else
+		ERROR(cdev, "%s:%d Invalid Interface %d!\n",
+		      __func__, __LINE__, intf);
+
+	return -EINVAL;
+}
+
+static void f_audio_disable(struct usb_function *f)
+{
+	return;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static void f_audio_build_desc(struct f_audio *audio)
+{
+	struct gaudio *card = &audio->card;
+	u8 *sam_freq;
+	int rate;
+
+	/* Set channel numbers */
+	input_terminal_desc.bNrChannels = u_audio_get_playback_channels(card);
+	as_type_i_desc.bNrChannels = u_audio_get_playback_channels(card);
+
+	/* Set sample rates */
+	rate = u_audio_get_playback_rate(card);
+	sam_freq = as_type_i_desc.tSamFreq[0];
+	memcpy(sam_freq, &rate, 3);
+
+	/* Todo: Set Sample bits and other parameters */
+
+	return;
+}
+
+/* audio function driver setup/binding */
+static int
+f_audio_bind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct usb_composite_dev *cdev = c->cdev;
+	struct f_audio		*audio = func_to_audio(f);
+	struct usb_string	*us;
+	int			status;
+	struct usb_ep		*ep = NULL;
+	struct f_uac1_legacy_opts	*audio_opts;
+
+	audio_opts = container_of(f->fi, struct f_uac1_legacy_opts, func_inst);
+	audio->card.gadget = c->cdev->gadget;
+	/* set up ASLA audio devices */
+	if (!audio_opts->bound) {
+		status = gaudio_setup(&audio->card);
+		if (status < 0)
+			return status;
+		audio_opts->bound = true;
+	}
+	us = usb_gstrings_attach(cdev, uac1_strings, ARRAY_SIZE(strings_uac1));
+	if (IS_ERR(us))
+		return PTR_ERR(us);
+	ac_interface_desc.iInterface = us[STR_AC_IF].id;
+	input_terminal_desc.iTerminal = us[STR_INPUT_TERMINAL].id;
+	input_terminal_desc.iChannelNames = us[STR_INPUT_TERMINAL_CH_NAMES].id;
+	feature_unit_desc.iFeature = us[STR_FEAT_DESC_0].id;
+	output_terminal_desc.iTerminal = us[STR_OUTPUT_TERMINAL].id;
+	as_interface_alt_0_desc.iInterface = us[STR_AS_IF_ALT0].id;
+	as_interface_alt_1_desc.iInterface = us[STR_AS_IF_ALT1].id;
+
+
+	f_audio_build_desc(audio);
+
+	/* allocate instance-specific interface IDs, and patch descriptors */
+	status = usb_interface_id(c, f);
+	if (status < 0)
+		goto fail;
+	ac_interface_desc.bInterfaceNumber = status;
+	audio->ac_intf = status;
+	audio->ac_alt = 0;
+
+	status = usb_interface_id(c, f);
+	if (status < 0)
+		goto fail;
+	as_interface_alt_0_desc.bInterfaceNumber = status;
+	as_interface_alt_1_desc.bInterfaceNumber = status;
+	audio->as_intf = status;
+	audio->as_alt = 0;
+
+	status = -ENODEV;
+
+	/* allocate instance-specific endpoints */
+	ep = usb_ep_autoconfig(cdev->gadget, &as_out_ep_desc);
+	if (!ep)
+		goto fail;
+	audio->out_ep = ep;
+	audio->out_ep->desc = &as_out_ep_desc;
+
+	status = -ENOMEM;
+
+	/* copy descriptors, and track endpoint copies */
+	status = usb_assign_descriptors(f, f_audio_desc, f_audio_desc, NULL,
+					NULL);
+	if (status)
+		goto fail;
+	return 0;
+
+fail:
+	gaudio_cleanup(&audio->card);
+	return status;
+}
+
+/*-------------------------------------------------------------------------*/
+
+static int generic_set_cmd(struct usb_audio_control *con, u8 cmd, int value)
+{
+	con->data[cmd] = value;
+
+	return 0;
+}
+
+static int generic_get_cmd(struct usb_audio_control *con, u8 cmd)
+{
+	return con->data[cmd];
+}
+
+/* Todo: add more control selecotor dynamically */
+static int control_selector_init(struct f_audio *audio)
+{
+	INIT_LIST_HEAD(&audio->cs);
+	list_add(&feature_unit.list, &audio->cs);
+
+	INIT_LIST_HEAD(&feature_unit.control);
+	list_add(&mute_control.list, &feature_unit.control);
+	list_add(&volume_control.list, &feature_unit.control);
+
+	volume_control.data[UAC__CUR] = 0xffc0;
+	volume_control.data[UAC__MIN] = 0xe3a0;
+	volume_control.data[UAC__MAX] = 0xfff0;
+	volume_control.data[UAC__RES] = 0x0030;
+
+	return 0;
+}
+
+static inline
+struct f_uac1_legacy_opts *to_f_uac1_opts(struct config_item *item)
+{
+	return container_of(to_config_group(item), struct f_uac1_legacy_opts,
+			    func_inst.group);
+}
+
+static void f_uac1_attr_release(struct config_item *item)
+{
+	struct f_uac1_legacy_opts *opts = to_f_uac1_opts(item);
+
+	usb_put_function_instance(&opts->func_inst);
+}
+
+static struct configfs_item_operations f_uac1_item_ops = {
+	.release	= f_uac1_attr_release,
+};
+
+#define UAC1_INT_ATTRIBUTE(name)					\
+static ssize_t f_uac1_opts_##name##_show(struct config_item *item,	\
+					 char *page)			\
+{									\
+	struct f_uac1_legacy_opts *opts = to_f_uac1_opts(item);		\
+	int result;							\
+									\
+	mutex_lock(&opts->lock);					\
+	result = sprintf(page, "%u\n", opts->name);			\
+	mutex_unlock(&opts->lock);					\
+									\
+	return result;							\
+}									\
+									\
+static ssize_t f_uac1_opts_##name##_store(struct config_item *item,		\
+					  const char *page, size_t len)	\
+{									\
+	struct f_uac1_legacy_opts *opts = to_f_uac1_opts(item);		\
+	int ret;							\
+	u32 num;							\
+									\
+	mutex_lock(&opts->lock);					\
+	if (opts->refcnt) {						\
+		ret = -EBUSY;						\
+		goto end;						\
+	}								\
+									\
+	ret = kstrtou32(page, 0, &num);					\
+	if (ret)							\
+		goto end;						\
+									\
+	opts->name = num;						\
+	ret = len;							\
+									\
+end:									\
+	mutex_unlock(&opts->lock);					\
+	return ret;							\
+}									\
+									\
+CONFIGFS_ATTR(f_uac1_opts_, name)
+
+UAC1_INT_ATTRIBUTE(req_buf_size);
+UAC1_INT_ATTRIBUTE(req_count);
+UAC1_INT_ATTRIBUTE(audio_buf_size);
+
+#define UAC1_STR_ATTRIBUTE(name)					\
+static ssize_t f_uac1_opts_##name##_show(struct config_item *item,	\
+					 char *page)			\
+{									\
+	struct f_uac1_legacy_opts *opts = to_f_uac1_opts(item);		\
+	int result;							\
+									\
+	mutex_lock(&opts->lock);					\
+	result = sprintf(page, "%s\n", opts->name);			\
+	mutex_unlock(&opts->lock);					\
+									\
+	return result;							\
+}									\
+									\
+static ssize_t f_uac1_opts_##name##_store(struct config_item *item,	\
+					  const char *page, size_t len)	\
+{									\
+	struct f_uac1_legacy_opts *opts = to_f_uac1_opts(item);		\
+	int ret = -EBUSY;						\
+	char *tmp;							\
+									\
+	mutex_lock(&opts->lock);					\
+	if (opts->refcnt)						\
+		goto end;						\
+									\
+	tmp = kstrndup(page, len, GFP_KERNEL);				\
+	if (tmp) {							\
+		ret = -ENOMEM;						\
+		goto end;						\
+	}								\
+	if (opts->name##_alloc)						\
+		kfree(opts->name);					\
+	opts->name##_alloc = true;					\
+	opts->name = tmp;						\
+	ret = len;							\
+									\
+end:									\
+	mutex_unlock(&opts->lock);					\
+	return ret;							\
+}									\
+									\
+CONFIGFS_ATTR(f_uac1_opts_, name)
+
+UAC1_STR_ATTRIBUTE(fn_play);
+UAC1_STR_ATTRIBUTE(fn_cap);
+UAC1_STR_ATTRIBUTE(fn_cntl);
+
+static struct configfs_attribute *f_uac1_attrs[] = {
+	&f_uac1_opts_attr_req_buf_size,
+	&f_uac1_opts_attr_req_count,
+	&f_uac1_opts_attr_audio_buf_size,
+	&f_uac1_opts_attr_fn_play,
+	&f_uac1_opts_attr_fn_cap,
+	&f_uac1_opts_attr_fn_cntl,
+	NULL,
+};
+
+static struct config_item_type f_uac1_func_type = {
+	.ct_item_ops	= &f_uac1_item_ops,
+	.ct_attrs	= f_uac1_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+static void f_audio_free_inst(struct usb_function_instance *f)
+{
+	struct f_uac1_legacy_opts *opts;
+
+	opts = container_of(f, struct f_uac1_legacy_opts, func_inst);
+	if (opts->fn_play_alloc)
+		kfree(opts->fn_play);
+	if (opts->fn_cap_alloc)
+		kfree(opts->fn_cap);
+	if (opts->fn_cntl_alloc)
+		kfree(opts->fn_cntl);
+	kfree(opts);
+}
+
+static struct usb_function_instance *f_audio_alloc_inst(void)
+{
+	struct f_uac1_legacy_opts *opts;
+
+	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
+	if (!opts)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_init(&opts->lock);
+	opts->func_inst.free_func_inst = f_audio_free_inst;
+
+	config_group_init_type_name(&opts->func_inst.group, "",
+				    &f_uac1_func_type);
+
+	opts->req_buf_size = UAC1_OUT_EP_MAX_PACKET_SIZE;
+	opts->req_count = UAC1_REQ_COUNT;
+	opts->audio_buf_size = UAC1_AUDIO_BUF_SIZE;
+	opts->fn_play = FILE_PCM_PLAYBACK;
+	opts->fn_cap = FILE_PCM_CAPTURE;
+	opts->fn_cntl = FILE_CONTROL;
+	return &opts->func_inst;
+}
+
+static void f_audio_free(struct usb_function *f)
+{
+	struct f_audio *audio = func_to_audio(f);
+	struct f_uac1_legacy_opts *opts;
+
+	gaudio_cleanup(&audio->card);
+	opts = container_of(f->fi, struct f_uac1_legacy_opts, func_inst);
+	kfree(audio);
+	mutex_lock(&opts->lock);
+	--opts->refcnt;
+	mutex_unlock(&opts->lock);
+}
+
+static void f_audio_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	usb_free_all_descriptors(f);
+}
+
+static struct usb_function *f_audio_alloc(struct usb_function_instance *fi)
+{
+	struct f_audio *audio;
+	struct f_uac1_legacy_opts *opts;
+
+	/* allocate and initialize one new instance */
+	audio = kzalloc(sizeof(*audio), GFP_KERNEL);
+	if (!audio)
+		return ERR_PTR(-ENOMEM);
+
+	audio->card.func.name = "g_audio";
+
+	opts = container_of(fi, struct f_uac1_legacy_opts, func_inst);
+	mutex_lock(&opts->lock);
+	++opts->refcnt;
+	mutex_unlock(&opts->lock);
+	INIT_LIST_HEAD(&audio->play_queue);
+	spin_lock_init(&audio->lock);
+
+	audio->card.func.bind = f_audio_bind;
+	audio->card.func.unbind = f_audio_unbind;
+	audio->card.func.set_alt = f_audio_set_alt;
+	audio->card.func.get_alt = f_audio_get_alt;
+	audio->card.func.setup = f_audio_setup;
+	audio->card.func.disable = f_audio_disable;
+	audio->card.func.free_func = f_audio_free;
+
+	control_selector_init(audio);
+
+	INIT_WORK(&audio->playback_work, f_audio_playback_work);
+
+	return &audio->card.func;
+}
+
+DECLARE_USB_FUNCTION_INIT(uac1_legacy, f_audio_alloc_inst, f_audio_alloc);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Bryan Wu");
diff --git a/drivers/usb/gadget/function/f_uac2.c b/drivers/usb/gadget/function/f_uac2.c
index f6a0d3a1311b..9082ce261e70 100644
--- a/drivers/usb/gadget/function/f_uac2.c
+++ b/drivers/usb/gadget/function/f_uac2.c
@@ -13,13 +13,9 @@
 
 #include <linux/usb/audio.h>
 #include <linux/usb/audio-v2.h>
-#include <linux/platform_device.h>
 #include <linux/module.h>
 
-#include <sound/core.h>
-#include <sound/pcm.h>
-#include <sound/pcm_params.h>
-
+#include "u_audio.h"
 #include "u_uac2.h"
 
 /*
@@ -51,506 +47,23 @@
 #define UNFLW_CTRL	8
 #define OVFLW_CTRL	10
 
-static const char *uac2_name = "snd_uac2";
-
-struct uac2_req {
-	struct uac2_rtd_params *pp; /* parent param */
-	struct usb_request *req;
-};
-
-struct uac2_rtd_params {
-	struct snd_uac2_chip *uac2; /* parent chip */
-	bool ep_enabled; /* if the ep is enabled */
-	/* Size of the ring buffer */
-	size_t dma_bytes;
-	unsigned char *dma_area;
-
-	struct snd_pcm_substream *ss;
-
-	/* Ring buffer */
-	ssize_t hw_ptr;
-
-	void *rbuf;
-
-	size_t period_size;
-
-	unsigned max_psize;
-	struct uac2_req *ureq;
-
-	spinlock_t lock;
-};
-
-struct snd_uac2_chip {
-	struct platform_device pdev;
-	struct platform_driver pdrv;
-
-	struct uac2_rtd_params p_prm;
-	struct uac2_rtd_params c_prm;
-
-	struct snd_card *card;
-	struct snd_pcm *pcm;
-
-	/* timekeeping for the playback endpoint */
-	unsigned int p_interval;
-	unsigned int p_residue;
-
-	/* pre-calculated values for playback iso completion */
-	unsigned int p_pktsize;
-	unsigned int p_pktsize_residue;
-	unsigned int p_framesize;
+struct f_uac2 {
+	struct g_audio g_audio;
+	u8 ac_intf, as_in_intf, as_out_intf;
+	u8 ac_alt, as_in_alt, as_out_alt;	/* needed for get_alt() */
 };
 
-#define BUFF_SIZE_MAX	(PAGE_SIZE * 16)
-#define PRD_SIZE_MAX	PAGE_SIZE
-#define MIN_PERIODS	4
-
-static struct snd_pcm_hardware uac2_pcm_hardware = {
-	.info = SNDRV_PCM_INFO_INTERLEAVED | SNDRV_PCM_INFO_BLOCK_TRANSFER
-		 | SNDRV_PCM_INFO_MMAP | SNDRV_PCM_INFO_MMAP_VALID
-		 | SNDRV_PCM_INFO_PAUSE | SNDRV_PCM_INFO_RESUME,
-	.rates = SNDRV_PCM_RATE_CONTINUOUS,
-	.periods_max = BUFF_SIZE_MAX / PRD_SIZE_MAX,
-	.buffer_bytes_max = BUFF_SIZE_MAX,
-	.period_bytes_max = PRD_SIZE_MAX,
-	.periods_min = MIN_PERIODS,
-};
-
-struct audio_dev {
-	u8 ac_intf, ac_alt;
-	u8 as_out_intf, as_out_alt;
-	u8 as_in_intf, as_in_alt;
-
-	struct usb_ep *in_ep, *out_ep;
-	struct usb_function func;
-
-	/* The ALSA Sound Card it represents on the USB-Client side */
-	struct snd_uac2_chip uac2;
-};
-
-static inline
-struct audio_dev *func_to_agdev(struct usb_function *f)
+static inline struct f_uac2 *func_to_uac2(struct usb_function *f)
 {
-	return container_of(f, struct audio_dev, func);
+	return container_of(f, struct f_uac2, g_audio.func);
 }
 
 static inline
-struct audio_dev *uac2_to_agdev(struct snd_uac2_chip *u)
-{
-	return container_of(u, struct audio_dev, uac2);
-}
-
-static inline
-struct snd_uac2_chip *pdev_to_uac2(struct platform_device *p)
-{
-	return container_of(p, struct snd_uac2_chip, pdev);
-}
-
-static inline
-struct f_uac2_opts *agdev_to_uac2_opts(struct audio_dev *agdev)
+struct f_uac2_opts *g_audio_to_uac2_opts(struct g_audio *agdev)
 {
 	return container_of(agdev->func.fi, struct f_uac2_opts, func_inst);
 }
 
-static inline
-uint num_channels(uint chanmask)
-{
-	uint num = 0;
-
-	while (chanmask) {
-		num += (chanmask & 1);
-		chanmask >>= 1;
-	}
-
-	return num;
-}
-
-static void
-agdev_iso_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	unsigned pending;
-	unsigned long flags;
-	unsigned int hw_ptr;
-	bool update_alsa = false;
-	int status = req->status;
-	struct uac2_req *ur = req->context;
-	struct snd_pcm_substream *substream;
-	struct uac2_rtd_params *prm = ur->pp;
-	struct snd_uac2_chip *uac2 = prm->uac2;
-
-	/* i/f shutting down */
-	if (!prm->ep_enabled || req->status == -ESHUTDOWN)
-		return;
-
-	/*
-	 * We can't really do much about bad xfers.
-	 * Afterall, the ISOCH xfers could fail legitimately.
-	 */
-	if (status)
-		pr_debug("%s: iso_complete status(%d) %d/%d\n",
-			__func__, status, req->actual, req->length);
-
-	substream = prm->ss;
-
-	/* Do nothing if ALSA isn't active */
-	if (!substream)
-		goto exit;
-
-	spin_lock_irqsave(&prm->lock, flags);
-
-	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
-		/*
-		 * For each IN packet, take the quotient of the current data
-		 * rate and the endpoint's interval as the base packet size.
-		 * If there is a residue from this division, add it to the
-		 * residue accumulator.
-		 */
-		req->length = uac2->p_pktsize;
-		uac2->p_residue += uac2->p_pktsize_residue;
-
-		/*
-		 * Whenever there are more bytes in the accumulator than we
-		 * need to add one more sample frame, increase this packet's
-		 * size and decrease the accumulator.
-		 */
-		if (uac2->p_residue / uac2->p_interval >= uac2->p_framesize) {
-			req->length += uac2->p_framesize;
-			uac2->p_residue -= uac2->p_framesize *
-					   uac2->p_interval;
-		}
-
-		req->actual = req->length;
-	}
-
-	pending = prm->hw_ptr % prm->period_size;
-	pending += req->actual;
-	if (pending >= prm->period_size)
-		update_alsa = true;
-
-	hw_ptr = prm->hw_ptr;
-	prm->hw_ptr = (prm->hw_ptr + req->actual) % prm->dma_bytes;
-
-	spin_unlock_irqrestore(&prm->lock, flags);
-
-	/* Pack USB load in ALSA ring buffer */
-	pending = prm->dma_bytes - hw_ptr;
-
-	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
-		if (unlikely(pending < req->actual)) {
-			memcpy(req->buf, prm->dma_area + hw_ptr, pending);
-			memcpy(req->buf + pending, prm->dma_area,
-			       req->actual - pending);
-		} else {
-			memcpy(req->buf, prm->dma_area + hw_ptr, req->actual);
-		}
-	} else {
-		if (unlikely(pending < req->actual)) {
-			memcpy(prm->dma_area + hw_ptr, req->buf, pending);
-			memcpy(prm->dma_area, req->buf + pending,
-			       req->actual - pending);
-		} else {
-			memcpy(prm->dma_area + hw_ptr, req->buf, req->actual);
-		}
-	}
-
-exit:
-	if (usb_ep_queue(ep, req, GFP_ATOMIC))
-		dev_err(&uac2->pdev.dev, "%d Error!\n", __LINE__);
-
-	if (update_alsa)
-		snd_pcm_period_elapsed(substream);
-
-	return;
-}
-
-static int
-uac2_pcm_trigger(struct snd_pcm_substream *substream, int cmd)
-{
-	struct snd_uac2_chip *uac2 = snd_pcm_substream_chip(substream);
-	struct audio_dev *agdev = uac2_to_agdev(uac2);
-	struct f_uac2_opts *uac2_opts = agdev_to_uac2_opts(agdev);
-	struct uac2_rtd_params *prm;
-	unsigned long flags;
-	int err = 0;
-
-	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
-		prm = &uac2->p_prm;
-	else
-		prm = &uac2->c_prm;
-
-	spin_lock_irqsave(&prm->lock, flags);
-
-	/* Reset */
-	prm->hw_ptr = 0;
-
-	switch (cmd) {
-	case SNDRV_PCM_TRIGGER_START:
-	case SNDRV_PCM_TRIGGER_RESUME:
-		prm->ss = substream;
-		break;
-	case SNDRV_PCM_TRIGGER_STOP:
-	case SNDRV_PCM_TRIGGER_SUSPEND:
-		prm->ss = NULL;
-		break;
-	default:
-		err = -EINVAL;
-	}
-
-	spin_unlock_irqrestore(&prm->lock, flags);
-
-	/* Clear buffer after Play stops */
-	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK && !prm->ss)
-		memset(prm->rbuf, 0, prm->max_psize * uac2_opts->req_number);
-
-	return err;
-}
-
-static snd_pcm_uframes_t uac2_pcm_pointer(struct snd_pcm_substream *substream)
-{
-	struct snd_uac2_chip *uac2 = snd_pcm_substream_chip(substream);
-	struct uac2_rtd_params *prm;
-
-	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
-		prm = &uac2->p_prm;
-	else
-		prm = &uac2->c_prm;
-
-	return bytes_to_frames(substream->runtime, prm->hw_ptr);
-}
-
-static int uac2_pcm_hw_params(struct snd_pcm_substream *substream,
-			       struct snd_pcm_hw_params *hw_params)
-{
-	struct snd_uac2_chip *uac2 = snd_pcm_substream_chip(substream);
-	struct uac2_rtd_params *prm;
-	int err;
-
-	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
-		prm = &uac2->p_prm;
-	else
-		prm = &uac2->c_prm;
-
-	err = snd_pcm_lib_malloc_pages(substream,
-					params_buffer_bytes(hw_params));
-	if (err >= 0) {
-		prm->dma_bytes = substream->runtime->dma_bytes;
-		prm->dma_area = substream->runtime->dma_area;
-		prm->period_size = params_period_bytes(hw_params);
-	}
-
-	return err;
-}
-
-static int uac2_pcm_hw_free(struct snd_pcm_substream *substream)
-{
-	struct snd_uac2_chip *uac2 = snd_pcm_substream_chip(substream);
-	struct uac2_rtd_params *prm;
-
-	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
-		prm = &uac2->p_prm;
-	else
-		prm = &uac2->c_prm;
-
-	prm->dma_area = NULL;
-	prm->dma_bytes = 0;
-	prm->period_size = 0;
-
-	return snd_pcm_lib_free_pages(substream);
-}
-
-static int uac2_pcm_open(struct snd_pcm_substream *substream)
-{
-	struct snd_uac2_chip *uac2 = snd_pcm_substream_chip(substream);
-	struct snd_pcm_runtime *runtime = substream->runtime;
-	struct audio_dev *audio_dev;
-	struct f_uac2_opts *opts;
-	int p_ssize, c_ssize;
-	int p_srate, c_srate;
-	int p_chmask, c_chmask;
-
-	audio_dev = uac2_to_agdev(uac2);
-	opts = container_of(audio_dev->func.fi, struct f_uac2_opts, func_inst);
-	p_ssize = opts->p_ssize;
-	c_ssize = opts->c_ssize;
-	p_srate = opts->p_srate;
-	c_srate = opts->c_srate;
-	p_chmask = opts->p_chmask;
-	c_chmask = opts->c_chmask;
-	uac2->p_residue = 0;
-
-	runtime->hw = uac2_pcm_hardware;
-
-	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
-		spin_lock_init(&uac2->p_prm.lock);
-		runtime->hw.rate_min = p_srate;
-		switch (p_ssize) {
-		case 3:
-			runtime->hw.formats = SNDRV_PCM_FMTBIT_S24_3LE;
-			break;
-		case 4:
-			runtime->hw.formats = SNDRV_PCM_FMTBIT_S32_LE;
-			break;
-		default:
-			runtime->hw.formats = SNDRV_PCM_FMTBIT_S16_LE;
-			break;
-		}
-		runtime->hw.channels_min = num_channels(p_chmask);
-		runtime->hw.period_bytes_min = 2 * uac2->p_prm.max_psize
-						/ runtime->hw.periods_min;
-	} else {
-		spin_lock_init(&uac2->c_prm.lock);
-		runtime->hw.rate_min = c_srate;
-		switch (c_ssize) {
-		case 3:
-			runtime->hw.formats = SNDRV_PCM_FMTBIT_S24_3LE;
-			break;
-		case 4:
-			runtime->hw.formats = SNDRV_PCM_FMTBIT_S32_LE;
-			break;
-		default:
-			runtime->hw.formats = SNDRV_PCM_FMTBIT_S16_LE;
-			break;
-		}
-		runtime->hw.channels_min = num_channels(c_chmask);
-		runtime->hw.period_bytes_min = 2 * uac2->c_prm.max_psize
-						/ runtime->hw.periods_min;
-	}
-
-	runtime->hw.rate_max = runtime->hw.rate_min;
-	runtime->hw.channels_max = runtime->hw.channels_min;
-
-	snd_pcm_hw_constraint_integer(runtime, SNDRV_PCM_HW_PARAM_PERIODS);
-
-	return 0;
-}
-
-/* ALSA cries without these function pointers */
-static int uac2_pcm_null(struct snd_pcm_substream *substream)
-{
-	return 0;
-}
-
-static struct snd_pcm_ops uac2_pcm_ops = {
-	.open = uac2_pcm_open,
-	.close = uac2_pcm_null,
-	.ioctl = snd_pcm_lib_ioctl,
-	.hw_params = uac2_pcm_hw_params,
-	.hw_free = uac2_pcm_hw_free,
-	.trigger = uac2_pcm_trigger,
-	.pointer = uac2_pcm_pointer,
-	.prepare = uac2_pcm_null,
-};
-
-static int snd_uac2_probe(struct platform_device *pdev)
-{
-	struct snd_uac2_chip *uac2 = pdev_to_uac2(pdev);
-	struct snd_card *card;
-	struct snd_pcm *pcm;
-	struct audio_dev *audio_dev;
-	struct f_uac2_opts *opts;
-	int err;
-	int p_chmask, c_chmask;
-
-	audio_dev = uac2_to_agdev(uac2);
-	opts = container_of(audio_dev->func.fi, struct f_uac2_opts, func_inst);
-	p_chmask = opts->p_chmask;
-	c_chmask = opts->c_chmask;
-
-	/* Choose any slot, with no id */
-	err = snd_card_new(&pdev->dev, -1, NULL, THIS_MODULE, 0, &card);
-	if (err < 0)
-		return err;
-
-	uac2->card = card;
-
-	/*
-	 * Create first PCM device
-	 * Create a substream only for non-zero channel streams
-	 */
-	err = snd_pcm_new(uac2->card, "UAC2 PCM", 0,
-			       p_chmask ? 1 : 0, c_chmask ? 1 : 0, &pcm);
-	if (err < 0)
-		goto snd_fail;
-
-	strcpy(pcm->name, "UAC2 PCM");
-	pcm->private_data = uac2;
-
-	uac2->pcm = pcm;
-
-	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_PLAYBACK, &uac2_pcm_ops);
-	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_CAPTURE, &uac2_pcm_ops);
-
-	strcpy(card->driver, "UAC2_Gadget");
-	strcpy(card->shortname, "UAC2_Gadget");
-	sprintf(card->longname, "UAC2_Gadget %i", pdev->id);
-
-	snd_pcm_lib_preallocate_pages_for_all(pcm, SNDRV_DMA_TYPE_CONTINUOUS,
-		snd_dma_continuous_data(GFP_KERNEL), 0, BUFF_SIZE_MAX);
-
-	err = snd_card_register(card);
-	if (!err) {
-		platform_set_drvdata(pdev, card);
-		return 0;
-	}
-
-snd_fail:
-	snd_card_free(card);
-
-	uac2->pcm = NULL;
-	uac2->card = NULL;
-
-	return err;
-}
-
-static int snd_uac2_remove(struct platform_device *pdev)
-{
-	struct snd_card *card = platform_get_drvdata(pdev);
-
-	if (card)
-		return snd_card_free(card);
-
-	return 0;
-}
-
-static void snd_uac2_release(struct device *dev)
-{
-	dev_dbg(dev, "releasing '%s'\n", dev_name(dev));
-}
-
-static int alsa_uac2_init(struct audio_dev *agdev)
-{
-	struct snd_uac2_chip *uac2 = &agdev->uac2;
-	int err;
-
-	uac2->pdrv.probe = snd_uac2_probe;
-	uac2->pdrv.remove = snd_uac2_remove;
-	uac2->pdrv.driver.name = uac2_name;
-
-	uac2->pdev.id = 0;
-	uac2->pdev.name = uac2_name;
-	uac2->pdev.dev.release = snd_uac2_release;
-
-	/* Register snd_uac2 driver */
-	err = platform_driver_register(&uac2->pdrv);
-	if (err)
-		return err;
-
-	/* Register snd_uac2 device */
-	err = platform_device_register(&uac2->pdev);
-	if (err)
-		platform_driver_unregister(&uac2->pdrv);
-
-	return err;
-}
-
-static void alsa_uac2_exit(struct audio_dev *agdev)
-{
-	struct snd_uac2_chip *uac2 = &agdev->uac2;
-
-	platform_driver_unregister(&uac2->pdrv);
-	platform_device_unregister(&uac2->pdev);
-}
-
-
 /* --------- USB Function Interface ------------- */
 
 enum {
@@ -938,32 +451,6 @@ struct cntrl_range_lay3 {
 	__u32	dRES;
 } __packed;
 
-static inline void
-free_ep(struct uac2_rtd_params *prm, struct usb_ep *ep)
-{
-	struct snd_uac2_chip *uac2 = prm->uac2;
-	struct audio_dev *agdev = uac2_to_agdev(uac2);
-	struct f_uac2_opts *uac2_opts = agdev_to_uac2_opts(agdev);
-	int i;
-
-	if (!prm->ep_enabled)
-		return;
-
-	prm->ep_enabled = false;
-
-	for (i = 0; i < uac2_opts->req_number; i++) {
-		if (prm->ureq[i].req) {
-			usb_ep_dequeue(ep, prm->ureq[i].req);
-			usb_ep_free_request(ep, prm->ureq[i].req);
-			prm->ureq[i].req = NULL;
-		}
-	}
-
-	if (usb_ep_disable(ep))
-		dev_err(&uac2->pdev.dev,
-			"%s:%d Error!\n", __func__, __LINE__);
-}
-
 static void set_ep_max_packet_size(const struct f_uac2_opts *uac2_opts,
 	struct usb_endpoint_descriptor *ep_desc,
 	unsigned int factor, bool is_playback)
@@ -990,12 +477,11 @@ static void set_ep_max_packet_size(const struct f_uac2_opts *uac2_opts,
 static int
 afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 {
-	struct audio_dev *agdev = func_to_agdev(fn);
-	struct snd_uac2_chip *uac2 = &agdev->uac2;
+	struct f_uac2 *uac2 = func_to_uac2(fn);
+	struct g_audio *agdev = func_to_g_audio(fn);
 	struct usb_composite_dev *cdev = cfg->cdev;
 	struct usb_gadget *gadget = cdev->gadget;
-	struct device *dev = &uac2->pdev.dev;
-	struct uac2_rtd_params *prm;
+	struct device *dev = &gadget->dev;
 	struct f_uac2_opts *uac2_opts;
 	struct usb_string *us;
 	int ret;
@@ -1042,8 +528,8 @@ afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 		return ret;
 	}
 	std_ac_if_desc.bInterfaceNumber = ret;
-	agdev->ac_intf = ret;
-	agdev->ac_alt = 0;
+	uac2->ac_intf = ret;
+	uac2->ac_alt = 0;
 
 	ret = usb_interface_id(cfg, fn);
 	if (ret < 0) {
@@ -1052,8 +538,8 @@ afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 	}
 	std_as_out_if0_desc.bInterfaceNumber = ret;
 	std_as_out_if1_desc.bInterfaceNumber = ret;
-	agdev->as_out_intf = ret;
-	agdev->as_out_alt = 0;
+	uac2->as_out_intf = ret;
+	uac2->as_out_alt = 0;
 
 	ret = usb_interface_id(cfg, fn);
 	if (ret < 0) {
@@ -1062,8 +548,14 @@ afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 	}
 	std_as_in_if0_desc.bInterfaceNumber = ret;
 	std_as_in_if1_desc.bInterfaceNumber = ret;
-	agdev->as_in_intf = ret;
-	agdev->as_in_alt = 0;
+	uac2->as_in_intf = ret;
+	uac2->as_in_alt = 0;
+
+	/* Calculate wMaxPacketSize according to audio bandwidth */
+	set_ep_max_packet_size(uac2_opts, &fs_epin_desc, 1000, true);
+	set_ep_max_packet_size(uac2_opts, &fs_epout_desc, 1000, false);
+	set_ep_max_packet_size(uac2_opts, &hs_epin_desc, 8000, true);
+	set_ep_max_packet_size(uac2_opts, &hs_epout_desc, 8000, false);
 
 	agdev->out_ep = usb_ep_autoconfig(gadget, &fs_epout_desc);
 	if (!agdev->out_ep) {
@@ -1077,14 +569,10 @@ afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 		return ret;
 	}
 
-	uac2->p_prm.uac2 = uac2;
-	uac2->c_prm.uac2 = uac2;
-
-	/* Calculate wMaxPacketSize according to audio bandwidth */
-	set_ep_max_packet_size(uac2_opts, &fs_epin_desc, 1000, true);
-	set_ep_max_packet_size(uac2_opts, &fs_epout_desc, 1000, false);
-	set_ep_max_packet_size(uac2_opts, &hs_epin_desc, 8000, true);
-	set_ep_max_packet_size(uac2_opts, &hs_epout_desc, 8000, false);
+	agdev->in_ep_maxpsize = max(fs_epin_desc.wMaxPacketSize,
+					hs_epin_desc.wMaxPacketSize);
+	agdev->out_ep_maxpsize = max(fs_epout_desc.wMaxPacketSize,
+					hs_epout_desc.wMaxPacketSize);
 
 	hs_epout_desc.bEndpointAddress = fs_epout_desc.bEndpointAddress;
 	hs_epin_desc.bEndpointAddress = fs_epin_desc.bEndpointAddress;
@@ -1094,48 +582,23 @@ afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 	if (ret)
 		return ret;
 
-	prm = &agdev->uac2.c_prm;
-	prm->max_psize = hs_epout_desc.wMaxPacketSize;
-	prm->ureq = kcalloc(uac2_opts->req_number, sizeof(struct uac2_req),
-			GFP_KERNEL);
-	if (!prm->ureq) {
-		ret = -ENOMEM;
-		goto err_free_descs;
-	}
-	prm->rbuf = kcalloc(uac2_opts->req_number, prm->max_psize, GFP_KERNEL);
-	if (!prm->rbuf) {
-		prm->max_psize = 0;
-		ret = -ENOMEM;
-		goto err_free_descs;
-	}
-
-	prm = &agdev->uac2.p_prm;
-	prm->max_psize = hs_epin_desc.wMaxPacketSize;
-	prm->ureq = kcalloc(uac2_opts->req_number, sizeof(struct uac2_req),
-			GFP_KERNEL);
-	if (!prm->ureq) {
-		ret = -ENOMEM;
-		goto err_free_descs;
-	}
-	prm->rbuf = kcalloc(uac2_opts->req_number, prm->max_psize, GFP_KERNEL);
-	if (!prm->rbuf) {
-		prm->max_psize = 0;
-		ret = -ENOMEM;
-		goto err_no_memory;
-	}
+	agdev->gadget = gadget;
 
-	ret = alsa_uac2_init(agdev);
+	agdev->params.p_chmask = uac2_opts->p_chmask;
+	agdev->params.p_srate = uac2_opts->p_srate;
+	agdev->params.p_ssize = uac2_opts->p_ssize;
+	agdev->params.c_chmask = uac2_opts->c_chmask;
+	agdev->params.c_srate = uac2_opts->c_srate;
+	agdev->params.c_ssize = uac2_opts->c_ssize;
+	agdev->params.req_number = uac2_opts->req_number;
+	ret = g_audio_setup(agdev, "UAC2 PCM", "UAC2_Gadget");
 	if (ret)
-		goto err_no_memory;
+		goto err_free_descs;
 	return 0;
 
-err_no_memory:
-	kfree(agdev->uac2.p_prm.ureq);
-	kfree(agdev->uac2.c_prm.ureq);
-	kfree(agdev->uac2.p_prm.rbuf);
-	kfree(agdev->uac2.c_prm.rbuf);
 err_free_descs:
 	usb_free_all_descriptors(fn);
+	agdev->gadget = NULL;
 	return ret;
 }
 
@@ -1143,15 +606,10 @@ static int
 afunc_set_alt(struct usb_function *fn, unsigned intf, unsigned alt)
 {
 	struct usb_composite_dev *cdev = fn->config->cdev;
-	struct audio_dev *agdev = func_to_agdev(fn);
-	struct f_uac2_opts *opts = agdev_to_uac2_opts(agdev);
-	struct snd_uac2_chip *uac2 = &agdev->uac2;
+	struct f_uac2 *uac2 = func_to_uac2(fn);
 	struct usb_gadget *gadget = cdev->gadget;
-	struct device *dev = &uac2->pdev.dev;
-	struct usb_request *req;
-	struct usb_ep *ep;
-	struct uac2_rtd_params *prm;
-	int req_len, i;
+	struct device *dev = &gadget->dev;
+	int ret = 0;
 
 	/* No i/f has more than 2 alt settings */
 	if (alt > 1) {
@@ -1159,7 +617,7 @@ afunc_set_alt(struct usb_function *fn, unsigned intf, unsigned alt)
 		return -EINVAL;
 	}
 
-	if (intf == agdev->ac_intf) {
+	if (intf == uac2->ac_intf) {
 		/* Control I/f has only 1 AltSetting - 0 */
 		if (alt) {
 			dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
@@ -1168,95 +626,42 @@ afunc_set_alt(struct usb_function *fn, unsigned intf, unsigned alt)
 		return 0;
 	}
 
-	if (intf == agdev->as_out_intf) {
-		ep = agdev->out_ep;
-		prm = &uac2->c_prm;
-		config_ep_by_speed(gadget, fn, ep);
-		agdev->as_out_alt = alt;
-		req_len = prm->max_psize;
-	} else if (intf == agdev->as_in_intf) {
-		unsigned int factor, rate;
-		struct usb_endpoint_descriptor *ep_desc;
-
-		ep = agdev->in_ep;
-		prm = &uac2->p_prm;
-		config_ep_by_speed(gadget, fn, ep);
-		agdev->as_in_alt = alt;
-
-		/* pre-calculate the playback endpoint's interval */
-		if (gadget->speed == USB_SPEED_FULL) {
-			ep_desc = &fs_epin_desc;
-			factor = 1000;
-		} else {
-			ep_desc = &hs_epin_desc;
-			factor = 8000;
-		}
-
-		/* pre-compute some values for iso_complete() */
-		uac2->p_framesize = opts->p_ssize *
-				    num_channels(opts->p_chmask);
-		rate = opts->p_srate * uac2->p_framesize;
-		uac2->p_interval = factor / (1 << (ep_desc->bInterval - 1));
-		uac2->p_pktsize = min_t(unsigned int, rate / uac2->p_interval,
-					prm->max_psize);
+	if (intf == uac2->as_out_intf) {
+		uac2->as_out_alt = alt;
 
-		if (uac2->p_pktsize < prm->max_psize)
-			uac2->p_pktsize_residue = rate % uac2->p_interval;
+		if (alt)
+			ret = u_audio_start_capture(&uac2->g_audio);
 		else
-			uac2->p_pktsize_residue = 0;
+			u_audio_stop_capture(&uac2->g_audio);
+	} else if (intf == uac2->as_in_intf) {
+		uac2->as_in_alt = alt;
 
-		req_len = uac2->p_pktsize;
-		uac2->p_residue = 0;
+		if (alt)
+			ret = u_audio_start_playback(&uac2->g_audio);
+		else
+			u_audio_stop_playback(&uac2->g_audio);
 	} else {
 		dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
 		return -EINVAL;
 	}
 
-	if (alt == 0) {
-		free_ep(prm, ep);
-		return 0;
-	}
-
-	prm->ep_enabled = true;
-	usb_ep_enable(ep);
-
-	for (i = 0; i < opts->req_number; i++) {
-		if (!prm->ureq[i].req) {
-			req = usb_ep_alloc_request(ep, GFP_ATOMIC);
-			if (req == NULL)
-				return -ENOMEM;
-
-			prm->ureq[i].req = req;
-			prm->ureq[i].pp = prm;
-
-			req->zero = 0;
-			req->context = &prm->ureq[i];
-			req->length = req_len;
-			req->complete = agdev_iso_complete;
-			req->buf = prm->rbuf + i * prm->max_psize;
-		}
-
-		if (usb_ep_queue(ep, prm->ureq[i].req, GFP_ATOMIC))
-			dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
-	}
-
-	return 0;
+	return ret;
 }
 
 static int
 afunc_get_alt(struct usb_function *fn, unsigned intf)
 {
-	struct audio_dev *agdev = func_to_agdev(fn);
-	struct snd_uac2_chip *uac2 = &agdev->uac2;
-
-	if (intf == agdev->ac_intf)
-		return agdev->ac_alt;
-	else if (intf == agdev->as_out_intf)
-		return agdev->as_out_alt;
-	else if (intf == agdev->as_in_intf)
-		return agdev->as_in_alt;
+	struct f_uac2 *uac2 = func_to_uac2(fn);
+	struct g_audio *agdev = func_to_g_audio(fn);
+
+	if (intf == uac2->ac_intf)
+		return uac2->ac_alt;
+	else if (intf == uac2->as_out_intf)
+		return uac2->as_out_alt;
+	else if (intf == uac2->as_in_intf)
+		return uac2->as_in_alt;
 	else
-		dev_err(&uac2->pdev.dev,
+		dev_err(&agdev->gadget->dev,
 			"%s:%d Invalid Interface %d!\n",
 			__func__, __LINE__, intf);
 
@@ -1266,22 +671,19 @@ afunc_get_alt(struct usb_function *fn, unsigned intf)
 static void
 afunc_disable(struct usb_function *fn)
 {
-	struct audio_dev *agdev = func_to_agdev(fn);
-	struct snd_uac2_chip *uac2 = &agdev->uac2;
-
-	free_ep(&uac2->p_prm, agdev->in_ep);
-	agdev->as_in_alt = 0;
+	struct f_uac2 *uac2 = func_to_uac2(fn);
 
-	free_ep(&uac2->c_prm, agdev->out_ep);
-	agdev->as_out_alt = 0;
+	uac2->as_in_alt = 0;
+	uac2->as_out_alt = 0;
+	u_audio_stop_capture(&uac2->g_audio);
+	u_audio_stop_playback(&uac2->g_audio);
 }
 
 static int
 in_rq_cur(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 {
 	struct usb_request *req = fn->config->cdev->req;
-	struct audio_dev *agdev = func_to_agdev(fn);
-	struct snd_uac2_chip *uac2 = &agdev->uac2;
+	struct g_audio *agdev = func_to_g_audio(fn);
 	struct f_uac2_opts *opts;
 	u16 w_length = le16_to_cpu(cr->wLength);
 	u16 w_index = le16_to_cpu(cr->wIndex);
@@ -1291,7 +693,7 @@ in_rq_cur(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 	int value = -EOPNOTSUPP;
 	int p_srate, c_srate;
 
-	opts = agdev_to_uac2_opts(agdev);
+	opts = g_audio_to_uac2_opts(agdev);
 	p_srate = opts->p_srate;
 	c_srate = opts->c_srate;
 
@@ -1310,7 +712,7 @@ in_rq_cur(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 		*(u8 *)req->buf = 1;
 		value = min_t(unsigned, w_length, 1);
 	} else {
-		dev_err(&uac2->pdev.dev,
+		dev_err(&agdev->gadget->dev,
 			"%s:%d control_selector=%d TODO!\n",
 			__func__, __LINE__, control_selector);
 	}
@@ -1322,8 +724,7 @@ static int
 in_rq_range(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 {
 	struct usb_request *req = fn->config->cdev->req;
-	struct audio_dev *agdev = func_to_agdev(fn);
-	struct snd_uac2_chip *uac2 = &agdev->uac2;
+	struct g_audio *agdev = func_to_g_audio(fn);
 	struct f_uac2_opts *opts;
 	u16 w_length = le16_to_cpu(cr->wLength);
 	u16 w_index = le16_to_cpu(cr->wIndex);
@@ -1334,7 +735,7 @@ in_rq_range(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 	int value = -EOPNOTSUPP;
 	int p_srate, c_srate;
 
-	opts = agdev_to_uac2_opts(agdev);
+	opts = g_audio_to_uac2_opts(agdev);
 	p_srate = opts->p_srate;
 	c_srate = opts->c_srate;
 
@@ -1353,7 +754,7 @@ in_rq_range(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 		value = min_t(unsigned, w_length, sizeof r);
 		memcpy(req->buf, &r, value);
 	} else {
-		dev_err(&uac2->pdev.dev,
+		dev_err(&agdev->gadget->dev,
 			"%s:%d control_selector=%d TODO!\n",
 			__func__, __LINE__, control_selector);
 	}
@@ -1388,13 +789,13 @@ out_rq_cur(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 static int
 setup_rq_inf(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 {
-	struct audio_dev *agdev = func_to_agdev(fn);
-	struct snd_uac2_chip *uac2 = &agdev->uac2;
+	struct f_uac2 *uac2 = func_to_uac2(fn);
+	struct g_audio *agdev = func_to_g_audio(fn);
 	u16 w_index = le16_to_cpu(cr->wIndex);
 	u8 intf = w_index & 0xff;
 
-	if (intf != agdev->ac_intf) {
-		dev_err(&uac2->pdev.dev,
+	if (intf != uac2->ac_intf) {
+		dev_err(&agdev->gadget->dev,
 			"%s:%d Error!\n", __func__, __LINE__);
 		return -EOPNOTSUPP;
 	}
@@ -1411,8 +812,7 @@ static int
 afunc_setup(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 {
 	struct usb_composite_dev *cdev = fn->config->cdev;
-	struct audio_dev *agdev = func_to_agdev(fn);
-	struct snd_uac2_chip *uac2 = &agdev->uac2;
+	struct g_audio *agdev = func_to_g_audio(fn);
 	struct usb_request *req = cdev->req;
 	u16 w_length = le16_to_cpu(cr->wLength);
 	int value = -EOPNOTSUPP;
@@ -1424,14 +824,15 @@ afunc_setup(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 	if ((cr->bRequestType & USB_RECIP_MASK) == USB_RECIP_INTERFACE)
 		value = setup_rq_inf(fn, cr);
 	else
-		dev_err(&uac2->pdev.dev, "%s:%d Error!\n", __func__, __LINE__);
+		dev_err(&agdev->gadget->dev, "%s:%d Error!\n",
+				__func__, __LINE__);
 
 	if (value >= 0) {
 		req->length = value;
 		req->zero = value < w_length;
 		value = usb_ep_queue(cdev->gadget->ep0, req, GFP_ATOMIC);
 		if (value < 0) {
-			dev_err(&uac2->pdev.dev,
+			dev_err(&agdev->gadget->dev,
 				"%s:%d Error!\n", __func__, __LINE__);
 			req->status = 0;
 		}
@@ -1557,10 +958,10 @@ static struct usb_function_instance *afunc_alloc_inst(void)
 
 static void afunc_free(struct usb_function *f)
 {
-	struct audio_dev *agdev;
+	struct g_audio *agdev;
 	struct f_uac2_opts *opts;
 
-	agdev = func_to_agdev(f);
+	agdev = func_to_g_audio(f);
 	opts = container_of(f->fi, struct f_uac2_opts, func_inst);
 	kfree(agdev);
 	mutex_lock(&opts->lock);
@@ -1570,27 +971,21 @@ static void afunc_free(struct usb_function *f)
 
 static void afunc_unbind(struct usb_configuration *c, struct usb_function *f)
 {
-	struct audio_dev *agdev = func_to_agdev(f);
-	struct uac2_rtd_params *prm;
+	struct g_audio *agdev = func_to_g_audio(f);
 
-	alsa_uac2_exit(agdev);
-
-	prm = &agdev->uac2.p_prm;
-	kfree(prm->rbuf);
-
-	prm = &agdev->uac2.c_prm;
-	kfree(prm->rbuf);
-	kfree(prm->ureq);
+	g_audio_cleanup(agdev);
 	usb_free_all_descriptors(f);
+
+	agdev->gadget = NULL;
 }
 
 static struct usb_function *afunc_alloc(struct usb_function_instance *fi)
 {
-	struct audio_dev *agdev;
+	struct f_uac2	*uac2;
 	struct f_uac2_opts *opts;
 
-	agdev = kzalloc(sizeof(*agdev), GFP_KERNEL);
-	if (agdev == NULL)
+	uac2 = kzalloc(sizeof(*uac2), GFP_KERNEL);
+	if (uac2 == NULL)
 		return ERR_PTR(-ENOMEM);
 
 	opts = container_of(fi, struct f_uac2_opts, func_inst);
@@ -1598,16 +993,16 @@ static struct usb_function *afunc_alloc(struct usb_function_instance *fi)
 	++opts->refcnt;
 	mutex_unlock(&opts->lock);
 
-	agdev->func.name = "uac2_func";
-	agdev->func.bind = afunc_bind;
-	agdev->func.unbind = afunc_unbind;
-	agdev->func.set_alt = afunc_set_alt;
-	agdev->func.get_alt = afunc_get_alt;
-	agdev->func.disable = afunc_disable;
-	agdev->func.setup = afunc_setup;
-	agdev->func.free_func = afunc_free;
+	uac2->g_audio.func.name = "uac2_func";
+	uac2->g_audio.func.bind = afunc_bind;
+	uac2->g_audio.func.unbind = afunc_unbind;
+	uac2->g_audio.func.set_alt = afunc_set_alt;
+	uac2->g_audio.func.get_alt = afunc_get_alt;
+	uac2->g_audio.func.disable = afunc_disable;
+	uac2->g_audio.func.setup = afunc_setup;
+	uac2->g_audio.func.free_func = afunc_free;
 
-	return &agdev->func;
+	return &uac2->g_audio.func;
 }
 
 DECLARE_USB_FUNCTION_INIT(uac2, afunc_alloc_inst, afunc_alloc);
diff --git a/drivers/usb/gadget/function/rndis.c b/drivers/usb/gadget/function/rndis.c
index a3b5e468b116..d6341045c631 100644
--- a/drivers/usb/gadget/function/rndis.c
+++ b/drivers/usb/gadget/function/rndis.c
@@ -999,7 +999,7 @@ void rndis_add_hdr(struct sk_buff *skb)
 
 	if (!skb)
 		return;
-	header = (void *)skb_push(skb, sizeof(*header));
+	header = skb_push(skb, sizeof(*header));
 	memset(header, 0, sizeof *header);
 	header->MessageType = cpu_to_le32(RNDIS_MSG_PACKET);
 	header->MessageLength = cpu_to_le32(skb->len);
diff --git a/drivers/usb/gadget/function/storage_common.h b/drivers/usb/gadget/function/storage_common.h
index e69848994cb4..e0814a960132 100644
--- a/drivers/usb/gadget/function/storage_common.h
+++ b/drivers/usb/gadget/function/storage_common.h
@@ -133,9 +133,10 @@ static inline bool fsg_lun_is_open(struct fsg_lun *curlun)
 #define FSG_MAX_LUNS	16
 
 enum fsg_buffer_state {
+	BUF_STATE_SENDING = -2,
+	BUF_STATE_RECEIVING,
 	BUF_STATE_EMPTY = 0,
-	BUF_STATE_FULL,
-	BUF_STATE_BUSY
+	BUF_STATE_FULL
 };
 
 struct fsg_buffhd {
@@ -151,23 +152,14 @@ struct fsg_buffhd {
 	unsigned int			bulk_out_intended_length;
 
 	struct usb_request		*inreq;
-	int				inreq_busy;
 	struct usb_request		*outreq;
-	int				outreq_busy;
 };
 
 enum fsg_state {
-	/* This one isn't used anywhere */
-	FSG_STATE_COMMAND_PHASE = -10,
-	FSG_STATE_DATA_PHASE,
-	FSG_STATE_STATUS_PHASE,
-
-	FSG_STATE_IDLE = 0,
+	FSG_STATE_NORMAL,
 	FSG_STATE_ABORT_BULK_OUT,
-	FSG_STATE_RESET,
-	FSG_STATE_INTERFACE_CHANGE,
+	FSG_STATE_PROTOCOL_RESET,
 	FSG_STATE_CONFIG_CHANGE,
-	FSG_STATE_DISCONNECT,
 	FSG_STATE_EXIT,
 	FSG_STATE_TERMINATED
 };
diff --git a/drivers/usb/gadget/function/u_audio.c b/drivers/usb/gadget/function/u_audio.c
new file mode 100644
index 000000000000..5dd73b9e5172
--- /dev/null
+++ b/drivers/usb/gadget/function/u_audio.c
@@ -0,0 +1,662 @@
+/*
+ * u_audio.c -- interface to USB gadget "ALSA sound card" utilities
+ *
+ * Copyright (C) 2016
+ * Author: Ruslan Bilovol <ruslan.bilovol@gmail.com>
+ *
+ * Sound card implementation was cut-and-pasted with changes
+ * from f_uac2.c and has:
+ *    Copyright (C) 2011
+ *    Yadwinder Singh (yadi.brar01@gmail.com)
+ *    Jaswinder Singh (jaswinder.singh@linaro.org)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+
+#include "u_audio.h"
+
+#define BUFF_SIZE_MAX	(PAGE_SIZE * 16)
+#define PRD_SIZE_MAX	PAGE_SIZE
+#define MIN_PERIODS	4
+
+struct uac_req {
+	struct uac_rtd_params *pp; /* parent param */
+	struct usb_request *req;
+};
+
+/* Runtime data params for one stream */
+struct uac_rtd_params {
+	struct snd_uac_chip *uac; /* parent chip */
+	bool ep_enabled; /* if the ep is enabled */
+	/* Size of the ring buffer */
+	size_t dma_bytes;
+	unsigned char *dma_area;
+
+	struct snd_pcm_substream *ss;
+
+	/* Ring buffer */
+	ssize_t hw_ptr;
+
+	void *rbuf;
+
+	size_t period_size;
+
+	unsigned max_psize;	/* MaxPacketSize of endpoint */
+	struct uac_req *ureq;
+
+	spinlock_t lock;
+};
+
+struct snd_uac_chip {
+	struct g_audio *audio_dev;
+
+	struct uac_rtd_params p_prm;
+	struct uac_rtd_params c_prm;
+
+	struct snd_card *card;
+	struct snd_pcm *pcm;
+
+	/* timekeeping for the playback endpoint */
+	unsigned int p_interval;
+	unsigned int p_residue;
+
+	/* pre-calculated values for playback iso completion */
+	unsigned int p_pktsize;
+	unsigned int p_pktsize_residue;
+	unsigned int p_framesize;
+};
+
+static struct snd_pcm_hardware uac_pcm_hardware = {
+	.info = SNDRV_PCM_INFO_INTERLEAVED | SNDRV_PCM_INFO_BLOCK_TRANSFER
+		 | SNDRV_PCM_INFO_MMAP | SNDRV_PCM_INFO_MMAP_VALID
+		 | SNDRV_PCM_INFO_PAUSE | SNDRV_PCM_INFO_RESUME,
+	.rates = SNDRV_PCM_RATE_CONTINUOUS,
+	.periods_max = BUFF_SIZE_MAX / PRD_SIZE_MAX,
+	.buffer_bytes_max = BUFF_SIZE_MAX,
+	.period_bytes_max = PRD_SIZE_MAX,
+	.periods_min = MIN_PERIODS,
+};
+
+static void u_audio_iso_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	unsigned pending;
+	unsigned long flags;
+	unsigned int hw_ptr;
+	bool update_alsa = false;
+	int status = req->status;
+	struct uac_req *ur = req->context;
+	struct snd_pcm_substream *substream;
+	struct uac_rtd_params *prm = ur->pp;
+	struct snd_uac_chip *uac = prm->uac;
+
+	/* i/f shutting down */
+	if (!prm->ep_enabled || req->status == -ESHUTDOWN)
+		return;
+
+	/*
+	 * We can't really do much about bad xfers.
+	 * Afterall, the ISOCH xfers could fail legitimately.
+	 */
+	if (status)
+		pr_debug("%s: iso_complete status(%d) %d/%d\n",
+			__func__, status, req->actual, req->length);
+
+	substream = prm->ss;
+
+	/* Do nothing if ALSA isn't active */
+	if (!substream)
+		goto exit;
+
+	spin_lock_irqsave(&prm->lock, flags);
+
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
+		/*
+		 * For each IN packet, take the quotient of the current data
+		 * rate and the endpoint's interval as the base packet size.
+		 * If there is a residue from this division, add it to the
+		 * residue accumulator.
+		 */
+		req->length = uac->p_pktsize;
+		uac->p_residue += uac->p_pktsize_residue;
+
+		/*
+		 * Whenever there are more bytes in the accumulator than we
+		 * need to add one more sample frame, increase this packet's
+		 * size and decrease the accumulator.
+		 */
+		if (uac->p_residue / uac->p_interval >= uac->p_framesize) {
+			req->length += uac->p_framesize;
+			uac->p_residue -= uac->p_framesize *
+					   uac->p_interval;
+		}
+
+		req->actual = req->length;
+	}
+
+	pending = prm->hw_ptr % prm->period_size;
+	pending += req->actual;
+	if (pending >= prm->period_size)
+		update_alsa = true;
+
+	hw_ptr = prm->hw_ptr;
+	prm->hw_ptr = (prm->hw_ptr + req->actual) % prm->dma_bytes;
+
+	spin_unlock_irqrestore(&prm->lock, flags);
+
+	/* Pack USB load in ALSA ring buffer */
+	pending = prm->dma_bytes - hw_ptr;
+
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
+		if (unlikely(pending < req->actual)) {
+			memcpy(req->buf, prm->dma_area + hw_ptr, pending);
+			memcpy(req->buf + pending, prm->dma_area,
+			       req->actual - pending);
+		} else {
+			memcpy(req->buf, prm->dma_area + hw_ptr, req->actual);
+		}
+	} else {
+		if (unlikely(pending < req->actual)) {
+			memcpy(prm->dma_area + hw_ptr, req->buf, pending);
+			memcpy(prm->dma_area, req->buf + pending,
+			       req->actual - pending);
+		} else {
+			memcpy(prm->dma_area + hw_ptr, req->buf, req->actual);
+		}
+	}
+
+exit:
+	if (usb_ep_queue(ep, req, GFP_ATOMIC))
+		dev_err(uac->card->dev, "%d Error!\n", __LINE__);
+
+	if (update_alsa)
+		snd_pcm_period_elapsed(substream);
+}
+
+static int uac_pcm_trigger(struct snd_pcm_substream *substream, int cmd)
+{
+	struct snd_uac_chip *uac = snd_pcm_substream_chip(substream);
+	struct uac_rtd_params *prm;
+	struct g_audio *audio_dev;
+	struct uac_params *params;
+	unsigned long flags;
+	int err = 0;
+
+	audio_dev = uac->audio_dev;
+	params = &audio_dev->params;
+
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
+		prm = &uac->p_prm;
+	else
+		prm = &uac->c_prm;
+
+	spin_lock_irqsave(&prm->lock, flags);
+
+	/* Reset */
+	prm->hw_ptr = 0;
+
+	switch (cmd) {
+	case SNDRV_PCM_TRIGGER_START:
+	case SNDRV_PCM_TRIGGER_RESUME:
+		prm->ss = substream;
+		break;
+	case SNDRV_PCM_TRIGGER_STOP:
+	case SNDRV_PCM_TRIGGER_SUSPEND:
+		prm->ss = NULL;
+		break;
+	default:
+		err = -EINVAL;
+	}
+
+	spin_unlock_irqrestore(&prm->lock, flags);
+
+	/* Clear buffer after Play stops */
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK && !prm->ss)
+		memset(prm->rbuf, 0, prm->max_psize * params->req_number);
+
+	return err;
+}
+
+static snd_pcm_uframes_t uac_pcm_pointer(struct snd_pcm_substream *substream)
+{
+	struct snd_uac_chip *uac = snd_pcm_substream_chip(substream);
+	struct uac_rtd_params *prm;
+
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
+		prm = &uac->p_prm;
+	else
+		prm = &uac->c_prm;
+
+	return bytes_to_frames(substream->runtime, prm->hw_ptr);
+}
+
+static int uac_pcm_hw_params(struct snd_pcm_substream *substream,
+			       struct snd_pcm_hw_params *hw_params)
+{
+	struct snd_uac_chip *uac = snd_pcm_substream_chip(substream);
+	struct uac_rtd_params *prm;
+	int err;
+
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
+		prm = &uac->p_prm;
+	else
+		prm = &uac->c_prm;
+
+	err = snd_pcm_lib_malloc_pages(substream,
+					params_buffer_bytes(hw_params));
+	if (err >= 0) {
+		prm->dma_bytes = substream->runtime->dma_bytes;
+		prm->dma_area = substream->runtime->dma_area;
+		prm->period_size = params_period_bytes(hw_params);
+	}
+
+	return err;
+}
+
+static int uac_pcm_hw_free(struct snd_pcm_substream *substream)
+{
+	struct snd_uac_chip *uac = snd_pcm_substream_chip(substream);
+	struct uac_rtd_params *prm;
+
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
+		prm = &uac->p_prm;
+	else
+		prm = &uac->c_prm;
+
+	prm->dma_area = NULL;
+	prm->dma_bytes = 0;
+	prm->period_size = 0;
+
+	return snd_pcm_lib_free_pages(substream);
+}
+
+static int uac_pcm_open(struct snd_pcm_substream *substream)
+{
+	struct snd_uac_chip *uac = snd_pcm_substream_chip(substream);
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct g_audio *audio_dev;
+	struct uac_params *params;
+	int p_ssize, c_ssize;
+	int p_srate, c_srate;
+	int p_chmask, c_chmask;
+
+	audio_dev = uac->audio_dev;
+	params = &audio_dev->params;
+	p_ssize = params->p_ssize;
+	c_ssize = params->c_ssize;
+	p_srate = params->p_srate;
+	c_srate = params->c_srate;
+	p_chmask = params->p_chmask;
+	c_chmask = params->c_chmask;
+	uac->p_residue = 0;
+
+	runtime->hw = uac_pcm_hardware;
+
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
+		spin_lock_init(&uac->p_prm.lock);
+		runtime->hw.rate_min = p_srate;
+		switch (p_ssize) {
+		case 3:
+			runtime->hw.formats = SNDRV_PCM_FMTBIT_S24_3LE;
+			break;
+		case 4:
+			runtime->hw.formats = SNDRV_PCM_FMTBIT_S32_LE;
+			break;
+		default:
+			runtime->hw.formats = SNDRV_PCM_FMTBIT_S16_LE;
+			break;
+		}
+		runtime->hw.channels_min = num_channels(p_chmask);
+		runtime->hw.period_bytes_min = 2 * uac->p_prm.max_psize
+						/ runtime->hw.periods_min;
+	} else {
+		spin_lock_init(&uac->c_prm.lock);
+		runtime->hw.rate_min = c_srate;
+		switch (c_ssize) {
+		case 3:
+			runtime->hw.formats = SNDRV_PCM_FMTBIT_S24_3LE;
+			break;
+		case 4:
+			runtime->hw.formats = SNDRV_PCM_FMTBIT_S32_LE;
+			break;
+		default:
+			runtime->hw.formats = SNDRV_PCM_FMTBIT_S16_LE;
+			break;
+		}
+		runtime->hw.channels_min = num_channels(c_chmask);
+		runtime->hw.period_bytes_min = 2 * uac->c_prm.max_psize
+						/ runtime->hw.periods_min;
+	}
+
+	runtime->hw.rate_max = runtime->hw.rate_min;
+	runtime->hw.channels_max = runtime->hw.channels_min;
+
+	snd_pcm_hw_constraint_integer(runtime, SNDRV_PCM_HW_PARAM_PERIODS);
+
+	return 0;
+}
+
+/* ALSA cries without these function pointers */
+static int uac_pcm_null(struct snd_pcm_substream *substream)
+{
+	return 0;
+}
+
+static struct snd_pcm_ops uac_pcm_ops = {
+	.open = uac_pcm_open,
+	.close = uac_pcm_null,
+	.ioctl = snd_pcm_lib_ioctl,
+	.hw_params = uac_pcm_hw_params,
+	.hw_free = uac_pcm_hw_free,
+	.trigger = uac_pcm_trigger,
+	.pointer = uac_pcm_pointer,
+	.prepare = uac_pcm_null,
+};
+
+static inline void free_ep(struct uac_rtd_params *prm, struct usb_ep *ep)
+{
+	struct snd_uac_chip *uac = prm->uac;
+	struct g_audio *audio_dev;
+	struct uac_params *params;
+	int i;
+
+	if (!prm->ep_enabled)
+		return;
+
+	prm->ep_enabled = false;
+
+	audio_dev = uac->audio_dev;
+	params = &audio_dev->params;
+
+	for (i = 0; i < params->req_number; i++) {
+		if (prm->ureq[i].req) {
+			usb_ep_dequeue(ep, prm->ureq[i].req);
+			usb_ep_free_request(ep, prm->ureq[i].req);
+			prm->ureq[i].req = NULL;
+		}
+	}
+
+	if (usb_ep_disable(ep))
+		dev_err(uac->card->dev, "%s:%d Error!\n", __func__, __LINE__);
+}
+
+
+int u_audio_start_capture(struct g_audio *audio_dev)
+{
+	struct snd_uac_chip *uac = audio_dev->uac;
+	struct usb_gadget *gadget = audio_dev->gadget;
+	struct device *dev = &gadget->dev;
+	struct usb_request *req;
+	struct usb_ep *ep;
+	struct uac_rtd_params *prm;
+	struct uac_params *params = &audio_dev->params;
+	int req_len, i;
+
+	ep = audio_dev->out_ep;
+	prm = &uac->c_prm;
+	config_ep_by_speed(gadget, &audio_dev->func, ep);
+	req_len = prm->max_psize;
+
+	prm->ep_enabled = true;
+	usb_ep_enable(ep);
+
+	for (i = 0; i < params->req_number; i++) {
+		if (!prm->ureq[i].req) {
+			req = usb_ep_alloc_request(ep, GFP_ATOMIC);
+			if (req == NULL)
+				return -ENOMEM;
+
+			prm->ureq[i].req = req;
+			prm->ureq[i].pp = prm;
+
+			req->zero = 0;
+			req->context = &prm->ureq[i];
+			req->length = req_len;
+			req->complete = u_audio_iso_complete;
+			req->buf = prm->rbuf + i * prm->max_psize;
+		}
+
+		if (usb_ep_queue(ep, prm->ureq[i].req, GFP_ATOMIC))
+			dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(u_audio_start_capture);
+
+void u_audio_stop_capture(struct g_audio *audio_dev)
+{
+	struct snd_uac_chip *uac = audio_dev->uac;
+
+	free_ep(&uac->c_prm, audio_dev->out_ep);
+}
+EXPORT_SYMBOL_GPL(u_audio_stop_capture);
+
+int u_audio_start_playback(struct g_audio *audio_dev)
+{
+	struct snd_uac_chip *uac = audio_dev->uac;
+	struct usb_gadget *gadget = audio_dev->gadget;
+	struct device *dev = &gadget->dev;
+	struct usb_request *req;
+	struct usb_ep *ep;
+	struct uac_rtd_params *prm;
+	struct uac_params *params = &audio_dev->params;
+	unsigned int factor, rate;
+	const struct usb_endpoint_descriptor *ep_desc;
+	int req_len, i;
+
+	ep = audio_dev->in_ep;
+	prm = &uac->p_prm;
+	config_ep_by_speed(gadget, &audio_dev->func, ep);
+
+	ep_desc = ep->desc;
+
+	/* pre-calculate the playback endpoint's interval */
+	if (gadget->speed == USB_SPEED_FULL)
+		factor = 1000;
+	else
+		factor = 8000;
+
+	/* pre-compute some values for iso_complete() */
+	uac->p_framesize = params->p_ssize *
+			    num_channels(params->p_chmask);
+	rate = params->p_srate * uac->p_framesize;
+	uac->p_interval = factor / (1 << (ep_desc->bInterval - 1));
+	uac->p_pktsize = min_t(unsigned int, rate / uac->p_interval,
+				prm->max_psize);
+
+	if (uac->p_pktsize < prm->max_psize)
+		uac->p_pktsize_residue = rate % uac->p_interval;
+	else
+		uac->p_pktsize_residue = 0;
+
+	req_len = uac->p_pktsize;
+	uac->p_residue = 0;
+
+	prm->ep_enabled = true;
+	usb_ep_enable(ep);
+
+	for (i = 0; i < params->req_number; i++) {
+		if (!prm->ureq[i].req) {
+			req = usb_ep_alloc_request(ep, GFP_ATOMIC);
+			if (req == NULL)
+				return -ENOMEM;
+
+			prm->ureq[i].req = req;
+			prm->ureq[i].pp = prm;
+
+			req->zero = 0;
+			req->context = &prm->ureq[i];
+			req->length = req_len;
+			req->complete = u_audio_iso_complete;
+			req->buf = prm->rbuf + i * prm->max_psize;
+		}
+
+		if (usb_ep_queue(ep, prm->ureq[i].req, GFP_ATOMIC))
+			dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(u_audio_start_playback);
+
+void u_audio_stop_playback(struct g_audio *audio_dev)
+{
+	struct snd_uac_chip *uac = audio_dev->uac;
+
+	free_ep(&uac->p_prm, audio_dev->in_ep);
+}
+EXPORT_SYMBOL_GPL(u_audio_stop_playback);
+
+int g_audio_setup(struct g_audio *g_audio, const char *pcm_name,
+					const char *card_name)
+{
+	struct snd_uac_chip *uac;
+	struct snd_card *card;
+	struct snd_pcm *pcm;
+	struct uac_params *params;
+	int p_chmask, c_chmask;
+	int err;
+
+	if (!g_audio)
+		return -EINVAL;
+
+	uac = kzalloc(sizeof(*uac), GFP_KERNEL);
+	if (!uac)
+		return -ENOMEM;
+	g_audio->uac = uac;
+	uac->audio_dev = g_audio;
+
+	params = &g_audio->params;
+	p_chmask = params->p_chmask;
+	c_chmask = params->c_chmask;
+
+	if (c_chmask) {
+		struct uac_rtd_params *prm = &uac->c_prm;
+
+		uac->c_prm.uac = uac;
+		prm->max_psize = g_audio->out_ep_maxpsize;
+
+		prm->ureq = kcalloc(params->req_number, sizeof(struct uac_req),
+				GFP_KERNEL);
+		if (!prm->ureq) {
+			err = -ENOMEM;
+			goto fail;
+		}
+
+		prm->rbuf = kcalloc(params->req_number, prm->max_psize,
+				GFP_KERNEL);
+		if (!prm->rbuf) {
+			prm->max_psize = 0;
+			err = -ENOMEM;
+			goto fail;
+		}
+	}
+
+	if (p_chmask) {
+		struct uac_rtd_params *prm = &uac->p_prm;
+
+		uac->p_prm.uac = uac;
+		prm->max_psize = g_audio->in_ep_maxpsize;
+
+		prm->ureq = kcalloc(params->req_number, sizeof(struct uac_req),
+				GFP_KERNEL);
+		if (!prm->ureq) {
+			err = -ENOMEM;
+			goto fail;
+		}
+
+		prm->rbuf = kcalloc(params->req_number, prm->max_psize,
+				GFP_KERNEL);
+		if (!prm->rbuf) {
+			prm->max_psize = 0;
+			err = -ENOMEM;
+			goto fail;
+		}
+	}
+
+	/* Choose any slot, with no id */
+	err = snd_card_new(&g_audio->gadget->dev,
+			-1, NULL, THIS_MODULE, 0, &card);
+	if (err < 0)
+		goto fail;
+
+	uac->card = card;
+
+	/*
+	 * Create first PCM device
+	 * Create a substream only for non-zero channel streams
+	 */
+	err = snd_pcm_new(uac->card, pcm_name, 0,
+			       p_chmask ? 1 : 0, c_chmask ? 1 : 0, &pcm);
+	if (err < 0)
+		goto snd_fail;
+
+	strcpy(pcm->name, pcm_name);
+	pcm->private_data = uac;
+	uac->pcm = pcm;
+
+	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_PLAYBACK, &uac_pcm_ops);
+	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_CAPTURE, &uac_pcm_ops);
+
+	strcpy(card->driver, card_name);
+	strcpy(card->shortname, card_name);
+	sprintf(card->longname, "%s %i", card_name, card->dev->id);
+
+	snd_pcm_lib_preallocate_pages_for_all(pcm, SNDRV_DMA_TYPE_CONTINUOUS,
+		snd_dma_continuous_data(GFP_KERNEL), 0, BUFF_SIZE_MAX);
+
+	err = snd_card_register(card);
+
+	if (!err)
+		return 0;
+
+snd_fail:
+	snd_card_free(card);
+fail:
+	kfree(uac->p_prm.ureq);
+	kfree(uac->c_prm.ureq);
+	kfree(uac->p_prm.rbuf);
+	kfree(uac->c_prm.rbuf);
+	kfree(uac);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(g_audio_setup);
+
+void g_audio_cleanup(struct g_audio *g_audio)
+{
+	struct snd_uac_chip *uac;
+	struct snd_card *card;
+
+	if (!g_audio || !g_audio->uac)
+		return;
+
+	uac = g_audio->uac;
+	card = uac->card;
+	if (card)
+		snd_card_free(card);
+
+	kfree(uac->p_prm.ureq);
+	kfree(uac->c_prm.ureq);
+	kfree(uac->p_prm.rbuf);
+	kfree(uac->c_prm.rbuf);
+	kfree(uac);
+}
+EXPORT_SYMBOL_GPL(g_audio_cleanup);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("USB gadget \"ALSA sound card\" utilities");
+MODULE_AUTHOR("Ruslan Bilovol");
diff --git a/drivers/usb/gadget/function/u_audio.h b/drivers/usb/gadget/function/u_audio.h
new file mode 100644
index 000000000000..07e13784cbb8
--- /dev/null
+++ b/drivers/usb/gadget/function/u_audio.h
@@ -0,0 +1,95 @@
+/*
+ * u_audio.h -- interface to USB gadget "ALSA sound card" utilities
+ *
+ * Copyright (C) 2016
+ * Author: Ruslan Bilovol <ruslan.bilovol@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef __U_AUDIO_H
+#define __U_AUDIO_H
+
+#include <linux/usb/composite.h>
+
+struct uac_params {
+	/* playback */
+	int p_chmask;	/* channel mask */
+	int p_srate;	/* rate in Hz */
+	int p_ssize;	/* sample size */
+
+	/* capture */
+	int c_chmask;	/* channel mask */
+	int c_srate;	/* rate in Hz */
+	int c_ssize;	/* sample size */
+
+	int req_number; /* number of preallocated requests */
+};
+
+struct g_audio {
+	struct usb_function func;
+	struct usb_gadget *gadget;
+
+	struct usb_ep *in_ep;
+	struct usb_ep *out_ep;
+
+	/* Max packet size for all in_ep possible speeds */
+	unsigned int in_ep_maxpsize;
+	/* Max packet size for all out_ep possible speeds */
+	unsigned int out_ep_maxpsize;
+
+	/* The ALSA Sound Card it represents on the USB-Client side */
+	struct snd_uac_chip *uac;
+
+	struct uac_params params;
+};
+
+static inline struct g_audio *func_to_g_audio(struct usb_function *f)
+{
+	return container_of(f, struct g_audio, func);
+}
+
+static inline uint num_channels(uint chanmask)
+{
+	uint num = 0;
+
+	while (chanmask) {
+		num += (chanmask & 1);
+		chanmask >>= 1;
+	}
+
+	return num;
+}
+
+/*
+ * g_audio_setup - initialize one virtual ALSA sound card
+ * @g_audio: struct with filled params, in_ep_maxpsize, out_ep_maxpsize
+ * @pcm_name: the id string for a PCM instance of this sound card
+ * @card_name: name of this soundcard
+ *
+ * This sets up the single virtual ALSA sound card that may be exported by a
+ * gadget driver using this framework.
+ *
+ * Context: may sleep
+ *
+ * Returns zero on success, or a negative error on failure.
+ */
+int g_audio_setup(struct g_audio *g_audio, const char *pcm_name,
+					const char *card_name);
+void g_audio_cleanup(struct g_audio *g_audio);
+
+int u_audio_start_capture(struct g_audio *g_audio);
+void u_audio_stop_capture(struct g_audio *g_audio);
+int u_audio_start_playback(struct g_audio *g_audio);
+void u_audio_stop_playback(struct g_audio *g_audio);
+
+#endif /* __U_AUDIO_H */
diff --git a/drivers/usb/gadget/function/u_fs.h b/drivers/usb/gadget/function/u_fs.h
index 4378cc2fcac3..540f1c48c1a8 100644
--- a/drivers/usb/gadget/function/u_fs.h
+++ b/drivers/usb/gadget/function/u_fs.h
@@ -216,6 +216,9 @@ struct ffs_data {
 #define FFS_FL_CALL_CLOSED_CALLBACK 0
 #define FFS_FL_BOUND                1
 
+	/* For waking up blocked threads when function is enabled. */
+	wait_queue_head_t		wait;
+
 	/* Active function */
 	struct ffs_function		*func;
 
diff --git a/drivers/usb/gadget/function/u_uac1.h b/drivers/usb/gadget/function/u_uac1.h
index 5c2ac8e8456d..6f188fd8633f 100644
--- a/drivers/usb/gadget/function/u_uac1.h
+++ b/drivers/usb/gadget/function/u_uac1.h
@@ -1,82 +1,41 @@
 /*
- * u_uac1.h -- interface to USB gadget "ALSA AUDIO" utilities
+ * u_uac1.h - Utility definitions for UAC1 function
  *
- * Copyright (C) 2008 Bryan Wu <cooloney@kernel.org>
- * Copyright (C) 2008 Analog Devices, Inc
+ * Copyright (C) 2016 Ruslan Bilovol <ruslan.bilovol@gmail.com>
  *
- * Enter bugs at http://blackfin.uclinux.org/
- *
- * Licensed under the GPL-2 or later.
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
  */
 
-#ifndef __U_AUDIO_H
-#define __U_AUDIO_H
+#ifndef __U_UAC1_H
+#define __U_UAC1_H
 
-#include <linux/device.h>
-#include <linux/err.h>
-#include <linux/usb/audio.h>
 #include <linux/usb/composite.h>
 
-#include <sound/core.h>
-#include <sound/pcm.h>
-#include <sound/pcm_params.h>
-
-#define FILE_PCM_PLAYBACK	"/dev/snd/pcmC0D0p"
-#define FILE_PCM_CAPTURE	"/dev/snd/pcmC0D0c"
-#define FILE_CONTROL		"/dev/snd/controlC0"
-
 #define UAC1_OUT_EP_MAX_PACKET_SIZE	200
-#define UAC1_REQ_COUNT			256
-#define UAC1_AUDIO_BUF_SIZE		48000
-
-/*
- * This represents the USB side of an audio card device, managed by a USB
- * function which provides control and stream interfaces.
- */
-
-struct gaudio_snd_dev {
-	struct gaudio			*card;
-	struct file			*filp;
-	struct snd_pcm_substream	*substream;
-	int				access;
-	int				format;
-	int				channels;
-	int				rate;
-};
-
-struct gaudio {
-	struct usb_function		func;
-	struct usb_gadget		*gadget;
+#define UAC1_DEF_CCHMASK	0x3
+#define UAC1_DEF_CSRATE		48000
+#define UAC1_DEF_CSSIZE		2
+#define UAC1_DEF_PCHMASK	0x3
+#define UAC1_DEF_PSRATE		48000
+#define UAC1_DEF_PSSIZE		2
+#define UAC1_DEF_REQ_NUM	2
 
-	/* ALSA sound device interfaces */
-	struct gaudio_snd_dev		control;
-	struct gaudio_snd_dev		playback;
-	struct gaudio_snd_dev		capture;
-
-	/* TODO */
-};
 
 struct f_uac1_opts {
 	struct usb_function_instance	func_inst;
-	int				req_buf_size;
-	int				req_count;
-	int				audio_buf_size;
-	char				*fn_play;
-	char				*fn_cap;
-	char				*fn_cntl;
+	int				c_chmask;
+	int				c_srate;
+	int				c_ssize;
+	int				p_chmask;
+	int				p_srate;
+	int				p_ssize;
+	int				req_number;
 	unsigned			bound:1;
-	unsigned			fn_play_alloc:1;
-	unsigned			fn_cap_alloc:1;
-	unsigned			fn_cntl_alloc:1;
+
 	struct mutex			lock;
 	int				refcnt;
 };
 
-int gaudio_setup(struct gaudio *card);
-void gaudio_cleanup(struct gaudio *the_card);
-
-size_t u_audio_playback(struct gaudio *card, void *buf, size_t count);
-int u_audio_get_playback_channels(struct gaudio *card);
-int u_audio_get_playback_rate(struct gaudio *card);
-
-#endif /* __U_AUDIO_H */
+#endif /* __U_UAC1_H */
diff --git a/drivers/usb/gadget/function/u_uac1.c b/drivers/usb/gadget/function/u_uac1_legacy.c
index c78c84138a28..fa4684a1c54c 100644
--- a/drivers/usb/gadget/function/u_uac1.c
+++ b/drivers/usb/gadget/function/u_uac1_legacy.c
@@ -18,7 +18,7 @@
 #include <linux/random.h>
 #include <linux/syscalls.h>
 
-#include "u_uac1.h"
+#include "u_uac1_legacy.h"
 
 /*
  * This component encapsulates the ALSA devices for USB audio gadget
@@ -157,7 +157,6 @@ size_t u_audio_playback(struct gaudio *card, void *buf, size_t count)
 	struct gaudio_snd_dev	*snd = &card->playback;
 	struct snd_pcm_substream *substream = snd->substream;
 	struct snd_pcm_runtime *runtime = substream->runtime;
-	mm_segment_t old_fs;
 	ssize_t result;
 	snd_pcm_sframes_t frames;
 
@@ -174,15 +173,11 @@ try_again:
 	}
 
 	frames = bytes_to_frames(runtime, count);
-	old_fs = get_fs();
-	set_fs(KERNEL_DS);
-	result = snd_pcm_lib_write(snd->substream, (void __user *)buf, frames);
+	result = snd_pcm_kernel_write(snd->substream, buf, frames);
 	if (result != frames) {
 		ERROR(card, "Playback error: %d\n", (int)result);
-		set_fs(old_fs);
 		goto try_again;
 	}
-	set_fs(old_fs);
 
 	return 0;
 }
@@ -205,10 +200,11 @@ static int gaudio_open_snd_dev(struct gaudio *card)
 {
 	struct snd_pcm_file *pcm_file;
 	struct gaudio_snd_dev *snd;
-	struct f_uac1_opts *opts;
+	struct f_uac1_legacy_opts *opts;
 	char *fn_play, *fn_cap, *fn_cntl;
 
-	opts = container_of(card->func.fi, struct f_uac1_opts, func_inst);
+	opts = container_of(card->func.fi, struct f_uac1_legacy_opts,
+			    func_inst);
 	fn_play = opts->fn_play;
 	fn_cap = opts->fn_cap;
 	fn_cntl = opts->fn_cntl;
diff --git a/drivers/usb/gadget/function/u_uac1_legacy.h b/drivers/usb/gadget/function/u_uac1_legacy.h
new file mode 100644
index 000000000000..d715b1af56a4
--- /dev/null
+++ b/drivers/usb/gadget/function/u_uac1_legacy.h
@@ -0,0 +1,82 @@
+/*
+ * u_uac1.h -- interface to USB gadget "ALSA AUDIO" utilities
+ *
+ * Copyright (C) 2008 Bryan Wu <cooloney@kernel.org>
+ * Copyright (C) 2008 Analog Devices, Inc
+ *
+ * Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#ifndef __U_UAC1_LEGACY_H
+#define __U_UAC1_LEGACY_H
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/usb/audio.h>
+#include <linux/usb/composite.h>
+
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+
+#define FILE_PCM_PLAYBACK	"/dev/snd/pcmC0D0p"
+#define FILE_PCM_CAPTURE	"/dev/snd/pcmC0D0c"
+#define FILE_CONTROL		"/dev/snd/controlC0"
+
+#define UAC1_OUT_EP_MAX_PACKET_SIZE	200
+#define UAC1_REQ_COUNT			256
+#define UAC1_AUDIO_BUF_SIZE		48000
+
+/*
+ * This represents the USB side of an audio card device, managed by a USB
+ * function which provides control and stream interfaces.
+ */
+
+struct gaudio_snd_dev {
+	struct gaudio			*card;
+	struct file			*filp;
+	struct snd_pcm_substream	*substream;
+	int				access;
+	int				format;
+	int				channels;
+	int				rate;
+};
+
+struct gaudio {
+	struct usb_function		func;
+	struct usb_gadget		*gadget;
+
+	/* ALSA sound device interfaces */
+	struct gaudio_snd_dev		control;
+	struct gaudio_snd_dev		playback;
+	struct gaudio_snd_dev		capture;
+
+	/* TODO */
+};
+
+struct f_uac1_legacy_opts {
+	struct usb_function_instance	func_inst;
+	int				req_buf_size;
+	int				req_count;
+	int				audio_buf_size;
+	char				*fn_play;
+	char				*fn_cap;
+	char				*fn_cntl;
+	unsigned			bound:1;
+	unsigned			fn_play_alloc:1;
+	unsigned			fn_cap_alloc:1;
+	unsigned			fn_cntl_alloc:1;
+	struct mutex			lock;
+	int				refcnt;
+};
+
+int gaudio_setup(struct gaudio *card);
+void gaudio_cleanup(struct gaudio *the_card);
+
+size_t u_audio_playback(struct gaudio *card, void *buf, size_t count);
+int u_audio_get_playback_channels(struct gaudio *card);
+int u_audio_get_playback_rate(struct gaudio *card);
+
+#endif /* __U_UAC1_LEGACY_H */
diff --git a/drivers/usb/gadget/legacy/Kconfig b/drivers/usb/gadget/legacy/Kconfig
index 0b36878eb5fd..a12fb459dbd9 100644
--- a/drivers/usb/gadget/legacy/Kconfig
+++ b/drivers/usb/gadget/legacy/Kconfig
@@ -54,8 +54,10 @@ config USB_AUDIO
 	depends on SND
 	select USB_LIBCOMPOSITE
 	select SND_PCM
-	select USB_F_UAC1 if GADGET_UAC1
+	select USB_F_UAC1 if (GADGET_UAC1 && !GADGET_UAC1_LEGACY)
+	select USB_F_UAC1_LEGACY if (GADGET_UAC1 && GADGET_UAC1_LEGACY)
 	select USB_F_UAC2 if !GADGET_UAC1
+	select USB_U_AUDIO if (USB_F_UAC2 || USB_F_UAC1)
 	help
 	  This Gadget Audio driver is compatible with USB Audio Class
 	  specification 2.0. It implements 1 AudioControl interface,
@@ -73,10 +75,17 @@ config USB_AUDIO
 	  dynamically linked module called "g_audio".
 
 config GADGET_UAC1
-	bool "UAC 1.0 (Legacy)"
+	bool "UAC 1.0"
 	depends on USB_AUDIO
 	help
-	  If you instead want older UAC Spec-1.0 driver that also has audio
+	  If you instead want older USB Audio Class specification 1.0 support
+	  with similar driver capabilities.
+
+config GADGET_UAC1_LEGACY
+	bool "UAC 1.0 (Legacy)"
+	depends on GADGET_UAC1
+	help
+	  If you instead want legacy UAC Spec-1.0 driver that also has audio
 	  paths hardwired to the Audio codec chip on-board and doesn't work
 	  without one.
 
diff --git a/drivers/usb/gadget/legacy/audio.c b/drivers/usb/gadget/legacy/audio.c
index 8a39f42a4d56..1f5cdbe162df 100644
--- a/drivers/usb/gadget/legacy/audio.c
+++ b/drivers/usb/gadget/legacy/audio.c
@@ -53,8 +53,41 @@ static int c_ssize = UAC2_DEF_CSSIZE;
 module_param(c_ssize, uint, S_IRUGO);
 MODULE_PARM_DESC(c_ssize, "Capture Sample Size(bytes)");
 #else
+#ifndef CONFIG_GADGET_UAC1_LEGACY
 #include "u_uac1.h"
 
+/* Playback(USB-IN) Default Stereo - Fl/Fr */
+static int p_chmask = UAC1_DEF_PCHMASK;
+module_param(p_chmask, uint, S_IRUGO);
+MODULE_PARM_DESC(p_chmask, "Playback Channel Mask");
+
+/* Playback Default 48 KHz */
+static int p_srate = UAC1_DEF_PSRATE;
+module_param(p_srate, uint, S_IRUGO);
+MODULE_PARM_DESC(p_srate, "Playback Sampling Rate");
+
+/* Playback Default 16bits/sample */
+static int p_ssize = UAC1_DEF_PSSIZE;
+module_param(p_ssize, uint, S_IRUGO);
+MODULE_PARM_DESC(p_ssize, "Playback Sample Size(bytes)");
+
+/* Capture(USB-OUT) Default Stereo - Fl/Fr */
+static int c_chmask = UAC1_DEF_CCHMASK;
+module_param(c_chmask, uint, S_IRUGO);
+MODULE_PARM_DESC(c_chmask, "Capture Channel Mask");
+
+/* Capture Default 48 KHz */
+static int c_srate = UAC1_DEF_CSRATE;
+module_param(c_srate, uint, S_IRUGO);
+MODULE_PARM_DESC(c_srate, "Capture Sampling Rate");
+
+/* Capture Default 16bits/sample */
+static int c_ssize = UAC1_DEF_CSSIZE;
+module_param(c_ssize, uint, S_IRUGO);
+MODULE_PARM_DESC(c_ssize, "Capture Sample Size(bytes)");
+#else /* CONFIG_GADGET_UAC1_LEGACY */
+#include "u_uac1_legacy.h"
+
 static char *fn_play = FILE_PCM_PLAYBACK;
 module_param(fn_play, charp, S_IRUGO);
 MODULE_PARM_DESC(fn_play, "Playback PCM device file name");
@@ -78,6 +111,7 @@ MODULE_PARM_DESC(req_count, "ISO OUT endpoint request count");
 static int audio_buf_size = UAC1_AUDIO_BUF_SIZE;
 module_param(audio_buf_size, int, S_IRUGO);
 MODULE_PARM_DESC(audio_buf_size, "Audio buffer size");
+#endif /* CONFIG_GADGET_UAC1_LEGACY */
 #endif
 
 /* string IDs are assigned dynamically */
@@ -125,7 +159,7 @@ static struct usb_device_descriptor device_desc = {
 
 	/* .bcdUSB = DYNAMIC */
 
-#ifdef CONFIG_GADGET_UAC1
+#ifdef CONFIG_GADGET_UAC1_LEGACY
 	.bDeviceClass =		USB_CLASS_PER_INTERFACE,
 	.bDeviceSubClass =	0,
 	.bDeviceProtocol =	0,
@@ -207,7 +241,11 @@ static int audio_bind(struct usb_composite_dev *cdev)
 #ifndef CONFIG_GADGET_UAC1
 	struct f_uac2_opts	*uac2_opts;
 #else
+#ifndef CONFIG_GADGET_UAC1_LEGACY
 	struct f_uac1_opts	*uac1_opts;
+#else
+	struct f_uac1_legacy_opts	*uac1_opts;
+#endif
 #endif
 	int			status;
 
@@ -216,7 +254,11 @@ static int audio_bind(struct usb_composite_dev *cdev)
 	if (IS_ERR(fi_uac2))
 		return PTR_ERR(fi_uac2);
 #else
+#ifndef CONFIG_GADGET_UAC1_LEGACY
 	fi_uac1 = usb_get_function_instance("uac1");
+#else
+	fi_uac1 = usb_get_function_instance("uac1_legacy");
+#endif
 	if (IS_ERR(fi_uac1))
 		return PTR_ERR(fi_uac1);
 #endif
@@ -231,13 +273,24 @@ static int audio_bind(struct usb_composite_dev *cdev)
 	uac2_opts->c_ssize = c_ssize;
 	uac2_opts->req_number = UAC2_DEF_REQ_NUM;
 #else
+#ifndef CONFIG_GADGET_UAC1_LEGACY
 	uac1_opts = container_of(fi_uac1, struct f_uac1_opts, func_inst);
+	uac1_opts->p_chmask = p_chmask;
+	uac1_opts->p_srate = p_srate;
+	uac1_opts->p_ssize = p_ssize;
+	uac1_opts->c_chmask = c_chmask;
+	uac1_opts->c_srate = c_srate;
+	uac1_opts->c_ssize = c_ssize;
+	uac1_opts->req_number = UAC1_DEF_REQ_NUM;
+#else /* CONFIG_GADGET_UAC1_LEGACY */
+	uac1_opts = container_of(fi_uac1, struct f_uac1_legacy_opts, func_inst);
 	uac1_opts->fn_play = fn_play;
 	uac1_opts->fn_cap = fn_cap;
 	uac1_opts->fn_cntl = fn_cntl;
 	uac1_opts->req_buf_size = req_buf_size;
 	uac1_opts->req_count = req_count;
 	uac1_opts->audio_buf_size = audio_buf_size;
+#endif /* CONFIG_GADGET_UAC1_LEGACY */
 #endif
 
 	status = usb_string_ids_tab(cdev, strings_dev);
diff --git a/drivers/usb/gadget/legacy/inode.c b/drivers/usb/gadget/legacy/inode.c
index b9ca0a26cbd9..684900fcfe24 100644
--- a/drivers/usb/gadget/legacy/inode.c
+++ b/drivers/usb/gadget/legacy/inode.c
@@ -1183,8 +1183,10 @@ dev_release (struct inode *inode, struct file *fd)
 
 	/* closing ep0 === shutdown all */
 
-	if (dev->gadget_registered)
+	if (dev->gadget_registered) {
 		usb_gadget_unregister_driver (&gadgetfs_driver);
+		dev->gadget_registered = false;
+	}
 
 	/* at this point "good" hardware has disconnected the
 	 * device from USB; the host won't see it any more.
@@ -1677,9 +1679,10 @@ static void
 gadgetfs_suspend (struct usb_gadget *gadget)
 {
 	struct dev_data		*dev = get_gadget_data (gadget);
+	unsigned long		flags;
 
 	INFO (dev, "suspended from state %d\n", dev->state);
-	spin_lock (&dev->lock);
+	spin_lock_irqsave(&dev->lock, flags);
 	switch (dev->state) {
 	case STATE_DEV_SETUP:		// VERY odd... host died??
 	case STATE_DEV_CONNECTED:
@@ -1690,7 +1693,7 @@ gadgetfs_suspend (struct usb_gadget *gadget)
 	default:
 		break;
 	}
-	spin_unlock (&dev->lock);
+	spin_unlock_irqrestore(&dev->lock, flags);
 }
 
 static struct usb_gadget_driver gadgetfs_driver = {
diff --git a/drivers/usb/gadget/legacy/mass_storage.c b/drivers/usb/gadget/legacy/mass_storage.c
index 125974f32f50..e99ab57ee3e5 100644
--- a/drivers/usb/gadget/legacy/mass_storage.c
+++ b/drivers/usb/gadget/legacy/mass_storage.c
@@ -210,7 +210,6 @@ static int msg_bind(struct usb_composite_dev *cdev)
 	usb_composite_overwrite_options(cdev, &coverwrite);
 	dev_info(&cdev->gadget->dev,
 		 DRIVER_DESC ", version: " DRIVER_VERSION "\n");
-	set_bit(0, &msg_registered);
 	return 0;
 
 fail_otg_desc:
@@ -257,7 +256,12 @@ MODULE_LICENSE("GPL");
 
 static int __init msg_init(void)
 {
-	return usb_composite_probe(&msg_driver);
+	int ret;
+
+	ret = usb_composite_probe(&msg_driver);
+	set_bit(0, &msg_registered);
+
+	return ret;
 }
 module_init(msg_init);
 
diff --git a/drivers/usb/gadget/udc/Kconfig b/drivers/usb/gadget/udc/Kconfig
index 1c14c283cc47..9ffb11ec9ed9 100644
--- a/drivers/usb/gadget/udc/Kconfig
+++ b/drivers/usb/gadget/udc/Kconfig
@@ -55,7 +55,7 @@ config USB_LPC32XX
 
 config USB_ATMEL_USBA
 	tristate "Atmel USBA"
-	depends on ((AVR32 && !OF) || ARCH_AT91)
+	depends on ARCH_AT91
 	help
 	  USBA is the integrated high-speed USB Device controller on
 	  the AT32AP700x, some AT91SAM9 and AT91CAP9 processors from Atmel.
@@ -256,7 +256,7 @@ config USB_MV_U3D
 	  controller, which support super speed USB peripheral.
 
 config USB_SNP_CORE
-	depends on USB_AMD5536UDC
+	depends on (USB_AMD5536UDC || USB_SNP_UDC_PLAT)
 	tristate
 	help
 	  This enables core driver support for Synopsys USB 2.0 Device
@@ -269,6 +269,20 @@ config USB_SNP_CORE
 	  This IP is different to the High Speed OTG IP that can be enabled
 	  by selecting USB_DWC2 or USB_DWC3 options.
 
+config USB_SNP_UDC_PLAT
+	tristate "Synopsys USB 2.0 Device controller"
+	depends on (USB_GADGET && OF)
+	select USB_GADGET_DUALSPEED
+	select USB_SNP_CORE
+	default ARCH_BCM_IPROC
+	help
+	  This adds Platform Device support for Synopsys Designware core
+	  AHB subsystem USB2.0 Device Controller (UDC).
+
+	  This driver works with UDCs integrated into Broadcom's Northstar2
+	  and Cygnus SoCs.
+
+	  If unsure, say N.
 #
 # Controllers available in both integrated and discrete versions
 #
diff --git a/drivers/usb/gadget/udc/Makefile b/drivers/usb/gadget/udc/Makefile
index 626e1f1c62da..ea9e1c7f1923 100644
--- a/drivers/usb/gadget/udc/Makefile
+++ b/drivers/usb/gadget/udc/Makefile
@@ -10,7 +10,7 @@ obj-$(CONFIG_USB_GADGET)	+= udc-core.o
 obj-$(CONFIG_USB_DUMMY_HCD)	+= dummy_hcd.o
 obj-$(CONFIG_USB_NET2272)	+= net2272.o
 obj-$(CONFIG_USB_NET2280)	+= net2280.o
-obj-$(CONFIG_USB_SNP_CORE)	+= amd5536udc.o
+obj-$(CONFIG_USB_SNP_CORE)	+= snps_udc_core.o
 obj-$(CONFIG_USB_AMD5536UDC)	+= amd5536udc_pci.o
 obj-$(CONFIG_USB_PXA25X)	+= pxa25x_udc.o
 obj-$(CONFIG_USB_PXA27X)	+= pxa27x_udc.o
@@ -37,4 +37,5 @@ obj-$(CONFIG_USB_FOTG210_UDC)	+= fotg210-udc.o
 obj-$(CONFIG_USB_MV_U3D)	+= mv_u3d_core.o
 obj-$(CONFIG_USB_GR_UDC)	+= gr_udc.o
 obj-$(CONFIG_USB_GADGET_XILINX)	+= udc-xilinx.o
+obj-$(CONFIG_USB_SNP_UDC_PLAT) += snps_udc_plat.o
 obj-$(CONFIG_USB_BDC_UDC)	+= bdc/
diff --git a/drivers/usb/gadget/udc/amd5536udc.h b/drivers/usb/gadget/udc/amd5536udc.h
index fae49bf3833e..4fe22d432af2 100644
--- a/drivers/usb/gadget/udc/amd5536udc.h
+++ b/drivers/usb/gadget/udc/amd5536udc.h
@@ -16,6 +16,7 @@
 /* debug control */
 /* #define UDC_VERBOSE */
 
+#include <linux/extcon.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
 
@@ -28,6 +29,9 @@
 #define UDC_HSA0_REV 1
 #define UDC_HSB1_REV 2
 
+/* Broadcom chip rev. */
+#define UDC_BCM_REV 10
+
 /*
  * SETUP usb commands
  * needed, because some SETUP's are handled in hw, but must be passed to
@@ -112,6 +116,7 @@
 #define UDC_DEVCTL_BRLEN_MASK			0x00ff0000
 #define UDC_DEVCTL_BRLEN_OFS			16
 
+#define UDC_DEVCTL_SRX_FLUSH			14
 #define UDC_DEVCTL_CSR_DONE			13
 #define UDC_DEVCTL_DEVNAK			12
 #define UDC_DEVCTL_SD				10
@@ -563,6 +568,16 @@ struct udc {
 	u16				cur_config;
 	u16				cur_intf;
 	u16				cur_alt;
+
+	/* for platform device and extcon support */
+	struct device			*dev;
+	struct phy			*udc_phy;
+	struct extcon_dev		*edev;
+	struct extcon_specific_cable_nb	extcon_nb;
+	struct notifier_block		nb;
+	struct delayed_work		drd_work;
+	struct workqueue_struct		*drd_wq;
+	u32				conn_type;
 };
 
 #define to_amd5536_udc(g)	(container_of((g), struct udc, gadget))
@@ -578,6 +593,7 @@ int udc_enable_dev_setup_interrupts(struct udc *dev);
 int udc_mask_unused_interrupts(struct udc *dev);
 irqreturn_t udc_irq(int irq, void *pdev);
 void gadget_release(struct device *pdev);
+void empty_req_queue(struct udc_ep *ep);
 void udc_basic_init(struct udc *dev);
 void free_dma_pools(struct udc *dev);
 int init_dma_pools(struct udc *dev);
@@ -639,7 +655,7 @@ MODULE_PARM_DESC(use_fullspeed, "true for fullspeed only");
 
 /* debug macros ------------------------------------------------------------*/
 
-#define DBG(udc , args...)	dev_dbg(&(udc)->pdev->dev, args)
+#define DBG(udc , args...)	dev_dbg(udc->dev, args)
 
 #ifdef UDC_VERBOSE
 #define VDBG			DBG
diff --git a/drivers/usb/gadget/udc/amd5536udc_pci.c b/drivers/usb/gadget/udc/amd5536udc_pci.c
index 2a2d0a96fe24..57a13f080a79 100644
--- a/drivers/usb/gadget/udc/amd5536udc_pci.c
+++ b/drivers/usb/gadget/udc/amd5536udc_pci.c
@@ -168,6 +168,7 @@ static int udc_pci_probe(
 	dev->phys_addr = resource;
 	dev->irq = pdev->irq;
 	dev->pdev = pdev;
+	dev->dev = &pdev->dev;
 
 	/* general probing */
 	if (udc_probe(dev)) {
diff --git a/drivers/usb/gadget/udc/atmel_usba_udc.c b/drivers/usb/gadget/udc/atmel_usba_udc.c
index 3ccc34176a5a..98d71400f8a1 100644
--- a/drivers/usb/gadget/udc/atmel_usba_udc.c
+++ b/drivers/usb/gadget/udc/atmel_usba_udc.c
@@ -152,7 +152,7 @@ static int regs_dbg_open(struct inode *inode, struct file *file)
 
 	spin_lock_irq(&udc->lock);
 	for (i = 0; i < inode->i_size / 4; i++)
-		data[i] = usba_io_readl(udc->regs + i * 4);
+		data[i] = readl_relaxed(udc->regs + i * 4);
 	spin_unlock_irq(&udc->lock);
 
 	file->private_data = data;
@@ -1369,7 +1369,7 @@ static int handle_ep0_setup(struct usba_udc *udc, struct usba_ep *ep,
 		if (crq->wLength != cpu_to_le16(sizeof(status)))
 			goto stall;
 		ep->state = DATA_STAGE_IN;
-		usba_io_writew(status, ep->fifo);
+		writew_relaxed(status, ep->fifo);
 		usba_ep_writel(ep, SET_STA, USBA_TX_PK_RDY);
 		break;
 	}
diff --git a/drivers/usb/gadget/udc/atmel_usba_udc.h b/drivers/usb/gadget/udc/atmel_usba_udc.h
index 9551b704bfd3..f8ebe0389bd4 100644
--- a/drivers/usb/gadget/udc/atmel_usba_udc.h
+++ b/drivers/usb/gadget/udc/atmel_usba_udc.h
@@ -43,13 +43,8 @@
 #define USBA_REMOTE_WAKE_UP			(1 << 10)
 #define USBA_PULLD_DIS				(1 << 11)
 
-#if defined(CONFIG_AVR32)
-#define USBA_ENABLE_MASK			USBA_EN_USBA
-#define USBA_DISABLE_MASK			0
-#elif defined(CONFIG_ARCH_AT91)
 #define USBA_ENABLE_MASK			(USBA_EN_USBA | USBA_PULLD_DIS)
 #define USBA_DISABLE_MASK			USBA_DETACH
-#endif /* CONFIG_ARCH_AT91 */
 
 /* Bitfields in FNUM */
 #define USBA_MICRO_FRAME_NUM_OFFSET		0
@@ -191,28 +186,18 @@
 	 | USBA_BF(name, value))
 
 /* Register access macros */
-#ifdef CONFIG_AVR32
-#define usba_io_readl	__raw_readl
-#define usba_io_writel	__raw_writel
-#define usba_io_writew	__raw_writew
-#else
-#define usba_io_readl	readl_relaxed
-#define usba_io_writel	writel_relaxed
-#define usba_io_writew	writew_relaxed
-#endif
-
 #define usba_readl(udc, reg)					\
-	usba_io_readl((udc)->regs + USBA_##reg)
+	readl_relaxed((udc)->regs + USBA_##reg)
 #define usba_writel(udc, reg, value)				\
-	usba_io_writel((value), (udc)->regs + USBA_##reg)
+	writel_relaxed((value), (udc)->regs + USBA_##reg)
 #define usba_ep_readl(ep, reg)					\
-	usba_io_readl((ep)->ep_regs + USBA_EPT_##reg)
+	readl_relaxed((ep)->ep_regs + USBA_EPT_##reg)
 #define usba_ep_writel(ep, reg, value)				\
-	usba_io_writel((value), (ep)->ep_regs + USBA_EPT_##reg)
+	writel_relaxed((value), (ep)->ep_regs + USBA_EPT_##reg)
 #define usba_dma_readl(ep, reg)					\
-	usba_io_readl((ep)->dma_regs + USBA_DMA_##reg)
+	readl_relaxed((ep)->dma_regs + USBA_DMA_##reg)
 #define usba_dma_writel(ep, reg, value)				\
-	usba_io_writel((value), (ep)->dma_regs + USBA_DMA_##reg)
+	writel_relaxed((value), (ep)->dma_regs + USBA_DMA_##reg)
 
 /* Calculate base address for a given endpoint or DMA controller */
 #define USBA_EPT_BASE(x)	(0x100 + (x) * 0x20)
diff --git a/drivers/usb/gadget/udc/bdc/bdc_core.c b/drivers/usb/gadget/udc/bdc/bdc_core.c
index ccb9c213cc9f..e9bd8d4abca0 100644
--- a/drivers/usb/gadget/udc/bdc/bdc_core.c
+++ b/drivers/usb/gadget/udc/bdc/bdc_core.c
@@ -475,7 +475,7 @@ static int bdc_probe(struct platform_device *pdev)
 	bdc->dev = dev;
 	dev_dbg(bdc->dev, "bdc->regs: %p irq=%d\n", bdc->regs, bdc->irq);
 
-	temp = bdc_readl(bdc->regs, BDC_BDCSC);
+	temp = bdc_readl(bdc->regs, BDC_BDCCAP1);
 	if ((temp & BDC_P64) &&
 			!dma_set_mask_and_coherent(dev, DMA_BIT_MASK(64))) {
 		dev_dbg(bdc->dev, "Using 64-bit address\n");
diff --git a/drivers/usb/gadget/udc/core.c b/drivers/usb/gadget/udc/core.c
index efce68e9a8e0..e6f04eee95c4 100644
--- a/drivers/usb/gadget/udc/core.c
+++ b/drivers/usb/gadget/udc/core.c
@@ -23,6 +23,7 @@
 #include <linux/list.h>
 #include <linux/err.h>
 #include <linux/dma-mapping.h>
+#include <linux/sched/task_stack.h>
 #include <linux/workqueue.h>
 
 #include <linux/usb/ch9.h>
@@ -139,10 +140,8 @@ int usb_ep_disable(struct usb_ep *ep)
 		goto out;
 
 	ret = ep->ops->disable(ep);
-	if (ret) {
-		ret = ret;
+	if (ret)
 		goto out;
-	}
 
 	ep->enabled = false;
 
@@ -798,6 +797,14 @@ int usb_gadget_map_request_by_dev(struct device *dev,
 
 		req->num_mapped_sgs = mapped;
 	} else {
+		if (is_vmalloc_addr(req->buf)) {
+			dev_err(dev, "buffer is not dma capable\n");
+			return -EFAULT;
+		} else if (object_is_on_stack(req->buf)) {
+			dev_err(dev, "buffer is on stack\n");
+			return -EFAULT;
+		}
+
 		req->dma = dma_map_single(dev, req->buf, req->length,
 				is_in ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
 
@@ -1058,6 +1065,23 @@ static inline void usb_gadget_udc_stop(struct usb_udc *udc)
 }
 
 /**
+ * usb_gadget_udc_set_speed - tells usb device controller speed supported by
+ *    current driver
+ * @udc: The device we want to set maximum speed
+ * @speed: The maximum speed to allowed to run
+ *
+ * This call is issued by the UDC Class driver before calling
+ * usb_gadget_udc_start() in order to make sure that we don't try to
+ * connect on speeds the gadget driver doesn't support.
+ */
+static inline void usb_gadget_udc_set_speed(struct usb_udc *udc,
+					    enum usb_device_speed speed)
+{
+	if (udc->gadget->ops->udc_set_speed)
+		udc->gadget->ops->udc_set_speed(udc->gadget, speed);
+}
+
+/**
  * usb_udc_release - release the usb_udc struct
  * @dev: the dev member within usb_udc
  *
@@ -1290,6 +1314,9 @@ static int udc_bind_to_driver(struct usb_udc *udc, struct usb_gadget_driver *dri
 	udc->dev.driver = &driver->driver;
 	udc->gadget->dev.driver = &driver->driver;
 
+	if (driver->max_speed < udc->gadget->max_speed)
+		usb_gadget_udc_set_speed(udc, driver->max_speed);
+
 	ret = driver->bind(udc->gadget, driver);
 	if (ret)
 		goto err1;
@@ -1442,6 +1469,18 @@ static ssize_t state_show(struct device *dev, struct device_attribute *attr,
 }
 static DEVICE_ATTR_RO(state);
 
+static ssize_t function_show(struct device *dev, struct device_attribute *attr,
+			     char *buf)
+{
+	struct usb_udc		*udc = container_of(dev, struct usb_udc, dev);
+	struct usb_gadget_driver *drv = udc->driver;
+
+	if (!drv || !drv->function)
+		return 0;
+	return scnprintf(buf, PAGE_SIZE, "%s\n", drv->function);
+}
+static DEVICE_ATTR_RO(function);
+
 #define USB_UDC_SPEED_ATTR(name, param)					\
 ssize_t name##_show(struct device *dev,					\
 		struct device_attribute *attr, char *buf)		\
@@ -1477,6 +1516,7 @@ static struct attribute *usb_udc_attrs[] = {
 	&dev_attr_srp.attr,
 	&dev_attr_soft_connect.attr,
 	&dev_attr_state.attr,
+	&dev_attr_function.attr,
 	&dev_attr_current_speed.attr,
 	&dev_attr_maximum_speed.attr,
 
diff --git a/drivers/usb/gadget/udc/dummy_hcd.c b/drivers/usb/gadget/udc/dummy_hcd.c
index ccabb51cb98d..3c3760315910 100644
--- a/drivers/usb/gadget/udc/dummy_hcd.c
+++ b/drivers/usb/gadget/udc/dummy_hcd.c
@@ -442,23 +442,16 @@ static void set_link_state(struct dummy_hcd *dum_hcd)
 		/* Report reset and disconnect events to the driver */
 		if (dum->driver && (disconnect || reset)) {
 			stop_activity(dum);
-			spin_unlock(&dum->lock);
 			if (reset)
 				usb_gadget_udc_reset(&dum->gadget, dum->driver);
 			else
 				dum->driver->disconnect(&dum->gadget);
-			spin_lock(&dum->lock);
 		}
 	} else if (dum_hcd->active != dum_hcd->old_active) {
-		if (dum_hcd->old_active && dum->driver->suspend) {
-			spin_unlock(&dum->lock);
+		if (dum_hcd->old_active && dum->driver->suspend)
 			dum->driver->suspend(&dum->gadget);
-			spin_lock(&dum->lock);
-		} else if (!dum_hcd->old_active &&  dum->driver->resume) {
-			spin_unlock(&dum->lock);
+		else if (!dum_hcd->old_active &&  dum->driver->resume)
 			dum->driver->resume(&dum->gadget);
-			spin_lock(&dum->lock);
-		}
 	}
 
 	dum_hcd->old_status = dum_hcd->port_status;
@@ -888,22 +881,6 @@ static int dummy_pullup(struct usb_gadget *_gadget, int value)
 	unsigned long	flags;
 
 	dum = gadget_dev_to_dummy(&_gadget->dev);
-
-	if (value && dum->driver) {
-		if (mod_data.is_super_speed)
-			dum->gadget.speed = dum->driver->max_speed;
-		else if (mod_data.is_high_speed)
-			dum->gadget.speed = min_t(u8, USB_SPEED_HIGH,
-					dum->driver->max_speed);
-		else
-			dum->gadget.speed = USB_SPEED_FULL;
-		dummy_udc_update_ep0(dum);
-
-		if (dum->gadget.speed < dum->driver->max_speed)
-			dev_dbg(udc_dev(dum), "This device can perform faster"
-				" if you connect it to a %s port...\n",
-				usb_speed_string(dum->driver->max_speed));
-	}
 	dum_hcd = gadget_to_dummy_hcd(_gadget);
 
 	spin_lock_irqsave(&dum->lock, flags);
@@ -915,6 +892,28 @@ static int dummy_pullup(struct usb_gadget *_gadget, int value)
 	return 0;
 }
 
+static void dummy_udc_set_speed(struct usb_gadget *_gadget,
+		enum usb_device_speed speed)
+{
+	struct dummy	*dum;
+
+	dum = gadget_dev_to_dummy(&_gadget->dev);
+
+	 if (mod_data.is_super_speed)
+		 dum->gadget.speed = min_t(u8, USB_SPEED_SUPER, speed);
+	 else if (mod_data.is_high_speed)
+		 dum->gadget.speed = min_t(u8, USB_SPEED_HIGH, speed);
+	 else
+		 dum->gadget.speed = USB_SPEED_FULL;
+
+	dummy_udc_update_ep0(dum);
+
+	if (dum->gadget.speed < speed)
+		dev_dbg(udc_dev(dum), "This device can perform faster"
+			" if you connect it to a %s port...\n",
+			usb_speed_string(speed));
+}
+
 static int dummy_udc_start(struct usb_gadget *g,
 		struct usb_gadget_driver *driver);
 static int dummy_udc_stop(struct usb_gadget *g);
@@ -926,6 +925,7 @@ static const struct usb_gadget_ops dummy_ops = {
 	.pullup		= dummy_pullup,
 	.udc_start	= dummy_udc_start,
 	.udc_stop	= dummy_udc_stop,
+	.udc_set_speed	= dummy_udc_set_speed,
 };
 
 /*-------------------------------------------------------------------------*/
@@ -983,7 +983,9 @@ static int dummy_udc_stop(struct usb_gadget *g)
 	struct dummy_hcd	*dum_hcd = gadget_to_dummy_hcd(g);
 	struct dummy		*dum = dum_hcd->dum;
 
+	spin_lock_irq(&dum->lock);
 	dum->driver = NULL;
+	spin_unlock_irq(&dum->lock);
 
 	return 0;
 }
diff --git a/drivers/usb/gadget/udc/mv_udc_core.c b/drivers/usb/gadget/udc/mv_udc_core.c
index 76f56c5762f9..8a708d0a1042 100644
--- a/drivers/usb/gadget/udc/mv_udc_core.c
+++ b/drivers/usb/gadget/udc/mv_udc_core.c
@@ -960,9 +960,9 @@ static const struct usb_ep_ops mv_ep_ops = {
 	.fifo_flush	= mv_ep_fifo_flush,	/* flush fifo */
 };
 
-static void udc_clock_enable(struct mv_udc *udc)
+static int udc_clock_enable(struct mv_udc *udc)
 {
-	clk_prepare_enable(udc->clk);
+	return clk_prepare_enable(udc->clk);
 }
 
 static void udc_clock_disable(struct mv_udc *udc)
@@ -1070,7 +1070,10 @@ static int mv_udc_enable_internal(struct mv_udc *udc)
 		return 0;
 
 	dev_dbg(&udc->dev->dev, "enable udc\n");
-	udc_clock_enable(udc);
+	retval = udc_clock_enable(udc);
+	if (retval)
+		return retval;
+
 	if (udc->pdata->phy_init) {
 		retval = udc->pdata->phy_init(udc->phy_regs);
 		if (retval) {
diff --git a/drivers/usb/gadget/udc/net2280.c b/drivers/usb/gadget/udc/net2280.c
index 6cf07857eaca..f608c1f85e61 100644
--- a/drivers/usb/gadget/udc/net2280.c
+++ b/drivers/usb/gadget/udc/net2280.c
@@ -2470,11 +2470,8 @@ static void stop_activity(struct net2280 *dev, struct usb_gadget_driver *driver)
 		nuke(&dev->ep[i]);
 
 	/* report disconnect; the driver is already quiesced */
-	if (driver) {
-		spin_unlock(&dev->lock);
+	if (driver)
 		driver->disconnect(&dev->gadget);
-		spin_lock(&dev->lock);
-	}
 
 	usb_reinit(dev);
 }
@@ -3348,8 +3345,6 @@ next_endpoints:
 		BIT(PCI_RETRY_ABORT_INTERRUPT))
 
 static void handle_stat1_irqs(struct net2280 *dev, u32 stat)
-__releases(dev->lock)
-__acquires(dev->lock)
 {
 	struct net2280_ep	*ep;
 	u32			tmp, num, mask, scratch;
@@ -3390,14 +3385,12 @@ __acquires(dev->lock)
 			if (disconnect || reset) {
 				stop_activity(dev, dev->driver);
 				ep0_start(dev);
-				spin_unlock(&dev->lock);
 				if (reset)
 					usb_gadget_udc_reset
 						(&dev->gadget, dev->driver);
 				else
 					(dev->driver->disconnect)
 						(&dev->gadget);
-				spin_lock(&dev->lock);
 				return;
 			}
 		}
@@ -3573,7 +3566,6 @@ static void net2280_remove(struct pci_dev *pdev)
 	BUG_ON(dev->driver);
 
 	/* then clean up the resources we allocated during probe() */
-	net2280_led_shutdown(dev);
 	if (dev->requests) {
 		int		i;
 		for (i = 1; i < 5; i++) {
@@ -3588,8 +3580,10 @@ static void net2280_remove(struct pci_dev *pdev)
 		free_irq(pdev->irq, dev);
 	if (dev->quirks & PLX_PCIE)
 		pci_disable_msi(pdev);
-	if (dev->regs)
+	if (dev->regs) {
+		net2280_led_shutdown(dev);
 		iounmap(dev->regs);
+	}
 	if (dev->region)
 		release_mem_region(pci_resource_start(pdev, 0),
 				pci_resource_len(pdev, 0));
diff --git a/drivers/usb/gadget/udc/renesas_usb3.c b/drivers/usb/gadget/udc/renesas_usb3.c
index cd4c88529721..d8278322d5ac 100644
--- a/drivers/usb/gadget/udc/renesas_usb3.c
+++ b/drivers/usb/gadget/udc/renesas_usb3.c
@@ -9,6 +9,7 @@
  */
 
 #include <linux/delay.h>
+#include <linux/dma-mapping.h>
 #include <linux/err.h>
 #include <linux/extcon.h>
 #include <linux/interrupt.h>
@@ -27,6 +28,8 @@
 #define USB3_AXI_INT_ENA	0x00c
 #define USB3_DMA_INT_STA	0x010
 #define USB3_DMA_INT_ENA	0x014
+#define USB3_DMA_CH0_CON(n)	(0x030 + ((n) - 1) * 0x10) /* n = 1 to 4 */
+#define USB3_DMA_CH0_PRD_ADR(n)	(0x034 + ((n) - 1) * 0x10) /* n = 1 to 4 */
 #define USB3_USB_COM_CON	0x200
 #define USB3_USB20_CON		0x204
 #define USB3_USB30_CON		0x208
@@ -64,6 +67,22 @@
 /* AXI_INT_ENA and AXI_INT_STA */
 #define AXI_INT_DMAINT		BIT(31)
 #define AXI_INT_EPCINT		BIT(30)
+/* PRD's n = from 1 to 4 */
+#define AXI_INT_PRDEN_CLR_STA_SHIFT(n)	(16 + (n) - 1)
+#define AXI_INT_PRDERR_STA_SHIFT(n)	(0 + (n) - 1)
+#define AXI_INT_PRDEN_CLR_STA(n)	(1 << AXI_INT_PRDEN_CLR_STA_SHIFT(n))
+#define AXI_INT_PRDERR_STA(n)		(1 << AXI_INT_PRDERR_STA_SHIFT(n))
+
+/* DMA_INT_ENA and DMA_INT_STA */
+#define DMA_INT(n)		BIT(n)
+
+/* DMA_CH0_CONn */
+#define DMA_CON_PIPE_DIR	BIT(15)		/* 1: In Transfer */
+#define DMA_CON_PIPE_NO_SHIFT	8
+#define DMA_CON_PIPE_NO_MASK	GENMASK(12, DMA_CON_PIPE_NO_SHIFT)
+#define DMA_COM_PIPE_NO(n)	(((n) << DMA_CON_PIPE_NO_SHIFT) & \
+					 DMA_CON_PIPE_NO_MASK)
+#define DMA_CON_PRD_EN		BIT(0)
 
 /* LCLKSEL */
 #define LCLKSEL_LSEL		BIT(18)
@@ -231,8 +250,50 @@
 #define USB3_EP0_BUF_SIZE		8
 #define USB3_MAX_NUM_PIPES		30
 #define USB3_WAIT_US			3
+#define USB3_DMA_NUM_SETTING_AREA	4
+/*
+ * To avoid double-meaning of "0" (xferred 65536 bytes or received zlp if
+ * buffer size is 65536), this driver uses the maximum size per a entry is
+ * 32768 bytes.
+ */
+#define USB3_DMA_MAX_XFER_SIZE		32768
+#define USB3_DMA_PRD_SIZE		4096
 
 struct renesas_usb3;
+
+/* Physical Region Descriptor Table */
+struct renesas_usb3_prd {
+	u32 word1;
+#define USB3_PRD1_E		BIT(30)		/* the end of chain */
+#define USB3_PRD1_U		BIT(29)		/* completion of transfer */
+#define USB3_PRD1_D		BIT(28)		/* Error occurred */
+#define USB3_PRD1_INT		BIT(27)		/* Interrupt occurred */
+#define USB3_PRD1_LST		BIT(26)		/* Last Packet */
+#define USB3_PRD1_B_INC		BIT(24)
+#define USB3_PRD1_MPS_8		0
+#define USB3_PRD1_MPS_16	BIT(21)
+#define USB3_PRD1_MPS_32	BIT(22)
+#define USB3_PRD1_MPS_64	(BIT(22) | BIT(21))
+#define USB3_PRD1_MPS_512	BIT(23)
+#define USB3_PRD1_MPS_1024	(BIT(23) | BIT(21))
+#define USB3_PRD1_MPS_RESERVED	(BIT(23) | BIT(22) | BIT(21))
+#define USB3_PRD1_SIZE_MASK	GENMASK(15, 0)
+
+	u32 bap;
+};
+#define USB3_DMA_NUM_PRD_ENTRIES	(USB3_DMA_PRD_SIZE / \
+					  sizeof(struct renesas_usb3_prd))
+#define USB3_DMA_MAX_XFER_SIZE_ALL_PRDS	(USB3_DMA_PRD_SIZE / \
+					 sizeof(struct renesas_usb3_prd) * \
+					 USB3_DMA_MAX_XFER_SIZE)
+
+struct renesas_usb3_dma {
+	struct renesas_usb3_prd *prd;
+	dma_addr_t prd_dma;
+	int num;	/* Setting area number (from 1 to 4) */
+	bool used;
+};
+
 struct renesas_usb3_request {
 	struct usb_request	req;
 	struct list_head	queue;
@@ -242,6 +303,7 @@ struct renesas_usb3_request {
 struct renesas_usb3_ep {
 	struct usb_ep ep;
 	struct renesas_usb3 *usb3;
+	struct renesas_usb3_dma *dma;
 	int num;
 	char ep_name[USB3_EP_NAME_SIZE];
 	struct list_head queue;
@@ -270,6 +332,8 @@ struct renesas_usb3 {
 	struct renesas_usb3_ep *usb3_ep;
 	int num_usb3_eps;
 
+	struct renesas_usb3_dma dma[USB3_DMA_NUM_SETTING_AREA];
+
 	spinlock_t lock;
 	int disabled_count;
 
@@ -298,8 +362,18 @@ struct renesas_usb3 {
 		     (i) < (usb3)->num_usb3_eps;		\
 		     (i)++, usb3_ep = usb3_get_ep(usb3, (i)))
 
+#define usb3_get_dma(usb3, i)	(&(usb3)->dma[i])
+#define usb3_for_each_dma(usb3, dma, i)				\
+		for ((i) = 0, dma = usb3_get_dma((usb3), (i));	\
+		     (i) < USB3_DMA_NUM_SETTING_AREA;		\
+		     (i)++, dma = usb3_get_dma((usb3), (i)))
+
 static const char udc_name[] = "renesas_usb3";
 
+static bool use_dma = 1;
+module_param(use_dma, bool, 0644);
+MODULE_PARM_DESC(use_dma, "use dedicated DMAC");
+
 static void usb3_write(struct renesas_usb3 *usb3, u32 data, u32 offs)
 {
 	iowrite32(data, usb3->reg + offs);
@@ -1059,6 +1133,273 @@ static void usb3_start_pipe0(struct renesas_usb3_ep *usb3_ep,
 	usb3_p0_xfer(usb3_ep, usb3_req);
 }
 
+static void usb3_enable_dma_pipen(struct renesas_usb3 *usb3)
+{
+	usb3_set_bit(usb3, PN_CON_DATAIF_EN, USB3_PN_CON);
+}
+
+static void usb3_disable_dma_pipen(struct renesas_usb3 *usb3)
+{
+	usb3_clear_bit(usb3, PN_CON_DATAIF_EN, USB3_PN_CON);
+}
+
+static void usb3_enable_dma_irq(struct renesas_usb3 *usb3, int num)
+{
+	usb3_set_bit(usb3, DMA_INT(num), USB3_DMA_INT_ENA);
+}
+
+static void usb3_disable_dma_irq(struct renesas_usb3 *usb3, int num)
+{
+	usb3_clear_bit(usb3, DMA_INT(num), USB3_DMA_INT_ENA);
+}
+
+static u32 usb3_dma_mps_to_prd_word1(struct renesas_usb3_ep *usb3_ep)
+{
+	switch (usb3_ep->ep.maxpacket) {
+	case 8:
+		return USB3_PRD1_MPS_8;
+	case 16:
+		return USB3_PRD1_MPS_16;
+	case 32:
+		return USB3_PRD1_MPS_32;
+	case 64:
+		return USB3_PRD1_MPS_64;
+	case 512:
+		return USB3_PRD1_MPS_512;
+	case 1024:
+		return USB3_PRD1_MPS_1024;
+	default:
+		return USB3_PRD1_MPS_RESERVED;
+	}
+}
+
+static bool usb3_dma_get_setting_area(struct renesas_usb3_ep *usb3_ep,
+				      struct renesas_usb3_request *usb3_req)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	struct renesas_usb3_dma *dma;
+	int i;
+	bool ret = false;
+
+	if (usb3_req->req.length > USB3_DMA_MAX_XFER_SIZE_ALL_PRDS) {
+		dev_dbg(usb3_to_dev(usb3), "%s: the length is too big (%d)\n",
+			__func__, usb3_req->req.length);
+		return false;
+	}
+
+	/* The driver doesn't handle zero-length packet via dmac */
+	if (!usb3_req->req.length)
+		return false;
+
+	if (usb3_dma_mps_to_prd_word1(usb3_ep) == USB3_PRD1_MPS_RESERVED)
+		return false;
+
+	usb3_for_each_dma(usb3, dma, i) {
+		if (dma->used)
+			continue;
+
+		if (usb_gadget_map_request(&usb3->gadget, &usb3_req->req,
+					   usb3_ep->dir_in) < 0)
+			break;
+
+		dma->used = true;
+		usb3_ep->dma = dma;
+		ret = true;
+		break;
+	}
+
+	return ret;
+}
+
+static void usb3_dma_put_setting_area(struct renesas_usb3_ep *usb3_ep,
+				      struct renesas_usb3_request *usb3_req)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	int i;
+	struct renesas_usb3_dma *dma;
+
+	usb3_for_each_dma(usb3, dma, i) {
+		if (usb3_ep->dma == dma) {
+			usb_gadget_unmap_request(&usb3->gadget, &usb3_req->req,
+						 usb3_ep->dir_in);
+			dma->used = false;
+			usb3_ep->dma = NULL;
+			break;
+		}
+	}
+}
+
+static void usb3_dma_fill_prd(struct renesas_usb3_ep *usb3_ep,
+			      struct renesas_usb3_request *usb3_req)
+{
+	struct renesas_usb3_prd *cur_prd = usb3_ep->dma->prd;
+	u32 remain = usb3_req->req.length;
+	u32 dma = usb3_req->req.dma;
+	u32 len;
+	int i = 0;
+
+	do {
+		len = min_t(u32, remain, USB3_DMA_MAX_XFER_SIZE) &
+			    USB3_PRD1_SIZE_MASK;
+		cur_prd->word1 = usb3_dma_mps_to_prd_word1(usb3_ep) |
+				 USB3_PRD1_B_INC | len;
+		cur_prd->bap = dma;
+		remain -= len;
+		dma += len;
+		if (!remain || (i + 1) < USB3_DMA_NUM_PRD_ENTRIES)
+			break;
+
+		cur_prd++;
+		i++;
+	} while (1);
+
+	cur_prd->word1 |= USB3_PRD1_E | USB3_PRD1_INT;
+	if (usb3_ep->dir_in)
+		cur_prd->word1 |= USB3_PRD1_LST;
+}
+
+static void usb3_dma_kick_prd(struct renesas_usb3_ep *usb3_ep)
+{
+	struct renesas_usb3_dma *dma = usb3_ep->dma;
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	u32 dma_con = DMA_COM_PIPE_NO(usb3_ep->num) | DMA_CON_PRD_EN;
+
+	if (usb3_ep->dir_in)
+		dma_con |= DMA_CON_PIPE_DIR;
+
+	wmb();	/* prd entries should be in system memory here */
+
+	usb3_write(usb3, 1 << usb3_ep->num, USB3_DMA_INT_STA);
+	usb3_write(usb3, AXI_INT_PRDEN_CLR_STA(dma->num) |
+		   AXI_INT_PRDERR_STA(dma->num), USB3_AXI_INT_STA);
+
+	usb3_write(usb3, dma->prd_dma, USB3_DMA_CH0_PRD_ADR(dma->num));
+	usb3_write(usb3, dma_con, USB3_DMA_CH0_CON(dma->num));
+	usb3_enable_dma_irq(usb3, usb3_ep->num);
+}
+
+static void usb3_dma_stop_prd(struct renesas_usb3_ep *usb3_ep)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	struct renesas_usb3_dma *dma = usb3_ep->dma;
+
+	usb3_disable_dma_irq(usb3, usb3_ep->num);
+	usb3_write(usb3, 0, USB3_DMA_CH0_CON(dma->num));
+}
+
+static int usb3_dma_update_status(struct renesas_usb3_ep *usb3_ep,
+				  struct renesas_usb3_request *usb3_req)
+{
+	struct renesas_usb3_prd *cur_prd = usb3_ep->dma->prd;
+	struct usb_request *req = &usb3_req->req;
+	u32 remain, len;
+	int i = 0;
+	int status = 0;
+
+	rmb();	/* The controller updated prd entries */
+
+	do {
+		if (cur_prd->word1 & USB3_PRD1_D)
+			status = -EIO;
+		if (cur_prd->word1 & USB3_PRD1_E)
+			len = req->length % USB3_DMA_MAX_XFER_SIZE;
+		else
+			len = USB3_DMA_MAX_XFER_SIZE;
+		remain = cur_prd->word1 & USB3_PRD1_SIZE_MASK;
+		req->actual += len - remain;
+
+		if (cur_prd->word1 & USB3_PRD1_E ||
+		    (i + 1) < USB3_DMA_NUM_PRD_ENTRIES)
+			break;
+
+		cur_prd++;
+		i++;
+	} while (1);
+
+	return status;
+}
+
+static bool usb3_dma_try_start(struct renesas_usb3_ep *usb3_ep,
+			       struct renesas_usb3_request *usb3_req)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+
+	if (!use_dma)
+		return false;
+
+	if (usb3_dma_get_setting_area(usb3_ep, usb3_req)) {
+		usb3_pn_stop(usb3);
+		usb3_enable_dma_pipen(usb3);
+		usb3_dma_fill_prd(usb3_ep, usb3_req);
+		usb3_dma_kick_prd(usb3_ep);
+		usb3_pn_start(usb3);
+		return true;
+	}
+
+	return false;
+}
+
+static int usb3_dma_try_stop(struct renesas_usb3_ep *usb3_ep,
+			     struct renesas_usb3_request *usb3_req)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	unsigned long flags;
+	int status = 0;
+
+	spin_lock_irqsave(&usb3->lock, flags);
+	if (!usb3_ep->dma)
+		goto out;
+
+	if (!usb3_pn_change(usb3, usb3_ep->num))
+		usb3_disable_dma_pipen(usb3);
+	usb3_dma_stop_prd(usb3_ep);
+	status = usb3_dma_update_status(usb3_ep, usb3_req);
+	usb3_dma_put_setting_area(usb3_ep, usb3_req);
+
+out:
+	spin_unlock_irqrestore(&usb3->lock, flags);
+	return status;
+}
+
+static int renesas_usb3_dma_free_prd(struct renesas_usb3 *usb3,
+				     struct device *dev)
+{
+	int i;
+	struct renesas_usb3_dma *dma;
+
+	usb3_for_each_dma(usb3, dma, i) {
+		if (dma->prd) {
+			dma_free_coherent(dev, USB3_DMA_MAX_XFER_SIZE,
+					  dma->prd, dma->prd_dma);
+			dma->prd = NULL;
+		}
+	}
+
+	return 0;
+}
+
+static int renesas_usb3_dma_alloc_prd(struct renesas_usb3 *usb3,
+				      struct device *dev)
+{
+	int i;
+	struct renesas_usb3_dma *dma;
+
+	if (!use_dma)
+		return 0;
+
+	usb3_for_each_dma(usb3, dma, i) {
+		dma->prd = dma_alloc_coherent(dev, USB3_DMA_PRD_SIZE,
+					      &dma->prd_dma, GFP_KERNEL);
+		if (!dma->prd) {
+			renesas_usb3_dma_free_prd(usb3, dev);
+			return -ENOMEM;
+		}
+		dma->num = i + 1;
+	}
+
+	return 0;
+}
+
 static void usb3_start_pipen(struct renesas_usb3_ep *usb3_ep,
 			     struct renesas_usb3_request *usb3_req)
 {
@@ -1078,6 +1419,10 @@ static void usb3_start_pipen(struct renesas_usb3_ep *usb3_ep,
 		goto out;
 
 	usb3_ep->started = true;
+
+	if (usb3_dma_try_start(usb3_ep, usb3_req))
+		goto out;
+
 	usb3_pn_start(usb3);
 
 	if (usb3_ep->dir_in) {
@@ -1603,12 +1948,49 @@ static void usb3_irq_epc(struct renesas_usb3 *usb3)
 	}
 }
 
+static void usb3_irq_dma_int(struct renesas_usb3 *usb3, u32 dma_sta)
+{
+	struct renesas_usb3_ep *usb3_ep;
+	struct renesas_usb3_request *usb3_req;
+	int i, status;
+
+	for (i = 0; i < usb3->num_usb3_eps; i++) {
+		if (!(dma_sta & DMA_INT(i)))
+			continue;
+
+		usb3_ep = usb3_get_ep(usb3, i);
+		if (!(usb3_read(usb3, USB3_AXI_INT_STA) &
+		    AXI_INT_PRDEN_CLR_STA(usb3_ep->dma->num)))
+			continue;
+
+		usb3_req = usb3_get_request(usb3_ep);
+		status = usb3_dma_try_stop(usb3_ep, usb3_req);
+		usb3_request_done_pipen(usb3, usb3_ep, usb3_req, status);
+	}
+}
+
+static void usb3_irq_dma(struct renesas_usb3 *usb3)
+{
+	u32 dma_sta = usb3_read(usb3, USB3_DMA_INT_STA);
+
+	dma_sta &= usb3_read(usb3, USB3_DMA_INT_ENA);
+	if (dma_sta) {
+		usb3_write(usb3, dma_sta, USB3_DMA_INT_STA);
+		usb3_irq_dma_int(usb3, dma_sta);
+	}
+}
+
 static irqreturn_t renesas_usb3_irq(int irq, void *_usb3)
 {
 	struct renesas_usb3 *usb3 = _usb3;
 	irqreturn_t ret = IRQ_NONE;
 	u32 axi_int_sta = usb3_read(usb3, USB3_AXI_INT_STA);
 
+	if (axi_int_sta & AXI_INT_DMAINT) {
+		usb3_irq_dma(usb3);
+		ret = IRQ_HANDLED;
+	}
+
 	if (axi_int_sta & AXI_INT_EPCINT) {
 		usb3_irq_epc(usb3);
 		ret = IRQ_HANDLED;
@@ -1708,6 +2090,7 @@ static int renesas_usb3_ep_disable(struct usb_ep *_ep)
 		usb3_req = usb3_get_request(usb3_ep);
 		if (!usb3_req)
 			break;
+		usb3_dma_try_stop(usb3_ep, usb3_req);
 		usb3_request_done(usb3_ep, usb3_req, -ESHUTDOWN);
 	} while (1);
 
@@ -1755,6 +2138,7 @@ static int renesas_usb3_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 	dev_dbg(usb3_to_dev(usb3), "ep_dequeue: ep%2d, %u\n", usb3_ep->num,
 		_req->length);
 
+	usb3_dma_try_stop(usb3_ep, usb3_req);
 	usb3_request_done_pipen(usb3, usb3_ep, usb3_req, -ECONNRESET);
 
 	return 0;
@@ -1917,6 +2301,7 @@ static int renesas_usb3_remove(struct platform_device *pdev)
 	device_remove_file(&pdev->dev, &dev_attr_role);
 
 	usb_del_gadget_udc(&usb3->gadget);
+	renesas_usb3_dma_free_prd(usb3, &pdev->dev);
 
 	__renesas_usb3_ep_free_request(usb3->ep0_req);
 
@@ -2111,6 +2496,10 @@ static int renesas_usb3_probe(struct platform_device *pdev)
 	if (!usb3->ep0_req)
 		return -ENOMEM;
 
+	ret = renesas_usb3_dma_alloc_prd(usb3, &pdev->dev);
+	if (ret < 0)
+		goto err_alloc_prd;
+
 	ret = usb_add_gadget_udc(&pdev->dev, &usb3->gadget);
 	if (ret < 0)
 		goto err_add_udc;
@@ -2129,6 +2518,9 @@ err_dev_create:
 	usb_del_gadget_udc(&usb3->gadget);
 
 err_add_udc:
+	renesas_usb3_dma_free_prd(usb3, &pdev->dev);
+
+err_alloc_prd:
 	__renesas_usb3_ep_free_request(usb3->ep0_req);
 
 	return ret;
diff --git a/drivers/usb/gadget/udc/amd5536udc.c b/drivers/usb/gadget/udc/snps_udc_core.c
index 4ecd2f20ea48..38a165dbf924 100644
--- a/drivers/usb/gadget/udc/amd5536udc.c
+++ b/drivers/usb/gadget/udc/snps_udc_core.c
@@ -41,7 +41,6 @@
 #include "amd5536udc.h"
 
 static void udc_tasklet_disconnect(unsigned long);
-static void empty_req_queue(struct udc_ep *);
 static void udc_setup_endpoints(struct udc *dev);
 static void udc_soft_reset(struct udc *dev);
 static struct udc_request *udc_alloc_bna_dummy(struct udc_ep *ep);
@@ -209,18 +208,18 @@ static void print_regs(struct udc *dev)
 	if (use_dma && use_dma_ppb && !use_dma_ppb_du) {
 		DBG(dev, "DMA mode       = PPBNDU (packet per buffer "
 			"WITHOUT desc. update)\n");
-		dev_info(&dev->pdev->dev, "DMA mode (%s)\n", "PPBNDU");
+		dev_info(dev->dev, "DMA mode (%s)\n", "PPBNDU");
 	} else if (use_dma && use_dma_ppb && use_dma_ppb_du) {
 		DBG(dev, "DMA mode       = PPBDU (packet per buffer "
 			"WITH desc. update)\n");
-		dev_info(&dev->pdev->dev, "DMA mode (%s)\n", "PPBDU");
+		dev_info(dev->dev, "DMA mode (%s)\n", "PPBDU");
 	}
 	if (use_dma && use_dma_bufferfill_mode) {
 		DBG(dev, "DMA mode       = BF (buffer fill mode)\n");
-		dev_info(&dev->pdev->dev, "DMA mode (%s)\n", "BF");
+		dev_info(dev->dev, "DMA mode (%s)\n", "BF");
 	}
 	if (!use_dma)
-		dev_info(&dev->pdev->dev, "FIFO mode\n");
+		dev_info(dev->dev, "FIFO mode\n");
 	DBG(dev, "-------------------------------------------------------\n");
 }
 
@@ -1244,7 +1243,7 @@ finished:
 }
 
 /* Empty request queue of an endpoint; caller holds spinlock */
-static void empty_req_queue(struct udc_ep *ep)
+void empty_req_queue(struct udc_ep *ep)
 {
 	struct udc_request	*req;
 
@@ -1256,6 +1255,7 @@ static void empty_req_queue(struct udc_ep *ep)
 		complete_req(ep, req, -ESHUTDOWN);
 	}
 }
+EXPORT_SYMBOL_GPL(empty_req_queue);
 
 /* Dequeues a request packet, called by gadget driver */
 static int udc_dequeue(struct usb_ep *usbep, struct usb_request *usbreq)
@@ -1623,8 +1623,11 @@ static void udc_setup_endpoints(struct udc *dev)
 /* Bringup after Connect event, initial bringup to be ready for ep0 events */
 static void usb_connect(struct udc *dev)
 {
+	/* Return if already connected */
+	if (dev->connected)
+		return;
 
-	dev_info(&dev->pdev->dev, "USB Connect\n");
+	dev_info(dev->dev, "USB Connect\n");
 
 	dev->connected = 1;
 
@@ -1641,8 +1644,11 @@ static void usb_connect(struct udc *dev)
  */
 static void usb_disconnect(struct udc *dev)
 {
+	/* Return if already disconnected */
+	if (!dev->connected)
+		return;
 
-	dev_info(&dev->pdev->dev, "USB Disconnect\n");
+	dev_info(dev->dev, "USB Disconnect\n");
 
 	dev->connected = 0;
 
@@ -1715,11 +1721,15 @@ static void udc_soft_reset(struct udc *dev)
 	/* device int. status reset */
 	writel(UDC_DEV_MSK_DISABLE, &dev->regs->irqsts);
 
-	spin_lock_irqsave(&udc_irq_spinlock, flags);
-	writel(AMD_BIT(UDC_DEVCFG_SOFTRESET), &dev->regs->cfg);
-	readl(&dev->regs->cfg);
-	spin_unlock_irqrestore(&udc_irq_spinlock, flags);
-
+	/* Don't do this for Broadcom UDC since this is a reserved
+	 * bit.
+	 */
+	if (dev->chiprev != UDC_BCM_REV) {
+		spin_lock_irqsave(&udc_irq_spinlock, flags);
+		writel(AMD_BIT(UDC_DEVCFG_SOFTRESET), &dev->regs->cfg);
+		readl(&dev->regs->cfg);
+		spin_unlock_irqrestore(&udc_irq_spinlock, flags);
+	}
 }
 
 /* RDE timer callback to set RDE bit */
@@ -2106,7 +2116,7 @@ static irqreturn_t udc_data_out_isr(struct udc *dev, int ep_ix)
 	}
 	/* HE event ? */
 	if (tmp & AMD_BIT(UDC_EPSTS_HE)) {
-		dev_err(&dev->pdev->dev, "HE ep%dout occurred\n", ep->num);
+		dev_err(dev->dev, "HE ep%dout occurred\n", ep->num);
 
 		/* clear HE */
 		writel(tmp | AMD_BIT(UDC_EPSTS_HE), &ep->regs->sts);
@@ -2305,7 +2315,7 @@ static irqreturn_t udc_data_in_isr(struct udc *dev, int ep_ix)
 	if (use_dma) {
 		/* BNA ? */
 		if (epsts & AMD_BIT(UDC_EPSTS_BNA)) {
-			dev_err(&dev->pdev->dev,
+			dev_err(dev->dev,
 				"BNA ep%din occurred - DESPTR = %08lx\n",
 				ep->num,
 				(unsigned long) readl(&ep->regs->desptr));
@@ -2318,7 +2328,7 @@ static irqreturn_t udc_data_in_isr(struct udc *dev, int ep_ix)
 	}
 	/* HE event ? */
 	if (epsts & AMD_BIT(UDC_EPSTS_HE)) {
-		dev_err(&dev->pdev->dev,
+		dev_err(dev->dev,
 			"HE ep%dn occurred - DESPTR = %08lx\n",
 			ep->num, (unsigned long) readl(&ep->regs->desptr));
 
@@ -2956,7 +2966,7 @@ __acquires(dev->lock)
 
 		/* link up all endpoints */
 		udc_setup_endpoints(dev);
-		dev_info(&dev->pdev->dev, "Connect: %s\n",
+		dev_info(dev->dev, "Connect: %s\n",
 			 usb_speed_string(dev->gadget.speed));
 
 		/* init ep 0 */
@@ -3097,7 +3107,7 @@ int init_dma_pools(struct udc *dev)
 	}
 
 	/* DMA setup */
-	dev->data_requests = dma_pool_create("data_requests", NULL,
+	dev->data_requests = dma_pool_create("data_requests", dev->dev,
 		sizeof(struct udc_data_dma), 0, 0);
 	if (!dev->data_requests) {
 		DBG(dev, "can't get request data pool\n");
@@ -3108,7 +3118,7 @@ int init_dma_pools(struct udc *dev)
 	dev->ep[UDC_EP0IN_IX].dma = &dev->regs->ctl;
 
 	/* dma desc for setup data */
-	dev->stp_requests = dma_pool_create("setup requests", NULL,
+	dev->stp_requests = dma_pool_create("setup requests", dev->dev,
 		sizeof(struct udc_stp_dma), 0, 0);
 	if (!dev->stp_requests) {
 		DBG(dev, "can't get stp request pool\n");
@@ -3168,24 +3178,30 @@ int udc_probe(struct udc *dev)
 	/* init registers, interrupts, ... */
 	startup_registers(dev);
 
-	dev_info(&dev->pdev->dev, "%s\n", mod_desc);
+	dev_info(dev->dev, "%s\n", mod_desc);
 
 	snprintf(tmp, sizeof(tmp), "%d", dev->irq);
-	dev_info(&dev->pdev->dev,
-		 "irq %s, pci mem %08lx, chip rev %02x(Geode5536 %s)\n",
-		 tmp, dev->phys_addr, dev->chiprev,
-		 (dev->chiprev == UDC_HSA0_REV) ? "A0" : "B1");
-	strcpy(tmp, UDC_DRIVER_VERSION_STRING);
-	if (dev->chiprev == UDC_HSA0_REV) {
-		dev_err(&dev->pdev->dev, "chip revision is A0; too old\n");
-		retval = -ENODEV;
-		goto finished;
+
+	/* Print this device info for AMD chips only*/
+	if (dev->chiprev == UDC_HSA0_REV ||
+	    dev->chiprev == UDC_HSB1_REV) {
+		dev_info(dev->dev, "irq %s, pci mem %08lx, chip rev %02x(Geode5536 %s)\n",
+			 tmp, dev->phys_addr, dev->chiprev,
+			 (dev->chiprev == UDC_HSA0_REV) ?
+			 "A0" : "B1");
+		strcpy(tmp, UDC_DRIVER_VERSION_STRING);
+		if (dev->chiprev == UDC_HSA0_REV) {
+			dev_err(dev->dev, "chip revision is A0; too old\n");
+			retval = -ENODEV;
+			goto finished;
+		}
+		dev_info(dev->dev,
+			 "driver version: %s(for Geode5536 B1)\n", tmp);
 	}
-	dev_info(&dev->pdev->dev,
-		 "driver version: %s(for Geode5536 B1)\n", tmp);
+
 	udc = dev;
 
-	retval = usb_add_gadget_udc_release(&udc->pdev->dev, &dev->gadget,
+	retval = usb_add_gadget_udc_release(udc->dev, &dev->gadget,
 					    gadget_release);
 	if (retval)
 		goto finished;
diff --git a/drivers/usb/gadget/udc/snps_udc_plat.c b/drivers/usb/gadget/udc/snps_udc_plat.c
new file mode 100644
index 000000000000..2e11f19e07ae
--- /dev/null
+++ b/drivers/usb/gadget/udc/snps_udc_plat.c
@@ -0,0 +1,344 @@
+/*
+ * snps_udc_plat.c - Synopsys UDC Platform Driver
+ *
+ * Copyright (C) 2016 Broadcom
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation 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/extcon.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_gpio.h>
+#include <linux/platform_device.h>
+#include <linux/phy/phy.h>
+#include <linux/module.h>
+#include <linux/dmapool.h>
+#include <linux/interrupt.h>
+#include <linux/moduleparam.h>
+#include "amd5536udc.h"
+
+/* description */
+#define UDC_MOD_DESCRIPTION     "Synopsys UDC platform driver"
+
+void start_udc(struct udc *udc)
+{
+	if (udc->driver) {
+		dev_info(udc->dev, "Connecting...\n");
+		udc_enable_dev_setup_interrupts(udc);
+		udc_basic_init(udc);
+		udc->connected = 1;
+	}
+}
+
+void stop_udc(struct udc *udc)
+{
+	int tmp;
+	u32 reg;
+
+	spin_lock(&udc->lock);
+
+	/* Flush the receieve fifo */
+	reg = readl(&udc->regs->ctl);
+	reg |= AMD_BIT(UDC_DEVCTL_SRX_FLUSH);
+	writel(reg, &udc->regs->ctl);
+
+	reg = readl(&udc->regs->ctl);
+	reg &= ~(AMD_BIT(UDC_DEVCTL_SRX_FLUSH));
+	writel(reg, &udc->regs->ctl);
+	dev_dbg(udc->dev, "ep rx queue flushed\n");
+
+	/* Mask interrupts. Required more so when the
+	 * UDC is connected to a DRD phy.
+	 */
+	udc_mask_unused_interrupts(udc);
+
+	/* Disconnect gadget driver */
+	if (udc->driver) {
+		spin_unlock(&udc->lock);
+		udc->driver->disconnect(&udc->gadget);
+		spin_lock(&udc->lock);
+
+		/* empty queues */
+		for (tmp = 0; tmp < UDC_EP_NUM; tmp++)
+			empty_req_queue(&udc->ep[tmp]);
+	}
+	udc->connected = 0;
+
+	spin_unlock(&udc->lock);
+	dev_info(udc->dev, "Device disconnected\n");
+}
+
+void udc_drd_work(struct work_struct *work)
+{
+	struct udc *udc;
+
+	udc = container_of(to_delayed_work(work),
+			   struct udc, drd_work);
+
+	if (udc->conn_type) {
+		dev_dbg(udc->dev, "idle -> device\n");
+		start_udc(udc);
+	} else {
+		dev_dbg(udc->dev, "device -> idle\n");
+		stop_udc(udc);
+	}
+}
+
+static int usbd_connect_notify(struct notifier_block *self,
+			       unsigned long event, void *ptr)
+{
+	struct udc *udc = container_of(self, struct udc, nb);
+
+	dev_dbg(udc->dev, "%s: event: %lu\n", __func__, event);
+
+	udc->conn_type = event;
+
+	schedule_delayed_work(&udc->drd_work, 0);
+
+	return NOTIFY_OK;
+}
+
+static int udc_plat_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	struct udc *udc;
+	int ret;
+
+	udc = devm_kzalloc(dev, sizeof(*udc), GFP_KERNEL);
+	if (!udc)
+		return -ENOMEM;
+
+	spin_lock_init(&udc->lock);
+	udc->dev = dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	udc->virt_addr = devm_ioremap_resource(dev, res);
+	if (IS_ERR(udc->regs))
+		return PTR_ERR(udc->regs);
+
+	/* udc csr registers base */
+	udc->csr = udc->virt_addr + UDC_CSR_ADDR;
+
+	/* dev registers base */
+	udc->regs = udc->virt_addr + UDC_DEVCFG_ADDR;
+
+	/* ep registers base */
+	udc->ep_regs = udc->virt_addr + UDC_EPREGS_ADDR;
+
+	/* fifo's base */
+	udc->rxfifo = (u32 __iomem *)(udc->virt_addr + UDC_RXFIFO_ADDR);
+	udc->txfifo = (u32 __iomem *)(udc->virt_addr + UDC_TXFIFO_ADDR);
+
+	udc->phys_addr = (unsigned long)res->start;
+
+	udc->irq = irq_of_parse_and_map(dev->of_node, 0);
+	if (udc->irq <= 0) {
+		dev_err(dev, "Can't parse and map interrupt\n");
+		return -EINVAL;
+	}
+
+	udc->udc_phy = devm_of_phy_get_by_index(dev, dev->of_node, 0);
+	if (IS_ERR(udc->udc_phy)) {
+		dev_err(dev, "Failed to obtain phy from device tree\n");
+		return PTR_ERR(udc->udc_phy);
+	}
+
+	ret = phy_init(udc->udc_phy);
+	if (ret) {
+		dev_err(dev, "UDC phy init failed");
+		return ret;
+	}
+
+	ret = phy_power_on(udc->udc_phy);
+	if (ret) {
+		dev_err(dev, "UDC phy power on failed");
+		phy_exit(udc->udc_phy);
+		return ret;
+	}
+
+	/* Register for extcon if supported */
+	if (of_get_property(dev->of_node, "extcon", NULL)) {
+		udc->edev = extcon_get_edev_by_phandle(dev, 0);
+		if (IS_ERR(udc->edev)) {
+			if (PTR_ERR(udc->edev) == -EPROBE_DEFER)
+				return -EPROBE_DEFER;
+			dev_err(dev, "Invalid or missing extcon\n");
+			ret = PTR_ERR(udc->edev);
+			goto exit_phy;
+		}
+
+		udc->nb.notifier_call = usbd_connect_notify;
+		ret = extcon_register_notifier(udc->edev, EXTCON_USB,
+					       &udc->nb);
+		if (ret < 0) {
+			dev_err(dev, "Can't register extcon device\n");
+			goto exit_phy;
+		}
+
+		ret = extcon_get_cable_state_(udc->edev, EXTCON_USB);
+		if (ret < 0) {
+			dev_err(dev, "Can't get cable state\n");
+			goto exit_extcon;
+		} else if (ret) {
+			udc->conn_type = ret;
+		}
+		INIT_DELAYED_WORK(&udc->drd_work, udc_drd_work);
+	}
+
+	/* init dma pools */
+	if (use_dma) {
+		ret = init_dma_pools(udc);
+		if (ret != 0)
+			goto exit_extcon;
+	}
+
+	ret = devm_request_irq(dev, udc->irq, udc_irq, IRQF_SHARED,
+			       "snps-udc", udc);
+	if (ret < 0) {
+		dev_err(dev, "Request irq %d failed for UDC\n", udc->irq);
+		goto exit_dma;
+	}
+
+	platform_set_drvdata(pdev, udc);
+	udc->chiprev = UDC_BCM_REV;
+
+	if (udc_probe(udc)) {
+		ret = -ENODEV;
+		goto exit_dma;
+	}
+	dev_info(dev, "Synopsys UDC platform driver probe successful\n");
+
+	return 0;
+
+exit_dma:
+	if (use_dma)
+		free_dma_pools(udc);
+exit_extcon:
+	if (udc->edev)
+		extcon_unregister_notifier(udc->edev, EXTCON_USB, &udc->nb);
+exit_phy:
+	if (udc->udc_phy) {
+		phy_power_off(udc->udc_phy);
+		phy_exit(udc->udc_phy);
+	}
+	return ret;
+}
+
+static int udc_plat_remove(struct platform_device *pdev)
+{
+	struct udc *dev;
+
+	dev = platform_get_drvdata(pdev);
+
+	usb_del_gadget_udc(&dev->gadget);
+	/* gadget driver must not be registered */
+	if (WARN_ON(dev->driver))
+		return 0;
+
+	/* dma pool cleanup */
+	free_dma_pools(dev);
+
+	udc_remove(dev);
+
+	platform_set_drvdata(pdev, NULL);
+
+	if (dev->drd_wq) {
+		flush_workqueue(dev->drd_wq);
+		destroy_workqueue(dev->drd_wq);
+	}
+
+	phy_power_off(dev->udc_phy);
+	phy_exit(dev->udc_phy);
+	extcon_unregister_notifier(dev->edev, EXTCON_USB, &dev->nb);
+
+	dev_info(&pdev->dev, "Synopsys UDC platform driver removed\n");
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int udc_plat_suspend(struct device *dev)
+{
+	struct udc *udc;
+
+	udc = dev_get_drvdata(dev);
+	stop_udc(udc);
+
+	if (extcon_get_cable_state_(udc->edev, EXTCON_USB) > 0) {
+		dev_dbg(udc->dev, "device -> idle\n");
+		stop_udc(udc);
+	}
+	phy_power_off(udc->udc_phy);
+	phy_exit(udc->udc_phy);
+
+	return 0;
+}
+
+static int udc_plat_resume(struct device *dev)
+{
+	struct udc *udc;
+	int ret;
+
+	udc = dev_get_drvdata(dev);
+
+	ret = phy_init(udc->udc_phy);
+	if (ret) {
+		dev_err(udc->dev, "UDC phy init failure");
+		return ret;
+	}
+
+	ret = phy_power_on(udc->udc_phy);
+	if (ret) {
+		dev_err(udc->dev, "UDC phy power on failure");
+		phy_exit(udc->udc_phy);
+		return ret;
+	}
+
+	if (extcon_get_cable_state_(udc->edev, EXTCON_USB) > 0) {
+		dev_dbg(udc->dev, "idle -> device\n");
+		start_udc(udc);
+	}
+
+	return 0;
+}
+static const struct dev_pm_ops udc_plat_pm_ops = {
+	.suspend	= udc_plat_suspend,
+	.resume		= udc_plat_resume,
+};
+#endif
+
+#if defined(CONFIG_OF)
+static const struct of_device_id of_udc_match[] = {
+	{ .compatible = "brcm,ns2-udc", },
+	{ .compatible = "brcm,cygnus-udc", },
+	{ .compatible = "brcm,iproc-udc", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, of_udc_match);
+#endif
+
+static struct platform_driver udc_plat_driver = {
+	.probe		= udc_plat_probe,
+	.remove		= udc_plat_remove,
+	.driver		= {
+		.name	= "snps-udc-plat",
+		.of_match_table = of_match_ptr(of_udc_match),
+#ifdef CONFIG_PM_SLEEP
+		.pm	= &udc_plat_pm_ops,
+#endif
+	},
+};
+module_platform_driver(udc_plat_driver);
+
+MODULE_DESCRIPTION(UDC_MOD_DESCRIPTION);
+MODULE_AUTHOR("Broadcom");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/usb/gadget/udc/udc-xilinx.c b/drivers/usb/gadget/udc/udc-xilinx.c
index 588e2531b8b8..de207a90571e 100644
--- a/drivers/usb/gadget/udc/udc-xilinx.c
+++ b/drivers/usb/gadget/udc/udc-xilinx.c
@@ -1151,7 +1151,7 @@ static int xudc_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 			break;
 	}
 	if (&req->usb_req != _req) {
-		spin_unlock_irqrestore(&ep->udc->lock, flags);
+		spin_unlock_irqrestore(&udc->lock, flags);
 		return -EINVAL;
 	}
 	xudc_done(ep, req, -ECONNRESET);
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index ababb91d654a..fa5692dec832 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -473,8 +473,12 @@ config USB_OHCI_HCD_AT91
 config USB_OHCI_HCD_OMAP3
 	tristate "OHCI support for OMAP3 and later chips"
 	depends on (ARCH_OMAP3 || ARCH_OMAP4 || SOC_OMAP5)
+	select USB_OHCI_HCD_PLATFORM
 	default y
-	---help---
+	help
+	  This option is deprecated now and the driver was removed, use
+	  USB_OHCI_HCD_PLATFORM instead.
+
 	  Enables support for the on-chip OHCI controller on
 	  OMAP3 and later chips.
 
@@ -627,7 +631,11 @@ config USB_UHCI_SUPPORT_NON_PCI_HC
 
 config USB_UHCI_PLATFORM
 	bool
-	default y if ARCH_VT8500
+	default y if (ARCH_VT8500 || ARCH_ASPEED)
+
+config USB_UHCI_ASPEED
+       bool
+       default y if ARCH_ASPEED
 
 config USB_UHCI_BIG_ENDIAN_MMIO
 	bool
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index c77b0a38557b..cf2691fffcc0 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -52,7 +52,6 @@ obj-$(CONFIG_USB_OHCI_HCD_PCI)	+= ohci-pci.o
 obj-$(CONFIG_USB_OHCI_HCD_PLATFORM)	+= ohci-platform.o
 obj-$(CONFIG_USB_OHCI_EXYNOS)	+= ohci-exynos.o
 obj-$(CONFIG_USB_OHCI_HCD_OMAP1)	+= ohci-omap.o
-obj-$(CONFIG_USB_OHCI_HCD_OMAP3)	+= ohci-omap3.o
 obj-$(CONFIG_USB_OHCI_HCD_SPEAR)	+= ohci-spear.o
 obj-$(CONFIG_USB_OHCI_HCD_STI)	+= ohci-st.o
 obj-$(CONFIG_USB_OHCI_HCD_AT91)	+= ohci-at91.o
diff --git a/drivers/usb/host/ehci-exynos.c b/drivers/usb/host/ehci-exynos.c
index 7a603f66a9bc..26b641100639 100644
--- a/drivers/usb/host/ehci-exynos.c
+++ b/drivers/usb/host/ehci-exynos.c
@@ -279,7 +279,9 @@ static int exynos_ehci_resume(struct device *dev)
 	struct exynos_ehci_hcd *exynos_ehci = to_exynos_ehci(hcd);
 	int ret;
 
-	clk_prepare_enable(exynos_ehci->clk);
+	ret = clk_prepare_enable(exynos_ehci->clk);
+	if (ret)
+		return ret;
 
 	ret = exynos_ehci_phy_enable(dev);
 	if (ret) {
diff --git a/drivers/usb/host/ehci-sched.c b/drivers/usb/host/ehci-sched.c
index 980a6b3b2da2..6bc6304672bc 100644
--- a/drivers/usb/host/ehci-sched.c
+++ b/drivers/usb/host/ehci-sched.c
@@ -1105,7 +1105,7 @@ iso_stream_init(
 		addr |= epnum << 8;
 		addr |= dev->devnum;
 		stream->ps.usecs = HS_USECS_ISO(maxp);
-		think_time = dev->tt ? dev->tt->think_time : 0;
+		think_time = dev->tt->think_time;
 		stream->ps.tt_usecs = NS_TO_US(think_time + usb_calc_bus_time(
 				dev->speed, is_input, 1, maxp));
 		hs_transfers = max(1u, (maxp + 187) / 188);
diff --git a/drivers/usb/host/ehci-timer.c b/drivers/usb/host/ehci-timer.c
index 3893b5bafd87..0b6cdb723192 100644
--- a/drivers/usb/host/ehci-timer.c
+++ b/drivers/usb/host/ehci-timer.c
@@ -424,7 +424,7 @@ static enum hrtimer_restart ehci_hrtimer_func(struct hrtimer *t)
 	 */
 	now = ktime_get();
 	for_each_set_bit(e, &events, EHCI_HRTIMER_NUM_EVENTS) {
-		if (now >= ehci->hr_timeouts[e])
+		if (ktime_compare(now, ehci->hr_timeouts[e]) >= 0)
 			event_handlers[e](ehci);
 		else
 			ehci_enable_event(ehci, e, false);
diff --git a/drivers/usb/host/fotg210-hcd.c b/drivers/usb/host/fotg210-hcd.c
index ced08dc229ad..457cc6525abd 100644
--- a/drivers/usb/host/fotg210-hcd.c
+++ b/drivers/usb/host/fotg210-hcd.c
@@ -1380,7 +1380,7 @@ static enum hrtimer_restart fotg210_hrtimer_func(struct hrtimer *t)
 	 */
 	now = ktime_get();
 	for_each_set_bit(e, &events, FOTG210_HRTIMER_NUM_EVENTS) {
-		if (now >= fotg210->hr_timeouts[e])
+		if (ktime_compare(now, fotg210->hr_timeouts[e]) >= 0)
 			event_handlers[e](fotg210);
 		else
 			fotg210_enable_event(fotg210, e, false);
diff --git a/drivers/usb/host/ohci-omap3.c b/drivers/usb/host/ohci-omap3.c
deleted file mode 100644
index ec15aebe8786..000000000000
--- a/drivers/usb/host/ohci-omap3.c
+++ /dev/null
@@ -1,211 +0,0 @@
-/*
- * ohci-omap3.c - driver for OHCI on OMAP3 and later processors
- *
- * Bus Glue for OMAP3 USBHOST 3 port OHCI controller
- * This controller is also used in later OMAPs and AM35x chips
- *
- * Copyright (C) 2007-2010 Texas Instruments, Inc.
- * Author: Vikram Pandita <vikram.pandita@ti.com>
- * Author: Anand Gadiyar <gadiyar@ti.com>
- * Author: Keshava Munegowda <keshava_mgowda@ti.com>
- *
- * Based on ehci-omap.c and some other ohci glue layers
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the 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
- *
- * TODO (last updated Feb 27, 2011):
- *	- add kernel-doc
- */
-
-#include <linux/dma-mapping.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/of.h>
-#include <linux/usb/otg.h>
-#include <linux/platform_device.h>
-#include <linux/pm_runtime.h>
-#include <linux/usb.h>
-#include <linux/usb/hcd.h>
-
-#include "ohci.h"
-
-#define DRIVER_DESC "OHCI OMAP3 driver"
-
-static const char hcd_name[] = "ohci-omap3";
-static struct hc_driver __read_mostly ohci_omap3_hc_driver;
-
-/*
- * configure so an HC device and id are always provided
- * always called with process context; sleeping is OK
- */
-
-/**
- * ohci_hcd_omap3_probe - initialize OMAP-based HCDs
- *
- * Allocates basic resources for this USB host controller, and
- * then invokes the start() method for the HCD associated with it
- * through the hotplug entry's driver_data.
- */
-static int ohci_hcd_omap3_probe(struct platform_device *pdev)
-{
-	struct device		*dev = &pdev->dev;
-	struct ohci_hcd		*ohci;
-	struct usb_hcd		*hcd = NULL;
-	void __iomem		*regs = NULL;
-	struct resource		*res;
-	int			ret;
-	int			irq;
-
-	if (usb_disabled())
-		return -ENODEV;
-
-	if (!dev->parent) {
-		dev_err(dev, "Missing parent device\n");
-		return -ENODEV;
-	}
-
-	irq = platform_get_irq(pdev, 0);
-	if (irq < 0) {
-		dev_err(dev, "OHCI irq failed\n");
-		return -ENODEV;
-	}
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(dev, "UHH OHCI get resource failed\n");
-		return -ENOMEM;
-	}
-
-	regs = ioremap(res->start, resource_size(res));
-	if (!regs) {
-		dev_err(dev, "UHH OHCI ioremap failed\n");
-		return -ENOMEM;
-	}
-
-	/*
-	 * 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 have dma capability bindings this can go away.
-	 */
-	ret = dma_coerce_mask_and_coherent(dev, DMA_BIT_MASK(32));
-	if (ret)
-		goto err_io;
-
-	ret = -ENODEV;
-	hcd = usb_create_hcd(&ohci_omap3_hc_driver, dev,
-			dev_name(dev));
-	if (!hcd) {
-		dev_err(dev, "usb_create_hcd failed\n");
-		goto err_io;
-	}
-
-	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = resource_size(res);
-	hcd->regs =  regs;
-
-	pm_runtime_enable(dev);
-	pm_runtime_get_sync(dev);
-
-	ohci = hcd_to_ohci(hcd);
-	/*
-	 * RemoteWakeupConnected has to be set explicitly before
-	 * calling ohci_run. The reset value of RWC is 0.
-	 */
-	ohci->hc_control = OHCI_CTRL_RWC;
-
-	ret = usb_add_hcd(hcd, irq, 0);
-	if (ret) {
-		dev_dbg(dev, "failed to add hcd with err %d\n", ret);
-		goto err_add_hcd;
-	}
-	device_wakeup_enable(hcd->self.controller);
-
-	return 0;
-
-err_add_hcd:
-	pm_runtime_put_sync(dev);
-	usb_put_hcd(hcd);
-
-err_io:
-	iounmap(regs);
-
-	return ret;
-}
-
-/*
- * may be called without controller electrically present
- * may be called with controller, bus, and devices active
- */
-
-/**
- * ohci_hcd_omap3_remove - shutdown processing for OHCI HCDs
- * @pdev: USB Host Controller being removed
- *
- * Reverses the effect of ohci_hcd_omap3_probe(), first invoking
- * the HCD's stop() method.  It is always called from a thread
- * context, normally "rmmod", "apmd", or something similar.
- */
-static int ohci_hcd_omap3_remove(struct platform_device *pdev)
-{
-	struct device *dev	= &pdev->dev;
-	struct usb_hcd *hcd	= dev_get_drvdata(dev);
-
-	iounmap(hcd->regs);
-	usb_remove_hcd(hcd);
-	pm_runtime_put_sync(dev);
-	pm_runtime_disable(dev);
-	usb_put_hcd(hcd);
-	return 0;
-}
-
-static const struct of_device_id omap_ohci_dt_ids[] = {
-	{ .compatible = "ti,ohci-omap3" },
-	{ }
-};
-
-MODULE_DEVICE_TABLE(of, omap_ohci_dt_ids);
-
-static struct platform_driver ohci_hcd_omap3_driver = {
-	.probe		= ohci_hcd_omap3_probe,
-	.remove		= ohci_hcd_omap3_remove,
-	.shutdown	= usb_hcd_platform_shutdown,
-	.driver		= {
-		.name	= "ohci-omap3",
-		.of_match_table = omap_ohci_dt_ids,
-	},
-};
-
-static int __init ohci_omap3_init(void)
-{
-	if (usb_disabled())
-		return -ENODEV;
-
-	pr_info("%s: " DRIVER_DESC "\n", hcd_name);
-
-	ohci_init_driver(&ohci_omap3_hc_driver, NULL);
-	return platform_driver_register(&ohci_hcd_omap3_driver);
-}
-module_init(ohci_omap3_init);
-
-static void __exit ohci_omap3_cleanup(void)
-{
-	platform_driver_unregister(&ohci_hcd_omap3_driver);
-}
-module_exit(ohci_omap3_cleanup);
-
-MODULE_DESCRIPTION(DRIVER_DESC);
-MODULE_ALIAS("platform:ohci-omap3");
-MODULE_AUTHOR("Anand Gadiyar <gadiyar@ti.com>");
-MODULE_LICENSE("GPL");
diff --git a/drivers/usb/host/ohci-platform.c b/drivers/usb/host/ohci-platform.c
index 6368fce43197..61fe2b985070 100644
--- a/drivers/usb/host/ohci-platform.c
+++ b/drivers/usb/host/ohci-platform.c
@@ -24,6 +24,7 @@
 #include <linux/err.h>
 #include <linux/phy/phy.h>
 #include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
 #include <linux/reset.h>
 #include <linux/usb/ohci_pdriver.h>
 #include <linux/usb.h>
@@ -163,6 +164,10 @@ static int ohci_platform_probe(struct platform_device *dev)
 		if (of_property_read_bool(dev->dev.of_node, "no-big-frame-no"))
 			ohci->flags |= OHCI_QUIRK_FRAME_NO;
 
+		if (of_property_read_bool(dev->dev.of_node,
+					  "remote-wakeup-connected"))
+			ohci->hc_control = OHCI_CTRL_RWC;
+
 		of_property_read_u32(dev->dev.of_node, "num-ports",
 				     &ohci->num_ports);
 
@@ -242,6 +247,8 @@ static int ohci_platform_probe(struct platform_device *dev)
 	}
 #endif
 
+	pm_runtime_set_active(&dev->dev);
+	pm_runtime_enable(&dev->dev);
 	if (pdata->power_on) {
 		err = pdata->power_on(dev);
 		if (err < 0)
@@ -271,6 +278,7 @@ err_power:
 	if (pdata->power_off)
 		pdata->power_off(dev);
 err_reset:
+	pm_runtime_disable(&dev->dev);
 	while (--rst >= 0)
 		reset_control_assert(priv->resets[rst]);
 err_put_clks:
@@ -292,6 +300,7 @@ static int ohci_platform_remove(struct platform_device *dev)
 	struct ohci_platform_priv *priv = hcd_to_ohci_priv(hcd);
 	int clk, rst;
 
+	pm_runtime_get_sync(&dev->dev);
 	usb_remove_hcd(hcd);
 
 	if (pdata->power_off)
@@ -305,6 +314,9 @@ static int ohci_platform_remove(struct platform_device *dev)
 
 	usb_put_hcd(hcd);
 
+	pm_runtime_put_sync(&dev->dev);
+	pm_runtime_disable(&dev->dev);
+
 	if (pdata == &ohci_platform_defaults)
 		dev->dev.platform_data = NULL;
 
@@ -350,6 +362,7 @@ static int ohci_platform_resume(struct device *dev)
 static const struct of_device_id ohci_platform_ids[] = {
 	{ .compatible = "generic-ohci", },
 	{ .compatible = "cavium,octeon-6335-ohci", },
+	{ .compatible = "ti,ohci-omap3", },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, ohci_platform_ids);
diff --git a/drivers/usb/host/ohci-pxa27x.c b/drivers/usb/host/ohci-pxa27x.c
index 79efde8f21e0..21c010ffb03c 100644
--- a/drivers/usb/host/ohci-pxa27x.c
+++ b/drivers/usb/host/ohci-pxa27x.c
@@ -274,14 +274,16 @@ extern void pxa27x_clear_otgph(void);
 
 static int pxa27x_start_hc(struct pxa27x_ohci *pxa_ohci, struct device *dev)
 {
-	int retval = 0;
+	int retval;
 	struct pxaohci_platform_data *inf;
 	uint32_t uhchr;
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
 
 	inf = dev_get_platdata(dev);
 
-	clk_prepare_enable(pxa_ohci->clk);
+	retval = clk_prepare_enable(pxa_ohci->clk);
+	if (retval)
+		return retval;
 
 	pxa27x_reset_hc(pxa_ohci);
 
@@ -296,8 +298,10 @@ static int pxa27x_start_hc(struct pxa27x_ohci *pxa_ohci, struct device *dev)
 	if (inf->init)
 		retval = inf->init(dev);
 
-	if (retval < 0)
+	if (retval < 0) {
+		clk_disable_unprepare(pxa_ohci->clk);
 		return retval;
+	}
 
 	if (cpu_is_pxa3xx())
 		pxa3xx_u2d_start_hc(&hcd->self);
diff --git a/drivers/usb/host/uhci-hcd.c b/drivers/usb/host/uhci-hcd.c
index 94b150196d4f..c3267a78c94e 100644
--- a/drivers/usb/host/uhci-hcd.c
+++ b/drivers/usb/host/uhci-hcd.c
@@ -265,9 +265,13 @@ static void configure_hc(struct uhci_hcd *uhci)
 
 static int resume_detect_interrupts_are_broken(struct uhci_hcd *uhci)
 {
-	/* If we have to ignore overcurrent events then almost by definition
-	 * we can't depend on resume-detect interrupts. */
-	if (ignore_oc)
+	/*
+	 * If we have to ignore overcurrent events then almost by definition
+	 * we can't depend on resume-detect interrupts.
+	 *
+	 * Those interrupts also don't seem to work on ASpeed SoCs.
+	 */
+	if (ignore_oc || uhci_is_aspeed(uhci))
 		return 1;
 
 	return uhci->resume_detect_interrupts_are_broken ?
@@ -384,6 +388,13 @@ static void start_rh(struct uhci_hcd *uhci)
 {
 	uhci->is_stopped = 0;
 
+	/*
+	 * Clear stale status bits on Aspeed as we get a stale HCH
+	 * which causes problems later on
+	 */
+	if (uhci_is_aspeed(uhci))
+		uhci_writew(uhci, uhci_readw(uhci, USBSTS), USBSTS);
+
 	/* Mark it configured and running with a 64-byte max packet.
 	 * All interrupts are enabled, even though RESUME won't do anything.
 	 */
diff --git a/drivers/usb/host/uhci-hcd.h b/drivers/usb/host/uhci-hcd.h
index 7fa318a3091d..91b22b2ea3aa 100644
--- a/drivers/usb/host/uhci-hcd.h
+++ b/drivers/usb/host/uhci-hcd.h
@@ -48,6 +48,8 @@
 /* USB port status and control registers */
 #define USBPORTSC1	16
 #define USBPORTSC2	18
+#define USBPORTSC3	20
+#define USBPORTSC4	22
 #define   USBPORTSC_CCS		0x0001	/* Current Connect Status
 					 * ("device present") */
 #define   USBPORTSC_CSC		0x0002	/* Connect Status Change */
@@ -427,6 +429,7 @@ struct uhci_hcd {
 	unsigned int wait_for_hp:1;		/* Wait for HP port reset */
 	unsigned int big_endian_mmio:1;		/* Big endian registers */
 	unsigned int big_endian_desc:1;		/* Big endian descriptors */
+	unsigned int is_aspeed:1;		/* Aspeed impl. workarounds */
 
 	/* Support for port suspend/resume/reset */
 	unsigned long port_c_suspend;		/* Bit-arrays of ports */
@@ -490,6 +493,12 @@ struct urb_priv {
 #define PCI_VENDOR_ID_GENESYS		0x17a0
 #define PCI_DEVICE_ID_GL880S_UHCI	0x8083
 
+/* Aspeed SoC needs some quirks */
+static inline bool uhci_is_aspeed(const struct uhci_hcd *uhci)
+{
+	return IS_ENABLED(CONFIG_USB_UHCI_ASPEED) && uhci->is_aspeed;
+}
+
 /*
  * Functions used to access controller registers. The UCHI spec says that host
  * controller I/O registers are mapped into PCI I/O space. For non-PCI hosts
@@ -545,10 +554,42 @@ static inline void uhci_writeb(const struct uhci_hcd *uhci, u8 val, int reg)
 #define uhci_big_endian_mmio(u)		0
 #endif
 
+static inline int uhci_aspeed_reg(unsigned int reg)
+{
+	switch (reg) {
+	case USBCMD:
+		return 00;
+	case USBSTS:
+		return 0x04;
+	case USBINTR:
+		return 0x08;
+	case USBFRNUM:
+		return 0x80;
+	case USBFLBASEADD:
+		return 0x0c;
+	case USBSOF:
+		return 0x84;
+	case USBPORTSC1:
+		return 0x88;
+	case USBPORTSC2:
+		return 0x8c;
+	case USBPORTSC3:
+		return 0x90;
+	case USBPORTSC4:
+		return 0x94;
+	default:
+		pr_warn("UHCI: Unsupported register 0x%02x on Aspeed\n", reg);
+		/* Return an unimplemented register */
+		return 0x10;
+	}
+}
+
 static inline u32 uhci_readl(const struct uhci_hcd *uhci, int reg)
 {
 	if (uhci_has_pci_registers(uhci))
 		return inl(uhci->io_addr + reg);
+	else if (uhci_is_aspeed(uhci))
+		return readl(uhci->regs + uhci_aspeed_reg(reg));
 #ifdef CONFIG_USB_UHCI_BIG_ENDIAN_MMIO
 	else if (uhci_big_endian_mmio(uhci))
 		return readl_be(uhci->regs + reg);
@@ -561,6 +602,8 @@ static inline void uhci_writel(const struct uhci_hcd *uhci, u32 val, int reg)
 {
 	if (uhci_has_pci_registers(uhci))
 		outl(val, uhci->io_addr + reg);
+	else if (uhci_is_aspeed(uhci))
+		writel(val, uhci->regs + uhci_aspeed_reg(reg));
 #ifdef CONFIG_USB_UHCI_BIG_ENDIAN_MMIO
 	else if (uhci_big_endian_mmio(uhci))
 		writel_be(val, uhci->regs + reg);
@@ -573,6 +616,8 @@ static inline u16 uhci_readw(const struct uhci_hcd *uhci, int reg)
 {
 	if (uhci_has_pci_registers(uhci))
 		return inw(uhci->io_addr + reg);
+	else if (uhci_is_aspeed(uhci))
+		return readl(uhci->regs + uhci_aspeed_reg(reg));
 #ifdef CONFIG_USB_UHCI_BIG_ENDIAN_MMIO
 	else if (uhci_big_endian_mmio(uhci))
 		return readw_be(uhci->regs + reg);
@@ -585,6 +630,8 @@ static inline void uhci_writew(const struct uhci_hcd *uhci, u16 val, int reg)
 {
 	if (uhci_has_pci_registers(uhci))
 		outw(val, uhci->io_addr + reg);
+	else if (uhci_is_aspeed(uhci))
+		writel(val, uhci->regs + uhci_aspeed_reg(reg));
 #ifdef CONFIG_USB_UHCI_BIG_ENDIAN_MMIO
 	else if (uhci_big_endian_mmio(uhci))
 		writew_be(val, uhci->regs + reg);
@@ -597,6 +644,8 @@ static inline u8 uhci_readb(const struct uhci_hcd *uhci, int reg)
 {
 	if (uhci_has_pci_registers(uhci))
 		return inb(uhci->io_addr + reg);
+	else if (uhci_is_aspeed(uhci))
+		return readl(uhci->regs + uhci_aspeed_reg(reg));
 #ifdef CONFIG_USB_UHCI_BIG_ENDIAN_MMIO
 	else if (uhci_big_endian_mmio(uhci))
 		return readb_be(uhci->regs + reg);
@@ -609,6 +658,8 @@ static inline void uhci_writeb(const struct uhci_hcd *uhci, u8 val, int reg)
 {
 	if (uhci_has_pci_registers(uhci))
 		outb(val, uhci->io_addr + reg);
+	else if (uhci_is_aspeed(uhci))
+		writel(val, uhci->regs + uhci_aspeed_reg(reg));
 #ifdef CONFIG_USB_UHCI_BIG_ENDIAN_MMIO
 	else if (uhci_big_endian_mmio(uhci))
 		writeb_be(val, uhci->regs + reg);
diff --git a/drivers/usb/host/uhci-pci.c b/drivers/usb/host/uhci-pci.c
index 02260cfdedb1..49effdc0d857 100644
--- a/drivers/usb/host/uhci-pci.c
+++ b/drivers/usb/host/uhci-pci.c
@@ -131,7 +131,7 @@ static int uhci_pci_init(struct usb_hcd *hcd)
 
 	/* Intel controllers use non-PME wakeup signalling */
 	if (to_pci_dev(uhci_dev(uhci))->vendor == PCI_VENDOR_ID_INTEL)
-		device_set_run_wake(uhci_dev(uhci), 1);
+		device_set_wakeup_capable(uhci_dev(uhci), true);
 
 	/* Set up pointers to PCI-specific functions */
 	uhci->reset_hc = uhci_pci_reset_hc;
diff --git a/drivers/usb/host/uhci-platform.c b/drivers/usb/host/uhci-platform.c
index 32a6f3d8deec..1b4e086c33a0 100644
--- a/drivers/usb/host/uhci-platform.c
+++ b/drivers/usb/host/uhci-platform.c
@@ -15,7 +15,9 @@ static int uhci_platform_init(struct usb_hcd *hcd)
 {
 	struct uhci_hcd *uhci = hcd_to_uhci(hcd);
 
-	uhci->rh_numports = uhci_count_ports(hcd);
+	/* Probe number of ports if not already provided by DT */
+	if (!uhci->rh_numports)
+		uhci->rh_numports = uhci_count_ports(hcd);
 
 	/* Set up pointers to to generic functions */
 	uhci->reset_hc = uhci_generic_reset_hc;
@@ -63,6 +65,7 @@ static const struct hc_driver uhci_platform_hc_driver = {
 
 static int uhci_hcd_platform_probe(struct platform_device *pdev)
 {
+	struct device_node *np = pdev->dev.of_node;
 	struct usb_hcd *hcd;
 	struct uhci_hcd	*uhci;
 	struct resource *res;
@@ -98,6 +101,23 @@ static int uhci_hcd_platform_probe(struct platform_device *pdev)
 
 	uhci->regs = hcd->regs;
 
+	/* Grab some things from the device-tree */
+	if (np) {
+		u32 num_ports;
+
+		if (of_property_read_u32(np, "#ports", &num_ports) == 0) {
+			uhci->rh_numports = num_ports;
+			dev_info(&pdev->dev,
+				"Detected %d ports from device-tree\n",
+				num_ports);
+		}
+		if (of_device_is_compatible(np, "aspeed,ast2400-uhci") ||
+		    of_device_is_compatible(np, "aspeed,ast2500-uhci")) {
+			uhci->is_aspeed = 1;
+			dev_info(&pdev->dev,
+				 "Enabled Aspeed implementation workarounds\n");
+		}
+	}
 	ret = usb_add_hcd(hcd, pdev->resource[1].start, IRQF_SHARED);
 	if (ret)
 		goto err_rmr;
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index 1f1687e888d6..2a82c927ded2 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -407,64 +407,17 @@ fail:
 	return NULL;
 }
 
-void xhci_free_or_cache_endpoint_ring(struct xhci_hcd *xhci,
+void xhci_free_endpoint_ring(struct xhci_hcd *xhci,
 		struct xhci_virt_device *virt_dev,
 		unsigned int ep_index)
 {
-	int rings_cached;
-
-	rings_cached = virt_dev->num_rings_cached;
-	if (rings_cached < XHCI_MAX_RINGS_CACHED) {
-		virt_dev->ring_cache[rings_cached] =
-			virt_dev->eps[ep_index].ring;
-		virt_dev->num_rings_cached++;
-		xhci_dbg(xhci, "Cached old ring, "
-				"%d ring%s cached\n",
-				virt_dev->num_rings_cached,
-				(virt_dev->num_rings_cached > 1) ? "s" : "");
-	} else {
-		xhci_ring_free(xhci, virt_dev->eps[ep_index].ring);
-		xhci_dbg(xhci, "Ring cache full (%d rings), "
-				"freeing ring\n",
-				virt_dev->num_rings_cached);
-	}
+	xhci_ring_free(xhci, virt_dev->eps[ep_index].ring);
 	virt_dev->eps[ep_index].ring = NULL;
 }
 
-/* Zero an endpoint ring (except for link TRBs) and move the enqueue and dequeue
- * pointers to the beginning of the ring.
- */
-static void xhci_reinit_cached_ring(struct xhci_hcd *xhci,
-			struct xhci_ring *ring, unsigned int cycle_state,
-			enum xhci_ring_type type)
-{
-	struct xhci_segment	*seg = ring->first_seg;
-	int i;
-
-	do {
-		memset(seg->trbs, 0,
-				sizeof(union xhci_trb)*TRBS_PER_SEGMENT);
-		if (cycle_state == 0) {
-			for (i = 0; i < TRBS_PER_SEGMENT; i++)
-				seg->trbs[i].link.control |=
-					cpu_to_le32(TRB_CYCLE);
-		}
-		/* All endpoint rings have link TRBs */
-		xhci_link_segments(xhci, seg, seg->next, type);
-		seg = seg->next;
-	} while (seg != ring->first_seg);
-	ring->type = type;
-	xhci_initialize_ring_info(ring, cycle_state);
-	/* td list should be empty since all URBs have been cancelled,
-	 * but just in case...
-	 */
-	INIT_LIST_HEAD(&ring->td_list);
-}
-
 /*
  * Expand an existing ring.
- * Look for a cached ring or allocate a new ring which has same segment numbers
- * and link the two rings.
+ * Allocate a new ring which has same segment numbers and link the two rings.
  */
 int xhci_ring_expansion(struct xhci_hcd *xhci, struct xhci_ring *ring,
 				unsigned int num_trbs, gfp_t flags)
@@ -968,12 +921,6 @@ void xhci_free_virt_device(struct xhci_hcd *xhci, int slot_id)
 	/* If necessary, update the number of active TTs on this root port */
 	xhci_update_tt_active_eps(xhci, dev, old_active_eps);
 
-	if (dev->ring_cache) {
-		for (i = 0; i < dev->num_rings_cached; i++)
-			xhci_ring_free(xhci, dev->ring_cache[i]);
-		kfree(dev->ring_cache);
-	}
-
 	if (dev->in_ctx)
 		xhci_free_container_ctx(xhci, dev->in_ctx);
 	if (dev->out_ctx)
@@ -1062,14 +1009,6 @@ int xhci_alloc_virt_device(struct xhci_hcd *xhci, int slot_id,
 	if (!dev->eps[0].ring)
 		goto fail;
 
-	/* Allocate pointers to the ring cache */
-	dev->ring_cache = kzalloc(
-			sizeof(struct xhci_ring *)*XHCI_MAX_RINGS_CACHED,
-			flags);
-	if (!dev->ring_cache)
-		goto fail;
-	dev->num_rings_cached = 0;
-
 	dev->udev = udev;
 
 	/* Point to output device context in dcbaa. */
@@ -1537,17 +1476,9 @@ int xhci_endpoint_init(struct xhci_hcd *xhci,
 	/* Set up the endpoint ring */
 	virt_dev->eps[ep_index].new_ring =
 		xhci_ring_alloc(xhci, 2, 1, ring_type, max_packet, mem_flags);
-	if (!virt_dev->eps[ep_index].new_ring) {
-		/* Attempt to use the ring cache */
-		if (virt_dev->num_rings_cached == 0)
-			return -ENOMEM;
-		virt_dev->num_rings_cached--;
-		virt_dev->eps[ep_index].new_ring =
-			virt_dev->ring_cache[virt_dev->num_rings_cached];
-		virt_dev->ring_cache[virt_dev->num_rings_cached] = NULL;
-		xhci_reinit_cached_ring(xhci, virt_dev->eps[ep_index].new_ring,
-					1, ring_type);
-	}
+	if (!virt_dev->eps[ep_index].new_ring)
+		return -ENOMEM;
+
 	virt_dev->eps[ep_index].skip = false;
 	ep_ring = virt_dev->eps[ep_index].new_ring;
 
@@ -2119,11 +2050,12 @@ static void xhci_add_in_port(struct xhci_hcd *xhci, unsigned int num_ports,
 {
 	u32 temp, port_offset, port_count;
 	int i;
-	u8 major_revision;
+	u8 major_revision, minor_revision;
 	struct xhci_hub *rhub;
 
 	temp = readl(addr);
 	major_revision = XHCI_EXT_PORT_MAJOR(temp);
+	minor_revision = XHCI_EXT_PORT_MINOR(temp);
 
 	if (major_revision == 0x03) {
 		rhub = &xhci->usb3_rhub;
@@ -2137,7 +2069,9 @@ static void xhci_add_in_port(struct xhci_hcd *xhci, unsigned int num_ports,
 		return;
 	}
 	rhub->maj_rev = XHCI_EXT_PORT_MAJOR(temp);
-	rhub->min_rev = XHCI_EXT_PORT_MINOR(temp);
+
+	if (rhub->min_rev < minor_revision)
+		rhub->min_rev = minor_revision;
 
 	/* Port offset and count in the third dword, see section 7.2 */
 	temp = readl(addr + 2);
diff --git a/drivers/usb/host/xhci-pci.c b/drivers/usb/host/xhci-pci.c
index fcf1f3f63e7a..783e6687bf4a 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -201,6 +201,9 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 	if (pdev->vendor == PCI_VENDOR_ID_ASMEDIA &&
 			pdev->device == 0x1042)
 		xhci->quirks |= XHCI_BROKEN_STREAMS;
+	if (pdev->vendor == PCI_VENDOR_ID_ASMEDIA &&
+			pdev->device == 0x1142)
+		xhci->quirks |= XHCI_TRUST_TX_LENGTH;
 
 	if (pdev->vendor == PCI_VENDOR_ID_TI && pdev->device == 0x8241)
 		xhci->quirks |= XHCI_LIMIT_ENDPOINT_INTERVAL_7;
@@ -213,13 +216,12 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 #ifdef CONFIG_ACPI
 static void xhci_pme_acpi_rtd3_enable(struct pci_dev *dev)
 {
-	static const u8 intel_dsm_uuid[] = {
-		0xb7, 0x0c, 0x34, 0xac,	0x01, 0xe9, 0xbf, 0x45,
-		0xb7, 0xe6, 0x2b, 0x34, 0xec, 0x93, 0x1e, 0x23,
-	};
+	static const guid_t intel_dsm_guid =
+		GUID_INIT(0xac340cb7, 0xe901, 0x45bf,
+			  0xb7, 0xe6, 0x2b, 0x34, 0xec, 0x93, 0x1e, 0x23);
 	union acpi_object *obj;
 
-	obj = acpi_evaluate_dsm(ACPI_HANDLE(&dev->dev), intel_dsm_uuid, 3, 1,
+	obj = acpi_evaluate_dsm(ACPI_HANDLE(&dev->dev), &intel_dsm_guid, 3, 1,
 				NULL);
 	ACPI_FREE(obj);
 }
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 03f63f50afb6..c50c902d009e 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -480,10 +480,34 @@ struct xhci_ring *xhci_triad_to_transfer_ring(struct xhci_hcd *xhci,
 	return NULL;
 }
 
+
+/*
+ * Get the hw dequeue pointer xHC stopped on, either directly from the
+ * endpoint context, or if streams are in use from the stream context.
+ * The returned hw_dequeue contains the lowest four bits with cycle state
+ * and possbile stream context type.
+ */
+static u64 xhci_get_hw_deq(struct xhci_hcd *xhci, struct xhci_virt_device *vdev,
+			   unsigned int ep_index, unsigned int stream_id)
+{
+	struct xhci_ep_ctx *ep_ctx;
+	struct xhci_stream_ctx *st_ctx;
+	struct xhci_virt_ep *ep;
+
+	ep = &vdev->eps[ep_index];
+
+	if (ep->ep_state & EP_HAS_STREAMS) {
+		st_ctx = &ep->stream_info->stream_ctx_array[stream_id];
+		return le64_to_cpu(st_ctx->stream_ring);
+	}
+	ep_ctx = xhci_get_ep_ctx(xhci, vdev->out_ctx, ep_index);
+	return le64_to_cpu(ep_ctx->deq);
+}
+
 /*
  * Move the xHC's endpoint ring dequeue pointer past cur_td.
  * Record the new state of the xHC's endpoint ring dequeue segment,
- * dequeue pointer, and new consumer cycle state in state.
+ * dequeue pointer, stream id, and new consumer cycle state in state.
  * Update our internal representation of the ring's dequeue pointer.
  *
  * We do this in three jumps:
@@ -521,24 +545,15 @@ void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
 				stream_id);
 		return;
 	}
-
 	/* Dig out the cycle state saved by the xHC during the stop ep cmd */
 	xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
 			"Finding endpoint context");
-	/* 4.6.9 the css flag is written to the stream context for streams */
-	if (ep->ep_state & EP_HAS_STREAMS) {
-		struct xhci_stream_ctx *ctx =
-			&ep->stream_info->stream_ctx_array[stream_id];
-		hw_dequeue = le64_to_cpu(ctx->stream_ring);
-	} else {
-		struct xhci_ep_ctx *ep_ctx
-			= xhci_get_ep_ctx(xhci, dev->out_ctx, ep_index);
-		hw_dequeue = le64_to_cpu(ep_ctx->deq);
-	}
 
+	hw_dequeue = xhci_get_hw_deq(xhci, dev, ep_index, stream_id);
 	new_seg = ep_ring->deq_seg;
 	new_deq = ep_ring->dequeue;
 	state->new_cycle_state = hw_dequeue & 0x1;
+	state->stream_id = stream_id;
 
 	/*
 	 * We want to find the pointer, segment and cycle state of the new trb
@@ -691,7 +706,7 @@ static void xhci_handle_cmd_stop_ep(struct xhci_hcd *xhci, int slot_id,
 	struct xhci_td *last_unlinked_td;
 	struct xhci_ep_ctx *ep_ctx;
 	struct xhci_virt_device *vdev;
-
+	u64 hw_deq;
 	struct xhci_dequeue_state deq_state;
 
 	if (unlikely(TRB_TO_SUSPEND_PORT(le32_to_cpu(trb->generic.field[3])))) {
@@ -715,7 +730,6 @@ static void xhci_handle_cmd_stop_ep(struct xhci_hcd *xhci, int slot_id,
 
 	if (list_empty(&ep->cancelled_td_list)) {
 		xhci_stop_watchdog_timer_in_irq(xhci, ep);
-		ep->stopped_td = NULL;
 		ring_doorbell_for_active_rings(xhci, slot_id, ep_index);
 		return;
 	}
@@ -753,12 +767,19 @@ static void xhci_handle_cmd_stop_ep(struct xhci_hcd *xhci, int slot_id,
 		 * If we stopped on the TD we need to cancel, then we have to
 		 * move the xHC endpoint ring dequeue pointer past this TD.
 		 */
-		if (cur_td == ep->stopped_td)
+		hw_deq = xhci_get_hw_deq(xhci, vdev, ep_index,
+					 cur_td->urb->stream_id);
+		hw_deq &= ~0xf;
+
+		if (trb_in_td(xhci, cur_td->start_seg, cur_td->first_trb,
+			      cur_td->last_trb, hw_deq, false)) {
 			xhci_find_new_dequeue_state(xhci, slot_id, ep_index,
-					cur_td->urb->stream_id,
-					cur_td, &deq_state);
-		else
+						    cur_td->urb->stream_id,
+						    cur_td, &deq_state);
+		} else {
 			td_to_noop(xhci, ep_ring, cur_td, false);
+		}
+
 remove_finished_td:
 		/*
 		 * The event handler won't see a completion for this TD anymore,
@@ -773,15 +794,13 @@ remove_finished_td:
 	/* If necessary, queue a Set Transfer Ring Dequeue Pointer command */
 	if (deq_state.new_deq_ptr && deq_state.new_deq_seg) {
 		xhci_queue_new_dequeue_state(xhci, slot_id, ep_index,
-				ep->stopped_td->urb->stream_id, &deq_state);
+					     &deq_state);
 		xhci_ring_cmd_db(xhci);
 	} else {
 		/* Otherwise ring the doorbell(s) to restart queued transfers */
 		ring_doorbell_for_active_rings(xhci, slot_id, ep_index);
 	}
 
-	ep->stopped_td = NULL;
-
 	/*
 	 * Drop the lock and complete the URBs in the cancelled TD list.
 	 * New TDs to be cancelled might be added to the end of the list before
@@ -1800,7 +1819,8 @@ struct xhci_segment *trb_in_td(struct xhci_hcd *xhci,
 static void xhci_cleanup_halted_endpoint(struct xhci_hcd *xhci,
 		unsigned int slot_id, unsigned int ep_index,
 		unsigned int stream_id,
-		struct xhci_td *td, union xhci_trb *ep_trb)
+		struct xhci_td *td, union xhci_trb *ep_trb,
+		enum xhci_ep_reset_type reset_type)
 {
 	struct xhci_virt_ep *ep = &xhci->devs[slot_id]->eps[ep_index];
 	struct xhci_command *command;
@@ -1809,12 +1829,11 @@ static void xhci_cleanup_halted_endpoint(struct xhci_hcd *xhci,
 		return;
 
 	ep->ep_state |= EP_HALTED;
-	ep->stopped_stream = stream_id;
 
-	xhci_queue_reset_ep(xhci, command, slot_id, ep_index);
-	xhci_cleanup_stalled_ring(xhci, ep_index, td);
+	xhci_queue_reset_ep(xhci, command, slot_id, ep_index, reset_type);
 
-	ep->stopped_stream = 0;
+	if (reset_type == EP_HARD_RESET)
+		xhci_cleanup_stalled_ring(xhci, ep_index, stream_id, td);
 
 	xhci_ring_cmd_db(xhci);
 }
@@ -1909,7 +1928,7 @@ static int xhci_td_cleanup(struct xhci_hcd *xhci, struct xhci_td *td,
 
 static int finish_td(struct xhci_hcd *xhci, struct xhci_td *td,
 	union xhci_trb *ep_trb, struct xhci_transfer_event *event,
-	struct xhci_virt_ep *ep, int *status, bool skip)
+	struct xhci_virt_ep *ep, int *status)
 {
 	struct xhci_virt_device *xdev;
 	struct xhci_ep_ctx *ep_ctx;
@@ -1925,9 +1944,6 @@ static int finish_td(struct xhci_hcd *xhci, struct xhci_td *td,
 	ep_ctx = xhci_get_ep_ctx(xhci, xdev->out_ctx, ep_index);
 	trb_comp_code = GET_COMP_CODE(le32_to_cpu(event->transfer_len));
 
-	if (skip)
-		goto td_cleanup;
-
 	if (trb_comp_code == COMP_STOPPED_LENGTH_INVALID ||
 			trb_comp_code == COMP_STOPPED ||
 			trb_comp_code == COMP_STOPPED_SHORT_PACKET) {
@@ -1935,7 +1951,6 @@ static int finish_td(struct xhci_hcd *xhci, struct xhci_td *td,
 		 * stopped TDs.  A stopped TD may be restarted, so don't update
 		 * the ring dequeue pointer or take this TD off any lists yet.
 		 */
-		ep->stopped_td = td;
 		return 0;
 	}
 	if (trb_comp_code == COMP_STALL_ERROR ||
@@ -1947,7 +1962,8 @@ static int finish_td(struct xhci_hcd *xhci, struct xhci_td *td,
 		 * The class driver clears the device side halt later.
 		 */
 		xhci_cleanup_halted_endpoint(xhci, slot_id, ep_index,
-					ep_ring->stream_id, td, ep_trb);
+					ep_ring->stream_id, td, ep_trb,
+					EP_HARD_RESET);
 	} else {
 		/* Update ring dequeue pointer */
 		while (ep_ring->dequeue != td->last_trb)
@@ -1955,7 +1971,6 @@ static int finish_td(struct xhci_hcd *xhci, struct xhci_td *td,
 		inc_deq(xhci, ep_ring);
 	}
 
-td_cleanup:
 	return xhci_td_cleanup(xhci, td, ep_ring, status);
 }
 
@@ -2075,7 +2090,7 @@ static int process_ctrl_td(struct xhci_hcd *xhci, struct xhci_td *td,
 		td->urb->actual_length = requested;
 
 finish_td:
-	return finish_td(xhci, td, ep_trb, event, ep, status, false);
+	return finish_td(xhci, td, ep_trb, event, ep, status);
 }
 
 /*
@@ -2162,7 +2177,7 @@ static int process_isoc_td(struct xhci_hcd *xhci, struct xhci_td *td,
 
 	td->urb->actual_length += frame->actual_length;
 
-	return finish_td(xhci, td, ep_trb, event, ep, status, false);
+	return finish_td(xhci, td, ep_trb, event, ep, status);
 }
 
 static int skip_isoc_td(struct xhci_hcd *xhci, struct xhci_td *td,
@@ -2190,7 +2205,7 @@ static int skip_isoc_td(struct xhci_hcd *xhci, struct xhci_td *td,
 		inc_deq(xhci, ep_ring);
 	inc_deq(xhci, ep_ring);
 
-	return finish_td(xhci, td, NULL, event, ep, status, true);
+	return xhci_td_cleanup(xhci, td, ep_ring, status);
 }
 
 /*
@@ -2252,7 +2267,7 @@ finish_td:
 			  remaining);
 		td->urb->actual_length = 0;
 	}
-	return finish_td(xhci, td, ep_trb, event, ep, status, false);
+	return finish_td(xhci, td, ep_trb, event, ep, status);
 }
 
 /*
@@ -2280,39 +2295,46 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 	bool handling_skipped_tds = false;
 
 	slot_id = TRB_TO_SLOT_ID(le32_to_cpu(event->flags));
+	ep_index = TRB_TO_EP_ID(le32_to_cpu(event->flags)) - 1;
+	trb_comp_code = GET_COMP_CODE(le32_to_cpu(event->transfer_len));
+	ep_trb_dma = le64_to_cpu(event->buffer);
+
 	xdev = xhci->devs[slot_id];
 	if (!xdev) {
 		xhci_err(xhci, "ERROR Transfer event pointed to bad slot %u\n",
 			 slot_id);
-		xhci_err(xhci, "@%016llx %08x %08x %08x %08x\n",
-			 (unsigned long long) xhci_trb_virt_to_dma(
-				 xhci->event_ring->deq_seg,
-				 xhci->event_ring->dequeue),
-			 lower_32_bits(le64_to_cpu(event->buffer)),
-			 upper_32_bits(le64_to_cpu(event->buffer)),
-			 le32_to_cpu(event->transfer_len),
-			 le32_to_cpu(event->flags));
-		return -ENODEV;
-	}
-
-	/* Endpoint ID is 1 based, our index is zero based */
-	ep_index = TRB_TO_EP_ID(le32_to_cpu(event->flags)) - 1;
+		goto err_out;
+	}
+
 	ep = &xdev->eps[ep_index];
-	ep_ring = xhci_dma_to_transfer_ring(ep, le64_to_cpu(event->buffer));
+	ep_ring = xhci_dma_to_transfer_ring(ep, ep_trb_dma);
 	ep_ctx = xhci_get_ep_ctx(xhci, xdev->out_ctx, ep_index);
-	if (!ep_ring ||  GET_EP_CTX_STATE(ep_ctx) == EP_STATE_DISABLED) {
+
+	if (GET_EP_CTX_STATE(ep_ctx) == EP_STATE_DISABLED) {
 		xhci_err(xhci,
-			 "ERROR Transfer event for disabled endpoint slot %u ep %u or incorrect stream ring\n",
+			 "ERROR Transfer event for disabled endpoint slot %u ep %u\n",
 			  slot_id, ep_index);
-		xhci_err(xhci, "@%016llx %08x %08x %08x %08x\n",
-			 (unsigned long long) xhci_trb_virt_to_dma(
-				 xhci->event_ring->deq_seg,
-				 xhci->event_ring->dequeue),
-			 lower_32_bits(le64_to_cpu(event->buffer)),
-			 upper_32_bits(le64_to_cpu(event->buffer)),
-			 le32_to_cpu(event->transfer_len),
-			 le32_to_cpu(event->flags));
-		return -ENODEV;
+		goto err_out;
+	}
+
+	/* Some transfer events don't always point to a trb, see xhci 4.17.4 */
+	if (!ep_ring) {
+		switch (trb_comp_code) {
+		case COMP_STALL_ERROR:
+		case COMP_USB_TRANSACTION_ERROR:
+		case COMP_INVALID_STREAM_TYPE_ERROR:
+		case COMP_INVALID_STREAM_ID_ERROR:
+			xhci_cleanup_halted_endpoint(xhci, slot_id, ep_index, 0,
+						     NULL, NULL, EP_SOFT_RESET);
+			goto cleanup;
+		case COMP_RING_UNDERRUN:
+		case COMP_RING_OVERRUN:
+			goto cleanup;
+		default:
+			xhci_err(xhci, "ERROR Transfer event for unknown stream ring slot %u ep %u\n",
+				 slot_id, ep_index);
+			goto err_out;
+		}
 	}
 
 	/* Count current td numbers if ep->skip is set */
@@ -2321,8 +2343,6 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 			td_num++;
 	}
 
-	ep_trb_dma = le64_to_cpu(event->buffer);
-	trb_comp_code = GET_COMP_CODE(le32_to_cpu(event->transfer_len));
 	/* Look for common error cases */
 	switch (trb_comp_code) {
 	/* Skip codes that require special handling depending on
@@ -2339,6 +2359,7 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 					      slot_id, ep_index);
 	case COMP_SHORT_PACKET:
 		break;
+	/* Completion codes for endpoint stopped state */
 	case COMP_STOPPED:
 		xhci_dbg(xhci, "Stopped on Transfer TRB for slot %u ep %u\n",
 			 slot_id, ep_index);
@@ -2353,18 +2374,13 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 			 "Stopped with short packet transfer detected for slot %u ep %u\n",
 			 slot_id, ep_index);
 		break;
+	/* Completion codes for endpoint halted state */
 	case COMP_STALL_ERROR:
 		xhci_dbg(xhci, "Stalled endpoint for slot %u ep %u\n", slot_id,
 			 ep_index);
 		ep->ep_state |= EP_HALTED;
 		status = -EPIPE;
 		break;
-	case COMP_TRB_ERROR:
-		xhci_warn(xhci,
-			  "WARN: TRB error for slot %u ep %u on endpoint\n",
-			  slot_id, ep_index);
-		status = -EILSEQ;
-		break;
 	case COMP_SPLIT_TRANSACTION_ERROR:
 	case COMP_USB_TRANSACTION_ERROR:
 		xhci_dbg(xhci, "Transfer error for slot %u ep %u on endpoint\n",
@@ -2376,6 +2392,14 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 			 slot_id, ep_index);
 		status = -EOVERFLOW;
 		break;
+	/* Completion codes for endpoint error state */
+	case COMP_TRB_ERROR:
+		xhci_warn(xhci,
+			  "WARN: TRB error for slot %u ep %u on endpoint\n",
+			  slot_id, ep_index);
+		status = -EILSEQ;
+		break;
+	/* completion codes not indicating endpoint state change */
 	case COMP_DATA_BUFFER_ERROR:
 		xhci_warn(xhci,
 			  "WARN: HC couldn't access mem fast enough for slot %u ep %u\n",
@@ -2413,12 +2437,6 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 				 TRB_TO_SLOT_ID(le32_to_cpu(event->flags)),
 				 ep_index);
 		goto cleanup;
-	case COMP_INCOMPATIBLE_DEVICE_ERROR:
-		xhci_warn(xhci,
-			  "WARN: detect an incompatible device for slot %u ep %u",
-			  slot_id, ep_index);
-		status = -EPROTO;
-		break;
 	case COMP_MISSED_SERVICE_ERROR:
 		/*
 		 * When encounter missed service error, one or more isoc tds
@@ -2437,6 +2455,14 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 			 "No Ping response error for slot %u ep %u, Skip one Isoc TD\n",
 			 slot_id, ep_index);
 		goto cleanup;
+
+	case COMP_INCOMPATIBLE_DEVICE_ERROR:
+		/* needs disable slot command to recover */
+		xhci_warn(xhci,
+			  "WARN: detect an incompatible device for slot %u ep %u",
+			  slot_id, ep_index);
+		status = -EPROTO;
+		break;
 	default:
 		if (xhci_is_vendor_info_code(xhci, trb_comp_code)) {
 			status = 0;
@@ -2589,6 +2615,17 @@ cleanup:
 	} while (handling_skipped_tds);
 
 	return 0;
+
+err_out:
+	xhci_err(xhci, "@%016llx %08x %08x %08x %08x\n",
+		 (unsigned long long) xhci_trb_virt_to_dma(
+			 xhci->event_ring->deq_seg,
+			 xhci->event_ring->dequeue),
+		 lower_32_bits(le64_to_cpu(event->buffer)),
+		 upper_32_bits(le64_to_cpu(event->buffer)),
+		 le32_to_cpu(event->transfer_len),
+		 le32_to_cpu(event->flags));
+	return -ENODEV;
 }
 
 /*
@@ -3963,13 +4000,12 @@ int xhci_queue_stop_endpoint(struct xhci_hcd *xhci, struct xhci_command *cmd,
 /* Set Transfer Ring Dequeue Pointer command */
 void xhci_queue_new_dequeue_state(struct xhci_hcd *xhci,
 		unsigned int slot_id, unsigned int ep_index,
-		unsigned int stream_id,
 		struct xhci_dequeue_state *deq_state)
 {
 	dma_addr_t addr;
 	u32 trb_slot_id = SLOT_ID_FOR_TRB(slot_id);
 	u32 trb_ep_index = EP_ID_FOR_TRB(ep_index);
-	u32 trb_stream_id = STREAM_ID_FOR_TRB(stream_id);
+	u32 trb_stream_id = STREAM_ID_FOR_TRB(deq_state->stream_id);
 	u32 trb_sct = 0;
 	u32 type = TRB_TYPE(TRB_SET_DEQ);
 	struct xhci_virt_ep *ep;
@@ -4007,7 +4043,7 @@ void xhci_queue_new_dequeue_state(struct xhci_hcd *xhci,
 
 	ep->queued_deq_seg = deq_state->new_deq_seg;
 	ep->queued_deq_ptr = deq_state->new_deq_ptr;
-	if (stream_id)
+	if (deq_state->stream_id)
 		trb_sct = SCT_FOR_TRB(SCT_PRI_TR);
 	ret = queue_command(xhci, cmd,
 		lower_32_bits(addr) | trb_sct | deq_state->new_cycle_state,
@@ -4027,12 +4063,16 @@ void xhci_queue_new_dequeue_state(struct xhci_hcd *xhci,
 }
 
 int xhci_queue_reset_ep(struct xhci_hcd *xhci, struct xhci_command *cmd,
-			int slot_id, unsigned int ep_index)
+			int slot_id, unsigned int ep_index,
+			enum xhci_ep_reset_type reset_type)
 {
 	u32 trb_slot_id = SLOT_ID_FOR_TRB(slot_id);
 	u32 trb_ep_index = EP_ID_FOR_TRB(ep_index);
 	u32 type = TRB_TYPE(TRB_RESET_EP);
 
+	if (reset_type == EP_SOFT_RESET)
+		type |= TRB_TSP;
+
 	return queue_command(xhci, cmd, 0, 0, 0,
 			trb_slot_id | trb_ep_index | type, false);
 }
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index 30f47d92a610..56f85df013db 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -1695,8 +1695,7 @@ static int xhci_add_endpoint(struct usb_hcd *hcd, struct usb_device *udev,
 	if (xhci->quirks & XHCI_MTK_HOST) {
 		ret = xhci_mtk_add_ep_quirk(hcd, udev, ep);
 		if (ret < 0) {
-			xhci_free_or_cache_endpoint_ring(xhci,
-				virt_dev, ep_index);
+			xhci_free_endpoint_ring(xhci, virt_dev, ep_index);
 			return ret;
 		}
 	}
@@ -2720,23 +2719,23 @@ static int xhci_check_bandwidth(struct usb_hcd *hcd, struct usb_device *udev)
 	for (i = 1; i < 31; i++) {
 		if ((le32_to_cpu(ctrl_ctx->drop_flags) & (1 << (i + 1))) &&
 		    !(le32_to_cpu(ctrl_ctx->add_flags) & (1 << (i + 1)))) {
-			xhci_free_or_cache_endpoint_ring(xhci, virt_dev, i);
+			xhci_free_endpoint_ring(xhci, virt_dev, i);
 			xhci_check_bw_drop_ep_streams(xhci, virt_dev, i);
 		}
 	}
 	xhci_zero_in_ctx(xhci, virt_dev);
 	/*
 	 * Install any rings for completely new endpoints or changed endpoints,
-	 * and free or cache any old rings from changed endpoints.
+	 * and free any old rings from changed endpoints.
 	 */
 	for (i = 1; i < 31; i++) {
 		if (!virt_dev->eps[i].new_ring)
 			continue;
-		/* Only cache or free the old ring if it exists.
+		/* Only free the old ring if it exists.
 		 * It may not if this is the first add of an endpoint.
 		 */
 		if (virt_dev->eps[i].ring) {
-			xhci_free_or_cache_endpoint_ring(xhci, virt_dev, i);
+			xhci_free_endpoint_ring(xhci, virt_dev, i);
 		}
 		xhci_check_bw_drop_ep_streams(xhci, virt_dev, i);
 		virt_dev->eps[i].ring = virt_dev->eps[i].new_ring;
@@ -2823,8 +2822,8 @@ static void xhci_setup_input_ctx_for_quirk(struct xhci_hcd *xhci,
 			added_ctxs, added_ctxs);
 }
 
-void xhci_cleanup_stalled_ring(struct xhci_hcd *xhci,
-			unsigned int ep_index, struct xhci_td *td)
+void xhci_cleanup_stalled_ring(struct xhci_hcd *xhci, unsigned int ep_index,
+			       unsigned int stream_id, struct xhci_td *td)
 {
 	struct xhci_dequeue_state deq_state;
 	struct xhci_virt_ep *ep;
@@ -2837,7 +2836,7 @@ void xhci_cleanup_stalled_ring(struct xhci_hcd *xhci,
 	 * or it will attempt to resend it on the next doorbell ring.
 	 */
 	xhci_find_new_dequeue_state(xhci, udev->slot_id,
-			ep_index, ep->stopped_stream, td, &deq_state);
+			ep_index, stream_id, td, &deq_state);
 
 	if (!deq_state.new_deq_ptr || !deq_state.new_deq_seg)
 		return;
@@ -2849,7 +2848,7 @@ void xhci_cleanup_stalled_ring(struct xhci_hcd *xhci,
 		xhci_dbg_trace(xhci, trace_xhci_dbg_reset_ep,
 				"Queueing new dequeue state");
 		xhci_queue_new_dequeue_state(xhci, udev->slot_id,
-				ep_index, ep->stopped_stream, &deq_state);
+				ep_index, &deq_state);
 	} else {
 		/* Better hope no one uses the input context between now and the
 		 * reset endpoint completion!
@@ -3336,7 +3335,7 @@ void xhci_free_device_endpoint_resources(struct xhci_hcd *xhci,
  *
  * Wait for the Reset Device command to finish.  Remove all structures
  * associated with the endpoints that were disabled.  Clear the input device
- * structure?  Cache the rings?  Reset the control endpoint 0 max packet size?
+ * structure? Reset the control endpoint 0 max packet size?
  *
  * If the virt_dev to be reset does not exist or does not match the udev,
  * it means the device is lost, possibly due to the xHC restore error and
@@ -3466,7 +3465,7 @@ static int xhci_discover_or_reset_device(struct usb_hcd *hcd,
 		spin_unlock_irqrestore(&xhci->lock, flags);
 	}
 
-	/* Everything but endpoint 0 is disabled, so free or cache the rings. */
+	/* Everything but endpoint 0 is disabled, so free the rings. */
 	last_freed_endpoint = 1;
 	for (i = 1; i < 31; i++) {
 		struct xhci_virt_ep *ep = &virt_dev->eps[i];
@@ -3480,7 +3479,7 @@ static int xhci_discover_or_reset_device(struct usb_hcd *hcd,
 		}
 
 		if (ep->ring) {
-			xhci_free_or_cache_endpoint_ring(xhci, virt_dev, i);
+			xhci_free_endpoint_ring(xhci, virt_dev, i);
 			last_freed_endpoint = i;
 		}
 		if (!list_empty(&virt_dev->eps[i].bw_endpoint_list))
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 73a28a986d5e..650a2d9d4aec 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -924,8 +924,6 @@ struct xhci_virt_ep {
 #define EP_GETTING_NO_STREAMS	(1 << 5)
 	/* ----  Related to URB cancellation ---- */
 	struct list_head	cancelled_td_list;
-	struct xhci_td		*stopped_td;
-	unsigned int		stopped_stream;
 	/* Watchdog timer for stop endpoint command to cancel URBs */
 	struct timer_list	stop_cmd_timer;
 	struct xhci_hcd		*xhci;
@@ -993,10 +991,6 @@ struct xhci_virt_device {
 	struct xhci_container_ctx       *out_ctx;
 	/* Used for addressing devices and configuration changes */
 	struct xhci_container_ctx       *in_ctx;
-	/* Rings saved to ensure old alt settings can be re-instated */
-	struct xhci_ring		**ring_cache;
-	int				num_rings_cached;
-#define	XHCI_MAX_RINGS_CACHED	31
 	struct xhci_virt_ep		eps[31];
 	u8				fake_port;
 	u8				real_port;
@@ -1210,6 +1204,11 @@ struct xhci_event_cmd {
 /* Stop Ring - Transfer State Preserve */
 #define TRB_TSP		(1<<9)
 
+enum xhci_ep_reset_type {
+	EP_HARD_RESET,
+	EP_SOFT_RESET,
+};
+
 /* Force Event */
 #define TRB_TO_VF_INTR_TARGET(p)	(((p) & (0x3ff << 22)) >> 22)
 #define TRB_TO_VF_ID(p)			(((p) & (0xff << 16)) >> 16)
@@ -1527,6 +1526,7 @@ struct xhci_dequeue_state {
 	struct xhci_segment *new_deq_seg;
 	union xhci_trb *new_deq_ptr;
 	int new_cycle_state;
+	unsigned int stream_id;
 };
 
 enum xhci_ring_type {
@@ -1960,7 +1960,7 @@ int xhci_endpoint_init(struct xhci_hcd *xhci, struct xhci_virt_device *virt_dev,
 void xhci_ring_free(struct xhci_hcd *xhci, struct xhci_ring *ring);
 int xhci_ring_expansion(struct xhci_hcd *xhci, struct xhci_ring *ring,
 				unsigned int num_trbs, gfp_t flags);
-void xhci_free_or_cache_endpoint_ring(struct xhci_hcd *xhci,
+void xhci_free_endpoint_ring(struct xhci_hcd *xhci,
 		struct xhci_virt_device *virt_dev,
 		unsigned int ep_index);
 struct xhci_stream_info *xhci_alloc_stream_info(struct xhci_hcd *xhci,
@@ -2044,7 +2044,8 @@ int xhci_queue_configure_endpoint(struct xhci_hcd *xhci,
 int xhci_queue_evaluate_context(struct xhci_hcd *xhci, struct xhci_command *cmd,
 		dma_addr_t in_ctx_ptr, u32 slot_id, bool command_must_succeed);
 int xhci_queue_reset_ep(struct xhci_hcd *xhci, struct xhci_command *cmd,
-		int slot_id, unsigned int ep_index);
+		int slot_id, unsigned int ep_index,
+		enum xhci_ep_reset_type reset_type);
 int xhci_queue_reset_device(struct xhci_hcd *xhci, struct xhci_command *cmd,
 		u32 slot_id);
 void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
@@ -2053,10 +2054,9 @@ void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
 		struct xhci_dequeue_state *state);
 void xhci_queue_new_dequeue_state(struct xhci_hcd *xhci,
 		unsigned int slot_id, unsigned int ep_index,
-		unsigned int stream_id,
 		struct xhci_dequeue_state *deq_state);
-void xhci_cleanup_stalled_ring(struct xhci_hcd *xhci,
-		unsigned int ep_index, struct xhci_td *td);
+void xhci_cleanup_stalled_ring(struct xhci_hcd *xhci, unsigned int ep_index,
+		unsigned int stream_id, struct xhci_td *td);
 void xhci_stop_endpoint_command_watchdog(unsigned long arg);
 void xhci_handle_command_timeout(struct work_struct *work);
 
diff --git a/drivers/usb/misc/Kconfig b/drivers/usb/misc/Kconfig
index 1d1d70d62a19..0f9f25db9163 100644
--- a/drivers/usb/misc/Kconfig
+++ b/drivers/usb/misc/Kconfig
@@ -275,29 +275,3 @@ config USB_CHAOSKEY
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called chaoskey.
-
-config UCSI
-	tristate "USB Type-C Connector System Software Interface driver"
-	depends on ACPI
-	help
-	  UCSI driver is meant to be used as a convenience tool for desktop and
-	  server systems that are not equipped to handle USB in device mode. It
-	  will always select USB host role for the USB Type-C ports on systems
-	  that provide UCSI interface.
-
-	  USB Type-C Connector System Software Interface (UCSI) is a
-	  specification for an interface that allows the Operating System to
-	  control the USB Type-C ports on a system. Things the need controlling
-	  include the USB Data Role (host or device), and when USB Power
-	  Delivery is supported, the Power Role (source or sink). With USB
-	  Type-C connectors, when two dual role capable devices are attached
-	  together, the data role is selected randomly. Therefore it is
-	  important to give the OS a way to select the role. Otherwise the user
-	  would have to unplug and replug in order in order to attempt to swap
-	  the data and power roles.
-
-	  The UCSI specification can be downloaded from:
-	  http://www.intel.com/content/www/us/en/io/universal-serial-bus/usb-type-c-ucsi-spec.html
-
-	  To compile the driver as a module, choose M here: the module will be
-	  called ucsi.
diff --git a/drivers/usb/misc/Makefile b/drivers/usb/misc/Makefile
index f6ac6c99a6e6..7fdb45fc976f 100644
--- a/drivers/usb/misc/Makefile
+++ b/drivers/usb/misc/Makefile
@@ -27,7 +27,6 @@ obj-$(CONFIG_USB_HUB_USB251XB)		+= usb251xb.o
 obj-$(CONFIG_USB_HSIC_USB3503)		+= usb3503.o
 obj-$(CONFIG_USB_HSIC_USB4604)		+= usb4604.o
 obj-$(CONFIG_USB_CHAOSKEY)		+= chaoskey.o
-obj-$(CONFIG_UCSI)			+= ucsi.o
 
 obj-$(CONFIG_USB_SISUSBVGA)		+= sisusbvga/
 obj-$(CONFIG_USB_LINK_LAYER_TEST)	+= lvstest.o
diff --git a/drivers/usb/misc/iowarrior.c b/drivers/usb/misc/iowarrior.c
index 83b05a287b0c..7ca4c7e0ea0d 100644
--- a/drivers/usb/misc/iowarrior.c
+++ b/drivers/usb/misc/iowarrior.c
@@ -368,14 +368,9 @@ static ssize_t iowarrior_write(struct file *file,
 	case USB_DEVICE_ID_CODEMERCS_IOWPV2:
 	case USB_DEVICE_ID_CODEMERCS_IOW40:
 		/* IOW24 and IOW40 use a synchronous call */
-		buf = kmalloc(count, GFP_KERNEL);
-		if (!buf) {
-			retval = -ENOMEM;
-			goto exit;
-		}
-		if (copy_from_user(buf, user_buffer, count)) {
-			retval = -EFAULT;
-			kfree(buf);
+		buf = memdup_user(user_buffer, count);
+		if (IS_ERR(buf)) {
+			retval = PTR_ERR(buf);
 			goto exit;
 		}
 		retval = usb_set_report(dev->interface, 2, 0, buf, count);
diff --git a/drivers/usb/misc/ucsi.c b/drivers/usb/misc/ucsi.c
deleted file mode 100644
index 07397bddefa3..000000000000
--- a/drivers/usb/misc/ucsi.c
+++ /dev/null
@@ -1,478 +0,0 @@
-/*
- * USB Type-C Connector System Software Interface driver
- *
- * Copyright (C) 2016, Intel Corporation
- * Author: Heikki Krogerus <heikki.krogerus@linux.intel.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include <linux/platform_device.h>
-#include <linux/module.h>
-#include <linux/delay.h>
-#include <linux/acpi.h>
-
-#include "ucsi.h"
-
-/* Double the time defined by MIN_TIME_TO_RESPOND_WITH_BUSY */
-#define UCSI_TIMEOUT_MS 20
-
-enum ucsi_status {
-	UCSI_IDLE = 0,
-	UCSI_BUSY,
-	UCSI_ERROR,
-};
-
-struct ucsi_connector {
-	int num;
-	struct ucsi *ucsi;
-	struct work_struct work;
-	struct ucsi_connector_capability cap;
-};
-
-struct ucsi {
-	struct device *dev;
-	struct ucsi_data __iomem *data;
-
-	enum ucsi_status status;
-	struct completion complete;
-	struct ucsi_capability cap;
-	struct ucsi_connector *connector;
-
-	/* device lock */
-	spinlock_t dev_lock;
-
-	/* PPM Communication lock */
-	struct mutex ppm_lock;
-
-	/* PPM communication flags */
-	unsigned long flags;
-#define EVENT_PENDING	0
-#define COMMAND_PENDING	1
-};
-
-static int ucsi_acpi_cmd(struct ucsi *ucsi, struct ucsi_control *ctrl)
-{
-	uuid_le uuid = UUID_LE(0x6f8398c2, 0x7ca4, 0x11e4,
-			       0xad, 0x36, 0x63, 0x10, 0x42, 0xb5, 0x00, 0x8f);
-	union acpi_object *obj;
-
-	ucsi->data->ctrl.raw_cmd = ctrl->raw_cmd;
-
-	obj = acpi_evaluate_dsm(ACPI_HANDLE(ucsi->dev), uuid.b, 1, 1, NULL);
-	if (!obj) {
-		dev_err(ucsi->dev, "%s: failed to evaluate _DSM\n", __func__);
-		return -EIO;
-	}
-
-	ACPI_FREE(obj);
-	return 0;
-}
-
-static void ucsi_acpi_notify(acpi_handle handle, u32 event, void *data)
-{
-	struct ucsi *ucsi = data;
-	struct ucsi_cci *cci;
-
-	spin_lock(&ucsi->dev_lock);
-
-	ucsi->status = UCSI_IDLE;
-	cci = &ucsi->data->cci;
-
-	/*
-	 * REVISIT: This is not documented behavior, but all known PPMs ACK
-	 * asynchronous events by sending notification with cleared CCI.
-	 */
-	if (!ucsi->data->raw_cci) {
-		if (test_bit(EVENT_PENDING, &ucsi->flags))
-			complete(&ucsi->complete);
-		else
-			dev_WARN(ucsi->dev, "spurious notification\n");
-		goto out_unlock;
-	}
-
-	if (test_bit(COMMAND_PENDING, &ucsi->flags)) {
-		if (cci->busy) {
-			ucsi->status = UCSI_BUSY;
-			complete(&ucsi->complete);
-
-			goto out_unlock;
-		} else if (cci->ack_complete || cci->cmd_complete) {
-			/* Error Indication is only valid with commands */
-			if (cci->error && cci->cmd_complete)
-				ucsi->status = UCSI_ERROR;
-
-			ucsi->data->ctrl.raw_cmd = 0;
-			complete(&ucsi->complete);
-		}
-	}
-
-	if (cci->connector_change) {
-		struct ucsi_connector *con;
-
-		/*
-		 * This is workaround for buggy PPMs that create asynchronous
-		 * event notifications before OPM has enabled them.
-		 */
-		if (!ucsi->connector)
-			goto out_unlock;
-
-		con = ucsi->connector + (cci->connector_change - 1);
-
-		/*
-		 * PPM will not clear the connector specific bit in Connector
-		 * Change Indication field of CCI until the driver has ACK it,
-		 * and the driver can not ACK it before it has been processed.
-		 * The PPM will not generate new events before the first has
-		 * been acknowledged, even if they are for an other connector.
-		 * So only one event at a time.
-		 */
-		if (!test_and_set_bit(EVENT_PENDING, &ucsi->flags))
-			schedule_work(&con->work);
-	}
-out_unlock:
-	spin_unlock(&ucsi->dev_lock);
-}
-
-static int ucsi_ack(struct ucsi *ucsi, u8 cmd)
-{
-	struct ucsi_control ctrl;
-	int ret;
-
-	ctrl.cmd.cmd = UCSI_ACK_CC_CI;
-	ctrl.cmd.length = 0;
-	ctrl.cmd.data = cmd;
-	ret = ucsi_acpi_cmd(ucsi, &ctrl);
-	if (ret)
-		return ret;
-
-	/* Waiting for ACK also with ACK CMD for now */
-	ret = wait_for_completion_timeout(&ucsi->complete,
-					  msecs_to_jiffies(UCSI_TIMEOUT_MS));
-	if (!ret)
-		return -ETIMEDOUT;
-	return 0;
-}
-
-static int ucsi_run_cmd(struct ucsi *ucsi, struct ucsi_control *ctrl,
-			void *data, size_t size)
-{
-	u16 err_value = 0;
-	int ret;
-
-	set_bit(COMMAND_PENDING, &ucsi->flags);
-
-	ret = ucsi_acpi_cmd(ucsi, ctrl);
-	if (ret)
-		goto err_clear_flag;
-
-	ret = wait_for_completion_timeout(&ucsi->complete,
-					  msecs_to_jiffies(UCSI_TIMEOUT_MS));
-	if (!ret) {
-		ret = -ETIMEDOUT;
-		goto err_clear_flag;
-	}
-
-	switch (ucsi->status) {
-	case UCSI_IDLE:
-		if (data)
-			memcpy(data, ucsi->data->message_in, size);
-
-		ret = ucsi_ack(ucsi, UCSI_ACK_CMD);
-		break;
-	case UCSI_BUSY:
-		/* The caller decides whether to cancel or not */
-		ret = -EBUSY;
-		goto err_clear_flag;
-	case UCSI_ERROR:
-		ret = ucsi_ack(ucsi, UCSI_ACK_CMD);
-		if (ret)
-			goto err_clear_flag;
-
-		ctrl->cmd.cmd = UCSI_GET_ERROR_STATUS;
-		ctrl->cmd.length = 0;
-		ctrl->cmd.data = 0;
-		ret = ucsi_acpi_cmd(ucsi, ctrl);
-		if (ret)
-			goto err_clear_flag;
-
-		ret = wait_for_completion_timeout(&ucsi->complete,
-					msecs_to_jiffies(UCSI_TIMEOUT_MS));
-		if (!ret) {
-			ret = -ETIMEDOUT;
-			goto err_clear_flag;
-		}
-
-		memcpy(&err_value, ucsi->data->message_in, sizeof(err_value));
-
-		/* Something has really gone wrong */
-		if (WARN_ON(ucsi->status == UCSI_ERROR)) {
-			ret = -ENODEV;
-			goto err_clear_flag;
-		}
-
-		ret = ucsi_ack(ucsi, UCSI_ACK_CMD);
-		if (ret)
-			goto err_clear_flag;
-
-		switch (err_value) {
-		case UCSI_ERROR_INCOMPATIBLE_PARTNER:
-			ret = -EOPNOTSUPP;
-			break;
-		case UCSI_ERROR_CC_COMMUNICATION_ERR:
-			ret = -ECOMM;
-			break;
-		case UCSI_ERROR_CONTRACT_NEGOTIATION_FAIL:
-			ret = -EIO;
-			break;
-		case UCSI_ERROR_DEAD_BATTERY:
-			dev_warn(ucsi->dev, "Dead battery condition!\n");
-			ret = -EPERM;
-			break;
-		/* The following mean a bug in this driver */
-		case UCSI_ERROR_INVALID_CON_NUM:
-		case UCSI_ERROR_UNREGONIZED_CMD:
-		case UCSI_ERROR_INVALID_CMD_ARGUMENT:
-		default:
-			dev_warn(ucsi->dev,
-				 "%s: possible UCSI driver bug - error %hu\n",
-				 __func__, err_value);
-			ret = -EINVAL;
-			break;
-		}
-		break;
-	}
-	ctrl->raw_cmd = 0;
-err_clear_flag:
-	clear_bit(COMMAND_PENDING, &ucsi->flags);
-	return ret;
-}
-
-static void ucsi_connector_change(struct work_struct *work)
-{
-	struct ucsi_connector *con = container_of(work, struct ucsi_connector,
-						  work);
-	struct ucsi_connector_status constat;
-	struct ucsi *ucsi = con->ucsi;
-	struct ucsi_control ctrl;
-	int ret;
-
-	mutex_lock(&ucsi->ppm_lock);
-
-	ctrl.cmd.cmd = UCSI_GET_CONNECTOR_STATUS;
-	ctrl.cmd.length = 0;
-	ctrl.cmd.data = con->num;
-	ret = ucsi_run_cmd(con->ucsi, &ctrl, &constat, sizeof(constat));
-	if (ret) {
-		dev_err(ucsi->dev, "%s: failed to read connector status (%d)\n",
-			__func__, ret);
-		goto out_ack_event;
-	}
-
-	/* Ignoring disconnections and Alternate Modes */
-	if (!constat.connected || !(constat.change &
-	    (UCSI_CONSTAT_PARTNER_CHANGE | UCSI_CONSTAT_CONNECT_CHANGE)) ||
-	    constat.partner_flags & UCSI_CONSTAT_PARTNER_FLAG_ALT_MODE)
-		goto out_ack_event;
-
-	/* If the partner got USB Host role, attempting swap */
-	if (constat.partner_type & UCSI_CONSTAT_PARTNER_TYPE_DFP) {
-		ctrl.uor.cmd = UCSI_SET_UOR;
-		ctrl.uor.con_num = con->num;
-		ctrl.uor.role = UCSI_UOR_ROLE_DFP;
-
-		ret = ucsi_run_cmd(con->ucsi, &ctrl, NULL, 0);
-		if (ret)
-			dev_err(ucsi->dev, "%s: failed to swap role (%d)\n",
-				__func__, ret);
-	}
-out_ack_event:
-	ucsi_ack(ucsi, UCSI_ACK_EVENT);
-	clear_bit(EVENT_PENDING, &ucsi->flags);
-	mutex_unlock(&ucsi->ppm_lock);
-}
-
-static int ucsi_reset_ppm(struct ucsi *ucsi)
-{
-	int timeout = UCSI_TIMEOUT_MS;
-	struct ucsi_control ctrl;
-	int ret;
-
-	memset(&ctrl, 0, sizeof(ctrl));
-	ctrl.cmd.cmd = UCSI_PPM_RESET;
-	ret = ucsi_acpi_cmd(ucsi, &ctrl);
-	if (ret)
-		return ret;
-
-	/* There is no quarantee the PPM will ever set the RESET_COMPLETE bit */
-	while (!ucsi->data->cci.reset_complete && timeout--)
-		usleep_range(1000, 2000);
-	return 0;
-}
-
-static int ucsi_init(struct ucsi *ucsi)
-{
-	struct ucsi_connector *con;
-	struct ucsi_control ctrl;
-	int ret;
-	int i;
-
-	init_completion(&ucsi->complete);
-	spin_lock_init(&ucsi->dev_lock);
-	mutex_init(&ucsi->ppm_lock);
-
-	/* Reset the PPM */
-	ret = ucsi_reset_ppm(ucsi);
-	if (ret)
-		return ret;
-
-	/*
-	 * REVISIT: Executing second reset to WA an issue seen on some of the
-	 * Broxton based platforms, where the first reset puts the PPM into a
-	 * state where it's unable to recognise some of the commands.
-	 */
-	ret = ucsi_reset_ppm(ucsi);
-	if (ret)
-		return ret;
-
-	mutex_lock(&ucsi->ppm_lock);
-
-	/* Enable basic notifications */
-	ctrl.cmd.cmd = UCSI_SET_NOTIFICATION_ENABLE;
-	ctrl.cmd.length = 0;
-	ctrl.cmd.data = UCSI_ENABLE_NTFY_CMD_COMPLETE | UCSI_ENABLE_NTFY_ERROR;
-	ret = ucsi_run_cmd(ucsi, &ctrl, NULL, 0);
-	if (ret)
-		goto err_reset;
-
-	/* Get PPM capabilities */
-	ctrl.cmd.cmd = UCSI_GET_CAPABILITY;
-	ret = ucsi_run_cmd(ucsi, &ctrl, &ucsi->cap, sizeof(ucsi->cap));
-	if (ret)
-		goto err_reset;
-
-	if (!ucsi->cap.num_connectors) {
-		ret = -ENODEV;
-		goto err_reset;
-	}
-
-	ucsi->connector = devm_kcalloc(ucsi->dev, ucsi->cap.num_connectors,
-				       sizeof(*ucsi->connector), GFP_KERNEL);
-	if (!ucsi->connector) {
-		ret = -ENOMEM;
-		goto err_reset;
-	}
-
-	for (i = 1, con = ucsi->connector; i < ucsi->cap.num_connectors + 1;
-	     i++, con++) {
-		/* Get connector capability */
-		ctrl.cmd.cmd = UCSI_GET_CONNECTOR_CAPABILITY;
-		ctrl.cmd.data = i;
-		ret = ucsi_run_cmd(ucsi, &ctrl, &con->cap, sizeof(con->cap));
-		if (ret)
-			goto err_reset;
-
-		con->num = i;
-		con->ucsi = ucsi;
-		INIT_WORK(&con->work, ucsi_connector_change);
-	}
-
-	/* Enable all notifications */
-	ctrl.cmd.cmd = UCSI_SET_NOTIFICATION_ENABLE;
-	ctrl.cmd.data = UCSI_ENABLE_NTFY_ALL;
-	ret = ucsi_run_cmd(ucsi, &ctrl, NULL, 0);
-	if (ret < 0)
-		goto err_reset;
-
-	mutex_unlock(&ucsi->ppm_lock);
-	return 0;
-err_reset:
-	ucsi_reset_ppm(ucsi);
-	mutex_unlock(&ucsi->ppm_lock);
-	return ret;
-}
-
-static int ucsi_acpi_probe(struct platform_device *pdev)
-{
-	struct resource *res;
-	acpi_status status;
-	struct ucsi *ucsi;
-	int ret;
-
-	ucsi = devm_kzalloc(&pdev->dev, sizeof(*ucsi), GFP_KERNEL);
-	if (!ucsi)
-		return -ENOMEM;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "missing memory resource\n");
-		return -ENODEV;
-	}
-
-	/*
-	 * NOTE: ACPI has claimed the memory region as it's also an Operation
-	 * Region. It's not possible to request it in the driver.
-	 */
-	ucsi->data = devm_ioremap(&pdev->dev, res->start, resource_size(res));
-	if (!ucsi->data)
-		return -ENOMEM;
-
-	ucsi->dev = &pdev->dev;
-
-	status = acpi_install_notify_handler(ACPI_HANDLE(&pdev->dev),
-					     ACPI_ALL_NOTIFY,
-					     ucsi_acpi_notify, ucsi);
-	if (ACPI_FAILURE(status))
-		return -ENODEV;
-
-	ret = ucsi_init(ucsi);
-	if (ret) {
-		acpi_remove_notify_handler(ACPI_HANDLE(&pdev->dev),
-					   ACPI_ALL_NOTIFY,
-					   ucsi_acpi_notify);
-		return ret;
-	}
-
-	platform_set_drvdata(pdev, ucsi);
-	return 0;
-}
-
-static int ucsi_acpi_remove(struct platform_device *pdev)
-{
-	struct ucsi *ucsi = platform_get_drvdata(pdev);
-
-	acpi_remove_notify_handler(ACPI_HANDLE(&pdev->dev),
-				   ACPI_ALL_NOTIFY, ucsi_acpi_notify);
-
-	/* Make sure there are no events in the middle of being processed */
-	if (wait_on_bit_timeout(&ucsi->flags, EVENT_PENDING,
-				TASK_UNINTERRUPTIBLE,
-				msecs_to_jiffies(UCSI_TIMEOUT_MS)))
-		dev_WARN(ucsi->dev, "%s: Events still pending\n", __func__);
-
-	ucsi_reset_ppm(ucsi);
-	return 0;
-}
-
-static const struct acpi_device_id ucsi_acpi_match[] = {
-	{ "PNP0CA0", 0 },
-	{ },
-};
-MODULE_DEVICE_TABLE(acpi, ucsi_acpi_match);
-
-static struct platform_driver ucsi_acpi_platform_driver = {
-	.driver = {
-		.name = "ucsi_acpi",
-		.acpi_match_table = ACPI_PTR(ucsi_acpi_match),
-	},
-	.probe = ucsi_acpi_probe,
-	.remove = ucsi_acpi_remove,
-};
-
-module_platform_driver(ucsi_acpi_platform_driver);
-
-MODULE_AUTHOR("Heikki Krogerus <heikki.krogerus@linux.intel.com>");
-MODULE_LICENSE("GPL v2");
-MODULE_DESCRIPTION("USB Type-C System Software Interface (UCSI) driver");
diff --git a/drivers/usb/misc/usbsevseg.c b/drivers/usb/misc/usbsevseg.c
index a0ba5298160c..388fae6373db 100644
--- a/drivers/usb/misc/usbsevseg.c
+++ b/drivers/usb/misc/usbsevseg.c
@@ -33,7 +33,7 @@ static const struct usb_device_id id_table[] = {
 MODULE_DEVICE_TABLE(usb, id_table);
 
 /* the different text display modes the device is capable of */
-static char *display_textmodes[] = {"raw", "hex", "ascii", NULL};
+static const char *display_textmodes[] = {"raw", "hex", "ascii"};
 
 struct usb_sevsegdev {
 	struct usb_device *udev;
@@ -280,7 +280,7 @@ static ssize_t show_attr_textmode(struct device *dev,
 
 	buf[0] = 0;
 
-	for (i = 0; display_textmodes[i]; i++) {
+	for (i = 0; i < ARRAY_SIZE(display_textmodes); i++) {
 		if (mydev->textmode == i) {
 			strcat(buf, " [");
 			strcat(buf, display_textmodes[i]);
@@ -304,15 +304,13 @@ static ssize_t set_attr_textmode(struct device *dev,
 	struct usb_sevsegdev *mydev = usb_get_intfdata(intf);
 	int i;
 
-	for (i = 0; display_textmodes[i]; i++) {
-		if (sysfs_streq(display_textmodes[i], buf)) {
-			mydev->textmode = i;
-			update_display_visual(mydev, GFP_KERNEL);
-			return count;
-		}
-	}
+	i = sysfs_match_string(display_textmodes, buf);
+	if (i < 0)
+		return i;
 
-	return -EINVAL;
+	mydev->textmode = i;
+	update_display_visual(mydev, GFP_KERNEL);
+	return count;
 }
 
 static DEVICE_ATTR(textmode, S_IRUGO | S_IWUSR, show_attr_textmode, set_attr_textmode);
diff --git a/drivers/usb/mtu3/mtu3.h b/drivers/usb/mtu3/mtu3.h
index aa6fd6a51221..7b6dc23d77e9 100644
--- a/drivers/usb/mtu3/mtu3.h
+++ b/drivers/usb/mtu3/mtu3.h
@@ -356,12 +356,8 @@ static inline struct mtu3_ep *to_mtu3_ep(struct usb_ep *ep)
 
 static inline struct mtu3_request *next_request(struct mtu3_ep *mep)
 {
-	struct list_head *queue = &mep->req_list;
-
-	if (list_empty(queue))
-		return NULL;
-
-	return list_first_entry(queue, struct mtu3_request, list);
+	return list_first_entry_or_null(&mep->req_list, struct mtu3_request,
+					list);
 }
 
 static inline void mtu3_writel(void __iomem *base, u32 offset, u32 data)
diff --git a/drivers/usb/mtu3/mtu3_plat.c b/drivers/usb/mtu3/mtu3_plat.c
index 42550c7db3e7..0d3ebb353e08 100644
--- a/drivers/usb/mtu3/mtu3_plat.c
+++ b/drivers/usb/mtu3/mtu3_plat.c
@@ -458,6 +458,7 @@ static int __maybe_unused mtu3_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct ssusb_mtk *ssusb = platform_get_drvdata(pdev);
+	int ret;
 
 	dev_dbg(dev, "%s\n", __func__);
 
@@ -465,12 +466,28 @@ static int __maybe_unused mtu3_resume(struct device *dev)
 		return 0;
 
 	ssusb_wakeup_disable(ssusb);
-	clk_prepare_enable(ssusb->sys_clk);
-	clk_prepare_enable(ssusb->ref_clk);
-	ssusb_phy_power_on(ssusb);
+	ret = clk_prepare_enable(ssusb->sys_clk);
+	if (ret)
+		goto err_sys_clk;
+
+	ret = clk_prepare_enable(ssusb->ref_clk);
+	if (ret)
+		goto err_ref_clk;
+
+	ret = ssusb_phy_power_on(ssusb);
+	if (ret)
+		goto err_power_on;
+
 	ssusb_host_enable(ssusb);
 
 	return 0;
+
+err_power_on:
+	clk_disable_unprepare(ssusb->ref_clk);
+err_ref_clk:
+	clk_disable_unprepare(ssusb->sys_clk);
+err_sys_clk:
+	return ret;
 }
 
 static const struct dev_pm_ops mtu3_pm_ops = {
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index 870da18f5077..87cbd56cc761 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -2224,6 +2224,9 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 		musb->io.ep_select = musb_flat_ep_select;
 	}
 
+	if (musb->io.quirks & MUSB_G_NO_SKB_RESERVE)
+		musb->g.quirk_avoids_skb_reserve = 1;
+
 	/* At least tusb6010 has its own offsets */
 	if (musb->ops->ep_offset)
 		musb->io.ep_offset = musb->ops->ep_offset;
diff --git a/drivers/usb/musb/musb_core.h b/drivers/usb/musb/musb_core.h
index 3e98d4268a64..9f22c5b8ce37 100644
--- a/drivers/usb/musb/musb_core.h
+++ b/drivers/usb/musb/musb_core.h
@@ -172,6 +172,7 @@ struct musb_io;
  */
 struct musb_platform_ops {
 
+#define MUSB_G_NO_SKB_RESERVE	BIT(9)
 #define MUSB_DA8XX		BIT(8)
 #define MUSB_PRESERVE_SESSION	BIT(7)
 #define MUSB_DMA_UX500		BIT(6)
diff --git a/drivers/usb/musb/musb_cppi41.c b/drivers/usb/musb/musb_cppi41.c
index e7c8b1b8bf22..ba255280a624 100644
--- a/drivers/usb/musb/musb_cppi41.c
+++ b/drivers/usb/musb/musb_cppi41.c
@@ -673,12 +673,15 @@ static int cppi41_dma_controller_start(struct cppi41_dma_controller *controller)
 		musb_dma->status = MUSB_DMA_STATUS_FREE;
 		musb_dma->max_len = SZ_4M;
 
-		dc = dma_request_slave_channel(dev->parent, str);
-		if (!dc) {
-			dev_err(dev, "Failed to request %s.\n", str);
-			ret = -EPROBE_DEFER;
+		dc = dma_request_chan(dev->parent, str);
+		if (IS_ERR(dc)) {
+			ret = PTR_ERR(dc);
+			if (ret != -EPROBE_DEFER)
+				dev_err(dev, "Failed to request %s: %d.\n",
+					str, ret);
 			goto err;
 		}
+
 		cppi41_channel->dc = dc;
 	}
 	return 0;
diff --git a/drivers/usb/musb/musb_host.c b/drivers/usb/musb/musb_host.c
index dbe617a735d8..76decb8011eb 100644
--- a/drivers/usb/musb/musb_host.c
+++ b/drivers/usb/musb/musb_host.c
@@ -1540,7 +1540,7 @@ static int musb_rx_dma_iso_cppi41(struct dma_controller *dma,
 	struct dma_channel *channel = hw_ep->rx_channel;
 	void __iomem *epio = hw_ep->regs;
 	dma_addr_t *buf;
-	u32 length, res;
+	u32 length;
 	u16 val;
 
 	buf = (void *)urb->iso_frame_desc[qh->iso_idx].offset +
@@ -1552,10 +1552,8 @@ static int musb_rx_dma_iso_cppi41(struct dma_controller *dma,
 	val |= MUSB_RXCSR_DMAENAB;
 	musb_writew(hw_ep->regs, MUSB_RXCSR, val);
 
-	res = dma->channel_program(channel, qh->maxpacket, 0,
+	return dma->channel_program(channel, qh->maxpacket, 0,
 				   (u32)buf, length);
-
-	return res;
 }
 #else
 static inline int musb_rx_dma_iso_cppi41(struct dma_controller *dma,
diff --git a/drivers/usb/musb/tusb6010.c b/drivers/usb/musb/tusb6010.c
index e85cc8e4e7a9..4eb640c54f2c 100644
--- a/drivers/usb/musb/tusb6010.c
+++ b/drivers/usb/musb/tusb6010.c
@@ -881,26 +881,14 @@ static irqreturn_t tusb_musb_interrupt(int irq, void *__hci)
 				| TUSB_INT_SRC_ID_STATUS_CHNG))
 		idle_timeout = tusb_otg_ints(musb, int_src, tbase);
 
-	/* TX dma callback must be handled here, RX dma callback is
-	 * handled in tusb_omap_dma_cb.
+	/*
+	 * Just clear the DMA interrupt if it comes as the completion for both
+	 * TX and RX is handled by the DMA callback in tusb6010_omap
 	 */
 	if ((int_src & TUSB_INT_SRC_TXRX_DMA_DONE)) {
 		u32	dma_src = musb_readl(tbase, TUSB_DMA_INT_SRC);
-		u32	real_dma_src = musb_readl(tbase, TUSB_DMA_INT_MASK);
 
 		dev_dbg(musb->controller, "DMA IRQ %08x\n", dma_src);
-		real_dma_src = ~real_dma_src & dma_src;
-		if (tusb_dma_omap(musb) && real_dma_src) {
-			int	tx_source = (real_dma_src & 0xffff);
-			int	i;
-
-			for (i = 1; i <= 15; i++) {
-				if (tx_source & (1 << i)) {
-					dev_dbg(musb->controller, "completing ep%i %s\n", i, "tx");
-					musb_dma_completion(musb, i, 1);
-				}
-			}
-		}
 		musb_writel(tbase, TUSB_DMA_INT_CLEAR, dma_src);
 	}
 
@@ -1181,7 +1169,8 @@ static int tusb_musb_exit(struct musb *musb)
 }
 
 static const struct musb_platform_ops tusb_ops = {
-	.quirks		= MUSB_DMA_TUSB_OMAP | MUSB_IN_TUSB,
+	.quirks		= MUSB_DMA_TUSB_OMAP | MUSB_IN_TUSB |
+			  MUSB_G_NO_SKB_RESERVE,
 	.init		= tusb_musb_init,
 	.exit		= tusb_musb_exit,
 
diff --git a/drivers/usb/musb/tusb6010_omap.c b/drivers/usb/musb/tusb6010_omap.c
index 7870b37e0ea5..e8060e49b0f4 100644
--- a/drivers/usb/musb/tusb6010_omap.c
+++ b/drivers/usb/musb/tusb6010_omap.c
@@ -15,7 +15,7 @@
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/slab.h>
-#include <linux/omap-dma.h>
+#include <linux/dmaengine.h>
 
 #include "musb_core.h"
 #include "tusb6010.h"
@@ -24,12 +24,10 @@
 
 #define MAX_DMAREQ		5	/* REVISIT: Really 6, but req5 not OK */
 
-#define OMAP24XX_DMA_EXT_DMAREQ0	2
-#define OMAP24XX_DMA_EXT_DMAREQ1	3
-#define OMAP242X_DMA_EXT_DMAREQ2	14
-#define OMAP242X_DMA_EXT_DMAREQ3	15
-#define OMAP242X_DMA_EXT_DMAREQ4	16
-#define OMAP242X_DMA_EXT_DMAREQ5	64
+struct tusb_dma_data {
+	s8			dmareq;
+	struct dma_chan		*chan;
+};
 
 struct tusb_omap_dma_ch {
 	struct musb		*musb;
@@ -39,9 +37,7 @@ struct tusb_omap_dma_ch {
 	u8			tx;
 	struct musb_hw_ep	*hw_ep;
 
-	int			ch;
-	s8			dmareq;
-	s8			sync_dev;
+	struct tusb_dma_data	*dma_data;
 
 	struct tusb_omap_dma	*tusb_dma;
 
@@ -58,9 +54,7 @@ struct tusb_omap_dma {
 	struct dma_controller		controller;
 	void __iomem			*tbase;
 
-	int				ch;
-	s8				dmareq;
-	s8				sync_dev;
+	struct tusb_dma_data		dma_pool[MAX_DMAREQ];
 	unsigned			multichannel:1;
 };
 
@@ -103,7 +97,7 @@ static inline void tusb_omap_free_shared_dmareq(struct tusb_omap_dma_ch *chdat)
  * See also musb_dma_completion in plat_uds.c and musb_g_[tx|rx]() in
  * musb_gadget.c.
  */
-static void tusb_omap_dma_cb(int lch, u16 ch_status, void *data)
+static void tusb_omap_dma_cb(void *data)
 {
 	struct dma_channel	*channel = (struct dma_channel *)data;
 	struct tusb_omap_dma_ch	*chdat = to_chdat(channel);
@@ -114,21 +108,11 @@ static void tusb_omap_dma_cb(int lch, u16 ch_status, void *data)
 	void __iomem		*ep_conf = hw_ep->conf;
 	void __iomem		*mbase = musb->mregs;
 	unsigned long		remaining, flags, pio;
-	int			ch;
 
 	spin_lock_irqsave(&musb->lock, flags);
 
-	if (tusb_dma->multichannel)
-		ch = chdat->ch;
-	else
-		ch = tusb_dma->ch;
-
-	if (ch_status != OMAP_DMA_BLOCK_IRQ)
-		printk(KERN_ERR "TUSB DMA error status: %i\n", ch_status);
-
-	dev_dbg(musb->controller, "ep%i %s dma callback ch: %i status: %x\n",
-		chdat->epnum, chdat->tx ? "tx" : "rx",
-		ch, ch_status);
+	dev_dbg(musb->controller, "ep%i %s dma callback\n",
+		chdat->epnum, chdat->tx ? "tx" : "rx");
 
 	if (chdat->tx)
 		remaining = musb_readl(ep_conf, TUSB_EP_TX_OFFSET);
@@ -139,9 +123,8 @@ static void tusb_omap_dma_cb(int lch, u16 ch_status, void *data)
 
 	/* HW issue #10: XFR_SIZE may get corrupt on DMA (both async & sync) */
 	if (unlikely(remaining > chdat->transfer_len)) {
-		dev_dbg(musb->controller, "Corrupt %s dma ch%i XFR_SIZE: 0x%08lx\n",
-			chdat->tx ? "tx" : "rx", chdat->ch,
-			remaining);
+		dev_dbg(musb->controller, "Corrupt %s XFR_SIZE: 0x%08lx\n",
+			chdat->tx ? "tx" : "rx", remaining);
 		remaining = 0;
 	}
 
@@ -175,13 +158,7 @@ static void tusb_omap_dma_cb(int lch, u16 ch_status, void *data)
 
 	channel->status = MUSB_DMA_STATUS_FREE;
 
-	/* Handle only RX callbacks here. TX callbacks must be handled based
-	 * on the TUSB DMA status interrupt.
-	 * REVISIT: Use both TUSB DMA status interrupt and OMAP DMA callback
-	 * interrupt for RX and TX.
-	 */
-	if (!chdat->tx)
-		musb_dma_completion(musb, chdat->epnum, chdat->tx);
+	musb_dma_completion(musb, chdat->epnum, chdat->tx);
 
 	/* We must terminate short tx transfers manually by setting TXPKTRDY.
 	 * REVISIT: This same problem may occur with other MUSB dma as well.
@@ -214,15 +191,16 @@ static int tusb_omap_dma_program(struct dma_channel *channel, u16 packet_sz,
 	struct musb_hw_ep		*hw_ep = chdat->hw_ep;
 	void __iomem			*mbase = musb->mregs;
 	void __iomem			*ep_conf = hw_ep->conf;
-	dma_addr_t			fifo = hw_ep->fifo_sync;
-	struct omap_dma_channel_params	dma_params;
+	dma_addr_t			fifo_addr = hw_ep->fifo_sync;
 	u32				dma_remaining;
-	int				src_burst, dst_burst;
 	u16				csr;
 	u32				psize;
-	int				ch;
-	s8				dmareq;
-	s8				sync_dev;
+	struct tusb_dma_data		*dma_data;
+	struct dma_async_tx_descriptor	*dma_desc;
+	struct dma_slave_config		dma_cfg;
+	enum dma_transfer_direction	dma_dir;
+	u32				port_window;
+	int				ret;
 
 	if (unlikely(dma_addr & 0x1) || (len < 32) || (len > packet_sz))
 		return false;
@@ -248,9 +226,8 @@ static int tusb_omap_dma_program(struct dma_channel *channel, u16 packet_sz,
 
 	dma_remaining = TUSB_EP_CONFIG_XFR_SIZE(dma_remaining);
 	if (dma_remaining) {
-		dev_dbg(musb->controller, "Busy %s dma ch%i, not using: %08x\n",
-			chdat->tx ? "tx" : "rx", chdat->ch,
-			dma_remaining);
+		dev_dbg(musb->controller, "Busy %s dma, not using: %08x\n",
+			chdat->tx ? "tx" : "rx", dma_remaining);
 		return false;
 	}
 
@@ -261,27 +238,19 @@ static int tusb_omap_dma_program(struct dma_channel *channel, u16 packet_sz,
 	else
 		chdat->transfer_packet_sz = packet_sz;
 
-	if (tusb_dma->multichannel) {
-		ch = chdat->ch;
-		dmareq = chdat->dmareq;
-		sync_dev = chdat->sync_dev;
-	} else {
+	dma_data = chdat->dma_data;
+	if (!tusb_dma->multichannel) {
 		if (tusb_omap_use_shared_dmareq(chdat) != 0) {
 			dev_dbg(musb->controller, "could not get dma for ep%i\n", chdat->epnum);
 			return false;
 		}
-		if (tusb_dma->ch < 0) {
+		if (dma_data->dmareq < 0) {
 			/* REVISIT: This should get blocked earlier, happens
 			 * with MSC ErrorRecoveryTest
 			 */
 			WARN_ON(1);
 			return false;
 		}
-
-		ch = tusb_dma->ch;
-		dmareq = tusb_dma->dmareq;
-		sync_dev = tusb_dma->sync_dev;
-		omap_set_dma_callback(ch, tusb_omap_dma_cb, channel);
 	}
 
 	chdat->packet_sz = packet_sz;
@@ -291,92 +260,80 @@ static int tusb_omap_dma_program(struct dma_channel *channel, u16 packet_sz,
 	channel->status = MUSB_DMA_STATUS_BUSY;
 
 	/* Since we're recycling dma areas, we need to clean or invalidate */
-	if (chdat->tx)
+	if (chdat->tx) {
+		dma_dir = DMA_MEM_TO_DEV;
 		dma_map_single(dev, phys_to_virt(dma_addr), len,
 				DMA_TO_DEVICE);
-	else
+	} else {
+		dma_dir = DMA_DEV_TO_MEM;
 		dma_map_single(dev, phys_to_virt(dma_addr), len,
 				DMA_FROM_DEVICE);
+	}
+
+	memset(&dma_cfg, 0, sizeof(dma_cfg));
 
 	/* Use 16-bit transfer if dma_addr is not 32-bit aligned */
 	if ((dma_addr & 0x3) == 0) {
-		dma_params.data_type = OMAP_DMA_DATA_TYPE_S32;
-		dma_params.elem_count = 8;		/* Elements in frame */
+		dma_cfg.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dma_cfg.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+		port_window = 8;
 	} else {
-		dma_params.data_type = OMAP_DMA_DATA_TYPE_S16;
-		dma_params.elem_count = 16;		/* Elements in frame */
-		fifo = hw_ep->fifo_async;
-	}
+		dma_cfg.src_addr_width = DMA_SLAVE_BUSWIDTH_2_BYTES;
+		dma_cfg.dst_addr_width = DMA_SLAVE_BUSWIDTH_2_BYTES;
+		port_window = 16;
 
-	dma_params.frame_count	= chdat->transfer_len / 32; /* Burst sz frame */
+		fifo_addr = hw_ep->fifo_async;
+	}
 
-	dev_dbg(musb->controller, "ep%i %s dma ch%i dma: %pad len: %u(%u) packet_sz: %i(%i)\n",
-		chdat->epnum, chdat->tx ? "tx" : "rx",
-		ch, &dma_addr, chdat->transfer_len, len,
-		chdat->transfer_packet_sz, packet_sz);
+	dev_dbg(musb->controller,
+		"ep%i %s dma: %pad len: %u(%u) packet_sz: %i(%i)\n",
+		chdat->epnum, chdat->tx ? "tx" : "rx", &dma_addr,
+		chdat->transfer_len, len, chdat->transfer_packet_sz, packet_sz);
+
+	dma_cfg.src_addr = fifo_addr;
+	dma_cfg.dst_addr = fifo_addr;
+	dma_cfg.src_port_window_size = port_window;
+	dma_cfg.src_maxburst = port_window;
+	dma_cfg.dst_port_window_size = port_window;
+	dma_cfg.dst_maxburst = port_window;
+
+	ret = dmaengine_slave_config(dma_data->chan, &dma_cfg);
+	if (ret) {
+		dev_err(musb->controller, "DMA slave config failed: %d\n", ret);
+		return false;
+	}
 
-	/*
-	 * Prepare omap DMA for transfer
-	 */
-	if (chdat->tx) {
-		dma_params.src_amode	= OMAP_DMA_AMODE_POST_INC;
-		dma_params.src_start	= (unsigned long)dma_addr;
-		dma_params.src_ei	= 0;
-		dma_params.src_fi	= 0;
-
-		dma_params.dst_amode	= OMAP_DMA_AMODE_DOUBLE_IDX;
-		dma_params.dst_start	= (unsigned long)fifo;
-		dma_params.dst_ei	= 1;
-		dma_params.dst_fi	= -31;	/* Loop 32 byte window */
-
-		dma_params.trigger	= sync_dev;
-		dma_params.sync_mode	= OMAP_DMA_SYNC_FRAME;
-		dma_params.src_or_dst_synch	= 0;	/* Dest sync */
-
-		src_burst = OMAP_DMA_DATA_BURST_16;	/* 16x32 read */
-		dst_burst = OMAP_DMA_DATA_BURST_8;	/* 8x32 write */
-	} else {
-		dma_params.src_amode	= OMAP_DMA_AMODE_DOUBLE_IDX;
-		dma_params.src_start	= (unsigned long)fifo;
-		dma_params.src_ei	= 1;
-		dma_params.src_fi	= -31;	/* Loop 32 byte window */
-
-		dma_params.dst_amode	= OMAP_DMA_AMODE_POST_INC;
-		dma_params.dst_start	= (unsigned long)dma_addr;
-		dma_params.dst_ei	= 0;
-		dma_params.dst_fi	= 0;
-
-		dma_params.trigger	= sync_dev;
-		dma_params.sync_mode	= OMAP_DMA_SYNC_FRAME;
-		dma_params.src_or_dst_synch	= 1;	/* Source sync */
-
-		src_burst = OMAP_DMA_DATA_BURST_8;	/* 8x32 read */
-		dst_burst = OMAP_DMA_DATA_BURST_16;	/* 16x32 write */
+	dma_desc = dmaengine_prep_slave_single(dma_data->chan, dma_addr,
+					chdat->transfer_len, dma_dir,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!dma_desc) {
+		dev_err(musb->controller, "DMA prep_slave_single failed\n");
+		return false;
 	}
 
-	dev_dbg(musb->controller, "ep%i %s using %i-bit %s dma from 0x%08lx to 0x%08lx\n",
+	dma_desc->callback = tusb_omap_dma_cb;
+	dma_desc->callback_param = channel;
+	dmaengine_submit(dma_desc);
+
+	dev_dbg(musb->controller,
+		"ep%i %s using %i-bit %s dma from %pad to %pad\n",
 		chdat->epnum, chdat->tx ? "tx" : "rx",
-		(dma_params.data_type == OMAP_DMA_DATA_TYPE_S32) ? 32 : 16,
+		dma_cfg.src_addr_width * 8,
 		((dma_addr & 0x3) == 0) ? "sync" : "async",
-		dma_params.src_start, dma_params.dst_start);
-
-	omap_set_dma_params(ch, &dma_params);
-	omap_set_dma_src_burst_mode(ch, src_burst);
-	omap_set_dma_dest_burst_mode(ch, dst_burst);
-	omap_set_dma_write_mode(ch, OMAP_DMA_WRITE_LAST_NON_POSTED);
+		(dma_dir == DMA_MEM_TO_DEV) ? &dma_addr : &fifo_addr,
+		(dma_dir == DMA_MEM_TO_DEV) ? &fifo_addr : &dma_addr);
 
 	/*
 	 * Prepare MUSB for DMA transfer
 	 */
+	musb_ep_select(mbase, chdat->epnum);
 	if (chdat->tx) {
-		musb_ep_select(mbase, chdat->epnum);
 		csr = musb_readw(hw_ep->regs, MUSB_TXCSR);
 		csr |= (MUSB_TXCSR_AUTOSET | MUSB_TXCSR_DMAENAB
 			| MUSB_TXCSR_DMAMODE | MUSB_TXCSR_MODE);
 		csr &= ~MUSB_TXCSR_P_UNDERRUN;
 		musb_writew(hw_ep->regs, MUSB_TXCSR, csr);
 	} else {
-		musb_ep_select(mbase, chdat->epnum);
 		csr = musb_readw(hw_ep->regs, MUSB_RXCSR);
 		csr |= MUSB_RXCSR_DMAENAB;
 		csr &= ~(MUSB_RXCSR_AUTOCLEAR | MUSB_RXCSR_DMAMODE);
@@ -384,10 +341,8 @@ static int tusb_omap_dma_program(struct dma_channel *channel, u16 packet_sz,
 			csr | MUSB_RXCSR_P_WZC_BITS);
 	}
 
-	/*
-	 * Start DMA transfer
-	 */
-	omap_start_dma(ch);
+	/* Start DMA transfer */
+	dma_async_issue_pending(dma_data->chan);
 
 	if (chdat->tx) {
 		/* Send transfer_packet_sz packets at a time */
@@ -415,18 +370,9 @@ static int tusb_omap_dma_program(struct dma_channel *channel, u16 packet_sz,
 static int tusb_omap_dma_abort(struct dma_channel *channel)
 {
 	struct tusb_omap_dma_ch	*chdat = to_chdat(channel);
-	struct tusb_omap_dma	*tusb_dma = chdat->tusb_dma;
 
-	if (!tusb_dma->multichannel) {
-		if (tusb_dma->ch >= 0) {
-			omap_stop_dma(tusb_dma->ch);
-			omap_free_dma(tusb_dma->ch);
-			tusb_dma->ch = -1;
-		}
-
-		tusb_dma->dmareq = -1;
-		tusb_dma->sync_dev = -1;
-	}
+	if (chdat->dma_data)
+		dmaengine_terminate_all(chdat->dma_data->chan);
 
 	channel->status = MUSB_DMA_STATUS_FREE;
 
@@ -438,15 +384,6 @@ static inline int tusb_omap_dma_allocate_dmareq(struct tusb_omap_dma_ch *chdat)
 	u32		reg = musb_readl(chdat->tbase, TUSB_DMA_EP_MAP);
 	int		i, dmareq_nr = -1;
 
-	const int sync_dev[6] = {
-		OMAP24XX_DMA_EXT_DMAREQ0,
-		OMAP24XX_DMA_EXT_DMAREQ1,
-		OMAP242X_DMA_EXT_DMAREQ2,
-		OMAP242X_DMA_EXT_DMAREQ3,
-		OMAP242X_DMA_EXT_DMAREQ4,
-		OMAP242X_DMA_EXT_DMAREQ5,
-	};
-
 	for (i = 0; i < MAX_DMAREQ; i++) {
 		int cur = (reg & (0xf << (i * 5))) >> (i * 5);
 		if (cur == 0) {
@@ -463,8 +400,7 @@ static inline int tusb_omap_dma_allocate_dmareq(struct tusb_omap_dma_ch *chdat)
 		reg |= ((1 << 4) << (dmareq_nr * 5));
 	musb_writel(chdat->tbase, TUSB_DMA_EP_MAP, reg);
 
-	chdat->dmareq = dmareq_nr;
-	chdat->sync_dev = sync_dev[chdat->dmareq];
+	chdat->dma_data = &chdat->tusb_dma->dma_pool[dmareq_nr];
 
 	return 0;
 }
@@ -473,15 +409,14 @@ static inline void tusb_omap_dma_free_dmareq(struct tusb_omap_dma_ch *chdat)
 {
 	u32 reg;
 
-	if (!chdat || chdat->dmareq < 0)
+	if (!chdat || !chdat->dma_data || chdat->dma_data->dmareq < 0)
 		return;
 
 	reg = musb_readl(chdat->tbase, TUSB_DMA_EP_MAP);
-	reg &= ~(0x1f << (chdat->dmareq * 5));
+	reg &= ~(0x1f << (chdat->dma_data->dmareq * 5));
 	musb_writel(chdat->tbase, TUSB_DMA_EP_MAP, reg);
 
-	chdat->dmareq = -1;
-	chdat->sync_dev = -1;
+	chdat->dma_data = NULL;
 }
 
 static struct dma_channel *dma_channel_pool[MAX_DMAREQ];
@@ -492,24 +427,14 @@ tusb_omap_dma_allocate(struct dma_controller *c,
 		u8 tx)
 {
 	int ret, i;
-	const char		*dev_name;
 	struct tusb_omap_dma	*tusb_dma;
 	struct musb		*musb;
-	void __iomem		*tbase;
 	struct dma_channel	*channel = NULL;
 	struct tusb_omap_dma_ch	*chdat = NULL;
-	u32			reg;
+	struct tusb_dma_data	*dma_data = NULL;
 
 	tusb_dma = container_of(c, struct tusb_omap_dma, controller);
 	musb = tusb_dma->controller.musb;
-	tbase = musb->ctrl_base;
-
-	reg = musb_readl(tbase, TUSB_DMA_INT_MASK);
-	if (tx)
-		reg &= ~(1 << hw_ep->epnum);
-	else
-		reg &= ~(1 << (hw_ep->epnum + 15));
-	musb_writel(tbase, TUSB_DMA_INT_MASK, reg);
 
 	/* REVISIT: Why does dmareq5 not work? */
 	if (hw_ep->epnum == 0) {
@@ -530,56 +455,38 @@ tusb_omap_dma_allocate(struct dma_controller *c,
 	if (!channel)
 		return NULL;
 
-	if (tx) {
-		chdat->tx = 1;
-		dev_name = "TUSB transmit";
-	} else {
-		chdat->tx = 0;
-		dev_name = "TUSB receive";
-	}
-
 	chdat->musb = tusb_dma->controller.musb;
 	chdat->tbase = tusb_dma->tbase;
 	chdat->hw_ep = hw_ep;
 	chdat->epnum = hw_ep->epnum;
-	chdat->dmareq = -1;
 	chdat->completed_len = 0;
 	chdat->tusb_dma = tusb_dma;
+	if (tx)
+		chdat->tx = 1;
+	else
+		chdat->tx = 0;
 
 	channel->max_len = 0x7fffffff;
 	channel->desired_mode = 0;
 	channel->actual_len = 0;
 
-	if (tusb_dma->multichannel) {
-		ret = tusb_omap_dma_allocate_dmareq(chdat);
-		if (ret != 0)
-			goto free_dmareq;
-
-		ret = omap_request_dma(chdat->sync_dev, dev_name,
-				tusb_omap_dma_cb, channel, &chdat->ch);
-		if (ret != 0)
-			goto free_dmareq;
-	} else if (tusb_dma->ch == -1) {
-		tusb_dma->dmareq = 0;
-		tusb_dma->sync_dev = OMAP24XX_DMA_EXT_DMAREQ0;
-
-		/* Callback data gets set later in the shared dmareq case */
-		ret = omap_request_dma(tusb_dma->sync_dev, "TUSB shared",
-				tusb_omap_dma_cb, NULL, &tusb_dma->ch);
-		if (ret != 0)
-			goto free_dmareq;
-
-		chdat->dmareq = -1;
-		chdat->ch = -1;
+	if (!chdat->dma_data) {
+		if (tusb_dma->multichannel) {
+			ret = tusb_omap_dma_allocate_dmareq(chdat);
+			if (ret != 0)
+				goto free_dmareq;
+		} else {
+			chdat->dma_data = &tusb_dma->dma_pool[0];
+		}
 	}
 
-	dev_dbg(musb->controller, "ep%i %s dma: %s dma%i dmareq%i sync%i\n",
+	dma_data = chdat->dma_data;
+
+	dev_dbg(musb->controller, "ep%i %s dma: %s dmareq%i\n",
 		chdat->epnum,
 		chdat->tx ? "tx" : "rx",
-		chdat->ch >= 0 ? "dedicated" : "shared",
-		chdat->ch >= 0 ? chdat->ch : tusb_dma->ch,
-		chdat->dmareq >= 0 ? chdat->dmareq : tusb_dma->dmareq,
-		chdat->sync_dev >= 0 ? chdat->sync_dev : tusb_dma->sync_dev);
+		tusb_dma->multichannel ? "shared" : "dedicated",
+		dma_data->dmareq);
 
 	return channel;
 
@@ -596,35 +503,13 @@ static void tusb_omap_dma_release(struct dma_channel *channel)
 {
 	struct tusb_omap_dma_ch	*chdat = to_chdat(channel);
 	struct musb		*musb = chdat->musb;
-	void __iomem		*tbase = musb->ctrl_base;
-	u32			reg;
-
-	dev_dbg(musb->controller, "ep%i ch%i\n", chdat->epnum, chdat->ch);
-
-	reg = musb_readl(tbase, TUSB_DMA_INT_MASK);
-	if (chdat->tx)
-		reg |= (1 << chdat->epnum);
-	else
-		reg |= (1 << (chdat->epnum + 15));
-	musb_writel(tbase, TUSB_DMA_INT_MASK, reg);
 
-	reg = musb_readl(tbase, TUSB_DMA_INT_CLEAR);
-	if (chdat->tx)
-		reg |= (1 << chdat->epnum);
-	else
-		reg |= (1 << (chdat->epnum + 15));
-	musb_writel(tbase, TUSB_DMA_INT_CLEAR, reg);
+	dev_dbg(musb->controller, "Release for ep%i\n", chdat->epnum);
 
 	channel->status = MUSB_DMA_STATUS_UNKNOWN;
 
-	if (chdat->ch >= 0) {
-		omap_stop_dma(chdat->ch);
-		omap_free_dma(chdat->ch);
-		chdat->ch = -1;
-	}
-
-	if (chdat->dmareq >= 0)
-		tusb_omap_dma_free_dmareq(chdat);
+	dmaengine_terminate_sync(chdat->dma_data->chan);
+	tusb_omap_dma_free_dmareq(chdat);
 
 	channel = NULL;
 }
@@ -641,15 +526,62 @@ void tusb_dma_controller_destroy(struct dma_controller *c)
 			kfree(ch->private_data);
 			kfree(ch);
 		}
-	}
 
-	if (tusb_dma && !tusb_dma->multichannel && tusb_dma->ch >= 0)
-		omap_free_dma(tusb_dma->ch);
+		/* Free up the DMA channels */
+		if (tusb_dma && tusb_dma->dma_pool[i].chan)
+			dma_release_channel(tusb_dma->dma_pool[i].chan);
+	}
 
 	kfree(tusb_dma);
 }
 EXPORT_SYMBOL_GPL(tusb_dma_controller_destroy);
 
+static int tusb_omap_allocate_dma_pool(struct tusb_omap_dma *tusb_dma)
+{
+	struct musb *musb = tusb_dma->controller.musb;
+	int i;
+	int ret = 0;
+
+	for (i = 0; i < MAX_DMAREQ; i++) {
+		struct tusb_dma_data *dma_data = &tusb_dma->dma_pool[i];
+
+		/*
+		 * Request DMA channels:
+		 * - one channel in case of non multichannel mode
+		 * - MAX_DMAREQ number of channels in multichannel mode
+		 */
+		if (i == 0 || tusb_dma->multichannel) {
+			char ch_name[8];
+
+			sprintf(ch_name, "dmareq%d", i);
+			dma_data->chan = dma_request_chan(musb->controller,
+							  ch_name);
+			if (IS_ERR(dma_data->chan)) {
+				dev_err(musb->controller,
+					"Failed to request %s\n", ch_name);
+				ret = PTR_ERR(dma_data->chan);
+				goto dma_error;
+			}
+
+			dma_data->dmareq = i;
+		} else {
+			dma_data->dmareq = -1;
+		}
+	}
+
+	return 0;
+
+dma_error:
+	for (; i >= 0; i--) {
+		struct tusb_dma_data *dma_data = &tusb_dma->dma_pool[i];
+
+		if (dma_data->dmareq >= 0)
+			dma_release_channel(dma_data->chan);
+	}
+
+	return ret;
+}
+
 struct dma_controller *
 tusb_dma_controller_create(struct musb *musb, void __iomem *base)
 {
@@ -674,10 +606,6 @@ tusb_dma_controller_create(struct musb *musb, void __iomem *base)
 	tusb_dma->controller.musb = musb;
 	tusb_dma->tbase = musb->ctrl_base;
 
-	tusb_dma->ch = -1;
-	tusb_dma->dmareq = -1;
-	tusb_dma->sync_dev = -1;
-
 	tusb_dma->controller.channel_alloc = tusb_omap_dma_allocate;
 	tusb_dma->controller.channel_release = tusb_omap_dma_release;
 	tusb_dma->controller.channel_program = tusb_omap_dma_program;
@@ -704,6 +632,9 @@ tusb_dma_controller_create(struct musb *musb, void __iomem *base)
 		ch->private_data = chdat;
 	}
 
+	if (tusb_omap_allocate_dma_pool(tusb_dma))
+		goto cleanup;
+
 	return &tusb_dma->controller;
 
 cleanup:
diff --git a/drivers/usb/phy/Kconfig b/drivers/usb/phy/Kconfig
index 3006f569c068..aff702c0eb9f 100644
--- a/drivers/usb/phy/Kconfig
+++ b/drivers/usb/phy/Kconfig
@@ -4,6 +4,7 @@
 menu "USB Physical Layer drivers"
 
 config USB_PHY
+	select EXTCON
 	def_bool n
 
 #
@@ -109,7 +110,7 @@ config OMAP_OTG
 
 config TAHVO_USB
 	tristate "Tahvo USB transceiver driver"
-	depends on MFD_RETU && EXTCON
+	depends on MFD_RETU
 	depends on USB_GADGET || !USB_GADGET # if USB_GADGET=m, this can't be 'y'
 	select USB_PHY
 	help
@@ -141,7 +142,6 @@ config USB_MSM_OTG
 	depends on (USB || USB_GADGET) && (ARCH_QCOM || COMPILE_TEST)
 	depends on USB_GADGET || !USB_GADGET # if USB_GADGET=m, this can't be 'y'
 	depends on RESET_CONTROLLER
-	depends on EXTCON
 	select USB_PHY
 	help
 	  Enable this to support the USB OTG transceiver on Qualcomm chips. It
@@ -155,7 +155,7 @@ config USB_MSM_OTG
 config USB_QCOM_8X16_PHY
 	tristate "Qualcomm APQ8016/MSM8916 on-chip USB PHY controller support"
 	depends on ARCH_QCOM || COMPILE_TEST
-	depends on RESET_CONTROLLER && EXTCON
+	depends on RESET_CONTROLLER
 	select USB_PHY
 	select USB_ULPI_VIEWPORT
 	help
diff --git a/drivers/usb/phy/phy-msm-usb.c b/drivers/usb/phy/phy-msm-usb.c
index 93d9aaad2994..8fb86a5f458e 100644
--- a/drivers/usb/phy/phy-msm-usb.c
+++ b/drivers/usb/phy/phy-msm-usb.c
@@ -146,17 +146,6 @@ struct msm_otg_platform_data {
 };
 
 /**
- * struct msm_usb_cable - structure for exteternal connector cable
- *			  state tracking
- * @nb: hold event notification callback
- * @conn: used for notification registration
- */
-struct msm_usb_cable {
-	struct notifier_block		nb;
-	struct extcon_dev		*extcon;
-};
-
-/**
  * struct msm_otg: OTG driver data. Shared by HCD and DCD.
  * @otg: USB OTG Transceiver structure.
  * @pdata: otg device platform data.
@@ -215,9 +204,6 @@ struct msm_otg {
 
 	bool manual_pullup;
 
-	struct msm_usb_cable vbus;
-	struct msm_usb_cable id;
-
 	struct gpio_desc *switch_gpio;
 	struct notifier_block reboot;
 };
@@ -1612,8 +1598,8 @@ MODULE_DEVICE_TABLE(of, msm_otg_dt_match);
 static int msm_otg_vbus_notifier(struct notifier_block *nb, unsigned long event,
 				void *ptr)
 {
-	struct msm_usb_cable *vbus = container_of(nb, struct msm_usb_cable, nb);
-	struct msm_otg *motg = container_of(vbus, struct msm_otg, vbus);
+	struct usb_phy *usb_phy = container_of(nb, struct usb_phy, vbus_nb);
+	struct msm_otg *motg = container_of(usb_phy, struct msm_otg, phy);
 
 	if (event)
 		set_bit(B_SESS_VLD, &motg->inputs);
@@ -1636,8 +1622,8 @@ static int msm_otg_vbus_notifier(struct notifier_block *nb, unsigned long event,
 static int msm_otg_id_notifier(struct notifier_block *nb, unsigned long event,
 				void *ptr)
 {
-	struct msm_usb_cable *id = container_of(nb, struct msm_usb_cable, nb);
-	struct msm_otg *motg = container_of(id, struct msm_otg, id);
+	struct usb_phy *usb_phy = container_of(nb, struct usb_phy, id_nb);
+	struct msm_otg *motg = container_of(usb_phy, struct msm_otg, phy);
 
 	if (event)
 		clear_bit(ID, &motg->inputs);
@@ -1652,7 +1638,6 @@ static int msm_otg_id_notifier(struct notifier_block *nb, unsigned long event,
 static int msm_otg_read_dt(struct platform_device *pdev, struct msm_otg *motg)
 {
 	struct msm_otg_platform_data *pdata;
-	struct extcon_dev *ext_id, *ext_vbus;
 	struct device_node *node = pdev->dev.of_node;
 	struct property *prop;
 	int len, ret, words;
@@ -1708,54 +1693,6 @@ static int msm_otg_read_dt(struct platform_device *pdev, struct msm_otg *motg)
 	if (IS_ERR(motg->switch_gpio))
 		return PTR_ERR(motg->switch_gpio);
 
-	ext_id = ERR_PTR(-ENODEV);
-	ext_vbus = ERR_PTR(-ENODEV);
-	if (of_property_read_bool(node, "extcon")) {
-
-		/* Each one of them is not mandatory */
-		ext_vbus = extcon_get_edev_by_phandle(&pdev->dev, 0);
-		if (IS_ERR(ext_vbus) && PTR_ERR(ext_vbus) != -ENODEV)
-			return PTR_ERR(ext_vbus);
-
-		ext_id = extcon_get_edev_by_phandle(&pdev->dev, 1);
-		if (IS_ERR(ext_id) && PTR_ERR(ext_id) != -ENODEV)
-			return PTR_ERR(ext_id);
-	}
-
-	if (!IS_ERR(ext_vbus)) {
-		motg->vbus.extcon = ext_vbus;
-		motg->vbus.nb.notifier_call = msm_otg_vbus_notifier;
-		ret = devm_extcon_register_notifier(&pdev->dev, ext_vbus,
-						EXTCON_USB, &motg->vbus.nb);
-		if (ret < 0) {
-			dev_err(&pdev->dev, "register VBUS notifier failed\n");
-			return ret;
-		}
-
-		ret = extcon_get_state(ext_vbus, EXTCON_USB);
-		if (ret)
-			set_bit(B_SESS_VLD, &motg->inputs);
-		else
-			clear_bit(B_SESS_VLD, &motg->inputs);
-	}
-
-	if (!IS_ERR(ext_id)) {
-		motg->id.extcon = ext_id;
-		motg->id.nb.notifier_call = msm_otg_id_notifier;
-		ret = devm_extcon_register_notifier(&pdev->dev, ext_id,
-						EXTCON_USB_HOST, &motg->id.nb);
-		if (ret < 0) {
-			dev_err(&pdev->dev, "register ID notifier failed\n");
-			return ret;
-		}
-
-		ret = extcon_get_state(ext_id, EXTCON_USB_HOST);
-		if (ret)
-			clear_bit(ID, &motg->inputs);
-		else
-			set_bit(ID, &motg->inputs);
-	}
-
 	prop = of_find_property(node, "qcom,phy-init-sequence", &len);
 	if (!prop || !len)
 		return 0;
@@ -1932,6 +1869,8 @@ static int msm_otg_probe(struct platform_device *pdev)
 	phy->init = msm_phy_init;
 	phy->notify_disconnect = msm_phy_notify_disconnect;
 	phy->type = USB_PHY_TYPE_USB2;
+	phy->vbus_nb.notifier_call = msm_otg_vbus_notifier;
+	phy->id_nb.notifier_call = msm_otg_id_notifier;
 
 	phy->io_ops = &msm_otg_io_ops;
 
@@ -1947,6 +1886,18 @@ static int msm_otg_probe(struct platform_device *pdev)
 		goto disable_ldo;
 	}
 
+	ret = extcon_get_state(phy->edev, EXTCON_USB);
+	if (ret)
+		set_bit(B_SESS_VLD, &motg->inputs);
+	else
+		clear_bit(B_SESS_VLD, &motg->inputs);
+
+	ret = extcon_get_state(phy->id_edev, EXTCON_USB_HOST);
+	if (ret)
+		clear_bit(ID, &motg->inputs);
+	else
+		set_bit(ID, &motg->inputs);
+
 	platform_set_drvdata(pdev, motg);
 	device_init_wakeup(&pdev->dev, 1);
 
diff --git a/drivers/usb/phy/phy-qcom-8x16-usb.c b/drivers/usb/phy/phy-qcom-8x16-usb.c
index fdf686398772..b6a83a5cbad3 100644
--- a/drivers/usb/phy/phy-qcom-8x16-usb.c
+++ b/drivers/usb/phy/phy-qcom-8x16-usb.c
@@ -69,9 +69,6 @@ struct phy_8x16 {
 
 	struct reset_control		*phy_reset;
 
-	struct extcon_dev		*vbus_edev;
-	struct notifier_block		vbus_notify;
-
 	struct gpio_desc		*switch_gpio;
 	struct notifier_block		reboot_notify;
 };
@@ -131,7 +128,8 @@ static int phy_8x16_vbus_off(struct phy_8x16 *qphy)
 static int phy_8x16_vbus_notify(struct notifier_block *nb, unsigned long event,
 				void *ptr)
 {
-	struct phy_8x16 *qphy = container_of(nb, struct phy_8x16, vbus_notify);
+	struct usb_phy *usb_phy = container_of(nb, struct usb_phy, vbus_nb);
+	struct phy_8x16 *qphy = container_of(usb_phy, struct phy_8x16, phy);
 
 	if (event)
 		phy_8x16_vbus_on(qphy);
@@ -187,7 +185,7 @@ static int phy_8x16_init(struct usb_phy *phy)
 	val = ULPI_PWR_OTG_COMP_DISABLE;
 	usb_phy_io_write(phy, val, ULPI_SET(ULPI_PWR_CLK_MNG_REG));
 
-	state = extcon_get_state(qphy->vbus_edev, EXTCON_USB);
+	state = extcon_get_state(qphy->phy.edev, EXTCON_USB);
 	if (state)
 		phy_8x16_vbus_on(qphy);
 	else
@@ -289,15 +287,13 @@ static int phy_8x16_probe(struct platform_device *pdev)
 	phy->io_priv		= qphy->regs + HSPHY_ULPI_VIEWPORT;
 	phy->io_ops		= &ulpi_viewport_access_ops;
 	phy->type		= USB_PHY_TYPE_USB2;
+	phy->vbus_nb.notifier_call = phy_8x16_vbus_notify;
+	phy->id_nb.notifier_call = NULL;
 
 	ret = phy_8x16_read_devicetree(qphy);
 	if (ret < 0)
 		return ret;
 
-	qphy->vbus_edev = extcon_get_edev_by_phandle(phy->dev, 0);
-	if (IS_ERR(qphy->vbus_edev))
-		return PTR_ERR(qphy->vbus_edev);
-
 	ret = clk_set_rate(qphy->core_clk, INT_MAX);
 	if (ret < 0)
 		dev_dbg(phy->dev, "Can't boost core clock\n");
@@ -315,12 +311,6 @@ static int phy_8x16_probe(struct platform_device *pdev)
 	if (WARN_ON(ret))
 		goto off_clks;
 
-	qphy->vbus_notify.notifier_call = phy_8x16_vbus_notify;
-	ret = devm_extcon_register_notifier(&pdev->dev, qphy->vbus_edev,
-					EXTCON_USB, &qphy->vbus_notify);
-	if (ret < 0)
-		goto off_power;
-
 	ret = usb_add_phy_dev(&qphy->phy);
 	if (ret)
 		goto off_power;
diff --git a/drivers/usb/phy/phy.c b/drivers/usb/phy/phy.c
index 98f75d2842b7..032f5afaad4b 100644
--- a/drivers/usb/phy/phy.c
+++ b/drivers/usb/phy/phy.c
@@ -100,6 +100,54 @@ static int devm_usb_phy_match(struct device *dev, void *res, void *match_data)
 	return *phy == match_data;
 }
 
+static int usb_add_extcon(struct usb_phy *x)
+{
+	int ret;
+
+	if (of_property_read_bool(x->dev->of_node, "extcon")) {
+		x->edev = extcon_get_edev_by_phandle(x->dev, 0);
+		if (IS_ERR(x->edev))
+			return PTR_ERR(x->edev);
+
+		x->id_edev = extcon_get_edev_by_phandle(x->dev, 1);
+		if (IS_ERR(x->id_edev)) {
+			x->id_edev = NULL;
+			dev_info(x->dev, "No separate ID extcon device\n");
+		}
+
+		if (x->vbus_nb.notifier_call) {
+			ret = devm_extcon_register_notifier(x->dev, x->edev,
+							    EXTCON_USB,
+							    &x->vbus_nb);
+			if (ret < 0) {
+				dev_err(x->dev,
+					"register VBUS notifier failed\n");
+				return ret;
+			}
+		}
+
+		if (x->id_nb.notifier_call) {
+			struct extcon_dev *id_ext;
+
+			if (x->id_edev)
+				id_ext = x->id_edev;
+			else
+				id_ext = x->edev;
+
+			ret = devm_extcon_register_notifier(x->dev, id_ext,
+							    EXTCON_USB_HOST,
+							    &x->id_nb);
+			if (ret < 0) {
+				dev_err(x->dev,
+					"register ID notifier failed\n");
+				return ret;
+			}
+		}
+	}
+
+	return 0;
+}
+
 /**
  * devm_usb_get_phy - find the USB PHY
  * @dev - device that requests this phy
@@ -388,6 +436,10 @@ int usb_add_phy(struct usb_phy *x, enum usb_phy_type type)
 		return -EINVAL;
 	}
 
+	ret = usb_add_extcon(x);
+	if (ret)
+		return ret;
+
 	ATOMIC_INIT_NOTIFIER_HEAD(&x->notifier);
 
 	spin_lock_irqsave(&phy_lock, flags);
@@ -422,12 +474,17 @@ int usb_add_phy_dev(struct usb_phy *x)
 {
 	struct usb_phy_bind *phy_bind;
 	unsigned long flags;
+	int ret;
 
 	if (!x->dev) {
 		dev_err(x->dev, "no device provided for PHY\n");
 		return -EINVAL;
 	}
 
+	ret = usb_add_extcon(x);
+	if (ret)
+		return ret;
+
 	ATOMIC_INIT_NOTIFIER_HEAD(&x->notifier);
 
 	spin_lock_irqsave(&phy_lock, flags);
diff --git a/drivers/usb/serial/cp210x.c b/drivers/usb/serial/cp210x.c
index 0c55e7f64269..f64e914a8985 100644
--- a/drivers/usb/serial/cp210x.c
+++ b/drivers/usb/serial/cp210x.c
@@ -141,6 +141,7 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x10C4, 0x8977) },	/* CEL MeshWorks DevKit Device */
 	{ USB_DEVICE(0x10C4, 0x8998) }, /* KCF Technologies PRN */
 	{ USB_DEVICE(0x10C4, 0x8A2A) }, /* HubZ dual ZigBee and Z-Wave dongle */
+	{ USB_DEVICE(0x10C4, 0x8A5E) }, /* CEL EM3588 ZigBee USB Stick Long Range */
 	{ USB_DEVICE(0x10C4, 0xEA60) }, /* Silicon Labs factory default */
 	{ USB_DEVICE(0x10C4, 0xEA61) }, /* Silicon Labs factory default */
 	{ USB_DEVICE(0x10C4, 0xEA70) }, /* Silicon Labs factory default */
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index aba74f817dc6..1cec03799cdf 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -1244,42 +1244,13 @@ static __u32 get_ftdi_divisor(struct tty_struct *tty,
 	int div_okay = 1;
 	int baud;
 
-	/*
-	 * The logic involved in setting the baudrate can be cleanly split into
-	 * 3 steps.
-	 * 1. Standard baud rates are set in tty->termios->c_cflag
-	 * 2. If these are not enough, you can set any speed using alt_speed as
-	 * follows:
-	 *    - set tty->termios->c_cflag speed to B38400
-	 *    - set your real speed in tty->alt_speed; it gets ignored when
-	 *      alt_speed==0, (or)
-	 *    - call TIOCSSERIAL ioctl with (struct serial_struct) set as
-	 *	follows:
-	 *      flags & ASYNC_SPD_MASK == ASYNC_SPD_[HI, VHI, SHI, WARP],
-	 *	this just sets alt_speed to (HI: 57600, VHI: 115200,
-	 *	SHI: 230400, WARP: 460800)
-	 * ** Steps 1, 2 are done courtesy of tty_get_baud_rate
-	 * 3. You can also set baud rate by setting custom divisor as follows
-	 *    - set tty->termios->c_cflag speed to B38400
-	 *    - call TIOCSSERIAL ioctl with (struct serial_struct) set as
-	 *	follows:
-	 *      o flags & ASYNC_SPD_MASK == ASYNC_SPD_CUST
-	 *      o custom_divisor set to baud_base / your_new_baudrate
-	 * ** Step 3 is done courtesy of code borrowed from serial.c
-	 *    I should really spend some time and separate + move this common
-	 *    code to serial.c, it is replicated in nearly every serial driver
-	 *    you see.
-	 */
-
-	/* 1. Get the baud rate from the tty settings, this observes
-	      alt_speed hack */
-
 	baud = tty_get_baud_rate(tty);
 	dev_dbg(dev, "%s - tty_get_baud_rate reports speed %d\n", __func__, baud);
 
-	/* 2. Observe async-compatible custom_divisor hack, update baudrate
-	   if needed */
-
+	/*
+	 * Observe deprecated async-compatible custom_divisor hack, update
+	 * baudrate if needed.
+	 */
 	if (baud == 38400 &&
 	    ((priv->flags & ASYNC_SPD_MASK) == ASYNC_SPD_CUST) &&
 	     (priv->custom_divisor)) {
@@ -1288,8 +1259,6 @@ static __u32 get_ftdi_divisor(struct tty_struct *tty,
 			__func__, priv->custom_divisor, baud);
 	}
 
-	/* 3. Convert baudrate to device-specific divisor */
-
 	if (!baud)
 		baud = 9600;
 	switch (priv->chip_type) {
@@ -1505,8 +1474,7 @@ static int set_serial_info(struct tty_struct *tty,
 	/* Do error checking and permission checking */
 
 	if (!capable(CAP_SYS_ADMIN)) {
-		if (((new_serial.flags & ~ASYNC_USR_MASK) !=
-		     (priv->flags & ~ASYNC_USR_MASK))) {
+		if ((new_serial.flags ^ priv->flags) & ~ASYNC_USR_MASK) {
 			mutex_unlock(&priv->cfg_lock);
 			return -EPERM;
 		}
@@ -1530,23 +1498,14 @@ static int set_serial_info(struct tty_struct *tty,
 check_and_exit:
 	write_latency_timer(port);
 
-	if ((old_priv.flags & ASYNC_SPD_MASK) !=
-	     (priv->flags & ASYNC_SPD_MASK)) {
-		if ((priv->flags & ASYNC_SPD_MASK) == ASYNC_SPD_HI)
-			tty->alt_speed = 57600;
-		else if ((priv->flags & ASYNC_SPD_MASK) == ASYNC_SPD_VHI)
-			tty->alt_speed = 115200;
-		else if ((priv->flags & ASYNC_SPD_MASK) == ASYNC_SPD_SHI)
-			tty->alt_speed = 230400;
-		else if ((priv->flags & ASYNC_SPD_MASK) == ASYNC_SPD_WARP)
-			tty->alt_speed = 460800;
-		else
-			tty->alt_speed = 0;
-	}
-	if (((old_priv.flags & ASYNC_SPD_MASK) !=
-	     (priv->flags & ASYNC_SPD_MASK)) ||
-	    (((priv->flags & ASYNC_SPD_MASK) == ASYNC_SPD_CUST) &&
-	     (old_priv.custom_divisor != priv->custom_divisor))) {
+	if ((priv->flags ^ old_priv.flags) & ASYNC_SPD_MASK ||
+			((priv->flags & ASYNC_SPD_MASK) == ASYNC_SPD_CUST &&
+			 priv->custom_divisor != old_priv.custom_divisor)) {
+
+		/* warn about deprecation unless clearing */
+		if (priv->flags & ASYNC_SPD_MASK)
+			dev_warn_ratelimited(&port->dev, "use of SPD flags is deprecated\n");
+
 		change_speed(tty, port);
 		mutex_unlock(&priv->cfg_lock);
 	}
diff --git a/drivers/usb/serial/qcserial.c b/drivers/usb/serial/qcserial.c
index fd509ed6cf70..4ac137d070fb 100644
--- a/drivers/usb/serial/qcserial.c
+++ b/drivers/usb/serial/qcserial.c
@@ -454,6 +454,8 @@ static struct usb_serial_driver qcdevice = {
 	.write		     = usb_wwan_write,
 	.write_room	     = usb_wwan_write_room,
 	.chars_in_buffer     = usb_wwan_chars_in_buffer,
+	.tiocmget            = usb_wwan_tiocmget,
+	.tiocmset            = usb_wwan_tiocmset,
 	.attach              = qc_attach,
 	.release	     = qc_release,
 	.port_probe          = usb_wwan_port_probe,
diff --git a/drivers/usb/serial/upd78f0730.c b/drivers/usb/serial/upd78f0730.c
index a028dd2310c9..6819a3486e5d 100644
--- a/drivers/usb/serial/upd78f0730.c
+++ b/drivers/usb/serial/upd78f0730.c
@@ -288,7 +288,7 @@ static void upd78f0730_dtr_rts(struct usb_serial_port *port, int on)
 static speed_t upd78f0730_get_baud_rate(struct tty_struct *tty)
 {
 	const speed_t baud_rate = tty_get_baud_rate(tty);
-	const speed_t supported[] = {
+	static const speed_t supported[] = {
 		0, 2400, 4800, 9600, 19200, 38400, 57600, 115200, 153600
 	};
 	int i;
@@ -384,7 +384,7 @@ static void upd78f0730_set_termios(struct tty_struct *tty,
 
 static int upd78f0730_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
-	struct upd78f0730_open_close request = {
+	static const struct upd78f0730_open_close request = {
 		.opcode = UPD78F0730_CMD_OPEN_CLOSE,
 		.state = UPD78F0730_PORT_OPEN
 	};
@@ -402,7 +402,7 @@ static int upd78f0730_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 static void upd78f0730_close(struct usb_serial_port *port)
 {
-	struct upd78f0730_open_close request = {
+	static const struct upd78f0730_open_close request = {
 		.opcode = UPD78F0730_CMD_OPEN_CLOSE,
 		.state = UPD78F0730_PORT_CLOSE
 	};
diff --git a/drivers/usb/serial/usb-serial.c b/drivers/usb/serial/usb-serial.c
index c7ca95f64edc..bb34f9f7eaf4 100644
--- a/drivers/usb/serial/usb-serial.c
+++ b/drivers/usb/serial/usb-serial.c
@@ -742,6 +742,124 @@ static void find_endpoints(struct usb_serial *serial,
 	}
 }
 
+static int setup_port_bulk_in(struct usb_serial_port *port,
+					struct usb_endpoint_descriptor *epd)
+{
+	struct usb_serial_driver *type = port->serial->type;
+	struct usb_device *udev = port->serial->dev;
+	int buffer_size;
+	int i;
+
+	buffer_size = max_t(int, type->bulk_in_size, usb_endpoint_maxp(epd));
+	port->bulk_in_size = buffer_size;
+	port->bulk_in_endpointAddress = epd->bEndpointAddress;
+
+	for (i = 0; i < ARRAY_SIZE(port->read_urbs); ++i) {
+		set_bit(i, &port->read_urbs_free);
+		port->read_urbs[i] = usb_alloc_urb(0, GFP_KERNEL);
+		if (!port->read_urbs[i])
+			return -ENOMEM;
+		port->bulk_in_buffers[i] = kmalloc(buffer_size, GFP_KERNEL);
+		if (!port->bulk_in_buffers[i])
+			return -ENOMEM;
+		usb_fill_bulk_urb(port->read_urbs[i], udev,
+				usb_rcvbulkpipe(udev, epd->bEndpointAddress),
+				port->bulk_in_buffers[i], buffer_size,
+				type->read_bulk_callback, port);
+	}
+
+	port->read_urb = port->read_urbs[0];
+	port->bulk_in_buffer = port->bulk_in_buffers[0];
+
+	return 0;
+}
+
+static int setup_port_bulk_out(struct usb_serial_port *port,
+					struct usb_endpoint_descriptor *epd)
+{
+	struct usb_serial_driver *type = port->serial->type;
+	struct usb_device *udev = port->serial->dev;
+	int buffer_size;
+	int i;
+
+	if (kfifo_alloc(&port->write_fifo, PAGE_SIZE, GFP_KERNEL))
+		return -ENOMEM;
+	if (type->bulk_out_size)
+		buffer_size = type->bulk_out_size;
+	else
+		buffer_size = usb_endpoint_maxp(epd);
+	port->bulk_out_size = buffer_size;
+	port->bulk_out_endpointAddress = epd->bEndpointAddress;
+
+	for (i = 0; i < ARRAY_SIZE(port->write_urbs); ++i) {
+		set_bit(i, &port->write_urbs_free);
+		port->write_urbs[i] = usb_alloc_urb(0, GFP_KERNEL);
+		if (!port->write_urbs[i])
+			return -ENOMEM;
+		port->bulk_out_buffers[i] = kmalloc(buffer_size, GFP_KERNEL);
+		if (!port->bulk_out_buffers[i])
+			return -ENOMEM;
+		usb_fill_bulk_urb(port->write_urbs[i], udev,
+				usb_sndbulkpipe(udev, epd->bEndpointAddress),
+				port->bulk_out_buffers[i], buffer_size,
+				type->write_bulk_callback, port);
+	}
+
+	port->write_urb = port->write_urbs[0];
+	port->bulk_out_buffer = port->bulk_out_buffers[0];
+
+	return 0;
+}
+
+static int setup_port_interrupt_in(struct usb_serial_port *port,
+					struct usb_endpoint_descriptor *epd)
+{
+	struct usb_serial_driver *type = port->serial->type;
+	struct usb_device *udev = port->serial->dev;
+	int buffer_size;
+
+	port->interrupt_in_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!port->interrupt_in_urb)
+		return -ENOMEM;
+	buffer_size = usb_endpoint_maxp(epd);
+	port->interrupt_in_endpointAddress = epd->bEndpointAddress;
+	port->interrupt_in_buffer = kmalloc(buffer_size, GFP_KERNEL);
+	if (!port->interrupt_in_buffer)
+		return -ENOMEM;
+	usb_fill_int_urb(port->interrupt_in_urb, udev,
+			usb_rcvintpipe(udev, epd->bEndpointAddress),
+			port->interrupt_in_buffer, buffer_size,
+			type->read_int_callback, port,
+			epd->bInterval);
+
+	return 0;
+}
+
+static int setup_port_interrupt_out(struct usb_serial_port *port,
+					struct usb_endpoint_descriptor *epd)
+{
+	struct usb_serial_driver *type = port->serial->type;
+	struct usb_device *udev = port->serial->dev;
+	int buffer_size;
+
+	port->interrupt_out_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!port->interrupt_out_urb)
+		return -ENOMEM;
+	buffer_size = usb_endpoint_maxp(epd);
+	port->interrupt_out_size = buffer_size;
+	port->interrupt_out_endpointAddress = epd->bEndpointAddress;
+	port->interrupt_out_buffer = kmalloc(buffer_size, GFP_KERNEL);
+	if (!port->interrupt_out_buffer)
+		return -ENOMEM;
+	usb_fill_int_urb(port->interrupt_out_urb, udev,
+			usb_sndintpipe(udev, epd->bEndpointAddress),
+			port->interrupt_out_buffer, buffer_size,
+			type->write_int_callback, port,
+			epd->bInterval);
+
+	return 0;
+}
+
 static int usb_serial_probe(struct usb_interface *interface,
 			       const struct usb_device_id *id)
 {
@@ -749,13 +867,10 @@ static int usb_serial_probe(struct usb_interface *interface,
 	struct usb_device *dev = interface_to_usbdev(interface);
 	struct usb_serial *serial = NULL;
 	struct usb_serial_port *port;
-	struct usb_endpoint_descriptor *endpoint;
 	struct usb_serial_endpoints *epds;
 	struct usb_serial_driver *type = NULL;
 	int retval;
-	int buffer_size;
 	int i;
-	int j;
 	int num_ports = 0;
 	unsigned char max_endpoints;
 
@@ -847,8 +962,10 @@ static int usb_serial_probe(struct usb_interface *interface,
 	dev_dbg(ddev, "setting up %d port structure(s)\n", max_endpoints);
 	for (i = 0; i < max_endpoints; ++i) {
 		port = kzalloc(sizeof(struct usb_serial_port), GFP_KERNEL);
-		if (!port)
-			goto probe_error;
+		if (!port) {
+			retval = -ENOMEM;
+			goto err_free_epds;
+		}
 		tty_port_init(&port->port);
 		port->port.ops = &serial_port_ops;
 		port->serial = serial;
@@ -867,86 +984,24 @@ static int usb_serial_probe(struct usb_interface *interface,
 
 	/* set up the endpoint information */
 	for (i = 0; i < epds->num_bulk_in; ++i) {
-		endpoint = epds->bulk_in[i];
-		port = serial->port[i];
-		buffer_size = max_t(int, serial->type->bulk_in_size,
-				usb_endpoint_maxp(endpoint));
-		port->bulk_in_size = buffer_size;
-		port->bulk_in_endpointAddress = endpoint->bEndpointAddress;
-
-		for (j = 0; j < ARRAY_SIZE(port->read_urbs); ++j) {
-			set_bit(j, &port->read_urbs_free);
-			port->read_urbs[j] = usb_alloc_urb(0, GFP_KERNEL);
-			if (!port->read_urbs[j])
-				goto probe_error;
-			port->bulk_in_buffers[j] = kmalloc(buffer_size,
-								GFP_KERNEL);
-			if (!port->bulk_in_buffers[j])
-				goto probe_error;
-			usb_fill_bulk_urb(port->read_urbs[j], dev,
-					usb_rcvbulkpipe(dev,
-						endpoint->bEndpointAddress),
-					port->bulk_in_buffers[j], buffer_size,
-					serial->type->read_bulk_callback,
-					port);
-		}
-
-		port->read_urb = port->read_urbs[0];
-		port->bulk_in_buffer = port->bulk_in_buffers[0];
+		retval = setup_port_bulk_in(serial->port[i], epds->bulk_in[i]);
+		if (retval)
+			goto err_free_epds;
 	}
 
 	for (i = 0; i < epds->num_bulk_out; ++i) {
-		endpoint = epds->bulk_out[i];
-		port = serial->port[i];
-		if (kfifo_alloc(&port->write_fifo, PAGE_SIZE, GFP_KERNEL))
-			goto probe_error;
-		buffer_size = serial->type->bulk_out_size;
-		if (!buffer_size)
-			buffer_size = usb_endpoint_maxp(endpoint);
-		port->bulk_out_size = buffer_size;
-		port->bulk_out_endpointAddress = endpoint->bEndpointAddress;
-
-		for (j = 0; j < ARRAY_SIZE(port->write_urbs); ++j) {
-			set_bit(j, &port->write_urbs_free);
-			port->write_urbs[j] = usb_alloc_urb(0, GFP_KERNEL);
-			if (!port->write_urbs[j])
-				goto probe_error;
-			port->bulk_out_buffers[j] = kmalloc(buffer_size,
-								GFP_KERNEL);
-			if (!port->bulk_out_buffers[j])
-				goto probe_error;
-			usb_fill_bulk_urb(port->write_urbs[j], dev,
-					usb_sndbulkpipe(dev,
-						endpoint->bEndpointAddress),
-					port->bulk_out_buffers[j], buffer_size,
-					serial->type->write_bulk_callback,
-					port);
-		}
-
-		port->write_urb = port->write_urbs[0];
-		port->bulk_out_buffer = port->bulk_out_buffers[0];
+		retval = setup_port_bulk_out(serial->port[i],
+				epds->bulk_out[i]);
+		if (retval)
+			goto err_free_epds;
 	}
 
 	if (serial->type->read_int_callback) {
 		for (i = 0; i < epds->num_interrupt_in; ++i) {
-			endpoint = epds->interrupt_in[i];
-			port = serial->port[i];
-			port->interrupt_in_urb = usb_alloc_urb(0, GFP_KERNEL);
-			if (!port->interrupt_in_urb)
-				goto probe_error;
-			buffer_size = usb_endpoint_maxp(endpoint);
-			port->interrupt_in_endpointAddress =
-						endpoint->bEndpointAddress;
-			port->interrupt_in_buffer = kmalloc(buffer_size,
-								GFP_KERNEL);
-			if (!port->interrupt_in_buffer)
-				goto probe_error;
-			usb_fill_int_urb(port->interrupt_in_urb, dev,
-				usb_rcvintpipe(dev,
-						endpoint->bEndpointAddress),
-				port->interrupt_in_buffer, buffer_size,
-				serial->type->read_int_callback, port,
-				endpoint->bInterval);
+			retval = setup_port_interrupt_in(serial->port[i],
+					epds->interrupt_in[i]);
+			if (retval)
+				goto err_free_epds;
 		}
 	} else if (epds->num_interrupt_in) {
 		dev_dbg(ddev, "The device claims to support interrupt in transfers, but read_int_callback is not defined\n");
@@ -954,25 +1009,10 @@ static int usb_serial_probe(struct usb_interface *interface,
 
 	if (serial->type->write_int_callback) {
 		for (i = 0; i < epds->num_interrupt_out; ++i) {
-			endpoint = epds->interrupt_out[i];
-			port = serial->port[i];
-			port->interrupt_out_urb = usb_alloc_urb(0, GFP_KERNEL);
-			if (!port->interrupt_out_urb)
-				goto probe_error;
-			buffer_size = usb_endpoint_maxp(endpoint);
-			port->interrupt_out_size = buffer_size;
-			port->interrupt_out_endpointAddress =
-						endpoint->bEndpointAddress;
-			port->interrupt_out_buffer = kmalloc(buffer_size,
-								GFP_KERNEL);
-			if (!port->interrupt_out_buffer)
-				goto probe_error;
-			usb_fill_int_urb(port->interrupt_out_urb, dev,
-				usb_sndintpipe(dev,
-						  endpoint->bEndpointAddress),
-				port->interrupt_out_buffer, buffer_size,
-				serial->type->write_int_callback, port,
-				endpoint->bInterval);
+			retval = setup_port_interrupt_out(serial->port[i],
+					epds->interrupt_out[i]);
+			if (retval)
+				goto err_free_epds;
 		}
 	} else if (epds->num_interrupt_out) {
 		dev_dbg(ddev, "The device claims to support interrupt out transfers, but write_int_callback is not defined\n");
@@ -984,7 +1024,7 @@ static int usb_serial_probe(struct usb_interface *interface,
 	if (type->attach) {
 		retval = type->attach(serial);
 		if (retval < 0)
-			goto probe_error;
+			goto err_free_epds;
 		serial->attached = 1;
 		if (retval > 0) {
 			/* quietly accept this device, but don't bind to a
@@ -996,9 +1036,10 @@ static int usb_serial_probe(struct usb_interface *interface,
 		serial->attached = 1;
 	}
 
-	if (allocate_minors(serial, num_ports)) {
+	retval = allocate_minors(serial, num_ports);
+	if (retval) {
 		dev_err(ddev, "No more free serial minor numbers\n");
-		goto probe_error;
+		goto err_free_epds;
 	}
 
 	/* register all of the individual ports with the driver core */
@@ -1020,8 +1061,6 @@ exit:
 	module_put(type->driver.owner);
 	return 0;
 
-probe_error:
-	retval = -EIO;
 err_free_epds:
 	kfree(epds);
 err_put_serial:
diff --git a/drivers/usb/storage/ene_ub6250.c b/drivers/usb/storage/ene_ub6250.c
index 44af719194b2..28100374f7bd 100644
--- a/drivers/usb/storage/ene_ub6250.c
+++ b/drivers/usb/storage/ene_ub6250.c
@@ -95,12 +95,12 @@ static struct us_unusual_dev ene_ub6250_unusual_dev_list[] = {
 #define REG_HW_TRAP1        0xFF89
 
 /* SRB Status */
-#define SS_SUCCESS                  0x00      /* No Sense */
-#define SS_NOT_READY                0x02
-#define SS_MEDIUM_ERR               0x03
-#define SS_HW_ERR                   0x04
-#define SS_ILLEGAL_REQUEST          0x05
-#define SS_UNIT_ATTENTION           0x06
+#define SS_SUCCESS		0x000000	/* No Sense */
+#define SS_NOT_READY		0x023A00	/* Medium not present */
+#define SS_MEDIUM_ERR		0x031100	/* Unrecovered read error */
+#define SS_HW_ERR		0x040800	/* Communication failure */
+#define SS_ILLEGAL_REQUEST	0x052000	/* Invalid command */
+#define SS_UNIT_ATTENTION	0x062900	/* Reset occurred */
 
 /* ENE Load FW Pattern */
 #define SD_INIT1_PATTERN   1
@@ -584,24 +584,26 @@ static int ene_send_scsi_cmd(struct us_data *us, u8 fDir, void *buf, int use_sg)
 	return USB_STOR_TRANSPORT_GOOD;
 }
 
-static int sd_scsi_test_unit_ready(struct us_data *us, struct scsi_cmnd *srb)
+static int do_scsi_request_sense(struct us_data *us, struct scsi_cmnd *srb)
 {
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
+	unsigned char buf[18];
 
-	if (info->SD_Status.Insert && info->SD_Status.Ready)
-		return USB_STOR_TRANSPORT_GOOD;
-	else {
-		ene_sd_init(us);
-		return USB_STOR_TRANSPORT_GOOD;
-	}
+	memset(buf, 0, 18);
+	buf[0] = 0x70;				/* Current error */
+	buf[2] = info->SrbStatus >> 16;		/* Sense key */
+	buf[7] = 10;				/* Additional length */
+	buf[12] = info->SrbStatus >> 8;		/* ASC */
+	buf[13] = info->SrbStatus;		/* ASCQ */
 
+	usb_stor_set_xfer_buf(buf, sizeof(buf), srb);
 	return USB_STOR_TRANSPORT_GOOD;
 }
 
-static int sd_scsi_inquiry(struct us_data *us, struct scsi_cmnd *srb)
+static int do_scsi_inquiry(struct us_data *us, struct scsi_cmnd *srb)
 {
 	unsigned char data_ptr[36] = {
-		0x00, 0x80, 0x02, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x55,
+		0x00, 0x00, 0x02, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x55,
 		0x53, 0x42, 0x32, 0x2E, 0x30, 0x20, 0x20, 0x43, 0x61,
 		0x72, 0x64, 0x52, 0x65, 0x61, 0x64, 0x65, 0x72, 0x20,
 		0x20, 0x20, 0x20, 0x20, 0x20, 0x30, 0x31, 0x30, 0x30 };
@@ -610,6 +612,20 @@ static int sd_scsi_inquiry(struct us_data *us, struct scsi_cmnd *srb)
 	return USB_STOR_TRANSPORT_GOOD;
 }
 
+static int sd_scsi_test_unit_ready(struct us_data *us, struct scsi_cmnd *srb)
+{
+	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
+
+	if (info->SD_Status.Insert && info->SD_Status.Ready)
+		return USB_STOR_TRANSPORT_GOOD;
+	else {
+		ene_sd_init(us);
+		return USB_STOR_TRANSPORT_GOOD;
+	}
+
+	return USB_STOR_TRANSPORT_GOOD;
+}
+
 static int sd_scsi_mode_sense(struct us_data *us, struct scsi_cmnd *srb)
 {
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
@@ -1455,19 +1471,6 @@ static int ms_scsi_test_unit_ready(struct us_data *us, struct scsi_cmnd *srb)
 	return USB_STOR_TRANSPORT_GOOD;
 }
 
-static int ms_scsi_inquiry(struct us_data *us, struct scsi_cmnd *srb)
-{
-	/* pr_info("MS_SCSI_Inquiry\n"); */
-	unsigned char data_ptr[36] = {
-		0x00, 0x80, 0x02, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x55,
-		0x53, 0x42, 0x32, 0x2E, 0x30, 0x20, 0x20, 0x43, 0x61,
-		0x72, 0x64, 0x52, 0x65, 0x61, 0x64, 0x65, 0x72, 0x20,
-		0x20, 0x20, 0x20, 0x20, 0x20, 0x30, 0x31, 0x30, 0x30};
-
-	usb_stor_set_xfer_buf(data_ptr, 36, srb);
-	return USB_STOR_TRANSPORT_GOOD;
-}
-
 static int ms_scsi_mode_sense(struct us_data *us, struct scsi_cmnd *srb)
 {
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
@@ -1940,6 +1943,8 @@ static int ene_load_bincode(struct us_data *us, unsigned char flag)
 	bcb->CDB[0] = 0xEF;
 
 	result = ene_send_scsi_cmd(us, FDIR_WRITE, buf, 0);
+	if (us->srb != NULL)
+		scsi_set_resid(us->srb, 0);
 	info->BIN_FLAG = flag;
 	kfree(buf);
 
@@ -2223,13 +2228,15 @@ static int sd_scsi_irp(struct us_data *us, struct scsi_cmnd *srb)
 	int    result;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *)us->extra;
 
-	info->SrbStatus = SS_SUCCESS;
 	switch (srb->cmnd[0]) {
 	case TEST_UNIT_READY:
 		result = sd_scsi_test_unit_ready(us, srb);
 		break; /* 0x00 */
+	case REQUEST_SENSE:
+		result = do_scsi_request_sense(us, srb);
+		break; /* 0x03 */
 	case INQUIRY:
-		result = sd_scsi_inquiry(us, srb);
+		result = do_scsi_inquiry(us, srb);
 		break; /* 0x12 */
 	case MODE_SENSE:
 		result = sd_scsi_mode_sense(us, srb);
@@ -2253,6 +2260,8 @@ static int sd_scsi_irp(struct us_data *us, struct scsi_cmnd *srb)
 		result = USB_STOR_TRANSPORT_FAILED;
 		break;
 	}
+	if (result == USB_STOR_TRANSPORT_GOOD)
+		info->SrbStatus = SS_SUCCESS;
 	return result;
 }
 
@@ -2263,13 +2272,16 @@ static int ms_scsi_irp(struct us_data *us, struct scsi_cmnd *srb)
 {
 	int result;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *)us->extra;
-	info->SrbStatus = SS_SUCCESS;
+
 	switch (srb->cmnd[0]) {
 	case TEST_UNIT_READY:
 		result = ms_scsi_test_unit_ready(us, srb);
 		break; /* 0x00 */
+	case REQUEST_SENSE:
+		result = do_scsi_request_sense(us, srb);
+		break; /* 0x03 */
 	case INQUIRY:
-		result = ms_scsi_inquiry(us, srb);
+		result = do_scsi_inquiry(us, srb);
 		break; /* 0x12 */
 	case MODE_SENSE:
 		result = ms_scsi_mode_sense(us, srb);
@@ -2288,26 +2300,29 @@ static int ms_scsi_irp(struct us_data *us, struct scsi_cmnd *srb)
 		result = USB_STOR_TRANSPORT_FAILED;
 		break;
 	}
+	if (result == USB_STOR_TRANSPORT_GOOD)
+		info->SrbStatus = SS_SUCCESS;
 	return result;
 }
 
 static int ene_transport(struct scsi_cmnd *srb, struct us_data *us)
 {
-	int result = 0;
+	int result = USB_STOR_XFER_GOOD;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *)(us->extra);
 
 	/*US_DEBUG(usb_stor_show_command(us, srb)); */
 	scsi_set_resid(srb, 0);
-	if (unlikely(!(info->SD_Status.Ready || info->MS_Status.Ready))) {
+	if (unlikely(!(info->SD_Status.Ready || info->MS_Status.Ready)))
 		result = ene_init(us);
-	} else {
+	if (result == USB_STOR_XFER_GOOD) {
+		result = USB_STOR_TRANSPORT_ERROR;
 		if (info->SD_Status.Ready)
 			result = sd_scsi_irp(us, srb);
 
 		if (info->MS_Status.Ready)
 			result = ms_scsi_irp(us, srb);
 	}
-	return 0;
+	return result;
 }
 
 static struct scsi_host_template ene_ub6250_host_template;
diff --git a/drivers/usb/typec/Kconfig b/drivers/usb/typec/Kconfig
index dfcfe459b7cf..bc1b7745f1d4 100644
--- a/drivers/usb/typec/Kconfig
+++ b/drivers/usb/typec/Kconfig
@@ -19,4 +19,6 @@ config TYPEC_WCOVE
 	  To compile this driver as module, choose M here: the module will be
 	  called typec_wcove
 
+source "drivers/usb/typec/ucsi/Kconfig"
+
 endmenu
diff --git a/drivers/usb/typec/Makefile b/drivers/usb/typec/Makefile
index b9cb862221af..bc214f15f1b5 100644
--- a/drivers/usb/typec/Makefile
+++ b/drivers/usb/typec/Makefile
@@ -1,2 +1,3 @@
 obj-$(CONFIG_TYPEC)		+= typec.o
 obj-$(CONFIG_TYPEC_WCOVE)	+= typec_wcove.o
+obj-$(CONFIG_TYPEC_UCSI)	+= ucsi/
diff --git a/drivers/usb/typec/typec.c b/drivers/usb/typec/typec.c
index 89e540bb7ff3..24e355ba109d 100644
--- a/drivers/usb/typec/typec.c
+++ b/drivers/usb/typec/typec.c
@@ -11,6 +11,7 @@
 
 #include <linux/device.h>
 #include <linux/module.h>
+#include <linux/mutex.h>
 #include <linux/slab.h>
 #include <linux/usb/typec.h>
 
@@ -69,6 +70,8 @@ struct typec_port {
 	enum typec_role			pwr_role;
 	enum typec_role			vconn_role;
 	enum typec_pwr_opmode		pwr_opmode;
+	enum typec_port_type		port_type;
+	struct mutex			port_type_lock;
 
 	const struct typec_capability	*cap;
 };
@@ -291,7 +294,7 @@ typec_altmode_roles_show(struct device *dev, struct device_attribute *attr,
 }
 
 static void typec_init_modes(struct typec_altmode *alt,
-			     struct typec_mode_desc *desc, bool is_port)
+			     const struct typec_mode_desc *desc, bool is_port)
 {
 	int i;
 
@@ -378,7 +381,8 @@ static const struct device_type typec_altmode_dev_type = {
 };
 
 static struct typec_altmode *
-typec_register_altmode(struct device *parent, struct typec_altmode_desc *desc)
+typec_register_altmode(struct device *parent,
+		       const struct typec_altmode_desc *desc)
 {
 	struct typec_altmode *alt;
 	int ret;
@@ -495,7 +499,7 @@ EXPORT_SYMBOL_GPL(typec_partner_set_identity);
  */
 struct typec_altmode *
 typec_partner_register_altmode(struct typec_partner *partner,
-			       struct typec_altmode_desc *desc)
+			       const struct typec_altmode_desc *desc)
 {
 	return typec_register_altmode(&partner->dev, desc);
 }
@@ -590,7 +594,7 @@ static const struct device_type typec_plug_dev_type = {
  */
 struct typec_altmode *
 typec_plug_register_altmode(struct typec_plug *plug,
-			    struct typec_altmode_desc *desc)
+			    const struct typec_altmode_desc *desc)
 {
 	return typec_register_altmode(&plug->dev, desc);
 }
@@ -789,6 +793,18 @@ static const char * const typec_data_roles[] = {
 	[TYPEC_HOST]	= "host",
 };
 
+static const char * const typec_port_types[] = {
+	[TYPEC_PORT_DFP] = "source",
+	[TYPEC_PORT_UFP] = "sink",
+	[TYPEC_PORT_DRP] = "dual",
+};
+
+static const char * const typec_port_types_drp[] = {
+	[TYPEC_PORT_DFP] = "dual [source] sink",
+	[TYPEC_PORT_UFP] = "dual source [sink]",
+	[TYPEC_PORT_DRP] = "[dual] source sink",
+};
+
 static ssize_t
 preferred_role_store(struct device *dev, struct device_attribute *attr,
 		     const char *buf, size_t size)
@@ -846,11 +862,6 @@ static ssize_t data_role_store(struct device *dev,
 	struct typec_port *port = to_typec_port(dev);
 	int ret;
 
-	if (port->cap->type != TYPEC_PORT_DRP) {
-		dev_dbg(dev, "data role swap only supported with DRP ports\n");
-		return -EOPNOTSUPP;
-	}
-
 	if (!port->cap->dr_set) {
 		dev_dbg(dev, "data role swapping not supported\n");
 		return -EOPNOTSUPP;
@@ -860,11 +871,22 @@ static ssize_t data_role_store(struct device *dev,
 	if (ret < 0)
 		return ret;
 
+	mutex_lock(&port->port_type_lock);
+	if (port->port_type != TYPEC_PORT_DRP) {
+		dev_dbg(dev, "port type fixed at \"%s\"",
+			     typec_port_types[port->port_type]);
+		ret = -EOPNOTSUPP;
+		goto unlock_and_ret;
+	}
+
 	ret = port->cap->dr_set(port->cap, ret);
 	if (ret)
-		return ret;
+		goto unlock_and_ret;
 
-	return size;
+	ret = size;
+unlock_and_ret:
+	mutex_unlock(&port->port_type_lock);
+	return ret;
 }
 
 static ssize_t data_role_show(struct device *dev,
@@ -885,7 +907,7 @@ static ssize_t power_role_store(struct device *dev,
 				const char *buf, size_t size)
 {
 	struct typec_port *port = to_typec_port(dev);
-	int ret = size;
+	int ret;
 
 	if (!port->cap->pd_revision) {
 		dev_dbg(dev, "USB Power Delivery not supported\n");
@@ -906,11 +928,22 @@ static ssize_t power_role_store(struct device *dev,
 	if (ret < 0)
 		return ret;
 
+	mutex_lock(&port->port_type_lock);
+	if (port->port_type != TYPEC_PORT_DRP) {
+		dev_dbg(dev, "port type fixed at \"%s\"",
+			     typec_port_types[port->port_type]);
+		ret = -EOPNOTSUPP;
+		goto unlock_and_ret;
+	}
+
 	ret = port->cap->pr_set(port->cap, ret);
 	if (ret)
-		return ret;
+		goto unlock_and_ret;
 
-	return size;
+	ret = size;
+unlock_and_ret:
+	mutex_unlock(&port->port_type_lock);
+	return ret;
 }
 
 static ssize_t power_role_show(struct device *dev,
@@ -926,6 +959,57 @@ static ssize_t power_role_show(struct device *dev,
 }
 static DEVICE_ATTR_RW(power_role);
 
+static ssize_t
+port_type_store(struct device *dev, struct device_attribute *attr,
+			const char *buf, size_t size)
+{
+	struct typec_port *port = to_typec_port(dev);
+	int ret;
+	enum typec_port_type type;
+
+	if (!port->cap->port_type_set || port->cap->type != TYPEC_PORT_DRP) {
+		dev_dbg(dev, "changing port type not supported\n");
+		return -EOPNOTSUPP;
+	}
+
+	ret = sysfs_match_string(typec_port_types, buf);
+	if (ret < 0)
+		return ret;
+
+	type = ret;
+	mutex_lock(&port->port_type_lock);
+
+	if (port->port_type == type) {
+		ret = size;
+		goto unlock_and_ret;
+	}
+
+	ret = port->cap->port_type_set(port->cap, type);
+	if (ret)
+		goto unlock_and_ret;
+
+	port->port_type = type;
+	ret = size;
+
+unlock_and_ret:
+	mutex_unlock(&port->port_type_lock);
+	return ret;
+}
+
+static ssize_t
+port_type_show(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct typec_port *port = to_typec_port(dev);
+
+	if (port->cap->type == TYPEC_PORT_DRP)
+		return sprintf(buf, "%s\n",
+			       typec_port_types_drp[port->port_type]);
+
+	return sprintf(buf, "[%s]\n", typec_port_types[port->cap->type]);
+}
+static DEVICE_ATTR_RW(port_type);
+
 static const char * const typec_pwr_opmodes[] = {
 	[TYPEC_PWR_MODE_USB]	= "default",
 	[TYPEC_PWR_MODE_1_5A]	= "1.5A",
@@ -1035,6 +1119,7 @@ static struct attribute *typec_attrs[] = {
 	&dev_attr_usb_power_delivery_revision.attr,
 	&dev_attr_usb_typec_revision.attr,
 	&dev_attr_vconn_source.attr,
+	&dev_attr_port_type.attr,
 	NULL,
 };
 ATTRIBUTE_GROUPS(typec);
@@ -1123,6 +1208,11 @@ void typec_set_vconn_role(struct typec_port *port, enum typec_role role)
 }
 EXPORT_SYMBOL_GPL(typec_set_vconn_role);
 
+static int partner_match(struct device *dev, void *data)
+{
+	return is_typec_partner(dev);
+}
+
 /**
  * typec_set_pwr_opmode - Report changed power operation mode
  * @port: The USB Type-C Port where the mode was changed
@@ -1136,12 +1226,26 @@ EXPORT_SYMBOL_GPL(typec_set_vconn_role);
 void typec_set_pwr_opmode(struct typec_port *port,
 			  enum typec_pwr_opmode opmode)
 {
+	struct device *partner_dev;
+
 	if (port->pwr_opmode == opmode)
 		return;
 
 	port->pwr_opmode = opmode;
 	sysfs_notify(&port->dev.kobj, NULL, "power_operation_mode");
 	kobject_uevent(&port->dev.kobj, KOBJ_CHANGE);
+
+	partner_dev = device_find_child(&port->dev, NULL, partner_match);
+	if (partner_dev) {
+		struct typec_partner *partner = to_typec_partner(partner_dev);
+
+		if (opmode == TYPEC_PWR_MODE_PD && !partner->usb_pd) {
+			partner->usb_pd = 1;
+			sysfs_notify(&partner_dev->kobj, NULL,
+				     "supports_usb_power_delivery");
+		}
+		put_device(partner_dev);
+	}
 }
 EXPORT_SYMBOL_GPL(typec_set_pwr_opmode);
 
@@ -1159,7 +1263,7 @@ EXPORT_SYMBOL_GPL(typec_set_pwr_opmode);
  */
 struct typec_altmode *
 typec_port_register_altmode(struct typec_port *port,
-			    struct typec_altmode_desc *desc)
+			    const struct typec_altmode_desc *desc)
 {
 	return typec_register_altmode(&port->dev, desc);
 }
@@ -1211,6 +1315,8 @@ struct typec_port *typec_register_port(struct device *parent,
 
 	port->id = id;
 	port->cap = cap;
+	port->port_type = cap->type;
+	mutex_init(&port->port_type_lock);
 	port->prefer_role = cap->prefer_role;
 
 	port->dev.class = typec_class;
diff --git a/drivers/usb/typec/typec_wcove.c b/drivers/usb/typec/typec_wcove.c
index d5a7b21fa3f1..c2ce25289027 100644
--- a/drivers/usb/typec/typec_wcove.c
+++ b/drivers/usb/typec/typec_wcove.c
@@ -105,8 +105,8 @@ enum wcove_typec_role {
 	WCOVE_ROLE_DEVICE,
 };
 
-static uuid_le uuid = UUID_LE(0x482383f0, 0x2876, 0x4e49,
-			      0x86, 0x85, 0xdb, 0x66, 0x21, 0x1a, 0xf0, 0x37);
+static guid_t guid = GUID_INIT(0x482383f0, 0x2876, 0x4e49,
+			       0x86, 0x85, 0xdb, 0x66, 0x21, 0x1a, 0xf0, 0x37);
 
 static int wcove_typec_func(struct wcove_typec *wcove,
 			    enum wcove_typec_func func, int param)
@@ -118,7 +118,7 @@ static int wcove_typec_func(struct wcove_typec *wcove,
 	tmp.type = ACPI_TYPE_INTEGER;
 	tmp.integer.value = param;
 
-	obj = acpi_evaluate_dsm(ACPI_HANDLE(wcove->dev), uuid.b, 1, func,
+	obj = acpi_evaluate_dsm(ACPI_HANDLE(wcove->dev), &guid, 1, func,
 				&argv4);
 	if (!obj) {
 		dev_err(wcove->dev, "%s: failed to evaluate _DSM\n", __func__);
@@ -314,7 +314,7 @@ static int wcove_typec_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	if (!acpi_check_dsm(ACPI_HANDLE(&pdev->dev), uuid.b, 0, 0x1f)) {
+	if (!acpi_check_dsm(ACPI_HANDLE(&pdev->dev), &guid, 0, 0x1f)) {
 		dev_err(&pdev->dev, "Missing _DSM functions\n");
 		return -ENODEV;
 	}
diff --git a/drivers/usb/typec/ucsi/Kconfig b/drivers/usb/typec/ucsi/Kconfig
new file mode 100644
index 000000000000..d0c31cee4720
--- /dev/null
+++ b/drivers/usb/typec/ucsi/Kconfig
@@ -0,0 +1,39 @@
+config TYPEC_UCSI
+	tristate "USB Type-C Connector System Software Interface driver"
+	depends on !CPU_BIG_ENDIAN
+	select TYPEC
+	help
+	  USB Type-C Connector System Software Interface (UCSI) is a
+	  specification for an interface that allows the operating system to
+	  control the USB Type-C ports. On UCSI system the USB Type-C ports
+	  function autonomously by default, but in order to get the status of
+	  the ports and support basic operations like role swapping, the driver
+	  is required. UCSI is available on most of the new Intel based systems
+	  that are equipped with Embedded Controller and USB Type-C ports.
+
+	  UCSI specification does not define the interface method, so depending
+	  on the platform, ACPI, PCI, I2C, etc. may be used. Therefore this
+	  driver only provides the core part, and separate drivers are needed
+	  for every supported interface method.
+
+	  The UCSI specification can be downloaded from:
+	  http://www.intel.com/content/www/us/en/io/universal-serial-bus/usb-type-c-ucsi-spec.html
+
+	  To compile the driver as a module, choose M here: the module will be
+	  called typec_ucsi.
+
+if TYPEC_UCSI
+
+config UCSI_ACPI
+	tristate "UCSI ACPI Interface Driver"
+	depends on ACPI
+	help
+	  This driver enables UCSI support on platforms that expose UCSI
+	  interface as ACPI device. On new Intel Atom based platforms starting
+	  from Broxton SoCs and Core platforms stating from Skylake, UCSI is an
+	  ACPI enumerated device.
+
+	  To compile the driver as a module, choose M here: the module will be
+	  called ucsi_acpi
+
+endif
diff --git a/drivers/usb/typec/ucsi/Makefile b/drivers/usb/typec/ucsi/Makefile
new file mode 100644
index 000000000000..8372fc22f9b3
--- /dev/null
+++ b/drivers/usb/typec/ucsi/Makefile
@@ -0,0 +1,9 @@
+CFLAGS_trace.o			:= -I$(src)
+
+obj-$(CONFIG_TYPEC_UCSI)	+= typec_ucsi.o
+
+typec_ucsi-y			:= ucsi.o
+
+typec_ucsi-$(CONFIG_FTRACE)	+= trace.o
+
+obj-$(CONFIG_UCSI_ACPI)		+= ucsi_acpi.o
diff --git a/drivers/usb/typec/ucsi/debug.h b/drivers/usb/typec/ucsi/debug.h
new file mode 100644
index 000000000000..e4d8fc763e6c
--- /dev/null
+++ b/drivers/usb/typec/ucsi/debug.h
@@ -0,0 +1,64 @@
+#ifndef __UCSI_DEBUG_H
+#define __UCSI_DEBUG_H
+
+#include "ucsi.h"
+
+static const char * const ucsi_cmd_strs[] = {
+	[0]				= "Unknown command",
+	[UCSI_PPM_RESET]		= "PPM_RESET",
+	[UCSI_CANCEL]			= "CANCEL",
+	[UCSI_CONNECTOR_RESET]		= "CONNECTOR_RESET",
+	[UCSI_ACK_CC_CI]		= "ACK_CC_CI",
+	[UCSI_SET_NOTIFICATION_ENABLE]	= "SET_NOTIFICATION_ENABLE",
+	[UCSI_GET_CAPABILITY]		= "GET_CAPABILITY",
+	[UCSI_GET_CONNECTOR_CAPABILITY]	= "GET_CONNECTOR_CAPABILITY",
+	[UCSI_SET_UOM]			= "SET_UOM",
+	[UCSI_SET_UOR]			= "SET_UOR",
+	[UCSI_SET_PDM]			= "SET_PDM",
+	[UCSI_SET_PDR]			= "SET_PDR",
+	[UCSI_GET_ALTERNATE_MODES]	= "GET_ALTERNATE_MODES",
+	[UCSI_GET_CAM_SUPPORTED]	= "GET_CAM_SUPPORTED",
+	[UCSI_GET_CURRENT_CAM]		= "GET_CURRENT_CAM",
+	[UCSI_SET_NEW_CAM]		= "SET_NEW_CAM",
+	[UCSI_GET_PDOS]			= "GET_PDOS",
+	[UCSI_GET_CABLE_PROPERTY]	= "GET_CABLE_PROPERTY",
+	[UCSI_GET_CONNECTOR_STATUS]	= "GET_CONNECTOR_STATUS",
+	[UCSI_GET_ERROR_STATUS]		= "GET_ERROR_STATUS",
+};
+
+static inline const char *ucsi_cmd_str(u64 raw_cmd)
+{
+	u8 cmd = raw_cmd & GENMASK(7, 0);
+
+	return ucsi_cmd_strs[(cmd >= ARRAY_SIZE(ucsi_cmd_strs)) ? 0 : cmd];
+}
+
+static const char * const ucsi_ack_strs[] = {
+	[0]				= "",
+	[UCSI_ACK_EVENT]		= "event",
+	[UCSI_ACK_CMD]			= "command",
+};
+
+static inline const char *ucsi_ack_str(u8 ack)
+{
+	return ucsi_ack_strs[(ack >= ARRAY_SIZE(ucsi_ack_strs)) ? 0 : ack];
+}
+
+static inline const char *ucsi_cci_str(u32 cci)
+{
+	if (cci & GENMASK(7, 0)) {
+		if (cci & BIT(29))
+			return "Event pending (ACK completed)";
+		if (cci & BIT(31))
+			return "Event pending (command completed)";
+		return "Connector Change";
+	}
+	if (cci & BIT(29))
+		return "ACK completed";
+	if (cci & BIT(31))
+		return "Command completed";
+
+	return "";
+}
+
+#endif /* __UCSI_DEBUG_H */
diff --git a/drivers/usb/typec/ucsi/trace.c b/drivers/usb/typec/ucsi/trace.c
new file mode 100644
index 000000000000..006f65c72a34
--- /dev/null
+++ b/drivers/usb/typec/ucsi/trace.c
@@ -0,0 +1,2 @@
+#define CREATE_TRACE_POINTS
+#include "trace.h"
diff --git a/drivers/usb/typec/ucsi/trace.h b/drivers/usb/typec/ucsi/trace.h
new file mode 100644
index 000000000000..98b404404834
--- /dev/null
+++ b/drivers/usb/typec/ucsi/trace.h
@@ -0,0 +1,143 @@
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM ucsi
+
+#if !defined(__UCSI_TRACE_H) || defined(TRACE_HEADER_MULTI_READ)
+#define __UCSI_TRACE_H
+
+#include <linux/tracepoint.h>
+#include "ucsi.h"
+#include "debug.h"
+
+DECLARE_EVENT_CLASS(ucsi_log_ack,
+	TP_PROTO(u8 ack),
+	TP_ARGS(ack),
+	TP_STRUCT__entry(
+		__field(u8, ack)
+	),
+	TP_fast_assign(
+		__entry->ack = ack;
+	),
+	TP_printk("ACK %s", ucsi_ack_str(__entry->ack))
+);
+
+DEFINE_EVENT(ucsi_log_ack, ucsi_ack,
+	TP_PROTO(u8 ack),
+	TP_ARGS(ack)
+);
+
+DECLARE_EVENT_CLASS(ucsi_log_control,
+	TP_PROTO(struct ucsi_control *ctrl),
+	TP_ARGS(ctrl),
+	TP_STRUCT__entry(
+		__field(u64, ctrl)
+	),
+	TP_fast_assign(
+		__entry->ctrl = ctrl->raw_cmd;
+	),
+	TP_printk("control=%08llx (%s)", __entry->ctrl,
+		ucsi_cmd_str(__entry->ctrl))
+);
+
+DEFINE_EVENT(ucsi_log_control, ucsi_command,
+	TP_PROTO(struct ucsi_control *ctrl),
+	TP_ARGS(ctrl)
+);
+
+DECLARE_EVENT_CLASS(ucsi_log_command,
+	TP_PROTO(struct ucsi_control *ctrl, int ret),
+	TP_ARGS(ctrl, ret),
+	TP_STRUCT__entry(
+		__field(u64, ctrl)
+		__field(int, ret)
+	),
+	TP_fast_assign(
+		__entry->ctrl = ctrl->raw_cmd;
+		__entry->ret = ret;
+	),
+	TP_printk("%s -> %s (err=%d)", ucsi_cmd_str(__entry->ctrl),
+		__entry->ret < 0 ? "FAIL" : "OK",
+		__entry->ret < 0 ? __entry->ret : 0)
+);
+
+DEFINE_EVENT(ucsi_log_command, ucsi_run_command,
+	TP_PROTO(struct ucsi_control *ctrl, int ret),
+	TP_ARGS(ctrl, ret)
+);
+
+DEFINE_EVENT(ucsi_log_command, ucsi_reset_ppm,
+	TP_PROTO(struct ucsi_control *ctrl, int ret),
+	TP_ARGS(ctrl, ret)
+);
+
+DECLARE_EVENT_CLASS(ucsi_log_cci,
+	TP_PROTO(u32 cci),
+	TP_ARGS(cci),
+	TP_STRUCT__entry(
+		__field(u32, cci)
+	),
+	TP_fast_assign(
+		__entry->cci = cci;
+	),
+	TP_printk("CCI=%08x %s", __entry->cci, ucsi_cci_str(__entry->cci))
+);
+
+DEFINE_EVENT(ucsi_log_cci, ucsi_notify,
+	TP_PROTO(u32 cci),
+	TP_ARGS(cci)
+);
+
+DECLARE_EVENT_CLASS(ucsi_log_connector_status,
+	TP_PROTO(int port, struct ucsi_connector_status *status),
+	TP_ARGS(port, status),
+	TP_STRUCT__entry(
+		__field(int, port)
+		__field(u16, change)
+		__field(u8, opmode)
+		__field(u8, connected)
+		__field(u8, pwr_dir)
+		__field(u8, partner_flags)
+		__field(u8, partner_type)
+		__field(u32, request_data_obj)
+		__field(u8, bc_status)
+	),
+	TP_fast_assign(
+		__entry->port = port - 1;
+		__entry->change = status->change;
+		__entry->opmode = status->pwr_op_mode;
+		__entry->connected = status->connected;
+		__entry->pwr_dir = status->pwr_dir;
+		__entry->partner_flags = status->partner_flags;
+		__entry->partner_type = status->partner_type;
+		__entry->request_data_obj = status->request_data_obj;
+		__entry->bc_status = status->bc_status;
+	),
+	TP_printk("port%d status: change=%04x, opmode=%x, connected=%d, "
+		"sourcing=%d, partner_flags=%x, partner_type=%x, "
+		"request_data_obj=%08x, BC status=%x", __entry->port,
+		__entry->change, __entry->opmode, __entry->connected,
+		__entry->pwr_dir, __entry->partner_flags, __entry->partner_type,
+		__entry->request_data_obj, __entry->bc_status)
+);
+
+DEFINE_EVENT(ucsi_log_connector_status, ucsi_connector_change,
+	TP_PROTO(int port, struct ucsi_connector_status *status),
+	TP_ARGS(port, status)
+);
+
+DEFINE_EVENT(ucsi_log_connector_status, ucsi_register_port,
+	TP_PROTO(int port, struct ucsi_connector_status *status),
+	TP_ARGS(port, status)
+);
+
+#endif /* __UCSI_TRACE_H */
+
+/* This part must be outside protection */
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE trace
+
+#include <trace/define_trace.h>
diff --git a/drivers/usb/typec/ucsi/ucsi.c b/drivers/usb/typec/ucsi/ucsi.c
new file mode 100644
index 000000000000..714c5bcedf2b
--- /dev/null
+++ b/drivers/usb/typec/ucsi/ucsi.c
@@ -0,0 +1,790 @@
+/*
+ * USB Type-C Connector System Software Interface driver
+ *
+ * Copyright (C) 2017, Intel Corporation
+ * Author: Heikki Krogerus <heikki.krogerus@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/completion.h>
+#include <linux/property.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/usb/typec.h>
+
+#include "ucsi.h"
+#include "trace.h"
+
+#define to_ucsi_connector(_cap_) container_of(_cap_, struct ucsi_connector, \
+					      typec_cap)
+
+/*
+ * UCSI_TIMEOUT_MS - PPM communication timeout
+ *
+ * Ideally we could use MIN_TIME_TO_RESPOND_WITH_BUSY (which is defined in UCSI
+ * specification) here as reference, but unfortunately we can't. It is very
+ * difficult to estimate the time it takes for the system to process the command
+ * before it is actually passed to the PPM.
+ */
+#define UCSI_TIMEOUT_MS		1000
+
+/*
+ * UCSI_SWAP_TIMEOUT_MS - Timeout for role swap requests
+ *
+ * 5 seconds is close to the time it takes for CapsCounter to reach 0, so even
+ * if the PPM does not generate Connector Change events before that with
+ * partners that do not support USB Power Delivery, this should still work.
+ */
+#define UCSI_SWAP_TIMEOUT_MS	5000
+
+enum ucsi_status {
+	UCSI_IDLE = 0,
+	UCSI_BUSY,
+	UCSI_ERROR,
+};
+
+struct ucsi_connector {
+	int num;
+
+	struct ucsi *ucsi;
+	struct work_struct work;
+	struct completion complete;
+
+	struct typec_port *port;
+	struct typec_partner *partner;
+
+	struct typec_capability typec_cap;
+
+	struct ucsi_connector_status status;
+	struct ucsi_connector_capability cap;
+};
+
+struct ucsi {
+	struct device *dev;
+	struct ucsi_ppm *ppm;
+
+	enum ucsi_status status;
+	struct completion complete;
+	struct ucsi_capability cap;
+	struct ucsi_connector *connector;
+
+	struct work_struct work;
+
+	/* PPM Communication lock */
+	struct mutex ppm_lock;
+
+	/* PPM communication flags */
+	unsigned long flags;
+#define EVENT_PENDING	0
+#define COMMAND_PENDING	1
+#define ACK_PENDING	2
+};
+
+static inline int ucsi_sync(struct ucsi *ucsi)
+{
+	if (ucsi->ppm && ucsi->ppm->sync)
+		return ucsi->ppm->sync(ucsi->ppm);
+	return 0;
+}
+
+static int ucsi_command(struct ucsi *ucsi, struct ucsi_control *ctrl)
+{
+	int ret;
+
+	trace_ucsi_command(ctrl);
+
+	set_bit(COMMAND_PENDING, &ucsi->flags);
+
+	ret = ucsi->ppm->cmd(ucsi->ppm, ctrl);
+	if (ret)
+		goto err_clear_flag;
+
+	if (!wait_for_completion_timeout(&ucsi->complete,
+					 msecs_to_jiffies(UCSI_TIMEOUT_MS))) {
+		dev_warn(ucsi->dev, "PPM NOT RESPONDING\n");
+		ret = -ETIMEDOUT;
+	}
+
+err_clear_flag:
+	clear_bit(COMMAND_PENDING, &ucsi->flags);
+
+	return ret;
+}
+
+static int ucsi_ack(struct ucsi *ucsi, u8 ack)
+{
+	struct ucsi_control ctrl;
+	int ret;
+
+	trace_ucsi_ack(ack);
+
+	set_bit(ACK_PENDING, &ucsi->flags);
+
+	UCSI_CMD_ACK(ctrl, ack);
+	ret = ucsi->ppm->cmd(ucsi->ppm, &ctrl);
+	if (ret)
+		goto out_clear_bit;
+
+	/* Waiting for ACK with ACK CMD, but not with EVENT for now */
+	if (ack == UCSI_ACK_EVENT)
+		goto out_clear_bit;
+
+	if (!wait_for_completion_timeout(&ucsi->complete,
+					 msecs_to_jiffies(UCSI_TIMEOUT_MS)))
+		ret = -ETIMEDOUT;
+
+out_clear_bit:
+	clear_bit(ACK_PENDING, &ucsi->flags);
+
+	if (ret)
+		dev_err(ucsi->dev, "%s: failed\n", __func__);
+
+	return ret;
+}
+
+static int ucsi_run_command(struct ucsi *ucsi, struct ucsi_control *ctrl,
+			    void *data, size_t size)
+{
+	struct ucsi_control _ctrl;
+	u8 data_length;
+	u16 error;
+	int ret;
+
+	ret = ucsi_command(ucsi, ctrl);
+	if (ret)
+		goto err;
+
+	switch (ucsi->status) {
+	case UCSI_IDLE:
+		ret = ucsi_sync(ucsi);
+		if (ret)
+			dev_warn(ucsi->dev, "%s: sync failed\n", __func__);
+
+		if (data)
+			memcpy(data, ucsi->ppm->data->message_in, size);
+
+		data_length = ucsi->ppm->data->cci.data_length;
+
+		ret = ucsi_ack(ucsi, UCSI_ACK_CMD);
+		if (!ret)
+			ret = data_length;
+		break;
+	case UCSI_BUSY:
+		/* The caller decides whether to cancel or not */
+		ret = -EBUSY;
+		break;
+	case UCSI_ERROR:
+		ret = ucsi_ack(ucsi, UCSI_ACK_CMD);
+		if (ret)
+			break;
+
+		_ctrl.raw_cmd = 0;
+		_ctrl.cmd.cmd = UCSI_GET_ERROR_STATUS;
+		ret = ucsi_command(ucsi, &_ctrl);
+		if (ret) {
+			dev_err(ucsi->dev, "reading error failed!\n");
+			break;
+		}
+
+		memcpy(&error, ucsi->ppm->data->message_in, sizeof(error));
+
+		/* Something has really gone wrong */
+		if (WARN_ON(ucsi->status == UCSI_ERROR)) {
+			ret = -ENODEV;
+			break;
+		}
+
+		ret = ucsi_ack(ucsi, UCSI_ACK_CMD);
+		if (ret)
+			break;
+
+		switch (error) {
+		case UCSI_ERROR_INCOMPATIBLE_PARTNER:
+			ret = -EOPNOTSUPP;
+			break;
+		case UCSI_ERROR_CC_COMMUNICATION_ERR:
+			ret = -ECOMM;
+			break;
+		case UCSI_ERROR_CONTRACT_NEGOTIATION_FAIL:
+			ret = -EPROTO;
+			break;
+		case UCSI_ERROR_DEAD_BATTERY:
+			dev_warn(ucsi->dev, "Dead battery condition!\n");
+			ret = -EPERM;
+			break;
+		/* The following mean a bug in this driver */
+		case UCSI_ERROR_INVALID_CON_NUM:
+		case UCSI_ERROR_UNREGONIZED_CMD:
+		case UCSI_ERROR_INVALID_CMD_ARGUMENT:
+			dev_warn(ucsi->dev,
+				 "%s: possible UCSI driver bug - error 0x%x\n",
+				 __func__, error);
+			ret = -EINVAL;
+			break;
+		default:
+			dev_warn(ucsi->dev,
+				 "%s: error without status\n", __func__);
+			ret = -EIO;
+			break;
+		}
+		break;
+	}
+
+err:
+	trace_ucsi_run_command(ctrl, ret);
+
+	return ret;
+}
+
+/* -------------------------------------------------------------------------- */
+
+static void ucsi_pwr_opmode_change(struct ucsi_connector *con)
+{
+	switch (con->status.pwr_op_mode) {
+	case UCSI_CONSTAT_PWR_OPMODE_PD:
+		typec_set_pwr_opmode(con->port, TYPEC_PWR_MODE_PD);
+		break;
+	case UCSI_CONSTAT_PWR_OPMODE_TYPEC1_5:
+		typec_set_pwr_opmode(con->port, TYPEC_PWR_MODE_1_5A);
+		break;
+	case UCSI_CONSTAT_PWR_OPMODE_TYPEC3_0:
+		typec_set_pwr_opmode(con->port, TYPEC_PWR_MODE_3_0A);
+		break;
+	default:
+		typec_set_pwr_opmode(con->port, TYPEC_PWR_MODE_USB);
+		break;
+	}
+}
+
+static int ucsi_register_partner(struct ucsi_connector *con)
+{
+	struct typec_partner_desc partner;
+
+	if (con->partner)
+		return 0;
+
+	memset(&partner, 0, sizeof(partner));
+
+	switch (con->status.partner_type) {
+	case UCSI_CONSTAT_PARTNER_TYPE_DEBUG:
+		partner.accessory = TYPEC_ACCESSORY_DEBUG;
+		break;
+	case UCSI_CONSTAT_PARTNER_TYPE_AUDIO:
+		partner.accessory = TYPEC_ACCESSORY_AUDIO;
+		break;
+	default:
+		break;
+	}
+
+	partner.usb_pd = con->status.pwr_op_mode == UCSI_CONSTAT_PWR_OPMODE_PD;
+
+	con->partner = typec_register_partner(con->port, &partner);
+	if (!con->partner) {
+		dev_err(con->ucsi->dev, "con%d: failed to register partner\n",
+			con->num);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void ucsi_unregister_partner(struct ucsi_connector *con)
+{
+	typec_unregister_partner(con->partner);
+	con->partner = NULL;
+}
+
+static void ucsi_connector_change(struct work_struct *work)
+{
+	struct ucsi_connector *con = container_of(work, struct ucsi_connector,
+						  work);
+	struct ucsi *ucsi = con->ucsi;
+	struct ucsi_control ctrl;
+	int ret;
+
+	mutex_lock(&ucsi->ppm_lock);
+
+	UCSI_CMD_GET_CONNECTOR_STATUS(ctrl, con->num);
+	ret = ucsi_run_command(ucsi, &ctrl, &con->status, sizeof(con->status));
+	if (ret < 0) {
+		dev_err(ucsi->dev, "%s: GET_CONNECTOR_STATUS failed (%d)\n",
+			__func__, ret);
+		goto out_unlock;
+	}
+
+	if (con->status.change & UCSI_CONSTAT_POWER_OPMODE_CHANGE)
+		ucsi_pwr_opmode_change(con);
+
+	if (con->status.change & UCSI_CONSTAT_POWER_DIR_CHANGE) {
+		typec_set_pwr_role(con->port, con->status.pwr_dir);
+
+		/* Complete pending power role swap */
+		if (!completion_done(&con->complete))
+			complete(&con->complete);
+	}
+
+	if (con->status.change & UCSI_CONSTAT_PARTNER_CHANGE) {
+		switch (con->status.partner_type) {
+		case UCSI_CONSTAT_PARTNER_TYPE_UFP:
+			typec_set_data_role(con->port, TYPEC_HOST);
+			break;
+		case UCSI_CONSTAT_PARTNER_TYPE_DFP:
+			typec_set_data_role(con->port, TYPEC_DEVICE);
+			break;
+		default:
+			break;
+		}
+
+		/* Complete pending data role swap */
+		if (!completion_done(&con->complete))
+			complete(&con->complete);
+	}
+
+	if (con->status.change & UCSI_CONSTAT_CONNECT_CHANGE) {
+		if (con->status.connected)
+			ucsi_register_partner(con);
+		else
+			ucsi_unregister_partner(con);
+	}
+
+	ret = ucsi_ack(ucsi, UCSI_ACK_EVENT);
+	if (ret)
+		dev_err(ucsi->dev, "%s: ACK failed (%d)", __func__, ret);
+
+	trace_ucsi_connector_change(con->num, &con->status);
+
+out_unlock:
+	clear_bit(EVENT_PENDING, &ucsi->flags);
+	mutex_unlock(&ucsi->ppm_lock);
+}
+
+/**
+ * ucsi_notify - PPM notification handler
+ * @ucsi: Source UCSI Interface for the notifications
+ *
+ * Handle notifications from PPM of @ucsi.
+ */
+void ucsi_notify(struct ucsi *ucsi)
+{
+	struct ucsi_cci *cci;
+
+	/* There is no requirement to sync here, but no harm either. */
+	ucsi_sync(ucsi);
+
+	cci = &ucsi->ppm->data->cci;
+
+	if (cci->error)
+		ucsi->status = UCSI_ERROR;
+	else if (cci->busy)
+		ucsi->status = UCSI_BUSY;
+	else
+		ucsi->status = UCSI_IDLE;
+
+	if (cci->cmd_complete && test_bit(COMMAND_PENDING, &ucsi->flags)) {
+		complete(&ucsi->complete);
+	} else if (cci->ack_complete && test_bit(ACK_PENDING, &ucsi->flags)) {
+		complete(&ucsi->complete);
+	} else if (cci->connector_change) {
+		struct ucsi_connector *con;
+
+		con = &ucsi->connector[cci->connector_change - 1];
+
+		if (!test_and_set_bit(EVENT_PENDING, &ucsi->flags))
+			schedule_work(&con->work);
+	}
+
+	trace_ucsi_notify(ucsi->ppm->data->raw_cci);
+}
+EXPORT_SYMBOL_GPL(ucsi_notify);
+
+/* -------------------------------------------------------------------------- */
+
+static int ucsi_reset_connector(struct ucsi_connector *con, bool hard)
+{
+	struct ucsi_control ctrl;
+
+	UCSI_CMD_CONNECTOR_RESET(ctrl, con, hard);
+
+	return ucsi_run_command(con->ucsi, &ctrl, NULL, 0);
+}
+
+static int ucsi_reset_ppm(struct ucsi *ucsi)
+{
+	struct ucsi_control ctrl;
+	unsigned long tmo;
+	int ret;
+
+	ctrl.raw_cmd = 0;
+	ctrl.cmd.cmd = UCSI_PPM_RESET;
+	trace_ucsi_command(&ctrl);
+	ret = ucsi->ppm->cmd(ucsi->ppm, &ctrl);
+	if (ret)
+		goto err;
+
+	tmo = jiffies + msecs_to_jiffies(UCSI_TIMEOUT_MS);
+
+	do {
+		/* Here sync is critical. */
+		ret = ucsi_sync(ucsi);
+		if (ret)
+			goto err;
+
+		if (ucsi->ppm->data->cci.reset_complete)
+			break;
+
+		/* If the PPM is still doing something else, reset it again. */
+		if (ucsi->ppm->data->raw_cci) {
+			dev_warn_ratelimited(ucsi->dev,
+				"Failed to reset PPM! Trying again..\n");
+
+			trace_ucsi_command(&ctrl);
+			ret = ucsi->ppm->cmd(ucsi->ppm, &ctrl);
+			if (ret)
+				goto err;
+		}
+
+		/* Letting the PPM settle down. */
+		msleep(20);
+
+		ret = -ETIMEDOUT;
+	} while (time_is_after_jiffies(tmo));
+
+err:
+	trace_ucsi_reset_ppm(&ctrl, ret);
+
+	return ret;
+}
+
+static int ucsi_role_cmd(struct ucsi_connector *con, struct ucsi_control *ctrl)
+{
+	int ret;
+
+	ret = ucsi_run_command(con->ucsi, ctrl, NULL, 0);
+	if (ret == -ETIMEDOUT) {
+		struct ucsi_control c;
+
+		/* PPM most likely stopped responding. Resetting everything. */
+		ucsi_reset_ppm(con->ucsi);
+
+		UCSI_CMD_SET_NTFY_ENABLE(c, UCSI_ENABLE_NTFY_ALL);
+		ucsi_run_command(con->ucsi, &c, NULL, 0);
+
+		ucsi_reset_connector(con, true);
+	}
+
+	return ret;
+}
+
+static int
+ucsi_dr_swap(const struct typec_capability *cap, enum typec_data_role role)
+{
+	struct ucsi_connector *con = to_ucsi_connector(cap);
+	struct ucsi_control ctrl;
+	int ret = 0;
+
+	if (!con->partner)
+		return -ENOTCONN;
+
+	mutex_lock(&con->ucsi->ppm_lock);
+
+	if ((con->status.partner_type == UCSI_CONSTAT_PARTNER_TYPE_DFP &&
+	     role == TYPEC_DEVICE) ||
+	    (con->status.partner_type == UCSI_CONSTAT_PARTNER_TYPE_UFP &&
+	     role == TYPEC_HOST))
+		goto out_unlock;
+
+	UCSI_CMD_SET_UOR(ctrl, con, role);
+	ret = ucsi_role_cmd(con, &ctrl);
+	if (ret < 0)
+		goto out_unlock;
+
+	mutex_unlock(&con->ucsi->ppm_lock);
+
+	if (!wait_for_completion_timeout(&con->complete,
+					msecs_to_jiffies(UCSI_SWAP_TIMEOUT_MS)))
+		return -ETIMEDOUT;
+
+	return 0;
+
+out_unlock:
+	mutex_unlock(&con->ucsi->ppm_lock);
+
+	return ret;
+}
+
+static int
+ucsi_pr_swap(const struct typec_capability *cap, enum typec_role role)
+{
+	struct ucsi_connector *con = to_ucsi_connector(cap);
+	struct ucsi_control ctrl;
+	int ret = 0;
+
+	if (!con->partner)
+		return -ENOTCONN;
+
+	mutex_lock(&con->ucsi->ppm_lock);
+
+	if (con->status.pwr_dir == role)
+		goto out_unlock;
+
+	UCSI_CMD_SET_PDR(ctrl, con, role);
+	ret = ucsi_role_cmd(con, &ctrl);
+	if (ret < 0)
+		goto out_unlock;
+
+	mutex_unlock(&con->ucsi->ppm_lock);
+
+	if (!wait_for_completion_timeout(&con->complete,
+					msecs_to_jiffies(UCSI_SWAP_TIMEOUT_MS)))
+		return -ETIMEDOUT;
+
+	mutex_lock(&con->ucsi->ppm_lock);
+
+	/* Something has gone wrong while swapping the role */
+	if (con->status.pwr_op_mode != UCSI_CONSTAT_PWR_OPMODE_PD) {
+		ucsi_reset_connector(con, true);
+		ret = -EPROTO;
+	}
+
+out_unlock:
+	mutex_unlock(&con->ucsi->ppm_lock);
+
+	return ret;
+}
+
+static struct fwnode_handle *ucsi_find_fwnode(struct ucsi_connector *con)
+{
+	struct fwnode_handle *fwnode;
+	int i = 1;
+
+	device_for_each_child_node(con->ucsi->dev, fwnode)
+		if (i++ == con->num)
+			return fwnode;
+	return NULL;
+}
+
+static int ucsi_register_port(struct ucsi *ucsi, int index)
+{
+	struct ucsi_connector *con = &ucsi->connector[index];
+	struct typec_capability *cap = &con->typec_cap;
+	enum typec_accessory *accessory = cap->accessory;
+	struct ucsi_control ctrl;
+	int ret;
+
+	INIT_WORK(&con->work, ucsi_connector_change);
+	init_completion(&con->complete);
+	con->num = index + 1;
+	con->ucsi = ucsi;
+
+	/* Get connector capability */
+	UCSI_CMD_GET_CONNECTOR_CAPABILITY(ctrl, con->num);
+	ret = ucsi_run_command(ucsi, &ctrl, &con->cap, sizeof(con->cap));
+	if (ret < 0)
+		return ret;
+
+	if (con->cap.op_mode & UCSI_CONCAP_OPMODE_DRP)
+		cap->type = TYPEC_PORT_DRP;
+	else if (con->cap.op_mode & UCSI_CONCAP_OPMODE_DFP)
+		cap->type = TYPEC_PORT_DFP;
+	else if (con->cap.op_mode & UCSI_CONCAP_OPMODE_UFP)
+		cap->type = TYPEC_PORT_UFP;
+
+	cap->revision = ucsi->cap.typec_version;
+	cap->pd_revision = ucsi->cap.pd_version;
+	cap->prefer_role = TYPEC_NO_PREFERRED_ROLE;
+
+	if (con->cap.op_mode & UCSI_CONCAP_OPMODE_AUDIO_ACCESSORY)
+		*accessory++ = TYPEC_ACCESSORY_AUDIO;
+	if (con->cap.op_mode & UCSI_CONCAP_OPMODE_DEBUG_ACCESSORY)
+		*accessory = TYPEC_ACCESSORY_DEBUG;
+
+	cap->fwnode = ucsi_find_fwnode(con);
+	cap->dr_set = ucsi_dr_swap;
+	cap->pr_set = ucsi_pr_swap;
+
+	/* Register the connector */
+	con->port = typec_register_port(ucsi->dev, cap);
+	if (!con->port)
+		return -ENODEV;
+
+	/* Get the status */
+	UCSI_CMD_GET_CONNECTOR_STATUS(ctrl, con->num);
+	ret = ucsi_run_command(ucsi, &ctrl, &con->status, sizeof(con->status));
+	if (ret < 0) {
+		dev_err(ucsi->dev, "con%d: failed to get status\n", con->num);
+		return 0;
+	}
+
+	ucsi_pwr_opmode_change(con);
+	typec_set_pwr_role(con->port, con->status.pwr_dir);
+
+	switch (con->status.partner_type) {
+	case UCSI_CONSTAT_PARTNER_TYPE_UFP:
+		typec_set_data_role(con->port, TYPEC_HOST);
+		break;
+	case UCSI_CONSTAT_PARTNER_TYPE_DFP:
+		typec_set_data_role(con->port, TYPEC_DEVICE);
+		break;
+	default:
+		break;
+	}
+
+	/* Check if there is already something connected */
+	if (con->status.connected)
+		ucsi_register_partner(con);
+
+	trace_ucsi_register_port(con->num, &con->status);
+
+	return 0;
+}
+
+static void ucsi_init(struct work_struct *work)
+{
+	struct ucsi *ucsi = container_of(work, struct ucsi, work);
+	struct ucsi_connector *con;
+	struct ucsi_control ctrl;
+	int ret;
+	int i;
+
+	mutex_lock(&ucsi->ppm_lock);
+
+	/* Reset the PPM */
+	ret = ucsi_reset_ppm(ucsi);
+	if (ret) {
+		dev_err(ucsi->dev, "failed to reset PPM!\n");
+		goto err;
+	}
+
+	/* Enable basic notifications */
+	UCSI_CMD_SET_NTFY_ENABLE(ctrl, UCSI_ENABLE_NTFY_CMD_COMPLETE |
+					UCSI_ENABLE_NTFY_ERROR);
+	ret = ucsi_run_command(ucsi, &ctrl, NULL, 0);
+	if (ret < 0)
+		goto err_reset;
+
+	/* Get PPM capabilities */
+	UCSI_CMD_GET_CAPABILITY(ctrl);
+	ret = ucsi_run_command(ucsi, &ctrl, &ucsi->cap, sizeof(ucsi->cap));
+	if (ret < 0)
+		goto err_reset;
+
+	if (!ucsi->cap.num_connectors) {
+		ret = -ENODEV;
+		goto err_reset;
+	}
+
+	/* Allocate the connectors. Released in ucsi_unregister_ppm() */
+	ucsi->connector = kcalloc(ucsi->cap.num_connectors + 1,
+				  sizeof(*ucsi->connector), GFP_KERNEL);
+	if (!ucsi->connector) {
+		ret = -ENOMEM;
+		goto err_reset;
+	}
+
+	/* Register all connectors */
+	for (i = 0; i < ucsi->cap.num_connectors; i++) {
+		ret = ucsi_register_port(ucsi, i);
+		if (ret)
+			goto err_unregister;
+	}
+
+	/* Enable all notifications */
+	UCSI_CMD_SET_NTFY_ENABLE(ctrl, UCSI_ENABLE_NTFY_ALL);
+	ret = ucsi_run_command(ucsi, &ctrl, NULL, 0);
+	if (ret < 0)
+		goto err_unregister;
+
+	mutex_unlock(&ucsi->ppm_lock);
+
+	return;
+
+err_unregister:
+	for (con = ucsi->connector; con->port; con++) {
+		ucsi_unregister_partner(con);
+		typec_unregister_port(con->port);
+		con->port = NULL;
+	}
+
+err_reset:
+	ucsi_reset_ppm(ucsi);
+err:
+	mutex_unlock(&ucsi->ppm_lock);
+	dev_err(ucsi->dev, "PPM init failed (%d)\n", ret);
+}
+
+/**
+ * ucsi_register_ppm - Register UCSI PPM Interface
+ * @dev: Device interface to the PPM
+ * @ppm: The PPM interface
+ *
+ * Allocates UCSI instance, associates it with @ppm and returns it to the
+ * caller, and schedules initialization of the interface.
+ */
+struct ucsi *ucsi_register_ppm(struct device *dev, struct ucsi_ppm *ppm)
+{
+	struct ucsi *ucsi;
+
+	ucsi = kzalloc(sizeof(*ucsi), GFP_KERNEL);
+	if (!ucsi)
+		return ERR_PTR(-ENOMEM);
+
+	INIT_WORK(&ucsi->work, ucsi_init);
+	init_completion(&ucsi->complete);
+	mutex_init(&ucsi->ppm_lock);
+
+	ucsi->dev = dev;
+	ucsi->ppm = ppm;
+
+	/*
+	 * Communication with the PPM takes a lot of time. It is not reasonable
+	 * to initialize the driver here. Using a work for now.
+	 */
+	queue_work(system_long_wq, &ucsi->work);
+
+	return ucsi;
+}
+EXPORT_SYMBOL_GPL(ucsi_register_ppm);
+
+/**
+ * ucsi_unregister_ppm - Unregister UCSI PPM Interface
+ * @ucsi: struct ucsi associated with the PPM
+ *
+ * Unregister UCSI PPM that was created with ucsi_register().
+ */
+void ucsi_unregister_ppm(struct ucsi *ucsi)
+{
+	struct ucsi_control ctrl;
+	int i;
+
+	/* Make sure that we are not in the middle of driver initialization */
+	cancel_work_sync(&ucsi->work);
+
+	mutex_lock(&ucsi->ppm_lock);
+
+	/* Disable everything except command complete notification */
+	UCSI_CMD_SET_NTFY_ENABLE(ctrl, UCSI_ENABLE_NTFY_CMD_COMPLETE)
+	ucsi_run_command(ucsi, &ctrl, NULL, 0);
+
+	mutex_unlock(&ucsi->ppm_lock);
+
+	for (i = 0; i < ucsi->cap.num_connectors; i++) {
+		cancel_work_sync(&ucsi->connector[i].work);
+		ucsi_unregister_partner(&ucsi->connector[i]);
+		typec_unregister_port(ucsi->connector[i].port);
+	}
+
+	ucsi_reset_ppm(ucsi);
+
+	kfree(ucsi->connector);
+	kfree(ucsi);
+}
+EXPORT_SYMBOL_GPL(ucsi_unregister_ppm);
+
+MODULE_AUTHOR("Heikki Krogerus <heikki.krogerus@linux.intel.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("USB Type-C Connector System Software Interface driver");
diff --git a/drivers/usb/misc/ucsi.h b/drivers/usb/typec/ucsi/ucsi.h
index 6dd11d1fe225..6b0d2f0918c6 100644
--- a/drivers/usb/misc/ucsi.h
+++ b/drivers/usb/typec/ucsi/ucsi.h
@@ -1,21 +1,25 @@
 
+#ifndef __DRIVER_USB_TYPEC_UCSI_H
+#define __DRIVER_USB_TYPEC_UCSI_H
+
+#include <linux/bitops.h>
 #include <linux/types.h>
 
 /* -------------------------------------------------------------------------- */
 
 /* Command Status and Connector Change Indication (CCI) data structure */
 struct ucsi_cci {
-	unsigned int RESERVED1:1;
-	unsigned int connector_change:7;
+	u8:1; /* reserved */
+	u8 connector_change:7;
 	u8 data_length;
-	unsigned int RESERVED9:9;
-	unsigned int not_supported:1;
-	unsigned int cancel_complete:1;
-	unsigned int reset_complete:1;
-	unsigned int busy:1;
-	unsigned int ack_complete:1;
-	unsigned int error:1;
-	unsigned int cmd_complete:1;
+	u16:9; /* reserved */
+	u16 not_supported:1;
+	u16 cancel_complete:1;
+	u16 reset_complete:1;
+	u16 busy:1;
+	u16 ack_complete:1;
+	u16 error:1;
+	u16 cmd_complete:1;
 } __packed;
 
 /* Default fields in CONTROL data structure */
@@ -25,16 +29,33 @@ struct ucsi_command {
 	u64 data:48;
 } __packed;
 
+/* ACK Command structure */
+struct ucsi_ack_cmd {
+	u8 cmd;
+	u8 length;
+	u8 cci_ack:1;
+	u8 cmd_ack:1;
+	u8:6; /* reserved */
+} __packed;
+
+/* Connector Reset Command structure */
+struct ucsi_con_rst {
+	u8 cmd;
+	u8 length;
+	u8 con_num:7;
+	u8 hard_reset:1;
+} __packed;
+
 /* Set USB Operation Mode Command structure */
 struct ucsi_uor_cmd {
 	u8 cmd;
 	u8 length;
-	u64 con_num:7;
-	u64 role:3;
+	u16 con_num:7;
+	u16 role:3;
 #define UCSI_UOR_ROLE_DFP			BIT(0)
 #define UCSI_UOR_ROLE_UFP			BIT(1)
 #define UCSI_UOR_ROLE_DRP			BIT(2)
-	u64 data:38;
+	u16:6; /* reserved */
 } __packed;
 
 struct ucsi_control {
@@ -42,20 +63,82 @@ struct ucsi_control {
 		u64 raw_cmd;
 		struct ucsi_command cmd;
 		struct ucsi_uor_cmd uor;
+		struct ucsi_ack_cmd ack;
+		struct ucsi_con_rst con_rst;
 	};
 };
 
-struct ucsi_data {
-	u16 version;
-	u16 RESERVED;
-	union {
-		u32 raw_cci;
-		struct ucsi_cci cci;
-	};
-	struct ucsi_control ctrl;
-	u32 message_in[4];
-	u32 message_out[4];
-} __packed;
+#define __UCSI_CMD(_ctrl_, _cmd_)					\
+{									\
+	(_ctrl_).raw_cmd = 0;						\
+	(_ctrl_).cmd.cmd = _cmd_;					\
+}
+
+/* Helper for preparing ucsi_control for CONNECTOR_RESET command. */
+#define UCSI_CMD_CONNECTOR_RESET(_ctrl_, _con_, _hard_)			\
+{									\
+	__UCSI_CMD(_ctrl_, UCSI_CONNECTOR_RESET)			\
+	(_ctrl_).con_rst.con_num = (_con_)->num;			\
+	(_ctrl_).con_rst.hard_reset = _hard_;				\
+}
+
+/* Helper for preparing ucsi_control for ACK_CC_CI command. */
+#define UCSI_CMD_ACK(_ctrl_, _ack_)					\
+{									\
+	__UCSI_CMD(_ctrl_, UCSI_ACK_CC_CI)				\
+	(_ctrl_).ack.cci_ack = ((_ack_) == UCSI_ACK_EVENT);		\
+	(_ctrl_).ack.cmd_ack = ((_ack_) == UCSI_ACK_CMD);		\
+}
+
+/* Helper for preparing ucsi_control for SET_NOTIFY_ENABLE command. */
+#define UCSI_CMD_SET_NTFY_ENABLE(_ctrl_, _ntfys_)			\
+{									\
+	__UCSI_CMD(_ctrl_, UCSI_SET_NOTIFICATION_ENABLE)		\
+	(_ctrl_).cmd.data = _ntfys_;					\
+}
+
+/* Helper for preparing ucsi_control for GET_CAPABILITY command. */
+#define UCSI_CMD_GET_CAPABILITY(_ctrl_)					\
+{									\
+	__UCSI_CMD(_ctrl_, UCSI_GET_CAPABILITY)				\
+}
+
+/* Helper for preparing ucsi_control for GET_CONNECTOR_CAPABILITY command. */
+#define UCSI_CMD_GET_CONNECTOR_CAPABILITY(_ctrl_, _con_)		\
+{									\
+	__UCSI_CMD(_ctrl_, UCSI_GET_CONNECTOR_CAPABILITY)		\
+	(_ctrl_).cmd.data = _con_;					\
+}
+
+/* Helper for preparing ucsi_control for GET_CONNECTOR_STATUS command. */
+#define UCSI_CMD_GET_CONNECTOR_STATUS(_ctrl_, _con_)			\
+{									\
+	__UCSI_CMD(_ctrl_, UCSI_GET_CONNECTOR_STATUS)			\
+	(_ctrl_).cmd.data = _con_;					\
+}
+
+#define __UCSI_ROLE(_ctrl_, _cmd_, _con_num_)				\
+{									\
+	__UCSI_CMD(_ctrl_, _cmd_)					\
+	(_ctrl_).uor.con_num = _con_num_;				\
+	(_ctrl_).uor.role = UCSI_UOR_ROLE_DRP;				\
+}
+
+/* Helper for preparing ucsi_control for SET_UOR command. */
+#define UCSI_CMD_SET_UOR(_ctrl_, _con_, _role_)				\
+{									\
+	__UCSI_ROLE(_ctrl_, UCSI_SET_UOR, (_con_)->num)		\
+	(_ctrl_).uor.role |= (_role_) == TYPEC_HOST ? UCSI_UOR_ROLE_DFP : \
+			  UCSI_UOR_ROLE_UFP;				\
+}
+
+/* Helper for preparing ucsi_control for SET_PDR command. */
+#define UCSI_CMD_SET_PDR(_ctrl_, _con_, _role_)			\
+{									\
+	__UCSI_ROLE(_ctrl_, UCSI_SET_PDR, (_con_)->num)		\
+	(_ctrl_).uor.role |= (_role_) == TYPEC_SOURCE ? UCSI_UOR_ROLE_DFP : \
+			UCSI_UOR_ROLE_UFP;				\
+}
 
 /* Commands */
 #define UCSI_PPM_RESET			0x01
@@ -67,12 +150,12 @@ struct ucsi_data {
 #define UCSI_GET_CONNECTOR_CAPABILITY	0x07
 #define UCSI_SET_UOM			0x08
 #define UCSI_SET_UOR			0x09
-#define UCSI_SET_PDM			0x0A
-#define UCSI_SET_PDR			0x0B
-#define UCSI_GET_ALTERNATE_MODES	0x0C
-#define UCSI_GET_CAM_SUPPORTED		0x0D
-#define UCSI_GET_CURRENT_CAM		0x0E
-#define UCSI_SET_NEW_CAM		0x0F
+#define UCSI_SET_PDM			0x0a
+#define UCSI_SET_PDR			0x0b
+#define UCSI_GET_ALTERNATE_MODES	0x0c
+#define UCSI_GET_CAM_SUPPORTED		0x0d
+#define UCSI_GET_CURRENT_CAM		0x0e
+#define UCSI_SET_NEW_CAM		0x0f
 #define UCSI_GET_PDOS			0x10
 #define UCSI_GET_CABLE_PROPERTY		0x11
 #define UCSI_GET_CONNECTOR_STATUS	0x12
@@ -116,7 +199,7 @@ struct ucsi_capability {
 #define UCSI_CAP_ATTR_POWER_AC_SUPPLY		BIT(8)
 #define UCSI_CAP_ATTR_POWER_OTHER		BIT(10)
 #define UCSI_CAP_ATTR_POWER_VBUS		BIT(14)
-	u8 num_connectors;
+	u32 num_connectors:8;
 	u32 features:24;
 #define UCSI_CAP_SET_UOM			BIT(0)
 #define UCSI_CAP_SET_PDM			BIT(1)
@@ -127,7 +210,7 @@ struct ucsi_capability {
 #define UCSI_CAP_EXT_SUPPLY_NOTIFICATIONS	BIT(6)
 #define UCSI_CAP_PD_RESET			BIT(7)
 	u8 num_alt_modes;
-	u8 RESERVED;
+	u8 reserved;
 	u16 bc_version;
 	u16 pd_version;
 	u16 typec_version;
@@ -146,6 +229,12 @@ struct ucsi_connector_capability {
 #define UCSI_CONCAP_OPMODE_ALT_MODE		BIT(7)
 	u8 provider:1;
 	u8 consumer:1;
+	u8:6; /* reserved */
+} __packed;
+
+struct ucsi_altmode {
+	u16 svid;
+	u32 mid;
 } __packed;
 
 /* Data structure filled by PPM in response to GET_CABLE_PROPERTY command. */
@@ -161,9 +250,9 @@ struct ucsi_cable_property {
 #define UCSI_CABLE_PROPERTY_PLUG_TYPE_C		2
 #define UCSI_CABLE_PROPERTY_PLUG_OTHER		3
 	u8 mode_support:1;
-	u8 RESERVED_2:2;
+	u8:2; /* reserved */
 	u8 latency:4;
-	u8 RESERVED_4:4;
+	u8:4; /* reserved */
 } __packed;
 
 /* Data structure filled by PPM in response to GET_CONNECTOR_STATUS command. */
@@ -185,7 +274,7 @@ struct ucsi_connector_status {
 #define UCSI_CONSTAT_PWR_OPMODE_DEFAULT		1
 #define UCSI_CONSTAT_PWR_OPMODE_BC		2
 #define UCSI_CONSTAT_PWR_OPMODE_PD		3
-#define UCSI_CONSTAT_PWR_OPMODE_TYPEC1_3	4
+#define UCSI_CONSTAT_PWR_OPMODE_TYPEC1_5	4
 #define UCSI_CONSTAT_PWR_OPMODE_TYPEC3_0	5
 	u16 connected:1;
 	u16 pwr_dir:1;
@@ -195,7 +284,7 @@ struct ucsi_connector_status {
 	u16 partner_type:3;
 #define UCSI_CONSTAT_PARTNER_TYPE_DFP		1
 #define UCSI_CONSTAT_PARTNER_TYPE_UFP		2
-#define UCSI_CONSTAT_PARTNER_TYPE_CABLE_NO_UFP	3 /* Powered Cable */
+#define UCSI_CONSTAT_PARTNER_TYPE_CABLE		3 /* Powered Cable */
 #define UCSI_CONSTAT_PARTNER_TYPE_CABLE_AND_UFP	4 /* Powered Cable */
 #define UCSI_CONSTAT_PARTNER_TYPE_DEBUG		5
 #define UCSI_CONSTAT_PARTNER_TYPE_AUDIO		6
@@ -208,8 +297,39 @@ struct ucsi_connector_status {
 	u8 provider_cap_limit_reason:4;
 #define UCSI_CONSTAT_CAP_PWR_LOWERED		0
 #define UCSI_CONSTAT_CAP_PWR_BUDGET_LIMIT	1
-	u8 RESERVED:2;
+	u8:2; /* reserved */
 } __packed;
 
 /* -------------------------------------------------------------------------- */
 
+struct ucsi;
+
+struct ucsi_data {
+	u16 version;
+	u16 reserved;
+	union {
+		u32 raw_cci;
+		struct ucsi_cci cci;
+	};
+	struct ucsi_control ctrl;
+	u32 message_in[4];
+	u32 message_out[4];
+} __packed;
+
+/*
+ * struct ucsi_ppm - Interface to UCSI Platform Policy Manager
+ * @data: memory location to the UCSI data structures
+ * @cmd: UCSI command execution routine
+ * @sync: Refresh UCSI mailbox (the data structures)
+ */
+struct ucsi_ppm {
+	struct ucsi_data *data;
+	int (*cmd)(struct ucsi_ppm *, struct ucsi_control *);
+	int (*sync)(struct ucsi_ppm *);
+};
+
+struct ucsi *ucsi_register_ppm(struct device *dev, struct ucsi_ppm *ppm);
+void ucsi_unregister_ppm(struct ucsi *ucsi);
+void ucsi_notify(struct ucsi *ucsi);
+
+#endif /* __DRIVER_USB_TYPEC_UCSI_H */
diff --git a/drivers/usb/typec/ucsi/ucsi_acpi.c b/drivers/usb/typec/ucsi/ucsi_acpi.c
new file mode 100644
index 000000000000..cabd47612b0a
--- /dev/null
+++ b/drivers/usb/typec/ucsi/ucsi_acpi.c
@@ -0,0 +1,158 @@
+/*
+ * UCSI ACPI driver
+ *
+ * Copyright (C) 2017, Intel Corporation
+ * Author: Heikki Krogerus <heikki.krogerus@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/acpi.h>
+
+#include "ucsi.h"
+
+#define UCSI_DSM_UUID		"6f8398c2-7ca4-11e4-ad36-631042b5008f"
+#define UCSI_DSM_FUNC_WRITE	1
+#define UCSI_DSM_FUNC_READ	2
+
+struct ucsi_acpi {
+	struct device *dev;
+	struct ucsi *ucsi;
+	struct ucsi_ppm ppm;
+	guid_t guid;
+};
+
+static int ucsi_acpi_dsm(struct ucsi_acpi *ua, int func)
+{
+	union acpi_object *obj;
+
+	obj = acpi_evaluate_dsm(ACPI_HANDLE(ua->dev), &ua->guid, 1, func,
+				NULL);
+	if (!obj) {
+		dev_err(ua->dev, "%s: failed to evaluate _DSM %d\n",
+			__func__, func);
+		return -EIO;
+	}
+
+	ACPI_FREE(obj);
+	return 0;
+}
+
+static int ucsi_acpi_cmd(struct ucsi_ppm *ppm, struct ucsi_control *ctrl)
+{
+	struct ucsi_acpi *ua = container_of(ppm, struct ucsi_acpi, ppm);
+
+	ppm->data->ctrl.raw_cmd = ctrl->raw_cmd;
+
+	return ucsi_acpi_dsm(ua, UCSI_DSM_FUNC_WRITE);
+}
+
+static int ucsi_acpi_sync(struct ucsi_ppm *ppm)
+{
+	struct ucsi_acpi *ua = container_of(ppm, struct ucsi_acpi, ppm);
+
+	return ucsi_acpi_dsm(ua, UCSI_DSM_FUNC_READ);
+}
+
+static void ucsi_acpi_notify(acpi_handle handle, u32 event, void *data)
+{
+	struct ucsi_acpi *ua = data;
+
+	ucsi_notify(ua->ucsi);
+}
+
+static int ucsi_acpi_probe(struct platform_device *pdev)
+{
+	struct ucsi_acpi *ua;
+	struct resource *res;
+	acpi_status status;
+	int ret;
+
+	ua = devm_kzalloc(&pdev->dev, sizeof(*ua), GFP_KERNEL);
+	if (!ua)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "missing memory resource\n");
+		return -ENODEV;
+	}
+
+	/*
+	 * NOTE: The memory region for the data structures is used also in an
+	 * operation region, which means ACPI has already reserved it. Therefore
+	 * it can not be requested here, and we can not use
+	 * devm_ioremap_resource().
+	 */
+	ua->ppm.data = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (!ua->ppm.data)
+		return -ENOMEM;
+
+	if (!ua->ppm.data->version)
+		return -ENODEV;
+
+	ret = guid_parse(UCSI_DSM_UUID, &ua->guid);
+	if (ret)
+		return ret;
+
+	ua->ppm.cmd = ucsi_acpi_cmd;
+	ua->ppm.sync = ucsi_acpi_sync;
+	ua->dev = &pdev->dev;
+
+	status = acpi_install_notify_handler(ACPI_HANDLE(&pdev->dev),
+					     ACPI_DEVICE_NOTIFY,
+					     ucsi_acpi_notify, ua);
+	if (ACPI_FAILURE(status)) {
+		dev_err(&pdev->dev, "failed to install notify handler\n");
+		return -ENODEV;
+	}
+
+	ua->ucsi = ucsi_register_ppm(&pdev->dev, &ua->ppm);
+	if (IS_ERR(ua->ucsi)) {
+		acpi_remove_notify_handler(ACPI_HANDLE(&pdev->dev),
+					   ACPI_DEVICE_NOTIFY,
+					   ucsi_acpi_notify);
+		return PTR_ERR(ua->ucsi);
+	}
+
+	platform_set_drvdata(pdev, ua);
+
+	return 0;
+}
+
+static int ucsi_acpi_remove(struct platform_device *pdev)
+{
+	struct ucsi_acpi *ua = platform_get_drvdata(pdev);
+
+	ucsi_unregister_ppm(ua->ucsi);
+
+	acpi_remove_notify_handler(ACPI_HANDLE(&pdev->dev), ACPI_DEVICE_NOTIFY,
+				   ucsi_acpi_notify);
+
+	return 0;
+}
+
+static const struct acpi_device_id ucsi_acpi_match[] = {
+	{ "PNP0CA0", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, ucsi_acpi_match);
+
+static struct platform_driver ucsi_acpi_platform_driver = {
+	.driver = {
+		.name = "ucsi_acpi",
+		.acpi_match_table = ACPI_PTR(ucsi_acpi_match),
+	},
+	.probe = ucsi_acpi_probe,
+	.remove = ucsi_acpi_remove,
+};
+
+module_platform_driver(ucsi_acpi_platform_driver);
+
+MODULE_AUTHOR("Heikki Krogerus <heikki.krogerus@linux.intel.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("UCSI ACPI driver");
diff --git a/drivers/usb/usbip/stub_main.c b/drivers/usb/usbip/stub_main.c
index 44ab43fc4fcc..660180a5d5c4 100644
--- a/drivers/usb/usbip/stub_main.c
+++ b/drivers/usb/usbip/stub_main.c
@@ -134,7 +134,7 @@ out:
 	return ret;
 }
 
-static ssize_t show_match_busid(struct device_driver *drv, char *buf)
+static ssize_t match_busid_show(struct device_driver *drv, char *buf)
 {
 	int i;
 	char *out = buf;
@@ -149,7 +149,7 @@ static ssize_t show_match_busid(struct device_driver *drv, char *buf)
 	return out - buf;
 }
 
-static ssize_t store_match_busid(struct device_driver *dev, const char *buf,
+static ssize_t match_busid_store(struct device_driver *dev, const char *buf,
 				 size_t count)
 {
 	int len;
@@ -181,8 +181,7 @@ static ssize_t store_match_busid(struct device_driver *dev, const char *buf,
 
 	return -EINVAL;
 }
-static DRIVER_ATTR(match_busid, S_IRUSR | S_IWUSR, show_match_busid,
-		   store_match_busid);
+static DRIVER_ATTR_RW(match_busid);
 
 static ssize_t rebind_store(struct device_driver *dev, const char *buf,
 				 size_t count)
@@ -262,7 +261,11 @@ void stub_device_cleanup_urbs(struct stub_device *sdev)
 		kmem_cache_free(stub_priv_cache, priv);
 
 		kfree(urb->transfer_buffer);
+		urb->transfer_buffer = NULL;
+
 		kfree(urb->setup_packet);
+		urb->setup_packet = NULL;
+
 		usb_free_urb(urb);
 	}
 }
diff --git a/drivers/usb/usbip/stub_tx.c b/drivers/usb/usbip/stub_tx.c
index 6b1e8c3f0e4b..be50cef645d8 100644
--- a/drivers/usb/usbip/stub_tx.c
+++ b/drivers/usb/usbip/stub_tx.c
@@ -28,7 +28,11 @@ static void stub_free_priv_and_urb(struct stub_priv *priv)
 	struct urb *urb = priv->urb;
 
 	kfree(urb->setup_packet);
+	urb->setup_packet = NULL;
+
 	kfree(urb->transfer_buffer);
+	urb->transfer_buffer = NULL;
+
 	list_del(&priv->list);
 	kmem_cache_free(stub_priv_cache, priv);
 	usb_free_urb(urb);
diff --git a/drivers/usb/usbip/vhci.h b/drivers/usb/usbip/vhci.h
index 88b71c4e068f..5cfb59e98e44 100644
--- a/drivers/usb/usbip/vhci.h
+++ b/drivers/usb/usbip/vhci.h
@@ -72,6 +72,11 @@ struct vhci_unlink {
 	unsigned long unlink_seqnum;
 };
 
+enum hub_speed {
+	HUB_SPEED_HIGH = 0,
+	HUB_SPEED_SUPER,
+};
+
 /* Number of supported ports. Value has an upperbound of USB_MAXCHILDREN */
 #ifdef CONFIG_USBIP_VHCI_HC_PORTS
 #define VHCI_HC_PORTS CONFIG_USBIP_VHCI_HC_PORTS
@@ -79,6 +84,9 @@ struct vhci_unlink {
 #define VHCI_HC_PORTS 8
 #endif
 
+/* Each VHCI has 2 hubs (USB2 and USB3), each has VHCI_HC_PORTS ports */
+#define VHCI_PORTS	(VHCI_HC_PORTS*2)
+
 #ifdef CONFIG_USBIP_VHCI_NR_HCS
 #define VHCI_NR_HCS CONFIG_USBIP_VHCI_NR_HCS
 #else
@@ -87,10 +95,19 @@ struct vhci_unlink {
 
 #define MAX_STATUS_NAME 16
 
-/* for usb_bus.hcpriv */
-struct vhci_hcd {
+struct vhci {
 	spinlock_t lock;
 
+	struct platform_device *pdev;
+
+	struct vhci_hcd *vhci_hcd_hs;
+	struct vhci_hcd *vhci_hcd_ss;
+};
+
+/* for usb_hcd.hcd_priv[0] */
+struct vhci_hcd {
+	struct vhci *vhci;
+
 	u32 port_status[VHCI_HC_PORTS];
 
 	unsigned resuming:1;
@@ -107,7 +124,7 @@ struct vhci_hcd {
 };
 
 extern int vhci_num_controllers;
-extern struct platform_device **vhci_pdevs;
+extern struct vhci *vhcis;
 extern struct attribute_group vhci_attr_group;
 
 /* vhci_hcd.c */
@@ -131,10 +148,10 @@ static inline __u32 port_to_rhport(__u32 port)
 
 static inline int port_to_pdev_nr(__u32 port)
 {
-	return port / VHCI_HC_PORTS;
+	return port / VHCI_PORTS;
 }
 
-static inline struct vhci_hcd *hcd_to_vhci(struct usb_hcd *hcd)
+static inline struct vhci_hcd *hcd_to_vhci_hcd(struct usb_hcd *hcd)
 {
 	return (struct vhci_hcd *) (hcd->hcd_priv);
 }
@@ -149,15 +166,14 @@ static inline const char *hcd_name(struct usb_hcd *hcd)
 	return (hcd)->self.bus_name;
 }
 
-static inline struct usb_hcd *vhci_to_hcd(struct vhci_hcd *vhci)
+static inline struct usb_hcd *vhci_hcd_to_hcd(struct vhci_hcd *vhci_hcd)
 {
-	return container_of((void *) vhci, struct usb_hcd, hcd_priv);
+	return container_of((void *) vhci_hcd, struct usb_hcd, hcd_priv);
 }
 
-static inline struct vhci_hcd *vdev_to_vhci(struct vhci_device *vdev)
+static inline struct vhci_hcd *vdev_to_vhci_hcd(struct vhci_device *vdev)
 {
-	return container_of(
-			(void *)(vdev - vdev->rhport), struct vhci_hcd, vdev);
+	return container_of((void *)(vdev - vdev->rhport), struct vhci_hcd, vdev);
 }
 
 #endif /* __USBIP_VHCI_H */
diff --git a/drivers/usb/usbip/vhci_hcd.c b/drivers/usb/usbip/vhci_hcd.c
index 0585078638db..2c4b2fd40406 100644
--- a/drivers/usb/usbip/vhci_hcd.c
+++ b/drivers/usb/usbip/vhci_hcd.c
@@ -58,8 +58,7 @@ static const char driver_name[] = "vhci_hcd";
 static const char driver_desc[] = "USB/IP Virtual Host Controller";
 
 int vhci_num_controllers = VHCI_NR_HCS;
-
-struct platform_device **vhci_pdevs;
+struct vhci *vhcis;
 
 static const char * const bit_desc[] = {
 	"CONNECTION",		/*0*/
@@ -67,7 +66,7 @@ static const char * const bit_desc[] = {
 	"SUSPEND",		/*2*/
 	"OVER_CURRENT",		/*3*/
 	"RESET",		/*4*/
-	"R5",			/*5*/
+	"L1",			/*5*/
 	"R6",			/*6*/
 	"R7",			/*7*/
 	"POWER",		/*8*/
@@ -83,7 +82,7 @@ static const char * const bit_desc[] = {
 	"C_SUSPEND",		/*18*/
 	"C_OVER_CURRENT",	/*19*/
 	"C_RESET",		/*20*/
-	"R21",			/*21*/
+	"C_L1",			/*21*/
 	"R22",			/*22*/
 	"R23",			/*23*/
 	"R24",			/*24*/
@@ -96,10 +95,49 @@ static const char * const bit_desc[] = {
 	"R31",			/*31*/
 };
 
-static void dump_port_status_diff(u32 prev_status, u32 new_status)
+static const char * const bit_desc_ss[] = {
+	"CONNECTION",		/*0*/
+	"ENABLE",		/*1*/
+	"SUSPEND",		/*2*/
+	"OVER_CURRENT",		/*3*/
+	"RESET",		/*4*/
+	"L1",			/*5*/
+	"R6",			/*6*/
+	"R7",			/*7*/
+	"R8",			/*8*/
+	"POWER",		/*9*/
+	"HIGHSPEED",		/*10*/
+	"PORT_TEST",		/*11*/
+	"INDICATOR",		/*12*/
+	"R13",			/*13*/
+	"R14",			/*14*/
+	"R15",			/*15*/
+	"C_CONNECTION",		/*16*/
+	"C_ENABLE",		/*17*/
+	"C_SUSPEND",		/*18*/
+	"C_OVER_CURRENT",	/*19*/
+	"C_RESET",		/*20*/
+	"C_BH_RESET",		/*21*/
+	"C_LINK_STATE",		/*22*/
+	"C_CONFIG_ERROR",	/*23*/
+	"R24",			/*24*/
+	"R25",			/*25*/
+	"R26",			/*26*/
+	"R27",			/*27*/
+	"R28",			/*28*/
+	"R29",			/*29*/
+	"R30",			/*30*/
+	"R31",			/*31*/
+};
+
+static void dump_port_status_diff(u32 prev_status, u32 new_status, bool usb3)
 {
 	int i = 0;
 	u32 bit = 1;
+	const char * const *desc = bit_desc;
+
+	if (usb3)
+		desc = bit_desc_ss;
 
 	pr_debug("status prev -> new: %08x -> %08x\n", prev_status, new_status);
 	while (bit) {
@@ -114,8 +152,12 @@ static void dump_port_status_diff(u32 prev_status, u32 new_status)
 		else
 			change = ' ';
 
-		if (prev || new)
-			pr_debug(" %c%s\n", change, bit_desc[i]);
+		if (prev || new) {
+			pr_debug(" %c%s\n", change, desc[i]);
+
+			if (bit == 1) /* USB_PORT_STAT_CONNECTION */
+				pr_debug(" %c%s\n", change, "USB_PORT_STAT_SPEED_5GBPS");
+		}
 		bit <<= 1;
 		i++;
 	}
@@ -124,7 +166,8 @@ static void dump_port_status_diff(u32 prev_status, u32 new_status)
 
 void rh_port_connect(struct vhci_device *vdev, enum usb_device_speed speed)
 {
-	struct vhci_hcd	*vhci = vdev_to_vhci(vdev);
+	struct vhci_hcd	*vhci_hcd = vdev_to_vhci_hcd(vdev);
+	struct vhci *vhci = vhci_hcd->vhci;
 	int		rhport = vdev->rhport;
 	u32		status;
 	unsigned long	flags;
@@ -133,7 +176,7 @@ void rh_port_connect(struct vhci_device *vdev, enum usb_device_speed speed)
 
 	spin_lock_irqsave(&vhci->lock, flags);
 
-	status = vhci->port_status[rhport];
+	status = vhci_hcd->port_status[rhport];
 
 	status |= USB_PORT_STAT_CONNECTION | (1 << USB_PORT_FEAT_C_CONNECTION);
 
@@ -148,16 +191,17 @@ void rh_port_connect(struct vhci_device *vdev, enum usb_device_speed speed)
 		break;
 	}
 
-	vhci->port_status[rhport] = status;
+	vhci_hcd->port_status[rhport] = status;
 
 	spin_unlock_irqrestore(&vhci->lock, flags);
 
-	usb_hcd_poll_rh_status(vhci_to_hcd(vhci));
+	usb_hcd_poll_rh_status(vhci_hcd_to_hcd(vhci_hcd));
 }
 
 static void rh_port_disconnect(struct vhci_device *vdev)
 {
-	struct vhci_hcd	*vhci = vdev_to_vhci(vdev);
+	struct vhci_hcd	*vhci_hcd = vdev_to_vhci_hcd(vdev);
+	struct vhci *vhci = vhci_hcd->vhci;
 	int		rhport = vdev->rhport;
 	u32		status;
 	unsigned long	flags;
@@ -166,15 +210,15 @@ static void rh_port_disconnect(struct vhci_device *vdev)
 
 	spin_lock_irqsave(&vhci->lock, flags);
 
-	status = vhci->port_status[rhport];
+	status = vhci_hcd->port_status[rhport];
 
 	status &= ~USB_PORT_STAT_CONNECTION;
 	status |= (1 << USB_PORT_FEAT_C_CONNECTION);
 
-	vhci->port_status[rhport] = status;
+	vhci_hcd->port_status[rhport] = status;
 
 	spin_unlock_irqrestore(&vhci->lock, flags);
-	usb_hcd_poll_rh_status(vhci_to_hcd(vhci));
+	usb_hcd_poll_rh_status(vhci_hcd_to_hcd(vhci_hcd));
 }
 
 #define PORT_C_MASK				\
@@ -197,17 +241,15 @@ static void rh_port_disconnect(struct vhci_device *vdev)
  */
 static int vhci_hub_status(struct usb_hcd *hcd, char *buf)
 {
-	struct vhci_hcd	*vhci;
-	int		retval;
+	struct vhci_hcd	*vhci_hcd = hcd_to_vhci_hcd(hcd);
+	struct vhci *vhci = vhci_hcd->vhci;
+	int		retval = DIV_ROUND_UP(VHCI_HC_PORTS + 1, 8);
 	int		rhport;
 	int		changed = 0;
 	unsigned long	flags;
 
-	retval = DIV_ROUND_UP(VHCI_HC_PORTS + 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 not on?\n");
@@ -216,7 +258,7 @@ static int vhci_hub_status(struct usb_hcd *hcd, char *buf)
 
 	/* check pseudo status register for each port */
 	for (rhport = 0; rhport < VHCI_HC_PORTS; rhport++) {
-		if ((vhci->port_status[rhport] & PORT_C_MASK)) {
+		if ((vhci_hcd->port_status[rhport] & PORT_C_MASK)) {
 			/* The status of a port has been changed, */
 			usbip_dbg_vhci_rh("port %d status changed\n", rhport);
 
@@ -233,6 +275,40 @@ done:
 	return changed ? retval : 0;
 }
 
+/* usb 3.0 root hub device descriptor */
+static struct {
+	struct usb_bos_descriptor bos;
+	struct usb_ss_cap_descriptor ss_cap;
+} __packed usb3_bos_desc = {
+
+	.bos = {
+		.bLength		= USB_DT_BOS_SIZE,
+		.bDescriptorType	= USB_DT_BOS,
+		.wTotalLength		= cpu_to_le16(sizeof(usb3_bos_desc)),
+		.bNumDeviceCaps		= 1,
+	},
+	.ss_cap = {
+		.bLength		= USB_DT_USB_SS_CAP_SIZE,
+		.bDescriptorType	= USB_DT_DEVICE_CAPABILITY,
+		.bDevCapabilityType	= USB_SS_CAP_TYPE,
+		.wSpeedSupported	= cpu_to_le16(USB_5GBPS_OPERATION),
+		.bFunctionalitySupport	= ilog2(USB_5GBPS_OPERATION),
+	},
+};
+
+static inline void
+ss_hub_descriptor(struct usb_hub_descriptor *desc)
+{
+	memset(desc, 0, sizeof *desc);
+	desc->bDescriptorType = USB_DT_SS_HUB;
+	desc->bDescLength = 12;
+	desc->wHubCharacteristics = cpu_to_le16(
+		HUB_CHAR_INDV_PORT_LPSM | HUB_CHAR_COMMON_OCPM);
+	desc->bNbrPorts = VHCI_HC_PORTS;
+	desc->u.ss.bHubHdrDecLat = 0x04; /* Worst case: 0.4 micro sec*/
+	desc->u.ss.DeviceRemovable = 0xffff;
+}
+
 static inline void hub_descriptor(struct usb_hub_descriptor *desc)
 {
 	int width;
@@ -253,7 +329,8 @@ static inline void hub_descriptor(struct usb_hub_descriptor *desc)
 static int vhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 			    u16 wIndex, char *buf, u16 wLength)
 {
-	struct vhci_hcd	*dum;
+	struct vhci_hcd	*vhci_hcd;
+	struct vhci	*vhci;
 	int             retval = 0;
 	int		rhport;
 	unsigned long	flags;
@@ -265,21 +342,24 @@ static int vhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 
 	/*
 	 * NOTE:
-	 * wIndex shows the port number and begins from 1.
+	 * wIndex (bits 0-7) shows the port number and begins from 1?
 	 */
+	wIndex = ((__u8)(wIndex & 0x00ff));
 	usbip_dbg_vhci_rh("typeReq %x wValue %x wIndex %x\n", typeReq, wValue,
 			  wIndex);
+
 	if (wIndex > VHCI_HC_PORTS)
 		pr_err("invalid port number %d\n", wIndex);
-	rhport = ((__u8)(wIndex & 0x00ff)) - 1;
+	rhport = wIndex - 1;
 
-	dum = hcd_to_vhci(hcd);
+	vhci_hcd = hcd_to_vhci_hcd(hcd);
+	vhci = vhci_hcd->vhci;
 
-	spin_lock_irqsave(&dum->lock, flags);
+	spin_lock_irqsave(&vhci->lock, flags);
 
 	/* store old status and compare now and old later */
 	if (usbip_dbg_flag_vhci_rh) {
-		memcpy(prev_port_status, dum->port_status,
+		memcpy(prev_port_status, vhci_hcd->port_status,
 			sizeof(prev_port_status));
 	}
 
@@ -290,45 +370,56 @@ static int vhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 	case ClearPortFeature:
 		switch (wValue) {
 		case USB_PORT_FEAT_SUSPEND:
-			if (dum->port_status[rhport] & USB_PORT_STAT_SUSPEND) {
+			if (hcd->speed == HCD_USB3) {
+				pr_err(" ClearPortFeature: USB_PORT_FEAT_SUSPEND req not "
+				       "supported for USB 3.0 roothub\n");
+				goto error;
+			}
+			usbip_dbg_vhci_rh(
+				" ClearPortFeature: USB_PORT_FEAT_SUSPEND\n");
+			if (vhci_hcd->port_status[rhport] & USB_PORT_STAT_SUSPEND) {
 				/* 20msec signaling */
-				dum->resuming = 1;
-				dum->re_timeout =
-					jiffies + msecs_to_jiffies(20);
+				vhci_hcd->resuming = 1;
+				vhci_hcd->re_timeout = jiffies + msecs_to_jiffies(20);
 			}
 			break;
 		case USB_PORT_FEAT_POWER:
 			usbip_dbg_vhci_rh(
 				" ClearPortFeature: USB_PORT_FEAT_POWER\n");
-			dum->port_status[rhport] = 0;
-			dum->resuming = 0;
-			break;
-		case USB_PORT_FEAT_C_RESET:
-			usbip_dbg_vhci_rh(
-				" ClearPortFeature: USB_PORT_FEAT_C_RESET\n");
-			switch (dum->vdev[rhport].speed) {
-			case USB_SPEED_HIGH:
-				dum->port_status[rhport] |=
-					USB_PORT_STAT_HIGH_SPEED;
-				break;
-			case USB_SPEED_LOW:
-				dum->port_status[rhport] |=
-					USB_PORT_STAT_LOW_SPEED;
-				break;
-			default:
-				break;
-			}
+			if (hcd->speed == HCD_USB3)
+				vhci_hcd->port_status[rhport] &= ~USB_SS_PORT_STAT_POWER;
+			else
+				vhci_hcd->port_status[rhport] &= ~USB_PORT_STAT_POWER;
 			break;
 		default:
 			usbip_dbg_vhci_rh(" ClearPortFeature: default %x\n",
 					  wValue);
-			dum->port_status[rhport] &= ~(1 << wValue);
+			vhci_hcd->port_status[rhport] &= ~(1 << wValue);
 			break;
 		}
 		break;
 	case GetHubDescriptor:
 		usbip_dbg_vhci_rh(" GetHubDescriptor\n");
-		hub_descriptor((struct usb_hub_descriptor *) buf);
+		if (hcd->speed == HCD_USB3 &&
+				(wLength < USB_DT_SS_HUB_SIZE ||
+				 wValue != (USB_DT_SS_HUB << 8))) {
+			pr_err("Wrong hub descriptor type for USB 3.0 roothub.\n");
+			goto error;
+		}
+		if (hcd->speed == HCD_USB3)
+			ss_hub_descriptor((struct usb_hub_descriptor *) buf);
+		else
+			hub_descriptor((struct usb_hub_descriptor *) buf);
+		break;
+	case DeviceRequest | USB_REQ_GET_DESCRIPTOR:
+		if (hcd->speed != HCD_USB3)
+			goto error;
+
+		if ((wValue >> 8) != USB_DT_BOS)
+			goto error;
+
+		memcpy(buf, &usb3_bos_desc, sizeof(usb3_bos_desc));
+		retval = sizeof(usb3_bos_desc);
 		break;
 	case GetHubStatus:
 		usbip_dbg_vhci_rh(" GetHubStatus\n");
@@ -336,7 +427,7 @@ static int vhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 		break;
 	case GetPortStatus:
 		usbip_dbg_vhci_rh(" GetPortStatus port %x\n", wIndex);
-		if (wIndex > VHCI_HC_PORTS || wIndex < 1) {
+		if (wIndex < 1) {
 			pr_err("invalid port number %d\n", wIndex);
 			retval = -EPIPE;
 		}
@@ -346,36 +437,48 @@ static int vhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 		/* whoever resets or resumes must GetPortStatus to
 		 * complete it!!
 		 */
-		if (dum->resuming && time_after(jiffies, dum->re_timeout)) {
-			dum->port_status[rhport] |=
-				(1 << USB_PORT_FEAT_C_SUSPEND);
-			dum->port_status[rhport] &=
-				~(1 << USB_PORT_FEAT_SUSPEND);
-			dum->resuming = 0;
-			dum->re_timeout = 0;
+		if (vhci_hcd->resuming && time_after(jiffies, vhci_hcd->re_timeout)) {
+			vhci_hcd->port_status[rhport] |= (1 << USB_PORT_FEAT_C_SUSPEND);
+			vhci_hcd->port_status[rhport] &= ~(1 << USB_PORT_FEAT_SUSPEND);
+			vhci_hcd->resuming = 0;
+			vhci_hcd->re_timeout = 0;
 		}
 
-		if ((dum->port_status[rhport] & (1 << USB_PORT_FEAT_RESET)) !=
-		    0 && time_after(jiffies, dum->re_timeout)) {
-			dum->port_status[rhport] |=
-				(1 << USB_PORT_FEAT_C_RESET);
-			dum->port_status[rhport] &=
-				~(1 << USB_PORT_FEAT_RESET);
-			dum->re_timeout = 0;
+		if ((vhci_hcd->port_status[rhport] & (1 << USB_PORT_FEAT_RESET)) !=
+		    0 && time_after(jiffies, vhci_hcd->re_timeout)) {
+			vhci_hcd->port_status[rhport] |= (1 << USB_PORT_FEAT_C_RESET);
+			vhci_hcd->port_status[rhport] &= ~(1 << USB_PORT_FEAT_RESET);
+			vhci_hcd->re_timeout = 0;
 
-			if (dum->vdev[rhport].ud.status ==
+			if (vhci_hcd->vdev[rhport].ud.status ==
 			    VDEV_ST_NOTASSIGNED) {
 				usbip_dbg_vhci_rh(
 					" enable rhport %d (status %u)\n",
 					rhport,
-					dum->vdev[rhport].ud.status);
-				dum->port_status[rhport] |=
+					vhci_hcd->vdev[rhport].ud.status);
+				vhci_hcd->port_status[rhport] |=
 					USB_PORT_STAT_ENABLE;
 			}
+
+			if (hcd->speed < HCD_USB3) {
+				switch (vhci_hcd->vdev[rhport].speed) {
+				case USB_SPEED_HIGH:
+					vhci_hcd->port_status[rhport] |=
+					      USB_PORT_STAT_HIGH_SPEED;
+					break;
+				case USB_SPEED_LOW:
+					vhci_hcd->port_status[rhport] |=
+						USB_PORT_STAT_LOW_SPEED;
+					break;
+				default:
+					pr_err("vhci_device speed not set\n");
+					break;
+				}
+			}
 		}
-		((__le16 *) buf)[0] = cpu_to_le16(dum->port_status[rhport]);
+		((__le16 *) buf)[0] = cpu_to_le16(vhci_hcd->port_status[rhport]);
 		((__le16 *) buf)[1] =
-			cpu_to_le16(dum->port_status[rhport] >> 16);
+			cpu_to_le16(vhci_hcd->port_status[rhport] >> 16);
 
 		usbip_dbg_vhci_rh(" GetPortStatus bye %x %x\n", ((u16 *)buf)[0],
 				  ((u16 *)buf)[1]);
@@ -386,36 +489,119 @@ static int vhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 		break;
 	case SetPortFeature:
 		switch (wValue) {
+		case USB_PORT_FEAT_LINK_STATE:
+			usbip_dbg_vhci_rh(
+				" SetPortFeature: USB_PORT_FEAT_LINK_STATE\n");
+			if (hcd->speed != HCD_USB3) {
+				pr_err("USB_PORT_FEAT_LINK_STATE req not "
+				       "supported for USB 2.0 roothub\n");
+				goto error;
+			}
+			/*
+			 * Since this is dummy we don't have an actual link so
+			 * there is nothing to do for the SET_LINK_STATE cmd
+			 */
+			break;
+		case USB_PORT_FEAT_U1_TIMEOUT:
+			usbip_dbg_vhci_rh(
+				" SetPortFeature: USB_PORT_FEAT_U1_TIMEOUT\n");
+		case USB_PORT_FEAT_U2_TIMEOUT:
+			usbip_dbg_vhci_rh(
+				" SetPortFeature: USB_PORT_FEAT_U2_TIMEOUT\n");
+			/* TODO: add suspend/resume support! */
+			if (hcd->speed != HCD_USB3) {
+				pr_err("USB_PORT_FEAT_U1/2_TIMEOUT req not "
+				       "supported for USB 2.0 roothub\n");
+				goto error;
+			}
+			break;
 		case USB_PORT_FEAT_SUSPEND:
 			usbip_dbg_vhci_rh(
 				" SetPortFeature: USB_PORT_FEAT_SUSPEND\n");
+			/* Applicable only for USB2.0 hub */
+			if (hcd->speed == HCD_USB3) {
+				pr_err("USB_PORT_FEAT_SUSPEND req not "
+				       "supported for USB 3.0 roothub\n");
+				goto error;
+			}
+
+			vhci_hcd->port_status[rhport] |= USB_PORT_STAT_SUSPEND;
 			break;
+		case USB_PORT_FEAT_POWER:
+			usbip_dbg_vhci_rh(
+				" SetPortFeature: USB_PORT_FEAT_POWER\n");
+			if (hcd->speed == HCD_USB3)
+				vhci_hcd->port_status[rhport] |= USB_SS_PORT_STAT_POWER;
+			else
+				vhci_hcd->port_status[rhport] |= USB_PORT_STAT_POWER;
+			break;
+		case USB_PORT_FEAT_BH_PORT_RESET:
+			usbip_dbg_vhci_rh(
+				" SetPortFeature: USB_PORT_FEAT_BH_PORT_RESET\n");
+			/* Applicable only for USB3.0 hub */
+			if (hcd->speed != HCD_USB3) {
+				pr_err("USB_PORT_FEAT_BH_PORT_RESET req not "
+				       "supported for USB 2.0 roothub\n");
+				goto error;
+			}
+			/* FALLS THROUGH */
 		case USB_PORT_FEAT_RESET:
 			usbip_dbg_vhci_rh(
 				" SetPortFeature: USB_PORT_FEAT_RESET\n");
-			/* if it's already running, disconnect first */
-			if (dum->port_status[rhport] & USB_PORT_STAT_ENABLE) {
-				dum->port_status[rhport] &=
-					~(USB_PORT_STAT_ENABLE |
-					  USB_PORT_STAT_LOW_SPEED |
-					  USB_PORT_STAT_HIGH_SPEED);
-				/* FIXME test that code path! */
+			/* if it's already enabled, disable */
+			if (hcd->speed == HCD_USB3) {
+				vhci_hcd->port_status[rhport] = 0;
+				vhci_hcd->port_status[rhport] =
+					(USB_SS_PORT_STAT_POWER |
+					 USB_PORT_STAT_CONNECTION |
+					 USB_PORT_STAT_RESET);
+			} else if (vhci_hcd->port_status[rhport] & USB_PORT_STAT_ENABLE) {
+				vhci_hcd->port_status[rhport] &= ~(USB_PORT_STAT_ENABLE
+					| USB_PORT_STAT_LOW_SPEED
+					| USB_PORT_STAT_HIGH_SPEED);
 			}
+
 			/* 50msec reset signaling */
-			dum->re_timeout = jiffies + msecs_to_jiffies(50);
+			vhci_hcd->re_timeout = jiffies + msecs_to_jiffies(50);
 
-			/* FALLTHROUGH */
+			/* FALLS THROUGH */
 		default:
 			usbip_dbg_vhci_rh(" SetPortFeature: default %d\n",
 					  wValue);
-			dum->port_status[rhport] |= (1 << wValue);
-			break;
+			if (hcd->speed == HCD_USB3) {
+				if ((vhci_hcd->port_status[rhport] &
+				     USB_SS_PORT_STAT_POWER) != 0) {
+					vhci_hcd->port_status[rhport] |= (1 << wValue);
+				}
+			} else
+				if ((vhci_hcd->port_status[rhport] &
+				     USB_PORT_STAT_POWER) != 0) {
+					vhci_hcd->port_status[rhport] |= (1 << wValue);
+				}
+		}
+		break;
+	case GetPortErrorCount:
+		usbip_dbg_vhci_rh(" GetPortErrorCount\n");
+		if (hcd->speed != HCD_USB3) {
+			pr_err("GetPortErrorCount req not "
+			       "supported for USB 2.0 roothub\n");
+			goto error;
+		}
+		/* We'll always return 0 since this is a dummy hub */
+		*(__le32 *) buf = cpu_to_le32(0);
+		break;
+	case SetHubDepth:
+		usbip_dbg_vhci_rh(" SetHubDepth\n");
+		if (hcd->speed != HCD_USB3) {
+			pr_err("SetHubDepth req not supported for "
+			       "USB 2.0 roothub\n");
+			goto error;
 		}
 		break;
-
 	default:
-		pr_err("default: no such request\n");
-
+		pr_err("default hub control req: %04x v%04x i%04x l%d\n",
+			typeReq, wValue, wIndex, wLength);
+error:
 		/* "protocol stall" on error */
 		retval = -EPIPE;
 	}
@@ -425,12 +611,16 @@ static int vhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 		/* Only dump valid port status */
 		if (rhport >= 0) {
 			dump_port_status_diff(prev_port_status[rhport],
-					      dum->port_status[rhport]);
+					      vhci_hcd->port_status[rhport],
+					      hcd->speed == HCD_USB3);
 		}
 	}
 	usbip_dbg_vhci_rh(" bye\n");
 
-	spin_unlock_irqrestore(&dum->lock, flags);
+	spin_unlock_irqrestore(&vhci->lock, flags);
+
+	if ((vhci_hcd->port_status[rhport] & PORT_C_MASK) != 0)
+		usb_hcd_poll_rh_status(hcd);
 
 	return retval;
 }
@@ -438,14 +628,14 @@ static int vhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 static void vhci_tx_urb(struct urb *urb, struct vhci_device *vdev)
 {
 	struct vhci_priv *priv;
-	struct vhci_hcd *vhci;
+	struct vhci_hcd *vhci_hcd;
 	unsigned long flags;
 
 	if (!vdev) {
 		pr_err("could not get virtual device");
 		return;
 	}
-	vhci = vdev_to_vhci(vdev);
+	vhci_hcd = vdev_to_vhci_hcd(vdev);
 
 	priv = kzalloc(sizeof(struct vhci_priv), GFP_ATOMIC);
 	if (!priv) {
@@ -455,7 +645,7 @@ static void vhci_tx_urb(struct urb *urb, struct vhci_device *vdev)
 
 	spin_lock_irqsave(&vdev->priv_lock, flags);
 
-	priv->seqnum = atomic_inc_return(&vhci->seqnum);
+	priv->seqnum = atomic_inc_return(&vhci_hcd->seqnum);
 	if (priv->seqnum == 0xffff)
 		dev_info(&urb->dev->dev, "seqnum max\n");
 
@@ -470,10 +660,10 @@ static void vhci_tx_urb(struct urb *urb, struct vhci_device *vdev)
 	spin_unlock_irqrestore(&vdev->priv_lock, flags);
 }
 
-static int vhci_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
-			    gfp_t mem_flags)
+static int vhci_urb_enqueue(struct usb_hcd *hcd, struct urb *urb, gfp_t mem_flags)
 {
-	struct vhci_hcd *vhci = hcd_to_vhci(hcd);
+	struct vhci_hcd *vhci_hcd = hcd_to_vhci_hcd(hcd);
+	struct vhci *vhci = vhci_hcd->vhci;
 	struct device *dev = &urb->dev->dev;
 	u8 portnum = urb->dev->portnum;
 	int ret = 0;
@@ -487,7 +677,7 @@ static int vhci_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
 		pr_err("invalid port number %d\n", portnum);
 		return -ENODEV;
 	}
-	vdev = &vhci->vdev[portnum-1];
+	vdev = &vhci_hcd->vdev[portnum-1];
 
 	/* patch to usb_sg_init() is in 2.5.60 */
 	BUG_ON(!urb->transfer_buffer && urb->transfer_buffer_length);
@@ -640,7 +830,8 @@ no_need_unlink:
  */
 static int vhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 {
-	struct vhci_hcd *vhci = hcd_to_vhci(hcd);
+	struct vhci_hcd *vhci_hcd = hcd_to_vhci_hcd(hcd);
+	struct vhci *vhci = vhci_hcd->vhci;
 	struct vhci_priv *priv;
 	struct vhci_device *vdev;
 	unsigned long flags;
@@ -691,7 +882,7 @@ static int vhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 		usb_hcd_unlink_urb_from_ep(hcd, urb);
 
 		spin_unlock_irqrestore(&vhci->lock, flags);
-		usb_hcd_giveback_urb(vhci_to_hcd(vhci), urb, urb->status);
+		usb_hcd_giveback_urb(hcd, urb, urb->status);
 		spin_lock_irqsave(&vhci->lock, flags);
 
 	} else {
@@ -709,7 +900,7 @@ static int vhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 			return -ENOMEM;
 		}
 
-		unlink->seqnum = atomic_inc_return(&vhci->seqnum);
+		unlink->seqnum = atomic_inc_return(&vhci_hcd->seqnum);
 		if (unlink->seqnum == 0xffff)
 			pr_info("seqnum max\n");
 
@@ -733,8 +924,9 @@ static int vhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 
 static void vhci_device_unlink_cleanup(struct vhci_device *vdev)
 {
-	struct vhci_hcd *vhci = vdev_to_vhci(vdev);
-	struct usb_hcd *hcd = vhci_to_hcd(vhci);
+	struct vhci_hcd *vhci_hcd = vdev_to_vhci_hcd(vdev);
+	struct usb_hcd *hcd = vhci_hcd_to_hcd(vhci_hcd);
+	struct vhci *vhci = vhci_hcd->vhci;
 	struct vhci_unlink *unlink, *tmp;
 	unsigned long flags;
 
@@ -846,7 +1038,6 @@ static void vhci_shutdown_connection(struct usbip_device *ud)
 	pr_info("disconnect device\n");
 }
 
-
 static void vhci_device_reset(struct usbip_device *ud)
 {
 	struct vhci_device *vdev = container_of(ud, struct vhci_device, ud);
@@ -918,29 +1109,58 @@ static int hcd_name_to_id(const char *name)
 	return val;
 }
 
+static int vhci_setup(struct usb_hcd *hcd)
+{
+	struct vhci *vhci = *((void **)dev_get_platdata(hcd->self.controller));
+	hcd->self.sg_tablesize = ~0;
+	if (usb_hcd_is_primary_hcd(hcd)) {
+		vhci->vhci_hcd_hs = hcd_to_vhci_hcd(hcd);
+		vhci->vhci_hcd_hs->vhci = vhci;
+		/*
+		 * Mark the first roothub as being USB 2.0.
+		 * The USB 3.0 roothub will be registered later by
+		 * vhci_hcd_probe()
+		 */
+		hcd->speed = HCD_USB2;
+		hcd->self.root_hub->speed = USB_SPEED_HIGH;
+	} else {
+		vhci->vhci_hcd_ss = hcd_to_vhci_hcd(hcd);
+		vhci->vhci_hcd_ss->vhci = vhci;
+		hcd->speed = HCD_USB3;
+		hcd->self.root_hub->speed = USB_SPEED_SUPER;
+	}
+	return 0;
+}
+
 static int vhci_start(struct usb_hcd *hcd)
 {
-	struct vhci_hcd *vhci = hcd_to_vhci(hcd);
+	struct vhci_hcd *vhci_hcd = hcd_to_vhci_hcd(hcd);
 	int id, rhport;
-	int err = 0;
+	int err;
 
 	usbip_dbg_vhci_hc("enter vhci_start\n");
 
+	if (usb_hcd_is_primary_hcd(hcd))
+		spin_lock_init(&vhci_hcd->vhci->lock);
+
 	/* initialize private data of usb_hcd */
 
 	for (rhport = 0; rhport < VHCI_HC_PORTS; rhport++) {
-		struct vhci_device *vdev = &vhci->vdev[rhport];
+		struct vhci_device *vdev = &vhci_hcd->vdev[rhport];
 
 		vhci_device_init(vdev);
 		vdev->rhport = rhport;
 	}
 
-	atomic_set(&vhci->seqnum, 0);
-	spin_lock_init(&vhci->lock);
+	atomic_set(&vhci_hcd->seqnum, 0);
 
 	hcd->power_budget = 0; /* no limit */
 	hcd->uses_new_polling = 1;
 
+#ifdef CONFIG_USB_OTG
+	hcd->self.otg_port = 1;
+#endif
+
 	id = hcd_name_to_id(hcd_name(hcd));
 	if (id < 0) {
 		pr_err("invalid vhci name %s\n", hcd_name(hcd));
@@ -948,7 +1168,7 @@ static int vhci_start(struct usb_hcd *hcd)
 	}
 
 	/* vhci_hcd is now ready to be controlled through sysfs */
-	if (id == 0) {
+	if (id == 0 && usb_hcd_is_primary_hcd(hcd)) {
 		err = vhci_init_attr_group();
 		if (err) {
 			pr_err("init attr group\n");
@@ -968,21 +1188,21 @@ static int vhci_start(struct usb_hcd *hcd)
 
 static void vhci_stop(struct usb_hcd *hcd)
 {
-	struct vhci_hcd *vhci = hcd_to_vhci(hcd);
+	struct vhci_hcd *vhci_hcd = hcd_to_vhci_hcd(hcd);
 	int id, rhport;
 
 	usbip_dbg_vhci_hc("stop VHCI controller\n");
 
 	/* 1. remove the userland interface of vhci_hcd */
 	id = hcd_name_to_id(hcd_name(hcd));
-	if (id == 0) {
+	if (id == 0 && usb_hcd_is_primary_hcd(hcd)) {
 		sysfs_remove_group(&hcd_dev(hcd)->kobj, &vhci_attr_group);
 		vhci_finish_attr_group();
 	}
 
 	/* 2. shutdown all the ports of vhci_hcd */
 	for (rhport = 0; rhport < VHCI_HC_PORTS; rhport++) {
-		struct vhci_device *vdev = &vhci->vdev[rhport];
+		struct vhci_device *vdev = &vhci_hcd->vdev[rhport];
 
 		usbip_event_add(&vdev->ud, VDEV_EVENT_REMOVED);
 		usbip_stop_eh(&vdev->ud);
@@ -1000,7 +1220,7 @@ static int vhci_get_frame_number(struct usb_hcd *hcd)
 /* FIXME: suspend/resume */
 static int vhci_bus_suspend(struct usb_hcd *hcd)
 {
-	struct vhci_hcd *vhci = hcd_to_vhci(hcd);
+	struct vhci *vhci = *((void **)dev_get_platdata(hcd->self.controller));
 	unsigned long flags;
 
 	dev_dbg(&hcd->self.root_hub->dev, "%s\n", __func__);
@@ -1014,7 +1234,7 @@ static int vhci_bus_suspend(struct usb_hcd *hcd)
 
 static int vhci_bus_resume(struct usb_hcd *hcd)
 {
-	struct vhci_hcd *vhci = hcd_to_vhci(hcd);
+	struct vhci *vhci = *((void **)dev_get_platdata(hcd->self.controller));
 	int rc = 0;
 	unsigned long flags;
 
@@ -1036,13 +1256,32 @@ static int vhci_bus_resume(struct usb_hcd *hcd)
 #define vhci_bus_resume       NULL
 #endif
 
+/* Change a group of bulk endpoints to support multiple stream IDs */
+static int vhci_alloc_streams(struct usb_hcd *hcd, struct usb_device *udev,
+	struct usb_host_endpoint **eps, unsigned int num_eps,
+	unsigned int num_streams, gfp_t mem_flags)
+{
+	dev_dbg(&hcd->self.root_hub->dev, "vhci_alloc_streams not implemented\n");
+	return 0;
+}
+
+/* Reverts a group of bulk endpoints back to not using stream IDs. */
+static int vhci_free_streams(struct usb_hcd *hcd, struct usb_device *udev,
+	struct usb_host_endpoint **eps, unsigned int num_eps,
+	gfp_t mem_flags)
+{
+	dev_dbg(&hcd->self.root_hub->dev, "vhci_free_streams not implemented\n");
+	return 0;
+}
+
 static struct hc_driver vhci_hc_driver = {
 	.description	= driver_name,
 	.product_desc	= driver_desc,
 	.hcd_priv_size	= sizeof(struct vhci_hcd),
 
-	.flags		= HCD_USB2,
+	.flags		= HCD_USB3 | HCD_SHARED,
 
+	.reset		= vhci_setup,
 	.start		= vhci_start,
 	.stop		= vhci_stop,
 
@@ -1055,11 +1294,16 @@ static struct hc_driver vhci_hc_driver = {
 	.hub_control    = vhci_hub_control,
 	.bus_suspend	= vhci_bus_suspend,
 	.bus_resume	= vhci_bus_resume,
+
+	.alloc_streams	= vhci_alloc_streams,
+	.free_streams	= vhci_free_streams,
 };
 
 static int vhci_hcd_probe(struct platform_device *pdev)
 {
-	struct usb_hcd		*hcd;
+	struct vhci             *vhci = *((void **)dev_get_platdata(&pdev->dev));
+	struct usb_hcd		*hcd_hs;
+	struct usb_hcd		*hcd_ss;
 	int			ret;
 
 	usbip_dbg_vhci_hc("name %s id %d\n", pdev->name, pdev->id);
@@ -1068,43 +1312,68 @@ static int vhci_hcd_probe(struct platform_device *pdev)
 	 * Allocate and initialize hcd.
 	 * Our private data is also allocated automatically.
 	 */
-	hcd = usb_create_hcd(&vhci_hc_driver, &pdev->dev, dev_name(&pdev->dev));
-	if (!hcd) {
-		pr_err("create hcd failed\n");
+	hcd_hs = usb_create_hcd(&vhci_hc_driver, &pdev->dev, dev_name(&pdev->dev));
+	if (!hcd_hs) {
+		pr_err("create primary hcd failed\n");
 		return -ENOMEM;
 	}
-	hcd->has_tt = 1;
+	hcd_hs->has_tt = 1;
 
 	/*
 	 * Finish generic HCD structure initialization and register.
 	 * Call the driver's reset() and start() routines.
 	 */
-	ret = usb_add_hcd(hcd, 0, 0);
+	ret = usb_add_hcd(hcd_hs, 0, 0);
 	if (ret != 0) {
-		pr_err("usb_add_hcd failed %d\n", ret);
-		usb_put_hcd(hcd);
-		return ret;
+		pr_err("usb_add_hcd hs failed %d\n", ret);
+		goto put_usb2_hcd;
+	}
+
+	hcd_ss = usb_create_shared_hcd(&vhci_hc_driver, &pdev->dev,
+				       dev_name(&pdev->dev), hcd_hs);
+	if (!hcd_ss) {
+		ret = -ENOMEM;
+		pr_err("create shared hcd failed\n");
+		goto remove_usb2_hcd;
+	}
+
+	ret = usb_add_hcd(hcd_ss, 0, 0);
+	if (ret) {
+		pr_err("usb_add_hcd ss failed %d\n", ret);
+		goto put_usb3_hcd;
 	}
 
 	usbip_dbg_vhci_hc("bye\n");
 	return 0;
+
+put_usb3_hcd:
+	usb_put_hcd(hcd_ss);
+remove_usb2_hcd:
+	usb_remove_hcd(hcd_hs);
+put_usb2_hcd:
+	usb_put_hcd(hcd_hs);
+	vhci->vhci_hcd_hs = NULL;
+	vhci->vhci_hcd_ss = NULL;
+	return ret;
 }
 
 static int vhci_hcd_remove(struct platform_device *pdev)
 {
-	struct usb_hcd	*hcd;
-
-	hcd = platform_get_drvdata(pdev);
-	if (!hcd)
-		return 0;
+	struct vhci *vhci = *((void **)dev_get_platdata(&pdev->dev));
 
 	/*
 	 * Disconnects the root hub,
 	 * then reverses the effects of usb_add_hcd(),
 	 * invoking the HCD's stop() methods.
 	 */
-	usb_remove_hcd(hcd);
-	usb_put_hcd(hcd);
+	usb_remove_hcd(vhci_hcd_to_hcd(vhci->vhci_hcd_ss));
+	usb_put_hcd(vhci_hcd_to_hcd(vhci->vhci_hcd_ss));
+
+	usb_remove_hcd(vhci_hcd_to_hcd(vhci->vhci_hcd_hs));
+	usb_put_hcd(vhci_hcd_to_hcd(vhci->vhci_hcd_hs));
+
+	vhci->vhci_hcd_hs = NULL;
+	vhci->vhci_hcd_ss = NULL;
 
 	return 0;
 }
@@ -1115,23 +1384,32 @@ static int vhci_hcd_remove(struct platform_device *pdev)
 static int vhci_hcd_suspend(struct platform_device *pdev, pm_message_t state)
 {
 	struct usb_hcd *hcd;
-	struct vhci_hcd *vhci;
+	struct vhci *vhci;
 	int rhport;
 	int connected = 0;
 	int ret = 0;
 	unsigned long flags;
 
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
 	hcd = platform_get_drvdata(pdev);
 	if (!hcd)
 		return 0;
-	vhci = hcd_to_vhci(hcd);
+
+	vhci = *((void **)dev_get_platdata(hcd->self.controller));
 
 	spin_lock_irqsave(&vhci->lock, flags);
 
-	for (rhport = 0; rhport < VHCI_HC_PORTS; rhport++)
-		if (vhci->port_status[rhport] & USB_PORT_STAT_CONNECTION)
+	for (rhport = 0; rhport < VHCI_HC_PORTS; rhport++) {
+		if (vhci->vhci_hcd_hs->port_status[rhport] &
+		    USB_PORT_STAT_CONNECTION)
 			connected += 1;
 
+		if (vhci->vhci_hcd_ss->port_status[rhport] &
+		    USB_PORT_STAT_CONNECTION)
+			connected += 1;
+	}
+
 	spin_unlock_irqrestore(&vhci->lock, flags);
 
 	if (connected > 0) {
@@ -1179,34 +1457,16 @@ static struct platform_driver vhci_driver = {
 	},
 };
 
-static int add_platform_device(int id)
-{
-	struct platform_device *pdev;
-	int dev_nr;
-
-	if (id == 0)
-		dev_nr = -1;
-	else
-		dev_nr = id;
-
-	pdev = platform_device_register_simple(driver_name, dev_nr, NULL, 0);
-	if (IS_ERR(pdev))
-		return PTR_ERR(pdev);
-
-	*(vhci_pdevs + id) = pdev;
-	return 0;
-}
-
 static void del_platform_devices(void)
 {
 	struct platform_device *pdev;
 	int i;
 
 	for (i = 0; i < vhci_num_controllers; i++) {
-		pdev = *(vhci_pdevs + i);
+		pdev = vhcis[i].pdev;
 		if (pdev != NULL)
 			platform_device_unregister(pdev);
-		*(vhci_pdevs + i) = NULL;
+		vhcis[i].pdev = NULL;
 	}
 	sysfs_remove_link(&platform_bus.kobj, driver_name);
 }
@@ -1221,28 +1481,51 @@ static int __init vhci_hcd_init(void)
 	if (vhci_num_controllers < 1)
 		vhci_num_controllers = 1;
 
-	vhci_pdevs = kcalloc(vhci_num_controllers, sizeof(void *), GFP_KERNEL);
-	if (vhci_pdevs == NULL)
+	vhcis = kcalloc(vhci_num_controllers, sizeof(struct vhci), GFP_KERNEL);
+	if (vhcis == NULL)
 		return -ENOMEM;
 
+	for (i = 0; i < vhci_num_controllers; i++) {
+		vhcis[i].pdev = platform_device_alloc(driver_name, i);
+		if (!vhcis[i].pdev) {
+			i--;
+			while (i >= 0)
+				platform_device_put(vhcis[i--].pdev);
+			ret = -ENOMEM;
+			goto err_device_alloc;
+		}
+	}
+	for (i = 0; i < vhci_num_controllers; i++) {
+		void *vhci = &vhcis[i];
+		ret = platform_device_add_data(vhcis[i].pdev, &vhci, sizeof(void *));
+		if (ret)
+			goto err_driver_register;
+	}
+
 	ret = platform_driver_register(&vhci_driver);
 	if (ret)
 		goto err_driver_register;
 
 	for (i = 0; i < vhci_num_controllers; i++) {
-		ret = add_platform_device(i);
-		if (ret)
-			goto err_platform_device_register;
+		ret = platform_device_add(vhcis[i].pdev);
+		if (ret < 0) {
+			i--;
+			while (i >= 0)
+				platform_device_del(vhcis[i--].pdev);
+			goto err_add_hcd;
+		}
 	}
 
 	pr_info(DRIVER_DESC " v" USBIP_VERSION "\n");
 	return ret;
 
-err_platform_device_register:
-	del_platform_devices();
+err_add_hcd:
 	platform_driver_unregister(&vhci_driver);
 err_driver_register:
-	kfree(vhci_pdevs);
+	for (i = 0; i < vhci_num_controllers; i++)
+		platform_device_put(vhcis[i].pdev);
+err_device_alloc:
+	kfree(vhcis);
 	return ret;
 }
 
@@ -1250,7 +1533,7 @@ static void __exit vhci_hcd_exit(void)
 {
 	del_platform_devices();
 	platform_driver_unregister(&vhci_driver);
-	kfree(vhci_pdevs);
+	kfree(vhcis);
 }
 
 module_init(vhci_hcd_init);
diff --git a/drivers/usb/usbip/vhci_rx.c b/drivers/usb/usbip/vhci_rx.c
index fc2d319e2360..ef2f2d5ca6b2 100644
--- a/drivers/usb/usbip/vhci_rx.c
+++ b/drivers/usb/usbip/vhci_rx.c
@@ -70,7 +70,8 @@ struct urb *pickup_urb_and_free_priv(struct vhci_device *vdev, __u32 seqnum)
 static void vhci_recv_ret_submit(struct vhci_device *vdev,
 				 struct usbip_header *pdu)
 {
-	struct vhci_hcd *vhci = vdev_to_vhci(vdev);
+	struct vhci_hcd *vhci_hcd = vdev_to_vhci_hcd(vdev);
+	struct vhci *vhci = vhci_hcd->vhci;
 	struct usbip_device *ud = &vdev->ud;
 	struct urb *urb;
 	unsigned long flags;
@@ -82,7 +83,7 @@ static void vhci_recv_ret_submit(struct vhci_device *vdev,
 	if (!urb) {
 		pr_err("cannot find a urb of seqnum %u\n", pdu->base.seqnum);
 		pr_info("max seqnum %d\n",
-			atomic_read(&vhci->seqnum));
+			atomic_read(&vhci_hcd->seqnum));
 		usbip_event_add(ud, VDEV_EVENT_ERROR_TCP);
 		return;
 	}
@@ -107,10 +108,10 @@ static void vhci_recv_ret_submit(struct vhci_device *vdev,
 	usbip_dbg_vhci_rx("now giveback urb %p\n", urb);
 
 	spin_lock_irqsave(&vhci->lock, flags);
-	usb_hcd_unlink_urb_from_ep(vhci_to_hcd(vhci), urb);
+	usb_hcd_unlink_urb_from_ep(vhci_hcd_to_hcd(vhci_hcd), urb);
 	spin_unlock_irqrestore(&vhci->lock, flags);
 
-	usb_hcd_giveback_urb(vhci_to_hcd(vhci), urb, urb->status);
+	usb_hcd_giveback_urb(vhci_hcd_to_hcd(vhci_hcd), urb, urb->status);
 
 	usbip_dbg_vhci_rx("Leave\n");
 }
@@ -143,7 +144,8 @@ static struct vhci_unlink *dequeue_pending_unlink(struct vhci_device *vdev,
 static void vhci_recv_ret_unlink(struct vhci_device *vdev,
 				 struct usbip_header *pdu)
 {
-	struct vhci_hcd *vhci = vdev_to_vhci(vdev);
+	struct vhci_hcd *vhci_hcd = vdev_to_vhci_hcd(vdev);
+	struct vhci *vhci = vhci_hcd->vhci;
 	struct vhci_unlink *unlink;
 	struct urb *urb;
 	unsigned long flags;
@@ -177,10 +179,10 @@ static void vhci_recv_ret_unlink(struct vhci_device *vdev,
 		pr_info("urb->status %d\n", urb->status);
 
 		spin_lock_irqsave(&vhci->lock, flags);
-		usb_hcd_unlink_urb_from_ep(vhci_to_hcd(vhci), urb);
+		usb_hcd_unlink_urb_from_ep(vhci_hcd_to_hcd(vhci_hcd), urb);
 		spin_unlock_irqrestore(&vhci->lock, flags);
 
-		usb_hcd_giveback_urb(vhci_to_hcd(vhci), urb, urb->status);
+		usb_hcd_giveback_urb(vhci_hcd_to_hcd(vhci_hcd), urb, urb->status);
 	}
 
 	kfree(unlink);
diff --git a/drivers/usb/usbip/vhci_sysfs.c b/drivers/usb/usbip/vhci_sysfs.c
index b96e5b189269..5778b640ba9c 100644
--- a/drivers/usb/usbip/vhci_sysfs.c
+++ b/drivers/usb/usbip/vhci_sysfs.c
@@ -29,13 +29,51 @@
 
 /* TODO: refine locking ?*/
 
+/*
+ * output example:
+ * hub port sta spd dev      socket           local_busid
+ * hs  0000 004 000 00000000         c5a7bb80 1-2.3
+ * ................................................
+ * ss  0008 004 000 00000000         d8cee980 2-3.4
+ * ................................................
+ *
+ * IP address can be retrieved from a socket pointer address by looking
+ * up /proc/net/{tcp,tcp6}. Also, a userland program may remember a
+ * port number and its peer IP address.
+ */
+static void port_show_vhci(char **out, int hub, int port, struct vhci_device *vdev)
+{
+	if (hub == HUB_SPEED_HIGH)
+		*out += sprintf(*out, "hs  %04u %03u ",
+				      port, vdev->ud.status);
+	else /* hub == HUB_SPEED_SUPER */
+		*out += sprintf(*out, "ss  %04u %03u ",
+				      port, vdev->ud.status);
+
+	if (vdev->ud.status == VDEV_ST_USED) {
+		*out += sprintf(*out, "%03u %08x ",
+				      vdev->speed, vdev->devid);
+		*out += sprintf(*out, "%16p %s",
+				      vdev->ud.tcp_socket,
+				      dev_name(&vdev->udev->dev));
+
+	} else {
+		*out += sprintf(*out, "000 00000000 ");
+		*out += sprintf(*out, "0000000000000000 0-0");
+	}
+
+	*out += sprintf(*out, "\n");
+}
+
 /* Sysfs entry to show port status */
 static ssize_t status_show_vhci(int pdev_nr, char *out)
 {
-	struct platform_device *pdev = *(vhci_pdevs + pdev_nr);
-	struct vhci_hcd *vhci;
+	struct platform_device *pdev = vhcis[pdev_nr].pdev;
+	struct vhci *vhci;
+	struct usb_hcd *hcd;
+	struct vhci_hcd *vhci_hcd;
 	char *s = out;
-	int i = 0;
+	int i;
 	unsigned long flags;
 
 	if (!pdev || !out) {
@@ -43,41 +81,27 @@ static ssize_t status_show_vhci(int pdev_nr, char *out)
 		return 0;
 	}
 
-	vhci = hcd_to_vhci(platform_get_drvdata(pdev));
+	hcd = platform_get_drvdata(pdev);
+	vhci_hcd = hcd_to_vhci_hcd(hcd);
+	vhci = vhci_hcd->vhci;
 
 	spin_lock_irqsave(&vhci->lock, flags);
 
-	/*
-	 * output example:
-	 * port sta spd dev      socket           local_busid
-	 * 0000 004 000 00000000         c5a7bb80 1-2.3
-	 * 0001 004 000 00000000         d8cee980 2-3.4
-	 *
-	 * IP address can be retrieved from a socket pointer address by looking
-	 * up /proc/net/{tcp,tcp6}. Also, a userland program may remember a
-	 * port number and its peer IP address.
-	 */
 	for (i = 0; i < VHCI_HC_PORTS; i++) {
-		struct vhci_device *vdev = &vhci->vdev[i];
+		struct vhci_device *vdev = &vhci->vhci_hcd_hs->vdev[i];
 
 		spin_lock(&vdev->ud.lock);
-		out += sprintf(out, "%04u %03u ",
-				    (pdev_nr * VHCI_HC_PORTS) + i,
-				    vdev->ud.status);
-
-		if (vdev->ud.status == VDEV_ST_USED) {
-			out += sprintf(out, "%03u %08x ",
-					    vdev->speed, vdev->devid);
-			out += sprintf(out, "%16p %s",
-					    vdev->ud.tcp_socket,
-					    dev_name(&vdev->udev->dev));
-
-		} else {
-			out += sprintf(out, "000 00000000 ");
-			out += sprintf(out, "0000000000000000 0-0");
-		}
+		port_show_vhci(&out, HUB_SPEED_HIGH,
+			       pdev_nr * VHCI_PORTS + i, vdev);
+		spin_unlock(&vdev->ud.lock);
+	}
 
-		out += sprintf(out, "\n");
+	for (i = 0; i < VHCI_HC_PORTS; i++) {
+		struct vhci_device *vdev = &vhci->vhci_hcd_ss->vdev[i];
+
+		spin_lock(&vdev->ud.lock);
+		port_show_vhci(&out, HUB_SPEED_SUPER,
+			       pdev_nr * VHCI_PORTS + VHCI_HC_PORTS + i, vdev);
 		spin_unlock(&vdev->ud.lock);
 	}
 
@@ -92,8 +116,16 @@ static ssize_t status_show_not_ready(int pdev_nr, char *out)
 	int i = 0;
 
 	for (i = 0; i < VHCI_HC_PORTS; i++) {
-		out += sprintf(out, "%04u %03u ",
-				    (pdev_nr * VHCI_HC_PORTS) + i,
+		out += sprintf(out, "hs  %04u %03u ",
+				    (pdev_nr * VHCI_PORTS) + i,
+				    VDEV_ST_NOTASSIGNED);
+		out += sprintf(out, "000 00000000 0000000000000000 0-0");
+		out += sprintf(out, "\n");
+	}
+
+	for (i = 0; i < VHCI_HC_PORTS; i++) {
+		out += sprintf(out, "ss  %04u %03u ",
+				    (pdev_nr * VHCI_PORTS) + VHCI_HC_PORTS + i,
 				    VDEV_ST_NOTASSIGNED);
 		out += sprintf(out, "000 00000000 0000000000000000 0-0");
 		out += sprintf(out, "\n");
@@ -125,7 +157,7 @@ static ssize_t status_show(struct device *dev,
 	int pdev_nr;
 
 	out += sprintf(out,
-		       "port sta spd dev      socket           local_busid\n");
+		       "hub port sta spd dev      socket           local_busid\n");
 
 	pdev_nr = status_name_to_id(attr->attr.name);
 	if (pdev_nr < 0)
@@ -141,15 +173,19 @@ static ssize_t nports_show(struct device *dev, struct device_attribute *attr,
 {
 	char *s = out;
 
-	out += sprintf(out, "%d\n", VHCI_HC_PORTS * vhci_num_controllers);
+	/*
+	 * Half the ports are for SPEED_HIGH and half for SPEED_SUPER, thus the * 2.
+	 */
+	out += sprintf(out, "%d\n", VHCI_PORTS * vhci_num_controllers);
 	return out - s;
 }
 static DEVICE_ATTR_RO(nports);
 
 /* Sysfs entry to shutdown a virtual connection */
-static int vhci_port_disconnect(struct vhci_hcd *vhci, __u32 rhport)
+static int vhci_port_disconnect(struct vhci_hcd *vhci_hcd, __u32 rhport)
 {
-	struct vhci_device *vdev = &vhci->vdev[rhport];
+	struct vhci_device *vdev = &vhci_hcd->vdev[rhport];
+	struct vhci *vhci = vhci_hcd->vhci;
 	unsigned long flags;
 
 	usbip_dbg_vhci_sysfs("enter\n");
@@ -195,6 +231,7 @@ static ssize_t store_detach(struct device *dev, struct device_attribute *attr,
 {
 	__u32 port = 0, pdev_nr = 0, rhport = 0;
 	struct usb_hcd *hcd;
+	struct vhci_hcd *vhci_hcd;
 	int ret;
 
 	if (kstrtoint(buf, 10, &port) < 0)
@@ -206,13 +243,20 @@ static ssize_t store_detach(struct device *dev, struct device_attribute *attr,
 	if (!valid_port(pdev_nr, rhport))
 		return -EINVAL;
 
-	hcd = platform_get_drvdata(*(vhci_pdevs + pdev_nr));
+	hcd = platform_get_drvdata(vhcis[pdev_nr].pdev);
 	if (hcd == NULL) {
 		dev_err(dev, "port is not ready %u\n", port);
 		return -EAGAIN;
 	}
 
-	ret = vhci_port_disconnect(hcd_to_vhci(hcd), rhport);
+	usbip_dbg_vhci_sysfs("rhport %d\n", rhport);
+
+	if ((port / VHCI_HC_PORTS) % 2)
+		vhci_hcd = hcd_to_vhci_hcd(hcd)->vhci->vhci_hcd_ss;
+	else
+		vhci_hcd = hcd_to_vhci_hcd(hcd)->vhci->vhci_hcd_hs;
+
+	ret = vhci_port_disconnect(vhci_hcd, rhport);
 	if (ret < 0)
 		return -EINVAL;
 
@@ -233,6 +277,7 @@ static int valid_args(__u32 pdev_nr, __u32 rhport, enum usb_device_speed speed)
 	case USB_SPEED_FULL:
 	case USB_SPEED_HIGH:
 	case USB_SPEED_WIRELESS:
+	case USB_SPEED_SUPER:
 		break;
 	default:
 		pr_err("Failed attach request for unsupported USB speed: %s\n",
@@ -262,8 +307,9 @@ static ssize_t store_attach(struct device *dev, struct device_attribute *attr,
 	int sockfd = 0;
 	__u32 port = 0, pdev_nr = 0, rhport = 0, devid = 0, speed = 0;
 	struct usb_hcd *hcd;
-	struct vhci_hcd *vhci;
+	struct vhci_hcd *vhci_hcd;
 	struct vhci_device *vdev;
+	struct vhci *vhci;
 	int err;
 	unsigned long flags;
 
@@ -287,13 +333,19 @@ static ssize_t store_attach(struct device *dev, struct device_attribute *attr,
 	if (!valid_args(pdev_nr, rhport, speed))
 		return -EINVAL;
 
-	hcd = platform_get_drvdata(*(vhci_pdevs + pdev_nr));
+	hcd = platform_get_drvdata(vhcis[pdev_nr].pdev);
 	if (hcd == NULL) {
 		dev_err(dev, "port %d is not ready\n", port);
 		return -EAGAIN;
 	}
-	vhci = hcd_to_vhci(hcd);
-	vdev = &vhci->vdev[rhport];
+
+	vhci_hcd = hcd_to_vhci_hcd(hcd);
+	vhci = vhci_hcd->vhci;
+
+	if (speed == USB_SPEED_SUPER)
+		vdev = &vhci->vhci_hcd_ss->vdev[rhport];
+	else
+		vdev = &vhci->vhci_hcd_hs->vdev[rhport];
 
 	/* Extract socket from fd. */
 	socket = sockfd_lookup(sockfd, &err);
diff --git a/drivers/uwb/driver.c b/drivers/uwb/driver.c
index 776bcb3c2140..ff2d4240b24a 100644
--- a/drivers/uwb/driver.c
+++ b/drivers/uwb/driver.c
@@ -94,17 +94,18 @@ ssize_t beacon_timeout_ms_store(struct class *class,
 	beacon_timeout_ms = bt;
 	return size;
 }
+static CLASS_ATTR_RW(beacon_timeout_ms);
 
-static struct class_attribute uwb_class_attrs[] = {
-	__ATTR(beacon_timeout_ms, S_IWUSR | S_IRUGO,
-	       beacon_timeout_ms_show, beacon_timeout_ms_store),
-	__ATTR_NULL,
+static struct attribute *uwb_class_attrs[] = {
+	&class_attr_beacon_timeout_ms.attr,
+	NULL,
 };
+ATTRIBUTE_GROUPS(uwb_class);
 
 /** Device model classes */
 struct class uwb_rc_class = {
 	.name        = "uwb_rc",
-	.class_attrs = uwb_class_attrs,
+	.class_groups = uwb_class_groups,
 };
 
 
diff --git a/drivers/uwb/i1480/dfu/phy.c b/drivers/uwb/i1480/dfu/phy.c
index 3b1a87de8e63..1ac8526bb689 100644
--- a/drivers/uwb/i1480/dfu/phy.c
+++ b/drivers/uwb/i1480/dfu/phy.c
@@ -126,6 +126,7 @@ int i1480_mpi_read(struct i1480 *i1480, u8 *data, u16 srcaddr, size_t size)
 		dev_err(i1480->dev, "MPI-READ: command execution failed: %d\n",
 			reply->bResultCode);
 		result = -EIO;
+		goto out;
 	}
 	for (cnt = 0; cnt < size; cnt++) {
 		if (reply->data[cnt].page != (srcaddr + cnt) >> 8)
diff --git a/drivers/vfio/virqfd.c b/drivers/vfio/virqfd.c
index 27c89cd5d70b..4797217e5e72 100644
--- a/drivers/vfio/virqfd.c
+++ b/drivers/vfio/virqfd.c
@@ -43,7 +43,7 @@ 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)
+static int virqfd_wakeup(wait_queue_entry_t *wait, unsigned mode, int sync, void *key)
 {
 	struct virqfd *virqfd = container_of(wait, struct virqfd, wait);
 	unsigned long flags = (unsigned long)key;
diff --git a/drivers/vhost/net.c b/drivers/vhost/net.c
index f61f852d6cfd..e3d7ea1288c6 100644
--- a/drivers/vhost/net.c
+++ b/drivers/vhost/net.c
@@ -28,6 +28,8 @@
 #include <linux/if_macvlan.h>
 #include <linux/if_tap.h>
 #include <linux/if_vlan.h>
+#include <linux/skb_array.h>
+#include <linux/skbuff.h>
 
 #include <net/sock.h>
 
@@ -85,6 +87,13 @@ struct vhost_net_ubuf_ref {
 	struct vhost_virtqueue *vq;
 };
 
+#define VHOST_RX_BATCH 64
+struct vhost_net_buf {
+	struct sk_buff **queue;
+	int tail;
+	int head;
+};
+
 struct vhost_net_virtqueue {
 	struct vhost_virtqueue vq;
 	size_t vhost_hlen;
@@ -99,6 +108,8 @@ struct vhost_net_virtqueue {
 	/* Reference counting for outstanding ubufs.
 	 * Protected by vq mutex. Writers must also take device mutex. */
 	struct vhost_net_ubuf_ref *ubufs;
+	struct skb_array *rx_array;
+	struct vhost_net_buf rxq;
 };
 
 struct vhost_net {
@@ -117,6 +128,71 @@ struct vhost_net {
 
 static unsigned vhost_net_zcopy_mask __read_mostly;
 
+static void *vhost_net_buf_get_ptr(struct vhost_net_buf *rxq)
+{
+	if (rxq->tail != rxq->head)
+		return rxq->queue[rxq->head];
+	else
+		return NULL;
+}
+
+static int vhost_net_buf_get_size(struct vhost_net_buf *rxq)
+{
+	return rxq->tail - rxq->head;
+}
+
+static int vhost_net_buf_is_empty(struct vhost_net_buf *rxq)
+{
+	return rxq->tail == rxq->head;
+}
+
+static void *vhost_net_buf_consume(struct vhost_net_buf *rxq)
+{
+	void *ret = vhost_net_buf_get_ptr(rxq);
+	++rxq->head;
+	return ret;
+}
+
+static int vhost_net_buf_produce(struct vhost_net_virtqueue *nvq)
+{
+	struct vhost_net_buf *rxq = &nvq->rxq;
+
+	rxq->head = 0;
+	rxq->tail = skb_array_consume_batched(nvq->rx_array, rxq->queue,
+					      VHOST_RX_BATCH);
+	return rxq->tail;
+}
+
+static void vhost_net_buf_unproduce(struct vhost_net_virtqueue *nvq)
+{
+	struct vhost_net_buf *rxq = &nvq->rxq;
+
+	if (nvq->rx_array && !vhost_net_buf_is_empty(rxq)) {
+		skb_array_unconsume(nvq->rx_array, rxq->queue + rxq->head,
+				    vhost_net_buf_get_size(rxq));
+		rxq->head = rxq->tail = 0;
+	}
+}
+
+static int vhost_net_buf_peek(struct vhost_net_virtqueue *nvq)
+{
+	struct vhost_net_buf *rxq = &nvq->rxq;
+
+	if (!vhost_net_buf_is_empty(rxq))
+		goto out;
+
+	if (!vhost_net_buf_produce(nvq))
+		return 0;
+
+out:
+	return __skb_array_len_with_tag(vhost_net_buf_get_ptr(rxq));
+}
+
+static void vhost_net_buf_init(struct vhost_net_buf *rxq)
+{
+	rxq->head = rxq->tail = 0;
+}
+
 static void vhost_net_enable_zcopy(int vq)
 {
 	vhost_net_zcopy_mask |= 0x1 << vq;
@@ -201,6 +277,7 @@ static void vhost_net_vq_reset(struct vhost_net *n)
 		n->vqs[i].ubufs = NULL;
 		n->vqs[i].vhost_hlen = 0;
 		n->vqs[i].sock_hlen = 0;
+		vhost_net_buf_init(&n->vqs[i].rxq);
 	}
 
 }
@@ -503,15 +580,14 @@ out:
 	mutex_unlock(&vq->mutex);
 }
 
-static int peek_head_len(struct sock *sk)
+static int peek_head_len(struct vhost_net_virtqueue *rvq, struct sock *sk)
 {
-	struct socket *sock = sk->sk_socket;
 	struct sk_buff *head;
 	int len = 0;
 	unsigned long flags;
 
-	if (sock->ops->peek_len)
-		return sock->ops->peek_len(sock);
+	if (rvq->rx_array)
+		return vhost_net_buf_peek(rvq);
 
 	spin_lock_irqsave(&sk->sk_receive_queue.lock, flags);
 	head = skb_peek(&sk->sk_receive_queue);
@@ -537,10 +613,11 @@ static int sk_has_rx_data(struct sock *sk)
 
 static int vhost_net_rx_peek_head_len(struct vhost_net *net, struct sock *sk)
 {
+	struct vhost_net_virtqueue *rvq = &net->vqs[VHOST_NET_VQ_RX];
 	struct vhost_net_virtqueue *nvq = &net->vqs[VHOST_NET_VQ_TX];
 	struct vhost_virtqueue *vq = &nvq->vq;
 	unsigned long uninitialized_var(endtime);
-	int len = peek_head_len(sk);
+	int len = peek_head_len(rvq, sk);
 
 	if (!len && vq->busyloop_timeout) {
 		/* Both tx vq and rx socket were polled here */
@@ -561,7 +638,7 @@ static int vhost_net_rx_peek_head_len(struct vhost_net *net, struct sock *sk)
 			vhost_poll_queue(&vq->poll);
 		mutex_unlock(&vq->mutex);
 
-		len = peek_head_len(sk);
+		len = peek_head_len(rvq, sk);
 	}
 
 	return len;
@@ -699,6 +776,8 @@ static void handle_rx(struct vhost_net *net)
 		/* On error, stop handling until the next kick. */
 		if (unlikely(headcount < 0))
 			goto out;
+		if (nvq->rx_array)
+			msg.msg_control = vhost_net_buf_consume(&nvq->rxq);
 		/* On overrun, truncate and discard */
 		if (unlikely(headcount > UIO_MAXIOV)) {
 			iov_iter_init(&msg.msg_iter, READ, vq->iov, 1, 1);
@@ -815,6 +894,7 @@ static int vhost_net_open(struct inode *inode, struct file *f)
 	struct vhost_net *n;
 	struct vhost_dev *dev;
 	struct vhost_virtqueue **vqs;
+	struct sk_buff **queue;
 	int i;
 
 	n = kvmalloc(sizeof *n, GFP_KERNEL | __GFP_REPEAT);
@@ -826,6 +906,15 @@ static int vhost_net_open(struct inode *inode, struct file *f)
 		return -ENOMEM;
 	}
 
+	queue = kmalloc_array(VHOST_RX_BATCH, sizeof(struct sk_buff *),
+			      GFP_KERNEL);
+	if (!queue) {
+		kfree(vqs);
+		kvfree(n);
+		return -ENOMEM;
+	}
+	n->vqs[VHOST_NET_VQ_RX].rxq.queue = queue;
+
 	dev = &n->dev;
 	vqs[VHOST_NET_VQ_TX] = &n->vqs[VHOST_NET_VQ_TX].vq;
 	vqs[VHOST_NET_VQ_RX] = &n->vqs[VHOST_NET_VQ_RX].vq;
@@ -838,6 +927,7 @@ static int vhost_net_open(struct inode *inode, struct file *f)
 		n->vqs[i].done_idx = 0;
 		n->vqs[i].vhost_hlen = 0;
 		n->vqs[i].sock_hlen = 0;
+		vhost_net_buf_init(&n->vqs[i].rxq);
 	}
 	vhost_dev_init(dev, vqs, VHOST_NET_VQ_MAX);
 
@@ -853,11 +943,14 @@ static struct socket *vhost_net_stop_vq(struct vhost_net *n,
 					struct vhost_virtqueue *vq)
 {
 	struct socket *sock;
+	struct vhost_net_virtqueue *nvq =
+		container_of(vq, struct vhost_net_virtqueue, vq);
 
 	mutex_lock(&vq->mutex);
 	sock = vq->private_data;
 	vhost_net_disable_vq(n, vq);
 	vq->private_data = NULL;
+	vhost_net_buf_unproduce(nvq);
 	mutex_unlock(&vq->mutex);
 	return sock;
 }
@@ -912,6 +1005,7 @@ static int vhost_net_release(struct inode *inode, struct file *f)
 	/* We do an extra flush before freeing memory,
 	 * since jobs can re-queue themselves. */
 	vhost_net_flush(n);
+	kfree(n->vqs[VHOST_NET_VQ_RX].rxq.queue);
 	kfree(n->dev.vqs);
 	kvfree(n);
 	return 0;
@@ -950,6 +1044,25 @@ err:
 	return ERR_PTR(r);
 }
 
+static struct skb_array *get_tap_skb_array(int fd)
+{
+	struct skb_array *array;
+	struct file *file = fget(fd);
+
+	if (!file)
+		return NULL;
+	array = tun_get_skb_array(file);
+	if (!IS_ERR(array))
+		goto out;
+	array = tap_get_skb_array(file);
+	if (!IS_ERR(array))
+		goto out;
+	array = NULL;
+out:
+	fput(file);
+	return array;
+}
+
 static struct socket *get_tap_socket(int fd)
 {
 	struct file *file = fget(fd);
@@ -1026,6 +1139,9 @@ static long vhost_net_set_backend(struct vhost_net *n, unsigned index, int fd)
 
 		vhost_net_disable_vq(n, vq);
 		vq->private_data = sock;
+		vhost_net_buf_unproduce(nvq);
+		if (index == VHOST_NET_VQ_RX)
+			nvq->rx_array = get_tap_skb_array(fd);
 		r = vhost_vq_init_access(vq);
 		if (r)
 			goto err_used;
diff --git a/drivers/vhost/vhost.c b/drivers/vhost/vhost.c
index 042030e5a035..e4613a3c362d 100644
--- a/drivers/vhost/vhost.c
+++ b/drivers/vhost/vhost.c
@@ -165,7 +165,7 @@ static void vhost_poll_func(struct file *file, wait_queue_head_t *wqh,
 	add_wait_queue(wqh, &poll->wait);
 }
 
-static int vhost_poll_wakeup(wait_queue_t *wait, unsigned mode, int sync,
+static int vhost_poll_wakeup(wait_queue_entry_t *wait, unsigned mode, int sync,
 			     void *key)
 {
 	struct vhost_poll *poll = container_of(wait, struct vhost_poll, wait);
diff --git a/drivers/vhost/vhost.h b/drivers/vhost/vhost.h
index f55671d53f28..f72095868b93 100644
--- a/drivers/vhost/vhost.h
+++ b/drivers/vhost/vhost.h
@@ -31,7 +31,7 @@ struct vhost_work {
 struct vhost_poll {
 	poll_table                table;
 	wait_queue_head_t        *wqh;
-	wait_queue_t              wait;
+	wait_queue_entry_t              wait;
 	struct vhost_work	  work;
 	unsigned long		  mask;
 	struct vhost_dev	 *dev;
diff --git a/drivers/vhost/vsock.c b/drivers/vhost/vsock.c
index 3acef3c5d8ed..3f63e03de8e8 100644
--- a/drivers/vhost/vsock.c
+++ b/drivers/vhost/vsock.c
@@ -706,7 +706,7 @@ static const struct file_operations vhost_vsock_fops = {
 };
 
 static struct miscdevice vhost_vsock_misc = {
-	.minor = MISC_DYNAMIC_MINOR,
+	.minor = VHOST_VSOCK_MINOR,
 	.name = "vhost-vsock",
 	.fops = &vhost_vsock_fops,
 };
@@ -778,3 +778,5 @@ module_exit(vhost_vsock_exit);
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Asias He");
 MODULE_DESCRIPTION("vhost transport for vsock ");
+MODULE_ALIAS_MISCDEV(VHOST_VSOCK_MINOR);
+MODULE_ALIAS("devname:vhost-vsock");
diff --git a/drivers/video/fbdev/core/fbmon.c b/drivers/video/fbdev/core/fbmon.c
index 687ebb053438..41d7979d81c5 100644
--- a/drivers/video/fbdev/core/fbmon.c
+++ b/drivers/video/fbdev/core/fbmon.c
@@ -1048,7 +1048,7 @@ void fb_edid_add_monspecs(unsigned char *edid, struct fb_monspecs *specs)
 
 	for (i = 0; i < (128 - edid[2]) / DETAILED_TIMING_DESCRIPTION_SIZE;
 	     i++, block += DETAILED_TIMING_DESCRIPTION_SIZE)
-		if (PIXEL_CLOCK)
+		if (PIXEL_CLOCK != 0)
 			edt[num++] = block - edid;
 
 	/* Yikes, EDID data is totally useless */
diff --git a/drivers/video/fbdev/jz4740_fb.c b/drivers/video/fbdev/jz4740_fb.c
index 87790e9644d0..b57df83fdbd3 100644
--- a/drivers/video/fbdev/jz4740_fb.c
+++ b/drivers/video/fbdev/jz4740_fb.c
@@ -17,6 +17,7 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/platform_device.h>
+#include <linux/pinctrl/consumer.h>
 
 #include <linux/clk.h>
 #include <linux/delay.h>
@@ -27,7 +28,6 @@
 #include <linux/dma-mapping.h>
 
 #include <asm/mach-jz4740/jz4740_fb.h>
-#include <asm/mach-jz4740/gpio.h>
 
 #define JZ_REG_LCD_CFG		0x00
 #define JZ_REG_LCD_VSYNC	0x04
@@ -146,93 +146,6 @@ static const struct fb_fix_screeninfo jzfb_fix = {
 	.accel		= FB_ACCEL_NONE,
 };
 
-static const struct jz_gpio_bulk_request jz_lcd_ctrl_pins[] = {
-	JZ_GPIO_BULK_PIN(LCD_PCLK),
-	JZ_GPIO_BULK_PIN(LCD_HSYNC),
-	JZ_GPIO_BULK_PIN(LCD_VSYNC),
-	JZ_GPIO_BULK_PIN(LCD_DE),
-	JZ_GPIO_BULK_PIN(LCD_PS),
-	JZ_GPIO_BULK_PIN(LCD_REV),
-	JZ_GPIO_BULK_PIN(LCD_CLS),
-	JZ_GPIO_BULK_PIN(LCD_SPL),
-};
-
-static const struct jz_gpio_bulk_request jz_lcd_data_pins[] = {
-	JZ_GPIO_BULK_PIN(LCD_DATA0),
-	JZ_GPIO_BULK_PIN(LCD_DATA1),
-	JZ_GPIO_BULK_PIN(LCD_DATA2),
-	JZ_GPIO_BULK_PIN(LCD_DATA3),
-	JZ_GPIO_BULK_PIN(LCD_DATA4),
-	JZ_GPIO_BULK_PIN(LCD_DATA5),
-	JZ_GPIO_BULK_PIN(LCD_DATA6),
-	JZ_GPIO_BULK_PIN(LCD_DATA7),
-	JZ_GPIO_BULK_PIN(LCD_DATA8),
-	JZ_GPIO_BULK_PIN(LCD_DATA9),
-	JZ_GPIO_BULK_PIN(LCD_DATA10),
-	JZ_GPIO_BULK_PIN(LCD_DATA11),
-	JZ_GPIO_BULK_PIN(LCD_DATA12),
-	JZ_GPIO_BULK_PIN(LCD_DATA13),
-	JZ_GPIO_BULK_PIN(LCD_DATA14),
-	JZ_GPIO_BULK_PIN(LCD_DATA15),
-	JZ_GPIO_BULK_PIN(LCD_DATA16),
-	JZ_GPIO_BULK_PIN(LCD_DATA17),
-};
-
-static unsigned int jzfb_num_ctrl_pins(struct jzfb *jzfb)
-{
-	unsigned int num;
-
-	switch (jzfb->pdata->lcd_type) {
-	case JZ_LCD_TYPE_GENERIC_16_BIT:
-		num = 4;
-		break;
-	case JZ_LCD_TYPE_GENERIC_18_BIT:
-		num = 4;
-		break;
-	case JZ_LCD_TYPE_8BIT_SERIAL:
-		num = 3;
-		break;
-	case JZ_LCD_TYPE_SPECIAL_TFT_1:
-	case JZ_LCD_TYPE_SPECIAL_TFT_2:
-	case JZ_LCD_TYPE_SPECIAL_TFT_3:
-		num = 8;
-		break;
-	default:
-		num = 0;
-		break;
-	}
-	return num;
-}
-
-static unsigned int jzfb_num_data_pins(struct jzfb *jzfb)
-{
-	unsigned int num;
-
-	switch (jzfb->pdata->lcd_type) {
-	case JZ_LCD_TYPE_GENERIC_16_BIT:
-		num = 16;
-		break;
-	case JZ_LCD_TYPE_GENERIC_18_BIT:
-		num = 18;
-		break;
-	case JZ_LCD_TYPE_8BIT_SERIAL:
-		num = 8;
-		break;
-	case JZ_LCD_TYPE_SPECIAL_TFT_1:
-	case JZ_LCD_TYPE_SPECIAL_TFT_2:
-	case JZ_LCD_TYPE_SPECIAL_TFT_3:
-		if (jzfb->pdata->bpp == 18)
-			num = 18;
-		else
-			num = 16;
-		break;
-	default:
-		num = 0;
-		break;
-	}
-	return num;
-}
-
 /* Based on CNVT_TOHW macro from skeletonfb.c */
 static inline uint32_t jzfb_convert_color_to_hw(unsigned val,
 	struct fb_bitfield *bf)
@@ -487,8 +400,7 @@ static void jzfb_enable(struct jzfb *jzfb)
 
 	clk_prepare_enable(jzfb->ldclk);
 
-	jz_gpio_bulk_resume(jz_lcd_ctrl_pins, jzfb_num_ctrl_pins(jzfb));
-	jz_gpio_bulk_resume(jz_lcd_data_pins, jzfb_num_data_pins(jzfb));
+	pinctrl_pm_select_default_state(&jzfb->pdev->dev);
 
 	writel(0, jzfb->base + JZ_REG_LCD_STATE);
 
@@ -511,8 +423,7 @@ static void jzfb_disable(struct jzfb *jzfb)
 		ctrl = readl(jzfb->base + JZ_REG_LCD_STATE);
 	} while (!(ctrl & JZ_LCD_STATE_DISABLED));
 
-	jz_gpio_bulk_suspend(jz_lcd_ctrl_pins, jzfb_num_ctrl_pins(jzfb));
-	jz_gpio_bulk_suspend(jz_lcd_data_pins, jzfb_num_data_pins(jzfb));
+	pinctrl_pm_select_sleep_state(&jzfb->pdev->dev);
 
 	clk_disable_unprepare(jzfb->ldclk);
 }
@@ -701,9 +612,6 @@ static int jzfb_probe(struct platform_device *pdev)
 	fb->mode = NULL;
 	jzfb_set_par(fb);
 
-	jz_gpio_bulk_request(jz_lcd_ctrl_pins, jzfb_num_ctrl_pins(jzfb));
-	jz_gpio_bulk_request(jz_lcd_data_pins, jzfb_num_data_pins(jzfb));
-
 	ret = register_framebuffer(fb);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to register framebuffer: %d\n", ret);
@@ -715,9 +623,6 @@ static int jzfb_probe(struct platform_device *pdev)
 	return 0;
 
 err_free_devmem:
-	jz_gpio_bulk_free(jz_lcd_ctrl_pins, jzfb_num_ctrl_pins(jzfb));
-	jz_gpio_bulk_free(jz_lcd_data_pins, jzfb_num_data_pins(jzfb));
-
 	fb_dealloc_cmap(&fb->cmap);
 	jzfb_free_devmem(jzfb);
 err_framebuffer_release:
@@ -731,9 +636,6 @@ static int jzfb_remove(struct platform_device *pdev)
 
 	jzfb_blank(FB_BLANK_POWERDOWN, jzfb->fb);
 
-	jz_gpio_bulk_free(jz_lcd_ctrl_pins, jzfb_num_ctrl_pins(jzfb));
-	jz_gpio_bulk_free(jz_lcd_data_pins, jzfb_num_data_pins(jzfb));
-
 	fb_dealloc_cmap(&jzfb->fb->cmap);
 	jzfb_free_devmem(jzfb);
 
diff --git a/drivers/video/fbdev/smscufx.c b/drivers/video/fbdev/smscufx.c
index ec2e7e353685..449fceaf79d5 100644
--- a/drivers/video/fbdev/smscufx.c
+++ b/drivers/video/fbdev/smscufx.c
@@ -1646,8 +1646,9 @@ static int ufx_usb_probe(struct usb_interface *interface,
 	dev_dbg(dev->gdev, "%s %s - serial #%s\n",
 		usbdev->manufacturer, usbdev->product, usbdev->serial);
 	dev_dbg(dev->gdev, "vid_%04x&pid_%04x&rev_%04x driver's ufx_data struct at %p\n",
-		usbdev->descriptor.idVendor, usbdev->descriptor.idProduct,
-		usbdev->descriptor.bcdDevice, dev);
+		le16_to_cpu(usbdev->descriptor.idVendor),
+		le16_to_cpu(usbdev->descriptor.idProduct),
+		le16_to_cpu(usbdev->descriptor.bcdDevice), dev);
 	dev_dbg(dev->gdev, "console enable=%d\n", console);
 	dev_dbg(dev->gdev, "fb_defio enable=%d\n", fb_defio);
 
diff --git a/drivers/video/fbdev/udlfb.c b/drivers/video/fbdev/udlfb.c
index 6a3c353de7c3..05ef657235df 100644
--- a/drivers/video/fbdev/udlfb.c
+++ b/drivers/video/fbdev/udlfb.c
@@ -1105,8 +1105,8 @@ static int dlfb_ops_blank(int blank_mode, struct fb_info *info)
 	char *bufptr;
 	struct urb *urb;
 
-	pr_info("/dev/fb%d FB_BLANK mode %d --> %d\n",
-		info->node, dev->blank_mode, blank_mode);
+	pr_debug("/dev/fb%d FB_BLANK mode %d --> %d\n",
+		 info->node, dev->blank_mode, blank_mode);
 
 	if ((dev->blank_mode == FB_BLANK_POWERDOWN) &&
 	    (blank_mode != FB_BLANK_POWERDOWN)) {
@@ -1613,8 +1613,9 @@ static int dlfb_usb_probe(struct usb_interface *interface,
 	pr_info("%s %s - serial #%s\n",
 		usbdev->manufacturer, usbdev->product, usbdev->serial);
 	pr_info("vid_%04x&pid_%04x&rev_%04x driver's dlfb_data struct at %p\n",
-		usbdev->descriptor.idVendor, usbdev->descriptor.idProduct,
-		usbdev->descriptor.bcdDevice, dev);
+		le16_to_cpu(usbdev->descriptor.idVendor),
+		le16_to_cpu(usbdev->descriptor.idProduct),
+		le16_to_cpu(usbdev->descriptor.bcdDevice), dev);
 	pr_info("console enable=%d\n", console);
 	pr_info("fb_defio enable=%d\n", fb_defio);
 	pr_info("shadow enable=%d\n", shadow);
diff --git a/drivers/video/fbdev/via/viafbdev.c b/drivers/video/fbdev/via/viafbdev.c
index f9718f012aae..badee04ef496 100644
--- a/drivers/video/fbdev/via/viafbdev.c
+++ b/drivers/video/fbdev/via/viafbdev.c
@@ -1630,16 +1630,14 @@ static void viafb_init_proc(struct viafb_shared *shared)
 }
 static void viafb_remove_proc(struct viafb_shared *shared)
 {
-	struct proc_dir_entry *viafb_entry = shared->proc_entry,
-		*iga1_entry = shared->iga1_proc_entry,
-		*iga2_entry = shared->iga2_proc_entry;
+	struct proc_dir_entry *viafb_entry = shared->proc_entry;
 
 	if (!viafb_entry)
 		return;
 
-	remove_proc_entry("output_devices", iga2_entry);
+	remove_proc_entry("output_devices", shared->iga2_proc_entry);
 	remove_proc_entry("iga2", viafb_entry);
-	remove_proc_entry("output_devices", iga1_entry);
+	remove_proc_entry("output_devices", shared->iga1_proc_entry);
 	remove_proc_entry("iga1", viafb_entry);
 	remove_proc_entry("supported_output_devices", viafb_entry);
 
diff --git a/drivers/virtio/virtio_balloon.c b/drivers/virtio/virtio_balloon.c
index 408c174ef0d5..22caf808bfab 100644
--- a/drivers/virtio/virtio_balloon.c
+++ b/drivers/virtio/virtio_balloon.c
@@ -663,6 +663,12 @@ static int virtballoon_restore(struct virtio_device *vdev)
 }
 #endif
 
+static int virtballoon_validate(struct virtio_device *vdev)
+{
+	__virtio_clear_bit(vdev, VIRTIO_F_IOMMU_PLATFORM);
+	return 0;
+}
+
 static unsigned int features[] = {
 	VIRTIO_BALLOON_F_MUST_TELL_HOST,
 	VIRTIO_BALLOON_F_STATS_VQ,
@@ -675,6 +681,7 @@ static struct virtio_driver virtio_balloon_driver = {
 	.driver.name =	KBUILD_MODNAME,
 	.driver.owner =	THIS_MODULE,
 	.id_table =	id_table,
+	.validate =	virtballoon_validate,
 	.probe =	virtballoon_probe,
 	.remove =	virtballoon_remove,
 	.config_changed = virtballoon_changed,
diff --git a/drivers/w1/masters/ds1wm.c b/drivers/w1/masters/ds1wm.c
index e0b8a4bc73df..fd2e9da27c4b 100644
--- a/drivers/w1/masters/ds1wm.c
+++ b/drivers/w1/masters/ds1wm.c
@@ -25,8 +25,7 @@
 
 #include <asm/io.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
+#include <linux/w1.h>
 
 
 #define DS1WM_CMD	0x00	/* R/W 4 bits command */
diff --git a/drivers/w1/masters/ds2482.c b/drivers/w1/masters/ds2482.c
index 2e30db1b1a43..d49681cd29af 100644
--- a/drivers/w1/masters/ds2482.c
+++ b/drivers/w1/masters/ds2482.c
@@ -20,8 +20,7 @@
 #include <linux/delay.h>
 #include <asm/delay.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
+#include <linux/w1.h>
 
 /**
  * Allow the active pullup to be disabled, default is enabled.
@@ -35,6 +34,8 @@
  */
 static int ds2482_active_pullup = 1;
 module_param_named(active_pullup, ds2482_active_pullup, int, 0644);
+MODULE_PARM_DESC(active_pullup, "Active pullup (apply to all buses): " \
+				"0-disable, 1-enable (default)");
 
 /**
  * The DS2482 registers - there are 3 registers that are addressed by a read
@@ -93,30 +94,6 @@ static const u8 ds2482_chan_rd[8] =
 #define DS2482_REG_STS_PPD		0x02
 #define DS2482_REG_STS_1WB		0x01
 
-
-static int ds2482_probe(struct i2c_client *client,
-			const struct i2c_device_id *id);
-static int ds2482_remove(struct i2c_client *client);
-
-
-/**
- * Driver data (common to all clients)
- */
-static const struct i2c_device_id ds2482_id[] = {
-	{ "ds2482", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, ds2482_id);
-
-static struct i2c_driver ds2482_driver = {
-	.driver = {
-		.name	= "ds2482",
-	},
-	.probe		= ds2482_probe,
-	.remove		= ds2482_remove,
-	.id_table	= ds2482_id,
-};
-
 /*
  * Client data (each client gets its own)
  */
@@ -560,10 +537,25 @@ static int ds2482_remove(struct i2c_client *client)
 	return 0;
 }
 
+/**
+ * Driver data (common to all clients)
+ */
+static const struct i2c_device_id ds2482_id[] = {
+	{ "ds2482", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ds2482_id);
+
+static struct i2c_driver ds2482_driver = {
+	.driver = {
+		.name	= "ds2482",
+	},
+	.probe		= ds2482_probe,
+	.remove		= ds2482_remove,
+	.id_table	= ds2482_id,
+};
 module_i2c_driver(ds2482_driver);
 
-MODULE_PARM_DESC(active_pullup, "Active pullup (apply to all buses): " \
-				"0-disable, 1-enable (default)");
 MODULE_AUTHOR("Ben Gardner <bgardner@wabtec.com>");
 MODULE_DESCRIPTION("DS2482 driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/w1/masters/ds2490.c b/drivers/w1/masters/ds2490.c
index be77b7914fad..46ccb2fc4f60 100644
--- a/drivers/w1/masters/ds2490.c
+++ b/drivers/w1/masters/ds2490.c
@@ -25,8 +25,7 @@
 #include <linux/usb.h>
 #include <linux/slab.h>
 
-#include "../w1_int.h"
-#include "../w1.h"
+#include <linux/w1.h>
 
 /* USB Standard */
 /* USB Control request vendor type */
@@ -179,28 +178,9 @@ struct ds_status
 	u8			reserved2;
 };
 
-static struct usb_device_id ds_id_table [] = {
-	{ USB_DEVICE(0x04fa, 0x2490) },
-	{ },
-};
-MODULE_DEVICE_TABLE(usb, ds_id_table);
-
-static int ds_probe(struct usb_interface *, const struct usb_device_id *);
-static void ds_disconnect(struct usb_interface *);
-
-static int ds_send_control(struct ds_device *, u16, u16);
-static int ds_send_control_cmd(struct ds_device *, u16, u16);
-
 static LIST_HEAD(ds_devices);
 static DEFINE_MUTEX(ds_mutex);
 
-static struct usb_driver ds_driver = {
-	.name =		"DS9490R",
-	.probe =	ds_probe,
-	.disconnect =	ds_disconnect,
-	.id_table =	ds_id_table,
-};
-
 static int ds_send_control_cmd(struct ds_device *dev, u16 value, u16 index)
 {
 	int err;
@@ -1108,8 +1088,20 @@ static void ds_disconnect(struct usb_interface *intf)
 	kfree(dev);
 }
 
+static struct usb_device_id ds_id_table [] = {
+	{ USB_DEVICE(0x04fa, 0x2490) },
+	{ },
+};
+MODULE_DEVICE_TABLE(usb, ds_id_table);
+
+static struct usb_driver ds_driver = {
+	.name =		"DS9490R",
+	.probe =	ds_probe,
+	.disconnect =	ds_disconnect,
+	.id_table =	ds_id_table,
+};
 module_usb_driver(ds_driver);
 
-MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Evgeniy Polyakov <zbr@ioremap.net>");
 MODULE_DESCRIPTION("DS2490 USB <-> W1 bus master driver (DS9490*)");
+MODULE_LICENSE("GPL");
diff --git a/drivers/w1/masters/matrox_w1.c b/drivers/w1/masters/matrox_w1.c
index 97a676bf5989..d83d7c99d81d 100644
--- a/drivers/w1/masters/matrox_w1.c
+++ b/drivers/w1/masters/matrox_w1.c
@@ -34,28 +34,7 @@
 #include <linux/pci_ids.h>
 #include <linux/pci.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Evgeniy Polyakov <zbr@ioremap.net>");
-MODULE_DESCRIPTION("Driver for transport(Dallas 1-wire protocol) over VGA DDC(matrox gpio).");
-
-static struct pci_device_id matrox_w1_tbl[] = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_MATROX, PCI_DEVICE_ID_MATROX_G400) },
-	{ },
-};
-MODULE_DEVICE_TABLE(pci, matrox_w1_tbl);
-
-static int matrox_w1_probe(struct pci_dev *, const struct pci_device_id *);
-static void matrox_w1_remove(struct pci_dev *);
-
-static struct pci_driver matrox_w1_pci_driver = {
-	.name = "matrox_w1",
-	.id_table = matrox_w1_tbl,
-	.probe = matrox_w1_probe,
-	.remove = matrox_w1_remove,
-};
+#include <linux/w1.h>
 
 /*
  * Matrox G400 DDC registers.
@@ -88,9 +67,6 @@ struct matrox_device
 	struct w1_bus_master *bus_master;
 };
 
-static u8 matrox_w1_read_ddc_bit(void *);
-static void matrox_w1_write_ddc_bit(void *, u8);
-
 /*
  * These functions read and write DDC Data bit.
  *
@@ -226,4 +202,21 @@ static void matrox_w1_remove(struct pci_dev *pdev)
 	}
 	kfree(dev);
 }
+
+static struct pci_device_id matrox_w1_tbl[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_MATROX, PCI_DEVICE_ID_MATROX_G400) },
+	{ },
+};
+MODULE_DEVICE_TABLE(pci, matrox_w1_tbl);
+
+static struct pci_driver matrox_w1_pci_driver = {
+	.name = "matrox_w1",
+	.id_table = matrox_w1_tbl,
+	.probe = matrox_w1_probe,
+	.remove = matrox_w1_remove,
+};
 module_pci_driver(matrox_w1_pci_driver);
+
+MODULE_AUTHOR("Evgeniy Polyakov <zbr@ioremap.net>");
+MODULE_DESCRIPTION("Driver for transport(Dallas 1-wire protocol) over VGA DDC(matrox gpio).");
+MODULE_LICENSE("GPL");
diff --git a/drivers/w1/masters/mxc_w1.c b/drivers/w1/masters/mxc_w1.c
index a4621757a47f..74f2e6e6202a 100644
--- a/drivers/w1/masters/mxc_w1.c
+++ b/drivers/w1/masters/mxc_w1.c
@@ -19,8 +19,7 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
+#include <linux/w1.h>
 
 /*
  * MXC W1 Register offsets
diff --git a/drivers/w1/masters/omap_hdq.c b/drivers/w1/masters/omap_hdq.c
index fb190c259607..3612542b6044 100644
--- a/drivers/w1/masters/omap_hdq.c
+++ b/drivers/w1/masters/omap_hdq.c
@@ -19,8 +19,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/of.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
+#include <linux/w1.h>
 
 #define	MOD_NAME	"OMAP_HDQ:"
 
@@ -53,7 +52,10 @@
 #define OMAP_HDQ_MAX_USER			4
 
 static DECLARE_WAIT_QUEUE_HEAD(hdq_wait_queue);
+
 static int w1_id;
+module_param(w1_id, int, S_IRUSR);
+MODULE_PARM_DESC(w1_id, "1-wire id for the slave detection in HDQ mode");
 
 struct hdq_data {
 	struct device		*dev;
@@ -76,36 +78,6 @@ struct hdq_data {
 
 };
 
-static int omap_hdq_probe(struct platform_device *pdev);
-static int omap_hdq_remove(struct platform_device *pdev);
-
-static const struct of_device_id omap_hdq_dt_ids[] = {
-	{ .compatible = "ti,omap3-1w" },
-	{ .compatible = "ti,am4372-hdq" },
-	{}
-};
-MODULE_DEVICE_TABLE(of, omap_hdq_dt_ids);
-
-static struct platform_driver omap_hdq_driver = {
-	.probe =	omap_hdq_probe,
-	.remove =	omap_hdq_remove,
-	.driver =	{
-		.name =	"omap_hdq",
-		.of_match_table = omap_hdq_dt_ids,
-	},
-};
-
-static u8 omap_w1_read_byte(void *_hdq);
-static void omap_w1_write_byte(void *_hdq, u8 byte);
-static u8 omap_w1_reset_bus(void *_hdq);
-
-
-static struct w1_bus_master omap_w1_master = {
-	.read_byte	= omap_w1_read_byte,
-	.write_byte	= omap_w1_write_byte,
-	.reset_bus	= omap_w1_reset_bus,
-};
-
 /* HDQ register I/O routines */
 static inline u8 hdq_reg_in(struct hdq_data *hdq_data, u32 offset)
 {
@@ -678,6 +650,12 @@ static void omap_w1_write_byte(void *_hdq, u8 byte)
 	}
 }
 
+static struct w1_bus_master omap_w1_master = {
+	.read_byte	= omap_w1_read_byte,
+	.write_byte	= omap_w1_write_byte,
+	.reset_bus	= omap_w1_reset_bus,
+};
+
 static int omap_hdq_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -787,10 +765,22 @@ static int omap_hdq_remove(struct platform_device *pdev)
 	return 0;
 }
 
-module_platform_driver(omap_hdq_driver);
+static const struct of_device_id omap_hdq_dt_ids[] = {
+	{ .compatible = "ti,omap3-1w" },
+	{ .compatible = "ti,am4372-hdq" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, omap_hdq_dt_ids);
 
-module_param(w1_id, int, S_IRUSR);
-MODULE_PARM_DESC(w1_id, "1-wire id for the slave detection in HDQ mode");
+static struct platform_driver omap_hdq_driver = {
+	.probe = omap_hdq_probe,
+	.remove = omap_hdq_remove,
+	.driver = {
+		.name =	"omap_hdq",
+		.of_match_table = omap_hdq_dt_ids,
+	},
+};
+module_platform_driver(omap_hdq_driver);
 
 MODULE_AUTHOR("Texas Instruments");
 MODULE_DESCRIPTION("HDQ-1W driver Library");
diff --git a/drivers/w1/masters/w1-gpio.c b/drivers/w1/masters/w1-gpio.c
index a373ae69d9f6..a90728ceec5a 100644
--- a/drivers/w1/masters/w1-gpio.c
+++ b/drivers/w1/masters/w1-gpio.c
@@ -20,8 +20,7 @@
 #include <linux/of.h>
 #include <linux/delay.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
+#include <linux/w1.h>
 
 static u8 w1_gpio_set_pullup(void *data, int delay)
 {
diff --git a/drivers/w1/slaves/w1_bq27000.c b/drivers/w1/slaves/w1_bq27000.c
index 9f4a86b754ba..8046ac45381a 100644
--- a/drivers/w1/slaves/w1_bq27000.c
+++ b/drivers/w1/slaves/w1_bq27000.c
@@ -17,14 +17,16 @@
 #include <linux/mutex.h>
 #include <linux/power/bq27xxx_battery.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
-#include "../w1_family.h"
+#include <linux/w1.h>
+
+#define W1_FAMILY_BQ27000	0x01
 
 #define HDQ_CMD_READ	(0)
 #define HDQ_CMD_WRITE	(1<<7)
 
 static int F_ID;
+module_param(F_ID, int, S_IRUSR);
+MODULE_PARM_DESC(F_ID, "1-wire slave FID for BQ device");
 
 static int w1_bq27000_read(struct device *dev, unsigned int reg)
 {
@@ -106,13 +108,10 @@ static void __exit w1_bq27000_exit(void)
 	w1_unregister_family(&w1_bq27000_family);
 }
 
-
 module_init(w1_bq27000_init);
 module_exit(w1_bq27000_exit);
 
-module_param(F_ID, int, S_IRUSR);
-MODULE_PARM_DESC(F_ID, "1-wire slave FID for BQ device");
-MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_BQ27000));
-MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Texas Instruments Ltd");
 MODULE_DESCRIPTION("HDQ/1-wire slave driver bq27000 battery monitor chip");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_BQ27000));
diff --git a/drivers/w1/slaves/w1_ds2405.c b/drivers/w1/slaves/w1_ds2405.c
index d5d54876cb64..42a1e81060ce 100644
--- a/drivers/w1/slaves/w1_ds2405.c
+++ b/drivers/w1/slaves/w1_ds2405.c
@@ -24,8 +24,9 @@
 #include <linux/string.h>
 #include <linux/types.h>
 
-#include "../w1.h"
-#include "../w1_family.h"
+#include <linux/w1.h>
+
+#define W1_FAMILY_DS2405	0x05
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Maciej S. Szmigiero <mail@maciej.szmigiero.name>");
diff --git a/drivers/w1/slaves/w1_ds2406.c b/drivers/w1/slaves/w1_ds2406.c
index 51f2f66d6555..fac266366ca3 100644
--- a/drivers/w1/slaves/w1_ds2406.c
+++ b/drivers/w1/slaves/w1_ds2406.c
@@ -17,13 +17,9 @@
 #include <linux/slab.h>
 #include <linux/crc16.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
-#include "../w1_family.h"
+#include <linux/w1.h>
 
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Scott Alfter <scott@alfter.us>");
-MODULE_DESCRIPTION("w1 family 12 driver for DS2406 2 Pin IO");
+#define W1_FAMILY_DS2406	0x12
 
 #define W1_F12_FUNC_READ_STATUS		   0xAA
 #define W1_F12_FUNC_WRITE_STATUS	   0x55
@@ -154,3 +150,7 @@ static struct w1_family w1_family_12 = {
 	.fops = &w1_f12_fops,
 };
 module_w1_family(w1_family_12);
+
+MODULE_AUTHOR("Scott Alfter <scott@alfter.us>");
+MODULE_DESCRIPTION("w1 family 12 driver for DS2406 2 Pin IO");
+MODULE_LICENSE("GPL");
diff --git a/drivers/w1/slaves/w1_ds2408.c b/drivers/w1/slaves/w1_ds2408.c
index aec5958e66e9..b535d5ec35b6 100644
--- a/drivers/w1/slaves/w1_ds2408.c
+++ b/drivers/w1/slaves/w1_ds2408.c
@@ -15,15 +15,9 @@
 #include <linux/delay.h>
 #include <linux/slab.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
-#include "../w1_family.h"
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Jean-Francois Dagenais <dagenaisj@sonatest.com>");
-MODULE_DESCRIPTION("w1 family 29 driver for DS2408 8 Pin IO");
-MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_DS2408));
+#include <linux/w1.h>
 
+#define W1_FAMILY_DS2408	0x29
 
 #define W1_F29_RETRIES		3
 
@@ -352,3 +346,8 @@ static struct w1_family w1_family_29 = {
 	.fops = &w1_f29_fops,
 };
 module_w1_family(w1_family_29);
+
+MODULE_AUTHOR("Jean-Francois Dagenais <dagenaisj@sonatest.com>");
+MODULE_DESCRIPTION("w1 family 29 driver for DS2408 8 Pin IO");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_DS2408));
diff --git a/drivers/w1/slaves/w1_ds2413.c b/drivers/w1/slaves/w1_ds2413.c
index f2e1c51533b9..492e3d010321 100644
--- a/drivers/w1/slaves/w1_ds2413.c
+++ b/drivers/w1/slaves/w1_ds2413.c
@@ -16,14 +16,9 @@
 #include <linux/delay.h>
 #include <linux/slab.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
-#include "../w1_family.h"
+#include <linux/w1.h>
 
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Mariusz Bialonczyk <manio@skyboo.net>");
-MODULE_DESCRIPTION("w1 family 3a driver for DS2413 2 Pin IO");
-MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_DS2413));
+#define W1_FAMILY_DS2413	0x3A
 
 #define W1_F3A_RETRIES                     3
 #define W1_F3A_FUNC_PIO_ACCESS_READ        0xF5
@@ -136,3 +131,8 @@ static struct w1_family w1_family_3a = {
 	.fops = &w1_f3a_fops,
 };
 module_w1_family(w1_family_3a);
+
+MODULE_AUTHOR("Mariusz Bialonczyk <manio@skyboo.net>");
+MODULE_DESCRIPTION("w1 family 3a driver for DS2413 2 Pin IO");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_DS2413));
diff --git a/drivers/w1/slaves/w1_ds2423.c b/drivers/w1/slaves/w1_ds2423.c
index 4ab54fd9dde2..050407c53b16 100644
--- a/drivers/w1/slaves/w1_ds2423.c
+++ b/drivers/w1/slaves/w1_ds2423.c
@@ -30,9 +30,9 @@
 #include <linux/delay.h>
 #include <linux/crc16.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
-#include "../w1_family.h"
+#include <linux/w1.h>
+
+#define W1_COUNTER_DS2423	0x1D
 
 #define CRC16_VALID	0xb001
 #define CRC16_INIT	0
@@ -140,7 +140,7 @@ static struct w1_family w1_family_1d = {
 };
 module_w1_family(w1_family_1d);
 
-MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Mika Laitio <lamikr@pilppa.org>");
 MODULE_DESCRIPTION("w1 family 1d driver for DS2423, 4 counters and 4kb ram");
+MODULE_LICENSE("GPL");
 MODULE_ALIAS("w1-family-" __stringify(W1_COUNTER_DS2423));
diff --git a/drivers/w1/slaves/w1_ds2431.c b/drivers/w1/slaves/w1_ds2431.c
index 80572cb63ba8..5adecd3face1 100644
--- a/drivers/w1/slaves/w1_ds2431.c
+++ b/drivers/w1/slaves/w1_ds2431.c
@@ -16,9 +16,9 @@
 #include <linux/types.h>
 #include <linux/delay.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
-#include "../w1_family.h"
+#include <linux/w1.h>
+
+#define W1_EEPROM_DS2431	0x2D
 
 #define W1_F2D_EEPROM_SIZE		128
 #define W1_F2D_PAGE_COUNT		4
@@ -290,7 +290,7 @@ static struct w1_family w1_family_2d = {
 };
 module_w1_family(w1_family_2d);
 
-MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Bernhard Weirich <bernhard.weirich@riedel.net>");
 MODULE_DESCRIPTION("w1 family 2d driver for DS2431, 1kb EEPROM");
+MODULE_LICENSE("GPL");
 MODULE_ALIAS("w1-family-" __stringify(W1_EEPROM_DS2431));
diff --git a/drivers/w1/slaves/w1_ds2433.c b/drivers/w1/slaves/w1_ds2433.c
index 6cf378c89ecb..75ad70cfe8e8 100644
--- a/drivers/w1/slaves/w1_ds2433.c
+++ b/drivers/w1/slaves/w1_ds2433.c
@@ -22,14 +22,9 @@
 
 #endif
 
-#include "../w1.h"
-#include "../w1_int.h"
-#include "../w1_family.h"
+#include <linux/w1.h>
 
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Ben Gardner <bgardner@wabtec.com>");
-MODULE_DESCRIPTION("w1 family 23 driver for DS2433, 4kb EEPROM");
-MODULE_ALIAS("w1-family-" __stringify(W1_EEPROM_DS2433));
+#define W1_EEPROM_DS2433	0x23
 
 #define W1_EEPROM_SIZE		512
 #define W1_PAGE_COUNT		16
@@ -306,3 +301,8 @@ static struct w1_family w1_family_23 = {
 	.fops = &w1_f23_fops,
 };
 module_w1_family(w1_family_23);
+
+MODULE_AUTHOR("Ben Gardner <bgardner@wabtec.com>");
+MODULE_DESCRIPTION("w1 family 23 driver for DS2433, 4kb EEPROM");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("w1-family-" __stringify(W1_EEPROM_DS2433));
diff --git a/drivers/w1/slaves/w1_ds2438.c b/drivers/w1/slaves/w1_ds2438.c
index 5ededb4965e1..6487fb772a20 100644
--- a/drivers/w1/slaves/w1_ds2438.c
+++ b/drivers/w1/slaves/w1_ds2438.c
@@ -13,8 +13,9 @@
 #include <linux/types.h>
 #include <linux/delay.h>
 
-#include "../w1.h"
-#include "../w1_family.h"
+#include <linux/w1.h>
+
+#define W1_FAMILY_DS2438		0x26
 
 #define W1_DS2438_RETRIES		3
 
diff --git a/drivers/w1/slaves/w1_ds2760.c b/drivers/w1/slaves/w1_ds2760.c
index ffa37f773b3b..26168abfb8b8 100644
--- a/drivers/w1/slaves/w1_ds2760.c
+++ b/drivers/w1/slaves/w1_ds2760.c
@@ -18,11 +18,12 @@
 #include <linux/idr.h>
 #include <linux/gfp.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
-#include "../w1_family.h"
+#include <linux/w1.h>
+
 #include "w1_ds2760.h"
 
+#define W1_FAMILY_DS2760	0x30
+
 static int w1_ds2760_io(struct device *dev, char *buf, int addr, size_t count,
 			int io)
 {
@@ -63,11 +64,13 @@ int w1_ds2760_read(struct device *dev, char *buf, int addr, size_t count)
 {
 	return w1_ds2760_io(dev, buf, addr, count, 0);
 }
+EXPORT_SYMBOL(w1_ds2760_read);
 
 int w1_ds2760_write(struct device *dev, char *buf, int addr, size_t count)
 {
 	return w1_ds2760_io(dev, buf, addr, count, 1);
 }
+EXPORT_SYMBOL(w1_ds2760_write);
 
 static int w1_ds2760_eeprom_cmd(struct device *dev, int addr, int cmd)
 {
@@ -91,11 +94,13 @@ int w1_ds2760_store_eeprom(struct device *dev, int addr)
 {
 	return w1_ds2760_eeprom_cmd(dev, addr, W1_DS2760_COPY_DATA);
 }
+EXPORT_SYMBOL(w1_ds2760_store_eeprom);
 
 int w1_ds2760_recall_eeprom(struct device *dev, int addr)
 {
 	return w1_ds2760_eeprom_cmd(dev, addr, W1_DS2760_RECALL_DATA);
 }
+EXPORT_SYMBOL(w1_ds2760_recall_eeprom);
 
 static ssize_t w1_slave_read(struct file *filp, struct kobject *kobj,
 			     struct bin_attribute *bin_attr, char *buf,
@@ -164,12 +169,7 @@ static struct w1_family w1_ds2760_family = {
 };
 module_w1_family(w1_ds2760_family);
 
-EXPORT_SYMBOL(w1_ds2760_read);
-EXPORT_SYMBOL(w1_ds2760_write);
-EXPORT_SYMBOL(w1_ds2760_store_eeprom);
-EXPORT_SYMBOL(w1_ds2760_recall_eeprom);
-
-MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Szabolcs Gyurko <szabolcs.gyurko@tlt.hu>");
 MODULE_DESCRIPTION("1-wire Driver Dallas 2760 battery monitor chip");
+MODULE_LICENSE("GPL");
 MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_DS2760));
diff --git a/drivers/w1/slaves/w1_ds2780.c b/drivers/w1/slaves/w1_ds2780.c
index f5c2aa429a92..a60528131154 100644
--- a/drivers/w1/slaves/w1_ds2780.c
+++ b/drivers/w1/slaves/w1_ds2780.c
@@ -21,11 +21,12 @@
 #include <linux/mutex.h>
 #include <linux/idr.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
-#include "../w1_family.h"
+#include <linux/w1.h>
+
 #include "w1_ds2780.h"
 
+#define W1_FAMILY_DS2780	0x32
+
 static int w1_ds2780_do_io(struct device *dev, char *buf, int addr,
 			size_t count, int io)
 {
@@ -156,7 +157,7 @@ static struct w1_family w1_ds2780_family = {
 };
 module_w1_family(w1_ds2780_family);
 
-MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Clifton Barnes <cabarnes@indesign-llc.com>");
 MODULE_DESCRIPTION("1-wire Driver for Maxim/Dallas DS2780 Stand-Alone Fuel Gauge IC");
+MODULE_LICENSE("GPL");
 MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_DS2780));
diff --git a/drivers/w1/slaves/w1_ds2781.c b/drivers/w1/slaves/w1_ds2781.c
index 9c03e014cf9e..645be6e0b24a 100644
--- a/drivers/w1/slaves/w1_ds2781.c
+++ b/drivers/w1/slaves/w1_ds2781.c
@@ -18,11 +18,12 @@
 #include <linux/platform_device.h>
 #include <linux/mutex.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
-#include "../w1_family.h"
+#include <linux/w1.h>
+
 #include "w1_ds2781.h"
 
+#define W1_FAMILY_DS2781	0x3D
+
 static int w1_ds2781_do_io(struct device *dev, char *buf, int addr,
 			size_t count, int io)
 {
@@ -153,7 +154,7 @@ static struct w1_family w1_ds2781_family = {
 };
 module_w1_family(w1_ds2781_family);
 
-MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Renata Sayakhova <renata@oktetlabs.ru>");
 MODULE_DESCRIPTION("1-wire Driver for Maxim/Dallas DS2781 Stand-Alone Fuel Gauge IC");
+MODULE_LICENSE("GPL");
 MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_DS2781));
diff --git a/drivers/w1/slaves/w1_ds28e04.c b/drivers/w1/slaves/w1_ds28e04.c
index 5e348d38ec5c..ec234b846eb3 100644
--- a/drivers/w1/slaves/w1_ds28e04.c
+++ b/drivers/w1/slaves/w1_ds28e04.c
@@ -20,14 +20,9 @@
 #define CRC16_INIT		0
 #define CRC16_VALID		0xb001
 
-#include "../w1.h"
-#include "../w1_int.h"
-#include "../w1_family.h"
+#include <linux/w1.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");
-MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_DS28E04));
+#define W1_FAMILY_DS28E04	0x1C
 
 /* Allow the strong pullup to be disabled, but default to enabled.
  * If it was disabled a parasite powered device might not get the required
@@ -428,3 +423,8 @@ static struct w1_family w1_family_1C = {
 	.fops = &w1_f1C_fops,
 };
 module_w1_family(w1_family_1C);
+
+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");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_DS28E04));
diff --git a/drivers/w1/slaves/w1_smem.c b/drivers/w1/slaves/w1_smem.c
index ed4c87506def..e556b0caff71 100644
--- a/drivers/w1/slaves/w1_smem.c
+++ b/drivers/w1/slaves/w1_smem.c
@@ -27,15 +27,10 @@
 #include <linux/device.h>
 #include <linux/types.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
-#include "../w1_family.h"
+#include <linux/w1.h>
 
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Evgeniy Polyakov <zbr@ioremap.net>");
-MODULE_DESCRIPTION("Driver for 1-wire Dallas network protocol, 64bit memory family.");
-MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_SMEM_01));
-MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_SMEM_81));
+#define W1_FAMILY_SMEM_01	0x01
+#define W1_FAMILY_SMEM_81	0x81
 
 static struct w1_family w1_smem_family_01 = {
 	.fid = W1_FAMILY_SMEM_01,
@@ -70,3 +65,9 @@ static void __exit w1_smem_fini(void)
 
 module_init(w1_smem_init);
 module_exit(w1_smem_fini);
+
+MODULE_AUTHOR("Evgeniy Polyakov <zbr@ioremap.net>");
+MODULE_DESCRIPTION("Driver for 1-wire Dallas network protocol, 64bit memory family.");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_SMEM_01));
+MODULE_ALIAS("w1-family-" __stringify(W1_FAMILY_SMEM_81));
diff --git a/drivers/w1/slaves/w1_therm.c b/drivers/w1/slaves/w1_therm.c
index 82611f197b0a..cb3fc3c6b0d1 100644
--- a/drivers/w1/slaves/w1_therm.c
+++ b/drivers/w1/slaves/w1_therm.c
@@ -30,18 +30,13 @@
 #include <linux/slab.h>
 #include <linux/delay.h>
 
-#include "../w1.h"
-#include "../w1_int.h"
-#include "../w1_family.h"
+#include <linux/w1.h>
 
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Evgeniy Polyakov <zbr@ioremap.net>");
-MODULE_DESCRIPTION("Driver for 1-wire Dallas network protocol, temperature family.");
-MODULE_ALIAS("w1-family-" __stringify(W1_THERM_DS18S20));
-MODULE_ALIAS("w1-family-" __stringify(W1_THERM_DS1822));
-MODULE_ALIAS("w1-family-" __stringify(W1_THERM_DS18B20));
-MODULE_ALIAS("w1-family-" __stringify(W1_THERM_DS1825));
-MODULE_ALIAS("w1-family-" __stringify(W1_THERM_DS28EA00));
+#define W1_THERM_DS18S20	0x10
+#define W1_THERM_DS1822		0x22
+#define W1_THERM_DS18B20	0x28
+#define W1_THERM_DS1825		0x3B
+#define W1_THERM_DS28EA00	0x42
 
 /* Allow the strong pullup to be disabled, but default to enabled.
  * If it was disabled a parasite powered device might not get the require
@@ -646,3 +641,12 @@ static void __exit w1_therm_fini(void)
 
 module_init(w1_therm_init);
 module_exit(w1_therm_fini);
+
+MODULE_AUTHOR("Evgeniy Polyakov <zbr@ioremap.net>");
+MODULE_DESCRIPTION("Driver for 1-wire Dallas network protocol, temperature family.");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("w1-family-" __stringify(W1_THERM_DS18S20));
+MODULE_ALIAS("w1-family-" __stringify(W1_THERM_DS1822));
+MODULE_ALIAS("w1-family-" __stringify(W1_THERM_DS18B20));
+MODULE_ALIAS("w1-family-" __stringify(W1_THERM_DS1825));
+MODULE_ALIAS("w1-family-" __stringify(W1_THERM_DS28EA00));
diff --git a/drivers/w1/w1.c b/drivers/w1/w1.c
index 8511d1685db9..95ea7e6b1d99 100644
--- a/drivers/w1/w1.c
+++ b/drivers/w1/w1.c
@@ -28,25 +28,20 @@
 
 #include <linux/atomic.h>
 
-#include "w1.h"
-#include "w1_int.h"
-#include "w1_family.h"
+#include "w1_internal.h"
 #include "w1_netlink.h"
 
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Evgeniy Polyakov <zbr@ioremap.net>");
-MODULE_DESCRIPTION("Driver for 1-wire Dallas network protocol.");
+#define W1_FAMILY_DEFAULT	0
 
 static int w1_timeout = 10;
-static int w1_timeout_us = 0;
-int w1_max_slave_count = 64;
-int w1_max_slave_ttl = 10;
-
 module_param_named(timeout, w1_timeout, int, 0);
 MODULE_PARM_DESC(timeout, "time in seconds between automatic slave searches");
+
+static int w1_timeout_us = 0;
 module_param_named(timeout_us, w1_timeout_us, int, 0);
 MODULE_PARM_DESC(timeout_us,
 		 "time in microseconds between automatic slave searches");
+
 /* A search stops when w1_max_slave_count devices have been found in that
  * search.  The next search will start over and detect the same set of devices
  * on a static 1-wire bus.  Memory is not allocated based on this number, just
@@ -55,9 +50,12 @@ MODULE_PARM_DESC(timeout_us,
  * device on the network and w1_max_slave_count is set to 1, the device id can
  * be read directly skipping the normal slower search process.
  */
+int w1_max_slave_count = 64;
 module_param_named(max_slave_count, w1_max_slave_count, int, 0);
 MODULE_PARM_DESC(max_slave_count,
 	"maximum number of slaves detected in a search");
+
+int w1_max_slave_ttl = 10;
 module_param_named(slave_ttl, w1_max_slave_ttl, int, 0);
 MODULE_PARM_DESC(slave_ttl,
 	"Number of searches not seeing a slave before it will be removed");
@@ -1228,3 +1226,7 @@ static void __exit w1_fini(void)
 
 module_init(w1_init);
 module_exit(w1_fini);
+
+MODULE_AUTHOR("Evgeniy Polyakov <zbr@ioremap.net>");
+MODULE_DESCRIPTION("Driver for 1-wire Dallas network protocol.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/w1/w1.h b/drivers/w1/w1.h
deleted file mode 100644
index 758a7a6322e9..000000000000
--- a/drivers/w1/w1.h
+++ /dev/null
@@ -1,336 +0,0 @@
-/*
- * Copyright (c) 2004 Evgeniy Polyakov <zbr@ioremap.net>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#ifndef __W1_H
-#define __W1_H
-
-/**
- * struct w1_reg_num - broken out slave device id
- *
- * @family: identifies the type of device
- * @id: along with family is the unique device id
- * @crc: checksum of the other bytes
- */
-struct w1_reg_num
-{
-#if defined(__LITTLE_ENDIAN_BITFIELD)
-	__u64	family:8,
-		id:48,
-		crc:8;
-#elif defined(__BIG_ENDIAN_BITFIELD)
-	__u64	crc:8,
-		id:48,
-		family:8;
-#else
-#error "Please fix <asm/byteorder.h>"
-#endif
-};
-
-#ifdef __KERNEL__
-
-#include <linux/completion.h>
-#include <linux/device.h>
-#include <linux/mutex.h>
-
-#include "w1_family.h"
-
-#define W1_MAXNAMELEN		32
-
-#define W1_SEARCH		0xF0
-#define W1_ALARM_SEARCH		0xEC
-#define W1_CONVERT_TEMP		0x44
-#define W1_SKIP_ROM		0xCC
-#define W1_COPY_SCRATCHPAD	0x48
-#define W1_WRITE_SCRATCHPAD	0x4E
-#define W1_READ_SCRATCHPAD	0xBE
-#define W1_READ_ROM		0x33
-#define W1_READ_PSUPPLY		0xB4
-#define W1_MATCH_ROM		0x55
-#define W1_RESUME_CMD		0xA5
-
-#define W1_SLAVE_ACTIVE		0
-#define W1_SLAVE_DETACH		1
-
-/**
- * struct w1_slave - holds a single slave device on the bus
- *
- * @owner: Points to the one wire "wire" kernel module.
- * @name: Device id is ascii.
- * @w1_slave_entry: data for the linked list
- * @reg_num: the slave id in binary
- * @refcnt: reference count, delete when 0
- * @flags: bit flags for W1_SLAVE_ACTIVE W1_SLAVE_DETACH
- * @ttl: decrement per search this slave isn't found, deatch at 0
- * @master: bus which this slave is on
- * @family: module for device family type
- * @family_data: pointer for use by the family module
- * @dev: kernel device identifier
- *
- */
-struct w1_slave
-{
-	struct module		*owner;
-	unsigned char		name[W1_MAXNAMELEN];
-	struct list_head	w1_slave_entry;
-	struct w1_reg_num	reg_num;
-	atomic_t		refcnt;
-	int			ttl;
-	unsigned long		flags;
-
-	struct w1_master	*master;
-	struct w1_family	*family;
-	void			*family_data;
-	struct device		dev;
-};
-
-typedef void (*w1_slave_found_callback)(struct w1_master *, u64);
-
-
-/**
- * struct w1_bus_master - operations available on a bus master
- *
- * @data: the first parameter in all the functions below
- *
- * @read_bit: Sample the line level @return the level read (0 or 1)
- *
- * @write_bit: Sets the line level
- *
- * @touch_bit: the lowest-level function for devices that really support the
- * 1-wire protocol.
- * touch_bit(0) = write-0 cycle
- * touch_bit(1) = write-1 / read cycle
- * @return the bit read (0 or 1)
- *
- * @read_byte: Reads a bytes. Same as 8 touch_bit(1) calls.
- * @return the byte read
- *
- * @write_byte: Writes a byte. Same as 8 touch_bit(x) calls.
- *
- * @read_block: Same as a series of read_byte() calls
- * @return the number of bytes read
- *
- * @write_block: Same as a series of write_byte() calls
- *
- * @triplet: Combines two reads and a smart write for ROM searches
- * @return bit0=Id bit1=comp_id bit2=dir_taken
- *
- * @reset_bus: long write-0 with a read for the presence pulse detection
- * @return -1=Error, 0=Device present, 1=No device present
- *
- * @set_pullup: Put out a strong pull-up pulse of the specified duration.
- * @return -1=Error, 0=completed
- *
- * @search: Really nice hardware can handles the different types of ROM search
- * w1_master* is passed to the slave found callback.
- * u8 is search_type, W1_SEARCH or W1_ALARM_SEARCH
- *
- * Note: read_bit and write_bit are very low level functions and should only
- * be used with hardware that doesn't really support 1-wire operations,
- * like a parallel/serial port.
- * Either define read_bit and write_bit OR define, at minimum, touch_bit and
- * reset_bus.
- *
- */
-struct w1_bus_master
-{
-	void		*data;
-
-	u8		(*read_bit)(void *);
-
-	void		(*write_bit)(void *, u8);
-
-	u8		(*touch_bit)(void *, u8);
-
-	u8		(*read_byte)(void *);
-
-	void		(*write_byte)(void *, u8);
-
-	u8		(*read_block)(void *, u8 *, int);
-
-	void		(*write_block)(void *, const u8 *, int);
-
-	u8		(*triplet)(void *, u8);
-
-	u8		(*reset_bus)(void *);
-
-	u8		(*set_pullup)(void *, int);
-
-	void		(*search)(void *, struct w1_master *,
-		u8, w1_slave_found_callback);
-};
-
-/**
- * enum w1_master_flags - bitfields used in w1_master.flags
- * @W1_ABORT_SEARCH: abort searching early on shutdown
- * @W1_WARN_MAX_COUNT: limit warning when the maximum count is reached
- */
-enum w1_master_flags {
-	W1_ABORT_SEARCH = 0,
-	W1_WARN_MAX_COUNT = 1,
-};
-
-/**
- * struct w1_master - one per bus master
- * @w1_master_entry:	master linked list
- * @owner:		module owner
- * @name:		dynamically allocate bus name
- * @list_mutex:		protect slist and async_list
- * @slist:		linked list of slaves
- * @async_list:		linked list of netlink commands to execute
- * @max_slave_count:	maximum number of slaves to search for at a time
- * @slave_count:	current number of slaves known
- * @attempts:		number of searches ran
- * @slave_ttl:		number of searches before a slave is timed out
- * @initialized:	prevent init/removal race conditions
- * @id:			w1 bus number
- * @search_count:	number of automatic searches to run, -1 unlimited
- * @search_id:		allows continuing a search
- * @refcnt:		reference count
- * @priv:		private data storage
- * @enable_pullup:	allows a strong pullup
- * @pullup_duration:	time for the next strong pullup
- * @flags:		one of w1_master_flags
- * @thread:		thread for bus search and netlink commands
- * @mutex:		protect most of w1_master
- * @bus_mutex:		pretect concurrent bus access
- * @driver:		sysfs driver
- * @dev:		sysfs device
- * @bus_master:		io operations available
- * @seq:		sequence number used for netlink broadcasts
- */
-struct w1_master
-{
-	struct list_head	w1_master_entry;
-	struct module		*owner;
-	unsigned char		name[W1_MAXNAMELEN];
-	/* list_mutex protects just slist and async_list so slaves can be
-	 * searched for and async commands added while the master has
-	 * w1_master.mutex locked and is operating on the bus.
-	 * lock order w1_mlock, w1_master.mutex, w1_master.list_mutex
-	 */
-	struct mutex		list_mutex;
-	struct list_head	slist;
-	struct list_head	async_list;
-	int			max_slave_count, slave_count;
-	unsigned long		attempts;
-	int			slave_ttl;
-	int			initialized;
-	u32			id;
-	int			search_count;
-	/* id to start searching on, to continue a search or 0 to restart */
-	u64			search_id;
-
-	atomic_t		refcnt;
-
-	void			*priv;
-
-	/** 5V strong pullup enabled flag, 1 enabled, zero disabled. */
-	int			enable_pullup;
-	/** 5V strong pullup duration in milliseconds, zero disabled. */
-	int			pullup_duration;
-
-	long			flags;
-
-	struct task_struct	*thread;
-	struct mutex		mutex;
-	struct mutex		bus_mutex;
-
-	struct device_driver	*driver;
-	struct device		dev;
-
-	struct w1_bus_master	*bus_master;
-
-	u32			seq;
-};
-
-/**
- * struct w1_async_cmd - execute callback from the w1_process kthread
- * @async_entry: link entry
- * @cb: callback function, must list_del and destroy this list before
- * returning
- *
- * When inserted into the w1_master async_list, w1_process will execute
- * the callback.  Embed this into the structure with the command details.
- */
-struct w1_async_cmd {
-	struct list_head	async_entry;
-	void (*cb)(struct w1_master *dev, struct w1_async_cmd *async_cmd);
-};
-
-int w1_create_master_attributes(struct w1_master *);
-void w1_destroy_master_attributes(struct w1_master *master);
-void w1_search(struct w1_master *dev, u8 search_type, w1_slave_found_callback cb);
-void w1_search_devices(struct w1_master *dev, u8 search_type, w1_slave_found_callback cb);
-/* call w1_unref_slave to release the reference counts w1_search_slave added */
-struct w1_slave *w1_search_slave(struct w1_reg_num *id);
-/* decrements the reference on sl->master and sl, and cleans up if zero
- * returns the reference count after it has been decremented */
-int w1_unref_slave(struct w1_slave *sl);
-void w1_slave_found(struct w1_master *dev, u64 rn);
-void w1_search_process_cb(struct w1_master *dev, u8 search_type,
-	w1_slave_found_callback cb);
-struct w1_slave *w1_slave_search_device(struct w1_master *dev,
-	struct w1_reg_num *rn);
-struct w1_master *w1_search_master_id(u32 id);
-
-/* Disconnect and reconnect devices in the given family.  Used for finding
- * unclaimed devices after a family has been registered or releasing devices
- * after a family has been unregistered.  Set attach to 1 when a new family
- * has just been registered, to 0 when it has been unregistered.
- */
-void w1_reconnect_slaves(struct w1_family *f, int attach);
-int w1_attach_slave_device(struct w1_master *dev, struct w1_reg_num *rn);
-/* 0 success, otherwise EBUSY */
-int w1_slave_detach(struct w1_slave *sl);
-
-u8 w1_triplet(struct w1_master *dev, int bdir);
-void w1_write_8(struct w1_master *, u8);
-u8 w1_read_8(struct w1_master *);
-int w1_reset_bus(struct w1_master *);
-u8 w1_calc_crc8(u8 *, int);
-void w1_write_block(struct w1_master *, const u8 *, int);
-void w1_touch_block(struct w1_master *, u8 *, int);
-u8 w1_read_block(struct w1_master *, u8 *, int);
-int w1_reset_select_slave(struct w1_slave *sl);
-int w1_reset_resume_command(struct w1_master *);
-void w1_next_pullup(struct w1_master *, int);
-
-static inline struct w1_slave* dev_to_w1_slave(struct device *dev)
-{
-	return container_of(dev, struct w1_slave, dev);
-}
-
-static inline struct w1_slave* kobj_to_w1_slave(struct kobject *kobj)
-{
-	return dev_to_w1_slave(container_of(kobj, struct device, kobj));
-}
-
-static inline struct w1_master* dev_to_w1_master(struct device *dev)
-{
-	return container_of(dev, struct w1_master, dev);
-}
-
-extern struct device_driver w1_master_driver;
-extern struct device w1_master_device;
-extern int w1_max_slave_count;
-extern int w1_max_slave_ttl;
-extern struct list_head w1_masters;
-extern struct mutex w1_mlock;
-
-extern int w1_process_callbacks(struct w1_master *dev);
-extern int w1_process(void *);
-
-#endif /* __KERNEL__ */
-
-#endif /* __W1_H */
diff --git a/drivers/w1/w1_family.c b/drivers/w1/w1_family.c
index 2096f460498f..f14ab0b340b5 100644
--- a/drivers/w1/w1_family.c
+++ b/drivers/w1/w1_family.c
@@ -18,8 +18,7 @@
 #include <linux/delay.h>
 #include <linux/export.h>
 
-#include "w1_family.h"
-#include "w1.h"
+#include "w1_internal.h"
 
 DEFINE_SPINLOCK(w1_flock);
 static LIST_HEAD(w1_families);
@@ -55,6 +54,7 @@ int w1_register_family(struct w1_family *newf)
 
 	return ret;
 }
+EXPORT_SYMBOL(w1_register_family);
 
 /**
  * w1_unregister_family() - unregister a device family driver
@@ -87,6 +87,7 @@ void w1_unregister_family(struct w1_family *fent)
 			flush_signals(current);
 	}
 }
+EXPORT_SYMBOL(w1_unregister_family);
 
 /*
  * Should be called under w1_flock held.
@@ -136,6 +137,3 @@ void __w1_family_get(struct w1_family *f)
 	atomic_inc(&f->refcnt);
 	smp_mb__after_atomic();
 }
-
-EXPORT_SYMBOL(w1_unregister_family);
-EXPORT_SYMBOL(w1_register_family);
diff --git a/drivers/w1/w1_family.h b/drivers/w1/w1_family.h
deleted file mode 100644
index 869a3ff87d29..000000000000
--- a/drivers/w1/w1_family.h
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
- * Copyright (c) 2004 Evgeniy Polyakov <zbr@ioremap.net>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#ifndef __W1_FAMILY_H
-#define __W1_FAMILY_H
-
-#include <linux/types.h>
-#include <linux/device.h>
-#include <linux/atomic.h>
-
-#define W1_FAMILY_DEFAULT	0
-#define W1_FAMILY_BQ27000	0x01
-#define W1_FAMILY_SMEM_01	0x01
-#define W1_FAMILY_SMEM_81	0x81
-#define W1_FAMILY_DS2405	0x05
-#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
-#define W1_FAMILY_DS2438	0x26
-#define W1_THERM_DS18B20 	0x28
-#define W1_FAMILY_DS2408	0x29
-#define W1_EEPROM_DS2431	0x2D
-#define W1_FAMILY_DS2760	0x30
-#define W1_FAMILY_DS2780	0x32
-#define W1_FAMILY_DS2413	0x3A
-#define W1_FAMILY_DS2406	0x12
-#define W1_THERM_DS1825		0x3B
-#define W1_FAMILY_DS2781	0x3D
-#define W1_THERM_DS28EA00	0x42
-
-#define MAXNAMELEN		32
-
-struct w1_slave;
-
-/**
- * struct w1_family_ops - operations for a family type
- * @add_slave: add_slave
- * @remove_slave: remove_slave
- * @groups: sysfs group
- */
-struct w1_family_ops
-{
-	int  (* add_slave)(struct w1_slave *);
-	void (* remove_slave)(struct w1_slave *);
-	const struct attribute_group **groups;
-};
-
-/**
- * struct w1_family - reference counted family structure.
- * @family_entry:	family linked list
- * @fid:		8 bit family identifier
- * @fops:		operations for this family
- * @refcnt:		reference counter
- */
-struct w1_family
-{
-	struct list_head	family_entry;
-	u8			fid;
-
-	struct w1_family_ops	*fops;
-
-	atomic_t		refcnt;
-};
-
-extern spinlock_t w1_flock;
-
-void w1_family_put(struct w1_family *);
-void __w1_family_get(struct w1_family *);
-struct w1_family * w1_family_registered(u8);
-void w1_unregister_family(struct w1_family *);
-int w1_register_family(struct w1_family *);
-
-/**
- * module_w1_driver() - Helper macro for registering a 1-Wire families
- * @__w1_family: w1_family struct
- *
- * Helper macro for 1-Wire families which do not do anything special in module
- * init/exit. This eliminates a lot of boilerplate. Each module may only
- * use this macro once, and calling it replaces module_init() and module_exit()
- */
-#define module_w1_family(__w1_family) \
-	module_driver(__w1_family, w1_register_family, \
-			w1_unregister_family)
-
-#endif /* __W1_FAMILY_H */
diff --git a/drivers/w1/w1_int.c b/drivers/w1/w1_int.c
index 1072a2e620bb..1c776178f598 100644
--- a/drivers/w1/w1_int.c
+++ b/drivers/w1/w1_int.c
@@ -21,9 +21,8 @@
 #include <linux/export.h>
 #include <linux/moduleparam.h>
 
-#include "w1.h"
+#include "w1_internal.h"
 #include "w1_netlink.h"
-#include "w1_int.h"
 
 static int w1_search_count = -1; /* Default is continual scan */
 module_param_named(search_count, w1_search_count, int, 0);
@@ -179,6 +178,7 @@ err_out_free_dev:
 
 	return retval;
 }
+EXPORT_SYMBOL(w1_add_master_device);
 
 void __w1_remove_master_device(struct w1_master *dev)
 {
@@ -251,6 +251,4 @@ void w1_remove_master_device(struct w1_bus_master *bm)
 
 	__w1_remove_master_device(found);
 }
-
-EXPORT_SYMBOL(w1_add_master_device);
 EXPORT_SYMBOL(w1_remove_master_device);
diff --git a/drivers/w1/w1_int.h b/drivers/w1/w1_int.h
deleted file mode 100644
index 371989159216..000000000000
--- a/drivers/w1/w1_int.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/*
- * Copyright (c) 2004 Evgeniy Polyakov <zbr@ioremap.net>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#ifndef __W1_INT_H
-#define __W1_INT_H
-
-#include <linux/kernel.h>
-#include <linux/device.h>
-
-#include "w1.h"
-
-int w1_add_master_device(struct w1_bus_master *);
-void w1_remove_master_device(struct w1_bus_master *);
-void __w1_remove_master_device(struct w1_master *);
-
-#endif /* __W1_INT_H */
diff --git a/drivers/w1/w1_internal.h b/drivers/w1/w1_internal.h
new file mode 100644
index 000000000000..1623e2fdccc7
--- /dev/null
+++ b/drivers/w1/w1_internal.h
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2004 Evgeniy Polyakov <zbr@ioremap.net>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __W1_H
+#define __W1_H
+
+#include <linux/w1.h>
+
+#include <linux/completion.h>
+#include <linux/mutex.h>
+
+#define W1_SLAVE_ACTIVE		0
+#define W1_SLAVE_DETACH		1
+
+/**
+ * struct w1_async_cmd - execute callback from the w1_process kthread
+ * @async_entry: link entry
+ * @cb: callback function, must list_del and destroy this list before
+ * returning
+ *
+ * When inserted into the w1_master async_list, w1_process will execute
+ * the callback.  Embed this into the structure with the command details.
+ */
+struct w1_async_cmd {
+	struct list_head	async_entry;
+	void (*cb)(struct w1_master *dev, struct w1_async_cmd *async_cmd);
+};
+
+int w1_create_master_attributes(struct w1_master *master);
+void w1_destroy_master_attributes(struct w1_master *master);
+void w1_search(struct w1_master *dev, u8 search_type,
+	       w1_slave_found_callback cb);
+void w1_search_devices(struct w1_master *dev, u8 search_type,
+		       w1_slave_found_callback cb);
+/* call w1_unref_slave to release the reference counts w1_search_slave added */
+struct w1_slave *w1_search_slave(struct w1_reg_num *id);
+/*
+ * decrements the reference on sl->master and sl, and cleans up if zero
+ * returns the reference count after it has been decremented
+ */
+int w1_unref_slave(struct w1_slave *sl);
+void w1_slave_found(struct w1_master *dev, u64 rn);
+void w1_search_process_cb(struct w1_master *dev, u8 search_type,
+			  w1_slave_found_callback cb);
+struct w1_slave *w1_slave_search_device(struct w1_master *dev,
+					struct w1_reg_num *rn);
+struct w1_master *w1_search_master_id(u32 id);
+
+/* Disconnect and reconnect devices in the given family.  Used for finding
+ * unclaimed devices after a family has been registered or releasing devices
+ * after a family has been unregistered.  Set attach to 1 when a new family
+ * has just been registered, to 0 when it has been unregistered.
+ */
+void w1_reconnect_slaves(struct w1_family *f, int attach);
+int w1_attach_slave_device(struct w1_master *dev, struct w1_reg_num *rn);
+/* 0 success, otherwise EBUSY */
+int w1_slave_detach(struct w1_slave *sl);
+
+void __w1_remove_master_device(struct w1_master *dev);
+
+void w1_family_put(struct w1_family *f);
+void __w1_family_get(struct w1_family *f);
+struct w1_family *w1_family_registered(u8 fid);
+
+extern struct device_driver w1_master_driver;
+extern struct device w1_master_device;
+extern int w1_max_slave_count;
+extern int w1_max_slave_ttl;
+extern struct list_head w1_masters;
+extern struct mutex w1_mlock;
+extern spinlock_t w1_flock;
+
+int w1_process_callbacks(struct w1_master *dev);
+int w1_process(void *data);
+
+#endif /* __W1_H */
diff --git a/drivers/w1/w1_io.c b/drivers/w1/w1_io.c
index 1134e6b1eb02..d191e1f80579 100644
--- a/drivers/w1/w1_io.c
+++ b/drivers/w1/w1_io.c
@@ -18,7 +18,7 @@
 #include <linux/moduleparam.h>
 #include <linux/module.h>
 
-#include "w1.h"
+#include "w1_internal.h"
 
 static int w1_delay_parm = 1;
 module_param_named(delay_coef, w1_delay_parm, int, 0);
diff --git a/drivers/w1/w1_netlink.c b/drivers/w1/w1_netlink.c
index 027950f997d1..f2f099caeb77 100644
--- a/drivers/w1/w1_netlink.c
+++ b/drivers/w1/w1_netlink.c
@@ -17,7 +17,7 @@
 #include <linux/netlink.h>
 #include <linux/connector.h>
 
-#include "w1.h"
+#include "w1_internal.h"
 #include "w1_netlink.h"
 
 #if defined(CONFIG_W1_CON) && (defined(CONFIG_CONNECTOR) || (defined(CONFIG_CONNECTOR_MODULE) && defined(CONFIG_W1_MODULE)))
diff --git a/drivers/w1/w1_netlink.h b/drivers/w1/w1_netlink.h
index b389e5ff5fa5..a36661cd1f05 100644
--- a/drivers/w1/w1_netlink.h
+++ b/drivers/w1/w1_netlink.h
@@ -18,7 +18,7 @@
 #include <asm/types.h>
 #include <linux/connector.h>
 
-#include "w1.h"
+#include "w1_internal.h"
 
 /**
  * enum w1_cn_msg_flags - bitfield flags for struct cn_msg.flags
diff --git a/drivers/xen/events/events_base.c b/drivers/xen/events/events_base.c
index b52852f38cff..2e567d8433b3 100644
--- a/drivers/xen/events/events_base.c
+++ b/drivers/xen/events/events_base.c
@@ -1343,8 +1343,12 @@ static int set_affinity_irq(struct irq_data *data, const struct cpumask *dest,
 			    bool force)
 {
 	unsigned tcpu = cpumask_first_and(dest, cpu_online_mask);
+	int ret = rebind_irq_to_cpu(data->irq, tcpu);
 
-	return rebind_irq_to_cpu(data->irq, tcpu);
+	if (!ret)
+		irq_data_update_effective_affinity(data, cpumask_of(tcpu));
+
+	return ret;
 }
 
 static void enable_dynirq(struct irq_data *data)
diff --git a/drivers/xen/manage.c b/drivers/xen/manage.c
index c1ec8ee80924..9e35032351a0 100644
--- a/drivers/xen/manage.c
+++ b/drivers/xen/manage.c
@@ -190,6 +190,7 @@ static void do_poweroff(void)
 {
 	switch (system_state) {
 	case SYSTEM_BOOTING:
+	case SYSTEM_SCHEDULING:
 		orderly_poweroff(true);
 		break;
 	case SYSTEM_RUNNING:
diff --git a/drivers/xen/mcelog.c b/drivers/xen/mcelog.c
index a493c7315e94..6cc1c15bcd84 100644
--- a/drivers/xen/mcelog.c
+++ b/drivers/xen/mcelog.c
@@ -408,6 +408,8 @@ static int __init xen_late_init_mcelog(void)
 	if (ret)
 		goto deregister;
 
+	pr_info("/dev/mcelog registered by Xen\n");
+
 	return 0;
 
 deregister:
diff --git a/drivers/xen/tmem.c b/drivers/xen/tmem.c
index 4ac2ca8a7656..bf13d1ec51f3 100644
--- a/drivers/xen/tmem.c
+++ b/drivers/xen/tmem.c
@@ -233,12 +233,12 @@ static int tmem_cleancache_init_fs(size_t pagesize)
 	return xen_tmem_new_pool(uuid_private, 0, pagesize);
 }
 
-static int tmem_cleancache_init_shared_fs(char *uuid, size_t pagesize)
+static int tmem_cleancache_init_shared_fs(uuid_t *uuid, size_t pagesize)
 {
 	struct tmem_pool_uuid shared_uuid;
 
-	shared_uuid.uuid_lo = *(u64 *)uuid;
-	shared_uuid.uuid_hi = *(u64 *)(&uuid[8]);
+	shared_uuid.uuid_lo = *(u64 *)&uuid->b[0];
+	shared_uuid.uuid_hi = *(u64 *)&uuid->b[8];
 	return xen_tmem_new_pool(shared_uuid, TMEM_POOL_SHARED, pagesize);
 }